3505 files changed, 280657 insertions, 119923 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index bfc918633fd..ece958d3762 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -112,6 +112,8 @@ source "drivers/auxdisplay/Kconfig"
 
 source "drivers/uio/Kconfig"
 
+source "drivers/vfio/Kconfig"
+
 source "drivers/vlynq/Kconfig"
 
 source "drivers/virtio/Kconfig"
@@ -148,4 +150,6 @@ source "drivers/iio/Kconfig"
 
 source "drivers/vme/Kconfig"
 
+source "drivers/pwm/Kconfig"
+
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index 2ba29ffef2c..5b421840c48 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -8,6 +8,7 @@
 # GPIO must come after pinctrl as gpios may need to mux pins etc
 obj-y				+= pinctrl/
 obj-y				+= gpio/
+obj-y				+= pwm/
 obj-$(CONFIG_PCI)		+= pci/
 obj-$(CONFIG_PARISC)		+= parisc/
 obj-$(CONFIG_RAPIDIO)		+= rapidio/
@@ -59,6 +60,7 @@ obj-$(CONFIG_ATM)		+= atm/
 obj-$(CONFIG_FUSION)		+= message/
 obj-y				+= firewire/
 obj-$(CONFIG_UIO)		+= uio/
+obj-$(CONFIG_VFIO)		+= vfio/
 obj-y				+= cdrom/
 obj-y				+= auxdisplay/
 obj-$(CONFIG_PCCARD)		+= pcmcia/
diff --git a/drivers/acpi/ac.c b/drivers/acpi/ac.c
index 6512b20aecc..ac7034129f3 100644
--- a/drivers/acpi/ac.c
+++ b/drivers/acpi/ac.c
@@ -61,7 +61,6 @@ static int acpi_ac_open_fs(struct inode *inode, struct file *file);
 
 static int acpi_ac_add(struct acpi_device *device);
 static int acpi_ac_remove(struct acpi_device *device, int type);
-static int acpi_ac_resume(struct acpi_device *device);
 static void acpi_ac_notify(struct acpi_device *device, u32 event);
 
 static const struct acpi_device_id ac_device_ids[] = {
@@ -70,6 +69,9 @@ static const struct acpi_device_id ac_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, ac_device_ids);
 
+static int acpi_ac_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(acpi_ac_pm, NULL, acpi_ac_resume);
+
 static struct acpi_driver acpi_ac_driver = {
 	.name = "ac",
 	.class = ACPI_AC_CLASS,
@@ -78,9 +80,9 @@ static struct acpi_driver acpi_ac_driver = {
 	.ops = {
 		.add = acpi_ac_add,
 		.remove = acpi_ac_remove,
-		.resume = acpi_ac_resume,
 		.notify = acpi_ac_notify,
 		},
+	.drv.pm = &acpi_ac_pm,
 };
 
 struct acpi_ac {
@@ -292,7 +294,9 @@ static int acpi_ac_add(struct acpi_device *device)
 	ac->charger.properties = ac_props;
 	ac->charger.num_properties = ARRAY_SIZE(ac_props);
 	ac->charger.get_property = get_ac_property;
-	power_supply_register(&ac->device->dev, &ac->charger);
+	result = power_supply_register(&ac->device->dev, &ac->charger);
+	if (result)
+		goto end;
 
 	printk(KERN_INFO PREFIX "%s [%s] (%s)\n",
 	       acpi_device_name(device), acpi_device_bid(device),
@@ -309,13 +313,18 @@ static int acpi_ac_add(struct acpi_device *device)
 	return result;
 }
 
-static int acpi_ac_resume(struct acpi_device *device)
+static int acpi_ac_resume(struct device *dev)
 {
 	struct acpi_ac *ac;
 	unsigned old_state;
-	if (!device || !acpi_driver_data(device))
+
+	if (!dev)
 		return -EINVAL;
-	ac = acpi_driver_data(device);
+
+	ac = acpi_driver_data(to_acpi_device(dev));
+	if (!ac)
+		return -EINVAL;
+
 	old_state = ac->state;
 	if (acpi_ac_get_state(ac))
 		return 0;
diff --git a/drivers/acpi/acpi_memhotplug.c b/drivers/acpi/acpi_memhotplug.c
index d9857138565..24c807f9663 100644
--- a/drivers/acpi/acpi_memhotplug.c
+++ b/drivers/acpi/acpi_memhotplug.c
@@ -341,7 +341,7 @@ static void acpi_memory_device_notify(acpi_handle handle, u32 event, void *data)
 {
 	struct acpi_memory_device *mem_device;
 	struct acpi_device *device;
-
+	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
 
 	switch (event) {
 	case ACPI_NOTIFY_BUS_CHECK:
@@ -354,15 +354,20 @@ static void acpi_memory_device_notify(acpi_handle handle, u32 event, void *data)
 					  "\nReceived DEVICE CHECK notification for device\n"));
 		if (acpi_memory_get_device(handle, &mem_device)) {
 			printk(KERN_ERR PREFIX "Cannot find driver data\n");
-			return;
+			break;
 		}
 
-		if (!acpi_memory_check_device(mem_device)) {
-			if (acpi_memory_enable_device(mem_device))
-				printk(KERN_ERR PREFIX
-					    "Cannot enable memory device\n");
+		if (acpi_memory_check_device(mem_device))
+			break;
+
+		if (acpi_memory_enable_device(mem_device)) {
+			printk(KERN_ERR PREFIX "Cannot enable memory device\n");
+			break;
 		}
+
+		ost_code = ACPI_OST_SC_SUCCESS;
 		break;
+
 	case ACPI_NOTIFY_EJECT_REQUEST:
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 				  "\nReceived EJECT REQUEST notification for device\n"));
@@ -383,19 +388,35 @@ static void acpi_memory_device_notify(acpi_handle handle, u32 event, void *data)
 		 * TBD: Can also be disabled by Callback registration
 		 *      with generic sysfs driver
 		 */
-		if (acpi_memory_disable_device(mem_device))
-			printk(KERN_ERR PREFIX
-				    "Disable memory device\n");
+		if (acpi_memory_disable_device(mem_device)) {
+			printk(KERN_ERR PREFIX "Disable memory device\n");
+			/*
+			 * If _EJ0 was called but failed, _OST is not
+			 * necessary.
+			 */
+			if (mem_device->state == MEMORY_INVALID_STATE)
+				return;
+
+			break;
+		}
+
 		/*
 		 * TBD: Invoke acpi_bus_remove to cleanup data structures
 		 */
-		break;
+
+		/* _EJ0 succeeded; _OST is not necessary */
+		return;
+
 	default:
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 				  "Unsupported event [0x%x]\n", event));
-		break;
+
+		/* non-hotplug event; possibly handled by other handler */
+		return;
 	}
 
+	/* Inform firmware that the hotplug operation has completed */
+	(void) acpi_evaluate_hotplug_ost(handle, event, ost_code, NULL);
 	return;
 }
 
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index 1502c50273b..af4aad6ee2e 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -145,7 +145,7 @@ static void exit_round_robin(unsigned int tsk_index)
 }
 
 static unsigned int idle_pct = 5; /* percentage */
-static unsigned int round_robin_time = 10; /* second */
+static unsigned int round_robin_time = 1; /* second */
 static int power_saving_thread(void *data)
 {
 	struct sched_param param = {.sched_priority = 1};
@@ -235,7 +235,7 @@ static int create_power_saving_task(void)
 
 	ps_tsks[ps_tsk_num] = kthread_run(power_saving_thread,
 		(void *)(unsigned long)ps_tsk_num,
-		"power_saving/%d", ps_tsk_num);
+		"acpi_pad/%d", ps_tsk_num);
 	rc = IS_ERR(ps_tsks[ps_tsk_num]) ? PTR_ERR(ps_tsks[ps_tsk_num]) : 0;
 	if (!rc)
 		ps_tsk_num++;
diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index 793b8cc8e25..0a1b3435f92 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -134,12 +134,14 @@ acpi-y +=		\
 	tbinstal.o	\
 	tbutils.o	\
 	tbxface.o	\
+	tbxfload.o	\
 	tbxfroot.o
 
 acpi-y +=		\
 	utaddress.o	\
 	utalloc.o	\
 	utcopy.o	\
+	utexcep.o	\
 	utdebug.o	\
 	utdecode.o	\
 	utdelete.o	\
diff --git a/drivers/acpi/acpica/acevents.h b/drivers/acpi/acpica/acevents.h
index d700f63e470..c0a43b38c6a 100644
--- a/drivers/acpi/acpica/acevents.h
+++ b/drivers/acpi/acpica/acevents.h
@@ -237,7 +237,7 @@ u32 acpi_ev_install_sci_handler(void);
 
 acpi_status acpi_ev_remove_sci_handler(void);
 
-u32 acpi_ev_initialize_sCI(u32 program_sCI);
+u32 acpi_ev_initialize_SCI(u32 program_SCI);
 
 ACPI_HW_DEPENDENT_RETURN_VOID(void acpi_ev_terminate(void))
 #endif				/* __ACEVENTS_H__  */
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index 4f7d3f57d05..ce79100fb5e 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -278,8 +278,7 @@ ACPI_EXTERN acpi_cache_t *acpi_gbl_operand_cache;
 
 /* Global handlers */
 
-ACPI_EXTERN struct acpi_object_notify_handler acpi_gbl_device_notify;
-ACPI_EXTERN struct acpi_object_notify_handler acpi_gbl_system_notify;
+ACPI_EXTERN struct acpi_global_notify_handler acpi_gbl_global_notify[2];
 ACPI_EXTERN acpi_exception_handler acpi_gbl_exception_handler;
 ACPI_EXTERN acpi_init_handler acpi_gbl_init_handler;
 ACPI_EXTERN acpi_tbl_handler acpi_gbl_table_handler;
@@ -327,14 +326,6 @@ extern const char *acpi_gbl_region_types[ACPI_NUM_PREDEFINED_REGIONS];
 
 #endif
 
-/* Exception codes */
-
-extern char const *acpi_gbl_exception_names_env[];
-extern char const *acpi_gbl_exception_names_pgm[];
-extern char const *acpi_gbl_exception_names_tbl[];
-extern char const *acpi_gbl_exception_names_aml[];
-extern char const *acpi_gbl_exception_names_ctrl[];
-
 /*****************************************************************************
  *
  * Namespace globals
@@ -463,4 +454,12 @@ ACPI_EXTERN u32 acpi_gbl_size_of_acpi_objects;
 
 #endif				/* ACPI_DEBUGGER */
 
+/*****************************************************************************
+ *
+ * Info/help support
+ *
+ ****************************************************************************/
+
+extern const struct ah_predefined_name asl_predefined_info[];
+
 #endif				/* __ACGLOBAL_H__ */
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index e3922ca20e7..cc80fe10e8e 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -299,7 +299,7 @@ acpi_status(*ACPI_INTERNAL_METHOD) (struct acpi_walk_state * walk_state);
  * Information structure for ACPI predefined names.
  * Each entry in the table contains the following items:
  *
- * Name                 - The ACPI reserved name
+ * name                 - The ACPI reserved name
  * param_count          - Number of arguments to the method
  * expected_return_btypes - Allowed type(s) for the return value
  */
@@ -404,6 +404,13 @@ struct acpi_gpe_handler_info {
 	u8 originally_enabled;  /* True if GPE was originally enabled */
 };
 
+/* Notify info for implicit notify, multiple device objects */
+
+struct acpi_gpe_notify_info {
+	struct acpi_namespace_node *device_node;	/* Device to be notified */
+	struct acpi_gpe_notify_info *next;
+};
+
 struct acpi_gpe_notify_object {
 	struct acpi_namespace_node *node;
 	struct acpi_gpe_notify_object *next;
@@ -412,7 +419,7 @@ struct acpi_gpe_notify_object {
 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_gpe_notify_object device;   /* List of _PRW devices for implicit notify */
+	struct acpi_gpe_notify_info *notify_list;	/* List of _PRW devices for implicit notifies */
 };
 
 /*
@@ -420,7 +427,7 @@ union acpi_gpe_dispatch_info {
  * NOTE: Important to keep this struct as small as possible.
  */
 struct acpi_gpe_event_info {
-	union acpi_gpe_dispatch_info dispatch;	/* Either Method or Handler */
+	union acpi_gpe_dispatch_info dispatch;	/* Either Method, Handler, or notify_list */
 	struct acpi_gpe_register_info *register_info;	/* Backpointer to register info */
 	u8 flags;		/* Misc info about this GPE */
 	u8 gpe_number;		/* This GPE */
@@ -600,13 +607,22 @@ acpi_status(*acpi_parse_downwards) (struct acpi_walk_state * walk_state,
 
 typedef acpi_status(*acpi_parse_upwards) (struct acpi_walk_state * walk_state);
 
+/* Global handlers for AML Notifies */
+
+struct acpi_global_notify_handler {
+	acpi_notify_handler handler;
+	void *context;
+};
+
 /*
  * Notify info - used to pass info to the deferred notify
  * handler/dispatcher.
  */
 struct acpi_notify_info {
-	ACPI_STATE_COMMON struct acpi_namespace_node *node;
-	union acpi_operand_object *handler_obj;
+	ACPI_STATE_COMMON u8 handler_list_id;
+	struct acpi_namespace_node *node;
+	union acpi_operand_object *handler_list_head;
+	struct acpi_global_notify_handler *global;
 };
 
 /* Generic state is union of structs above */
@@ -718,7 +734,7 @@ struct acpi_parse_obj_named {
 	u32 name;		/* 4-byte name or zero if no name */
 };
 
-/* This version is used by the i_aSL compiler only */
+/* This version is used by the iASL compiler only */
 
 #define ACPI_MAX_PARSEOP_NAME   20
 
@@ -787,6 +803,7 @@ struct acpi_parse_state {
 #define ACPI_PARSEOP_IGNORE             0x01
 #define ACPI_PARSEOP_PARAMLIST          0x02
 #define ACPI_PARSEOP_EMPTY_TERMLIST     0x04
+#define ACPI_PARSEOP_PREDEF_CHECKED     0x08
 #define ACPI_PARSEOP_SPECIAL            0x10
 
 /*****************************************************************************
@@ -1075,4 +1092,18 @@ struct acpi_debug_mem_block {
 #define ACPI_MEM_LIST_MAX               1
 #define ACPI_NUM_MEM_LISTS              2
 
+/*****************************************************************************
+ *
+ * Info/help support
+ *
+ ****************************************************************************/
+
+struct ah_predefined_name {
+	char *name;
+	char *description;
+#ifndef ACPI_ASL_COMPILER
+	char *action;
+#endif
+};
+
 #endif				/* __ACLOCAL_H__ */
diff --git a/drivers/acpi/acpica/acmacros.h b/drivers/acpi/acpica/acmacros.h
index f119f473f71..832b6198652 100644
--- a/drivers/acpi/acpica/acmacros.h
+++ b/drivers/acpi/acpica/acmacros.h
@@ -62,7 +62,7 @@
  * printf() format helpers
  */
 
-/* Split 64-bit integer into two 32-bit values. Use with %8.8_x%8.8_x */
+/* Split 64-bit integer into two 32-bit values. Use with %8.8X%8.8X */
 
 #define ACPI_FORMAT_UINT64(i)           ACPI_HIDWORD(i), ACPI_LODWORD(i)
 
@@ -283,8 +283,8 @@
 #define ACPI_INSERT_BITS(target, mask, source)          target = ((target & (~(mask))) | (source & mask))
 
 /*
- * A struct acpi_namespace_node can appear in some contexts
- * where a pointer to a union acpi_operand_object can also
+ * An object of type struct acpi_namespace_node can appear in some contexts
+ * where a pointer to an object of type union acpi_operand_object can also
  * appear. This macro is used to distinguish them.
  *
  * The "Descriptor" field is the first field in both structures.
diff --git a/drivers/acpi/acpica/acobject.h b/drivers/acpi/acpica/acobject.h
index c065078ca83..364a1303fb8 100644
--- a/drivers/acpi/acpica/acobject.h
+++ b/drivers/acpi/acpica/acobject.h
@@ -113,8 +113,8 @@ struct acpi_object_integer {
 };
 
 /*
- * Note: The String and Buffer object must be identical through the Pointer
- * and length elements.  There is code that depends on this.
+ * Note: The String and Buffer object must be identical through the
+ * pointer and length elements. There is code that depends on this.
  *
  * Fields common to both Strings and Buffers
  */
@@ -206,8 +206,7 @@ struct acpi_object_method {
  * Common fields for objects that support ASL notifications
  */
 #define ACPI_COMMON_NOTIFY_INFO \
-	union acpi_operand_object       *system_notify;     /* Handler for system notifies */\
-	union acpi_operand_object       *device_notify;     /* Handler for driver notifies */\
+	union acpi_operand_object       *notify_list[2];    /* Handlers for system/device notifies */\
 	union acpi_operand_object       *handler;	/* Handler for Address space */
 
 struct acpi_object_notify_common {	/* COMMON NOTIFY for POWER, PROCESSOR, DEVICE, and THERMAL */
@@ -296,10 +295,10 @@ struct acpi_object_buffer_field {
 
 struct acpi_object_notify_handler {
 	ACPI_OBJECT_COMMON_HEADER struct acpi_namespace_node *node;	/* Parent device */
-	u32 handler_type;
-	acpi_notify_handler handler;
+	u32 handler_type;	/* Type: Device/System/Both */
+	acpi_notify_handler handler;	/* Handler address */
 	void *context;
-	struct acpi_object_notify_handler *next;
+	union acpi_operand_object *next[2];	/* Device and System handler lists */
 };
 
 struct acpi_object_addr_handler {
@@ -382,7 +381,7 @@ struct acpi_object_cache_list {
 
 /******************************************************************************
  *
- * union acpi_operand_object Descriptor - a giant union of all of the above
+ * union acpi_operand_object descriptor - a giant union of all of the above
  *
  *****************************************************************************/
 
diff --git a/drivers/acpi/acpica/acpredef.h b/drivers/acpi/acpica/acpredef.h
index bbb34c9be4e..3080c017f5b 100644
--- a/drivers/acpi/acpica/acpredef.h
+++ b/drivers/acpi/acpica/acpredef.h
@@ -140,7 +140,7 @@ enum acpi_return_package_types {
  *
  * The main entries in the table each contain the following items:
  *
- * Name                 - The ACPI reserved name
+ * name                 - The ACPI reserved name
  * param_count          - Number of arguments to the method
  * expected_btypes      - Allowed type(s) for the return value.
  *                        0 means that no return value is expected.
@@ -511,14 +511,14 @@ static const union acpi_predefined_info predefined_names[] =
 	{{"_TMP", 0, ACPI_RTYPE_INTEGER}},
 	{{"_TPC", 0, ACPI_RTYPE_INTEGER}},
 	{{"_TPT", 1, 0}},
-	{{"_TRT", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 2_ref/6_int */
+	{{"_TRT", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 2 Ref/6 Int */
 			  {{{ACPI_PTYPE2, ACPI_RTYPE_REFERENCE, 2, ACPI_RTYPE_INTEGER}, 6, 0}},
 
-	{{"_TSD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 5_int with count */
+	{{"_TSD", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 5 Int with count */
 			  {{{ACPI_PTYPE2_COUNT,ACPI_RTYPE_INTEGER, 5,0}, 0,0}},
 
 	{{"_TSP", 0, ACPI_RTYPE_INTEGER}},
-	{{"_TSS", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 5_int */
+	{{"_TSS", 0, ACPI_RTYPE_PACKAGE}}, /* Variable-length (Pkgs) each 5 Int */
 			  {{{ACPI_PTYPE2, ACPI_RTYPE_INTEGER, 5,0}, 0,0}},
 
 	{{"_TST", 0, ACPI_RTYPE_INTEGER}},
diff --git a/drivers/acpi/acpica/acstruct.h b/drivers/acpi/acpica/acstruct.h
index 0404df605bc..f196e2c9a71 100644
--- a/drivers/acpi/acpica/acstruct.h
+++ b/drivers/acpi/acpica/acstruct.h
@@ -68,7 +68,7 @@
 #define ACPI_WALK_METHOD            0x01
 #define ACPI_WALK_METHOD_RESTART    0x02
 
-/* Flags for i_aSL compiler only */
+/* Flags for iASL compiler only */
 
 #define ACPI_WALK_CONST_REQUIRED    0x10
 #define ACPI_WALK_CONST_OPTIONAL    0x20
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index 925ccf22101..5035327ebcc 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -460,6 +460,8 @@ acpi_ut_short_divide(u64 in_dividend,
 /*
  * utmisc
  */
+void ut_convert_backslashes(char *pathname);
+
 const char *acpi_ut_validate_exception(acpi_status status);
 
 u8 acpi_ut_is_pci_root_bridge(char *id);
diff --git a/drivers/acpi/acpica/amlcode.h b/drivers/acpi/acpica/amlcode.h
index 905280fec0f..c26f8ff6c3b 100644
--- a/drivers/acpi/acpica/amlcode.h
+++ b/drivers/acpi/acpica/amlcode.h
@@ -182,7 +182,7 @@
 
 /*
  * Combination opcodes (actually two one-byte opcodes)
- * Used by the disassembler and i_aSL compiler
+ * Used by the disassembler and iASL compiler
  */
 #define AML_LGREATEREQUAL_OP        (u16) 0x9295
 #define AML_LLESSEQUAL_OP           (u16) 0x9294
@@ -280,7 +280,7 @@
 
 /* Multiple/complex types */
 
-#define ARGI_DATAOBJECT             0x12	/* Buffer, String, package or reference to a Node - Used only by size_of operator */
+#define ARGI_DATAOBJECT             0x12	/* Buffer, String, package or reference to a node - Used only by size_of operator */
 #define ARGI_COMPLEXOBJ             0x13	/* Buffer, String, or package (Used by INDEX op only) */
 #define ARGI_REF_OR_STRING          0x14	/* Reference or String (Used by DEREFOF op only) */
 #define ARGI_REGION_OR_BUFFER       0x15	/* Used by LOAD op only */
diff --git a/drivers/acpi/acpica/amlresrc.h b/drivers/acpi/acpica/amlresrc.h
index 7b2128f274e..af4947956ec 100644
--- a/drivers/acpi/acpica/amlresrc.h
+++ b/drivers/acpi/acpica/amlresrc.h
@@ -98,7 +98,7 @@
 #define ACPI_RESTAG_TRANSLATION                 "_TRA"
 #define ACPI_RESTAG_TRANSTYPE                   "_TRS"	/* Sparse(1), Dense(0) */
 #define ACPI_RESTAG_TYPE                        "_TTP"	/* Translation(1), Static (0) */
-#define ACPI_RESTAG_XFERTYPE                    "_SIZ"	/* 8(0), 8_and16(1), 16(2) */
+#define ACPI_RESTAG_XFERTYPE                    "_SIZ"	/* 8(0), 8And16(1), 16(2) */
 #define ACPI_RESTAG_VENDORDATA                  "_VEN"
 
 /* Default sizes for "small" resource descriptors */
@@ -235,7 +235,7 @@ AML_RESOURCE_LARGE_HEADER_COMMON AML_RESOURCE_ADDRESS_COMMON};
 
 struct aml_resource_extended_address64 {
 	AML_RESOURCE_LARGE_HEADER_COMMON
-	    AML_RESOURCE_ADDRESS_COMMON u8 revision_iD;
+	    AML_RESOURCE_ADDRESS_COMMON u8 revision_ID;
 	u8 reserved;
 	u64 granularity;
 	u64 minimum;
diff --git a/drivers/acpi/acpica/dsargs.c b/drivers/acpi/acpica/dsargs.c
index 80eb1900297..c8b5e2565b9 100644
--- a/drivers/acpi/acpica/dsargs.c
+++ b/drivers/acpi/acpica/dsargs.c
@@ -62,7 +62,7 @@ acpi_ds_execute_arguments(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_ds_execute_arguments
  *
- * PARAMETERS:  Node                - Object NS node
+ * PARAMETERS:  node                - Object NS node
  *              scope_node          - Parent NS node
  *              aml_length          - Length of executable AML
  *              aml_start           - Pointer to the AML
diff --git a/drivers/acpi/acpica/dscontrol.c b/drivers/acpi/acpica/dscontrol.c
index effe4ca1133..465f02134b8 100644
--- a/drivers/acpi/acpica/dscontrol.c
+++ b/drivers/acpi/acpica/dscontrol.c
@@ -56,7 +56,7 @@ ACPI_MODULE_NAME("dscontrol")
  * FUNCTION:    acpi_ds_exec_begin_control_op
  *
  * PARAMETERS:  walk_list       - The list that owns the walk stack
- *              Op              - The control Op
+ *              op              - The control Op
  *
  * RETURN:      Status
  *
@@ -153,7 +153,7 @@ acpi_ds_exec_begin_control_op(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_exec_end_control_op
  *
  * PARAMETERS:  walk_list       - The list that owns the walk stack
- *              Op              - The control Op
+ *              op              - The control Op
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/dsfield.c b/drivers/acpi/acpica/dsfield.c
index cd243cf2cab..3da6fd8530c 100644
--- a/drivers/acpi/acpica/dsfield.c
+++ b/drivers/acpi/acpica/dsfield.c
@@ -53,16 +53,84 @@
 ACPI_MODULE_NAME("dsfield")
 
 /* Local prototypes */
+#ifdef ACPI_ASL_COMPILER
+#include "acdisasm.h"
+static acpi_status
+acpi_ds_create_external_region(acpi_status lookup_status,
+			       union acpi_parse_object *op,
+			       char *path,
+			       struct acpi_walk_state *walk_state,
+			       struct acpi_namespace_node **node);
+#endif
+
 static acpi_status
 acpi_ds_get_field_names(struct acpi_create_field_info *info,
 			struct acpi_walk_state *walk_state,
 			union acpi_parse_object *arg);
 
+#ifdef ACPI_ASL_COMPILER
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ds_create_external_region (iASL Disassembler only)
+ *
+ * PARAMETERS:  lookup_status   - Status from ns_lookup operation
+ *              op              - Op containing the Field definition and args
+ *              path            - Pathname of the region
+ *  `           walk_state      - Current method state
+ *              node            - Where the new region node is returned
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Add region to the external list if NOT_FOUND. Create a new
+ *              region node/object.
+ *
+ ******************************************************************************/
+
+static acpi_status
+acpi_ds_create_external_region(acpi_status lookup_status,
+			       union acpi_parse_object *op,
+			       char *path,
+			       struct acpi_walk_state *walk_state,
+			       struct acpi_namespace_node **node)
+{
+	acpi_status status;
+	union acpi_operand_object *obj_desc;
+
+	if (lookup_status != AE_NOT_FOUND) {
+		return (lookup_status);
+	}
+
+	/*
+	 * Table disassembly:
+	 * operation_region not found. Generate an External for it, and
+	 * insert the name into the namespace.
+	 */
+	acpi_dm_add_to_external_list(op, path, ACPI_TYPE_REGION, 0);
+	status = acpi_ns_lookup(walk_state->scope_info, path, ACPI_TYPE_REGION,
+				ACPI_IMODE_LOAD_PASS1, ACPI_NS_SEARCH_PARENT,
+				walk_state, node);
+	if (ACPI_FAILURE(status)) {
+		return (status);
+	}
+
+	/* Must create and install a region object for the new node */
+
+	obj_desc = acpi_ut_create_internal_object(ACPI_TYPE_REGION);
+	if (!obj_desc) {
+		return (AE_NO_MEMORY);
+	}
+
+	obj_desc->region.node = *node;
+	status = acpi_ns_attach_object(*node, obj_desc, ACPI_TYPE_REGION);
+	return (status);
+}
+#endif
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_ds_create_buffer_field
  *
- * PARAMETERS:  Op                  - Current parse op (create_xXField)
+ * PARAMETERS:  op                  - Current parse op (create_XXField)
  *              walk_state          - Current state
  *
  * RETURN:      Status
@@ -99,7 +167,7 @@ acpi_ds_create_buffer_field(union acpi_parse_object *op,
 
 		arg = acpi_ps_get_arg(op, 3);
 	} else {
-		/* For all other create_xXXField operators, name is the 3rd argument */
+		/* For all other create_XXXField operators, name is the 3rd argument */
 
 		arg = acpi_ps_get_arg(op, 2);
 	}
@@ -203,9 +271,9 @@ acpi_ds_create_buffer_field(union acpi_parse_object *op,
  *
  * FUNCTION:    acpi_ds_get_field_names
  *
- * PARAMETERS:  Info            - create_field info structure
+ * PARAMETERS:  info            - create_field info structure
  *  `           walk_state      - Current method state
- *              Arg             - First parser arg for the field name list
+ *              arg             - First parser arg for the field name list
  *
  * RETURN:      Status
  *
@@ -234,10 +302,10 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
 	while (arg) {
 		/*
 		 * Four types of field elements are handled:
-		 * 1) Name - Enters a new named field into the namespace
-		 * 2) Offset - specifies a bit offset
+		 * 1) name - Enters a new named field into the namespace
+		 * 2) offset - specifies a bit offset
 		 * 3) access_as - changes the access mode/attributes
-		 * 4) Connection - Associate a resource template with the field
+		 * 4) connection - Associate a resource template with the field
 		 */
 		switch (arg->common.aml_opcode) {
 		case AML_INT_RESERVEDFIELD_OP:
@@ -389,7 +457,7 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
  *
  * FUNCTION:    acpi_ds_create_field
  *
- * PARAMETERS:  Op              - Op containing the Field definition and args
+ * PARAMETERS:  op              - Op containing the Field definition and args
  *              region_node     - Object for the containing Operation Region
  *  `           walk_state      - Current method state
  *
@@ -413,12 +481,19 @@ acpi_ds_create_field(union acpi_parse_object *op,
 	/* First arg is the name of the parent op_region (must already exist) */
 
 	arg = op->common.value.arg;
+
 	if (!region_node) {
 		status =
 		    acpi_ns_lookup(walk_state->scope_info,
 				   arg->common.value.name, ACPI_TYPE_REGION,
 				   ACPI_IMODE_EXECUTE, ACPI_NS_SEARCH_PARENT,
 				   walk_state, &region_node);
+#ifdef ACPI_ASL_COMPILER
+		status = acpi_ds_create_external_region(status, arg,
+							arg->common.value.name,
+							walk_state,
+							&region_node);
+#endif
 		if (ACPI_FAILURE(status)) {
 			ACPI_ERROR_NAMESPACE(arg->common.value.name, status);
 			return_ACPI_STATUS(status);
@@ -446,7 +521,7 @@ acpi_ds_create_field(union acpi_parse_object *op,
  *
  * FUNCTION:    acpi_ds_init_field_objects
  *
- * PARAMETERS:  Op              - Op containing the Field definition and args
+ * PARAMETERS:  op              - Op containing the Field definition and args
  *  `           walk_state      - Current method state
  *
  * RETURN:      Status
@@ -561,7 +636,7 @@ acpi_ds_init_field_objects(union acpi_parse_object *op,
  *
  * FUNCTION:    acpi_ds_create_bank_field
  *
- * PARAMETERS:  Op              - Op containing the Field definition and args
+ * PARAMETERS:  op              - Op containing the Field definition and args
  *              region_node     - Object for the containing Operation Region
  *              walk_state      - Current method state
  *
@@ -591,6 +666,12 @@ acpi_ds_create_bank_field(union acpi_parse_object *op,
 				   arg->common.value.name, ACPI_TYPE_REGION,
 				   ACPI_IMODE_EXECUTE, ACPI_NS_SEARCH_PARENT,
 				   walk_state, &region_node);
+#ifdef ACPI_ASL_COMPILER
+		status = acpi_ds_create_external_region(status, arg,
+							arg->common.value.name,
+							walk_state,
+							&region_node);
+#endif
 		if (ACPI_FAILURE(status)) {
 			ACPI_ERROR_NAMESPACE(arg->common.value.name, status);
 			return_ACPI_STATUS(status);
@@ -645,7 +726,7 @@ acpi_ds_create_bank_field(union acpi_parse_object *op,
  *
  * FUNCTION:    acpi_ds_create_index_field
  *
- * PARAMETERS:  Op              - Op containing the Field definition and args
+ * PARAMETERS:  op              - Op containing the Field definition and args
  *              region_node     - Object for the containing Operation Region
  *  `           walk_state      - Current method state
  *
diff --git a/drivers/acpi/acpica/dsinit.c b/drivers/acpi/acpica/dsinit.c
index 9e5ac7f780a..87eff701ecf 100644
--- a/drivers/acpi/acpica/dsinit.c
+++ b/drivers/acpi/acpica/dsinit.c
@@ -60,8 +60,8 @@ acpi_ds_init_one_object(acpi_handle obj_handle,
  * FUNCTION:    acpi_ds_init_one_object
  *
  * PARAMETERS:  obj_handle      - Node for the object
- *              Level           - Current nesting level
- *              Context         - Points to a init info struct
+ *              level           - Current nesting level
+ *              context         - Points to a init info struct
  *              return_value    - Not used
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/dsmethod.c b/drivers/acpi/acpica/dsmethod.c
index 00f5dab5bcc..aa9a5d4e405 100644
--- a/drivers/acpi/acpica/dsmethod.c
+++ b/drivers/acpi/acpica/dsmethod.c
@@ -61,7 +61,7 @@ acpi_ds_create_method_mutex(union acpi_operand_object *method_desc);
  *
  * FUNCTION:    acpi_ds_method_error
  *
- * PARAMETERS:  Status          - Execution status
+ * PARAMETERS:  status          - Execution status
  *              walk_state      - Current state
  *
  * RETURN:      Status
@@ -306,9 +306,9 @@ acpi_ds_begin_method_execution(struct acpi_namespace_node *method_node,
  *
  * FUNCTION:    acpi_ds_call_control_method
  *
- * PARAMETERS:  Thread              - Info for this thread
+ * PARAMETERS:  thread              - Info for this thread
  *              this_walk_state     - Current walk state
- *              Op                  - Current Op to be walked
+ *              op                  - Current Op to be walked
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/dsmthdat.c b/drivers/acpi/acpica/dsmthdat.c
index b40bd507be5..8d55cebaa65 100644
--- a/drivers/acpi/acpica/dsmthdat.c
+++ b/drivers/acpi/acpica/dsmthdat.c
@@ -177,7 +177,7 @@ void acpi_ds_method_data_delete_all(struct acpi_walk_state *walk_state)
  *
  * FUNCTION:    acpi_ds_method_data_init_args
  *
- * PARAMETERS:  *Params         - Pointer to a parameter list for the method
+ * PARAMETERS:  *params         - Pointer to a parameter list for the method
  *              max_param_count - The arg count for this method
  *              walk_state      - Current walk state object
  *
@@ -232,11 +232,11 @@ acpi_ds_method_data_init_args(union acpi_operand_object **params,
  *
  * FUNCTION:    acpi_ds_method_data_get_node
  *
- * PARAMETERS:  Type                - Either ACPI_REFCLASS_LOCAL or
+ * PARAMETERS:  type                - Either ACPI_REFCLASS_LOCAL or
  *                                    ACPI_REFCLASS_ARG
- *              Index               - Which Local or Arg whose type to get
+ *              index               - Which Local or Arg whose type to get
  *              walk_state          - Current walk state object
- *              Node                - Where the node is returned.
+ *              node                - Where the node is returned.
  *
  * RETURN:      Status and node
  *
@@ -296,10 +296,10 @@ acpi_ds_method_data_get_node(u8 type,
  *
  * FUNCTION:    acpi_ds_method_data_set_value
  *
- * PARAMETERS:  Type                - Either ACPI_REFCLASS_LOCAL or
+ * PARAMETERS:  type                - Either ACPI_REFCLASS_LOCAL or
  *                                    ACPI_REFCLASS_ARG
- *              Index               - Which Local or Arg to get
- *              Object              - Object to be inserted into the stack entry
+ *              index               - Which Local or Arg to get
+ *              object              - Object to be inserted into the stack entry
  *              walk_state          - Current walk state object
  *
  * RETURN:      Status
@@ -336,7 +336,7 @@ acpi_ds_method_data_set_value(u8 type,
 	 * Increment ref count so object can't be deleted while installed.
 	 * NOTE: We do not copy the object in order to preserve the call by
 	 * reference semantics of ACPI Control Method invocation.
-	 * (See ACPI Specification 2.0_c)
+	 * (See ACPI Specification 2.0C)
 	 */
 	acpi_ut_add_reference(object);
 
@@ -350,9 +350,9 @@ acpi_ds_method_data_set_value(u8 type,
  *
  * FUNCTION:    acpi_ds_method_data_get_value
  *
- * PARAMETERS:  Type                - Either ACPI_REFCLASS_LOCAL or
+ * PARAMETERS:  type                - Either ACPI_REFCLASS_LOCAL or
  *                                    ACPI_REFCLASS_ARG
- *              Index               - Which local_var or argument to get
+ *              index               - Which localVar or argument to get
  *              walk_state          - Current walk state object
  *              dest_desc           - Where Arg or Local value is returned
  *
@@ -458,9 +458,9 @@ acpi_ds_method_data_get_value(u8 type,
  *
  * FUNCTION:    acpi_ds_method_data_delete_value
  *
- * PARAMETERS:  Type                - Either ACPI_REFCLASS_LOCAL or
+ * PARAMETERS:  type                - Either ACPI_REFCLASS_LOCAL or
  *                                    ACPI_REFCLASS_ARG
- *              Index               - Which local_var or argument to delete
+ *              index               - Which localVar or argument to delete
  *              walk_state          - Current walk state object
  *
  * RETURN:      None
@@ -515,9 +515,9 @@ acpi_ds_method_data_delete_value(u8 type,
  *
  * FUNCTION:    acpi_ds_store_object_to_local
  *
- * PARAMETERS:  Type                - Either ACPI_REFCLASS_LOCAL or
+ * PARAMETERS:  type                - Either ACPI_REFCLASS_LOCAL or
  *                                    ACPI_REFCLASS_ARG
- *              Index               - Which Local or Arg to set
+ *              index               - Which Local or Arg to set
  *              obj_desc            - Value to be stored
  *              walk_state          - Current walk state
  *
@@ -670,8 +670,8 @@ acpi_ds_store_object_to_local(u8 type,
  *
  * FUNCTION:    acpi_ds_method_data_get_type
  *
- * PARAMETERS:  Opcode              - Either AML_LOCAL_OP or AML_ARG_OP
- *              Index               - Which Local or Arg whose type to get
+ * PARAMETERS:  opcode              - Either AML_LOCAL_OP or AML_ARG_OP
+ *              index               - Which Local or Arg whose type to get
  *              walk_state          - Current walk state object
  *
  * RETURN:      Data type of current value of the selected Arg or Local
diff --git a/drivers/acpi/acpica/dsobject.c b/drivers/acpi/acpica/dsobject.c
index d7045ca3e32..68592dd3496 100644
--- a/drivers/acpi/acpica/dsobject.c
+++ b/drivers/acpi/acpica/dsobject.c
@@ -64,7 +64,7 @@ acpi_ds_build_internal_object(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_build_internal_object
  *
  * PARAMETERS:  walk_state      - Current walk state
- *              Op              - Parser object to be translated
+ *              op              - Parser object to be translated
  *              obj_desc_ptr    - Where the ACPI internal object is returned
  *
  * RETURN:      Status
@@ -250,7 +250,7 @@ acpi_ds_build_internal_object(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_build_internal_buffer_obj
  *
  * PARAMETERS:  walk_state      - Current walk state
- *              Op              - Parser object to be translated
+ *              op              - Parser object to be translated
  *              buffer_length   - Length of the buffer
  *              obj_desc_ptr    - Where the ACPI internal object is returned
  *
@@ -354,7 +354,7 @@ acpi_ds_build_internal_buffer_obj(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_build_internal_package_obj
  *
  * PARAMETERS:  walk_state      - Current walk state
- *              Op              - Parser object to be translated
+ *              op              - Parser object to be translated
  *              element_count   - Number of elements in the package - this is
  *                                the num_elements argument to Package()
  *              obj_desc_ptr    - Where the ACPI internal object is returned
@@ -547,8 +547,8 @@ acpi_ds_build_internal_package_obj(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_create_node
  *
  * PARAMETERS:  walk_state      - Current walk state
- *              Node            - NS Node to be initialized
- *              Op              - Parser object to be translated
+ *              node            - NS Node to be initialized
+ *              op              - Parser object to be translated
  *
  * RETURN:      Status
  *
@@ -611,8 +611,8 @@ acpi_ds_create_node(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_init_object_from_op
  *
  * PARAMETERS:  walk_state      - Current walk state
- *              Op              - Parser op used to init the internal object
- *              Opcode          - AML opcode associated with the object
+ *              op              - Parser op used to init the internal object
+ *              opcode          - AML opcode associated with the object
  *              ret_obj_desc    - Namespace object to be initialized
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/dsopcode.c b/drivers/acpi/acpica/dsopcode.c
index e5eff758510..aa34d8984d3 100644
--- a/drivers/acpi/acpica/dsopcode.c
+++ b/drivers/acpi/acpica/dsopcode.c
@@ -286,7 +286,7 @@ acpi_ds_init_buffer_field(u16 aml_opcode,
  * FUNCTION:    acpi_ds_eval_buffer_field_operands
  *
  * PARAMETERS:  walk_state      - Current walk
- *              Op              - A valid buffer_field Op object
+ *              op              - A valid buffer_field Op object
  *
  * RETURN:      Status
  *
@@ -370,7 +370,7 @@ acpi_ds_eval_buffer_field_operands(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_eval_region_operands
  *
  * PARAMETERS:  walk_state      - Current walk
- *              Op              - A valid region Op object
+ *              op              - A valid region Op object
  *
  * RETURN:      Status
  *
@@ -397,7 +397,7 @@ acpi_ds_eval_region_operands(struct acpi_walk_state *walk_state,
 	 */
 	node = op->common.node;
 
-	/* next_op points to the op that holds the space_iD */
+	/* next_op points to the op that holds the space_ID */
 
 	next_op = op->common.value.arg;
 
@@ -461,7 +461,7 @@ acpi_ds_eval_region_operands(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_eval_table_region_operands
  *
  * PARAMETERS:  walk_state      - Current walk
- *              Op              - A valid region Op object
+ *              op              - A valid region Op object
  *
  * RETURN:      Status
  *
@@ -560,7 +560,7 @@ acpi_ds_eval_table_region_operands(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_eval_data_object_operands
  *
  * PARAMETERS:  walk_state      - Current walk
- *              Op              - A valid data_object Op object
+ *              op              - A valid data_object Op object
  *              obj_desc        - data_object
  *
  * RETURN:      Status
@@ -662,7 +662,7 @@ acpi_ds_eval_data_object_operands(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ds_eval_bank_field_operands
  *
  * PARAMETERS:  walk_state      - Current walk
- *              Op              - A valid bank_field Op object
+ *              op              - A valid bank_field Op object
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/dsutils.c b/drivers/acpi/acpica/dsutils.c
index 1abcda31037..73a5447475f 100644
--- a/drivers/acpi/acpica/dsutils.c
+++ b/drivers/acpi/acpica/dsutils.c
@@ -157,7 +157,7 @@ acpi_ds_do_implicit_return(union acpi_operand_object *return_desc,
  *
  * FUNCTION:    acpi_ds_is_result_used
  *
- * PARAMETERS:  Op                  - Current Op
+ * PARAMETERS:  op                  - Current Op
  *              walk_state          - Current State
  *
  * RETURN:      TRUE if result is used, FALSE otherwise
@@ -323,7 +323,7 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
  *
  * FUNCTION:    acpi_ds_delete_result_if_not_used
  *
- * PARAMETERS:  Op              - Current parse Op
+ * PARAMETERS:  op              - Current parse Op
  *              result_obj      - Result of the operation
  *              walk_state      - Current state
  *
@@ -445,7 +445,7 @@ void acpi_ds_clear_operands(struct acpi_walk_state *walk_state)
  * FUNCTION:    acpi_ds_create_operand
  *
  * PARAMETERS:  walk_state      - Current walk state
- *              Arg             - Parse object for the argument
+ *              arg             - Parse object for the argument
  *              arg_index       - Which argument (zero based)
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/dswscope.c b/drivers/acpi/acpica/dswscope.c
index 9e9490a9cbf..f6c4295470a 100644
--- a/drivers/acpi/acpica/dswscope.c
+++ b/drivers/acpi/acpica/dswscope.c
@@ -85,8 +85,8 @@ void acpi_ds_scope_stack_clear(struct acpi_walk_state *walk_state)
  *
  * FUNCTION:    acpi_ds_scope_stack_push
  *
- * PARAMETERS:  Node            - Name to be made current
- *              Type            - Type of frame being pushed
+ * PARAMETERS:  node            - Name to be made current
+ *              type            - Type of frame being pushed
  *              walk_state      - Current state
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/dswstate.c b/drivers/acpi/acpica/dswstate.c
index c9c2ac13e7c..d0e6555061e 100644
--- a/drivers/acpi/acpica/dswstate.c
+++ b/drivers/acpi/acpica/dswstate.c
@@ -58,7 +58,7 @@ static acpi_status acpi_ds_result_stack_pop(struct acpi_walk_state *ws);
  *
  * FUNCTION:    acpi_ds_result_pop
  *
- * PARAMETERS:  Object              - Where to return the popped object
+ * PARAMETERS:  object              - Where to return the popped object
  *              walk_state          - Current Walk state
  *
  * RETURN:      Status
@@ -132,7 +132,7 @@ acpi_ds_result_pop(union acpi_operand_object **object,
  *
  * FUNCTION:    acpi_ds_result_push
  *
- * PARAMETERS:  Object              - Where to return the popped object
+ * PARAMETERS:  object              - Where to return the popped object
  *              walk_state          - Current Walk state
  *
  * RETURN:      Status
@@ -296,7 +296,7 @@ static acpi_status acpi_ds_result_stack_pop(struct acpi_walk_state *walk_state)
  *
  * FUNCTION:    acpi_ds_obj_stack_push
  *
- * PARAMETERS:  Object              - Object to push
+ * PARAMETERS:  object              - Object to push
  *              walk_state          - Current Walk state
  *
  * RETURN:      Status
@@ -433,7 +433,7 @@ acpi_ds_obj_stack_pop_and_delete(u32 pop_count,
  *
  * FUNCTION:    acpi_ds_get_current_walk_state
  *
- * PARAMETERS:  Thread          - Get current active state for this Thread
+ * PARAMETERS:  thread          - Get current active state for this Thread
  *
  * RETURN:      Pointer to the current walk state
  *
@@ -462,7 +462,7 @@ struct acpi_walk_state *acpi_ds_get_current_walk_state(struct acpi_thread_state
  * FUNCTION:    acpi_ds_push_walk_state
  *
  * PARAMETERS:  walk_state      - State to push
- *              Thread          - Thread state object
+ *              thread          - Thread state object
  *
  * RETURN:      None
  *
@@ -486,7 +486,7 @@ acpi_ds_push_walk_state(struct acpi_walk_state *walk_state,
  *
  * FUNCTION:    acpi_ds_pop_walk_state
  *
- * PARAMETERS:  Thread      - Current thread state
+ * PARAMETERS:  thread      - Current thread state
  *
  * RETURN:      A walk_state object popped from the thread's stack
  *
@@ -525,9 +525,9 @@ struct acpi_walk_state *acpi_ds_pop_walk_state(struct acpi_thread_state *thread)
  * FUNCTION:    acpi_ds_create_walk_state
  *
  * PARAMETERS:  owner_id        - ID for object creation
- *              Origin          - Starting point for this walk
+ *              origin          - Starting point for this walk
  *              method_desc     - Method object
- *              Thread          - Current thread state
+ *              thread          - Current thread state
  *
  * RETURN:      Pointer to the new walk state.
  *
@@ -578,11 +578,11 @@ struct acpi_walk_state *acpi_ds_create_walk_state(acpi_owner_id owner_id, union
  * FUNCTION:    acpi_ds_init_aml_walk
  *
  * PARAMETERS:  walk_state      - New state to be initialized
- *              Op              - Current parse op
+ *              op              - Current parse op
  *              method_node     - Control method NS node, if any
  *              aml_start       - Start of AML
  *              aml_length      - Length of AML
- *              Info            - Method info block (params, etc.)
+ *              info            - Method info block (params, etc.)
  *              pass_number     - 1, 2, or 3
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/evevent.c b/drivers/acpi/acpica/evevent.c
index 07e4dc44f81..d4acfbbe5b2 100644
--- a/drivers/acpi/acpica/evevent.c
+++ b/drivers/acpi/acpica/evevent.c
@@ -251,7 +251,7 @@ u32 acpi_ev_fixed_event_detect(void)
  *
  * FUNCTION:    acpi_ev_fixed_event_dispatch
  *
- * PARAMETERS:  Event               - Event type
+ * PARAMETERS:  event               - Event type
  *
  * RETURN:      INTERRUPT_HANDLED or INTERRUPT_NOT_HANDLED
  *
diff --git a/drivers/acpi/acpica/evglock.c b/drivers/acpi/acpica/evglock.c
index cfeab38795d..af14a713763 100644
--- a/drivers/acpi/acpica/evglock.c
+++ b/drivers/acpi/acpica/evglock.c
@@ -135,7 +135,7 @@ acpi_status acpi_ev_remove_global_lock_handler(void)
  *
  * FUNCTION:    acpi_ev_global_lock_handler
  *
- * PARAMETERS:  Context         - From thread interface, not used
+ * PARAMETERS:  context         - From thread interface, not used
  *
  * RETURN:      ACPI_INTERRUPT_HANDLED
  *
@@ -182,7 +182,7 @@ static u32 acpi_ev_global_lock_handler(void *context)
  *
  * FUNCTION:    acpi_ev_acquire_global_lock
  *
- * PARAMETERS:  Timeout         - Max time to wait for the lock, in millisec.
+ * PARAMETERS:  timeout         - Max time to wait for the lock, in millisec.
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index 8ba0e5f1709..afbd5cb391f 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -466,7 +466,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;
+	struct acpi_gpe_notify_info *notify;
 
 	ACPI_FUNCTION_TRACE(ev_asynch_execute_gpe_method);
 
@@ -517,17 +517,17 @@ static void ACPI_SYSTEM_XFACE acpi_ev_asynch_execute_gpe_method(void *context)
 		 * completes. The notify handlers are NOT invoked synchronously
 		 * from this thread -- because handlers may in turn run other
 		 * control methods.
+		 *
+		 * June 2012: Expand implicit notify mechanism to support
+		 * notifies on multiple device objects.
 		 */
-		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;
+		notify = local_gpe_event_info->dispatch.notify_list;
+		while (ACPI_SUCCESS(status) && notify) {
+			status =
+			    acpi_ev_queue_notify_request(notify->device_node,
+							 ACPI_NOTIFY_DEVICE_WAKE);
+
+			notify = notify->next;
 		}
 
 		break;
diff --git a/drivers/acpi/acpica/evgpeblk.c b/drivers/acpi/acpica/evgpeblk.c
index 23a3ca86b2e..8cf4c104c7b 100644
--- a/drivers/acpi/acpica/evgpeblk.c
+++ b/drivers/acpi/acpica/evgpeblk.c
@@ -318,7 +318,7 @@ acpi_ev_create_gpe_info_blocks(struct acpi_gpe_block_info *gpe_block)
  * FUNCTION:    acpi_ev_create_gpe_block
  *
  * PARAMETERS:  gpe_device          - Handle to the parent GPE block
- *              gpe_block_address   - Address and space_iD
+ *              gpe_block_address   - Address and space_ID
  *              register_count      - Number of GPE register pairs in the block
  *              gpe_block_base_number - Starting GPE number for the block
  *              interrupt_number    - H/W interrupt for the block
diff --git a/drivers/acpi/acpica/evgpeutil.c b/drivers/acpi/acpica/evgpeutil.c
index 3c43796b836..cb50dd91bc1 100644
--- a/drivers/acpi/acpica/evgpeutil.c
+++ b/drivers/acpi/acpica/evgpeutil.c
@@ -54,7 +54,7 @@ ACPI_MODULE_NAME("evgpeutil")
  * FUNCTION:    acpi_ev_walk_gpe_list
  *
  * PARAMETERS:  gpe_walk_callback   - Routine called for each GPE block
- *              Context             - Value passed to callback
+ *              context             - Value passed to callback
  *
  * RETURN:      Status
  *
@@ -347,6 +347,8 @@ acpi_ev_delete_gpe_handlers(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 			    void *context)
 {
 	struct acpi_gpe_event_info *gpe_event_info;
+	struct acpi_gpe_notify_info *notify;
+	struct acpi_gpe_notify_info *next;
 	u32 i;
 	u32 j;
 
@@ -365,10 +367,28 @@ acpi_ev_delete_gpe_handlers(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 
 			if ((gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK) ==
 			    ACPI_GPE_DISPATCH_HANDLER) {
+
+				/* Delete an installed handler block */
+
 				ACPI_FREE(gpe_event_info->dispatch.handler);
 				gpe_event_info->dispatch.handler = NULL;
 				gpe_event_info->flags &=
 				    ~ACPI_GPE_DISPATCH_MASK;
+			} else if ((gpe_event_info->
+				 flags & ACPI_GPE_DISPATCH_MASK) ==
+				ACPI_GPE_DISPATCH_NOTIFY) {
+
+				/* Delete the implicit notification device list */
+
+				notify = gpe_event_info->dispatch.notify_list;
+				while (notify) {
+					next = notify->next;
+					ACPI_FREE(notify);
+					notify = next;
+				}
+				gpe_event_info->dispatch.notify_list = NULL;
+				gpe_event_info->flags &=
+				    ~ACPI_GPE_DISPATCH_MASK;
 			}
 		}
 	}
diff --git a/drivers/acpi/acpica/evmisc.c b/drivers/acpi/acpica/evmisc.c
index 51ef9f5e002..51f537937c1 100644
--- a/drivers/acpi/acpica/evmisc.c
+++ b/drivers/acpi/acpica/evmisc.c
@@ -56,7 +56,7 @@ static void ACPI_SYSTEM_XFACE acpi_ev_notify_dispatch(void *context);
  *
  * FUNCTION:    acpi_ev_is_notify_object
  *
- * PARAMETERS:  Node            - Node to check
+ * PARAMETERS:  node            - Node to check
  *
  * RETURN:      TRUE if notifies allowed on this object
  *
@@ -86,7 +86,7 @@ u8 acpi_ev_is_notify_object(struct acpi_namespace_node *node)
  *
  * FUNCTION:    acpi_ev_queue_notify_request
  *
- * PARAMETERS:  Node            - NS node for the notified object
+ * PARAMETERS:  node            - NS node for the notified object
  *              notify_value    - Value from the Notify() request
  *
  * RETURN:      Status
@@ -101,102 +101,77 @@ acpi_ev_queue_notify_request(struct acpi_namespace_node * node,
 			     u32 notify_value)
 {
 	union acpi_operand_object *obj_desc;
-	union acpi_operand_object *handler_obj = NULL;
-	union acpi_generic_state *notify_info;
+	union acpi_operand_object *handler_list_head = NULL;
+	union acpi_generic_state *info;
+	u8 handler_list_id = 0;
 	acpi_status status = AE_OK;
 
 	ACPI_FUNCTION_NAME(ev_queue_notify_request);
 
-	/*
-	 * For value 0x03 (Ejection Request), may need to run a device method.
-	 * For value 0x02 (Device Wake), if _PRW exists, may need to run
-	 *   the _PS0 method.
-	 * For value 0x80 (Status Change) on the power button or sleep button,
-	 *   initiate soft-off or sleep operation.
-	 *
-	 * For all cases, simply dispatch the notify to the handler.
-	 */
-	ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-			  "Dispatching Notify on [%4.4s] (%s) Value 0x%2.2X (%s) Node %p\n",
-			  acpi_ut_get_node_name(node),
-			  acpi_ut_get_type_name(node->type), notify_value,
-			  acpi_ut_get_notify_name(notify_value), node));
+	/* Are Notifies allowed on this object? */
 
-	/* Get the notify object attached to the NS Node */
-
-	obj_desc = acpi_ns_get_attached_object(node);
-	if (obj_desc) {
-
-		/* We have the notify object, Get the correct handler */
-
-		switch (node->type) {
+	if (!acpi_ev_is_notify_object(node)) {
+		return (AE_TYPE);
+	}
 
-			/* Notify is allowed only on these types */
+	/* Get the correct notify list type (System or Device) */
 
-		case ACPI_TYPE_DEVICE:
-		case ACPI_TYPE_THERMAL:
-		case ACPI_TYPE_PROCESSOR:
+	if (notify_value <= ACPI_MAX_SYS_NOTIFY) {
+		handler_list_id = ACPI_SYSTEM_HANDLER_LIST;
+	} else {
+		handler_list_id = ACPI_DEVICE_HANDLER_LIST;
+	}
 
-			if (notify_value <= ACPI_MAX_SYS_NOTIFY) {
-				handler_obj =
-				    obj_desc->common_notify.system_notify;
-			} else {
-				handler_obj =
-				    obj_desc->common_notify.device_notify;
-			}
-			break;
+	/* Get the notify object attached to the namespace Node */
 
-		default:
+	obj_desc = acpi_ns_get_attached_object(node);
+	if (obj_desc) {
 
-			/* All other types are not supported */
+		/* We have an attached object, Get the correct handler list */
 
-			return (AE_TYPE);
-		}
+		handler_list_head =
+		    obj_desc->common_notify.notify_list[handler_list_id];
 	}
 
 	/*
-	 * If there is a handler to run, schedule the dispatcher.
-	 * Check for:
-	 * 1) Global system notify handler
-	 * 2) Global device notify handler
-	 * 3) Per-device notify handler
+	 * If there is no notify handler (Global or Local)
+	 * for this object, just ignore the notify
 	 */
-	if ((acpi_gbl_system_notify.handler &&
-	     (notify_value <= ACPI_MAX_SYS_NOTIFY)) ||
-	    (acpi_gbl_device_notify.handler &&
-	     (notify_value > ACPI_MAX_SYS_NOTIFY)) || handler_obj) {
-		notify_info = acpi_ut_create_generic_state();
-		if (!notify_info) {
-			return (AE_NO_MEMORY);
-		}
+	if (!acpi_gbl_global_notify[handler_list_id].handler
+	    && !handler_list_head) {
+		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
+				  "No notify handler for Notify, ignoring (%4.4s, %X) node %p\n",
+				  acpi_ut_get_node_name(node), notify_value,
+				  node));
 
-		if (!handler_obj) {
-			ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-					  "Executing system notify handler for Notify (%4.4s, %X) "
-					  "node %p\n",
-					  acpi_ut_get_node_name(node),
-					  notify_value, node));
-		}
+		return (AE_OK);
+	}
 
-		notify_info->common.descriptor_type =
-		    ACPI_DESC_TYPE_STATE_NOTIFY;
-		notify_info->notify.node = node;
-		notify_info->notify.value = (u16) notify_value;
-		notify_info->notify.handler_obj = handler_obj;
+	/* Setup notify info and schedule the notify dispatcher */
 
-		status =
-		    acpi_os_execute(OSL_NOTIFY_HANDLER, acpi_ev_notify_dispatch,
-				    notify_info);
-		if (ACPI_FAILURE(status)) {
-			acpi_ut_delete_generic_state(notify_info);
-		}
-	} else {
-		/* There is no notify handler (per-device or system) for this device */
+	info = acpi_ut_create_generic_state();
+	if (!info) {
+		return (AE_NO_MEMORY);
+	}
 
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "No notify handler for Notify (%4.4s, %X) node %p\n",
-				  acpi_ut_get_node_name(node), notify_value,
-				  node));
+	info->common.descriptor_type = ACPI_DESC_TYPE_STATE_NOTIFY;
+
+	info->notify.node = node;
+	info->notify.value = (u16)notify_value;
+	info->notify.handler_list_id = handler_list_id;
+	info->notify.handler_list_head = handler_list_head;
+	info->notify.global = &acpi_gbl_global_notify[handler_list_id];
+
+	ACPI_DEBUG_PRINT((ACPI_DB_INFO,
+			  "Dispatching Notify on [%4.4s] (%s) Value 0x%2.2X (%s) Node %p\n",
+			  acpi_ut_get_node_name(node),
+			  acpi_ut_get_type_name(node->type), notify_value,
+			  acpi_ut_get_notify_name(notify_value), node));
+
+	status = acpi_os_execute(OSL_NOTIFY_HANDLER, acpi_ev_notify_dispatch,
+				 info);
+	if (ACPI_FAILURE(status)) {
+		acpi_ut_delete_generic_state(info);
 	}
 
 	return (status);
@@ -206,7 +181,7 @@ acpi_ev_queue_notify_request(struct acpi_namespace_node * node,
  *
  * FUNCTION:    acpi_ev_notify_dispatch
  *
- * PARAMETERS:  Context         - To be passed to the notify handler
+ * PARAMETERS:  context         - To be passed to the notify handler
  *
  * RETURN:      None.
  *
@@ -217,60 +192,34 @@ acpi_ev_queue_notify_request(struct acpi_namespace_node * node,
 
 static void ACPI_SYSTEM_XFACE acpi_ev_notify_dispatch(void *context)
 {
-	union acpi_generic_state *notify_info =
-	    (union acpi_generic_state *)context;
-	acpi_notify_handler global_handler = NULL;
-	void *global_context = NULL;
+	union acpi_generic_state *info = (union acpi_generic_state *)context;
 	union acpi_operand_object *handler_obj;
 
 	ACPI_FUNCTION_ENTRY();
 
-	/*
-	 * We will invoke a global notify handler if installed. This is done
-	 * _before_ we invoke the per-device handler attached to the device.
-	 */
-	if (notify_info->notify.value <= ACPI_MAX_SYS_NOTIFY) {
-
-		/* Global system notification handler */
-
-		if (acpi_gbl_system_notify.handler) {
-			global_handler = acpi_gbl_system_notify.handler;
-			global_context = acpi_gbl_system_notify.context;
-		}
-	} else {
-		/* Global driver notification handler */
-
-		if (acpi_gbl_device_notify.handler) {
-			global_handler = acpi_gbl_device_notify.handler;
-			global_context = acpi_gbl_device_notify.context;
-		}
-	}
-
-	/* Invoke the system handler first, if present */
+	/* Invoke a global notify handler if installed */
 
-	if (global_handler) {
-		global_handler(notify_info->notify.node,
-			       notify_info->notify.value, global_context);
+	if (info->notify.global->handler) {
+		info->notify.global->handler(info->notify.node,
+					     info->notify.value,
+					     info->notify.global->context);
 	}
 
-	/* Now invoke the per-device handler, if present */
+	/* Now invoke the local notify handler(s) if any are installed */
 
-	handler_obj = notify_info->notify.handler_obj;
-	if (handler_obj) {
-		struct acpi_object_notify_handler *notifier;
+	handler_obj = info->notify.handler_list_head;
+	while (handler_obj) {
+		handler_obj->notify.handler(info->notify.node,
+					    info->notify.value,
+					    handler_obj->notify.context);
 
-		notifier = &handler_obj->notify;
-		while (notifier) {
-			notifier->handler(notify_info->notify.node,
-					  notify_info->notify.value,
-					  notifier->context);
-			notifier = notifier->next;
-		}
+		handler_obj =
+		    handler_obj->notify.next[info->notify.handler_list_id];
 	}
 
 	/* All done with the info object */
 
-	acpi_ut_delete_generic_state(notify_info);
+	acpi_ut_delete_generic_state(info);
 }
 
 #if (!ACPI_REDUCED_HARDWARE)
diff --git a/drivers/acpi/acpica/evregion.c b/drivers/acpi/acpica/evregion.c
index 1b0180a1b79..0cc6a16fedc 100644
--- a/drivers/acpi/acpica/evregion.c
+++ b/drivers/acpi/acpica/evregion.c
@@ -150,7 +150,7 @@ acpi_status acpi_ev_install_region_handlers(void)
  *
  * FUNCTION:    acpi_ev_has_default_handler
  *
- * PARAMETERS:  Node                - Namespace node for the device
+ * PARAMETERS:  node                - Namespace node for the device
  *              space_id            - The address space ID
  *
  * RETURN:      TRUE if default handler is installed, FALSE otherwise
@@ -244,7 +244,7 @@ acpi_status acpi_ev_initialize_op_regions(void)
  * FUNCTION:    acpi_ev_execute_reg_method
  *
  * PARAMETERS:  region_obj          - Region object
- *              Function            - Passed to _REG: On (1) or Off (0)
+ *              function            - Passed to _REG: On (1) or Off (0)
  *
  * RETURN:      Status
  *
@@ -286,10 +286,10 @@ acpi_ev_execute_reg_method(union acpi_operand_object *region_obj, u32 function)
 	/*
 	 * The _REG method has two arguments:
 	 *
-	 * Arg0 - Integer:
+	 * arg0 - Integer:
 	 *  Operation region space ID Same value as region_obj->Region.space_id
 	 *
-	 * Arg1 - Integer:
+	 * arg1 - Integer:
 	 *  connection status 1 for connecting the handler, 0 for disconnecting
 	 *  the handler (Passed as a parameter)
 	 */
@@ -330,10 +330,10 @@ acpi_ev_execute_reg_method(union acpi_operand_object *region_obj, u32 function)
  *
  * PARAMETERS:  region_obj          - Internal region object
  *              field_obj           - Corresponding field. Can be NULL.
- *              Function            - Read or Write operation
+ *              function            - Read or Write operation
  *              region_offset       - Where in the region to read or write
  *              bit_width           - Field width in bits (8, 16, 32, or 64)
- *              Value               - Pointer to in or out value, must be
+ *              value               - Pointer to in or out value, must be
  *                                    a full 64-bit integer
  *
  * RETURN:      Status
@@ -840,11 +840,11 @@ acpi_ev_install_handler(acpi_handle obj_handle,
  *
  * FUNCTION:    acpi_ev_install_space_handler
  *
- * PARAMETERS:  Node            - Namespace node for the device
+ * PARAMETERS:  node            - Namespace node for the device
  *              space_id        - The address space ID
- *              Handler         - Address of the handler
- *              Setup           - Address of the setup function
- *              Context         - Value passed to the handler on each access
+ *              handler         - Address of the handler
+ *              setup           - Address of the setup function
+ *              context         - Value passed to the handler on each access
  *
  * RETURN:      Status
  *
@@ -1061,7 +1061,7 @@ acpi_ev_install_space_handler(struct acpi_namespace_node * node,
  *
  * FUNCTION:    acpi_ev_execute_reg_methods
  *
- * PARAMETERS:  Node            - Namespace node for the device
+ * PARAMETERS:  node            - Namespace node for the device
  *              space_id        - The address space ID
  *
  * RETURN:      Status
@@ -1104,7 +1104,7 @@ acpi_ev_execute_reg_methods(struct acpi_namespace_node *node,
  *
  * PARAMETERS:  walk_namespace callback
  *
- * DESCRIPTION: Run _REG method for region objects of the requested space_iD
+ * DESCRIPTION: Run _REG method for region objects of the requested spaceID
  *
  ******************************************************************************/
 
diff --git a/drivers/acpi/acpica/evrgnini.c b/drivers/acpi/acpica/evrgnini.c
index 819c17f5897..4c1c8261166 100644
--- a/drivers/acpi/acpica/evrgnini.c
+++ b/drivers/acpi/acpica/evrgnini.c
@@ -56,8 +56,8 @@ static u8 acpi_ev_is_pci_root_bridge(struct acpi_namespace_node *node);
  *
  * FUNCTION:    acpi_ev_system_memory_region_setup
  *
- * PARAMETERS:  Handle              - Region we are interested in
- *              Function            - Start or stop
+ * PARAMETERS:  handle              - Region we are interested in
+ *              function            - Start or stop
  *              handler_context     - Address space handler context
  *              region_context      - Region specific context
  *
@@ -118,8 +118,8 @@ acpi_ev_system_memory_region_setup(acpi_handle handle,
  *
  * FUNCTION:    acpi_ev_io_space_region_setup
  *
- * PARAMETERS:  Handle              - Region we are interested in
- *              Function            - Start or stop
+ * PARAMETERS:  handle              - Region we are interested in
+ *              function            - Start or stop
  *              handler_context     - Address space handler context
  *              region_context      - Region specific context
  *
@@ -149,8 +149,8 @@ acpi_ev_io_space_region_setup(acpi_handle handle,
  *
  * FUNCTION:    acpi_ev_pci_config_region_setup
  *
- * PARAMETERS:  Handle              - Region we are interested in
- *              Function            - Start or stop
+ * PARAMETERS:  handle              - Region we are interested in
+ *              function            - Start or stop
  *              handler_context     - Address space handler context
  *              region_context      - Region specific context
  *
@@ -338,7 +338,7 @@ acpi_ev_pci_config_region_setup(acpi_handle handle,
  *
  * FUNCTION:    acpi_ev_is_pci_root_bridge
  *
- * PARAMETERS:  Node            - Device node being examined
+ * PARAMETERS:  node            - Device node being examined
  *
  * RETURN:      TRUE if device is a PCI/PCI-Express Root Bridge
  *
@@ -393,14 +393,14 @@ static u8 acpi_ev_is_pci_root_bridge(struct acpi_namespace_node *node)
  *
  * FUNCTION:    acpi_ev_pci_bar_region_setup
  *
- * PARAMETERS:  Handle              - Region we are interested in
- *              Function            - Start or stop
+ * PARAMETERS:  handle              - Region we are interested in
+ *              function            - Start or stop
  *              handler_context     - Address space handler context
  *              region_context      - Region specific context
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Setup a pci_bAR operation region
+ * DESCRIPTION: Setup a pci_BAR operation region
  *
  * MUTEX:       Assumes namespace is not locked
  *
@@ -420,8 +420,8 @@ acpi_ev_pci_bar_region_setup(acpi_handle handle,
  *
  * FUNCTION:    acpi_ev_cmos_region_setup
  *
- * PARAMETERS:  Handle              - Region we are interested in
- *              Function            - Start or stop
+ * PARAMETERS:  handle              - Region we are interested in
+ *              function            - Start or stop
  *              handler_context     - Address space handler context
  *              region_context      - Region specific context
  *
@@ -447,8 +447,8 @@ acpi_ev_cmos_region_setup(acpi_handle handle,
  *
  * FUNCTION:    acpi_ev_default_region_setup
  *
- * PARAMETERS:  Handle              - Region we are interested in
- *              Function            - Start or stop
+ * PARAMETERS:  handle              - Region we are interested in
+ *              function            - Start or stop
  *              handler_context     - Address space handler context
  *              region_context      - Region specific context
  *
diff --git a/drivers/acpi/acpica/evsci.c b/drivers/acpi/acpica/evsci.c
index 6a57aa2d70d..f9661e2b46a 100644
--- a/drivers/acpi/acpica/evsci.c
+++ b/drivers/acpi/acpica/evsci.c
@@ -56,7 +56,7 @@ static u32 ACPI_SYSTEM_XFACE acpi_ev_sci_xrupt_handler(void *context);
  *
  * FUNCTION:    acpi_ev_sci_xrupt_handler
  *
- * PARAMETERS:  Context   - Calling Context
+ * PARAMETERS:  context   - Calling Context
  *
  * RETURN:      Status code indicates whether interrupt was handled.
  *
@@ -96,7 +96,7 @@ static u32 ACPI_SYSTEM_XFACE acpi_ev_sci_xrupt_handler(void *context)
  *
  * FUNCTION:    acpi_ev_gpe_xrupt_handler
  *
- * PARAMETERS:  Context   - Calling Context
+ * PARAMETERS:  context   - Calling Context
  *
  * RETURN:      Status code indicates whether interrupt was handled.
  *
diff --git a/drivers/acpi/acpica/evxface.c b/drivers/acpi/acpica/evxface.c
index 44bef5744eb..7587eb6c958 100644
--- a/drivers/acpi/acpica/evxface.c
+++ b/drivers/acpi/acpica/evxface.c
@@ -54,86 +54,25 @@ ACPI_MODULE_NAME("evxface")
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_populate_handler_object
- *
- * PARAMETERS:  handler_obj        - Handler object to populate
- *              handler_type       - The type of handler:
- *                                  ACPI_SYSTEM_NOTIFY: system_handler (00-7f)
- *                                  ACPI_DEVICE_NOTIFY: driver_handler (80-ff)
- *                                  ACPI_ALL_NOTIFY:  both system and device
- *              handler            - Address of the handler
- *              context            - Value passed to the handler on each GPE
- *              next               - Address of a handler object to link to
- *
- * RETURN:      None
- *
- * DESCRIPTION: Populate a handler object.
- *
- ******************************************************************************/
-static void
-acpi_populate_handler_object(struct acpi_object_notify_handler *handler_obj,
-			     u32 handler_type,
-			     acpi_notify_handler handler, void *context,
-			     struct acpi_object_notify_handler *next)
-{
-	handler_obj->handler_type = handler_type;
-	handler_obj->handler = handler;
-	handler_obj->context = context;
-	handler_obj->next = next;
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_add_handler_object
- *
- * PARAMETERS:  parent_obj         - Parent of the new object
- *              handler            - Address of the handler
- *              context            - Value passed to the handler on each GPE
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Create a new handler object and populate it.
- *
- ******************************************************************************/
-static acpi_status
-acpi_add_handler_object(struct acpi_object_notify_handler *parent_obj,
-			acpi_notify_handler handler, void *context)
-{
-	struct acpi_object_notify_handler *handler_obj;
-
-	/* The parent must not be a defice notify handler object. */
-	if (parent_obj->handler_type & ACPI_DEVICE_NOTIFY)
-		return AE_BAD_PARAMETER;
-
-	handler_obj = ACPI_ALLOCATE_ZEROED(sizeof(*handler_obj));
-	if (!handler_obj)
-		return AE_NO_MEMORY;
-
-	acpi_populate_handler_object(handler_obj,
-					ACPI_SYSTEM_NOTIFY,
-					handler, context,
-					parent_obj->next);
-	parent_obj->next = handler_obj;
-
-	return AE_OK;
-}
-
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_install_notify_handler
  *
  * PARAMETERS:  Device          - The device for which notifies will be handled
  *              handler_type    - The type of handler:
- *                                  ACPI_SYSTEM_NOTIFY: system_handler (00-7f)
- *                                  ACPI_DEVICE_NOTIFY: driver_handler (80-ff)
- *                                  ACPI_ALL_NOTIFY:  both system and device
+ *                                  ACPI_SYSTEM_NOTIFY: System Handler (00-7F)
+ *                                  ACPI_DEVICE_NOTIFY: Device Handler (80-FF)
+ *                                  ACPI_ALL_NOTIFY:    Both System and Device
  *              Handler         - Address of the handler
  *              Context         - Value passed to the handler on each GPE
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Install a handler for notifies on an ACPI device
+ * DESCRIPTION: Install a handler for notifications on an ACPI Device,
+ *              thermal_zone, or Processor object.
+ *
+ * NOTES:       The Root namespace object may have only one handler for each
+ *              type of notify (System/Device). Device/Thermal/Processor objects
+ *              may have one device notify handler, and multiple system notify
+ *              handlers.
  *
  ******************************************************************************/
 acpi_status
@@ -141,17 +80,19 @@ acpi_install_notify_handler(acpi_handle device,
 			    u32 handler_type,
 			    acpi_notify_handler handler, void *context)
 {
+	struct acpi_namespace_node *node =
+	    ACPI_CAST_PTR(struct acpi_namespace_node, device);
 	union acpi_operand_object *obj_desc;
-	union acpi_operand_object *notify_obj;
-	struct acpi_namespace_node *node;
+	union acpi_operand_object *handler_obj;
 	acpi_status status;
+	u32 i;
 
 	ACPI_FUNCTION_TRACE(acpi_install_notify_handler);
 
 	/* Parameter validation */
 
-	if ((!device) ||
-	    (!handler) || (handler_type > ACPI_MAX_NOTIFY_HANDLER_TYPE)) {
+	if ((!device) || (!handler) || (!handler_type) ||
+	    (handler_type > ACPI_MAX_NOTIFY_HANDLER_TYPE)) {
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
@@ -160,144 +101,112 @@ acpi_install_notify_handler(acpi_handle device,
 		return_ACPI_STATUS(status);
 	}
 
-	/* Convert and validate the device handle */
-
-	node = acpi_ns_validate_handle(device);
-	if (!node) {
-		status = AE_BAD_PARAMETER;
-		goto unlock_and_exit;
-	}
-
 	/*
 	 * Root Object:
 	 * Registering a notify handler on the root object indicates that the
 	 * caller wishes to receive notifications for all objects. Note that
-	 * only one <external> global handler can be regsitered (per notify type).
+	 * only one global handler can be registered per notify type.
+	 * Ensure that a handler is not already installed.
 	 */
 	if (device == ACPI_ROOT_OBJECT) {
+		for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
+			if (handler_type & (i + 1)) {
+				if (acpi_gbl_global_notify[i].handler) {
+					status = AE_ALREADY_EXISTS;
+					goto unlock_and_exit;
+				}
 
-		/* Make sure the handler is not already installed */
-
-		if (((handler_type & ACPI_SYSTEM_NOTIFY) &&
-		     acpi_gbl_system_notify.handler) ||
-		    ((handler_type & ACPI_DEVICE_NOTIFY) &&
-		     acpi_gbl_device_notify.handler)) {
-			status = AE_ALREADY_EXISTS;
-			goto unlock_and_exit;
-		}
-
-		if (handler_type & ACPI_SYSTEM_NOTIFY) {
-			acpi_gbl_system_notify.node = node;
-			acpi_gbl_system_notify.handler = handler;
-			acpi_gbl_system_notify.context = context;
-		}
-
-		if (handler_type & ACPI_DEVICE_NOTIFY) {
-			acpi_gbl_device_notify.node = node;
-			acpi_gbl_device_notify.handler = handler;
-			acpi_gbl_device_notify.context = context;
+				acpi_gbl_global_notify[i].handler = handler;
+				acpi_gbl_global_notify[i].context = context;
+			}
 		}
 
-		/* Global notify handler installed */
+		goto unlock_and_exit;	/* Global notify handler installed, all done */
 	}
 
 	/*
 	 * All Other Objects:
-	 * Caller will only receive notifications specific to the target object.
-	 * Note that only certain object types can receive notifications.
+	 * Caller will only receive notifications specific to the target
+	 * object. Note that only certain object types are allowed to
+	 * receive notifications.
 	 */
-	else {
-		/* Notifies allowed on this object? */
 
-		if (!acpi_ev_is_notify_object(node)) {
-			status = AE_TYPE;
-			goto unlock_and_exit;
-		}
+	/* Are Notifies allowed on this object? */
 
-		/* Check for an existing internal object */
+	if (!acpi_ev_is_notify_object(node)) {
+		status = AE_TYPE;
+		goto unlock_and_exit;
+	}
 
-		obj_desc = acpi_ns_get_attached_object(node);
-		if (obj_desc) {
+	/* Check for an existing internal object, might not exist */
 
-			/* Object exists. */
+	obj_desc = acpi_ns_get_attached_object(node);
+	if (!obj_desc) {
 
-			/* For a device notify, make sure there's no handler. */
-			if ((handler_type & ACPI_DEVICE_NOTIFY) &&
-			     obj_desc->common_notify.device_notify) {
-				status = AE_ALREADY_EXISTS;
-				goto unlock_and_exit;
-			}
+		/* Create a new object */
 
-			/* System notifies may have more handlers installed. */
-			notify_obj = obj_desc->common_notify.system_notify;
+		obj_desc = acpi_ut_create_internal_object(node->type);
+		if (!obj_desc) {
+			status = AE_NO_MEMORY;
+			goto unlock_and_exit;
+		}
 
-			if ((handler_type & ACPI_SYSTEM_NOTIFY) && notify_obj) {
-				struct acpi_object_notify_handler *parent_obj;
+		/* Attach new object to the Node, remove local reference */
+
+		status = acpi_ns_attach_object(device, obj_desc, node->type);
+		acpi_ut_remove_reference(obj_desc);
+		if (ACPI_FAILURE(status)) {
+			goto unlock_and_exit;
+		}
+	}
 
-				if (handler_type & ACPI_DEVICE_NOTIFY) {
+	/* Ensure that the handler is not already installed in the lists */
+
+	for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
+		if (handler_type & (i + 1)) {
+			handler_obj = obj_desc->common_notify.notify_list[i];
+			while (handler_obj) {
+				if (handler_obj->notify.handler == handler) {
 					status = AE_ALREADY_EXISTS;
 					goto unlock_and_exit;
 				}
 
-				parent_obj = &notify_obj->notify;
-				status = acpi_add_handler_object(parent_obj,
-								 handler,
-								 context);
-				goto unlock_and_exit;
-			}
-		} else {
-			/* Create a new object */
-
-			obj_desc = acpi_ut_create_internal_object(node->type);
-			if (!obj_desc) {
-				status = AE_NO_MEMORY;
-				goto unlock_and_exit;
-			}
-
-			/* Attach new object to the Node */
-
-			status =
-			    acpi_ns_attach_object(device, obj_desc, node->type);
-
-			/* Remove local reference to the object */
-
-			acpi_ut_remove_reference(obj_desc);
-			if (ACPI_FAILURE(status)) {
-				goto unlock_and_exit;
+				handler_obj = handler_obj->notify.next[i];
 			}
 		}
+	}
 
-		/* Install the handler */
+	/* Create and populate a new notify handler object */
 
-		notify_obj =
-		    acpi_ut_create_internal_object(ACPI_TYPE_LOCAL_NOTIFY);
-		if (!notify_obj) {
-			status = AE_NO_MEMORY;
-			goto unlock_and_exit;
-		}
+	handler_obj = acpi_ut_create_internal_object(ACPI_TYPE_LOCAL_NOTIFY);
+	if (!handler_obj) {
+		status = AE_NO_MEMORY;
+		goto unlock_and_exit;
+	}
 
-		acpi_populate_handler_object(&notify_obj->notify,
-						handler_type,
-						handler, context,
-						NULL);
+	handler_obj->notify.node = node;
+	handler_obj->notify.handler_type = handler_type;
+	handler_obj->notify.handler = handler;
+	handler_obj->notify.context = context;
 
-		if (handler_type & ACPI_SYSTEM_NOTIFY) {
-			obj_desc->common_notify.system_notify = notify_obj;
-		}
+	/* Install the handler at the list head(s) */
 
-		if (handler_type & ACPI_DEVICE_NOTIFY) {
-			obj_desc->common_notify.device_notify = notify_obj;
-		}
+	for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
+		if (handler_type & (i + 1)) {
+			handler_obj->notify.next[i] =
+			    obj_desc->common_notify.notify_list[i];
 
-		if (handler_type == ACPI_ALL_NOTIFY) {
+			obj_desc->common_notify.notify_list[i] = handler_obj;
+		}
+	}
 
-			/* Extra ref if installed in both */
+	/* Add an extra reference if handler was installed in both lists */
 
-			acpi_ut_add_reference(notify_obj);
-		}
+	if (handler_type == ACPI_ALL_NOTIFY) {
+		acpi_ut_add_reference(handler_obj);
 	}
 
-      unlock_and_exit:
+unlock_and_exit:
 	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
 	return_ACPI_STATUS(status);
 }
@@ -308,11 +217,11 @@ ACPI_EXPORT_SYMBOL(acpi_install_notify_handler)
  *
  * FUNCTION:    acpi_remove_notify_handler
  *
- * PARAMETERS:  Device          - The device for which notifies will be handled
+ * PARAMETERS:  Device          - The device for which the handler is installed
  *              handler_type    - The type of handler:
- *                                  ACPI_SYSTEM_NOTIFY: system_handler (00-7f)
- *                                  ACPI_DEVICE_NOTIFY: driver_handler (80-ff)
- *                                  ACPI_ALL_NOTIFY:  both system and device
+ *                                  ACPI_SYSTEM_NOTIFY: System Handler (00-7F)
+ *                                  ACPI_DEVICE_NOTIFY: Device Handler (80-FF)
+ *                                  ACPI_ALL_NOTIFY:    Both System and Device
  *              Handler         - Address of the handler
  *
  * RETURN:      Status
@@ -324,165 +233,106 @@ acpi_status
 acpi_remove_notify_handler(acpi_handle device,
 			   u32 handler_type, acpi_notify_handler handler)
 {
-	union acpi_operand_object *notify_obj;
+	struct acpi_namespace_node *node =
+	    ACPI_CAST_PTR(struct acpi_namespace_node, device);
 	union acpi_operand_object *obj_desc;
-	struct acpi_namespace_node *node;
+	union acpi_operand_object *handler_obj;
+	union acpi_operand_object *previous_handler_obj;
 	acpi_status status;
+	u32 i;
 
 	ACPI_FUNCTION_TRACE(acpi_remove_notify_handler);
 
 	/* Parameter validation */
 
-	if ((!device) ||
-	    (!handler) || (handler_type > ACPI_MAX_NOTIFY_HANDLER_TYPE)) {
-		status = AE_BAD_PARAMETER;
-		goto exit;
+	if ((!device) || (!handler) || (!handler_type) ||
+	    (handler_type > ACPI_MAX_NOTIFY_HANDLER_TYPE)) {
+		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
-
-
 	/* Make sure all deferred tasks are completed */
-	acpi_os_wait_events_complete(NULL);
+
+	acpi_os_wait_events_complete();
 
 	status = acpi_ut_acquire_mutex(ACPI_MTX_NAMESPACE);
 	if (ACPI_FAILURE(status)) {
-		goto exit;
-	}
-
-	/* Convert and validate the device handle */
-
-	node = acpi_ns_validate_handle(device);
-	if (!node) {
-		status = AE_BAD_PARAMETER;
-		goto unlock_and_exit;
+		return_ACPI_STATUS(status);
 	}
 
-	/* Root Object */
+	/* Root Object. Global handlers are removed here */
 
 	if (device == ACPI_ROOT_OBJECT) {
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "Removing notify handler for namespace root object\n"));
+		for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
+			if (handler_type & (i + 1)) {
+				if (!acpi_gbl_global_notify[i].handler ||
+				    (acpi_gbl_global_notify[i].handler !=
+				     handler)) {
+					status = AE_NOT_EXIST;
+					goto unlock_and_exit;
+				}
 
-		if (((handler_type & ACPI_SYSTEM_NOTIFY) &&
-		     !acpi_gbl_system_notify.handler) ||
-		    ((handler_type & ACPI_DEVICE_NOTIFY) &&
-		     !acpi_gbl_device_notify.handler)) {
-			status = AE_NOT_EXIST;
-			goto unlock_and_exit;
-		}
+				ACPI_DEBUG_PRINT((ACPI_DB_INFO,
+						  "Removing global notify handler\n"));
 
-		if (handler_type & ACPI_SYSTEM_NOTIFY) {
-			acpi_gbl_system_notify.node = NULL;
-			acpi_gbl_system_notify.handler = NULL;
-			acpi_gbl_system_notify.context = NULL;
+				acpi_gbl_global_notify[i].handler = NULL;
+				acpi_gbl_global_notify[i].context = NULL;
+			}
 		}
 
-		if (handler_type & ACPI_DEVICE_NOTIFY) {
-			acpi_gbl_device_notify.node = NULL;
-			acpi_gbl_device_notify.handler = NULL;
-			acpi_gbl_device_notify.context = NULL;
-		}
+		goto unlock_and_exit;
 	}
 
-	/* All Other Objects */
+	/* All other objects: Are Notifies allowed on this object? */
 
-	else {
-		/* Notifies allowed on this object? */
+	if (!acpi_ev_is_notify_object(node)) {
+		status = AE_TYPE;
+		goto unlock_and_exit;
+	}
 
-		if (!acpi_ev_is_notify_object(node)) {
-			status = AE_TYPE;
-			goto unlock_and_exit;
-		}
+	/* Must have an existing internal object */
 
-		/* Check for an existing internal object */
+	obj_desc = acpi_ns_get_attached_object(node);
+	if (!obj_desc) {
+		status = AE_NOT_EXIST;
+		goto unlock_and_exit;
+	}
 
-		obj_desc = acpi_ns_get_attached_object(node);
-		if (!obj_desc) {
-			status = AE_NOT_EXIST;
-			goto unlock_and_exit;
-		}
+	/* Internal object exists. Find the handler and remove it */
 
-		/* Object exists - make sure there's an existing handler */
+	for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
+		if (handler_type & (i + 1)) {
+			handler_obj = obj_desc->common_notify.notify_list[i];
+			previous_handler_obj = NULL;
 
-		if (handler_type & ACPI_SYSTEM_NOTIFY) {
-			struct acpi_object_notify_handler *handler_obj;
-			struct acpi_object_notify_handler *parent_obj;
+			/* Attempt to find the handler in the handler list */
 
-			notify_obj = obj_desc->common_notify.system_notify;
-			if (!notify_obj) {
-				status = AE_NOT_EXIST;
-				goto unlock_and_exit;
-			}
-
-			handler_obj = &notify_obj->notify;
-			parent_obj = NULL;
-			while (handler_obj->handler != handler) {
-				if (handler_obj->next) {
-					parent_obj = handler_obj;
-					handler_obj = handler_obj->next;
-				} else {
-					break;
-				}
+			while (handler_obj &&
+			       (handler_obj->notify.handler != handler)) {
+				previous_handler_obj = handler_obj;
+				handler_obj = handler_obj->notify.next[i];
 			}
 
-			if (handler_obj->handler != handler) {
-				status = AE_BAD_PARAMETER;
+			if (!handler_obj) {
+				status = AE_NOT_EXIST;
 				goto unlock_and_exit;
 			}
 
-			/*
-			 * Remove the handler.  There are three possible cases.
-			 * First, we may need to remove a non-embedded object.
-			 * Second, we may need to remove the embedded object's
-			 * handler data, while non-embedded objects exist.
-			 * Finally, we may need to remove the embedded object
-			 * entirely along with its container.
-			 */
-			if (parent_obj) {
-				/* Non-embedded object is being removed. */
-				parent_obj->next = handler_obj->next;
-				ACPI_FREE(handler_obj);
-			} else if (notify_obj->notify.next) {
-				/*
-				 * The handler matches the embedded object, but
-				 * there are more handler objects in the list.
-				 * Replace the embedded object's data with the
-				 * first next object's data and remove that
-				 * object.
-				 */
-				parent_obj = &notify_obj->notify;
-				handler_obj = notify_obj->notify.next;
-				*parent_obj = *handler_obj;
-				ACPI_FREE(handler_obj);
-			} else {
-				/* No more handler objects in the list. */
-				obj_desc->common_notify.system_notify = NULL;
-				acpi_ut_remove_reference(notify_obj);
-			}
-		}
+			/* Remove the handler object from the list */
 
-		if (handler_type & ACPI_DEVICE_NOTIFY) {
-			notify_obj = obj_desc->common_notify.device_notify;
-			if (!notify_obj) {
-				status = AE_NOT_EXIST;
-				goto unlock_and_exit;
-			}
+			if (previous_handler_obj) {	/* Handler is not at the list head */
+				previous_handler_obj->notify.next[i] =
+				    handler_obj->notify.next[i];
+			} else {	/* Handler is at the list head */
 
-			if (notify_obj->notify.handler != handler) {
-				status = AE_BAD_PARAMETER;
-				goto unlock_and_exit;
+				obj_desc->common_notify.notify_list[i] =
+				    handler_obj->notify.next[i];
 			}
 
-			/* Remove the handler */
-			obj_desc->common_notify.device_notify = NULL;
-			acpi_ut_remove_reference(notify_obj);
+			acpi_ut_remove_reference(handler_obj);
 		}
 	}
 
-      unlock_and_exit:
+unlock_and_exit:
 	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-      exit:
-	if (ACPI_FAILURE(status))
-		ACPI_EXCEPTION((AE_INFO, status, "Removing notify handler"));
 	return_ACPI_STATUS(status);
 }
 
@@ -492,7 +342,7 @@ ACPI_EXPORT_SYMBOL(acpi_remove_notify_handler)
  *
  * FUNCTION:    acpi_install_exception_handler
  *
- * PARAMETERS:  Handler         - Pointer to the handler function for the
+ * PARAMETERS:  handler         - Pointer to the handler function for the
  *                                event
  *
  * RETURN:      Status
@@ -536,8 +386,8 @@ ACPI_EXPORT_SYMBOL(acpi_install_exception_handler)
  *
  * FUNCTION:    acpi_install_global_event_handler
  *
- * PARAMETERS:  Handler         - Pointer to the global event handler function
- *              Context         - Value passed to the handler on each event
+ * PARAMETERS:  handler         - Pointer to the global event handler function
+ *              context         - Value passed to the handler on each event
  *
  * RETURN:      Status
  *
@@ -586,10 +436,10 @@ ACPI_EXPORT_SYMBOL(acpi_install_global_event_handler)
  *
  * FUNCTION:    acpi_install_fixed_event_handler
  *
- * PARAMETERS:  Event           - Event type to enable.
- *              Handler         - Pointer to the handler function for the
+ * PARAMETERS:  event           - Event type to enable.
+ *              handler         - Pointer to the handler function for the
  *                                event
- *              Context         - Value passed to the handler on each GPE
+ *              context         - Value passed to the handler on each GPE
  *
  * RETURN:      Status
  *
@@ -656,8 +506,8 @@ ACPI_EXPORT_SYMBOL(acpi_install_fixed_event_handler)
  *
  * FUNCTION:    acpi_remove_fixed_event_handler
  *
- * PARAMETERS:  Event           - Event type to disable.
- *              Handler         - Address of the handler
+ * PARAMETERS:  event           - Event type to disable.
+ *              handler         - Address of the handler
  *
  * RETURN:      Status
  *
@@ -713,10 +563,10 @@ ACPI_EXPORT_SYMBOL(acpi_remove_fixed_event_handler)
  * PARAMETERS:  gpe_device      - Namespace node for the GPE (NULL for FADT
  *                                defined GPEs)
  *              gpe_number      - The GPE number within the GPE block
- *              Type            - Whether this GPE should be treated as an
+ *              type            - Whether this GPE should be treated as an
  *                                edge- or level-triggered interrupt.
- *              Address         - Address of the handler
- *              Context         - Value passed to the handler on each GPE
+ *              address         - Address of the handler
+ *              context         - Value passed to the handler on each GPE
  *
  * RETURN:      Status
  *
@@ -823,7 +673,7 @@ ACPI_EXPORT_SYMBOL(acpi_install_gpe_handler)
  * PARAMETERS:  gpe_device      - Namespace node for the GPE (NULL for FADT
  *                                defined GPEs)
  *              gpe_number      - The event to remove a handler
- *              Address         - Address of the handler
+ *              address         - Address of the handler
  *
  * RETURN:      Status
  *
@@ -849,7 +699,7 @@ acpi_remove_gpe_handler(acpi_handle gpe_device,
 
 	/* Make sure all deferred tasks are completed */
 
-	acpi_os_wait_events_complete(NULL);
+	acpi_os_wait_events_complete();
 
 	status = acpi_ut_acquire_mutex(ACPI_MTX_EVENTS);
 	if (ACPI_FAILURE(status)) {
@@ -919,8 +769,8 @@ ACPI_EXPORT_SYMBOL(acpi_remove_gpe_handler)
  *
  * FUNCTION:    acpi_acquire_global_lock
  *
- * PARAMETERS:  Timeout         - How long the caller is willing to wait
- *              Handle          - Where the handle to the lock is returned
+ * PARAMETERS:  timeout         - How long the caller is willing to wait
+ *              handle          - Where the handle to the lock is returned
  *                                (if acquired)
  *
  * RETURN:      Status
@@ -967,7 +817,7 @@ ACPI_EXPORT_SYMBOL(acpi_acquire_global_lock)
  *
  * FUNCTION:    acpi_release_global_lock
  *
- * PARAMETERS:  Handle      - Returned from acpi_acquire_global_lock
+ * PARAMETERS:  handle      - Returned from acpi_acquire_global_lock
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/evxfevnt.c b/drivers/acpi/acpica/evxfevnt.c
index 77cee5a5e89..35520c6eeef 100644
--- a/drivers/acpi/acpica/evxfevnt.c
+++ b/drivers/acpi/acpica/evxfevnt.c
@@ -153,8 +153,8 @@ ACPI_EXPORT_SYMBOL(acpi_disable)
  *
  * FUNCTION:    acpi_enable_event
  *
- * PARAMETERS:  Event           - The fixed eventto be enabled
- *              Flags           - Reserved
+ * PARAMETERS:  event           - The fixed eventto be enabled
+ *              flags           - Reserved
  *
  * RETURN:      Status
  *
@@ -265,7 +265,7 @@ ACPI_EXPORT_SYMBOL(acpi_disable_event)
  *
  * FUNCTION:    acpi_clear_event
  *
- * PARAMETERS:  Event           - The fixed event to be cleared
+ * PARAMETERS:  event           - The fixed event to be cleared
  *
  * RETURN:      Status
  *
@@ -301,7 +301,7 @@ ACPI_EXPORT_SYMBOL(acpi_clear_event)
  *
  * FUNCTION:    acpi_get_event_status
  *
- * PARAMETERS:  Event           - The fixed event
+ * PARAMETERS:  event           - The fixed event
  *              event_status    - Where the current status of the event will
  *                                be returned
  *
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 86f9b343ebd..6affbdb4b88 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -197,12 +197,12 @@ acpi_status
 acpi_setup_gpe_for_wake(acpi_handle wake_device,
 			acpi_handle gpe_device, u32 gpe_number)
 {
-	acpi_status status = AE_BAD_PARAMETER;
+	acpi_status status;
 	struct acpi_gpe_event_info *gpe_event_info;
 	struct acpi_namespace_node *device_node;
-	struct acpi_gpe_notify_object *notify_object;
+	struct acpi_gpe_notify_info *notify;
+	struct acpi_gpe_notify_info *new_notify;
 	acpi_cpu_flags flags;
-	u8 gpe_dispatch_mask;
 
 	ACPI_FUNCTION_TRACE(acpi_setup_gpe_for_wake);
 
@@ -216,63 +216,95 @@ acpi_setup_gpe_for_wake(acpi_handle wake_device,
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
+	/* Handle root object case */
+
+	if (wake_device == ACPI_ROOT_OBJECT) {
+		device_node = acpi_gbl_root_node;
+	} else {
+		device_node = ACPI_CAST_PTR(struct acpi_namespace_node, wake_device);
+	}
+
+	/* Validate WakeDevice is of type Device */
+
+	if (device_node->type != ACPI_TYPE_DEVICE) {
+		return_ACPI_STATUS (AE_BAD_PARAMETER);
+	}
+
+	/*
+	 * Allocate a new notify object up front, in case it is needed.
+	 * Memory allocation while holding a spinlock is a big no-no
+	 * on some hosts.
+	 */
+	new_notify = ACPI_ALLOCATE_ZEROED(sizeof(struct acpi_gpe_notify_info));
+	if (!new_notify) {
+		return_ACPI_STATUS(AE_NO_MEMORY);
+	}
+
 	flags = acpi_os_acquire_lock(acpi_gbl_gpe_lock);
 
 	/* Ensure that we have a valid GPE number */
 
 	gpe_event_info = acpi_ev_get_gpe_event_info(gpe_device, gpe_number);
 	if (!gpe_event_info) {
+		status = AE_BAD_PARAMETER;
 		goto unlock_and_exit;
 	}
 
-	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
+	 * wake_device will be notified whenever this GPE fires. This is
+	 * known as an "implicit notify". Note: The GPE is assumed to be
 	 * level-triggered (for windows compatibility).
 	 */
-	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 */
-
-	device_node = ACPI_CAST_PTR(struct acpi_namespace_node, wake_device);
-	if (device_node->type != ACPI_TYPE_DEVICE) {
-		goto unlock_and_exit;
+	if ((gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK) ==
+	    ACPI_GPE_DISPATCH_NONE) {
+		/*
+		 * This is the first device for implicit notify on this GPE.
+		 * Just set the flags here, and enter the NOTIFY block below.
+		 */
+		gpe_event_info->flags =
+		    (ACPI_GPE_DISPATCH_NOTIFY | ACPI_GPE_LEVEL_TRIGGERED);
 	}
 
-	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.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;
+	/*
+	 * If we already have an implicit notify on this GPE, add
+	 * this device to the notify list.
+	 */
+	if ((gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK) ==
+	    ACPI_GPE_DISPATCH_NOTIFY) {
+
+		/* Ensure that the device is not already in the list */
+
+		notify = gpe_event_info->dispatch.notify_list;
+		while (notify) {
+			if (notify->device_node == device_node) {
+				status = AE_ALREADY_EXISTS;
+				goto unlock_and_exit;
+			}
+			notify = notify->next;
 		}
 
-		notify_object->node = device_node;
-		notify_object->next = gpe_event_info->dispatch.device.next;
-		gpe_event_info->dispatch.device.next = notify_object;
+		/* Add this device to the notify list for this GPE */
+
+		new_notify->device_node = device_node;
+		new_notify->next = gpe_event_info->dispatch.notify_list;
+		gpe_event_info->dispatch.notify_list = new_notify;
+		new_notify = NULL;
 	}
 
- out:
+	/* Mark the GPE as a possible wake event */
+
 	gpe_event_info->flags |= ACPI_GPE_CAN_WAKE;
 	status = AE_OK;
 
- unlock_and_exit:
+unlock_and_exit:
 	acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
+
+	/* Delete the notify object if it was not used above */
+
+	if (new_notify) {
+		ACPI_FREE(new_notify);
+	}
 	return_ACPI_STATUS(status);
 }
 ACPI_EXPORT_SYMBOL(acpi_setup_gpe_for_wake)
@@ -283,7 +315,7 @@ ACPI_EXPORT_SYMBOL(acpi_setup_gpe_for_wake)
  *
  * PARAMETERS:  gpe_device      - Parent GPE Device. NULL for GPE0/GPE1
  *              gpe_number      - GPE level within the GPE block
- *              Action          - Enable or Disable
+ *              action              - Enable or Disable
  *
  * RETURN:      Status
  *
@@ -508,7 +540,7 @@ ACPI_EXPORT_SYMBOL(acpi_enable_all_runtime_gpes)
  * FUNCTION:    acpi_install_gpe_block
  *
  * PARAMETERS:  gpe_device          - Handle to the parent GPE Block Device
- *              gpe_block_address   - Address and space_iD
+ *              gpe_block_address   - Address and space_ID
  *              register_count      - Number of GPE register pairs in the block
  *              interrupt_number    - H/W interrupt for the block
  *
@@ -653,7 +685,7 @@ ACPI_EXPORT_SYMBOL(acpi_remove_gpe_block)
  *
  * FUNCTION:    acpi_get_gpe_device
  *
- * PARAMETERS:  Index               - System GPE index (0-current_gpe_count)
+ * PARAMETERS:  index               - System GPE index (0-current_gpe_count)
  *              gpe_device          - Where the parent GPE Device is returned
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/evxfregn.c b/drivers/acpi/acpica/evxfregn.c
index 6019208cd4b..96b412d0395 100644
--- a/drivers/acpi/acpica/evxfregn.c
+++ b/drivers/acpi/acpica/evxfregn.c
@@ -55,11 +55,11 @@ ACPI_MODULE_NAME("evxfregn")
  *
  * FUNCTION:    acpi_install_address_space_handler
  *
- * PARAMETERS:  Device          - Handle for the device
+ * PARAMETERS:  device          - Handle for the device
  *              space_id        - The address space ID
- *              Handler         - Address of the handler
- *              Setup           - Address of the setup function
- *              Context         - Value passed to the handler on each access
+ *              handler         - Address of the handler
+ *              setup           - Address of the setup function
+ *              context         - Value passed to the handler on each access
  *
  * RETURN:      Status
  *
@@ -112,16 +112,16 @@ acpi_install_address_space_handler(acpi_handle device,
 	}
 
 	/*
-	 * For the default space_iDs, (the IDs for which there are default region handlers
+	 * For the default space_IDs, (the IDs for which there are default region handlers
 	 * installed) Only execute the _REG methods if the global initialization _REG
 	 * methods have already been run (via acpi_initialize_objects). In other words,
-	 * we will defer the execution of the _REG methods for these space_iDs until
+	 * we will defer the execution of the _REG methods for these space_IDs until
 	 * execution of acpi_initialize_objects. This is done because we need the handlers
 	 * for the default spaces (mem/io/pci/table) to be installed before we can run
 	 * any control methods (or _REG methods). There is known BIOS code that depends
 	 * on this.
 	 *
-	 * For all other space_iDs, we can safely execute the _REG methods immediately.
+	 * For all other space_IDs, we can safely execute the _REG methods immediately.
 	 * This means that for IDs like embedded_controller, this function should be called
 	 * only after acpi_enable_subsystem has been called.
 	 */
@@ -157,9 +157,9 @@ ACPI_EXPORT_SYMBOL(acpi_install_address_space_handler)
  *
  * FUNCTION:    acpi_remove_address_space_handler
  *
- * PARAMETERS:  Device          - Handle for the device
+ * PARAMETERS:  device          - Handle for the device
  *              space_id        - The address space ID
- *              Handler         - Address of the handler
+ *              handler         - Address of the handler
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/exconfig.c b/drivers/acpi/acpica/exconfig.c
index c86d44e41bc..16219bde48d 100644
--- a/drivers/acpi/acpica/exconfig.c
+++ b/drivers/acpi/acpica/exconfig.c
@@ -66,7 +66,7 @@ acpi_ex_region_read(union acpi_operand_object *obj_desc,
  *
  * FUNCTION:    acpi_ex_add_table
  *
- * PARAMETERS:  Table               - Pointer to raw table
+ * PARAMETERS:  table               - Pointer to raw table
  *              parent_node         - Where to load the table (scope)
  *              ddb_handle          - Where to return the table handle.
  *
@@ -276,8 +276,8 @@ acpi_ex_load_table_op(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ex_region_read
  *
  * PARAMETERS:  obj_desc        - Region descriptor
- *              Length          - Number of bytes to read
- *              Buffer          - Pointer to where to put the data
+ *              length          - Number of bytes to read
+ *              buffer          - Pointer to where to put the data
  *
  * RETURN:      Status
  *
@@ -318,7 +318,7 @@ acpi_ex_region_read(union acpi_operand_object *obj_desc, u32 length, u8 *buffer)
  *
  * PARAMETERS:  obj_desc        - Region or Buffer/Field where the table will be
  *                                obtained
- *              Target          - Where a handle to the table will be stored
+ *              target          - Where a handle to the table will be stored
  *              walk_state      - Current state
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/exconvrt.c b/drivers/acpi/acpica/exconvrt.c
index e385436bd42..bfb062e4c4b 100644
--- a/drivers/acpi/acpica/exconvrt.c
+++ b/drivers/acpi/acpica/exconvrt.c
@@ -60,7 +60,7 @@ acpi_ex_convert_to_ascii(u64 integer, u16 base, u8 *string, u8 max_length);
  * PARAMETERS:  obj_desc        - Object to be converted. Must be an
  *                                Integer, Buffer, or String
  *              result_desc     - Where the new Integer object is returned
- *              Flags           - Used for string conversion
+ *              flags           - Used for string conversion
  *
  * RETURN:      Status
  *
@@ -272,9 +272,9 @@ acpi_ex_convert_to_buffer(union acpi_operand_object *obj_desc,
  *
  * FUNCTION:    acpi_ex_convert_to_ascii
  *
- * PARAMETERS:  Integer         - Value to be converted
- *              Base            - ACPI_STRING_DECIMAL or ACPI_STRING_HEX
- *              String          - Where the string is returned
+ * PARAMETERS:  integer         - Value to be converted
+ *              base            - ACPI_STRING_DECIMAL or ACPI_STRING_HEX
+ *              string          - Where the string is returned
  *              data_width      - Size of data item to be converted, in bytes
  *
  * RETURN:      Actual string length
@@ -385,7 +385,7 @@ acpi_ex_convert_to_ascii(u64 integer, u16 base, u8 *string, u8 data_width)
  * PARAMETERS:  obj_desc        - Object to be converted. Must be an
  *                                Integer, Buffer, or String
  *              result_desc     - Where the string object is returned
- *              Type            - String flags (base and conversion type)
+ *              type            - String flags (base and conversion type)
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/excreate.c b/drivers/acpi/acpica/excreate.c
index 3f5bc998c1c..691d4763102 100644
--- a/drivers/acpi/acpica/excreate.c
+++ b/drivers/acpi/acpica/excreate.c
@@ -369,7 +369,7 @@ acpi_ex_create_region(u8 * aml_start,
  *
  * DESCRIPTION: Create a new processor object and populate the fields
  *
- *              Processor (Name[0], cpu_iD[1], pblock_addr[2], pblock_length[3])
+ *              Processor (Name[0], cpu_ID[1], pblock_addr[2], pblock_length[3])
  *
  ******************************************************************************/
 
diff --git a/drivers/acpi/acpica/exdebug.c b/drivers/acpi/acpica/exdebug.c
index e211e9c1921..bc5b9a6a131 100644
--- a/drivers/acpi/acpica/exdebug.c
+++ b/drivers/acpi/acpica/exdebug.c
@@ -54,8 +54,8 @@ ACPI_MODULE_NAME("exdebug")
  * FUNCTION:    acpi_ex_do_debug_object
  *
  * PARAMETERS:  source_desc         - Object to be output to "Debug Object"
- *              Level               - Indentation level (used for packages)
- *              Index               - Current package element, zero if not pkg
+ *              level               - Indentation level (used for packages)
+ *              index               - Current package element, zero if not pkg
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/exdump.c b/drivers/acpi/acpica/exdump.c
index 2a6ac0a3bc1..213c081776f 100644
--- a/drivers/acpi/acpica/exdump.c
+++ b/drivers/acpi/acpica/exdump.c
@@ -109,9 +109,9 @@ static struct acpi_exdump_info acpi_ex_dump_package[5] = {
 static struct acpi_exdump_info acpi_ex_dump_device[4] = {
 	{ACPI_EXD_INIT, ACPI_EXD_TABLE_SIZE(acpi_ex_dump_device), NULL},
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(device.handler), "Handler"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(device.system_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(device.notify_list[0]),
 	 "System Notify"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(device.device_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(device.notify_list[1]),
 	 "Device Notify"}
 };
 
@@ -158,9 +158,9 @@ static struct acpi_exdump_info acpi_ex_dump_power[5] = {
 	 "System Level"},
 	{ACPI_EXD_UINT32, ACPI_EXD_OFFSET(power_resource.resource_order),
 	 "Resource Order"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(power_resource.system_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(power_resource.notify_list[0]),
 	 "System Notify"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(power_resource.device_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(power_resource.notify_list[1]),
 	 "Device Notify"}
 };
 
@@ -169,18 +169,18 @@ static struct acpi_exdump_info acpi_ex_dump_processor[7] = {
 	{ACPI_EXD_UINT8, ACPI_EXD_OFFSET(processor.proc_id), "Processor ID"},
 	{ACPI_EXD_UINT8, ACPI_EXD_OFFSET(processor.length), "Length"},
 	{ACPI_EXD_ADDRESS, ACPI_EXD_OFFSET(processor.address), "Address"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(processor.system_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(processor.notify_list[0]),
 	 "System Notify"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(processor.device_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(processor.notify_list[1]),
 	 "Device Notify"},
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(processor.handler), "Handler"}
 };
 
 static struct acpi_exdump_info acpi_ex_dump_thermal[4] = {
 	{ACPI_EXD_INIT, ACPI_EXD_TABLE_SIZE(acpi_ex_dump_thermal), NULL},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(thermal_zone.system_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(thermal_zone.notify_list[0]),
 	 "System Notify"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(thermal_zone.device_notify),
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(thermal_zone.notify_list[1]),
 	 "Device Notify"},
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(thermal_zone.handler), "Handler"}
 };
@@ -241,10 +241,15 @@ static struct acpi_exdump_info acpi_ex_dump_address_handler[6] = {
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(address_space.context), "Context"}
 };
 
-static struct acpi_exdump_info acpi_ex_dump_notify[3] = {
+static struct acpi_exdump_info acpi_ex_dump_notify[7] = {
 	{ACPI_EXD_INIT, ACPI_EXD_TABLE_SIZE(acpi_ex_dump_notify), NULL},
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(notify.node), "Node"},
-	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(notify.context), "Context"}
+	{ACPI_EXD_UINT32, ACPI_EXD_OFFSET(notify.handler_type), "Handler Type"},
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(notify.handler), "Handler"},
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(notify.context), "Context"},
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(notify.next[0]),
+	 "Next System Notify"},
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(notify.next[1]), "Next Device Notify"}
 };
 
 /* Miscellaneous tables */
@@ -318,7 +323,7 @@ static struct acpi_exdump_info *acpi_ex_dump_info[] = {
  * FUNCTION:    acpi_ex_dump_object
  *
  * PARAMETERS:  obj_desc            - Descriptor to dump
- *              Info                - Info table corresponding to this object
+ *              info                - Info table corresponding to this object
  *                                    type
  *
  * RETURN:      None
@@ -444,7 +449,7 @@ acpi_ex_dump_object(union acpi_operand_object *obj_desc,
  * FUNCTION:    acpi_ex_dump_operand
  *
  * PARAMETERS:  *obj_desc       - Pointer to entry to be dumped
- *              Depth           - Current nesting depth
+ *              depth           - Current nesting depth
  *
  * RETURN:      None
  *
@@ -726,7 +731,7 @@ void acpi_ex_dump_operand(union acpi_operand_object *obj_desc, u32 depth)
  *
  * FUNCTION:    acpi_ex_dump_operands
  *
- * PARAMETERS:	Operands	    - A list of Operand objects
+ * PARAMETERS:  operands            - A list of Operand objects
  *		opcode_name	    - AML opcode name
  *		num_operands	    - Operand count for this opcode
  *
@@ -769,8 +774,8 @@ acpi_ex_dump_operands(union acpi_operand_object **operands,
  *
  * FUNCTION:    acpi_ex_out* functions
  *
- * PARAMETERS:  Title               - Descriptive text
- *              Value               - Value to be displayed
+ * PARAMETERS:  title               - Descriptive text
+ *              value               - Value to be displayed
  *
  * DESCRIPTION: Object dump output formatting functions.  These functions
  *              reduce the number of format strings required and keeps them
@@ -792,8 +797,8 @@ static void acpi_ex_out_pointer(char *title, void *value)
  *
  * FUNCTION:    acpi_ex_dump_namespace_node
  *
- * PARAMETERS:  Node                - Descriptor to dump
- *              Flags               - Force display if TRUE
+ * PARAMETERS:  node                - Descriptor to dump
+ *              flags               - Force display if TRUE
  *
  * DESCRIPTION: Dumps the members of the given.Node
  *
@@ -825,7 +830,7 @@ void acpi_ex_dump_namespace_node(struct acpi_namespace_node *node, u32 flags)
  *
  * FUNCTION:    acpi_ex_dump_reference_obj
  *
- * PARAMETERS:  Object              - Descriptor to dump
+ * PARAMETERS:  object              - Descriptor to dump
  *
  * DESCRIPTION: Dumps a reference object
  *
@@ -882,8 +887,8 @@ static void acpi_ex_dump_reference_obj(union acpi_operand_object *obj_desc)
  * FUNCTION:    acpi_ex_dump_package_obj
  *
  * PARAMETERS:  obj_desc            - Descriptor to dump
- *              Level               - Indentation Level
- *              Index               - Package index for this object
+ *              level               - Indentation Level
+ *              index               - Package index for this object
  *
  * DESCRIPTION: Dumps the elements of the package
  *
@@ -926,9 +931,7 @@ acpi_ex_dump_package_obj(union acpi_operand_object *obj_desc,
 	case ACPI_TYPE_STRING:
 
 		acpi_os_printf("[String] Value: ");
-		for (i = 0; i < obj_desc->string.length; i++) {
-			acpi_os_printf("%c", obj_desc->string.pointer[i]);
-		}
+		acpi_ut_print_string(obj_desc->string.pointer, ACPI_UINT8_MAX);
 		acpi_os_printf("\n");
 		break;
 
@@ -977,7 +980,7 @@ acpi_ex_dump_package_obj(union acpi_operand_object *obj_desc,
  * FUNCTION:    acpi_ex_dump_object_descriptor
  *
  * PARAMETERS:  obj_desc            - Descriptor to dump
- *              Flags               - Force display if TRUE
+ *              flags               - Force display if TRUE
  *
  * DESCRIPTION: Dumps the members of the object descriptor given.
  *
diff --git a/drivers/acpi/acpica/exfldio.c b/drivers/acpi/acpica/exfldio.c
index 149de45fdad..a7784152ed3 100644
--- a/drivers/acpi/acpica/exfldio.c
+++ b/drivers/acpi/acpica/exfldio.c
@@ -222,9 +222,9 @@ acpi_ex_setup_region(union acpi_operand_object *obj_desc,
  * PARAMETERS:  obj_desc                - Field to be read
  *              field_datum_byte_offset - Byte offset of this datum within the
  *                                        parent field
- *              Value                   - Where to store value (must at least
+ *              value                   - Where to store value (must at least
  *                                        64 bits)
- *              Function                - Read or Write flag plus other region-
+ *              function                - Read or Write flag plus other region-
  *                                        dependent flags
  *
  * RETURN:      Status
@@ -315,7 +315,7 @@ acpi_ex_access_region(union acpi_operand_object *obj_desc,
  * FUNCTION:    acpi_ex_register_overflow
  *
  * PARAMETERS:  obj_desc                - Register(Field) to be written
- *              Value                   - Value to be stored
+ *              value                   - Value to be stored
  *
  * RETURN:      TRUE if value overflows the field, FALSE otherwise
  *
@@ -365,7 +365,7 @@ acpi_ex_register_overflow(union acpi_operand_object *obj_desc, u64 value)
  * PARAMETERS:  obj_desc                - Field to be read
  *              field_datum_byte_offset - Byte offset of this datum within the
  *                                        parent field
- *              Value                   - Where to store value (must be 64 bits)
+ *              value                   - Where to store value (must be 64 bits)
  *              read_write              - Read or Write flag
  *
  * RETURN:      Status
@@ -574,7 +574,7 @@ acpi_ex_field_datum_io(union acpi_operand_object *obj_desc,
  * FUNCTION:    acpi_ex_write_with_update_rule
  *
  * PARAMETERS:  obj_desc                - Field to be written
- *              Mask                    - bitmask within field datum
+ *              mask                    - bitmask within field datum
  *              field_value             - Value to write
  *              field_datum_byte_offset - Offset of datum within field
  *
@@ -678,7 +678,7 @@ acpi_ex_write_with_update_rule(union acpi_operand_object *obj_desc,
  * FUNCTION:    acpi_ex_extract_from_field
  *
  * PARAMETERS:  obj_desc            - Field to be read
- *              Buffer              - Where to store the field data
+ *              buffer              - Where to store the field data
  *              buffer_length       - Length of Buffer
  *
  * RETURN:      Status
@@ -823,7 +823,7 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
  * FUNCTION:    acpi_ex_insert_into_field
  *
  * PARAMETERS:  obj_desc            - Field to be written
- *              Buffer              - Data to be written
+ *              buffer              - Data to be written
  *              buffer_length       - Length of Buffer
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/exmisc.c b/drivers/acpi/acpica/exmisc.c
index 0a089331034..271c0c57ea1 100644
--- a/drivers/acpi/acpica/exmisc.c
+++ b/drivers/acpi/acpica/exmisc.c
@@ -144,8 +144,8 @@ acpi_ex_get_object_reference(union acpi_operand_object *obj_desc,
  *
  * FUNCTION:    acpi_ex_concat_template
  *
- * PARAMETERS:  Operand0            - First source object
- *              Operand1            - Second source object
+ * PARAMETERS:  operand0            - First source object
+ *              operand1            - Second source object
  *              actual_return_desc  - Where to place the return object
  *              walk_state          - Current walk state
  *
@@ -229,8 +229,8 @@ acpi_ex_concat_template(union acpi_operand_object *operand0,
  *
  * FUNCTION:    acpi_ex_do_concatenate
  *
- * PARAMETERS:  Operand0            - First source object
- *              Operand1            - Second source object
+ * PARAMETERS:  operand0            - First source object
+ *              operand1            - Second source object
  *              actual_return_desc  - Where to place the return object
  *              walk_state          - Current walk state
  *
@@ -397,9 +397,9 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
  *
  * FUNCTION:    acpi_ex_do_math_op
  *
- * PARAMETERS:  Opcode              - AML opcode
- *              Integer0            - Integer operand #0
- *              Integer1            - Integer operand #1
+ * PARAMETERS:  opcode              - AML opcode
+ *              integer0            - Integer operand #0
+ *              integer1            - Integer operand #1
  *
  * RETURN:      Integer result of the operation
  *
@@ -479,9 +479,9 @@ u64 acpi_ex_do_math_op(u16 opcode, u64 integer0, u64 integer1)
  *
  * FUNCTION:    acpi_ex_do_logical_numeric_op
  *
- * PARAMETERS:  Opcode              - AML opcode
- *              Integer0            - Integer operand #0
- *              Integer1            - Integer operand #1
+ * PARAMETERS:  opcode              - AML opcode
+ *              integer0            - Integer operand #0
+ *              integer1            - Integer operand #1
  *              logical_result      - TRUE/FALSE result of the operation
  *
  * RETURN:      Status
@@ -534,9 +534,9 @@ acpi_ex_do_logical_numeric_op(u16 opcode,
  *
  * FUNCTION:    acpi_ex_do_logical_op
  *
- * PARAMETERS:  Opcode              - AML opcode
- *              Operand0            - operand #0
- *              Operand1            - operand #1
+ * PARAMETERS:  opcode              - AML opcode
+ *              operand0            - operand #0
+ *              operand1            - operand #1
  *              logical_result      - TRUE/FALSE result of the operation
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/exmutex.c b/drivers/acpi/acpica/exmutex.c
index 60933e9dc3c..bcceda5be9e 100644
--- a/drivers/acpi/acpica/exmutex.c
+++ b/drivers/acpi/acpica/exmutex.c
@@ -102,7 +102,7 @@ void acpi_ex_unlink_mutex(union acpi_operand_object *obj_desc)
  * FUNCTION:    acpi_ex_link_mutex
  *
  * PARAMETERS:  obj_desc            - The mutex to be linked
- *              Thread              - Current executing thread object
+ *              thread              - Current executing thread object
  *
  * RETURN:      None
  *
@@ -138,7 +138,7 @@ acpi_ex_link_mutex(union acpi_operand_object *obj_desc,
  *
  * FUNCTION:    acpi_ex_acquire_mutex_object
  *
- * PARAMETERS:  Timeout             - Timeout in milliseconds
+ * PARAMETERS:  timeout             - Timeout in milliseconds
  *              obj_desc            - Mutex object
  *              thread_id           - Current thread state
  *
@@ -443,7 +443,7 @@ acpi_ex_release_mutex(union acpi_operand_object *obj_desc,
  *
  * FUNCTION:    acpi_ex_release_all_mutexes
  *
- * PARAMETERS:  Thread              - Current executing thread object
+ * PARAMETERS:  thread              - Current executing thread object
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/exprep.c b/drivers/acpi/acpica/exprep.c
index 30157f5a12d..81eca60d274 100644
--- a/drivers/acpi/acpica/exprep.c
+++ b/drivers/acpi/acpica/exprep.c
@@ -391,12 +391,12 @@ acpi_ex_prep_common_field_object(union acpi_operand_object *obj_desc,
  *
  * FUNCTION:    acpi_ex_prep_field_value
  *
- * PARAMETERS:  Info    - Contains all field creation info
+ * PARAMETERS:  info    - Contains all field creation info
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Construct a union acpi_operand_object of type def_field and
- *              connect it to the parent Node.
+ * DESCRIPTION: Construct an object of type union acpi_operand_object with a
+ *              subtype of def_field and connect it to the parent Node.
  *
  ******************************************************************************/
 
diff --git a/drivers/acpi/acpica/exregion.c b/drivers/acpi/acpica/exregion.c
index 12d51df6d3b..1f1ce0c3d2f 100644
--- a/drivers/acpi/acpica/exregion.c
+++ b/drivers/acpi/acpica/exregion.c
@@ -53,10 +53,10 @@ ACPI_MODULE_NAME("exregion")
  *
  * FUNCTION:    acpi_ex_system_memory_space_handler
  *
- * PARAMETERS:  Function            - Read or Write operation
- *              Address             - Where in the space to read or write
+ * PARAMETERS:  function            - Read or Write operation
+ *              address             - Where in the space to read or write
  *              bit_width           - Field width in bits (8, 16, or 32)
- *              Value               - Pointer to in or out value
+ *              value               - Pointer to in or out value
  *              handler_context     - Pointer to Handler's context
  *              region_context      - Pointer to context specific to the
  *                                    accessed region
@@ -270,10 +270,10 @@ acpi_ex_system_memory_space_handler(u32 function,
  *
  * FUNCTION:    acpi_ex_system_io_space_handler
  *
- * PARAMETERS:  Function            - Read or Write operation
- *              Address             - Where in the space to read or write
+ * PARAMETERS:  function            - Read or Write operation
+ *              address             - Where in the space to read or write
  *              bit_width           - Field width in bits (8, 16, or 32)
- *              Value               - Pointer to in or out value
+ *              value               - Pointer to in or out value
  *              handler_context     - Pointer to Handler's context
  *              region_context      - Pointer to context specific to the
  *                                    accessed region
@@ -329,10 +329,10 @@ acpi_ex_system_io_space_handler(u32 function,
  *
  * FUNCTION:    acpi_ex_pci_config_space_handler
  *
- * PARAMETERS:  Function            - Read or Write operation
- *              Address             - Where in the space to read or write
+ * PARAMETERS:  function            - Read or Write operation
+ *              address             - Where in the space to read or write
  *              bit_width           - Field width in bits (8, 16, or 32)
- *              Value               - Pointer to in or out value
+ *              value               - Pointer to in or out value
  *              handler_context     - Pointer to Handler's context
  *              region_context      - Pointer to context specific to the
  *                                    accessed region
@@ -365,7 +365,7 @@ acpi_ex_pci_config_space_handler(u32 function,
 	 *  pci_function is the PCI device function number
 	 *  pci_register is the Config space register range 0-255 bytes
 	 *
-	 *  Value - input value for write, output address for read
+	 *  value - input value for write, output address for read
 	 *
 	 */
 	pci_id = (struct acpi_pci_id *)region_context;
@@ -402,10 +402,10 @@ acpi_ex_pci_config_space_handler(u32 function,
  *
  * FUNCTION:    acpi_ex_cmos_space_handler
  *
- * PARAMETERS:  Function            - Read or Write operation
- *              Address             - Where in the space to read or write
+ * PARAMETERS:  function            - Read or Write operation
+ *              address             - Where in the space to read or write
  *              bit_width           - Field width in bits (8, 16, or 32)
- *              Value               - Pointer to in or out value
+ *              value               - Pointer to in or out value
  *              handler_context     - Pointer to Handler's context
  *              region_context      - Pointer to context specific to the
  *                                    accessed region
@@ -434,10 +434,10 @@ acpi_ex_cmos_space_handler(u32 function,
  *
  * FUNCTION:    acpi_ex_pci_bar_space_handler
  *
- * PARAMETERS:  Function            - Read or Write operation
- *              Address             - Where in the space to read or write
+ * PARAMETERS:  function            - Read or Write operation
+ *              address             - Where in the space to read or write
  *              bit_width           - Field width in bits (8, 16, or 32)
- *              Value               - Pointer to in or out value
+ *              value               - Pointer to in or out value
  *              handler_context     - Pointer to Handler's context
  *              region_context      - Pointer to context specific to the
  *                                    accessed region
@@ -466,10 +466,10 @@ acpi_ex_pci_bar_space_handler(u32 function,
  *
  * FUNCTION:    acpi_ex_data_table_space_handler
  *
- * PARAMETERS:  Function            - Read or Write operation
- *              Address             - Where in the space to read or write
+ * PARAMETERS:  function            - Read or Write operation
+ *              address             - Where in the space to read or write
  *              bit_width           - Field width in bits (8, 16, or 32)
- *              Value               - Pointer to in or out value
+ *              value               - Pointer to in or out value
  *              handler_context     - Pointer to Handler's context
  *              region_context      - Pointer to context specific to the
  *                                    accessed region
diff --git a/drivers/acpi/acpica/exresolv.c b/drivers/acpi/acpica/exresolv.c
index 6e335dc3452..bbf40ac2758 100644
--- a/drivers/acpi/acpica/exresolv.c
+++ b/drivers/acpi/acpica/exresolv.c
@@ -147,7 +147,7 @@ acpi_ex_resolve_object_to_value(union acpi_operand_object **stack_ptr,
 
 	stack_desc = *stack_ptr;
 
-	/* This is a union acpi_operand_object    */
+	/* This is an object of type union acpi_operand_object */
 
 	switch (stack_desc->common.type) {
 	case ACPI_TYPE_LOCAL_REFERENCE:
@@ -321,7 +321,7 @@ acpi_ex_resolve_object_to_value(union acpi_operand_object **stack_ptr,
  * FUNCTION:    acpi_ex_resolve_multiple
  *
  * PARAMETERS:  walk_state          - Current state (contains AML opcode)
- *              Operand             - Starting point for resolution
+ *              operand             - Starting point for resolution
  *              return_type         - Where the object type is returned
  *              return_desc         - Where the resolved object is returned
  *
diff --git a/drivers/acpi/acpica/exresop.c b/drivers/acpi/acpica/exresop.c
index a67b1d925dd..f232fbabdea 100644
--- a/drivers/acpi/acpica/exresop.c
+++ b/drivers/acpi/acpica/exresop.c
@@ -113,7 +113,7 @@ acpi_ex_check_object_type(acpi_object_type type_needed,
  *
  * FUNCTION:    acpi_ex_resolve_operands
  *
- * PARAMETERS:  Opcode              - Opcode being interpreted
+ * PARAMETERS:  opcode              - Opcode being interpreted
  *              stack_ptr           - Pointer to the operand stack to be
  *                                    resolved
  *              walk_state          - Current state
@@ -307,7 +307,7 @@ acpi_ex_resolve_operands(u16 opcode,
 		case ARGI_DEVICE_REF:
 		case ARGI_TARGETREF:	/* Allows implicit conversion rules before store */
 		case ARGI_FIXED_TARGET:	/* No implicit conversion before store to target */
-		case ARGI_SIMPLE_TARGET:	/* Name, Local, or Arg - no implicit conversion  */
+		case ARGI_SIMPLE_TARGET:	/* Name, Local, or arg - no implicit conversion  */
 
 			/*
 			 * Need an operand of type ACPI_TYPE_LOCAL_REFERENCE
@@ -410,7 +410,7 @@ acpi_ex_resolve_operands(u16 opcode,
 			/*
 			 * Need an operand of type ACPI_TYPE_INTEGER,
 			 * But we can implicitly convert from a STRING or BUFFER
-			 * Aka - "Implicit Source Operand Conversion"
+			 * aka - "Implicit Source Operand Conversion"
 			 */
 			status =
 			    acpi_ex_convert_to_integer(obj_desc, stack_ptr, 16);
@@ -437,7 +437,7 @@ acpi_ex_resolve_operands(u16 opcode,
 			/*
 			 * Need an operand of type ACPI_TYPE_BUFFER,
 			 * But we can implicitly convert from a STRING or INTEGER
-			 * Aka - "Implicit Source Operand Conversion"
+			 * aka - "Implicit Source Operand Conversion"
 			 */
 			status = acpi_ex_convert_to_buffer(obj_desc, stack_ptr);
 			if (ACPI_FAILURE(status)) {
@@ -463,7 +463,7 @@ acpi_ex_resolve_operands(u16 opcode,
 			/*
 			 * Need an operand of type ACPI_TYPE_STRING,
 			 * But we can implicitly convert from a BUFFER or INTEGER
-			 * Aka - "Implicit Source Operand Conversion"
+			 * aka - "Implicit Source Operand Conversion"
 			 */
 			status = acpi_ex_convert_to_string(obj_desc, stack_ptr,
 							   ACPI_IMPLICIT_CONVERT_HEX);
diff --git a/drivers/acpi/acpica/exstore.c b/drivers/acpi/acpica/exstore.c
index c6cf843cc4c..5fffe7ab5ec 100644
--- a/drivers/acpi/acpica/exstore.c
+++ b/drivers/acpi/acpica/exstore.c
@@ -62,8 +62,8 @@ acpi_ex_store_object_to_index(union acpi_operand_object *val_desc,
  * FUNCTION:    acpi_ex_store
  *
  * PARAMETERS:  *source_desc        - Value to be stored
- *              *dest_desc          - Where to store it.  Must be an NS node
- *                                    or a union acpi_operand_object of type
+ *              *dest_desc          - Where to store it. Must be an NS node
+ *                                    or union acpi_operand_object of type
  *                                    Reference;
  *              walk_state          - Current walk state
  *
@@ -361,7 +361,7 @@ acpi_ex_store_object_to_index(union acpi_operand_object *source_desc,
  * FUNCTION:    acpi_ex_store_object_to_node
  *
  * PARAMETERS:  source_desc             - Value to be stored
- *              Node                    - Named object to receive the value
+ *              node                    - Named object to receive the value
  *              walk_state              - Current walk state
  *              implicit_conversion     - Perform implicit conversion (yes/no)
  *
diff --git a/drivers/acpi/acpica/exstorob.c b/drivers/acpi/acpica/exstorob.c
index 65a45d8335c..53c24847354 100644
--- a/drivers/acpi/acpica/exstorob.c
+++ b/drivers/acpi/acpica/exstorob.c
@@ -110,7 +110,7 @@ acpi_ex_store_buffer_to_buffer(union acpi_operand_object *source_desc,
 		 * NOTE: ACPI versions up to 3.0 specified that the buffer must be
 		 * truncated if the string is smaller than the buffer.  However, "other"
 		 * implementations of ACPI never did this and thus became the defacto
-		 * standard. ACPI 3.0_a changes this behavior such that the buffer
+		 * standard. ACPI 3.0A changes this behavior such that the buffer
 		 * is no longer truncated.
 		 */
 
diff --git a/drivers/acpi/acpica/exsystem.c b/drivers/acpi/acpica/exsystem.c
index 191a1294522..b760641e2fc 100644
--- a/drivers/acpi/acpica/exsystem.c
+++ b/drivers/acpi/acpica/exsystem.c
@@ -53,8 +53,8 @@ ACPI_MODULE_NAME("exsystem")
  *
  * FUNCTION:    acpi_ex_system_wait_semaphore
  *
- * PARAMETERS:  Semaphore       - Semaphore to wait on
- *              Timeout         - Max time to wait
+ * PARAMETERS:  semaphore       - Semaphore to wait on
+ *              timeout         - Max time to wait
  *
  * RETURN:      Status
  *
@@ -98,8 +98,8 @@ acpi_status acpi_ex_system_wait_semaphore(acpi_semaphore semaphore, u16 timeout)
  *
  * FUNCTION:    acpi_ex_system_wait_mutex
  *
- * PARAMETERS:  Mutex           - Mutex to wait on
- *              Timeout         - Max time to wait
+ * PARAMETERS:  mutex           - Mutex to wait on
+ *              timeout         - Max time to wait
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/exutils.c b/drivers/acpi/acpica/exutils.c
index eb6798ba8b5..d1ab7917eed 100644
--- a/drivers/acpi/acpica/exutils.c
+++ b/drivers/acpi/acpica/exutils.c
@@ -109,7 +109,7 @@ void acpi_ex_enter_interpreter(void)
  *
  * DESCRIPTION: Reacquire the interpreter execution region from within the
  *              interpreter code. Failure to enter the interpreter region is a
- *              fatal system error. Used in  conjunction with
+ *              fatal system error. Used in conjunction with
  *              relinquish_interpreter
  *
  ******************************************************************************/
@@ -317,8 +317,8 @@ void acpi_ex_release_global_lock(u32 field_flags)
  *
  * FUNCTION:    acpi_ex_digits_needed
  *
- * PARAMETERS:  Value           - Value to be represented
- *              Base            - Base of representation
+ * PARAMETERS:  value           - Value to be represented
+ *              base            - Base of representation
  *
  * RETURN:      The number of digits.
  *
@@ -408,7 +408,7 @@ void acpi_ex_eisa_id_to_string(char *out_string, u64 compressed_id)
  * PARAMETERS:  out_string      - Where to put the converted string. At least
  *                                21 bytes are needed to hold the largest
  *                                possible 64-bit integer.
- *              Value           - Value to be converted
+ *              value           - Value to be converted
  *
  * RETURN:      None, string
  *
@@ -443,7 +443,7 @@ void acpi_ex_integer_to_string(char *out_string, u64 value)
  *
  * RETURN:      TRUE if valid/supported ID.
  *
- * DESCRIPTION: Validate an operation region space_iD.
+ * DESCRIPTION: Validate an operation region space_ID.
  *
  ******************************************************************************/
 
diff --git a/drivers/acpi/acpica/hwacpi.c b/drivers/acpi/acpica/hwacpi.c
index d0b9ed5df97..a1e71d0ef57 100644
--- a/drivers/acpi/acpica/hwacpi.c
+++ b/drivers/acpi/acpica/hwacpi.c
@@ -53,7 +53,7 @@ ACPI_MODULE_NAME("hwacpi")
  *
  * FUNCTION:    acpi_hw_set_mode
  *
- * PARAMETERS:  Mode            - SYS_MODE_ACPI or SYS_MODE_LEGACY
+ * PARAMETERS:  mode            - SYS_MODE_ACPI or SYS_MODE_LEGACY
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/hwesleep.c b/drivers/acpi/acpica/hwesleep.c
index 29e859293ed..48518dac534 100644
--- a/drivers/acpi/acpica/hwesleep.c
+++ b/drivers/acpi/acpica/hwesleep.c
@@ -90,7 +90,7 @@ void acpi_hw_execute_sleep_method(char *method_pathname, u32 integer_argument)
  * FUNCTION:    acpi_hw_extended_sleep
  *
  * PARAMETERS:  sleep_state         - Which sleep state to enter
- *              Flags               - ACPI_EXECUTE_GTS to run optional method
+ *              flags               - ACPI_EXECUTE_GTS to run optional method
  *
  * RETURN:      Status
  *
@@ -117,7 +117,8 @@ acpi_status acpi_hw_extended_sleep(u8 sleep_state, u8 flags)
 
 	/* Clear wake status (WAK_STS) */
 
-	status = acpi_write(ACPI_X_WAKE_STATUS, &acpi_gbl_FADT.sleep_status);
+	status =
+	    acpi_write((u64)ACPI_X_WAKE_STATUS, &acpi_gbl_FADT.sleep_status);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -147,7 +148,7 @@ acpi_status acpi_hw_extended_sleep(u8 sleep_state, u8 flags)
 	    ((acpi_gbl_sleep_type_a << ACPI_X_SLEEP_TYPE_POSITION) &
 	     ACPI_X_SLEEP_TYPE_MASK);
 
-	status = acpi_write((sleep_type_value | ACPI_X_SLEEP_ENABLE),
+	status = acpi_write((u64)(sleep_type_value | ACPI_X_SLEEP_ENABLE),
 			    &acpi_gbl_FADT.sleep_control);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
@@ -171,7 +172,7 @@ acpi_status acpi_hw_extended_sleep(u8 sleep_state, u8 flags)
  * FUNCTION:    acpi_hw_extended_wake_prep
  *
  * PARAMETERS:  sleep_state         - Which sleep state we just exited
- *              Flags               - ACPI_EXECUTE_BFS to run optional method
+ *              flags               - ACPI_EXECUTE_BFS to run optional method
  *
  * RETURN:      Status
  *
@@ -195,7 +196,7 @@ acpi_status acpi_hw_extended_wake_prep(u8 sleep_state, u8 flags)
 		    ((acpi_gbl_sleep_type_a << ACPI_X_SLEEP_TYPE_POSITION) &
 		     ACPI_X_SLEEP_TYPE_MASK);
 
-		(void)acpi_write((sleep_type_value | ACPI_X_SLEEP_ENABLE),
+		(void)acpi_write((u64)(sleep_type_value | ACPI_X_SLEEP_ENABLE),
 				 &acpi_gbl_FADT.sleep_control);
 	}
 
@@ -212,7 +213,7 @@ acpi_status acpi_hw_extended_wake_prep(u8 sleep_state, u8 flags)
  * FUNCTION:    acpi_hw_extended_wake
  *
  * PARAMETERS:  sleep_state         - Which sleep state we just exited
- *              Flags               - Reserved, set to zero
+ *              flags               - Reserved, set to zero
  *
  * RETURN:      Status
  *
@@ -239,7 +240,7 @@ acpi_status acpi_hw_extended_wake(u8 sleep_state, u8 flags)
 	 * and use it to determine whether the system is rebooting or
 	 * resuming. Clear WAK_STS for compatibility.
 	 */
-	(void)acpi_write(ACPI_X_WAKE_STATUS, &acpi_gbl_FADT.sleep_status);
+	(void)acpi_write((u64)ACPI_X_WAKE_STATUS, &acpi_gbl_FADT.sleep_status);
 	acpi_gbl_system_awake_and_running = TRUE;
 
 	acpi_hw_execute_sleep_method(METHOD_PATHNAME__SST, ACPI_SST_WORKING);
diff --git a/drivers/acpi/acpica/hwregs.c b/drivers/acpi/acpica/hwregs.c
index 6b6c83b87b5..4af6d20ef07 100644
--- a/drivers/acpi/acpica/hwregs.c
+++ b/drivers/acpi/acpica/hwregs.c
@@ -69,9 +69,9 @@ acpi_hw_write_multiple(u32 value,
  *
  * FUNCTION:    acpi_hw_validate_register
  *
- * PARAMETERS:  Reg                 - GAS register structure
+ * PARAMETERS:  reg                 - GAS register structure
  *              max_bit_width       - Max bit_width supported (32 or 64)
- *              Address             - Pointer to where the gas->address
+ *              address             - Pointer to where the gas->address
  *                                    is returned
  *
  * RETURN:      Status
@@ -102,7 +102,7 @@ acpi_hw_validate_register(struct acpi_generic_address *reg,
 		return (AE_BAD_ADDRESS);
 	}
 
-	/* Validate the space_iD */
+	/* Validate the space_ID */
 
 	if ((reg->space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY) &&
 	    (reg->space_id != ACPI_ADR_SPACE_SYSTEM_IO)) {
@@ -137,8 +137,8 @@ acpi_hw_validate_register(struct acpi_generic_address *reg,
  *
  * FUNCTION:    acpi_hw_read
  *
- * PARAMETERS:  Value               - Where the value is returned
- *              Reg                 - GAS register structure
+ * PARAMETERS:  value               - Where the value is returned
+ *              reg                 - GAS register structure
  *
  * RETURN:      Status
  *
@@ -148,7 +148,7 @@ acpi_hw_validate_register(struct acpi_generic_address *reg,
  *
  * LIMITATIONS: <These limitations also apply to acpi_hw_write>
  *      bit_width must be exactly 8, 16, or 32.
- *      space_iD must be system_memory or system_iO.
+ *      space_ID must be system_memory or system_IO.
  *      bit_offset and access_width are currently ignored, as there has
  *          not been a need to implement these.
  *
@@ -200,8 +200,8 @@ acpi_status acpi_hw_read(u32 *value, struct acpi_generic_address *reg)
  *
  * FUNCTION:    acpi_hw_write
  *
- * PARAMETERS:  Value               - Value to be written
- *              Reg                 - GAS register structure
+ * PARAMETERS:  value               - Value to be written
+ *              reg                 - GAS register structure
  *
  * RETURN:      Status
  *
@@ -439,7 +439,7 @@ acpi_hw_register_read(u32 register_id, u32 * return_value)
  * FUNCTION:    acpi_hw_register_write
  *
  * PARAMETERS:  register_id         - ACPI Register ID
- *              Value               - The value to write
+ *              value               - The value to write
  *
  * RETURN:      Status
  *
@@ -571,7 +571,7 @@ acpi_status acpi_hw_register_write(u32 register_id, u32 value)
  *
  * FUNCTION:    acpi_hw_read_multiple
  *
- * PARAMETERS:  Value               - Where the register value is returned
+ * PARAMETERS:  value               - Where the register value is returned
  *              register_a           - First ACPI register (required)
  *              register_b           - Second ACPI register (optional)
  *
@@ -624,7 +624,7 @@ acpi_hw_read_multiple(u32 *value,
  *
  * FUNCTION:    acpi_hw_write_multiple
  *
- * PARAMETERS:  Value               - The value to write
+ * PARAMETERS:  value               - The value to write
  *              register_a           - First ACPI register (required)
  *              register_b           - Second ACPI register (optional)
  *
diff --git a/drivers/acpi/acpica/hwsleep.c b/drivers/acpi/acpica/hwsleep.c
index 615996a36be..9960fe9ef53 100644
--- a/drivers/acpi/acpica/hwsleep.c
+++ b/drivers/acpi/acpica/hwsleep.c
@@ -56,7 +56,7 @@ ACPI_MODULE_NAME("hwsleep")
  * FUNCTION:    acpi_hw_legacy_sleep
  *
  * PARAMETERS:  sleep_state         - Which sleep state to enter
- *              Flags               - ACPI_EXECUTE_GTS to run optional method
+ *              flags               - ACPI_EXECUTE_GTS to run optional method
  *
  * RETURN:      Status
  *
@@ -214,7 +214,7 @@ acpi_status acpi_hw_legacy_sleep(u8 sleep_state, u8 flags)
  * FUNCTION:    acpi_hw_legacy_wake_prep
  *
  * PARAMETERS:  sleep_state         - Which sleep state we just exited
- *              Flags               - ACPI_EXECUTE_BFS to run optional method
+ *              flags               - ACPI_EXECUTE_BFS to run optional method
  *
  * RETURN:      Status
  *
@@ -288,7 +288,7 @@ acpi_status acpi_hw_legacy_wake_prep(u8 sleep_state, u8 flags)
  * FUNCTION:    acpi_hw_legacy_wake
  *
  * PARAMETERS:  sleep_state         - Which sleep state we just exited
- *              Flags               - Reserved, set to zero
+ *              flags               - Reserved, set to zero
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/hwtimer.c b/drivers/acpi/acpica/hwtimer.c
index f1b2c3b94ca..b6411f16832 100644
--- a/drivers/acpi/acpica/hwtimer.c
+++ b/drivers/acpi/acpica/hwtimer.c
@@ -54,7 +54,7 @@ ACPI_MODULE_NAME("hwtimer")
  *
  * FUNCTION:    acpi_get_timer_resolution
  *
- * PARAMETERS:  Resolution          - Where the resolution is returned
+ * PARAMETERS:  resolution          - Where the resolution is returned
  *
  * RETURN:      Status and timer resolution
  *
@@ -84,7 +84,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_timer_resolution)
  *
  * FUNCTION:    acpi_get_timer
  *
- * PARAMETERS:  Ticks               - Where the timer value is returned
+ * PARAMETERS:  ticks               - Where the timer value is returned
  *
  * RETURN:      Status and current timer value (ticks)
  *
diff --git a/drivers/acpi/acpica/hwvalid.c b/drivers/acpi/acpica/hwvalid.c
index 6e5c43a60bb..c99d546b217 100644
--- a/drivers/acpi/acpica/hwvalid.c
+++ b/drivers/acpi/acpica/hwvalid.c
@@ -58,7 +58,7 @@ acpi_hw_validate_io_request(acpi_io_address address, u32 bit_width);
  *
  * The table is used to implement the Microsoft port access rules that
  * first appeared in Windows XP. Some ports are always illegal, and some
- * ports are only illegal if the BIOS calls _OSI with a win_xP string or
+ * ports are only illegal if the BIOS calls _OSI with a win_XP string or
  * later (meaning that the BIOS itelf is post-XP.)
  *
  * This provides ACPICA with the desired port protections and
@@ -66,7 +66,7 @@ acpi_hw_validate_io_request(acpi_io_address address, u32 bit_width);
  *
  * Description of port entries:
  *  DMA:   DMA controller
- *  PIC0:  Programmable Interrupt Controller (8259_a)
+ *  PIC0:  Programmable Interrupt Controller (8259A)
  *  PIT1:  System Timer 1
  *  PIT2:  System Timer 2 failsafe
  *  RTC:   Real-time clock
diff --git a/drivers/acpi/acpica/hwxface.c b/drivers/acpi/acpica/hwxface.c
index a716fede4f2..7bfd649d199 100644
--- a/drivers/acpi/acpica/hwxface.c
+++ b/drivers/acpi/acpica/hwxface.c
@@ -104,8 +104,8 @@ ACPI_EXPORT_SYMBOL(acpi_reset)
  *
  * FUNCTION:    acpi_read
  *
- * PARAMETERS:  Value               - Where the value is returned
- *              Reg                 - GAS register structure
+ * PARAMETERS:  value               - Where the value is returned
+ *              reg                 - GAS register structure
  *
  * RETURN:      Status
  *
@@ -113,7 +113,7 @@ ACPI_EXPORT_SYMBOL(acpi_reset)
  *
  * LIMITATIONS: <These limitations also apply to acpi_write>
  *      bit_width must be exactly 8, 16, 32, or 64.
- *      space_iD must be system_memory or system_iO.
+ *      space_ID must be system_memory or system_IO.
  *      bit_offset and access_width are currently ignored, as there has
  *          not been a need to implement these.
  *
@@ -196,8 +196,8 @@ ACPI_EXPORT_SYMBOL(acpi_read)
  *
  * FUNCTION:    acpi_write
  *
- * PARAMETERS:  Value               - Value to be written
- *              Reg                 - GAS register structure
+ * PARAMETERS:  value               - Value to be written
+ *              reg                 - GAS register structure
  *
  * RETURN:      Status
  *
@@ -441,7 +441,7 @@ ACPI_EXPORT_SYMBOL(acpi_write_bit_register)
  *              *sleep_type_a        - Where SLP_TYPa is returned
  *              *sleep_type_b        - Where SLP_TYPb is returned
  *
- * RETURN:      Status - ACPI status
+ * RETURN:      status - ACPI status
  *
  * DESCRIPTION: Obtain the SLP_TYPa and SLP_TYPb values for the requested sleep
  *              state.
diff --git a/drivers/acpi/acpica/hwxfsleep.c b/drivers/acpi/acpica/hwxfsleep.c
index 762d059bb50..f8684bfe790 100644
--- a/drivers/acpi/acpica/hwxfsleep.c
+++ b/drivers/acpi/acpica/hwxfsleep.c
@@ -205,7 +205,7 @@ acpi_status asmlinkage acpi_enter_sleep_state_s4bios(void)
 	ACPI_FLUSH_CPU_CACHE();
 
 	status = acpi_hw_write_port(acpi_gbl_FADT.smi_command,
-				    (u32)acpi_gbl_FADT.S4bios_request, 8);
+				    (u32)acpi_gbl_FADT.s4_bios_request, 8);
 
 	do {
 		acpi_os_stall(1000);
@@ -349,7 +349,7 @@ ACPI_EXPORT_SYMBOL(acpi_enter_sleep_state_prep)
  * FUNCTION:    acpi_enter_sleep_state
  *
  * PARAMETERS:  sleep_state         - Which sleep state to enter
- *              Flags               - ACPI_EXECUTE_GTS to run optional method
+ *              flags               - ACPI_EXECUTE_GTS to run optional method
  *
  * RETURN:      Status
  *
@@ -382,7 +382,7 @@ ACPI_EXPORT_SYMBOL(acpi_enter_sleep_state)
  * FUNCTION:    acpi_leave_sleep_state_prep
  *
  * PARAMETERS:  sleep_state         - Which sleep state we are exiting
- *              Flags               - ACPI_EXECUTE_BFS to run optional method
+ *              flags               - ACPI_EXECUTE_BFS to run optional method
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/nsaccess.c b/drivers/acpi/acpica/nsaccess.c
index 61623f3f682..23db53ce229 100644
--- a/drivers/acpi/acpica/nsaccess.c
+++ b/drivers/acpi/acpica/nsaccess.c
@@ -157,7 +157,7 @@ acpi_status acpi_ns_root_initialize(void)
 
 #if defined (ACPI_ASL_COMPILER)
 
-				/* Save the parameter count for the i_aSL compiler */
+				/* Save the parameter count for the iASL compiler */
 
 				new_node->value = obj_desc->method.param_count;
 #else
@@ -258,11 +258,11 @@ acpi_status acpi_ns_root_initialize(void)
  * FUNCTION:    acpi_ns_lookup
  *
  * PARAMETERS:  scope_info      - Current scope info block
- *              Pathname        - Search pathname, in internal format
+ *              pathname        - Search pathname, in internal format
  *                                (as represented in the AML stream)
- *              Type            - Type associated with name
+ *              type            - Type associated with name
  *              interpreter_mode - IMODE_LOAD_PASS2 => add name if not found
- *              Flags           - Flags describing the search restrictions
+ *              flags           - Flags describing the search restrictions
  *              walk_state      - Current state of the walk
  *              return_node     - Where the Node is placed (if found
  *                                or created successfully)
diff --git a/drivers/acpi/acpica/nsalloc.c b/drivers/acpi/acpica/nsalloc.c
index 7c3d3ceb98b..ac389e5bb59 100644
--- a/drivers/acpi/acpica/nsalloc.c
+++ b/drivers/acpi/acpica/nsalloc.c
@@ -52,7 +52,7 @@ ACPI_MODULE_NAME("nsalloc")
  *
  * FUNCTION:    acpi_ns_create_node
  *
- * PARAMETERS:  Name            - Name of the new node (4 char ACPI name)
+ * PARAMETERS:  name            - Name of the new node (4 char ACPI name)
  *
  * RETURN:      New namespace node (Null on failure)
  *
@@ -92,7 +92,7 @@ struct acpi_namespace_node *acpi_ns_create_node(u32 name)
  *
  * FUNCTION:    acpi_ns_delete_node
  *
- * PARAMETERS:  Node            - Node to be deleted
+ * PARAMETERS:  node            - Node to be deleted
  *
  * RETURN:      None
  *
@@ -143,7 +143,7 @@ void acpi_ns_delete_node(struct acpi_namespace_node *node)
  *
  * FUNCTION:    acpi_ns_remove_node
  *
- * PARAMETERS:  Node            - Node to be removed/deleted
+ * PARAMETERS:  node            - Node to be removed/deleted
  *
  * RETURN:      None
  *
@@ -196,8 +196,8 @@ void acpi_ns_remove_node(struct acpi_namespace_node *node)
  *
  * PARAMETERS:  walk_state      - Current state of the walk
  *              parent_node     - The parent of the new Node
- *              Node            - The new Node to install
- *              Type            - ACPI object type of the new Node
+ *              node            - The new Node to install
+ *              type            - ACPI object type of the new Node
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/nsdump.c b/drivers/acpi/acpica/nsdump.c
index 3f7f3f6e7dd..7ee4e6aeb0a 100644
--- a/drivers/acpi/acpica/nsdump.c
+++ b/drivers/acpi/acpica/nsdump.c
@@ -63,7 +63,7 @@ acpi_ns_dump_one_device(acpi_handle obj_handle,
  * FUNCTION:    acpi_ns_print_pathname
  *
  * PARAMETERS:  num_segments        - Number of ACPI name segments
- *              Pathname            - The compressed (internal) path
+ *              pathname            - The compressed (internal) path
  *
  * RETURN:      None
  *
@@ -107,10 +107,10 @@ void acpi_ns_print_pathname(u32 num_segments, char *pathname)
  *
  * FUNCTION:    acpi_ns_dump_pathname
  *
- * PARAMETERS:  Handle              - Object
- *              Msg                 - Prefix message
- *              Level               - Desired debug level
- *              Component           - Caller's component ID
+ * PARAMETERS:  handle              - Object
+ *              msg                 - Prefix message
+ *              level               - Desired debug level
+ *              component           - Caller's component ID
  *
  * RETURN:      None
  *
@@ -143,8 +143,8 @@ acpi_ns_dump_pathname(acpi_handle handle, char *msg, u32 level, u32 component)
  * FUNCTION:    acpi_ns_dump_one_object
  *
  * PARAMETERS:  obj_handle          - Node to be dumped
- *              Level               - Nesting level of the handle
- *              Context             - Passed into walk_namespace
+ *              level               - Nesting level of the handle
+ *              context             - Passed into walk_namespace
  *              return_value        - Not used
  *
  * RETURN:      Status
@@ -615,7 +615,7 @@ acpi_ns_dump_one_object(acpi_handle obj_handle,
  *
  * FUNCTION:    acpi_ns_dump_objects
  *
- * PARAMETERS:  Type                - Object type to be dumped
+ * PARAMETERS:  type                - Object type to be dumped
  *              display_type        - 0 or ACPI_DISPLAY_SUMMARY
  *              max_depth           - Maximum depth of dump. Use ACPI_UINT32_MAX
  *                                    for an effectively unlimited depth.
@@ -671,7 +671,7 @@ acpi_ns_dump_objects(acpi_object_type type,
  *
  * FUNCTION:    acpi_ns_dump_entry
  *
- * PARAMETERS:  Handle              - Node to be dumped
+ * PARAMETERS:  handle              - Node to be dumped
  *              debug_level         - Output level
  *
  * RETURN:      None
diff --git a/drivers/acpi/acpica/nsdumpdv.c b/drivers/acpi/acpica/nsdumpdv.c
index 3b5acb0eb40..944d4c8d943 100644
--- a/drivers/acpi/acpica/nsdumpdv.c
+++ b/drivers/acpi/acpica/nsdumpdv.c
@@ -55,9 +55,9 @@ ACPI_MODULE_NAME("nsdumpdv")
  *
  * FUNCTION:    acpi_ns_dump_one_device
  *
- * PARAMETERS:  Handle              - Node to be dumped
- *              Level               - Nesting level of the handle
- *              Context             - Passed into walk_namespace
+ * PARAMETERS:  handle              - Node to be dumped
+ *              level               - Nesting level of the handle
+ *              context             - Passed into walk_namespace
  *              return_value        - Not used
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/nseval.c b/drivers/acpi/acpica/nseval.c
index f375cb82e32..69074be498e 100644
--- a/drivers/acpi/acpica/nseval.c
+++ b/drivers/acpi/acpica/nseval.c
@@ -59,11 +59,11 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
  *
  * FUNCTION:    acpi_ns_evaluate
  *
- * PARAMETERS:  Info            - Evaluation info block, contains:
+ * PARAMETERS:  info            - Evaluation info block, contains:
  *                  prefix_node     - Prefix or Method/Object Node to execute
- *                  Pathname        - Name of method to execute, If NULL, the
+ *                  pathname        - Name of method to execute, If NULL, the
  *                                    Node is the object to execute
- *                  Parameters      - List of parameters to pass to the method,
+ *                  parameters      - List of parameters to pass to the method,
  *                                    terminated by NULL. Params itself may be
  *                                    NULL if no parameters are being passed.
  *                  return_object   - Where to put method's return value (if
@@ -71,7 +71,7 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
  *                  parameter_type  - Type of Parameter list
  *                  return_object   - Where to put method's return value (if
  *                                    any). If NULL, no value is returned.
- *                  Flags           - ACPI_IGNORE_RETURN_VALUE to delete return
+ *                  flags           - ACPI_IGNORE_RETURN_VALUE to delete return
  *
  * RETURN:      Status
  *
@@ -351,7 +351,7 @@ void acpi_ns_exec_module_code_list(void)
  * FUNCTION:    acpi_ns_exec_module_code
  *
  * PARAMETERS:  method_obj          - Object container for the module-level code
- *              Info                - Info block for method evaluation
+ *              info                - Info block for method evaluation
  *
  * RETURN:      None. Exceptions during method execution are ignored, since
  *              we cannot abort a table load.
diff --git a/drivers/acpi/acpica/nsinit.c b/drivers/acpi/acpica/nsinit.c
index 9d84ec2f021..95ffe8dfa1f 100644
--- a/drivers/acpi/acpica/nsinit.c
+++ b/drivers/acpi/acpica/nsinit.c
@@ -224,8 +224,8 @@ acpi_status acpi_ns_initialize_devices(void)
  * FUNCTION:    acpi_ns_init_one_object
  *
  * PARAMETERS:  obj_handle      - Node
- *              Level           - Current nesting level
- *              Context         - Points to a init info struct
+ *              level           - Current nesting level
+ *              context         - Points to a init info struct
  *              return_value    - Not used
  *
  * RETURN:      Status
@@ -530,7 +530,7 @@ acpi_ns_init_one_device(acpi_handle obj_handle,
 			 * we will not run _INI, but we continue to examine the children
 			 * of this device.
 			 *
-			 * From the ACPI spec, description of _STA: (Note - no mention
+			 * From the ACPI spec, description of _STA: (note - no mention
 			 * of whether to run _INI or not on the device in question)
 			 *
 			 * "_STA may return bit 0 clear (not present) with bit 3 set
diff --git a/drivers/acpi/acpica/nsload.c b/drivers/acpi/acpica/nsload.c
index 5cbf15ffe7d..76935ff2928 100644
--- a/drivers/acpi/acpica/nsload.c
+++ b/drivers/acpi/acpica/nsload.c
@@ -63,7 +63,7 @@ static acpi_status acpi_ns_delete_subtree(acpi_handle start_handle);
  * FUNCTION:    acpi_ns_load_table
  *
  * PARAMETERS:  table_index     - Index for table to be loaded
- *              Node            - Owning NS node
+ *              node            - Owning NS node
  *
  * RETURN:      Status
  *
@@ -278,7 +278,7 @@ static acpi_status acpi_ns_delete_subtree(acpi_handle start_handle)
  *
  *  FUNCTION:       acpi_ns_unload_name_space
  *
- *  PARAMETERS:     Handle          - Root of namespace subtree to be deleted
+ *  PARAMETERS:     handle          - Root of namespace subtree to be deleted
  *
  *  RETURN:         Status
  *
diff --git a/drivers/acpi/acpica/nsnames.c b/drivers/acpi/acpica/nsnames.c
index b20e7c8c3ff..96e0eb609bb 100644
--- a/drivers/acpi/acpica/nsnames.c
+++ b/drivers/acpi/acpica/nsnames.c
@@ -53,8 +53,8 @@ ACPI_MODULE_NAME("nsnames")
  *
  * FUNCTION:    acpi_ns_build_external_path
  *
- * PARAMETERS:  Node            - NS node whose pathname is needed
- *              Size            - Size of the pathname
+ * PARAMETERS:  node            - NS node whose pathname is needed
+ *              size            - Size of the pathname
  *              *name_buffer    - Where to return the pathname
  *
  * RETURN:      Status
@@ -120,7 +120,7 @@ acpi_ns_build_external_path(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_ns_get_external_pathname
  *
- * PARAMETERS:  Node            - Namespace node whose pathname is needed
+ * PARAMETERS:  node            - Namespace node whose pathname is needed
  *
  * RETURN:      Pointer to storage containing the fully qualified name of
  *              the node, In external format (name segments separated by path
@@ -168,7 +168,7 @@ char *acpi_ns_get_external_pathname(struct acpi_namespace_node *node)
  *
  * FUNCTION:    acpi_ns_get_pathname_length
  *
- * PARAMETERS:  Node        - Namespace node
+ * PARAMETERS:  node        - Namespace node
  *
  * RETURN:      Length of path, including prefix
  *
@@ -214,7 +214,7 @@ acpi_size acpi_ns_get_pathname_length(struct acpi_namespace_node *node)
  *
  * PARAMETERS:  target_handle           - Handle of named object whose name is
  *                                        to be found
- *              Buffer                  - Where the pathname is returned
+ *              buffer                  - Where the pathname is returned
  *
  * RETURN:      Status, Buffer is filled with pathname if status is AE_OK
  *
diff --git a/drivers/acpi/acpica/nsobject.c b/drivers/acpi/acpica/nsobject.c
index dd77a3ce6e5..d6c9a3cc671 100644
--- a/drivers/acpi/acpica/nsobject.c
+++ b/drivers/acpi/acpica/nsobject.c
@@ -53,9 +53,9 @@ ACPI_MODULE_NAME("nsobject")
  *
  * FUNCTION:    acpi_ns_attach_object
  *
- * PARAMETERS:  Node                - Parent Node
- *              Object              - Object to be attached
- *              Type                - Type of object, or ACPI_TYPE_ANY if not
+ * PARAMETERS:  node                - Parent Node
+ *              object              - Object to be attached
+ *              type                - Type of object, or ACPI_TYPE_ANY if not
  *                                    known
  *
  * RETURN:      Status
@@ -191,7 +191,7 @@ acpi_ns_attach_object(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_ns_detach_object
  *
- * PARAMETERS:  Node           - A Namespace node whose object will be detached
+ * PARAMETERS:  node           - A Namespace node whose object will be detached
  *
  * RETURN:      None.
  *
@@ -250,7 +250,7 @@ void acpi_ns_detach_object(struct acpi_namespace_node *node)
  *
  * FUNCTION:    acpi_ns_get_attached_object
  *
- * PARAMETERS:  Node             - Namespace node
+ * PARAMETERS:  node             - Namespace node
  *
  * RETURN:      Current value of the object field from the Node whose
  *              handle is passed
@@ -285,7 +285,7 @@ union acpi_operand_object *acpi_ns_get_attached_object(struct
  *
  * FUNCTION:    acpi_ns_get_secondary_object
  *
- * PARAMETERS:  Node             - Namespace node
+ * PARAMETERS:  node             - Namespace node
  *
  * RETURN:      Current value of the object field from the Node whose
  *              handle is passed.
@@ -315,9 +315,9 @@ union acpi_operand_object *acpi_ns_get_secondary_object(union
  *
  * FUNCTION:    acpi_ns_attach_data
  *
- * PARAMETERS:  Node            - Namespace node
- *              Handler         - Handler to be associated with the data
- *              Data            - Data to be attached
+ * PARAMETERS:  node            - Namespace node
+ *              handler         - Handler to be associated with the data
+ *              data            - Data to be attached
  *
  * RETURN:      Status
  *
@@ -372,8 +372,8 @@ acpi_ns_attach_data(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_ns_detach_data
  *
- * PARAMETERS:  Node            - Namespace node
- *              Handler         - Handler associated with the data
+ * PARAMETERS:  node            - Namespace node
+ *              handler         - Handler associated with the data
  *
  * RETURN:      Status
  *
@@ -416,9 +416,9 @@ acpi_ns_detach_data(struct acpi_namespace_node * node,
  *
  * FUNCTION:    acpi_ns_get_attached_data
  *
- * PARAMETERS:  Node            - Namespace node
- *              Handler         - Handler associated with the data
- *              Data            - Where the data is returned
+ * PARAMETERS:  node            - Namespace node
+ *              handler         - Handler associated with the data
+ *              data            - Where the data is returned
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/nspredef.c b/drivers/acpi/acpica/nspredef.c
index fe662603549..2419f417ea3 100644
--- a/drivers/acpi/acpica/nspredef.c
+++ b/drivers/acpi/acpica/nspredef.c
@@ -116,7 +116,7 @@ static const char *acpi_rtype_names[] = {
  *
  * FUNCTION:    acpi_ns_check_predefined_names
  *
- * PARAMETERS:  Node            - Namespace node for the method/object
+ * PARAMETERS:  node            - Namespace node for the method/object
  *              user_param_count - Number of parameters actually passed
  *              return_status   - Status from the object evaluation
  *              return_object_ptr - Pointer to the object returned from the
@@ -275,10 +275,10 @@ cleanup:
  *
  * FUNCTION:    acpi_ns_check_parameter_count
  *
- * PARAMETERS:  Pathname        - Full pathname to the node (for error msgs)
- *              Node            - Namespace node for the method/object
+ * PARAMETERS:  pathname        - Full pathname to the node (for error msgs)
+ *              node            - Namespace node for the method/object
  *              user_param_count - Number of args passed in by the caller
- *              Predefined      - Pointer to entry in predefined name table
+ *              predefined      - Pointer to entry in predefined name table
  *
  * RETURN:      None
  *
@@ -364,7 +364,7 @@ acpi_ns_check_parameter_count(char *pathname,
  *
  * FUNCTION:    acpi_ns_check_for_predefined_name
  *
- * PARAMETERS:  Node            - Namespace node for the method/object
+ * PARAMETERS:  node            - Namespace node for the method/object
  *
  * RETURN:      Pointer to entry in predefined table. NULL indicates not found.
  *
@@ -410,7 +410,7 @@ const union acpi_predefined_info *acpi_ns_check_for_predefined_name(struct
  *
  * FUNCTION:    acpi_ns_check_package
  *
- * PARAMETERS:  Data            - Pointer to validation data structure
+ * PARAMETERS:  data            - Pointer to validation data structure
  *              return_object_ptr - Pointer to the object returned from the
  *                                evaluation of a method or object
  *
@@ -685,11 +685,11 @@ package_too_small:
  *
  * FUNCTION:    acpi_ns_check_package_list
  *
- * PARAMETERS:  Data            - Pointer to validation data structure
- *              Package         - Pointer to package-specific info for method
- *              Elements        - Element list of parent package. All elements
+ * PARAMETERS:  data            - Pointer to validation data structure
+ *              package         - Pointer to package-specific info for method
+ *              elements        - Element list of parent package. All elements
  *                                of this list should be of type Package.
- *              Count           - Count of subpackages
+ *              count           - Count of subpackages
  *
  * RETURN:      Status
  *
@@ -911,12 +911,12 @@ package_too_small:
  *
  * FUNCTION:    acpi_ns_check_package_elements
  *
- * PARAMETERS:  Data            - Pointer to validation data structure
- *              Elements        - Pointer to the package elements array
- *              Type1           - Object type for first group
- *              Count1          - Count for first group
- *              Type2           - Object type for second group
- *              Count2          - Count for second group
+ * PARAMETERS:  data            - Pointer to validation data structure
+ *              elements        - Pointer to the package elements array
+ *              type1           - Object type for first group
+ *              count1          - Count for first group
+ *              type2           - Object type for second group
+ *              count2          - Count for second group
  *              start_index     - Start of the first group of elements
  *
  * RETURN:      Status
@@ -968,7 +968,7 @@ acpi_ns_check_package_elements(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_check_object_type
  *
- * PARAMETERS:  Data            - Pointer to validation data structure
+ * PARAMETERS:  data            - Pointer to validation data structure
  *              return_object_ptr - Pointer to the object returned from the
  *                                evaluation of a method or object
  *              expected_btypes - Bitmap of expected return type(s)
@@ -1102,7 +1102,7 @@ acpi_ns_check_object_type(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_check_reference
  *
- * PARAMETERS:  Data            - Pointer to validation data structure
+ * PARAMETERS:  data            - Pointer to validation data structure
  *              return_object   - Object returned from the evaluation of a
  *                                method or object
  *
@@ -1140,7 +1140,7 @@ acpi_ns_check_reference(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_get_expected_types
  *
- * PARAMETERS:  Buffer          - Pointer to where the string is returned
+ * PARAMETERS:  buffer          - Pointer to where the string is returned
  *              expected_btypes - Bitmap of expected return type(s)
  *
  * RETURN:      Buffer is populated with type names.
diff --git a/drivers/acpi/acpica/nsrepair.c b/drivers/acpi/acpica/nsrepair.c
index 5519a64a353..8c5f292860f 100644
--- a/drivers/acpi/acpica/nsrepair.c
+++ b/drivers/acpi/acpica/nsrepair.c
@@ -94,7 +94,7 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
  *
  * FUNCTION:    acpi_ns_repair_object
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              expected_btypes     - Object types expected
  *              package_index       - Index of object within parent package (if
  *                                    applicable - ACPI_NOT_PACKAGE_ELEMENT
@@ -470,7 +470,7 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
  *
  * FUNCTION:    acpi_ns_repair_null_element
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              expected_btypes     - Object types expected
  *              package_index       - Index of object within parent package (if
  *                                    applicable - ACPI_NOT_PACKAGE_ELEMENT
@@ -509,17 +509,17 @@ acpi_ns_repair_null_element(struct acpi_predefined_data *data,
 	 */
 	if (expected_btypes & ACPI_RTYPE_INTEGER) {
 
-		/* Need an Integer - create a zero-value integer */
+		/* Need an integer - create a zero-value integer */
 
 		new_object = acpi_ut_create_integer_object((u64)0);
 	} else if (expected_btypes & ACPI_RTYPE_STRING) {
 
-		/* Need a String - create a NULL string */
+		/* Need a string - create a NULL string */
 
 		new_object = acpi_ut_create_string_object(0);
 	} else if (expected_btypes & ACPI_RTYPE_BUFFER) {
 
-		/* Need a Buffer - create a zero-length buffer */
+		/* Need a buffer - create a zero-length buffer */
 
 		new_object = acpi_ut_create_buffer_object(0);
 	} else {
@@ -552,7 +552,7 @@ acpi_ns_repair_null_element(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_remove_null_elements
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              package_type        - An acpi_return_package_types value
  *              obj_desc            - A Package object
  *
@@ -635,7 +635,7 @@ acpi_ns_remove_null_elements(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_wrap_with_package
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              original_object     - Pointer to the object to repair.
  *              obj_desc_ptr        - The new package object is returned here
  *
diff --git a/drivers/acpi/acpica/nsrepair2.c b/drivers/acpi/acpica/nsrepair2.c
index 726bc8e687f..90189251cdf 100644
--- a/drivers/acpi/acpica/nsrepair2.c
+++ b/drivers/acpi/acpica/nsrepair2.c
@@ -149,8 +149,8 @@ static const struct acpi_repair_info acpi_ns_repairable_names[] = {
  *
  * FUNCTION:    acpi_ns_complex_repairs
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
- *              Node                - Namespace node for the method/object
+ * PARAMETERS:  data                - Pointer to validation data structure
+ *              node                - Namespace node for the method/object
  *              validate_status     - Original status of earlier validation
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
@@ -187,7 +187,7 @@ acpi_ns_complex_repairs(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_match_repairable_name
  *
- * PARAMETERS:  Node                - Namespace node for the method/object
+ * PARAMETERS:  node                - Namespace node for the method/object
  *
  * RETURN:      Pointer to entry in repair table. NULL indicates not found.
  *
@@ -218,7 +218,7 @@ static const struct acpi_repair_info *acpi_ns_match_repairable_name(struct
  *
  * FUNCTION:    acpi_ns_repair_ALR
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
  *
@@ -247,7 +247,7 @@ acpi_ns_repair_ALR(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_repair_FDE
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
  *
@@ -335,7 +335,7 @@ acpi_ns_repair_FDE(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_repair_CID
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
  *
@@ -405,7 +405,7 @@ acpi_ns_repair_CID(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_repair_HID
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
  *
@@ -487,7 +487,7 @@ acpi_ns_repair_HID(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_repair_TSS
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
  *
@@ -531,7 +531,7 @@ acpi_ns_repair_TSS(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_repair_PSS
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object_ptr   - Pointer to the object returned from the
  *                                    evaluation of a method or object
  *
@@ -600,7 +600,7 @@ acpi_ns_repair_PSS(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_check_sorted_list
  *
- * PARAMETERS:  Data                - Pointer to validation data structure
+ * PARAMETERS:  data                - Pointer to validation data structure
  *              return_object       - Pointer to the top-level returned object
  *              expected_count      - Minimum length of each sub-package
  *              sort_index          - Sub-package entry to sort on
@@ -707,9 +707,9 @@ acpi_ns_check_sorted_list(struct acpi_predefined_data *data,
  *
  * FUNCTION:    acpi_ns_sort_list
  *
- * PARAMETERS:  Elements            - Package object element list
- *              Count               - Element count for above
- *              Index               - Sort by which package element
+ * PARAMETERS:  elements            - Package object element list
+ *              count               - Element count for above
+ *              index               - Sort by which package element
  *              sort_direction      - Ascending or Descending sort
  *
  * RETURN:      None
diff --git a/drivers/acpi/acpica/nssearch.c b/drivers/acpi/acpica/nssearch.c
index 507043d6611..456cc859f86 100644
--- a/drivers/acpi/acpica/nssearch.c
+++ b/drivers/acpi/acpica/nssearch.c
@@ -65,7 +65,7 @@ acpi_ns_search_parent_tree(u32 target_name,
  *
  * PARAMETERS:  target_name     - Ascii ACPI name to search for
  *              parent_node     - Starting node where search will begin
- *              Type            - Object type to match
+ *              type            - Object type to match
  *              return_node     - Where the matched Named obj is returned
  *
  * RETURN:      Status
@@ -175,8 +175,8 @@ acpi_ns_search_one_scope(u32 target_name,
  * FUNCTION:    acpi_ns_search_parent_tree
  *
  * PARAMETERS:  target_name     - Ascii ACPI name to search for
- *              Node            - Starting node where search will begin
- *              Type            - Object type to match
+ *              node            - Starting node where search will begin
+ *              type            - Object type to match
  *              return_node     - Where the matched Node is returned
  *
  * RETURN:      Status
@@ -264,11 +264,11 @@ acpi_ns_search_parent_tree(u32 target_name,
  *
  * PARAMETERS:  target_name         - Ascii ACPI name to search for (4 chars)
  *              walk_state          - Current state of the walk
- *              Node                - Starting node where search will begin
+ *              node                - Starting node where search will begin
  *              interpreter_mode    - Add names only in ACPI_MODE_LOAD_PASS_x.
  *                                    Otherwise,search only.
- *              Type                - Object type to match
- *              Flags               - Flags describing the search restrictions
+ *              type                - Object type to match
+ *              flags               - Flags describing the search restrictions
  *              return_node         - Where the Node is returned
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/nsutils.c b/drivers/acpi/acpica/nsutils.c
index 75113759f69..ef753a41e08 100644
--- a/drivers/acpi/acpica/nsutils.c
+++ b/drivers/acpi/acpica/nsutils.c
@@ -62,8 +62,8 @@ acpi_name acpi_ns_find_parent_name(struct acpi_namespace_node *node_to_search);
  *
  * FUNCTION:    acpi_ns_print_node_pathname
  *
- * PARAMETERS:  Node            - Object
- *              Message         - Prefix message
+ * PARAMETERS:  node            - Object
+ *              message         - Prefix message
  *
  * DESCRIPTION: Print an object's full namespace pathname
  *              Manages allocation/freeing of a pathname buffer
@@ -101,7 +101,7 @@ acpi_ns_print_node_pathname(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_ns_valid_root_prefix
  *
- * PARAMETERS:  Prefix          - Character to be checked
+ * PARAMETERS:  prefix          - Character to be checked
  *
  * RETURN:      TRUE if a valid prefix
  *
@@ -119,7 +119,7 @@ u8 acpi_ns_valid_root_prefix(char prefix)
  *
  * FUNCTION:    acpi_ns_valid_path_separator
  *
- * PARAMETERS:  Sep         - Character to be checked
+ * PARAMETERS:  sep         - Character to be checked
  *
  * RETURN:      TRUE if a valid path separator
  *
@@ -137,7 +137,7 @@ static u8 acpi_ns_valid_path_separator(char sep)
  *
  * FUNCTION:    acpi_ns_get_type
  *
- * PARAMETERS:  Node        - Parent Node to be examined
+ * PARAMETERS:  node        - Parent Node to be examined
  *
  * RETURN:      Type field from Node whose handle is passed
  *
@@ -161,7 +161,7 @@ acpi_object_type acpi_ns_get_type(struct acpi_namespace_node * node)
  *
  * FUNCTION:    acpi_ns_local
  *
- * PARAMETERS:  Type        - A namespace object type
+ * PARAMETERS:  type        - A namespace object type
  *
  * RETURN:      LOCAL if names must be found locally in objects of the
  *              passed type, 0 if enclosing scopes should be searched
@@ -189,7 +189,7 @@ u32 acpi_ns_local(acpi_object_type type)
  *
  * FUNCTION:    acpi_ns_get_internal_name_length
  *
- * PARAMETERS:  Info            - Info struct initialized with the
+ * PARAMETERS:  info            - Info struct initialized with the
  *                                external name pointer.
  *
  * RETURN:      None
@@ -260,7 +260,7 @@ void acpi_ns_get_internal_name_length(struct acpi_namestring_info *info)
  *
  * FUNCTION:    acpi_ns_build_internal_name
  *
- * PARAMETERS:  Info            - Info struct fully initialized
+ * PARAMETERS:  info            - Info struct fully initialized
  *
  * RETURN:      Status
  *
@@ -371,7 +371,7 @@ acpi_status acpi_ns_build_internal_name(struct acpi_namestring_info *info)
  * FUNCTION:    acpi_ns_internalize_name
  *
  * PARAMETERS:  *external_name          - External representation of name
- *              **Converted Name        - Where to return the resulting
+ *              **Converted name        - Where to return the resulting
  *                                        internal represention of the name
  *
  * RETURN:      Status
@@ -575,7 +575,7 @@ acpi_ns_externalize_name(u32 internal_name_length,
  *
  * FUNCTION:    acpi_ns_validate_handle
  *
- * PARAMETERS:  Handle          - Handle to be validated and typecast to a
+ * PARAMETERS:  handle          - Handle to be validated and typecast to a
  *                                namespace node.
  *
  * RETURN:      A pointer to a namespace node
@@ -651,7 +651,7 @@ void acpi_ns_terminate(void)
  *
  * FUNCTION:    acpi_ns_opens_scope
  *
- * PARAMETERS:  Type        - A valid namespace type
+ * PARAMETERS:  type        - A valid namespace type
  *
  * RETURN:      NEWSCOPE if the passed type "opens a name scope" according
  *              to the ACPI specification, else 0
@@ -677,14 +677,14 @@ u32 acpi_ns_opens_scope(acpi_object_type type)
  *
  * FUNCTION:    acpi_ns_get_node
  *
- * PARAMETERS:  *Pathname   - Name to be found, in external (ASL) format. The
+ * PARAMETERS:  *pathname   - Name to be found, in external (ASL) format. The
  *                            \ (backslash) and ^ (carat) prefixes, and the
  *                            . (period) to separate segments are supported.
  *              prefix_node  - Root of subtree to be searched, or NS_ALL for the
  *                            root of the name space.  If Name is fully
  *                            qualified (first s8 is '\'), the passed value
  *                            of Scope will not be accessed.
- *              Flags       - Used to indicate whether to perform upsearch or
+ *              flags       - Used to indicate whether to perform upsearch or
  *                            not.
  *              return_node - Where the Node is returned
  *
diff --git a/drivers/acpi/acpica/nswalk.c b/drivers/acpi/acpica/nswalk.c
index f69895a5489..730bccc5e7f 100644
--- a/drivers/acpi/acpica/nswalk.c
+++ b/drivers/acpi/acpica/nswalk.c
@@ -88,7 +88,7 @@ struct acpi_namespace_node *acpi_ns_get_next_node(struct acpi_namespace_node
  *
  * FUNCTION:    acpi_ns_get_next_node_typed
  *
- * PARAMETERS:  Type                - Type of node to be searched for
+ * PARAMETERS:  type                - Type of node to be searched for
  *              parent_node         - Parent node whose children we are
  *                                    getting
  *              child_node          - Previous child that was found.
@@ -151,16 +151,16 @@ struct acpi_namespace_node *acpi_ns_get_next_node_typed(acpi_object_type type,
  *
  * FUNCTION:    acpi_ns_walk_namespace
  *
- * PARAMETERS:  Type                - acpi_object_type to search for
+ * PARAMETERS:  type                - acpi_object_type to search for
  *              start_node          - Handle in namespace where search begins
  *              max_depth           - Depth to which search is to reach
- *              Flags               - Whether to unlock the NS before invoking
+ *              flags               - Whether to unlock the NS before invoking
  *                                    the callback routine
  *              pre_order_visit     - Called during tree pre-order visit
  *                                    when an object of "Type" is found
  *              post_order_visit    - Called during tree post-order visit
  *                                    when an object of "Type" is found
- *              Context             - Passed to user function(s) above
+ *              context             - Passed to user function(s) above
  *              return_value        - from the user_function if terminated
  *                                    early. Otherwise, returns NULL.
  * RETURNS:     Status
diff --git a/drivers/acpi/acpica/nsxfeval.c b/drivers/acpi/acpica/nsxfeval.c
index 71d15f61807..9692e670233 100644
--- a/drivers/acpi/acpica/nsxfeval.c
+++ b/drivers/acpi/acpica/nsxfeval.c
@@ -58,8 +58,8 @@ static void acpi_ns_resolve_references(struct acpi_evaluate_info *info);
  *
  * FUNCTION:    acpi_evaluate_object_typed
  *
- * PARAMETERS:  Handle              - Object handle (optional)
- *              Pathname            - Object pathname (optional)
+ * PARAMETERS:  handle              - Object handle (optional)
+ *              pathname            - Object pathname (optional)
  *              external_params     - List of parameters to pass to method,
  *                                    terminated by NULL.  May be NULL
  *                                    if no parameters are being passed.
@@ -152,8 +152,8 @@ ACPI_EXPORT_SYMBOL(acpi_evaluate_object_typed)
  *
  * FUNCTION:    acpi_evaluate_object
  *
- * PARAMETERS:  Handle              - Object handle (optional)
- *              Pathname            - Object pathname (optional)
+ * PARAMETERS:  handle              - Object handle (optional)
+ *              pathname            - Object pathname (optional)
  *              external_params     - List of parameters to pass to method,
  *                                    terminated by NULL.  May be NULL
  *                                    if no parameters are being passed.
@@ -364,7 +364,7 @@ ACPI_EXPORT_SYMBOL(acpi_evaluate_object)
  *
  * FUNCTION:    acpi_ns_resolve_references
  *
- * PARAMETERS:  Info                    - Evaluation info block
+ * PARAMETERS:  info                    - Evaluation info block
  *
  * RETURN:      Info->return_object is replaced with the dereferenced object
  *
@@ -431,14 +431,14 @@ static void acpi_ns_resolve_references(struct acpi_evaluate_info *info)
  *
  * FUNCTION:    acpi_walk_namespace
  *
- * PARAMETERS:  Type                - acpi_object_type to search for
+ * PARAMETERS:  type                - acpi_object_type to search for
  *              start_object        - Handle in namespace where search begins
  *              max_depth           - Depth to which search is to reach
  *              pre_order_visit     - Called during tree pre-order visit
  *                                    when an object of "Type" is found
  *              post_order_visit    - Called during tree post-order visit
  *                                    when an object of "Type" is found
- *              Context             - Passed to user function(s) above
+ *              context             - Passed to user function(s) above
  *              return_value        - Location where return value of
  *                                    user_function is put if terminated early
  *
@@ -646,7 +646,7 @@ acpi_ns_get_device_callback(acpi_handle obj_handle,
  *
  * PARAMETERS:  HID                 - HID to search for. Can be NULL.
  *              user_function       - Called when a matching object is found
- *              Context             - Passed to user function
+ *              context             - Passed to user function
  *              return_value        - Location where return value of
  *                                    user_function is put if terminated early
  *
@@ -716,8 +716,8 @@ ACPI_EXPORT_SYMBOL(acpi_get_devices)
  * FUNCTION:    acpi_attach_data
  *
  * PARAMETERS:  obj_handle          - Namespace node
- *              Handler             - Handler for this attachment
- *              Data                - Pointer to data to be attached
+ *              handler             - Handler for this attachment
+ *              data                - Pointer to data to be attached
  *
  * RETURN:      Status
  *
@@ -764,7 +764,7 @@ ACPI_EXPORT_SYMBOL(acpi_attach_data)
  * FUNCTION:    acpi_detach_data
  *
  * PARAMETERS:  obj_handle          - Namespace node handle
- *              Handler             - Handler used in call to acpi_attach_data
+ *              handler             - Handler used in call to acpi_attach_data
  *
  * RETURN:      Status
  *
@@ -810,8 +810,8 @@ ACPI_EXPORT_SYMBOL(acpi_detach_data)
  * FUNCTION:    acpi_get_data
  *
  * PARAMETERS:  obj_handle          - Namespace node
- *              Handler             - Handler used in call to attach_data
- *              Data                - Where the data is returned
+ *              handler             - Handler used in call to attach_data
+ *              data                - Where the data is returned
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/nsxfname.c b/drivers/acpi/acpica/nsxfname.c
index af401c9c4df..08e9610b34c 100644
--- a/drivers/acpi/acpica/nsxfname.c
+++ b/drivers/acpi/acpica/nsxfname.c
@@ -61,8 +61,8 @@ static char *acpi_ns_copy_device_id(struct acpica_device_id *dest,
  *
  * FUNCTION:    acpi_get_handle
  *
- * PARAMETERS:  Parent          - Object to search under (search scope).
- *              Pathname        - Pointer to an asciiz string containing the
+ * PARAMETERS:  parent          - Object to search under (search scope).
+ *              pathname        - Pointer to an asciiz string containing the
  *                                name
  *              ret_handle      - Where the return handle is returned
  *
@@ -142,9 +142,9 @@ ACPI_EXPORT_SYMBOL(acpi_get_handle)
  *
  * FUNCTION:    acpi_get_name
  *
- * PARAMETERS:  Handle          - Handle to be converted to a pathname
+ * PARAMETERS:  handle          - Handle to be converted to a pathname
  *              name_type       - Full pathname or single segment
- *              Buffer          - Buffer for returned path
+ *              buffer          - Buffer for returned path
  *
  * RETURN:      Pointer to a string containing the fully qualified Name.
  *
@@ -219,8 +219,8 @@ ACPI_EXPORT_SYMBOL(acpi_get_name)
  *
  * FUNCTION:    acpi_ns_copy_device_id
  *
- * PARAMETERS:  Dest                - Pointer to the destination DEVICE_ID
- *              Source              - Pointer to the source DEVICE_ID
+ * PARAMETERS:  dest                - Pointer to the destination DEVICE_ID
+ *              source              - Pointer to the source DEVICE_ID
  *              string_area         - Pointer to where to copy the dest string
  *
  * RETURN:      Pointer to the next string area
@@ -247,7 +247,7 @@ static char *acpi_ns_copy_device_id(struct acpica_device_id *dest,
  *
  * FUNCTION:    acpi_get_object_info
  *
- * PARAMETERS:  Handle              - Object Handle
+ * PARAMETERS:  handle              - Object Handle
  *              return_buffer       - Where the info is returned
  *
  * RETURN:      Status
@@ -493,7 +493,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_object_info)
  *
  * FUNCTION:    acpi_install_method
  *
- * PARAMETERS:  Buffer         - An ACPI table containing one control method
+ * PARAMETERS:  buffer         - An ACPI table containing one control method
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/nsxfobj.c b/drivers/acpi/acpica/nsxfobj.c
index 880a605cee2..6766fc4f088 100644
--- a/drivers/acpi/acpica/nsxfobj.c
+++ b/drivers/acpi/acpica/nsxfobj.c
@@ -98,7 +98,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_id)
  *
  * FUNCTION:    acpi_get_type
  *
- * PARAMETERS:  Handle          - Handle of object whose type is desired
+ * PARAMETERS:  handle          - Handle of object whose type is desired
  *              ret_type        - Where the type will be placed
  *
  * RETURN:      Status
@@ -151,7 +151,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_type)
  *
  * FUNCTION:    acpi_get_parent
  *
- * PARAMETERS:  Handle          - Handle of object whose parent is desired
+ * PARAMETERS:  handle          - Handle of object whose parent is desired
  *              ret_handle      - Where the parent handle will be placed
  *
  * RETURN:      Status
@@ -212,8 +212,8 @@ ACPI_EXPORT_SYMBOL(acpi_get_parent)
  *
  * FUNCTION:    acpi_get_next_object
  *
- * PARAMETERS:  Type            - Type of object to be searched for
- *              Parent          - Parent object whose children we are getting
+ * PARAMETERS:  type            - Type of object to be searched for
+ *              parent          - Parent object whose children we are getting
  *              last_child      - Previous child that was found.
  *                                The NEXT child will be returned
  *              ret_handle      - Where handle to the next object is placed
diff --git a/drivers/acpi/acpica/psargs.c b/drivers/acpi/acpica/psargs.c
index 5ac36aba507..844464c4f90 100644
--- a/drivers/acpi/acpica/psargs.c
+++ b/drivers/acpi/acpica/psargs.c
@@ -210,7 +210,7 @@ char *acpi_ps_get_next_namestring(struct acpi_parse_state *parser_state)
  * FUNCTION:    acpi_ps_get_next_namepath
  *
  * PARAMETERS:  parser_state        - Current parser state object
- *              Arg                 - Where the namepath will be stored
+ *              arg                 - Where the namepath will be stored
  *              arg_count           - If the namepath points to a control method
  *                                    the method's argument is returned here.
  *              possible_method_call - Whether the namepath can possibly be the
@@ -379,7 +379,7 @@ acpi_ps_get_next_namepath(struct acpi_walk_state *walk_state,
  *
  * PARAMETERS:  parser_state        - Current parser state object
  *              arg_type            - The argument type (AML_*_ARG)
- *              Arg                 - Where the argument is returned
+ *              arg                 - Where the argument is returned
  *
  * RETURN:      None
  *
@@ -618,6 +618,7 @@ static union acpi_parse_object *acpi_ps_get_next_field(struct acpi_parse_state
 
 				arg = acpi_ps_alloc_op(AML_INT_BYTELIST_OP);
 				if (!arg) {
+					acpi_ps_free_op(field);
 					return_PTR(NULL);
 				}
 
@@ -662,6 +663,7 @@ static union acpi_parse_object *acpi_ps_get_next_field(struct acpi_parse_state
 		} else {
 			arg = acpi_ps_alloc_op(AML_INT_NAMEPATH_OP);
 			if (!arg) {
+				acpi_ps_free_op(field);
 				return_PTR(NULL);
 			}
 
diff --git a/drivers/acpi/acpica/psloop.c b/drivers/acpi/acpica/psloop.c
index 9547ad8a620..799162c1b6d 100644
--- a/drivers/acpi/acpica/psloop.c
+++ b/drivers/acpi/acpica/psloop.c
@@ -167,7 +167,7 @@ static acpi_status acpi_ps_get_aml_opcode(struct acpi_walk_state *walk_state)
  * PARAMETERS:  walk_state          - Current state
  *              aml_op_start        - Begin of named Op in AML
  *              unnamed_op          - Early Op (not a named Op)
- *              Op                  - Returned Op
+ *              op                  - Returned Op
  *
  * RETURN:      Status
  *
@@ -323,7 +323,7 @@ acpi_ps_create_op(struct acpi_walk_state *walk_state,
 
 	if (walk_state->op_info->flags & AML_CREATE) {
 		/*
-		 * Backup to beginning of create_xXXfield declaration
+		 * Backup to beginning of create_XXXfield declaration
 		 * body_length is unknown until we parse the body
 		 */
 		op->named.data = aml_op_start;
@@ -380,7 +380,7 @@ acpi_ps_create_op(struct acpi_walk_state *walk_state,
  *
  * PARAMETERS:  walk_state          - Current state
  *              aml_op_start        - Op start in AML
- *              Op                  - Current Op
+ *              op                  - Current Op
  *
  * RETURN:      Status
  *
@@ -679,8 +679,8 @@ acpi_ps_link_module_code(union acpi_parse_object *parent_op,
  * FUNCTION:    acpi_ps_complete_op
  *
  * PARAMETERS:  walk_state          - Current state
- *              Op                  - Returned Op
- *              Status              - Parse status before complete Op
+ *              op                  - Returned Op
+ *              status              - Parse status before complete Op
  *
  * RETURN:      Status
  *
@@ -853,8 +853,8 @@ acpi_ps_complete_op(struct acpi_walk_state *walk_state,
  * FUNCTION:    acpi_ps_complete_final_op
  *
  * PARAMETERS:  walk_state          - Current state
- *              Op                  - Current Op
- *              Status              - Current parse status before complete last
+ *              op                  - Current Op
+ *              status              - Current parse status before complete last
  *                                    Op
  *
  * RETURN:      Status
@@ -1165,7 +1165,7 @@ acpi_status acpi_ps_parse_loop(struct acpi_walk_state *walk_state)
 
 		if (walk_state->op_info->flags & AML_CREATE) {
 			/*
-			 * Backup to beginning of create_xXXfield declaration (1 for
+			 * Backup to beginning of create_XXXfield declaration (1 for
 			 * Opcode)
 			 *
 			 * body_length is unknown until we parse the body
diff --git a/drivers/acpi/acpica/psopcode.c b/drivers/acpi/acpica/psopcode.c
index a0226fdcf75..ed1d457bd5c 100644
--- a/drivers/acpi/acpica/psopcode.c
+++ b/drivers/acpi/acpica/psopcode.c
@@ -724,7 +724,7 @@ static const u8 acpi_gbl_long_op_index[NUM_EXTENDED_OPCODE] = {
  *
  * FUNCTION:    acpi_ps_get_opcode_info
  *
- * PARAMETERS:  Opcode              - The AML opcode
+ * PARAMETERS:  opcode              - The AML opcode
  *
  * RETURN:      A pointer to the info about the opcode.
  *
@@ -769,7 +769,7 @@ const struct acpi_opcode_info *acpi_ps_get_opcode_info(u16 opcode)
  *
  * FUNCTION:    acpi_ps_get_opcode_name
  *
- * PARAMETERS:  Opcode              - The AML opcode
+ * PARAMETERS:  opcode              - The AML opcode
  *
  * RETURN:      A pointer to the name of the opcode (ASCII String)
  *              Note: Never returns NULL.
diff --git a/drivers/acpi/acpica/psparse.c b/drivers/acpi/acpica/psparse.c
index 2ff9c35a196..01985703bb9 100644
--- a/drivers/acpi/acpica/psparse.c
+++ b/drivers/acpi/acpica/psparse.c
@@ -64,7 +64,7 @@ ACPI_MODULE_NAME("psparse")
  *
  * FUNCTION:    acpi_ps_get_opcode_size
  *
- * PARAMETERS:  Opcode          - An AML opcode
+ * PARAMETERS:  opcode          - An AML opcode
  *
  * RETURN:      Size of the opcode, in bytes (1 or 2)
  *
@@ -121,7 +121,7 @@ u16 acpi_ps_peek_opcode(struct acpi_parse_state * parser_state)
  * FUNCTION:    acpi_ps_complete_this_op
  *
  * PARAMETERS:  walk_state      - Current State
- *              Op              - Op to complete
+ *              op              - Op to complete
  *
  * RETURN:      Status
  *
@@ -311,7 +311,7 @@ acpi_ps_complete_this_op(struct acpi_walk_state * walk_state,
  * FUNCTION:    acpi_ps_next_parse_state
  *
  * PARAMETERS:  walk_state          - Current state
- *              Op                  - Current parse op
+ *              op                  - Current parse op
  *              callback_status     - Status from previous operation
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/psscope.c b/drivers/acpi/acpica/psscope.c
index c872aa4b926..608dc20dc17 100644
--- a/drivers/acpi/acpica/psscope.c
+++ b/drivers/acpi/acpica/psscope.c
@@ -93,7 +93,7 @@ u8 acpi_ps_has_completed_scope(struct acpi_parse_state * parser_state)
  * FUNCTION:    acpi_ps_init_scope
  *
  * PARAMETERS:  parser_state        - Current parser state object
- *              Root                - the Root Node of this new scope
+ *              root                - the Root Node of this new scope
  *
  * RETURN:      Status
  *
@@ -131,7 +131,7 @@ acpi_ps_init_scope(struct acpi_parse_state * parser_state,
  * FUNCTION:    acpi_ps_push_scope
  *
  * PARAMETERS:  parser_state        - Current parser state object
- *              Op                  - Current op to be pushed
+ *              op                  - Current op to be pushed
  *              remaining_args      - List of args remaining
  *              arg_count           - Fixed or variable number of args
  *
@@ -184,7 +184,7 @@ acpi_ps_push_scope(struct acpi_parse_state *parser_state,
  * FUNCTION:    acpi_ps_pop_scope
  *
  * PARAMETERS:  parser_state        - Current parser state object
- *              Op                  - Where the popped op is returned
+ *              op                  - Where the popped op is returned
  *              arg_list            - Where the popped "next argument" is
  *                                    returned
  *              arg_count           - Count of objects in arg_list
diff --git a/drivers/acpi/acpica/pstree.c b/drivers/acpi/acpica/pstree.c
index 2b03cdbbe1c..fdb2e71f304 100644
--- a/drivers/acpi/acpica/pstree.c
+++ b/drivers/acpi/acpica/pstree.c
@@ -58,8 +58,8 @@ union acpi_parse_object *acpi_ps_get_child(union acpi_parse_object *op);
  *
  * FUNCTION:    acpi_ps_get_arg
  *
- * PARAMETERS:  Op              - Get an argument for this op
- *              Argn            - Nth argument to get
+ * PARAMETERS:  op              - Get an argument for this op
+ *              argn            - Nth argument to get
  *
  * RETURN:      The argument (as an Op object). NULL if argument does not exist
  *
@@ -114,8 +114,8 @@ union acpi_parse_object *acpi_ps_get_arg(union acpi_parse_object *op, u32 argn)
  *
  * FUNCTION:    acpi_ps_append_arg
  *
- * PARAMETERS:  Op              - Append an argument to this Op.
- *              Arg             - Argument Op to append
+ * PARAMETERS:  op              - Append an argument to this Op.
+ *              arg             - Argument Op to append
  *
  * RETURN:      None.
  *
@@ -188,8 +188,8 @@ acpi_ps_append_arg(union acpi_parse_object *op, union acpi_parse_object *arg)
  *
  * FUNCTION:    acpi_ps_get_depth_next
  *
- * PARAMETERS:  Origin          - Root of subtree to search
- *              Op              - Last (previous) Op that was found
+ * PARAMETERS:  origin          - Root of subtree to search
+ *              op              - Last (previous) Op that was found
  *
  * RETURN:      Next Op found in the search.
  *
@@ -261,7 +261,7 @@ union acpi_parse_object *acpi_ps_get_depth_next(union acpi_parse_object *origin,
  *
  * FUNCTION:    acpi_ps_get_child
  *
- * PARAMETERS:  Op              - Get the child of this Op
+ * PARAMETERS:  op              - Get the child of this Op
  *
  * RETURN:      Child Op, Null if none is found.
  *
diff --git a/drivers/acpi/acpica/psutils.c b/drivers/acpi/acpica/psutils.c
index 13bb131ae12..8736ad5f04d 100644
--- a/drivers/acpi/acpica/psutils.c
+++ b/drivers/acpi/acpica/psutils.c
@@ -77,8 +77,8 @@ union acpi_parse_object *acpi_ps_create_scope_op(void)
  *
  * FUNCTION:    acpi_ps_init_op
  *
- * PARAMETERS:  Op              - A newly allocated Op object
- *              Opcode          - Opcode to store in the Op
+ * PARAMETERS:  op              - A newly allocated Op object
+ *              opcode          - Opcode to store in the Op
  *
  * RETURN:      None
  *
@@ -103,7 +103,7 @@ void acpi_ps_init_op(union acpi_parse_object *op, u16 opcode)
  *
  * FUNCTION:    acpi_ps_alloc_op
  *
- * PARAMETERS:  Opcode          - Opcode that will be stored in the new Op
+ * PARAMETERS:  opcode          - Opcode that will be stored in the new Op
  *
  * RETURN:      Pointer to the new Op, null on failure
  *
@@ -160,7 +160,7 @@ union acpi_parse_object *acpi_ps_alloc_op(u16 opcode)
  *
  * FUNCTION:    acpi_ps_free_op
  *
- * PARAMETERS:  Op              - Op to be freed
+ * PARAMETERS:  op              - Op to be freed
  *
  * RETURN:      None.
  *
diff --git a/drivers/acpi/acpica/psxface.c b/drivers/acpi/acpica/psxface.c
index 9d98c5ff66a..963e1622579 100644
--- a/drivers/acpi/acpica/psxface.c
+++ b/drivers/acpi/acpica/psxface.c
@@ -66,7 +66,7 @@ acpi_ps_update_parameter_list(struct acpi_evaluate_info *info, u16 action);
  * PARAMETERS:  method_name     - Valid ACPI name string
  *              debug_level     - Optional level mask. 0 to use default
  *              debug_layer     - Optional layer mask. 0 to use default
- *              Flags           - bit 1: one shot(1) or persistent(0)
+ *              flags           - bit 1: one shot(1) or persistent(0)
  *
  * RETURN:      Status
  *
@@ -105,7 +105,7 @@ acpi_debug_trace(char *name, u32 debug_level, u32 debug_layer, u32 flags)
  *
  * FUNCTION:    acpi_ps_start_trace
  *
- * PARAMETERS:  Info        - Method info struct
+ * PARAMETERS:  info        - Method info struct
  *
  * RETURN:      None
  *
@@ -150,7 +150,7 @@ static void acpi_ps_start_trace(struct acpi_evaluate_info *info)
  *
  * FUNCTION:    acpi_ps_stop_trace
  *
- * PARAMETERS:  Info        - Method info struct
+ * PARAMETERS:  info        - Method info struct
  *
  * RETURN:      None
  *
@@ -193,10 +193,10 @@ static void acpi_ps_stop_trace(struct acpi_evaluate_info *info)
  *
  * FUNCTION:    acpi_ps_execute_method
  *
- * PARAMETERS:  Info            - Method info block, contains:
- *                  Node            - Method Node to execute
+ * PARAMETERS:  info            - Method info block, contains:
+ *                  node            - Method Node to execute
  *                  obj_desc        - Method object
- *                  Parameters      - List of parameters to pass to the method,
+ *                  parameters      - List of parameters to pass to the method,
  *                                    terminated by NULL. Params itself may be
  *                                    NULL if no parameters are being passed.
  *                  return_object   - Where to put method's return value (if
@@ -361,9 +361,9 @@ acpi_status acpi_ps_execute_method(struct acpi_evaluate_info *info)
  *
  * FUNCTION:    acpi_ps_update_parameter_list
  *
- * PARAMETERS:  Info            - See struct acpi_evaluate_info
+ * PARAMETERS:  info            - See struct acpi_evaluate_info
  *                                (Used: parameter_type and Parameters)
- *              Action          - Add or Remove reference
+ *              action          - Add or Remove reference
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/rsaddr.c b/drivers/acpi/acpica/rsaddr.c
index a0305652394..856ff075b6a 100644
--- a/drivers/acpi/acpica/rsaddr.c
+++ b/drivers/acpi/acpica/rsaddr.c
@@ -182,8 +182,8 @@ struct acpi_rsconvert_info acpi_rs_convert_ext_address64[5] = {
 
 	/* Revision ID */
 
-	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.ext_address64.revision_iD),
-	 AML_OFFSET(ext_address64.revision_iD),
+	{ACPI_RSC_MOVE8, ACPI_RS_OFFSET(data.ext_address64.revision_ID),
+	 AML_OFFSET(ext_address64.revision_ID),
 	 1},
 	/*
 	 * These fields are contiguous in both the source and destination:
@@ -215,7 +215,7 @@ static struct acpi_rsconvert_info acpi_rs_convert_general_flags[6] = {
 	 AML_OFFSET(address.resource_type),
 	 1},
 
-	/* General Flags - Consume, Decode, min_fixed, max_fixed */
+	/* General flags - Consume, Decode, min_fixed, max_fixed */
 
 	{ACPI_RSC_1BITFLAG, ACPI_RS_OFFSET(data.address.producer_consumer),
 	 AML_OFFSET(address.flags),
@@ -293,8 +293,8 @@ static struct acpi_rsconvert_info acpi_rs_convert_io_flags[4] = {
  *
  * FUNCTION:    acpi_rs_get_address_common
  *
- * PARAMETERS:  Resource            - Pointer to the internal resource struct
- *              Aml                 - Pointer to the AML resource descriptor
+ * PARAMETERS:  resource            - Pointer to the internal resource struct
+ *              aml                 - Pointer to the AML resource descriptor
  *
  * RETURN:      TRUE if the resource_type field is OK, FALSE otherwise
  *
@@ -343,8 +343,8 @@ acpi_rs_get_address_common(struct acpi_resource *resource,
  *
  * FUNCTION:    acpi_rs_set_address_common
  *
- * PARAMETERS:  Aml                 - Pointer to the AML resource descriptor
- *              Resource            - Pointer to the internal resource struct
+ * PARAMETERS:  aml                 - Pointer to the AML resource descriptor
+ *              resource            - Pointer to the internal resource struct
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/rscalc.c b/drivers/acpi/acpica/rscalc.c
index 3c6df4b7eb2..de12469d1c9 100644
--- a/drivers/acpi/acpica/rscalc.c
+++ b/drivers/acpi/acpica/rscalc.c
@@ -173,7 +173,7 @@ acpi_rs_stream_option_length(u32 resource_length,
  *
  * FUNCTION:    acpi_rs_get_aml_length
  *
- * PARAMETERS:  Resource            - Pointer to the resource linked list
+ * PARAMETERS:  resource            - Pointer to the resource linked list
  *              size_needed         - Where the required size is returned
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/rscreate.c b/drivers/acpi/acpica/rscreate.c
index 46d6eb38ae6..311cbc4f05f 100644
--- a/drivers/acpi/acpica/rscreate.c
+++ b/drivers/acpi/acpica/rscreate.c
@@ -190,8 +190,8 @@ acpi_rs_create_resource_list(union acpi_operand_object *aml_buffer,
  *
  * FUNCTION:    acpi_rs_create_pci_routing_table
  *
- * PARAMETERS:  package_object          - Pointer to a union acpi_operand_object
- *                                        package
+ * PARAMETERS:  package_object          - Pointer to a package containing one
+ *                                        of more ACPI_OPERAND_OBJECTs
  *              output_buffer           - Pointer to the user's buffer
  *
  * RETURN:      Status  AE_OK if okay, else a valid acpi_status code.
@@ -199,7 +199,7 @@ acpi_rs_create_resource_list(union acpi_operand_object *aml_buffer,
  *              AE_BUFFER_OVERFLOW and output_buffer->Length will point
  *              to the size buffer needed.
  *
- * DESCRIPTION: Takes the union acpi_operand_object    package and creates a
+ * DESCRIPTION: Takes the union acpi_operand_object package and creates a
  *              linked list of PCI interrupt descriptions
  *
  * NOTE: It is the caller's responsibility to ensure that the start of the
diff --git a/drivers/acpi/acpica/rsdump.c b/drivers/acpi/acpica/rsdump.c
index b4c58113239..4d11b072388 100644
--- a/drivers/acpi/acpica/rsdump.c
+++ b/drivers/acpi/acpica/rsdump.c
@@ -703,7 +703,7 @@ acpi_rs_dump_resource_source(struct acpi_resource_source *resource_source)
  *
  * FUNCTION:    acpi_rs_dump_address_common
  *
- * PARAMETERS:  Resource        - Pointer to an internal resource descriptor
+ * PARAMETERS:  resource        - Pointer to an internal resource descriptor
  *
  * RETURN:      None
  *
@@ -850,8 +850,8 @@ void acpi_rs_dump_irq_list(u8 * route_table)
  *
  * FUNCTION:    acpi_rs_out*
  *
- * PARAMETERS:  Title       - Name of the resource field
- *              Value       - Value of the resource field
+ * PARAMETERS:  title       - Name of the resource field
+ *              value       - Value of the resource field
  *
  * RETURN:      None
  *
@@ -898,8 +898,8 @@ static void acpi_rs_out_title(char *title)
  *
  * FUNCTION:    acpi_rs_dump*List
  *
- * PARAMETERS:  Length      - Number of elements in the list
- *              Data        - Start of the list
+ * PARAMETERS:  length      - Number of elements in the list
+ *              data        - Start of the list
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/rslist.c b/drivers/acpi/acpica/rslist.c
index 9be129f5d6f..46b5324b22d 100644
--- a/drivers/acpi/acpica/rslist.c
+++ b/drivers/acpi/acpica/rslist.c
@@ -139,7 +139,7 @@ acpi_rs_convert_aml_to_resources(u8 * aml,
  *
  * FUNCTION:    acpi_rs_convert_resources_to_aml
  *
- * PARAMETERS:  Resource            - Pointer to the resource linked list
+ * PARAMETERS:  resource            - Pointer to the resource linked list
  *              aml_size_needed     - Calculated size of the byte stream
  *                                    needed from calling acpi_rs_get_aml_length()
  *                                    The size of the output_buffer is
diff --git a/drivers/acpi/acpica/rsmisc.c b/drivers/acpi/acpica/rsmisc.c
index 8073b371cc7..c6f291c2bc8 100644
--- a/drivers/acpi/acpica/rsmisc.c
+++ b/drivers/acpi/acpica/rsmisc.c
@@ -57,9 +57,9 @@ ACPI_MODULE_NAME("rsmisc")
  *
  * FUNCTION:    acpi_rs_convert_aml_to_resource
  *
- * PARAMETERS:  Resource            - Pointer to the resource descriptor
- *              Aml                 - Where the AML descriptor is returned
- *              Info                - Pointer to appropriate conversion table
+ * PARAMETERS:  resource            - Pointer to the resource descriptor
+ *              aml                 - Where the AML descriptor is returned
+ *              info                - Pointer to appropriate conversion table
  *
  * RETURN:      Status
  *
@@ -406,7 +406,7 @@ acpi_rs_convert_aml_to_resource(struct acpi_resource *resource,
 
 		case ACPI_RSC_EXIT_NE:
 			/*
-			 * Control - Exit conversion if not equal
+			 * control - Exit conversion if not equal
 			 */
 			switch (info->resource_offset) {
 			case ACPI_RSC_COMPARE_AML_LENGTH:
@@ -454,9 +454,9 @@ acpi_rs_convert_aml_to_resource(struct acpi_resource *resource,
  *
  * FUNCTION:    acpi_rs_convert_resource_to_aml
  *
- * PARAMETERS:  Resource            - Pointer to the resource descriptor
- *              Aml                 - Where the AML descriptor is returned
- *              Info                - Pointer to appropriate conversion table
+ * PARAMETERS:  resource            - Pointer to the resource descriptor
+ *              aml                 - Where the AML descriptor is returned
+ *              info                - Pointer to appropriate conversion table
  *
  * RETURN:      Status
  *
@@ -726,7 +726,7 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 
 		case ACPI_RSC_EXIT_LE:
 			/*
-			 * Control - Exit conversion if less than or equal
+			 * control - Exit conversion if less than or equal
 			 */
 			if (item_count <= info->value) {
 				goto exit;
@@ -735,7 +735,7 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 
 		case ACPI_RSC_EXIT_NE:
 			/*
-			 * Control - Exit conversion if not equal
+			 * control - Exit conversion if not equal
 			 */
 			switch (COMPARE_OPCODE(info)) {
 			case ACPI_RSC_COMPARE_VALUE:
@@ -757,7 +757,7 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 
 		case ACPI_RSC_EXIT_EQ:
 			/*
-			 * Control - Exit conversion if equal
+			 * control - Exit conversion if equal
 			 */
 			if (*ACPI_ADD_PTR(u8, resource,
 					  COMPARE_TARGET(info)) ==
@@ -783,7 +783,7 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 #if 0
 /* Previous resource validations */
 
-if (aml->ext_address64.revision_iD != AML_RESOURCE_EXTENDED_ADDRESS_REVISION) {
+if (aml->ext_address64.revision_ID != AML_RESOURCE_EXTENDED_ADDRESS_REVISION) {
 	return_ACPI_STATUS(AE_SUPPORT);
 }
 
diff --git a/drivers/acpi/acpica/rsutils.c b/drivers/acpi/acpica/rsutils.c
index 433a375deb9..37d5241c0ac 100644
--- a/drivers/acpi/acpica/rsutils.c
+++ b/drivers/acpi/acpica/rsutils.c
@@ -53,8 +53,8 @@ ACPI_MODULE_NAME("rsutils")
  *
  * FUNCTION:    acpi_rs_decode_bitmask
  *
- * PARAMETERS:  Mask            - Bitmask to decode
- *              List            - Where the converted list is returned
+ * PARAMETERS:  mask            - Bitmask to decode
+ *              list            - Where the converted list is returned
  *
  * RETURN:      Count of bits set (length of list)
  *
@@ -86,8 +86,8 @@ u8 acpi_rs_decode_bitmask(u16 mask, u8 * list)
  *
  * FUNCTION:    acpi_rs_encode_bitmask
  *
- * PARAMETERS:  List            - List of values to encode
- *              Count           - Length of list
+ * PARAMETERS:  list            - List of values to encode
+ *              count           - Length of list
  *
  * RETURN:      Encoded bitmask
  *
@@ -115,8 +115,8 @@ u16 acpi_rs_encode_bitmask(u8 * list, u8 count)
  *
  * FUNCTION:    acpi_rs_move_data
  *
- * PARAMETERS:  Destination         - Pointer to the destination descriptor
- *              Source              - Pointer to the source descriptor
+ * PARAMETERS:  destination         - Pointer to the destination descriptor
+ *              source              - Pointer to the source descriptor
  *              item_count          - How many items to move
  *              move_type           - Byte width
  *
@@ -183,7 +183,7 @@ acpi_rs_move_data(void *destination, void *source, u16 item_count, u8 move_type)
  *
  * PARAMETERS:  total_length        - Length of the AML descriptor, including
  *                                    the header and length fields.
- *              Aml                 - Pointer to the raw AML descriptor
+ *              aml                 - Pointer to the raw AML descriptor
  *
  * RETURN:      None
  *
@@ -235,7 +235,7 @@ acpi_rs_set_resource_length(acpi_rsdesc_size total_length,
  * PARAMETERS:  descriptor_type     - Byte to be inserted as the type
  *              total_length        - Length of the AML descriptor, including
  *                                    the header and length fields.
- *              Aml                 - Pointer to the raw AML descriptor
+ *              aml                 - Pointer to the raw AML descriptor
  *
  * RETURN:      None
  *
@@ -265,8 +265,8 @@ acpi_rs_set_resource_header(u8 descriptor_type,
  *
  * FUNCTION:    acpi_rs_strcpy
  *
- * PARAMETERS:  Destination         - Pointer to the destination string
- *              Source              - Pointer to the source string
+ * PARAMETERS:  destination         - Pointer to the destination string
+ *              source              - Pointer to the source string
  *
  * RETURN:      String length, including NULL terminator
  *
@@ -300,7 +300,7 @@ static u16 acpi_rs_strcpy(char *destination, char *source)
  *              minimum_length      - Minimum length of the descriptor (minus
  *                                    any optional fields)
  *              resource_source     - Where the resource_source is returned
- *              Aml                 - Pointer to the raw AML descriptor
+ *              aml                 - Pointer to the raw AML descriptor
  *              string_ptr          - (optional) where to store the actual
  *                                    resource_source string
  *
@@ -386,7 +386,7 @@ acpi_rs_get_resource_source(acpi_rs_length resource_length,
  *
  * FUNCTION:    acpi_rs_set_resource_source
  *
- * PARAMETERS:  Aml                 - Pointer to the raw AML descriptor
+ * PARAMETERS:  aml                 - Pointer to the raw AML descriptor
  *              minimum_length      - Minimum length of the descriptor (minus
  *                                    any optional fields)
  *              resource_source     - Internal resource_source
@@ -445,7 +445,7 @@ acpi_rs_set_resource_source(union aml_resource * aml,
  *
  * FUNCTION:    acpi_rs_get_prt_method_data
  *
- * PARAMETERS:  Node            - Device node
+ * PARAMETERS:  node            - Device node
  *              ret_buffer      - Pointer to a buffer structure for the
  *                                results
  *
@@ -494,7 +494,7 @@ acpi_rs_get_prt_method_data(struct acpi_namespace_node * node,
  *
  * FUNCTION:    acpi_rs_get_crs_method_data
  *
- * PARAMETERS:  Node            - Device node
+ * PARAMETERS:  node            - Device node
  *              ret_buffer      - Pointer to a buffer structure for the
  *                                results
  *
@@ -534,7 +534,7 @@ acpi_rs_get_crs_method_data(struct acpi_namespace_node *node,
 	 */
 	status = acpi_rs_create_resource_list(obj_desc, ret_buffer);
 
-	/* On exit, we must delete the object returned by evaluate_object */
+	/* On exit, we must delete the object returned by evaluateObject */
 
 	acpi_ut_remove_reference(obj_desc);
 	return_ACPI_STATUS(status);
@@ -544,7 +544,7 @@ acpi_rs_get_crs_method_data(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_rs_get_prs_method_data
  *
- * PARAMETERS:  Node            - Device node
+ * PARAMETERS:  node            - Device node
  *              ret_buffer      - Pointer to a buffer structure for the
  *                                results
  *
@@ -585,7 +585,7 @@ acpi_rs_get_prs_method_data(struct acpi_namespace_node *node,
 	 */
 	status = acpi_rs_create_resource_list(obj_desc, ret_buffer);
 
-	/* On exit, we must delete the object returned by evaluate_object */
+	/* On exit, we must delete the object returned by evaluateObject */
 
 	acpi_ut_remove_reference(obj_desc);
 	return_ACPI_STATUS(status);
@@ -596,7 +596,7 @@ acpi_rs_get_prs_method_data(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_rs_get_aei_method_data
  *
- * PARAMETERS:  Node            - Device node
+ * PARAMETERS:  node            - Device node
  *              ret_buffer      - Pointer to a buffer structure for the
  *                                results
  *
@@ -636,7 +636,7 @@ acpi_rs_get_aei_method_data(struct acpi_namespace_node *node,
 	 */
 	status = acpi_rs_create_resource_list(obj_desc, ret_buffer);
 
-	/* On exit, we must delete the object returned by evaluate_object */
+	/* On exit, we must delete the object returned by evaluateObject */
 
 	acpi_ut_remove_reference(obj_desc);
 	return_ACPI_STATUS(status);
@@ -646,8 +646,8 @@ acpi_rs_get_aei_method_data(struct acpi_namespace_node *node,
  *
  * FUNCTION:    acpi_rs_get_method_data
  *
- * PARAMETERS:  Handle          - Handle to the containing object
- *              Path            - Path to method, relative to Handle
+ * PARAMETERS:  handle          - Handle to the containing object
+ *              path            - Path to method, relative to Handle
  *              ret_buffer      - Pointer to a buffer structure for the
  *                                results
  *
@@ -697,7 +697,7 @@ acpi_rs_get_method_data(acpi_handle handle,
  *
  * FUNCTION:    acpi_rs_set_srs_method_data
  *
- * PARAMETERS:  Node            - Device node
+ * PARAMETERS:  node            - Device node
  *              in_buffer       - Pointer to a buffer structure of the
  *                                parameter
  *
diff --git a/drivers/acpi/acpica/rsxface.c b/drivers/acpi/acpica/rsxface.c
index f58c098c7ae..5aad744b5b8 100644
--- a/drivers/acpi/acpica/rsxface.c
+++ b/drivers/acpi/acpica/rsxface.c
@@ -79,7 +79,7 @@ acpi_rs_validate_parameters(acpi_handle device_handle,
  * FUNCTION:    acpi_rs_validate_parameters
  *
  * PARAMETERS:  device_handle   - Handle to a device
- *              Buffer          - Pointer to a data buffer
+ *              buffer          - Pointer to a data buffer
  *              return_node     - Pointer to where the device node is returned
  *
  * RETURN:      Status
@@ -351,8 +351,8 @@ ACPI_EXPORT_SYMBOL(acpi_get_event_resources)
  *
  * FUNCTION:    acpi_resource_to_address64
  *
- * PARAMETERS:  Resource        - Pointer to a resource
- *              Out             - Pointer to the users's return buffer
+ * PARAMETERS:  resource        - Pointer to a resource
+ *              out             - Pointer to the users's return buffer
  *                                (a struct acpi_resource_address64)
  *
  * RETURN:      Status
@@ -415,9 +415,9 @@ ACPI_EXPORT_SYMBOL(acpi_resource_to_address64)
  * FUNCTION:    acpi_get_vendor_resource
  *
  * PARAMETERS:  device_handle   - Handle for the parent device object
- *              Name            - Method name for the parent resource
+ *              name            - Method name for the parent resource
  *                                (METHOD_NAME__CRS or METHOD_NAME__PRS)
- *              Uuid            - Pointer to the UUID to be matched.
+ *              uuid            - Pointer to the UUID to be matched.
  *                                includes both subtype and 16-byte UUID
  *              ret_buffer      - Where the vendor resource is returned
  *
@@ -526,11 +526,11 @@ acpi_rs_match_vendor_resource(struct acpi_resource *resource, void *context)
  *
  * PARAMETERS:  device_handle   - Handle to the device object for the
  *                                device we are querying
- *              Name            - Method name of the resources we want.
+ *              name            - Method name of the resources we want.
  *                                (METHOD_NAME__CRS, METHOD_NAME__PRS, or
  *                                METHOD_NAME__AEI)
  *              user_function   - Called for each resource
- *              Context         - Passed to user_function
+ *              context         - Passed to user_function
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index 4c9c760db4a..390651860bf 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -49,9 +49,10 @@
 ACPI_MODULE_NAME("tbfadt")
 
 /* Local prototypes */
-static ACPI_INLINE void
+static void
 acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
-			     u8 space_id, u8 byte_width, u64 address);
+			     u8 space_id,
+			     u8 byte_width, u64 address, char *register_name);
 
 static void acpi_tb_convert_fadt(void);
 
@@ -172,7 +173,7 @@ static struct acpi_fadt_pm_info fadt_pm_info_table[] = {
  *
  * PARAMETERS:  generic_address     - GAS struct to be initialized
  *              byte_width          - Width of this register
- *              Address             - Address of the register
+ *              address             - Address of the register
  *
  * RETURN:      None
  *
@@ -182,10 +183,25 @@ static struct acpi_fadt_pm_info fadt_pm_info_table[] = {
  *
  ******************************************************************************/
 
-static ACPI_INLINE void
+static void
 acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
-			     u8 space_id, u8 byte_width, u64 address)
+			     u8 space_id,
+			     u8 byte_width, u64 address, char *register_name)
 {
+	u8 bit_width;
+
+	/* Bit width field in the GAS is only one byte long, 255 max */
+
+	bit_width = (u8)(byte_width * 8);
+
+	if (byte_width > 31) {	/* (31*8)=248 */
+		ACPI_ERROR((AE_INFO,
+			    "%s - 32-bit FADT register is too long (%u bytes, %u bits) "
+			    "to convert to GAS struct - 255 bits max, truncating",
+			    register_name, byte_width, (byte_width * 8)));
+
+		bit_width = 255;
+	}
 
 	/*
 	 * The 64-bit Address field is non-aligned in the byte packed
@@ -196,7 +212,7 @@ acpi_tb_init_generic_address(struct acpi_generic_address *generic_address,
 	/* All other fields are byte-wide */
 
 	generic_address->space_id = space_id;
-	generic_address->bit_width = (u8)ACPI_MUL_8(byte_width);
+	generic_address->bit_width = bit_width;
 	generic_address->bit_offset = 0;
 	generic_address->access_width = 0;	/* Access width ANY */
 }
@@ -267,8 +283,8 @@ void acpi_tb_parse_fadt(u32 table_index)
  *
  * FUNCTION:    acpi_tb_create_local_fadt
  *
- * PARAMETERS:  Table               - Pointer to BIOS FADT
- *              Length              - Length of the table
+ * PARAMETERS:  table               - Pointer to BIOS FADT
+ *              length              - Length of the table
  *
  * RETURN:      None
  *
@@ -287,11 +303,11 @@ void acpi_tb_create_local_fadt(struct acpi_table_header *table, u32 length)
 	 * a warning.
 	 */
 	if (length > sizeof(struct acpi_table_fadt)) {
-		ACPI_WARNING((AE_INFO,
-			      "FADT (revision %u) is longer than ACPI 5.0 version, "
-			      "truncating length %u to %u",
-			      table->revision, length,
-			      (u32)sizeof(struct acpi_table_fadt)));
+		ACPI_BIOS_WARNING((AE_INFO,
+				   "FADT (revision %u) is longer than ACPI 5.0 version, "
+				   "truncating length %u to %u",
+				   table->revision, length,
+				   (u32)sizeof(struct acpi_table_fadt)));
 	}
 
 	/* Clear the entire local FADT */
@@ -436,11 +452,13 @@ static void acpi_tb_convert_fadt(void)
 		 * they must match.
 		 */
 		if (address64->address && address32 &&
-		    (address64->address != (u64) address32)) {
-			ACPI_ERROR((AE_INFO,
-				    "32/64X address mismatch in %s: 0x%8.8X/0x%8.8X%8.8X, using 32",
-				    fadt_info_table[i].name, address32,
-				    ACPI_FORMAT_UINT64(address64->address)));
+		    (address64->address != (u64)address32)) {
+			ACPI_BIOS_ERROR((AE_INFO,
+					 "32/64X address mismatch in FADT/%s: "
+					 "0x%8.8X/0x%8.8X%8.8X, using 32",
+					 fadt_info_table[i].name, address32,
+					 ACPI_FORMAT_UINT64(address64->
+							    address)));
 		}
 
 		/* Always use 32-bit address if it is valid (non-null) */
@@ -456,7 +474,8 @@ static void acpi_tb_convert_fadt(void)
 								   &acpi_gbl_FADT,
 								   fadt_info_table
 								   [i].length),
-						     (u64) address32);
+						     (u64) address32,
+						     fadt_info_table[i].name);
 		}
 	}
 }
@@ -465,7 +484,7 @@ static void acpi_tb_convert_fadt(void)
  *
  * FUNCTION:    acpi_tb_validate_fadt
  *
- * PARAMETERS:  Table           - Pointer to the FADT to be validated
+ * PARAMETERS:  table           - Pointer to the FADT to be validated
  *
  * RETURN:      None
  *
@@ -494,25 +513,25 @@ static void acpi_tb_validate_fadt(void)
 	 * DSDT/X_DSDT) would indicate the presence of two FACS or two DSDT tables.
 	 */
 	if (acpi_gbl_FADT.facs &&
-	    (acpi_gbl_FADT.Xfacs != (u64) acpi_gbl_FADT.facs)) {
-		ACPI_WARNING((AE_INFO,
-			      "32/64X FACS address mismatch in FADT - "
-			      "0x%8.8X/0x%8.8X%8.8X, using 32",
-			      acpi_gbl_FADT.facs,
-			      ACPI_FORMAT_UINT64(acpi_gbl_FADT.Xfacs)));
-
-		acpi_gbl_FADT.Xfacs = (u64) acpi_gbl_FADT.facs;
+	    (acpi_gbl_FADT.Xfacs != (u64)acpi_gbl_FADT.facs)) {
+		ACPI_BIOS_WARNING((AE_INFO,
+				   "32/64X FACS address mismatch in FADT - "
+				   "0x%8.8X/0x%8.8X%8.8X, using 32",
+				   acpi_gbl_FADT.facs,
+				   ACPI_FORMAT_UINT64(acpi_gbl_FADT.Xfacs)));
+
+		acpi_gbl_FADT.Xfacs = (u64)acpi_gbl_FADT.facs;
 	}
 
 	if (acpi_gbl_FADT.dsdt &&
-	    (acpi_gbl_FADT.Xdsdt != (u64) acpi_gbl_FADT.dsdt)) {
-		ACPI_WARNING((AE_INFO,
-			      "32/64X DSDT address mismatch in FADT - "
-			      "0x%8.8X/0x%8.8X%8.8X, using 32",
-			      acpi_gbl_FADT.dsdt,
-			      ACPI_FORMAT_UINT64(acpi_gbl_FADT.Xdsdt)));
-
-		acpi_gbl_FADT.Xdsdt = (u64) acpi_gbl_FADT.dsdt;
+	    (acpi_gbl_FADT.Xdsdt != (u64)acpi_gbl_FADT.dsdt)) {
+		ACPI_BIOS_WARNING((AE_INFO,
+				   "32/64X DSDT address mismatch in FADT - "
+				   "0x%8.8X/0x%8.8X%8.8X, using 32",
+				   acpi_gbl_FADT.dsdt,
+				   ACPI_FORMAT_UINT64(acpi_gbl_FADT.Xdsdt)));
+
+		acpi_gbl_FADT.Xdsdt = (u64)acpi_gbl_FADT.dsdt;
 	}
 
 	/* If Hardware Reduced flag is set, we are all done */
@@ -542,10 +561,10 @@ static void acpi_tb_validate_fadt(void)
 		 */
 		if (address64->address &&
 		    (address64->bit_width != ACPI_MUL_8(length))) {
-			ACPI_WARNING((AE_INFO,
-				      "32/64X length mismatch in %s: %u/%u",
-				      name, ACPI_MUL_8(length),
-				      address64->bit_width));
+			ACPI_BIOS_WARNING((AE_INFO,
+					   "32/64X length mismatch in FADT/%s: %u/%u",
+					   name, ACPI_MUL_8(length),
+					   address64->bit_width));
 		}
 
 		if (fadt_info_table[i].type & ACPI_FADT_REQUIRED) {
@@ -554,29 +573,29 @@ static void acpi_tb_validate_fadt(void)
 			 * Both the address and length must be non-zero.
 			 */
 			if (!address64->address || !length) {
-				ACPI_ERROR((AE_INFO,
-					    "Required field %s has zero address and/or length:"
-					    " 0x%8.8X%8.8X/0x%X",
-					    name,
-					    ACPI_FORMAT_UINT64(address64->
-							       address),
-					    length));
+				ACPI_BIOS_ERROR((AE_INFO,
+						 "Required FADT field %s has zero address and/or length: "
+						 "0x%8.8X%8.8X/0x%X",
+						 name,
+						 ACPI_FORMAT_UINT64(address64->
+								    address),
+						 length));
 			}
 		} else if (fadt_info_table[i].type & ACPI_FADT_SEPARATE_LENGTH) {
 			/*
-			 * Field is optional (PM2Control, GPE0, GPE1) AND has its own
+			 * Field is optional (Pm2_control, GPE0, GPE1) AND has its own
 			 * length field. If present, both the address and length must
 			 * be valid.
 			 */
 			if ((address64->address && !length) ||
 			    (!address64->address && length)) {
-				ACPI_WARNING((AE_INFO,
-					      "Optional field %s has zero address or length: "
-					      "0x%8.8X%8.8X/0x%X",
-					      name,
-					      ACPI_FORMAT_UINT64(address64->
-								 address),
-					      length));
+				ACPI_BIOS_WARNING((AE_INFO,
+						   "Optional FADT field %s has zero address or length: "
+						   "0x%8.8X%8.8X/0x%X",
+						   name,
+						   ACPI_FORMAT_UINT64
+						   (address64->address),
+						   length));
 			}
 		}
 	}
@@ -621,12 +640,12 @@ static void acpi_tb_setup_fadt_registers(void)
 			    (fadt_info_table[i].default_length > 0) &&
 			    (fadt_info_table[i].default_length !=
 			     target64->bit_width)) {
-				ACPI_WARNING((AE_INFO,
-					      "Invalid length for %s: %u, using default %u",
-					      fadt_info_table[i].name,
-					      target64->bit_width,
-					      fadt_info_table[i].
-					      default_length));
+				ACPI_BIOS_WARNING((AE_INFO,
+						   "Invalid length for FADT/%s: %u, using default %u",
+						   fadt_info_table[i].name,
+						   target64->bit_width,
+						   fadt_info_table[i].
+						   default_length));
 
 				/* Incorrect size, set width to the default */
 
@@ -670,7 +689,8 @@ static void acpi_tb_setup_fadt_registers(void)
 						     source64->address +
 						     (fadt_pm_info_table[i].
 						      register_num *
-						      pm1_register_byte_width));
+						      pm1_register_byte_width),
+						     "PmRegisters");
 		}
 	}
 }
diff --git a/drivers/acpi/acpica/tbfind.c b/drivers/acpi/acpica/tbfind.c
index 4903e36ea75..57deae16657 100644
--- a/drivers/acpi/acpica/tbfind.c
+++ b/drivers/acpi/acpica/tbfind.c
@@ -52,7 +52,7 @@ ACPI_MODULE_NAME("tbfind")
  *
  * FUNCTION:    acpi_tb_find_table
  *
- * PARAMETERS:  Signature           - String with ACPI table signature
+ * PARAMETERS:  signature           - String with ACPI table signature
  *              oem_id              - String with the table OEM ID
  *              oem_table_id        - String with the OEM Table ID
  *              table_index         - Where the table index is returned
diff --git a/drivers/acpi/acpica/tbinstal.c b/drivers/acpi/acpica/tbinstal.c
index c03500b4cc7..74f97d74db1 100644
--- a/drivers/acpi/acpica/tbinstal.c
+++ b/drivers/acpi/acpica/tbinstal.c
@@ -138,13 +138,14 @@ acpi_tb_add_table(struct acpi_table_desc *table_desc, u32 *table_index)
 	if ((table_desc->pointer->signature[0] != 0x00) &&
 	    (!ACPI_COMPARE_NAME(table_desc->pointer->signature, ACPI_SIG_SSDT))
 	    && (ACPI_STRNCMP(table_desc->pointer->signature, "OEM", 3))) {
-		ACPI_ERROR((AE_INFO,
-			    "Table has invalid signature [%4.4s] (0x%8.8X), must be SSDT or OEMx",
-			    acpi_ut_valid_acpi_name(*(u32 *)table_desc->
-						    pointer->
-						    signature) ? table_desc->
-			    pointer->signature : "????",
-			    *(u32 *)table_desc->pointer->signature));
+		ACPI_BIOS_ERROR((AE_INFO,
+				 "Table has invalid signature [%4.4s] (0x%8.8X), "
+				 "must be SSDT or OEMx",
+				 acpi_ut_valid_acpi_name(*(u32 *)table_desc->
+							 pointer->
+							 signature) ?
+				 table_desc->pointer->signature : "????",
+				 *(u32 *)table_desc->pointer->signature));
 
 		return_ACPI_STATUS(AE_BAD_SIGNATURE);
 	}
@@ -396,10 +397,10 @@ acpi_status acpi_tb_resize_root_table_list(void)
  *
  * FUNCTION:    acpi_tb_store_table
  *
- * PARAMETERS:  Address             - Table address
- *              Table               - Table header
- *              Length              - Table length
- *              Flags               - flags
+ * PARAMETERS:  address             - Table address
+ *              table               - Table header
+ *              length              - Table length
+ *              flags               - flags
  *
  * RETURN:      Status and table index.
  *
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index 0a706cac37d..b6cea30da63 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -178,8 +178,8 @@ u8 acpi_tb_tables_loaded(void)
  *
  * FUNCTION:    acpi_tb_fix_string
  *
- * PARAMETERS:  String              - String to be repaired
- *              Length              - Maximum length
+ * PARAMETERS:  string              - String to be repaired
+ *              length              - Maximum length
  *
  * RETURN:      None
  *
@@ -205,7 +205,7 @@ static void acpi_tb_fix_string(char *string, acpi_size length)
  * FUNCTION:    acpi_tb_cleanup_table_header
  *
  * PARAMETERS:  out_header          - Where the cleaned header is returned
- *              Header              - Input ACPI table header
+ *              header              - Input ACPI table header
  *
  * RETURN:      Returns the cleaned header in out_header
  *
@@ -231,8 +231,8 @@ acpi_tb_cleanup_table_header(struct acpi_table_header *out_header,
  *
  * FUNCTION:    acpi_tb_print_table_header
  *
- * PARAMETERS:  Address             - Table physical address
- *              Header              - Table header
+ * PARAMETERS:  address             - Table physical address
+ *              header              - Table header
  *
  * RETURN:      None
  *
@@ -296,8 +296,8 @@ acpi_tb_print_table_header(acpi_physical_address address,
  *
  * FUNCTION:    acpi_tb_validate_checksum
  *
- * PARAMETERS:  Table               - ACPI table to verify
- *              Length              - Length of entire table
+ * PARAMETERS:  table               - ACPI table to verify
+ *              length              - Length of entire table
  *
  * RETURN:      Status
  *
@@ -317,10 +317,11 @@ acpi_status acpi_tb_verify_checksum(struct acpi_table_header *table, u32 length)
 	/* Checksum ok? (should be zero) */
 
 	if (checksum) {
-		ACPI_WARNING((AE_INFO,
-			      "Incorrect checksum in table [%4.4s] - 0x%2.2X, should be 0x%2.2X",
-			      table->signature, table->checksum,
-			      (u8) (table->checksum - checksum)));
+		ACPI_BIOS_WARNING((AE_INFO,
+				   "Incorrect checksum in table [%4.4s] - 0x%2.2X, "
+				   "should be 0x%2.2X",
+				   table->signature, table->checksum,
+				   (u8)(table->checksum - checksum)));
 
 #if (ACPI_CHECKSUM_ABORT)
 
@@ -335,8 +336,8 @@ acpi_status acpi_tb_verify_checksum(struct acpi_table_header *table, u32 length)
  *
  * FUNCTION:    acpi_tb_checksum
  *
- * PARAMETERS:  Buffer          - Pointer to memory region to be checked
- *              Length          - Length of this memory region
+ * PARAMETERS:  buffer          - Pointer to memory region to be checked
+ *              length          - Length of this memory region
  *
  * RETURN:      Checksum (u8)
  *
@@ -377,8 +378,9 @@ void acpi_tb_check_dsdt_header(void)
 
 	if (acpi_gbl_original_dsdt_header.length != acpi_gbl_DSDT->length ||
 	    acpi_gbl_original_dsdt_header.checksum != acpi_gbl_DSDT->checksum) {
-		ACPI_ERROR((AE_INFO,
-			    "The DSDT has been corrupted or replaced - old, new headers below"));
+		ACPI_BIOS_ERROR((AE_INFO,
+				 "The DSDT has been corrupted or replaced - "
+				 "old, new headers below"));
 		acpi_tb_print_table_header(0, &acpi_gbl_original_dsdt_header);
 		acpi_tb_print_table_header(0, acpi_gbl_DSDT);
 
@@ -438,8 +440,8 @@ struct acpi_table_header *acpi_tb_copy_dsdt(u32 table_index)
  *
  * FUNCTION:    acpi_tb_install_table
  *
- * PARAMETERS:  Address                 - Physical address of DSDT or FACS
- *              Signature               - Table signature, NULL if no need to
+ * PARAMETERS:  address                 - Physical address of DSDT or FACS
+ *              signature               - Table signature, NULL if no need to
  *                                        match
  *              table_index             - Index into root table array
  *
@@ -480,9 +482,10 @@ acpi_tb_install_table(acpi_physical_address address,
 	/* If a particular signature is expected (DSDT/FACS), it must match */
 
 	if (signature && !ACPI_COMPARE_NAME(table->signature, signature)) {
-		ACPI_ERROR((AE_INFO,
-			    "Invalid signature 0x%X for ACPI table, expected [%s]",
-			    *ACPI_CAST_PTR(u32, table->signature), signature));
+		ACPI_BIOS_ERROR((AE_INFO,
+				 "Invalid signature 0x%X for ACPI table, expected [%s]",
+				 *ACPI_CAST_PTR(u32, table->signature),
+				 signature));
 		goto unmap_and_exit;
 	}
 
@@ -589,10 +592,10 @@ acpi_tb_get_root_table_entry(u8 *table_entry, u32 table_entry_size)
 
 			/* Will truncate 64-bit address to 32 bits, issue warning */
 
-			ACPI_WARNING((AE_INFO,
-				      "64-bit Physical Address in XSDT is too large (0x%8.8X%8.8X),"
-				      " truncating",
-				      ACPI_FORMAT_UINT64(address64)));
+			ACPI_BIOS_WARNING((AE_INFO,
+					   "64-bit Physical Address in XSDT is too large (0x%8.8X%8.8X),"
+					   " truncating",
+					   ACPI_FORMAT_UINT64(address64)));
 		}
 #endif
 		return ((acpi_physical_address) (address64));
@@ -603,7 +606,7 @@ acpi_tb_get_root_table_entry(u8 *table_entry, u32 table_entry_size)
  *
  * FUNCTION:    acpi_tb_parse_root_table
  *
- * PARAMETERS:  Rsdp                    - Pointer to the RSDP
+ * PARAMETERS:  rsdp                    - Pointer to the RSDP
  *
  * RETURN:      Status
  *
@@ -694,8 +697,9 @@ acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 	acpi_os_unmap_memory(table, sizeof(struct acpi_table_header));
 
 	if (length < sizeof(struct acpi_table_header)) {
-		ACPI_ERROR((AE_INFO, "Invalid length 0x%X in RSDT/XSDT",
-			    length));
+		ACPI_BIOS_ERROR((AE_INFO,
+				 "Invalid table length 0x%X in RSDT/XSDT",
+				 length));
 		return_ACPI_STATUS(AE_INVALID_TABLE_LENGTH);
 	}
 
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index abcc6412c24..ea4c6d52605 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -1,7 +1,6 @@
 /******************************************************************************
  *
- * Module Name: tbxface - Public interfaces to the ACPI subsystem
- *                         ACPI table oriented interfaces
+ * Module Name: tbxface - ACPI table oriented external interfaces
  *
  *****************************************************************************/
 
@@ -51,11 +50,6 @@
 #define _COMPONENT          ACPI_TABLES
 ACPI_MODULE_NAME("tbxface")
 
-/* Local prototypes */
-static acpi_status acpi_tb_load_namespace(void);
-
-static int no_auto_ssdt;
-
 /*******************************************************************************
  *
  * FUNCTION:    acpi_allocate_root_table
@@ -65,11 +59,10 @@ static int no_auto_ssdt;
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Allocate a root table array. Used by i_aSL compiler and
+ * DESCRIPTION: Allocate a root table array. Used by iASL compiler and
  *              acpi_initialize_tables.
  *
  ******************************************************************************/
-
 acpi_status acpi_allocate_root_table(u32 initial_table_count)
 {
 
@@ -222,52 +215,10 @@ acpi_status acpi_reallocate_root_table(void)
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_load_table
- *
- * PARAMETERS:  table_ptr       - pointer to a buffer containing the entire
- *                                table to be loaded
- *
- * RETURN:      Status
- *
- * DESCRIPTION: This function is called to load a table from the caller's
- *              buffer. The buffer must contain an entire ACPI Table including
- *              a valid header. The header fields will be verified, and if it
- *              is determined that the table is invalid, the call will fail.
- *
- ******************************************************************************/
-acpi_status acpi_load_table(struct acpi_table_header *table_ptr)
-{
-	acpi_status status;
-	u32 table_index;
-	struct acpi_table_desc table_desc;
-
-	if (!table_ptr)
-		return AE_BAD_PARAMETER;
-
-	ACPI_MEMSET(&table_desc, 0, sizeof(struct acpi_table_desc));
-	table_desc.pointer = table_ptr;
-	table_desc.length = table_ptr->length;
-	table_desc.flags = ACPI_TABLE_ORIGIN_UNKNOWN;
-
-	/*
-	 * Install the new table into the local data structures
-	 */
-	status = acpi_tb_add_table(&table_desc, &table_index);
-	if (ACPI_FAILURE(status)) {
-		return status;
-	}
-	status = acpi_ns_load_table(table_index, acpi_gbl_root_node);
-	return status;
-}
-
-ACPI_EXPORT_SYMBOL(acpi_load_table)
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_get_table_header
  *
- * PARAMETERS:  Signature           - ACPI signature of needed table
- *              Instance            - Which instance (for SSDTs)
+ * PARAMETERS:  signature           - ACPI signature of needed table
+ *              instance            - Which instance (for SSDTs)
  *              out_table_header    - The pointer to the table header to fill
  *
  * RETURN:      Status and pointer to mapped table header
@@ -382,8 +333,8 @@ ACPI_EXPORT_SYMBOL(acpi_unload_table_id)
  *
  * FUNCTION:    acpi_get_table_with_size
  *
- * PARAMETERS:  Signature           - ACPI signature of needed table
- *              Instance            - Which instance (for SSDTs)
+ * PARAMETERS:  signature           - ACPI signature of needed table
+ *              instance            - Which instance (for SSDTs)
  *              out_table           - Where the pointer to the table is returned
  *
  * RETURN:      Status and pointer to table
@@ -453,7 +404,7 @@ ACPI_EXPORT_SYMBOL(acpi_get_table)
  * FUNCTION:    acpi_get_table_by_index
  *
  * PARAMETERS:  table_index         - Table index
- *              Table               - Where the pointer to the table is returned
+ *              table               - Where the pointer to the table is returned
  *
  * RETURN:      Status and pointer to the table
  *
@@ -502,157 +453,13 @@ acpi_get_table_by_index(u32 table_index, struct acpi_table_header **table)
 
 ACPI_EXPORT_SYMBOL(acpi_get_table_by_index)
 
-/*******************************************************************************
- *
- * FUNCTION:    acpi_tb_load_namespace
- *
- * PARAMETERS:  None
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Load the namespace from the DSDT and all SSDTs/PSDTs found in
- *              the RSDT/XSDT.
- *
- ******************************************************************************/
-static acpi_status acpi_tb_load_namespace(void)
-{
-	acpi_status status;
-	u32 i;
-	struct acpi_table_header *new_dsdt;
-
-	ACPI_FUNCTION_TRACE(tb_load_namespace);
-
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-
-	/*
-	 * Load the namespace. The DSDT is required, but any SSDT and
-	 * PSDT tables are optional. Verify the DSDT.
-	 */
-	if (!acpi_gbl_root_table_list.current_table_count ||
-	    !ACPI_COMPARE_NAME(&
-			       (acpi_gbl_root_table_list.
-				tables[ACPI_TABLE_INDEX_DSDT].signature),
-			       ACPI_SIG_DSDT)
-	    ||
-	    ACPI_FAILURE(acpi_tb_verify_table
-			 (&acpi_gbl_root_table_list.
-			  tables[ACPI_TABLE_INDEX_DSDT]))) {
-		status = AE_NO_ACPI_TABLES;
-		goto unlock_and_exit;
-	}
-
-	/*
-	 * Save the DSDT pointer for simple access. This is the mapped memory
-	 * address. We must take care here because the address of the .Tables
-	 * array can change dynamically as tables are loaded at run-time. Note:
-	 * .Pointer field is not validated until after call to acpi_tb_verify_table.
-	 */
-	acpi_gbl_DSDT =
-	    acpi_gbl_root_table_list.tables[ACPI_TABLE_INDEX_DSDT].pointer;
-
-	/*
-	 * Optionally copy the entire DSDT to local memory (instead of simply
-	 * mapping it.) There are some BIOSs that corrupt or replace the original
-	 * DSDT, creating the need for this option. Default is FALSE, do not copy
-	 * the DSDT.
-	 */
-	if (acpi_gbl_copy_dsdt_locally) {
-		new_dsdt = acpi_tb_copy_dsdt(ACPI_TABLE_INDEX_DSDT);
-		if (new_dsdt) {
-			acpi_gbl_DSDT = new_dsdt;
-		}
-	}
-
-	/*
-	 * Save the original DSDT header for detection of table corruption
-	 * and/or replacement of the DSDT from outside the OS.
-	 */
-	ACPI_MEMCPY(&acpi_gbl_original_dsdt_header, acpi_gbl_DSDT,
-		    sizeof(struct acpi_table_header));
-
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-
-	/* Load and parse tables */
-
-	status = acpi_ns_load_table(ACPI_TABLE_INDEX_DSDT, acpi_gbl_root_node);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
-	/* Load any SSDT or PSDT tables. Note: Loop leaves tables locked */
-
-	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
-		if ((!ACPI_COMPARE_NAME
-		     (&(acpi_gbl_root_table_list.tables[i].signature),
-		      ACPI_SIG_SSDT)
-		     &&
-		     !ACPI_COMPARE_NAME(&
-					(acpi_gbl_root_table_list.tables[i].
-					 signature), ACPI_SIG_PSDT))
-		    ||
-		    ACPI_FAILURE(acpi_tb_verify_table
-				 (&acpi_gbl_root_table_list.tables[i]))) {
-			continue;
-		}
-
-		if (no_auto_ssdt) {
-			printk(KERN_WARNING "ACPI: SSDT ignored due to \"acpi_no_auto_ssdt\"\n");
-			continue;
-		}
-
-		/* Ignore errors while loading tables, get as many as possible */
-
-		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-		(void)acpi_ns_load_table(i, acpi_gbl_root_node);
-		(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
-	}
-
-	ACPI_DEBUG_PRINT((ACPI_DB_INIT, "ACPI Tables successfully acquired\n"));
-
-      unlock_and_exit:
-	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
-	return_ACPI_STATUS(status);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_load_tables
- *
- * PARAMETERS:  None
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Load the ACPI tables from the RSDT/XSDT
- *
- ******************************************************************************/
-
-acpi_status acpi_load_tables(void)
-{
-	acpi_status status;
-
-	ACPI_FUNCTION_TRACE(acpi_load_tables);
-
-	/* Load the namespace from the tables */
-
-	status = acpi_tb_load_namespace();
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status,
-				"While loading namespace from ACPI tables"));
-	}
-
-	return_ACPI_STATUS(status);
-}
-
-ACPI_EXPORT_SYMBOL(acpi_load_tables)
-
 
 /*******************************************************************************
  *
  * FUNCTION:    acpi_install_table_handler
  *
- * PARAMETERS:  Handler         - Table event handler
- *              Context         - Value passed to the handler on each event
+ * PARAMETERS:  handler         - Table event handler
+ *              context         - Value passed to the handler on each event
  *
  * RETURN:      Status
  *
@@ -698,7 +505,7 @@ ACPI_EXPORT_SYMBOL(acpi_install_table_handler)
  *
  * FUNCTION:    acpi_remove_table_handler
  *
- * PARAMETERS:  Handler         - Table event handler that was installed
+ * PARAMETERS:  handler         - Table event handler that was installed
  *                                previously.
  *
  * RETURN:      Status
@@ -734,15 +541,3 @@ acpi_status acpi_remove_table_handler(acpi_tbl_handler handler)
 }
 
 ACPI_EXPORT_SYMBOL(acpi_remove_table_handler)
-
-
-static int __init acpi_no_auto_ssdt_setup(char *s) {
-
-        printk(KERN_NOTICE "ACPI: SSDT auto-load disabled\n");
-
-        no_auto_ssdt = 1;
-
-        return 1;
-}
-
-__setup("acpi_no_auto_ssdt", acpi_no_auto_ssdt_setup);
diff --git a/drivers/acpi/acpica/tbxfload.c b/drivers/acpi/acpica/tbxfload.c
new file mode 100644
index 00000000000..f87cc63e69a
--- /dev/null
+++ b/drivers/acpi/acpica/tbxfload.c
@@ -0,0 +1,389 @@
+/******************************************************************************
+ *
+ * Module Name: tbxfload - Table load/unload external interfaces
+ *
+ *****************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2012, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#include <linux/export.h>
+#include <acpi/acpi.h>
+#include "accommon.h"
+#include "acnamesp.h"
+#include "actables.h"
+
+#define _COMPONENT          ACPI_TABLES
+ACPI_MODULE_NAME("tbxfload")
+
+/* Local prototypes */
+static acpi_status acpi_tb_load_namespace(void);
+
+static int no_auto_ssdt;
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_load_tables
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Load the ACPI tables from the RSDT/XSDT
+ *
+ ******************************************************************************/
+
+acpi_status acpi_load_tables(void)
+{
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE(acpi_load_tables);
+
+	/* Load the namespace from the tables */
+
+	status = acpi_tb_load_namespace();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"While loading namespace from ACPI tables"));
+	}
+
+	return_ACPI_STATUS(status);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_load_tables)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_tb_load_namespace
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Load the namespace from the DSDT and all SSDTs/PSDTs found in
+ *              the RSDT/XSDT.
+ *
+ ******************************************************************************/
+static acpi_status acpi_tb_load_namespace(void)
+{
+	acpi_status status;
+	u32 i;
+	struct acpi_table_header *new_dsdt;
+
+	ACPI_FUNCTION_TRACE(tb_load_namespace);
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+
+	/*
+	 * Load the namespace. The DSDT is required, but any SSDT and
+	 * PSDT tables are optional. Verify the DSDT.
+	 */
+	if (!acpi_gbl_root_table_list.current_table_count ||
+	    !ACPI_COMPARE_NAME(&
+			       (acpi_gbl_root_table_list.
+				tables[ACPI_TABLE_INDEX_DSDT].signature),
+			       ACPI_SIG_DSDT)
+	    ||
+	    ACPI_FAILURE(acpi_tb_verify_table
+			 (&acpi_gbl_root_table_list.
+			  tables[ACPI_TABLE_INDEX_DSDT]))) {
+		status = AE_NO_ACPI_TABLES;
+		goto unlock_and_exit;
+	}
+
+	/*
+	 * Save the DSDT pointer for simple access. This is the mapped memory
+	 * address. We must take care here because the address of the .Tables
+	 * array can change dynamically as tables are loaded at run-time. Note:
+	 * .Pointer field is not validated until after call to acpi_tb_verify_table.
+	 */
+	acpi_gbl_DSDT =
+	    acpi_gbl_root_table_list.tables[ACPI_TABLE_INDEX_DSDT].pointer;
+
+	/*
+	 * Optionally copy the entire DSDT to local memory (instead of simply
+	 * mapping it.) There are some BIOSs that corrupt or replace the original
+	 * DSDT, creating the need for this option. Default is FALSE, do not copy
+	 * the DSDT.
+	 */
+	if (acpi_gbl_copy_dsdt_locally) {
+		new_dsdt = acpi_tb_copy_dsdt(ACPI_TABLE_INDEX_DSDT);
+		if (new_dsdt) {
+			acpi_gbl_DSDT = new_dsdt;
+		}
+	}
+
+	/*
+	 * Save the original DSDT header for detection of table corruption
+	 * and/or replacement of the DSDT from outside the OS.
+	 */
+	ACPI_MEMCPY(&acpi_gbl_original_dsdt_header, acpi_gbl_DSDT,
+		    sizeof(struct acpi_table_header));
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+
+	/* Load and parse tables */
+
+	status = acpi_ns_load_table(ACPI_TABLE_INDEX_DSDT, acpi_gbl_root_node);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Load any SSDT or PSDT tables. Note: Loop leaves tables locked */
+
+	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
+		if ((!ACPI_COMPARE_NAME
+		     (&(acpi_gbl_root_table_list.tables[i].signature),
+		      ACPI_SIG_SSDT)
+		     &&
+		     !ACPI_COMPARE_NAME(&
+					(acpi_gbl_root_table_list.tables[i].
+					 signature), ACPI_SIG_PSDT))
+		    ||
+		    ACPI_FAILURE(acpi_tb_verify_table
+				 (&acpi_gbl_root_table_list.tables[i]))) {
+			continue;
+		}
+
+		if (no_auto_ssdt) {
+			printk(KERN_WARNING "ACPI: SSDT ignored due to \"acpi_no_auto_ssdt\"\n");
+			continue;
+		}
+
+		/* Ignore errors while loading tables, get as many as possible */
+
+		(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+		(void)acpi_ns_load_table(i, acpi_gbl_root_node);
+		(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
+	}
+
+	ACPI_DEBUG_PRINT((ACPI_DB_INIT, "ACPI Tables successfully acquired\n"));
+
+      unlock_and_exit:
+	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
+	return_ACPI_STATUS(status);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_load_table
+ *
+ * PARAMETERS:  table               - Pointer to a buffer containing the ACPI
+ *                                    table to be loaded.
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Dynamically load an ACPI table from the caller's buffer. Must
+ *              be a valid ACPI table with a valid ACPI table header.
+ *              Note1: Mainly intended to support hotplug addition of SSDTs.
+ *              Note2: Does not copy the incoming table. User is reponsible
+ *              to ensure that the table is not deleted or unmapped.
+ *
+ ******************************************************************************/
+
+acpi_status acpi_load_table(struct acpi_table_header *table)
+{
+	acpi_status status;
+	struct acpi_table_desc table_desc;
+	u32 table_index;
+
+	ACPI_FUNCTION_TRACE(acpi_load_table);
+
+	/* Parameter validation */
+
+	if (!table) {
+		return_ACPI_STATUS(AE_BAD_PARAMETER);
+	}
+
+	/* Init local table descriptor */
+
+	ACPI_MEMSET(&table_desc, 0, sizeof(struct acpi_table_desc));
+	table_desc.address = ACPI_PTR_TO_PHYSADDR(table);
+	table_desc.pointer = table;
+	table_desc.length = table->length;
+	table_desc.flags = ACPI_TABLE_ORIGIN_UNKNOWN;
+
+	/* Must acquire the interpreter lock during this operation */
+
+	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Install the table and load it into the namespace */
+
+	ACPI_INFO((AE_INFO, "Host-directed Dynamic ACPI Table Load:"));
+	status = acpi_tb_add_table(&table_desc, &table_index);
+	if (ACPI_FAILURE(status)) {
+		goto unlock_and_exit;
+	}
+
+	status = acpi_ns_load_table(table_index, acpi_gbl_root_node);
+
+	/* Invoke table handler if present */
+
+	if (acpi_gbl_table_handler) {
+		(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_LOAD, table,
+					     acpi_gbl_table_handler_context);
+	}
+
+      unlock_and_exit:
+	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
+	return_ACPI_STATUS(status);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_load_table)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_unload_parent_table
+ *
+ * PARAMETERS:  object              - Handle to any namespace object owned by
+ *                                    the table to be unloaded
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Via any namespace object within an SSDT or OEMx table, unloads
+ *              the table and deletes all namespace objects associated with
+ *              that table. Unloading of the DSDT is not allowed.
+ *              Note: Mainly intended to support hotplug removal of SSDTs.
+ *
+ ******************************************************************************/
+acpi_status acpi_unload_parent_table(acpi_handle object)
+{
+	struct acpi_namespace_node *node =
+	    ACPI_CAST_PTR(struct acpi_namespace_node, object);
+	acpi_status status = AE_NOT_EXIST;
+	acpi_owner_id owner_id;
+	u32 i;
+
+	ACPI_FUNCTION_TRACE(acpi_unload_parent_table);
+
+	/* Parameter validation */
+
+	if (!object) {
+		return_ACPI_STATUS(AE_BAD_PARAMETER);
+	}
+
+	/*
+	 * The node owner_id is currently the same as the parent table ID.
+	 * However, this could change in the future.
+	 */
+	owner_id = node->owner_id;
+	if (!owner_id) {
+
+		/* owner_id==0 means DSDT is the owner. DSDT cannot be unloaded */
+
+		return_ACPI_STATUS(AE_TYPE);
+	}
+
+	/* Must acquire the interpreter lock during this operation */
+
+	status = acpi_ut_acquire_mutex(ACPI_MTX_INTERPRETER);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Find the table in the global table list */
+
+	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; i++) {
+		if (owner_id != acpi_gbl_root_table_list.tables[i].owner_id) {
+			continue;
+		}
+
+		/*
+		 * Allow unload of SSDT and OEMx tables only. Do not allow unload
+		 * of the DSDT. No other types of tables should get here, since
+		 * only these types can contain AML and thus are the only types
+		 * that can create namespace objects.
+		 */
+		if (ACPI_COMPARE_NAME
+		    (acpi_gbl_root_table_list.tables[i].signature.ascii,
+		     ACPI_SIG_DSDT)) {
+			status = AE_TYPE;
+			break;
+		}
+
+		/* Ensure the table is actually loaded */
+
+		if (!acpi_tb_is_table_loaded(i)) {
+			status = AE_NOT_EXIST;
+			break;
+		}
+
+		/* Invoke table handler if present */
+
+		if (acpi_gbl_table_handler) {
+			(void)acpi_gbl_table_handler(ACPI_TABLE_EVENT_UNLOAD,
+						     acpi_gbl_root_table_list.
+						     tables[i].pointer,
+						     acpi_gbl_table_handler_context);
+		}
+
+		/*
+		 * Delete all namespace objects owned by this table. Note that
+		 * these objects can appear anywhere in the namespace by virtue
+		 * of the AML "Scope" operator. Thus, we need to track ownership
+		 * by an ID, not simply a position within the hierarchy.
+		 */
+		status = acpi_tb_delete_namespace_by_owner(i);
+		if (ACPI_FAILURE(status)) {
+			break;
+		}
+
+		status = acpi_tb_release_owner_id(i);
+		acpi_tb_set_table_loaded_flag(i, FALSE);
+		break;
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_INTERPRETER);
+	return_ACPI_STATUS(status);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_unload_parent_table)
+
+static int __init acpi_no_auto_ssdt_setup(char *s) {
+
+        printk(KERN_NOTICE "ACPI: SSDT auto-load disabled\n");
+
+        no_auto_ssdt = 1;
+
+        return 1;
+}
+
+__setup("acpi_no_auto_ssdt", acpi_no_auto_ssdt_setup);
diff --git a/drivers/acpi/acpica/tbxfroot.c b/drivers/acpi/acpica/tbxfroot.c
index 4258f647ca3..74e72080003 100644
--- a/drivers/acpi/acpica/tbxfroot.c
+++ b/drivers/acpi/acpica/tbxfroot.c
@@ -57,7 +57,7 @@ static acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp *rsdp);
  *
  * FUNCTION:    acpi_tb_validate_rsdp
  *
- * PARAMETERS:  Rsdp                - Pointer to unvalidated RSDP
+ * PARAMETERS:  rsdp                - Pointer to unvalidated RSDP
  *
  * RETURN:      Status
  *
@@ -107,10 +107,10 @@ static acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp *rsdp)
  *
  * RETURN:      Status, RSDP physical address
  *
- * DESCRIPTION: Search lower 1_mbyte of memory for the root system descriptor
+ * DESCRIPTION: Search lower 1Mbyte of memory for the root system descriptor
  *              pointer structure.  If it is found, set *RSDP to point to it.
  *
- * NOTE1:       The RSDP must be either in the first 1_k of the Extended
+ * NOTE1:       The RSDP must be either in the first 1K of the Extended
  *              BIOS Data Area or between E0000 and FFFFF (From ACPI Spec.)
  *              Only a 32-bit physical address is necessary.
  *
@@ -152,7 +152,7 @@ acpi_status acpi_find_root_pointer(acpi_size *table_address)
 	if (physical_address > 0x400) {
 		/*
 		 * 1b) Search EBDA paragraphs (EBDA is required to be a
-		 *     minimum of 1_k length)
+		 *     minimum of 1K length)
 		 */
 		table_ptr = acpi_os_map_memory((acpi_physical_address)
 					       physical_address,
@@ -216,7 +216,7 @@ acpi_status acpi_find_root_pointer(acpi_size *table_address)
 
 	/* A valid RSDP was not found */
 
-	ACPI_ERROR((AE_INFO, "A valid RSDP was not found"));
+	ACPI_BIOS_ERROR((AE_INFO, "A valid RSDP was not found"));
 	return_ACPI_STATUS(AE_NOT_FOUND);
 }
 
@@ -225,7 +225,7 @@ acpi_status acpi_find_root_pointer(acpi_size *table_address)
  * FUNCTION:    acpi_tb_scan_memory_for_rsdp
  *
  * PARAMETERS:  start_address       - Starting pointer for search
- *              Length              - Maximum length to search
+ *              length              - Maximum length to search
  *
  * RETURN:      Pointer to the RSDP if found, otherwise NULL.
  *
diff --git a/drivers/acpi/acpica/utaddress.c b/drivers/acpi/acpica/utaddress.c
index 67932aebe6d..64880306133 100644
--- a/drivers/acpi/acpica/utaddress.c
+++ b/drivers/acpi/acpica/utaddress.c
@@ -53,8 +53,8 @@ ACPI_MODULE_NAME("utaddress")
  * FUNCTION:    acpi_ut_add_address_range
  *
  * PARAMETERS:  space_id            - Address space ID
- *              Address             - op_region start address
- *              Length              - op_region length
+ *              address             - op_region start address
+ *              length              - op_region length
  *              region_node         - op_region namespace node
  *
  * RETURN:      Status
@@ -186,9 +186,9 @@ acpi_ut_remove_address_range(acpi_adr_space_type space_id,
  * FUNCTION:    acpi_ut_check_address_range
  *
  * PARAMETERS:  space_id            - Address space ID
- *              Address             - Start address
- *              Length              - Length of address range
- *              Warn                - TRUE if warning on overlap desired
+ *              address             - Start address
+ *              length              - Length of address range
+ *              warn                - TRUE if warning on overlap desired
  *
  * RETURN:      Count of the number of conflicts detected. Zero is always
  *              returned for Space IDs other than Memory or I/O.
diff --git a/drivers/acpi/acpica/utalloc.c b/drivers/acpi/acpica/utalloc.c
index 9982d2ea66f..ed29d474095 100644
--- a/drivers/acpi/acpica/utalloc.c
+++ b/drivers/acpi/acpica/utalloc.c
@@ -189,7 +189,7 @@ acpi_status acpi_ut_delete_caches(void)
  *
  * FUNCTION:    acpi_ut_validate_buffer
  *
- * PARAMETERS:  Buffer              - Buffer descriptor to be validated
+ * PARAMETERS:  buffer              - Buffer descriptor to be validated
  *
  * RETURN:      Status
  *
@@ -227,7 +227,7 @@ acpi_status acpi_ut_validate_buffer(struct acpi_buffer * buffer)
  *
  * FUNCTION:    acpi_ut_initialize_buffer
  *
- * PARAMETERS:  Buffer              - Buffer to be validated
+ * PARAMETERS:  buffer              - Buffer to be validated
  *              required_length     - Length needed
  *
  * RETURN:      Status
@@ -308,10 +308,10 @@ acpi_ut_initialize_buffer(struct acpi_buffer * buffer,
  *
  * FUNCTION:    acpi_ut_allocate
  *
- * PARAMETERS:  Size                - Size of the allocation
- *              Component           - Component type of caller
- *              Module              - Source file name of caller
- *              Line                - Line number of caller
+ * PARAMETERS:  size                - Size of the allocation
+ *              component           - Component type of caller
+ *              module              - Source file name of caller
+ *              line                - Line number of caller
  *
  * RETURN:      Address of the allocated memory on success, NULL on failure.
  *
@@ -352,10 +352,10 @@ void *acpi_ut_allocate(acpi_size size,
  *
  * FUNCTION:    acpi_ut_allocate_zeroed
  *
- * PARAMETERS:  Size                - Size of the allocation
- *              Component           - Component type of caller
- *              Module              - Source file name of caller
- *              Line                - Line number of caller
+ * PARAMETERS:  size                - Size of the allocation
+ *              component           - Component type of caller
+ *              module              - Source file name of caller
+ *              line                - Line number of caller
  *
  * RETURN:      Address of the allocated memory on success, NULL on failure.
  *
diff --git a/drivers/acpi/acpica/utcopy.c b/drivers/acpi/acpica/utcopy.c
index 3317c0a406e..294692ae76e 100644
--- a/drivers/acpi/acpica/utcopy.c
+++ b/drivers/acpi/acpica/utcopy.c
@@ -317,7 +317,7 @@ acpi_ut_copy_ielement_to_eelement(u8 object_type,
  * FUNCTION:    acpi_ut_copy_ipackage_to_epackage
  *
  * PARAMETERS:  internal_object     - Pointer to the object we are returning
- *              Buffer              - Where the object is returned
+ *              buffer              - Where the object is returned
  *              space_used          - Where the object length is returned
  *
  * RETURN:      Status
diff --git a/drivers/acpi/acpica/utdebug.c b/drivers/acpi/acpica/utdebug.c
index a0998a88631..e810894149a 100644
--- a/drivers/acpi/acpica/utdebug.c
+++ b/drivers/acpi/acpica/utdebug.c
@@ -145,7 +145,7 @@ static const char *acpi_ut_trim_function_name(const char *function_name)
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Format              - Printf format field
+ *              format              - Printf format field
  *              ...                 - Optional printf arguments
  *
  * RETURN:      None
@@ -217,7 +217,7 @@ ACPI_EXPORT_SYMBOL(acpi_debug_print)
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Format              - Printf format field
+ *              format              - Printf format field
  *              ...                 - Optional printf arguments
  *
  * RETURN:      None
@@ -286,7 +286,7 @@ ACPI_EXPORT_SYMBOL(acpi_ut_trace)
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Pointer             - Pointer to display
+ *              pointer             - Pointer to display
  *
  * RETURN:      None
  *
@@ -315,7 +315,7 @@ acpi_ut_trace_ptr(u32 line_number,
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              String              - Additional string to display
+ *              string              - Additional string to display
  *
  * RETURN:      None
  *
@@ -346,7 +346,7 @@ acpi_ut_trace_str(u32 line_number,
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Integer             - Integer to display
+ *              integer             - Integer to display
  *
  * RETURN:      None
  *
@@ -408,7 +408,7 @@ ACPI_EXPORT_SYMBOL(acpi_ut_exit)
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Status              - Exit status code
+ *              status              - Exit status code
  *
  * RETURN:      None
  *
@@ -449,7 +449,7 @@ ACPI_EXPORT_SYMBOL(acpi_ut_status_exit)
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Value               - Value to be printed with exit msg
+ *              value               - Value to be printed with exit msg
  *
  * RETURN:      None
  *
@@ -481,7 +481,7 @@ ACPI_EXPORT_SYMBOL(acpi_ut_value_exit)
  *              function_name       - Caller's procedure name
  *              module_name         - Caller's module name
  *              component_id        - Caller's component ID
- *              Ptr                 - Pointer to display
+ *              ptr                 - Pointer to display
  *
  * RETURN:      None
  *
@@ -508,10 +508,10 @@ acpi_ut_ptr_exit(u32 line_number,
  *
  * FUNCTION:    acpi_ut_dump_buffer
  *
- * PARAMETERS:  Buffer              - Buffer to dump
- *              Count               - Amount to dump, in bytes
- *              Display             - BYTE, WORD, DWORD, or QWORD display
- *              component_iD        - Caller's component ID
+ * PARAMETERS:  buffer              - Buffer to dump
+ *              count               - Amount to dump, in bytes
+ *              display             - BYTE, WORD, DWORD, or QWORD display
+ *              component_ID        - Caller's component ID
  *
  * RETURN:      None
  *
@@ -625,10 +625,10 @@ void acpi_ut_dump_buffer2(u8 * buffer, u32 count, u32 display)
  *
  * FUNCTION:    acpi_ut_dump_buffer
  *
- * PARAMETERS:  Buffer              - Buffer to dump
- *              Count               - Amount to dump, in bytes
- *              Display             - BYTE, WORD, DWORD, or QWORD display
- *              component_iD        - Caller's component ID
+ * PARAMETERS:  buffer              - Buffer to dump
+ *              count               - Amount to dump, in bytes
+ *              display             - BYTE, WORD, DWORD, or QWORD display
+ *              component_ID        - Caller's component ID
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/utdecode.c b/drivers/acpi/acpica/utdecode.c
index 684849949bf..60a158472d8 100644
--- a/drivers/acpi/acpica/utdecode.c
+++ b/drivers/acpi/acpica/utdecode.c
@@ -49,41 +49,6 @@
 #define _COMPONENT          ACPI_UTILITIES
 ACPI_MODULE_NAME("utdecode")
 
-/*******************************************************************************
- *
- * FUNCTION:    acpi_format_exception
- *
- * PARAMETERS:  Status       - The acpi_status code to be formatted
- *
- * RETURN:      A string containing the exception text. A valid pointer is
- *              always returned.
- *
- * DESCRIPTION: This function translates an ACPI exception into an ASCII string
- *              It is here instead of utxface.c so it is always present.
- *
- ******************************************************************************/
-const char *acpi_format_exception(acpi_status status)
-{
-	const char *exception = NULL;
-
-	ACPI_FUNCTION_ENTRY();
-
-	exception = acpi_ut_validate_exception(status);
-	if (!exception) {
-
-		/* Exception code was not recognized */
-
-		ACPI_ERROR((AE_INFO,
-			    "Unknown exception code: 0x%8.8X", status));
-
-		exception = "UNKNOWN_STATUS_CODE";
-	}
-
-	return (ACPI_CAST_PTR(const char, exception));
-}
-
-ACPI_EXPORT_SYMBOL(acpi_format_exception)
-
 /*
  * Properties of the ACPI Object Types, both internal and external.
  * The table is indexed by values of acpi_object_type
@@ -126,8 +91,8 @@ const u8 acpi_gbl_ns_properties[ACPI_NUM_NS_TYPES] = {
  *
  * FUNCTION:    acpi_ut_hex_to_ascii_char
  *
- * PARAMETERS:  Integer             - Contains the hex digit
- *              Position            - bit position of the digit within the
+ * PARAMETERS:  integer             - Contains the hex digit
+ *              position            - bit position of the digit within the
  *                                    integer (multiple of 4)
  *
  * RETURN:      The converted Ascii character
@@ -164,16 +129,17 @@ char acpi_ut_hex_to_ascii_char(u64 integer, u32 position)
 /* Region type decoding */
 
 const char *acpi_gbl_region_types[ACPI_NUM_PREDEFINED_REGIONS] = {
-	"SystemMemory",
-	"SystemIO",
-	"PCI_Config",
-	"EmbeddedControl",
-	"SMBus",
-	"SystemCMOS",
-	"PCIBARTarget",
-	"IPMI",
-	"GeneralPurposeIo",
-	"GenericSerialBus"
+	"SystemMemory",		/* 0x00 */
+	"SystemIO",		/* 0x01 */
+	"PCI_Config",		/* 0x02 */
+	"EmbeddedControl",	/* 0x03 */
+	"SMBus",		/* 0x04 */
+	"SystemCMOS",		/* 0x05 */
+	"PCIBARTarget",		/* 0x06 */
+	"IPMI",			/* 0x07 */
+	"GeneralPurposeIo",	/* 0x08 */
+	"GenericSerialBus",	/* 0x09 */
+	"PCC"			/* 0x0A */
 };
 
 char *acpi_ut_get_region_name(u8 space_id)
@@ -228,7 +194,7 @@ char *acpi_ut_get_event_name(u32 event_id)
  *
  * FUNCTION:    acpi_ut_get_type_name
  *
- * PARAMETERS:  Type                - An ACPI object type
+ * PARAMETERS:  type                - An ACPI object type
  *
  * RETURN:      Decoded ACPI object type name
  *
@@ -306,7 +272,7 @@ char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc)
  *
  * FUNCTION:    acpi_ut_get_node_name
  *
- * PARAMETERS:  Object               - A namespace node
+ * PARAMETERS:  object               - A namespace node
  *
  * RETURN:      ASCII name of the node
  *
@@ -351,7 +317,7 @@ char *acpi_ut_get_node_name(void *object)
  *
  * FUNCTION:    acpi_ut_get_descriptor_name
  *
- * PARAMETERS:  Object               - An ACPI object
+ * PARAMETERS:  object               - An ACPI object
  *
  * RETURN:      Decoded name of the descriptor type
  *
@@ -401,7 +367,7 @@ char *acpi_ut_get_descriptor_name(void *object)
  *
  * FUNCTION:    acpi_ut_get_reference_name
  *
- * PARAMETERS:  Object               - An ACPI reference object
+ * PARAMETERS:  object               - An ACPI reference object
  *
  * RETURN:      Decoded name of the type of reference
  *
@@ -532,7 +498,7 @@ const char *acpi_ut_get_notify_name(u32 notify_value)
  *
  * FUNCTION:    acpi_ut_valid_object_type
  *
- * PARAMETERS:  Type            - Object type to be validated
+ * PARAMETERS:  type            - Object type to be validated
  *
  * RETURN:      TRUE if valid object type, FALSE otherwise
  *
diff --git a/drivers/acpi/acpica/utdelete.c b/drivers/acpi/acpica/utdelete.c
index 2a6c3e18369..798105443d0 100644
--- a/drivers/acpi/acpica/utdelete.c
+++ b/drivers/acpi/acpica/utdelete.c
@@ -60,7 +60,7 @@ acpi_ut_update_ref_count(union acpi_operand_object *object, u32 action);
  *
  * FUNCTION:    acpi_ut_delete_internal_obj
  *
- * PARAMETERS:  Object         - Object to be deleted
+ * PARAMETERS:  object         - Object to be deleted
  *
  * RETURN:      None
  *
@@ -152,7 +152,7 @@ static void acpi_ut_delete_internal_obj(union acpi_operand_object *object)
 	case ACPI_TYPE_PROCESSOR:
 	case ACPI_TYPE_THERMAL:
 
-		/* Walk the notify handler list for this object */
+		/* Walk the address handler list for this object */
 
 		handler_desc = object->common_notify.handler;
 		while (handler_desc) {
@@ -358,8 +358,8 @@ void acpi_ut_delete_internal_object_list(union acpi_operand_object **obj_list)
  *
  * FUNCTION:    acpi_ut_update_ref_count
  *
- * PARAMETERS:  Object          - Object whose ref count is to be updated
- *              Action          - What to do
+ * PARAMETERS:  object          - Object whose ref count is to be updated
+ *              action          - What to do
  *
  * RETURN:      New ref count
  *
@@ -456,9 +456,9 @@ acpi_ut_update_ref_count(union acpi_operand_object *object, u32 action)
  *
  * FUNCTION:    acpi_ut_update_object_reference
  *
- * PARAMETERS:  Object              - Increment ref count for this object
+ * PARAMETERS:  object              - Increment ref count for this object
  *                                    and all sub-objects
- *              Action              - Either REF_INCREMENT or REF_DECREMENT or
+ *              action              - Either REF_INCREMENT or REF_DECREMENT or
  *                                    REF_FORCE_DELETE
  *
  * RETURN:      Status
@@ -480,6 +480,7 @@ acpi_ut_update_object_reference(union acpi_operand_object *object, u16 action)
 	acpi_status status = AE_OK;
 	union acpi_generic_state *state_list = NULL;
 	union acpi_operand_object *next_object = NULL;
+	union acpi_operand_object *prev_object;
 	union acpi_generic_state *state;
 	u32 i;
 
@@ -505,12 +506,21 @@ acpi_ut_update_object_reference(union acpi_operand_object *object, u16 action)
 		case ACPI_TYPE_POWER:
 		case ACPI_TYPE_THERMAL:
 
-			/* Update the notify objects for these types (if present) */
-
-			acpi_ut_update_ref_count(object->common_notify.
-						 system_notify, action);
-			acpi_ut_update_ref_count(object->common_notify.
-						 device_notify, action);
+			/*
+			 * Update the notify objects for these types (if present)
+			 * Two lists, system and device notify handlers.
+			 */
+			for (i = 0; i < ACPI_NUM_NOTIFY_TYPES; i++) {
+				prev_object =
+				    object->common_notify.notify_list[i];
+				while (prev_object) {
+					next_object =
+					    prev_object->notify.next[i];
+					acpi_ut_update_ref_count(prev_object,
+								 action);
+					prev_object = next_object;
+				}
+			}
 			break;
 
 		case ACPI_TYPE_PACKAGE:
@@ -630,7 +640,7 @@ acpi_ut_update_object_reference(union acpi_operand_object *object, u16 action)
  *
  * FUNCTION:    acpi_ut_add_reference
  *
- * PARAMETERS:  Object          - Object whose reference count is to be
+ * PARAMETERS:  object          - Object whose reference count is to be
  *                                incremented
  *
  * RETURN:      None
@@ -664,7 +674,7 @@ void acpi_ut_add_reference(union acpi_operand_object *object)
  *
  * FUNCTION:    acpi_ut_remove_reference
  *
- * PARAMETERS:  Object         - Object whose ref count will be decremented
+ * PARAMETERS:  object         - Object whose ref count will be decremented
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/uteval.c b/drivers/acpi/acpica/uteval.c
index 479f32b3341..a9c65fbea5f 100644
--- a/drivers/acpi/acpica/uteval.c
+++ b/drivers/acpi/acpica/uteval.c
@@ -53,7 +53,7 @@ ACPI_MODULE_NAME("uteval")
  * FUNCTION:    acpi_ut_evaluate_object
  *
  * PARAMETERS:  prefix_node         - Starting node
- *              Path                - Path to object from starting node
+ *              path                - Path to object from starting node
  *              expected_return_types - Bitmap of allowed return types
  *              return_desc         - Where a return value is stored
  *
@@ -187,7 +187,7 @@ acpi_ut_evaluate_object(struct acpi_namespace_node *prefix_node,
  *
  * PARAMETERS:  object_name         - Object name to be evaluated
  *              device_node         - Node for the device
- *              Value               - Where the value is returned
+ *              value               - Where the value is returned
  *
  * RETURN:      Status
  *
@@ -229,7 +229,7 @@ acpi_ut_evaluate_numeric_object(char *object_name,
  * FUNCTION:    acpi_ut_execute_STA
  *
  * PARAMETERS:  device_node         - Node for the device
- *              Flags               - Where the status flags are returned
+ *              flags               - Where the status flags are returned
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/utexcep.c b/drivers/acpi/acpica/utexcep.c
new file mode 100644
index 00000000000..23b98945f6b
--- /dev/null
+++ b/drivers/acpi/acpica/utexcep.c
@@ -0,0 +1,153 @@
+/*******************************************************************************
+ *
+ * Module Name: utexcep - Exception code support
+ *
+ ******************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2012, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#define ACPI_DEFINE_EXCEPTION_TABLE
+#include <linux/export.h>
+#include <acpi/acpi.h>
+#include "accommon.h"
+
+#define _COMPONENT          ACPI_UTILITIES
+ACPI_MODULE_NAME("utexcep")
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_format_exception
+ *
+ * PARAMETERS:  status              - The acpi_status code to be formatted
+ *
+ * RETURN:      A string containing the exception text. A valid pointer is
+ *              always returned.
+ *
+ * DESCRIPTION: This function translates an ACPI exception into an ASCII
+ *              string. Returns "unknown status" string for invalid codes.
+ *
+ ******************************************************************************/
+const char *acpi_format_exception(acpi_status status)
+{
+	const char *exception = NULL;
+
+	ACPI_FUNCTION_ENTRY();
+
+	exception = acpi_ut_validate_exception(status);
+	if (!exception) {
+
+		/* Exception code was not recognized */
+
+		ACPI_ERROR((AE_INFO,
+			    "Unknown exception code: 0x%8.8X", status));
+
+		exception = "UNKNOWN_STATUS_CODE";
+	}
+
+	return (ACPI_CAST_PTR(const char, exception));
+}
+
+ACPI_EXPORT_SYMBOL(acpi_format_exception)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_validate_exception
+ *
+ * PARAMETERS:  status              - The acpi_status code to be formatted
+ *
+ * RETURN:      A string containing the exception text. NULL if exception is
+ *              not valid.
+ *
+ * DESCRIPTION: This function validates and translates an ACPI exception into
+ *              an ASCII string.
+ *
+ ******************************************************************************/
+const char *acpi_ut_validate_exception(acpi_status status)
+{
+	u32 sub_status;
+	const char *exception = NULL;
+
+	ACPI_FUNCTION_ENTRY();
+
+	/*
+	 * Status is composed of two parts, a "type" and an actual code
+	 */
+	sub_status = (status & ~AE_CODE_MASK);
+
+	switch (status & AE_CODE_MASK) {
+	case AE_CODE_ENVIRONMENTAL:
+
+		if (sub_status <= AE_CODE_ENV_MAX) {
+			exception = acpi_gbl_exception_names_env[sub_status];
+		}
+		break;
+
+	case AE_CODE_PROGRAMMER:
+
+		if (sub_status <= AE_CODE_PGM_MAX) {
+			exception = acpi_gbl_exception_names_pgm[sub_status];
+		}
+		break;
+
+	case AE_CODE_ACPI_TABLES:
+
+		if (sub_status <= AE_CODE_TBL_MAX) {
+			exception = acpi_gbl_exception_names_tbl[sub_status];
+		}
+		break;
+
+	case AE_CODE_AML:
+
+		if (sub_status <= AE_CODE_AML_MAX) {
+			exception = acpi_gbl_exception_names_aml[sub_status];
+		}
+		break;
+
+	case AE_CODE_CONTROL:
+
+		if (sub_status <= AE_CODE_CTRL_MAX) {
+			exception = acpi_gbl_exception_names_ctrl[sub_status];
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return (ACPI_CAST_PTR(const char, exception));
+}
diff --git a/drivers/acpi/acpica/utglobal.c b/drivers/acpi/acpica/utglobal.c
index 90f53b42eca..ed1893155f8 100644
--- a/drivers/acpi/acpica/utglobal.c
+++ b/drivers/acpi/acpica/utglobal.c
@@ -247,8 +247,9 @@ struct acpi_fixed_event_info acpi_gbl_fixed_event_info[ACPI_NUM_FIXED_EVENTS] =
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Init library globals.  All globals that require specific
- *              initialization should be initialized here!
+ * DESCRIPTION: Initialize ACPICA globals. All globals that require specific
+ *              initialization should be initialized here. This allows for
+ *              a warm restart.
  *
  ******************************************************************************/
 
@@ -284,7 +285,7 @@ acpi_status acpi_ut_init_globals(void)
 		acpi_gbl_owner_id_mask[i] = 0;
 	}
 
-	/* Last owner_iD is never valid */
+	/* Last owner_ID is never valid */
 
 	acpi_gbl_owner_id_mask[ACPI_NUM_OWNERID_MASKS - 1] = 0x80000000;
 
@@ -304,8 +305,8 @@ acpi_status acpi_ut_init_globals(void)
 
 	/* Global handlers */
 
-	acpi_gbl_system_notify.handler = NULL;
-	acpi_gbl_device_notify.handler = NULL;
+	acpi_gbl_global_notify[0].handler = NULL;
+	acpi_gbl_global_notify[1].handler = NULL;
 	acpi_gbl_exception_handler = NULL;
 	acpi_gbl_init_handler = NULL;
 	acpi_gbl_table_handler = NULL;
diff --git a/drivers/acpi/acpica/utids.c b/drivers/acpi/acpica/utids.c
index c92eb1d9378..5d84e195457 100644
--- a/drivers/acpi/acpica/utids.c
+++ b/drivers/acpi/acpica/utids.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Module Name: utids - support for device IDs - HID, UID, CID
+ * Module Name: utids - support for device Ids - HID, UID, CID
  *
  *****************************************************************************/
 
diff --git a/drivers/acpi/acpica/utlock.c b/drivers/acpi/acpica/utlock.c
index 155fd786d0f..b1eb7f17e11 100644
--- a/drivers/acpi/acpica/utlock.c
+++ b/drivers/acpi/acpica/utlock.c
@@ -52,7 +52,7 @@ ACPI_MODULE_NAME("utlock")
  * FUNCTION:    acpi_ut_create_rw_lock
  *              acpi_ut_delete_rw_lock
  *
- * PARAMETERS:  Lock                - Pointer to a valid RW lock
+ * PARAMETERS:  lock                - Pointer to a valid RW lock
  *
  * RETURN:      Status
  *
@@ -89,7 +89,7 @@ void acpi_ut_delete_rw_lock(struct acpi_rw_lock *lock)
  * FUNCTION:    acpi_ut_acquire_read_lock
  *              acpi_ut_release_read_lock
  *
- * PARAMETERS:  Lock                - Pointer to a valid RW lock
+ * PARAMETERS:  lock                - Pointer to a valid RW lock
  *
  * RETURN:      Status
  *
@@ -149,7 +149,7 @@ acpi_status acpi_ut_release_read_lock(struct acpi_rw_lock *lock)
  * FUNCTION:    acpi_ut_acquire_write_lock
  *              acpi_ut_release_write_lock
  *
- * PARAMETERS:  Lock                - Pointer to a valid RW lock
+ * PARAMETERS:  lock                - Pointer to a valid RW lock
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/utmath.c b/drivers/acpi/acpica/utmath.c
index 2491a552b0e..d88a8aaab2a 100644
--- a/drivers/acpi/acpica/utmath.c
+++ b/drivers/acpi/acpica/utmath.c
@@ -73,8 +73,8 @@ typedef union uint64_overlay {
  *
  * FUNCTION:    acpi_ut_short_divide
  *
- * PARAMETERS:  Dividend            - 64-bit dividend
- *              Divisor             - 32-bit divisor
+ * PARAMETERS:  dividend            - 64-bit dividend
+ *              divisor             - 32-bit divisor
  *              out_quotient        - Pointer to where the quotient is returned
  *              out_remainder       - Pointer to where the remainder is returned
  *
diff --git a/drivers/acpi/acpica/utmisc.c b/drivers/acpi/acpica/utmisc.c
index 86f19db74e0..33c6cf7ff46 100644
--- a/drivers/acpi/acpica/utmisc.c
+++ b/drivers/acpi/acpica/utmisc.c
@@ -50,79 +50,41 @@
 #define _COMPONENT          ACPI_UTILITIES
 ACPI_MODULE_NAME("utmisc")
 
+#if defined ACPI_ASL_COMPILER || defined ACPI_EXEC_APP
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_validate_exception
+ * FUNCTION:    ut_convert_backslashes
  *
- * PARAMETERS:  Status       - The acpi_status code to be formatted
+ * PARAMETERS:  pathname        - File pathname string to be converted
  *
- * RETURN:      A string containing the exception text. NULL if exception is
- *              not valid.
+ * RETURN:      Modifies the input Pathname
  *
- * DESCRIPTION: This function validates and translates an ACPI exception into
- *              an ASCII string.
+ * DESCRIPTION: Convert all backslashes (0x5C) to forward slashes (0x2F) within
+ *              the entire input file pathname string.
  *
  ******************************************************************************/
-const char *acpi_ut_validate_exception(acpi_status status)
+void ut_convert_backslashes(char *pathname)
 {
-	u32 sub_status;
-	const char *exception = NULL;
 
-	ACPI_FUNCTION_ENTRY();
-
-	/*
-	 * Status is composed of two parts, a "type" and an actual code
-	 */
-	sub_status = (status & ~AE_CODE_MASK);
-
-	switch (status & AE_CODE_MASK) {
-	case AE_CODE_ENVIRONMENTAL:
-
-		if (sub_status <= AE_CODE_ENV_MAX) {
-			exception = acpi_gbl_exception_names_env[sub_status];
-		}
-		break;
-
-	case AE_CODE_PROGRAMMER:
-
-		if (sub_status <= AE_CODE_PGM_MAX) {
-			exception = acpi_gbl_exception_names_pgm[sub_status];
-		}
-		break;
-
-	case AE_CODE_ACPI_TABLES:
-
-		if (sub_status <= AE_CODE_TBL_MAX) {
-			exception = acpi_gbl_exception_names_tbl[sub_status];
-		}
-		break;
-
-	case AE_CODE_AML:
-
-		if (sub_status <= AE_CODE_AML_MAX) {
-			exception = acpi_gbl_exception_names_aml[sub_status];
-		}
-		break;
-
-	case AE_CODE_CONTROL:
+	if (!pathname) {
+		return;
+	}
 
-		if (sub_status <= AE_CODE_CTRL_MAX) {
-			exception = acpi_gbl_exception_names_ctrl[sub_status];
+	while (*pathname) {
+		if (*pathname == '\\') {
+			*pathname = '/';
 		}
-		break;
 
-	default:
-		break;
+		pathname++;
 	}
-
-	return (ACPI_CAST_PTR(const char, exception));
 }
+#endif
 
 /*******************************************************************************
  *
  * FUNCTION:    acpi_ut_is_pci_root_bridge
  *
- * PARAMETERS:  Id              - The HID/CID in string format
+ * PARAMETERS:  id              - The HID/CID in string format
  *
  * RETURN:      TRUE if the Id is a match for a PCI/PCI-Express Root Bridge
  *
@@ -150,7 +112,7 @@ u8 acpi_ut_is_pci_root_bridge(char *id)
  *
  * FUNCTION:    acpi_ut_is_aml_table
  *
- * PARAMETERS:  Table               - An ACPI table
+ * PARAMETERS:  table               - An ACPI table
  *
  * RETURN:      TRUE if table contains executable AML; FALSE otherwise
  *
@@ -284,7 +246,7 @@ acpi_status acpi_ut_allocate_owner_id(acpi_owner_id * owner_id)
  *
  * FUNCTION:    acpi_ut_release_owner_id
  *
- * PARAMETERS:  owner_id_ptr        - Pointer to a previously allocated owner_iD
+ * PARAMETERS:  owner_id_ptr        - Pointer to a previously allocated owner_ID
  *
  * RETURN:      None. No error is returned because we are either exiting a
  *              control method or unloading a table. Either way, we would
@@ -307,7 +269,7 @@ void acpi_ut_release_owner_id(acpi_owner_id * owner_id_ptr)
 
 	*owner_id_ptr = 0;
 
-	/* Zero is not a valid owner_iD */
+	/* Zero is not a valid owner_ID */
 
 	if (owner_id == 0) {
 		ACPI_ERROR((AE_INFO, "Invalid OwnerId: 0x%2.2X", owner_id));
@@ -381,7 +343,7 @@ void acpi_ut_strupr(char *src_string)
  *
  * FUNCTION:    acpi_ut_print_string
  *
- * PARAMETERS:  String          - Null terminated ASCII string
+ * PARAMETERS:  string          - Null terminated ASCII string
  *              max_length      - Maximum output length
  *
  * RETURN:      None
@@ -467,7 +429,7 @@ void acpi_ut_print_string(char *string, u8 max_length)
  *
  * FUNCTION:    acpi_ut_dword_byte_swap
  *
- * PARAMETERS:  Value           - Value to be converted
+ * PARAMETERS:  value           - Value to be converted
  *
  * RETURN:      u32 integer with bytes swapped
  *
@@ -537,9 +499,9 @@ void acpi_ut_set_integer_width(u8 revision)
  *
  * FUNCTION:    acpi_ut_display_init_pathname
  *
- * PARAMETERS:  Type                - Object type of the node
+ * PARAMETERS:  type                - Object type of the node
  *              obj_handle          - Handle whose pathname will be displayed
- *              Path                - Additional path string to be appended.
+ *              path                - Additional path string to be appended.
  *                                      (NULL if no extra path)
  *
  * RETURN:      acpi_status
@@ -604,8 +566,8 @@ acpi_ut_display_init_pathname(u8 type,
  *
  * FUNCTION:    acpi_ut_valid_acpi_char
  *
- * PARAMETERS:  Char            - The character to be examined
- *              Position        - Byte position (0-3)
+ * PARAMETERS:  char            - The character to be examined
+ *              position        - Byte position (0-3)
  *
  * RETURN:      TRUE if the character is valid, FALSE otherwise
  *
@@ -640,7 +602,7 @@ u8 acpi_ut_valid_acpi_char(char character, u32 position)
  *
  * FUNCTION:    acpi_ut_valid_acpi_name
  *
- * PARAMETERS:  Name            - The name to be examined
+ * PARAMETERS:  name            - The name to be examined
  *
  * RETURN:      TRUE if the name is valid, FALSE otherwise
  *
@@ -671,7 +633,7 @@ u8 acpi_ut_valid_acpi_name(u32 name)
  *
  * FUNCTION:    acpi_ut_repair_name
  *
- * PARAMETERS:  Name            - The ACPI name to be repaired
+ * PARAMETERS:  name            - The ACPI name to be repaired
  *
  * RETURN:      Repaired version of the name
  *
@@ -705,8 +667,8 @@ acpi_name acpi_ut_repair_name(char *name)
  *
  * FUNCTION:    acpi_ut_strtoul64
  *
- * PARAMETERS:  String          - Null terminated string
- *              Base            - Radix of the string: 16 or ACPI_ANY_BASE;
+ * PARAMETERS:  string          - Null terminated string
+ *              base            - Radix of the string: 16 or ACPI_ANY_BASE;
  *                                ACPI_ANY_BASE means 'in behalf of to_integer'
  *              ret_integer     - Where the converted integer is returned
  *
@@ -755,7 +717,7 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 * ret_integer)
 
 	if (to_integer_op) {
 		/*
-		 * Base equal to ACPI_ANY_BASE means 'to_integer operation case'.
+		 * Base equal to ACPI_ANY_BASE means 'ToInteger operation case'.
 		 * We need to determine if it is decimal or hexadecimal.
 		 */
 		if ((*string == '0') && (ACPI_TOLOWER(*(string + 1)) == 'x')) {
@@ -878,8 +840,8 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 * ret_integer)
  *
  * FUNCTION:    acpi_ut_create_update_state_and_push
  *
- * PARAMETERS:  Object          - Object to be added to the new state
- *              Action          - Increment/Decrement
+ * PARAMETERS:  object          - Object to be added to the new state
+ *              action          - Increment/Decrement
  *              state_list      - List the state will be added to
  *
  * RETURN:      Status
@@ -919,7 +881,7 @@ acpi_ut_create_update_state_and_push(union acpi_operand_object *object,
  * PARAMETERS:  source_object       - The package to walk
  *              target_object       - Target object (if package is being copied)
  *              walk_callback       - Called once for each package element
- *              Context             - Passed to the callback function
+ *              context             - Passed to the callback function
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/utmutex.c b/drivers/acpi/acpica/utmutex.c
index 43174df3312..296baa676bc 100644
--- a/drivers/acpi/acpica/utmutex.c
+++ b/drivers/acpi/acpica/utmutex.c
@@ -147,7 +147,7 @@ void acpi_ut_mutex_terminate(void)
  *
  * FUNCTION:    acpi_ut_create_mutex
  *
- * PARAMETERS:  mutex_iD        - ID of the mutex to be created
+ * PARAMETERS:  mutex_ID        - ID of the mutex to be created
  *
  * RETURN:      Status
  *
@@ -176,7 +176,7 @@ static acpi_status acpi_ut_create_mutex(acpi_mutex_handle mutex_id)
  *
  * FUNCTION:    acpi_ut_delete_mutex
  *
- * PARAMETERS:  mutex_iD        - ID of the mutex to be deleted
+ * PARAMETERS:  mutex_ID        - ID of the mutex to be deleted
  *
  * RETURN:      Status
  *
@@ -199,7 +199,7 @@ static void acpi_ut_delete_mutex(acpi_mutex_handle mutex_id)
  *
  * FUNCTION:    acpi_ut_acquire_mutex
  *
- * PARAMETERS:  mutex_iD        - ID of the mutex to be acquired
+ * PARAMETERS:  mutex_ID        - ID of the mutex to be acquired
  *
  * RETURN:      Status
  *
@@ -283,7 +283,7 @@ acpi_status acpi_ut_acquire_mutex(acpi_mutex_handle mutex_id)
  *
  * FUNCTION:    acpi_ut_release_mutex
  *
- * PARAMETERS:  mutex_iD        - ID of the mutex to be released
+ * PARAMETERS:  mutex_ID        - ID of the mutex to be released
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/utobject.c b/drivers/acpi/acpica/utobject.c
index b112744fc9a..655f0799a39 100644
--- a/drivers/acpi/acpica/utobject.c
+++ b/drivers/acpi/acpica/utobject.c
@@ -69,7 +69,7 @@ acpi_ut_get_element_length(u8 object_type,
  * PARAMETERS:  module_name         - Source file name of caller
  *              line_number         - Line number of caller
  *              component_id        - Component type of caller
- *              Type                - ACPI Type of the new object
+ *              type                - ACPI Type of the new object
  *
  * RETURN:      A new internal object, null on failure
  *
@@ -150,7 +150,7 @@ union acpi_operand_object *acpi_ut_create_internal_object_dbg(const char
  *
  * FUNCTION:    acpi_ut_create_package_object
  *
- * PARAMETERS:  Count               - Number of package elements
+ * PARAMETERS:  count               - Number of package elements
  *
  * RETURN:      Pointer to a new Package object, null on failure
  *
@@ -323,11 +323,11 @@ union acpi_operand_object *acpi_ut_create_string_object(acpi_size string_size)
  *
  * FUNCTION:    acpi_ut_valid_internal_object
  *
- * PARAMETERS:  Object              - Object to be validated
+ * PARAMETERS:  object              - Object to be validated
  *
  * RETURN:      TRUE if object is valid, FALSE otherwise
  *
- * DESCRIPTION: Validate a pointer to be a union acpi_operand_object
+ * DESCRIPTION: Validate a pointer to be of type union acpi_operand_object
  *
  ******************************************************************************/
 
@@ -348,7 +348,7 @@ u8 acpi_ut_valid_internal_object(void *object)
 	switch (ACPI_GET_DESCRIPTOR_TYPE(object)) {
 	case ACPI_DESC_TYPE_OPERAND:
 
-		/* The object appears to be a valid union acpi_operand_object    */
+		/* The object appears to be a valid union acpi_operand_object */
 
 		return (TRUE);
 
@@ -407,7 +407,7 @@ void *acpi_ut_allocate_object_desc_dbg(const char *module_name,
  *
  * FUNCTION:    acpi_ut_delete_object_desc
  *
- * PARAMETERS:  Object          - An Acpi internal object to be deleted
+ * PARAMETERS:  object          - An Acpi internal object to be deleted
  *
  * RETURN:      None.
  *
@@ -419,7 +419,7 @@ void acpi_ut_delete_object_desc(union acpi_operand_object *object)
 {
 	ACPI_FUNCTION_TRACE_PTR(ut_delete_object_desc, object);
 
-	/* Object must be a union acpi_operand_object    */
+	/* Object must be a union acpi_operand_object */
 
 	if (ACPI_GET_DESCRIPTOR_TYPE(object) != ACPI_DESC_TYPE_OPERAND) {
 		ACPI_ERROR((AE_INFO,
diff --git a/drivers/acpi/acpica/utosi.c b/drivers/acpi/acpica/utosi.c
index 2360cf70c18..34ef0bd7e4b 100644
--- a/drivers/acpi/acpica/utosi.c
+++ b/drivers/acpi/acpica/utosi.c
@@ -68,7 +68,7 @@ static struct acpi_interface_info acpi_default_supported_interfaces[] = {
 	{"Windows 2001.1", NULL, 0, ACPI_OSI_WINSRV_2003},	/* Windows Server 2003 */
 	{"Windows 2001 SP2", NULL, 0, ACPI_OSI_WIN_XP_SP2},	/* Windows XP SP2 */
 	{"Windows 2001.1 SP1", NULL, 0, ACPI_OSI_WINSRV_2003_SP1},	/* Windows Server 2003 SP1 - Added 03/2006 */
-	{"Windows 2006", NULL, 0, ACPI_OSI_WIN_VISTA},	/* Windows Vista - Added 03/2006 */
+	{"Windows 2006", NULL, 0, ACPI_OSI_WIN_VISTA},	/* Windows vista - Added 03/2006 */
 	{"Windows 2006.1", NULL, 0, ACPI_OSI_WINSRV_2008},	/* Windows Server 2008 - Added 09/2009 */
 	{"Windows 2006 SP1", NULL, 0, ACPI_OSI_WIN_VISTA_SP1},	/* Windows Vista SP1 - Added 09/2009 */
 	{"Windows 2006 SP2", NULL, 0, ACPI_OSI_WIN_VISTA_SP2},	/* Windows Vista SP2 - Added 09/2010 */
diff --git a/drivers/acpi/acpica/utresrc.c b/drivers/acpi/acpica/utresrc.c
index 9d441ea7030..e38bef4980b 100644
--- a/drivers/acpi/acpica/utresrc.c
+++ b/drivers/acpi/acpica/utresrc.c
@@ -356,13 +356,13 @@ static const u8 acpi_gbl_resource_types[] = {
 	ACPI_SMALL_VARIABLE_LENGTH,	/* 06 start_dependent_functions */
 	ACPI_FIXED_LENGTH,	/* 07 end_dependent_functions */
 	ACPI_FIXED_LENGTH,	/* 08 IO */
-	ACPI_FIXED_LENGTH,	/* 09 fixed_iO */
-	ACPI_FIXED_LENGTH,	/* 0_a fixed_dMA */
+	ACPI_FIXED_LENGTH,	/* 09 fixed_IO */
+	ACPI_FIXED_LENGTH,	/* 0A fixed_DMA */
 	0,
 	0,
 	0,
-	ACPI_VARIABLE_LENGTH,	/* 0_e vendor_short */
-	ACPI_FIXED_LENGTH,	/* 0_f end_tag */
+	ACPI_VARIABLE_LENGTH,	/* 0E vendor_short */
+	ACPI_FIXED_LENGTH,	/* 0F end_tag */
 
 	/* Large descriptors */
 
@@ -375,16 +375,16 @@ static const u8 acpi_gbl_resource_types[] = {
 	ACPI_FIXED_LENGTH,	/* 06 memory32_fixed */
 	ACPI_VARIABLE_LENGTH,	/* 07 Dword* address */
 	ACPI_VARIABLE_LENGTH,	/* 08 Word* address */
-	ACPI_VARIABLE_LENGTH,	/* 09 extended_iRQ */
-	ACPI_VARIABLE_LENGTH,	/* 0_a Qword* address */
-	ACPI_FIXED_LENGTH,	/* 0_b Extended* address */
-	ACPI_VARIABLE_LENGTH,	/* 0_c Gpio* */
+	ACPI_VARIABLE_LENGTH,	/* 09 extended_IRQ */
+	ACPI_VARIABLE_LENGTH,	/* 0A Qword* address */
+	ACPI_FIXED_LENGTH,	/* 0B Extended* address */
+	ACPI_VARIABLE_LENGTH,	/* 0C Gpio* */
 	0,
-	ACPI_VARIABLE_LENGTH	/* 0_e *serial_bus */
+	ACPI_VARIABLE_LENGTH	/* 0E *serial_bus */
 };
 
 /*
- * For the i_aSL compiler/disassembler, we don't want any error messages
+ * For the iASL compiler/disassembler, we don't want any error messages
  * because the disassembler uses the resource validation code to determine
  * if Buffer objects are actually Resource Templates.
  */
@@ -398,11 +398,11 @@ static const u8 acpi_gbl_resource_types[] = {
  *
  * FUNCTION:    acpi_ut_walk_aml_resources
  *
- * PARAMETERS:  Aml             - Pointer to the raw AML resource template
+ * PARAMETERS:  aml             - Pointer to the raw AML resource template
  *              aml_length      - Length of the entire template
  *              user_function   - Called once for each descriptor found. If
  *                                NULL, a pointer to the end_tag is returned
- *              Context         - Passed to user_function
+ *              context         - Passed to user_function
  *
  * RETURN:      Status
  *
@@ -513,7 +513,7 @@ acpi_ut_walk_aml_resources(u8 * aml,
  *
  * FUNCTION:    acpi_ut_validate_resource
  *
- * PARAMETERS:  Aml             - Pointer to the raw AML resource descriptor
+ * PARAMETERS:  aml             - Pointer to the raw AML resource descriptor
  *              return_index    - Where the resource index is returned. NULL
  *                                if the index is not required.
  *
@@ -664,7 +664,7 @@ acpi_status acpi_ut_validate_resource(void *aml, u8 * return_index)
  *
  * FUNCTION:    acpi_ut_get_resource_type
  *
- * PARAMETERS:  Aml             - Pointer to the raw AML resource descriptor
+ * PARAMETERS:  aml             - Pointer to the raw AML resource descriptor
  *
  * RETURN:      The Resource Type with no extraneous bits (except the
  *              Large/Small descriptor bit -- this is left alone)
@@ -698,7 +698,7 @@ u8 acpi_ut_get_resource_type(void *aml)
  *
  * FUNCTION:    acpi_ut_get_resource_length
  *
- * PARAMETERS:  Aml             - Pointer to the raw AML resource descriptor
+ * PARAMETERS:  aml             - Pointer to the raw AML resource descriptor
  *
  * RETURN:      Byte Length
  *
@@ -738,7 +738,7 @@ u16 acpi_ut_get_resource_length(void *aml)
  *
  * FUNCTION:    acpi_ut_get_resource_header_length
  *
- * PARAMETERS:  Aml             - Pointer to the raw AML resource descriptor
+ * PARAMETERS:  aml             - Pointer to the raw AML resource descriptor
  *
  * RETURN:      Length of the AML header (depends on large/small descriptor)
  *
@@ -763,7 +763,7 @@ u8 acpi_ut_get_resource_header_length(void *aml)
  *
  * FUNCTION:    acpi_ut_get_descriptor_length
  *
- * PARAMETERS:  Aml             - Pointer to the raw AML resource descriptor
+ * PARAMETERS:  aml             - Pointer to the raw AML resource descriptor
  *
  * RETURN:      Byte length
  *
diff --git a/drivers/acpi/acpica/utstate.c b/drivers/acpi/acpica/utstate.c
index 4267477c279..a1c98826007 100644
--- a/drivers/acpi/acpica/utstate.c
+++ b/drivers/acpi/acpica/utstate.c
@@ -51,8 +51,8 @@ ACPI_MODULE_NAME("utstate")
  *
  * FUNCTION:    acpi_ut_create_pkg_state_and_push
  *
- * PARAMETERS:  Object          - Object to be added to the new state
- *              Action          - Increment/Decrement
+ * PARAMETERS:  object          - Object to be added to the new state
+ *              action          - Increment/Decrement
  *              state_list      - List the state will be added to
  *
  * RETURN:      Status
@@ -85,7 +85,7 @@ acpi_ut_create_pkg_state_and_push(void *internal_object,
  * FUNCTION:    acpi_ut_push_generic_state
  *
  * PARAMETERS:  list_head           - Head of the state stack
- *              State               - State object to push
+ *              state               - State object to push
  *
  * RETURN:      None
  *
@@ -214,8 +214,8 @@ struct acpi_thread_state *acpi_ut_create_thread_state(void)
  *
  * FUNCTION:    acpi_ut_create_update_state
  *
- * PARAMETERS:  Object          - Initial Object to be installed in the state
- *              Action          - Update action to be performed
+ * PARAMETERS:  object          - Initial Object to be installed in the state
+ *              action          - Update action to be performed
  *
  * RETURN:      New state object, null on failure
  *
@@ -252,8 +252,8 @@ union acpi_generic_state *acpi_ut_create_update_state(union acpi_operand_object
  *
  * FUNCTION:    acpi_ut_create_pkg_state
  *
- * PARAMETERS:  Object          - Initial Object to be installed in the state
- *              Action          - Update action to be performed
+ * PARAMETERS:  object          - Initial Object to be installed in the state
+ *              action          - Update action to be performed
  *
  * RETURN:      New state object, null on failure
  *
@@ -325,7 +325,7 @@ union acpi_generic_state *acpi_ut_create_control_state(void)
  *
  * FUNCTION:    acpi_ut_delete_generic_state
  *
- * PARAMETERS:  State               - The state object to be deleted
+ * PARAMETERS:  state               - The state object to be deleted
  *
  * RETURN:      None
  *
diff --git a/drivers/acpi/acpica/utxface.c b/drivers/acpi/acpica/utxface.c
index afa94f51ff0..534179f1177 100644
--- a/drivers/acpi/acpica/utxface.c
+++ b/drivers/acpi/acpica/utxface.c
@@ -131,7 +131,7 @@ acpi_status __init acpi_initialize_subsystem(void)
  *
  * FUNCTION:    acpi_enable_subsystem
  *
- * PARAMETERS:  Flags           - Init/enable Options
+ * PARAMETERS:  flags           - Init/enable Options
  *
  * RETURN:      Status
  *
@@ -234,7 +234,7 @@ ACPI_EXPORT_SYMBOL(acpi_enable_subsystem)
  *
  * FUNCTION:    acpi_initialize_objects
  *
- * PARAMETERS:  Flags           - Init/enable Options
+ * PARAMETERS:  flags           - Init/enable Options
  *
  * RETURN:      Status
  *
@@ -409,7 +409,7 @@ ACPI_EXPORT_SYMBOL(acpi_subsystem_status)
  * PARAMETERS:  out_buffer      - A buffer to receive the resources for the
  *                                device
  *
- * RETURN:      Status          - the status of the call
+ * RETURN:      status          - the status of the call
  *
  * DESCRIPTION: This function is called to get information about the current
  *              state of the ACPI subsystem.  It will return system information
@@ -480,8 +480,8 @@ ACPI_EXPORT_SYMBOL(acpi_get_system_info)
  *
  * FUNCTION:    acpi_install_initialization_handler
  *
- * PARAMETERS:  Handler             - Callback procedure
- *              Function            - Not (currently) used, see below
+ * PARAMETERS:  handler             - Callback procedure
+ *              function            - Not (currently) used, see below
  *
  * RETURN:      Status
  *
@@ -618,7 +618,7 @@ ACPI_EXPORT_SYMBOL(acpi_remove_interface)
  *
  * FUNCTION:    acpi_install_interface_handler
  *
- * PARAMETERS:  Handler             - The _OSI interface handler to install
+ * PARAMETERS:  handler             - The _OSI interface handler to install
  *                                    NULL means "remove existing handler"
  *
  * RETURN:      Status
@@ -651,9 +651,9 @@ ACPI_EXPORT_SYMBOL(acpi_install_interface_handler)
  * FUNCTION:    acpi_check_address_range
  *
  * PARAMETERS:  space_id            - Address space ID
- *              Address             - Start address
- *              Length              - Length
- *              Warn                - TRUE if warning on overlap desired
+ *              address             - Start address
+ *              length              - Length
+ *              warn                - TRUE if warning on overlap desired
  *
  * RETURN:      Count of the number of conflicts detected.
  *
diff --git a/drivers/acpi/acpica/utxferror.c b/drivers/acpi/acpica/utxferror.c
index 52b568af181..6d63cc39b9a 100644
--- a/drivers/acpi/acpica/utxferror.c
+++ b/drivers/acpi/acpica/utxferror.c
@@ -53,7 +53,7 @@ ACPI_MODULE_NAME("utxferror")
  * This module is used for the in-kernel ACPICA as well as the ACPICA
  * tools/applications.
  *
- * For the i_aSL compiler case, the output is redirected to stderr so that
+ * For the iASL compiler case, the output is redirected to stderr so that
  * any of the various ACPI errors and warnings do not appear in the output
  * files, for either the compiler or disassembler portions of the tool.
  */
@@ -70,7 +70,7 @@ extern FILE *acpi_gbl_output_file;
 
 #else
 /*
- * non-i_aSL case - no redirection, nothing to do
+ * non-iASL case - no redirection, nothing to do
  */
 #define ACPI_MSG_REDIRECT_BEGIN
 #define ACPI_MSG_REDIRECT_END
@@ -82,6 +82,8 @@ extern FILE *acpi_gbl_output_file;
 #define ACPI_MSG_EXCEPTION      "ACPI Exception: "
 #define ACPI_MSG_WARNING        "ACPI Warning: "
 #define ACPI_MSG_INFO           "ACPI: "
+#define ACPI_MSG_BIOS_ERROR     "ACPI BIOS Bug: Error: "
+#define ACPI_MSG_BIOS_WARNING   "ACPI BIOS Bug: Warning: "
 /*
  * Common message suffix
  */
@@ -93,7 +95,7 @@ extern FILE *acpi_gbl_output_file;
  *
  * PARAMETERS:  module_name         - Caller's module name (for error output)
  *              line_number         - Caller's line number (for error output)
- *              Format              - Printf format string + additional args
+ *              format              - Printf format string + additional args
  *
  * RETURN:      None
  *
@@ -124,8 +126,8 @@ ACPI_EXPORT_SYMBOL(acpi_error)
  *
  * PARAMETERS:  module_name         - Caller's module name (for error output)
  *              line_number         - Caller's line number (for error output)
- *              Status              - Status to be formatted
- *              Format              - Printf format string + additional args
+ *              status              - Status to be formatted
+ *              format              - Printf format string + additional args
  *
  * RETURN:      None
  *
@@ -159,7 +161,7 @@ ACPI_EXPORT_SYMBOL(acpi_exception)
  *
  * PARAMETERS:  module_name         - Caller's module name (for error output)
  *              line_number         - Caller's line number (for error output)
- *              Format              - Printf format string + additional args
+ *              format              - Printf format string + additional args
  *
  * RETURN:      None
  *
@@ -190,7 +192,7 @@ ACPI_EXPORT_SYMBOL(acpi_warning)
  *
  * PARAMETERS:  module_name         - Caller's module name (for error output)
  *              line_number         - Caller's line number (for error output)
- *              Format              - Printf format string + additional args
+ *              format              - Printf format string + additional args
  *
  * RETURN:      None
  *
@@ -218,6 +220,72 @@ acpi_info(const char *module_name, u32 line_number, const char *format, ...)
 
 ACPI_EXPORT_SYMBOL(acpi_info)
 
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_bios_error
+ *
+ * PARAMETERS:  module_name         - Caller's module name (for error output)
+ *              line_number         - Caller's line number (for error output)
+ *              format              - Printf format string + additional args
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Print "ACPI Firmware Error" message with module/line/version
+ *              info
+ *
+ ******************************************************************************/
+void ACPI_INTERNAL_VAR_XFACE
+acpi_bios_error(const char *module_name,
+		u32 line_number, const char *format, ...)
+{
+	va_list arg_list;
+
+	ACPI_MSG_REDIRECT_BEGIN;
+	acpi_os_printf(ACPI_MSG_BIOS_ERROR);
+
+	va_start(arg_list, format);
+	acpi_os_vprintf(format, arg_list);
+	ACPI_MSG_SUFFIX;
+	va_end(arg_list);
+
+	ACPI_MSG_REDIRECT_END;
+}
+
+ACPI_EXPORT_SYMBOL(acpi_bios_error)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_bios_warning
+ *
+ * PARAMETERS:  module_name         - Caller's module name (for error output)
+ *              line_number         - Caller's line number (for error output)
+ *              format              - Printf format string + additional args
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Print "ACPI Firmware Warning" message with module/line/version
+ *              info
+ *
+ ******************************************************************************/
+void ACPI_INTERNAL_VAR_XFACE
+acpi_bios_warning(const char *module_name,
+		  u32 line_number, const char *format, ...)
+{
+	va_list arg_list;
+
+	ACPI_MSG_REDIRECT_BEGIN;
+	acpi_os_printf(ACPI_MSG_BIOS_WARNING);
+
+	va_start(arg_list, format);
+	acpi_os_vprintf(format, arg_list);
+	ACPI_MSG_SUFFIX;
+	va_end(arg_list);
+
+	ACPI_MSG_REDIRECT_END;
+}
+
+ACPI_EXPORT_SYMBOL(acpi_bios_warning)
+
 /*
  * The remainder of this module contains internal error functions that may
  * be configured out.
@@ -271,9 +339,9 @@ acpi_ut_predefined_warning(const char *module_name,
  *
  * PARAMETERS:  module_name     - Caller's module name (for error output)
  *              line_number     - Caller's line number (for error output)
- *              Pathname        - Full pathname to the node
+ *              pathname        - Full pathname to the node
  *              node_flags      - From Namespace node for the method/object
- *              Format          - Printf format string + additional args
+ *              format          - Printf format string + additional args
  *
  * RETURN:      None
  *
@@ -373,9 +441,9 @@ acpi_ut_namespace_error(const char *module_name,
  *
  * PARAMETERS:  module_name         - Caller's module name (for error output)
  *              line_number         - Caller's line number (for error output)
- *              Message             - Error message to use on failure
+ *              message             - Error message to use on failure
  *              prefix_node         - Prefix relative to the path
- *              Path                - Path to the node (optional)
+ *              path                - Path to the node (optional)
  *              method_status       - Execution status
  *
  * RETURN:      None
diff --git a/drivers/acpi/acpica/utxfmutex.c b/drivers/acpi/acpica/utxfmutex.c
index 1427d191d15..0a40a851b35 100644
--- a/drivers/acpi/acpica/utxfmutex.c
+++ b/drivers/acpi/acpica/utxfmutex.c
@@ -58,8 +58,8 @@ acpi_ut_get_mutex_object(acpi_handle handle,
  *
  * FUNCTION:    acpi_ut_get_mutex_object
  *
- * PARAMETERS:  Handle              - Mutex or prefix handle (optional)
- *              Pathname            - Mutex pathname (optional)
+ * PARAMETERS:  handle              - Mutex or prefix handle (optional)
+ *              pathname            - Mutex pathname (optional)
  *              ret_obj             - Where the mutex object is returned
  *
  * RETURN:      Status
@@ -118,9 +118,9 @@ acpi_ut_get_mutex_object(acpi_handle handle,
  *
  * FUNCTION:    acpi_acquire_mutex
  *
- * PARAMETERS:  Handle              - Mutex or prefix handle (optional)
- *              Pathname            - Mutex pathname (optional)
- *              Timeout             - Max time to wait for the lock (millisec)
+ * PARAMETERS:  handle              - Mutex or prefix handle (optional)
+ *              pathname            - Mutex pathname (optional)
+ *              timeout             - Max time to wait for the lock (millisec)
  *
  * RETURN:      Status
  *
@@ -155,8 +155,8 @@ acpi_acquire_mutex(acpi_handle handle, acpi_string pathname, u16 timeout)
  *
  * FUNCTION:    acpi_release_mutex
  *
- * PARAMETERS:  Handle              - Mutex or prefix handle (optional)
- *              Pathname            - Mutex pathname (optional)
+ * PARAMETERS:  handle              - Mutex or prefix handle (optional)
+ *              pathname            - Mutex pathname (optional)
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/apei/apei-base.c b/drivers/acpi/apei/apei-base.c
index 6686b1eaf13..00a783661d0 100644
--- a/drivers/acpi/apei/apei-base.c
+++ b/drivers/acpi/apei/apei-base.c
@@ -586,6 +586,11 @@ static int apei_check_gar(struct acpi_generic_address *reg, u64 *paddr,
 	}
 	*access_bit_width = 1UL << (access_size_code + 2);
 
+	/* Fixup common BIOS bug */
+	if (bit_width == 32 && bit_offset == 0 && (*paddr & 0x03) == 0 &&
+	    *access_bit_width < 32)
+		*access_bit_width = 32;
+
 	if ((bit_width + bit_offset) > *access_bit_width) {
 		pr_warning(FW_BUG APEI_PFX
 			   "Invalid bit width + offset in GAR [0x%llx/%u/%u/%u/%u]\n",
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index 7dd3f9fb9f3..ff2c876ec41 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -250,6 +250,13 @@ static int acpi_battery_get_property(struct power_supply *psy,
 		else
 			val->intval = battery->capacity_now * 1000;
 		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		if (battery->capacity_now && battery->full_charge_capacity)
+			val->intval = battery->capacity_now * 100/
+					battery->full_charge_capacity;
+		else
+			val->intval = 0;
+		break;
 	case POWER_SUPPLY_PROP_MODEL_NAME:
 		val->strval = battery->model_number;
 		break;
@@ -276,6 +283,7 @@ static enum power_supply_property charge_battery_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
 	POWER_SUPPLY_PROP_CHARGE_FULL,
 	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
 	POWER_SUPPLY_PROP_MODEL_NAME,
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
@@ -292,6 +300,7 @@ static enum power_supply_property energy_battery_props[] = {
 	POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN,
 	POWER_SUPPLY_PROP_ENERGY_FULL,
 	POWER_SUPPLY_PROP_ENERGY_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
 	POWER_SUPPLY_PROP_MODEL_NAME,
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
@@ -1044,17 +1053,24 @@ static int acpi_battery_remove(struct acpi_device *device, int type)
 }
 
 /* this is needed to learn about changes made in suspended state */
-static int acpi_battery_resume(struct acpi_device *device)
+static int acpi_battery_resume(struct device *dev)
 {
 	struct acpi_battery *battery;
-	if (!device)
+
+	if (!dev)
 		return -EINVAL;
-	battery = acpi_driver_data(device);
+
+	battery = acpi_driver_data(to_acpi_device(dev));
+	if (!battery)
+		return -EINVAL;
+
 	battery->update_time = 0;
 	acpi_battery_update(battery);
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(acpi_battery_pm, NULL, acpi_battery_resume);
+
 static struct acpi_driver acpi_battery_driver = {
 	.name = "battery",
 	.class = ACPI_BATTERY_CLASS,
@@ -1062,10 +1078,10 @@ static struct acpi_driver acpi_battery_driver = {
 	.flags = ACPI_DRIVER_ALL_NOTIFY_EVENTS,
 	.ops = {
 		.add = acpi_battery_add,
-		.resume = acpi_battery_resume,
 		.remove = acpi_battery_remove,
 		.notify = acpi_battery_notify,
 		},
+	.drv.pm = &acpi_battery_pm,
 };
 
 static void __init acpi_battery_init_async(void *unused, async_cookie_t cookie)
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index adceafda9c1..9628652e080 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -574,6 +574,10 @@ static void acpi_bus_osc_support(void)
 	capbuf[OSC_SUPPORT_TYPE] |= OSC_SB_PPC_OST_SUPPORT;
 #endif
 
+#ifdef ACPI_HOTPLUG_OST
+	capbuf[OSC_SUPPORT_TYPE] |= OSC_SB_HOTPLUG_OST_SUPPORT;
+#endif
+
 	if (!ghes_disable)
 		capbuf[OSC_SUPPORT_TYPE] |= OSC_SB_APEI_SUPPORT;
 	if (ACPI_FAILURE(acpi_get_handle(NULL, "\\_SB", &handle)))
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index d27d072472f..79d4c22f7a6 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -76,19 +76,21 @@ MODULE_DEVICE_TABLE(acpi, button_device_ids);
 
 static int acpi_button_add(struct acpi_device *device);
 static int acpi_button_remove(struct acpi_device *device, int type);
-static int acpi_button_resume(struct acpi_device *device);
 static void acpi_button_notify(struct acpi_device *device, u32 event);
 
+static int acpi_button_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(acpi_button_pm, NULL, acpi_button_resume);
+
 static struct acpi_driver acpi_button_driver = {
 	.name = "button",
 	.class = ACPI_BUTTON_CLASS,
 	.ids = button_device_ids,
 	.ops = {
 		.add = acpi_button_add,
-		.resume = acpi_button_resume,
 		.remove = acpi_button_remove,
 		.notify = acpi_button_notify,
 	},
+	.drv.pm = &acpi_button_pm,
 };
 
 struct acpi_button {
@@ -308,8 +310,9 @@ static void acpi_button_notify(struct acpi_device *device, u32 event)
 	}
 }
 
-static int acpi_button_resume(struct acpi_device *device)
+static int acpi_button_resume(struct device *dev)
 {
+	struct acpi_device *device = to_acpi_device(dev);
 	struct acpi_button *button = acpi_driver_data(device);
 
 	if (button->type == ACPI_BUTTON_TYPE_LID)
diff --git a/drivers/acpi/container.c b/drivers/acpi/container.c
index 45cd03b4630..1f9f7d7d7bc 100644
--- a/drivers/acpi/container.c
+++ b/drivers/acpi/container.c
@@ -158,9 +158,7 @@ static void container_notify_cb(acpi_handle handle, u32 type, void *context)
 	int result;
 	int present;
 	acpi_status status;
-
-
-	present = is_device_present(handle);
+	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
 
 	switch (type) {
 	case ACPI_NOTIFY_BUS_CHECK:
@@ -169,32 +167,47 @@ static void container_notify_cb(acpi_handle handle, u32 type, void *context)
 		printk(KERN_WARNING "Container driver received %s event\n",
 		       (type == ACPI_NOTIFY_BUS_CHECK) ?
 		       "ACPI_NOTIFY_BUS_CHECK" : "ACPI_NOTIFY_DEVICE_CHECK");
+
+		present = is_device_present(handle);
 		status = acpi_bus_get_device(handle, &device);
-		if (present) {
-			if (ACPI_FAILURE(status) || !device) {
-				result = container_device_add(&device, handle);
-				if (!result)
-					kobject_uevent(&device->dev.kobj,
-						       KOBJ_ONLINE);
-				else
-					printk(KERN_WARNING
-					       "Failed to add container\n");
-			}
-		} else {
+		if (!present) {
 			if (ACPI_SUCCESS(status)) {
 				/* device exist and this is a remove request */
+				device->flags.eject_pending = 1;
 				kobject_uevent(&device->dev.kobj, KOBJ_OFFLINE);
+				return;
 			}
+			break;
+		}
+
+		if (!ACPI_FAILURE(status) || device)
+			break;
+
+		result = container_device_add(&device, handle);
+		if (result) {
+			printk(KERN_WARNING "Failed to add container\n");
+			break;
 		}
+
+		kobject_uevent(&device->dev.kobj, KOBJ_ONLINE);
+		ost_code = ACPI_OST_SC_SUCCESS;
 		break;
+
 	case ACPI_NOTIFY_EJECT_REQUEST:
 		if (!acpi_bus_get_device(handle, &device) && device) {
+			device->flags.eject_pending = 1;
 			kobject_uevent(&device->dev.kobj, KOBJ_OFFLINE);
+			return;
 		}
 		break;
+
 	default:
-		break;
+		/* non-hotplug event; possibly handled by other handler */
+		return;
 	}
+
+	/* Inform firmware that the hotplug operation has completed */
+	(void) acpi_evaluate_hotplug_ost(handle, type, ost_code, NULL);
 	return;
 }
 
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index 0f0356ca1a9..669d9ee80d1 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -46,8 +46,6 @@ MODULE_LICENSE("GPL");
 
 static int acpi_fan_add(struct acpi_device *device);
 static int acpi_fan_remove(struct acpi_device *device, int type);
-static int acpi_fan_suspend(struct acpi_device *device, pm_message_t state);
-static int acpi_fan_resume(struct acpi_device *device);
 
 static const struct acpi_device_id fan_device_ids[] = {
 	{"PNP0C0B", 0},
@@ -55,6 +53,10 @@ static const struct acpi_device_id fan_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, fan_device_ids);
 
+static int acpi_fan_suspend(struct device *dev);
+static int acpi_fan_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(acpi_fan_pm, acpi_fan_suspend, acpi_fan_resume);
+
 static struct acpi_driver acpi_fan_driver = {
 	.name = "fan",
 	.class = ACPI_FAN_CLASS,
@@ -62,9 +64,8 @@ static struct acpi_driver acpi_fan_driver = {
 	.ops = {
 		.add = acpi_fan_add,
 		.remove = acpi_fan_remove,
-		.suspend = acpi_fan_suspend,
-		.resume = acpi_fan_resume,
 		},
+	.drv.pm = &acpi_fan_pm,
 };
 
 /* thermal cooling device callbacks */
@@ -183,24 +184,24 @@ static int acpi_fan_remove(struct acpi_device *device, int type)
 	return 0;
 }
 
-static int acpi_fan_suspend(struct acpi_device *device, pm_message_t state)
+static int acpi_fan_suspend(struct device *dev)
 {
-	if (!device)
+	if (!dev)
 		return -EINVAL;
 
-	acpi_bus_set_power(device->handle, ACPI_STATE_D0);
+	acpi_bus_set_power(to_acpi_device(dev)->handle, ACPI_STATE_D0);
 
 	return AE_OK;
 }
 
-static int acpi_fan_resume(struct acpi_device *device)
+static int acpi_fan_resume(struct device *dev)
 {
 	int result;
 
-	if (!device)
+	if (!dev)
 		return -EINVAL;
 
-	result = acpi_bus_update_power(device->handle, NULL);
+	result = acpi_bus_update_power(to_acpi_device(dev)->handle, NULL);
 	if (result)
 		printk(KERN_ERR PREFIX "Error updating fan power state\n");
 
diff --git a/drivers/acpi/glue.c b/drivers/acpi/glue.c
index 1564e0927c2..243ee85e4d2 100644
--- a/drivers/acpi/glue.c
+++ b/drivers/acpi/glue.c
@@ -39,6 +39,7 @@ int register_acpi_bus_type(struct acpi_bus_type *type)
 	}
 	return -ENODEV;
 }
+EXPORT_SYMBOL_GPL(register_acpi_bus_type);
 
 int unregister_acpi_bus_type(struct acpi_bus_type *type)
 {
@@ -54,6 +55,7 @@ int unregister_acpi_bus_type(struct acpi_bus_type *type)
 	}
 	return -ENODEV;
 }
+EXPORT_SYMBOL_GPL(unregister_acpi_bus_type);
 
 static struct acpi_bus_type *acpi_get_bus_type(struct bus_type *type)
 {
@@ -69,7 +71,6 @@ static struct acpi_bus_type *acpi_get_bus_type(struct bus_type *type)
 	up_read(&bus_type_sem);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(register_acpi_bus_type);
 
 static int acpi_find_bridge_device(struct device *dev, acpi_handle * handle)
 {
@@ -86,7 +87,6 @@ static int acpi_find_bridge_device(struct device *dev, acpi_handle * handle)
 	up_read(&bus_type_sem);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(unregister_acpi_bus_type);
 
 /* Get device's handler per its address under its parent */
 struct acpi_find_child {
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index c3881b2eb8b..9eaf708f588 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -891,7 +891,7 @@ static void acpi_os_execute_deferred(struct work_struct *work)
 	struct acpi_os_dpc *dpc = container_of(work, struct acpi_os_dpc, work);
 
 	if (dpc->wait)
-		acpi_os_wait_events_complete(NULL);
+		acpi_os_wait_events_complete();
 
 	dpc->function(dpc->context);
 	kfree(dpc);
@@ -987,7 +987,7 @@ acpi_status acpi_os_hotplug_execute(acpi_osd_exec_callback function,
 	return __acpi_os_execute(0, function, context, 1);
 }
 
-void acpi_os_wait_events_complete(void *context)
+void acpi_os_wait_events_complete(void)
 {
 	flush_workqueue(kacpid_wq);
 	flush_workqueue(kacpi_notify_wq);
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 7aff6312ce7..ec54014c321 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -505,6 +505,8 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 	strcpy(acpi_device_class(device), ACPI_PCI_ROOT_CLASS);
 	device->driver_data = root;
 
+	root->mcfg_addr = acpi_pci_root_get_mcfg_addr(device->handle);
+
 	/*
 	 * All supported architectures that use ACPI have support for
 	 * PCI domains, so we indicate this in _OSC support capabilities.
diff --git a/drivers/acpi/power.c b/drivers/acpi/power.c
index dd6d6a3c678..215ecd09740 100644
--- a/drivers/acpi/power.c
+++ b/drivers/acpi/power.c
@@ -60,7 +60,6 @@ ACPI_MODULE_NAME("power");
 
 static int acpi_power_add(struct acpi_device *device);
 static int acpi_power_remove(struct acpi_device *device, int type);
-static int acpi_power_resume(struct acpi_device *device);
 
 static const struct acpi_device_id power_device_ids[] = {
 	{ACPI_POWER_HID, 0},
@@ -68,6 +67,9 @@ static const struct acpi_device_id power_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, power_device_ids);
 
+static int acpi_power_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(acpi_power_pm, NULL, acpi_power_resume);
+
 static struct acpi_driver acpi_power_driver = {
 	.name = "power",
 	.class = ACPI_POWER_CLASS,
@@ -75,8 +77,8 @@ static struct acpi_driver acpi_power_driver = {
 	.ops = {
 		.add = acpi_power_add,
 		.remove = acpi_power_remove,
-		.resume = acpi_power_resume,
 		},
+	.drv.pm = &acpi_power_pm,
 };
 
 /*
@@ -390,6 +392,7 @@ void acpi_power_resource_unregister_device(struct device *dev, acpi_handle handl
 		__acpi_power_resource_unregister_device(dev,
 			list->handles[i]);
 }
+EXPORT_SYMBOL_GPL(acpi_power_resource_unregister_device);
 
 static int __acpi_power_resource_register_device(
 	struct acpi_power_managed_device *powered_device, acpi_handle handle)
@@ -460,6 +463,7 @@ no_power_resource:
 	printk(KERN_WARNING PREFIX "Invalid Power Resource to register!");
 	return -ENODEV;
 }
+EXPORT_SYMBOL_GPL(acpi_power_resource_register_device);
 
 /**
  * acpi_device_sleep_wake - execute _DSW (Device Sleep Wake) or (deprecated in
@@ -771,14 +775,16 @@ static int acpi_power_remove(struct acpi_device *device, int type)
 	return 0;
 }
 
-static int acpi_power_resume(struct acpi_device *device)
+static int acpi_power_resume(struct device *dev)
 {
 	int result = 0, state;
+	struct acpi_device *device;
 	struct acpi_power_resource *resource;
 
-	if (!device)
+	if (!dev)
 		return -EINVAL;
 
+	device = to_acpi_device(dev);
 	resource = acpi_driver_data(device);
 	if (!resource)
 		return -EINVAL;
diff --git a/drivers/acpi/processor_driver.c b/drivers/acpi/processor_driver.c
index 0734086537b..ff8e04f2fab 100644
--- a/drivers/acpi/processor_driver.c
+++ b/drivers/acpi/processor_driver.c
@@ -93,6 +93,9 @@ static const struct acpi_device_id processor_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, processor_device_ids);
 
+static SIMPLE_DEV_PM_OPS(acpi_processor_pm,
+			 acpi_processor_suspend, acpi_processor_resume);
+
 static struct acpi_driver acpi_processor_driver = {
 	.name = "processor",
 	.class = ACPI_PROCESSOR_CLASS,
@@ -100,10 +103,9 @@ static struct acpi_driver acpi_processor_driver = {
 	.ops = {
 		.add = acpi_processor_add,
 		.remove = acpi_processor_remove,
-		.suspend = acpi_processor_suspend,
-		.resume = acpi_processor_resume,
 		.notify = acpi_processor_notify,
 		},
+	.drv.pm = &acpi_processor_pm,
 };
 
 #define INSTALL_NOTIFY_HANDLER		1
@@ -427,18 +429,11 @@ static int acpi_cpu_soft_notify(struct notifier_block *nfb,
 		 * Initialize missing things
 		 */
 		if (pr->flags.need_hotplug_init) {
-			struct cpuidle_driver *idle_driver =
-				cpuidle_get_driver();
-
 			printk(KERN_INFO "Will online and init hotplugged "
 			       "CPU: %d\n", pr->id);
 			WARN(acpi_processor_start(pr), "Failed to start CPU:"
 				" %d\n", pr->id);
 			pr->flags.need_hotplug_init = 0;
-			if (idle_driver && !strcmp(idle_driver->name,
-						   "intel_idle")) {
-				intel_idle_cpu_init(pr->id);
-			}
 		/* Normal CPU soft online event */
 		} else {
 			acpi_processor_ppc_has_changed(pr, 0);
@@ -701,9 +696,9 @@ static void acpi_processor_hotplug_notify(acpi_handle handle,
 {
 	struct acpi_processor *pr;
 	struct acpi_device *device = NULL;
+	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
 	int result;
 
-
 	switch (event) {
 	case ACPI_NOTIFY_BUS_CHECK:
 	case ACPI_NOTIFY_DEVICE_CHECK:
@@ -715,14 +710,18 @@ static void acpi_processor_hotplug_notify(acpi_handle handle,
 		if (!is_processor_present(handle))
 			break;
 
-		if (acpi_bus_get_device(handle, &device)) {
-			result = acpi_processor_device_add(handle, &device);
-			if (result)
-				printk(KERN_ERR PREFIX
-					    "Unable to add the device\n");
+		if (!acpi_bus_get_device(handle, &device))
+			break;
+
+		result = acpi_processor_device_add(handle, &device);
+		if (result) {
+			printk(KERN_ERR PREFIX "Unable to add the device\n");
 			break;
 		}
+
+		ost_code = ACPI_OST_SC_SUCCESS;
 		break;
+
 	case ACPI_NOTIFY_EJECT_REQUEST:
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 				  "received ACPI_NOTIFY_EJECT_REQUEST\n"));
@@ -736,15 +735,23 @@ static void acpi_processor_hotplug_notify(acpi_handle handle,
 		if (!pr) {
 			printk(KERN_ERR PREFIX
 				    "Driver data is NULL, dropping EJECT\n");
-			return;
+			break;
 		}
+
+		/* REVISIT: update when eject is supported */
+		ost_code = ACPI_OST_SC_EJECT_NOT_SUPPORTED;
 		break;
+
 	default:
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 				  "Unsupported event [0x%x]\n", event));
-		break;
+
+		/* non-hotplug event; possibly handled by other handler */
+		return;
 	}
 
+	/* Inform firmware that the hotplug operation has completed */
+	(void) acpi_evaluate_hotplug_ost(handle, event, ost_code, NULL);
 	return;
 }
 
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 47a8caa89db..ad3730b4038 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -221,10 +221,6 @@ static void lapic_timer_state_broadcast(struct acpi_processor *pr,
 
 #endif
 
-/*
- * Suspend / resume control
- */
-static int acpi_idle_suspend;
 static u32 saved_bm_rld;
 
 static void acpi_idle_bm_rld_save(void)
@@ -241,23 +237,15 @@ static void acpi_idle_bm_rld_restore(void)
 		acpi_write_bit_register(ACPI_BITREG_BUS_MASTER_RLD, saved_bm_rld);
 }
 
-int acpi_processor_suspend(struct acpi_device * device, pm_message_t state)
+int acpi_processor_suspend(struct device *dev)
 {
-	if (acpi_idle_suspend == 1)
-		return 0;
-
 	acpi_idle_bm_rld_save();
-	acpi_idle_suspend = 1;
 	return 0;
 }
 
-int acpi_processor_resume(struct acpi_device * device)
+int acpi_processor_resume(struct device *dev)
 {
-	if (acpi_idle_suspend == 0)
-		return 0;
-
 	acpi_idle_bm_rld_restore();
-	acpi_idle_suspend = 0;
 	return 0;
 }
 
@@ -313,16 +301,16 @@ static int acpi_processor_get_power_info_fadt(struct acpi_processor *pr)
 	pr->power.states[ACPI_STATE_C3].address = pr->pblk + 5;
 
 	/* determine latencies from FADT */
-	pr->power.states[ACPI_STATE_C2].latency = acpi_gbl_FADT.C2latency;
-	pr->power.states[ACPI_STATE_C3].latency = acpi_gbl_FADT.C3latency;
+	pr->power.states[ACPI_STATE_C2].latency = acpi_gbl_FADT.c2_latency;
+	pr->power.states[ACPI_STATE_C3].latency = acpi_gbl_FADT.c3_latency;
 
 	/*
 	 * FADT specified C2 latency must be less than or equal to
 	 * 100 microseconds.
 	 */
-	if (acpi_gbl_FADT.C2latency > ACPI_PROCESSOR_MAX_C2_LATENCY) {
+	if (acpi_gbl_FADT.c2_latency > ACPI_PROCESSOR_MAX_C2_LATENCY) {
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-			"C2 latency too large [%d]\n", acpi_gbl_FADT.C2latency));
+			"C2 latency too large [%d]\n", acpi_gbl_FADT.c2_latency));
 		/* invalidate C2 */
 		pr->power.states[ACPI_STATE_C2].address = 0;
 	}
@@ -331,9 +319,9 @@ static int acpi_processor_get_power_info_fadt(struct acpi_processor *pr)
 	 * FADT supplied C3 latency must be less than or equal to
 	 * 1000 microseconds.
 	 */
-	if (acpi_gbl_FADT.C3latency > ACPI_PROCESSOR_MAX_C3_LATENCY) {
+	if (acpi_gbl_FADT.c3_latency > ACPI_PROCESSOR_MAX_C3_LATENCY) {
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-			"C3 latency too large [%d]\n", acpi_gbl_FADT.C3latency));
+			"C3 latency too large [%d]\n", acpi_gbl_FADT.c3_latency));
 		/* invalidate C3 */
 		pr->power.states[ACPI_STATE_C3].address = 0;
 	}
@@ -595,7 +583,6 @@ static void acpi_processor_power_verify_c3(struct acpi_processor *pr,
 	 */
 	cx->valid = 1;
 
-	cx->latency_ticks = cx->latency;
 	/*
 	 * On older chipsets, BM_RLD needs to be set
 	 * in order for Bus Master activity to wake the
@@ -628,7 +615,6 @@ static int acpi_processor_power_verify(struct acpi_processor *pr)
 			if (!cx->address)
 				break;
 			cx->valid = 1; 
-			cx->latency_ticks = cx->latency; /* Normalize latency */
 			break;
 
 		case ACPI_STATE_C3:
@@ -763,11 +749,6 @@ static int acpi_idle_enter_c1(struct cpuidle_device *dev,
 
 	local_irq_disable();
 
-	if (acpi_idle_suspend) {
-		local_irq_enable();
-		cpu_relax();
-		return -EBUSY;
-	}
 
 	lapic_timer_state_broadcast(pr, cx, 1);
 	kt1 = ktime_get_real();
@@ -779,7 +760,6 @@ static int acpi_idle_enter_c1(struct cpuidle_device *dev,
 	dev->last_residency = (int)idle_time;
 
 	local_irq_enable();
-	cx->usage++;
 	lapic_timer_state_broadcast(pr, cx, 0);
 
 	return index;
@@ -838,11 +818,6 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
 
 	local_irq_disable();
 
-	if (acpi_idle_suspend) {
-		local_irq_enable();
-		cpu_relax();
-		return -EBUSY;
-	}
 
 	if (cx->entry_method != ACPI_CSTATE_FFH) {
 		current_thread_info()->status &= ~TS_POLLING;
@@ -887,10 +862,7 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
 	if (cx->entry_method != ACPI_CSTATE_FFH)
 		current_thread_info()->status |= TS_POLLING;
 
-	cx->usage++;
-
 	lapic_timer_state_broadcast(pr, cx, 0);
-	cx->time += idle_time;
 	return index;
 }
 
@@ -928,8 +900,7 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
 						drv, drv->safe_state_index);
 		} else {
 			local_irq_disable();
-			if (!acpi_idle_suspend)
-				acpi_safe_halt();
+			acpi_safe_halt();
 			local_irq_enable();
 			return -EBUSY;
 		}
@@ -937,11 +908,6 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
 
 	local_irq_disable();
 
-	if (acpi_idle_suspend) {
-		local_irq_enable();
-		cpu_relax();
-		return -EBUSY;
-	}
 
 	if (cx->entry_method != ACPI_CSTATE_FFH) {
 		current_thread_info()->status &= ~TS_POLLING;
@@ -1014,10 +980,7 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
 	if (cx->entry_method != ACPI_CSTATE_FFH)
 		current_thread_info()->status |= TS_POLLING;
 
-	cx->usage++;
-
 	lapic_timer_state_broadcast(pr, cx, 0);
-	cx->time += idle_time;
 	return index;
 }
 
diff --git a/drivers/acpi/sbs.c b/drivers/acpi/sbs.c
index 6e36d0c0057..c0b9aa5faf4 100644
--- a/drivers/acpi/sbs.c
+++ b/drivers/acpi/sbs.c
@@ -988,16 +988,18 @@ static void acpi_sbs_rmdirs(void)
 #endif
 }
 
-static int acpi_sbs_resume(struct acpi_device *device)
+static int acpi_sbs_resume(struct device *dev)
 {
 	struct acpi_sbs *sbs;
-	if (!device)
+	if (!dev)
 		return -EINVAL;
-	sbs = device->driver_data;
+	sbs = to_acpi_device(dev)->driver_data;
 	acpi_sbs_callback(sbs);
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(acpi_sbs_pm, NULL, acpi_sbs_resume);
+
 static struct acpi_driver acpi_sbs_driver = {
 	.name = "sbs",
 	.class = ACPI_SBS_CLASS,
@@ -1005,8 +1007,8 @@ static struct acpi_driver acpi_sbs_driver = {
 	.ops = {
 		.add = acpi_sbs_add,
 		.remove = acpi_sbs_remove,
-		.resume = acpi_sbs_resume,
 		},
+	.drv.pm = &acpi_sbs_pm,
 };
 
 static int __init acpi_sbs_init(void)
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index c8a1f3b6811..d1ecca2b641 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -83,19 +83,29 @@ acpi_device_modalias_show(struct device *dev, struct device_attribute *attr, cha
 }
 static DEVICE_ATTR(modalias, 0444, acpi_device_modalias_show, NULL);
 
-static void acpi_bus_hot_remove_device(void *context)
+/**
+ * acpi_bus_hot_remove_device: hot-remove a device and its children
+ * @context: struct acpi_eject_event pointer (freed in this func)
+ *
+ * Hot-remove a device and its children. This function frees up the
+ * memory space passed by arg context, so that the caller may call
+ * this function asynchronously through acpi_os_hotplug_execute().
+ */
+void acpi_bus_hot_remove_device(void *context)
 {
+	struct acpi_eject_event *ej_event = (struct acpi_eject_event *) context;
 	struct acpi_device *device;
-	acpi_handle handle = context;
+	acpi_handle handle = ej_event->handle;
 	struct acpi_object_list arg_list;
 	union acpi_object arg;
 	acpi_status status = AE_OK;
+	u32 ost_code = ACPI_OST_SC_NON_SPECIFIC_FAILURE; /* default */
 
 	if (acpi_bus_get_device(handle, &device))
-		return;
+		goto err_out;
 
 	if (!device)
-		return;
+		goto err_out;
 
 	ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 		"Hot-removing device %s...\n", dev_name(&device->dev)));
@@ -103,7 +113,7 @@ static void acpi_bus_hot_remove_device(void *context)
 	if (acpi_bus_trim(device, 1)) {
 		printk(KERN_ERR PREFIX
 				"Removing device failed\n");
-		return;
+		goto err_out;
 	}
 
 	/* power off device */
@@ -129,10 +139,21 @@ static void acpi_bus_hot_remove_device(void *context)
 	 * TBD: _EJD support.
 	 */
 	status = acpi_evaluate_object(handle, "_EJ0", &arg_list, NULL);
-	if (ACPI_FAILURE(status))
-		printk(KERN_WARNING PREFIX
-				"Eject device failed\n");
+	if (ACPI_FAILURE(status)) {
+		if (status != AE_NOT_FOUND)
+			printk(KERN_WARNING PREFIX
+					"Eject device failed\n");
+		goto err_out;
+	}
+
+	kfree(context);
+	return;
 
+err_out:
+	/* Inform firmware the hot-remove operation has completed w/ error */
+	(void) acpi_evaluate_hotplug_ost(handle,
+				ej_event->event, ost_code, NULL);
+	kfree(context);
 	return;
 }
 
@@ -144,6 +165,7 @@ acpi_eject_store(struct device *d, struct device_attribute *attr,
 	acpi_status status;
 	acpi_object_type type = 0;
 	struct acpi_device *acpi_device = to_acpi_device(d);
+	struct acpi_eject_event *ej_event;
 
 	if ((!count) || (buf[0] != '1')) {
 		return -EINVAL;
@@ -160,7 +182,25 @@ acpi_eject_store(struct device *d, struct device_attribute *attr,
 		goto err;
 	}
 
-	acpi_os_hotplug_execute(acpi_bus_hot_remove_device, acpi_device->handle);
+	ej_event = kmalloc(sizeof(*ej_event), GFP_KERNEL);
+	if (!ej_event) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	ej_event->handle = acpi_device->handle;
+	if (acpi_device->flags.eject_pending) {
+		/* event originated from ACPI eject notification */
+		ej_event->event = ACPI_NOTIFY_EJECT_REQUEST;
+		acpi_device->flags.eject_pending = 0;
+	} else {
+		/* event originated from user */
+		ej_event->event = ACPI_OST_EC_OSPM_EJECT;
+		(void) acpi_evaluate_hotplug_ost(ej_event->handle,
+			ej_event->event, ACPI_OST_SC_EJECT_IN_PROGRESS, NULL);
+	}
+
+	acpi_os_hotplug_execute(acpi_bus_hot_remove_device, (void *)ej_event);
 err:
 	return ret;
 }
@@ -290,26 +330,6 @@ static void acpi_device_release(struct device *dev)
 	kfree(acpi_dev);
 }
 
-static int acpi_device_suspend(struct device *dev, pm_message_t state)
-{
-	struct acpi_device *acpi_dev = to_acpi_device(dev);
-	struct acpi_driver *acpi_drv = acpi_dev->driver;
-
-	if (acpi_drv && acpi_drv->ops.suspend)
-		return acpi_drv->ops.suspend(acpi_dev, state);
-	return 0;
-}
-
-static int acpi_device_resume(struct device *dev)
-{
-	struct acpi_device *acpi_dev = to_acpi_device(dev);
-	struct acpi_driver *acpi_drv = acpi_dev->driver;
-
-	if (acpi_drv && acpi_drv->ops.resume)
-		return acpi_drv->ops.resume(acpi_dev);
-	return 0;
-}
-
 static int acpi_bus_match(struct device *dev, struct device_driver *drv)
 {
 	struct acpi_device *acpi_dev = to_acpi_device(dev);
@@ -441,8 +461,6 @@ static int acpi_device_remove(struct device * dev)
 
 struct bus_type acpi_bus_type = {
 	.name		= "acpi",
-	.suspend	= acpi_device_suspend,
-	.resume		= acpi_device_resume,
 	.match		= acpi_bus_match,
 	.probe		= acpi_device_probe,
 	.remove		= acpi_device_remove,
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 88561029cca..7a7a9c92924 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -143,7 +143,7 @@ void __init acpi_old_suspend_ordering(void)
 static int acpi_pm_freeze(void)
 {
 	acpi_disable_all_gpes();
-	acpi_os_wait_events_complete(NULL);
+	acpi_os_wait_events_complete();
 	acpi_ec_block_transactions();
 	return 0;
 }
@@ -716,8 +716,9 @@ int acpi_suspend(u32 acpi_state)
  *	@dev: device to examine; its driver model wakeup flags control
  *		whether it should be able to wake up the system
  *	@d_min_p: used to store the upper limit of allowed states range
- *	Return value: preferred power state of the device on success, -ENODEV on
- *		failure (ie. if there's no 'struct acpi_device' for @dev)
+ *	@d_max_in: specify the lowest allowed states
+ *	Return value: preferred power state of the device on success, -ENODEV
+ *	(ie. if there's no 'struct acpi_device' for @dev) or -EINVAL on failure
  *
  *	Find the lowest power (highest number) ACPI device power state that
  *	device @dev can be in while the system is in the sleep state represented
@@ -732,13 +733,15 @@ int acpi_suspend(u32 acpi_state)
  *	via @wake.
  */
 
-int acpi_pm_device_sleep_state(struct device *dev, int *d_min_p)
+int acpi_pm_device_sleep_state(struct device *dev, int *d_min_p, int d_max_in)
 {
 	acpi_handle handle = DEVICE_ACPI_HANDLE(dev);
 	struct acpi_device *adev;
 	char acpi_method[] = "_SxD";
 	unsigned long long d_min, d_max;
 
+	if (d_max_in < ACPI_STATE_D0 || d_max_in > ACPI_STATE_D3)
+		return -EINVAL;
 	if (!handle || ACPI_FAILURE(acpi_bus_get_device(handle, &adev))) {
 		printk(KERN_DEBUG "ACPI handle has no context!\n");
 		return -ENODEV;
@@ -746,8 +749,10 @@ int acpi_pm_device_sleep_state(struct device *dev, int *d_min_p)
 
 	acpi_method[2] = '0' + acpi_target_sleep_state;
 	/*
-	 * If the sleep state is S0, we will return D3, but if the device has
-	 * _S0W, we will use the value from _S0W
+	 * If the sleep state is S0, the lowest limit from ACPI is D3,
+	 * but if the device has _S0W, we will use the value from _S0W
+	 * as the lowest limit from ACPI.  Finally, we will constrain
+	 * the lowest limit with the specified one.
 	 */
 	d_min = ACPI_STATE_D0;
 	d_max = ACPI_STATE_D3;
@@ -791,10 +796,20 @@ int acpi_pm_device_sleep_state(struct device *dev, int *d_min_p)
 		}
 	}
 
+	if (d_max_in < d_min)
+		return -EINVAL;
 	if (d_min_p)
 		*d_min_p = d_min;
+	/* constrain d_max with specified lowest limit (max number) */
+	if (d_max > d_max_in) {
+		for (d_max = d_max_in; d_max > d_min; d_max--) {
+			if (adev->power.states[d_max].flags.valid)
+				break;
+		}
+	}
 	return d_max;
 }
+EXPORT_SYMBOL(acpi_pm_device_sleep_state);
 #endif /* CONFIG_PM */
 
 #ifdef CONFIG_PM_SLEEP
@@ -831,6 +846,7 @@ int acpi_pm_device_run_wake(struct device *phys_dev, bool enable)
 
 	return 0;
 }
+EXPORT_SYMBOL(acpi_pm_device_run_wake);
 
 /**
  *	acpi_pm_device_sleep_wake - enable or disable the system wake-up
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index 7dbebea1ec3..9fe90e9fecb 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -98,7 +98,6 @@ MODULE_PARM_DESC(psv, "Disable or override all passive trip points.");
 
 static int acpi_thermal_add(struct acpi_device *device);
 static int acpi_thermal_remove(struct acpi_device *device, int type);
-static int acpi_thermal_resume(struct acpi_device *device);
 static void acpi_thermal_notify(struct acpi_device *device, u32 event);
 
 static const struct acpi_device_id  thermal_device_ids[] = {
@@ -107,6 +106,9 @@ static const struct acpi_device_id  thermal_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, thermal_device_ids);
 
+static int acpi_thermal_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(acpi_thermal_pm, NULL, acpi_thermal_resume);
+
 static struct acpi_driver acpi_thermal_driver = {
 	.name = "thermal",
 	.class = ACPI_THERMAL_CLASS,
@@ -114,9 +116,9 @@ static struct acpi_driver acpi_thermal_driver = {
 	.ops = {
 		.add = acpi_thermal_add,
 		.remove = acpi_thermal_remove,
-		.resume = acpi_thermal_resume,
 		.notify = acpi_thermal_notify,
 		},
+	.drv.pm = &acpi_thermal_pm,
 };
 
 struct acpi_thermal_state {
@@ -550,8 +552,6 @@ static int thermal_get_temp(struct thermal_zone_device *thermal,
 	return 0;
 }
 
-static const char enabled[] = "kernel";
-static const char disabled[] = "user";
 static int thermal_get_mode(struct thermal_zone_device *thermal,
 				enum thermal_device_mode *mode)
 {
@@ -588,8 +588,8 @@ static int thermal_set_mode(struct thermal_zone_device *thermal,
 	if (enable != tz->tz_enabled) {
 		tz->tz_enabled = enable;
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-			"%s ACPI thermal control\n",
-			tz->tz_enabled ? enabled : disabled));
+			"%s kernel ACPI thermal control\n",
+			tz->tz_enabled ? "Enable" : "Disable"));
 		acpi_thermal_check(tz);
 	}
 	return 0;
@@ -845,7 +845,7 @@ static int acpi_thermal_register_thermal_zone(struct acpi_thermal *tz)
 
 	if (tz->trips.passive.flags.valid)
 		tz->thermal_zone =
-			thermal_zone_device_register("acpitz", trips, tz,
+			thermal_zone_device_register("acpitz", trips, 0, tz,
 						     &acpi_thermal_zone_ops,
 						     tz->trips.passive.tc1,
 						     tz->trips.passive.tc2,
@@ -853,7 +853,7 @@ static int acpi_thermal_register_thermal_zone(struct acpi_thermal *tz)
 						     tz->polling_frequency*100);
 	else
 		tz->thermal_zone =
-			thermal_zone_device_register("acpitz", trips, tz,
+			thermal_zone_device_register("acpitz", trips, 0, tz,
 						     &acpi_thermal_zone_ops,
 						     0, 0, 0,
 						     tz->polling_frequency*100);
@@ -1041,16 +1041,17 @@ static int acpi_thermal_remove(struct acpi_device *device, int type)
 	return 0;
 }
 
-static int acpi_thermal_resume(struct acpi_device *device)
+static int acpi_thermal_resume(struct device *dev)
 {
-	struct acpi_thermal *tz = NULL;
+	struct acpi_thermal *tz;
 	int i, j, power_state, result;
 
-
-	if (!device || !acpi_driver_data(device))
+	if (!dev)
 		return -EINVAL;
 
-	tz = acpi_driver_data(device);
+	tz = acpi_driver_data(to_acpi_device(dev));
+	if (!tz)
+		return -EINVAL;
 
 	for (i = 0; i < ACPI_THERMAL_MAX_ACTIVE; i++) {
 		if (!(&tz->trips.active[i]))
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index adbbc1c80a2..3e87c9c538a 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -412,3 +412,45 @@ out:
 	return status;
 }
 EXPORT_SYMBOL(acpi_get_physical_device_location);
+
+/**
+ * acpi_evaluate_hotplug_ost: Evaluate _OST for hotplug operations
+ * @handle: ACPI device handle
+ * @source_event: source event code
+ * @status_code: status code
+ * @status_buf: optional detailed information (NULL if none)
+ *
+ * Evaluate _OST for hotplug operations. All ACPI hotplug handlers
+ * must call this function when evaluating _OST for hotplug operations.
+ * When the platform does not support _OST, this function has no effect.
+ */
+acpi_status
+acpi_evaluate_hotplug_ost(acpi_handle handle, u32 source_event,
+		u32 status_code, struct acpi_buffer *status_buf)
+{
+#ifdef ACPI_HOTPLUG_OST
+	union acpi_object params[3] = {
+		{.type = ACPI_TYPE_INTEGER,},
+		{.type = ACPI_TYPE_INTEGER,},
+		{.type = ACPI_TYPE_BUFFER,}
+	};
+	struct acpi_object_list arg_list = {3, params};
+	acpi_status status;
+
+	params[0].integer.value = source_event;
+	params[1].integer.value = status_code;
+	if (status_buf != NULL) {
+		params[2].buffer.pointer = status_buf->pointer;
+		params[2].buffer.length = status_buf->length;
+	} else {
+		params[2].buffer.pointer = NULL;
+		params[2].buffer.length = 0;
+	}
+
+	status = acpi_evaluate_object(handle, "_OST", &arg_list, NULL);
+	return status;
+#else
+	return AE_OK;
+#endif
+}
+EXPORT_SYMBOL(acpi_evaluate_hotplug_ost);
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index 45d8097ef4c..b728880ef10 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -132,6 +132,33 @@ find_video(acpi_handle handle, u32 lvl, void *context, void **rv)
 	return AE_OK;
 }
 
+/* Force to use vendor driver when the ACPI device is known to be
+ * buggy */
+static int video_detect_force_vendor(const struct dmi_system_id *d)
+{
+	acpi_video_support |= ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
+	return 0;
+}
+
+static struct dmi_system_id video_detect_dmi_table[] = {
+	/* On Samsung X360, the BIOS will set a flag (VDRV) if generic
+	 * ACPI backlight device is used. This flag will definitively break
+	 * the backlight interface (even the vendor interface) untill next
+	 * reboot. It's why we should prevent video.ko from being used here
+	 * and we can't rely on a later call to acpi_video_unregister().
+	 */
+	{
+	 .callback = video_detect_force_vendor,
+	 .ident = "X360",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "X360"),
+		DMI_MATCH(DMI_BOARD_NAME, "X360"),
+		},
+	},
+	{ },
+};
+
 /*
  * Returns the video capabilities of a specific ACPI graphics device
  *
@@ -164,6 +191,8 @@ long acpi_video_get_capabilities(acpi_handle graphics_handle)
 		 *		ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
 		 *}
 		 */
+
+		dmi_check_system(video_detect_dmi_table);
 	} else {
 		status = acpi_bus_get_device(graphics_handle, &tmp_dev);
 		if (ACPI_FAILURE(status)) {
@@ -182,8 +211,7 @@ long acpi_video_get_capabilities(acpi_handle graphics_handle)
 }
 EXPORT_SYMBOL(acpi_video_get_capabilities);
 
-/* Returns true if video.ko can do backlight switching */
-int acpi_video_backlight_support(void)
+static void acpi_video_caps_check(void)
 {
 	/*
 	 * We must check whether the ACPI graphics device is physically plugged
@@ -191,6 +219,34 @@ int acpi_video_backlight_support(void)
 	 */
 	if (!acpi_video_caps_checked)
 		acpi_video_get_capabilities(NULL);
+}
+
+/* Promote the vendor interface instead of the generic video module.
+ * This function allow DMI blacklists to be implemented by externals
+ * platform drivers instead of putting a big blacklist in video_detect.c
+ * After calling this function you will probably want to call
+ * acpi_video_unregister() to make sure the video module is not loaded
+ */
+void acpi_video_dmi_promote_vendor(void)
+{
+	acpi_video_caps_check();
+	acpi_video_support |= ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
+}
+EXPORT_SYMBOL(acpi_video_dmi_promote_vendor);
+
+/* To be called when a driver who previously promoted the vendor
+ * interface */
+void acpi_video_dmi_demote_vendor(void)
+{
+	acpi_video_caps_check();
+	acpi_video_support &= ~ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
+}
+EXPORT_SYMBOL(acpi_video_dmi_demote_vendor);
+
+/* Returns true if video.ko can do backlight switching */
+int acpi_video_backlight_support(void)
+{
+	acpi_video_caps_check();
 
 	/* First check for boot param -> highest prio */
 	if (acpi_video_support & ACPI_VIDEO_BACKLIGHT_FORCE_VENDOR)
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index b7e72851728..e8eb91bd0d2 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -16,9 +16,9 @@
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
 #include <linux/amba/bus.h>
+#include <linux/sizes.h>
 
 #include <asm/irq.h>
-#include <asm/sizes.h>
 
 #define to_amba_driver(d)	container_of(d, struct amba_driver, drv)
 
diff --git a/drivers/amba/tegra-ahb.c b/drivers/amba/tegra-ahb.c
index aa0b1f16052..0b6f0b28a48 100644
--- a/drivers/amba/tegra-ahb.c
+++ b/drivers/amba/tegra-ahb.c
@@ -264,11 +264,6 @@ static int __devinit tegra_ahb_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __devexit tegra_ahb_remove(struct platform_device *pdev)
-{
-	return 0;
-}
-
 static const struct of_device_id tegra_ahb_of_match[] __devinitconst = {
 	{ .compatible = "nvidia,tegra30-ahb", },
 	{ .compatible = "nvidia,tegra20-ahb", },
@@ -277,7 +272,6 @@ static const struct of_device_id tegra_ahb_of_match[] __devinitconst = {
 
 static struct platform_driver tegra_ahb_driver = {
 	.probe = tegra_ahb_probe,
-	.remove = __devexit_p(tegra_ahb_remove),
 	.driver = {
 		.name = DRV_NAME,
 		.owner = THIS_MODULE,
diff --git a/drivers/ata/acard-ahci.c b/drivers/ata/acard-ahci.c
index 3bc8c79bf2c..4e94ba29cb8 100644
--- a/drivers/ata/acard-ahci.c
+++ b/drivers/ata/acard-ahci.c
@@ -503,21 +503,10 @@ static int acard_ahci_init_one(struct pci_dev *pdev, const struct pci_device_id
 				 &acard_ahci_sht);
 }
 
-static int __init acard_ahci_init(void)
-{
-	return pci_register_driver(&acard_ahci_pci_driver);
-}
-
-static void __exit acard_ahci_exit(void)
-{
-	pci_unregister_driver(&acard_ahci_pci_driver);
-}
+module_pci_driver(acard_ahci_pci_driver);
 
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("ACard AHCI SATA low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, acard_ahci_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(acard_ahci_init);
-module_exit(acard_ahci_exit);
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index ebaf67e4b2b..062e6a1a248 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -105,31 +105,27 @@ static struct ata_port_operations ahci_p5wdh_ops = {
 
 static const struct ata_port_info ahci_port_info[] = {
 	/* by features */
-	[board_ahci] =
-	{
+	[board_ahci] = {
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_ign_iferr] =
-	{
+	[board_ahci_ign_iferr] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_IGN_IRQ_IF_ERR),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_nosntf] =
-	{
+	[board_ahci_nosntf] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_SNTF),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_yes_fbs] =
-	{
+	[board_ahci_yes_fbs] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_YES_FBS),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
@@ -137,8 +133,7 @@ static const struct ata_port_info ahci_port_info[] = {
 		.port_ops	= &ahci_ops,
 	},
 	/* by chipsets */
-	[board_ahci_mcp65] =
-	{
+	[board_ahci_mcp65] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_FPDMA_AA | AHCI_HFLAG_NO_PMP |
 				 AHCI_HFLAG_YES_NCQ),
 		.flags		= AHCI_FLAG_COMMON | ATA_FLAG_NO_DIPM,
@@ -146,24 +141,21 @@ static const struct ata_port_info ahci_port_info[] = {
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_mcp77] =
-	{
+	[board_ahci_mcp77] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_FPDMA_AA | AHCI_HFLAG_NO_PMP),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_mcp89] =
-	{
+	[board_ahci_mcp89] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_FPDMA_AA),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_mv] =
-	{
+	[board_ahci_mv] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_NCQ | AHCI_HFLAG_NO_MSI |
 				 AHCI_HFLAG_MV_PATA | AHCI_HFLAG_NO_PMP),
 		.flags		= ATA_FLAG_SATA | ATA_FLAG_PIO_DMA,
@@ -171,8 +163,7 @@ static const struct ata_port_info ahci_port_info[] = {
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	[board_ahci_sb600] =
-	{
+	[board_ahci_sb600] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_IGN_SERR_INTERNAL |
 				 AHCI_HFLAG_NO_MSI | AHCI_HFLAG_SECT255 |
 				 AHCI_HFLAG_32BIT_ONLY),
@@ -181,16 +172,14 @@ static const struct ata_port_info ahci_port_info[] = {
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_pmp_retry_srst_ops,
 	},
-	[board_ahci_sb700] =	/* for SB700 and SB800 */
-	{
+	[board_ahci_sb700] = {	/* for SB700 and SB800 */
 		AHCI_HFLAGS	(AHCI_HFLAG_IGN_SERR_INTERNAL),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_pmp_retry_srst_ops,
 	},
-	[board_ahci_vt8251] =
-	{
+	[board_ahci_vt8251] = {
 		AHCI_HFLAGS	(AHCI_HFLAG_NO_NCQ | AHCI_HFLAG_NO_PMP),
 		.flags		= AHCI_FLAG_COMMON,
 		.pio_mask	= ATA_PIO4,
@@ -777,6 +766,22 @@ static bool ahci_sb600_enable_64bit(struct pci_dev *pdev)
 			},
 		},
 		/*
+		 * All BIOS versions for the MSI K9AGM2 (MS-7327) support
+		 * 64bit DMA.
+		 *
+		 * This board also had the typo mentioned above in the
+		 * Manufacturer DMI field (fixed in BIOS version 1.5), so
+		 * match on DMI_BOARD_VENDOR of "MICRO-STAR INTER" again.
+		 */
+		{
+			.ident = "MSI K9AGM2",
+			.matches = {
+				DMI_MATCH(DMI_BOARD_VENDOR,
+					  "MICRO-STAR INTER"),
+				DMI_MATCH(DMI_BOARD_NAME, "MS-7327"),
+			},
+		},
+		/*
 		 * All BIOS versions for the Asus M3A support 64bit DMA.
 		 * (all release versions from 0301 to 1206 were tested)
 		 */
@@ -1233,22 +1238,10 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 				 &ahci_sht);
 }
 
-static int __init ahci_init(void)
-{
-	return pci_register_driver(&ahci_pci_driver);
-}
-
-static void __exit ahci_exit(void)
-{
-	pci_unregister_driver(&ahci_pci_driver);
-}
-
+module_pci_driver(ahci_pci_driver);
 
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("AHCI SATA low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, ahci_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(ahci_init);
-module_exit(ahci_exit);
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index 9e419e1c200..09728e09cb3 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -15,6 +15,7 @@
 #include <linux/kernel.h>
 #include <linux/gfp.h>
 #include <linux/module.h>
+#include <linux/pm.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/device.h>
@@ -271,13 +272,10 @@ static int ahci_resume(struct device *dev)
 
 	return 0;
 }
-
-static struct dev_pm_ops ahci_pm_ops = {
-	.suspend		= &ahci_suspend,
-	.resume			= &ahci_resume,
-};
 #endif
 
+SIMPLE_DEV_PM_OPS(ahci_pm_ops, ahci_suspend, ahci_resume);
+
 static const struct of_device_id ahci_of_match[] = {
 	{ .compatible = "calxeda,hb-ahci", },
 	{ .compatible = "snps,spear-ahci", },
@@ -291,9 +289,7 @@ static struct platform_driver ahci_driver = {
 		.name = "ahci",
 		.owner = THIS_MODULE,
 		.of_match_table = ahci_of_match,
-#ifdef CONFIG_PM
 		.pm = &ahci_pm_ops,
-#endif
 	},
 	.id_table	= ahci_devtype,
 };
diff --git a/drivers/ata/ata_generic.c b/drivers/ata/ata_generic.c
index aae115600b7..f8f38a08abc 100644
--- a/drivers/ata/ata_generic.c
+++ b/drivers/ata/ata_generic.c
@@ -255,17 +255,7 @@ static struct pci_driver ata_generic_pci_driver = {
 #endif
 };
 
-static int __init ata_generic_init(void)
-{
-	return pci_register_driver(&ata_generic_pci_driver);
-}
-
-
-static void __exit ata_generic_exit(void)
-{
-	pci_unregister_driver(&ata_generic_pci_driver);
-}
-
+module_pci_driver(ata_generic_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for generic ATA");
@@ -273,7 +263,4 @@ MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, ata_generic);
 MODULE_VERSION(DRV_VERSION);
 
-module_init(ata_generic_init);
-module_exit(ata_generic_exit);
-
 module_param(all_generic_ide, int, 0);
diff --git a/drivers/ata/libata-acpi.c b/drivers/ata/libata-acpi.c
index bb7c5f1085c..902b5a45717 100644
--- a/drivers/ata/libata-acpi.c
+++ b/drivers/ata/libata-acpi.c
@@ -16,6 +16,7 @@
 #include <linux/libata.h>
 #include <linux/pci.h>
 #include <linux/slab.h>
+#include <linux/pm_runtime.h>
 #include <scsi/scsi_device.h>
 #include "libata.h"
 
@@ -48,62 +49,53 @@ static void ata_acpi_clear_gtf(struct ata_device *dev)
 }
 
 /**
- * ata_acpi_associate_sata_port - associate SATA port with ACPI objects
- * @ap: target SATA port
+ * ata_ap_acpi_handle - provide the acpi_handle for an ata_port
+ * @ap: the acpi_handle returned will correspond to this port
  *
- * Look up ACPI objects associated with @ap and initialize acpi_handle
- * fields of @ap, the port and devices accordingly.
- *
- * LOCKING:
- * EH context.
- *
- * RETURNS:
- * 0 on success, -errno on failure.
+ * Returns the acpi_handle for the ACPI namespace object corresponding to
+ * the ata_port passed into the function, or NULL if no such object exists
  */
-void ata_acpi_associate_sata_port(struct ata_port *ap)
+acpi_handle ata_ap_acpi_handle(struct ata_port *ap)
 {
-	WARN_ON(!(ap->flags & ATA_FLAG_ACPI_SATA));
-
-	if (!sata_pmp_attached(ap)) {
-		u64 adr = SATA_ADR(ap->port_no, NO_PORT_MULT);
-
-		ap->link.device->acpi_handle =
-			acpi_get_child(ap->host->acpi_handle, adr);
-	} else {
-		struct ata_link *link;
-
-		ap->link.device->acpi_handle = NULL;
-
-		ata_for_each_link(link, ap, EDGE) {
-			u64 adr = SATA_ADR(ap->port_no, link->pmp);
+	if (ap->flags & ATA_FLAG_ACPI_SATA)
+		return NULL;
 
-			link->device->acpi_handle =
-				acpi_get_child(ap->host->acpi_handle, adr);
-		}
-	}
+	/*
+	 * If acpi bind operation has already happened, we can get the handle
+	 * for the port by checking the corresponding scsi_host device's
+	 * firmware node, otherwise we will need to find out the handle from
+	 * its parent's acpi node.
+	 */
+	if (ap->scsi_host)
+		return DEVICE_ACPI_HANDLE(&ap->scsi_host->shost_gendev);
+	else
+		return acpi_get_child(DEVICE_ACPI_HANDLE(ap->host->dev),
+				ap->port_no);
 }
+EXPORT_SYMBOL(ata_ap_acpi_handle);
 
-static void ata_acpi_associate_ide_port(struct ata_port *ap)
+/**
+ * ata_dev_acpi_handle - provide the acpi_handle for an ata_device
+ * @dev: the acpi_device returned will correspond to this port
+ *
+ * Returns the acpi_handle for the ACPI namespace object corresponding to
+ * the ata_device passed into the function, or NULL if no such object exists
+ */
+acpi_handle ata_dev_acpi_handle(struct ata_device *dev)
 {
-	int max_devices, i;
-
-	ap->acpi_handle = acpi_get_child(ap->host->acpi_handle, ap->port_no);
-	if (!ap->acpi_handle)
-		return;
-
-	max_devices = 1;
-	if (ap->flags & ATA_FLAG_SLAVE_POSS)
-		max_devices++;
-
-	for (i = 0; i < max_devices; i++) {
-		struct ata_device *dev = &ap->link.device[i];
-
-		dev->acpi_handle = acpi_get_child(ap->acpi_handle, i);
-	}
+	acpi_integer adr;
+	struct ata_port *ap = dev->link->ap;
 
-	if (ata_acpi_gtm(ap, &ap->__acpi_init_gtm) == 0)
-		ap->pflags |= ATA_PFLAG_INIT_GTM_VALID;
+	if (ap->flags & ATA_FLAG_ACPI_SATA) {
+		if (!sata_pmp_attached(ap))
+			adr = SATA_ADR(ap->port_no, NO_PORT_MULT);
+		else
+			adr = SATA_ADR(ap->port_no, dev->link->pmp);
+		return acpi_get_child(DEVICE_ACPI_HANDLE(ap->host->dev), adr);
+	} else
+		return acpi_get_child(ata_ap_acpi_handle(ap), dev->devno);
 }
+EXPORT_SYMBOL(ata_dev_acpi_handle);
 
 /* @ap and @dev are the same as ata_acpi_handle_hotplug() */
 static void ata_acpi_detach_device(struct ata_port *ap, struct ata_device *dev)
@@ -229,56 +221,6 @@ static const struct acpi_dock_ops ata_acpi_ap_dock_ops = {
 };
 
 /**
- * ata_acpi_associate - associate ATA host with ACPI objects
- * @host: target ATA host
- *
- * Look up ACPI objects associated with @host and initialize
- * acpi_handle fields of @host, its ports and devices accordingly.
- *
- * LOCKING:
- * EH context.
- *
- * RETURNS:
- * 0 on success, -errno on failure.
- */
-void ata_acpi_associate(struct ata_host *host)
-{
-	int i, j;
-
-	if (!is_pci_dev(host->dev) || libata_noacpi)
-		return;
-
-	host->acpi_handle = DEVICE_ACPI_HANDLE(host->dev);
-	if (!host->acpi_handle)
-		return;
-
-	for (i = 0; i < host->n_ports; i++) {
-		struct ata_port *ap = host->ports[i];
-
-		if (host->ports[0]->flags & ATA_FLAG_ACPI_SATA)
-			ata_acpi_associate_sata_port(ap);
-		else
-			ata_acpi_associate_ide_port(ap);
-
-		if (ap->acpi_handle) {
-			/* we might be on a docking station */
-			register_hotplug_dock_device(ap->acpi_handle,
-					     &ata_acpi_ap_dock_ops, ap);
-		}
-
-		for (j = 0; j < ata_link_max_devices(&ap->link); j++) {
-			struct ata_device *dev = &ap->link.device[j];
-
-			if (dev->acpi_handle) {
-				/* we might be on a docking station */
-				register_hotplug_dock_device(dev->acpi_handle,
-					     &ata_acpi_dev_dock_ops, dev);
-			}
-		}
-	}
-}
-
-/**
  * ata_acpi_dissociate - dissociate ATA host from ACPI objects
  * @host: target ATA host
  *
@@ -299,7 +241,7 @@ void ata_acpi_dissociate(struct ata_host *host)
 		struct ata_port *ap = host->ports[i];
 		const struct ata_acpi_gtm *gtm = ata_acpi_init_gtm(ap);
 
-		if (ap->acpi_handle && gtm)
+		if (ata_ap_acpi_handle(ap) && gtm)
 			ata_acpi_stm(ap, gtm);
 	}
 }
@@ -324,7 +266,8 @@ int ata_acpi_gtm(struct ata_port *ap, struct ata_acpi_gtm *gtm)
 	acpi_status status;
 	int rc = 0;
 
-	status = acpi_evaluate_object(ap->acpi_handle, "_GTM", NULL, &output);
+	status = acpi_evaluate_object(ata_ap_acpi_handle(ap), "_GTM", NULL,
+				      &output);
 
 	rc = -ENOENT;
 	if (status == AE_NOT_FOUND)
@@ -394,7 +337,8 @@ int ata_acpi_stm(struct ata_port *ap, const struct ata_acpi_gtm *stm)
 	input.count = 3;
 	input.pointer = in_params;
 
-	status = acpi_evaluate_object(ap->acpi_handle, "_STM", &input, NULL);
+	status = acpi_evaluate_object(ata_ap_acpi_handle(ap), "_STM", &input,
+				      NULL);
 
 	if (status == AE_NOT_FOUND)
 		return -ENOENT;
@@ -451,7 +395,8 @@ static int ata_dev_get_GTF(struct ata_device *dev, struct ata_acpi_gtf **gtf)
 			    __func__, ap->port_no);
 
 	/* _GTF has no input parameters */
-	status = acpi_evaluate_object(dev->acpi_handle, "_GTF", NULL, &output);
+	status = acpi_evaluate_object(ata_dev_acpi_handle(dev), "_GTF", NULL,
+				      &output);
 	out_obj = dev->gtf_cache = output.pointer;
 
 	if (ACPI_FAILURE(status)) {
@@ -817,7 +762,8 @@ static int ata_acpi_push_id(struct ata_device *dev)
 
 	/* It's OK for _SDD to be missing too. */
 	swap_buf_le16(dev->id, ATA_ID_WORDS);
-	status = acpi_evaluate_object(dev->acpi_handle, "_SDD", &input, NULL);
+	status = acpi_evaluate_object(ata_dev_acpi_handle(dev), "_SDD", &input,
+				      NULL);
 	swap_buf_le16(dev->id, ATA_ID_WORDS);
 
 	if (status == AE_NOT_FOUND)
@@ -867,7 +813,7 @@ void ata_acpi_on_resume(struct ata_port *ap)
 	const struct ata_acpi_gtm *gtm = ata_acpi_init_gtm(ap);
 	struct ata_device *dev;
 
-	if (ap->acpi_handle && gtm) {
+	if (ata_ap_acpi_handle(ap) && gtm) {
 		/* _GTM valid */
 
 		/* restore timing parameters */
@@ -907,23 +853,39 @@ void ata_acpi_on_resume(struct ata_port *ap)
 void ata_acpi_set_state(struct ata_port *ap, pm_message_t state)
 {
 	struct ata_device *dev;
-
-	if (!ap->acpi_handle || (ap->flags & ATA_FLAG_ACPI_SATA))
-		return;
+	acpi_handle handle;
+	int acpi_state;
 
 	/* channel first and then drives for power on and vica versa
 	   for power off */
-	if (state.event == PM_EVENT_ON)
-		acpi_bus_set_power(ap->acpi_handle, ACPI_STATE_D0);
+	handle = ata_ap_acpi_handle(ap);
+	if (handle && state.event == PM_EVENT_ON)
+		acpi_bus_set_power(handle, ACPI_STATE_D0);
 
 	ata_for_each_dev(dev, &ap->link, ENABLED) {
-		if (dev->acpi_handle)
-			acpi_bus_set_power(dev->acpi_handle,
-				state.event == PM_EVENT_ON ?
-					ACPI_STATE_D0 : ACPI_STATE_D3);
+		handle = ata_dev_acpi_handle(dev);
+		if (!handle)
+			continue;
+
+		if (state.event != PM_EVENT_ON) {
+			acpi_state = acpi_pm_device_sleep_state(
+				&dev->sdev->sdev_gendev, NULL, ACPI_STATE_D3);
+			if (acpi_state > 0)
+				acpi_bus_set_power(handle, acpi_state);
+			/* TBD: need to check if it's runtime pm request */
+			acpi_pm_device_run_wake(
+				&dev->sdev->sdev_gendev, true);
+		} else {
+			/* Ditto */
+			acpi_pm_device_run_wake(
+				&dev->sdev->sdev_gendev, false);
+			acpi_bus_set_power(handle, ACPI_STATE_D0);
+		}
 	}
-	if (state.event != PM_EVENT_ON)
-		acpi_bus_set_power(ap->acpi_handle, ACPI_STATE_D3);
+
+	handle = ata_ap_acpi_handle(ap);
+	if (handle && state.event != PM_EVENT_ON)
+		acpi_bus_set_power(handle, ACPI_STATE_D3);
 }
 
 /**
@@ -948,7 +910,7 @@ int ata_acpi_on_devcfg(struct ata_device *dev)
 	int nr_executed = 0;
 	int rc;
 
-	if (!dev->acpi_handle)
+	if (!ata_dev_acpi_handle(dev))
 		return 0;
 
 	/* do we need to do _GTF? */
@@ -994,7 +956,6 @@ int ata_acpi_on_devcfg(struct ata_device *dev)
 	}
 
 	ata_dev_warn(dev, "ACPI: failed the second time, disabled\n");
-	dev->acpi_handle = NULL;
 
 	/* We can safely continue if no _GTF command has been executed
 	 * and port is not frozen.
@@ -1018,3 +979,218 @@ void ata_acpi_on_disable(struct ata_device *dev)
 {
 	ata_acpi_clear_gtf(dev);
 }
+
+static void ata_acpi_wake_dev(acpi_handle handle, u32 event, void *context)
+{
+	struct ata_device *ata_dev = context;
+
+	if (event == ACPI_NOTIFY_DEVICE_WAKE && ata_dev &&
+			pm_runtime_suspended(&ata_dev->sdev->sdev_gendev))
+		scsi_autopm_get_device(ata_dev->sdev);
+}
+
+static void ata_acpi_add_pm_notifier(struct ata_device *dev)
+{
+	struct acpi_device *acpi_dev;
+	acpi_handle handle;
+	acpi_status status;
+
+	handle = ata_dev_acpi_handle(dev);
+	if (!handle)
+		return;
+
+	status = acpi_bus_get_device(handle, &acpi_dev);
+	if (ACPI_FAILURE(status))
+		return;
+
+	if (dev->sdev->can_power_off) {
+		acpi_install_notify_handler(handle, ACPI_SYSTEM_NOTIFY,
+			ata_acpi_wake_dev, dev);
+		device_set_run_wake(&dev->sdev->sdev_gendev, true);
+	}
+}
+
+static void ata_acpi_remove_pm_notifier(struct ata_device *dev)
+{
+	struct acpi_device *acpi_dev;
+	acpi_handle handle;
+	acpi_status status;
+
+	handle = ata_dev_acpi_handle(dev);
+	if (!handle)
+		return;
+
+	status = acpi_bus_get_device(handle, &acpi_dev);
+	if (ACPI_FAILURE(status))
+		return;
+
+	if (dev->sdev->can_power_off) {
+		device_set_run_wake(&dev->sdev->sdev_gendev, false);
+		acpi_remove_notify_handler(handle, ACPI_SYSTEM_NOTIFY,
+			ata_acpi_wake_dev);
+	}
+}
+
+static void ata_acpi_register_power_resource(struct ata_device *dev)
+{
+	struct scsi_device *sdev = dev->sdev;
+	acpi_handle handle;
+	struct device *device;
+
+	handle = ata_dev_acpi_handle(dev);
+	if (!handle)
+		return;
+
+	device = &sdev->sdev_gendev;
+
+	acpi_power_resource_register_device(device, handle);
+}
+
+static void ata_acpi_unregister_power_resource(struct ata_device *dev)
+{
+	struct scsi_device *sdev = dev->sdev;
+	acpi_handle handle;
+	struct device *device;
+
+	handle = ata_dev_acpi_handle(dev);
+	if (!handle)
+		return;
+
+	device = &sdev->sdev_gendev;
+
+	acpi_power_resource_unregister_device(device, handle);
+}
+
+void ata_acpi_bind(struct ata_device *dev)
+{
+	ata_acpi_add_pm_notifier(dev);
+	ata_acpi_register_power_resource(dev);
+}
+
+void ata_acpi_unbind(struct ata_device *dev)
+{
+	ata_acpi_remove_pm_notifier(dev);
+	ata_acpi_unregister_power_resource(dev);
+}
+
+static int compat_pci_ata(struct ata_port *ap)
+{
+	struct device *dev = ap->tdev.parent;
+	struct pci_dev *pdev;
+
+	if (!is_pci_dev(dev))
+		return 0;
+
+	pdev = to_pci_dev(dev);
+
+	if ((pdev->class >> 8) != PCI_CLASS_STORAGE_SATA &&
+	    (pdev->class >> 8) != PCI_CLASS_STORAGE_IDE)
+		return 0;
+
+	return 1;
+}
+
+static int ata_acpi_bind_host(struct ata_port *ap, acpi_handle *handle)
+{
+	if (ap->flags & ATA_FLAG_ACPI_SATA)
+		return -ENODEV;
+
+	*handle = acpi_get_child(DEVICE_ACPI_HANDLE(ap->tdev.parent),
+			ap->port_no);
+
+	if (!*handle)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int ata_acpi_bind_device(struct ata_port *ap, struct scsi_device *sdev,
+				acpi_handle *handle)
+{
+	struct ata_device *ata_dev;
+	acpi_status status;
+	struct acpi_device *acpi_dev;
+	struct acpi_device_power_state *states;
+
+	if (ap->flags & ATA_FLAG_ACPI_SATA)
+		ata_dev = &ap->link.device[sdev->channel];
+	else
+		ata_dev = &ap->link.device[sdev->id];
+
+	*handle = ata_dev_acpi_handle(ata_dev);
+
+	if (!*handle)
+		return -ENODEV;
+
+	status = acpi_bus_get_device(*handle, &acpi_dev);
+	if (ACPI_FAILURE(status))
+		return 0;
+
+	/*
+	 * If firmware has _PS3 or _PR3 for this device,
+	 * and this ata ODD device support device attention,
+	 * it means this device can be powered off
+	 */
+	states = acpi_dev->power.states;
+	if ((states[ACPI_STATE_D3_HOT].flags.valid ||
+			states[ACPI_STATE_D3_COLD].flags.explicit_set) &&
+			ata_dev->flags & ATA_DFLAG_DA)
+		sdev->can_power_off = 1;
+
+	return 0;
+}
+
+static int is_ata_port(const struct device *dev)
+{
+	return dev->type == &ata_port_type;
+}
+
+static struct ata_port *dev_to_ata_port(struct device *dev)
+{
+	while (!is_ata_port(dev)) {
+		if (!dev->parent)
+			return NULL;
+		dev = dev->parent;
+	}
+	return to_ata_port(dev);
+}
+
+static int ata_acpi_find_device(struct device *dev, acpi_handle *handle)
+{
+	struct ata_port *ap = dev_to_ata_port(dev);
+
+	if (!ap)
+		return -ENODEV;
+
+	if (!compat_pci_ata(ap))
+		return -ENODEV;
+
+	if (scsi_is_host_device(dev))
+		return ata_acpi_bind_host(ap, handle);
+	else if (scsi_is_sdev_device(dev)) {
+		struct scsi_device *sdev = to_scsi_device(dev);
+
+		return ata_acpi_bind_device(ap, sdev, handle);
+	} else
+		return -ENODEV;
+}
+
+static int ata_acpi_find_dummy(struct device *dev, acpi_handle *handle)
+{
+	return -ENODEV;
+}
+
+static struct acpi_bus_type ata_acpi_bus = {
+	.find_bridge = ata_acpi_find_dummy,
+	.find_device = ata_acpi_find_device,
+};
+
+int ata_acpi_register(void)
+{
+	return scsi_register_acpi_bus_type(&ata_acpi_bus);
+}
+
+void ata_acpi_unregister(void)
+{
+	scsi_unregister_acpi_bus_type(&ata_acpi_bus);
+}
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index cece3a4d11e..fadd5866d40 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -80,6 +80,8 @@ const struct ata_port_operations ata_base_port_ops = {
 	.prereset		= ata_std_prereset,
 	.postreset		= ata_std_postreset,
 	.error_handler		= ata_std_error_handler,
+	.sched_eh		= ata_std_sched_eh,
+	.end_eh			= ata_std_end_eh,
 };
 
 const struct ata_port_operations sata_port_ops = {
@@ -2374,6 +2376,9 @@ int ata_dev_configure(struct ata_device *dev)
 			dma_dir_string = ", DMADIR";
 		}
 
+		if (ata_id_has_da(dev->id))
+			dev->flags |= ATA_DFLAG_DA;
+
 		/* print device info to dmesg */
 		if (ata_msg_drv(ap) && print_info)
 			ata_dev_info(dev,
@@ -4126,6 +4131,7 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 
 	/* Devices which aren't very happy with higher link speeds */
 	{ "WD My Book",			NULL,	ATA_HORKAGE_1_5_GBPS, },
+	{ "Seagate FreeAgent GoFlex",	NULL,	ATA_HORKAGE_1_5_GBPS, },
 
 	/*
 	 * Devices which choke on SETXFER.  Applies only if both the
@@ -5288,8 +5294,6 @@ static int ata_port_request_pm(struct ata_port *ap, pm_message_t mesg,
 	return rc;
 }
 
-#define to_ata_port(d) container_of(d, struct ata_port, tdev)
-
 static int ata_port_suspend_common(struct device *dev, pm_message_t mesg)
 {
 	struct ata_port *ap = to_ata_port(dev);
@@ -6051,9 +6055,6 @@ int ata_host_register(struct ata_host *host, struct scsi_host_template *sht)
 	if (rc)
 		goto err_tadd;
 
-	/* associate with ACPI nodes */
-	ata_acpi_associate(host);
-
 	/* set cable, sata_spd_limit and report */
 	for (i = 0; i < host->n_ports; i++) {
 		struct ata_port *ap = host->ports[i];
@@ -6513,6 +6514,8 @@ static int __init ata_init(void)
 
 	ata_parse_force_param();
 
+	ata_acpi_register();
+
 	rc = ata_sff_init();
 	if (rc) {
 		kfree(ata_force_tbl);
@@ -6539,6 +6542,7 @@ static void __exit ata_exit(void)
 	ata_release_transport(ata_scsi_transport_template);
 	libata_transport_exit();
 	ata_sff_exit();
+	ata_acpi_unregister();
 	kfree(ata_force_tbl);
 }
 
@@ -6642,6 +6646,8 @@ struct ata_port_operations ata_dummy_port_ops = {
 	.qc_prep		= ata_noop_qc_prep,
 	.qc_issue		= ata_dummy_qc_issue,
 	.error_handler		= ata_dummy_error_handler,
+	.sched_eh		= ata_std_sched_eh,
+	.end_eh			= ata_std_end_eh,
 };
 
 const struct ata_port_info ata_dummy_port_info = {
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index 6d53cf9b3b6..7d4535e989b 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -419,7 +419,7 @@ int ata_ering_map(struct ata_ering *ering,
 	return rc;
 }
 
-int ata_ering_clear_cb(struct ata_ering_entry *ent, void *void_arg)
+static int ata_ering_clear_cb(struct ata_ering_entry *ent, void *void_arg)
 {
 	ent->eflags |= ATA_EFLAG_OLD_ER;
 	return 0;
@@ -793,12 +793,12 @@ void ata_scsi_port_error_handler(struct Scsi_Host *host, struct ata_port *ap)
 		ata_for_each_link(link, ap, HOST_FIRST)
 			memset(&link->eh_info, 0, sizeof(link->eh_info));
 
-		/* Clear host_eh_scheduled while holding ap->lock such
-		 * that if exception occurs after this point but
-		 * before EH completion, SCSI midlayer will
+		/* end eh (clear host_eh_scheduled) while holding
+		 * ap->lock such that if exception occurs after this
+		 * point but before EH completion, SCSI midlayer will
 		 * re-initiate EH.
 		 */
-		host->host_eh_scheduled = 0;
+		ap->ops->end_eh(ap);
 
 		spin_unlock_irqrestore(ap->lock, flags);
 		ata_eh_release(ap);
@@ -986,16 +986,13 @@ void ata_qc_schedule_eh(struct ata_queued_cmd *qc)
 }
 
 /**
- *	ata_port_schedule_eh - schedule error handling without a qc
- *	@ap: ATA port to schedule EH for
- *
- *	Schedule error handling for @ap.  EH will kick in as soon as
- *	all commands are drained.
+ * ata_std_sched_eh - non-libsas ata_ports issue eh with this common routine
+ * @ap: ATA port to schedule EH for
  *
- *	LOCKING:
+ *	LOCKING: inherited from ata_port_schedule_eh
  *	spin_lock_irqsave(host lock)
  */
-void ata_port_schedule_eh(struct ata_port *ap)
+void ata_std_sched_eh(struct ata_port *ap)
 {
 	WARN_ON(!ap->ops->error_handler);
 
@@ -1007,6 +1004,44 @@ void ata_port_schedule_eh(struct ata_port *ap)
 
 	DPRINTK("port EH scheduled\n");
 }
+EXPORT_SYMBOL_GPL(ata_std_sched_eh);
+
+/**
+ * ata_std_end_eh - non-libsas ata_ports complete eh with this common routine
+ * @ap: ATA port to end EH for
+ *
+ * In the libata object model there is a 1:1 mapping of ata_port to
+ * shost, so host fields can be directly manipulated under ap->lock, in
+ * the libsas case we need to hold a lock at the ha->level to coordinate
+ * these events.
+ *
+ *	LOCKING:
+ *	spin_lock_irqsave(host lock)
+ */
+void ata_std_end_eh(struct ata_port *ap)
+{
+	struct Scsi_Host *host = ap->scsi_host;
+
+	host->host_eh_scheduled = 0;
+}
+EXPORT_SYMBOL(ata_std_end_eh);
+
+
+/**
+ *	ata_port_schedule_eh - schedule error handling without a qc
+ *	@ap: ATA port to schedule EH for
+ *
+ *	Schedule error handling for @ap.  EH will kick in as soon as
+ *	all commands are drained.
+ *
+ *	LOCKING:
+ *	spin_lock_irqsave(host lock)
+ */
+void ata_port_schedule_eh(struct ata_port *ap)
+{
+	/* see: ata_std_sched_eh, unless you know better */
+	ap->ops->sched_eh(ap);
+}
 
 static int ata_do_link_abort(struct ata_port *ap, struct ata_link *link)
 {
diff --git a/drivers/ata/libata-pmp.c b/drivers/ata/libata-pmp.c
index 21b80c555c6..61c59ee45ce 100644
--- a/drivers/ata/libata-pmp.c
+++ b/drivers/ata/libata-pmp.c
@@ -529,8 +529,6 @@ int sata_pmp_attach(struct ata_device *dev)
 	ata_for_each_link(tlink, ap, EDGE)
 		sata_link_init_spd(tlink);
 
-	ata_acpi_associate_sata_port(ap);
-
 	return 0;
 
  fail:
@@ -570,8 +568,6 @@ static void sata_pmp_detach(struct ata_device *dev)
 	ap->nr_pmp_links = 0;
 	link->pmp = 0;
 	spin_unlock_irqrestore(ap->lock, flags);
-
-	ata_acpi_associate_sata_port(ap);
 }
 
 /**
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 22226350cd0..8ec81ca8f65 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -3445,6 +3445,7 @@ void ata_scsi_scan_host(struct ata_port *ap, int sync)
 			if (!IS_ERR(sdev)) {
 				dev->sdev = sdev;
 				scsi_device_put(sdev);
+				ata_acpi_bind(dev);
 			} else {
 				dev->sdev = NULL;
 			}
@@ -3541,6 +3542,8 @@ static void ata_scsi_remove_dev(struct ata_device *dev)
 	mutex_lock(&ap->scsi_host->scan_mutex);
 	spin_lock_irqsave(ap->lock, flags);
 
+	ata_acpi_unbind(dev);
+
 	/* clearing dev->sdev is protected by host lock */
 	sdev = dev->sdev;
 	dev->sdev = NULL;
diff --git a/drivers/ata/libata-transport.c b/drivers/ata/libata-transport.c
index c3419048537..c04d393d20c 100644
--- a/drivers/ata/libata-transport.c
+++ b/drivers/ata/libata-transport.c
@@ -232,7 +232,7 @@ static void ata_tport_release(struct device *dev)
  * Returns:
  *	%1 if the device represents a ATA Port, %0 else
  */
-int ata_is_port(const struct device *dev)
+static int ata_is_port(const struct device *dev)
 {
 	return dev->release == ata_tport_release;
 }
@@ -355,7 +355,7 @@ static void ata_tlink_release(struct device *dev)
  * Returns:
  *	%1 if the device represents a ATA link, %0 else
  */
-int ata_is_link(const struct device *dev)
+static int ata_is_link(const struct device *dev)
 {
 	return dev->release == ata_tlink_release;
 }
@@ -572,7 +572,7 @@ static void ata_tdev_release(struct device *dev)
  * Returns:
  *	%1 if the device represents a ATA device, %0 else
  */
-int ata_is_ata_dev(const struct device *dev)
+static int ata_is_ata_dev(const struct device *dev)
 {
 	return dev->release == ata_tdev_release;
 }
diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h
index 9d0fd0b7185..50e4dff0604 100644
--- a/drivers/ata/libata.h
+++ b/drivers/ata/libata.h
@@ -107,21 +107,22 @@ extern const char *sata_spd_string(unsigned int spd);
 extern int ata_port_probe(struct ata_port *ap);
 extern void __ata_port_probe(struct ata_port *ap);
 
+#define to_ata_port(d) container_of(d, struct ata_port, tdev)
+
 /* libata-acpi.c */
 #ifdef CONFIG_ATA_ACPI
 extern unsigned int ata_acpi_gtf_filter;
-
-extern void ata_acpi_associate_sata_port(struct ata_port *ap);
-extern void ata_acpi_associate(struct ata_host *host);
 extern void ata_acpi_dissociate(struct ata_host *host);
 extern int ata_acpi_on_suspend(struct ata_port *ap);
 extern void ata_acpi_on_resume(struct ata_port *ap);
 extern int ata_acpi_on_devcfg(struct ata_device *dev);
 extern void ata_acpi_on_disable(struct ata_device *dev);
 extern void ata_acpi_set_state(struct ata_port *ap, pm_message_t state);
+extern int ata_acpi_register(void);
+extern void ata_acpi_unregister(void);
+extern void ata_acpi_bind(struct ata_device *dev);
+extern void ata_acpi_unbind(struct ata_device *dev);
 #else
-static inline void ata_acpi_associate_sata_port(struct ata_port *ap) { }
-static inline void ata_acpi_associate(struct ata_host *host) { }
 static inline void ata_acpi_dissociate(struct ata_host *host) { }
 static inline int ata_acpi_on_suspend(struct ata_port *ap) { return 0; }
 static inline void ata_acpi_on_resume(struct ata_port *ap) { }
@@ -129,6 +130,10 @@ static inline int ata_acpi_on_devcfg(struct ata_device *dev) { return 0; }
 static inline void ata_acpi_on_disable(struct ata_device *dev) { }
 static inline void ata_acpi_set_state(struct ata_port *ap,
 				      pm_message_t state) { }
+static inline int ata_acpi_register(void) { return 0; }
+static inline void ata_acpi_unregister(void) { }
+static inline void ata_acpi_bind(struct ata_device *dev) { }
+static inline void ata_acpi_unbind(struct ata_device *dev) { }
 #endif
 
 /* libata-scsi.c */
diff --git a/drivers/ata/pata_acpi.c b/drivers/ata/pata_acpi.c
index 54145edf50e..09723b76bea 100644
--- a/drivers/ata/pata_acpi.c
+++ b/drivers/ata/pata_acpi.c
@@ -39,7 +39,7 @@ static int pacpi_pre_reset(struct ata_link *link, unsigned long deadline)
 {
 	struct ata_port *ap = link->ap;
 	struct pata_acpi *acpi = ap->private_data;
-	if (ap->acpi_handle == NULL || ata_acpi_gtm(ap, &acpi->gtm) < 0)
+	if (ata_ap_acpi_handle(ap) == NULL || ata_acpi_gtm(ap, &acpi->gtm) < 0)
 		return -ENODEV;
 
 	return ata_sff_prereset(link, deadline);
@@ -195,7 +195,7 @@ static int pacpi_port_start(struct ata_port *ap)
 	struct pci_dev *pdev = to_pci_dev(ap->host->dev);
 	struct pata_acpi *acpi;
 
-	if (ap->acpi_handle == NULL)
+	if (ata_ap_acpi_handle(ap) == NULL)
 		return -ENODEV;
 
 	acpi = ap->private_data = devm_kzalloc(&pdev->dev, sizeof(struct pata_acpi), GFP_KERNEL);
@@ -273,22 +273,10 @@ static struct pci_driver pacpi_pci_driver = {
 #endif
 };
 
-static int __init pacpi_init(void)
-{
-	return pci_register_driver(&pacpi_pci_driver);
-}
-
-static void __exit pacpi_exit(void)
-{
-	pci_unregister_driver(&pacpi_pci_driver);
-}
-
-module_init(pacpi_init);
-module_exit(pacpi_exit);
+module_pci_driver(pacpi_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for ATA in ACPI mode");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, pacpi_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_amd.c b/drivers/ata/pata_amd.c
index dc6b5dae046..82a08922afc 100644
--- a/drivers/ata/pata_amd.c
+++ b/drivers/ata/pata_amd.c
@@ -632,21 +632,10 @@ static struct pci_driver amd_pci_driver = {
 #endif
 };
 
-static int __init amd_init(void)
-{
-	return pci_register_driver(&amd_pci_driver);
-}
-
-static void __exit amd_exit(void)
-{
-	pci_unregister_driver(&amd_pci_driver);
-}
+module_pci_driver(amd_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for AMD and Nvidia PATA IDE");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, amd);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(amd_init);
-module_exit(amd_exit);
diff --git a/drivers/ata/pata_arasan_cf.c b/drivers/ata/pata_arasan_cf.c
index ac6a5beb28f..bfaa5cb1629 100644
--- a/drivers/ata/pata_arasan_cf.c
+++ b/drivers/ata/pata_arasan_cf.c
@@ -184,10 +184,8 @@
 struct arasan_cf_dev {
 	/* pointer to ata_host structure */
 	struct ata_host *host;
-	/* clk structure, only if HAVE_CLK is defined */
-#ifdef CONFIG_HAVE_CLK
+	/* clk structure */
 	struct clk *clk;
-#endif
 
 	/* physical base address of controller */
 	dma_addr_t pbase;
@@ -312,13 +310,11 @@ static int cf_init(struct arasan_cf_dev *acdev)
 	unsigned long flags;
 	int ret = 0;
 
-#ifdef CONFIG_HAVE_CLK
 	ret = clk_enable(acdev->clk);
 	if (ret) {
 		dev_dbg(acdev->host->dev, "clock enable failed");
 		return ret;
 	}
-#endif
 
 	spin_lock_irqsave(&acdev->host->lock, flags);
 	/* configure CF interface clock */
@@ -344,9 +340,7 @@ static void cf_exit(struct arasan_cf_dev *acdev)
 	writel(readl(acdev->vbase + OP_MODE) & ~CFHOST_ENB,
 			acdev->vbase + OP_MODE);
 	spin_unlock_irqrestore(&acdev->host->lock, flags);
-#ifdef CONFIG_HAVE_CLK
 	clk_disable(acdev->clk);
-#endif
 }
 
 static void dma_callback(void *dev)
@@ -828,13 +822,11 @@ static int __devinit arasan_cf_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-#ifdef CONFIG_HAVE_CLK
 	acdev->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(acdev->clk)) {
 		dev_warn(&pdev->dev, "Clock not found\n");
 		return PTR_ERR(acdev->clk);
 	}
-#endif
 
 	/* allocate host */
 	host = ata_host_alloc(&pdev->dev, 1);
@@ -899,9 +891,7 @@ static int __devinit arasan_cf_probe(struct platform_device *pdev)
 			&arasan_cf_sht);
 
 free_clk:
-#ifdef CONFIG_HAVE_CLK
 	clk_put(acdev->clk);
-#endif
 	return ret;
 }
 
@@ -912,9 +902,7 @@ static int __devexit arasan_cf_remove(struct platform_device *pdev)
 
 	ata_host_detach(host);
 	cf_exit(acdev);
-#ifdef CONFIG_HAVE_CLK
 	clk_put(acdev->clk);
-#endif
 
 	return 0;
 }
diff --git a/drivers/ata/pata_artop.c b/drivers/ata/pata_artop.c
index 4b8b22efc00..74b215c09b2 100644
--- a/drivers/ata/pata_artop.c
+++ b/drivers/ata/pata_artop.c
@@ -451,18 +451,7 @@ static struct pci_driver artop_pci_driver = {
 #endif
 };
 
-static int __init artop_init(void)
-{
-	return pci_register_driver(&artop_pci_driver);
-}
-
-static void __exit artop_exit(void)
-{
-	pci_unregister_driver(&artop_pci_driver);
-}
-
-module_init(artop_init);
-module_exit(artop_exit);
+module_pci_driver(artop_pci_driver);
 
 MODULE_AUTHOR("Alan Cox, Bartlomiej Zolnierkiewicz");
 MODULE_DESCRIPTION("SCSI low-level driver for ARTOP PATA");
diff --git a/drivers/ata/pata_atiixp.c b/drivers/ata/pata_atiixp.c
index be1aa1486d3..361c75cea57 100644
--- a/drivers/ata/pata_atiixp.c
+++ b/drivers/ata/pata_atiixp.c
@@ -289,22 +289,10 @@ static struct pci_driver atiixp_pci_driver = {
 #endif
 };
 
-static int __init atiixp_init(void)
-{
-	return pci_register_driver(&atiixp_pci_driver);
-}
-
-
-static void __exit atiixp_exit(void)
-{
-	pci_unregister_driver(&atiixp_pci_driver);
-}
+module_pci_driver(atiixp_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for ATI IXP200/300/400");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, atiixp);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(atiixp_init);
-module_exit(atiixp_exit);
diff --git a/drivers/ata/pata_atp867x.c b/drivers/ata/pata_atp867x.c
index 3cfabb262af..041f50d5324 100644
--- a/drivers/ata/pata_atp867x.c
+++ b/drivers/ata/pata_atp867x.c
@@ -565,21 +565,10 @@ static struct pci_driver atp867x_driver = {
 #endif
 };
 
-static int __init atp867x_init(void)
-{
-	return pci_register_driver(&atp867x_driver);
-}
-
-static void __exit atp867x_exit(void)
-{
-	pci_unregister_driver(&atp867x_driver);
-}
+module_pci_driver(atp867x_driver);
 
 MODULE_AUTHOR("John(Jung-Ik) Lee, Google Inc.");
 MODULE_DESCRIPTION("low level driver for Artop/Acard 867x ATA controller");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, atp867x_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(atp867x_init);
-module_exit(atp867x_exit);
diff --git a/drivers/ata/pata_cmd640.c b/drivers/ata/pata_cmd640.c
index 549d28dbf90..504b98b58e1 100644
--- a/drivers/ata/pata_cmd640.c
+++ b/drivers/ata/pata_cmd640.c
@@ -263,21 +263,10 @@ static struct pci_driver cmd640_pci_driver = {
 #endif
 };
 
-static int __init cmd640_init(void)
-{
-	return pci_register_driver(&cmd640_pci_driver);
-}
-
-static void __exit cmd640_exit(void)
-{
-	pci_unregister_driver(&cmd640_pci_driver);
-}
+module_pci_driver(cmd640_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for CMD640 PATA controllers");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, cmd640);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cmd640_init);
-module_exit(cmd640_exit);
diff --git a/drivers/ata/pata_cmd64x.c b/drivers/ata/pata_cmd64x.c
index 1c17cd1e8b2..7ba01415b67 100644
--- a/drivers/ata/pata_cmd64x.c
+++ b/drivers/ata/pata_cmd64x.c
@@ -423,7 +423,7 @@ static int cmd64x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 			.port_ops = &cmd648_port_ops
 		}
 	};
-	const struct ata_port_info *ppi[] = { 
+	const struct ata_port_info *ppi[] = {
 		&cmd_info[id->driver_data],
 		&cmd_info[id->driver_data],
 		NULL
@@ -478,7 +478,7 @@ static int cmd64x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (port_ok && cntrl_ch0_ok && !(reg & CNTRL_CH0)) {
 		dev_printk(KERN_NOTICE, &pdev->dev, "Primary port is disabled\n");
 		ppi[0] = &ata_dummy_port_info;
-		
+
 	}
 	if (port_ok && !(reg & CNTRL_CH1)) {
 		dev_printk(KERN_NOTICE, &pdev->dev, "Secondary port is disabled\n");
@@ -525,21 +525,10 @@ static struct pci_driver cmd64x_pci_driver = {
 #endif
 };
 
-static int __init cmd64x_init(void)
-{
-	return pci_register_driver(&cmd64x_pci_driver);
-}
-
-static void __exit cmd64x_exit(void)
-{
-	pci_unregister_driver(&cmd64x_pci_driver);
-}
+module_pci_driver(cmd64x_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for CMD64x series PATA controllers");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, cmd64x);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cmd64x_init);
-module_exit(cmd64x_exit);
diff --git a/drivers/ata/pata_cs5520.c b/drivers/ata/pata_cs5520.c
index 9ddcddc66a2..de74d804f03 100644
--- a/drivers/ata/pata_cs5520.c
+++ b/drivers/ata/pata_cs5520.c
@@ -302,22 +302,10 @@ static struct pci_driver cs5520_pci_driver = {
 #endif
 };
 
-static int __init cs5520_init(void)
-{
-	return pci_register_driver(&cs5520_pci_driver);
-}
-
-static void __exit cs5520_exit(void)
-{
-	pci_unregister_driver(&cs5520_pci_driver);
-}
+module_pci_driver(cs5520_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Cyrix CS5510/5520");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, pata_cs5520);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cs5520_init);
-module_exit(cs5520_exit);
-
diff --git a/drivers/ata/pata_cs5530.c b/drivers/ata/pata_cs5530.c
index f792330f0d8..48389ae0b33 100644
--- a/drivers/ata/pata_cs5530.c
+++ b/drivers/ata/pata_cs5530.c
@@ -363,21 +363,10 @@ static struct pci_driver cs5530_pci_driver = {
 #endif
 };
 
-static int __init cs5530_init(void)
-{
-	return pci_register_driver(&cs5530_pci_driver);
-}
-
-static void __exit cs5530_exit(void)
-{
-	pci_unregister_driver(&cs5530_pci_driver);
-}
+module_pci_driver(cs5530_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the Cyrix/NS/AMD 5530");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, cs5530);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cs5530_init);
-module_exit(cs5530_exit);
diff --git a/drivers/ata/pata_cs5535.c b/drivers/ata/pata_cs5535.c
index a0b4640125a..997e16a3a63 100644
--- a/drivers/ata/pata_cs5535.c
+++ b/drivers/ata/pata_cs5535.c
@@ -207,21 +207,10 @@ static struct pci_driver cs5535_pci_driver = {
 #endif
 };
 
-static int __init cs5535_init(void)
-{
-	return pci_register_driver(&cs5535_pci_driver);
-}
-
-static void __exit cs5535_exit(void)
-{
-	pci_unregister_driver(&cs5535_pci_driver);
-}
+module_pci_driver(cs5535_pci_driver);
 
 MODULE_AUTHOR("Alan Cox, Jens Altmann, Wolfgan Zuleger, Alexander Kiausch");
 MODULE_DESCRIPTION("low-level driver for the NS/AMD 5535");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, cs5535);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cs5535_init);
-module_exit(cs5535_exit);
diff --git a/drivers/ata/pata_cs5536.c b/drivers/ata/pata_cs5536.c
index 7a402c75ab9..dec1b6c4b35 100644
--- a/drivers/ata/pata_cs5536.c
+++ b/drivers/ata/pata_cs5536.c
@@ -274,21 +274,10 @@ static struct pci_driver cs5536_pci_driver = {
 #endif
 };
 
-static int __init cs5536_init(void)
-{
-	return pci_register_driver(&cs5536_pci_driver);
-}
-
-static void __exit cs5536_exit(void)
-{
-	pci_unregister_driver(&cs5536_pci_driver);
-}
+module_pci_driver(cs5536_pci_driver);
 
 MODULE_AUTHOR("Martin K. Petersen");
 MODULE_DESCRIPTION("low-level driver for the CS5536 IDE controller");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, cs5536);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cs5536_init);
-module_exit(cs5536_exit);
diff --git a/drivers/ata/pata_cypress.c b/drivers/ata/pata_cypress.c
index 6d915b063d9..810bc9964dd 100644
--- a/drivers/ata/pata_cypress.c
+++ b/drivers/ata/pata_cypress.c
@@ -158,23 +158,10 @@ static struct pci_driver cy82c693_pci_driver = {
 #endif
 };
 
-static int __init cy82c693_init(void)
-{
-	return pci_register_driver(&cy82c693_pci_driver);
-}
-
-
-static void __exit cy82c693_exit(void)
-{
-	pci_unregister_driver(&cy82c693_pci_driver);
-}
-
+module_pci_driver(cy82c693_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the CY82C693 PATA controller");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, cy82c693);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(cy82c693_init);
-module_exit(cy82c693_exit);
diff --git a/drivers/ata/pata_efar.c b/drivers/ata/pata_efar.c
index f0243ed206f..3c12fd7acd4 100644
--- a/drivers/ata/pata_efar.c
+++ b/drivers/ata/pata_efar.c
@@ -295,22 +295,10 @@ static struct pci_driver efar_pci_driver = {
 #endif
 };
 
-static int __init efar_init(void)
-{
-	return pci_register_driver(&efar_pci_driver);
-}
-
-static void __exit efar_exit(void)
-{
-	pci_unregister_driver(&efar_pci_driver);
-}
-
-module_init(efar_init);
-module_exit(efar_exit);
+module_pci_driver(efar_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for EFAR PIIX clones");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, efar_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_hpt366.c b/drivers/ata/pata_hpt366.c
index 42cffd38910..4be884a9f5e 100644
--- a/drivers/ata/pata_hpt366.c
+++ b/drivers/ata/pata_hpt366.c
@@ -418,21 +418,10 @@ static struct pci_driver hpt36x_pci_driver = {
 #endif
 };
 
-static int __init hpt36x_init(void)
-{
-	return pci_register_driver(&hpt36x_pci_driver);
-}
-
-static void __exit hpt36x_exit(void)
-{
-	pci_unregister_driver(&hpt36x_pci_driver);
-}
+module_pci_driver(hpt36x_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the Highpoint HPT366/368");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, hpt36x);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(hpt36x_init);
-module_exit(hpt36x_exit);
diff --git a/drivers/ata/pata_hpt37x.c b/drivers/ata/pata_hpt37x.c
index 9620636aa40..a9d74eff5fc 100644
--- a/drivers/ata/pata_hpt37x.c
+++ b/drivers/ata/pata_hpt37x.c
@@ -1058,21 +1058,10 @@ static struct pci_driver hpt37x_pci_driver = {
 	.remove		= ata_pci_remove_one
 };
 
-static int __init hpt37x_init(void)
-{
-	return pci_register_driver(&hpt37x_pci_driver);
-}
-
-static void __exit hpt37x_exit(void)
-{
-	pci_unregister_driver(&hpt37x_pci_driver);
-}
+module_pci_driver(hpt37x_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the Highpoint HPT37x/30x");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, hpt37x);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(hpt37x_init);
-module_exit(hpt37x_exit);
diff --git a/drivers/ata/pata_hpt3x2n.c b/drivers/ata/pata_hpt3x2n.c
index 765f136d8cd..4be0398c153 100644
--- a/drivers/ata/pata_hpt3x2n.c
+++ b/drivers/ata/pata_hpt3x2n.c
@@ -621,21 +621,10 @@ static struct pci_driver hpt3x2n_pci_driver = {
 	.remove		= ata_pci_remove_one
 };
 
-static int __init hpt3x2n_init(void)
-{
-	return pci_register_driver(&hpt3x2n_pci_driver);
-}
-
-static void __exit hpt3x2n_exit(void)
-{
-	pci_unregister_driver(&hpt3x2n_pci_driver);
-}
+module_pci_driver(hpt3x2n_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the Highpoint HPT3xxN");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, hpt3x2n);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(hpt3x2n_init);
-module_exit(hpt3x2n_exit);
diff --git a/drivers/ata/pata_hpt3x3.c b/drivers/ata/pata_hpt3x3.c
index b3042dab08b..76c9314bb82 100644
--- a/drivers/ata/pata_hpt3x3.c
+++ b/drivers/ata/pata_hpt3x3.c
@@ -284,23 +284,10 @@ static struct pci_driver hpt3x3_pci_driver = {
 #endif
 };
 
-static int __init hpt3x3_init(void)
-{
-	return pci_register_driver(&hpt3x3_pci_driver);
-}
-
-
-static void __exit hpt3x3_exit(void)
-{
-	pci_unregister_driver(&hpt3x3_pci_driver);
-}
-
+module_pci_driver(hpt3x3_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the Highpoint HPT343/363");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, hpt3x3);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(hpt3x3_init);
-module_exit(hpt3x3_exit);
diff --git a/drivers/ata/pata_imx.c b/drivers/ata/pata_imx.c
index c5af97f5107..87bb05b3caf 100644
--- a/drivers/ata/pata_imx.c
+++ b/drivers/ata/pata_imx.c
@@ -118,7 +118,7 @@ static int __devinit pata_imx_probe(struct platform_device *pdev)
 		return PTR_ERR(priv->clk);
 	}
 
-	clk_enable(priv->clk);
+	clk_prepare_enable(priv->clk);
 
 	host = ata_host_alloc(&pdev->dev, 1);
 	if (!host)
@@ -162,7 +162,7 @@ static int __devinit pata_imx_probe(struct platform_device *pdev)
 				&pata_imx_sht);
 
 free_priv:
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 	clk_put(priv->clk);
 	return -ENOMEM;
 }
@@ -176,7 +176,7 @@ static int __devexit pata_imx_remove(struct platform_device *pdev)
 
 	__raw_writel(0, priv->host_regs + PATA_IMX_ATA_INT_EN);
 
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 	clk_put(priv->clk);
 
 	return 0;
@@ -194,7 +194,7 @@ static int pata_imx_suspend(struct device *dev)
 		__raw_writel(0, priv->host_regs + PATA_IMX_ATA_INT_EN);
 		priv->ata_ctl =
 			__raw_readl(priv->host_regs + PATA_IMX_ATA_CONTROL);
-		clk_disable(priv->clk);
+		clk_disable_unprepare(priv->clk);
 	}
 
 	return ret;
@@ -205,7 +205,7 @@ static int pata_imx_resume(struct device *dev)
 	struct ata_host *host = dev_get_drvdata(dev);
 	struct pata_imx_priv *priv = host->private_data;
 
-	clk_enable(priv->clk);
+	clk_prepare_enable(priv->clk);
 
 	__raw_writel(priv->ata_ctl, priv->host_regs + PATA_IMX_ATA_CONTROL);
 
diff --git a/drivers/ata/pata_it8213.c b/drivers/ata/pata_it8213.c
index cf9164d79f1..2a8dd9527ec 100644
--- a/drivers/ata/pata_it8213.c
+++ b/drivers/ata/pata_it8213.c
@@ -290,18 +290,7 @@ static struct pci_driver it8213_pci_driver = {
 #endif
 };
 
-static int __init it8213_init(void)
-{
-	return pci_register_driver(&it8213_pci_driver);
-}
-
-static void __exit it8213_exit(void)
-{
-	pci_unregister_driver(&it8213_pci_driver);
-}
-
-module_init(it8213_init);
-module_exit(it8213_exit);
+module_pci_driver(it8213_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for the ITE 8213");
diff --git a/drivers/ata/pata_it821x.c b/drivers/ata/pata_it821x.c
index 62c5d00abd2..9cc05d808ad 100644
--- a/drivers/ata/pata_it821x.c
+++ b/drivers/ata/pata_it821x.c
@@ -972,15 +972,7 @@ static struct pci_driver it821x_pci_driver = {
 #endif
 };
 
-static int __init it821x_init(void)
-{
-	return pci_register_driver(&it821x_pci_driver);
-}
-
-static void __exit it821x_exit(void)
-{
-	pci_unregister_driver(&it821x_pci_driver);
-}
+module_pci_driver(it821x_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for the IT8211/IT8212 IDE RAID controller");
@@ -988,9 +980,5 @@ MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, it821x);
 MODULE_VERSION(DRV_VERSION);
 
-
 module_param_named(noraid, it8212_noraid, int, S_IRUGO);
 MODULE_PARM_DESC(noraid, "Force card into bypass mode");
-
-module_init(it821x_init);
-module_exit(it821x_exit);
diff --git a/drivers/ata/pata_jmicron.c b/drivers/ata/pata_jmicron.c
index cb3babbb703..76e739b031b 100644
--- a/drivers/ata/pata_jmicron.c
+++ b/drivers/ata/pata_jmicron.c
@@ -164,18 +164,7 @@ static struct pci_driver jmicron_pci_driver = {
 #endif
 };
 
-static int __init jmicron_init(void)
-{
-	return pci_register_driver(&jmicron_pci_driver);
-}
-
-static void __exit jmicron_exit(void)
-{
-	pci_unregister_driver(&jmicron_pci_driver);
-}
-
-module_init(jmicron_init);
-module_exit(jmicron_exit);
+module_pci_driver(jmicron_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for Jmicron PATA ports");
diff --git a/drivers/ata/pata_marvell.c b/drivers/ata/pata_marvell.c
index 5d7f58a7e34..a4f5e781c8c 100644
--- a/drivers/ata/pata_marvell.c
+++ b/drivers/ata/pata_marvell.c
@@ -178,22 +178,10 @@ static struct pci_driver marvell_pci_driver = {
 #endif
 };
 
-static int __init marvell_init(void)
-{
-	return pci_register_driver(&marvell_pci_driver);
-}
-
-static void __exit marvell_exit(void)
-{
-	pci_unregister_driver(&marvell_pci_driver);
-}
-
-module_init(marvell_init);
-module_exit(marvell_exit);
+module_pci_driver(marvell_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for Marvell ATA in legacy mode");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, marvell_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_mpiix.c b/drivers/ata/pata_mpiix.c
index 9dc16df8419..1f5f28bb0bb 100644
--- a/drivers/ata/pata_mpiix.c
+++ b/drivers/ata/pata_mpiix.c
@@ -230,21 +230,10 @@ static struct pci_driver mpiix_pci_driver = {
 #endif
 };
 
-static int __init mpiix_init(void)
-{
-	return pci_register_driver(&mpiix_pci_driver);
-}
-
-static void __exit mpiix_exit(void)
-{
-	pci_unregister_driver(&mpiix_pci_driver);
-}
+module_pci_driver(mpiix_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Intel MPIIX");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, mpiix);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(mpiix_init);
-module_exit(mpiix_exit);
diff --git a/drivers/ata/pata_netcell.c b/drivers/ata/pata_netcell.c
index 9979a43bc59..ad1a0febd62 100644
--- a/drivers/ata/pata_netcell.c
+++ b/drivers/ata/pata_netcell.c
@@ -99,22 +99,10 @@ static struct pci_driver netcell_pci_driver = {
 #endif
 };
 
-static int __init netcell_init(void)
-{
-	return pci_register_driver(&netcell_pci_driver);
-}
-
-static void __exit netcell_exit(void)
-{
-	pci_unregister_driver(&netcell_pci_driver);
-}
-
-module_init(netcell_init);
-module_exit(netcell_exit);
+module_pci_driver(netcell_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for Netcell PATA RAID");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, netcell_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_ninja32.c b/drivers/ata/pata_ninja32.c
index e277a142138..12010ed596c 100644
--- a/drivers/ata/pata_ninja32.c
+++ b/drivers/ata/pata_ninja32.c
@@ -190,21 +190,10 @@ static struct pci_driver ninja32_pci_driver = {
 #endif
 };
 
-static int __init ninja32_init(void)
-{
-	return pci_register_driver(&ninja32_pci_driver);
-}
-
-static void __exit ninja32_exit(void)
-{
-	pci_unregister_driver(&ninja32_pci_driver);
-}
+module_pci_driver(ninja32_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Ninja32 ATA");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, ninja32);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(ninja32_init);
-module_exit(ninja32_exit);
diff --git a/drivers/ata/pata_ns87410.c b/drivers/ata/pata_ns87410.c
index 31d5986537a..0c424dae56e 100644
--- a/drivers/ata/pata_ns87410.c
+++ b/drivers/ata/pata_ns87410.c
@@ -168,21 +168,10 @@ static struct pci_driver ns87410_pci_driver = {
 #endif
 };
 
-static int __init ns87410_init(void)
-{
-	return pci_register_driver(&ns87410_pci_driver);
-}
-
-static void __exit ns87410_exit(void)
-{
-	pci_unregister_driver(&ns87410_pci_driver);
-}
+module_pci_driver(ns87410_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Nat Semi 87410");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, ns87410);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(ns87410_init);
-module_exit(ns87410_exit);
diff --git a/drivers/ata/pata_ns87415.c b/drivers/ata/pata_ns87415.c
index f1d517bc5b4..6f6fa106050 100644
--- a/drivers/ata/pata_ns87415.c
+++ b/drivers/ata/pata_ns87415.c
@@ -414,18 +414,7 @@ static struct pci_driver ns87415_pci_driver = {
 #endif
 };
 
-static int __init ns87415_init(void)
-{
-	return pci_register_driver(&ns87415_pci_driver);
-}
-
-static void __exit ns87415_exit(void)
-{
-	pci_unregister_driver(&ns87415_pci_driver);
-}
-
-module_init(ns87415_init);
-module_exit(ns87415_exit);
+module_pci_driver(ns87415_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("ATA low-level driver for NS87415 controllers");
diff --git a/drivers/ata/pata_oldpiix.c b/drivers/ata/pata_oldpiix.c
index 98cdf50e406..d77b2e1054e 100644
--- a/drivers/ata/pata_oldpiix.c
+++ b/drivers/ata/pata_oldpiix.c
@@ -265,22 +265,10 @@ static struct pci_driver oldpiix_pci_driver = {
 #endif
 };
 
-static int __init oldpiix_init(void)
-{
-	return pci_register_driver(&oldpiix_pci_driver);
-}
-
-static void __exit oldpiix_exit(void)
-{
-	pci_unregister_driver(&oldpiix_pci_driver);
-}
-
-module_init(oldpiix_init);
-module_exit(oldpiix_exit);
+module_pci_driver(oldpiix_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for early PIIX series controllers");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, oldpiix_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_opti.c b/drivers/ata/pata_opti.c
index accc033faf7..4ea70cd22ae 100644
--- a/drivers/ata/pata_opti.c
+++ b/drivers/ata/pata_opti.c
@@ -191,22 +191,10 @@ static struct pci_driver opti_pci_driver = {
 #endif
 };
 
-static int __init opti_init(void)
-{
-	return pci_register_driver(&opti_pci_driver);
-}
-
-static void __exit opti_exit(void)
-{
-	pci_unregister_driver(&opti_pci_driver);
-}
-
+module_pci_driver(opti_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Opti 621/621X");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, opti);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(opti_init);
-module_exit(opti_exit);
diff --git a/drivers/ata/pata_optidma.c b/drivers/ata/pata_optidma.c
index 77cb9140863..78ede3fd187 100644
--- a/drivers/ata/pata_optidma.c
+++ b/drivers/ata/pata_optidma.c
@@ -447,21 +447,10 @@ static struct pci_driver optidma_pci_driver = {
 #endif
 };
 
-static int __init optidma_init(void)
-{
-	return pci_register_driver(&optidma_pci_driver);
-}
-
-static void __exit optidma_exit(void)
-{
-	pci_unregister_driver(&optidma_pci_driver);
-}
+module_pci_driver(optidma_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Opti Firestar/Firestar Plus");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, optidma);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(optidma_init);
-module_exit(optidma_exit);
diff --git a/drivers/ata/pata_pcmcia.c b/drivers/ata/pata_pcmcia.c
index a808ba03bd7..958238dda8f 100644
--- a/drivers/ata/pata_pcmcia.c
+++ b/drivers/ata/pata_pcmcia.c
@@ -170,7 +170,8 @@ static int pcmcia_check_one_config(struct pcmcia_device *pdev, void *priv_data)
 {
 	int *is_kme = priv_data;
 
-	if (!(pdev->resource[0]->flags & IO_DATA_PATH_WIDTH_8)) {
+	if ((pdev->resource[0]->flags & IO_DATA_PATH_WIDTH)
+	    != IO_DATA_PATH_WIDTH_8) {
 		pdev->resource[0]->flags &= ~IO_DATA_PATH_WIDTH;
 		pdev->resource[0]->flags |= IO_DATA_PATH_WIDTH_AUTO;
 	}
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index 7d63f24179c..c9399c8688c 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -784,21 +784,4 @@ static int pdc2027x_reinit_one(struct pci_dev *pdev)
 }
 #endif
 
-/**
- * pdc2027x_init - Called after this module is loaded into the kernel.
- */
-static int __init pdc2027x_init(void)
-{
-	return pci_register_driver(&pdc2027x_pci_driver);
-}
-
-/**
- * pdc2027x_exit - Called before this module unloaded from the kernel
- */
-static void __exit pdc2027x_exit(void)
-{
-	pci_unregister_driver(&pdc2027x_pci_driver);
-}
-
-module_init(pdc2027x_init);
-module_exit(pdc2027x_exit);
+module_pci_driver(pdc2027x_pci_driver);
diff --git a/drivers/ata/pata_pdc202xx_old.c b/drivers/ata/pata_pdc202xx_old.c
index c2ed5868dda..c34fc50070a 100644
--- a/drivers/ata/pata_pdc202xx_old.c
+++ b/drivers/ata/pata_pdc202xx_old.c
@@ -384,21 +384,10 @@ static struct pci_driver pdc202xx_pci_driver = {
 #endif
 };
 
-static int __init pdc202xx_init(void)
-{
-	return pci_register_driver(&pdc202xx_pci_driver);
-}
-
-static void __exit pdc202xx_exit(void)
-{
-	pci_unregister_driver(&pdc202xx_pci_driver);
-}
+module_pci_driver(pdc202xx_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Promise 2024x and 20262-20267");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, pdc202xx);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(pdc202xx_init);
-module_exit(pdc202xx_exit);
diff --git a/drivers/ata/pata_piccolo.c b/drivers/ata/pata_piccolo.c
index cb01bf9496f..2beb6b5045f 100644
--- a/drivers/ata/pata_piccolo.c
+++ b/drivers/ata/pata_piccolo.c
@@ -117,24 +117,10 @@ static struct pci_driver ata_tosh_pci_driver = {
 #endif
 };
 
-static int __init ata_tosh_init(void)
-{
-	return pci_register_driver(&ata_tosh_pci_driver);
-}
-
-
-static void __exit ata_tosh_exit(void)
-{
-	pci_unregister_driver(&ata_tosh_pci_driver);
-}
-
+module_pci_driver(ata_tosh_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("Low level driver for Toshiba Piccolo ATA");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, ata_tosh);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(ata_tosh_init);
-module_exit(ata_tosh_exit);
-
diff --git a/drivers/ata/pata_radisys.c b/drivers/ata/pata_radisys.c
index b2d3a2bb4e6..f582ba180a7 100644
--- a/drivers/ata/pata_radisys.c
+++ b/drivers/ata/pata_radisys.c
@@ -244,22 +244,10 @@ static struct pci_driver radisys_pci_driver = {
 #endif
 };
 
-static int __init radisys_init(void)
-{
-	return pci_register_driver(&radisys_pci_driver);
-}
-
-static void __exit radisys_exit(void)
-{
-	pci_unregister_driver(&radisys_pci_driver);
-}
-
-module_init(radisys_init);
-module_exit(radisys_exit);
+module_pci_driver(radisys_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for Radisys R82600 controllers");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, radisys_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_rdc.c b/drivers/ata/pata_rdc.c
index e6a2dd7809c..32a3499e83e 100644
--- a/drivers/ata/pata_rdc.c
+++ b/drivers/ata/pata_rdc.c
@@ -394,18 +394,7 @@ static struct pci_driver rdc_pci_driver = {
 };
 
 
-static int __init rdc_init(void)
-{
-	return pci_register_driver(&rdc_pci_driver);
-}
-
-static void __exit rdc_exit(void)
-{
-	pci_unregister_driver(&rdc_pci_driver);
-}
-
-module_init(rdc_init);
-module_exit(rdc_exit);
+module_pci_driver(rdc_pci_driver);
 
 MODULE_AUTHOR("Alan Cox (based on ata_piix)");
 MODULE_DESCRIPTION("SCSI low-level driver for RDC PATA controllers");
diff --git a/drivers/ata/pata_rz1000.c b/drivers/ata/pata_rz1000.c
index aca321e1e6a..60f4de2dd47 100644
--- a/drivers/ata/pata_rz1000.c
+++ b/drivers/ata/pata_rz1000.c
@@ -140,22 +140,10 @@ static struct pci_driver rz1000_pci_driver = {
 #endif
 };
 
-static int __init rz1000_init(void)
-{
-	return pci_register_driver(&rz1000_pci_driver);
-}
-
-static void __exit rz1000_exit(void)
-{
-	pci_unregister_driver(&rz1000_pci_driver);
-}
+module_pci_driver(rz1000_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for RZ1000 PCI ATA");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, pata_rz1000);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(rz1000_init);
-module_exit(rz1000_exit);
-
diff --git a/drivers/ata/pata_sc1200.c b/drivers/ata/pata_sc1200.c
index c0e603a84f7..ce2f828c17b 100644
--- a/drivers/ata/pata_sc1200.c
+++ b/drivers/ata/pata_sc1200.c
@@ -261,21 +261,10 @@ static struct pci_driver sc1200_pci_driver = {
 #endif
 };
 
-static int __init sc1200_init(void)
-{
-	return pci_register_driver(&sc1200_pci_driver);
-}
-
-static void __exit sc1200_exit(void)
-{
-	pci_unregister_driver(&sc1200_pci_driver);
-}
+module_pci_driver(sc1200_pci_driver);
 
 MODULE_AUTHOR("Alan Cox, Mark Lord");
 MODULE_DESCRIPTION("low-level driver for the NS/AMD SC1200");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, sc1200);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(sc1200_init);
-module_exit(sc1200_exit);
diff --git a/drivers/ata/pata_scc.c b/drivers/ata/pata_scc.c
index e265f835c95..f35f15f4d83 100644
--- a/drivers/ata/pata_scc.c
+++ b/drivers/ata/pata_scc.c
@@ -1103,26 +1103,7 @@ static struct pci_driver scc_pci_driver = {
 #endif
 };
 
-static int __init scc_init (void)
-{
-	int rc;
-
-	DPRINTK("pci_register_driver\n");
-	rc = pci_register_driver(&scc_pci_driver);
-	if (rc)
-		return rc;
-
-	DPRINTK("done\n");
-	return 0;
-}
-
-static void __exit scc_exit (void)
-{
-	pci_unregister_driver(&scc_pci_driver);
-}
-
-module_init(scc_init);
-module_exit(scc_exit);
+module_pci_driver(scc_pci_driver);
 
 MODULE_AUTHOR("Toshiba corp");
 MODULE_DESCRIPTION("SCSI low-level driver for Toshiba SCC PATA controller");
diff --git a/drivers/ata/pata_sch.c b/drivers/ata/pata_sch.c
index 7c78b999362..db0d18cf1c2 100644
--- a/drivers/ata/pata_sch.c
+++ b/drivers/ata/pata_sch.c
@@ -179,15 +179,4 @@ static int __devinit sch_init_one(struct pci_dev *pdev,
 	return ata_pci_bmdma_init_one(pdev, ppi, &sch_sht, NULL, 0);
 }
 
-static int __init sch_init(void)
-{
-	return pci_register_driver(&sch_pci_driver);
-}
-
-static void __exit sch_exit(void)
-{
-	pci_unregister_driver(&sch_pci_driver);
-}
-
-module_init(sch_init);
-module_exit(sch_exit);
+module_pci_driver(sch_pci_driver);
diff --git a/drivers/ata/pata_serverworks.c b/drivers/ata/pata_serverworks.c
index 71eaf385e97..f3febbce6c4 100644
--- a/drivers/ata/pata_serverworks.c
+++ b/drivers/ata/pata_serverworks.c
@@ -475,21 +475,10 @@ static struct pci_driver serverworks_pci_driver = {
 #endif
 };
 
-static int __init serverworks_init(void)
-{
-	return pci_register_driver(&serverworks_pci_driver);
-}
-
-static void __exit serverworks_exit(void)
-{
-	pci_unregister_driver(&serverworks_pci_driver);
-}
+module_pci_driver(serverworks_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Serverworks OSB4/CSB5/CSB6");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, serverworks);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(serverworks_init);
-module_exit(serverworks_exit);
diff --git a/drivers/ata/pata_sil680.c b/drivers/ata/pata_sil680.c
index b92eacf8dd3..5cfdf94823d 100644
--- a/drivers/ata/pata_sil680.c
+++ b/drivers/ata/pata_sil680.c
@@ -437,21 +437,10 @@ static struct pci_driver sil680_pci_driver = {
 #endif
 };
 
-static int __init sil680_init(void)
-{
-	return pci_register_driver(&sil680_pci_driver);
-}
-
-static void __exit sil680_exit(void)
-{
-	pci_unregister_driver(&sil680_pci_driver);
-}
+module_pci_driver(sil680_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for SI680 PATA");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, sil680);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(sil680_init);
-module_exit(sil680_exit);
diff --git a/drivers/ata/pata_sis.c b/drivers/ata/pata_sis.c
index b0edc7de7b2..2d5ac136126 100644
--- a/drivers/ata/pata_sis.c
+++ b/drivers/ata/pata_sis.c
@@ -906,22 +906,10 @@ static struct pci_driver sis_pci_driver = {
 #endif
 };
 
-static int __init sis_init(void)
-{
-	return pci_register_driver(&sis_pci_driver);
-}
-
-static void __exit sis_exit(void)
-{
-	pci_unregister_driver(&sis_pci_driver);
-}
-
-module_init(sis_init);
-module_exit(sis_exit);
+module_pci_driver(sis_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("SCSI low-level driver for SiS ATA");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, sis_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
diff --git a/drivers/ata/pata_sl82c105.c b/drivers/ata/pata_sl82c105.c
index 24cf200dd1c..738e000107d 100644
--- a/drivers/ata/pata_sl82c105.c
+++ b/drivers/ata/pata_sl82c105.c
@@ -372,21 +372,10 @@ static struct pci_driver sl82c105_pci_driver = {
 #endif
 };
 
-static int __init sl82c105_init(void)
-{
-	return pci_register_driver(&sl82c105_pci_driver);
-}
-
-static void __exit sl82c105_exit(void)
-{
-	pci_unregister_driver(&sl82c105_pci_driver);
-}
+module_pci_driver(sl82c105_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Sl82c105");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, sl82c105);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(sl82c105_init);
-module_exit(sl82c105_exit);
diff --git a/drivers/ata/pata_triflex.c b/drivers/ata/pata_triflex.c
index 28da1c6becf..c8e589d9123 100644
--- a/drivers/ata/pata_triflex.c
+++ b/drivers/ata/pata_triflex.c
@@ -240,21 +240,10 @@ static struct pci_driver triflex_pci_driver = {
 #endif
 };
 
-static int __init triflex_init(void)
-{
-	return pci_register_driver(&triflex_pci_driver);
-}
-
-static void __exit triflex_exit(void)
-{
-	pci_unregister_driver(&triflex_pci_driver);
-}
+module_pci_driver(triflex_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for Compaq Triflex");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, triflex);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(triflex_init);
-module_exit(triflex_exit);
diff --git a/drivers/ata/pata_via.c b/drivers/ata/pata_via.c
index 255f336cd7e..8d2a9fdf6b8 100644
--- a/drivers/ata/pata_via.c
+++ b/drivers/ata/pata_via.c
@@ -711,21 +711,10 @@ static struct pci_driver via_pci_driver = {
 #endif
 };
 
-static int __init via_init(void)
-{
-	return pci_register_driver(&via_pci_driver);
-}
-
-static void __exit via_exit(void)
-{
-	pci_unregister_driver(&via_pci_driver);
-}
+module_pci_driver(via_pci_driver);
 
 MODULE_AUTHOR("Alan Cox");
 MODULE_DESCRIPTION("low-level driver for VIA PATA");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, via);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(via_init);
-module_exit(via_exit);
diff --git a/drivers/ata/pdc_adma.c b/drivers/ata/pdc_adma.c
index 04911d52f59..505333340ad 100644
--- a/drivers/ata/pdc_adma.c
+++ b/drivers/ata/pdc_adma.c
@@ -660,21 +660,10 @@ static int adma_ata_init_one(struct pci_dev *pdev,
 				 &adma_ata_sht);
 }
 
-static int __init adma_ata_init(void)
-{
-	return pci_register_driver(&adma_ata_pci_driver);
-}
-
-static void __exit adma_ata_exit(void)
-{
-	pci_unregister_driver(&adma_ata_pci_driver);
-}
+module_pci_driver(adma_ata_pci_driver);
 
 MODULE_AUTHOR("Mark Lord");
 MODULE_DESCRIPTION("Pacific Digital Corporation ADMA low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, adma_ata_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(adma_ata_init);
-module_exit(adma_ata_exit);
diff --git a/drivers/ata/sata_dwc_460ex.c b/drivers/ata/sata_dwc_460ex.c
index 69f7cde49c6..937aeb34b31 100644..100755
--- a/drivers/ata/sata_dwc_460ex.c
+++ b/drivers/ata/sata_dwc_460ex.c
@@ -158,6 +158,7 @@ enum {
 /* Assign HW handshaking interface (x) to destination / source peripheral */
 #define	DMA_CFG_HW_HS_DEST(int_num) (((int_num) & 0xF) << 11)
 #define	DMA_CFG_HW_HS_SRC(int_num) (((int_num) & 0xF) << 7)
+#define	DMA_CFG_HW_CH_PRIOR(int_num) (((int_num) & 0xF) << 5)
 #define	DMA_LLP_LMS(addr, master) (((addr) & 0xfffffffc) | (master))
 
 /*
@@ -318,6 +319,7 @@ struct sata_dwc_host_priv {
 	u32	dma_interrupt_count;
 	struct	ahb_dma_regs	*sata_dma_regs;
 	struct	device	*dwc_dev;
+	int	dma_channel;
 };
 struct sata_dwc_host_priv host_pvt;
 /*
@@ -437,15 +439,12 @@ static  void clear_chan_interrupts(int c)
  */
 static int dma_request_channel(void)
 {
-	int i;
-
-	for (i = 0; i < DMA_NUM_CHANS; i++) {
-		if (!(in_le32(&(host_pvt.sata_dma_regs->dma_chan_en.low)) &\
-			DMA_CHANNEL(i)))
-			return i;
-	}
-	dev_err(host_pvt.dwc_dev, "%s NO channel chan_en: 0x%08x\n", __func__,
-		in_le32(&(host_pvt.sata_dma_regs->dma_chan_en.low)));
+	/* Check if the channel is not currently in use */
+	if (!(in_le32(&(host_pvt.sata_dma_regs->dma_chan_en.low)) &
+		DMA_CHANNEL(host_pvt.dma_channel)))
+		return host_pvt.dma_channel;
+	dev_err(host_pvt.dwc_dev, "%s Channel %d is currently in use\n",
+		__func__, host_pvt.dma_channel);
 	return -1;
 }
 
@@ -481,7 +480,8 @@ static irqreturn_t dma_dwc_interrupt(int irq, void *hsdev_instance)
 	dev_dbg(ap->dev, "eot=0x%08x err=0x%08x pending=%d active port=%d\n",
 		tfr_reg, err_reg, hsdevp->dma_pending[tag], port);
 
-	for (chan = 0; chan < DMA_NUM_CHANS; chan++) {
+	chan = host_pvt.dma_channel;
+	if (chan >= 0) {
 		/* Check for end-of-transfer interrupt. */
 		if (tfr_reg & DMA_CHANNEL(chan)) {
 			/*
@@ -534,9 +534,9 @@ static irqreturn_t dma_dwc_interrupt(int irq, void *hsdev_instance)
 static int dma_request_interrupts(struct sata_dwc_device *hsdev, int irq)
 {
 	int retval = 0;
-	int chan;
+	int chan = host_pvt.dma_channel;
 
-	for (chan = 0; chan < DMA_NUM_CHANS; chan++) {
+	if (chan >= 0) {
 		/* Unmask error interrupt */
 		out_le32(&(host_pvt.sata_dma_regs)->interrupt_mask.error.low,
 			 DMA_ENABLE_CHAN(chan));
@@ -575,7 +575,10 @@ static int map_sg_to_lli(struct scatterlist *sg, int num_elems,
 	int fis_len = 0;
 	dma_addr_t next_llp;
 	int bl;
+	int sms_val, dms_val;
 
+	sms_val = 0;
+	dms_val = 1 + host_pvt.dma_channel;
 	dev_dbg(host_pvt.dwc_dev, "%s: sg=%p nelem=%d lli=%p dma_lli=0x%08x"
 		" dmadr=0x%08x\n", __func__, sg, num_elems, lli, (u32)dma_lli,
 		(u32)dmadr_addr);
@@ -635,8 +638,8 @@ static int map_sg_to_lli(struct scatterlist *sg, int num_elems,
 
 				lli[idx].ctl.low = cpu_to_le32(
 					DMA_CTL_TTFC(DMA_CTL_TTFC_P2M_DMAC) |
-					DMA_CTL_SMS(0) |
-					DMA_CTL_DMS(1) |
+					DMA_CTL_SMS(sms_val) |
+					DMA_CTL_DMS(dms_val) |
 					DMA_CTL_SRC_MSIZE(bl) |
 					DMA_CTL_DST_MSIZE(bl) |
 					DMA_CTL_SINC_NOCHANGE |
@@ -651,8 +654,8 @@ static int map_sg_to_lli(struct scatterlist *sg, int num_elems,
 
 				lli[idx].ctl.low = cpu_to_le32(
 					DMA_CTL_TTFC(DMA_CTL_TTFC_M2P_PER) |
-					DMA_CTL_SMS(1) |
-					DMA_CTL_DMS(0) |
+					DMA_CTL_SMS(dms_val) |
+					DMA_CTL_DMS(sms_val) |
 					DMA_CTL_SRC_MSIZE(bl) |
 					DMA_CTL_DST_MSIZE(bl) |
 					DMA_CTL_DINC_NOCHANGE |
@@ -744,8 +747,10 @@ static int dma_dwc_xfer_setup(struct scatterlist *sg, int num_elems,
 
 	/* Program the CFG register. */
 	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].cfg.high),
+		 DMA_CFG_HW_HS_SRC(dma_ch) | DMA_CFG_HW_HS_DEST(dma_ch) |
 		 DMA_CFG_PROTCTL | DMA_CFG_FCMOD_REQ);
-	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].cfg.low), 0);
+	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].cfg.low),
+		 DMA_CFG_HW_CH_PRIOR(dma_ch));
 
 	/* Program the address of the linked list */
 	out_le32(&(host_pvt.sata_dma_regs->chan_regs[dma_ch].llp.low),
@@ -1581,10 +1586,31 @@ static void sata_dwc_qc_prep(struct ata_queued_cmd *qc)
 
 static void sata_dwc_error_handler(struct ata_port *ap)
 {
-	ap->link.flags |= ATA_LFLAG_NO_HRST;
 	ata_sff_error_handler(ap);
 }
 
+int sata_dwc_hardreset(struct ata_link *link, unsigned int *class,
+			unsigned long deadline)
+{
+	struct sata_dwc_device *hsdev = HSDEV_FROM_AP(link->ap);
+	int ret;
+
+	ret = sata_sff_hardreset(link, class, deadline);
+
+	sata_dwc_enable_interrupts(hsdev);
+
+	/* Reconfigure the DMA control register */
+	out_le32(&hsdev->sata_dwc_regs->dmacr,
+		 SATA_DWC_DMACR_TXRXCH_CLEAR);
+
+	/* Reconfigure the DMA Burst Transaction Size register */
+	out_le32(&hsdev->sata_dwc_regs->dbtsr,
+		 SATA_DWC_DBTSR_MWR(AHB_DMA_BRST_DFLT) |
+		 SATA_DWC_DBTSR_MRD(AHB_DMA_BRST_DFLT));
+
+	return ret;
+}
+
 /*
  * scsi mid-layer and libata interface structures
  */
@@ -1604,6 +1630,7 @@ static struct ata_port_operations sata_dwc_ops = {
 	.inherits		= &ata_sff_port_ops,
 
 	.error_handler		= sata_dwc_error_handler,
+	.hardreset		= sata_dwc_hardreset,
 
 	.qc_prep		= sata_dwc_qc_prep,
 	.qc_issue		= sata_dwc_qc_issue,
@@ -1638,6 +1665,8 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 	struct ata_host *host;
 	struct ata_port_info pi = sata_dwc_port_info[0];
 	const struct ata_port_info *ppi[] = { &pi, NULL };
+	struct device_node *np = ofdev->dev.of_node;
+	u32 dma_chan;
 
 	/* Allocate DWC SATA device */
 	hsdev = kzalloc(sizeof(*hsdev), GFP_KERNEL);
@@ -1647,6 +1676,13 @@ static int sata_dwc_probe(struct platform_device *ofdev)
 		goto error;
 	}
 
+	if (of_property_read_u32(np, "dma-channel", &dma_chan)) {
+		dev_warn(&ofdev->dev, "no dma-channel property set."
+			 " Use channel 0\n");
+		dma_chan = 0;
+	}
+	host_pvt.dma_channel = dma_chan;
+
 	/* Ioremap SATA registers */
 	base = of_iomap(ofdev->dev.of_node, 0);
 	if (!base) {
diff --git a/drivers/ata/sata_inic162x.c b/drivers/ata/sata_inic162x.c
index 5c7d70c03bf..dc35f4d42b8 100644
--- a/drivers/ata/sata_inic162x.c
+++ b/drivers/ata/sata_inic162x.c
@@ -894,21 +894,10 @@ static struct pci_driver inic_pci_driver = {
 	.remove		= ata_pci_remove_one,
 };
 
-static int __init inic_init(void)
-{
-	return pci_register_driver(&inic_pci_driver);
-}
-
-static void __exit inic_exit(void)
-{
-	pci_unregister_driver(&inic_pci_driver);
-}
+module_pci_driver(inic_pci_driver);
 
 MODULE_AUTHOR("Tejun Heo");
 MODULE_DESCRIPTION("low-level driver for Initio 162x SATA");
 MODULE_LICENSE("GPL v2");
 MODULE_DEVICE_TABLE(pci, inic_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(inic_init);
-module_exit(inic_exit);
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 24712adf69d..311be18d3f0 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -65,6 +65,8 @@
 #include <linux/mbus.h>
 #include <linux/bitops.h>
 #include <linux/gfp.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_device.h>
@@ -4026,7 +4028,7 @@ static int mv_platform_probe(struct platform_device *pdev)
 	struct ata_host *host;
 	struct mv_host_priv *hpriv;
 	struct resource *res;
-	int n_ports = 0;
+	int n_ports = 0, irq = 0;
 	int rc;
 #if defined(CONFIG_HAVE_CLK)
 	int port;
@@ -4050,8 +4052,14 @@ static int mv_platform_probe(struct platform_device *pdev)
 		return -EINVAL;
 
 	/* allocate host */
-	mv_platform_data = pdev->dev.platform_data;
-	n_ports = mv_platform_data->n_ports;
+	if (pdev->dev.of_node) {
+		of_property_read_u32(pdev->dev.of_node, "nr-ports", &n_ports);
+		irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	} else {
+		mv_platform_data = pdev->dev.platform_data;
+		n_ports = mv_platform_data->n_ports;
+		irq = platform_get_irq(pdev, 0);
+	}
 
 	host = ata_host_alloc_pinfo(&pdev->dev, ppi, n_ports);
 	hpriv = devm_kzalloc(&pdev->dev, sizeof(*hpriv), GFP_KERNEL);
@@ -4109,8 +4117,7 @@ static int mv_platform_probe(struct platform_device *pdev)
 	dev_info(&pdev->dev, "slots %u ports %d\n",
 		 (unsigned)MV_MAX_Q_DEPTH, host->n_ports);
 
-	rc = ata_host_activate(host, platform_get_irq(pdev, 0), mv_interrupt,
-			       IRQF_SHARED, &mv6_sht);
+	rc = ata_host_activate(host, irq, mv_interrupt, IRQF_SHARED, &mv6_sht);
 	if (!rc)
 		return 0;
 
@@ -4205,15 +4212,24 @@ static int mv_platform_resume(struct platform_device *pdev)
 #define mv_platform_resume NULL
 #endif
 
+#ifdef CONFIG_OF
+static struct of_device_id mv_sata_dt_ids[] __devinitdata = {
+	{ .compatible = "marvell,orion-sata", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mv_sata_dt_ids);
+#endif
+
 static struct platform_driver mv_platform_driver = {
-	.probe			= mv_platform_probe,
-	.remove			= __devexit_p(mv_platform_remove),
-	.suspend		= mv_platform_suspend,
-	.resume			= mv_platform_resume,
-	.driver			= {
-				   .name = DRV_NAME,
-				   .owner = THIS_MODULE,
-				  },
+	.probe		= mv_platform_probe,
+	.remove		= __devexit_p(mv_platform_remove),
+	.suspend	= mv_platform_suspend,
+	.resume		= mv_platform_resume,
+	.driver		= {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(mv_sata_dt_ids),
+	},
 };
 
 
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index 55d6179dde5..85ee4993ca7 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -2510,22 +2510,11 @@ static void nv_adma_host_stop(struct ata_host *host)
 	nv_ck804_host_stop(host);
 }
 
-static int __init nv_init(void)
-{
-	return pci_register_driver(&nv_pci_driver);
-}
-
-static void __exit nv_exit(void)
-{
-	pci_unregister_driver(&nv_pci_driver);
-}
+module_pci_driver(nv_pci_driver);
 
-module_init(nv_init);
-module_exit(nv_exit);
 module_param_named(adma, adma_enabled, bool, 0444);
 MODULE_PARM_DESC(adma, "Enable use of ADMA (Default: false)");
 module_param_named(swncq, swncq_enabled, bool, 0444);
 MODULE_PARM_DESC(swncq, "Enable use of SWNCQ (Default: true)");
 module_param_named(msi, msi_enabled, bool, 0444);
 MODULE_PARM_DESC(msi, "Enable use of MSI (Default: false)");
-
diff --git a/drivers/ata/sata_promise.c b/drivers/ata/sata_promise.c
index 000fcc99e01..489c8176832 100644
--- a/drivers/ata/sata_promise.c
+++ b/drivers/ata/sata_promise.c
@@ -1249,21 +1249,10 @@ static int pdc_ata_init_one(struct pci_dev *pdev,
 				 &pdc_ata_sht);
 }
 
-static int __init pdc_ata_init(void)
-{
-	return pci_register_driver(&pdc_ata_pci_driver);
-}
-
-static void __exit pdc_ata_exit(void)
-{
-	pci_unregister_driver(&pdc_ata_pci_driver);
-}
+module_pci_driver(pdc_ata_pci_driver);
 
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("Promise ATA TX2/TX4/TX4000 low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, pdc_ata_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(pdc_ata_init);
-module_exit(pdc_ata_exit);
diff --git a/drivers/ata/sata_qstor.c b/drivers/ata/sata_qstor.c
index 9d1a47bb21b..3b0dd57984e 100644
--- a/drivers/ata/sata_qstor.c
+++ b/drivers/ata/sata_qstor.c
@@ -635,21 +635,10 @@ static int qs_ata_init_one(struct pci_dev *pdev,
 				 &qs_ata_sht);
 }
 
-static int __init qs_ata_init(void)
-{
-	return pci_register_driver(&qs_ata_pci_driver);
-}
-
-static void __exit qs_ata_exit(void)
-{
-	pci_unregister_driver(&qs_ata_pci_driver);
-}
+module_pci_driver(qs_ata_pci_driver);
 
 MODULE_AUTHOR("Mark Lord");
 MODULE_DESCRIPTION("Pacific Digital Corporation QStor SATA low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, qs_ata_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(qs_ata_init);
-module_exit(qs_ata_exit);
diff --git a/drivers/ata/sata_sil.c b/drivers/ata/sata_sil.c
index 9dfb40b8c2c..a7b31672c4b 100644
--- a/drivers/ata/sata_sil.c
+++ b/drivers/ata/sata_sil.c
@@ -819,16 +819,4 @@ static int sil_pci_device_resume(struct pci_dev *pdev)
 }
 #endif
 
-static int __init sil_init(void)
-{
-	return pci_register_driver(&sil_pci_driver);
-}
-
-static void __exit sil_exit(void)
-{
-	pci_unregister_driver(&sil_pci_driver);
-}
-
-
-module_init(sil_init);
-module_exit(sil_exit);
+module_pci_driver(sil_pci_driver);
diff --git a/drivers/ata/sata_sil24.c b/drivers/ata/sata_sil24.c
index e7e610aa9a7..a5f2a563a26 100644
--- a/drivers/ata/sata_sil24.c
+++ b/drivers/ata/sata_sil24.c
@@ -1382,20 +1382,9 @@ static int sil24_port_resume(struct ata_port *ap)
 }
 #endif
 
-static int __init sil24_init(void)
-{
-	return pci_register_driver(&sil24_pci_driver);
-}
-
-static void __exit sil24_exit(void)
-{
-	pci_unregister_driver(&sil24_pci_driver);
-}
+module_pci_driver(sil24_pci_driver);
 
 MODULE_AUTHOR("Tejun Heo");
 MODULE_DESCRIPTION("Silicon Image 3124/3132 SATA low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, sil24_pci_tbl);
-
-module_init(sil24_init);
-module_exit(sil24_exit);
diff --git a/drivers/ata/sata_sis.c b/drivers/ata/sata_sis.c
index 95ec435f0eb..fe3ca0989b1 100644
--- a/drivers/ata/sata_sis.c
+++ b/drivers/ata/sata_sis.c
@@ -308,15 +308,4 @@ static int sis_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 				 IRQF_SHARED, &sis_sht);
 }
 
-static int __init sis_init(void)
-{
-	return pci_register_driver(&sis_pci_driver);
-}
-
-static void __exit sis_exit(void)
-{
-	pci_unregister_driver(&sis_pci_driver);
-}
-
-module_init(sis_init);
-module_exit(sis_exit);
+module_pci_driver(sis_pci_driver);
diff --git a/drivers/ata/sata_svw.c b/drivers/ata/sata_svw.c
index c646118943f..44a4256533e 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -525,21 +525,10 @@ static struct pci_driver k2_sata_pci_driver = {
 	.remove			= ata_pci_remove_one,
 };
 
-static int __init k2_sata_init(void)
-{
-	return pci_register_driver(&k2_sata_pci_driver);
-}
-
-static void __exit k2_sata_exit(void)
-{
-	pci_unregister_driver(&k2_sata_pci_driver);
-}
+module_pci_driver(k2_sata_pci_driver);
 
 MODULE_AUTHOR("Benjamin Herrenschmidt");
 MODULE_DESCRIPTION("low-level driver for K2 SATA controller");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, k2_sata_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(k2_sata_init);
-module_exit(k2_sata_exit);
diff --git a/drivers/ata/sata_sx4.c b/drivers/ata/sata_sx4.c
index cdaebbe3d18..12260559316 100644
--- a/drivers/ata/sata_sx4.c
+++ b/drivers/ata/sata_sx4.c
@@ -1498,24 +1498,10 @@ static int pdc_sata_init_one(struct pci_dev *pdev,
 				 IRQF_SHARED, &pdc_sata_sht);
 }
 
-
-static int __init pdc_sata_init(void)
-{
-	return pci_register_driver(&pdc_sata_pci_driver);
-}
-
-
-static void __exit pdc_sata_exit(void)
-{
-	pci_unregister_driver(&pdc_sata_pci_driver);
-}
-
+module_pci_driver(pdc_sata_pci_driver);
 
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("Promise SATA low-level driver");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, pdc_sata_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(pdc_sata_init);
-module_exit(pdc_sata_exit);
diff --git a/drivers/ata/sata_uli.c b/drivers/ata/sata_uli.c
index b54ebfcdda3..6d648911887 100644
--- a/drivers/ata/sata_uli.c
+++ b/drivers/ata/sata_uli.c
@@ -243,16 +243,4 @@ static int uli_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 				 IRQF_SHARED, &uli_sht);
 }
 
-static int __init uli_init(void)
-{
-	return pci_register_driver(&uli_pci_driver);
-}
-
-static void __exit uli_exit(void)
-{
-	pci_unregister_driver(&uli_pci_driver);
-}
-
-
-module_init(uli_init);
-module_exit(uli_exit);
+module_pci_driver(uli_pci_driver);
diff --git a/drivers/ata/sata_via.c b/drivers/ata/sata_via.c
index f93e43b0ccd..5913ea9d57b 100644
--- a/drivers/ata/sata_via.c
+++ b/drivers/ata/sata_via.c
@@ -655,15 +655,4 @@ static int svia_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 				 IRQF_SHARED, &svia_sht);
 }
 
-static int __init svia_init(void)
-{
-	return pci_register_driver(&svia_pci_driver);
-}
-
-static void __exit svia_exit(void)
-{
-	pci_unregister_driver(&svia_pci_driver);
-}
-
-module_init(svia_init);
-module_exit(svia_exit);
+module_pci_driver(svia_pci_driver);
diff --git a/drivers/ata/sata_vsc.c b/drivers/ata/sata_vsc.c
index 6135a528869..e8cf88ba145 100644
--- a/drivers/ata/sata_vsc.c
+++ b/drivers/ata/sata_vsc.c
@@ -436,21 +436,10 @@ static struct pci_driver vsc_sata_pci_driver = {
 	.remove			= ata_pci_remove_one,
 };
 
-static int __init vsc_sata_init(void)
-{
-	return pci_register_driver(&vsc_sata_pci_driver);
-}
-
-static void __exit vsc_sata_exit(void)
-{
-	pci_unregister_driver(&vsc_sata_pci_driver);
-}
+module_pci_driver(vsc_sata_pci_driver);
 
 MODULE_AUTHOR("Jeremy Higdon");
 MODULE_DESCRIPTION("low-level driver for Vitesse VSC7174 SATA controller");
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, vsc_sata_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
-
-module_init(vsc_sata_init);
-module_exit(vsc_sata_exit);
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 9b21469482a..08b4c520938 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -196,6 +196,7 @@ config CMA
 	bool "Contiguous Memory Allocator (EXPERIMENTAL)"
 	depends on HAVE_DMA_CONTIGUOUS && HAVE_MEMBLOCK && EXPERIMENTAL
 	select MIGRATION
+	select MEMORY_ISOLATION
 	help
 	  This enables the Contiguous Memory Allocator which allows drivers
 	  to allocate big physically-contiguous blocks of memory for use with
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 2bcef657a60..181ed2660b3 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -743,7 +743,6 @@ int bus_add_driver(struct device_driver *drv)
 		}
 	}
 
-	kobject_uevent(&priv->kobj, KOBJ_ADD);
 	return 0;
 
 out_unregister:
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 346be8b78b2..f338037a4f3 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -85,14 +85,13 @@ const char *dev_driver_string(const struct device *dev)
 }
 EXPORT_SYMBOL(dev_driver_string);
 
-#define to_dev(obj) container_of(obj, struct device, kobj)
 #define to_dev_attr(_attr) container_of(_attr, struct device_attribute, attr)
 
 static ssize_t dev_attr_show(struct kobject *kobj, struct attribute *attr,
 			     char *buf)
 {
 	struct device_attribute *dev_attr = to_dev_attr(attr);
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	ssize_t ret = -EIO;
 
 	if (dev_attr->show)
@@ -108,7 +107,7 @@ static ssize_t dev_attr_store(struct kobject *kobj, struct attribute *attr,
 			      const char *buf, size_t count)
 {
 	struct device_attribute *dev_attr = to_dev_attr(attr);
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	ssize_t ret = -EIO;
 
 	if (dev_attr->store)
@@ -182,7 +181,7 @@ EXPORT_SYMBOL_GPL(device_show_int);
  */
 static void device_release(struct kobject *kobj)
 {
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct device_private *p = dev->p;
 
 	if (dev->release)
@@ -200,7 +199,7 @@ static void device_release(struct kobject *kobj)
 
 static const void *device_namespace(struct kobject *kobj)
 {
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	const void *ns = NULL;
 
 	if (dev->class && dev->class->ns_type)
@@ -221,7 +220,7 @@ static int dev_uevent_filter(struct kset *kset, struct kobject *kobj)
 	struct kobj_type *ktype = get_ktype(kobj);
 
 	if (ktype == &device_ktype) {
-		struct device *dev = to_dev(kobj);
+		struct device *dev = kobj_to_dev(kobj);
 		if (dev->bus)
 			return 1;
 		if (dev->class)
@@ -232,7 +231,7 @@ static int dev_uevent_filter(struct kset *kset, struct kobject *kobj)
 
 static const char *dev_uevent_name(struct kset *kset, struct kobject *kobj)
 {
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 
 	if (dev->bus)
 		return dev->bus->name;
@@ -244,7 +243,7 @@ static const char *dev_uevent_name(struct kset *kset, struct kobject *kobj)
 static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		      struct kobj_uevent_env *env)
 {
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	int retval = 0;
 
 	/* add device node properties if present */
@@ -1132,7 +1131,7 @@ int device_register(struct device *dev)
  */
 struct device *get_device(struct device *dev)
 {
-	return dev ? to_dev(kobject_get(&dev->kobj)) : NULL;
+	return dev ? kobj_to_dev(kobject_get(&dev->kobj)) : NULL;
 }
 
 /**
@@ -1754,25 +1753,25 @@ int device_move(struct device *dev, struct device *new_parent,
 		set_dev_node(dev, dev_to_node(new_parent));
 	}
 
-	if (!dev->class)
-		goto out_put;
-	error = device_move_class_links(dev, old_parent, new_parent);
-	if (error) {
-		/* We ignore errors on cleanup since we're hosed anyway... */
-		device_move_class_links(dev, new_parent, old_parent);
-		if (!kobject_move(&dev->kobj, &old_parent->kobj)) {
-			if (new_parent)
-				klist_remove(&dev->p->knode_parent);
-			dev->parent = old_parent;
-			if (old_parent) {
-				klist_add_tail(&dev->p->knode_parent,
-					       &old_parent->p->klist_children);
-				set_dev_node(dev, dev_to_node(old_parent));
+	if (dev->class) {
+		error = device_move_class_links(dev, old_parent, new_parent);
+		if (error) {
+			/* We ignore errors on cleanup since we're hosed anyway... */
+			device_move_class_links(dev, new_parent, old_parent);
+			if (!kobject_move(&dev->kobj, &old_parent->kobj)) {
+				if (new_parent)
+					klist_remove(&dev->p->knode_parent);
+				dev->parent = old_parent;
+				if (old_parent) {
+					klist_add_tail(&dev->p->knode_parent,
+						       &old_parent->p->klist_children);
+					set_dev_node(dev, dev_to_node(old_parent));
+				}
 			}
+			cleanup_glue_dir(dev, new_parent_kobj);
+			put_device(new_parent);
+			goto out;
 		}
-		cleanup_glue_dir(dev, new_parent_kobj);
-		put_device(new_parent);
-		goto out;
 	}
 	switch (dpm_order) {
 	case DPM_ORDER_NONE:
@@ -1787,7 +1786,7 @@ int device_move(struct device *dev, struct device *new_parent,
 		device_pm_move_last(dev);
 		break;
 	}
-out_put:
+
 	put_device(old_parent);
 out:
 	device_pm_unlock();
@@ -1812,6 +1811,13 @@ void device_shutdown(void)
 	while (!list_empty(&devices_kset->list)) {
 		dev = list_entry(devices_kset->list.prev, struct device,
 				kobj.entry);
+
+		/*
+		 * hold reference count of device's parent to
+		 * prevent it from being freed because parent's
+		 * lock is to be held
+		 */
+		get_device(dev->parent);
 		get_device(dev);
 		/*
 		 * Make sure the device is off the kset list, in the
@@ -1820,6 +1826,11 @@ void device_shutdown(void)
 		list_del_init(&dev->kobj.entry);
 		spin_unlock(&devices_kset->list_lock);
 
+		/* hold lock to avoid race with probe/release */
+		if (dev->parent)
+			device_lock(dev->parent);
+		device_lock(dev);
+
 		/* Don't allow any more runtime suspends */
 		pm_runtime_get_noresume(dev);
 		pm_runtime_barrier(dev);
@@ -1831,7 +1842,13 @@ void device_shutdown(void)
 			dev_dbg(dev, "shutdown\n");
 			dev->driver->shutdown(dev);
 		}
+
+		device_unlock(dev);
+		if (dev->parent)
+			device_unlock(dev->parent);
+
 		put_device(dev);
+		put_device(dev->parent);
 
 		spin_lock(&devices_kset->list_lock);
 	}
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index dcb8a6e4869..e3bbed8a617 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -85,8 +85,20 @@ static void deferred_probe_work_func(struct work_struct *work)
 		 * manipulate the deferred list
 		 */
 		mutex_unlock(&deferred_probe_mutex);
+
+		/*
+		 * Force the device to the end of the dpm_list since
+		 * the PM code assumes that the order we add things to
+		 * the list is a good order for suspend but deferred
+		 * probe makes that very unsafe.
+		 */
+		device_pm_lock();
+		device_pm_move_last(dev);
+		device_pm_unlock();
+
 		dev_dbg(dev, "Retrying from deferred list\n");
 		bus_probe_device(dev);
+
 		mutex_lock(&deferred_probe_mutex);
 
 		put_device(dev);
@@ -283,6 +295,7 @@ probe_failed:
 	devres_release_all(dev);
 	driver_sysfs_remove(dev);
 	dev->driver = NULL;
+	dev_set_drvdata(dev, NULL);
 
 	if (ret == -EPROBE_DEFER) {
 		/* Driver requested deferred probing */
@@ -356,10 +369,9 @@ int driver_probe_device(struct device_driver *drv, struct device *dev)
 	pr_debug("bus: '%s': %s: matched device %s with driver %s\n",
 		 drv->bus->name, __func__, dev_name(dev), drv->name);
 
-	pm_runtime_get_noresume(dev);
 	pm_runtime_barrier(dev);
 	ret = really_probe(dev, drv);
-	pm_runtime_put_sync(dev);
+	pm_runtime_idle(dev);
 
 	return ret;
 }
@@ -406,9 +418,8 @@ int device_attach(struct device *dev)
 			ret = 0;
 		}
 	} else {
-		pm_runtime_get_noresume(dev);
 		ret = bus_for_each_drv(dev->bus, NULL, dev, __device_attach);
-		pm_runtime_put_sync(dev);
+		pm_runtime_idle(dev);
 	}
 out_unlock:
 	device_unlock(dev);
@@ -487,6 +498,7 @@ static void __device_release_driver(struct device *dev)
 			drv->remove(dev);
 		devres_release_all(dev);
 		dev->driver = NULL;
+		dev_set_drvdata(dev, NULL);
 		klist_remove(&dev->p->knode_driver);
 		if (dev->bus)
 			blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
diff --git a/drivers/base/devtmpfs.c b/drivers/base/devtmpfs.c
index 765c3a28077..deb4a456cf8 100644
--- a/drivers/base/devtmpfs.c
+++ b/drivers/base/devtmpfs.c
@@ -156,9 +156,7 @@ static int dev_mkdir(const char *name, umode_t mode)
 	if (!err)
 		/* mark as kernel-created inode */
 		dentry->d_inode->i_private = &thread;
-	dput(dentry);
-	mutex_unlock(&path.dentry->d_inode->i_mutex);
-	path_put(&path);
+	done_path_create(&path, dentry);
 	return err;
 }
 
@@ -218,42 +216,30 @@ static int handle_create(const char *nodename, umode_t mode, struct device *dev)
 		/* mark as kernel-created inode */
 		dentry->d_inode->i_private = &thread;
 	}
-	dput(dentry);
-
-	mutex_unlock(&path.dentry->d_inode->i_mutex);
-	path_put(&path);
+	done_path_create(&path, dentry);
 	return err;
 }
 
 static int dev_rmdir(const char *name)
 {
-	struct nameidata nd;
+	struct path parent;
 	struct dentry *dentry;
 	int err;
 
-	err = kern_path_parent(name, &nd);
-	if (err)
-		return err;
-
-	mutex_lock_nested(&nd.path.dentry->d_inode->i_mutex, I_MUTEX_PARENT);
-	dentry = lookup_one_len(nd.last.name, nd.path.dentry, nd.last.len);
-	if (!IS_ERR(dentry)) {
-		if (dentry->d_inode) {
-			if (dentry->d_inode->i_private == &thread)
-				err = vfs_rmdir(nd.path.dentry->d_inode,
-						dentry);
-			else
-				err = -EPERM;
-		} else {
-			err = -ENOENT;
-		}
-		dput(dentry);
+	dentry = kern_path_locked(name, &parent);
+	if (IS_ERR(dentry))
+		return PTR_ERR(dentry);
+	if (dentry->d_inode) {
+		if (dentry->d_inode->i_private == &thread)
+			err = vfs_rmdir(parent.dentry->d_inode, dentry);
+		else
+			err = -EPERM;
 	} else {
-		err = PTR_ERR(dentry);
+		err = -ENOENT;
 	}
-
-	mutex_unlock(&nd.path.dentry->d_inode->i_mutex);
-	path_put(&nd.path);
+	dput(dentry);
+	mutex_unlock(&parent.dentry->d_inode->i_mutex);
+	path_put(&parent);
 	return err;
 }
 
@@ -305,50 +291,43 @@ static int dev_mynode(struct device *dev, struct inode *inode, struct kstat *sta
 
 static int handle_remove(const char *nodename, struct device *dev)
 {
-	struct nameidata nd;
+	struct path parent;
 	struct dentry *dentry;
-	struct kstat stat;
 	int deleted = 1;
 	int err;
 
-	err = kern_path_parent(nodename, &nd);
-	if (err)
-		return err;
+	dentry = kern_path_locked(nodename, &parent);
+	if (IS_ERR(dentry))
+		return PTR_ERR(dentry);
 
-	mutex_lock_nested(&nd.path.dentry->d_inode->i_mutex, I_MUTEX_PARENT);
-	dentry = lookup_one_len(nd.last.name, nd.path.dentry, nd.last.len);
-	if (!IS_ERR(dentry)) {
-		if (dentry->d_inode) {
-			err = vfs_getattr(nd.path.mnt, dentry, &stat);
-			if (!err && dev_mynode(dev, dentry->d_inode, &stat)) {
-				struct iattr newattrs;
-				/*
-				 * before unlinking this node, reset permissions
-				 * of possible references like hardlinks
-				 */
-				newattrs.ia_uid = 0;
-				newattrs.ia_gid = 0;
-				newattrs.ia_mode = stat.mode & ~0777;
-				newattrs.ia_valid =
-					ATTR_UID|ATTR_GID|ATTR_MODE;
-				mutex_lock(&dentry->d_inode->i_mutex);
-				notify_change(dentry, &newattrs);
-				mutex_unlock(&dentry->d_inode->i_mutex);
-				err = vfs_unlink(nd.path.dentry->d_inode,
-						 dentry);
-				if (!err || err == -ENOENT)
-					deleted = 1;
-			}
-		} else {
-			err = -ENOENT;
+	if (dentry->d_inode) {
+		struct kstat stat;
+		err = vfs_getattr(parent.mnt, dentry, &stat);
+		if (!err && dev_mynode(dev, dentry->d_inode, &stat)) {
+			struct iattr newattrs;
+			/*
+			 * before unlinking this node, reset permissions
+			 * of possible references like hardlinks
+			 */
+			newattrs.ia_uid = 0;
+			newattrs.ia_gid = 0;
+			newattrs.ia_mode = stat.mode & ~0777;
+			newattrs.ia_valid =
+				ATTR_UID|ATTR_GID|ATTR_MODE;
+			mutex_lock(&dentry->d_inode->i_mutex);
+			notify_change(dentry, &newattrs);
+			mutex_unlock(&dentry->d_inode->i_mutex);
+			err = vfs_unlink(parent.dentry->d_inode, dentry);
+			if (!err || err == -ENOENT)
+				deleted = 1;
 		}
-		dput(dentry);
 	} else {
-		err = PTR_ERR(dentry);
+		err = -ENOENT;
 	}
-	mutex_unlock(&nd.path.dentry->d_inode->i_mutex);
+	dput(dentry);
+	mutex_unlock(&parent.dentry->d_inode->i_mutex);
 
-	path_put(&nd.path);
+	path_put(&parent);
 	if (deleted && strchr(nodename, '/'))
 		delete_path(nodename);
 	return err;
diff --git a/drivers/base/dma-buf.c b/drivers/base/dma-buf.c
index 24e88fe29ec..c30f3e1d0ef 100644
--- a/drivers/base/dma-buf.c
+++ b/drivers/base/dma-buf.c
@@ -493,6 +493,7 @@ EXPORT_SYMBOL_GPL(dma_buf_vmap);
 /**
  * dma_buf_vunmap - Unmap a vmap obtained by dma_buf_vmap.
  * @dmabuf:	[in]	buffer to vunmap
+ * @vaddr:	[in]	vmap to vunmap
  */
 void dma_buf_vunmap(struct dma_buf *dmabuf, void *vaddr)
 {
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index 1b85949e3d2..560a7173f81 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -186,6 +186,7 @@ EXPORT_SYMBOL(dma_release_from_coherent);
  * @vma:	vm_area for the userspace memory
  * @vaddr:	cpu address returned by dma_alloc_from_coherent
  * @size:	size of the memory buffer allocated by dma_alloc_from_coherent
+ * @ret:	result from remap_pfn_range()
  *
  * This checks whether the memory was allocated from the per-device
  * coherent memory pool and if so, maps that memory to the provided vma.
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index 6f3676f1559..3fbedc75e7c 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -10,6 +10,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/export.h>
 #include <linux/gfp.h>
+#include <asm-generic/dma-coherent.h>
 
 /*
  * Managed DMA API
@@ -217,4 +218,52 @@ void dmam_release_declared_memory(struct device *dev)
 }
 EXPORT_SYMBOL(dmam_release_declared_memory);
 
+/*
+ * Create scatter-list for the already allocated DMA buffer.
+ */
+int dma_common_get_sgtable(struct device *dev, struct sg_table *sgt,
+		 void *cpu_addr, dma_addr_t handle, size_t size)
+{
+	struct page *page = virt_to_page(cpu_addr);
+	int ret;
+
+	ret = sg_alloc_table(sgt, 1, GFP_KERNEL);
+	if (unlikely(ret))
+		return ret;
+
+	sg_set_page(sgt->sgl, page, PAGE_ALIGN(size), 0);
+	return 0;
+}
+EXPORT_SYMBOL(dma_common_get_sgtable);
+
 #endif
+
+/*
+ * Create userspace mapping for the DMA-coherent memory.
+ */
+int dma_common_mmap(struct device *dev, struct vm_area_struct *vma,
+		    void *cpu_addr, dma_addr_t dma_addr, size_t size)
+{
+	int ret = -ENXIO;
+#ifdef CONFIG_MMU
+	unsigned long user_count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	unsigned long count = PAGE_ALIGN(size) >> PAGE_SHIFT;
+	unsigned long pfn = page_to_pfn(virt_to_page(cpu_addr));
+	unsigned long off = vma->vm_pgoff;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (dma_mmap_from_coherent(dev, vma, cpu_addr, size, &ret))
+		return ret;
+
+	if (off < count && user_count <= (count - off)) {
+		ret = remap_pfn_range(vma, vma->vm_start,
+				      pfn + off,
+				      user_count << PAGE_SHIFT,
+				      vma->vm_page_prot);
+	}
+#endif	/* CONFIG_MMU */
+
+	return ret;
+}
+EXPORT_SYMBOL(dma_common_mmap);
diff --git a/drivers/base/driver.c b/drivers/base/driver.c
index 207c27ddf82..974e301a1ef 100644
--- a/drivers/base/driver.c
+++ b/drivers/base/driver.c
@@ -185,8 +185,12 @@ int driver_register(struct device_driver *drv)
 	if (ret)
 		return ret;
 	ret = driver_add_groups(drv, drv->groups);
-	if (ret)
+	if (ret) {
 		bus_remove_driver(drv);
+		return ret;
+	}
+	kobject_uevent(&drv->p->kobj, KOBJ_ADD);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(driver_register);
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 5401814c874..803cfc1597a 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -22,8 +22,6 @@
 #include <linux/slab.h>
 #include <linux/sched.h>
 
-#define to_dev(obj) container_of(obj, struct device, kobj)
-
 MODULE_AUTHOR("Manuel Estrada Sainz");
 MODULE_DESCRIPTION("Multi purpose firmware loading support");
 MODULE_LICENSE("GPL");
@@ -290,7 +288,7 @@ static ssize_t firmware_data_read(struct file *filp, struct kobject *kobj,
 				  struct bin_attribute *bin_attr,
 				  char *buffer, loff_t offset, size_t count)
 {
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
 	struct firmware *fw;
 	ssize_t ret_count;
@@ -384,7 +382,7 @@ static ssize_t firmware_data_write(struct file *filp, struct kobject *kobj,
 				   struct bin_attribute *bin_attr,
 				   char *buffer, loff_t offset, size_t count)
 {
-	struct device *dev = to_dev(kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct firmware_priv *fw_priv = to_firmware_priv(dev);
 	struct firmware *fw;
 	ssize_t retval;
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 83aa694a8ef..ba3487c9835 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -75,19 +75,6 @@ static int genpd_start_dev(struct generic_pm_domain *genpd, struct device *dev)
 					start_latency_ns, "start");
 }
 
-static int genpd_save_dev(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_TIMED_CALLBACK(genpd, int, save_state, dev,
-					save_state_latency_ns, "state save");
-}
-
-static int genpd_restore_dev(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_TIMED_CALLBACK(genpd, int, restore_state, dev,
-					restore_state_latency_ns,
-					"state restore");
-}
-
 static bool genpd_sd_counter_dec(struct generic_pm_domain *genpd)
 {
 	bool ret = false;
@@ -139,6 +126,19 @@ static void genpd_set_active(struct generic_pm_domain *genpd)
 		genpd->status = GPD_STATE_ACTIVE;
 }
 
+static void genpd_recalc_cpu_exit_latency(struct generic_pm_domain *genpd)
+{
+	s64 usecs64;
+
+	if (!genpd->cpu_data)
+		return;
+
+	usecs64 = genpd->power_on_latency_ns;
+	do_div(usecs64, NSEC_PER_USEC);
+	usecs64 += genpd->cpu_data->saved_exit_latency;
+	genpd->cpu_data->idle_state->exit_latency = usecs64;
+}
+
 /**
  * __pm_genpd_poweron - Restore power to a given PM domain and its masters.
  * @genpd: PM domain to power up.
@@ -146,7 +146,7 @@ static void genpd_set_active(struct generic_pm_domain *genpd)
  * Restore power to @genpd and all of its masters so that it is possible to
  * resume a device belonging to it.
  */
-int __pm_genpd_poweron(struct generic_pm_domain *genpd)
+static int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 	__releases(&genpd->lock) __acquires(&genpd->lock)
 {
 	struct gpd_link *link;
@@ -176,6 +176,13 @@ int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 		return 0;
 	}
 
+	if (genpd->cpu_data) {
+		cpuidle_pause_and_lock();
+		genpd->cpu_data->idle_state->disabled = true;
+		cpuidle_resume_and_unlock();
+		goto out;
+	}
+
 	/*
 	 * The list is guaranteed not to change while the loop below is being
 	 * executed, unless one of the masters' .power_on() callbacks fiddles
@@ -215,6 +222,7 @@ int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 		if (elapsed_ns > genpd->power_on_latency_ns) {
 			genpd->power_on_latency_ns = elapsed_ns;
 			genpd->max_off_time_changed = true;
+			genpd_recalc_cpu_exit_latency(genpd);
 			if (genpd->name)
 				pr_warning("%s: Power-on latency exceeded, "
 					"new value %lld ns\n", genpd->name,
@@ -222,6 +230,7 @@ int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 		}
 	}
 
+ out:
 	genpd_set_active(genpd);
 
 	return 0;
@@ -251,6 +260,19 @@ int pm_genpd_poweron(struct generic_pm_domain *genpd)
 
 #ifdef CONFIG_PM_RUNTIME
 
+static int genpd_save_dev(struct generic_pm_domain *genpd, struct device *dev)
+{
+	return GENPD_DEV_TIMED_CALLBACK(genpd, int, save_state, dev,
+					save_state_latency_ns, "state save");
+}
+
+static int genpd_restore_dev(struct generic_pm_domain *genpd, struct device *dev)
+{
+	return GENPD_DEV_TIMED_CALLBACK(genpd, int, restore_state, dev,
+					restore_state_latency_ns,
+					"state restore");
+}
+
 static int genpd_dev_pm_qos_notifier(struct notifier_block *nb,
 				     unsigned long val, void *ptr)
 {
@@ -275,7 +297,7 @@ static int genpd_dev_pm_qos_notifier(struct notifier_block *nb,
 
 		pdd = dev->power.subsys_data ?
 				dev->power.subsys_data->domain_data : NULL;
-		if (pdd) {
+		if (pdd && pdd->dev) {
 			to_gpd_data(pdd)->td.constraint_changed = true;
 			genpd = dev_to_genpd(dev);
 		} else {
@@ -339,19 +361,16 @@ static void __pm_genpd_restore_device(struct pm_domain_data *pdd,
 {
 	struct generic_pm_domain_data *gpd_data = to_gpd_data(pdd);
 	struct device *dev = pdd->dev;
+	bool need_restore = gpd_data->need_restore;
 
-	if (!gpd_data->need_restore)
-		return;
-
+	gpd_data->need_restore = false;
 	mutex_unlock(&genpd->lock);
 
 	genpd_start_dev(genpd, dev);
-	genpd_restore_dev(genpd, dev);
-	genpd_stop_dev(genpd, dev);
+	if (need_restore)
+		genpd_restore_dev(genpd, dev);
 
 	mutex_lock(&genpd->lock);
-
-	gpd_data->need_restore = false;
 }
 
 /**
@@ -458,6 +477,21 @@ static int pm_genpd_poweroff(struct generic_pm_domain *genpd)
 		}
 	}
 
+	if (genpd->cpu_data) {
+		/*
+		 * If cpu_data is set, cpuidle should turn the domain off when
+		 * the CPU in it is idle.  In that case we don't decrement the
+		 * subdomain counts of the master domains, so that power is not
+		 * removed from the current domain prematurely as a result of
+		 * cutting off the masters' power.
+		 */
+		genpd->status = GPD_STATE_POWER_OFF;
+		cpuidle_pause_and_lock();
+		genpd->cpu_data->idle_state->disabled = false;
+		cpuidle_resume_and_unlock();
+		goto out;
+	}
+
 	if (genpd->power_off) {
 		ktime_t time_start;
 		s64 elapsed_ns;
@@ -595,7 +629,7 @@ static int pm_genpd_runtime_resume(struct device *dev)
 
 	/* If power.irq_safe, the PM domain is never powered off. */
 	if (dev->power.irq_safe)
-		goto out;
+		return genpd_start_dev(genpd, dev);
 
 	mutex_lock(&genpd->lock);
 	ret = __pm_genpd_poweron(genpd);
@@ -628,9 +662,6 @@ static int pm_genpd_runtime_resume(struct device *dev)
 	wake_up_all(&genpd->status_wait_queue);
 	mutex_unlock(&genpd->lock);
 
- out:
-	genpd_start_dev(genpd, dev);
-
 	return 0;
 }
 
@@ -1235,6 +1266,27 @@ static void pm_genpd_complete(struct device *dev)
 
 #endif /* CONFIG_PM_SLEEP */
 
+static struct generic_pm_domain_data *__pm_genpd_alloc_dev_data(struct device *dev)
+{
+	struct generic_pm_domain_data *gpd_data;
+
+	gpd_data = kzalloc(sizeof(*gpd_data), GFP_KERNEL);
+	if (!gpd_data)
+		return NULL;
+
+	mutex_init(&gpd_data->lock);
+	gpd_data->nb.notifier_call = genpd_dev_pm_qos_notifier;
+	dev_pm_qos_add_notifier(dev, &gpd_data->nb);
+	return gpd_data;
+}
+
+static void __pm_genpd_free_dev_data(struct device *dev,
+				     struct generic_pm_domain_data *gpd_data)
+{
+	dev_pm_qos_remove_notifier(dev, &gpd_data->nb);
+	kfree(gpd_data);
+}
+
 /**
  * __pm_genpd_add_device - Add a device to an I/O PM domain.
  * @genpd: PM domain to add the device to.
@@ -1244,7 +1296,7 @@ static void pm_genpd_complete(struct device *dev)
 int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 			  struct gpd_timing_data *td)
 {
-	struct generic_pm_domain_data *gpd_data;
+	struct generic_pm_domain_data *gpd_data_new, *gpd_data = NULL;
 	struct pm_domain_data *pdd;
 	int ret = 0;
 
@@ -1253,14 +1305,10 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(dev))
 		return -EINVAL;
 
-	gpd_data = kzalloc(sizeof(*gpd_data), GFP_KERNEL);
-	if (!gpd_data)
+	gpd_data_new = __pm_genpd_alloc_dev_data(dev);
+	if (!gpd_data_new)
 		return -ENOMEM;
 
-	mutex_init(&gpd_data->lock);
-	gpd_data->nb.notifier_call = genpd_dev_pm_qos_notifier;
-	dev_pm_qos_add_notifier(dev, &gpd_data->nb);
-
 	genpd_acquire_lock(genpd);
 
 	if (genpd->prepared_count > 0) {
@@ -1274,35 +1322,42 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 			goto out;
 		}
 
+	ret = dev_pm_get_subsys_data(dev);
+	if (ret)
+		goto out;
+
 	genpd->device_count++;
 	genpd->max_off_time_changed = true;
 
-	dev_pm_get_subsys_data(dev);
-
-	mutex_lock(&gpd_data->lock);
 	spin_lock_irq(&dev->power.lock);
+
 	dev->pm_domain = &genpd->domain;
-	dev->power.subsys_data->domain_data = &gpd_data->base;
-	gpd_data->base.dev = dev;
-	list_add_tail(&gpd_data->base.list_node, &genpd->dev_list);
-	gpd_data->need_restore = genpd->status == GPD_STATE_POWER_OFF;
+	if (dev->power.subsys_data->domain_data) {
+		gpd_data = to_gpd_data(dev->power.subsys_data->domain_data);
+	} else {
+		gpd_data = gpd_data_new;
+		dev->power.subsys_data->domain_data = &gpd_data->base;
+	}
+	gpd_data->refcount++;
 	if (td)
 		gpd_data->td = *td;
 
+	spin_unlock_irq(&dev->power.lock);
+
+	mutex_lock(&gpd_data->lock);
+	gpd_data->base.dev = dev;
+	list_add_tail(&gpd_data->base.list_node, &genpd->dev_list);
+	gpd_data->need_restore = genpd->status == GPD_STATE_POWER_OFF;
 	gpd_data->td.constraint_changed = true;
 	gpd_data->td.effective_constraint_ns = -1;
-	spin_unlock_irq(&dev->power.lock);
 	mutex_unlock(&gpd_data->lock);
 
-	genpd_release_lock(genpd);
-
-	return 0;
-
  out:
 	genpd_release_lock(genpd);
 
-	dev_pm_qos_remove_notifier(dev, &gpd_data->nb);
-	kfree(gpd_data);
+	if (gpd_data != gpd_data_new)
+		__pm_genpd_free_dev_data(dev, gpd_data_new);
+
 	return ret;
 }
 
@@ -1348,6 +1403,7 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 {
 	struct generic_pm_domain_data *gpd_data;
 	struct pm_domain_data *pdd;
+	bool remove = false;
 	int ret = 0;
 
 	dev_dbg(dev, "%s()\n", __func__);
@@ -1368,22 +1424,28 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 	genpd->max_off_time_changed = true;
 
 	spin_lock_irq(&dev->power.lock);
+
 	dev->pm_domain = NULL;
 	pdd = dev->power.subsys_data->domain_data;
 	list_del_init(&pdd->list_node);
-	dev->power.subsys_data->domain_data = NULL;
+	gpd_data = to_gpd_data(pdd);
+	if (--gpd_data->refcount == 0) {
+		dev->power.subsys_data->domain_data = NULL;
+		remove = true;
+	}
+
 	spin_unlock_irq(&dev->power.lock);
 
-	gpd_data = to_gpd_data(pdd);
 	mutex_lock(&gpd_data->lock);
 	pdd->dev = NULL;
 	mutex_unlock(&gpd_data->lock);
 
 	genpd_release_lock(genpd);
 
-	dev_pm_qos_remove_notifier(dev, &gpd_data->nb);
-	kfree(gpd_data);
 	dev_pm_put_subsys_data(dev);
+	if (remove)
+		__pm_genpd_free_dev_data(dev, gpd_data);
+
 	return 0;
 
  out:
@@ -1541,33 +1603,52 @@ int pm_genpd_remove_subdomain(struct generic_pm_domain *genpd,
  * @dev: Device to add the callbacks to.
  * @ops: Set of callbacks to add.
  * @td: Timing data to add to the device along with the callbacks (optional).
+ *
+ * Every call to this routine should be balanced with a call to
+ * __pm_genpd_remove_callbacks() and they must not be nested.
  */
 int pm_genpd_add_callbacks(struct device *dev, struct gpd_dev_ops *ops,
 			   struct gpd_timing_data *td)
 {
-	struct pm_domain_data *pdd;
+	struct generic_pm_domain_data *gpd_data_new, *gpd_data = NULL;
 	int ret = 0;
 
-	if (!(dev && dev->power.subsys_data && ops))
+	if (!(dev && ops))
 		return -EINVAL;
 
+	gpd_data_new = __pm_genpd_alloc_dev_data(dev);
+	if (!gpd_data_new)
+		return -ENOMEM;
+
 	pm_runtime_disable(dev);
 	device_pm_lock();
 
-	pdd = dev->power.subsys_data->domain_data;
-	if (pdd) {
-		struct generic_pm_domain_data *gpd_data = to_gpd_data(pdd);
+	ret = dev_pm_get_subsys_data(dev);
+	if (ret)
+		goto out;
 
-		gpd_data->ops = *ops;
-		if (td)
-			gpd_data->td = *td;
+	spin_lock_irq(&dev->power.lock);
+
+	if (dev->power.subsys_data->domain_data) {
+		gpd_data = to_gpd_data(dev->power.subsys_data->domain_data);
 	} else {
-		ret = -EINVAL;
+		gpd_data = gpd_data_new;
+		dev->power.subsys_data->domain_data = &gpd_data->base;
 	}
+	gpd_data->refcount++;
+	gpd_data->ops = *ops;
+	if (td)
+		gpd_data->td = *td;
+
+	spin_unlock_irq(&dev->power.lock);
 
+ out:
 	device_pm_unlock();
 	pm_runtime_enable(dev);
 
+	if (gpd_data != gpd_data_new)
+		__pm_genpd_free_dev_data(dev, gpd_data_new);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(pm_genpd_add_callbacks);
@@ -1576,10 +1657,13 @@ EXPORT_SYMBOL_GPL(pm_genpd_add_callbacks);
  * __pm_genpd_remove_callbacks - Remove PM domain callbacks from a given device.
  * @dev: Device to remove the callbacks from.
  * @clear_td: If set, clear the device's timing data too.
+ *
+ * This routine can only be called after pm_genpd_add_callbacks().
  */
 int __pm_genpd_remove_callbacks(struct device *dev, bool clear_td)
 {
-	struct pm_domain_data *pdd;
+	struct generic_pm_domain_data *gpd_data = NULL;
+	bool remove = false;
 	int ret = 0;
 
 	if (!(dev && dev->power.subsys_data))
@@ -1588,24 +1672,118 @@ int __pm_genpd_remove_callbacks(struct device *dev, bool clear_td)
 	pm_runtime_disable(dev);
 	device_pm_lock();
 
-	pdd = dev->power.subsys_data->domain_data;
-	if (pdd) {
-		struct generic_pm_domain_data *gpd_data = to_gpd_data(pdd);
+	spin_lock_irq(&dev->power.lock);
 
-		gpd_data->ops = (struct gpd_dev_ops){ 0 };
+	if (dev->power.subsys_data->domain_data) {
+		gpd_data = to_gpd_data(dev->power.subsys_data->domain_data);
+		gpd_data->ops = (struct gpd_dev_ops){ NULL };
 		if (clear_td)
 			gpd_data->td = (struct gpd_timing_data){ 0 };
+
+		if (--gpd_data->refcount == 0) {
+			dev->power.subsys_data->domain_data = NULL;
+			remove = true;
+		}
 	} else {
 		ret = -EINVAL;
 	}
 
+	spin_unlock_irq(&dev->power.lock);
+
 	device_pm_unlock();
 	pm_runtime_enable(dev);
 
-	return ret;
+	if (ret)
+		return ret;
+
+	dev_pm_put_subsys_data(dev);
+	if (remove)
+		__pm_genpd_free_dev_data(dev, gpd_data);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(__pm_genpd_remove_callbacks);
 
+int genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
+{
+	struct cpuidle_driver *cpuidle_drv;
+	struct gpd_cpu_data *cpu_data;
+	struct cpuidle_state *idle_state;
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(genpd) || state < 0)
+		return -EINVAL;
+
+	genpd_acquire_lock(genpd);
+
+	if (genpd->cpu_data) {
+		ret = -EEXIST;
+		goto out;
+	}
+	cpu_data = kzalloc(sizeof(*cpu_data), GFP_KERNEL);
+	if (!cpu_data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	cpuidle_drv = cpuidle_driver_ref();
+	if (!cpuidle_drv) {
+		ret = -ENODEV;
+		goto out;
+	}
+	if (cpuidle_drv->state_count <= state) {
+		ret = -EINVAL;
+		goto err;
+	}
+	idle_state = &cpuidle_drv->states[state];
+	if (!idle_state->disabled) {
+		ret = -EAGAIN;
+		goto err;
+	}
+	cpu_data->idle_state = idle_state;
+	cpu_data->saved_exit_latency = idle_state->exit_latency;
+	genpd->cpu_data = cpu_data;
+	genpd_recalc_cpu_exit_latency(genpd);
+
+ out:
+	genpd_release_lock(genpd);
+	return ret;
+
+ err:
+	cpuidle_driver_unref();
+	goto out;
+}
+
+int genpd_detach_cpuidle(struct generic_pm_domain *genpd)
+{
+	struct gpd_cpu_data *cpu_data;
+	struct cpuidle_state *idle_state;
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(genpd))
+		return -EINVAL;
+
+	genpd_acquire_lock(genpd);
+
+	cpu_data = genpd->cpu_data;
+	if (!cpu_data) {
+		ret = -ENODEV;
+		goto out;
+	}
+	idle_state = cpu_data->idle_state;
+	if (!idle_state->disabled) {
+		ret = -EAGAIN;
+		goto out;
+	}
+	idle_state->exit_latency = cpu_data->saved_exit_latency;
+	cpuidle_driver_unref();
+	genpd->cpu_data = NULL;
+	kfree(cpu_data);
+
+ out:
+	genpd_release_lock(genpd);
+	return ret;
+}
+
 /* Default device callbacks for generic PM domains. */
 
 /**
@@ -1615,16 +1793,24 @@ EXPORT_SYMBOL_GPL(__pm_genpd_remove_callbacks);
 static int pm_genpd_default_save_state(struct device *dev)
 {
 	int (*cb)(struct device *__dev);
-	struct device_driver *drv = dev->driver;
 
 	cb = dev_gpd_data(dev)->ops.save_state;
 	if (cb)
 		return cb(dev);
 
-	if (drv && drv->pm && drv->pm->runtime_suspend)
-		return drv->pm->runtime_suspend(dev);
+	if (dev->type && dev->type->pm)
+		cb = dev->type->pm->runtime_suspend;
+	else if (dev->class && dev->class->pm)
+		cb = dev->class->pm->runtime_suspend;
+	else if (dev->bus && dev->bus->pm)
+		cb = dev->bus->pm->runtime_suspend;
+	else
+		cb = NULL;
 
-	return 0;
+	if (!cb && dev->driver && dev->driver->pm)
+		cb = dev->driver->pm->runtime_suspend;
+
+	return cb ? cb(dev) : 0;
 }
 
 /**
@@ -1634,16 +1820,24 @@ static int pm_genpd_default_save_state(struct device *dev)
 static int pm_genpd_default_restore_state(struct device *dev)
 {
 	int (*cb)(struct device *__dev);
-	struct device_driver *drv = dev->driver;
 
 	cb = dev_gpd_data(dev)->ops.restore_state;
 	if (cb)
 		return cb(dev);
 
-	if (drv && drv->pm && drv->pm->runtime_resume)
-		return drv->pm->runtime_resume(dev);
+	if (dev->type && dev->type->pm)
+		cb = dev->type->pm->runtime_resume;
+	else if (dev->class && dev->class->pm)
+		cb = dev->class->pm->runtime_resume;
+	else if (dev->bus && dev->bus->pm)
+		cb = dev->bus->pm->runtime_resume;
+	else
+		cb = NULL;
 
-	return 0;
+	if (!cb && dev->driver && dev->driver->pm)
+		cb = dev->driver->pm->runtime_resume;
+
+	return cb ? cb(dev) : 0;
 }
 
 #ifdef CONFIG_PM_SLEEP
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 9cb845e4933..0113adc310d 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -28,7 +28,7 @@
 #include <linux/sched.h>
 #include <linux/async.h>
 #include <linux/suspend.h>
-
+#include <linux/cpuidle.h>
 #include "../base.h"
 #include "power.h"
 
@@ -45,10 +45,10 @@ typedef int (*pm_callback_t)(struct device *);
  */
 
 LIST_HEAD(dpm_list);
-LIST_HEAD(dpm_prepared_list);
-LIST_HEAD(dpm_suspended_list);
-LIST_HEAD(dpm_late_early_list);
-LIST_HEAD(dpm_noirq_list);
+static LIST_HEAD(dpm_prepared_list);
+static LIST_HEAD(dpm_suspended_list);
+static LIST_HEAD(dpm_late_early_list);
+static LIST_HEAD(dpm_noirq_list);
 
 struct suspend_stats suspend_stats;
 static DEFINE_MUTEX(dpm_list_mtx);
@@ -166,7 +166,7 @@ static ktime_t initcall_debug_start(struct device *dev)
 {
 	ktime_t calltime = ktime_set(0, 0);
 
-	if (initcall_debug) {
+	if (pm_print_times_enabled) {
 		pr_info("calling  %s+ @ %i, parent: %s\n",
 			dev_name(dev), task_pid_nr(current),
 			dev->parent ? dev_name(dev->parent) : "none");
@@ -181,7 +181,7 @@ static void initcall_debug_report(struct device *dev, ktime_t calltime,
 {
 	ktime_t delta, rettime;
 
-	if (initcall_debug) {
+	if (pm_print_times_enabled) {
 		rettime = ktime_get();
 		delta = ktime_sub(rettime, calltime);
 		pr_info("call %s+ returned %d after %Ld usecs\n", dev_name(dev),
@@ -467,6 +467,7 @@ static void dpm_resume_noirq(pm_message_t state)
 	mutex_unlock(&dpm_list_mtx);
 	dpm_show_time(starttime, state, "noirq");
 	resume_device_irqs();
+	cpuidle_resume();
 }
 
 /**
@@ -867,6 +868,7 @@ static int dpm_suspend_noirq(pm_message_t state)
 	ktime_t starttime = ktime_get();
 	int error = 0;
 
+	cpuidle_pause();
 	suspend_device_irqs();
 	mutex_lock(&dpm_list_mtx);
 	while (!list_empty(&dpm_late_early_list)) {
@@ -989,8 +991,16 @@ static int dpm_suspend_late(pm_message_t state)
 int dpm_suspend_end(pm_message_t state)
 {
 	int error = dpm_suspend_late(state);
+	if (error)
+		return error;
+
+	error = dpm_suspend_noirq(state);
+	if (error) {
+		dpm_resume_early(state);
+		return error;
+	}
 
-	return error ? : dpm_suspend_noirq(state);
+	return 0;
 }
 EXPORT_SYMBOL_GPL(dpm_suspend_end);
 
diff --git a/drivers/base/power/qos.c b/drivers/base/power/qos.c
index fd849a2c4fa..74a67e0019a 100644
--- a/drivers/base/power/qos.c
+++ b/drivers/base/power/qos.c
@@ -462,7 +462,7 @@ EXPORT_SYMBOL_GPL(dev_pm_qos_add_ancestor_request);
 static void __dev_pm_qos_drop_user_request(struct device *dev)
 {
 	dev_pm_qos_remove_request(dev->power.pq_req);
-	dev->power.pq_req = 0;
+	dev->power.pq_req = NULL;
 }
 
 /**
diff --git a/drivers/base/power/sysfs.c b/drivers/base/power/sysfs.c
index 48be2ad4dd2..b91dc6f1e91 100644
--- a/drivers/base/power/sysfs.c
+++ b/drivers/base/power/sysfs.c
@@ -474,6 +474,8 @@ static DEVICE_ATTR(runtime_enabled, 0444, rtpm_enabled_show, NULL);
 
 #endif
 
+#ifdef CONFIG_PM_SLEEP
+
 static ssize_t async_show(struct device *dev, struct device_attribute *attr,
 			  char *buf)
 {
@@ -500,6 +502,8 @@ static ssize_t async_store(struct device *dev, struct device_attribute *attr,
 }
 
 static DEVICE_ATTR(async, 0644, async_show, async_store);
+
+#endif
 #endif /* CONFIG_PM_ADVANCED_DEBUG */
 
 static struct attribute *power_attrs[] = {
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index b986b8660b0..80f9ab9c3aa 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -95,6 +95,9 @@ struct regmap {
 
 	/* if set, converts bulk rw to single rw */
 	bool use_single_rw;
+
+	struct rb_root range_tree;
+	void *selector_work_buf;	/* Scratch buffer used for selector */
 };
 
 struct regcache_ops {
@@ -115,6 +118,20 @@ bool regmap_precious(struct regmap *map, unsigned int reg);
 int _regmap_write(struct regmap *map, unsigned int reg,
 		  unsigned int val);
 
+struct regmap_range_node {
+	struct rb_node node;
+
+	unsigned int range_min;
+	unsigned int range_max;
+
+	unsigned int selector_reg;
+	unsigned int selector_mask;
+	int selector_shift;
+
+	unsigned int window_start;
+	unsigned int window_len;
+};
+
 #ifdef CONFIG_DEBUG_FS
 extern void regmap_debugfs_initcall(void);
 extern void regmap_debugfs_init(struct regmap *map, const char *name);
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index 4fac4b9be88..a89734621e5 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -24,14 +24,18 @@ struct regmap_irq_chip_data {
 	struct mutex lock;
 
 	struct regmap *map;
-	struct regmap_irq_chip *chip;
+	const struct regmap_irq_chip *chip;
 
 	int irq_base;
 	struct irq_domain *domain;
 
+	int irq;
+	int wake_count;
+
 	unsigned int *status_buf;
 	unsigned int *mask_buf;
 	unsigned int *mask_buf_def;
+	unsigned int *wake_buf;
 
 	unsigned int irq_reg_stride;
 };
@@ -71,6 +75,16 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 				d->chip->mask_base + (i * map->reg_stride));
 	}
 
+	/* If we've changed our wakeup count propagate it to the parent */
+	if (d->wake_count < 0)
+		for (i = d->wake_count; i < 0; i++)
+			irq_set_irq_wake(d->irq, 0);
+	else if (d->wake_count > 0)
+		for (i = 0; i < d->wake_count; i++)
+			irq_set_irq_wake(d->irq, 1);
+
+	d->wake_count = 0;
+
 	mutex_unlock(&d->lock);
 }
 
@@ -92,18 +106,41 @@ static void regmap_irq_disable(struct irq_data *data)
 	d->mask_buf[irq_data->reg_offset / map->reg_stride] |= irq_data->mask;
 }
 
+static int regmap_irq_set_wake(struct irq_data *data, unsigned int on)
+{
+	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+	struct regmap *map = d->map;
+	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->hwirq);
+
+	if (!d->chip->wake_base)
+		return -EINVAL;
+
+	if (on) {
+		d->wake_buf[irq_data->reg_offset / map->reg_stride]
+			&= ~irq_data->mask;
+		d->wake_count++;
+	} else {
+		d->wake_buf[irq_data->reg_offset / map->reg_stride]
+			|= irq_data->mask;
+		d->wake_count--;
+	}
+
+	return 0;
+}
+
 static struct irq_chip regmap_irq_chip = {
 	.name			= "regmap",
 	.irq_bus_lock		= regmap_irq_lock,
 	.irq_bus_sync_unlock	= regmap_irq_sync_unlock,
 	.irq_disable		= regmap_irq_disable,
 	.irq_enable		= regmap_irq_enable,
+	.irq_set_wake		= regmap_irq_set_wake,
 };
 
 static irqreturn_t regmap_irq_thread(int irq, void *d)
 {
 	struct regmap_irq_chip_data *data = d;
-	struct regmap_irq_chip *chip = data->chip;
+	const struct regmap_irq_chip *chip = data->chip;
 	struct regmap *map = data->map;
 	int ret, i;
 	bool handled = false;
@@ -195,7 +232,7 @@ static struct irq_domain_ops regmap_domain_ops = {
  * register values used by the IRQ controller over suspend and resume.
  */
 int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
-			int irq_base, struct regmap_irq_chip *chip,
+			int irq_base, const struct regmap_irq_chip *chip,
 			struct regmap_irq_chip_data **data)
 {
 	struct regmap_irq_chip_data *d;
@@ -240,6 +277,14 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	if (!d->mask_buf_def)
 		goto err_alloc;
 
+	if (chip->wake_base) {
+		d->wake_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
+				      GFP_KERNEL);
+		if (!d->wake_buf)
+			goto err_alloc;
+	}
+
+	d->irq = irq;
 	d->map = map;
 	d->chip = chip;
 	d->irq_base = irq_base;
@@ -294,6 +339,7 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 err_domain:
 	/* Should really dispose of the domain but... */
 err_alloc:
+	kfree(d->wake_buf);
 	kfree(d->mask_buf_def);
 	kfree(d->mask_buf);
 	kfree(d->status_buf);
@@ -315,6 +361,7 @@ void regmap_del_irq_chip(int irq, struct regmap_irq_chip_data *d)
 
 	free_irq(irq, d);
 	/* We should unmap the domain but... */
+	kfree(d->wake_buf);
 	kfree(d->mask_buf_def);
 	kfree(d->mask_buf);
 	kfree(d->status_buf);
@@ -346,6 +393,10 @@ EXPORT_SYMBOL_GPL(regmap_irq_chip_get_base);
  */
 int regmap_irq_get_virq(struct regmap_irq_chip_data *data, int irq)
 {
+	/* Handle holes in the IRQ list */
+	if (!data->chip->irqs[irq].mask)
+		return -EINVAL;
+
 	return irq_create_mapping(data->domain, irq);
 }
 EXPORT_SYMBOL_GPL(regmap_irq_get_virq);
diff --git a/drivers/base/regmap/regmap-mmio.c b/drivers/base/regmap/regmap-mmio.c
index febd6de6c8a..f05fc74dd84 100644
--- a/drivers/base/regmap/regmap-mmio.c
+++ b/drivers/base/regmap/regmap-mmio.c
@@ -37,7 +37,7 @@ static int regmap_mmio_gather_write(void *context,
 
 	BUG_ON(reg_size != 4);
 
-	offset = be32_to_cpup(reg);
+	offset = *(u32 *)reg;
 
 	while (val_size) {
 		switch (ctx->val_bytes) {
@@ -45,14 +45,14 @@ static int regmap_mmio_gather_write(void *context,
 			writeb(*(u8 *)val, ctx->regs + offset);
 			break;
 		case 2:
-			writew(be16_to_cpup(val), ctx->regs + offset);
+			writew(*(u16 *)val, ctx->regs + offset);
 			break;
 		case 4:
-			writel(be32_to_cpup(val), ctx->regs + offset);
+			writel(*(u32 *)val, ctx->regs + offset);
 			break;
 #ifdef CONFIG_64BIT
 		case 8:
-			writeq(be64_to_cpup(val), ctx->regs + offset);
+			writeq(*(u64 *)val, ctx->regs + offset);
 			break;
 #endif
 		default:
@@ -83,7 +83,7 @@ static int regmap_mmio_read(void *context,
 
 	BUG_ON(reg_size != 4);
 
-	offset = be32_to_cpup(reg);
+	offset = *(u32 *)reg;
 
 	while (val_size) {
 		switch (ctx->val_bytes) {
@@ -91,14 +91,14 @@ static int regmap_mmio_read(void *context,
 			*(u8 *)val = readb(ctx->regs + offset);
 			break;
 		case 2:
-			*(u16 *)val = cpu_to_be16(readw(ctx->regs + offset));
+			*(u16 *)val = readw(ctx->regs + offset);
 			break;
 		case 4:
-			*(u32 *)val = cpu_to_be32(readl(ctx->regs + offset));
+			*(u32 *)val = readl(ctx->regs + offset);
 			break;
 #ifdef CONFIG_64BIT
 		case 8:
-			*(u64 *)val = cpu_to_be32(readq(ctx->regs + offset));
+			*(u64 *)val = readq(ctx->regs + offset);
 			break;
 #endif
 		default:
@@ -124,9 +124,11 @@ static struct regmap_bus regmap_mmio = {
 	.gather_write = regmap_mmio_gather_write,
 	.read = regmap_mmio_read,
 	.free_context = regmap_mmio_free_context,
+	.reg_format_endian_default = REGMAP_ENDIAN_NATIVE,
+	.val_format_endian_default = REGMAP_ENDIAN_NATIVE,
 };
 
-struct regmap_mmio_context *regmap_mmio_gen_context(void __iomem *regs,
+static struct regmap_mmio_context *regmap_mmio_gen_context(void __iomem *regs,
 					const struct regmap_config *config)
 {
 	struct regmap_mmio_context *ctx;
@@ -162,7 +164,15 @@ struct regmap_mmio_context *regmap_mmio_gen_context(void __iomem *regs,
 	if (config->reg_stride < min_stride)
 		return ERR_PTR(-EINVAL);
 
-	ctx = kzalloc(GFP_KERNEL, sizeof(*ctx));
+	switch (config->reg_format_endian) {
+	case REGMAP_ENDIAN_DEFAULT:
+	case REGMAP_ENDIAN_NATIVE:
+		break;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
 		return ERR_PTR(-ENOMEM);
 
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index c89aa01fb1d..c241ae2f2f1 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -15,12 +15,25 @@
 #include <linux/export.h>
 #include <linux/mutex.h>
 #include <linux/err.h>
+#include <linux/rbtree.h>
 
 #define CREATE_TRACE_POINTS
 #include <trace/events/regmap.h>
 
 #include "internal.h"
 
+/*
+ * Sometimes for failures during very early init the trace
+ * infrastructure isn't available early enough to be used.  For this
+ * sort of problem defining LOG_DEVICE will add printks for basic
+ * register I/O on a specific device.
+ */
+#undef LOG_DEVICE
+
+static int _regmap_update_bits(struct regmap *map, unsigned int reg,
+			       unsigned int mask, unsigned int val,
+			       bool *change);
+
 bool regmap_writeable(struct regmap *map, unsigned int reg)
 {
 	if (map->max_register && reg > map->max_register)
@@ -119,13 +132,19 @@ static void regmap_format_8(void *buf, unsigned int val, unsigned int shift)
 	b[0] = val << shift;
 }
 
-static void regmap_format_16(void *buf, unsigned int val, unsigned int shift)
+static void regmap_format_16_be(void *buf, unsigned int val, unsigned int shift)
 {
 	__be16 *b = buf;
 
 	b[0] = cpu_to_be16(val << shift);
 }
 
+static void regmap_format_16_native(void *buf, unsigned int val,
+				    unsigned int shift)
+{
+	*(u16 *)buf = val << shift;
+}
+
 static void regmap_format_24(void *buf, unsigned int val, unsigned int shift)
 {
 	u8 *b = buf;
@@ -137,13 +156,19 @@ static void regmap_format_24(void *buf, unsigned int val, unsigned int shift)
 	b[2] = val;
 }
 
-static void regmap_format_32(void *buf, unsigned int val, unsigned int shift)
+static void regmap_format_32_be(void *buf, unsigned int val, unsigned int shift)
 {
 	__be32 *b = buf;
 
 	b[0] = cpu_to_be32(val << shift);
 }
 
+static void regmap_format_32_native(void *buf, unsigned int val,
+				    unsigned int shift)
+{
+	*(u32 *)buf = val << shift;
+}
+
 static unsigned int regmap_parse_8(void *buf)
 {
 	u8 *b = buf;
@@ -151,7 +176,7 @@ static unsigned int regmap_parse_8(void *buf)
 	return b[0];
 }
 
-static unsigned int regmap_parse_16(void *buf)
+static unsigned int regmap_parse_16_be(void *buf)
 {
 	__be16 *b = buf;
 
@@ -160,6 +185,11 @@ static unsigned int regmap_parse_16(void *buf)
 	return b[0];
 }
 
+static unsigned int regmap_parse_16_native(void *buf)
+{
+	return *(u16 *)buf;
+}
+
 static unsigned int regmap_parse_24(void *buf)
 {
 	u8 *b = buf;
@@ -170,7 +200,7 @@ static unsigned int regmap_parse_24(void *buf)
 	return ret;
 }
 
-static unsigned int regmap_parse_32(void *buf)
+static unsigned int regmap_parse_32_be(void *buf)
 {
 	__be32 *b = buf;
 
@@ -179,6 +209,11 @@ static unsigned int regmap_parse_32(void *buf)
 	return b[0];
 }
 
+static unsigned int regmap_parse_32_native(void *buf)
+{
+	return *(u32 *)buf;
+}
+
 static void regmap_lock_mutex(struct regmap *map)
 {
 	mutex_lock(&map->mutex);
@@ -208,6 +243,67 @@ static void dev_get_regmap_release(struct device *dev, void *res)
 	 */
 }
 
+static bool _regmap_range_add(struct regmap *map,
+			      struct regmap_range_node *data)
+{
+	struct rb_root *root = &map->range_tree;
+	struct rb_node **new = &(root->rb_node), *parent = NULL;
+
+	while (*new) {
+		struct regmap_range_node *this =
+			container_of(*new, struct regmap_range_node, node);
+
+		parent = *new;
+		if (data->range_max < this->range_min)
+			new = &((*new)->rb_left);
+		else if (data->range_min > this->range_max)
+			new = &((*new)->rb_right);
+		else
+			return false;
+	}
+
+	rb_link_node(&data->node, parent, new);
+	rb_insert_color(&data->node, root);
+
+	return true;
+}
+
+static struct regmap_range_node *_regmap_range_lookup(struct regmap *map,
+						      unsigned int reg)
+{
+	struct rb_node *node = map->range_tree.rb_node;
+
+	while (node) {
+		struct regmap_range_node *this =
+			container_of(node, struct regmap_range_node, node);
+
+		if (reg < this->range_min)
+			node = node->rb_left;
+		else if (reg > this->range_max)
+			node = node->rb_right;
+		else
+			return this;
+	}
+
+	return NULL;
+}
+
+static void regmap_range_exit(struct regmap *map)
+{
+	struct rb_node *next;
+	struct regmap_range_node *range_node;
+
+	next = rb_first(&map->range_tree);
+	while (next) {
+		range_node = rb_entry(next, struct regmap_range_node, node);
+		next = rb_next(&range_node->node);
+		rb_erase(&range_node->node, &map->range_tree);
+		kfree(range_node);
+	}
+
+	kfree(map->selector_work_buf);
+}
+
 /**
  * regmap_init(): Initialise register map
  *
@@ -227,6 +323,8 @@ struct regmap *regmap_init(struct device *dev,
 {
 	struct regmap *map, **m;
 	int ret = -EINVAL;
+	enum regmap_endian reg_endian, val_endian;
+	int i, j;
 
 	if (!bus || !config)
 		goto err;
@@ -275,6 +373,18 @@ struct regmap *regmap_init(struct device *dev,
 		map->read_flag_mask = bus->read_flag_mask;
 	}
 
+	reg_endian = config->reg_format_endian;
+	if (reg_endian == REGMAP_ENDIAN_DEFAULT)
+		reg_endian = bus->reg_format_endian_default;
+	if (reg_endian == REGMAP_ENDIAN_DEFAULT)
+		reg_endian = REGMAP_ENDIAN_BIG;
+
+	val_endian = config->val_format_endian;
+	if (val_endian == REGMAP_ENDIAN_DEFAULT)
+		val_endian = bus->val_format_endian_default;
+	if (val_endian == REGMAP_ENDIAN_DEFAULT)
+		val_endian = REGMAP_ENDIAN_BIG;
+
 	switch (config->reg_bits + map->reg_shift) {
 	case 2:
 		switch (config->val_bits) {
@@ -321,11 +431,29 @@ struct regmap *regmap_init(struct device *dev,
 		break;
 
 	case 16:
-		map->format.format_reg = regmap_format_16;
+		switch (reg_endian) {
+		case REGMAP_ENDIAN_BIG:
+			map->format.format_reg = regmap_format_16_be;
+			break;
+		case REGMAP_ENDIAN_NATIVE:
+			map->format.format_reg = regmap_format_16_native;
+			break;
+		default:
+			goto err_map;
+		}
 		break;
 
 	case 32:
-		map->format.format_reg = regmap_format_32;
+		switch (reg_endian) {
+		case REGMAP_ENDIAN_BIG:
+			map->format.format_reg = regmap_format_32_be;
+			break;
+		case REGMAP_ENDIAN_NATIVE:
+			map->format.format_reg = regmap_format_32_native;
+			break;
+		default:
+			goto err_map;
+		}
 		break;
 
 	default:
@@ -338,21 +466,47 @@ struct regmap *regmap_init(struct device *dev,
 		map->format.parse_val = regmap_parse_8;
 		break;
 	case 16:
-		map->format.format_val = regmap_format_16;
-		map->format.parse_val = regmap_parse_16;
+		switch (val_endian) {
+		case REGMAP_ENDIAN_BIG:
+			map->format.format_val = regmap_format_16_be;
+			map->format.parse_val = regmap_parse_16_be;
+			break;
+		case REGMAP_ENDIAN_NATIVE:
+			map->format.format_val = regmap_format_16_native;
+			map->format.parse_val = regmap_parse_16_native;
+			break;
+		default:
+			goto err_map;
+		}
 		break;
 	case 24:
+		if (val_endian != REGMAP_ENDIAN_BIG)
+			goto err_map;
 		map->format.format_val = regmap_format_24;
 		map->format.parse_val = regmap_parse_24;
 		break;
 	case 32:
-		map->format.format_val = regmap_format_32;
-		map->format.parse_val = regmap_parse_32;
+		switch (val_endian) {
+		case REGMAP_ENDIAN_BIG:
+			map->format.format_val = regmap_format_32_be;
+			map->format.parse_val = regmap_parse_32_be;
+			break;
+		case REGMAP_ENDIAN_NATIVE:
+			map->format.format_val = regmap_format_32_native;
+			map->format.parse_val = regmap_parse_32_native;
+			break;
+		default:
+			goto err_map;
+		}
 		break;
 	}
 
-	if (map->format.format_write)
+	if (map->format.format_write) {
+		if ((reg_endian != REGMAP_ENDIAN_BIG) ||
+		    (val_endian != REGMAP_ENDIAN_BIG))
+			goto err_map;
 		map->use_single_rw = true;
+	}
 
 	if (!map->format.format_write &&
 	    !(map->format.format_reg && map->format.format_val))
@@ -364,27 +518,88 @@ struct regmap *regmap_init(struct device *dev,
 		goto err_map;
 	}
 
-	regmap_debugfs_init(map, config->name);
+	map->range_tree = RB_ROOT;
+	for (i = 0; i < config->n_ranges; i++) {
+		const struct regmap_range_cfg *range_cfg = &config->ranges[i];
+		struct regmap_range_node *new;
+
+		/* Sanity check */
+		if (range_cfg->range_max < range_cfg->range_min ||
+		    range_cfg->range_max > map->max_register ||
+		    range_cfg->selector_reg > map->max_register ||
+		    range_cfg->window_len == 0)
+			goto err_range;
+
+		/* Make sure, that this register range has no selector
+		   or data window within its boundary */
+		for (j = 0; j < config->n_ranges; j++) {
+			unsigned sel_reg = config->ranges[j].selector_reg;
+			unsigned win_min = config->ranges[j].window_start;
+			unsigned win_max = win_min +
+					   config->ranges[j].window_len - 1;
+
+			if (range_cfg->range_min <= sel_reg &&
+			    sel_reg <= range_cfg->range_max) {
+				goto err_range;
+			}
+
+			if (!(win_max < range_cfg->range_min ||
+			      win_min > range_cfg->range_max)) {
+				goto err_range;
+			}
+		}
+
+		new = kzalloc(sizeof(*new), GFP_KERNEL);
+		if (new == NULL) {
+			ret = -ENOMEM;
+			goto err_range;
+		}
+
+		new->range_min = range_cfg->range_min;
+		new->range_max = range_cfg->range_max;
+		new->selector_reg = range_cfg->selector_reg;
+		new->selector_mask = range_cfg->selector_mask;
+		new->selector_shift = range_cfg->selector_shift;
+		new->window_start = range_cfg->window_start;
+		new->window_len = range_cfg->window_len;
+
+		if (_regmap_range_add(map, new) == false) {
+			kfree(new);
+			goto err_range;
+		}
+
+		if (map->selector_work_buf == NULL) {
+			map->selector_work_buf =
+				kzalloc(map->format.buf_size, GFP_KERNEL);
+			if (map->selector_work_buf == NULL) {
+				ret = -ENOMEM;
+				goto err_range;
+			}
+		}
+	}
 
 	ret = regcache_init(map, config);
 	if (ret < 0)
-		goto err_debugfs;
+		goto err_range;
+
+	regmap_debugfs_init(map, config->name);
 
 	/* Add a devres resource for dev_get_regmap() */
 	m = devres_alloc(dev_get_regmap_release, sizeof(*m), GFP_KERNEL);
 	if (!m) {
 		ret = -ENOMEM;
-		goto err_cache;
+		goto err_debugfs;
 	}
 	*m = map;
 	devres_add(dev, m);
 
 	return map;
 
-err_cache:
-	regcache_exit(map);
 err_debugfs:
 	regmap_debugfs_exit(map);
+	regcache_exit(map);
+err_range:
+	regmap_range_exit(map);
 	kfree(map->work_buf);
 err_map:
 	kfree(map);
@@ -481,6 +696,7 @@ void regmap_exit(struct regmap *map)
 {
 	regcache_exit(map);
 	regmap_debugfs_exit(map);
+	regmap_range_exit(map);
 	if (map->bus->free_context)
 		map->bus->free_context(map->bus_context);
 	kfree(map->work_buf);
@@ -526,6 +742,57 @@ struct regmap *dev_get_regmap(struct device *dev, const char *name)
 }
 EXPORT_SYMBOL_GPL(dev_get_regmap);
 
+static int _regmap_select_page(struct regmap *map, unsigned int *reg,
+			       unsigned int val_num)
+{
+	struct regmap_range_node *range;
+	void *orig_work_buf;
+	unsigned int win_offset;
+	unsigned int win_page;
+	bool page_chg;
+	int ret;
+
+	range = _regmap_range_lookup(map, *reg);
+	if (range) {
+		win_offset = (*reg - range->range_min) % range->window_len;
+		win_page = (*reg - range->range_min) / range->window_len;
+
+		if (val_num > 1) {
+			/* Bulk write shouldn't cross range boundary */
+			if (*reg + val_num - 1 > range->range_max)
+				return -EINVAL;
+
+			/* ... or single page boundary */
+			if (val_num > range->window_len - win_offset)
+				return -EINVAL;
+		}
+
+		/* It is possible to have selector register inside data window.
+		   In that case, selector register is located on every page and
+		   it needs no page switching, when accessed alone. */
+		if (val_num > 1 ||
+		    range->window_start + win_offset != range->selector_reg) {
+			/* Use separate work_buf during page switching */
+			orig_work_buf = map->work_buf;
+			map->work_buf = map->selector_work_buf;
+
+			ret = _regmap_update_bits(map, range->selector_reg,
+					range->selector_mask,
+					win_page << range->selector_shift,
+					&page_chg);
+
+			map->work_buf = orig_work_buf;
+
+			if (ret < 0)
+				return ret;
+		}
+
+		*reg = range->window_start + win_offset;
+	}
+
+	return 0;
+}
+
 static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 			     const void *val, size_t val_len)
 {
@@ -563,6 +830,10 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 		}
 	}
 
+	ret = _regmap_select_page(map, &reg, val_len / map->format.val_bytes);
+	if (ret < 0)
+		return ret;
+
 	map->format.format_reg(map->work_buf, reg, map->reg_shift);
 
 	u8[0] |= map->write_flag_mask;
@@ -623,9 +894,18 @@ int _regmap_write(struct regmap *map, unsigned int reg,
 		}
 	}
 
+#ifdef LOG_DEVICE
+	if (strcmp(dev_name(map->dev), LOG_DEVICE) == 0)
+		dev_info(map->dev, "%x <= %x\n", reg, val);
+#endif
+
 	trace_regmap_reg_write(map->dev, reg, val);
 
 	if (map->format.format_write) {
+		ret = _regmap_select_page(map, &reg, 1);
+		if (ret < 0)
+			return ret;
+
 		map->format.format_write(map, reg, val);
 
 		trace_regmap_hw_write_start(map->dev, reg, 1);
@@ -783,6 +1063,10 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	u8 *u8 = map->work_buf;
 	int ret;
 
+	ret = _regmap_select_page(map, &reg, val_len / map->format.val_bytes);
+	if (ret < 0)
+		return ret;
+
 	map->format.format_reg(map->work_buf, reg, map->reg_shift);
 
 	/*
@@ -826,6 +1110,12 @@ static int _regmap_read(struct regmap *map, unsigned int reg,
 	ret = _regmap_raw_read(map, reg, map->work_buf, map->format.val_bytes);
 	if (ret == 0) {
 		*val = map->format.parse_val(map->work_buf);
+
+#ifdef LOG_DEVICE
+		if (strcmp(dev_name(map->dev), LOG_DEVICE) == 0)
+			dev_info(map->dev, "%x => %x\n", reg, *val);
+#endif
+
 		trace_regmap_reg_read(map->dev, reg, *val);
 	}
 
@@ -982,11 +1272,9 @@ static int _regmap_update_bits(struct regmap *map, unsigned int reg,
 	int ret;
 	unsigned int tmp, orig;
 
-	map->lock(map);
-
 	ret = _regmap_read(map, reg, &orig);
 	if (ret != 0)
-		goto out;
+		return ret;
 
 	tmp = orig & ~mask;
 	tmp |= val & mask;
@@ -998,9 +1286,6 @@ static int _regmap_update_bits(struct regmap *map, unsigned int reg,
 		*change = false;
 	}
 
-out:
-	map->unlock(map);
-
 	return ret;
 }
 
@@ -1018,7 +1303,13 @@ int regmap_update_bits(struct regmap *map, unsigned int reg,
 		       unsigned int mask, unsigned int val)
 {
 	bool change;
-	return _regmap_update_bits(map, reg, mask, val, &change);
+	int ret;
+
+	map->lock(map);
+	ret = _regmap_update_bits(map, reg, mask, val, &change);
+	map->unlock(map);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(regmap_update_bits);
 
@@ -1038,7 +1329,12 @@ int regmap_update_bits_check(struct regmap *map, unsigned int reg,
 			     unsigned int mask, unsigned int val,
 			     bool *change)
 {
-	return _regmap_update_bits(map, reg, mask, val, change);
+	int ret;
+
+	map->lock(map);
+	ret = _regmap_update_bits(map, reg, mask, val, change);
+	map->unlock(map);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(regmap_update_bits_check);
 
diff --git a/drivers/bcma/Kconfig b/drivers/bcma/Kconfig
index fb7c80fb721..06b3207adeb 100644
--- a/drivers/bcma/Kconfig
+++ b/drivers/bcma/Kconfig
@@ -46,6 +46,25 @@ config BCMA_DRIVER_MIPS
 
 	  If unsure, say N
 
+config BCMA_SFLASH
+	bool
+	depends on BCMA_DRIVER_MIPS && BROKEN
+	default y
+
+config BCMA_NFLASH
+	bool
+	depends on BCMA_DRIVER_MIPS && BROKEN
+	default y
+
+config BCMA_DRIVER_GMAC_CMN
+	bool "BCMA Broadcom GBIT MAC COMMON core driver"
+	depends on BCMA
+	help
+	  Driver for the Broadcom GBIT MAC COMMON core attached to Broadcom
+	  specific Advanced Microcontroller Bus.
+
+	  If unsure, say N
+
 config BCMA_DEBUG
 	bool "BCMA debugging"
 	depends on BCMA
diff --git a/drivers/bcma/Makefile b/drivers/bcma/Makefile
index 82de24e5340..8ad42d41b2f 100644
--- a/drivers/bcma/Makefile
+++ b/drivers/bcma/Makefile
@@ -1,8 +1,11 @@
 bcma-y					+= main.o scan.o core.o sprom.o
 bcma-y					+= driver_chipcommon.o driver_chipcommon_pmu.o
+bcma-$(CONFIG_BCMA_SFLASH)		+= driver_chipcommon_sflash.o
+bcma-$(CONFIG_BCMA_NFLASH)		+= driver_chipcommon_nflash.o
 bcma-y					+= driver_pci.o
 bcma-$(CONFIG_BCMA_DRIVER_PCI_HOSTMODE)	+= driver_pci_host.o
 bcma-$(CONFIG_BCMA_DRIVER_MIPS)		+= driver_mips.o
+bcma-$(CONFIG_BCMA_DRIVER_GMAC_CMN)	+= driver_gmac_cmn.o
 bcma-$(CONFIG_BCMA_HOST_PCI)		+= host_pci.o
 bcma-$(CONFIG_BCMA_HOST_SOC)		+= host_soc.o
 obj-$(CONFIG_BCMA)			+= bcma.o
diff --git a/drivers/bcma/bcma_private.h b/drivers/bcma/bcma_private.h
index b81755bb479..3cf9cc923cd 100644
--- a/drivers/bcma/bcma_private.h
+++ b/drivers/bcma/bcma_private.h
@@ -10,6 +10,15 @@
 
 #define BCMA_CORE_SIZE		0x1000
 
+#define bcma_err(bus, fmt, ...) \
+	pr_err("bus%d: " fmt, (bus)->num, ##__VA_ARGS__)
+#define bcma_warn(bus, fmt, ...) \
+	pr_warn("bus%d: " fmt, (bus)->num, ##__VA_ARGS__)
+#define bcma_info(bus, fmt, ...) \
+	pr_info("bus%d: " fmt, (bus)->num, ##__VA_ARGS__)
+#define bcma_debug(bus, fmt, ...) \
+	pr_debug("bus%d: " fmt, (bus)->num, ##__VA_ARGS__)
+
 struct bcma_bus;
 
 /* main.c */
@@ -42,6 +51,28 @@ void bcma_chipco_serial_init(struct bcma_drv_cc *cc);
 u32 bcma_pmu_alp_clock(struct bcma_drv_cc *cc);
 u32 bcma_pmu_get_clockcpu(struct bcma_drv_cc *cc);
 
+#ifdef CONFIG_BCMA_SFLASH
+/* driver_chipcommon_sflash.c */
+int bcma_sflash_init(struct bcma_drv_cc *cc);
+#else
+static inline int bcma_sflash_init(struct bcma_drv_cc *cc)
+{
+	bcma_err(cc->core->bus, "Serial flash not supported\n");
+	return 0;
+}
+#endif /* CONFIG_BCMA_SFLASH */
+
+#ifdef CONFIG_BCMA_NFLASH
+/* driver_chipcommon_nflash.c */
+int bcma_nflash_init(struct bcma_drv_cc *cc);
+#else
+static inline int bcma_nflash_init(struct bcma_drv_cc *cc)
+{
+	bcma_err(cc->core->bus, "NAND flash not supported\n");
+	return 0;
+}
+#endif /* CONFIG_BCMA_NFLASH */
+
 #ifdef CONFIG_BCMA_HOST_PCI
 /* host_pci.c */
 extern int __init bcma_host_pci_init(void);
diff --git a/drivers/bcma/core.c b/drivers/bcma/core.c
index bc6e89212ad..63c8b470536 100644
--- a/drivers/bcma/core.c
+++ b/drivers/bcma/core.c
@@ -75,7 +75,7 @@ void bcma_core_set_clockmode(struct bcma_device *core,
 			udelay(10);
 		}
 		if (i)
-			pr_err("HT force timeout\n");
+			bcma_err(core->bus, "HT force timeout\n");
 		break;
 	case BCMA_CLKMODE_DYNAMIC:
 		bcma_set32(core, BCMA_CLKCTLST, ~BCMA_CLKCTLST_FORCEHT);
@@ -102,9 +102,9 @@ void bcma_core_pll_ctl(struct bcma_device *core, u32 req, u32 status, bool on)
 			udelay(10);
 		}
 		if (i)
-			pr_err("PLL enable timeout\n");
+			bcma_err(core->bus, "PLL enable timeout\n");
 	} else {
-		pr_warn("Disabling PLL not supported yet!\n");
+		bcma_warn(core->bus, "Disabling PLL not supported yet!\n");
 	}
 }
 EXPORT_SYMBOL_GPL(bcma_core_pll_ctl);
@@ -120,8 +120,8 @@ u32 bcma_core_dma_translation(struct bcma_device *core)
 		else
 			return BCMA_DMA_TRANSLATION_DMA32_CMT;
 	default:
-		pr_err("DMA translation unknown for host %d\n",
-		       core->bus->hosttype);
+		bcma_err(core->bus, "DMA translation unknown for host %d\n",
+			 core->bus->hosttype);
 	}
 	return BCMA_DMA_TRANSLATION_NONE;
 }
diff --git a/drivers/bcma/driver_chipcommon.c b/drivers/bcma/driver_chipcommon.c
index e9f1b3fd252..a4c3ebcc4c8 100644
--- a/drivers/bcma/driver_chipcommon.c
+++ b/drivers/bcma/driver_chipcommon.c
@@ -44,7 +44,7 @@ void bcma_core_chipcommon_init(struct bcma_drv_cc *cc)
 	if (cc->capabilities & BCMA_CC_CAP_PMU)
 		bcma_pmu_init(cc);
 	if (cc->capabilities & BCMA_CC_CAP_PCTL)
-		pr_err("Power control not implemented!\n");
+		bcma_err(cc->core->bus, "Power control not implemented!\n");
 
 	if (cc->core->id.rev >= 16) {
 		if (cc->core->bus->sprom.leddc_on_time &&
@@ -137,8 +137,7 @@ void bcma_chipco_serial_init(struct bcma_drv_cc *cc)
 				       | BCMA_CC_CORECTL_UARTCLKEN);
 		}
 	} else {
-		pr_err("serial not supported on this device ccrev: 0x%x\n",
-		       ccrev);
+		bcma_err(cc->core->bus, "serial not supported on this device ccrev: 0x%x\n", ccrev);
 		return;
 	}
 
diff --git a/drivers/bcma/driver_chipcommon_nflash.c b/drivers/bcma/driver_chipcommon_nflash.c
new file mode 100644
index 00000000000..574d62435bc
--- /dev/null
+++ b/drivers/bcma/driver_chipcommon_nflash.c
@@ -0,0 +1,19 @@
+/*
+ * Broadcom specific AMBA
+ * ChipCommon NAND flash interface
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include <linux/bcma/bcma.h>
+#include <linux/bcma/bcma_driver_chipcommon.h>
+#include <linux/delay.h>
+
+#include "bcma_private.h"
+
+/* Initialize NAND flash access */
+int bcma_nflash_init(struct bcma_drv_cc *cc)
+{
+	bcma_err(cc->core->bus, "NAND flash support is broken\n");
+	return 0;
+}
diff --git a/drivers/bcma/driver_chipcommon_pmu.c b/drivers/bcma/driver_chipcommon_pmu.c
index 61ce4054b3c..c9a4f46c514 100644
--- a/drivers/bcma/driver_chipcommon_pmu.c
+++ b/drivers/bcma/driver_chipcommon_pmu.c
@@ -3,7 +3,8 @@
  * ChipCommon Power Management Unit driver
  *
  * Copyright 2009, Michael Buesch <m@bues.ch>
- * Copyright 2007, Broadcom Corporation
+ * Copyright 2007, 2011, Broadcom Corporation
+ * Copyright 2011, 2012, Hauke Mehrtens <hauke@hauke-m.de>
  *
  * Licensed under the GNU/GPL. See COPYING for details.
  */
@@ -54,39 +55,19 @@ void bcma_chipco_regctl_maskset(struct bcma_drv_cc *cc, u32 offset, u32 mask,
 }
 EXPORT_SYMBOL_GPL(bcma_chipco_regctl_maskset);
 
-static void bcma_pmu_pll_init(struct bcma_drv_cc *cc)
-{
-	struct bcma_bus *bus = cc->core->bus;
-
-	switch (bus->chipinfo.id) {
-	case 0x4313:
-	case 0x4331:
-	case 43224:
-	case 43225:
-		break;
-	default:
-		pr_err("PLL init unknown for device 0x%04X\n",
-			bus->chipinfo.id);
-	}
-}
-
 static void bcma_pmu_resources_init(struct bcma_drv_cc *cc)
 {
 	struct bcma_bus *bus = cc->core->bus;
 	u32 min_msk = 0, max_msk = 0;
 
 	switch (bus->chipinfo.id) {
-	case 0x4313:
+	case BCMA_CHIP_ID_BCM4313:
 		min_msk = 0x200D;
 		max_msk = 0xFFFF;
 		break;
-	case 0x4331:
-	case 43224:
-	case 43225:
-		break;
 	default:
-		pr_err("PMU resource config unknown for device 0x%04X\n",
-			bus->chipinfo.id);
+		bcma_debug(bus, "PMU resource config unknown or not needed for device 0x%04X\n",
+			   bus->chipinfo.id);
 	}
 
 	/* Set the resource masks. */
@@ -94,22 +75,9 @@ static void bcma_pmu_resources_init(struct bcma_drv_cc *cc)
 		bcma_cc_write32(cc, BCMA_CC_PMU_MINRES_MSK, min_msk);
 	if (max_msk)
 		bcma_cc_write32(cc, BCMA_CC_PMU_MAXRES_MSK, max_msk);
-}
-
-void bcma_pmu_swreg_init(struct bcma_drv_cc *cc)
-{
-	struct bcma_bus *bus = cc->core->bus;
 
-	switch (bus->chipinfo.id) {
-	case 0x4313:
-	case 0x4331:
-	case 43224:
-	case 43225:
-		break;
-	default:
-		pr_err("PMU switch/regulators init unknown for device "
-			"0x%04X\n", bus->chipinfo.id);
-	}
+	/* Add some delay; allow resources to come up and settle. */
+	mdelay(2);
 }
 
 /* Disable to allow reading SPROM. Don't know the adventages of enabling it. */
@@ -123,8 +91,11 @@ void bcma_chipco_bcm4331_ext_pa_lines_ctl(struct bcma_drv_cc *cc, bool enable)
 		val |= BCMA_CHIPCTL_4331_EXTPA_EN;
 		if (bus->chipinfo.pkg == 9 || bus->chipinfo.pkg == 11)
 			val |= BCMA_CHIPCTL_4331_EXTPA_ON_GPIO2_5;
+		else if (bus->chipinfo.rev > 0)
+			val |= BCMA_CHIPCTL_4331_EXTPA_EN2;
 	} else {
 		val &= ~BCMA_CHIPCTL_4331_EXTPA_EN;
+		val &= ~BCMA_CHIPCTL_4331_EXTPA_EN2;
 		val &= ~BCMA_CHIPCTL_4331_EXTPA_ON_GPIO2_5;
 	}
 	bcma_cc_write32(cc, BCMA_CC_CHIPCTL, val);
@@ -135,28 +106,38 @@ void bcma_pmu_workarounds(struct bcma_drv_cc *cc)
 	struct bcma_bus *bus = cc->core->bus;
 
 	switch (bus->chipinfo.id) {
-	case 0x4313:
-		bcma_chipco_chipctl_maskset(cc, 0, ~0, 0x7);
+	case BCMA_CHIP_ID_BCM4313:
+		/* enable 12 mA drive strenth for 4313 and set chipControl
+		   register bit 1 */
+		bcma_chipco_chipctl_maskset(cc, 0,
+					    ~BCMA_CCTRL_4313_12MA_LED_DRIVE,
+					    BCMA_CCTRL_4313_12MA_LED_DRIVE);
 		break;
-	case 0x4331:
-	case 43431:
+	case BCMA_CHIP_ID_BCM4331:
+	case BCMA_CHIP_ID_BCM43431:
 		/* Ext PA lines must be enabled for tx on BCM4331 */
 		bcma_chipco_bcm4331_ext_pa_lines_ctl(cc, true);
 		break;
-	case 43224:
+	case BCMA_CHIP_ID_BCM43224:
+	case BCMA_CHIP_ID_BCM43421:
+		/* enable 12 mA drive strenth for 43224 and set chipControl
+		   register bit 15 */
 		if (bus->chipinfo.rev == 0) {
-			pr_err("Workarounds for 43224 rev 0 not fully "
-				"implemented\n");
-			bcma_chipco_chipctl_maskset(cc, 0, ~0, 0x00F000F0);
+			bcma_cc_maskset32(cc, BCMA_CC_CHIPCTL,
+					  ~BCMA_CCTRL_43224_GPIO_TOGGLE,
+					  BCMA_CCTRL_43224_GPIO_TOGGLE);
+			bcma_chipco_chipctl_maskset(cc, 0,
+						    ~BCMA_CCTRL_43224A0_12MA_LED_DRIVE,
+						    BCMA_CCTRL_43224A0_12MA_LED_DRIVE);
 		} else {
-			bcma_chipco_chipctl_maskset(cc, 0, ~0, 0xF0);
+			bcma_chipco_chipctl_maskset(cc, 0,
+						    ~BCMA_CCTRL_43224B0_12MA_LED_DRIVE,
+						    BCMA_CCTRL_43224B0_12MA_LED_DRIVE);
 		}
 		break;
-	case 43225:
-		break;
 	default:
-		pr_err("Workarounds unknown for device 0x%04X\n",
-			bus->chipinfo.id);
+		bcma_debug(bus, "Workarounds unknown or not needed for device 0x%04X\n",
+			   bus->chipinfo.id);
 	}
 }
 
@@ -167,8 +148,8 @@ void bcma_pmu_init(struct bcma_drv_cc *cc)
 	pmucap = bcma_cc_read32(cc, BCMA_CC_PMU_CAP);
 	cc->pmu.rev = (pmucap & BCMA_CC_PMU_CAP_REVISION);
 
-	pr_debug("Found rev %u PMU (capabilities 0x%08X)\n", cc->pmu.rev,
-		 pmucap);
+	bcma_debug(cc->core->bus, "Found rev %u PMU (capabilities 0x%08X)\n",
+		   cc->pmu.rev, pmucap);
 
 	if (cc->pmu.rev == 1)
 		bcma_cc_mask32(cc, BCMA_CC_PMU_CTL,
@@ -177,12 +158,7 @@ void bcma_pmu_init(struct bcma_drv_cc *cc)
 		bcma_cc_set32(cc, BCMA_CC_PMU_CTL,
 			     BCMA_CC_PMU_CTL_NOILPONW);
 
-	if (cc->core->id.id == 0x4329 && cc->core->id.rev == 2)
-		pr_err("Fix for 4329b0 bad LPOM state not implemented!\n");
-
-	bcma_pmu_pll_init(cc);
 	bcma_pmu_resources_init(cc);
-	bcma_pmu_swreg_init(cc);
 	bcma_pmu_workarounds(cc);
 }
 
@@ -191,23 +167,22 @@ u32 bcma_pmu_alp_clock(struct bcma_drv_cc *cc)
 	struct bcma_bus *bus = cc->core->bus;
 
 	switch (bus->chipinfo.id) {
-	case 0x4716:
-	case 0x4748:
-	case 47162:
-	case 0x4313:
-	case 0x5357:
-	case 0x4749:
-	case 53572:
+	case BCMA_CHIP_ID_BCM4716:
+	case BCMA_CHIP_ID_BCM4748:
+	case BCMA_CHIP_ID_BCM47162:
+	case BCMA_CHIP_ID_BCM4313:
+	case BCMA_CHIP_ID_BCM5357:
+	case BCMA_CHIP_ID_BCM4749:
+	case BCMA_CHIP_ID_BCM53572:
 		/* always 20Mhz */
 		return 20000 * 1000;
-	case 0x5356:
-	case 0x5300:
+	case BCMA_CHIP_ID_BCM5356:
+	case BCMA_CHIP_ID_BCM4706:
 		/* always 25Mhz */
 		return 25000 * 1000;
 	default:
-		pr_warn("No ALP clock specified for %04X device, "
-			"pmu rev. %d, using default %d Hz\n",
-			bus->chipinfo.id, cc->pmu.rev, BCMA_CC_PMU_ALP_CLOCK);
+		bcma_warn(bus, "No ALP clock specified for %04X device, pmu rev. %d, using default %d Hz\n",
+			  bus->chipinfo.id, cc->pmu.rev, BCMA_CC_PMU_ALP_CLOCK);
 	}
 	return BCMA_CC_PMU_ALP_CLOCK;
 }
@@ -224,7 +199,8 @@ static u32 bcma_pmu_clock(struct bcma_drv_cc *cc, u32 pll0, u32 m)
 
 	BUG_ON(!m || m > 4);
 
-	if (bus->chipinfo.id == 0x5357 || bus->chipinfo.id == 0x4749) {
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM5357 ||
+	    bus->chipinfo.id == BCMA_CHIP_ID_BCM4749) {
 		/* Detect failure in clock setting */
 		tmp = bcma_cc_read32(cc, BCMA_CC_CHIPSTAT);
 		if (tmp & 0x40000)
@@ -250,33 +226,62 @@ static u32 bcma_pmu_clock(struct bcma_drv_cc *cc, u32 pll0, u32 m)
 	return (fc / div) * 1000000;
 }
 
+static u32 bcma_pmu_clock_bcm4706(struct bcma_drv_cc *cc, u32 pll0, u32 m)
+{
+	u32 tmp, ndiv, p1div, p2div;
+	u32 clock;
+
+	BUG_ON(!m || m > 4);
+
+	/* Get N, P1 and P2 dividers to determine CPU clock */
+	tmp = bcma_chipco_pll_read(cc, pll0 + BCMA_CC_PMU6_4706_PROCPLL_OFF);
+	ndiv = (tmp & BCMA_CC_PMU6_4706_PROC_NDIV_INT_MASK)
+		>> BCMA_CC_PMU6_4706_PROC_NDIV_INT_SHIFT;
+	p1div = (tmp & BCMA_CC_PMU6_4706_PROC_P1DIV_MASK)
+		>> BCMA_CC_PMU6_4706_PROC_P1DIV_SHIFT;
+	p2div = (tmp & BCMA_CC_PMU6_4706_PROC_P2DIV_MASK)
+		>> BCMA_CC_PMU6_4706_PROC_P2DIV_SHIFT;
+
+	tmp = bcma_cc_read32(cc, BCMA_CC_CHIPSTAT);
+	if (tmp & BCMA_CC_CHIPST_4706_PKG_OPTION)
+		/* Low cost bonding: Fixed reference clock 25MHz and m = 4 */
+		clock = (25000000 / 4) * ndiv * p2div / p1div;
+	else
+		/* Fixed reference clock 25MHz and m = 2 */
+		clock = (25000000 / 2) * ndiv * p2div / p1div;
+
+	if (m == BCMA_CC_PMU5_MAINPLL_SSB)
+		clock = clock / 4;
+
+	return clock;
+}
+
 /* query bus clock frequency for PMU-enabled chipcommon */
 u32 bcma_pmu_get_clockcontrol(struct bcma_drv_cc *cc)
 {
 	struct bcma_bus *bus = cc->core->bus;
 
 	switch (bus->chipinfo.id) {
-	case 0x4716:
-	case 0x4748:
-	case 47162:
+	case BCMA_CHIP_ID_BCM4716:
+	case BCMA_CHIP_ID_BCM4748:
+	case BCMA_CHIP_ID_BCM47162:
 		return bcma_pmu_clock(cc, BCMA_CC_PMU4716_MAINPLL_PLL0,
 				      BCMA_CC_PMU5_MAINPLL_SSB);
-	case 0x5356:
+	case BCMA_CHIP_ID_BCM5356:
 		return bcma_pmu_clock(cc, BCMA_CC_PMU5356_MAINPLL_PLL0,
 				      BCMA_CC_PMU5_MAINPLL_SSB);
-	case 0x5357:
-	case 0x4749:
+	case BCMA_CHIP_ID_BCM5357:
+	case BCMA_CHIP_ID_BCM4749:
 		return bcma_pmu_clock(cc, BCMA_CC_PMU5357_MAINPLL_PLL0,
 				      BCMA_CC_PMU5_MAINPLL_SSB);
-	case 0x5300:
-		return bcma_pmu_clock(cc, BCMA_CC_PMU4706_MAINPLL_PLL0,
-				      BCMA_CC_PMU5_MAINPLL_SSB);
-	case 53572:
+	case BCMA_CHIP_ID_BCM4706:
+		return bcma_pmu_clock_bcm4706(cc, BCMA_CC_PMU4706_MAINPLL_PLL0,
+					      BCMA_CC_PMU5_MAINPLL_SSB);
+	case BCMA_CHIP_ID_BCM53572:
 		return 75000000;
 	default:
-		pr_warn("No backplane clock specified for %04X device, "
-			"pmu rev. %d, using default %d Hz\n",
-			bus->chipinfo.id, cc->pmu.rev, BCMA_CC_PMU_HT_CLOCK);
+		bcma_warn(bus, "No backplane clock specified for %04X device, pmu rev. %d, using default %d Hz\n",
+			  bus->chipinfo.id, cc->pmu.rev, BCMA_CC_PMU_HT_CLOCK);
 	}
 	return BCMA_CC_PMU_HT_CLOCK;
 }
@@ -286,17 +291,21 @@ u32 bcma_pmu_get_clockcpu(struct bcma_drv_cc *cc)
 {
 	struct bcma_bus *bus = cc->core->bus;
 
-	if (bus->chipinfo.id == 53572)
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM53572)
 		return 300000000;
 
 	if (cc->pmu.rev >= 5) {
 		u32 pll;
 		switch (bus->chipinfo.id) {
-		case 0x5356:
+		case BCMA_CHIP_ID_BCM4706:
+			return bcma_pmu_clock_bcm4706(cc,
+						BCMA_CC_PMU4706_MAINPLL_PLL0,
+						BCMA_CC_PMU5_MAINPLL_CPU);
+		case BCMA_CHIP_ID_BCM5356:
 			pll = BCMA_CC_PMU5356_MAINPLL_PLL0;
 			break;
-		case 0x5357:
-		case 0x4749:
+		case BCMA_CHIP_ID_BCM5357:
+		case BCMA_CHIP_ID_BCM4749:
 			pll = BCMA_CC_PMU5357_MAINPLL_PLL0;
 			break;
 		default:
@@ -304,10 +313,188 @@ u32 bcma_pmu_get_clockcpu(struct bcma_drv_cc *cc)
 			break;
 		}
 
-		/* TODO: if (bus->chipinfo.id == 0x5300)
-		  return si_4706_pmu_clock(sih, osh, cc, PMU4706_MAINPLL_PLL0, PMU5_MAINPLL_CPU); */
 		return bcma_pmu_clock(cc, pll, BCMA_CC_PMU5_MAINPLL_CPU);
 	}
 
 	return bcma_pmu_get_clockcontrol(cc);
 }
+
+static void bcma_pmu_spuravoid_pll_write(struct bcma_drv_cc *cc, u32 offset,
+					 u32 value)
+{
+	bcma_cc_write32(cc, BCMA_CC_PLLCTL_ADDR, offset);
+	bcma_cc_write32(cc, BCMA_CC_PLLCTL_DATA, value);
+}
+
+void bcma_pmu_spuravoid_pllupdate(struct bcma_drv_cc *cc, int spuravoid)
+{
+	u32 tmp = 0;
+	u8 phypll_offset = 0;
+	u8 bcm5357_bcm43236_p1div[] = {0x1, 0x5, 0x5};
+	u8 bcm5357_bcm43236_ndiv[] = {0x30, 0xf6, 0xfc};
+	struct bcma_bus *bus = cc->core->bus;
+
+	switch (bus->chipinfo.id) {
+	case BCMA_CHIP_ID_BCM5357:
+	case BCMA_CHIP_ID_BCM4749:
+	case BCMA_CHIP_ID_BCM53572:
+		/* 5357[ab]0, 43236[ab]0, and 6362b0 */
+
+		/* BCM5357 needs to touch PLL1_PLLCTL[02],
+		   so offset PLL0_PLLCTL[02] by 6 */
+		phypll_offset = (bus->chipinfo.id == BCMA_CHIP_ID_BCM5357 ||
+		       bus->chipinfo.id == BCMA_CHIP_ID_BCM4749 ||
+		       bus->chipinfo.id == BCMA_CHIP_ID_BCM53572) ? 6 : 0;
+
+		/* RMW only the P1 divider */
+		bcma_cc_write32(cc, BCMA_CC_PLLCTL_ADDR,
+				BCMA_CC_PMU_PLL_CTL0 + phypll_offset);
+		tmp = bcma_cc_read32(cc, BCMA_CC_PLLCTL_DATA);
+		tmp &= (~(BCMA_CC_PMU1_PLL0_PC0_P1DIV_MASK));
+		tmp |= (bcm5357_bcm43236_p1div[spuravoid] << BCMA_CC_PMU1_PLL0_PC0_P1DIV_SHIFT);
+		bcma_cc_write32(cc, BCMA_CC_PLLCTL_DATA, tmp);
+
+		/* RMW only the int feedback divider */
+		bcma_cc_write32(cc, BCMA_CC_PLLCTL_ADDR,
+				BCMA_CC_PMU_PLL_CTL2 + phypll_offset);
+		tmp = bcma_cc_read32(cc, BCMA_CC_PLLCTL_DATA);
+		tmp &= ~(BCMA_CC_PMU1_PLL0_PC2_NDIV_INT_MASK);
+		tmp |= (bcm5357_bcm43236_ndiv[spuravoid]) << BCMA_CC_PMU1_PLL0_PC2_NDIV_INT_SHIFT;
+		bcma_cc_write32(cc, BCMA_CC_PLLCTL_DATA, tmp);
+
+		tmp = 1 << 10;
+		break;
+
+	case BCMA_CHIP_ID_BCM4331:
+	case BCMA_CHIP_ID_BCM43431:
+		if (spuravoid == 2) {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11500014);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x0FC00a08);
+		} else if (spuravoid == 1) {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11500014);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x0F600a08);
+		} else {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11100014);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x03000a08);
+		}
+		tmp = 1 << 10;
+		break;
+
+	case BCMA_CHIP_ID_BCM43224:
+	case BCMA_CHIP_ID_BCM43225:
+	case BCMA_CHIP_ID_BCM43421:
+		if (spuravoid == 1) {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11500010);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL1,
+						     0x000C0C06);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x0F600a08);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL3,
+						     0x00000000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL4,
+						     0x2001E920);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
+						     0x88888815);
+		} else {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11100010);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL1,
+						     0x000c0c06);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x03000a08);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL3,
+						     0x00000000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL4,
+						     0x200005c0);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
+						     0x88888815);
+		}
+		tmp = 1 << 10;
+		break;
+
+	case BCMA_CHIP_ID_BCM4716:
+	case BCMA_CHIP_ID_BCM4748:
+	case BCMA_CHIP_ID_BCM47162:
+		if (spuravoid == 1) {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11500060);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL1,
+						     0x080C0C06);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x0F600000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL3,
+						     0x00000000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL4,
+						     0x2001E924);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
+						     0x88888815);
+		} else {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11100060);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL1,
+						     0x080c0c06);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x03000000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL3,
+						     0x00000000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL4,
+						     0x200005c0);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
+						     0x88888815);
+		}
+
+		tmp = 3 << 9;
+		break;
+
+	case BCMA_CHIP_ID_BCM43227:
+	case BCMA_CHIP_ID_BCM43228:
+	case BCMA_CHIP_ID_BCM43428:
+		/* LCNXN */
+		/* PLL Settings for spur avoidance on/off mode,
+		   no on2 support for 43228A0 */
+		if (spuravoid == 1) {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x01100014);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL1,
+						     0x040C0C06);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x03140A08);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL3,
+						     0x00333333);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL4,
+						     0x202C2820);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
+						     0x88888815);
+		} else {
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL0,
+						     0x11100014);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL1,
+						     0x040c0c06);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL2,
+						     0x03000a08);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL3,
+						     0x00000000);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL4,
+						     0x200005c0);
+			bcma_pmu_spuravoid_pll_write(cc, BCMA_CC_PMU_PLL_CTL5,
+						     0x88888815);
+		}
+		tmp = 1 << 10;
+		break;
+	default:
+		bcma_err(bus, "Unknown spuravoidance settings for chip 0x%04X, not changing PLL\n",
+			 bus->chipinfo.id);
+		break;
+	}
+
+	tmp |= bcma_cc_read32(cc, BCMA_CC_PMU_CTL);
+	bcma_cc_write32(cc, BCMA_CC_PMU_CTL, tmp);
+}
+EXPORT_SYMBOL_GPL(bcma_pmu_spuravoid_pllupdate);
diff --git a/drivers/bcma/driver_chipcommon_sflash.c b/drivers/bcma/driver_chipcommon_sflash.c
new file mode 100644
index 00000000000..6e157a58a1d
--- /dev/null
+++ b/drivers/bcma/driver_chipcommon_sflash.c
@@ -0,0 +1,19 @@
+/*
+ * Broadcom specific AMBA
+ * ChipCommon serial flash interface
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include <linux/bcma/bcma.h>
+#include <linux/bcma/bcma_driver_chipcommon.h>
+#include <linux/delay.h>
+
+#include "bcma_private.h"
+
+/* Initialize serial flash access */
+int bcma_sflash_init(struct bcma_drv_cc *cc)
+{
+	bcma_err(cc->core->bus, "Serial flash support is broken\n");
+	return 0;
+}
diff --git a/drivers/bcma/driver_gmac_cmn.c b/drivers/bcma/driver_gmac_cmn.c
new file mode 100644
index 00000000000..834225f65e8
--- /dev/null
+++ b/drivers/bcma/driver_gmac_cmn.c
@@ -0,0 +1,14 @@
+/*
+ * Broadcom specific AMBA
+ * GBIT MAC COMMON Core
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include "bcma_private.h"
+#include <linux/bcma/bcma.h>
+
+void __devinit bcma_core_gmac_cmn_init(struct bcma_drv_gmac_cmn *gc)
+{
+	mutex_init(&gc->phy_mutex);
+}
diff --git a/drivers/bcma/driver_mips.c b/drivers/bcma/driver_mips.c
index c3e9dff4224..cc65b45b436 100644
--- a/drivers/bcma/driver_mips.c
+++ b/drivers/bcma/driver_mips.c
@@ -22,15 +22,15 @@
 /* The 47162a0 hangs when reading MIPS DMP registers registers */
 static inline bool bcma_core_mips_bcm47162a0_quirk(struct bcma_device *dev)
 {
-	return dev->bus->chipinfo.id == 47162 && dev->bus->chipinfo.rev == 0 &&
-	       dev->id.id == BCMA_CORE_MIPS_74K;
+	return dev->bus->chipinfo.id == BCMA_CHIP_ID_BCM47162 &&
+	       dev->bus->chipinfo.rev == 0 && dev->id.id == BCMA_CORE_MIPS_74K;
 }
 
 /* The 5357b0 hangs when reading USB20H DMP registers */
 static inline bool bcma_core_mips_bcm5357b0_quirk(struct bcma_device *dev)
 {
-	return (dev->bus->chipinfo.id == 0x5357 ||
-		dev->bus->chipinfo.id == 0x4749) &&
+	return (dev->bus->chipinfo.id == BCMA_CHIP_ID_BCM5357 ||
+		dev->bus->chipinfo.id == BCMA_CHIP_ID_BCM4749) &&
 	       dev->bus->chipinfo.pkg == 11 &&
 	       dev->id.id == BCMA_CORE_USB20_HOST;
 }
@@ -131,7 +131,7 @@ static void bcma_core_mips_set_irq(struct bcma_device *dev, unsigned int irq)
 			/* backplane irq line is in use, find out who uses
 			 * it and set user to irq 0
 			 */
-			list_for_each_entry_reverse(core, &bus->cores, list) {
+			list_for_each_entry(core, &bus->cores, list) {
 				if ((1 << bcma_core_mips_irqflag(core)) ==
 				    oldirqflag) {
 					bcma_core_mips_set_irq(core, 0);
@@ -143,8 +143,8 @@ static void bcma_core_mips_set_irq(struct bcma_device *dev, unsigned int irq)
 			     1 << irqflag);
 	}
 
-	pr_info("set_irq: core 0x%04x, irq %d => %d\n",
-		dev->id.id, oldirq + 2, irq + 2);
+	bcma_info(bus, "set_irq: core 0x%04x, irq %d => %d\n",
+		  dev->id.id, oldirq + 2, irq + 2);
 }
 
 static void bcma_core_mips_print_irq(struct bcma_device *dev, unsigned int irq)
@@ -161,7 +161,7 @@ static void bcma_core_mips_dump_irq(struct bcma_bus *bus)
 {
 	struct bcma_device *core;
 
-	list_for_each_entry_reverse(core, &bus->cores, list) {
+	list_for_each_entry(core, &bus->cores, list) {
 		bcma_core_mips_print_irq(core, bcma_core_mips_irq(core));
 	}
 }
@@ -173,7 +173,7 @@ u32 bcma_cpu_clock(struct bcma_drv_mips *mcore)
 	if (bus->drv_cc.capabilities & BCMA_CC_CAP_PMU)
 		return bcma_pmu_get_clockcpu(&bus->drv_cc);
 
-	pr_err("No PMU available, need this to get the cpu clock\n");
+	bcma_err(bus, "No PMU available, need this to get the cpu clock\n");
 	return 0;
 }
 EXPORT_SYMBOL(bcma_cpu_clock);
@@ -185,10 +185,11 @@ static void bcma_core_mips_flash_detect(struct bcma_drv_mips *mcore)
 	switch (bus->drv_cc.capabilities & BCMA_CC_CAP_FLASHT) {
 	case BCMA_CC_FLASHT_STSER:
 	case BCMA_CC_FLASHT_ATSER:
-		pr_err("Serial flash not supported.\n");
+		bcma_debug(bus, "Found serial flash\n");
+		bcma_sflash_init(&bus->drv_cc);
 		break;
 	case BCMA_CC_FLASHT_PARA:
-		pr_info("found parallel flash.\n");
+		bcma_debug(bus, "Found parallel flash\n");
 		bus->drv_cc.pflash.window = 0x1c000000;
 		bus->drv_cc.pflash.window_size = 0x02000000;
 
@@ -199,7 +200,15 @@ static void bcma_core_mips_flash_detect(struct bcma_drv_mips *mcore)
 			bus->drv_cc.pflash.buswidth = 2;
 		break;
 	default:
-		pr_err("flash not supported.\n");
+		bcma_err(bus, "Flash type not supported\n");
+	}
+
+	if (bus->drv_cc.core->id.rev == 38 ||
+	    bus->chipinfo.id == BCMA_CHIP_ID_BCM4706) {
+		if (bus->drv_cc.capabilities & BCMA_CC_CAP_NFLASH) {
+			bcma_debug(bus, "Found NAND flash\n");
+			bcma_nflash_init(&bus->drv_cc);
+		}
 	}
 }
 
@@ -209,13 +218,13 @@ void bcma_core_mips_init(struct bcma_drv_mips *mcore)
 	struct bcma_device *core;
 	bus = mcore->core->bus;
 
-	pr_info("Initializing MIPS core...\n");
+	bcma_info(bus, "Initializing MIPS core...\n");
 
 	if (!mcore->setup_done)
 		mcore->assigned_irqs = 1;
 
 	/* Assign IRQs to all cores on the bus */
-	list_for_each_entry_reverse(core, &bus->cores, list) {
+	list_for_each_entry(core, &bus->cores, list) {
 		int mips_irq;
 		if (core->irq)
 			continue;
@@ -244,7 +253,7 @@ void bcma_core_mips_init(struct bcma_drv_mips *mcore)
 			break;
 		}
 	}
-	pr_info("IRQ reconfiguration done\n");
+	bcma_info(bus, "IRQ reconfiguration done\n");
 	bcma_core_mips_dump_irq(bus);
 
 	if (mcore->setup_done)
diff --git a/drivers/bcma/driver_pci_host.c b/drivers/bcma/driver_pci_host.c
index b9a86edfec3..cbae2c23133 100644
--- a/drivers/bcma/driver_pci_host.c
+++ b/drivers/bcma/driver_pci_host.c
@@ -36,7 +36,7 @@ bool __devinit bcma_core_pci_is_in_hostmode(struct bcma_drv_pci *pc)
 		return false;
 
 	if (bus->sprom.boardflags_lo & BCMA_CORE_PCI_BFL_NOPCI) {
-		pr_info("This PCI core is disabled and not working\n");
+		bcma_info(bus, "This PCI core is disabled and not working\n");
 		return false;
 	}
 
@@ -215,7 +215,8 @@ static int bcma_extpci_write_config(struct bcma_drv_pci *pc, unsigned int dev,
 	} else {
 		writel(val, mmio);
 
-		if (chipid == 0x4716 || chipid == 0x4748)
+		if (chipid == BCMA_CHIP_ID_BCM4716 ||
+		    chipid == BCMA_CHIP_ID_BCM4748)
 			readl(mmio);
 	}
 
@@ -340,6 +341,7 @@ static u8 __devinit bcma_find_pci_capability(struct bcma_drv_pci *pc,
  */
 static void __devinit bcma_core_pci_enable_crs(struct bcma_drv_pci *pc)
 {
+	struct bcma_bus *bus = pc->core->bus;
 	u8 cap_ptr, root_ctrl, root_cap, dev;
 	u16 val16;
 	int i;
@@ -378,7 +380,8 @@ static void __devinit bcma_core_pci_enable_crs(struct bcma_drv_pci *pc)
 				udelay(10);
 			}
 			if (val16 == 0x1)
-				pr_err("PCI: Broken device in slot %d\n", dev);
+				bcma_err(bus, "PCI: Broken device in slot %d\n",
+					 dev);
 		}
 	}
 }
@@ -391,11 +394,11 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
 	u32 pci_membase_1G;
 	unsigned long io_map_base;
 
-	pr_info("PCIEcore in host mode found\n");
+	bcma_info(bus, "PCIEcore in host mode found\n");
 
 	pc_host = kzalloc(sizeof(*pc_host), GFP_KERNEL);
 	if (!pc_host)  {
-		pr_err("can not allocate memory");
+		bcma_err(bus, "can not allocate memory");
 		return;
 	}
 
@@ -434,13 +437,14 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
 	 * as mips can't generate 64-bit address on the
 	 * backplane.
 	 */
-	if (bus->chipinfo.id == 0x4716 || bus->chipinfo.id == 0x4748) {
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4716 ||
+	    bus->chipinfo.id == BCMA_CHIP_ID_BCM4748) {
 		pc_host->mem_resource.start = BCMA_SOC_PCI_MEM;
 		pc_host->mem_resource.end = BCMA_SOC_PCI_MEM +
 					    BCMA_SOC_PCI_MEM_SZ - 1;
 		pcicore_write32(pc, BCMA_CORE_PCI_SBTOPCI0,
 				BCMA_CORE_PCI_SBTOPCI_MEM | BCMA_SOC_PCI_MEM);
-	} else if (bus->chipinfo.id == 0x5300) {
+	} else if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4706) {
 		tmp = BCMA_CORE_PCI_SBTOPCI_MEM;
 		tmp |= BCMA_CORE_PCI_SBTOPCI_PREF;
 		tmp |= BCMA_CORE_PCI_SBTOPCI_BURST;
diff --git a/drivers/bcma/host_pci.c b/drivers/bcma/host_pci.c
index 6c05cf470f9..11b32d2642d 100644
--- a/drivers/bcma/host_pci.c
+++ b/drivers/bcma/host_pci.c
@@ -18,7 +18,7 @@ static void bcma_host_pci_switch_core(struct bcma_device *core)
 	pci_write_config_dword(core->bus->host_pci, BCMA_PCI_BAR0_WIN2,
 			       core->wrap);
 	core->bus->mapped_core = core;
-	pr_debug("Switched to core: 0x%X\n", core->id.id);
+	bcma_debug(core->bus, "Switched to core: 0x%X\n", core->id.id);
 }
 
 /* Provides access to the requested core. Returns base offset that has to be
@@ -188,7 +188,7 @@ static int __devinit bcma_host_pci_probe(struct pci_dev *dev,
 
 	/* SSB needed additional powering up, do we have any AMBA PCI cards? */
 	if (!pci_is_pcie(dev))
-		pr_err("PCI card detected, report problems.\n");
+		bcma_err(bus, "PCI card detected, report problems.\n");
 
 	/* Map MMIO */
 	err = -ENOMEM;
@@ -268,6 +268,7 @@ static SIMPLE_DEV_PM_OPS(bcma_pm_ops, bcma_host_pci_suspend,
 
 static DEFINE_PCI_DEVICE_TABLE(bcma_pci_bridge_tbl) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x0576) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 43224) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4331) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4353) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4357) },
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 7e138ec2135..758af9ccdef 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -61,6 +61,13 @@ static struct bus_type bcma_bus_type = {
 	.dev_attrs	= bcma_device_attrs,
 };
 
+static u16 bcma_cc_core_id(struct bcma_bus *bus)
+{
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4706)
+		return BCMA_CORE_4706_CHIPCOMMON;
+	return BCMA_CORE_CHIPCOMMON;
+}
+
 struct bcma_device *bcma_find_core(struct bcma_bus *bus, u16 coreid)
 {
 	struct bcma_device *core;
@@ -91,10 +98,12 @@ static int bcma_register_cores(struct bcma_bus *bus)
 	list_for_each_entry(core, &bus->cores, list) {
 		/* We support that cores ourself */
 		switch (core->id.id) {
+		case BCMA_CORE_4706_CHIPCOMMON:
 		case BCMA_CORE_CHIPCOMMON:
 		case BCMA_CORE_PCI:
 		case BCMA_CORE_PCIE:
 		case BCMA_CORE_MIPS_74K:
+		case BCMA_CORE_4706_MAC_GBIT_COMMON:
 			continue;
 		}
 
@@ -118,8 +127,9 @@ static int bcma_register_cores(struct bcma_bus *bus)
 
 		err = device_register(&core->dev);
 		if (err) {
-			pr_err("Could not register dev for core 0x%03X\n",
-			       core->id.id);
+			bcma_err(bus,
+				 "Could not register dev for core 0x%03X\n",
+				 core->id.id);
 			continue;
 		}
 		core->dev_registered = true;
@@ -151,12 +161,12 @@ int __devinit bcma_bus_register(struct bcma_bus *bus)
 	/* Scan for devices (cores) */
 	err = bcma_bus_scan(bus);
 	if (err) {
-		pr_err("Failed to scan: %d\n", err);
+		bcma_err(bus, "Failed to scan: %d\n", err);
 		return -1;
 	}
 
 	/* Init CC core */
-	core = bcma_find_core(bus, BCMA_CORE_CHIPCOMMON);
+	core = bcma_find_core(bus, bcma_cc_core_id(bus));
 	if (core) {
 		bus->drv_cc.core = core;
 		bcma_core_chipcommon_init(&bus->drv_cc);
@@ -176,17 +186,24 @@ int __devinit bcma_bus_register(struct bcma_bus *bus)
 		bcma_core_pci_init(&bus->drv_pci);
 	}
 
+	/* Init GBIT MAC COMMON core */
+	core = bcma_find_core(bus, BCMA_CORE_4706_MAC_GBIT_COMMON);
+	if (core) {
+		bus->drv_gmac_cmn.core = core;
+		bcma_core_gmac_cmn_init(&bus->drv_gmac_cmn);
+	}
+
 	/* Try to get SPROM */
 	err = bcma_sprom_get(bus);
 	if (err == -ENOENT) {
-		pr_err("No SPROM available\n");
+		bcma_err(bus, "No SPROM available\n");
 	} else if (err)
-		pr_err("Failed to get SPROM: %d\n", err);
+		bcma_err(bus, "Failed to get SPROM: %d\n", err);
 
 	/* Register found cores */
 	bcma_register_cores(bus);
 
-	pr_info("Bus registered\n");
+	bcma_info(bus, "Bus registered\n");
 
 	return 0;
 }
@@ -207,14 +224,14 @@ int __init bcma_bus_early_register(struct bcma_bus *bus,
 	bcma_init_bus(bus);
 
 	match.manuf = BCMA_MANUF_BCM;
-	match.id = BCMA_CORE_CHIPCOMMON;
+	match.id = bcma_cc_core_id(bus);
 	match.class = BCMA_CL_SIM;
 	match.rev = BCMA_ANY_REV;
 
 	/* Scan for chip common core */
 	err = bcma_bus_scan_early(bus, &match, core_cc);
 	if (err) {
-		pr_err("Failed to scan for common core: %d\n", err);
+		bcma_err(bus, "Failed to scan for common core: %d\n", err);
 		return -1;
 	}
 
@@ -226,12 +243,12 @@ int __init bcma_bus_early_register(struct bcma_bus *bus,
 	/* Scan for mips core */
 	err = bcma_bus_scan_early(bus, &match, core_mips);
 	if (err) {
-		pr_err("Failed to scan for mips core: %d\n", err);
+		bcma_err(bus, "Failed to scan for mips core: %d\n", err);
 		return -1;
 	}
 
 	/* Init CC core */
-	core = bcma_find_core(bus, BCMA_CORE_CHIPCOMMON);
+	core = bcma_find_core(bus, bcma_cc_core_id(bus));
 	if (core) {
 		bus->drv_cc.core = core;
 		bcma_core_chipcommon_init(&bus->drv_cc);
@@ -244,7 +261,7 @@ int __init bcma_bus_early_register(struct bcma_bus *bus,
 		bcma_core_mips_init(&bus->drv_mips);
 	}
 
-	pr_info("Early bus registered\n");
+	bcma_info(bus, "Early bus registered\n");
 
 	return 0;
 }
@@ -270,8 +287,7 @@ int bcma_bus_resume(struct bcma_bus *bus)
 	struct bcma_device *core;
 
 	/* Init CC core */
-	core = bcma_find_core(bus, BCMA_CORE_CHIPCOMMON);
-	if (core) {
+	if (bus->drv_cc.core) {
 		bus->drv_cc.setup_done = false;
 		bcma_core_chipcommon_init(&bus->drv_cc);
 	}
diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index 5ed0718fc66..8d0b5716401 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -21,6 +21,7 @@ struct bcma_device_id_name {
 };
 
 static const struct bcma_device_id_name bcma_arm_device_names[] = {
+	{ BCMA_CORE_4706_MAC_GBIT_COMMON, "BCM4706 GBit MAC Common" },
 	{ BCMA_CORE_ARM_1176, "ARM 1176" },
 	{ BCMA_CORE_ARM_7TDMI, "ARM 7TDMI" },
 	{ BCMA_CORE_ARM_CM3, "ARM CM3" },
@@ -28,6 +29,11 @@ static const struct bcma_device_id_name bcma_arm_device_names[] = {
 
 static const struct bcma_device_id_name bcma_bcm_device_names[] = {
 	{ BCMA_CORE_OOB_ROUTER, "OOB Router" },
+	{ BCMA_CORE_4706_CHIPCOMMON, "BCM4706 ChipCommon" },
+	{ BCMA_CORE_4706_SOC_RAM, "BCM4706 SOC RAM" },
+	{ BCMA_CORE_4706_MAC_GBIT, "BCM4706 GBit MAC" },
+	{ BCMA_CORE_AMEMC, "AMEMC (DDR)" },
+	{ BCMA_CORE_ALTA, "ALTA (I2S)" },
 	{ BCMA_CORE_INVALID, "Invalid" },
 	{ BCMA_CORE_CHIPCOMMON, "ChipCommon" },
 	{ BCMA_CORE_ILINE20, "ILine 20" },
@@ -289,11 +295,15 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 
 	/* check if component is a core at all */
 	if (wrappers[0] + wrappers[1] == 0) {
-		/* we could save addrl of the router
-		if (cid == BCMA_CORE_OOB_ROUTER)
-		 */
-		bcma_erom_skip_component(bus, eromptr);
-		return -ENXIO;
+		/* Some specific cores don't need wrappers */
+		switch (core->id.id) {
+		case BCMA_CORE_4706_MAC_GBIT_COMMON:
+		/* Not used yet: case BCMA_CORE_OOB_ROUTER: */
+			break;
+		default:
+			bcma_erom_skip_component(bus, eromptr);
+			return -ENXIO;
+		}
 	}
 
 	if (bcma_erom_is_bridge(bus, eromptr)) {
@@ -334,7 +344,7 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 		if (tmp <= 0) {
 			return -EILSEQ;
 		} else {
-			pr_info("Bridge found\n");
+			bcma_info(bus, "Bridge found\n");
 			return -ENXIO;
 		}
 	}
@@ -421,8 +431,8 @@ void bcma_init_bus(struct bcma_bus *bus)
 	chipinfo->id = (tmp & BCMA_CC_ID_ID) >> BCMA_CC_ID_ID_SHIFT;
 	chipinfo->rev = (tmp & BCMA_CC_ID_REV) >> BCMA_CC_ID_REV_SHIFT;
 	chipinfo->pkg = (tmp & BCMA_CC_ID_PKG) >> BCMA_CC_ID_PKG_SHIFT;
-	pr_info("Found chip with id 0x%04X, rev 0x%02X and package 0x%02X\n",
-		chipinfo->id, chipinfo->rev, chipinfo->pkg);
+	bcma_info(bus, "Found chip with id 0x%04X, rev 0x%02X and package 0x%02X\n",
+		  chipinfo->id, chipinfo->rev, chipinfo->pkg);
 
 	bus->init_done = true;
 }
@@ -452,8 +462,10 @@ int bcma_bus_scan(struct bcma_bus *bus)
 	while (eromptr < eromend) {
 		struct bcma_device *other_core;
 		struct bcma_device *core = kzalloc(sizeof(*core), GFP_KERNEL);
-		if (!core)
-			return -ENOMEM;
+		if (!core) {
+			err = -ENOMEM;
+			goto out;
+		}
 		INIT_LIST_HEAD(&core->list);
 		core->bus = bus;
 
@@ -468,7 +480,7 @@ int bcma_bus_scan(struct bcma_bus *bus)
 			} else if (err == -ESPIPE) {
 				break;
 			}
-			return err;
+			goto out;
 		}
 
 		core->core_index = core_num++;
@@ -476,19 +488,20 @@ int bcma_bus_scan(struct bcma_bus *bus)
 		other_core = bcma_find_core_reverse(bus, core->id.id);
 		core->core_unit = (other_core == NULL) ? 0 : other_core->core_unit + 1;
 
-		pr_info("Core %d found: %s "
-			"(manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",
-			core->core_index, bcma_device_name(&core->id),
-			core->id.manuf, core->id.id, core->id.rev,
-			core->id.class);
+		bcma_info(bus, "Core %d found: %s (manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",
+			  core->core_index, bcma_device_name(&core->id),
+			  core->id.manuf, core->id.id, core->id.rev,
+			  core->id.class);
 
-		list_add(&core->list, &bus->cores);
+		list_add_tail(&core->list, &bus->cores);
 	}
 
+	err = 0;
+out:
 	if (bus->hosttype == BCMA_HOSTTYPE_SOC)
 		iounmap(eromptr);
 
-	return 0;
+	return err;
 }
 
 int __init bcma_bus_scan_early(struct bcma_bus *bus,
@@ -528,21 +541,21 @@ int __init bcma_bus_scan_early(struct bcma_bus *bus,
 		else if (err == -ESPIPE)
 			break;
 		else if (err < 0)
-			return err;
+			goto out;
 
 		core->core_index = core_num++;
 		bus->nr_cores++;
-		pr_info("Core %d found: %s "
-			"(manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",
-			core->core_index, bcma_device_name(&core->id),
-			core->id.manuf, core->id.id, core->id.rev,
-			core->id.class);
+		bcma_info(bus, "Core %d found: %s (manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",
+			  core->core_index, bcma_device_name(&core->id),
+			  core->id.manuf, core->id.id, core->id.rev,
+			  core->id.class);
 
-		list_add(&core->list, &bus->cores);
+		list_add_tail(&core->list, &bus->cores);
 		err = 0;
 		break;
 	}
 
+out:
 	if (bus->hosttype == BCMA_HOSTTYPE_SOC)
 		iounmap(eromptr);
 
diff --git a/drivers/bcma/scan.h b/drivers/bcma/scan.h
index 113e6a66884..30eb475e4d1 100644
--- a/drivers/bcma/scan.h
+++ b/drivers/bcma/scan.h
@@ -27,7 +27,7 @@
 #define SCAN_CIB_NMW		0x0007C000
 #define SCAN_CIB_NMW_SHIFT	14
 #define SCAN_CIB_NSW		0x00F80000
-#define SCAN_CIB_NSW_SHIFT	17
+#define SCAN_CIB_NSW_SHIFT	19
 #define SCAN_CIB_REV		0xFF000000
 #define SCAN_CIB_REV_SHIFT	24
 
diff --git a/drivers/bcma/sprom.c b/drivers/bcma/sprom.c
index f16f42d3607..26823d97fd9 100644
--- a/drivers/bcma/sprom.c
+++ b/drivers/bcma/sprom.c
@@ -60,11 +60,11 @@ static int bcma_fill_sprom_with_fallback(struct bcma_bus *bus,
 	if (err)
 		goto fail;
 
-	pr_debug("Using SPROM revision %d provided by"
-		 " platform.\n", bus->sprom.revision);
+	bcma_debug(bus, "Using SPROM revision %d provided by platform.\n",
+		   bus->sprom.revision);
 	return 0;
 fail:
-	pr_warn("Using fallback SPROM failed (err %d)\n", err);
+	bcma_warn(bus, "Using fallback SPROM failed (err %d)\n", err);
 	return err;
 }
 
@@ -468,11 +468,11 @@ static bool bcma_sprom_ext_available(struct bcma_bus *bus)
 	/* older chipcommon revisions use chip status register */
 	chip_status = bcma_read32(bus->drv_cc.core, BCMA_CC_CHIPSTAT);
 	switch (bus->chipinfo.id) {
-	case 0x4313:
+	case BCMA_CHIP_ID_BCM4313:
 		present_mask = BCMA_CC_CHIPST_4313_SPROM_PRESENT;
 		break;
 
-	case 0x4331:
+	case BCMA_CHIP_ID_BCM4331:
 		present_mask = BCMA_CC_CHIPST_4331_SPROM_PRESENT;
 		break;
 
@@ -494,16 +494,16 @@ static bool bcma_sprom_onchip_available(struct bcma_bus *bus)
 
 	chip_status = bcma_read32(bus->drv_cc.core, BCMA_CC_CHIPSTAT);
 	switch (bus->chipinfo.id) {
-	case 0x4313:
+	case BCMA_CHIP_ID_BCM4313:
 		present = chip_status & BCMA_CC_CHIPST_4313_OTP_PRESENT;
 		break;
 
-	case 0x4331:
+	case BCMA_CHIP_ID_BCM4331:
 		present = chip_status & BCMA_CC_CHIPST_4331_OTP_PRESENT;
 		break;
 
-	case 43224:
-	case 43225:
+	case BCMA_CHIP_ID_BCM43224:
+	case BCMA_CHIP_ID_BCM43225:
 		/* for these chips OTP is always available */
 		present = true;
 		break;
@@ -579,13 +579,15 @@ int bcma_sprom_get(struct bcma_bus *bus)
 	if (!sprom)
 		return -ENOMEM;
 
-	if (bus->chipinfo.id == 0x4331 || bus->chipinfo.id == 43431)
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4331 ||
+	    bus->chipinfo.id == BCMA_CHIP_ID_BCM43431)
 		bcma_chipco_bcm4331_ext_pa_lines_ctl(&bus->drv_cc, false);
 
-	pr_debug("SPROM offset 0x%x\n", offset);
+	bcma_debug(bus, "SPROM offset 0x%x\n", offset);
 	bcma_sprom_read(bus, offset, sprom);
 
-	if (bus->chipinfo.id == 0x4331 || bus->chipinfo.id == 43431)
+	if (bus->chipinfo.id == BCMA_CHIP_ID_BCM4331 ||
+	    bus->chipinfo.id == BCMA_CHIP_ID_BCM43431)
 		bcma_chipco_bcm4331_ext_pa_lines_ctl(&bus->drv_cc, true);
 
 	err = bcma_sprom_valid(sprom);
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index e54e31b02b8..3fbef018ce5 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -411,7 +411,7 @@ w_al_write_transaction(struct drbd_conf *mdev, struct drbd_work *w, int unused)
 		+ mdev->ldev->md.al_offset + mdev->al_tr_pos;
 
 	if (!drbd_md_sync_page_io(mdev, mdev->ldev, sector, WRITE))
-		drbd_chk_io_error(mdev, 1, true);
+		drbd_chk_io_error(mdev, 1, DRBD_META_IO_ERROR);
 
 	if (++mdev->al_tr_pos >
 	    div_ceil(mdev->act_log->nr_elements, AL_EXTENTS_PT))
@@ -876,7 +876,11 @@ int __drbd_set_out_of_sync(struct drbd_conf *mdev, sector_t sector, int size,
 	unsigned int enr, count = 0;
 	struct lc_element *e;
 
-	if (size <= 0 || (size & 0x1ff) != 0 || size > DRBD_MAX_BIO_SIZE) {
+	/* this should be an empty REQ_FLUSH */
+	if (size == 0)
+		return 0;
+
+	if (size < 0 || (size & 0x1ff) != 0 || size > DRBD_MAX_BIO_SIZE) {
 		dev_err(DEV, "sector: %llus, size: %d\n",
 			(unsigned long long)sector, size);
 		return 0;
diff --git a/drivers/block/drbd/drbd_bitmap.c b/drivers/block/drbd/drbd_bitmap.c
index fcb956bb4b4..ba91b408aba 100644
--- a/drivers/block/drbd/drbd_bitmap.c
+++ b/drivers/block/drbd/drbd_bitmap.c
@@ -1096,7 +1096,7 @@ static int bm_rw(struct drbd_conf *mdev, int rw, unsigned flags, unsigned lazy_w
 
 	if (ctx->error) {
 		dev_alert(DEV, "we had at least one MD IO ERROR during bitmap IO\n");
-		drbd_chk_io_error(mdev, 1, true);
+		drbd_chk_io_error(mdev, 1, DRBD_META_IO_ERROR);
 		err = -EIO; /* ctx->error ? */
 	}
 
@@ -1212,7 +1212,7 @@ int drbd_bm_write_page(struct drbd_conf *mdev, unsigned int idx) __must_hold(loc
 	wait_until_done_or_disk_failure(mdev, mdev->ldev, &ctx->done);
 
 	if (ctx->error)
-		drbd_chk_io_error(mdev, 1, true);
+		drbd_chk_io_error(mdev, 1, DRBD_META_IO_ERROR);
 		/* that should force detach, so the in memory bitmap will be
 		 * gone in a moment as well. */
 
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index 02f013a073a..b2ca143d005 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -813,7 +813,6 @@ enum {
 	SIGNAL_ASENDER,		/* whether asender wants to be interrupted */
 	SEND_PING,		/* whether asender should send a ping asap */
 
-	UNPLUG_QUEUED,		/* only relevant with kernel 2.4 */
 	UNPLUG_REMOTE,		/* sending a "UnplugRemote" could help */
 	MD_DIRTY,		/* current uuids and flags not yet on disk */
 	DISCARD_CONCURRENT,	/* Set on one node, cleared on the peer! */
@@ -824,7 +823,6 @@ enum {
 	CRASHED_PRIMARY,	/* This node was a crashed primary.
 				 * Gets cleared when the state.conn
 				 * goes into C_CONNECTED state. */
-	NO_BARRIER_SUPP,	/* underlying block device doesn't implement barriers */
 	CONSIDER_RESYNC,
 
 	MD_NO_FUA,		/* Users wants us to not use FUA/FLUSH on meta data dev */
@@ -834,6 +832,7 @@ enum {
 	BITMAP_IO_QUEUED,       /* Started bitmap IO */
 	GO_DISKLESS,		/* Disk is being detached, on io-error or admin request. */
 	WAS_IO_ERROR,		/* Local disk failed returned IO error */
+	FORCE_DETACH,		/* Force-detach from local disk, aborting any pending local IO */
 	RESYNC_AFTER_NEG,       /* Resync after online grow after the attach&negotiate finished. */
 	NET_CONGESTED,		/* The data socket is congested */
 
@@ -851,6 +850,13 @@ enum {
 	AL_SUSPENDED,		/* Activity logging is currently suspended. */
 	AHEAD_TO_SYNC_SOURCE,   /* Ahead -> SyncSource queued */
 	STATE_SENT,		/* Do not change state/UUIDs while this is set */
+
+	CALLBACK_PENDING,	/* Whether we have a call_usermodehelper(, UMH_WAIT_PROC)
+				 * pending, from drbd worker context.
+				 * If set, bdi_write_congested() returns true,
+				 * so shrink_page_list() would not recurse into,
+				 * and potentially deadlock on, this drbd worker.
+				 */
 };
 
 struct drbd_bitmap; /* opaque for drbd_conf */
@@ -1130,8 +1136,8 @@ struct drbd_conf {
 	int rs_in_flight; /* resync sectors in flight (to proxy, in proxy and from proxy) */
 	int rs_planed;    /* resync sectors already planned */
 	atomic_t ap_in_flight; /* App sectors in flight (waiting for ack) */
-	int peer_max_bio_size;
-	int local_max_bio_size;
+	unsigned int peer_max_bio_size;
+	unsigned int local_max_bio_size;
 };
 
 static inline struct drbd_conf *minor_to_mdev(unsigned int minor)
@@ -1435,9 +1441,9 @@ struct bm_extent {
  * hash table. */
 #define HT_SHIFT 8
 #define DRBD_MAX_BIO_SIZE (1U<<(9+HT_SHIFT))
-#define DRBD_MAX_BIO_SIZE_SAFE (1 << 12)       /* Works always = 4k */
+#define DRBD_MAX_BIO_SIZE_SAFE (1U << 12)       /* Works always = 4k */
 
-#define DRBD_MAX_SIZE_H80_PACKET (1 << 15) /* The old header only allows packets up to 32Kib data */
+#define DRBD_MAX_SIZE_H80_PACKET (1U << 15) /* The old header only allows packets up to 32Kib data */
 
 /* Number of elements in the app_reads_hash */
 #define APP_R_HSIZE 15
@@ -1840,12 +1846,20 @@ static inline int drbd_request_state(struct drbd_conf *mdev,
 	return _drbd_request_state(mdev, mask, val, CS_VERBOSE + CS_ORDERED);
 }
 
+enum drbd_force_detach_flags {
+	DRBD_IO_ERROR,
+	DRBD_META_IO_ERROR,
+	DRBD_FORCE_DETACH,
+};
+
 #define __drbd_chk_io_error(m,f) __drbd_chk_io_error_(m,f, __func__)
-static inline void __drbd_chk_io_error_(struct drbd_conf *mdev, int forcedetach, const char *where)
+static inline void __drbd_chk_io_error_(struct drbd_conf *mdev,
+		enum drbd_force_detach_flags forcedetach,
+		const char *where)
 {
 	switch (mdev->ldev->dc.on_io_error) {
 	case EP_PASS_ON:
-		if (!forcedetach) {
+		if (forcedetach == DRBD_IO_ERROR) {
 			if (__ratelimit(&drbd_ratelimit_state))
 				dev_err(DEV, "Local IO failed in %s.\n", where);
 			if (mdev->state.disk > D_INCONSISTENT)
@@ -1856,6 +1870,8 @@ static inline void __drbd_chk_io_error_(struct drbd_conf *mdev, int forcedetach,
 	case EP_DETACH:
 	case EP_CALL_HELPER:
 		set_bit(WAS_IO_ERROR, &mdev->flags);
+		if (forcedetach == DRBD_FORCE_DETACH)
+			set_bit(FORCE_DETACH, &mdev->flags);
 		if (mdev->state.disk > D_FAILED) {
 			_drbd_set_state(_NS(mdev, disk, D_FAILED), CS_HARD, NULL);
 			dev_err(DEV,
@@ -1875,7 +1891,7 @@ static inline void __drbd_chk_io_error_(struct drbd_conf *mdev, int forcedetach,
  */
 #define drbd_chk_io_error(m,e,f) drbd_chk_io_error_(m,e,f, __func__)
 static inline void drbd_chk_io_error_(struct drbd_conf *mdev,
-	int error, int forcedetach, const char *where)
+	int error, enum drbd_force_detach_flags forcedetach, const char *where)
 {
 	if (error) {
 		unsigned long flags;
@@ -2405,15 +2421,17 @@ static inline void dec_ap_bio(struct drbd_conf *mdev)
 	int ap_bio = atomic_dec_return(&mdev->ap_bio_cnt);
 
 	D_ASSERT(ap_bio >= 0);
+
+	if (ap_bio == 0 && test_bit(BITMAP_IO, &mdev->flags)) {
+		if (!test_and_set_bit(BITMAP_IO_QUEUED, &mdev->flags))
+			drbd_queue_work(&mdev->data.work, &mdev->bm_io_work.w);
+	}
+
 	/* this currently does wake_up for every dec_ap_bio!
 	 * maybe rather introduce some type of hysteresis?
 	 * e.g. (ap_bio == mxb/2 || ap_bio == 0) ? */
 	if (ap_bio < mxb)
 		wake_up(&mdev->misc_wait);
-	if (ap_bio == 0 && test_bit(BITMAP_IO, &mdev->flags)) {
-		if (!test_and_set_bit(BITMAP_IO_QUEUED, &mdev->flags))
-			drbd_queue_work(&mdev->data.work, &mdev->bm_io_work.w);
-	}
 }
 
 static inline int drbd_set_ed_uuid(struct drbd_conf *mdev, u64 val)
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 920ede2829d..2e0e7fc1dbb 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -1514,6 +1514,13 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
 
 	/* Do not change the order of the if above and the two below... */
 	if (os.pdsk == D_DISKLESS && ns.pdsk > D_DISKLESS) {      /* attach on the peer */
+		/* we probably will start a resync soon.
+		 * make sure those things are properly reset. */
+		mdev->rs_total = 0;
+		mdev->rs_failed = 0;
+		atomic_set(&mdev->rs_pending_cnt, 0);
+		drbd_rs_cancel_all(mdev);
+
 		drbd_send_uuids(mdev);
 		drbd_send_state(mdev, ns);
 	}
@@ -1630,9 +1637,24 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
 			eh = mdev->ldev->dc.on_io_error;
 			was_io_error = test_and_clear_bit(WAS_IO_ERROR, &mdev->flags);
 
-			/* Immediately allow completion of all application IO, that waits
-			   for completion from the local disk. */
-			tl_abort_disk_io(mdev);
+			if (was_io_error && eh == EP_CALL_HELPER)
+				drbd_khelper(mdev, "local-io-error");
+
+			/* Immediately allow completion of all application IO,
+			 * that waits for completion from the local disk,
+			 * if this was a force-detach due to disk_timeout
+			 * or administrator request (drbdsetup detach --force).
+			 * Do NOT abort otherwise.
+			 * Aborting local requests may cause serious problems,
+			 * if requests are completed to upper layers already,
+			 * and then later the already submitted local bio completes.
+			 * This can cause DMA into former bio pages that meanwhile
+			 * have been re-used for other things.
+			 * So aborting local requests may cause crashes,
+			 * or even worse, silent data corruption.
+			 */
+			if (test_and_clear_bit(FORCE_DETACH, &mdev->flags))
+				tl_abort_disk_io(mdev);
 
 			/* current state still has to be D_FAILED,
 			 * there is only one way out: to D_DISKLESS,
@@ -1653,9 +1675,6 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
 			drbd_md_sync(mdev);
 		}
 		put_ldev(mdev);
-
-		if (was_io_error && eh == EP_CALL_HELPER)
-			drbd_khelper(mdev, "local-io-error");
 	}
 
         /* second half of local IO error, failure to attach,
@@ -1669,10 +1688,6 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
                                 "ASSERT FAILED: disk is %s while going diskless\n",
                                 drbd_disk_str(mdev->state.disk));
 
-                mdev->rs_total = 0;
-                mdev->rs_failed = 0;
-                atomic_set(&mdev->rs_pending_cnt, 0);
-
 		if (ns.conn >= C_CONNECTED)
 			drbd_send_state(mdev, ns);
 
@@ -2194,7 +2209,8 @@ int drbd_send_sizes(struct drbd_conf *mdev, int trigger_reply, enum dds_flags fl
 {
 	struct p_sizes p;
 	sector_t d_size, u_size;
-	int q_order_type, max_bio_size;
+	int q_order_type;
+	unsigned int max_bio_size;
 	int ok;
 
 	if (get_ldev_if_state(mdev, D_NEGOTIATING)) {
@@ -2203,7 +2219,7 @@ int drbd_send_sizes(struct drbd_conf *mdev, int trigger_reply, enum dds_flags fl
 		u_size = mdev->ldev->dc.disk_size;
 		q_order_type = drbd_queue_order_type(mdev);
 		max_bio_size = queue_max_hw_sectors(mdev->ldev->backing_bdev->bd_disk->queue) << 9;
-		max_bio_size = min_t(int, max_bio_size, DRBD_MAX_BIO_SIZE);
+		max_bio_size = min(max_bio_size, DRBD_MAX_BIO_SIZE);
 		put_ldev(mdev);
 	} else {
 		d_size = 0;
@@ -2214,7 +2230,7 @@ int drbd_send_sizes(struct drbd_conf *mdev, int trigger_reply, enum dds_flags fl
 
 	/* Never allow old drbd (up to 8.3.7) to see more than 32KiB */
 	if (mdev->agreed_pro_version <= 94)
-		max_bio_size = min_t(int, max_bio_size, DRBD_MAX_SIZE_H80_PACKET);
+		max_bio_size = min(max_bio_size, DRBD_MAX_SIZE_H80_PACKET);
 
 	p.d_size = cpu_to_be64(d_size);
 	p.u_size = cpu_to_be64(u_size);
@@ -3541,6 +3557,22 @@ static int drbd_congested(void *congested_data, int bdi_bits)
 		goto out;
 	}
 
+	if (test_bit(CALLBACK_PENDING, &mdev->flags)) {
+		r |= (1 << BDI_async_congested);
+		/* Without good local data, we would need to read from remote,
+		 * and that would need the worker thread as well, which is
+		 * currently blocked waiting for that usermode helper to
+		 * finish.
+		 */
+		if (!get_ldev_if_state(mdev, D_UP_TO_DATE))
+			r |= (1 << BDI_sync_congested);
+		else
+			put_ldev(mdev);
+		r &= bdi_bits;
+		reason = 'c';
+		goto out;
+	}
+
 	if (get_ldev(mdev)) {
 		q = bdev_get_queue(mdev->ldev->backing_bdev);
 		r = bdi_congested(&q->backing_dev_info, bdi_bits);
@@ -3604,6 +3636,7 @@ struct drbd_conf *drbd_new_device(unsigned int minor)
 	q->backing_dev_info.congested_data = mdev;
 
 	blk_queue_make_request(q, drbd_make_request);
+	blk_queue_flush(q, REQ_FLUSH | REQ_FUA);
 	/* Setting the max_hw_sectors to an odd value of 8kibyte here
 	   This triggers a max_bio_size message upon first attach or connect */
 	blk_queue_max_hw_sectors(q, DRBD_MAX_BIO_SIZE_SAFE >> 8);
@@ -3870,7 +3903,7 @@ void drbd_md_sync(struct drbd_conf *mdev)
 	if (!drbd_md_sync_page_io(mdev, mdev->ldev, sector, WRITE)) {
 		/* this was a try anyways ... */
 		dev_err(DEV, "meta data update failed!\n");
-		drbd_chk_io_error(mdev, 1, true);
+		drbd_chk_io_error(mdev, 1, DRBD_META_IO_ERROR);
 	}
 
 	/* Update mdev->ldev->md.la_size_sect,
@@ -3950,9 +3983,9 @@ int drbd_md_read(struct drbd_conf *mdev, struct drbd_backing_dev *bdev)
 
 	spin_lock_irq(&mdev->req_lock);
 	if (mdev->state.conn < C_CONNECTED) {
-		int peer;
+		unsigned int peer;
 		peer = be32_to_cpu(buffer->la_peer_max_bio_size);
-		peer = max_t(int, peer, DRBD_MAX_BIO_SIZE_SAFE);
+		peer = max(peer, DRBD_MAX_BIO_SIZE_SAFE);
 		mdev->peer_max_bio_size = peer;
 	}
 	spin_unlock_irq(&mdev->req_lock);
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 6d4de6a72e8..fb9dce8daa2 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -147,6 +147,9 @@ int drbd_khelper(struct drbd_conf *mdev, char *cmd)
 	char *argv[] = {usermode_helper, cmd, mb, NULL };
 	int ret;
 
+	if (current == mdev->worker.task)
+		set_bit(CALLBACK_PENDING, &mdev->flags);
+
 	snprintf(mb, 12, "minor-%d", mdev_to_minor(mdev));
 
 	if (get_net_conf(mdev)) {
@@ -189,6 +192,9 @@ int drbd_khelper(struct drbd_conf *mdev, char *cmd)
 				usermode_helper, cmd, mb,
 				(ret >> 8) & 0xff, ret);
 
+	if (current == mdev->worker.task)
+		clear_bit(CALLBACK_PENDING, &mdev->flags);
+
 	if (ret < 0) /* Ignore any ERRNOs we got. */
 		ret = 0;
 
@@ -795,8 +801,8 @@ static int drbd_check_al_size(struct drbd_conf *mdev)
 static void drbd_setup_queue_param(struct drbd_conf *mdev, unsigned int max_bio_size)
 {
 	struct request_queue * const q = mdev->rq_queue;
-	int max_hw_sectors = max_bio_size >> 9;
-	int max_segments = 0;
+	unsigned int max_hw_sectors = max_bio_size >> 9;
+	unsigned int max_segments = 0;
 
 	if (get_ldev_if_state(mdev, D_ATTACHING)) {
 		struct request_queue * const b = mdev->ldev->backing_bdev->bd_disk->queue;
@@ -829,7 +835,7 @@ static void drbd_setup_queue_param(struct drbd_conf *mdev, unsigned int max_bio_
 
 void drbd_reconsider_max_bio_size(struct drbd_conf *mdev)
 {
-	int now, new, local, peer;
+	unsigned int now, new, local, peer;
 
 	now = queue_max_hw_sectors(mdev->rq_queue) << 9;
 	local = mdev->local_max_bio_size; /* Eventually last known value, from volatile memory */
@@ -840,13 +846,14 @@ void drbd_reconsider_max_bio_size(struct drbd_conf *mdev)
 		mdev->local_max_bio_size = local;
 		put_ldev(mdev);
 	}
+	local = min(local, DRBD_MAX_BIO_SIZE);
 
 	/* We may ignore peer limits if the peer is modern enough.
 	   Because new from 8.3.8 onwards the peer can use multiple
 	   BIOs for a single peer_request */
 	if (mdev->state.conn >= C_CONNECTED) {
 		if (mdev->agreed_pro_version < 94) {
-			peer = min_t(int, mdev->peer_max_bio_size, DRBD_MAX_SIZE_H80_PACKET);
+			peer = min(mdev->peer_max_bio_size, DRBD_MAX_SIZE_H80_PACKET);
 			/* Correct old drbd (up to 8.3.7) if it believes it can do more than 32KiB */
 		} else if (mdev->agreed_pro_version == 94)
 			peer = DRBD_MAX_SIZE_H80_PACKET;
@@ -854,10 +861,10 @@ void drbd_reconsider_max_bio_size(struct drbd_conf *mdev)
 			peer = DRBD_MAX_BIO_SIZE;
 	}
 
-	new = min_t(int, local, peer);
+	new = min(local, peer);
 
 	if (mdev->state.role == R_PRIMARY && new < now)
-		dev_err(DEV, "ASSERT FAILED new < now; (%d < %d)\n", new, now);
+		dev_err(DEV, "ASSERT FAILED new < now; (%u < %u)\n", new, now);
 
 	if (new != now)
 		dev_info(DEV, "max BIO size = %u\n", new);
@@ -950,6 +957,14 @@ static int drbd_nl_disk_conf(struct drbd_conf *mdev, struct drbd_nl_cfg_req *nlp
 	 * to realize a "hot spare" feature (not that I'd recommend that) */
 	wait_event(mdev->misc_wait, !atomic_read(&mdev->local_cnt));
 
+	/* make sure there is no leftover from previous force-detach attempts */
+	clear_bit(FORCE_DETACH, &mdev->flags);
+
+	/* and no leftover from previously aborted resync or verify, either */
+	mdev->rs_total = 0;
+	mdev->rs_failed = 0;
+	atomic_set(&mdev->rs_pending_cnt, 0);
+
 	/* allocation not in the IO path, cqueue thread context */
 	nbc = kzalloc(sizeof(struct drbd_backing_dev), GFP_KERNEL);
 	if (!nbc) {
@@ -1345,6 +1360,7 @@ static int drbd_nl_detach(struct drbd_conf *mdev, struct drbd_nl_cfg_req *nlp,
 	}
 
 	if (dt.detach_force) {
+		set_bit(FORCE_DETACH, &mdev->flags);
 		drbd_force_state(mdev, NS(disk, D_FAILED));
 		reply->ret_code = SS_SUCCESS;
 		goto out;
@@ -1962,9 +1978,11 @@ static int drbd_nl_invalidate(struct drbd_conf *mdev, struct drbd_nl_cfg_req *nl
 	int retcode;
 
 	/* If there is still bitmap IO pending, probably because of a previous
-	 * resync just being finished, wait for it before requesting a new resync. */
+	 * resync just being finished, wait for it before requesting a new resync.
+	 * Also wait for it's after_state_ch(). */
 	drbd_suspend_io(mdev);
 	wait_event(mdev->misc_wait, !test_bit(BITMAP_IO, &mdev->flags));
+	drbd_flush_workqueue(mdev);
 
 	retcode = _drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_T), CS_ORDERED);
 
@@ -2003,9 +2021,11 @@ static int drbd_nl_invalidate_peer(struct drbd_conf *mdev, struct drbd_nl_cfg_re
 	int retcode;
 
 	/* If there is still bitmap IO pending, probably because of a previous
-	 * resync just being finished, wait for it before requesting a new resync. */
+	 * resync just being finished, wait for it before requesting a new resync.
+	 * Also wait for it's after_state_ch(). */
 	drbd_suspend_io(mdev);
 	wait_event(mdev->misc_wait, !test_bit(BITMAP_IO, &mdev->flags));
+	drbd_flush_workqueue(mdev);
 
 	retcode = _drbd_request_state(mdev, NS(conn, C_STARTING_SYNC_S), CS_ORDERED);
 
diff --git a/drivers/block/drbd/drbd_proc.c b/drivers/block/drbd/drbd_proc.c
index 869bada2ed0..5496104f90b 100644
--- a/drivers/block/drbd/drbd_proc.c
+++ b/drivers/block/drbd/drbd_proc.c
@@ -245,6 +245,9 @@ static int drbd_seq_show(struct seq_file *seq, void *v)
 		    mdev->state.role == R_SECONDARY) {
 			seq_printf(seq, "%2d: cs:Unconfigured\n", i);
 		} else {
+			/* reset mdev->congestion_reason */
+			bdi_rw_congested(&mdev->rq_queue->backing_dev_info);
+
 			seq_printf(seq,
 			   "%2d: cs:%s ro:%s/%s ds:%s/%s %c %c%c%c%c%c%c\n"
 			   "    ns:%u nr:%u dw:%u dr:%u al:%u bm:%u "
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index ea4836e0ae9..c74ca2df743 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -277,6 +277,9 @@ static void drbd_pp_free(struct drbd_conf *mdev, struct page *page, int is_net)
 	atomic_t *a = is_net ? &mdev->pp_in_use_by_net : &mdev->pp_in_use;
 	int i;
 
+	if (page == NULL)
+		return;
+
 	if (drbd_pp_vacant > (DRBD_MAX_BIO_SIZE/PAGE_SIZE)*minor_count)
 		i = page_chain_free(page);
 	else {
@@ -316,7 +319,7 @@ struct drbd_epoch_entry *drbd_alloc_ee(struct drbd_conf *mdev,
 				     gfp_t gfp_mask) __must_hold(local)
 {
 	struct drbd_epoch_entry *e;
-	struct page *page;
+	struct page *page = NULL;
 	unsigned nr_pages = (data_size + PAGE_SIZE -1) >> PAGE_SHIFT;
 
 	if (drbd_insert_fault(mdev, DRBD_FAULT_AL_EE))
@@ -329,9 +332,11 @@ struct drbd_epoch_entry *drbd_alloc_ee(struct drbd_conf *mdev,
 		return NULL;
 	}
 
-	page = drbd_pp_alloc(mdev, nr_pages, (gfp_mask & __GFP_WAIT));
-	if (!page)
-		goto fail;
+	if (data_size) {
+		page = drbd_pp_alloc(mdev, nr_pages, (gfp_mask & __GFP_WAIT));
+		if (!page)
+			goto fail;
+	}
 
 	INIT_HLIST_NODE(&e->collision);
 	e->epoch = NULL;
@@ -1270,7 +1275,6 @@ read_in_block(struct drbd_conf *mdev, u64 id, sector_t sector, int data_size) __
 
 	data_size -= dgs;
 
-	ERR_IF(data_size == 0) return NULL;
 	ERR_IF(data_size &  0x1ff) return NULL;
 	ERR_IF(data_size >  DRBD_MAX_BIO_SIZE) return NULL;
 
@@ -1291,6 +1295,9 @@ read_in_block(struct drbd_conf *mdev, u64 id, sector_t sector, int data_size) __
 	if (!e)
 		return NULL;
 
+	if (!data_size)
+		return e;
+
 	ds = data_size;
 	page = e->pages;
 	page_chain_for_each(page) {
@@ -1715,6 +1722,10 @@ static int receive_Data(struct drbd_conf *mdev, enum drbd_packets cmd, unsigned
 
 	dp_flags = be32_to_cpu(p->dp_flags);
 	rw |= wire_flags_to_bio(mdev, dp_flags);
+	if (e->pages == NULL) {
+		D_ASSERT(e->size == 0);
+		D_ASSERT(dp_flags & DP_FLUSH);
+	}
 
 	if (dp_flags & DP_MAY_SET_IN_SYNC)
 		e->flags |= EE_MAY_SET_IN_SYNC;
@@ -3801,11 +3812,18 @@ void drbd_free_tl_hash(struct drbd_conf *mdev)
 	mdev->ee_hash = NULL;
 	mdev->ee_hash_s = 0;
 
-	/* paranoia code */
-	for (h = mdev->tl_hash; h < mdev->tl_hash + mdev->tl_hash_s; h++)
-		if (h->first)
-			dev_err(DEV, "ASSERT FAILED tl_hash[%u] == %p, expected NULL\n",
-				(int)(h - mdev->tl_hash), h->first);
+	/* We may not have had the chance to wait for all locally pending
+	 * application requests. The hlist_add_fake() prevents access after
+	 * free on master bio completion. */
+	for (h = mdev->tl_hash; h < mdev->tl_hash + mdev->tl_hash_s; h++) {
+		struct drbd_request *req;
+		struct hlist_node *pos, *n;
+		hlist_for_each_entry_safe(req, pos, n, h, collision) {
+			hlist_del_init(&req->collision);
+			hlist_add_fake(&req->collision);
+		}
+	}
+
 	kfree(mdev->tl_hash);
 	mdev->tl_hash = NULL;
 	mdev->tl_hash_s = 0;
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 8e93a6ac9bb..910335c3092 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -455,7 +455,7 @@ int __req_mod(struct drbd_request *req, enum drbd_req_event what,
 		req->rq_state |= RQ_LOCAL_COMPLETED;
 		req->rq_state &= ~RQ_LOCAL_PENDING;
 
-		__drbd_chk_io_error(mdev, false);
+		__drbd_chk_io_error(mdev, DRBD_IO_ERROR);
 		_req_may_be_done_not_susp(req, m);
 		break;
 
@@ -477,7 +477,7 @@ int __req_mod(struct drbd_request *req, enum drbd_req_event what,
 			break;
 		}
 
-		__drbd_chk_io_error(mdev, false);
+		__drbd_chk_io_error(mdev, DRBD_IO_ERROR);
 
 	goto_queue_for_net_read:
 
@@ -1111,13 +1111,12 @@ void drbd_make_request(struct request_queue *q, struct bio *bio)
 	/*
 	 * what we "blindly" assume:
 	 */
-	D_ASSERT(bio->bi_size > 0);
 	D_ASSERT((bio->bi_size & 0x1ff) == 0);
 
 	/* to make some things easier, force alignment of requests within the
 	 * granularity of our hash tables */
 	s_enr = bio->bi_sector >> HT_SHIFT;
-	e_enr = (bio->bi_sector+(bio->bi_size>>9)-1) >> HT_SHIFT;
+	e_enr = bio->bi_size ? (bio->bi_sector+(bio->bi_size>>9)-1) >> HT_SHIFT : s_enr;
 
 	if (likely(s_enr == e_enr)) {
 		do {
@@ -1275,7 +1274,7 @@ void request_timer_fn(unsigned long data)
 		 time_after(now, req->start_time + dt) &&
 		!time_in_range(now, mdev->last_reattach_jif, mdev->last_reattach_jif + dt)) {
 		dev_warn(DEV, "Local backing device failed to meet the disk-timeout\n");
-		__drbd_chk_io_error(mdev, 1);
+		__drbd_chk_io_error(mdev, DRBD_FORCE_DETACH);
 	}
 	nt = (time_after(now, req->start_time + et) ? now : req->start_time) + et;
 	spin_unlock_irq(&mdev->req_lock);
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 620c70ff223..6bce2cc179d 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -111,7 +111,7 @@ void drbd_endio_read_sec_final(struct drbd_epoch_entry *e) __releases(local)
 	if (list_empty(&mdev->read_ee))
 		wake_up(&mdev->ee_wait);
 	if (test_bit(__EE_WAS_ERROR, &e->flags))
-		__drbd_chk_io_error(mdev, false);
+		__drbd_chk_io_error(mdev, DRBD_IO_ERROR);
 	spin_unlock_irqrestore(&mdev->req_lock, flags);
 
 	drbd_queue_work(&mdev->data.work, &e->w);
@@ -154,7 +154,7 @@ static void drbd_endio_write_sec_final(struct drbd_epoch_entry *e) __releases(lo
 		: list_empty(&mdev->active_ee);
 
 	if (test_bit(__EE_WAS_ERROR, &e->flags))
-		__drbd_chk_io_error(mdev, false);
+		__drbd_chk_io_error(mdev, DRBD_IO_ERROR);
 	spin_unlock_irqrestore(&mdev->req_lock, flags);
 
 	if (is_syncer_req)
@@ -1501,14 +1501,6 @@ void drbd_start_resync(struct drbd_conf *mdev, enum drbd_conns side)
 		return;
 	}
 
-	if (mdev->state.conn < C_AHEAD) {
-		/* In case a previous resync run was aborted by an IO error/detach on the peer. */
-		drbd_rs_cancel_all(mdev);
-		/* This should be done when we abort the resync. We definitely do not
-		   want to have this for connections going back and forth between
-		   Ahead/Behind and SyncSource/SyncTarget */
-	}
-
 	if (side == C_SYNC_TARGET) {
 		/* Since application IO was locked out during C_WF_BITMAP_T and
 		   C_WF_SYNC_UUID we are still unmodified. Before going to C_SYNC_TARGET
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 553f43a9095..a7d6347aaa7 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -191,6 +191,7 @@ static int print_unex = 1;
 #include <linux/mutex.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
+#include <linux/async.h>
 
 /*
  * PS/2 floppies have much slower step rates than regular floppies.
@@ -2516,8 +2517,7 @@ static int make_raw_rw_request(void)
 	set_fdc((long)current_req->rq_disk->private_data);
 
 	raw_cmd = &default_raw_cmd;
-	raw_cmd->flags = FD_RAW_SPIN | FD_RAW_NEED_DISK | FD_RAW_NEED_DISK |
-	    FD_RAW_NEED_SEEK;
+	raw_cmd->flags = FD_RAW_SPIN | FD_RAW_NEED_DISK | FD_RAW_NEED_SEEK;
 	raw_cmd->cmd_count = NR_RW;
 	if (rq_data_dir(current_req) == READ) {
 		raw_cmd->flags |= FD_RAW_READ;
@@ -4123,7 +4123,7 @@ static struct kobject *floppy_find(dev_t dev, int *part, void *data)
 	return get_disk(disks[drive]);
 }
 
-static int __init floppy_init(void)
+static int __init do_floppy_init(void)
 {
 	int i, unit, drive;
 	int err, dr;
@@ -4338,6 +4338,24 @@ out_put_disk:
 	return err;
 }
 
+#ifndef MODULE
+static __init void floppy_async_init(void *data, async_cookie_t cookie)
+{
+	do_floppy_init();
+}
+#endif
+
+static int __init floppy_init(void)
+{
+#ifdef MODULE
+	return do_floppy_init();
+#else
+	/* Don't hold up the bootup by the floppy initialization */
+	async_schedule(floppy_async_init, NULL);
+	return 0;
+#endif
+}
+
 static const struct io_region {
 	int offset;
 	int size;
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index 76fa3deaee8..1788f491e0f 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -780,9 +780,9 @@ static const struct block_device_operations mg_disk_ops = {
 	.getgeo = mg_getgeo
 };
 
-static int mg_suspend(struct platform_device *plat_dev, pm_message_t state)
+static int mg_suspend(struct device *dev)
 {
-	struct mg_drv_data *prv_data = plat_dev->dev.platform_data;
+	struct mg_drv_data *prv_data = dev->platform_data;
 	struct mg_host *host = prv_data->host;
 
 	if (mg_wait(host, MG_STAT_READY, MG_TMAX_CONF_TO_CMD))
@@ -804,9 +804,9 @@ static int mg_suspend(struct platform_device *plat_dev, pm_message_t state)
 	return 0;
 }
 
-static int mg_resume(struct platform_device *plat_dev)
+static int mg_resume(struct device *dev)
 {
-	struct mg_drv_data *prv_data = plat_dev->dev.platform_data;
+	struct mg_drv_data *prv_data = dev->platform_data;
 	struct mg_host *host = prv_data->host;
 
 	if (mg_wait(host, MG_STAT_READY, MG_TMAX_CONF_TO_CMD))
@@ -825,6 +825,8 @@ static int mg_resume(struct platform_device *plat_dev)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(mg_pm, mg_suspend, mg_resume);
+
 static int mg_probe(struct platform_device *plat_dev)
 {
 	struct mg_host *host;
@@ -1074,11 +1076,10 @@ static int mg_remove(struct platform_device *plat_dev)
 static struct platform_driver mg_disk_driver = {
 	.probe = mg_probe,
 	.remove = mg_remove,
-	.suspend = mg_suspend,
-	.resume = mg_resume,
 	.driver = {
 		.name = MG_DEV_NAME,
 		.owner = THIS_MODULE,
+		.pm = &mg_pm,
 	}
 };
 
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 061427a75d3..d07c9f7fded 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -154,6 +154,7 @@ static int sock_xmit(struct nbd_device *nbd, int send, void *buf, int size,
 	struct msghdr msg;
 	struct kvec iov;
 	sigset_t blocked, oldset;
+	unsigned long pflags = current->flags;
 
 	if (unlikely(!sock)) {
 		dev_err(disk_to_dev(nbd->disk),
@@ -167,8 +168,9 @@ static int sock_xmit(struct nbd_device *nbd, int send, void *buf, int size,
 	siginitsetinv(&blocked, sigmask(SIGKILL));
 	sigprocmask(SIG_SETMASK, &blocked, &oldset);
 
+	current->flags |= PF_MEMALLOC;
 	do {
-		sock->sk->sk_allocation = GFP_NOIO;
+		sock->sk->sk_allocation = GFP_NOIO | __GFP_MEMALLOC;
 		iov.iov_base = buf;
 		iov.iov_len = size;
 		msg.msg_name = NULL;
@@ -214,6 +216,7 @@ static int sock_xmit(struct nbd_device *nbd, int send, void *buf, int size,
 	} while (size > 0);
 
 	sigprocmask(SIG_SETMASK, &oldset, NULL);
+	tsk_restore_flags(current, pflags, PF_MEMALLOC);
 
 	return result;
 }
@@ -405,6 +408,7 @@ static int nbd_do_it(struct nbd_device *nbd)
 
 	BUG_ON(nbd->magic != NBD_MAGIC);
 
+	sk_set_memalloc(nbd->sock->sk);
 	nbd->pid = task_pid_nr(current);
 	ret = device_create_file(disk_to_dev(nbd->disk), &pid_attr);
 	if (ret) {
@@ -481,7 +485,7 @@ static void nbd_handle_req(struct nbd_device *nbd, struct request *req)
 		nbd_end_request(req);
 	} else {
 		spin_lock(&nbd->queue_lock);
-		list_add(&req->queuelist, &nbd->queue_head);
+		list_add_tail(&req->queuelist, &nbd->queue_head);
 		spin_unlock(&nbd->queue_lock);
 	}
 
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 65665c9c42c..9917943a357 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -55,8 +55,6 @@
 
 #define RBD_MINORS_PER_MAJOR	256		/* max minors per blkdev */
 
-#define RBD_MAX_MD_NAME_LEN	(RBD_MAX_OBJ_NAME_LEN + sizeof(RBD_SUFFIX))
-#define RBD_MAX_POOL_NAME_LEN	64
 #define RBD_MAX_SNAP_NAME_LEN	32
 #define RBD_MAX_OPT_LEN		1024
 
@@ -78,13 +76,12 @@
  */
 struct rbd_image_header {
 	u64 image_size;
-	char block_name[32];
+	char *object_prefix;
 	__u8 obj_order;
 	__u8 crypt_type;
 	__u8 comp_type;
 	struct ceph_snap_context *snapc;
 	size_t snap_names_len;
-	u64 snap_seq;
 	u32 total_snaps;
 
 	char *snap_names;
@@ -150,7 +147,7 @@ struct rbd_snap {
  * a single device
  */
 struct rbd_device {
-	int			id;		/* blkdev unique id */
+	int			dev_id;		/* blkdev unique id */
 
 	int			major;		/* blkdev assigned major */
 	struct gendisk		*disk;		/* blkdev's gendisk and rq */
@@ -163,20 +160,24 @@ struct rbd_device {
 	spinlock_t		lock;		/* queue lock */
 
 	struct rbd_image_header	header;
-	char			obj[RBD_MAX_OBJ_NAME_LEN]; /* rbd image name */
-	int			obj_len;
-	char			obj_md_name[RBD_MAX_MD_NAME_LEN]; /* hdr nm. */
-	char			pool_name[RBD_MAX_POOL_NAME_LEN];
-	int			poolid;
+	char			*image_name;
+	size_t			image_name_len;
+	char			*header_name;
+	char			*pool_name;
+	int			pool_id;
 
 	struct ceph_osd_event   *watch_event;
 	struct ceph_osd_request *watch_request;
 
 	/* protects updating the header */
 	struct rw_semaphore     header_rwsem;
-	char                    snap_name[RBD_MAX_SNAP_NAME_LEN];
+	/* name of the snapshot this device reads from */
+	char                    *snap_name;
+	/* id of the snapshot this device reads from */
 	u64                     snap_id;	/* current snapshot id */
-	int read_only;
+	/* whether the snap_id this device reads from still exists */
+	bool                    snap_exists;
+	int                     read_only;
 
 	struct list_head	node;
 
@@ -201,8 +202,7 @@ static ssize_t rbd_snap_add(struct device *dev,
 			    struct device_attribute *attr,
 			    const char *buf,
 			    size_t count);
-static void __rbd_remove_snap_dev(struct rbd_device *rbd_dev,
-				  struct rbd_snap *snap);
+static void __rbd_remove_snap_dev(struct rbd_snap *snap);
 
 static ssize_t rbd_add(struct bus_type *bus, const char *buf,
 		       size_t count);
@@ -240,7 +240,7 @@ static void rbd_put_dev(struct rbd_device *rbd_dev)
 	put_device(&rbd_dev->dev);
 }
 
-static int __rbd_refresh_header(struct rbd_device *rbd_dev);
+static int rbd_refresh_header(struct rbd_device *rbd_dev, u64 *hver);
 
 static int rbd_open(struct block_device *bdev, fmode_t mode)
 {
@@ -273,9 +273,9 @@ static const struct block_device_operations rbd_bd_ops = {
 
 /*
  * Initialize an rbd client instance.
- * We own *opt.
+ * We own *ceph_opts.
  */
-static struct rbd_client *rbd_client_create(struct ceph_options *opt,
+static struct rbd_client *rbd_client_create(struct ceph_options *ceph_opts,
 					    struct rbd_options *rbd_opts)
 {
 	struct rbd_client *rbdc;
@@ -291,10 +291,10 @@ static struct rbd_client *rbd_client_create(struct ceph_options *opt,
 
 	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
 
-	rbdc->client = ceph_create_client(opt, rbdc, 0, 0);
+	rbdc->client = ceph_create_client(ceph_opts, rbdc, 0, 0);
 	if (IS_ERR(rbdc->client))
 		goto out_mutex;
-	opt = NULL; /* Now rbdc->client is responsible for opt */
+	ceph_opts = NULL; /* Now rbdc->client is responsible for ceph_opts */
 
 	ret = ceph_open_session(rbdc->client);
 	if (ret < 0)
@@ -317,23 +317,23 @@ out_mutex:
 	mutex_unlock(&ctl_mutex);
 	kfree(rbdc);
 out_opt:
-	if (opt)
-		ceph_destroy_options(opt);
+	if (ceph_opts)
+		ceph_destroy_options(ceph_opts);
 	return ERR_PTR(ret);
 }
 
 /*
  * Find a ceph client with specific addr and configuration.
  */
-static struct rbd_client *__rbd_client_find(struct ceph_options *opt)
+static struct rbd_client *__rbd_client_find(struct ceph_options *ceph_opts)
 {
 	struct rbd_client *client_node;
 
-	if (opt->flags & CEPH_OPT_NOSHARE)
+	if (ceph_opts->flags & CEPH_OPT_NOSHARE)
 		return NULL;
 
 	list_for_each_entry(client_node, &rbd_client_list, node)
-		if (ceph_compare_options(opt, client_node->client) == 0)
+		if (!ceph_compare_options(ceph_opts, client_node->client))
 			return client_node;
 	return NULL;
 }
@@ -349,7 +349,7 @@ enum {
 	/* string args above */
 };
 
-static match_table_t rbdopt_tokens = {
+static match_table_t rbd_opts_tokens = {
 	{Opt_notify_timeout, "notify_timeout=%d"},
 	/* int args above */
 	/* string args above */
@@ -358,11 +358,11 @@ static match_table_t rbdopt_tokens = {
 
 static int parse_rbd_opts_token(char *c, void *private)
 {
-	struct rbd_options *rbdopt = private;
+	struct rbd_options *rbd_opts = private;
 	substring_t argstr[MAX_OPT_ARGS];
 	int token, intval, ret;
 
-	token = match_token(c, rbdopt_tokens, argstr);
+	token = match_token(c, rbd_opts_tokens, argstr);
 	if (token < 0)
 		return -EINVAL;
 
@@ -383,7 +383,7 @@ static int parse_rbd_opts_token(char *c, void *private)
 
 	switch (token) {
 	case Opt_notify_timeout:
-		rbdopt->notify_timeout = intval;
+		rbd_opts->notify_timeout = intval;
 		break;
 	default:
 		BUG_ON(token);
@@ -400,7 +400,7 @@ static struct rbd_client *rbd_get_client(const char *mon_addr,
 					 char *options)
 {
 	struct rbd_client *rbdc;
-	struct ceph_options *opt;
+	struct ceph_options *ceph_opts;
 	struct rbd_options *rbd_opts;
 
 	rbd_opts = kzalloc(sizeof(*rbd_opts), GFP_KERNEL);
@@ -409,29 +409,29 @@ static struct rbd_client *rbd_get_client(const char *mon_addr,
 
 	rbd_opts->notify_timeout = RBD_NOTIFY_TIMEOUT_DEFAULT;
 
-	opt = ceph_parse_options(options, mon_addr,
-				mon_addr + mon_addr_len,
-				parse_rbd_opts_token, rbd_opts);
-	if (IS_ERR(opt)) {
+	ceph_opts = ceph_parse_options(options, mon_addr,
+					mon_addr + mon_addr_len,
+					parse_rbd_opts_token, rbd_opts);
+	if (IS_ERR(ceph_opts)) {
 		kfree(rbd_opts);
-		return ERR_CAST(opt);
+		return ERR_CAST(ceph_opts);
 	}
 
 	spin_lock(&rbd_client_list_lock);
-	rbdc = __rbd_client_find(opt);
+	rbdc = __rbd_client_find(ceph_opts);
 	if (rbdc) {
 		/* using an existing client */
 		kref_get(&rbdc->kref);
 		spin_unlock(&rbd_client_list_lock);
 
-		ceph_destroy_options(opt);
+		ceph_destroy_options(ceph_opts);
 		kfree(rbd_opts);
 
 		return rbdc;
 	}
 	spin_unlock(&rbd_client_list_lock);
 
-	rbdc = rbd_client_create(opt, rbd_opts);
+	rbdc = rbd_client_create(ceph_opts, rbd_opts);
 
 	if (IS_ERR(rbdc))
 		kfree(rbd_opts);
@@ -480,46 +480,60 @@ static void rbd_coll_release(struct kref *kref)
 	kfree(coll);
 }
 
+static bool rbd_dev_ondisk_valid(struct rbd_image_header_ondisk *ondisk)
+{
+	return !memcmp(&ondisk->text,
+			RBD_HEADER_TEXT, sizeof (RBD_HEADER_TEXT));
+}
+
 /*
  * Create a new header structure, translate header format from the on-disk
  * header.
  */
 static int rbd_header_from_disk(struct rbd_image_header *header,
 				 struct rbd_image_header_ondisk *ondisk,
-				 u32 allocated_snaps,
-				 gfp_t gfp_flags)
+				 u32 allocated_snaps)
 {
-	u32 i, snap_count;
+	u32 snap_count;
 
-	if (memcmp(ondisk, RBD_HEADER_TEXT, sizeof(RBD_HEADER_TEXT)))
+	if (!rbd_dev_ondisk_valid(ondisk))
 		return -ENXIO;
 
 	snap_count = le32_to_cpu(ondisk->snap_count);
-	if (snap_count > (UINT_MAX - sizeof(struct ceph_snap_context))
-			 / sizeof (*ondisk))
+	if (snap_count > (SIZE_MAX - sizeof(struct ceph_snap_context))
+				 / sizeof (u64))
 		return -EINVAL;
 	header->snapc = kmalloc(sizeof(struct ceph_snap_context) +
-				snap_count * sizeof (*ondisk),
-				gfp_flags);
+				snap_count * sizeof(u64),
+				GFP_KERNEL);
 	if (!header->snapc)
 		return -ENOMEM;
 
-	header->snap_names_len = le64_to_cpu(ondisk->snap_names_len);
 	if (snap_count) {
+		header->snap_names_len = le64_to_cpu(ondisk->snap_names_len);
 		header->snap_names = kmalloc(header->snap_names_len,
-					     gfp_flags);
+					     GFP_KERNEL);
 		if (!header->snap_names)
 			goto err_snapc;
 		header->snap_sizes = kmalloc(snap_count * sizeof(u64),
-					     gfp_flags);
+					     GFP_KERNEL);
 		if (!header->snap_sizes)
 			goto err_names;
 	} else {
+		WARN_ON(ondisk->snap_names_len);
+		header->snap_names_len = 0;
 		header->snap_names = NULL;
 		header->snap_sizes = NULL;
 	}
-	memcpy(header->block_name, ondisk->block_name,
+
+	header->object_prefix = kmalloc(sizeof (ondisk->block_name) + 1,
+					GFP_KERNEL);
+	if (!header->object_prefix)
+		goto err_sizes;
+
+	memcpy(header->object_prefix, ondisk->block_name,
 	       sizeof(ondisk->block_name));
+	header->object_prefix[sizeof (ondisk->block_name)] = '\0';
 
 	header->image_size = le64_to_cpu(ondisk->image_size);
 	header->obj_order = ondisk->options.order;
@@ -527,11 +541,13 @@ static int rbd_header_from_disk(struct rbd_image_header *header,
 	header->comp_type = ondisk->options.comp_type;
 
 	atomic_set(&header->snapc->nref, 1);
-	header->snap_seq = le64_to_cpu(ondisk->snap_seq);
+	header->snapc->seq = le64_to_cpu(ondisk->snap_seq);
 	header->snapc->num_snaps = snap_count;
 	header->total_snaps = snap_count;
 
 	if (snap_count && allocated_snaps == snap_count) {
+		int i;
+
 		for (i = 0; i < snap_count; i++) {
 			header->snapc->snaps[i] =
 				le64_to_cpu(ondisk->snaps[i].id);
@@ -540,16 +556,22 @@ static int rbd_header_from_disk(struct rbd_image_header *header,
 		}
 
 		/* copy snapshot names */
-		memcpy(header->snap_names, &ondisk->snaps[i],
+		memcpy(header->snap_names, &ondisk->snaps[snap_count],
 			header->snap_names_len);
 	}
 
 	return 0;
 
+err_sizes:
+	kfree(header->snap_sizes);
+	header->snap_sizes = NULL;
 err_names:
 	kfree(header->snap_names);
+	header->snap_names = NULL;
 err_snapc:
 	kfree(header->snapc);
+	header->snapc = NULL;
+
 	return -ENOMEM;
 }
 
@@ -575,52 +597,50 @@ static int snap_by_name(struct rbd_image_header *header, const char *snap_name,
 	return -ENOENT;
 }
 
-static int rbd_header_set_snap(struct rbd_device *dev, u64 *size)
+static int rbd_header_set_snap(struct rbd_device *rbd_dev, u64 *size)
 {
-	struct rbd_image_header *header = &dev->header;
-	struct ceph_snap_context *snapc = header->snapc;
-	int ret = -ENOENT;
-
-	BUILD_BUG_ON(sizeof (dev->snap_name) < sizeof (RBD_SNAP_HEAD_NAME));
+	int ret;
 
-	down_write(&dev->header_rwsem);
+	down_write(&rbd_dev->header_rwsem);
 
-	if (!memcmp(dev->snap_name, RBD_SNAP_HEAD_NAME,
+	if (!memcmp(rbd_dev->snap_name, RBD_SNAP_HEAD_NAME,
 		    sizeof (RBD_SNAP_HEAD_NAME))) {
-		if (header->total_snaps)
-			snapc->seq = header->snap_seq;
-		else
-			snapc->seq = 0;
-		dev->snap_id = CEPH_NOSNAP;
-		dev->read_only = 0;
+		rbd_dev->snap_id = CEPH_NOSNAP;
+		rbd_dev->snap_exists = false;
+		rbd_dev->read_only = 0;
 		if (size)
-			*size = header->image_size;
+			*size = rbd_dev->header.image_size;
 	} else {
-		ret = snap_by_name(header, dev->snap_name, &snapc->seq, size);
+		u64 snap_id = 0;
+
+		ret = snap_by_name(&rbd_dev->header, rbd_dev->snap_name,
+					&snap_id, size);
 		if (ret < 0)
 			goto done;
-		dev->snap_id = snapc->seq;
-		dev->read_only = 1;
+		rbd_dev->snap_id = snap_id;
+		rbd_dev->snap_exists = true;
+		rbd_dev->read_only = 1;
 	}
 
 	ret = 0;
 done:
-	up_write(&dev->header_rwsem);
+	up_write(&rbd_dev->header_rwsem);
 	return ret;
 }
 
 static void rbd_header_free(struct rbd_image_header *header)
 {
-	kfree(header->snapc);
-	kfree(header->snap_names);
+	kfree(header->object_prefix);
 	kfree(header->snap_sizes);
+	kfree(header->snap_names);
+	ceph_put_snap_context(header->snapc);
 }
 
 /*
  * get the actual striped segment name, offset and length
  */
 static u64 rbd_get_segment(struct rbd_image_header *header,
-			   const char *block_name,
+			   const char *object_prefix,
 			   u64 ofs, u64 len,
 			   char *seg_name, u64 *segofs)
 {
@@ -628,7 +648,7 @@ static u64 rbd_get_segment(struct rbd_image_header *header,
 
 	if (seg_name)
 		snprintf(seg_name, RBD_MAX_SEG_NAME_LEN,
-			 "%s.%012llx", block_name, seg);
+			 "%s.%012llx", object_prefix, seg);
 
 	ofs = ofs & ((1 << header->obj_order) - 1);
 	len = min_t(u64, len, (1 << header->obj_order) - ofs);
@@ -726,9 +746,8 @@ static struct bio *bio_chain_clone(struct bio **old, struct bio **next,
 			 * split_bio will BUG_ON if this is not the case
 			 */
 			dout("bio_chain_clone split! total=%d remaining=%d"
-			     "bi_size=%d\n",
-			     (int)total, (int)len-total,
-			     (int)old_chain->bi_size);
+			     "bi_size=%u\n",
+			     total, len - total, old_chain->bi_size);
 
 			/* split the bio. We'll release it either in the next
 			   call, or it will have to be released outside */
@@ -777,22 +796,24 @@ err_out:
 /*
  * helpers for osd request op vectors.
  */
-static int rbd_create_rw_ops(struct ceph_osd_req_op **ops,
-			    int num_ops,
-			    int opcode,
-			    u32 payload_len)
-{
-	*ops = kzalloc(sizeof(struct ceph_osd_req_op) * (num_ops + 1),
-		       GFP_NOIO);
-	if (!*ops)
-		return -ENOMEM;
-	(*ops)[0].op = opcode;
+static struct ceph_osd_req_op *rbd_create_rw_ops(int num_ops,
+					int opcode, u32 payload_len)
+{
+	struct ceph_osd_req_op *ops;
+
+	ops = kzalloc(sizeof (*ops) * (num_ops + 1), GFP_NOIO);
+	if (!ops)
+		return NULL;
+
+	ops[0].op = opcode;
+
 	/*
 	 * op extent offset and length will be set later on
 	 * in calc_raw_layout()
 	 */
-	(*ops)[0].payload_len = payload_len;
-	return 0;
+	ops[0].payload_len = payload_len;
+
+	return ops;
 }
 
 static void rbd_destroy_ops(struct ceph_osd_req_op *ops)
@@ -808,8 +829,8 @@ static void rbd_coll_end_req_index(struct request *rq,
 	struct request_queue *q;
 	int min, max, i;
 
-	dout("rbd_coll_end_req_index %p index %d ret %d len %lld\n",
-	     coll, index, ret, len);
+	dout("rbd_coll_end_req_index %p index %d ret %d len %llu\n",
+	     coll, index, ret, (unsigned long long) len);
 
 	if (!rq)
 		return;
@@ -848,16 +869,15 @@ static void rbd_coll_end_req(struct rbd_request *req,
  * Send ceph osd request
  */
 static int rbd_do_request(struct request *rq,
-			  struct rbd_device *dev,
+			  struct rbd_device *rbd_dev,
 			  struct ceph_snap_context *snapc,
 			  u64 snapid,
-			  const char *obj, u64 ofs, u64 len,
+			  const char *object_name, u64 ofs, u64 len,
 			  struct bio *bio,
 			  struct page **pages,
 			  int num_pages,
 			  int flags,
 			  struct ceph_osd_req_op *ops,
-			  int num_reply,
 			  struct rbd_req_coll *coll,
 			  int coll_index,
 			  void (*rbd_cb)(struct ceph_osd_request *req,
@@ -887,15 +907,13 @@ static int rbd_do_request(struct request *rq,
 		req_data->coll_index = coll_index;
 	}
 
-	dout("rbd_do_request obj=%s ofs=%lld len=%lld\n", obj, len, ofs);
-
-	down_read(&dev->header_rwsem);
+	dout("rbd_do_request object_name=%s ofs=%llu len=%llu\n", object_name,
+		(unsigned long long) ofs, (unsigned long long) len);
 
-	osdc = &dev->rbd_client->client->osdc;
+	osdc = &rbd_dev->rbd_client->client->osdc;
 	req = ceph_osdc_alloc_request(osdc, flags, snapc, ops,
 					false, GFP_NOIO, pages, bio);
 	if (!req) {
-		up_read(&dev->header_rwsem);
 		ret = -ENOMEM;
 		goto done_pages;
 	}
@@ -912,7 +930,7 @@ static int rbd_do_request(struct request *rq,
 	reqhead = req->r_request->front.iov_base;
 	reqhead->snapid = cpu_to_le64(CEPH_NOSNAP);
 
-	strncpy(req->r_oid, obj, sizeof(req->r_oid));
+	strncpy(req->r_oid, object_name, sizeof(req->r_oid));
 	req->r_oid_len = strlen(req->r_oid);
 
 	layout = &req->r_file_layout;
@@ -920,7 +938,7 @@ static int rbd_do_request(struct request *rq,
 	layout->fl_stripe_unit = cpu_to_le32(1 << RBD_MAX_OBJ_ORDER);
 	layout->fl_stripe_count = cpu_to_le32(1);
 	layout->fl_object_size = cpu_to_le32(1 << RBD_MAX_OBJ_ORDER);
-	layout->fl_pg_pool = cpu_to_le32(dev->poolid);
+	layout->fl_pg_pool = cpu_to_le32(rbd_dev->pool_id);
 	ceph_calc_raw_layout(osdc, layout, snapid, ofs, &len, &bno,
 				req, ops);
 
@@ -929,7 +947,6 @@ static int rbd_do_request(struct request *rq,
 				snapc,
 				&mtime,
 				req->r_oid, req->r_oid_len);
-	up_read(&dev->header_rwsem);
 
 	if (linger_req) {
 		ceph_osdc_set_request_linger(osdc, req);
@@ -944,8 +961,9 @@ static int rbd_do_request(struct request *rq,
 		ret = ceph_osdc_wait_request(osdc, req);
 		if (ver)
 			*ver = le64_to_cpu(req->r_reassert_version.version);
-		dout("reassert_ver=%lld\n",
-		     le64_to_cpu(req->r_reassert_version.version));
+		dout("reassert_ver=%llu\n",
+			(unsigned long long)
+				le64_to_cpu(req->r_reassert_version.version));
 		ceph_osdc_put_request(req);
 	}
 	return ret;
@@ -977,9 +995,10 @@ static void rbd_req_cb(struct ceph_osd_request *req, struct ceph_msg *msg)
 	op = (void *)(replyhead + 1);
 	rc = le32_to_cpu(replyhead->result);
 	bytes = le64_to_cpu(op->extent.length);
-	read_op = (le32_to_cpu(op->op) == CEPH_OSD_OP_READ);
+	read_op = (le16_to_cpu(op->op) == CEPH_OSD_OP_READ);
 
-	dout("rbd_req_cb bytes=%lld readop=%d rc=%d\n", bytes, read_op, rc);
+	dout("rbd_req_cb bytes=%llu readop=%d rc=%d\n",
+		(unsigned long long) bytes, read_op, (int) rc);
 
 	if (rc == -ENOENT && read_op) {
 		zero_bio_chain(req_data->bio, 0);
@@ -1006,14 +1025,12 @@ static void rbd_simple_req_cb(struct ceph_osd_request *req, struct ceph_msg *msg
 /*
  * Do a synchronous ceph osd operation
  */
-static int rbd_req_sync_op(struct rbd_device *dev,
+static int rbd_req_sync_op(struct rbd_device *rbd_dev,
 			   struct ceph_snap_context *snapc,
 			   u64 snapid,
-			   int opcode,
 			   int flags,
-			   struct ceph_osd_req_op *orig_ops,
-			   int num_reply,
-			   const char *obj,
+			   struct ceph_osd_req_op *ops,
+			   const char *object_name,
 			   u64 ofs, u64 len,
 			   char *buf,
 			   struct ceph_osd_request **linger_req,
@@ -1022,45 +1039,28 @@ static int rbd_req_sync_op(struct rbd_device *dev,
 	int ret;
 	struct page **pages;
 	int num_pages;
-	struct ceph_osd_req_op *ops = orig_ops;
-	u32 payload_len;
+
+	BUG_ON(ops == NULL);
 
 	num_pages = calc_pages_for(ofs , len);
 	pages = ceph_alloc_page_vector(num_pages, GFP_KERNEL);
 	if (IS_ERR(pages))
 		return PTR_ERR(pages);
 
-	if (!orig_ops) {
-		payload_len = (flags & CEPH_OSD_FLAG_WRITE ? len : 0);
-		ret = rbd_create_rw_ops(&ops, 1, opcode, payload_len);
-		if (ret < 0)
-			goto done;
-
-		if ((flags & CEPH_OSD_FLAG_WRITE) && buf) {
-			ret = ceph_copy_to_page_vector(pages, buf, ofs, len);
-			if (ret < 0)
-				goto done_ops;
-		}
-	}
-
-	ret = rbd_do_request(NULL, dev, snapc, snapid,
-			  obj, ofs, len, NULL,
+	ret = rbd_do_request(NULL, rbd_dev, snapc, snapid,
+			  object_name, ofs, len, NULL,
 			  pages, num_pages,
 			  flags,
 			  ops,
-			  2,
 			  NULL, 0,
 			  NULL,
 			  linger_req, ver);
 	if (ret < 0)
-		goto done_ops;
+		goto done;
 
 	if ((flags & CEPH_OSD_FLAG_READ) && buf)
 		ret = ceph_copy_from_page_vector(pages, buf, ofs, ret);
 
-done_ops:
-	if (!orig_ops)
-		rbd_destroy_ops(ops);
 done:
 	ceph_release_page_vector(pages, num_pages);
 	return ret;
@@ -1070,10 +1070,10 @@ done:
  * Do an asynchronous ceph osd operation
  */
 static int rbd_do_op(struct request *rq,
-		     struct rbd_device *rbd_dev ,
+		     struct rbd_device *rbd_dev,
 		     struct ceph_snap_context *snapc,
 		     u64 snapid,
-		     int opcode, int flags, int num_reply,
+		     int opcode, int flags,
 		     u64 ofs, u64 len,
 		     struct bio *bio,
 		     struct rbd_req_coll *coll,
@@ -1091,14 +1091,15 @@ static int rbd_do_op(struct request *rq,
 		return -ENOMEM;
 
 	seg_len = rbd_get_segment(&rbd_dev->header,
-				  rbd_dev->header.block_name,
+				  rbd_dev->header.object_prefix,
 				  ofs, len,
 				  seg_name, &seg_ofs);
 
 	payload_len = (flags & CEPH_OSD_FLAG_WRITE ? seg_len : 0);
 
-	ret = rbd_create_rw_ops(&ops, 1, opcode, payload_len);
-	if (ret < 0)
+	ret = -ENOMEM;
+	ops = rbd_create_rw_ops(1, opcode, payload_len);
+	if (!ops)
 		goto done;
 
 	/* we've taken care of segment sizes earlier when we
@@ -1112,7 +1113,6 @@ static int rbd_do_op(struct request *rq,
 			     NULL, 0,
 			     flags,
 			     ops,
-			     num_reply,
 			     coll, coll_index,
 			     rbd_req_cb, 0, NULL);
 
@@ -1136,7 +1136,6 @@ static int rbd_req_write(struct request *rq,
 	return rbd_do_op(rq, rbd_dev, snapc, CEPH_NOSNAP,
 			 CEPH_OSD_OP_WRITE,
 			 CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
-			 2,
 			 ofs, len, bio, coll, coll_index);
 }
 
@@ -1155,55 +1154,58 @@ static int rbd_req_read(struct request *rq,
 			 snapid,
 			 CEPH_OSD_OP_READ,
 			 CEPH_OSD_FLAG_READ,
-			 2,
 			 ofs, len, bio, coll, coll_index);
 }
 
 /*
  * Request sync osd read
  */
-static int rbd_req_sync_read(struct rbd_device *dev,
-			  struct ceph_snap_context *snapc,
+static int rbd_req_sync_read(struct rbd_device *rbd_dev,
 			  u64 snapid,
-			  const char *obj,
+			  const char *object_name,
 			  u64 ofs, u64 len,
 			  char *buf,
 			  u64 *ver)
 {
-	return rbd_req_sync_op(dev, NULL,
+	struct ceph_osd_req_op *ops;
+	int ret;
+
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_READ, 0);
+	if (!ops)
+		return -ENOMEM;
+
+	ret = rbd_req_sync_op(rbd_dev, NULL,
 			       snapid,
-			       CEPH_OSD_OP_READ,
 			       CEPH_OSD_FLAG_READ,
-			       NULL,
-			       1, obj, ofs, len, buf, NULL, ver);
+			       ops, object_name, ofs, len, buf, NULL, ver);
+	rbd_destroy_ops(ops);
+
+	return ret;
 }
 
 /*
  * Request sync osd watch
  */
-static int rbd_req_sync_notify_ack(struct rbd_device *dev,
+static int rbd_req_sync_notify_ack(struct rbd_device *rbd_dev,
 				   u64 ver,
-				   u64 notify_id,
-				   const char *obj)
+				   u64 notify_id)
 {
 	struct ceph_osd_req_op *ops;
-	struct page **pages = NULL;
 	int ret;
 
-	ret = rbd_create_rw_ops(&ops, 1, CEPH_OSD_OP_NOTIFY_ACK, 0);
-	if (ret < 0)
-		return ret;
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_NOTIFY_ACK, 0);
+	if (!ops)
+		return -ENOMEM;
 
-	ops[0].watch.ver = cpu_to_le64(dev->header.obj_version);
+	ops[0].watch.ver = cpu_to_le64(ver);
 	ops[0].watch.cookie = notify_id;
 	ops[0].watch.flag = 0;
 
-	ret = rbd_do_request(NULL, dev, NULL, CEPH_NOSNAP,
-			  obj, 0, 0, NULL,
-			  pages, 0,
+	ret = rbd_do_request(NULL, rbd_dev, NULL, CEPH_NOSNAP,
+			  rbd_dev->header_name, 0, 0, NULL,
+			  NULL, 0,
 			  CEPH_OSD_FLAG_READ,
 			  ops,
-			  1,
 			  NULL, 0,
 			  rbd_simple_req_cb, 0, NULL);
 
@@ -1213,54 +1215,53 @@ static int rbd_req_sync_notify_ack(struct rbd_device *dev,
 
 static void rbd_watch_cb(u64 ver, u64 notify_id, u8 opcode, void *data)
 {
-	struct rbd_device *dev = (struct rbd_device *)data;
+	struct rbd_device *rbd_dev = (struct rbd_device *)data;
+	u64 hver;
 	int rc;
 
-	if (!dev)
+	if (!rbd_dev)
 		return;
 
-	dout("rbd_watch_cb %s notify_id=%lld opcode=%d\n", dev->obj_md_name,
-		notify_id, (int)opcode);
-	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
-	rc = __rbd_refresh_header(dev);
-	mutex_unlock(&ctl_mutex);
+	dout("rbd_watch_cb %s notify_id=%llu opcode=%u\n",
+		rbd_dev->header_name, (unsigned long long) notify_id,
+		(unsigned int) opcode);
+	rc = rbd_refresh_header(rbd_dev, &hver);
 	if (rc)
 		pr_warning(RBD_DRV_NAME "%d got notification but failed to "
-			   " update snaps: %d\n", dev->major, rc);
+			   " update snaps: %d\n", rbd_dev->major, rc);
 
-	rbd_req_sync_notify_ack(dev, ver, notify_id, dev->obj_md_name);
+	rbd_req_sync_notify_ack(rbd_dev, hver, notify_id);
 }
 
 /*
  * Request sync osd watch
  */
-static int rbd_req_sync_watch(struct rbd_device *dev,
-			      const char *obj,
-			      u64 ver)
+static int rbd_req_sync_watch(struct rbd_device *rbd_dev)
 {
 	struct ceph_osd_req_op *ops;
-	struct ceph_osd_client *osdc = &dev->rbd_client->client->osdc;
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	int ret;
 
-	int ret = rbd_create_rw_ops(&ops, 1, CEPH_OSD_OP_WATCH, 0);
-	if (ret < 0)
-		return ret;
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_WATCH, 0);
+	if (!ops)
+		return -ENOMEM;
 
 	ret = ceph_osdc_create_event(osdc, rbd_watch_cb, 0,
-				     (void *)dev, &dev->watch_event);
+				     (void *)rbd_dev, &rbd_dev->watch_event);
 	if (ret < 0)
 		goto fail;
 
-	ops[0].watch.ver = cpu_to_le64(ver);
-	ops[0].watch.cookie = cpu_to_le64(dev->watch_event->cookie);
+	ops[0].watch.ver = cpu_to_le64(rbd_dev->header.obj_version);
+	ops[0].watch.cookie = cpu_to_le64(rbd_dev->watch_event->cookie);
 	ops[0].watch.flag = 1;
 
-	ret = rbd_req_sync_op(dev, NULL,
+	ret = rbd_req_sync_op(rbd_dev, NULL,
 			      CEPH_NOSNAP,
-			      0,
 			      CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
 			      ops,
-			      1, obj, 0, 0, NULL,
-			      &dev->watch_request, NULL);
+			      rbd_dev->header_name,
+			      0, 0, NULL,
+			      &rbd_dev->watch_request, NULL);
 
 	if (ret < 0)
 		goto fail_event;
@@ -1269,8 +1270,8 @@ static int rbd_req_sync_watch(struct rbd_device *dev,
 	return 0;
 
 fail_event:
-	ceph_osdc_cancel_event(dev->watch_event);
-	dev->watch_event = NULL;
+	ceph_osdc_cancel_event(rbd_dev->watch_event);
+	rbd_dev->watch_event = NULL;
 fail:
 	rbd_destroy_ops(ops);
 	return ret;
@@ -1279,64 +1280,65 @@ fail:
 /*
  * Request sync osd unwatch
  */
-static int rbd_req_sync_unwatch(struct rbd_device *dev,
-				const char *obj)
+static int rbd_req_sync_unwatch(struct rbd_device *rbd_dev)
 {
 	struct ceph_osd_req_op *ops;
+	int ret;
 
-	int ret = rbd_create_rw_ops(&ops, 1, CEPH_OSD_OP_WATCH, 0);
-	if (ret < 0)
-		return ret;
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_WATCH, 0);
+	if (!ops)
+		return -ENOMEM;
 
 	ops[0].watch.ver = 0;
-	ops[0].watch.cookie = cpu_to_le64(dev->watch_event->cookie);
+	ops[0].watch.cookie = cpu_to_le64(rbd_dev->watch_event->cookie);
 	ops[0].watch.flag = 0;
 
-	ret = rbd_req_sync_op(dev, NULL,
+	ret = rbd_req_sync_op(rbd_dev, NULL,
 			      CEPH_NOSNAP,
-			      0,
 			      CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
 			      ops,
-			      1, obj, 0, 0, NULL, NULL, NULL);
+			      rbd_dev->header_name,
+			      0, 0, NULL, NULL, NULL);
+
 
 	rbd_destroy_ops(ops);
-	ceph_osdc_cancel_event(dev->watch_event);
-	dev->watch_event = NULL;
+	ceph_osdc_cancel_event(rbd_dev->watch_event);
+	rbd_dev->watch_event = NULL;
 	return ret;
 }
 
 struct rbd_notify_info {
-	struct rbd_device *dev;
+	struct rbd_device *rbd_dev;
 };
 
 static void rbd_notify_cb(u64 ver, u64 notify_id, u8 opcode, void *data)
 {
-	struct rbd_device *dev = (struct rbd_device *)data;
-	if (!dev)
+	struct rbd_device *rbd_dev = (struct rbd_device *)data;
+	if (!rbd_dev)
 		return;
 
-	dout("rbd_notify_cb %s notify_id=%lld opcode=%d\n", dev->obj_md_name,
-		notify_id, (int)opcode);
+	dout("rbd_notify_cb %s notify_id=%llu opcode=%u\n",
+			rbd_dev->header_name, (unsigned long long) notify_id,
+			(unsigned int) opcode);
 }
 
 /*
  * Request sync osd notify
  */
-static int rbd_req_sync_notify(struct rbd_device *dev,
-		          const char *obj)
+static int rbd_req_sync_notify(struct rbd_device *rbd_dev)
 {
 	struct ceph_osd_req_op *ops;
-	struct ceph_osd_client *osdc = &dev->rbd_client->client->osdc;
+	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
 	struct ceph_osd_event *event;
 	struct rbd_notify_info info;
 	int payload_len = sizeof(u32) + sizeof(u32);
 	int ret;
 
-	ret = rbd_create_rw_ops(&ops, 1, CEPH_OSD_OP_NOTIFY, payload_len);
-	if (ret < 0)
-		return ret;
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_NOTIFY, payload_len);
+	if (!ops)
+		return -ENOMEM;
 
-	info.dev = dev;
+	info.rbd_dev = rbd_dev;
 
 	ret = ceph_osdc_create_event(osdc, rbd_notify_cb, 1,
 				     (void *)&info, &event);
@@ -1349,12 +1351,12 @@ static int rbd_req_sync_notify(struct rbd_device *dev,
 	ops[0].watch.prot_ver = RADOS_NOTIFY_VER;
 	ops[0].watch.timeout = 12;
 
-	ret = rbd_req_sync_op(dev, NULL,
+	ret = rbd_req_sync_op(rbd_dev, NULL,
 			       CEPH_NOSNAP,
-			       0,
 			       CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
 			       ops,
-			       1, obj, 0, 0, NULL, NULL, NULL);
+			       rbd_dev->header_name,
+			       0, 0, NULL, NULL, NULL);
 	if (ret < 0)
 		goto fail_event;
 
@@ -1373,36 +1375,37 @@ fail:
 /*
  * Request sync osd read
  */
-static int rbd_req_sync_exec(struct rbd_device *dev,
-			     const char *obj,
-			     const char *cls,
-			     const char *method,
+static int rbd_req_sync_exec(struct rbd_device *rbd_dev,
+			     const char *object_name,
+			     const char *class_name,
+			     const char *method_name,
 			     const char *data,
 			     int len,
 			     u64 *ver)
 {
 	struct ceph_osd_req_op *ops;
-	int cls_len = strlen(cls);
-	int method_len = strlen(method);
-	int ret = rbd_create_rw_ops(&ops, 1, CEPH_OSD_OP_CALL,
-				    cls_len + method_len + len);
-	if (ret < 0)
-		return ret;
+	int class_name_len = strlen(class_name);
+	int method_name_len = strlen(method_name);
+	int ret;
 
-	ops[0].cls.class_name = cls;
-	ops[0].cls.class_len = (__u8)cls_len;
-	ops[0].cls.method_name = method;
-	ops[0].cls.method_len = (__u8)method_len;
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_CALL,
+				    class_name_len + method_name_len + len);
+	if (!ops)
+		return -ENOMEM;
+
+	ops[0].cls.class_name = class_name;
+	ops[0].cls.class_len = (__u8) class_name_len;
+	ops[0].cls.method_name = method_name;
+	ops[0].cls.method_len = (__u8) method_name_len;
 	ops[0].cls.argc = 0;
 	ops[0].cls.indata = data;
 	ops[0].cls.indata_len = len;
 
-	ret = rbd_req_sync_op(dev, NULL,
+	ret = rbd_req_sync_op(rbd_dev, NULL,
 			       CEPH_NOSNAP,
-			       0,
 			       CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
 			       ops,
-			       1, obj, 0, 0, NULL, NULL, ver);
+			       object_name, 0, 0, NULL, NULL, ver);
 
 	rbd_destroy_ops(ops);
 
@@ -1437,10 +1440,12 @@ static void rbd_rq_fn(struct request_queue *q)
 		struct bio *bio;
 		struct bio *rq_bio, *next_bio = NULL;
 		bool do_write;
-		int size, op_size = 0;
+		unsigned int size;
+		u64 op_size = 0;
 		u64 ofs;
 		int num_segs, cur_seg = 0;
 		struct rbd_req_coll *coll;
+		struct ceph_snap_context *snapc;
 
 		/* peek at request from block layer */
 		if (!rq)
@@ -1467,23 +1472,38 @@ static void rbd_rq_fn(struct request_queue *q)
 
 		spin_unlock_irq(q->queue_lock);
 
+		down_read(&rbd_dev->header_rwsem);
+
+		if (rbd_dev->snap_id != CEPH_NOSNAP && !rbd_dev->snap_exists) {
+			up_read(&rbd_dev->header_rwsem);
+			dout("request for non-existent snapshot");
+			spin_lock_irq(q->queue_lock);
+			__blk_end_request_all(rq, -ENXIO);
+			continue;
+		}
+
+		snapc = ceph_get_snap_context(rbd_dev->header.snapc);
+
+		up_read(&rbd_dev->header_rwsem);
+
 		dout("%s 0x%x bytes at 0x%llx\n",
 		     do_write ? "write" : "read",
-		     size, blk_rq_pos(rq) * SECTOR_SIZE);
+		     size, (unsigned long long) blk_rq_pos(rq) * SECTOR_SIZE);
 
 		num_segs = rbd_get_num_segments(&rbd_dev->header, ofs, size);
 		coll = rbd_alloc_coll(num_segs);
 		if (!coll) {
 			spin_lock_irq(q->queue_lock);
 			__blk_end_request_all(rq, -ENOMEM);
+			ceph_put_snap_context(snapc);
 			continue;
 		}
 
 		do {
 			/* a bio clone to be passed down to OSD req */
-			dout("rq->bio->bi_vcnt=%d\n", rq->bio->bi_vcnt);
+			dout("rq->bio->bi_vcnt=%hu\n", rq->bio->bi_vcnt);
 			op_size = rbd_get_segment(&rbd_dev->header,
-						  rbd_dev->header.block_name,
+						  rbd_dev->header.object_prefix,
 						  ofs, size,
 						  NULL, NULL);
 			kref_get(&coll->kref);
@@ -1499,7 +1519,7 @@ static void rbd_rq_fn(struct request_queue *q)
 			/* init OSD command: write or read */
 			if (do_write)
 				rbd_req_write(rq, rbd_dev,
-					      rbd_dev->header.snapc,
+					      snapc,
 					      ofs,
 					      op_size, bio,
 					      coll, cur_seg);
@@ -1522,6 +1542,8 @@ next_seg:
 		if (bp)
 			bio_pair_release(bp);
 		spin_lock_irq(q->queue_lock);
+
+		ceph_put_snap_context(snapc);
 	}
 }
 
@@ -1592,18 +1614,19 @@ static int rbd_read_header(struct rbd_device *rbd_dev,
 			return -ENOMEM;
 
 		rc = rbd_req_sync_read(rbd_dev,
-				       NULL, CEPH_NOSNAP,
-				       rbd_dev->obj_md_name,
+				       CEPH_NOSNAP,
+				       rbd_dev->header_name,
 				       0, len,
 				       (char *)dh, &ver);
 		if (rc < 0)
 			goto out_dh;
 
-		rc = rbd_header_from_disk(header, dh, snap_count, GFP_KERNEL);
+		rc = rbd_header_from_disk(header, dh, snap_count);
 		if (rc < 0) {
 			if (rc == -ENXIO)
 				pr_warning("unrecognized header format"
-					   " for image %s", rbd_dev->obj);
+					   " for image %s\n",
+					   rbd_dev->image_name);
 			goto out_dh;
 		}
 
@@ -1628,7 +1651,7 @@ out_dh:
 /*
  * create a snapshot
  */
-static int rbd_header_add_snap(struct rbd_device *dev,
+static int rbd_header_add_snap(struct rbd_device *rbd_dev,
 			       const char *snap_name,
 			       gfp_t gfp_flags)
 {
@@ -1636,16 +1659,15 @@ static int rbd_header_add_snap(struct rbd_device *dev,
 	u64 new_snapid;
 	int ret;
 	void *data, *p, *e;
-	u64 ver;
 	struct ceph_mon_client *monc;
 
 	/* we should create a snapshot only if we're pointing at the head */
-	if (dev->snap_id != CEPH_NOSNAP)
+	if (rbd_dev->snap_id != CEPH_NOSNAP)
 		return -EINVAL;
 
-	monc = &dev->rbd_client->client->monc;
-	ret = ceph_monc_create_snapid(monc, dev->poolid, &new_snapid);
-	dout("created snapid=%lld\n", new_snapid);
+	monc = &rbd_dev->rbd_client->client->monc;
+	ret = ceph_monc_create_snapid(monc, rbd_dev->pool_id, &new_snapid);
+	dout("created snapid=%llu\n", (unsigned long long) new_snapid);
 	if (ret < 0)
 		return ret;
 
@@ -1659,19 +1681,13 @@ static int rbd_header_add_snap(struct rbd_device *dev,
 	ceph_encode_string_safe(&p, e, snap_name, name_len, bad);
 	ceph_encode_64_safe(&p, e, new_snapid, bad);
 
-	ret = rbd_req_sync_exec(dev, dev->obj_md_name, "rbd", "snap_add",
-				data, p - data, &ver);
+	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
+				"rbd", "snap_add",
+				data, p - data, NULL);
 
 	kfree(data);
 
-	if (ret < 0)
-		return ret;
-
-	down_write(&dev->header_rwsem);
-	dev->header.snapc->seq = new_snapid;
-	up_write(&dev->header_rwsem);
-
-	return 0;
+	return ret < 0 ? ret : 0;
 bad:
 	return -ERANGE;
 }
@@ -1679,52 +1695,52 @@ bad:
 static void __rbd_remove_all_snaps(struct rbd_device *rbd_dev)
 {
 	struct rbd_snap *snap;
+	struct rbd_snap *next;
 
-	while (!list_empty(&rbd_dev->snaps)) {
-		snap = list_first_entry(&rbd_dev->snaps, struct rbd_snap, node);
-		__rbd_remove_snap_dev(rbd_dev, snap);
-	}
+	list_for_each_entry_safe(snap, next, &rbd_dev->snaps, node)
+		__rbd_remove_snap_dev(snap);
 }
 
 /*
  * only read the first part of the ondisk header, without the snaps info
  */
-static int __rbd_refresh_header(struct rbd_device *rbd_dev)
+static int __rbd_refresh_header(struct rbd_device *rbd_dev, u64 *hver)
 {
 	int ret;
 	struct rbd_image_header h;
-	u64 snap_seq;
-	int follow_seq = 0;
 
 	ret = rbd_read_header(rbd_dev, &h);
 	if (ret < 0)
 		return ret;
 
-	/* resized? */
-	set_capacity(rbd_dev->disk, h.image_size / SECTOR_SIZE);
-
 	down_write(&rbd_dev->header_rwsem);
 
-	snap_seq = rbd_dev->header.snapc->seq;
-	if (rbd_dev->header.total_snaps &&
-	    rbd_dev->header.snapc->snaps[0] == snap_seq)
-		/* pointing at the head, will need to follow that
-		   if head moves */
-		follow_seq = 1;
+	/* resized? */
+	if (rbd_dev->snap_id == CEPH_NOSNAP) {
+		sector_t size = (sector_t) h.image_size / SECTOR_SIZE;
 
-	kfree(rbd_dev->header.snapc);
-	kfree(rbd_dev->header.snap_names);
+		dout("setting size to %llu sectors", (unsigned long long) size);
+		set_capacity(rbd_dev->disk, size);
+	}
+
+	/* rbd_dev->header.object_prefix shouldn't change */
 	kfree(rbd_dev->header.snap_sizes);
+	kfree(rbd_dev->header.snap_names);
+	/* osd requests may still refer to snapc */
+	ceph_put_snap_context(rbd_dev->header.snapc);
 
+	if (hver)
+		*hver = h.obj_version;
+	rbd_dev->header.obj_version = h.obj_version;
+	rbd_dev->header.image_size = h.image_size;
 	rbd_dev->header.total_snaps = h.total_snaps;
 	rbd_dev->header.snapc = h.snapc;
 	rbd_dev->header.snap_names = h.snap_names;
 	rbd_dev->header.snap_names_len = h.snap_names_len;
 	rbd_dev->header.snap_sizes = h.snap_sizes;
-	if (follow_seq)
-		rbd_dev->header.snapc->seq = rbd_dev->header.snapc->snaps[0];
-	else
-		rbd_dev->header.snapc->seq = snap_seq;
+	/* Free the extra copy of the object prefix */
+	WARN_ON(strcmp(rbd_dev->header.object_prefix, h.object_prefix));
+	kfree(h.object_prefix);
 
 	ret = __rbd_init_snaps_header(rbd_dev);
 
@@ -1733,6 +1749,17 @@ static int __rbd_refresh_header(struct rbd_device *rbd_dev)
 	return ret;
 }
 
+static int rbd_refresh_header(struct rbd_device *rbd_dev, u64 *hver)
+{
+	int ret;
+
+	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
+	ret = __rbd_refresh_header(rbd_dev, hver);
+	mutex_unlock(&ctl_mutex);
+
+	return ret;
+}
+
 static int rbd_init_disk(struct rbd_device *rbd_dev)
 {
 	struct gendisk *disk;
@@ -1762,7 +1789,7 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 		goto out;
 
 	snprintf(disk->disk_name, sizeof(disk->disk_name), RBD_DRV_NAME "%d",
-		 rbd_dev->id);
+		 rbd_dev->dev_id);
 	disk->major = rbd_dev->major;
 	disk->first_minor = 0;
 	disk->fops = &rbd_bd_ops;
@@ -1819,8 +1846,13 @@ static ssize_t rbd_size_show(struct device *dev,
 			     struct device_attribute *attr, char *buf)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+	sector_t size;
+
+	down_read(&rbd_dev->header_rwsem);
+	size = get_capacity(rbd_dev->disk);
+	up_read(&rbd_dev->header_rwsem);
 
-	return sprintf(buf, "%llu\n", (unsigned long long)rbd_dev->header.image_size);
+	return sprintf(buf, "%llu\n", (unsigned long long) size * SECTOR_SIZE);
 }
 
 static ssize_t rbd_major_show(struct device *dev,
@@ -1848,12 +1880,20 @@ static ssize_t rbd_pool_show(struct device *dev,
 	return sprintf(buf, "%s\n", rbd_dev->pool_name);
 }
 
+static ssize_t rbd_pool_id_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+
+	return sprintf(buf, "%d\n", rbd_dev->pool_id);
+}
+
 static ssize_t rbd_name_show(struct device *dev,
 			     struct device_attribute *attr, char *buf)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
-	return sprintf(buf, "%s\n", rbd_dev->obj);
+	return sprintf(buf, "%s\n", rbd_dev->image_name);
 }
 
 static ssize_t rbd_snap_show(struct device *dev,
@@ -1871,23 +1911,18 @@ static ssize_t rbd_image_refresh(struct device *dev,
 				 size_t size)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
-	int rc;
-	int ret = size;
-
-	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
+	int ret;
 
-	rc = __rbd_refresh_header(rbd_dev);
-	if (rc < 0)
-		ret = rc;
+	ret = rbd_refresh_header(rbd_dev, NULL);
 
-	mutex_unlock(&ctl_mutex);
-	return ret;
+	return ret < 0 ? ret : size;
 }
 
 static DEVICE_ATTR(size, S_IRUGO, rbd_size_show, NULL);
 static DEVICE_ATTR(major, S_IRUGO, rbd_major_show, NULL);
 static DEVICE_ATTR(client_id, S_IRUGO, rbd_client_id_show, NULL);
 static DEVICE_ATTR(pool, S_IRUGO, rbd_pool_show, NULL);
+static DEVICE_ATTR(pool_id, S_IRUGO, rbd_pool_id_show, NULL);
 static DEVICE_ATTR(name, S_IRUGO, rbd_name_show, NULL);
 static DEVICE_ATTR(refresh, S_IWUSR, NULL, rbd_image_refresh);
 static DEVICE_ATTR(current_snap, S_IRUGO, rbd_snap_show, NULL);
@@ -1898,6 +1933,7 @@ static struct attribute *rbd_attrs[] = {
 	&dev_attr_major.attr,
 	&dev_attr_client_id.attr,
 	&dev_attr_pool.attr,
+	&dev_attr_pool_id.attr,
 	&dev_attr_name.attr,
 	&dev_attr_current_snap.attr,
 	&dev_attr_refresh.attr,
@@ -1977,15 +2013,13 @@ static struct device_type rbd_snap_device_type = {
 	.release	= rbd_snap_dev_release,
 };
 
-static void __rbd_remove_snap_dev(struct rbd_device *rbd_dev,
-				  struct rbd_snap *snap)
+static void __rbd_remove_snap_dev(struct rbd_snap *snap)
 {
 	list_del(&snap->node);
 	device_unregister(&snap->dev);
 }
 
-static int rbd_register_snap_dev(struct rbd_device *rbd_dev,
-				  struct rbd_snap *snap,
+static int rbd_register_snap_dev(struct rbd_snap *snap,
 				  struct device *parent)
 {
 	struct device *dev = &snap->dev;
@@ -2000,29 +2034,36 @@ static int rbd_register_snap_dev(struct rbd_device *rbd_dev,
 	return ret;
 }
 
-static int __rbd_add_snap_dev(struct rbd_device *rbd_dev,
-			      int i, const char *name,
-			      struct rbd_snap **snapp)
+static struct rbd_snap *__rbd_add_snap_dev(struct rbd_device *rbd_dev,
+					      int i, const char *name)
 {
+	struct rbd_snap *snap;
 	int ret;
-	struct rbd_snap *snap = kzalloc(sizeof(*snap), GFP_KERNEL);
+
+	snap = kzalloc(sizeof (*snap), GFP_KERNEL);
 	if (!snap)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
+
+	ret = -ENOMEM;
 	snap->name = kstrdup(name, GFP_KERNEL);
+	if (!snap->name)
+		goto err;
+
 	snap->size = rbd_dev->header.snap_sizes[i];
 	snap->id = rbd_dev->header.snapc->snaps[i];
 	if (device_is_registered(&rbd_dev->dev)) {
-		ret = rbd_register_snap_dev(rbd_dev, snap,
-					     &rbd_dev->dev);
+		ret = rbd_register_snap_dev(snap, &rbd_dev->dev);
 		if (ret < 0)
 			goto err;
 	}
-	*snapp = snap;
-	return 0;
+
+	return snap;
+
 err:
 	kfree(snap->name);
 	kfree(snap);
-	return ret;
+
+	return ERR_PTR(ret);
 }
 
 /*
@@ -2055,7 +2096,6 @@ static int __rbd_init_snaps_header(struct rbd_device *rbd_dev)
 	const char *name, *first_name;
 	int i = rbd_dev->header.total_snaps;
 	struct rbd_snap *snap, *old_snap = NULL;
-	int ret;
 	struct list_head *p, *n;
 
 	first_name = rbd_dev->header.snap_names;
@@ -2070,8 +2110,15 @@ static int __rbd_init_snaps_header(struct rbd_device *rbd_dev)
 			cur_id = rbd_dev->header.snapc->snaps[i - 1];
 
 		if (!i || old_snap->id < cur_id) {
-			/* old_snap->id was skipped, thus was removed */
-			__rbd_remove_snap_dev(rbd_dev, old_snap);
+			/*
+			 * old_snap->id was skipped, thus was
+			 * removed.  If this rbd_dev is mapped to
+			 * the removed snapshot, record that it no
+			 * longer exists, to prevent further I/O.
+			 */
+			if (rbd_dev->snap_id == old_snap->id)
+				rbd_dev->snap_exists = false;
+			__rbd_remove_snap_dev(old_snap);
 			continue;
 		}
 		if (old_snap->id == cur_id) {
@@ -2091,9 +2138,9 @@ static int __rbd_init_snaps_header(struct rbd_device *rbd_dev)
 			if (cur_id >= old_snap->id)
 				break;
 			/* a new snapshot */
-			ret = __rbd_add_snap_dev(rbd_dev, i - 1, name, &snap);
-			if (ret < 0)
-				return ret;
+			snap = __rbd_add_snap_dev(rbd_dev, i - 1, name);
+			if (IS_ERR(snap))
+				return PTR_ERR(snap);
 
 			/* note that we add it backward so using n and not p */
 			list_add(&snap->node, n);
@@ -2107,9 +2154,9 @@ static int __rbd_init_snaps_header(struct rbd_device *rbd_dev)
 			WARN_ON(1);
 			return -EINVAL;
 		}
-		ret = __rbd_add_snap_dev(rbd_dev, i - 1, name, &snap);
-		if (ret < 0)
-			return ret;
+		snap = __rbd_add_snap_dev(rbd_dev, i - 1, name);
+		if (IS_ERR(snap))
+			return PTR_ERR(snap);
 		list_add(&snap->node, &rbd_dev->snaps);
 	}
 
@@ -2129,14 +2176,13 @@ static int rbd_bus_add_dev(struct rbd_device *rbd_dev)
 	dev->type = &rbd_device_type;
 	dev->parent = &rbd_root_dev;
 	dev->release = rbd_dev_release;
-	dev_set_name(dev, "%d", rbd_dev->id);
+	dev_set_name(dev, "%d", rbd_dev->dev_id);
 	ret = device_register(dev);
 	if (ret < 0)
 		goto out;
 
 	list_for_each_entry(snap, &rbd_dev->snaps, node) {
-		ret = rbd_register_snap_dev(rbd_dev, snap,
-					     &rbd_dev->dev);
+		ret = rbd_register_snap_dev(snap, &rbd_dev->dev);
 		if (ret < 0)
 			break;
 	}
@@ -2155,12 +2201,9 @@ static int rbd_init_watch_dev(struct rbd_device *rbd_dev)
 	int ret, rc;
 
 	do {
-		ret = rbd_req_sync_watch(rbd_dev, rbd_dev->obj_md_name,
-					 rbd_dev->header.obj_version);
+		ret = rbd_req_sync_watch(rbd_dev);
 		if (ret == -ERANGE) {
-			mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
-			rc = __rbd_refresh_header(rbd_dev);
-			mutex_unlock(&ctl_mutex);
+			rc = rbd_refresh_header(rbd_dev, NULL);
 			if (rc < 0)
 				return rc;
 		}
@@ -2177,7 +2220,7 @@ static atomic64_t rbd_id_max = ATOMIC64_INIT(0);
  */
 static void rbd_id_get(struct rbd_device *rbd_dev)
 {
-	rbd_dev->id = atomic64_inc_return(&rbd_id_max);
+	rbd_dev->dev_id = atomic64_inc_return(&rbd_id_max);
 
 	spin_lock(&rbd_dev_list_lock);
 	list_add_tail(&rbd_dev->node, &rbd_dev_list);
@@ -2191,7 +2234,7 @@ static void rbd_id_get(struct rbd_device *rbd_dev)
 static void rbd_id_put(struct rbd_device *rbd_dev)
 {
 	struct list_head *tmp;
-	int rbd_id = rbd_dev->id;
+	int rbd_id = rbd_dev->dev_id;
 	int max_id;
 
 	BUG_ON(rbd_id < 1);
@@ -2282,19 +2325,58 @@ static inline size_t copy_token(const char **buf,
 }
 
 /*
- * This fills in the pool_name, obj, obj_len, snap_name, obj_len,
+ * Finds the next token in *buf, dynamically allocates a buffer big
+ * enough to hold a copy of it, and copies the token into the new
+ * buffer.  The copy is guaranteed to be terminated with '\0'.  Note
+ * that a duplicate buffer is created even for a zero-length token.
+ *
+ * Returns a pointer to the newly-allocated duplicate, or a null
+ * pointer if memory for the duplicate was not available.  If
+ * the lenp argument is a non-null pointer, the length of the token
+ * (not including the '\0') is returned in *lenp.
+ *
+ * If successful, the *buf pointer will be updated to point beyond
+ * the end of the found token.
+ *
+ * Note: uses GFP_KERNEL for allocation.
+ */
+static inline char *dup_token(const char **buf, size_t *lenp)
+{
+	char *dup;
+	size_t len;
+
+	len = next_token(buf);
+	dup = kmalloc(len + 1, GFP_KERNEL);
+	if (!dup)
+		return NULL;
+
+	memcpy(dup, *buf, len);
+	*(dup + len) = '\0';
+	*buf += len;
+
+	if (lenp)
+		*lenp = len;
+
+	return dup;
+}
+
+/*
+ * This fills in the pool_name, image_name, image_name_len, snap_name,
  * rbd_dev, rbd_md_name, and name fields of the given rbd_dev, based
  * on the list of monitor addresses and other options provided via
  * /sys/bus/rbd/add.
+ *
+ * Note: rbd_dev is assumed to have been initially zero-filled.
  */
 static int rbd_add_parse_args(struct rbd_device *rbd_dev,
 			      const char *buf,
 			      const char **mon_addrs,
 			      size_t *mon_addrs_size,
 			      char *options,
-			      size_t options_size)
+			     size_t options_size)
 {
-	size_t	len;
+	size_t len;
+	int ret;
 
 	/* The first four tokens are required */
 
@@ -2310,56 +2392,74 @@ static int rbd_add_parse_args(struct rbd_device *rbd_dev,
 	if (!len || len >= options_size)
 		return -EINVAL;
 
-	len = copy_token(&buf, rbd_dev->pool_name, sizeof (rbd_dev->pool_name));
-	if (!len || len >= sizeof (rbd_dev->pool_name))
-		return -EINVAL;
-
-	len = copy_token(&buf, rbd_dev->obj, sizeof (rbd_dev->obj));
-	if (!len || len >= sizeof (rbd_dev->obj))
-		return -EINVAL;
+	ret = -ENOMEM;
+	rbd_dev->pool_name = dup_token(&buf, NULL);
+	if (!rbd_dev->pool_name)
+		goto out_err;
 
-	/* We have the object length in hand, save it. */
+	rbd_dev->image_name = dup_token(&buf, &rbd_dev->image_name_len);
+	if (!rbd_dev->image_name)
+		goto out_err;
 
-	rbd_dev->obj_len = len;
+	/* Create the name of the header object */
 
-	BUILD_BUG_ON(RBD_MAX_MD_NAME_LEN
-				< RBD_MAX_OBJ_NAME_LEN + sizeof (RBD_SUFFIX));
-	sprintf(rbd_dev->obj_md_name, "%s%s", rbd_dev->obj, RBD_SUFFIX);
+	rbd_dev->header_name = kmalloc(rbd_dev->image_name_len
+						+ sizeof (RBD_SUFFIX),
+					GFP_KERNEL);
+	if (!rbd_dev->header_name)
+		goto out_err;
+	sprintf(rbd_dev->header_name, "%s%s", rbd_dev->image_name, RBD_SUFFIX);
 
 	/*
-	 * The snapshot name is optional, but it's an error if it's
-	 * too long.  If no snapshot is supplied, fill in the default.
+	 * The snapshot name is optional.  If none is is supplied,
+	 * we use the default value.
 	 */
-	len = copy_token(&buf, rbd_dev->snap_name, sizeof (rbd_dev->snap_name));
-	if (!len)
+	rbd_dev->snap_name = dup_token(&buf, &len);
+	if (!rbd_dev->snap_name)
+		goto out_err;
+	if (!len) {
+		/* Replace the empty name with the default */
+		kfree(rbd_dev->snap_name);
+		rbd_dev->snap_name
+			= kmalloc(sizeof (RBD_SNAP_HEAD_NAME), GFP_KERNEL);
+		if (!rbd_dev->snap_name)
+			goto out_err;
+
 		memcpy(rbd_dev->snap_name, RBD_SNAP_HEAD_NAME,
 			sizeof (RBD_SNAP_HEAD_NAME));
-	else if (len >= sizeof (rbd_dev->snap_name))
-		return -EINVAL;
+	}
 
 	return 0;
+
+out_err:
+	kfree(rbd_dev->header_name);
+	kfree(rbd_dev->image_name);
+	kfree(rbd_dev->pool_name);
+	rbd_dev->pool_name = NULL;
+
+	return ret;
 }
 
 static ssize_t rbd_add(struct bus_type *bus,
 		       const char *buf,
 		       size_t count)
 {
-	struct rbd_device *rbd_dev;
+	char *options;
+	struct rbd_device *rbd_dev = NULL;
 	const char *mon_addrs = NULL;
 	size_t mon_addrs_size = 0;
-	char *options = NULL;
 	struct ceph_osd_client *osdc;
 	int rc = -ENOMEM;
 
 	if (!try_module_get(THIS_MODULE))
 		return -ENODEV;
 
-	rbd_dev = kzalloc(sizeof(*rbd_dev), GFP_KERNEL);
-	if (!rbd_dev)
-		goto err_nomem;
 	options = kmalloc(count, GFP_KERNEL);
 	if (!options)
 		goto err_nomem;
+	rbd_dev = kzalloc(sizeof(*rbd_dev), GFP_KERNEL);
+	if (!rbd_dev)
+		goto err_nomem;
 
 	/* static rbd_device initialization */
 	spin_lock_init(&rbd_dev->lock);
@@ -2367,15 +2467,13 @@ static ssize_t rbd_add(struct bus_type *bus,
 	INIT_LIST_HEAD(&rbd_dev->snaps);
 	init_rwsem(&rbd_dev->header_rwsem);
 
-	init_rwsem(&rbd_dev->header_rwsem);
-
 	/* generate unique id: find highest unique id, add one */
 	rbd_id_get(rbd_dev);
 
 	/* Fill in the device name, now that we have its id. */
 	BUILD_BUG_ON(DEV_NAME_LEN
 			< sizeof (RBD_DRV_NAME) + MAX_INT_FORMAT_WIDTH);
-	sprintf(rbd_dev->name, "%s%d", RBD_DRV_NAME, rbd_dev->id);
+	sprintf(rbd_dev->name, "%s%d", RBD_DRV_NAME, rbd_dev->dev_id);
 
 	/* parse add command */
 	rc = rbd_add_parse_args(rbd_dev, buf, &mon_addrs, &mon_addrs_size,
@@ -2395,7 +2493,7 @@ static ssize_t rbd_add(struct bus_type *bus,
 	rc = ceph_pg_poolid_by_name(osdc->osdmap, rbd_dev->pool_name);
 	if (rc < 0)
 		goto err_out_client;
-	rbd_dev->poolid = rc;
+	rbd_dev->pool_id = rc;
 
 	/* register our block device */
 	rc = register_blkdev(0, rbd_dev->name);
@@ -2435,10 +2533,16 @@ err_out_blkdev:
 err_out_client:
 	rbd_put_client(rbd_dev);
 err_put_id:
+	if (rbd_dev->pool_name) {
+		kfree(rbd_dev->snap_name);
+		kfree(rbd_dev->header_name);
+		kfree(rbd_dev->image_name);
+		kfree(rbd_dev->pool_name);
+	}
 	rbd_id_put(rbd_dev);
 err_nomem:
-	kfree(options);
 	kfree(rbd_dev);
+	kfree(options);
 
 	dout("Error adding device %s\n", buf);
 	module_put(THIS_MODULE);
@@ -2446,7 +2550,7 @@ err_nomem:
 	return (ssize_t) rc;
 }
 
-static struct rbd_device *__rbd_get_dev(unsigned long id)
+static struct rbd_device *__rbd_get_dev(unsigned long dev_id)
 {
 	struct list_head *tmp;
 	struct rbd_device *rbd_dev;
@@ -2454,7 +2558,7 @@ static struct rbd_device *__rbd_get_dev(unsigned long id)
 	spin_lock(&rbd_dev_list_lock);
 	list_for_each(tmp, &rbd_dev_list) {
 		rbd_dev = list_entry(tmp, struct rbd_device, node);
-		if (rbd_dev->id == id) {
+		if (rbd_dev->dev_id == dev_id) {
 			spin_unlock(&rbd_dev_list_lock);
 			return rbd_dev;
 		}
@@ -2474,7 +2578,7 @@ static void rbd_dev_release(struct device *dev)
 						    rbd_dev->watch_request);
 	}
 	if (rbd_dev->watch_event)
-		rbd_req_sync_unwatch(rbd_dev, rbd_dev->obj_md_name);
+		rbd_req_sync_unwatch(rbd_dev);
 
 	rbd_put_client(rbd_dev);
 
@@ -2483,6 +2587,10 @@ static void rbd_dev_release(struct device *dev)
 	unregister_blkdev(rbd_dev->major, rbd_dev->name);
 
 	/* done with the id, and with the rbd_dev */
+	kfree(rbd_dev->snap_name);
+	kfree(rbd_dev->header_name);
+	kfree(rbd_dev->pool_name);
+	kfree(rbd_dev->image_name);
 	rbd_id_put(rbd_dev);
 	kfree(rbd_dev);
 
@@ -2544,7 +2652,7 @@ static ssize_t rbd_snap_add(struct device *dev,
 	if (ret < 0)
 		goto err_unlock;
 
-	ret = __rbd_refresh_header(rbd_dev);
+	ret = __rbd_refresh_header(rbd_dev, NULL);
 	if (ret < 0)
 		goto err_unlock;
 
@@ -2553,7 +2661,7 @@ static ssize_t rbd_snap_add(struct device *dev,
 	mutex_unlock(&ctl_mutex);
 
 	/* make a best effort, don't error if failed */
-	rbd_req_sync_notify(rbd_dev, rbd_dev->obj_md_name);
+	rbd_req_sync_notify(rbd_dev);
 
 	ret = count;
 	kfree(name);
diff --git a/drivers/block/rbd_types.h b/drivers/block/rbd_types.h
index 950708688f1..0924e9e41a6 100644
--- a/drivers/block/rbd_types.h
+++ b/drivers/block/rbd_types.h
@@ -31,7 +31,6 @@
 #define RBD_MIN_OBJ_ORDER       16
 #define RBD_MAX_OBJ_ORDER       30
 
-#define RBD_MAX_OBJ_NAME_LEN	96
 #define RBD_MAX_SEG_NAME_LEN	128
 
 #define RBD_COMP_NONE		0
diff --git a/drivers/block/umem.c b/drivers/block/umem.c
index 9a72277a31d..eb0d8216f55 100644
--- a/drivers/block/umem.c
+++ b/drivers/block/umem.c
@@ -513,42 +513,19 @@ static void process_page(unsigned long data)
 	}
 }
 
-struct mm_plug_cb {
-	struct blk_plug_cb cb;
-	struct cardinfo *card;
-};
-
-static void mm_unplug(struct blk_plug_cb *cb)
+static void mm_unplug(struct blk_plug_cb *cb, bool from_schedule)
 {
-	struct mm_plug_cb *mmcb = container_of(cb, struct mm_plug_cb, cb);
+	struct cardinfo *card = cb->data;
 
-	spin_lock_irq(&mmcb->card->lock);
-	activate(mmcb->card);
-	spin_unlock_irq(&mmcb->card->lock);
-	kfree(mmcb);
+	spin_lock_irq(&card->lock);
+	activate(card);
+	spin_unlock_irq(&card->lock);
+	kfree(cb);
 }
 
 static int mm_check_plugged(struct cardinfo *card)
 {
-	struct blk_plug *plug = current->plug;
-	struct mm_plug_cb *mmcb;
-
-	if (!plug)
-		return 0;
-
-	list_for_each_entry(mmcb, &plug->cb_list, cb.list) {
-		if (mmcb->cb.callback == mm_unplug && mmcb->card == card)
-			return 1;
-	}
-	/* Not currently on the callback list */
-	mmcb = kmalloc(sizeof(*mmcb), GFP_ATOMIC);
-	if (!mmcb)
-		return 0;
-
-	mmcb->card = card;
-	mmcb->cb.callback = mm_unplug;
-	list_add(&mmcb->cb.list, &plug->cb_list);
-	return 1;
+	return !!blk_check_plugged(mm_unplug, card, sizeof(struct blk_plug_cb));
 }
 
 static void mm_make_request(struct request_queue *q, struct bio *bio)
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 693187df760..c0bbeb47075 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -21,8 +21,6 @@ struct workqueue_struct *virtblk_wq;
 
 struct virtio_blk
 {
-	spinlock_t lock;
-
 	struct virtio_device *vdev;
 	struct virtqueue *vq;
 
@@ -65,7 +63,7 @@ static void blk_done(struct virtqueue *vq)
 	unsigned int len;
 	unsigned long flags;
 
-	spin_lock_irqsave(&vblk->lock, flags);
+	spin_lock_irqsave(vblk->disk->queue->queue_lock, flags);
 	while ((vbr = virtqueue_get_buf(vblk->vq, &len)) != NULL) {
 		int error;
 
@@ -99,7 +97,7 @@ static void blk_done(struct virtqueue *vq)
 	}
 	/* In case queue is stopped waiting for more buffers. */
 	blk_start_queue(vblk->disk->queue);
-	spin_unlock_irqrestore(&vblk->lock, flags);
+	spin_unlock_irqrestore(vblk->disk->queue->queue_lock, flags);
 }
 
 static bool do_req(struct request_queue *q, struct virtio_blk *vblk,
@@ -397,6 +395,83 @@ static int virtblk_name_format(char *prefix, int index, char *buf, int buflen)
 	return 0;
 }
 
+static int virtblk_get_cache_mode(struct virtio_device *vdev)
+{
+	u8 writeback;
+	int err;
+
+	err = virtio_config_val(vdev, VIRTIO_BLK_F_CONFIG_WCE,
+				offsetof(struct virtio_blk_config, wce),
+				&writeback);
+	if (err)
+		writeback = virtio_has_feature(vdev, VIRTIO_BLK_F_WCE);
+
+	return writeback;
+}
+
+static void virtblk_update_cache_mode(struct virtio_device *vdev)
+{
+	u8 writeback = virtblk_get_cache_mode(vdev);
+	struct virtio_blk *vblk = vdev->priv;
+
+	if (writeback)
+		blk_queue_flush(vblk->disk->queue, REQ_FLUSH);
+	else
+		blk_queue_flush(vblk->disk->queue, 0);
+
+	revalidate_disk(vblk->disk);
+}
+
+static const char *const virtblk_cache_types[] = {
+	"write through", "write back"
+};
+
+static ssize_t
+virtblk_cache_type_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	struct gendisk *disk = dev_to_disk(dev);
+	struct virtio_blk *vblk = disk->private_data;
+	struct virtio_device *vdev = vblk->vdev;
+	int i;
+	u8 writeback;
+
+	BUG_ON(!virtio_has_feature(vblk->vdev, VIRTIO_BLK_F_CONFIG_WCE));
+	for (i = ARRAY_SIZE(virtblk_cache_types); --i >= 0; )
+		if (sysfs_streq(buf, virtblk_cache_types[i]))
+			break;
+
+	if (i < 0)
+		return -EINVAL;
+
+	writeback = i;
+	vdev->config->set(vdev,
+			  offsetof(struct virtio_blk_config, wce),
+			  &writeback, sizeof(writeback));
+
+	virtblk_update_cache_mode(vdev);
+	return count;
+}
+
+static ssize_t
+virtblk_cache_type_show(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct gendisk *disk = dev_to_disk(dev);
+	struct virtio_blk *vblk = disk->private_data;
+	u8 writeback = virtblk_get_cache_mode(vblk->vdev);
+
+	BUG_ON(writeback >= ARRAY_SIZE(virtblk_cache_types));
+	return snprintf(buf, 40, "%s\n", virtblk_cache_types[writeback]);
+}
+
+static const struct device_attribute dev_attr_cache_type_ro =
+	__ATTR(cache_type, S_IRUGO,
+	       virtblk_cache_type_show, NULL);
+static const struct device_attribute dev_attr_cache_type_rw =
+	__ATTR(cache_type, S_IRUGO|S_IWUSR,
+	       virtblk_cache_type_show, virtblk_cache_type_store);
+
 static int __devinit virtblk_probe(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk;
@@ -431,7 +506,6 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
 		goto out_free_index;
 	}
 
-	spin_lock_init(&vblk->lock);
 	vblk->vdev = vdev;
 	vblk->sg_elems = sg_elems;
 	sg_init_table(vblk->sg, vblk->sg_elems);
@@ -456,7 +530,7 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
 		goto out_mempool;
 	}
 
-	q = vblk->disk->queue = blk_init_queue(do_virtblk_request, &vblk->lock);
+	q = vblk->disk->queue = blk_init_queue(do_virtblk_request, NULL);
 	if (!q) {
 		err = -ENOMEM;
 		goto out_put_disk;
@@ -474,8 +548,7 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
 	vblk->index = index;
 
 	/* configure queue flush support */
-	if (virtio_has_feature(vdev, VIRTIO_BLK_F_FLUSH))
-		blk_queue_flush(q, REQ_FLUSH);
+	virtblk_update_cache_mode(vdev);
 
 	/* If disk is read-only in the host, the guest should obey */
 	if (virtio_has_feature(vdev, VIRTIO_BLK_F_RO))
@@ -553,6 +626,14 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
 	if (err)
 		goto out_del_disk;
 
+	if (virtio_has_feature(vdev, VIRTIO_BLK_F_CONFIG_WCE))
+		err = device_create_file(disk_to_dev(vblk->disk),
+					 &dev_attr_cache_type_rw);
+	else
+		err = device_create_file(disk_to_dev(vblk->disk),
+					 &dev_attr_cache_type_ro);
+	if (err)
+		goto out_del_disk;
 	return 0;
 
 out_del_disk:
@@ -576,30 +657,20 @@ static void __devexit virtblk_remove(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk = vdev->priv;
 	int index = vblk->index;
-	struct virtblk_req *vbr;
-	unsigned long flags;
 
 	/* Prevent config work handler from accessing the device. */
 	mutex_lock(&vblk->config_lock);
 	vblk->config_enable = false;
 	mutex_unlock(&vblk->config_lock);
 
+	del_gendisk(vblk->disk);
+	blk_cleanup_queue(vblk->disk->queue);
+
 	/* Stop all the virtqueues. */
 	vdev->config->reset(vdev);
 
 	flush_work(&vblk->config_work);
 
-	del_gendisk(vblk->disk);
-
-	/* Abort requests dispatched to driver. */
-	spin_lock_irqsave(&vblk->lock, flags);
-	while ((vbr = virtqueue_detach_unused_buf(vblk->vq))) {
-		__blk_end_request_all(vbr->req, -EIO);
-		mempool_free(vbr, vblk->pool);
-	}
-	spin_unlock_irqrestore(&vblk->lock, flags);
-
-	blk_cleanup_queue(vblk->disk->queue);
 	put_disk(vblk->disk);
 	mempool_destroy(vblk->pool);
 	vdev->config->del_vqs(vdev);
@@ -655,7 +726,7 @@ static const struct virtio_device_id id_table[] = {
 static unsigned int features[] = {
 	VIRTIO_BLK_F_SEG_MAX, VIRTIO_BLK_F_SIZE_MAX, VIRTIO_BLK_F_GEOMETRY,
 	VIRTIO_BLK_F_RO, VIRTIO_BLK_F_BLK_SIZE, VIRTIO_BLK_F_SCSI,
-	VIRTIO_BLK_F_FLUSH, VIRTIO_BLK_F_TOPOLOGY
+	VIRTIO_BLK_F_WCE, VIRTIO_BLK_F_TOPOLOGY, VIRTIO_BLK_F_CONFIG_WCE
 };
 
 /*
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index e4fb3374dcd..2c2d2e5c159 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -888,9 +888,8 @@ static int setup_blkring(struct xenbus_device *dev,
 	if (err)
 		goto fail;
 
-	err = bind_evtchn_to_irqhandler(info->evtchn,
-					blkif_interrupt,
-					IRQF_SAMPLE_RANDOM, "blkif", info);
+	err = bind_evtchn_to_irqhandler(info->evtchn, blkif_interrupt, 0,
+					"blkif", info);
 	if (err <= 0) {
 		xenbus_dev_fatal(dev, err,
 				 "bind_evtchn_to_irqhandler failed");
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index 5ccf142ef0b..e9f203eadb1 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -81,6 +81,18 @@ config BT_HCIUART_LL
 
 	  Say Y here to compile support for HCILL protocol.
 
+config BT_HCIUART_3WIRE
+	bool "Three-wire UART (H5) protocol support"
+	depends on BT_HCIUART
+	help
+	  The HCI Three-wire UART Transport Layer makes it possible to
+	  user the Bluetooth HCI over a serial port interface. The HCI
+	  Three-wire UART Transport Layer assumes that the UART
+	  communication may have bit errors, overrun errors or burst
+	  errors and thereby making CTS/RTS lines unnecessary.
+
+	  Say Y here to compile support for Three-wire UART protocol.
+
 config BT_HCIBCM203X
 	tristate "HCI BCM203x USB driver"
 	depends on USB
diff --git a/drivers/bluetooth/Makefile b/drivers/bluetooth/Makefile
index f4460f4f4b7..4afae20df51 100644
--- a/drivers/bluetooth/Makefile
+++ b/drivers/bluetooth/Makefile
@@ -28,4 +28,5 @@ hci_uart-$(CONFIG_BT_HCIUART_H4)	+= hci_h4.o
 hci_uart-$(CONFIG_BT_HCIUART_BCSP)	+= hci_bcsp.o
 hci_uart-$(CONFIG_BT_HCIUART_LL)	+= hci_ll.o
 hci_uart-$(CONFIG_BT_HCIUART_ATH3K)	+= hci_ath.o
+hci_uart-$(CONFIG_BT_HCIUART_3WIRE)	+= hci_h5.o
 hci_uart-objs				:= $(hci_uart-y)
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 1fcd9238035..66c3a6770c4 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -231,12 +231,12 @@ static void bluecard_write_wakeup(bluecard_info_t *info)
 	}
 
 	do {
-		register unsigned int iobase = info->p_dev->resource[0]->start;
-		register unsigned int offset;
-		register unsigned char command;
-		register unsigned long ready_bit;
+		unsigned int iobase = info->p_dev->resource[0]->start;
+		unsigned int offset;
+		unsigned char command;
+		unsigned long ready_bit;
 		register struct sk_buff *skb;
-		register int len;
+		int len;
 
 		clear_bit(XMIT_WAKEUP, &(info->tx_state));
 
@@ -621,7 +621,6 @@ static int bluecard_hci_flush(struct hci_dev *hdev)
 static int bluecard_hci_open(struct hci_dev *hdev)
 {
 	bluecard_info_t *info = hci_get_drvdata(hdev);
-	unsigned int iobase = info->p_dev->resource[0]->start;
 
 	if (test_bit(CARD_HAS_PCCARD_ID, &(info->hw_state)))
 		bluecard_hci_set_baud_rate(hdev, DEFAULT_BAUD_RATE);
@@ -630,6 +629,8 @@ static int bluecard_hci_open(struct hci_dev *hdev)
 		return 0;
 
 	if (test_bit(CARD_HAS_PCCARD_ID, &(info->hw_state))) {
+		unsigned int iobase = info->p_dev->resource[0]->start;
+
 		/* Enable LED */
 		outb(0x08 | 0x20, iobase + 0x30);
 	}
@@ -641,7 +642,6 @@ static int bluecard_hci_open(struct hci_dev *hdev)
 static int bluecard_hci_close(struct hci_dev *hdev)
 {
 	bluecard_info_t *info = hci_get_drvdata(hdev);
-	unsigned int iobase = info->p_dev->resource[0]->start;
 
 	if (!test_and_clear_bit(HCI_RUNNING, &(hdev->flags)))
 		return 0;
@@ -649,6 +649,8 @@ static int bluecard_hci_close(struct hci_dev *hdev)
 	bluecard_hci_flush(hdev);
 
 	if (test_bit(CARD_HAS_PCCARD_ID, &(info->hw_state))) {
+		unsigned int iobase = info->p_dev->resource[0]->start;
+
 		/* Disable LED */
 		outb(0x00, iobase + 0x30);
 	}
diff --git a/drivers/bluetooth/bpa10x.c b/drivers/bluetooth/bpa10x.c
index 609861a53c2..29caaed2d71 100644
--- a/drivers/bluetooth/bpa10x.c
+++ b/drivers/bluetooth/bpa10x.c
@@ -470,7 +470,7 @@ static int bpa10x_probe(struct usb_interface *intf, const struct usb_device_id *
 	hdev->flush    = bpa10x_flush;
 	hdev->send     = bpa10x_send_frame;
 
-	set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+	set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
 	err = hci_register_dev(hdev);
 	if (err < 0) {
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 308c8599ab5..8925b6d672a 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -186,9 +186,9 @@ static void bt3c_write_wakeup(bt3c_info_t *info)
 		return;
 
 	do {
-		register unsigned int iobase = info->p_dev->resource[0]->start;
+		unsigned int iobase = info->p_dev->resource[0]->start;
 		register struct sk_buff *skb;
-		register int len;
+		int len;
 
 		if (!pcmcia_dev_present(info->p_dev))
 			break;
@@ -664,7 +664,7 @@ static int bt3c_check_config(struct pcmcia_device *p_dev, void *priv_data)
 {
 	int *try = priv_data;
 
-	if (try == 0)
+	if (!try)
 		p_dev->io_lines = 16;
 
 	if ((p_dev->resource[0]->end != 8) || (p_dev->resource[0]->start == 0))
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index dc304def840..3a4343b3bd6 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -47,10 +47,11 @@ EXPORT_SYMBOL_GPL(btmrvl_interrupt);
 bool btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb)
 {
 	struct hci_event_hdr *hdr = (void *) skb->data;
-	struct hci_ev_cmd_complete *ec;
-	u16 opcode, ocf, ogf;
 
 	if (hdr->evt == HCI_EV_CMD_COMPLETE) {
+		struct hci_ev_cmd_complete *ec;
+		u16 opcode, ocf, ogf;
+
 		ec = (void *) (skb->data + HCI_EVENT_HDR_SIZE);
 		opcode = __le16_to_cpu(ec->opcode);
 		ocf = hci_opcode_ocf(opcode);
@@ -64,7 +65,8 @@ bool btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb)
 		}
 
 		if (ogf == OGF) {
-			BT_DBG("vendor event skipped: ogf 0x%4.4x", ogf);
+			BT_DBG("vendor event skipped: ogf 0x%4.4x ocf 0x%4.4x",
+			       ogf, ocf);
 			kfree_skb(skb);
 			return false;
 		}
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 0cd61d9f07c..6a9e9717d3a 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -110,6 +110,9 @@ static const struct sdio_device_id btmrvl_sdio_ids[] = {
 	/* Marvell SD8787 Bluetooth device */
 	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x911A),
 			.driver_data = (unsigned long) &btmrvl_sdio_sd8787 },
+	/* Marvell SD8787 Bluetooth AMP device */
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x911B),
+			.driver_data = (unsigned long) &btmrvl_sdio_sd8787 },
 	/* Marvell SD8797 Bluetooth device */
 	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x912A),
 			.driver_data = (unsigned long) &btmrvl_sdio_sd8797 },
@@ -565,8 +568,9 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
 		if (type == HCI_EVENT_PKT) {
 			if (btmrvl_check_evtpkt(priv, skb))
 				hci_recv_frame(skb);
-		} else
+		} else {
 			hci_recv_frame(skb);
+		}
 
 		hdev->stat.byte_rx += buf_len;
 		break;
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index c4fc2f3fc32..21e803a6a28 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -140,9 +140,9 @@ static void btuart_write_wakeup(btuart_info_t *info)
 	}
 
 	do {
-		register unsigned int iobase = info->p_dev->resource[0]->start;
+		unsigned int iobase = info->p_dev->resource[0]->start;
 		register struct sk_buff *skb;
-		register int len;
+		int len;
 
 		clear_bit(XMIT_WAKEUP, &(info->tx_state));
 
@@ -593,7 +593,7 @@ static int btuart_check_config(struct pcmcia_device *p_dev, void *priv_data)
 {
 	int *try = priv_data;
 
-	if (try == 0)
+	if (!try)
 		p_dev->io_lines = 16;
 
 	if ((p_dev->resource[0]->end != 8) || (p_dev->resource[0]->start == 0))
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 83ebb241bfc..e2722141103 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -21,15 +21,7 @@
  *
  */
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/sched.h>
-#include <linux/errno.h>
-#include <linux/skbuff.h>
-
 #include <linux/usb.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -1028,7 +1020,7 @@ static int btusb_probe(struct usb_interface *intf,
 	data->isoc = usb_ifnum_to_if(data->udev, 1);
 
 	if (!reset)
-		set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+		set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
 	if (force_scofix || id->driver_info & BTUSB_WRONG_SCO_MTU) {
 		if (!disable_scofix)
@@ -1040,7 +1032,7 @@ static int btusb_probe(struct usb_interface *intf,
 
 	if (id->driver_info & BTUSB_DIGIANSWER) {
 		data->cmdreq_type = USB_TYPE_VENDOR;
-		set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+		set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 	}
 
 	if (id->driver_info & BTUSB_CSR) {
@@ -1048,7 +1040,7 @@ static int btusb_probe(struct usb_interface *intf,
 
 		/* Old firmware would otherwise execute USB reset */
 		if (le16_to_cpu(udev->descriptor.bcdDevice) < 0x117)
-			set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+			set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 	}
 
 	if (id->driver_info & BTUSB_SNIFFER) {
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 6e8d9618968..97a7784db4a 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -144,9 +144,9 @@ static void dtl1_write_wakeup(dtl1_info_t *info)
 	}
 
 	do {
-		register unsigned int iobase = info->p_dev->resource[0]->start;
+		unsigned int iobase = info->p_dev->resource[0]->start;
 		register struct sk_buff *skb;
-		register int len;
+		int len;
 
 		clear_bit(XMIT_WAKEUP, &(info->tx_state));
 
@@ -586,29 +586,31 @@ static int dtl1_confcheck(struct pcmcia_device *p_dev, void *priv_data)
 static int dtl1_config(struct pcmcia_device *link)
 {
 	dtl1_info_t *info = link->priv;
-	int i;
+	int ret;
 
 	/* Look for a generic full-sized window */
 	link->resource[0]->end = 8;
-	if (pcmcia_loop_config(link, dtl1_confcheck, NULL) < 0)
+	ret = pcmcia_loop_config(link, dtl1_confcheck, NULL);
+	if (ret)
 		goto failed;
 
-	i = pcmcia_request_irq(link, dtl1_interrupt);
-	if (i != 0)
+	ret = pcmcia_request_irq(link, dtl1_interrupt);
+	if (ret)
 		goto failed;
 
-	i = pcmcia_enable_device(link);
-	if (i != 0)
+	ret = pcmcia_enable_device(link);
+	if (ret)
 		goto failed;
 
-	if (dtl1_open(info) != 0)
+	ret = dtl1_open(info);
+	if (ret)
 		goto failed;
 
 	return 0;
 
 failed:
 	dtl1_detach(link);
-	return -ENODEV;
+	return ret;
 }
 
 static const struct pcmcia_device_id dtl1_ids[] = {
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 661a8dc4d2f..57e502e0608 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -552,7 +552,7 @@ static u16 bscp_get_crc(struct bcsp_struct *bcsp)
 static int bcsp_recv(struct hci_uart *hu, void *data, int count)
 {
 	struct bcsp_struct *bcsp = hu->priv;
-	register unsigned char *ptr;
+	unsigned char *ptr;
 
 	BT_DBG("hu %p count %d rx_state %d rx_count %ld", 
 		hu, count, bcsp->rx_state, bcsp->rx_count);
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index 748329468d2..c60623f206d 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -126,7 +126,7 @@ static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 
 static inline int h4_check_data_len(struct h4_struct *h4, int len)
 {
-	register int room = skb_tailroom(h4->rx_skb);
+	int room = skb_tailroom(h4->rx_skb);
 
 	BT_DBG("len %d room %d", len, room);
 
diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
new file mode 100644
index 00000000000..b6154d5a07a
--- /dev/null
+++ b/drivers/bluetooth/hci_h5.c
@@ -0,0 +1,747 @@
+/*
+ *
+ *  Bluetooth HCI Three-wire UART driver
+ *
+ *  Copyright (C) 2012  Intel Corporation
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/skbuff.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "hci_uart.h"
+
+#define HCI_3WIRE_ACK_PKT	0
+#define HCI_3WIRE_LINK_PKT	15
+
+/* Sliding window size */
+#define H5_TX_WIN_MAX		4
+
+#define H5_ACK_TIMEOUT	msecs_to_jiffies(250)
+#define H5_SYNC_TIMEOUT	msecs_to_jiffies(100)
+
+/*
+ * Maximum Three-wire packet:
+ *     4 byte header + max value for 12-bit length + 2 bytes for CRC
+ */
+#define H5_MAX_LEN (4 + 0xfff + 2)
+
+/* Convenience macros for reading Three-wire header values */
+#define H5_HDR_SEQ(hdr)		((hdr)[0] & 0x07)
+#define H5_HDR_ACK(hdr)		(((hdr)[0] >> 3) & 0x07)
+#define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
+#define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
+#define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
+#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0xff) + ((hdr)[2] << 4))
+
+#define SLIP_DELIMITER	0xc0
+#define SLIP_ESC	0xdb
+#define SLIP_ESC_DELIM	0xdc
+#define SLIP_ESC_ESC	0xdd
+
+/* H5 state flags */
+enum {
+	H5_RX_ESC,	/* SLIP escape mode */
+	H5_TX_ACK_REQ,	/* Pending ack to send */
+};
+
+struct h5 {
+	struct sk_buff_head	unack;		/* Unack'ed packets queue */
+	struct sk_buff_head	rel;		/* Reliable packets queue */
+	struct sk_buff_head	unrel;		/* Unreliable packets queue */
+
+	unsigned long		flags;
+
+	struct sk_buff		*rx_skb;	/* Receive buffer */
+	size_t			rx_pending;	/* Expecting more bytes */
+	u8			rx_ack;		/* Last ack number received */
+
+	int			(*rx_func) (struct hci_uart *hu, u8 c);
+
+	struct timer_list	timer;		/* Retransmission timer */
+
+	u8			tx_seq;		/* Next seq number to send */
+	u8			tx_ack;		/* Next ack number to send */
+	u8			tx_win;		/* Sliding window size */
+
+	enum {
+		H5_UNINITIALIZED,
+		H5_INITIALIZED,
+		H5_ACTIVE,
+	} state;
+
+	enum {
+		H5_AWAKE,
+		H5_SLEEPING,
+		H5_WAKING_UP,
+	} sleep;
+};
+
+static void h5_reset_rx(struct h5 *h5);
+
+static void h5_link_control(struct hci_uart *hu, const void *data, size_t len)
+{
+	struct h5 *h5 = hu->priv;
+	struct sk_buff *nskb;
+
+	nskb = alloc_skb(3, GFP_ATOMIC);
+	if (!nskb)
+		return;
+
+	bt_cb(nskb)->pkt_type = HCI_3WIRE_LINK_PKT;
+
+	memcpy(skb_put(nskb, len), data, len);
+
+	skb_queue_tail(&h5->unrel, nskb);
+}
+
+static u8 h5_cfg_field(struct h5 *h5)
+{
+	u8 field = 0;
+
+	/* Sliding window size (first 3 bits) */
+	field |= (h5->tx_win & 7);
+
+	return field;
+}
+
+static void h5_timed_event(unsigned long arg)
+{
+	const unsigned char sync_req[] = { 0x01, 0x7e };
+	unsigned char conf_req[] = { 0x03, 0xfc, 0x01 };
+	struct hci_uart *hu = (struct hci_uart *) arg;
+	struct h5 *h5 = hu->priv;
+	struct sk_buff *skb;
+	unsigned long flags;
+
+	BT_DBG("%s", hu->hdev->name);
+
+	if (h5->state == H5_UNINITIALIZED)
+		h5_link_control(hu, sync_req, sizeof(sync_req));
+
+	if (h5->state == H5_INITIALIZED) {
+		conf_req[2] = h5_cfg_field(h5);
+		h5_link_control(hu, conf_req, sizeof(conf_req));
+	}
+
+	if (h5->state != H5_ACTIVE) {
+		mod_timer(&h5->timer, jiffies + H5_SYNC_TIMEOUT);
+		goto wakeup;
+	}
+
+	if (h5->sleep != H5_AWAKE) {
+		h5->sleep = H5_SLEEPING;
+		goto wakeup;
+	}
+
+	BT_DBG("hu %p retransmitting %u pkts", hu, h5->unack.qlen);
+
+	spin_lock_irqsave_nested(&h5->unack.lock, flags, SINGLE_DEPTH_NESTING);
+
+	while ((skb = __skb_dequeue_tail(&h5->unack)) != NULL) {
+		h5->tx_seq = (h5->tx_seq - 1) & 0x07;
+		skb_queue_head(&h5->rel, skb);
+	}
+
+	spin_unlock_irqrestore(&h5->unack.lock, flags);
+
+wakeup:
+	hci_uart_tx_wakeup(hu);
+}
+
+static int h5_open(struct hci_uart *hu)
+{
+	struct h5 *h5;
+	const unsigned char sync[] = { 0x01, 0x7e };
+
+	BT_DBG("hu %p", hu);
+
+	h5 = kzalloc(sizeof(*h5), GFP_KERNEL);
+	if (!h5)
+		return -ENOMEM;
+
+	hu->priv = h5;
+
+	skb_queue_head_init(&h5->unack);
+	skb_queue_head_init(&h5->rel);
+	skb_queue_head_init(&h5->unrel);
+
+	h5_reset_rx(h5);
+
+	init_timer(&h5->timer);
+	h5->timer.function = h5_timed_event;
+	h5->timer.data = (unsigned long) hu;
+
+	h5->tx_win = H5_TX_WIN_MAX;
+
+	set_bit(HCI_UART_INIT_PENDING, &hu->hdev_flags);
+
+	/* Send initial sync request */
+	h5_link_control(hu, sync, sizeof(sync));
+	mod_timer(&h5->timer, jiffies + H5_SYNC_TIMEOUT);
+
+	return 0;
+}
+
+static int h5_close(struct hci_uart *hu)
+{
+	struct h5 *h5 = hu->priv;
+
+	skb_queue_purge(&h5->unack);
+	skb_queue_purge(&h5->rel);
+	skb_queue_purge(&h5->unrel);
+
+	del_timer(&h5->timer);
+
+	kfree(h5);
+
+	return 0;
+}
+
+static void h5_pkt_cull(struct h5 *h5)
+{
+	struct sk_buff *skb, *tmp;
+	unsigned long flags;
+	int i, to_remove;
+	u8 seq;
+
+	spin_lock_irqsave(&h5->unack.lock, flags);
+
+	to_remove = skb_queue_len(&h5->unack);
+	if (to_remove == 0)
+		goto unlock;
+
+	seq = h5->tx_seq;
+
+	while (to_remove > 0) {
+		if (h5->rx_ack == seq)
+			break;
+
+		to_remove--;
+		seq = (seq - 1) % 8;
+	}
+
+	if (seq != h5->rx_ack)
+		BT_ERR("Controller acked invalid packet");
+
+	i = 0;
+	skb_queue_walk_safe(&h5->unack, skb, tmp) {
+		if (i++ >= to_remove)
+			break;
+
+		__skb_unlink(skb, &h5->unack);
+		kfree_skb(skb);
+	}
+
+	if (skb_queue_empty(&h5->unack))
+		del_timer(&h5->timer);
+
+unlock:
+	spin_unlock_irqrestore(&h5->unack.lock, flags);
+}
+
+static void h5_handle_internal_rx(struct hci_uart *hu)
+{
+	struct h5 *h5 = hu->priv;
+	const unsigned char sync_req[] = { 0x01, 0x7e };
+	const unsigned char sync_rsp[] = { 0x02, 0x7d };
+	unsigned char conf_req[] = { 0x03, 0xfc, 0x01 };
+	const unsigned char conf_rsp[] = { 0x04, 0x7b };
+	const unsigned char wakeup_req[] = { 0x05, 0xfa };
+	const unsigned char woken_req[] = { 0x06, 0xf9 };
+	const unsigned char sleep_req[] = { 0x07, 0x78 };
+	const unsigned char *hdr = h5->rx_skb->data;
+	const unsigned char *data = &h5->rx_skb->data[4];
+
+	BT_DBG("%s", hu->hdev->name);
+
+	if (H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT)
+		return;
+
+	if (H5_HDR_LEN(hdr) < 2)
+		return;
+
+	conf_req[2] = h5_cfg_field(h5);
+
+	if (memcmp(data, sync_req, 2) == 0) {
+		h5_link_control(hu, sync_rsp, 2);
+	} else if (memcmp(data, sync_rsp, 2) == 0) {
+		h5->state = H5_INITIALIZED;
+		h5_link_control(hu, conf_req, 3);
+	} else if (memcmp(data, conf_req, 2) == 0) {
+		h5_link_control(hu, conf_rsp, 2);
+		h5_link_control(hu, conf_req, 3);
+	} else if (memcmp(data, conf_rsp, 2) == 0) {
+		if (H5_HDR_LEN(hdr) > 2)
+			h5->tx_win = (data[2] & 7);
+		BT_DBG("Three-wire init complete. tx_win %u", h5->tx_win);
+		h5->state = H5_ACTIVE;
+		hci_uart_init_ready(hu);
+		return;
+	} else if (memcmp(data, sleep_req, 2) == 0) {
+		BT_DBG("Peer went to sleep");
+		h5->sleep = H5_SLEEPING;
+		return;
+	} else if (memcmp(data, woken_req, 2) == 0) {
+		BT_DBG("Peer woke up");
+		h5->sleep = H5_AWAKE;
+	} else if (memcmp(data, wakeup_req, 2) == 0) {
+		BT_DBG("Peer requested wakeup");
+		h5_link_control(hu, woken_req, 2);
+		h5->sleep = H5_AWAKE;
+	} else {
+		BT_DBG("Link Control: 0x%02hhx 0x%02hhx", data[0], data[1]);
+		return;
+	}
+
+	hci_uart_tx_wakeup(hu);
+}
+
+static void h5_complete_rx_pkt(struct hci_uart *hu)
+{
+	struct h5 *h5 = hu->priv;
+	const unsigned char *hdr = h5->rx_skb->data;
+
+	if (H5_HDR_RELIABLE(hdr)) {
+		h5->tx_ack = (h5->tx_ack + 1) % 8;
+		set_bit(H5_TX_ACK_REQ, &h5->flags);
+		hci_uart_tx_wakeup(hu);
+	}
+
+	h5->rx_ack = H5_HDR_ACK(hdr);
+
+	h5_pkt_cull(h5);
+
+	switch (H5_HDR_PKT_TYPE(hdr)) {
+	case HCI_EVENT_PKT:
+	case HCI_ACLDATA_PKT:
+	case HCI_SCODATA_PKT:
+		bt_cb(h5->rx_skb)->pkt_type = H5_HDR_PKT_TYPE(hdr);
+
+		/* Remove Three-wire header */
+		skb_pull(h5->rx_skb, 4);
+
+		hci_recv_frame(h5->rx_skb);
+		h5->rx_skb = NULL;
+
+		break;
+
+	default:
+		h5_handle_internal_rx(hu);
+		break;
+	}
+
+	h5_reset_rx(h5);
+}
+
+static int h5_rx_crc(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+
+	h5_complete_rx_pkt(hu);
+	h5_reset_rx(h5);
+
+	return 0;
+}
+
+static int h5_rx_payload(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+	const unsigned char *hdr = h5->rx_skb->data;
+
+	if (H5_HDR_CRC(hdr)) {
+		h5->rx_func = h5_rx_crc;
+		h5->rx_pending = 2;
+	} else {
+		h5_complete_rx_pkt(hu);
+		h5_reset_rx(h5);
+	}
+
+	return 0;
+}
+
+static int h5_rx_3wire_hdr(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+	const unsigned char *hdr = h5->rx_skb->data;
+
+	BT_DBG("%s rx: seq %u ack %u crc %u rel %u type %u len %u",
+	       hu->hdev->name, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
+	       H5_HDR_CRC(hdr), H5_HDR_RELIABLE(hdr), H5_HDR_PKT_TYPE(hdr),
+	       H5_HDR_LEN(hdr));
+
+	if (((hdr[0] + hdr[1] + hdr[2] + hdr[3]) & 0xff) != 0xff) {
+		BT_ERR("Invalid header checksum");
+		h5_reset_rx(h5);
+		return 0;
+	}
+
+	if (H5_HDR_RELIABLE(hdr) && H5_HDR_SEQ(hdr) != h5->tx_ack) {
+		BT_ERR("Out-of-order packet arrived (%u != %u)",
+		       H5_HDR_SEQ(hdr), h5->tx_ack);
+		h5_reset_rx(h5);
+		return 0;
+	}
+
+	if (h5->state != H5_ACTIVE &&
+	    H5_HDR_PKT_TYPE(hdr) != HCI_3WIRE_LINK_PKT) {
+		BT_ERR("Non-link packet received in non-active state");
+		h5_reset_rx(h5);
+	}
+
+	h5->rx_func = h5_rx_payload;
+	h5->rx_pending = H5_HDR_LEN(hdr);
+
+	return 0;
+}
+
+static int h5_rx_pkt_start(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+
+	if (c == SLIP_DELIMITER)
+		return 1;
+
+	h5->rx_func = h5_rx_3wire_hdr;
+	h5->rx_pending = 4;
+
+	h5->rx_skb = bt_skb_alloc(H5_MAX_LEN, GFP_ATOMIC);
+	if (!h5->rx_skb) {
+		BT_ERR("Can't allocate mem for new packet");
+		h5_reset_rx(h5);
+		return -ENOMEM;
+	}
+
+	h5->rx_skb->dev = (void *) hu->hdev;
+
+	return 0;
+}
+
+static int h5_rx_delimiter(struct hci_uart *hu, unsigned char c)
+{
+	struct h5 *h5 = hu->priv;
+
+	if (c == SLIP_DELIMITER)
+		h5->rx_func = h5_rx_pkt_start;
+
+	return 1;
+}
+
+static void h5_unslip_one_byte(struct h5 *h5, unsigned char c)
+{
+	const u8 delim = SLIP_DELIMITER, esc = SLIP_ESC;
+	const u8 *byte = &c;
+
+	if (!test_bit(H5_RX_ESC, &h5->flags) && c == SLIP_ESC) {
+		set_bit(H5_RX_ESC, &h5->flags);
+		return;
+	}
+
+	if (test_and_clear_bit(H5_RX_ESC, &h5->flags)) {
+		switch (c) {
+		case SLIP_ESC_DELIM:
+			byte = &delim;
+			break;
+		case SLIP_ESC_ESC:
+			byte = &esc;
+			break;
+		default:
+			BT_ERR("Invalid esc byte 0x%02hhx", c);
+			h5_reset_rx(h5);
+			return;
+		}
+	}
+
+	memcpy(skb_put(h5->rx_skb, 1), byte, 1);
+	h5->rx_pending--;
+
+	BT_DBG("unsliped 0x%02hhx, rx_pending %zu", *byte, h5->rx_pending);
+}
+
+static void h5_reset_rx(struct h5 *h5)
+{
+	if (h5->rx_skb) {
+		kfree_skb(h5->rx_skb);
+		h5->rx_skb = NULL;
+	}
+
+	h5->rx_func = h5_rx_delimiter;
+	h5->rx_pending = 0;
+	clear_bit(H5_RX_ESC, &h5->flags);
+}
+
+static int h5_recv(struct hci_uart *hu, void *data, int count)
+{
+	struct h5 *h5 = hu->priv;
+	unsigned char *ptr = data;
+
+	BT_DBG("%s pending %zu count %d", hu->hdev->name, h5->rx_pending,
+	       count);
+
+	while (count > 0) {
+		int processed;
+
+		if (h5->rx_pending > 0) {
+			if (*ptr == SLIP_DELIMITER) {
+				BT_ERR("Too short H5 packet");
+				h5_reset_rx(h5);
+				continue;
+			}
+
+			h5_unslip_one_byte(h5, *ptr);
+
+			ptr++; count--;
+			continue;
+		}
+
+		processed = h5->rx_func(hu, *ptr);
+		if (processed < 0)
+			return processed;
+
+		ptr += processed;
+		count -= processed;
+	}
+
+	return 0;
+}
+
+static int h5_enqueue(struct hci_uart *hu, struct sk_buff *skb)
+{
+	struct h5 *h5 = hu->priv;
+
+	if (skb->len > 0xfff) {
+		BT_ERR("Packet too long (%u bytes)", skb->len);
+		kfree_skb(skb);
+		return 0;
+	}
+
+	if (h5->state != H5_ACTIVE) {
+		BT_ERR("Ignoring HCI data in non-active state");
+		kfree_skb(skb);
+		return 0;
+	}
+
+	switch (bt_cb(skb)->pkt_type) {
+	case HCI_ACLDATA_PKT:
+	case HCI_COMMAND_PKT:
+		skb_queue_tail(&h5->rel, skb);
+		break;
+
+	case HCI_SCODATA_PKT:
+		skb_queue_tail(&h5->unrel, skb);
+		break;
+
+	default:
+		BT_ERR("Unknown packet type %u", bt_cb(skb)->pkt_type);
+		kfree_skb(skb);
+		break;
+	}
+
+	return 0;
+}
+
+static void h5_slip_delim(struct sk_buff *skb)
+{
+	const char delim = SLIP_DELIMITER;
+
+	memcpy(skb_put(skb, 1), &delim, 1);
+}
+
+static void h5_slip_one_byte(struct sk_buff *skb, u8 c)
+{
+	const char esc_delim[2] = { SLIP_ESC, SLIP_ESC_DELIM };
+	const char esc_esc[2] = { SLIP_ESC, SLIP_ESC_ESC };
+
+	switch (c) {
+	case SLIP_DELIMITER:
+		memcpy(skb_put(skb, 2), &esc_delim, 2);
+		break;
+	case SLIP_ESC:
+		memcpy(skb_put(skb, 2), &esc_esc, 2);
+		break;
+	default:
+		memcpy(skb_put(skb, 1), &c, 1);
+	}
+}
+
+static bool valid_packet_type(u8 type)
+{
+	switch (type) {
+	case HCI_ACLDATA_PKT:
+	case HCI_COMMAND_PKT:
+	case HCI_SCODATA_PKT:
+	case HCI_3WIRE_LINK_PKT:
+	case HCI_3WIRE_ACK_PKT:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct sk_buff *h5_prepare_pkt(struct hci_uart *hu, u8 pkt_type,
+				      const u8 *data, size_t len)
+{
+	struct h5 *h5 = hu->priv;
+	struct sk_buff *nskb;
+	u8 hdr[4];
+	int i;
+
+	if (!valid_packet_type(pkt_type)) {
+		BT_ERR("Unknown packet type %u", pkt_type);
+		return NULL;
+	}
+
+	/*
+	 * Max len of packet: (original len + 4 (H5 hdr) + 2 (crc)) * 2
+	 * (because bytes 0xc0 and 0xdb are escaped, worst case is when
+	 * the packet is all made of 0xc0 and 0xdb) + 2 (0xc0
+	 * delimiters at start and end).
+	 */
+	nskb = alloc_skb((len + 6) * 2 + 2, GFP_ATOMIC);
+	if (!nskb)
+		return NULL;
+
+	bt_cb(nskb)->pkt_type = pkt_type;
+
+	h5_slip_delim(nskb);
+
+	hdr[0] = h5->tx_ack << 3;
+	clear_bit(H5_TX_ACK_REQ, &h5->flags);
+
+	/* Reliable packet? */
+	if (pkt_type == HCI_ACLDATA_PKT || pkt_type == HCI_COMMAND_PKT) {
+		hdr[0] |= 1 << 7;
+		hdr[0] |= h5->tx_seq;
+		h5->tx_seq = (h5->tx_seq + 1) % 8;
+	}
+
+	hdr[1] = pkt_type | ((len & 0x0f) << 4);
+	hdr[2] = len >> 4;
+	hdr[3] = ~((hdr[0] + hdr[1] + hdr[2]) & 0xff);
+
+	BT_DBG("%s tx: seq %u ack %u crc %u rel %u type %u len %u",
+	       hu->hdev->name, H5_HDR_SEQ(hdr), H5_HDR_ACK(hdr),
+	       H5_HDR_CRC(hdr), H5_HDR_RELIABLE(hdr), H5_HDR_PKT_TYPE(hdr),
+	       H5_HDR_LEN(hdr));
+
+	for (i = 0; i < 4; i++)
+		h5_slip_one_byte(nskb, hdr[i]);
+
+	for (i = 0; i < len; i++)
+		h5_slip_one_byte(nskb, data[i]);
+
+	h5_slip_delim(nskb);
+
+	return nskb;
+}
+
+static struct sk_buff *h5_dequeue(struct hci_uart *hu)
+{
+	struct h5 *h5 = hu->priv;
+	unsigned long flags;
+	struct sk_buff *skb, *nskb;
+
+	if (h5->sleep != H5_AWAKE) {
+		const unsigned char wakeup_req[] = { 0x05, 0xfa };
+
+		if (h5->sleep == H5_WAKING_UP)
+			return NULL;
+
+		h5->sleep = H5_WAKING_UP;
+		BT_DBG("Sending wakeup request");
+
+		mod_timer(&h5->timer, jiffies + HZ / 100);
+		return h5_prepare_pkt(hu, HCI_3WIRE_LINK_PKT, wakeup_req, 2);
+	}
+
+	if ((skb = skb_dequeue(&h5->unrel)) != NULL) {
+		nskb = h5_prepare_pkt(hu, bt_cb(skb)->pkt_type,
+				      skb->data, skb->len);
+		if (nskb) {
+			kfree_skb(skb);
+			return nskb;
+		}
+
+		skb_queue_head(&h5->unrel, skb);
+		BT_ERR("Could not dequeue pkt because alloc_skb failed");
+	}
+
+	spin_lock_irqsave_nested(&h5->unack.lock, flags, SINGLE_DEPTH_NESTING);
+
+	if (h5->unack.qlen >= h5->tx_win)
+		goto unlock;
+
+	if ((skb = skb_dequeue(&h5->rel)) != NULL) {
+		nskb = h5_prepare_pkt(hu, bt_cb(skb)->pkt_type,
+				      skb->data, skb->len);
+		if (nskb) {
+			__skb_queue_tail(&h5->unack, skb);
+			mod_timer(&h5->timer, jiffies + H5_ACK_TIMEOUT);
+			spin_unlock_irqrestore(&h5->unack.lock, flags);
+			return nskb;
+		}
+
+		skb_queue_head(&h5->rel, skb);
+		BT_ERR("Could not dequeue pkt because alloc_skb failed");
+	}
+
+unlock:
+	spin_unlock_irqrestore(&h5->unack.lock, flags);
+
+	if (test_bit(H5_TX_ACK_REQ, &h5->flags))
+		return h5_prepare_pkt(hu, HCI_3WIRE_ACK_PKT, NULL, 0);
+
+	return NULL;
+}
+
+static int h5_flush(struct hci_uart *hu)
+{
+	BT_DBG("hu %p", hu);
+	return 0;
+}
+
+static struct hci_uart_proto h5p = {
+	.id		= HCI_UART_3WIRE,
+	.open		= h5_open,
+	.close		= h5_close,
+	.recv		= h5_recv,
+	.enqueue	= h5_enqueue,
+	.dequeue	= h5_dequeue,
+	.flush		= h5_flush,
+};
+
+int __init h5_init(void)
+{
+	int err = hci_uart_register_proto(&h5p);
+
+	if (!err)
+		BT_INFO("HCI Three-wire UART (H5) protocol initialized");
+	else
+		BT_ERR("HCI Three-wire UART (H5) protocol init failed");
+
+	return err;
+}
+
+int __exit h5_deinit(void)
+{
+	return hci_uart_unregister_proto(&h5p);
+}
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index e564579a611..74e0966b3ea 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -156,6 +156,35 @@ restart:
 	return 0;
 }
 
+static void hci_uart_init_work(struct work_struct *work)
+{
+	struct hci_uart *hu = container_of(work, struct hci_uart, init_ready);
+	int err;
+
+	if (!test_and_clear_bit(HCI_UART_INIT_PENDING, &hu->hdev_flags))
+		return;
+
+	err = hci_register_dev(hu->hdev);
+	if (err < 0) {
+		BT_ERR("Can't register HCI device");
+		hci_free_dev(hu->hdev);
+		hu->hdev = NULL;
+		hu->proto->close(hu);
+	}
+
+	set_bit(HCI_UART_REGISTERED, &hu->flags);
+}
+
+int hci_uart_init_ready(struct hci_uart *hu)
+{
+	if (!test_bit(HCI_UART_INIT_PENDING, &hu->hdev_flags))
+		return -EALREADY;
+
+	schedule_work(&hu->init_ready);
+
+	return 0;
+}
+
 /* ------- Interface to HCI layer ------ */
 /* Initialize device */
 static int hci_uart_open(struct hci_dev *hdev)
@@ -264,6 +293,8 @@ static int hci_uart_tty_open(struct tty_struct *tty)
 	hu->tty = tty;
 	tty->receive_room = 65536;
 
+	INIT_WORK(&hu->init_ready, hci_uart_init_work);
+
 	spin_lock_init(&hu->rx_lock);
 
 	/* Flush any pending characters in the driver and line discipline. */
@@ -286,28 +317,30 @@ static int hci_uart_tty_open(struct tty_struct *tty)
 static void hci_uart_tty_close(struct tty_struct *tty)
 {
 	struct hci_uart *hu = (void *)tty->disc_data;
+	struct hci_dev *hdev;
 
 	BT_DBG("tty %p", tty);
 
 	/* Detach from the tty */
 	tty->disc_data = NULL;
 
-	if (hu) {
-		struct hci_dev *hdev = hu->hdev;
+	if (!hu)
+		return;
 
-		if (hdev)
-			hci_uart_close(hdev);
+	hdev = hu->hdev;
+	if (hdev)
+		hci_uart_close(hdev);
 
-		if (test_and_clear_bit(HCI_UART_PROTO_SET, &hu->flags)) {
-			if (hdev) {
+	if (test_and_clear_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+		if (hdev) {
+			if (test_bit(HCI_UART_REGISTERED, &hu->flags))
 				hci_unregister_dev(hdev);
-				hci_free_dev(hdev);
-			}
-			hu->proto->close(hu);
+			hci_free_dev(hdev);
 		}
-
-		kfree(hu);
+		hu->proto->close(hu);
 	}
+
+	kfree(hu);
 }
 
 /* hci_uart_tty_wakeup()
@@ -394,19 +427,24 @@ static int hci_uart_register_dev(struct hci_uart *hu)
 		set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
 
 	if (!test_bit(HCI_UART_RESET_ON_INIT, &hu->hdev_flags))
-		set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+		set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
 	if (test_bit(HCI_UART_CREATE_AMP, &hu->hdev_flags))
 		hdev->dev_type = HCI_AMP;
 	else
 		hdev->dev_type = HCI_BREDR;
 
+	if (test_bit(HCI_UART_INIT_PENDING, &hu->hdev_flags))
+		return 0;
+
 	if (hci_register_dev(hdev) < 0) {
 		BT_ERR("Can't register HCI device");
 		hci_free_dev(hdev);
 		return -ENODEV;
 	}
 
+	set_bit(HCI_UART_REGISTERED, &hu->flags);
+
 	return 0;
 }
 
@@ -558,6 +596,9 @@ static int __init hci_uart_init(void)
 #ifdef CONFIG_BT_HCIUART_ATH3K
 	ath_init();
 #endif
+#ifdef CONFIG_BT_HCIUART_3WIRE
+	h5_init();
+#endif
 
 	return 0;
 }
@@ -578,6 +619,9 @@ static void __exit hci_uart_exit(void)
 #ifdef CONFIG_BT_HCIUART_ATH3K
 	ath_deinit();
 #endif
+#ifdef CONFIG_BT_HCIUART_3WIRE
+	h5_deinit();
+#endif
 
 	/* Release tty registration of line discipline */
 	if ((err = tty_unregister_ldisc(N_HCI)))
diff --git a/drivers/bluetooth/hci_ll.c b/drivers/bluetooth/hci_ll.c
index b874c0efde2..ff6d589c34a 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -348,7 +348,7 @@ static int ll_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 
 static inline int ll_check_data_len(struct ll_struct *ll, int len)
 {
-	register int room = skb_tailroom(ll->rx_skb);
+	int room = skb_tailroom(ll->rx_skb);
 
 	BT_DBG("len %d room %d", len, room);
 
@@ -374,11 +374,11 @@ static inline int ll_check_data_len(struct ll_struct *ll, int len)
 static int ll_recv(struct hci_uart *hu, void *data, int count)
 {
 	struct ll_struct *ll = hu->priv;
-	register char *ptr;
+	char *ptr;
 	struct hci_event_hdr *eh;
 	struct hci_acl_hdr   *ah;
 	struct hci_sco_hdr   *sh;
-	register int len, type, dlen;
+	int len, type, dlen;
 
 	BT_DBG("hu %p count %d rx_state %ld rx_count %ld", hu, count, ll->rx_state, ll->rx_count);
 
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index 6cf6ab22ad2..fffa61ff5cb 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -47,6 +47,7 @@
 #define HCI_UART_RAW_DEVICE	0
 #define HCI_UART_RESET_ON_INIT	1
 #define HCI_UART_CREATE_AMP	2
+#define HCI_UART_INIT_PENDING	3
 
 struct hci_uart;
 
@@ -66,6 +67,8 @@ struct hci_uart {
 	unsigned long		flags;
 	unsigned long		hdev_flags;
 
+	struct work_struct	init_ready;
+
 	struct hci_uart_proto	*proto;
 	void			*priv;
 
@@ -76,6 +79,7 @@ struct hci_uart {
 
 /* HCI_UART proto flag bits */
 #define HCI_UART_PROTO_SET	0
+#define HCI_UART_REGISTERED	1
 
 /* TX states  */
 #define HCI_UART_SENDING	1
@@ -84,6 +88,7 @@ struct hci_uart {
 int hci_uart_register_proto(struct hci_uart_proto *p);
 int hci_uart_unregister_proto(struct hci_uart_proto *p);
 int hci_uart_tx_wakeup(struct hci_uart *hu);
+int hci_uart_init_ready(struct hci_uart *hu);
 
 #ifdef CONFIG_BT_HCIUART_H4
 int h4_init(void);
@@ -104,3 +109,8 @@ int ll_deinit(void);
 int ath_init(void);
 int ath_deinit(void);
 #endif
+
+#ifdef CONFIG_BT_HCIUART_3WIRE
+int h5_init(void);
+int h5_deinit(void);
+#endif
diff --git a/drivers/char/agp/intel-agp.c b/drivers/char/agp/intel-agp.c
index 0a418527941..b130df0a195 100644
--- a/drivers/char/agp/intel-agp.c
+++ b/drivers/char/agp/intel-agp.c
@@ -12,6 +12,7 @@
 #include <asm/smp.h>
 #include "agp.h"
 #include "intel-agp.h"
+#include <drm/intel-gtt.h>
 
 int intel_agp_enabled;
 EXPORT_SYMBOL(intel_agp_enabled);
@@ -747,7 +748,7 @@ static int __devinit agp_intel_probe(struct pci_dev *pdev,
 
 	bridge->capndx = cap_ptr;
 
-	if (intel_gmch_probe(pdev, bridge))
+	if (intel_gmch_probe(pdev, NULL, bridge))
 		goto found_gmch;
 
 	for (i = 0; intel_agp_chipsets[i].name != NULL; i++) {
@@ -824,7 +825,7 @@ static void __devexit agp_intel_remove(struct pci_dev *pdev)
 
 	agp_remove_bridge(bridge);
 
-	intel_gmch_remove(pdev);
+	intel_gmch_remove();
 
 	agp_put_bridge(bridge);
 }
@@ -902,17 +903,6 @@ static struct pci_device_id agp_intel_pci_table[] = {
 	ID(PCI_DEVICE_ID_INTEL_IRONLAKE_M_HB),
 	ID(PCI_DEVICE_ID_INTEL_IRONLAKE_MA_HB),
 	ID(PCI_DEVICE_ID_INTEL_IRONLAKE_MC2_HB),
-	ID(PCI_DEVICE_ID_INTEL_SANDYBRIDGE_HB),
-	ID(PCI_DEVICE_ID_INTEL_SANDYBRIDGE_M_HB),
-	ID(PCI_DEVICE_ID_INTEL_SANDYBRIDGE_S_HB),
-	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_HB),
-	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_M_HB),
-	ID(PCI_DEVICE_ID_INTEL_IVYBRIDGE_S_HB),
-	ID(PCI_DEVICE_ID_INTEL_VALLEYVIEW_HB),
-	ID(PCI_DEVICE_ID_INTEL_HASWELL_HB),
-	ID(PCI_DEVICE_ID_INTEL_HASWELL_M_HB),
-	ID(PCI_DEVICE_ID_INTEL_HASWELL_S_HB),
-	ID(PCI_DEVICE_ID_INTEL_HASWELL_E_HB),
 	{ }
 };
 
diff --git a/drivers/char/agp/intel-agp.h b/drivers/char/agp/intel-agp.h
index 8e2d9140f30..57226424690 100644
--- a/drivers/char/agp/intel-agp.h
+++ b/drivers/char/agp/intel-agp.h
@@ -251,7 +251,4 @@
 #define PCI_DEVICE_ID_INTEL_HASWELL_SDV		0x0c16 /* SDV */
 #define PCI_DEVICE_ID_INTEL_HASWELL_E_HB			0x0c04
 
-int intel_gmch_probe(struct pci_dev *pdev,
-			       struct agp_bridge_data *bridge);
-void intel_gmch_remove(struct pci_dev *pdev);
 #endif
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index 1237e7575c3..9ed92ef5829 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -66,7 +66,6 @@ static struct _intel_private {
 	struct pci_dev *bridge_dev;
 	u8 __iomem *registers;
 	phys_addr_t gtt_bus_addr;
-	phys_addr_t gma_bus_addr;
 	u32 PGETBL_save;
 	u32 __iomem *gtt;		/* I915G */
 	bool clear_fake_agp; /* on first access via agp, fill with scratch */
@@ -76,6 +75,7 @@ static struct _intel_private {
 	struct resource ifp_resource;
 	int resource_valid;
 	struct page *scratch_page;
+	int refcount;
 } intel_private;
 
 #define INTEL_GTT_GEN	intel_private.driver->gen
@@ -648,6 +648,7 @@ static void intel_gtt_cleanup(void)
 
 static int intel_gtt_init(void)
 {
+	u32 gma_addr;
 	u32 gtt_map_size;
 	int ret;
 
@@ -694,6 +695,15 @@ static int intel_gtt_init(void)
 		return ret;
 	}
 
+	if (INTEL_GTT_GEN <= 2)
+		pci_read_config_dword(intel_private.pcidev, I810_GMADDR,
+				      &gma_addr);
+	else
+		pci_read_config_dword(intel_private.pcidev, I915_GMADDR,
+				      &gma_addr);
+
+	intel_private.base.gma_bus_addr = (gma_addr & PCI_BASE_ADDRESS_MEM_MASK);
+
 	return 0;
 }
 
@@ -767,20 +777,10 @@ static void i830_write_entry(dma_addr_t addr, unsigned int entry,
 	writel(addr | pte_flags, intel_private.gtt + entry);
 }
 
-static bool intel_enable_gtt(void)
+bool intel_enable_gtt(void)
 {
-	u32 gma_addr;
 	u8 __iomem *reg;
 
-	if (INTEL_GTT_GEN <= 2)
-		pci_read_config_dword(intel_private.pcidev, I810_GMADDR,
-				      &gma_addr);
-	else
-		pci_read_config_dword(intel_private.pcidev, I915_GMADDR,
-				      &gma_addr);
-
-	intel_private.gma_bus_addr = (gma_addr & PCI_BASE_ADDRESS_MEM_MASK);
-
 	if (INTEL_GTT_GEN >= 6)
 	    return true;
 
@@ -823,6 +823,7 @@ static bool intel_enable_gtt(void)
 
 	return true;
 }
+EXPORT_SYMBOL(intel_enable_gtt);
 
 static int i830_setup(void)
 {
@@ -860,7 +861,7 @@ static int intel_fake_agp_configure(void)
 	    return -EIO;
 
 	intel_private.clear_fake_agp = true;
-	agp_bridge->gart_bus_addr = intel_private.gma_bus_addr;
+	agp_bridge->gart_bus_addr = intel_private.base.gma_bus_addr;
 
 	return 0;
 }
@@ -1182,9 +1183,17 @@ static void gen6_write_entry(dma_addr_t addr, unsigned int entry,
 static void valleyview_write_entry(dma_addr_t addr, unsigned int entry,
 				   unsigned int flags)
 {
+	unsigned int type_mask = flags & ~AGP_USER_CACHED_MEMORY_GFDT;
+	unsigned int gfdt = flags & AGP_USER_CACHED_MEMORY_GFDT;
 	u32 pte_flags;
 
-	pte_flags = GEN6_PTE_UNCACHED | I810_PTE_VALID;
+	if (type_mask == AGP_USER_MEMORY)
+		pte_flags = GEN6_PTE_UNCACHED | I810_PTE_VALID;
+	else {
+		pte_flags = GEN6_PTE_LLC | I810_PTE_VALID;
+		if (gfdt)
+			pte_flags |= GEN6_PTE_GFDT;
+	}
 
 	/* gen6 has bit11-4 for physical addr bit39-32 */
 	addr |= (addr >> 28) & 0xff0;
@@ -1244,6 +1253,7 @@ static int i9xx_setup(void)
 		switch (INTEL_GTT_GEN) {
 		case 5:
 		case 6:
+		case 7:
 			gtt_offset = MB(2);
 			break;
 		case 4:
@@ -1379,7 +1389,6 @@ static const struct intel_gtt_driver valleyview_gtt_driver = {
 	.write_entry = valleyview_write_entry,
 	.dma_mask_size = 40,
 	.check_flags = gen6_check_flags,
-	.chipset_flush = i9xx_chipset_flush,
 };
 
 /* Table to describe Intel GMCH and AGP/PCIE GART drivers.  At least one of
@@ -1523,14 +1532,32 @@ static int find_gmch(u16 device)
 	return 1;
 }
 
-int intel_gmch_probe(struct pci_dev *pdev,
-				      struct agp_bridge_data *bridge)
+int intel_gmch_probe(struct pci_dev *bridge_pdev, struct pci_dev *gpu_pdev,
+		     struct agp_bridge_data *bridge)
 {
 	int i, mask;
-	intel_private.driver = NULL;
+
+	/*
+	 * Can be called from the fake agp driver but also directly from
+	 * drm/i915.ko. Hence we need to check whether everything is set up
+	 * already.
+	 */
+	if (intel_private.driver) {
+		intel_private.refcount++;
+		return 1;
+	}
 
 	for (i = 0; intel_gtt_chipsets[i].name != NULL; i++) {
-		if (find_gmch(intel_gtt_chipsets[i].gmch_chip_id)) {
+		if (gpu_pdev) {
+			if (gpu_pdev->device ==
+			    intel_gtt_chipsets[i].gmch_chip_id) {
+				intel_private.pcidev = pci_dev_get(gpu_pdev);
+				intel_private.driver =
+					intel_gtt_chipsets[i].gtt_driver;
+
+				break;
+			}
+		} else if (find_gmch(intel_gtt_chipsets[i].gmch_chip_id)) {
 			intel_private.driver =
 				intel_gtt_chipsets[i].gtt_driver;
 			break;
@@ -1540,13 +1567,17 @@ int intel_gmch_probe(struct pci_dev *pdev,
 	if (!intel_private.driver)
 		return 0;
 
-	bridge->driver = &intel_fake_agp_driver;
-	bridge->dev_private_data = &intel_private;
-	bridge->dev = pdev;
+	intel_private.refcount++;
+
+	if (bridge) {
+		bridge->driver = &intel_fake_agp_driver;
+		bridge->dev_private_data = &intel_private;
+		bridge->dev = bridge_pdev;
+	}
 
-	intel_private.bridge_dev = pci_dev_get(pdev);
+	intel_private.bridge_dev = pci_dev_get(bridge_pdev);
 
-	dev_info(&pdev->dev, "Intel %s Chipset\n", intel_gtt_chipsets[i].name);
+	dev_info(&bridge_pdev->dev, "Intel %s Chipset\n", intel_gtt_chipsets[i].name);
 
 	mask = intel_private.driver->dma_mask_size;
 	if (pci_set_dma_mask(intel_private.pcidev, DMA_BIT_MASK(mask)))
@@ -1556,11 +1587,11 @@ int intel_gmch_probe(struct pci_dev *pdev,
 		pci_set_consistent_dma_mask(intel_private.pcidev,
 					    DMA_BIT_MASK(mask));
 
-	/*if (bridge->driver == &intel_810_driver)
-		return 1;*/
+	if (intel_gtt_init() != 0) {
+		intel_gmch_remove();
 
-	if (intel_gtt_init() != 0)
 		return 0;
+	}
 
 	return 1;
 }
@@ -1579,12 +1610,16 @@ void intel_gtt_chipset_flush(void)
 }
 EXPORT_SYMBOL(intel_gtt_chipset_flush);
 
-void intel_gmch_remove(struct pci_dev *pdev)
+void intel_gmch_remove(void)
 {
+	if (--intel_private.refcount)
+		return;
+
 	if (intel_private.pcidev)
 		pci_dev_put(intel_private.pcidev);
 	if (intel_private.bridge_dev)
 		pci_dev_put(intel_private.bridge_dev);
+	intel_private.driver = NULL;
 }
 EXPORT_SYMBOL(intel_gmch_remove);
 
diff --git a/drivers/char/bsr.c b/drivers/char/bsr.c
index 0c688232aab..97467053a01 100644
--- a/drivers/char/bsr.c
+++ b/drivers/char/bsr.c
@@ -297,7 +297,6 @@ static int __init bsr_init(void)
 	struct device_node *np;
 	dev_t bsr_dev;
 	int ret = -ENODEV;
-	int result;
 
 	np = of_find_compatible_node(NULL, NULL, "ibm,bsr");
 	if (!np)
@@ -306,13 +305,14 @@ static int __init bsr_init(void)
 	bsr_class = class_create(THIS_MODULE, "bsr");
 	if (IS_ERR(bsr_class)) {
 		printk(KERN_ERR "class_create() failed for bsr_class\n");
+		ret = PTR_ERR(bsr_class);
 		goto out_err_1;
 	}
 	bsr_class->dev_attrs = bsr_dev_attrs;
 
-	result = alloc_chrdev_region(&bsr_dev, 0, BSR_MAX_DEVS, "bsr");
+	ret = alloc_chrdev_region(&bsr_dev, 0, BSR_MAX_DEVS, "bsr");
 	bsr_major = MAJOR(bsr_dev);
-	if (result < 0) {
+	if (ret < 0) {
 		printk(KERN_ERR "alloc_chrdev_region() failed for bsr\n");
 		goto out_err_2;
 	}
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index f45dad39a18..7c0d391996b 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -73,6 +73,20 @@ config HW_RANDOM_ATMEL
 
 	  If unsure, say Y.
 
+config HW_RANDOM_BCM63XX
+	tristate "Broadcom BCM63xx Random Number Generator support"
+	depends on HW_RANDOM && BCM63XX
+	default HW_RANDOM
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on the Broadcom BCM63xx SoCs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bcm63xx-rng
+
+	  If unusure, say Y.
+
+
 config HW_RANDOM_GEODE
 	tristate "AMD Geode HW Random Number Generator support"
 	depends on HW_RANDOM && X86_32 && PCI
@@ -263,3 +277,15 @@ config HW_RANDOM_PSERIES
 	  module will be called pseries-rng.
 
 	  If unsure, say Y.
+
+config HW_RANDOM_EXYNOS
+	tristate "EXYNOS HW random number generator support"
+	depends on HW_RANDOM && HAS_IOMEM && HAVE_CLK
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on EXYNOS SOCs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos-rng.
+
+	  If unsure, say Y.
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index d901dfa3032..39a757ca15b 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_HW_RANDOM_TIMERIOMEM) += timeriomem-rng.o
 obj-$(CONFIG_HW_RANDOM_INTEL) += intel-rng.o
 obj-$(CONFIG_HW_RANDOM_AMD) += amd-rng.o
 obj-$(CONFIG_HW_RANDOM_ATMEL) += atmel-rng.o
+obj-$(CONFIG_HW_RANDOM_BCM63XX)	+= bcm63xx-rng.o
 obj-$(CONFIG_HW_RANDOM_GEODE) += geode-rng.o
 obj-$(CONFIG_HW_RANDOM_N2RNG) += n2-rng.o
 n2-rng-y := n2-drv.o n2-asm.o
@@ -23,3 +24,4 @@ obj-$(CONFIG_HW_RANDOM_NOMADIK) += nomadik-rng.o
 obj-$(CONFIG_HW_RANDOM_PICOXCELL) += picoxcell-rng.o
 obj-$(CONFIG_HW_RANDOM_PPC4XX) += ppc4xx-rng.o
 obj-$(CONFIG_HW_RANDOM_PSERIES) += pseries-rng.o
+obj-$(CONFIG_HW_RANDOM_EXYNOS)	+= exynos-rng.o
diff --git a/drivers/char/hw_random/bcm63xx-rng.c b/drivers/char/hw_random/bcm63xx-rng.c
new file mode 100644
index 00000000000..aec6a4277ca
--- /dev/null
+++ b/drivers/char/hw_random/bcm63xx-rng.c
@@ -0,0 +1,175 @@
+/*
+ * Broadcom BCM63xx Random Number Generator support
+ *
+ * Copyright (C) 2011, Florian Fainelli <florian@openwrt.org>
+ * Copyright (C) 2009, Broadcom Corporation
+ *
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/hw_random.h>
+
+#include <bcm63xx_io.h>
+#include <bcm63xx_regs.h>
+
+struct bcm63xx_rng_priv {
+	struct clk *clk;
+	void __iomem *regs;
+};
+
+#define to_rng_priv(rng)	((struct bcm63xx_rng_priv *)rng->priv)
+
+static int bcm63xx_rng_init(struct hwrng *rng)
+{
+	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
+	u32 val;
+
+	val = bcm_readl(priv->regs + RNG_CTRL);
+	val |= RNG_EN;
+	bcm_writel(val, priv->regs + RNG_CTRL);
+
+	return 0;
+}
+
+static void bcm63xx_rng_cleanup(struct hwrng *rng)
+{
+	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
+	u32 val;
+
+	val = bcm_readl(priv->regs + RNG_CTRL);
+	val &= ~RNG_EN;
+	bcm_writel(val, priv->regs + RNG_CTRL);
+}
+
+static int bcm63xx_rng_data_present(struct hwrng *rng, int wait)
+{
+	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
+
+	return bcm_readl(priv->regs + RNG_STAT) & RNG_AVAIL_MASK;
+}
+
+static int bcm63xx_rng_data_read(struct hwrng *rng, u32 *data)
+{
+	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
+
+	*data = bcm_readl(priv->regs + RNG_DATA);
+
+	return 4;
+}
+
+static int __devinit bcm63xx_rng_probe(struct platform_device *pdev)
+{
+	struct resource *r;
+	struct clk *clk;
+	int ret;
+	struct bcm63xx_rng_priv *priv;
+	struct hwrng *rng;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "no iomem resource\n");
+		ret = -ENXIO;
+		goto out;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "no memory for private structure\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	rng = kzalloc(sizeof(*rng), GFP_KERNEL);
+	if (!rng) {
+		dev_err(&pdev->dev, "no memory for rng structure\n");
+		ret = -ENOMEM;
+		goto out_free_priv;
+	}
+
+	platform_set_drvdata(pdev, rng);
+	rng->priv = (unsigned long)priv;
+	rng->name = pdev->name;
+	rng->init = bcm63xx_rng_init;
+	rng->cleanup = bcm63xx_rng_cleanup;
+	rng->data_present = bcm63xx_rng_data_present;
+	rng->data_read = bcm63xx_rng_data_read;
+
+	clk = clk_get(&pdev->dev, "ipsec");
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "no clock for device\n");
+		ret = PTR_ERR(clk);
+		goto out_free_rng;
+	}
+
+	priv->clk = clk;
+
+	if (!devm_request_mem_region(&pdev->dev, r->start,
+					resource_size(r), pdev->name)) {
+		dev_err(&pdev->dev, "request mem failed");
+		ret = -ENOMEM;
+		goto out_free_rng;
+	}
+
+	priv->regs = devm_ioremap_nocache(&pdev->dev, r->start,
+					resource_size(r));
+	if (!priv->regs) {
+		dev_err(&pdev->dev, "ioremap failed");
+		ret = -ENOMEM;
+		goto out_free_rng;
+	}
+
+	clk_enable(clk);
+
+	ret = hwrng_register(rng);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register rng device\n");
+		goto out_clk_disable;
+	}
+
+	dev_info(&pdev->dev, "registered RNG driver\n");
+
+	return 0;
+
+out_clk_disable:
+	clk_disable(clk);
+out_free_rng:
+	platform_set_drvdata(pdev, NULL);
+	kfree(rng);
+out_free_priv:
+	kfree(priv);
+out:
+	return ret;
+}
+
+static int __devexit bcm63xx_rng_remove(struct platform_device *pdev)
+{
+	struct hwrng *rng = platform_get_drvdata(pdev);
+	struct bcm63xx_rng_priv *priv = to_rng_priv(rng);
+
+	hwrng_unregister(rng);
+	clk_disable(priv->clk);
+	kfree(priv);
+	kfree(rng);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver bcm63xx_rng_driver = {
+	.probe		= bcm63xx_rng_probe,
+	.remove		= __devexit_p(bcm63xx_rng_remove),
+	.driver		= {
+		.name	= "bcm63xx-rng",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(bcm63xx_rng_driver);
+
+MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
+MODULE_DESCRIPTION("Broadcom BCM63xx RNG driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/hw_random/exynos-rng.c b/drivers/char/hw_random/exynos-rng.c
new file mode 100644
index 00000000000..232ba9ce579
--- /dev/null
+++ b/drivers/char/hw_random/exynos-rng.c
@@ -0,0 +1,182 @@
+/*
+ * exynos-rng.c - Random Number Generator driver for the exynos
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * Jonghwa Lee <jonghwa3.lee@smasung.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;
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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/hw_random.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/err.h>
+
+#define EXYNOS_PRNG_STATUS_OFFSET	0x10
+#define EXYNOS_PRNG_SEED_OFFSET		0x140
+#define EXYNOS_PRNG_OUT1_OFFSET		0x160
+#define SEED_SETTING_DONE		BIT(1)
+#define PRNG_START			0x18
+#define PRNG_DONE			BIT(5)
+#define EXYNOS_AUTOSUSPEND_DELAY	100
+
+struct exynos_rng {
+	struct device *dev;
+	struct hwrng rng;
+	void __iomem *mem;
+	struct clk *clk;
+};
+
+static u32 exynos_rng_readl(struct exynos_rng *rng, u32 offset)
+{
+	return	__raw_readl(rng->mem + offset);
+}
+
+static void exynos_rng_writel(struct exynos_rng *rng, u32 val, u32 offset)
+{
+	__raw_writel(val, rng->mem + offset);
+}
+
+static int exynos_init(struct hwrng *rng)
+{
+	struct exynos_rng *exynos_rng = container_of(rng,
+						struct exynos_rng, rng);
+	int i;
+	int ret = 0;
+
+	pm_runtime_get_sync(exynos_rng->dev);
+
+	for (i = 0 ; i < 5 ; i++)
+		exynos_rng_writel(exynos_rng, jiffies,
+				EXYNOS_PRNG_SEED_OFFSET + 4*i);
+
+	if (!(exynos_rng_readl(exynos_rng, EXYNOS_PRNG_STATUS_OFFSET)
+						 & SEED_SETTING_DONE))
+		ret = -EIO;
+
+	pm_runtime_put_noidle(exynos_rng->dev);
+
+	return ret;
+}
+
+static int exynos_read(struct hwrng *rng, void *buf,
+					size_t max, bool wait)
+{
+	struct exynos_rng *exynos_rng = container_of(rng,
+						struct exynos_rng, rng);
+	u32 *data = buf;
+
+	pm_runtime_get_sync(exynos_rng->dev);
+
+	exynos_rng_writel(exynos_rng, PRNG_START, 0);
+
+	while (!(exynos_rng_readl(exynos_rng,
+			EXYNOS_PRNG_STATUS_OFFSET) & PRNG_DONE))
+		cpu_relax();
+
+	exynos_rng_writel(exynos_rng, PRNG_DONE, EXYNOS_PRNG_STATUS_OFFSET);
+
+	*data = exynos_rng_readl(exynos_rng, EXYNOS_PRNG_OUT1_OFFSET);
+
+	pm_runtime_mark_last_busy(exynos_rng->dev);
+	pm_runtime_autosuspend(exynos_rng->dev);
+
+	return 4;
+}
+
+static int __devinit exynos_rng_probe(struct platform_device *pdev)
+{
+	struct exynos_rng *exynos_rng;
+
+	exynos_rng = devm_kzalloc(&pdev->dev, sizeof(struct exynos_rng),
+					GFP_KERNEL);
+	if (!exynos_rng)
+		return -ENOMEM;
+
+	exynos_rng->dev = &pdev->dev;
+	exynos_rng->rng.name = "exynos";
+	exynos_rng->rng.init =	exynos_init;
+	exynos_rng->rng.read = exynos_read;
+	exynos_rng->clk = devm_clk_get(&pdev->dev, "secss");
+	if (IS_ERR(exynos_rng->clk)) {
+		dev_err(&pdev->dev, "Couldn't get clock.\n");
+		return -ENOENT;
+	}
+
+	exynos_rng->mem = devm_request_and_ioremap(&pdev->dev,
+			platform_get_resource(pdev, IORESOURCE_MEM, 0));
+	if (!exynos_rng->mem)
+		return -EBUSY;
+
+	platform_set_drvdata(pdev, exynos_rng);
+
+	pm_runtime_set_autosuspend_delay(&pdev->dev, EXYNOS_AUTOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	return hwrng_register(&exynos_rng->rng);
+}
+
+static int __devexit exynos_rng_remove(struct platform_device *pdev)
+{
+	struct exynos_rng *exynos_rng = platform_get_drvdata(pdev);
+
+	hwrng_unregister(&exynos_rng->rng);
+
+	return 0;
+}
+
+static int exynos_rng_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos_rng *exynos_rng = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(exynos_rng->clk);
+
+	return 0;
+}
+
+static int exynos_rng_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos_rng *exynos_rng = platform_get_drvdata(pdev);
+
+	return clk_prepare_enable(exynos_rng->clk);
+}
+
+
+UNIVERSAL_DEV_PM_OPS(exynos_rng_pm_ops, exynos_rng_runtime_suspend,
+					exynos_rng_runtime_resume, NULL);
+
+static struct platform_driver exynos_rng_driver = {
+	.driver		= {
+		.name	= "exynos-rng",
+		.owner	= THIS_MODULE,
+		.pm	= &exynos_rng_pm_ops,
+	},
+	.probe		= exynos_rng_probe,
+	.remove		= __devexit_p(exynos_rng_remove),
+};
+
+module_platform_driver(exynos_rng_driver);
+
+MODULE_DESCRIPTION("EXYNOS 4 H/W Random Number Generator driver");
+MODULE_AUTHOR("Jonghwa Lee <jonghwa3.lee@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/hw_random/mxc-rnga.c b/drivers/char/hw_random/mxc-rnga.c
index 187c6be80f4..85074de5042 100644
--- a/drivers/char/hw_random/mxc-rnga.c
+++ b/drivers/char/hw_random/mxc-rnga.c
@@ -24,6 +24,7 @@
 #include <linux/ioport.h>
 #include <linux/platform_device.h>
 #include <linux/hw_random.h>
+#include <linux/delay.h>
 #include <linux/io.h>
 
 /* RNGA Registers */
@@ -60,16 +61,20 @@
 
 static struct platform_device *rng_dev;
 
-static int mxc_rnga_data_present(struct hwrng *rng)
+static int mxc_rnga_data_present(struct hwrng *rng, int wait)
 {
-	int level;
 	void __iomem *rng_base = (void __iomem *)rng->priv;
-
-	/* how many random numbers is in FIFO? [0-16] */
-	level = ((__raw_readl(rng_base + RNGA_STATUS) &
-			RNGA_STATUS_LEVEL_MASK) >> 8);
-
-	return level > 0 ? 1 : 0;
+	int i;
+
+	for (i = 0; i < 20; i++) {
+		/* how many random numbers are in FIFO? [0-16] */
+		int level = (__raw_readl(rng_base + RNGA_STATUS) &
+				RNGA_STATUS_LEVEL_MASK) >> 8;
+		if (level || !wait)
+			return !!level;
+		udelay(10);
+	}
+	return 0;
 }
 
 static int mxc_rnga_data_read(struct hwrng *rng, u32 * data)
diff --git a/drivers/char/hw_random/omap-rng.c b/drivers/char/hw_random/omap-rng.c
index 1412565c01a..d706bd0e9e8 100644
--- a/drivers/char/hw_random/omap-rng.c
+++ b/drivers/char/hw_random/omap-rng.c
@@ -162,22 +162,24 @@ static int __exit omap_rng_remove(struct platform_device *pdev)
 
 #ifdef CONFIG_PM
 
-static int omap_rng_suspend(struct platform_device *pdev, pm_message_t message)
+static int omap_rng_suspend(struct device *dev)
 {
 	omap_rng_write_reg(RNG_MASK_REG, 0x0);
 	return 0;
 }
 
-static int omap_rng_resume(struct platform_device *pdev)
+static int omap_rng_resume(struct device *dev)
 {
 	omap_rng_write_reg(RNG_MASK_REG, 0x1);
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(omap_rng_pm, omap_rng_suspend, omap_rng_resume);
+#define	OMAP_RNG_PM	(&omap_rng_pm)
+
 #else
 
-#define	omap_rng_suspend	NULL
-#define	omap_rng_resume		NULL
+#define	OMAP_RNG_PM	NULL
 
 #endif
 
@@ -188,11 +190,10 @@ static struct platform_driver omap_rng_driver = {
 	.driver = {
 		.name		= "omap_rng",
 		.owner		= THIS_MODULE,
+		.pm		= OMAP_RNG_PM,
 	},
 	.probe		= omap_rng_probe,
 	.remove		= __exit_p(omap_rng_remove),
-	.suspend	= omap_rng_suspend,
-	.resume		= omap_rng_resume
 };
 
 static int __init omap_rng_init(void)
diff --git a/drivers/char/hw_random/virtio-rng.c b/drivers/char/hw_random/virtio-rng.c
index 723725bbb96..5708299507d 100644
--- a/drivers/char/hw_random/virtio-rng.c
+++ b/drivers/char/hw_random/virtio-rng.c
@@ -55,6 +55,7 @@ static void register_buffer(u8 *buf, size_t size)
 
 static int virtio_read(struct hwrng *rng, void *buf, size_t size, bool wait)
 {
+	int ret;
 
 	if (!busy) {
 		busy = true;
@@ -65,7 +66,9 @@ static int virtio_read(struct hwrng *rng, void *buf, size_t size, bool wait)
 	if (!wait)
 		return 0;
 
-	wait_for_completion(&have_data);
+	ret = wait_for_completion_killable(&have_data);
+	if (ret < 0)
+		return ret;
 
 	busy = false;
 
@@ -85,7 +88,7 @@ static struct hwrng virtio_hwrng = {
 	.read		= virtio_read,
 };
 
-static int virtrng_probe(struct virtio_device *vdev)
+static int probe_common(struct virtio_device *vdev)
 {
 	int err;
 
@@ -103,13 +106,37 @@ static int virtrng_probe(struct virtio_device *vdev)
 	return 0;
 }
 
-static void __devexit virtrng_remove(struct virtio_device *vdev)
+static void remove_common(struct virtio_device *vdev)
 {
 	vdev->config->reset(vdev);
+	busy = false;
 	hwrng_unregister(&virtio_hwrng);
 	vdev->config->del_vqs(vdev);
 }
 
+static int virtrng_probe(struct virtio_device *vdev)
+{
+	return probe_common(vdev);
+}
+
+static void __devexit virtrng_remove(struct virtio_device *vdev)
+{
+	remove_common(vdev);
+}
+
+#ifdef CONFIG_PM
+static int virtrng_freeze(struct virtio_device *vdev)
+{
+	remove_common(vdev);
+	return 0;
+}
+
+static int virtrng_restore(struct virtio_device *vdev)
+{
+	return probe_common(vdev);
+}
+#endif
+
 static struct virtio_device_id id_table[] = {
 	{ VIRTIO_ID_RNG, VIRTIO_DEV_ANY_ID },
 	{ 0 },
@@ -121,6 +148,10 @@ static struct virtio_driver virtio_rng_driver = {
 	.id_table =	id_table,
 	.probe =	virtrng_probe,
 	.remove =	__devexit_p(virtrng_remove),
+#ifdef CONFIG_PM
+	.freeze =	virtrng_freeze,
+	.restore =	virtrng_restore,
+#endif
 };
 
 static int __init init(void)
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 1e638fff40e..83f85cf7fb1 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -2503,18 +2503,6 @@ static void __devexit ipmi_pci_remove(struct pci_dev *pdev)
 	cleanup_one_si(info);
 }
 
-#ifdef CONFIG_PM
-static int ipmi_pci_suspend(struct pci_dev *pdev, pm_message_t state)
-{
-	return 0;
-}
-
-static int ipmi_pci_resume(struct pci_dev *pdev)
-{
-	return 0;
-}
-#endif
-
 static struct pci_device_id ipmi_pci_devices[] = {
 	{ PCI_DEVICE(PCI_HP_VENDOR_ID, PCI_MMC_DEVICE_ID) },
 	{ PCI_DEVICE_CLASS(PCI_ERMC_CLASSCODE, PCI_ERMC_CLASSCODE_MASK) },
@@ -2527,10 +2515,6 @@ static struct pci_driver ipmi_pci_driver = {
 	.id_table =     ipmi_pci_devices,
 	.probe =        ipmi_pci_probe,
 	.remove =       __devexit_p(ipmi_pci_remove),
-#ifdef CONFIG_PM
-	.suspend =      ipmi_pci_suspend,
-	.resume =       ipmi_pci_resume,
-#endif
 };
 #endif /* CONFIG_PCI */
 
diff --git a/drivers/char/ipmi/ipmi_watchdog.c b/drivers/char/ipmi/ipmi_watchdog.c
index 7ed356e5203..37b8be7cba9 100644
--- a/drivers/char/ipmi/ipmi_watchdog.c
+++ b/drivers/char/ipmi/ipmi_watchdog.c
@@ -141,17 +141,6 @@
 
 #define IPMI_WDOG_TIMER_NOT_INIT_RESP	0x80
 
-/* These are here until the real ones get into the watchdog.h interface. */
-#ifndef WDIOC_GETTIMEOUT
-#define	WDIOC_GETTIMEOUT        _IOW(WATCHDOG_IOCTL_BASE, 20, int)
-#endif
-#ifndef WDIOC_SET_PRETIMEOUT
-#define	WDIOC_SET_PRETIMEOUT     _IOW(WATCHDOG_IOCTL_BASE, 21, int)
-#endif
-#ifndef WDIOC_GET_PRETIMEOUT
-#define	WDIOC_GET_PRETIMEOUT     _IOW(WATCHDOG_IOCTL_BASE, 22, int)
-#endif
-
 static DEFINE_MUTEX(ipmi_watchdog_mutex);
 static bool nowayout = WATCHDOG_NOWAYOUT;
 
@@ -732,7 +721,6 @@ static int ipmi_ioctl(struct file *file,
 			return -EFAULT;
 		return 0;
 
-	case WDIOC_SET_PRETIMEOUT:
 	case WDIOC_SETPRETIMEOUT:
 		i = copy_from_user(&val, argp, sizeof(int));
 		if (i)
@@ -740,7 +728,6 @@ static int ipmi_ioctl(struct file *file,
 		pretimeout = val;
 		return ipmi_set_timeout(IPMI_SET_TIMEOUT_HB_IF_NECESSARY);
 
-	case WDIOC_GET_PRETIMEOUT:
 	case WDIOC_GETPRETIMEOUT:
 		i = copy_to_user(argp, &pretimeout, sizeof(pretimeout));
 		if (i)
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 67c3371723c..e5eedfa24c9 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -27,14 +27,16 @@
 #include <linux/splice.h>
 #include <linux/pfn.h>
 #include <linux/export.h>
+#include <linux/io.h>
 
 #include <asm/uaccess.h>
-#include <asm/io.h>
 
 #ifdef CONFIG_IA64
 # include <linux/efi.h>
 #endif
 
+#define DEVPORT_MINOR	4
+
 static inline unsigned long size_inside_page(unsigned long start,
 					     unsigned long size)
 {
@@ -894,6 +896,13 @@ static int __init chr_dev_init(void)
 	for (minor = 1; minor < ARRAY_SIZE(devlist); minor++) {
 		if (!devlist[minor].name)
 			continue;
+
+		/*
+		 * Create /dev/port? 
+		 */
+		if ((minor == DEVPORT_MINOR) && !arch_has_dev_port())
+			continue;
+
 		device_create(mem_class, NULL, MKDEV(MEM_MAJOR, minor),
 			      NULL, devlist[minor].name);
 	}
diff --git a/drivers/char/mspec.c b/drivers/char/mspec.c
index 8b78750f1ef..845f97fd183 100644
--- a/drivers/char/mspec.c
+++ b/drivers/char/mspec.c
@@ -283,7 +283,7 @@ mspec_mmap(struct file *file, struct vm_area_struct *vma,
 	vdata->flags = flags;
 	vdata->type = type;
 	spin_lock_init(&vdata->lock);
-	vdata->refcnt = ATOMIC_INIT(1);
+	atomic_set(&vdata->refcnt, 1);
 	vma->vm_private_data = vdata;
 
 	vma->vm_flags |= (VM_IO | VM_RESERVED | VM_PFNMAP | VM_DONTEXPAND);
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 4ec04a75473..b86eae9b77d 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -125,21 +125,26 @@
  * The current exported interfaces for gathering environmental noise
  * from the devices are:
  *
+ *	void add_device_randomness(const void *buf, unsigned int size);
  * 	void add_input_randomness(unsigned int type, unsigned int code,
  *                                unsigned int value);
- * 	void add_interrupt_randomness(int irq);
+ *	void add_interrupt_randomness(int irq, int irq_flags);
  * 	void add_disk_randomness(struct gendisk *disk);
  *
+ * add_device_randomness() is for adding data to the random pool that
+ * is likely to differ between two devices (or possibly even per boot).
+ * This would be things like MAC addresses or serial numbers, or the
+ * read-out of the RTC. This does *not* add any actual entropy to the
+ * pool, but it initializes the pool to different values for devices
+ * that might otherwise be identical and have very little entropy
+ * available to them (particularly common in the embedded world).
+ *
  * add_input_randomness() uses the input layer interrupt timing, as well as
  * the event type information from the hardware.
  *
- * add_interrupt_randomness() uses the inter-interrupt timing as random
- * 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.  Network Interface
- * Controller interrupts are a better measure, since the timing of the
- * NIC interrupts are more unpredictable.
+ * add_interrupt_randomness() uses the interrupt timing as random
+ * inputs to the entropy pool. Using the cycle counters and the irq source
+ * as inputs, it feeds the randomness roughly once a second.
  *
  * 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
@@ -248,6 +253,8 @@
 #include <linux/percpu.h>
 #include <linux/cryptohash.h>
 #include <linux/fips.h>
+#include <linux/ptrace.h>
+#include <linux/kmemcheck.h>
 
 #ifdef CONFIG_GENERIC_HARDIRQS
 # include <linux/irq.h>
@@ -256,8 +263,12 @@
 #include <asm/processor.h>
 #include <asm/uaccess.h>
 #include <asm/irq.h>
+#include <asm/irq_regs.h>
 #include <asm/io.h>
 
+#define CREATE_TRACE_POINTS
+#include <trace/events/random.h>
+
 /*
  * Configuration information
  */
@@ -266,6 +277,8 @@
 #define SEC_XFER_SIZE 512
 #define EXTRACT_SIZE 10
 
+#define LONGS(x) (((x) + sizeof(unsigned long) - 1)/sizeof(unsigned long))
+
 /*
  * The minimum number of bits of entropy before we wake up a read on
  * /dev/random.  Should be enough to do a significant reseed.
@@ -420,8 +433,10 @@ struct entropy_store {
 	/* read-write data: */
 	spinlock_t lock;
 	unsigned add_ptr;
+	unsigned input_rotate;
 	int entropy_count;
-	int input_rotate;
+	int entropy_total;
+	unsigned int initialized:1;
 	__u8 last_data[EXTRACT_SIZE];
 };
 
@@ -454,6 +469,10 @@ static struct entropy_store nonblocking_pool = {
 	.pool = nonblocking_pool_data
 };
 
+static __u32 const twist_table[8] = {
+	0x00000000, 0x3b6e20c8, 0x76dc4190, 0x4db26158,
+	0xedb88320, 0xd6d6a3e8, 0x9b64c2b0, 0xa00ae278 };
+
 /*
  * This function adds bytes into the entropy "pool".  It does not
  * update the entropy estimate.  The caller should call
@@ -464,29 +483,24 @@ static struct entropy_store nonblocking_pool = {
  * it's cheap to do so and helps slightly in the expected case where
  * the entropy is concentrated in the low-order bits.
  */
-static void mix_pool_bytes_extract(struct entropy_store *r, const void *in,
-				   int nbytes, __u8 out[64])
+static void _mix_pool_bytes(struct entropy_store *r, const void *in,
+			    int nbytes, __u8 out[64])
 {
-	static __u32 const twist_table[8] = {
-		0x00000000, 0x3b6e20c8, 0x76dc4190, 0x4db26158,
-		0xedb88320, 0xd6d6a3e8, 0x9b64c2b0, 0xa00ae278 };
 	unsigned long i, j, tap1, tap2, tap3, tap4, tap5;
 	int input_rotate;
 	int wordmask = r->poolinfo->poolwords - 1;
 	const char *bytes = in;
 	__u32 w;
-	unsigned long flags;
 
-	/* Taps are constant, so we can load them without holding r->lock.  */
 	tap1 = r->poolinfo->tap1;
 	tap2 = r->poolinfo->tap2;
 	tap3 = r->poolinfo->tap3;
 	tap4 = r->poolinfo->tap4;
 	tap5 = r->poolinfo->tap5;
 
-	spin_lock_irqsave(&r->lock, flags);
-	input_rotate = r->input_rotate;
-	i = r->add_ptr;
+	smp_rmb();
+	input_rotate = ACCESS_ONCE(r->input_rotate);
+	i = ACCESS_ONCE(r->add_ptr);
 
 	/* mix one byte at a time to simplify size handling and churn faster */
 	while (nbytes--) {
@@ -513,19 +527,61 @@ static void mix_pool_bytes_extract(struct entropy_store *r, const void *in,
 		input_rotate += i ? 7 : 14;
 	}
 
-	r->input_rotate = input_rotate;
-	r->add_ptr = i;
+	ACCESS_ONCE(r->input_rotate) = input_rotate;
+	ACCESS_ONCE(r->add_ptr) = i;
+	smp_wmb();
 
 	if (out)
 		for (j = 0; j < 16; j++)
 			((__u32 *)out)[j] = r->pool[(i - j) & wordmask];
+}
+
+static void __mix_pool_bytes(struct entropy_store *r, const void *in,
+			     int nbytes, __u8 out[64])
+{
+	trace_mix_pool_bytes_nolock(r->name, nbytes, _RET_IP_);
+	_mix_pool_bytes(r, in, nbytes, out);
+}
+
+static void mix_pool_bytes(struct entropy_store *r, const void *in,
+			   int nbytes, __u8 out[64])
+{
+	unsigned long flags;
 
+	trace_mix_pool_bytes(r->name, nbytes, _RET_IP_);
+	spin_lock_irqsave(&r->lock, flags);
+	_mix_pool_bytes(r, in, nbytes, out);
 	spin_unlock_irqrestore(&r->lock, flags);
 }
 
-static void mix_pool_bytes(struct entropy_store *r, const void *in, int bytes)
+struct fast_pool {
+	__u32		pool[4];
+	unsigned long	last;
+	unsigned short	count;
+	unsigned char	rotate;
+	unsigned char	last_timer_intr;
+};
+
+/*
+ * This is a fast mixing routine used by the interrupt randomness
+ * collector.  It's hardcoded for an 128 bit pool and assumes that any
+ * locks that might be needed are taken by the caller.
+ */
+static void fast_mix(struct fast_pool *f, const void *in, int nbytes)
 {
-       mix_pool_bytes_extract(r, in, bytes, NULL);
+	const char	*bytes = in;
+	__u32		w;
+	unsigned	i = f->count;
+	unsigned	input_rotate = f->rotate;
+
+	while (nbytes--) {
+		w = rol32(*bytes++, input_rotate & 31) ^ f->pool[i & 3] ^
+			f->pool[(i + 1) & 3];
+		f->pool[i & 3] = (w >> 3) ^ twist_table[w & 7];
+		input_rotate += (i++ & 3) ? 7 : 14;
+	}
+	f->count = i;
+	f->rotate = input_rotate;
 }
 
 /*
@@ -533,30 +589,38 @@ static void mix_pool_bytes(struct entropy_store *r, const void *in, int bytes)
  */
 static void credit_entropy_bits(struct entropy_store *r, int nbits)
 {
-	unsigned long flags;
-	int entropy_count;
+	int entropy_count, orig;
 
 	if (!nbits)
 		return;
 
-	spin_lock_irqsave(&r->lock, flags);
-
 	DEBUG_ENT("added %d entropy credits to %s\n", nbits, r->name);
-	entropy_count = r->entropy_count;
+retry:
+	entropy_count = orig = ACCESS_ONCE(r->entropy_count);
 	entropy_count += nbits;
+
 	if (entropy_count < 0) {
 		DEBUG_ENT("negative entropy/overflow\n");
 		entropy_count = 0;
 	} else if (entropy_count > r->poolinfo->POOLBITS)
 		entropy_count = r->poolinfo->POOLBITS;
-	r->entropy_count = entropy_count;
+	if (cmpxchg(&r->entropy_count, orig, entropy_count) != orig)
+		goto retry;
+
+	if (!r->initialized && nbits > 0) {
+		r->entropy_total += nbits;
+		if (r->entropy_total > 128)
+			r->initialized = 1;
+	}
+
+	trace_credit_entropy_bits(r->name, nbits, entropy_count,
+				  r->entropy_total, _RET_IP_);
 
 	/* should we wake readers? */
 	if (r == &input_pool && entropy_count >= random_read_wakeup_thresh) {
 		wake_up_interruptible(&random_read_wait);
 		kill_fasync(&fasync, SIGIO, POLL_IN);
 	}
-	spin_unlock_irqrestore(&r->lock, flags);
 }
 
 /*********************************************************************
@@ -572,42 +636,24 @@ struct timer_rand_state {
 	unsigned dont_count_entropy:1;
 };
 
-#ifndef CONFIG_GENERIC_HARDIRQS
-
-static struct timer_rand_state *irq_timer_state[NR_IRQS];
-
-static struct timer_rand_state *get_timer_rand_state(unsigned int irq)
-{
-	return irq_timer_state[irq];
-}
-
-static void set_timer_rand_state(unsigned int irq,
-				 struct timer_rand_state *state)
-{
-	irq_timer_state[irq] = state;
-}
-
-#else
-
-static struct timer_rand_state *get_timer_rand_state(unsigned int irq)
-{
-	struct irq_desc *desc;
-
-	desc = irq_to_desc(irq);
-
-	return desc->timer_rand_state;
-}
-
-static void set_timer_rand_state(unsigned int irq,
-				 struct timer_rand_state *state)
+/*
+ * Add device- or boot-specific data to the input and nonblocking
+ * pools to help initialize them to unique values.
+ *
+ * None of this adds any entropy, it is meant to avoid the
+ * problem of the nonblocking pool having similar initial state
+ * across largely identical devices.
+ */
+void add_device_randomness(const void *buf, unsigned int size)
 {
-	struct irq_desc *desc;
+	unsigned long time = get_cycles() ^ jiffies;
 
-	desc = irq_to_desc(irq);
-
-	desc->timer_rand_state = state;
+	mix_pool_bytes(&input_pool, buf, size, NULL);
+	mix_pool_bytes(&input_pool, &time, sizeof(time), NULL);
+	mix_pool_bytes(&nonblocking_pool, buf, size, NULL);
+	mix_pool_bytes(&nonblocking_pool, &time, sizeof(time), NULL);
 }
-#endif
+EXPORT_SYMBOL(add_device_randomness);
 
 static struct timer_rand_state input_timer_state;
 
@@ -637,13 +683,9 @@ static void add_timer_randomness(struct timer_rand_state *state, unsigned num)
 		goto out;
 
 	sample.jiffies = jiffies;
-
-	/* Use arch random value, fall back to cycles */
-	if (!arch_get_random_int(&sample.cycles))
-		sample.cycles = get_cycles();
-
+	sample.cycles = get_cycles();
 	sample.num = num;
-	mix_pool_bytes(&input_pool, &sample, sizeof(sample));
+	mix_pool_bytes(&input_pool, &sample, sizeof(sample), NULL);
 
 	/*
 	 * Calculate number of bits of randomness we probably added.
@@ -700,17 +742,48 @@ void add_input_randomness(unsigned int type, unsigned int code,
 }
 EXPORT_SYMBOL_GPL(add_input_randomness);
 
-void add_interrupt_randomness(int irq)
+static DEFINE_PER_CPU(struct fast_pool, irq_randomness);
+
+void add_interrupt_randomness(int irq, int irq_flags)
 {
-	struct timer_rand_state *state;
+	struct entropy_store	*r;
+	struct fast_pool	*fast_pool = &__get_cpu_var(irq_randomness);
+	struct pt_regs		*regs = get_irq_regs();
+	unsigned long		now = jiffies;
+	__u32			input[4], cycles = get_cycles();
+
+	input[0] = cycles ^ jiffies;
+	input[1] = irq;
+	if (regs) {
+		__u64 ip = instruction_pointer(regs);
+		input[2] = ip;
+		input[3] = ip >> 32;
+	}
 
-	state = get_timer_rand_state(irq);
+	fast_mix(fast_pool, input, sizeof(input));
 
-	if (state == NULL)
+	if ((fast_pool->count & 1023) &&
+	    !time_after(now, fast_pool->last + HZ))
 		return;
 
-	DEBUG_ENT("irq event %d\n", irq);
-	add_timer_randomness(state, 0x100 + irq);
+	fast_pool->last = now;
+
+	r = nonblocking_pool.initialized ? &input_pool : &nonblocking_pool;
+	__mix_pool_bytes(r, &fast_pool->pool, sizeof(fast_pool->pool), NULL);
+	/*
+	 * If we don't have a valid cycle counter, and we see
+	 * back-to-back timer interrupts, then skip giving credit for
+	 * any entropy.
+	 */
+	if (cycles == 0) {
+		if (irq_flags & __IRQF_TIMER) {
+			if (fast_pool->last_timer_intr)
+				return;
+			fast_pool->last_timer_intr = 1;
+		} else
+			fast_pool->last_timer_intr = 0;
+	}
+	credit_entropy_bits(r, 1);
 }
 
 #ifdef CONFIG_BLOCK
@@ -742,7 +815,7 @@ static ssize_t extract_entropy(struct entropy_store *r, void *buf,
  */
 static void xfer_secondary_pool(struct entropy_store *r, size_t nbytes)
 {
-	__u32 tmp[OUTPUT_POOL_WORDS];
+	__u32	tmp[OUTPUT_POOL_WORDS];
 
 	if (r->pull && r->entropy_count < nbytes * 8 &&
 	    r->entropy_count < r->poolinfo->POOLBITS) {
@@ -761,7 +834,7 @@ static void xfer_secondary_pool(struct entropy_store *r, size_t nbytes)
 
 		bytes = extract_entropy(r->pull, tmp, bytes,
 					random_read_wakeup_thresh / 8, rsvd);
-		mix_pool_bytes(r, tmp, bytes);
+		mix_pool_bytes(r, tmp, bytes, NULL);
 		credit_entropy_bits(r, bytes*8);
 	}
 }
@@ -820,13 +893,19 @@ static size_t account(struct entropy_store *r, size_t nbytes, int min,
 static void extract_buf(struct entropy_store *r, __u8 *out)
 {
 	int i;
-	__u32 hash[5], workspace[SHA_WORKSPACE_WORDS];
+	union {
+		__u32 w[5];
+		unsigned long l[LONGS(EXTRACT_SIZE)];
+	} hash;
+	__u32 workspace[SHA_WORKSPACE_WORDS];
 	__u8 extract[64];
+	unsigned long flags;
 
 	/* Generate a hash across the pool, 16 words (512 bits) at a time */
-	sha_init(hash);
+	sha_init(hash.w);
+	spin_lock_irqsave(&r->lock, flags);
 	for (i = 0; i < r->poolinfo->poolwords; i += 16)
-		sha_transform(hash, (__u8 *)(r->pool + i), workspace);
+		sha_transform(hash.w, (__u8 *)(r->pool + i), workspace);
 
 	/*
 	 * We mix the hash back into the pool to prevent backtracking
@@ -837,13 +916,14 @@ static void extract_buf(struct entropy_store *r, __u8 *out)
 	 * brute-forcing the feedback as hard as brute-forcing the
 	 * hash.
 	 */
-	mix_pool_bytes_extract(r, hash, sizeof(hash), extract);
+	__mix_pool_bytes(r, hash.w, sizeof(hash.w), extract);
+	spin_unlock_irqrestore(&r->lock, flags);
 
 	/*
 	 * To avoid duplicates, we atomically extract a portion of the
 	 * pool while mixing, and hash one final time.
 	 */
-	sha_transform(hash, extract, workspace);
+	sha_transform(hash.w, extract, workspace);
 	memset(extract, 0, sizeof(extract));
 	memset(workspace, 0, sizeof(workspace));
 
@@ -852,20 +932,32 @@ static void extract_buf(struct entropy_store *r, __u8 *out)
 	 * pattern, we fold it in half. Thus, we always feed back
 	 * twice as much data as we output.
 	 */
-	hash[0] ^= hash[3];
-	hash[1] ^= hash[4];
-	hash[2] ^= rol32(hash[2], 16);
-	memcpy(out, hash, EXTRACT_SIZE);
-	memset(hash, 0, sizeof(hash));
+	hash.w[0] ^= hash.w[3];
+	hash.w[1] ^= hash.w[4];
+	hash.w[2] ^= rol32(hash.w[2], 16);
+
+	/*
+	 * If we have a architectural hardware random number
+	 * generator, mix that in, too.
+	 */
+	for (i = 0; i < LONGS(EXTRACT_SIZE); i++) {
+		unsigned long v;
+		if (!arch_get_random_long(&v))
+			break;
+		hash.l[i] ^= v;
+	}
+
+	memcpy(out, &hash, EXTRACT_SIZE);
+	memset(&hash, 0, sizeof(hash));
 }
 
 static ssize_t extract_entropy(struct entropy_store *r, void *buf,
-			       size_t nbytes, int min, int reserved)
+				 size_t nbytes, int min, int reserved)
 {
 	ssize_t ret = 0, i;
 	__u8 tmp[EXTRACT_SIZE];
-	unsigned long flags;
 
+	trace_extract_entropy(r->name, nbytes, r->entropy_count, _RET_IP_);
 	xfer_secondary_pool(r, nbytes);
 	nbytes = account(r, nbytes, min, reserved);
 
@@ -873,6 +965,8 @@ static ssize_t extract_entropy(struct entropy_store *r, void *buf,
 		extract_buf(r, tmp);
 
 		if (fips_enabled) {
+			unsigned long flags;
+
 			spin_lock_irqsave(&r->lock, flags);
 			if (!memcmp(tmp, r->last_data, EXTRACT_SIZE))
 				panic("Hardware RNG duplicated output!\n");
@@ -898,6 +992,7 @@ static ssize_t extract_entropy_user(struct entropy_store *r, void __user *buf,
 	ssize_t ret = 0, i;
 	__u8 tmp[EXTRACT_SIZE];
 
+	trace_extract_entropy_user(r->name, nbytes, r->entropy_count, _RET_IP_);
 	xfer_secondary_pool(r, nbytes);
 	nbytes = account(r, nbytes, 0, 0);
 
@@ -931,17 +1026,35 @@ static ssize_t extract_entropy_user(struct entropy_store *r, void __user *buf,
 
 /*
  * This function is the exported kernel interface.  It returns some
- * number of good random numbers, suitable for seeding TCP sequence
- * numbers, etc.
+ * number of good random numbers, suitable for key generation, seeding
+ * TCP sequence numbers, etc.  It does not use the hw random number
+ * generator, if available; use get_random_bytes_arch() for that.
  */
 void get_random_bytes(void *buf, int nbytes)
 {
+	extract_entropy(&nonblocking_pool, buf, nbytes, 0, 0);
+}
+EXPORT_SYMBOL(get_random_bytes);
+
+/*
+ * This function will use the architecture-specific hardware random
+ * number generator if it is available.  The arch-specific hw RNG will
+ * almost certainly be faster than what we can do in software, but it
+ * is impossible to verify that it is implemented securely (as
+ * opposed, to, say, the AES encryption of a sequence number using a
+ * key known by the NSA).  So it's useful if we need the speed, but
+ * only if we're willing to trust the hardware manufacturer not to
+ * have put in a back door.
+ */
+void get_random_bytes_arch(void *buf, int nbytes)
+{
 	char *p = buf;
 
+	trace_get_random_bytes(nbytes, _RET_IP_);
 	while (nbytes) {
 		unsigned long v;
 		int chunk = min(nbytes, (int)sizeof(unsigned long));
-		
+
 		if (!arch_get_random_long(&v))
 			break;
 		
@@ -950,9 +1063,11 @@ void get_random_bytes(void *buf, int nbytes)
 		nbytes -= chunk;
 	}
 
-	extract_entropy(&nonblocking_pool, p, nbytes, 0, 0);
+	if (nbytes)
+		extract_entropy(&nonblocking_pool, p, nbytes, 0, 0);
 }
-EXPORT_SYMBOL(get_random_bytes);
+EXPORT_SYMBOL(get_random_bytes_arch);
+
 
 /*
  * init_std_data - initialize pool with system data
@@ -966,23 +1081,30 @@ EXPORT_SYMBOL(get_random_bytes);
 static void init_std_data(struct entropy_store *r)
 {
 	int i;
-	ktime_t now;
-	unsigned long flags;
+	ktime_t now = ktime_get_real();
+	unsigned long rv;
 
-	spin_lock_irqsave(&r->lock, flags);
 	r->entropy_count = 0;
-	spin_unlock_irqrestore(&r->lock, flags);
-
-	now = ktime_get_real();
-	mix_pool_bytes(r, &now, sizeof(now));
-	for (i = r->poolinfo->POOLBYTES; i > 0; i -= sizeof flags) {
-		if (!arch_get_random_long(&flags))
+	r->entropy_total = 0;
+	mix_pool_bytes(r, &now, sizeof(now), NULL);
+	for (i = r->poolinfo->POOLBYTES; i > 0; i -= sizeof(rv)) {
+		if (!arch_get_random_long(&rv))
 			break;
-		mix_pool_bytes(r, &flags, sizeof(flags));
+		mix_pool_bytes(r, &rv, sizeof(rv), NULL);
 	}
-	mix_pool_bytes(r, utsname(), sizeof(*(utsname())));
+	mix_pool_bytes(r, utsname(), sizeof(*(utsname())), NULL);
 }
 
+/*
+ * Note that setup_arch() may call add_device_randomness()
+ * long before we get here. This allows seeding of the pools
+ * with some platform dependent data very early in the boot
+ * process. But it limits our options here. We must use
+ * statically allocated structures that already have all
+ * initializations complete at compile time. We should also
+ * take care not to overwrite the precious per platform data
+ * we were given.
+ */
 static int rand_initialize(void)
 {
 	init_std_data(&input_pool);
@@ -992,24 +1114,6 @@ static int rand_initialize(void)
 }
 module_init(rand_initialize);
 
-void rand_initialize_irq(int irq)
-{
-	struct timer_rand_state *state;
-
-	state = get_timer_rand_state(irq);
-
-	if (state)
-		return;
-
-	/*
-	 * If kzalloc returns null, we just won't use that entropy
-	 * source.
-	 */
-	state = kzalloc(sizeof(struct timer_rand_state), GFP_KERNEL);
-	if (state)
-		set_timer_rand_state(irq, state);
-}
-
 #ifdef CONFIG_BLOCK
 void rand_initialize_disk(struct gendisk *disk)
 {
@@ -1117,7 +1221,7 @@ write_pool(struct entropy_store *r, const char __user *buffer, size_t count)
 		count -= bytes;
 		p += bytes;
 
-		mix_pool_bytes(r, buf, bytes);
+		mix_pool_bytes(r, buf, bytes, NULL);
 		cond_resched();
 	}
 
@@ -1279,6 +1383,7 @@ static int proc_do_uuid(ctl_table *table, int write,
 }
 
 static int sysctl_poolsize = INPUT_POOL_WORDS * 32;
+extern ctl_table random_table[];
 ctl_table random_table[] = {
 	{
 		.procname	= "poolsize",
@@ -1344,7 +1449,7 @@ late_initcall(random_int_secret_init);
  * value is not cryptographically secure but for several uses the cost of
  * depleting entropy is too high
  */
-DEFINE_PER_CPU(__u32 [MD5_DIGEST_WORDS], get_random_int_hash);
+static DEFINE_PER_CPU(__u32 [MD5_DIGEST_WORDS], get_random_int_hash);
 unsigned int get_random_int(void)
 {
 	__u32 *hash;
diff --git a/drivers/char/sonypi.c b/drivers/char/sonypi.c
index 45713f0e7d6..f87780502b4 100644
--- a/drivers/char/sonypi.c
+++ b/drivers/char/sonypi.c
@@ -1459,7 +1459,7 @@ static int __devexit sonypi_remove(struct platform_device *dev)
 #ifdef CONFIG_PM
 static int old_camera_power;
 
-static int sonypi_suspend(struct platform_device *dev, pm_message_t state)
+static int sonypi_suspend(struct device *dev)
 {
 	old_camera_power = sonypi_device.camera_power;
 	sonypi_disable();
@@ -1467,14 +1467,16 @@ static int sonypi_suspend(struct platform_device *dev, pm_message_t state)
 	return 0;
 }
 
-static int sonypi_resume(struct platform_device *dev)
+static int sonypi_resume(struct device *dev)
 {
 	sonypi_enable(old_camera_power);
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(sonypi_pm, sonypi_suspend, sonypi_resume);
+#define SONYPI_PM	(&sonypi_pm)
 #else
-#define sonypi_suspend	NULL
-#define sonypi_resume	NULL
+#define SONYPI_PM	NULL
 #endif
 
 static void sonypi_shutdown(struct platform_device *dev)
@@ -1486,12 +1488,11 @@ static struct platform_driver sonypi_driver = {
 	.driver		= {
 		.name	= "sonypi",
 		.owner	= THIS_MODULE,
+		.pm	= SONYPI_PM,
 	},
 	.probe		= sonypi_probe,
 	.remove		= __devexit_p(sonypi_remove),
 	.shutdown	= sonypi_shutdown,
-	.suspend	= sonypi_suspend,
-	.resume		= sonypi_resume,
 };
 
 static struct platform_device *sonypi_platform_device;
diff --git a/drivers/char/tpm/tpm.c b/drivers/char/tpm/tpm.c
index ad7c7320dd1..817f0ee202b 100644
--- a/drivers/char/tpm/tpm.c
+++ b/drivers/char/tpm/tpm.c
@@ -827,10 +827,10 @@ EXPORT_SYMBOL_GPL(tpm_pcr_extend);
 int tpm_do_selftest(struct tpm_chip *chip)
 {
 	int rc;
-	u8 digest[TPM_DIGEST_SIZE];
 	unsigned int loops;
 	unsigned int delay_msec = 1000;
 	unsigned long duration;
+	struct tpm_cmd_t cmd;
 
 	duration = tpm_calc_ordinal_duration(chip,
 	                                     TPM_ORD_CONTINUE_SELFTEST);
@@ -845,7 +845,15 @@ int tpm_do_selftest(struct tpm_chip *chip)
 		return rc;
 
 	do {
-		rc = __tpm_pcr_read(chip, 0, digest);
+		/* Attempt to read a PCR value */
+		cmd.header.in = pcrread_header;
+		cmd.params.pcrread_in.pcr_idx = cpu_to_be32(0);
+		rc = tpm_transmit(chip, (u8 *) &cmd, READ_PCR_RESULT_SIZE);
+
+		if (rc < TPM_HEADER_SIZE)
+			return -EFAULT;
+
+		rc = be32_to_cpu(cmd.header.out.return_code);
 		if (rc == TPM_ERR_DISABLED || rc == TPM_ERR_DEACTIVATED) {
 			dev_info(chip->dev,
 				 "TPM is disabled/deactivated (0x%X)\n", rc);
@@ -1274,7 +1282,7 @@ static struct tpm_input_header savestate_header = {
  * We are about to suspend. Save the TPM state
  * so that it can be restored.
  */
-int tpm_pm_suspend(struct device *dev, pm_message_t pm_state)
+int tpm_pm_suspend(struct device *dev)
 {
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 	struct tpm_cmd_t cmd;
@@ -1322,6 +1330,9 @@ EXPORT_SYMBOL_GPL(tpm_pm_resume);
 
 void tpm_dev_vendor_release(struct tpm_chip *chip)
 {
+	if (!chip)
+		return;
+
 	if (chip->vendor.release)
 		chip->vendor.release(chip->dev);
 
@@ -1339,6 +1350,9 @@ void tpm_dev_release(struct device *dev)
 {
 	struct tpm_chip *chip = dev_get_drvdata(dev);
 
+	if (!chip)
+		return;
+
 	tpm_dev_vendor_release(chip);
 
 	chip->release(dev);
@@ -1405,15 +1419,12 @@ struct tpm_chip *tpm_register_hardware(struct device *dev,
 			"unable to misc_register %s, minor %d\n",
 			chip->vendor.miscdev.name,
 			chip->vendor.miscdev.minor);
-		put_device(chip->dev);
-		return NULL;
+		goto put_device;
 	}
 
 	if (sysfs_create_group(&dev->kobj, chip->vendor.attr_group)) {
 		misc_deregister(&chip->vendor.miscdev);
-		put_device(chip->dev);
-
-		return NULL;
+		goto put_device;
 	}
 
 	chip->bios_dir = tpm_bios_log_setup(devname);
@@ -1425,6 +1436,8 @@ struct tpm_chip *tpm_register_hardware(struct device *dev,
 
 	return chip;
 
+put_device:
+	put_device(chip->dev);
 out_free:
 	kfree(chip);
 	kfree(devname);
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index b1c5280ac15..917f727e674 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -299,7 +299,7 @@ extern ssize_t tpm_write(struct file *, const char __user *, size_t,
 			 loff_t *);
 extern ssize_t tpm_read(struct file *, char __user *, size_t, loff_t *);
 extern void tpm_remove_hardware(struct device *);
-extern int tpm_pm_suspend(struct device *, pm_message_t);
+extern int tpm_pm_suspend(struct device *);
 extern int tpm_pm_resume(struct device *);
 extern int wait_for_tpm_stat(struct tpm_chip *, u8, unsigned long,
 			     wait_queue_head_t *);
diff --git a/drivers/char/tpm/tpm_atmel.c b/drivers/char/tpm/tpm_atmel.c
index c64a1bc6534..678d57019dc 100644
--- a/drivers/char/tpm/tpm_atmel.c
+++ b/drivers/char/tpm/tpm_atmel.c
@@ -168,22 +168,14 @@ static void atml_plat_remove(void)
 	}
 }
 
-static int tpm_atml_suspend(struct platform_device *dev, pm_message_t msg)
-{
-	return tpm_pm_suspend(&dev->dev, msg);
-}
+static SIMPLE_DEV_PM_OPS(tpm_atml_pm, tpm_pm_suspend, tpm_pm_resume);
 
-static int tpm_atml_resume(struct platform_device *dev)
-{
-	return tpm_pm_resume(&dev->dev);
-}
 static struct platform_driver atml_drv = {
 	.driver = {
 		.name = "tpm_atmel",
 		.owner		= THIS_MODULE,
+		.pm		= &tpm_atml_pm,
 	},
-	.suspend = tpm_atml_suspend,
-	.resume = tpm_atml_resume,
 };
 
 static int __init init_atmel(void)
diff --git a/drivers/char/tpm/tpm_infineon.c b/drivers/char/tpm/tpm_infineon.c
index 76da32e11f1..3251a44e8ce 100644
--- a/drivers/char/tpm/tpm_infineon.c
+++ b/drivers/char/tpm/tpm_infineon.c
@@ -4,8 +4,8 @@
  * SLD 9630 TT 1.1 and SLB 9635 TT 1.2 Trusted Platform Module
  * Specifications at www.trustedcomputinggroup.org
  *
- * Copyright (C) 2005, Marcel Selhorst <m.selhorst@sirrix.com>
- * Sirrix AG - security technologies, http://www.sirrix.com and
+ * Copyright (C) 2005, Marcel Selhorst <tpmdd@selhorst.net>
+ * Sirrix AG - security technologies <tpmdd@sirrix.com> and
  * Applied Data Security Group, Ruhr-University Bochum, Germany
  * Project-Homepage: http://www.trust.rub.de/projects/linux-device-driver-infineon-tpm/ 
  *
@@ -671,7 +671,7 @@ static void __exit cleanup_inf(void)
 module_init(init_inf);
 module_exit(cleanup_inf);
 
-MODULE_AUTHOR("Marcel Selhorst <m.selhorst@sirrix.com>");
+MODULE_AUTHOR("Marcel Selhorst <tpmdd@sirrix.com>");
 MODULE_DESCRIPTION("Driver for Infineon TPM SLD 9630 TT 1.1 / SLB 9635 TT 1.2");
 MODULE_VERSION("1.9.2");
 MODULE_LICENSE("GPL");
diff --git a/drivers/char/tpm/tpm_nsc.c b/drivers/char/tpm/tpm_nsc.c
index 4d2464871ad..640c9a427b5 100644
--- a/drivers/char/tpm/tpm_nsc.c
+++ b/drivers/char/tpm/tpm_nsc.c
@@ -274,22 +274,13 @@ static void tpm_nsc_remove(struct device *dev)
 	}
 }
 
-static int tpm_nsc_suspend(struct platform_device *dev, pm_message_t msg)
-{
-	return tpm_pm_suspend(&dev->dev, msg);
-}
-
-static int tpm_nsc_resume(struct platform_device *dev)
-{
-	return tpm_pm_resume(&dev->dev);
-}
+static SIMPLE_DEV_PM_OPS(tpm_nsc_pm, tpm_pm_suspend, tpm_pm_resume);
 
 static struct platform_driver nsc_drv = {
-	.suspend         = tpm_nsc_suspend,
-	.resume          = tpm_nsc_resume,
 	.driver          = {
 		.name    = "tpm_nsc",
 		.owner   = THIS_MODULE,
+		.pm      = &tpm_nsc_pm,
 	},
 };
 
diff --git a/drivers/char/tpm/tpm_tis.c b/drivers/char/tpm/tpm_tis.c
index d2a70cae76d..89682fa8801 100644
--- a/drivers/char/tpm/tpm_tis.c
+++ b/drivers/char/tpm/tpm_tis.c
@@ -750,7 +750,7 @@ static int __devinit tpm_tis_pnp_init(struct pnp_dev *pnp_dev,
 
 static int tpm_tis_pnp_suspend(struct pnp_dev *dev, pm_message_t msg)
 {
-	return tpm_pm_suspend(&dev->dev, msg);
+	return tpm_pm_suspend(&dev->dev);
 }
 
 static int tpm_tis_pnp_resume(struct pnp_dev *dev)
@@ -806,27 +806,25 @@ module_param_string(hid, tpm_pnp_tbl[TIS_HID_USR_IDX].id,
 		    sizeof(tpm_pnp_tbl[TIS_HID_USR_IDX].id), 0444);
 MODULE_PARM_DESC(hid, "Set additional specific HID for this driver to probe");
 #endif
-static int tpm_tis_suspend(struct platform_device *dev, pm_message_t msg)
-{
-	return tpm_pm_suspend(&dev->dev, msg);
-}
 
-static int tpm_tis_resume(struct platform_device *dev)
+static int tpm_tis_resume(struct device *dev)
 {
-	struct tpm_chip *chip = dev_get_drvdata(&dev->dev);
+	struct tpm_chip *chip = dev_get_drvdata(dev);
 
 	if (chip->vendor.irq)
 		tpm_tis_reenable_interrupts(chip);
 
-	return tpm_pm_resume(&dev->dev);
+	return tpm_pm_resume(dev);
 }
+
+static SIMPLE_DEV_PM_OPS(tpm_tis_pm, tpm_pm_suspend, tpm_tis_resume);
+
 static struct platform_driver tis_drv = {
 	.driver = {
 		.name = "tpm_tis",
 		.owner		= THIS_MODULE,
+		.pm		= &tpm_tis_pm,
 	},
-	.suspend = tpm_tis_suspend,
-	.resume = tpm_tis_resume,
 };
 
 static struct platform_device *pdev;
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 4864407e3fc..7f0b5ca7851 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -25,7 +25,6 @@ menu "Common Clock Framework"
 
 config COMMON_CLK_DEBUG
 	bool "DebugFS representation of clock tree"
-	depends on COMMON_CLK
 	select DEBUG_FS
 	---help---
 	  Creates a directory hierchy in debugfs for visualizing the clk
@@ -34,4 +33,11 @@ config COMMON_CLK_DEBUG
 	  clk_flags, clk_prepare_count, clk_enable_count &
 	  clk_notifier_count.
 
+config COMMON_CLK_WM831X
+	tristate "Clock driver for WM831x/2x PMICs"
+	depends on MFD_WM831X
+	---help---
+          Supports the clocking subsystem of the WM831x/2x series of
+	  PMICs from Wolfson Microlectronics.
+
 endmenu
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index b9a5158a30b..5869ea38705 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -1,7 +1,15 @@
-
+# common clock types
 obj-$(CONFIG_CLKDEV_LOOKUP)	+= clkdev.o
 obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-fixed-rate.o clk-gate.o \
 				   clk-mux.o clk-divider.o clk-fixed-factor.o
 # SoCs specific
+obj-$(CONFIG_ARCH_NOMADIK)	+= clk-nomadik.o
+obj-$(CONFIG_ARCH_HIGHBANK)	+= clk-highbank.o
 obj-$(CONFIG_ARCH_MXS)		+= mxs/
+obj-$(CONFIG_ARCH_SOCFPGA)	+= socfpga/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
+obj-$(CONFIG_ARCH_U300)		+= clk-u300.o
+obj-$(CONFIG_ARCH_INTEGRATOR)	+= versatile/
+
+# Chip specific
+obj-$(CONFIG_COMMON_CLK_WM831X) += clk-wm831x.o
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index 8ea11b44452..a9204c69148 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -30,18 +30,89 @@
 #define to_clk_divider(_hw) container_of(_hw, struct clk_divider, hw)
 
 #define div_mask(d)	((1 << (d->width)) - 1)
+#define is_power_of_two(i)	!(i & ~i)
+
+static unsigned int _get_table_maxdiv(const struct clk_div_table *table)
+{
+	unsigned int maxdiv = 0;
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->div > maxdiv)
+			maxdiv = clkt->div;
+	return maxdiv;
+}
+
+static unsigned int _get_maxdiv(struct clk_divider *divider)
+{
+	if (divider->flags & CLK_DIVIDER_ONE_BASED)
+		return div_mask(divider);
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		return 1 << div_mask(divider);
+	if (divider->table)
+		return _get_table_maxdiv(divider->table);
+	return div_mask(divider) + 1;
+}
+
+static unsigned int _get_table_div(const struct clk_div_table *table,
+							unsigned int val)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->val == val)
+			return clkt->div;
+	return 0;
+}
+
+static unsigned int _get_div(struct clk_divider *divider, unsigned int val)
+{
+	if (divider->flags & CLK_DIVIDER_ONE_BASED)
+		return val;
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		return 1 << val;
+	if (divider->table)
+		return _get_table_div(divider->table, val);
+	return val + 1;
+}
+
+static unsigned int _get_table_val(const struct clk_div_table *table,
+							unsigned int div)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->div == div)
+			return clkt->val;
+	return 0;
+}
+
+static unsigned int _get_val(struct clk_divider *divider, u8 div)
+{
+	if (divider->flags & CLK_DIVIDER_ONE_BASED)
+		return div;
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		return __ffs(div);
+	if (divider->table)
+		return  _get_table_val(divider->table, div);
+	return div - 1;
+}
 
 static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clk_divider *divider = to_clk_divider(hw);
-	unsigned int div;
+	unsigned int div, val;
 
-	div = readl(divider->reg) >> divider->shift;
-	div &= div_mask(divider);
+	val = readl(divider->reg) >> divider->shift;
+	val &= div_mask(divider);
 
-	if (!(divider->flags & CLK_DIVIDER_ONE_BASED))
-		div++;
+	div = _get_div(divider, val);
+	if (!div) {
+		WARN(1, "%s: Invalid divisor for clock %s\n", __func__,
+						__clk_get_name(hw->clk));
+		return parent_rate;
+	}
 
 	return parent_rate / div;
 }
@@ -52,6 +123,26 @@ static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
  */
 #define MULT_ROUND_UP(r, m) ((r) * (m) + (m) - 1)
 
+static bool _is_valid_table_div(const struct clk_div_table *table,
+							 unsigned int div)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->div == div)
+			return true;
+	return false;
+}
+
+static bool _is_valid_div(struct clk_divider *divider, unsigned int div)
+{
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		return is_power_of_two(div);
+	if (divider->table)
+		return _is_valid_table_div(divider->table, div);
+	return true;
+}
+
 static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 		unsigned long *best_parent_rate)
 {
@@ -62,10 +153,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 	if (!rate)
 		rate = 1;
 
-	maxdiv = (1 << divider->width);
-
-	if (divider->flags & CLK_DIVIDER_ONE_BASED)
-		maxdiv--;
+	maxdiv = _get_maxdiv(divider);
 
 	if (!(__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT)) {
 		parent_rate = *best_parent_rate;
@@ -82,6 +170,8 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 	maxdiv = min(ULONG_MAX / rate, maxdiv);
 
 	for (i = 1; i <= maxdiv; i++) {
+		if (!_is_valid_div(divider, i))
+			continue;
 		parent_rate = __clk_round_rate(__clk_get_parent(hw->clk),
 				MULT_ROUND_UP(rate, i));
 		now = parent_rate / i;
@@ -93,9 +183,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 	}
 
 	if (!bestdiv) {
-		bestdiv = (1 << divider->width);
-		if (divider->flags & CLK_DIVIDER_ONE_BASED)
-			bestdiv--;
+		bestdiv = _get_maxdiv(divider);
 		*best_parent_rate = __clk_round_rate(__clk_get_parent(hw->clk), 1);
 	}
 
@@ -115,24 +203,22 @@ static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
 				unsigned long parent_rate)
 {
 	struct clk_divider *divider = to_clk_divider(hw);
-	unsigned int div;
+	unsigned int div, value;
 	unsigned long flags = 0;
 	u32 val;
 
 	div = parent_rate / rate;
+	value = _get_val(divider, div);
 
-	if (!(divider->flags & CLK_DIVIDER_ONE_BASED))
-		div--;
-
-	if (div > div_mask(divider))
-		div = div_mask(divider);
+	if (value > div_mask(divider))
+		value = div_mask(divider);
 
 	if (divider->lock)
 		spin_lock_irqsave(divider->lock, flags);
 
 	val = readl(divider->reg);
 	val &= ~(div_mask(divider) << divider->shift);
-	val |= div << divider->shift;
+	val |= value << divider->shift;
 	writel(val, divider->reg);
 
 	if (divider->lock)
@@ -148,22 +234,11 @@ const struct clk_ops clk_divider_ops = {
 };
 EXPORT_SYMBOL_GPL(clk_divider_ops);
 
-/**
- * clk_register_divider - register a divider clock with the clock framework
- * @dev: device registering this clock
- * @name: name of this clock
- * @parent_name: name of clock's parent
- * @flags: framework-specific flags
- * @reg: register address to adjust divider
- * @shift: number of bits to shift the bitfield
- * @width: width of the bitfield
- * @clk_divider_flags: divider-specific flags for this clock
- * @lock: shared register lock for this clock
- */
-struct clk *clk_register_divider(struct device *dev, const char *name,
+static struct clk *_register_divider(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		void __iomem *reg, u8 shift, u8 width,
-		u8 clk_divider_flags, spinlock_t *lock)
+		u8 clk_divider_flags, const struct clk_div_table *table,
+		spinlock_t *lock)
 {
 	struct clk_divider *div;
 	struct clk *clk;
@@ -178,7 +253,7 @@ struct clk *clk_register_divider(struct device *dev, const char *name,
 
 	init.name = name;
 	init.ops = &clk_divider_ops;
-	init.flags = flags;
+	init.flags = flags | CLK_IS_BASIC;
 	init.parent_names = (parent_name ? &parent_name: NULL);
 	init.num_parents = (parent_name ? 1 : 0);
 
@@ -189,6 +264,7 @@ struct clk *clk_register_divider(struct device *dev, const char *name,
 	div->flags = clk_divider_flags;
 	div->lock = lock;
 	div->hw.init = &init;
+	div->table = table;
 
 	/* register the clock */
 	clk = clk_register(dev, &div->hw);
@@ -198,3 +274,48 @@ struct clk *clk_register_divider(struct device *dev, const char *name,
 
 	return clk;
 }
+
+/**
+ * clk_register_divider - register a divider clock with the clock framework
+ * @dev: device registering this clock
+ * @name: name of this clock
+ * @parent_name: name of clock's parent
+ * @flags: framework-specific flags
+ * @reg: register address to adjust divider
+ * @shift: number of bits to shift the bitfield
+ * @width: width of the bitfield
+ * @clk_divider_flags: divider-specific flags for this clock
+ * @lock: shared register lock for this clock
+ */
+struct clk *clk_register_divider(struct device *dev, const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *reg, u8 shift, u8 width,
+		u8 clk_divider_flags, spinlock_t *lock)
+{
+	return _register_divider(dev, name, parent_name, flags, reg, shift,
+			width, clk_divider_flags, NULL, lock);
+}
+
+/**
+ * clk_register_divider_table - register a table based divider clock with
+ * the clock framework
+ * @dev: device registering this clock
+ * @name: name of this clock
+ * @parent_name: name of clock's parent
+ * @flags: framework-specific flags
+ * @reg: register address to adjust divider
+ * @shift: number of bits to shift the bitfield
+ * @width: width of the bitfield
+ * @clk_divider_flags: divider-specific flags for this clock
+ * @table: array of divider/value pairs ending with a div set to 0
+ * @lock: shared register lock for this clock
+ */
+struct clk *clk_register_divider_table(struct device *dev, const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *reg, u8 shift, u8 width,
+		u8 clk_divider_flags, const struct clk_div_table *table,
+		spinlock_t *lock)
+{
+	return _register_divider(dev, name, parent_name, flags, reg, shift,
+			width, clk_divider_flags, table, lock);
+}
diff --git a/drivers/clk/clk-fixed-factor.c b/drivers/clk/clk-fixed-factor.c
index c8c003e217a..a4899855c0f 100644
--- a/drivers/clk/clk-fixed-factor.c
+++ b/drivers/clk/clk-fixed-factor.c
@@ -82,7 +82,7 @@ struct clk *clk_register_fixed_factor(struct device *dev, const char *name,
 
 	init.name = name;
 	init.ops = &clk_fixed_factor_ops;
-	init.flags = flags;
+	init.flags = flags | CLK_IS_BASIC;
 	init.parent_names = &parent_name;
 	init.num_parents = 1;
 
diff --git a/drivers/clk/clk-fixed-rate.c b/drivers/clk/clk-fixed-rate.c
index cbd24622978..f5ec0eebd4d 100644
--- a/drivers/clk/clk-fixed-rate.c
+++ b/drivers/clk/clk-fixed-rate.c
@@ -14,6 +14,7 @@
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/err.h>
+#include <linux/of.h>
 
 /*
  * DOC: basic fixed-rate clock that cannot gate
@@ -63,7 +64,7 @@ struct clk *clk_register_fixed_rate(struct device *dev, const char *name,
 
 	init.name = name;
 	init.ops = &clk_fixed_rate_ops;
-	init.flags = flags;
+	init.flags = flags | CLK_IS_BASIC;
 	init.parent_names = (parent_name ? &parent_name: NULL);
 	init.num_parents = (parent_name ? 1 : 0);
 
@@ -79,3 +80,25 @@ struct clk *clk_register_fixed_rate(struct device *dev, const char *name,
 
 	return clk;
 }
+
+#ifdef CONFIG_OF
+/**
+ * of_fixed_clk_setup() - Setup function for simple fixed rate clock
+ */
+void __init of_fixed_clk_setup(struct device_node *node)
+{
+	struct clk *clk;
+	const char *clk_name = node->name;
+	u32 rate;
+
+	if (of_property_read_u32(node, "clock-frequency", &rate))
+		return;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	clk = clk_register_fixed_rate(NULL, clk_name, NULL, CLK_IS_ROOT, rate);
+	if (clk)
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+}
+EXPORT_SYMBOL_GPL(of_fixed_clk_setup);
+#endif
diff --git a/drivers/clk/clk-gate.c b/drivers/clk/clk-gate.c
index 578465e04be..15114febfd9 100644
--- a/drivers/clk/clk-gate.c
+++ b/drivers/clk/clk-gate.c
@@ -130,7 +130,7 @@ struct clk *clk_register_gate(struct device *dev, const char *name,
 
 	init.name = name;
 	init.ops = &clk_gate_ops;
-	init.flags = flags;
+	init.flags = flags | CLK_IS_BASIC;
 	init.parent_names = (parent_name ? &parent_name: NULL);
 	init.num_parents = (parent_name ? 1 : 0);
 
diff --git a/drivers/clk/clk-highbank.c b/drivers/clk/clk-highbank.c
new file mode 100644
index 00000000000..52fecadf004
--- /dev/null
+++ b/drivers/clk/clk-highbank.c
@@ -0,0 +1,346 @@
+/*
+ * Copyright 2011-2012 Calxeda, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+extern void __iomem *sregs_base;
+
+#define HB_PLL_LOCK_500		0x20000000
+#define HB_PLL_LOCK		0x10000000
+#define HB_PLL_DIVF_SHIFT	20
+#define HB_PLL_DIVF_MASK	0x0ff00000
+#define HB_PLL_DIVQ_SHIFT	16
+#define HB_PLL_DIVQ_MASK	0x00070000
+#define HB_PLL_DIVR_SHIFT	8
+#define HB_PLL_DIVR_MASK	0x00001f00
+#define HB_PLL_RANGE_SHIFT	4
+#define HB_PLL_RANGE_MASK	0x00000070
+#define HB_PLL_BYPASS		0x00000008
+#define HB_PLL_RESET		0x00000004
+#define HB_PLL_EXT_BYPASS	0x00000002
+#define HB_PLL_EXT_ENA		0x00000001
+
+#define HB_PLL_VCO_MIN_FREQ	2133000000
+#define HB_PLL_MAX_FREQ		HB_PLL_VCO_MIN_FREQ
+#define HB_PLL_MIN_FREQ		(HB_PLL_VCO_MIN_FREQ / 64)
+
+#define HB_A9_BCLK_DIV_MASK	0x00000006
+#define HB_A9_BCLK_DIV_SHIFT	1
+#define HB_A9_PCLK_DIV		0x00000001
+
+struct hb_clk {
+        struct clk_hw	hw;
+	void __iomem	*reg;
+	char *parent_name;
+};
+#define to_hb_clk(p) container_of(p, struct hb_clk, hw)
+
+static int clk_pll_prepare(struct clk_hw *hwclk)
+	{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 reg;
+
+	reg = readl(hbclk->reg);
+	reg &= ~HB_PLL_RESET;
+	writel(reg, hbclk->reg);
+
+	while ((readl(hbclk->reg) & HB_PLL_LOCK) == 0)
+		;
+	while ((readl(hbclk->reg) & HB_PLL_LOCK_500) == 0)
+		;
+
+	return 0;
+}
+
+static void clk_pll_unprepare(struct clk_hw *hwclk)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 reg;
+
+	reg = readl(hbclk->reg);
+	reg |= HB_PLL_RESET;
+	writel(reg, hbclk->reg);
+}
+
+static int clk_pll_enable(struct clk_hw *hwclk)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 reg;
+
+	reg = readl(hbclk->reg);
+	reg |= HB_PLL_EXT_ENA;
+	writel(reg, hbclk->reg);
+
+	return 0;
+}
+
+static void clk_pll_disable(struct clk_hw *hwclk)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 reg;
+
+	reg = readl(hbclk->reg);
+	reg &= ~HB_PLL_EXT_ENA;
+	writel(reg, hbclk->reg);
+}
+
+static unsigned long clk_pll_recalc_rate(struct clk_hw *hwclk,
+					 unsigned long parent_rate)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	unsigned long divf, divq, vco_freq, reg;
+
+	reg = readl(hbclk->reg);
+	if (reg & HB_PLL_EXT_BYPASS)
+		return parent_rate;
+
+	divf = (reg & HB_PLL_DIVF_MASK) >> HB_PLL_DIVF_SHIFT;
+	divq = (reg & HB_PLL_DIVQ_MASK) >> HB_PLL_DIVQ_SHIFT;
+	vco_freq = parent_rate * (divf + 1);
+
+	return vco_freq / (1 << divq);
+}
+
+static void clk_pll_calc(unsigned long rate, unsigned long ref_freq,
+			u32 *pdivq, u32 *pdivf)
+{
+	u32 divq, divf;
+	unsigned long vco_freq;
+
+	if (rate < HB_PLL_MIN_FREQ)
+		rate = HB_PLL_MIN_FREQ;
+	if (rate > HB_PLL_MAX_FREQ)
+		rate = HB_PLL_MAX_FREQ;
+
+	for (divq = 1; divq <= 6; divq++) {
+		if ((rate * (1 << divq)) >= HB_PLL_VCO_MIN_FREQ)
+			break;
+	}
+
+	vco_freq = rate * (1 << divq);
+	divf = (vco_freq + (ref_freq / 2)) / ref_freq;
+	divf--;
+
+	*pdivq = divq;
+	*pdivf = divf;
+}
+
+static long clk_pll_round_rate(struct clk_hw *hwclk, unsigned long rate,
+			       unsigned long *parent_rate)
+{
+	u32 divq, divf;
+	unsigned long ref_freq = *parent_rate;
+
+	clk_pll_calc(rate, ref_freq, &divq, &divf);
+
+	return (ref_freq * (divf + 1)) / (1 << divq);
+}
+
+static int clk_pll_set_rate(struct clk_hw *hwclk, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 divq, divf;
+	u32 reg;
+
+	clk_pll_calc(rate, parent_rate, &divq, &divf);
+
+	reg = readl(hbclk->reg);
+	if (divf != ((reg & HB_PLL_DIVF_MASK) >> HB_PLL_DIVF_SHIFT)) {
+		/* Need to re-lock PLL, so put it into bypass mode */
+		reg |= HB_PLL_EXT_BYPASS;
+		writel(reg | HB_PLL_EXT_BYPASS, hbclk->reg);
+
+		writel(reg | HB_PLL_RESET, hbclk->reg);
+		reg &= ~(HB_PLL_DIVF_MASK | HB_PLL_DIVQ_MASK);
+		reg |= (divf << HB_PLL_DIVF_SHIFT) | (divq << HB_PLL_DIVQ_SHIFT);
+		writel(reg | HB_PLL_RESET, hbclk->reg);
+		writel(reg, hbclk->reg);
+
+		while ((readl(hbclk->reg) & HB_PLL_LOCK) == 0)
+			;
+		while ((readl(hbclk->reg) & HB_PLL_LOCK_500) == 0)
+			;
+		reg |= HB_PLL_EXT_ENA;
+		reg &= ~HB_PLL_EXT_BYPASS;
+	} else {
+		reg &= ~HB_PLL_DIVQ_MASK;
+		reg |= divq << HB_PLL_DIVQ_SHIFT;
+	}
+	writel(reg, hbclk->reg);
+
+	return 0;
+}
+
+static const struct clk_ops clk_pll_ops = {
+	.prepare = clk_pll_prepare,
+	.unprepare = clk_pll_unprepare,
+	.enable = clk_pll_enable,
+	.disable = clk_pll_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+	.round_rate = clk_pll_round_rate,
+	.set_rate = clk_pll_set_rate,
+};
+
+static unsigned long clk_cpu_periphclk_recalc_rate(struct clk_hw *hwclk,
+						   unsigned long parent_rate)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 div = (readl(hbclk->reg) & HB_A9_PCLK_DIV) ? 8 : 4;
+	return parent_rate / div;
+}
+
+static const struct clk_ops a9periphclk_ops = {
+	.recalc_rate = clk_cpu_periphclk_recalc_rate,
+};
+
+static unsigned long clk_cpu_a9bclk_recalc_rate(struct clk_hw *hwclk,
+						unsigned long parent_rate)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 div = (readl(hbclk->reg) & HB_A9_BCLK_DIV_MASK) >> HB_A9_BCLK_DIV_SHIFT;
+
+	return parent_rate / (div + 2);
+}
+
+static const struct clk_ops a9bclk_ops = {
+	.recalc_rate = clk_cpu_a9bclk_recalc_rate,
+};
+
+static unsigned long clk_periclk_recalc_rate(struct clk_hw *hwclk,
+					     unsigned long parent_rate)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 div;
+
+	div = readl(hbclk->reg) & 0x1f;
+	div++;
+	div *= 2;
+
+	return parent_rate / div;
+}
+
+static long clk_periclk_round_rate(struct clk_hw *hwclk, unsigned long rate,
+				   unsigned long *parent_rate)
+{
+	u32 div;
+
+	div = *parent_rate / rate;
+	div++;
+	div &= ~0x1;
+
+	return *parent_rate / div;
+}
+
+static int clk_periclk_set_rate(struct clk_hw *hwclk, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct hb_clk *hbclk = to_hb_clk(hwclk);
+	u32 div;
+
+	div = parent_rate / rate;
+	if (div & 0x1)
+		return -EINVAL;
+
+	writel(div >> 1, hbclk->reg);
+	return 0;
+}
+
+static const struct clk_ops periclk_ops = {
+	.recalc_rate = clk_periclk_recalc_rate,
+	.round_rate = clk_periclk_round_rate,
+	.set_rate = clk_periclk_set_rate,
+};
+
+static __init struct clk *hb_clk_init(struct device_node *node, const struct clk_ops *ops)
+{
+	u32 reg;
+	struct clk *clk;
+	struct hb_clk *hb_clk;
+	const char *clk_name = node->name;
+	const char *parent_name;
+	struct clk_init_data init;
+	int rc;
+
+	rc = of_property_read_u32(node, "reg", &reg);
+	if (WARN_ON(rc))
+		return NULL;
+
+	hb_clk = kzalloc(sizeof(*hb_clk), GFP_KERNEL);
+	if (WARN_ON(!hb_clk))
+		return NULL;
+
+	hb_clk->reg = sregs_base + reg;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	init.name = clk_name;
+	init.ops = ops;
+	init.flags = 0;
+	parent_name = of_clk_get_parent_name(node, 0);
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	hb_clk->hw.init = &init;
+
+	clk = clk_register(NULL, &hb_clk->hw);
+	if (WARN_ON(IS_ERR(clk))) {
+		kfree(hb_clk);
+		return NULL;
+	}
+	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	return clk;
+}
+
+static void __init hb_pll_init(struct device_node *node)
+{
+	hb_clk_init(node, &clk_pll_ops);
+}
+
+static void __init hb_a9periph_init(struct device_node *node)
+{
+	hb_clk_init(node, &a9periphclk_ops);
+}
+
+static void __init hb_a9bus_init(struct device_node *node)
+{
+	struct clk *clk = hb_clk_init(node, &a9bclk_ops);
+	clk_prepare_enable(clk);
+}
+
+static void __init hb_emmc_init(struct device_node *node)
+{
+	hb_clk_init(node, &periclk_ops);
+}
+
+static const __initconst struct of_device_id clk_match[] = {
+	{ .compatible = "fixed-clock", .data = of_fixed_clk_setup, },
+	{ .compatible = "calxeda,hb-pll-clock", .data = hb_pll_init, },
+	{ .compatible = "calxeda,hb-a9periph-clock", .data = hb_a9periph_init, },
+	{ .compatible = "calxeda,hb-a9bus-clock", .data = hb_a9bus_init, },
+	{ .compatible = "calxeda,hb-emmc-clock", .data = hb_emmc_init, },
+	{}
+};
+
+void __init highbank_clocks_init(void)
+{
+	of_clk_init(clk_match);
+}
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index fd36a8ea73d..508c032edce 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -106,7 +106,7 @@ struct clk *clk_register_mux(struct device *dev, const char *name,
 
 	init.name = name;
 	init.ops = &clk_mux_ops;
-	init.flags = flags;
+	init.flags = flags | CLK_IS_BASIC;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
 
diff --git a/drivers/clk/clk-nomadik.c b/drivers/clk/clk-nomadik.c
new file mode 100644
index 00000000000..517a8ff7121
--- /dev/null
+++ b/drivers/clk/clk-nomadik.c
@@ -0,0 +1,47 @@
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+
+/*
+ * The Nomadik clock tree is described in the STN8815A12 DB V4.2
+ * reference manual for the chip, page 94 ff.
+ */
+
+void __init nomadik_clk_init(void)
+{
+	struct clk *clk;
+
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+	clk_register_clkdev(clk, NULL, "gpio.0");
+	clk_register_clkdev(clk, NULL, "gpio.1");
+	clk_register_clkdev(clk, NULL, "gpio.2");
+	clk_register_clkdev(clk, NULL, "gpio.3");
+	clk_register_clkdev(clk, NULL, "rng");
+
+	/*
+	 * The 2.4 MHz TIMCLK reference clock is active at boot time, this is
+	 * actually the MXTALCLK @19.2 MHz divided by 8. This clock is used
+	 * by the timers and watchdog. See page 105 ff.
+	 */
+	clk = clk_register_fixed_rate(NULL, "TIMCLK", NULL, CLK_IS_ROOT,
+				      2400000);
+	clk_register_clkdev(clk, NULL, "mtu0");
+	clk_register_clkdev(clk, NULL, "mtu1");
+
+	/*
+	 * At boot time, PLL2 is set to generate a set of fixed clocks,
+	 * one of them is CLK48, the 48 MHz clock, routed to the UART, MMC/SD
+	 * I2C, IrDA, USB and SSP blocks.
+	 */
+	clk = clk_register_fixed_rate(NULL, "CLK48", NULL, CLK_IS_ROOT,
+				      48000000);
+	clk_register_clkdev(clk, NULL, "uart0");
+	clk_register_clkdev(clk, NULL, "uart1");
+	clk_register_clkdev(clk, NULL, "mmci");
+	clk_register_clkdev(clk, NULL, "ssp");
+	clk_register_clkdev(clk, NULL, "nmk-i2c.0");
+	clk_register_clkdev(clk, NULL, "nmk-i2c.1");
+}
diff --git a/drivers/clk/clk-u300.c b/drivers/clk/clk-u300.c
new file mode 100644
index 00000000000..a15f7928fb1
--- /dev/null
+++ b/drivers/clk/clk-u300.c
@@ -0,0 +1,746 @@
+/*
+ * U300 clock implementation
+ * Copyright (C) 2007-2012 ST-Ericsson AB
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Linus Walleij <linus.walleij@stericsson.com>
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
+ */
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+#include <linux/spinlock.h>
+#include <mach/syscon.h>
+
+/*
+ * The clocking hierarchy currently looks like this.
+ * NOTE: the idea is NOT to show how the clocks are routed on the chip!
+ * The ideas is to show dependencies, so a clock higher up in the
+ * hierarchy has to be on in order for another clock to be on. Now,
+ * both CPU and DMA can actually be on top of the hierarchy, and that
+ * is not modeled currently. Instead we have the backbone AMBA bus on
+ * top. This bus cannot be programmed in any way but conceptually it
+ * needs to be active for the bridges and devices to transport data.
+ *
+ * Please be aware that a few clocks are hw controlled, which mean that
+ * the hw itself can turn on/off or change the rate of the clock when
+ * needed!
+ *
+ *  AMBA bus
+ *  |
+ *  +- CPU
+ *  +- FSMC NANDIF NAND Flash interface
+ *  +- SEMI Shared Memory interface
+ *  +- ISP Image Signal Processor (U335 only)
+ *  +- CDS (U335 only)
+ *  +- DMA Direct Memory Access Controller
+ *  +- AAIF APP/ACC Inteface (Mobile Scalable Link, MSL)
+ *  +- APEX
+ *  +- VIDEO_ENC AVE2/3 Video Encoder
+ *  +- XGAM Graphics Accelerator Controller
+ *  +- AHB
+ *  |
+ *  +- ahb:0 AHB Bridge
+ *  |  |
+ *  |  +- ahb:1 INTCON Interrupt controller
+ *  |  +- ahb:3 MSPRO  Memory Stick Pro controller
+ *  |  +- ahb:4 EMIF   External Memory interface
+ *  |
+ *  +- fast:0 FAST bridge
+ *  |  |
+ *  |  +- fast:1 MMCSD MMC/SD card reader controller
+ *  |  +- fast:2 I2S0  PCM I2S channel 0 controller
+ *  |  +- fast:3 I2S1  PCM I2S channel 1 controller
+ *  |  +- fast:4 I2C0  I2C channel 0 controller
+ *  |  +- fast:5 I2C1  I2C channel 1 controller
+ *  |  +- fast:6 SPI   SPI controller
+ *  |  +- fast:7 UART1 Secondary UART (U335 only)
+ *  |
+ *  +- slow:0 SLOW bridge
+ *     |
+ *     +- slow:1 SYSCON (not possible to control)
+ *     +- slow:2 WDOG Watchdog
+ *     +- slow:3 UART0 primary UART
+ *     +- slow:4 TIMER_APP Application timer - used in Linux
+ *     +- slow:5 KEYPAD controller
+ *     +- slow:6 GPIO controller
+ *     +- slow:7 RTC controller
+ *     +- slow:8 BT Bus Tracer (not used currently)
+ *     +- slow:9 EH Event Handler (not used currently)
+ *     +- slow:a TIMER_ACC Access style timer (not used currently)
+ *     +- slow:b PPM (U335 only, what is that?)
+ */
+
+/* Global syscon virtual base */
+static void __iomem *syscon_vbase;
+
+/**
+ * struct clk_syscon - U300 syscon clock
+ * @hw: corresponding clock hardware entry
+ * @hw_ctrld: whether this clock is hardware controlled (for refcount etc)
+ *	and does not need any magic pokes to be enabled/disabled
+ * @reset: state holder, whether this block's reset line is asserted or not
+ * @res_reg: reset line enable/disable flag register
+ * @res_bit: bit for resetting or taking this consumer out of reset
+ * @en_reg: clock line enable/disable flag register
+ * @en_bit: bit for enabling/disabling this consumer clock line
+ * @clk_val: magic value to poke in the register to enable/disable
+ *	this one clock
+ */
+struct clk_syscon {
+	struct clk_hw hw;
+	bool hw_ctrld;
+	bool reset;
+	void __iomem *res_reg;
+	u8 res_bit;
+	void __iomem *en_reg;
+	u8 en_bit;
+	u16 clk_val;
+};
+
+#define to_syscon(_hw) container_of(_hw, struct clk_syscon, hw)
+
+static DEFINE_SPINLOCK(syscon_resetreg_lock);
+
+/*
+ * Reset control functions. We remember if a block has been
+ * taken out of reset and don't remove the reset assertion again
+ * and vice versa. Currently we only remove resets so the
+ * enablement function is defined out.
+ */
+static void syscon_block_reset_enable(struct clk_syscon *sclk)
+{
+	unsigned long iflags;
+	u16 val;
+
+	/* Not all blocks support resetting */
+	if (!sclk->res_reg)
+		return;
+	spin_lock_irqsave(&syscon_resetreg_lock, iflags);
+	val = readw(sclk->res_reg);
+	val |= BIT(sclk->res_bit);
+	writew(val, sclk->res_reg);
+	spin_unlock_irqrestore(&syscon_resetreg_lock, iflags);
+	sclk->reset = true;
+}
+
+static void syscon_block_reset_disable(struct clk_syscon *sclk)
+{
+	unsigned long iflags;
+	u16 val;
+
+	/* Not all blocks support resetting */
+	if (!sclk->res_reg)
+		return;
+	spin_lock_irqsave(&syscon_resetreg_lock, iflags);
+	val = readw(sclk->res_reg);
+	val &= ~BIT(sclk->res_bit);
+	writew(val, sclk->res_reg);
+	spin_unlock_irqrestore(&syscon_resetreg_lock, iflags);
+	sclk->reset = false;
+}
+
+static int syscon_clk_prepare(struct clk_hw *hw)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+
+	/* If the block is in reset, bring it out */
+	if (sclk->reset)
+		syscon_block_reset_disable(sclk);
+	return 0;
+}
+
+static void syscon_clk_unprepare(struct clk_hw *hw)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+
+	/* Please don't force the console into reset */
+	if (sclk->clk_val == U300_SYSCON_SBCER_UART_CLK_EN)
+		return;
+	/* When unpreparing, force block into reset */
+	if (!sclk->reset)
+		syscon_block_reset_enable(sclk);
+}
+
+static int syscon_clk_enable(struct clk_hw *hw)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+
+	/* Don't touch the hardware controlled clocks */
+	if (sclk->hw_ctrld)
+		return 0;
+	/* These cannot be controlled */
+	if (sclk->clk_val == 0xFFFFU)
+		return 0;
+
+	writew(sclk->clk_val, syscon_vbase + U300_SYSCON_SBCER);
+	return 0;
+}
+
+static void syscon_clk_disable(struct clk_hw *hw)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+
+	/* Don't touch the hardware controlled clocks */
+	if (sclk->hw_ctrld)
+		return;
+	if (sclk->clk_val == 0xFFFFU)
+		return;
+	/* Please don't disable the console port */
+	if (sclk->clk_val == U300_SYSCON_SBCER_UART_CLK_EN)
+		return;
+
+	writew(sclk->clk_val, syscon_vbase + U300_SYSCON_SBCDR);
+}
+
+static int syscon_clk_is_enabled(struct clk_hw *hw)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+	u16 val;
+
+	/* If no enable register defined, it's always-on */
+	if (!sclk->en_reg)
+		return 1;
+
+	val = readw(sclk->en_reg);
+	val &= BIT(sclk->en_bit);
+
+	return val ? 1 : 0;
+}
+
+static u16 syscon_get_perf(void)
+{
+	u16 val;
+
+	val = readw(syscon_vbase + U300_SYSCON_CCR);
+	val &= U300_SYSCON_CCR_CLKING_PERFORMANCE_MASK;
+	return val;
+}
+
+static unsigned long
+syscon_clk_recalc_rate(struct clk_hw *hw,
+		       unsigned long parent_rate)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+	u16 perf = syscon_get_perf();
+
+	switch(sclk->clk_val) {
+	case U300_SYSCON_SBCER_FAST_BRIDGE_CLK_EN:
+	case U300_SYSCON_SBCER_I2C0_CLK_EN:
+	case U300_SYSCON_SBCER_I2C1_CLK_EN:
+	case U300_SYSCON_SBCER_MMC_CLK_EN:
+	case U300_SYSCON_SBCER_SPI_CLK_EN:
+		/* The FAST clocks have one progression */
+		switch(perf) {
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW_POWER:
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW:
+			return 13000000;
+		default:
+			return parent_rate; /* 26 MHz */
+		}
+	case U300_SYSCON_SBCER_DMAC_CLK_EN:
+	case U300_SYSCON_SBCER_NANDIF_CLK_EN:
+	case U300_SYSCON_SBCER_XGAM_CLK_EN:
+		/* AMBA interconnect peripherals */
+		switch(perf) {
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW_POWER:
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW:
+			return 6500000;
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_INTERMEDIATE:
+			return 26000000;
+		default:
+			return parent_rate; /* 52 MHz */
+		}
+	case U300_SYSCON_SBCER_SEMI_CLK_EN:
+	case U300_SYSCON_SBCER_EMIF_CLK_EN:
+		/* EMIF speeds */
+		switch(perf) {
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW_POWER:
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW:
+			return 13000000;
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_INTERMEDIATE:
+			return 52000000;
+		default:
+			return 104000000;
+		}
+	case U300_SYSCON_SBCER_CPU_CLK_EN:
+		/* And the fast CPU clock */
+		switch(perf) {
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW_POWER:
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW:
+			return 13000000;
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_INTERMEDIATE:
+			return 52000000;
+		case U300_SYSCON_CCR_CLKING_PERFORMANCE_HIGH:
+			return 104000000;
+		default:
+			return parent_rate; /* 208 MHz */
+		}
+	default:
+		/*
+		 * The SLOW clocks and default just inherit the rate of
+		 * their parent (typically PLL13 13 MHz).
+		 */
+		return parent_rate;
+	}
+}
+
+static long
+syscon_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+		      unsigned long *prate)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+
+	if (sclk->clk_val != U300_SYSCON_SBCER_CPU_CLK_EN)
+		return *prate;
+	/* We really only support setting the rate of the CPU clock */
+	if (rate <= 13000000)
+		return 13000000;
+	if (rate <= 52000000)
+		return 52000000;
+	if (rate <= 104000000)
+		return 104000000;
+	return 208000000;
+}
+
+static int syscon_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate)
+{
+	struct clk_syscon *sclk = to_syscon(hw);
+	u16 val;
+
+	/* We only support setting the rate of the CPU clock */
+	if (sclk->clk_val != U300_SYSCON_SBCER_CPU_CLK_EN)
+		return -EINVAL;
+	switch (rate) {
+	case 13000000:
+		val = U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW_POWER;
+		break;
+	case 52000000:
+		val = U300_SYSCON_CCR_CLKING_PERFORMANCE_INTERMEDIATE;
+		break;
+	case 104000000:
+		val = U300_SYSCON_CCR_CLKING_PERFORMANCE_HIGH;
+		break;
+	case 208000000:
+		val = U300_SYSCON_CCR_CLKING_PERFORMANCE_BEST;
+		break;
+	default:
+		return -EINVAL;
+	}
+	val |= readw(syscon_vbase + U300_SYSCON_CCR) &
+		~U300_SYSCON_CCR_CLKING_PERFORMANCE_MASK ;
+	writew(val, syscon_vbase + U300_SYSCON_CCR);
+	return 0;
+}
+
+static const struct clk_ops syscon_clk_ops = {
+	.prepare = syscon_clk_prepare,
+	.unprepare = syscon_clk_unprepare,
+	.enable = syscon_clk_enable,
+	.disable = syscon_clk_disable,
+	.is_enabled = syscon_clk_is_enabled,
+	.recalc_rate = syscon_clk_recalc_rate,
+	.round_rate = syscon_clk_round_rate,
+	.set_rate = syscon_clk_set_rate,
+};
+
+static struct clk * __init
+syscon_clk_register(struct device *dev, const char *name,
+		    const char *parent_name, unsigned long flags,
+		    bool hw_ctrld,
+		    void __iomem *res_reg, u8 res_bit,
+		    void __iomem *en_reg, u8 en_bit,
+		    u16 clk_val)
+{
+	struct clk *clk;
+	struct clk_syscon *sclk;
+	struct clk_init_data init;
+
+	sclk = kzalloc(sizeof(struct clk_syscon), GFP_KERNEL);
+	if (!sclk) {
+		pr_err("could not allocate syscon clock %s\n",
+			name);
+		return ERR_PTR(-ENOMEM);
+	}
+	init.name = name;
+	init.ops = &syscon_clk_ops;
+	init.flags = flags;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+	sclk->hw.init = &init;
+	sclk->hw_ctrld = hw_ctrld;
+	/* Assume the block is in reset at registration */
+	sclk->reset = true;
+	sclk->res_reg = res_reg;
+	sclk->res_bit = res_bit;
+	sclk->en_reg = en_reg;
+	sclk->en_bit = en_bit;
+	sclk->clk_val = clk_val;
+
+	clk = clk_register(dev, &sclk->hw);
+	if (IS_ERR(clk))
+		kfree(sclk);
+
+	return clk;
+}
+
+/**
+ * struct clk_mclk - U300 MCLK clock (MMC/SD clock)
+ * @hw: corresponding clock hardware entry
+ * @is_mspro: if this is the memory stick clock rather than MMC/SD
+ */
+struct clk_mclk {
+	struct clk_hw hw;
+	bool is_mspro;
+};
+
+#define to_mclk(_hw) container_of(_hw, struct clk_mclk, hw)
+
+static int mclk_clk_prepare(struct clk_hw *hw)
+{
+	struct clk_mclk *mclk = to_mclk(hw);
+	u16 val;
+
+	/* The MMC and MSPRO clocks need some special set-up */
+	if (!mclk->is_mspro) {
+		/* Set default MMC clock divisor to 18.9 MHz */
+		writew(0x0054U, syscon_vbase + U300_SYSCON_MMF0R);
+		val = readw(syscon_vbase + U300_SYSCON_MMCR);
+		/* Disable the MMC feedback clock */
+		val &= ~U300_SYSCON_MMCR_MMC_FB_CLK_SEL_ENABLE;
+		/* Disable MSPRO frequency */
+		val &= ~U300_SYSCON_MMCR_MSPRO_FREQSEL_ENABLE;
+		writew(val, syscon_vbase + U300_SYSCON_MMCR);
+	} else {
+		val = readw(syscon_vbase + U300_SYSCON_MMCR);
+		/* Disable the MMC feedback clock */
+		val &= ~U300_SYSCON_MMCR_MMC_FB_CLK_SEL_ENABLE;
+		/* Enable MSPRO frequency */
+		val |= U300_SYSCON_MMCR_MSPRO_FREQSEL_ENABLE;
+		writew(val, syscon_vbase + U300_SYSCON_MMCR);
+	}
+
+	return 0;
+}
+
+static unsigned long
+mclk_clk_recalc_rate(struct clk_hw *hw,
+		     unsigned long parent_rate)
+{
+	u16 perf = syscon_get_perf();
+
+	switch (perf) {
+	case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW_POWER:
+		/*
+		 * Here, the 208 MHz PLL gets shut down and the always
+		 * on 13 MHz PLL used for RTC etc kicks into use
+		 * instead.
+		 */
+		return 13000000;
+	case U300_SYSCON_CCR_CLKING_PERFORMANCE_LOW:
+	case U300_SYSCON_CCR_CLKING_PERFORMANCE_INTERMEDIATE:
+	case U300_SYSCON_CCR_CLKING_PERFORMANCE_HIGH:
+	case U300_SYSCON_CCR_CLKING_PERFORMANCE_BEST:
+	{
+		/*
+		 * This clock is under program control. The register is
+		 * divided in two nybbles, bit 7-4 gives cycles-1 to count
+		 * high, bit 3-0 gives cycles-1 to count low. Distribute
+		 * these with no more than 1 cycle difference between
+		 * low and high and add low and high to get the actual
+		 * divisor. The base PLL is 208 MHz. Writing 0x00 will
+		 * divide by 1 and 1 so the highest frequency possible
+		 * is 104 MHz.
+		 *
+		 * e.g. 0x54 =>
+		 * f = 208 / ((5+1) + (4+1)) = 208 / 11 = 18.9 MHz
+		 */
+		u16 val = readw(syscon_vbase + U300_SYSCON_MMF0R) &
+			U300_SYSCON_MMF0R_MASK;
+		switch (val) {
+		case 0x0054:
+			return 18900000;
+		case 0x0044:
+			return 20800000;
+		case 0x0043:
+			return 23100000;
+		case 0x0033:
+			return 26000000;
+		case 0x0032:
+			return 29700000;
+		case 0x0022:
+			return 34700000;
+		case 0x0021:
+			return 41600000;
+		case 0x0011:
+			return 52000000;
+		case 0x0000:
+			return 104000000;
+		default:
+			break;
+		}
+	}
+	default:
+		break;
+	}
+	return parent_rate;
+}
+
+static long
+mclk_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+		    unsigned long *prate)
+{
+	if (rate <= 18900000)
+		return 18900000;
+	if (rate <= 20800000)
+		return 20800000;
+	if (rate <= 23100000)
+		return 23100000;
+	if (rate <= 26000000)
+		return 26000000;
+	if (rate <= 29700000)
+		return 29700000;
+	if (rate <= 34700000)
+		return 34700000;
+	if (rate <= 41600000)
+		return 41600000;
+	/* Highest rate */
+	return 52000000;
+}
+
+static int mclk_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+			     unsigned long parent_rate)
+{
+	u16 val;
+	u16 reg;
+
+	switch (rate) {
+	case 18900000:
+		val = 0x0054;
+		break;
+	case 20800000:
+		val = 0x0044;
+		break;
+	case 23100000:
+		val = 0x0043;
+		break;
+	case 26000000:
+		val = 0x0033;
+		break;
+	case 29700000:
+		val = 0x0032;
+		break;
+	case 34700000:
+		val = 0x0022;
+		break;
+	case 41600000:
+		val = 0x0021;
+		break;
+	case 52000000:
+		val = 0x0011;
+		break;
+	case 104000000:
+		val = 0x0000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	reg = readw(syscon_vbase + U300_SYSCON_MMF0R) &
+		~U300_SYSCON_MMF0R_MASK;
+	writew(reg | val, syscon_vbase + U300_SYSCON_MMF0R);
+	return 0;
+}
+
+static const struct clk_ops mclk_ops = {
+	.prepare = mclk_clk_prepare,
+	.recalc_rate = mclk_clk_recalc_rate,
+	.round_rate = mclk_clk_round_rate,
+	.set_rate = mclk_clk_set_rate,
+};
+
+static struct clk * __init
+mclk_clk_register(struct device *dev, const char *name,
+		  const char *parent_name, bool is_mspro)
+{
+	struct clk *clk;
+	struct clk_mclk *mclk;
+	struct clk_init_data init;
+
+	mclk = kzalloc(sizeof(struct clk_mclk), GFP_KERNEL);
+	if (!mclk) {
+		pr_err("could not allocate MMC/SD clock %s\n",
+		       name);
+		return ERR_PTR(-ENOMEM);
+	}
+	init.name = "mclk";
+	init.ops = &mclk_ops;
+	init.flags = 0;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+	mclk->hw.init = &init;
+	mclk->is_mspro = is_mspro;
+
+	clk = clk_register(dev, &mclk->hw);
+	if (IS_ERR(clk))
+		kfree(mclk);
+
+	return clk;
+}
+
+void __init u300_clk_init(void __iomem *base)
+{
+	u16 val;
+	struct clk *clk;
+
+	syscon_vbase = base;
+
+	/* Set system to run at PLL208, max performance, a known state. */
+	val = readw(syscon_vbase + U300_SYSCON_CCR);
+	val &= ~U300_SYSCON_CCR_CLKING_PERFORMANCE_MASK;
+	writew(val, syscon_vbase + U300_SYSCON_CCR);
+	/* Wait for the PLL208 to lock if not locked in yet */
+	while (!(readw(syscon_vbase + U300_SYSCON_CSR) &
+		 U300_SYSCON_CSR_PLL208_LOCK_IND));
+
+	/* Power management enable */
+	val = readw(syscon_vbase + U300_SYSCON_PMCR);
+	val |= U300_SYSCON_PMCR_PWR_MGNT_ENABLE;
+	writew(val, syscon_vbase + U300_SYSCON_PMCR);
+
+	/* These are always available (RTC and PLL13) */
+	clk = clk_register_fixed_rate(NULL, "app_32_clk", NULL,
+				      CLK_IS_ROOT, 32768);
+	/* The watchdog sits directly on the 32 kHz clock */
+	clk_register_clkdev(clk, NULL, "coh901327_wdog");
+	clk = clk_register_fixed_rate(NULL, "pll13", NULL,
+				      CLK_IS_ROOT, 13000000);
+
+	/* These derive from PLL208 */
+	clk = clk_register_fixed_rate(NULL, "pll208", NULL,
+				      CLK_IS_ROOT, 208000000);
+	clk = clk_register_fixed_factor(NULL, "app_208_clk", "pll208",
+					0, 1, 1);
+	clk = clk_register_fixed_factor(NULL, "app_104_clk", "pll208",
+					0, 1, 2);
+	clk = clk_register_fixed_factor(NULL, "app_52_clk", "pll208",
+					0, 1, 4);
+	/* The 52 MHz is divided down to 26 MHz */
+	clk = clk_register_fixed_factor(NULL, "app_26_clk", "app_52_clk",
+					0, 1, 2);
+
+	/* Directly on the AMBA interconnect */
+	clk = syscon_clk_register(NULL, "cpu_clk", "app_208_clk", 0, true,
+				  syscon_vbase + U300_SYSCON_RRR, 3,
+				  syscon_vbase + U300_SYSCON_CERR, 3,
+				  U300_SYSCON_SBCER_CPU_CLK_EN);
+	clk = syscon_clk_register(NULL, "dmac_clk", "app_52_clk", 0, true,
+				  syscon_vbase + U300_SYSCON_RRR, 4,
+				  syscon_vbase + U300_SYSCON_CERR, 4,
+				  U300_SYSCON_SBCER_DMAC_CLK_EN);
+	clk_register_clkdev(clk, NULL, "dma");
+	clk = syscon_clk_register(NULL, "fsmc_clk", "app_52_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RRR, 6,
+				  syscon_vbase + U300_SYSCON_CERR, 6,
+				  U300_SYSCON_SBCER_NANDIF_CLK_EN);
+	clk_register_clkdev(clk, NULL, "fsmc-nand");
+	clk = syscon_clk_register(NULL, "xgam_clk", "app_52_clk", 0, true,
+				  syscon_vbase + U300_SYSCON_RRR, 8,
+				  syscon_vbase + U300_SYSCON_CERR, 8,
+				  U300_SYSCON_SBCER_XGAM_CLK_EN);
+	clk_register_clkdev(clk, NULL, "xgam");
+	clk = syscon_clk_register(NULL, "semi_clk", "app_104_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RRR, 9,
+				  syscon_vbase + U300_SYSCON_CERR, 9,
+				  U300_SYSCON_SBCER_SEMI_CLK_EN);
+	clk_register_clkdev(clk, NULL, "semi");
+
+	/* AHB bridge clocks */
+	clk = syscon_clk_register(NULL, "ahb_subsys_clk", "app_52_clk", 0, true,
+				  syscon_vbase + U300_SYSCON_RRR, 10,
+				  syscon_vbase + U300_SYSCON_CERR, 10,
+				  U300_SYSCON_SBCER_AHB_SUBSYS_BRIDGE_CLK_EN);
+	clk = syscon_clk_register(NULL, "intcon_clk", "ahb_subsys_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RRR, 12,
+				  syscon_vbase + U300_SYSCON_CERR, 12,
+				  /* Cannot be enabled, just taken out of reset */
+				  0xFFFFU);
+	clk_register_clkdev(clk, NULL, "intcon");
+	clk = syscon_clk_register(NULL, "emif_clk", "ahb_subsys_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RRR, 5,
+				  syscon_vbase + U300_SYSCON_CERR, 5,
+				  U300_SYSCON_SBCER_EMIF_CLK_EN);
+	clk_register_clkdev(clk, NULL, "pl172");
+
+	/* FAST bridge clocks */
+	clk = syscon_clk_register(NULL, "fast_clk", "app_26_clk", 0, true,
+				  syscon_vbase + U300_SYSCON_RFR, 0,
+				  syscon_vbase + U300_SYSCON_CEFR, 0,
+				  U300_SYSCON_SBCER_FAST_BRIDGE_CLK_EN);
+	clk = syscon_clk_register(NULL, "i2c0_p_clk", "fast_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RFR, 1,
+				  syscon_vbase + U300_SYSCON_CEFR, 1,
+				  U300_SYSCON_SBCER_I2C0_CLK_EN);
+	clk_register_clkdev(clk, NULL, "stu300.0");
+	clk = syscon_clk_register(NULL, "i2c1_p_clk", "fast_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RFR, 2,
+				  syscon_vbase + U300_SYSCON_CEFR, 2,
+				  U300_SYSCON_SBCER_I2C1_CLK_EN);
+	clk_register_clkdev(clk, NULL, "stu300.1");
+	clk = syscon_clk_register(NULL, "mmc_p_clk", "fast_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RFR, 5,
+				  syscon_vbase + U300_SYSCON_CEFR, 5,
+				  U300_SYSCON_SBCER_MMC_CLK_EN);
+	clk_register_clkdev(clk, "apb_pclk", "mmci");
+	clk = syscon_clk_register(NULL, "spi_p_clk", "fast_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RFR, 6,
+				  syscon_vbase + U300_SYSCON_CEFR, 6,
+				  U300_SYSCON_SBCER_SPI_CLK_EN);
+	/* The SPI has no external clock for the outward bus, uses the pclk */
+	clk_register_clkdev(clk, NULL, "pl022");
+	clk_register_clkdev(clk, "apb_pclk", "pl022");
+
+	/* SLOW bridge clocks */
+	clk = syscon_clk_register(NULL, "slow_clk", "pll13", 0, true,
+				  syscon_vbase + U300_SYSCON_RSR, 0,
+				  syscon_vbase + U300_SYSCON_CESR, 0,
+				  U300_SYSCON_SBCER_SLOW_BRIDGE_CLK_EN);
+	clk = syscon_clk_register(NULL, "uart0_clk", "slow_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RSR, 1,
+				  syscon_vbase + U300_SYSCON_CESR, 1,
+				  U300_SYSCON_SBCER_UART_CLK_EN);
+	/* Same clock is used for APB and outward bus */
+	clk_register_clkdev(clk, NULL, "uart0");
+	clk_register_clkdev(clk, "apb_pclk", "uart0");
+	clk = syscon_clk_register(NULL, "gpio_clk", "slow_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RSR, 4,
+				  syscon_vbase + U300_SYSCON_CESR, 4,
+				  U300_SYSCON_SBCER_GPIO_CLK_EN);
+	clk_register_clkdev(clk, NULL, "u300-gpio");
+	clk = syscon_clk_register(NULL, "keypad_clk", "slow_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RSR, 5,
+				  syscon_vbase + U300_SYSCON_CESR, 6,
+				  U300_SYSCON_SBCER_KEYPAD_CLK_EN);
+	clk_register_clkdev(clk, NULL, "coh901461-keypad");
+	clk = syscon_clk_register(NULL, "rtc_clk", "slow_clk", 0, true,
+				  syscon_vbase + U300_SYSCON_RSR, 6,
+				  /* No clock enable register bit */
+				  NULL, 0, 0xFFFFU);
+	clk_register_clkdev(clk, NULL, "rtc-coh901331");
+	clk = syscon_clk_register(NULL, "app_tmr_clk", "slow_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RSR, 7,
+				  syscon_vbase + U300_SYSCON_CESR, 7,
+				  U300_SYSCON_SBCER_APP_TMR_CLK_EN);
+	clk_register_clkdev(clk, NULL, "apptimer");
+	clk = syscon_clk_register(NULL, "acc_tmr_clk", "slow_clk", 0, false,
+				  syscon_vbase + U300_SYSCON_RSR, 8,
+				  syscon_vbase + U300_SYSCON_CESR, 8,
+				  U300_SYSCON_SBCER_ACC_TMR_CLK_EN);
+	clk_register_clkdev(clk, NULL, "timer");
+
+	/* Then this special MMC/SD clock */
+	clk = mclk_clk_register(NULL, "mmc_clk", "mmc_p_clk", false);
+	clk_register_clkdev(clk, NULL, "mmci");
+}
diff --git a/drivers/clk/clk-wm831x.c b/drivers/clk/clk-wm831x.c
new file mode 100644
index 00000000000..e7b7765e85f
--- /dev/null
+++ b/drivers/clk/clk-wm831x.c
@@ -0,0 +1,428 @@
+/*
+ * WM831x clock control
+ *
+ * Copyright 2011-2 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/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/wm831x/core.h>
+
+struct wm831x_clk {
+	struct wm831x *wm831x;
+	struct clk_hw xtal_hw;
+	struct clk_hw fll_hw;
+	struct clk_hw clkout_hw;
+	struct clk *xtal;
+	struct clk *fll;
+	struct clk *clkout;
+	bool xtal_ena;
+};
+
+static int wm831x_xtal_is_enabled(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  xtal_hw);
+
+	return clkdata->xtal_ena;
+}
+
+static unsigned long wm831x_xtal_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  xtal_hw);
+
+	if (clkdata->xtal_ena)
+		return 32768;
+	else
+		return 0;
+}
+
+static const struct clk_ops wm831x_xtal_ops = {
+	.is_enabled = wm831x_xtal_is_enabled,
+	.recalc_rate = wm831x_xtal_recalc_rate,
+};
+
+static struct clk_init_data wm831x_xtal_init = {
+	.name = "xtal",
+	.ops = &wm831x_xtal_ops,
+	.flags = CLK_IS_ROOT,
+};
+
+static const unsigned long wm831x_fll_auto_rates[] = {
+	 2048000,
+	11289600,
+	12000000,
+	12288000,
+	19200000,
+	22579600,
+	24000000,
+	24576000,
+};
+
+static int wm831x_fll_is_enabled(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  fll_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_reg_read(wm831x, WM831X_FLL_CONTROL_1);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read FLL_CONTROL_1: %d\n",
+			ret);
+		return true;
+	}
+
+	return (ret & WM831X_FLL_ENA) != 0;
+}
+
+static int wm831x_fll_prepare(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  fll_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_set_bits(wm831x, WM831X_FLL_CONTROL_2,
+			      WM831X_FLL_ENA, WM831X_FLL_ENA);
+	if (ret != 0)
+		dev_crit(wm831x->dev, "Failed to enable FLL: %d\n", ret);
+
+	usleep_range(2000, 2000);
+
+	return ret;
+}
+
+static void wm831x_fll_unprepare(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  fll_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_set_bits(wm831x, WM831X_FLL_CONTROL_2, WM831X_FLL_ENA, 0);
+	if (ret != 0)
+		dev_crit(wm831x->dev, "Failed to disaable FLL: %d\n", ret);
+}
+
+static unsigned long wm831x_fll_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  fll_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_reg_read(wm831x, WM831X_CLOCK_CONTROL_2);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read CLOCK_CONTROL_2: %d\n",
+			ret);
+		return 0;
+	}
+
+	if (ret & WM831X_FLL_AUTO)
+		return wm831x_fll_auto_rates[ret & WM831X_FLL_AUTO_FREQ_MASK];
+
+	dev_err(wm831x->dev, "FLL only supported in AUTO mode\n");
+
+	return 0;
+}
+
+static long wm831x_fll_round_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long *unused)
+{
+	int best = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(wm831x_fll_auto_rates); i++)
+		if (abs(wm831x_fll_auto_rates[i] - rate) <
+		    abs(wm831x_fll_auto_rates[best] - rate))
+			best = i;
+
+	return wm831x_fll_auto_rates[best];
+}
+
+static int wm831x_fll_set_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long parent_rate)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  fll_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(wm831x_fll_auto_rates); i++)
+		if (wm831x_fll_auto_rates[i] == rate)
+			break;
+	if (i == ARRAY_SIZE(wm831x_fll_auto_rates))
+		return -EINVAL;
+
+	if (wm831x_fll_is_enabled(hw))
+		return -EPERM;
+
+	return wm831x_set_bits(wm831x, WM831X_CLOCK_CONTROL_2,
+			       WM831X_FLL_AUTO_FREQ_MASK, i);
+}
+
+static const char *wm831x_fll_parents[] = {
+	"xtal",
+	"clkin",
+};
+
+static u8 wm831x_fll_get_parent(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  fll_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	/* AUTO mode is always clocked from the crystal */
+	ret = wm831x_reg_read(wm831x, WM831X_CLOCK_CONTROL_2);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read CLOCK_CONTROL_2: %d\n",
+			ret);
+		return 0;
+	}
+
+	if (ret & WM831X_FLL_AUTO)
+		return 0;
+
+	ret = wm831x_reg_read(wm831x, WM831X_FLL_CONTROL_5);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read FLL_CONTROL_5: %d\n",
+			ret);
+		return 0;
+	}
+
+	switch (ret & WM831X_FLL_CLK_SRC_MASK) {
+	case 0:
+		return 0;
+	case 1:
+		return 1;
+	default:
+		dev_err(wm831x->dev, "Unsupported FLL clock source %d\n",
+			ret & WM831X_FLL_CLK_SRC_MASK);
+		return 0;
+	}
+}
+
+static const struct clk_ops wm831x_fll_ops = {
+	.is_enabled = wm831x_fll_is_enabled,
+	.prepare = wm831x_fll_prepare,
+	.unprepare = wm831x_fll_unprepare,
+	.round_rate = wm831x_fll_round_rate,
+	.recalc_rate = wm831x_fll_recalc_rate,
+	.set_rate = wm831x_fll_set_rate,
+	.get_parent = wm831x_fll_get_parent,
+};
+
+static struct clk_init_data wm831x_fll_init = {
+	.name = "fll",
+	.ops = &wm831x_fll_ops,
+	.parent_names = wm831x_fll_parents,
+	.num_parents = ARRAY_SIZE(wm831x_fll_parents),
+	.flags = CLK_SET_RATE_GATE,
+};
+
+static int wm831x_clkout_is_enabled(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  clkout_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_reg_read(wm831x, WM831X_CLOCK_CONTROL_1);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read CLOCK_CONTROL_1: %d\n",
+			ret);
+		return true;
+	}
+
+	return (ret & WM831X_CLKOUT_ENA) != 0;
+}
+
+static int wm831x_clkout_prepare(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  clkout_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_reg_unlock(wm831x);
+	if (ret != 0) {
+		dev_crit(wm831x->dev, "Failed to lock registers: %d\n", ret);
+		return ret;
+	}
+
+	ret = wm831x_set_bits(wm831x, WM831X_CLOCK_CONTROL_1,
+			      WM831X_CLKOUT_ENA, WM831X_CLKOUT_ENA);
+	if (ret != 0)
+		dev_crit(wm831x->dev, "Failed to enable CLKOUT: %d\n", ret);
+
+	wm831x_reg_lock(wm831x);
+
+	return ret;
+}
+
+static void wm831x_clkout_unprepare(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  clkout_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_reg_unlock(wm831x);
+	if (ret != 0) {
+		dev_crit(wm831x->dev, "Failed to lock registers: %d\n", ret);
+		return;
+	}
+
+	ret = wm831x_set_bits(wm831x, WM831X_CLOCK_CONTROL_1,
+			      WM831X_CLKOUT_ENA, 0);
+	if (ret != 0)
+		dev_crit(wm831x->dev, "Failed to disable CLKOUT: %d\n", ret);
+
+	wm831x_reg_lock(wm831x);
+}
+
+static const char *wm831x_clkout_parents[] = {
+	"xtal",
+	"fll",
+};
+
+static u8 wm831x_clkout_get_parent(struct clk_hw *hw)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  clkout_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+	int ret;
+
+	ret = wm831x_reg_read(wm831x, WM831X_CLOCK_CONTROL_1);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read CLOCK_CONTROL_1: %d\n",
+			ret);
+		return 0;
+	}
+
+	if (ret & WM831X_CLKOUT_SRC)
+		return 0;
+	else
+		return 1;
+}
+
+static int wm831x_clkout_set_parent(struct clk_hw *hw, u8 parent)
+{
+	struct wm831x_clk *clkdata = container_of(hw, struct wm831x_clk,
+						  clkout_hw);
+	struct wm831x *wm831x = clkdata->wm831x;
+
+	return wm831x_set_bits(wm831x, WM831X_CLOCK_CONTROL_1,
+			       WM831X_CLKOUT_SRC,
+			       parent << WM831X_CLKOUT_SRC_SHIFT);
+}
+
+static const struct clk_ops wm831x_clkout_ops = {
+	.is_enabled = wm831x_clkout_is_enabled,
+	.prepare = wm831x_clkout_prepare,
+	.unprepare = wm831x_clkout_unprepare,
+	.get_parent = wm831x_clkout_get_parent,
+	.set_parent = wm831x_clkout_set_parent,
+};
+
+static struct clk_init_data wm831x_clkout_init = {
+	.name = "clkout",
+	.ops = &wm831x_clkout_ops,
+	.parent_names = wm831x_clkout_parents,
+	.num_parents = ARRAY_SIZE(wm831x_clkout_parents),
+	.flags = CLK_SET_RATE_PARENT,
+};
+
+static __devinit int wm831x_clk_probe(struct platform_device *pdev)
+{
+	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
+	struct wm831x_clk *clkdata;
+	int ret;
+
+	clkdata = devm_kzalloc(&pdev->dev, sizeof(*clkdata), GFP_KERNEL);
+	if (!clkdata)
+		return -ENOMEM;
+
+	/* XTAL_ENA can only be set via OTP/InstantConfig so just read once */
+	ret = wm831x_reg_read(wm831x, WM831X_CLOCK_CONTROL_2);
+	if (ret < 0) {
+		dev_err(wm831x->dev, "Unable to read CLOCK_CONTROL_2: %d\n",
+			ret);
+		return ret;
+	}
+	clkdata->xtal_ena = ret & WM831X_XTAL_ENA;
+
+	clkdata->xtal_hw.init = &wm831x_xtal_init;
+	clkdata->xtal = clk_register(&pdev->dev, &clkdata->xtal_hw);
+	if (!clkdata->xtal)
+		return -EINVAL;
+
+	clkdata->fll_hw.init = &wm831x_fll_init;
+	clkdata->fll = clk_register(&pdev->dev, &clkdata->fll_hw);
+	if (!clkdata->fll) {
+		ret = -EINVAL;
+		goto err_xtal;
+	}
+
+	clkdata->clkout_hw.init = &wm831x_clkout_init;
+	clkdata->clkout = clk_register(&pdev->dev, &clkdata->clkout_hw);
+	if (!clkdata->clkout) {
+		ret = -EINVAL;
+		goto err_fll;
+	}
+
+	dev_set_drvdata(&pdev->dev, clkdata);
+
+	return 0;
+
+err_fll:
+	clk_unregister(clkdata->fll);
+err_xtal:
+	clk_unregister(clkdata->xtal);
+	return ret;
+}
+
+static int __devexit wm831x_clk_remove(struct platform_device *pdev)
+{
+	struct wm831x_clk *clkdata = dev_get_drvdata(&pdev->dev);
+
+	clk_unregister(clkdata->clkout);
+	clk_unregister(clkdata->fll);
+	clk_unregister(clkdata->xtal);
+
+	return 0;
+}
+
+static struct platform_driver wm831x_clk_driver = {
+	.probe = wm831x_clk_probe,
+	.remove = __devexit_p(wm831x_clk_remove),
+	.driver		= {
+		.name	= "wm831x-clk",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(wm831x_clk_driver);
+
+/* Module information */
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_DESCRIPTION("WM831x clock driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:wm831x-clk");
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 9a1eb0cfa95..efdfd009c27 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -16,6 +16,7 @@
 #include <linux/err.h>
 #include <linux/list.h>
 #include <linux/slab.h>
+#include <linux/of.h>
 
 static DEFINE_SPINLOCK(enable_lock);
 static DEFINE_MUTEX(prepare_lock);
@@ -464,6 +465,9 @@ static void __clk_disable(struct clk *clk)
 	if (!clk)
 		return;
 
+	if (WARN_ON(IS_ERR(clk)))
+		return;
+
 	if (WARN_ON(clk->enable_count == 0))
 		return;
 
@@ -1235,8 +1239,8 @@ int __clk_init(struct device *dev, struct clk *clk)
 	 * If clk->parents is not NULL we skip this entire block.  This allows
 	 * for clock drivers to statically initialize clk->parents.
 	 */
-	if (clk->num_parents && !clk->parents) {
-		clk->parents = kmalloc((sizeof(struct clk*) * clk->num_parents),
+	if (clk->num_parents > 1 && !clk->parents) {
+		clk->parents = kzalloc((sizeof(struct clk*) * clk->num_parents),
 				GFP_KERNEL);
 		/*
 		 * __clk_lookup returns NULL for parents that have not been
@@ -1550,3 +1554,142 @@ int clk_notifier_unregister(struct clk *clk, struct notifier_block *nb)
 	return ret;
 }
 EXPORT_SYMBOL_GPL(clk_notifier_unregister);
+
+#ifdef CONFIG_OF
+/**
+ * struct of_clk_provider - Clock provider registration structure
+ * @link: Entry in global list of clock providers
+ * @node: Pointer to device tree node of clock provider
+ * @get: Get clock callback.  Returns NULL or a struct clk for the
+ *       given clock specifier
+ * @data: context pointer to be passed into @get callback
+ */
+struct of_clk_provider {
+	struct list_head link;
+
+	struct device_node *node;
+	struct clk *(*get)(struct of_phandle_args *clkspec, void *data);
+	void *data;
+};
+
+static LIST_HEAD(of_clk_providers);
+static DEFINE_MUTEX(of_clk_lock);
+
+struct clk *of_clk_src_simple_get(struct of_phandle_args *clkspec,
+				     void *data)
+{
+	return data;
+}
+EXPORT_SYMBOL_GPL(of_clk_src_simple_get);
+
+/**
+ * of_clk_add_provider() - Register a clock provider for a node
+ * @np: Device node pointer associated with clock provider
+ * @clk_src_get: callback for decoding clock
+ * @data: context pointer for @clk_src_get callback.
+ */
+int of_clk_add_provider(struct device_node *np,
+			struct clk *(*clk_src_get)(struct of_phandle_args *clkspec,
+						   void *data),
+			void *data)
+{
+	struct of_clk_provider *cp;
+
+	cp = kzalloc(sizeof(struct of_clk_provider), GFP_KERNEL);
+	if (!cp)
+		return -ENOMEM;
+
+	cp->node = of_node_get(np);
+	cp->data = data;
+	cp->get = clk_src_get;
+
+	mutex_lock(&of_clk_lock);
+	list_add(&cp->link, &of_clk_providers);
+	mutex_unlock(&of_clk_lock);
+	pr_debug("Added clock from %s\n", np->full_name);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(of_clk_add_provider);
+
+/**
+ * of_clk_del_provider() - Remove a previously registered clock provider
+ * @np: Device node pointer associated with clock provider
+ */
+void of_clk_del_provider(struct device_node *np)
+{
+	struct of_clk_provider *cp;
+
+	mutex_lock(&of_clk_lock);
+	list_for_each_entry(cp, &of_clk_providers, link) {
+		if (cp->node == np) {
+			list_del(&cp->link);
+			of_node_put(cp->node);
+			kfree(cp);
+			break;
+		}
+	}
+	mutex_unlock(&of_clk_lock);
+}
+EXPORT_SYMBOL_GPL(of_clk_del_provider);
+
+struct clk *of_clk_get_from_provider(struct of_phandle_args *clkspec)
+{
+	struct of_clk_provider *provider;
+	struct clk *clk = ERR_PTR(-ENOENT);
+
+	/* Check if we have such a provider in our array */
+	mutex_lock(&of_clk_lock);
+	list_for_each_entry(provider, &of_clk_providers, link) {
+		if (provider->node == clkspec->np)
+			clk = provider->get(clkspec, provider->data);
+		if (!IS_ERR(clk))
+			break;
+	}
+	mutex_unlock(&of_clk_lock);
+
+	return clk;
+}
+
+const char *of_clk_get_parent_name(struct device_node *np, int index)
+{
+	struct of_phandle_args clkspec;
+	const char *clk_name;
+	int rc;
+
+	if (index < 0)
+		return NULL;
+
+	rc = of_parse_phandle_with_args(np, "clocks", "#clock-cells", index,
+					&clkspec);
+	if (rc)
+		return NULL;
+
+	if (of_property_read_string_index(clkspec.np, "clock-output-names",
+					  clkspec.args_count ? clkspec.args[0] : 0,
+					  &clk_name) < 0)
+		clk_name = clkspec.np->name;
+
+	of_node_put(clkspec.np);
+	return clk_name;
+}
+EXPORT_SYMBOL_GPL(of_clk_get_parent_name);
+
+/**
+ * of_clk_init() - Scan and init clock providers from the DT
+ * @matches: array of compatible values and init functions for providers.
+ *
+ * This function scans the device tree for matching clock providers and
+ * calls their initialization functions
+ */
+void __init of_clk_init(const struct of_device_id *matches)
+{
+	struct device_node *np;
+
+	for_each_matching_node(np, matches) {
+		const struct of_device_id *match = of_match_node(matches, np);
+		of_clk_init_cb_t clk_init_cb = match->data;
+		clk_init_cb(np);
+	}
+}
+#endif
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index c535cf8c577..d423c9bdd71 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -19,10 +19,80 @@
 #include <linux/mutex.h>
 #include <linux/clk.h>
 #include <linux/clkdev.h>
+#include <linux/of.h>
 
 static LIST_HEAD(clocks);
 static DEFINE_MUTEX(clocks_mutex);
 
+#if defined(CONFIG_OF) && defined(CONFIG_COMMON_CLK)
+struct clk *of_clk_get(struct device_node *np, int index)
+{
+	struct of_phandle_args clkspec;
+	struct clk *clk;
+	int rc;
+
+	if (index < 0)
+		return ERR_PTR(-EINVAL);
+
+	rc = of_parse_phandle_with_args(np, "clocks", "#clock-cells", index,
+					&clkspec);
+	if (rc)
+		return ERR_PTR(rc);
+
+	clk = of_clk_get_from_provider(&clkspec);
+	of_node_put(clkspec.np);
+	return clk;
+}
+EXPORT_SYMBOL(of_clk_get);
+
+/**
+ * of_clk_get_by_name() - Parse and lookup a clock referenced by a device node
+ * @np: pointer to clock consumer node
+ * @name: name of consumer's clock input, or NULL for the first clock reference
+ *
+ * This function parses the clocks and clock-names properties,
+ * and uses them to look up the struct clk from the registered list of clock
+ * providers.
+ */
+struct clk *of_clk_get_by_name(struct device_node *np, const char *name)
+{
+	struct clk *clk = ERR_PTR(-ENOENT);
+
+	/* Walk up the tree of devices looking for a clock that matches */
+	while (np) {
+		int index = 0;
+
+		/*
+		 * For named clocks, first look up the name in the
+		 * "clock-names" property.  If it cannot be found, then
+		 * index will be an error code, and of_clk_get() will fail.
+		 */
+		if (name)
+			index = of_property_match_string(np, "clock-names", name);
+		clk = of_clk_get(np, index);
+		if (!IS_ERR(clk))
+			break;
+		else if (name && index >= 0) {
+			pr_err("ERROR: could not get clock %s:%s(%i)\n",
+				np->full_name, name ? name : "", index);
+			return clk;
+		}
+
+		/*
+		 * No matching clock found on this node.  If the parent node
+		 * has a "clock-ranges" property, then we can try one of its
+		 * clocks.
+		 */
+		np = np->parent;
+		if (np && !of_get_property(np, "clock-ranges", NULL))
+			break;
+	}
+
+	return clk;
+}
+EXPORT_SYMBOL(of_clk_get_by_name);
+#endif
+
 /*
  * Find the correct struct clk for the device and connection ID.
  * We do slightly fuzzy matching here:
@@ -83,6 +153,13 @@ EXPORT_SYMBOL(clk_get_sys);
 struct clk *clk_get(struct device *dev, const char *con_id)
 {
 	const char *dev_id = dev ? dev_name(dev) : NULL;
+	struct clk *clk;
+
+	if (dev) {
+		clk = of_clk_get_by_name(dev->of_node, con_id);
+		if (!IS_ERR(clk) && __clk_get(clk))
+			return clk;
+	}
 
 	return clk_get_sys(dev_id, con_id);
 }
diff --git a/drivers/clk/mxs/clk-imx23.c b/drivers/clk/mxs/clk-imx23.c
index db2391c054e..844043ad0fe 100644
--- a/drivers/clk/mxs/clk-imx23.c
+++ b/drivers/clk/mxs/clk-imx23.c
@@ -106,7 +106,7 @@ static struct clk_lookup lcdif_lookups[] = {
 
 static struct clk_lookup gpmi_lookups[] = {
 	{ .dev_id = "imx23-gpmi-nand", },
-	{ .dev_id = "8000c000.gpmi", },
+	{ .dev_id = "8000c000.gpmi-nand", },
 };
 
 static const char *sel_pll[]  __initconst = { "pll", "ref_xtal", };
@@ -189,6 +189,7 @@ int __init mx23_clocks_init(void)
 		}
 
 	clk_register_clkdev(clks[clk32k], NULL, "timrot");
+	clk_register_clkdev(clks[pwm], NULL, "80064000.pwm");
 	clk_register_clkdevs(clks[hbus], hbus_lookups, ARRAY_SIZE(hbus_lookups));
 	clk_register_clkdevs(clks[xbus], xbus_lookups, ARRAY_SIZE(xbus_lookups));
 	clk_register_clkdevs(clks[uart], uart_lookups, ARRAY_SIZE(uart_lookups));
diff --git a/drivers/clk/mxs/clk-imx28.c b/drivers/clk/mxs/clk-imx28.c
index 7fad6c8c13d..e3aab67b3eb 100644
--- a/drivers/clk/mxs/clk-imx28.c
+++ b/drivers/clk/mxs/clk-imx28.c
@@ -112,11 +112,11 @@ static void __init clk_misc_init(void)
 
 	/*
 	 * 480 MHz seems too high to be ssp clock source directly,
-	 * so set frac0 to get a 288 MHz ref_io0.
+	 * so set frac0 to get a 288 MHz ref_io0 and ref_io1.
 	 */
 	val = readl_relaxed(FRAC0);
-	val &= ~(0x3f << BP_FRAC0_IO0FRAC);
-	val |= 30 << BP_FRAC0_IO0FRAC;
+	val &= ~((0x3f << BP_FRAC0_IO0FRAC) | (0x3f << BP_FRAC0_IO1FRAC));
+	val |= (30 << BP_FRAC0_IO0FRAC) | (30 << BP_FRAC0_IO1FRAC);
 	writel_relaxed(val, FRAC0);
 }
 
@@ -174,7 +174,7 @@ static struct clk_lookup lcdif_lookups[] = {
 
 static struct clk_lookup gpmi_lookups[] = {
 	{ .dev_id = "imx28-gpmi-nand", },
-	{ .dev_id = "8000c000.gpmi", },
+	{ .dev_id = "8000c000.gpmi-nand", },
 };
 
 static struct clk_lookup fec_lookups[] = {
@@ -314,6 +314,7 @@ int __init mx28_clocks_init(void)
 
 	clk_register_clkdev(clks[clk32k], NULL, "timrot");
 	clk_register_clkdev(clks[enet_out], NULL, "enet_out");
+	clk_register_clkdev(clks[pwm], NULL, "80064000.pwm");
 	clk_register_clkdevs(clks[hbus], hbus_lookups, ARRAY_SIZE(hbus_lookups));
 	clk_register_clkdevs(clks[xbus], xbus_lookups, ARRAY_SIZE(xbus_lookups));
 	clk_register_clkdevs(clks[uart], uart_lookups, ARRAY_SIZE(uart_lookups));
@@ -328,6 +329,10 @@ int __init mx28_clocks_init(void)
 	clk_register_clkdevs(clks[fec], fec_lookups, ARRAY_SIZE(fec_lookups));
 	clk_register_clkdevs(clks[can0], can0_lookups, ARRAY_SIZE(can0_lookups));
 	clk_register_clkdevs(clks[can1], can1_lookups, ARRAY_SIZE(can1_lookups));
+	clk_register_clkdev(clks[usb0_pwr], NULL, "8007c000.usbphy");
+	clk_register_clkdev(clks[usb1_pwr], NULL, "8007e000.usbphy");
+	clk_register_clkdev(clks[usb0], NULL, "80080000.usb");
+	clk_register_clkdev(clks[usb1], NULL, "80090000.usb");
 
 	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
 		clk_prepare_enable(clks[clks_init_on[i]]);
diff --git a/drivers/clk/socfpga/Makefile b/drivers/clk/socfpga/Makefile
new file mode 100644
index 00000000000..0303c0b99cd
--- /dev/null
+++ b/drivers/clk/socfpga/Makefile
@@ -0,0 +1 @@
+obj-y += clk.o
diff --git a/drivers/clk/socfpga/clk.c b/drivers/clk/socfpga/clk.c
new file mode 100644
index 00000000000..2c855a6394f
--- /dev/null
+++ b/drivers/clk/socfpga/clk.c
@@ -0,0 +1,51 @@
+/*
+ *  Copyright (C) 2012 Altera Corporation <www.altera.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+
+#define SOCFPGA_OSC1_CLK	10000000
+#define SOCFPGA_MPU_CLK		800000000
+#define SOCFPGA_MAIN_QSPI_CLK		432000000
+#define SOCFPGA_MAIN_NAND_SDMMC_CLK	250000000
+#define SOCFPGA_S2F_USR_CLK		125000000
+
+void __init socfpga_init_clocks(void)
+{
+	struct clk *clk;
+
+	clk = clk_register_fixed_rate(NULL, "osc1_clk", NULL, CLK_IS_ROOT, SOCFPGA_OSC1_CLK);
+	clk_register_clkdev(clk, "osc1_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "mpu_clk", NULL, CLK_IS_ROOT, SOCFPGA_MPU_CLK);
+	clk_register_clkdev(clk, "mpu_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "main_clk", NULL, CLK_IS_ROOT, SOCFPGA_MPU_CLK/2);
+	clk_register_clkdev(clk, "main_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "dbg_base_clk", NULL, CLK_IS_ROOT, SOCFPGA_MPU_CLK/2);
+	clk_register_clkdev(clk, "dbg_base_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "main_qspi_clk", NULL, CLK_IS_ROOT, SOCFPGA_MAIN_QSPI_CLK);
+	clk_register_clkdev(clk, "main_qspi_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "main_nand_sdmmc_clk", NULL, CLK_IS_ROOT, SOCFPGA_MAIN_NAND_SDMMC_CLK);
+	clk_register_clkdev(clk, "main_nand_sdmmc_clk", NULL);
+
+	clk = clk_register_fixed_rate(NULL, "s2f_usr_clk", NULL, CLK_IS_ROOT, SOCFPGA_S2F_USR_CLK);
+	clk_register_clkdev(clk, "s2f_usr_clk", NULL);
+}
diff --git a/drivers/clk/spear/spear1310_clock.c b/drivers/clk/spear/spear1310_clock.c
index 8f05652d53e..0fcec2aae19 100644
--- a/drivers/clk/spear/spear1310_clock.c
+++ b/drivers/clk/spear/spear1310_clock.c
@@ -345,31 +345,30 @@ static struct frac_rate_tbl gen_rtbl[] = {
 /* clock parents */
 static const char *vco_parents[] = { "osc_24m_clk", "osc_25m_clk", };
 static const char *gpt_parents[] = { "osc_24m_clk", "apb_clk", };
-static const char *uart0_parents[] = { "pll5_clk", "uart_synth_gate_clk", };
-static const char *c3_parents[] = { "pll5_clk", "c3_synth_gate_clk", };
-static const char *gmac_phy_input_parents[] = { "gmii_125m_pad_clk", "pll2_clk",
+static const char *uart0_parents[] = { "pll5_clk", "uart_syn_gclk", };
+static const char *c3_parents[] = { "pll5_clk", "c3_syn_gclk", };
+static const char *gmac_phy_input_parents[] = { "gmii_pad_clk", "pll2_clk",
 	"osc_25m_clk", };
-static const char *gmac_phy_parents[] = { "gmac_phy_input_mux_clk",
-	"gmac_phy_synth_gate_clk", };
+static const char *gmac_phy_parents[] = { "phy_input_mclk", "phy_syn_gclk", };
 static const char *clcd_synth_parents[] = { "vco1div4_clk", "pll2_clk", };
-static const char *clcd_pixel_parents[] = { "pll5_clk", "clcd_synth_clk", };
+static const char *clcd_pixel_parents[] = { "pll5_clk", "clcd_syn_clk", };
 static const char *i2s_src_parents[] = { "vco1div2_clk", "none", "pll3_clk",
 	"i2s_src_pad_clk", };
-static const char *i2s_ref_parents[] = { "i2s_src_mux_clk", "i2s_prs1_clk", };
+static const char *i2s_ref_parents[] = { "i2s_src_mclk", "i2s_prs1_clk", };
 static const char *gen_synth0_1_parents[] = { "vco1div4_clk", "vco3div2_clk",
 	"pll3_clk", };
 static const char *gen_synth2_3_parents[] = { "vco1div4_clk", "vco3div2_clk",
 	"pll2_clk", };
 static const char *rmii_phy_parents[] = { "ras_tx50_clk", "none",
-	"ras_pll2_clk", "ras_synth0_clk", };
+	"ras_pll2_clk", "ras_syn0_clk", };
 static const char *smii_rgmii_phy_parents[] = { "none", "ras_tx125_clk",
-	"ras_pll2_clk", "ras_synth0_clk", };
-static const char *uart_parents[] = { "ras_apb_clk", "gen_synth3_clk", };
-static const char *i2c_parents[] = { "ras_apb_clk", "gen_synth1_clk", };
-static const char *ssp1_parents[] = { "ras_apb_clk", "gen_synth1_clk",
+	"ras_pll2_clk", "ras_syn0_clk", };
+static const char *uart_parents[] = { "ras_apb_clk", "gen_syn3_clk", };
+static const char *i2c_parents[] = { "ras_apb_clk", "gen_syn1_clk", };
+static const char *ssp1_parents[] = { "ras_apb_clk", "gen_syn1_clk",
 	"ras_plclk0_clk", };
-static const char *pci_parents[] = { "ras_pll3_clk", "gen_synth2_clk", };
-static const char *tdm_parents[] = { "ras_pll3_clk", "gen_synth1_clk", };
+static const char *pci_parents[] = { "ras_pll3_clk", "gen_syn2_clk", };
+static const char *tdm_parents[] = { "ras_pll3_clk", "gen_syn1_clk", };
 
 void __init spear1310_clk_init(void)
 {
@@ -390,9 +389,9 @@ void __init spear1310_clk_init(void)
 			25000000);
 	clk_register_clkdev(clk, "osc_25m_clk", NULL);
 
-	clk = clk_register_fixed_rate(NULL, "gmii_125m_pad_clk", NULL,
-			CLK_IS_ROOT, 125000000);
-	clk_register_clkdev(clk, "gmii_125m_pad_clk", NULL);
+	clk = clk_register_fixed_rate(NULL, "gmii_pad_clk", NULL, CLK_IS_ROOT,
+			125000000);
+	clk_register_clkdev(clk, "gmii_pad_clk", NULL);
 
 	clk = clk_register_fixed_rate(NULL, "i2s_src_pad_clk", NULL,
 			CLK_IS_ROOT, 12288000);
@@ -406,34 +405,34 @@ void __init spear1310_clk_init(void)
 
 	/* clock derived from 24 or 25 MHz osc clk */
 	/* vco-pll */
-	clk = clk_register_mux(NULL, "vco1_mux_clk", vco_parents,
+	clk = clk_register_mux(NULL, "vco1_mclk", vco_parents,
 			ARRAY_SIZE(vco_parents), 0, SPEAR1310_PLL_CFG,
 			SPEAR1310_PLL1_CLK_SHIFT, SPEAR1310_PLL_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "vco1_mux_clk", NULL);
-	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "vco1_mux_clk",
+	clk_register_clkdev(clk, "vco1_mclk", NULL);
+	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "vco1_mclk",
 			0, SPEAR1310_PLL1_CTR, SPEAR1310_PLL1_FRQ, pll_rtbl,
 			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco1_clk", NULL);
 	clk_register_clkdev(clk1, "pll1_clk", NULL);
 
-	clk = clk_register_mux(NULL, "vco2_mux_clk", vco_parents,
+	clk = clk_register_mux(NULL, "vco2_mclk", vco_parents,
 			ARRAY_SIZE(vco_parents), 0, SPEAR1310_PLL_CFG,
 			SPEAR1310_PLL2_CLK_SHIFT, SPEAR1310_PLL_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "vco2_mux_clk", NULL);
-	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "vco2_mux_clk",
+	clk_register_clkdev(clk, "vco2_mclk", NULL);
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "vco2_mclk",
 			0, SPEAR1310_PLL2_CTR, SPEAR1310_PLL2_FRQ, pll_rtbl,
 			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco2_clk", NULL);
 	clk_register_clkdev(clk1, "pll2_clk", NULL);
 
-	clk = clk_register_mux(NULL, "vco3_mux_clk", vco_parents,
+	clk = clk_register_mux(NULL, "vco3_mclk", vco_parents,
 			ARRAY_SIZE(vco_parents), 0, SPEAR1310_PLL_CFG,
 			SPEAR1310_PLL3_CLK_SHIFT, SPEAR1310_PLL_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "vco3_mux_clk", NULL);
-	clk = clk_register_vco_pll("vco3_clk", "pll3_clk", NULL, "vco3_mux_clk",
+	clk_register_clkdev(clk, "vco3_mclk", NULL);
+	clk = clk_register_vco_pll("vco3_clk", "pll3_clk", NULL, "vco3_mclk",
 			0, SPEAR1310_PLL3_CTR, SPEAR1310_PLL3_FRQ, pll_rtbl,
 			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco3_clk", NULL);
@@ -473,7 +472,7 @@ void __init spear1310_clk_init(void)
 	/* peripherals */
 	clk_register_fixed_factor(NULL, "thermal_clk", "osc_24m_clk", 0, 1,
 			128);
-	clk = clk_register_gate(NULL, "thermal_gate_clk", "thermal_clk", 0,
+	clk = clk_register_gate(NULL, "thermal_gclk", "thermal_clk", 0,
 			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_THSENS_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_thermal");
@@ -500,177 +499,176 @@ void __init spear1310_clk_init(void)
 	clk_register_clkdev(clk, "apb_clk", NULL);
 
 	/* gpt clocks */
-	clk = clk_register_mux(NULL, "gpt0_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt0_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
 			SPEAR1310_GPT0_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt0_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt0_clk", "gpt0_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt0_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt0_clk", "gpt0_mclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GPT0_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt0");
 
-	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt1_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
 			SPEAR1310_GPT1_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt1_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_GPT1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt1");
 
-	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt2_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
 			SPEAR1310_GPT2_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt2_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mclk", 0,
 			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_GPT2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt2");
 
-	clk = clk_register_mux(NULL, "gpt3_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt3_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1310_PERIP_CLK_CFG,
 			SPEAR1310_GPT3_CLK_SHIFT, SPEAR1310_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt3_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt3_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mclk", 0,
 			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_GPT3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt3");
 
 	/* others */
-	clk = clk_register_aux("uart_synth_clk", "uart_synth_gate_clk",
-			"vco1div2_clk", 0, SPEAR1310_UART_CLK_SYNT, NULL,
-			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "uart_synth_clk", NULL);
-	clk_register_clkdev(clk1, "uart_synth_gate_clk", NULL);
+	clk = clk_register_aux("uart_syn_clk", "uart_syn_gclk", "vco1div2_clk",
+			0, SPEAR1310_UART_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "uart_syn_clk", NULL);
+	clk_register_clkdev(clk1, "uart_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "uart0_mux_clk", uart0_parents,
+	clk = clk_register_mux(NULL, "uart0_mclk", uart0_parents,
 			ARRAY_SIZE(uart0_parents), 0, SPEAR1310_PERIP_CLK_CFG,
 			SPEAR1310_UART_CLK_SHIFT, SPEAR1310_UART_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "uart0_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart0_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart0_clk", "uart0_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart0_clk", "uart0_mclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_UART_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "e0000000.serial");
 
-	clk = clk_register_aux("sdhci_synth_clk", "sdhci_synth_gate_clk",
+	clk = clk_register_aux("sdhci_syn_clk", "sdhci_syn_gclk",
 			"vco1div2_clk", 0, SPEAR1310_SDHCI_CLK_SYNT, NULL,
 			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "sdhci_synth_clk", NULL);
-	clk_register_clkdev(clk1, "sdhci_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "sdhci_syn_clk", NULL);
+	clk_register_clkdev(clk1, "sdhci_syn_gclk", NULL);
 
-	clk = clk_register_gate(NULL, "sdhci_clk", "sdhci_synth_gate_clk", 0,
+	clk = clk_register_gate(NULL, "sdhci_clk", "sdhci_syn_gclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_SDHCI_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "b3000000.sdhci");
 
-	clk = clk_register_aux("cfxd_synth_clk", "cfxd_synth_gate_clk",
-			"vco1div2_clk", 0, SPEAR1310_CFXD_CLK_SYNT, NULL,
-			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "cfxd_synth_clk", NULL);
-	clk_register_clkdev(clk1, "cfxd_synth_gate_clk", NULL);
+	clk = clk_register_aux("cfxd_syn_clk", "cfxd_syn_gclk", "vco1div2_clk",
+			0, SPEAR1310_CFXD_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "cfxd_syn_clk", NULL);
+	clk_register_clkdev(clk1, "cfxd_syn_gclk", NULL);
 
-	clk = clk_register_gate(NULL, "cfxd_clk", "cfxd_synth_gate_clk", 0,
+	clk = clk_register_gate(NULL, "cfxd_clk", "cfxd_syn_gclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_CFXD_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "b2800000.cf");
 	clk_register_clkdev(clk, NULL, "arasan_xd");
 
-	clk = clk_register_aux("c3_synth_clk", "c3_synth_gate_clk",
-			"vco1div2_clk", 0, SPEAR1310_C3_CLK_SYNT, NULL,
-			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "c3_synth_clk", NULL);
-	clk_register_clkdev(clk1, "c3_synth_gate_clk", NULL);
+	clk = clk_register_aux("c3_syn_clk", "c3_syn_gclk", "vco1div2_clk",
+			0, SPEAR1310_C3_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "c3_syn_clk", NULL);
+	clk_register_clkdev(clk1, "c3_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "c3_mux_clk", c3_parents,
+	clk = clk_register_mux(NULL, "c3_mclk", c3_parents,
 			ARRAY_SIZE(c3_parents), 0, SPEAR1310_PERIP_CLK_CFG,
 			SPEAR1310_C3_CLK_SHIFT, SPEAR1310_C3_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "c3_mux_clk", NULL);
+	clk_register_clkdev(clk, "c3_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "c3_clk", "c3_mux_clk", 0,
+	clk = clk_register_gate(NULL, "c3_clk", "c3_mclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_C3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "c3");
 
 	/* gmac */
-	clk = clk_register_mux(NULL, "gmac_phy_input_mux_clk",
-			gmac_phy_input_parents,
+	clk = clk_register_mux(NULL, "phy_input_mclk", gmac_phy_input_parents,
 			ARRAY_SIZE(gmac_phy_input_parents), 0,
 			SPEAR1310_GMAC_CLK_CFG,
 			SPEAR1310_GMAC_PHY_INPUT_CLK_SHIFT,
 			SPEAR1310_GMAC_PHY_INPUT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gmac_phy_input_mux_clk", NULL);
+	clk_register_clkdev(clk, "phy_input_mclk", NULL);
 
-	clk = clk_register_aux("gmac_phy_synth_clk", "gmac_phy_synth_gate_clk",
-			"gmac_phy_input_mux_clk", 0, SPEAR1310_GMAC_CLK_SYNT,
-			NULL, gmac_rtbl, ARRAY_SIZE(gmac_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "gmac_phy_synth_clk", NULL);
-	clk_register_clkdev(clk1, "gmac_phy_synth_gate_clk", NULL);
+	clk = clk_register_aux("phy_syn_clk", "phy_syn_gclk", "phy_input_mclk",
+			0, SPEAR1310_GMAC_CLK_SYNT, NULL, gmac_rtbl,
+			ARRAY_SIZE(gmac_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "phy_syn_clk", NULL);
+	clk_register_clkdev(clk1, "phy_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "gmac_phy_mux_clk", gmac_phy_parents,
+	clk = clk_register_mux(NULL, "phy_mclk", gmac_phy_parents,
 			ARRAY_SIZE(gmac_phy_parents), 0,
 			SPEAR1310_PERIP_CLK_CFG, SPEAR1310_GMAC_PHY_CLK_SHIFT,
 			SPEAR1310_GMAC_PHY_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "stmmacphy.0");
 
 	/* clcd */
-	clk = clk_register_mux(NULL, "clcd_synth_mux_clk", clcd_synth_parents,
+	clk = clk_register_mux(NULL, "clcd_syn_mclk", clcd_synth_parents,
 			ARRAY_SIZE(clcd_synth_parents), 0,
 			SPEAR1310_CLCD_CLK_SYNT, SPEAR1310_CLCD_SYNT_CLK_SHIFT,
 			SPEAR1310_CLCD_SYNT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "clcd_synth_mux_clk", NULL);
+	clk_register_clkdev(clk, "clcd_syn_mclk", NULL);
 
-	clk = clk_register_frac("clcd_synth_clk", "clcd_synth_mux_clk", 0,
+	clk = clk_register_frac("clcd_syn_clk", "clcd_syn_mclk", 0,
 			SPEAR1310_CLCD_CLK_SYNT, clcd_rtbl,
 			ARRAY_SIZE(clcd_rtbl), &_lock);
-	clk_register_clkdev(clk, "clcd_synth_clk", NULL);
+	clk_register_clkdev(clk, "clcd_syn_clk", NULL);
 
-	clk = clk_register_mux(NULL, "clcd_pixel_mux_clk", clcd_pixel_parents,
+	clk = clk_register_mux(NULL, "clcd_pixel_mclk", clcd_pixel_parents,
 			ARRAY_SIZE(clcd_pixel_parents), 0,
 			SPEAR1310_PERIP_CLK_CFG, SPEAR1310_CLCD_CLK_SHIFT,
 			SPEAR1310_CLCD_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, "clcd_pixel_clk", NULL);
 
-	clk = clk_register_gate(NULL, "clcd_clk", "clcd_pixel_mux_clk", 0,
+	clk = clk_register_gate(NULL, "clcd_clk", "clcd_pixel_mclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_CLCD_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, "clcd_clk", NULL);
 
 	/* i2s */
-	clk = clk_register_mux(NULL, "i2s_src_mux_clk", i2s_src_parents,
+	clk = clk_register_mux(NULL, "i2s_src_mclk", i2s_src_parents,
 			ARRAY_SIZE(i2s_src_parents), 0, SPEAR1310_I2S_CLK_CFG,
 			SPEAR1310_I2S_SRC_CLK_SHIFT, SPEAR1310_I2S_SRC_CLK_MASK,
 			0, &_lock);
 	clk_register_clkdev(clk, "i2s_src_clk", NULL);
 
-	clk = clk_register_aux("i2s_prs1_clk", NULL, "i2s_src_mux_clk", 0,
+	clk = clk_register_aux("i2s_prs1_clk", NULL, "i2s_src_mclk", 0,
 			SPEAR1310_I2S_CLK_CFG, &i2s_prs1_masks, i2s_prs1_rtbl,
 			ARRAY_SIZE(i2s_prs1_rtbl), &_lock, NULL);
 	clk_register_clkdev(clk, "i2s_prs1_clk", NULL);
 
-	clk = clk_register_mux(NULL, "i2s_ref_mux_clk", i2s_ref_parents,
+	clk = clk_register_mux(NULL, "i2s_ref_mclk", i2s_ref_parents,
 			ARRAY_SIZE(i2s_ref_parents), 0, SPEAR1310_I2S_CLK_CFG,
 			SPEAR1310_I2S_REF_SHIFT, SPEAR1310_I2S_REF_SEL_MASK, 0,
 			&_lock);
 	clk_register_clkdev(clk, "i2s_ref_clk", NULL);
 
-	clk = clk_register_gate(NULL, "i2s_ref_pad_clk", "i2s_ref_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2s_ref_pad_clk", "i2s_ref_mclk", 0,
 			SPEAR1310_PERIP2_CLK_ENB, SPEAR1310_I2S_REF_PAD_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, "i2s_ref_pad_clk", NULL);
 
-	clk = clk_register_aux("i2s_sclk_clk", "i2s_sclk_gate_clk",
+	clk = clk_register_aux("i2s_sclk_clk", "i2s_sclk_gclk",
 			"i2s_ref_pad_clk", 0, SPEAR1310_I2S_CLK_CFG,
 			&i2s_sclk_masks, i2s_sclk_rtbl,
 			ARRAY_SIZE(i2s_sclk_rtbl), &_lock, &clk1);
 	clk_register_clkdev(clk, "i2s_sclk_clk", NULL);
-	clk_register_clkdev(clk1, "i2s_sclk_gate_clk", NULL);
+	clk_register_clkdev(clk1, "i2s_sclk_gclk", NULL);
 
 	/* clock derived from ahb clk */
 	clk = clk_register_gate(NULL, "i2c0_clk", "ahb_clk", 0,
@@ -747,13 +745,13 @@ void __init spear1310_clk_init(void)
 			&_lock);
 	clk_register_clkdev(clk, "sysram1_clk", NULL);
 
-	clk = clk_register_aux("adc_synth_clk", "adc_synth_gate_clk", "ahb_clk",
+	clk = clk_register_aux("adc_syn_clk", "adc_syn_gclk", "ahb_clk",
 			0, SPEAR1310_ADC_CLK_SYNT, NULL, adc_rtbl,
 			ARRAY_SIZE(adc_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "adc_synth_clk", NULL);
-	clk_register_clkdev(clk1, "adc_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "adc_syn_clk", NULL);
+	clk_register_clkdev(clk1, "adc_syn_gclk", NULL);
 
-	clk = clk_register_gate(NULL, "adc_clk", "adc_synth_gate_clk", 0,
+	clk = clk_register_gate(NULL, "adc_clk", "adc_syn_gclk", 0,
 			SPEAR1310_PERIP1_CLK_ENB, SPEAR1310_ADC_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "adc_clk");
@@ -790,37 +788,37 @@ void __init spear1310_clk_init(void)
 	clk_register_clkdev(clk, NULL, "e0300000.kbd");
 
 	/* RAS clks */
-	clk = clk_register_mux(NULL, "gen_synth0_1_mux_clk",
-			gen_synth0_1_parents, ARRAY_SIZE(gen_synth0_1_parents),
-			0, SPEAR1310_PLL_CFG, SPEAR1310_RAS_SYNT0_1_CLK_SHIFT,
+	clk = clk_register_mux(NULL, "gen_syn0_1_mclk", gen_synth0_1_parents,
+			ARRAY_SIZE(gen_synth0_1_parents), 0, SPEAR1310_PLL_CFG,
+			SPEAR1310_RAS_SYNT0_1_CLK_SHIFT,
 			SPEAR1310_RAS_SYNT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gen_synth0_1_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn0_1_clk", NULL);
 
-	clk = clk_register_mux(NULL, "gen_synth2_3_mux_clk",
-			gen_synth2_3_parents, ARRAY_SIZE(gen_synth2_3_parents),
-			0, SPEAR1310_PLL_CFG, SPEAR1310_RAS_SYNT2_3_CLK_SHIFT,
+	clk = clk_register_mux(NULL, "gen_syn2_3_mclk", gen_synth2_3_parents,
+			ARRAY_SIZE(gen_synth2_3_parents), 0, SPEAR1310_PLL_CFG,
+			SPEAR1310_RAS_SYNT2_3_CLK_SHIFT,
 			SPEAR1310_RAS_SYNT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gen_synth2_3_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn2_3_clk", NULL);
 
-	clk = clk_register_frac("gen_synth0_clk", "gen_synth0_1_clk", 0,
+	clk = clk_register_frac("gen_syn0_clk", "gen_syn0_1_clk", 0,
 			SPEAR1310_RAS_CLK_SYNT0, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth0_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn0_clk", NULL);
 
-	clk = clk_register_frac("gen_synth1_clk", "gen_synth0_1_clk", 0,
+	clk = clk_register_frac("gen_syn1_clk", "gen_syn0_1_clk", 0,
 			SPEAR1310_RAS_CLK_SYNT1, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth1_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn1_clk", NULL);
 
-	clk = clk_register_frac("gen_synth2_clk", "gen_synth2_3_clk", 0,
+	clk = clk_register_frac("gen_syn2_clk", "gen_syn2_3_clk", 0,
 			SPEAR1310_RAS_CLK_SYNT2, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth2_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn2_clk", NULL);
 
-	clk = clk_register_frac("gen_synth3_clk", "gen_synth2_3_clk", 0,
+	clk = clk_register_frac("gen_syn3_clk", "gen_syn2_3_clk", 0,
 			SPEAR1310_RAS_CLK_SYNT3, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth3_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn3_clk", NULL);
 
 	clk = clk_register_gate(NULL, "ras_osc_24m_clk", "osc_24m_clk", 0,
 			SPEAR1310_RAS_CLK_ENB, SPEAR1310_OSC_24M_CLK_ENB, 0,
@@ -847,7 +845,7 @@ void __init spear1310_clk_init(void)
 			&_lock);
 	clk_register_clkdev(clk, "ras_pll3_clk", NULL);
 
-	clk = clk_register_gate(NULL, "ras_tx125_clk", "gmii_125m_pad_clk", 0,
+	clk = clk_register_gate(NULL, "ras_tx125_clk", "gmii_pad_clk", 0,
 			SPEAR1310_RAS_CLK_ENB, SPEAR1310_C125M_PAD_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, "ras_tx125_clk", NULL);
@@ -912,7 +910,7 @@ void __init spear1310_clk_init(void)
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5c700000.eth");
 
-	clk = clk_register_mux(NULL, "smii_rgmii_phy_mux_clk",
+	clk = clk_register_mux(NULL, "smii_rgmii_phy_mclk",
 			smii_rgmii_phy_parents,
 			ARRAY_SIZE(smii_rgmii_phy_parents), 0,
 			SPEAR1310_RAS_CTRL_REG1,
@@ -922,184 +920,184 @@ void __init spear1310_clk_init(void)
 	clk_register_clkdev(clk, NULL, "stmmacphy.2");
 	clk_register_clkdev(clk, NULL, "stmmacphy.4");
 
-	clk = clk_register_mux(NULL, "rmii_phy_mux_clk", rmii_phy_parents,
+	clk = clk_register_mux(NULL, "rmii_phy_mclk", rmii_phy_parents,
 			ARRAY_SIZE(rmii_phy_parents), 0,
 			SPEAR1310_RAS_CTRL_REG1, SPEAR1310_RMII_PHY_CLK_SHIFT,
 			SPEAR1310_PHY_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "stmmacphy.3");
 
-	clk = clk_register_mux(NULL, "uart1_mux_clk", uart_parents,
+	clk = clk_register_mux(NULL, "uart1_mclk", uart_parents,
 			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_UART1_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
 			0, &_lock);
-	clk_register_clkdev(clk, "uart1_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart1_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart1_clk", "uart1_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart1_clk", "uart1_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5c800000.serial");
 
-	clk = clk_register_mux(NULL, "uart2_mux_clk", uart_parents,
+	clk = clk_register_mux(NULL, "uart2_mclk", uart_parents,
 			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_UART2_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
 			0, &_lock);
-	clk_register_clkdev(clk, "uart2_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart2_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart2_clk", "uart2_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart2_clk", "uart2_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5c900000.serial");
 
-	clk = clk_register_mux(NULL, "uart3_mux_clk", uart_parents,
+	clk = clk_register_mux(NULL, "uart3_mclk", uart_parents,
 			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_UART3_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
 			0, &_lock);
-	clk_register_clkdev(clk, "uart3_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart3_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart3_clk", "uart3_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart3_clk", "uart3_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5ca00000.serial");
 
-	clk = clk_register_mux(NULL, "uart4_mux_clk", uart_parents,
+	clk = clk_register_mux(NULL, "uart4_mclk", uart_parents,
 			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_UART4_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
 			0, &_lock);
-	clk_register_clkdev(clk, "uart4_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart4_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart4_clk", "uart4_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart4_clk", "uart4_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART4_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5cb00000.serial");
 
-	clk = clk_register_mux(NULL, "uart5_mux_clk", uart_parents,
+	clk = clk_register_mux(NULL, "uart5_mclk", uart_parents,
 			ARRAY_SIZE(uart_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_UART5_CLK_SHIFT, SPEAR1310_RAS_UART_CLK_MASK,
 			0, &_lock);
-	clk_register_clkdev(clk, "uart5_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart5_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart5_clk", "uart5_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart5_clk", "uart5_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_UART5_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5cc00000.serial");
 
-	clk = clk_register_mux(NULL, "i2c1_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c1_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C1_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c1_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c1_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c1_clk", "i2c1_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c1_clk", "i2c1_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5cd00000.i2c");
 
-	clk = clk_register_mux(NULL, "i2c2_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c2_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C2_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c2_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c2_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c2_clk", "i2c2_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c2_clk", "i2c2_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5ce00000.i2c");
 
-	clk = clk_register_mux(NULL, "i2c3_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c3_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C3_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c3_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c3_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c3_clk", "i2c3_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c3_clk", "i2c3_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5cf00000.i2c");
 
-	clk = clk_register_mux(NULL, "i2c4_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c4_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C4_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c4_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c4_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c4_clk", "i2c4_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c4_clk", "i2c4_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C4_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5d000000.i2c");
 
-	clk = clk_register_mux(NULL, "i2c5_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c5_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C5_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c5_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c5_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c5_clk", "i2c5_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c5_clk", "i2c5_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C5_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5d100000.i2c");
 
-	clk = clk_register_mux(NULL, "i2c6_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c6_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C6_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c6_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c6_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c6_clk", "i2c6_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c6_clk", "i2c6_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C6_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5d200000.i2c");
 
-	clk = clk_register_mux(NULL, "i2c7_mux_clk", i2c_parents,
+	clk = clk_register_mux(NULL, "i2c7_mclk", i2c_parents,
 			ARRAY_SIZE(i2c_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_I2C7_CLK_SHIFT, SPEAR1310_I2C_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "i2c7_mux_clk", NULL);
+	clk_register_clkdev(clk, "i2c7_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "i2c7_clk", "i2c7_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2c7_clk", "i2c7_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_I2C7_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5d300000.i2c");
 
-	clk = clk_register_mux(NULL, "ssp1_mux_clk", ssp1_parents,
+	clk = clk_register_mux(NULL, "ssp1_mclk", ssp1_parents,
 			ARRAY_SIZE(ssp1_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_SSP1_CLK_SHIFT, SPEAR1310_SSP1_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "ssp1_mux_clk", NULL);
+	clk_register_clkdev(clk, "ssp1_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "ssp1_clk", "ssp1_mux_clk", 0,
+	clk = clk_register_gate(NULL, "ssp1_clk", "ssp1_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_SSP1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "5d400000.spi");
 
-	clk = clk_register_mux(NULL, "pci_mux_clk", pci_parents,
+	clk = clk_register_mux(NULL, "pci_mclk", pci_parents,
 			ARRAY_SIZE(pci_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_PCI_CLK_SHIFT, SPEAR1310_PCI_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "pci_mux_clk", NULL);
+	clk_register_clkdev(clk, "pci_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "pci_clk", "pci_mux_clk", 0,
+	clk = clk_register_gate(NULL, "pci_clk", "pci_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_PCI_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "pci");
 
-	clk = clk_register_mux(NULL, "tdm1_mux_clk", tdm_parents,
+	clk = clk_register_mux(NULL, "tdm1_mclk", tdm_parents,
 			ARRAY_SIZE(tdm_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_TDM1_CLK_SHIFT, SPEAR1310_TDM_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "tdm1_mux_clk", NULL);
+	clk_register_clkdev(clk, "tdm1_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "tdm1_clk", "tdm1_mux_clk", 0,
+	clk = clk_register_gate(NULL, "tdm1_clk", "tdm1_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_TDM1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "tdm_hdlc.0");
 
-	clk = clk_register_mux(NULL, "tdm2_mux_clk", tdm_parents,
+	clk = clk_register_mux(NULL, "tdm2_mclk", tdm_parents,
 			ARRAY_SIZE(tdm_parents), 0, SPEAR1310_RAS_CTRL_REG0,
 			SPEAR1310_TDM2_CLK_SHIFT, SPEAR1310_TDM_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "tdm2_mux_clk", NULL);
+	clk_register_clkdev(clk, "tdm2_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "tdm2_clk", "tdm2_mux_clk", 0,
+	clk = clk_register_gate(NULL, "tdm2_clk", "tdm2_mclk", 0,
 			SPEAR1310_RAS_SW_CLK_CTRL, SPEAR1310_TDM2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "tdm_hdlc.1");
diff --git a/drivers/clk/spear/spear1340_clock.c b/drivers/clk/spear/spear1340_clock.c
index e3ea7216223..2352cee7f64 100644
--- a/drivers/clk/spear/spear1340_clock.c
+++ b/drivers/clk/spear/spear1340_clock.c
@@ -369,27 +369,25 @@ static struct frac_rate_tbl gen_rtbl[] = {
 
 /* clock parents */
 static const char *vco_parents[] = { "osc_24m_clk", "osc_25m_clk", };
-static const char *sys_parents[] = { "none", "pll1_clk", "none", "none",
-	"sys_synth_clk", "none", "pll2_clk", "pll3_clk", };
-static const char *ahb_parents[] = { "cpu_div3_clk", "amba_synth_clk", };
+static const char *sys_parents[] = { "pll1_clk", "pll1_clk", "pll1_clk",
+	"pll1_clk", "sys_synth_clk", "sys_synth_clk", "pll2_clk", "pll3_clk", };
+static const char *ahb_parents[] = { "cpu_div3_clk", "amba_syn_clk", };
 static const char *gpt_parents[] = { "osc_24m_clk", "apb_clk", };
 static const char *uart0_parents[] = { "pll5_clk", "osc_24m_clk",
-	"uart0_synth_gate_clk", };
+	"uart0_syn_gclk", };
 static const char *uart1_parents[] = { "pll5_clk", "osc_24m_clk",
-	"uart1_synth_gate_clk", };
-static const char *c3_parents[] = { "pll5_clk", "c3_synth_gate_clk", };
-static const char *gmac_phy_input_parents[] = { "gmii_125m_pad_clk", "pll2_clk",
+	"uart1_syn_gclk", };
+static const char *c3_parents[] = { "pll5_clk", "c3_syn_gclk", };
+static const char *gmac_phy_input_parents[] = { "gmii_pad_clk", "pll2_clk",
 	"osc_25m_clk", };
-static const char *gmac_phy_parents[] = { "gmac_phy_input_mux_clk",
-	"gmac_phy_synth_gate_clk", };
+static const char *gmac_phy_parents[] = { "phy_input_mclk", "phy_syn_gclk", };
 static const char *clcd_synth_parents[] = { "vco1div4_clk", "pll2_clk", };
-static const char *clcd_pixel_parents[] = { "pll5_clk", "clcd_synth_clk", };
+static const char *clcd_pixel_parents[] = { "pll5_clk", "clcd_syn_clk", };
 static const char *i2s_src_parents[] = { "vco1div2_clk", "pll2_clk", "pll3_clk",
 	"i2s_src_pad_clk", };
-static const char *i2s_ref_parents[] = { "i2s_src_mux_clk", "i2s_prs1_clk", };
-static const char *spdif_out_parents[] = { "i2s_src_pad_clk", "gen_synth2_clk",
-};
-static const char *spdif_in_parents[] = { "pll2_clk", "gen_synth3_clk", };
+static const char *i2s_ref_parents[] = { "i2s_src_mclk", "i2s_prs1_clk", };
+static const char *spdif_out_parents[] = { "i2s_src_pad_clk", "gen_syn2_clk", };
+static const char *spdif_in_parents[] = { "pll2_clk", "gen_syn3_clk", };
 
 static const char *gen_synth0_1_parents[] = { "vco1div4_clk", "vco3div2_clk",
 	"pll3_clk", };
@@ -415,9 +413,9 @@ void __init spear1340_clk_init(void)
 			25000000);
 	clk_register_clkdev(clk, "osc_25m_clk", NULL);
 
-	clk = clk_register_fixed_rate(NULL, "gmii_125m_pad_clk", NULL,
-			CLK_IS_ROOT, 125000000);
-	clk_register_clkdev(clk, "gmii_125m_pad_clk", NULL);
+	clk = clk_register_fixed_rate(NULL, "gmii_pad_clk", NULL, CLK_IS_ROOT,
+			125000000);
+	clk_register_clkdev(clk, "gmii_pad_clk", NULL);
 
 	clk = clk_register_fixed_rate(NULL, "i2s_src_pad_clk", NULL,
 			CLK_IS_ROOT, 12288000);
@@ -431,35 +429,35 @@ void __init spear1340_clk_init(void)
 
 	/* clock derived from 24 or 25 MHz osc clk */
 	/* vco-pll */
-	clk = clk_register_mux(NULL, "vco1_mux_clk", vco_parents,
+	clk = clk_register_mux(NULL, "vco1_mclk", vco_parents,
 			ARRAY_SIZE(vco_parents), 0, SPEAR1340_PLL_CFG,
 			SPEAR1340_PLL1_CLK_SHIFT, SPEAR1340_PLL_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "vco1_mux_clk", NULL);
-	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "vco1_mux_clk",
-			0, SPEAR1340_PLL1_CTR, SPEAR1340_PLL1_FRQ, pll_rtbl,
+	clk_register_clkdev(clk, "vco1_mclk", NULL);
+	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "vco1_mclk", 0,
+			SPEAR1340_PLL1_CTR, SPEAR1340_PLL1_FRQ, pll_rtbl,
 			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco1_clk", NULL);
 	clk_register_clkdev(clk1, "pll1_clk", NULL);
 
-	clk = clk_register_mux(NULL, "vco2_mux_clk", vco_parents,
+	clk = clk_register_mux(NULL, "vco2_mclk", vco_parents,
 			ARRAY_SIZE(vco_parents), 0, SPEAR1340_PLL_CFG,
 			SPEAR1340_PLL2_CLK_SHIFT, SPEAR1340_PLL_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "vco2_mux_clk", NULL);
-	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "vco2_mux_clk",
-			0, SPEAR1340_PLL2_CTR, SPEAR1340_PLL2_FRQ, pll_rtbl,
+	clk_register_clkdev(clk, "vco2_mclk", NULL);
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "vco2_mclk", 0,
+			SPEAR1340_PLL2_CTR, SPEAR1340_PLL2_FRQ, pll_rtbl,
 			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco2_clk", NULL);
 	clk_register_clkdev(clk1, "pll2_clk", NULL);
 
-	clk = clk_register_mux(NULL, "vco3_mux_clk", vco_parents,
+	clk = clk_register_mux(NULL, "vco3_mclk", vco_parents,
 			ARRAY_SIZE(vco_parents), 0, SPEAR1340_PLL_CFG,
 			SPEAR1340_PLL3_CLK_SHIFT, SPEAR1340_PLL_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "vco3_mux_clk", NULL);
-	clk = clk_register_vco_pll("vco3_clk", "pll3_clk", NULL, "vco3_mux_clk",
-			0, SPEAR1340_PLL3_CTR, SPEAR1340_PLL3_FRQ, pll_rtbl,
+	clk_register_clkdev(clk, "vco3_mclk", NULL);
+	clk = clk_register_vco_pll("vco3_clk", "pll3_clk", NULL, "vco3_mclk", 0,
+			SPEAR1340_PLL3_CTR, SPEAR1340_PLL3_FRQ, pll_rtbl,
 			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco3_clk", NULL);
 	clk_register_clkdev(clk1, "pll3_clk", NULL);
@@ -498,7 +496,7 @@ void __init spear1340_clk_init(void)
 	/* peripherals */
 	clk_register_fixed_factor(NULL, "thermal_clk", "osc_24m_clk", 0, 1,
 			128);
-	clk = clk_register_gate(NULL, "thermal_gate_clk", "thermal_clk", 0,
+	clk = clk_register_gate(NULL, "thermal_gclk", "thermal_clk", 0,
 			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_THSENS_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_thermal");
@@ -509,23 +507,23 @@ void __init spear1340_clk_init(void)
 	clk_register_clkdev(clk, "ddr_clk", NULL);
 
 	/* clock derived from pll1 clk */
-	clk = clk_register_frac("sys_synth_clk", "vco1div2_clk", 0,
+	clk = clk_register_frac("sys_syn_clk", "vco1div2_clk", 0,
 			SPEAR1340_SYS_CLK_SYNT, sys_synth_rtbl,
 			ARRAY_SIZE(sys_synth_rtbl), &_lock);
-	clk_register_clkdev(clk, "sys_synth_clk", NULL);
+	clk_register_clkdev(clk, "sys_syn_clk", NULL);
 
-	clk = clk_register_frac("amba_synth_clk", "vco1div2_clk", 0,
+	clk = clk_register_frac("amba_syn_clk", "vco1div2_clk", 0,
 			SPEAR1340_AMBA_CLK_SYNT, amba_synth_rtbl,
 			ARRAY_SIZE(amba_synth_rtbl), &_lock);
-	clk_register_clkdev(clk, "amba_synth_clk", NULL);
+	clk_register_clkdev(clk, "amba_syn_clk", NULL);
 
-	clk = clk_register_mux(NULL, "sys_mux_clk", sys_parents,
+	clk = clk_register_mux(NULL, "sys_mclk", sys_parents,
 			ARRAY_SIZE(sys_parents), 0, SPEAR1340_SYS_CLK_CTRL,
 			SPEAR1340_SCLK_SRC_SEL_SHIFT,
 			SPEAR1340_SCLK_SRC_SEL_MASK, 0, &_lock);
 	clk_register_clkdev(clk, "sys_clk", NULL);
 
-	clk = clk_register_fixed_factor(NULL, "cpu_clk", "sys_mux_clk", 0, 1,
+	clk = clk_register_fixed_factor(NULL, "cpu_clk", "sys_mclk", 0, 1,
 			2);
 	clk_register_clkdev(clk, "cpu_clk", NULL);
 
@@ -548,194 +546,193 @@ void __init spear1340_clk_init(void)
 	clk_register_clkdev(clk, "apb_clk", NULL);
 
 	/* gpt clocks */
-	clk = clk_register_mux(NULL, "gpt0_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt0_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_GPT0_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt0_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt0_clk", "gpt0_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt0_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt0_clk", "gpt0_mclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GPT0_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt0");
 
-	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt1_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_GPT1_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt1_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_GPT1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt1");
 
-	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt2_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_GPT2_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt2_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mclk", 0,
 			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_GPT2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt2");
 
-	clk = clk_register_mux(NULL, "gpt3_mux_clk", gpt_parents,
+	clk = clk_register_mux(NULL, "gpt3_mclk", gpt_parents,
 			ARRAY_SIZE(gpt_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_GPT3_CLK_SHIFT, SPEAR1340_GPT_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gpt3_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt3_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mclk", 0,
 			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_GPT3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "gpt3");
 
 	/* others */
-	clk = clk_register_aux("uart0_synth_clk", "uart0_synth_gate_clk",
+	clk = clk_register_aux("uart0_syn_clk", "uart0_syn_gclk",
 			"vco1div2_clk", 0, SPEAR1340_UART0_CLK_SYNT, NULL,
 			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "uart0_synth_clk", NULL);
-	clk_register_clkdev(clk1, "uart0_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "uart0_syn_clk", NULL);
+	clk_register_clkdev(clk1, "uart0_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "uart0_mux_clk", uart0_parents,
+	clk = clk_register_mux(NULL, "uart0_mclk", uart0_parents,
 			ARRAY_SIZE(uart0_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_UART0_CLK_SHIFT, SPEAR1340_UART_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "uart0_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart0_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart0_clk", "uart0_mux_clk", 0,
+	clk = clk_register_gate(NULL, "uart0_clk", "uart0_mclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UART0_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "e0000000.serial");
 
-	clk = clk_register_aux("uart1_synth_clk", "uart1_synth_gate_clk",
+	clk = clk_register_aux("uart1_syn_clk", "uart1_syn_gclk",
 			"vco1div2_clk", 0, SPEAR1340_UART1_CLK_SYNT, NULL,
 			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "uart1_synth_clk", NULL);
-	clk_register_clkdev(clk1, "uart1_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "uart1_syn_clk", NULL);
+	clk_register_clkdev(clk1, "uart1_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "uart1_mux_clk", uart1_parents,
+	clk = clk_register_mux(NULL, "uart1_mclk", uart1_parents,
 			ARRAY_SIZE(uart1_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_UART1_CLK_SHIFT, SPEAR1340_UART_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "uart1_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart1_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart1_clk", "uart1_mux_clk", 0,
-			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_UART1_CLK_ENB, 0,
+	clk = clk_register_gate(NULL, "uart1_clk", "uart1_mclk", 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_UART1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "b4100000.serial");
 
-	clk = clk_register_aux("sdhci_synth_clk", "sdhci_synth_gate_clk",
+	clk = clk_register_aux("sdhci_syn_clk", "sdhci_syn_gclk",
 			"vco1div2_clk", 0, SPEAR1340_SDHCI_CLK_SYNT, NULL,
 			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "sdhci_synth_clk", NULL);
-	clk_register_clkdev(clk1, "sdhci_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "sdhci_syn_clk", NULL);
+	clk_register_clkdev(clk1, "sdhci_syn_gclk", NULL);
 
-	clk = clk_register_gate(NULL, "sdhci_clk", "sdhci_synth_gate_clk", 0,
+	clk = clk_register_gate(NULL, "sdhci_clk", "sdhci_syn_gclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_SDHCI_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "b3000000.sdhci");
 
-	clk = clk_register_aux("cfxd_synth_clk", "cfxd_synth_gate_clk",
-			"vco1div2_clk", 0, SPEAR1340_CFXD_CLK_SYNT, NULL,
-			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "cfxd_synth_clk", NULL);
-	clk_register_clkdev(clk1, "cfxd_synth_gate_clk", NULL);
+	clk = clk_register_aux("cfxd_syn_clk", "cfxd_syn_gclk", "vco1div2_clk",
+			0, SPEAR1340_CFXD_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "cfxd_syn_clk", NULL);
+	clk_register_clkdev(clk1, "cfxd_syn_gclk", NULL);
 
-	clk = clk_register_gate(NULL, "cfxd_clk", "cfxd_synth_gate_clk", 0,
+	clk = clk_register_gate(NULL, "cfxd_clk", "cfxd_syn_gclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_CFXD_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "b2800000.cf");
 	clk_register_clkdev(clk, NULL, "arasan_xd");
 
-	clk = clk_register_aux("c3_synth_clk", "c3_synth_gate_clk",
-			"vco1div2_clk", 0, SPEAR1340_C3_CLK_SYNT, NULL,
-			aux_rtbl, ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "c3_synth_clk", NULL);
-	clk_register_clkdev(clk1, "c3_synth_gate_clk", NULL);
+	clk = clk_register_aux("c3_syn_clk", "c3_syn_gclk", "vco1div2_clk", 0,
+			SPEAR1340_C3_CLK_SYNT, NULL, aux_rtbl,
+			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "c3_syn_clk", NULL);
+	clk_register_clkdev(clk1, "c3_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "c3_mux_clk", c3_parents,
+	clk = clk_register_mux(NULL, "c3_mclk", c3_parents,
 			ARRAY_SIZE(c3_parents), 0, SPEAR1340_PERIP_CLK_CFG,
 			SPEAR1340_C3_CLK_SHIFT, SPEAR1340_C3_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "c3_mux_clk", NULL);
+	clk_register_clkdev(clk, "c3_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "c3_clk", "c3_mux_clk", 0,
+	clk = clk_register_gate(NULL, "c3_clk", "c3_mclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_C3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "c3");
 
 	/* gmac */
-	clk = clk_register_mux(NULL, "gmac_phy_input_mux_clk",
-			gmac_phy_input_parents,
+	clk = clk_register_mux(NULL, "phy_input_mclk", gmac_phy_input_parents,
 			ARRAY_SIZE(gmac_phy_input_parents), 0,
 			SPEAR1340_GMAC_CLK_CFG,
 			SPEAR1340_GMAC_PHY_INPUT_CLK_SHIFT,
 			SPEAR1340_GMAC_PHY_INPUT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gmac_phy_input_mux_clk", NULL);
+	clk_register_clkdev(clk, "phy_input_mclk", NULL);
 
-	clk = clk_register_aux("gmac_phy_synth_clk", "gmac_phy_synth_gate_clk",
-			"gmac_phy_input_mux_clk", 0, SPEAR1340_GMAC_CLK_SYNT,
-			NULL, gmac_rtbl, ARRAY_SIZE(gmac_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "gmac_phy_synth_clk", NULL);
-	clk_register_clkdev(clk1, "gmac_phy_synth_gate_clk", NULL);
+	clk = clk_register_aux("phy_syn_clk", "phy_syn_gclk", "phy_input_mclk",
+			0, SPEAR1340_GMAC_CLK_SYNT, NULL, gmac_rtbl,
+			ARRAY_SIZE(gmac_rtbl), &_lock, &clk1);
+	clk_register_clkdev(clk, "phy_syn_clk", NULL);
+	clk_register_clkdev(clk1, "phy_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "gmac_phy_mux_clk", gmac_phy_parents,
+	clk = clk_register_mux(NULL, "phy_mclk", gmac_phy_parents,
 			ARRAY_SIZE(gmac_phy_parents), 0,
 			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_GMAC_PHY_CLK_SHIFT,
 			SPEAR1340_GMAC_PHY_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "stmmacphy.0");
 
 	/* clcd */
-	clk = clk_register_mux(NULL, "clcd_synth_mux_clk", clcd_synth_parents,
+	clk = clk_register_mux(NULL, "clcd_syn_mclk", clcd_synth_parents,
 			ARRAY_SIZE(clcd_synth_parents), 0,
 			SPEAR1340_CLCD_CLK_SYNT, SPEAR1340_CLCD_SYNT_CLK_SHIFT,
 			SPEAR1340_CLCD_SYNT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "clcd_synth_mux_clk", NULL);
+	clk_register_clkdev(clk, "clcd_syn_mclk", NULL);
 
-	clk = clk_register_frac("clcd_synth_clk", "clcd_synth_mux_clk", 0,
+	clk = clk_register_frac("clcd_syn_clk", "clcd_syn_mclk", 0,
 			SPEAR1340_CLCD_CLK_SYNT, clcd_rtbl,
 			ARRAY_SIZE(clcd_rtbl), &_lock);
-	clk_register_clkdev(clk, "clcd_synth_clk", NULL);
+	clk_register_clkdev(clk, "clcd_syn_clk", NULL);
 
-	clk = clk_register_mux(NULL, "clcd_pixel_mux_clk", clcd_pixel_parents,
+	clk = clk_register_mux(NULL, "clcd_pixel_mclk", clcd_pixel_parents,
 			ARRAY_SIZE(clcd_pixel_parents), 0,
 			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_CLCD_CLK_SHIFT,
 			SPEAR1340_CLCD_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, "clcd_pixel_clk", NULL);
 
-	clk = clk_register_gate(NULL, "clcd_clk", "clcd_pixel_mux_clk", 0,
+	clk = clk_register_gate(NULL, "clcd_clk", "clcd_pixel_mclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_CLCD_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, "clcd_clk", NULL);
 
 	/* i2s */
-	clk = clk_register_mux(NULL, "i2s_src_mux_clk", i2s_src_parents,
+	clk = clk_register_mux(NULL, "i2s_src_mclk", i2s_src_parents,
 			ARRAY_SIZE(i2s_src_parents), 0, SPEAR1340_I2S_CLK_CFG,
 			SPEAR1340_I2S_SRC_CLK_SHIFT, SPEAR1340_I2S_SRC_CLK_MASK,
 			0, &_lock);
 	clk_register_clkdev(clk, "i2s_src_clk", NULL);
 
-	clk = clk_register_aux("i2s_prs1_clk", NULL, "i2s_src_mux_clk", 0,
+	clk = clk_register_aux("i2s_prs1_clk", NULL, "i2s_src_mclk", 0,
 			SPEAR1340_I2S_CLK_CFG, &i2s_prs1_masks, i2s_prs1_rtbl,
 			ARRAY_SIZE(i2s_prs1_rtbl), &_lock, NULL);
 	clk_register_clkdev(clk, "i2s_prs1_clk", NULL);
 
-	clk = clk_register_mux(NULL, "i2s_ref_mux_clk", i2s_ref_parents,
+	clk = clk_register_mux(NULL, "i2s_ref_mclk", i2s_ref_parents,
 			ARRAY_SIZE(i2s_ref_parents), 0, SPEAR1340_I2S_CLK_CFG,
 			SPEAR1340_I2S_REF_SHIFT, SPEAR1340_I2S_REF_SEL_MASK, 0,
 			&_lock);
 	clk_register_clkdev(clk, "i2s_ref_clk", NULL);
 
-	clk = clk_register_gate(NULL, "i2s_ref_pad_clk", "i2s_ref_mux_clk", 0,
+	clk = clk_register_gate(NULL, "i2s_ref_pad_clk", "i2s_ref_mclk", 0,
 			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_I2S_REF_PAD_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, "i2s_ref_pad_clk", NULL);
 
-	clk = clk_register_aux("i2s_sclk_clk", "i2s_sclk_gate_clk",
-			"i2s_ref_mux_clk", 0, SPEAR1340_I2S_CLK_CFG,
-			&i2s_sclk_masks, i2s_sclk_rtbl,
-			ARRAY_SIZE(i2s_sclk_rtbl), &_lock, &clk1);
+	clk = clk_register_aux("i2s_sclk_clk", "i2s_sclk_gclk", "i2s_ref_mclk",
+			0, SPEAR1340_I2S_CLK_CFG, &i2s_sclk_masks,
+			i2s_sclk_rtbl, ARRAY_SIZE(i2s_sclk_rtbl), &_lock,
+			&clk1);
 	clk_register_clkdev(clk, "i2s_sclk_clk", NULL);
-	clk_register_clkdev(clk1, "i2s_sclk_gate_clk", NULL);
+	clk_register_clkdev(clk1, "i2s_sclk_gclk", NULL);
 
 	/* clock derived from ahb clk */
 	clk = clk_register_gate(NULL, "i2c0_clk", "ahb_clk", 0,
@@ -744,7 +741,7 @@ void __init spear1340_clk_init(void)
 	clk_register_clkdev(clk, NULL, "e0280000.i2c");
 
 	clk = clk_register_gate(NULL, "i2c1_clk", "ahb_clk", 0,
-			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_I2C1_CLK_ENB, 0,
+			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_I2C1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "b4000000.i2c");
 
@@ -800,13 +797,13 @@ void __init spear1340_clk_init(void)
 			&_lock);
 	clk_register_clkdev(clk, "sysram1_clk", NULL);
 
-	clk = clk_register_aux("adc_synth_clk", "adc_synth_gate_clk", "ahb_clk",
+	clk = clk_register_aux("adc_syn_clk", "adc_syn_gclk", "ahb_clk",
 			0, SPEAR1340_ADC_CLK_SYNT, NULL, adc_rtbl,
 			ARRAY_SIZE(adc_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "adc_synth_clk", NULL);
-	clk_register_clkdev(clk1, "adc_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "adc_syn_clk", NULL);
+	clk_register_clkdev(clk1, "adc_syn_gclk", NULL);
 
-	clk = clk_register_gate(NULL, "adc_clk", "adc_synth_gate_clk", 0,
+	clk = clk_register_gate(NULL, "adc_clk", "adc_syn_gclk", 0,
 			SPEAR1340_PERIP1_CLK_ENB, SPEAR1340_ADC_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "adc_clk");
@@ -843,39 +840,39 @@ void __init spear1340_clk_init(void)
 	clk_register_clkdev(clk, NULL, "e0300000.kbd");
 
 	/* RAS clks */
-	clk = clk_register_mux(NULL, "gen_synth0_1_mux_clk",
-			gen_synth0_1_parents, ARRAY_SIZE(gen_synth0_1_parents),
-			0, SPEAR1340_PLL_CFG, SPEAR1340_GEN_SYNT0_1_CLK_SHIFT,
+	clk = clk_register_mux(NULL, "gen_syn0_1_mclk", gen_synth0_1_parents,
+			ARRAY_SIZE(gen_synth0_1_parents), 0, SPEAR1340_PLL_CFG,
+			SPEAR1340_GEN_SYNT0_1_CLK_SHIFT,
 			SPEAR1340_GEN_SYNT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gen_synth0_1_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn0_1_clk", NULL);
 
-	clk = clk_register_mux(NULL, "gen_synth2_3_mux_clk",
-			gen_synth2_3_parents, ARRAY_SIZE(gen_synth2_3_parents),
-			0, SPEAR1340_PLL_CFG, SPEAR1340_GEN_SYNT2_3_CLK_SHIFT,
+	clk = clk_register_mux(NULL, "gen_syn2_3_mclk", gen_synth2_3_parents,
+			ARRAY_SIZE(gen_synth2_3_parents), 0, SPEAR1340_PLL_CFG,
+			SPEAR1340_GEN_SYNT2_3_CLK_SHIFT,
 			SPEAR1340_GEN_SYNT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gen_synth2_3_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn2_3_clk", NULL);
 
-	clk = clk_register_frac("gen_synth0_clk", "gen_synth0_1_clk", 0,
+	clk = clk_register_frac("gen_syn0_clk", "gen_syn0_1_clk", 0,
 			SPEAR1340_GEN_CLK_SYNT0, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth0_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn0_clk", NULL);
 
-	clk = clk_register_frac("gen_synth1_clk", "gen_synth0_1_clk", 0,
+	clk = clk_register_frac("gen_syn1_clk", "gen_syn0_1_clk", 0,
 			SPEAR1340_GEN_CLK_SYNT1, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth1_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn1_clk", NULL);
 
-	clk = clk_register_frac("gen_synth2_clk", "gen_synth2_3_clk", 0,
+	clk = clk_register_frac("gen_syn2_clk", "gen_syn2_3_clk", 0,
 			SPEAR1340_GEN_CLK_SYNT2, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth2_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn2_clk", NULL);
 
-	clk = clk_register_frac("gen_synth3_clk", "gen_synth2_3_clk", 0,
+	clk = clk_register_frac("gen_syn3_clk", "gen_syn2_3_clk", 0,
 			SPEAR1340_GEN_CLK_SYNT3, gen_rtbl, ARRAY_SIZE(gen_rtbl),
 			&_lock);
-	clk_register_clkdev(clk, "gen_synth3_clk", NULL);
+	clk_register_clkdev(clk, "gen_syn3_clk", NULL);
 
-	clk = clk_register_gate(NULL, "mali_clk", "gen_synth3_clk", 0,
+	clk = clk_register_gate(NULL, "mali_clk", "gen_syn3_clk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_MALI_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "mali");
@@ -890,74 +887,74 @@ void __init spear1340_clk_init(void)
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_cec.1");
 
-	clk = clk_register_mux(NULL, "spdif_out_mux_clk", spdif_out_parents,
+	clk = clk_register_mux(NULL, "spdif_out_mclk", spdif_out_parents,
 			ARRAY_SIZE(spdif_out_parents), 0,
 			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_SPDIF_OUT_CLK_SHIFT,
 			SPEAR1340_SPDIF_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "spdif_out_mux_clk", NULL);
+	clk_register_clkdev(clk, "spdif_out_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "spdif_out_clk", "spdif_out_mux_clk", 0,
+	clk = clk_register_gate(NULL, "spdif_out_clk", "spdif_out_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_SPDIF_OUT_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, NULL, "spdif-out");
 
-	clk = clk_register_mux(NULL, "spdif_in_mux_clk", spdif_in_parents,
+	clk = clk_register_mux(NULL, "spdif_in_mclk", spdif_in_parents,
 			ARRAY_SIZE(spdif_in_parents), 0,
 			SPEAR1340_PERIP_CLK_CFG, SPEAR1340_SPDIF_IN_CLK_SHIFT,
 			SPEAR1340_SPDIF_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "spdif_in_mux_clk", NULL);
+	clk_register_clkdev(clk, "spdif_in_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "spdif_in_clk", "spdif_in_mux_clk", 0,
+	clk = clk_register_gate(NULL, "spdif_in_clk", "spdif_in_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_SPDIF_IN_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spdif-in");
 
-	clk = clk_register_gate(NULL, "acp_clk", "acp_mux_clk", 0,
+	clk = clk_register_gate(NULL, "acp_clk", "acp_mclk", 0,
 			SPEAR1340_PERIP2_CLK_ENB, SPEAR1340_ACP_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "acp_clk");
 
-	clk = clk_register_gate(NULL, "plgpio_clk", "plgpio_mux_clk", 0,
+	clk = clk_register_gate(NULL, "plgpio_clk", "plgpio_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_PLGPIO_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "plgpio");
 
-	clk = clk_register_gate(NULL, "video_dec_clk", "video_dec_mux_clk", 0,
+	clk = clk_register_gate(NULL, "video_dec_clk", "video_dec_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_DEC_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, NULL, "video_dec");
 
-	clk = clk_register_gate(NULL, "video_enc_clk", "video_enc_mux_clk", 0,
+	clk = clk_register_gate(NULL, "video_enc_clk", "video_enc_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_ENC_CLK_ENB,
 			0, &_lock);
 	clk_register_clkdev(clk, NULL, "video_enc");
 
-	clk = clk_register_gate(NULL, "video_in_clk", "video_in_mux_clk", 0,
+	clk = clk_register_gate(NULL, "video_in_clk", "video_in_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_VIDEO_IN_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_vip");
 
-	clk = clk_register_gate(NULL, "cam0_clk", "cam0_mux_clk", 0,
+	clk = clk_register_gate(NULL, "cam0_clk", "cam0_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM0_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_camif.0");
 
-	clk = clk_register_gate(NULL, "cam1_clk", "cam1_mux_clk", 0,
+	clk = clk_register_gate(NULL, "cam1_clk", "cam1_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM1_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_camif.1");
 
-	clk = clk_register_gate(NULL, "cam2_clk", "cam2_mux_clk", 0,
+	clk = clk_register_gate(NULL, "cam2_clk", "cam2_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM2_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_camif.2");
 
-	clk = clk_register_gate(NULL, "cam3_clk", "cam3_mux_clk", 0,
+	clk = clk_register_gate(NULL, "cam3_clk", "cam3_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_CAM3_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "spear_camif.3");
 
-	clk = clk_register_gate(NULL, "pwm_clk", "pwm_mux_clk", 0,
+	clk = clk_register_gate(NULL, "pwm_clk", "pwm_mclk", 0,
 			SPEAR1340_PERIP3_CLK_ENB, SPEAR1340_PWM_CLK_ENB, 0,
 			&_lock);
 	clk_register_clkdev(clk, NULL, "pwm");
diff --git a/drivers/clk/spear/spear3xx_clock.c b/drivers/clk/spear/spear3xx_clock.c
index 01dd6daff2a..c3157454bb3 100644
--- a/drivers/clk/spear/spear3xx_clock.c
+++ b/drivers/clk/spear/spear3xx_clock.c
@@ -122,12 +122,12 @@ static struct gpt_rate_tbl gpt_rtbl[] = {
 };
 
 /* clock parents */
-static const char *uart0_parents[] = { "pll3_48m_clk", "uart_synth_gate_clk", };
-static const char *firda_parents[] = { "pll3_48m_clk", "firda_synth_gate_clk",
+static const char *uart0_parents[] = { "pll3_clk", "uart_syn_gclk", };
+static const char *firda_parents[] = { "pll3_clk", "firda_syn_gclk",
 };
-static const char *gpt0_parents[] = { "pll3_48m_clk", "gpt0_synth_clk", };
-static const char *gpt1_parents[] = { "pll3_48m_clk", "gpt1_synth_clk", };
-static const char *gpt2_parents[] = { "pll3_48m_clk", "gpt2_synth_clk", };
+static const char *gpt0_parents[] = { "pll3_clk", "gpt0_syn_clk", };
+static const char *gpt1_parents[] = { "pll3_clk", "gpt1_syn_clk", };
+static const char *gpt2_parents[] = { "pll3_clk", "gpt2_syn_clk", };
 static const char *gen2_3_parents[] = { "pll1_clk", "pll2_clk", };
 static const char *ddr_parents[] = { "ahb_clk", "ahbmult2_clk", "none",
 	"pll2_clk", };
@@ -137,7 +137,7 @@ static void __init spear300_clk_init(void)
 {
 	struct clk *clk;
 
-	clk = clk_register_fixed_factor(NULL, "clcd_clk", "ras_pll3_48m_clk", 0,
+	clk = clk_register_fixed_factor(NULL, "clcd_clk", "ras_pll3_clk", 0,
 			1, 1);
 	clk_register_clkdev(clk, NULL, "60000000.clcd");
 
@@ -219,15 +219,11 @@ static void __init spear310_clk_init(void)
 	#define SPEAR320_UARTX_PCLK_VAL_SYNTH1		0x0
 	#define SPEAR320_UARTX_PCLK_VAL_APB		0x1
 
-static const char *i2s_ref_parents[] = { "ras_pll2_clk",
-	"ras_gen2_synth_gate_clk", };
-static const char *sdhci_parents[] = { "ras_pll3_48m_clk",
-	"ras_gen3_synth_gate_clk",
-};
+static const char *i2s_ref_parents[] = { "ras_pll2_clk", "ras_syn2_gclk", };
+static const char *sdhci_parents[] = { "ras_pll3_clk", "ras_syn3_gclk", };
 static const char *smii0_parents[] = { "smii_125m_pad", "ras_pll2_clk",
-	"ras_gen0_synth_gate_clk", };
-static const char *uartx_parents[] = { "ras_gen1_synth_gate_clk", "ras_apb_clk",
-};
+	"ras_syn0_gclk", };
+static const char *uartx_parents[] = { "ras_syn1_gclk", "ras_apb_clk", };
 
 static void __init spear320_clk_init(void)
 {
@@ -237,7 +233,7 @@ static void __init spear320_clk_init(void)
 			CLK_IS_ROOT, 125000000);
 	clk_register_clkdev(clk, "smii_125m_pad", NULL);
 
-	clk = clk_register_fixed_factor(NULL, "clcd_clk", "ras_pll3_48m_clk", 0,
+	clk = clk_register_fixed_factor(NULL, "clcd_clk", "ras_pll3_clk", 0,
 			1, 1);
 	clk_register_clkdev(clk, NULL, "90000000.clcd");
 
@@ -363,9 +359,9 @@ void __init spear3xx_clk_init(void)
 	clk_register_clkdev(clk, NULL, "fc900000.rtc");
 
 	/* clock derived from 24 MHz osc clk */
-	clk = clk_register_fixed_rate(NULL, "pll3_48m_clk", "osc_24m_clk", 0,
+	clk = clk_register_fixed_rate(NULL, "pll3_clk", "osc_24m_clk", 0,
 			48000000);
-	clk_register_clkdev(clk, "pll3_48m_clk", NULL);
+	clk_register_clkdev(clk, "pll3_clk", NULL);
 
 	clk = clk_register_fixed_factor(NULL, "wdt_clk", "osc_24m_clk", 0, 1,
 			1);
@@ -392,98 +388,98 @@ void __init spear3xx_clk_init(void)
 			HCLK_RATIO_MASK, 0, &_lock);
 	clk_register_clkdev(clk, "ahb_clk", NULL);
 
-	clk = clk_register_aux("uart_synth_clk", "uart_synth_gate_clk",
-			"pll1_clk", 0, UART_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "uart_synth_clk", NULL);
-	clk_register_clkdev(clk1, "uart_synth_gate_clk", NULL);
+	clk = clk_register_aux("uart_syn_clk", "uart_syn_gclk", "pll1_clk", 0,
+			UART_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "uart_syn_clk", NULL);
+	clk_register_clkdev(clk1, "uart_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "uart0_mux_clk", uart0_parents,
+	clk = clk_register_mux(NULL, "uart0_mclk", uart0_parents,
 			ARRAY_SIZE(uart0_parents), 0, PERIP_CLK_CFG,
 			UART_CLK_SHIFT, UART_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "uart0_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart0_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart0", "uart0_mux_clk", 0,
-			PERIP1_CLK_ENB, UART_CLK_ENB, 0, &_lock);
+	clk = clk_register_gate(NULL, "uart0", "uart0_mclk", 0, PERIP1_CLK_ENB,
+			UART_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "d0000000.serial");
 
-	clk = clk_register_aux("firda_synth_clk", "firda_synth_gate_clk",
-			"pll1_clk", 0, FIRDA_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "firda_synth_clk", NULL);
-	clk_register_clkdev(clk1, "firda_synth_gate_clk", NULL);
+	clk = clk_register_aux("firda_syn_clk", "firda_syn_gclk", "pll1_clk", 0,
+			FIRDA_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "firda_syn_clk", NULL);
+	clk_register_clkdev(clk1, "firda_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "firda_mux_clk", firda_parents,
+	clk = clk_register_mux(NULL, "firda_mclk", firda_parents,
 			ARRAY_SIZE(firda_parents), 0, PERIP_CLK_CFG,
 			FIRDA_CLK_SHIFT, FIRDA_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "firda_mux_clk", NULL);
+	clk_register_clkdev(clk, "firda_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "firda_clk", "firda_mux_clk", 0,
+	clk = clk_register_gate(NULL, "firda_clk", "firda_mclk", 0,
 			PERIP1_CLK_ENB, FIRDA_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "firda");
 
 	/* gpt clocks */
-	clk_register_gpt("gpt0_synth_clk", "pll1_clk", 0, PRSC0_CLK_CFG,
-			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk_register_gpt("gpt0_syn_clk", "pll1_clk", 0, PRSC0_CLK_CFG, gpt_rtbl,
+			ARRAY_SIZE(gpt_rtbl), &_lock);
 	clk = clk_register_mux(NULL, "gpt0_clk", gpt0_parents,
 			ARRAY_SIZE(gpt0_parents), 0, PERIP_CLK_CFG,
 			GPT0_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt0");
 
-	clk_register_gpt("gpt1_synth_clk", "pll1_clk", 0, PRSC1_CLK_CFG,
-			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
-	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt1_parents,
+	clk_register_gpt("gpt1_syn_clk", "pll1_clk", 0, PRSC1_CLK_CFG, gpt_rtbl,
+			ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk = clk_register_mux(NULL, "gpt1_mclk", gpt1_parents,
 			ARRAY_SIZE(gpt1_parents), 0, PERIP_CLK_CFG,
 			GPT1_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt1_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mclk", 0,
 			PERIP1_CLK_ENB, GPT1_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt1");
 
-	clk_register_gpt("gpt2_synth_clk", "pll1_clk", 0, PRSC2_CLK_CFG,
-			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
-	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt2_parents,
+	clk_register_gpt("gpt2_syn_clk", "pll1_clk", 0, PRSC2_CLK_CFG, gpt_rtbl,
+			ARRAY_SIZE(gpt_rtbl), &_lock);
+	clk = clk_register_mux(NULL, "gpt2_mclk", gpt2_parents,
 			ARRAY_SIZE(gpt2_parents), 0, PERIP_CLK_CFG,
 			GPT2_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
-	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+	clk_register_clkdev(clk, "gpt2_mclk", NULL);
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mclk", 0,
 			PERIP1_CLK_ENB, GPT2_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt2");
 
 	/* general synths clocks */
-	clk = clk_register_aux("gen0_synth_clk", "gen0_synth_gate_clk",
-			"pll1_clk", 0, GEN0_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "gen0_synth_clk", NULL);
-	clk_register_clkdev(clk1, "gen0_synth_gate_clk", NULL);
-
-	clk = clk_register_aux("gen1_synth_clk", "gen1_synth_gate_clk",
-			"pll1_clk", 0, GEN1_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "gen1_synth_clk", NULL);
-	clk_register_clkdev(clk1, "gen1_synth_gate_clk", NULL);
-
-	clk = clk_register_mux(NULL, "gen2_3_parent_clk", gen2_3_parents,
+	clk = clk_register_aux("gen0_syn_clk", "gen0_syn_gclk", "pll1_clk",
+			0, GEN0_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "gen0_syn_clk", NULL);
+	clk_register_clkdev(clk1, "gen0_syn_gclk", NULL);
+
+	clk = clk_register_aux("gen1_syn_clk", "gen1_syn_gclk", "pll1_clk",
+			0, GEN1_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "gen1_syn_clk", NULL);
+	clk_register_clkdev(clk1, "gen1_syn_gclk", NULL);
+
+	clk = clk_register_mux(NULL, "gen2_3_par_clk", gen2_3_parents,
 			ARRAY_SIZE(gen2_3_parents), 0, CORE_CLK_CFG,
 			GEN_SYNTH2_3_CLK_SHIFT, GEN_SYNTH2_3_CLK_MASK, 0,
 			&_lock);
-	clk_register_clkdev(clk, "gen2_3_parent_clk", NULL);
+	clk_register_clkdev(clk, "gen2_3_par_clk", NULL);
 
-	clk = clk_register_aux("gen2_synth_clk", "gen2_synth_gate_clk",
-			"gen2_3_parent_clk", 0, GEN2_CLK_SYNT, NULL, aux_rtbl,
+	clk = clk_register_aux("gen2_syn_clk", "gen2_syn_gclk",
+			"gen2_3_par_clk", 0, GEN2_CLK_SYNT, NULL, aux_rtbl,
 			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "gen2_synth_clk", NULL);
-	clk_register_clkdev(clk1, "gen2_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "gen2_syn_clk", NULL);
+	clk_register_clkdev(clk1, "gen2_syn_gclk", NULL);
 
-	clk = clk_register_aux("gen3_synth_clk", "gen3_synth_gate_clk",
-			"gen2_3_parent_clk", 0, GEN3_CLK_SYNT, NULL, aux_rtbl,
+	clk = clk_register_aux("gen3_syn_clk", "gen3_syn_gclk",
+			"gen2_3_par_clk", 0, GEN3_CLK_SYNT, NULL, aux_rtbl,
 			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "gen3_synth_clk", NULL);
-	clk_register_clkdev(clk1, "gen3_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "gen3_syn_clk", NULL);
+	clk_register_clkdev(clk1, "gen3_syn_gclk", NULL);
 
 	/* clock derived from pll3 clk */
-	clk = clk_register_gate(NULL, "usbh_clk", "pll3_48m_clk", 0,
-			PERIP1_CLK_ENB, USBH_CLK_ENB, 0, &_lock);
+	clk = clk_register_gate(NULL, "usbh_clk", "pll3_clk", 0, PERIP1_CLK_ENB,
+			USBH_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, "usbh_clk", NULL);
 
 	clk = clk_register_fixed_factor(NULL, "usbh.0_clk", "usbh_clk", 0, 1,
@@ -494,8 +490,8 @@ void __init spear3xx_clk_init(void)
 			1);
 	clk_register_clkdev(clk, "usbh.1_clk", NULL);
 
-	clk = clk_register_gate(NULL, "usbd_clk", "pll3_48m_clk", 0,
-			PERIP1_CLK_ENB, USBD_CLK_ENB, 0, &_lock);
+	clk = clk_register_gate(NULL, "usbd_clk", "pll3_clk", 0, PERIP1_CLK_ENB,
+			USBD_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "designware_udc");
 
 	/* clock derived from ahb clk */
@@ -579,29 +575,25 @@ void __init spear3xx_clk_init(void)
 			RAS_CLK_ENB, RAS_PLL2_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, "ras_pll2_clk", NULL);
 
-	clk = clk_register_gate(NULL, "ras_pll3_48m_clk", "pll3_48m_clk", 0,
+	clk = clk_register_gate(NULL, "ras_pll3_clk", "pll3_clk", 0,
 			RAS_CLK_ENB, RAS_48M_CLK_ENB, 0, &_lock);
-	clk_register_clkdev(clk, "ras_pll3_48m_clk", NULL);
-
-	clk = clk_register_gate(NULL, "ras_gen0_synth_gate_clk",
-			"gen0_synth_gate_clk", 0, RAS_CLK_ENB,
-			RAS_SYNT0_CLK_ENB, 0, &_lock);
-	clk_register_clkdev(clk, "ras_gen0_synth_gate_clk", NULL);
-
-	clk = clk_register_gate(NULL, "ras_gen1_synth_gate_clk",
-			"gen1_synth_gate_clk", 0, RAS_CLK_ENB,
-			RAS_SYNT1_CLK_ENB, 0, &_lock);
-	clk_register_clkdev(clk, "ras_gen1_synth_gate_clk", NULL);
-
-	clk = clk_register_gate(NULL, "ras_gen2_synth_gate_clk",
-			"gen2_synth_gate_clk", 0, RAS_CLK_ENB,
-			RAS_SYNT2_CLK_ENB, 0, &_lock);
-	clk_register_clkdev(clk, "ras_gen2_synth_gate_clk", NULL);
-
-	clk = clk_register_gate(NULL, "ras_gen3_synth_gate_clk",
-			"gen3_synth_gate_clk", 0, RAS_CLK_ENB,
-			RAS_SYNT3_CLK_ENB, 0, &_lock);
-	clk_register_clkdev(clk, "ras_gen3_synth_gate_clk", NULL);
+	clk_register_clkdev(clk, "ras_pll3_clk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_syn0_gclk", "gen0_syn_gclk", 0,
+			RAS_CLK_ENB, RAS_SYNT0_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_syn0_gclk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_syn1_gclk", "gen1_syn_gclk", 0,
+			RAS_CLK_ENB, RAS_SYNT1_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_syn1_gclk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_syn2_gclk", "gen2_syn_gclk", 0,
+			RAS_CLK_ENB, RAS_SYNT2_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_syn2_gclk", NULL);
+
+	clk = clk_register_gate(NULL, "ras_syn3_gclk", "gen3_syn_gclk", 0,
+			RAS_CLK_ENB, RAS_SYNT3_CLK_ENB, 0, &_lock);
+	clk_register_clkdev(clk, "ras_syn3_gclk", NULL);
 
 	if (of_machine_is_compatible("st,spear300"))
 		spear300_clk_init();
diff --git a/drivers/clk/spear/spear6xx_clock.c b/drivers/clk/spear/spear6xx_clock.c
index 61026ae564a..a98d0866f54 100644
--- a/drivers/clk/spear/spear6xx_clock.c
+++ b/drivers/clk/spear/spear6xx_clock.c
@@ -97,13 +97,12 @@ static struct aux_rate_tbl aux_rtbl[] = {
 	{.xscale = 1, .yscale = 2, .eq = 1}, /* 166 MHz */
 };
 
-static const char *clcd_parents[] = { "pll3_48m_clk", "clcd_synth_gate_clk", };
-static const char *firda_parents[] = { "pll3_48m_clk", "firda_synth_gate_clk",
-};
-static const char *uart_parents[] = { "pll3_48m_clk", "uart_synth_gate_clk", };
-static const char *gpt0_1_parents[] = { "pll3_48m_clk", "gpt0_1_synth_clk", };
-static const char *gpt2_parents[] = { "pll3_48m_clk", "gpt2_synth_clk", };
-static const char *gpt3_parents[] = { "pll3_48m_clk", "gpt3_synth_clk", };
+static const char *clcd_parents[] = { "pll3_clk", "clcd_syn_gclk", };
+static const char *firda_parents[] = { "pll3_clk", "firda_syn_gclk", };
+static const char *uart_parents[] = { "pll3_clk", "uart_syn_gclk", };
+static const char *gpt0_1_parents[] = { "pll3_clk", "gpt0_1_syn_clk", };
+static const char *gpt2_parents[] = { "pll3_clk", "gpt2_syn_clk", };
+static const char *gpt3_parents[] = { "pll3_clk", "gpt3_syn_clk", };
 static const char *ddr_parents[] = { "ahb_clk", "ahbmult2_clk", "none",
 	"pll2_clk", };
 
@@ -136,9 +135,9 @@ void __init spear6xx_clk_init(void)
 	clk_register_clkdev(clk, NULL, "rtc-spear");
 
 	/* clock derived from 30 MHz osc clk */
-	clk = clk_register_fixed_rate(NULL, "pll3_48m_clk", "osc_24m_clk", 0,
+	clk = clk_register_fixed_rate(NULL, "pll3_clk", "osc_24m_clk", 0,
 			48000000);
-	clk_register_clkdev(clk, "pll3_48m_clk", NULL);
+	clk_register_clkdev(clk, "pll3_clk", NULL);
 
 	clk = clk_register_vco_pll("vco1_clk", "pll1_clk", NULL, "osc_30m_clk",
 			0, PLL1_CTR, PLL1_FRQ, pll_rtbl, ARRAY_SIZE(pll_rtbl),
@@ -146,9 +145,9 @@ void __init spear6xx_clk_init(void)
 	clk_register_clkdev(clk, "vco1_clk", NULL);
 	clk_register_clkdev(clk1, "pll1_clk", NULL);
 
-	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL,
-			"osc_30m_clk", 0, PLL2_CTR, PLL2_FRQ, pll_rtbl,
-			ARRAY_SIZE(pll_rtbl), &_lock, &clk1, NULL);
+	clk = clk_register_vco_pll("vco2_clk", "pll2_clk", NULL, "osc_30m_clk",
+			0, PLL2_CTR, PLL2_FRQ, pll_rtbl, ARRAY_SIZE(pll_rtbl),
+			&_lock, &clk1, NULL);
 	clk_register_clkdev(clk, "vco2_clk", NULL);
 	clk_register_clkdev(clk1, "pll2_clk", NULL);
 
@@ -165,111 +164,111 @@ void __init spear6xx_clk_init(void)
 			HCLK_RATIO_MASK, 0, &_lock);
 	clk_register_clkdev(clk, "ahb_clk", NULL);
 
-	clk = clk_register_aux("uart_synth_clk", "uart_synth_gate_clk",
-			"pll1_clk", 0, UART_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "uart_synth_clk", NULL);
-	clk_register_clkdev(clk1, "uart_synth_gate_clk", NULL);
+	clk = clk_register_aux("uart_syn_clk", "uart_syn_gclk", "pll1_clk", 0,
+			UART_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "uart_syn_clk", NULL);
+	clk_register_clkdev(clk1, "uart_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "uart_mux_clk", uart_parents,
+	clk = clk_register_mux(NULL, "uart_mclk", uart_parents,
 			ARRAY_SIZE(uart_parents), 0, PERIP_CLK_CFG,
 			UART_CLK_SHIFT, UART_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "uart_mux_clk", NULL);
+	clk_register_clkdev(clk, "uart_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "uart0", "uart_mux_clk", 0,
-			PERIP1_CLK_ENB, UART0_CLK_ENB, 0, &_lock);
+	clk = clk_register_gate(NULL, "uart0", "uart_mclk", 0, PERIP1_CLK_ENB,
+			UART0_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "d0000000.serial");
 
-	clk = clk_register_gate(NULL, "uart1", "uart_mux_clk", 0,
-			PERIP1_CLK_ENB, UART1_CLK_ENB, 0, &_lock);
+	clk = clk_register_gate(NULL, "uart1", "uart_mclk", 0, PERIP1_CLK_ENB,
+			UART1_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "d0080000.serial");
 
-	clk = clk_register_aux("firda_synth_clk", "firda_synth_gate_clk",
-			"pll1_clk", 0, FIRDA_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "firda_synth_clk", NULL);
-	clk_register_clkdev(clk1, "firda_synth_gate_clk", NULL);
+	clk = clk_register_aux("firda_syn_clk", "firda_syn_gclk", "pll1_clk",
+			0, FIRDA_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "firda_syn_clk", NULL);
+	clk_register_clkdev(clk1, "firda_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "firda_mux_clk", firda_parents,
+	clk = clk_register_mux(NULL, "firda_mclk", firda_parents,
 			ARRAY_SIZE(firda_parents), 0, PERIP_CLK_CFG,
 			FIRDA_CLK_SHIFT, FIRDA_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "firda_mux_clk", NULL);
+	clk_register_clkdev(clk, "firda_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "firda_clk", "firda_mux_clk", 0,
+	clk = clk_register_gate(NULL, "firda_clk", "firda_mclk", 0,
 			PERIP1_CLK_ENB, FIRDA_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "firda");
 
-	clk = clk_register_aux("clcd_synth_clk", "clcd_synth_gate_clk",
-			"pll1_clk", 0, CLCD_CLK_SYNT, NULL, aux_rtbl,
-			ARRAY_SIZE(aux_rtbl), &_lock, &clk1);
-	clk_register_clkdev(clk, "clcd_synth_clk", NULL);
-	clk_register_clkdev(clk1, "clcd_synth_gate_clk", NULL);
+	clk = clk_register_aux("clcd_syn_clk", "clcd_syn_gclk", "pll1_clk",
+			0, CLCD_CLK_SYNT, NULL, aux_rtbl, ARRAY_SIZE(aux_rtbl),
+			&_lock, &clk1);
+	clk_register_clkdev(clk, "clcd_syn_clk", NULL);
+	clk_register_clkdev(clk1, "clcd_syn_gclk", NULL);
 
-	clk = clk_register_mux(NULL, "clcd_mux_clk", clcd_parents,
+	clk = clk_register_mux(NULL, "clcd_mclk", clcd_parents,
 			ARRAY_SIZE(clcd_parents), 0, PERIP_CLK_CFG,
 			CLCD_CLK_SHIFT, CLCD_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "clcd_mux_clk", NULL);
+	clk_register_clkdev(clk, "clcd_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "clcd_clk", "clcd_mux_clk", 0,
+	clk = clk_register_gate(NULL, "clcd_clk", "clcd_mclk", 0,
 			PERIP1_CLK_ENB, CLCD_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "clcd");
 
 	/* gpt clocks */
-	clk = clk_register_gpt("gpt0_1_synth_clk", "pll1_clk", 0, PRSC0_CLK_CFG,
+	clk = clk_register_gpt("gpt0_1_syn_clk", "pll1_clk", 0, PRSC0_CLK_CFG,
 			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
-	clk_register_clkdev(clk, "gpt0_1_synth_clk", NULL);
+	clk_register_clkdev(clk, "gpt0_1_syn_clk", NULL);
 
-	clk = clk_register_mux(NULL, "gpt0_mux_clk", gpt0_1_parents,
+	clk = clk_register_mux(NULL, "gpt0_mclk", gpt0_1_parents,
 			ARRAY_SIZE(gpt0_1_parents), 0, PERIP_CLK_CFG,
 			GPT0_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt0");
 
-	clk = clk_register_mux(NULL, "gpt1_mux_clk", gpt0_1_parents,
+	clk = clk_register_mux(NULL, "gpt1_mclk", gpt0_1_parents,
 			ARRAY_SIZE(gpt0_1_parents), 0, PERIP_CLK_CFG,
 			GPT1_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gpt1_mux_clk", NULL);
+	clk_register_clkdev(clk, "gpt1_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mux_clk", 0,
+	clk = clk_register_gate(NULL, "gpt1_clk", "gpt1_mclk", 0,
 			PERIP1_CLK_ENB, GPT1_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt1");
 
-	clk = clk_register_gpt("gpt2_synth_clk", "pll1_clk", 0, PRSC1_CLK_CFG,
+	clk = clk_register_gpt("gpt2_syn_clk", "pll1_clk", 0, PRSC1_CLK_CFG,
 			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
-	clk_register_clkdev(clk, "gpt2_synth_clk", NULL);
+	clk_register_clkdev(clk, "gpt2_syn_clk", NULL);
 
-	clk = clk_register_mux(NULL, "gpt2_mux_clk", gpt2_parents,
+	clk = clk_register_mux(NULL, "gpt2_mclk", gpt2_parents,
 			ARRAY_SIZE(gpt2_parents), 0, PERIP_CLK_CFG,
 			GPT2_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gpt2_mux_clk", NULL);
+	clk_register_clkdev(clk, "gpt2_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mux_clk", 0,
+	clk = clk_register_gate(NULL, "gpt2_clk", "gpt2_mclk", 0,
 			PERIP1_CLK_ENB, GPT2_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt2");
 
-	clk = clk_register_gpt("gpt3_synth_clk", "pll1_clk", 0, PRSC2_CLK_CFG,
+	clk = clk_register_gpt("gpt3_syn_clk", "pll1_clk", 0, PRSC2_CLK_CFG,
 			gpt_rtbl, ARRAY_SIZE(gpt_rtbl), &_lock);
-	clk_register_clkdev(clk, "gpt3_synth_clk", NULL);
+	clk_register_clkdev(clk, "gpt3_syn_clk", NULL);
 
-	clk = clk_register_mux(NULL, "gpt3_mux_clk", gpt3_parents,
+	clk = clk_register_mux(NULL, "gpt3_mclk", gpt3_parents,
 			ARRAY_SIZE(gpt3_parents), 0, PERIP_CLK_CFG,
 			GPT3_CLK_SHIFT, GPT_CLK_MASK, 0, &_lock);
-	clk_register_clkdev(clk, "gpt3_mux_clk", NULL);
+	clk_register_clkdev(clk, "gpt3_mclk", NULL);
 
-	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mux_clk", 0,
+	clk = clk_register_gate(NULL, "gpt3_clk", "gpt3_mclk", 0,
 			PERIP1_CLK_ENB, GPT3_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "gpt3");
 
 	/* clock derived from pll3 clk */
-	clk = clk_register_gate(NULL, "usbh0_clk", "pll3_48m_clk", 0,
+	clk = clk_register_gate(NULL, "usbh0_clk", "pll3_clk", 0,
 			PERIP1_CLK_ENB, USBH0_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "usbh.0_clk");
 
-	clk = clk_register_gate(NULL, "usbh1_clk", "pll3_48m_clk", 0,
+	clk = clk_register_gate(NULL, "usbh1_clk", "pll3_clk", 0,
 			PERIP1_CLK_ENB, USBH1_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "usbh.1_clk");
 
-	clk = clk_register_gate(NULL, "usbd_clk", "pll3_48m_clk", 0,
-			PERIP1_CLK_ENB, USBD_CLK_ENB, 0, &_lock);
+	clk = clk_register_gate(NULL, "usbd_clk", "pll3_clk", 0, PERIP1_CLK_ENB,
+			USBD_CLK_ENB, 0, &_lock);
 	clk_register_clkdev(clk, NULL, "designware_udc");
 
 	/* clock derived from ahb clk */
@@ -278,9 +277,8 @@ void __init spear6xx_clk_init(void)
 	clk_register_clkdev(clk, "ahbmult2_clk", NULL);
 
 	clk = clk_register_mux(NULL, "ddr_clk", ddr_parents,
-			ARRAY_SIZE(ddr_parents),
-			0, PLL_CLK_CFG, MCTR_CLK_SHIFT, MCTR_CLK_MASK, 0,
-			&_lock);
+			ARRAY_SIZE(ddr_parents), 0, PLL_CLK_CFG, MCTR_CLK_SHIFT,
+			MCTR_CLK_MASK, 0, &_lock);
 	clk_register_clkdev(clk, "ddr_clk", NULL);
 
 	clk = clk_register_divider(NULL, "apb_clk", "ahb_clk",
diff --git a/drivers/clk/versatile/Makefile b/drivers/clk/versatile/Makefile
new file mode 100644
index 00000000000..50cf6a2ee69
--- /dev/null
+++ b/drivers/clk/versatile/Makefile
@@ -0,0 +1,3 @@
+# Makefile for Versatile-specific clocks
+obj-$(CONFIG_ICST)		+= clk-icst.o
+obj-$(CONFIG_ARCH_INTEGRATOR)	+= clk-integrator.o
diff --git a/drivers/clk/versatile/clk-icst.c b/drivers/clk/versatile/clk-icst.c
new file mode 100644
index 00000000000..f555b50a5fa
--- /dev/null
+++ b/drivers/clk/versatile/clk-icst.c
@@ -0,0 +1,100 @@
+/*
+ * Driver for the ICST307 VCO clock found in the ARM Reference designs.
+ * We wrap the custom interface from <asm/hardware/icst.h> into the generic
+ * clock framework.
+ *
+ * TODO: when all ARM reference designs are migrated to generic clocks, the
+ * ICST clock code from the ARM tree should probably be merged into this
+ * file.
+ */
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+
+#include "clk-icst.h"
+
+/**
+ * struct clk_icst - ICST VCO clock wrapper
+ * @hw: corresponding clock hardware entry
+ * @params: parameters for this ICST instance
+ * @rate: current rate
+ * @setvco: function to commit ICST settings to hardware
+ */
+struct clk_icst {
+	struct clk_hw hw;
+	const struct icst_params *params;
+	unsigned long rate;
+	struct icst_vco (*getvco)(void);
+	void (*setvco)(struct icst_vco);
+};
+
+#define to_icst(_hw) container_of(_hw, struct clk_icst, hw)
+
+static unsigned long icst_recalc_rate(struct clk_hw *hw,
+				      unsigned long parent_rate)
+{
+	struct clk_icst *icst = to_icst(hw);
+	struct icst_vco vco;
+
+	vco = icst->getvco();
+	icst->rate = icst_hz(icst->params, vco);
+	return icst->rate;
+}
+
+static long icst_round_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long *prate)
+{
+	struct clk_icst *icst = to_icst(hw);
+	struct icst_vco vco;
+
+	vco = icst_hz_to_vco(icst->params, rate);
+	return icst_hz(icst->params, vco);
+}
+
+static int icst_set_rate(struct clk_hw *hw, unsigned long rate,
+			 unsigned long parent_rate)
+{
+	struct clk_icst *icst = to_icst(hw);
+	struct icst_vco vco;
+
+	vco = icst_hz_to_vco(icst->params, rate);
+	icst->rate = icst_hz(icst->params, vco);
+	icst->setvco(vco);
+	return 0;
+}
+
+static const struct clk_ops icst_ops = {
+	.recalc_rate = icst_recalc_rate,
+	.round_rate = icst_round_rate,
+	.set_rate = icst_set_rate,
+};
+
+struct clk * __init icst_clk_register(struct device *dev,
+				      const struct clk_icst_desc *desc)
+{
+	struct clk *clk;
+	struct clk_icst *icst;
+	struct clk_init_data init;
+
+	icst = kzalloc(sizeof(struct clk_icst), GFP_KERNEL);
+	if (!icst) {
+		pr_err("could not allocate ICST clock!\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	init.name = "icst";
+	init.ops = &icst_ops;
+	init.flags = CLK_IS_ROOT;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	icst->hw.init = &init;
+	icst->params = desc->params;
+	icst->getvco = desc->getvco;
+	icst->setvco = desc->setvco;
+
+	clk = clk_register(dev, &icst->hw);
+	if (IS_ERR(clk))
+		kfree(icst);
+
+	return clk;
+}
diff --git a/drivers/clk/versatile/clk-icst.h b/drivers/clk/versatile/clk-icst.h
new file mode 100644
index 00000000000..71b4c56c141
--- /dev/null
+++ b/drivers/clk/versatile/clk-icst.h
@@ -0,0 +1,10 @@
+#include <asm/hardware/icst.h>
+
+struct clk_icst_desc {
+	const struct icst_params *params;
+	struct icst_vco (*getvco)(void);
+	void (*setvco)(struct icst_vco);
+};
+
+struct clk *icst_clk_register(struct device *dev,
+			      const struct clk_icst_desc *desc);
diff --git a/drivers/clk/versatile/clk-integrator.c b/drivers/clk/versatile/clk-integrator.c
new file mode 100644
index 00000000000..a5053921bf7
--- /dev/null
+++ b/drivers/clk/versatile/clk-integrator.c
@@ -0,0 +1,111 @@
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+
+#include <mach/hardware.h>
+#include <mach/platform.h>
+
+#include "clk-icst.h"
+
+/*
+ * Implementation of the ARM Integrator/AP and Integrator/CP clock tree.
+ * Inspired by portions of:
+ * plat-versatile/clock.c and plat-versatile/include/plat/clock.h
+ */
+#define CM_LOCK		(__io_address(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_LOCK_OFFSET)
+#define CM_AUXOSC	(__io_address(INTEGRATOR_HDR_BASE)+0x1c)
+
+/**
+ * cp_auxvco_get() - get ICST VCO settings for the Integrator/CP
+ * @vco: ICST VCO parameters to update with hardware status
+ */
+static struct icst_vco cp_auxvco_get(void)
+{
+	u32 val;
+	struct icst_vco vco;
+
+	val = readl(CM_AUXOSC);
+	vco.v = val & 0x1ff;
+	vco.r = (val >> 9) & 0x7f;
+	vco.s = (val >> 16) & 03;
+	return vco;
+}
+
+/**
+ * cp_auxvco_set() - commit changes to Integrator/CP ICST VCO
+ * @vco: ICST VCO parameters to commit
+ */
+static void cp_auxvco_set(struct icst_vco vco)
+{
+	u32 val;
+
+	val = readl(CM_AUXOSC) & ~0x7ffff;
+	val |= vco.v | (vco.r << 9) | (vco.s << 16);
+
+	/* This magic unlocks the CM VCO so it can be controlled */
+	writel(0xa05f, CM_LOCK);
+	writel(val, CM_AUXOSC);
+	/* This locks the CM again */
+	writel(0, CM_LOCK);
+}
+
+static const struct icst_params cp_auxvco_params = {
+	.ref		= 24000000,
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	.vd_min 	= 8,
+	.vd_max 	= 263,
+	.rd_min 	= 3,
+	.rd_max 	= 65,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
+static const struct clk_icst_desc __initdata cp_icst_desc = {
+	.params = &cp_auxvco_params,
+	.getvco = cp_auxvco_get,
+	.setvco = cp_auxvco_set,
+};
+
+/*
+ * integrator_clk_init() - set up the integrator clock tree
+ * @is_cp: pass true if it's the Integrator/CP else AP is assumed
+ */
+void __init integrator_clk_init(bool is_cp)
+{
+	struct clk *clk;
+
+	/* APB clock dummy */
+	clk = clk_register_fixed_rate(NULL, "apb_pclk", NULL, CLK_IS_ROOT, 0);
+	clk_register_clkdev(clk, "apb_pclk", NULL);
+
+	/* UART reference clock */
+	clk = clk_register_fixed_rate(NULL, "uartclk", NULL, CLK_IS_ROOT,
+				14745600);
+	clk_register_clkdev(clk, NULL, "uart0");
+	clk_register_clkdev(clk, NULL, "uart1");
+	if (is_cp)
+		clk_register_clkdev(clk, NULL, "mmci");
+
+	/* 24 MHz clock */
+	clk = clk_register_fixed_rate(NULL, "clk24mhz", NULL, CLK_IS_ROOT,
+				24000000);
+	clk_register_clkdev(clk, NULL, "kmi0");
+	clk_register_clkdev(clk, NULL, "kmi1");
+	if (!is_cp)
+		clk_register_clkdev(clk, NULL, "ap_timer");
+
+	if (!is_cp)
+		return;
+
+	/* 1 MHz clock */
+	clk = clk_register_fixed_rate(NULL, "clk1mhz", NULL, CLK_IS_ROOT,
+				1000000);
+	clk_register_clkdev(clk, NULL, "sp804");
+
+	/* ICST VCO clock used on the Integrator/CP CLCD */
+	clk = icst_clk_register(NULL, &cp_icst_desc);
+	clk_register_clkdev(clk, NULL, "clcd");
+}
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 99c6b203e6c..d53cd0afc20 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -16,6 +16,12 @@ config CLKSRC_MMIO
 config DW_APB_TIMER
 	bool
 
+config DW_APB_TIMER_OF
+	bool
+
+config ARMADA_370_XP_TIMER
+	bool
+
 config CLKSRC_DBX500_PRCMU
 	bool "Clocksource PRCMU Timer"
 	depends on UX500_SOC_DB8500
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index dd3e661a124..b65d0c56ab3 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -10,4 +10,6 @@ obj-$(CONFIG_EM_TIMER_STI)	+= em_sti.o
 obj-$(CONFIG_CLKBLD_I8253)	+= i8253.o
 obj-$(CONFIG_CLKSRC_MMIO)	+= mmio.o
 obj-$(CONFIG_DW_APB_TIMER)	+= dw_apb_timer.o
-obj-$(CONFIG_CLKSRC_DBX500_PRCMU)	+= clksrc-dbx500-prcmu.o
\ No newline at end of file
+obj-$(CONFIG_DW_APB_TIMER_OF)	+= dw_apb_timer_of.o
+obj-$(CONFIG_CLKSRC_DBX500_PRCMU)	+= clksrc-dbx500-prcmu.o
+obj-$(CONFIG_ARMADA_370_XP_TIMER)	+= time-armada-370-xp.o
diff --git a/drivers/clocksource/dw_apb_timer_of.c b/drivers/clocksource/dw_apb_timer_of.c
new file mode 100644
index 00000000000..f7dba5b79b4
--- /dev/null
+++ b/drivers/clocksource/dw_apb_timer_of.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2012 Altera Corporation
+ * Copyright (c) 2011 Picochip Ltd., Jamie Iles
+ *
+ * Modified from mach-picoxcell/time.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU 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/dw_apb_timer.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#include <asm/mach/time.h>
+#include <asm/sched_clock.h>
+
+static void timer_get_base_and_rate(struct device_node *np,
+				    void __iomem **base, u32 *rate)
+{
+	*base = of_iomap(np, 0);
+
+	if (!*base)
+		panic("Unable to map regs for %s", np->name);
+
+	if (of_property_read_u32(np, "clock-freq", rate) &&
+		of_property_read_u32(np, "clock-frequency", rate))
+		panic("No clock-frequency property for %s", np->name);
+}
+
+static void add_clockevent(struct device_node *event_timer)
+{
+	void __iomem *iobase;
+	struct dw_apb_clock_event_device *ced;
+	u32 irq, rate;
+
+	irq = irq_of_parse_and_map(event_timer, 0);
+	if (irq == NO_IRQ)
+		panic("No IRQ for clock event timer");
+
+	timer_get_base_and_rate(event_timer, &iobase, &rate);
+
+	ced = dw_apb_clockevent_init(0, event_timer->name, 300, iobase, irq,
+				     rate);
+	if (!ced)
+		panic("Unable to initialise clockevent device");
+
+	dw_apb_clockevent_register(ced);
+}
+
+static void add_clocksource(struct device_node *source_timer)
+{
+	void __iomem *iobase;
+	struct dw_apb_clocksource *cs;
+	u32 rate;
+
+	timer_get_base_and_rate(source_timer, &iobase, &rate);
+
+	cs = dw_apb_clocksource_init(300, source_timer->name, iobase, rate);
+	if (!cs)
+		panic("Unable to initialise clocksource device");
+
+	dw_apb_clocksource_start(cs);
+	dw_apb_clocksource_register(cs);
+}
+
+static void __iomem *sched_io_base;
+
+static u32 read_sched_clock(void)
+{
+	return __raw_readl(sched_io_base);
+}
+
+static const struct of_device_id sptimer_ids[] __initconst = {
+	{ .compatible = "picochip,pc3x2-rtc" },
+	{ .compatible = "snps,dw-apb-timer-sp" },
+	{ /* Sentinel */ },
+};
+
+static void init_sched_clock(void)
+{
+	struct device_node *sched_timer;
+	u32 rate;
+
+	sched_timer = of_find_matching_node(NULL, sptimer_ids);
+	if (!sched_timer)
+		panic("No RTC for sched clock to use");
+
+	timer_get_base_and_rate(sched_timer, &sched_io_base, &rate);
+	of_node_put(sched_timer);
+
+	setup_sched_clock(read_sched_clock, 32, rate);
+}
+
+static const struct of_device_id osctimer_ids[] __initconst = {
+	{ .compatible = "picochip,pc3x2-timer" },
+	{ .compatible = "snps,dw-apb-timer-osc" },
+	{},
+};
+
+static void __init timer_init(void)
+{
+	struct device_node *event_timer, *source_timer;
+
+	event_timer = of_find_matching_node(NULL, osctimer_ids);
+	if (!event_timer)
+		panic("No timer for clockevent");
+	add_clockevent(event_timer);
+
+	source_timer = of_find_matching_node(event_timer, osctimer_ids);
+	if (!source_timer)
+		panic("No timer for clocksource");
+	add_clocksource(source_timer);
+
+	of_node_put(source_timer);
+
+	init_sched_clock();
+}
+
+struct sys_timer dw_apb_timer = {
+	.init = timer_init,
+};
diff --git a/drivers/clocksource/time-armada-370-xp.c b/drivers/clocksource/time-armada-370-xp.c
new file mode 100644
index 00000000000..4674f94957c
--- /dev/null
+++ b/drivers/clocksource/time-armada-370-xp.c
@@ -0,0 +1,226 @@
+/*
+ * Marvell Armada 370/XP SoC timer handling.
+ *
+ * Copyright (C) 2012 Marvell
+ *
+ * Lior Amsalem <alior@marvell.com>
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ * Timer 0 is used as free-running clocksource, while timer 1 is
+ * used as clock_event_device.
+ */
+
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/timer.h>
+#include <linux/clockchips.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <asm/sched_clock.h>
+
+/*
+ * Timer block registers.
+ */
+#define TIMER_CTRL_OFF		0x0000
+#define  TIMER0_EN		 0x0001
+#define  TIMER0_RELOAD_EN	 0x0002
+#define  TIMER0_25MHZ            0x0800
+#define  TIMER0_DIV(div)         ((div) << 19)
+#define  TIMER1_EN		 0x0004
+#define  TIMER1_RELOAD_EN	 0x0008
+#define  TIMER1_25MHZ            0x1000
+#define  TIMER1_DIV(div)         ((div) << 22)
+#define TIMER_EVENTS_STATUS	0x0004
+#define  TIMER0_CLR_MASK         (~0x1)
+#define  TIMER1_CLR_MASK         (~0x100)
+#define TIMER0_RELOAD_OFF	0x0010
+#define TIMER0_VAL_OFF		0x0014
+#define TIMER1_RELOAD_OFF	0x0018
+#define TIMER1_VAL_OFF		0x001c
+
+/* Global timers are connected to the coherency fabric clock, and the
+   below divider reduces their incrementing frequency. */
+#define TIMER_DIVIDER_SHIFT     5
+#define TIMER_DIVIDER           (1 << TIMER_DIVIDER_SHIFT)
+
+/*
+ * SoC-specific data.
+ */
+static void __iomem *timer_base;
+static int timer_irq;
+
+/*
+ * Number of timer ticks per jiffy.
+ */
+static u32 ticks_per_jiffy;
+
+static u32 notrace armada_370_xp_read_sched_clock(void)
+{
+	return ~readl(timer_base + TIMER0_VAL_OFF);
+}
+
+/*
+ * Clockevent handling.
+ */
+static int
+armada_370_xp_clkevt_next_event(unsigned long delta,
+				struct clock_event_device *dev)
+{
+	u32 u;
+
+	/*
+	 * Clear clockevent timer interrupt.
+	 */
+	writel(TIMER1_CLR_MASK, timer_base + TIMER_EVENTS_STATUS);
+
+	/*
+	 * Setup new clockevent timer value.
+	 */
+	writel(delta, timer_base + TIMER1_VAL_OFF);
+
+	/*
+	 * Enable the timer.
+	 */
+	u = readl(timer_base + TIMER_CTRL_OFF);
+	u = ((u & ~TIMER1_RELOAD_EN) | TIMER1_EN |
+	     TIMER1_DIV(TIMER_DIVIDER_SHIFT));
+	writel(u, timer_base + TIMER_CTRL_OFF);
+
+	return 0;
+}
+
+static void
+armada_370_xp_clkevt_mode(enum clock_event_mode mode,
+			  struct clock_event_device *dev)
+{
+	u32 u;
+
+	if (mode == CLOCK_EVT_MODE_PERIODIC) {
+		/*
+		 * Setup timer to fire at 1/HZ intervals.
+		 */
+		writel(ticks_per_jiffy - 1, timer_base + TIMER1_RELOAD_OFF);
+		writel(ticks_per_jiffy - 1, timer_base + TIMER1_VAL_OFF);
+
+		/*
+		 * Enable timer.
+		 */
+		u = readl(timer_base + TIMER_CTRL_OFF);
+
+		writel((u | TIMER1_EN | TIMER1_RELOAD_EN |
+			TIMER1_DIV(TIMER_DIVIDER_SHIFT)),
+		       timer_base + TIMER_CTRL_OFF);
+	} else {
+		/*
+		 * Disable timer.
+		 */
+		u = readl(timer_base + TIMER_CTRL_OFF);
+		writel(u & ~TIMER1_EN, timer_base + TIMER_CTRL_OFF);
+
+		/*
+		 * ACK pending timer interrupt.
+		 */
+		writel(TIMER1_CLR_MASK, timer_base + TIMER_EVENTS_STATUS);
+
+	}
+}
+
+static struct clock_event_device armada_370_xp_clkevt = {
+	.name		= "armada_370_xp_tick",
+	.features	= CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_FEAT_PERIODIC,
+	.shift		= 32,
+	.rating		= 300,
+	.set_next_event	= armada_370_xp_clkevt_next_event,
+	.set_mode	= armada_370_xp_clkevt_mode,
+};
+
+static irqreturn_t armada_370_xp_timer_interrupt(int irq, void *dev_id)
+{
+	/*
+	 * ACK timer interrupt and call event handler.
+	 */
+
+	writel(TIMER1_CLR_MASK, timer_base + TIMER_EVENTS_STATUS);
+	armada_370_xp_clkevt.event_handler(&armada_370_xp_clkevt);
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction armada_370_xp_timer_irq = {
+	.name		= "armada_370_xp_tick",
+	.flags		= IRQF_DISABLED | IRQF_TIMER,
+	.handler	= armada_370_xp_timer_interrupt
+};
+
+void __init armada_370_xp_timer_init(void)
+{
+	u32 u;
+	struct device_node *np;
+	unsigned int timer_clk;
+	int ret;
+	np = of_find_compatible_node(NULL, NULL, "marvell,armada-370-xp-timer");
+	timer_base = of_iomap(np, 0);
+	WARN_ON(!timer_base);
+
+	if (of_find_property(np, "marvell,timer-25Mhz", NULL)) {
+		/* The fixed 25MHz timer is available so let's use it */
+		u = readl(timer_base + TIMER_CTRL_OFF);
+		writel(u | TIMER0_25MHZ | TIMER1_25MHZ,
+		       timer_base + TIMER_CTRL_OFF);
+		timer_clk = 25000000;
+	} else {
+		u32 clk = 0;
+		ret = of_property_read_u32(np, "clock-frequency", &clk);
+		WARN_ON(!clk || ret < 0);
+		u = readl(timer_base + TIMER_CTRL_OFF);
+		writel(u & ~(TIMER0_25MHZ | TIMER1_25MHZ),
+		       timer_base + TIMER_CTRL_OFF);
+		timer_clk = clk / TIMER_DIVIDER;
+	}
+
+	/* We use timer 0 as clocksource, and timer 1 for
+	   clockevents */
+	timer_irq = irq_of_parse_and_map(np, 1);
+
+	ticks_per_jiffy = (timer_clk + HZ / 2) / HZ;
+
+	/*
+	 * Set scale and timer for sched_clock.
+	 */
+	setup_sched_clock(armada_370_xp_read_sched_clock, 32, timer_clk);
+
+	/*
+	 * Setup free-running clocksource timer (interrupts
+	 * disabled).
+	 */
+	writel(0xffffffff, timer_base + TIMER0_VAL_OFF);
+	writel(0xffffffff, timer_base + TIMER0_RELOAD_OFF);
+
+	u = readl(timer_base + TIMER_CTRL_OFF);
+
+	writel((u | TIMER0_EN | TIMER0_RELOAD_EN |
+		TIMER0_DIV(TIMER_DIVIDER_SHIFT)), timer_base + TIMER_CTRL_OFF);
+
+	clocksource_mmio_init(timer_base + TIMER0_VAL_OFF,
+			      "armada_370_xp_clocksource",
+			      timer_clk, 300, 32, clocksource_mmio_readl_down);
+
+	/*
+	 * Setup clockevent timer (interrupt-driven).
+	 */
+	setup_irq(timer_irq, &armada_370_xp_timer_irq);
+	armada_370_xp_clkevt.cpumask = cpumask_of(0);
+	clockevents_config_and_register(&armada_370_xp_clkevt,
+					timer_clk, 1, 0xfffffffe);
+}
+
diff --git a/drivers/connector/cn_proc.c b/drivers/connector/cn_proc.c
index 77e1e6cd66c..3e92b7d3fcd 100644
--- a/drivers/connector/cn_proc.c
+++ b/drivers/connector/cn_proc.c
@@ -46,7 +46,7 @@ static DEFINE_PER_CPU(__u32, proc_event_counts) = { 0 };
 static inline void get_seq(__u32 *ts, int *cpu)
 {
 	preempt_disable();
-	*ts = __this_cpu_inc_return(proc_event_counts) -1;
+	*ts = __this_cpu_inc_return(proc_event_counts) - 1;
 	*cpu = smp_processor_id();
 	preempt_enable();
 }
@@ -62,8 +62,8 @@ void proc_fork_connector(struct task_struct *task)
 	if (atomic_read(&proc_event_num_listeners) < 1)
 		return;
 
-	msg = (struct cn_msg*)buffer;
-	ev = (struct proc_event*)msg->data;
+	msg = (struct cn_msg *)buffer;
+	ev = (struct proc_event *)msg->data;
 	get_seq(&msg->seq, &ev->cpu);
 	ktime_get_ts(&ts); /* get high res monotonic timestamp */
 	put_unaligned(timespec_to_ns(&ts), (__u64 *)&ev->timestamp_ns);
@@ -93,8 +93,8 @@ void proc_exec_connector(struct task_struct *task)
 	if (atomic_read(&proc_event_num_listeners) < 1)
 		return;
 
-	msg = (struct cn_msg*)buffer;
-	ev = (struct proc_event*)msg->data;
+	msg = (struct cn_msg *)buffer;
+	ev = (struct proc_event *)msg->data;
 	get_seq(&msg->seq, &ev->cpu);
 	ktime_get_ts(&ts); /* get high res monotonic timestamp */
 	put_unaligned(timespec_to_ns(&ts), (__u64 *)&ev->timestamp_ns);
@@ -119,8 +119,8 @@ void proc_id_connector(struct task_struct *task, int which_id)
 	if (atomic_read(&proc_event_num_listeners) < 1)
 		return;
 
-	msg = (struct cn_msg*)buffer;
-	ev = (struct proc_event*)msg->data;
+	msg = (struct cn_msg *)buffer;
+	ev = (struct proc_event *)msg->data;
 	ev->what = which_id;
 	ev->event_data.id.process_pid = task->pid;
 	ev->event_data.id.process_tgid = task->tgid;
@@ -134,7 +134,7 @@ void proc_id_connector(struct task_struct *task, int which_id)
 		ev->event_data.id.e.egid = cred->egid;
 	} else {
 		rcu_read_unlock();
-	     	return;
+		return;
 	}
 	rcu_read_unlock();
 	get_seq(&msg->seq, &ev->cpu);
@@ -241,8 +241,8 @@ void proc_exit_connector(struct task_struct *task)
 	if (atomic_read(&proc_event_num_listeners) < 1)
 		return;
 
-	msg = (struct cn_msg*)buffer;
-	ev = (struct proc_event*)msg->data;
+	msg = (struct cn_msg *)buffer;
+	ev = (struct proc_event *)msg->data;
 	get_seq(&msg->seq, &ev->cpu);
 	ktime_get_ts(&ts); /* get high res monotonic timestamp */
 	put_unaligned(timespec_to_ns(&ts), (__u64 *)&ev->timestamp_ns);
@@ -276,8 +276,8 @@ static void cn_proc_ack(int err, int rcvd_seq, int rcvd_ack)
 	if (atomic_read(&proc_event_num_listeners) < 1)
 		return;
 
-	msg = (struct cn_msg*)buffer;
-	ev = (struct proc_event*)msg->data;
+	msg = (struct cn_msg *)buffer;
+	ev = (struct proc_event *)msg->data;
 	msg->seq = rcvd_seq;
 	ktime_get_ts(&ts); /* get high res monotonic timestamp */
 	put_unaligned(timespec_to_ns(&ts), (__u64 *)&ev->timestamp_ns);
@@ -303,7 +303,7 @@ static void cn_proc_mcast_ctl(struct cn_msg *msg,
 	if (msg->len != sizeof(*mc_op))
 		return;
 
-	mc_op = (enum proc_cn_mcast_op*)msg->data;
+	mc_op = (enum proc_cn_mcast_op *)msg->data;
 	switch (*mc_op) {
 	case PROC_CN_MCAST_LISTEN:
 		atomic_inc(&proc_event_num_listeners);
@@ -325,11 +325,11 @@ static void cn_proc_mcast_ctl(struct cn_msg *msg,
  */
 static int __init cn_proc_init(void)
 {
-	int err;
-
-	if ((err = cn_add_callback(&cn_proc_event_id, "cn_proc",
-	 			   &cn_proc_mcast_ctl))) {
-		printk(KERN_WARNING "cn_proc failed to register\n");
+	int err = cn_add_callback(&cn_proc_event_id,
+				  "cn_proc",
+				  &cn_proc_mcast_ctl);
+	if (err) {
+		pr_warn("cn_proc failed to register\n");
 		return err;
 	}
 	return 0;
diff --git a/drivers/connector/cn_queue.c b/drivers/connector/cn_queue.c
index c42c9d51779..1f8bf054d11 100644
--- a/drivers/connector/cn_queue.c
+++ b/drivers/connector/cn_queue.c
@@ -1,5 +1,5 @@
 /*
- * 	cn_queue.c
+ *	cn_queue.c
  *
  * 2004+ Copyright (c) Evgeniy Polyakov <zbr@ioremap.net>
  * All rights reserved.
@@ -34,13 +34,14 @@
 static struct cn_callback_entry *
 cn_queue_alloc_callback_entry(struct cn_queue_dev *dev, const char *name,
 			      struct cb_id *id,
-			      void (*callback)(struct cn_msg *, struct netlink_skb_parms *))
+			      void (*callback)(struct cn_msg *,
+					       struct netlink_skb_parms *))
 {
 	struct cn_callback_entry *cbq;
 
 	cbq = kzalloc(sizeof(*cbq), GFP_KERNEL);
 	if (!cbq) {
-		printk(KERN_ERR "Failed to create new callback queue.\n");
+		pr_err("Failed to create new callback queue.\n");
 		return NULL;
 	}
 
@@ -71,7 +72,8 @@ int cn_cb_equal(struct cb_id *i1, struct cb_id *i2)
 
 int cn_queue_add_callback(struct cn_queue_dev *dev, const char *name,
 			  struct cb_id *id,
-			  void (*callback)(struct cn_msg *, struct netlink_skb_parms *))
+			  void (*callback)(struct cn_msg *,
+					   struct netlink_skb_parms *))
 {
 	struct cn_callback_entry *cbq, *__cbq;
 	int found = 0;
@@ -149,7 +151,7 @@ void cn_queue_free_dev(struct cn_queue_dev *dev)
 	spin_unlock_bh(&dev->queue_lock);
 
 	while (atomic_read(&dev->refcnt)) {
-		printk(KERN_INFO "Waiting for %s to become free: refcnt=%d.\n",
+		pr_info("Waiting for %s to become free: refcnt=%d.\n",
 		       dev->name, atomic_read(&dev->refcnt));
 		msleep(1000);
 	}
diff --git a/drivers/connector/connector.c b/drivers/connector/connector.c
index dde6a0fad40..82fa4f0f91d 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -1,5 +1,5 @@
 /*
- * 	connector.c
+ *	connector.c
  *
  * 2004+ Copyright (c) Evgeniy Polyakov <zbr@ioremap.net>
  * All rights reserved.
@@ -101,19 +101,19 @@ int cn_netlink_send(struct cn_msg *msg, u32 __group, gfp_t gfp_mask)
 	if (!skb)
 		return -ENOMEM;
 
-	nlh = NLMSG_PUT(skb, 0, msg->seq, NLMSG_DONE, size - sizeof(*nlh));
+	nlh = nlmsg_put(skb, 0, msg->seq, NLMSG_DONE, size - sizeof(*nlh), 0);
+	if (!nlh) {
+		kfree_skb(skb);
+		return -EMSGSIZE;
+	}
 
-	data = NLMSG_DATA(nlh);
+	data = nlmsg_data(nlh);
 
 	memcpy(data, msg, sizeof(*data) + msg->len);
 
 	NETLINK_CB(skb).dst_group = group;
 
 	return netlink_broadcast(dev->nls, skb, 0, group, gfp_mask);
-
-nlmsg_failure:
-	kfree_skb(skb);
-	return -EINVAL;
 }
 EXPORT_SYMBOL_GPL(cn_netlink_send);
 
@@ -185,7 +185,8 @@ static void cn_rx_skb(struct sk_buff *__skb)
  * May sleep.
  */
 int cn_add_callback(struct cb_id *id, const char *name,
-		    void (*callback)(struct cn_msg *, struct netlink_skb_parms *))
+		    void (*callback)(struct cn_msg *,
+				     struct netlink_skb_parms *))
 {
 	int err;
 	struct cn_dev *dev = &cdev;
@@ -251,15 +252,20 @@ static const struct file_operations cn_file_ops = {
 	.release = single_release
 };
 
+static struct cn_dev cdev = {
+	.input   = cn_rx_skb,
+};
+
 static int __devinit cn_init(void)
 {
 	struct cn_dev *dev = &cdev;
-
-	dev->input = cn_rx_skb;
+	struct netlink_kernel_cfg cfg = {
+		.groups	= CN_NETLINK_USERS + 0xf,
+		.input	= dev->input,
+	};
 
 	dev->nls = netlink_kernel_create(&init_net, NETLINK_CONNECTOR,
-					 CN_NETLINK_USERS + 0xf,
-					 dev->input, NULL, THIS_MODULE);
+					 THIS_MODULE, &cfg);
 	if (!dev->nls)
 		return -EIO;
 
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 7f2f149ae40..fb8a5279c5d 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -138,7 +138,7 @@ void disable_cpufreq(void)
 static LIST_HEAD(cpufreq_governor_list);
 static DEFINE_MUTEX(cpufreq_governor_mutex);
 
-struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
+static struct cpufreq_policy *__cpufreq_cpu_get(unsigned int cpu, bool sysfs)
 {
 	struct cpufreq_policy *data;
 	unsigned long flags;
@@ -162,7 +162,7 @@ struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
 	if (!data)
 		goto err_out_put_module;
 
-	if (!kobject_get(&data->kobj))
+	if (!sysfs && !kobject_get(&data->kobj))
 		goto err_out_put_module;
 
 	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
@@ -175,16 +175,35 @@ err_out_unlock:
 err_out:
 	return NULL;
 }
+
+struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
+{
+	return __cpufreq_cpu_get(cpu, false);
+}
 EXPORT_SYMBOL_GPL(cpufreq_cpu_get);
 
+static struct cpufreq_policy *cpufreq_cpu_get_sysfs(unsigned int cpu)
+{
+	return __cpufreq_cpu_get(cpu, true);
+}
 
-void cpufreq_cpu_put(struct cpufreq_policy *data)
+static void __cpufreq_cpu_put(struct cpufreq_policy *data, bool sysfs)
 {
-	kobject_put(&data->kobj);
+	if (!sysfs)
+		kobject_put(&data->kobj);
 	module_put(cpufreq_driver->owner);
 }
+
+void cpufreq_cpu_put(struct cpufreq_policy *data)
+{
+	__cpufreq_cpu_put(data, false);
+}
 EXPORT_SYMBOL_GPL(cpufreq_cpu_put);
 
+static void cpufreq_cpu_put_sysfs(struct cpufreq_policy *data)
+{
+	__cpufreq_cpu_put(data, true);
+}
 
 /*********************************************************************
  *            EXTERNALLY AFFECTING FREQUENCY CHANGES                 *
@@ -617,7 +636,7 @@ static ssize_t show(struct kobject *kobj, struct attribute *attr, char *buf)
 	struct cpufreq_policy *policy = to_policy(kobj);
 	struct freq_attr *fattr = to_attr(attr);
 	ssize_t ret = -EINVAL;
-	policy = cpufreq_cpu_get(policy->cpu);
+	policy = cpufreq_cpu_get_sysfs(policy->cpu);
 	if (!policy)
 		goto no_policy;
 
@@ -631,7 +650,7 @@ static ssize_t show(struct kobject *kobj, struct attribute *attr, char *buf)
 
 	unlock_policy_rwsem_read(policy->cpu);
 fail:
-	cpufreq_cpu_put(policy);
+	cpufreq_cpu_put_sysfs(policy);
 no_policy:
 	return ret;
 }
@@ -642,7 +661,7 @@ static ssize_t store(struct kobject *kobj, struct attribute *attr,
 	struct cpufreq_policy *policy = to_policy(kobj);
 	struct freq_attr *fattr = to_attr(attr);
 	ssize_t ret = -EINVAL;
-	policy = cpufreq_cpu_get(policy->cpu);
+	policy = cpufreq_cpu_get_sysfs(policy->cpu);
 	if (!policy)
 		goto no_policy;
 
@@ -656,7 +675,7 @@ static ssize_t store(struct kobject *kobj, struct attribute *attr,
 
 	unlock_policy_rwsem_write(policy->cpu);
 fail:
-	cpufreq_cpu_put(policy);
+	cpufreq_cpu_put_sysfs(policy);
 no_policy:
 	return ret;
 }
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index b243a7ee01f..af2d81e10f7 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -62,8 +62,18 @@ static int exynos_target(struct cpufreq_policy *policy,
 		goto out;
 	}
 
-	if (cpufreq_frequency_table_target(policy, freq_table,
-					   freqs.old, relation, &old_index)) {
+	/*
+	 * The policy max have been changed so that we cannot get proper
+	 * old_index with cpufreq_frequency_table_target(). Thus, ignore
+	 * policy and get the index from the raw freqeuncy table.
+	 */
+	for (old_index = 0;
+		freq_table[old_index].frequency != CPUFREQ_TABLE_END;
+		old_index++)
+		if (freq_table[old_index].frequency == freqs.old)
+			break;
+
+	if (freq_table[old_index].frequency == CPUFREQ_TABLE_END) {
 		ret = -EINVAL;
 		goto out;
 	}
diff --git a/drivers/cpufreq/exynos5250-cpufreq.c b/drivers/cpufreq/exynos5250-cpufreq.c
index a88331644eb..e64c253cb16 100644
--- a/drivers/cpufreq/exynos5250-cpufreq.c
+++ b/drivers/cpufreq/exynos5250-cpufreq.c
@@ -65,20 +65,20 @@ static unsigned int clkdiv_cpu0_5250[CPUFREQ_LEVEL_END][8] = {
 	 * Clock divider value for following
 	 * { ARM, CPUD, ACP, PERIPH, ATB, PCLK_DBG, APLL, ARM2 }
 	 */
-	{ 0, 3, 7, 7, 6, 1, 3, 0 },	/* 1700 MHz - N/A */
-	{ 0, 3, 7, 7, 6, 1, 3, 0 },	/* 1600 MHz - N/A */
-	{ 0, 3, 7, 7, 5, 1, 3, 0 },	/* 1500 MHz - N/A */
-	{ 0, 3, 7, 7, 6, 1, 3, 0 },	/* 1400 MHz */
-	{ 0, 3, 7, 7, 6, 1, 3, 0 },	/* 1300 MHz */
-	{ 0, 3, 7, 7, 5, 1, 3, 0 },	/* 1200 MHz */
-	{ 0, 2, 7, 7, 5, 1, 2, 0 },	/* 1100 MHz */
-	{ 0, 2, 7, 7, 4, 1, 2, 0 },	/* 1000 MHz */
-	{ 0, 2, 7, 7, 4, 1, 2, 0 },	/* 900 MHz */
-	{ 0, 2, 7, 7, 3, 1, 1, 0 },	/* 800 MHz */
+	{ 0, 3, 7, 7, 7, 3, 5, 0 },	/* 1700 MHz */
+	{ 0, 3, 7, 7, 7, 1, 4, 0 },	/* 1600 MHz */
+	{ 0, 2, 7, 7, 7, 1, 4, 0 },	/* 1500 MHz */
+	{ 0, 2, 7, 7, 6, 1, 4, 0 },	/* 1400 MHz */
+	{ 0, 2, 7, 7, 6, 1, 3, 0 },	/* 1300 MHz */
+	{ 0, 2, 7, 7, 5, 1, 3, 0 },	/* 1200 MHz */
+	{ 0, 3, 7, 7, 5, 1, 3, 0 },	/* 1100 MHz */
+	{ 0, 1, 7, 7, 4, 1, 2, 0 },	/* 1000 MHz */
+	{ 0, 1, 7, 7, 4, 1, 2, 0 },	/* 900 MHz */
+	{ 0, 1, 7, 7, 4, 1, 2, 0 },	/* 800 MHz */
 	{ 0, 1, 7, 7, 3, 1, 1, 0 },	/* 700 MHz */
-	{ 0, 1, 7, 7, 2, 1, 1, 0 },	/* 600 MHz */
+	{ 0, 1, 7, 7, 3, 1, 1, 0 },	/* 600 MHz */
 	{ 0, 1, 7, 7, 2, 1, 1, 0 },	/* 500 MHz */
-	{ 0, 1, 7, 7, 1, 1, 1, 0 },	/* 400 MHz */
+	{ 0, 1, 7, 7, 2, 1, 1, 0 },	/* 400 MHz */
 	{ 0, 1, 7, 7, 1, 1, 1, 0 },	/* 300 MHz */
 	{ 0, 1, 7, 7, 1, 1, 1, 0 },	/* 200 MHz */
 };
@@ -87,9 +87,9 @@ static unsigned int clkdiv_cpu1_5250[CPUFREQ_LEVEL_END][2] = {
 	/* Clock divider value for following
 	 * { COPY, HPM }
 	 */
-	{ 0, 2 },	/* 1700 MHz - N/A */
-	{ 0, 2 },	/* 1600 MHz - N/A */
-	{ 0, 2 },	/* 1500 MHz - N/A */
+	{ 0, 2 },	/* 1700 MHz */
+	{ 0, 2 },	/* 1600 MHz */
+	{ 0, 2 },	/* 1500 MHz */
 	{ 0, 2 },	/* 1400 MHz */
 	{ 0, 2 },	/* 1300 MHz */
 	{ 0, 2 },	/* 1200 MHz */
@@ -106,10 +106,10 @@ static unsigned int clkdiv_cpu1_5250[CPUFREQ_LEVEL_END][2] = {
 };
 
 static unsigned int exynos5_apll_pms_table[CPUFREQ_LEVEL_END] = {
-	(0),				/* 1700 MHz - N/A */
-	(0),				/* 1600 MHz - N/A */
-	(0),				/* 1500 MHz - N/A */
-	(0),				/* 1400 MHz */
+	((425 << 16) | (6 << 8) | 0),	/* 1700 MHz */
+	((200 << 16) | (3 << 8) | 0),	/* 1600 MHz */
+	((250 << 16) | (4 << 8) | 0),	/* 1500 MHz */
+	((175 << 16) | (3 << 8) | 0),	/* 1400 MHz */
 	((325 << 16) | (6 << 8) | 0),	/* 1300 MHz */
 	((200 << 16) | (4 << 8) | 0),	/* 1200 MHz */
 	((275 << 16) | (6 << 8) | 0),	/* 1100 MHz */
@@ -126,9 +126,10 @@ static unsigned int exynos5_apll_pms_table[CPUFREQ_LEVEL_END] = {
 
 /* ASV group voltage table */
 static const unsigned int asv_voltage_5250[CPUFREQ_LEVEL_END] = {
-	0, 0, 0, 0, 0, 0, 0,	/* 1700 MHz ~ 1100 MHz Not supported */
-	1175000, 1125000, 1075000, 1050000, 1000000,
-	950000, 925000, 925000, 900000
+	1300000, 1250000, 1225000, 1200000, 1150000,
+	1125000, 1100000, 1075000, 1050000, 1025000,
+	1012500, 1000000,  975000,  950000,  937500,
+	925000
 };
 
 static void set_clkdiv(unsigned int div_index)
@@ -248,15 +249,7 @@ static void __init set_volt_table(void)
 {
 	unsigned int i;
 
-	exynos5250_freq_table[L0].frequency = CPUFREQ_ENTRY_INVALID;
-	exynos5250_freq_table[L1].frequency = CPUFREQ_ENTRY_INVALID;
-	exynos5250_freq_table[L2].frequency = CPUFREQ_ENTRY_INVALID;
-	exynos5250_freq_table[L3].frequency = CPUFREQ_ENTRY_INVALID;
-	exynos5250_freq_table[L4].frequency = CPUFREQ_ENTRY_INVALID;
-	exynos5250_freq_table[L5].frequency = CPUFREQ_ENTRY_INVALID;
-	exynos5250_freq_table[L6].frequency = CPUFREQ_ENTRY_INVALID;
-
-	max_support_idx = L7;
+	max_support_idx = L0;
 
 	for (i = 0 ; i < CPUFREQ_LEVEL_END ; i++)
 		exynos5250_volt_table[i] = asv_voltage_5250[i];
diff --git a/drivers/cpufreq/s3c2416-cpufreq.c b/drivers/cpufreq/s3c2416-cpufreq.c
index 50d2f15a3c8..bcc053bc02c 100644
--- a/drivers/cpufreq/s3c2416-cpufreq.c
+++ b/drivers/cpufreq/s3c2416-cpufreq.c
@@ -153,7 +153,7 @@ static int s3c2416_cpufreq_enter_dvs(struct s3c2416_data *s3c_freq, int idx)
 	if (s3c_freq->vddarm) {
 		dvfs = &s3c2416_dvfs_table[idx];
 
-		pr_debug("cpufreq: setting regultor to %d-%d\n",
+		pr_debug("cpufreq: setting regulator to %d-%d\n",
 			 dvfs->vddarm_min, dvfs->vddarm_max);
 		ret = regulator_set_voltage(s3c_freq->vddarm,
 					    dvfs->vddarm_min,
@@ -186,7 +186,7 @@ static int s3c2416_cpufreq_leave_dvs(struct s3c2416_data *s3c_freq, int idx)
 	if (s3c_freq->vddarm) {
 		dvfs = &s3c2416_dvfs_table[idx];
 
-		pr_debug("cpufreq: setting regultor to %d-%d\n",
+		pr_debug("cpufreq: setting regulator to %d-%d\n",
 			 dvfs->vddarm_min, dvfs->vddarm_max);
 		ret = regulator_set_voltage(s3c_freq->vddarm,
 					    dvfs->vddarm_min,
diff --git a/drivers/cpufreq/speedstep-ich.c b/drivers/cpufreq/speedstep-ich.c
index 7432b3a72cd..e29b59aa68a 100644
--- a/drivers/cpufreq/speedstep-ich.c
+++ b/drivers/cpufreq/speedstep-ich.c
@@ -203,7 +203,7 @@ static unsigned int speedstep_detect_chipset(void)
 	if (speedstep_chipset_dev) {
 		/* speedstep.c causes lockups on Dell Inspirons 8000 and
 		 * 8100 which use a pretty old revision of the 82815
-		 * host brige. Abort on these systems.
+		 * host bridge. Abort on these systems.
 		 */
 		static struct pci_dev *hostbridge;
 
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index 78a666d1e5f..a76b689e553 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -18,3 +18,6 @@ config CPU_IDLE_GOV_MENU
 	bool
 	depends on CPU_IDLE && NO_HZ
 	default y
+
+config ARCH_NEEDS_CPU_IDLE_COUPLED
+	def_bool n
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index 5634f88379d..38c8f69f30c 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -3,3 +3,4 @@
 #
 
 obj-y += cpuidle.o driver.o governor.o sysfs.o governors/
+obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
diff --git a/drivers/cpuidle/coupled.c b/drivers/cpuidle/coupled.c
new file mode 100644
index 00000000000..2c9bf269223
--- /dev/null
+++ b/drivers/cpuidle/coupled.c
@@ -0,0 +1,715 @@
+/*
+ * coupled.c - helper functions to enter the same idle state on multiple cpus
+ *
+ * Copyright (c) 2011 Google, Inc.
+ *
+ * Author: Colin Cross <ccross@android.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/cpu.h>
+#include <linux/cpuidle.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "cpuidle.h"
+
+/**
+ * DOC: Coupled cpuidle states
+ *
+ * On some ARM SMP SoCs (OMAP4460, Tegra 2, and probably more), the
+ * cpus cannot be independently powered down, either due to
+ * sequencing restrictions (on Tegra 2, cpu 0 must be the last to
+ * power down), or due to HW bugs (on OMAP4460, a cpu powering up
+ * will corrupt the gic state unless the other cpu runs a work
+ * around).  Each cpu has a power state that it can enter without
+ * coordinating with the other cpu (usually Wait For Interrupt, or
+ * WFI), and one or more "coupled" power states that affect blocks
+ * shared between the cpus (L2 cache, interrupt controller, and
+ * sometimes the whole SoC).  Entering a coupled power state must
+ * be tightly controlled on both cpus.
+ *
+ * This file implements a solution, where each cpu will wait in the
+ * WFI state until all cpus are ready to enter a coupled state, at
+ * which point the coupled state function will be called on all
+ * cpus at approximately the same time.
+ *
+ * Once all cpus are ready to enter idle, they are woken by an smp
+ * cross call.  At this point, there is a chance that one of the
+ * cpus will find work to do, and choose not to enter idle.  A
+ * final pass is needed to guarantee that all cpus will call the
+ * power state enter function at the same time.  During this pass,
+ * each cpu will increment the ready counter, and continue once the
+ * ready counter matches the number of online coupled cpus.  If any
+ * cpu exits idle, the other cpus will decrement their counter and
+ * retry.
+ *
+ * requested_state stores the deepest coupled idle state each cpu
+ * is ready for.  It is assumed that the states are indexed from
+ * shallowest (highest power, lowest exit latency) to deepest
+ * (lowest power, highest exit latency).  The requested_state
+ * variable is not locked.  It is only written from the cpu that
+ * it stores (or by the on/offlining cpu if that cpu is offline),
+ * and only read after all the cpus are ready for the coupled idle
+ * state are are no longer updating it.
+ *
+ * Three atomic counters are used.  alive_count tracks the number
+ * of cpus in the coupled set that are currently or soon will be
+ * online.  waiting_count tracks the number of cpus that are in
+ * the waiting loop, in the ready loop, or in the coupled idle state.
+ * ready_count tracks the number of cpus that are in the ready loop
+ * or in the coupled idle state.
+ *
+ * To use coupled cpuidle states, a cpuidle driver must:
+ *
+ *    Set struct cpuidle_device.coupled_cpus to the mask of all
+ *    coupled cpus, usually the same as cpu_possible_mask if all cpus
+ *    are part of the same cluster.  The coupled_cpus mask must be
+ *    set in the struct cpuidle_device for each cpu.
+ *
+ *    Set struct cpuidle_device.safe_state to a state that is not a
+ *    coupled state.  This is usually WFI.
+ *
+ *    Set CPUIDLE_FLAG_COUPLED in struct cpuidle_state.flags for each
+ *    state that affects multiple cpus.
+ *
+ *    Provide a struct cpuidle_state.enter function for each state
+ *    that affects multiple cpus.  This function is guaranteed to be
+ *    called on all cpus at approximately the same time.  The driver
+ *    should ensure that the cpus all abort together if any cpu tries
+ *    to abort once the function is called.  The function should return
+ *    with interrupts still disabled.
+ */
+
+/**
+ * struct cpuidle_coupled - data for set of cpus that share a coupled idle state
+ * @coupled_cpus: mask of cpus that are part of the coupled set
+ * @requested_state: array of requested states for cpus in the coupled set
+ * @ready_waiting_counts: combined count of cpus  in ready or waiting loops
+ * @online_count: count of cpus that are online
+ * @refcnt: reference count of cpuidle devices that are using this struct
+ * @prevent: flag to prevent coupled idle while a cpu is hotplugging
+ */
+struct cpuidle_coupled {
+	cpumask_t coupled_cpus;
+	int requested_state[NR_CPUS];
+	atomic_t ready_waiting_counts;
+	int online_count;
+	int refcnt;
+	int prevent;
+};
+
+#define WAITING_BITS 16
+#define MAX_WAITING_CPUS (1 << WAITING_BITS)
+#define WAITING_MASK (MAX_WAITING_CPUS - 1)
+#define READY_MASK (~WAITING_MASK)
+
+#define CPUIDLE_COUPLED_NOT_IDLE	(-1)
+
+static DEFINE_MUTEX(cpuidle_coupled_lock);
+static DEFINE_PER_CPU(struct call_single_data, cpuidle_coupled_poke_cb);
+
+/*
+ * The cpuidle_coupled_poked_mask mask is used to avoid calling
+ * __smp_call_function_single with the per cpu call_single_data struct already
+ * in use.  This prevents a deadlock where two cpus are waiting for each others
+ * call_single_data struct to be available
+ */
+static cpumask_t cpuidle_coupled_poked_mask;
+
+/**
+ * cpuidle_coupled_parallel_barrier - synchronize all online coupled cpus
+ * @dev: cpuidle_device of the calling cpu
+ * @a:   atomic variable to hold the barrier
+ *
+ * No caller to this function will return from this function until all online
+ * cpus in the same coupled group have called this function.  Once any caller
+ * has returned from this function, the barrier is immediately available for
+ * reuse.
+ *
+ * The atomic variable a must be initialized to 0 before any cpu calls
+ * this function, will be reset to 0 before any cpu returns from this function.
+ *
+ * Must only be called from within a coupled idle state handler
+ * (state.enter when state.flags has CPUIDLE_FLAG_COUPLED set).
+ *
+ * Provides full smp barrier semantics before and after calling.
+ */
+void cpuidle_coupled_parallel_barrier(struct cpuidle_device *dev, atomic_t *a)
+{
+	int n = dev->coupled->online_count;
+
+	smp_mb__before_atomic_inc();
+	atomic_inc(a);
+
+	while (atomic_read(a) < n)
+		cpu_relax();
+
+	if (atomic_inc_return(a) == n * 2) {
+		atomic_set(a, 0);
+		return;
+	}
+
+	while (atomic_read(a) > n)
+		cpu_relax();
+}
+
+/**
+ * cpuidle_state_is_coupled - check if a state is part of a coupled set
+ * @dev: struct cpuidle_device for the current cpu
+ * @drv: struct cpuidle_driver for the platform
+ * @state: index of the target state in drv->states
+ *
+ * Returns true if the target state is coupled with cpus besides this one
+ */
+bool cpuidle_state_is_coupled(struct cpuidle_device *dev,
+	struct cpuidle_driver *drv, int state)
+{
+	return drv->states[state].flags & CPUIDLE_FLAG_COUPLED;
+}
+
+/**
+ * cpuidle_coupled_set_ready - mark a cpu as ready
+ * @coupled: the struct coupled that contains the current cpu
+ */
+static inline void cpuidle_coupled_set_ready(struct cpuidle_coupled *coupled)
+{
+	atomic_add(MAX_WAITING_CPUS, &coupled->ready_waiting_counts);
+}
+
+/**
+ * cpuidle_coupled_set_not_ready - mark a cpu as not ready
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Decrements the ready counter, unless the ready (and thus the waiting) counter
+ * is equal to the number of online cpus.  Prevents a race where one cpu
+ * decrements the waiting counter and then re-increments it just before another
+ * cpu has decremented its ready counter, leading to the ready counter going
+ * down from the number of online cpus without going through the coupled idle
+ * state.
+ *
+ * Returns 0 if the counter was decremented successfully, -EINVAL if the ready
+ * counter was equal to the number of online cpus.
+ */
+static
+inline int cpuidle_coupled_set_not_ready(struct cpuidle_coupled *coupled)
+{
+	int all;
+	int ret;
+
+	all = coupled->online_count || (coupled->online_count << WAITING_BITS);
+	ret = atomic_add_unless(&coupled->ready_waiting_counts,
+		-MAX_WAITING_CPUS, all);
+
+	return ret ? 0 : -EINVAL;
+}
+
+/**
+ * cpuidle_coupled_no_cpus_ready - check if no cpus in a coupled set are ready
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Returns true if all of the cpus in a coupled set are out of the ready loop.
+ */
+static inline int cpuidle_coupled_no_cpus_ready(struct cpuidle_coupled *coupled)
+{
+	int r = atomic_read(&coupled->ready_waiting_counts) >> WAITING_BITS;
+	return r == 0;
+}
+
+/**
+ * cpuidle_coupled_cpus_ready - check if all cpus in a coupled set are ready
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Returns true if all cpus coupled to this target state are in the ready loop
+ */
+static inline bool cpuidle_coupled_cpus_ready(struct cpuidle_coupled *coupled)
+{
+	int r = atomic_read(&coupled->ready_waiting_counts) >> WAITING_BITS;
+	return r == coupled->online_count;
+}
+
+/**
+ * cpuidle_coupled_cpus_waiting - check if all cpus in a coupled set are waiting
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Returns true if all cpus coupled to this target state are in the wait loop
+ */
+static inline bool cpuidle_coupled_cpus_waiting(struct cpuidle_coupled *coupled)
+{
+	int w = atomic_read(&coupled->ready_waiting_counts) & WAITING_MASK;
+	return w == coupled->online_count;
+}
+
+/**
+ * cpuidle_coupled_no_cpus_waiting - check if no cpus in coupled set are waiting
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Returns true if all of the cpus in a coupled set are out of the waiting loop.
+ */
+static inline int cpuidle_coupled_no_cpus_waiting(struct cpuidle_coupled *coupled)
+{
+	int w = atomic_read(&coupled->ready_waiting_counts) & WAITING_MASK;
+	return w == 0;
+}
+
+/**
+ * cpuidle_coupled_get_state - determine the deepest idle state
+ * @dev: struct cpuidle_device for this cpu
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Returns the deepest idle state that all coupled cpus can enter
+ */
+static inline int cpuidle_coupled_get_state(struct cpuidle_device *dev,
+		struct cpuidle_coupled *coupled)
+{
+	int i;
+	int state = INT_MAX;
+
+	/*
+	 * Read barrier ensures that read of requested_state is ordered after
+	 * reads of ready_count.  Matches the write barriers
+	 * cpuidle_set_state_waiting.
+	 */
+	smp_rmb();
+
+	for_each_cpu_mask(i, coupled->coupled_cpus)
+		if (cpu_online(i) && coupled->requested_state[i] < state)
+			state = coupled->requested_state[i];
+
+	return state;
+}
+
+static void cpuidle_coupled_poked(void *info)
+{
+	int cpu = (unsigned long)info;
+	cpumask_clear_cpu(cpu, &cpuidle_coupled_poked_mask);
+}
+
+/**
+ * cpuidle_coupled_poke - wake up a cpu that may be waiting
+ * @cpu: target cpu
+ *
+ * Ensures that the target cpu exits it's waiting idle state (if it is in it)
+ * and will see updates to waiting_count before it re-enters it's waiting idle
+ * state.
+ *
+ * If cpuidle_coupled_poked_mask is already set for the target cpu, that cpu
+ * either has or will soon have a pending IPI that will wake it out of idle,
+ * or it is currently processing the IPI and is not in idle.
+ */
+static void cpuidle_coupled_poke(int cpu)
+{
+	struct call_single_data *csd = &per_cpu(cpuidle_coupled_poke_cb, cpu);
+
+	if (!cpumask_test_and_set_cpu(cpu, &cpuidle_coupled_poked_mask))
+		__smp_call_function_single(cpu, csd, 0);
+}
+
+/**
+ * cpuidle_coupled_poke_others - wake up all other cpus that may be waiting
+ * @dev: struct cpuidle_device for this cpu
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Calls cpuidle_coupled_poke on all other online cpus.
+ */
+static void cpuidle_coupled_poke_others(int this_cpu,
+		struct cpuidle_coupled *coupled)
+{
+	int cpu;
+
+	for_each_cpu_mask(cpu, coupled->coupled_cpus)
+		if (cpu != this_cpu && cpu_online(cpu))
+			cpuidle_coupled_poke(cpu);
+}
+
+/**
+ * cpuidle_coupled_set_waiting - mark this cpu as in the wait loop
+ * @dev: struct cpuidle_device for this cpu
+ * @coupled: the struct coupled that contains the current cpu
+ * @next_state: the index in drv->states of the requested state for this cpu
+ *
+ * Updates the requested idle state for the specified cpuidle device,
+ * poking all coupled cpus out of idle if necessary to let them see the new
+ * state.
+ */
+static void cpuidle_coupled_set_waiting(int cpu,
+		struct cpuidle_coupled *coupled, int next_state)
+{
+	int w;
+
+	coupled->requested_state[cpu] = next_state;
+
+	/*
+	 * If this is the last cpu to enter the waiting state, poke
+	 * all the other cpus out of their waiting state so they can
+	 * enter a deeper state.  This can race with one of the cpus
+	 * exiting the waiting state due to an interrupt and
+	 * decrementing waiting_count, see comment below.
+	 *
+	 * The atomic_inc_return provides a write barrier to order the write
+	 * to requested_state with the later write that increments ready_count.
+	 */
+	w = atomic_inc_return(&coupled->ready_waiting_counts) & WAITING_MASK;
+	if (w == coupled->online_count)
+		cpuidle_coupled_poke_others(cpu, coupled);
+}
+
+/**
+ * cpuidle_coupled_set_not_waiting - mark this cpu as leaving the wait loop
+ * @dev: struct cpuidle_device for this cpu
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Removes the requested idle state for the specified cpuidle device.
+ */
+static void cpuidle_coupled_set_not_waiting(int cpu,
+		struct cpuidle_coupled *coupled)
+{
+	/*
+	 * Decrementing waiting count can race with incrementing it in
+	 * cpuidle_coupled_set_waiting, but that's OK.  Worst case, some
+	 * cpus will increment ready_count and then spin until they
+	 * notice that this cpu has cleared it's requested_state.
+	 */
+	atomic_dec(&coupled->ready_waiting_counts);
+
+	coupled->requested_state[cpu] = CPUIDLE_COUPLED_NOT_IDLE;
+}
+
+/**
+ * cpuidle_coupled_set_done - mark this cpu as leaving the ready loop
+ * @cpu: the current cpu
+ * @coupled: the struct coupled that contains the current cpu
+ *
+ * Marks this cpu as no longer in the ready and waiting loops.  Decrements
+ * the waiting count first to prevent another cpu looping back in and seeing
+ * this cpu as waiting just before it exits idle.
+ */
+static void cpuidle_coupled_set_done(int cpu, struct cpuidle_coupled *coupled)
+{
+	cpuidle_coupled_set_not_waiting(cpu, coupled);
+	atomic_sub(MAX_WAITING_CPUS, &coupled->ready_waiting_counts);
+}
+
+/**
+ * cpuidle_coupled_clear_pokes - spin until the poke interrupt is processed
+ * @cpu - this cpu
+ *
+ * Turns on interrupts and spins until any outstanding poke interrupts have
+ * been processed and the poke bit has been cleared.
+ *
+ * Other interrupts may also be processed while interrupts are enabled, so
+ * need_resched() must be tested after turning interrupts off again to make sure
+ * the interrupt didn't schedule work that should take the cpu out of idle.
+ *
+ * Returns 0 if need_resched was false, -EINTR if need_resched was true.
+ */
+static int cpuidle_coupled_clear_pokes(int cpu)
+{
+	local_irq_enable();
+	while (cpumask_test_cpu(cpu, &cpuidle_coupled_poked_mask))
+		cpu_relax();
+	local_irq_disable();
+
+	return need_resched() ? -EINTR : 0;
+}
+
+/**
+ * cpuidle_enter_state_coupled - attempt to enter a state with coupled cpus
+ * @dev: struct cpuidle_device for the current cpu
+ * @drv: struct cpuidle_driver for the platform
+ * @next_state: index of the requested state in drv->states
+ *
+ * Coordinate with coupled cpus to enter the target state.  This is a two
+ * stage process.  In the first stage, the cpus are operating independently,
+ * and may call into cpuidle_enter_state_coupled at completely different times.
+ * To save as much power as possible, the first cpus to call this function will
+ * go to an intermediate state (the cpuidle_device's safe state), and wait for
+ * all the other cpus to call this function.  Once all coupled cpus are idle,
+ * the second stage will start.  Each coupled cpu will spin until all cpus have
+ * guaranteed that they will call the target_state.
+ *
+ * This function must be called with interrupts disabled.  It may enable
+ * interrupts while preparing for idle, and it will always return with
+ * interrupts enabled.
+ */
+int cpuidle_enter_state_coupled(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int next_state)
+{
+	int entered_state = -1;
+	struct cpuidle_coupled *coupled = dev->coupled;
+
+	if (!coupled)
+		return -EINVAL;
+
+	while (coupled->prevent) {
+		if (cpuidle_coupled_clear_pokes(dev->cpu)) {
+			local_irq_enable();
+			return entered_state;
+		}
+		entered_state = cpuidle_enter_state(dev, drv,
+			dev->safe_state_index);
+	}
+
+	/* Read barrier ensures online_count is read after prevent is cleared */
+	smp_rmb();
+
+	cpuidle_coupled_set_waiting(dev->cpu, coupled, next_state);
+
+retry:
+	/*
+	 * Wait for all coupled cpus to be idle, using the deepest state
+	 * allowed for a single cpu.
+	 */
+	while (!cpuidle_coupled_cpus_waiting(coupled)) {
+		if (cpuidle_coupled_clear_pokes(dev->cpu)) {
+			cpuidle_coupled_set_not_waiting(dev->cpu, coupled);
+			goto out;
+		}
+
+		if (coupled->prevent) {
+			cpuidle_coupled_set_not_waiting(dev->cpu, coupled);
+			goto out;
+		}
+
+		entered_state = cpuidle_enter_state(dev, drv,
+			dev->safe_state_index);
+	}
+
+	if (cpuidle_coupled_clear_pokes(dev->cpu)) {
+		cpuidle_coupled_set_not_waiting(dev->cpu, coupled);
+		goto out;
+	}
+
+	/*
+	 * All coupled cpus are probably idle.  There is a small chance that
+	 * one of the other cpus just became active.  Increment the ready count,
+	 * and spin until all coupled cpus have incremented the counter. Once a
+	 * cpu has incremented the ready counter, it cannot abort idle and must
+	 * spin until either all cpus have incremented the ready counter, or
+	 * another cpu leaves idle and decrements the waiting counter.
+	 */
+
+	cpuidle_coupled_set_ready(coupled);
+	while (!cpuidle_coupled_cpus_ready(coupled)) {
+		/* Check if any other cpus bailed out of idle. */
+		if (!cpuidle_coupled_cpus_waiting(coupled))
+			if (!cpuidle_coupled_set_not_ready(coupled))
+				goto retry;
+
+		cpu_relax();
+	}
+
+	/* all cpus have acked the coupled state */
+	next_state = cpuidle_coupled_get_state(dev, coupled);
+
+	entered_state = cpuidle_enter_state(dev, drv, next_state);
+
+	cpuidle_coupled_set_done(dev->cpu, coupled);
+
+out:
+	/*
+	 * Normal cpuidle states are expected to return with irqs enabled.
+	 * That leads to an inefficiency where a cpu receiving an interrupt
+	 * that brings it out of idle will process that interrupt before
+	 * exiting the idle enter function and decrementing ready_count.  All
+	 * other cpus will need to spin waiting for the cpu that is processing
+	 * the interrupt.  If the driver returns with interrupts disabled,
+	 * all other cpus will loop back into the safe idle state instead of
+	 * spinning, saving power.
+	 *
+	 * Calling local_irq_enable here allows coupled states to return with
+	 * interrupts disabled, but won't cause problems for drivers that
+	 * exit with interrupts enabled.
+	 */
+	local_irq_enable();
+
+	/*
+	 * Wait until all coupled cpus have exited idle.  There is no risk that
+	 * a cpu exits and re-enters the ready state because this cpu has
+	 * already decremented its waiting_count.
+	 */
+	while (!cpuidle_coupled_no_cpus_ready(coupled))
+		cpu_relax();
+
+	return entered_state;
+}
+
+static void cpuidle_coupled_update_online_cpus(struct cpuidle_coupled *coupled)
+{
+	cpumask_t cpus;
+	cpumask_and(&cpus, cpu_online_mask, &coupled->coupled_cpus);
+	coupled->online_count = cpumask_weight(&cpus);
+}
+
+/**
+ * cpuidle_coupled_register_device - register a coupled cpuidle device
+ * @dev: struct cpuidle_device for the current cpu
+ *
+ * Called from cpuidle_register_device to handle coupled idle init.  Finds the
+ * cpuidle_coupled struct for this set of coupled cpus, or creates one if none
+ * exists yet.
+ */
+int cpuidle_coupled_register_device(struct cpuidle_device *dev)
+{
+	int cpu;
+	struct cpuidle_device *other_dev;
+	struct call_single_data *csd;
+	struct cpuidle_coupled *coupled;
+
+	if (cpumask_empty(&dev->coupled_cpus))
+		return 0;
+
+	for_each_cpu_mask(cpu, dev->coupled_cpus) {
+		other_dev = per_cpu(cpuidle_devices, cpu);
+		if (other_dev && other_dev->coupled) {
+			coupled = other_dev->coupled;
+			goto have_coupled;
+		}
+	}
+
+	/* No existing coupled info found, create a new one */
+	coupled = kzalloc(sizeof(struct cpuidle_coupled), GFP_KERNEL);
+	if (!coupled)
+		return -ENOMEM;
+
+	coupled->coupled_cpus = dev->coupled_cpus;
+
+have_coupled:
+	dev->coupled = coupled;
+	if (WARN_ON(!cpumask_equal(&dev->coupled_cpus, &coupled->coupled_cpus)))
+		coupled->prevent++;
+
+	cpuidle_coupled_update_online_cpus(coupled);
+
+	coupled->refcnt++;
+
+	csd = &per_cpu(cpuidle_coupled_poke_cb, dev->cpu);
+	csd->func = cpuidle_coupled_poked;
+	csd->info = (void *)(unsigned long)dev->cpu;
+
+	return 0;
+}
+
+/**
+ * cpuidle_coupled_unregister_device - unregister a coupled cpuidle device
+ * @dev: struct cpuidle_device for the current cpu
+ *
+ * Called from cpuidle_unregister_device to tear down coupled idle.  Removes the
+ * cpu from the coupled idle set, and frees the cpuidle_coupled_info struct if
+ * this was the last cpu in the set.
+ */
+void cpuidle_coupled_unregister_device(struct cpuidle_device *dev)
+{
+	struct cpuidle_coupled *coupled = dev->coupled;
+
+	if (cpumask_empty(&dev->coupled_cpus))
+		return;
+
+	if (--coupled->refcnt)
+		kfree(coupled);
+	dev->coupled = NULL;
+}
+
+/**
+ * cpuidle_coupled_prevent_idle - prevent cpus from entering a coupled state
+ * @coupled: the struct coupled that contains the cpu that is changing state
+ *
+ * Disables coupled cpuidle on a coupled set of cpus.  Used to ensure that
+ * cpu_online_mask doesn't change while cpus are coordinating coupled idle.
+ */
+static void cpuidle_coupled_prevent_idle(struct cpuidle_coupled *coupled)
+{
+	int cpu = get_cpu();
+
+	/* Force all cpus out of the waiting loop. */
+	coupled->prevent++;
+	cpuidle_coupled_poke_others(cpu, coupled);
+	put_cpu();
+	while (!cpuidle_coupled_no_cpus_waiting(coupled))
+		cpu_relax();
+}
+
+/**
+ * cpuidle_coupled_allow_idle - allows cpus to enter a coupled state
+ * @coupled: the struct coupled that contains the cpu that is changing state
+ *
+ * Enables coupled cpuidle on a coupled set of cpus.  Used to ensure that
+ * cpu_online_mask doesn't change while cpus are coordinating coupled idle.
+ */
+static void cpuidle_coupled_allow_idle(struct cpuidle_coupled *coupled)
+{
+	int cpu = get_cpu();
+
+	/*
+	 * Write barrier ensures readers see the new online_count when they
+	 * see prevent == 0.
+	 */
+	smp_wmb();
+	coupled->prevent--;
+	/* Force cpus out of the prevent loop. */
+	cpuidle_coupled_poke_others(cpu, coupled);
+	put_cpu();
+}
+
+/**
+ * cpuidle_coupled_cpu_notify - notifier called during hotplug transitions
+ * @nb: notifier block
+ * @action: hotplug transition
+ * @hcpu: target cpu number
+ *
+ * Called when a cpu is brought on or offline using hotplug.  Updates the
+ * coupled cpu set appropriately
+ */
+static int cpuidle_coupled_cpu_notify(struct notifier_block *nb,
+		unsigned long action, void *hcpu)
+{
+	int cpu = (unsigned long)hcpu;
+	struct cpuidle_device *dev;
+
+	mutex_lock(&cpuidle_lock);
+
+	dev = per_cpu(cpuidle_devices, cpu);
+	if (!dev->coupled)
+		goto out;
+
+	switch (action & ~CPU_TASKS_FROZEN) {
+	case CPU_UP_PREPARE:
+	case CPU_DOWN_PREPARE:
+		cpuidle_coupled_prevent_idle(dev->coupled);
+		break;
+	case CPU_ONLINE:
+	case CPU_DEAD:
+		cpuidle_coupled_update_online_cpus(dev->coupled);
+		/* Fall through */
+	case CPU_UP_CANCELED:
+	case CPU_DOWN_FAILED:
+		cpuidle_coupled_allow_idle(dev->coupled);
+		break;
+	}
+
+out:
+	mutex_unlock(&cpuidle_lock);
+	return NOTIFY_OK;
+}
+
+static struct notifier_block cpuidle_coupled_cpu_notifier = {
+	.notifier_call = cpuidle_coupled_cpu_notify,
+};
+
+static int __init cpuidle_coupled_init(void)
+{
+	return register_cpu_notifier(&cpuidle_coupled_cpu_notifier);
+}
+core_initcall(cpuidle_coupled_init);
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index d90519cec88..e28f6ea46f1 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -92,6 +92,34 @@ int cpuidle_play_dead(void)
 }
 
 /**
+ * cpuidle_enter_state - enter the state and update stats
+ * @dev: cpuidle device for this cpu
+ * @drv: cpuidle driver for this cpu
+ * @next_state: index into drv->states of the state to enter
+ */
+int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
+		int next_state)
+{
+	int entered_state;
+
+	entered_state = cpuidle_enter_ops(dev, drv, next_state);
+
+	if (entered_state >= 0) {
+		/* Update cpuidle counters */
+		/* This can be moved to within driver enter routine
+		 * but that results in multiple copies of same code.
+		 */
+		dev->states_usage[entered_state].time +=
+				(unsigned long long)dev->last_residency;
+		dev->states_usage[entered_state].usage++;
+	} else {
+		dev->last_residency = 0;
+	}
+
+	return entered_state;
+}
+
+/**
  * cpuidle_idle_call - the main idle loop
  *
  * NOTE: no locks or semaphores should be used here
@@ -113,15 +141,6 @@ int cpuidle_idle_call(void)
 	if (!dev || !dev->enabled)
 		return -EBUSY;
 
-#if 0
-	/* shows regressions, re-enable for 2.6.29 */
-	/*
-	 * run any timers that can be run now, at this point
-	 * before calculating the idle duration etc.
-	 */
-	hrtimer_peek_ahead_timers();
-#endif
-
 	/* ask the governor for the next state */
 	next_state = cpuidle_curr_governor->select(drv, dev);
 	if (need_resched()) {
@@ -132,23 +151,15 @@ int cpuidle_idle_call(void)
 	trace_power_start_rcuidle(POWER_CSTATE, next_state, dev->cpu);
 	trace_cpu_idle_rcuidle(next_state, dev->cpu);
 
-	entered_state = cpuidle_enter_ops(dev, drv, next_state);
+	if (cpuidle_state_is_coupled(dev, drv, next_state))
+		entered_state = cpuidle_enter_state_coupled(dev, drv,
+							    next_state);
+	else
+		entered_state = cpuidle_enter_state(dev, drv, next_state);
 
 	trace_power_end_rcuidle(dev->cpu);
 	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, dev->cpu);
 
-	if (entered_state >= 0) {
-		/* Update cpuidle counters */
-		/* This can be moved to within driver enter routine
-		 * but that results in multiple copies of same code.
-		 */
-		dev->states_usage[entered_state].time +=
-				(unsigned long long)dev->last_residency;
-		dev->states_usage[entered_state].usage++;
-	} else {
-		dev->last_residency = 0;
-	}
-
 	/* give the governor an opportunity to reflect on the outcome */
 	if (cpuidle_curr_governor->reflect)
 		cpuidle_curr_governor->reflect(dev, entered_state);
@@ -201,6 +212,22 @@ void cpuidle_resume_and_unlock(void)
 
 EXPORT_SYMBOL_GPL(cpuidle_resume_and_unlock);
 
+/* Currently used in suspend/resume path to suspend cpuidle */
+void cpuidle_pause(void)
+{
+	mutex_lock(&cpuidle_lock);
+	cpuidle_uninstall_idle_handler();
+	mutex_unlock(&cpuidle_lock);
+}
+
+/* Currently used in suspend/resume path to resume cpuidle */
+void cpuidle_resume(void)
+{
+	mutex_lock(&cpuidle_lock);
+	cpuidle_install_idle_handler();
+	mutex_unlock(&cpuidle_lock);
+}
+
 /**
  * cpuidle_wrap_enter - performs timekeeping and irqen around enter function
  * @dev: pointer to a valid cpuidle_device object
@@ -265,7 +292,7 @@ static void poll_idle_init(struct cpuidle_driver *drv)
 	state->power_usage = -1;
 	state->flags = 0;
 	state->enter = poll_idle;
-	state->disable = 0;
+	state->disabled = false;
 }
 #else
 static void poll_idle_init(struct cpuidle_driver *drv) {}
@@ -283,6 +310,9 @@ int cpuidle_enable_device(struct cpuidle_device *dev)
 	int ret, i;
 	struct cpuidle_driver *drv = cpuidle_get_driver();
 
+	if (!dev)
+		return -EINVAL;
+
 	if (dev->enabled)
 		return 0;
 	if (!drv || !cpuidle_curr_governor)
@@ -367,8 +397,6 @@ static int __cpuidle_register_device(struct cpuidle_device *dev)
 	struct device *cpu_dev = get_cpu_device((unsigned long)dev->cpu);
 	struct cpuidle_driver *cpuidle_driver = cpuidle_get_driver();
 
-	if (!dev)
-		return -EINVAL;
 	if (!try_module_get(cpuidle_driver->owner))
 		return -EINVAL;
 
@@ -376,13 +404,25 @@ static int __cpuidle_register_device(struct cpuidle_device *dev)
 
 	per_cpu(cpuidle_devices, dev->cpu) = dev;
 	list_add(&dev->device_list, &cpuidle_detected_devices);
-	if ((ret = cpuidle_add_sysfs(cpu_dev))) {
-		module_put(cpuidle_driver->owner);
-		return ret;
-	}
+	ret = cpuidle_add_sysfs(cpu_dev);
+	if (ret)
+		goto err_sysfs;
+
+	ret = cpuidle_coupled_register_device(dev);
+	if (ret)
+		goto err_coupled;
 
 	dev->registered = 1;
 	return 0;
+
+err_coupled:
+	cpuidle_remove_sysfs(cpu_dev);
+	wait_for_completion(&dev->kobj_unregister);
+err_sysfs:
+	list_del(&dev->device_list);
+	per_cpu(cpuidle_devices, dev->cpu) = NULL;
+	module_put(cpuidle_driver->owner);
+	return ret;
 }
 
 /**
@@ -393,6 +433,9 @@ int cpuidle_register_device(struct cpuidle_device *dev)
 {
 	int ret;
 
+	if (!dev)
+		return -EINVAL;
+
 	mutex_lock(&cpuidle_lock);
 
 	if ((ret = __cpuidle_register_device(dev))) {
@@ -432,6 +475,8 @@ void cpuidle_unregister_device(struct cpuidle_device *dev)
 	wait_for_completion(&dev->kobj_unregister);
 	per_cpu(cpuidle_devices, dev->cpu) = NULL;
 
+	cpuidle_coupled_unregister_device(dev);
+
 	cpuidle_resume_and_unlock();
 
 	module_put(cpuidle_driver->owner);
diff --git a/drivers/cpuidle/cpuidle.h b/drivers/cpuidle/cpuidle.h
index 7db186685c2..76e7f696ad8 100644
--- a/drivers/cpuidle/cpuidle.h
+++ b/drivers/cpuidle/cpuidle.h
@@ -14,6 +14,8 @@ extern struct list_head cpuidle_detected_devices;
 extern struct mutex cpuidle_lock;
 extern spinlock_t cpuidle_driver_lock;
 extern int cpuidle_disabled(void);
+extern int cpuidle_enter_state(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int next_state);
 
 /* idle loop */
 extern void cpuidle_install_idle_handler(void);
@@ -30,4 +32,34 @@ extern void cpuidle_remove_state_sysfs(struct cpuidle_device *device);
 extern int cpuidle_add_sysfs(struct device *dev);
 extern void cpuidle_remove_sysfs(struct device *dev);
 
+#ifdef CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED
+bool cpuidle_state_is_coupled(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int state);
+int cpuidle_enter_state_coupled(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int next_state);
+int cpuidle_coupled_register_device(struct cpuidle_device *dev);
+void cpuidle_coupled_unregister_device(struct cpuidle_device *dev);
+#else
+static inline bool cpuidle_state_is_coupled(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int state)
+{
+	return false;
+}
+
+static inline int cpuidle_enter_state_coupled(struct cpuidle_device *dev,
+		struct cpuidle_driver *drv, int next_state)
+{
+	return -1;
+}
+
+static inline int cpuidle_coupled_register_device(struct cpuidle_device *dev)
+{
+	return 0;
+}
+
+static inline void cpuidle_coupled_unregister_device(struct cpuidle_device *dev)
+{
+}
+#endif
+
 #endif /* __DRIVER_CPUIDLE_H */
diff --git a/drivers/cpuidle/driver.c b/drivers/cpuidle/driver.c
index 40cd3f3024d..58bf3b1ac9c 100644
--- a/drivers/cpuidle/driver.c
+++ b/drivers/cpuidle/driver.c
@@ -16,6 +16,7 @@
 
 static struct cpuidle_driver *cpuidle_curr_driver;
 DEFINE_SPINLOCK(cpuidle_driver_lock);
+int cpuidle_driver_refcount;
 
 static void __cpuidle_register_driver(struct cpuidle_driver *drv)
 {
@@ -89,8 +90,34 @@ void cpuidle_unregister_driver(struct cpuidle_driver *drv)
 	}
 
 	spin_lock(&cpuidle_driver_lock);
-	cpuidle_curr_driver = NULL;
+
+	if (!WARN_ON(cpuidle_driver_refcount > 0))
+		cpuidle_curr_driver = NULL;
+
 	spin_unlock(&cpuidle_driver_lock);
 }
 
 EXPORT_SYMBOL_GPL(cpuidle_unregister_driver);
+
+struct cpuidle_driver *cpuidle_driver_ref(void)
+{
+	struct cpuidle_driver *drv;
+
+	spin_lock(&cpuidle_driver_lock);
+
+	drv = cpuidle_curr_driver;
+	cpuidle_driver_refcount++;
+
+	spin_unlock(&cpuidle_driver_lock);
+	return drv;
+}
+
+void cpuidle_driver_unref(void)
+{
+	spin_lock(&cpuidle_driver_lock);
+
+	if (!WARN_ON(cpuidle_driver_refcount <= 0))
+		cpuidle_driver_refcount--;
+
+	spin_unlock(&cpuidle_driver_lock);
+}
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index 06335756ea1..5b1f2c372c1 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -281,7 +281,8 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 * unless the timer is happening really really soon.
 	 */
 	if (data->expected_us > 5 &&
-		drv->states[CPUIDLE_DRIVER_STATE_START].disable == 0)
+	    !drv->states[CPUIDLE_DRIVER_STATE_START].disabled &&
+		dev->states_usage[CPUIDLE_DRIVER_STATE_START].disable == 0)
 		data->last_state_idx = CPUIDLE_DRIVER_STATE_START;
 
 	/*
@@ -290,8 +291,9 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 */
 	for (i = CPUIDLE_DRIVER_STATE_START; i < drv->state_count; i++) {
 		struct cpuidle_state *s = &drv->states[i];
+		struct cpuidle_state_usage *su = &dev->states_usage[i];
 
-		if (s->disable)
+		if (s->disabled || su->disable)
 			continue;
 		if (s->target_residency > data->predicted_us)
 			continue;
diff --git a/drivers/cpuidle/sysfs.c b/drivers/cpuidle/sysfs.c
index 88032b4dc6d..5f809e337b8 100644
--- a/drivers/cpuidle/sysfs.c
+++ b/drivers/cpuidle/sysfs.c
@@ -217,7 +217,8 @@ struct cpuidle_state_attr {
 	struct attribute attr;
 	ssize_t (*show)(struct cpuidle_state *, \
 					struct cpuidle_state_usage *, char *);
-	ssize_t (*store)(struct cpuidle_state *, const char *, size_t);
+	ssize_t (*store)(struct cpuidle_state *, \
+			struct cpuidle_state_usage *, const char *, size_t);
 };
 
 #define define_one_state_ro(_name, show) \
@@ -233,21 +234,22 @@ static ssize_t show_state_##_name(struct cpuidle_state *state, \
 	return sprintf(buf, "%u\n", state->_name);\
 }
 
-#define define_store_state_function(_name) \
+#define define_store_state_ull_function(_name) \
 static ssize_t store_state_##_name(struct cpuidle_state *state, \
+		struct cpuidle_state_usage *state_usage, \
 		const char *buf, size_t size) \
 { \
-	long value; \
+	unsigned long long value; \
 	int err; \
 	if (!capable(CAP_SYS_ADMIN)) \
 		return -EPERM; \
-	err = kstrtol(buf, 0, &value); \
+	err = kstrtoull(buf, 0, &value); \
 	if (err) \
 		return err; \
 	if (value) \
-		state->disable = 1; \
+		state_usage->_name = 1; \
 	else \
-		state->disable = 0; \
+		state_usage->_name = 0; \
 	return size; \
 }
 
@@ -273,8 +275,8 @@ define_show_state_ull_function(usage)
 define_show_state_ull_function(time)
 define_show_state_str_function(name)
 define_show_state_str_function(desc)
-define_show_state_function(disable)
-define_store_state_function(disable)
+define_show_state_ull_function(disable)
+define_store_state_ull_function(disable)
 
 define_one_state_ro(name, show_state_name);
 define_one_state_ro(desc, show_state_desc);
@@ -318,10 +320,11 @@ static ssize_t cpuidle_state_store(struct kobject *kobj,
 {
 	int ret = -EIO;
 	struct cpuidle_state *state = kobj_to_state(kobj);
+	struct cpuidle_state_usage *state_usage = kobj_to_state_usage(kobj);
 	struct cpuidle_state_attr *cattr = attr_to_stateattr(attr);
 
 	if (cattr->store)
-		ret = cattr->store(state, buf, size);
+		ret = cattr->store(state, state_usage, buf, size);
 
 	return ret;
 }
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 1092a770482..7d74d092aa8 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -298,7 +298,7 @@ config CRYPTO_DEV_TEGRA_AES
 	  will be called tegra-aes.
 
 config CRYPTO_DEV_NX
-	tristate "Support for Power7+ in-Nest cryptographic accleration"
+	tristate "Support for Power7+ in-Nest cryptographic acceleration"
 	depends on PPC64 && IBMVIO
 	select CRYPTO_AES
 	select CRYPTO_CBC
@@ -325,4 +325,58 @@ if CRYPTO_DEV_UX500
 	source "drivers/crypto/ux500/Kconfig"
 endif # if CRYPTO_DEV_UX500
 
+config CRYPTO_DEV_BFIN_CRC
+	tristate "Support for Blackfin CRC hardware"
+	depends on BF60x
+	help
+	  Newer Blackfin processors have CRC hardware. Select this if you
+	  want to use the Blackfin CRC module.
+
+config CRYPTO_DEV_ATMEL_AES
+	tristate "Support for Atmel AES hw accelerator"
+	depends on ARCH_AT91
+	select CRYPTO_CBC
+	select CRYPTO_ECB
+	select CRYPTO_AES
+	select CRYPTO_ALGAPI
+	select CRYPTO_BLKCIPHER
+	select CONFIG_AT_HDMAC
+	help
+	  Some Atmel processors have AES hw accelerator.
+	  Select this if you want to use the Atmel module for
+	  AES algorithms.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called atmel-aes.
+
+config CRYPTO_DEV_ATMEL_TDES
+	tristate "Support for Atmel DES/TDES hw accelerator"
+	depends on ARCH_AT91
+	select CRYPTO_DES
+	select CRYPTO_CBC
+	select CRYPTO_ECB
+	select CRYPTO_ALGAPI
+	select CRYPTO_BLKCIPHER
+	help
+	  Some Atmel processors have DES/TDES hw accelerator.
+	  Select this if you want to use the Atmel module for
+	  DES/TDES algorithms.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called atmel-tdes.
+
+config CRYPTO_DEV_ATMEL_SHA
+	tristate "Support for Atmel SHA1/SHA256 hw accelerator"
+	depends on ARCH_AT91
+	select CRYPTO_SHA1
+	select CRYPTO_SHA256
+	select CRYPTO_ALGAPI
+	help
+	  Some Atmel processors have SHA1/SHA256 hw accelerator.
+	  Select this if you want to use the Atmel module for
+	  SHA1/SHA256 algorithms.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called atmel-sha.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 01390325d72..880a47b0b02 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -14,4 +14,9 @@ obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_S5P) += s5p-sss.o
 obj-$(CONFIG_CRYPTO_DEV_TEGRA_AES) += tegra-aes.o
-obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
\ No newline at end of file
+obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
+obj-$(CONFIG_CRYPTO_DEV_BFIN_CRC) += bfin_crc.o
+obj-$(CONFIG_CRYPTO_DEV_NX) += nx/
+obj-$(CONFIG_CRYPTO_DEV_ATMEL_AES) += atmel-aes.o
+obj-$(CONFIG_CRYPTO_DEV_ATMEL_TDES) += atmel-tdes.o
+obj-$(CONFIG_CRYPTO_DEV_ATMEL_SHA) += atmel-sha.o
diff --git a/drivers/crypto/atmel-aes-regs.h b/drivers/crypto/atmel-aes-regs.h
new file mode 100644
index 00000000000..2786bb1a5aa
--- /dev/null
+++ b/drivers/crypto/atmel-aes-regs.h
@@ -0,0 +1,62 @@
+#ifndef __ATMEL_AES_REGS_H__
+#define __ATMEL_AES_REGS_H__
+
+#define AES_CR			0x00
+#define AES_CR_START		(1 << 0)
+#define AES_CR_SWRST		(1 << 8)
+#define AES_CR_LOADSEED		(1 << 16)
+
+#define	AES_MR			0x04
+#define AES_MR_CYPHER_DEC		(0 << 0)
+#define AES_MR_CYPHER_ENC		(1 << 0)
+#define	AES_MR_DUALBUFF			(1 << 3)
+#define AES_MR_PROCDLY_MASK		(0xF << 4)
+#define AES_MR_PROCDLY_OFFSET	4
+#define AES_MR_SMOD_MASK		(0x3 << 8)
+#define AES_MR_SMOD_MANUAL		(0x0 << 8)
+#define AES_MR_SMOD_AUTO		(0x1 << 8)
+#define AES_MR_SMOD_IDATAR0		(0x2 << 8)
+#define	AES_MR_KEYSIZE_MASK		(0x3 << 10)
+#define	AES_MR_KEYSIZE_128		(0x0 << 10)
+#define	AES_MR_KEYSIZE_192		(0x1 << 10)
+#define	AES_MR_KEYSIZE_256		(0x2 << 10)
+#define AES_MR_OPMOD_MASK		(0x7 << 12)
+#define AES_MR_OPMOD_ECB		(0x0 << 12)
+#define AES_MR_OPMOD_CBC		(0x1 << 12)
+#define AES_MR_OPMOD_OFB		(0x2 << 12)
+#define AES_MR_OPMOD_CFB		(0x3 << 12)
+#define AES_MR_OPMOD_CTR		(0x4 << 12)
+#define AES_MR_LOD				(0x1 << 15)
+#define AES_MR_CFBS_MASK		(0x7 << 16)
+#define AES_MR_CFBS_128b		(0x0 << 16)
+#define AES_MR_CFBS_64b			(0x1 << 16)
+#define AES_MR_CFBS_32b			(0x2 << 16)
+#define AES_MR_CFBS_16b			(0x3 << 16)
+#define AES_MR_CFBS_8b			(0x4 << 16)
+#define AES_MR_CKEY_MASK		(0xF << 20)
+#define AES_MR_CKEY_OFFSET		20
+#define AES_MR_CMTYP_MASK		(0x1F << 24)
+#define AES_MR_CMTYP_OFFSET		24
+
+#define	AES_IER		0x10
+#define	AES_IDR		0x14
+#define	AES_IMR		0x18
+#define	AES_ISR		0x1C
+#define AES_INT_DATARDY		(1 << 0)
+#define AES_INT_URAD		(1 << 8)
+#define AES_ISR_URAT_MASK	(0xF << 12)
+#define AES_ISR_URAT_IDR_WR_PROC	(0x0 << 12)
+#define AES_ISR_URAT_ODR_RD_PROC	(0x1 << 12)
+#define AES_ISR_URAT_MR_WR_PROC		(0x2 << 12)
+#define AES_ISR_URAT_ODR_RD_SUBK	(0x3 << 12)
+#define AES_ISR_URAT_MR_WR_SUBK		(0x4 << 12)
+#define AES_ISR_URAT_WOR_RD			(0x5 << 12)
+
+#define AES_KEYWR(x)	(0x20 + ((x) * 0x04))
+#define AES_IDATAR(x)	(0x40 + ((x) * 0x04))
+#define AES_ODATAR(x)	(0x50 + ((x) * 0x04))
+#define AES_IVR(x)		(0x60 + ((x) * 0x04))
+
+#define AES_HW_VERSION	0xFC
+
+#endif /* __ATMEL_AES_REGS_H__ */
diff --git a/drivers/crypto/atmel-aes.c b/drivers/crypto/atmel-aes.c
new file mode 100644
index 00000000000..6bb20fffbf4
--- /dev/null
+++ b/drivers/crypto/atmel-aes.c
@@ -0,0 +1,1206 @@
+/*
+ * Cryptographic API.
+ *
+ * Support for ATMEL AES HW acceleration.
+ *
+ * Copyright (c) 2012 Eukréa Electromatique - ATMEL
+ * Author: Nicolas Royer <nicolas@eukrea.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.
+ *
+ * Some ideas are from omap-aes.c driver.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/hw_random.h>
+#include <linux/platform_device.h>
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/crypto.h>
+#include <linux/cryptohash.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+#include <crypto/aes.h>
+#include <crypto/hash.h>
+#include <crypto/internal/hash.h>
+#include <linux/platform_data/atmel-aes.h>
+#include "atmel-aes-regs.h"
+
+#define CFB8_BLOCK_SIZE		1
+#define CFB16_BLOCK_SIZE	2
+#define CFB32_BLOCK_SIZE	4
+#define CFB64_BLOCK_SIZE	8
+
+/* AES flags */
+#define AES_FLAGS_MODE_MASK	0x01ff
+#define AES_FLAGS_ENCRYPT	BIT(0)
+#define AES_FLAGS_CBC		BIT(1)
+#define AES_FLAGS_CFB		BIT(2)
+#define AES_FLAGS_CFB8		BIT(3)
+#define AES_FLAGS_CFB16		BIT(4)
+#define AES_FLAGS_CFB32		BIT(5)
+#define AES_FLAGS_CFB64		BIT(6)
+#define AES_FLAGS_OFB		BIT(7)
+#define AES_FLAGS_CTR		BIT(8)
+
+#define AES_FLAGS_INIT		BIT(16)
+#define AES_FLAGS_DMA		BIT(17)
+#define AES_FLAGS_BUSY		BIT(18)
+
+#define AES_FLAGS_DUALBUFF	BIT(24)
+
+#define ATMEL_AES_QUEUE_LENGTH	1
+#define ATMEL_AES_CACHE_SIZE	0
+
+#define ATMEL_AES_DMA_THRESHOLD		16
+
+
+struct atmel_aes_dev;
+
+struct atmel_aes_ctx {
+	struct atmel_aes_dev *dd;
+
+	int		keylen;
+	u32		key[AES_KEYSIZE_256 / sizeof(u32)];
+};
+
+struct atmel_aes_reqctx {
+	unsigned long mode;
+};
+
+struct atmel_aes_dma {
+	struct dma_chan			*chan;
+	struct dma_slave_config dma_conf;
+};
+
+struct atmel_aes_dev {
+	struct list_head	list;
+	unsigned long		phys_base;
+	void __iomem		*io_base;
+
+	struct atmel_aes_ctx	*ctx;
+	struct device		*dev;
+	struct clk		*iclk;
+	int	irq;
+
+	unsigned long		flags;
+	int	err;
+
+	spinlock_t		lock;
+	struct crypto_queue	queue;
+
+	struct tasklet_struct	done_task;
+	struct tasklet_struct	queue_task;
+
+	struct ablkcipher_request	*req;
+	size_t	total;
+
+	struct scatterlist	*in_sg;
+	unsigned int		nb_in_sg;
+
+	struct scatterlist	*out_sg;
+	unsigned int		nb_out_sg;
+
+	size_t	bufcnt;
+
+	u8	buf_in[ATMEL_AES_DMA_THRESHOLD] __aligned(sizeof(u32));
+	int	dma_in;
+	struct atmel_aes_dma	dma_lch_in;
+
+	u8	buf_out[ATMEL_AES_DMA_THRESHOLD] __aligned(sizeof(u32));
+	int	dma_out;
+	struct atmel_aes_dma	dma_lch_out;
+
+	u32	hw_version;
+};
+
+struct atmel_aes_drv {
+	struct list_head	dev_list;
+	spinlock_t		lock;
+};
+
+static struct atmel_aes_drv atmel_aes = {
+	.dev_list = LIST_HEAD_INIT(atmel_aes.dev_list),
+	.lock = __SPIN_LOCK_UNLOCKED(atmel_aes.lock),
+};
+
+static int atmel_aes_sg_length(struct ablkcipher_request *req,
+			struct scatterlist *sg)
+{
+	unsigned int total = req->nbytes;
+	int sg_nb;
+	unsigned int len;
+	struct scatterlist *sg_list;
+
+	sg_nb = 0;
+	sg_list = sg;
+	total = req->nbytes;
+
+	while (total) {
+		len = min(sg_list->length, total);
+
+		sg_nb++;
+		total -= len;
+
+		sg_list = sg_next(sg_list);
+		if (!sg_list)
+			total = 0;
+	}
+
+	return sg_nb;
+}
+
+static inline u32 atmel_aes_read(struct atmel_aes_dev *dd, u32 offset)
+{
+	return readl_relaxed(dd->io_base + offset);
+}
+
+static inline void atmel_aes_write(struct atmel_aes_dev *dd,
+					u32 offset, u32 value)
+{
+	writel_relaxed(value, dd->io_base + offset);
+}
+
+static void atmel_aes_read_n(struct atmel_aes_dev *dd, u32 offset,
+					u32 *value, int count)
+{
+	for (; count--; value++, offset += 4)
+		*value = atmel_aes_read(dd, offset);
+}
+
+static void atmel_aes_write_n(struct atmel_aes_dev *dd, u32 offset,
+					u32 *value, int count)
+{
+	for (; count--; value++, offset += 4)
+		atmel_aes_write(dd, offset, *value);
+}
+
+static void atmel_aes_dualbuff_test(struct atmel_aes_dev *dd)
+{
+	atmel_aes_write(dd, AES_MR, AES_MR_DUALBUFF);
+
+	if (atmel_aes_read(dd, AES_MR) & AES_MR_DUALBUFF)
+		dd->flags |= AES_FLAGS_DUALBUFF;
+}
+
+static struct atmel_aes_dev *atmel_aes_find_dev(struct atmel_aes_ctx *ctx)
+{
+	struct atmel_aes_dev *aes_dd = NULL;
+	struct atmel_aes_dev *tmp;
+
+	spin_lock_bh(&atmel_aes.lock);
+	if (!ctx->dd) {
+		list_for_each_entry(tmp, &atmel_aes.dev_list, list) {
+			aes_dd = tmp;
+			break;
+		}
+		ctx->dd = aes_dd;
+	} else {
+		aes_dd = ctx->dd;
+	}
+
+	spin_unlock_bh(&atmel_aes.lock);
+
+	return aes_dd;
+}
+
+static int atmel_aes_hw_init(struct atmel_aes_dev *dd)
+{
+	clk_prepare_enable(dd->iclk);
+
+	if (!(dd->flags & AES_FLAGS_INIT)) {
+		atmel_aes_write(dd, AES_CR, AES_CR_SWRST);
+		atmel_aes_dualbuff_test(dd);
+		dd->flags |= AES_FLAGS_INIT;
+		dd->err = 0;
+	}
+
+	return 0;
+}
+
+static void atmel_aes_hw_version_init(struct atmel_aes_dev *dd)
+{
+	atmel_aes_hw_init(dd);
+
+	dd->hw_version = atmel_aes_read(dd, AES_HW_VERSION);
+
+	clk_disable_unprepare(dd->iclk);
+}
+
+static void atmel_aes_finish_req(struct atmel_aes_dev *dd, int err)
+{
+	struct ablkcipher_request *req = dd->req;
+
+	clk_disable_unprepare(dd->iclk);
+	dd->flags &= ~AES_FLAGS_BUSY;
+
+	req->base.complete(&req->base, err);
+}
+
+static void atmel_aes_dma_callback(void *data)
+{
+	struct atmel_aes_dev *dd = data;
+
+	/* dma_lch_out - completed */
+	tasklet_schedule(&dd->done_task);
+}
+
+static int atmel_aes_crypt_dma(struct atmel_aes_dev *dd)
+{
+	struct dma_async_tx_descriptor	*in_desc, *out_desc;
+	int nb_dma_sg_in, nb_dma_sg_out;
+
+	dd->nb_in_sg = atmel_aes_sg_length(dd->req, dd->in_sg);
+	if (!dd->nb_in_sg)
+		goto exit_err;
+
+	nb_dma_sg_in = dma_map_sg(dd->dev, dd->in_sg, dd->nb_in_sg,
+			DMA_TO_DEVICE);
+	if (!nb_dma_sg_in)
+		goto exit_err;
+
+	in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, dd->in_sg,
+				nb_dma_sg_in, DMA_MEM_TO_DEV,
+				DMA_PREP_INTERRUPT  |  DMA_CTRL_ACK);
+
+	if (!in_desc)
+		goto unmap_in;
+
+	/* callback not needed */
+
+	dd->nb_out_sg = atmel_aes_sg_length(dd->req, dd->out_sg);
+	if (!dd->nb_out_sg)
+		goto unmap_in;
+
+	nb_dma_sg_out = dma_map_sg(dd->dev, dd->out_sg, dd->nb_out_sg,
+			DMA_FROM_DEVICE);
+	if (!nb_dma_sg_out)
+		goto unmap_out;
+
+	out_desc = dmaengine_prep_slave_sg(dd->dma_lch_out.chan, dd->out_sg,
+				nb_dma_sg_out, DMA_DEV_TO_MEM,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+
+	if (!out_desc)
+		goto unmap_out;
+
+	out_desc->callback = atmel_aes_dma_callback;
+	out_desc->callback_param = dd;
+
+	dd->total -= dd->req->nbytes;
+
+	dmaengine_submit(out_desc);
+	dma_async_issue_pending(dd->dma_lch_out.chan);
+
+	dmaengine_submit(in_desc);
+	dma_async_issue_pending(dd->dma_lch_in.chan);
+
+	return 0;
+
+unmap_out:
+	dma_unmap_sg(dd->dev, dd->out_sg, dd->nb_out_sg,
+		DMA_FROM_DEVICE);
+unmap_in:
+	dma_unmap_sg(dd->dev, dd->in_sg, dd->nb_in_sg,
+		DMA_TO_DEVICE);
+exit_err:
+	return -EINVAL;
+}
+
+static int atmel_aes_crypt_cpu_start(struct atmel_aes_dev *dd)
+{
+	dd->flags &= ~AES_FLAGS_DMA;
+
+	/* use cache buffers */
+	dd->nb_in_sg = atmel_aes_sg_length(dd->req, dd->in_sg);
+	if (!dd->nb_in_sg)
+		return -EINVAL;
+
+	dd->nb_out_sg = atmel_aes_sg_length(dd->req, dd->out_sg);
+	if (!dd->nb_in_sg)
+		return -EINVAL;
+
+	dd->bufcnt = sg_copy_to_buffer(dd->in_sg, dd->nb_in_sg,
+					dd->buf_in, dd->total);
+
+	if (!dd->bufcnt)
+		return -EINVAL;
+
+	dd->total -= dd->bufcnt;
+
+	atmel_aes_write(dd, AES_IER, AES_INT_DATARDY);
+	atmel_aes_write_n(dd, AES_IDATAR(0), (u32 *) dd->buf_in,
+				dd->bufcnt >> 2);
+
+	return 0;
+}
+
+static int atmel_aes_crypt_dma_start(struct atmel_aes_dev *dd)
+{
+	int err;
+
+	if (dd->flags & AES_FLAGS_CFB8) {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_1_BYTE;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_1_BYTE;
+	} else if (dd->flags & AES_FLAGS_CFB16) {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_2_BYTES;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_2_BYTES;
+	} else {
+		dd->dma_lch_in.dma_conf.dst_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		dd->dma_lch_out.dma_conf.src_addr_width =
+			DMA_SLAVE_BUSWIDTH_4_BYTES;
+	}
+
+	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
+	dmaengine_slave_config(dd->dma_lch_out.chan, &dd->dma_lch_out.dma_conf);
+
+	dd->flags |= AES_FLAGS_DMA;
+	err = atmel_aes_crypt_dma(dd);
+
+	return err;
+}
+
+static int atmel_aes_write_ctrl(struct atmel_aes_dev *dd)
+{
+	int err;
+	u32 valcr = 0, valmr = 0;
+
+	err = atmel_aes_hw_init(dd);
+
+	if (err)
+		return err;
+
+	/* MR register must be set before IV registers */
+	if (dd->ctx->keylen == AES_KEYSIZE_128)
+		valmr |= AES_MR_KEYSIZE_128;
+	else if (dd->ctx->keylen == AES_KEYSIZE_192)
+		valmr |= AES_MR_KEYSIZE_192;
+	else
+		valmr |= AES_MR_KEYSIZE_256;
+
+	if (dd->flags & AES_FLAGS_CBC) {
+		valmr |= AES_MR_OPMOD_CBC;
+	} else if (dd->flags & AES_FLAGS_CFB) {
+		valmr |= AES_MR_OPMOD_CFB;
+		if (dd->flags & AES_FLAGS_CFB8)
+			valmr |= AES_MR_CFBS_8b;
+		else if (dd->flags & AES_FLAGS_CFB16)
+			valmr |= AES_MR_CFBS_16b;
+		else if (dd->flags & AES_FLAGS_CFB32)
+			valmr |= AES_MR_CFBS_32b;
+		else if (dd->flags & AES_FLAGS_CFB64)
+			valmr |= AES_MR_CFBS_64b;
+	} else if (dd->flags & AES_FLAGS_OFB) {
+		valmr |= AES_MR_OPMOD_OFB;
+	} else if (dd->flags & AES_FLAGS_CTR) {
+		valmr |= AES_MR_OPMOD_CTR;
+	} else {
+		valmr |= AES_MR_OPMOD_ECB;
+	}
+
+	if (dd->flags & AES_FLAGS_ENCRYPT)
+		valmr |= AES_MR_CYPHER_ENC;
+
+	if (dd->total > ATMEL_AES_DMA_THRESHOLD) {
+		valmr |= AES_MR_SMOD_IDATAR0;
+		if (dd->flags & AES_FLAGS_DUALBUFF)
+			valmr |= AES_MR_DUALBUFF;
+	} else {
+		valmr |= AES_MR_SMOD_AUTO;
+	}
+
+	atmel_aes_write(dd, AES_CR, valcr);
+	atmel_aes_write(dd, AES_MR, valmr);
+
+	atmel_aes_write_n(dd, AES_KEYWR(0), dd->ctx->key,
+						dd->ctx->keylen >> 2);
+
+	if (((dd->flags & AES_FLAGS_CBC) || (dd->flags & AES_FLAGS_CFB) ||
+	   (dd->flags & AES_FLAGS_OFB) || (dd->flags & AES_FLAGS_CTR)) &&
+	   dd->req->info) {
+		atmel_aes_write_n(dd, AES_IVR(0), dd->req->info, 4);
+	}
+
+	return 0;
+}
+
+static int atmel_aes_handle_queue(struct atmel_aes_dev *dd,
+			       struct ablkcipher_request *req)
+{
+	struct crypto_async_request *async_req, *backlog;
+	struct atmel_aes_ctx *ctx;
+	struct atmel_aes_reqctx *rctx;
+	unsigned long flags;
+	int err, ret = 0;
+
+	spin_lock_irqsave(&dd->lock, flags);
+	if (req)
+		ret = ablkcipher_enqueue_request(&dd->queue, req);
+	if (dd->flags & AES_FLAGS_BUSY) {
+		spin_unlock_irqrestore(&dd->lock, flags);
+		return ret;
+	}
+	backlog = crypto_get_backlog(&dd->queue);
+	async_req = crypto_dequeue_request(&dd->queue);
+	if (async_req)
+		dd->flags |= AES_FLAGS_BUSY;
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+	if (!async_req)
+		return ret;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ablkcipher_request_cast(async_req);
+
+	/* assign new request to device */
+	dd->req = req;
+	dd->total = req->nbytes;
+	dd->in_sg = req->src;
+	dd->out_sg = req->dst;
+
+	rctx = ablkcipher_request_ctx(req);
+	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
+	rctx->mode &= AES_FLAGS_MODE_MASK;
+	dd->flags = (dd->flags & ~AES_FLAGS_MODE_MASK) | rctx->mode;
+	dd->ctx = ctx;
+	ctx->dd = dd;
+
+	err = atmel_aes_write_ctrl(dd);
+	if (!err) {
+		if (dd->total > ATMEL_AES_DMA_THRESHOLD)
+			err = atmel_aes_crypt_dma_start(dd);
+		else
+			err = atmel_aes_crypt_cpu_start(dd);
+	}
+	if (err) {
+		/* aes_task will not finish it, so do it here */
+		atmel_aes_finish_req(dd, err);
+		tasklet_schedule(&dd->queue_task);
+	}
+
+	return ret;
+}
+
+static int atmel_aes_crypt_dma_stop(struct atmel_aes_dev *dd)
+{
+	int err = -EINVAL;
+
+	if (dd->flags & AES_FLAGS_DMA) {
+		dma_unmap_sg(dd->dev, dd->out_sg,
+			dd->nb_out_sg, DMA_FROM_DEVICE);
+		dma_unmap_sg(dd->dev, dd->in_sg,
+			dd->nb_in_sg, DMA_TO_DEVICE);
+		err = 0;
+	}
+
+	return err;
+}
+
+static int atmel_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
+{
+	struct atmel_aes_ctx *ctx = crypto_ablkcipher_ctx(
+			crypto_ablkcipher_reqtfm(req));
+	struct atmel_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	struct atmel_aes_dev *dd;
+
+	if (!IS_ALIGNED(req->nbytes, AES_BLOCK_SIZE)) {
+		pr_err("request size is not exact amount of AES blocks\n");
+		return -EINVAL;
+	}
+
+	dd = atmel_aes_find_dev(ctx);
+	if (!dd)
+		return -ENODEV;
+
+	rctx->mode = mode;
+
+	return atmel_aes_handle_queue(dd, req);
+}
+
+static bool atmel_aes_filter(struct dma_chan *chan, void *slave)
+{
+	struct at_dma_slave	*sl = slave;
+
+	if (sl && sl->dma_dev == chan->device->dev) {
+		chan->private = sl;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static int atmel_aes_dma_init(struct atmel_aes_dev *dd)
+{
+	int err = -ENOMEM;
+	struct aes_platform_data	*pdata;
+	dma_cap_mask_t mask_in, mask_out;
+
+	pdata = dd->dev->platform_data;
+
+	if (pdata && pdata->dma_slave->txdata.dma_dev &&
+		pdata->dma_slave->rxdata.dma_dev) {
+
+		/* Try to grab 2 DMA channels */
+		dma_cap_zero(mask_in);
+		dma_cap_set(DMA_SLAVE, mask_in);
+
+		dd->dma_lch_in.chan = dma_request_channel(mask_in,
+				atmel_aes_filter, &pdata->dma_slave->rxdata);
+		if (!dd->dma_lch_in.chan)
+			goto err_dma_in;
+
+		dd->dma_lch_in.dma_conf.direction = DMA_MEM_TO_DEV;
+		dd->dma_lch_in.dma_conf.dst_addr = dd->phys_base +
+			AES_IDATAR(0);
+		dd->dma_lch_in.dma_conf.src_maxburst = 1;
+		dd->dma_lch_in.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_in.dma_conf.device_fc = false;
+
+		dma_cap_zero(mask_out);
+		dma_cap_set(DMA_SLAVE, mask_out);
+		dd->dma_lch_out.chan = dma_request_channel(mask_out,
+				atmel_aes_filter, &pdata->dma_slave->txdata);
+		if (!dd->dma_lch_out.chan)
+			goto err_dma_out;
+
+		dd->dma_lch_out.dma_conf.direction = DMA_DEV_TO_MEM;
+		dd->dma_lch_out.dma_conf.src_addr = dd->phys_base +
+			AES_ODATAR(0);
+		dd->dma_lch_out.dma_conf.src_maxburst = 1;
+		dd->dma_lch_out.dma_conf.dst_maxburst = 1;
+		dd->dma_lch_out.dma_conf.device_fc = false;
+
+		return 0;
+	} else {
+		return -ENODEV;
+	}
+
+err_dma_out:
+	dma_release_channel(dd->dma_lch_in.chan);
+err_dma_in:
+	return err;
+}
+
+static void atmel_aes_dma_cleanup(struct atmel_aes_dev *dd)
+{
+	dma_release_channel(dd->dma_lch_in.chan);
+	dma_release_channel(dd->dma_lch_out.chan);
+}
+
+static int atmel_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+			   unsigned int keylen)
+{
+	struct atmel_aes_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+
+	if (keylen != AES_KEYSIZE_128 && keylen != AES_KEYSIZE_192 &&
+		   keylen != AES_KEYSIZE_256) {
+		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return 0;
+}
+
+static int atmel_aes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT);
+}
+
+static int atmel_aes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		0);
+}
+
+static int atmel_aes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CBC);
+}
+
+static int atmel_aes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CBC);
+}
+
+static int atmel_aes_ofb_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_OFB);
+}
+
+static int atmel_aes_ofb_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_OFB);
+}
+
+static int atmel_aes_cfb_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB);
+}
+
+static int atmel_aes_cfb_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CFB);
+}
+
+static int atmel_aes_cfb64_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB64);
+}
+
+static int atmel_aes_cfb64_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CFB | AES_FLAGS_CFB64);
+}
+
+static int atmel_aes_cfb32_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB32);
+}
+
+static int atmel_aes_cfb32_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CFB | AES_FLAGS_CFB32);
+}
+
+static int atmel_aes_cfb16_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB16);
+}
+
+static int atmel_aes_cfb16_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CFB | AES_FLAGS_CFB16);
+}
+
+static int atmel_aes_cfb8_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT |	AES_FLAGS_CFB | AES_FLAGS_CFB8);
+}
+
+static int atmel_aes_cfb8_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CFB | AES_FLAGS_CFB8);
+}
+
+static int atmel_aes_ctr_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_ENCRYPT | AES_FLAGS_CTR);
+}
+
+static int atmel_aes_ctr_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_aes_crypt(req,
+		AES_FLAGS_CTR);
+}
+
+static int atmel_aes_cra_init(struct crypto_tfm *tfm)
+{
+	tfm->crt_ablkcipher.reqsize = sizeof(struct atmel_aes_reqctx);
+
+	return 0;
+}
+
+static void atmel_aes_cra_exit(struct crypto_tfm *tfm)
+{
+}
+
+static struct crypto_alg aes_algs[] = {
+{
+	.cra_name		= "ecb(aes)",
+	.cra_driver_name	= "atmel-ecb-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_ecb_encrypt,
+		.decrypt	= atmel_aes_ecb_decrypt,
+	}
+},
+{
+	.cra_name		= "cbc(aes)",
+	.cra_driver_name	= "atmel-cbc-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_cbc_encrypt,
+		.decrypt	= atmel_aes_cbc_decrypt,
+	}
+},
+{
+	.cra_name		= "ofb(aes)",
+	.cra_driver_name	= "atmel-ofb-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_ofb_encrypt,
+		.decrypt	= atmel_aes_ofb_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb(aes)",
+	.cra_driver_name	= "atmel-cfb-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_cfb_encrypt,
+		.decrypt	= atmel_aes_cfb_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb32(aes)",
+	.cra_driver_name	= "atmel-cfb32-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB32_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_cfb32_encrypt,
+		.decrypt	= atmel_aes_cfb32_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb16(aes)",
+	.cra_driver_name	= "atmel-cfb16-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB16_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_cfb16_encrypt,
+		.decrypt	= atmel_aes_cfb16_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb8(aes)",
+	.cra_driver_name	= "atmel-cfb8-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB64_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_cfb8_encrypt,
+		.decrypt	= atmel_aes_cfb8_decrypt,
+	}
+},
+{
+	.cra_name		= "ctr(aes)",
+	.cra_driver_name	= "atmel-ctr-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= AES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_ctr_encrypt,
+		.decrypt	= atmel_aes_ctr_decrypt,
+	}
+},
+};
+
+static struct crypto_alg aes_cfb64_alg[] = {
+{
+	.cra_name		= "cfb64(aes)",
+	.cra_driver_name	= "atmel-cfb64-aes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB64_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_alignmask		= 0x0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_aes_cra_init,
+	.cra_exit		= atmel_aes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= AES_MIN_KEY_SIZE,
+		.max_keysize	= AES_MAX_KEY_SIZE,
+		.ivsize		= AES_BLOCK_SIZE,
+		.setkey		= atmel_aes_setkey,
+		.encrypt	= atmel_aes_cfb64_encrypt,
+		.decrypt	= atmel_aes_cfb64_decrypt,
+	}
+},
+};
+
+static void atmel_aes_queue_task(unsigned long data)
+{
+	struct atmel_aes_dev *dd = (struct atmel_aes_dev *)data;
+
+	atmel_aes_handle_queue(dd, NULL);
+}
+
+static void atmel_aes_done_task(unsigned long data)
+{
+	struct atmel_aes_dev *dd = (struct atmel_aes_dev *) data;
+	int err;
+
+	if (!(dd->flags & AES_FLAGS_DMA)) {
+		atmel_aes_read_n(dd, AES_ODATAR(0), (u32 *) dd->buf_out,
+				dd->bufcnt >> 2);
+
+		if (sg_copy_from_buffer(dd->out_sg, dd->nb_out_sg,
+			dd->buf_out, dd->bufcnt))
+			err = 0;
+		else
+			err = -EINVAL;
+
+		goto cpu_end;
+	}
+
+	err = atmel_aes_crypt_dma_stop(dd);
+
+	err = dd->err ? : err;
+
+	if (dd->total && !err) {
+		err = atmel_aes_crypt_dma_start(dd);
+		if (!err)
+			return; /* DMA started. Not fininishing. */
+	}
+
+cpu_end:
+	atmel_aes_finish_req(dd, err);
+	atmel_aes_handle_queue(dd, NULL);
+}
+
+static irqreturn_t atmel_aes_irq(int irq, void *dev_id)
+{
+	struct atmel_aes_dev *aes_dd = dev_id;
+	u32 reg;
+
+	reg = atmel_aes_read(aes_dd, AES_ISR);
+	if (reg & atmel_aes_read(aes_dd, AES_IMR)) {
+		atmel_aes_write(aes_dd, AES_IDR, reg);
+		if (AES_FLAGS_BUSY & aes_dd->flags)
+			tasklet_schedule(&aes_dd->done_task);
+		else
+			dev_warn(aes_dd->dev, "AES interrupt when no active requests.\n");
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static void atmel_aes_unregister_algs(struct atmel_aes_dev *dd)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
+		crypto_unregister_alg(&aes_algs[i]);
+	if (dd->hw_version >= 0x130)
+		crypto_unregister_alg(&aes_cfb64_alg[0]);
+}
+
+static int atmel_aes_register_algs(struct atmel_aes_dev *dd)
+{
+	int err, i, j;
+
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++) {
+		INIT_LIST_HEAD(&aes_algs[i].cra_list);
+		err = crypto_register_alg(&aes_algs[i]);
+		if (err)
+			goto err_aes_algs;
+	}
+
+	atmel_aes_hw_version_init(dd);
+
+	if (dd->hw_version >= 0x130) {
+		INIT_LIST_HEAD(&aes_cfb64_alg[0].cra_list);
+		err = crypto_register_alg(&aes_cfb64_alg[0]);
+		if (err)
+			goto err_aes_cfb64_alg;
+	}
+
+	return 0;
+
+err_aes_cfb64_alg:
+	i = ARRAY_SIZE(aes_algs);
+err_aes_algs:
+	for (j = 0; j < i; j++)
+		crypto_unregister_alg(&aes_algs[j]);
+
+	return err;
+}
+
+static int __devinit atmel_aes_probe(struct platform_device *pdev)
+{
+	struct atmel_aes_dev *aes_dd;
+	struct aes_platform_data	*pdata;
+	struct device *dev = &pdev->dev;
+	struct resource *aes_res;
+	unsigned long aes_phys_size;
+	int err;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		err = -ENXIO;
+		goto aes_dd_err;
+	}
+
+	aes_dd = kzalloc(sizeof(struct atmel_aes_dev), GFP_KERNEL);
+	if (aes_dd == NULL) {
+		dev_err(dev, "unable to alloc data struct.\n");
+		err = -ENOMEM;
+		goto aes_dd_err;
+	}
+
+	aes_dd->dev = dev;
+
+	platform_set_drvdata(pdev, aes_dd);
+
+	INIT_LIST_HEAD(&aes_dd->list);
+
+	tasklet_init(&aes_dd->done_task, atmel_aes_done_task,
+					(unsigned long)aes_dd);
+	tasklet_init(&aes_dd->queue_task, atmel_aes_queue_task,
+					(unsigned long)aes_dd);
+
+	crypto_init_queue(&aes_dd->queue, ATMEL_AES_QUEUE_LENGTH);
+
+	aes_dd->irq = -1;
+
+	/* Get the base address */
+	aes_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!aes_res) {
+		dev_err(dev, "no MEM resource info\n");
+		err = -ENODEV;
+		goto res_err;
+	}
+	aes_dd->phys_base = aes_res->start;
+	aes_phys_size = resource_size(aes_res);
+
+	/* Get the IRQ */
+	aes_dd->irq = platform_get_irq(pdev,  0);
+	if (aes_dd->irq < 0) {
+		dev_err(dev, "no IRQ resource info\n");
+		err = aes_dd->irq;
+		goto aes_irq_err;
+	}
+
+	err = request_irq(aes_dd->irq, atmel_aes_irq, IRQF_SHARED, "atmel-aes",
+						aes_dd);
+	if (err) {
+		dev_err(dev, "unable to request aes irq.\n");
+		goto aes_irq_err;
+	}
+
+	/* Initializing the clock */
+	aes_dd->iclk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(aes_dd->iclk)) {
+		dev_err(dev, "clock intialization failed.\n");
+		err = PTR_ERR(aes_dd->iclk);
+		goto clk_err;
+	}
+
+	aes_dd->io_base = ioremap(aes_dd->phys_base, aes_phys_size);
+	if (!aes_dd->io_base) {
+		dev_err(dev, "can't ioremap\n");
+		err = -ENOMEM;
+		goto aes_io_err;
+	}
+
+	err = atmel_aes_dma_init(aes_dd);
+	if (err)
+		goto err_aes_dma;
+
+	spin_lock(&atmel_aes.lock);
+	list_add_tail(&aes_dd->list, &atmel_aes.dev_list);
+	spin_unlock(&atmel_aes.lock);
+
+	err = atmel_aes_register_algs(aes_dd);
+	if (err)
+		goto err_algs;
+
+	dev_info(dev, "Atmel AES\n");
+
+	return 0;
+
+err_algs:
+	spin_lock(&atmel_aes.lock);
+	list_del(&aes_dd->list);
+	spin_unlock(&atmel_aes.lock);
+	atmel_aes_dma_cleanup(aes_dd);
+err_aes_dma:
+	iounmap(aes_dd->io_base);
+aes_io_err:
+	clk_put(aes_dd->iclk);
+clk_err:
+	free_irq(aes_dd->irq, aes_dd);
+aes_irq_err:
+res_err:
+	tasklet_kill(&aes_dd->done_task);
+	tasklet_kill(&aes_dd->queue_task);
+	kfree(aes_dd);
+	aes_dd = NULL;
+aes_dd_err:
+	dev_err(dev, "initialization failed.\n");
+
+	return err;
+}
+
+static int __devexit atmel_aes_remove(struct platform_device *pdev)
+{
+	static struct atmel_aes_dev *aes_dd;
+
+	aes_dd = platform_get_drvdata(pdev);
+	if (!aes_dd)
+		return -ENODEV;
+	spin_lock(&atmel_aes.lock);
+	list_del(&aes_dd->list);
+	spin_unlock(&atmel_aes.lock);
+
+	atmel_aes_unregister_algs(aes_dd);
+
+	tasklet_kill(&aes_dd->done_task);
+	tasklet_kill(&aes_dd->queue_task);
+
+	atmel_aes_dma_cleanup(aes_dd);
+
+	iounmap(aes_dd->io_base);
+
+	clk_put(aes_dd->iclk);
+
+	if (aes_dd->irq > 0)
+		free_irq(aes_dd->irq, aes_dd);
+
+	kfree(aes_dd);
+	aes_dd = NULL;
+
+	return 0;
+}
+
+static struct platform_driver atmel_aes_driver = {
+	.probe		= atmel_aes_probe,
+	.remove		= __devexit_p(atmel_aes_remove),
+	.driver		= {
+		.name	= "atmel_aes",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(atmel_aes_driver);
+
+MODULE_DESCRIPTION("Atmel AES hw acceleration support.");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Nicolas Royer - Eukréa Electromatique");
diff --git a/drivers/crypto/atmel-sha-regs.h b/drivers/crypto/atmel-sha-regs.h
new file mode 100644
index 00000000000..dc53a20d7da
--- /dev/null
+++ b/drivers/crypto/atmel-sha-regs.h
@@ -0,0 +1,46 @@
+#ifndef __ATMEL_SHA_REGS_H__
+#define __ATMEL_SHA_REGS_H__
+
+#define SHA_REG_DIGEST(x)		(0x80 + ((x) * 0x04))
+#define SHA_REG_DIN(x)			(0x40 + ((x) * 0x04))
+
+#define SHA_CR				0x00
+#define SHA_CR_START			(1 << 0)
+#define SHA_CR_FIRST			(1 << 4)
+#define SHA_CR_SWRST			(1 << 8)
+
+#define SHA_MR				0x04
+#define SHA_MR_MODE_MASK		(0x3 << 0)
+#define SHA_MR_MODE_MANUAL		0x0
+#define SHA_MR_MODE_AUTO		0x1
+#define SHA_MR_MODE_PDC			0x2
+#define	SHA_MR_DUALBUFF			(1 << 3)
+#define SHA_MR_PROCDLY			(1 << 4)
+#define SHA_MR_ALGO_SHA1		(0 << 8)
+#define SHA_MR_ALGO_SHA256		(1 << 8)
+
+#define SHA_IER				0x10
+#define SHA_IDR				0x14
+#define SHA_IMR				0x18
+#define SHA_ISR				0x1C
+#define SHA_INT_DATARDY			(1 << 0)
+#define SHA_INT_ENDTX			(1 << 1)
+#define SHA_INT_TXBUFE			(1 << 2)
+#define SHA_INT_URAD			(1 << 8)
+#define SHA_ISR_URAT_MASK		(0x7 << 12)
+#define SHA_ISR_URAT_IDR		(0x0 << 12)
+#define SHA_ISR_URAT_ODR		(0x1 << 12)
+#define SHA_ISR_URAT_MR			(0x2 << 12)
+#define SHA_ISR_URAT_WO			(0x5 << 12)
+
+#define SHA_TPR				0x108
+#define SHA_TCR				0x10C
+#define SHA_TNPR			0x118
+#define SHA_TNCR			0x11C
+#define SHA_PTCR			0x120
+#define SHA_PTCR_TXTEN		(1 << 8)
+#define SHA_PTCR_TXTDIS		(1 << 9)
+#define SHA_PTSR			0x124
+#define SHA_PTSR_TXTEN		(1 << 8)
+
+#endif /* __ATMEL_SHA_REGS_H__ */
diff --git a/drivers/crypto/atmel-sha.c b/drivers/crypto/atmel-sha.c
new file mode 100644
index 00000000000..f938b9d79b6
--- /dev/null
+++ b/drivers/crypto/atmel-sha.c
@@ -0,0 +1,1112 @@
+/*
+ * Cryptographic API.
+ *
+ * Support for ATMEL SHA1/SHA256 HW acceleration.
+ *
+ * Copyright (c) 2012 Eukréa Electromatique - ATMEL
+ * Author: Nicolas Royer <nicolas@eukrea.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.
+ *
+ * Some ideas are from omap-sham.c drivers.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/hw_random.h>
+#include <linux/platform_device.h>
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/crypto.h>
+#include <linux/cryptohash.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+#include <crypto/sha.h>
+#include <crypto/hash.h>
+#include <crypto/internal/hash.h>
+#include "atmel-sha-regs.h"
+
+/* SHA flags */
+#define SHA_FLAGS_BUSY			BIT(0)
+#define	SHA_FLAGS_FINAL			BIT(1)
+#define SHA_FLAGS_DMA_ACTIVE	BIT(2)
+#define SHA_FLAGS_OUTPUT_READY	BIT(3)
+#define SHA_FLAGS_INIT			BIT(4)
+#define SHA_FLAGS_CPU			BIT(5)
+#define SHA_FLAGS_DMA_READY		BIT(6)
+
+#define SHA_FLAGS_FINUP		BIT(16)
+#define SHA_FLAGS_SG		BIT(17)
+#define SHA_FLAGS_SHA1		BIT(18)
+#define SHA_FLAGS_SHA256	BIT(19)
+#define SHA_FLAGS_ERROR		BIT(20)
+#define SHA_FLAGS_PAD		BIT(21)
+
+#define SHA_FLAGS_DUALBUFF	BIT(24)
+
+#define SHA_OP_UPDATE	1
+#define SHA_OP_FINAL	2
+
+#define SHA_BUFFER_LEN		PAGE_SIZE
+
+#define ATMEL_SHA_DMA_THRESHOLD		56
+
+
+struct atmel_sha_dev;
+
+struct atmel_sha_reqctx {
+	struct atmel_sha_dev	*dd;
+	unsigned long	flags;
+	unsigned long	op;
+
+	u8	digest[SHA256_DIGEST_SIZE] __aligned(sizeof(u32));
+	size_t	digcnt;
+	size_t	bufcnt;
+	size_t	buflen;
+	dma_addr_t	dma_addr;
+
+	/* walk state */
+	struct scatterlist	*sg;
+	unsigned int	offset;	/* offset in current sg */
+	unsigned int	total;	/* total request */
+
+	u8	buffer[0] __aligned(sizeof(u32));
+};
+
+struct atmel_sha_ctx {
+	struct atmel_sha_dev	*dd;
+
+	unsigned long		flags;
+
+	/* fallback stuff */
+	struct crypto_shash	*fallback;
+
+};
+
+#define ATMEL_SHA_QUEUE_LENGTH	1
+
+struct atmel_sha_dev {
+	struct list_head	list;
+	unsigned long		phys_base;
+	struct device		*dev;
+	struct clk			*iclk;
+	int					irq;
+	void __iomem		*io_base;
+
+	spinlock_t		lock;
+	int			err;
+	struct tasklet_struct	done_task;
+
+	unsigned long		flags;
+	struct crypto_queue	queue;
+	struct ahash_request	*req;
+};
+
+struct atmel_sha_drv {
+	struct list_head	dev_list;
+	spinlock_t		lock;
+};
+
+static struct atmel_sha_drv atmel_sha = {
+	.dev_list = LIST_HEAD_INIT(atmel_sha.dev_list),
+	.lock = __SPIN_LOCK_UNLOCKED(atmel_sha.lock),
+};
+
+static inline u32 atmel_sha_read(struct atmel_sha_dev *dd, u32 offset)
+{
+	return readl_relaxed(dd->io_base + offset);
+}
+
+static inline void atmel_sha_write(struct atmel_sha_dev *dd,
+					u32 offset, u32 value)
+{
+	writel_relaxed(value, dd->io_base + offset);
+}
+
+static void atmel_sha_dualbuff_test(struct atmel_sha_dev *dd)
+{
+	atmel_sha_write(dd, SHA_MR, SHA_MR_DUALBUFF);
+
+	if (atmel_sha_read(dd, SHA_MR) & SHA_MR_DUALBUFF)
+		dd->flags |= SHA_FLAGS_DUALBUFF;
+}
+
+static size_t atmel_sha_append_sg(struct atmel_sha_reqctx *ctx)
+{
+	size_t count;
+
+	while ((ctx->bufcnt < ctx->buflen) && ctx->total) {
+		count = min(ctx->sg->length - ctx->offset, ctx->total);
+		count = min(count, ctx->buflen - ctx->bufcnt);
+
+		if (count <= 0)
+			break;
+
+		scatterwalk_map_and_copy(ctx->buffer + ctx->bufcnt, ctx->sg,
+			ctx->offset, count, 0);
+
+		ctx->bufcnt += count;
+		ctx->offset += count;
+		ctx->total -= count;
+
+		if (ctx->offset == ctx->sg->length) {
+			ctx->sg = sg_next(ctx->sg);
+			if (ctx->sg)
+				ctx->offset = 0;
+			else
+				ctx->total = 0;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * The purpose of this padding is to ensure that the padded message
+ * is a multiple of 512 bits. The bit "1" is appended at the end of
+ * the message followed by "padlen-1" zero bits. Then a 64 bits block
+ * equals to the message length in bits is appended.
+ *
+ * padlen is calculated as followed:
+ *  - if message length < 56 bytes then padlen = 56 - message length
+ *  - else padlen = 64 + 56 - message length
+ */
+static void atmel_sha_fill_padding(struct atmel_sha_reqctx *ctx, int length)
+{
+	unsigned int index, padlen;
+	u64 bits;
+	u64 size;
+
+	bits = (ctx->bufcnt + ctx->digcnt + length) << 3;
+	size = cpu_to_be64(bits);
+
+	index = ctx->bufcnt & 0x3f;
+	padlen = (index < 56) ? (56 - index) : ((64+56) - index);
+	*(ctx->buffer + ctx->bufcnt) = 0x80;
+	memset(ctx->buffer + ctx->bufcnt + 1, 0, padlen-1);
+	memcpy(ctx->buffer + ctx->bufcnt + padlen, &size, 8);
+	ctx->bufcnt += padlen + 8;
+	ctx->flags |= SHA_FLAGS_PAD;
+}
+
+static int atmel_sha_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct atmel_sha_ctx *tctx = crypto_ahash_ctx(tfm);
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	struct atmel_sha_dev *dd = NULL;
+	struct atmel_sha_dev *tmp;
+
+	spin_lock_bh(&atmel_sha.lock);
+	if (!tctx->dd) {
+		list_for_each_entry(tmp, &atmel_sha.dev_list, list) {
+			dd = tmp;
+			break;
+		}
+		tctx->dd = dd;
+	} else {
+		dd = tctx->dd;
+	}
+
+	spin_unlock_bh(&atmel_sha.lock);
+
+	ctx->dd = dd;
+
+	ctx->flags = 0;
+
+	dev_dbg(dd->dev, "init: digest size: %d\n",
+		crypto_ahash_digestsize(tfm));
+
+	if (crypto_ahash_digestsize(tfm) == SHA1_DIGEST_SIZE)
+		ctx->flags |= SHA_FLAGS_SHA1;
+	else if (crypto_ahash_digestsize(tfm) == SHA256_DIGEST_SIZE)
+		ctx->flags |= SHA_FLAGS_SHA256;
+
+	ctx->bufcnt = 0;
+	ctx->digcnt = 0;
+	ctx->buflen = SHA_BUFFER_LEN;
+
+	return 0;
+}
+
+static void atmel_sha_write_ctrl(struct atmel_sha_dev *dd, int dma)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	u32 valcr = 0, valmr = SHA_MR_MODE_AUTO;
+
+	if (likely(dma)) {
+		atmel_sha_write(dd, SHA_IER, SHA_INT_TXBUFE);
+		valmr = SHA_MR_MODE_PDC;
+		if (dd->flags & SHA_FLAGS_DUALBUFF)
+			valmr = SHA_MR_DUALBUFF;
+	} else {
+		atmel_sha_write(dd, SHA_IER, SHA_INT_DATARDY);
+	}
+
+	if (ctx->flags & SHA_FLAGS_SHA256)
+		valmr |= SHA_MR_ALGO_SHA256;
+
+	/* Setting CR_FIRST only for the first iteration */
+	if (!ctx->digcnt)
+		valcr = SHA_CR_FIRST;
+
+	atmel_sha_write(dd, SHA_CR, valcr);
+	atmel_sha_write(dd, SHA_MR, valmr);
+}
+
+static int atmel_sha_xmit_cpu(struct atmel_sha_dev *dd, const u8 *buf,
+			      size_t length, int final)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	int count, len32;
+	const u32 *buffer = (const u32 *)buf;
+
+	dev_dbg(dd->dev, "xmit_cpu: digcnt: %d, length: %d, final: %d\n",
+						ctx->digcnt, length, final);
+
+	atmel_sha_write_ctrl(dd, 0);
+
+	/* should be non-zero before next lines to disable clocks later */
+	ctx->digcnt += length;
+
+	if (final)
+		dd->flags |= SHA_FLAGS_FINAL; /* catch last interrupt */
+
+	len32 = DIV_ROUND_UP(length, sizeof(u32));
+
+	dd->flags |= SHA_FLAGS_CPU;
+
+	for (count = 0; count < len32; count++)
+		atmel_sha_write(dd, SHA_REG_DIN(count), buffer[count]);
+
+	return -EINPROGRESS;
+}
+
+static int atmel_sha_xmit_pdc(struct atmel_sha_dev *dd, dma_addr_t dma_addr1,
+		size_t length1, dma_addr_t dma_addr2, size_t length2, int final)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	int len32;
+
+	dev_dbg(dd->dev, "xmit_pdc: digcnt: %d, length: %d, final: %d\n",
+						ctx->digcnt, length1, final);
+
+	len32 = DIV_ROUND_UP(length1, sizeof(u32));
+	atmel_sha_write(dd, SHA_PTCR, SHA_PTCR_TXTDIS);
+	atmel_sha_write(dd, SHA_TPR, dma_addr1);
+	atmel_sha_write(dd, SHA_TCR, len32);
+
+	len32 = DIV_ROUND_UP(length2, sizeof(u32));
+	atmel_sha_write(dd, SHA_TNPR, dma_addr2);
+	atmel_sha_write(dd, SHA_TNCR, len32);
+
+	atmel_sha_write_ctrl(dd, 1);
+
+	/* should be non-zero before next lines to disable clocks later */
+	ctx->digcnt += length1;
+
+	if (final)
+		dd->flags |= SHA_FLAGS_FINAL; /* catch last interrupt */
+
+	dd->flags |=  SHA_FLAGS_DMA_ACTIVE;
+
+	/* Start DMA transfer */
+	atmel_sha_write(dd, SHA_PTCR, SHA_PTCR_TXTEN);
+
+	return -EINPROGRESS;
+}
+
+static int atmel_sha_update_cpu(struct atmel_sha_dev *dd)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	int bufcnt;
+
+	atmel_sha_append_sg(ctx);
+	atmel_sha_fill_padding(ctx, 0);
+
+	bufcnt = ctx->bufcnt;
+	ctx->bufcnt = 0;
+
+	return atmel_sha_xmit_cpu(dd, ctx->buffer, bufcnt, 1);
+}
+
+static int atmel_sha_xmit_dma_map(struct atmel_sha_dev *dd,
+					struct atmel_sha_reqctx *ctx,
+					size_t length, int final)
+{
+	ctx->dma_addr = dma_map_single(dd->dev, ctx->buffer,
+				ctx->buflen + SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+	if (dma_mapping_error(dd->dev, ctx->dma_addr)) {
+		dev_err(dd->dev, "dma %u bytes error\n", ctx->buflen +
+				SHA1_BLOCK_SIZE);
+		return -EINVAL;
+	}
+
+	ctx->flags &= ~SHA_FLAGS_SG;
+
+	/* next call does not fail... so no unmap in the case of error */
+	return atmel_sha_xmit_pdc(dd, ctx->dma_addr, length, 0, 0, final);
+}
+
+static int atmel_sha_update_dma_slow(struct atmel_sha_dev *dd)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	unsigned int final;
+	size_t count;
+
+	atmel_sha_append_sg(ctx);
+
+	final = (ctx->flags & SHA_FLAGS_FINUP) && !ctx->total;
+
+	dev_dbg(dd->dev, "slow: bufcnt: %u, digcnt: %d, final: %d\n",
+					 ctx->bufcnt, ctx->digcnt, final);
+
+	if (final)
+		atmel_sha_fill_padding(ctx, 0);
+
+	if (final || (ctx->bufcnt == ctx->buflen && ctx->total)) {
+		count = ctx->bufcnt;
+		ctx->bufcnt = 0;
+		return atmel_sha_xmit_dma_map(dd, ctx, count, final);
+	}
+
+	return 0;
+}
+
+static int atmel_sha_update_dma_start(struct atmel_sha_dev *dd)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+	unsigned int length, final, tail;
+	struct scatterlist *sg;
+	unsigned int count;
+
+	if (!ctx->total)
+		return 0;
+
+	if (ctx->bufcnt || ctx->offset)
+		return atmel_sha_update_dma_slow(dd);
+
+	dev_dbg(dd->dev, "fast: digcnt: %d, bufcnt: %u, total: %u\n",
+			ctx->digcnt, ctx->bufcnt, ctx->total);
+
+	sg = ctx->sg;
+
+	if (!IS_ALIGNED(sg->offset, sizeof(u32)))
+		return atmel_sha_update_dma_slow(dd);
+
+	if (!sg_is_last(sg) && !IS_ALIGNED(sg->length, SHA1_BLOCK_SIZE))
+		/* size is not SHA1_BLOCK_SIZE aligned */
+		return atmel_sha_update_dma_slow(dd);
+
+	length = min(ctx->total, sg->length);
+
+	if (sg_is_last(sg)) {
+		if (!(ctx->flags & SHA_FLAGS_FINUP)) {
+			/* not last sg must be SHA1_BLOCK_SIZE aligned */
+			tail = length & (SHA1_BLOCK_SIZE - 1);
+			length -= tail;
+			if (length == 0) {
+				/* offset where to start slow */
+				ctx->offset = length;
+				return atmel_sha_update_dma_slow(dd);
+			}
+		}
+	}
+
+	ctx->total -= length;
+	ctx->offset = length; /* offset where to start slow */
+
+	final = (ctx->flags & SHA_FLAGS_FINUP) && !ctx->total;
+
+	/* Add padding */
+	if (final) {
+		tail = length & (SHA1_BLOCK_SIZE - 1);
+		length -= tail;
+		ctx->total += tail;
+		ctx->offset = length; /* offset where to start slow */
+
+		sg = ctx->sg;
+		atmel_sha_append_sg(ctx);
+
+		atmel_sha_fill_padding(ctx, length);
+
+		ctx->dma_addr = dma_map_single(dd->dev, ctx->buffer,
+			ctx->buflen + SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+		if (dma_mapping_error(dd->dev, ctx->dma_addr)) {
+			dev_err(dd->dev, "dma %u bytes error\n",
+				ctx->buflen + SHA1_BLOCK_SIZE);
+			return -EINVAL;
+		}
+
+		if (length == 0) {
+			ctx->flags &= ~SHA_FLAGS_SG;
+			count = ctx->bufcnt;
+			ctx->bufcnt = 0;
+			return atmel_sha_xmit_pdc(dd, ctx->dma_addr, count, 0,
+					0, final);
+		} else {
+			ctx->sg = sg;
+			if (!dma_map_sg(dd->dev, ctx->sg, 1,
+				DMA_TO_DEVICE)) {
+					dev_err(dd->dev, "dma_map_sg  error\n");
+					return -EINVAL;
+			}
+
+			ctx->flags |= SHA_FLAGS_SG;
+
+			count = ctx->bufcnt;
+			ctx->bufcnt = 0;
+			return atmel_sha_xmit_pdc(dd, sg_dma_address(ctx->sg),
+					length, ctx->dma_addr, count, final);
+		}
+	}
+
+	if (!dma_map_sg(dd->dev, ctx->sg, 1, DMA_TO_DEVICE)) {
+		dev_err(dd->dev, "dma_map_sg  error\n");
+		return -EINVAL;
+	}
+
+	ctx->flags |= SHA_FLAGS_SG;
+
+	/* next call does not fail... so no unmap in the case of error */
+	return atmel_sha_xmit_pdc(dd, sg_dma_address(ctx->sg), length, 0,
+								0, final);
+}
+
+static int atmel_sha_update_dma_stop(struct atmel_sha_dev *dd)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(dd->req);
+
+	if (ctx->flags & SHA_FLAGS_SG) {
+		dma_unmap_sg(dd->dev, ctx->sg, 1, DMA_TO_DEVICE);
+		if (ctx->sg->length == ctx->offset) {
+			ctx->sg = sg_next(ctx->sg);
+			if (ctx->sg)
+				ctx->offset = 0;
+		}
+		if (ctx->flags & SHA_FLAGS_PAD)
+			dma_unmap_single(dd->dev, ctx->dma_addr,
+				ctx->buflen + SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+	} else {
+		dma_unmap_single(dd->dev, ctx->dma_addr, ctx->buflen +
+						SHA1_BLOCK_SIZE, DMA_TO_DEVICE);
+	}
+
+	return 0;
+}
+
+static int atmel_sha_update_req(struct atmel_sha_dev *dd)
+{
+	struct ahash_request *req = dd->req;
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	int err;
+
+	dev_dbg(dd->dev, "update_req: total: %u, digcnt: %d, finup: %d\n",
+		 ctx->total, ctx->digcnt, (ctx->flags & SHA_FLAGS_FINUP) != 0);
+
+	if (ctx->flags & SHA_FLAGS_CPU)
+		err = atmel_sha_update_cpu(dd);
+	else
+		err = atmel_sha_update_dma_start(dd);
+
+	/* wait for dma completion before can take more data */
+	dev_dbg(dd->dev, "update: err: %d, digcnt: %d\n",
+			err, ctx->digcnt);
+
+	return err;
+}
+
+static int atmel_sha_final_req(struct atmel_sha_dev *dd)
+{
+	struct ahash_request *req = dd->req;
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	int err = 0;
+	int count;
+
+	if (ctx->bufcnt >= ATMEL_SHA_DMA_THRESHOLD) {
+		atmel_sha_fill_padding(ctx, 0);
+		count = ctx->bufcnt;
+		ctx->bufcnt = 0;
+		err = atmel_sha_xmit_dma_map(dd, ctx, count, 1);
+	}
+	/* faster to handle last block with cpu */
+	else {
+		atmel_sha_fill_padding(ctx, 0);
+		count = ctx->bufcnt;
+		ctx->bufcnt = 0;
+		err = atmel_sha_xmit_cpu(dd, ctx->buffer, count, 1);
+	}
+
+	dev_dbg(dd->dev, "final_req: err: %d\n", err);
+
+	return err;
+}
+
+static void atmel_sha_copy_hash(struct ahash_request *req)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	u32 *hash = (u32 *)ctx->digest;
+	int i;
+
+	if (likely(ctx->flags & SHA_FLAGS_SHA1))
+		for (i = 0; i < SHA1_DIGEST_SIZE / sizeof(u32); i++)
+			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
+	else
+		for (i = 0; i < SHA256_DIGEST_SIZE / sizeof(u32); i++)
+			hash[i] = atmel_sha_read(ctx->dd, SHA_REG_DIGEST(i));
+}
+
+static void atmel_sha_copy_ready_hash(struct ahash_request *req)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+
+	if (!req->result)
+		return;
+
+	if (likely(ctx->flags & SHA_FLAGS_SHA1))
+		memcpy(req->result, ctx->digest, SHA1_DIGEST_SIZE);
+	else
+		memcpy(req->result, ctx->digest, SHA256_DIGEST_SIZE);
+}
+
+static int atmel_sha_finish(struct ahash_request *req)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	struct atmel_sha_dev *dd = ctx->dd;
+	int err = 0;
+
+	if (ctx->digcnt)
+		atmel_sha_copy_ready_hash(req);
+
+	dev_dbg(dd->dev, "digcnt: %d, bufcnt: %d\n", ctx->digcnt,
+		ctx->bufcnt);
+
+	return err;
+}
+
+static void atmel_sha_finish_req(struct ahash_request *req, int err)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	struct atmel_sha_dev *dd = ctx->dd;
+
+	if (!err) {
+		atmel_sha_copy_hash(req);
+		if (SHA_FLAGS_FINAL & dd->flags)
+			err = atmel_sha_finish(req);
+	} else {
+		ctx->flags |= SHA_FLAGS_ERROR;
+	}
+
+	/* atomic operation is not needed here */
+	dd->flags &= ~(SHA_FLAGS_BUSY | SHA_FLAGS_FINAL | SHA_FLAGS_CPU |
+			SHA_FLAGS_DMA_READY | SHA_FLAGS_OUTPUT_READY);
+
+	clk_disable_unprepare(dd->iclk);
+
+	if (req->base.complete)
+		req->base.complete(&req->base, err);
+
+	/* handle new request */
+	tasklet_schedule(&dd->done_task);
+}
+
+static int atmel_sha_hw_init(struct atmel_sha_dev *dd)
+{
+	clk_prepare_enable(dd->iclk);
+
+	if (SHA_FLAGS_INIT & dd->flags) {
+		atmel_sha_write(dd, SHA_CR, SHA_CR_SWRST);
+		atmel_sha_dualbuff_test(dd);
+		dd->flags |= SHA_FLAGS_INIT;
+		dd->err = 0;
+	}
+
+	return 0;
+}
+
+static int atmel_sha_handle_queue(struct atmel_sha_dev *dd,
+				  struct ahash_request *req)
+{
+	struct crypto_async_request *async_req, *backlog;
+	struct atmel_sha_reqctx *ctx;
+	unsigned long flags;
+	int err = 0, ret = 0;
+
+	spin_lock_irqsave(&dd->lock, flags);
+	if (req)
+		ret = ahash_enqueue_request(&dd->queue, req);
+
+	if (SHA_FLAGS_BUSY & dd->flags) {
+		spin_unlock_irqrestore(&dd->lock, flags);
+		return ret;
+	}
+
+	backlog = crypto_get_backlog(&dd->queue);
+	async_req = crypto_dequeue_request(&dd->queue);
+	if (async_req)
+		dd->flags |= SHA_FLAGS_BUSY;
+
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+	if (!async_req)
+		return ret;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ahash_request_cast(async_req);
+	dd->req = req;
+	ctx = ahash_request_ctx(req);
+
+	dev_dbg(dd->dev, "handling new req, op: %lu, nbytes: %d\n",
+						ctx->op, req->nbytes);
+
+	err = atmel_sha_hw_init(dd);
+
+	if (err)
+		goto err1;
+
+	if (ctx->op == SHA_OP_UPDATE) {
+		err = atmel_sha_update_req(dd);
+		if (err != -EINPROGRESS && (ctx->flags & SHA_FLAGS_FINUP)) {
+			/* no final() after finup() */
+			err = atmel_sha_final_req(dd);
+		}
+	} else if (ctx->op == SHA_OP_FINAL) {
+		err = atmel_sha_final_req(dd);
+	}
+
+err1:
+	if (err != -EINPROGRESS)
+		/* done_task will not finish it, so do it here */
+		atmel_sha_finish_req(req, err);
+
+	dev_dbg(dd->dev, "exit, err: %d\n", err);
+
+	return ret;
+}
+
+static int atmel_sha_enqueue(struct ahash_request *req, unsigned int op)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	struct atmel_sha_ctx *tctx = crypto_tfm_ctx(req->base.tfm);
+	struct atmel_sha_dev *dd = tctx->dd;
+
+	ctx->op = op;
+
+	return atmel_sha_handle_queue(dd, req);
+}
+
+static int atmel_sha_update(struct ahash_request *req)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+
+	if (!req->nbytes)
+		return 0;
+
+	ctx->total = req->nbytes;
+	ctx->sg = req->src;
+	ctx->offset = 0;
+
+	if (ctx->flags & SHA_FLAGS_FINUP) {
+		if (ctx->bufcnt + ctx->total < ATMEL_SHA_DMA_THRESHOLD)
+			/* faster to use CPU for short transfers */
+			ctx->flags |= SHA_FLAGS_CPU;
+	} else if (ctx->bufcnt + ctx->total < ctx->buflen) {
+		atmel_sha_append_sg(ctx);
+		return 0;
+	}
+	return atmel_sha_enqueue(req, SHA_OP_UPDATE);
+}
+
+static int atmel_sha_final(struct ahash_request *req)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	struct atmel_sha_ctx *tctx = crypto_tfm_ctx(req->base.tfm);
+	struct atmel_sha_dev *dd = tctx->dd;
+
+	int err = 0;
+
+	ctx->flags |= SHA_FLAGS_FINUP;
+
+	if (ctx->flags & SHA_FLAGS_ERROR)
+		return 0; /* uncompleted hash is not needed */
+
+	if (ctx->bufcnt) {
+		return atmel_sha_enqueue(req, SHA_OP_FINAL);
+	} else if (!(ctx->flags & SHA_FLAGS_PAD)) { /* add padding */
+		err = atmel_sha_hw_init(dd);
+		if (err)
+			goto err1;
+
+		dd->flags |= SHA_FLAGS_BUSY;
+		err = atmel_sha_final_req(dd);
+	} else {
+		/* copy ready hash (+ finalize hmac) */
+		return atmel_sha_finish(req);
+	}
+
+err1:
+	if (err != -EINPROGRESS)
+		/* done_task will not finish it, so do it here */
+		atmel_sha_finish_req(req, err);
+
+	return err;
+}
+
+static int atmel_sha_finup(struct ahash_request *req)
+{
+	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
+	int err1, err2;
+
+	ctx->flags |= SHA_FLAGS_FINUP;
+
+	err1 = atmel_sha_update(req);
+	if (err1 == -EINPROGRESS || err1 == -EBUSY)
+		return err1;
+
+	/*
+	 * final() has to be always called to cleanup resources
+	 * even if udpate() failed, except EINPROGRESS
+	 */
+	err2 = atmel_sha_final(req);
+
+	return err1 ?: err2;
+}
+
+static int atmel_sha_digest(struct ahash_request *req)
+{
+	return atmel_sha_init(req) ?: atmel_sha_finup(req);
+}
+
+static int atmel_sha_cra_init_alg(struct crypto_tfm *tfm, const char *alg_base)
+{
+	struct atmel_sha_ctx *tctx = crypto_tfm_ctx(tfm);
+	const char *alg_name = crypto_tfm_alg_name(tfm);
+
+	/* Allocate a fallback and abort if it failed. */
+	tctx->fallback = crypto_alloc_shash(alg_name, 0,
+					    CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(tctx->fallback)) {
+		pr_err("atmel-sha: fallback driver '%s' could not be loaded.\n",
+				alg_name);
+		return PTR_ERR(tctx->fallback);
+	}
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct atmel_sha_reqctx) +
+				 SHA_BUFFER_LEN + SHA256_BLOCK_SIZE);
+
+	return 0;
+}
+
+static int atmel_sha_cra_init(struct crypto_tfm *tfm)
+{
+	return atmel_sha_cra_init_alg(tfm, NULL);
+}
+
+static void atmel_sha_cra_exit(struct crypto_tfm *tfm)
+{
+	struct atmel_sha_ctx *tctx = crypto_tfm_ctx(tfm);
+
+	crypto_free_shash(tctx->fallback);
+	tctx->fallback = NULL;
+}
+
+static struct ahash_alg sha_algs[] = {
+{
+	.init		= atmel_sha_init,
+	.update		= atmel_sha_update,
+	.final		= atmel_sha_final,
+	.finup		= atmel_sha_finup,
+	.digest		= atmel_sha_digest,
+	.halg = {
+		.digestsize	= SHA1_DIGEST_SIZE,
+		.base	= {
+			.cra_name		= "sha1",
+			.cra_driver_name	= "atmel-sha1",
+			.cra_priority		= 100,
+			.cra_flags		= CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize		= SHA1_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct atmel_sha_ctx),
+			.cra_alignmask		= 0,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= atmel_sha_cra_init,
+			.cra_exit		= atmel_sha_cra_exit,
+		}
+	}
+},
+{
+	.init		= atmel_sha_init,
+	.update		= atmel_sha_update,
+	.final		= atmel_sha_final,
+	.finup		= atmel_sha_finup,
+	.digest		= atmel_sha_digest,
+	.halg = {
+		.digestsize	= SHA256_DIGEST_SIZE,
+		.base	= {
+			.cra_name		= "sha256",
+			.cra_driver_name	= "atmel-sha256",
+			.cra_priority		= 100,
+			.cra_flags		= CRYPTO_ALG_ASYNC |
+						CRYPTO_ALG_NEED_FALLBACK,
+			.cra_blocksize		= SHA256_BLOCK_SIZE,
+			.cra_ctxsize		= sizeof(struct atmel_sha_ctx),
+			.cra_alignmask		= 0,
+			.cra_module		= THIS_MODULE,
+			.cra_init		= atmel_sha_cra_init,
+			.cra_exit		= atmel_sha_cra_exit,
+		}
+	}
+},
+};
+
+static void atmel_sha_done_task(unsigned long data)
+{
+	struct atmel_sha_dev *dd = (struct atmel_sha_dev *)data;
+	int err = 0;
+
+	if (!(SHA_FLAGS_BUSY & dd->flags)) {
+		atmel_sha_handle_queue(dd, NULL);
+		return;
+	}
+
+	if (SHA_FLAGS_CPU & dd->flags) {
+		if (SHA_FLAGS_OUTPUT_READY & dd->flags) {
+			dd->flags &= ~SHA_FLAGS_OUTPUT_READY;
+			goto finish;
+		}
+	} else if (SHA_FLAGS_DMA_READY & dd->flags) {
+		if (SHA_FLAGS_DMA_ACTIVE & dd->flags) {
+			dd->flags &= ~SHA_FLAGS_DMA_ACTIVE;
+			atmel_sha_update_dma_stop(dd);
+			if (dd->err) {
+				err = dd->err;
+				goto finish;
+			}
+		}
+		if (SHA_FLAGS_OUTPUT_READY & dd->flags) {
+			/* hash or semi-hash ready */
+			dd->flags &= ~(SHA_FLAGS_DMA_READY |
+						SHA_FLAGS_OUTPUT_READY);
+			err = atmel_sha_update_dma_start(dd);
+			if (err != -EINPROGRESS)
+				goto finish;
+		}
+	}
+	return;
+
+finish:
+	/* finish curent request */
+	atmel_sha_finish_req(dd->req, err);
+}
+
+static irqreturn_t atmel_sha_irq(int irq, void *dev_id)
+{
+	struct atmel_sha_dev *sha_dd = dev_id;
+	u32 reg;
+
+	reg = atmel_sha_read(sha_dd, SHA_ISR);
+	if (reg & atmel_sha_read(sha_dd, SHA_IMR)) {
+		atmel_sha_write(sha_dd, SHA_IDR, reg);
+		if (SHA_FLAGS_BUSY & sha_dd->flags) {
+			sha_dd->flags |= SHA_FLAGS_OUTPUT_READY;
+			if (!(SHA_FLAGS_CPU & sha_dd->flags))
+				sha_dd->flags |= SHA_FLAGS_DMA_READY;
+			tasklet_schedule(&sha_dd->done_task);
+		} else {
+			dev_warn(sha_dd->dev, "SHA interrupt when no active requests.\n");
+		}
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static void atmel_sha_unregister_algs(struct atmel_sha_dev *dd)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(sha_algs); i++)
+		crypto_unregister_ahash(&sha_algs[i]);
+}
+
+static int atmel_sha_register_algs(struct atmel_sha_dev *dd)
+{
+	int err, i, j;
+
+	for (i = 0; i < ARRAY_SIZE(sha_algs); i++) {
+		err = crypto_register_ahash(&sha_algs[i]);
+		if (err)
+			goto err_sha_algs;
+	}
+
+	return 0;
+
+err_sha_algs:
+	for (j = 0; j < i; j++)
+		crypto_unregister_ahash(&sha_algs[j]);
+
+	return err;
+}
+
+static int __devinit atmel_sha_probe(struct platform_device *pdev)
+{
+	struct atmel_sha_dev *sha_dd;
+	struct device *dev = &pdev->dev;
+	struct resource *sha_res;
+	unsigned long sha_phys_size;
+	int err;
+
+	sha_dd = kzalloc(sizeof(struct atmel_sha_dev), GFP_KERNEL);
+	if (sha_dd == NULL) {
+		dev_err(dev, "unable to alloc data struct.\n");
+		err = -ENOMEM;
+		goto sha_dd_err;
+	}
+
+	sha_dd->dev = dev;
+
+	platform_set_drvdata(pdev, sha_dd);
+
+	INIT_LIST_HEAD(&sha_dd->list);
+
+	tasklet_init(&sha_dd->done_task, atmel_sha_done_task,
+					(unsigned long)sha_dd);
+
+	crypto_init_queue(&sha_dd->queue, ATMEL_SHA_QUEUE_LENGTH);
+
+	sha_dd->irq = -1;
+
+	/* Get the base address */
+	sha_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!sha_res) {
+		dev_err(dev, "no MEM resource info\n");
+		err = -ENODEV;
+		goto res_err;
+	}
+	sha_dd->phys_base = sha_res->start;
+	sha_phys_size = resource_size(sha_res);
+
+	/* Get the IRQ */
+	sha_dd->irq = platform_get_irq(pdev,  0);
+	if (sha_dd->irq < 0) {
+		dev_err(dev, "no IRQ resource info\n");
+		err = sha_dd->irq;
+		goto res_err;
+	}
+
+	err = request_irq(sha_dd->irq, atmel_sha_irq, IRQF_SHARED, "atmel-sha",
+						sha_dd);
+	if (err) {
+		dev_err(dev, "unable to request sha irq.\n");
+		goto res_err;
+	}
+
+	/* Initializing the clock */
+	sha_dd->iclk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(sha_dd->iclk)) {
+		dev_err(dev, "clock intialization failed.\n");
+		err = PTR_ERR(sha_dd->iclk);
+		goto clk_err;
+	}
+
+	sha_dd->io_base = ioremap(sha_dd->phys_base, sha_phys_size);
+	if (!sha_dd->io_base) {
+		dev_err(dev, "can't ioremap\n");
+		err = -ENOMEM;
+		goto sha_io_err;
+	}
+
+	spin_lock(&atmel_sha.lock);
+	list_add_tail(&sha_dd->list, &atmel_sha.dev_list);
+	spin_unlock(&atmel_sha.lock);
+
+	err = atmel_sha_register_algs(sha_dd);
+	if (err)
+		goto err_algs;
+
+	dev_info(dev, "Atmel SHA1/SHA256\n");
+
+	return 0;
+
+err_algs:
+	spin_lock(&atmel_sha.lock);
+	list_del(&sha_dd->list);
+	spin_unlock(&atmel_sha.lock);
+	iounmap(sha_dd->io_base);
+sha_io_err:
+	clk_put(sha_dd->iclk);
+clk_err:
+	free_irq(sha_dd->irq, sha_dd);
+res_err:
+	tasklet_kill(&sha_dd->done_task);
+	kfree(sha_dd);
+	sha_dd = NULL;
+sha_dd_err:
+	dev_err(dev, "initialization failed.\n");
+
+	return err;
+}
+
+static int __devexit atmel_sha_remove(struct platform_device *pdev)
+{
+	static struct atmel_sha_dev *sha_dd;
+
+	sha_dd = platform_get_drvdata(pdev);
+	if (!sha_dd)
+		return -ENODEV;
+	spin_lock(&atmel_sha.lock);
+	list_del(&sha_dd->list);
+	spin_unlock(&atmel_sha.lock);
+
+	atmel_sha_unregister_algs(sha_dd);
+
+	tasklet_kill(&sha_dd->done_task);
+
+	iounmap(sha_dd->io_base);
+
+	clk_put(sha_dd->iclk);
+
+	if (sha_dd->irq >= 0)
+		free_irq(sha_dd->irq, sha_dd);
+
+	kfree(sha_dd);
+	sha_dd = NULL;
+
+	return 0;
+}
+
+static struct platform_driver atmel_sha_driver = {
+	.probe		= atmel_sha_probe,
+	.remove		= __devexit_p(atmel_sha_remove),
+	.driver		= {
+		.name	= "atmel_sha",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(atmel_sha_driver);
+
+MODULE_DESCRIPTION("Atmel SHA1/SHA256 hw acceleration support.");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Nicolas Royer - Eukréa Electromatique");
diff --git a/drivers/crypto/atmel-tdes-regs.h b/drivers/crypto/atmel-tdes-regs.h
new file mode 100644
index 00000000000..5ac2a900d80
--- /dev/null
+++ b/drivers/crypto/atmel-tdes-regs.h
@@ -0,0 +1,89 @@
+#ifndef __ATMEL_TDES_REGS_H__
+#define __ATMEL_TDES_REGS_H__
+
+#define TDES_CR			0x00
+#define TDES_CR_START			(1 << 0)
+#define TDES_CR_SWRST			(1 << 8)
+#define TDES_CR_LOADSEED		(1 << 16)
+
+#define	TDES_MR			0x04
+#define TDES_MR_CYPHER_DEC		(0 << 0)
+#define TDES_MR_CYPHER_ENC		(1 << 0)
+#define TDES_MR_TDESMOD_MASK	(0x3 << 1)
+#define TDES_MR_TDESMOD_DES		(0x0 << 1)
+#define TDES_MR_TDESMOD_TDES	(0x1 << 1)
+#define TDES_MR_TDESMOD_XTEA	(0x2 << 1)
+#define TDES_MR_KEYMOD_3KEY		(0 << 4)
+#define TDES_MR_KEYMOD_2KEY		(1 << 4)
+#define TDES_MR_SMOD_MASK		(0x3 << 8)
+#define TDES_MR_SMOD_MANUAL		(0x0 << 8)
+#define TDES_MR_SMOD_AUTO		(0x1 << 8)
+#define TDES_MR_SMOD_PDC		(0x2 << 8)
+#define TDES_MR_OPMOD_MASK		(0x3 << 12)
+#define TDES_MR_OPMOD_ECB		(0x0 << 12)
+#define TDES_MR_OPMOD_CBC		(0x1 << 12)
+#define TDES_MR_OPMOD_OFB		(0x2 << 12)
+#define TDES_MR_OPMOD_CFB		(0x3 << 12)
+#define TDES_MR_LOD				(0x1 << 15)
+#define TDES_MR_CFBS_MASK		(0x3 << 16)
+#define TDES_MR_CFBS_64b		(0x0 << 16)
+#define TDES_MR_CFBS_32b		(0x1 << 16)
+#define TDES_MR_CFBS_16b		(0x2 << 16)
+#define TDES_MR_CFBS_8b			(0x3 << 16)
+#define TDES_MR_CKEY_MASK		(0xF << 20)
+#define TDES_MR_CKEY_OFFSET		20
+#define TDES_MR_CTYPE_MASK		(0x3F << 24)
+#define TDES_MR_CTYPE_OFFSET	24
+
+#define	TDES_IER		0x10
+#define	TDES_IDR		0x14
+#define	TDES_IMR		0x18
+#define	TDES_ISR		0x1C
+#define TDES_INT_DATARDY		(1 << 0)
+#define TDES_INT_ENDRX			(1 << 1)
+#define TDES_INT_ENDTX			(1 << 2)
+#define TDES_INT_RXBUFF			(1 << 3)
+#define TDES_INT_TXBUFE			(1 << 4)
+#define TDES_INT_URAD			(1 << 8)
+#define TDES_ISR_URAT_MASK		(0x3 << 12)
+#define TDES_ISR_URAT_IDR		(0x0 << 12)
+#define TDES_ISR_URAT_ODR		(0x1 << 12)
+#define TDES_ISR_URAT_MR		(0x2 << 12)
+#define TDES_ISR_URAT_WO		(0x3 << 12)
+
+
+#define	TDES_KEY1W1R	0x20
+#define	TDES_KEY1W2R	0x24
+#define	TDES_KEY2W1R	0x28
+#define	TDES_KEY2W2R	0x2C
+#define	TDES_KEY3W1R	0x30
+#define	TDES_KEY3W2R	0x34
+#define	TDES_IDATA1R	0x40
+#define	TDES_IDATA2R	0x44
+#define	TDES_ODATA1R	0x50
+#define	TDES_ODATA2R	0x54
+#define	TDES_IV1R		0x60
+#define	TDES_IV2R		0x64
+
+#define	TDES_XTEARNDR	0x70
+#define	TDES_XTEARNDR_XTEA_RNDS_MASK	(0x3F << 0)
+#define	TDES_XTEARNDR_XTEA_RNDS_OFFSET	0
+
+#define TDES_RPR		0x100
+#define TDES_RCR		0x104
+#define TDES_TPR		0x108
+#define TDES_TCR		0x10C
+#define TDES_RNPR		0x118
+#define TDES_RNCR		0x11C
+#define TDES_TNPR		0x118
+#define TDES_TNCR		0x11C
+#define TDES_PTCR		0x120
+#define TDES_PTCR_RXTEN			(1 << 0)
+#define TDES_PTCR_RXTDIS		(1 << 1)
+#define TDES_PTCR_TXTEN			(1 << 8)
+#define TDES_PTCR_TXTDIS		(1 << 9)
+#define TDES_PTSR		0x124
+#define TDES_PTSR_RXTEN			(1 << 0)
+#define TDES_PTSR_TXTEN			(1 << 8)
+
+#endif /* __ATMEL_TDES_REGS_H__ */
diff --git a/drivers/crypto/atmel-tdes.c b/drivers/crypto/atmel-tdes.c
new file mode 100644
index 00000000000..eb2b61e57e2
--- /dev/null
+++ b/drivers/crypto/atmel-tdes.c
@@ -0,0 +1,1215 @@
+/*
+ * Cryptographic API.
+ *
+ * Support for ATMEL DES/TDES HW acceleration.
+ *
+ * Copyright (c) 2012 Eukréa Electromatique - ATMEL
+ * Author: Nicolas Royer <nicolas@eukrea.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.
+ *
+ * Some ideas are from omap-aes.c drivers.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/hw_random.h>
+#include <linux/platform_device.h>
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/crypto.h>
+#include <linux/cryptohash.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+#include <crypto/des.h>
+#include <crypto/hash.h>
+#include <crypto/internal/hash.h>
+#include "atmel-tdes-regs.h"
+
+/* TDES flags  */
+#define TDES_FLAGS_MODE_MASK		0x007f
+#define TDES_FLAGS_ENCRYPT	BIT(0)
+#define TDES_FLAGS_CBC		BIT(1)
+#define TDES_FLAGS_CFB		BIT(2)
+#define TDES_FLAGS_CFB8		BIT(3)
+#define TDES_FLAGS_CFB16	BIT(4)
+#define TDES_FLAGS_CFB32	BIT(5)
+#define TDES_FLAGS_OFB		BIT(6)
+
+#define TDES_FLAGS_INIT		BIT(16)
+#define TDES_FLAGS_FAST		BIT(17)
+#define TDES_FLAGS_BUSY		BIT(18)
+
+#define ATMEL_TDES_QUEUE_LENGTH	1
+
+#define CFB8_BLOCK_SIZE		1
+#define CFB16_BLOCK_SIZE	2
+#define CFB32_BLOCK_SIZE	4
+#define CFB64_BLOCK_SIZE	8
+
+
+struct atmel_tdes_dev;
+
+struct atmel_tdes_ctx {
+	struct atmel_tdes_dev *dd;
+
+	int		keylen;
+	u32		key[3*DES_KEY_SIZE / sizeof(u32)];
+	unsigned long	flags;
+};
+
+struct atmel_tdes_reqctx {
+	unsigned long mode;
+};
+
+struct atmel_tdes_dev {
+	struct list_head	list;
+	unsigned long		phys_base;
+	void __iomem		*io_base;
+
+	struct atmel_tdes_ctx	*ctx;
+	struct device		*dev;
+	struct clk			*iclk;
+	int					irq;
+
+	unsigned long		flags;
+	int			err;
+
+	spinlock_t		lock;
+	struct crypto_queue	queue;
+
+	struct tasklet_struct	done_task;
+	struct tasklet_struct	queue_task;
+
+	struct ablkcipher_request	*req;
+	size_t				total;
+
+	struct scatterlist	*in_sg;
+	size_t				in_offset;
+	struct scatterlist	*out_sg;
+	size_t				out_offset;
+
+	size_t	buflen;
+	size_t	dma_size;
+
+	void	*buf_in;
+	int		dma_in;
+	dma_addr_t	dma_addr_in;
+
+	void	*buf_out;
+	int		dma_out;
+	dma_addr_t	dma_addr_out;
+};
+
+struct atmel_tdes_drv {
+	struct list_head	dev_list;
+	spinlock_t		lock;
+};
+
+static struct atmel_tdes_drv atmel_tdes = {
+	.dev_list = LIST_HEAD_INIT(atmel_tdes.dev_list),
+	.lock = __SPIN_LOCK_UNLOCKED(atmel_tdes.lock),
+};
+
+static int atmel_tdes_sg_copy(struct scatterlist **sg, size_t *offset,
+			void *buf, size_t buflen, size_t total, int out)
+{
+	unsigned int count, off = 0;
+
+	while (buflen && total) {
+		count = min((*sg)->length - *offset, total);
+		count = min(count, buflen);
+
+		if (!count)
+			return off;
+
+		scatterwalk_map_and_copy(buf + off, *sg, *offset, count, out);
+
+		off += count;
+		buflen -= count;
+		*offset += count;
+		total -= count;
+
+		if (*offset == (*sg)->length) {
+			*sg = sg_next(*sg);
+			if (*sg)
+				*offset = 0;
+			else
+				total = 0;
+		}
+	}
+
+	return off;
+}
+
+static inline u32 atmel_tdes_read(struct atmel_tdes_dev *dd, u32 offset)
+{
+	return readl_relaxed(dd->io_base + offset);
+}
+
+static inline void atmel_tdes_write(struct atmel_tdes_dev *dd,
+					u32 offset, u32 value)
+{
+	writel_relaxed(value, dd->io_base + offset);
+}
+
+static void atmel_tdes_write_n(struct atmel_tdes_dev *dd, u32 offset,
+					u32 *value, int count)
+{
+	for (; count--; value++, offset += 4)
+		atmel_tdes_write(dd, offset, *value);
+}
+
+static struct atmel_tdes_dev *atmel_tdes_find_dev(struct atmel_tdes_ctx *ctx)
+{
+	struct atmel_tdes_dev *tdes_dd = NULL;
+	struct atmel_tdes_dev *tmp;
+
+	spin_lock_bh(&atmel_tdes.lock);
+	if (!ctx->dd) {
+		list_for_each_entry(tmp, &atmel_tdes.dev_list, list) {
+			tdes_dd = tmp;
+			break;
+		}
+		ctx->dd = tdes_dd;
+	} else {
+		tdes_dd = ctx->dd;
+	}
+	spin_unlock_bh(&atmel_tdes.lock);
+
+	return tdes_dd;
+}
+
+static int atmel_tdes_hw_init(struct atmel_tdes_dev *dd)
+{
+	clk_prepare_enable(dd->iclk);
+
+	if (!(dd->flags & TDES_FLAGS_INIT)) {
+		atmel_tdes_write(dd, TDES_CR, TDES_CR_SWRST);
+		dd->flags |= TDES_FLAGS_INIT;
+		dd->err = 0;
+	}
+
+	return 0;
+}
+
+static int atmel_tdes_write_ctrl(struct atmel_tdes_dev *dd)
+{
+	int err;
+	u32 valcr = 0, valmr = TDES_MR_SMOD_PDC;
+
+	err = atmel_tdes_hw_init(dd);
+
+	if (err)
+		return err;
+
+	atmel_tdes_write(dd, TDES_PTCR, TDES_PTCR_TXTDIS|TDES_PTCR_RXTDIS);
+
+	/* MR register must be set before IV registers */
+	if (dd->ctx->keylen > (DES_KEY_SIZE << 1)) {
+		valmr |= TDES_MR_KEYMOD_3KEY;
+		valmr |= TDES_MR_TDESMOD_TDES;
+	} else if (dd->ctx->keylen > DES_KEY_SIZE) {
+		valmr |= TDES_MR_KEYMOD_2KEY;
+		valmr |= TDES_MR_TDESMOD_TDES;
+	} else {
+		valmr |= TDES_MR_TDESMOD_DES;
+	}
+
+	if (dd->flags & TDES_FLAGS_CBC) {
+		valmr |= TDES_MR_OPMOD_CBC;
+	} else if (dd->flags & TDES_FLAGS_CFB) {
+		valmr |= TDES_MR_OPMOD_CFB;
+
+		if (dd->flags & TDES_FLAGS_CFB8)
+			valmr |= TDES_MR_CFBS_8b;
+		else if (dd->flags & TDES_FLAGS_CFB16)
+			valmr |= TDES_MR_CFBS_16b;
+		else if (dd->flags & TDES_FLAGS_CFB32)
+			valmr |= TDES_MR_CFBS_32b;
+	} else if (dd->flags & TDES_FLAGS_OFB) {
+		valmr |= TDES_MR_OPMOD_OFB;
+	}
+
+	if ((dd->flags & TDES_FLAGS_ENCRYPT) || (dd->flags & TDES_FLAGS_OFB))
+		valmr |= TDES_MR_CYPHER_ENC;
+
+	atmel_tdes_write(dd, TDES_CR, valcr);
+	atmel_tdes_write(dd, TDES_MR, valmr);
+
+	atmel_tdes_write_n(dd, TDES_KEY1W1R, dd->ctx->key,
+						dd->ctx->keylen >> 2);
+
+	if (((dd->flags & TDES_FLAGS_CBC) || (dd->flags & TDES_FLAGS_CFB) ||
+		(dd->flags & TDES_FLAGS_OFB)) && dd->req->info) {
+		atmel_tdes_write_n(dd, TDES_IV1R, dd->req->info, 2);
+	}
+
+	return 0;
+}
+
+static int atmel_tdes_crypt_dma_stop(struct atmel_tdes_dev *dd)
+{
+	int err = 0;
+	size_t count;
+
+	atmel_tdes_write(dd, TDES_PTCR, TDES_PTCR_TXTDIS|TDES_PTCR_RXTDIS);
+
+	if (dd->flags & TDES_FLAGS_FAST) {
+		dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_FROM_DEVICE);
+		dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+	} else {
+		dma_sync_single_for_device(dd->dev, dd->dma_addr_out,
+					   dd->dma_size, DMA_FROM_DEVICE);
+
+		/* copy data */
+		count = atmel_tdes_sg_copy(&dd->out_sg, &dd->out_offset,
+				dd->buf_out, dd->buflen, dd->dma_size, 1);
+		if (count != dd->dma_size) {
+			err = -EINVAL;
+			pr_err("not all data converted: %u\n", count);
+		}
+	}
+
+	return err;
+}
+
+static int atmel_tdes_dma_init(struct atmel_tdes_dev *dd)
+{
+	int err = -ENOMEM;
+
+	dd->buf_in = (void *)__get_free_pages(GFP_KERNEL, 0);
+	dd->buf_out = (void *)__get_free_pages(GFP_KERNEL, 0);
+	dd->buflen = PAGE_SIZE;
+	dd->buflen &= ~(DES_BLOCK_SIZE - 1);
+
+	if (!dd->buf_in || !dd->buf_out) {
+		dev_err(dd->dev, "unable to alloc pages.\n");
+		goto err_alloc;
+	}
+
+	/* MAP here */
+	dd->dma_addr_in = dma_map_single(dd->dev, dd->buf_in,
+					dd->buflen, DMA_TO_DEVICE);
+	if (dma_mapping_error(dd->dev, dd->dma_addr_in)) {
+		dev_err(dd->dev, "dma %d bytes error\n", dd->buflen);
+		err = -EINVAL;
+		goto err_map_in;
+	}
+
+	dd->dma_addr_out = dma_map_single(dd->dev, dd->buf_out,
+					dd->buflen, DMA_FROM_DEVICE);
+	if (dma_mapping_error(dd->dev, dd->dma_addr_out)) {
+		dev_err(dd->dev, "dma %d bytes error\n", dd->buflen);
+		err = -EINVAL;
+		goto err_map_out;
+	}
+
+	return 0;
+
+err_map_out:
+	dma_unmap_single(dd->dev, dd->dma_addr_in, dd->buflen,
+		DMA_TO_DEVICE);
+err_map_in:
+	free_page((unsigned long)dd->buf_out);
+	free_page((unsigned long)dd->buf_in);
+err_alloc:
+	if (err)
+		pr_err("error: %d\n", err);
+	return err;
+}
+
+static void atmel_tdes_dma_cleanup(struct atmel_tdes_dev *dd)
+{
+	dma_unmap_single(dd->dev, dd->dma_addr_out, dd->buflen,
+			 DMA_FROM_DEVICE);
+	dma_unmap_single(dd->dev, dd->dma_addr_in, dd->buflen,
+		DMA_TO_DEVICE);
+	free_page((unsigned long)dd->buf_out);
+	free_page((unsigned long)dd->buf_in);
+}
+
+static int atmel_tdes_crypt_dma(struct crypto_tfm *tfm, dma_addr_t dma_addr_in,
+			       dma_addr_t dma_addr_out, int length)
+{
+	struct atmel_tdes_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct atmel_tdes_dev *dd = ctx->dd;
+	int len32;
+
+	dd->dma_size = length;
+
+	if (!(dd->flags & TDES_FLAGS_FAST)) {
+		dma_sync_single_for_device(dd->dev, dma_addr_in, length,
+					   DMA_TO_DEVICE);
+	}
+
+	if ((dd->flags & TDES_FLAGS_CFB) && (dd->flags & TDES_FLAGS_CFB8))
+		len32 = DIV_ROUND_UP(length, sizeof(u8));
+	else if ((dd->flags & TDES_FLAGS_CFB) && (dd->flags & TDES_FLAGS_CFB16))
+		len32 = DIV_ROUND_UP(length, sizeof(u16));
+	else
+		len32 = DIV_ROUND_UP(length, sizeof(u32));
+
+	atmel_tdes_write(dd, TDES_PTCR, TDES_PTCR_TXTDIS|TDES_PTCR_RXTDIS);
+	atmel_tdes_write(dd, TDES_TPR, dma_addr_in);
+	atmel_tdes_write(dd, TDES_TCR, len32);
+	atmel_tdes_write(dd, TDES_RPR, dma_addr_out);
+	atmel_tdes_write(dd, TDES_RCR, len32);
+
+	/* Enable Interrupt */
+	atmel_tdes_write(dd, TDES_IER, TDES_INT_ENDRX);
+
+	/* Start DMA transfer */
+	atmel_tdes_write(dd, TDES_PTCR, TDES_PTCR_TXTEN | TDES_PTCR_RXTEN);
+
+	return 0;
+}
+
+static int atmel_tdes_crypt_dma_start(struct atmel_tdes_dev *dd)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(
+					crypto_ablkcipher_reqtfm(dd->req));
+	int err, fast = 0, in, out;
+	size_t count;
+	dma_addr_t addr_in, addr_out;
+
+	if (sg_is_last(dd->in_sg) && sg_is_last(dd->out_sg)) {
+		/* check for alignment */
+		in = IS_ALIGNED((u32)dd->in_sg->offset, sizeof(u32));
+		out = IS_ALIGNED((u32)dd->out_sg->offset, sizeof(u32));
+
+		fast = in && out;
+	}
+
+	if (fast)  {
+		count = min(dd->total, sg_dma_len(dd->in_sg));
+		count = min(count, sg_dma_len(dd->out_sg));
+
+		if (count != dd->total) {
+			pr_err("request length != buffer length\n");
+			return -EINVAL;
+		}
+
+		err = dma_map_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+		if (!err) {
+			dev_err(dd->dev, "dma_map_sg() error\n");
+			return -EINVAL;
+		}
+
+		err = dma_map_sg(dd->dev, dd->out_sg, 1,
+				DMA_FROM_DEVICE);
+		if (!err) {
+			dev_err(dd->dev, "dma_map_sg() error\n");
+			dma_unmap_sg(dd->dev, dd->in_sg, 1,
+				DMA_TO_DEVICE);
+			return -EINVAL;
+		}
+
+		addr_in = sg_dma_address(dd->in_sg);
+		addr_out = sg_dma_address(dd->out_sg);
+
+		dd->flags |= TDES_FLAGS_FAST;
+
+	} else {
+		/* use cache buffers */
+		count = atmel_tdes_sg_copy(&dd->in_sg, &dd->in_offset,
+				dd->buf_in, dd->buflen, dd->total, 0);
+
+		addr_in = dd->dma_addr_in;
+		addr_out = dd->dma_addr_out;
+
+		dd->flags &= ~TDES_FLAGS_FAST;
+
+	}
+
+	dd->total -= count;
+
+	err = atmel_tdes_crypt_dma(tfm, addr_in, addr_out, count);
+	if (err) {
+		dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
+		dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_TO_DEVICE);
+	}
+
+	return err;
+}
+
+
+static void atmel_tdes_finish_req(struct atmel_tdes_dev *dd, int err)
+{
+	struct ablkcipher_request *req = dd->req;
+
+	clk_disable_unprepare(dd->iclk);
+
+	dd->flags &= ~TDES_FLAGS_BUSY;
+
+	req->base.complete(&req->base, err);
+}
+
+static int atmel_tdes_handle_queue(struct atmel_tdes_dev *dd,
+			       struct ablkcipher_request *req)
+{
+	struct crypto_async_request *async_req, *backlog;
+	struct atmel_tdes_ctx *ctx;
+	struct atmel_tdes_reqctx *rctx;
+	unsigned long flags;
+	int err, ret = 0;
+
+	spin_lock_irqsave(&dd->lock, flags);
+	if (req)
+		ret = ablkcipher_enqueue_request(&dd->queue, req);
+	if (dd->flags & TDES_FLAGS_BUSY) {
+		spin_unlock_irqrestore(&dd->lock, flags);
+		return ret;
+	}
+	backlog = crypto_get_backlog(&dd->queue);
+	async_req = crypto_dequeue_request(&dd->queue);
+	if (async_req)
+		dd->flags |= TDES_FLAGS_BUSY;
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+	if (!async_req)
+		return ret;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ablkcipher_request_cast(async_req);
+
+	/* assign new request to device */
+	dd->req = req;
+	dd->total = req->nbytes;
+	dd->in_offset = 0;
+	dd->in_sg = req->src;
+	dd->out_offset = 0;
+	dd->out_sg = req->dst;
+
+	rctx = ablkcipher_request_ctx(req);
+	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
+	rctx->mode &= TDES_FLAGS_MODE_MASK;
+	dd->flags = (dd->flags & ~TDES_FLAGS_MODE_MASK) | rctx->mode;
+	dd->ctx = ctx;
+	ctx->dd = dd;
+
+	err = atmel_tdes_write_ctrl(dd);
+	if (!err)
+		err = atmel_tdes_crypt_dma_start(dd);
+	if (err) {
+		/* des_task will not finish it, so do it here */
+		atmel_tdes_finish_req(dd, err);
+		tasklet_schedule(&dd->queue_task);
+	}
+
+	return ret;
+}
+
+
+static int atmel_tdes_crypt(struct ablkcipher_request *req, unsigned long mode)
+{
+	struct atmel_tdes_ctx *ctx = crypto_ablkcipher_ctx(
+			crypto_ablkcipher_reqtfm(req));
+	struct atmel_tdes_reqctx *rctx = ablkcipher_request_ctx(req);
+	struct atmel_tdes_dev *dd;
+
+	if (mode & TDES_FLAGS_CFB8) {
+		if (!IS_ALIGNED(req->nbytes, CFB8_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB8 blocks\n");
+			return -EINVAL;
+		}
+	} else if (mode & TDES_FLAGS_CFB16) {
+		if (!IS_ALIGNED(req->nbytes, CFB16_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB16 blocks\n");
+			return -EINVAL;
+		}
+	} else if (mode & TDES_FLAGS_CFB32) {
+		if (!IS_ALIGNED(req->nbytes, CFB32_BLOCK_SIZE)) {
+			pr_err("request size is not exact amount of CFB32 blocks\n");
+			return -EINVAL;
+		}
+	} else if (!IS_ALIGNED(req->nbytes, DES_BLOCK_SIZE)) {
+		pr_err("request size is not exact amount of DES blocks\n");
+		return -EINVAL;
+	}
+
+	dd = atmel_tdes_find_dev(ctx);
+	if (!dd)
+		return -ENODEV;
+
+	rctx->mode = mode;
+
+	return atmel_tdes_handle_queue(dd, req);
+}
+
+static int atmel_des_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+			   unsigned int keylen)
+{
+	u32 tmp[DES_EXPKEY_WORDS];
+	int err;
+	struct crypto_tfm *ctfm = crypto_ablkcipher_tfm(tfm);
+
+	struct atmel_tdes_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+
+	if (keylen != DES_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	err = des_ekey(tmp, key);
+	if (err == 0 && (ctfm->crt_flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		ctfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return 0;
+}
+
+static int atmel_tdes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+			   unsigned int keylen)
+{
+	struct atmel_tdes_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	const char *alg_name;
+
+	alg_name = crypto_tfm_alg_name(crypto_ablkcipher_tfm(tfm));
+
+	/*
+	 * HW bug in cfb 3-keys mode.
+	 */
+	if (strstr(alg_name, "cfb") && (keylen != 2*DES_KEY_SIZE)) {
+		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	} else if ((keylen != 2*DES_KEY_SIZE) && (keylen != 3*DES_KEY_SIZE)) {
+		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return 0;
+}
+
+static int atmel_tdes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT);
+}
+
+static int atmel_tdes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, 0);
+}
+
+static int atmel_tdes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT | TDES_FLAGS_CBC);
+}
+
+static int atmel_tdes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_CBC);
+}
+static int atmel_tdes_cfb_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT | TDES_FLAGS_CFB);
+}
+
+static int atmel_tdes_cfb_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_CFB);
+}
+
+static int atmel_tdes_cfb8_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT | TDES_FLAGS_CFB |
+						TDES_FLAGS_CFB8);
+}
+
+static int atmel_tdes_cfb8_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_CFB | TDES_FLAGS_CFB8);
+}
+
+static int atmel_tdes_cfb16_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT | TDES_FLAGS_CFB |
+						TDES_FLAGS_CFB16);
+}
+
+static int atmel_tdes_cfb16_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_CFB | TDES_FLAGS_CFB16);
+}
+
+static int atmel_tdes_cfb32_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT | TDES_FLAGS_CFB |
+						TDES_FLAGS_CFB32);
+}
+
+static int atmel_tdes_cfb32_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_CFB | TDES_FLAGS_CFB32);
+}
+
+static int atmel_tdes_ofb_encrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_ENCRYPT | TDES_FLAGS_OFB);
+}
+
+static int atmel_tdes_ofb_decrypt(struct ablkcipher_request *req)
+{
+	return atmel_tdes_crypt(req, TDES_FLAGS_OFB);
+}
+
+static int atmel_tdes_cra_init(struct crypto_tfm *tfm)
+{
+	tfm->crt_ablkcipher.reqsize = sizeof(struct atmel_tdes_reqctx);
+
+	return 0;
+}
+
+static void atmel_tdes_cra_exit(struct crypto_tfm *tfm)
+{
+}
+
+static struct crypto_alg tdes_algs[] = {
+{
+	.cra_name		= "ecb(des)",
+	.cra_driver_name	= "atmel-ecb-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_ecb_encrypt,
+		.decrypt	= atmel_tdes_ecb_decrypt,
+	}
+},
+{
+	.cra_name		= "cbc(des)",
+	.cra_driver_name	= "atmel-cbc-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_cbc_encrypt,
+		.decrypt	= atmel_tdes_cbc_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb(des)",
+	.cra_driver_name	= "atmel-cfb-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_cfb_encrypt,
+		.decrypt	= atmel_tdes_cfb_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb8(des)",
+	.cra_driver_name	= "atmel-cfb8-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB8_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_cfb8_encrypt,
+		.decrypt	= atmel_tdes_cfb8_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb16(des)",
+	.cra_driver_name	= "atmel-cfb16-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB16_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_cfb16_encrypt,
+		.decrypt	= atmel_tdes_cfb16_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb32(des)",
+	.cra_driver_name	= "atmel-cfb32-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB32_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_cfb32_encrypt,
+		.decrypt	= atmel_tdes_cfb32_decrypt,
+	}
+},
+{
+	.cra_name		= "ofb(des)",
+	.cra_driver_name	= "atmel-ofb-des",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= DES_KEY_SIZE,
+		.max_keysize	= DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_des_setkey,
+		.encrypt	= atmel_tdes_ofb_encrypt,
+		.decrypt	= atmel_tdes_ofb_decrypt,
+	}
+},
+{
+	.cra_name		= "ecb(des3_ede)",
+	.cra_driver_name	= "atmel-ecb-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2 * DES_KEY_SIZE,
+		.max_keysize	= 3 * DES_KEY_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_ecb_encrypt,
+		.decrypt	= atmel_tdes_ecb_decrypt,
+	}
+},
+{
+	.cra_name		= "cbc(des3_ede)",
+	.cra_driver_name	= "atmel-cbc-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2*DES_KEY_SIZE,
+		.max_keysize	= 3*DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_cbc_encrypt,
+		.decrypt	= atmel_tdes_cbc_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb(des3_ede)",
+	.cra_driver_name	= "atmel-cfb-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2*DES_KEY_SIZE,
+		.max_keysize	= 2*DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_cfb_encrypt,
+		.decrypt	= atmel_tdes_cfb_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb8(des3_ede)",
+	.cra_driver_name	= "atmel-cfb8-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB8_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2*DES_KEY_SIZE,
+		.max_keysize	= 2*DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_cfb8_encrypt,
+		.decrypt	= atmel_tdes_cfb8_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb16(des3_ede)",
+	.cra_driver_name	= "atmel-cfb16-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB16_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2*DES_KEY_SIZE,
+		.max_keysize	= 2*DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_cfb16_encrypt,
+		.decrypt	= atmel_tdes_cfb16_decrypt,
+	}
+},
+{
+	.cra_name		= "cfb32(des3_ede)",
+	.cra_driver_name	= "atmel-cfb32-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= CFB32_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2*DES_KEY_SIZE,
+		.max_keysize	= 2*DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_cfb32_encrypt,
+		.decrypt	= atmel_tdes_cfb32_decrypt,
+	}
+},
+{
+	.cra_name		= "ofb(des3_ede)",
+	.cra_driver_name	= "atmel-ofb-tdes",
+	.cra_priority		= 100,
+	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+	.cra_blocksize		= DES_BLOCK_SIZE,
+	.cra_ctxsize		= sizeof(struct atmel_tdes_ctx),
+	.cra_alignmask		= 0,
+	.cra_type		= &crypto_ablkcipher_type,
+	.cra_module		= THIS_MODULE,
+	.cra_init		= atmel_tdes_cra_init,
+	.cra_exit		= atmel_tdes_cra_exit,
+	.cra_u.ablkcipher = {
+		.min_keysize	= 2*DES_KEY_SIZE,
+		.max_keysize	= 3*DES_KEY_SIZE,
+		.ivsize		= DES_BLOCK_SIZE,
+		.setkey		= atmel_tdes_setkey,
+		.encrypt	= atmel_tdes_ofb_encrypt,
+		.decrypt	= atmel_tdes_ofb_decrypt,
+	}
+},
+};
+
+static void atmel_tdes_queue_task(unsigned long data)
+{
+	struct atmel_tdes_dev *dd = (struct atmel_tdes_dev *)data;
+
+	atmel_tdes_handle_queue(dd, NULL);
+}
+
+static void atmel_tdes_done_task(unsigned long data)
+{
+	struct atmel_tdes_dev *dd = (struct atmel_tdes_dev *) data;
+	int err;
+
+	err = atmel_tdes_crypt_dma_stop(dd);
+
+	err = dd->err ? : err;
+
+	if (dd->total && !err) {
+		err = atmel_tdes_crypt_dma_start(dd);
+		if (!err)
+			return;
+	}
+
+	atmel_tdes_finish_req(dd, err);
+	atmel_tdes_handle_queue(dd, NULL);
+}
+
+static irqreturn_t atmel_tdes_irq(int irq, void *dev_id)
+{
+	struct atmel_tdes_dev *tdes_dd = dev_id;
+	u32 reg;
+
+	reg = atmel_tdes_read(tdes_dd, TDES_ISR);
+	if (reg & atmel_tdes_read(tdes_dd, TDES_IMR)) {
+		atmel_tdes_write(tdes_dd, TDES_IDR, reg);
+		if (TDES_FLAGS_BUSY & tdes_dd->flags)
+			tasklet_schedule(&tdes_dd->done_task);
+		else
+			dev_warn(tdes_dd->dev, "TDES interrupt when no active requests.\n");
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static void atmel_tdes_unregister_algs(struct atmel_tdes_dev *dd)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tdes_algs); i++)
+		crypto_unregister_alg(&tdes_algs[i]);
+}
+
+static int atmel_tdes_register_algs(struct atmel_tdes_dev *dd)
+{
+	int err, i, j;
+
+	for (i = 0; i < ARRAY_SIZE(tdes_algs); i++) {
+		INIT_LIST_HEAD(&tdes_algs[i].cra_list);
+		err = crypto_register_alg(&tdes_algs[i]);
+		if (err)
+			goto err_tdes_algs;
+	}
+
+	return 0;
+
+err_tdes_algs:
+	for (j = 0; j < i; j++)
+		crypto_unregister_alg(&tdes_algs[j]);
+
+	return err;
+}
+
+static int __devinit atmel_tdes_probe(struct platform_device *pdev)
+{
+	struct atmel_tdes_dev *tdes_dd;
+	struct device *dev = &pdev->dev;
+	struct resource *tdes_res;
+	unsigned long tdes_phys_size;
+	int err;
+
+	tdes_dd = kzalloc(sizeof(struct atmel_tdes_dev), GFP_KERNEL);
+	if (tdes_dd == NULL) {
+		dev_err(dev, "unable to alloc data struct.\n");
+		err = -ENOMEM;
+		goto tdes_dd_err;
+	}
+
+	tdes_dd->dev = dev;
+
+	platform_set_drvdata(pdev, tdes_dd);
+
+	INIT_LIST_HEAD(&tdes_dd->list);
+
+	tasklet_init(&tdes_dd->done_task, atmel_tdes_done_task,
+					(unsigned long)tdes_dd);
+	tasklet_init(&tdes_dd->queue_task, atmel_tdes_queue_task,
+					(unsigned long)tdes_dd);
+
+	crypto_init_queue(&tdes_dd->queue, ATMEL_TDES_QUEUE_LENGTH);
+
+	tdes_dd->irq = -1;
+
+	/* Get the base address */
+	tdes_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!tdes_res) {
+		dev_err(dev, "no MEM resource info\n");
+		err = -ENODEV;
+		goto res_err;
+	}
+	tdes_dd->phys_base = tdes_res->start;
+	tdes_phys_size = resource_size(tdes_res);
+
+	/* Get the IRQ */
+	tdes_dd->irq = platform_get_irq(pdev,  0);
+	if (tdes_dd->irq < 0) {
+		dev_err(dev, "no IRQ resource info\n");
+		err = tdes_dd->irq;
+		goto res_err;
+	}
+
+	err = request_irq(tdes_dd->irq, atmel_tdes_irq, IRQF_SHARED,
+			"atmel-tdes", tdes_dd);
+	if (err) {
+		dev_err(dev, "unable to request tdes irq.\n");
+		goto tdes_irq_err;
+	}
+
+	/* Initializing the clock */
+	tdes_dd->iclk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(tdes_dd->iclk)) {
+		dev_err(dev, "clock intialization failed.\n");
+		err = PTR_ERR(tdes_dd->iclk);
+		goto clk_err;
+	}
+
+	tdes_dd->io_base = ioremap(tdes_dd->phys_base, tdes_phys_size);
+	if (!tdes_dd->io_base) {
+		dev_err(dev, "can't ioremap\n");
+		err = -ENOMEM;
+		goto tdes_io_err;
+	}
+
+	err = atmel_tdes_dma_init(tdes_dd);
+	if (err)
+		goto err_tdes_dma;
+
+	spin_lock(&atmel_tdes.lock);
+	list_add_tail(&tdes_dd->list, &atmel_tdes.dev_list);
+	spin_unlock(&atmel_tdes.lock);
+
+	err = atmel_tdes_register_algs(tdes_dd);
+	if (err)
+		goto err_algs;
+
+	dev_info(dev, "Atmel DES/TDES\n");
+
+	return 0;
+
+err_algs:
+	spin_lock(&atmel_tdes.lock);
+	list_del(&tdes_dd->list);
+	spin_unlock(&atmel_tdes.lock);
+	atmel_tdes_dma_cleanup(tdes_dd);
+err_tdes_dma:
+	iounmap(tdes_dd->io_base);
+tdes_io_err:
+	clk_put(tdes_dd->iclk);
+clk_err:
+	free_irq(tdes_dd->irq, tdes_dd);
+tdes_irq_err:
+res_err:
+	tasklet_kill(&tdes_dd->done_task);
+	tasklet_kill(&tdes_dd->queue_task);
+	kfree(tdes_dd);
+	tdes_dd = NULL;
+tdes_dd_err:
+	dev_err(dev, "initialization failed.\n");
+
+	return err;
+}
+
+static int __devexit atmel_tdes_remove(struct platform_device *pdev)
+{
+	static struct atmel_tdes_dev *tdes_dd;
+
+	tdes_dd = platform_get_drvdata(pdev);
+	if (!tdes_dd)
+		return -ENODEV;
+	spin_lock(&atmel_tdes.lock);
+	list_del(&tdes_dd->list);
+	spin_unlock(&atmel_tdes.lock);
+
+	atmel_tdes_unregister_algs(tdes_dd);
+
+	tasklet_kill(&tdes_dd->done_task);
+	tasklet_kill(&tdes_dd->queue_task);
+
+	atmel_tdes_dma_cleanup(tdes_dd);
+
+	iounmap(tdes_dd->io_base);
+
+	clk_put(tdes_dd->iclk);
+
+	if (tdes_dd->irq >= 0)
+		free_irq(tdes_dd->irq, tdes_dd);
+
+	kfree(tdes_dd);
+	tdes_dd = NULL;
+
+	return 0;
+}
+
+static struct platform_driver atmel_tdes_driver = {
+	.probe		= atmel_tdes_probe,
+	.remove		= __devexit_p(atmel_tdes_remove),
+	.driver		= {
+		.name	= "atmel_tdes",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(atmel_tdes_driver);
+
+MODULE_DESCRIPTION("Atmel DES/TDES hw acceleration support.");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Nicolas Royer - Eukréa Electromatique");
diff --git a/drivers/crypto/bfin_crc.c b/drivers/crypto/bfin_crc.c
new file mode 100644
index 00000000000..5398580b431
--- /dev/null
+++ b/drivers/crypto/bfin_crc.c
@@ -0,0 +1,780 @@
+/*
+ * Cryptographic API.
+ *
+ * Support Blackfin CRC HW acceleration.
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/unaligned/access_ok.h>
+#include <linux/crypto.h>
+#include <linux/cryptohash.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+#include <crypto/hash.h>
+#include <crypto/internal/hash.h>
+
+#include <asm/blackfin.h>
+#include <asm/bfin_crc.h>
+#include <asm/dma.h>
+#include <asm/portmux.h>
+
+#define CRC_CCRYPTO_QUEUE_LENGTH	5
+
+#define DRIVER_NAME "bfin-hmac-crc"
+#define CHKSUM_DIGEST_SIZE      4
+#define CHKSUM_BLOCK_SIZE       1
+
+#define CRC_MAX_DMA_DESC	100
+
+#define CRC_CRYPTO_STATE_UPDATE		1
+#define CRC_CRYPTO_STATE_FINALUPDATE	2
+#define CRC_CRYPTO_STATE_FINISH		3
+
+struct bfin_crypto_crc {
+	struct list_head	list;
+	struct device		*dev;
+	spinlock_t		lock;
+
+	int			irq;
+	int			dma_ch;
+	u32			poly;
+	volatile struct crc_register *regs;
+
+	struct ahash_request	*req; /* current request in operation */
+	struct dma_desc_array	*sg_cpu; /* virt addr of sg dma descriptors */
+	dma_addr_t		sg_dma; /* phy addr of sg dma descriptors */
+	u8			*sg_mid_buf;
+
+	struct tasklet_struct	done_task;
+	struct crypto_queue	queue; /* waiting requests */
+
+	u8			busy:1; /* crc device in operation flag */
+};
+
+static struct bfin_crypto_crc_list {
+	struct list_head	dev_list;
+	spinlock_t		lock;
+} crc_list;
+
+struct bfin_crypto_crc_reqctx {
+	struct bfin_crypto_crc	*crc;
+
+	unsigned int		total;	/* total request bytes */
+	size_t			sg_buflen; /* bytes for this update */
+	unsigned int		sg_nents;
+	struct scatterlist	*sg; /* sg list head for this update*/
+	struct scatterlist	bufsl[2]; /* chained sg list */
+
+	size_t			bufnext_len;
+	size_t			buflast_len;
+	u8			bufnext[CHKSUM_DIGEST_SIZE]; /* extra bytes for next udpate */
+	u8			buflast[CHKSUM_DIGEST_SIZE]; /* extra bytes from last udpate */
+
+	u8			flag;
+};
+
+struct bfin_crypto_crc_ctx {
+	struct bfin_crypto_crc	*crc;
+	u32			key;
+};
+
+
+/*
+ * derive number of elements in scatterlist
+ */
+static int sg_count(struct scatterlist *sg_list)
+{
+	struct scatterlist *sg = sg_list;
+	int sg_nents = 1;
+
+	if (sg_list == NULL)
+		return 0;
+
+	while (!sg_is_last(sg)) {
+		sg_nents++;
+		sg = scatterwalk_sg_next(sg);
+	}
+
+	return sg_nents;
+}
+
+/*
+ * get element in scatter list by given index
+ */
+static struct scatterlist *sg_get(struct scatterlist *sg_list, unsigned int nents,
+				unsigned int index)
+{
+	struct scatterlist *sg = NULL;
+	int i;
+
+	for_each_sg(sg_list, sg, nents, i)
+		if (i == index)
+			break;
+
+	return sg;
+}
+
+static int bfin_crypto_crc_init_hw(struct bfin_crypto_crc *crc, u32 key)
+{
+	crc->regs->datacntrld = 0;
+	crc->regs->control = MODE_CALC_CRC << OPMODE_OFFSET;
+	crc->regs->curresult = key;
+
+	/* setup CRC interrupts */
+	crc->regs->status = CMPERRI | DCNTEXPI;
+	crc->regs->intrenset = CMPERRI | DCNTEXPI;
+	SSYNC();
+
+	return 0;
+}
+
+static int bfin_crypto_crc_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct bfin_crypto_crc_ctx *crc_ctx = crypto_ahash_ctx(tfm);
+	struct bfin_crypto_crc_reqctx *ctx = ahash_request_ctx(req);
+	struct bfin_crypto_crc *crc;
+
+	dev_dbg(crc->dev, "crc_init\n");
+	spin_lock_bh(&crc_list.lock);
+	list_for_each_entry(crc, &crc_list.dev_list, list) {
+		crc_ctx->crc = crc;
+		break;
+	}
+	spin_unlock_bh(&crc_list.lock);
+
+	if (sg_count(req->src) > CRC_MAX_DMA_DESC) {
+		dev_dbg(crc->dev, "init: requested sg list is too big > %d\n",
+			CRC_MAX_DMA_DESC);
+		return -EINVAL;
+	}
+
+	ctx->crc = crc;
+	ctx->bufnext_len = 0;
+	ctx->buflast_len = 0;
+	ctx->sg_buflen = 0;
+	ctx->total = 0;
+	ctx->flag = 0;
+
+	/* init crc results */
+	put_unaligned_le32(crc_ctx->key, req->result);
+
+	dev_dbg(crc->dev, "init: digest size: %d\n",
+		crypto_ahash_digestsize(tfm));
+
+	return bfin_crypto_crc_init_hw(crc, crc_ctx->key);
+}
+
+static void bfin_crypto_crc_config_dma(struct bfin_crypto_crc *crc)
+{
+	struct scatterlist *sg;
+	struct bfin_crypto_crc_reqctx *ctx = ahash_request_ctx(crc->req);
+	int i = 0, j = 0;
+	unsigned long dma_config;
+	unsigned int dma_count;
+	unsigned int dma_addr;
+	unsigned int mid_dma_count = 0;
+	int dma_mod;
+
+	dma_map_sg(crc->dev, ctx->sg, ctx->sg_nents, DMA_TO_DEVICE);
+
+	for_each_sg(ctx->sg, sg, ctx->sg_nents, j) {
+		dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 | DMAEN | PSIZE_32;
+		dma_addr = sg_dma_address(sg);
+		/* deduce extra bytes in last sg */
+		if (sg_is_last(sg))
+			dma_count = sg_dma_len(sg) - ctx->bufnext_len;
+		else
+			dma_count = sg_dma_len(sg);
+
+		if (mid_dma_count) {
+			/* Append last middle dma buffer to 4 bytes with first
+			   bytes in current sg buffer. Move addr of current
+			   sg and deduce the length of current sg.
+			 */
+			memcpy(crc->sg_mid_buf +((i-1) << 2) + mid_dma_count,
+				(void *)dma_addr,
+				CHKSUM_DIGEST_SIZE - mid_dma_count);
+			dma_addr += CHKSUM_DIGEST_SIZE - mid_dma_count;
+			dma_count -= CHKSUM_DIGEST_SIZE - mid_dma_count;
+		}
+		/* chop current sg dma len to multiple of 32 bits */
+		mid_dma_count = dma_count % 4;
+		dma_count &= ~0x3;
+
+		if (dma_addr % 4 == 0) {
+			dma_config |= WDSIZE_32;
+			dma_count >>= 2;
+			dma_mod = 4;
+		} else if (dma_addr % 2 == 0) {
+			dma_config |= WDSIZE_16;
+			dma_count >>= 1;
+			dma_mod = 2;
+		} else {
+			dma_config |= WDSIZE_8;
+			dma_mod = 1;
+		}
+
+		crc->sg_cpu[i].start_addr = dma_addr;
+		crc->sg_cpu[i].cfg = dma_config;
+		crc->sg_cpu[i].x_count = dma_count;
+		crc->sg_cpu[i].x_modify = dma_mod;
+		dev_dbg(crc->dev, "%d: crc_dma: start_addr:0x%lx, "
+			"cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
+			i, crc->sg_cpu[i].start_addr,
+			crc->sg_cpu[i].cfg, crc->sg_cpu[i].x_count,
+			crc->sg_cpu[i].x_modify);
+		i++;
+
+		if (mid_dma_count) {
+			/* copy extra bytes to next middle dma buffer */
+			dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 |
+				DMAEN | PSIZE_32 | WDSIZE_32;
+			memcpy(crc->sg_mid_buf + (i << 2),
+				(void *)(dma_addr + (dma_count << 2)),
+				mid_dma_count);
+			/* setup new dma descriptor for next middle dma */
+			crc->sg_cpu[i].start_addr = dma_map_single(crc->dev,
+					crc->sg_mid_buf + (i << 2),
+					CHKSUM_DIGEST_SIZE, DMA_TO_DEVICE);
+			crc->sg_cpu[i].cfg = dma_config;
+			crc->sg_cpu[i].x_count = 1;
+			crc->sg_cpu[i].x_modify = CHKSUM_DIGEST_SIZE;
+			dev_dbg(crc->dev, "%d: crc_dma: start_addr:0x%lx, "
+				"cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
+				i, crc->sg_cpu[i].start_addr,
+				crc->sg_cpu[i].cfg, crc->sg_cpu[i].x_count,
+				crc->sg_cpu[i].x_modify);
+			i++;
+		}
+	}
+
+	dma_config = DMAFLOW_ARRAY | RESTART | NDSIZE_3 | DMAEN | PSIZE_32 | WDSIZE_32;
+	/* For final update req, append the buffer for next update as well*/
+	if (ctx->bufnext_len && (ctx->flag == CRC_CRYPTO_STATE_FINALUPDATE ||
+		ctx->flag == CRC_CRYPTO_STATE_FINISH)) {
+		crc->sg_cpu[i].start_addr = dma_map_single(crc->dev, ctx->bufnext,
+						CHKSUM_DIGEST_SIZE, DMA_TO_DEVICE);
+		crc->sg_cpu[i].cfg = dma_config;
+		crc->sg_cpu[i].x_count = 1;
+		crc->sg_cpu[i].x_modify = CHKSUM_DIGEST_SIZE;
+		dev_dbg(crc->dev, "%d: crc_dma: start_addr:0x%lx, "
+			"cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
+			i, crc->sg_cpu[i].start_addr,
+			crc->sg_cpu[i].cfg, crc->sg_cpu[i].x_count,
+			crc->sg_cpu[i].x_modify);
+		i++;
+	}
+
+	if (i == 0)
+		return;
+
+	flush_dcache_range((unsigned int)crc->sg_cpu,
+			(unsigned int)crc->sg_cpu +
+			i * sizeof(struct dma_desc_array));
+
+	/* Set the last descriptor to stop mode */
+	crc->sg_cpu[i - 1].cfg &= ~(DMAFLOW | NDSIZE);
+	crc->sg_cpu[i - 1].cfg |= DI_EN;
+	set_dma_curr_desc_addr(crc->dma_ch, (unsigned long *)crc->sg_dma);
+	set_dma_x_count(crc->dma_ch, 0);
+	set_dma_x_modify(crc->dma_ch, 0);
+	SSYNC();
+	set_dma_config(crc->dma_ch, dma_config);
+}
+
+static int bfin_crypto_crc_handle_queue(struct bfin_crypto_crc *crc,
+				  struct ahash_request *req)
+{
+	struct crypto_async_request *async_req, *backlog;
+	struct bfin_crypto_crc_reqctx *ctx;
+	struct scatterlist *sg;
+	int ret = 0;
+	int nsg, i, j;
+	unsigned int nextlen;
+	unsigned long flags;
+
+	spin_lock_irqsave(&crc->lock, flags);
+	if (req)
+		ret = ahash_enqueue_request(&crc->queue, req);
+	if (crc->busy) {
+		spin_unlock_irqrestore(&crc->lock, flags);
+		return ret;
+	}
+	backlog = crypto_get_backlog(&crc->queue);
+	async_req = crypto_dequeue_request(&crc->queue);
+	if (async_req)
+		crc->busy = 1;
+	spin_unlock_irqrestore(&crc->lock, flags);
+
+	if (!async_req)
+		return ret;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ahash_request_cast(async_req);
+	crc->req = req;
+	ctx = ahash_request_ctx(req);
+	ctx->sg = NULL;
+	ctx->sg_buflen = 0;
+	ctx->sg_nents = 0;
+
+	dev_dbg(crc->dev, "handling new req, flag=%u, nbytes: %d\n",
+						ctx->flag, req->nbytes);
+
+	if (ctx->flag == CRC_CRYPTO_STATE_FINISH) {
+		if (ctx->bufnext_len == 0) {
+			crc->busy = 0;
+			return 0;
+		}
+
+		/* Pack last crc update buffer to 32bit */
+		memset(ctx->bufnext + ctx->bufnext_len, 0,
+				CHKSUM_DIGEST_SIZE - ctx->bufnext_len);
+	} else {
+		/* Pack small data which is less than 32bit to buffer for next update. */
+		if (ctx->bufnext_len + req->nbytes < CHKSUM_DIGEST_SIZE) {
+			memcpy(ctx->bufnext + ctx->bufnext_len,
+				sg_virt(req->src), req->nbytes);
+			ctx->bufnext_len += req->nbytes;
+			if (ctx->flag == CRC_CRYPTO_STATE_FINALUPDATE &&
+				ctx->bufnext_len) {
+				goto finish_update;
+			} else {
+				crc->busy = 0;
+				return 0;
+			}
+		}
+
+		if (ctx->bufnext_len) {
+			/* Chain in extra bytes of last update */
+			ctx->buflast_len = ctx->bufnext_len;
+			memcpy(ctx->buflast, ctx->bufnext, ctx->buflast_len);
+
+			nsg = ctx->sg_buflen ? 2 : 1;
+			sg_init_table(ctx->bufsl, nsg);
+			sg_set_buf(ctx->bufsl, ctx->buflast, ctx->buflast_len);
+			if (nsg > 1)
+				scatterwalk_sg_chain(ctx->bufsl, nsg,
+						req->src);
+			ctx->sg = ctx->bufsl;
+		} else
+			ctx->sg = req->src;
+
+		/* Chop crc buffer size to multiple of 32 bit */
+		nsg = ctx->sg_nents = sg_count(ctx->sg);
+		ctx->sg_buflen = ctx->buflast_len + req->nbytes;
+		ctx->bufnext_len = ctx->sg_buflen % 4;
+		ctx->sg_buflen &= ~0x3;
+
+		if (ctx->bufnext_len) {
+			/* copy extra bytes to buffer for next update */
+			memset(ctx->bufnext, 0, CHKSUM_DIGEST_SIZE);
+			nextlen = ctx->bufnext_len;
+			for (i = nsg - 1; i >= 0; i--) {
+				sg = sg_get(ctx->sg, nsg, i);
+				j = min(nextlen, sg_dma_len(sg));
+				memcpy(ctx->bufnext + nextlen - j,
+					sg_virt(sg) + sg_dma_len(sg) - j, j);
+				if (j == sg_dma_len(sg))
+					ctx->sg_nents--;
+				nextlen -= j;
+				if (nextlen == 0)
+					break;
+			}
+		}
+	}
+
+finish_update:
+	if (ctx->bufnext_len && (ctx->flag == CRC_CRYPTO_STATE_FINALUPDATE ||
+		ctx->flag == CRC_CRYPTO_STATE_FINISH))
+		ctx->sg_buflen += CHKSUM_DIGEST_SIZE;
+
+	/* set CRC data count before start DMA */
+	crc->regs->datacnt = ctx->sg_buflen >> 2;
+
+	/* setup and enable CRC DMA */
+	bfin_crypto_crc_config_dma(crc);
+
+	/* finally kick off CRC operation */
+	crc->regs->control |= BLKEN;
+	SSYNC();
+
+	return -EINPROGRESS;
+}
+
+static int bfin_crypto_crc_update(struct ahash_request *req)
+{
+	struct bfin_crypto_crc_reqctx *ctx = ahash_request_ctx(req);
+
+	if (!req->nbytes)
+		return 0;
+
+	dev_dbg(ctx->crc->dev, "crc_update\n");
+	ctx->total += req->nbytes;
+	ctx->flag = CRC_CRYPTO_STATE_UPDATE;
+
+	return bfin_crypto_crc_handle_queue(ctx->crc, req);
+}
+
+static int bfin_crypto_crc_final(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct bfin_crypto_crc_ctx *crc_ctx = crypto_ahash_ctx(tfm);
+	struct bfin_crypto_crc_reqctx *ctx = ahash_request_ctx(req);
+
+	dev_dbg(ctx->crc->dev, "crc_final\n");
+	ctx->flag = CRC_CRYPTO_STATE_FINISH;
+	crc_ctx->key = 0;
+
+	return bfin_crypto_crc_handle_queue(ctx->crc, req);
+}
+
+static int bfin_crypto_crc_finup(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct bfin_crypto_crc_ctx *crc_ctx = crypto_ahash_ctx(tfm);
+	struct bfin_crypto_crc_reqctx *ctx = ahash_request_ctx(req);
+
+	dev_dbg(ctx->crc->dev, "crc_finishupdate\n");
+	ctx->total += req->nbytes;
+	ctx->flag = CRC_CRYPTO_STATE_FINALUPDATE;
+	crc_ctx->key = 0;
+
+	return bfin_crypto_crc_handle_queue(ctx->crc, req);
+}
+
+static int bfin_crypto_crc_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = bfin_crypto_crc_init(req);
+	if (ret)
+		return ret;
+
+	return bfin_crypto_crc_finup(req);
+}
+
+static int bfin_crypto_crc_setkey(struct crypto_ahash *tfm, const u8 *key,
+			unsigned int keylen)
+{
+	struct bfin_crypto_crc_ctx *crc_ctx = crypto_ahash_ctx(tfm);
+
+	dev_dbg(crc_ctx->crc->dev, "crc_setkey\n");
+	if (keylen != CHKSUM_DIGEST_SIZE) {
+		crypto_ahash_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	crc_ctx->key = get_unaligned_le32(key);
+
+	return 0;
+}
+
+static int bfin_crypto_crc_cra_init(struct crypto_tfm *tfm)
+{
+	struct bfin_crypto_crc_ctx *crc_ctx = crypto_tfm_ctx(tfm);
+
+	crc_ctx->key = 0;
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct bfin_crypto_crc_reqctx));
+
+	return 0;
+}
+
+static void bfin_crypto_crc_cra_exit(struct crypto_tfm *tfm)
+{
+}
+
+static struct ahash_alg algs = {
+	.init		= bfin_crypto_crc_init,
+	.update		= bfin_crypto_crc_update,
+	.final		= bfin_crypto_crc_final,
+	.finup		= bfin_crypto_crc_finup,
+	.digest		= bfin_crypto_crc_digest,
+	.setkey		= bfin_crypto_crc_setkey,
+	.halg.digestsize	= CHKSUM_DIGEST_SIZE,
+	.halg.base	= {
+		.cra_name		= "hmac(crc32)",
+		.cra_driver_name	= DRIVER_NAME,
+		.cra_priority		= 100,
+		.cra_flags		= CRYPTO_ALG_TYPE_AHASH |
+						CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= CHKSUM_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct bfin_crypto_crc_ctx),
+		.cra_alignmask		= 3,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= bfin_crypto_crc_cra_init,
+		.cra_exit		= bfin_crypto_crc_cra_exit,
+	}
+};
+
+static void bfin_crypto_crc_done_task(unsigned long data)
+{
+	struct bfin_crypto_crc *crc = (struct bfin_crypto_crc *)data;
+
+	bfin_crypto_crc_handle_queue(crc, NULL);
+}
+
+static irqreturn_t bfin_crypto_crc_handler(int irq, void *dev_id)
+{
+	struct bfin_crypto_crc *crc = dev_id;
+
+	if (crc->regs->status & DCNTEXP) {
+		crc->regs->status = DCNTEXP;
+		SSYNC();
+
+		/* prepare results */
+		put_unaligned_le32(crc->regs->result, crc->req->result);
+
+		crc->regs->control &= ~BLKEN;
+		crc->busy = 0;
+
+		if (crc->req->base.complete)
+			crc->req->base.complete(&crc->req->base, 0);
+
+		tasklet_schedule(&crc->done_task);
+
+		return IRQ_HANDLED;
+	} else
+		return IRQ_NONE;
+}
+
+#ifdef CONFIG_PM
+/**
+ *	bfin_crypto_crc_suspend - suspend crc device
+ *	@pdev: device being suspended
+ *	@state: requested suspend state
+ */
+static int bfin_crypto_crc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct bfin_crypto_crc *crc = platform_get_drvdata(pdev);
+	int i = 100000;
+
+	while ((crc->regs->control & BLKEN) && --i)
+		cpu_relax();
+
+	if (i == 0)
+		return -EBUSY;
+
+	return 0;
+}
+#else
+# define bfin_crypto_crc_suspend NULL
+#endif
+
+#define bfin_crypto_crc_resume NULL
+
+/**
+ *	bfin_crypto_crc_probe - Initialize module
+ *
+ */
+static int __devinit bfin_crypto_crc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct bfin_crypto_crc *crc;
+	unsigned int timeout = 100000;
+	int ret;
+
+	crc = kzalloc(sizeof(*crc), GFP_KERNEL);
+	if (!crc) {
+		dev_err(&pdev->dev, "fail to malloc bfin_crypto_crc\n");
+		return -ENOMEM;
+	}
+
+	crc->dev = dev;
+
+	INIT_LIST_HEAD(&crc->list);
+	spin_lock_init(&crc->lock);
+	tasklet_init(&crc->done_task, bfin_crypto_crc_done_task, (unsigned long)crc);
+	crypto_init_queue(&crc->queue, CRC_CCRYPTO_QUEUE_LENGTH);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "Cannot get IORESOURCE_MEM\n");
+		ret = -ENOENT;
+		goto out_error_free_mem;
+	}
+
+	crc->regs = ioremap(res->start, resource_size(res));
+	if (!crc->regs) {
+		dev_err(&pdev->dev, "Cannot map CRC IO\n");
+		ret = -ENXIO;
+		goto out_error_free_mem;
+	}
+
+	crc->irq = platform_get_irq(pdev, 0);
+	if (crc->irq < 0) {
+		dev_err(&pdev->dev, "No CRC DCNTEXP IRQ specified\n");
+		ret = -ENOENT;
+		goto out_error_unmap;
+	}
+
+	ret = request_irq(crc->irq, bfin_crypto_crc_handler, IRQF_SHARED, dev_name(dev), crc);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request blackfin crc irq\n");
+		goto out_error_unmap;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_DMA, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "No CRC DMA channel specified\n");
+		ret = -ENOENT;
+		goto out_error_irq;
+	}
+	crc->dma_ch = res->start;
+
+	ret = request_dma(crc->dma_ch, dev_name(dev));
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to attach Blackfin CRC DMA channel\n");
+		goto out_error_irq;
+	}
+
+	crc->sg_cpu = dma_alloc_coherent(&pdev->dev, PAGE_SIZE, &crc->sg_dma, GFP_KERNEL);
+	if (crc->sg_cpu == NULL) {
+		ret = -ENOMEM;
+		goto out_error_dma;
+	}
+	/*
+	 * need at most CRC_MAX_DMA_DESC sg + CRC_MAX_DMA_DESC middle  +
+	 * 1 last + 1 next dma descriptors
+	 */
+	crc->sg_mid_buf = (u8 *)(crc->sg_cpu + ((CRC_MAX_DMA_DESC + 1) << 1));
+
+	crc->regs->control = 0;
+	SSYNC();
+	crc->regs->poly = crc->poly = (u32)pdev->dev.platform_data;
+	SSYNC();
+
+	while (!(crc->regs->status & LUTDONE) && (--timeout) > 0)
+		cpu_relax();
+
+	if (timeout == 0)
+		dev_info(&pdev->dev, "init crc poly timeout\n");
+
+	spin_lock(&crc_list.lock);
+	list_add(&crc->list, &crc_list.dev_list);
+	spin_unlock(&crc_list.lock);
+
+	platform_set_drvdata(pdev, crc);
+
+	ret = crypto_register_ahash(&algs);
+	if (ret) {
+		spin_lock(&crc_list.lock);
+		list_del(&crc->list);
+		spin_unlock(&crc_list.lock);
+		dev_err(&pdev->dev, "Cann't register crypto ahash device\n");
+		goto out_error_dma;
+	}
+
+	dev_info(&pdev->dev, "initialized\n");
+
+	return 0;
+
+out_error_dma:
+	if (crc->sg_cpu)
+		dma_free_coherent(&pdev->dev, PAGE_SIZE, crc->sg_cpu, crc->sg_dma);
+	free_dma(crc->dma_ch);
+out_error_irq:
+	free_irq(crc->irq, crc->dev);
+out_error_unmap:
+	iounmap((void *)crc->regs);
+out_error_free_mem:
+	kfree(crc);
+
+	return ret;
+}
+
+/**
+ *	bfin_crypto_crc_remove - Initialize module
+ *
+ */
+static int __devexit bfin_crypto_crc_remove(struct platform_device *pdev)
+{
+	struct bfin_crypto_crc *crc = platform_get_drvdata(pdev);
+
+	if (!crc)
+		return -ENODEV;
+
+	spin_lock(&crc_list.lock);
+	list_del(&crc->list);
+	spin_unlock(&crc_list.lock);
+
+	crypto_unregister_ahash(&algs);
+	tasklet_kill(&crc->done_task);
+	iounmap((void *)crc->regs);
+	free_dma(crc->dma_ch);
+	if (crc->irq > 0)
+		free_irq(crc->irq, crc->dev);
+	kfree(crc);
+
+	return 0;
+}
+
+static struct platform_driver bfin_crypto_crc_driver = {
+	.probe     = bfin_crypto_crc_probe,
+	.remove    = __devexit_p(bfin_crypto_crc_remove),
+	.suspend   = bfin_crypto_crc_suspend,
+	.resume    = bfin_crypto_crc_resume,
+	.driver    = {
+		.name  = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+/**
+ *	bfin_crypto_crc_mod_init - Initialize module
+ *
+ *	Checks the module params and registers the platform driver.
+ *	Real work is in the platform probe function.
+ */
+static int __init bfin_crypto_crc_mod_init(void)
+{
+	int ret;
+
+	pr_info("Blackfin hardware CRC crypto driver\n");
+
+	INIT_LIST_HEAD(&crc_list.dev_list);
+	spin_lock_init(&crc_list.lock);
+
+	ret = platform_driver_register(&bfin_crypto_crc_driver);
+	if (ret) {
+		pr_info(KERN_ERR "unable to register driver\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ *	bfin_crypto_crc_mod_exit - Deinitialize module
+ */
+static void __exit bfin_crypto_crc_mod_exit(void)
+{
+	platform_driver_unregister(&bfin_crypto_crc_driver);
+}
+
+module_init(bfin_crypto_crc_mod_init);
+module_exit(bfin_crypto_crc_mod_exit);
+
+MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
+MODULE_DESCRIPTION("Blackfin CRC hardware crypto driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/crypto/caam/Kconfig b/drivers/crypto/caam/Kconfig
index 2d876bb98ff..65c7668614a 100644
--- a/drivers/crypto/caam/Kconfig
+++ b/drivers/crypto/caam/Kconfig
@@ -32,10 +32,13 @@ config CRYPTO_DEV_FSL_CAAM_RINGSIZE
 config CRYPTO_DEV_FSL_CAAM_INTC
 	bool "Job Ring interrupt coalescing"
 	depends on CRYPTO_DEV_FSL_CAAM
-	default y
+	default n
 	help
 	  Enable the Job Ring's interrupt coalescing feature.
 
+	  Note: the driver already provides adequate
+	  interrupt coalescing in software.
+
 config CRYPTO_DEV_FSL_CAAM_INTC_COUNT_THLD
 	int "Job Ring interrupt coalescing count threshold"
 	depends on CRYPTO_DEV_FSL_CAAM_INTC
@@ -70,3 +73,28 @@ config CRYPTO_DEV_FSL_CAAM_CRYPTO_API
 
 	  To compile this as a module, choose M here: the module
 	  will be called caamalg.
+
+config CRYPTO_DEV_FSL_CAAM_AHASH_API
+	tristate "Register hash algorithm implementations with Crypto API"
+	depends on CRYPTO_DEV_FSL_CAAM
+	default y
+	select CRYPTO_AHASH
+	help
+	  Selecting this will offload ahash for users of the
+	  scatterlist crypto API to the SEC4 via job ring.
+
+	  To compile this as a module, choose M here: the module
+	  will be called caamhash.
+
+config CRYPTO_DEV_FSL_CAAM_RNG_API
+	tristate "Register caam device for hwrng API"
+	depends on CRYPTO_DEV_FSL_CAAM
+	default y
+	select CRYPTO_RNG
+	select HW_RANDOM
+	help
+	  Selecting this will register the SEC4 hardware rng to
+	  the hw_random API for suppying the kernel entropy pool.
+
+	  To compile this as a module, choose M here: the module
+	  will be called caamrng.
diff --git a/drivers/crypto/caam/Makefile b/drivers/crypto/caam/Makefile
index ef39011b450..b1eb44838db 100644
--- a/drivers/crypto/caam/Makefile
+++ b/drivers/crypto/caam/Makefile
@@ -4,5 +4,7 @@
 
 obj-$(CONFIG_CRYPTO_DEV_FSL_CAAM) += caam.o
 obj-$(CONFIG_CRYPTO_DEV_FSL_CAAM_CRYPTO_API) += caamalg.o
+obj-$(CONFIG_CRYPTO_DEV_FSL_CAAM_AHASH_API) += caamhash.o
+obj-$(CONFIG_CRYPTO_DEV_FSL_CAAM_RNG_API) += caamrng.o
 
-caam-objs := ctrl.o jr.o error.o
+caam-objs := ctrl.o jr.o error.o key_gen.o
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index 4eec389184d..0c1ea8492ef 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -37,9 +37,10 @@
  * | ShareDesc Pointer |
  * | SEQ_OUT_PTR       |
  * | (output buffer)   |
+ * | (output length)   |
  * | SEQ_IN_PTR        |
  * | (input buffer)    |
- * | LOAD (to DECO)    |
+ * | (input length)    |
  * ---------------------
  */
 
@@ -50,6 +51,8 @@
 #include "desc_constr.h"
 #include "jr.h"
 #include "error.h"
+#include "sg_sw_sec4.h"
+#include "key_gen.h"
 
 /*
  * crypto alg
@@ -62,7 +65,7 @@
 #define CAAM_MAX_IV_LENGTH		16
 
 /* length of descriptors text */
-#define DESC_JOB_IO_LEN			(CAAM_CMD_SZ * 3 + CAAM_PTR_SZ * 3)
+#define DESC_JOB_IO_LEN			(CAAM_CMD_SZ * 5 + CAAM_PTR_SZ * 3)
 
 #define DESC_AEAD_BASE			(4 * CAAM_CMD_SZ)
 #define DESC_AEAD_ENC_LEN		(DESC_AEAD_BASE + 16 * CAAM_CMD_SZ)
@@ -143,11 +146,11 @@ static inline void aead_append_ld_iv(u32 *desc, int ivsize)
  */
 static inline void ablkcipher_append_src_dst(u32 *desc)
 {
-	append_math_add(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ); \
-	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ); \
-	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | \
-			     KEY_VLF | FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1); \
-	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | KEY_VLF); \
+	append_math_add(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 |
+			     KEY_VLF | FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1);
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | KEY_VLF);
 }
 
 /*
@@ -452,121 +455,12 @@ static int aead_setauthsize(struct crypto_aead *authenc,
 	return 0;
 }
 
-struct split_key_result {
-	struct completion completion;
-	int err;
-};
-
-static void split_key_done(struct device *dev, u32 *desc, u32 err,
-			   void *context)
+static u32 gen_split_aead_key(struct caam_ctx *ctx, const u8 *key_in,
+			      u32 authkeylen)
 {
-	struct split_key_result *res = context;
-
-#ifdef DEBUG
-	dev_err(dev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
-#endif
-
-	if (err) {
-		char tmp[CAAM_ERROR_STR_MAX];
-
-		dev_err(dev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
-	}
-
-	res->err = err;
-
-	complete(&res->completion);
-}
-
-/*
-get a split ipad/opad key
-
-Split key generation-----------------------------------------------
-
-[00] 0xb0810008    jobdesc: stidx=1 share=never len=8
-[01] 0x04000014        key: class2->keyreg len=20
-			@0xffe01000
-[03] 0x84410014  operation: cls2-op sha1 hmac init dec
-[04] 0x24940000     fifold: class2 msgdata-last2 len=0 imm
-[05] 0xa4000001       jump: class2 local all ->1 [06]
-[06] 0x64260028    fifostr: class2 mdsplit-jdk len=40
-			@0xffe04000
-*/
-static u32 gen_split_key(struct caam_ctx *ctx, const u8 *key_in, u32 authkeylen)
-{
-	struct device *jrdev = ctx->jrdev;
-	u32 *desc;
-	struct split_key_result result;
-	dma_addr_t dma_addr_in, dma_addr_out;
-	int ret = 0;
-
-	desc = kmalloc(CAAM_CMD_SZ * 6 + CAAM_PTR_SZ * 2, GFP_KERNEL | GFP_DMA);
-
-	init_job_desc(desc, 0);
-
-	dma_addr_in = dma_map_single(jrdev, (void *)key_in, authkeylen,
-				     DMA_TO_DEVICE);
-	if (dma_mapping_error(jrdev, dma_addr_in)) {
-		dev_err(jrdev, "unable to map key input memory\n");
-		kfree(desc);
-		return -ENOMEM;
-	}
-	append_key(desc, dma_addr_in, authkeylen, CLASS_2 |
-		       KEY_DEST_CLASS_REG);
-
-	/* Sets MDHA up into an HMAC-INIT */
-	append_operation(desc, ctx->alg_op | OP_ALG_DECRYPT |
-			     OP_ALG_AS_INIT);
-
-	/*
-	 * do a FIFO_LOAD of zero, this will trigger the internal key expansion
-	   into both pads inside MDHA
-	 */
-	append_fifo_load_as_imm(desc, NULL, 0, LDST_CLASS_2_CCB |
-				FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST2);
-
-	/*
-	 * FIFO_STORE with the explicit split-key content store
-	 * (0x26 output type)
-	 */
-	dma_addr_out = dma_map_single(jrdev, ctx->key, ctx->split_key_pad_len,
-				      DMA_FROM_DEVICE);
-	if (dma_mapping_error(jrdev, dma_addr_out)) {
-		dev_err(jrdev, "unable to map key output memory\n");
-		kfree(desc);
-		return -ENOMEM;
-	}
-	append_fifo_store(desc, dma_addr_out, ctx->split_key_len,
-			  LDST_CLASS_2_CCB | FIFOST_TYPE_SPLIT_KEK);
-
-#ifdef DEBUG
-	print_hex_dump(KERN_ERR, "ctx.key@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, key_in, authkeylen, 1);
-	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
-#endif
-
-	result.err = 0;
-	init_completion(&result.completion);
-
-	ret = caam_jr_enqueue(jrdev, desc, split_key_done, &result);
-	if (!ret) {
-		/* in progress */
-		wait_for_completion_interruptible(&result.completion);
-		ret = result.err;
-#ifdef DEBUG
-		print_hex_dump(KERN_ERR, "ctx.key@"xstr(__LINE__)": ",
-			       DUMP_PREFIX_ADDRESS, 16, 4, ctx->key,
-			       ctx->split_key_pad_len, 1);
-#endif
-	}
-
-	dma_unmap_single(jrdev, dma_addr_out, ctx->split_key_pad_len,
-			 DMA_FROM_DEVICE);
-	dma_unmap_single(jrdev, dma_addr_in, authkeylen, DMA_TO_DEVICE);
-
-	kfree(desc);
-
-	return ret;
+	return gen_split_key(ctx->jrdev, ctx->key, ctx->split_key_len,
+			       ctx->split_key_pad_len, key_in, authkeylen,
+			       ctx->alg_op);
 }
 
 static int aead_setkey(struct crypto_aead *aead,
@@ -610,7 +504,7 @@ static int aead_setkey(struct crypto_aead *aead,
 		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
 #endif
 
-	ret = gen_split_key(ctx, key, authkeylen);
+	ret = gen_split_aead_key(ctx, key, authkeylen);
 	if (ret) {
 		goto badkey;
 	}
@@ -757,72 +651,78 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
 	return ret;
 }
 
-struct link_tbl_entry {
-	u64 ptr;
-	u32 len;
-	u8 reserved;
-	u8 buf_pool_id;
-	u16 offset;
-};
-
 /*
  * aead_edesc - s/w-extended aead descriptor
  * @assoc_nents: number of segments in associated data (SPI+Seq) scatterlist
+ * @assoc_chained: if source is chained
  * @src_nents: number of segments in input scatterlist
+ * @src_chained: if source is chained
  * @dst_nents: number of segments in output scatterlist
+ * @dst_chained: if destination is chained
  * @iv_dma: dma address of iv for checking continuity and link table
  * @desc: h/w descriptor (variable length; must not exceed MAX_CAAM_DESCSIZE)
- * @link_tbl_bytes: length of dma mapped link_tbl space
- * @link_tbl_dma: bus physical mapped address of h/w link table
+ * @sec4_sg_bytes: length of dma mapped sec4_sg space
+ * @sec4_sg_dma: bus physical mapped address of h/w link table
  * @hw_desc: the h/w job descriptor followed by any referenced link tables
  */
 struct aead_edesc {
 	int assoc_nents;
+	bool assoc_chained;
 	int src_nents;
+	bool src_chained;
 	int dst_nents;
+	bool dst_chained;
 	dma_addr_t iv_dma;
-	int link_tbl_bytes;
-	dma_addr_t link_tbl_dma;
-	struct link_tbl_entry *link_tbl;
+	int sec4_sg_bytes;
+	dma_addr_t sec4_sg_dma;
+	struct sec4_sg_entry *sec4_sg;
 	u32 hw_desc[0];
 };
 
 /*
  * ablkcipher_edesc - s/w-extended ablkcipher descriptor
  * @src_nents: number of segments in input scatterlist
+ * @src_chained: if source is chained
  * @dst_nents: number of segments in output scatterlist
+ * @dst_chained: if destination is chained
  * @iv_dma: dma address of iv for checking continuity and link table
  * @desc: h/w descriptor (variable length; must not exceed MAX_CAAM_DESCSIZE)
- * @link_tbl_bytes: length of dma mapped link_tbl space
- * @link_tbl_dma: bus physical mapped address of h/w link table
+ * @sec4_sg_bytes: length of dma mapped sec4_sg space
+ * @sec4_sg_dma: bus physical mapped address of h/w link table
  * @hw_desc: the h/w job descriptor followed by any referenced link tables
  */
 struct ablkcipher_edesc {
 	int src_nents;
+	bool src_chained;
 	int dst_nents;
+	bool dst_chained;
 	dma_addr_t iv_dma;
-	int link_tbl_bytes;
-	dma_addr_t link_tbl_dma;
-	struct link_tbl_entry *link_tbl;
+	int sec4_sg_bytes;
+	dma_addr_t sec4_sg_dma;
+	struct sec4_sg_entry *sec4_sg;
 	u32 hw_desc[0];
 };
 
 static void caam_unmap(struct device *dev, struct scatterlist *src,
-		       struct scatterlist *dst, int src_nents, int dst_nents,
-		       dma_addr_t iv_dma, int ivsize, dma_addr_t link_tbl_dma,
-		       int link_tbl_bytes)
+		       struct scatterlist *dst, int src_nents,
+		       bool src_chained, int dst_nents, bool dst_chained,
+		       dma_addr_t iv_dma, int ivsize, dma_addr_t sec4_sg_dma,
+		       int sec4_sg_bytes)
 {
-	if (unlikely(dst != src)) {
-		dma_unmap_sg(dev, src, src_nents, DMA_TO_DEVICE);
-		dma_unmap_sg(dev, dst, dst_nents, DMA_FROM_DEVICE);
+	if (dst != src) {
+		dma_unmap_sg_chained(dev, src, src_nents ? : 1, DMA_TO_DEVICE,
+				     src_chained);
+		dma_unmap_sg_chained(dev, dst, dst_nents ? : 1, DMA_FROM_DEVICE,
+				     dst_chained);
 	} else {
-		dma_unmap_sg(dev, src, src_nents, DMA_BIDIRECTIONAL);
+		dma_unmap_sg_chained(dev, src, src_nents ? : 1,
+				     DMA_BIDIRECTIONAL, src_chained);
 	}
 
 	if (iv_dma)
 		dma_unmap_single(dev, iv_dma, ivsize, DMA_TO_DEVICE);
-	if (link_tbl_bytes)
-		dma_unmap_single(dev, link_tbl_dma, link_tbl_bytes,
+	if (sec4_sg_bytes)
+		dma_unmap_single(dev, sec4_sg_dma, sec4_sg_bytes,
 				 DMA_TO_DEVICE);
 }
 
@@ -833,12 +733,13 @@ static void aead_unmap(struct device *dev,
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	int ivsize = crypto_aead_ivsize(aead);
 
-	dma_unmap_sg(dev, req->assoc, edesc->assoc_nents, DMA_TO_DEVICE);
+	dma_unmap_sg_chained(dev, req->assoc, edesc->assoc_nents,
+			     DMA_TO_DEVICE, edesc->assoc_chained);
 
 	caam_unmap(dev, req->src, req->dst,
-		   edesc->src_nents, edesc->dst_nents,
-		   edesc->iv_dma, ivsize, edesc->link_tbl_dma,
-		   edesc->link_tbl_bytes);
+		   edesc->src_nents, edesc->src_chained, edesc->dst_nents,
+		   edesc->dst_chained, edesc->iv_dma, ivsize,
+		   edesc->sec4_sg_dma, edesc->sec4_sg_bytes);
 }
 
 static void ablkcipher_unmap(struct device *dev,
@@ -849,9 +750,9 @@ static void ablkcipher_unmap(struct device *dev,
 	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
 
 	caam_unmap(dev, req->src, req->dst,
-		   edesc->src_nents, edesc->dst_nents,
-		   edesc->iv_dma, ivsize, edesc->link_tbl_dma,
-		   edesc->link_tbl_bytes);
+		   edesc->src_nents, edesc->src_chained, edesc->dst_nents,
+		   edesc->dst_chained, edesc->iv_dma, ivsize,
+		   edesc->sec4_sg_dma, edesc->sec4_sg_bytes);
 }
 
 static void aead_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
@@ -942,7 +843,7 @@ static void aead_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 		       sizeof(struct iphdr) + req->assoclen +
 		       ((req->cryptlen > 1500) ? 1500 : req->cryptlen) +
 		       ctx->authsize + 36, 1);
-	if (!err && edesc->link_tbl_bytes) {
+	if (!err && edesc->sec4_sg_bytes) {
 		struct scatterlist *sg = sg_last(req->src, edesc->src_nents);
 		print_hex_dump(KERN_ERR, "sglastout@"xstr(__LINE__)": ",
 			       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(sg),
@@ -1026,50 +927,6 @@ static void ablkcipher_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	ablkcipher_request_complete(req, err);
 }
 
-static void sg_to_link_tbl_one(struct link_tbl_entry *link_tbl_ptr,
-			       dma_addr_t dma, u32 len, u32 offset)
-{
-	link_tbl_ptr->ptr = dma;
-	link_tbl_ptr->len = len;
-	link_tbl_ptr->reserved = 0;
-	link_tbl_ptr->buf_pool_id = 0;
-	link_tbl_ptr->offset = offset;
-#ifdef DEBUG
-	print_hex_dump(KERN_ERR, "link_tbl_ptr@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, link_tbl_ptr,
-		       sizeof(struct link_tbl_entry), 1);
-#endif
-}
-
-/*
- * convert scatterlist to h/w link table format
- * but does not have final bit; instead, returns last entry
- */
-static struct link_tbl_entry *sg_to_link_tbl(struct scatterlist *sg,
-					     int sg_count, struct link_tbl_entry
-					     *link_tbl_ptr, u32 offset)
-{
-	while (sg_count) {
-		sg_to_link_tbl_one(link_tbl_ptr, sg_dma_address(sg),
-				   sg_dma_len(sg), offset);
-		link_tbl_ptr++;
-		sg = sg_next(sg);
-		sg_count--;
-	}
-	return link_tbl_ptr - 1;
-}
-
-/*
- * convert scatterlist to h/w link table format
- * scatterlist must have been previously dma mapped
- */
-static void sg_to_link_tbl_last(struct scatterlist *sg, int sg_count,
-				struct link_tbl_entry *link_tbl_ptr, u32 offset)
-{
-	link_tbl_ptr = sg_to_link_tbl(sg, sg_count, link_tbl_ptr, offset);
-	link_tbl_ptr->len |= 0x40000000;
-}
-
 /*
  * Fill in aead job descriptor
  */
@@ -1085,7 +942,7 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 	u32 *desc = edesc->hw_desc;
 	u32 out_options = 0, in_options;
 	dma_addr_t dst_dma, src_dma;
-	int len, link_tbl_index = 0;
+	int len, sec4_sg_index = 0;
 
 #ifdef DEBUG
 	debug("assoclen %d cryptlen %d authsize %d\n",
@@ -1111,9 +968,9 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 		src_dma = sg_dma_address(req->assoc);
 		in_options = 0;
 	} else {
-		src_dma = edesc->link_tbl_dma;
-		link_tbl_index += (edesc->assoc_nents ? : 1) + 1 +
-				  (edesc->src_nents ? : 1);
+		src_dma = edesc->sec4_sg_dma;
+		sec4_sg_index += (edesc->assoc_nents ? : 1) + 1 +
+				 (edesc->src_nents ? : 1);
 		in_options = LDST_SGF;
 	}
 	if (encrypt)
@@ -1127,7 +984,7 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 		if (all_contig) {
 			dst_dma = sg_dma_address(req->src);
 		} else {
-			dst_dma = src_dma + sizeof(struct link_tbl_entry) *
+			dst_dma = src_dma + sizeof(struct sec4_sg_entry) *
 				  ((edesc->assoc_nents ? : 1) + 1);
 			out_options = LDST_SGF;
 		}
@@ -1135,9 +992,9 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 		if (!edesc->dst_nents) {
 			dst_dma = sg_dma_address(req->dst);
 		} else {
-			dst_dma = edesc->link_tbl_dma +
-				  link_tbl_index *
-				  sizeof(struct link_tbl_entry);
+			dst_dma = edesc->sec4_sg_dma +
+				  sec4_sg_index *
+				  sizeof(struct sec4_sg_entry);
 			out_options = LDST_SGF;
 		}
 	}
@@ -1163,7 +1020,7 @@ static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
 	u32 *desc = edesc->hw_desc;
 	u32 out_options = 0, in_options;
 	dma_addr_t dst_dma, src_dma;
-	int len, link_tbl_index = 0;
+	int len, sec4_sg_index = 0;
 
 #ifdef DEBUG
 	debug("assoclen %d cryptlen %d authsize %d\n",
@@ -1188,8 +1045,8 @@ static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
 		src_dma = sg_dma_address(req->assoc);
 		in_options = 0;
 	} else {
-		src_dma = edesc->link_tbl_dma;
-		link_tbl_index += edesc->assoc_nents + 1 + edesc->src_nents;
+		src_dma = edesc->sec4_sg_dma;
+		sec4_sg_index += edesc->assoc_nents + 1 + edesc->src_nents;
 		in_options = LDST_SGF;
 	}
 	append_seq_in_ptr(desc, src_dma, req->assoclen + ivsize +
@@ -1199,13 +1056,13 @@ static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
 		dst_dma = edesc->iv_dma;
 	} else {
 		if (likely(req->src == req->dst)) {
-			dst_dma = src_dma + sizeof(struct link_tbl_entry) *
+			dst_dma = src_dma + sizeof(struct sec4_sg_entry) *
 				  edesc->assoc_nents;
 			out_options = LDST_SGF;
 		} else {
-			dst_dma = edesc->link_tbl_dma +
-				  link_tbl_index *
-				  sizeof(struct link_tbl_entry);
+			dst_dma = edesc->sec4_sg_dma +
+				  sec4_sg_index *
+				  sizeof(struct sec4_sg_entry);
 			out_options = LDST_SGF;
 		}
 	}
@@ -1226,7 +1083,7 @@ static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
 	u32 *desc = edesc->hw_desc;
 	u32 out_options = 0, in_options;
 	dma_addr_t dst_dma, src_dma;
-	int len, link_tbl_index = 0;
+	int len, sec4_sg_index = 0;
 
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "presciv@"xstr(__LINE__)": ",
@@ -1244,8 +1101,8 @@ static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
 		src_dma = edesc->iv_dma;
 		in_options = 0;
 	} else {
-		src_dma = edesc->link_tbl_dma;
-		link_tbl_index += (iv_contig ? 0 : 1) + edesc->src_nents;
+		src_dma = edesc->sec4_sg_dma;
+		sec4_sg_index += (iv_contig ? 0 : 1) + edesc->src_nents;
 		in_options = LDST_SGF;
 	}
 	append_seq_in_ptr(desc, src_dma, req->nbytes + ivsize, in_options);
@@ -1254,16 +1111,16 @@ static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
 		if (!edesc->src_nents && iv_contig) {
 			dst_dma = sg_dma_address(req->src);
 		} else {
-			dst_dma = edesc->link_tbl_dma +
-				sizeof(struct link_tbl_entry);
+			dst_dma = edesc->sec4_sg_dma +
+				sizeof(struct sec4_sg_entry);
 			out_options = LDST_SGF;
 		}
 	} else {
 		if (!edesc->dst_nents) {
 			dst_dma = sg_dma_address(req->dst);
 		} else {
-			dst_dma = edesc->link_tbl_dma +
-				link_tbl_index * sizeof(struct link_tbl_entry);
+			dst_dma = edesc->sec4_sg_dma +
+				sec4_sg_index * sizeof(struct sec4_sg_entry);
 			out_options = LDST_SGF;
 		}
 	}
@@ -1271,28 +1128,6 @@ static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
 }
 
 /*
- * derive number of elements in 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;
-		if (!sg_is_last(sg) && (sg + 1)->length == 0)
-			BUG(); /* Not support chaining */
-		sg = scatterwalk_sg_next(sg);
-	}
-
-	if (likely(sg_nents == 1))
-		return 0;
-
-	return sg_nents;
-}
-
-/*
  * allocate and map the aead extended descriptor
  */
 static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
@@ -1308,25 +1143,26 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 	dma_addr_t iv_dma = 0;
 	int sgc;
 	bool all_contig = true;
+	bool assoc_chained = false, src_chained = false, dst_chained = false;
 	int ivsize = crypto_aead_ivsize(aead);
-	int link_tbl_index, link_tbl_len = 0, link_tbl_bytes;
+	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
 
-	assoc_nents = sg_count(req->assoc, req->assoclen);
-	src_nents = sg_count(req->src, req->cryptlen);
+	assoc_nents = sg_count(req->assoc, req->assoclen, &assoc_chained);
+	src_nents = sg_count(req->src, req->cryptlen, &src_chained);
 
 	if (unlikely(req->dst != req->src))
-		dst_nents = sg_count(req->dst, req->cryptlen);
+		dst_nents = sg_count(req->dst, req->cryptlen, &dst_chained);
 
-	sgc = dma_map_sg(jrdev, req->assoc, assoc_nents ? : 1,
-			 DMA_BIDIRECTIONAL);
+	sgc = dma_map_sg_chained(jrdev, req->assoc, assoc_nents ? : 1,
+				 DMA_BIDIRECTIONAL, assoc_chained);
 	if (likely(req->src == req->dst)) {
-		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
-				 DMA_BIDIRECTIONAL);
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_BIDIRECTIONAL, src_chained);
 	} else {
-		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
-				 DMA_TO_DEVICE);
-		sgc = dma_map_sg(jrdev, req->dst, dst_nents ? : 1,
-				 DMA_FROM_DEVICE);
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_TO_DEVICE, src_chained);
+		sgc = dma_map_sg_chained(jrdev, req->dst, dst_nents ? : 1,
+					 DMA_FROM_DEVICE, dst_chained);
 	}
 
 	/* Check if data are contiguous */
@@ -1337,50 +1173,53 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 		all_contig = false;
 		assoc_nents = assoc_nents ? : 1;
 		src_nents = src_nents ? : 1;
-		link_tbl_len = assoc_nents + 1 + src_nents;
+		sec4_sg_len = assoc_nents + 1 + src_nents;
 	}
-	link_tbl_len += dst_nents;
+	sec4_sg_len += dst_nents;
 
-	link_tbl_bytes = link_tbl_len * sizeof(struct link_tbl_entry);
+	sec4_sg_bytes = sec4_sg_len * sizeof(struct sec4_sg_entry);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
 	edesc = kmalloc(sizeof(struct aead_edesc) + desc_bytes +
-			link_tbl_bytes, GFP_DMA | flags);
+			sec4_sg_bytes, GFP_DMA | flags);
 	if (!edesc) {
 		dev_err(jrdev, "could not allocate extended descriptor\n");
 		return ERR_PTR(-ENOMEM);
 	}
 
 	edesc->assoc_nents = assoc_nents;
+	edesc->assoc_chained = assoc_chained;
 	edesc->src_nents = src_nents;
+	edesc->src_chained = src_chained;
 	edesc->dst_nents = dst_nents;
+	edesc->dst_chained = dst_chained;
 	edesc->iv_dma = iv_dma;
-	edesc->link_tbl_bytes = link_tbl_bytes;
-	edesc->link_tbl = (void *)edesc + sizeof(struct aead_edesc) +
-			  desc_bytes;
-	edesc->link_tbl_dma = dma_map_single(jrdev, edesc->link_tbl,
-					     link_tbl_bytes, DMA_TO_DEVICE);
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct aead_edesc) +
+			 desc_bytes;
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
 	*all_contig_ptr = all_contig;
 
-	link_tbl_index = 0;
+	sec4_sg_index = 0;
 	if (!all_contig) {
-		sg_to_link_tbl(req->assoc,
-			       (assoc_nents ? : 1),
-			       edesc->link_tbl +
-			       link_tbl_index, 0);
-		link_tbl_index += assoc_nents ? : 1;
-		sg_to_link_tbl_one(edesc->link_tbl + link_tbl_index,
+		sg_to_sec4_sg(req->assoc,
+			      (assoc_nents ? : 1),
+			      edesc->sec4_sg +
+			      sec4_sg_index, 0);
+		sec4_sg_index += assoc_nents ? : 1;
+		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
 				   iv_dma, ivsize, 0);
-		link_tbl_index += 1;
-		sg_to_link_tbl_last(req->src,
-				    (src_nents ? : 1),
-				    edesc->link_tbl +
-				    link_tbl_index, 0);
-		link_tbl_index += src_nents ? : 1;
+		sec4_sg_index += 1;
+		sg_to_sec4_sg_last(req->src,
+				   (src_nents ? : 1),
+				   edesc->sec4_sg +
+				   sec4_sg_index, 0);
+		sec4_sg_index += src_nents ? : 1;
 	}
 	if (dst_nents) {
-		sg_to_link_tbl_last(req->dst, dst_nents,
-				    edesc->link_tbl + link_tbl_index, 0);
+		sg_to_sec4_sg_last(req->dst, dst_nents,
+				   edesc->sec4_sg + sec4_sg_index, 0);
 	}
 
 	return edesc;
@@ -1487,24 +1326,25 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 	int sgc;
 	u32 contig = GIV_SRC_CONTIG | GIV_DST_CONTIG;
 	int ivsize = crypto_aead_ivsize(aead);
-	int link_tbl_index, link_tbl_len = 0, link_tbl_bytes;
+	bool assoc_chained = false, src_chained = false, dst_chained = false;
+	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
 
-	assoc_nents = sg_count(req->assoc, req->assoclen);
-	src_nents = sg_count(req->src, req->cryptlen);
+	assoc_nents = sg_count(req->assoc, req->assoclen, &assoc_chained);
+	src_nents = sg_count(req->src, req->cryptlen, &src_chained);
 
 	if (unlikely(req->dst != req->src))
-		dst_nents = sg_count(req->dst, req->cryptlen);
+		dst_nents = sg_count(req->dst, req->cryptlen, &dst_chained);
 
-	sgc = dma_map_sg(jrdev, req->assoc, assoc_nents ? : 1,
-			 DMA_BIDIRECTIONAL);
+	sgc = dma_map_sg_chained(jrdev, req->assoc, assoc_nents ? : 1,
+				 DMA_BIDIRECTIONAL, assoc_chained);
 	if (likely(req->src == req->dst)) {
-		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
-				 DMA_BIDIRECTIONAL);
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_BIDIRECTIONAL, src_chained);
 	} else {
-		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
-				 DMA_TO_DEVICE);
-		sgc = dma_map_sg(jrdev, req->dst, dst_nents ? : 1,
-				 DMA_FROM_DEVICE);
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_TO_DEVICE, src_chained);
+		sgc = dma_map_sg_chained(jrdev, req->dst, dst_nents ? : 1,
+					 DMA_FROM_DEVICE, dst_chained);
 	}
 
 	/* Check if data are contiguous */
@@ -1516,58 +1356,61 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 		contig &= ~GIV_DST_CONTIG;
 		if (unlikely(req->src != req->dst)) {
 			dst_nents = dst_nents ? : 1;
-			link_tbl_len += 1;
+			sec4_sg_len += 1;
 		}
 	if (!(contig & GIV_SRC_CONTIG)) {
 		assoc_nents = assoc_nents ? : 1;
 		src_nents = src_nents ? : 1;
-		link_tbl_len += assoc_nents + 1 + src_nents;
+		sec4_sg_len += assoc_nents + 1 + src_nents;
 		if (likely(req->src == req->dst))
 			contig &= ~GIV_DST_CONTIG;
 	}
-	link_tbl_len += dst_nents;
+	sec4_sg_len += dst_nents;
 
-	link_tbl_bytes = link_tbl_len * sizeof(struct link_tbl_entry);
+	sec4_sg_bytes = sec4_sg_len * sizeof(struct sec4_sg_entry);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
 	edesc = kmalloc(sizeof(struct aead_edesc) + desc_bytes +
-			link_tbl_bytes, GFP_DMA | flags);
+			sec4_sg_bytes, GFP_DMA | flags);
 	if (!edesc) {
 		dev_err(jrdev, "could not allocate extended descriptor\n");
 		return ERR_PTR(-ENOMEM);
 	}
 
 	edesc->assoc_nents = assoc_nents;
+	edesc->assoc_chained = assoc_chained;
 	edesc->src_nents = src_nents;
+	edesc->src_chained = src_chained;
 	edesc->dst_nents = dst_nents;
+	edesc->dst_chained = dst_chained;
 	edesc->iv_dma = iv_dma;
-	edesc->link_tbl_bytes = link_tbl_bytes;
-	edesc->link_tbl = (void *)edesc + sizeof(struct aead_edesc) +
-			  desc_bytes;
-	edesc->link_tbl_dma = dma_map_single(jrdev, edesc->link_tbl,
-					     link_tbl_bytes, DMA_TO_DEVICE);
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct aead_edesc) +
+			 desc_bytes;
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
 	*contig_ptr = contig;
 
-	link_tbl_index = 0;
+	sec4_sg_index = 0;
 	if (!(contig & GIV_SRC_CONTIG)) {
-		sg_to_link_tbl(req->assoc, assoc_nents,
-			       edesc->link_tbl +
-			       link_tbl_index, 0);
-		link_tbl_index += assoc_nents;
-		sg_to_link_tbl_one(edesc->link_tbl + link_tbl_index,
+		sg_to_sec4_sg(req->assoc, assoc_nents,
+			      edesc->sec4_sg +
+			      sec4_sg_index, 0);
+		sec4_sg_index += assoc_nents;
+		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
 				   iv_dma, ivsize, 0);
-		link_tbl_index += 1;
-		sg_to_link_tbl_last(req->src, src_nents,
-				    edesc->link_tbl +
-				    link_tbl_index, 0);
-		link_tbl_index += src_nents;
+		sec4_sg_index += 1;
+		sg_to_sec4_sg_last(req->src, src_nents,
+				   edesc->sec4_sg +
+				   sec4_sg_index, 0);
+		sec4_sg_index += src_nents;
 	}
 	if (unlikely(req->src != req->dst && !(contig & GIV_DST_CONTIG))) {
-		sg_to_link_tbl_one(edesc->link_tbl + link_tbl_index,
+		dma_to_sec4_sg_one(edesc->sec4_sg + sec4_sg_index,
 				   iv_dma, ivsize, 0);
-		link_tbl_index += 1;
-		sg_to_link_tbl_last(req->dst, dst_nents,
-				    edesc->link_tbl + link_tbl_index, 0);
+		sec4_sg_index += 1;
+		sg_to_sec4_sg_last(req->dst, dst_nents,
+				   edesc->sec4_sg + sec4_sg_index, 0);
 	}
 
 	return edesc;
@@ -1633,27 +1476,28 @@ static struct ablkcipher_edesc *ablkcipher_edesc_alloc(struct ablkcipher_request
 	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
 					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
 		       GFP_KERNEL : GFP_ATOMIC;
-	int src_nents, dst_nents = 0, link_tbl_bytes;
+	int src_nents, dst_nents = 0, sec4_sg_bytes;
 	struct ablkcipher_edesc *edesc;
 	dma_addr_t iv_dma = 0;
 	bool iv_contig = false;
 	int sgc;
 	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
-	int link_tbl_index;
+	bool src_chained = false, dst_chained = false;
+	int sec4_sg_index;
 
-	src_nents = sg_count(req->src, req->nbytes);
+	src_nents = sg_count(req->src, req->nbytes, &src_chained);
 
-	if (unlikely(req->dst != req->src))
-		dst_nents = sg_count(req->dst, req->nbytes);
+	if (req->dst != req->src)
+		dst_nents = sg_count(req->dst, req->nbytes, &dst_chained);
 
 	if (likely(req->src == req->dst)) {
-		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
-				 DMA_BIDIRECTIONAL);
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_BIDIRECTIONAL, src_chained);
 	} else {
-		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
-				 DMA_TO_DEVICE);
-		sgc = dma_map_sg(jrdev, req->dst, dst_nents ? : 1,
-				 DMA_FROM_DEVICE);
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_TO_DEVICE, src_chained);
+		sgc = dma_map_sg_chained(jrdev, req->dst, dst_nents ? : 1,
+					 DMA_FROM_DEVICE, dst_chained);
 	}
 
 	/*
@@ -1665,44 +1509,46 @@ static struct ablkcipher_edesc *ablkcipher_edesc_alloc(struct ablkcipher_request
 		iv_contig = true;
 	else
 		src_nents = src_nents ? : 1;
-	link_tbl_bytes = ((iv_contig ? 0 : 1) + src_nents + dst_nents) *
-			 sizeof(struct link_tbl_entry);
+	sec4_sg_bytes = ((iv_contig ? 0 : 1) + src_nents + dst_nents) *
+			sizeof(struct sec4_sg_entry);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
 	edesc = kmalloc(sizeof(struct ablkcipher_edesc) + desc_bytes +
-			link_tbl_bytes, GFP_DMA | flags);
+			sec4_sg_bytes, GFP_DMA | flags);
 	if (!edesc) {
 		dev_err(jrdev, "could not allocate extended descriptor\n");
 		return ERR_PTR(-ENOMEM);
 	}
 
 	edesc->src_nents = src_nents;
+	edesc->src_chained = src_chained;
 	edesc->dst_nents = dst_nents;
-	edesc->link_tbl_bytes = link_tbl_bytes;
-	edesc->link_tbl = (void *)edesc + sizeof(struct ablkcipher_edesc) +
-			  desc_bytes;
+	edesc->dst_chained = dst_chained;
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct ablkcipher_edesc) +
+			 desc_bytes;
 
-	link_tbl_index = 0;
+	sec4_sg_index = 0;
 	if (!iv_contig) {
-		sg_to_link_tbl_one(edesc->link_tbl, iv_dma, ivsize, 0);
-		sg_to_link_tbl_last(req->src, src_nents,
-				    edesc->link_tbl + 1, 0);
-		link_tbl_index += 1 + src_nents;
+		dma_to_sec4_sg_one(edesc->sec4_sg, iv_dma, ivsize, 0);
+		sg_to_sec4_sg_last(req->src, src_nents,
+				   edesc->sec4_sg + 1, 0);
+		sec4_sg_index += 1 + src_nents;
 	}
 
-	if (unlikely(dst_nents)) {
-		sg_to_link_tbl_last(req->dst, dst_nents,
-			edesc->link_tbl + link_tbl_index, 0);
+	if (dst_nents) {
+		sg_to_sec4_sg_last(req->dst, dst_nents,
+			edesc->sec4_sg + sec4_sg_index, 0);
 	}
 
-	edesc->link_tbl_dma = dma_map_single(jrdev, edesc->link_tbl,
-					     link_tbl_bytes, DMA_TO_DEVICE);
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
 	edesc->iv_dma = iv_dma;
 
 #ifdef DEBUG
-	print_hex_dump(KERN_ERR, "ablkcipher link_tbl@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->link_tbl,
-		       link_tbl_bytes, 1);
+	print_hex_dump(KERN_ERR, "ablkcipher sec4_sg@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->sec4_sg,
+		       sec4_sg_bytes, 1);
 #endif
 
 	*iv_contig_out = iv_contig;
@@ -2227,7 +2073,7 @@ static int caam_cra_init(struct crypto_tfm *tfm)
 	 * distribute tfms across job rings to ensure in-order
 	 * crypto request processing per tfm
 	 */
-	ctx->jrdev = priv->algapi_jr[(tgt_jr / 2) % priv->num_jrs_for_algapi];
+	ctx->jrdev = priv->jrdev[(tgt_jr / 2) % priv->total_jobrs];
 
 	/* copy descriptor header template value */
 	ctx->class1_alg_type = OP_TYPE_CLASS1_ALG | caam_alg->class1_alg_type;
@@ -2264,7 +2110,6 @@ static void __exit caam_algapi_exit(void)
 	struct device *ctrldev;
 	struct caam_drv_private *priv;
 	struct caam_crypto_alg *t_alg, *n;
-	int i, err;
 
 	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
 	if (!dev_node) {
@@ -2289,13 +2134,6 @@ static void __exit caam_algapi_exit(void)
 		list_del(&t_alg->entry);
 		kfree(t_alg);
 	}
-
-	for (i = 0; i < priv->total_jobrs; i++) {
-		err = caam_jr_deregister(priv->algapi_jr[i]);
-		if (err < 0)
-			break;
-	}
-	kfree(priv->algapi_jr);
 }
 
 static struct caam_crypto_alg *caam_alg_alloc(struct device *ctrldev,
@@ -2348,7 +2186,7 @@ static int __init caam_algapi_init(void)
 {
 	struct device_node *dev_node;
 	struct platform_device *pdev;
-	struct device *ctrldev, **jrdev;
+	struct device *ctrldev;
 	struct caam_drv_private *priv;
 	int i = 0, err = 0;
 
@@ -2369,24 +2207,6 @@ static int __init caam_algapi_init(void)
 
 	INIT_LIST_HEAD(&priv->alg_list);
 
-	jrdev = kmalloc(sizeof(*jrdev) * priv->total_jobrs, GFP_KERNEL);
-	if (!jrdev)
-		return -ENOMEM;
-
-	for (i = 0; i < priv->total_jobrs; i++) {
-		err = caam_jr_register(ctrldev, &jrdev[i]);
-		if (err < 0)
-			break;
-	}
-	if (err < 0 && i == 0) {
-		dev_err(ctrldev, "algapi error in job ring registration: %d\n",
-			err);
-		kfree(jrdev);
-		return err;
-	}
-
-	priv->num_jrs_for_algapi = i;
-	priv->algapi_jr = jrdev;
 	atomic_set(&priv->tfm_count, -1);
 
 	/* register crypto algorithms the device supports */
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
new file mode 100644
index 00000000000..895aaf2bca9
--- /dev/null
+++ b/drivers/crypto/caam/caamhash.c
@@ -0,0 +1,1878 @@
+/*
+ * caam - Freescale FSL CAAM support for ahash functions of crypto API
+ *
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * Based on caamalg.c crypto API driver.
+ *
+ * relationship of digest job descriptor or first job descriptor after init to
+ * shared descriptors:
+ *
+ * ---------------                     ---------------
+ * | JobDesc #1  |-------------------->|  ShareDesc  |
+ * | *(packet 1) |                     |  (hashKey)  |
+ * ---------------                     | (operation) |
+ *                                     ---------------
+ *
+ * relationship of subsequent job descriptors to shared descriptors:
+ *
+ * ---------------                     ---------------
+ * | JobDesc #2  |-------------------->|  ShareDesc  |
+ * | *(packet 2) |      |------------->|  (hashKey)  |
+ * ---------------      |    |-------->| (operation) |
+ *       .              |    |         | (load ctx2) |
+ *       .              |    |         ---------------
+ * ---------------      |    |
+ * | JobDesc #3  |------|    |
+ * | *(packet 3) |           |
+ * ---------------           |
+ *       .                   |
+ *       .                   |
+ * ---------------           |
+ * | JobDesc #4  |------------
+ * | *(packet 4) |
+ * ---------------
+ *
+ * The SharedDesc never changes for a connection unless rekeyed, but
+ * each packet will likely be in a different place. So all we need
+ * to know to process the packet is where the input is, where the
+ * output goes, and what context we want to process with. Context is
+ * in the SharedDesc, packet references in the JobDesc.
+ *
+ * So, a job desc looks like:
+ *
+ * ---------------------
+ * | Header            |
+ * | ShareDesc Pointer |
+ * | SEQ_OUT_PTR       |
+ * | (output buffer)   |
+ * | (output length)   |
+ * | SEQ_IN_PTR        |
+ * | (input buffer)    |
+ * | (input length)    |
+ * ---------------------
+ */
+
+#include "compat.h"
+
+#include "regs.h"
+#include "intern.h"
+#include "desc_constr.h"
+#include "jr.h"
+#include "error.h"
+#include "sg_sw_sec4.h"
+#include "key_gen.h"
+
+#define CAAM_CRA_PRIORITY		3000
+
+/* max hash key is max split key size */
+#define CAAM_MAX_HASH_KEY_SIZE		(SHA512_DIGEST_SIZE * 2)
+
+#define CAAM_MAX_HASH_BLOCK_SIZE	SHA512_BLOCK_SIZE
+#define CAAM_MAX_HASH_DIGEST_SIZE	SHA512_DIGEST_SIZE
+
+/* length of descriptors text */
+#define DESC_JOB_IO_LEN			(CAAM_CMD_SZ * 5 + CAAM_PTR_SZ * 3)
+
+#define DESC_AHASH_BASE			(4 * CAAM_CMD_SZ)
+#define DESC_AHASH_UPDATE_LEN		(6 * CAAM_CMD_SZ)
+#define DESC_AHASH_UPDATE_FIRST_LEN	(DESC_AHASH_BASE + 4 * CAAM_CMD_SZ)
+#define DESC_AHASH_FINAL_LEN		(DESC_AHASH_BASE + 5 * CAAM_CMD_SZ)
+#define DESC_AHASH_FINUP_LEN		(DESC_AHASH_BASE + 5 * CAAM_CMD_SZ)
+#define DESC_AHASH_DIGEST_LEN		(DESC_AHASH_BASE + 4 * CAAM_CMD_SZ)
+
+#define DESC_HASH_MAX_USED_BYTES	(DESC_AHASH_FINAL_LEN + \
+					 CAAM_MAX_HASH_KEY_SIZE)
+#define DESC_HASH_MAX_USED_LEN		(DESC_HASH_MAX_USED_BYTES / CAAM_CMD_SZ)
+
+/* caam context sizes for hashes: running digest + 8 */
+#define HASH_MSG_LEN			8
+#define MAX_CTX_LEN			(HASH_MSG_LEN + SHA512_DIGEST_SIZE)
+
+#ifdef DEBUG
+/* for print_hex_dumps with line references */
+#define xstr(s) str(s)
+#define str(s) #s
+#define debug(format, arg...) printk(format, arg)
+#else
+#define debug(format, arg...)
+#endif
+
+/* ahash per-session context */
+struct caam_hash_ctx {
+	struct device *jrdev;
+	u32 sh_desc_update[DESC_HASH_MAX_USED_LEN];
+	u32 sh_desc_update_first[DESC_HASH_MAX_USED_LEN];
+	u32 sh_desc_fin[DESC_HASH_MAX_USED_LEN];
+	u32 sh_desc_digest[DESC_HASH_MAX_USED_LEN];
+	u32 sh_desc_finup[DESC_HASH_MAX_USED_LEN];
+	dma_addr_t sh_desc_update_dma;
+	dma_addr_t sh_desc_update_first_dma;
+	dma_addr_t sh_desc_fin_dma;
+	dma_addr_t sh_desc_digest_dma;
+	dma_addr_t sh_desc_finup_dma;
+	u32 alg_type;
+	u32 alg_op;
+	u8 key[CAAM_MAX_HASH_KEY_SIZE];
+	dma_addr_t key_dma;
+	int ctx_len;
+	unsigned int split_key_len;
+	unsigned int split_key_pad_len;
+};
+
+/* ahash state */
+struct caam_hash_state {
+	dma_addr_t buf_dma;
+	dma_addr_t ctx_dma;
+	u8 buf_0[CAAM_MAX_HASH_BLOCK_SIZE] ____cacheline_aligned;
+	int buflen_0;
+	u8 buf_1[CAAM_MAX_HASH_BLOCK_SIZE] ____cacheline_aligned;
+	int buflen_1;
+	u8 caam_ctx[MAX_CTX_LEN];
+	int (*update)(struct ahash_request *req);
+	int (*final)(struct ahash_request *req);
+	int (*finup)(struct ahash_request *req);
+	int current_buf;
+};
+
+/* Common job descriptor seq in/out ptr routines */
+
+/* Map state->caam_ctx, and append seq_out_ptr command that points to it */
+static inline void map_seq_out_ptr_ctx(u32 *desc, struct device *jrdev,
+				       struct caam_hash_state *state,
+				       int ctx_len)
+{
+	state->ctx_dma = dma_map_single(jrdev, state->caam_ctx,
+					ctx_len, DMA_FROM_DEVICE);
+	append_seq_out_ptr(desc, state->ctx_dma, ctx_len, 0);
+}
+
+/* Map req->result, and append seq_out_ptr command that points to it */
+static inline dma_addr_t map_seq_out_ptr_result(u32 *desc, struct device *jrdev,
+						u8 *result, int digestsize)
+{
+	dma_addr_t dst_dma;
+
+	dst_dma = dma_map_single(jrdev, result, digestsize, DMA_FROM_DEVICE);
+	append_seq_out_ptr(desc, dst_dma, digestsize, 0);
+
+	return dst_dma;
+}
+
+/* Map current buffer in state and put it in link table */
+static inline dma_addr_t buf_map_to_sec4_sg(struct device *jrdev,
+					    struct sec4_sg_entry *sec4_sg,
+					    u8 *buf, int buflen)
+{
+	dma_addr_t buf_dma;
+
+	buf_dma = dma_map_single(jrdev, buf, buflen, DMA_TO_DEVICE);
+	dma_to_sec4_sg_one(sec4_sg, buf_dma, buflen, 0);
+
+	return buf_dma;
+}
+
+/* Map req->src and put it in link table */
+static inline void src_map_to_sec4_sg(struct device *jrdev,
+				      struct scatterlist *src, int src_nents,
+				      struct sec4_sg_entry *sec4_sg,
+				      bool chained)
+{
+	dma_map_sg_chained(jrdev, src, src_nents, DMA_TO_DEVICE, chained);
+	sg_to_sec4_sg_last(src, src_nents, sec4_sg, 0);
+}
+
+/*
+ * Only put buffer in link table if it contains data, which is possible,
+ * since a buffer has previously been used, and needs to be unmapped,
+ */
+static inline dma_addr_t
+try_buf_map_to_sec4_sg(struct device *jrdev, struct sec4_sg_entry *sec4_sg,
+		       u8 *buf, dma_addr_t buf_dma, int buflen,
+		       int last_buflen)
+{
+	if (buf_dma && !dma_mapping_error(jrdev, buf_dma))
+		dma_unmap_single(jrdev, buf_dma, last_buflen, DMA_TO_DEVICE);
+	if (buflen)
+		buf_dma = buf_map_to_sec4_sg(jrdev, sec4_sg, buf, buflen);
+	else
+		buf_dma = 0;
+
+	return buf_dma;
+}
+
+/* Map state->caam_ctx, and add it to link table */
+static inline void ctx_map_to_sec4_sg(u32 *desc, struct device *jrdev,
+				      struct caam_hash_state *state,
+				      int ctx_len,
+				      struct sec4_sg_entry *sec4_sg,
+				      u32 flag)
+{
+	state->ctx_dma = dma_map_single(jrdev, state->caam_ctx, ctx_len, flag);
+	dma_to_sec4_sg_one(sec4_sg, state->ctx_dma, ctx_len, 0);
+}
+
+/* Common shared descriptor commands */
+static inline void append_key_ahash(u32 *desc, struct caam_hash_ctx *ctx)
+{
+	append_key_as_imm(desc, ctx->key, ctx->split_key_pad_len,
+			  ctx->split_key_len, CLASS_2 |
+			  KEY_DEST_MDHA_SPLIT | KEY_ENC);
+}
+
+/* Append key if it has been set */
+static inline void init_sh_desc_key_ahash(u32 *desc, struct caam_hash_ctx *ctx)
+{
+	u32 *key_jump_cmd;
+
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+
+	if (ctx->split_key_len) {
+		/* Skip if already shared */
+		key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+					   JUMP_COND_SHRD);
+
+		append_key_ahash(desc, ctx);
+
+		set_jump_tgt_here(desc, key_jump_cmd);
+	}
+
+	/* Propagate errors from shared to job descriptor */
+	append_cmd(desc, SET_OK_NO_PROP_ERRORS | CMD_LOAD);
+}
+
+/*
+ * For ahash read data from seqin following state->caam_ctx,
+ * and write resulting class2 context to seqout, which may be state->caam_ctx
+ * or req->result
+ */
+static inline void ahash_append_load_str(u32 *desc, int digestsize)
+{
+	/* Calculate remaining bytes to read */
+	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+
+	/* Read remaining bytes */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_LAST2 |
+			     FIFOLD_TYPE_MSG | KEY_VLF);
+
+	/* Store class2 context bytes */
+	append_seq_store(desc, digestsize, LDST_CLASS_2_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+}
+
+/*
+ * For ahash update, final and finup, import context, read and write to seqout
+ */
+static inline void ahash_ctx_data_to_out(u32 *desc, u32 op, u32 state,
+					 int digestsize,
+					 struct caam_hash_ctx *ctx)
+{
+	init_sh_desc_key_ahash(desc, ctx);
+
+	/* Import context from software */
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
+		   LDST_CLASS_2_CCB | ctx->ctx_len);
+
+	/* Class 2 operation */
+	append_operation(desc, op | state | OP_ALG_ENCRYPT);
+
+	/*
+	 * Load from buf and/or src and write to req->result or state->context
+	 */
+	ahash_append_load_str(desc, digestsize);
+}
+
+/* For ahash firsts and digest, read and write to seqout */
+static inline void ahash_data_to_out(u32 *desc, u32 op, u32 state,
+				     int digestsize, struct caam_hash_ctx *ctx)
+{
+	init_sh_desc_key_ahash(desc, ctx);
+
+	/* Class 2 operation */
+	append_operation(desc, op | state | OP_ALG_ENCRYPT);
+
+	/*
+	 * Load from buf and/or src and write to req->result or state->context
+	 */
+	ahash_append_load_str(desc, digestsize);
+}
+
+static int ahash_set_sh_desc(struct crypto_ahash *ahash)
+{
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	int digestsize = crypto_ahash_digestsize(ahash);
+	struct device *jrdev = ctx->jrdev;
+	u32 have_key = 0;
+	u32 *desc;
+
+	if (ctx->split_key_len)
+		have_key = OP_ALG_AAI_HMAC_PRECOMP;
+
+	/* ahash_update shared descriptor */
+	desc = ctx->sh_desc_update;
+
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+
+	/* Import context from software */
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
+		   LDST_CLASS_2_CCB | ctx->ctx_len);
+
+	/* Class 2 operation */
+	append_operation(desc, ctx->alg_type | OP_ALG_AS_UPDATE |
+			 OP_ALG_ENCRYPT);
+
+	/* Load data and write to result or context */
+	ahash_append_load_str(desc, ctx->ctx_len);
+
+	ctx->sh_desc_update_dma = dma_map_single(jrdev, desc, desc_bytes(desc),
+						 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_update_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ahash update shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	/* ahash_update_first shared descriptor */
+	desc = ctx->sh_desc_update_first;
+
+	ahash_data_to_out(desc, have_key | ctx->alg_type, OP_ALG_AS_INIT,
+			  ctx->ctx_len, ctx);
+
+	ctx->sh_desc_update_first_dma = dma_map_single(jrdev, desc,
+						       desc_bytes(desc),
+						       DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_update_first_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ahash update first shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	/* ahash_final shared descriptor */
+	desc = ctx->sh_desc_fin;
+
+	ahash_ctx_data_to_out(desc, have_key | ctx->alg_type,
+			      OP_ALG_AS_FINALIZE, digestsize, ctx);
+
+	ctx->sh_desc_fin_dma = dma_map_single(jrdev, desc, desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_fin_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ahash final shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/* ahash_finup shared descriptor */
+	desc = ctx->sh_desc_finup;
+
+	ahash_ctx_data_to_out(desc, have_key | ctx->alg_type,
+			      OP_ALG_AS_FINALIZE, digestsize, ctx);
+
+	ctx->sh_desc_finup_dma = dma_map_single(jrdev, desc, desc_bytes(desc),
+						DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_finup_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ahash finup shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	/* ahash_digest shared descriptor */
+	desc = ctx->sh_desc_digest;
+
+	ahash_data_to_out(desc, have_key | ctx->alg_type, OP_ALG_AS_INITFINAL,
+			  digestsize, ctx);
+
+	ctx->sh_desc_digest_dma = dma_map_single(jrdev, desc,
+						 desc_bytes(desc),
+						 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_digest_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ahash digest shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	return 0;
+}
+
+static u32 gen_split_hash_key(struct caam_hash_ctx *ctx, const u8 *key_in,
+			      u32 keylen)
+{
+	return gen_split_key(ctx->jrdev, ctx->key, ctx->split_key_len,
+			       ctx->split_key_pad_len, key_in, keylen,
+			       ctx->alg_op);
+}
+
+/* Digest hash size if it is too large */
+static u32 hash_digest_key(struct caam_hash_ctx *ctx, const u8 *key_in,
+			   u32 *keylen, u8 *key_out, u32 digestsize)
+{
+	struct device *jrdev = ctx->jrdev;
+	u32 *desc;
+	struct split_key_result result;
+	dma_addr_t src_dma, dst_dma;
+	int ret = 0;
+
+	desc = kmalloc(CAAM_CMD_SZ * 6 + CAAM_PTR_SZ * 2, GFP_KERNEL | GFP_DMA);
+
+	init_job_desc(desc, 0);
+
+	src_dma = dma_map_single(jrdev, (void *)key_in, *keylen,
+				 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, src_dma)) {
+		dev_err(jrdev, "unable to map key input memory\n");
+		kfree(desc);
+		return -ENOMEM;
+	}
+	dst_dma = dma_map_single(jrdev, (void *)key_out, digestsize,
+				 DMA_FROM_DEVICE);
+	if (dma_mapping_error(jrdev, dst_dma)) {
+		dev_err(jrdev, "unable to map key output memory\n");
+		dma_unmap_single(jrdev, src_dma, *keylen, DMA_TO_DEVICE);
+		kfree(desc);
+		return -ENOMEM;
+	}
+
+	/* Job descriptor to perform unkeyed hash on key_in */
+	append_operation(desc, ctx->alg_type | OP_ALG_ENCRYPT |
+			 OP_ALG_AS_INITFINAL);
+	append_seq_in_ptr(desc, src_dma, *keylen, 0);
+	append_seq_fifo_load(desc, *keylen, FIFOLD_CLASS_CLASS2 |
+			     FIFOLD_TYPE_LAST2 | FIFOLD_TYPE_MSG);
+	append_seq_out_ptr(desc, dst_dma, digestsize, 0);
+	append_seq_store(desc, digestsize, LDST_CLASS_2_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "key_in@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key_in, *keylen, 1);
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	result.err = 0;
+	init_completion(&result.completion);
+
+	ret = caam_jr_enqueue(jrdev, desc, split_key_done, &result);
+	if (!ret) {
+		/* in progress */
+		wait_for_completion_interruptible(&result.completion);
+		ret = result.err;
+#ifdef DEBUG
+		print_hex_dump(KERN_ERR, "digested key@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, key_in,
+			       digestsize, 1);
+#endif
+	}
+	*keylen = digestsize;
+
+	dma_unmap_single(jrdev, src_dma, *keylen, DMA_TO_DEVICE);
+	dma_unmap_single(jrdev, dst_dma, digestsize, DMA_FROM_DEVICE);
+
+	kfree(desc);
+
+	return ret;
+}
+
+static int ahash_setkey(struct crypto_ahash *ahash,
+			const u8 *key, unsigned int keylen)
+{
+	/* Sizes for MDHA pads (*not* keys): MD5, SHA1, 224, 256, 384, 512 */
+	static const u8 mdpadlen[] = { 16, 20, 32, 32, 64, 64 };
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct device *jrdev = ctx->jrdev;
+	int blocksize = crypto_tfm_alg_blocksize(&ahash->base);
+	int digestsize = crypto_ahash_digestsize(ahash);
+	int ret = 0;
+	u8 *hashed_key = NULL;
+
+#ifdef DEBUG
+	printk(KERN_ERR "keylen %d\n", keylen);
+#endif
+
+	if (keylen > blocksize) {
+		hashed_key = kmalloc(sizeof(u8) * digestsize, GFP_KERNEL |
+				     GFP_DMA);
+		if (!hashed_key)
+			return -ENOMEM;
+		ret = hash_digest_key(ctx, key, &keylen, hashed_key,
+				      digestsize);
+		if (ret)
+			goto badkey;
+		key = hashed_key;
+	}
+
+	/* Pick class 2 key length from algorithm submask */
+	ctx->split_key_len = mdpadlen[(ctx->alg_op & OP_ALG_ALGSEL_SUBMASK) >>
+				      OP_ALG_ALGSEL_SHIFT] * 2;
+	ctx->split_key_pad_len = ALIGN(ctx->split_key_len, 16);
+
+#ifdef DEBUG
+	printk(KERN_ERR "split_key_len %d split_key_pad_len %d\n",
+	       ctx->split_key_len, ctx->split_key_pad_len);
+	print_hex_dump(KERN_ERR, "key in @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
+#endif
+
+	ret = gen_split_hash_key(ctx, key, keylen);
+	if (ret)
+		goto badkey;
+
+	ctx->key_dma = dma_map_single(jrdev, ctx->key, ctx->split_key_pad_len,
+				      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->key_dma)) {
+		dev_err(jrdev, "unable to map key i/o memory\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ctx.key@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, ctx->key,
+		       ctx->split_key_pad_len, 1);
+#endif
+
+	ret = ahash_set_sh_desc(ahash);
+	if (ret) {
+		dma_unmap_single(jrdev, ctx->key_dma, ctx->split_key_pad_len,
+				 DMA_TO_DEVICE);
+	}
+
+	kfree(hashed_key);
+	return ret;
+badkey:
+	kfree(hashed_key);
+	crypto_ahash_set_flags(ahash, CRYPTO_TFM_RES_BAD_KEY_LEN);
+	return -EINVAL;
+}
+
+/*
+ * ahash_edesc - s/w-extended ahash descriptor
+ * @dst_dma: physical mapped address of req->result
+ * @sec4_sg_dma: physical mapped address of h/w link table
+ * @chained: if source is chained
+ * @src_nents: number of segments in input scatterlist
+ * @sec4_sg_bytes: length of dma mapped sec4_sg space
+ * @sec4_sg: pointer to h/w link table
+ * @hw_desc: the h/w job descriptor followed by any referenced link tables
+ */
+struct ahash_edesc {
+	dma_addr_t dst_dma;
+	dma_addr_t sec4_sg_dma;
+	bool chained;
+	int src_nents;
+	int sec4_sg_bytes;
+	struct sec4_sg_entry *sec4_sg;
+	u32 hw_desc[0];
+};
+
+static inline void ahash_unmap(struct device *dev,
+			struct ahash_edesc *edesc,
+			struct ahash_request *req, int dst_len)
+{
+	if (edesc->src_nents)
+		dma_unmap_sg_chained(dev, req->src, edesc->src_nents,
+				     DMA_TO_DEVICE, edesc->chained);
+	if (edesc->dst_dma)
+		dma_unmap_single(dev, edesc->dst_dma, dst_len, DMA_FROM_DEVICE);
+
+	if (edesc->sec4_sg_bytes)
+		dma_unmap_single(dev, edesc->sec4_sg_dma,
+				 edesc->sec4_sg_bytes, DMA_TO_DEVICE);
+}
+
+static inline void ahash_unmap_ctx(struct device *dev,
+			struct ahash_edesc *edesc,
+			struct ahash_request *req, int dst_len, u32 flag)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	if (state->ctx_dma)
+		dma_unmap_single(dev, state->ctx_dma, ctx->ctx_len, flag);
+	ahash_unmap(dev, edesc, req, dst_len);
+}
+
+static void ahash_done(struct device *jrdev, u32 *desc, u32 err,
+		       void *context)
+{
+	struct ahash_request *req = context;
+	struct ahash_edesc *edesc;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	int digestsize = crypto_ahash_digestsize(ahash);
+#ifdef DEBUG
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = (struct ahash_edesc *)((char *)desc -
+		 offsetof(struct ahash_edesc, hw_desc));
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	ahash_unmap(jrdev, edesc, req, digestsize);
+	kfree(edesc);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ctx@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, state->caam_ctx,
+		       ctx->ctx_len, 1);
+	if (req->result)
+		print_hex_dump(KERN_ERR, "result@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, req->result,
+			       digestsize, 1);
+#endif
+
+	req->base.complete(&req->base, err);
+}
+
+static void ahash_done_bi(struct device *jrdev, u32 *desc, u32 err,
+			    void *context)
+{
+	struct ahash_request *req = context;
+	struct ahash_edesc *edesc;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+#ifdef DEBUG
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	int digestsize = crypto_ahash_digestsize(ahash);
+
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = (struct ahash_edesc *)((char *)desc -
+		 offsetof(struct ahash_edesc, hw_desc));
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_BIDIRECTIONAL);
+	kfree(edesc);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ctx@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, state->caam_ctx,
+		       ctx->ctx_len, 1);
+	if (req->result)
+		print_hex_dump(KERN_ERR, "result@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, req->result,
+			       digestsize, 1);
+#endif
+
+	req->base.complete(&req->base, err);
+}
+
+static void ahash_done_ctx_src(struct device *jrdev, u32 *desc, u32 err,
+			       void *context)
+{
+	struct ahash_request *req = context;
+	struct ahash_edesc *edesc;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	int digestsize = crypto_ahash_digestsize(ahash);
+#ifdef DEBUG
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = (struct ahash_edesc *)((char *)desc -
+		 offsetof(struct ahash_edesc, hw_desc));
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
+	kfree(edesc);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ctx@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, state->caam_ctx,
+		       ctx->ctx_len, 1);
+	if (req->result)
+		print_hex_dump(KERN_ERR, "result@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, req->result,
+			       digestsize, 1);
+#endif
+
+	req->base.complete(&req->base, err);
+}
+
+static void ahash_done_ctx_dst(struct device *jrdev, u32 *desc, u32 err,
+			       void *context)
+{
+	struct ahash_request *req = context;
+	struct ahash_edesc *edesc;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+#ifdef DEBUG
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	int digestsize = crypto_ahash_digestsize(ahash);
+
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = (struct ahash_edesc *)((char *)desc -
+		 offsetof(struct ahash_edesc, hw_desc));
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len, DMA_TO_DEVICE);
+	kfree(edesc);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ctx@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, state->caam_ctx,
+		       ctx->ctx_len, 1);
+	if (req->result)
+		print_hex_dump(KERN_ERR, "result@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, req->result,
+			       digestsize, 1);
+#endif
+
+	req->base.complete(&req->base, err);
+}
+
+/* submit update job descriptor */
+static int ahash_update_ctx(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int *buflen = state->current_buf ? &state->buflen_1 : &state->buflen_0;
+	u8 *next_buf = state->current_buf ? state->buf_0 : state->buf_1;
+	int *next_buflen = state->current_buf ? &state->buflen_0 :
+			   &state->buflen_1, last_buflen;
+	int in_len = *buflen + req->nbytes, to_hash;
+	u32 *sh_desc = ctx->sh_desc_update, *desc;
+	dma_addr_t ptr = ctx->sh_desc_update_dma;
+	int src_nents, sec4_sg_bytes, sec4_sg_src_index;
+	struct ahash_edesc *edesc;
+	bool chained = false;
+	int ret = 0;
+	int sh_len;
+
+	last_buflen = *next_buflen;
+	*next_buflen = in_len & (crypto_tfm_alg_blocksize(&ahash->base) - 1);
+	to_hash = in_len - *next_buflen;
+
+	if (to_hash) {
+		src_nents = __sg_count(req->src, req->nbytes - (*next_buflen),
+				       &chained);
+		sec4_sg_src_index = 1 + (*buflen ? 1 : 0);
+		sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
+				 sizeof(struct sec4_sg_entry);
+
+		/*
+		 * allocate space for base edesc and hw desc commands,
+		 * link tables
+		 */
+		edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN +
+				sec4_sg_bytes, GFP_DMA | flags);
+		if (!edesc) {
+			dev_err(jrdev,
+				"could not allocate extended descriptor\n");
+			return -ENOMEM;
+		}
+
+		edesc->src_nents = src_nents;
+		edesc->chained = chained;
+		edesc->sec4_sg_bytes = sec4_sg_bytes;
+		edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+				 DESC_JOB_IO_LEN;
+		edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+						     sec4_sg_bytes,
+						     DMA_TO_DEVICE);
+
+		ctx_map_to_sec4_sg(desc, jrdev, state, ctx->ctx_len,
+				   edesc->sec4_sg, DMA_BIDIRECTIONAL);
+
+		state->buf_dma = try_buf_map_to_sec4_sg(jrdev,
+							edesc->sec4_sg + 1,
+							buf, state->buf_dma,
+							*buflen, last_buflen);
+
+		if (src_nents) {
+			src_map_to_sec4_sg(jrdev, req->src, src_nents,
+					   edesc->sec4_sg + sec4_sg_src_index,
+					   chained);
+			if (*next_buflen) {
+				sg_copy_part(next_buf, req->src, to_hash -
+					     *buflen, req->nbytes);
+				state->current_buf = !state->current_buf;
+			}
+		} else {
+			(edesc->sec4_sg + sec4_sg_src_index - 1)->len |=
+							SEC4_SG_LEN_FIN;
+		}
+
+		sh_len = desc_len(sh_desc);
+		desc = edesc->hw_desc;
+		init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER |
+				     HDR_REVERSE);
+
+		append_seq_in_ptr(desc, edesc->sec4_sg_dma, ctx->ctx_len +
+				       to_hash, LDST_SGF);
+
+		append_seq_out_ptr(desc, state->ctx_dma, ctx->ctx_len, 0);
+
+#ifdef DEBUG
+		print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+			       desc_bytes(desc), 1);
+#endif
+
+		ret = caam_jr_enqueue(jrdev, desc, ahash_done_bi, req);
+		if (!ret) {
+			ret = -EINPROGRESS;
+		} else {
+			ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len,
+					   DMA_BIDIRECTIONAL);
+			kfree(edesc);
+		}
+	} else if (*next_buflen) {
+		sg_copy(buf + *buflen, req->src, req->nbytes);
+		*buflen = *next_buflen;
+		*next_buflen = last_buflen;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "buf@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, buf, *buflen, 1);
+	print_hex_dump(KERN_ERR, "next buf@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, next_buf,
+		       *next_buflen, 1);
+#endif
+
+	return ret;
+}
+
+static int ahash_final_ctx(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
+	int last_buflen = state->current_buf ? state->buflen_0 :
+			  state->buflen_1;
+	u32 *sh_desc = ctx->sh_desc_fin, *desc;
+	dma_addr_t ptr = ctx->sh_desc_fin_dma;
+	int sec4_sg_bytes;
+	int digestsize = crypto_ahash_digestsize(ahash);
+	struct ahash_edesc *edesc;
+	int ret = 0;
+	int sh_len;
+
+	sec4_sg_bytes = (1 + (buflen ? 1 : 0)) * sizeof(struct sec4_sg_entry);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN +
+			sec4_sg_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return -ENOMEM;
+	}
+
+	sh_len = desc_len(sh_desc);
+	desc = edesc->hw_desc;
+	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+			 DESC_JOB_IO_LEN;
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
+	edesc->src_nents = 0;
+
+	ctx_map_to_sec4_sg(desc, jrdev, state, ctx->ctx_len, edesc->sec4_sg,
+			   DMA_TO_DEVICE);
+
+	state->buf_dma = try_buf_map_to_sec4_sg(jrdev, edesc->sec4_sg + 1,
+						buf, state->buf_dma, buflen,
+						last_buflen);
+	(edesc->sec4_sg + sec4_sg_bytes - 1)->len |= SEC4_SG_LEN_FIN;
+
+	append_seq_in_ptr(desc, edesc->sec4_sg_dma, ctx->ctx_len + buflen,
+			  LDST_SGF);
+
+	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
+						digestsize);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_src, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+static int ahash_finup_ctx(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
+	int last_buflen = state->current_buf ? state->buflen_0 :
+			  state->buflen_1;
+	u32 *sh_desc = ctx->sh_desc_finup, *desc;
+	dma_addr_t ptr = ctx->sh_desc_finup_dma;
+	int sec4_sg_bytes, sec4_sg_src_index;
+	int src_nents;
+	int digestsize = crypto_ahash_digestsize(ahash);
+	struct ahash_edesc *edesc;
+	bool chained = false;
+	int ret = 0;
+	int sh_len;
+
+	src_nents = __sg_count(req->src, req->nbytes, &chained);
+	sec4_sg_src_index = 1 + (buflen ? 1 : 0);
+	sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
+			 sizeof(struct sec4_sg_entry);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN +
+			sec4_sg_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return -ENOMEM;
+	}
+
+	sh_len = desc_len(sh_desc);
+	desc = edesc->hw_desc;
+	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	edesc->src_nents = src_nents;
+	edesc->chained = chained;
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+			 DESC_JOB_IO_LEN;
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
+
+	ctx_map_to_sec4_sg(desc, jrdev, state, ctx->ctx_len, edesc->sec4_sg,
+			   DMA_TO_DEVICE);
+
+	state->buf_dma = try_buf_map_to_sec4_sg(jrdev, edesc->sec4_sg + 1,
+						buf, state->buf_dma, buflen,
+						last_buflen);
+
+	src_map_to_sec4_sg(jrdev, req->src, src_nents, edesc->sec4_sg +
+			   sec4_sg_src_index, chained);
+
+	append_seq_in_ptr(desc, edesc->sec4_sg_dma, ctx->ctx_len +
+			       buflen + req->nbytes, LDST_SGF);
+
+	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
+						digestsize);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_src, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ahash_unmap_ctx(jrdev, edesc, req, digestsize, DMA_FROM_DEVICE);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+static int ahash_digest(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u32 *sh_desc = ctx->sh_desc_digest, *desc;
+	dma_addr_t ptr = ctx->sh_desc_digest_dma;
+	int digestsize = crypto_ahash_digestsize(ahash);
+	int src_nents, sec4_sg_bytes;
+	dma_addr_t src_dma;
+	struct ahash_edesc *edesc;
+	bool chained = false;
+	int ret = 0;
+	u32 options;
+	int sh_len;
+
+	src_nents = sg_count(req->src, req->nbytes, &chained);
+	dma_map_sg_chained(jrdev, req->src, src_nents ? : 1, DMA_TO_DEVICE,
+			   chained);
+	sec4_sg_bytes = src_nents * sizeof(struct sec4_sg_entry);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct ahash_edesc) + sec4_sg_bytes +
+			DESC_JOB_IO_LEN, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return -ENOMEM;
+	}
+	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+			  DESC_JOB_IO_LEN;
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
+	edesc->src_nents = src_nents;
+	edesc->chained = chained;
+
+	sh_len = desc_len(sh_desc);
+	desc = edesc->hw_desc;
+	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	if (src_nents) {
+		sg_to_sec4_sg_last(req->src, src_nents, edesc->sec4_sg, 0);
+		src_dma = edesc->sec4_sg_dma;
+		options = LDST_SGF;
+	} else {
+		src_dma = sg_dma_address(req->src);
+		options = 0;
+	}
+	append_seq_in_ptr(desc, src_dma, req->nbytes, options);
+
+	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
+						digestsize);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	ret = caam_jr_enqueue(jrdev, desc, ahash_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ahash_unmap(jrdev, edesc, req, digestsize);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+/* submit ahash final if it the first job descriptor */
+static int ahash_final_no_ctx(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
+	u32 *sh_desc = ctx->sh_desc_digest, *desc;
+	dma_addr_t ptr = ctx->sh_desc_digest_dma;
+	int digestsize = crypto_ahash_digestsize(ahash);
+	struct ahash_edesc *edesc;
+	int ret = 0;
+	int sh_len;
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN,
+			GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return -ENOMEM;
+	}
+
+	sh_len = desc_len(sh_desc);
+	desc = edesc->hw_desc;
+	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	state->buf_dma = dma_map_single(jrdev, buf, buflen, DMA_TO_DEVICE);
+
+	append_seq_in_ptr(desc, state->buf_dma, buflen, 0);
+
+	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
+						digestsize);
+	edesc->src_nents = 0;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	ret = caam_jr_enqueue(jrdev, desc, ahash_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ahash_unmap(jrdev, edesc, req, digestsize);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+/* submit ahash update if it the first job descriptor after update */
+static int ahash_update_no_ctx(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int *buflen = state->current_buf ? &state->buflen_1 : &state->buflen_0;
+	u8 *next_buf = state->current_buf ? state->buf_0 : state->buf_1;
+	int *next_buflen = state->current_buf ? &state->buflen_0 :
+			   &state->buflen_1;
+	int in_len = *buflen + req->nbytes, to_hash;
+	int sec4_sg_bytes, src_nents;
+	struct ahash_edesc *edesc;
+	u32 *desc, *sh_desc = ctx->sh_desc_update_first;
+	dma_addr_t ptr = ctx->sh_desc_update_first_dma;
+	bool chained = false;
+	int ret = 0;
+	int sh_len;
+
+	*next_buflen = in_len & (crypto_tfm_alg_blocksize(&ahash->base) - 1);
+	to_hash = in_len - *next_buflen;
+
+	if (to_hash) {
+		src_nents = __sg_count(req->src, req->nbytes - (*next_buflen),
+				       &chained);
+		sec4_sg_bytes = (1 + src_nents) *
+				sizeof(struct sec4_sg_entry);
+
+		/*
+		 * allocate space for base edesc and hw desc commands,
+		 * link tables
+		 */
+		edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN +
+				sec4_sg_bytes, GFP_DMA | flags);
+		if (!edesc) {
+			dev_err(jrdev,
+				"could not allocate extended descriptor\n");
+			return -ENOMEM;
+		}
+
+		edesc->src_nents = src_nents;
+		edesc->chained = chained;
+		edesc->sec4_sg_bytes = sec4_sg_bytes;
+		edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+				 DESC_JOB_IO_LEN;
+		edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+						    sec4_sg_bytes,
+						    DMA_TO_DEVICE);
+
+		state->buf_dma = buf_map_to_sec4_sg(jrdev, edesc->sec4_sg,
+						    buf, *buflen);
+		src_map_to_sec4_sg(jrdev, req->src, src_nents,
+				   edesc->sec4_sg + 1, chained);
+		if (*next_buflen) {
+			sg_copy_part(next_buf, req->src, to_hash - *buflen,
+				    req->nbytes);
+			state->current_buf = !state->current_buf;
+		}
+
+		sh_len = desc_len(sh_desc);
+		desc = edesc->hw_desc;
+		init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER |
+				     HDR_REVERSE);
+
+		append_seq_in_ptr(desc, edesc->sec4_sg_dma, to_hash, LDST_SGF);
+
+		map_seq_out_ptr_ctx(desc, jrdev, state, ctx->ctx_len);
+
+#ifdef DEBUG
+		print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+			       desc_bytes(desc), 1);
+#endif
+
+		ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_dst, req);
+		if (!ret) {
+			ret = -EINPROGRESS;
+			state->update = ahash_update_ctx;
+			state->finup = ahash_finup_ctx;
+			state->final = ahash_final_ctx;
+		} else {
+			ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len,
+					DMA_TO_DEVICE);
+			kfree(edesc);
+		}
+	} else if (*next_buflen) {
+		sg_copy(buf + *buflen, req->src, req->nbytes);
+		*buflen = *next_buflen;
+		*next_buflen = 0;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "buf@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, buf, *buflen, 1);
+	print_hex_dump(KERN_ERR, "next buf@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, next_buf,
+		       *next_buflen, 1);
+#endif
+
+	return ret;
+}
+
+/* submit ahash finup if it the first job descriptor after update */
+static int ahash_finup_no_ctx(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int buflen = state->current_buf ? state->buflen_1 : state->buflen_0;
+	int last_buflen = state->current_buf ? state->buflen_0 :
+			  state->buflen_1;
+	u32 *sh_desc = ctx->sh_desc_digest, *desc;
+	dma_addr_t ptr = ctx->sh_desc_digest_dma;
+	int sec4_sg_bytes, sec4_sg_src_index, src_nents;
+	int digestsize = crypto_ahash_digestsize(ahash);
+	struct ahash_edesc *edesc;
+	bool chained = false;
+	int sh_len;
+	int ret = 0;
+
+	src_nents = __sg_count(req->src, req->nbytes, &chained);
+	sec4_sg_src_index = 2;
+	sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
+			 sizeof(struct sec4_sg_entry);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN +
+			sec4_sg_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return -ENOMEM;
+	}
+
+	sh_len = desc_len(sh_desc);
+	desc = edesc->hw_desc;
+	init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	edesc->src_nents = src_nents;
+	edesc->chained = chained;
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+	edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+			 DESC_JOB_IO_LEN;
+	edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+					    sec4_sg_bytes, DMA_TO_DEVICE);
+
+	state->buf_dma = try_buf_map_to_sec4_sg(jrdev, edesc->sec4_sg, buf,
+						state->buf_dma, buflen,
+						last_buflen);
+
+	src_map_to_sec4_sg(jrdev, req->src, src_nents, edesc->sec4_sg + 1,
+			   chained);
+
+	append_seq_in_ptr(desc, edesc->sec4_sg_dma, buflen +
+			       req->nbytes, LDST_SGF);
+
+	edesc->dst_dma = map_seq_out_ptr_result(desc, jrdev, req->result,
+						digestsize);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	ret = caam_jr_enqueue(jrdev, desc, ahash_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ahash_unmap(jrdev, edesc, req, digestsize);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+/* submit first update job descriptor after init */
+static int ahash_update_first(struct ahash_request *req)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	u8 *next_buf = state->buf_0 + state->current_buf *
+		       CAAM_MAX_HASH_BLOCK_SIZE;
+	int *next_buflen = &state->buflen_0 + state->current_buf;
+	int to_hash;
+	u32 *sh_desc = ctx->sh_desc_update_first, *desc;
+	dma_addr_t ptr = ctx->sh_desc_update_first_dma;
+	int sec4_sg_bytes, src_nents;
+	dma_addr_t src_dma;
+	u32 options;
+	struct ahash_edesc *edesc;
+	bool chained = false;
+	int ret = 0;
+	int sh_len;
+
+	*next_buflen = req->nbytes & (crypto_tfm_alg_blocksize(&ahash->base) -
+				      1);
+	to_hash = req->nbytes - *next_buflen;
+
+	if (to_hash) {
+		src_nents = sg_count(req->src, req->nbytes - (*next_buflen),
+				     &chained);
+		dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+				   DMA_TO_DEVICE, chained);
+		sec4_sg_bytes = src_nents * sizeof(struct sec4_sg_entry);
+
+		/*
+		 * allocate space for base edesc and hw desc commands,
+		 * link tables
+		 */
+		edesc = kmalloc(sizeof(struct ahash_edesc) + DESC_JOB_IO_LEN +
+				sec4_sg_bytes, GFP_DMA | flags);
+		if (!edesc) {
+			dev_err(jrdev,
+				"could not allocate extended descriptor\n");
+			return -ENOMEM;
+		}
+
+		edesc->src_nents = src_nents;
+		edesc->chained = chained;
+		edesc->sec4_sg_bytes = sec4_sg_bytes;
+		edesc->sec4_sg = (void *)edesc + sizeof(struct ahash_edesc) +
+				 DESC_JOB_IO_LEN;
+		edesc->sec4_sg_dma = dma_map_single(jrdev, edesc->sec4_sg,
+						    sec4_sg_bytes,
+						    DMA_TO_DEVICE);
+
+		if (src_nents) {
+			sg_to_sec4_sg_last(req->src, src_nents,
+					   edesc->sec4_sg, 0);
+			src_dma = edesc->sec4_sg_dma;
+			options = LDST_SGF;
+		} else {
+			src_dma = sg_dma_address(req->src);
+			options = 0;
+		}
+
+		if (*next_buflen)
+			sg_copy_part(next_buf, req->src, to_hash, req->nbytes);
+
+		sh_len = desc_len(sh_desc);
+		desc = edesc->hw_desc;
+		init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER |
+				     HDR_REVERSE);
+
+		append_seq_in_ptr(desc, src_dma, to_hash, options);
+
+		map_seq_out_ptr_ctx(desc, jrdev, state, ctx->ctx_len);
+
+#ifdef DEBUG
+		print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+			       desc_bytes(desc), 1);
+#endif
+
+		ret = caam_jr_enqueue(jrdev, desc, ahash_done_ctx_dst,
+				      req);
+		if (!ret) {
+			ret = -EINPROGRESS;
+			state->update = ahash_update_ctx;
+			state->finup = ahash_finup_ctx;
+			state->final = ahash_final_ctx;
+		} else {
+			ahash_unmap_ctx(jrdev, edesc, req, ctx->ctx_len,
+					DMA_TO_DEVICE);
+			kfree(edesc);
+		}
+	} else if (*next_buflen) {
+		state->update = ahash_update_no_ctx;
+		state->finup = ahash_finup_no_ctx;
+		state->final = ahash_final_no_ctx;
+		sg_copy(next_buf, req->src, req->nbytes);
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "next buf@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, next_buf,
+		       *next_buflen, 1);
+#endif
+
+	return ret;
+}
+
+static int ahash_finup_first(struct ahash_request *req)
+{
+	return ahash_digest(req);
+}
+
+static int ahash_init(struct ahash_request *req)
+{
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	state->update = ahash_update_first;
+	state->finup = ahash_finup_first;
+	state->final = ahash_final_no_ctx;
+
+	state->current_buf = 0;
+
+	return 0;
+}
+
+static int ahash_update(struct ahash_request *req)
+{
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	return state->update(req);
+}
+
+static int ahash_finup(struct ahash_request *req)
+{
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	return state->finup(req);
+}
+
+static int ahash_final(struct ahash_request *req)
+{
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	return state->final(req);
+}
+
+static int ahash_export(struct ahash_request *req, void *out)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	memcpy(out, ctx, sizeof(struct caam_hash_ctx));
+	memcpy(out + sizeof(struct caam_hash_ctx), state,
+	       sizeof(struct caam_hash_state));
+	return 0;
+}
+
+static int ahash_import(struct ahash_request *req, const void *in)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct caam_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	struct caam_hash_state *state = ahash_request_ctx(req);
+
+	memcpy(ctx, in, sizeof(struct caam_hash_ctx));
+	memcpy(state, in + sizeof(struct caam_hash_ctx),
+	       sizeof(struct caam_hash_state));
+	return 0;
+}
+
+struct caam_hash_template {
+	char name[CRYPTO_MAX_ALG_NAME];
+	char driver_name[CRYPTO_MAX_ALG_NAME];
+	char hmac_name[CRYPTO_MAX_ALG_NAME];
+	char hmac_driver_name[CRYPTO_MAX_ALG_NAME];
+	unsigned int blocksize;
+	struct ahash_alg template_ahash;
+	u32 alg_type;
+	u32 alg_op;
+};
+
+/* ahash descriptors */
+static struct caam_hash_template driver_hash[] = {
+	{
+		.name = "sha1",
+		.driver_name = "sha1-caam",
+		.hmac_name = "hmac(sha1)",
+		.hmac_driver_name = "hmac-sha1-caam",
+		.blocksize = SHA1_BLOCK_SIZE,
+		.template_ahash = {
+			.init = ahash_init,
+			.update = ahash_update,
+			.final = ahash_final,
+			.finup = ahash_finup,
+			.digest = ahash_digest,
+			.export = ahash_export,
+			.import = ahash_import,
+			.setkey = ahash_setkey,
+			.halg = {
+				.digestsize = SHA1_DIGEST_SIZE,
+				},
+			},
+		.alg_type = OP_ALG_ALGSEL_SHA1,
+		.alg_op = OP_ALG_ALGSEL_SHA1 | OP_ALG_AAI_HMAC,
+	}, {
+		.name = "sha224",
+		.driver_name = "sha224-caam",
+		.hmac_name = "hmac(sha224)",
+		.hmac_driver_name = "hmac-sha224-caam",
+		.blocksize = SHA224_BLOCK_SIZE,
+		.template_ahash = {
+			.init = ahash_init,
+			.update = ahash_update,
+			.final = ahash_final,
+			.finup = ahash_finup,
+			.digest = ahash_digest,
+			.export = ahash_export,
+			.import = ahash_import,
+			.setkey = ahash_setkey,
+			.halg = {
+				.digestsize = SHA224_DIGEST_SIZE,
+				},
+			},
+		.alg_type = OP_ALG_ALGSEL_SHA224,
+		.alg_op = OP_ALG_ALGSEL_SHA224 | OP_ALG_AAI_HMAC,
+	}, {
+		.name = "sha256",
+		.driver_name = "sha256-caam",
+		.hmac_name = "hmac(sha256)",
+		.hmac_driver_name = "hmac-sha256-caam",
+		.blocksize = SHA256_BLOCK_SIZE,
+		.template_ahash = {
+			.init = ahash_init,
+			.update = ahash_update,
+			.final = ahash_final,
+			.finup = ahash_finup,
+			.digest = ahash_digest,
+			.export = ahash_export,
+			.import = ahash_import,
+			.setkey = ahash_setkey,
+			.halg = {
+				.digestsize = SHA256_DIGEST_SIZE,
+				},
+			},
+		.alg_type = OP_ALG_ALGSEL_SHA256,
+		.alg_op = OP_ALG_ALGSEL_SHA256 | OP_ALG_AAI_HMAC,
+	}, {
+		.name = "sha384",
+		.driver_name = "sha384-caam",
+		.hmac_name = "hmac(sha384)",
+		.hmac_driver_name = "hmac-sha384-caam",
+		.blocksize = SHA384_BLOCK_SIZE,
+		.template_ahash = {
+			.init = ahash_init,
+			.update = ahash_update,
+			.final = ahash_final,
+			.finup = ahash_finup,
+			.digest = ahash_digest,
+			.export = ahash_export,
+			.import = ahash_import,
+			.setkey = ahash_setkey,
+			.halg = {
+				.digestsize = SHA384_DIGEST_SIZE,
+				},
+			},
+		.alg_type = OP_ALG_ALGSEL_SHA384,
+		.alg_op = OP_ALG_ALGSEL_SHA384 | OP_ALG_AAI_HMAC,
+	}, {
+		.name = "sha512",
+		.driver_name = "sha512-caam",
+		.hmac_name = "hmac(sha512)",
+		.hmac_driver_name = "hmac-sha512-caam",
+		.blocksize = SHA512_BLOCK_SIZE,
+		.template_ahash = {
+			.init = ahash_init,
+			.update = ahash_update,
+			.final = ahash_final,
+			.finup = ahash_finup,
+			.digest = ahash_digest,
+			.export = ahash_export,
+			.import = ahash_import,
+			.setkey = ahash_setkey,
+			.halg = {
+				.digestsize = SHA512_DIGEST_SIZE,
+				},
+			},
+		.alg_type = OP_ALG_ALGSEL_SHA512,
+		.alg_op = OP_ALG_ALGSEL_SHA512 | OP_ALG_AAI_HMAC,
+	}, {
+		.name = "md5",
+		.driver_name = "md5-caam",
+		.hmac_name = "hmac(md5)",
+		.hmac_driver_name = "hmac-md5-caam",
+		.blocksize = MD5_BLOCK_WORDS * 4,
+		.template_ahash = {
+			.init = ahash_init,
+			.update = ahash_update,
+			.final = ahash_final,
+			.finup = ahash_finup,
+			.digest = ahash_digest,
+			.export = ahash_export,
+			.import = ahash_import,
+			.setkey = ahash_setkey,
+			.halg = {
+				.digestsize = MD5_DIGEST_SIZE,
+				},
+			},
+		.alg_type = OP_ALG_ALGSEL_MD5,
+		.alg_op = OP_ALG_ALGSEL_MD5 | OP_ALG_AAI_HMAC,
+	},
+};
+
+struct caam_hash_alg {
+	struct list_head entry;
+	struct device *ctrldev;
+	int alg_type;
+	int alg_op;
+	struct ahash_alg ahash_alg;
+};
+
+static int caam_hash_cra_init(struct crypto_tfm *tfm)
+{
+	struct crypto_ahash *ahash = __crypto_ahash_cast(tfm);
+	struct crypto_alg *base = tfm->__crt_alg;
+	struct hash_alg_common *halg =
+		 container_of(base, struct hash_alg_common, base);
+	struct ahash_alg *alg =
+		 container_of(halg, struct ahash_alg, halg);
+	struct caam_hash_alg *caam_hash =
+		 container_of(alg, struct caam_hash_alg, ahash_alg);
+	struct caam_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct caam_drv_private *priv = dev_get_drvdata(caam_hash->ctrldev);
+	/* Sizes for MDHA running digests: MD5, SHA1, 224, 256, 384, 512 */
+	static const u8 runninglen[] = { HASH_MSG_LEN + MD5_DIGEST_SIZE,
+					 HASH_MSG_LEN + SHA1_DIGEST_SIZE,
+					 HASH_MSG_LEN + 32,
+					 HASH_MSG_LEN + SHA256_DIGEST_SIZE,
+					 HASH_MSG_LEN + 64,
+					 HASH_MSG_LEN + SHA512_DIGEST_SIZE };
+	int tgt_jr = atomic_inc_return(&priv->tfm_count);
+	int ret = 0;
+
+	/*
+	 * distribute tfms across job rings to ensure in-order
+	 * crypto request processing per tfm
+	 */
+	ctx->jrdev = priv->jrdev[tgt_jr % priv->total_jobrs];
+
+	/* copy descriptor header template value */
+	ctx->alg_type = OP_TYPE_CLASS2_ALG | caam_hash->alg_type;
+	ctx->alg_op = OP_TYPE_CLASS2_ALG | caam_hash->alg_op;
+
+	ctx->ctx_len = runninglen[(ctx->alg_op & OP_ALG_ALGSEL_SUBMASK) >>
+				  OP_ALG_ALGSEL_SHIFT];
+
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct caam_hash_state));
+
+	ret = ahash_set_sh_desc(ahash);
+
+	return ret;
+}
+
+static void caam_hash_cra_exit(struct crypto_tfm *tfm)
+{
+	struct caam_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (ctx->sh_desc_update_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_update_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_update_dma,
+				 desc_bytes(ctx->sh_desc_update),
+				 DMA_TO_DEVICE);
+	if (ctx->sh_desc_update_first_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_update_first_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_update_first_dma,
+				 desc_bytes(ctx->sh_desc_update_first),
+				 DMA_TO_DEVICE);
+	if (ctx->sh_desc_fin_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_fin_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_fin_dma,
+				 desc_bytes(ctx->sh_desc_fin), DMA_TO_DEVICE);
+	if (ctx->sh_desc_digest_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_digest_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_digest_dma,
+				 desc_bytes(ctx->sh_desc_digest),
+				 DMA_TO_DEVICE);
+	if (ctx->sh_desc_finup_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_finup_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_finup_dma,
+				 desc_bytes(ctx->sh_desc_finup), DMA_TO_DEVICE);
+}
+
+static void __exit caam_algapi_hash_exit(void)
+{
+	struct device_node *dev_node;
+	struct platform_device *pdev;
+	struct device *ctrldev;
+	struct caam_drv_private *priv;
+	struct caam_hash_alg *t_alg, *n;
+
+	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
+	if (!dev_node)
+		return;
+
+	pdev = of_find_device_by_node(dev_node);
+	if (!pdev)
+		return;
+
+	ctrldev = &pdev->dev;
+	of_node_put(dev_node);
+	priv = dev_get_drvdata(ctrldev);
+
+	if (!priv->hash_list.next)
+		return;
+
+	list_for_each_entry_safe(t_alg, n, &priv->hash_list, entry) {
+		crypto_unregister_ahash(&t_alg->ahash_alg);
+		list_del(&t_alg->entry);
+		kfree(t_alg);
+	}
+}
+
+static struct caam_hash_alg *
+caam_hash_alloc(struct device *ctrldev, struct caam_hash_template *template,
+		bool keyed)
+{
+	struct caam_hash_alg *t_alg;
+	struct ahash_alg *halg;
+	struct crypto_alg *alg;
+
+	t_alg = kzalloc(sizeof(struct caam_hash_alg), GFP_KERNEL);
+	if (!t_alg) {
+		dev_err(ctrldev, "failed to allocate t_alg\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	t_alg->ahash_alg = template->template_ahash;
+	halg = &t_alg->ahash_alg;
+	alg = &halg->halg.base;
+
+	if (keyed) {
+		snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->hmac_name);
+		snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->hmac_driver_name);
+	} else {
+		snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->name);
+		snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->driver_name);
+	}
+	alg->cra_module = THIS_MODULE;
+	alg->cra_init = caam_hash_cra_init;
+	alg->cra_exit = caam_hash_cra_exit;
+	alg->cra_ctxsize = sizeof(struct caam_hash_ctx);
+	alg->cra_priority = CAAM_CRA_PRIORITY;
+	alg->cra_blocksize = template->blocksize;
+	alg->cra_alignmask = 0;
+	alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_TYPE_AHASH;
+	alg->cra_type = &crypto_ahash_type;
+
+	t_alg->alg_type = template->alg_type;
+	t_alg->alg_op = template->alg_op;
+	t_alg->ctrldev = ctrldev;
+
+	return t_alg;
+}
+
+static int __init caam_algapi_hash_init(void)
+{
+	struct device_node *dev_node;
+	struct platform_device *pdev;
+	struct device *ctrldev;
+	struct caam_drv_private *priv;
+	int i = 0, err = 0;
+
+	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
+	if (!dev_node)
+		return -ENODEV;
+
+	pdev = of_find_device_by_node(dev_node);
+	if (!pdev)
+		return -ENODEV;
+
+	ctrldev = &pdev->dev;
+	priv = dev_get_drvdata(ctrldev);
+	of_node_put(dev_node);
+
+	INIT_LIST_HEAD(&priv->hash_list);
+
+	atomic_set(&priv->tfm_count, -1);
+
+	/* register crypto algorithms the device supports */
+	for (i = 0; i < ARRAY_SIZE(driver_hash); i++) {
+		/* TODO: check if h/w supports alg */
+		struct caam_hash_alg *t_alg;
+
+		/* register hmac version */
+		t_alg = caam_hash_alloc(ctrldev, &driver_hash[i], true);
+		if (IS_ERR(t_alg)) {
+			err = PTR_ERR(t_alg);
+			dev_warn(ctrldev, "%s alg allocation failed\n",
+				 driver_hash[i].driver_name);
+			continue;
+		}
+
+		err = crypto_register_ahash(&t_alg->ahash_alg);
+		if (err) {
+			dev_warn(ctrldev, "%s alg registration failed\n",
+				t_alg->ahash_alg.halg.base.cra_driver_name);
+			kfree(t_alg);
+		} else
+			list_add_tail(&t_alg->entry, &priv->hash_list);
+
+		/* register unkeyed version */
+		t_alg = caam_hash_alloc(ctrldev, &driver_hash[i], false);
+		if (IS_ERR(t_alg)) {
+			err = PTR_ERR(t_alg);
+			dev_warn(ctrldev, "%s alg allocation failed\n",
+				 driver_hash[i].driver_name);
+			continue;
+		}
+
+		err = crypto_register_ahash(&t_alg->ahash_alg);
+		if (err) {
+			dev_warn(ctrldev, "%s alg registration failed\n",
+				t_alg->ahash_alg.halg.base.cra_driver_name);
+			kfree(t_alg);
+		} else
+			list_add_tail(&t_alg->entry, &priv->hash_list);
+	}
+
+	return err;
+}
+
+module_init(caam_algapi_hash_init);
+module_exit(caam_algapi_hash_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("FSL CAAM support for ahash functions of crypto API");
+MODULE_AUTHOR("Freescale Semiconductor - NMG");
diff --git a/drivers/crypto/caam/caamrng.c b/drivers/crypto/caam/caamrng.c
new file mode 100644
index 00000000000..e2bfe161dec
--- /dev/null
+++ b/drivers/crypto/caam/caamrng.c
@@ -0,0 +1,309 @@
+/*
+ * caam - Freescale FSL CAAM support for hw_random
+ *
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ *
+ * Based on caamalg.c crypto API driver.
+ *
+ * relationship between job descriptors to shared descriptors:
+ *
+ * ---------------                     --------------
+ * | JobDesc #0  |-------------------->| ShareDesc  |
+ * | *(buffer 0) |      |------------->| (generate) |
+ * ---------------      |              | (move)     |
+ *                      |              | (store)    |
+ * ---------------      |              --------------
+ * | JobDesc #1  |------|
+ * | *(buffer 1) |
+ * ---------------
+ *
+ * A job desc looks like this:
+ *
+ * ---------------------
+ * | Header            |
+ * | ShareDesc Pointer |
+ * | SEQ_OUT_PTR       |
+ * | (output buffer)   |
+ * ---------------------
+ *
+ * The SharedDesc never changes, and each job descriptor points to one of two
+ * buffers for each device, from which the data will be copied into the
+ * requested destination
+ */
+
+#include <linux/hw_random.h>
+#include <linux/completion.h>
+#include <linux/atomic.h>
+
+#include "compat.h"
+
+#include "regs.h"
+#include "intern.h"
+#include "desc_constr.h"
+#include "jr.h"
+#include "error.h"
+
+/*
+ * Maximum buffer size: maximum number of random, cache-aligned bytes that
+ * will be generated and moved to seq out ptr (extlen not allowed)
+ */
+#define RN_BUF_SIZE			(0xffff / L1_CACHE_BYTES * \
+					 L1_CACHE_BYTES)
+
+/* length of descriptors */
+#define DESC_JOB_O_LEN			(CAAM_CMD_SZ * 2 + CAAM_PTR_SZ * 2)
+#define DESC_RNG_LEN			(10 * CAAM_CMD_SZ)
+
+/* Buffer, its dma address and lock */
+struct buf_data {
+	u8 buf[RN_BUF_SIZE];
+	dma_addr_t addr;
+	struct completion filled;
+	u32 hw_desc[DESC_JOB_O_LEN];
+#define BUF_NOT_EMPTY 0
+#define BUF_EMPTY 1
+#define BUF_PENDING 2  /* Empty, but with job pending --don't submit another */
+	atomic_t empty;
+};
+
+/* rng per-device context */
+struct caam_rng_ctx {
+	struct device *jrdev;
+	dma_addr_t sh_desc_dma;
+	u32 sh_desc[DESC_RNG_LEN];
+	unsigned int cur_buf_idx;
+	int current_buf;
+	struct buf_data bufs[2];
+};
+
+static struct caam_rng_ctx rng_ctx;
+
+static inline void rng_unmap_buf(struct device *jrdev, struct buf_data *bd)
+{
+	if (bd->addr)
+		dma_unmap_single(jrdev, bd->addr, RN_BUF_SIZE,
+				 DMA_FROM_DEVICE);
+}
+
+static inline void rng_unmap_ctx(struct caam_rng_ctx *ctx)
+{
+	struct device *jrdev = ctx->jrdev;
+
+	if (ctx->sh_desc_dma)
+		dma_unmap_single(jrdev, ctx->sh_desc_dma, DESC_RNG_LEN,
+				 DMA_TO_DEVICE);
+	rng_unmap_buf(jrdev, &ctx->bufs[0]);
+	rng_unmap_buf(jrdev, &ctx->bufs[1]);
+}
+
+static void rng_done(struct device *jrdev, u32 *desc, u32 err, void *context)
+{
+	struct buf_data *bd;
+
+	bd = (struct buf_data *)((char *)desc -
+	      offsetof(struct buf_data, hw_desc));
+
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	atomic_set(&bd->empty, BUF_NOT_EMPTY);
+	complete(&bd->filled);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rng refreshed buf@: ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, bd->buf, RN_BUF_SIZE, 1);
+#endif
+}
+
+static inline int submit_job(struct caam_rng_ctx *ctx, int to_current)
+{
+	struct buf_data *bd = &ctx->bufs[!(to_current ^ ctx->current_buf)];
+	struct device *jrdev = ctx->jrdev;
+	u32 *desc = bd->hw_desc;
+	int err;
+
+	dev_dbg(jrdev, "submitting job %d\n", !(to_current ^ ctx->current_buf));
+	init_completion(&bd->filled);
+	err = caam_jr_enqueue(jrdev, desc, rng_done, ctx);
+	if (err)
+		complete(&bd->filled); /* don't wait on failed job*/
+	else
+		atomic_inc(&bd->empty); /* note if pending */
+
+	return err;
+}
+
+static int caam_read(struct hwrng *rng, void *data, size_t max, bool wait)
+{
+	struct caam_rng_ctx *ctx = &rng_ctx;
+	struct buf_data *bd = &ctx->bufs[ctx->current_buf];
+	int next_buf_idx, copied_idx;
+	int err;
+
+	if (atomic_read(&bd->empty)) {
+		/* try to submit job if there wasn't one */
+		if (atomic_read(&bd->empty) == BUF_EMPTY) {
+			err = submit_job(ctx, 1);
+			/* if can't submit job, can't even wait */
+			if (err)
+				return 0;
+		}
+		/* no immediate data, so exit if not waiting */
+		if (!wait)
+			return 0;
+
+		/* waiting for pending job */
+		if (atomic_read(&bd->empty))
+			wait_for_completion(&bd->filled);
+	}
+
+	next_buf_idx = ctx->cur_buf_idx + max;
+	dev_dbg(ctx->jrdev, "%s: start reading at buffer %d, idx %d\n",
+		 __func__, ctx->current_buf, ctx->cur_buf_idx);
+
+	/* if enough data in current buffer */
+	if (next_buf_idx < RN_BUF_SIZE) {
+		memcpy(data, bd->buf + ctx->cur_buf_idx, max);
+		ctx->cur_buf_idx = next_buf_idx;
+		return max;
+	}
+
+	/* else, copy what's left... */
+	copied_idx = RN_BUF_SIZE - ctx->cur_buf_idx;
+	memcpy(data, bd->buf + ctx->cur_buf_idx, copied_idx);
+	ctx->cur_buf_idx = 0;
+	atomic_set(&bd->empty, BUF_EMPTY);
+
+	/* ...refill... */
+	submit_job(ctx, 1);
+
+	/* and use next buffer */
+	ctx->current_buf = !ctx->current_buf;
+	dev_dbg(ctx->jrdev, "switched to buffer %d\n", ctx->current_buf);
+
+	/* since there already is some data read, don't wait */
+	return copied_idx + caam_read(rng, data + copied_idx,
+				      max - copied_idx, false);
+}
+
+static inline void rng_create_sh_desc(struct caam_rng_ctx *ctx)
+{
+	struct device *jrdev = ctx->jrdev;
+	u32 *desc = ctx->sh_desc;
+
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+
+	/* Propagate errors from shared to job descriptor */
+	append_cmd(desc, SET_OK_NO_PROP_ERRORS | CMD_LOAD);
+
+	/* Generate random bytes */
+	append_operation(desc, OP_ALG_ALGSEL_RNG | OP_TYPE_CLASS1_ALG);
+
+	/* Store bytes */
+	append_seq_fifo_store(desc, RN_BUF_SIZE, FIFOST_TYPE_RNGSTORE);
+
+	ctx->sh_desc_dma = dma_map_single(jrdev, desc, desc_bytes(desc),
+					  DMA_TO_DEVICE);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rng shdesc@: ", DUMP_PREFIX_ADDRESS, 16, 4,
+		       desc, desc_bytes(desc), 1);
+#endif
+}
+
+static inline void rng_create_job_desc(struct caam_rng_ctx *ctx, int buf_id)
+{
+	struct device *jrdev = ctx->jrdev;
+	struct buf_data *bd = &ctx->bufs[buf_id];
+	u32 *desc = bd->hw_desc;
+	int sh_len = desc_len(ctx->sh_desc);
+
+	init_job_desc_shared(desc, ctx->sh_desc_dma, sh_len, HDR_SHARE_DEFER |
+			     HDR_REVERSE);
+
+	bd->addr = dma_map_single(jrdev, bd->buf, RN_BUF_SIZE, DMA_FROM_DEVICE);
+
+	append_seq_out_ptr_intlen(desc, bd->addr, RN_BUF_SIZE, 0);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "rng job desc@: ", DUMP_PREFIX_ADDRESS, 16, 4,
+		       desc, desc_bytes(desc), 1);
+#endif
+}
+
+static void caam_cleanup(struct hwrng *rng)
+{
+	int i;
+	struct buf_data *bd;
+
+	for (i = 0; i < 2; i++) {
+		bd = &rng_ctx.bufs[i];
+		if (atomic_read(&bd->empty) == BUF_PENDING)
+			wait_for_completion(&bd->filled);
+	}
+
+	rng_unmap_ctx(&rng_ctx);
+}
+
+static void caam_init_buf(struct caam_rng_ctx *ctx, int buf_id)
+{
+	struct buf_data *bd = &ctx->bufs[buf_id];
+
+	rng_create_job_desc(ctx, buf_id);
+	atomic_set(&bd->empty, BUF_EMPTY);
+	submit_job(ctx, buf_id == ctx->current_buf);
+	wait_for_completion(&bd->filled);
+}
+
+static void caam_init_rng(struct caam_rng_ctx *ctx, struct device *jrdev)
+{
+	ctx->jrdev = jrdev;
+	rng_create_sh_desc(ctx);
+	ctx->current_buf = 0;
+	ctx->cur_buf_idx = 0;
+	caam_init_buf(ctx, 0);
+	caam_init_buf(ctx, 1);
+}
+
+static struct hwrng caam_rng = {
+	.name		= "rng-caam",
+	.cleanup	= caam_cleanup,
+	.read		= caam_read,
+};
+
+static void __exit caam_rng_exit(void)
+{
+	hwrng_unregister(&caam_rng);
+}
+
+static int __init caam_rng_init(void)
+{
+	struct device_node *dev_node;
+	struct platform_device *pdev;
+	struct device *ctrldev;
+	struct caam_drv_private *priv;
+
+	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
+	if (!dev_node)
+		return -ENODEV;
+
+	pdev = of_find_device_by_node(dev_node);
+	if (!pdev)
+		return -ENODEV;
+
+	ctrldev = &pdev->dev;
+	priv = dev_get_drvdata(ctrldev);
+	of_node_put(dev_node);
+
+	caam_init_rng(&rng_ctx, priv->jrdev[0]);
+
+	dev_info(priv->jrdev[0], "registering rng-caam\n");
+	return hwrng_register(&caam_rng);
+}
+
+module_init(caam_rng_init);
+module_exit(caam_rng_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("FSL CAAM support for hw_random API");
+MODULE_AUTHOR("Freescale Semiconductor - NMG");
diff --git a/drivers/crypto/caam/compat.h b/drivers/crypto/caam/compat.h
index a63bc65fae8..762aeff626a 100644
--- a/drivers/crypto/caam/compat.h
+++ b/drivers/crypto/caam/compat.h
@@ -11,6 +11,7 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/crypto.h>
+#include <linux/hash.h>
 #include <linux/hw_random.h>
 #include <linux/of_platform.h>
 #include <linux/dma-mapping.h>
@@ -33,5 +34,6 @@
 #include <crypto/authenc.h>
 #include <crypto/scatterwalk.h>
 #include <crypto/internal/skcipher.h>
+#include <crypto/internal/hash.h>
 
 #endif /* !defined(CAAM_COMPAT_H) */
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 77557ebcd33..414ba20c05a 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -2,13 +2,16 @@
  * CAAM control-plane driver backend
  * Controller-level driver, kernel property detection, initialization
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #include "compat.h"
 #include "regs.h"
 #include "intern.h"
 #include "jr.h"
+#include "desc_constr.h"
+#include "error.h"
+#include "ctrl.h"
 
 static int caam_remove(struct platform_device *pdev)
 {
@@ -43,10 +46,154 @@ static int caam_remove(struct platform_device *pdev)
 	return ret;
 }
 
+/*
+ * Descriptor to instantiate RNG State Handle 0 in normal mode and
+ * load the JDKEK, TDKEK and TDSK registers
+ */
+static void build_instantiation_desc(u32 *desc)
+{
+	u32 *jump_cmd;
+
+	init_job_desc(desc, 0);
+
+	/* INIT RNG in non-test mode */
+	append_operation(desc, OP_TYPE_CLASS1_ALG | OP_ALG_ALGSEL_RNG |
+			 OP_ALG_AS_INIT);
+
+	/* wait for done */
+	jump_cmd = append_jump(desc, JUMP_CLASS_CLASS1);
+	set_jump_tgt_here(desc, jump_cmd);
+
+	/*
+	 * load 1 to clear written reg:
+	 * resets the done interrrupt and returns the RNG to idle.
+	 */
+	append_load_imm_u32(desc, 1, LDST_SRCDST_WORD_CLRW);
+
+	/* generate secure keys (non-test) */
+	append_operation(desc, OP_TYPE_CLASS1_ALG | OP_ALG_ALGSEL_RNG |
+			 OP_ALG_RNG4_SK);
+}
+
+struct instantiate_result {
+	struct completion completion;
+	int err;
+};
+
+static void rng4_init_done(struct device *dev, u32 *desc, u32 err,
+			   void *context)
+{
+	struct instantiate_result *instantiation = context;
+
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(dev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	instantiation->err = err;
+	complete(&instantiation->completion);
+}
+
+static int instantiate_rng(struct device *jrdev)
+{
+	struct instantiate_result instantiation;
+
+	dma_addr_t desc_dma;
+	u32 *desc;
+	int ret;
+
+	desc = kmalloc(CAAM_CMD_SZ * 6, GFP_KERNEL | GFP_DMA);
+	if (!desc) {
+		dev_err(jrdev, "cannot allocate RNG init descriptor memory\n");
+		return -ENOMEM;
+	}
+
+	build_instantiation_desc(desc);
+	desc_dma = dma_map_single(jrdev, desc, desc_bytes(desc), DMA_TO_DEVICE);
+	init_completion(&instantiation.completion);
+	ret = caam_jr_enqueue(jrdev, desc, rng4_init_done, &instantiation);
+	if (!ret) {
+		wait_for_completion_interruptible(&instantiation.completion);
+		ret = instantiation.err;
+		if (ret)
+			dev_err(jrdev, "unable to instantiate RNG\n");
+	}
+
+	dma_unmap_single(jrdev, desc_dma, desc_bytes(desc), DMA_TO_DEVICE);
+
+	kfree(desc);
+
+	return ret;
+}
+
+/*
+ * By default, the TRNG runs for 200 clocks per sample;
+ * 800 clocks per sample generates better entropy.
+ */
+static void kick_trng(struct platform_device *pdev)
+{
+	struct device *ctrldev = &pdev->dev;
+	struct caam_drv_private *ctrlpriv = dev_get_drvdata(ctrldev);
+	struct caam_full __iomem *topregs;
+	struct rng4tst __iomem *r4tst;
+	u32 val;
+
+	topregs = (struct caam_full __iomem *)ctrlpriv->ctrl;
+	r4tst = &topregs->ctrl.r4tst[0];
+
+	/* put RNG4 into program mode */
+	setbits32(&r4tst->rtmctl, RTMCTL_PRGM);
+	/* 800 clocks per sample */
+	val = rd_reg32(&r4tst->rtsdctl);
+	val = (val & ~RTSDCTL_ENT_DLY_MASK) | (800 << RTSDCTL_ENT_DLY_SHIFT);
+	wr_reg32(&r4tst->rtsdctl, val);
+	/* min. freq. count */
+	wr_reg32(&r4tst->rtfrqmin, 400);
+	/* max. freq. count */
+	wr_reg32(&r4tst->rtfrqmax, 6400);
+	/* put RNG4 into run mode */
+	clrbits32(&r4tst->rtmctl, RTMCTL_PRGM);
+}
+
+/**
+ * caam_get_era() - Return the ERA of the SEC on SoC, based
+ * on the SEC_VID register.
+ * Returns the ERA number (1..4) or -ENOTSUPP if the ERA is unknown.
+ * @caam_id - the value of the SEC_VID register
+ **/
+int caam_get_era(u64 caam_id)
+{
+	struct sec_vid *sec_vid = (struct sec_vid *)&caam_id;
+	static const struct {
+		u16 ip_id;
+		u8 maj_rev;
+		u8 era;
+	} caam_eras[] = {
+		{0x0A10, 1, 1},
+		{0x0A10, 2, 2},
+		{0x0A12, 1, 3},
+		{0x0A14, 1, 3},
+		{0x0A14, 2, 4},
+		{0x0A16, 1, 4},
+		{0x0A11, 1, 4}
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(caam_eras); i++)
+		if (caam_eras[i].ip_id == sec_vid->ip_id &&
+			caam_eras[i].maj_rev == sec_vid->maj_rev)
+				return caam_eras[i].era;
+
+	return -ENOTSUPP;
+}
+EXPORT_SYMBOL(caam_get_era);
+
 /* Probe routine for CAAM top (controller) level */
 static int caam_probe(struct platform_device *pdev)
 {
-	int ring, rspec;
+	int ret, ring, rspec;
+	u64 caam_id;
 	struct device *dev;
 	struct device_node *nprop, *np;
 	struct caam_ctrl __iomem *ctrl;
@@ -82,13 +229,18 @@ static int caam_probe(struct platform_device *pdev)
 
 	/*
 	 * Enable DECO watchdogs and, if this is a PHYS_ADDR_T_64BIT kernel,
-	 * 36-bit pointers in master configuration register
+	 * long pointers in master configuration register
 	 */
 	setbits32(&topregs->ctrl.mcr, MCFGR_WDENABLE |
 		  (sizeof(dma_addr_t) == sizeof(u64) ? MCFGR_LONG_PTR : 0));
 
 	if (sizeof(dma_addr_t) == sizeof(u64))
-		dma_set_mask(dev, DMA_BIT_MASK(36));
+		if (of_device_is_compatible(nprop, "fsl,sec-v5.0"))
+			dma_set_mask(dev, DMA_BIT_MASK(40));
+		else
+			dma_set_mask(dev, DMA_BIT_MASK(36));
+	else
+		dma_set_mask(dev, DMA_BIT_MASK(32));
 
 	/*
 	 * Detect and enable JobRs
@@ -141,14 +293,29 @@ static int caam_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	/*
+	 * RNG4 based SECs (v5+) need special initialization prior
+	 * to executing any descriptors
+	 */
+	if (of_device_is_compatible(nprop, "fsl,sec-v5.0")) {
+		kick_trng(pdev);
+		ret = instantiate_rng(ctrlpriv->jrdev[0]);
+		if (ret) {
+			caam_remove(pdev);
+			return ret;
+		}
+	}
+
 	/* NOTE: RTIC detection ought to go here, around Si time */
 
 	/* Initialize queue allocator lock */
 	spin_lock_init(&ctrlpriv->jr_alloc_lock);
 
+	caam_id = rd_reg64(&topregs->ctrl.perfmon.caam_id);
+
 	/* Report "alive" for developer to see */
-	dev_info(dev, "device ID = 0x%016llx\n",
-		 rd_reg64(&topregs->ctrl.perfmon.caam_id));
+	dev_info(dev, "device ID = 0x%016llx (Era %d)\n", caam_id,
+		 caam_get_era(caam_id));
 	dev_info(dev, "job rings = %d, qi = %d\n",
 		 ctrlpriv->total_jobrs, ctrlpriv->qi_present);
 
diff --git a/drivers/crypto/caam/ctrl.h b/drivers/crypto/caam/ctrl.h
new file mode 100644
index 00000000000..980d44eaaf4
--- /dev/null
+++ b/drivers/crypto/caam/ctrl.h
@@ -0,0 +1,13 @@
+/*
+ * CAAM control-plane driver backend public-level include definitions
+ *
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ */
+
+#ifndef CTRL_H
+#define CTRL_H
+
+/* Prototypes for backend-level services exposed to APIs */
+int caam_get_era(u64 caam_id);
+
+#endif /* CTRL_H */
diff --git a/drivers/crypto/caam/desc.h b/drivers/crypto/caam/desc.h
index a17c2958dab..f7f833be8c6 100644
--- a/drivers/crypto/caam/desc.h
+++ b/drivers/crypto/caam/desc.h
@@ -8,6 +8,16 @@
 #ifndef DESC_H
 #define DESC_H
 
+struct sec4_sg_entry {
+	u64 ptr;
+#define SEC4_SG_LEN_FIN 0x40000000
+#define SEC4_SG_LEN_EXT 0x80000000
+	u32 len;
+	u8 reserved;
+	u8 buf_pool_id;
+	u16 offset;
+};
+
 /* Max size of any CAAM descriptor in 32-bit words, inclusive of header */
 #define MAX_CAAM_DESCSIZE	64
 
@@ -1162,6 +1172,11 @@
 #define OP_ALG_AAI_GSM		(0x10 << OP_ALG_AAI_SHIFT)
 #define OP_ALG_AAI_EDGE		(0x20 << OP_ALG_AAI_SHIFT)
 
+/* RNG4 set */
+#define OP_ALG_RNG4_SHIFT	4
+#define OP_ALG_RNG4_MASK	(0x1f3 << OP_ALG_RNG4_SHIFT)
+
+#define OP_ALG_RNG4_SK		(0x100 << OP_ALG_RNG4_SHIFT)
 
 #define OP_ALG_AS_SHIFT		2
 #define OP_ALG_AS_MASK		(0x3 << OP_ALG_AS_SHIFT)
@@ -1585,20 +1600,4 @@
 #define NFIFOENTRY_PLEN_SHIFT	0
 #define NFIFOENTRY_PLEN_MASK	(0xFF << NFIFOENTRY_PLEN_SHIFT)
 
-/*
- * PDB internal definitions
- */
-
-/* IPSec ESP CBC Encap/Decap Options */
-#define PDBOPTS_ESPCBC_ARSNONE	0x00	/* no antireplay window	*/
-#define PDBOPTS_ESPCBC_ARS32	0x40	/* 32-entry antireplay window */
-#define PDBOPTS_ESPCBC_ARS64	0xc0	/* 64-entry antireplay window */
-#define PDBOPTS_ESPCBC_IVSRC	0x20	/* IV comes from internal random gen */
-#define PDBOPTS_ESPCBC_ESN	0x10	/* extended sequence included */
-#define PDBOPTS_ESPCBC_OUTFMT	0x08	/* output only decapsulation (decap) */
-#define PDBOPTS_ESPCBC_IPHDRSRC 0x08	/* IP header comes from PDB (encap) */
-#define PDBOPTS_ESPCBC_INCIPHDR 0x04	/* Prepend IP header to output frame */
-#define PDBOPTS_ESPCBC_IPVSN	0x02	/* process IPv6 header */
-#define PDBOPTS_ESPCBC_TUNNEL	0x01	/* tunnel mode next-header byte */
-
 #endif /* DESC_H */
diff --git a/drivers/crypto/caam/desc_constr.h b/drivers/crypto/caam/desc_constr.h
index 348b882275f..c85c1f05840 100644
--- a/drivers/crypto/caam/desc_constr.h
+++ b/drivers/crypto/caam/desc_constr.h
@@ -1,7 +1,7 @@
 /*
  * caam descriptor construction helper functions
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #include "desc.h"
@@ -51,7 +51,7 @@ static inline void *sh_desc_pdb(u32 *desc)
 
 static inline void init_desc(u32 *desc, u32 options)
 {
-	*desc = options | HDR_ONE | 1;
+	*desc = (options | HDR_ONE) + 1;
 }
 
 static inline void init_sh_desc(u32 *desc, u32 options)
@@ -62,9 +62,9 @@ static inline void init_sh_desc(u32 *desc, u32 options)
 
 static inline void init_sh_desc_pdb(u32 *desc, u32 options, size_t pdb_bytes)
 {
-	u32 pdb_len = pdb_bytes / CAAM_CMD_SZ + 1;
+	u32 pdb_len = (pdb_bytes + CAAM_CMD_SZ - 1) / CAAM_CMD_SZ;
 
-	init_sh_desc(desc, ((pdb_len << HDR_START_IDX_SHIFT) + pdb_len) |
+	init_sh_desc(desc, (((pdb_len + 1) << HDR_START_IDX_SHIFT) + pdb_len) |
 		     options);
 }
 
@@ -117,6 +117,15 @@ static inline void append_cmd_ptr(u32 *desc, dma_addr_t ptr, int len,
 	append_ptr(desc, ptr);
 }
 
+/* Write length after pointer, rather than inside command */
+static inline void append_cmd_ptr_extlen(u32 *desc, dma_addr_t ptr,
+					 unsigned int len, u32 command)
+{
+	append_cmd(desc, command);
+	append_ptr(desc, ptr);
+	append_cmd(desc, len);
+}
+
 static inline void append_cmd_data(u32 *desc, void *data, int len,
 				   u32 command)
 {
@@ -166,13 +175,22 @@ static inline void append_##cmd(u32 *desc, dma_addr_t ptr, unsigned int len, \
 	append_cmd_ptr(desc, ptr, len, CMD_##op | options); \
 }
 APPEND_CMD_PTR(key, KEY)
-APPEND_CMD_PTR(seq_in_ptr, SEQ_IN_PTR)
-APPEND_CMD_PTR(seq_out_ptr, SEQ_OUT_PTR)
 APPEND_CMD_PTR(load, LOAD)
 APPEND_CMD_PTR(store, STORE)
 APPEND_CMD_PTR(fifo_load, FIFO_LOAD)
 APPEND_CMD_PTR(fifo_store, FIFO_STORE)
 
+#define APPEND_SEQ_PTR_INTLEN(cmd, op) \
+static inline void append_seq_##cmd##_ptr_intlen(u32 *desc, dma_addr_t ptr, \
+						 unsigned int len, \
+						 u32 options) \
+{ \
+	PRINT_POS; \
+	append_cmd_ptr(desc, ptr, len, CMD_SEQ_##op##_PTR | options); \
+}
+APPEND_SEQ_PTR_INTLEN(in, IN)
+APPEND_SEQ_PTR_INTLEN(out, OUT)
+
 #define APPEND_CMD_PTR_TO_IMM(cmd, op) \
 static inline void append_##cmd##_as_imm(u32 *desc, void *data, \
 					 unsigned int len, u32 options) \
@@ -183,6 +201,33 @@ static inline void append_##cmd##_as_imm(u32 *desc, void *data, \
 APPEND_CMD_PTR_TO_IMM(load, LOAD);
 APPEND_CMD_PTR_TO_IMM(fifo_load, FIFO_LOAD);
 
+#define APPEND_CMD_PTR_EXTLEN(cmd, op) \
+static inline void append_##cmd##_extlen(u32 *desc, dma_addr_t ptr, \
+					 unsigned int len, u32 options) \
+{ \
+	PRINT_POS; \
+	append_cmd_ptr_extlen(desc, ptr, len, CMD_##op | SQIN_EXT | options); \
+}
+APPEND_CMD_PTR_EXTLEN(seq_in_ptr, SEQ_IN_PTR)
+APPEND_CMD_PTR_EXTLEN(seq_out_ptr, SEQ_OUT_PTR)
+
+/*
+ * Determine whether to store length internally or externally depending on
+ * the size of its type
+ */
+#define APPEND_CMD_PTR_LEN(cmd, op, type) \
+static inline void append_##cmd(u32 *desc, dma_addr_t ptr, \
+				type len, u32 options) \
+{ \
+	PRINT_POS; \
+	if (sizeof(type) > sizeof(u16)) \
+		append_##cmd##_extlen(desc, ptr, len, options); \
+	else \
+		append_##cmd##_intlen(desc, ptr, len, options); \
+}
+APPEND_CMD_PTR_LEN(seq_in_ptr, SEQ_IN_PTR, u32)
+APPEND_CMD_PTR_LEN(seq_out_ptr, SEQ_OUT_PTR, u32)
+
 /*
  * 2nd variant for commands whose specified immediate length differs
  * from length of immediate data provided, e.g., split keys
diff --git a/drivers/crypto/caam/error.c b/drivers/crypto/caam/error.c
index 7e2d54bffad..9955ed9643e 100644
--- a/drivers/crypto/caam/error.c
+++ b/drivers/crypto/caam/error.c
@@ -39,18 +39,20 @@ static void report_ccb_status(u32 status, char *outstr)
 	char *cha_id_list[] = {
 		"",
 		"AES",
-		"DES, 3DES",
+		"DES",
 		"ARC4",
-		"MD5, SHA-1, SH-224, SHA-256, SHA-384, SHA-512",
+		"MDHA",
 		"RNG",
 		"SNOW f8",
-		"Kasumi f8, f9",
-		"All Public Key Algorithms",
-		"CRC",
+		"Kasumi f8/9",
+		"PKHA",
+		"CRCA",
 		"SNOW f9",
+		"ZUCE",
+		"ZUCA",
 	};
 	char *err_id_list[] = {
-		"None. No error.",
+		"No error.",
 		"Mode error.",
 		"Data size error.",
 		"Key size error.",
@@ -67,6 +69,20 @@ static void report_ccb_status(u32 status, char *outstr)
 		"Invalid CHA combination was selected",
 		"Invalid CHA selected.",
 	};
+	char *rng_err_id_list[] = {
+		"",
+		"",
+		"",
+		"Instantiate",
+		"Not instantiated",
+		"Test instantiate",
+		"Prediction resistance",
+		"",
+		"Prediction resistance and test request",
+		"Uninstantiate",
+		"",
+		"Secure key generation",
+	};
 	u8 cha_id = (status & JRSTA_CCBERR_CHAID_MASK) >>
 		    JRSTA_CCBERR_CHAID_SHIFT;
 	u8 err_id = status & JRSTA_CCBERR_ERRID_MASK;
@@ -81,7 +97,13 @@ static void report_ccb_status(u32 status, char *outstr)
 			   cha_id, sizeof("ff"));
 	}
 
-	if (err_id < ARRAY_SIZE(err_id_list)) {
+	if ((cha_id << JRSTA_CCBERR_CHAID_SHIFT) == JRSTA_CCBERR_CHAID_RNG &&
+	    err_id < ARRAY_SIZE(rng_err_id_list) &&
+	    strlen(rng_err_id_list[err_id])) {
+		/* RNG-only error */
+		SPRINTFCAT(outstr, "%s", rng_err_id_list[err_id],
+			   strlen(rng_err_id_list[err_id]));
+	} else if (err_id < ARRAY_SIZE(err_id_list)) {
 		SPRINTFCAT(outstr, "%s", err_id_list[err_id],
 			   strlen(err_id_list[err_id]));
 	} else {
@@ -101,10 +123,10 @@ static void report_deco_status(u32 status, char *outstr)
 		u8 value;
 		char *error_text;
 	} desc_error_list[] = {
-		{ 0x00, "None. No error." },
+		{ 0x00, "No error." },
 		{ 0x01, "SGT Length Error. The descriptor is trying to read "
 			"more data than is contained in the SGT table." },
-		{ 0x02, "Reserved." },
+		{ 0x02, "SGT Null Entry Error." },
 		{ 0x03, "Job Ring Control Error. There is a bad value in the "
 			"Job Ring Control register." },
 		{ 0x04, "Invalid Descriptor Command. The Descriptor Command "
@@ -116,7 +138,7 @@ static void report_deco_status(u32 status, char *outstr)
 		{ 0x09, "Invalid OPERATION Command" },
 		{ 0x0A, "Invalid FIFO LOAD Command" },
 		{ 0x0B, "Invalid FIFO STORE Command" },
-		{ 0x0C, "Invalid MOVE Command" },
+		{ 0x0C, "Invalid MOVE/MOVE_LEN Command" },
 		{ 0x0D, "Invalid JUMP Command. A nonlocal JUMP Command is "
 			"invalid because the target is not a Job Header "
 			"Command, or the jump is from a Trusted Descriptor to "
@@ -166,6 +188,8 @@ static void report_deco_status(u32 status, char *outstr)
 			"(input frame; block ciphers) and IPsec decap (output "
 			"frame, when doing the next header byte update) and "
 			"DCRC (output frame)." },
+		{ 0x23, "Read Input Frame error" },
+		{ 0x24, "JDKEK, TDKEK or TDSK not loaded error" },
 		{ 0x80, "DNR (do not run) error" },
 		{ 0x81, "undefined protocol command" },
 		{ 0x82, "invalid setting in PDB" },
diff --git a/drivers/crypto/caam/intern.h b/drivers/crypto/caam/intern.h
index a34be01b0b2..5cd4c1b268a 100644
--- a/drivers/crypto/caam/intern.h
+++ b/drivers/crypto/caam/intern.h
@@ -43,7 +43,7 @@ struct caam_drv_private_jr {
 	struct device *parentdev;	/* points back to controller dev */
 	int ridx;
 	struct caam_job_ring __iomem *rregs;	/* JobR's register space */
-	struct tasklet_struct irqtask[NR_CPUS];
+	struct tasklet_struct irqtask;
 	int irq;			/* One per queue */
 	int assign;			/* busy/free */
 
@@ -86,10 +86,10 @@ struct caam_drv_private {
 
 	/* which jr allocated to scatterlist crypto */
 	atomic_t tfm_count ____cacheline_aligned;
-	int num_jrs_for_algapi;
-	struct device **algapi_jr;
 	/* list of registered crypto algorithms (mk generic context handle?) */
 	struct list_head alg_list;
+	/* list of registered hash algorithms (mk generic context handle?) */
+	struct list_head hash_list;
 
 	/*
 	 * debugfs entries for developer view into driver/device
diff --git a/drivers/crypto/caam/jr.c b/drivers/crypto/caam/jr.c
index 340fa322c0f..53c8c51d588 100644
--- a/drivers/crypto/caam/jr.c
+++ b/drivers/crypto/caam/jr.c
@@ -2,7 +2,7 @@
  * CAAM/SEC 4.x transport/backend driver
  * JobR backend functionality
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #include "compat.h"
@@ -43,7 +43,7 @@ static irqreturn_t caam_jr_interrupt(int irq, void *st_dev)
 	wr_reg32(&jrp->rregs->jrintstatus, irqstate);
 
 	preempt_disable();
-	tasklet_schedule(&jrp->irqtask[smp_processor_id()]);
+	tasklet_schedule(&jrp->irqtask);
 	preempt_enable();
 
 	return IRQ_HANDLED;
@@ -58,17 +58,16 @@ static void caam_jr_dequeue(unsigned long devarg)
 	void (*usercall)(struct device *dev, u32 *desc, u32 status, void *arg);
 	u32 *userdesc, userstatus;
 	void *userarg;
-	unsigned long flags;
 
-	spin_lock_irqsave(&jrp->outlock, flags);
+	while (rd_reg32(&jrp->rregs->outring_used)) {
 
-	head = ACCESS_ONCE(jrp->head);
-	sw_idx = tail = jrp->tail;
+		head = ACCESS_ONCE(jrp->head);
 
-	while (CIRC_CNT(head, tail, JOBR_DEPTH) >= 1 &&
-	       rd_reg32(&jrp->rregs->outring_used)) {
+		spin_lock_bh(&jrp->outlock);
 
+		sw_idx = tail = jrp->tail;
 		hw_idx = jrp->out_ring_read_index;
+
 		for (i = 0; CIRC_CNT(head, tail + i, JOBR_DEPTH) >= 1; i++) {
 			sw_idx = (tail + i) & (JOBR_DEPTH - 1);
 
@@ -95,7 +94,8 @@ static void caam_jr_dequeue(unsigned long devarg)
 		userdesc = jrp->entinfo[sw_idx].desc_addr_virt;
 		userstatus = jrp->outring[hw_idx].jrstatus;
 
-		smp_mb();
+		/* set done */
+		wr_reg32(&jrp->rregs->outring_rmvd, 1);
 
 		jrp->out_ring_read_index = (jrp->out_ring_read_index + 1) &
 					   (JOBR_DEPTH - 1);
@@ -115,22 +115,12 @@ static void caam_jr_dequeue(unsigned long devarg)
 			jrp->tail = tail;
 		}
 
-		/* set done */
-		wr_reg32(&jrp->rregs->outring_rmvd, 1);
-
-		spin_unlock_irqrestore(&jrp->outlock, flags);
+		spin_unlock_bh(&jrp->outlock);
 
 		/* Finally, execute user's callback */
 		usercall(dev, userdesc, userstatus, userarg);
-
-		spin_lock_irqsave(&jrp->outlock, flags);
-
-		head = ACCESS_ONCE(jrp->head);
-		sw_idx = tail = jrp->tail;
 	}
 
-	spin_unlock_irqrestore(&jrp->outlock, flags);
-
 	/* reenable / unmask IRQs */
 	clrbits32(&jrp->rregs->rconfig_lo, JRCFG_IMSK);
 }
@@ -148,23 +138,22 @@ int caam_jr_register(struct device *ctrldev, struct device **rdev)
 {
 	struct caam_drv_private *ctrlpriv = dev_get_drvdata(ctrldev);
 	struct caam_drv_private_jr *jrpriv = NULL;
-	unsigned long flags;
 	int ring;
 
 	/* Lock, if free ring - assign, unlock */
-	spin_lock_irqsave(&ctrlpriv->jr_alloc_lock, flags);
+	spin_lock(&ctrlpriv->jr_alloc_lock);
 	for (ring = 0; ring < ctrlpriv->total_jobrs; ring++) {
 		jrpriv = dev_get_drvdata(ctrlpriv->jrdev[ring]);
 		if (jrpriv->assign == JOBR_UNASSIGNED) {
 			jrpriv->assign = JOBR_ASSIGNED;
 			*rdev = ctrlpriv->jrdev[ring];
-			spin_unlock_irqrestore(&ctrlpriv->jr_alloc_lock, flags);
+			spin_unlock(&ctrlpriv->jr_alloc_lock);
 			return ring;
 		}
 	}
 
 	/* If assigned, write dev where caller needs it */
-	spin_unlock_irqrestore(&ctrlpriv->jr_alloc_lock, flags);
+	spin_unlock(&ctrlpriv->jr_alloc_lock);
 	*rdev = NULL;
 
 	return -ENODEV;
@@ -182,7 +171,6 @@ int caam_jr_deregister(struct device *rdev)
 {
 	struct caam_drv_private_jr *jrpriv = dev_get_drvdata(rdev);
 	struct caam_drv_private *ctrlpriv;
-	unsigned long flags;
 
 	/* Get the owning controller's private space */
 	ctrlpriv = dev_get_drvdata(jrpriv->parentdev);
@@ -195,9 +183,9 @@ int caam_jr_deregister(struct device *rdev)
 		return -EBUSY;
 
 	/* Release ring */
-	spin_lock_irqsave(&ctrlpriv->jr_alloc_lock, flags);
+	spin_lock(&ctrlpriv->jr_alloc_lock);
 	jrpriv->assign = JOBR_UNASSIGNED;
-	spin_unlock_irqrestore(&ctrlpriv->jr_alloc_lock, flags);
+	spin_unlock(&ctrlpriv->jr_alloc_lock);
 
 	return 0;
 }
@@ -238,7 +226,6 @@ int caam_jr_enqueue(struct device *dev, u32 *desc,
 {
 	struct caam_drv_private_jr *jrp = dev_get_drvdata(dev);
 	struct caam_jrentry_info *head_entry;
-	unsigned long flags;
 	int head, tail, desc_size;
 	dma_addr_t desc_dma;
 
@@ -249,14 +236,14 @@ int caam_jr_enqueue(struct device *dev, u32 *desc,
 		return -EIO;
 	}
 
-	spin_lock_irqsave(&jrp->inplock, flags);
+	spin_lock(&jrp->inplock);
 
 	head = jrp->head;
 	tail = ACCESS_ONCE(jrp->tail);
 
 	if (!rd_reg32(&jrp->rregs->inpring_avail) ||
 	    CIRC_SPACE(head, tail, JOBR_DEPTH) <= 0) {
-		spin_unlock_irqrestore(&jrp->inplock, flags);
+		spin_unlock(&jrp->inplock);
 		dma_unmap_single(dev, desc_dma, desc_size, DMA_TO_DEVICE);
 		return -EBUSY;
 	}
@@ -276,11 +263,9 @@ int caam_jr_enqueue(struct device *dev, u32 *desc,
 				    (JOBR_DEPTH - 1);
 	jrp->head = (head + 1) & (JOBR_DEPTH - 1);
 
-	wmb();
-
 	wr_reg32(&jrp->rregs->inpring_jobadd, 1);
 
-	spin_unlock_irqrestore(&jrp->inplock, flags);
+	spin_unlock(&jrp->inplock);
 
 	return 0;
 }
@@ -337,11 +322,9 @@ static int caam_jr_init(struct device *dev)
 
 	jrp = dev_get_drvdata(dev);
 
-	/* Connect job ring interrupt handler. */
-	for_each_possible_cpu(i)
-		tasklet_init(&jrp->irqtask[i], caam_jr_dequeue,
-			     (unsigned long)dev);
+	tasklet_init(&jrp->irqtask, caam_jr_dequeue, (unsigned long)dev);
 
+	/* Connect job ring interrupt handler. */
 	error = request_irq(jrp->irq, caam_jr_interrupt, IRQF_SHARED,
 			    "caam-jobr", dev);
 	if (error) {
@@ -356,10 +339,11 @@ static int caam_jr_init(struct device *dev)
 	if (error)
 		return error;
 
-	jrp->inpring = kzalloc(sizeof(dma_addr_t) * JOBR_DEPTH,
-			       GFP_KERNEL | GFP_DMA);
-	jrp->outring = kzalloc(sizeof(struct jr_outentry) *
-			       JOBR_DEPTH, GFP_KERNEL | GFP_DMA);
+	jrp->inpring = dma_alloc_coherent(dev, sizeof(dma_addr_t) * JOBR_DEPTH,
+					  &inpbusaddr, GFP_KERNEL);
+
+	jrp->outring = dma_alloc_coherent(dev, sizeof(struct jr_outentry) *
+					  JOBR_DEPTH, &outbusaddr, GFP_KERNEL);
 
 	jrp->entinfo = kzalloc(sizeof(struct caam_jrentry_info) * JOBR_DEPTH,
 			       GFP_KERNEL);
@@ -375,31 +359,6 @@ static int caam_jr_init(struct device *dev)
 		jrp->entinfo[i].desc_addr_dma = !0;
 
 	/* Setup rings */
-	inpbusaddr = dma_map_single(dev, jrp->inpring,
-				    sizeof(u32 *) * JOBR_DEPTH,
-				    DMA_BIDIRECTIONAL);
-	if (dma_mapping_error(dev, inpbusaddr)) {
-		dev_err(dev, "caam_jr_init(): can't map input ring\n");
-		kfree(jrp->inpring);
-		kfree(jrp->outring);
-		kfree(jrp->entinfo);
-		return -EIO;
-	}
-
-	outbusaddr = dma_map_single(dev, jrp->outring,
-				    sizeof(struct jr_outentry) * JOBR_DEPTH,
-				    DMA_BIDIRECTIONAL);
-	if (dma_mapping_error(dev, outbusaddr)) {
-		dev_err(dev, "caam_jr_init(): can't map output ring\n");
-			dma_unmap_single(dev, inpbusaddr,
-					 sizeof(u32 *) * JOBR_DEPTH,
-					 DMA_BIDIRECTIONAL);
-		kfree(jrp->inpring);
-		kfree(jrp->outring);
-		kfree(jrp->entinfo);
-		return -EIO;
-	}
-
 	jrp->inp_ring_write_index = 0;
 	jrp->out_ring_read_index = 0;
 	jrp->head = 0;
@@ -431,12 +390,11 @@ int caam_jr_shutdown(struct device *dev)
 {
 	struct caam_drv_private_jr *jrp = dev_get_drvdata(dev);
 	dma_addr_t inpbusaddr, outbusaddr;
-	int ret, i;
+	int ret;
 
 	ret = caam_reset_hw_jr(dev);
 
-	for_each_possible_cpu(i)
-		tasklet_kill(&jrp->irqtask[i]);
+	tasklet_kill(&jrp->irqtask);
 
 	/* Release interrupt */
 	free_irq(jrp->irq, dev);
@@ -444,13 +402,10 @@ int caam_jr_shutdown(struct device *dev)
 	/* Free rings */
 	inpbusaddr = rd_reg64(&jrp->rregs->inpring_base);
 	outbusaddr = rd_reg64(&jrp->rregs->outring_base);
-	dma_unmap_single(dev, outbusaddr,
-			 sizeof(struct jr_outentry) * JOBR_DEPTH,
-			 DMA_BIDIRECTIONAL);
-	dma_unmap_single(dev, inpbusaddr, sizeof(u32 *) * JOBR_DEPTH,
-			 DMA_BIDIRECTIONAL);
-	kfree(jrp->outring);
-	kfree(jrp->inpring);
+	dma_free_coherent(dev, sizeof(dma_addr_t) * JOBR_DEPTH,
+			  jrp->inpring, inpbusaddr);
+	dma_free_coherent(dev, sizeof(struct jr_outentry) * JOBR_DEPTH,
+			  jrp->outring, outbusaddr);
 	kfree(jrp->entinfo);
 
 	return ret;
@@ -503,6 +458,14 @@ int caam_jr_probe(struct platform_device *pdev, struct device_node *np,
 	dev_set_drvdata(jrdev, jrpriv);
 	ctrlpriv->jrdev[ring] = jrdev;
 
+	if (sizeof(dma_addr_t) == sizeof(u64))
+		if (of_device_is_compatible(np, "fsl,sec-v5.0-job-ring"))
+			dma_set_mask(jrdev, DMA_BIT_MASK(40));
+		else
+			dma_set_mask(jrdev, DMA_BIT_MASK(36));
+	else
+		dma_set_mask(jrdev, DMA_BIT_MASK(32));
+
 	/* Identify the interrupt */
 	jrpriv->irq = of_irq_to_resource(np, 0, NULL);
 
diff --git a/drivers/crypto/caam/key_gen.c b/drivers/crypto/caam/key_gen.c
new file mode 100644
index 00000000000..002888185f1
--- /dev/null
+++ b/drivers/crypto/caam/key_gen.c
@@ -0,0 +1,122 @@
+/*
+ * CAAM/SEC 4.x functions for handling key-generation jobs
+ *
+ * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ *
+ */
+#include "compat.h"
+#include "jr.h"
+#include "error.h"
+#include "desc_constr.h"
+#include "key_gen.h"
+
+void split_key_done(struct device *dev, u32 *desc, u32 err,
+			   void *context)
+{
+	struct split_key_result *res = context;
+
+#ifdef DEBUG
+	dev_err(dev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(dev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+	res->err = err;
+
+	complete(&res->completion);
+}
+EXPORT_SYMBOL(split_key_done);
+/*
+get a split ipad/opad key
+
+Split key generation-----------------------------------------------
+
+[00] 0xb0810008    jobdesc: stidx=1 share=never len=8
+[01] 0x04000014        key: class2->keyreg len=20
+			@0xffe01000
+[03] 0x84410014  operation: cls2-op sha1 hmac init dec
+[04] 0x24940000     fifold: class2 msgdata-last2 len=0 imm
+[05] 0xa4000001       jump: class2 local all ->1 [06]
+[06] 0x64260028    fifostr: class2 mdsplit-jdk len=40
+			@0xffe04000
+*/
+u32 gen_split_key(struct device *jrdev, u8 *key_out, int split_key_len,
+		  int split_key_pad_len, const u8 *key_in, u32 keylen,
+		  u32 alg_op)
+{
+	u32 *desc;
+	struct split_key_result result;
+	dma_addr_t dma_addr_in, dma_addr_out;
+	int ret = 0;
+
+	desc = kmalloc(CAAM_CMD_SZ * 6 + CAAM_PTR_SZ * 2, GFP_KERNEL | GFP_DMA);
+
+	init_job_desc(desc, 0);
+
+	dma_addr_in = dma_map_single(jrdev, (void *)key_in, keylen,
+				     DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, dma_addr_in)) {
+		dev_err(jrdev, "unable to map key input memory\n");
+		kfree(desc);
+		return -ENOMEM;
+	}
+	append_key(desc, dma_addr_in, keylen, CLASS_2 | KEY_DEST_CLASS_REG);
+
+	/* Sets MDHA up into an HMAC-INIT */
+	append_operation(desc, alg_op | OP_ALG_DECRYPT | OP_ALG_AS_INIT);
+
+	/*
+	 * do a FIFO_LOAD of zero, this will trigger the internal key expansion
+	 * into both pads inside MDHA
+	 */
+	append_fifo_load_as_imm(desc, NULL, 0, LDST_CLASS_2_CCB |
+				FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST2);
+
+	/*
+	 * FIFO_STORE with the explicit split-key content store
+	 * (0x26 output type)
+	 */
+	dma_addr_out = dma_map_single(jrdev, key_out, split_key_pad_len,
+				      DMA_FROM_DEVICE);
+	if (dma_mapping_error(jrdev, dma_addr_out)) {
+		dev_err(jrdev, "unable to map key output memory\n");
+		kfree(desc);
+		return -ENOMEM;
+	}
+	append_fifo_store(desc, dma_addr_out, split_key_len,
+			  LDST_CLASS_2_CCB | FIFOST_TYPE_SPLIT_KEK);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ctx.key@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key_in, keylen, 1);
+	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc), 1);
+#endif
+
+	result.err = 0;
+	init_completion(&result.completion);
+
+	ret = caam_jr_enqueue(jrdev, desc, split_key_done, &result);
+	if (!ret) {
+		/* in progress */
+		wait_for_completion_interruptible(&result.completion);
+		ret = result.err;
+#ifdef DEBUG
+		print_hex_dump(KERN_ERR, "ctx.key@"xstr(__LINE__)": ",
+			       DUMP_PREFIX_ADDRESS, 16, 4, key_out,
+			       split_key_pad_len, 1);
+#endif
+	}
+
+	dma_unmap_single(jrdev, dma_addr_out, split_key_pad_len,
+			 DMA_FROM_DEVICE);
+	dma_unmap_single(jrdev, dma_addr_in, keylen, DMA_TO_DEVICE);
+
+	kfree(desc);
+
+	return ret;
+}
diff --git a/drivers/crypto/caam/key_gen.h b/drivers/crypto/caam/key_gen.h
new file mode 100644
index 00000000000..d95d290c6e8
--- /dev/null
+++ b/drivers/crypto/caam/key_gen.h
@@ -0,0 +1,17 @@
+/*
+ * CAAM/SEC 4.x definitions for handling key-generation jobs
+ *
+ * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ *
+ */
+
+struct split_key_result {
+	struct completion completion;
+	int err;
+};
+
+void split_key_done(struct device *dev, u32 *desc, u32 err, void *context);
+
+u32 gen_split_key(struct device *jrdev, u8 *key_out, int split_key_len,
+		    int split_key_pad_len, const u8 *key_in, u32 keylen,
+		    u32 alg_op);
diff --git a/drivers/crypto/caam/pdb.h b/drivers/crypto/caam/pdb.h
new file mode 100644
index 00000000000..62950d22ac1
--- /dev/null
+++ b/drivers/crypto/caam/pdb.h
@@ -0,0 +1,401 @@
+/*
+ * CAAM Protocol Data Block (PDB) definition header file
+ *
+ * Copyright 2008-2012 Freescale Semiconductor, Inc.
+ *
+ */
+
+#ifndef CAAM_PDB_H
+#define CAAM_PDB_H
+
+/*
+ * PDB- IPSec ESP Header Modification Options
+ */
+#define PDBHMO_ESP_DECAP_SHIFT	12
+#define PDBHMO_ESP_ENCAP_SHIFT	4
+/*
+ * Encap and Decap - Decrement TTL (Hop Limit) - Based on the value of the
+ * Options Byte IP version (IPvsn) field:
+ * if IPv4, decrement the inner IP header TTL field (byte 8);
+ * if IPv6 decrement the inner IP header Hop Limit field (byte 7).
+*/
+#define PDBHMO_ESP_DECAP_DEC_TTL	(0x02 << PDBHMO_ESP_DECAP_SHIFT)
+#define PDBHMO_ESP_ENCAP_DEC_TTL	(0x02 << PDBHMO_ESP_ENCAP_SHIFT)
+/*
+ * Decap - DiffServ Copy - Copy the IPv4 TOS or IPv6 Traffic Class byte
+ * from the outer IP header to the inner IP header.
+ */
+#define PDBHMO_ESP_DIFFSERV		(0x01 << PDBHMO_ESP_DECAP_SHIFT)
+/*
+ * Encap- Copy DF bit -if an IPv4 tunnel mode outer IP header is coming from
+ * the PDB, copy the DF bit from the inner IP header to the outer IP header.
+ */
+#define PDBHMO_ESP_DFBIT		(0x04 << PDBHMO_ESP_ENCAP_SHIFT)
+
+/*
+ * PDB - IPSec ESP Encap/Decap Options
+ */
+#define PDBOPTS_ESP_ARSNONE	0x00 /* no antireplay window */
+#define PDBOPTS_ESP_ARS32	0x40 /* 32-entry antireplay window */
+#define PDBOPTS_ESP_ARS64	0xc0 /* 64-entry antireplay window */
+#define PDBOPTS_ESP_IVSRC	0x20 /* IV comes from internal random gen */
+#define PDBOPTS_ESP_ESN		0x10 /* extended sequence included */
+#define PDBOPTS_ESP_OUTFMT	0x08 /* output only decapsulation (decap) */
+#define PDBOPTS_ESP_IPHDRSRC	0x08 /* IP header comes from PDB (encap) */
+#define PDBOPTS_ESP_INCIPHDR	0x04 /* Prepend IP header to output frame */
+#define PDBOPTS_ESP_IPVSN	0x02 /* process IPv6 header */
+#define PDBOPTS_ESP_TUNNEL	0x01 /* tunnel mode next-header byte */
+#define PDBOPTS_ESP_IPV6	0x02 /* ip header version is V6 */
+#define PDBOPTS_ESP_DIFFSERV	0x40 /* copy TOS/TC from inner iphdr */
+#define PDBOPTS_ESP_UPDATE_CSUM 0x80 /* encap-update ip header checksum */
+#define PDBOPTS_ESP_VERIFY_CSUM 0x20 /* decap-validate ip header checksum */
+
+/*
+ * General IPSec encap/decap PDB definitions
+ */
+struct ipsec_encap_cbc {
+	u32 iv[4];
+};
+
+struct ipsec_encap_ctr {
+	u32 ctr_nonce;
+	u32 ctr_initial;
+	u32 iv[2];
+};
+
+struct ipsec_encap_ccm {
+	u32 salt; /* lower 24 bits */
+	u8 b0_flags;
+	u8 ctr_flags;
+	u16 ctr_initial;
+	u32 iv[2];
+};
+
+struct ipsec_encap_gcm {
+	u32 salt; /* lower 24 bits */
+	u32 rsvd1;
+	u32 iv[2];
+};
+
+struct ipsec_encap_pdb {
+	u8 hmo_rsvd;
+	u8 ip_nh;
+	u8 ip_nh_offset;
+	u8 options;
+	u32 seq_num_ext_hi;
+	u32 seq_num;
+	union {
+		struct ipsec_encap_cbc cbc;
+		struct ipsec_encap_ctr ctr;
+		struct ipsec_encap_ccm ccm;
+		struct ipsec_encap_gcm gcm;
+	};
+	u32 spi;
+	u16 rsvd1;
+	u16 ip_hdr_len;
+	u32 ip_hdr[0]; /* optional IP Header content */
+};
+
+struct ipsec_decap_cbc {
+	u32 rsvd[2];
+};
+
+struct ipsec_decap_ctr {
+	u32 salt;
+	u32 ctr_initial;
+};
+
+struct ipsec_decap_ccm {
+	u32 salt;
+	u8 iv_flags;
+	u8 ctr_flags;
+	u16 ctr_initial;
+};
+
+struct ipsec_decap_gcm {
+	u32 salt;
+	u32 resvd;
+};
+
+struct ipsec_decap_pdb {
+	u16 hmo_ip_hdr_len;
+	u8 ip_nh_offset;
+	u8 options;
+	union {
+		struct ipsec_decap_cbc cbc;
+		struct ipsec_decap_ctr ctr;
+		struct ipsec_decap_ccm ccm;
+		struct ipsec_decap_gcm gcm;
+	};
+	u32 seq_num_ext_hi;
+	u32 seq_num;
+	u32 anti_replay[2];
+	u32 end_index[0];
+};
+
+/*
+ * IPSec ESP Datapath Protocol Override Register (DPOVRD)
+ */
+struct ipsec_deco_dpovrd {
+#define IPSEC_ENCAP_DECO_DPOVRD_USE 0x80
+	u8 ovrd_ecn;
+	u8 ip_hdr_len;
+	u8 nh_offset;
+	u8 next_header; /* reserved if decap */
+};
+
+/*
+ * IEEE 802.11i WiFi Protocol Data Block
+ */
+#define WIFI_PDBOPTS_FCS	0x01
+#define WIFI_PDBOPTS_AR		0x40
+
+struct wifi_encap_pdb {
+	u16 mac_hdr_len;
+	u8 rsvd;
+	u8 options;
+	u8 iv_flags;
+	u8 pri;
+	u16 pn1;
+	u32 pn2;
+	u16 frm_ctrl_mask;
+	u16 seq_ctrl_mask;
+	u8 rsvd1[2];
+	u8 cnst;
+	u8 key_id;
+	u8 ctr_flags;
+	u8 rsvd2;
+	u16 ctr_init;
+};
+
+struct wifi_decap_pdb {
+	u16 mac_hdr_len;
+	u8 rsvd;
+	u8 options;
+	u8 iv_flags;
+	u8 pri;
+	u16 pn1;
+	u32 pn2;
+	u16 frm_ctrl_mask;
+	u16 seq_ctrl_mask;
+	u8 rsvd1[4];
+	u8 ctr_flags;
+	u8 rsvd2;
+	u16 ctr_init;
+};
+
+/*
+ * IEEE 802.16 WiMAX Protocol Data Block
+ */
+#define WIMAX_PDBOPTS_FCS	0x01
+#define WIMAX_PDBOPTS_AR	0x40 /* decap only */
+
+struct wimax_encap_pdb {
+	u8 rsvd[3];
+	u8 options;
+	u32 nonce;
+	u8 b0_flags;
+	u8 ctr_flags;
+	u16 ctr_init;
+	/* begin DECO writeback region */
+	u32 pn;
+	/* end DECO writeback region */
+};
+
+struct wimax_decap_pdb {
+	u8 rsvd[3];
+	u8 options;
+	u32 nonce;
+	u8 iv_flags;
+	u8 ctr_flags;
+	u16 ctr_init;
+	/* begin DECO writeback region */
+	u32 pn;
+	u8 rsvd1[2];
+	u16 antireplay_len;
+	u64 antireplay_scorecard;
+	/* end DECO writeback region */
+};
+
+/*
+ * IEEE 801.AE MacSEC Protocol Data Block
+ */
+#define MACSEC_PDBOPTS_FCS	0x01
+#define MACSEC_PDBOPTS_AR	0x40 /* used in decap only */
+
+struct macsec_encap_pdb {
+	u16 aad_len;
+	u8 rsvd;
+	u8 options;
+	u64 sci;
+	u16 ethertype;
+	u8 tci_an;
+	u8 rsvd1;
+	/* begin DECO writeback region */
+	u32 pn;
+	/* end DECO writeback region */
+};
+
+struct macsec_decap_pdb {
+	u16 aad_len;
+	u8 rsvd;
+	u8 options;
+	u64 sci;
+	u8 rsvd1[3];
+	/* begin DECO writeback region */
+	u8 antireplay_len;
+	u32 pn;
+	u64 antireplay_scorecard;
+	/* end DECO writeback region */
+};
+
+/*
+ * SSL/TLS/DTLS Protocol Data Blocks
+ */
+
+#define TLS_PDBOPTS_ARS32	0x40
+#define TLS_PDBOPTS_ARS64	0xc0
+#define TLS_PDBOPTS_OUTFMT	0x08
+#define TLS_PDBOPTS_IV_WRTBK	0x02 /* 1.1/1.2/DTLS only */
+#define TLS_PDBOPTS_EXP_RND_IV	0x01 /* 1.1/1.2/DTLS only */
+
+struct tls_block_encap_pdb {
+	u8 type;
+	u8 version[2];
+	u8 options;
+	u64 seq_num;
+	u32 iv[4];
+};
+
+struct tls_stream_encap_pdb {
+	u8 type;
+	u8 version[2];
+	u8 options;
+	u64 seq_num;
+	u8 i;
+	u8 j;
+	u8 rsvd1[2];
+};
+
+struct dtls_block_encap_pdb {
+	u8 type;
+	u8 version[2];
+	u8 options;
+	u16 epoch;
+	u16 seq_num[3];
+	u32 iv[4];
+};
+
+struct tls_block_decap_pdb {
+	u8 rsvd[3];
+	u8 options;
+	u64 seq_num;
+	u32 iv[4];
+};
+
+struct tls_stream_decap_pdb {
+	u8 rsvd[3];
+	u8 options;
+	u64 seq_num;
+	u8 i;
+	u8 j;
+	u8 rsvd1[2];
+};
+
+struct dtls_block_decap_pdb {
+	u8 rsvd[3];
+	u8 options;
+	u16 epoch;
+	u16 seq_num[3];
+	u32 iv[4];
+	u64 antireplay_scorecard;
+};
+
+/*
+ * SRTP Protocol Data Blocks
+ */
+#define SRTP_PDBOPTS_MKI	0x08
+#define SRTP_PDBOPTS_AR		0x40
+
+struct srtp_encap_pdb {
+	u8 x_len;
+	u8 mki_len;
+	u8 n_tag;
+	u8 options;
+	u32 cnst0;
+	u8 rsvd[2];
+	u16 cnst1;
+	u16 salt[7];
+	u16 cnst2;
+	u32 rsvd1;
+	u32 roc;
+	u32 opt_mki;
+};
+
+struct srtp_decap_pdb {
+	u8 x_len;
+	u8 mki_len;
+	u8 n_tag;
+	u8 options;
+	u32 cnst0;
+	u8 rsvd[2];
+	u16 cnst1;
+	u16 salt[7];
+	u16 cnst2;
+	u16 rsvd1;
+	u16 seq_num;
+	u32 roc;
+	u64 antireplay_scorecard;
+};
+
+/*
+ * DSA/ECDSA Protocol Data Blocks
+ * Two of these exist: DSA-SIGN, and DSA-VERIFY. They are similar
+ * except for the treatment of "w" for verify, "s" for sign,
+ * and the placement of "a,b".
+ */
+#define DSA_PDB_SGF_SHIFT	24
+#define DSA_PDB_SGF_MASK	(0xff << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_Q		(0x80 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_R		(0x40 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_G		(0x20 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_W		(0x10 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_S		(0x10 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_F		(0x08 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_C		(0x04 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_D		(0x02 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_AB_SIGN	(0x02 << DSA_PDB_SGF_SHIFT)
+#define DSA_PDB_SGF_AB_VERIFY	(0x01 << DSA_PDB_SGF_SHIFT)
+
+#define DSA_PDB_L_SHIFT		7
+#define DSA_PDB_L_MASK		(0x3ff << DSA_PDB_L_SHIFT)
+
+#define DSA_PDB_N_MASK		0x7f
+
+struct dsa_sign_pdb {
+	u32 sgf_ln; /* Use DSA_PDB_ defintions per above */
+	u8 *q;
+	u8 *r;
+	u8 *g;	/* or Gx,y */
+	u8 *s;
+	u8 *f;
+	u8 *c;
+	u8 *d;
+	u8 *ab; /* ECC only */
+	u8 *u;
+};
+
+struct dsa_verify_pdb {
+	u32 sgf_ln;
+	u8 *q;
+	u8 *r;
+	u8 *g;	/* or Gx,y */
+	u8 *w; /* or Wx,y */
+	u8 *f;
+	u8 *c;
+	u8 *d;
+	u8 *tmp; /* temporary data block */
+	u8 *ab; /* only used if ECC processing */
+};
+
+#endif
diff --git a/drivers/crypto/caam/regs.h b/drivers/crypto/caam/regs.h
index e9f7a70cdd5..3223fc6d647 100644
--- a/drivers/crypto/caam/regs.h
+++ b/drivers/crypto/caam/regs.h
@@ -117,6 +117,12 @@ struct jr_outentry {
 #define CHA_NUM_DECONUM_SHIFT	56
 #define CHA_NUM_DECONUM_MASK	(0xfull << CHA_NUM_DECONUM_SHIFT)
 
+struct sec_vid {
+	u16 ip_id;
+	u8 maj_rev;
+	u8 min_rev;
+};
+
 struct caam_perfmon {
 	/* Performance Monitor Registers			f00-f9f */
 	u64 req_dequeued;	/* PC_REQ_DEQ - Dequeued Requests	     */
@@ -167,7 +173,7 @@ struct partid {
 	u32 pidr;	/* partition ID, DECO */
 };
 
-/* RNG test mode (replicated twice in some configurations) */
+/* RNGB test mode (replicated twice in some configurations) */
 /* Padded out to 0x100 */
 struct rngtst {
 	u32 mode;		/* RTSTMODEx - Test mode */
@@ -200,6 +206,31 @@ struct rngtst {
 	u32 rsvd14[15];
 };
 
+/* RNG4 TRNG test registers */
+struct rng4tst {
+#define RTMCTL_PRGM 0x00010000	/* 1 -> program mode, 0 -> run mode */
+	u32 rtmctl;		/* misc. control register */
+	u32 rtscmisc;		/* statistical check misc. register */
+	u32 rtpkrrng;		/* poker range register */
+	union {
+		u32 rtpkrmax;	/* PRGM=1: poker max. limit register */
+		u32 rtpkrsq;	/* PRGM=0: poker square calc. result register */
+	};
+#define RTSDCTL_ENT_DLY_SHIFT 16
+#define RTSDCTL_ENT_DLY_MASK (0xffff << RTSDCTL_ENT_DLY_SHIFT)
+	u32 rtsdctl;		/* seed control register */
+	union {
+		u32 rtsblim;	/* PRGM=1: sparse bit limit register */
+		u32 rttotsam;	/* PRGM=0: total samples register */
+	};
+	u32 rtfrqmin;		/* frequency count min. limit register */
+	union {
+		u32 rtfrqmax;	/* PRGM=1: freq. count max. limit register */
+		u32 rtfrqcnt;	/* PRGM=0: freq. count register */
+	};
+	u32 rsvd1[56];
+};
+
 /*
  * caam_ctrl - basic core configuration
  * starts base + 0x0000 padded out to 0x1000
@@ -249,7 +280,10 @@ struct caam_ctrl {
 
 	/* RNG Test/Verification/Debug Access                   600-7ff */
 	/* (Useful in Test/Debug modes only...)                         */
-	struct rngtst rtst[2];
+	union {
+		struct rngtst rtst[2];
+		struct rng4tst r4tst[2];
+	};
 
 	u32 rsvd9[448];
 
diff --git a/drivers/crypto/caam/sg_sw_sec4.h b/drivers/crypto/caam/sg_sw_sec4.h
new file mode 100644
index 00000000000..e0037c8ee24
--- /dev/null
+++ b/drivers/crypto/caam/sg_sw_sec4.h
@@ -0,0 +1,156 @@
+/*
+ * CAAM/SEC 4.x functions for using scatterlists in caam driver
+ *
+ * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ *
+ */
+
+struct sec4_sg_entry;
+
+/*
+ * convert single dma address to h/w link table format
+ */
+static inline void dma_to_sec4_sg_one(struct sec4_sg_entry *sec4_sg_ptr,
+				      dma_addr_t dma, u32 len, u32 offset)
+{
+	sec4_sg_ptr->ptr = dma;
+	sec4_sg_ptr->len = len;
+	sec4_sg_ptr->reserved = 0;
+	sec4_sg_ptr->buf_pool_id = 0;
+	sec4_sg_ptr->offset = offset;
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "sec4_sg_ptr@: ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sec4_sg_ptr,
+		       sizeof(struct sec4_sg_entry), 1);
+#endif
+}
+
+/*
+ * convert scatterlist to h/w link table format
+ * but does not have final bit; instead, returns last entry
+ */
+static inline struct sec4_sg_entry *
+sg_to_sec4_sg(struct scatterlist *sg, int sg_count,
+	      struct sec4_sg_entry *sec4_sg_ptr, u32 offset)
+{
+	while (sg_count) {
+		dma_to_sec4_sg_one(sec4_sg_ptr, sg_dma_address(sg),
+				   sg_dma_len(sg), offset);
+		sec4_sg_ptr++;
+		sg = scatterwalk_sg_next(sg);
+		sg_count--;
+	}
+	return sec4_sg_ptr - 1;
+}
+
+/*
+ * convert scatterlist to h/w link table format
+ * scatterlist must have been previously dma mapped
+ */
+static inline void sg_to_sec4_sg_last(struct scatterlist *sg, int sg_count,
+				      struct sec4_sg_entry *sec4_sg_ptr,
+				      u32 offset)
+{
+	sec4_sg_ptr = sg_to_sec4_sg(sg, sg_count, sec4_sg_ptr, offset);
+	sec4_sg_ptr->len |= SEC4_SG_LEN_FIN;
+}
+
+/* count number of elements in scatterlist */
+static inline int __sg_count(struct scatterlist *sg_list, int nbytes,
+			     bool *chained)
+{
+	struct scatterlist *sg = sg_list;
+	int sg_nents = 0;
+
+	while (nbytes > 0) {
+		sg_nents++;
+		nbytes -= sg->length;
+		if (!sg_is_last(sg) && (sg + 1)->length == 0)
+			*chained = true;
+		sg = scatterwalk_sg_next(sg);
+	}
+
+	return sg_nents;
+}
+
+/* derive number of elements in scatterlist, but return 0 for 1 */
+static inline int sg_count(struct scatterlist *sg_list, int nbytes,
+			     bool *chained)
+{
+	int sg_nents = __sg_count(sg_list, nbytes, chained);
+
+	if (likely(sg_nents == 1))
+		return 0;
+
+	return sg_nents;
+}
+
+static int dma_map_sg_chained(struct device *dev, struct scatterlist *sg,
+			      unsigned int nents, enum dma_data_direction dir,
+			      bool chained)
+{
+	if (unlikely(chained)) {
+		int i;
+		for (i = 0; i < nents; i++) {
+			dma_map_sg(dev, sg, 1, dir);
+			sg = scatterwalk_sg_next(sg);
+		}
+	} else {
+		dma_map_sg(dev, sg, nents, dir);
+	}
+	return nents;
+}
+
+static int dma_unmap_sg_chained(struct device *dev, struct scatterlist *sg,
+				unsigned int nents, enum dma_data_direction dir,
+				bool chained)
+{
+	if (unlikely(chained)) {
+		int i;
+		for (i = 0; i < nents; i++) {
+			dma_unmap_sg(dev, sg, 1, dir);
+			sg = scatterwalk_sg_next(sg);
+		}
+	} else {
+		dma_unmap_sg(dev, sg, nents, dir);
+	}
+	return nents;
+}
+
+/* Copy from len bytes of sg to dest, starting from beginning */
+static inline void sg_copy(u8 *dest, struct scatterlist *sg, unsigned int len)
+{
+	struct scatterlist *current_sg = sg;
+	int cpy_index = 0, next_cpy_index = current_sg->length;
+
+	while (next_cpy_index < len) {
+		memcpy(dest + cpy_index, (u8 *) sg_virt(current_sg),
+		       current_sg->length);
+		current_sg = scatterwalk_sg_next(current_sg);
+		cpy_index = next_cpy_index;
+		next_cpy_index += current_sg->length;
+	}
+	if (cpy_index < len)
+		memcpy(dest + cpy_index, (u8 *) sg_virt(current_sg),
+		       len - cpy_index);
+}
+
+/* Copy sg data, from to_skip to end, to dest */
+static inline void sg_copy_part(u8 *dest, struct scatterlist *sg,
+				      int to_skip, unsigned int end)
+{
+	struct scatterlist *current_sg = sg;
+	int sg_index, cpy_index;
+
+	sg_index = current_sg->length;
+	while (sg_index <= to_skip) {
+		current_sg = scatterwalk_sg_next(current_sg);
+		sg_index += current_sg->length;
+	}
+	cpy_index = sg_index - to_skip;
+	memcpy(dest, (u8 *) sg_virt(current_sg) +
+	       current_sg->length - cpy_index, cpy_index);
+	current_sg = scatterwalk_sg_next(current_sg);
+	if (end - sg_index)
+		sg_copy(dest + cpy_index, current_sg, end - sg_index);
+}
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index 1cc6b3f3e26..21c1a87032b 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -24,6 +24,7 @@
 
 #define MV_CESA	"MV-CESA:"
 #define MAX_HW_HASH_SIZE	0xFFFF
+#define MV_CESA_EXPIRE		500 /* msec */
 
 /*
  * STM:
@@ -87,6 +88,7 @@ struct crypto_priv {
 	spinlock_t lock;
 	struct crypto_queue queue;
 	enum engine_status eng_st;
+	struct timer_list completion_timer;
 	struct crypto_async_request *cur_req;
 	struct req_progress p;
 	int max_req_size;
@@ -138,6 +140,29 @@ struct mv_req_hash_ctx {
 	int count_add;
 };
 
+static void mv_completion_timer_callback(unsigned long unused)
+{
+	int active = readl(cpg->reg + SEC_ACCEL_CMD) & SEC_CMD_EN_SEC_ACCL0;
+
+	printk(KERN_ERR MV_CESA
+	       "completion timer expired (CESA %sactive), cleaning up.\n",
+	       active ? "" : "in");
+
+	del_timer(&cpg->completion_timer);
+	writel(SEC_CMD_DISABLE_SEC, cpg->reg + SEC_ACCEL_CMD);
+	while(readl(cpg->reg + SEC_ACCEL_CMD) & SEC_CMD_DISABLE_SEC)
+		printk(KERN_INFO MV_CESA "%s: waiting for engine finishing\n", __func__);
+	cpg->eng_st = ENGINE_W_DEQUEUE;
+	wake_up_process(cpg->queue_th);
+}
+
+static void mv_setup_timer(void)
+{
+	setup_timer(&cpg->completion_timer, &mv_completion_timer_callback, 0);
+	mod_timer(&cpg->completion_timer,
+			jiffies + msecs_to_jiffies(MV_CESA_EXPIRE));
+}
+
 static void compute_aes_dec_key(struct mv_ctx *ctx)
 {
 	struct crypto_aes_ctx gen_aes_key;
@@ -273,12 +298,8 @@ static void mv_process_current_q(int first_block)
 			sizeof(struct sec_accel_config));
 
 	/* GO */
+	mv_setup_timer();
 	writel(SEC_CMD_EN_SEC_ACCL0, cpg->reg + SEC_ACCEL_CMD);
-
-	/*
-	 * XXX: add timer if the interrupt does not occur for some mystery
-	 * reason
-	 */
 }
 
 static void mv_crypto_algo_completion(void)
@@ -357,12 +378,8 @@ static void mv_process_hash_current(int first_block)
 	memcpy(cpg->sram + SRAM_CONFIG, &op, sizeof(struct sec_accel_config));
 
 	/* GO */
+	mv_setup_timer();
 	writel(SEC_CMD_EN_SEC_ACCL0, cpg->reg + SEC_ACCEL_CMD);
-
-	/*
-	* XXX: add timer if the interrupt does not occur for some mystery
-	* reason
-	*/
 }
 
 static inline int mv_hash_import_sha1_ctx(const struct mv_req_hash_ctx *ctx,
@@ -406,6 +423,15 @@ out:
 	return rc;
 }
 
+static void mv_save_digest_state(struct mv_req_hash_ctx *ctx)
+{
+	ctx->state[0] = readl(cpg->reg + DIGEST_INITIAL_VAL_A);
+	ctx->state[1] = readl(cpg->reg + DIGEST_INITIAL_VAL_B);
+	ctx->state[2] = readl(cpg->reg + DIGEST_INITIAL_VAL_C);
+	ctx->state[3] = readl(cpg->reg + DIGEST_INITIAL_VAL_D);
+	ctx->state[4] = readl(cpg->reg + DIGEST_INITIAL_VAL_E);
+}
+
 static void mv_hash_algo_completion(void)
 {
 	struct ahash_request *req = ahash_request_cast(cpg->cur_req);
@@ -420,14 +446,12 @@ static void mv_hash_algo_completion(void)
 			memcpy(req->result, cpg->sram + SRAM_DIGEST_BUF,
 			       crypto_ahash_digestsize(crypto_ahash_reqtfm
 						       (req)));
-		} else
+		} else {
+			mv_save_digest_state(ctx);
 			mv_hash_final_fallback(req);
+		}
 	} else {
-		ctx->state[0] = readl(cpg->reg + DIGEST_INITIAL_VAL_A);
-		ctx->state[1] = readl(cpg->reg + DIGEST_INITIAL_VAL_B);
-		ctx->state[2] = readl(cpg->reg + DIGEST_INITIAL_VAL_C);
-		ctx->state[3] = readl(cpg->reg + DIGEST_INITIAL_VAL_D);
-		ctx->state[4] = readl(cpg->reg + DIGEST_INITIAL_VAL_E);
+		mv_save_digest_state(ctx);
 	}
 }
 
@@ -888,6 +912,10 @@ irqreturn_t crypto_int(int irq, void *priv)
 	if (!(val & SEC_INT_ACCEL0_DONE))
 		return IRQ_NONE;
 
+	if (!del_timer(&cpg->completion_timer)) {
+		printk(KERN_WARNING MV_CESA
+		       "got an interrupt but no pending timer?\n");
+	}
 	val &= ~SEC_INT_ACCEL0_DONE;
 	writel(val, cpg->reg + FPGA_INT_STATUS);
 	writel(val, cpg->reg + SEC_ACCEL_INT_STATUS);
@@ -1061,6 +1089,7 @@ static int mv_probe(struct platform_device *pdev)
 	if (!IS_ERR(cp->clk))
 		clk_prepare_enable(cp->clk);
 
+	writel(0, cpg->reg + SEC_ACCEL_INT_STATUS);
 	writel(SEC_INT_ACCEL0_DONE, cpg->reg + SEC_ACCEL_INT_MASK);
 	writel(SEC_CFG_STOP_DIG_ERR, cpg->reg + SEC_ACCEL_CFG);
 	writel(SRAM_CONFIG, cpg->reg + SEC_ACCEL_DESC_P0);
@@ -1098,6 +1127,10 @@ err_unreg_ecb:
 	crypto_unregister_alg(&mv_aes_alg_ecb);
 err_irq:
 	free_irq(irq, cp);
+	if (!IS_ERR(cp->clk)) {
+		clk_disable_unprepare(cp->clk);
+		clk_put(cp->clk);
+	}
 err_thread:
 	kthread_stop(cp->queue_th);
 err_unmap_sram:
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 67b97c5fd85..a8bd0310f8f 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -1610,8 +1610,7 @@ static int spu_map_ino(struct platform_device *dev, struct spu_mdesc_info *ip,
 
 	sprintf(p->irq_name, "%s-%d", irq_name, index);
 
-	return request_irq(p->irq, handler, IRQF_SAMPLE_RANDOM,
-			   p->irq_name, p);
+	return request_irq(p->irq, handler, 0, p->irq_name, p);
 }
 
 static struct kmem_cache *queue_cache[2];
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index 921039e56f8..efff788d2f1 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -53,117 +53,6 @@
 
 #include "talitos.h"
 
-#define TALITOS_TIMEOUT 100000
-#define TALITOS_MAX_DATA_LEN 65535
-
-#define DESC_TYPE(desc_hdr) ((be32_to_cpu(desc_hdr) >> 3) & 0x1f)
-#define PRIMARY_EU(desc_hdr) ((be32_to_cpu(desc_hdr) >> 28) & 0xf)
-#define SECONDARY_EU(desc_hdr) ((be32_to_cpu(desc_hdr) >> 16) & 0xf)
-
-/* descriptor pointer entry */
-struct talitos_ptr {
-	__be16 len;	/* length */
-	u8 j_extent;	/* jump to sg link table and/or extent */
-	u8 eptr;	/* extended address */
-	__be32 ptr;	/* address */
-};
-
-static const struct talitos_ptr zero_entry = {
-	.len = 0,
-	.j_extent = 0,
-	.eptr = 0,
-	.ptr = 0
-};
-
-/* descriptor */
-struct talitos_desc {
-	__be32 hdr;			/* header high bits */
-	__be32 hdr_lo;			/* header low bits */
-	struct talitos_ptr ptr[7];	/* ptr/len pair array */
-};
-
-/**
- * talitos_request - descriptor submission request
- * @desc: descriptor pointer (kernel virtual)
- * @dma_desc: descriptor's physical bus address
- * @callback: whom to call when descriptor processing is done
- * @context: caller context (optional)
- */
-struct talitos_request {
-	struct talitos_desc *desc;
-	dma_addr_t dma_desc;
-	void (*callback) (struct device *dev, struct talitos_desc *desc,
-	                  void *context, int error);
-	void *context;
-};
-
-/* per-channel fifo management */
-struct talitos_channel {
-	void __iomem *reg;
-
-	/* request fifo */
-	struct talitos_request *fifo;
-
-	/* number of requests pending in channel h/w fifo */
-	atomic_t submit_count ____cacheline_aligned;
-
-	/* request submission (head) lock */
-	spinlock_t head_lock ____cacheline_aligned;
-	/* index to next free descriptor request */
-	int head;
-
-	/* request release (tail) lock */
-	spinlock_t tail_lock ____cacheline_aligned;
-	/* index to next in-progress/done descriptor request */
-	int tail;
-};
-
-struct talitos_private {
-	struct device *dev;
-	struct platform_device *ofdev;
-	void __iomem *reg;
-	int irq[2];
-
-	/* SEC global registers lock  */
-	spinlock_t reg_lock ____cacheline_aligned;
-
-	/* SEC version geometry (from device tree node) */
-	unsigned int num_channels;
-	unsigned int chfifo_len;
-	unsigned int exec_units;
-	unsigned int desc_types;
-
-	/* SEC Compatibility info */
-	unsigned long features;
-
-	/*
-	 * length of the request fifo
-	 * fifo_len is chfifo_len rounded up to next power of 2
-	 * so we can use bitwise ops to wrap
-	 */
-	unsigned int fifo_len;
-
-	struct talitos_channel *chan;
-
-	/* next channel to be assigned next incoming descriptor */
-	atomic_t last_chan ____cacheline_aligned;
-
-	/* request callback tasklet */
-	struct tasklet_struct done_task[2];
-
-	/* list of registered algorithms */
-	struct list_head alg_list;
-
-	/* hwrng device */
-	struct hwrng rng;
-};
-
-/* .features flag */
-#define TALITOS_FTR_SRC_LINK_TBL_LEN_INCLUDES_EXTENT 0x00000001
-#define TALITOS_FTR_HW_AUTH_CHECK 0x00000002
-#define TALITOS_FTR_SHA224_HWINIT 0x00000004
-#define TALITOS_FTR_HMAC_OK 0x00000008
-
 static void to_talitos_ptr(struct talitos_ptr *talitos_ptr, dma_addr_t dma_addr)
 {
 	talitos_ptr->ptr = cpu_to_be32(lower_32_bits(dma_addr));
@@ -303,11 +192,11 @@ static int init_device(struct device *dev)
  * callback must check err and feedback in descriptor header
  * for device processing status.
  */
-static int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
-			  void (*callback)(struct device *dev,
-					   struct talitos_desc *desc,
-					   void *context, int error),
-			  void *context)
+int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
+		   void (*callback)(struct device *dev,
+				    struct talitos_desc *desc,
+				    void *context, int error),
+		   void *context)
 {
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	struct talitos_request *request;
@@ -348,6 +237,7 @@ static int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
 
 	return -EINPROGRESS;
 }
+EXPORT_SYMBOL(talitos_submit);
 
 /*
  * process what was done, notify callback of error if not
@@ -733,7 +623,7 @@ static void talitos_unregister_rng(struct device *dev)
  * crypto alg
  */
 #define TALITOS_CRA_PRIORITY		3000
-#define TALITOS_MAX_KEY_SIZE		64
+#define TALITOS_MAX_KEY_SIZE		96
 #define TALITOS_MAX_IV_LENGTH		16 /* max of AES_BLOCK_SIZE, DES3_EDE_BLOCK_SIZE */
 
 #define MD5_BLOCK_SIZE    64
@@ -2066,6 +1956,59 @@ static struct talitos_alg_template driver_algs[] = {
 		                     DESC_HDR_MODE1_MDEU_PAD |
 		                     DESC_HDR_MODE1_MDEU_SHA1_HMAC,
 	},
+	{       .type = CRYPTO_ALG_TYPE_AEAD,
+		.alg.crypto = {
+			.cra_name = "authenc(hmac(sha224),cbc(aes))",
+			.cra_driver_name = "authenc-hmac-sha224-cbc-aes-talitos",
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_type = &crypto_aead_type,
+			.cra_aead = {
+				.setkey = aead_setkey,
+				.setauthsize = aead_setauthsize,
+				.encrypt = aead_encrypt,
+				.decrypt = aead_decrypt,
+				.givencrypt = aead_givencrypt,
+				.geniv = "<built-in>",
+				.ivsize = AES_BLOCK_SIZE,
+				.maxauthsize = SHA224_DIGEST_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_IPSEC_ESP |
+				     DESC_HDR_SEL0_AESU |
+				     DESC_HDR_MODE0_AESU_CBC |
+				     DESC_HDR_SEL1_MDEUA |
+				     DESC_HDR_MODE1_MDEU_INIT |
+				     DESC_HDR_MODE1_MDEU_PAD |
+				     DESC_HDR_MODE1_MDEU_SHA224_HMAC,
+	},
+	{	.type = CRYPTO_ALG_TYPE_AEAD,
+		.alg.crypto = {
+			.cra_name = "authenc(hmac(sha224),cbc(des3_ede))",
+			.cra_driver_name = "authenc-hmac-sha224-cbc-3des-talitos",
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_type = &crypto_aead_type,
+			.cra_aead = {
+				.setkey = aead_setkey,
+				.setauthsize = aead_setauthsize,
+				.encrypt = aead_encrypt,
+				.decrypt = aead_decrypt,
+				.givencrypt = aead_givencrypt,
+				.geniv = "<built-in>",
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+				.maxauthsize = SHA224_DIGEST_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_IPSEC_ESP |
+			             DESC_HDR_SEL0_DEU |
+		                     DESC_HDR_MODE0_DEU_CBC |
+		                     DESC_HDR_MODE0_DEU_3DES |
+		                     DESC_HDR_SEL1_MDEUA |
+		                     DESC_HDR_MODE1_MDEU_INIT |
+		                     DESC_HDR_MODE1_MDEU_PAD |
+		                     DESC_HDR_MODE1_MDEU_SHA224_HMAC,
+	},
 	{	.type = CRYPTO_ALG_TYPE_AEAD,
 		.alg.crypto = {
 			.cra_name = "authenc(hmac(sha256),cbc(aes))",
@@ -2121,6 +2064,112 @@ static struct talitos_alg_template driver_algs[] = {
 	},
 	{	.type = CRYPTO_ALG_TYPE_AEAD,
 		.alg.crypto = {
+			.cra_name = "authenc(hmac(sha384),cbc(aes))",
+			.cra_driver_name = "authenc-hmac-sha384-cbc-aes-talitos",
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_type = &crypto_aead_type,
+			.cra_aead = {
+				.setkey = aead_setkey,
+				.setauthsize = aead_setauthsize,
+				.encrypt = aead_encrypt,
+				.decrypt = aead_decrypt,
+				.givencrypt = aead_givencrypt,
+				.geniv = "<built-in>",
+				.ivsize = AES_BLOCK_SIZE,
+				.maxauthsize = SHA384_DIGEST_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_IPSEC_ESP |
+			             DESC_HDR_SEL0_AESU |
+		                     DESC_HDR_MODE0_AESU_CBC |
+		                     DESC_HDR_SEL1_MDEUB |
+		                     DESC_HDR_MODE1_MDEU_INIT |
+		                     DESC_HDR_MODE1_MDEU_PAD |
+		                     DESC_HDR_MODE1_MDEUB_SHA384_HMAC,
+	},
+	{	.type = CRYPTO_ALG_TYPE_AEAD,
+		.alg.crypto = {
+			.cra_name = "authenc(hmac(sha384),cbc(des3_ede))",
+			.cra_driver_name = "authenc-hmac-sha384-cbc-3des-talitos",
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_type = &crypto_aead_type,
+			.cra_aead = {
+				.setkey = aead_setkey,
+				.setauthsize = aead_setauthsize,
+				.encrypt = aead_encrypt,
+				.decrypt = aead_decrypt,
+				.givencrypt = aead_givencrypt,
+				.geniv = "<built-in>",
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+				.maxauthsize = SHA384_DIGEST_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_IPSEC_ESP |
+			             DESC_HDR_SEL0_DEU |
+		                     DESC_HDR_MODE0_DEU_CBC |
+		                     DESC_HDR_MODE0_DEU_3DES |
+		                     DESC_HDR_SEL1_MDEUB |
+		                     DESC_HDR_MODE1_MDEU_INIT |
+		                     DESC_HDR_MODE1_MDEU_PAD |
+		                     DESC_HDR_MODE1_MDEUB_SHA384_HMAC,
+	},
+	{	.type = CRYPTO_ALG_TYPE_AEAD,
+		.alg.crypto = {
+			.cra_name = "authenc(hmac(sha512),cbc(aes))",
+			.cra_driver_name = "authenc-hmac-sha512-cbc-aes-talitos",
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_type = &crypto_aead_type,
+			.cra_aead = {
+				.setkey = aead_setkey,
+				.setauthsize = aead_setauthsize,
+				.encrypt = aead_encrypt,
+				.decrypt = aead_decrypt,
+				.givencrypt = aead_givencrypt,
+				.geniv = "<built-in>",
+				.ivsize = AES_BLOCK_SIZE,
+				.maxauthsize = SHA512_DIGEST_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_IPSEC_ESP |
+			             DESC_HDR_SEL0_AESU |
+		                     DESC_HDR_MODE0_AESU_CBC |
+		                     DESC_HDR_SEL1_MDEUB |
+		                     DESC_HDR_MODE1_MDEU_INIT |
+		                     DESC_HDR_MODE1_MDEU_PAD |
+		                     DESC_HDR_MODE1_MDEUB_SHA512_HMAC,
+	},
+	{	.type = CRYPTO_ALG_TYPE_AEAD,
+		.alg.crypto = {
+			.cra_name = "authenc(hmac(sha512),cbc(des3_ede))",
+			.cra_driver_name = "authenc-hmac-sha512-cbc-3des-talitos",
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC,
+			.cra_type = &crypto_aead_type,
+			.cra_aead = {
+				.setkey = aead_setkey,
+				.setauthsize = aead_setauthsize,
+				.encrypt = aead_encrypt,
+				.decrypt = aead_decrypt,
+				.givencrypt = aead_givencrypt,
+				.geniv = "<built-in>",
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+				.maxauthsize = SHA512_DIGEST_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_IPSEC_ESP |
+			             DESC_HDR_SEL0_DEU |
+		                     DESC_HDR_MODE0_DEU_CBC |
+		                     DESC_HDR_MODE0_DEU_3DES |
+		                     DESC_HDR_SEL1_MDEUB |
+		                     DESC_HDR_MODE1_MDEU_INIT |
+		                     DESC_HDR_MODE1_MDEU_PAD |
+		                     DESC_HDR_MODE1_MDEUB_SHA512_HMAC,
+	},
+	{	.type = CRYPTO_ALG_TYPE_AEAD,
+		.alg.crypto = {
 			.cra_name = "authenc(hmac(md5),cbc(aes))",
 			.cra_driver_name = "authenc-hmac-md5-cbc-aes-talitos",
 			.cra_blocksize = AES_BLOCK_SIZE,
diff --git a/drivers/crypto/talitos.h b/drivers/crypto/talitos.h
index 3c173954ef2..61a14054aa3 100644
--- a/drivers/crypto/talitos.h
+++ b/drivers/crypto/talitos.h
@@ -28,6 +28,123 @@
  *
  */
 
+#define TALITOS_TIMEOUT 100000
+#define TALITOS_MAX_DATA_LEN 65535
+
+#define DESC_TYPE(desc_hdr) ((be32_to_cpu(desc_hdr) >> 3) & 0x1f)
+#define PRIMARY_EU(desc_hdr) ((be32_to_cpu(desc_hdr) >> 28) & 0xf)
+#define SECONDARY_EU(desc_hdr) ((be32_to_cpu(desc_hdr) >> 16) & 0xf)
+
+/* descriptor pointer entry */
+struct talitos_ptr {
+	__be16 len;     /* length */
+	u8 j_extent;    /* jump to sg link table and/or extent */
+	u8 eptr;        /* extended address */
+	__be32 ptr;     /* address */
+};
+
+static const struct talitos_ptr zero_entry = {
+	.len = 0,
+	.j_extent = 0,
+	.eptr = 0,
+	.ptr = 0
+};
+
+/* descriptor */
+struct talitos_desc {
+	__be32 hdr;                     /* header high bits */
+	__be32 hdr_lo;                  /* header low bits */
+	struct talitos_ptr ptr[7];      /* ptr/len pair array */
+};
+
+/**
+ * talitos_request - descriptor submission request
+ * @desc: descriptor pointer (kernel virtual)
+ * @dma_desc: descriptor's physical bus address
+ * @callback: whom to call when descriptor processing is done
+ * @context: caller context (optional)
+ */
+struct talitos_request {
+	struct talitos_desc *desc;
+	dma_addr_t dma_desc;
+	void (*callback) (struct device *dev, struct talitos_desc *desc,
+			  void *context, int error);
+	void *context;
+};
+
+/* per-channel fifo management */
+struct talitos_channel {
+	void __iomem *reg;
+
+	/* request fifo */
+	struct talitos_request *fifo;
+
+	/* number of requests pending in channel h/w fifo */
+	atomic_t submit_count ____cacheline_aligned;
+
+	/* request submission (head) lock */
+	spinlock_t head_lock ____cacheline_aligned;
+	/* index to next free descriptor request */
+	int head;
+
+	/* request release (tail) lock */
+	spinlock_t tail_lock ____cacheline_aligned;
+	/* index to next in-progress/done descriptor request */
+	int tail;
+};
+
+struct talitos_private {
+	struct device *dev;
+	struct platform_device *ofdev;
+	void __iomem *reg;
+	int irq[2];
+
+	/* SEC global registers lock  */
+	spinlock_t reg_lock ____cacheline_aligned;
+
+	/* SEC version geometry (from device tree node) */
+	unsigned int num_channels;
+	unsigned int chfifo_len;
+	unsigned int exec_units;
+	unsigned int desc_types;
+
+	/* SEC Compatibility info */
+	unsigned long features;
+
+	/*
+	 * length of the request fifo
+	 * fifo_len is chfifo_len rounded up to next power of 2
+	 * so we can use bitwise ops to wrap
+	 */
+	unsigned int fifo_len;
+
+	struct talitos_channel *chan;
+
+	/* next channel to be assigned next incoming descriptor */
+	atomic_t last_chan ____cacheline_aligned;
+
+	/* request callback tasklet */
+	struct tasklet_struct done_task[2];
+
+	/* list of registered algorithms */
+	struct list_head alg_list;
+
+	/* hwrng device */
+	struct hwrng rng;
+};
+
+extern int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
+			  void (*callback)(struct device *dev,
+					   struct talitos_desc *desc,
+					   void *context, int error),
+			  void *context);
+
+/* .features flag */
+#define TALITOS_FTR_SRC_LINK_TBL_LEN_INCLUDES_EXTENT 0x00000001
+#define TALITOS_FTR_HW_AUTH_CHECK 0x00000002
+#define TALITOS_FTR_SHA224_HWINIT 0x00000004
+#define TALITOS_FTR_HMAC_OK 0x00000008
+
 /*
  * TALITOS_xxx_LO addresses point to the low data bits (32-63) of the register
  */
@@ -209,6 +326,12 @@
 					 DESC_HDR_MODE1_MDEU_HMAC)
 #define	DESC_HDR_MODE1_MDEU_SHA1_HMAC	(DESC_HDR_MODE1_MDEU_SHA1 | \
 					 DESC_HDR_MODE1_MDEU_HMAC)
+#define DESC_HDR_MODE1_MDEU_SHA224_HMAC	(DESC_HDR_MODE1_MDEU_SHA224 | \
+					 DESC_HDR_MODE1_MDEU_HMAC)
+#define DESC_HDR_MODE1_MDEUB_SHA384_HMAC	(DESC_HDR_MODE1_MDEUB_SHA384 | \
+						 DESC_HDR_MODE1_MDEU_HMAC)
+#define DESC_HDR_MODE1_MDEUB_SHA512_HMAC	(DESC_HDR_MODE1_MDEUB_SHA512 | \
+						 DESC_HDR_MODE1_MDEU_HMAC)
 
 /* direction of overall data flow (DIR) */
 #define	DESC_HDR_DIR_INBOUND		cpu_to_be32(0x00000002)
diff --git a/drivers/crypto/tegra-aes.c b/drivers/crypto/tegra-aes.c
index 422a9766c7c..ac236f6724f 100644
--- a/drivers/crypto/tegra-aes.c
+++ b/drivers/crypto/tegra-aes.c
@@ -572,7 +572,7 @@ static void aes_workqueue_handler(struct work_struct *work)
 	struct tegra_aes_dev *dd = aes_dev;
 	int ret;
 
-	ret = clk_enable(dd->aes_clk);
+	ret = clk_prepare_enable(dd->aes_clk);
 	if (ret)
 		BUG_ON("clock enable failed");
 
@@ -581,7 +581,7 @@ static void aes_workqueue_handler(struct work_struct *work)
 		ret = tegra_aes_handle_req(dd);
 	} while (!ret);
 
-	clk_disable(dd->aes_clk);
+	clk_disable_unprepare(dd->aes_clk);
 }
 
 static irqreturn_t aes_irq(int irq, void *dev_id)
@@ -673,7 +673,7 @@ static int tegra_aes_get_random(struct crypto_rng *tfm, u8 *rdata,
 	/* take mutex to access the aes hw */
 	mutex_lock(&aes_lock);
 
-	ret = clk_enable(dd->aes_clk);
+	ret = clk_prepare_enable(dd->aes_clk);
 	if (ret)
 		return ret;
 
@@ -700,7 +700,7 @@ static int tegra_aes_get_random(struct crypto_rng *tfm, u8 *rdata,
 	}
 
 out:
-	clk_disable(dd->aes_clk);
+	clk_disable_unprepare(dd->aes_clk);
 	mutex_unlock(&aes_lock);
 
 	dev_dbg(dd->dev, "%s: done\n", __func__);
@@ -758,7 +758,7 @@ static int tegra_aes_rng_reset(struct crypto_rng *tfm, u8 *seed,
 
 	dd->flags = FLAGS_ENCRYPT | FLAGS_RNG;
 
-	ret = clk_enable(dd->aes_clk);
+	ret = clk_prepare_enable(dd->aes_clk);
 	if (ret)
 		return ret;
 
@@ -788,7 +788,7 @@ static int tegra_aes_rng_reset(struct crypto_rng *tfm, u8 *seed,
 	memcpy(dd->dt, dt, DEFAULT_RNG_BLK_SZ);
 
 out:
-	clk_disable(dd->aes_clk);
+	clk_disable_unprepare(dd->aes_clk);
 	mutex_unlock(&aes_lock);
 
 	dev_dbg(dd->dev, "%s: done\n", __func__);
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
index 7cac12793a4..1c307e1b840 100644
--- a/drivers/crypto/ux500/cryp/cryp_core.c
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -1661,27 +1661,26 @@ static void ux500_cryp_shutdown(struct platform_device *pdev)
 
 }
 
-static int ux500_cryp_suspend(struct platform_device *pdev, pm_message_t state)
+static int ux500_cryp_suspend(struct device *dev)
 {
 	int ret;
+	struct platform_device *pdev = to_platform_device(dev);
 	struct cryp_device_data *device_data;
 	struct resource *res_irq;
 	struct cryp_ctx *temp_ctx = NULL;
 
-	dev_dbg(&pdev->dev, "[%s]", __func__);
+	dev_dbg(dev, "[%s]", __func__);
 
 	/* Handle state? */
 	device_data = platform_get_drvdata(pdev);
 	if (!device_data) {
-		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
-			__func__);
+		dev_err(dev, "[%s]: platform_get_drvdata() failed!", __func__);
 		return -ENOMEM;
 	}
 
 	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res_irq)
-		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
-			__func__);
+		dev_err(dev, "[%s]: IORESOURCE_IRQ, unavailable", __func__);
 	else
 		disable_irq(res_irq->start);
 
@@ -1692,32 +1691,32 @@ static int ux500_cryp_suspend(struct platform_device *pdev, pm_message_t state)
 
 	if (device_data->current_ctx == ++temp_ctx) {
 		if (down_interruptible(&driver_data.device_allocation))
-			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
-					"failed", __func__);
-		ret = cryp_disable_power(&pdev->dev, device_data, false);
+			dev_dbg(dev, "[%s]: down_interruptible() failed",
+				__func__);
+		ret = cryp_disable_power(dev, device_data, false);
 
 	} else
-		ret = cryp_disable_power(&pdev->dev, device_data, true);
+		ret = cryp_disable_power(dev, device_data, true);
 
 	if (ret)
-		dev_err(&pdev->dev, "[%s]: cryp_disable_power()", __func__);
+		dev_err(dev, "[%s]: cryp_disable_power()", __func__);
 
 	return ret;
 }
 
-static int ux500_cryp_resume(struct platform_device *pdev)
+static int ux500_cryp_resume(struct device *dev)
 {
 	int ret = 0;
+	struct platform_device *pdev = to_platform_device(dev);
 	struct cryp_device_data *device_data;
 	struct resource *res_irq;
 	struct cryp_ctx *temp_ctx = NULL;
 
-	dev_dbg(&pdev->dev, "[%s]", __func__);
+	dev_dbg(dev, "[%s]", __func__);
 
 	device_data = platform_get_drvdata(pdev);
 	if (!device_data) {
-		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
-			__func__);
+		dev_err(dev, "[%s]: platform_get_drvdata() failed!", __func__);
 		return -ENOMEM;
 	}
 
@@ -1730,11 +1729,10 @@ static int ux500_cryp_resume(struct platform_device *pdev)
 	if (!device_data->current_ctx)
 		up(&driver_data.device_allocation);
 	else
-		ret = cryp_enable_power(&pdev->dev, device_data, true);
+		ret = cryp_enable_power(dev, device_data, true);
 
 	if (ret)
-		dev_err(&pdev->dev, "[%s]: cryp_enable_power() failed!",
-			__func__);
+		dev_err(dev, "[%s]: cryp_enable_power() failed!", __func__);
 	else {
 		res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 		if (res_irq)
@@ -1744,15 +1742,16 @@ static int ux500_cryp_resume(struct platform_device *pdev)
 	return ret;
 }
 
+static SIMPLE_DEV_PM_OPS(ux500_cryp_pm, ux500_cryp_suspend, ux500_cryp_resume);
+
 static struct platform_driver cryp_driver = {
 	.probe  = ux500_cryp_probe,
 	.remove = ux500_cryp_remove,
 	.shutdown = ux500_cryp_shutdown,
-	.suspend  = ux500_cryp_suspend,
-	.resume   = ux500_cryp_resume,
 	.driver = {
 		.owner = THIS_MODULE,
 		.name  = "cryp1"
+		.pm    = &ux500_cryp_pm,
 	}
 };
 
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
index 6dbb9ec709a..08d5032cb56 100644
--- a/drivers/crypto/ux500/hash/hash_core.c
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -1894,19 +1894,17 @@ static void ux500_hash_shutdown(struct platform_device *pdev)
 
 /**
  * ux500_hash_suspend - Function that suspends the hash device.
- * @pdev:	The platform device.
- * @state:	-
+ * @dev:	Device to suspend.
  */
-static int ux500_hash_suspend(struct platform_device *pdev, pm_message_t state)
+static int ux500_hash_suspend(struct device *dev)
 {
 	int ret;
 	struct hash_device_data *device_data;
 	struct hash_ctx *temp_ctx = NULL;
 
-	device_data = platform_get_drvdata(pdev);
+	device_data = dev_get_drvdata(dev);
 	if (!device_data) {
-		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
-				__func__);
+		dev_err(dev, "[%s] platform_get_drvdata() failed!", __func__);
 		return -ENOMEM;
 	}
 
@@ -1917,33 +1915,32 @@ static int ux500_hash_suspend(struct platform_device *pdev, pm_message_t state)
 
 	if (device_data->current_ctx == ++temp_ctx) {
 		if (down_interruptible(&driver_data.device_allocation))
-			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
-					"failed", __func__);
+			dev_dbg(dev, "[%s]: down_interruptible() failed",
+				__func__);
 		ret = hash_disable_power(device_data, false);
 
 	} else
 		ret = hash_disable_power(device_data, true);
 
 	if (ret)
-		dev_err(&pdev->dev, "[%s]: hash_disable_power()", __func__);
+		dev_err(dev, "[%s]: hash_disable_power()", __func__);
 
 	return ret;
 }
 
 /**
  * ux500_hash_resume - Function that resume the hash device.
- * @pdev:	The platform device.
+ * @dev:	Device to resume.
  */
-static int ux500_hash_resume(struct platform_device *pdev)
+static int ux500_hash_resume(struct device *dev)
 {
 	int ret = 0;
 	struct hash_device_data *device_data;
 	struct hash_ctx *temp_ctx = NULL;
 
-	device_data = platform_get_drvdata(pdev);
+	device_data = dev_get_drvdata(dev);
 	if (!device_data) {
-		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
-				__func__);
+		dev_err(dev, "[%s] platform_get_drvdata() failed!", __func__);
 		return -ENOMEM;
 	}
 
@@ -1958,21 +1955,21 @@ static int ux500_hash_resume(struct platform_device *pdev)
 		ret = hash_enable_power(device_data, true);
 
 	if (ret)
-		dev_err(&pdev->dev, "[%s]: hash_enable_power() failed!",
-			__func__);
+		dev_err(dev, "[%s]: hash_enable_power() failed!", __func__);
 
 	return ret;
 }
 
+static SIMPLE_DEV_PM_OPS(ux500_hash_pm, ux500_hash_suspend, ux500_hash_resume);
+
 static struct platform_driver hash_driver = {
 	.probe  = ux500_hash_probe,
 	.remove = ux500_hash_remove,
 	.shutdown = ux500_hash_shutdown,
-	.suspend  = ux500_hash_suspend,
-	.resume   = ux500_hash_resume,
 	.driver = {
 		.owner = THIS_MODULE,
 		.name  = "hash1",
+		.pm    = &ux500_hash_pm,
 	}
 };
 
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index aadeb5be9db..d06ea2950dd 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -53,6 +53,7 @@ config AMBA_PL08X
 	bool "ARM PrimeCell PL080 or PL081 support"
 	depends on ARM_AMBA && EXPERIMENTAL
 	select DMA_ENGINE
+	select DMA_VIRTUAL_CHANNELS
 	help
 	  Platform has a PL08x DMAC device
 	  which can provide DMA engine support
@@ -148,6 +149,20 @@ config TXX9_DMAC
 	  Support the TXx9 SoC internal DMA controller.  This can be
 	  integrated in chips such as the Toshiba TX4927/38/39.
 
+config TEGRA20_APB_DMA
+	bool "NVIDIA Tegra20 APB DMA support"
+	depends on ARCH_TEGRA
+	select DMA_ENGINE
+	help
+	  Support for the NVIDIA Tegra20 APB DMA controller driver. The
+	  DMA controller is having multiple DMA channel which can be
+	  configured for different peripherals like audio, UART, SPI,
+	  I2C etc which is in APB bus.
+	  This DMA controller transfers data from memory to peripheral fifo
+	  or vice versa. It does not support memory to memory data transfer.
+
+
+
 config SH_DMAE
 	tristate "Renesas SuperH DMAC support"
 	depends on (SUPERH && SH_DMA) || (ARM && ARCH_SHMOBILE)
@@ -237,7 +252,7 @@ config IMX_DMA
 
 config MXS_DMA
 	bool "MXS DMA support"
-	depends on SOC_IMX23 || SOC_IMX28
+	depends on SOC_IMX23 || SOC_IMX28 || SOC_IMX6Q
 	select STMP_DEVICE
 	select DMA_ENGINE
 	help
@@ -255,14 +270,34 @@ config DMA_SA11X0
 	tristate "SA-11x0 DMA support"
 	depends on ARCH_SA1100
 	select DMA_ENGINE
+	select DMA_VIRTUAL_CHANNELS
 	help
 	  Support the DMA engine found on Intel StrongARM SA-1100 and
 	  SA-1110 SoCs.  This DMA engine can only be used with on-chip
 	  devices.
 
+config MMP_TDMA
+	bool "MMP Two-Channel DMA support"
+	depends on ARCH_MMP
+	select DMA_ENGINE
+	help
+	  Support the MMP Two-Channel DMA engine.
+	  This engine used for MMP Audio DMA and pxa910 SQU.
+
+	  Say Y here if you enabled MMP ADMA, otherwise say N.
+
+config DMA_OMAP
+	tristate "OMAP DMA support"
+	depends on ARCH_OMAP
+	select DMA_ENGINE
+	select DMA_VIRTUAL_CHANNELS
+
 config DMA_ENGINE
 	bool
 
+config DMA_VIRTUAL_CHANNELS
+	tristate
+
 comment "DMA Clients"
 	depends on DMA_ENGINE
 
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 86b795baba9..4cf6b128ab9 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -2,6 +2,7 @@ ccflags-$(CONFIG_DMADEVICES_DEBUG)  := -DDEBUG
 ccflags-$(CONFIG_DMADEVICES_VDEBUG) += -DVERBOSE_DEBUG
 
 obj-$(CONFIG_DMA_ENGINE) += dmaengine.o
+obj-$(CONFIG_DMA_VIRTUAL_CHANNELS) += virt-dma.o
 obj-$(CONFIG_NET_DMA) += iovlock.o
 obj-$(CONFIG_INTEL_MID_DMAC) += intel_mid_dma.o
 obj-$(CONFIG_DMATEST) += dmatest.o
@@ -14,7 +15,7 @@ obj-$(CONFIG_DW_DMAC) += dw_dmac.o
 obj-$(CONFIG_AT_HDMAC) += at_hdmac.o
 obj-$(CONFIG_MX3_IPU) += ipu/
 obj-$(CONFIG_TXX9_DMAC) += txx9dmac.o
-obj-$(CONFIG_SH_DMAE) += shdma.o
+obj-$(CONFIG_SH_DMAE) += sh/
 obj-$(CONFIG_COH901318) += coh901318.o coh901318_lli.o
 obj-$(CONFIG_AMCC_PPC440SPE_ADMA) += ppc4xx/
 obj-$(CONFIG_IMX_SDMA) += imx-sdma.o
@@ -23,8 +24,11 @@ obj-$(CONFIG_MXS_DMA) += mxs-dma.o
 obj-$(CONFIG_TIMB_DMA) += timb_dma.o
 obj-$(CONFIG_SIRF_DMA) += sirf-dma.o
 obj-$(CONFIG_STE_DMA40) += ste_dma40.o ste_dma40_ll.o
+obj-$(CONFIG_TEGRA20_APB_DMA) += tegra20-apb-dma.o
 obj-$(CONFIG_PL330_DMA) += pl330.o
 obj-$(CONFIG_PCH_DMA) += pch_dma.o
 obj-$(CONFIG_AMBA_PL08X) += amba-pl08x.o
 obj-$(CONFIG_EP93XX_DMA) += ep93xx_dma.o
 obj-$(CONFIG_DMA_SA11X0) += sa11x0-dma.o
+obj-$(CONFIG_MMP_TDMA) += mmp_tdma.o
+obj-$(CONFIG_DMA_OMAP) += omap-dma.o
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 49ecbbb8932..6fbeebb9486 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -86,10 +86,12 @@
 #include <asm/hardware/pl080.h>
 
 #include "dmaengine.h"
+#include "virt-dma.h"
 
 #define DRIVER_NAME	"pl08xdmac"
 
 static struct amba_driver pl08x_amba_driver;
+struct pl08x_driver_data;
 
 /**
  * struct vendor_data - vendor-specific config parameters for PL08x derivatives
@@ -119,6 +121,123 @@ struct pl08x_lli {
 };
 
 /**
+ * struct pl08x_bus_data - information of source or destination
+ * busses for a transfer
+ * @addr: current address
+ * @maxwidth: the maximum width of a transfer on this bus
+ * @buswidth: the width of this bus in bytes: 1, 2 or 4
+ */
+struct pl08x_bus_data {
+	dma_addr_t addr;
+	u8 maxwidth;
+	u8 buswidth;
+};
+
+/**
+ * struct pl08x_phy_chan - holder for the physical channels
+ * @id: physical index to this channel
+ * @lock: a lock to use when altering an instance of this struct
+ * @serving: the virtual channel currently being served by this physical
+ * channel
+ * @locked: channel unavailable for the system, e.g. dedicated to secure
+ * world
+ */
+struct pl08x_phy_chan {
+	unsigned int id;
+	void __iomem *base;
+	spinlock_t lock;
+	struct pl08x_dma_chan *serving;
+	bool locked;
+};
+
+/**
+ * struct pl08x_sg - structure containing data per sg
+ * @src_addr: src address of sg
+ * @dst_addr: dst address of sg
+ * @len: transfer len in bytes
+ * @node: node for txd's dsg_list
+ */
+struct pl08x_sg {
+	dma_addr_t src_addr;
+	dma_addr_t dst_addr;
+	size_t len;
+	struct list_head node;
+};
+
+/**
+ * struct pl08x_txd - wrapper for struct dma_async_tx_descriptor
+ * @vd: virtual DMA descriptor
+ * @dsg_list: list of children sg's
+ * @llis_bus: DMA memory address (physical) start for the LLIs
+ * @llis_va: virtual memory address start for the LLIs
+ * @cctl: control reg values for current txd
+ * @ccfg: config reg values for current txd
+ * @done: this marks completed descriptors, which should not have their
+ *   mux released.
+ */
+struct pl08x_txd {
+	struct virt_dma_desc vd;
+	struct list_head dsg_list;
+	dma_addr_t llis_bus;
+	struct pl08x_lli *llis_va;
+	/* Default cctl value for LLIs */
+	u32 cctl;
+	/*
+	 * Settings to be put into the physical channel when we
+	 * trigger this txd.  Other registers are in llis_va[0].
+	 */
+	u32 ccfg;
+	bool done;
+};
+
+/**
+ * struct pl08x_dma_chan_state - holds the PL08x specific virtual channel
+ * states
+ * @PL08X_CHAN_IDLE: the channel is idle
+ * @PL08X_CHAN_RUNNING: the channel has allocated a physical transport
+ * channel and is running a transfer on it
+ * @PL08X_CHAN_PAUSED: the channel has allocated a physical transport
+ * channel, but the transfer is currently paused
+ * @PL08X_CHAN_WAITING: the channel is waiting for a physical transport
+ * channel to become available (only pertains to memcpy channels)
+ */
+enum pl08x_dma_chan_state {
+	PL08X_CHAN_IDLE,
+	PL08X_CHAN_RUNNING,
+	PL08X_CHAN_PAUSED,
+	PL08X_CHAN_WAITING,
+};
+
+/**
+ * struct pl08x_dma_chan - this structure wraps a DMA ENGINE channel
+ * @vc: wrappped virtual channel
+ * @phychan: the physical channel utilized by this channel, if there is one
+ * @name: name of channel
+ * @cd: channel platform data
+ * @runtime_addr: address for RX/TX according to the runtime config
+ * @at: active transaction on this channel
+ * @lock: a lock for this channel data
+ * @host: a pointer to the host (internal use)
+ * @state: whether the channel is idle, paused, running etc
+ * @slave: whether this channel is a device (slave) or for memcpy
+ * @signal: the physical DMA request signal which this channel is using
+ * @mux_use: count of descriptors using this DMA request signal setting
+ */
+struct pl08x_dma_chan {
+	struct virt_dma_chan vc;
+	struct pl08x_phy_chan *phychan;
+	const char *name;
+	const struct pl08x_channel_data *cd;
+	struct dma_slave_config cfg;
+	struct pl08x_txd *at;
+	struct pl08x_driver_data *host;
+	enum pl08x_dma_chan_state state;
+	bool slave;
+	int signal;
+	unsigned mux_use;
+};
+
+/**
  * struct pl08x_driver_data - the local state holder for the PL08x
  * @slave: slave engine for this instance
  * @memcpy: memcpy engine for this instance
@@ -128,7 +247,6 @@ struct pl08x_lli {
  * @pd: platform data passed in from the platform/machine
  * @phy_chans: array of data for the physical channels
  * @pool: a pool for the LLI descriptors
- * @pool_ctr: counter of LLIs in the pool
  * @lli_buses: bitmask to or in to LLI pointer selecting AHB port for LLI
  * fetches
  * @mem_buses: set to indicate memory transfers on AHB2.
@@ -143,10 +261,8 @@ struct pl08x_driver_data {
 	struct pl08x_platform_data *pd;
 	struct pl08x_phy_chan *phy_chans;
 	struct dma_pool *pool;
-	int pool_ctr;
 	u8 lli_buses;
 	u8 mem_buses;
-	spinlock_t lock;
 };
 
 /*
@@ -162,12 +278,51 @@ struct pl08x_driver_data {
 
 static inline struct pl08x_dma_chan *to_pl08x_chan(struct dma_chan *chan)
 {
-	return container_of(chan, struct pl08x_dma_chan, chan);
+	return container_of(chan, struct pl08x_dma_chan, vc.chan);
 }
 
 static inline struct pl08x_txd *to_pl08x_txd(struct dma_async_tx_descriptor *tx)
 {
-	return container_of(tx, struct pl08x_txd, tx);
+	return container_of(tx, struct pl08x_txd, vd.tx);
+}
+
+/*
+ * Mux handling.
+ *
+ * This gives us the DMA request input to the PL08x primecell which the
+ * peripheral described by the channel data will be routed to, possibly
+ * via a board/SoC specific external MUX.  One important point to note
+ * here is that this does not depend on the physical channel.
+ */
+static int pl08x_request_mux(struct pl08x_dma_chan *plchan)
+{
+	const struct pl08x_platform_data *pd = plchan->host->pd;
+	int ret;
+
+	if (plchan->mux_use++ == 0 && pd->get_signal) {
+		ret = pd->get_signal(plchan->cd);
+		if (ret < 0) {
+			plchan->mux_use = 0;
+			return ret;
+		}
+
+		plchan->signal = ret;
+	}
+	return 0;
+}
+
+static void pl08x_release_mux(struct pl08x_dma_chan *plchan)
+{
+	const struct pl08x_platform_data *pd = plchan->host->pd;
+
+	if (plchan->signal >= 0) {
+		WARN_ON(plchan->mux_use == 0);
+
+		if (--plchan->mux_use == 0 && pd->put_signal) {
+			pd->put_signal(plchan->cd, plchan->signal);
+			plchan->signal = -1;
+		}
+	}
 }
 
 /*
@@ -189,20 +344,25 @@ static int pl08x_phy_channel_busy(struct pl08x_phy_chan *ch)
  * been set when the LLIs were constructed.  Poke them into the hardware
  * and start the transfer.
  */
-static void pl08x_start_txd(struct pl08x_dma_chan *plchan,
-	struct pl08x_txd *txd)
+static void pl08x_start_next_txd(struct pl08x_dma_chan *plchan)
 {
 	struct pl08x_driver_data *pl08x = plchan->host;
 	struct pl08x_phy_chan *phychan = plchan->phychan;
-	struct pl08x_lli *lli = &txd->llis_va[0];
+	struct virt_dma_desc *vd = vchan_next_desc(&plchan->vc);
+	struct pl08x_txd *txd = to_pl08x_txd(&vd->tx);
+	struct pl08x_lli *lli;
 	u32 val;
 
+	list_del(&txd->vd.node);
+
 	plchan->at = txd;
 
 	/* Wait for channel inactive */
 	while (pl08x_phy_channel_busy(phychan))
 		cpu_relax();
 
+	lli = &txd->llis_va[0];
+
 	dev_vdbg(&pl08x->adev->dev,
 		"WRITE channel %d: csrc=0x%08x, cdst=0x%08x, "
 		"clli=0x%08x, cctl=0x%08x, ccfg=0x%08x\n",
@@ -311,10 +471,8 @@ static u32 pl08x_getbytes_chan(struct pl08x_dma_chan *plchan)
 {
 	struct pl08x_phy_chan *ch;
 	struct pl08x_txd *txd;
-	unsigned long flags;
 	size_t bytes = 0;
 
-	spin_lock_irqsave(&plchan->lock, flags);
 	ch = plchan->phychan;
 	txd = plchan->at;
 
@@ -354,18 +512,6 @@ static u32 pl08x_getbytes_chan(struct pl08x_dma_chan *plchan)
 		}
 	}
 
-	/* Sum up all queued transactions */
-	if (!list_empty(&plchan->pend_list)) {
-		struct pl08x_txd *txdi;
-		list_for_each_entry(txdi, &plchan->pend_list, node) {
-			struct pl08x_sg *dsg;
-			list_for_each_entry(dsg, &txd->dsg_list, node)
-				bytes += dsg->len;
-		}
-	}
-
-	spin_unlock_irqrestore(&plchan->lock, flags);
-
 	return bytes;
 }
 
@@ -391,7 +537,6 @@ pl08x_get_phy_channel(struct pl08x_driver_data *pl08x,
 
 		if (!ch->locked && !ch->serving) {
 			ch->serving = virt_chan;
-			ch->signal = -1;
 			spin_unlock_irqrestore(&ch->lock, flags);
 			break;
 		}
@@ -404,25 +549,114 @@ pl08x_get_phy_channel(struct pl08x_driver_data *pl08x,
 		return NULL;
 	}
 
-	pm_runtime_get_sync(&pl08x->adev->dev);
 	return ch;
 }
 
+/* Mark the physical channel as free.  Note, this write is atomic. */
 static inline void pl08x_put_phy_channel(struct pl08x_driver_data *pl08x,
 					 struct pl08x_phy_chan *ch)
 {
-	unsigned long flags;
+	ch->serving = NULL;
+}
+
+/*
+ * Try to allocate a physical channel.  When successful, assign it to
+ * this virtual channel, and initiate the next descriptor.  The
+ * virtual channel lock must be held at this point.
+ */
+static void pl08x_phy_alloc_and_start(struct pl08x_dma_chan *plchan)
+{
+	struct pl08x_driver_data *pl08x = plchan->host;
+	struct pl08x_phy_chan *ch;
 
-	spin_lock_irqsave(&ch->lock, flags);
+	ch = pl08x_get_phy_channel(pl08x, plchan);
+	if (!ch) {
+		dev_dbg(&pl08x->adev->dev, "no physical channel available for xfer on %s\n", plchan->name);
+		plchan->state = PL08X_CHAN_WAITING;
+		return;
+	}
 
-	/* Stop the channel and clear its interrupts */
-	pl08x_terminate_phy_chan(pl08x, ch);
+	dev_dbg(&pl08x->adev->dev, "allocated physical channel %d for xfer on %s\n",
+		ch->id, plchan->name);
 
-	pm_runtime_put(&pl08x->adev->dev);
+	plchan->phychan = ch;
+	plchan->state = PL08X_CHAN_RUNNING;
+	pl08x_start_next_txd(plchan);
+}
 
-	/* Mark it as free */
-	ch->serving = NULL;
-	spin_unlock_irqrestore(&ch->lock, flags);
+static void pl08x_phy_reassign_start(struct pl08x_phy_chan *ch,
+	struct pl08x_dma_chan *plchan)
+{
+	struct pl08x_driver_data *pl08x = plchan->host;
+
+	dev_dbg(&pl08x->adev->dev, "reassigned physical channel %d for xfer on %s\n",
+		ch->id, plchan->name);
+
+	/*
+	 * We do this without taking the lock; we're really only concerned
+	 * about whether this pointer is NULL or not, and we're guaranteed
+	 * that this will only be called when it _already_ is non-NULL.
+	 */
+	ch->serving = plchan;
+	plchan->phychan = ch;
+	plchan->state = PL08X_CHAN_RUNNING;
+	pl08x_start_next_txd(plchan);
+}
+
+/*
+ * Free a physical DMA channel, potentially reallocating it to another
+ * virtual channel if we have any pending.
+ */
+static void pl08x_phy_free(struct pl08x_dma_chan *plchan)
+{
+	struct pl08x_driver_data *pl08x = plchan->host;
+	struct pl08x_dma_chan *p, *next;
+
+ retry:
+	next = NULL;
+
+	/* Find a waiting virtual channel for the next transfer. */
+	list_for_each_entry(p, &pl08x->memcpy.channels, vc.chan.device_node)
+		if (p->state == PL08X_CHAN_WAITING) {
+			next = p;
+			break;
+		}
+
+	if (!next) {
+		list_for_each_entry(p, &pl08x->slave.channels, vc.chan.device_node)
+			if (p->state == PL08X_CHAN_WAITING) {
+				next = p;
+				break;
+			}
+	}
+
+	/* Ensure that the physical channel is stopped */
+	pl08x_terminate_phy_chan(pl08x, plchan->phychan);
+
+	if (next) {
+		bool success;
+
+		/*
+		 * Eww.  We know this isn't going to deadlock
+		 * but lockdep probably doesn't.
+		 */
+		spin_lock(&next->vc.lock);
+		/* Re-check the state now that we have the lock */
+		success = next->state == PL08X_CHAN_WAITING;
+		if (success)
+			pl08x_phy_reassign_start(plchan->phychan, next);
+		spin_unlock(&next->vc.lock);
+
+		/* If the state changed, try to find another channel */
+		if (!success)
+			goto retry;
+	} else {
+		/* No more jobs, so free up the physical channel */
+		pl08x_put_phy_channel(pl08x, plchan->phychan);
+	}
+
+	plchan->phychan = NULL;
+	plchan->state = PL08X_CHAN_IDLE;
 }
 
 /*
@@ -585,8 +819,6 @@ static int pl08x_fill_llis_for_desc(struct pl08x_driver_data *pl08x,
 		return 0;
 	}
 
-	pl08x->pool_ctr++;
-
 	bd.txd = txd;
 	bd.lli_bus = (pl08x->lli_buses & PL08X_AHB2) ? PL080_LLI_LM_AHB2 : 0;
 	cctl = txd->cctl;
@@ -802,18 +1034,14 @@ static int pl08x_fill_llis_for_desc(struct pl08x_driver_data *pl08x,
 	return num_llis;
 }
 
-/* You should call this with the struct pl08x lock held */
 static void pl08x_free_txd(struct pl08x_driver_data *pl08x,
 			   struct pl08x_txd *txd)
 {
 	struct pl08x_sg *dsg, *_dsg;
 
-	/* Free the LLI */
 	if (txd->llis_va)
 		dma_pool_free(pl08x->pool, txd->llis_va, txd->llis_bus);
 
-	pl08x->pool_ctr--;
-
 	list_for_each_entry_safe(dsg, _dsg, &txd->dsg_list, node) {
 		list_del(&dsg->node);
 		kfree(dsg);
@@ -822,133 +1050,75 @@ static void pl08x_free_txd(struct pl08x_driver_data *pl08x,
 	kfree(txd);
 }
 
-static void pl08x_free_txd_list(struct pl08x_driver_data *pl08x,
-				struct pl08x_dma_chan *plchan)
+static void pl08x_unmap_buffers(struct pl08x_txd *txd)
 {
-	struct pl08x_txd *txdi = NULL;
-	struct pl08x_txd *next;
-
-	if (!list_empty(&plchan->pend_list)) {
-		list_for_each_entry_safe(txdi,
-					 next, &plchan->pend_list, node) {
-			list_del(&txdi->node);
-			pl08x_free_txd(pl08x, txdi);
+	struct device *dev = txd->vd.tx.chan->device->dev;
+	struct pl08x_sg *dsg;
+
+	if (!(txd->vd.tx.flags & DMA_COMPL_SKIP_SRC_UNMAP)) {
+		if (txd->vd.tx.flags & DMA_COMPL_SRC_UNMAP_SINGLE)
+			list_for_each_entry(dsg, &txd->dsg_list, node)
+				dma_unmap_single(dev, dsg->src_addr, dsg->len,
+						DMA_TO_DEVICE);
+		else {
+			list_for_each_entry(dsg, &txd->dsg_list, node)
+				dma_unmap_page(dev, dsg->src_addr, dsg->len,
+						DMA_TO_DEVICE);
 		}
 	}
+	if (!(txd->vd.tx.flags & DMA_COMPL_SKIP_DEST_UNMAP)) {
+		if (txd->vd.tx.flags & DMA_COMPL_DEST_UNMAP_SINGLE)
+			list_for_each_entry(dsg, &txd->dsg_list, node)
+				dma_unmap_single(dev, dsg->dst_addr, dsg->len,
+						DMA_FROM_DEVICE);
+		else
+			list_for_each_entry(dsg, &txd->dsg_list, node)
+				dma_unmap_page(dev, dsg->dst_addr, dsg->len,
+						DMA_FROM_DEVICE);
+	}
 }
 
-/*
- * The DMA ENGINE API
- */
-static int pl08x_alloc_chan_resources(struct dma_chan *chan)
+static void pl08x_desc_free(struct virt_dma_desc *vd)
 {
-	return 0;
-}
+	struct pl08x_txd *txd = to_pl08x_txd(&vd->tx);
+	struct pl08x_dma_chan *plchan = to_pl08x_chan(vd->tx.chan);
 
-static void pl08x_free_chan_resources(struct dma_chan *chan)
-{
+	if (!plchan->slave)
+		pl08x_unmap_buffers(txd);
+
+	if (!txd->done)
+		pl08x_release_mux(plchan);
+
+	pl08x_free_txd(plchan->host, txd);
 }
 
-/*
- * This should be called with the channel plchan->lock held
- */
-static int prep_phy_channel(struct pl08x_dma_chan *plchan,
-			    struct pl08x_txd *txd)
+static void pl08x_free_txd_list(struct pl08x_driver_data *pl08x,
+				struct pl08x_dma_chan *plchan)
 {
-	struct pl08x_driver_data *pl08x = plchan->host;
-	struct pl08x_phy_chan *ch;
-	int ret;
-
-	/* Check if we already have a channel */
-	if (plchan->phychan) {
-		ch = plchan->phychan;
-		goto got_channel;
-	}
+	LIST_HEAD(head);
+	struct pl08x_txd *txd;
 
-	ch = pl08x_get_phy_channel(pl08x, plchan);
-	if (!ch) {
-		/* No physical channel available, cope with it */
-		dev_dbg(&pl08x->adev->dev, "no physical channel available for xfer on %s\n", plchan->name);
-		return -EBUSY;
-	}
+	vchan_get_all_descriptors(&plchan->vc, &head);
 
-	/*
-	 * OK we have a physical channel: for memcpy() this is all we
-	 * need, but for slaves the physical signals may be muxed!
-	 * Can the platform allow us to use this channel?
-	 */
-	if (plchan->slave && pl08x->pd->get_signal) {
-		ret = pl08x->pd->get_signal(plchan);
-		if (ret < 0) {
-			dev_dbg(&pl08x->adev->dev,
-				"unable to use physical channel %d for transfer on %s due to platform restrictions\n",
-				ch->id, plchan->name);
-			/* Release physical channel & return */
-			pl08x_put_phy_channel(pl08x, ch);
-			return -EBUSY;
-		}
-		ch->signal = ret;
+	while (!list_empty(&head)) {
+		txd = list_first_entry(&head, struct pl08x_txd, vd.node);
+		list_del(&txd->vd.node);
+		pl08x_desc_free(&txd->vd);
 	}
-
-	plchan->phychan = ch;
-	dev_dbg(&pl08x->adev->dev, "allocated physical channel %d and signal %d for xfer on %s\n",
-		 ch->id,
-		 ch->signal,
-		 plchan->name);
-
-got_channel:
-	/* Assign the flow control signal to this channel */
-	if (txd->direction == DMA_MEM_TO_DEV)
-		txd->ccfg |= ch->signal << PL080_CONFIG_DST_SEL_SHIFT;
-	else if (txd->direction == DMA_DEV_TO_MEM)
-		txd->ccfg |= ch->signal << PL080_CONFIG_SRC_SEL_SHIFT;
-
-	plchan->phychan_hold++;
-
-	return 0;
 }
 
-static void release_phy_channel(struct pl08x_dma_chan *plchan)
+/*
+ * The DMA ENGINE API
+ */
+static int pl08x_alloc_chan_resources(struct dma_chan *chan)
 {
-	struct pl08x_driver_data *pl08x = plchan->host;
-
-	if ((plchan->phychan->signal >= 0) && pl08x->pd->put_signal) {
-		pl08x->pd->put_signal(plchan);
-		plchan->phychan->signal = -1;
-	}
-	pl08x_put_phy_channel(pl08x, plchan->phychan);
-	plchan->phychan = NULL;
+	return 0;
 }
 
-static dma_cookie_t pl08x_tx_submit(struct dma_async_tx_descriptor *tx)
+static void pl08x_free_chan_resources(struct dma_chan *chan)
 {
-	struct pl08x_dma_chan *plchan = to_pl08x_chan(tx->chan);
-	struct pl08x_txd *txd = to_pl08x_txd(tx);
-	unsigned long flags;
-	dma_cookie_t cookie;
-
-	spin_lock_irqsave(&plchan->lock, flags);
-	cookie = dma_cookie_assign(tx);
-
-	/* Put this onto the pending list */
-	list_add_tail(&txd->node, &plchan->pend_list);
-
-	/*
-	 * If there was no physical channel available for this memcpy,
-	 * stack the request up and indicate that the channel is waiting
-	 * for a free physical channel.
-	 */
-	if (!plchan->slave && !plchan->phychan) {
-		/* Do this memcpy whenever there is a channel ready */
-		plchan->state = PL08X_CHAN_WAITING;
-		plchan->waiting = txd;
-	} else {
-		plchan->phychan_hold--;
-	}
-
-	spin_unlock_irqrestore(&plchan->lock, flags);
-
-	return cookie;
+	/* Ensure all queued descriptors are freed */
+	vchan_free_chan_resources(to_virt_chan(chan));
 }
 
 static struct dma_async_tx_descriptor *pl08x_prep_dma_interrupt(
@@ -968,23 +1138,53 @@ static enum dma_status pl08x_dma_tx_status(struct dma_chan *chan,
 		dma_cookie_t cookie, struct dma_tx_state *txstate)
 {
 	struct pl08x_dma_chan *plchan = to_pl08x_chan(chan);
+	struct virt_dma_desc *vd;
+	unsigned long flags;
 	enum dma_status ret;
+	size_t bytes = 0;
 
 	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret == DMA_SUCCESS)
 		return ret;
 
 	/*
+	 * There's no point calculating the residue if there's
+	 * no txstate to store the value.
+	 */
+	if (!txstate) {
+		if (plchan->state == PL08X_CHAN_PAUSED)
+			ret = DMA_PAUSED;
+		return ret;
+	}
+
+	spin_lock_irqsave(&plchan->vc.lock, flags);
+	ret = dma_cookie_status(chan, cookie, txstate);
+	if (ret != DMA_SUCCESS) {
+		vd = vchan_find_desc(&plchan->vc, cookie);
+		if (vd) {
+			/* On the issued list, so hasn't been processed yet */
+			struct pl08x_txd *txd = to_pl08x_txd(&vd->tx);
+			struct pl08x_sg *dsg;
+
+			list_for_each_entry(dsg, &txd->dsg_list, node)
+				bytes += dsg->len;
+		} else {
+			bytes = pl08x_getbytes_chan(plchan);
+		}
+	}
+	spin_unlock_irqrestore(&plchan->vc.lock, flags);
+
+	/*
 	 * This cookie not complete yet
 	 * Get number of bytes left in the active transactions and queue
 	 */
-	dma_set_residue(txstate, pl08x_getbytes_chan(plchan));
+	dma_set_residue(txstate, bytes);
 
-	if (plchan->state == PL08X_CHAN_PAUSED)
-		return DMA_PAUSED;
+	if (plchan->state == PL08X_CHAN_PAUSED && ret == DMA_IN_PROGRESS)
+		ret = DMA_PAUSED;
 
 	/* Whether waiting or running, we're in progress */
-	return DMA_IN_PROGRESS;
+	return ret;
 }
 
 /* PrimeCell DMA extension */
@@ -1080,38 +1280,14 @@ static u32 pl08x_burst(u32 maxburst)
 	return burst_sizes[i].reg;
 }
 
-static int dma_set_runtime_config(struct dma_chan *chan,
-				  struct dma_slave_config *config)
+static u32 pl08x_get_cctl(struct pl08x_dma_chan *plchan,
+	enum dma_slave_buswidth addr_width, u32 maxburst)
 {
-	struct pl08x_dma_chan *plchan = to_pl08x_chan(chan);
-	struct pl08x_driver_data *pl08x = plchan->host;
-	enum dma_slave_buswidth addr_width;
-	u32 width, burst, maxburst;
-	u32 cctl = 0;
-
-	if (!plchan->slave)
-		return -EINVAL;
-
-	/* Transfer direction */
-	plchan->runtime_direction = config->direction;
-	if (config->direction == DMA_MEM_TO_DEV) {
-		addr_width = config->dst_addr_width;
-		maxburst = config->dst_maxburst;
-	} else if (config->direction == DMA_DEV_TO_MEM) {
-		addr_width = config->src_addr_width;
-		maxburst = config->src_maxburst;
-	} else {
-		dev_err(&pl08x->adev->dev,
-			"bad runtime_config: alien transfer direction\n");
-		return -EINVAL;
-	}
+	u32 width, burst, cctl = 0;
 
 	width = pl08x_width(addr_width);
-	if (width == ~0) {
-		dev_err(&pl08x->adev->dev,
-			"bad runtime_config: alien address width\n");
-		return -EINVAL;
-	}
+	if (width == ~0)
+		return ~0;
 
 	cctl |= width << PL080_CONTROL_SWIDTH_SHIFT;
 	cctl |= width << PL080_CONTROL_DWIDTH_SHIFT;
@@ -1128,28 +1304,23 @@ static int dma_set_runtime_config(struct dma_chan *chan,
 	cctl |= burst << PL080_CONTROL_SB_SIZE_SHIFT;
 	cctl |= burst << PL080_CONTROL_DB_SIZE_SHIFT;
 
-	plchan->device_fc = config->device_fc;
+	return pl08x_cctl(cctl);
+}
 
-	if (plchan->runtime_direction == DMA_DEV_TO_MEM) {
-		plchan->src_addr = config->src_addr;
-		plchan->src_cctl = pl08x_cctl(cctl) | PL080_CONTROL_DST_INCR |
-			pl08x_select_bus(plchan->cd->periph_buses,
-					 pl08x->mem_buses);
-	} else {
-		plchan->dst_addr = config->dst_addr;
-		plchan->dst_cctl = pl08x_cctl(cctl) | PL080_CONTROL_SRC_INCR |
-			pl08x_select_bus(pl08x->mem_buses,
-					 plchan->cd->periph_buses);
-	}
+static int dma_set_runtime_config(struct dma_chan *chan,
+				  struct dma_slave_config *config)
+{
+	struct pl08x_dma_chan *plchan = to_pl08x_chan(chan);
 
-	dev_dbg(&pl08x->adev->dev,
-		"configured channel %s (%s) for %s, data width %d, "
-		"maxburst %d words, LE, CCTL=0x%08x\n",
-		dma_chan_name(chan), plchan->name,
-		(config->direction == DMA_DEV_TO_MEM) ? "RX" : "TX",
-		addr_width,
-		maxburst,
-		cctl);
+	if (!plchan->slave)
+		return -EINVAL;
+
+	/* Reject definitely invalid configurations */
+	if (config->src_addr_width == DMA_SLAVE_BUSWIDTH_8_BYTES ||
+	    config->dst_addr_width == DMA_SLAVE_BUSWIDTH_8_BYTES)
+		return -EINVAL;
+
+	plchan->cfg = *config;
 
 	return 0;
 }
@@ -1163,95 +1334,19 @@ static void pl08x_issue_pending(struct dma_chan *chan)
 	struct pl08x_dma_chan *plchan = to_pl08x_chan(chan);
 	unsigned long flags;
 
-	spin_lock_irqsave(&plchan->lock, flags);
-	/* Something is already active, or we're waiting for a channel... */
-	if (plchan->at || plchan->state == PL08X_CHAN_WAITING) {
-		spin_unlock_irqrestore(&plchan->lock, flags);
-		return;
-	}
-
-	/* Take the first element in the queue and execute it */
-	if (!list_empty(&plchan->pend_list)) {
-		struct pl08x_txd *next;
-
-		next = list_first_entry(&plchan->pend_list,
-					struct pl08x_txd,
-					node);
-		list_del(&next->node);
-		plchan->state = PL08X_CHAN_RUNNING;
-
-		pl08x_start_txd(plchan, next);
+	spin_lock_irqsave(&plchan->vc.lock, flags);
+	if (vchan_issue_pending(&plchan->vc)) {
+		if (!plchan->phychan && plchan->state != PL08X_CHAN_WAITING)
+			pl08x_phy_alloc_and_start(plchan);
 	}
-
-	spin_unlock_irqrestore(&plchan->lock, flags);
-}
-
-static int pl08x_prep_channel_resources(struct pl08x_dma_chan *plchan,
-					struct pl08x_txd *txd)
-{
-	struct pl08x_driver_data *pl08x = plchan->host;
-	unsigned long flags;
-	int num_llis, ret;
-
-	num_llis = pl08x_fill_llis_for_desc(pl08x, txd);
-	if (!num_llis) {
-		spin_lock_irqsave(&plchan->lock, flags);
-		pl08x_free_txd(pl08x, txd);
-		spin_unlock_irqrestore(&plchan->lock, flags);
-		return -EINVAL;
-	}
-
-	spin_lock_irqsave(&plchan->lock, flags);
-
-	/*
-	 * See if we already have a physical channel allocated,
-	 * else this is the time to try to get one.
-	 */
-	ret = prep_phy_channel(plchan, txd);
-	if (ret) {
-		/*
-		 * No physical channel was available.
-		 *
-		 * memcpy transfers can be sorted out at submission time.
-		 *
-		 * Slave transfers may have been denied due to platform
-		 * channel muxing restrictions.  Since there is no guarantee
-		 * that this will ever be resolved, and the signal must be
-		 * acquired AFTER acquiring the physical channel, we will let
-		 * them be NACK:ed with -EBUSY here. The drivers can retry
-		 * the prep() call if they are eager on doing this using DMA.
-		 */
-		if (plchan->slave) {
-			pl08x_free_txd_list(pl08x, plchan);
-			pl08x_free_txd(pl08x, txd);
-			spin_unlock_irqrestore(&plchan->lock, flags);
-			return -EBUSY;
-		}
-	} else
-		/*
-		 * Else we're all set, paused and ready to roll, status
-		 * will switch to PL08X_CHAN_RUNNING when we call
-		 * issue_pending(). If there is something running on the
-		 * channel already we don't change its state.
-		 */
-		if (plchan->state == PL08X_CHAN_IDLE)
-			plchan->state = PL08X_CHAN_PAUSED;
-
-	spin_unlock_irqrestore(&plchan->lock, flags);
-
-	return 0;
+	spin_unlock_irqrestore(&plchan->vc.lock, flags);
 }
 
-static struct pl08x_txd *pl08x_get_txd(struct pl08x_dma_chan *plchan,
-	unsigned long flags)
+static struct pl08x_txd *pl08x_get_txd(struct pl08x_dma_chan *plchan)
 {
 	struct pl08x_txd *txd = kzalloc(sizeof(*txd), GFP_NOWAIT);
 
 	if (txd) {
-		dma_async_tx_descriptor_init(&txd->tx, &plchan->chan);
-		txd->tx.flags = flags;
-		txd->tx.tx_submit = pl08x_tx_submit;
-		INIT_LIST_HEAD(&txd->node);
 		INIT_LIST_HEAD(&txd->dsg_list);
 
 		/* Always enable error and terminal interrupts */
@@ -1274,7 +1369,7 @@ static struct dma_async_tx_descriptor *pl08x_prep_dma_memcpy(
 	struct pl08x_sg *dsg;
 	int ret;
 
-	txd = pl08x_get_txd(plchan, flags);
+	txd = pl08x_get_txd(plchan);
 	if (!txd) {
 		dev_err(&pl08x->adev->dev,
 			"%s no memory for descriptor\n", __func__);
@@ -1290,14 +1385,13 @@ static struct dma_async_tx_descriptor *pl08x_prep_dma_memcpy(
 	}
 	list_add_tail(&dsg->node, &txd->dsg_list);
 
-	txd->direction = DMA_NONE;
 	dsg->src_addr = src;
 	dsg->dst_addr = dest;
 	dsg->len = len;
 
 	/* Set platform data for m2m */
 	txd->ccfg |= PL080_FLOW_MEM2MEM << PL080_CONFIG_FLOW_CONTROL_SHIFT;
-	txd->cctl = pl08x->pd->memcpy_channel.cctl &
+	txd->cctl = pl08x->pd->memcpy_channel.cctl_memcpy &
 			~(PL080_CONTROL_DST_AHB2 | PL080_CONTROL_SRC_AHB2);
 
 	/* Both to be incremented or the code will break */
@@ -1307,11 +1401,13 @@ static struct dma_async_tx_descriptor *pl08x_prep_dma_memcpy(
 		txd->cctl |= pl08x_select_bus(pl08x->mem_buses,
 					      pl08x->mem_buses);
 
-	ret = pl08x_prep_channel_resources(plchan, txd);
-	if (ret)
+	ret = pl08x_fill_llis_for_desc(plchan->host, txd);
+	if (!ret) {
+		pl08x_free_txd(pl08x, txd);
 		return NULL;
+	}
 
-	return &txd->tx;
+	return vchan_tx_prep(&plchan->vc, &txd->vd, flags);
 }
 
 static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
@@ -1324,36 +1420,40 @@ static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 	struct pl08x_txd *txd;
 	struct pl08x_sg *dsg;
 	struct scatterlist *sg;
+	enum dma_slave_buswidth addr_width;
 	dma_addr_t slave_addr;
 	int ret, tmp;
+	u8 src_buses, dst_buses;
+	u32 maxburst, cctl;
 
 	dev_dbg(&pl08x->adev->dev, "%s prepare transaction of %d bytes from %s\n",
 			__func__, sg_dma_len(sgl), plchan->name);
 
-	txd = pl08x_get_txd(plchan, flags);
+	txd = pl08x_get_txd(plchan);
 	if (!txd) {
 		dev_err(&pl08x->adev->dev, "%s no txd\n", __func__);
 		return NULL;
 	}
 
-	if (direction != plchan->runtime_direction)
-		dev_err(&pl08x->adev->dev, "%s DMA setup does not match "
-			"the direction configured for the PrimeCell\n",
-			__func__);
-
 	/*
 	 * Set up addresses, the PrimeCell configured address
 	 * will take precedence since this may configure the
 	 * channel target address dynamically at runtime.
 	 */
-	txd->direction = direction;
-
 	if (direction == DMA_MEM_TO_DEV) {
-		txd->cctl = plchan->dst_cctl;
-		slave_addr = plchan->dst_addr;
+		cctl = PL080_CONTROL_SRC_INCR;
+		slave_addr = plchan->cfg.dst_addr;
+		addr_width = plchan->cfg.dst_addr_width;
+		maxburst = plchan->cfg.dst_maxburst;
+		src_buses = pl08x->mem_buses;
+		dst_buses = plchan->cd->periph_buses;
 	} else if (direction == DMA_DEV_TO_MEM) {
-		txd->cctl = plchan->src_cctl;
-		slave_addr = plchan->src_addr;
+		cctl = PL080_CONTROL_DST_INCR;
+		slave_addr = plchan->cfg.src_addr;
+		addr_width = plchan->cfg.src_addr_width;
+		maxburst = plchan->cfg.src_maxburst;
+		src_buses = plchan->cd->periph_buses;
+		dst_buses = pl08x->mem_buses;
 	} else {
 		pl08x_free_txd(pl08x, txd);
 		dev_err(&pl08x->adev->dev,
@@ -1361,7 +1461,17 @@ static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 		return NULL;
 	}
 
-	if (plchan->device_fc)
+	cctl |= pl08x_get_cctl(plchan, addr_width, maxburst);
+	if (cctl == ~0) {
+		pl08x_free_txd(pl08x, txd);
+		dev_err(&pl08x->adev->dev,
+			"DMA slave configuration botched?\n");
+		return NULL;
+	}
+
+	txd->cctl = cctl | pl08x_select_bus(src_buses, dst_buses);
+
+	if (plchan->cfg.device_fc)
 		tmp = (direction == DMA_MEM_TO_DEV) ? PL080_FLOW_MEM2PER_PER :
 			PL080_FLOW_PER2MEM_PER;
 	else
@@ -1370,9 +1480,28 @@ static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 
 	txd->ccfg |= tmp << PL080_CONFIG_FLOW_CONTROL_SHIFT;
 
+	ret = pl08x_request_mux(plchan);
+	if (ret < 0) {
+		pl08x_free_txd(pl08x, txd);
+		dev_dbg(&pl08x->adev->dev,
+			"unable to mux for transfer on %s due to platform restrictions\n",
+			plchan->name);
+		return NULL;
+	}
+
+	dev_dbg(&pl08x->adev->dev, "allocated DMA request signal %d for xfer on %s\n",
+		 plchan->signal, plchan->name);
+
+	/* Assign the flow control signal to this channel */
+	if (direction == DMA_MEM_TO_DEV)
+		txd->ccfg |= plchan->signal << PL080_CONFIG_DST_SEL_SHIFT;
+	else
+		txd->ccfg |= plchan->signal << PL080_CONFIG_SRC_SEL_SHIFT;
+
 	for_each_sg(sgl, sg, sg_len, tmp) {
 		dsg = kzalloc(sizeof(struct pl08x_sg), GFP_NOWAIT);
 		if (!dsg) {
+			pl08x_release_mux(plchan);
 			pl08x_free_txd(pl08x, txd);
 			dev_err(&pl08x->adev->dev, "%s no mem for pl080 sg\n",
 					__func__);
@@ -1390,11 +1519,14 @@ static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 		}
 	}
 
-	ret = pl08x_prep_channel_resources(plchan, txd);
-	if (ret)
+	ret = pl08x_fill_llis_for_desc(plchan->host, txd);
+	if (!ret) {
+		pl08x_release_mux(plchan);
+		pl08x_free_txd(pl08x, txd);
 		return NULL;
+	}
 
-	return &txd->tx;
+	return vchan_tx_prep(&plchan->vc, &txd->vd, flags);
 }
 
 static int pl08x_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
@@ -1415,9 +1547,9 @@ static int pl08x_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	 * Anything succeeds on channels with no physical allocation and
 	 * no queued transfers.
 	 */
-	spin_lock_irqsave(&plchan->lock, flags);
+	spin_lock_irqsave(&plchan->vc.lock, flags);
 	if (!plchan->phychan && !plchan->at) {
-		spin_unlock_irqrestore(&plchan->lock, flags);
+		spin_unlock_irqrestore(&plchan->vc.lock, flags);
 		return 0;
 	}
 
@@ -1426,18 +1558,15 @@ static int pl08x_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		plchan->state = PL08X_CHAN_IDLE;
 
 		if (plchan->phychan) {
-			pl08x_terminate_phy_chan(pl08x, plchan->phychan);
-
 			/*
 			 * Mark physical channel as free and free any slave
 			 * signal
 			 */
-			release_phy_channel(plchan);
-			plchan->phychan_hold = 0;
+			pl08x_phy_free(plchan);
 		}
 		/* Dequeue jobs and free LLIs */
 		if (plchan->at) {
-			pl08x_free_txd(pl08x, plchan->at);
+			pl08x_desc_free(&plchan->at->vd);
 			plchan->at = NULL;
 		}
 		/* Dequeue jobs not yet fired as well */
@@ -1457,7 +1586,7 @@ static int pl08x_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		break;
 	}
 
-	spin_unlock_irqrestore(&plchan->lock, flags);
+	spin_unlock_irqrestore(&plchan->vc.lock, flags);
 
 	return ret;
 }
@@ -1494,123 +1623,6 @@ static void pl08x_ensure_on(struct pl08x_driver_data *pl08x)
 	writel(PL080_CONFIG_ENABLE, pl08x->base + PL080_CONFIG);
 }
 
-static void pl08x_unmap_buffers(struct pl08x_txd *txd)
-{
-	struct device *dev = txd->tx.chan->device->dev;
-	struct pl08x_sg *dsg;
-
-	if (!(txd->tx.flags & DMA_COMPL_SKIP_SRC_UNMAP)) {
-		if (txd->tx.flags & DMA_COMPL_SRC_UNMAP_SINGLE)
-			list_for_each_entry(dsg, &txd->dsg_list, node)
-				dma_unmap_single(dev, dsg->src_addr, dsg->len,
-						DMA_TO_DEVICE);
-		else {
-			list_for_each_entry(dsg, &txd->dsg_list, node)
-				dma_unmap_page(dev, dsg->src_addr, dsg->len,
-						DMA_TO_DEVICE);
-		}
-	}
-	if (!(txd->tx.flags & DMA_COMPL_SKIP_DEST_UNMAP)) {
-		if (txd->tx.flags & DMA_COMPL_DEST_UNMAP_SINGLE)
-			list_for_each_entry(dsg, &txd->dsg_list, node)
-				dma_unmap_single(dev, dsg->dst_addr, dsg->len,
-						DMA_FROM_DEVICE);
-		else
-			list_for_each_entry(dsg, &txd->dsg_list, node)
-				dma_unmap_page(dev, dsg->dst_addr, dsg->len,
-						DMA_FROM_DEVICE);
-	}
-}
-
-static void pl08x_tasklet(unsigned long data)
-{
-	struct pl08x_dma_chan *plchan = (struct pl08x_dma_chan *) data;
-	struct pl08x_driver_data *pl08x = plchan->host;
-	struct pl08x_txd *txd;
-	unsigned long flags;
-
-	spin_lock_irqsave(&plchan->lock, flags);
-
-	txd = plchan->at;
-	plchan->at = NULL;
-
-	if (txd) {
-		/* Update last completed */
-		dma_cookie_complete(&txd->tx);
-	}
-
-	/* If a new descriptor is queued, set it up plchan->at is NULL here */
-	if (!list_empty(&plchan->pend_list)) {
-		struct pl08x_txd *next;
-
-		next = list_first_entry(&plchan->pend_list,
-					struct pl08x_txd,
-					node);
-		list_del(&next->node);
-
-		pl08x_start_txd(plchan, next);
-	} else if (plchan->phychan_hold) {
-		/*
-		 * This channel is still in use - we have a new txd being
-		 * prepared and will soon be queued.  Don't give up the
-		 * physical channel.
-		 */
-	} else {
-		struct pl08x_dma_chan *waiting = NULL;
-
-		/*
-		 * No more jobs, so free up the physical channel
-		 * Free any allocated signal on slave transfers too
-		 */
-		release_phy_channel(plchan);
-		plchan->state = PL08X_CHAN_IDLE;
-
-		/*
-		 * And NOW before anyone else can grab that free:d up
-		 * physical channel, see if there is some memcpy pending
-		 * that seriously needs to start because of being stacked
-		 * up while we were choking the physical channels with data.
-		 */
-		list_for_each_entry(waiting, &pl08x->memcpy.channels,
-				    chan.device_node) {
-			if (waiting->state == PL08X_CHAN_WAITING &&
-				waiting->waiting != NULL) {
-				int ret;
-
-				/* This should REALLY not fail now */
-				ret = prep_phy_channel(waiting,
-						       waiting->waiting);
-				BUG_ON(ret);
-				waiting->phychan_hold--;
-				waiting->state = PL08X_CHAN_RUNNING;
-				waiting->waiting = NULL;
-				pl08x_issue_pending(&waiting->chan);
-				break;
-			}
-		}
-	}
-
-	spin_unlock_irqrestore(&plchan->lock, flags);
-
-	if (txd) {
-		dma_async_tx_callback callback = txd->tx.callback;
-		void *callback_param = txd->tx.callback_param;
-
-		/* Don't try to unmap buffers on slave channels */
-		if (!plchan->slave)
-			pl08x_unmap_buffers(txd);
-
-		/* Free the descriptor */
-		spin_lock_irqsave(&plchan->lock, flags);
-		pl08x_free_txd(pl08x, txd);
-		spin_unlock_irqrestore(&plchan->lock, flags);
-
-		/* Callback to signal completion */
-		if (callback)
-			callback(callback_param);
-	}
-}
-
 static irqreturn_t pl08x_irq(int irq, void *dev)
 {
 	struct pl08x_driver_data *pl08x = dev;
@@ -1635,6 +1647,7 @@ static irqreturn_t pl08x_irq(int irq, void *dev)
 			/* Locate physical channel */
 			struct pl08x_phy_chan *phychan = &pl08x->phy_chans[i];
 			struct pl08x_dma_chan *plchan = phychan->serving;
+			struct pl08x_txd *tx;
 
 			if (!plchan) {
 				dev_err(&pl08x->adev->dev,
@@ -1643,8 +1656,29 @@ static irqreturn_t pl08x_irq(int irq, void *dev)
 				continue;
 			}
 
-			/* Schedule tasklet on this channel */
-			tasklet_schedule(&plchan->tasklet);
+			spin_lock(&plchan->vc.lock);
+			tx = plchan->at;
+			if (tx) {
+				plchan->at = NULL;
+				/*
+				 * This descriptor is done, release its mux
+				 * reservation.
+				 */
+				pl08x_release_mux(plchan);
+				tx->done = true;
+				vchan_cookie_complete(&tx->vd);
+
+				/*
+				 * And start the next descriptor (if any),
+				 * otherwise free this channel.
+				 */
+				if (vchan_next_desc(&plchan->vc))
+					pl08x_start_next_txd(plchan);
+				else
+					pl08x_phy_free(plchan);
+			}
+			spin_unlock(&plchan->vc.lock);
+
 			mask |= (1 << i);
 		}
 	}
@@ -1654,16 +1688,10 @@ static irqreturn_t pl08x_irq(int irq, void *dev)
 
 static void pl08x_dma_slave_init(struct pl08x_dma_chan *chan)
 {
-	u32 cctl = pl08x_cctl(chan->cd->cctl);
-
 	chan->slave = true;
 	chan->name = chan->cd->bus_id;
-	chan->src_addr = chan->cd->addr;
-	chan->dst_addr = chan->cd->addr;
-	chan->src_cctl = cctl | PL080_CONTROL_DST_INCR |
-		pl08x_select_bus(chan->cd->periph_buses, chan->host->mem_buses);
-	chan->dst_cctl = cctl | PL080_CONTROL_SRC_INCR |
-		pl08x_select_bus(chan->host->mem_buses, chan->cd->periph_buses);
+	chan->cfg.src_addr = chan->cd->addr;
+	chan->cfg.dst_addr = chan->cd->addr;
 }
 
 /*
@@ -1693,6 +1721,7 @@ static int pl08x_dma_init_virtual_channels(struct pl08x_driver_data *pl08x,
 
 		chan->host = pl08x;
 		chan->state = PL08X_CHAN_IDLE;
+		chan->signal = -1;
 
 		if (slave) {
 			chan->cd = &pl08x->pd->slave_channels[i];
@@ -1705,26 +1734,12 @@ static int pl08x_dma_init_virtual_channels(struct pl08x_driver_data *pl08x,
 				return -ENOMEM;
 			}
 		}
-		if (chan->cd->circular_buffer) {
-			dev_err(&pl08x->adev->dev,
-				"channel %s: circular buffers not supported\n",
-				chan->name);
-			kfree(chan);
-			continue;
-		}
 		dev_dbg(&pl08x->adev->dev,
 			 "initialize virtual channel \"%s\"\n",
 			 chan->name);
 
-		chan->chan.device = dmadev;
-		dma_cookie_init(&chan->chan);
-
-		spin_lock_init(&chan->lock);
-		INIT_LIST_HEAD(&chan->pend_list);
-		tasklet_init(&chan->tasklet, pl08x_tasklet,
-			     (unsigned long) chan);
-
-		list_add_tail(&chan->chan.device_node, &dmadev->channels);
+		chan->vc.desc_free = pl08x_desc_free;
+		vchan_init(&chan->vc, dmadev);
 	}
 	dev_info(&pl08x->adev->dev, "initialized %d virtual %s channels\n",
 		 i, slave ? "slave" : "memcpy");
@@ -1737,8 +1752,8 @@ static void pl08x_free_virtual_channels(struct dma_device *dmadev)
 	struct pl08x_dma_chan *next;
 
 	list_for_each_entry_safe(chan,
-				 next, &dmadev->channels, chan.device_node) {
-		list_del(&chan->chan.device_node);
+				 next, &dmadev->channels, vc.chan.device_node) {
+		list_del(&chan->vc.chan.device_node);
 		kfree(chan);
 	}
 }
@@ -1791,7 +1806,7 @@ static int pl08x_debugfs_show(struct seq_file *s, void *data)
 	seq_printf(s, "\nPL08x virtual memcpy channels:\n");
 	seq_printf(s, "CHANNEL:\tSTATE:\n");
 	seq_printf(s, "--------\t------\n");
-	list_for_each_entry(chan, &pl08x->memcpy.channels, chan.device_node) {
+	list_for_each_entry(chan, &pl08x->memcpy.channels, vc.chan.device_node) {
 		seq_printf(s, "%s\t\t%s\n", chan->name,
 			   pl08x_state_str(chan->state));
 	}
@@ -1799,7 +1814,7 @@ static int pl08x_debugfs_show(struct seq_file *s, void *data)
 	seq_printf(s, "\nPL08x virtual slave channels:\n");
 	seq_printf(s, "CHANNEL:\tSTATE:\n");
 	seq_printf(s, "--------\t------\n");
-	list_for_each_entry(chan, &pl08x->slave.channels, chan.device_node) {
+	list_for_each_entry(chan, &pl08x->slave.channels, vc.chan.device_node) {
 		seq_printf(s, "%s\t\t%s\n", chan->name,
 			   pl08x_state_str(chan->state));
 	}
@@ -1851,9 +1866,6 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 		goto out_no_pl08x;
 	}
 
-	pm_runtime_set_active(&adev->dev);
-	pm_runtime_enable(&adev->dev);
-
 	/* Initialize memcpy engine */
 	dma_cap_set(DMA_MEMCPY, pl08x->memcpy.cap_mask);
 	pl08x->memcpy.dev = &adev->dev;
@@ -1903,8 +1915,6 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 		goto out_no_lli_pool;
 	}
 
-	spin_lock_init(&pl08x->lock);
-
 	pl08x->base = ioremap(adev->res.start, resource_size(&adev->res));
 	if (!pl08x->base) {
 		ret = -ENOMEM;
@@ -1942,7 +1952,6 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 		ch->id = i;
 		ch->base = pl08x->base + PL080_Cx_BASE(i);
 		spin_lock_init(&ch->lock);
-		ch->signal = -1;
 
 		/*
 		 * Nomadik variants can have channels that are locked
@@ -2007,7 +2016,6 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 		 amba_part(adev), amba_rev(adev),
 		 (unsigned long long)adev->res.start, adev->irq[0]);
 
-	pm_runtime_put(&adev->dev);
 	return 0;
 
 out_no_slave_reg:
@@ -2026,9 +2034,6 @@ out_no_ioremap:
 	dma_pool_destroy(pl08x->pool);
 out_no_lli_pool:
 out_no_platdata:
-	pm_runtime_put(&adev->dev);
-	pm_runtime_disable(&adev->dev);
-
 	kfree(pl08x);
 out_no_pl08x:
 	amba_release_regions(adev);
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 7292aa87b2d..3934fcc4e00 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -9,10 +9,9 @@
  * (at your option) any later version.
  *
  *
- * This supports the Atmel AHB DMA Controller,
- *
- * The driver has currently been tested with the Atmel AT91SAM9RL
- * and AT91SAM9G45 series.
+ * This supports the Atmel AHB DMA Controller found in several Atmel SoCs.
+ * The only Atmel DMA Controller that is not covered by this driver is the one
+ * found on AT91SAM9263.
  */
 
 #include <linux/clk.h>
@@ -1217,7 +1216,7 @@ static const struct platform_device_id atdma_devtypes[] = {
 	}
 };
 
-static inline struct at_dma_platform_data * __init at_dma_get_driver_data(
+static inline const struct at_dma_platform_data * __init at_dma_get_driver_data(
 						struct platform_device *pdev)
 {
 	if (pdev->dev.of_node) {
@@ -1255,7 +1254,7 @@ static int __init at_dma_probe(struct platform_device *pdev)
 	int			irq;
 	int			err;
 	int			i;
-	struct at_dma_platform_data *plat_dat;
+	const struct at_dma_platform_data *plat_dat;
 
 	/* setup platform data for each SoC */
 	dma_cap_set(DMA_MEMCPY, at91sam9rl_config.cap_mask);
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index e67b4e06a91..aa384e53b7a 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -1438,34 +1438,32 @@ static int __init coh901318_probe(struct platform_device *pdev)
 
 	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!io)
-		goto err_get_resource;
+		return -ENODEV;
 
 	/* Map DMA controller registers to virtual memory */
-	if (request_mem_region(io->start,
-			       resource_size(io),
-			       pdev->dev.driver->name) == NULL) {
-		err = -EBUSY;
-		goto err_request_mem;
-	}
+	if (devm_request_mem_region(&pdev->dev,
+				    io->start,
+				    resource_size(io),
+				    pdev->dev.driver->name) == NULL)
+		return -ENOMEM;
 
 	pdata = pdev->dev.platform_data;
 	if (!pdata)
-		goto err_no_platformdata;
+		return -ENODEV;
 
-	base = kmalloc(ALIGN(sizeof(struct coh901318_base), 4) +
-		       pdata->max_channels *
-		       sizeof(struct coh901318_chan),
-		       GFP_KERNEL);
+	base = devm_kzalloc(&pdev->dev,
+			    ALIGN(sizeof(struct coh901318_base), 4) +
+			    pdata->max_channels *
+			    sizeof(struct coh901318_chan),
+			    GFP_KERNEL);
 	if (!base)
-		goto err_alloc_coh_dma_channels;
+		return -ENOMEM;
 
 	base->chans = ((void *)base) + ALIGN(sizeof(struct coh901318_base), 4);
 
-	base->virtbase = ioremap(io->start, resource_size(io));
-	if (!base->virtbase) {
-		err = -ENOMEM;
-		goto err_no_ioremap;
-	}
+	base->virtbase = devm_ioremap(&pdev->dev, io->start, resource_size(io));
+	if (!base->virtbase)
+		return -ENOMEM;
 
 	base->dev = &pdev->dev;
 	base->platform = pdata;
@@ -1474,25 +1472,20 @@ static int __init coh901318_probe(struct platform_device *pdev)
 
 	COH901318_DEBUGFS_ASSIGN(debugfs_dma_base, base);
 
-	platform_set_drvdata(pdev, base);
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
-		goto err_no_irq;
-
-	err = request_irq(irq, dma_irq_handler, IRQF_DISABLED,
-			  "coh901318", base);
-	if (err) {
-		dev_crit(&pdev->dev,
-			 "Cannot allocate IRQ for DMA controller!\n");
-		goto err_request_irq;
-	}
+		return irq;
+
+	err = devm_request_irq(&pdev->dev, irq, dma_irq_handler, IRQF_DISABLED,
+			       "coh901318", base);
+	if (err)
+		return err;
 
 	err = coh901318_pool_create(&base->pool, &pdev->dev,
 				    sizeof(struct coh901318_lli),
 				    32);
 	if (err)
-		goto err_pool_create;
+		return err;
 
 	/* init channels for device transfers */
 	coh901318_base_init(&base->dma_slave,  base->platform->chans_slave,
@@ -1538,6 +1531,7 @@ static int __init coh901318_probe(struct platform_device *pdev)
 	if (err)
 		goto err_register_memcpy;
 
+	platform_set_drvdata(pdev, base);
 	dev_info(&pdev->dev, "Initialized COH901318 DMA on virtual base 0x%08x\n",
 		(u32) base->virtbase);
 
@@ -1547,19 +1541,6 @@ static int __init coh901318_probe(struct platform_device *pdev)
 	dma_async_device_unregister(&base->dma_slave);
  err_register_slave:
 	coh901318_pool_destroy(&base->pool);
- err_pool_create:
-	free_irq(platform_get_irq(pdev, 0), base);
- err_request_irq:
- err_no_irq:
-	iounmap(base->virtbase);
- err_no_ioremap:
-	kfree(base);
- err_alloc_coh_dma_channels:
- err_no_platformdata:
-	release_mem_region(pdev->resource->start,
-			   resource_size(pdev->resource));
- err_request_mem:
- err_get_resource:
 	return err;
 }
 
@@ -1570,11 +1551,6 @@ static int __exit coh901318_remove(struct platform_device *pdev)
 	dma_async_device_unregister(&base->dma_memcpy);
 	dma_async_device_unregister(&base->dma_slave);
 	coh901318_pool_destroy(&base->pool);
-	free_irq(platform_get_irq(pdev, 0), base);
-	iounmap(base->virtbase);
-	kfree(base);
-	release_mem_region(pdev->resource->start,
-			   resource_size(pdev->resource));
 	return 0;
 }
 
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index 2397f6f451b..3491654cdf7 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -45,6 +45,8 @@
  * See Documentation/dmaengine.txt for more details
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/dma-mapping.h>
 #include <linux/init.h>
 #include <linux/module.h>
@@ -261,7 +263,7 @@ enum dma_status dma_sync_wait(struct dma_chan *chan, dma_cookie_t cookie)
 	do {
 		status = dma_async_is_tx_complete(chan, cookie, NULL, NULL);
 		if (time_after_eq(jiffies, dma_sync_wait_timeout)) {
-			printk(KERN_ERR "dma_sync_wait_timeout!\n");
+			pr_err("%s: timeout!\n", __func__);
 			return DMA_ERROR;
 		}
 	} while (status == DMA_IN_PROGRESS);
@@ -312,7 +314,7 @@ static int __init dma_channel_table_init(void)
 	}
 
 	if (err) {
-		pr_err("dmaengine: initialization failure\n");
+		pr_err("initialization failure\n");
 		for_each_dma_cap_mask(cap, dma_cap_mask_all)
 			if (channel_table[cap])
 				free_percpu(channel_table[cap]);
@@ -520,12 +522,12 @@ struct dma_chan *__dma_request_channel(dma_cap_mask_t *mask, dma_filter_fn fn, v
 			err = dma_chan_get(chan);
 
 			if (err == -ENODEV) {
-				pr_debug("%s: %s module removed\n", __func__,
-					 dma_chan_name(chan));
+				pr_debug("%s: %s module removed\n",
+					 __func__, dma_chan_name(chan));
 				list_del_rcu(&device->global_node);
 			} else if (err)
 				pr_debug("%s: failed to get %s: (%d)\n",
-					__func__, dma_chan_name(chan), err);
+					 __func__, dma_chan_name(chan), err);
 			else
 				break;
 			if (--device->privatecnt == 0)
@@ -535,7 +537,9 @@ struct dma_chan *__dma_request_channel(dma_cap_mask_t *mask, dma_filter_fn fn, v
 	}
 	mutex_unlock(&dma_list_mutex);
 
-	pr_debug("%s: %s (%s)\n", __func__, chan ? "success" : "fail",
+	pr_debug("%s: %s (%s)\n",
+		 __func__,
+		 chan ? "success" : "fail",
 		 chan ? dma_chan_name(chan) : NULL);
 
 	return chan;
@@ -579,7 +583,7 @@ void dmaengine_get(void)
 				break;
 			} else if (err)
 				pr_err("%s: failed to get %s: (%d)\n",
-					__func__, dma_chan_name(chan), err);
+				       __func__, dma_chan_name(chan), err);
 		}
 	}
 
@@ -1015,7 +1019,7 @@ dma_wait_for_async_tx(struct dma_async_tx_descriptor *tx)
 	while (tx->cookie == -EBUSY) {
 		if (time_after_eq(jiffies, dma_sync_wait_timeout)) {
 			pr_err("%s timeout waiting for descriptor submission\n",
-				__func__);
+			       __func__);
 			return DMA_ERROR;
 		}
 		cpu_relax();
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index 72129615757..d3c5a5a88f1 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -105,13 +105,13 @@ static struct dw_desc *dwc_desc_get(struct dw_dma_chan *dwc)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 	list_for_each_entry_safe(desc, _desc, &dwc->free_list, desc_node) {
+		i++;
 		if (async_tx_test_ack(&desc->txd)) {
 			list_del(&desc->desc_node);
 			ret = desc;
 			break;
 		}
 		dev_dbg(chan2dev(&dwc->chan), "desc %p not ACKed\n", desc);
-		i++;
 	}
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
@@ -191,6 +191,42 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 
 /*----------------------------------------------------------------------*/
 
+static inline unsigned int dwc_fast_fls(unsigned long long v)
+{
+	/*
+	 * We can be a lot more clever here, but this should take care
+	 * of the most common optimization.
+	 */
+	if (!(v & 7))
+		return 3;
+	else if (!(v & 3))
+		return 2;
+	else if (!(v & 1))
+		return 1;
+	return 0;
+}
+
+static void dwc_dump_chan_regs(struct dw_dma_chan *dwc)
+{
+	dev_err(chan2dev(&dwc->chan),
+		"  SAR: 0x%x DAR: 0x%x LLP: 0x%x CTL: 0x%x:%08x\n",
+		channel_readl(dwc, SAR),
+		channel_readl(dwc, DAR),
+		channel_readl(dwc, LLP),
+		channel_readl(dwc, CTL_HI),
+		channel_readl(dwc, CTL_LO));
+}
+
+
+static inline void dwc_chan_disable(struct dw_dma *dw, struct dw_dma_chan *dwc)
+{
+	channel_clear_bit(dw, CH_EN, dwc->mask);
+	while (dma_readl(dw, CH_EN) & dwc->mask)
+		cpu_relax();
+}
+
+/*----------------------------------------------------------------------*/
+
 /* Called with dwc->lock held and bh disabled */
 static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 {
@@ -200,13 +236,7 @@ static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 	if (dma_readl(dw, CH_EN) & dwc->mask) {
 		dev_err(chan2dev(&dwc->chan),
 			"BUG: Attempted to start non-idle channel\n");
-		dev_err(chan2dev(&dwc->chan),
-			"  SAR: 0x%x DAR: 0x%x LLP: 0x%x CTL: 0x%x:%08x\n",
-			channel_readl(dwc, SAR),
-			channel_readl(dwc, DAR),
-			channel_readl(dwc, LLP),
-			channel_readl(dwc, CTL_HI),
-			channel_readl(dwc, CTL_LO));
+		dwc_dump_chan_regs(dwc);
 
 		/* The tasklet will hopefully advance the queue... */
 		return;
@@ -290,9 +320,7 @@ static void dwc_complete_all(struct dw_dma *dw, struct dw_dma_chan *dwc)
 			"BUG: XFER bit set, but channel not idle!\n");
 
 		/* Try to continue after resetting the channel... */
-		channel_clear_bit(dw, CH_EN, dwc->mask);
-		while (dma_readl(dw, CH_EN) & dwc->mask)
-			cpu_relax();
+		dwc_chan_disable(dw, dwc);
 	}
 
 	/*
@@ -337,7 +365,8 @@ static void dwc_scan_descriptors(struct dw_dma *dw, struct dw_dma_chan *dwc)
 		return;
 	}
 
-	dev_vdbg(chan2dev(&dwc->chan), "scan_descriptors: llp=0x%x\n", llp);
+	dev_vdbg(chan2dev(&dwc->chan), "%s: llp=0x%llx\n", __func__,
+			(unsigned long long)llp);
 
 	list_for_each_entry_safe(desc, _desc, &dwc->active_list, desc_node) {
 		/* check first descriptors addr */
@@ -373,9 +402,7 @@ static void dwc_scan_descriptors(struct dw_dma *dw, struct dw_dma_chan *dwc)
 		"BUG: All descriptors done, but channel not idle!\n");
 
 	/* Try to continue after resetting the channel... */
-	channel_clear_bit(dw, CH_EN, dwc->mask);
-	while (dma_readl(dw, CH_EN) & dwc->mask)
-		cpu_relax();
+	dwc_chan_disable(dw, dwc);
 
 	if (!list_empty(&dwc->queue)) {
 		list_move(dwc->queue.next, &dwc->active_list);
@@ -384,12 +411,11 @@ static void dwc_scan_descriptors(struct dw_dma *dw, struct dw_dma_chan *dwc)
 	spin_unlock_irqrestore(&dwc->lock, flags);
 }
 
-static void dwc_dump_lli(struct dw_dma_chan *dwc, struct dw_lli *lli)
+static inline void dwc_dump_lli(struct dw_dma_chan *dwc, struct dw_lli *lli)
 {
 	dev_printk(KERN_CRIT, chan2dev(&dwc->chan),
 			"  desc: s0x%x d0x%x l0x%x c0x%x:%x\n",
-			lli->sar, lli->dar, lli->llp,
-			lli->ctlhi, lli->ctllo);
+			lli->sar, lli->dar, lli->llp, lli->ctlhi, lli->ctllo);
 }
 
 static void dwc_handle_error(struct dw_dma *dw, struct dw_dma_chan *dwc)
@@ -487,17 +513,9 @@ static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
 
 		spin_lock_irqsave(&dwc->lock, flags);
 
-		dev_err(chan2dev(&dwc->chan),
-			"  SAR: 0x%x DAR: 0x%x LLP: 0x%x CTL: 0x%x:%08x\n",
-			channel_readl(dwc, SAR),
-			channel_readl(dwc, DAR),
-			channel_readl(dwc, LLP),
-			channel_readl(dwc, CTL_HI),
-			channel_readl(dwc, CTL_LO));
+		dwc_dump_chan_regs(dwc);
 
-		channel_clear_bit(dw, CH_EN, dwc->mask);
-		while (dma_readl(dw, CH_EN) & dwc->mask)
-			cpu_relax();
+		dwc_chan_disable(dw, dwc);
 
 		/* make sure DMA does not restart by loading a new list */
 		channel_writel(dwc, LLP, 0);
@@ -527,7 +545,7 @@ static void dw_dma_tasklet(unsigned long data)
 	status_xfer = dma_readl(dw, RAW.XFER);
 	status_err = dma_readl(dw, RAW.ERROR);
 
-	dev_vdbg(dw->dma.dev, "tasklet: status_err=%x\n", status_err);
+	dev_vdbg(dw->dma.dev, "%s: status_err=%x\n", __func__, status_err);
 
 	for (i = 0; i < dw->dma.chancnt; i++) {
 		dwc = &dw->chan[i];
@@ -551,7 +569,7 @@ static irqreturn_t dw_dma_interrupt(int irq, void *dev_id)
 	struct dw_dma *dw = dev_id;
 	u32 status;
 
-	dev_vdbg(dw->dma.dev, "interrupt: status=0x%x\n",
+	dev_vdbg(dw->dma.dev, "%s: status=0x%x\n", __func__,
 			dma_readl(dw, STATUS_INT));
 
 	/*
@@ -597,12 +615,12 @@ static dma_cookie_t dwc_tx_submit(struct dma_async_tx_descriptor *tx)
 	 * for DMA. But this is hard to do in a race-free manner.
 	 */
 	if (list_empty(&dwc->active_list)) {
-		dev_vdbg(chan2dev(tx->chan), "tx_submit: started %u\n",
+		dev_vdbg(chan2dev(tx->chan), "%s: started %u\n", __func__,
 				desc->txd.cookie);
 		list_add_tail(&desc->desc_node, &dwc->active_list);
 		dwc_dostart(dwc, dwc_first_active(dwc));
 	} else {
-		dev_vdbg(chan2dev(tx->chan), "tx_submit: queued %u\n",
+		dev_vdbg(chan2dev(tx->chan), "%s: queued %u\n", __func__,
 				desc->txd.cookie);
 
 		list_add_tail(&desc->desc_node, &dwc->queue);
@@ -627,26 +645,17 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	unsigned int		dst_width;
 	u32			ctllo;
 
-	dev_vdbg(chan2dev(chan), "prep_dma_memcpy d0x%x s0x%x l0x%zx f0x%lx\n",
-			dest, src, len, flags);
+	dev_vdbg(chan2dev(chan),
+			"%s: d0x%llx s0x%llx l0x%zx f0x%lx\n", __func__,
+			(unsigned long long)dest, (unsigned long long)src,
+			len, flags);
 
 	if (unlikely(!len)) {
-		dev_dbg(chan2dev(chan), "prep_dma_memcpy: length is zero!\n");
+		dev_dbg(chan2dev(chan), "%s: length is zero!\n", __func__);
 		return NULL;
 	}
 
-	/*
-	 * We can be a lot more clever here, but this should take care
-	 * of the most common optimization.
-	 */
-	if (!((src | dest  | len) & 7))
-		src_width = dst_width = 3;
-	else if (!((src | dest  | len) & 3))
-		src_width = dst_width = 2;
-	else if (!((src | dest | len) & 1))
-		src_width = dst_width = 1;
-	else
-		src_width = dst_width = 0;
+	src_width = dst_width = dwc_fast_fls(src | dest | len);
 
 	ctllo = DWC_DEFAULT_CTLLO(chan)
 			| DWC_CTLL_DST_WIDTH(dst_width)
@@ -720,7 +729,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 	struct scatterlist	*sg;
 	size_t			total_len = 0;
 
-	dev_vdbg(chan2dev(chan), "prep_dma_slave\n");
+	dev_vdbg(chan2dev(chan), "%s\n", __func__);
 
 	if (unlikely(!dws || !sg_len))
 		return NULL;
@@ -746,14 +755,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
 
-			if (!((mem | len) & 7))
-				mem_width = 3;
-			else if (!((mem | len) & 3))
-				mem_width = 2;
-			else if (!((mem | len) & 1))
-				mem_width = 1;
-			else
-				mem_width = 0;
+			mem_width = dwc_fast_fls(mem | len);
 
 slave_sg_todev_fill_desc:
 			desc = dwc_desc_get(dwc);
@@ -813,14 +815,7 @@ slave_sg_todev_fill_desc:
 			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
 
-			if (!((mem | len) & 7))
-				mem_width = 3;
-			else if (!((mem | len) & 3))
-				mem_width = 2;
-			else if (!((mem | len) & 1))
-				mem_width = 1;
-			else
-				mem_width = 0;
+			mem_width = dwc_fast_fls(mem | len);
 
 slave_sg_fromdev_fill_desc:
 			desc = dwc_desc_get(dwc);
@@ -950,9 +945,7 @@ static int dwc_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	} else if (cmd == DMA_TERMINATE_ALL) {
 		spin_lock_irqsave(&dwc->lock, flags);
 
-		channel_clear_bit(dw, CH_EN, dwc->mask);
-		while (dma_readl(dw, CH_EN) & dwc->mask)
-			cpu_relax();
+		dwc_chan_disable(dw, dwc);
 
 		dwc->paused = false;
 
@@ -1014,7 +1007,7 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 	int			i;
 	unsigned long		flags;
 
-	dev_vdbg(chan2dev(chan), "alloc_chan_resources\n");
+	dev_vdbg(chan2dev(chan), "%s\n", __func__);
 
 	/* ASSERT:  channel is idle */
 	if (dma_readl(dw, CH_EN) & dwc->mask) {
@@ -1057,8 +1050,7 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
-	dev_dbg(chan2dev(chan),
-		"alloc_chan_resources allocated %d descriptors\n", i);
+	dev_dbg(chan2dev(chan), "%s: allocated %d descriptors\n", __func__, i);
 
 	return i;
 }
@@ -1071,7 +1063,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	unsigned long		flags;
 	LIST_HEAD(list);
 
-	dev_dbg(chan2dev(chan), "free_chan_resources (descs allocated=%u)\n",
+	dev_dbg(chan2dev(chan), "%s: descs allocated=%u\n", __func__,
 			dwc->descs_allocated);
 
 	/* ASSERT:  channel is idle */
@@ -1097,7 +1089,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 		kfree(desc);
 	}
 
-	dev_vdbg(chan2dev(chan), "free_chan_resources done\n");
+	dev_vdbg(chan2dev(chan), "%s: done\n", __func__);
 }
 
 /* --------------------- Cyclic DMA API extensions -------------------- */
@@ -1126,13 +1118,7 @@ int dw_dma_cyclic_start(struct dma_chan *chan)
 	if (dma_readl(dw, CH_EN) & dwc->mask) {
 		dev_err(chan2dev(&dwc->chan),
 			"BUG: Attempted to start non-idle channel\n");
-		dev_err(chan2dev(&dwc->chan),
-			"  SAR: 0x%x DAR: 0x%x LLP: 0x%x CTL: 0x%x:%08x\n",
-			channel_readl(dwc, SAR),
-			channel_readl(dwc, DAR),
-			channel_readl(dwc, LLP),
-			channel_readl(dwc, CTL_HI),
-			channel_readl(dwc, CTL_LO));
+		dwc_dump_chan_regs(dwc);
 		spin_unlock_irqrestore(&dwc->lock, flags);
 		return -EBUSY;
 	}
@@ -1167,9 +1153,7 @@ void dw_dma_cyclic_stop(struct dma_chan *chan)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
-	channel_clear_bit(dw, CH_EN, dwc->mask);
-	while (dma_readl(dw, CH_EN) & dwc->mask)
-		cpu_relax();
+	dwc_chan_disable(dw, dwc);
 
 	spin_unlock_irqrestore(&dwc->lock, flags);
 }
@@ -1308,9 +1292,9 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 	dma_sync_single_for_device(chan2parent(chan), last->txd.phys,
 			sizeof(last->lli), DMA_TO_DEVICE);
 
-	dev_dbg(chan2dev(&dwc->chan), "cyclic prepared buf 0x%08x len %zu "
-			"period %zu periods %d\n", buf_addr, buf_len,
-			period_len, periods);
+	dev_dbg(chan2dev(&dwc->chan), "cyclic prepared buf 0x%llx len %zu "
+			"period %zu periods %d\n", (unsigned long long)buf_addr,
+			buf_len, period_len, periods);
 
 	cdesc->periods = periods;
 	dwc->cdesc = cdesc;
@@ -1340,16 +1324,14 @@ void dw_dma_cyclic_free(struct dma_chan *chan)
 	int			i;
 	unsigned long		flags;
 
-	dev_dbg(chan2dev(&dwc->chan), "cyclic free\n");
+	dev_dbg(chan2dev(&dwc->chan), "%s\n", __func__);
 
 	if (!cdesc)
 		return;
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
-	channel_clear_bit(dw, CH_EN, dwc->mask);
-	while (dma_readl(dw, CH_EN) & dwc->mask)
-		cpu_relax();
+	dwc_chan_disable(dw, dwc);
 
 	dma_writel(dw, CLEAR.ERROR, dwc->mask);
 	dma_writel(dw, CLEAR.XFER, dwc->mask);
@@ -1386,7 +1368,7 @@ static void dw_dma_off(struct dw_dma *dw)
 		dw->chan[i].initialized = false;
 }
 
-static int __init dw_probe(struct platform_device *pdev)
+static int __devinit dw_probe(struct platform_device *pdev)
 {
 	struct dw_dma_platform_data *pdata;
 	struct resource		*io;
@@ -1432,9 +1414,15 @@ static int __init dw_probe(struct platform_device *pdev)
 	}
 	clk_prepare_enable(dw->clk);
 
+	/* Calculate all channel mask before DMA setup */
+	dw->all_chan_mask = (1 << pdata->nr_channels) - 1;
+
 	/* force dma off, just in case */
 	dw_dma_off(dw);
 
+	/* disable BLOCK interrupts as well */
+	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
+
 	err = request_irq(irq, dw_dma_interrupt, 0, "dw_dmac", dw);
 	if (err)
 		goto err_irq;
@@ -1443,8 +1431,6 @@ static int __init dw_probe(struct platform_device *pdev)
 
 	tasklet_init(&dw->tasklet, dw_dma_tasklet, (unsigned long)dw);
 
-	dw->all_chan_mask = (1 << pdata->nr_channels) - 1;
-
 	INIT_LIST_HEAD(&dw->dma.channels);
 	for (i = 0; i < pdata->nr_channels; i++) {
 		struct dw_dma_chan	*dwc = &dw->chan[i];
@@ -1474,17 +1460,13 @@ static int __init dw_probe(struct platform_device *pdev)
 		channel_clear_bit(dw, CH_EN, dwc->mask);
 	}
 
-	/* Clear/disable all interrupts on all channels. */
+	/* Clear all interrupts on all channels. */
 	dma_writel(dw, CLEAR.XFER, dw->all_chan_mask);
+	dma_writel(dw, CLEAR.BLOCK, dw->all_chan_mask);
 	dma_writel(dw, CLEAR.SRC_TRAN, dw->all_chan_mask);
 	dma_writel(dw, CLEAR.DST_TRAN, dw->all_chan_mask);
 	dma_writel(dw, CLEAR.ERROR, dw->all_chan_mask);
 
-	channel_clear_bit(dw, MASK.XFER, dw->all_chan_mask);
-	channel_clear_bit(dw, MASK.SRC_TRAN, dw->all_chan_mask);
-	channel_clear_bit(dw, MASK.DST_TRAN, dw->all_chan_mask);
-	channel_clear_bit(dw, MASK.ERROR, dw->all_chan_mask);
-
 	dma_cap_set(DMA_MEMCPY, dw->dma.cap_mask);
 	dma_cap_set(DMA_SLAVE, dw->dma.cap_mask);
 	if (pdata->is_private)
@@ -1523,7 +1505,7 @@ err_kfree:
 	return err;
 }
 
-static int __exit dw_remove(struct platform_device *pdev)
+static int __devexit dw_remove(struct platform_device *pdev)
 {
 	struct dw_dma		*dw = platform_get_drvdata(pdev);
 	struct dw_dma_chan	*dwc, *_dwc;
@@ -1602,7 +1584,7 @@ MODULE_DEVICE_TABLE(of, dw_dma_id_table);
 #endif
 
 static struct platform_driver dw_driver = {
-	.remove		= __exit_p(dw_remove),
+	.remove		= __devexit_p(dw_remove),
 	.shutdown	= dw_shutdown,
 	.driver = {
 		.name	= "dw_dmac",
diff --git a/drivers/dma/dw_dmac_regs.h b/drivers/dma/dw_dmac_regs.h
index f298f69ecbf..50830bee087 100644
--- a/drivers/dma/dw_dmac_regs.h
+++ b/drivers/dma/dw_dmac_regs.h
@@ -82,7 +82,7 @@ struct dw_dma_regs {
 	DW_REG(ID);
 	DW_REG(TEST);
 
-	/* optional encoded params, 0x3c8..0x3 */
+	/* optional encoded params, 0x3c8..0x3f7 */
 };
 
 /* Bitfields in CTL_LO */
@@ -219,9 +219,9 @@ static inline struct dw_dma *to_dw_dma(struct dma_device *ddev)
 /* LLI == Linked List Item; a.k.a. DMA block descriptor */
 struct dw_lli {
 	/* values that are not changed by hardware */
-	dma_addr_t	sar;
-	dma_addr_t	dar;
-	dma_addr_t	llp;		/* chain to next lli */
+	u32		sar;
+	u32		dar;
+	u32		llp;		/* chain to next lli */
 	u32		ctllo;
 	/* values that may get written back: */
 	u32		ctlhi;
diff --git a/drivers/dma/ipu/ipu_idmac.c b/drivers/dma/ipu/ipu_idmac.c
index 5ec72044ea4..c7573e50aa1 100644
--- a/drivers/dma/ipu/ipu_idmac.c
+++ b/drivers/dma/ipu/ipu_idmac.c
@@ -1663,7 +1663,6 @@ static void __exit ipu_idmac_exit(struct ipu *ipu)
 
 static int __init ipu_probe(struct platform_device *pdev)
 {
-	struct ipu_platform_data *pdata = pdev->dev.platform_data;
 	struct resource *mem_ipu, *mem_ic;
 	int ret;
 
@@ -1671,7 +1670,7 @@ static int __init ipu_probe(struct platform_device *pdev)
 
 	mem_ipu	= platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	mem_ic	= platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (!pdata || !mem_ipu || !mem_ic)
+	if (!mem_ipu || !mem_ic)
 		return -EINVAL;
 
 	ipu_data.dev = &pdev->dev;
@@ -1688,10 +1687,9 @@ static int __init ipu_probe(struct platform_device *pdev)
 		goto err_noirq;
 
 	ipu_data.irq_err = ret;
-	ipu_data.irq_base = pdata->irq_base;
 
-	dev_dbg(&pdev->dev, "fn irq %u, err irq %u, irq-base %u\n",
-		ipu_data.irq_fn, ipu_data.irq_err, ipu_data.irq_base);
+	dev_dbg(&pdev->dev, "fn irq %u, err irq %u\n",
+		ipu_data.irq_fn, ipu_data.irq_err);
 
 	/* Remap IPU common registers */
 	ipu_data.reg_ipu = ioremap(mem_ipu->start, resource_size(mem_ipu));
diff --git a/drivers/dma/ipu/ipu_irq.c b/drivers/dma/ipu/ipu_irq.c
index a71f55e72be..fa95bcc3de1 100644
--- a/drivers/dma/ipu/ipu_irq.c
+++ b/drivers/dma/ipu/ipu_irq.c
@@ -14,6 +14,7 @@
 #include <linux/clk.h>
 #include <linux/irq.h>
 #include <linux/io.h>
+#include <linux/module.h>
 
 #include <mach/ipu.h>
 
@@ -354,10 +355,12 @@ static struct irq_chip ipu_irq_chip = {
 /* Install the IRQ handler */
 int __init ipu_irq_attach_irq(struct ipu *ipu, struct platform_device *dev)
 {
-	struct ipu_platform_data *pdata = dev->dev.platform_data;
-	unsigned int irq, irq_base, i;
+	unsigned int irq, i;
+	int irq_base = irq_alloc_descs(-1, 0, CONFIG_MX3_IPU_IRQS,
+				       numa_node_id());
 
-	irq_base = pdata->irq_base;
+	if (irq_base < 0)
+		return irq_base;
 
 	for (i = 0; i < IPU_IRQ_NR_BANKS; i++)
 		irq_bank[i].ipu = ipu;
@@ -387,15 +390,16 @@ int __init ipu_irq_attach_irq(struct ipu *ipu, struct platform_device *dev)
 	irq_set_handler_data(ipu->irq_err, ipu);
 	irq_set_chained_handler(ipu->irq_err, ipu_irq_err);
 
+	ipu->irq_base = irq_base;
+
 	return 0;
 }
 
 void ipu_irq_detach_irq(struct ipu *ipu, struct platform_device *dev)
 {
-	struct ipu_platform_data *pdata = dev->dev.platform_data;
 	unsigned int irq, irq_base;
 
-	irq_base = pdata->irq_base;
+	irq_base = ipu->irq_base;
 
 	irq_set_chained_handler(ipu->irq_fn, NULL);
 	irq_set_handler_data(ipu->irq_fn, NULL);
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
new file mode 100644
index 00000000000..8a15cf2163d
--- /dev/null
+++ b/drivers/dma/mmp_tdma.c
@@ -0,0 +1,610 @@
+/*
+ * Driver For Marvell Two-channel DMA Engine
+ *
+ * Copyright: Marvell International Ltd.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <mach/regs-icu.h>
+#include <mach/sram.h>
+
+#include "dmaengine.h"
+
+/*
+ * Two-Channel DMA registers
+ */
+#define TDBCR		0x00	/* Byte Count */
+#define TDSAR		0x10	/* Src Addr */
+#define TDDAR		0x20	/* Dst Addr */
+#define TDNDPR		0x30	/* Next Desc */
+#define TDCR		0x40	/* Control */
+#define TDCP		0x60	/* Priority*/
+#define TDCDPR		0x70	/* Current Desc */
+#define TDIMR		0x80	/* Int Mask */
+#define TDISR		0xa0	/* Int Status */
+
+/* Two-Channel DMA Control Register */
+#define TDCR_SSZ_8_BITS		(0x0 << 22)	/* Sample Size */
+#define TDCR_SSZ_12_BITS	(0x1 << 22)
+#define TDCR_SSZ_16_BITS	(0x2 << 22)
+#define TDCR_SSZ_20_BITS	(0x3 << 22)
+#define TDCR_SSZ_24_BITS	(0x4 << 22)
+#define TDCR_SSZ_32_BITS	(0x5 << 22)
+#define TDCR_SSZ_SHIFT		(0x1 << 22)
+#define TDCR_SSZ_MASK		(0x7 << 22)
+#define TDCR_SSPMOD		(0x1 << 21)	/* SSP MOD */
+#define TDCR_ABR		(0x1 << 20)	/* Channel Abort */
+#define TDCR_CDE		(0x1 << 17)	/* Close Desc Enable */
+#define TDCR_PACKMOD		(0x1 << 16)	/* Pack Mode (ADMA Only) */
+#define TDCR_CHANACT		(0x1 << 14)	/* Channel Active */
+#define TDCR_FETCHND		(0x1 << 13)	/* Fetch Next Desc */
+#define TDCR_CHANEN		(0x1 << 12)	/* Channel Enable */
+#define TDCR_INTMODE		(0x1 << 10)	/* Interrupt Mode */
+#define TDCR_CHAINMOD		(0x1 << 9)	/* Chain Mode */
+#define TDCR_BURSTSZ_MSK	(0x7 << 6)	/* Burst Size */
+#define TDCR_BURSTSZ_4B		(0x0 << 6)
+#define TDCR_BURSTSZ_8B		(0x1 << 6)
+#define TDCR_BURSTSZ_16B	(0x3 << 6)
+#define TDCR_BURSTSZ_32B	(0x6 << 6)
+#define TDCR_BURSTSZ_64B	(0x7 << 6)
+#define TDCR_BURSTSZ_SQU_32B	(0x7 << 6)
+#define TDCR_BURSTSZ_128B	(0x5 << 6)
+#define TDCR_DSTDIR_MSK		(0x3 << 4)	/* Dst Direction */
+#define TDCR_DSTDIR_ADDR_HOLD	(0x2 << 4)	/* Dst Addr Hold */
+#define TDCR_DSTDIR_ADDR_INC	(0x0 << 4)	/* Dst Addr Increment */
+#define TDCR_SRCDIR_MSK		(0x3 << 2)	/* Src Direction */
+#define TDCR_SRCDIR_ADDR_HOLD	(0x2 << 2)	/* Src Addr Hold */
+#define TDCR_SRCDIR_ADDR_INC	(0x0 << 2)	/* Src Addr Increment */
+#define TDCR_DSTDESCCONT	(0x1 << 1)
+#define TDCR_SRCDESTCONT	(0x1 << 0)
+
+/* Two-Channel DMA Int Mask Register */
+#define TDIMR_COMP		(0x1 << 0)
+
+/* Two-Channel DMA Int Status Register */
+#define TDISR_COMP		(0x1 << 0)
+
+/*
+ * Two-Channel DMA Descriptor Struct
+ * NOTE: desc's buf must be aligned to 16 bytes.
+ */
+struct mmp_tdma_desc {
+	u32 byte_cnt;
+	u32 src_addr;
+	u32 dst_addr;
+	u32 nxt_desc;
+};
+
+enum mmp_tdma_type {
+	MMP_AUD_TDMA = 0,
+	PXA910_SQU,
+};
+
+#define TDMA_ALIGNMENT		3
+#define TDMA_MAX_XFER_BYTES    SZ_64K
+
+struct mmp_tdma_chan {
+	struct device			*dev;
+	struct dma_chan			chan;
+	struct dma_async_tx_descriptor	desc;
+	struct tasklet_struct		tasklet;
+
+	struct mmp_tdma_desc		*desc_arr;
+	phys_addr_t			desc_arr_phys;
+	int				desc_num;
+	enum dma_transfer_direction	dir;
+	dma_addr_t			dev_addr;
+	u32				burst_sz;
+	enum dma_slave_buswidth		buswidth;
+	enum dma_status			status;
+
+	int				idx;
+	enum mmp_tdma_type		type;
+	int				irq;
+	unsigned long			reg_base;
+
+	size_t				buf_len;
+	size_t				period_len;
+	size_t				pos;
+};
+
+#define TDMA_CHANNEL_NUM 2
+struct mmp_tdma_device {
+	struct device			*dev;
+	void __iomem			*base;
+	struct dma_device		device;
+	struct mmp_tdma_chan		*tdmac[TDMA_CHANNEL_NUM];
+	int				irq;
+};
+
+#define to_mmp_tdma_chan(dchan) container_of(dchan, struct mmp_tdma_chan, chan)
+
+static void mmp_tdma_chan_set_desc(struct mmp_tdma_chan *tdmac, dma_addr_t phys)
+{
+	writel(phys, tdmac->reg_base + TDNDPR);
+	writel(readl(tdmac->reg_base + TDCR) | TDCR_FETCHND,
+					tdmac->reg_base + TDCR);
+}
+
+static void mmp_tdma_enable_chan(struct mmp_tdma_chan *tdmac)
+{
+	/* enable irq */
+	writel(TDIMR_COMP, tdmac->reg_base + TDIMR);
+	/* enable dma chan */
+	writel(readl(tdmac->reg_base + TDCR) | TDCR_CHANEN,
+					tdmac->reg_base + TDCR);
+	tdmac->status = DMA_IN_PROGRESS;
+}
+
+static void mmp_tdma_disable_chan(struct mmp_tdma_chan *tdmac)
+{
+	writel(readl(tdmac->reg_base + TDCR) & ~TDCR_CHANEN,
+					tdmac->reg_base + TDCR);
+	tdmac->status = DMA_SUCCESS;
+}
+
+static void mmp_tdma_resume_chan(struct mmp_tdma_chan *tdmac)
+{
+	writel(readl(tdmac->reg_base + TDCR) | TDCR_CHANEN,
+					tdmac->reg_base + TDCR);
+	tdmac->status = DMA_IN_PROGRESS;
+}
+
+static void mmp_tdma_pause_chan(struct mmp_tdma_chan *tdmac)
+{
+	writel(readl(tdmac->reg_base + TDCR) & ~TDCR_CHANEN,
+					tdmac->reg_base + TDCR);
+	tdmac->status = DMA_PAUSED;
+}
+
+static int mmp_tdma_config_chan(struct mmp_tdma_chan *tdmac)
+{
+	unsigned int tdcr;
+
+	mmp_tdma_disable_chan(tdmac);
+
+	if (tdmac->dir == DMA_MEM_TO_DEV)
+		tdcr = TDCR_DSTDIR_ADDR_HOLD | TDCR_SRCDIR_ADDR_INC;
+	else if (tdmac->dir == DMA_DEV_TO_MEM)
+		tdcr = TDCR_SRCDIR_ADDR_HOLD | TDCR_DSTDIR_ADDR_INC;
+
+	if (tdmac->type == MMP_AUD_TDMA) {
+		tdcr |= TDCR_PACKMOD;
+
+		switch (tdmac->burst_sz) {
+		case 4:
+			tdcr |= TDCR_BURSTSZ_4B;
+			break;
+		case 8:
+			tdcr |= TDCR_BURSTSZ_8B;
+			break;
+		case 16:
+			tdcr |= TDCR_BURSTSZ_16B;
+			break;
+		case 32:
+			tdcr |= TDCR_BURSTSZ_32B;
+			break;
+		case 64:
+			tdcr |= TDCR_BURSTSZ_64B;
+			break;
+		case 128:
+			tdcr |= TDCR_BURSTSZ_128B;
+			break;
+		default:
+			dev_err(tdmac->dev, "mmp_tdma: unknown burst size.\n");
+			return -EINVAL;
+		}
+
+		switch (tdmac->buswidth) {
+		case DMA_SLAVE_BUSWIDTH_1_BYTE:
+			tdcr |= TDCR_SSZ_8_BITS;
+			break;
+		case DMA_SLAVE_BUSWIDTH_2_BYTES:
+			tdcr |= TDCR_SSZ_16_BITS;
+			break;
+		case DMA_SLAVE_BUSWIDTH_4_BYTES:
+			tdcr |= TDCR_SSZ_32_BITS;
+			break;
+		default:
+			dev_err(tdmac->dev, "mmp_tdma: unknown bus size.\n");
+			return -EINVAL;
+		}
+	} else if (tdmac->type == PXA910_SQU) {
+		tdcr |= TDCR_BURSTSZ_SQU_32B;
+		tdcr |= TDCR_SSPMOD;
+	}
+
+	writel(tdcr, tdmac->reg_base + TDCR);
+	return 0;
+}
+
+static int mmp_tdma_clear_chan_irq(struct mmp_tdma_chan *tdmac)
+{
+	u32 reg = readl(tdmac->reg_base + TDISR);
+
+	if (reg & TDISR_COMP) {
+		/* clear irq */
+		reg &= ~TDISR_COMP;
+		writel(reg, tdmac->reg_base + TDISR);
+
+		return 0;
+	}
+	return -EAGAIN;
+}
+
+static irqreturn_t mmp_tdma_chan_handler(int irq, void *dev_id)
+{
+	struct mmp_tdma_chan *tdmac = dev_id;
+
+	if (mmp_tdma_clear_chan_irq(tdmac) == 0) {
+		tdmac->pos = (tdmac->pos + tdmac->period_len) % tdmac->buf_len;
+		tasklet_schedule(&tdmac->tasklet);
+		return IRQ_HANDLED;
+	} else
+		return IRQ_NONE;
+}
+
+static irqreturn_t mmp_tdma_int_handler(int irq, void *dev_id)
+{
+	struct mmp_tdma_device *tdev = dev_id;
+	int i, ret;
+	int irq_num = 0;
+
+	for (i = 0; i < TDMA_CHANNEL_NUM; i++) {
+		struct mmp_tdma_chan *tdmac = tdev->tdmac[i];
+
+		ret = mmp_tdma_chan_handler(irq, tdmac);
+		if (ret == IRQ_HANDLED)
+			irq_num++;
+	}
+
+	if (irq_num)
+		return IRQ_HANDLED;
+	else
+		return IRQ_NONE;
+}
+
+static void dma_do_tasklet(unsigned long data)
+{
+	struct mmp_tdma_chan *tdmac = (struct mmp_tdma_chan *)data;
+
+	if (tdmac->desc.callback)
+		tdmac->desc.callback(tdmac->desc.callback_param);
+
+}
+
+static void mmp_tdma_free_descriptor(struct mmp_tdma_chan *tdmac)
+{
+	struct gen_pool *gpool;
+	int size = tdmac->desc_num * sizeof(struct mmp_tdma_desc);
+
+	gpool = sram_get_gpool("asram");
+	if (tdmac->desc_arr)
+		gen_pool_free(gpool, (unsigned long)tdmac->desc_arr,
+				size);
+	tdmac->desc_arr = NULL;
+
+	return;
+}
+
+static dma_cookie_t mmp_tdma_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(tx->chan);
+
+	mmp_tdma_chan_set_desc(tdmac, tdmac->desc_arr_phys);
+
+	return 0;
+}
+
+static int mmp_tdma_alloc_chan_resources(struct dma_chan *chan)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+	int ret;
+
+	dma_async_tx_descriptor_init(&tdmac->desc, chan);
+	tdmac->desc.tx_submit = mmp_tdma_tx_submit;
+
+	if (tdmac->irq) {
+		ret = devm_request_irq(tdmac->dev, tdmac->irq,
+			mmp_tdma_chan_handler, IRQF_DISABLED, "tdma", tdmac);
+		if (ret)
+			return ret;
+	}
+	return 1;
+}
+
+static void mmp_tdma_free_chan_resources(struct dma_chan *chan)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+
+	if (tdmac->irq)
+		devm_free_irq(tdmac->dev, tdmac->irq, tdmac);
+	mmp_tdma_free_descriptor(tdmac);
+	return;
+}
+
+struct mmp_tdma_desc *mmp_tdma_alloc_descriptor(struct mmp_tdma_chan *tdmac)
+{
+	struct gen_pool *gpool;
+	int size = tdmac->desc_num * sizeof(struct mmp_tdma_desc);
+
+	gpool = sram_get_gpool("asram");
+	if (!gpool)
+		return NULL;
+
+	tdmac->desc_arr = (void *)gen_pool_alloc(gpool, size);
+	if (!tdmac->desc_arr)
+		return NULL;
+
+	tdmac->desc_arr_phys = gen_pool_virt_to_phys(gpool,
+			(unsigned long)tdmac->desc_arr);
+
+	return tdmac->desc_arr;
+}
+
+static struct dma_async_tx_descriptor *mmp_tdma_prep_dma_cyclic(
+		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
+		size_t period_len, enum dma_transfer_direction direction,
+		void *context)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+	struct mmp_tdma_desc *desc;
+	int num_periods = buf_len / period_len;
+	int i = 0, buf = 0;
+
+	if (tdmac->status != DMA_SUCCESS)
+		return NULL;
+
+	if (period_len > TDMA_MAX_XFER_BYTES) {
+		dev_err(tdmac->dev,
+				"maximum period size exceeded: %d > %d\n",
+				period_len, TDMA_MAX_XFER_BYTES);
+		goto err_out;
+	}
+
+	tdmac->status = DMA_IN_PROGRESS;
+	tdmac->desc_num = num_periods;
+	desc = mmp_tdma_alloc_descriptor(tdmac);
+	if (!desc)
+		goto err_out;
+
+	while (buf < buf_len) {
+		desc = &tdmac->desc_arr[i];
+
+		if (i + 1 == num_periods)
+			desc->nxt_desc = tdmac->desc_arr_phys;
+		else
+			desc->nxt_desc = tdmac->desc_arr_phys +
+				sizeof(*desc) * (i + 1);
+
+		if (direction == DMA_MEM_TO_DEV) {
+			desc->src_addr = dma_addr;
+			desc->dst_addr = tdmac->dev_addr;
+		} else {
+			desc->src_addr = tdmac->dev_addr;
+			desc->dst_addr = dma_addr;
+		}
+		desc->byte_cnt = period_len;
+		dma_addr += period_len;
+		buf += period_len;
+		i++;
+	}
+
+	tdmac->buf_len = buf_len;
+	tdmac->period_len = period_len;
+	tdmac->pos = 0;
+
+	return &tdmac->desc;
+
+err_out:
+	tdmac->status = DMA_ERROR;
+	return NULL;
+}
+
+static int mmp_tdma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+		unsigned long arg)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+	struct dma_slave_config *dmaengine_cfg = (void *)arg;
+	int ret = 0;
+
+	switch (cmd) {
+	case DMA_TERMINATE_ALL:
+		mmp_tdma_disable_chan(tdmac);
+		break;
+	case DMA_PAUSE:
+		mmp_tdma_pause_chan(tdmac);
+		break;
+	case DMA_RESUME:
+		mmp_tdma_resume_chan(tdmac);
+		break;
+	case DMA_SLAVE_CONFIG:
+		if (dmaengine_cfg->direction == DMA_DEV_TO_MEM) {
+			tdmac->dev_addr = dmaengine_cfg->src_addr;
+			tdmac->burst_sz = dmaengine_cfg->src_maxburst;
+			tdmac->buswidth = dmaengine_cfg->src_addr_width;
+		} else {
+			tdmac->dev_addr = dmaengine_cfg->dst_addr;
+			tdmac->burst_sz = dmaengine_cfg->dst_maxburst;
+			tdmac->buswidth = dmaengine_cfg->dst_addr_width;
+		}
+		tdmac->dir = dmaengine_cfg->direction;
+		return mmp_tdma_config_chan(tdmac);
+	default:
+		ret = -ENOSYS;
+	}
+
+	return ret;
+}
+
+static enum dma_status mmp_tdma_tx_status(struct dma_chan *chan,
+			dma_cookie_t cookie, struct dma_tx_state *txstate)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+
+	dma_set_residue(txstate, tdmac->buf_len - tdmac->pos);
+
+	return tdmac->status;
+}
+
+static void mmp_tdma_issue_pending(struct dma_chan *chan)
+{
+	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
+
+	mmp_tdma_enable_chan(tdmac);
+}
+
+static int __devexit mmp_tdma_remove(struct platform_device *pdev)
+{
+	struct mmp_tdma_device *tdev = platform_get_drvdata(pdev);
+
+	dma_async_device_unregister(&tdev->device);
+	return 0;
+}
+
+static int __devinit mmp_tdma_chan_init(struct mmp_tdma_device *tdev,
+						int idx, int irq, int type)
+{
+	struct mmp_tdma_chan *tdmac;
+
+	if (idx >= TDMA_CHANNEL_NUM) {
+		dev_err(tdev->dev, "too many channels for device!\n");
+		return -EINVAL;
+	}
+
+	/* alloc channel */
+	tdmac = devm_kzalloc(tdev->dev, sizeof(*tdmac), GFP_KERNEL);
+	if (!tdmac) {
+		dev_err(tdev->dev, "no free memory for DMA channels!\n");
+		return -ENOMEM;
+	}
+	if (irq)
+		tdmac->irq = irq + idx;
+	tdmac->dev	   = tdev->dev;
+	tdmac->chan.device = &tdev->device;
+	tdmac->idx	   = idx;
+	tdmac->type	   = type;
+	tdmac->reg_base	   = (unsigned long)tdev->base + idx * 4;
+	tdmac->status = DMA_SUCCESS;
+	tdev->tdmac[tdmac->idx] = tdmac;
+	tasklet_init(&tdmac->tasklet, dma_do_tasklet, (unsigned long)tdmac);
+
+	/* add the channel to tdma_chan list */
+	list_add_tail(&tdmac->chan.device_node,
+			&tdev->device.channels);
+
+	return 0;
+}
+
+static int __devinit mmp_tdma_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+	enum mmp_tdma_type type = id->driver_data;
+	struct mmp_tdma_device *tdev;
+	struct resource *iores;
+	int i, ret;
+	int irq = 0;
+	int chan_num = TDMA_CHANNEL_NUM;
+
+	/* always have couple channels */
+	tdev = devm_kzalloc(&pdev->dev, sizeof(*tdev), GFP_KERNEL);
+	if (!tdev)
+		return -ENOMEM;
+
+	tdev->dev = &pdev->dev;
+	iores = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!iores)
+		return -EINVAL;
+
+	if (resource_size(iores) != chan_num)
+		tdev->irq = iores->start;
+	else
+		irq = iores->start;
+
+	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!iores)
+		return -EINVAL;
+
+	tdev->base = devm_request_and_ioremap(&pdev->dev, iores);
+	if (!tdev->base)
+		return -EADDRNOTAVAIL;
+
+	if (tdev->irq) {
+		ret = devm_request_irq(&pdev->dev, tdev->irq,
+			mmp_tdma_int_handler, IRQF_DISABLED, "tdma", tdev);
+		if (ret)
+			return ret;
+	}
+
+	dma_cap_set(DMA_SLAVE, tdev->device.cap_mask);
+	dma_cap_set(DMA_CYCLIC, tdev->device.cap_mask);
+
+	INIT_LIST_HEAD(&tdev->device.channels);
+
+	/* initialize channel parameters */
+	for (i = 0; i < chan_num; i++) {
+		ret = mmp_tdma_chan_init(tdev, i, irq, type);
+		if (ret)
+			return ret;
+	}
+
+	tdev->device.dev = &pdev->dev;
+	tdev->device.device_alloc_chan_resources =
+					mmp_tdma_alloc_chan_resources;
+	tdev->device.device_free_chan_resources =
+					mmp_tdma_free_chan_resources;
+	tdev->device.device_prep_dma_cyclic = mmp_tdma_prep_dma_cyclic;
+	tdev->device.device_tx_status = mmp_tdma_tx_status;
+	tdev->device.device_issue_pending = mmp_tdma_issue_pending;
+	tdev->device.device_control = mmp_tdma_control;
+	tdev->device.copy_align = TDMA_ALIGNMENT;
+
+	dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
+	platform_set_drvdata(pdev, tdev);
+
+	ret = dma_async_device_register(&tdev->device);
+	if (ret) {
+		dev_err(tdev->device.dev, "unable to register\n");
+		return ret;
+	}
+
+	dev_info(tdev->device.dev, "initialized\n");
+	return 0;
+}
+
+static const struct platform_device_id mmp_tdma_id_table[] = {
+	{ "mmp-adma",	MMP_AUD_TDMA },
+	{ "pxa910-squ",	PXA910_SQU },
+	{ },
+};
+
+static struct platform_driver mmp_tdma_driver = {
+	.driver		= {
+		.name	= "mmp-tdma",
+		.owner  = THIS_MODULE,
+	},
+	.id_table	= mmp_tdma_id_table,
+	.probe		= mmp_tdma_probe,
+	.remove		= __devexit_p(mmp_tdma_remove),
+};
+
+module_platform_driver(mmp_tdma_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MMP Two-Channel DMA Driver");
+MODULE_ALIAS("platform:mmp-tdma");
+MODULE_AUTHOR("Leo Yan <leoy@marvell.com>");
+MODULE_AUTHOR("Zhangfei Gao <zhangfei.gao@marvell.com>");
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index c96ab15319f..7f41b25805f 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -29,7 +29,6 @@
 #include <linux/of_device.h>
 
 #include <asm/irq.h>
-#include <mach/mxs.h>
 
 #include "dmaengine.h"
 
@@ -201,6 +200,7 @@ int mxs_dma_is_apbh(struct dma_chan *chan)
 
 	return dma_is_apbh(mxs_dma);
 }
+EXPORT_SYMBOL_GPL(mxs_dma_is_apbh);
 
 int mxs_dma_is_apbx(struct dma_chan *chan)
 {
@@ -209,6 +209,7 @@ int mxs_dma_is_apbx(struct dma_chan *chan)
 
 	return !dma_is_apbh(mxs_dma);
 }
+EXPORT_SYMBOL_GPL(mxs_dma_is_apbx);
 
 static void mxs_dma_reset_chan(struct mxs_dma_chan *mxs_chan)
 {
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
new file mode 100644
index 00000000000..ae056182613
--- /dev/null
+++ b/drivers/dma/omap-dma.c
@@ -0,0 +1,669 @@
+/*
+ * OMAP DMAengine support
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/omap-dma.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "virt-dma.h"
+#include <plat/dma.h>
+
+struct omap_dmadev {
+	struct dma_device ddev;
+	spinlock_t lock;
+	struct tasklet_struct task;
+	struct list_head pending;
+};
+
+struct omap_chan {
+	struct virt_dma_chan vc;
+	struct list_head node;
+
+	struct dma_slave_config	cfg;
+	unsigned dma_sig;
+	bool cyclic;
+
+	int dma_ch;
+	struct omap_desc *desc;
+	unsigned sgidx;
+};
+
+struct omap_sg {
+	dma_addr_t addr;
+	uint32_t en;		/* number of elements (24-bit) */
+	uint32_t fn;		/* number of frames (16-bit) */
+};
+
+struct omap_desc {
+	struct virt_dma_desc vd;
+	enum dma_transfer_direction dir;
+	dma_addr_t dev_addr;
+
+	int16_t fi;		/* for OMAP_DMA_SYNC_PACKET */
+	uint8_t es;		/* OMAP_DMA_DATA_TYPE_xxx */
+	uint8_t sync_mode;	/* OMAP_DMA_SYNC_xxx */
+	uint8_t sync_type;	/* OMAP_DMA_xxx_SYNC* */
+	uint8_t periph_port;	/* Peripheral port */
+
+	unsigned sglen;
+	struct omap_sg sg[0];
+};
+
+static const unsigned es_bytes[] = {
+	[OMAP_DMA_DATA_TYPE_S8] = 1,
+	[OMAP_DMA_DATA_TYPE_S16] = 2,
+	[OMAP_DMA_DATA_TYPE_S32] = 4,
+};
+
+static inline struct omap_dmadev *to_omap_dma_dev(struct dma_device *d)
+{
+	return container_of(d, struct omap_dmadev, ddev);
+}
+
+static inline struct omap_chan *to_omap_dma_chan(struct dma_chan *c)
+{
+	return container_of(c, struct omap_chan, vc.chan);
+}
+
+static inline struct omap_desc *to_omap_dma_desc(struct dma_async_tx_descriptor *t)
+{
+	return container_of(t, struct omap_desc, vd.tx);
+}
+
+static void omap_dma_desc_free(struct virt_dma_desc *vd)
+{
+	kfree(container_of(vd, struct omap_desc, vd));
+}
+
+static void omap_dma_start_sg(struct omap_chan *c, struct omap_desc *d,
+	unsigned idx)
+{
+	struct omap_sg *sg = d->sg + idx;
+
+	if (d->dir == DMA_DEV_TO_MEM)
+		omap_set_dma_dest_params(c->dma_ch, OMAP_DMA_PORT_EMIFF,
+			OMAP_DMA_AMODE_POST_INC, sg->addr, 0, 0);
+	else
+		omap_set_dma_src_params(c->dma_ch, OMAP_DMA_PORT_EMIFF,
+			OMAP_DMA_AMODE_POST_INC, sg->addr, 0, 0);
+
+	omap_set_dma_transfer_params(c->dma_ch, d->es, sg->en, sg->fn,
+		d->sync_mode, c->dma_sig, d->sync_type);
+
+	omap_start_dma(c->dma_ch);
+}
+
+static void omap_dma_start_desc(struct omap_chan *c)
+{
+	struct virt_dma_desc *vd = vchan_next_desc(&c->vc);
+	struct omap_desc *d;
+
+	if (!vd) {
+		c->desc = NULL;
+		return;
+	}
+
+	list_del(&vd->node);
+
+	c->desc = d = to_omap_dma_desc(&vd->tx);
+	c->sgidx = 0;
+
+	if (d->dir == DMA_DEV_TO_MEM)
+		omap_set_dma_src_params(c->dma_ch, d->periph_port,
+			OMAP_DMA_AMODE_CONSTANT, d->dev_addr, 0, d->fi);
+	else
+		omap_set_dma_dest_params(c->dma_ch, d->periph_port,
+			OMAP_DMA_AMODE_CONSTANT, d->dev_addr, 0, d->fi);
+
+	omap_dma_start_sg(c, d, 0);
+}
+
+static void omap_dma_callback(int ch, u16 status, void *data)
+{
+	struct omap_chan *c = data;
+	struct omap_desc *d;
+	unsigned long flags;
+
+	spin_lock_irqsave(&c->vc.lock, flags);
+	d = c->desc;
+	if (d) {
+		if (!c->cyclic) {
+			if (++c->sgidx < d->sglen) {
+				omap_dma_start_sg(c, d, c->sgidx);
+			} else {
+				omap_dma_start_desc(c);
+				vchan_cookie_complete(&d->vd);
+			}
+		} else {
+			vchan_cyclic_callback(&d->vd);
+		}
+	}
+	spin_unlock_irqrestore(&c->vc.lock, flags);
+}
+
+/*
+ * This callback schedules all pending channels.  We could be more
+ * clever here by postponing allocation of the real DMA channels to
+ * this point, and freeing them when our virtual channel becomes idle.
+ *
+ * We would then need to deal with 'all channels in-use'
+ */
+static void omap_dma_sched(unsigned long data)
+{
+	struct omap_dmadev *d = (struct omap_dmadev *)data;
+	LIST_HEAD(head);
+
+	spin_lock_irq(&d->lock);
+	list_splice_tail_init(&d->pending, &head);
+	spin_unlock_irq(&d->lock);
+
+	while (!list_empty(&head)) {
+		struct omap_chan *c = list_first_entry(&head,
+			struct omap_chan, node);
+
+		spin_lock_irq(&c->vc.lock);
+		list_del_init(&c->node);
+		omap_dma_start_desc(c);
+		spin_unlock_irq(&c->vc.lock);
+	}
+}
+
+static int omap_dma_alloc_chan_resources(struct dma_chan *chan)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+
+	dev_info(c->vc.chan.device->dev, "allocating channel for %u\n", c->dma_sig);
+
+	return omap_request_dma(c->dma_sig, "DMA engine",
+		omap_dma_callback, c, &c->dma_ch);
+}
+
+static void omap_dma_free_chan_resources(struct dma_chan *chan)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+
+	vchan_free_chan_resources(&c->vc);
+	omap_free_dma(c->dma_ch);
+
+	dev_info(c->vc.chan.device->dev, "freeing channel for %u\n", c->dma_sig);
+}
+
+static size_t omap_dma_sg_size(struct omap_sg *sg)
+{
+	return sg->en * sg->fn;
+}
+
+static size_t omap_dma_desc_size(struct omap_desc *d)
+{
+	unsigned i;
+	size_t size;
+
+	for (size = i = 0; i < d->sglen; i++)
+		size += omap_dma_sg_size(&d->sg[i]);
+
+	return size * es_bytes[d->es];
+}
+
+static size_t omap_dma_desc_size_pos(struct omap_desc *d, dma_addr_t addr)
+{
+	unsigned i;
+	size_t size, es_size = es_bytes[d->es];
+
+	for (size = i = 0; i < d->sglen; i++) {
+		size_t this_size = omap_dma_sg_size(&d->sg[i]) * es_size;
+
+		if (size)
+			size += this_size;
+		else if (addr >= d->sg[i].addr &&
+			 addr < d->sg[i].addr + this_size)
+			size += d->sg[i].addr + this_size - addr;
+	}
+	return size;
+}
+
+static enum dma_status omap_dma_tx_status(struct dma_chan *chan,
+	dma_cookie_t cookie, struct dma_tx_state *txstate)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+	struct virt_dma_desc *vd;
+	enum dma_status ret;
+	unsigned long flags;
+
+	ret = dma_cookie_status(chan, cookie, txstate);
+	if (ret == DMA_SUCCESS || !txstate)
+		return ret;
+
+	spin_lock_irqsave(&c->vc.lock, flags);
+	vd = vchan_find_desc(&c->vc, cookie);
+	if (vd) {
+		txstate->residue = omap_dma_desc_size(to_omap_dma_desc(&vd->tx));
+	} else if (c->desc && c->desc->vd.tx.cookie == cookie) {
+		struct omap_desc *d = c->desc;
+		dma_addr_t pos;
+
+		if (d->dir == DMA_MEM_TO_DEV)
+			pos = omap_get_dma_src_pos(c->dma_ch);
+		else if (d->dir == DMA_DEV_TO_MEM)
+			pos = omap_get_dma_dst_pos(c->dma_ch);
+		else
+			pos = 0;
+
+		txstate->residue = omap_dma_desc_size_pos(d, pos);
+	} else {
+		txstate->residue = 0;
+	}
+	spin_unlock_irqrestore(&c->vc.lock, flags);
+
+	return ret;
+}
+
+static void omap_dma_issue_pending(struct dma_chan *chan)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+	unsigned long flags;
+
+	spin_lock_irqsave(&c->vc.lock, flags);
+	if (vchan_issue_pending(&c->vc) && !c->desc) {
+		struct omap_dmadev *d = to_omap_dma_dev(chan->device);
+		spin_lock(&d->lock);
+		if (list_empty(&c->node))
+			list_add_tail(&c->node, &d->pending);
+		spin_unlock(&d->lock);
+		tasklet_schedule(&d->task);
+	}
+	spin_unlock_irqrestore(&c->vc.lock, flags);
+}
+
+static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
+	struct dma_chan *chan, struct scatterlist *sgl, unsigned sglen,
+	enum dma_transfer_direction dir, unsigned long tx_flags, void *context)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+	enum dma_slave_buswidth dev_width;
+	struct scatterlist *sgent;
+	struct omap_desc *d;
+	dma_addr_t dev_addr;
+	unsigned i, j = 0, es, en, frame_bytes, sync_type;
+	u32 burst;
+
+	if (dir == DMA_DEV_TO_MEM) {
+		dev_addr = c->cfg.src_addr;
+		dev_width = c->cfg.src_addr_width;
+		burst = c->cfg.src_maxburst;
+		sync_type = OMAP_DMA_SRC_SYNC;
+	} else if (dir == DMA_MEM_TO_DEV) {
+		dev_addr = c->cfg.dst_addr;
+		dev_width = c->cfg.dst_addr_width;
+		burst = c->cfg.dst_maxburst;
+		sync_type = OMAP_DMA_DST_SYNC;
+	} else {
+		dev_err(chan->device->dev, "%s: bad direction?\n", __func__);
+		return NULL;
+	}
+
+	/* Bus width translates to the element size (ES) */
+	switch (dev_width) {
+	case DMA_SLAVE_BUSWIDTH_1_BYTE:
+		es = OMAP_DMA_DATA_TYPE_S8;
+		break;
+	case DMA_SLAVE_BUSWIDTH_2_BYTES:
+		es = OMAP_DMA_DATA_TYPE_S16;
+		break;
+	case DMA_SLAVE_BUSWIDTH_4_BYTES:
+		es = OMAP_DMA_DATA_TYPE_S32;
+		break;
+	default: /* not reached */
+		return NULL;
+	}
+
+	/* Now allocate and setup the descriptor. */
+	d = kzalloc(sizeof(*d) + sglen * sizeof(d->sg[0]), GFP_ATOMIC);
+	if (!d)
+		return NULL;
+
+	d->dir = dir;
+	d->dev_addr = dev_addr;
+	d->es = es;
+	d->sync_mode = OMAP_DMA_SYNC_FRAME;
+	d->sync_type = sync_type;
+	d->periph_port = OMAP_DMA_PORT_TIPB;
+
+	/*
+	 * Build our scatterlist entries: each contains the address,
+	 * the number of elements (EN) in each frame, and the number of
+	 * frames (FN).  Number of bytes for this entry = ES * EN * FN.
+	 *
+	 * Burst size translates to number of elements with frame sync.
+	 * Note: DMA engine defines burst to be the number of dev-width
+	 * transfers.
+	 */
+	en = burst;
+	frame_bytes = es_bytes[es] * en;
+	for_each_sg(sgl, sgent, sglen, i) {
+		d->sg[j].addr = sg_dma_address(sgent);
+		d->sg[j].en = en;
+		d->sg[j].fn = sg_dma_len(sgent) / frame_bytes;
+		j++;
+	}
+
+	d->sglen = j;
+
+	return vchan_tx_prep(&c->vc, &d->vd, tx_flags);
+}
+
+static struct dma_async_tx_descriptor *omap_dma_prep_dma_cyclic(
+	struct dma_chan *chan, dma_addr_t buf_addr, size_t buf_len,
+	size_t period_len, enum dma_transfer_direction dir, void *context)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+	enum dma_slave_buswidth dev_width;
+	struct omap_desc *d;
+	dma_addr_t dev_addr;
+	unsigned es, sync_type;
+	u32 burst;
+
+	if (dir == DMA_DEV_TO_MEM) {
+		dev_addr = c->cfg.src_addr;
+		dev_width = c->cfg.src_addr_width;
+		burst = c->cfg.src_maxburst;
+		sync_type = OMAP_DMA_SRC_SYNC;
+	} else if (dir == DMA_MEM_TO_DEV) {
+		dev_addr = c->cfg.dst_addr;
+		dev_width = c->cfg.dst_addr_width;
+		burst = c->cfg.dst_maxburst;
+		sync_type = OMAP_DMA_DST_SYNC;
+	} else {
+		dev_err(chan->device->dev, "%s: bad direction?\n", __func__);
+		return NULL;
+	}
+
+	/* Bus width translates to the element size (ES) */
+	switch (dev_width) {
+	case DMA_SLAVE_BUSWIDTH_1_BYTE:
+		es = OMAP_DMA_DATA_TYPE_S8;
+		break;
+	case DMA_SLAVE_BUSWIDTH_2_BYTES:
+		es = OMAP_DMA_DATA_TYPE_S16;
+		break;
+	case DMA_SLAVE_BUSWIDTH_4_BYTES:
+		es = OMAP_DMA_DATA_TYPE_S32;
+		break;
+	default: /* not reached */
+		return NULL;
+	}
+
+	/* Now allocate and setup the descriptor. */
+	d = kzalloc(sizeof(*d) + sizeof(d->sg[0]), GFP_ATOMIC);
+	if (!d)
+		return NULL;
+
+	d->dir = dir;
+	d->dev_addr = dev_addr;
+	d->fi = burst;
+	d->es = es;
+	d->sync_mode = OMAP_DMA_SYNC_PACKET;
+	d->sync_type = sync_type;
+	d->periph_port = OMAP_DMA_PORT_MPUI;
+	d->sg[0].addr = buf_addr;
+	d->sg[0].en = period_len / es_bytes[es];
+	d->sg[0].fn = buf_len / period_len;
+	d->sglen = 1;
+
+	if (!c->cyclic) {
+		c->cyclic = true;
+		omap_dma_link_lch(c->dma_ch, c->dma_ch);
+		omap_enable_dma_irq(c->dma_ch, OMAP_DMA_FRAME_IRQ);
+		omap_disable_dma_irq(c->dma_ch, OMAP_DMA_BLOCK_IRQ);
+	}
+
+	if (!cpu_class_is_omap1()) {
+		omap_set_dma_src_burst_mode(c->dma_ch, OMAP_DMA_DATA_BURST_16);
+		omap_set_dma_dest_burst_mode(c->dma_ch, OMAP_DMA_DATA_BURST_16);
+	}
+
+	return vchan_tx_prep(&c->vc, &d->vd, DMA_CTRL_ACK | DMA_PREP_INTERRUPT);
+}
+
+static int omap_dma_slave_config(struct omap_chan *c, struct dma_slave_config *cfg)
+{
+	if (cfg->src_addr_width == DMA_SLAVE_BUSWIDTH_8_BYTES ||
+	    cfg->dst_addr_width == DMA_SLAVE_BUSWIDTH_8_BYTES)
+		return -EINVAL;
+
+	memcpy(&c->cfg, cfg, sizeof(c->cfg));
+
+	return 0;
+}
+
+static int omap_dma_terminate_all(struct omap_chan *c)
+{
+	struct omap_dmadev *d = to_omap_dma_dev(c->vc.chan.device);
+	unsigned long flags;
+	LIST_HEAD(head);
+
+	spin_lock_irqsave(&c->vc.lock, flags);
+
+	/* Prevent this channel being scheduled */
+	spin_lock(&d->lock);
+	list_del_init(&c->node);
+	spin_unlock(&d->lock);
+
+	/*
+	 * Stop DMA activity: we assume the callback will not be called
+	 * after omap_stop_dma() returns (even if it does, it will see
+	 * c->desc is NULL and exit.)
+	 */
+	if (c->desc) {
+		c->desc = NULL;
+		omap_stop_dma(c->dma_ch);
+	}
+
+	if (c->cyclic) {
+		c->cyclic = false;
+		omap_dma_unlink_lch(c->dma_ch, c->dma_ch);
+	}
+
+	vchan_get_all_descriptors(&c->vc, &head);
+	spin_unlock_irqrestore(&c->vc.lock, flags);
+	vchan_dma_desc_free_list(&c->vc, &head);
+
+	return 0;
+}
+
+static int omap_dma_pause(struct omap_chan *c)
+{
+	/* FIXME: not supported by platform private API */
+	return -EINVAL;
+}
+
+static int omap_dma_resume(struct omap_chan *c)
+{
+	/* FIXME: not supported by platform private API */
+	return -EINVAL;
+}
+
+static int omap_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+	unsigned long arg)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+	int ret;
+
+	switch (cmd) {
+	case DMA_SLAVE_CONFIG:
+		ret = omap_dma_slave_config(c, (struct dma_slave_config *)arg);
+		break;
+
+	case DMA_TERMINATE_ALL:
+		ret = omap_dma_terminate_all(c);
+		break;
+
+	case DMA_PAUSE:
+		ret = omap_dma_pause(c);
+		break;
+
+	case DMA_RESUME:
+		ret = omap_dma_resume(c);
+		break;
+
+	default:
+		ret = -ENXIO;
+		break;
+	}
+
+	return ret;
+}
+
+static int omap_dma_chan_init(struct omap_dmadev *od, int dma_sig)
+{
+	struct omap_chan *c;
+
+	c = kzalloc(sizeof(*c), GFP_KERNEL);
+	if (!c)
+		return -ENOMEM;
+
+	c->dma_sig = dma_sig;
+	c->vc.desc_free = omap_dma_desc_free;
+	vchan_init(&c->vc, &od->ddev);
+	INIT_LIST_HEAD(&c->node);
+
+	od->ddev.chancnt++;
+
+	return 0;
+}
+
+static void omap_dma_free(struct omap_dmadev *od)
+{
+	tasklet_kill(&od->task);
+	while (!list_empty(&od->ddev.channels)) {
+		struct omap_chan *c = list_first_entry(&od->ddev.channels,
+			struct omap_chan, vc.chan.device_node);
+
+		list_del(&c->vc.chan.device_node);
+		tasklet_kill(&c->vc.task);
+		kfree(c);
+	}
+	kfree(od);
+}
+
+static int omap_dma_probe(struct platform_device *pdev)
+{
+	struct omap_dmadev *od;
+	int rc, i;
+
+	od = kzalloc(sizeof(*od), GFP_KERNEL);
+	if (!od)
+		return -ENOMEM;
+
+	dma_cap_set(DMA_SLAVE, od->ddev.cap_mask);
+	dma_cap_set(DMA_CYCLIC, od->ddev.cap_mask);
+	od->ddev.device_alloc_chan_resources = omap_dma_alloc_chan_resources;
+	od->ddev.device_free_chan_resources = omap_dma_free_chan_resources;
+	od->ddev.device_tx_status = omap_dma_tx_status;
+	od->ddev.device_issue_pending = omap_dma_issue_pending;
+	od->ddev.device_prep_slave_sg = omap_dma_prep_slave_sg;
+	od->ddev.device_prep_dma_cyclic = omap_dma_prep_dma_cyclic;
+	od->ddev.device_control = omap_dma_control;
+	od->ddev.dev = &pdev->dev;
+	INIT_LIST_HEAD(&od->ddev.channels);
+	INIT_LIST_HEAD(&od->pending);
+	spin_lock_init(&od->lock);
+
+	tasklet_init(&od->task, omap_dma_sched, (unsigned long)od);
+
+	for (i = 0; i < 127; i++) {
+		rc = omap_dma_chan_init(od, i);
+		if (rc) {
+			omap_dma_free(od);
+			return rc;
+		}
+	}
+
+	rc = dma_async_device_register(&od->ddev);
+	if (rc) {
+		pr_warn("OMAP-DMA: failed to register slave DMA engine device: %d\n",
+			rc);
+		omap_dma_free(od);
+	} else {
+		platform_set_drvdata(pdev, od);
+	}
+
+	dev_info(&pdev->dev, "OMAP DMA engine driver\n");
+
+	return rc;
+}
+
+static int omap_dma_remove(struct platform_device *pdev)
+{
+	struct omap_dmadev *od = platform_get_drvdata(pdev);
+
+	dma_async_device_unregister(&od->ddev);
+	omap_dma_free(od);
+
+	return 0;
+}
+
+static struct platform_driver omap_dma_driver = {
+	.probe	= omap_dma_probe,
+	.remove	= omap_dma_remove,
+	.driver = {
+		.name = "omap-dma-engine",
+		.owner = THIS_MODULE,
+	},
+};
+
+bool omap_dma_filter_fn(struct dma_chan *chan, void *param)
+{
+	if (chan->device->dev->driver == &omap_dma_driver.driver) {
+		struct omap_chan *c = to_omap_dma_chan(chan);
+		unsigned req = *(unsigned *)param;
+
+		return req == c->dma_sig;
+	}
+	return false;
+}
+EXPORT_SYMBOL_GPL(omap_dma_filter_fn);
+
+static struct platform_device *pdev;
+
+static const struct platform_device_info omap_dma_dev_info = {
+	.name = "omap-dma-engine",
+	.id = -1,
+	.dma_mask = DMA_BIT_MASK(32),
+};
+
+static int omap_dma_init(void)
+{
+	int rc = platform_driver_register(&omap_dma_driver);
+
+	if (rc == 0) {
+		pdev = platform_device_register_full(&omap_dma_dev_info);
+		if (IS_ERR(pdev)) {
+			platform_driver_unregister(&omap_dma_driver);
+			rc = PTR_ERR(pdev);
+		}
+	}
+	return rc;
+}
+subsys_initcall(omap_dma_init);
+
+static void __exit omap_dma_exit(void)
+{
+	platform_device_unregister(pdev);
+	platform_driver_unregister(&omap_dma_driver);
+}
+module_exit(omap_dma_exit);
+
+MODULE_AUTHOR("Russell King");
+MODULE_LICENSE("GPL");
diff --git a/drivers/dma/sa11x0-dma.c b/drivers/dma/sa11x0-dma.c
index ec78ccef913..f5a73606217 100644
--- a/drivers/dma/sa11x0-dma.c
+++ b/drivers/dma/sa11x0-dma.c
@@ -21,6 +21,8 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 
+#include "virt-dma.h"
+
 #define NR_PHY_CHAN	6
 #define DMA_ALIGN	3
 #define DMA_MAX_SIZE	0x1fff
@@ -72,12 +74,13 @@ struct sa11x0_dma_sg {
 };
 
 struct sa11x0_dma_desc {
-	struct dma_async_tx_descriptor tx;
+	struct virt_dma_desc	vd;
+
 	u32			ddar;
 	size_t			size;
+	unsigned		period;
+	bool			cyclic;
 
-	/* maybe protected by c->lock */
-	struct list_head	node;
 	unsigned		sglen;
 	struct sa11x0_dma_sg	sg[0];
 };
@@ -85,15 +88,11 @@ struct sa11x0_dma_desc {
 struct sa11x0_dma_phy;
 
 struct sa11x0_dma_chan {
-	struct dma_chan		chan;
-	spinlock_t		lock;
-	dma_cookie_t		lc;
+	struct virt_dma_chan	vc;
 
-	/* protected by c->lock */
+	/* protected by c->vc.lock */
 	struct sa11x0_dma_phy	*phy;
 	enum dma_status		status;
-	struct list_head	desc_submitted;
-	struct list_head	desc_issued;
 
 	/* protected by d->lock */
 	struct list_head	node;
@@ -109,7 +108,7 @@ struct sa11x0_dma_phy {
 
 	struct sa11x0_dma_chan	*vchan;
 
-	/* Protected by c->lock */
+	/* Protected by c->vc.lock */
 	unsigned		sg_load;
 	struct sa11x0_dma_desc	*txd_load;
 	unsigned		sg_done;
@@ -127,13 +126,12 @@ struct sa11x0_dma_dev {
 	spinlock_t		lock;
 	struct tasklet_struct	task;
 	struct list_head	chan_pending;
-	struct list_head	desc_complete;
 	struct sa11x0_dma_phy	phy[NR_PHY_CHAN];
 };
 
 static struct sa11x0_dma_chan *to_sa11x0_dma_chan(struct dma_chan *chan)
 {
-	return container_of(chan, struct sa11x0_dma_chan, chan);
+	return container_of(chan, struct sa11x0_dma_chan, vc.chan);
 }
 
 static struct sa11x0_dma_dev *to_sa11x0_dma(struct dma_device *dmadev)
@@ -141,27 +139,26 @@ static struct sa11x0_dma_dev *to_sa11x0_dma(struct dma_device *dmadev)
 	return container_of(dmadev, struct sa11x0_dma_dev, slave);
 }
 
-static struct sa11x0_dma_desc *to_sa11x0_dma_tx(struct dma_async_tx_descriptor *tx)
+static struct sa11x0_dma_desc *sa11x0_dma_next_desc(struct sa11x0_dma_chan *c)
 {
-	return container_of(tx, struct sa11x0_dma_desc, tx);
+	struct virt_dma_desc *vd = vchan_next_desc(&c->vc);
+
+	return vd ? container_of(vd, struct sa11x0_dma_desc, vd) : NULL;
 }
 
-static struct sa11x0_dma_desc *sa11x0_dma_next_desc(struct sa11x0_dma_chan *c)
+static void sa11x0_dma_free_desc(struct virt_dma_desc *vd)
 {
-	if (list_empty(&c->desc_issued))
-		return NULL;
-
-	return list_first_entry(&c->desc_issued, struct sa11x0_dma_desc, node);
+	kfree(container_of(vd, struct sa11x0_dma_desc, vd));
 }
 
 static void sa11x0_dma_start_desc(struct sa11x0_dma_phy *p, struct sa11x0_dma_desc *txd)
 {
-	list_del(&txd->node);
+	list_del(&txd->vd.node);
 	p->txd_load = txd;
 	p->sg_load = 0;
 
 	dev_vdbg(p->dev->slave.dev, "pchan %u: txd %p[%x]: starting: DDAR:%x\n",
-		p->num, txd, txd->tx.cookie, txd->ddar);
+		p->num, &txd->vd, txd->vd.tx.cookie, txd->ddar);
 }
 
 static void noinline sa11x0_dma_start_sg(struct sa11x0_dma_phy *p,
@@ -183,19 +180,24 @@ static void noinline sa11x0_dma_start_sg(struct sa11x0_dma_phy *p,
 		return;
 
 	if (p->sg_load == txd->sglen) {
-		struct sa11x0_dma_desc *txn = sa11x0_dma_next_desc(c);
+		if (!txd->cyclic) {
+			struct sa11x0_dma_desc *txn = sa11x0_dma_next_desc(c);
 
-		/*
-		 * We have reached the end of the current descriptor.
-		 * Peek at the next descriptor, and if compatible with
-		 * the current, start processing it.
-		 */
-		if (txn && txn->ddar == txd->ddar) {
-			txd = txn;
-			sa11x0_dma_start_desc(p, txn);
+			/*
+			 * We have reached the end of the current descriptor.
+			 * Peek at the next descriptor, and if compatible with
+			 * the current, start processing it.
+			 */
+			if (txn && txn->ddar == txd->ddar) {
+				txd = txn;
+				sa11x0_dma_start_desc(p, txn);
+			} else {
+				p->txd_load = NULL;
+				return;
+			}
 		} else {
-			p->txd_load = NULL;
-			return;
+			/* Cyclic: reset back to beginning */
+			p->sg_load = 0;
 		}
 	}
 
@@ -229,21 +231,21 @@ static void noinline sa11x0_dma_complete(struct sa11x0_dma_phy *p,
 	struct sa11x0_dma_desc *txd = p->txd_done;
 
 	if (++p->sg_done == txd->sglen) {
-		struct sa11x0_dma_dev *d = p->dev;
-
-		dev_vdbg(d->slave.dev, "pchan %u: txd %p[%x]: completed\n",
-			p->num, p->txd_done, p->txd_done->tx.cookie);
-
-		c->lc = txd->tx.cookie;
+		if (!txd->cyclic) {
+			vchan_cookie_complete(&txd->vd);
 
-		spin_lock(&d->lock);
-		list_add_tail(&txd->node, &d->desc_complete);
-		spin_unlock(&d->lock);
+			p->sg_done = 0;
+			p->txd_done = p->txd_load;
 
-		p->sg_done = 0;
-		p->txd_done = p->txd_load;
+			if (!p->txd_done)
+				tasklet_schedule(&p->dev->task);
+		} else {
+			if ((p->sg_done % txd->period) == 0)
+				vchan_cyclic_callback(&txd->vd);
 
-		tasklet_schedule(&d->task);
+			/* Cyclic: reset back to beginning */
+			p->sg_done = 0;
+		}
 	}
 
 	sa11x0_dma_start_sg(p, c);
@@ -280,7 +282,7 @@ static irqreturn_t sa11x0_dma_irq(int irq, void *dev_id)
 	if (c) {
 		unsigned long flags;
 
-		spin_lock_irqsave(&c->lock, flags);
+		spin_lock_irqsave(&c->vc.lock, flags);
 		/*
 		 * Now that we're holding the lock, check that the vchan
 		 * really is associated with this pchan before touching the
@@ -294,7 +296,7 @@ static irqreturn_t sa11x0_dma_irq(int irq, void *dev_id)
 			if (dcsr & DCSR_DONEB)
 				sa11x0_dma_complete(p, c);
 		}
-		spin_unlock_irqrestore(&c->lock, flags);
+		spin_unlock_irqrestore(&c->vc.lock, flags);
 	}
 
 	return IRQ_HANDLED;
@@ -332,28 +334,15 @@ static void sa11x0_dma_tasklet(unsigned long arg)
 	struct sa11x0_dma_dev *d = (struct sa11x0_dma_dev *)arg;
 	struct sa11x0_dma_phy *p;
 	struct sa11x0_dma_chan *c;
-	struct sa11x0_dma_desc *txd, *txn;
-	LIST_HEAD(head);
 	unsigned pch, pch_alloc = 0;
 
 	dev_dbg(d->slave.dev, "tasklet enter\n");
 
-	/* Get the completed tx descriptors */
-	spin_lock_irq(&d->lock);
-	list_splice_init(&d->desc_complete, &head);
-	spin_unlock_irq(&d->lock);
-
-	list_for_each_entry(txd, &head, node) {
-		c = to_sa11x0_dma_chan(txd->tx.chan);
-
-		dev_dbg(d->slave.dev, "vchan %p: txd %p[%x] completed\n",
-			c, txd, txd->tx.cookie);
-
-		spin_lock_irq(&c->lock);
+	list_for_each_entry(c, &d->slave.channels, vc.chan.device_node) {
+		spin_lock_irq(&c->vc.lock);
 		p = c->phy;
-		if (p) {
-			if (!p->txd_done)
-				sa11x0_dma_start_txd(c);
+		if (p && !p->txd_done) {
+			sa11x0_dma_start_txd(c);
 			if (!p->txd_done) {
 				/* No current txd associated with this channel */
 				dev_dbg(d->slave.dev, "pchan %u: free\n", p->num);
@@ -363,7 +352,7 @@ static void sa11x0_dma_tasklet(unsigned long arg)
 				p->vchan = NULL;
 			}
 		}
-		spin_unlock_irq(&c->lock);
+		spin_unlock_irq(&c->vc.lock);
 	}
 
 	spin_lock_irq(&d->lock);
@@ -380,7 +369,7 @@ static void sa11x0_dma_tasklet(unsigned long arg)
 			/* Mark this channel allocated */
 			p->vchan = c;
 
-			dev_dbg(d->slave.dev, "pchan %u: alloc vchan %p\n", pch, c);
+			dev_dbg(d->slave.dev, "pchan %u: alloc vchan %p\n", pch, &c->vc);
 		}
 	}
 	spin_unlock_irq(&d->lock);
@@ -390,42 +379,18 @@ static void sa11x0_dma_tasklet(unsigned long arg)
 			p = &d->phy[pch];
 			c = p->vchan;
 
-			spin_lock_irq(&c->lock);
+			spin_lock_irq(&c->vc.lock);
 			c->phy = p;
 
 			sa11x0_dma_start_txd(c);
-			spin_unlock_irq(&c->lock);
+			spin_unlock_irq(&c->vc.lock);
 		}
 	}
 
-	/* Now free the completed tx descriptor, and call their callbacks */
-	list_for_each_entry_safe(txd, txn, &head, node) {
-		dma_async_tx_callback callback = txd->tx.callback;
-		void *callback_param = txd->tx.callback_param;
-
-		dev_dbg(d->slave.dev, "txd %p[%x]: callback and free\n",
-			txd, txd->tx.cookie);
-
-		kfree(txd);
-
-		if (callback)
-			callback(callback_param);
-	}
-
 	dev_dbg(d->slave.dev, "tasklet exit\n");
 }
 
 
-static void sa11x0_dma_desc_free(struct sa11x0_dma_dev *d, struct list_head *head)
-{
-	struct sa11x0_dma_desc *txd, *txn;
-
-	list_for_each_entry_safe(txd, txn, head, node) {
-		dev_dbg(d->slave.dev, "txd %p: freeing\n", txd);
-		kfree(txd);
-	}
-}
-
 static int sa11x0_dma_alloc_chan_resources(struct dma_chan *chan)
 {
 	return 0;
@@ -436,18 +401,12 @@ static void sa11x0_dma_free_chan_resources(struct dma_chan *chan)
 	struct sa11x0_dma_chan *c = to_sa11x0_dma_chan(chan);
 	struct sa11x0_dma_dev *d = to_sa11x0_dma(chan->device);
 	unsigned long flags;
-	LIST_HEAD(head);
 
-	spin_lock_irqsave(&c->lock, flags);
-	spin_lock(&d->lock);
+	spin_lock_irqsave(&d->lock, flags);
 	list_del_init(&c->node);
-	spin_unlock(&d->lock);
-
-	list_splice_tail_init(&c->desc_submitted, &head);
-	list_splice_tail_init(&c->desc_issued, &head);
-	spin_unlock_irqrestore(&c->lock, flags);
+	spin_unlock_irqrestore(&d->lock, flags);
 
-	sa11x0_dma_desc_free(d, &head);
+	vchan_free_chan_resources(&c->vc);
 }
 
 static dma_addr_t sa11x0_dma_pos(struct sa11x0_dma_phy *p)
@@ -472,33 +431,47 @@ static enum dma_status sa11x0_dma_tx_status(struct dma_chan *chan,
 	struct sa11x0_dma_chan *c = to_sa11x0_dma_chan(chan);
 	struct sa11x0_dma_dev *d = to_sa11x0_dma(chan->device);
 	struct sa11x0_dma_phy *p;
-	struct sa11x0_dma_desc *txd;
-	dma_cookie_t last_used, last_complete;
+	struct virt_dma_desc *vd;
 	unsigned long flags;
 	enum dma_status ret;
-	size_t bytes = 0;
-
-	last_used = c->chan.cookie;
-	last_complete = c->lc;
 
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
-	if (ret == DMA_SUCCESS) {
-		dma_set_tx_state(state, last_complete, last_used, 0);
+	ret = dma_cookie_status(&c->vc.chan, cookie, state);
+	if (ret == DMA_SUCCESS)
 		return ret;
-	}
 
-	spin_lock_irqsave(&c->lock, flags);
+	if (!state)
+		return c->status;
+
+	spin_lock_irqsave(&c->vc.lock, flags);
 	p = c->phy;
-	ret = c->status;
-	if (p) {
-		dma_addr_t addr = sa11x0_dma_pos(p);
 
-		dev_vdbg(d->slave.dev, "tx_status: addr:%x\n", addr);
+	/*
+	 * If the cookie is on our issue queue, then the residue is
+	 * its total size.
+	 */
+	vd = vchan_find_desc(&c->vc, cookie);
+	if (vd) {
+		state->residue = container_of(vd, struct sa11x0_dma_desc, vd)->size;
+	} else if (!p) {
+		state->residue = 0;
+	} else {
+		struct sa11x0_dma_desc *txd;
+		size_t bytes = 0;
 
-		txd = p->txd_done;
+		if (p->txd_done && p->txd_done->vd.tx.cookie == cookie)
+			txd = p->txd_done;
+		else if (p->txd_load && p->txd_load->vd.tx.cookie == cookie)
+			txd = p->txd_load;
+		else
+			txd = NULL;
+
+		ret = c->status;
 		if (txd) {
+			dma_addr_t addr = sa11x0_dma_pos(p);
 			unsigned i;
 
+			dev_vdbg(d->slave.dev, "tx_status: addr:%x\n", addr);
+
 			for (i = 0; i < txd->sglen; i++) {
 				dev_vdbg(d->slave.dev, "tx_status: [%u] %x+%x\n",
 					i, txd->sg[i].addr, txd->sg[i].len);
@@ -521,17 +494,11 @@ static enum dma_status sa11x0_dma_tx_status(struct dma_chan *chan,
 				bytes += txd->sg[i].len;
 			}
 		}
-		if (txd != p->txd_load && p->txd_load)
-			bytes += p->txd_load->size;
-	}
-	list_for_each_entry(txd, &c->desc_issued, node) {
-		bytes += txd->size;
+		state->residue = bytes;
 	}
-	spin_unlock_irqrestore(&c->lock, flags);
-
-	dma_set_tx_state(state, last_complete, last_used, bytes);
+	spin_unlock_irqrestore(&c->vc.lock, flags);
 
-	dev_vdbg(d->slave.dev, "tx_status: bytes 0x%zx\n", bytes);
+	dev_vdbg(d->slave.dev, "tx_status: bytes 0x%zx\n", state->residue);
 
 	return ret;
 }
@@ -547,40 +514,20 @@ static void sa11x0_dma_issue_pending(struct dma_chan *chan)
 	struct sa11x0_dma_dev *d = to_sa11x0_dma(chan->device);
 	unsigned long flags;
 
-	spin_lock_irqsave(&c->lock, flags);
-	list_splice_tail_init(&c->desc_submitted, &c->desc_issued);
-	if (!list_empty(&c->desc_issued)) {
-		spin_lock(&d->lock);
-		if (!c->phy && list_empty(&c->node)) {
-			list_add_tail(&c->node, &d->chan_pending);
-			tasklet_schedule(&d->task);
-			dev_dbg(d->slave.dev, "vchan %p: issued\n", c);
+	spin_lock_irqsave(&c->vc.lock, flags);
+	if (vchan_issue_pending(&c->vc)) {
+		if (!c->phy) {
+			spin_lock(&d->lock);
+			if (list_empty(&c->node)) {
+				list_add_tail(&c->node, &d->chan_pending);
+				tasklet_schedule(&d->task);
+				dev_dbg(d->slave.dev, "vchan %p: issued\n", &c->vc);
+			}
+			spin_unlock(&d->lock);
 		}
-		spin_unlock(&d->lock);
 	} else
-		dev_dbg(d->slave.dev, "vchan %p: nothing to issue\n", c);
-	spin_unlock_irqrestore(&c->lock, flags);
-}
-
-static dma_cookie_t sa11x0_dma_tx_submit(struct dma_async_tx_descriptor *tx)
-{
-	struct sa11x0_dma_chan *c = to_sa11x0_dma_chan(tx->chan);
-	struct sa11x0_dma_desc *txd = to_sa11x0_dma_tx(tx);
-	unsigned long flags;
-
-	spin_lock_irqsave(&c->lock, flags);
-	c->chan.cookie += 1;
-	if (c->chan.cookie < 0)
-		c->chan.cookie = 1;
-	txd->tx.cookie = c->chan.cookie;
-
-	list_add_tail(&txd->node, &c->desc_submitted);
-	spin_unlock_irqrestore(&c->lock, flags);
-
-	dev_dbg(tx->chan->device->dev, "vchan %p: txd %p[%x]: submitted\n",
-		c, txd, txd->tx.cookie);
-
-	return txd->tx.cookie;
+		dev_dbg(d->slave.dev, "vchan %p: nothing to issue\n", &c->vc);
+	spin_unlock_irqrestore(&c->vc.lock, flags);
 }
 
 static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
@@ -596,7 +543,7 @@ static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
 	/* SA11x0 channels can only operate in their native direction */
 	if (dir != (c->ddar & DDAR_RW ? DMA_DEV_TO_MEM : DMA_MEM_TO_DEV)) {
 		dev_err(chan->device->dev, "vchan %p: bad DMA direction: DDAR:%08x dir:%u\n",
-			c, c->ddar, dir);
+			&c->vc, c->ddar, dir);
 		return NULL;
 	}
 
@@ -612,14 +559,14 @@ static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
 			j += DIV_ROUND_UP(len, DMA_MAX_SIZE & ~DMA_ALIGN) - 1;
 		if (addr & DMA_ALIGN) {
 			dev_dbg(chan->device->dev, "vchan %p: bad buffer alignment: %08x\n",
-				c, addr);
+				&c->vc, addr);
 			return NULL;
 		}
 	}
 
 	txd = kzalloc(sizeof(*txd) + j * sizeof(txd->sg[0]), GFP_ATOMIC);
 	if (!txd) {
-		dev_dbg(chan->device->dev, "vchan %p: kzalloc failed\n", c);
+		dev_dbg(chan->device->dev, "vchan %p: kzalloc failed\n", &c->vc);
 		return NULL;
 	}
 
@@ -655,17 +602,73 @@ static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
 		} while (len);
 	}
 
-	dma_async_tx_descriptor_init(&txd->tx, &c->chan);
-	txd->tx.flags = flags;
-	txd->tx.tx_submit = sa11x0_dma_tx_submit;
 	txd->ddar = c->ddar;
 	txd->size = size;
 	txd->sglen = j;
 
 	dev_dbg(chan->device->dev, "vchan %p: txd %p: size %u nr %u\n",
-		c, txd, txd->size, txd->sglen);
+		&c->vc, &txd->vd, txd->size, txd->sglen);
 
-	return &txd->tx;
+	return vchan_tx_prep(&c->vc, &txd->vd, flags);
+}
+
+static struct dma_async_tx_descriptor *sa11x0_dma_prep_dma_cyclic(
+	struct dma_chan *chan, dma_addr_t addr, size_t size, size_t period,
+	enum dma_transfer_direction dir, void *context)
+{
+	struct sa11x0_dma_chan *c = to_sa11x0_dma_chan(chan);
+	struct sa11x0_dma_desc *txd;
+	unsigned i, j, k, sglen, sgperiod;
+
+	/* SA11x0 channels can only operate in their native direction */
+	if (dir != (c->ddar & DDAR_RW ? DMA_DEV_TO_MEM : DMA_MEM_TO_DEV)) {
+		dev_err(chan->device->dev, "vchan %p: bad DMA direction: DDAR:%08x dir:%u\n",
+			&c->vc, c->ddar, dir);
+		return NULL;
+	}
+
+	sgperiod = DIV_ROUND_UP(period, DMA_MAX_SIZE & ~DMA_ALIGN);
+	sglen = size * sgperiod / period;
+
+	/* Do not allow zero-sized txds */
+	if (sglen == 0)
+		return NULL;
+
+	txd = kzalloc(sizeof(*txd) + sglen * sizeof(txd->sg[0]), GFP_ATOMIC);
+	if (!txd) {
+		dev_dbg(chan->device->dev, "vchan %p: kzalloc failed\n", &c->vc);
+		return NULL;
+	}
+
+	for (i = k = 0; i < size / period; i++) {
+		size_t tlen, len = period;
+
+		for (j = 0; j < sgperiod; j++, k++) {
+			tlen = len;
+
+			if (tlen > DMA_MAX_SIZE) {
+				unsigned mult = DIV_ROUND_UP(tlen, DMA_MAX_SIZE & ~DMA_ALIGN);
+				tlen = (tlen / mult) & ~DMA_ALIGN;
+			}
+
+			txd->sg[k].addr = addr;
+			txd->sg[k].len = tlen;
+			addr += tlen;
+			len -= tlen;
+		}
+
+		WARN_ON(len != 0);
+	}
+
+	WARN_ON(k != sglen);
+
+	txd->ddar = c->ddar;
+	txd->size = size;
+	txd->sglen = sglen;
+	txd->cyclic = 1;
+	txd->period = sgperiod;
+
+	return vchan_tx_prep(&c->vc, &txd->vd, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 }
 
 static int sa11x0_dma_slave_config(struct sa11x0_dma_chan *c, struct dma_slave_config *cfg)
@@ -695,8 +698,8 @@ static int sa11x0_dma_slave_config(struct sa11x0_dma_chan *c, struct dma_slave_c
 	if (maxburst == 8)
 		ddar |= DDAR_BS;
 
-	dev_dbg(c->chan.device->dev, "vchan %p: dma_slave_config addr %x width %u burst %u\n",
-		c, addr, width, maxburst);
+	dev_dbg(c->vc.chan.device->dev, "vchan %p: dma_slave_config addr %x width %u burst %u\n",
+		&c->vc, addr, width, maxburst);
 
 	c->ddar = ddar | (addr & 0xf0000000) | (addr & 0x003ffffc) << 6;
 
@@ -718,16 +721,13 @@ static int sa11x0_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		return sa11x0_dma_slave_config(c, (struct dma_slave_config *)arg);
 
 	case DMA_TERMINATE_ALL:
-		dev_dbg(d->slave.dev, "vchan %p: terminate all\n", c);
+		dev_dbg(d->slave.dev, "vchan %p: terminate all\n", &c->vc);
 		/* Clear the tx descriptor lists */
-		spin_lock_irqsave(&c->lock, flags);
-		list_splice_tail_init(&c->desc_submitted, &head);
-		list_splice_tail_init(&c->desc_issued, &head);
+		spin_lock_irqsave(&c->vc.lock, flags);
+		vchan_get_all_descriptors(&c->vc, &head);
 
 		p = c->phy;
 		if (p) {
-			struct sa11x0_dma_desc *txd, *txn;
-
 			dev_dbg(d->slave.dev, "pchan %u: terminating\n", p->num);
 			/* vchan is assigned to a pchan - stop the channel */
 			writel(DCSR_RUN | DCSR_IE |
@@ -735,17 +735,13 @@ static int sa11x0_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 				DCSR_STRTB | DCSR_DONEB,
 				p->base + DMA_DCSR_C);
 
-			list_for_each_entry_safe(txd, txn, &d->desc_complete, node)
-				if (txd->tx.chan == &c->chan)
-					list_move(&txd->node, &head);
-
 			if (p->txd_load) {
 				if (p->txd_load != p->txd_done)
-					list_add_tail(&p->txd_load->node, &head);
+					list_add_tail(&p->txd_load->vd.node, &head);
 				p->txd_load = NULL;
 			}
 			if (p->txd_done) {
-				list_add_tail(&p->txd_done->node, &head);
+				list_add_tail(&p->txd_done->vd.node, &head);
 				p->txd_done = NULL;
 			}
 			c->phy = NULL;
@@ -754,14 +750,14 @@ static int sa11x0_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 			spin_unlock(&d->lock);
 			tasklet_schedule(&d->task);
 		}
-		spin_unlock_irqrestore(&c->lock, flags);
-		sa11x0_dma_desc_free(d, &head);
+		spin_unlock_irqrestore(&c->vc.lock, flags);
+		vchan_dma_desc_free_list(&c->vc, &head);
 		ret = 0;
 		break;
 
 	case DMA_PAUSE:
-		dev_dbg(d->slave.dev, "vchan %p: pause\n", c);
-		spin_lock_irqsave(&c->lock, flags);
+		dev_dbg(d->slave.dev, "vchan %p: pause\n", &c->vc);
+		spin_lock_irqsave(&c->vc.lock, flags);
 		if (c->status == DMA_IN_PROGRESS) {
 			c->status = DMA_PAUSED;
 
@@ -774,26 +770,26 @@ static int sa11x0_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 				spin_unlock(&d->lock);
 			}
 		}
-		spin_unlock_irqrestore(&c->lock, flags);
+		spin_unlock_irqrestore(&c->vc.lock, flags);
 		ret = 0;
 		break;
 
 	case DMA_RESUME:
-		dev_dbg(d->slave.dev, "vchan %p: resume\n", c);
-		spin_lock_irqsave(&c->lock, flags);
+		dev_dbg(d->slave.dev, "vchan %p: resume\n", &c->vc);
+		spin_lock_irqsave(&c->vc.lock, flags);
 		if (c->status == DMA_PAUSED) {
 			c->status = DMA_IN_PROGRESS;
 
 			p = c->phy;
 			if (p) {
 				writel(DCSR_RUN | DCSR_IE, p->base + DMA_DCSR_S);
-			} else if (!list_empty(&c->desc_issued)) {
+			} else if (!list_empty(&c->vc.desc_issued)) {
 				spin_lock(&d->lock);
 				list_add_tail(&c->node, &d->chan_pending);
 				spin_unlock(&d->lock);
 			}
 		}
-		spin_unlock_irqrestore(&c->lock, flags);
+		spin_unlock_irqrestore(&c->vc.lock, flags);
 		ret = 0;
 		break;
 
@@ -853,15 +849,13 @@ static int __devinit sa11x0_dma_init_dmadev(struct dma_device *dmadev,
 			return -ENOMEM;
 		}
 
-		c->chan.device = dmadev;
 		c->status = DMA_IN_PROGRESS;
 		c->ddar = chan_desc[i].ddar;
 		c->name = chan_desc[i].name;
-		spin_lock_init(&c->lock);
-		INIT_LIST_HEAD(&c->desc_submitted);
-		INIT_LIST_HEAD(&c->desc_issued);
 		INIT_LIST_HEAD(&c->node);
-		list_add_tail(&c->chan.device_node, &dmadev->channels);
+
+		c->vc.desc_free = sa11x0_dma_free_desc;
+		vchan_init(&c->vc, dmadev);
 	}
 
 	return dma_async_device_register(dmadev);
@@ -890,8 +884,9 @@ static void sa11x0_dma_free_channels(struct dma_device *dmadev)
 {
 	struct sa11x0_dma_chan *c, *cn;
 
-	list_for_each_entry_safe(c, cn, &dmadev->channels, chan.device_node) {
-		list_del(&c->chan.device_node);
+	list_for_each_entry_safe(c, cn, &dmadev->channels, vc.chan.device_node) {
+		list_del(&c->vc.chan.device_node);
+		tasklet_kill(&c->vc.task);
 		kfree(c);
 	}
 }
@@ -915,7 +910,6 @@ static int __devinit sa11x0_dma_probe(struct platform_device *pdev)
 
 	spin_lock_init(&d->lock);
 	INIT_LIST_HEAD(&d->chan_pending);
-	INIT_LIST_HEAD(&d->desc_complete);
 
 	d->base = ioremap(res->start, resource_size(res));
 	if (!d->base) {
@@ -947,7 +941,9 @@ static int __devinit sa11x0_dma_probe(struct platform_device *pdev)
 	}
 
 	dma_cap_set(DMA_SLAVE, d->slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, d->slave.cap_mask);
 	d->slave.device_prep_slave_sg = sa11x0_dma_prep_slave_sg;
+	d->slave.device_prep_dma_cyclic = sa11x0_dma_prep_dma_cyclic;
 	ret = sa11x0_dma_init_dmadev(&d->slave, &pdev->dev);
 	if (ret) {
 		dev_warn(d->slave.dev, "failed to register slave async device: %d\n",
diff --git a/drivers/dma/sh/Makefile b/drivers/dma/sh/Makefile
new file mode 100644
index 00000000000..54ae9572b0a
--- /dev/null
+++ b/drivers/dma/sh/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_SH_DMAE) += shdma-base.o
+obj-$(CONFIG_SH_DMAE) += shdma.o
diff --git a/drivers/dma/sh/shdma-base.c b/drivers/dma/sh/shdma-base.c
new file mode 100644
index 00000000000..f4cd946d259
--- /dev/null
+++ b/drivers/dma/sh/shdma-base.c
@@ -0,0 +1,943 @@
+/*
+ * Dmaengine driver base library for DMA controllers, found on SH-based SoCs
+ *
+ * extracted from shdma.c
+ *
+ * Copyright (C) 2011-2012 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ * Copyright (C) 2009 Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>
+ * Copyright (C) 2009 Renesas Solutions, Inc. All rights reserved.
+ * Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * This is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/shdma-base.h>
+#include <linux/dmaengine.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "../dmaengine.h"
+
+/* DMA descriptor control */
+enum shdma_desc_status {
+	DESC_IDLE,
+	DESC_PREPARED,
+	DESC_SUBMITTED,
+	DESC_COMPLETED,	/* completed, have to call callback */
+	DESC_WAITING,	/* callback called, waiting for ack / re-submit */
+};
+
+#define NR_DESCS_PER_CHANNEL 32
+
+#define to_shdma_chan(c) container_of(c, struct shdma_chan, dma_chan)
+#define to_shdma_dev(d) container_of(d, struct shdma_dev, dma_dev)
+
+/*
+ * For slave DMA we assume, that there is a finite number of DMA slaves in the
+ * system, and that each such slave can only use a finite number of channels.
+ * We use slave channel IDs to make sure, that no such slave channel ID is
+ * allocated more than once.
+ */
+static unsigned int slave_num = 256;
+module_param(slave_num, uint, 0444);
+
+/* A bitmask with slave_num bits */
+static unsigned long *shdma_slave_used;
+
+/* Called under spin_lock_irq(&schan->chan_lock") */
+static void shdma_chan_xfer_ld_queue(struct shdma_chan *schan)
+{
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	struct shdma_desc *sdesc;
+
+	/* DMA work check */
+	if (ops->channel_busy(schan))
+		return;
+
+	/* Find the first not transferred descriptor */
+	list_for_each_entry(sdesc, &schan->ld_queue, node)
+		if (sdesc->mark == DESC_SUBMITTED) {
+			ops->start_xfer(schan, sdesc);
+			break;
+		}
+}
+
+static dma_cookie_t shdma_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct shdma_desc *chunk, *c, *desc =
+		container_of(tx, struct shdma_desc, async_tx),
+		*last = desc;
+	struct shdma_chan *schan = to_shdma_chan(tx->chan);
+	dma_async_tx_callback callback = tx->callback;
+	dma_cookie_t cookie;
+	bool power_up;
+
+	spin_lock_irq(&schan->chan_lock);
+
+	power_up = list_empty(&schan->ld_queue);
+
+	cookie = dma_cookie_assign(tx);
+
+	/* Mark all chunks of this descriptor as submitted, move to the queue */
+	list_for_each_entry_safe(chunk, c, desc->node.prev, node) {
+		/*
+		 * All chunks are on the global ld_free, so, we have to find
+		 * the end of the chain ourselves
+		 */
+		if (chunk != desc && (chunk->mark == DESC_IDLE ||
+				      chunk->async_tx.cookie > 0 ||
+				      chunk->async_tx.cookie == -EBUSY ||
+				      &chunk->node == &schan->ld_free))
+			break;
+		chunk->mark = DESC_SUBMITTED;
+		/* Callback goes to the last chunk */
+		chunk->async_tx.callback = NULL;
+		chunk->cookie = cookie;
+		list_move_tail(&chunk->node, &schan->ld_queue);
+		last = chunk;
+
+		dev_dbg(schan->dev, "submit #%d@%p on %d\n",
+			tx->cookie, &last->async_tx, schan->id);
+	}
+
+	last->async_tx.callback = callback;
+	last->async_tx.callback_param = tx->callback_param;
+
+	if (power_up) {
+		int ret;
+		schan->pm_state = SHDMA_PM_BUSY;
+
+		ret = pm_runtime_get(schan->dev);
+
+		spin_unlock_irq(&schan->chan_lock);
+		if (ret < 0)
+			dev_err(schan->dev, "%s(): GET = %d\n", __func__, ret);
+
+		pm_runtime_barrier(schan->dev);
+
+		spin_lock_irq(&schan->chan_lock);
+
+		/* Have we been reset, while waiting? */
+		if (schan->pm_state != SHDMA_PM_ESTABLISHED) {
+			struct shdma_dev *sdev =
+				to_shdma_dev(schan->dma_chan.device);
+			const struct shdma_ops *ops = sdev->ops;
+			dev_dbg(schan->dev, "Bring up channel %d\n",
+				schan->id);
+			/*
+			 * TODO: .xfer_setup() might fail on some platforms.
+			 * Make it int then, on error remove chunks from the
+			 * queue again
+			 */
+			ops->setup_xfer(schan, schan->slave_id);
+
+			if (schan->pm_state == SHDMA_PM_PENDING)
+				shdma_chan_xfer_ld_queue(schan);
+			schan->pm_state = SHDMA_PM_ESTABLISHED;
+		}
+	} else {
+		/*
+		 * Tell .device_issue_pending() not to run the queue, interrupts
+		 * will do it anyway
+		 */
+		schan->pm_state = SHDMA_PM_PENDING;
+	}
+
+	spin_unlock_irq(&schan->chan_lock);
+
+	return cookie;
+}
+
+/* Called with desc_lock held */
+static struct shdma_desc *shdma_get_desc(struct shdma_chan *schan)
+{
+	struct shdma_desc *sdesc;
+
+	list_for_each_entry(sdesc, &schan->ld_free, node)
+		if (sdesc->mark != DESC_PREPARED) {
+			BUG_ON(sdesc->mark != DESC_IDLE);
+			list_del(&sdesc->node);
+			return sdesc;
+		}
+
+	return NULL;
+}
+
+static int shdma_setup_slave(struct shdma_chan *schan, int slave_id)
+{
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	int ret;
+
+	if (slave_id < 0 || slave_id >= slave_num)
+		return -EINVAL;
+
+	if (test_and_set_bit(slave_id, shdma_slave_used))
+		return -EBUSY;
+
+	ret = ops->set_slave(schan, slave_id, false);
+	if (ret < 0) {
+		clear_bit(slave_id, shdma_slave_used);
+		return ret;
+	}
+
+	schan->slave_id = slave_id;
+
+	return 0;
+}
+
+/*
+ * This is the standard shdma filter function to be used as a replacement to the
+ * "old" method, using the .private pointer. If for some reason you allocate a
+ * channel without slave data, use something like ERR_PTR(-EINVAL) as a filter
+ * parameter. If this filter is used, the slave driver, after calling
+ * dma_request_channel(), will also have to call dmaengine_slave_config() with
+ * .slave_id, .direction, and either .src_addr or .dst_addr set.
+ * NOTE: this filter doesn't support multiple DMAC drivers with the DMA_SLAVE
+ * capability! If this becomes a requirement, hardware glue drivers, using this
+ * services would have to provide their own filters, which first would check
+ * the device driver, similar to how other DMAC drivers, e.g., sa11x0-dma.c, do
+ * this, and only then, in case of a match, call this common filter.
+ */
+bool shdma_chan_filter(struct dma_chan *chan, void *arg)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	int slave_id = (int)arg;
+	int ret;
+
+	if (slave_id < 0)
+		/* No slave requested - arbitrary channel */
+		return true;
+
+	if (slave_id >= slave_num)
+		return false;
+
+	ret = ops->set_slave(schan, slave_id, true);
+	if (ret < 0)
+		return false;
+
+	return true;
+}
+EXPORT_SYMBOL(shdma_chan_filter);
+
+static int shdma_alloc_chan_resources(struct dma_chan *chan)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	struct shdma_desc *desc;
+	struct shdma_slave *slave = chan->private;
+	int ret, i;
+
+	/*
+	 * This relies on the guarantee from dmaengine that alloc_chan_resources
+	 * never runs concurrently with itself or free_chan_resources.
+	 */
+	if (slave) {
+		/* Legacy mode: .private is set in filter */
+		ret = shdma_setup_slave(schan, slave->slave_id);
+		if (ret < 0)
+			goto esetslave;
+	} else {
+		schan->slave_id = -EINVAL;
+	}
+
+	schan->desc = kcalloc(NR_DESCS_PER_CHANNEL,
+			      sdev->desc_size, GFP_KERNEL);
+	if (!schan->desc) {
+		ret = -ENOMEM;
+		goto edescalloc;
+	}
+	schan->desc_num = NR_DESCS_PER_CHANNEL;
+
+	for (i = 0; i < NR_DESCS_PER_CHANNEL; i++) {
+		desc = ops->embedded_desc(schan->desc, i);
+		dma_async_tx_descriptor_init(&desc->async_tx,
+					     &schan->dma_chan);
+		desc->async_tx.tx_submit = shdma_tx_submit;
+		desc->mark = DESC_IDLE;
+
+		list_add(&desc->node, &schan->ld_free);
+	}
+
+	return NR_DESCS_PER_CHANNEL;
+
+edescalloc:
+	if (slave)
+esetslave:
+		clear_bit(slave->slave_id, shdma_slave_used);
+	chan->private = NULL;
+	return ret;
+}
+
+static dma_async_tx_callback __ld_cleanup(struct shdma_chan *schan, bool all)
+{
+	struct shdma_desc *desc, *_desc;
+	/* Is the "exposed" head of a chain acked? */
+	bool head_acked = false;
+	dma_cookie_t cookie = 0;
+	dma_async_tx_callback callback = NULL;
+	void *param = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&schan->chan_lock, flags);
+	list_for_each_entry_safe(desc, _desc, &schan->ld_queue, node) {
+		struct dma_async_tx_descriptor *tx = &desc->async_tx;
+
+		BUG_ON(tx->cookie > 0 && tx->cookie != desc->cookie);
+		BUG_ON(desc->mark != DESC_SUBMITTED &&
+		       desc->mark != DESC_COMPLETED &&
+		       desc->mark != DESC_WAITING);
+
+		/*
+		 * queue is ordered, and we use this loop to (1) clean up all
+		 * completed descriptors, and to (2) update descriptor flags of
+		 * any chunks in a (partially) completed chain
+		 */
+		if (!all && desc->mark == DESC_SUBMITTED &&
+		    desc->cookie != cookie)
+			break;
+
+		if (tx->cookie > 0)
+			cookie = tx->cookie;
+
+		if (desc->mark == DESC_COMPLETED && desc->chunks == 1) {
+			if (schan->dma_chan.completed_cookie != desc->cookie - 1)
+				dev_dbg(schan->dev,
+					"Completing cookie %d, expected %d\n",
+					desc->cookie,
+					schan->dma_chan.completed_cookie + 1);
+			schan->dma_chan.completed_cookie = desc->cookie;
+		}
+
+		/* Call callback on the last chunk */
+		if (desc->mark == DESC_COMPLETED && tx->callback) {
+			desc->mark = DESC_WAITING;
+			callback = tx->callback;
+			param = tx->callback_param;
+			dev_dbg(schan->dev, "descriptor #%d@%p on %d callback\n",
+				tx->cookie, tx, schan->id);
+			BUG_ON(desc->chunks != 1);
+			break;
+		}
+
+		if (tx->cookie > 0 || tx->cookie == -EBUSY) {
+			if (desc->mark == DESC_COMPLETED) {
+				BUG_ON(tx->cookie < 0);
+				desc->mark = DESC_WAITING;
+			}
+			head_acked = async_tx_test_ack(tx);
+		} else {
+			switch (desc->mark) {
+			case DESC_COMPLETED:
+				desc->mark = DESC_WAITING;
+				/* Fall through */
+			case DESC_WAITING:
+				if (head_acked)
+					async_tx_ack(&desc->async_tx);
+			}
+		}
+
+		dev_dbg(schan->dev, "descriptor %p #%d completed.\n",
+			tx, tx->cookie);
+
+		if (((desc->mark == DESC_COMPLETED ||
+		      desc->mark == DESC_WAITING) &&
+		     async_tx_test_ack(&desc->async_tx)) || all) {
+			/* Remove from ld_queue list */
+			desc->mark = DESC_IDLE;
+
+			list_move(&desc->node, &schan->ld_free);
+
+			if (list_empty(&schan->ld_queue)) {
+				dev_dbg(schan->dev, "Bring down channel %d\n", schan->id);
+				pm_runtime_put(schan->dev);
+				schan->pm_state = SHDMA_PM_ESTABLISHED;
+			}
+		}
+	}
+
+	if (all && !callback)
+		/*
+		 * Terminating and the loop completed normally: forgive
+		 * uncompleted cookies
+		 */
+		schan->dma_chan.completed_cookie = schan->dma_chan.cookie;
+
+	spin_unlock_irqrestore(&schan->chan_lock, flags);
+
+	if (callback)
+		callback(param);
+
+	return callback;
+}
+
+/*
+ * shdma_chan_ld_cleanup - Clean up link descriptors
+ *
+ * Clean up the ld_queue of DMA channel.
+ */
+static void shdma_chan_ld_cleanup(struct shdma_chan *schan, bool all)
+{
+	while (__ld_cleanup(schan, all))
+		;
+}
+
+/*
+ * shdma_free_chan_resources - Free all resources of the channel.
+ */
+static void shdma_free_chan_resources(struct dma_chan *chan)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct shdma_dev *sdev = to_shdma_dev(chan->device);
+	const struct shdma_ops *ops = sdev->ops;
+	LIST_HEAD(list);
+
+	/* Protect against ISR */
+	spin_lock_irq(&schan->chan_lock);
+	ops->halt_channel(schan);
+	spin_unlock_irq(&schan->chan_lock);
+
+	/* Now no new interrupts will occur */
+
+	/* Prepared and not submitted descriptors can still be on the queue */
+	if (!list_empty(&schan->ld_queue))
+		shdma_chan_ld_cleanup(schan, true);
+
+	if (schan->slave_id >= 0) {
+		/* The caller is holding dma_list_mutex */
+		clear_bit(schan->slave_id, shdma_slave_used);
+		chan->private = NULL;
+	}
+
+	spin_lock_irq(&schan->chan_lock);
+
+	list_splice_init(&schan->ld_free, &list);
+	schan->desc_num = 0;
+
+	spin_unlock_irq(&schan->chan_lock);
+
+	kfree(schan->desc);
+}
+
+/**
+ * shdma_add_desc - get, set up and return one transfer descriptor
+ * @schan:	DMA channel
+ * @flags:	DMA transfer flags
+ * @dst:	destination DMA address, incremented when direction equals
+ *		DMA_DEV_TO_MEM or DMA_MEM_TO_MEM
+ * @src:	source DMA address, incremented when direction equals
+ *		DMA_MEM_TO_DEV or DMA_MEM_TO_MEM
+ * @len:	DMA transfer length
+ * @first:	if NULL, set to the current descriptor and cookie set to -EBUSY
+ * @direction:	needed for slave DMA to decide which address to keep constant,
+ *		equals DMA_MEM_TO_MEM for MEMCPY
+ * Returns 0 or an error
+ * Locks: called with desc_lock held
+ */
+static struct shdma_desc *shdma_add_desc(struct shdma_chan *schan,
+	unsigned long flags, dma_addr_t *dst, dma_addr_t *src, size_t *len,
+	struct shdma_desc **first, enum dma_transfer_direction direction)
+{
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	struct shdma_desc *new;
+	size_t copy_size = *len;
+
+	if (!copy_size)
+		return NULL;
+
+	/* Allocate the link descriptor from the free list */
+	new = shdma_get_desc(schan);
+	if (!new) {
+		dev_err(schan->dev, "No free link descriptor available\n");
+		return NULL;
+	}
+
+	ops->desc_setup(schan, new, *src, *dst, &copy_size);
+
+	if (!*first) {
+		/* First desc */
+		new->async_tx.cookie = -EBUSY;
+		*first = new;
+	} else {
+		/* Other desc - invisible to the user */
+		new->async_tx.cookie = -EINVAL;
+	}
+
+	dev_dbg(schan->dev,
+		"chaining (%u/%u)@%x -> %x with %p, cookie %d\n",
+		copy_size, *len, *src, *dst, &new->async_tx,
+		new->async_tx.cookie);
+
+	new->mark = DESC_PREPARED;
+	new->async_tx.flags = flags;
+	new->direction = direction;
+	new->partial = 0;
+
+	*len -= copy_size;
+	if (direction == DMA_MEM_TO_MEM || direction == DMA_MEM_TO_DEV)
+		*src += copy_size;
+	if (direction == DMA_MEM_TO_MEM || direction == DMA_DEV_TO_MEM)
+		*dst += copy_size;
+
+	return new;
+}
+
+/*
+ * shdma_prep_sg - prepare transfer descriptors from an SG list
+ *
+ * Common routine for public (MEMCPY) and slave DMA. The MEMCPY case is also
+ * converted to scatter-gather to guarantee consistent locking and a correct
+ * list manipulation. For slave DMA direction carries the usual meaning, and,
+ * logically, the SG list is RAM and the addr variable contains slave address,
+ * e.g., the FIFO I/O register. For MEMCPY direction equals DMA_MEM_TO_MEM
+ * and the SG list contains only one element and points at the source buffer.
+ */
+static struct dma_async_tx_descriptor *shdma_prep_sg(struct shdma_chan *schan,
+	struct scatterlist *sgl, unsigned int sg_len, dma_addr_t *addr,
+	enum dma_transfer_direction direction, unsigned long flags)
+{
+	struct scatterlist *sg;
+	struct shdma_desc *first = NULL, *new = NULL /* compiler... */;
+	LIST_HEAD(tx_list);
+	int chunks = 0;
+	unsigned long irq_flags;
+	int i;
+
+	for_each_sg(sgl, sg, sg_len, i)
+		chunks += DIV_ROUND_UP(sg_dma_len(sg), schan->max_xfer_len);
+
+	/* Have to lock the whole loop to protect against concurrent release */
+	spin_lock_irqsave(&schan->chan_lock, irq_flags);
+
+	/*
+	 * Chaining:
+	 * first descriptor is what user is dealing with in all API calls, its
+	 *	cookie is at first set to -EBUSY, at tx-submit to a positive
+	 *	number
+	 * if more than one chunk is needed further chunks have cookie = -EINVAL
+	 * the last chunk, if not equal to the first, has cookie = -ENOSPC
+	 * all chunks are linked onto the tx_list head with their .node heads
+	 *	only during this function, then they are immediately spliced
+	 *	back onto the free list in form of a chain
+	 */
+	for_each_sg(sgl, sg, sg_len, i) {
+		dma_addr_t sg_addr = sg_dma_address(sg);
+		size_t len = sg_dma_len(sg);
+
+		if (!len)
+			goto err_get_desc;
+
+		do {
+			dev_dbg(schan->dev, "Add SG #%d@%p[%d], dma %llx\n",
+				i, sg, len, (unsigned long long)sg_addr);
+
+			if (direction == DMA_DEV_TO_MEM)
+				new = shdma_add_desc(schan, flags,
+						&sg_addr, addr, &len, &first,
+						direction);
+			else
+				new = shdma_add_desc(schan, flags,
+						addr, &sg_addr, &len, &first,
+						direction);
+			if (!new)
+				goto err_get_desc;
+
+			new->chunks = chunks--;
+			list_add_tail(&new->node, &tx_list);
+		} while (len);
+	}
+
+	if (new != first)
+		new->async_tx.cookie = -ENOSPC;
+
+	/* Put them back on the free list, so, they don't get lost */
+	list_splice_tail(&tx_list, &schan->ld_free);
+
+	spin_unlock_irqrestore(&schan->chan_lock, irq_flags);
+
+	return &first->async_tx;
+
+err_get_desc:
+	list_for_each_entry(new, &tx_list, node)
+		new->mark = DESC_IDLE;
+	list_splice(&tx_list, &schan->ld_free);
+
+	spin_unlock_irqrestore(&schan->chan_lock, irq_flags);
+
+	return NULL;
+}
+
+static struct dma_async_tx_descriptor *shdma_prep_memcpy(
+	struct dma_chan *chan, dma_addr_t dma_dest, dma_addr_t dma_src,
+	size_t len, unsigned long flags)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct scatterlist sg;
+
+	if (!chan || !len)
+		return NULL;
+
+	BUG_ON(!schan->desc_num);
+
+	sg_init_table(&sg, 1);
+	sg_set_page(&sg, pfn_to_page(PFN_DOWN(dma_src)), len,
+		    offset_in_page(dma_src));
+	sg_dma_address(&sg) = dma_src;
+	sg_dma_len(&sg) = len;
+
+	return shdma_prep_sg(schan, &sg, 1, &dma_dest, DMA_MEM_TO_MEM, flags);
+}
+
+static struct dma_async_tx_descriptor *shdma_prep_slave_sg(
+	struct dma_chan *chan, struct scatterlist *sgl, unsigned int sg_len,
+	enum dma_transfer_direction direction, unsigned long flags, void *context)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct shdma_dev *sdev = to_shdma_dev(schan->dma_chan.device);
+	const struct shdma_ops *ops = sdev->ops;
+	int slave_id = schan->slave_id;
+	dma_addr_t slave_addr;
+
+	if (!chan)
+		return NULL;
+
+	BUG_ON(!schan->desc_num);
+
+	/* Someone calling slave DMA on a generic channel? */
+	if (slave_id < 0 || !sg_len) {
+		dev_warn(schan->dev, "%s: bad parameter: len=%d, id=%d\n",
+			 __func__, sg_len, slave_id);
+		return NULL;
+	}
+
+	slave_addr = ops->slave_addr(schan);
+
+	return shdma_prep_sg(schan, sgl, sg_len, &slave_addr,
+			      direction, flags);
+}
+
+static int shdma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+			  unsigned long arg)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	struct shdma_dev *sdev = to_shdma_dev(chan->device);
+	const struct shdma_ops *ops = sdev->ops;
+	struct dma_slave_config *config;
+	unsigned long flags;
+	int ret;
+
+	if (!chan)
+		return -EINVAL;
+
+	switch (cmd) {
+	case DMA_TERMINATE_ALL:
+		spin_lock_irqsave(&schan->chan_lock, flags);
+		ops->halt_channel(schan);
+
+		if (ops->get_partial && !list_empty(&schan->ld_queue)) {
+			/* Record partial transfer */
+			struct shdma_desc *desc = list_first_entry(&schan->ld_queue,
+						struct shdma_desc, node);
+			desc->partial = ops->get_partial(schan, desc);
+		}
+
+		spin_unlock_irqrestore(&schan->chan_lock, flags);
+
+		shdma_chan_ld_cleanup(schan, true);
+		break;
+	case DMA_SLAVE_CONFIG:
+		/*
+		 * So far only .slave_id is used, but the slave drivers are
+		 * encouraged to also set a transfer direction and an address.
+		 */
+		if (!arg)
+			return -EINVAL;
+		/*
+		 * We could lock this, but you shouldn't be configuring the
+		 * channel, while using it...
+		 */
+		config = (struct dma_slave_config *)arg;
+		ret = shdma_setup_slave(schan, config->slave_id);
+		if (ret < 0)
+			return ret;
+		break;
+	default:
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static void shdma_issue_pending(struct dma_chan *chan)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+
+	spin_lock_irq(&schan->chan_lock);
+	if (schan->pm_state == SHDMA_PM_ESTABLISHED)
+		shdma_chan_xfer_ld_queue(schan);
+	else
+		schan->pm_state = SHDMA_PM_PENDING;
+	spin_unlock_irq(&schan->chan_lock);
+}
+
+static enum dma_status shdma_tx_status(struct dma_chan *chan,
+					dma_cookie_t cookie,
+					struct dma_tx_state *txstate)
+{
+	struct shdma_chan *schan = to_shdma_chan(chan);
+	enum dma_status status;
+	unsigned long flags;
+
+	shdma_chan_ld_cleanup(schan, false);
+
+	spin_lock_irqsave(&schan->chan_lock, flags);
+
+	status = dma_cookie_status(chan, cookie, txstate);
+
+	/*
+	 * If we don't find cookie on the queue, it has been aborted and we have
+	 * to report error
+	 */
+	if (status != DMA_SUCCESS) {
+		struct shdma_desc *sdesc;
+		status = DMA_ERROR;
+		list_for_each_entry(sdesc, &schan->ld_queue, node)
+			if (sdesc->cookie == cookie) {
+				status = DMA_IN_PROGRESS;
+				break;
+			}
+	}
+
+	spin_unlock_irqrestore(&schan->chan_lock, flags);
+
+	return status;
+}
+
+/* Called from error IRQ or NMI */
+bool shdma_reset(struct shdma_dev *sdev)
+{
+	const struct shdma_ops *ops = sdev->ops;
+	struct shdma_chan *schan;
+	unsigned int handled = 0;
+	int i;
+
+	/* Reset all channels */
+	shdma_for_each_chan(schan, sdev, i) {
+		struct shdma_desc *sdesc;
+		LIST_HEAD(dl);
+
+		if (!schan)
+			continue;
+
+		spin_lock(&schan->chan_lock);
+
+		/* Stop the channel */
+		ops->halt_channel(schan);
+
+		list_splice_init(&schan->ld_queue, &dl);
+
+		if (!list_empty(&dl)) {
+			dev_dbg(schan->dev, "Bring down channel %d\n", schan->id);
+			pm_runtime_put(schan->dev);
+		}
+		schan->pm_state = SHDMA_PM_ESTABLISHED;
+
+		spin_unlock(&schan->chan_lock);
+
+		/* Complete all  */
+		list_for_each_entry(sdesc, &dl, node) {
+			struct dma_async_tx_descriptor *tx = &sdesc->async_tx;
+			sdesc->mark = DESC_IDLE;
+			if (tx->callback)
+				tx->callback(tx->callback_param);
+		}
+
+		spin_lock(&schan->chan_lock);
+		list_splice(&dl, &schan->ld_free);
+		spin_unlock(&schan->chan_lock);
+
+		handled++;
+	}
+
+	return !!handled;
+}
+EXPORT_SYMBOL(shdma_reset);
+
+static irqreturn_t chan_irq(int irq, void *dev)
+{
+	struct shdma_chan *schan = dev;
+	const struct shdma_ops *ops =
+		to_shdma_dev(schan->dma_chan.device)->ops;
+	irqreturn_t ret;
+
+	spin_lock(&schan->chan_lock);
+
+	ret = ops->chan_irq(schan, irq) ? IRQ_WAKE_THREAD : IRQ_NONE;
+
+	spin_unlock(&schan->chan_lock);
+
+	return ret;
+}
+
+static irqreturn_t chan_irqt(int irq, void *dev)
+{
+	struct shdma_chan *schan = dev;
+	const struct shdma_ops *ops =
+		to_shdma_dev(schan->dma_chan.device)->ops;
+	struct shdma_desc *sdesc;
+
+	spin_lock_irq(&schan->chan_lock);
+	list_for_each_entry(sdesc, &schan->ld_queue, node) {
+		if (sdesc->mark == DESC_SUBMITTED &&
+		    ops->desc_completed(schan, sdesc)) {
+			dev_dbg(schan->dev, "done #%d@%p\n",
+				sdesc->async_tx.cookie, &sdesc->async_tx);
+			sdesc->mark = DESC_COMPLETED;
+			break;
+		}
+	}
+	/* Next desc */
+	shdma_chan_xfer_ld_queue(schan);
+	spin_unlock_irq(&schan->chan_lock);
+
+	shdma_chan_ld_cleanup(schan, false);
+
+	return IRQ_HANDLED;
+}
+
+int shdma_request_irq(struct shdma_chan *schan, int irq,
+			   unsigned long flags, const char *name)
+{
+	int ret = request_threaded_irq(irq, chan_irq, chan_irqt,
+				       flags, name, schan);
+
+	schan->irq = ret < 0 ? ret : irq;
+
+	return ret;
+}
+EXPORT_SYMBOL(shdma_request_irq);
+
+void shdma_free_irq(struct shdma_chan *schan)
+{
+	if (schan->irq >= 0)
+		free_irq(schan->irq, schan);
+}
+EXPORT_SYMBOL(shdma_free_irq);
+
+void shdma_chan_probe(struct shdma_dev *sdev,
+			   struct shdma_chan *schan, int id)
+{
+	schan->pm_state = SHDMA_PM_ESTABLISHED;
+
+	/* reference struct dma_device */
+	schan->dma_chan.device = &sdev->dma_dev;
+	dma_cookie_init(&schan->dma_chan);
+
+	schan->dev = sdev->dma_dev.dev;
+	schan->id = id;
+
+	if (!schan->max_xfer_len)
+		schan->max_xfer_len = PAGE_SIZE;
+
+	spin_lock_init(&schan->chan_lock);
+
+	/* Init descripter manage list */
+	INIT_LIST_HEAD(&schan->ld_queue);
+	INIT_LIST_HEAD(&schan->ld_free);
+
+	/* Add the channel to DMA device channel list */
+	list_add_tail(&schan->dma_chan.device_node,
+			&sdev->dma_dev.channels);
+	sdev->schan[sdev->dma_dev.chancnt++] = schan;
+}
+EXPORT_SYMBOL(shdma_chan_probe);
+
+void shdma_chan_remove(struct shdma_chan *schan)
+{
+	list_del(&schan->dma_chan.device_node);
+}
+EXPORT_SYMBOL(shdma_chan_remove);
+
+int shdma_init(struct device *dev, struct shdma_dev *sdev,
+		    int chan_num)
+{
+	struct dma_device *dma_dev = &sdev->dma_dev;
+
+	/*
+	 * Require all call-backs for now, they can trivially be made optional
+	 * later as required
+	 */
+	if (!sdev->ops ||
+	    !sdev->desc_size ||
+	    !sdev->ops->embedded_desc ||
+	    !sdev->ops->start_xfer ||
+	    !sdev->ops->setup_xfer ||
+	    !sdev->ops->set_slave ||
+	    !sdev->ops->desc_setup ||
+	    !sdev->ops->slave_addr ||
+	    !sdev->ops->channel_busy ||
+	    !sdev->ops->halt_channel ||
+	    !sdev->ops->desc_completed)
+		return -EINVAL;
+
+	sdev->schan = kcalloc(chan_num, sizeof(*sdev->schan), GFP_KERNEL);
+	if (!sdev->schan)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&dma_dev->channels);
+
+	/* Common and MEMCPY operations */
+	dma_dev->device_alloc_chan_resources
+		= shdma_alloc_chan_resources;
+	dma_dev->device_free_chan_resources = shdma_free_chan_resources;
+	dma_dev->device_prep_dma_memcpy = shdma_prep_memcpy;
+	dma_dev->device_tx_status = shdma_tx_status;
+	dma_dev->device_issue_pending = shdma_issue_pending;
+
+	/* Compulsory for DMA_SLAVE fields */
+	dma_dev->device_prep_slave_sg = shdma_prep_slave_sg;
+	dma_dev->device_control = shdma_control;
+
+	dma_dev->dev = dev;
+
+	return 0;
+}
+EXPORT_SYMBOL(shdma_init);
+
+void shdma_cleanup(struct shdma_dev *sdev)
+{
+	kfree(sdev->schan);
+}
+EXPORT_SYMBOL(shdma_cleanup);
+
+static int __init shdma_enter(void)
+{
+	shdma_slave_used = kzalloc(DIV_ROUND_UP(slave_num, BITS_PER_LONG) *
+				    sizeof(long), GFP_KERNEL);
+	if (!shdma_slave_used)
+		return -ENOMEM;
+	return 0;
+}
+module_init(shdma_enter);
+
+static void __exit shdma_exit(void)
+{
+	kfree(shdma_slave_used);
+}
+module_exit(shdma_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("SH-DMA driver base library");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
diff --git a/drivers/dma/sh/shdma.c b/drivers/dma/sh/shdma.c
new file mode 100644
index 00000000000..f41bcc5267f
--- /dev/null
+++ b/drivers/dma/sh/shdma.c
@@ -0,0 +1,955 @@
+/*
+ * Renesas SuperH DMA Engine support
+ *
+ * base is drivers/dma/flsdma.c
+ *
+ * Copyright (C) 2011-2012 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ * Copyright (C) 2009 Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>
+ * Copyright (C) 2009 Renesas Solutions, Inc. All rights reserved.
+ * Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * This is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * - DMA of SuperH does not have Hardware DMA chain mode.
+ * - MAX DMA size is 16MB.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/dmaengine.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/sh_dma.h>
+#include <linux/notifier.h>
+#include <linux/kdebug.h>
+#include <linux/spinlock.h>
+#include <linux/rculist.h>
+
+#include "../dmaengine.h"
+#include "shdma.h"
+
+#define SH_DMAE_DRV_NAME "sh-dma-engine"
+
+/* Default MEMCPY transfer size = 2^2 = 4 bytes */
+#define LOG2_DEFAULT_XFER_SIZE	2
+#define SH_DMA_SLAVE_NUMBER 256
+#define SH_DMA_TCR_MAX (16 * 1024 * 1024 - 1)
+
+/*
+ * Used for write-side mutual exclusion for the global device list,
+ * read-side synchronization by way of RCU, and per-controller data.
+ */
+static DEFINE_SPINLOCK(sh_dmae_lock);
+static LIST_HEAD(sh_dmae_devices);
+
+static void chclr_write(struct sh_dmae_chan *sh_dc, u32 data)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_dc);
+
+	__raw_writel(data, shdev->chan_reg +
+		     shdev->pdata->channel[sh_dc->shdma_chan.id].chclr_offset);
+}
+
+static void sh_dmae_writel(struct sh_dmae_chan *sh_dc, u32 data, u32 reg)
+{
+	__raw_writel(data, sh_dc->base + reg / sizeof(u32));
+}
+
+static u32 sh_dmae_readl(struct sh_dmae_chan *sh_dc, u32 reg)
+{
+	return __raw_readl(sh_dc->base + reg / sizeof(u32));
+}
+
+static u16 dmaor_read(struct sh_dmae_device *shdev)
+{
+	u32 __iomem *addr = shdev->chan_reg + DMAOR / sizeof(u32);
+
+	if (shdev->pdata->dmaor_is_32bit)
+		return __raw_readl(addr);
+	else
+		return __raw_readw(addr);
+}
+
+static void dmaor_write(struct sh_dmae_device *shdev, u16 data)
+{
+	u32 __iomem *addr = shdev->chan_reg + DMAOR / sizeof(u32);
+
+	if (shdev->pdata->dmaor_is_32bit)
+		__raw_writel(data, addr);
+	else
+		__raw_writew(data, addr);
+}
+
+static void chcr_write(struct sh_dmae_chan *sh_dc, u32 data)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_dc);
+
+	__raw_writel(data, sh_dc->base + shdev->chcr_offset / sizeof(u32));
+}
+
+static u32 chcr_read(struct sh_dmae_chan *sh_dc)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_dc);
+
+	return __raw_readl(sh_dc->base + shdev->chcr_offset / sizeof(u32));
+}
+
+/*
+ * Reset DMA controller
+ *
+ * SH7780 has two DMAOR register
+ */
+static void sh_dmae_ctl_stop(struct sh_dmae_device *shdev)
+{
+	unsigned short dmaor;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sh_dmae_lock, flags);
+
+	dmaor = dmaor_read(shdev);
+	dmaor_write(shdev, dmaor & ~(DMAOR_NMIF | DMAOR_AE | DMAOR_DME));
+
+	spin_unlock_irqrestore(&sh_dmae_lock, flags);
+}
+
+static int sh_dmae_rst(struct sh_dmae_device *shdev)
+{
+	unsigned short dmaor;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sh_dmae_lock, flags);
+
+	dmaor = dmaor_read(shdev) & ~(DMAOR_NMIF | DMAOR_AE | DMAOR_DME);
+
+	if (shdev->pdata->chclr_present) {
+		int i;
+		for (i = 0; i < shdev->pdata->channel_num; i++) {
+			struct sh_dmae_chan *sh_chan = shdev->chan[i];
+			if (sh_chan)
+				chclr_write(sh_chan, 0);
+		}
+	}
+
+	dmaor_write(shdev, dmaor | shdev->pdata->dmaor_init);
+
+	dmaor = dmaor_read(shdev);
+
+	spin_unlock_irqrestore(&sh_dmae_lock, flags);
+
+	if (dmaor & (DMAOR_AE | DMAOR_NMIF)) {
+		dev_warn(shdev->shdma_dev.dma_dev.dev, "Can't initialize DMAOR.\n");
+		return -EIO;
+	}
+	if (shdev->pdata->dmaor_init & ~dmaor)
+		dev_warn(shdev->shdma_dev.dma_dev.dev,
+			 "DMAOR=0x%x hasn't latched the initial value 0x%x.\n",
+			 dmaor, shdev->pdata->dmaor_init);
+	return 0;
+}
+
+static bool dmae_is_busy(struct sh_dmae_chan *sh_chan)
+{
+	u32 chcr = chcr_read(sh_chan);
+
+	if ((chcr & (CHCR_DE | CHCR_TE)) == CHCR_DE)
+		return true; /* working */
+
+	return false; /* waiting */
+}
+
+static unsigned int calc_xmit_shift(struct sh_dmae_chan *sh_chan, u32 chcr)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
+	struct sh_dmae_pdata *pdata = shdev->pdata;
+	int cnt = ((chcr & pdata->ts_low_mask) >> pdata->ts_low_shift) |
+		((chcr & pdata->ts_high_mask) >> pdata->ts_high_shift);
+
+	if (cnt >= pdata->ts_shift_num)
+		cnt = 0;
+
+	return pdata->ts_shift[cnt];
+}
+
+static u32 log2size_to_chcr(struct sh_dmae_chan *sh_chan, int l2size)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
+	struct sh_dmae_pdata *pdata = shdev->pdata;
+	int i;
+
+	for (i = 0; i < pdata->ts_shift_num; i++)
+		if (pdata->ts_shift[i] == l2size)
+			break;
+
+	if (i == pdata->ts_shift_num)
+		i = 0;
+
+	return ((i << pdata->ts_low_shift) & pdata->ts_low_mask) |
+		((i << pdata->ts_high_shift) & pdata->ts_high_mask);
+}
+
+static void dmae_set_reg(struct sh_dmae_chan *sh_chan, struct sh_dmae_regs *hw)
+{
+	sh_dmae_writel(sh_chan, hw->sar, SAR);
+	sh_dmae_writel(sh_chan, hw->dar, DAR);
+	sh_dmae_writel(sh_chan, hw->tcr >> sh_chan->xmit_shift, TCR);
+}
+
+static void dmae_start(struct sh_dmae_chan *sh_chan)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
+	u32 chcr = chcr_read(sh_chan);
+
+	if (shdev->pdata->needs_tend_set)
+		sh_dmae_writel(sh_chan, 0xFFFFFFFF, TEND);
+
+	chcr |= CHCR_DE | shdev->chcr_ie_bit;
+	chcr_write(sh_chan, chcr & ~CHCR_TE);
+}
+
+static void dmae_init(struct sh_dmae_chan *sh_chan)
+{
+	/*
+	 * Default configuration for dual address memory-memory transfer.
+	 * 0x400 represents auto-request.
+	 */
+	u32 chcr = DM_INC | SM_INC | 0x400 | log2size_to_chcr(sh_chan,
+						   LOG2_DEFAULT_XFER_SIZE);
+	sh_chan->xmit_shift = calc_xmit_shift(sh_chan, chcr);
+	chcr_write(sh_chan, chcr);
+}
+
+static int dmae_set_chcr(struct sh_dmae_chan *sh_chan, u32 val)
+{
+	/* If DMA is active, cannot set CHCR. TODO: remove this superfluous check */
+	if (dmae_is_busy(sh_chan))
+		return -EBUSY;
+
+	sh_chan->xmit_shift = calc_xmit_shift(sh_chan, val);
+	chcr_write(sh_chan, val);
+
+	return 0;
+}
+
+static int dmae_set_dmars(struct sh_dmae_chan *sh_chan, u16 val)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
+	struct sh_dmae_pdata *pdata = shdev->pdata;
+	const struct sh_dmae_channel *chan_pdata = &pdata->channel[sh_chan->shdma_chan.id];
+	u16 __iomem *addr = shdev->dmars;
+	unsigned int shift = chan_pdata->dmars_bit;
+
+	if (dmae_is_busy(sh_chan))
+		return -EBUSY;
+
+	if (pdata->no_dmars)
+		return 0;
+
+	/* in the case of a missing DMARS resource use first memory window */
+	if (!addr)
+		addr = (u16 __iomem *)shdev->chan_reg;
+	addr += chan_pdata->dmars / sizeof(u16);
+
+	__raw_writew((__raw_readw(addr) & (0xff00 >> shift)) | (val << shift),
+		     addr);
+
+	return 0;
+}
+
+static void sh_dmae_start_xfer(struct shdma_chan *schan,
+			       struct shdma_desc *sdesc)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+	struct sh_dmae_desc *sh_desc = container_of(sdesc,
+					struct sh_dmae_desc, shdma_desc);
+	dev_dbg(sh_chan->shdma_chan.dev, "Queue #%d to %d: %u@%x -> %x\n",
+		sdesc->async_tx.cookie, sh_chan->shdma_chan.id,
+		sh_desc->hw.tcr, sh_desc->hw.sar, sh_desc->hw.dar);
+	/* Get the ld start address from ld_queue */
+	dmae_set_reg(sh_chan, &sh_desc->hw);
+	dmae_start(sh_chan);
+}
+
+static bool sh_dmae_channel_busy(struct shdma_chan *schan)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+	return dmae_is_busy(sh_chan);
+}
+
+static void sh_dmae_setup_xfer(struct shdma_chan *schan,
+			       int slave_id)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+
+	if (slave_id >= 0) {
+		const struct sh_dmae_slave_config *cfg =
+			sh_chan->config;
+
+		dmae_set_dmars(sh_chan, cfg->mid_rid);
+		dmae_set_chcr(sh_chan, cfg->chcr);
+	} else {
+		dmae_init(sh_chan);
+	}
+}
+
+static const struct sh_dmae_slave_config *dmae_find_slave(
+	struct sh_dmae_chan *sh_chan, int slave_id)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
+	struct sh_dmae_pdata *pdata = shdev->pdata;
+	const struct sh_dmae_slave_config *cfg;
+	int i;
+
+	if (slave_id >= SH_DMA_SLAVE_NUMBER)
+		return NULL;
+
+	for (i = 0, cfg = pdata->slave; i < pdata->slave_num; i++, cfg++)
+		if (cfg->slave_id == slave_id)
+			return cfg;
+
+	return NULL;
+}
+
+static int sh_dmae_set_slave(struct shdma_chan *schan,
+			     int slave_id, bool try)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+	const struct sh_dmae_slave_config *cfg = dmae_find_slave(sh_chan, slave_id);
+	if (!cfg)
+		return -ENODEV;
+
+	if (!try)
+		sh_chan->config = cfg;
+
+	return 0;
+}
+
+static void dmae_halt(struct sh_dmae_chan *sh_chan)
+{
+	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
+	u32 chcr = chcr_read(sh_chan);
+
+	chcr &= ~(CHCR_DE | CHCR_TE | shdev->chcr_ie_bit);
+	chcr_write(sh_chan, chcr);
+}
+
+static int sh_dmae_desc_setup(struct shdma_chan *schan,
+			      struct shdma_desc *sdesc,
+			      dma_addr_t src, dma_addr_t dst, size_t *len)
+{
+	struct sh_dmae_desc *sh_desc = container_of(sdesc,
+					struct sh_dmae_desc, shdma_desc);
+
+	if (*len > schan->max_xfer_len)
+		*len = schan->max_xfer_len;
+
+	sh_desc->hw.sar = src;
+	sh_desc->hw.dar = dst;
+	sh_desc->hw.tcr = *len;
+
+	return 0;
+}
+
+static void sh_dmae_halt(struct shdma_chan *schan)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+	dmae_halt(sh_chan);
+}
+
+static bool sh_dmae_chan_irq(struct shdma_chan *schan, int irq)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+
+	if (!(chcr_read(sh_chan) & CHCR_TE))
+		return false;
+
+	/* DMA stop */
+	dmae_halt(sh_chan);
+
+	return true;
+}
+
+static size_t sh_dmae_get_partial(struct shdma_chan *schan,
+				  struct shdma_desc *sdesc)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan, struct sh_dmae_chan,
+						    shdma_chan);
+	struct sh_dmae_desc *sh_desc = container_of(sdesc,
+					struct sh_dmae_desc, shdma_desc);
+	return (sh_desc->hw.tcr - sh_dmae_readl(sh_chan, TCR)) <<
+		sh_chan->xmit_shift;
+}
+
+/* Called from error IRQ or NMI */
+static bool sh_dmae_reset(struct sh_dmae_device *shdev)
+{
+	bool ret;
+
+	/* halt the dma controller */
+	sh_dmae_ctl_stop(shdev);
+
+	/* We cannot detect, which channel caused the error, have to reset all */
+	ret = shdma_reset(&shdev->shdma_dev);
+
+	sh_dmae_rst(shdev);
+
+	return ret;
+}
+
+static irqreturn_t sh_dmae_err(int irq, void *data)
+{
+	struct sh_dmae_device *shdev = data;
+
+	if (!(dmaor_read(shdev) & DMAOR_AE))
+		return IRQ_NONE;
+
+	sh_dmae_reset(shdev);
+	return IRQ_HANDLED;
+}
+
+static bool sh_dmae_desc_completed(struct shdma_chan *schan,
+				   struct shdma_desc *sdesc)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan,
+					struct sh_dmae_chan, shdma_chan);
+	struct sh_dmae_desc *sh_desc = container_of(sdesc,
+					struct sh_dmae_desc, shdma_desc);
+	u32 sar_buf = sh_dmae_readl(sh_chan, SAR);
+	u32 dar_buf = sh_dmae_readl(sh_chan, DAR);
+
+	return	(sdesc->direction == DMA_DEV_TO_MEM &&
+		 (sh_desc->hw.dar + sh_desc->hw.tcr) == dar_buf) ||
+		(sdesc->direction != DMA_DEV_TO_MEM &&
+		 (sh_desc->hw.sar + sh_desc->hw.tcr) == sar_buf);
+}
+
+static bool sh_dmae_nmi_notify(struct sh_dmae_device *shdev)
+{
+	/* Fast path out if NMIF is not asserted for this controller */
+	if ((dmaor_read(shdev) & DMAOR_NMIF) == 0)
+		return false;
+
+	return sh_dmae_reset(shdev);
+}
+
+static int sh_dmae_nmi_handler(struct notifier_block *self,
+			       unsigned long cmd, void *data)
+{
+	struct sh_dmae_device *shdev;
+	int ret = NOTIFY_DONE;
+	bool triggered;
+
+	/*
+	 * Only concern ourselves with NMI events.
+	 *
+	 * Normally we would check the die chain value, but as this needs
+	 * to be architecture independent, check for NMI context instead.
+	 */
+	if (!in_nmi())
+		return NOTIFY_DONE;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(shdev, &sh_dmae_devices, node) {
+		/*
+		 * Only stop if one of the controllers has NMIF asserted,
+		 * we do not want to interfere with regular address error
+		 * handling or NMI events that don't concern the DMACs.
+		 */
+		triggered = sh_dmae_nmi_notify(shdev);
+		if (triggered == true)
+			ret = NOTIFY_OK;
+	}
+	rcu_read_unlock();
+
+	return ret;
+}
+
+static struct notifier_block sh_dmae_nmi_notifier __read_mostly = {
+	.notifier_call	= sh_dmae_nmi_handler,
+
+	/* Run before NMI debug handler and KGDB */
+	.priority	= 1,
+};
+
+static int __devinit sh_dmae_chan_probe(struct sh_dmae_device *shdev, int id,
+					int irq, unsigned long flags)
+{
+	const struct sh_dmae_channel *chan_pdata = &shdev->pdata->channel[id];
+	struct shdma_dev *sdev = &shdev->shdma_dev;
+	struct platform_device *pdev = to_platform_device(sdev->dma_dev.dev);
+	struct sh_dmae_chan *sh_chan;
+	struct shdma_chan *schan;
+	int err;
+
+	sh_chan = kzalloc(sizeof(struct sh_dmae_chan), GFP_KERNEL);
+	if (!sh_chan) {
+		dev_err(sdev->dma_dev.dev,
+			"No free memory for allocating dma channels!\n");
+		return -ENOMEM;
+	}
+
+	schan = &sh_chan->shdma_chan;
+	schan->max_xfer_len = SH_DMA_TCR_MAX + 1;
+
+	shdma_chan_probe(sdev, schan, id);
+
+	sh_chan->base = shdev->chan_reg + chan_pdata->offset / sizeof(u32);
+
+	/* set up channel irq */
+	if (pdev->id >= 0)
+		snprintf(sh_chan->dev_id, sizeof(sh_chan->dev_id),
+			 "sh-dmae%d.%d", pdev->id, id);
+	else
+		snprintf(sh_chan->dev_id, sizeof(sh_chan->dev_id),
+			 "sh-dma%d", id);
+
+	err = shdma_request_irq(schan, irq, flags, sh_chan->dev_id);
+	if (err) {
+		dev_err(sdev->dma_dev.dev,
+			"DMA channel %d request_irq error %d\n",
+			id, err);
+		goto err_no_irq;
+	}
+
+	shdev->chan[id] = sh_chan;
+	return 0;
+
+err_no_irq:
+	/* remove from dmaengine device node */
+	shdma_chan_remove(schan);
+	kfree(sh_chan);
+	return err;
+}
+
+static void sh_dmae_chan_remove(struct sh_dmae_device *shdev)
+{
+	struct dma_device *dma_dev = &shdev->shdma_dev.dma_dev;
+	struct shdma_chan *schan;
+	int i;
+
+	shdma_for_each_chan(schan, &shdev->shdma_dev, i) {
+		struct sh_dmae_chan *sh_chan = container_of(schan,
+					struct sh_dmae_chan, shdma_chan);
+		BUG_ON(!schan);
+
+		shdma_free_irq(&sh_chan->shdma_chan);
+
+		shdma_chan_remove(schan);
+		kfree(sh_chan);
+	}
+	dma_dev->chancnt = 0;
+}
+
+static void sh_dmae_shutdown(struct platform_device *pdev)
+{
+	struct sh_dmae_device *shdev = platform_get_drvdata(pdev);
+	sh_dmae_ctl_stop(shdev);
+}
+
+static int sh_dmae_runtime_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int sh_dmae_runtime_resume(struct device *dev)
+{
+	struct sh_dmae_device *shdev = dev_get_drvdata(dev);
+
+	return sh_dmae_rst(shdev);
+}
+
+#ifdef CONFIG_PM
+static int sh_dmae_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int sh_dmae_resume(struct device *dev)
+{
+	struct sh_dmae_device *shdev = dev_get_drvdata(dev);
+	int i, ret;
+
+	ret = sh_dmae_rst(shdev);
+	if (ret < 0)
+		dev_err(dev, "Failed to reset!\n");
+
+	for (i = 0; i < shdev->pdata->channel_num; i++) {
+		struct sh_dmae_chan *sh_chan = shdev->chan[i];
+
+		if (!sh_chan->shdma_chan.desc_num)
+			continue;
+
+		if (sh_chan->shdma_chan.slave_id >= 0) {
+			const struct sh_dmae_slave_config *cfg = sh_chan->config;
+			dmae_set_dmars(sh_chan, cfg->mid_rid);
+			dmae_set_chcr(sh_chan, cfg->chcr);
+		} else {
+			dmae_init(sh_chan);
+		}
+	}
+
+	return 0;
+}
+#else
+#define sh_dmae_suspend NULL
+#define sh_dmae_resume NULL
+#endif
+
+const struct dev_pm_ops sh_dmae_pm = {
+	.suspend		= sh_dmae_suspend,
+	.resume			= sh_dmae_resume,
+	.runtime_suspend	= sh_dmae_runtime_suspend,
+	.runtime_resume		= sh_dmae_runtime_resume,
+};
+
+static dma_addr_t sh_dmae_slave_addr(struct shdma_chan *schan)
+{
+	struct sh_dmae_chan *sh_chan = container_of(schan,
+					struct sh_dmae_chan, shdma_chan);
+
+	/*
+	 * Implicit BUG_ON(!sh_chan->config)
+	 * This is an exclusive slave DMA operation, may only be called after a
+	 * successful slave configuration.
+	 */
+	return sh_chan->config->addr;
+}
+
+static struct shdma_desc *sh_dmae_embedded_desc(void *buf, int i)
+{
+	return &((struct sh_dmae_desc *)buf)[i].shdma_desc;
+}
+
+static const struct shdma_ops sh_dmae_shdma_ops = {
+	.desc_completed = sh_dmae_desc_completed,
+	.halt_channel = sh_dmae_halt,
+	.channel_busy = sh_dmae_channel_busy,
+	.slave_addr = sh_dmae_slave_addr,
+	.desc_setup = sh_dmae_desc_setup,
+	.set_slave = sh_dmae_set_slave,
+	.setup_xfer = sh_dmae_setup_xfer,
+	.start_xfer = sh_dmae_start_xfer,
+	.embedded_desc = sh_dmae_embedded_desc,
+	.chan_irq = sh_dmae_chan_irq,
+	.get_partial = sh_dmae_get_partial,
+};
+
+static int __devinit sh_dmae_probe(struct platform_device *pdev)
+{
+	struct sh_dmae_pdata *pdata = pdev->dev.platform_data;
+	unsigned long irqflags = IRQF_DISABLED,
+		chan_flag[SH_DMAE_MAX_CHANNELS] = {};
+	int errirq, chan_irq[SH_DMAE_MAX_CHANNELS];
+	int err, i, irq_cnt = 0, irqres = 0, irq_cap = 0;
+	struct sh_dmae_device *shdev;
+	struct dma_device *dma_dev;
+	struct resource *chan, *dmars, *errirq_res, *chanirq_res;
+
+	/* get platform data */
+	if (!pdata || !pdata->channel_num)
+		return -ENODEV;
+
+	chan = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	/* DMARS area is optional */
+	dmars = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	/*
+	 * IRQ resources:
+	 * 1. there always must be at least one IRQ IO-resource. On SH4 it is
+	 *    the error IRQ, in which case it is the only IRQ in this resource:
+	 *    start == end. If it is the only IRQ resource, all channels also
+	 *    use the same IRQ.
+	 * 2. DMA channel IRQ resources can be specified one per resource or in
+	 *    ranges (start != end)
+	 * 3. iff all events (channels and, optionally, error) on this
+	 *    controller use the same IRQ, only one IRQ resource can be
+	 *    specified, otherwise there must be one IRQ per channel, even if
+	 *    some of them are equal
+	 * 4. if all IRQs on this controller are equal or if some specific IRQs
+	 *    specify IORESOURCE_IRQ_SHAREABLE in their resources, they will be
+	 *    requested with the IRQF_SHARED flag
+	 */
+	errirq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!chan || !errirq_res)
+		return -ENODEV;
+
+	if (!request_mem_region(chan->start, resource_size(chan), pdev->name)) {
+		dev_err(&pdev->dev, "DMAC register region already claimed\n");
+		return -EBUSY;
+	}
+
+	if (dmars && !request_mem_region(dmars->start, resource_size(dmars), pdev->name)) {
+		dev_err(&pdev->dev, "DMAC DMARS region already claimed\n");
+		err = -EBUSY;
+		goto ermrdmars;
+	}
+
+	err = -ENOMEM;
+	shdev = kzalloc(sizeof(struct sh_dmae_device), GFP_KERNEL);
+	if (!shdev) {
+		dev_err(&pdev->dev, "Not enough memory\n");
+		goto ealloc;
+	}
+
+	dma_dev = &shdev->shdma_dev.dma_dev;
+
+	shdev->chan_reg = ioremap(chan->start, resource_size(chan));
+	if (!shdev->chan_reg)
+		goto emapchan;
+	if (dmars) {
+		shdev->dmars = ioremap(dmars->start, resource_size(dmars));
+		if (!shdev->dmars)
+			goto emapdmars;
+	}
+
+	if (!pdata->slave_only)
+		dma_cap_set(DMA_MEMCPY, dma_dev->cap_mask);
+	if (pdata->slave && pdata->slave_num)
+		dma_cap_set(DMA_SLAVE, dma_dev->cap_mask);
+
+	/* Default transfer size of 32 bytes requires 32-byte alignment */
+	dma_dev->copy_align = LOG2_DEFAULT_XFER_SIZE;
+
+	shdev->shdma_dev.ops = &sh_dmae_shdma_ops;
+	shdev->shdma_dev.desc_size = sizeof(struct sh_dmae_desc);
+	err = shdma_init(&pdev->dev, &shdev->shdma_dev,
+			      pdata->channel_num);
+	if (err < 0)
+		goto eshdma;
+
+	/* platform data */
+	shdev->pdata = pdev->dev.platform_data;
+
+	if (pdata->chcr_offset)
+		shdev->chcr_offset = pdata->chcr_offset;
+	else
+		shdev->chcr_offset = CHCR;
+
+	if (pdata->chcr_ie_bit)
+		shdev->chcr_ie_bit = pdata->chcr_ie_bit;
+	else
+		shdev->chcr_ie_bit = CHCR_IE;
+
+	platform_set_drvdata(pdev, shdev);
+
+	pm_runtime_enable(&pdev->dev);
+	err = pm_runtime_get_sync(&pdev->dev);
+	if (err < 0)
+		dev_err(&pdev->dev, "%s(): GET = %d\n", __func__, err);
+
+	spin_lock_irq(&sh_dmae_lock);
+	list_add_tail_rcu(&shdev->node, &sh_dmae_devices);
+	spin_unlock_irq(&sh_dmae_lock);
+
+	/* reset dma controller - only needed as a test */
+	err = sh_dmae_rst(shdev);
+	if (err)
+		goto rst_err;
+
+#if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
+	chanirq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+
+	if (!chanirq_res)
+		chanirq_res = errirq_res;
+	else
+		irqres++;
+
+	if (chanirq_res == errirq_res ||
+	    (errirq_res->flags & IORESOURCE_BITS) == IORESOURCE_IRQ_SHAREABLE)
+		irqflags = IRQF_SHARED;
+
+	errirq = errirq_res->start;
+
+	err = request_irq(errirq, sh_dmae_err, irqflags,
+			  "DMAC Address Error", shdev);
+	if (err) {
+		dev_err(&pdev->dev,
+			"DMA failed requesting irq #%d, error %d\n",
+			errirq, err);
+		goto eirq_err;
+	}
+
+#else
+	chanirq_res = errirq_res;
+#endif /* CONFIG_CPU_SH4 || CONFIG_ARCH_SHMOBILE */
+
+	if (chanirq_res->start == chanirq_res->end &&
+	    !platform_get_resource(pdev, IORESOURCE_IRQ, 1)) {
+		/* Special case - all multiplexed */
+		for (; irq_cnt < pdata->channel_num; irq_cnt++) {
+			if (irq_cnt < SH_DMAE_MAX_CHANNELS) {
+				chan_irq[irq_cnt] = chanirq_res->start;
+				chan_flag[irq_cnt] = IRQF_SHARED;
+			} else {
+				irq_cap = 1;
+				break;
+			}
+		}
+	} else {
+		do {
+			for (i = chanirq_res->start; i <= chanirq_res->end; i++) {
+				if (irq_cnt >= SH_DMAE_MAX_CHANNELS) {
+					irq_cap = 1;
+					break;
+				}
+
+				if ((errirq_res->flags & IORESOURCE_BITS) ==
+				    IORESOURCE_IRQ_SHAREABLE)
+					chan_flag[irq_cnt] = IRQF_SHARED;
+				else
+					chan_flag[irq_cnt] = IRQF_DISABLED;
+				dev_dbg(&pdev->dev,
+					"Found IRQ %d for channel %d\n",
+					i, irq_cnt);
+				chan_irq[irq_cnt++] = i;
+			}
+
+			if (irq_cnt >= SH_DMAE_MAX_CHANNELS)
+				break;
+
+			chanirq_res = platform_get_resource(pdev,
+						IORESOURCE_IRQ, ++irqres);
+		} while (irq_cnt < pdata->channel_num && chanirq_res);
+	}
+
+	/* Create DMA Channel */
+	for (i = 0; i < irq_cnt; i++) {
+		err = sh_dmae_chan_probe(shdev, i, chan_irq[i], chan_flag[i]);
+		if (err)
+			goto chan_probe_err;
+	}
+
+	if (irq_cap)
+		dev_notice(&pdev->dev, "Attempting to register %d DMA "
+			   "channels when a maximum of %d are supported.\n",
+			   pdata->channel_num, SH_DMAE_MAX_CHANNELS);
+
+	pm_runtime_put(&pdev->dev);
+
+	err = dma_async_device_register(&shdev->shdma_dev.dma_dev);
+	if (err < 0)
+		goto edmadevreg;
+
+	return err;
+
+edmadevreg:
+	pm_runtime_get(&pdev->dev);
+
+chan_probe_err:
+	sh_dmae_chan_remove(shdev);
+
+#if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
+	free_irq(errirq, shdev);
+eirq_err:
+#endif
+rst_err:
+	spin_lock_irq(&sh_dmae_lock);
+	list_del_rcu(&shdev->node);
+	spin_unlock_irq(&sh_dmae_lock);
+
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	platform_set_drvdata(pdev, NULL);
+	shdma_cleanup(&shdev->shdma_dev);
+eshdma:
+	if (dmars)
+		iounmap(shdev->dmars);
+emapdmars:
+	iounmap(shdev->chan_reg);
+	synchronize_rcu();
+emapchan:
+	kfree(shdev);
+ealloc:
+	if (dmars)
+		release_mem_region(dmars->start, resource_size(dmars));
+ermrdmars:
+	release_mem_region(chan->start, resource_size(chan));
+
+	return err;
+}
+
+static int __devexit sh_dmae_remove(struct platform_device *pdev)
+{
+	struct sh_dmae_device *shdev = platform_get_drvdata(pdev);
+	struct dma_device *dma_dev = &shdev->shdma_dev.dma_dev;
+	struct resource *res;
+	int errirq = platform_get_irq(pdev, 0);
+
+	dma_async_device_unregister(dma_dev);
+
+	if (errirq > 0)
+		free_irq(errirq, shdev);
+
+	spin_lock_irq(&sh_dmae_lock);
+	list_del_rcu(&shdev->node);
+	spin_unlock_irq(&sh_dmae_lock);
+
+	pm_runtime_disable(&pdev->dev);
+
+	sh_dmae_chan_remove(shdev);
+	shdma_cleanup(&shdev->shdma_dev);
+
+	if (shdev->dmars)
+		iounmap(shdev->dmars);
+	iounmap(shdev->chan_reg);
+
+	platform_set_drvdata(pdev, NULL);
+
+	synchronize_rcu();
+	kfree(shdev);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	return 0;
+}
+
+static struct platform_driver sh_dmae_driver = {
+	.driver 	= {
+		.owner	= THIS_MODULE,
+		.pm	= &sh_dmae_pm,
+		.name	= SH_DMAE_DRV_NAME,
+	},
+	.remove		= __devexit_p(sh_dmae_remove),
+	.shutdown	= sh_dmae_shutdown,
+};
+
+static int __init sh_dmae_init(void)
+{
+	/* Wire up NMI handling */
+	int err = register_die_notifier(&sh_dmae_nmi_notifier);
+	if (err)
+		return err;
+
+	return platform_driver_probe(&sh_dmae_driver, sh_dmae_probe);
+}
+module_init(sh_dmae_init);
+
+static void __exit sh_dmae_exit(void)
+{
+	platform_driver_unregister(&sh_dmae_driver);
+
+	unregister_die_notifier(&sh_dmae_nmi_notifier);
+}
+module_exit(sh_dmae_exit);
+
+MODULE_AUTHOR("Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>");
+MODULE_DESCRIPTION("Renesas SH DMA Engine driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" SH_DMAE_DRV_NAME);
diff --git a/drivers/dma/shdma.h b/drivers/dma/sh/shdma.h
index 0b1d2c105f0..9314e93225d 100644
--- a/drivers/dma/shdma.h
+++ b/drivers/dma/sh/shdma.h
@@ -13,42 +13,29 @@
 #ifndef __DMA_SHDMA_H
 #define __DMA_SHDMA_H
 
+#include <linux/sh_dma.h>
+#include <linux/shdma-base.h>
 #include <linux/dmaengine.h>
 #include <linux/interrupt.h>
 #include <linux/list.h>
 
-#define SH_DMAC_MAX_CHANNELS 20
-#define SH_DMA_SLAVE_NUMBER 256
-#define SH_DMA_TCR_MAX 0x00FFFFFF	/* 16MB */
+#define SH_DMAE_MAX_CHANNELS 20
+#define SH_DMAE_TCR_MAX 0x00FFFFFF	/* 16MB */
 
 struct device;
 
-enum dmae_pm_state {
-	DMAE_PM_ESTABLISHED,
-	DMAE_PM_BUSY,
-	DMAE_PM_PENDING,
-};
-
 struct sh_dmae_chan {
-	spinlock_t desc_lock;		/* Descriptor operation lock */
-	struct list_head ld_queue;	/* Link descriptors queue */
-	struct list_head ld_free;	/* Link descriptors free */
-	struct dma_chan common;		/* DMA common channel */
-	struct device *dev;		/* Channel device */
-	struct tasklet_struct tasklet;	/* Tasklet */
-	int descs_allocated;		/* desc count */
+	struct shdma_chan shdma_chan;
+	const struct sh_dmae_slave_config *config; /* Slave DMA configuration */
 	int xmit_shift;			/* log_2(bytes_per_xfer) */
-	int irq;
-	int id;				/* Raw id of this channel */
 	u32 __iomem *base;
 	char dev_id[16];		/* unique name per DMAC of channel */
 	int pm_error;
-	enum dmae_pm_state pm_state;
 };
 
 struct sh_dmae_device {
-	struct dma_device common;
-	struct sh_dmae_chan *chan[SH_DMAC_MAX_CHANNELS];
+	struct shdma_dev shdma_dev;
+	struct sh_dmae_chan *chan[SH_DMAE_MAX_CHANNELS];
 	struct sh_dmae_pdata *pdata;
 	struct list_head node;
 	u32 __iomem *chan_reg;
@@ -57,10 +44,21 @@ struct sh_dmae_device {
 	u32 chcr_ie_bit;
 };
 
-#define to_sh_chan(chan) container_of(chan, struct sh_dmae_chan, common)
+struct sh_dmae_regs {
+	u32 sar; /* SAR / source address */
+	u32 dar; /* DAR / destination address */
+	u32 tcr; /* TCR / transfer count */
+};
+
+struct sh_dmae_desc {
+	struct sh_dmae_regs hw;
+	struct shdma_desc shdma_desc;
+};
+
+#define to_sh_chan(chan) container_of(chan, struct sh_dmae_chan, shdma_chan)
 #define to_sh_desc(lh) container_of(lh, struct sh_desc, node)
 #define tx_to_sh_desc(tx) container_of(tx, struct sh_desc, async_tx)
-#define to_sh_dev(chan) container_of(chan->common.device,\
-				     struct sh_dmae_device, common)
+#define to_sh_dev(chan) container_of(chan->shdma_chan.dma_chan.device,\
+				     struct sh_dmae_device, shdma_dev.dma_dev)
 
 #endif	/* __DMA_SHDMA_H */
diff --git a/drivers/dma/shdma.c b/drivers/dma/shdma.c
deleted file mode 100644
index 19d7a8d3975..00000000000
--- a/drivers/dma/shdma.c
+++ /dev/null
@@ -1,1524 +0,0 @@
-/*
- * Renesas SuperH DMA Engine support
- *
- * base is drivers/dma/flsdma.c
- *
- * Copyright (C) 2009 Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>
- * Copyright (C) 2009 Renesas Solutions, Inc. All rights reserved.
- * Copyright (C) 2007 Freescale Semiconductor, Inc. All rights reserved.
- *
- * This is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * - DMA of SuperH does not have Hardware DMA chain mode.
- * - MAX DMA size is 16MB.
- *
- */
-
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/dmaengine.h>
-#include <linux/delay.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/sh_dma.h>
-#include <linux/notifier.h>
-#include <linux/kdebug.h>
-#include <linux/spinlock.h>
-#include <linux/rculist.h>
-
-#include "dmaengine.h"
-#include "shdma.h"
-
-/* DMA descriptor control */
-enum sh_dmae_desc_status {
-	DESC_IDLE,
-	DESC_PREPARED,
-	DESC_SUBMITTED,
-	DESC_COMPLETED,	/* completed, have to call callback */
-	DESC_WAITING,	/* callback called, waiting for ack / re-submit */
-};
-
-#define NR_DESCS_PER_CHANNEL 32
-/* Default MEMCPY transfer size = 2^2 = 4 bytes */
-#define LOG2_DEFAULT_XFER_SIZE	2
-
-/*
- * Used for write-side mutual exclusion for the global device list,
- * read-side synchronization by way of RCU, and per-controller data.
- */
-static DEFINE_SPINLOCK(sh_dmae_lock);
-static LIST_HEAD(sh_dmae_devices);
-
-/* A bitmask with bits enough for enum sh_dmae_slave_chan_id */
-static unsigned long sh_dmae_slave_used[BITS_TO_LONGS(SH_DMA_SLAVE_NUMBER)];
-
-static void sh_dmae_chan_ld_cleanup(struct sh_dmae_chan *sh_chan, bool all);
-static void sh_chan_xfer_ld_queue(struct sh_dmae_chan *sh_chan);
-
-static void chclr_write(struct sh_dmae_chan *sh_dc, u32 data)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_dc);
-
-	__raw_writel(data, shdev->chan_reg +
-		     shdev->pdata->channel[sh_dc->id].chclr_offset);
-}
-
-static void sh_dmae_writel(struct sh_dmae_chan *sh_dc, u32 data, u32 reg)
-{
-	__raw_writel(data, sh_dc->base + reg / sizeof(u32));
-}
-
-static u32 sh_dmae_readl(struct sh_dmae_chan *sh_dc, u32 reg)
-{
-	return __raw_readl(sh_dc->base + reg / sizeof(u32));
-}
-
-static u16 dmaor_read(struct sh_dmae_device *shdev)
-{
-	u32 __iomem *addr = shdev->chan_reg + DMAOR / sizeof(u32);
-
-	if (shdev->pdata->dmaor_is_32bit)
-		return __raw_readl(addr);
-	else
-		return __raw_readw(addr);
-}
-
-static void dmaor_write(struct sh_dmae_device *shdev, u16 data)
-{
-	u32 __iomem *addr = shdev->chan_reg + DMAOR / sizeof(u32);
-
-	if (shdev->pdata->dmaor_is_32bit)
-		__raw_writel(data, addr);
-	else
-		__raw_writew(data, addr);
-}
-
-static void chcr_write(struct sh_dmae_chan *sh_dc, u32 data)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_dc);
-
-	__raw_writel(data, sh_dc->base + shdev->chcr_offset / sizeof(u32));
-}
-
-static u32 chcr_read(struct sh_dmae_chan *sh_dc)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_dc);
-
-	return __raw_readl(sh_dc->base + shdev->chcr_offset / sizeof(u32));
-}
-
-/*
- * Reset DMA controller
- *
- * SH7780 has two DMAOR register
- */
-static void sh_dmae_ctl_stop(struct sh_dmae_device *shdev)
-{
-	unsigned short dmaor;
-	unsigned long flags;
-
-	spin_lock_irqsave(&sh_dmae_lock, flags);
-
-	dmaor = dmaor_read(shdev);
-	dmaor_write(shdev, dmaor & ~(DMAOR_NMIF | DMAOR_AE | DMAOR_DME));
-
-	spin_unlock_irqrestore(&sh_dmae_lock, flags);
-}
-
-static int sh_dmae_rst(struct sh_dmae_device *shdev)
-{
-	unsigned short dmaor;
-	unsigned long flags;
-
-	spin_lock_irqsave(&sh_dmae_lock, flags);
-
-	dmaor = dmaor_read(shdev) & ~(DMAOR_NMIF | DMAOR_AE | DMAOR_DME);
-
-	if (shdev->pdata->chclr_present) {
-		int i;
-		for (i = 0; i < shdev->pdata->channel_num; i++) {
-			struct sh_dmae_chan *sh_chan = shdev->chan[i];
-			if (sh_chan)
-				chclr_write(sh_chan, 0);
-		}
-	}
-
-	dmaor_write(shdev, dmaor | shdev->pdata->dmaor_init);
-
-	dmaor = dmaor_read(shdev);
-
-	spin_unlock_irqrestore(&sh_dmae_lock, flags);
-
-	if (dmaor & (DMAOR_AE | DMAOR_NMIF)) {
-		dev_warn(shdev->common.dev, "Can't initialize DMAOR.\n");
-		return -EIO;
-	}
-	if (shdev->pdata->dmaor_init & ~dmaor)
-		dev_warn(shdev->common.dev,
-			 "DMAOR=0x%x hasn't latched the initial value 0x%x.\n",
-			 dmaor, shdev->pdata->dmaor_init);
-	return 0;
-}
-
-static bool dmae_is_busy(struct sh_dmae_chan *sh_chan)
-{
-	u32 chcr = chcr_read(sh_chan);
-
-	if ((chcr & (CHCR_DE | CHCR_TE)) == CHCR_DE)
-		return true; /* working */
-
-	return false; /* waiting */
-}
-
-static unsigned int calc_xmit_shift(struct sh_dmae_chan *sh_chan, u32 chcr)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
-	struct sh_dmae_pdata *pdata = shdev->pdata;
-	int cnt = ((chcr & pdata->ts_low_mask) >> pdata->ts_low_shift) |
-		((chcr & pdata->ts_high_mask) >> pdata->ts_high_shift);
-
-	if (cnt >= pdata->ts_shift_num)
-		cnt = 0;
-
-	return pdata->ts_shift[cnt];
-}
-
-static u32 log2size_to_chcr(struct sh_dmae_chan *sh_chan, int l2size)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
-	struct sh_dmae_pdata *pdata = shdev->pdata;
-	int i;
-
-	for (i = 0; i < pdata->ts_shift_num; i++)
-		if (pdata->ts_shift[i] == l2size)
-			break;
-
-	if (i == pdata->ts_shift_num)
-		i = 0;
-
-	return ((i << pdata->ts_low_shift) & pdata->ts_low_mask) |
-		((i << pdata->ts_high_shift) & pdata->ts_high_mask);
-}
-
-static void dmae_set_reg(struct sh_dmae_chan *sh_chan, struct sh_dmae_regs *hw)
-{
-	sh_dmae_writel(sh_chan, hw->sar, SAR);
-	sh_dmae_writel(sh_chan, hw->dar, DAR);
-	sh_dmae_writel(sh_chan, hw->tcr >> sh_chan->xmit_shift, TCR);
-}
-
-static void dmae_start(struct sh_dmae_chan *sh_chan)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
-	u32 chcr = chcr_read(sh_chan);
-
-	if (shdev->pdata->needs_tend_set)
-		sh_dmae_writel(sh_chan, 0xFFFFFFFF, TEND);
-
-	chcr |= CHCR_DE | shdev->chcr_ie_bit;
-	chcr_write(sh_chan, chcr & ~CHCR_TE);
-}
-
-static void dmae_halt(struct sh_dmae_chan *sh_chan)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
-	u32 chcr = chcr_read(sh_chan);
-
-	chcr &= ~(CHCR_DE | CHCR_TE | shdev->chcr_ie_bit);
-	chcr_write(sh_chan, chcr);
-}
-
-static void dmae_init(struct sh_dmae_chan *sh_chan)
-{
-	/*
-	 * Default configuration for dual address memory-memory transfer.
-	 * 0x400 represents auto-request.
-	 */
-	u32 chcr = DM_INC | SM_INC | 0x400 | log2size_to_chcr(sh_chan,
-						   LOG2_DEFAULT_XFER_SIZE);
-	sh_chan->xmit_shift = calc_xmit_shift(sh_chan, chcr);
-	chcr_write(sh_chan, chcr);
-}
-
-static int dmae_set_chcr(struct sh_dmae_chan *sh_chan, u32 val)
-{
-	/* If DMA is active, cannot set CHCR. TODO: remove this superfluous check */
-	if (dmae_is_busy(sh_chan))
-		return -EBUSY;
-
-	sh_chan->xmit_shift = calc_xmit_shift(sh_chan, val);
-	chcr_write(sh_chan, val);
-
-	return 0;
-}
-
-static int dmae_set_dmars(struct sh_dmae_chan *sh_chan, u16 val)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
-	struct sh_dmae_pdata *pdata = shdev->pdata;
-	const struct sh_dmae_channel *chan_pdata = &pdata->channel[sh_chan->id];
-	u16 __iomem *addr = shdev->dmars;
-	unsigned int shift = chan_pdata->dmars_bit;
-
-	if (dmae_is_busy(sh_chan))
-		return -EBUSY;
-
-	if (pdata->no_dmars)
-		return 0;
-
-	/* in the case of a missing DMARS resource use first memory window */
-	if (!addr)
-		addr = (u16 __iomem *)shdev->chan_reg;
-	addr += chan_pdata->dmars / sizeof(u16);
-
-	__raw_writew((__raw_readw(addr) & (0xff00 >> shift)) | (val << shift),
-		     addr);
-
-	return 0;
-}
-
-static dma_cookie_t sh_dmae_tx_submit(struct dma_async_tx_descriptor *tx)
-{
-	struct sh_desc *desc = tx_to_sh_desc(tx), *chunk, *last = desc, *c;
-	struct sh_dmae_chan *sh_chan = to_sh_chan(tx->chan);
-	struct sh_dmae_slave *param = tx->chan->private;
-	dma_async_tx_callback callback = tx->callback;
-	dma_cookie_t cookie;
-	bool power_up;
-
-	spin_lock_irq(&sh_chan->desc_lock);
-
-	if (list_empty(&sh_chan->ld_queue))
-		power_up = true;
-	else
-		power_up = false;
-
-	cookie = dma_cookie_assign(tx);
-
-	/* Mark all chunks of this descriptor as submitted, move to the queue */
-	list_for_each_entry_safe(chunk, c, desc->node.prev, node) {
-		/*
-		 * All chunks are on the global ld_free, so, we have to find
-		 * the end of the chain ourselves
-		 */
-		if (chunk != desc && (chunk->mark == DESC_IDLE ||
-				      chunk->async_tx.cookie > 0 ||
-				      chunk->async_tx.cookie == -EBUSY ||
-				      &chunk->node == &sh_chan->ld_free))
-			break;
-		chunk->mark = DESC_SUBMITTED;
-		/* Callback goes to the last chunk */
-		chunk->async_tx.callback = NULL;
-		chunk->cookie = cookie;
-		list_move_tail(&chunk->node, &sh_chan->ld_queue);
-		last = chunk;
-	}
-
-	last->async_tx.callback = callback;
-	last->async_tx.callback_param = tx->callback_param;
-
-	dev_dbg(sh_chan->dev, "submit #%d@%p on %d: %x[%d] -> %x\n",
-		tx->cookie, &last->async_tx, sh_chan->id,
-		desc->hw.sar, desc->hw.tcr, desc->hw.dar);
-
-	if (power_up) {
-		sh_chan->pm_state = DMAE_PM_BUSY;
-
-		pm_runtime_get(sh_chan->dev);
-
-		spin_unlock_irq(&sh_chan->desc_lock);
-
-		pm_runtime_barrier(sh_chan->dev);
-
-		spin_lock_irq(&sh_chan->desc_lock);
-
-		/* Have we been reset, while waiting? */
-		if (sh_chan->pm_state != DMAE_PM_ESTABLISHED) {
-			dev_dbg(sh_chan->dev, "Bring up channel %d\n",
-				sh_chan->id);
-			if (param) {
-				const struct sh_dmae_slave_config *cfg =
-					param->config;
-
-				dmae_set_dmars(sh_chan, cfg->mid_rid);
-				dmae_set_chcr(sh_chan, cfg->chcr);
-			} else {
-				dmae_init(sh_chan);
-			}
-
-			if (sh_chan->pm_state == DMAE_PM_PENDING)
-				sh_chan_xfer_ld_queue(sh_chan);
-			sh_chan->pm_state = DMAE_PM_ESTABLISHED;
-		}
-	} else {
-		sh_chan->pm_state = DMAE_PM_PENDING;
-	}
-
-	spin_unlock_irq(&sh_chan->desc_lock);
-
-	return cookie;
-}
-
-/* Called with desc_lock held */
-static struct sh_desc *sh_dmae_get_desc(struct sh_dmae_chan *sh_chan)
-{
-	struct sh_desc *desc;
-
-	list_for_each_entry(desc, &sh_chan->ld_free, node)
-		if (desc->mark != DESC_PREPARED) {
-			BUG_ON(desc->mark != DESC_IDLE);
-			list_del(&desc->node);
-			return desc;
-		}
-
-	return NULL;
-}
-
-static const struct sh_dmae_slave_config *sh_dmae_find_slave(
-	struct sh_dmae_chan *sh_chan, struct sh_dmae_slave *param)
-{
-	struct sh_dmae_device *shdev = to_sh_dev(sh_chan);
-	struct sh_dmae_pdata *pdata = shdev->pdata;
-	int i;
-
-	if (param->slave_id >= SH_DMA_SLAVE_NUMBER)
-		return NULL;
-
-	for (i = 0; i < pdata->slave_num; i++)
-		if (pdata->slave[i].slave_id == param->slave_id)
-			return pdata->slave + i;
-
-	return NULL;
-}
-
-static int sh_dmae_alloc_chan_resources(struct dma_chan *chan)
-{
-	struct sh_dmae_chan *sh_chan = to_sh_chan(chan);
-	struct sh_desc *desc;
-	struct sh_dmae_slave *param = chan->private;
-	int ret;
-
-	/*
-	 * This relies on the guarantee from dmaengine that alloc_chan_resources
-	 * never runs concurrently with itself or free_chan_resources.
-	 */
-	if (param) {
-		const struct sh_dmae_slave_config *cfg;
-
-		cfg = sh_dmae_find_slave(sh_chan, param);
-		if (!cfg) {
-			ret = -EINVAL;
-			goto efindslave;
-		}
-
-		if (test_and_set_bit(param->slave_id, sh_dmae_slave_used)) {
-			ret = -EBUSY;
-			goto etestused;
-		}
-
-		param->config = cfg;
-	}
-
-	while (sh_chan->descs_allocated < NR_DESCS_PER_CHANNEL) {
-		desc = kzalloc(sizeof(struct sh_desc), GFP_KERNEL);
-		if (!desc)
-			break;
-		dma_async_tx_descriptor_init(&desc->async_tx,
-					&sh_chan->common);
-		desc->async_tx.tx_submit = sh_dmae_tx_submit;
-		desc->mark = DESC_IDLE;
-
-		list_add(&desc->node, &sh_chan->ld_free);
-		sh_chan->descs_allocated++;
-	}
-
-	if (!sh_chan->descs_allocated) {
-		ret = -ENOMEM;
-		goto edescalloc;
-	}
-
-	return sh_chan->descs_allocated;
-
-edescalloc:
-	if (param)
-		clear_bit(param->slave_id, sh_dmae_slave_used);
-etestused:
-efindslave:
-	chan->private = NULL;
-	return ret;
-}
-
-/*
- * sh_dma_free_chan_resources - Free all resources of the channel.
- */
-static void sh_dmae_free_chan_resources(struct dma_chan *chan)
-{
-	struct sh_dmae_chan *sh_chan = to_sh_chan(chan);
-	struct sh_desc *desc, *_desc;
-	LIST_HEAD(list);
-
-	/* Protect against ISR */
-	spin_lock_irq(&sh_chan->desc_lock);
-	dmae_halt(sh_chan);
-	spin_unlock_irq(&sh_chan->desc_lock);
-
-	/* Now no new interrupts will occur */
-
-	/* Prepared and not submitted descriptors can still be on the queue */
-	if (!list_empty(&sh_chan->ld_queue))
-		sh_dmae_chan_ld_cleanup(sh_chan, true);
-
-	if (chan->private) {
-		/* The caller is holding dma_list_mutex */
-		struct sh_dmae_slave *param = chan->private;
-		clear_bit(param->slave_id, sh_dmae_slave_used);
-		chan->private = NULL;
-	}
-
-	spin_lock_irq(&sh_chan->desc_lock);
-
-	list_splice_init(&sh_chan->ld_free, &list);
-	sh_chan->descs_allocated = 0;
-
-	spin_unlock_irq(&sh_chan->desc_lock);
-
-	list_for_each_entry_safe(desc, _desc, &list, node)
-		kfree(desc);
-}
-
-/**
- * sh_dmae_add_desc - get, set up and return one transfer descriptor
- * @sh_chan:	DMA channel
- * @flags:	DMA transfer flags
- * @dest:	destination DMA address, incremented when direction equals
- *		DMA_DEV_TO_MEM
- * @src:	source DMA address, incremented when direction equals
- *		DMA_MEM_TO_DEV
- * @len:	DMA transfer length
- * @first:	if NULL, set to the current descriptor and cookie set to -EBUSY
- * @direction:	needed for slave DMA to decide which address to keep constant,
- *		equals DMA_MEM_TO_MEM for MEMCPY
- * Returns 0 or an error
- * Locks: called with desc_lock held
- */
-static struct sh_desc *sh_dmae_add_desc(struct sh_dmae_chan *sh_chan,
-	unsigned long flags, dma_addr_t *dest, dma_addr_t *src, size_t *len,
-	struct sh_desc **first, enum dma_transfer_direction direction)
-{
-	struct sh_desc *new;
-	size_t copy_size;
-
-	if (!*len)
-		return NULL;
-
-	/* Allocate the link descriptor from the free list */
-	new = sh_dmae_get_desc(sh_chan);
-	if (!new) {
-		dev_err(sh_chan->dev, "No free link descriptor available\n");
-		return NULL;
-	}
-
-	copy_size = min(*len, (size_t)SH_DMA_TCR_MAX + 1);
-
-	new->hw.sar = *src;
-	new->hw.dar = *dest;
-	new->hw.tcr = copy_size;
-
-	if (!*first) {
-		/* First desc */
-		new->async_tx.cookie = -EBUSY;
-		*first = new;
-	} else {
-		/* Other desc - invisible to the user */
-		new->async_tx.cookie = -EINVAL;
-	}
-
-	dev_dbg(sh_chan->dev,
-		"chaining (%u/%u)@%x -> %x with %p, cookie %d, shift %d\n",
-		copy_size, *len, *src, *dest, &new->async_tx,
-		new->async_tx.cookie, sh_chan->xmit_shift);
-
-	new->mark = DESC_PREPARED;
-	new->async_tx.flags = flags;
-	new->direction = direction;
-
-	*len -= copy_size;
-	if (direction == DMA_MEM_TO_MEM || direction == DMA_MEM_TO_DEV)
-		*src += copy_size;
-	if (direction == DMA_MEM_TO_MEM || direction == DMA_DEV_TO_MEM)
-		*dest += copy_size;
-
-	return new;
-}
-
-/*
- * sh_dmae_prep_sg - prepare transfer descriptors from an SG list
- *
- * Common routine for public (MEMCPY) and slave DMA. The MEMCPY case is also
- * converted to scatter-gather to guarantee consistent locking and a correct
- * list manipulation. For slave DMA direction carries the usual meaning, and,
- * logically, the SG list is RAM and the addr variable contains slave address,
- * e.g., the FIFO I/O register. For MEMCPY direction equals DMA_MEM_TO_MEM
- * and the SG list contains only one element and points at the source buffer.
- */
-static struct dma_async_tx_descriptor *sh_dmae_prep_sg(struct sh_dmae_chan *sh_chan,
-	struct scatterlist *sgl, unsigned int sg_len, dma_addr_t *addr,
-	enum dma_transfer_direction direction, unsigned long flags)
-{
-	struct scatterlist *sg;
-	struct sh_desc *first = NULL, *new = NULL /* compiler... */;
-	LIST_HEAD(tx_list);
-	int chunks = 0;
-	unsigned long irq_flags;
-	int i;
-
-	if (!sg_len)
-		return NULL;
-
-	for_each_sg(sgl, sg, sg_len, i)
-		chunks += (sg_dma_len(sg) + SH_DMA_TCR_MAX) /
-			(SH_DMA_TCR_MAX + 1);
-
-	/* Have to lock the whole loop to protect against concurrent release */
-	spin_lock_irqsave(&sh_chan->desc_lock, irq_flags);
-
-	/*
-	 * Chaining:
-	 * first descriptor is what user is dealing with in all API calls, its
-	 *	cookie is at first set to -EBUSY, at tx-submit to a positive
-	 *	number
-	 * if more than one chunk is needed further chunks have cookie = -EINVAL
-	 * the last chunk, if not equal to the first, has cookie = -ENOSPC
-	 * all chunks are linked onto the tx_list head with their .node heads
-	 *	only during this function, then they are immediately spliced
-	 *	back onto the free list in form of a chain
-	 */
-	for_each_sg(sgl, sg, sg_len, i) {
-		dma_addr_t sg_addr = sg_dma_address(sg);
-		size_t len = sg_dma_len(sg);
-
-		if (!len)
-			goto err_get_desc;
-
-		do {
-			dev_dbg(sh_chan->dev, "Add SG #%d@%p[%d], dma %llx\n",
-				i, sg, len, (unsigned long long)sg_addr);
-
-			if (direction == DMA_DEV_TO_MEM)
-				new = sh_dmae_add_desc(sh_chan, flags,
-						&sg_addr, addr, &len, &first,
-						direction);
-			else
-				new = sh_dmae_add_desc(sh_chan, flags,
-						addr, &sg_addr, &len, &first,
-						direction);
-			if (!new)
-				goto err_get_desc;
-
-			new->chunks = chunks--;
-			list_add_tail(&new->node, &tx_list);
-		} while (len);
-	}
-
-	if (new != first)
-		new->async_tx.cookie = -ENOSPC;
-
-	/* Put them back on the free list, so, they don't get lost */
-	list_splice_tail(&tx_list, &sh_chan->ld_free);
-
-	spin_unlock_irqrestore(&sh_chan->desc_lock, irq_flags);
-
-	return &first->async_tx;
-
-err_get_desc:
-	list_for_each_entry(new, &tx_list, node)
-		new->mark = DESC_IDLE;
-	list_splice(&tx_list, &sh_chan->ld_free);
-
-	spin_unlock_irqrestore(&sh_chan->desc_lock, irq_flags);
-
-	return NULL;
-}
-
-static struct dma_async_tx_descriptor *sh_dmae_prep_memcpy(
-	struct dma_chan *chan, dma_addr_t dma_dest, dma_addr_t dma_src,
-	size_t len, unsigned long flags)
-{
-	struct sh_dmae_chan *sh_chan;
-	struct scatterlist sg;
-
-	if (!chan || !len)
-		return NULL;
-
-	sh_chan = to_sh_chan(chan);
-
-	sg_init_table(&sg, 1);
-	sg_set_page(&sg, pfn_to_page(PFN_DOWN(dma_src)), len,
-		    offset_in_page(dma_src));
-	sg_dma_address(&sg) = dma_src;
-	sg_dma_len(&sg) = len;
-
-	return sh_dmae_prep_sg(sh_chan, &sg, 1, &dma_dest, DMA_MEM_TO_MEM,
-			       flags);
-}
-
-static struct dma_async_tx_descriptor *sh_dmae_prep_slave_sg(
-	struct dma_chan *chan, struct scatterlist *sgl, unsigned int sg_len,
-	enum dma_transfer_direction direction, unsigned long flags,
-	void *context)
-{
-	struct sh_dmae_slave *param;
-	struct sh_dmae_chan *sh_chan;
-	dma_addr_t slave_addr;
-
-	if (!chan)
-		return NULL;
-
-	sh_chan = to_sh_chan(chan);
-	param = chan->private;
-
-	/* Someone calling slave DMA on a public channel? */
-	if (!param || !sg_len) {
-		dev_warn(sh_chan->dev, "%s: bad parameter: %p, %d, %d\n",
-			 __func__, param, sg_len, param ? param->slave_id : -1);
-		return NULL;
-	}
-
-	slave_addr = param->config->addr;
-
-	/*
-	 * if (param != NULL), this is a successfully requested slave channel,
-	 * therefore param->config != NULL too.
-	 */
-	return sh_dmae_prep_sg(sh_chan, sgl, sg_len, &slave_addr,
-			       direction, flags);
-}
-
-static int sh_dmae_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
-			   unsigned long arg)
-{
-	struct sh_dmae_chan *sh_chan = to_sh_chan(chan);
-	unsigned long flags;
-
-	/* Only supports DMA_TERMINATE_ALL */
-	if (cmd != DMA_TERMINATE_ALL)
-		return -ENXIO;
-
-	if (!chan)
-		return -EINVAL;
-
-	spin_lock_irqsave(&sh_chan->desc_lock, flags);
-	dmae_halt(sh_chan);
-
-	if (!list_empty(&sh_chan->ld_queue)) {
-		/* Record partial transfer */
-		struct sh_desc *desc = list_entry(sh_chan->ld_queue.next,
-						  struct sh_desc, node);
-		desc->partial = (desc->hw.tcr - sh_dmae_readl(sh_chan, TCR)) <<
-			sh_chan->xmit_shift;
-	}
-	spin_unlock_irqrestore(&sh_chan->desc_lock, flags);
-
-	sh_dmae_chan_ld_cleanup(sh_chan, true);
-
-	return 0;
-}
-
-static dma_async_tx_callback __ld_cleanup(struct sh_dmae_chan *sh_chan, bool all)
-{
-	struct sh_desc *desc, *_desc;
-	/* Is the "exposed" head of a chain acked? */
-	bool head_acked = false;
-	dma_cookie_t cookie = 0;
-	dma_async_tx_callback callback = NULL;
-	void *param = NULL;
-	unsigned long flags;
-
-	spin_lock_irqsave(&sh_chan->desc_lock, flags);
-	list_for_each_entry_safe(desc, _desc, &sh_chan->ld_queue, node) {
-		struct dma_async_tx_descriptor *tx = &desc->async_tx;
-
-		BUG_ON(tx->cookie > 0 && tx->cookie != desc->cookie);
-		BUG_ON(desc->mark != DESC_SUBMITTED &&
-		       desc->mark != DESC_COMPLETED &&
-		       desc->mark != DESC_WAITING);
-
-		/*
-		 * queue is ordered, and we use this loop to (1) clean up all
-		 * completed descriptors, and to (2) update descriptor flags of
-		 * any chunks in a (partially) completed chain
-		 */
-		if (!all && desc->mark == DESC_SUBMITTED &&
-		    desc->cookie != cookie)
-			break;
-
-		if (tx->cookie > 0)
-			cookie = tx->cookie;
-
-		if (desc->mark == DESC_COMPLETED && desc->chunks == 1) {
-			if (sh_chan->common.completed_cookie != desc->cookie - 1)
-				dev_dbg(sh_chan->dev,
-					"Completing cookie %d, expected %d\n",
-					desc->cookie,
-					sh_chan->common.completed_cookie + 1);
-			sh_chan->common.completed_cookie = desc->cookie;
-		}
-
-		/* Call callback on the last chunk */
-		if (desc->mark == DESC_COMPLETED && tx->callback) {
-			desc->mark = DESC_WAITING;
-			callback = tx->callback;
-			param = tx->callback_param;
-			dev_dbg(sh_chan->dev, "descriptor #%d@%p on %d callback\n",
-				tx->cookie, tx, sh_chan->id);
-			BUG_ON(desc->chunks != 1);
-			break;
-		}
-
-		if (tx->cookie > 0 || tx->cookie == -EBUSY) {
-			if (desc->mark == DESC_COMPLETED) {
-				BUG_ON(tx->cookie < 0);
-				desc->mark = DESC_WAITING;
-			}
-			head_acked = async_tx_test_ack(tx);
-		} else {
-			switch (desc->mark) {
-			case DESC_COMPLETED:
-				desc->mark = DESC_WAITING;
-				/* Fall through */
-			case DESC_WAITING:
-				if (head_acked)
-					async_tx_ack(&desc->async_tx);
-			}
-		}
-
-		dev_dbg(sh_chan->dev, "descriptor %p #%d completed.\n",
-			tx, tx->cookie);
-
-		if (((desc->mark == DESC_COMPLETED ||
-		      desc->mark == DESC_WAITING) &&
-		     async_tx_test_ack(&desc->async_tx)) || all) {
-			/* Remove from ld_queue list */
-			desc->mark = DESC_IDLE;
-
-			list_move(&desc->node, &sh_chan->ld_free);
-
-			if (list_empty(&sh_chan->ld_queue)) {
-				dev_dbg(sh_chan->dev, "Bring down channel %d\n", sh_chan->id);
-				pm_runtime_put(sh_chan->dev);
-			}
-		}
-	}
-
-	if (all && !callback)
-		/*
-		 * Terminating and the loop completed normally: forgive
-		 * uncompleted cookies
-		 */
-		sh_chan->common.completed_cookie = sh_chan->common.cookie;
-
-	spin_unlock_irqrestore(&sh_chan->desc_lock, flags);
-
-	if (callback)
-		callback(param);
-
-	return callback;
-}
-
-/*
- * sh_chan_ld_cleanup - Clean up link descriptors
- *
- * This function cleans up the ld_queue of DMA channel.
- */
-static void sh_dmae_chan_ld_cleanup(struct sh_dmae_chan *sh_chan, bool all)
-{
-	while (__ld_cleanup(sh_chan, all))
-		;
-}
-
-/* Called under spin_lock_irq(&sh_chan->desc_lock) */
-static void sh_chan_xfer_ld_queue(struct sh_dmae_chan *sh_chan)
-{
-	struct sh_desc *desc;
-
-	/* DMA work check */
-	if (dmae_is_busy(sh_chan))
-		return;
-
-	/* 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",
-				desc->async_tx.cookie, sh_chan->id,
-				desc->hw.tcr, desc->hw.sar, desc->hw.dar);
-			/* Get the ld start address from ld_queue */
-			dmae_set_reg(sh_chan, &desc->hw);
-			dmae_start(sh_chan);
-			break;
-		}
-}
-
-static void sh_dmae_memcpy_issue_pending(struct dma_chan *chan)
-{
-	struct sh_dmae_chan *sh_chan = to_sh_chan(chan);
-
-	spin_lock_irq(&sh_chan->desc_lock);
-	if (sh_chan->pm_state == DMAE_PM_ESTABLISHED)
-		sh_chan_xfer_ld_queue(sh_chan);
-	else
-		sh_chan->pm_state = DMAE_PM_PENDING;
-	spin_unlock_irq(&sh_chan->desc_lock);
-}
-
-static enum dma_status sh_dmae_tx_status(struct dma_chan *chan,
-					dma_cookie_t cookie,
-					struct dma_tx_state *txstate)
-{
-	struct sh_dmae_chan *sh_chan = to_sh_chan(chan);
-	enum dma_status status;
-	unsigned long flags;
-
-	sh_dmae_chan_ld_cleanup(sh_chan, false);
-
-	spin_lock_irqsave(&sh_chan->desc_lock, flags);
-
-	status = dma_cookie_status(chan, cookie, txstate);
-
-	/*
-	 * If we don't find cookie on the queue, it has been aborted and we have
-	 * to report error
-	 */
-	if (status != DMA_SUCCESS) {
-		struct sh_desc *desc;
-		status = DMA_ERROR;
-		list_for_each_entry(desc, &sh_chan->ld_queue, node)
-			if (desc->cookie == cookie) {
-				status = DMA_IN_PROGRESS;
-				break;
-			}
-	}
-
-	spin_unlock_irqrestore(&sh_chan->desc_lock, flags);
-
-	return status;
-}
-
-static irqreturn_t sh_dmae_interrupt(int irq, void *data)
-{
-	irqreturn_t ret = IRQ_NONE;
-	struct sh_dmae_chan *sh_chan = data;
-	u32 chcr;
-
-	spin_lock(&sh_chan->desc_lock);
-
-	chcr = chcr_read(sh_chan);
-
-	if (chcr & CHCR_TE) {
-		/* DMA stop */
-		dmae_halt(sh_chan);
-
-		ret = IRQ_HANDLED;
-		tasklet_schedule(&sh_chan->tasklet);
-	}
-
-	spin_unlock(&sh_chan->desc_lock);
-
-	return ret;
-}
-
-/* Called from error IRQ or NMI */
-static bool sh_dmae_reset(struct sh_dmae_device *shdev)
-{
-	unsigned int handled = 0;
-	int i;
-
-	/* halt the dma controller */
-	sh_dmae_ctl_stop(shdev);
-
-	/* We cannot detect, which channel caused the error, have to reset all */
-	for (i = 0; i < SH_DMAC_MAX_CHANNELS; i++) {
-		struct sh_dmae_chan *sh_chan = shdev->chan[i];
-		struct sh_desc *desc;
-		LIST_HEAD(dl);
-
-		if (!sh_chan)
-			continue;
-
-		spin_lock(&sh_chan->desc_lock);
-
-		/* Stop the channel */
-		dmae_halt(sh_chan);
-
-		list_splice_init(&sh_chan->ld_queue, &dl);
-
-		if (!list_empty(&dl)) {
-			dev_dbg(sh_chan->dev, "Bring down channel %d\n", sh_chan->id);
-			pm_runtime_put(sh_chan->dev);
-		}
-		sh_chan->pm_state = DMAE_PM_ESTABLISHED;
-
-		spin_unlock(&sh_chan->desc_lock);
-
-		/* Complete all  */
-		list_for_each_entry(desc, &dl, node) {
-			struct dma_async_tx_descriptor *tx = &desc->async_tx;
-			desc->mark = DESC_IDLE;
-			if (tx->callback)
-				tx->callback(tx->callback_param);
-		}
-
-		spin_lock(&sh_chan->desc_lock);
-		list_splice(&dl, &sh_chan->ld_free);
-		spin_unlock(&sh_chan->desc_lock);
-
-		handled++;
-	}
-
-	sh_dmae_rst(shdev);
-
-	return !!handled;
-}
-
-static irqreturn_t sh_dmae_err(int irq, void *data)
-{
-	struct sh_dmae_device *shdev = data;
-
-	if (!(dmaor_read(shdev) & DMAOR_AE))
-		return IRQ_NONE;
-
-	sh_dmae_reset(data);
-	return IRQ_HANDLED;
-}
-
-static void dmae_do_tasklet(unsigned long data)
-{
-	struct sh_dmae_chan *sh_chan = (struct sh_dmae_chan *)data;
-	struct sh_desc *desc;
-	u32 sar_buf = sh_dmae_readl(sh_chan, SAR);
-	u32 dar_buf = sh_dmae_readl(sh_chan, DAR);
-
-	spin_lock_irq(&sh_chan->desc_lock);
-	list_for_each_entry(desc, &sh_chan->ld_queue, node) {
-		if (desc->mark == DESC_SUBMITTED &&
-		    ((desc->direction == DMA_DEV_TO_MEM &&
-		      (desc->hw.dar + desc->hw.tcr) == dar_buf) ||
-		     (desc->hw.sar + desc->hw.tcr) == sar_buf)) {
-			dev_dbg(sh_chan->dev, "done #%d@%p dst %u\n",
-				desc->async_tx.cookie, &desc->async_tx,
-				desc->hw.dar);
-			desc->mark = DESC_COMPLETED;
-			break;
-		}
-	}
-	/* Next desc */
-	sh_chan_xfer_ld_queue(sh_chan);
-	spin_unlock_irq(&sh_chan->desc_lock);
-
-	sh_dmae_chan_ld_cleanup(sh_chan, false);
-}
-
-static bool sh_dmae_nmi_notify(struct sh_dmae_device *shdev)
-{
-	/* Fast path out if NMIF is not asserted for this controller */
-	if ((dmaor_read(shdev) & DMAOR_NMIF) == 0)
-		return false;
-
-	return sh_dmae_reset(shdev);
-}
-
-static int sh_dmae_nmi_handler(struct notifier_block *self,
-			       unsigned long cmd, void *data)
-{
-	struct sh_dmae_device *shdev;
-	int ret = NOTIFY_DONE;
-	bool triggered;
-
-	/*
-	 * Only concern ourselves with NMI events.
-	 *
-	 * Normally we would check the die chain value, but as this needs
-	 * to be architecture independent, check for NMI context instead.
-	 */
-	if (!in_nmi())
-		return NOTIFY_DONE;
-
-	rcu_read_lock();
-	list_for_each_entry_rcu(shdev, &sh_dmae_devices, node) {
-		/*
-		 * Only stop if one of the controllers has NMIF asserted,
-		 * we do not want to interfere with regular address error
-		 * handling or NMI events that don't concern the DMACs.
-		 */
-		triggered = sh_dmae_nmi_notify(shdev);
-		if (triggered == true)
-			ret = NOTIFY_OK;
-	}
-	rcu_read_unlock();
-
-	return ret;
-}
-
-static struct notifier_block sh_dmae_nmi_notifier __read_mostly = {
-	.notifier_call	= sh_dmae_nmi_handler,
-
-	/* Run before NMI debug handler and KGDB */
-	.priority	= 1,
-};
-
-static int __devinit sh_dmae_chan_probe(struct sh_dmae_device *shdev, int id,
-					int irq, unsigned long flags)
-{
-	int err;
-	const struct sh_dmae_channel *chan_pdata = &shdev->pdata->channel[id];
-	struct platform_device *pdev = to_platform_device(shdev->common.dev);
-	struct sh_dmae_chan *new_sh_chan;
-
-	/* alloc channel */
-	new_sh_chan = kzalloc(sizeof(struct sh_dmae_chan), GFP_KERNEL);
-	if (!new_sh_chan) {
-		dev_err(shdev->common.dev,
-			"No free memory for allocating dma channels!\n");
-		return -ENOMEM;
-	}
-
-	new_sh_chan->pm_state = DMAE_PM_ESTABLISHED;
-
-	/* reference struct dma_device */
-	new_sh_chan->common.device = &shdev->common;
-	dma_cookie_init(&new_sh_chan->common);
-
-	new_sh_chan->dev = shdev->common.dev;
-	new_sh_chan->id = id;
-	new_sh_chan->irq = irq;
-	new_sh_chan->base = shdev->chan_reg + chan_pdata->offset / sizeof(u32);
-
-	/* Init DMA tasklet */
-	tasklet_init(&new_sh_chan->tasklet, dmae_do_tasklet,
-			(unsigned long)new_sh_chan);
-
-	spin_lock_init(&new_sh_chan->desc_lock);
-
-	/* Init descripter manage list */
-	INIT_LIST_HEAD(&new_sh_chan->ld_queue);
-	INIT_LIST_HEAD(&new_sh_chan->ld_free);
-
-	/* Add the channel to DMA device channel list */
-	list_add_tail(&new_sh_chan->common.device_node,
-			&shdev->common.channels);
-	shdev->common.chancnt++;
-
-	if (pdev->id >= 0)
-		snprintf(new_sh_chan->dev_id, sizeof(new_sh_chan->dev_id),
-			 "sh-dmae%d.%d", pdev->id, new_sh_chan->id);
-	else
-		snprintf(new_sh_chan->dev_id, sizeof(new_sh_chan->dev_id),
-			 "sh-dma%d", new_sh_chan->id);
-
-	/* set up channel irq */
-	err = request_irq(irq, &sh_dmae_interrupt, flags,
-			  new_sh_chan->dev_id, new_sh_chan);
-	if (err) {
-		dev_err(shdev->common.dev, "DMA channel %d request_irq error "
-			"with return %d\n", id, err);
-		goto err_no_irq;
-	}
-
-	shdev->chan[id] = new_sh_chan;
-	return 0;
-
-err_no_irq:
-	/* remove from dmaengine device node */
-	list_del(&new_sh_chan->common.device_node);
-	kfree(new_sh_chan);
-	return err;
-}
-
-static void sh_dmae_chan_remove(struct sh_dmae_device *shdev)
-{
-	int i;
-
-	for (i = shdev->common.chancnt - 1 ; i >= 0 ; i--) {
-		if (shdev->chan[i]) {
-			struct sh_dmae_chan *sh_chan = shdev->chan[i];
-
-			free_irq(sh_chan->irq, sh_chan);
-
-			list_del(&sh_chan->common.device_node);
-			kfree(sh_chan);
-			shdev->chan[i] = NULL;
-		}
-	}
-	shdev->common.chancnt = 0;
-}
-
-static int __init sh_dmae_probe(struct platform_device *pdev)
-{
-	struct sh_dmae_pdata *pdata = pdev->dev.platform_data;
-	unsigned long irqflags = IRQF_DISABLED,
-		chan_flag[SH_DMAC_MAX_CHANNELS] = {};
-	int errirq, chan_irq[SH_DMAC_MAX_CHANNELS];
-	int err, i, irq_cnt = 0, irqres = 0, irq_cap = 0;
-	struct sh_dmae_device *shdev;
-	struct resource *chan, *dmars, *errirq_res, *chanirq_res;
-
-	/* get platform data */
-	if (!pdata || !pdata->channel_num)
-		return -ENODEV;
-
-	chan = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	/* DMARS area is optional */
-	dmars = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	/*
-	 * IRQ resources:
-	 * 1. there always must be at least one IRQ IO-resource. On SH4 it is
-	 *    the error IRQ, in which case it is the only IRQ in this resource:
-	 *    start == end. If it is the only IRQ resource, all channels also
-	 *    use the same IRQ.
-	 * 2. DMA channel IRQ resources can be specified one per resource or in
-	 *    ranges (start != end)
-	 * 3. iff all events (channels and, optionally, error) on this
-	 *    controller use the same IRQ, only one IRQ resource can be
-	 *    specified, otherwise there must be one IRQ per channel, even if
-	 *    some of them are equal
-	 * 4. if all IRQs on this controller are equal or if some specific IRQs
-	 *    specify IORESOURCE_IRQ_SHAREABLE in their resources, they will be
-	 *    requested with the IRQF_SHARED flag
-	 */
-	errirq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!chan || !errirq_res)
-		return -ENODEV;
-
-	if (!request_mem_region(chan->start, resource_size(chan), pdev->name)) {
-		dev_err(&pdev->dev, "DMAC register region already claimed\n");
-		return -EBUSY;
-	}
-
-	if (dmars && !request_mem_region(dmars->start, resource_size(dmars), pdev->name)) {
-		dev_err(&pdev->dev, "DMAC DMARS region already claimed\n");
-		err = -EBUSY;
-		goto ermrdmars;
-	}
-
-	err = -ENOMEM;
-	shdev = kzalloc(sizeof(struct sh_dmae_device), GFP_KERNEL);
-	if (!shdev) {
-		dev_err(&pdev->dev, "Not enough memory\n");
-		goto ealloc;
-	}
-
-	shdev->chan_reg = ioremap(chan->start, resource_size(chan));
-	if (!shdev->chan_reg)
-		goto emapchan;
-	if (dmars) {
-		shdev->dmars = ioremap(dmars->start, resource_size(dmars));
-		if (!shdev->dmars)
-			goto emapdmars;
-	}
-
-	/* platform data */
-	shdev->pdata = pdata;
-
-	if (pdata->chcr_offset)
-		shdev->chcr_offset = pdata->chcr_offset;
-	else
-		shdev->chcr_offset = CHCR;
-
-	if (pdata->chcr_ie_bit)
-		shdev->chcr_ie_bit = pdata->chcr_ie_bit;
-	else
-		shdev->chcr_ie_bit = CHCR_IE;
-
-	platform_set_drvdata(pdev, shdev);
-
-	shdev->common.dev = &pdev->dev;
-
-	pm_runtime_enable(&pdev->dev);
-	pm_runtime_get_sync(&pdev->dev);
-
-	spin_lock_irq(&sh_dmae_lock);
-	list_add_tail_rcu(&shdev->node, &sh_dmae_devices);
-	spin_unlock_irq(&sh_dmae_lock);
-
-	/* reset dma controller - only needed as a test */
-	err = sh_dmae_rst(shdev);
-	if (err)
-		goto rst_err;
-
-	INIT_LIST_HEAD(&shdev->common.channels);
-
-	if (!pdata->slave_only)
-		dma_cap_set(DMA_MEMCPY, shdev->common.cap_mask);
-	if (pdata->slave && pdata->slave_num)
-		dma_cap_set(DMA_SLAVE, shdev->common.cap_mask);
-
-	shdev->common.device_alloc_chan_resources
-		= sh_dmae_alloc_chan_resources;
-	shdev->common.device_free_chan_resources = sh_dmae_free_chan_resources;
-	shdev->common.device_prep_dma_memcpy = sh_dmae_prep_memcpy;
-	shdev->common.device_tx_status = sh_dmae_tx_status;
-	shdev->common.device_issue_pending = sh_dmae_memcpy_issue_pending;
-
-	/* Compulsory for DMA_SLAVE fields */
-	shdev->common.device_prep_slave_sg = sh_dmae_prep_slave_sg;
-	shdev->common.device_control = sh_dmae_control;
-
-	/* Default transfer size of 32 bytes requires 32-byte alignment */
-	shdev->common.copy_align = LOG2_DEFAULT_XFER_SIZE;
-
-#if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
-	chanirq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
-
-	if (!chanirq_res)
-		chanirq_res = errirq_res;
-	else
-		irqres++;
-
-	if (chanirq_res == errirq_res ||
-	    (errirq_res->flags & IORESOURCE_BITS) == IORESOURCE_IRQ_SHAREABLE)
-		irqflags = IRQF_SHARED;
-
-	errirq = errirq_res->start;
-
-	err = request_irq(errirq, sh_dmae_err, irqflags,
-			  "DMAC Address Error", shdev);
-	if (err) {
-		dev_err(&pdev->dev,
-			"DMA failed requesting irq #%d, error %d\n",
-			errirq, err);
-		goto eirq_err;
-	}
-
-#else
-	chanirq_res = errirq_res;
-#endif /* CONFIG_CPU_SH4 || CONFIG_ARCH_SHMOBILE */
-
-	if (chanirq_res->start == chanirq_res->end &&
-	    !platform_get_resource(pdev, IORESOURCE_IRQ, 1)) {
-		/* Special case - all multiplexed */
-		for (; irq_cnt < pdata->channel_num; irq_cnt++) {
-			if (irq_cnt < SH_DMAC_MAX_CHANNELS) {
-				chan_irq[irq_cnt] = chanirq_res->start;
-				chan_flag[irq_cnt] = IRQF_SHARED;
-			} else {
-				irq_cap = 1;
-				break;
-			}
-		}
-	} else {
-		do {
-			for (i = chanirq_res->start; i <= chanirq_res->end; i++) {
-				if (irq_cnt >= SH_DMAC_MAX_CHANNELS) {
-					irq_cap = 1;
-					break;
-				}
-
-				if ((errirq_res->flags & IORESOURCE_BITS) ==
-				    IORESOURCE_IRQ_SHAREABLE)
-					chan_flag[irq_cnt] = IRQF_SHARED;
-				else
-					chan_flag[irq_cnt] = IRQF_DISABLED;
-				dev_dbg(&pdev->dev,
-					"Found IRQ %d for channel %d\n",
-					i, irq_cnt);
-				chan_irq[irq_cnt++] = i;
-			}
-
-			if (irq_cnt >= SH_DMAC_MAX_CHANNELS)
-				break;
-
-			chanirq_res = platform_get_resource(pdev,
-						IORESOURCE_IRQ, ++irqres);
-		} while (irq_cnt < pdata->channel_num && chanirq_res);
-	}
-
-	/* Create DMA Channel */
-	for (i = 0; i < irq_cnt; i++) {
-		err = sh_dmae_chan_probe(shdev, i, chan_irq[i], chan_flag[i]);
-		if (err)
-			goto chan_probe_err;
-	}
-
-	if (irq_cap)
-		dev_notice(&pdev->dev, "Attempting to register %d DMA "
-			   "channels when a maximum of %d are supported.\n",
-			   pdata->channel_num, SH_DMAC_MAX_CHANNELS);
-
-	pm_runtime_put(&pdev->dev);
-
-	dma_async_device_register(&shdev->common);
-
-	return err;
-
-chan_probe_err:
-	sh_dmae_chan_remove(shdev);
-
-#if defined(CONFIG_CPU_SH4) || defined(CONFIG_ARCH_SHMOBILE)
-	free_irq(errirq, shdev);
-eirq_err:
-#endif
-rst_err:
-	spin_lock_irq(&sh_dmae_lock);
-	list_del_rcu(&shdev->node);
-	spin_unlock_irq(&sh_dmae_lock);
-
-	pm_runtime_put(&pdev->dev);
-	pm_runtime_disable(&pdev->dev);
-
-	if (dmars)
-		iounmap(shdev->dmars);
-
-	platform_set_drvdata(pdev, NULL);
-emapdmars:
-	iounmap(shdev->chan_reg);
-	synchronize_rcu();
-emapchan:
-	kfree(shdev);
-ealloc:
-	if (dmars)
-		release_mem_region(dmars->start, resource_size(dmars));
-ermrdmars:
-	release_mem_region(chan->start, resource_size(chan));
-
-	return err;
-}
-
-static int __exit sh_dmae_remove(struct platform_device *pdev)
-{
-	struct sh_dmae_device *shdev = platform_get_drvdata(pdev);
-	struct resource *res;
-	int errirq = platform_get_irq(pdev, 0);
-
-	dma_async_device_unregister(&shdev->common);
-
-	if (errirq > 0)
-		free_irq(errirq, shdev);
-
-	spin_lock_irq(&sh_dmae_lock);
-	list_del_rcu(&shdev->node);
-	spin_unlock_irq(&sh_dmae_lock);
-
-	/* channel data remove */
-	sh_dmae_chan_remove(shdev);
-
-	pm_runtime_disable(&pdev->dev);
-
-	if (shdev->dmars)
-		iounmap(shdev->dmars);
-	iounmap(shdev->chan_reg);
-
-	platform_set_drvdata(pdev, NULL);
-
-	synchronize_rcu();
-	kfree(shdev);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res)
-		release_mem_region(res->start, resource_size(res));
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (res)
-		release_mem_region(res->start, resource_size(res));
-
-	return 0;
-}
-
-static void sh_dmae_shutdown(struct platform_device *pdev)
-{
-	struct sh_dmae_device *shdev = platform_get_drvdata(pdev);
-	sh_dmae_ctl_stop(shdev);
-}
-
-static int sh_dmae_runtime_suspend(struct device *dev)
-{
-	return 0;
-}
-
-static int sh_dmae_runtime_resume(struct device *dev)
-{
-	struct sh_dmae_device *shdev = dev_get_drvdata(dev);
-
-	return sh_dmae_rst(shdev);
-}
-
-#ifdef CONFIG_PM
-static int sh_dmae_suspend(struct device *dev)
-{
-	return 0;
-}
-
-static int sh_dmae_resume(struct device *dev)
-{
-	struct sh_dmae_device *shdev = dev_get_drvdata(dev);
-	int i, ret;
-
-	ret = sh_dmae_rst(shdev);
-	if (ret < 0)
-		dev_err(dev, "Failed to reset!\n");
-
-	for (i = 0; i < shdev->pdata->channel_num; i++) {
-		struct sh_dmae_chan *sh_chan = shdev->chan[i];
-		struct sh_dmae_slave *param = sh_chan->common.private;
-
-		if (!sh_chan->descs_allocated)
-			continue;
-
-		if (param) {
-			const struct sh_dmae_slave_config *cfg = param->config;
-			dmae_set_dmars(sh_chan, cfg->mid_rid);
-			dmae_set_chcr(sh_chan, cfg->chcr);
-		} else {
-			dmae_init(sh_chan);
-		}
-	}
-
-	return 0;
-}
-#else
-#define sh_dmae_suspend NULL
-#define sh_dmae_resume NULL
-#endif
-
-const struct dev_pm_ops sh_dmae_pm = {
-	.suspend		= sh_dmae_suspend,
-	.resume			= sh_dmae_resume,
-	.runtime_suspend	= sh_dmae_runtime_suspend,
-	.runtime_resume		= sh_dmae_runtime_resume,
-};
-
-static struct platform_driver sh_dmae_driver = {
-	.remove		= __exit_p(sh_dmae_remove),
-	.shutdown	= sh_dmae_shutdown,
-	.driver = {
-		.owner	= THIS_MODULE,
-		.name	= "sh-dma-engine",
-		.pm	= &sh_dmae_pm,
-	},
-};
-
-static int __init sh_dmae_init(void)
-{
-	/* Wire up NMI handling */
-	int err = register_die_notifier(&sh_dmae_nmi_notifier);
-	if (err)
-		return err;
-
-	return platform_driver_probe(&sh_dmae_driver, sh_dmae_probe);
-}
-module_init(sh_dmae_init);
-
-static void __exit sh_dmae_exit(void)
-{
-	platform_driver_unregister(&sh_dmae_driver);
-
-	unregister_die_notifier(&sh_dmae_nmi_notifier);
-}
-module_exit(sh_dmae_exit);
-
-MODULE_AUTHOR("Nobuhiro Iwamatsu <iwamatsu.nobuhiro@renesas.com>");
-MODULE_DESCRIPTION("Renesas SH DMA Engine driver");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:sh-dma-engine");
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
new file mode 100644
index 00000000000..d52dbc6c54a
--- /dev/null
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -0,0 +1,1415 @@
+/*
+ * DMA driver for Nvidia's Tegra20 APB DMA controller.
+ *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <mach/clk.h>
+#include "dmaengine.h"
+
+#define TEGRA_APBDMA_GENERAL			0x0
+#define TEGRA_APBDMA_GENERAL_ENABLE		BIT(31)
+
+#define TEGRA_APBDMA_CONTROL			0x010
+#define TEGRA_APBDMA_IRQ_MASK			0x01c
+#define TEGRA_APBDMA_IRQ_MASK_SET		0x020
+
+/* CSR register */
+#define TEGRA_APBDMA_CHAN_CSR			0x00
+#define TEGRA_APBDMA_CSR_ENB			BIT(31)
+#define TEGRA_APBDMA_CSR_IE_EOC			BIT(30)
+#define TEGRA_APBDMA_CSR_HOLD			BIT(29)
+#define TEGRA_APBDMA_CSR_DIR			BIT(28)
+#define TEGRA_APBDMA_CSR_ONCE			BIT(27)
+#define TEGRA_APBDMA_CSR_FLOW			BIT(21)
+#define TEGRA_APBDMA_CSR_REQ_SEL_SHIFT		16
+#define TEGRA_APBDMA_CSR_WCOUNT_MASK		0xFFFC
+
+/* STATUS register */
+#define TEGRA_APBDMA_CHAN_STATUS		0x004
+#define TEGRA_APBDMA_STATUS_BUSY		BIT(31)
+#define TEGRA_APBDMA_STATUS_ISE_EOC		BIT(30)
+#define TEGRA_APBDMA_STATUS_HALT		BIT(29)
+#define TEGRA_APBDMA_STATUS_PING_PONG		BIT(28)
+#define TEGRA_APBDMA_STATUS_COUNT_SHIFT		2
+#define TEGRA_APBDMA_STATUS_COUNT_MASK		0xFFFC
+
+/* AHB memory address */
+#define TEGRA_APBDMA_CHAN_AHBPTR		0x010
+
+/* AHB sequence register */
+#define TEGRA_APBDMA_CHAN_AHBSEQ		0x14
+#define TEGRA_APBDMA_AHBSEQ_INTR_ENB		BIT(31)
+#define TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_8		(0 << 28)
+#define TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_16	(1 << 28)
+#define TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_32	(2 << 28)
+#define TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_64	(3 << 28)
+#define TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_128	(4 << 28)
+#define TEGRA_APBDMA_AHBSEQ_DATA_SWAP		BIT(27)
+#define TEGRA_APBDMA_AHBSEQ_BURST_1		(4 << 24)
+#define TEGRA_APBDMA_AHBSEQ_BURST_4		(5 << 24)
+#define TEGRA_APBDMA_AHBSEQ_BURST_8		(6 << 24)
+#define TEGRA_APBDMA_AHBSEQ_DBL_BUF		BIT(19)
+#define TEGRA_APBDMA_AHBSEQ_WRAP_SHIFT		16
+#define TEGRA_APBDMA_AHBSEQ_WRAP_NONE		0
+
+/* APB address */
+#define TEGRA_APBDMA_CHAN_APBPTR		0x018
+
+/* APB sequence register */
+#define TEGRA_APBDMA_CHAN_APBSEQ		0x01c
+#define TEGRA_APBDMA_APBSEQ_BUS_WIDTH_8		(0 << 28)
+#define TEGRA_APBDMA_APBSEQ_BUS_WIDTH_16	(1 << 28)
+#define TEGRA_APBDMA_APBSEQ_BUS_WIDTH_32	(2 << 28)
+#define TEGRA_APBDMA_APBSEQ_BUS_WIDTH_64	(3 << 28)
+#define TEGRA_APBDMA_APBSEQ_BUS_WIDTH_128	(4 << 28)
+#define TEGRA_APBDMA_APBSEQ_DATA_SWAP		BIT(27)
+#define TEGRA_APBDMA_APBSEQ_WRAP_WORD_1		(1 << 16)
+
+/*
+ * If any burst is in flight and DMA paused then this is the time to complete
+ * on-flight burst and update DMA status register.
+ */
+#define TEGRA_APBDMA_BURST_COMPLETE_TIME	20
+
+/* Channel base address offset from APBDMA base address */
+#define TEGRA_APBDMA_CHANNEL_BASE_ADD_OFFSET	0x1000
+
+/* DMA channel register space size */
+#define TEGRA_APBDMA_CHANNEL_REGISTER_SIZE	0x20
+
+struct tegra_dma;
+
+/*
+ * tegra_dma_chip_data Tegra chip specific DMA data
+ * @nr_channels: Number of channels available in the controller.
+ * @max_dma_count: Maximum DMA transfer count supported by DMA controller.
+ */
+struct tegra_dma_chip_data {
+	int nr_channels;
+	int max_dma_count;
+};
+
+/* DMA channel registers */
+struct tegra_dma_channel_regs {
+	unsigned long	csr;
+	unsigned long	ahb_ptr;
+	unsigned long	apb_ptr;
+	unsigned long	ahb_seq;
+	unsigned long	apb_seq;
+};
+
+/*
+ * tegra_dma_sg_req: Dma request details to configure hardware. This
+ * contains the details for one transfer to configure DMA hw.
+ * The client's request for data transfer can be broken into multiple
+ * sub-transfer as per requester details and hw support.
+ * This sub transfer get added in the list of transfer and point to Tegra
+ * DMA descriptor which manages the transfer details.
+ */
+struct tegra_dma_sg_req {
+	struct tegra_dma_channel_regs	ch_regs;
+	int				req_len;
+	bool				configured;
+	bool				last_sg;
+	bool				half_done;
+	struct list_head		node;
+	struct tegra_dma_desc		*dma_desc;
+};
+
+/*
+ * tegra_dma_desc: Tegra DMA descriptors which manages the client requests.
+ * This descriptor keep track of transfer status, callbacks and request
+ * counts etc.
+ */
+struct tegra_dma_desc {
+	struct dma_async_tx_descriptor	txd;
+	int				bytes_requested;
+	int				bytes_transferred;
+	enum dma_status			dma_status;
+	struct list_head		node;
+	struct list_head		tx_list;
+	struct list_head		cb_node;
+	int				cb_count;
+};
+
+struct tegra_dma_channel;
+
+typedef void (*dma_isr_handler)(struct tegra_dma_channel *tdc,
+				bool to_terminate);
+
+/* tegra_dma_channel: Channel specific information */
+struct tegra_dma_channel {
+	struct dma_chan		dma_chan;
+	bool			config_init;
+	int			id;
+	int			irq;
+	unsigned long		chan_base_offset;
+	spinlock_t		lock;
+	bool			busy;
+	struct tegra_dma	*tdma;
+	bool			cyclic;
+
+	/* Different lists for managing the requests */
+	struct list_head	free_sg_req;
+	struct list_head	pending_sg_req;
+	struct list_head	free_dma_desc;
+	struct list_head	cb_desc;
+
+	/* ISR handler and tasklet for bottom half of isr handling */
+	dma_isr_handler		isr_handler;
+	struct tasklet_struct	tasklet;
+	dma_async_tx_callback	callback;
+	void			*callback_param;
+
+	/* Channel-slave specific configuration */
+	struct dma_slave_config dma_sconfig;
+};
+
+/* tegra_dma: Tegra DMA specific information */
+struct tegra_dma {
+	struct dma_device		dma_dev;
+	struct device			*dev;
+	struct clk			*dma_clk;
+	spinlock_t			global_lock;
+	void __iomem			*base_addr;
+	struct tegra_dma_chip_data	*chip_data;
+
+	/* Some register need to be cache before suspend */
+	u32				reg_gen;
+
+	/* Last member of the structure */
+	struct tegra_dma_channel channels[0];
+};
+
+static inline void tdma_write(struct tegra_dma *tdma, u32 reg, u32 val)
+{
+	writel(val, tdma->base_addr + reg);
+}
+
+static inline u32 tdma_read(struct tegra_dma *tdma, u32 reg)
+{
+	return readl(tdma->base_addr + reg);
+}
+
+static inline void tdc_write(struct tegra_dma_channel *tdc,
+		u32 reg, u32 val)
+{
+	writel(val, tdc->tdma->base_addr + tdc->chan_base_offset + reg);
+}
+
+static inline u32 tdc_read(struct tegra_dma_channel *tdc, u32 reg)
+{
+	return readl(tdc->tdma->base_addr + tdc->chan_base_offset + reg);
+}
+
+static inline struct tegra_dma_channel *to_tegra_dma_chan(struct dma_chan *dc)
+{
+	return container_of(dc, struct tegra_dma_channel, dma_chan);
+}
+
+static inline struct tegra_dma_desc *txd_to_tegra_dma_desc(
+		struct dma_async_tx_descriptor *td)
+{
+	return container_of(td, struct tegra_dma_desc, txd);
+}
+
+static inline struct device *tdc2dev(struct tegra_dma_channel *tdc)
+{
+	return &tdc->dma_chan.dev->device;
+}
+
+static dma_cookie_t tegra_dma_tx_submit(struct dma_async_tx_descriptor *tx);
+static int tegra_dma_runtime_suspend(struct device *dev);
+static int tegra_dma_runtime_resume(struct device *dev);
+
+/* Get DMA desc from free list, if not there then allocate it.  */
+static struct tegra_dma_desc *tegra_dma_desc_get(
+		struct tegra_dma_channel *tdc)
+{
+	struct tegra_dma_desc *dma_desc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+
+	/* Do not allocate if desc are waiting for ack */
+	list_for_each_entry(dma_desc, &tdc->free_dma_desc, node) {
+		if (async_tx_test_ack(&dma_desc->txd)) {
+			list_del(&dma_desc->node);
+			spin_unlock_irqrestore(&tdc->lock, flags);
+			return dma_desc;
+		}
+	}
+
+	spin_unlock_irqrestore(&tdc->lock, flags);
+
+	/* Allocate DMA desc */
+	dma_desc = kzalloc(sizeof(*dma_desc), GFP_ATOMIC);
+	if (!dma_desc) {
+		dev_err(tdc2dev(tdc), "dma_desc alloc failed\n");
+		return NULL;
+	}
+
+	dma_async_tx_descriptor_init(&dma_desc->txd, &tdc->dma_chan);
+	dma_desc->txd.tx_submit = tegra_dma_tx_submit;
+	dma_desc->txd.flags = 0;
+	return dma_desc;
+}
+
+static void tegra_dma_desc_put(struct tegra_dma_channel *tdc,
+		struct tegra_dma_desc *dma_desc)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	if (!list_empty(&dma_desc->tx_list))
+		list_splice_init(&dma_desc->tx_list, &tdc->free_sg_req);
+	list_add_tail(&dma_desc->node, &tdc->free_dma_desc);
+	spin_unlock_irqrestore(&tdc->lock, flags);
+}
+
+static struct tegra_dma_sg_req *tegra_dma_sg_req_get(
+		struct tegra_dma_channel *tdc)
+{
+	struct tegra_dma_sg_req *sg_req = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	if (!list_empty(&tdc->free_sg_req)) {
+		sg_req = list_first_entry(&tdc->free_sg_req,
+					typeof(*sg_req), node);
+		list_del(&sg_req->node);
+		spin_unlock_irqrestore(&tdc->lock, flags);
+		return sg_req;
+	}
+	spin_unlock_irqrestore(&tdc->lock, flags);
+
+	sg_req = kzalloc(sizeof(struct tegra_dma_sg_req), GFP_ATOMIC);
+	if (!sg_req)
+		dev_err(tdc2dev(tdc), "sg_req alloc failed\n");
+	return sg_req;
+}
+
+static int tegra_dma_slave_config(struct dma_chan *dc,
+		struct dma_slave_config *sconfig)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+
+	if (!list_empty(&tdc->pending_sg_req)) {
+		dev_err(tdc2dev(tdc), "Configuration not allowed\n");
+		return -EBUSY;
+	}
+
+	memcpy(&tdc->dma_sconfig, sconfig, sizeof(*sconfig));
+	tdc->config_init = true;
+	return 0;
+}
+
+static void tegra_dma_global_pause(struct tegra_dma_channel *tdc,
+	bool wait_for_burst_complete)
+{
+	struct tegra_dma *tdma = tdc->tdma;
+
+	spin_lock(&tdma->global_lock);
+	tdma_write(tdma, TEGRA_APBDMA_GENERAL, 0);
+	if (wait_for_burst_complete)
+		udelay(TEGRA_APBDMA_BURST_COMPLETE_TIME);
+}
+
+static void tegra_dma_global_resume(struct tegra_dma_channel *tdc)
+{
+	struct tegra_dma *tdma = tdc->tdma;
+
+	tdma_write(tdma, TEGRA_APBDMA_GENERAL, TEGRA_APBDMA_GENERAL_ENABLE);
+	spin_unlock(&tdma->global_lock);
+}
+
+static void tegra_dma_stop(struct tegra_dma_channel *tdc)
+{
+	u32 csr;
+	u32 status;
+
+	/* Disable interrupts */
+	csr = tdc_read(tdc, TEGRA_APBDMA_CHAN_CSR);
+	csr &= ~TEGRA_APBDMA_CSR_IE_EOC;
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_CSR, csr);
+
+	/* Disable DMA */
+	csr &= ~TEGRA_APBDMA_CSR_ENB;
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_CSR, csr);
+
+	/* Clear interrupt status if it is there */
+	status = tdc_read(tdc, TEGRA_APBDMA_CHAN_STATUS);
+	if (status & TEGRA_APBDMA_STATUS_ISE_EOC) {
+		dev_dbg(tdc2dev(tdc), "%s():clearing interrupt\n", __func__);
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_STATUS, status);
+	}
+	tdc->busy = false;
+}
+
+static void tegra_dma_start(struct tegra_dma_channel *tdc,
+		struct tegra_dma_sg_req *sg_req)
+{
+	struct tegra_dma_channel_regs *ch_regs = &sg_req->ch_regs;
+
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_CSR, ch_regs->csr);
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_APBSEQ, ch_regs->apb_seq);
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_APBPTR, ch_regs->apb_ptr);
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_AHBSEQ, ch_regs->ahb_seq);
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_AHBPTR, ch_regs->ahb_ptr);
+
+	/* Start DMA */
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_CSR,
+				ch_regs->csr | TEGRA_APBDMA_CSR_ENB);
+}
+
+static void tegra_dma_configure_for_next(struct tegra_dma_channel *tdc,
+		struct tegra_dma_sg_req *nsg_req)
+{
+	unsigned long status;
+
+	/*
+	 * The DMA controller reloads the new configuration for next transfer
+	 * after last burst of current transfer completes.
+	 * If there is no IEC status then this makes sure that last burst
+	 * has not be completed. There may be case that last burst is on
+	 * flight and so it can complete but because DMA is paused, it
+	 * will not generates interrupt as well as not reload the new
+	 * configuration.
+	 * If there is already IEC status then interrupt handler need to
+	 * load new configuration.
+	 */
+	tegra_dma_global_pause(tdc, false);
+	status  = tdc_read(tdc, TEGRA_APBDMA_CHAN_STATUS);
+
+	/*
+	 * If interrupt is pending then do nothing as the ISR will handle
+	 * the programing for new request.
+	 */
+	if (status & TEGRA_APBDMA_STATUS_ISE_EOC) {
+		dev_err(tdc2dev(tdc),
+			"Skipping new configuration as interrupt is pending\n");
+		tegra_dma_global_resume(tdc);
+		return;
+	}
+
+	/* Safe to program new configuration */
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_APBPTR, nsg_req->ch_regs.apb_ptr);
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_AHBPTR, nsg_req->ch_regs.ahb_ptr);
+	tdc_write(tdc, TEGRA_APBDMA_CHAN_CSR,
+				nsg_req->ch_regs.csr | TEGRA_APBDMA_CSR_ENB);
+	nsg_req->configured = true;
+
+	tegra_dma_global_resume(tdc);
+}
+
+static void tdc_start_head_req(struct tegra_dma_channel *tdc)
+{
+	struct tegra_dma_sg_req *sg_req;
+
+	if (list_empty(&tdc->pending_sg_req))
+		return;
+
+	sg_req = list_first_entry(&tdc->pending_sg_req,
+					typeof(*sg_req), node);
+	tegra_dma_start(tdc, sg_req);
+	sg_req->configured = true;
+	tdc->busy = true;
+}
+
+static void tdc_configure_next_head_desc(struct tegra_dma_channel *tdc)
+{
+	struct tegra_dma_sg_req *hsgreq;
+	struct tegra_dma_sg_req *hnsgreq;
+
+	if (list_empty(&tdc->pending_sg_req))
+		return;
+
+	hsgreq = list_first_entry(&tdc->pending_sg_req, typeof(*hsgreq), node);
+	if (!list_is_last(&hsgreq->node, &tdc->pending_sg_req)) {
+		hnsgreq = list_first_entry(&hsgreq->node,
+					typeof(*hnsgreq), node);
+		tegra_dma_configure_for_next(tdc, hnsgreq);
+	}
+}
+
+static inline int get_current_xferred_count(struct tegra_dma_channel *tdc,
+	struct tegra_dma_sg_req *sg_req, unsigned long status)
+{
+	return sg_req->req_len - (status & TEGRA_APBDMA_STATUS_COUNT_MASK) - 4;
+}
+
+static void tegra_dma_abort_all(struct tegra_dma_channel *tdc)
+{
+	struct tegra_dma_sg_req *sgreq;
+	struct tegra_dma_desc *dma_desc;
+
+	while (!list_empty(&tdc->pending_sg_req)) {
+		sgreq = list_first_entry(&tdc->pending_sg_req,
+						typeof(*sgreq), node);
+		list_del(&sgreq->node);
+		list_add_tail(&sgreq->node, &tdc->free_sg_req);
+		if (sgreq->last_sg) {
+			dma_desc = sgreq->dma_desc;
+			dma_desc->dma_status = DMA_ERROR;
+			list_add_tail(&dma_desc->node, &tdc->free_dma_desc);
+
+			/* Add in cb list if it is not there. */
+			if (!dma_desc->cb_count)
+				list_add_tail(&dma_desc->cb_node,
+							&tdc->cb_desc);
+			dma_desc->cb_count++;
+		}
+	}
+	tdc->isr_handler = NULL;
+}
+
+static bool handle_continuous_head_request(struct tegra_dma_channel *tdc,
+		struct tegra_dma_sg_req *last_sg_req, bool to_terminate)
+{
+	struct tegra_dma_sg_req *hsgreq = NULL;
+
+	if (list_empty(&tdc->pending_sg_req)) {
+		dev_err(tdc2dev(tdc), "Dma is running without req\n");
+		tegra_dma_stop(tdc);
+		return false;
+	}
+
+	/*
+	 * Check that head req on list should be in flight.
+	 * If it is not in flight then abort transfer as
+	 * looping of transfer can not continue.
+	 */
+	hsgreq = list_first_entry(&tdc->pending_sg_req, typeof(*hsgreq), node);
+	if (!hsgreq->configured) {
+		tegra_dma_stop(tdc);
+		dev_err(tdc2dev(tdc), "Error in dma transfer, aborting dma\n");
+		tegra_dma_abort_all(tdc);
+		return false;
+	}
+
+	/* Configure next request */
+	if (!to_terminate)
+		tdc_configure_next_head_desc(tdc);
+	return true;
+}
+
+static void handle_once_dma_done(struct tegra_dma_channel *tdc,
+	bool to_terminate)
+{
+	struct tegra_dma_sg_req *sgreq;
+	struct tegra_dma_desc *dma_desc;
+
+	tdc->busy = false;
+	sgreq = list_first_entry(&tdc->pending_sg_req, typeof(*sgreq), node);
+	dma_desc = sgreq->dma_desc;
+	dma_desc->bytes_transferred += sgreq->req_len;
+
+	list_del(&sgreq->node);
+	if (sgreq->last_sg) {
+		dma_desc->dma_status = DMA_SUCCESS;
+		dma_cookie_complete(&dma_desc->txd);
+		if (!dma_desc->cb_count)
+			list_add_tail(&dma_desc->cb_node, &tdc->cb_desc);
+		dma_desc->cb_count++;
+		list_add_tail(&dma_desc->node, &tdc->free_dma_desc);
+	}
+	list_add_tail(&sgreq->node, &tdc->free_sg_req);
+
+	/* Do not start DMA if it is going to be terminate */
+	if (to_terminate || list_empty(&tdc->pending_sg_req))
+		return;
+
+	tdc_start_head_req(tdc);
+	return;
+}
+
+static void handle_cont_sngl_cycle_dma_done(struct tegra_dma_channel *tdc,
+		bool to_terminate)
+{
+	struct tegra_dma_sg_req *sgreq;
+	struct tegra_dma_desc *dma_desc;
+	bool st;
+
+	sgreq = list_first_entry(&tdc->pending_sg_req, typeof(*sgreq), node);
+	dma_desc = sgreq->dma_desc;
+	dma_desc->bytes_transferred += sgreq->req_len;
+
+	/* Callback need to be call */
+	if (!dma_desc->cb_count)
+		list_add_tail(&dma_desc->cb_node, &tdc->cb_desc);
+	dma_desc->cb_count++;
+
+	/* If not last req then put at end of pending list */
+	if (!list_is_last(&sgreq->node, &tdc->pending_sg_req)) {
+		list_del(&sgreq->node);
+		list_add_tail(&sgreq->node, &tdc->pending_sg_req);
+		sgreq->configured = false;
+		st = handle_continuous_head_request(tdc, sgreq, to_terminate);
+		if (!st)
+			dma_desc->dma_status = DMA_ERROR;
+	}
+	return;
+}
+
+static void tegra_dma_tasklet(unsigned long data)
+{
+	struct tegra_dma_channel *tdc = (struct tegra_dma_channel *)data;
+	dma_async_tx_callback callback = NULL;
+	void *callback_param = NULL;
+	struct tegra_dma_desc *dma_desc;
+	unsigned long flags;
+	int cb_count;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	while (!list_empty(&tdc->cb_desc)) {
+		dma_desc  = list_first_entry(&tdc->cb_desc,
+					typeof(*dma_desc), cb_node);
+		list_del(&dma_desc->cb_node);
+		callback = dma_desc->txd.callback;
+		callback_param = dma_desc->txd.callback_param;
+		cb_count = dma_desc->cb_count;
+		dma_desc->cb_count = 0;
+		spin_unlock_irqrestore(&tdc->lock, flags);
+		while (cb_count-- && callback)
+			callback(callback_param);
+		spin_lock_irqsave(&tdc->lock, flags);
+	}
+	spin_unlock_irqrestore(&tdc->lock, flags);
+}
+
+static irqreturn_t tegra_dma_isr(int irq, void *dev_id)
+{
+	struct tegra_dma_channel *tdc = dev_id;
+	unsigned long status;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+
+	status = tdc_read(tdc, TEGRA_APBDMA_CHAN_STATUS);
+	if (status & TEGRA_APBDMA_STATUS_ISE_EOC) {
+		tdc_write(tdc, TEGRA_APBDMA_CHAN_STATUS, status);
+		tdc->isr_handler(tdc, false);
+		tasklet_schedule(&tdc->tasklet);
+		spin_unlock_irqrestore(&tdc->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	spin_unlock_irqrestore(&tdc->lock, flags);
+	dev_info(tdc2dev(tdc),
+		"Interrupt already served status 0x%08lx\n", status);
+	return IRQ_NONE;
+}
+
+static dma_cookie_t tegra_dma_tx_submit(struct dma_async_tx_descriptor *txd)
+{
+	struct tegra_dma_desc *dma_desc = txd_to_tegra_dma_desc(txd);
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(txd->chan);
+	unsigned long flags;
+	dma_cookie_t cookie;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	dma_desc->dma_status = DMA_IN_PROGRESS;
+	cookie = dma_cookie_assign(&dma_desc->txd);
+	list_splice_tail_init(&dma_desc->tx_list, &tdc->pending_sg_req);
+	spin_unlock_irqrestore(&tdc->lock, flags);
+	return cookie;
+}
+
+static void tegra_dma_issue_pending(struct dma_chan *dc)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	if (list_empty(&tdc->pending_sg_req)) {
+		dev_err(tdc2dev(tdc), "No DMA request\n");
+		goto end;
+	}
+	if (!tdc->busy) {
+		tdc_start_head_req(tdc);
+
+		/* Continuous single mode: Configure next req */
+		if (tdc->cyclic) {
+			/*
+			 * Wait for 1 burst time for configure DMA for
+			 * next transfer.
+			 */
+			udelay(TEGRA_APBDMA_BURST_COMPLETE_TIME);
+			tdc_configure_next_head_desc(tdc);
+		}
+	}
+end:
+	spin_unlock_irqrestore(&tdc->lock, flags);
+	return;
+}
+
+static void tegra_dma_terminate_all(struct dma_chan *dc)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+	struct tegra_dma_sg_req *sgreq;
+	struct tegra_dma_desc *dma_desc;
+	unsigned long flags;
+	unsigned long status;
+	bool was_busy;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	if (list_empty(&tdc->pending_sg_req)) {
+		spin_unlock_irqrestore(&tdc->lock, flags);
+		return;
+	}
+
+	if (!tdc->busy)
+		goto skip_dma_stop;
+
+	/* Pause DMA before checking the queue status */
+	tegra_dma_global_pause(tdc, true);
+
+	status = tdc_read(tdc, TEGRA_APBDMA_CHAN_STATUS);
+	if (status & TEGRA_APBDMA_STATUS_ISE_EOC) {
+		dev_dbg(tdc2dev(tdc), "%s():handling isr\n", __func__);
+		tdc->isr_handler(tdc, true);
+		status = tdc_read(tdc, TEGRA_APBDMA_CHAN_STATUS);
+	}
+
+	was_busy = tdc->busy;
+	tegra_dma_stop(tdc);
+
+	if (!list_empty(&tdc->pending_sg_req) && was_busy) {
+		sgreq = list_first_entry(&tdc->pending_sg_req,
+					typeof(*sgreq), node);
+		sgreq->dma_desc->bytes_transferred +=
+				get_current_xferred_count(tdc, sgreq, status);
+	}
+	tegra_dma_global_resume(tdc);
+
+skip_dma_stop:
+	tegra_dma_abort_all(tdc);
+
+	while (!list_empty(&tdc->cb_desc)) {
+		dma_desc  = list_first_entry(&tdc->cb_desc,
+					typeof(*dma_desc), cb_node);
+		list_del(&dma_desc->cb_node);
+		dma_desc->cb_count = 0;
+	}
+	spin_unlock_irqrestore(&tdc->lock, flags);
+}
+
+static enum dma_status tegra_dma_tx_status(struct dma_chan *dc,
+	dma_cookie_t cookie, struct dma_tx_state *txstate)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+	struct tegra_dma_desc *dma_desc;
+	struct tegra_dma_sg_req *sg_req;
+	enum dma_status ret;
+	unsigned long flags;
+	unsigned int residual;
+
+	spin_lock_irqsave(&tdc->lock, flags);
+
+	ret = dma_cookie_status(dc, cookie, txstate);
+	if (ret == DMA_SUCCESS) {
+		dma_set_residue(txstate, 0);
+		spin_unlock_irqrestore(&tdc->lock, flags);
+		return ret;
+	}
+
+	/* Check on wait_ack desc status */
+	list_for_each_entry(dma_desc, &tdc->free_dma_desc, node) {
+		if (dma_desc->txd.cookie == cookie) {
+			residual =  dma_desc->bytes_requested -
+					(dma_desc->bytes_transferred %
+						dma_desc->bytes_requested);
+			dma_set_residue(txstate, residual);
+			ret = dma_desc->dma_status;
+			spin_unlock_irqrestore(&tdc->lock, flags);
+			return ret;
+		}
+	}
+
+	/* Check in pending list */
+	list_for_each_entry(sg_req, &tdc->pending_sg_req, node) {
+		dma_desc = sg_req->dma_desc;
+		if (dma_desc->txd.cookie == cookie) {
+			residual =  dma_desc->bytes_requested -
+					(dma_desc->bytes_transferred %
+						dma_desc->bytes_requested);
+			dma_set_residue(txstate, residual);
+			ret = dma_desc->dma_status;
+			spin_unlock_irqrestore(&tdc->lock, flags);
+			return ret;
+		}
+	}
+
+	dev_dbg(tdc2dev(tdc), "cookie %d does not found\n", cookie);
+	spin_unlock_irqrestore(&tdc->lock, flags);
+	return ret;
+}
+
+static int tegra_dma_device_control(struct dma_chan *dc, enum dma_ctrl_cmd cmd,
+			unsigned long arg)
+{
+	switch (cmd) {
+	case DMA_SLAVE_CONFIG:
+		return tegra_dma_slave_config(dc,
+				(struct dma_slave_config *)arg);
+
+	case DMA_TERMINATE_ALL:
+		tegra_dma_terminate_all(dc);
+		return 0;
+
+	default:
+		break;
+	}
+
+	return -ENXIO;
+}
+
+static inline int get_bus_width(struct tegra_dma_channel *tdc,
+		enum dma_slave_buswidth slave_bw)
+{
+	switch (slave_bw) {
+	case DMA_SLAVE_BUSWIDTH_1_BYTE:
+		return TEGRA_APBDMA_APBSEQ_BUS_WIDTH_8;
+	case DMA_SLAVE_BUSWIDTH_2_BYTES:
+		return TEGRA_APBDMA_APBSEQ_BUS_WIDTH_16;
+	case DMA_SLAVE_BUSWIDTH_4_BYTES:
+		return TEGRA_APBDMA_APBSEQ_BUS_WIDTH_32;
+	case DMA_SLAVE_BUSWIDTH_8_BYTES:
+		return TEGRA_APBDMA_APBSEQ_BUS_WIDTH_64;
+	default:
+		dev_warn(tdc2dev(tdc),
+			"slave bw is not supported, using 32bits\n");
+		return TEGRA_APBDMA_APBSEQ_BUS_WIDTH_32;
+	}
+}
+
+static inline int get_burst_size(struct tegra_dma_channel *tdc,
+	u32 burst_size, enum dma_slave_buswidth slave_bw, int len)
+{
+	int burst_byte;
+	int burst_ahb_width;
+
+	/*
+	 * burst_size from client is in terms of the bus_width.
+	 * convert them into AHB memory width which is 4 byte.
+	 */
+	burst_byte = burst_size * slave_bw;
+	burst_ahb_width = burst_byte / 4;
+
+	/* If burst size is 0 then calculate the burst size based on length */
+	if (!burst_ahb_width) {
+		if (len & 0xF)
+			return TEGRA_APBDMA_AHBSEQ_BURST_1;
+		else if ((len >> 4) & 0x1)
+			return TEGRA_APBDMA_AHBSEQ_BURST_4;
+		else
+			return TEGRA_APBDMA_AHBSEQ_BURST_8;
+	}
+	if (burst_ahb_width < 4)
+		return TEGRA_APBDMA_AHBSEQ_BURST_1;
+	else if (burst_ahb_width < 8)
+		return TEGRA_APBDMA_AHBSEQ_BURST_4;
+	else
+		return TEGRA_APBDMA_AHBSEQ_BURST_8;
+}
+
+static int get_transfer_param(struct tegra_dma_channel *tdc,
+	enum dma_transfer_direction direction, unsigned long *apb_addr,
+	unsigned long *apb_seq,	unsigned long *csr, unsigned int *burst_size,
+	enum dma_slave_buswidth *slave_bw)
+{
+
+	switch (direction) {
+	case DMA_MEM_TO_DEV:
+		*apb_addr = tdc->dma_sconfig.dst_addr;
+		*apb_seq = get_bus_width(tdc, tdc->dma_sconfig.dst_addr_width);
+		*burst_size = tdc->dma_sconfig.dst_maxburst;
+		*slave_bw = tdc->dma_sconfig.dst_addr_width;
+		*csr = TEGRA_APBDMA_CSR_DIR;
+		return 0;
+
+	case DMA_DEV_TO_MEM:
+		*apb_addr = tdc->dma_sconfig.src_addr;
+		*apb_seq = get_bus_width(tdc, tdc->dma_sconfig.src_addr_width);
+		*burst_size = tdc->dma_sconfig.src_maxburst;
+		*slave_bw = tdc->dma_sconfig.src_addr_width;
+		*csr = 0;
+		return 0;
+
+	default:
+		dev_err(tdc2dev(tdc), "Dma direction is not supported\n");
+		return -EINVAL;
+	}
+	return -EINVAL;
+}
+
+static struct dma_async_tx_descriptor *tegra_dma_prep_slave_sg(
+	struct dma_chan *dc, struct scatterlist *sgl, unsigned int sg_len,
+	enum dma_transfer_direction direction, unsigned long flags,
+	void *context)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+	struct tegra_dma_desc *dma_desc;
+	unsigned int	    i;
+	struct scatterlist      *sg;
+	unsigned long csr, ahb_seq, apb_ptr, apb_seq;
+	struct list_head req_list;
+	struct tegra_dma_sg_req  *sg_req = NULL;
+	u32 burst_size;
+	enum dma_slave_buswidth slave_bw;
+	int ret;
+
+	if (!tdc->config_init) {
+		dev_err(tdc2dev(tdc), "dma channel is not configured\n");
+		return NULL;
+	}
+	if (sg_len < 1) {
+		dev_err(tdc2dev(tdc), "Invalid segment length %d\n", sg_len);
+		return NULL;
+	}
+
+	ret = get_transfer_param(tdc, direction, &apb_ptr, &apb_seq, &csr,
+				&burst_size, &slave_bw);
+	if (ret < 0)
+		return NULL;
+
+	INIT_LIST_HEAD(&req_list);
+
+	ahb_seq = TEGRA_APBDMA_AHBSEQ_INTR_ENB;
+	ahb_seq |= TEGRA_APBDMA_AHBSEQ_WRAP_NONE <<
+					TEGRA_APBDMA_AHBSEQ_WRAP_SHIFT;
+	ahb_seq |= TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_32;
+
+	csr |= TEGRA_APBDMA_CSR_ONCE | TEGRA_APBDMA_CSR_FLOW;
+	csr |= tdc->dma_sconfig.slave_id << TEGRA_APBDMA_CSR_REQ_SEL_SHIFT;
+	if (flags & DMA_PREP_INTERRUPT)
+		csr |= TEGRA_APBDMA_CSR_IE_EOC;
+
+	apb_seq |= TEGRA_APBDMA_APBSEQ_WRAP_WORD_1;
+
+	dma_desc = tegra_dma_desc_get(tdc);
+	if (!dma_desc) {
+		dev_err(tdc2dev(tdc), "Dma descriptors not available\n");
+		return NULL;
+	}
+	INIT_LIST_HEAD(&dma_desc->tx_list);
+	INIT_LIST_HEAD(&dma_desc->cb_node);
+	dma_desc->cb_count = 0;
+	dma_desc->bytes_requested = 0;
+	dma_desc->bytes_transferred = 0;
+	dma_desc->dma_status = DMA_IN_PROGRESS;
+
+	/* Make transfer requests */
+	for_each_sg(sgl, sg, sg_len, i) {
+		u32 len, mem;
+
+		mem = sg_dma_address(sg);
+		len = sg_dma_len(sg);
+
+		if ((len & 3) || (mem & 3) ||
+				(len > tdc->tdma->chip_data->max_dma_count)) {
+			dev_err(tdc2dev(tdc),
+				"Dma length/memory address is not supported\n");
+			tegra_dma_desc_put(tdc, dma_desc);
+			return NULL;
+		}
+
+		sg_req = tegra_dma_sg_req_get(tdc);
+		if (!sg_req) {
+			dev_err(tdc2dev(tdc), "Dma sg-req not available\n");
+			tegra_dma_desc_put(tdc, dma_desc);
+			return NULL;
+		}
+
+		ahb_seq |= get_burst_size(tdc, burst_size, slave_bw, len);
+		dma_desc->bytes_requested += len;
+
+		sg_req->ch_regs.apb_ptr = apb_ptr;
+		sg_req->ch_regs.ahb_ptr = mem;
+		sg_req->ch_regs.csr = csr | ((len - 4) & 0xFFFC);
+		sg_req->ch_regs.apb_seq = apb_seq;
+		sg_req->ch_regs.ahb_seq = ahb_seq;
+		sg_req->configured = false;
+		sg_req->last_sg = false;
+		sg_req->dma_desc = dma_desc;
+		sg_req->req_len = len;
+
+		list_add_tail(&sg_req->node, &dma_desc->tx_list);
+	}
+	sg_req->last_sg = true;
+	if (flags & DMA_CTRL_ACK)
+		dma_desc->txd.flags = DMA_CTRL_ACK;
+
+	/*
+	 * Make sure that mode should not be conflicting with currently
+	 * configured mode.
+	 */
+	if (!tdc->isr_handler) {
+		tdc->isr_handler = handle_once_dma_done;
+		tdc->cyclic = false;
+	} else {
+		if (tdc->cyclic) {
+			dev_err(tdc2dev(tdc), "DMA configured in cyclic mode\n");
+			tegra_dma_desc_put(tdc, dma_desc);
+			return NULL;
+		}
+	}
+
+	return &dma_desc->txd;
+}
+
+struct dma_async_tx_descriptor *tegra_dma_prep_dma_cyclic(
+	struct dma_chan *dc, dma_addr_t buf_addr, size_t buf_len,
+	size_t period_len, enum dma_transfer_direction direction,
+	void *context)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+	struct tegra_dma_desc *dma_desc = NULL;
+	struct tegra_dma_sg_req  *sg_req = NULL;
+	unsigned long csr, ahb_seq, apb_ptr, apb_seq;
+	int len;
+	size_t remain_len;
+	dma_addr_t mem = buf_addr;
+	u32 burst_size;
+	enum dma_slave_buswidth slave_bw;
+	int ret;
+
+	if (!buf_len || !period_len) {
+		dev_err(tdc2dev(tdc), "Invalid buffer/period len\n");
+		return NULL;
+	}
+
+	if (!tdc->config_init) {
+		dev_err(tdc2dev(tdc), "DMA slave is not configured\n");
+		return NULL;
+	}
+
+	/*
+	 * We allow to take more number of requests till DMA is
+	 * not started. The driver will loop over all requests.
+	 * Once DMA is started then new requests can be queued only after
+	 * terminating the DMA.
+	 */
+	if (tdc->busy) {
+		dev_err(tdc2dev(tdc), "Request not allowed when dma running\n");
+		return NULL;
+	}
+
+	/*
+	 * We only support cycle transfer when buf_len is multiple of
+	 * period_len.
+	 */
+	if (buf_len % period_len) {
+		dev_err(tdc2dev(tdc), "buf_len is not multiple of period_len\n");
+		return NULL;
+	}
+
+	len = period_len;
+	if ((len & 3) || (buf_addr & 3) ||
+			(len > tdc->tdma->chip_data->max_dma_count)) {
+		dev_err(tdc2dev(tdc), "Req len/mem address is not correct\n");
+		return NULL;
+	}
+
+	ret = get_transfer_param(tdc, direction, &apb_ptr, &apb_seq, &csr,
+				&burst_size, &slave_bw);
+	if (ret < 0)
+		return NULL;
+
+
+	ahb_seq = TEGRA_APBDMA_AHBSEQ_INTR_ENB;
+	ahb_seq |= TEGRA_APBDMA_AHBSEQ_WRAP_NONE <<
+					TEGRA_APBDMA_AHBSEQ_WRAP_SHIFT;
+	ahb_seq |= TEGRA_APBDMA_AHBSEQ_BUS_WIDTH_32;
+
+	csr |= TEGRA_APBDMA_CSR_FLOW | TEGRA_APBDMA_CSR_IE_EOC;
+	csr |= tdc->dma_sconfig.slave_id << TEGRA_APBDMA_CSR_REQ_SEL_SHIFT;
+
+	apb_seq |= TEGRA_APBDMA_APBSEQ_WRAP_WORD_1;
+
+	dma_desc = tegra_dma_desc_get(tdc);
+	if (!dma_desc) {
+		dev_err(tdc2dev(tdc), "not enough descriptors available\n");
+		return NULL;
+	}
+
+	INIT_LIST_HEAD(&dma_desc->tx_list);
+	INIT_LIST_HEAD(&dma_desc->cb_node);
+	dma_desc->cb_count = 0;
+
+	dma_desc->bytes_transferred = 0;
+	dma_desc->bytes_requested = buf_len;
+	remain_len = buf_len;
+
+	/* Split transfer equal to period size */
+	while (remain_len) {
+		sg_req = tegra_dma_sg_req_get(tdc);
+		if (!sg_req) {
+			dev_err(tdc2dev(tdc), "Dma sg-req not available\n");
+			tegra_dma_desc_put(tdc, dma_desc);
+			return NULL;
+		}
+
+		ahb_seq |= get_burst_size(tdc, burst_size, slave_bw, len);
+		sg_req->ch_regs.apb_ptr = apb_ptr;
+		sg_req->ch_regs.ahb_ptr = mem;
+		sg_req->ch_regs.csr = csr | ((len - 4) & 0xFFFC);
+		sg_req->ch_regs.apb_seq = apb_seq;
+		sg_req->ch_regs.ahb_seq = ahb_seq;
+		sg_req->configured = false;
+		sg_req->half_done = false;
+		sg_req->last_sg = false;
+		sg_req->dma_desc = dma_desc;
+		sg_req->req_len = len;
+
+		list_add_tail(&sg_req->node, &dma_desc->tx_list);
+		remain_len -= len;
+		mem += len;
+	}
+	sg_req->last_sg = true;
+	dma_desc->txd.flags = 0;
+
+	/*
+	 * Make sure that mode should not be conflicting with currently
+	 * configured mode.
+	 */
+	if (!tdc->isr_handler) {
+		tdc->isr_handler = handle_cont_sngl_cycle_dma_done;
+		tdc->cyclic = true;
+	} else {
+		if (!tdc->cyclic) {
+			dev_err(tdc2dev(tdc), "DMA configuration conflict\n");
+			tegra_dma_desc_put(tdc, dma_desc);
+			return NULL;
+		}
+	}
+
+	return &dma_desc->txd;
+}
+
+static int tegra_dma_alloc_chan_resources(struct dma_chan *dc)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+
+	dma_cookie_init(&tdc->dma_chan);
+	tdc->config_init = false;
+	return 0;
+}
+
+static void tegra_dma_free_chan_resources(struct dma_chan *dc)
+{
+	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
+
+	struct tegra_dma_desc *dma_desc;
+	struct tegra_dma_sg_req *sg_req;
+	struct list_head dma_desc_list;
+	struct list_head sg_req_list;
+	unsigned long flags;
+
+	INIT_LIST_HEAD(&dma_desc_list);
+	INIT_LIST_HEAD(&sg_req_list);
+
+	dev_dbg(tdc2dev(tdc), "Freeing channel %d\n", tdc->id);
+
+	if (tdc->busy)
+		tegra_dma_terminate_all(dc);
+
+	spin_lock_irqsave(&tdc->lock, flags);
+	list_splice_init(&tdc->pending_sg_req, &sg_req_list);
+	list_splice_init(&tdc->free_sg_req, &sg_req_list);
+	list_splice_init(&tdc->free_dma_desc, &dma_desc_list);
+	INIT_LIST_HEAD(&tdc->cb_desc);
+	tdc->config_init = false;
+	spin_unlock_irqrestore(&tdc->lock, flags);
+
+	while (!list_empty(&dma_desc_list)) {
+		dma_desc = list_first_entry(&dma_desc_list,
+					typeof(*dma_desc), node);
+		list_del(&dma_desc->node);
+		kfree(dma_desc);
+	}
+
+	while (!list_empty(&sg_req_list)) {
+		sg_req = list_first_entry(&sg_req_list, typeof(*sg_req), node);
+		list_del(&sg_req->node);
+		kfree(sg_req);
+	}
+}
+
+/* Tegra20 specific DMA controller information */
+static struct tegra_dma_chip_data tegra20_dma_chip_data = {
+	.nr_channels		= 16,
+	.max_dma_count		= 1024UL * 64,
+};
+
+#if defined(CONFIG_OF)
+/* Tegra30 specific DMA controller information */
+static struct tegra_dma_chip_data tegra30_dma_chip_data = {
+	.nr_channels		= 32,
+	.max_dma_count		= 1024UL * 64,
+};
+
+static const struct of_device_id tegra_dma_of_match[] __devinitconst = {
+	{
+		.compatible = "nvidia,tegra30-apbdma",
+		.data = &tegra30_dma_chip_data,
+	}, {
+		.compatible = "nvidia,tegra20-apbdma",
+		.data = &tegra20_dma_chip_data,
+	}, {
+	},
+};
+MODULE_DEVICE_TABLE(of, tegra_dma_of_match);
+#endif
+
+static int __devinit tegra_dma_probe(struct platform_device *pdev)
+{
+	struct resource	*res;
+	struct tegra_dma *tdma;
+	int ret;
+	int i;
+	struct tegra_dma_chip_data *cdata = NULL;
+
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_device(of_match_ptr(tegra_dma_of_match),
+					&pdev->dev);
+		if (!match) {
+			dev_err(&pdev->dev, "Error: No device match found\n");
+			return -ENODEV;
+		}
+		cdata = match->data;
+	} else {
+		/* If no device tree then fallback to tegra20 */
+		cdata = &tegra20_dma_chip_data;
+	}
+
+	tdma = devm_kzalloc(&pdev->dev, sizeof(*tdma) + cdata->nr_channels *
+			sizeof(struct tegra_dma_channel), GFP_KERNEL);
+	if (!tdma) {
+		dev_err(&pdev->dev, "Error: memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	tdma->dev = &pdev->dev;
+	tdma->chip_data = cdata;
+	platform_set_drvdata(pdev, tdma);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "No mem resource for DMA\n");
+		return -EINVAL;
+	}
+
+	tdma->base_addr = devm_request_and_ioremap(&pdev->dev, res);
+	if (!tdma->base_addr) {
+		dev_err(&pdev->dev,
+			"Cannot request memregion/iomap dma address\n");
+		return -EADDRNOTAVAIL;
+	}
+
+	tdma->dma_clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(tdma->dma_clk)) {
+		dev_err(&pdev->dev, "Error: Missing controller clock\n");
+		return PTR_ERR(tdma->dma_clk);
+	}
+
+	spin_lock_init(&tdma->global_lock);
+
+	pm_runtime_enable(&pdev->dev);
+	if (!pm_runtime_enabled(&pdev->dev)) {
+		ret = tegra_dma_runtime_resume(&pdev->dev);
+		if (ret) {
+			dev_err(&pdev->dev, "dma_runtime_resume failed %d\n",
+				ret);
+			goto err_pm_disable;
+		}
+	}
+
+	/* Reset DMA controller */
+	tegra_periph_reset_assert(tdma->dma_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(tdma->dma_clk);
+
+	/* Enable global DMA registers */
+	tdma_write(tdma, TEGRA_APBDMA_GENERAL, TEGRA_APBDMA_GENERAL_ENABLE);
+	tdma_write(tdma, TEGRA_APBDMA_CONTROL, 0);
+	tdma_write(tdma, TEGRA_APBDMA_IRQ_MASK_SET, 0xFFFFFFFFul);
+
+	INIT_LIST_HEAD(&tdma->dma_dev.channels);
+	for (i = 0; i < cdata->nr_channels; i++) {
+		struct tegra_dma_channel *tdc = &tdma->channels[i];
+		char irq_name[30];
+
+		tdc->chan_base_offset = TEGRA_APBDMA_CHANNEL_BASE_ADD_OFFSET +
+					i * TEGRA_APBDMA_CHANNEL_REGISTER_SIZE;
+
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+		if (!res) {
+			ret = -EINVAL;
+			dev_err(&pdev->dev, "No irq resource for chan %d\n", i);
+			goto err_irq;
+		}
+		tdc->irq = res->start;
+		snprintf(irq_name, sizeof(irq_name), "apbdma.%d", i);
+		ret = devm_request_irq(&pdev->dev, tdc->irq,
+				tegra_dma_isr, 0, irq_name, tdc);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"request_irq failed with err %d channel %d\n",
+				i, ret);
+			goto err_irq;
+		}
+
+		tdc->dma_chan.device = &tdma->dma_dev;
+		dma_cookie_init(&tdc->dma_chan);
+		list_add_tail(&tdc->dma_chan.device_node,
+				&tdma->dma_dev.channels);
+		tdc->tdma = tdma;
+		tdc->id = i;
+
+		tasklet_init(&tdc->tasklet, tegra_dma_tasklet,
+				(unsigned long)tdc);
+		spin_lock_init(&tdc->lock);
+
+		INIT_LIST_HEAD(&tdc->pending_sg_req);
+		INIT_LIST_HEAD(&tdc->free_sg_req);
+		INIT_LIST_HEAD(&tdc->free_dma_desc);
+		INIT_LIST_HEAD(&tdc->cb_desc);
+	}
+
+	dma_cap_set(DMA_SLAVE, tdma->dma_dev.cap_mask);
+	dma_cap_set(DMA_PRIVATE, tdma->dma_dev.cap_mask);
+	dma_cap_set(DMA_CYCLIC, tdma->dma_dev.cap_mask);
+
+	tdma->dma_dev.dev = &pdev->dev;
+	tdma->dma_dev.device_alloc_chan_resources =
+					tegra_dma_alloc_chan_resources;
+	tdma->dma_dev.device_free_chan_resources =
+					tegra_dma_free_chan_resources;
+	tdma->dma_dev.device_prep_slave_sg = tegra_dma_prep_slave_sg;
+	tdma->dma_dev.device_prep_dma_cyclic = tegra_dma_prep_dma_cyclic;
+	tdma->dma_dev.device_control = tegra_dma_device_control;
+	tdma->dma_dev.device_tx_status = tegra_dma_tx_status;
+	tdma->dma_dev.device_issue_pending = tegra_dma_issue_pending;
+
+	ret = dma_async_device_register(&tdma->dma_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"Tegra20 APB DMA driver registration failed %d\n", ret);
+		goto err_irq;
+	}
+
+	dev_info(&pdev->dev, "Tegra20 APB DMA driver register %d channels\n",
+			cdata->nr_channels);
+	return 0;
+
+err_irq:
+	while (--i >= 0) {
+		struct tegra_dma_channel *tdc = &tdma->channels[i];
+		tasklet_kill(&tdc->tasklet);
+	}
+
+err_pm_disable:
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		tegra_dma_runtime_suspend(&pdev->dev);
+	return ret;
+}
+
+static int __devexit tegra_dma_remove(struct platform_device *pdev)
+{
+	struct tegra_dma *tdma = platform_get_drvdata(pdev);
+	int i;
+	struct tegra_dma_channel *tdc;
+
+	dma_async_device_unregister(&tdma->dma_dev);
+
+	for (i = 0; i < tdma->chip_data->nr_channels; ++i) {
+		tdc = &tdma->channels[i];
+		tasklet_kill(&tdc->tasklet);
+	}
+
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		tegra_dma_runtime_suspend(&pdev->dev);
+
+	return 0;
+}
+
+static int tegra_dma_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_dma *tdma = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(tdma->dma_clk);
+	return 0;
+}
+
+static int tegra_dma_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_dma *tdma = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = clk_prepare_enable(tdma->dma_clk);
+	if (ret < 0) {
+		dev_err(dev, "clk_enable failed: %d\n", ret);
+		return ret;
+	}
+	return 0;
+}
+
+static const struct dev_pm_ops tegra_dma_dev_pm_ops __devinitconst = {
+#ifdef CONFIG_PM_RUNTIME
+	.runtime_suspend = tegra_dma_runtime_suspend,
+	.runtime_resume = tegra_dma_runtime_resume,
+#endif
+};
+
+static struct platform_driver tegra_dmac_driver = {
+	.driver = {
+		.name	= "tegra-apbdma",
+		.owner = THIS_MODULE,
+		.pm	= &tegra_dma_dev_pm_ops,
+		.of_match_table = of_match_ptr(tegra_dma_of_match),
+	},
+	.probe		= tegra_dma_probe,
+	.remove		= __devexit_p(tegra_dma_remove),
+};
+
+module_platform_driver(tegra_dmac_driver);
+
+MODULE_ALIAS("platform:tegra20-apbdma");
+MODULE_DESCRIPTION("NVIDIA Tegra APB DMA Controller driver");
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/virt-dma.c b/drivers/dma/virt-dma.c
new file mode 100644
index 00000000000..6f80432a3f0
--- /dev/null
+++ b/drivers/dma/virt-dma.c
@@ -0,0 +1,123 @@
+/*
+ * Virtual DMA channel support for DMAengine
+ *
+ * Copyright (C) 2012 Russell King
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/device.h>
+#include <linux/dmaengine.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+
+#include "virt-dma.h"
+
+static struct virt_dma_desc *to_virt_desc(struct dma_async_tx_descriptor *tx)
+{
+	return container_of(tx, struct virt_dma_desc, tx);
+}
+
+dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct virt_dma_chan *vc = to_virt_chan(tx->chan);
+	struct virt_dma_desc *vd = to_virt_desc(tx);
+	unsigned long flags;
+	dma_cookie_t cookie;
+
+	spin_lock_irqsave(&vc->lock, flags);
+	cookie = dma_cookie_assign(tx);
+
+	list_add_tail(&vd->node, &vc->desc_submitted);
+	spin_unlock_irqrestore(&vc->lock, flags);
+
+	dev_dbg(vc->chan.device->dev, "vchan %p: txd %p[%x]: submitted\n",
+		vc, vd, cookie);
+
+	return cookie;
+}
+EXPORT_SYMBOL_GPL(vchan_tx_submit);
+
+struct virt_dma_desc *vchan_find_desc(struct virt_dma_chan *vc,
+	dma_cookie_t cookie)
+{
+	struct virt_dma_desc *vd;
+
+	list_for_each_entry(vd, &vc->desc_issued, node)
+		if (vd->tx.cookie == cookie)
+			return vd;
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(vchan_find_desc);
+
+/*
+ * This tasklet handles the completion of a DMA descriptor by
+ * calling its callback and freeing it.
+ */
+static void vchan_complete(unsigned long arg)
+{
+	struct virt_dma_chan *vc = (struct virt_dma_chan *)arg;
+	struct virt_dma_desc *vd;
+	dma_async_tx_callback cb = NULL;
+	void *cb_data = NULL;
+	LIST_HEAD(head);
+
+	spin_lock_irq(&vc->lock);
+	list_splice_tail_init(&vc->desc_completed, &head);
+	vd = vc->cyclic;
+	if (vd) {
+		vc->cyclic = NULL;
+		cb = vd->tx.callback;
+		cb_data = vd->tx.callback_param;
+	}
+	spin_unlock_irq(&vc->lock);
+
+	if (cb)
+		cb(cb_data);
+
+	while (!list_empty(&head)) {
+		vd = list_first_entry(&head, struct virt_dma_desc, node);
+		cb = vd->tx.callback;
+		cb_data = vd->tx.callback_param;
+
+		list_del(&vd->node);
+
+		vc->desc_free(vd);
+
+		if (cb)
+			cb(cb_data);
+	}
+}
+
+void vchan_dma_desc_free_list(struct virt_dma_chan *vc, struct list_head *head)
+{
+	while (!list_empty(head)) {
+		struct virt_dma_desc *vd = list_first_entry(head,
+			struct virt_dma_desc, node);
+		list_del(&vd->node);
+		dev_dbg(vc->chan.device->dev, "txd %p: freeing\n", vd);
+		vc->desc_free(vd);
+	}
+}
+EXPORT_SYMBOL_GPL(vchan_dma_desc_free_list);
+
+void vchan_init(struct virt_dma_chan *vc, struct dma_device *dmadev)
+{
+	dma_cookie_init(&vc->chan);
+
+	spin_lock_init(&vc->lock);
+	INIT_LIST_HEAD(&vc->desc_submitted);
+	INIT_LIST_HEAD(&vc->desc_issued);
+	INIT_LIST_HEAD(&vc->desc_completed);
+
+	tasklet_init(&vc->task, vchan_complete, (unsigned long)vc);
+
+	vc->chan.device = dmadev;
+	list_add_tail(&vc->chan.device_node, &dmadev->channels);
+}
+EXPORT_SYMBOL_GPL(vchan_init);
+
+MODULE_AUTHOR("Russell King");
+MODULE_LICENSE("GPL");
diff --git a/drivers/dma/virt-dma.h b/drivers/dma/virt-dma.h
new file mode 100644
index 00000000000..85c19d63f9f
--- /dev/null
+++ b/drivers/dma/virt-dma.h
@@ -0,0 +1,152 @@
+/*
+ * Virtual DMA channel support for DMAengine
+ *
+ * Copyright (C) 2012 Russell King
+ *
+ * This program is free software; you can 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 VIRT_DMA_H
+#define VIRT_DMA_H
+
+#include <linux/dmaengine.h>
+#include <linux/interrupt.h>
+
+#include "dmaengine.h"
+
+struct virt_dma_desc {
+	struct dma_async_tx_descriptor tx;
+	/* protected by vc.lock */
+	struct list_head node;
+};
+
+struct virt_dma_chan {
+	struct dma_chan	chan;
+	struct tasklet_struct task;
+	void (*desc_free)(struct virt_dma_desc *);
+
+	spinlock_t lock;
+
+	/* protected by vc.lock */
+	struct list_head desc_submitted;
+	struct list_head desc_issued;
+	struct list_head desc_completed;
+
+	struct virt_dma_desc *cyclic;
+};
+
+static inline struct virt_dma_chan *to_virt_chan(struct dma_chan *chan)
+{
+	return container_of(chan, struct virt_dma_chan, chan);
+}
+
+void vchan_dma_desc_free_list(struct virt_dma_chan *vc, struct list_head *head);
+void vchan_init(struct virt_dma_chan *vc, struct dma_device *dmadev);
+struct virt_dma_desc *vchan_find_desc(struct virt_dma_chan *, dma_cookie_t);
+
+/**
+ * vchan_tx_prep - prepare a descriptor
+ * vc: virtual channel allocating this descriptor
+ * vd: virtual descriptor to prepare
+ * tx_flags: flags argument passed in to prepare function
+ */
+static inline struct dma_async_tx_descriptor *vchan_tx_prep(struct virt_dma_chan *vc,
+	struct virt_dma_desc *vd, unsigned long tx_flags)
+{
+	extern dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *);
+
+	dma_async_tx_descriptor_init(&vd->tx, &vc->chan);
+	vd->tx.flags = tx_flags;
+	vd->tx.tx_submit = vchan_tx_submit;
+
+	return &vd->tx;
+}
+
+/**
+ * vchan_issue_pending - move submitted descriptors to issued list
+ * vc: virtual channel to update
+ *
+ * vc.lock must be held by caller
+ */
+static inline bool vchan_issue_pending(struct virt_dma_chan *vc)
+{
+	list_splice_tail_init(&vc->desc_submitted, &vc->desc_issued);
+	return !list_empty(&vc->desc_issued);
+}
+
+/**
+ * vchan_cookie_complete - report completion of a descriptor
+ * vd: virtual descriptor to update
+ *
+ * vc.lock must be held by caller
+ */
+static inline void vchan_cookie_complete(struct virt_dma_desc *vd)
+{
+	struct virt_dma_chan *vc = to_virt_chan(vd->tx.chan);
+
+	dma_cookie_complete(&vd->tx);
+	dev_vdbg(vc->chan.device->dev, "txd %p[%x]: marked complete\n",
+		vd, vd->tx.cookie);
+	list_add_tail(&vd->node, &vc->desc_completed);
+
+	tasklet_schedule(&vc->task);
+}
+
+/**
+ * vchan_cyclic_callback - report the completion of a period
+ * vd: virtual descriptor
+ */
+static inline void vchan_cyclic_callback(struct virt_dma_desc *vd)
+{
+	struct virt_dma_chan *vc = to_virt_chan(vd->tx.chan);
+
+	vc->cyclic = vd;
+	tasklet_schedule(&vc->task);
+}
+
+/**
+ * vchan_next_desc - peek at the next descriptor to be processed
+ * vc: virtual channel to obtain descriptor from
+ *
+ * vc.lock must be held by caller
+ */
+static inline struct virt_dma_desc *vchan_next_desc(struct virt_dma_chan *vc)
+{
+	if (list_empty(&vc->desc_issued))
+		return NULL;
+
+	return list_first_entry(&vc->desc_issued, struct virt_dma_desc, node);
+}
+
+/**
+ * vchan_get_all_descriptors - obtain all submitted and issued descriptors
+ * vc: virtual channel to get descriptors from
+ * head: list of descriptors found
+ *
+ * vc.lock must be held by caller
+ *
+ * Removes all submitted and issued descriptors from internal lists, and
+ * provides a list of all descriptors found
+ */
+static inline void vchan_get_all_descriptors(struct virt_dma_chan *vc,
+	struct list_head *head)
+{
+	list_splice_tail_init(&vc->desc_submitted, head);
+	list_splice_tail_init(&vc->desc_issued, head);
+	list_splice_tail_init(&vc->desc_completed, head);
+}
+
+static inline void vchan_free_chan_resources(struct virt_dma_chan *vc)
+{
+	unsigned long flags;
+	LIST_HEAD(head);
+
+	spin_lock_irqsave(&vc->lock, flags);
+	vchan_get_all_descriptors(vc, &head);
+	spin_unlock_irqrestore(&vc->lock, flags);
+
+	vchan_dma_desc_free_list(vc, &head);
+}
+
+#endif
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index fdffa1beca1..409b92b8d34 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -7,7 +7,7 @@
 menuconfig EDAC
 	bool "EDAC (Error Detection And Correction) reporting"
 	depends on HAS_IOMEM
-	depends on X86 || PPC || TILE
+	depends on X86 || PPC || TILE || ARM
 	help
 	  EDAC is designed to report errors in the core system.
 	  These are low-level errors that are reported in the CPU or
@@ -31,6 +31,14 @@ if EDAC
 
 comment "Reporting subsystems"
 
+config EDAC_LEGACY_SYSFS
+	bool "EDAC legacy sysfs"
+	default y
+	help
+	  Enable the compatibility sysfs nodes.
+	  Use 'Y' if your edac utilities aren't ported to work with the newer
+	  structures.
+
 config EDAC_DEBUG
 	bool "Debugging"
 	help
@@ -294,4 +302,18 @@ config EDAC_TILE
 	  Support for error detection and correction on the
 	  Tilera memory controller.
 
+config EDAC_HIGHBANK_MC
+	tristate "Highbank Memory Controller"
+	depends on EDAC_MM_EDAC && ARCH_HIGHBANK
+	help
+	  Support for error detection and correction on the
+	  Calxeda Highbank memory controller.
+
+config EDAC_HIGHBANK_L2
+	tristate "Highbank L2 Cache"
+	depends on EDAC_MM_EDAC && ARCH_HIGHBANK
+	help
+	  Support for error detection and correction on the
+	  Calxeda Highbank memory controller.
+
 endif # EDAC
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index 196a63dd37c..7e5129a733f 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -55,3 +55,6 @@ obj-$(CONFIG_EDAC_AMD8111)		+= amd8111_edac.o
 obj-$(CONFIG_EDAC_AMD8131)		+= amd8131_edac.o
 
 obj-$(CONFIG_EDAC_TILE)			+= tile_edac.o
+
+obj-$(CONFIG_EDAC_HIGHBANK_MC)	+= highbank_mc_edac.o
+obj-$(CONFIG_EDAC_HIGHBANK_L2)	+= highbank_l2_edac.o
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 7be9b7288e9..5a297a26211 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -321,8 +321,8 @@ found:
 	return edac_mc_find((int)node_id);
 
 err_no_match:
-	debugf2("sys_addr 0x%lx doesn't match any node\n",
-		(unsigned long)sys_addr);
+	edac_dbg(2, "sys_addr 0x%lx doesn't match any node\n",
+		 (unsigned long)sys_addr);
 
 	return NULL;
 }
@@ -393,15 +393,15 @@ static int input_addr_to_csrow(struct mem_ctl_info *mci, u64 input_addr)
 		mask = ~mask;
 
 		if ((input_addr & mask) == (base & mask)) {
-			debugf2("InputAddr 0x%lx matches csrow %d (node %d)\n",
-				(unsigned long)input_addr, csrow,
-				pvt->mc_node_id);
+			edac_dbg(2, "InputAddr 0x%lx matches csrow %d (node %d)\n",
+				 (unsigned long)input_addr, csrow,
+				 pvt->mc_node_id);
 
 			return csrow;
 		}
 	}
-	debugf2("no matching csrow for InputAddr 0x%lx (MC node %d)\n",
-		(unsigned long)input_addr, pvt->mc_node_id);
+	edac_dbg(2, "no matching csrow for InputAddr 0x%lx (MC node %d)\n",
+		 (unsigned long)input_addr, pvt->mc_node_id);
 
 	return -1;
 }
@@ -430,20 +430,20 @@ int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 
 	/* only revE and later have the DRAM Hole Address Register */
 	if (boot_cpu_data.x86 == 0xf && pvt->ext_model < K8_REV_E) {
-		debugf1("  revision %d for node %d does not support DHAR\n",
-			pvt->ext_model, pvt->mc_node_id);
+		edac_dbg(1, "  revision %d for node %d does not support DHAR\n",
+			 pvt->ext_model, pvt->mc_node_id);
 		return 1;
 	}
 
 	/* 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");
+		edac_dbg(1, "  Dram Memory Hoisting is DISABLED on this system\n");
 		return 1;
 	}
 
 	if (!dhar_valid(pvt)) {
-		debugf1("  Dram Memory Hoisting is DISABLED on this node %d\n",
-			pvt->mc_node_id);
+		edac_dbg(1, "  Dram Memory Hoisting is DISABLED on this node %d\n",
+			 pvt->mc_node_id);
 		return 1;
 	}
 
@@ -475,9 +475,9 @@ int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 	else
 		*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,
-		(unsigned long)*hole_offset, (unsigned long)*hole_size);
+	edac_dbg(1, "  DHAR info for node %d base 0x%lx offset 0x%lx size 0x%lx\n",
+		 pvt->mc_node_id, (unsigned long)*hole_base,
+		 (unsigned long)*hole_offset, (unsigned long)*hole_size);
 
 	return 0;
 }
@@ -528,10 +528,9 @@ static u64 sys_addr_to_dram_addr(struct mem_ctl_info *mci, u64 sys_addr)
 			/* use DHAR to translate SysAddr to DramAddr */
 			dram_addr = sys_addr - hole_offset;
 
-			debugf2("using DHAR to translate SysAddr 0x%lx to "
-				"DramAddr 0x%lx\n",
-				(unsigned long)sys_addr,
-				(unsigned long)dram_addr);
+			edac_dbg(2, "using DHAR to translate SysAddr 0x%lx to DramAddr 0x%lx\n",
+				 (unsigned long)sys_addr,
+				 (unsigned long)dram_addr);
 
 			return dram_addr;
 		}
@@ -548,9 +547,8 @@ static u64 sys_addr_to_dram_addr(struct mem_ctl_info *mci, u64 sys_addr)
 	 */
 	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,
-		(unsigned long)dram_addr);
+	edac_dbg(2, "using DRAM Base register to translate SysAddr 0x%lx to DramAddr 0x%lx\n",
+		 (unsigned long)sys_addr, (unsigned long)dram_addr);
 	return dram_addr;
 }
 
@@ -586,9 +584,9 @@ static u64 dram_addr_to_input_addr(struct mem_ctl_info *mci, u64 dram_addr)
 	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,
-		(unsigned long)input_addr);
+	edac_dbg(2, "  Intlv Shift=%d DramAddr=0x%lx maps to InputAddr=0x%lx\n",
+		 intlv_shift, (unsigned long)dram_addr,
+		 (unsigned long)input_addr);
 
 	return input_addr;
 }
@@ -604,8 +602,8 @@ static u64 sys_addr_to_input_addr(struct mem_ctl_info *mci, u64 sys_addr)
 	input_addr =
 	    dram_addr_to_input_addr(mci, sys_addr_to_dram_addr(mci, sys_addr));
 
-	debugf2("SysAdddr 0x%lx translates to InputAddr 0x%lx\n",
-		(unsigned long)sys_addr, (unsigned long)input_addr);
+	edac_dbg(2, "SysAdddr 0x%lx translates to InputAddr 0x%lx\n",
+		 (unsigned long)sys_addr, (unsigned long)input_addr);
 
 	return input_addr;
 }
@@ -637,8 +635,8 @@ static u64 input_addr_to_dram_addr(struct mem_ctl_info *mci, u64 input_addr)
 
 	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);
+		edac_dbg(1, "    InputAddr 0x%lx translates to DramAddr of same value\n",
+			 (unsigned long)input_addr);
 
 		return input_addr;
 	}
@@ -649,9 +647,9 @@ static u64 input_addr_to_dram_addr(struct mem_ctl_info *mci, u64 input_addr)
 	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 "
-		"(%d node interleave bits)\n", (unsigned long)input_addr,
-		(unsigned long)dram_addr, intlv_shift);
+	edac_dbg(1, "InputAddr 0x%lx translates to DramAddr 0x%lx (%d node interleave bits)\n",
+		 (unsigned long)input_addr,
+		 (unsigned long)dram_addr, intlv_shift);
 
 	return dram_addr;
 }
@@ -673,9 +671,9 @@ static u64 dram_addr_to_sys_addr(struct mem_ctl_info *mci, u64 dram_addr)
 		    (dram_addr < (hole_base + hole_size))) {
 			sys_addr = dram_addr + hole_offset;
 
-			debugf1("using DHAR to translate DramAddr 0x%lx to "
-				"SysAddr 0x%lx\n", (unsigned long)dram_addr,
-				(unsigned long)sys_addr);
+			edac_dbg(1, "using DHAR to translate DramAddr 0x%lx to SysAddr 0x%lx\n",
+				 (unsigned long)dram_addr,
+				 (unsigned long)sys_addr);
 
 			return sys_addr;
 		}
@@ -697,9 +695,9 @@ static u64 dram_addr_to_sys_addr(struct mem_ctl_info *mci, u64 dram_addr)
 	 */
 	sys_addr |= ~((sys_addr & (1ull << 39)) - 1);
 
-	debugf1("    Node %d, DramAddr 0x%lx to SysAddr 0x%lx\n",
-		pvt->mc_node_id, (unsigned long)dram_addr,
-		(unsigned long)sys_addr);
+	edac_dbg(1, "    Node %d, DramAddr 0x%lx to SysAddr 0x%lx\n",
+		 pvt->mc_node_id, (unsigned long)dram_addr,
+		 (unsigned long)sys_addr);
 
 	return sys_addr;
 }
@@ -768,49 +766,48 @@ static void amd64_debug_display_dimm_sizes(struct amd64_pvt *, u8);
 
 static void amd64_dump_dramcfg_low(u32 dclr, int chan)
 {
-	debugf1("F2x%d90 (DRAM Cfg Low): 0x%08x\n", chan, dclr);
+	edac_dbg(1, "F2x%d90 (DRAM Cfg Low): 0x%08x\n", chan, dclr);
 
-	debugf1("  DIMM type: %sbuffered; all DIMMs support ECC: %s\n",
-		(dclr & BIT(16)) ?  "un" : "",
-		(dclr & BIT(19)) ? "yes" : "no");
+	edac_dbg(1, "  DIMM type: %sbuffered; all DIMMs support ECC: %s\n",
+		 (dclr & BIT(16)) ?  "un" : "",
+		 (dclr & BIT(19)) ? "yes" : "no");
 
-	debugf1("  PAR/ERR parity: %s\n",
-		(dclr & BIT(8)) ?  "enabled" : "disabled");
+	edac_dbg(1, "  PAR/ERR parity: %s\n",
+		 (dclr & BIT(8)) ?  "enabled" : "disabled");
 
 	if (boot_cpu_data.x86 == 0x10)
-		debugf1("  DCT 128bit mode width: %s\n",
-			(dclr & BIT(11)) ?  "128b" : "64b");
+		edac_dbg(1, "  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",
-		(dclr & BIT(13)) ?  "yes" : "no",
-		(dclr & BIT(14)) ?  "yes" : "no",
-		(dclr & BIT(15)) ?  "yes" : "no");
+	edac_dbg(1, "  x4 logical DIMMs present: L0: %s L1: %s L2: %s L3: %s\n",
+		 (dclr & BIT(12)) ?  "yes" : "no",
+		 (dclr & BIT(13)) ?  "yes" : "no",
+		 (dclr & BIT(14)) ?  "yes" : "no",
+		 (dclr & BIT(15)) ?  "yes" : "no");
 }
 
 /* Display and decode various NB registers for debug purposes. */
 static void dump_misc_regs(struct amd64_pvt *pvt)
 {
-	debugf1("F3xE8 (NB Cap): 0x%08x\n", pvt->nbcap);
+	edac_dbg(1, "F3xE8 (NB Cap): 0x%08x\n", pvt->nbcap);
 
-	debugf1("  NB two channel DRAM capable: %s\n",
-		(pvt->nbcap & NBCAP_DCT_DUAL) ? "yes" : "no");
+	edac_dbg(1, "  NB two channel DRAM capable: %s\n",
+		 (pvt->nbcap & NBCAP_DCT_DUAL) ? "yes" : "no");
 
-	debugf1("  ECC capable: %s, ChipKill ECC capable: %s\n",
-		(pvt->nbcap & NBCAP_SECDED) ? "yes" : "no",
-		(pvt->nbcap & NBCAP_CHIPKILL) ? "yes" : "no");
+	edac_dbg(1, "  ECC capable: %s, ChipKill ECC capable: %s\n",
+		 (pvt->nbcap & NBCAP_SECDED) ? "yes" : "no",
+		 (pvt->nbcap & NBCAP_CHIPKILL) ? "yes" : "no");
 
 	amd64_dump_dramcfg_low(pvt->dclr0, 0);
 
-	debugf1("F3xB0 (Online Spare): 0x%08x\n", pvt->online_spare);
+	edac_dbg(1, "F3xB0 (Online Spare): 0x%08x\n", pvt->online_spare);
 
-	debugf1("F1xF0 (DRAM Hole Address): 0x%08x, base: 0x%08x, "
-			"offset: 0x%08x\n",
-			pvt->dhar, dhar_base(pvt),
-			(boot_cpu_data.x86 == 0xf) ? k8_dhar_offset(pvt)
-						   : f10_dhar_offset(pvt));
+	edac_dbg(1, "F1xF0 (DRAM Hole Address): 0x%08x, base: 0x%08x, offset: 0x%08x\n",
+		 pvt->dhar, dhar_base(pvt),
+		 (boot_cpu_data.x86 == 0xf) ? k8_dhar_offset(pvt)
+		 : f10_dhar_offset(pvt));
 
-	debugf1("  DramHoleValid: %s\n", dhar_valid(pvt) ? "yes" : "no");
+	edac_dbg(1, "  DramHoleValid: %s\n", dhar_valid(pvt) ? "yes" : "no");
 
 	amd64_debug_display_dimm_sizes(pvt, 0);
 
@@ -857,15 +854,15 @@ static void read_dct_base_mask(struct amd64_pvt *pvt)
 		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, *base0, reg0);
+			edac_dbg(0, "  DCSB0[%d]=0x%08x reg: F2x%x\n",
+				 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);
+			edac_dbg(0, "  DCSB1[%d]=0x%08x reg: F2x%x\n",
+				 cs, *base1, reg1);
 	}
 
 	for_each_chip_select_mask(cs, 0, pvt) {
@@ -875,15 +872,15 @@ static void read_dct_base_mask(struct amd64_pvt *pvt)
 		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, *mask0, reg0);
+			edac_dbg(0, "    DCSM0[%d]=0x%08x reg: F2x%x\n",
+				 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);
+			edac_dbg(0, "    DCSM1[%d]=0x%08x reg: F2x%x\n",
+				 cs, *mask1, reg1);
 	}
 }
 
@@ -1049,24 +1046,22 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 	if (!src_mci) {
 		amd64_mc_err(mci, "failed to map error addr 0x%lx to a node\n",
 			     (unsigned long)sys_addr);
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     page, offset, syndrome,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
 				     "failed to map error addr to a node",
-				     NULL);
+				     "");
 		return;
 	}
 
 	/* Now map the sys_addr to a CSROW */
 	csrow = sys_addr_to_csrow(src_mci, sys_addr);
 	if (csrow < 0) {
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     page, offset, syndrome,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
 				     "failed to map error addr to a csrow",
-				     NULL);
+				     "");
 		return;
 	}
 
@@ -1082,12 +1077,11 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 			amd64_mc_warn(src_mci, "unknown syndrome 0x%04x - "
 				      "possible error reporting race\n",
 				      syndrome);
-			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 					     page, offset, syndrome,
 					     csrow, -1, -1,
-					     EDAC_MOD_STR,
 					     "unknown syndrome - possible error reporting race",
-					     NULL);
+					     "");
 			return;
 		}
 	} else {
@@ -1102,10 +1096,10 @@ static void k8_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 		channel = ((sys_addr & BIT(3)) != 0);
 	}
 
-	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, src_mci,
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, src_mci, 1,
 			     page, offset, syndrome,
 			     csrow, channel, -1,
-			     EDAC_MOD_STR, "", NULL);
+			     "", "");
 }
 
 static int ddr2_cs_size(unsigned i, bool dct_width)
@@ -1193,7 +1187,7 @@ static int f1x_early_channel_count(struct amd64_pvt *pvt)
 	 * Need to check DCT0[0] and DCT1[0] to see if only one of them has
 	 * their CSEnable bit on. If so, then SINGLE DIMM case.
 	 */
-	debugf0("Data width is not 128 bits - need more decoding\n");
+	edac_dbg(0, "Data width is not 128 bits - need more decoding\n");
 
 	/*
 	 * Check DRAM Bank Address Mapping values for each DIMM to see if there
@@ -1272,25 +1266,24 @@ static void read_dram_ctl_register(struct amd64_pvt *pvt)
 		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));
+		edac_dbg(0, "F2x110 (DCTSelLow): 0x%08x, High range addrs at: 0x%x\n",
+			 pvt->dct_sel_lo, dct_sel_baseaddr(pvt));
 
-		debugf0("  DCTs operate in %s mode.\n",
-			(dct_ganging_enabled(pvt) ? "ganged" : "unganged"));
+		edac_dbg(0, "  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"));
+			edac_dbg(0, "  Address range split per DCT: %s\n",
+				 (dct_high_range_enabled(pvt) ? "yes" : "no"));
 
-		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"));
+		edac_dbg(0, "  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("  channel interleave: %s, "
-			"interleave bits selector: 0x%x\n",
-			(dct_interleave_enabled(pvt) ? "enabled" : "disabled"),
-			dct_sel_interleave_addr(pvt));
+		edac_dbg(0, "  channel interleave: %s, "
+			 "interleave bits selector: 0x%x\n",
+			 (dct_interleave_enabled(pvt) ? "enabled" : "disabled"),
+			 dct_sel_interleave_addr(pvt));
 	}
 
 	amd64_read_dct_pci_cfg(pvt, DCT_SEL_HI, &pvt->dct_sel_hi);
@@ -1428,7 +1421,7 @@ static int f1x_lookup_addr_in_dct(u64 in_addr, u32 nid, u8 dct)
 
 	pvt = mci->pvt_info;
 
-	debugf1("input addr: 0x%llx, DCT: %d\n", in_addr, dct);
+	edac_dbg(1, "input addr: 0x%llx, DCT: %d\n", in_addr, dct);
 
 	for_each_chip_select(csrow, dct, pvt) {
 		if (!csrow_enabled(csrow, dct, pvt))
@@ -1436,19 +1429,18 @@ static int f1x_lookup_addr_in_dct(u64 in_addr, u32 nid, u8 dct)
 
 		get_cs_base_and_mask(pvt, csrow, dct, &cs_base, &cs_mask);
 
-		debugf1("    CSROW=%d CSBase=0x%llx CSMask=0x%llx\n",
-			csrow, cs_base, cs_mask);
+		edac_dbg(1, "    CSROW=%d CSBase=0x%llx CSMask=0x%llx\n",
+			 csrow, cs_base, cs_mask);
 
 		cs_mask = ~cs_mask;
 
-		debugf1("    (InputAddr & ~CSMask)=0x%llx "
-			"(CSBase & ~CSMask)=0x%llx\n",
-			(in_addr & cs_mask), (cs_base & cs_mask));
+		edac_dbg(1, "    (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(pvt, dct, csrow);
 
-			debugf1(" MATCH csrow=%d\n", cs_found);
+			edac_dbg(1, " MATCH csrow=%d\n", cs_found);
 			break;
 		}
 	}
@@ -1505,8 +1497,8 @@ static int f1x_match_to_this_node(struct amd64_pvt *pvt, unsigned 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));
+	edac_dbg(1, "(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 &&
@@ -1562,7 +1554,7 @@ static int f1x_match_to_this_node(struct amd64_pvt *pvt, unsigned range,
 				     (chan_addr & 0xfff);
 	}
 
-	debugf1("   Normalized DCT addr: 0x%llx\n", chan_addr);
+	edac_dbg(1, "   Normalized DCT addr: 0x%llx\n", chan_addr);
 
 	cs_found = f1x_lookup_addr_in_dct(chan_addr, node_id, channel);
 
@@ -1616,12 +1608,11 @@ static void f1x_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 	csrow = f1x_translate_sysaddr_to_cs(pvt, sys_addr, &nid, &chan);
 
 	if (csrow < 0) {
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     page, offset, syndrome,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
 				     "failed to map error addr to a csrow",
-				     NULL);
+				     "");
 		return;
 	}
 
@@ -1633,10 +1624,10 @@ static void f1x_map_sysaddr_to_csrow(struct mem_ctl_info *mci, u64 sys_addr,
 	if (dct_ganging_enabled(pvt))
 		chan = get_channel_from_ecc_syndrome(mci, syndrome);
 
-	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 			     page, offset, syndrome,
 			     csrow, chan, -1,
-			     EDAC_MOD_STR, "", NULL);
+			     "", "");
 }
 
 /*
@@ -1664,7 +1655,8 @@ static void amd64_debug_display_dimm_sizes(struct amd64_pvt *pvt, u8 ctrl)
 	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);
+	edac_dbg(1, "F2x%d80 (DRAM Bank Address Mapping): 0x%08x\n",
+		 ctrl, dbam);
 
 	edac_printk(KERN_DEBUG, EDAC_MC, "DCT%d chip selects:\n", ctrl);
 
@@ -1840,7 +1832,7 @@ static int decode_syndrome(u16 syndrome, u16 *vectors, unsigned num_vecs,
 		}
 	}
 
-	debugf0("syndrome(%x) not found\n", syndrome);
+	edac_dbg(0, "syndrome(%x) not found\n", syndrome);
 	return -1;
 }
 
@@ -1917,12 +1909,11 @@ static void amd64_handle_ce(struct mem_ctl_info *mci, struct mce *m)
 	/* Ensure that the Error Address is VALID */
 	if (!(m->status & MCI_STATUS_ADDRV)) {
 		amd64_mc_err(mci, "HW has no ERROR_ADDRESS available\n");
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     0, 0, 0,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
 				     "HW has no ERROR_ADDRESS available",
-				     NULL);
+				     "");
 		return;
 	}
 
@@ -1946,12 +1937,11 @@ static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 
 	if (!(m->status & MCI_STATUS_ADDRV)) {
 		amd64_mc_err(mci, "HW has no ERROR_ADDRESS available\n");
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     0, 0, 0,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
 				     "HW has no ERROR_ADDRESS available",
-				     NULL);
+				     "");
 		return;
 	}
 
@@ -1966,11 +1956,11 @@ static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 	if (!src_mci) {
 		amd64_mc_err(mci, "ERROR ADDRESS (0x%lx) NOT mapped to a MC\n",
 				  (unsigned long)sys_addr);
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     page, offset, 0,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
-				     "ERROR ADDRESS NOT mapped to a MC", NULL);
+				     "ERROR ADDRESS NOT mapped to a MC",
+				     "");
 		return;
 	}
 
@@ -1980,17 +1970,16 @@ static void amd64_handle_ue(struct mem_ctl_info *mci, struct mce *m)
 	if (csrow < 0) {
 		amd64_mc_err(mci, "ERROR_ADDRESS (0x%lx) NOT mapped to CS\n",
 				  (unsigned long)sys_addr);
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     page, offset, 0,
 				     -1, -1, -1,
-				     EDAC_MOD_STR,
 				     "ERROR ADDRESS NOT mapped to CS",
-				     NULL);
+				     "");
 	} else {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     page, offset, 0,
 				     csrow, -1, -1,
-				     EDAC_MOD_STR, "", NULL);
+				     "", "");
 	}
 }
 
@@ -2047,9 +2036,9 @@ static int reserve_mc_sibling_devs(struct amd64_pvt *pvt, u16 f1_id, u16 f3_id)
 
 		return -ENODEV;
 	}
-	debugf1("F1: %s\n", pci_name(pvt->F1));
-	debugf1("F2: %s\n", pci_name(pvt->F2));
-	debugf1("F3: %s\n", pci_name(pvt->F3));
+	edac_dbg(1, "F1: %s\n", pci_name(pvt->F1));
+	edac_dbg(1, "F2: %s\n", pci_name(pvt->F2));
+	edac_dbg(1, "F3: %s\n", pci_name(pvt->F3));
 
 	return 0;
 }
@@ -2076,15 +2065,15 @@ static void read_mc_regs(struct amd64_pvt *pvt)
 	 * those are Read-As-Zero
 	 */
 	rdmsrl(MSR_K8_TOP_MEM1, pvt->top_mem);
-	debugf0("  TOP_MEM:  0x%016llx\n", pvt->top_mem);
+	edac_dbg(0, "  TOP_MEM:  0x%016llx\n", pvt->top_mem);
 
 	/* check first whether TOP_MEM2 is enabled */
 	rdmsrl(MSR_K8_SYSCFG, msr_val);
 	if (msr_val & (1U << 21)) {
 		rdmsrl(MSR_K8_TOP_MEM2, pvt->top_mem2);
-		debugf0("  TOP_MEM2: 0x%016llx\n", pvt->top_mem2);
+		edac_dbg(0, "  TOP_MEM2: 0x%016llx\n", pvt->top_mem2);
 	} else
-		debugf0("  TOP_MEM2 disabled.\n");
+		edac_dbg(0, "  TOP_MEM2 disabled\n");
 
 	amd64_read_pci_cfg(pvt->F3, NBCAP, &pvt->nbcap);
 
@@ -2100,17 +2089,17 @@ static void read_mc_regs(struct amd64_pvt *pvt)
 		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));
+		edac_dbg(1, "  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));
+		edac_dbg(1, "   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));
 	}
 
 	read_dct_base_mask(pvt);
@@ -2191,9 +2180,9 @@ static u32 amd64_csrow_nr_pages(struct amd64_pvt *pvt, u8 dct, int csrow_nr)
 
 	nr_pages = pvt->ops->dbam_to_cs(pvt, dct, cs_mode) << (20 - PAGE_SHIFT);
 
-	debugf0("  (csrow=%d) DBAM map index= %d\n", csrow_nr, cs_mode);
-	debugf0("    nr_pages/channel= %u  channel-count = %d\n",
-		nr_pages, pvt->channel_count);
+	edac_dbg(0, "  (csrow=%d) DBAM map index= %d\n", csrow_nr, cs_mode);
+	edac_dbg(0, "    nr_pages/channel= %u  channel-count = %d\n",
+		 nr_pages, pvt->channel_count);
 
 	return nr_pages;
 }
@@ -2205,6 +2194,7 @@ static u32 amd64_csrow_nr_pages(struct amd64_pvt *pvt, u8 dct, int csrow_nr)
 static int init_csrows(struct mem_ctl_info *mci)
 {
 	struct csrow_info *csrow;
+	struct dimm_info *dimm;
 	struct amd64_pvt *pvt = mci->pvt_info;
 	u64 base, mask;
 	u32 val;
@@ -2217,22 +2207,19 @@ static int init_csrows(struct mem_ctl_info *mci)
 
 	pvt->nbcfg = val;
 
-	debugf0("node %d, NBCFG=0x%08x[ChipKillEccCap: %d|DramEccEn: %d]\n",
-		pvt->mc_node_id, val,
-		!!(val & NBCFG_CHIPKILL), !!(val & NBCFG_ECC_ENABLE));
+	edac_dbg(0, "node %d, NBCFG=0x%08x[ChipKillEccCap: %d|DramEccEn: %d]\n",
+		 pvt->mc_node_id, val,
+		 !!(val & NBCFG_CHIPKILL), !!(val & NBCFG_ECC_ENABLE));
 
 	for_each_chip_select(i, 0, pvt) {
-		csrow = &mci->csrows[i];
+		csrow = mci->csrows[i];
 
 		if (!csrow_enabled(i, 0, pvt) && !csrow_enabled(i, 1, pvt)) {
-			debugf1("----CSROW %d EMPTY for node %d\n", i,
-				pvt->mc_node_id);
+			edac_dbg(1, "----CSROW %d VALID for MC node %d\n",
+				 i, pvt->mc_node_id);
 			continue;
 		}
 
-		debugf1("----CSROW %d VALID for MC node %d\n",
-			i, pvt->mc_node_id);
-
 		empty = 0;
 		if (csrow_enabled(i, 0, pvt))
 			nr_pages = amd64_csrow_nr_pages(pvt, 0, i);
@@ -2244,8 +2231,9 @@ static int init_csrows(struct mem_ctl_info *mci)
 
 		mtype = amd64_determine_memory_type(pvt, i);
 
-		debugf1("  for MC node %d csrow %d:\n", pvt->mc_node_id, i);
-		debugf1("    nr_pages: %u\n", nr_pages * pvt->channel_count);
+		edac_dbg(1, "  for MC node %d csrow %d:\n", pvt->mc_node_id, i);
+		edac_dbg(1, "    nr_pages: %u\n",
+			 nr_pages * pvt->channel_count);
 
 		/*
 		 * determine whether CHIPKILL or JUST ECC or NO ECC is operating
@@ -2257,9 +2245,10 @@ static int init_csrows(struct mem_ctl_info *mci)
 			edac_mode = EDAC_NONE;
 
 		for (j = 0; j < pvt->channel_count; j++) {
-			csrow->channels[j].dimm->mtype = mtype;
-			csrow->channels[j].dimm->edac_mode = edac_mode;
-			csrow->channels[j].dimm->nr_pages = nr_pages;
+			dimm = csrow->channels[j]->dimm;
+			dimm->mtype = mtype;
+			dimm->edac_mode = edac_mode;
+			dimm->nr_pages = nr_pages;
 		}
 	}
 
@@ -2296,9 +2285,9 @@ static bool amd64_nb_mce_bank_enabled_on_node(unsigned nid)
 		struct msr *reg = per_cpu_ptr(msrs, cpu);
 		nbe = reg->l & MSR_MCGCTL_NBE;
 
-		debugf0("core: %u, MCG_CTL: 0x%llx, NB MSR is %s\n",
-			cpu, reg->q,
-			(nbe ? "enabled" : "disabled"));
+		edac_dbg(0, "core: %u, MCG_CTL: 0x%llx, NB MSR is %s\n",
+			 cpu, reg->q,
+			 (nbe ? "enabled" : "disabled"));
 
 		if (!nbe)
 			goto out;
@@ -2369,8 +2358,8 @@ static bool enable_ecc_error_reporting(struct ecc_settings *s, u8 nid,
 
 	amd64_read_pci_cfg(F3, NBCFG, &value);
 
-	debugf0("1: node %d, NBCFG=0x%08x[DramEccEn: %d]\n",
-		nid, value, !!(value & NBCFG_ECC_ENABLE));
+	edac_dbg(0, "1: node %d, NBCFG=0x%08x[DramEccEn: %d]\n",
+		 nid, value, !!(value & NBCFG_ECC_ENABLE));
 
 	if (!(value & NBCFG_ECC_ENABLE)) {
 		amd64_warn("DRAM ECC disabled on this node, enabling...\n");
@@ -2394,8 +2383,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[DramEccEn: %d]\n",
-		nid, value, !!(value & NBCFG_ECC_ENABLE));
+	edac_dbg(0, "2: node %d, NBCFG=0x%08x[DramEccEn: %d]\n",
+		 nid, value, !!(value & NBCFG_ECC_ENABLE));
 
 	return ret;
 }
@@ -2463,26 +2452,29 @@ static bool ecc_enabled(struct pci_dev *F3, u8 nid)
 	return true;
 }
 
-struct mcidev_sysfs_attribute sysfs_attrs[ARRAY_SIZE(amd64_dbg_attrs) +
-					  ARRAY_SIZE(amd64_inj_attrs) +
-					  1];
-
-struct mcidev_sysfs_attribute terminator = { .attr = { .name = NULL } };
-
-static void set_mc_sysfs_attrs(struct mem_ctl_info *mci)
+static int set_mc_sysfs_attrs(struct mem_ctl_info *mci)
 {
-	unsigned int i = 0, j = 0;
+	int rc;
 
-	for (; i < ARRAY_SIZE(amd64_dbg_attrs); i++)
-		sysfs_attrs[i] = amd64_dbg_attrs[i];
+	rc = amd64_create_sysfs_dbg_files(mci);
+	if (rc < 0)
+		return rc;
 
-	if (boot_cpu_data.x86 >= 0x10)
-		for (j = 0; j < ARRAY_SIZE(amd64_inj_attrs); j++, i++)
-			sysfs_attrs[i] = amd64_inj_attrs[j];
+	if (boot_cpu_data.x86 >= 0x10) {
+		rc = amd64_create_sysfs_inject_files(mci);
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
 
-	sysfs_attrs[i] = terminator;
+static void del_mc_sysfs_attrs(struct mem_ctl_info *mci)
+{
+	amd64_remove_sysfs_dbg_files(mci);
 
-	mci->mc_driver_sysfs_attributes = sysfs_attrs;
+	if (boot_cpu_data.x86 >= 0x10)
+		amd64_remove_sysfs_inject_files(mci);
 }
 
 static void setup_mci_misc_attrs(struct mem_ctl_info *mci,
@@ -2601,20 +2593,22 @@ static int amd64_init_one_instance(struct pci_dev *F2)
 		goto err_siblings;
 
 	mci->pvt_info = pvt;
-	mci->dev = &pvt->F2->dev;
+	mci->pdev = &pvt->F2->dev;
 
 	setup_mci_misc_attrs(mci, fam_type);
 
 	if (init_csrows(mci))
 		mci->edac_cap = EDAC_FLAG_NONE;
 
-	set_mc_sysfs_attrs(mci);
-
 	ret = -ENODEV;
 	if (edac_mc_add_mc(mci)) {
-		debugf1("failed edac_mc_add_mc()\n");
+		edac_dbg(1, "failed edac_mc_add_mc()\n");
 		goto err_add_mc;
 	}
+	if (set_mc_sysfs_attrs(mci)) {
+		edac_dbg(1, "failed edac_mc_add_mc()\n");
+		goto err_add_sysfs;
+	}
 
 	/* register stuff with EDAC MCE */
 	if (report_gart_errors)
@@ -2628,6 +2622,8 @@ static int amd64_init_one_instance(struct pci_dev *F2)
 
 	return 0;
 
+err_add_sysfs:
+	edac_mc_del_mc(mci->pdev);
 err_add_mc:
 	edac_mc_free(mci);
 
@@ -2651,7 +2647,7 @@ static int __devinit amd64_probe_one_instance(struct pci_dev *pdev,
 
 	ret = pci_enable_device(pdev);
 	if (ret < 0) {
-		debugf0("ret=%d\n", ret);
+		edac_dbg(0, "ret=%d\n", ret);
 		return -EIO;
 	}
 
@@ -2698,6 +2694,8 @@ static void __devexit amd64_remove_one_instance(struct pci_dev *pdev)
 	struct pci_dev *F3 = node_to_amd_nb(nid)->misc;
 	struct ecc_settings *s = ecc_stngs[nid];
 
+	mci = find_mci_by_dev(&pdev->dev);
+	del_mc_sysfs_attrs(mci);
 	/* Remove from EDAC CORE tracking list */
 	mci = edac_mc_del_mc(&pdev->dev);
 	if (!mci)
diff --git a/drivers/edac/amd64_edac.h b/drivers/edac/amd64_edac.h
index 9a666cb985b..8d4804732ba 100644
--- a/drivers/edac/amd64_edac.h
+++ b/drivers/edac/amd64_edac.h
@@ -413,20 +413,33 @@ struct ecc_settings {
 };
 
 #ifdef CONFIG_EDAC_DEBUG
-#define NUM_DBG_ATTRS 5
+int amd64_create_sysfs_dbg_files(struct mem_ctl_info *mci);
+void amd64_remove_sysfs_dbg_files(struct mem_ctl_info *mci);
+
 #else
-#define NUM_DBG_ATTRS 0
+static inline int amd64_create_sysfs_dbg_files(struct mem_ctl_info *mci)
+{
+	return 0;
+}
+static void inline amd64_remove_sysfs_dbg_files(struct mem_ctl_info *mci)
+{
+}
 #endif
 
 #ifdef CONFIG_EDAC_AMD64_ERROR_INJECTION
-#define NUM_INJ_ATTRS 5
+int amd64_create_sysfs_inject_files(struct mem_ctl_info *mci);
+void amd64_remove_sysfs_inject_files(struct mem_ctl_info *mci);
+
 #else
-#define NUM_INJ_ATTRS 0
+static inline int amd64_create_sysfs_inject_files(struct mem_ctl_info *mci)
+{
+	return 0;
+}
+static inline void amd64_remove_sysfs_inject_files(struct mem_ctl_info *mci)
+{
+}
 #endif
 
-extern struct mcidev_sysfs_attribute amd64_dbg_attrs[NUM_DBG_ATTRS],
-				     amd64_inj_attrs[NUM_INJ_ATTRS];
-
 /*
  * Each of the PCI Device IDs types have their own set of hardware accessor
  * functions and per device encoding/decoding logic.
@@ -460,3 +473,5 @@ int __amd64_write_pci_cfg_dword(struct pci_dev *pdev, int offset,
 
 int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 			     u64 *hole_offset, u64 *hole_size);
+
+#define to_mci(k) container_of(k, struct mem_ctl_info, dev)
diff --git a/drivers/edac/amd64_edac_dbg.c b/drivers/edac/amd64_edac_dbg.c
index e3562288f4c..2c1bbf74060 100644
--- a/drivers/edac/amd64_edac_dbg.c
+++ b/drivers/edac/amd64_edac_dbg.c
@@ -1,8 +1,11 @@
 #include "amd64_edac.h"
 
 #define EDAC_DCT_ATTR_SHOW(reg)						\
-static ssize_t amd64_##reg##_show(struct mem_ctl_info *mci, char *data)	\
+static ssize_t amd64_##reg##_show(struct device *dev,			\
+			       struct device_attribute *mattr,		\
+			       char *data)				\
 {									\
+	struct mem_ctl_info *mci = to_mci(dev);				\
 	struct amd64_pvt *pvt = mci->pvt_info;				\
 		return sprintf(data, "0x%016llx\n", (u64)pvt->reg);	\
 }
@@ -12,8 +15,12 @@ EDAC_DCT_ATTR_SHOW(dbam0);
 EDAC_DCT_ATTR_SHOW(top_mem);
 EDAC_DCT_ATTR_SHOW(top_mem2);
 
-static ssize_t amd64_hole_show(struct mem_ctl_info *mci, char *data)
+static ssize_t amd64_hole_show(struct device *dev,
+			       struct device_attribute *mattr,
+			       char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	u64 hole_base = 0;
 	u64 hole_offset = 0;
 	u64 hole_size = 0;
@@ -27,46 +34,40 @@ static ssize_t amd64_hole_show(struct mem_ctl_info *mci, char *data)
 /*
  * update NUM_DBG_ATTRS in case you add new members
  */
-struct mcidev_sysfs_attribute amd64_dbg_attrs[] = {
+static DEVICE_ATTR(dhar, S_IRUGO, amd64_dhar_show, NULL);
+static DEVICE_ATTR(dbam, S_IRUGO, amd64_dbam0_show, NULL);
+static DEVICE_ATTR(topmem, S_IRUGO, amd64_top_mem_show, NULL);
+static DEVICE_ATTR(topmem2, S_IRUGO, amd64_top_mem2_show, NULL);
+static DEVICE_ATTR(dram_hole, S_IRUGO, amd64_hole_show, NULL);
+
+int amd64_create_sysfs_dbg_files(struct mem_ctl_info *mci)
+{
+	int rc;
+
+	rc = device_create_file(&mci->dev, &dev_attr_dhar);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_dbam);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_topmem);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_topmem2);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_dram_hole);
+	if (rc < 0)
+		return rc;
 
-	{
-		.attr = {
-			.name = "dhar",
-			.mode = (S_IRUGO)
-		},
-		.show = amd64_dhar_show,
-		.store = NULL,
-	},
-	{
-		.attr = {
-			.name = "dbam",
-			.mode = (S_IRUGO)
-		},
-		.show = amd64_dbam0_show,
-		.store = NULL,
-	},
-	{
-		.attr = {
-			.name = "topmem",
-			.mode = (S_IRUGO)
-		},
-		.show = amd64_top_mem_show,
-		.store = NULL,
-	},
-	{
-		.attr = {
-			.name = "topmem2",
-			.mode = (S_IRUGO)
-		},
-		.show = amd64_top_mem2_show,
-		.store = NULL,
-	},
-	{
-		.attr = {
-			.name = "dram_hole",
-			.mode = (S_IRUGO)
-		},
-		.show = amd64_hole_show,
-		.store = NULL,
-	},
-};
+	return 0;
+}
+
+void amd64_remove_sysfs_dbg_files(struct mem_ctl_info *mci)
+{
+	device_remove_file(&mci->dev, &dev_attr_dhar);
+	device_remove_file(&mci->dev, &dev_attr_dbam);
+	device_remove_file(&mci->dev, &dev_attr_topmem);
+	device_remove_file(&mci->dev, &dev_attr_topmem2);
+	device_remove_file(&mci->dev, &dev_attr_dram_hole);
+}
diff --git a/drivers/edac/amd64_edac_inj.c b/drivers/edac/amd64_edac_inj.c
index 303f10e03dd..53d972e00df 100644
--- a/drivers/edac/amd64_edac_inj.c
+++ b/drivers/edac/amd64_edac_inj.c
@@ -1,7 +1,10 @@
 #include "amd64_edac.h"
 
-static ssize_t amd64_inject_section_show(struct mem_ctl_info *mci, char *buf)
+static ssize_t amd64_inject_section_show(struct device *dev,
+					 struct device_attribute *mattr,
+					 char *buf)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	return sprintf(buf, "0x%x\n", pvt->injection.section);
 }
@@ -12,9 +15,11 @@ static ssize_t amd64_inject_section_show(struct mem_ctl_info *mci, char *buf)
  *
  * range: 0..3
  */
-static ssize_t amd64_inject_section_store(struct mem_ctl_info *mci,
+static ssize_t amd64_inject_section_store(struct device *dev,
+					  struct device_attribute *mattr,
 					  const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	int ret = 0;
@@ -33,8 +38,11 @@ static ssize_t amd64_inject_section_store(struct mem_ctl_info *mci,
 	return ret;
 }
 
-static ssize_t amd64_inject_word_show(struct mem_ctl_info *mci, char *buf)
+static ssize_t amd64_inject_word_show(struct device *dev,
+					struct device_attribute *mattr,
+					char *buf)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	return sprintf(buf, "0x%x\n", pvt->injection.word);
 }
@@ -45,9 +53,11 @@ static ssize_t amd64_inject_word_show(struct mem_ctl_info *mci, char *buf)
  *
  * range: 0..8
  */
-static ssize_t amd64_inject_word_store(struct mem_ctl_info *mci,
-					const char *data, size_t count)
+static ssize_t amd64_inject_word_store(struct device *dev,
+				       struct device_attribute *mattr,
+				       const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	int ret = 0;
@@ -66,8 +76,11 @@ static ssize_t amd64_inject_word_store(struct mem_ctl_info *mci,
 	return ret;
 }
 
-static ssize_t amd64_inject_ecc_vector_show(struct mem_ctl_info *mci, char *buf)
+static ssize_t amd64_inject_ecc_vector_show(struct device *dev,
+					    struct device_attribute *mattr,
+					    char *buf)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	return sprintf(buf, "0x%x\n", pvt->injection.bit_map);
 }
@@ -77,9 +90,11 @@ static ssize_t amd64_inject_ecc_vector_show(struct mem_ctl_info *mci, char *buf)
  * corresponding bit within the error injection word above. When used during a
  * DRAM ECC read, it holds the contents of the of the DRAM ECC bits.
  */
-static ssize_t amd64_inject_ecc_vector_store(struct mem_ctl_info *mci,
-					     const char *data, size_t count)
+static ssize_t amd64_inject_ecc_vector_store(struct device *dev,
+				       struct device_attribute *mattr,
+				       const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	int ret = 0;
@@ -103,9 +118,11 @@ static ssize_t amd64_inject_ecc_vector_store(struct mem_ctl_info *mci,
  * Do a DRAM ECC read. Assemble staged values in the pvt area, format into
  * fields needed by the injection registers and read the NB Array Data Port.
  */
-static ssize_t amd64_inject_read_store(struct mem_ctl_info *mci,
-					const char *data, size_t count)
+static ssize_t amd64_inject_read_store(struct device *dev,
+				       struct device_attribute *mattr,
+				       const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	u32 section, word_bits;
@@ -125,7 +142,8 @@ static ssize_t amd64_inject_read_store(struct mem_ctl_info *mci,
 		/* Issue 'word' and 'bit' along with the READ request */
 		amd64_write_pci_cfg(pvt->F3, F10_NB_ARRAY_DATA, word_bits);
 
-		debugf0("section=0x%x word_bits=0x%x\n", section, word_bits);
+		edac_dbg(0, "section=0x%x word_bits=0x%x\n",
+			 section, word_bits);
 
 		return count;
 	}
@@ -136,9 +154,11 @@ static ssize_t amd64_inject_read_store(struct mem_ctl_info *mci,
  * Do a DRAM ECC write. Assemble staged values in the pvt area and format into
  * fields needed by the injection registers.
  */
-static ssize_t amd64_inject_write_store(struct mem_ctl_info *mci,
+static ssize_t amd64_inject_write_store(struct device *dev,
+					struct device_attribute *mattr,
 					const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct amd64_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	u32 section, word_bits;
@@ -158,7 +178,8 @@ static ssize_t amd64_inject_write_store(struct mem_ctl_info *mci,
 		/* Issue 'word' and 'bit' along with the READ request */
 		amd64_write_pci_cfg(pvt->F3, F10_NB_ARRAY_DATA, word_bits);
 
-		debugf0("section=0x%x word_bits=0x%x\n", section, word_bits);
+		edac_dbg(0, "section=0x%x word_bits=0x%x\n",
+			 section, word_bits);
 
 		return count;
 	}
@@ -168,46 +189,47 @@ static ssize_t amd64_inject_write_store(struct mem_ctl_info *mci,
 /*
  * update NUM_INJ_ATTRS in case you add new members
  */
-struct mcidev_sysfs_attribute amd64_inj_attrs[] = {
-
-	{
-		.attr = {
-			.name = "inject_section",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show = amd64_inject_section_show,
-		.store = amd64_inject_section_store,
-	},
-	{
-		.attr = {
-			.name = "inject_word",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show = amd64_inject_word_show,
-		.store = amd64_inject_word_store,
-	},
-	{
-		.attr = {
-			.name = "inject_ecc_vector",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show = amd64_inject_ecc_vector_show,
-		.store = amd64_inject_ecc_vector_store,
-	},
-	{
-		.attr = {
-			.name = "inject_write",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show = NULL,
-		.store = amd64_inject_write_store,
-	},
-	{
-		.attr = {
-			.name = "inject_read",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show = NULL,
-		.store = amd64_inject_read_store,
-	},
-};
+
+static DEVICE_ATTR(inject_section, S_IRUGO | S_IWUSR,
+		   amd64_inject_section_show, amd64_inject_section_store);
+static DEVICE_ATTR(inject_word, S_IRUGO | S_IWUSR,
+		   amd64_inject_word_show, amd64_inject_word_store);
+static DEVICE_ATTR(inject_ecc_vector, S_IRUGO | S_IWUSR,
+		   amd64_inject_ecc_vector_show, amd64_inject_ecc_vector_store);
+static DEVICE_ATTR(inject_write, S_IRUGO | S_IWUSR,
+		   NULL, amd64_inject_write_store);
+static DEVICE_ATTR(inject_read, S_IRUGO | S_IWUSR,
+		   NULL, amd64_inject_read_store);
+
+
+int amd64_create_sysfs_inject_files(struct mem_ctl_info *mci)
+{
+	int rc;
+
+	rc = device_create_file(&mci->dev, &dev_attr_inject_section);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_word);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_ecc_vector);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_write);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_read);
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
+void amd64_remove_sysfs_inject_files(struct mem_ctl_info *mci)
+{
+	device_remove_file(&mci->dev, &dev_attr_inject_section);
+	device_remove_file(&mci->dev, &dev_attr_inject_word);
+	device_remove_file(&mci->dev, &dev_attr_inject_ecc_vector);
+	device_remove_file(&mci->dev, &dev_attr_inject_write);
+	device_remove_file(&mci->dev, &dev_attr_inject_read);
+}
diff --git a/drivers/edac/amd76x_edac.c b/drivers/edac/amd76x_edac.c
index 9774d443fa5..29eeb68a200 100644
--- a/drivers/edac/amd76x_edac.c
+++ b/drivers/edac/amd76x_edac.c
@@ -105,7 +105,7 @@ static void amd76x_get_error_info(struct mem_ctl_info *mci,
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 	pci_read_config_dword(pdev, AMD76X_ECC_MODE_STATUS,
 			&info->ecc_mode_status);
 
@@ -145,10 +145,10 @@ static int amd76x_process_error_info(struct mem_ctl_info *mci,
 
 		if (handle_errors) {
 			row = (info->ecc_mode_status >> 4) & 0xf;
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
-					     mci->csrows[row].first_page, 0, 0,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
+					     mci->csrows[row]->first_page, 0, 0,
 					     row, 0, -1,
-					     mci->ctl_name, "", NULL);
+					     mci->ctl_name, "");
 		}
 	}
 
@@ -160,10 +160,10 @@ static int amd76x_process_error_info(struct mem_ctl_info *mci,
 
 		if (handle_errors) {
 			row = info->ecc_mode_status & 0xf;
-			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
-					     mci->csrows[row].first_page, 0, 0,
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
+					     mci->csrows[row]->first_page, 0, 0,
 					     row, 0, -1,
-					     mci->ctl_name, "", NULL);
+					     mci->ctl_name, "");
 		}
 	}
 
@@ -180,7 +180,7 @@ static int amd76x_process_error_info(struct mem_ctl_info *mci,
 static void amd76x_check(struct mem_ctl_info *mci)
 {
 	struct amd76x_error_info info;
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 	amd76x_get_error_info(mci, &info);
 	amd76x_process_error_info(mci, &info, 1);
 }
@@ -194,8 +194,8 @@ static void amd76x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 	int index;
 
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
-		dimm = csrow->channels[0].dimm;
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
 
 		/* find the DRAM Chip Select Base address and mask */
 		pci_read_config_dword(pdev,
@@ -241,7 +241,7 @@ static int amd76x_probe1(struct pci_dev *pdev, int dev_idx)
 	u32 ems_mode;
 	struct amd76x_error_info discard;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 	pci_read_config_dword(pdev, AMD76X_ECC_MODE_STATUS, &ems);
 	ems_mode = (ems >> 10) & 0x3;
 
@@ -256,8 +256,8 @@ static int amd76x_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf0("%s(): mci = %p\n", __func__, mci);
-	mci->dev = &pdev->dev;
+	edac_dbg(0, "mci = %p\n", mci);
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_RDDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_EC | EDAC_FLAG_SECDED;
 	mci->edac_cap = ems_mode ?
@@ -276,7 +276,7 @@ static int amd76x_probe1(struct pci_dev *pdev, int dev_idx)
 	 * type of memory controller.  The ID is therefore hardcoded to 0.
 	 */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
@@ -292,7 +292,7 @@ static int amd76x_probe1(struct pci_dev *pdev, int dev_idx)
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	return 0;
 
 fail:
@@ -304,7 +304,7 @@ fail:
 static int __devinit amd76x_init_one(struct pci_dev *pdev,
 				const struct pci_device_id *ent)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* don't need to call pci_enable_device() */
 	return amd76x_probe1(pdev, ent->driver_data);
@@ -322,7 +322,7 @@ static void __devexit amd76x_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (amd76x_pci)
 		edac_pci_release_generic_ctl(amd76x_pci);
diff --git a/drivers/edac/cell_edac.c b/drivers/edac/cell_edac.c
index 69ee6aab5c7..a1bbd8edd25 100644
--- a/drivers/edac/cell_edac.c
+++ b/drivers/edac/cell_edac.c
@@ -33,10 +33,10 @@ struct cell_edac_priv
 static void cell_edac_count_ce(struct mem_ctl_info *mci, int chan, u64 ar)
 {
 	struct cell_edac_priv		*priv = mci->pvt_info;
-	struct csrow_info		*csrow = &mci->csrows[0];
+	struct csrow_info		*csrow = mci->csrows[0];
 	unsigned long			address, pfn, offset, syndrome;
 
-	dev_dbg(mci->dev, "ECC CE err on node %d, channel %d, ar = 0x%016llx\n",
+	dev_dbg(mci->pdev, "ECC CE err on node %d, channel %d, ar = 0x%016llx\n",
 		priv->node, chan, ar);
 
 	/* Address decoding is likely a bit bogus, to dbl check */
@@ -48,18 +48,18 @@ static void cell_edac_count_ce(struct mem_ctl_info *mci, int chan, u64 ar)
 	syndrome = (ar & 0x000000001fe00000ul) >> 21;
 
 	/* TODO: Decoding of the error address */
-	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 			     csrow->first_page + pfn, offset, syndrome,
-			     0, chan, -1, "", "", NULL);
+			     0, chan, -1, "", "");
 }
 
 static void cell_edac_count_ue(struct mem_ctl_info *mci, int chan, u64 ar)
 {
 	struct cell_edac_priv		*priv = mci->pvt_info;
-	struct csrow_info		*csrow = &mci->csrows[0];
+	struct csrow_info		*csrow = mci->csrows[0];
 	unsigned long			address, pfn, offset;
 
-	dev_dbg(mci->dev, "ECC UE err on node %d, channel %d, ar = 0x%016llx\n",
+	dev_dbg(mci->pdev, "ECC UE err on node %d, channel %d, ar = 0x%016llx\n",
 		priv->node, chan, ar);
 
 	/* Address decoding is likely a bit bogus, to dbl check */
@@ -70,9 +70,9 @@ static void cell_edac_count_ue(struct mem_ctl_info *mci, int chan, u64 ar)
 	offset = address & ~PAGE_MASK;
 
 	/* TODO: Decoding of the error address */
-	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 			     csrow->first_page + pfn, offset, 0,
-			     0, chan, -1, "", "", NULL);
+			     0, chan, -1, "", "");
 }
 
 static void cell_edac_check(struct mem_ctl_info *mci)
@@ -83,7 +83,7 @@ static void cell_edac_check(struct mem_ctl_info *mci)
 	fir = in_be64(&priv->regs->mic_fir);
 #ifdef DEBUG
 	if (fir != priv->prev_fir) {
-		dev_dbg(mci->dev, "fir change : 0x%016lx\n", fir);
+		dev_dbg(mci->pdev, "fir change : 0x%016lx\n", fir);
 		priv->prev_fir = fir;
 	}
 #endif
@@ -119,14 +119,14 @@ static void cell_edac_check(struct mem_ctl_info *mci)
 		mb();	/* sync up */
 #ifdef DEBUG
 		fir = in_be64(&priv->regs->mic_fir);
-		dev_dbg(mci->dev, "fir clear  : 0x%016lx\n", fir);
+		dev_dbg(mci->pdev, "fir clear  : 0x%016lx\n", fir);
 #endif
 	}
 }
 
 static void __devinit cell_edac_init_csrows(struct mem_ctl_info *mci)
 {
-	struct csrow_info		*csrow = &mci->csrows[0];
+	struct csrow_info		*csrow = mci->csrows[0];
 	struct dimm_info		*dimm;
 	struct cell_edac_priv		*priv = mci->pvt_info;
 	struct device_node		*np;
@@ -150,12 +150,12 @@ static void __devinit cell_edac_init_csrows(struct mem_ctl_info *mci)
 		csrow->last_page = csrow->first_page + nr_pages - 1;
 
 		for (j = 0; j < csrow->nr_channels; j++) {
-			dimm = csrow->channels[j].dimm;
+			dimm = csrow->channels[j]->dimm;
 			dimm->mtype = MEM_XDR;
 			dimm->edac_mode = EDAC_SECDED;
 			dimm->nr_pages = nr_pages / csrow->nr_channels;
 		}
-		dev_dbg(mci->dev,
+		dev_dbg(mci->pdev,
 			"Initialized on node %d, chanmask=0x%x,"
 			" first_page=0x%lx, nr_pages=0x%x\n",
 			priv->node, priv->chanmask,
@@ -212,7 +212,7 @@ static int __devinit cell_edac_probe(struct platform_device *pdev)
 	priv->regs = regs;
 	priv->node = pdev->id;
 	priv->chanmask = chanmask;
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_XDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_EC | EDAC_FLAG_SECDED;
 	mci->edac_cap = EDAC_FLAG_EC | EDAC_FLAG_SECDED;
diff --git a/drivers/edac/cpc925_edac.c b/drivers/edac/cpc925_edac.c
index e22030a9de6..c2ef1349587 100644
--- a/drivers/edac/cpc925_edac.c
+++ b/drivers/edac/cpc925_edac.c
@@ -316,13 +316,12 @@ static void get_total_mem(struct cpc925_mc_pdata *pdata)
 		reg += aw;
 		size = of_read_number(reg, sw);
 		reg += sw;
-		debugf1("%s: start 0x%lx, size 0x%lx\n", __func__,
-			start, size);
+		edac_dbg(1, "start 0x%lx, size 0x%lx\n", start, size);
 		pdata->total_mem += size;
 	} while (reg < reg_end);
 
 	of_node_put(np);
-	debugf0("%s: total_mem 0x%lx\n", __func__, pdata->total_mem);
+	edac_dbg(0, "total_mem 0x%lx\n", pdata->total_mem);
 }
 
 static void cpc925_init_csrows(struct mem_ctl_info *mci)
@@ -330,8 +329,9 @@ static void cpc925_init_csrows(struct mem_ctl_info *mci)
 	struct cpc925_mc_pdata *pdata = mci->pvt_info;
 	struct csrow_info *csrow;
 	struct dimm_info *dimm;
+	enum dev_type dtype;
 	int index, j;
-	u32 mbmr, mbbar, bba;
+	u32 mbmr, mbbar, bba, grain;
 	unsigned long row_size, nr_pages, last_nr_pages = 0;
 
 	get_total_mem(pdata);
@@ -347,7 +347,7 @@ static void cpc925_init_csrows(struct mem_ctl_info *mci)
 		if (bba == 0)
 			continue; /* not populated */
 
-		csrow = &mci->csrows[index];
+		csrow = mci->csrows[index];
 
 		row_size = bba * (1UL << 28);	/* 256M */
 		csrow->first_page = last_nr_pages;
@@ -355,37 +355,36 @@ static void cpc925_init_csrows(struct mem_ctl_info *mci)
 		csrow->last_page = csrow->first_page + nr_pages - 1;
 		last_nr_pages = csrow->last_page + 1;
 
+		switch (csrow->nr_channels) {
+		case 1: /* Single channel */
+			grain = 32; /* four-beat burst of 32 bytes */
+			break;
+		case 2: /* Dual channel */
+		default:
+			grain = 64; /* four-beat burst of 64 bytes */
+			break;
+		}
+		switch ((mbmr & MBMR_MODE_MASK) >> MBMR_MODE_SHIFT) {
+		case 6: /* 0110, no way to differentiate X8 VS X16 */
+		case 5:	/* 0101 */
+		case 8: /* 1000 */
+			dtype = DEV_X16;
+			break;
+		case 7: /* 0111 */
+		case 9: /* 1001 */
+			dtype = DEV_X8;
+			break;
+		default:
+			dtype = DEV_UNKNOWN;
+		break;
+		}
 		for (j = 0; j < csrow->nr_channels; j++) {
-			dimm = csrow->channels[j].dimm;
-
+			dimm = csrow->channels[j]->dimm;
 			dimm->nr_pages = nr_pages / csrow->nr_channels;
 			dimm->mtype = MEM_RDDR;
 			dimm->edac_mode = EDAC_SECDED;
-
-			switch (csrow->nr_channels) {
-			case 1: /* Single channel */
-				dimm->grain = 32; /* four-beat burst of 32 bytes */
-				break;
-			case 2: /* Dual channel */
-			default:
-				dimm->grain = 64; /* four-beat burst of 64 bytes */
-				break;
-			}
-
-			switch ((mbmr & MBMR_MODE_MASK) >> MBMR_MODE_SHIFT) {
-			case 6: /* 0110, no way to differentiate X8 VS X16 */
-			case 5:	/* 0101 */
-			case 8: /* 1000 */
-				dimm->dtype = DEV_X16;
-				break;
-			case 7: /* 0111 */
-			case 9: /* 1001 */
-				dimm->dtype = DEV_X8;
-				break;
-			default:
-				dimm->dtype = DEV_UNKNOWN;
-				break;
-			}
+			dimm->grain = grain;
+			dimm->dtype = dtype;
 		}
 	}
 }
@@ -463,7 +462,7 @@ static void cpc925_mc_get_pfn(struct mem_ctl_info *mci, u32 mear,
 	*csrow = rank;
 
 #ifdef CONFIG_EDAC_DEBUG
-	if (mci->csrows[rank].first_page == 0) {
+	if (mci->csrows[rank]->first_page == 0) {
 		cpc925_mc_printk(mci, KERN_ERR, "ECC occurs in a "
 			"non-populated csrow, broken hardware?\n");
 		return;
@@ -471,7 +470,7 @@ static void cpc925_mc_get_pfn(struct mem_ctl_info *mci, u32 mear,
 #endif
 
 	/* Revert csrow number */
-	pa = mci->csrows[rank].first_page << PAGE_SHIFT;
+	pa = mci->csrows[rank]->first_page << PAGE_SHIFT;
 
 	/* Revert column address */
 	col += bcnt;
@@ -512,7 +511,7 @@ static void cpc925_mc_get_pfn(struct mem_ctl_info *mci, u32 mear,
 	*offset = pa & (PAGE_SIZE - 1);
 	*pfn = pa >> PAGE_SHIFT;
 
-	debugf0("%s: ECC physical address 0x%lx\n", __func__, pa);
+	edac_dbg(0, "ECC physical address 0x%lx\n", pa);
 }
 
 static int cpc925_mc_find_channel(struct mem_ctl_info *mci, u16 syndrome)
@@ -555,18 +554,18 @@ static void cpc925_mc_check(struct mem_ctl_info *mci)
 	if (apiexcp & CECC_EXCP_DETECTED) {
 		cpc925_mc_printk(mci, KERN_INFO, "DRAM CECC Fault\n");
 		channel = cpc925_mc_find_channel(mci, syndrome);
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     pfn, offset, syndrome,
 				     csrow, channel, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 	}
 
 	if (apiexcp & UECC_EXCP_DETECTED) {
 		cpc925_mc_printk(mci, KERN_INFO, "DRAM UECC Fault\n");
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     pfn, offset, 0,
 				     csrow, -1, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 	}
 
 	cpc925_mc_printk(mci, KERN_INFO, "Dump registers:\n");
@@ -852,8 +851,8 @@ static void cpc925_add_edac_devices(void __iomem *vbase)
 			goto err2;
 		}
 
-		debugf0("%s: Successfully added edac device for %s\n",
-			__func__, dev_info->ctl_name);
+		edac_dbg(0, "Successfully added edac device for %s\n",
+			 dev_info->ctl_name);
 
 		continue;
 
@@ -884,8 +883,8 @@ static void cpc925_del_edac_devices(void)
 		if (dev_info->exit)
 			dev_info->exit(dev_info);
 
-		debugf0("%s: Successfully deleted edac device for %s\n",
-			__func__, dev_info->ctl_name);
+		edac_dbg(0, "Successfully deleted edac device for %s\n",
+			 dev_info->ctl_name);
 	}
 }
 
@@ -900,7 +899,7 @@ static int cpc925_get_sdram_scrub_rate(struct mem_ctl_info *mci)
 	mscr = __raw_readl(pdata->vbase + REG_MSCR_OFFSET);
 	si = (mscr & MSCR_SI_MASK) >> MSCR_SI_SHIFT;
 
-	debugf0("%s, Mem Scrub Ctrl Register 0x%x\n", __func__, mscr);
+	edac_dbg(0, "Mem Scrub Ctrl Register 0x%x\n", mscr);
 
 	if (((mscr & MSCR_SCRUB_MOD_MASK) != MSCR_BACKGR_SCRUB) ||
 	    (si == 0)) {
@@ -928,8 +927,7 @@ static int cpc925_mc_get_channels(void __iomem *vbase)
 	    ((mbcr & MBCR_64BITBUS_MASK) == 0))
 		dual = 1;
 
-	debugf0("%s: %s channel\n", __func__,
-		(dual > 0) ? "Dual" : "Single");
+	edac_dbg(0, "%s channel\n", (dual > 0) ? "Dual" : "Single");
 
 	return dual;
 }
@@ -944,7 +942,7 @@ static int __devinit cpc925_probe(struct platform_device *pdev)
 	struct resource *r;
 	int res = 0, nr_channels;
 
-	debugf0("%s: %s platform device found!\n", __func__, pdev->name);
+	edac_dbg(0, "%s platform device found!\n", pdev->name);
 
 	if (!devres_open_group(&pdev->dev, cpc925_probe, GFP_KERNEL)) {
 		res = -ENOMEM;
@@ -995,7 +993,7 @@ static int __devinit cpc925_probe(struct platform_device *pdev)
 	pdata->edac_idx = edac_mc_idx++;
 	pdata->name = pdev->name;
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	platform_set_drvdata(pdev, mci);
 	mci->dev_name = dev_name(&pdev->dev);
 	mci->mtype_cap = MEM_FLAG_RDDR | MEM_FLAG_DDR;
@@ -1026,7 +1024,7 @@ static int __devinit cpc925_probe(struct platform_device *pdev)
 	cpc925_add_edac_devices(vbase);
 
 	/* get this far and it's successful */
-	debugf0("%s: success\n", __func__);
+	edac_dbg(0, "success\n");
 
 	res = 0;
 	goto out;
diff --git a/drivers/edac/e752x_edac.c b/drivers/edac/e752x_edac.c
index 3186512c973..a5ed6b795fd 100644
--- a/drivers/edac/e752x_edac.c
+++ b/drivers/edac/e752x_edac.c
@@ -309,7 +309,7 @@ static unsigned long ctl_page_to_phys(struct mem_ctl_info *mci,
 	u32 remap;
 	struct e752x_pvt *pvt = (struct e752x_pvt *)mci->pvt_info;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	if (page < pvt->tolm)
 		return page;
@@ -335,7 +335,7 @@ static void do_process_ce(struct mem_ctl_info *mci, u16 error_one,
 	int i;
 	struct e752x_pvt *pvt = (struct e752x_pvt *)mci->pvt_info;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	/* convert the addr to 4k page */
 	page = sec1_add >> (PAGE_SHIFT - 4);
@@ -371,10 +371,10 @@ static void do_process_ce(struct mem_ctl_info *mci, u16 error_one,
 	channel = !(error_one & 1);
 
 	/* e752x mc reads 34:6 of the DRAM linear address */
-	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 			     page, offset_in_page(sec1_add << 4), sec1_syndrome,
 			     row, channel, -1,
-			     "e752x CE", "", NULL);
+			     "e752x CE", "");
 }
 
 static inline void process_ce(struct mem_ctl_info *mci, u16 error_one,
@@ -394,7 +394,7 @@ static void do_process_ue(struct mem_ctl_info *mci, u16 error_one,
 	int row;
 	struct e752x_pvt *pvt = (struct e752x_pvt *)mci->pvt_info;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	if (error_one & 0x0202) {
 		error_2b = ded_add;
@@ -408,11 +408,11 @@ static void do_process_ue(struct mem_ctl_info *mci, u16 error_one,
 			edac_mc_find_csrow_by_page(mci, block_page);
 
 		/* e752x mc reads 34:6 of the DRAM linear address */
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					block_page,
 					offset_in_page(error_2b << 4), 0,
 					 row, -1, -1,
-					"e752x UE from Read", "", NULL);
+					"e752x UE from Read", "");
 
 	}
 	if (error_one & 0x0404) {
@@ -427,11 +427,11 @@ static void do_process_ue(struct mem_ctl_info *mci, u16 error_one,
 			edac_mc_find_csrow_by_page(mci, block_page);
 
 		/* e752x mc reads 34:6 of the DRAM linear address */
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					block_page,
 					offset_in_page(error_2b << 4), 0,
 					row, -1, -1,
-					"e752x UE from Scruber", "", NULL);
+					"e752x UE from Scruber", "");
 	}
 }
 
@@ -453,10 +453,10 @@ static inline void process_ue_no_info_wr(struct mem_ctl_info *mci,
 	if (!handle_error)
 		return;
 
-	debugf3("%s()\n", __func__);
-	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+	edac_dbg(3, "\n");
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
 			     -1, -1, -1,
-			     "e752x UE log memory write", "", NULL);
+			     "e752x UE log memory write", "");
 }
 
 static void do_process_ded_retry(struct mem_ctl_info *mci, u16 error,
@@ -982,7 +982,7 @@ static void e752x_check(struct mem_ctl_info *mci)
 {
 	struct e752x_error_info info;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 	e752x_get_error_info(mci, &info);
 	e752x_process_error_info(mci, &info, 1);
 }
@@ -1069,6 +1069,7 @@ static void e752x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 			u16 ddrcsr)
 {
 	struct csrow_info *csrow;
+	enum edac_type edac_mode;
 	unsigned long last_cumul_size;
 	int index, mem_dev, drc_chan;
 	int drc_drbg;		/* DRB granularity 0=64mb, 1=128mb */
@@ -1095,14 +1096,13 @@ static void e752x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 	for (last_cumul_size = index = 0; index < mci->nr_csrows; index++) {
 		/* mem_dev 0=x8, 1=x4 */
 		mem_dev = (dra >> (index * 4 + 2)) & 0x3;
-		csrow = &mci->csrows[remap_csrow_index(mci, index)];
+		csrow = mci->csrows[remap_csrow_index(mci, index)];
 
 		mem_dev = (mem_dev == 2);
 		pci_read_config_byte(pdev, E752X_DRB + index, &value);
 		/* convert a 128 or 64 MiB DRB to a page size. */
 		cumul_size = value << (25 + drc_drbg - PAGE_SHIFT);
-		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
-			cumul_size);
+		edac_dbg(3, "(%d) cumul_size 0x%x\n", index, cumul_size);
 		if (cumul_size == last_cumul_size)
 			continue;	/* not populated */
 
@@ -1111,29 +1111,29 @@ static void e752x_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 		nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
 
+		/*
+		* if single channel or x8 devices then SECDED
+		* if dual channel and x4 then S4ECD4ED
+		*/
+		if (drc_ddim) {
+			if (drc_chan && mem_dev) {
+				edac_mode = EDAC_S4ECD4ED;
+				mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
+			} else {
+				edac_mode = EDAC_SECDED;
+				mci->edac_cap |= EDAC_FLAG_SECDED;
+			}
+		} else
+			edac_mode = EDAC_NONE;
 		for (i = 0; i < csrow->nr_channels; i++) {
-			struct dimm_info *dimm = csrow->channels[i].dimm;
+			struct dimm_info *dimm = csrow->channels[i]->dimm;
 
-			debugf3("Initializing rank at (%i,%i)\n", index, i);
+			edac_dbg(3, "Initializing rank at (%i,%i)\n", index, i);
 			dimm->nr_pages = nr_pages / csrow->nr_channels;
 			dimm->grain = 1 << 12;	/* 4KiB - resolution of CELOG */
 			dimm->mtype = MEM_RDDR;	/* only one type supported */
 			dimm->dtype = mem_dev ? DEV_X4 : DEV_X8;
-
-			/*
-			* if single channel or x8 devices then SECDED
-			* if dual channel and x4 then S4ECD4ED
-			*/
-			if (drc_ddim) {
-				if (drc_chan && mem_dev) {
-					dimm->edac_mode = EDAC_S4ECD4ED;
-					mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
-				} else {
-					dimm->edac_mode = EDAC_SECDED;
-					mci->edac_cap |= EDAC_FLAG_SECDED;
-				}
-			} else
-				dimm->edac_mode = EDAC_NONE;
+			dimm->edac_mode = edac_mode;
 		}
 	}
 }
@@ -1269,8 +1269,8 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	int drc_chan;		/* Number of channels 0=1chan,1=2chan */
 	struct e752x_error_info discard;
 
-	debugf0("%s(): mci\n", __func__);
-	debugf0("Starting Probe1\n");
+	edac_dbg(0, "mci\n");
+	edac_dbg(0, "Starting Probe1\n");
 
 	/* check to see if device 0 function 1 is enabled; if it isn't, we
 	 * assume the BIOS has reserved it for a reason and is expecting
@@ -1300,7 +1300,7 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf3("%s(): init mci\n", __func__);
+	edac_dbg(3, "init mci\n");
 	mci->mtype_cap = MEM_FLAG_RDDR;
 	/* 3100 IMCH supports SECDEC only */
 	mci->edac_ctl_cap = (dev_idx == I3100) ? EDAC_FLAG_SECDED :
@@ -1308,9 +1308,9 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	/* FIXME - what if different memory types are in different csrows? */
 	mci->mod_name = EDAC_MOD_STR;
 	mci->mod_ver = E752X_REVISION;
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 
-	debugf3("%s(): init pvt\n", __func__);
+	edac_dbg(3, "init pvt\n");
 	pvt = (struct e752x_pvt *)mci->pvt_info;
 	pvt->dev_info = &e752x_devs[dev_idx];
 	pvt->mc_symmetric = ((ddrcsr & 0x10) != 0);
@@ -1320,7 +1320,7 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 		return -ENODEV;
 	}
 
-	debugf3("%s(): more mci init\n", __func__);
+	edac_dbg(3, "more mci init\n");
 	mci->ctl_name = pvt->dev_info->ctl_name;
 	mci->dev_name = pci_name(pdev);
 	mci->edac_check = e752x_check;
@@ -1342,7 +1342,7 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 		mci->edac_cap = EDAC_FLAG_SECDED; /* the only mode supported */
 	else
 		mci->edac_cap |= EDAC_FLAG_NONE;
-	debugf3("%s(): tolm, remapbase, remaplimit\n", __func__);
+	edac_dbg(3, "tolm, remapbase, remaplimit\n");
 
 	/* load the top of low memory, remap base, and remap limit vars */
 	pci_read_config_word(pdev, E752X_TOLM, &pci_data);
@@ -1359,7 +1359,7 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	 * type of memory controller.  The ID is therefore hardcoded to 0.
 	 */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
@@ -1377,7 +1377,7 @@ static int e752x_probe1(struct pci_dev *pdev, int dev_idx)
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	return 0;
 
 fail:
@@ -1393,7 +1393,7 @@ fail:
 static int __devinit e752x_init_one(struct pci_dev *pdev,
 				const struct pci_device_id *ent)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* wake up and enable device */
 	if (pci_enable_device(pdev) < 0)
@@ -1407,7 +1407,7 @@ static void __devexit e752x_remove_one(struct pci_dev *pdev)
 	struct mem_ctl_info *mci;
 	struct e752x_pvt *pvt;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (e752x_pci)
 		edac_pci_release_generic_ctl(e752x_pci);
@@ -1453,7 +1453,7 @@ static int __init e752x_init(void)
 {
 	int pci_rc;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -1464,7 +1464,7 @@ static int __init e752x_init(void)
 
 static void __exit e752x_exit(void)
 {
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 	pci_unregister_driver(&e752x_driver);
 }
 
diff --git a/drivers/edac/e7xxx_edac.c b/drivers/edac/e7xxx_edac.c
index 9a9c1a54679..9ff57f361a4 100644
--- a/drivers/edac/e7xxx_edac.c
+++ b/drivers/edac/e7xxx_edac.c
@@ -166,7 +166,7 @@ static const struct e7xxx_dev_info e7xxx_devs[] = {
 /* FIXME - is this valid for both SECDED and S4ECD4ED? */
 static inline int e7xxx_find_channel(u16 syndrome)
 {
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	if ((syndrome & 0xff00) == 0)
 		return 0;
@@ -186,7 +186,7 @@ static unsigned long ctl_page_to_phys(struct mem_ctl_info *mci,
 	u32 remap;
 	struct e7xxx_pvt *pvt = (struct e7xxx_pvt *)mci->pvt_info;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	if ((page < pvt->tolm) ||
 		((page >= 0x100000) && (page < pvt->remapbase)))
@@ -208,7 +208,7 @@ static void process_ce(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
 	int row;
 	int channel;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 	/* read the error address */
 	error_1b = info->dram_celog_add;
 	/* FIXME - should use PAGE_SHIFT */
@@ -219,15 +219,15 @@ static void process_ce(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
 	row = edac_mc_find_csrow_by_page(mci, page);
 	/* convert syndrome to channel */
 	channel = e7xxx_find_channel(syndrome);
-	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, page, 0, syndrome,
-			     row, channel, -1, "e7xxx CE", "", NULL);
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1, page, 0, syndrome,
+			     row, channel, -1, "e7xxx CE", "");
 }
 
 static void process_ce_no_info(struct mem_ctl_info *mci)
 {
-	debugf3("%s()\n", __func__);
-	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0, -1, -1, -1,
-			     "e7xxx CE log register overflow", "", NULL);
+	edac_dbg(3, "\n");
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0, -1, -1, -1,
+			     "e7xxx CE log register overflow", "");
 }
 
 static void process_ue(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
@@ -235,23 +235,23 @@ static void process_ue(struct mem_ctl_info *mci, struct e7xxx_error_info *info)
 	u32 error_2b, block_page;
 	int row;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 	/* read the error address */
 	error_2b = info->dram_uelog_add;
 	/* FIXME - should use PAGE_SHIFT */
 	block_page = error_2b >> 6;	/* convert to 4k address */
 	row = edac_mc_find_csrow_by_page(mci, block_page);
 
-	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, block_page, 0, 0,
-			     row, -1, -1, "e7xxx UE", "", NULL);
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, block_page, 0, 0,
+			     row, -1, -1, "e7xxx UE", "");
 }
 
 static void process_ue_no_info(struct mem_ctl_info *mci)
 {
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
-	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0, -1, -1, -1,
-			     "e7xxx UE log register overflow", "", NULL);
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0, -1, -1, -1,
+			     "e7xxx UE log register overflow", "");
 }
 
 static void e7xxx_get_error_info(struct mem_ctl_info *mci,
@@ -334,7 +334,7 @@ static void e7xxx_check(struct mem_ctl_info *mci)
 {
 	struct e7xxx_error_info info;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 	e7xxx_get_error_info(mci, &info);
 	e7xxx_process_error_info(mci, &info, 1);
 }
@@ -362,6 +362,7 @@ static void e7xxx_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 	int drc_chan, drc_drbg, drc_ddim, mem_dev;
 	struct csrow_info *csrow;
 	struct dimm_info *dimm;
+	enum edac_type edac_mode;
 
 	pci_read_config_dword(pdev, E7XXX_DRA, &dra);
 	drc_chan = dual_channel_active(drc, dev_idx);
@@ -377,13 +378,12 @@ static void e7xxx_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 	for (index = 0; index < mci->nr_csrows; index++) {
 		/* mem_dev 0=x8, 1=x4 */
 		mem_dev = (dra >> (index * 4 + 3)) & 0x1;
-		csrow = &mci->csrows[index];
+		csrow = mci->csrows[index];
 
 		pci_read_config_byte(pdev, E7XXX_DRB + index, &value);
 		/* convert a 64 or 32 MiB DRB to a page size. */
 		cumul_size = value << (25 + drc_drbg - PAGE_SHIFT);
-		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
-			cumul_size);
+		edac_dbg(3, "(%d) cumul_size 0x%x\n", index, cumul_size);
 		if (cumul_size == last_cumul_size)
 			continue;	/* not populated */
 
@@ -392,28 +392,29 @@ static void e7xxx_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 		nr_pages = cumul_size - last_cumul_size;
 		last_cumul_size = cumul_size;
 
+		/*
+		* if single channel or x8 devices then SECDED
+		* if dual channel and x4 then S4ECD4ED
+		*/
+		if (drc_ddim) {
+			if (drc_chan && mem_dev) {
+				edac_mode = EDAC_S4ECD4ED;
+				mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
+			} else {
+				edac_mode = EDAC_SECDED;
+				mci->edac_cap |= EDAC_FLAG_SECDED;
+			}
+		} else
+			edac_mode = EDAC_NONE;
+
 		for (j = 0; j < drc_chan + 1; j++) {
-			dimm = csrow->channels[j].dimm;
+			dimm = csrow->channels[j]->dimm;
 
 			dimm->nr_pages = nr_pages / (drc_chan + 1);
 			dimm->grain = 1 << 12;	/* 4KiB - resolution of CELOG */
 			dimm->mtype = MEM_RDDR;	/* only one type supported */
 			dimm->dtype = mem_dev ? DEV_X4 : DEV_X8;
-
-			/*
-			* if single channel or x8 devices then SECDED
-			* if dual channel and x4 then S4ECD4ED
-			*/
-			if (drc_ddim) {
-				if (drc_chan && mem_dev) {
-					dimm->edac_mode = EDAC_S4ECD4ED;
-					mci->edac_cap |= EDAC_FLAG_S4ECD4ED;
-				} else {
-					dimm->edac_mode = EDAC_SECDED;
-					mci->edac_cap |= EDAC_FLAG_SECDED;
-				}
-			} else
-				dimm->edac_mode = EDAC_NONE;
+			dimm->edac_mode = edac_mode;
 		}
 	}
 }
@@ -428,7 +429,7 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 	int drc_chan;
 	struct e7xxx_error_info discard;
 
-	debugf0("%s(): mci\n", __func__);
+	edac_dbg(0, "mci\n");
 
 	pci_read_config_dword(pdev, E7XXX_DRC, &drc);
 
@@ -451,15 +452,15 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf3("%s(): init mci\n", __func__);
+	edac_dbg(3, "init mci\n");
 	mci->mtype_cap = MEM_FLAG_RDDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED |
 		EDAC_FLAG_S4ECD4ED;
 	/* FIXME - what if different memory types are in different csrows? */
 	mci->mod_name = EDAC_MOD_STR;
 	mci->mod_ver = E7XXX_REVISION;
-	mci->dev = &pdev->dev;
-	debugf3("%s(): init pvt\n", __func__);
+	mci->pdev = &pdev->dev;
+	edac_dbg(3, "init pvt\n");
 	pvt = (struct e7xxx_pvt *)mci->pvt_info;
 	pvt->dev_info = &e7xxx_devs[dev_idx];
 	pvt->bridge_ck = pci_get_device(PCI_VENDOR_ID_INTEL,
@@ -472,14 +473,14 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 		goto fail0;
 	}
 
-	debugf3("%s(): more mci init\n", __func__);
+	edac_dbg(3, "more mci init\n");
 	mci->ctl_name = pvt->dev_info->ctl_name;
 	mci->dev_name = pci_name(pdev);
 	mci->edac_check = e7xxx_check;
 	mci->ctl_page_to_phys = ctl_page_to_phys;
 	e7xxx_init_csrows(mci, pdev, dev_idx, drc);
 	mci->edac_cap |= EDAC_FLAG_NONE;
-	debugf3("%s(): tolm, remapbase, remaplimit\n", __func__);
+	edac_dbg(3, "tolm, remapbase, remaplimit\n");
 	/* load the top of low memory, remap base, and remap limit vars */
 	pci_read_config_word(pdev, E7XXX_TOLM, &pci_data);
 	pvt->tolm = ((u32) pci_data) << 4;
@@ -498,7 +499,7 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 	 * type of memory controller.  The ID is therefore hardcoded to 0.
 	 */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail1;
 	}
 
@@ -514,7 +515,7 @@ static int e7xxx_probe1(struct pci_dev *pdev, int dev_idx)
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	return 0;
 
 fail1:
@@ -530,7 +531,7 @@ fail0:
 static int __devinit e7xxx_init_one(struct pci_dev *pdev,
 				const struct pci_device_id *ent)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* wake up and enable device */
 	return pci_enable_device(pdev) ?
@@ -542,7 +543,7 @@ static void __devexit e7xxx_remove_one(struct pci_dev *pdev)
 	struct mem_ctl_info *mci;
 	struct e7xxx_pvt *pvt;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (e7xxx_pci)
 		edac_pci_release_generic_ctl(e7xxx_pci);
diff --git a/drivers/edac/edac_core.h b/drivers/edac/edac_core.h
index 117490d4f83..23bb99fa44f 100644
--- a/drivers/edac/edac_core.h
+++ b/drivers/edac/edac_core.h
@@ -71,26 +71,21 @@ extern const char *edac_mem_types[];
 #ifdef CONFIG_EDAC_DEBUG
 extern int edac_debug_level;
 
-#define edac_debug_printk(level, fmt, arg...)                           \
-	do {                                                            \
-		if (level <= edac_debug_level)                          \
-			edac_printk(KERN_DEBUG, EDAC_DEBUG,		\
-				    "%s: " fmt, __func__, ##arg);	\
-	} while (0)
-
-#define debugf0( ... ) edac_debug_printk(0, __VA_ARGS__ )
-#define debugf1( ... ) edac_debug_printk(1, __VA_ARGS__ )
-#define debugf2( ... ) edac_debug_printk(2, __VA_ARGS__ )
-#define debugf3( ... ) edac_debug_printk(3, __VA_ARGS__ )
-#define debugf4( ... ) edac_debug_printk(4, __VA_ARGS__ )
+#define edac_dbg(level, fmt, ...)					\
+do {									\
+	if (level <= edac_debug_level)					\
+		edac_printk(KERN_DEBUG, EDAC_DEBUG,			\
+			    "%s: " fmt, __func__, ##__VA_ARGS__);	\
+} while (0)
 
 #else				/* !CONFIG_EDAC_DEBUG */
 
-#define debugf0( ... )
-#define debugf1( ... )
-#define debugf2( ... )
-#define debugf3( ... )
-#define debugf4( ... )
+#define edac_dbg(level, fmt, ...)					\
+do {									\
+	if (0)								\
+		edac_printk(KERN_DEBUG, EDAC_DEBUG,			\
+			    "%s: " fmt, __func__, ##__VA_ARGS__);	\
+} while (0)
 
 #endif				/* !CONFIG_EDAC_DEBUG */
 
@@ -460,15 +455,15 @@ extern int edac_mc_find_csrow_by_page(struct mem_ctl_info *mci,
 				      unsigned long page);
 void edac_mc_handle_error(const enum hw_event_mc_err_type type,
 			  struct mem_ctl_info *mci,
+			  const u16 error_count,
 			  const unsigned long page_frame_number,
 			  const unsigned long offset_in_page,
 			  const unsigned long syndrome,
-			  const int layer0,
-			  const int layer1,
-			  const int layer2,
+			  const int top_layer,
+			  const int mid_layer,
+			  const int low_layer,
 			  const char *msg,
-			  const char *other_detail,
-			  const void *mcelog);
+			  const char *other_detail);
 
 /*
  * edac_device APIs
diff --git a/drivers/edac/edac_device.c b/drivers/edac/edac_device.c
index ee3f1f810c1..211021dfec7 100644
--- a/drivers/edac/edac_device.c
+++ b/drivers/edac/edac_device.c
@@ -40,12 +40,13 @@ static LIST_HEAD(edac_device_list);
 #ifdef CONFIG_EDAC_DEBUG
 static void edac_device_dump_device(struct edac_device_ctl_info *edac_dev)
 {
-	debugf3("\tedac_dev = %p dev_idx=%d \n", edac_dev, edac_dev->dev_idx);
-	debugf4("\tedac_dev->edac_check = %p\n", edac_dev->edac_check);
-	debugf3("\tdev = %p\n", edac_dev->dev);
-	debugf3("\tmod_name:ctl_name = %s:%s\n",
-		edac_dev->mod_name, edac_dev->ctl_name);
-	debugf3("\tpvt_info = %p\n\n", edac_dev->pvt_info);
+	edac_dbg(3, "\tedac_dev = %p dev_idx=%d\n",
+		 edac_dev, edac_dev->dev_idx);
+	edac_dbg(4, "\tedac_dev->edac_check = %p\n", edac_dev->edac_check);
+	edac_dbg(3, "\tdev = %p\n", edac_dev->dev);
+	edac_dbg(3, "\tmod_name:ctl_name = %s:%s\n",
+		 edac_dev->mod_name, edac_dev->ctl_name);
+	edac_dbg(3, "\tpvt_info = %p\n\n", edac_dev->pvt_info);
 }
 #endif				/* CONFIG_EDAC_DEBUG */
 
@@ -82,8 +83,7 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 	void *pvt, *p;
 	int err;
 
-	debugf4("%s() instances=%d blocks=%d\n",
-		__func__, nr_instances, nr_blocks);
+	edac_dbg(4, "instances=%d blocks=%d\n", nr_instances, nr_blocks);
 
 	/* Calculate the size of memory we need to allocate AND
 	 * determine the offsets of the various item arrays
@@ -156,8 +156,8 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 	/* Name of this edac device */
 	snprintf(dev_ctl->name,sizeof(dev_ctl->name),"%s",edac_device_name);
 
-	debugf4("%s() edac_dev=%p next after end=%p\n",
-		__func__, dev_ctl, pvt + sz_private );
+	edac_dbg(4, "edac_dev=%p next after end=%p\n",
+		 dev_ctl, pvt + sz_private);
 
 	/* Initialize every Instance */
 	for (instance = 0; instance < nr_instances; instance++) {
@@ -178,10 +178,8 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 			snprintf(blk->name, sizeof(blk->name),
 				 "%s%d", edac_block_name, block+offset_value);
 
-			debugf4("%s() instance=%d inst_p=%p block=#%d "
-				"block_p=%p name='%s'\n",
-				__func__, instance, inst, block,
-				blk, blk->name);
+			edac_dbg(4, "instance=%d inst_p=%p block=#%d block_p=%p name='%s'\n",
+				 instance, inst, block, blk, blk->name);
 
 			/* if there are NO attributes OR no attribute pointer
 			 * then continue on to next block iteration
@@ -194,8 +192,8 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 			attrib_p = &dev_attrib[block*nr_instances*nr_attrib];
 			blk->block_attributes = attrib_p;
 
-			debugf4("%s() THIS BLOCK_ATTRIB=%p\n",
-				__func__, blk->block_attributes);
+			edac_dbg(4, "THIS BLOCK_ATTRIB=%p\n",
+				 blk->block_attributes);
 
 			/* Initialize every user specified attribute in this
 			 * block with the data the caller passed in
@@ -214,11 +212,10 @@ struct edac_device_ctl_info *edac_device_alloc_ctl_info(
 
 				attrib->block = blk;	/* up link */
 
-				debugf4("%s() alloc-attrib=%p attrib_name='%s' "
-					"attrib-spec=%p spec-name=%s\n",
-					__func__, attrib, attrib->attr.name,
-					&attrib_spec[attr],
-					attrib_spec[attr].attr.name
+				edac_dbg(4, "alloc-attrib=%p attrib_name='%s' attrib-spec=%p spec-name=%s\n",
+					 attrib, attrib->attr.name,
+					 &attrib_spec[attr],
+					 attrib_spec[attr].attr.name
 					);
 			}
 		}
@@ -273,7 +270,7 @@ static struct edac_device_ctl_info *find_edac_device_by_dev(struct device *dev)
 	struct edac_device_ctl_info *edac_dev;
 	struct list_head *item;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	list_for_each(item, &edac_device_list) {
 		edac_dev = list_entry(item, struct edac_device_ctl_info, link);
@@ -408,7 +405,7 @@ static void edac_device_workq_function(struct work_struct *work_req)
 void edac_device_workq_setup(struct edac_device_ctl_info *edac_dev,
 				unsigned msec)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* take the arg 'msec' and set it into the control structure
 	 * to used in the time period calculation
@@ -496,7 +493,7 @@ EXPORT_SYMBOL_GPL(edac_device_alloc_index);
  */
 int edac_device_add_device(struct edac_device_ctl_info *edac_dev)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 #ifdef CONFIG_EDAC_DEBUG
 	if (edac_debug_level >= 3)
@@ -570,7 +567,7 @@ struct edac_device_ctl_info *edac_device_del_device(struct device *dev)
 {
 	struct edac_device_ctl_info *edac_dev;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	mutex_lock(&device_ctls_mutex);
 
diff --git a/drivers/edac/edac_device_sysfs.c b/drivers/edac/edac_device_sysfs.c
index b4ea185cceb..fb68a06ad68 100644
--- a/drivers/edac/edac_device_sysfs.c
+++ b/drivers/edac/edac_device_sysfs.c
@@ -202,7 +202,7 @@ static void edac_device_ctrl_master_release(struct kobject *kobj)
 {
 	struct edac_device_ctl_info *edac_dev = to_edacdev(kobj);
 
-	debugf4("%s() control index=%d\n", __func__, edac_dev->dev_idx);
+	edac_dbg(4, "control index=%d\n", edac_dev->dev_idx);
 
 	/* decrement the EDAC CORE module ref count */
 	module_put(edac_dev->owner);
@@ -233,12 +233,12 @@ int edac_device_register_sysfs_main_kobj(struct edac_device_ctl_info *edac_dev)
 	struct bus_type *edac_subsys;
 	int err;
 
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	/* get the /sys/devices/system/edac reference */
 	edac_subsys = edac_get_sysfs_subsys();
 	if (edac_subsys == NULL) {
-		debugf1("%s() no edac_subsys error\n", __func__);
+		edac_dbg(1, "no edac_subsys error\n");
 		err = -ENODEV;
 		goto err_out;
 	}
@@ -264,8 +264,8 @@ int edac_device_register_sysfs_main_kobj(struct edac_device_ctl_info *edac_dev)
 				   &edac_subsys->dev_root->kobj,
 				   "%s", edac_dev->name);
 	if (err) {
-		debugf1("%s()Failed to register '.../edac/%s'\n",
-			__func__, edac_dev->name);
+		edac_dbg(1, "Failed to register '.../edac/%s'\n",
+			 edac_dev->name);
 		goto err_kobj_reg;
 	}
 	kobject_uevent(&edac_dev->kobj, KOBJ_ADD);
@@ -274,8 +274,7 @@ int edac_device_register_sysfs_main_kobj(struct edac_device_ctl_info *edac_dev)
 	 * edac_device_unregister_sysfs_main_kobj() must be used
 	 */
 
-	debugf4("%s() Registered '.../edac/%s' kobject\n",
-		__func__, edac_dev->name);
+	edac_dbg(4, "Registered '.../edac/%s' kobject\n", edac_dev->name);
 
 	return 0;
 
@@ -296,9 +295,8 @@ err_out:
  */
 void edac_device_unregister_sysfs_main_kobj(struct edac_device_ctl_info *dev)
 {
-	debugf0("%s()\n", __func__);
-	debugf4("%s() name of kobject is: %s\n",
-		__func__, kobject_name(&dev->kobj));
+	edac_dbg(0, "\n");
+	edac_dbg(4, "name of kobject is: %s\n", kobject_name(&dev->kobj));
 
 	/*
 	 * Unregister the edac device's kobject and
@@ -336,7 +334,7 @@ static void edac_device_ctrl_instance_release(struct kobject *kobj)
 {
 	struct edac_device_instance *instance;
 
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	/* map from this kobj to the main control struct
 	 * and then dec the main kobj count
@@ -442,7 +440,7 @@ static void edac_device_ctrl_block_release(struct kobject *kobj)
 {
 	struct edac_device_block *block;
 
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	/* get the container of the kobj */
 	block = to_block(kobj);
@@ -524,10 +522,10 @@ static int edac_device_create_block(struct edac_device_ctl_info *edac_dev,
 	struct edac_dev_sysfs_block_attribute *sysfs_attrib;
 	struct kobject *main_kobj;
 
-	debugf4("%s() Instance '%s' inst_p=%p  block '%s'  block_p=%p\n",
-		__func__, instance->name, instance, block->name, block);
-	debugf4("%s() block kobj=%p  block kobj->parent=%p\n",
-		__func__, &block->kobj, &block->kobj.parent);
+	edac_dbg(4, "Instance '%s' inst_p=%p  block '%s'  block_p=%p\n",
+		 instance->name, instance, block->name, block);
+	edac_dbg(4, "block kobj=%p  block kobj->parent=%p\n",
+		 &block->kobj, &block->kobj.parent);
 
 	/* init this block's kobject */
 	memset(&block->kobj, 0, sizeof(struct kobject));
@@ -546,8 +544,7 @@ static int edac_device_create_block(struct edac_device_ctl_info *edac_dev,
 				   &instance->kobj,
 				   "%s", block->name);
 	if (err) {
-		debugf1("%s() Failed to register instance '%s'\n",
-			__func__, block->name);
+		edac_dbg(1, "Failed to register instance '%s'\n", block->name);
 		kobject_put(main_kobj);
 		err = -ENODEV;
 		goto err_out;
@@ -560,11 +557,9 @@ static int edac_device_create_block(struct edac_device_ctl_info *edac_dev,
 	if (sysfs_attrib && block->nr_attribs) {
 		for (i = 0; i < block->nr_attribs; i++, sysfs_attrib++) {
 
-			debugf4("%s() creating block attrib='%s' "
-				"attrib->%p to kobj=%p\n",
-				__func__,
-				sysfs_attrib->attr.name,
-				sysfs_attrib, &block->kobj);
+			edac_dbg(4, "creating block attrib='%s' attrib->%p to kobj=%p\n",
+				 sysfs_attrib->attr.name,
+				 sysfs_attrib, &block->kobj);
 
 			/* Create each block_attribute file */
 			err = sysfs_create_file(&block->kobj,
@@ -647,14 +642,14 @@ static int edac_device_create_instance(struct edac_device_ctl_info *edac_dev,
 	err = kobject_init_and_add(&instance->kobj, &ktype_instance_ctrl,
 				   &edac_dev->kobj, "%s", instance->name);
 	if (err != 0) {
-		debugf2("%s() Failed to register instance '%s'\n",
-			__func__, instance->name);
+		edac_dbg(2, "Failed to register instance '%s'\n",
+			 instance->name);
 		kobject_put(main_kobj);
 		goto err_out;
 	}
 
-	debugf4("%s() now register '%d' blocks for instance %d\n",
-		__func__, instance->nr_blocks, idx);
+	edac_dbg(4, "now register '%d' blocks for instance %d\n",
+		 instance->nr_blocks, idx);
 
 	/* register all blocks of this instance */
 	for (i = 0; i < instance->nr_blocks; i++) {
@@ -670,8 +665,8 @@ static int edac_device_create_instance(struct edac_device_ctl_info *edac_dev,
 	}
 	kobject_uevent(&instance->kobj, KOBJ_ADD);
 
-	debugf4("%s() Registered instance %d '%s' kobject\n",
-		__func__, idx, instance->name);
+	edac_dbg(4, "Registered instance %d '%s' kobject\n",
+		 idx, instance->name);
 
 	return 0;
 
@@ -715,7 +710,7 @@ static int edac_device_create_instances(struct edac_device_ctl_info *edac_dev)
 	int i, j;
 	int err;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* iterate over creation of the instances */
 	for (i = 0; i < edac_dev->nr_instances; i++) {
@@ -817,12 +812,12 @@ int edac_device_create_sysfs(struct edac_device_ctl_info *edac_dev)
 	int err;
 	struct kobject *edac_kobj = &edac_dev->kobj;
 
-	debugf0("%s() idx=%d\n", __func__, edac_dev->dev_idx);
+	edac_dbg(0, "idx=%d\n", edac_dev->dev_idx);
 
 	/*  go create any main attributes callers wants */
 	err = edac_device_add_main_sysfs_attributes(edac_dev);
 	if (err) {
-		debugf0("%s() failed to add sysfs attribs\n", __func__);
+		edac_dbg(0, "failed to add sysfs attribs\n");
 		goto err_out;
 	}
 
@@ -832,8 +827,7 @@ int edac_device_create_sysfs(struct edac_device_ctl_info *edac_dev)
 	err = sysfs_create_link(edac_kobj,
 				&edac_dev->dev->kobj, EDAC_DEVICE_SYMLINK);
 	if (err) {
-		debugf0("%s() sysfs_create_link() returned err= %d\n",
-			__func__, err);
+		edac_dbg(0, "sysfs_create_link() returned err= %d\n", err);
 		goto err_remove_main_attribs;
 	}
 
@@ -843,14 +837,13 @@ int edac_device_create_sysfs(struct edac_device_ctl_info *edac_dev)
 	 */
 	err = edac_device_create_instances(edac_dev);
 	if (err) {
-		debugf0("%s() edac_device_create_instances() "
-			"returned err= %d\n", __func__, err);
+		edac_dbg(0, "edac_device_create_instances() returned err= %d\n",
+			 err);
 		goto err_remove_link;
 	}
 
 
-	debugf4("%s() create-instances done, idx=%d\n",
-		__func__, edac_dev->dev_idx);
+	edac_dbg(4, "create-instances done, idx=%d\n", edac_dev->dev_idx);
 
 	return 0;
 
@@ -873,7 +866,7 @@ err_out:
  */
 void edac_device_remove_sysfs(struct edac_device_ctl_info *edac_dev)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* remove any main attributes for this device */
 	edac_device_remove_main_sysfs_attributes(edac_dev);
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index de5ba86e8b8..616d90bcb3a 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -27,70 +27,95 @@
 #include <linux/list.h>
 #include <linux/ctype.h>
 #include <linux/edac.h>
+#include <linux/bitops.h>
 #include <asm/uaccess.h>
 #include <asm/page.h>
 #include <asm/edac.h>
 #include "edac_core.h"
 #include "edac_module.h"
 
+#define CREATE_TRACE_POINTS
+#define TRACE_INCLUDE_PATH ../../include/ras
+#include <ras/ras_event.h>
+
 /* lock to memory controller's control array */
 static DEFINE_MUTEX(mem_ctls_mutex);
 static LIST_HEAD(mc_devices);
 
+unsigned edac_dimm_info_location(struct dimm_info *dimm, char *buf,
+			         unsigned len)
+{
+	struct mem_ctl_info *mci = dimm->mci;
+	int i, n, count = 0;
+	char *p = buf;
+
+	for (i = 0; i < mci->n_layers; i++) {
+		n = snprintf(p, len, "%s %d ",
+			      edac_layer_name[mci->layers[i].type],
+			      dimm->location[i]);
+		p += n;
+		len -= n;
+		count += n;
+		if (!len)
+			break;
+	}
+
+	return count;
+}
+
 #ifdef CONFIG_EDAC_DEBUG
 
 static void edac_mc_dump_channel(struct rank_info *chan)
 {
-	debugf4("\tchannel = %p\n", chan);
-	debugf4("\tchannel->chan_idx = %d\n", chan->chan_idx);
-	debugf4("\tchannel->csrow = %p\n\n", chan->csrow);
-	debugf4("\tchannel->dimm = %p\n", chan->dimm);
+	edac_dbg(4, "  channel->chan_idx = %d\n", chan->chan_idx);
+	edac_dbg(4, "    channel = %p\n", chan);
+	edac_dbg(4, "    channel->csrow = %p\n", chan->csrow);
+	edac_dbg(4, "    channel->dimm = %p\n", chan->dimm);
 }
 
-static void edac_mc_dump_dimm(struct dimm_info *dimm)
+static void edac_mc_dump_dimm(struct dimm_info *dimm, int number)
 {
-	int i;
-
-	debugf4("\tdimm = %p\n", dimm);
-	debugf4("\tdimm->label = '%s'\n", dimm->label);
-	debugf4("\tdimm->nr_pages = 0x%x\n", dimm->nr_pages);
-	debugf4("\tdimm location ");
-	for (i = 0; i < dimm->mci->n_layers; i++) {
-		printk(KERN_CONT "%d", dimm->location[i]);
-		if (i < dimm->mci->n_layers - 1)
-			printk(KERN_CONT ".");
-	}
-	printk(KERN_CONT "\n");
-	debugf4("\tdimm->grain = %d\n", dimm->grain);
-	debugf4("\tdimm->nr_pages = 0x%x\n", dimm->nr_pages);
+	char location[80];
+
+	edac_dimm_info_location(dimm, location, sizeof(location));
+
+	edac_dbg(4, "%s%i: %smapped as virtual row %d, chan %d\n",
+		 dimm->mci->mem_is_per_rank ? "rank" : "dimm",
+		 number, location, dimm->csrow, dimm->cschannel);
+	edac_dbg(4, "  dimm = %p\n", dimm);
+	edac_dbg(4, "  dimm->label = '%s'\n", dimm->label);
+	edac_dbg(4, "  dimm->nr_pages = 0x%x\n", dimm->nr_pages);
+	edac_dbg(4, "  dimm->grain = %d\n", dimm->grain);
+	edac_dbg(4, "  dimm->nr_pages = 0x%x\n", dimm->nr_pages);
 }
 
 static void edac_mc_dump_csrow(struct csrow_info *csrow)
 {
-	debugf4("\tcsrow = %p\n", csrow);
-	debugf4("\tcsrow->csrow_idx = %d\n", csrow->csrow_idx);
-	debugf4("\tcsrow->first_page = 0x%lx\n", csrow->first_page);
-	debugf4("\tcsrow->last_page = 0x%lx\n", csrow->last_page);
-	debugf4("\tcsrow->page_mask = 0x%lx\n", csrow->page_mask);
-	debugf4("\tcsrow->nr_channels = %d\n", csrow->nr_channels);
-	debugf4("\tcsrow->channels = %p\n", csrow->channels);
-	debugf4("\tcsrow->mci = %p\n\n", csrow->mci);
+	edac_dbg(4, "csrow->csrow_idx = %d\n", csrow->csrow_idx);
+	edac_dbg(4, "  csrow = %p\n", csrow);
+	edac_dbg(4, "  csrow->first_page = 0x%lx\n", csrow->first_page);
+	edac_dbg(4, "  csrow->last_page = 0x%lx\n", csrow->last_page);
+	edac_dbg(4, "  csrow->page_mask = 0x%lx\n", csrow->page_mask);
+	edac_dbg(4, "  csrow->nr_channels = %d\n", csrow->nr_channels);
+	edac_dbg(4, "  csrow->channels = %p\n", csrow->channels);
+	edac_dbg(4, "  csrow->mci = %p\n", csrow->mci);
 }
 
 static void edac_mc_dump_mci(struct mem_ctl_info *mci)
 {
-	debugf3("\tmci = %p\n", mci);
-	debugf3("\tmci->mtype_cap = %lx\n", mci->mtype_cap);
-	debugf3("\tmci->edac_ctl_cap = %lx\n", mci->edac_ctl_cap);
-	debugf3("\tmci->edac_cap = %lx\n", mci->edac_cap);
-	debugf4("\tmci->edac_check = %p\n", mci->edac_check);
-	debugf3("\tmci->nr_csrows = %d, csrows = %p\n",
-		mci->nr_csrows, mci->csrows);
-	debugf3("\tmci->nr_dimms = %d, dimms = %p\n",
-		mci->tot_dimms, mci->dimms);
-	debugf3("\tdev = %p\n", mci->dev);
-	debugf3("\tmod_name:ctl_name = %s:%s\n", mci->mod_name, mci->ctl_name);
-	debugf3("\tpvt_info = %p\n\n", mci->pvt_info);
+	edac_dbg(3, "\tmci = %p\n", mci);
+	edac_dbg(3, "\tmci->mtype_cap = %lx\n", mci->mtype_cap);
+	edac_dbg(3, "\tmci->edac_ctl_cap = %lx\n", mci->edac_ctl_cap);
+	edac_dbg(3, "\tmci->edac_cap = %lx\n", mci->edac_cap);
+	edac_dbg(4, "\tmci->edac_check = %p\n", mci->edac_check);
+	edac_dbg(3, "\tmci->nr_csrows = %d, csrows = %p\n",
+		 mci->nr_csrows, mci->csrows);
+	edac_dbg(3, "\tmci->nr_dimms = %d, dimms = %p\n",
+		 mci->tot_dimms, mci->dimms);
+	edac_dbg(3, "\tdev = %p\n", mci->pdev);
+	edac_dbg(3, "\tmod_name:ctl_name = %s:%s\n",
+		 mci->mod_name, mci->ctl_name);
+	edac_dbg(3, "\tpvt_info = %p\n\n", mci->pvt_info);
 }
 
 #endif				/* CONFIG_EDAC_DEBUG */
@@ -205,15 +230,15 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 {
 	struct mem_ctl_info *mci;
 	struct edac_mc_layer *layer;
-	struct csrow_info *csi, *csr;
-	struct rank_info *chi, *chp, *chan;
+	struct csrow_info *csr;
+	struct rank_info *chan;
 	struct dimm_info *dimm;
 	u32 *ce_per_layer[EDAC_MAX_LAYERS], *ue_per_layer[EDAC_MAX_LAYERS];
 	unsigned pos[EDAC_MAX_LAYERS];
 	unsigned size, tot_dimms = 1, count = 1;
 	unsigned tot_csrows = 1, tot_channels = 1, tot_errcount = 0;
 	void *pvt, *p, *ptr = NULL;
-	int i, j, err, row, chn, n, len;
+	int i, j, row, chn, n, len, off;
 	bool per_rank = false;
 
 	BUG_ON(n_layers > EDAC_MAX_LAYERS || n_layers == 0);
@@ -239,26 +264,24 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 	 */
 	mci = edac_align_ptr(&ptr, sizeof(*mci), 1);
 	layer = edac_align_ptr(&ptr, sizeof(*layer), n_layers);
-	csi = edac_align_ptr(&ptr, sizeof(*csi), tot_csrows);
-	chi = edac_align_ptr(&ptr, sizeof(*chi), tot_csrows * tot_channels);
-	dimm = edac_align_ptr(&ptr, sizeof(*dimm), tot_dimms);
 	for (i = 0; i < n_layers; i++) {
 		count *= layers[i].size;
-		debugf4("%s: errcount layer %d size %d\n", __func__, i, count);
+		edac_dbg(4, "errcount layer %d size %d\n", i, count);
 		ce_per_layer[i] = edac_align_ptr(&ptr, sizeof(u32), count);
 		ue_per_layer[i] = edac_align_ptr(&ptr, sizeof(u32), count);
 		tot_errcount += 2 * count;
 	}
 
-	debugf4("%s: allocating %d error counters\n", __func__, tot_errcount);
+	edac_dbg(4, "allocating %d error counters\n", tot_errcount);
 	pvt = edac_align_ptr(&ptr, sz_pvt, 1);
 	size = ((unsigned long)pvt) + sz_pvt;
 
-	debugf1("%s(): allocating %u bytes for mci data (%d %s, %d csrows/channels)\n",
-		__func__, size,
-		tot_dimms,
-		per_rank ? "ranks" : "dimms",
-		tot_csrows * tot_channels);
+	edac_dbg(1, "allocating %u bytes for mci data (%d %s, %d csrows/channels)\n",
+		 size,
+		 tot_dimms,
+		 per_rank ? "ranks" : "dimms",
+		 tot_csrows * tot_channels);
+
 	mci = kzalloc(size, GFP_KERNEL);
 	if (mci == NULL)
 		return NULL;
@@ -267,9 +290,6 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 	 * rather than an imaginary chunk of memory located at address 0.
 	 */
 	layer = (struct edac_mc_layer *)(((char *)mci) + ((unsigned long)layer));
-	csi = (struct csrow_info *)(((char *)mci) + ((unsigned long)csi));
-	chi = (struct rank_info *)(((char *)mci) + ((unsigned long)chi));
-	dimm = (struct dimm_info *)(((char *)mci) + ((unsigned long)dimm));
 	for (i = 0; i < n_layers; i++) {
 		mci->ce_per_layer[i] = (u32 *)((char *)mci + ((unsigned long)ce_per_layer[i]));
 		mci->ue_per_layer[i] = (u32 *)((char *)mci + ((unsigned long)ue_per_layer[i]));
@@ -278,8 +298,6 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 
 	/* setup index and various internal pointers */
 	mci->mc_idx = mc_num;
-	mci->csrows = csi;
-	mci->dimms  = dimm;
 	mci->tot_dimms = tot_dimms;
 	mci->pvt_info = pvt;
 	mci->n_layers = n_layers;
@@ -290,40 +308,57 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 	mci->mem_is_per_rank = per_rank;
 
 	/*
-	 * Fill the csrow struct
+	 * Alocate and fill the csrow/channels structs
 	 */
+	mci->csrows = kcalloc(sizeof(*mci->csrows), tot_csrows, GFP_KERNEL);
+	if (!mci->csrows)
+		goto error;
 	for (row = 0; row < tot_csrows; row++) {
-		csr = &csi[row];
+		csr = kzalloc(sizeof(**mci->csrows), GFP_KERNEL);
+		if (!csr)
+			goto error;
+		mci->csrows[row] = csr;
 		csr->csrow_idx = row;
 		csr->mci = mci;
 		csr->nr_channels = tot_channels;
-		chp = &chi[row * tot_channels];
-		csr->channels = chp;
+		csr->channels = kcalloc(sizeof(*csr->channels), tot_channels,
+					GFP_KERNEL);
+		if (!csr->channels)
+			goto error;
 
 		for (chn = 0; chn < tot_channels; chn++) {
-			chan = &chp[chn];
+			chan = kzalloc(sizeof(**csr->channels), GFP_KERNEL);
+			if (!chan)
+				goto error;
+			csr->channels[chn] = chan;
 			chan->chan_idx = chn;
 			chan->csrow = csr;
 		}
 	}
 
 	/*
-	 * Fill the dimm struct
+	 * Allocate and fill the dimm structs
 	 */
+	mci->dimms  = kcalloc(sizeof(*mci->dimms), tot_dimms, GFP_KERNEL);
+	if (!mci->dimms)
+		goto error;
+
 	memset(&pos, 0, sizeof(pos));
 	row = 0;
 	chn = 0;
-	debugf4("%s: initializing %d %s\n", __func__, tot_dimms,
-		per_rank ? "ranks" : "dimms");
 	for (i = 0; i < tot_dimms; i++) {
-		chan = &csi[row].channels[chn];
-		dimm = EDAC_DIMM_PTR(layer, mci->dimms, n_layers,
-			       pos[0], pos[1], pos[2]);
-		dimm->mci = mci;
+		chan = mci->csrows[row]->channels[chn];
+		off = EDAC_DIMM_OFF(layer, n_layers, pos[0], pos[1], pos[2]);
+		if (off < 0 || off >= tot_dimms) {
+			edac_mc_printk(mci, KERN_ERR, "EDAC core bug: EDAC_DIMM_OFF is trying to do an illegal data access\n");
+			goto error;
+		}
 
-		debugf2("%s: %d: %s%zd (%d:%d:%d): row %d, chan %d\n", __func__,
-			i, per_rank ? "rank" : "dimm", (dimm - mci->dimms),
-			pos[0], pos[1], pos[2], row, chn);
+		dimm = kzalloc(sizeof(**mci->dimms), GFP_KERNEL);
+		if (!dimm)
+			goto error;
+		mci->dimms[off] = dimm;
+		dimm->mci = mci;
 
 		/*
 		 * Copy DIMM location and initialize it.
@@ -367,16 +402,6 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 	}
 
 	mci->op_state = OP_ALLOC;
-	INIT_LIST_HEAD(&mci->grp_kobj_list);
-
-	/*
-	 * Initialize the 'root' kobj for the edac_mc controller
-	 */
-	err = edac_mc_register_sysfs_main_kobj(mci);
-	if (err) {
-		kfree(mci);
-		return NULL;
-	}
 
 	/* at this point, the root kobj is valid, and in order to
 	 * 'free' the object, then the function:
@@ -384,7 +409,30 @@ struct mem_ctl_info *edac_mc_alloc(unsigned mc_num,
 	 * which will perform kobj unregistration and the actual free
 	 * will occur during the kobject callback operation
 	 */
+
 	return mci;
+
+error:
+	if (mci->dimms) {
+		for (i = 0; i < tot_dimms; i++)
+			kfree(mci->dimms[i]);
+		kfree(mci->dimms);
+	}
+	if (mci->csrows) {
+		for (chn = 0; chn < tot_channels; chn++) {
+			csr = mci->csrows[chn];
+			if (csr) {
+				for (chn = 0; chn < tot_channels; chn++)
+					kfree(csr->channels[chn]);
+				kfree(csr);
+			}
+			kfree(mci->csrows[i]);
+		}
+		kfree(mci->csrows);
+	}
+	kfree(mci);
+
+	return NULL;
 }
 EXPORT_SYMBOL_GPL(edac_mc_alloc);
 
@@ -395,12 +443,10 @@ EXPORT_SYMBOL_GPL(edac_mc_alloc);
  */
 void edac_mc_free(struct mem_ctl_info *mci)
 {
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
-	edac_mc_unregister_sysfs_main_kobj(mci);
-
-	/* free the mci instance memory here */
-	kfree(mci);
+	/* the mci instance is freed here, when the sysfs object is dropped */
+	edac_unregister_sysfs(mci);
 }
 EXPORT_SYMBOL_GPL(edac_mc_free);
 
@@ -417,12 +463,12 @@ struct mem_ctl_info *find_mci_by_dev(struct device *dev)
 	struct mem_ctl_info *mci;
 	struct list_head *item;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	list_for_each(item, &mc_devices) {
 		mci = list_entry(item, struct mem_ctl_info, link);
 
-		if (mci->dev == dev)
+		if (mci->pdev == dev)
 			return mci;
 	}
 
@@ -485,7 +531,7 @@ static void edac_mc_workq_function(struct work_struct *work_req)
  */
 static void edac_mc_workq_setup(struct mem_ctl_info *mci, unsigned msec)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* if this instance is not in the POLL state, then simply return */
 	if (mci->op_state != OP_RUNNING_POLL)
@@ -512,8 +558,7 @@ static void edac_mc_workq_teardown(struct mem_ctl_info *mci)
 
 	status = cancel_delayed_work(&mci->work);
 	if (status == 0) {
-		debugf0("%s() not canceled, flush the queue\n",
-			__func__);
+		edac_dbg(0, "not canceled, flush the queue\n");
 
 		/* workq instance might be running, wait for it */
 		flush_workqueue(edac_workqueue);
@@ -574,7 +619,7 @@ static int add_mc_to_global_list(struct mem_ctl_info *mci)
 
 	insert_before = &mc_devices;
 
-	p = find_mci_by_dev(mci->dev);
+	p = find_mci_by_dev(mci->pdev);
 	if (unlikely(p != NULL))
 		goto fail0;
 
@@ -596,7 +641,7 @@ static int add_mc_to_global_list(struct mem_ctl_info *mci)
 
 fail0:
 	edac_printk(KERN_WARNING, EDAC_MC,
-		"%s (%s) %s %s already assigned %d\n", dev_name(p->dev),
+		"%s (%s) %s %s already assigned %d\n", dev_name(p->pdev),
 		edac_dev_name(mci), p->mod_name, p->ctl_name, p->mc_idx);
 	return 1;
 
@@ -660,7 +705,7 @@ EXPORT_SYMBOL(edac_mc_find);
 /* FIXME - should a warning be printed if no error detection? correction? */
 int edac_mc_add_mc(struct mem_ctl_info *mci)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 #ifdef CONFIG_EDAC_DEBUG
 	if (edac_debug_level >= 3)
@@ -670,15 +715,22 @@ int edac_mc_add_mc(struct mem_ctl_info *mci)
 		int i;
 
 		for (i = 0; i < mci->nr_csrows; i++) {
+			struct csrow_info *csrow = mci->csrows[i];
+			u32 nr_pages = 0;
 			int j;
 
-			edac_mc_dump_csrow(&mci->csrows[i]);
-			for (j = 0; j < mci->csrows[i].nr_channels; j++)
-				edac_mc_dump_channel(&mci->csrows[i].
-						channels[j]);
+			for (j = 0; j < csrow->nr_channels; j++)
+				nr_pages += csrow->channels[j]->dimm->nr_pages;
+			if (!nr_pages)
+				continue;
+			edac_mc_dump_csrow(csrow);
+			for (j = 0; j < csrow->nr_channels; j++)
+				if (csrow->channels[j]->dimm->nr_pages)
+					edac_mc_dump_channel(csrow->channels[j]);
 		}
 		for (i = 0; i < mci->tot_dimms; i++)
-			edac_mc_dump_dimm(&mci->dimms[i]);
+			if (mci->dimms[i]->nr_pages)
+				edac_mc_dump_dimm(mci->dimms[i], i);
 	}
 #endif
 	mutex_lock(&mem_ctls_mutex);
@@ -732,7 +784,7 @@ struct mem_ctl_info *edac_mc_del_mc(struct device *dev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	mutex_lock(&mem_ctls_mutex);
 
@@ -770,7 +822,7 @@ static void edac_mc_scrub_block(unsigned long page, unsigned long offset,
 	void *virt_addr;
 	unsigned long flags = 0;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	/* ECC error page was not in our memory. Ignore it. */
 	if (!pfn_valid(page))
@@ -797,26 +849,26 @@ static void edac_mc_scrub_block(unsigned long page, unsigned long offset,
 /* FIXME - should return -1 */
 int edac_mc_find_csrow_by_page(struct mem_ctl_info *mci, unsigned long page)
 {
-	struct csrow_info *csrows = mci->csrows;
+	struct csrow_info **csrows = mci->csrows;
 	int row, i, j, n;
 
-	debugf1("MC%d: %s(): 0x%lx\n", mci->mc_idx, __func__, page);
+	edac_dbg(1, "MC%d: 0x%lx\n", mci->mc_idx, page);
 	row = -1;
 
 	for (i = 0; i < mci->nr_csrows; i++) {
-		struct csrow_info *csrow = &csrows[i];
+		struct csrow_info *csrow = csrows[i];
 		n = 0;
 		for (j = 0; j < csrow->nr_channels; j++) {
-			struct dimm_info *dimm = csrow->channels[j].dimm;
+			struct dimm_info *dimm = csrow->channels[j]->dimm;
 			n += dimm->nr_pages;
 		}
 		if (n == 0)
 			continue;
 
-		debugf3("MC%d: %s(): first(0x%lx) page(0x%lx) last(0x%lx) "
-			"mask(0x%lx)\n", mci->mc_idx, __func__,
-			csrow->first_page, page, csrow->last_page,
-			csrow->page_mask);
+		edac_dbg(3, "MC%d: first(0x%lx) page(0x%lx) last(0x%lx) mask(0x%lx)\n",
+			 mci->mc_idx,
+			 csrow->first_page, page, csrow->last_page,
+			 csrow->page_mask);
 
 		if ((page >= csrow->first_page) &&
 		    (page <= csrow->last_page) &&
@@ -845,15 +897,16 @@ const char *edac_layer_name[] = {
 EXPORT_SYMBOL_GPL(edac_layer_name);
 
 static void edac_inc_ce_error(struct mem_ctl_info *mci,
-				    bool enable_per_layer_report,
-				    const int pos[EDAC_MAX_LAYERS])
+			      bool enable_per_layer_report,
+			      const int pos[EDAC_MAX_LAYERS],
+			      const u16 count)
 {
 	int i, index = 0;
 
-	mci->ce_mc++;
+	mci->ce_mc += count;
 
 	if (!enable_per_layer_report) {
-		mci->ce_noinfo_count++;
+		mci->ce_noinfo_count += count;
 		return;
 	}
 
@@ -861,7 +914,7 @@ static void edac_inc_ce_error(struct mem_ctl_info *mci,
 		if (pos[i] < 0)
 			break;
 		index += pos[i];
-		mci->ce_per_layer[i][index]++;
+		mci->ce_per_layer[i][index] += count;
 
 		if (i < mci->n_layers - 1)
 			index *= mci->layers[i + 1].size;
@@ -870,14 +923,15 @@ static void edac_inc_ce_error(struct mem_ctl_info *mci,
 
 static void edac_inc_ue_error(struct mem_ctl_info *mci,
 				    bool enable_per_layer_report,
-				    const int pos[EDAC_MAX_LAYERS])
+				    const int pos[EDAC_MAX_LAYERS],
+				    const u16 count)
 {
 	int i, index = 0;
 
-	mci->ue_mc++;
+	mci->ue_mc += count;
 
 	if (!enable_per_layer_report) {
-		mci->ce_noinfo_count++;
+		mci->ce_noinfo_count += count;
 		return;
 	}
 
@@ -885,7 +939,7 @@ static void edac_inc_ue_error(struct mem_ctl_info *mci,
 		if (pos[i] < 0)
 			break;
 		index += pos[i];
-		mci->ue_per_layer[i][index]++;
+		mci->ue_per_layer[i][index] += count;
 
 		if (i < mci->n_layers - 1)
 			index *= mci->layers[i + 1].size;
@@ -893,6 +947,7 @@ static void edac_inc_ue_error(struct mem_ctl_info *mci,
 }
 
 static void edac_ce_error(struct mem_ctl_info *mci,
+			  const u16 error_count,
 			  const int pos[EDAC_MAX_LAYERS],
 			  const char *msg,
 			  const char *location,
@@ -902,23 +957,25 @@ static void edac_ce_error(struct mem_ctl_info *mci,
 			  const bool enable_per_layer_report,
 			  const unsigned long page_frame_number,
 			  const unsigned long offset_in_page,
-			  u32 grain)
+			  long grain)
 {
 	unsigned long remapped_page;
 
 	if (edac_mc_get_log_ce()) {
 		if (other_detail && *other_detail)
 			edac_mc_printk(mci, KERN_WARNING,
-				       "CE %s on %s (%s%s - %s)\n",
+				       "%d CE %s on %s (%s %s - %s)\n",
+				       error_count,
 				       msg, label, location,
 				       detail, other_detail);
 		else
 			edac_mc_printk(mci, KERN_WARNING,
-				       "CE %s on %s (%s%s)\n",
+				       "%d CE %s on %s (%s %s)\n",
+				       error_count,
 				       msg, label, location,
 				       detail);
 	}
-	edac_inc_ce_error(mci, enable_per_layer_report, pos);
+	edac_inc_ce_error(mci, enable_per_layer_report, pos, error_count);
 
 	if (mci->scrub_mode & SCRUB_SW_SRC) {
 		/*
@@ -942,6 +999,7 @@ static void edac_ce_error(struct mem_ctl_info *mci,
 }
 
 static void edac_ue_error(struct mem_ctl_info *mci,
+			  const u16 error_count,
 			  const int pos[EDAC_MAX_LAYERS],
 			  const char *msg,
 			  const char *location,
@@ -953,12 +1011,14 @@ static void edac_ue_error(struct mem_ctl_info *mci,
 	if (edac_mc_get_log_ue()) {
 		if (other_detail && *other_detail)
 			edac_mc_printk(mci, KERN_WARNING,
-				       "UE %s on %s (%s%s - %s)\n",
+				       "%d UE %s on %s (%s %s - %s)\n",
+				       error_count,
 			               msg, label, location, detail,
 				       other_detail);
 		else
 			edac_mc_printk(mci, KERN_WARNING,
-				       "UE %s on %s (%s%s)\n",
+				       "%d UE %s on %s (%s %s)\n",
+				       error_count,
 			               msg, label, location, detail);
 	}
 
@@ -971,33 +1031,53 @@ static void edac_ue_error(struct mem_ctl_info *mci,
 			      msg, label, location, detail);
 	}
 
-	edac_inc_ue_error(mci, enable_per_layer_report, pos);
+	edac_inc_ue_error(mci, enable_per_layer_report, pos, error_count);
 }
 
 #define OTHER_LABEL " or "
+
+/**
+ * edac_mc_handle_error - reports a memory event to userspace
+ *
+ * @type:		severity of the error (CE/UE/Fatal)
+ * @mci:		a struct mem_ctl_info pointer
+ * @error_count:	Number of errors of the same type
+ * @page_frame_number:	mem page where the error occurred
+ * @offset_in_page:	offset of the error inside the page
+ * @syndrome:		ECC syndrome
+ * @top_layer:		Memory layer[0] position
+ * @mid_layer:		Memory layer[1] position
+ * @low_layer:		Memory layer[2] position
+ * @msg:		Message meaningful to the end users that
+ *			explains the event
+ * @other_detail:	Technical details about the event that
+ *			may help hardware manufacturers and
+ *			EDAC developers to analyse the event
+ */
 void edac_mc_handle_error(const enum hw_event_mc_err_type type,
 			  struct mem_ctl_info *mci,
+			  const u16 error_count,
 			  const unsigned long page_frame_number,
 			  const unsigned long offset_in_page,
 			  const unsigned long syndrome,
-			  const int layer0,
-			  const int layer1,
-			  const int layer2,
+			  const int top_layer,
+			  const int mid_layer,
+			  const int low_layer,
 			  const char *msg,
-			  const char *other_detail,
-			  const void *mcelog)
+			  const char *other_detail)
 {
 	/* FIXME: too much for stack: move it to some pre-alocated area */
 	char detail[80], location[80];
 	char label[(EDAC_MC_LABEL_LEN + 1 + sizeof(OTHER_LABEL)) * mci->tot_dimms];
 	char *p;
 	int row = -1, chan = -1;
-	int pos[EDAC_MAX_LAYERS] = { layer0, layer1, layer2 };
+	int pos[EDAC_MAX_LAYERS] = { top_layer, mid_layer, low_layer };
 	int i;
-	u32 grain;
+	long grain;
 	bool enable_per_layer_report = false;
+	u8 grain_bits;
 
-	debugf3("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(3, "MC%d\n", mci->mc_idx);
 
 	/*
 	 * Check if the event report is consistent and if the memory
@@ -1043,13 +1123,13 @@ void edac_mc_handle_error(const enum hw_event_mc_err_type type,
 	p = label;
 	*p = '\0';
 	for (i = 0; i < mci->tot_dimms; i++) {
-		struct dimm_info *dimm = &mci->dimms[i];
+		struct dimm_info *dimm = mci->dimms[i];
 
-		if (layer0 >= 0 && layer0 != dimm->location[0])
+		if (top_layer >= 0 && top_layer != dimm->location[0])
 			continue;
-		if (layer1 >= 0 && layer1 != dimm->location[1])
+		if (mid_layer >= 0 && mid_layer != dimm->location[1])
 			continue;
-		if (layer2 >= 0 && layer2 != dimm->location[2])
+		if (low_layer >= 0 && low_layer != dimm->location[2])
 			continue;
 
 		/* get the max grain, over the error match range */
@@ -1075,11 +1155,9 @@ void edac_mc_handle_error(const enum hw_event_mc_err_type type,
 			 * get csrow/channel of the DIMM, in order to allow
 			 * incrementing the compat API counters
 			 */
-			debugf4("%s: %s csrows map: (%d,%d)\n",
-				__func__,
-				mci->mem_is_per_rank ? "rank" : "dimm",
-				dimm->csrow, dimm->cschannel);
-
+			edac_dbg(4, "%s csrows map: (%d,%d)\n",
+				 mci->mem_is_per_rank ? "rank" : "dimm",
+				 dimm->csrow, dimm->cschannel);
 			if (row == -1)
 				row = dimm->csrow;
 			else if (row >= 0 && row != dimm->csrow)
@@ -1095,19 +1173,18 @@ void edac_mc_handle_error(const enum hw_event_mc_err_type type,
 	if (!enable_per_layer_report) {
 		strcpy(label, "any memory");
 	} else {
-		debugf4("%s: csrow/channel to increment: (%d,%d)\n",
-			__func__, row, chan);
+		edac_dbg(4, "csrow/channel to increment: (%d,%d)\n", row, chan);
 		if (p == label)
 			strcpy(label, "unknown memory");
 		if (type == HW_EVENT_ERR_CORRECTED) {
 			if (row >= 0) {
-				mci->csrows[row].ce_count++;
+				mci->csrows[row]->ce_count += error_count;
 				if (chan >= 0)
-					mci->csrows[row].channels[chan].ce_count++;
+					mci->csrows[row]->channels[chan]->ce_count += error_count;
 			}
 		} else
 			if (row >= 0)
-				mci->csrows[row].ue_count++;
+				mci->csrows[row]->ue_count += error_count;
 	}
 
 	/* Fill the RAM location data */
@@ -1120,23 +1197,33 @@ void edac_mc_handle_error(const enum hw_event_mc_err_type type,
 			     edac_layer_name[mci->layers[i].type],
 			     pos[i]);
 	}
+	if (p > location)
+		*(p - 1) = '\0';
+
+	/* Report the error via the trace interface */
+
+	grain_bits = fls_long(grain) + 1;
+	trace_mc_event(type, msg, label, error_count,
+		       mci->mc_idx, top_layer, mid_layer, low_layer,
+		       PAGES_TO_MiB(page_frame_number) | offset_in_page,
+		       grain_bits, syndrome, other_detail);
 
 	/* Memory type dependent details about the error */
 	if (type == HW_EVENT_ERR_CORRECTED) {
 		snprintf(detail, sizeof(detail),
-			"page:0x%lx offset:0x%lx grain:%d syndrome:0x%lx",
+			"page:0x%lx offset:0x%lx grain:%ld syndrome:0x%lx",
 			page_frame_number, offset_in_page,
 			grain, syndrome);
-		edac_ce_error(mci, pos, msg, location, label, detail,
-			      other_detail, enable_per_layer_report,
+		edac_ce_error(mci, error_count, pos, msg, location, label,
+			      detail, other_detail, enable_per_layer_report,
 			      page_frame_number, offset_in_page, grain);
 	} else {
 		snprintf(detail, sizeof(detail),
-			"page:0x%lx offset:0x%lx grain:%d",
+			"page:0x%lx offset:0x%lx grain:%ld",
 			page_frame_number, offset_in_page, grain);
 
-		edac_ue_error(mci, pos, msg, location, label, detail,
-			      other_detail, enable_per_layer_report);
+		edac_ue_error(mci, error_count, pos, msg, location, label,
+			      detail, other_detail, enable_per_layer_report);
 	}
 }
 EXPORT_SYMBOL_GPL(edac_mc_handle_error);
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index f6a29b0eedc..ed0bc07b850 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -7,17 +7,21 @@
  *
  * Written Doug Thompson <norsk5@xmission.com> www.softwarebitmaker.com
  *
+ * (c) 2012 - Mauro Carvalho Chehab <mchehab@redhat.com>
+ *	The entire API were re-written, and ported to use struct device
+ *
  */
 
 #include <linux/ctype.h>
 #include <linux/slab.h>
 #include <linux/edac.h>
 #include <linux/bug.h>
+#include <linux/pm_runtime.h>
+#include <linux/uaccess.h>
 
 #include "edac_core.h"
 #include "edac_module.h"
 
-
 /* MC EDAC Controls, setable by module parameter, and sysfs */
 static int edac_mc_log_ue = 1;
 static int edac_mc_log_ce = 1;
@@ -78,6 +82,8 @@ module_param_call(edac_mc_poll_msec, edac_set_poll_msec, param_get_int,
 		  &edac_mc_poll_msec, 0644);
 MODULE_PARM_DESC(edac_mc_poll_msec, "Polling period in milliseconds");
 
+static struct device *mci_pdev;
+
 /*
  * various constants for Memory Controllers
  */
@@ -125,317 +131,526 @@ static const char *edac_caps[] = {
 	[EDAC_S16ECD16ED] = "S16ECD16ED"
 };
 
-/* EDAC sysfs CSROW data structures and methods
+#ifdef CONFIG_EDAC_LEGACY_SYSFS
+/*
+ * EDAC sysfs CSROW data structures and methods
+ */
+
+#define to_csrow(k) container_of(k, struct csrow_info, dev)
+
+/*
+ * We need it to avoid namespace conflicts between the legacy API
+ * and the per-dimm/per-rank one
  */
+#define DEVICE_ATTR_LEGACY(_name, _mode, _show, _store) \
+	struct device_attribute dev_attr_legacy_##_name = __ATTR(_name, _mode, _show, _store)
+
+struct dev_ch_attribute {
+	struct device_attribute attr;
+	int channel;
+};
+
+#define DEVICE_CHANNEL(_name, _mode, _show, _store, _var) \
+	struct dev_ch_attribute dev_attr_legacy_##_name = \
+		{ __ATTR(_name, _mode, _show, _store), (_var) }
+
+#define to_channel(k) (container_of(k, struct dev_ch_attribute, attr)->channel)
 
 /* Set of more default csrow<id> attribute show/store functions */
-static ssize_t csrow_ue_count_show(struct csrow_info *csrow, char *data,
-				int private)
+static ssize_t csrow_ue_count_show(struct device *dev,
+				   struct device_attribute *mattr, char *data)
 {
+	struct csrow_info *csrow = to_csrow(dev);
+
 	return sprintf(data, "%u\n", csrow->ue_count);
 }
 
-static ssize_t csrow_ce_count_show(struct csrow_info *csrow, char *data,
-				int private)
+static ssize_t csrow_ce_count_show(struct device *dev,
+				   struct device_attribute *mattr, char *data)
 {
+	struct csrow_info *csrow = to_csrow(dev);
+
 	return sprintf(data, "%u\n", csrow->ce_count);
 }
 
-static ssize_t csrow_size_show(struct csrow_info *csrow, char *data,
-				int private)
+static ssize_t csrow_size_show(struct device *dev,
+			       struct device_attribute *mattr, char *data)
 {
+	struct csrow_info *csrow = to_csrow(dev);
 	int i;
 	u32 nr_pages = 0;
 
 	for (i = 0; i < csrow->nr_channels; i++)
-		nr_pages += csrow->channels[i].dimm->nr_pages;
-
+		nr_pages += csrow->channels[i]->dimm->nr_pages;
 	return sprintf(data, "%u\n", PAGES_TO_MiB(nr_pages));
 }
 
-static ssize_t csrow_mem_type_show(struct csrow_info *csrow, char *data,
-				int private)
+static ssize_t csrow_mem_type_show(struct device *dev,
+				   struct device_attribute *mattr, char *data)
 {
-	return sprintf(data, "%s\n", mem_types[csrow->channels[0].dimm->mtype]);
+	struct csrow_info *csrow = to_csrow(dev);
+
+	return sprintf(data, "%s\n", mem_types[csrow->channels[0]->dimm->mtype]);
 }
 
-static ssize_t csrow_dev_type_show(struct csrow_info *csrow, char *data,
-				int private)
+static ssize_t csrow_dev_type_show(struct device *dev,
+				   struct device_attribute *mattr, char *data)
 {
-	return sprintf(data, "%s\n", dev_types[csrow->channels[0].dimm->dtype]);
+	struct csrow_info *csrow = to_csrow(dev);
+
+	return sprintf(data, "%s\n", dev_types[csrow->channels[0]->dimm->dtype]);
 }
 
-static ssize_t csrow_edac_mode_show(struct csrow_info *csrow, char *data,
-				int private)
+static ssize_t csrow_edac_mode_show(struct device *dev,
+				    struct device_attribute *mattr,
+				    char *data)
 {
-	return sprintf(data, "%s\n", edac_caps[csrow->channels[0].dimm->edac_mode]);
+	struct csrow_info *csrow = to_csrow(dev);
+
+	return sprintf(data, "%s\n", edac_caps[csrow->channels[0]->dimm->edac_mode]);
 }
 
 /* show/store functions for DIMM Label attributes */
-static ssize_t channel_dimm_label_show(struct csrow_info *csrow,
-				char *data, int channel)
+static ssize_t channel_dimm_label_show(struct device *dev,
+				       struct device_attribute *mattr,
+				       char *data)
 {
+	struct csrow_info *csrow = to_csrow(dev);
+	unsigned chan = to_channel(mattr);
+	struct rank_info *rank = csrow->channels[chan];
+
 	/* if field has not been initialized, there is nothing to send */
-	if (!csrow->channels[channel].dimm->label[0])
+	if (!rank->dimm->label[0])
 		return 0;
 
 	return snprintf(data, EDAC_MC_LABEL_LEN, "%s\n",
-			csrow->channels[channel].dimm->label);
+			rank->dimm->label);
 }
 
-static ssize_t channel_dimm_label_store(struct csrow_info *csrow,
-					const char *data,
-					size_t count, int channel)
+static ssize_t channel_dimm_label_store(struct device *dev,
+					struct device_attribute *mattr,
+					const char *data, size_t count)
 {
+	struct csrow_info *csrow = to_csrow(dev);
+	unsigned chan = to_channel(mattr);
+	struct rank_info *rank = csrow->channels[chan];
+
 	ssize_t max_size = 0;
 
 	max_size = min((ssize_t) count, (ssize_t) EDAC_MC_LABEL_LEN - 1);
-	strncpy(csrow->channels[channel].dimm->label, data, max_size);
-	csrow->channels[channel].dimm->label[max_size] = '\0';
+	strncpy(rank->dimm->label, data, max_size);
+	rank->dimm->label[max_size] = '\0';
 
 	return max_size;
 }
 
 /* show function for dynamic chX_ce_count attribute */
-static ssize_t channel_ce_count_show(struct csrow_info *csrow,
-				char *data, int channel)
+static ssize_t channel_ce_count_show(struct device *dev,
+				     struct device_attribute *mattr, char *data)
 {
-	return sprintf(data, "%u\n", csrow->channels[channel].ce_count);
+	struct csrow_info *csrow = to_csrow(dev);
+	unsigned chan = to_channel(mattr);
+	struct rank_info *rank = csrow->channels[chan];
+
+	return sprintf(data, "%u\n", rank->ce_count);
 }
 
-/* csrow specific attribute structure */
-struct csrowdev_attribute {
-	struct attribute attr;
-	 ssize_t(*show) (struct csrow_info *, char *, int);
-	 ssize_t(*store) (struct csrow_info *, const char *, size_t, int);
-	int private;
-};
+/* cwrow<id>/attribute files */
+DEVICE_ATTR_LEGACY(size_mb, S_IRUGO, csrow_size_show, NULL);
+DEVICE_ATTR_LEGACY(dev_type, S_IRUGO, csrow_dev_type_show, NULL);
+DEVICE_ATTR_LEGACY(mem_type, S_IRUGO, csrow_mem_type_show, NULL);
+DEVICE_ATTR_LEGACY(edac_mode, S_IRUGO, csrow_edac_mode_show, NULL);
+DEVICE_ATTR_LEGACY(ue_count, S_IRUGO, csrow_ue_count_show, NULL);
+DEVICE_ATTR_LEGACY(ce_count, S_IRUGO, csrow_ce_count_show, NULL);
 
-#define to_csrow(k) container_of(k, struct csrow_info, kobj)
-#define to_csrowdev_attr(a) container_of(a, struct csrowdev_attribute, attr)
+/* default attributes of the CSROW<id> object */
+static struct attribute *csrow_attrs[] = {
+	&dev_attr_legacy_dev_type.attr,
+	&dev_attr_legacy_mem_type.attr,
+	&dev_attr_legacy_edac_mode.attr,
+	&dev_attr_legacy_size_mb.attr,
+	&dev_attr_legacy_ue_count.attr,
+	&dev_attr_legacy_ce_count.attr,
+	NULL,
+};
 
-/* Set of show/store higher level functions for default csrow attributes */
-static ssize_t csrowdev_show(struct kobject *kobj,
-			struct attribute *attr, char *buffer)
-{
-	struct csrow_info *csrow = to_csrow(kobj);
-	struct csrowdev_attribute *csrowdev_attr = to_csrowdev_attr(attr);
+static struct attribute_group csrow_attr_grp = {
+	.attrs	= csrow_attrs,
+};
 
-	if (csrowdev_attr->show)
-		return csrowdev_attr->show(csrow,
-					buffer, csrowdev_attr->private);
-	return -EIO;
-}
+static const struct attribute_group *csrow_attr_groups[] = {
+	&csrow_attr_grp,
+	NULL
+};
 
-static ssize_t csrowdev_store(struct kobject *kobj, struct attribute *attr,
-			const char *buffer, size_t count)
+static void csrow_attr_release(struct device *dev)
 {
-	struct csrow_info *csrow = to_csrow(kobj);
-	struct csrowdev_attribute *csrowdev_attr = to_csrowdev_attr(attr);
-
-	if (csrowdev_attr->store)
-		return csrowdev_attr->store(csrow,
-					buffer,
-					count, csrowdev_attr->private);
-	return -EIO;
-}
+	struct csrow_info *csrow = container_of(dev, struct csrow_info, dev);
 
-static const struct sysfs_ops csrowfs_ops = {
-	.show = csrowdev_show,
-	.store = csrowdev_store
-};
+	edac_dbg(1, "Releasing csrow device %s\n", dev_name(dev));
+	kfree(csrow);
+}
 
-#define CSROWDEV_ATTR(_name,_mode,_show,_store,_private)	\
-static struct csrowdev_attribute attr_##_name = {			\
-	.attr = {.name = __stringify(_name), .mode = _mode },	\
-	.show   = _show,					\
-	.store  = _store,					\
-	.private = _private,					\
+static struct device_type csrow_attr_type = {
+	.groups		= csrow_attr_groups,
+	.release	= csrow_attr_release,
 };
 
-/* default cwrow<id>/attribute files */
-CSROWDEV_ATTR(size_mb, S_IRUGO, csrow_size_show, NULL, 0);
-CSROWDEV_ATTR(dev_type, S_IRUGO, csrow_dev_type_show, NULL, 0);
-CSROWDEV_ATTR(mem_type, S_IRUGO, csrow_mem_type_show, NULL, 0);
-CSROWDEV_ATTR(edac_mode, S_IRUGO, csrow_edac_mode_show, NULL, 0);
-CSROWDEV_ATTR(ue_count, S_IRUGO, csrow_ue_count_show, NULL, 0);
-CSROWDEV_ATTR(ce_count, S_IRUGO, csrow_ce_count_show, NULL, 0);
+/*
+ * possible dynamic channel DIMM Label attribute files
+ *
+ */
 
-/* default attributes of the CSROW<id> object */
-static struct csrowdev_attribute *default_csrow_attr[] = {
-	&attr_dev_type,
-	&attr_mem_type,
-	&attr_edac_mode,
-	&attr_size_mb,
-	&attr_ue_count,
-	&attr_ce_count,
-	NULL,
-};
+#define EDAC_NR_CHANNELS	6
 
-/* possible dynamic channel DIMM Label attribute files */
-CSROWDEV_ATTR(ch0_dimm_label, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch0_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 0);
-CSROWDEV_ATTR(ch1_dimm_label, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch1_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 1);
-CSROWDEV_ATTR(ch2_dimm_label, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch2_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 2);
-CSROWDEV_ATTR(ch3_dimm_label, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch3_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 3);
-CSROWDEV_ATTR(ch4_dimm_label, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch4_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 4);
-CSROWDEV_ATTR(ch5_dimm_label, S_IRUGO | S_IWUSR,
+DEVICE_CHANNEL(ch5_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 5);
 
 /* Total possible dynamic DIMM Label attribute file table */
-static struct csrowdev_attribute *dynamic_csrow_dimm_attr[] = {
-	&attr_ch0_dimm_label,
-	&attr_ch1_dimm_label,
-	&attr_ch2_dimm_label,
-	&attr_ch3_dimm_label,
-	&attr_ch4_dimm_label,
-	&attr_ch5_dimm_label
+static struct device_attribute *dynamic_csrow_dimm_attr[] = {
+	&dev_attr_legacy_ch0_dimm_label.attr,
+	&dev_attr_legacy_ch1_dimm_label.attr,
+	&dev_attr_legacy_ch2_dimm_label.attr,
+	&dev_attr_legacy_ch3_dimm_label.attr,
+	&dev_attr_legacy_ch4_dimm_label.attr,
+	&dev_attr_legacy_ch5_dimm_label.attr
 };
 
 /* possible dynamic channel ce_count attribute files */
-CSROWDEV_ATTR(ch0_ce_count, S_IRUGO | S_IWUSR, channel_ce_count_show, NULL, 0);
-CSROWDEV_ATTR(ch1_ce_count, S_IRUGO | S_IWUSR, channel_ce_count_show, NULL, 1);
-CSROWDEV_ATTR(ch2_ce_count, S_IRUGO | S_IWUSR, channel_ce_count_show, NULL, 2);
-CSROWDEV_ATTR(ch3_ce_count, S_IRUGO | S_IWUSR, channel_ce_count_show, NULL, 3);
-CSROWDEV_ATTR(ch4_ce_count, S_IRUGO | S_IWUSR, channel_ce_count_show, NULL, 4);
-CSROWDEV_ATTR(ch5_ce_count, S_IRUGO | S_IWUSR, channel_ce_count_show, NULL, 5);
+DEVICE_CHANNEL(ch0_ce_count, S_IRUGO | S_IWUSR,
+		   channel_ce_count_show, NULL, 0);
+DEVICE_CHANNEL(ch1_ce_count, S_IRUGO | S_IWUSR,
+		   channel_ce_count_show, NULL, 1);
+DEVICE_CHANNEL(ch2_ce_count, S_IRUGO | S_IWUSR,
+		   channel_ce_count_show, NULL, 2);
+DEVICE_CHANNEL(ch3_ce_count, S_IRUGO | S_IWUSR,
+		   channel_ce_count_show, NULL, 3);
+DEVICE_CHANNEL(ch4_ce_count, S_IRUGO | S_IWUSR,
+		   channel_ce_count_show, NULL, 4);
+DEVICE_CHANNEL(ch5_ce_count, S_IRUGO | S_IWUSR,
+		   channel_ce_count_show, NULL, 5);
 
 /* Total possible dynamic ce_count attribute file table */
-static struct csrowdev_attribute *dynamic_csrow_ce_count_attr[] = {
-	&attr_ch0_ce_count,
-	&attr_ch1_ce_count,
-	&attr_ch2_ce_count,
-	&attr_ch3_ce_count,
-	&attr_ch4_ce_count,
-	&attr_ch5_ce_count
+static struct device_attribute *dynamic_csrow_ce_count_attr[] = {
+	&dev_attr_legacy_ch0_ce_count.attr,
+	&dev_attr_legacy_ch1_ce_count.attr,
+	&dev_attr_legacy_ch2_ce_count.attr,
+	&dev_attr_legacy_ch3_ce_count.attr,
+	&dev_attr_legacy_ch4_ce_count.attr,
+	&dev_attr_legacy_ch5_ce_count.attr
 };
 
-#define EDAC_NR_CHANNELS	6
+static inline int nr_pages_per_csrow(struct csrow_info *csrow)
+{
+	int chan, nr_pages = 0;
+
+	for (chan = 0; chan < csrow->nr_channels; chan++)
+		nr_pages += csrow->channels[chan]->dimm->nr_pages;
+
+	return nr_pages;
+}
 
-/* Create dynamic CHANNEL files, indexed by 'chan',  under specifed CSROW */
-static int edac_create_channel_files(struct kobject *kobj, int chan)
+/* Create a CSROW object under specifed edac_mc_device */
+static int edac_create_csrow_object(struct mem_ctl_info *mci,
+				    struct csrow_info *csrow, int index)
 {
-	int err = -ENODEV;
+	int err, chan;
+
+	if (csrow->nr_channels >= EDAC_NR_CHANNELS)
+		return -ENODEV;
+
+	csrow->dev.type = &csrow_attr_type;
+	csrow->dev.bus = &mci->bus;
+	device_initialize(&csrow->dev);
+	csrow->dev.parent = &mci->dev;
+	dev_set_name(&csrow->dev, "csrow%d", index);
+	dev_set_drvdata(&csrow->dev, csrow);
 
-	if (chan >= EDAC_NR_CHANNELS)
+	edac_dbg(0, "creating (virtual) csrow node %s\n",
+		 dev_name(&csrow->dev));
+
+	err = device_add(&csrow->dev);
+	if (err < 0)
 		return err;
 
-	/* create the DIMM label attribute file */
-	err = sysfs_create_file(kobj,
-				(struct attribute *)
-				dynamic_csrow_dimm_attr[chan]);
-
-	if (!err) {
-		/* create the CE Count attribute file */
-		err = sysfs_create_file(kobj,
-					(struct attribute *)
-					dynamic_csrow_ce_count_attr[chan]);
-	} else {
-		debugf1("%s()  dimm labels and ce_count files created",
-			__func__);
+	for (chan = 0; chan < csrow->nr_channels; chan++) {
+		/* Only expose populated DIMMs */
+		if (!csrow->channels[chan]->dimm->nr_pages)
+			continue;
+		err = device_create_file(&csrow->dev,
+					 dynamic_csrow_dimm_attr[chan]);
+		if (err < 0)
+			goto error;
+		err = device_create_file(&csrow->dev,
+					 dynamic_csrow_ce_count_attr[chan]);
+		if (err < 0) {
+			device_remove_file(&csrow->dev,
+					   dynamic_csrow_dimm_attr[chan]);
+			goto error;
+		}
+	}
+
+	return 0;
+
+error:
+	for (--chan; chan >= 0; chan--) {
+		device_remove_file(&csrow->dev,
+					dynamic_csrow_dimm_attr[chan]);
+		device_remove_file(&csrow->dev,
+					   dynamic_csrow_ce_count_attr[chan]);
 	}
+	put_device(&csrow->dev);
 
 	return err;
 }
 
-/* No memory to release for this kobj */
-static void edac_csrow_instance_release(struct kobject *kobj)
+/* Create a CSROW object under specifed edac_mc_device */
+static int edac_create_csrow_objects(struct mem_ctl_info *mci)
 {
-	struct mem_ctl_info *mci;
-	struct csrow_info *cs;
+	int err, i, chan;
+	struct csrow_info *csrow;
+
+	for (i = 0; i < mci->nr_csrows; i++) {
+		csrow = mci->csrows[i];
+		if (!nr_pages_per_csrow(csrow))
+			continue;
+		err = edac_create_csrow_object(mci, mci->csrows[i], i);
+		if (err < 0)
+			goto error;
+	}
+	return 0;
 
-	debugf1("%s()\n", __func__);
+error:
+	for (--i; i >= 0; i--) {
+		csrow = mci->csrows[i];
+		if (!nr_pages_per_csrow(csrow))
+			continue;
+		for (chan = csrow->nr_channels - 1; chan >= 0; chan--) {
+			if (!csrow->channels[chan]->dimm->nr_pages)
+				continue;
+			device_remove_file(&csrow->dev,
+						dynamic_csrow_dimm_attr[chan]);
+			device_remove_file(&csrow->dev,
+						dynamic_csrow_ce_count_attr[chan]);
+		}
+		put_device(&mci->csrows[i]->dev);
+	}
 
-	cs = container_of(kobj, struct csrow_info, kobj);
-	mci = cs->mci;
+	return err;
+}
 
-	kobject_put(&mci->edac_mci_kobj);
+static void edac_delete_csrow_objects(struct mem_ctl_info *mci)
+{
+	int i, chan;
+	struct csrow_info *csrow;
+
+	for (i = mci->nr_csrows - 1; i >= 0; i--) {
+		csrow = mci->csrows[i];
+		if (!nr_pages_per_csrow(csrow))
+			continue;
+		for (chan = csrow->nr_channels - 1; chan >= 0; chan--) {
+			if (!csrow->channels[chan]->dimm->nr_pages)
+				continue;
+			edac_dbg(1, "Removing csrow %d channel %d sysfs nodes\n",
+				 i, chan);
+			device_remove_file(&csrow->dev,
+						dynamic_csrow_dimm_attr[chan]);
+			device_remove_file(&csrow->dev,
+						dynamic_csrow_ce_count_attr[chan]);
+		}
+		put_device(&mci->csrows[i]->dev);
+		device_del(&mci->csrows[i]->dev);
+	}
 }
+#endif
 
-/* the kobj_type instance for a CSROW */
-static struct kobj_type ktype_csrow = {
-	.release = edac_csrow_instance_release,
-	.sysfs_ops = &csrowfs_ops,
-	.default_attrs = (struct attribute **)default_csrow_attr,
+/*
+ * Per-dimm (or per-rank) devices
+ */
+
+#define to_dimm(k) container_of(k, struct dimm_info, dev)
+
+/* show/store functions for DIMM Label attributes */
+static ssize_t dimmdev_location_show(struct device *dev,
+				     struct device_attribute *mattr, char *data)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	return edac_dimm_info_location(dimm, data, PAGE_SIZE);
+}
+
+static ssize_t dimmdev_label_show(struct device *dev,
+				  struct device_attribute *mattr, char *data)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	/* if field has not been initialized, there is nothing to send */
+	if (!dimm->label[0])
+		return 0;
+
+	return snprintf(data, EDAC_MC_LABEL_LEN, "%s\n", dimm->label);
+}
+
+static ssize_t dimmdev_label_store(struct device *dev,
+				   struct device_attribute *mattr,
+				   const char *data,
+				   size_t count)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	ssize_t max_size = 0;
+
+	max_size = min((ssize_t) count, (ssize_t) EDAC_MC_LABEL_LEN - 1);
+	strncpy(dimm->label, data, max_size);
+	dimm->label[max_size] = '\0';
+
+	return max_size;
+}
+
+static ssize_t dimmdev_size_show(struct device *dev,
+				 struct device_attribute *mattr, char *data)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	return sprintf(data, "%u\n", PAGES_TO_MiB(dimm->nr_pages));
+}
+
+static ssize_t dimmdev_mem_type_show(struct device *dev,
+				     struct device_attribute *mattr, char *data)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	return sprintf(data, "%s\n", mem_types[dimm->mtype]);
+}
+
+static ssize_t dimmdev_dev_type_show(struct device *dev,
+				     struct device_attribute *mattr, char *data)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	return sprintf(data, "%s\n", dev_types[dimm->dtype]);
+}
+
+static ssize_t dimmdev_edac_mode_show(struct device *dev,
+				      struct device_attribute *mattr,
+				      char *data)
+{
+	struct dimm_info *dimm = to_dimm(dev);
+
+	return sprintf(data, "%s\n", edac_caps[dimm->edac_mode]);
+}
+
+/* dimm/rank attribute files */
+static DEVICE_ATTR(dimm_label, S_IRUGO | S_IWUSR,
+		   dimmdev_label_show, dimmdev_label_store);
+static DEVICE_ATTR(dimm_location, S_IRUGO, dimmdev_location_show, NULL);
+static DEVICE_ATTR(size, S_IRUGO, dimmdev_size_show, NULL);
+static DEVICE_ATTR(dimm_mem_type, S_IRUGO, dimmdev_mem_type_show, NULL);
+static DEVICE_ATTR(dimm_dev_type, S_IRUGO, dimmdev_dev_type_show, NULL);
+static DEVICE_ATTR(dimm_edac_mode, S_IRUGO, dimmdev_edac_mode_show, NULL);
+
+/* attributes of the dimm<id>/rank<id> object */
+static struct attribute *dimm_attrs[] = {
+	&dev_attr_dimm_label.attr,
+	&dev_attr_dimm_location.attr,
+	&dev_attr_size.attr,
+	&dev_attr_dimm_mem_type.attr,
+	&dev_attr_dimm_dev_type.attr,
+	&dev_attr_dimm_edac_mode.attr,
+	NULL,
 };
 
-/* Create a CSROW object under specifed edac_mc_device */
-static int edac_create_csrow_object(struct mem_ctl_info *mci,
-					struct csrow_info *csrow, int index)
+static struct attribute_group dimm_attr_grp = {
+	.attrs	= dimm_attrs,
+};
+
+static const struct attribute_group *dimm_attr_groups[] = {
+	&dimm_attr_grp,
+	NULL
+};
+
+static void dimm_attr_release(struct device *dev)
 {
-	struct kobject *kobj_mci = &mci->edac_mci_kobj;
-	struct kobject *kobj;
-	int chan;
-	int err;
+	struct dimm_info *dimm = container_of(dev, struct dimm_info, dev);
 
-	/* generate ..../edac/mc/mc<id>/csrow<index>   */
-	memset(&csrow->kobj, 0, sizeof(csrow->kobj));
-	csrow->mci = mci;	/* include container up link */
+	edac_dbg(1, "Releasing dimm device %s\n", dev_name(dev));
+	kfree(dimm);
+}
 
-	/* bump the mci instance's kobject's ref count */
-	kobj = kobject_get(&mci->edac_mci_kobj);
-	if (!kobj) {
-		err = -ENODEV;
-		goto err_out;
-	}
+static struct device_type dimm_attr_type = {
+	.groups		= dimm_attr_groups,
+	.release	= dimm_attr_release,
+};
+
+/* Create a DIMM object under specifed memory controller device */
+static int edac_create_dimm_object(struct mem_ctl_info *mci,
+				   struct dimm_info *dimm,
+				   int index)
+{
+	int err;
+	dimm->mci = mci;
 
-	/* Instanstiate the csrow object */
-	err = kobject_init_and_add(&csrow->kobj, &ktype_csrow, kobj_mci,
-				   "csrow%d", index);
-	if (err)
-		goto err_release_top_kobj;
+	dimm->dev.type = &dimm_attr_type;
+	dimm->dev.bus = &mci->bus;
+	device_initialize(&dimm->dev);
 
-	/* At this point, to release a csrow kobj, one must
-	 * call the kobject_put and allow that tear down
-	 * to work the releasing
-	 */
+	dimm->dev.parent = &mci->dev;
+	if (mci->mem_is_per_rank)
+		dev_set_name(&dimm->dev, "rank%d", index);
+	else
+		dev_set_name(&dimm->dev, "dimm%d", index);
+	dev_set_drvdata(&dimm->dev, dimm);
+	pm_runtime_forbid(&mci->dev);
 
-	/* Create the dyanmic attribute files on this csrow,
-	 * namely, the DIMM labels and the channel ce_count
-	 */
-	for (chan = 0; chan < csrow->nr_channels; chan++) {
-		err = edac_create_channel_files(&csrow->kobj, chan);
-		if (err) {
-			/* special case the unregister here */
-			kobject_put(&csrow->kobj);
-			goto err_out;
-		}
-	}
-	kobject_uevent(&csrow->kobj, KOBJ_ADD);
-	return 0;
+	err =  device_add(&dimm->dev);
 
-	/* error unwind stack */
-err_release_top_kobj:
-	kobject_put(&mci->edac_mci_kobj);
+	edac_dbg(0, "creating rank/dimm device %s\n", dev_name(&dimm->dev));
 
-err_out:
 	return err;
 }
 
-/* default sysfs methods and data structures for the main MCI kobject */
+/*
+ * Memory controller device
+ */
+
+#define to_mci(k) container_of(k, struct mem_ctl_info, dev)
 
-static ssize_t mci_reset_counters_store(struct mem_ctl_info *mci,
+static ssize_t mci_reset_counters_store(struct device *dev,
+					struct device_attribute *mattr,
 					const char *data, size_t count)
 {
-	int row, chan;
-
-	mci->ue_noinfo_count = 0;
-	mci->ce_noinfo_count = 0;
+	struct mem_ctl_info *mci = to_mci(dev);
+	int cnt, row, chan, i;
 	mci->ue_mc = 0;
 	mci->ce_mc = 0;
+	mci->ue_noinfo_count = 0;
+	mci->ce_noinfo_count = 0;
 
 	for (row = 0; row < mci->nr_csrows; row++) {
-		struct csrow_info *ri = &mci->csrows[row];
+		struct csrow_info *ri = mci->csrows[row];
 
 		ri->ue_count = 0;
 		ri->ce_count = 0;
 
 		for (chan = 0; chan < ri->nr_channels; chan++)
-			ri->channels[chan].ce_count = 0;
+			ri->channels[chan]->ce_count = 0;
+	}
+
+	cnt = 1;
+	for (i = 0; i < mci->n_layers; i++) {
+		cnt *= mci->layers[i].size;
+		memset(mci->ce_per_layer[i], 0, cnt * sizeof(u32));
+		memset(mci->ue_per_layer[i], 0, cnt * sizeof(u32));
 	}
 
 	mci->start_time = jiffies;
@@ -451,9 +666,11 @@ static ssize_t mci_reset_counters_store(struct mem_ctl_info *mci,
  * Negative value still means that an error has occurred while setting
  * the scrub rate.
  */
-static ssize_t mci_sdram_scrub_rate_store(struct mem_ctl_info *mci,
+static ssize_t mci_sdram_scrub_rate_store(struct device *dev,
+					  struct device_attribute *mattr,
 					  const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	unsigned long bandwidth = 0;
 	int new_bw = 0;
 
@@ -476,8 +693,11 @@ static ssize_t mci_sdram_scrub_rate_store(struct mem_ctl_info *mci,
 /*
  * ->get_sdram_scrub_rate() return value semantics same as above.
  */
-static ssize_t mci_sdram_scrub_rate_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_sdram_scrub_rate_show(struct device *dev,
+					 struct device_attribute *mattr,
+					 char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	int bandwidth = 0;
 
 	if (!mci->get_sdram_scrub_rate)
@@ -493,45 +713,72 @@ static ssize_t mci_sdram_scrub_rate_show(struct mem_ctl_info *mci, char *data)
 }
 
 /* default attribute files for the MCI object */
-static ssize_t mci_ue_count_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_ue_count_show(struct device *dev,
+				 struct device_attribute *mattr,
+				 char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	return sprintf(data, "%d\n", mci->ue_mc);
 }
 
-static ssize_t mci_ce_count_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_ce_count_show(struct device *dev,
+				 struct device_attribute *mattr,
+				 char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	return sprintf(data, "%d\n", mci->ce_mc);
 }
 
-static ssize_t mci_ce_noinfo_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_ce_noinfo_show(struct device *dev,
+				  struct device_attribute *mattr,
+				  char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	return sprintf(data, "%d\n", mci->ce_noinfo_count);
 }
 
-static ssize_t mci_ue_noinfo_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_ue_noinfo_show(struct device *dev,
+				  struct device_attribute *mattr,
+				  char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	return sprintf(data, "%d\n", mci->ue_noinfo_count);
 }
 
-static ssize_t mci_seconds_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_seconds_show(struct device *dev,
+				struct device_attribute *mattr,
+				char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	return sprintf(data, "%ld\n", (jiffies - mci->start_time) / HZ);
 }
 
-static ssize_t mci_ctl_name_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_ctl_name_show(struct device *dev,
+				 struct device_attribute *mattr,
+				 char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
+
 	return sprintf(data, "%s\n", mci->ctl_name);
 }
 
-static ssize_t mci_size_mb_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mci_size_mb_show(struct device *dev,
+				struct device_attribute *mattr,
+				char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	int total_pages = 0, csrow_idx, j;
 
 	for (csrow_idx = 0; csrow_idx < mci->nr_csrows; csrow_idx++) {
-		struct csrow_info *csrow = &mci->csrows[csrow_idx];
+		struct csrow_info *csrow = mci->csrows[csrow_idx];
 
 		for (j = 0; j < csrow->nr_channels; j++) {
-			struct dimm_info *dimm = csrow->channels[j].dimm;
+			struct dimm_info *dimm = csrow->channels[j]->dimm;
 
 			total_pages += dimm->nr_pages;
 		}
@@ -540,361 +787,187 @@ static ssize_t mci_size_mb_show(struct mem_ctl_info *mci, char *data)
 	return sprintf(data, "%u\n", PAGES_TO_MiB(total_pages));
 }
 
-#define to_mci(k) container_of(k, struct mem_ctl_info, edac_mci_kobj)
-#define to_mcidev_attr(a) container_of(a,struct mcidev_sysfs_attribute,attr)
-
-/* MCI show/store functions for top most object */
-static ssize_t mcidev_show(struct kobject *kobj, struct attribute *attr,
-			char *buffer)
+static ssize_t mci_max_location_show(struct device *dev,
+				     struct device_attribute *mattr,
+				     char *data)
 {
-	struct mem_ctl_info *mem_ctl_info = to_mci(kobj);
-	struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
-
-	debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
+	struct mem_ctl_info *mci = to_mci(dev);
+	int i;
+	char *p = data;
 
-	if (mcidev_attr->show)
-		return mcidev_attr->show(mem_ctl_info, buffer);
+	for (i = 0; i < mci->n_layers; i++) {
+		p += sprintf(p, "%s %d ",
+			     edac_layer_name[mci->layers[i].type],
+			     mci->layers[i].size - 1);
+	}
 
-	return -EIO;
+	return p - data;
 }
 
-static ssize_t mcidev_store(struct kobject *kobj, struct attribute *attr,
-			const char *buffer, size_t count)
+#ifdef CONFIG_EDAC_DEBUG
+static ssize_t edac_fake_inject_write(struct file *file,
+				      const char __user *data,
+				      size_t count, loff_t *ppos)
 {
-	struct mem_ctl_info *mem_ctl_info = to_mci(kobj);
-	struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
-
-	debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
-
-	if (mcidev_attr->store)
-		return mcidev_attr->store(mem_ctl_info, buffer, count);
+	struct device *dev = file->private_data;
+	struct mem_ctl_info *mci = to_mci(dev);
+	static enum hw_event_mc_err_type type;
+	u16 errcount = mci->fake_inject_count;
+
+	if (!errcount)
+		errcount = 1;
+
+	type = mci->fake_inject_ue ? HW_EVENT_ERR_UNCORRECTED
+				   : HW_EVENT_ERR_CORRECTED;
+
+	printk(KERN_DEBUG
+	       "Generating %d %s fake error%s to %d.%d.%d to test core handling. NOTE: this won't test the driver-specific decoding logic.\n",
+		errcount,
+		(type == HW_EVENT_ERR_UNCORRECTED) ? "UE" : "CE",
+		errcount > 1 ? "s" : "",
+		mci->fake_inject_layer[0],
+		mci->fake_inject_layer[1],
+		mci->fake_inject_layer[2]
+	       );
+	edac_mc_handle_error(type, mci, errcount, 0, 0, 0,
+			     mci->fake_inject_layer[0],
+			     mci->fake_inject_layer[1],
+			     mci->fake_inject_layer[2],
+			     "FAKE ERROR", "for EDAC testing only");
 
-	return -EIO;
+	return count;
 }
 
-/* Intermediate show/store table */
-static const struct sysfs_ops mci_ops = {
-	.show = mcidev_show,
-	.store = mcidev_store
-};
+static int debugfs_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
 
-#define MCIDEV_ATTR(_name,_mode,_show,_store)			\
-static struct mcidev_sysfs_attribute mci_attr_##_name = {			\
-	.attr = {.name = __stringify(_name), .mode = _mode },	\
-	.show   = _show,					\
-	.store  = _store,					\
+static const struct file_operations debug_fake_inject_fops = {
+	.open = debugfs_open,
+	.write = edac_fake_inject_write,
+	.llseek = generic_file_llseek,
 };
+#endif
 
 /* default Control file */
-MCIDEV_ATTR(reset_counters, S_IWUSR, NULL, mci_reset_counters_store);
+DEVICE_ATTR(reset_counters, S_IWUSR, NULL, mci_reset_counters_store);
 
 /* default Attribute files */
-MCIDEV_ATTR(mc_name, S_IRUGO, mci_ctl_name_show, NULL);
-MCIDEV_ATTR(size_mb, S_IRUGO, mci_size_mb_show, NULL);
-MCIDEV_ATTR(seconds_since_reset, S_IRUGO, mci_seconds_show, NULL);
-MCIDEV_ATTR(ue_noinfo_count, S_IRUGO, mci_ue_noinfo_show, NULL);
-MCIDEV_ATTR(ce_noinfo_count, S_IRUGO, mci_ce_noinfo_show, NULL);
-MCIDEV_ATTR(ue_count, S_IRUGO, mci_ue_count_show, NULL);
-MCIDEV_ATTR(ce_count, S_IRUGO, mci_ce_count_show, NULL);
+DEVICE_ATTR(mc_name, S_IRUGO, mci_ctl_name_show, NULL);
+DEVICE_ATTR(size_mb, S_IRUGO, mci_size_mb_show, NULL);
+DEVICE_ATTR(seconds_since_reset, S_IRUGO, mci_seconds_show, NULL);
+DEVICE_ATTR(ue_noinfo_count, S_IRUGO, mci_ue_noinfo_show, NULL);
+DEVICE_ATTR(ce_noinfo_count, S_IRUGO, mci_ce_noinfo_show, NULL);
+DEVICE_ATTR(ue_count, S_IRUGO, mci_ue_count_show, NULL);
+DEVICE_ATTR(ce_count, S_IRUGO, mci_ce_count_show, NULL);
+DEVICE_ATTR(max_location, S_IRUGO, mci_max_location_show, NULL);
 
 /* memory scrubber attribute file */
-MCIDEV_ATTR(sdram_scrub_rate, S_IRUGO | S_IWUSR, mci_sdram_scrub_rate_show,
+DEVICE_ATTR(sdram_scrub_rate, S_IRUGO | S_IWUSR, mci_sdram_scrub_rate_show,
 	mci_sdram_scrub_rate_store);
 
-static struct mcidev_sysfs_attribute *mci_attr[] = {
-	&mci_attr_reset_counters,
-	&mci_attr_mc_name,
-	&mci_attr_size_mb,
-	&mci_attr_seconds_since_reset,
-	&mci_attr_ue_noinfo_count,
-	&mci_attr_ce_noinfo_count,
-	&mci_attr_ue_count,
-	&mci_attr_ce_count,
-	&mci_attr_sdram_scrub_rate,
+static struct attribute *mci_attrs[] = {
+	&dev_attr_reset_counters.attr,
+	&dev_attr_mc_name.attr,
+	&dev_attr_size_mb.attr,
+	&dev_attr_seconds_since_reset.attr,
+	&dev_attr_ue_noinfo_count.attr,
+	&dev_attr_ce_noinfo_count.attr,
+	&dev_attr_ue_count.attr,
+	&dev_attr_ce_count.attr,
+	&dev_attr_sdram_scrub_rate.attr,
+	&dev_attr_max_location.attr,
 	NULL
 };
 
+static struct attribute_group mci_attr_grp = {
+	.attrs	= mci_attrs,
+};
 
-/*
- * Release of a MC controlling instance
- *
- *	each MC control instance has the following resources upon entry:
- *		a) a ref count on the top memctl kobj
- *		b) a ref count on this module
- *
- *	this function must decrement those ref counts and then
- *	issue a free on the instance's memory
- */
-static void edac_mci_control_release(struct kobject *kobj)
-{
-	struct mem_ctl_info *mci;
-
-	mci = to_mci(kobj);
+static const struct attribute_group *mci_attr_groups[] = {
+	&mci_attr_grp,
+	NULL
+};
 
-	debugf0("%s() mci instance idx=%d releasing\n", __func__, mci->mc_idx);
+static void mci_attr_release(struct device *dev)
+{
+	struct mem_ctl_info *mci = container_of(dev, struct mem_ctl_info, dev);
 
-	/* decrement the module ref count */
-	module_put(mci->owner);
+	edac_dbg(1, "Releasing csrow device %s\n", dev_name(dev));
+	kfree(mci);
 }
 
-static struct kobj_type ktype_mci = {
-	.release = edac_mci_control_release,
-	.sysfs_ops = &mci_ops,
-	.default_attrs = (struct attribute **)mci_attr,
+static struct device_type mci_attr_type = {
+	.groups		= mci_attr_groups,
+	.release	= mci_attr_release,
 };
 
-/* EDAC memory controller sysfs kset:
- *	/sys/devices/system/edac/mc
- */
-static struct kset *mc_kset;
+#ifdef CONFIG_EDAC_DEBUG
+static struct dentry *edac_debugfs;
 
-/*
- * edac_mc_register_sysfs_main_kobj
- *
- *	setups and registers the main kobject for each mci
- */
-int edac_mc_register_sysfs_main_kobj(struct mem_ctl_info *mci)
+int __init edac_debugfs_init(void)
 {
-	struct kobject *kobj_mci;
-	int err;
-
-	debugf1("%s()\n", __func__);
-
-	kobj_mci = &mci->edac_mci_kobj;
-
-	/* Init the mci's kobject */
-	memset(kobj_mci, 0, sizeof(*kobj_mci));
-
-	/* Record which module 'owns' this control structure
-	 * and bump the ref count of the module
-	 */
-	mci->owner = THIS_MODULE;
-
-	/* bump ref count on this module */
-	if (!try_module_get(mci->owner)) {
-		err = -ENODEV;
-		goto fail_out;
-	}
-
-	/* this instance become part of the mc_kset */
-	kobj_mci->kset = mc_kset;
-
-	/* register the mc<id> kobject to the mc_kset */
-	err = kobject_init_and_add(kobj_mci, &ktype_mci, NULL,
-				   "mc%d", mci->mc_idx);
-	if (err) {
-		debugf1("%s()Failed to register '.../edac/mc%d'\n",
-			__func__, mci->mc_idx);
-		goto kobj_reg_fail;
+	edac_debugfs = debugfs_create_dir("edac", NULL);
+	if (IS_ERR(edac_debugfs)) {
+		edac_debugfs = NULL;
+		return -ENOMEM;
 	}
-	kobject_uevent(kobj_mci, KOBJ_ADD);
-
-	/* At this point, to 'free' the control struct,
-	 * edac_mc_unregister_sysfs_main_kobj() must be used
-	 */
-
-	debugf1("%s() Registered '.../edac/mc%d' kobject\n",
-		__func__, mci->mc_idx);
-
 	return 0;
-
-	/* Error exit stack */
-
-kobj_reg_fail:
-	module_put(mci->owner);
-
-fail_out:
-	return err;
-}
-
-/*
- * edac_mc_register_sysfs_main_kobj
- *
- *	tears down and the main mci kobject from the mc_kset
- */
-void edac_mc_unregister_sysfs_main_kobj(struct mem_ctl_info *mci)
-{
-	debugf1("%s()\n", __func__);
-
-	/* delete the kobj from the mc_kset */
-	kobject_put(&mci->edac_mci_kobj);
-}
-
-#define EDAC_DEVICE_SYMLINK	"device"
-
-#define grp_to_mci(k) (container_of(k, struct mcidev_sysfs_group_kobj, kobj)->mci)
-
-/* MCI show/store functions for top most object */
-static ssize_t inst_grp_show(struct kobject *kobj, struct attribute *attr,
-			char *buffer)
-{
-	struct mem_ctl_info *mem_ctl_info = grp_to_mci(kobj);
-	struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
-
-	debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
-
-	if (mcidev_attr->show)
-		return mcidev_attr->show(mem_ctl_info, buffer);
-
-	return -EIO;
 }
 
-static ssize_t inst_grp_store(struct kobject *kobj, struct attribute *attr,
-			const char *buffer, size_t count)
+void __exit edac_debugfs_exit(void)
 {
-	struct mem_ctl_info *mem_ctl_info = grp_to_mci(kobj);
-	struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
-
-	debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
-
-	if (mcidev_attr->store)
-		return mcidev_attr->store(mem_ctl_info, buffer, count);
-
-	return -EIO;
+	debugfs_remove(edac_debugfs);
 }
 
-/* No memory to release for this kobj */
-static void edac_inst_grp_release(struct kobject *kobj)
+int edac_create_debug_nodes(struct mem_ctl_info *mci)
 {
-	struct mcidev_sysfs_group_kobj *grp;
-	struct mem_ctl_info *mci;
-
-	debugf1("%s()\n", __func__);
-
-	grp = container_of(kobj, struct mcidev_sysfs_group_kobj, kobj);
-	mci = grp->mci;
-}
-
-/* Intermediate show/store table */
-static struct sysfs_ops inst_grp_ops = {
-	.show = inst_grp_show,
-	.store = inst_grp_store
-};
-
-/* the kobj_type instance for a instance group */
-static struct kobj_type ktype_inst_grp = {
-	.release = edac_inst_grp_release,
-	.sysfs_ops = &inst_grp_ops,
-};
-
+	struct dentry *d, *parent;
+	char name[80];
+	int i;
 
-/*
- * edac_create_mci_instance_attributes
- *	create MC driver specific attributes bellow an specified kobj
- * This routine calls itself recursively, in order to create an entire
- * object tree.
- */
-static int edac_create_mci_instance_attributes(struct mem_ctl_info *mci,
-				const struct mcidev_sysfs_attribute *sysfs_attrib,
-				struct kobject *kobj)
-{
-	int err;
+	if (!edac_debugfs)
+		return -ENODEV;
 
-	debugf4("%s()\n", __func__);
-
-	while (sysfs_attrib) {
-		debugf4("%s() sysfs_attrib = %p\n",__func__, sysfs_attrib);
-		if (sysfs_attrib->grp) {
-			struct mcidev_sysfs_group_kobj *grp_kobj;
-
-			grp_kobj = kzalloc(sizeof(*grp_kobj), GFP_KERNEL);
-			if (!grp_kobj)
-				return -ENOMEM;
-
-			grp_kobj->grp = sysfs_attrib->grp;
-			grp_kobj->mci = mci;
-			list_add_tail(&grp_kobj->list, &mci->grp_kobj_list);
-
-			debugf0("%s() grp %s, mci %p\n", __func__,
-				sysfs_attrib->grp->name, mci);
-
-			err = kobject_init_and_add(&grp_kobj->kobj,
-						&ktype_inst_grp,
-						&mci->edac_mci_kobj,
-						sysfs_attrib->grp->name);
-			if (err < 0) {
-				printk(KERN_ERR "kobject_init_and_add failed: %d\n", err);
-				return err;
-			}
-			err = edac_create_mci_instance_attributes(mci,
-					grp_kobj->grp->mcidev_attr,
-					&grp_kobj->kobj);
-
-			if (err < 0)
-				return err;
-		} else if (sysfs_attrib->attr.name) {
-			debugf4("%s() file %s\n", __func__,
-				sysfs_attrib->attr.name);
-
-			err = sysfs_create_file(kobj, &sysfs_attrib->attr);
-			if (err < 0) {
-				printk(KERN_ERR "sysfs_create_file failed: %d\n", err);
-				return err;
-			}
-		} else
-			break;
-
-		sysfs_attrib++;
+	d = debugfs_create_dir(mci->dev.kobj.name, edac_debugfs);
+	if (!d)
+		return -ENOMEM;
+	parent = d;
+
+	for (i = 0; i < mci->n_layers; i++) {
+		sprintf(name, "fake_inject_%s",
+			     edac_layer_name[mci->layers[i].type]);
+		d = debugfs_create_u8(name, S_IRUGO | S_IWUSR, parent,
+				      &mci->fake_inject_layer[i]);
+		if (!d)
+			goto nomem;
 	}
 
-	return 0;
-}
+	d = debugfs_create_bool("fake_inject_ue", S_IRUGO | S_IWUSR, parent,
+				&mci->fake_inject_ue);
+	if (!d)
+		goto nomem;
 
-/*
- * edac_remove_mci_instance_attributes
- *	remove MC driver specific attributes at the topmost level
- *	directory of this mci instance.
- */
-static void edac_remove_mci_instance_attributes(struct mem_ctl_info *mci,
-				const struct mcidev_sysfs_attribute *sysfs_attrib,
-				struct kobject *kobj, int count)
-{
-	struct mcidev_sysfs_group_kobj *grp_kobj, *tmp;
+	d = debugfs_create_u16("fake_inject_count", S_IRUGO | S_IWUSR, parent,
+				&mci->fake_inject_count);
+	if (!d)
+		goto nomem;
 
-	debugf1("%s()\n", __func__);
-
-	/*
-	 * loop if there are attributes and until we hit a NULL entry
-	 * Remove first all the attributes
-	 */
-	while (sysfs_attrib) {
-		debugf4("%s() sysfs_attrib = %p\n",__func__, sysfs_attrib);
-		if (sysfs_attrib->grp) {
-			debugf4("%s() seeking for group %s\n",
-				__func__, sysfs_attrib->grp->name);
-			list_for_each_entry(grp_kobj,
-					    &mci->grp_kobj_list, list) {
-				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);
-					debugf4("%s() group %s\n", __func__,
-						sysfs_attrib->grp->name);
-					kobject_put(&grp_kobj->kobj);
-				}
-			}
-			debugf4("%s() end of seeking for group %s\n",
-				__func__, sysfs_attrib->grp->name);
-		} else if (sysfs_attrib->attr.name) {
-			debugf4("%s() file %s\n", __func__,
-				sysfs_attrib->attr.name);
-			sysfs_remove_file(kobj, &sysfs_attrib->attr);
-		} else
-			break;
-		sysfs_attrib++;
-	}
+	d = debugfs_create_file("fake_inject", S_IWUSR, parent,
+				&mci->dev,
+				&debug_fake_inject_fops);
+	if (!d)
+		goto nomem;
 
-	/* Remove the group objects */
-	if (count)
-		return;
-	list_for_each_entry_safe(grp_kobj, tmp,
-				 &mci->grp_kobj_list, list) {
-		list_del(&grp_kobj->list);
-		kfree(grp_kobj);
-	}
+	mci->debugfs = parent;
+	return 0;
+nomem:
+	debugfs_remove(mci->debugfs);
+	return -ENOMEM;
 }
-
+#endif
 
 /*
  * Create a new Memory Controller kobject instance,
@@ -906,77 +979,87 @@ static void edac_remove_mci_instance_attributes(struct mem_ctl_info *mci,
  */
 int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 {
-	int i, j;
-	int err;
-	struct csrow_info *csrow;
-	struct kobject *kobj_mci = &mci->edac_mci_kobj;
+	int i, err;
 
-	debugf0("%s() idx=%d\n", __func__, mci->mc_idx);
-
-	INIT_LIST_HEAD(&mci->grp_kobj_list);
+	/*
+	 * The memory controller needs its own bus, in order to avoid
+	 * namespace conflicts at /sys/bus/edac.
+	 */
+	mci->bus.name = kasprintf(GFP_KERNEL, "mc%d", mci->mc_idx);
+	if (!mci->bus.name)
+		return -ENOMEM;
+	edac_dbg(0, "creating bus %s\n", mci->bus.name);
+	err = bus_register(&mci->bus);
+	if (err < 0)
+		return err;
 
-	/* create a symlink for the device */
-	err = sysfs_create_link(kobj_mci, &mci->dev->kobj,
-				EDAC_DEVICE_SYMLINK);
-	if (err) {
-		debugf1("%s() failure to create symlink\n", __func__);
-		goto fail0;
+	/* get the /sys/devices/system/edac subsys reference */
+	mci->dev.type = &mci_attr_type;
+	device_initialize(&mci->dev);
+
+	mci->dev.parent = mci_pdev;
+	mci->dev.bus = &mci->bus;
+	dev_set_name(&mci->dev, "mc%d", mci->mc_idx);
+	dev_set_drvdata(&mci->dev, mci);
+	pm_runtime_forbid(&mci->dev);
+
+	edac_dbg(0, "creating device %s\n", dev_name(&mci->dev));
+	err = device_add(&mci->dev);
+	if (err < 0) {
+		bus_unregister(&mci->bus);
+		kfree(mci->bus.name);
+		return err;
 	}
 
-	/* If the low level driver desires some attributes,
-	 * then create them now for the driver.
+	/*
+	 * Create the dimm/rank devices
 	 */
-	if (mci->mc_driver_sysfs_attributes) {
-		err = edac_create_mci_instance_attributes(mci,
-					mci->mc_driver_sysfs_attributes,
-					&mci->edac_mci_kobj);
+	for (i = 0; i < mci->tot_dimms; i++) {
+		struct dimm_info *dimm = mci->dimms[i];
+		/* Only expose populated DIMMs */
+		if (dimm->nr_pages == 0)
+			continue;
+#ifdef CONFIG_EDAC_DEBUG
+		edac_dbg(1, "creating dimm%d, located at ", i);
+		if (edac_debug_level >= 1) {
+			int lay;
+			for (lay = 0; lay < mci->n_layers; lay++)
+				printk(KERN_CONT "%s %d ",
+					edac_layer_name[mci->layers[lay].type],
+					dimm->location[lay]);
+			printk(KERN_CONT "\n");
+		}
+#endif
+		err = edac_create_dimm_object(mci, dimm, i);
 		if (err) {
-			debugf1("%s() failure to create mci attributes\n",
-				__func__);
-			goto fail0;
+			edac_dbg(1, "failure: create dimm %d obj\n", i);
+			goto fail;
 		}
 	}
 
-	/* Make directories for each CSROW object under the mc<id> kobject
-	 */
-	for (i = 0; i < mci->nr_csrows; i++) {
-		int nr_pages = 0;
-
-		csrow = &mci->csrows[i];
-		for (j = 0; j < csrow->nr_channels; j++)
-			nr_pages += csrow->channels[j].dimm->nr_pages;
-
-		if (nr_pages > 0) {
-			err = edac_create_csrow_object(mci, csrow, i);
-			if (err) {
-				debugf1("%s() failure: create csrow %d obj\n",
-					__func__, i);
-				goto fail1;
-			}
-		}
-	}
+#ifdef CONFIG_EDAC_LEGACY_SYSFS
+	err = edac_create_csrow_objects(mci);
+	if (err < 0)
+		goto fail;
+#endif
 
+#ifdef CONFIG_EDAC_DEBUG
+	edac_create_debug_nodes(mci);
+#endif
 	return 0;
 
-fail1:
+fail:
 	for (i--; i >= 0; i--) {
-		int nr_pages = 0;
-
-		csrow = &mci->csrows[i];
-		for (j = 0; j < csrow->nr_channels; j++)
-			nr_pages += csrow->channels[j].dimm->nr_pages;
-		if (nr_pages > 0)
-			kobject_put(&mci->csrows[i].kobj);
+		struct dimm_info *dimm = mci->dimms[i];
+		if (dimm->nr_pages == 0)
+			continue;
+		put_device(&dimm->dev);
+		device_del(&dimm->dev);
 	}
-
-	/* remove the mci instance's attributes, if any */
-	edac_remove_mci_instance_attributes(mci,
-		mci->mc_driver_sysfs_attributes, &mci->edac_mci_kobj, 0);
-
-	/* remove the symlink */
-	sysfs_remove_link(kobj_mci, EDAC_DEVICE_SYMLINK);
-
-fail0:
+	put_device(&mci->dev);
+	device_del(&mci->dev);
+	bus_unregister(&mci->bus);
+	kfree(mci->bus.name);
 	return err;
 }
 
@@ -985,98 +1068,84 @@ fail0:
  */
 void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci)
 {
-	struct csrow_info *csrow;
-	int i, j;
-
-	debugf0("%s()\n", __func__);
-
-	/* remove all csrow kobjects */
-	debugf4("%s()  unregister this mci kobj\n", __func__);
-	for (i = 0; i < mci->nr_csrows; i++) {
-		int nr_pages = 0;
-
-		csrow = &mci->csrows[i];
-		for (j = 0; j < csrow->nr_channels; j++)
-			nr_pages += csrow->channels[j].dimm->nr_pages;
-		if (nr_pages > 0) {
-			debugf0("%s()  unreg csrow-%d\n", __func__, i);
-			kobject_put(&mci->csrows[i].kobj);
-		}
-	}
+	int i;
 
-	/* remove this mci instance's attribtes */
-	if (mci->mc_driver_sysfs_attributes) {
-		debugf4("%s()  unregister mci private attributes\n", __func__);
-		edac_remove_mci_instance_attributes(mci,
-						mci->mc_driver_sysfs_attributes,
-						&mci->edac_mci_kobj, 0);
+	edac_dbg(0, "\n");
+
+#ifdef CONFIG_EDAC_DEBUG
+	debugfs_remove(mci->debugfs);
+#endif
+#ifdef CONFIG_EDAC_LEGACY_SYSFS
+	edac_delete_csrow_objects(mci);
+#endif
+
+	for (i = 0; i < mci->tot_dimms; i++) {
+		struct dimm_info *dimm = mci->dimms[i];
+		if (dimm->nr_pages == 0)
+			continue;
+		edac_dbg(0, "removing device %s\n", dev_name(&dimm->dev));
+		put_device(&dimm->dev);
+		device_del(&dimm->dev);
 	}
-
-	/* remove the symlink */
-	debugf4("%s()  remove_link\n", __func__);
-	sysfs_remove_link(&mci->edac_mci_kobj, EDAC_DEVICE_SYMLINK);
-
-	/* unregister this instance's kobject */
-	debugf4("%s()  remove_mci_instance\n", __func__);
-	kobject_put(&mci->edac_mci_kobj);
 }
 
+void edac_unregister_sysfs(struct mem_ctl_info *mci)
+{
+	edac_dbg(1, "Unregistering device %s\n", dev_name(&mci->dev));
+	put_device(&mci->dev);
+	device_del(&mci->dev);
+	bus_unregister(&mci->bus);
+	kfree(mci->bus.name);
+}
 
+static void mc_attr_release(struct device *dev)
+{
+	/*
+	 * There's no container structure here, as this is just the mci
+	 * parent device, used to create the /sys/devices/mc sysfs node.
+	 * So, there are no attributes on it.
+	 */
+	edac_dbg(1, "Releasing device %s\n", dev_name(dev));
+	kfree(dev);
+}
 
-
+static struct device_type mc_attr_type = {
+	.release	= mc_attr_release,
+};
 /*
- * edac_setup_sysfs_mc_kset(void)
- *
- * Initialize the mc_kset for the 'mc' entry
- *	This requires creating the top 'mc' directory with a kset
- *	and its controls/attributes.
- *
- *	To this 'mc' kset, instance 'mci' will be grouped as children.
- *
- * Return:  0 SUCCESS
- *         !0 FAILURE error code
+ * Init/exit code for the module. Basically, creates/removes /sys/class/rc
  */
-int edac_sysfs_setup_mc_kset(void)
+int __init edac_mc_sysfs_init(void)
 {
-	int err = -EINVAL;
 	struct bus_type *edac_subsys;
-
-	debugf1("%s()\n", __func__);
+	int err;
 
 	/* get the /sys/devices/system/edac subsys reference */
 	edac_subsys = edac_get_sysfs_subsys();
 	if (edac_subsys == NULL) {
-		debugf1("%s() no edac_subsys error=%d\n", __func__, err);
-		goto fail_out;
+		edac_dbg(1, "no edac_subsys\n");
+		return -EINVAL;
 	}
 
-	/* Init the MC's kobject */
-	mc_kset = kset_create_and_add("mc", NULL, &edac_subsys->dev_root->kobj);
-	if (!mc_kset) {
-		err = -ENOMEM;
-		debugf1("%s() Failed to register '.../edac/mc'\n", __func__);
-		goto fail_kset;
-	}
+	mci_pdev = kzalloc(sizeof(*mci_pdev), GFP_KERNEL);
 
-	debugf1("%s() Registered '.../edac/mc' kobject\n", __func__);
+	mci_pdev->bus = edac_subsys;
+	mci_pdev->type = &mc_attr_type;
+	device_initialize(mci_pdev);
+	dev_set_name(mci_pdev, "mc");
 
-	return 0;
+	err = device_add(mci_pdev);
+	if (err < 0)
+		return err;
 
-fail_kset:
-	edac_put_sysfs_subsys();
+	edac_dbg(0, "device %s created\n", dev_name(mci_pdev));
 
-fail_out:
-	return err;
+	return 0;
 }
 
-/*
- * edac_sysfs_teardown_mc_kset
- *
- *	deconstruct the mc_ket for memory controllers
- */
-void edac_sysfs_teardown_mc_kset(void)
+void __exit edac_mc_sysfs_exit(void)
 {
-	kset_unregister(mc_kset);
+	put_device(mci_pdev);
+	device_del(mci_pdev);
 	edac_put_sysfs_subsys();
 }
-
diff --git a/drivers/edac/edac_module.c b/drivers/edac/edac_module.c
index 5ddaa86d6a6..58a28d838f3 100644
--- a/drivers/edac/edac_module.c
+++ b/drivers/edac/edac_module.c
@@ -15,7 +15,7 @@
 #include "edac_core.h"
 #include "edac_module.h"
 
-#define EDAC_VERSION "Ver: 2.1.0"
+#define EDAC_VERSION "Ver: 3.0.0"
 
 #ifdef CONFIG_EDAC_DEBUG
 /* Values of 0 to 4 will generate output */
@@ -90,26 +90,21 @@ static int __init edac_init(void)
 	 */
 	edac_pci_clear_parity_errors();
 
-	/*
-	 * now set up the mc_kset under the edac class object
-	 */
-	err = edac_sysfs_setup_mc_kset();
+	err = edac_mc_sysfs_init();
 	if (err)
 		goto error;
 
+	edac_debugfs_init();
+
 	/* Setup/Initialize the workq for this core */
 	err = edac_workqueue_setup();
 	if (err) {
 		edac_printk(KERN_ERR, EDAC_MC, "init WorkQueue failure\n");
-		goto workq_fail;
+		goto error;
 	}
 
 	return 0;
 
-	/* Error teardown stack */
-workq_fail:
-	edac_sysfs_teardown_mc_kset();
-
 error:
 	return err;
 }
@@ -120,11 +115,12 @@ error:
  */
 static void __exit edac_exit(void)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* tear down the various subsystems */
 	edac_workqueue_teardown();
-	edac_sysfs_teardown_mc_kset();
+	edac_mc_sysfs_exit();
+	edac_debugfs_exit();
 }
 
 /*
diff --git a/drivers/edac/edac_module.h b/drivers/edac/edac_module.h
index 0ea7d14cb93..3d139c6e7fe 100644
--- a/drivers/edac/edac_module.h
+++ b/drivers/edac/edac_module.h
@@ -19,12 +19,12 @@
  *
  * edac_mc objects
  */
-extern int edac_sysfs_setup_mc_kset(void);
-extern void edac_sysfs_teardown_mc_kset(void);
-extern int edac_mc_register_sysfs_main_kobj(struct mem_ctl_info *mci);
-extern void edac_mc_unregister_sysfs_main_kobj(struct mem_ctl_info *mci);
+	/* on edac_mc_sysfs.c */
+int edac_mc_sysfs_init(void);
+void edac_mc_sysfs_exit(void);
 extern int edac_create_sysfs_mci_device(struct mem_ctl_info *mci);
 extern void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci);
+void edac_unregister_sysfs(struct mem_ctl_info *mci);
 extern int edac_get_log_ue(void);
 extern int edac_get_log_ce(void);
 extern int edac_get_panic_on_ue(void);
@@ -34,6 +34,10 @@ extern int edac_mc_get_panic_on_ue(void);
 extern int edac_get_poll_msec(void);
 extern int edac_mc_get_poll_msec(void);
 
+unsigned edac_dimm_info_location(struct dimm_info *dimm, char *buf,
+				 unsigned len);
+
+	/* on edac_device.c */
 extern int edac_device_register_sysfs_main_kobj(
 				struct edac_device_ctl_info *edac_dev);
 extern void edac_device_unregister_sysfs_main_kobj(
@@ -53,6 +57,20 @@ extern void edac_mc_reset_delay_period(int value);
 extern void *edac_align_ptr(void **p, unsigned size, int n_elems);
 
 /*
+ * EDAC debugfs functions
+ */
+#ifdef CONFIG_EDAC_DEBUG
+int edac_debugfs_init(void);
+void edac_debugfs_exit(void);
+#else
+static inline int edac_debugfs_init(void)
+{
+	return -ENODEV;
+}
+static inline void edac_debugfs_exit(void) {}
+#endif
+
+/*
  * EDAC PCI functions
  */
 #ifdef	CONFIG_PCI
diff --git a/drivers/edac/edac_pci.c b/drivers/edac/edac_pci.c
index f1ac8664988..ee87ef972ea 100644
--- a/drivers/edac/edac_pci.c
+++ b/drivers/edac/edac_pci.c
@@ -45,7 +45,7 @@ struct edac_pci_ctl_info *edac_pci_alloc_ctl_info(unsigned int sz_pvt,
 	void *p = NULL, *pvt;
 	unsigned int size;
 
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	pci = edac_align_ptr(&p, sizeof(*pci), 1);
 	pvt = edac_align_ptr(&p, 1, sz_pvt);
@@ -80,7 +80,7 @@ EXPORT_SYMBOL_GPL(edac_pci_alloc_ctl_info);
  */
 void edac_pci_free_ctl_info(struct edac_pci_ctl_info *pci)
 {
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	edac_pci_remove_sysfs(pci);
 }
@@ -97,7 +97,7 @@ static struct edac_pci_ctl_info *find_edac_pci_by_dev(struct device *dev)
 	struct edac_pci_ctl_info *pci;
 	struct list_head *item;
 
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	list_for_each(item, &edac_pci_list) {
 		pci = list_entry(item, struct edac_pci_ctl_info, link);
@@ -122,7 +122,7 @@ static int add_edac_pci_to_global_list(struct edac_pci_ctl_info *pci)
 	struct list_head *item, *insert_before;
 	struct edac_pci_ctl_info *rover;
 
-	debugf1("%s()\n", __func__);
+	edac_dbg(1, "\n");
 
 	insert_before = &edac_pci_list;
 
@@ -226,7 +226,7 @@ static void edac_pci_workq_function(struct work_struct *work_req)
 	int msec;
 	unsigned long delay;
 
-	debugf3("%s() checking\n", __func__);
+	edac_dbg(3, "checking\n");
 
 	mutex_lock(&edac_pci_ctls_mutex);
 
@@ -261,7 +261,7 @@ static void edac_pci_workq_function(struct work_struct *work_req)
 static void edac_pci_workq_setup(struct edac_pci_ctl_info *pci,
 				 unsigned int msec)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	INIT_DELAYED_WORK(&pci->work, edac_pci_workq_function);
 	queue_delayed_work(edac_workqueue, &pci->work,
@@ -276,7 +276,7 @@ static void edac_pci_workq_teardown(struct edac_pci_ctl_info *pci)
 {
 	int status;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	status = cancel_delayed_work(&pci->work);
 	if (status == 0)
@@ -293,7 +293,7 @@ static void edac_pci_workq_teardown(struct edac_pci_ctl_info *pci)
 void edac_pci_reset_delay_period(struct edac_pci_ctl_info *pci,
 				 unsigned long value)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	edac_pci_workq_teardown(pci);
 
@@ -333,7 +333,7 @@ EXPORT_SYMBOL_GPL(edac_pci_alloc_index);
  */
 int edac_pci_add_device(struct edac_pci_ctl_info *pci, int edac_idx)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	pci->pci_idx = edac_idx;
 	pci->start_time = jiffies;
@@ -393,7 +393,7 @@ struct edac_pci_ctl_info *edac_pci_del_device(struct device *dev)
 {
 	struct edac_pci_ctl_info *pci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	mutex_lock(&edac_pci_ctls_mutex);
 
@@ -430,7 +430,7 @@ EXPORT_SYMBOL_GPL(edac_pci_del_device);
  */
 static void edac_pci_generic_check(struct edac_pci_ctl_info *pci)
 {
-	debugf4("%s()\n", __func__);
+	edac_dbg(4, "\n");
 	edac_pci_do_parity_check();
 }
 
@@ -475,7 +475,7 @@ struct edac_pci_ctl_info *edac_pci_create_generic_ctl(struct device *dev,
 	pdata->edac_idx = edac_pci_idx++;
 
 	if (edac_pci_add_device(pci, pdata->edac_idx) > 0) {
-		debugf3("%s(): failed edac_pci_add_device()\n", __func__);
+		edac_dbg(3, "failed edac_pci_add_device()\n");
 		edac_pci_free_ctl_info(pci);
 		return NULL;
 	}
@@ -491,7 +491,7 @@ EXPORT_SYMBOL_GPL(edac_pci_create_generic_ctl);
  */
 void edac_pci_release_generic_ctl(struct edac_pci_ctl_info *pci)
 {
-	debugf0("%s() pci mod=%s\n", __func__, pci->mod_name);
+	edac_dbg(0, "pci mod=%s\n", pci->mod_name);
 
 	edac_pci_del_device(pci->dev);
 	edac_pci_free_ctl_info(pci);
diff --git a/drivers/edac/edac_pci_sysfs.c b/drivers/edac/edac_pci_sysfs.c
index 97f5064e399..e164c555a33 100644
--- a/drivers/edac/edac_pci_sysfs.c
+++ b/drivers/edac/edac_pci_sysfs.c
@@ -78,7 +78,7 @@ static void edac_pci_instance_release(struct kobject *kobj)
 {
 	struct edac_pci_ctl_info *pci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* Form pointer to containing struct, the pci control struct */
 	pci = to_instance(kobj);
@@ -161,7 +161,7 @@ static int edac_pci_create_instance_kobj(struct edac_pci_ctl_info *pci, int idx)
 	struct kobject *main_kobj;
 	int err;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* First bump the ref count on the top main kobj, which will
 	 * track the number of PCI instances we have, and thus nest
@@ -177,14 +177,13 @@ static int edac_pci_create_instance_kobj(struct edac_pci_ctl_info *pci, int idx)
 	err = kobject_init_and_add(&pci->kobj, &ktype_pci_instance,
 				   edac_pci_top_main_kobj, "pci%d", idx);
 	if (err != 0) {
-		debugf2("%s() failed to register instance pci%d\n",
-			__func__, idx);
+		edac_dbg(2, "failed to register instance pci%d\n", idx);
 		kobject_put(edac_pci_top_main_kobj);
 		goto error_out;
 	}
 
 	kobject_uevent(&pci->kobj, KOBJ_ADD);
-	debugf1("%s() Register instance 'pci%d' kobject\n", __func__, idx);
+	edac_dbg(1, "Register instance 'pci%d' kobject\n", idx);
 
 	return 0;
 
@@ -201,7 +200,7 @@ error_out:
 static void edac_pci_unregister_sysfs_instance_kobj(
 			struct edac_pci_ctl_info *pci)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* Unregister the instance kobject and allow its release
 	 * function release the main reference count and then
@@ -317,7 +316,7 @@ static struct edac_pci_dev_attribute *edac_pci_attr[] = {
  */
 static void edac_pci_release_main_kobj(struct kobject *kobj)
 {
-	debugf0("%s() here to module_put(THIS_MODULE)\n", __func__);
+	edac_dbg(0, "here to module_put(THIS_MODULE)\n");
 
 	kfree(kobj);
 
@@ -345,7 +344,7 @@ static int edac_pci_main_kobj_setup(void)
 	int err;
 	struct bus_type *edac_subsys;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* check and count if we have already created the main kobject */
 	if (atomic_inc_return(&edac_pci_sysfs_refcount) != 1)
@@ -356,7 +355,7 @@ static int edac_pci_main_kobj_setup(void)
 	 */
 	edac_subsys = edac_get_sysfs_subsys();
 	if (edac_subsys == NULL) {
-		debugf1("%s() no edac_subsys\n", __func__);
+		edac_dbg(1, "no edac_subsys\n");
 		err = -ENODEV;
 		goto decrement_count_fail;
 	}
@@ -366,14 +365,14 @@ static int edac_pci_main_kobj_setup(void)
 	 * level main kobj for EDAC PCI
 	 */
 	if (!try_module_get(THIS_MODULE)) {
-		debugf1("%s() try_module_get() failed\n", __func__);
+		edac_dbg(1, "try_module_get() failed\n");
 		err = -ENODEV;
 		goto mod_get_fail;
 	}
 
 	edac_pci_top_main_kobj = kzalloc(sizeof(struct kobject), GFP_KERNEL);
 	if (!edac_pci_top_main_kobj) {
-		debugf1("Failed to allocate\n");
+		edac_dbg(1, "Failed to allocate\n");
 		err = -ENOMEM;
 		goto kzalloc_fail;
 	}
@@ -383,7 +382,7 @@ static int edac_pci_main_kobj_setup(void)
 				   &ktype_edac_pci_main_kobj,
 				   &edac_subsys->dev_root->kobj, "pci");
 	if (err) {
-		debugf1("Failed to register '.../edac/pci'\n");
+		edac_dbg(1, "Failed to register '.../edac/pci'\n");
 		goto kobject_init_and_add_fail;
 	}
 
@@ -392,7 +391,7 @@ static int edac_pci_main_kobj_setup(void)
 	 * must be used, for resources to be cleaned up properly
 	 */
 	kobject_uevent(edac_pci_top_main_kobj, KOBJ_ADD);
-	debugf1("Registered '.../edac/pci' kobject\n");
+	edac_dbg(1, "Registered '.../edac/pci' kobject\n");
 
 	return 0;
 
@@ -421,15 +420,14 @@ decrement_count_fail:
  */
 static void edac_pci_main_kobj_teardown(void)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* Decrement the count and only if no more controller instances
 	 * are connected perform the unregisteration of the top level
 	 * main kobj
 	 */
 	if (atomic_dec_return(&edac_pci_sysfs_refcount) == 0) {
-		debugf0("%s() called kobject_put on main kobj\n",
-			__func__);
+		edac_dbg(0, "called kobject_put on main kobj\n");
 		kobject_put(edac_pci_top_main_kobj);
 	}
 	edac_put_sysfs_subsys();
@@ -446,7 +444,7 @@ int edac_pci_create_sysfs(struct edac_pci_ctl_info *pci)
 	int err;
 	struct kobject *edac_kobj = &pci->kobj;
 
-	debugf0("%s() idx=%d\n", __func__, pci->pci_idx);
+	edac_dbg(0, "idx=%d\n", pci->pci_idx);
 
 	/* create the top main EDAC PCI kobject, IF needed */
 	err = edac_pci_main_kobj_setup();
@@ -460,8 +458,7 @@ int edac_pci_create_sysfs(struct edac_pci_ctl_info *pci)
 
 	err = sysfs_create_link(edac_kobj, &pci->dev->kobj, EDAC_PCI_SYMLINK);
 	if (err) {
-		debugf0("%s() sysfs_create_link() returned err= %d\n",
-			__func__, err);
+		edac_dbg(0, "sysfs_create_link() returned err= %d\n", err);
 		goto symlink_fail;
 	}
 
@@ -484,7 +481,7 @@ unregister_cleanup:
  */
 void edac_pci_remove_sysfs(struct edac_pci_ctl_info *pci)
 {
-	debugf0("%s() index=%d\n", __func__, pci->pci_idx);
+	edac_dbg(0, "index=%d\n", pci->pci_idx);
 
 	/* Remove the symlink */
 	sysfs_remove_link(&pci->kobj, EDAC_PCI_SYMLINK);
@@ -496,7 +493,7 @@ void edac_pci_remove_sysfs(struct edac_pci_ctl_info *pci)
 	 * if this 'pci' is the last instance.
 	 * If it is, the main kobject will be unregistered as a result
 	 */
-	debugf0("%s() calling edac_pci_main_kobj_teardown()\n", __func__);
+	edac_dbg(0, "calling edac_pci_main_kobj_teardown()\n");
 	edac_pci_main_kobj_teardown();
 }
 
@@ -572,7 +569,7 @@ static void edac_pci_dev_parity_test(struct pci_dev *dev)
 
 	local_irq_restore(flags);
 
-	debugf4("PCI STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
+	edac_dbg(4, "PCI STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
 
 	/* check the status reg for errors on boards NOT marked as broken
 	 * if broken, we cannot trust any of the status bits
@@ -603,13 +600,15 @@ static void edac_pci_dev_parity_test(struct pci_dev *dev)
 	}
 
 
-	debugf4("PCI HEADER TYPE= 0x%02x %s\n", header_type, dev_name(&dev->dev));
+	edac_dbg(4, "PCI HEADER TYPE= 0x%02x %s\n",
+		 header_type, dev_name(&dev->dev));
 
 	if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
 		/* On bridges, need to examine secondary status register  */
 		status = get_pci_parity_status(dev, 1);
 
-		debugf4("PCI SEC_STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
+		edac_dbg(4, "PCI SEC_STATUS= 0x%04x %s\n",
+			 status, dev_name(&dev->dev));
 
 		/* check the secondary status reg for errors,
 		 * on NOT broken boards
@@ -671,7 +670,7 @@ void edac_pci_do_parity_check(void)
 {
 	int before_count;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	/* if policy has PCI check off, leave now */
 	if (!check_pci_errors)
diff --git a/drivers/edac/highbank_l2_edac.c b/drivers/edac/highbank_l2_edac.c
new file mode 100644
index 00000000000..e599b00c05a
--- /dev/null
+++ b/drivers/edac/highbank_l2_edac.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright 2011-2012 Calxeda, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/ctype.h>
+#include <linux/edac.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/of_platform.h>
+
+#include "edac_core.h"
+#include "edac_module.h"
+
+#define SR_CLR_SB_ECC_INTR	0x0
+#define SR_CLR_DB_ECC_INTR	0x4
+
+struct hb_l2_drvdata {
+	void __iomem *base;
+	int sb_irq;
+	int db_irq;
+};
+
+static irqreturn_t highbank_l2_err_handler(int irq, void *dev_id)
+{
+	struct edac_device_ctl_info *dci = dev_id;
+	struct hb_l2_drvdata *drvdata = dci->pvt_info;
+
+	if (irq == drvdata->sb_irq) {
+		writel(1, drvdata->base + SR_CLR_SB_ECC_INTR);
+		edac_device_handle_ce(dci, 0, 0, dci->ctl_name);
+	}
+	if (irq == drvdata->db_irq) {
+		writel(1, drvdata->base + SR_CLR_DB_ECC_INTR);
+		edac_device_handle_ue(dci, 0, 0, dci->ctl_name);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit highbank_l2_err_probe(struct platform_device *pdev)
+{
+	struct edac_device_ctl_info *dci;
+	struct hb_l2_drvdata *drvdata;
+	struct resource *r;
+	int res = 0;
+
+	dci = edac_device_alloc_ctl_info(sizeof(*drvdata), "cpu",
+		1, "L", 1, 2, NULL, 0, 0);
+	if (!dci)
+		return -ENOMEM;
+
+	drvdata = dci->pvt_info;
+	dci->dev = &pdev->dev;
+	platform_set_drvdata(pdev, dci);
+
+	if (!devres_open_group(&pdev->dev, NULL, GFP_KERNEL))
+		return -ENOMEM;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "Unable to get mem resource\n");
+		res = -ENODEV;
+		goto err;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, r->start,
+				     resource_size(r), dev_name(&pdev->dev))) {
+		dev_err(&pdev->dev, "Error while requesting mem region\n");
+		res = -EBUSY;
+		goto err;
+	}
+
+	drvdata->base = devm_ioremap(&pdev->dev, r->start, resource_size(r));
+	if (!drvdata->base) {
+		dev_err(&pdev->dev, "Unable to map regs\n");
+		res = -ENOMEM;
+		goto err;
+	}
+
+	drvdata->db_irq = platform_get_irq(pdev, 0);
+	res = devm_request_irq(&pdev->dev, drvdata->db_irq,
+			       highbank_l2_err_handler,
+			       0, dev_name(&pdev->dev), dci);
+	if (res < 0)
+		goto err;
+
+	drvdata->sb_irq = platform_get_irq(pdev, 1);
+	res = devm_request_irq(&pdev->dev, drvdata->sb_irq,
+			       highbank_l2_err_handler,
+			       0, dev_name(&pdev->dev), dci);
+	if (res < 0)
+		goto err;
+
+	dci->mod_name = dev_name(&pdev->dev);
+	dci->dev_name = dev_name(&pdev->dev);
+
+	if (edac_device_add_device(dci))
+		goto err;
+
+	devres_close_group(&pdev->dev, NULL);
+	return 0;
+err:
+	devres_release_group(&pdev->dev, NULL);
+	edac_device_free_ctl_info(dci);
+	return res;
+}
+
+static int highbank_l2_err_remove(struct platform_device *pdev)
+{
+	struct edac_device_ctl_info *dci = platform_get_drvdata(pdev);
+
+	edac_device_del_device(&pdev->dev);
+	edac_device_free_ctl_info(dci);
+	return 0;
+}
+
+static const struct of_device_id hb_l2_err_of_match[] = {
+	{ .compatible = "calxeda,hb-sregs-l2-ecc", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, hb_l2_err_of_match);
+
+static struct platform_driver highbank_l2_edac_driver = {
+	.probe = highbank_l2_err_probe,
+	.remove = highbank_l2_err_remove,
+	.driver = {
+		.name = "hb_l2_edac",
+		.of_match_table = hb_l2_err_of_match,
+	},
+};
+
+module_platform_driver(highbank_l2_edac_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Calxeda, Inc.");
+MODULE_DESCRIPTION("EDAC Driver for Calxeda Highbank L2 Cache");
diff --git a/drivers/edac/highbank_mc_edac.c b/drivers/edac/highbank_mc_edac.c
new file mode 100644
index 00000000000..c769f477fd2
--- /dev/null
+++ b/drivers/edac/highbank_mc_edac.c
@@ -0,0 +1,264 @@
+/*
+ * Copyright 2011-2012 Calxeda, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/ctype.h>
+#include <linux/edac.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/of_platform.h>
+#include <linux/uaccess.h>
+
+#include "edac_core.h"
+#include "edac_module.h"
+
+/* DDR Ctrlr Error Registers */
+#define HB_DDR_ECC_OPT			0x128
+#define HB_DDR_ECC_U_ERR_ADDR		0x130
+#define HB_DDR_ECC_U_ERR_STAT		0x134
+#define HB_DDR_ECC_U_ERR_DATAL		0x138
+#define HB_DDR_ECC_U_ERR_DATAH		0x13c
+#define HB_DDR_ECC_C_ERR_ADDR		0x140
+#define HB_DDR_ECC_C_ERR_STAT		0x144
+#define HB_DDR_ECC_C_ERR_DATAL		0x148
+#define HB_DDR_ECC_C_ERR_DATAH		0x14c
+#define HB_DDR_ECC_INT_STATUS		0x180
+#define HB_DDR_ECC_INT_ACK		0x184
+#define HB_DDR_ECC_U_ERR_ID		0x424
+#define HB_DDR_ECC_C_ERR_ID		0x428
+
+#define HB_DDR_ECC_INT_STAT_CE		0x8
+#define HB_DDR_ECC_INT_STAT_DOUBLE_CE	0x10
+#define HB_DDR_ECC_INT_STAT_UE		0x20
+#define HB_DDR_ECC_INT_STAT_DOUBLE_UE	0x40
+
+#define HB_DDR_ECC_OPT_MODE_MASK	0x3
+#define HB_DDR_ECC_OPT_FWC		0x100
+#define HB_DDR_ECC_OPT_XOR_SHIFT	16
+
+struct hb_mc_drvdata {
+	void __iomem *mc_vbase;
+};
+
+static irqreturn_t highbank_mc_err_handler(int irq, void *dev_id)
+{
+	struct mem_ctl_info *mci = dev_id;
+	struct hb_mc_drvdata *drvdata = mci->pvt_info;
+	u32 status, err_addr;
+
+	/* Read the interrupt status register */
+	status = readl(drvdata->mc_vbase + HB_DDR_ECC_INT_STATUS);
+
+	if (status & HB_DDR_ECC_INT_STAT_UE) {
+		err_addr = readl(drvdata->mc_vbase + HB_DDR_ECC_U_ERR_ADDR);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
+				     err_addr >> PAGE_SHIFT,
+				     err_addr & ~PAGE_MASK, 0,
+				     0, 0, -1,
+				     mci->ctl_name, "");
+	}
+	if (status & HB_DDR_ECC_INT_STAT_CE) {
+		u32 syndrome = readl(drvdata->mc_vbase + HB_DDR_ECC_C_ERR_STAT);
+		syndrome = (syndrome >> 8) & 0xff;
+		err_addr = readl(drvdata->mc_vbase + HB_DDR_ECC_C_ERR_ADDR);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
+				     err_addr >> PAGE_SHIFT,
+				     err_addr & ~PAGE_MASK, syndrome,
+				     0, 0, -1,
+				     mci->ctl_name, "");
+	}
+
+	/* clear the error, clears the interrupt */
+	writel(status, drvdata->mc_vbase + HB_DDR_ECC_INT_ACK);
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_EDAC_DEBUG
+static ssize_t highbank_mc_err_inject_write(struct file *file,
+				      const char __user *data,
+				      size_t count, loff_t *ppos)
+{
+	struct mem_ctl_info *mci = file->private_data;
+	struct hb_mc_drvdata *pdata = mci->pvt_info;
+	char buf[32];
+	size_t buf_size;
+	u32 reg;
+	u8 synd;
+
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, data, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	if (!kstrtou8(buf, 16, &synd)) {
+		reg = readl(pdata->mc_vbase + HB_DDR_ECC_OPT);
+		reg &= HB_DDR_ECC_OPT_MODE_MASK;
+		reg |= (synd << HB_DDR_ECC_OPT_XOR_SHIFT) | HB_DDR_ECC_OPT_FWC;
+		writel(reg, pdata->mc_vbase + HB_DDR_ECC_OPT);
+	}
+
+	return count;
+}
+
+static int debugfs_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static const struct file_operations highbank_mc_debug_inject_fops = {
+	.open = debugfs_open,
+	.write = highbank_mc_err_inject_write,
+	.llseek = generic_file_llseek,
+};
+
+static void __devinit highbank_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
+{
+	if (mci->debugfs)
+		debugfs_create_file("inject_ctrl", S_IWUSR, mci->debugfs, mci,
+				    &highbank_mc_debug_inject_fops);
+;
+}
+#else
+static void __devinit highbank_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
+{}
+#endif
+
+static int __devinit highbank_mc_probe(struct platform_device *pdev)
+{
+	struct edac_mc_layer layers[2];
+	struct mem_ctl_info *mci;
+	struct hb_mc_drvdata *drvdata;
+	struct dimm_info *dimm;
+	struct resource *r;
+	u32 control;
+	int irq;
+	int res = 0;
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 1;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers,
+			    sizeof(struct hb_mc_drvdata));
+	if (!mci)
+		return -ENOMEM;
+
+	mci->pdev = &pdev->dev;
+	drvdata = mci->pvt_info;
+	platform_set_drvdata(pdev, mci);
+
+	if (!devres_open_group(&pdev->dev, NULL, GFP_KERNEL))
+		return -ENOMEM;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "Unable to get mem resource\n");
+		res = -ENODEV;
+		goto err;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, r->start,
+				     resource_size(r), dev_name(&pdev->dev))) {
+		dev_err(&pdev->dev, "Error while requesting mem region\n");
+		res = -EBUSY;
+		goto err;
+	}
+
+	drvdata->mc_vbase = devm_ioremap(&pdev->dev,
+					  r->start, resource_size(r));
+	if (!drvdata->mc_vbase) {
+		dev_err(&pdev->dev, "Unable to map regs\n");
+		res = -ENOMEM;
+		goto err;
+	}
+
+	control = readl(drvdata->mc_vbase + HB_DDR_ECC_OPT) & 0x3;
+	if (!control || (control == 0x2)) {
+		dev_err(&pdev->dev, "No ECC present, or ECC disabled\n");
+		res = -ENODEV;
+		goto err;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	res = devm_request_irq(&pdev->dev, irq, highbank_mc_err_handler,
+			       0, dev_name(&pdev->dev), mci);
+	if (res < 0) {
+		dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
+		goto err;
+	}
+
+	mci->mtype_cap = MEM_FLAG_DDR3;
+	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
+	mci->edac_cap = EDAC_FLAG_SECDED;
+	mci->mod_name = dev_name(&pdev->dev);
+	mci->mod_ver = "1";
+	mci->ctl_name = dev_name(&pdev->dev);
+	mci->scrub_mode = SCRUB_SW_SRC;
+
+	/* Only a single 4GB DIMM is supported */
+	dimm = *mci->dimms;
+	dimm->nr_pages = (~0UL >> PAGE_SHIFT) + 1;
+	dimm->grain = 8;
+	dimm->dtype = DEV_X8;
+	dimm->mtype = MEM_DDR3;
+	dimm->edac_mode = EDAC_SECDED;
+
+	res = edac_mc_add_mc(mci);
+	if (res < 0)
+		goto err;
+
+	highbank_mc_create_debugfs_nodes(mci);
+
+	devres_close_group(&pdev->dev, NULL);
+	return 0;
+err:
+	devres_release_group(&pdev->dev, NULL);
+	edac_mc_free(mci);
+	return res;
+}
+
+static int highbank_mc_remove(struct platform_device *pdev)
+{
+	struct mem_ctl_info *mci = platform_get_drvdata(pdev);
+
+	edac_mc_del_mc(&pdev->dev);
+	edac_mc_free(mci);
+	return 0;
+}
+
+static const struct of_device_id hb_ddr_ctrl_of_match[] = {
+	{ .compatible = "calxeda,hb-ddr-ctrl", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, hb_ddr_ctrl_of_match);
+
+static struct platform_driver highbank_mc_edac_driver = {
+	.probe = highbank_mc_probe,
+	.remove = highbank_mc_remove,
+	.driver = {
+		.name = "hb_mc_edac",
+		.of_match_table = hb_ddr_ctrl_of_match,
+	},
+};
+
+module_platform_driver(highbank_mc_edac_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Calxeda, Inc.");
+MODULE_DESCRIPTION("EDAC Driver for Calxeda Highbank");
diff --git a/drivers/edac/i3000_edac.c b/drivers/edac/i3000_edac.c
index 8ad1744faac..d3d19cc4e9a 100644
--- a/drivers/edac/i3000_edac.c
+++ b/drivers/edac/i3000_edac.c
@@ -194,7 +194,7 @@ static void i3000_get_error_info(struct mem_ctl_info *mci,
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * This is a mess because there is no atomic way to read all the
@@ -236,7 +236,7 @@ static int i3000_process_error_info(struct mem_ctl_info *mci,
 	int row, multi_chan, channel;
 	unsigned long pfn, offset;
 
-	multi_chan = mci->csrows[0].nr_channels - 1;
+	multi_chan = mci->csrows[0]->nr_channels - 1;
 
 	if (!(info->errsts & I3000_ERRSTS_BITS))
 		return 0;
@@ -245,9 +245,9 @@ static int i3000_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & I3000_ERRSTS_BITS) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
 				     -1, -1, -1,
-				     "UE overwrote CE", "", NULL);
+				     "UE overwrote CE", "");
 		info->errsts = info->errsts2;
 	}
 
@@ -258,15 +258,15 @@ static int i3000_process_error_info(struct mem_ctl_info *mci,
 	row = edac_mc_find_csrow_by_page(mci, pfn);
 
 	if (info->errsts & I3000_ERRSTS_UE)
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     pfn, offset, 0,
 				     row, -1, -1,
-				     "i3000 UE", "", NULL);
+				     "i3000 UE", "");
 	else
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     pfn, offset, info->derrsyn,
 				     row, multi_chan ? channel : 0, -1,
-				     "i3000 CE", "", NULL);
+				     "i3000 CE", "");
 
 	return 1;
 }
@@ -275,7 +275,7 @@ static void i3000_check(struct mem_ctl_info *mci)
 {
 	struct i3000_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	i3000_get_error_info(mci, &info);
 	i3000_process_error_info(mci, &info, 1);
 }
@@ -322,7 +322,7 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 	unsigned long mchbar;
 	void __iomem *window;
 
-	debugf0("MC: %s()\n", __func__);
+	edac_dbg(0, "MC:\n");
 
 	pci_read_config_dword(pdev, I3000_MCHBAR, (u32 *) & mchbar);
 	mchbar &= I3000_MCHBAR_MASK;
@@ -366,9 +366,9 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 	if (!mci)
 		return -ENOMEM;
 
-	debugf3("MC: %s(): init mci\n", __func__);
+	edac_dbg(3, "MC: init mci\n");
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR2;
 
 	mci->edac_ctl_cap = EDAC_FLAG_SECDED;
@@ -393,14 +393,13 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 	for (last_cumul_size = i = 0; i < mci->nr_csrows; i++) {
 		u8 value;
 		u32 cumul_size;
-		struct csrow_info *csrow = &mci->csrows[i];
+		struct csrow_info *csrow = mci->csrows[i];
 
 		value = drb[i];
 		cumul_size = value << (I3000_DRB_SHIFT - PAGE_SHIFT);
 		if (interleaved)
 			cumul_size <<= 1;
-		debugf3("MC: %s(): (%d) cumul_size 0x%x\n",
-			__func__, i, cumul_size);
+		edac_dbg(3, "MC: (%d) cumul_size 0x%x\n", i, cumul_size);
 		if (cumul_size == last_cumul_size)
 			continue;
 
@@ -410,7 +409,7 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 		last_cumul_size = cumul_size;
 
 		for (j = 0; j < nr_channels; j++) {
-			struct dimm_info *dimm = csrow->channels[j].dimm;
+			struct dimm_info *dimm = csrow->channels[j]->dimm;
 
 			dimm->nr_pages = nr_pages / nr_channels;
 			dimm->grain = I3000_DEAP_GRAIN;
@@ -429,7 +428,7 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 
 	rc = -ENODEV;
 	if (edac_mc_add_mc(mci)) {
-		debugf3("MC: %s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "MC: failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
@@ -445,7 +444,7 @@ static int i3000_probe1(struct pci_dev *pdev, int dev_idx)
 	}
 
 	/* get this far and it's successful */
-	debugf3("MC: %s(): success\n", __func__);
+	edac_dbg(3, "MC: success\n");
 	return 0;
 
 fail:
@@ -461,7 +460,7 @@ static int __devinit i3000_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("MC: %s()\n", __func__);
+	edac_dbg(0, "MC:\n");
 
 	if (pci_enable_device(pdev) < 0)
 		return -EIO;
@@ -477,7 +476,7 @@ static void __devexit i3000_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (i3000_pci)
 		edac_pci_release_generic_ctl(i3000_pci);
@@ -511,7 +510,7 @@ static int __init i3000_init(void)
 {
 	int pci_rc;
 
-	debugf3("MC: %s()\n", __func__);
+	edac_dbg(3, "MC:\n");
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -525,14 +524,14 @@ static int __init i3000_init(void)
 		mci_pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
 					PCI_DEVICE_ID_INTEL_3000_HB, NULL);
 		if (!mci_pdev) {
-			debugf0("i3000 pci_get_device fail\n");
+			edac_dbg(0, "i3000 pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
 
 		pci_rc = i3000_init_one(mci_pdev, i3000_pci_tbl);
 		if (pci_rc < 0) {
-			debugf0("i3000 init fail\n");
+			edac_dbg(0, "i3000 init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -552,7 +551,7 @@ fail0:
 
 static void __exit i3000_exit(void)
 {
-	debugf3("MC: %s()\n", __func__);
+	edac_dbg(3, "MC:\n");
 
 	pci_unregister_driver(&i3000_driver);
 	if (!i3000_registered) {
diff --git a/drivers/edac/i3200_edac.c b/drivers/edac/i3200_edac.c
index bbe43ef7182..47180a08eda 100644
--- a/drivers/edac/i3200_edac.c
+++ b/drivers/edac/i3200_edac.c
@@ -110,10 +110,10 @@ static int how_many_channels(struct pci_dev *pdev)
 
 	pci_read_config_byte(pdev, I3200_CAPID0 + 8, &capid0_8b);
 	if (capid0_8b & 0x20) { /* check DCD: Dual Channel Disable */
-		debugf0("In single channel mode.\n");
+		edac_dbg(0, "In single channel mode\n");
 		return 1;
 	} else {
-		debugf0("In dual channel mode.\n");
+		edac_dbg(0, "In dual channel mode\n");
 		return 2;
 	}
 }
@@ -159,7 +159,7 @@ static void i3200_clear_error_info(struct mem_ctl_info *mci)
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * Clear any error bits.
@@ -176,7 +176,7 @@ static void i3200_get_and_clear_error_info(struct mem_ctl_info *mci,
 	struct i3200_priv *priv = mci->pvt_info;
 	void __iomem *window = priv->window;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * This is a mess because there is no atomic way to read all the
@@ -218,25 +218,25 @@ static void i3200_process_error_info(struct mem_ctl_info *mci,
 		return;
 
 	if ((info->errsts ^ info->errsts2) & I3200_ERRSTS_BITS) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
-				     -1, -1, -1, "UE overwrote CE", "", NULL);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
+				     -1, -1, -1, "UE overwrote CE", "");
 		info->errsts = info->errsts2;
 	}
 
 	for (channel = 0; channel < nr_channels; channel++) {
 		log = info->eccerrlog[channel];
 		if (log & I3200_ECCERRLOG_UE) {
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					     0, 0, 0,
 					     eccerrlog_row(channel, log),
 					     -1, -1,
-					     "i3000 UE", "", NULL);
+					     "i3000 UE", "");
 		} else if (log & I3200_ECCERRLOG_CE) {
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					     0, 0, eccerrlog_syndrome(log),
 					     eccerrlog_row(channel, log),
 					     -1, -1,
-					     "i3000 UE", "", NULL);
+					     "i3000 UE", "");
 		}
 	}
 }
@@ -245,7 +245,7 @@ static void i3200_check(struct mem_ctl_info *mci)
 {
 	struct i3200_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	i3200_get_and_clear_error_info(mci, &info);
 	i3200_process_error_info(mci, &info);
 }
@@ -332,7 +332,7 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 	void __iomem *window;
 	struct i3200_priv *priv;
 
-	debugf0("MC: %s()\n", __func__);
+	edac_dbg(0, "MC:\n");
 
 	window = i3200_map_mchbar(pdev);
 	if (!window)
@@ -352,9 +352,9 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 	if (!mci)
 		return -ENOMEM;
 
-	debugf3("MC: %s(): init mci\n", __func__);
+	edac_dbg(3, "MC: init mci\n");
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR2;
 
 	mci->edac_ctl_cap = EDAC_FLAG_SECDED;
@@ -379,7 +379,7 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 	 */
 	for (i = 0; i < mci->nr_csrows; i++) {
 		unsigned long nr_pages;
-		struct csrow_info *csrow = &mci->csrows[i];
+		struct csrow_info *csrow = mci->csrows[i];
 
 		nr_pages = drb_to_nr_pages(drbs, stacked,
 			i / I3200_RANKS_PER_CHANNEL,
@@ -389,7 +389,7 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 			continue;
 
 		for (j = 0; j < nr_channels; j++) {
-			struct dimm_info *dimm = csrow->channels[j].dimm;
+			struct dimm_info *dimm = csrow->channels[j]->dimm;
 
 			dimm->nr_pages = nr_pages / nr_channels;
 			dimm->grain = nr_pages << PAGE_SHIFT;
@@ -403,12 +403,12 @@ static int i3200_probe1(struct pci_dev *pdev, int dev_idx)
 
 	rc = -ENODEV;
 	if (edac_mc_add_mc(mci)) {
-		debugf3("MC: %s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "MC: failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
 	/* get this far and it's successful */
-	debugf3("MC: %s(): success\n", __func__);
+	edac_dbg(3, "MC: success\n");
 	return 0;
 
 fail:
@@ -424,7 +424,7 @@ static int __devinit i3200_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("MC: %s()\n", __func__);
+	edac_dbg(0, "MC:\n");
 
 	if (pci_enable_device(pdev) < 0)
 		return -EIO;
@@ -441,7 +441,7 @@ static void __devexit i3200_remove_one(struct pci_dev *pdev)
 	struct mem_ctl_info *mci;
 	struct i3200_priv *priv;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	mci = edac_mc_del_mc(&pdev->dev);
 	if (!mci)
@@ -475,7 +475,7 @@ static int __init i3200_init(void)
 {
 	int pci_rc;
 
-	debugf3("MC: %s()\n", __func__);
+	edac_dbg(3, "MC:\n");
 
 	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
 	opstate_init();
@@ -489,14 +489,14 @@ static int __init i3200_init(void)
 		mci_pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
 				PCI_DEVICE_ID_INTEL_3200_HB, NULL);
 		if (!mci_pdev) {
-			debugf0("i3200 pci_get_device fail\n");
+			edac_dbg(0, "i3200 pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
 
 		pci_rc = i3200_init_one(mci_pdev, i3200_pci_tbl);
 		if (pci_rc < 0) {
-			debugf0("i3200 init fail\n");
+			edac_dbg(0, "i3200 init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -516,7 +516,7 @@ fail0:
 
 static void __exit i3200_exit(void)
 {
-	debugf3("MC: %s()\n", __func__);
+	edac_dbg(3, "MC:\n");
 
 	pci_unregister_driver(&i3200_driver);
 	if (!i3200_registered) {
diff --git a/drivers/edac/i5000_edac.c b/drivers/edac/i5000_edac.c
index 11ea835f155..39c63757c2a 100644
--- a/drivers/edac/i5000_edac.c
+++ b/drivers/edac/i5000_edac.c
@@ -273,7 +273,7 @@
 #define CHANNELS_PER_BRANCH	2
 #define MAX_BRANCHES		2
 
-/* Defines to extract the vaious fields from the
+/* Defines to extract the various fields from the
  *	MTRx - Memory Technology Registers
  */
 #define MTR_DIMMS_PRESENT(mtr)		((mtr) & (0x1 << 8))
@@ -287,22 +287,6 @@
 #define MTR_DIMM_COLS(mtr)		((mtr) & 0x3)
 #define MTR_DIMM_COLS_ADDR_BITS(mtr)	(MTR_DIMM_COLS(mtr) + 10)
 
-#ifdef CONFIG_EDAC_DEBUG
-static char *numrow_toString[] = {
-	"8,192 - 13 rows",
-	"16,384 - 14 rows",
-	"32,768 - 15 rows",
-	"reserved"
-};
-
-static char *numcol_toString[] = {
-	"1,024 - 10 columns",
-	"2,048 - 11 columns",
-	"4,096 - 12 columns",
-	"reserved"
-};
-#endif
-
 /* enables the report of miscellaneous messages as CE errors - default off */
 static int misc_messages;
 
@@ -344,7 +328,13 @@ struct i5000_pvt {
 	struct pci_dev *branch_1;	/* 22.0 */
 
 	u16 tolm;		/* top of low memory */
-	u64 ambase;		/* AMB BAR */
+	union {
+		u64 ambase;		/* AMB BAR */
+		struct {
+			u32 ambase_bottom;
+			u32 ambase_top;
+		} u __packed;
+	};
 
 	u16 mir0, mir1, mir2;
 
@@ -494,10 +484,9 @@ static void i5000_process_fatal_error_info(struct mem_ctl_info *mci,
 	ras = NREC_RAS(info->nrecmemb);
 	cas = NREC_CAS(info->nrecmemb);
 
-	debugf0("\t\tCSROW= %d  Channel= %d "
-		"(DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
-		rank, channel, bank,
-		rdwr ? "Write" : "Read", ras, cas);
+	edac_dbg(0, "\t\tCSROW= %d  Channel= %d (DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+		 rank, channel, bank,
+		 rdwr ? "Write" : "Read", ras, cas);
 
 	/* Only 1 bit will be on */
 	switch (allErrors) {
@@ -536,10 +525,10 @@ static void i5000_process_fatal_error_info(struct mem_ctl_info *mci,
 		 bank, ras, cas, allErrors, specific);
 
 	/* Call the helper to output message */
-	edac_mc_handle_error(HW_EVENT_ERR_FATAL, mci, 0, 0, 0,
+	edac_mc_handle_error(HW_EVENT_ERR_FATAL, mci, 1, 0, 0, 0,
 			     channel >> 1, channel & 1, rank,
 			     rdwr ? "Write error" : "Read error",
-			     msg, NULL);
+			     msg);
 }
 
 /*
@@ -574,7 +563,7 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 	/* ONLY ONE of the possible error bits will be set, as per the docs */
 	ue_errors = allErrors & FERR_NF_UNCORRECTABLE;
 	if (ue_errors) {
-		debugf0("\tUncorrected bits= 0x%x\n", ue_errors);
+		edac_dbg(0, "\tUncorrected bits= 0x%x\n", ue_errors);
 
 		branch = EXTRACT_FBDCHAN_INDX(info->ferr_nf_fbd);
 
@@ -590,11 +579,9 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 		ras = NREC_RAS(info->nrecmemb);
 		cas = NREC_CAS(info->nrecmemb);
 
-		debugf0
-			("\t\tCSROW= %d  Channels= %d,%d  (Branch= %d "
-			"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
-			rank, channel, channel + 1, branch >> 1, bank,
-			rdwr ? "Write" : "Read", ras, cas);
+		edac_dbg(0, "\t\tCSROW= %d  Channels= %d,%d  (Branch= %d DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+			 rank, channel, channel + 1, branch >> 1, bank,
+			 rdwr ? "Write" : "Read", ras, cas);
 
 		switch (ue_errors) {
 		case FERR_NF_M12ERR:
@@ -637,16 +624,16 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 			 rank, bank, ras, cas, ue_errors, specific);
 
 		/* Call the helper to output message */
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
 				channel >> 1, -1, rank,
 				rdwr ? "Write error" : "Read error",
-				msg, NULL);
+				msg);
 	}
 
 	/* Check correctable errors */
 	ce_errors = allErrors & FERR_NF_CORRECTABLE;
 	if (ce_errors) {
-		debugf0("\tCorrected bits= 0x%x\n", ce_errors);
+		edac_dbg(0, "\tCorrected bits= 0x%x\n", ce_errors);
 
 		branch = EXTRACT_FBDCHAN_INDX(info->ferr_nf_fbd);
 
@@ -664,10 +651,9 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 		ras = REC_RAS(info->recmemb);
 		cas = REC_CAS(info->recmemb);
 
-		debugf0("\t\tCSROW= %d Channel= %d  (Branch %d "
-			"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
-			rank, channel, branch >> 1, bank,
-			rdwr ? "Write" : "Read", ras, cas);
+		edac_dbg(0, "\t\tCSROW= %d Channel= %d  (Branch %d DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+			 rank, channel, branch >> 1, bank,
+			 rdwr ? "Write" : "Read", ras, cas);
 
 		switch (ce_errors) {
 		case FERR_NF_M17ERR:
@@ -692,10 +678,10 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 			 specific);
 
 		/* Call the helper to output message */
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1, 0, 0, 0,
 				channel >> 1, channel % 2, rank,
 				rdwr ? "Write error" : "Read error",
-				msg, NULL);
+				msg);
 	}
 
 	if (!misc_messages)
@@ -738,9 +724,9 @@ static void i5000_process_nonfatal_error_info(struct mem_ctl_info *mci,
 			 "Err=%#x (%s)", misc_errors, specific);
 
 		/* Call the helper to output message */
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1, 0, 0, 0,
 				branch >> 1, -1, -1,
-				"Misc error", msg, NULL);
+				"Misc error", msg);
 	}
 }
 
@@ -779,7 +765,7 @@ static void i5000_clear_error(struct mem_ctl_info *mci)
 static void i5000_check_error(struct mem_ctl_info *mci)
 {
 	struct i5000_error_info info;
-	debugf4("MC%d: %s: %s()\n", mci->mc_idx, __FILE__, __func__);
+	edac_dbg(4, "MC%d\n", mci->mc_idx);
 	i5000_get_error_info(mci, &info);
 	i5000_process_error_info(mci, &info, 1);
 }
@@ -850,15 +836,16 @@ static int i5000_get_devices(struct mem_ctl_info *mci, int dev_idx)
 
 	pvt->fsb_error_regs = pdev;
 
-	debugf1("System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->system_address),
-		pvt->system_address->vendor, pvt->system_address->device);
-	debugf1("Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->branchmap_werrors),
-		pvt->branchmap_werrors->vendor, pvt->branchmap_werrors->device);
-	debugf1("FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->fsb_error_regs),
-		pvt->fsb_error_regs->vendor, pvt->fsb_error_regs->device);
+	edac_dbg(1, "System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->system_address),
+		 pvt->system_address->vendor, pvt->system_address->device);
+	edac_dbg(1, "Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->branchmap_werrors),
+		 pvt->branchmap_werrors->vendor,
+		 pvt->branchmap_werrors->device);
+	edac_dbg(1, "FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->fsb_error_regs),
+		 pvt->fsb_error_regs->vendor, pvt->fsb_error_regs->device);
 
 	pdev = NULL;
 	pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
@@ -981,16 +968,25 @@ static void decode_mtr(int slot_row, u16 mtr)
 
 	ans = MTR_DIMMS_PRESENT(mtr);
 
-	debugf2("\tMTR%d=0x%x:  DIMMs are %s\n", slot_row, mtr,
-		ans ? "Present" : "NOT Present");
+	edac_dbg(2, "\tMTR%d=0x%x:  DIMMs are %sPresent\n",
+		 slot_row, mtr, ans ? "" : "NOT ");
 	if (!ans)
 		return;
 
-	debugf2("\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
-	debugf2("\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
-	debugf2("\t\tNUMRANK: %s\n", MTR_DIMM_RANK(mtr) ? "double" : "single");
-	debugf2("\t\tNUMROW: %s\n", numrow_toString[MTR_DIMM_ROWS(mtr)]);
-	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
+	edac_dbg(2, "\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
+	edac_dbg(2, "\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
+	edac_dbg(2, "\t\tNUMRANK: %s\n",
+		 MTR_DIMM_RANK(mtr) ? "double" : "single");
+	edac_dbg(2, "\t\tNUMROW: %s\n",
+		 MTR_DIMM_ROWS(mtr) == 0 ? "8,192 - 13 rows" :
+		 MTR_DIMM_ROWS(mtr) == 1 ? "16,384 - 14 rows" :
+		 MTR_DIMM_ROWS(mtr) == 2 ? "32,768 - 15 rows" :
+		 "reserved");
+	edac_dbg(2, "\t\tNUMCOL: %s\n",
+		 MTR_DIMM_COLS(mtr) == 0 ? "1,024 - 10 columns" :
+		 MTR_DIMM_COLS(mtr) == 1 ? "2,048 - 11 columns" :
+		 MTR_DIMM_COLS(mtr) == 2 ? "4,096 - 12 columns" :
+		 "reserved");
 }
 
 static void handle_channel(struct i5000_pvt *pvt, int slot, int channel,
@@ -1061,7 +1057,7 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 				"--------------------------------");
 			p += n;
 			space -= n;
-			debugf2("%s\n", mem_buffer);
+			edac_dbg(2, "%s\n", mem_buffer);
 			p = mem_buffer;
 			space = PAGE_SIZE;
 		}
@@ -1082,7 +1078,7 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 		}
 		p += n;
 		space -= n;
-		debugf2("%s\n", mem_buffer);
+		edac_dbg(2, "%s\n", mem_buffer);
 		p = mem_buffer;
 		space = PAGE_SIZE;
 	}
@@ -1092,7 +1088,7 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 		"--------------------------------");
 	p += n;
 	space -= n;
-	debugf2("%s\n", mem_buffer);
+	edac_dbg(2, "%s\n", mem_buffer);
 	p = mem_buffer;
 	space = PAGE_SIZE;
 
@@ -1105,7 +1101,7 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 		p += n;
 		space -= n;
 	}
-	debugf2("%s\n", mem_buffer);
+	edac_dbg(2, "%s\n", mem_buffer);
 	p = mem_buffer;
 	space = PAGE_SIZE;
 
@@ -1118,7 +1114,7 @@ static void calculate_dimm_size(struct i5000_pvt *pvt)
 	}
 
 	/* output the last message and free buffer */
-	debugf2("%s\n", mem_buffer);
+	edac_dbg(2, "%s\n", mem_buffer);
 	kfree(mem_buffer);
 }
 
@@ -1141,24 +1137,25 @@ static void i5000_get_mc_regs(struct mem_ctl_info *mci)
 	pvt = mci->pvt_info;
 
 	pci_read_config_dword(pvt->system_address, AMBASE,
-			(u32 *) & pvt->ambase);
+			&pvt->u.ambase_bottom);
 	pci_read_config_dword(pvt->system_address, AMBASE + sizeof(u32),
-			((u32 *) & pvt->ambase) + sizeof(u32));
+			&pvt->u.ambase_top);
 
 	maxdimmperch = pvt->maxdimmperch;
 	maxch = pvt->maxch;
 
-	debugf2("AMBASE= 0x%lx  MAXCH= %d  MAX-DIMM-Per-CH= %d\n",
-		(long unsigned int)pvt->ambase, pvt->maxch, pvt->maxdimmperch);
+	edac_dbg(2, "AMBASE= 0x%lx  MAXCH= %d  MAX-DIMM-Per-CH= %d\n",
+		 (long unsigned int)pvt->ambase, pvt->maxch, pvt->maxdimmperch);
 
 	/* Get the Branch Map regs */
 	pci_read_config_word(pvt->branchmap_werrors, TOLM, &pvt->tolm);
 	pvt->tolm >>= 12;
-	debugf2("\nTOLM (number of 256M regions) =%u (0x%x)\n", pvt->tolm,
-		pvt->tolm);
+	edac_dbg(2, "TOLM (number of 256M regions) =%u (0x%x)\n",
+		 pvt->tolm, pvt->tolm);
 
 	actual_tolm = pvt->tolm << 28;
-	debugf2("Actual TOLM byte addr=%u (0x%x)\n", actual_tolm, actual_tolm);
+	edac_dbg(2, "Actual TOLM byte addr=%u (0x%x)\n",
+		 actual_tolm, actual_tolm);
 
 	pci_read_config_word(pvt->branchmap_werrors, MIR0, &pvt->mir0);
 	pci_read_config_word(pvt->branchmap_werrors, MIR1, &pvt->mir1);
@@ -1168,15 +1165,18 @@ static void i5000_get_mc_regs(struct mem_ctl_info *mci)
 	limit = (pvt->mir0 >> 4) & 0x0FFF;
 	way0 = pvt->mir0 & 0x1;
 	way1 = pvt->mir0 & 0x2;
-	debugf2("MIR0: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	edac_dbg(2, "MIR0: limit= 0x%x  WAY1= %u  WAY0= %x\n",
+		 limit, way1, way0);
 	limit = (pvt->mir1 >> 4) & 0x0FFF;
 	way0 = pvt->mir1 & 0x1;
 	way1 = pvt->mir1 & 0x2;
-	debugf2("MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	edac_dbg(2, "MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n",
+		 limit, way1, way0);
 	limit = (pvt->mir2 >> 4) & 0x0FFF;
 	way0 = pvt->mir2 & 0x1;
 	way1 = pvt->mir2 & 0x2;
-	debugf2("MIR2: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	edac_dbg(2, "MIR2: limit= 0x%x  WAY1= %u  WAY0= %x\n",
+		 limit, way1, way0);
 
 	/* Get the MTR[0-3] regs */
 	for (slot_row = 0; slot_row < NUM_MTRS; slot_row++) {
@@ -1185,31 +1185,31 @@ static void i5000_get_mc_regs(struct mem_ctl_info *mci)
 		pci_read_config_word(pvt->branch_0, where,
 				&pvt->b0_mtr[slot_row]);
 
-		debugf2("MTR%d where=0x%x B0 value=0x%x\n", slot_row, where,
-			pvt->b0_mtr[slot_row]);
+		edac_dbg(2, "MTR%d where=0x%x B0 value=0x%x\n",
+			 slot_row, where, pvt->b0_mtr[slot_row]);
 
 		if (pvt->maxch >= CHANNELS_PER_BRANCH) {
 			pci_read_config_word(pvt->branch_1, where,
 					&pvt->b1_mtr[slot_row]);
-			debugf2("MTR%d where=0x%x B1 value=0x%x\n", slot_row,
-				where, pvt->b1_mtr[slot_row]);
+			edac_dbg(2, "MTR%d where=0x%x B1 value=0x%x\n",
+				 slot_row, where, pvt->b1_mtr[slot_row]);
 		} else {
 			pvt->b1_mtr[slot_row] = 0;
 		}
 	}
 
 	/* Read and dump branch 0's MTRs */
-	debugf2("\nMemory Technology Registers:\n");
-	debugf2("   Branch 0:\n");
+	edac_dbg(2, "Memory Technology Registers:\n");
+	edac_dbg(2, "   Branch 0:\n");
 	for (slot_row = 0; slot_row < NUM_MTRS; slot_row++) {
 		decode_mtr(slot_row, pvt->b0_mtr[slot_row]);
 	}
 	pci_read_config_word(pvt->branch_0, AMB_PRESENT_0,
 			&pvt->b0_ambpresent0);
-	debugf2("\t\tAMB-Branch 0-present0 0x%x:\n", pvt->b0_ambpresent0);
+	edac_dbg(2, "\t\tAMB-Branch 0-present0 0x%x:\n", pvt->b0_ambpresent0);
 	pci_read_config_word(pvt->branch_0, AMB_PRESENT_1,
 			&pvt->b0_ambpresent1);
-	debugf2("\t\tAMB-Branch 0-present1 0x%x:\n", pvt->b0_ambpresent1);
+	edac_dbg(2, "\t\tAMB-Branch 0-present1 0x%x:\n", pvt->b0_ambpresent1);
 
 	/* Only if we have 2 branchs (4 channels) */
 	if (pvt->maxch < CHANNELS_PER_BRANCH) {
@@ -1217,18 +1217,18 @@ static void i5000_get_mc_regs(struct mem_ctl_info *mci)
 		pvt->b1_ambpresent1 = 0;
 	} else {
 		/* Read and dump  branch 1's MTRs */
-		debugf2("   Branch 1:\n");
+		edac_dbg(2, "   Branch 1:\n");
 		for (slot_row = 0; slot_row < NUM_MTRS; slot_row++) {
 			decode_mtr(slot_row, pvt->b1_mtr[slot_row]);
 		}
 		pci_read_config_word(pvt->branch_1, AMB_PRESENT_0,
 				&pvt->b1_ambpresent0);
-		debugf2("\t\tAMB-Branch 1-present0 0x%x:\n",
-			pvt->b1_ambpresent0);
+		edac_dbg(2, "\t\tAMB-Branch 1-present0 0x%x:\n",
+			 pvt->b1_ambpresent0);
 		pci_read_config_word(pvt->branch_1, AMB_PRESENT_1,
 				&pvt->b1_ambpresent1);
-		debugf2("\t\tAMB-Branch 1-present1 0x%x:\n",
-			pvt->b1_ambpresent1);
+		edac_dbg(2, "\t\tAMB-Branch 1-present1 0x%x:\n",
+			 pvt->b1_ambpresent1);
 	}
 
 	/* Go and determine the size of each DIMM and place in an
@@ -1363,10 +1363,9 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 	int num_channels;
 	int num_dimms_per_channel;
 
-	debugf0("MC: %s: %s(), pdev bus %u dev=0x%x fn=0x%x\n",
-		__FILE__, __func__,
-		pdev->bus->number,
-		PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+	edac_dbg(0, "MC: pdev bus %u dev=0x%x fn=0x%x\n",
+		 pdev->bus->number,
+		 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 
 	/* We only are looking for func 0 of the set */
 	if (PCI_FUNC(pdev->devfn) != 0)
@@ -1388,8 +1387,8 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 	i5000_get_dimm_and_channel_counts(pdev, &num_dimms_per_channel,
 					&num_channels);
 
-	debugf0("MC: %s(): Number of Branches=2 Channels= %d  DIMMS= %d\n",
-		__func__, num_channels, num_dimms_per_channel);
+	edac_dbg(0, "MC: Number of Branches=2 Channels= %d  DIMMS= %d\n",
+		 num_channels, num_dimms_per_channel);
 
 	/* allocate a new MC control structure */
 
@@ -1406,10 +1405,9 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	kobject_get(&mci->edac_mci_kobj);
-	debugf0("MC: %s: %s(): mci = %p\n", __FILE__, __func__, mci);
+	edac_dbg(0, "MC: mci = %p\n", mci);
 
-	mci->dev = &pdev->dev;	/* record ptr  to the generic device */
+	mci->pdev = &pdev->dev;	/* record ptr  to the generic device */
 
 	pvt = mci->pvt_info;
 	pvt->system_address = pdev;	/* Record this device in our private */
@@ -1439,19 +1437,16 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 	/* initialize the MC control structure 'csrows' table
 	 * with the mapping and control information */
 	if (i5000_init_csrows(mci)) {
-		debugf0("MC: Setting mci->edac_cap to EDAC_FLAG_NONE\n"
-			"    because i5000_init_csrows() returned nonzero "
-			"value\n");
+		edac_dbg(0, "MC: Setting mci->edac_cap to EDAC_FLAG_NONE because i5000_init_csrows() returned nonzero value\n");
 		mci->edac_cap = EDAC_FLAG_NONE;	/* no csrows found */
 	} else {
-		debugf1("MC: Enable error reporting now\n");
+		edac_dbg(1, "MC: Enable error reporting now\n");
 		i5000_enable_error_reporting(mci);
 	}
 
 	/* add this new MC control structure to EDAC's list of MCs */
 	if (edac_mc_add_mc(mci)) {
-		debugf0("MC: %s: %s(): failed edac_mc_add_mc()\n",
-			__FILE__, __func__);
+		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		/* FIXME: perhaps some code should go here that disables error
 		 * reporting if we just enabled it
 		 */
@@ -1479,7 +1474,6 @@ fail1:
 	i5000_put_devices(mci);
 
 fail0:
-	kobject_put(&mci->edac_mci_kobj);
 	edac_mc_free(mci);
 	return -ENODEV;
 }
@@ -1496,7 +1490,7 @@ static int __devinit i5000_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "MC:\n");
 
 	/* wake up device */
 	rc = pci_enable_device(pdev);
@@ -1515,7 +1509,7 @@ static void __devexit i5000_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "\n");
 
 	if (i5000_pci)
 		edac_pci_release_generic_ctl(i5000_pci);
@@ -1525,7 +1519,6 @@ static void __devexit i5000_remove_one(struct pci_dev *pdev)
 
 	/* retrieve references to resources, and free those resources */
 	i5000_put_devices(mci);
-	kobject_put(&mci->edac_mci_kobj);
 	edac_mc_free(mci);
 }
 
@@ -1562,7 +1555,7 @@ static int __init i5000_init(void)
 {
 	int pci_rc;
 
-	debugf2("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(2, "MC:\n");
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -1578,7 +1571,7 @@ static int __init i5000_init(void)
  */
 static void __exit i5000_exit(void)
 {
-	debugf2("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(2, "MC:\n");
 	pci_unregister_driver(&i5000_driver);
 }
 
diff --git a/drivers/edac/i5100_edac.c b/drivers/edac/i5100_edac.c
index e9e7c2a29dc..c4b5e5f868e 100644
--- a/drivers/edac/i5100_edac.c
+++ b/drivers/edac/i5100_edac.c
@@ -431,10 +431,10 @@ static void i5100_handle_ce(struct mem_ctl_info *mci,
 		 "bank %u, cas %u, ras %u\n",
 		 bank, cas, ras);
 
-	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+	edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 			     0, 0, syndrome,
 			     chan, rank, -1,
-			     msg, detail, NULL);
+			     msg, detail);
 }
 
 static void i5100_handle_ue(struct mem_ctl_info *mci,
@@ -453,10 +453,10 @@ static void i5100_handle_ue(struct mem_ctl_info *mci,
 		 "bank %u, cas %u, ras %u\n",
 		 bank, cas, ras);
 
-	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+	edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 			     0, 0, syndrome,
 			     chan, rank, -1,
-			     msg, detail, NULL);
+			     msg, detail);
 }
 
 static void i5100_read_log(struct mem_ctl_info *mci, int chan,
@@ -859,8 +859,8 @@ static void __devinit i5100_init_csrows(struct mem_ctl_info *mci)
 				i5100_rank_to_slot(mci, chan, rank));
 		}
 
-		debugf2("dimm channel %d, rank %d, size %ld\n",
-			chan, rank, (long)PAGES_TO_MiB(npages));
+		edac_dbg(2, "dimm channel %d, rank %d, size %ld\n",
+			 chan, rank, (long)PAGES_TO_MiB(npages));
 	}
 }
 
@@ -943,7 +943,7 @@ static int __devinit i5100_init_one(struct pci_dev *pdev,
 		goto bail_disable_ch1;
 	}
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 
 	priv = mci->pvt_info;
 	priv->ranksperchan = ranksperch;
diff --git a/drivers/edac/i5400_edac.c b/drivers/edac/i5400_edac.c
index 6640c29e188..277246998b8 100644
--- a/drivers/edac/i5400_edac.c
+++ b/drivers/edac/i5400_edac.c
@@ -300,24 +300,6 @@ static inline int extract_fbdchan_indx(u32 x)
 	return (x>>28) & 0x3;
 }
 
-#ifdef CONFIG_EDAC_DEBUG
-/* MTR NUMROW */
-static const char *numrow_toString[] = {
-	"8,192 - 13 rows",
-	"16,384 - 14 rows",
-	"32,768 - 15 rows",
-	"65,536 - 16 rows"
-};
-
-/* MTR NUMCOL */
-static const char *numcol_toString[] = {
-	"1,024 - 10 columns",
-	"2,048 - 11 columns",
-	"4,096 - 12 columns",
-	"reserved"
-};
-#endif
-
 /* Device name and register DID (Device ID) */
 struct i5400_dev_info {
 	const char *ctl_name;	/* name for this device */
@@ -345,7 +327,13 @@ struct i5400_pvt {
 	struct pci_dev *branch_1;		/* 22.0 */
 
 	u16 tolm;				/* top of low memory */
-	u64 ambase;				/* AMB BAR */
+	union {
+		u64 ambase;				/* AMB BAR */
+		struct {
+			u32 ambase_bottom;
+			u32 ambase_top;
+		} u __packed;
+	};
 
 	u16 mir0, mir1;
 
@@ -560,10 +548,9 @@ static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
 	ras = nrec_ras(info);
 	cas = nrec_cas(info);
 
-	debugf0("\t\tDIMM= %d  Channels= %d,%d  (Branch= %d "
-		"DRAM Bank= %d Buffer ID = %d rdwr= %s ras= %d cas= %d)\n",
-		rank, channel, channel + 1, branch >> 1, bank,
-		buf_id, rdwr_str(rdwr), ras, cas);
+	edac_dbg(0, "\t\tDIMM= %d  Channels= %d,%d  (Branch= %d DRAM Bank= %d Buffer ID = %d rdwr= %s ras= %d cas= %d)\n",
+		 rank, channel, channel + 1, branch >> 1, bank,
+		 buf_id, rdwr_str(rdwr), ras, cas);
 
 	/* Only 1 bit will be on */
 	errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
@@ -573,10 +560,10 @@ static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
 		 "Bank=%d Buffer ID = %d RAS=%d CAS=%d Err=0x%lx (%s)",
 		 bank, buf_id, ras, cas, allErrors, error_name[errnum]);
 
-	edac_mc_handle_error(tp_event, mci, 0, 0, 0,
+	edac_mc_handle_error(tp_event, mci, 1, 0, 0, 0,
 			     branch >> 1, -1, rank,
 			     rdwr ? "Write error" : "Read error",
-			     msg, NULL);
+			     msg);
 }
 
 /*
@@ -613,7 +600,7 @@ static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
 
 	/* Correctable errors */
 	if (allErrors & ERROR_NF_CORRECTABLE) {
-		debugf0("\tCorrected bits= 0x%lx\n", allErrors);
+		edac_dbg(0, "\tCorrected bits= 0x%lx\n", allErrors);
 
 		branch = extract_fbdchan_indx(info->ferr_nf_fbd);
 
@@ -634,10 +621,9 @@ static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
 		/* Only 1 bit will be on */
 		errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
 
-		debugf0("\t\tDIMM= %d Channel= %d  (Branch %d "
-			"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
-			rank, channel, branch >> 1, bank,
-			rdwr_str(rdwr), ras, cas);
+		edac_dbg(0, "\t\tDIMM= %d Channel= %d  (Branch %d DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+			 rank, channel, branch >> 1, bank,
+			 rdwr_str(rdwr), ras, cas);
 
 		/* Form out message */
 		snprintf(msg, sizeof(msg),
@@ -646,10 +632,10 @@ static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
 			 branch >> 1, bank, rdwr_str(rdwr), ras, cas,
 			 allErrors, error_name[errnum]);
 
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1, 0, 0, 0,
 				     branch >> 1, channel % 2, rank,
 				     rdwr ? "Write error" : "Read error",
-				     msg, NULL);
+				     msg);
 
 		return;
 	}
@@ -700,7 +686,7 @@ static void i5400_clear_error(struct mem_ctl_info *mci)
 static void i5400_check_error(struct mem_ctl_info *mci)
 {
 	struct i5400_error_info info;
-	debugf4("MC%d: %s: %s()\n", mci->mc_idx, __FILE__, __func__);
+	edac_dbg(4, "MC%d\n", mci->mc_idx);
 	i5400_get_error_info(mci, &info);
 	i5400_process_error_info(mci, &info);
 }
@@ -786,15 +772,16 @@ static int i5400_get_devices(struct mem_ctl_info *mci, int dev_idx)
 	}
 	pvt->fsb_error_regs = pdev;
 
-	debugf1("System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->system_address),
-		pvt->system_address->vendor, pvt->system_address->device);
-	debugf1("Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->branchmap_werrors),
-		pvt->branchmap_werrors->vendor, pvt->branchmap_werrors->device);
-	debugf1("FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->fsb_error_regs),
-		pvt->fsb_error_regs->vendor, pvt->fsb_error_regs->device);
+	edac_dbg(1, "System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->system_address),
+		 pvt->system_address->vendor, pvt->system_address->device);
+	edac_dbg(1, "Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->branchmap_werrors),
+		 pvt->branchmap_werrors->vendor,
+		 pvt->branchmap_werrors->device);
+	edac_dbg(1, "FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->fsb_error_regs),
+		 pvt->fsb_error_regs->vendor, pvt->fsb_error_regs->device);
 
 	pvt->branch_0 = pci_get_device(PCI_VENDOR_ID_INTEL,
 				       PCI_DEVICE_ID_INTEL_5400_FBD0, NULL);
@@ -882,8 +869,8 @@ static int determine_mtr(struct i5400_pvt *pvt, int dimm, int channel)
 	n = dimm;
 
 	if (n >= DIMMS_PER_CHANNEL) {
-		debugf0("ERROR: trying to access an invalid dimm: %d\n",
-			dimm);
+		edac_dbg(0, "ERROR: trying to access an invalid dimm: %d\n",
+			 dimm);
 		return 0;
 	}
 
@@ -903,20 +890,29 @@ static void decode_mtr(int slot_row, u16 mtr)
 
 	ans = MTR_DIMMS_PRESENT(mtr);
 
-	debugf2("\tMTR%d=0x%x:  DIMMs are %s\n", slot_row, mtr,
-		ans ? "Present" : "NOT Present");
+	edac_dbg(2, "\tMTR%d=0x%x:  DIMMs are %sPresent\n",
+		 slot_row, mtr, ans ? "" : "NOT ");
 	if (!ans)
 		return;
 
-	debugf2("\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
-
-	debugf2("\t\tELECTRICAL THROTTLING is %s\n",
-		MTR_DIMMS_ETHROTTLE(mtr) ? "enabled" : "disabled");
-
-	debugf2("\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
-	debugf2("\t\tNUMRANK: %s\n", MTR_DIMM_RANK(mtr) ? "double" : "single");
-	debugf2("\t\tNUMROW: %s\n", numrow_toString[MTR_DIMM_ROWS(mtr)]);
-	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
+	edac_dbg(2, "\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
+
+	edac_dbg(2, "\t\tELECTRICAL THROTTLING is %s\n",
+		 MTR_DIMMS_ETHROTTLE(mtr) ? "enabled" : "disabled");
+
+	edac_dbg(2, "\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
+	edac_dbg(2, "\t\tNUMRANK: %s\n",
+		 MTR_DIMM_RANK(mtr) ? "double" : "single");
+	edac_dbg(2, "\t\tNUMROW: %s\n",
+		 MTR_DIMM_ROWS(mtr) == 0 ? "8,192 - 13 rows" :
+		 MTR_DIMM_ROWS(mtr) == 1 ? "16,384 - 14 rows" :
+		 MTR_DIMM_ROWS(mtr) == 2 ? "32,768 - 15 rows" :
+		 "65,536 - 16 rows");
+	edac_dbg(2, "\t\tNUMCOL: %s\n",
+		 MTR_DIMM_COLS(mtr) == 0 ? "1,024 - 10 columns" :
+		 MTR_DIMM_COLS(mtr) == 1 ? "2,048 - 11 columns" :
+		 MTR_DIMM_COLS(mtr) == 2 ? "4,096 - 12 columns" :
+		 "reserved");
 }
 
 static void handle_channel(struct i5400_pvt *pvt, int dimm, int channel,
@@ -989,7 +985,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 					"-------------------------------");
 			p += n;
 			space -= n;
-			debugf2("%s\n", mem_buffer);
+			edac_dbg(2, "%s\n", mem_buffer);
 			p = mem_buffer;
 			space = PAGE_SIZE;
 		}
@@ -1004,7 +1000,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 			p += n;
 			space -= n;
 		}
-		debugf2("%s\n", mem_buffer);
+		edac_dbg(2, "%s\n", mem_buffer);
 		p = mem_buffer;
 		space = PAGE_SIZE;
 	}
@@ -1014,7 +1010,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 			"-------------------------------");
 	p += n;
 	space -= n;
-	debugf2("%s\n", mem_buffer);
+	edac_dbg(2, "%s\n", mem_buffer);
 	p = mem_buffer;
 	space = PAGE_SIZE;
 
@@ -1029,7 +1025,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 	}
 
 	space -= n;
-	debugf2("%s\n", mem_buffer);
+	edac_dbg(2, "%s\n", mem_buffer);
 	p = mem_buffer;
 	space = PAGE_SIZE;
 
@@ -1042,7 +1038,7 @@ static void calculate_dimm_size(struct i5400_pvt *pvt)
 	}
 
 	/* output the last message and free buffer */
-	debugf2("%s\n", mem_buffer);
+	edac_dbg(2, "%s\n", mem_buffer);
 	kfree(mem_buffer);
 }
 
@@ -1065,25 +1061,25 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 	pvt = mci->pvt_info;
 
 	pci_read_config_dword(pvt->system_address, AMBASE,
-			(u32 *) &pvt->ambase);
+			&pvt->u.ambase_bottom);
 	pci_read_config_dword(pvt->system_address, AMBASE + sizeof(u32),
-			((u32 *) &pvt->ambase) + sizeof(u32));
+			&pvt->u.ambase_top);
 
 	maxdimmperch = pvt->maxdimmperch;
 	maxch = pvt->maxch;
 
-	debugf2("AMBASE= 0x%lx  MAXCH= %d  MAX-DIMM-Per-CH= %d\n",
-		(long unsigned int)pvt->ambase, pvt->maxch, pvt->maxdimmperch);
+	edac_dbg(2, "AMBASE= 0x%lx  MAXCH= %d  MAX-DIMM-Per-CH= %d\n",
+		 (long unsigned int)pvt->ambase, pvt->maxch, pvt->maxdimmperch);
 
 	/* Get the Branch Map regs */
 	pci_read_config_word(pvt->branchmap_werrors, TOLM, &pvt->tolm);
 	pvt->tolm >>= 12;
-	debugf2("\nTOLM (number of 256M regions) =%u (0x%x)\n", pvt->tolm,
-		pvt->tolm);
+	edac_dbg(2, "\nTOLM (number of 256M regions) =%u (0x%x)\n",
+		 pvt->tolm, pvt->tolm);
 
 	actual_tolm = (u32) ((1000l * pvt->tolm) >> (30 - 28));
-	debugf2("Actual TOLM byte addr=%u.%03u GB (0x%x)\n",
-		actual_tolm/1000, actual_tolm % 1000, pvt->tolm << 28);
+	edac_dbg(2, "Actual TOLM byte addr=%u.%03u GB (0x%x)\n",
+		 actual_tolm/1000, actual_tolm % 1000, pvt->tolm << 28);
 
 	pci_read_config_word(pvt->branchmap_werrors, MIR0, &pvt->mir0);
 	pci_read_config_word(pvt->branchmap_werrors, MIR1, &pvt->mir1);
@@ -1092,11 +1088,13 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 	limit = (pvt->mir0 >> 4) & 0x0fff;
 	way0 = pvt->mir0 & 0x1;
 	way1 = pvt->mir0 & 0x2;
-	debugf2("MIR0: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	edac_dbg(2, "MIR0: limit= 0x%x  WAY1= %u  WAY0= %x\n",
+		 limit, way1, way0);
 	limit = (pvt->mir1 >> 4) & 0xfff;
 	way0 = pvt->mir1 & 0x1;
 	way1 = pvt->mir1 & 0x2;
-	debugf2("MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	edac_dbg(2, "MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n",
+		 limit, way1, way0);
 
 	/* Get the set of MTR[0-3] regs by each branch */
 	for (slot_row = 0; slot_row < DIMMS_PER_CHANNEL; slot_row++) {
@@ -1106,8 +1104,8 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 		pci_read_config_word(pvt->branch_0, where,
 				&pvt->b0_mtr[slot_row]);
 
-		debugf2("MTR%d where=0x%x B0 value=0x%x\n", slot_row, where,
-			pvt->b0_mtr[slot_row]);
+		edac_dbg(2, "MTR%d where=0x%x B0 value=0x%x\n",
+			 slot_row, where, pvt->b0_mtr[slot_row]);
 
 		if (pvt->maxch < CHANNELS_PER_BRANCH) {
 			pvt->b1_mtr[slot_row] = 0;
@@ -1117,22 +1115,22 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 		/* Branch 1 set of MTR registers */
 		pci_read_config_word(pvt->branch_1, where,
 				&pvt->b1_mtr[slot_row]);
-		debugf2("MTR%d where=0x%x B1 value=0x%x\n", slot_row, where,
-			pvt->b1_mtr[slot_row]);
+		edac_dbg(2, "MTR%d where=0x%x B1 value=0x%x\n",
+			 slot_row, where, pvt->b1_mtr[slot_row]);
 	}
 
 	/* Read and dump branch 0's MTRs */
-	debugf2("\nMemory Technology Registers:\n");
-	debugf2("   Branch 0:\n");
+	edac_dbg(2, "Memory Technology Registers:\n");
+	edac_dbg(2, "   Branch 0:\n");
 	for (slot_row = 0; slot_row < DIMMS_PER_CHANNEL; slot_row++)
 		decode_mtr(slot_row, pvt->b0_mtr[slot_row]);
 
 	pci_read_config_word(pvt->branch_0, AMBPRESENT_0,
 			&pvt->b0_ambpresent0);
-	debugf2("\t\tAMB-Branch 0-present0 0x%x:\n", pvt->b0_ambpresent0);
+	edac_dbg(2, "\t\tAMB-Branch 0-present0 0x%x:\n", pvt->b0_ambpresent0);
 	pci_read_config_word(pvt->branch_0, AMBPRESENT_1,
 			&pvt->b0_ambpresent1);
-	debugf2("\t\tAMB-Branch 0-present1 0x%x:\n", pvt->b0_ambpresent1);
+	edac_dbg(2, "\t\tAMB-Branch 0-present1 0x%x:\n", pvt->b0_ambpresent1);
 
 	/* Only if we have 2 branchs (4 channels) */
 	if (pvt->maxch < CHANNELS_PER_BRANCH) {
@@ -1140,18 +1138,18 @@ static void i5400_get_mc_regs(struct mem_ctl_info *mci)
 		pvt->b1_ambpresent1 = 0;
 	} else {
 		/* Read and dump  branch 1's MTRs */
-		debugf2("   Branch 1:\n");
+		edac_dbg(2, "   Branch 1:\n");
 		for (slot_row = 0; slot_row < DIMMS_PER_CHANNEL; slot_row++)
 			decode_mtr(slot_row, pvt->b1_mtr[slot_row]);
 
 		pci_read_config_word(pvt->branch_1, AMBPRESENT_0,
 				&pvt->b1_ambpresent0);
-		debugf2("\t\tAMB-Branch 1-present0 0x%x:\n",
-			pvt->b1_ambpresent0);
+		edac_dbg(2, "\t\tAMB-Branch 1-present0 0x%x:\n",
+			 pvt->b1_ambpresent0);
 		pci_read_config_word(pvt->branch_1, AMBPRESENT_1,
 				&pvt->b1_ambpresent1);
-		debugf2("\t\tAMB-Branch 1-present1 0x%x:\n",
-			pvt->b1_ambpresent1);
+		edac_dbg(2, "\t\tAMB-Branch 1-present1 0x%x:\n",
+			 pvt->b1_ambpresent1);
 	}
 
 	/* Go and determine the size of each DIMM and place in an
@@ -1203,10 +1201,9 @@ static int i5400_init_dimms(struct mem_ctl_info *mci)
 
 			size_mb =  pvt->dimm_info[slot][channel].megabytes;
 
-			debugf2("%s: dimm%zd (branch %d channel %d slot %d): %d.%03d GB\n",
-				__func__, dimm - mci->dimms,
-				channel / 2, channel % 2, slot,
-				size_mb / 1000, size_mb % 1000);
+			edac_dbg(2, "dimm (branch %d channel %d slot %d): %d.%03d GB\n",
+				 channel / 2, channel % 2, slot,
+				 size_mb / 1000, size_mb % 1000);
 
 			dimm->nr_pages = size_mb << 8;
 			dimm->grain = 8;
@@ -1227,7 +1224,7 @@ static int i5400_init_dimms(struct mem_ctl_info *mci)
 	 * With such single-DIMM mode, the SDCC algorithm degrades to SECDEC+.
 	 */
 	if (ndimms == 1)
-		mci->dimms[0].edac_mode = EDAC_SECDED;
+		mci->dimms[0]->edac_mode = EDAC_SECDED;
 
 	return (ndimms == 0);
 }
@@ -1270,10 +1267,9 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 	if (dev_idx >= ARRAY_SIZE(i5400_devs))
 		return -EINVAL;
 
-	debugf0("MC: %s: %s(), pdev bus %u dev=0x%x fn=0x%x\n",
-		__FILE__, __func__,
-		pdev->bus->number,
-		PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+	edac_dbg(0, "MC: pdev bus %u dev=0x%x fn=0x%x\n",
+		 pdev->bus->number,
+		 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 
 	/* We only are looking for func 0 of the set */
 	if (PCI_FUNC(pdev->devfn) != 0)
@@ -1297,9 +1293,9 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf0("MC: %s: %s(): mci = %p\n", __FILE__, __func__, mci);
+	edac_dbg(0, "MC: mci = %p\n", mci);
 
-	mci->dev = &pdev->dev;	/* record ptr  to the generic device */
+	mci->pdev = &pdev->dev;	/* record ptr  to the generic device */
 
 	pvt = mci->pvt_info;
 	pvt->system_address = pdev;	/* Record this device in our private */
@@ -1329,19 +1325,16 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 	/* initialize the MC control structure 'dimms' table
 	 * with the mapping and control information */
 	if (i5400_init_dimms(mci)) {
-		debugf0("MC: Setting mci->edac_cap to EDAC_FLAG_NONE\n"
-			"    because i5400_init_dimms() returned nonzero "
-			"value\n");
+		edac_dbg(0, "MC: Setting mci->edac_cap to EDAC_FLAG_NONE because i5400_init_dimms() returned nonzero value\n");
 		mci->edac_cap = EDAC_FLAG_NONE;	/* no dimms found */
 	} else {
-		debugf1("MC: Enable error reporting now\n");
+		edac_dbg(1, "MC: Enable error reporting now\n");
 		i5400_enable_error_reporting(mci);
 	}
 
 	/* add this new MC control structure to EDAC's list of MCs */
 	if (edac_mc_add_mc(mci)) {
-		debugf0("MC: %s: %s(): failed edac_mc_add_mc()\n",
-			__FILE__, __func__);
+		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		/* FIXME: perhaps some code should go here that disables error
 		 * reporting if we just enabled it
 		 */
@@ -1385,7 +1378,7 @@ static int __devinit i5400_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "MC:\n");
 
 	/* wake up device */
 	rc = pci_enable_device(pdev);
@@ -1404,7 +1397,7 @@ static void __devexit i5400_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "\n");
 
 	if (i5400_pci)
 		edac_pci_release_generic_ctl(i5400_pci);
@@ -1450,7 +1443,7 @@ static int __init i5400_init(void)
 {
 	int pci_rc;
 
-	debugf2("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(2, "MC:\n");
 
 	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
 	opstate_init();
@@ -1466,7 +1459,7 @@ static int __init i5400_init(void)
  */
 static void __exit i5400_exit(void)
 {
-	debugf2("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(2, "MC:\n");
 	pci_unregister_driver(&i5400_driver);
 }
 
diff --git a/drivers/edac/i7300_edac.c b/drivers/edac/i7300_edac.c
index 97c22fd650e..a09d0667f72 100644
--- a/drivers/edac/i7300_edac.c
+++ b/drivers/edac/i7300_edac.c
@@ -182,24 +182,6 @@ static const u16 mtr_regs[MAX_SLOTS] = {
 #define MTR_DIMM_COLS(mtr)		((mtr) & 0x3)
 #define MTR_DIMM_COLS_ADDR_BITS(mtr)	(MTR_DIMM_COLS(mtr) + 10)
 
-#ifdef CONFIG_EDAC_DEBUG
-/* MTR NUMROW */
-static const char *numrow_toString[] = {
-	"8,192 - 13 rows",
-	"16,384 - 14 rows",
-	"32,768 - 15 rows",
-	"65,536 - 16 rows"
-};
-
-/* MTR NUMCOL */
-static const char *numcol_toString[] = {
-	"1,024 - 10 columns",
-	"2,048 - 11 columns",
-	"4,096 - 12 columns",
-	"reserved"
-};
-#endif
-
 /************************************************
  * i7300 Register definitions for error detection
  ************************************************/
@@ -467,10 +449,10 @@ static void i7300_process_fbd_error(struct mem_ctl_info *mci)
 			 "Bank=%d RAS=%d CAS=%d Err=0x%lx (%s))",
 			 bank, ras, cas, errors, specific);
 
-		edac_mc_handle_error(HW_EVENT_ERR_FATAL, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_FATAL, mci, 1, 0, 0, 0,
 				     branch, -1, rank,
 				     is_wr ? "Write error" : "Read error",
-				     pvt->tmp_prt_buffer, NULL);
+				     pvt->tmp_prt_buffer);
 
 	}
 
@@ -513,11 +495,11 @@ static void i7300_process_fbd_error(struct mem_ctl_info *mci)
 			 "DRAM-Bank=%d RAS=%d CAS=%d, Err=0x%lx (%s))",
 			 bank, ras, cas, errors, specific);
 
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1, 0, 0,
 				     syndrome,
 				     branch >> 1, channel % 2, rank,
 				     is_wr ? "Write error" : "Read error",
-				     pvt->tmp_prt_buffer, NULL);
+				     pvt->tmp_prt_buffer);
 	}
 	return;
 }
@@ -614,9 +596,8 @@ static int decode_mtr(struct i7300_pvt *pvt,
 	mtr = pvt->mtr[slot][branch];
 	ans = MTR_DIMMS_PRESENT(mtr) ? 1 : 0;
 
-	debugf2("\tMTR%d CH%d: DIMMs are %s (mtr)\n",
-		slot, channel,
-		ans ? "Present" : "NOT Present");
+	edac_dbg(2, "\tMTR%d CH%d: DIMMs are %sPresent (mtr)\n",
+		 slot, channel, ans ? "" : "NOT ");
 
 	/* Determine if there is a DIMM present in this DIMM slot */
 	if (!ans)
@@ -638,16 +619,25 @@ static int decode_mtr(struct i7300_pvt *pvt,
 
 	dinfo->megabytes = 1 << addrBits;
 
-	debugf2("\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
-
-	debugf2("\t\tELECTRICAL THROTTLING is %s\n",
-		MTR_DIMMS_ETHROTTLE(mtr) ? "enabled" : "disabled");
-
-	debugf2("\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
-	debugf2("\t\tNUMRANK: %s\n", MTR_DIMM_RANKS(mtr) ? "double" : "single");
-	debugf2("\t\tNUMROW: %s\n", numrow_toString[MTR_DIMM_ROWS(mtr)]);
-	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
-	debugf2("\t\tSIZE: %d MB\n", dinfo->megabytes);
+	edac_dbg(2, "\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
+
+	edac_dbg(2, "\t\tELECTRICAL THROTTLING is %s\n",
+		 MTR_DIMMS_ETHROTTLE(mtr) ? "enabled" : "disabled");
+
+	edac_dbg(2, "\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
+	edac_dbg(2, "\t\tNUMRANK: %s\n",
+		 MTR_DIMM_RANKS(mtr) ? "double" : "single");
+	edac_dbg(2, "\t\tNUMROW: %s\n",
+		 MTR_DIMM_ROWS(mtr) == 0 ? "8,192 - 13 rows" :
+		 MTR_DIMM_ROWS(mtr) == 1 ? "16,384 - 14 rows" :
+		 MTR_DIMM_ROWS(mtr) == 2 ? "32,768 - 15 rows" :
+		 "65,536 - 16 rows");
+	edac_dbg(2, "\t\tNUMCOL: %s\n",
+		 MTR_DIMM_COLS(mtr) == 0 ? "1,024 - 10 columns" :
+		 MTR_DIMM_COLS(mtr) == 1 ? "2,048 - 11 columns" :
+		 MTR_DIMM_COLS(mtr) == 2 ? "4,096 - 12 columns" :
+		 "reserved");
+	edac_dbg(2, "\t\tSIZE: %d MB\n", dinfo->megabytes);
 
 	/*
 	 * The type of error detection actually depends of the
@@ -663,9 +653,9 @@ static int decode_mtr(struct i7300_pvt *pvt,
 	dimm->mtype = MEM_FB_DDR2;
 	if (IS_SINGLE_MODE(pvt->mc_settings_a)) {
 		dimm->edac_mode = EDAC_SECDED;
-		debugf2("\t\tECC code is 8-byte-over-32-byte SECDED+ code\n");
+		edac_dbg(2, "\t\tECC code is 8-byte-over-32-byte SECDED+ code\n");
 	} else {
-		debugf2("\t\tECC code is on Lockstep mode\n");
+		edac_dbg(2, "\t\tECC code is on Lockstep mode\n");
 		if (MTR_DRAM_WIDTH(mtr) == 8)
 			dimm->edac_mode = EDAC_S8ECD8ED;
 		else
@@ -674,9 +664,9 @@ static int decode_mtr(struct i7300_pvt *pvt,
 
 	/* ask what device type on this row */
 	if (MTR_DRAM_WIDTH(mtr) == 8) {
-		debugf2("\t\tScrub algorithm for x8 is on %s mode\n",
-			IS_SCRBALGO_ENHANCED(pvt->mc_settings) ?
-					    "enhanced" : "normal");
+		edac_dbg(2, "\t\tScrub algorithm for x8 is on %s mode\n",
+			 IS_SCRBALGO_ENHANCED(pvt->mc_settings) ?
+			 "enhanced" : "normal");
 
 		dimm->dtype = DEV_X8;
 	} else
@@ -710,14 +700,14 @@ static void print_dimm_size(struct i7300_pvt *pvt)
 		p += n;
 		space -= n;
 	}
-	debugf2("%s\n", pvt->tmp_prt_buffer);
+	edac_dbg(2, "%s\n", pvt->tmp_prt_buffer);
 	p = pvt->tmp_prt_buffer;
 	space = PAGE_SIZE;
 	n = snprintf(p, space, "-------------------------------"
 			       "------------------------------");
 	p += n;
 	space -= n;
-	debugf2("%s\n", pvt->tmp_prt_buffer);
+	edac_dbg(2, "%s\n", pvt->tmp_prt_buffer);
 	p = pvt->tmp_prt_buffer;
 	space = PAGE_SIZE;
 
@@ -733,7 +723,7 @@ static void print_dimm_size(struct i7300_pvt *pvt)
 			space -= n;
 		}
 
-		debugf2("%s\n", pvt->tmp_prt_buffer);
+		edac_dbg(2, "%s\n", pvt->tmp_prt_buffer);
 		p = pvt->tmp_prt_buffer;
 		space = PAGE_SIZE;
 	}
@@ -742,7 +732,7 @@ static void print_dimm_size(struct i7300_pvt *pvt)
 			       "------------------------------");
 	p += n;
 	space -= n;
-	debugf2("%s\n", pvt->tmp_prt_buffer);
+	edac_dbg(2, "%s\n", pvt->tmp_prt_buffer);
 	p = pvt->tmp_prt_buffer;
 	space = PAGE_SIZE;
 #endif
@@ -765,7 +755,7 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 
 	pvt = mci->pvt_info;
 
-	debugf2("Memory Technology Registers:\n");
+	edac_dbg(2, "Memory Technology Registers:\n");
 
 	/* Get the AMB present registers for the four channels */
 	for (branch = 0; branch < MAX_BRANCHES; branch++) {
@@ -774,15 +764,15 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 		pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 				     AMBPRESENT_0,
 				&pvt->ambpresent[channel]);
-		debugf2("\t\tAMB-present CH%d = 0x%x:\n",
-			channel, pvt->ambpresent[channel]);
+		edac_dbg(2, "\t\tAMB-present CH%d = 0x%x:\n",
+			 channel, pvt->ambpresent[channel]);
 
 		channel = to_channel(1, branch);
 		pci_read_config_word(pvt->pci_dev_2x_0_fbd_branch[branch],
 				     AMBPRESENT_1,
 				&pvt->ambpresent[channel]);
-		debugf2("\t\tAMB-present CH%d = 0x%x:\n",
-			channel, pvt->ambpresent[channel]);
+		edac_dbg(2, "\t\tAMB-present CH%d = 0x%x:\n",
+			 channel, pvt->ambpresent[channel]);
 	}
 
 	/* Get the set of MTR[0-7] regs by each branch */
@@ -824,12 +814,11 @@ static int i7300_init_csrows(struct mem_ctl_info *mci)
 static void decode_mir(int mir_no, u16 mir[MAX_MIR])
 {
 	if (mir[mir_no] & 3)
-		debugf2("MIR%d: limit= 0x%x Branch(es) that participate:"
-			" %s %s\n",
-			mir_no,
-			(mir[mir_no] >> 4) & 0xfff,
-			(mir[mir_no] & 1) ? "B0" : "",
-			(mir[mir_no] & 2) ? "B1" : "");
+		edac_dbg(2, "MIR%d: limit= 0x%x Branch(es) that participate: %s %s\n",
+			 mir_no,
+			 (mir[mir_no] >> 4) & 0xfff,
+			 (mir[mir_no] & 1) ? "B0" : "",
+			 (mir[mir_no] & 2) ? "B1" : "");
 }
 
 /**
@@ -849,17 +838,17 @@ static int i7300_get_mc_regs(struct mem_ctl_info *mci)
 	pci_read_config_dword(pvt->pci_dev_16_0_fsb_ctlr, AMBASE,
 			(u32 *) &pvt->ambase);
 
-	debugf2("AMBASE= 0x%lx\n", (long unsigned int)pvt->ambase);
+	edac_dbg(2, "AMBASE= 0x%lx\n", (long unsigned int)pvt->ambase);
 
 	/* Get the Branch Map regs */
 	pci_read_config_word(pvt->pci_dev_16_1_fsb_addr_map, TOLM, &pvt->tolm);
 	pvt->tolm >>= 12;
-	debugf2("TOLM (number of 256M regions) =%u (0x%x)\n", pvt->tolm,
-		pvt->tolm);
+	edac_dbg(2, "TOLM (number of 256M regions) =%u (0x%x)\n",
+		 pvt->tolm, pvt->tolm);
 
 	actual_tolm = (u32) ((1000l * pvt->tolm) >> (30 - 28));
-	debugf2("Actual TOLM byte addr=%u.%03u GB (0x%x)\n",
-		actual_tolm/1000, actual_tolm % 1000, pvt->tolm << 28);
+	edac_dbg(2, "Actual TOLM byte addr=%u.%03u GB (0x%x)\n",
+		 actual_tolm/1000, actual_tolm % 1000, pvt->tolm << 28);
 
 	/* Get memory controller settings */
 	pci_read_config_dword(pvt->pci_dev_16_1_fsb_addr_map, MC_SETTINGS,
@@ -868,15 +857,15 @@ static int i7300_get_mc_regs(struct mem_ctl_info *mci)
 			     &pvt->mc_settings_a);
 
 	if (IS_SINGLE_MODE(pvt->mc_settings_a))
-		debugf0("Memory controller operating on single mode\n");
+		edac_dbg(0, "Memory controller operating on single mode\n");
 	else
-		debugf0("Memory controller operating on %s mode\n",
-		IS_MIRRORED(pvt->mc_settings) ? "mirrored" : "non-mirrored");
+		edac_dbg(0, "Memory controller operating on %smirrored mode\n",
+			 IS_MIRRORED(pvt->mc_settings) ? "" : "non-");
 
-	debugf0("Error detection is %s\n",
-		IS_ECC_ENABLED(pvt->mc_settings) ? "enabled" : "disabled");
-	debugf0("Retry is %s\n",
-		IS_RETRY_ENABLED(pvt->mc_settings) ? "enabled" : "disabled");
+	edac_dbg(0, "Error detection is %s\n",
+		 IS_ECC_ENABLED(pvt->mc_settings) ? "enabled" : "disabled");
+	edac_dbg(0, "Retry is %s\n",
+		 IS_RETRY_ENABLED(pvt->mc_settings) ? "enabled" : "disabled");
 
 	/* Get Memory Interleave Range registers */
 	pci_read_config_word(pvt->pci_dev_16_1_fsb_addr_map, MIR0,
@@ -970,18 +959,18 @@ static int __devinit i7300_get_devices(struct mem_ctl_info *mci)
 		}
 	}
 
-	debugf1("System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->pci_dev_16_0_fsb_ctlr),
-		pvt->pci_dev_16_0_fsb_ctlr->vendor,
-		pvt->pci_dev_16_0_fsb_ctlr->device);
-	debugf1("Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->pci_dev_16_1_fsb_addr_map),
-		pvt->pci_dev_16_1_fsb_addr_map->vendor,
-		pvt->pci_dev_16_1_fsb_addr_map->device);
-	debugf1("FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
-		pci_name(pvt->pci_dev_16_2_fsb_err_regs),
-		pvt->pci_dev_16_2_fsb_err_regs->vendor,
-		pvt->pci_dev_16_2_fsb_err_regs->device);
+	edac_dbg(1, "System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->pci_dev_16_0_fsb_ctlr),
+		 pvt->pci_dev_16_0_fsb_ctlr->vendor,
+		 pvt->pci_dev_16_0_fsb_ctlr->device);
+	edac_dbg(1, "Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->pci_dev_16_1_fsb_addr_map),
+		 pvt->pci_dev_16_1_fsb_addr_map->vendor,
+		 pvt->pci_dev_16_1_fsb_addr_map->device);
+	edac_dbg(1, "FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
+		 pci_name(pvt->pci_dev_16_2_fsb_err_regs),
+		 pvt->pci_dev_16_2_fsb_err_regs->vendor,
+		 pvt->pci_dev_16_2_fsb_err_regs->device);
 
 	pvt->pci_dev_2x_0_fbd_branch[0] = pci_get_device(PCI_VENDOR_ID_INTEL,
 					    PCI_DEVICE_ID_INTEL_I7300_MCH_FB0,
@@ -1032,10 +1021,9 @@ static int __devinit i7300_init_one(struct pci_dev *pdev,
 	if (rc == -EIO)
 		return rc;
 
-	debugf0("MC: " __FILE__ ": %s(), pdev bus %u dev=0x%x fn=0x%x\n",
-		__func__,
-		pdev->bus->number,
-		PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+	edac_dbg(0, "MC: pdev bus %u dev=0x%x fn=0x%x\n",
+		 pdev->bus->number,
+		 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 
 	/* We only are looking for func 0 of the set */
 	if (PCI_FUNC(pdev->devfn) != 0)
@@ -1055,9 +1043,9 @@ static int __devinit i7300_init_one(struct pci_dev *pdev,
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+	edac_dbg(0, "MC: mci = %p\n", mci);
 
-	mci->dev = &pdev->dev;	/* record ptr  to the generic device */
+	mci->pdev = &pdev->dev;	/* record ptr  to the generic device */
 
 	pvt = mci->pvt_info;
 	pvt->pci_dev_16_0_fsb_ctlr = pdev;	/* Record this device in our private */
@@ -1088,19 +1076,16 @@ static int __devinit i7300_init_one(struct pci_dev *pdev,
 	/* initialize the MC control structure 'csrows' table
 	 * with the mapping and control information */
 	if (i7300_get_mc_regs(mci)) {
-		debugf0("MC: Setting mci->edac_cap to EDAC_FLAG_NONE\n"
-			"    because i7300_init_csrows() returned nonzero "
-			"value\n");
+		edac_dbg(0, "MC: Setting mci->edac_cap to EDAC_FLAG_NONE because i7300_init_csrows() returned nonzero value\n");
 		mci->edac_cap = EDAC_FLAG_NONE;	/* no csrows found */
 	} else {
-		debugf1("MC: Enable error reporting now\n");
+		edac_dbg(1, "MC: Enable error reporting now\n");
 		i7300_enable_error_reporting(mci);
 	}
 
 	/* add this new MC control structure to EDAC's list of MCs */
 	if (edac_mc_add_mc(mci)) {
-		debugf0("MC: " __FILE__
-			": %s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		/* FIXME: perhaps some code should go here that disables error
 		 * reporting if we just enabled it
 		 */
@@ -1142,7 +1127,7 @@ static void __devexit i7300_remove_one(struct pci_dev *pdev)
 	struct mem_ctl_info *mci;
 	char *tmp;
 
-	debugf0(__FILE__ ": %s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (i7300_pci)
 		edac_pci_release_generic_ctl(i7300_pci);
@@ -1189,7 +1174,7 @@ static int __init i7300_init(void)
 {
 	int pci_rc;
 
-	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	edac_dbg(2, "\n");
 
 	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
 	opstate_init();
@@ -1204,7 +1189,7 @@ static int __init i7300_init(void)
  */
 static void __exit i7300_exit(void)
 {
-	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	edac_dbg(2, "\n");
 	pci_unregister_driver(&i7300_driver);
 }
 
diff --git a/drivers/edac/i7core_edac.c b/drivers/edac/i7core_edac.c
index a499c7ed820..3672101023b 100644
--- a/drivers/edac/i7core_edac.c
+++ b/drivers/edac/i7core_edac.c
@@ -248,6 +248,8 @@ struct i7core_dev {
 };
 
 struct i7core_pvt {
+	struct device *addrmatch_dev, *chancounts_dev;
+
 	struct pci_dev	*pci_noncore;
 	struct pci_dev	*pci_mcr[MAX_MCR_FUNC + 1];
 	struct pci_dev	*pci_ch[NUM_CHANS][MAX_CHAN_FUNC + 1];
@@ -514,29 +516,28 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 	pci_read_config_dword(pdev, MC_MAX_DOD, &pvt->info.max_dod);
 	pci_read_config_dword(pdev, MC_CHANNEL_MAPPER, &pvt->info.ch_map);
 
-	debugf0("QPI %d control=0x%08x status=0x%08x dod=0x%08x map=0x%08x\n",
-		pvt->i7core_dev->socket, pvt->info.mc_control, pvt->info.mc_status,
-		pvt->info.max_dod, pvt->info.ch_map);
+	edac_dbg(0, "QPI %d control=0x%08x status=0x%08x dod=0x%08x map=0x%08x\n",
+		 pvt->i7core_dev->socket, pvt->info.mc_control,
+		 pvt->info.mc_status, pvt->info.max_dod, pvt->info.ch_map);
 
 	if (ECC_ENABLED(pvt)) {
-		debugf0("ECC enabled with x%d SDCC\n", ECCx8(pvt) ? 8 : 4);
+		edac_dbg(0, "ECC enabled with x%d SDCC\n", ECCx8(pvt) ? 8 : 4);
 		if (ECCx8(pvt))
 			mode = EDAC_S8ECD8ED;
 		else
 			mode = EDAC_S4ECD4ED;
 	} else {
-		debugf0("ECC disabled\n");
+		edac_dbg(0, "ECC disabled\n");
 		mode = EDAC_NONE;
 	}
 
 	/* FIXME: need to handle the error codes */
-	debugf0("DOD Max limits: DIMMS: %d, %d-ranked, %d-banked "
-		"x%x x 0x%x\n",
-		numdimms(pvt->info.max_dod),
-		numrank(pvt->info.max_dod >> 2),
-		numbank(pvt->info.max_dod >> 4),
-		numrow(pvt->info.max_dod >> 6),
-		numcol(pvt->info.max_dod >> 9));
+	edac_dbg(0, "DOD Max limits: DIMMS: %d, %d-ranked, %d-banked x%x x 0x%x\n",
+		 numdimms(pvt->info.max_dod),
+		 numrank(pvt->info.max_dod >> 2),
+		 numbank(pvt->info.max_dod >> 4),
+		 numrow(pvt->info.max_dod >> 6),
+		 numcol(pvt->info.max_dod >> 9));
 
 	for (i = 0; i < NUM_CHANS; i++) {
 		u32 data, dimm_dod[3], value[8];
@@ -545,11 +546,11 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 			continue;
 
 		if (!CH_ACTIVE(pvt, i)) {
-			debugf0("Channel %i is not active\n", i);
+			edac_dbg(0, "Channel %i is not active\n", i);
 			continue;
 		}
 		if (CH_DISABLED(pvt, i)) {
-			debugf0("Channel %i is disabled\n", i);
+			edac_dbg(0, "Channel %i is disabled\n", i);
 			continue;
 		}
 
@@ -580,15 +581,14 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		pci_read_config_dword(pvt->pci_ch[i][1],
 				MC_DOD_CH_DIMM2, &dimm_dod[2]);
 
-		debugf0("Ch%d phy rd%d, wr%d (0x%08x): "
-			"%s%s%s%cDIMMs\n",
-			i,
-			RDLCH(pvt->info.ch_map, i), WRLCH(pvt->info.ch_map, i),
-			data,
-			pvt->channel[i].is_3dimms_present ? "3DIMMS " : "",
-			pvt->channel[i].is_3dimms_present ? "SINGLE_4R " : "",
-			pvt->channel[i].has_4rank ? "HAS_4R " : "",
-			(data & REGISTERED_DIMM) ? 'R' : 'U');
+		edac_dbg(0, "Ch%d phy rd%d, wr%d (0x%08x): %s%s%s%cDIMMs\n",
+			 i,
+			 RDLCH(pvt->info.ch_map, i), WRLCH(pvt->info.ch_map, i),
+			 data,
+			 pvt->channel[i].is_3dimms_present ? "3DIMMS " : "",
+			 pvt->channel[i].is_3dimms_present ? "SINGLE_4R " : "",
+			 pvt->channel[i].has_4rank ? "HAS_4R " : "",
+			 (data & REGISTERED_DIMM) ? 'R' : 'U');
 
 		for (j = 0; j < 3; j++) {
 			u32 banks, ranks, rows, cols;
@@ -607,11 +607,10 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 			/* DDR3 has 8 I/O banks */
 			size = (rows * cols * banks * ranks) >> (20 - 3);
 
-			debugf0("\tdimm %d %d Mb offset: %x, "
-				"bank: %d, rank: %d, row: %#x, col: %#x\n",
-				j, size,
-				RANKOFFSET(dimm_dod[j]),
-				banks, ranks, rows, cols);
+			edac_dbg(0, "\tdimm %d %d Mb offset: %x, bank: %d, rank: %d, row: %#x, col: %#x\n",
+				 j, size,
+				 RANKOFFSET(dimm_dod[j]),
+				 banks, ranks, rows, cols);
 
 			npages = MiB_TO_PAGES(size);
 
@@ -647,12 +646,12 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		pci_read_config_dword(pdev, MC_SAG_CH_5, &value[5]);
 		pci_read_config_dword(pdev, MC_SAG_CH_6, &value[6]);
 		pci_read_config_dword(pdev, MC_SAG_CH_7, &value[7]);
-		debugf1("\t[%i] DIVBY3\tREMOVED\tOFFSET\n", i);
+		edac_dbg(1, "\t[%i] DIVBY3\tREMOVED\tOFFSET\n", i);
 		for (j = 0; j < 8; j++)
-			debugf1("\t\t%#x\t%#x\t%#x\n",
-				(value[j] >> 27) & 0x1,
-				(value[j] >> 24) & 0x7,
-				(value[j] & ((1 << 24) - 1)));
+			edac_dbg(1, "\t\t%#x\t%#x\t%#x\n",
+				 (value[j] >> 27) & 0x1,
+				 (value[j] >> 24) & 0x7,
+				 (value[j] & ((1 << 24) - 1)));
 	}
 
 	return 0;
@@ -662,6 +661,8 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 			Error insertion routines
  ****************************************************************************/
 
+#define to_mci(k) container_of(k, struct mem_ctl_info, dev)
+
 /* The i7core has independent error injection features per channel.
    However, to have a simpler code, we don't allow enabling error injection
    on more than one channel.
@@ -691,9 +692,11 @@ static int disable_inject(const struct mem_ctl_info *mci)
  *	bit 0 - refers to the lower 32-byte half cacheline
  *	bit 1 - refers to the upper 32-byte half cacheline
  */
-static ssize_t i7core_inject_section_store(struct mem_ctl_info *mci,
+static ssize_t i7core_inject_section_store(struct device *dev,
+					   struct device_attribute *mattr,
 					   const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	int rc;
@@ -709,9 +712,11 @@ static ssize_t i7core_inject_section_store(struct mem_ctl_info *mci,
 	return count;
 }
 
-static ssize_t i7core_inject_section_show(struct mem_ctl_info *mci,
-					      char *data)
+static ssize_t i7core_inject_section_show(struct device *dev,
+					  struct device_attribute *mattr,
+					  char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
 	return sprintf(data, "0x%08x\n", pvt->inject.section);
 }
@@ -724,10 +729,12 @@ static ssize_t i7core_inject_section_show(struct mem_ctl_info *mci,
  *	bit 1 - inject ECC error
  *	bit 2 - inject parity error
  */
-static ssize_t i7core_inject_type_store(struct mem_ctl_info *mci,
+static ssize_t i7core_inject_type_store(struct device *dev,
+					struct device_attribute *mattr,
 					const char *data, size_t count)
 {
-	struct i7core_pvt *pvt = mci->pvt_info;
+	struct mem_ctl_info *mci = to_mci(dev);
+struct i7core_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	int rc;
 
@@ -742,10 +749,13 @@ static ssize_t i7core_inject_type_store(struct mem_ctl_info *mci,
 	return count;
 }
 
-static ssize_t i7core_inject_type_show(struct mem_ctl_info *mci,
-					      char *data)
+static ssize_t i7core_inject_type_show(struct device *dev,
+				       struct device_attribute *mattr,
+				       char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
+
 	return sprintf(data, "0x%08x\n", pvt->inject.type);
 }
 
@@ -759,9 +769,11 @@ static ssize_t i7core_inject_type_show(struct mem_ctl_info *mci,
  *   23:16 and 31:24). Flipping bits in two symbol pairs will cause an
  *   uncorrectable error to be injected.
  */
-static ssize_t i7core_inject_eccmask_store(struct mem_ctl_info *mci,
-					const char *data, size_t count)
+static ssize_t i7core_inject_eccmask_store(struct device *dev,
+					   struct device_attribute *mattr,
+					   const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
 	unsigned long value;
 	int rc;
@@ -777,10 +789,13 @@ static ssize_t i7core_inject_eccmask_store(struct mem_ctl_info *mci,
 	return count;
 }
 
-static ssize_t i7core_inject_eccmask_show(struct mem_ctl_info *mci,
-					      char *data)
+static ssize_t i7core_inject_eccmask_show(struct device *dev,
+					  struct device_attribute *mattr,
+					  char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
+
 	return sprintf(data, "0x%08x\n", pvt->inject.eccmask);
 }
 
@@ -797,14 +812,16 @@ static ssize_t i7core_inject_eccmask_show(struct mem_ctl_info *mci,
 
 #define DECLARE_ADDR_MATCH(param, limit)			\
 static ssize_t i7core_inject_store_##param(			\
-		struct mem_ctl_info *mci,			\
-		const char *data, size_t count)			\
+	struct device *dev,					\
+	struct device_attribute *mattr,				\
+	const char *data, size_t count)				\
 {								\
+	struct mem_ctl_info *mci = to_mci(dev);			\
 	struct i7core_pvt *pvt;					\
 	long value;						\
 	int rc;							\
 								\
-	debugf1("%s()\n", __func__);				\
+	edac_dbg(1, "\n");					\
 	pvt = mci->pvt_info;					\
 								\
 	if (pvt->inject.enable)					\
@@ -824,13 +841,15 @@ static ssize_t i7core_inject_store_##param(			\
 }								\
 								\
 static ssize_t i7core_inject_show_##param(			\
-		struct mem_ctl_info *mci,			\
-		char *data)					\
+	struct device *dev,					\
+	struct device_attribute *mattr,				\
+	char *data)						\
 {								\
+	struct mem_ctl_info *mci = to_mci(dev);			\
 	struct i7core_pvt *pvt;					\
 								\
 	pvt = mci->pvt_info;					\
-	debugf1("%s() pvt=%p\n", __func__, pvt);		\
+	edac_dbg(1, "pvt=%p\n", pvt);				\
 	if (pvt->inject.param < 0)				\
 		return sprintf(data, "any\n");			\
 	else							\
@@ -838,14 +857,9 @@ static ssize_t i7core_inject_show_##param(			\
 }
 
 #define ATTR_ADDR_MATCH(param)					\
-	{							\
-		.attr = {					\
-			.name = #param,				\
-			.mode = (S_IRUGO | S_IWUSR)		\
-		},						\
-		.show  = i7core_inject_show_##param,		\
-		.store = i7core_inject_store_##param,		\
-	}
+	static DEVICE_ATTR(param, S_IRUGO | S_IWUSR,		\
+		    i7core_inject_show_##param,			\
+		    i7core_inject_store_##param)
 
 DECLARE_ADDR_MATCH(channel, 3);
 DECLARE_ADDR_MATCH(dimm, 3);
@@ -854,14 +868,21 @@ DECLARE_ADDR_MATCH(bank, 32);
 DECLARE_ADDR_MATCH(page, 0x10000);
 DECLARE_ADDR_MATCH(col, 0x4000);
 
+ATTR_ADDR_MATCH(channel);
+ATTR_ADDR_MATCH(dimm);
+ATTR_ADDR_MATCH(rank);
+ATTR_ADDR_MATCH(bank);
+ATTR_ADDR_MATCH(page);
+ATTR_ADDR_MATCH(col);
+
 static int write_and_test(struct pci_dev *dev, const int where, const u32 val)
 {
 	u32 read;
 	int count;
 
-	debugf0("setting pci %02x:%02x.%x reg=%02x value=%08x\n",
-		dev->bus->number, PCI_SLOT(dev->devfn), PCI_FUNC(dev->devfn),
-		where, val);
+	edac_dbg(0, "setting pci %02x:%02x.%x reg=%02x value=%08x\n",
+		 dev->bus->number, PCI_SLOT(dev->devfn), PCI_FUNC(dev->devfn),
+		 where, val);
 
 	for (count = 0; count < 10; count++) {
 		if (count)
@@ -899,9 +920,11 @@ static int write_and_test(struct pci_dev *dev, const int where, const u32 val)
  *    is reliable enough to check if the MC is using the
  *    three channels. However, this is not clear at the datasheet.
  */
-static ssize_t i7core_inject_enable_store(struct mem_ctl_info *mci,
-				       const char *data, size_t count)
+static ssize_t i7core_inject_enable_store(struct device *dev,
+					  struct device_attribute *mattr,
+					  const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
 	u32 injectmask;
 	u64 mask = 0;
@@ -994,17 +1017,18 @@ static ssize_t i7core_inject_enable_store(struct mem_ctl_info *mci,
 	pci_write_config_dword(pvt->pci_noncore,
 			       MC_CFG_CONTROL, 8);
 
-	debugf0("Error inject addr match 0x%016llx, ecc 0x%08x,"
-		" inject 0x%08x\n",
-		mask, pvt->inject.eccmask, injectmask);
+	edac_dbg(0, "Error inject addr match 0x%016llx, ecc 0x%08x, inject 0x%08x\n",
+		 mask, pvt->inject.eccmask, injectmask);
 
 
 	return count;
 }
 
-static ssize_t i7core_inject_enable_show(struct mem_ctl_info *mci,
-					char *data)
+static ssize_t i7core_inject_enable_show(struct device *dev,
+					 struct device_attribute *mattr,
+					 char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct i7core_pvt *pvt = mci->pvt_info;
 	u32 injectmask;
 
@@ -1014,7 +1038,7 @@ static ssize_t i7core_inject_enable_show(struct mem_ctl_info *mci,
 	pci_read_config_dword(pvt->pci_ch[pvt->inject.channel][0],
 			       MC_CHANNEL_ERROR_INJECT, &injectmask);
 
-	debugf0("Inject error read: 0x%018x\n", injectmask);
+	edac_dbg(0, "Inject error read: 0x%018x\n", injectmask);
 
 	if (injectmask & 0x0c)
 		pvt->inject.enable = 1;
@@ -1024,12 +1048,14 @@ static ssize_t i7core_inject_enable_show(struct mem_ctl_info *mci,
 
 #define DECLARE_COUNTER(param)					\
 static ssize_t i7core_show_counter_##param(			\
-		struct mem_ctl_info *mci,			\
-		char *data)					\
+	struct device *dev,					\
+	struct device_attribute *mattr,				\
+	char *data)						\
 {								\
+	struct mem_ctl_info *mci = to_mci(dev);			\
 	struct i7core_pvt *pvt = mci->pvt_info;			\
 								\
-	debugf1("%s() \n", __func__);				\
+	edac_dbg(1, "\n");					\
 	if (!pvt->ce_count_available || (pvt->is_registered))	\
 		return sprintf(data, "data unavailable\n");	\
 	return sprintf(data, "%lu\n",				\
@@ -1037,121 +1063,179 @@ static ssize_t i7core_show_counter_##param(			\
 }
 
 #define ATTR_COUNTER(param)					\
-	{							\
-		.attr = {					\
-			.name = __stringify(udimm##param),	\
-			.mode = (S_IRUGO | S_IWUSR)		\
-		},						\
-		.show  = i7core_show_counter_##param		\
-	}
+	static DEVICE_ATTR(udimm##param, S_IRUGO | S_IWUSR,	\
+		    i7core_show_counter_##param,		\
+		    NULL)
 
 DECLARE_COUNTER(0);
 DECLARE_COUNTER(1);
 DECLARE_COUNTER(2);
 
+ATTR_COUNTER(0);
+ATTR_COUNTER(1);
+ATTR_COUNTER(2);
+
 /*
- * Sysfs struct
+ * inject_addrmatch device sysfs struct
  */
 
-static const struct mcidev_sysfs_attribute i7core_addrmatch_attrs[] = {
-	ATTR_ADDR_MATCH(channel),
-	ATTR_ADDR_MATCH(dimm),
-	ATTR_ADDR_MATCH(rank),
-	ATTR_ADDR_MATCH(bank),
-	ATTR_ADDR_MATCH(page),
-	ATTR_ADDR_MATCH(col),
-	{ } /* End of list */
+static struct attribute *i7core_addrmatch_attrs[] = {
+	&dev_attr_channel.attr,
+	&dev_attr_dimm.attr,
+	&dev_attr_rank.attr,
+	&dev_attr_bank.attr,
+	&dev_attr_page.attr,
+	&dev_attr_col.attr,
+	NULL
 };
 
-static const struct mcidev_sysfs_group i7core_inject_addrmatch = {
-	.name  = "inject_addrmatch",
-	.mcidev_attr = i7core_addrmatch_attrs,
+static struct attribute_group addrmatch_grp = {
+	.attrs	= i7core_addrmatch_attrs,
 };
 
-static const struct mcidev_sysfs_attribute i7core_udimm_counters_attrs[] = {
-	ATTR_COUNTER(0),
-	ATTR_COUNTER(1),
-	ATTR_COUNTER(2),
-	{ .attr = { .name = NULL } }
+static const struct attribute_group *addrmatch_groups[] = {
+	&addrmatch_grp,
+	NULL
 };
 
-static const struct mcidev_sysfs_group i7core_udimm_counters = {
-	.name  = "all_channel_counts",
-	.mcidev_attr = i7core_udimm_counters_attrs,
+static void addrmatch_release(struct device *device)
+{
+	edac_dbg(1, "Releasing device %s\n", dev_name(device));
+	kfree(device);
+}
+
+static struct device_type addrmatch_type = {
+	.groups		= addrmatch_groups,
+	.release	= addrmatch_release,
 };
 
-static const struct mcidev_sysfs_attribute i7core_sysfs_rdimm_attrs[] = {
-	{
-		.attr = {
-			.name = "inject_section",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_section_show,
-		.store = i7core_inject_section_store,
-	}, {
-		.attr = {
-			.name = "inject_type",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_type_show,
-		.store = i7core_inject_type_store,
-	}, {
-		.attr = {
-			.name = "inject_eccmask",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_eccmask_show,
-		.store = i7core_inject_eccmask_store,
-	}, {
-		.grp = &i7core_inject_addrmatch,
-	}, {
-		.attr = {
-			.name = "inject_enable",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_enable_show,
-		.store = i7core_inject_enable_store,
-	},
-	{ }	/* End of list */
+/*
+ * all_channel_counts sysfs struct
+ */
+
+static struct attribute *i7core_udimm_counters_attrs[] = {
+	&dev_attr_udimm0.attr,
+	&dev_attr_udimm1.attr,
+	&dev_attr_udimm2.attr,
+	NULL
 };
 
-static const struct mcidev_sysfs_attribute i7core_sysfs_udimm_attrs[] = {
-	{
-		.attr = {
-			.name = "inject_section",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_section_show,
-		.store = i7core_inject_section_store,
-	}, {
-		.attr = {
-			.name = "inject_type",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_type_show,
-		.store = i7core_inject_type_store,
-	}, {
-		.attr = {
-			.name = "inject_eccmask",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_eccmask_show,
-		.store = i7core_inject_eccmask_store,
-	}, {
-		.grp = &i7core_inject_addrmatch,
-	}, {
-		.attr = {
-			.name = "inject_enable",
-			.mode = (S_IRUGO | S_IWUSR)
-		},
-		.show  = i7core_inject_enable_show,
-		.store = i7core_inject_enable_store,
-	}, {
-		.grp = &i7core_udimm_counters,
-	},
-	{ }	/* End of list */
+static struct attribute_group all_channel_counts_grp = {
+	.attrs	= i7core_udimm_counters_attrs,
 };
 
+static const struct attribute_group *all_channel_counts_groups[] = {
+	&all_channel_counts_grp,
+	NULL
+};
+
+static void all_channel_counts_release(struct device *device)
+{
+	edac_dbg(1, "Releasing device %s\n", dev_name(device));
+	kfree(device);
+}
+
+static struct device_type all_channel_counts_type = {
+	.groups		= all_channel_counts_groups,
+	.release	= all_channel_counts_release,
+};
+
+/*
+ * inject sysfs attributes
+ */
+
+static DEVICE_ATTR(inject_section, S_IRUGO | S_IWUSR,
+		   i7core_inject_section_show, i7core_inject_section_store);
+
+static DEVICE_ATTR(inject_type, S_IRUGO | S_IWUSR,
+		   i7core_inject_type_show, i7core_inject_type_store);
+
+
+static DEVICE_ATTR(inject_eccmask, S_IRUGO | S_IWUSR,
+		   i7core_inject_eccmask_show, i7core_inject_eccmask_store);
+
+static DEVICE_ATTR(inject_enable, S_IRUGO | S_IWUSR,
+		   i7core_inject_enable_show, i7core_inject_enable_store);
+
+static int i7core_create_sysfs_devices(struct mem_ctl_info *mci)
+{
+	struct i7core_pvt *pvt = mci->pvt_info;
+	int rc;
+
+	rc = device_create_file(&mci->dev, &dev_attr_inject_section);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_type);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_eccmask);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_enable);
+	if (rc < 0)
+		return rc;
+
+	pvt->addrmatch_dev = kzalloc(sizeof(*pvt->addrmatch_dev), GFP_KERNEL);
+	if (!pvt->addrmatch_dev)
+		return rc;
+
+	pvt->addrmatch_dev->type = &addrmatch_type;
+	pvt->addrmatch_dev->bus = mci->dev.bus;
+	device_initialize(pvt->addrmatch_dev);
+	pvt->addrmatch_dev->parent = &mci->dev;
+	dev_set_name(pvt->addrmatch_dev, "inject_addrmatch");
+	dev_set_drvdata(pvt->addrmatch_dev, mci);
+
+	edac_dbg(1, "creating %s\n", dev_name(pvt->addrmatch_dev));
+
+	rc = device_add(pvt->addrmatch_dev);
+	if (rc < 0)
+		return rc;
+
+	if (!pvt->is_registered) {
+		pvt->chancounts_dev = kzalloc(sizeof(*pvt->chancounts_dev),
+					      GFP_KERNEL);
+		if (!pvt->chancounts_dev) {
+			put_device(pvt->addrmatch_dev);
+			device_del(pvt->addrmatch_dev);
+			return rc;
+		}
+
+		pvt->chancounts_dev->type = &all_channel_counts_type;
+		pvt->chancounts_dev->bus = mci->dev.bus;
+		device_initialize(pvt->chancounts_dev);
+		pvt->chancounts_dev->parent = &mci->dev;
+		dev_set_name(pvt->chancounts_dev, "all_channel_counts");
+		dev_set_drvdata(pvt->chancounts_dev, mci);
+
+		edac_dbg(1, "creating %s\n", dev_name(pvt->chancounts_dev));
+
+		rc = device_add(pvt->chancounts_dev);
+		if (rc < 0)
+			return rc;
+	}
+	return 0;
+}
+
+static void i7core_delete_sysfs_devices(struct mem_ctl_info *mci)
+{
+	struct i7core_pvt *pvt = mci->pvt_info;
+
+	edac_dbg(1, "\n");
+
+	device_remove_file(&mci->dev, &dev_attr_inject_section);
+	device_remove_file(&mci->dev, &dev_attr_inject_type);
+	device_remove_file(&mci->dev, &dev_attr_inject_eccmask);
+	device_remove_file(&mci->dev, &dev_attr_inject_enable);
+
+	if (!pvt->is_registered) {
+		put_device(pvt->chancounts_dev);
+		device_del(pvt->chancounts_dev);
+	}
+	put_device(pvt->addrmatch_dev);
+	device_del(pvt->addrmatch_dev);
+}
+
 /****************************************************************************
 	Device initialization routines: put/get, init/exit
  ****************************************************************************/
@@ -1164,14 +1248,14 @@ static void i7core_put_devices(struct i7core_dev *i7core_dev)
 {
 	int i;
 
-	debugf0(__FILE__ ": %s()\n", __func__);
+	edac_dbg(0, "\n");
 	for (i = 0; i < i7core_dev->n_devs; i++) {
 		struct pci_dev *pdev = i7core_dev->pdev[i];
 		if (!pdev)
 			continue;
-		debugf0("Removing dev %02x:%02x.%d\n",
-			pdev->bus->number,
-			PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+		edac_dbg(0, "Removing dev %02x:%02x.%d\n",
+			 pdev->bus->number,
+			 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 		pci_dev_put(pdev);
 	}
 }
@@ -1214,12 +1298,12 @@ static unsigned i7core_pci_lastbus(void)
 
 	while ((b = pci_find_next_bus(b)) != NULL) {
 		bus = b->number;
-		debugf0("Found bus %d\n", bus);
+		edac_dbg(0, "Found bus %d\n", bus);
 		if (bus > last_bus)
 			last_bus = bus;
 	}
 
-	debugf0("Last bus %d\n", last_bus);
+	edac_dbg(0, "Last bus %d\n", last_bus);
 
 	return last_bus;
 }
@@ -1326,10 +1410,10 @@ static int i7core_get_onedevice(struct pci_dev **prev,
 		return -ENODEV;
 	}
 
-	debugf0("Detected socket %d dev %02x:%02x.%d PCI ID %04x:%04x\n",
-		socket, bus, dev_descr->dev,
-		dev_descr->func,
-		PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
+	edac_dbg(0, "Detected socket %d dev %02x:%02x.%d PCI ID %04x:%04x\n",
+		 socket, bus, dev_descr->dev,
+		 dev_descr->func,
+		 PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
 
 	/*
 	 * As stated on drivers/pci/search.c, the reference count for
@@ -1427,13 +1511,13 @@ static int mci_bind_devs(struct mem_ctl_info *mci,
 				family = "unknown";
 				pvt->enable_scrub = false;
 			}
-			debugf0("Detected a processor type %s\n", family);
+			edac_dbg(0, "Detected a processor type %s\n", family);
 		} else
 			goto error;
 
-		debugf0("Associated fn %d.%d, dev = %p, socket %d\n",
-			PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn),
-			pdev, i7core_dev->socket);
+		edac_dbg(0, "Associated fn %d.%d, dev = %p, socket %d\n",
+			 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn),
+			 pdev, i7core_dev->socket);
 
 		if (PCI_SLOT(pdev->devfn) == 3 &&
 			PCI_FUNC(pdev->devfn) == 2)
@@ -1452,18 +1536,6 @@ error:
 /****************************************************************************
 			Error check routines
  ****************************************************************************/
-static void i7core_rdimm_update_errcount(struct mem_ctl_info *mci,
-				      const int chan,
-				      const int dimm,
-				      const int add)
-{
-	int i;
-
-	for (i = 0; i < add; i++) {
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 0, 0, 0,
-				     chan, dimm, -1, "error", "", NULL);
-	}
-}
 
 static void i7core_rdimm_update_ce_count(struct mem_ctl_info *mci,
 					 const int chan,
@@ -1502,12 +1574,17 @@ static void i7core_rdimm_update_ce_count(struct mem_ctl_info *mci,
 
 	/*updated the edac core */
 	if (add0 != 0)
-		i7core_rdimm_update_errcount(mci, chan, 0, add0);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, add0,
+				     0, 0, 0,
+				     chan, 0, -1, "error", "");
 	if (add1 != 0)
-		i7core_rdimm_update_errcount(mci, chan, 1, add1);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, add1,
+				     0, 0, 0,
+				     chan, 1, -1, "error", "");
 	if (add2 != 0)
-		i7core_rdimm_update_errcount(mci, chan, 2, add2);
-
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, add2,
+				     0, 0, 0,
+				     chan, 2, -1, "error", "");
 }
 
 static void i7core_rdimm_check_mc_ecc_err(struct mem_ctl_info *mci)
@@ -1530,8 +1607,8 @@ static void i7core_rdimm_check_mc_ecc_err(struct mem_ctl_info *mci)
 	pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_5,
 								&rcv[2][1]);
 	for (i = 0 ; i < 3; i++) {
-		debugf3("MC_COR_ECC_CNT%d = 0x%x; MC_COR_ECC_CNT%d = 0x%x\n",
-			(i * 2), rcv[i][0], (i * 2) + 1, rcv[i][1]);
+		edac_dbg(3, "MC_COR_ECC_CNT%d = 0x%x; MC_COR_ECC_CNT%d = 0x%x\n",
+			 (i * 2), rcv[i][0], (i * 2) + 1, rcv[i][1]);
 		/*if the channel has 3 dimms*/
 		if (pvt->channel[i].dimms > 2) {
 			new0 = DIMM_BOT_COR_ERR(rcv[i][0]);
@@ -1562,7 +1639,7 @@ static void i7core_udimm_check_mc_ecc_err(struct mem_ctl_info *mci)
 	int new0, new1, new2;
 
 	if (!pvt->pci_mcr[4]) {
-		debugf0("%s MCR registers not found\n", __func__);
+		edac_dbg(0, "MCR registers not found\n");
 		return;
 	}
 
@@ -1626,7 +1703,7 @@ static void i7core_mce_output_error(struct mem_ctl_info *mci,
 				    const struct mce *m)
 {
 	struct i7core_pvt *pvt = mci->pvt_info;
-	char *type, *optype, *err, msg[80];
+	char *type, *optype, *err;
 	enum hw_event_mc_err_type tp_event;
 	unsigned long error = m->status & 0x1ff0000l;
 	bool uncorrected_error = m->mcgstatus & 1ll << 61;
@@ -1704,20 +1781,18 @@ static void i7core_mce_output_error(struct mem_ctl_info *mci,
 		err = "unknown";
 	}
 
-	snprintf(msg, sizeof(msg), "count=%d %s", core_err_cnt, optype);
-
 	/*
 	 * Call the helper to output message
 	 * FIXME: what to do if core_err_cnt > 1? Currently, it generates
 	 * only one event
 	 */
 	if (uncorrected_error || !pvt->is_registered)
-		edac_mc_handle_error(tp_event, mci,
+		edac_mc_handle_error(tp_event, mci, core_err_cnt,
 				     m->addr >> PAGE_SHIFT,
 				     m->addr & ~PAGE_MASK,
 				     syndrome,
 				     channel, dimm, -1,
-				     err, msg, m);
+				     err, optype);
 }
 
 /*
@@ -2094,8 +2169,7 @@ static void i7core_unregister_mci(struct i7core_dev *i7core_dev)
 	struct i7core_pvt *pvt;
 
 	if (unlikely(!mci || !mci->pvt_info)) {
-		debugf0("MC: " __FILE__ ": %s(): dev = %p\n",
-			__func__, &i7core_dev->pdev[0]->dev);
+		edac_dbg(0, "MC: dev = %p\n", &i7core_dev->pdev[0]->dev);
 
 		i7core_printk(KERN_ERR, "Couldn't find mci handler\n");
 		return;
@@ -2103,8 +2177,7 @@ static void i7core_unregister_mci(struct i7core_dev *i7core_dev)
 
 	pvt = mci->pvt_info;
 
-	debugf0("MC: " __FILE__ ": %s(): mci = %p, dev = %p\n",
-		__func__, mci, &i7core_dev->pdev[0]->dev);
+	edac_dbg(0, "MC: mci = %p, dev = %p\n", mci, &i7core_dev->pdev[0]->dev);
 
 	/* Disable scrubrate setting */
 	if (pvt->enable_scrub)
@@ -2114,9 +2187,10 @@ static void i7core_unregister_mci(struct i7core_dev *i7core_dev)
 	i7core_pci_ctl_release(pvt);
 
 	/* Remove MC sysfs nodes */
-	edac_mc_del_mc(mci->dev);
+	i7core_delete_sysfs_devices(mci);
+	edac_mc_del_mc(mci->pdev);
 
-	debugf1("%s: free mci struct\n", mci->ctl_name);
+	edac_dbg(1, "%s: free mci struct\n", mci->ctl_name);
 	kfree(mci->ctl_name);
 	edac_mc_free(mci);
 	i7core_dev->mci = NULL;
@@ -2142,8 +2216,7 @@ static int i7core_register_mci(struct i7core_dev *i7core_dev)
 	if (unlikely(!mci))
 		return -ENOMEM;
 
-	debugf0("MC: " __FILE__ ": %s(): mci = %p, dev = %p\n",
-		__func__, mci, &i7core_dev->pdev[0]->dev);
+	edac_dbg(0, "MC: mci = %p, dev = %p\n", mci, &i7core_dev->pdev[0]->dev);
 
 	pvt = mci->pvt_info;
 	memset(pvt, 0, sizeof(*pvt));
@@ -2172,15 +2245,11 @@ static int i7core_register_mci(struct i7core_dev *i7core_dev)
 	if (unlikely(rc < 0))
 		goto fail0;
 
-	if (pvt->is_registered)
-		mci->mc_driver_sysfs_attributes = i7core_sysfs_rdimm_attrs;
-	else
-		mci->mc_driver_sysfs_attributes = i7core_sysfs_udimm_attrs;
 
 	/* Get dimm basic config */
 	get_dimm_config(mci);
 	/* record ptr to the generic device */
-	mci->dev = &i7core_dev->pdev[0]->dev;
+	mci->pdev = &i7core_dev->pdev[0]->dev;
 	/* Set the function pointer to an actual operation function */
 	mci->edac_check = i7core_check_error;
 
@@ -2190,8 +2259,7 @@ static int i7core_register_mci(struct i7core_dev *i7core_dev)
 
 	/* add this new MC control structure to EDAC's list of MCs */
 	if (unlikely(edac_mc_add_mc(mci))) {
-		debugf0("MC: " __FILE__
-			": %s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		/* FIXME: perhaps some code should go here that disables error
 		 * reporting if we just enabled it
 		 */
@@ -2199,6 +2267,12 @@ static int i7core_register_mci(struct i7core_dev *i7core_dev)
 		rc = -EINVAL;
 		goto fail0;
 	}
+	if (i7core_create_sysfs_devices(mci)) {
+		edac_dbg(0, "MC: failed to create sysfs nodes\n");
+		edac_mc_del_mc(mci->pdev);
+		rc = -EINVAL;
+		goto fail0;
+	}
 
 	/* Default error mask is any memory */
 	pvt->inject.channel = 0;
@@ -2298,7 +2372,7 @@ static void __devexit i7core_remove(struct pci_dev *pdev)
 {
 	struct i7core_dev *i7core_dev;
 
-	debugf0(__FILE__ ": %s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/*
 	 * we have a trouble here: pdev value for removal will be wrong, since
@@ -2347,7 +2421,7 @@ static int __init i7core_init(void)
 {
 	int pci_rc;
 
-	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	edac_dbg(2, "\n");
 
 	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
 	opstate_init();
@@ -2374,7 +2448,7 @@ static int __init i7core_init(void)
  */
 static void __exit i7core_exit(void)
 {
-	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	edac_dbg(2, "\n");
 	pci_unregister_driver(&i7core_driver);
 	mce_unregister_decode_chain(&i7_mce_dec);
 }
diff --git a/drivers/edac/i82443bxgx_edac.c b/drivers/edac/i82443bxgx_edac.c
index 52072c28a8a..90f303db5d1 100644
--- a/drivers/edac/i82443bxgx_edac.c
+++ b/drivers/edac/i82443bxgx_edac.c
@@ -124,7 +124,7 @@ static void i82443bxgx_edacmc_get_error_info(struct mem_ctl_info *mci,
 				*info)
 {
 	struct pci_dev *pdev;
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 	pci_read_config_dword(pdev, I82443BXGX_EAP, &info->eap);
 	if (info->eap & I82443BXGX_EAP_OFFSET_SBE)
 		/* Clear error to allow next error to be reported [p.61] */
@@ -156,19 +156,19 @@ static int i82443bxgx_edacmc_process_error_info(struct mem_ctl_info *mci,
 	if (info->eap & I82443BXGX_EAP_OFFSET_SBE) {
 		error_found = 1;
 		if (handle_errors)
-			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 					     page, pageoffset, 0,
 					     edac_mc_find_csrow_by_page(mci, page),
-					     0, -1, mci->ctl_name, "", NULL);
+					     0, -1, mci->ctl_name, "");
 	}
 
 	if (info->eap & I82443BXGX_EAP_OFFSET_MBE) {
 		error_found = 1;
 		if (handle_errors)
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					     page, pageoffset, 0,
 					     edac_mc_find_csrow_by_page(mci, page),
-					     0, -1, mci->ctl_name, "", NULL);
+					     0, -1, mci->ctl_name, "");
 	}
 
 	return error_found;
@@ -178,7 +178,7 @@ static void i82443bxgx_edacmc_check(struct mem_ctl_info *mci)
 {
 	struct i82443bxgx_edacmc_error_info info;
 
-	debugf1("MC%d: %s: %s()\n", mci->mc_idx, __FILE__, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	i82443bxgx_edacmc_get_error_info(mci, &info);
 	i82443bxgx_edacmc_process_error_info(mci, &info, 1);
 }
@@ -197,18 +197,17 @@ static void i82443bxgx_init_csrows(struct mem_ctl_info *mci,
 	pci_read_config_byte(pdev, I82443BXGX_DRAMC, &dramc);
 	row_high_limit_last = 0;
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
-		dimm = csrow->channels[0].dimm;
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
 
 		pci_read_config_byte(pdev, I82443BXGX_DRB + index, &drbar);
-		debugf1("MC%d: %s: %s() Row=%d DRB = %#0x\n",
-			mci->mc_idx, __FILE__, __func__, index, drbar);
+		edac_dbg(1, "MC%d: Row=%d DRB = %#0x\n",
+			 mci->mc_idx, index, drbar);
 		row_high_limit = ((u32) drbar << 23);
 		/* find the DRAM Chip Select Base address and mask */
-		debugf1("MC%d: %s: %s() Row=%d, "
-			"Boundary Address=%#0x, Last = %#0x\n",
-			mci->mc_idx, __FILE__, __func__, index, row_high_limit,
-			row_high_limit_last);
+		edac_dbg(1, "MC%d: Row=%d, Boundary Address=%#0x, Last = %#0x\n",
+			 mci->mc_idx, index, row_high_limit,
+			 row_high_limit_last);
 
 		/* 440GX goes to 2GB, represented with a DRB of 0. */
 		if (row_high_limit_last && !row_high_limit)
@@ -241,7 +240,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 	enum mem_type mtype;
 	enum edac_type edac_mode;
 
-	debugf0("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "MC:\n");
 
 	/* Something is really hosed if PCI config space reads from
 	 * the MC aren't working.
@@ -259,8 +258,8 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf0("MC: %s: %s(): mci = %p\n", __FILE__, __func__, mci);
-	mci->dev = &pdev->dev;
+	edac_dbg(0, "MC: mci = %p\n", mci);
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_EDO | MEM_FLAG_SDR | MEM_FLAG_RDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_EC | EDAC_FLAG_SECDED;
 	pci_read_config_byte(pdev, I82443BXGX_DRAMC, &dramc);
@@ -275,8 +274,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 		mtype = MEM_RDR;
 		break;
 	default:
-		debugf0("Unknown/reserved DRAM type value "
-			"in DRAMC register!\n");
+		edac_dbg(0, "Unknown/reserved DRAM type value in DRAMC register!\n");
 		mtype = -MEM_UNKNOWN;
 	}
 
@@ -305,8 +303,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 		edac_mode = EDAC_SECDED;
 		break;
 	default:
-		debugf0("%s(): Unknown/reserved ECC state "
-			"in NBXCFG register!\n", __func__);
+		edac_dbg(0, "Unknown/reserved ECC state in NBXCFG register!\n");
 		edac_mode = EDAC_UNKNOWN;
 		break;
 	}
@@ -330,7 +327,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 	mci->ctl_page_to_phys = NULL;
 
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
@@ -345,7 +342,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
 			__func__);
 	}
 
-	debugf3("MC: %s: %s(): success\n", __FILE__, __func__);
+	edac_dbg(3, "MC: success\n");
 	return 0;
 
 fail:
@@ -361,7 +358,7 @@ static int __devinit i82443bxgx_edacmc_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("MC: %s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "MC:\n");
 
 	/* don't need to call pci_enable_device() */
 	rc = i82443bxgx_edacmc_probe1(pdev, ent->driver_data);
@@ -376,7 +373,7 @@ static void __devexit i82443bxgx_edacmc_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s: %s()\n", __FILE__, __func__);
+	edac_dbg(0, "\n");
 
 	if (i82443bxgx_pci)
 		edac_pci_release_generic_ctl(i82443bxgx_pci);
@@ -428,7 +425,7 @@ static int __init i82443bxgx_edacmc_init(void)
 			id = &i82443bxgx_pci_tbl[i];
 		}
 		if (!mci_pdev) {
-			debugf0("i82443bxgx pci_get_device fail\n");
+			edac_dbg(0, "i82443bxgx pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -436,7 +433,7 @@ static int __init i82443bxgx_edacmc_init(void)
 		pci_rc = i82443bxgx_edacmc_init_one(mci_pdev, i82443bxgx_pci_tbl);
 
 		if (pci_rc < 0) {
-			debugf0("i82443bxgx init fail\n");
+			edac_dbg(0, "i82443bxgx init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
diff --git a/drivers/edac/i82860_edac.c b/drivers/edac/i82860_edac.c
index 08045059d10..1faa7497151 100644
--- a/drivers/edac/i82860_edac.c
+++ b/drivers/edac/i82860_edac.c
@@ -67,7 +67,7 @@ static void i82860_get_error_info(struct mem_ctl_info *mci,
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * This is a mess because there is no atomic way to read all the
@@ -109,25 +109,25 @@ static int i82860_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & 0x0003) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
-				     -1, -1, -1, "UE overwrote CE", "", NULL);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
+				     -1, -1, -1, "UE overwrote CE", "");
 		info->errsts = info->errsts2;
 	}
 
 	info->eap >>= PAGE_SHIFT;
 	row = edac_mc_find_csrow_by_page(mci, info->eap);
-	dimm = mci->csrows[row].channels[0].dimm;
+	dimm = mci->csrows[row]->channels[0]->dimm;
 
 	if (info->errsts & 0x0002)
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     info->eap, 0, 0,
 				     dimm->location[0], dimm->location[1], -1,
-				     "i82860 UE", "", NULL);
+				     "i82860 UE", "");
 	else
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     info->eap, 0, info->derrsyn,
 				     dimm->location[0], dimm->location[1], -1,
-				     "i82860 CE", "", NULL);
+				     "i82860 CE", "");
 
 	return 1;
 }
@@ -136,7 +136,7 @@ static void i82860_check(struct mem_ctl_info *mci)
 {
 	struct i82860_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	i82860_get_error_info(mci, &info);
 	i82860_process_error_info(mci, &info, 1);
 }
@@ -161,14 +161,13 @@ static void i82860_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev)
 	 * in all eight rows.
 	 */
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
-		dimm = csrow->channels[0].dimm;
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
 
 		pci_read_config_word(pdev, I82860_GBA + index * 2, &value);
 		cumul_size = (value & I82860_GBA_MASK) <<
 			(I82860_GBA_SHIFT - PAGE_SHIFT);
-		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
-			cumul_size);
+		edac_dbg(3, "(%d) cumul_size 0x%x\n", index, cumul_size);
 
 		if (cumul_size == last_cumul_size)
 			continue;	/* not populated */
@@ -210,8 +209,8 @@ static int i82860_probe1(struct pci_dev *pdev, int dev_idx)
 	if (!mci)
 		return -ENOMEM;
 
-	debugf3("%s(): init mci\n", __func__);
-	mci->dev = &pdev->dev;
+	edac_dbg(3, "init mci\n");
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
 	/* I"m not sure about this but I think that all RDRAM is SECDED */
@@ -229,7 +228,7 @@ static int i82860_probe1(struct pci_dev *pdev, int dev_idx)
 	 * type of memory controller.  The ID is therefore hardcoded to 0.
 	 */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
@@ -245,7 +244,7 @@ static int i82860_probe1(struct pci_dev *pdev, int dev_idx)
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 
 	return 0;
 
@@ -260,7 +259,7 @@ static int __devinit i82860_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 	i82860_printk(KERN_INFO, "i82860 init one\n");
 
 	if (pci_enable_device(pdev) < 0)
@@ -278,7 +277,7 @@ static void __devexit i82860_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (i82860_pci)
 		edac_pci_release_generic_ctl(i82860_pci);
@@ -311,7 +310,7 @@ static int __init i82860_init(void)
 {
 	int pci_rc;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -324,7 +323,7 @@ static int __init i82860_init(void)
 					PCI_DEVICE_ID_INTEL_82860_0, NULL);
 
 		if (mci_pdev == NULL) {
-			debugf0("860 pci_get_device fail\n");
+			edac_dbg(0, "860 pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -332,7 +331,7 @@ static int __init i82860_init(void)
 		pci_rc = i82860_init_one(mci_pdev, i82860_pci_tbl);
 
 		if (pci_rc < 0) {
-			debugf0("860 init fail\n");
+			edac_dbg(0, "860 init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -352,7 +351,7 @@ fail0:
 
 static void __exit i82860_exit(void)
 {
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	pci_unregister_driver(&i82860_driver);
 
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index b613e31c16e..3e416b1a6b5 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -189,7 +189,7 @@ static void i82875p_get_error_info(struct mem_ctl_info *mci,
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * This is a mess because there is no atomic way to read all the
@@ -227,7 +227,7 @@ static int i82875p_process_error_info(struct mem_ctl_info *mci,
 {
 	int row, multi_chan;
 
-	multi_chan = mci->csrows[0].nr_channels - 1;
+	multi_chan = mci->csrows[0]->nr_channels - 1;
 
 	if (!(info->errsts & 0x0081))
 		return 0;
@@ -236,9 +236,9 @@ static int i82875p_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & 0x0081) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
 				     -1, -1, -1,
-				     "UE overwrote CE", "", NULL);
+				     "UE overwrote CE", "");
 		info->errsts = info->errsts2;
 	}
 
@@ -246,15 +246,15 @@ static int i82875p_process_error_info(struct mem_ctl_info *mci,
 	row = edac_mc_find_csrow_by_page(mci, info->eap);
 
 	if (info->errsts & 0x0080)
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     info->eap, 0, 0,
 				     row, -1, -1,
-				     "i82875p UE", "", NULL);
+				     "i82875p UE", "");
 	else
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     info->eap, 0, info->derrsyn,
 				     row, multi_chan ? (info->des & 0x1) : 0,
-				     -1, "i82875p CE", "", NULL);
+				     -1, "i82875p CE", "");
 
 	return 1;
 }
@@ -263,7 +263,7 @@ static void i82875p_check(struct mem_ctl_info *mci)
 {
 	struct i82875p_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	i82875p_get_error_info(mci, &info);
 	i82875p_process_error_info(mci, &info, 1);
 }
@@ -367,12 +367,11 @@ static void i82875p_init_csrows(struct mem_ctl_info *mci,
 	 */
 
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
+		csrow = mci->csrows[index];
 
 		value = readb(ovrfl_window + I82875P_DRB + index);
 		cumul_size = value << (I82875P_DRB_SHIFT - PAGE_SHIFT);
-		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
-			cumul_size);
+		edac_dbg(3, "(%d) cumul_size 0x%x\n", index, cumul_size);
 		if (cumul_size == last_cumul_size)
 			continue;	/* not populated */
 
@@ -382,7 +381,7 @@ static void i82875p_init_csrows(struct mem_ctl_info *mci,
 		last_cumul_size = cumul_size;
 
 		for (j = 0; j < nr_chans; j++) {
-			dimm = csrow->channels[j].dimm;
+			dimm = csrow->channels[j]->dimm;
 
 			dimm->nr_pages = nr_pages / nr_chans;
 			dimm->grain = 1 << 12;	/* I82875P_EAP has 4KiB reolution */
@@ -405,7 +404,7 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 	u32 nr_chans;
 	struct i82875p_error_info discard;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	ovrfl_pdev = pci_get_device(PCI_VEND_DEV(INTEL, 82875_6), NULL);
 
@@ -426,11 +425,8 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 		goto fail0;
 	}
 
-	/* Keeps mci available after edac_mc_del_mc() till edac_mc_free() */
-	kobject_get(&mci->edac_mci_kobj);
-
-	debugf3("%s(): init mci\n", __func__);
-	mci->dev = &pdev->dev;
+	edac_dbg(3, "init mci\n");
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
 	mci->edac_cap = EDAC_FLAG_UNKNOWN;
@@ -440,7 +436,7 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 	mci->dev_name = pci_name(pdev);
 	mci->edac_check = i82875p_check;
 	mci->ctl_page_to_phys = NULL;
-	debugf3("%s(): init pvt\n", __func__);
+	edac_dbg(3, "init pvt\n");
 	pvt = (struct i82875p_pvt *)mci->pvt_info;
 	pvt->ovrfl_pdev = ovrfl_pdev;
 	pvt->ovrfl_window = ovrfl_window;
@@ -451,7 +447,7 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 	 * type of memory controller.  The ID is therefore hardcoded to 0.
 	 */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail1;
 	}
 
@@ -467,11 +463,10 @@ static int i82875p_probe1(struct pci_dev *pdev, int dev_idx)
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	return 0;
 
 fail1:
-	kobject_put(&mci->edac_mci_kobj);
 	edac_mc_free(mci);
 
 fail0:
@@ -489,7 +484,7 @@ static int __devinit i82875p_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 	i82875p_printk(KERN_INFO, "i82875p init one\n");
 
 	if (pci_enable_device(pdev) < 0)
@@ -508,7 +503,7 @@ static void __devexit i82875p_remove_one(struct pci_dev *pdev)
 	struct mem_ctl_info *mci;
 	struct i82875p_pvt *pvt = NULL;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (i82875p_pci)
 		edac_pci_release_generic_ctl(i82875p_pci);
@@ -554,7 +549,7 @@ static int __init i82875p_init(void)
 {
 	int pci_rc;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -569,7 +564,7 @@ static int __init i82875p_init(void)
 					PCI_DEVICE_ID_INTEL_82875_0, NULL);
 
 		if (!mci_pdev) {
-			debugf0("875p pci_get_device fail\n");
+			edac_dbg(0, "875p pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -577,7 +572,7 @@ static int __init i82875p_init(void)
 		pci_rc = i82875p_init_one(mci_pdev, i82875p_pci_tbl);
 
 		if (pci_rc < 0) {
-			debugf0("875p init fail\n");
+			edac_dbg(0, "875p init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -597,7 +592,7 @@ fail0:
 
 static void __exit i82875p_exit(void)
 {
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	i82875p_remove_one(mci_pdev);
 	pci_dev_put(mci_pdev);
diff --git a/drivers/edac/i82975x_edac.c b/drivers/edac/i82975x_edac.c
index 433332c7cdb..069e26c11c4 100644
--- a/drivers/edac/i82975x_edac.c
+++ b/drivers/edac/i82975x_edac.c
@@ -241,7 +241,7 @@ static void i82975x_get_error_info(struct mem_ctl_info *mci,
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * This is a mess because there is no atomic way to read all the
@@ -288,8 +288,8 @@ static int i82975x_process_error_info(struct mem_ctl_info *mci,
 		return 1;
 
 	if ((info->errsts ^ info->errsts2) & 0x0003) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
-				     -1, -1, -1, "UE overwrote CE", "", NULL);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
+				     -1, -1, -1, "UE overwrote CE", "");
 		info->errsts = info->errsts2;
 	}
 
@@ -308,21 +308,21 @@ static int i82975x_process_error_info(struct mem_ctl_info *mci,
 			(info->xeap & 1) ? 1 : 0, info->eap, (unsigned int) page);
 		return 0;
 	}
-	chan = (mci->csrows[row].nr_channels == 1) ? 0 : info->eap & 1;
+	chan = (mci->csrows[row]->nr_channels == 1) ? 0 : info->eap & 1;
 	offst = info->eap
 			& ((1 << PAGE_SHIFT) -
-			   (1 << mci->csrows[row].channels[chan].dimm->grain));
+			   (1 << mci->csrows[row]->channels[chan]->dimm->grain));
 
 	if (info->errsts & 0x0002)
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     page, offst, 0,
 				     row, -1, -1,
-				     "i82975x UE", "", NULL);
+				     "i82975x UE", "");
 	else
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     page, offst, info->derrsyn,
 				     row, chan ? chan : 0, -1,
-				     "i82975x CE", "", NULL);
+				     "i82975x CE", "");
 
 	return 1;
 }
@@ -331,7 +331,7 @@ static void i82975x_check(struct mem_ctl_info *mci)
 {
 	struct i82975x_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	i82975x_get_error_info(mci, &info);
 	i82975x_process_error_info(mci, &info, 1);
 }
@@ -394,7 +394,7 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 	 */
 
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
+		csrow = mci->csrows[index];
 
 		value = readb(mch_window + I82975X_DRB + index +
 					((index >= 4) ? 0x80 : 0));
@@ -406,8 +406,7 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 		 */
 		if (csrow->nr_channels > 1)
 			cumul_size <<= 1;
-		debugf3("%s(): (%d) cumul_size 0x%x\n", __func__, index,
-			cumul_size);
+		edac_dbg(3, "(%d) cumul_size 0x%x\n", index, cumul_size);
 
 		nr_pages = cumul_size - last_cumul_size;
 		if (!nr_pages)
@@ -421,10 +420,10 @@ static void i82975x_init_csrows(struct mem_ctl_info *mci,
 		 */
 		dtype = i82975x_dram_type(mch_window, index);
 		for (chan = 0; chan < csrow->nr_channels; chan++) {
-			dimm = mci->csrows[index].channels[chan].dimm;
+			dimm = mci->csrows[index]->channels[chan]->dimm;
 
 			dimm->nr_pages = nr_pages / csrow->nr_channels;
-			strncpy(csrow->channels[chan].dimm->label,
+			strncpy(csrow->channels[chan]->dimm->label,
 					labels[(index >> 1) + (chan * 2)],
 					EDAC_MC_LABEL_LEN);
 			dimm->grain = 1 << 7;	/* 128Byte cache-line resolution */
@@ -489,11 +488,11 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 	u8 c1drb[4];
 #endif
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	pci_read_config_dword(pdev, I82975X_MCHBAR, &mchbar);
 	if (!(mchbar & 1)) {
-		debugf3("%s(): failed, MCHBAR disabled!\n", __func__);
+		edac_dbg(3, "failed, MCHBAR disabled!\n");
 		goto fail0;
 	}
 	mchbar &= 0xffffc000;	/* bits 31:14 used for 16K window */
@@ -558,8 +557,8 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 		goto fail1;
 	}
 
-	debugf3("%s(): init mci\n", __func__);
-	mci->dev = &pdev->dev;
+	edac_dbg(3, "init mci\n");
+	mci->pdev = &pdev->dev;
 	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;
@@ -569,7 +568,7 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 	mci->dev_name = pci_name(pdev);
 	mci->edac_check = i82975x_check;
 	mci->ctl_page_to_phys = NULL;
-	debugf3("%s(): init pvt\n", __func__);
+	edac_dbg(3, "init pvt\n");
 	pvt = (struct i82975x_pvt *) mci->pvt_info;
 	pvt->mch_window = mch_window;
 	i82975x_init_csrows(mci, pdev, mch_window);
@@ -578,12 +577,12 @@ static int i82975x_probe1(struct pci_dev *pdev, int dev_idx)
 
 	/* finalize this instance of memory controller with edac core */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail2;
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	return 0;
 
 fail2:
@@ -601,7 +600,7 @@ static int __devinit i82975x_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (pci_enable_device(pdev) < 0)
 		return -EIO;
@@ -619,7 +618,7 @@ static void __devexit i82975x_remove_one(struct pci_dev *pdev)
 	struct mem_ctl_info *mci;
 	struct i82975x_pvt *pvt;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	mci = edac_mc_del_mc(&pdev->dev);
 	if (mci  == NULL)
@@ -655,7 +654,7 @@ static int __init i82975x_init(void)
 {
 	int pci_rc;
 
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -669,7 +668,7 @@ static int __init i82975x_init(void)
 				PCI_DEVICE_ID_INTEL_82975_0, NULL);
 
 		if (!mci_pdev) {
-			debugf0("i82975x pci_get_device fail\n");
+			edac_dbg(0, "i82975x pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -677,7 +676,7 @@ static int __init i82975x_init(void)
 		pci_rc = i82975x_init_one(mci_pdev, i82975x_pci_tbl);
 
 		if (pci_rc < 0) {
-			debugf0("i82975x init fail\n");
+			edac_dbg(0, "i82975x init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -697,7 +696,7 @@ fail0:
 
 static void __exit i82975x_exit(void)
 {
-	debugf3("%s()\n", __func__);
+	edac_dbg(3, "\n");
 
 	pci_unregister_driver(&i82975x_driver);
 
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 0e374625f6f..a1e791ec25d 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -49,34 +49,45 @@ static u32 orig_hid1[2];
 
 /************************ MC SYSFS parts ***********************************/
 
-static ssize_t mpc85xx_mc_inject_data_hi_show(struct mem_ctl_info *mci,
+#define to_mci(k) container_of(k, struct mem_ctl_info, dev)
+
+static ssize_t mpc85xx_mc_inject_data_hi_show(struct device *dev,
+					      struct device_attribute *mattr,
 					      char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	return sprintf(data, "0x%08x",
 		       in_be32(pdata->mc_vbase +
 			       MPC85XX_MC_DATA_ERR_INJECT_HI));
 }
 
-static ssize_t mpc85xx_mc_inject_data_lo_show(struct mem_ctl_info *mci,
+static ssize_t mpc85xx_mc_inject_data_lo_show(struct device *dev,
+					      struct device_attribute *mattr,
 					      char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	return sprintf(data, "0x%08x",
 		       in_be32(pdata->mc_vbase +
 			       MPC85XX_MC_DATA_ERR_INJECT_LO));
 }
 
-static ssize_t mpc85xx_mc_inject_ctrl_show(struct mem_ctl_info *mci, char *data)
+static ssize_t mpc85xx_mc_inject_ctrl_show(struct device *dev,
+					   struct device_attribute *mattr,
+					   char *data)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	return sprintf(data, "0x%08x",
 		       in_be32(pdata->mc_vbase + MPC85XX_MC_ECC_ERR_INJECT));
 }
 
-static ssize_t mpc85xx_mc_inject_data_hi_store(struct mem_ctl_info *mci,
+static ssize_t mpc85xx_mc_inject_data_hi_store(struct device *dev,
+					       struct device_attribute *mattr,
 					       const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	if (isdigit(*data)) {
 		out_be32(pdata->mc_vbase + MPC85XX_MC_DATA_ERR_INJECT_HI,
@@ -86,9 +97,11 @@ static ssize_t mpc85xx_mc_inject_data_hi_store(struct mem_ctl_info *mci,
 	return 0;
 }
 
-static ssize_t mpc85xx_mc_inject_data_lo_store(struct mem_ctl_info *mci,
+static ssize_t mpc85xx_mc_inject_data_lo_store(struct device *dev,
+					       struct device_attribute *mattr,
 					       const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	if (isdigit(*data)) {
 		out_be32(pdata->mc_vbase + MPC85XX_MC_DATA_ERR_INJECT_LO,
@@ -98,9 +111,11 @@ static ssize_t mpc85xx_mc_inject_data_lo_store(struct mem_ctl_info *mci,
 	return 0;
 }
 
-static ssize_t mpc85xx_mc_inject_ctrl_store(struct mem_ctl_info *mci,
-					    const char *data, size_t count)
+static ssize_t mpc85xx_mc_inject_ctrl_store(struct device *dev,
+					       struct device_attribute *mattr,
+					       const char *data, size_t count)
 {
+	struct mem_ctl_info *mci = to_mci(dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 	if (isdigit(*data)) {
 		out_be32(pdata->mc_vbase + MPC85XX_MC_ECC_ERR_INJECT,
@@ -110,38 +125,35 @@ static ssize_t mpc85xx_mc_inject_ctrl_store(struct mem_ctl_info *mci,
 	return 0;
 }
 
-static struct mcidev_sysfs_attribute mpc85xx_mc_sysfs_attributes[] = {
-	{
-	 .attr = {
-		  .name = "inject_data_hi",
-		  .mode = (S_IRUGO | S_IWUSR)
-		  },
-	 .show = mpc85xx_mc_inject_data_hi_show,
-	 .store = mpc85xx_mc_inject_data_hi_store},
-	{
-	 .attr = {
-		  .name = "inject_data_lo",
-		  .mode = (S_IRUGO | S_IWUSR)
-		  },
-	 .show = mpc85xx_mc_inject_data_lo_show,
-	 .store = mpc85xx_mc_inject_data_lo_store},
-	{
-	 .attr = {
-		  .name = "inject_ctrl",
-		  .mode = (S_IRUGO | S_IWUSR)
-		  },
-	 .show = mpc85xx_mc_inject_ctrl_show,
-	 .store = mpc85xx_mc_inject_ctrl_store},
+DEVICE_ATTR(inject_data_hi, S_IRUGO | S_IWUSR,
+	    mpc85xx_mc_inject_data_hi_show, mpc85xx_mc_inject_data_hi_store);
+DEVICE_ATTR(inject_data_lo, S_IRUGO | S_IWUSR,
+	    mpc85xx_mc_inject_data_lo_show, mpc85xx_mc_inject_data_lo_store);
+DEVICE_ATTR(inject_ctrl, S_IRUGO | S_IWUSR,
+	    mpc85xx_mc_inject_ctrl_show, mpc85xx_mc_inject_ctrl_store);
 
-	/* End of list */
-	{
-	 .attr = {.name = NULL}
-	 }
-};
+static int mpc85xx_create_sysfs_attributes(struct mem_ctl_info *mci)
+{
+	int rc;
+
+	rc = device_create_file(&mci->dev, &dev_attr_inject_data_hi);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_data_lo);
+	if (rc < 0)
+		return rc;
+	rc = device_create_file(&mci->dev, &dev_attr_inject_ctrl);
+	if (rc < 0)
+		return rc;
 
-static void mpc85xx_set_mc_sysfs_attributes(struct mem_ctl_info *mci)
+	return 0;
+}
+
+static void mpc85xx_remove_sysfs_attributes(struct mem_ctl_info *mci)
 {
-	mci->mc_driver_sysfs_attributes = mpc85xx_mc_sysfs_attributes;
+	device_remove_file(&mci->dev, &dev_attr_inject_data_hi);
+	device_remove_file(&mci->dev, &dev_attr_inject_data_lo);
+	device_remove_file(&mci->dev, &dev_attr_inject_ctrl);
 }
 
 /**************************** PCI Err device ***************************/
@@ -268,7 +280,7 @@ static int __devinit mpc85xx_pci_err_probe(struct platform_device *op)
 	out_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_DR, ~0);
 
 	if (edac_pci_add_device(pci, pdata->edac_idx) > 0) {
-		debugf3("%s(): failed edac_pci_add_device()\n", __func__);
+		edac_dbg(3, "failed edac_pci_add_device()\n");
 		goto err;
 	}
 
@@ -291,7 +303,7 @@ static int __devinit mpc85xx_pci_err_probe(struct platform_device *op)
 	}
 
 	devres_remove_group(&op->dev, mpc85xx_pci_err_probe);
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	printk(KERN_INFO EDAC_MOD_STR " PCI err registered\n");
 
 	return 0;
@@ -309,7 +321,7 @@ static int mpc85xx_pci_err_remove(struct platform_device *op)
 	struct edac_pci_ctl_info *pci = dev_get_drvdata(&op->dev);
 	struct mpc85xx_pci_pdata *pdata = pci->pvt_info;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	out_be32(pdata->pci_vbase + MPC85XX_PCI_ERR_CAP_DR,
 		 orig_pci_err_cap_dr);
@@ -570,7 +582,7 @@ static int __devinit mpc85xx_l2_err_probe(struct platform_device *op)
 	pdata->edac_idx = edac_dev_idx++;
 
 	if (edac_device_add_device(edac_dev) > 0) {
-		debugf3("%s(): failed edac_device_add_device()\n", __func__);
+		edac_dbg(3, "failed edac_device_add_device()\n");
 		goto err;
 	}
 
@@ -598,7 +610,7 @@ static int __devinit mpc85xx_l2_err_probe(struct platform_device *op)
 
 	devres_remove_group(&op->dev, mpc85xx_l2_err_probe);
 
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	printk(KERN_INFO EDAC_MOD_STR " L2 err registered\n");
 
 	return 0;
@@ -616,7 +628,7 @@ static int mpc85xx_l2_err_remove(struct platform_device *op)
 	struct edac_device_ctl_info *edac_dev = dev_get_drvdata(&op->dev);
 	struct mpc85xx_l2_pdata *pdata = edac_dev->pvt_info;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (edac_op_state == EDAC_OPSTATE_INT) {
 		out_be32(pdata->l2_vbase + MPC85XX_L2_ERRINTEN, 0);
@@ -813,7 +825,7 @@ static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 	pfn = err_addr >> PAGE_SHIFT;
 
 	for (row_index = 0; row_index < mci->nr_csrows; row_index++) {
-		csrow = &mci->csrows[row_index];
+		csrow = mci->csrows[row_index];
 		if ((pfn >= csrow->first_page) && (pfn <= csrow->last_page))
 			break;
 	}
@@ -854,16 +866,16 @@ static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 		mpc85xx_mc_printk(mci, KERN_ERR, "PFN out of range!\n");
 
 	if (err_detect & DDR_EDE_SBE)
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     pfn, err_addr & ~PAGE_MASK, syndrome,
 				     row_index, 0, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 
 	if (err_detect & DDR_EDE_MBE)
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     pfn, err_addr & ~PAGE_MASK, syndrome,
 				     row_index, 0, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 
 	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, err_detect);
 }
@@ -933,8 +945,8 @@ static void __devinit mpc85xx_init_csrows(struct mem_ctl_info *mci)
 		u32 start;
 		u32 end;
 
-		csrow = &mci->csrows[index];
-		dimm = csrow->channels[0].dimm;
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
 
 		cs_bnds = in_be32(pdata->mc_vbase + MPC85XX_MC_CS_BNDS_0 +
 				  (index * MPC85XX_MC_CS_BNDS_OFS));
@@ -990,9 +1002,9 @@ static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 	pdata = mci->pvt_info;
 	pdata->name = "mpc85xx_mc_err";
 	pdata->irq = NO_IRQ;
-	mci->dev = &op->dev;
+	mci->pdev = &op->dev;
 	pdata->edac_idx = edac_mc_idx++;
-	dev_set_drvdata(mci->dev, mci);
+	dev_set_drvdata(mci->pdev, mci);
 	mci->ctl_name = pdata->name;
 	mci->dev_name = pdata->name;
 
@@ -1026,7 +1038,7 @@ static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 		goto err;
 	}
 
-	debugf3("%s(): init mci\n", __func__);
+	edac_dbg(3, "init mci\n");
 	mci->mtype_cap = MEM_FLAG_RDDR | MEM_FLAG_RDDR2 |
 	    MEM_FLAG_DDR | MEM_FLAG_DDR2;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
@@ -1041,8 +1053,6 @@ static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 
 	mci->scrub_mode = SCRUB_SW_SRC;
 
-	mpc85xx_set_mc_sysfs_attributes(mci);
-
 	mpc85xx_init_csrows(mci);
 
 	/* store the original error disable bits */
@@ -1054,7 +1064,13 @@ static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_DETECT, ~0);
 
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
+		goto err;
+	}
+
+	if (mpc85xx_create_sysfs_attributes(mci)) {
+		edac_mc_del_mc(mci->pdev);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto err;
 	}
 
@@ -1088,7 +1104,7 @@ static int __devinit mpc85xx_mc_err_probe(struct platform_device *op)
 	}
 
 	devres_remove_group(&op->dev, mpc85xx_mc_err_probe);
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	printk(KERN_INFO EDAC_MOD_STR " MC err registered\n");
 
 	return 0;
@@ -1106,7 +1122,7 @@ static int mpc85xx_mc_err_remove(struct platform_device *op)
 	struct mem_ctl_info *mci = dev_get_drvdata(&op->dev);
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (edac_op_state == EDAC_OPSTATE_INT) {
 		out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_INT_EN, 0);
@@ -1117,6 +1133,7 @@ static int mpc85xx_mc_err_remove(struct platform_device *op)
 		 orig_ddr_err_disable);
 	out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_SBE, orig_ddr_err_sbe);
 
+	mpc85xx_remove_sysfs_attributes(mci);
 	edac_mc_del_mc(&op->dev);
 	edac_mc_free(mci);
 	return 0;
diff --git a/drivers/edac/mv64x60_edac.c b/drivers/edac/mv64x60_edac.c
index b0bb5a3d252..2b315c2edc3 100644
--- a/drivers/edac/mv64x60_edac.c
+++ b/drivers/edac/mv64x60_edac.c
@@ -169,7 +169,7 @@ static int __devinit mv64x60_pci_err_probe(struct platform_device *pdev)
 		 MV64X60_PCIx_ERR_MASK_VAL);
 
 	if (edac_pci_add_device(pci, pdata->edac_idx) > 0) {
-		debugf3("%s(): failed edac_pci_add_device()\n", __func__);
+		edac_dbg(3, "failed edac_pci_add_device()\n");
 		goto err;
 	}
 
@@ -194,7 +194,7 @@ static int __devinit mv64x60_pci_err_probe(struct platform_device *pdev)
 	devres_remove_group(&pdev->dev, mv64x60_pci_err_probe);
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 
 	return 0;
 
@@ -210,7 +210,7 @@ static int mv64x60_pci_err_remove(struct platform_device *pdev)
 {
 	struct edac_pci_ctl_info *pci = platform_get_drvdata(pdev);
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	edac_pci_del_device(&pdev->dev);
 
@@ -336,7 +336,7 @@ static int __devinit mv64x60_sram_err_probe(struct platform_device *pdev)
 	pdata->edac_idx = edac_dev_idx++;
 
 	if (edac_device_add_device(edac_dev) > 0) {
-		debugf3("%s(): failed edac_device_add_device()\n", __func__);
+		edac_dbg(3, "failed edac_device_add_device()\n");
 		goto err;
 	}
 
@@ -363,7 +363,7 @@ static int __devinit mv64x60_sram_err_probe(struct platform_device *pdev)
 	devres_remove_group(&pdev->dev, mv64x60_sram_err_probe);
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 
 	return 0;
 
@@ -379,7 +379,7 @@ static int mv64x60_sram_err_remove(struct platform_device *pdev)
 {
 	struct edac_device_ctl_info *edac_dev = platform_get_drvdata(pdev);
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	edac_device_del_device(&pdev->dev);
 	edac_device_free_ctl_info(edac_dev);
@@ -531,7 +531,7 @@ static int __devinit mv64x60_cpu_err_probe(struct platform_device *pdev)
 	pdata->edac_idx = edac_dev_idx++;
 
 	if (edac_device_add_device(edac_dev) > 0) {
-		debugf3("%s(): failed edac_device_add_device()\n", __func__);
+		edac_dbg(3, "failed edac_device_add_device()\n");
 		goto err;
 	}
 
@@ -558,7 +558,7 @@ static int __devinit mv64x60_cpu_err_probe(struct platform_device *pdev)
 	devres_remove_group(&pdev->dev, mv64x60_cpu_err_probe);
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 
 	return 0;
 
@@ -574,7 +574,7 @@ static int mv64x60_cpu_err_remove(struct platform_device *pdev)
 {
 	struct edac_device_ctl_info *edac_dev = platform_get_drvdata(pdev);
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	edac_device_del_device(&pdev->dev);
 	edac_device_free_ctl_info(edac_dev);
@@ -611,17 +611,17 @@ static void mv64x60_mc_check(struct mem_ctl_info *mci)
 
 	/* first bit clear in ECC Err Reg, 1 bit error, correctable by HW */
 	if (!(reg & 0x1))
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     err_addr >> PAGE_SHIFT,
 				     err_addr & PAGE_MASK, syndrome,
 				     0, 0, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 	else	/* 2 bit error, UE */
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 				     err_addr >> PAGE_SHIFT,
 				     err_addr & PAGE_MASK, 0,
 				     0, 0, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 
 	/* clear the error */
 	out_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ADDR, 0);
@@ -670,8 +670,8 @@ static void mv64x60_init_csrows(struct mem_ctl_info *mci,
 
 	ctl = in_le32(pdata->mc_vbase + MV64X60_SDRAM_CONFIG);
 
-	csrow = &mci->csrows[0];
-	dimm = csrow->channels[0].dimm;
+	csrow = mci->csrows[0];
+	dimm = csrow->channels[0]->dimm;
 
 	dimm->nr_pages = pdata->total_mem >> PAGE_SHIFT;
 	dimm->grain = 8;
@@ -724,7 +724,7 @@ static int __devinit mv64x60_mc_err_probe(struct platform_device *pdev)
 	}
 
 	pdata = mci->pvt_info;
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	platform_set_drvdata(pdev, mci);
 	pdata->name = "mv64x60_mc_err";
 	pdata->irq = NO_IRQ;
@@ -766,7 +766,7 @@ static int __devinit mv64x60_mc_err_probe(struct platform_device *pdev)
 		goto err2;
 	}
 
-	debugf3("%s(): init mci\n", __func__);
+	edac_dbg(3, "init mci\n");
 	mci->mtype_cap = MEM_FLAG_RDDR | MEM_FLAG_DDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
 	mci->edac_cap = EDAC_FLAG_SECDED;
@@ -790,7 +790,7 @@ static int __devinit mv64x60_mc_err_probe(struct platform_device *pdev)
 	out_le32(pdata->mc_vbase + MV64X60_SDRAM_ERR_ECC_CNTL, ctl);
 
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto err;
 	}
 
@@ -815,7 +815,7 @@ static int __devinit mv64x60_mc_err_probe(struct platform_device *pdev)
 	}
 
 	/* get this far and it's successful */
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 
 	return 0;
 
@@ -831,7 +831,7 @@ static int mv64x60_mc_err_remove(struct platform_device *pdev)
 {
 	struct mem_ctl_info *mci = platform_get_drvdata(pdev);
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	edac_mc_del_mc(&pdev->dev);
 	edac_mc_free(mci);
diff --git a/drivers/edac/pasemi_edac.c b/drivers/edac/pasemi_edac.c
index b095a906a99..2d35b78ada3 100644
--- a/drivers/edac/pasemi_edac.c
+++ b/drivers/edac/pasemi_edac.c
@@ -74,7 +74,7 @@ static int system_mmc_id;
 
 static u32 pasemi_edac_get_error_info(struct mem_ctl_info *mci)
 {
-	struct pci_dev *pdev = to_pci_dev(mci->dev);
+	struct pci_dev *pdev = to_pci_dev(mci->pdev);
 	u32 tmp;
 
 	pci_read_config_dword(pdev, MCDEBUG_ERRSTA,
@@ -95,7 +95,7 @@ static u32 pasemi_edac_get_error_info(struct mem_ctl_info *mci)
 
 static void pasemi_edac_process_error_info(struct mem_ctl_info *mci, u32 errsta)
 {
-	struct pci_dev *pdev = to_pci_dev(mci->dev);
+	struct pci_dev *pdev = to_pci_dev(mci->pdev);
 	u32 errlog1a;
 	u32 cs;
 
@@ -110,16 +110,16 @@ static void pasemi_edac_process_error_info(struct mem_ctl_info *mci, u32 errsta)
 	/* uncorrectable/multi-bit errors */
 	if (errsta & (MCDEBUG_ERRSTA_MBE_STATUS |
 		      MCDEBUG_ERRSTA_RFL_STATUS)) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
-				     mci->csrows[cs].first_page, 0, 0,
-				     cs, 0, -1, mci->ctl_name, "", NULL);
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
+				     mci->csrows[cs]->first_page, 0, 0,
+				     cs, 0, -1, mci->ctl_name, "");
 	}
 
 	/* correctable/single-bit errors */
 	if (errsta & MCDEBUG_ERRSTA_SBE_STATUS)
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
-				     mci->csrows[cs].first_page, 0, 0,
-				     cs, 0, -1, mci->ctl_name, "", NULL);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
+				     mci->csrows[cs]->first_page, 0, 0,
+				     cs, 0, -1, mci->ctl_name, "");
 }
 
 static void pasemi_edac_check(struct mem_ctl_info *mci)
@@ -141,8 +141,8 @@ static int pasemi_edac_init_csrows(struct mem_ctl_info *mci,
 	int index;
 
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
-		dimm = csrow->channels[0].dimm;
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
 
 		pci_read_config_dword(pdev,
 				      MCDRAM_RANKCFG + (index * 12),
@@ -225,7 +225,7 @@ static int __devinit pasemi_edac_probe(struct pci_dev *pdev,
 		MCCFG_ERRCOR_ECC_GEN_EN |
 		MCCFG_ERRCOR_ECC_CRR_EN;
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR | MEM_FLAG_RDDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_EC | EDAC_FLAG_SECDED;
 	mci->edac_cap = (errcor & MCCFG_ERRCOR_ECC_GEN_EN) ?
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index f3f9fed06ad..bf095763599 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -727,10 +727,10 @@ ppc4xx_edac_handle_ce(struct mem_ctl_info *mci,
 
 	for (row = 0; row < mci->nr_csrows; row++)
 		if (ppc4xx_edac_check_bank_error(status, row))
-			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 					     0, 0, 0,
 					     row, 0, -1,
-					     message, "", NULL);
+					     message, "");
 }
 
 /**
@@ -758,10 +758,10 @@ ppc4xx_edac_handle_ue(struct mem_ctl_info *mci,
 
 	for (row = 0; row < mci->nr_csrows; row++)
 		if (ppc4xx_edac_check_bank_error(status, row))
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					     page, offset, 0,
 					     row, 0, -1,
-					     message, "", NULL);
+					     message, "");
 }
 
 /**
@@ -1027,9 +1027,9 @@ ppc4xx_edac_mc_init(struct mem_ctl_info *mci,
 
 	/* Initial driver pointers and private data */
 
-	mci->dev		= &op->dev;
+	mci->pdev		= &op->dev;
 
-	dev_set_drvdata(mci->dev, mci);
+	dev_set_drvdata(mci->pdev, mci);
 
 	pdata			= mci->pvt_info;
 
@@ -1334,7 +1334,7 @@ static int __devinit ppc4xx_edac_probe(struct platform_device *op)
 	return 0;
 
  fail1:
-	edac_mc_del_mc(mci->dev);
+	edac_mc_del_mc(mci->pdev);
 
  fail:
 	edac_mc_free(mci);
@@ -1368,7 +1368,7 @@ ppc4xx_edac_remove(struct platform_device *op)
 
 	dcr_unmap(pdata->dcr_host, SDRAM_DCR_RESOURCE_LEN);
 
-	edac_mc_del_mc(mci->dev);
+	edac_mc_del_mc(mci->pdev);
 	edac_mc_free(mci);
 
 	return 0;
diff --git a/drivers/edac/r82600_edac.c b/drivers/edac/r82600_edac.c
index e1cacd164f3..f854debd553 100644
--- a/drivers/edac/r82600_edac.c
+++ b/drivers/edac/r82600_edac.c
@@ -140,7 +140,7 @@ static void r82600_get_error_info(struct mem_ctl_info *mci,
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 	pci_read_config_dword(pdev, R82600_EAP, &info->eapr);
 
 	if (info->eapr & BIT(0))
@@ -179,11 +179,11 @@ static int r82600_process_error_info(struct mem_ctl_info *mci,
 		error_found = 1;
 
 		if (handle_errors)
-			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 					     page, 0, syndrome,
 					     edac_mc_find_csrow_by_page(mci, page),
 					     0, -1,
-					     mci->ctl_name, "", NULL);
+					     mci->ctl_name, "");
 	}
 
 	if (info->eapr & BIT(1)) {	/* UE? */
@@ -191,11 +191,11 @@ static int r82600_process_error_info(struct mem_ctl_info *mci,
 
 		if (handle_errors)
 			/* 82600 doesn't give enough info */
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					     page, 0, 0,
 					     edac_mc_find_csrow_by_page(mci, page),
 					     0, -1,
-					     mci->ctl_name, "", NULL);
+					     mci->ctl_name, "");
 	}
 
 	return error_found;
@@ -205,7 +205,7 @@ static void r82600_check(struct mem_ctl_info *mci)
 {
 	struct r82600_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	r82600_get_error_info(mci, &info);
 	r82600_process_error_info(mci, &info, 1);
 }
@@ -230,19 +230,19 @@ static void r82600_init_csrows(struct mem_ctl_info *mci, struct pci_dev *pdev,
 	row_high_limit_last = 0;
 
 	for (index = 0; index < mci->nr_csrows; index++) {
-		csrow = &mci->csrows[index];
-		dimm = csrow->channels[0].dimm;
+		csrow = mci->csrows[index];
+		dimm = csrow->channels[0]->dimm;
 
 		/* find the DRAM Chip Select Base address and mask */
 		pci_read_config_byte(pdev, R82600_DRBA + index, &drbar);
 
-		debugf1("%s() Row=%d DRBA = %#0x\n", __func__, index, drbar);
+		edac_dbg(1, "Row=%d DRBA = %#0x\n", index, drbar);
 
 		row_high_limit = ((u32) drbar << 24);
 /*		row_high_limit = ((u32)drbar << 24) | 0xffffffUL; */
 
-		debugf1("%s() Row=%d, Boundary Address=%#0x, Last = %#0x\n",
-			__func__, index, row_high_limit, row_high_limit_last);
+		edac_dbg(1, "Row=%d, Boundary Address=%#0x, Last = %#0x\n",
+			 index, row_high_limit, row_high_limit_last);
 
 		/* Empty row [p.57] */
 		if (row_high_limit == row_high_limit_last)
@@ -277,14 +277,13 @@ static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 	u32 sdram_refresh_rate;
 	struct r82600_error_info discard;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 	pci_read_config_byte(pdev, R82600_DRAMC, &dramcr);
 	pci_read_config_dword(pdev, R82600_EAP, &eapr);
 	scrub_disabled = eapr & BIT(31);
 	sdram_refresh_rate = dramcr & (BIT(0) | BIT(1));
-	debugf2("%s(): sdram refresh rate = %#0x\n", __func__,
-		sdram_refresh_rate);
-	debugf2("%s(): DRAMC register = %#0x\n", __func__, dramcr);
+	edac_dbg(2, "sdram refresh rate = %#0x\n", sdram_refresh_rate);
+	edac_dbg(2, "DRAMC register = %#0x\n", dramcr);
 	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
 	layers[0].size = R82600_NR_CSROWS;
 	layers[0].is_virt_csrow = true;
@@ -295,8 +294,8 @@ static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 	if (mci == NULL)
 		return -ENOMEM;
 
-	debugf0("%s(): mci = %p\n", __func__, mci);
-	mci->dev = &pdev->dev;
+	edac_dbg(0, "mci = %p\n", mci);
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_RDDR | MEM_FLAG_DDR;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_EC | EDAC_FLAG_SECDED;
 	/* FIXME try to work out if the chip leads have been used for COM2
@@ -311,8 +310,8 @@ static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 
 	if (ecc_enabled(dramcr)) {
 		if (scrub_disabled)
-			debugf3("%s(): mci = %p - Scrubbing disabled! EAP: "
-				"%#0x\n", __func__, mci, eapr);
+			edac_dbg(3, "mci = %p - Scrubbing disabled! EAP: %#0x\n",
+				 mci, eapr);
 	} else
 		mci->edac_cap = EDAC_FLAG_NONE;
 
@@ -329,15 +328,14 @@ static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 	 * type of memory controller.  The ID is therefore hardcoded to 0.
 	 */
 	if (edac_mc_add_mc(mci)) {
-		debugf3("%s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
 	/* get this far and it's successful */
 
 	if (disable_hardware_scrub) {
-		debugf3("%s(): Disabling Hardware Scrub (scrub on error)\n",
-			__func__);
+		edac_dbg(3, "Disabling Hardware Scrub (scrub on error)\n");
 		pci_write_bits32(pdev, R82600_EAP, BIT(31), BIT(31));
 	}
 
@@ -352,7 +350,7 @@ static int r82600_probe1(struct pci_dev *pdev, int dev_idx)
 			__func__);
 	}
 
-	debugf3("%s(): success\n", __func__);
+	edac_dbg(3, "success\n");
 	return 0;
 
 fail:
@@ -364,7 +362,7 @@ fail:
 static int __devinit r82600_init_one(struct pci_dev *pdev,
 				const struct pci_device_id *ent)
 {
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/* don't need to call pci_enable_device() */
 	return r82600_probe1(pdev, ent->driver_data);
@@ -374,7 +372,7 @@ static void __devexit r82600_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	if (r82600_pci)
 		edac_pci_release_generic_ctl(r82600_pci);
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index 36ad17e79d6..f3b1f9fafa4 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -381,8 +381,8 @@ static inline int numrank(u32 mtr)
 	int ranks = (1 << RANK_CNT_BITS(mtr));
 
 	if (ranks > 4) {
-		debugf0("Invalid number of ranks: %d (max = 4) raw value = %x (%04x)",
-			ranks, (unsigned int)RANK_CNT_BITS(mtr), mtr);
+		edac_dbg(0, "Invalid number of ranks: %d (max = 4) raw value = %x (%04x)\n",
+			 ranks, (unsigned int)RANK_CNT_BITS(mtr), mtr);
 		return -EINVAL;
 	}
 
@@ -394,8 +394,8 @@ static inline int numrow(u32 mtr)
 	int rows = (RANK_WIDTH_BITS(mtr) + 12);
 
 	if (rows < 13 || rows > 18) {
-		debugf0("Invalid number of rows: %d (should be between 14 and 17) raw value = %x (%04x)",
-			rows, (unsigned int)RANK_WIDTH_BITS(mtr), mtr);
+		edac_dbg(0, "Invalid number of rows: %d (should be between 14 and 17) raw value = %x (%04x)\n",
+			 rows, (unsigned int)RANK_WIDTH_BITS(mtr), mtr);
 		return -EINVAL;
 	}
 
@@ -407,8 +407,8 @@ static inline int numcol(u32 mtr)
 	int cols = (COL_WIDTH_BITS(mtr) + 10);
 
 	if (cols > 12) {
-		debugf0("Invalid number of cols: %d (max = 4) raw value = %x (%04x)",
-			cols, (unsigned int)COL_WIDTH_BITS(mtr), mtr);
+		edac_dbg(0, "Invalid number of cols: %d (max = 4) raw value = %x (%04x)\n",
+			 cols, (unsigned int)COL_WIDTH_BITS(mtr), mtr);
 		return -EINVAL;
 	}
 
@@ -475,8 +475,8 @@ static struct pci_dev *get_pdev_slot_func(u8 bus, unsigned slot,
 
 		if (PCI_SLOT(sbridge_dev->pdev[i]->devfn) == slot &&
 		    PCI_FUNC(sbridge_dev->pdev[i]->devfn) == func) {
-			debugf1("Associated %02x.%02x.%d with %p\n",
-				bus, slot, func, sbridge_dev->pdev[i]);
+			edac_dbg(1, "Associated %02x.%02x.%d with %p\n",
+				 bus, slot, func, sbridge_dev->pdev[i]);
 			return sbridge_dev->pdev[i];
 		}
 	}
@@ -523,45 +523,45 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 
 	pci_read_config_dword(pvt->pci_br, SAD_CONTROL, &reg);
 	pvt->sbridge_dev->node_id = NODE_ID(reg);
-	debugf0("mc#%d: Node ID: %d, source ID: %d\n",
-		pvt->sbridge_dev->mc,
-		pvt->sbridge_dev->node_id,
-		pvt->sbridge_dev->source_id);
+	edac_dbg(0, "mc#%d: Node ID: %d, source ID: %d\n",
+		 pvt->sbridge_dev->mc,
+		 pvt->sbridge_dev->node_id,
+		 pvt->sbridge_dev->source_id);
 
 	pci_read_config_dword(pvt->pci_ras, RASENABLES, &reg);
 	if (IS_MIRROR_ENABLED(reg)) {
-		debugf0("Memory mirror is enabled\n");
+		edac_dbg(0, "Memory mirror is enabled\n");
 		pvt->is_mirrored = true;
 	} else {
-		debugf0("Memory mirror is disabled\n");
+		edac_dbg(0, "Memory mirror is disabled\n");
 		pvt->is_mirrored = false;
 	}
 
 	pci_read_config_dword(pvt->pci_ta, MCMTR, &pvt->info.mcmtr);
 	if (IS_LOCKSTEP_ENABLED(pvt->info.mcmtr)) {
-		debugf0("Lockstep is enabled\n");
+		edac_dbg(0, "Lockstep is enabled\n");
 		mode = EDAC_S8ECD8ED;
 		pvt->is_lockstep = true;
 	} else {
-		debugf0("Lockstep is disabled\n");
+		edac_dbg(0, "Lockstep is disabled\n");
 		mode = EDAC_S4ECD4ED;
 		pvt->is_lockstep = false;
 	}
 	if (IS_CLOSE_PG(pvt->info.mcmtr)) {
-		debugf0("address map is on closed page mode\n");
+		edac_dbg(0, "address map is on closed page mode\n");
 		pvt->is_close_pg = true;
 	} else {
-		debugf0("address map is on open page mode\n");
+		edac_dbg(0, "address map is on open page mode\n");
 		pvt->is_close_pg = false;
 	}
 
 	pci_read_config_dword(pvt->pci_ddrio, RANK_CFG_A, &reg);
 	if (IS_RDIMM_ENABLED(reg)) {
 		/* FIXME: Can also be LRDIMM */
-		debugf0("Memory is registered\n");
+		edac_dbg(0, "Memory is registered\n");
 		mtype = MEM_RDDR3;
 	} else {
-		debugf0("Memory is unregistered\n");
+		edac_dbg(0, "Memory is unregistered\n");
 		mtype = MEM_DDR3;
 	}
 
@@ -576,7 +576,7 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				       i, j, 0);
 			pci_read_config_dword(pvt->pci_tad[i],
 					      mtr_regs[j], &mtr);
-			debugf4("Channel #%d  MTR%d = %x\n", i, j, mtr);
+			edac_dbg(4, "Channel #%d  MTR%d = %x\n", i, j, mtr);
 			if (IS_DIMM_PRESENT(mtr)) {
 				pvt->channel[i].dimms++;
 
@@ -588,10 +588,10 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				size = (rows * cols * banks * ranks) >> (20 - 3);
 				npages = MiB_TO_PAGES(size);
 
-				debugf0("mc#%d: channel %d, dimm %d, %d Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
-					pvt->sbridge_dev->mc, i, j,
-					size, npages,
-					banks, ranks, rows, cols);
+				edac_dbg(0, "mc#%d: channel %d, dimm %d, %d Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
+					 pvt->sbridge_dev->mc, i, j,
+					 size, npages,
+					 banks, ranks, rows, cols);
 
 				dimm->nr_pages = npages;
 				dimm->grain = 32;
@@ -629,8 +629,7 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 	tmp_mb = (1 + pvt->tolm) >> 20;
 
 	mb = div_u64_rem(tmp_mb, 1000, &kb);
-	debugf0("TOLM: %u.%03u GB (0x%016Lx)\n",
-		mb, kb, (u64)pvt->tolm);
+	edac_dbg(0, "TOLM: %u.%03u GB (0x%016Lx)\n", mb, kb, (u64)pvt->tolm);
 
 	/* Address range is already 45:25 */
 	pci_read_config_dword(pvt->pci_sad1, TOHM,
@@ -639,8 +638,7 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 	tmp_mb = (1 + pvt->tohm) >> 20;
 
 	mb = div_u64_rem(tmp_mb, 1000, &kb);
-	debugf0("TOHM: %u.%03u GB (0x%016Lx)",
-		mb, kb, (u64)pvt->tohm);
+	edac_dbg(0, "TOHM: %u.%03u GB (0x%016Lx)", mb, kb, (u64)pvt->tohm);
 
 	/*
 	 * Step 2) Get SAD range and SAD Interleave list
@@ -663,13 +661,13 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 
 		tmp_mb = (limit + 1) >> 20;
 		mb = div_u64_rem(tmp_mb, 1000, &kb);
-		debugf0("SAD#%d %s up to %u.%03u GB (0x%016Lx) %s reg=0x%08x\n",
-			n_sads,
-			get_dram_attr(reg),
-			mb, kb,
-			((u64)tmp_mb) << 20L,
-			INTERLEAVE_MODE(reg) ? "Interleave: 8:6" : "Interleave: [8:6]XOR[18:16]",
-			reg);
+		edac_dbg(0, "SAD#%d %s up to %u.%03u GB (0x%016Lx) Interleave: %s reg=0x%08x\n",
+			 n_sads,
+			 get_dram_attr(reg),
+			 mb, kb,
+			 ((u64)tmp_mb) << 20L,
+			 INTERLEAVE_MODE(reg) ? "8:6" : "[8:6]XOR[18:16]",
+			 reg);
 		prv = limit;
 
 		pci_read_config_dword(pvt->pci_sad0, interleave_list[n_sads],
@@ -679,8 +677,8 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 			if (j > 0 && sad_interl == sad_pkg(reg, j))
 				break;
 
-			debugf0("SAD#%d, interleave #%d: %d\n",
-			n_sads, j, sad_pkg(reg, j));
+			edac_dbg(0, "SAD#%d, interleave #%d: %d\n",
+				 n_sads, j, sad_pkg(reg, j));
 		}
 	}
 
@@ -697,16 +695,16 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 		tmp_mb = (limit + 1) >> 20;
 
 		mb = div_u64_rem(tmp_mb, 1000, &kb);
-		debugf0("TAD#%d: up to %u.%03u GB (0x%016Lx), socket interleave %d, memory interleave %d, TGT: %d, %d, %d, %d, reg=0x%08x\n",
-			n_tads, mb, kb,
-			((u64)tmp_mb) << 20L,
-			(u32)TAD_SOCK(reg),
-			(u32)TAD_CH(reg),
-			(u32)TAD_TGT0(reg),
-			(u32)TAD_TGT1(reg),
-			(u32)TAD_TGT2(reg),
-			(u32)TAD_TGT3(reg),
-			reg);
+		edac_dbg(0, "TAD#%d: up to %u.%03u GB (0x%016Lx), socket interleave %d, memory interleave %d, TGT: %d, %d, %d, %d, reg=0x%08x\n",
+			 n_tads, mb, kb,
+			 ((u64)tmp_mb) << 20L,
+			 (u32)TAD_SOCK(reg),
+			 (u32)TAD_CH(reg),
+			 (u32)TAD_TGT0(reg),
+			 (u32)TAD_TGT1(reg),
+			 (u32)TAD_TGT2(reg),
+			 (u32)TAD_TGT3(reg),
+			 reg);
 		prv = limit;
 	}
 
@@ -722,11 +720,11 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 					      &reg);
 			tmp_mb = TAD_OFFSET(reg) >> 20;
 			mb = div_u64_rem(tmp_mb, 1000, &kb);
-			debugf0("TAD CH#%d, offset #%d: %u.%03u GB (0x%016Lx), reg=0x%08x\n",
-				i, j,
-				mb, kb,
-				((u64)tmp_mb) << 20L,
-				reg);
+			edac_dbg(0, "TAD CH#%d, offset #%d: %u.%03u GB (0x%016Lx), reg=0x%08x\n",
+				 i, j,
+				 mb, kb,
+				 ((u64)tmp_mb) << 20L,
+				 reg);
 		}
 	}
 
@@ -747,12 +745,12 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 			tmp_mb = RIR_LIMIT(reg) >> 20;
 			rir_way = 1 << RIR_WAY(reg);
 			mb = div_u64_rem(tmp_mb, 1000, &kb);
-			debugf0("CH#%d RIR#%d, limit: %u.%03u GB (0x%016Lx), way: %d, reg=0x%08x\n",
-				i, j,
-				mb, kb,
-				((u64)tmp_mb) << 20L,
-				rir_way,
-				reg);
+			edac_dbg(0, "CH#%d RIR#%d, limit: %u.%03u GB (0x%016Lx), way: %d, reg=0x%08x\n",
+				 i, j,
+				 mb, kb,
+				 ((u64)tmp_mb) << 20L,
+				 rir_way,
+				 reg);
 
 			for (k = 0; k < rir_way; k++) {
 				pci_read_config_dword(pvt->pci_tad[i],
@@ -761,12 +759,12 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 				tmp_mb = RIR_OFFSET(reg) << 6;
 
 				mb = div_u64_rem(tmp_mb, 1000, &kb);
-				debugf0("CH#%d RIR#%d INTL#%d, offset %u.%03u GB (0x%016Lx), tgt: %d, reg=0x%08x\n",
-					i, j, k,
-					mb, kb,
-					((u64)tmp_mb) << 20L,
-					(u32)RIR_RNK_TGT(reg),
-					reg);
+				edac_dbg(0, "CH#%d RIR#%d INTL#%d, offset %u.%03u GB (0x%016Lx), tgt: %d, reg=0x%08x\n",
+					 i, j, k,
+					 mb, kb,
+					 ((u64)tmp_mb) << 20L,
+					 (u32)RIR_RNK_TGT(reg),
+					 reg);
 			}
 		}
 	}
@@ -853,16 +851,16 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		if (sad_way > 0 && sad_interl == sad_pkg(reg, sad_way))
 			break;
 		sad_interleave[sad_way] = sad_pkg(reg, sad_way);
-		debugf0("SAD interleave #%d: %d\n",
-			sad_way, sad_interleave[sad_way]);
+		edac_dbg(0, "SAD interleave #%d: %d\n",
+			 sad_way, sad_interleave[sad_way]);
 	}
-	debugf0("mc#%d: Error detected on SAD#%d: address 0x%016Lx < 0x%016Lx, Interleave [%d:6]%s\n",
-		pvt->sbridge_dev->mc,
-		n_sads,
-		addr,
-		limit,
-		sad_way + 7,
-		interleave_mode ? "" : "XOR[18:16]");
+	edac_dbg(0, "mc#%d: Error detected on SAD#%d: address 0x%016Lx < 0x%016Lx, Interleave [%d:6]%s\n",
+		 pvt->sbridge_dev->mc,
+		 n_sads,
+		 addr,
+		 limit,
+		 sad_way + 7,
+		 interleave_mode ? "" : "XOR[18:16]");
 	if (interleave_mode)
 		idx = ((addr >> 6) ^ (addr >> 16)) & 7;
 	else
@@ -884,8 +882,8 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		return -EINVAL;
 	}
 	*socket = sad_interleave[idx];
-	debugf0("SAD interleave index: %d (wayness %d) = CPU socket %d\n",
-		idx, sad_way, *socket);
+	edac_dbg(0, "SAD interleave index: %d (wayness %d) = CPU socket %d\n",
+		 idx, sad_way, *socket);
 
 	/*
 	 * Move to the proper node structure, in order to access the
@@ -972,16 +970,16 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 
 	offset = TAD_OFFSET(tad_offset);
 
-	debugf0("TAD#%d: address 0x%016Lx < 0x%016Lx, socket interleave %d, channel interleave %d (offset 0x%08Lx), index %d, base ch: %d, ch mask: 0x%02lx\n",
-		n_tads,
-		addr,
-		limit,
-		(u32)TAD_SOCK(reg),
-		ch_way,
-		offset,
-		idx,
-		base_ch,
-		*channel_mask);
+	edac_dbg(0, "TAD#%d: address 0x%016Lx < 0x%016Lx, socket interleave %d, channel interleave %d (offset 0x%08Lx), index %d, base ch: %d, ch mask: 0x%02lx\n",
+		 n_tads,
+		 addr,
+		 limit,
+		 (u32)TAD_SOCK(reg),
+		 ch_way,
+		 offset,
+		 idx,
+		 base_ch,
+		 *channel_mask);
 
 	/* Calculate channel address */
 	/* Remove the TAD offset */
@@ -1017,11 +1015,11 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 
 		limit = RIR_LIMIT(reg);
 		mb = div_u64_rem(limit >> 20, 1000, &kb);
-		debugf0("RIR#%d, limit: %u.%03u GB (0x%016Lx), way: %d\n",
-			n_rir,
-			mb, kb,
-			limit,
-			1 << RIR_WAY(reg));
+		edac_dbg(0, "RIR#%d, limit: %u.%03u GB (0x%016Lx), way: %d\n",
+			 n_rir,
+			 mb, kb,
+			 limit,
+			 1 << RIR_WAY(reg));
 		if  (ch_addr <= limit)
 			break;
 	}
@@ -1042,12 +1040,12 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 			      &reg);
 	*rank = RIR_RNK_TGT(reg);
 
-	debugf0("RIR#%d: channel address 0x%08Lx < 0x%08Lx, RIR interleave %d, index %d\n",
-		n_rir,
-		ch_addr,
-		limit,
-		rir_way,
-		idx);
+	edac_dbg(0, "RIR#%d: channel address 0x%08Lx < 0x%08Lx, RIR interleave %d, index %d\n",
+		 n_rir,
+		 ch_addr,
+		 limit,
+		 rir_way,
+		 idx);
 
 	return 0;
 }
@@ -1064,14 +1062,14 @@ static void sbridge_put_devices(struct sbridge_dev *sbridge_dev)
 {
 	int i;
 
-	debugf0(__FILE__ ": %s()\n", __func__);
+	edac_dbg(0, "\n");
 	for (i = 0; i < sbridge_dev->n_devs; i++) {
 		struct pci_dev *pdev = sbridge_dev->pdev[i];
 		if (!pdev)
 			continue;
-		debugf0("Removing dev %02x:%02x.%d\n",
-			pdev->bus->number,
-			PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+		edac_dbg(0, "Removing dev %02x:%02x.%d\n",
+			 pdev->bus->number,
+			 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 		pci_dev_put(pdev);
 	}
 }
@@ -1177,10 +1175,9 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
 		return -ENODEV;
 	}
 
-	debugf0("Detected dev %02x:%d.%d PCI ID %04x:%04x\n",
-		bus, dev_descr->dev,
-		dev_descr->func,
-		PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
+	edac_dbg(0, "Detected dev %02x:%d.%d PCI ID %04x:%04x\n",
+		 bus, dev_descr->dev, dev_descr->func,
+		 PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
 
 	/*
 	 * As stated on drivers/pci/search.c, the reference count for
@@ -1297,10 +1294,10 @@ static int mci_bind_devs(struct mem_ctl_info *mci,
 			goto error;
 		}
 
-		debugf0("Associated PCI %02x.%02d.%d with dev = %p\n",
-			sbridge_dev->bus,
-			PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn),
-			pdev);
+		edac_dbg(0, "Associated PCI %02x.%02d.%d with dev = %p\n",
+			 sbridge_dev->bus,
+			 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn),
+			 pdev);
 	}
 
 	/* Check if everything were registered */
@@ -1435,8 +1432,7 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	 * to the group of dimm's where the error may be happening.
 	 */
 	snprintf(msg, sizeof(msg),
-		 "count:%d%s%s area:%s err_code:%04x:%04x socket:%d channel_mask:%ld rank:%d",
-		 core_err_cnt,
+		 "%s%s area:%s err_code:%04x:%04x socket:%d channel_mask:%ld rank:%d",
 		 overflow ? " OVERFLOW" : "",
 		 (uncorrected_error && recoverable) ? " recoverable" : "",
 		 area_type,
@@ -1445,20 +1441,20 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 		 channel_mask,
 		 rank);
 
-	debugf0("%s", msg);
+	edac_dbg(0, "%s\n", msg);
 
 	/* FIXME: need support for channel mask */
 
 	/* Call the helper to output message */
-	edac_mc_handle_error(tp_event, mci,
+	edac_mc_handle_error(tp_event, mci, core_err_cnt,
 			     m->addr >> PAGE_SHIFT, m->addr & ~PAGE_MASK, 0,
 			     channel, dimm, -1,
-			     optype, msg, m);
+			     optype, msg);
 	return;
 err_parsing:
-	edac_mc_handle_error(tp_event, mci, 0, 0, 0,
+	edac_mc_handle_error(tp_event, mci, core_err_cnt, 0, 0, 0,
 			     -1, -1, -1,
-			     msg, "", m);
+			     msg, "");
 
 }
 
@@ -1592,8 +1588,7 @@ static void sbridge_unregister_mci(struct sbridge_dev *sbridge_dev)
 	struct sbridge_pvt *pvt;
 
 	if (unlikely(!mci || !mci->pvt_info)) {
-		debugf0("MC: " __FILE__ ": %s(): dev = %p\n",
-			__func__, &sbridge_dev->pdev[0]->dev);
+		edac_dbg(0, "MC: dev = %p\n", &sbridge_dev->pdev[0]->dev);
 
 		sbridge_printk(KERN_ERR, "Couldn't find mci handler\n");
 		return;
@@ -1601,13 +1596,13 @@ static void sbridge_unregister_mci(struct sbridge_dev *sbridge_dev)
 
 	pvt = mci->pvt_info;
 
-	debugf0("MC: " __FILE__ ": %s(): mci = %p, dev = %p\n",
-		__func__, mci, &sbridge_dev->pdev[0]->dev);
+	edac_dbg(0, "MC: mci = %p, dev = %p\n",
+		 mci, &sbridge_dev->pdev[0]->dev);
 
 	/* Remove MC sysfs nodes */
-	edac_mc_del_mc(mci->dev);
+	edac_mc_del_mc(mci->pdev);
 
-	debugf1("%s: free mci struct\n", mci->ctl_name);
+	edac_dbg(1, "%s: free mci struct\n", mci->ctl_name);
 	kfree(mci->ctl_name);
 	edac_mc_free(mci);
 	sbridge_dev->mci = NULL;
@@ -1638,8 +1633,8 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev)
 	if (unlikely(!mci))
 		return -ENOMEM;
 
-	debugf0("MC: " __FILE__ ": %s(): mci = %p, dev = %p\n",
-		__func__, mci, &sbridge_dev->pdev[0]->dev);
+	edac_dbg(0, "MC: mci = %p, dev = %p\n",
+		 mci, &sbridge_dev->pdev[0]->dev);
 
 	pvt = mci->pvt_info;
 	memset(pvt, 0, sizeof(*pvt));
@@ -1670,12 +1665,11 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev)
 	get_memory_layout(mci);
 
 	/* record ptr to the generic device */
-	mci->dev = &sbridge_dev->pdev[0]->dev;
+	mci->pdev = &sbridge_dev->pdev[0]->dev;
 
 	/* add this new MC control structure to EDAC's list of MCs */
 	if (unlikely(edac_mc_add_mc(mci))) {
-		debugf0("MC: " __FILE__
-			": %s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(0, "MC: failed edac_mc_add_mc()\n");
 		rc = -EINVAL;
 		goto fail0;
 	}
@@ -1722,7 +1716,8 @@ static int __devinit sbridge_probe(struct pci_dev *pdev,
 	mc = 0;
 
 	list_for_each_entry(sbridge_dev, &sbridge_edac_list, list) {
-		debugf0("Registering MC#%d (%d of %d)\n", mc, mc + 1, num_mc);
+		edac_dbg(0, "Registering MC#%d (%d of %d)\n",
+			 mc, mc + 1, num_mc);
 		sbridge_dev->mc = mc++;
 		rc = sbridge_register_mci(sbridge_dev);
 		if (unlikely(rc < 0))
@@ -1752,7 +1747,7 @@ static void __devexit sbridge_remove(struct pci_dev *pdev)
 {
 	struct sbridge_dev *sbridge_dev;
 
-	debugf0(__FILE__ ": %s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	/*
 	 * we have a trouble here: pdev value for removal will be wrong, since
@@ -1801,7 +1796,7 @@ static int __init sbridge_init(void)
 {
 	int pci_rc;
 
-	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	edac_dbg(2, "\n");
 
 	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
 	opstate_init();
@@ -1825,7 +1820,7 @@ static int __init sbridge_init(void)
  */
 static void __exit sbridge_exit(void)
 {
-	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	edac_dbg(2, "\n");
 	pci_unregister_driver(&sbridge_driver);
 	mce_unregister_decode_chain(&sbridge_mce_dec);
 }
diff --git a/drivers/edac/tile_edac.c b/drivers/edac/tile_edac.c
index 7bb4614730d..1e904b7b79a 100644
--- a/drivers/edac/tile_edac.c
+++ b/drivers/edac/tile_edac.c
@@ -69,12 +69,12 @@ static void tile_edac_check(struct mem_ctl_info *mci)
 
 	/* Check if the current error count is different from the saved one. */
 	if (mem_error.sbe_count != priv->ce_count) {
-		dev_dbg(mci->dev, "ECC CE err on node %d\n", priv->node);
+		dev_dbg(mci->pdev, "ECC CE err on node %d\n", priv->node);
 		priv->ce_count = mem_error.sbe_count;
-		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 				     0, 0, 0,
 				     0, 0, -1,
-				     mci->ctl_name, "", NULL);
+				     mci->ctl_name, "");
 	}
 }
 
@@ -84,10 +84,10 @@ static void tile_edac_check(struct mem_ctl_info *mci)
  */
 static int __devinit tile_edac_init_csrows(struct mem_ctl_info *mci)
 {
-	struct csrow_info	*csrow = &mci->csrows[0];
+	struct csrow_info	*csrow = mci->csrows[0];
 	struct tile_edac_priv	*priv = mci->pvt_info;
 	struct mshim_mem_info	mem_info;
-	struct dimm_info *dimm = csrow->channels[0].dimm;
+	struct dimm_info *dimm = csrow->channels[0]->dimm;
 
 	if (hv_dev_pread(priv->hv_devhdl, 0, (HV_VirtAddr)&mem_info,
 		sizeof(struct mshim_mem_info), MSHIM_MEM_INFO_OFF) !=
@@ -149,7 +149,7 @@ static int __devinit tile_edac_mc_probe(struct platform_device *pdev)
 	priv->node = pdev->id;
 	priv->hv_devhdl = hv_devhdl;
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR2;
 	mci->edac_ctl_cap = EDAC_FLAG_SECDED;
 
diff --git a/drivers/edac/x38_edac.c b/drivers/edac/x38_edac.c
index 1ac7962d63e..08a992693e6 100644
--- a/drivers/edac/x38_edac.c
+++ b/drivers/edac/x38_edac.c
@@ -103,10 +103,10 @@ static int how_many_channel(struct pci_dev *pdev)
 
 	pci_read_config_byte(pdev, X38_CAPID0 + 8, &capid0_8b);
 	if (capid0_8b & 0x20) {	/* check DCD: Dual Channel Disable */
-		debugf0("In single channel mode.\n");
+		edac_dbg(0, "In single channel mode\n");
 		x38_channel_num = 1;
 	} else {
-		debugf0("In dual channel mode.\n");
+		edac_dbg(0, "In dual channel mode\n");
 		x38_channel_num = 2;
 	}
 
@@ -151,7 +151,7 @@ static void x38_clear_error_info(struct mem_ctl_info *mci)
 {
 	struct pci_dev *pdev;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * Clear any error bits.
@@ -172,7 +172,7 @@ static void x38_get_and_clear_error_info(struct mem_ctl_info *mci,
 	struct pci_dev *pdev;
 	void __iomem *window = mci->pvt_info;
 
-	pdev = to_pci_dev(mci->dev);
+	pdev = to_pci_dev(mci->pdev);
 
 	/*
 	 * This is a mess because there is no atomic way to read all the
@@ -215,26 +215,26 @@ static void x38_process_error_info(struct mem_ctl_info *mci,
 		return;
 
 	if ((info->errsts ^ info->errsts2) & X38_ERRSTS_BITS) {
-		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 0, 0, 0,
+		edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1, 0, 0, 0,
 				     -1, -1, -1,
-				     "UE overwrote CE", "", NULL);
+				     "UE overwrote CE", "");
 		info->errsts = info->errsts2;
 	}
 
 	for (channel = 0; channel < x38_channel_num; channel++) {
 		log = info->eccerrlog[channel];
 		if (log & X38_ECCERRLOG_UE) {
-			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci, 1,
 					     0, 0, 0,
 					     eccerrlog_row(channel, log),
 					     -1, -1,
-					     "x38 UE", "", NULL);
+					     "x38 UE", "");
 		} else if (log & X38_ECCERRLOG_CE) {
-			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, 1,
 					     0, 0, eccerrlog_syndrome(log),
 					     eccerrlog_row(channel, log),
 					     -1, -1,
-					     "x38 CE", "", NULL);
+					     "x38 CE", "");
 		}
 	}
 }
@@ -243,7 +243,7 @@ static void x38_check(struct mem_ctl_info *mci)
 {
 	struct x38_error_info info;
 
-	debugf1("MC%d: %s()\n", mci->mc_idx, __func__);
+	edac_dbg(1, "MC%d\n", mci->mc_idx);
 	x38_get_and_clear_error_info(mci, &info);
 	x38_process_error_info(mci, &info);
 }
@@ -331,7 +331,7 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 	bool stacked;
 	void __iomem *window;
 
-	debugf0("MC: %s()\n", __func__);
+	edac_dbg(0, "MC:\n");
 
 	window = x38_map_mchbar(pdev);
 	if (!window)
@@ -352,9 +352,9 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 	if (!mci)
 		return -ENOMEM;
 
-	debugf3("MC: %s(): init mci\n", __func__);
+	edac_dbg(3, "MC: init mci\n");
 
-	mci->dev = &pdev->dev;
+	mci->pdev = &pdev->dev;
 	mci->mtype_cap = MEM_FLAG_DDR2;
 
 	mci->edac_ctl_cap = EDAC_FLAG_SECDED;
@@ -378,7 +378,7 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 	 */
 	for (i = 0; i < mci->nr_csrows; i++) {
 		unsigned long nr_pages;
-		struct csrow_info *csrow = &mci->csrows[i];
+		struct csrow_info *csrow = mci->csrows[i];
 
 		nr_pages = drb_to_nr_pages(drbs, stacked,
 			i / X38_RANKS_PER_CHANNEL,
@@ -388,7 +388,7 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 			continue;
 
 		for (j = 0; j < x38_channel_num; j++) {
-			struct dimm_info *dimm = csrow->channels[j].dimm;
+			struct dimm_info *dimm = csrow->channels[j]->dimm;
 
 			dimm->nr_pages = nr_pages / x38_channel_num;
 			dimm->grain = nr_pages << PAGE_SHIFT;
@@ -402,12 +402,12 @@ static int x38_probe1(struct pci_dev *pdev, int dev_idx)
 
 	rc = -ENODEV;
 	if (edac_mc_add_mc(mci)) {
-		debugf3("MC: %s(): failed edac_mc_add_mc()\n", __func__);
+		edac_dbg(3, "MC: failed edac_mc_add_mc()\n");
 		goto fail;
 	}
 
 	/* get this far and it's successful */
-	debugf3("MC: %s(): success\n", __func__);
+	edac_dbg(3, "MC: success\n");
 	return 0;
 
 fail:
@@ -423,7 +423,7 @@ static int __devinit x38_init_one(struct pci_dev *pdev,
 {
 	int rc;
 
-	debugf0("MC: %s()\n", __func__);
+	edac_dbg(0, "MC:\n");
 
 	if (pci_enable_device(pdev) < 0)
 		return -EIO;
@@ -439,7 +439,7 @@ static void __devexit x38_remove_one(struct pci_dev *pdev)
 {
 	struct mem_ctl_info *mci;
 
-	debugf0("%s()\n", __func__);
+	edac_dbg(0, "\n");
 
 	mci = edac_mc_del_mc(&pdev->dev);
 	if (!mci)
@@ -472,7 +472,7 @@ static int __init x38_init(void)
 {
 	int pci_rc;
 
-	debugf3("MC: %s()\n", __func__);
+	edac_dbg(3, "MC:\n");
 
 	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
 	opstate_init();
@@ -486,14 +486,14 @@ static int __init x38_init(void)
 		mci_pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
 					PCI_DEVICE_ID_INTEL_X38_HB, NULL);
 		if (!mci_pdev) {
-			debugf0("x38 pci_get_device fail\n");
+			edac_dbg(0, "x38 pci_get_device fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
 
 		pci_rc = x38_init_one(mci_pdev, x38_pci_tbl);
 		if (pci_rc < 0) {
-			debugf0("x38 init fail\n");
+			edac_dbg(0, "x38 init fail\n");
 			pci_rc = -ENODEV;
 			goto fail1;
 		}
@@ -513,7 +513,7 @@ fail0:
 
 static void __exit x38_exit(void)
 {
-	debugf3("MC: %s()\n", __func__);
+	edac_dbg(3, "MC:\n");
 
 	pci_unregister_driver(&x38_driver);
 	if (!x38_registered) {
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index 29c5cf852ef..e175c8ed4ec 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -21,12 +21,30 @@ config EXTCON_GPIO
 	  Say Y here to enable GPIO based extcon support. Note that GPIO
 	  extcon supports single state per extcon instance.
 
+config EXTCON_MAX77693
+	tristate "MAX77693 EXTCON Support"
+	depends on MFD_MAX77693
+	select IRQ_DOMAIN
+	select REGMAP_I2C
+	help
+	  If you say yes here you get support for the MUIC device of
+	  Maxim MAX77693 PMIC. The MAX77693 MUIC is a USB port accessory
+	  detector and switch.
+
 config EXTCON_MAX8997
 	tristate "MAX8997 EXTCON Support"
-	depends on MFD_MAX8997
+	depends on MFD_MAX8997 && IRQ_DOMAIN
 	help
 	  If you say yes here you get support for the MUIC device of
 	  Maxim MAX8997 PMIC. The MAX8997 MUIC is a USB port accessory
 	  detector and switch.
 
+config EXTCON_ARIZONA
+	tristate "Wolfson Arizona EXTCON support"
+	depends on MFD_ARIZONA
+	help
+	  Say Y here to enable support for external accessory detection
+	  with Wolfson Arizona devices. These are audio CODECs with
+	  advanced audio accessory detection support.
+
 endif # MULTISTATE_SWITCH
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
index 86020bdb6da..88961b33234 100644
--- a/drivers/extcon/Makefile
+++ b/drivers/extcon/Makefile
@@ -4,4 +4,6 @@
 
 obj-$(CONFIG_EXTCON)		+= extcon_class.o
 obj-$(CONFIG_EXTCON_GPIO)	+= extcon_gpio.o
+obj-$(CONFIG_EXTCON_MAX77693)	+= extcon-max77693.o
 obj-$(CONFIG_EXTCON_MAX8997)	+= extcon-max8997.o
+obj-$(CONFIG_EXTCON_ARIZONA)	+= extcon-arizona.o
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
new file mode 100644
index 00000000000..427a289f32a
--- /dev/null
+++ b/drivers/extcon/extcon-arizona.c
@@ -0,0 +1,490 @@
+/*
+ * extcon-arizona.c - Extcon driver Wolfson Arizona devices
+ *
+ *  Copyright (C) 2012 Wolfson Microelectronics plc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/i2c.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/extcon.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/pdata.h>
+#include <linux/mfd/arizona/registers.h>
+
+struct arizona_extcon_info {
+	struct device *dev;
+	struct arizona *arizona;
+	struct mutex lock;
+	struct regulator *micvdd;
+
+	int micd_mode;
+	const struct arizona_micd_config *micd_modes;
+	int micd_num_modes;
+
+	bool micd_reva;
+
+	bool mic;
+	bool detecting;
+	int jack_flips;
+
+	struct extcon_dev edev;
+};
+
+static const struct arizona_micd_config micd_default_modes[] = {
+	{ ARIZONA_ACCDET_SRC, 1 << ARIZONA_MICD_BIAS_SRC_SHIFT, 0 },
+	{ 0,                  2 << ARIZONA_MICD_BIAS_SRC_SHIFT, 1 },
+};
+
+#define ARIZONA_CABLE_MECHANICAL 0
+#define ARIZONA_CABLE_MICROPHONE 1
+#define ARIZONA_CABLE_HEADPHONE  2
+
+static const char *arizona_cable[] = {
+	"Mechanical",
+	"Microphone",
+	"Headphone",
+	NULL,
+};
+
+static void arizona_extcon_set_mode(struct arizona_extcon_info *info, int mode)
+{
+	struct arizona *arizona = info->arizona;
+
+	gpio_set_value_cansleep(arizona->pdata.micd_pol_gpio,
+				info->micd_modes[mode].gpio);
+	regmap_update_bits(arizona->regmap, ARIZONA_MIC_DETECT_1,
+			   ARIZONA_MICD_BIAS_SRC_MASK,
+			   info->micd_modes[mode].bias);
+	regmap_update_bits(arizona->regmap, ARIZONA_ACCESSORY_DETECT_MODE_1,
+			   ARIZONA_ACCDET_SRC, info->micd_modes[mode].src);
+
+	info->micd_mode = mode;
+
+	dev_dbg(arizona->dev, "Set jack polarity to %d\n", mode);
+}
+
+static void arizona_start_mic(struct arizona_extcon_info *info)
+{
+	struct arizona *arizona = info->arizona;
+	bool change;
+	int ret;
+
+	info->detecting = true;
+	info->mic = false;
+	info->jack_flips = 0;
+
+	/* Microphone detection can't use idle mode */
+	pm_runtime_get(info->dev);
+
+	ret = regulator_enable(info->micvdd);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to enable MICVDD: %d\n",
+			ret);
+	}
+
+	if (info->micd_reva) {
+		regmap_write(arizona->regmap, 0x80, 0x3);
+		regmap_write(arizona->regmap, 0x294, 0);
+		regmap_write(arizona->regmap, 0x80, 0x0);
+	}
+
+	regmap_update_bits_check(arizona->regmap, ARIZONA_MIC_DETECT_1,
+				 ARIZONA_MICD_ENA, ARIZONA_MICD_ENA,
+				 &change);
+	if (!change) {
+		regulator_disable(info->micvdd);
+		pm_runtime_put_autosuspend(info->dev);
+	}
+}
+
+static void arizona_stop_mic(struct arizona_extcon_info *info)
+{
+	struct arizona *arizona = info->arizona;
+	bool change;
+
+	regmap_update_bits_check(arizona->regmap, ARIZONA_MIC_DETECT_1,
+				 ARIZONA_MICD_ENA, 0,
+				 &change);
+
+	if (info->micd_reva) {
+		regmap_write(arizona->regmap, 0x80, 0x3);
+		regmap_write(arizona->regmap, 0x294, 2);
+		regmap_write(arizona->regmap, 0x80, 0x0);
+	}
+
+	if (change) {
+		regulator_disable(info->micvdd);
+		pm_runtime_put_autosuspend(info->dev);
+	}
+}
+
+static irqreturn_t arizona_micdet(int irq, void *data)
+{
+	struct arizona_extcon_info *info = data;
+	struct arizona *arizona = info->arizona;
+	unsigned int val;
+	int ret;
+
+	mutex_lock(&info->lock);
+
+	ret = regmap_read(arizona->regmap, ARIZONA_MIC_DETECT_3, &val);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to read MICDET: %d\n", ret);
+		return IRQ_NONE;
+	}
+
+	dev_dbg(arizona->dev, "MICDET: %x\n", val);
+
+	if (!(val & ARIZONA_MICD_VALID)) {
+		dev_warn(arizona->dev, "Microphone detection state invalid\n");
+		mutex_unlock(&info->lock);
+		return IRQ_NONE;
+	}
+
+	/* Due to jack detect this should never happen */
+	if (!(val & ARIZONA_MICD_STS)) {
+		dev_warn(arizona->dev, "Detected open circuit\n");
+		info->detecting = false;
+		goto handled;
+	}
+
+	/* If we got a high impedence we should have a headset, report it. */
+	if (info->detecting && (val & 0x400)) {
+		ret = extcon_update_state(&info->edev,
+					  1 << ARIZONA_CABLE_MICROPHONE |
+					  1 << ARIZONA_CABLE_HEADPHONE,
+					  1 << ARIZONA_CABLE_MICROPHONE |
+					  1 << ARIZONA_CABLE_HEADPHONE);
+
+		if (ret != 0)
+			dev_err(arizona->dev, "Headset report failed: %d\n",
+				ret);
+
+		info->mic = true;
+		info->detecting = false;
+		goto handled;
+	}
+
+	/* If we detected a lower impedence during initial startup
+	 * then we probably have the wrong polarity, flip it.  Don't
+	 * do this for the lowest impedences to speed up detection of
+	 * plain headphones.  If both polarities report a low
+	 * impedence then give up and report headphones.
+	 */
+	if (info->detecting && (val & 0x3f8)) {
+		info->jack_flips++;
+
+		if (info->jack_flips >= info->micd_num_modes) {
+			dev_dbg(arizona->dev, "Detected headphone\n");
+			info->detecting = false;
+			arizona_stop_mic(info);
+
+			ret = extcon_set_cable_state_(&info->edev,
+						      ARIZONA_CABLE_HEADPHONE,
+						      true);
+			if (ret != 0)
+				dev_err(arizona->dev,
+					"Headphone report failed: %d\n",
+				ret);
+		} else {
+			info->micd_mode++;
+			if (info->micd_mode == info->micd_num_modes)
+				info->micd_mode = 0;
+			arizona_extcon_set_mode(info, info->micd_mode);
+
+			info->jack_flips++;
+		}
+
+		goto handled;
+	}
+
+	/*
+	 * If we're still detecting and we detect a short then we've
+	 * got a headphone.  Otherwise it's a button press, the
+	 * button reporting is stubbed out for now.
+	 */
+	if (val & 0x3fc) {
+		if (info->mic) {
+			dev_dbg(arizona->dev, "Mic button detected\n");
+
+		} else if (info->detecting) {
+			dev_dbg(arizona->dev, "Headphone detected\n");
+			info->detecting = false;
+			arizona_stop_mic(info);
+
+			ret = extcon_set_cable_state_(&info->edev,
+						      ARIZONA_CABLE_HEADPHONE,
+						      true);
+			if (ret != 0)
+				dev_err(arizona->dev,
+					"Headphone report failed: %d\n",
+				ret);
+		} else {
+			dev_warn(arizona->dev, "Button with no mic: %x\n",
+				 val);
+		}
+	} else {
+		dev_dbg(arizona->dev, "Mic button released\n");
+	}
+
+handled:
+	pm_runtime_mark_last_busy(info->dev);
+	mutex_unlock(&info->lock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arizona_jackdet(int irq, void *data)
+{
+	struct arizona_extcon_info *info = data;
+	struct arizona *arizona = info->arizona;
+	unsigned int val;
+	int ret;
+
+	pm_runtime_get_sync(info->dev);
+
+	mutex_lock(&info->lock);
+
+	ret = regmap_read(arizona->regmap, ARIZONA_AOD_IRQ_RAW_STATUS, &val);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to read jackdet status: %d\n",
+			ret);
+		mutex_unlock(&info->lock);
+		pm_runtime_put_autosuspend(info->dev);
+		return IRQ_NONE;
+	}
+
+	if (val & ARIZONA_JD1_STS) {
+		dev_dbg(arizona->dev, "Detected jack\n");
+		ret = extcon_set_cable_state_(&info->edev,
+					      ARIZONA_CABLE_MECHANICAL, true);
+
+		if (ret != 0)
+			dev_err(arizona->dev, "Mechanical report failed: %d\n",
+				ret);
+
+		arizona_start_mic(info);
+	} else {
+		dev_dbg(arizona->dev, "Detected jack removal\n");
+
+		arizona_stop_mic(info);
+
+		ret = extcon_update_state(&info->edev, 0xffffffff, 0);
+		if (ret != 0)
+			dev_err(arizona->dev, "Removal report failed: %d\n",
+				ret);
+	}
+
+	mutex_unlock(&info->lock);
+
+	pm_runtime_mark_last_busy(info->dev);
+	pm_runtime_put_autosuspend(info->dev);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit arizona_extcon_probe(struct platform_device *pdev)
+{
+	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
+	struct arizona_pdata *pdata;
+	struct arizona_extcon_info *info;
+	int ret, mode;
+
+	pdata = dev_get_platdata(arizona->dev);
+
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	info->micvdd = devm_regulator_get(arizona->dev, "MICVDD");
+	if (IS_ERR(info->micvdd)) {
+		ret = PTR_ERR(info->micvdd);
+		dev_err(arizona->dev, "Failed to get MICVDD: %d\n", ret);
+		goto err;
+	}
+
+	mutex_init(&info->lock);
+	info->arizona = arizona;
+	info->dev = &pdev->dev;
+	info->detecting = true;
+	platform_set_drvdata(pdev, info);
+
+	switch (arizona->type) {
+	case WM5102:
+		switch (arizona->rev) {
+		case 0:
+			info->micd_reva = true;
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+
+	info->edev.name = "Headset Jack";
+	info->edev.supported_cable = arizona_cable;
+
+	ret = extcon_dev_register(&info->edev, arizona->dev);
+	if (ret < 0) {
+		dev_err(arizona->dev, "extcon_dev_regster() failed: %d\n",
+			ret);
+		goto err;
+	}
+
+	if (pdata->num_micd_configs) {
+		info->micd_modes = pdata->micd_configs;
+		info->micd_num_modes = pdata->num_micd_configs;
+	} else {
+		info->micd_modes = micd_default_modes;
+		info->micd_num_modes = ARRAY_SIZE(micd_default_modes);
+	}
+
+	if (arizona->pdata.micd_pol_gpio > 0) {
+		if (info->micd_modes[0].gpio)
+			mode = GPIOF_OUT_INIT_HIGH;
+		else
+			mode = GPIOF_OUT_INIT_LOW;
+
+		ret = devm_gpio_request_one(&pdev->dev,
+					    arizona->pdata.micd_pol_gpio,
+					    mode,
+					    "MICD polarity");
+		if (ret != 0) {
+			dev_err(arizona->dev, "Failed to request GPIO%d: %d\n",
+				arizona->pdata.micd_pol_gpio, ret);
+			goto err_register;
+		}
+	}
+
+	arizona_extcon_set_mode(info, 0);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_idle(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+
+	ret = arizona_request_irq(arizona, ARIZONA_IRQ_JD_RISE,
+				  "JACKDET rise", arizona_jackdet, info);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Failed to get JACKDET rise IRQ: %d\n",
+			ret);
+		goto err_register;
+	}
+
+	ret = arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_RISE, 1);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Failed to set JD rise IRQ wake: %d\n",
+			ret);
+		goto err_rise;
+	}
+
+	ret = arizona_request_irq(arizona, ARIZONA_IRQ_JD_FALL,
+				  "JACKDET fall", arizona_jackdet, info);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Failed to get JD fall IRQ: %d\n", ret);
+		goto err_rise_wake;
+	}
+
+	ret = arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_FALL, 1);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Failed to set JD fall IRQ wake: %d\n",
+			ret);
+		goto err_fall;
+	}
+
+	ret = arizona_request_irq(arizona, ARIZONA_IRQ_MICDET,
+				  "MICDET", arizona_micdet, info);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Failed to get MICDET IRQ: %d\n", ret);
+		goto err_fall_wake;
+	}
+
+	regmap_update_bits(arizona->regmap, ARIZONA_MIC_DETECT_1,
+			   ARIZONA_MICD_BIAS_STARTTIME_MASK |
+			   ARIZONA_MICD_RATE_MASK,
+			   7 << ARIZONA_MICD_BIAS_STARTTIME_SHIFT |
+			   8 << ARIZONA_MICD_RATE_SHIFT);
+
+	arizona_clk32k_enable(arizona);
+	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_DEBOUNCE,
+			   ARIZONA_JD1_DB, ARIZONA_JD1_DB);
+	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE,
+			   ARIZONA_JD1_ENA, ARIZONA_JD1_ENA);
+
+	pm_runtime_put(&pdev->dev);
+
+	return 0;
+
+err_fall_wake:
+	arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_FALL, 0);
+err_fall:
+	arizona_free_irq(arizona, ARIZONA_IRQ_JD_FALL, info);
+err_rise_wake:
+	arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_RISE, 0);
+err_rise:
+	arizona_free_irq(arizona, ARIZONA_IRQ_JD_RISE, info);
+err_register:
+	pm_runtime_disable(&pdev->dev);
+	extcon_dev_unregister(&info->edev);
+err:
+	return ret;
+}
+
+static int __devexit arizona_extcon_remove(struct platform_device *pdev)
+{
+	struct arizona_extcon_info *info = platform_get_drvdata(pdev);
+	struct arizona *arizona = info->arizona;
+
+	pm_runtime_disable(&pdev->dev);
+
+	arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_RISE, 0);
+	arizona_set_irq_wake(arizona, ARIZONA_IRQ_JD_FALL, 0);
+	arizona_free_irq(arizona, ARIZONA_IRQ_MICDET, info);
+	arizona_free_irq(arizona, ARIZONA_IRQ_JD_RISE, info);
+	arizona_free_irq(arizona, ARIZONA_IRQ_JD_FALL, info);
+	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE,
+			   ARIZONA_JD1_ENA, 0);
+	arizona_clk32k_disable(arizona);
+	extcon_dev_unregister(&info->edev);
+
+	return 0;
+}
+
+static struct platform_driver arizona_extcon_driver = {
+	.driver		= {
+		.name	= "arizona-extcon",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= arizona_extcon_probe,
+	.remove		= __devexit_p(arizona_extcon_remove),
+};
+
+module_platform_driver(arizona_extcon_driver);
+
+MODULE_DESCRIPTION("Arizona Extcon driver");
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:extcon-arizona");
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
new file mode 100644
index 00000000000..920a609b2c3
--- /dev/null
+++ b/drivers/extcon/extcon-max77693.c
@@ -0,0 +1,779 @@
+/*
+ * extcon-max77693.c - MAX77693 extcon driver to support MAX77693 MUIC
+ *
+ * Copyright (C) 2012 Samsung Electrnoics
+ * Chanwoo Choi <cw00.choi@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/i2c.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/max77693.h>
+#include <linux/mfd/max77693-private.h>
+#include <linux/extcon.h>
+#include <linux/regmap.h>
+#include <linux/irqdomain.h>
+
+#define	DEV_NAME			"max77693-muic"
+
+/* MAX77693 MUIC - STATUS1~3 Register */
+#define STATUS1_ADC_SHIFT		(0)
+#define STATUS1_ADCLOW_SHIFT		(5)
+#define STATUS1_ADCERR_SHIFT		(6)
+#define STATUS1_ADC1K_SHIFT		(7)
+#define STATUS1_ADC_MASK		(0x1f << STATUS1_ADC_SHIFT)
+#define STATUS1_ADCLOW_MASK		(0x1 << STATUS1_ADCLOW_SHIFT)
+#define STATUS1_ADCERR_MASK		(0x1 << STATUS1_ADCERR_SHIFT)
+#define STATUS1_ADC1K_MASK		(0x1 << STATUS1_ADC1K_SHIFT)
+
+#define STATUS2_CHGTYP_SHIFT		(0)
+#define STATUS2_CHGDETRUN_SHIFT		(3)
+#define STATUS2_DCDTMR_SHIFT		(4)
+#define STATUS2_DXOVP_SHIFT		(5)
+#define STATUS2_VBVOLT_SHIFT		(6)
+#define STATUS2_VIDRM_SHIFT		(7)
+#define STATUS2_CHGTYP_MASK		(0x7 << STATUS2_CHGTYP_SHIFT)
+#define STATUS2_CHGDETRUN_MASK		(0x1 << STATUS2_CHGDETRUN_SHIFT)
+#define STATUS2_DCDTMR_MASK		(0x1 << STATUS2_DCDTMR_SHIFT)
+#define STATUS2_DXOVP_MASK		(0x1 << STATUS2_DXOVP_SHIFT)
+#define STATUS2_VBVOLT_MASK		(0x1 << STATUS2_VBVOLT_SHIFT)
+#define STATUS2_VIDRM_MASK		(0x1 << STATUS2_VIDRM_SHIFT)
+
+#define STATUS3_OVP_SHIFT		(2)
+#define STATUS3_OVP_MASK		(0x1 << STATUS3_OVP_SHIFT)
+
+/* MAX77693 CDETCTRL1~2 register */
+#define CDETCTRL1_CHGDETEN_SHIFT	(0)
+#define CDETCTRL1_CHGTYPMAN_SHIFT	(1)
+#define CDETCTRL1_DCDEN_SHIFT		(2)
+#define CDETCTRL1_DCD2SCT_SHIFT		(3)
+#define CDETCTRL1_CDDELAY_SHIFT		(4)
+#define CDETCTRL1_DCDCPL_SHIFT		(5)
+#define CDETCTRL1_CDPDET_SHIFT		(7)
+#define CDETCTRL1_CHGDETEN_MASK		(0x1 << CDETCTRL1_CHGDETEN_SHIFT)
+#define CDETCTRL1_CHGTYPMAN_MASK	(0x1 << CDETCTRL1_CHGTYPMAN_SHIFT)
+#define CDETCTRL1_DCDEN_MASK		(0x1 << CDETCTRL1_DCDEN_SHIFT)
+#define CDETCTRL1_DCD2SCT_MASK		(0x1 << CDETCTRL1_DCD2SCT_SHIFT)
+#define CDETCTRL1_CDDELAY_MASK		(0x1 << CDETCTRL1_CDDELAY_SHIFT)
+#define CDETCTRL1_DCDCPL_MASK		(0x1 << CDETCTRL1_DCDCPL_SHIFT)
+#define CDETCTRL1_CDPDET_MASK		(0x1 << CDETCTRL1_CDPDET_SHIFT)
+
+#define CDETCTRL2_VIDRMEN_SHIFT		(1)
+#define CDETCTRL2_DXOVPEN_SHIFT		(3)
+#define CDETCTRL2_VIDRMEN_MASK		(0x1 << CDETCTRL2_VIDRMEN_SHIFT)
+#define CDETCTRL2_DXOVPEN_MASK		(0x1 << CDETCTRL2_DXOVPEN_SHIFT)
+
+/* MAX77693 MUIC - CONTROL1~3 register */
+#define COMN1SW_SHIFT			(0)
+#define COMP2SW_SHIFT			(3)
+#define COMN1SW_MASK			(0x7 << COMN1SW_SHIFT)
+#define COMP2SW_MASK			(0x7 << COMP2SW_SHIFT)
+#define COMP_SW_MASK			(COMP2SW_MASK | COMN1SW_MASK)
+#define CONTROL1_SW_USB			((1 << COMP2SW_SHIFT) \
+						| (1 << COMN1SW_SHIFT))
+#define CONTROL1_SW_AUDIO		((2 << COMP2SW_SHIFT) \
+						| (2 << COMN1SW_SHIFT))
+#define CONTROL1_SW_UART		((3 << COMP2SW_SHIFT) \
+						| (3 << COMN1SW_SHIFT))
+#define CONTROL1_SW_OPEN		((0 << COMP2SW_SHIFT) \
+						| (0 << COMN1SW_SHIFT))
+
+#define CONTROL2_LOWPWR_SHIFT		(0)
+#define CONTROL2_ADCEN_SHIFT		(1)
+#define CONTROL2_CPEN_SHIFT		(2)
+#define CONTROL2_SFOUTASRT_SHIFT	(3)
+#define CONTROL2_SFOUTORD_SHIFT		(4)
+#define CONTROL2_ACCDET_SHIFT		(5)
+#define CONTROL2_USBCPINT_SHIFT		(6)
+#define CONTROL2_RCPS_SHIFT		(7)
+#define CONTROL2_LOWPWR_MASK		(0x1 << CONTROL2_LOWPWR_SHIFT)
+#define CONTROL2_ADCEN_MASK		(0x1 << CONTROL2_ADCEN_SHIFT)
+#define CONTROL2_CPEN_MASK		(0x1 << CONTROL2_CPEN_SHIFT)
+#define CONTROL2_SFOUTASRT_MASK		(0x1 << CONTROL2_SFOUTASRT_SHIFT)
+#define CONTROL2_SFOUTORD_MASK		(0x1 << CONTROL2_SFOUTORD_SHIFT)
+#define CONTROL2_ACCDET_MASK		(0x1 << CONTROL2_ACCDET_SHIFT)
+#define CONTROL2_USBCPINT_MASK		(0x1 << CONTROL2_USBCPINT_SHIFT)
+#define CONTROL2_RCPS_MASK		(0x1 << CONTROL2_RCPS_SHIFT)
+
+#define CONTROL3_JIGSET_SHIFT		(0)
+#define CONTROL3_BTLDSET_SHIFT		(2)
+#define CONTROL3_ADCDBSET_SHIFT		(4)
+#define CONTROL3_JIGSET_MASK		(0x3 << CONTROL3_JIGSET_SHIFT)
+#define CONTROL3_BTLDSET_MASK		(0x3 << CONTROL3_BTLDSET_SHIFT)
+#define CONTROL3_ADCDBSET_MASK		(0x3 << CONTROL3_ADCDBSET_SHIFT)
+
+enum max77693_muic_adc_debounce_time {
+	ADC_DEBOUNCE_TIME_5MS = 0,
+	ADC_DEBOUNCE_TIME_10MS,
+	ADC_DEBOUNCE_TIME_25MS,
+	ADC_DEBOUNCE_TIME_38_62MS,
+};
+
+struct max77693_muic_info {
+	struct device *dev;
+	struct max77693_dev *max77693;
+	struct extcon_dev *edev;
+	int prev_adc;
+	int prev_adc_gnd;
+	int prev_chg_type;
+	u8 status[2];
+
+	int irq;
+	struct work_struct irq_work;
+	struct mutex mutex;
+};
+
+enum max77693_muic_charger_type {
+	MAX77693_CHARGER_TYPE_NONE = 0,
+	MAX77693_CHARGER_TYPE_USB,
+	MAX77693_CHARGER_TYPE_DOWNSTREAM_PORT,
+	MAX77693_CHARGER_TYPE_DEDICATED_CHG,
+	MAX77693_CHARGER_TYPE_APPLE_500MA,
+	MAX77693_CHARGER_TYPE_APPLE_1A_2A,
+	MAX77693_CHARGER_TYPE_DEAD_BATTERY = 7,
+};
+
+/**
+ * struct max77693_muic_irq
+ * @irq: the index of irq list of MUIC device.
+ * @name: the name of irq.
+ * @virq: the virtual irq to use irq domain
+ */
+struct max77693_muic_irq {
+	unsigned int irq;
+	const char *name;
+	unsigned int virq;
+};
+
+static struct max77693_muic_irq muic_irqs[] = {
+	{ MAX77693_MUIC_IRQ_INT1_ADC,		"muic-ADC" },
+	{ MAX77693_MUIC_IRQ_INT1_ADC_LOW,	"muic-ADCLOW" },
+	{ MAX77693_MUIC_IRQ_INT1_ADC_ERR,	"muic-ADCError" },
+	{ MAX77693_MUIC_IRQ_INT1_ADC1K,		"muic-ADC1K" },
+	{ MAX77693_MUIC_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
+	{ MAX77693_MUIC_IRQ_INT2_CHGDETREUN,	"muic-CHGDETREUN" },
+	{ MAX77693_MUIC_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
+	{ MAX77693_MUIC_IRQ_INT2_DXOVP,		"muic-DXOVP" },
+	{ MAX77693_MUIC_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
+	{ MAX77693_MUIC_IRQ_INT2_VIDRM,		"muic-VIDRM" },
+	{ MAX77693_MUIC_IRQ_INT3_EOC,		"muic-EOC" },
+	{ MAX77693_MUIC_IRQ_INT3_CGMBC,		"muic-CGMBC" },
+	{ MAX77693_MUIC_IRQ_INT3_OVP,		"muic-OVP" },
+	{ MAX77693_MUIC_IRQ_INT3_MBCCHG_ERR,	"muic-MBCCHG_ERR" },
+	{ MAX77693_MUIC_IRQ_INT3_CHG_ENABLED,	"muic-CHG_ENABLED" },
+	{ MAX77693_MUIC_IRQ_INT3_BAT_DET,	"muic-BAT_DET" },
+};
+
+/* Define supported accessory type */
+enum max77693_muic_acc_type {
+	MAX77693_MUIC_ADC_GROUND = 0x0,
+	MAX77693_MUIC_ADC_SEND_END_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S1_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S2_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S3_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S4_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S5_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S6_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S7_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S8_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S9_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S10_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S11_BUTTON,
+	MAX77693_MUIC_ADC_REMOTE_S12_BUTTON,
+	MAX77693_MUIC_ADC_RESERVED_ACC_1,
+	MAX77693_MUIC_ADC_RESERVED_ACC_2,
+	MAX77693_MUIC_ADC_RESERVED_ACC_3,
+	MAX77693_MUIC_ADC_RESERVED_ACC_4,
+	MAX77693_MUIC_ADC_RESERVED_ACC_5,
+	MAX77693_MUIC_ADC_CEA936_AUDIO,
+	MAX77693_MUIC_ADC_PHONE_POWERED_DEV,
+	MAX77693_MUIC_ADC_TTY_CONVERTER,
+	MAX77693_MUIC_ADC_UART_CABLE,
+	MAX77693_MUIC_ADC_CEA936A_TYPE1_CHG,
+	MAX77693_MUIC_ADC_FACTORY_MODE_USB_OFF,
+	MAX77693_MUIC_ADC_FACTORY_MODE_USB_ON,
+	MAX77693_MUIC_ADC_AV_CABLE_NOLOAD,
+	MAX77693_MUIC_ADC_CEA936A_TYPE2_CHG,
+	MAX77693_MUIC_ADC_FACTORY_MODE_UART_OFF,
+	MAX77693_MUIC_ADC_FACTORY_MODE_UART_ON,
+	MAX77693_MUIC_ADC_AUDIO_MODE_REMOTE,
+	MAX77693_MUIC_ADC_OPEN,
+
+	/* The below accessories have same ADC value so ADCLow and
+	   ADC1K bit is used to separate specific accessory */
+	MAX77693_MUIC_GND_USB_OTG = 0x100,	/* ADC:0x0, ADCLow:0, ADC1K:0 */
+	MAX77693_MUIC_GND_AV_CABLE_LOAD = 0x102,/* ADC:0x0, ADCLow:1, ADC1K:0 */
+	MAX77693_MUIC_GND_MHL_CABLE = 0x103,	/* ADC:0x0, ADCLow:1, ADC1K:1 */
+};
+
+/* MAX77693 MUIC device support below list of accessories(external connector) */
+const char *max77693_extcon_cable[] = {
+	[0] = "USB",
+	[1] = "USB-Host",
+	[2] = "TA",
+	[3] = "Fast-charger",
+	[4] = "Slow-charger",
+	[5] = "Charge-downstream",
+	[6] = "MHL",
+	[7] = "Audio-video-load",
+	[8] = "Audio-video-noload",
+	[9] = "JIG",
+
+	NULL,
+};
+
+static int max77693_muic_set_debounce_time(struct max77693_muic_info *info,
+		enum max77693_muic_adc_debounce_time time)
+{
+	int ret = 0;
+	u8 ctrl3;
+
+	switch (time) {
+	case ADC_DEBOUNCE_TIME_5MS:
+	case ADC_DEBOUNCE_TIME_10MS:
+	case ADC_DEBOUNCE_TIME_25MS:
+	case ADC_DEBOUNCE_TIME_38_62MS:
+		ret = max77693_read_reg(info->max77693->regmap_muic,
+				MAX77693_MUIC_REG_CTRL3, &ctrl3);
+		ctrl3 &= ~CONTROL3_ADCDBSET_MASK;
+		ctrl3 |= (time << CONTROL3_ADCDBSET_SHIFT);
+
+		ret = max77693_write_reg(info->max77693->regmap_muic,
+				MAX77693_MUIC_REG_CTRL3, ctrl3);
+		if (ret) {
+			dev_err(info->dev, "failed to set ADC debounce time\n");
+			ret = -EINVAL;
+		}
+		break;
+	default:
+		dev_err(info->dev, "invalid ADC debounce time\n");
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+};
+
+static int max77693_muic_set_path(struct max77693_muic_info *info,
+		u8 val, bool attached)
+{
+	int ret = 0;
+	u8 ctrl1, ctrl2 = 0;
+
+	if (attached)
+		ctrl1 = val;
+	else
+		ctrl1 = CONTROL1_SW_OPEN;
+
+	ret = max77693_update_reg(info->max77693->regmap_muic,
+			MAX77693_MUIC_REG_CTRL1, ctrl1, COMP_SW_MASK);
+	if (ret < 0) {
+		dev_err(info->dev, "failed to update MUIC register\n");
+		goto out;
+	}
+
+	if (attached)
+		ctrl2 |= CONTROL2_CPEN_MASK;	/* LowPwr=0, CPEn=1 */
+	else
+		ctrl2 |= CONTROL2_LOWPWR_MASK;	/* LowPwr=1, CPEn=0 */
+
+	ret = max77693_update_reg(info->max77693->regmap_muic,
+			MAX77693_MUIC_REG_CTRL2, ctrl2,
+			CONTROL2_LOWPWR_MASK | CONTROL2_CPEN_MASK);
+	if (ret < 0) {
+		dev_err(info->dev, "failed to update MUIC register\n");
+		goto out;
+	}
+
+	dev_info(info->dev,
+		"CONTROL1 : 0x%02x, CONTROL2 : 0x%02x, state : %s\n",
+		ctrl1, ctrl2, attached ? "attached" : "detached");
+out:
+	return ret;
+}
+
+static int max77693_muic_adc_ground_handler(struct max77693_muic_info *info,
+		bool attached)
+{
+	int ret = 0;
+	int type;
+	int adc, adc1k, adclow;
+
+	if (attached) {
+		adc = info->status[0] & STATUS1_ADC_MASK;
+		adclow = info->status[0] & STATUS1_ADCLOW_MASK;
+		adclow >>= STATUS1_ADCLOW_SHIFT;
+		adc1k = info->status[0] & STATUS1_ADC1K_MASK;
+		adc1k >>= STATUS1_ADC1K_SHIFT;
+
+		/**
+		 * [0x1][ADCLow][ADC1K]
+		 * [0x1    0       0  ]	: USB_OTG
+		 * [0x1    1       0  ] : Audio Video Cable with load
+		 * [0x1    1       1  ] : MHL
+		 */
+		type = ((0x1 << 8) | (adclow << 1) | adc1k);
+
+		/* Store previous ADC value to handle accessory
+		   when accessory will be detached */
+		info->prev_adc = adc;
+		info->prev_adc_gnd = type;
+	} else
+		type = info->prev_adc_gnd;
+
+	switch (type) {
+	case MAX77693_MUIC_GND_USB_OTG:
+		/* USB_OTG */
+		ret = max77693_muic_set_path(info, CONTROL1_SW_USB, attached);
+		if (ret < 0)
+			goto out;
+		extcon_set_cable_state(info->edev, "USB-Host", attached);
+		break;
+	case MAX77693_MUIC_GND_AV_CABLE_LOAD:
+		/* Audio Video Cable with load */
+		ret = max77693_muic_set_path(info, CONTROL1_SW_AUDIO, attached);
+		if (ret < 0)
+			goto out;
+		extcon_set_cable_state(info->edev,
+				"Audio-video-load", attached);
+		break;
+	case MAX77693_MUIC_GND_MHL_CABLE:
+		/* MHL */
+		extcon_set_cable_state(info->edev, "MHL", attached);
+		break;
+	default:
+		dev_err(info->dev, "faild to detect %s accessory\n",
+			attached ? "attached" : "detached");
+		dev_err(info->dev, "- adc:0x%x, adclow:0x%x, adc1k:0x%x\n",
+			adc, adclow, adc1k);
+		ret = -EINVAL;
+		break;
+	}
+
+out:
+	return ret;
+}
+
+static int max77693_muic_adc_handler(struct max77693_muic_info *info,
+		int curr_adc, bool attached)
+{
+	int ret = 0;
+	int adc;
+
+	if (attached) {
+		/* Store ADC value to handle accessory
+		   when accessory will be detached */
+		info->prev_adc = curr_adc;
+		adc = curr_adc;
+	} else
+		adc = info->prev_adc;
+
+	dev_info(info->dev,
+		"external connector is %s (adc:0x%02x, prev_adc:0x%x)\n",
+		attached ? "attached" : "detached", curr_adc, info->prev_adc);
+
+	switch (adc) {
+	case MAX77693_MUIC_ADC_GROUND:
+		/* USB_OTG/MHL/Audio */
+		max77693_muic_adc_ground_handler(info, attached);
+		break;
+	case MAX77693_MUIC_ADC_FACTORY_MODE_USB_OFF:
+	case MAX77693_MUIC_ADC_FACTORY_MODE_USB_ON:
+		/* USB */
+		ret = max77693_muic_set_path(info, CONTROL1_SW_USB, attached);
+		if (ret < 0)
+			goto out;
+		extcon_set_cable_state(info->edev, "USB", attached);
+		break;
+	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_OFF:
+	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_ON:
+		/* JIG */
+		ret = max77693_muic_set_path(info, CONTROL1_SW_UART, attached);
+		if (ret < 0)
+			goto out;
+		extcon_set_cable_state(info->edev, "JIG", attached);
+		break;
+	case MAX77693_MUIC_ADC_AUDIO_MODE_REMOTE:
+		/* Audio Video cable with no-load */
+		ret = max77693_muic_set_path(info, CONTROL1_SW_AUDIO, attached);
+		if (ret < 0)
+			goto out;
+		extcon_set_cable_state(info->edev,
+				"Audio-video-noload", attached);
+		break;
+	case MAX77693_MUIC_ADC_SEND_END_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S1_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S2_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S3_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S4_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S5_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S6_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S7_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S8_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S9_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S10_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S11_BUTTON:
+	case MAX77693_MUIC_ADC_REMOTE_S12_BUTTON:
+	case MAX77693_MUIC_ADC_RESERVED_ACC_1:
+	case MAX77693_MUIC_ADC_RESERVED_ACC_2:
+	case MAX77693_MUIC_ADC_RESERVED_ACC_3:
+	case MAX77693_MUIC_ADC_RESERVED_ACC_4:
+	case MAX77693_MUIC_ADC_RESERVED_ACC_5:
+	case MAX77693_MUIC_ADC_CEA936_AUDIO:
+	case MAX77693_MUIC_ADC_PHONE_POWERED_DEV:
+	case MAX77693_MUIC_ADC_TTY_CONVERTER:
+	case MAX77693_MUIC_ADC_UART_CABLE:
+	case MAX77693_MUIC_ADC_CEA936A_TYPE1_CHG:
+	case MAX77693_MUIC_ADC_AV_CABLE_NOLOAD:
+	case MAX77693_MUIC_ADC_CEA936A_TYPE2_CHG:
+		/* This accessory isn't used in general case if it is specially
+		   needed to detect additional accessory, should implement
+		   proper operation when this accessory is attached/detached. */
+		dev_info(info->dev,
+			"accessory is %s but it isn't used (adc:0x%x)\n",
+			attached ? "attached" : "detached", adc);
+		goto out;
+	default:
+		dev_err(info->dev,
+			"failed to detect %s accessory (adc:0x%x)\n",
+			attached ? "attached" : "detached", adc);
+		ret = -EINVAL;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static int max77693_muic_chg_handler(struct max77693_muic_info *info,
+		int curr_chg_type, bool attached)
+{
+	int ret = 0;
+	int chg_type;
+
+	if (attached) {
+		/* Store previous charger type to control
+		   when charger accessory will be detached */
+		info->prev_chg_type = curr_chg_type;
+		chg_type = curr_chg_type;
+	} else
+		chg_type = info->prev_chg_type;
+
+	dev_info(info->dev,
+		"external connector is %s(chg_type:0x%x, prev_chg_type:0x%x)\n",
+			attached ? "attached" : "detached",
+			curr_chg_type, info->prev_chg_type);
+
+	switch (chg_type) {
+	case MAX77693_CHARGER_TYPE_USB:
+		ret = max77693_muic_set_path(info, CONTROL1_SW_USB, attached);
+		if (ret < 0)
+			goto out;
+		extcon_set_cable_state(info->edev, "USB", attached);
+		break;
+	case MAX77693_CHARGER_TYPE_DOWNSTREAM_PORT:
+		extcon_set_cable_state(info->edev,
+				"Charge-downstream", attached);
+		break;
+	case MAX77693_CHARGER_TYPE_DEDICATED_CHG:
+		extcon_set_cable_state(info->edev, "TA", attached);
+		break;
+	case MAX77693_CHARGER_TYPE_APPLE_500MA:
+		extcon_set_cable_state(info->edev, "Slow-charger", attached);
+		break;
+	case MAX77693_CHARGER_TYPE_APPLE_1A_2A:
+		extcon_set_cable_state(info->edev, "Fast-charger", attached);
+		break;
+	case MAX77693_CHARGER_TYPE_DEAD_BATTERY:
+		break;
+	default:
+		dev_err(info->dev,
+			"failed to detect %s accessory (chg_type:0x%x)\n",
+			attached ? "attached" : "detached", chg_type);
+		ret = -EINVAL;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static void max77693_muic_irq_work(struct work_struct *work)
+{
+	struct max77693_muic_info *info = container_of(work,
+			struct max77693_muic_info, irq_work);
+	int curr_adc, curr_chg_type;
+	int irq_type = -1;
+	int i, ret = 0;
+	bool attached = true;
+
+	if (!info->edev)
+		return;
+
+	mutex_lock(&info->mutex);
+
+	for (i = 0 ; i < ARRAY_SIZE(muic_irqs) ; i++)
+		if (info->irq == muic_irqs[i].virq)
+			irq_type = muic_irqs[i].irq;
+
+	ret = max77693_bulk_read(info->max77693->regmap_muic,
+			MAX77693_MUIC_REG_STATUS1, 2, info->status);
+	if (ret) {
+		dev_err(info->dev, "failed to read MUIC register\n");
+		mutex_unlock(&info->mutex);
+		return;
+	}
+
+	switch (irq_type) {
+	case MAX77693_MUIC_IRQ_INT1_ADC:
+	case MAX77693_MUIC_IRQ_INT1_ADC_LOW:
+	case MAX77693_MUIC_IRQ_INT1_ADC_ERR:
+	case MAX77693_MUIC_IRQ_INT1_ADC1K:
+		/* Handle all of accessory except for
+		   type of charger accessory */
+		curr_adc = info->status[0] & STATUS1_ADC_MASK;
+		curr_adc >>= STATUS1_ADC_SHIFT;
+
+		/* Check accossory state which is either detached or attached */
+		if (curr_adc == MAX77693_MUIC_ADC_OPEN)
+			attached = false;
+
+		ret = max77693_muic_adc_handler(info, curr_adc, attached);
+		break;
+	case MAX77693_MUIC_IRQ_INT2_CHGTYP:
+	case MAX77693_MUIC_IRQ_INT2_CHGDETREUN:
+	case MAX77693_MUIC_IRQ_INT2_DCDTMR:
+	case MAX77693_MUIC_IRQ_INT2_DXOVP:
+	case MAX77693_MUIC_IRQ_INT2_VBVOLT:
+	case MAX77693_MUIC_IRQ_INT2_VIDRM:
+		/* Handle charger accessory */
+		curr_chg_type = info->status[1] & STATUS2_CHGTYP_MASK;
+		curr_chg_type >>= STATUS2_CHGTYP_SHIFT;
+
+		/* Check charger accossory state which
+		   is either detached or attached */
+		if (curr_chg_type == MAX77693_CHARGER_TYPE_NONE)
+			attached = false;
+
+		ret = max77693_muic_chg_handler(info, curr_chg_type, attached);
+		break;
+	case MAX77693_MUIC_IRQ_INT3_EOC:
+	case MAX77693_MUIC_IRQ_INT3_CGMBC:
+	case MAX77693_MUIC_IRQ_INT3_OVP:
+	case MAX77693_MUIC_IRQ_INT3_MBCCHG_ERR:
+	case MAX77693_MUIC_IRQ_INT3_CHG_ENABLED:
+	case MAX77693_MUIC_IRQ_INT3_BAT_DET:
+		break;
+	default:
+		dev_err(info->dev, "muic interrupt: irq %d occurred\n",
+				irq_type);
+		break;
+	}
+
+	if (ret < 0)
+		dev_err(info->dev, "failed to handle MUIC interrupt\n");
+
+	mutex_unlock(&info->mutex);
+
+	return;
+}
+
+static irqreturn_t max77693_muic_irq_handler(int irq, void *data)
+{
+	struct max77693_muic_info *info = data;
+
+	info->irq = irq;
+	schedule_work(&info->irq_work);
+
+	return IRQ_HANDLED;
+}
+
+static struct regmap_config max77693_muic_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+static int max77693_muic_detect_accessory(struct max77693_muic_info *info)
+{
+	int ret = 0;
+	int adc, chg_type;
+
+	mutex_lock(&info->mutex);
+
+	/* Read STATUSx register to detect accessory */
+	ret = max77693_bulk_read(info->max77693->regmap_muic,
+			MAX77693_MUIC_REG_STATUS1, 2, info->status);
+	if (ret) {
+		dev_err(info->dev, "failed to read MUIC register\n");
+		mutex_unlock(&info->mutex);
+		return -EINVAL;
+	}
+
+	adc = info->status[0] & STATUS1_ADC_MASK;
+	adc >>= STATUS1_ADC_SHIFT;
+
+	if (adc != MAX77693_MUIC_ADC_OPEN) {
+		dev_info(info->dev,
+			"external connector is attached (adc:0x%02x)\n", adc);
+
+		ret = max77693_muic_adc_handler(info, adc, true);
+		if (ret < 0)
+			dev_err(info->dev, "failed to detect accessory\n");
+		goto out;
+	}
+
+	chg_type = info->status[1] & STATUS2_CHGTYP_MASK;
+	chg_type >>= STATUS2_CHGTYP_SHIFT;
+
+	if (chg_type != MAX77693_CHARGER_TYPE_NONE) {
+		dev_info(info->dev,
+			"external connector is attached (chg_type:0x%x)\n",
+			chg_type);
+
+		max77693_muic_chg_handler(info, chg_type, true);
+		if (ret < 0)
+			dev_err(info->dev, "failed to detect charger accessory\n");
+	}
+
+out:
+	mutex_unlock(&info->mutex);
+	return ret;
+}
+
+static int __devinit max77693_muic_probe(struct platform_device *pdev)
+{
+	struct max77693_dev *max77693 = dev_get_drvdata(pdev->dev.parent);
+	struct max77693_muic_info *info;
+	int ret, i;
+	u8 id;
+
+	info = kzalloc(sizeof(struct max77693_muic_info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		ret = -ENOMEM;
+		goto err_kfree;
+	}
+	info->dev = &pdev->dev;
+	info->max77693 = max77693;
+	info->max77693->regmap_muic = regmap_init_i2c(info->max77693->muic,
+					 &max77693_muic_regmap_config);
+	if (IS_ERR(info->max77693->regmap_muic)) {
+		ret = PTR_ERR(info->max77693->regmap_muic);
+		dev_err(max77693->dev,
+			"failed to allocate register map: %d\n", ret);
+		goto err_regmap;
+	}
+	platform_set_drvdata(pdev, info);
+	mutex_init(&info->mutex);
+
+	INIT_WORK(&info->irq_work, max77693_muic_irq_work);
+
+	/* Support irq domain for MAX77693 MUIC device */
+	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++) {
+		struct max77693_muic_irq *muic_irq = &muic_irqs[i];
+		int virq = 0;
+
+		virq = irq_create_mapping(max77693->irq_domain, muic_irq->irq);
+		if (!virq)
+			goto err_irq;
+		muic_irq->virq = virq;
+
+		ret = request_threaded_irq(virq, NULL,
+				max77693_muic_irq_handler,
+				0, muic_irq->name, info);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed: irq request (IRQ: %d,"
+				" error :%d)\n",
+				muic_irq->irq, ret);
+
+			for (i = i - 1; i >= 0; i--)
+				free_irq(muic_irq->virq, info);
+			goto err_irq;
+		}
+	}
+
+	/* Initialize extcon device */
+	info->edev = kzalloc(sizeof(struct extcon_dev), GFP_KERNEL);
+	if (!info->edev) {
+		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
+		ret = -ENOMEM;
+		goto err_irq;
+	}
+	info->edev->name = DEV_NAME;
+	info->edev->supported_cable = max77693_extcon_cable;
+	ret = extcon_dev_register(info->edev, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register extcon device\n");
+		goto err_extcon;
+	}
+
+	/* Check revision number of MUIC device*/
+	ret = max77693_read_reg(info->max77693->regmap_muic,
+			MAX77693_MUIC_REG_ID, &id);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to read revision number\n");
+		goto err_extcon;
+	}
+	dev_info(info->dev, "device ID : 0x%x\n", id);
+
+	/* Set ADC debounce time */
+	max77693_muic_set_debounce_time(info, ADC_DEBOUNCE_TIME_25MS);
+
+	/* Detect accessory on boot */
+	max77693_muic_detect_accessory(info);
+
+	return ret;
+
+err_extcon:
+	kfree(info->edev);
+err_irq:
+err_regmap:
+	kfree(info);
+err_kfree:
+	return ret;
+}
+
+static int __devexit max77693_muic_remove(struct platform_device *pdev)
+{
+	struct max77693_muic_info *info = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++)
+		free_irq(muic_irqs[i].virq, info);
+	cancel_work_sync(&info->irq_work);
+	extcon_dev_unregister(info->edev);
+	kfree(info);
+
+	return 0;
+}
+
+static struct platform_driver max77693_muic_driver = {
+	.driver		= {
+		.name	= DEV_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= max77693_muic_probe,
+	.remove		= __devexit_p(max77693_muic_remove),
+};
+
+module_platform_driver(max77693_muic_driver);
+
+MODULE_DESCRIPTION("Maxim MAX77693 Extcon driver");
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:extcon-max77693");
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index a4ed30bd9a4..ef9090a4271 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -26,6 +26,7 @@
 #include <linux/mfd/max8997.h>
 #include <linux/mfd/max8997-private.h>
 #include <linux/extcon.h>
+#include <linux/irqdomain.h>
 
 #define	DEV_NAME			"max8997-muic"
 
@@ -77,6 +78,7 @@
 struct max8997_muic_irq {
 	unsigned int irq;
 	const char *name;
+	unsigned int virq;
 };
 
 static struct max8997_muic_irq muic_irqs[] = {
@@ -343,12 +345,10 @@ static void max8997_muic_irq_work(struct work_struct *work)
 {
 	struct max8997_muic_info *info = container_of(work,
 			struct max8997_muic_info, irq_work);
-	struct max8997_dev *max8997 = i2c_get_clientdata(info->muic);
 	u8 status[2];
 	u8 adc, chg_type;
-
-	int irq_type = info->irq - max8997->irq_base;
-	int ret;
+	int irq_type = 0;
+	int i, ret;
 
 	mutex_lock(&info->mutex);
 
@@ -363,6 +363,10 @@ static void max8997_muic_irq_work(struct work_struct *work)
 	dev_dbg(info->dev, "%s: STATUS1:0x%x, 2:0x%x\n", __func__,
 			status[0], status[1]);
 
+	for (i = 0 ; i < ARRAY_SIZE(muic_irqs) ; i++)
+		if (info->irq == muic_irqs[i].virq)
+			irq_type = muic_irqs[i].irq;
+
 	switch (irq_type) {
 	case MAX8997_MUICIRQ_ADC:
 		adc = status[0] & STATUS1_ADC_MASK;
@@ -448,11 +452,15 @@ static int __devinit max8997_muic_probe(struct platform_device *pdev)
 
 	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++) {
 		struct max8997_muic_irq *muic_irq = &muic_irqs[i];
+		int virq = 0;
+
+		virq = irq_create_mapping(max8997->irq_domain, muic_irq->irq);
+		if (!virq)
+			goto err_irq;
+		muic_irq->virq = virq;
 
-		ret = request_threaded_irq(pdata->irq_base + muic_irq->irq,
-				NULL, max8997_muic_irq_handler,
-				0, muic_irq->name,
-				info);
+		ret = request_threaded_irq(virq, NULL,max8997_muic_irq_handler,
+				0, muic_irq->name, info);
 		if (ret) {
 			dev_err(&pdev->dev,
 				"failed: irq request (IRQ: %d,"
@@ -496,7 +504,7 @@ err_extcon:
 	kfree(info->edev);
 err_irq:
 	while (--i >= 0)
-		free_irq(pdata->irq_base + muic_irqs[i].irq, info);
+		free_irq(muic_irqs[i].virq, info);
 	kfree(info);
 err_kfree:
 	return ret;
@@ -505,11 +513,10 @@ err_kfree:
 static int __devexit max8997_muic_remove(struct platform_device *pdev)
 {
 	struct max8997_muic_info *info = platform_get_drvdata(pdev);
-	struct max8997_dev *max8997 = i2c_get_clientdata(info->muic);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++)
-		free_irq(max8997->irq_base + muic_irqs[i].irq, info);
+		free_irq(muic_irqs[i].virq, info);
 	cancel_work_sync(&info->irq_work);
 
 	extcon_dev_unregister(info->edev);
diff --git a/drivers/extcon/extcon_class.c b/drivers/extcon/extcon_class.c
index 159aeb07b3b..f6419f9db76 100644
--- a/drivers/extcon/extcon_class.c
+++ b/drivers/extcon/extcon_class.c
@@ -65,7 +65,7 @@ const char *extcon_cable_name[] = {
 	NULL,
 };
 
-struct class *extcon_class;
+static struct class *extcon_class;
 #if defined(CONFIG_ANDROID)
 static struct class_compat *switch_class;
 #endif /* CONFIG_ANDROID */
diff --git a/drivers/extcon/extcon_gpio.c b/drivers/extcon/extcon_gpio.c
index 8a0dcc11c7c..fe3db45fa83 100644
--- a/drivers/extcon/extcon_gpio.c
+++ b/drivers/extcon/extcon_gpio.c
@@ -105,25 +105,25 @@ static int __devinit gpio_extcon_probe(struct platform_device *pdev)
 
 	ret = extcon_dev_register(&extcon_data->edev, &pdev->dev);
 	if (ret < 0)
-		goto err_extcon_dev_register;
+		return ret;
 
 	ret = gpio_request_one(extcon_data->gpio, GPIOF_DIR_IN, pdev->name);
 	if (ret < 0)
-		goto err_request_gpio;
+		goto err;
 
 	INIT_DELAYED_WORK(&extcon_data->work, gpio_extcon_work);
 
 	extcon_data->irq = gpio_to_irq(extcon_data->gpio);
 	if (extcon_data->irq < 0) {
 		ret = extcon_data->irq;
-		goto err_detect_irq_num_failed;
+		goto err;
 	}
 
 	ret = request_any_context_irq(extcon_data->irq, gpio_irq_handler,
 				      pdata->irq_flags, pdev->name,
 				      extcon_data);
 	if (ret < 0)
-		goto err_request_irq;
+		goto err;
 
 	platform_set_drvdata(pdev, extcon_data);
 	/* Perform initial detection */
@@ -131,13 +131,8 @@ static int __devinit gpio_extcon_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_request_irq:
-err_detect_irq_num_failed:
-	gpio_free(extcon_data->gpio);
-err_request_gpio:
+err:
 	extcon_dev_unregister(&extcon_data->edev);
-err_extcon_dev_register:
-	devm_kfree(&pdev->dev, extcon_data);
 
 	return ret;
 }
@@ -148,9 +143,7 @@ static int __devexit gpio_extcon_remove(struct platform_device *pdev)
 
 	cancel_delayed_work_sync(&extcon_data->work);
 	free_irq(extcon_data->irq, extcon_data);
-	gpio_free(extcon_data->gpio);
 	extcon_dev_unregister(&extcon_data->edev);
-	devm_kfree(&pdev->dev, extcon_data);
 
 	return 0;
 }
diff --git a/drivers/firewire/core-device.c b/drivers/firewire/core-device.c
index 4d460ef8716..7a05fd24d68 100644
--- a/drivers/firewire/core-device.c
+++ b/drivers/firewire/core-device.c
@@ -398,6 +398,14 @@ static ssize_t guid_show(struct device *dev,
 	return ret;
 }
 
+static ssize_t is_local_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct fw_device *device = fw_device(dev);
+
+	return sprintf(buf, "%u\n", device->is_local);
+}
+
 static int units_sprintf(char *buf, const u32 *directory)
 {
 	struct fw_csr_iterator ci;
@@ -447,6 +455,7 @@ static ssize_t units_show(struct device *dev,
 static struct device_attribute fw_device_attributes[] = {
 	__ATTR_RO(config_rom),
 	__ATTR_RO(guid),
+	__ATTR_RO(is_local),
 	__ATTR_RO(units),
 	__ATTR_NULL,
 };
diff --git a/drivers/firewire/core-iso.c b/drivers/firewire/core-iso.c
index 8382e27e9a2..38c0aa60b2c 100644
--- a/drivers/firewire/core-iso.c
+++ b/drivers/firewire/core-iso.c
@@ -146,7 +146,7 @@ EXPORT_SYMBOL(fw_iso_buffer_destroy);
 /* Convert DMA address to offset into virtually contiguous buffer. */
 size_t fw_iso_buffer_lookup(struct fw_iso_buffer *buffer, dma_addr_t completed)
 {
-	int i;
+	size_t i;
 	dma_addr_t address;
 	ssize_t offset;
 
diff --git a/drivers/firewire/core-transaction.c b/drivers/firewire/core-transaction.c
index 780708dc6e2..87d6f2d2f02 100644
--- a/drivers/firewire/core-transaction.c
+++ b/drivers/firewire/core-transaction.c
@@ -525,9 +525,10 @@ const struct fw_address_region fw_high_memory_region =
 	{ .start = 0x000100000000ULL, .end = 0xffffe0000000ULL,  };
 EXPORT_SYMBOL(fw_high_memory_region);
 
-#if 0
-const struct fw_address_region fw_low_memory_region =
+static const struct fw_address_region low_memory_region =
 	{ .start = 0x000000000000ULL, .end = 0x000100000000ULL,  };
+
+#if 0
 const struct fw_address_region fw_private_region =
 	{ .start = 0xffffe0000000ULL, .end = 0xfffff0000000ULL,  };
 const struct fw_address_region fw_csr_region =
@@ -1198,6 +1199,23 @@ static struct fw_address_handler registers = {
 	.address_callback	= handle_registers,
 };
 
+static void handle_low_memory(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,
+		void *callback_data)
+{
+	/*
+	 * This catches requests not handled by the physical DMA unit,
+	 * i.e., wrong transaction types or unauthorized source nodes.
+	 */
+	fw_send_response(card, request, RCODE_TYPE_ERROR);
+}
+
+static struct fw_address_handler low_memory = {
+	.length			= 0x000100000000ULL,
+	.address_callback	= handle_low_memory,
+};
+
 MODULE_AUTHOR("Kristian Hoegsberg <krh@bitplanet.net>");
 MODULE_DESCRIPTION("Core IEEE1394 transaction logic");
 MODULE_LICENSE("GPL");
@@ -1259,6 +1277,7 @@ static int __init fw_core_init(void)
 
 	fw_core_add_address_handler(&topology_map, &topology_map_region);
 	fw_core_add_address_handler(&registers, &registers_region);
+	fw_core_add_address_handler(&low_memory, &low_memory_region);
 	fw_core_add_descriptor(&vendor_id_descriptor);
 	fw_core_add_descriptor(&model_id_descriptor);
 
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index c1af05e834b..c788dbdaf3b 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -191,6 +191,7 @@ struct fw_ohci {
 	unsigned quirks;
 	unsigned int pri_req_max;
 	u32 bus_time;
+	bool bus_time_running;
 	bool is_root;
 	bool csr_state_setclear_abdicate;
 	int n_ir;
@@ -1726,6 +1727,13 @@ static u32 update_bus_time(struct fw_ohci *ohci)
 {
 	u32 cycle_time_seconds = get_cycle_time(ohci) >> 25;
 
+	if (unlikely(!ohci->bus_time_running)) {
+		reg_write(ohci, OHCI1394_IntMaskSet, OHCI1394_cycle64Seconds);
+		ohci->bus_time = (lower_32_bits(get_seconds()) & ~0x7f) |
+		                 (cycle_time_seconds & 0x40);
+		ohci->bus_time_running = true;
+	}
+
 	if ((ohci->bus_time & 0x40) != (cycle_time_seconds & 0x40))
 		ohci->bus_time += 0x40;
 
@@ -2213,7 +2221,7 @@ static int ohci_enable(struct fw_card *card,
 {
 	struct fw_ohci *ohci = fw_ohci(card);
 	struct pci_dev *dev = to_pci_dev(card->device);
-	u32 lps, seconds, version, irqs;
+	u32 lps, version, irqs;
 	int i, ret;
 
 	if (software_reset(ohci)) {
@@ -2269,9 +2277,12 @@ static int ohci_enable(struct fw_card *card,
 		  (OHCI1394_MAX_PHYS_RESP_RETRIES << 8) |
 		  (200 << 16));
 
-	seconds = lower_32_bits(get_seconds());
-	reg_write(ohci, OHCI1394_IsochronousCycleTimer, seconds << 25);
-	ohci->bus_time = seconds & ~0x3f;
+	ohci->bus_time_running = false;
+
+	for (i = 0; i < 32; i++)
+		if (ohci->ir_context_support & (1 << i))
+			reg_write(ohci, OHCI1394_IsoRcvContextControlClear(i),
+				  IR_CONTEXT_MULTI_CHANNEL_MODE);
 
 	version = reg_read(ohci, OHCI1394_Version) & 0x00ff00ff;
 	if (version >= OHCI_VERSION_1_1) {
@@ -2369,7 +2380,6 @@ static int ohci_enable(struct fw_card *card,
 		OHCI1394_postedWriteErr |
 		OHCI1394_selfIDComplete |
 		OHCI1394_regAccessFail |
-		OHCI1394_cycle64Seconds |
 		OHCI1394_cycleInconsistent |
 		OHCI1394_unrecoverableError |
 		OHCI1394_cycleTooLong |
@@ -2658,7 +2668,8 @@ static void ohci_write_csr(struct fw_card *card, int csr_offset, u32 value)
 
 	case CSR_BUS_TIME:
 		spin_lock_irqsave(&ohci->lock, flags);
-		ohci->bus_time = (ohci->bus_time & 0x7f) | (value & ~0x7f);
+		ohci->bus_time = (update_bus_time(ohci) & 0x40) |
+		                 (value & ~0x7f);
 		spin_unlock_irqrestore(&ohci->lock, flags);
 		break;
 
@@ -3539,6 +3550,13 @@ static int __devinit pci_probe(struct pci_dev *dev,
 
 	INIT_WORK(&ohci->bus_reset_work, bus_reset_work);
 
+	if (!(pci_resource_flags(dev, 0) & IORESOURCE_MEM) ||
+	    pci_resource_len(dev, 0) < OHCI1394_REGISTER_SIZE) {
+		dev_err(&dev->dev, "invalid MMIO resource\n");
+		err = -ENXIO;
+		goto fail_disable;
+	}
+
 	err = pci_request_region(dev, 0, ohci_driver_name);
 	if (err) {
 		dev_err(&dev->dev, "MMIO resource unavailable\n");
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 153980be4ee..b298158cb92 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -6,6 +6,7 @@
 #include <linux/dmi.h>
 #include <linux/efi.h>
 #include <linux/bootmem.h>
+#include <linux/random.h>
 #include <asm/dmi.h>
 
 /*
@@ -111,6 +112,8 @@ static int __init dmi_walk_early(void (*decode)(const struct dmi_header *,
 
 	dmi_table(buf, dmi_len, dmi_num, decode, NULL);
 
+	add_device_randomness(buf, dmi_len);
+
 	dmi_iounmap(buf, dmi_len);
 	return 0;
 }
diff --git a/drivers/firmware/memmap.c b/drivers/firmware/memmap.c
index adc07102a20..c1cdc923666 100644
--- a/drivers/firmware/memmap.c
+++ b/drivers/firmware/memmap.c
@@ -98,7 +98,7 @@ static LIST_HEAD(map_entries);
 /**
  * firmware_map_add_entry() - Does the real work to add a firmware memmap entry.
  * @start: Start of the memory range.
- * @end:   End of the memory range (inclusive).
+ * @end:   End of the memory range (exclusive).
  * @type:  Type of the memory range.
  * @entry: Pre-allocated (either kmalloc() or bootmem allocator), uninitialised
  *         entry.
@@ -113,7 +113,7 @@ static int firmware_map_add_entry(u64 start, u64 end,
 	BUG_ON(start > end);
 
 	entry->start = start;
-	entry->end = end;
+	entry->end = end - 1;
 	entry->type = type;
 	INIT_LIST_HEAD(&entry->list);
 	kobject_init(&entry->kobj, &memmap_ktype);
@@ -148,7 +148,7 @@ static int add_sysfs_fw_map_entry(struct firmware_map_entry *entry)
  * firmware_map_add_hotplug() - Adds a firmware mapping entry when we do
  * memory hotplug.
  * @start: Start of the memory range.
- * @end:   End of the memory range (inclusive).
+ * @end:   End of the memory range (exclusive)
  * @type:  Type of the memory range.
  *
  * Adds a firmware mapping entry. This function is for memory hotplug, it is
@@ -175,7 +175,7 @@ int __meminit firmware_map_add_hotplug(u64 start, u64 end, const char *type)
 /**
  * firmware_map_add_early() - Adds a firmware mapping entry.
  * @start: Start of the memory range.
- * @end:   End of the memory range (inclusive).
+ * @end:   End of the memory range.
  * @type:  Type of the memory range.
  *
  * Adds a firmware mapping entry. This function uses the bootmem allocator
diff --git a/drivers/firmware/pcdp.c b/drivers/firmware/pcdp.c
index 51e0e2d8fac..a330492e06f 100644
--- a/drivers/firmware/pcdp.c
+++ b/drivers/firmware/pcdp.c
@@ -95,7 +95,7 @@ efi_setup_pcdp_console(char *cmdline)
 	if (efi.hcdp == EFI_INVALID_TABLE_ADDR)
 		return -ENODEV;
 
-	pcdp = ioremap(efi.hcdp, 4096);
+	pcdp = early_ioremap(efi.hcdp, 4096);
 	printk(KERN_INFO "PCDP: v%d at 0x%lx\n", pcdp->rev, efi.hcdp);
 
 	if (strstr(cmdline, "console=hcdp")) {
@@ -131,6 +131,6 @@ efi_setup_pcdp_console(char *cmdline)
 	}
 
 out:
-	iounmap(pcdp);
+	early_iounmap(pcdp, 4096);
 	return rc;
 }
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 542f0c04b69..b16c8a72a2e 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -253,6 +253,12 @@ config GPIO_GE_FPGA
 
 comment "I2C GPIO expanders:"
 
+config GPIO_ARIZONA
+	tristate "Wolfson Microelectronics Arizona class devices"
+	depends on MFD_ARIZONA
+	help
+	  Support for GPIOs on Wolfson Arizona class devices.
+
 config GPIO_MAX7300
 	tristate "Maxim MAX7300 GPIO expander"
 	depends on I2C
@@ -466,6 +472,18 @@ config GPIO_BT8XX
 
 	  If unsure, say N.
 
+config GPIO_AMD8111
+	tristate "AMD 8111 GPIO driver"
+	depends on PCI
+	help
+	  The AMD 8111 south bridge contains 32 GPIO pins which can be used.
+
+	  Note, that usually system firmware/ACPI handles GPIO pins on their
+	  own and users might easily break their systems with uncarefull usage
+	  of this driver!
+
+	  If unsure, say N
+
 config GPIO_LANGWELL
 	bool "Intel Langwell/Penwell GPIO support"
 	depends on PCI && X86
@@ -579,6 +597,13 @@ config GPIO_AB8500
 	help
 	  Select this to enable the AB8500 IC GPIO driver
 
+config GPIO_TPS6586X
+	bool "TPS6586X GPIO"
+	depends on MFD_TPS6586X
+	help
+	  Select this option to enable GPIO driver for the TPS6586X
+	  chip family.
+
 config GPIO_TPS65910
 	bool "TPS65910 GPIO"
 	depends on MFD_TPS65910
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 0f55662002c..153caceeb05 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -12,6 +12,8 @@ obj-$(CONFIG_GPIO_74X164)	+= gpio-74x164.o
 obj-$(CONFIG_GPIO_AB8500)	+= gpio-ab8500.o
 obj-$(CONFIG_GPIO_ADP5520)	+= gpio-adp5520.o
 obj-$(CONFIG_GPIO_ADP5588)	+= gpio-adp5588.o
+obj-$(CONFIG_GPIO_AMD8111)	+= gpio-amd8111.o
+obj-$(CONFIG_GPIO_ARIZONA)	+= gpio-arizona.o
 obj-$(CONFIG_GPIO_BT8XX)	+= gpio-bt8xx.o
 obj-$(CONFIG_GPIO_CS5535)	+= gpio-cs5535.o
 obj-$(CONFIG_GPIO_DA9052)	+= gpio-da9052.o
@@ -61,6 +63,7 @@ obj-$(CONFIG_GPIO_TC3589X)	+= gpio-tc3589x.o
 obj-$(CONFIG_ARCH_TEGRA)	+= gpio-tegra.o
 obj-$(CONFIG_GPIO_TIMBERDALE)	+= gpio-timberdale.o
 obj-$(CONFIG_ARCH_DAVINCI_TNETV107X) += gpio-tnetv107x.o
+obj-$(CONFIG_GPIO_TPS6586X)	+= gpio-tps6586x.o
 obj-$(CONFIG_GPIO_TPS65910)	+= gpio-tps65910.o
 obj-$(CONFIG_GPIO_TPS65912)	+= gpio-tps65912.o
 obj-$(CONFIG_GPIO_TWL4030)	+= gpio-twl4030.o
diff --git a/drivers/gpio/gpio-amd8111.c b/drivers/gpio/gpio-amd8111.c
new file mode 100644
index 00000000000..710fafcdd1b
--- /dev/null
+++ b/drivers/gpio/gpio-amd8111.c
@@ -0,0 +1,246 @@
+/*
+ * GPIO driver for AMD 8111 south bridges
+ *
+ * Copyright (c) 2012 Dmitry Eremin-Solenikov
+ *
+ * Based on the AMD RNG driver:
+ * Copyright 2005 (c) MontaVista Software, Inc.
+ * with the majority of the code coming from:
+ *
+ * Hardware driver for the Intel/AMD/VIA Random Number Generators (RNG)
+ * (c) Copyright 2003 Red Hat Inc <jgarzik@redhat.com>
+ *
+ * derived from
+ *
+ * Hardware driver for the AMD 768 Random Number Generator (RNG)
+ * (c) Copyright 2001 Red Hat Inc
+ *
+ * derived from
+ *
+ * Hardware driver for Intel i810 Random Number Generator (RNG)
+ * Copyright 2000,2001 Jeff Garzik <jgarzik@pobox.com>
+ * Copyright 2000,2001 Philipp Rumpf <prumpf@mandrakesoft.com>
+ *
+ * This file is licensed under  the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#define PMBASE_OFFSET 0xb0
+#define PMBASE_SIZE   0x30
+
+#define AMD_REG_GPIO(i) (0x10 + (i))
+
+#define AMD_GPIO_LTCH_STS	0x40 /* Latch status, w1 */
+#define AMD_GPIO_RTIN		0x20 /* Real Time in, ro */
+#define AMD_GPIO_DEBOUNCE	0x10 /* Debounce, rw */
+#define AMD_GPIO_MODE_MASK	0x0c /* Pin Mode Select, rw */
+#define AMD_GPIO_MODE_IN	0x00
+#define AMD_GPIO_MODE_OUT	0x04
+/* Enable alternative (e.g. clkout, IRQ, etc) function of the pin */
+#define AMD_GPIO_MODE_ALTFN	0x08 /* Or 0x09 */
+#define AMD_GPIO_X_MASK		0x03 /* In/Out specific, rw */
+#define AMD_GPIO_X_IN_ACTIVEHI	0x01 /* Active High */
+#define AMD_GPIO_X_IN_LATCH	0x02 /* Latched version is selected */
+#define AMD_GPIO_X_OUT_LOW	0x00
+#define AMD_GPIO_X_OUT_HI	0x01
+#define AMD_GPIO_X_OUT_CLK0	0x02
+#define AMD_GPIO_X_OUT_CLK1	0x03
+
+/*
+ * Data for PCI driver interface
+ *
+ * This data only exists for exporting the supported
+ * PCI ids via MODULE_DEVICE_TABLE.  We do not actually
+ * register a pci_driver, because someone else might one day
+ * want to register another driver on the same PCI id.
+ */
+static DEFINE_PCI_DEVICE_TABLE(pci_tbl) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_8111_SMBUS), 0 },
+	{ 0, },	/* terminate list */
+};
+MODULE_DEVICE_TABLE(pci, pci_tbl);
+
+struct amd_gpio {
+	struct gpio_chip	chip;
+	u32			pmbase;
+	void __iomem		*pm;
+	struct pci_dev		*pdev;
+	spinlock_t		lock; /* guards hw registers and orig table */
+	u8			orig[32];
+};
+
+#define to_agp(chip)	container_of(chip, struct amd_gpio, chip)
+
+static int amd_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	struct amd_gpio *agp = to_agp(chip);
+
+	agp->orig[offset] = ioread8(agp->pm + AMD_REG_GPIO(offset)) &
+		(AMD_GPIO_DEBOUNCE | AMD_GPIO_MODE_MASK | AMD_GPIO_X_MASK);
+
+	dev_dbg(&agp->pdev->dev, "Requested gpio %d, data %x\n", offset, agp->orig[offset]);
+
+	return 0;
+}
+
+static void amd_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	struct amd_gpio *agp = to_agp(chip);
+
+	dev_dbg(&agp->pdev->dev, "Freed gpio %d, data %x\n", offset, agp->orig[offset]);
+
+	iowrite8(agp->orig[offset], agp->pm + AMD_REG_GPIO(offset));
+}
+
+static void amd_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct amd_gpio *agp = to_agp(chip);
+	u8 temp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&agp->lock, flags);
+	temp = ioread8(agp->pm + AMD_REG_GPIO(offset));
+	temp = (temp & AMD_GPIO_DEBOUNCE) | AMD_GPIO_MODE_OUT | (value ? AMD_GPIO_X_OUT_HI : AMD_GPIO_X_OUT_LOW);
+	iowrite8(temp, agp->pm + AMD_REG_GPIO(offset));
+	spin_unlock_irqrestore(&agp->lock, flags);
+
+	dev_dbg(&agp->pdev->dev, "Setting gpio %d, value %d, reg=%02x\n", offset, !!value, temp);
+}
+
+static int amd_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct amd_gpio *agp = to_agp(chip);
+	u8 temp;
+
+	temp = ioread8(agp->pm + AMD_REG_GPIO(offset));
+
+	dev_dbg(&agp->pdev->dev, "Getting gpio %d, reg=%02x\n", offset, temp);
+
+	return (temp & AMD_GPIO_RTIN) ? 1 : 0;
+}
+
+static int amd_gpio_dirout(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct amd_gpio *agp = to_agp(chip);
+	u8 temp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&agp->lock, flags);
+	temp = ioread8(agp->pm + AMD_REG_GPIO(offset));
+	temp = (temp & AMD_GPIO_DEBOUNCE) | AMD_GPIO_MODE_OUT | (value ? AMD_GPIO_X_OUT_HI : AMD_GPIO_X_OUT_LOW);
+	iowrite8(temp, agp->pm + AMD_REG_GPIO(offset));
+	spin_unlock_irqrestore(&agp->lock, flags);
+
+	dev_dbg(&agp->pdev->dev, "Dirout gpio %d, value %d, reg=%02x\n", offset, !!value, temp);
+
+	return 0;
+}
+
+static int amd_gpio_dirin(struct gpio_chip *chip, unsigned offset)
+{
+	struct amd_gpio *agp = to_agp(chip);
+	u8 temp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&agp->lock, flags);
+	temp = ioread8(agp->pm + AMD_REG_GPIO(offset));
+	temp = (temp & AMD_GPIO_DEBOUNCE) | AMD_GPIO_MODE_IN;
+	iowrite8(temp, agp->pm + AMD_REG_GPIO(offset));
+	spin_unlock_irqrestore(&agp->lock, flags);
+
+	dev_dbg(&agp->pdev->dev, "Dirin gpio %d, reg=%02x\n", offset, temp);
+
+	return 0;
+}
+
+static struct amd_gpio gp = {
+	.chip = {
+		.label		= "AMD GPIO",
+		.owner		= THIS_MODULE,
+		.base		= -1,
+		.ngpio		= 32,
+		.request	= amd_gpio_request,
+		.free		= amd_gpio_free,
+		.set		= amd_gpio_set,
+		.get		= amd_gpio_get,
+		.direction_output = amd_gpio_dirout,
+		.direction_input = amd_gpio_dirin,
+	},
+};
+
+static int __init amd_gpio_init(void)
+{
+	int err = -ENODEV;
+	struct pci_dev *pdev = NULL;
+	const struct pci_device_id *ent;
+
+
+	/* We look for our device - AMD South Bridge
+	 * I don't know about a system with two such bridges,
+	 * so we can assume that there is max. one device.
+	 *
+	 * We can't use plain pci_driver mechanism,
+	 * as the device is really a multiple function device,
+	 * main driver that binds to the pci_device is an smbus
+	 * driver and have to find & bind to the device this way.
+	 */
+	for_each_pci_dev(pdev) {
+		ent = pci_match_id(pci_tbl, pdev);
+		if (ent)
+			goto found;
+	}
+	/* Device not found. */
+	goto out;
+
+found:
+	err = pci_read_config_dword(pdev, 0x58, &gp.pmbase);
+	if (err)
+		goto out;
+	err = -EIO;
+	gp.pmbase &= 0x0000FF00;
+	if (gp.pmbase == 0)
+		goto out;
+	if (!request_region(gp.pmbase + PMBASE_OFFSET, PMBASE_SIZE, "AMD GPIO")) {
+		dev_err(&pdev->dev, "AMD GPIO region 0x%x already in use!\n",
+			gp.pmbase + PMBASE_OFFSET);
+		err = -EBUSY;
+		goto out;
+	}
+	gp.pm = ioport_map(gp.pmbase + PMBASE_OFFSET, PMBASE_SIZE);
+	gp.pdev = pdev;
+	gp.chip.dev = &pdev->dev;
+
+	spin_lock_init(&gp.lock);
+
+	printk(KERN_INFO "AMD-8111 GPIO detected\n");
+	err = gpiochip_add(&gp.chip);
+	if (err) {
+		printk(KERN_ERR "GPIO registering failed (%d)\n",
+		       err);
+		release_region(gp.pmbase + PMBASE_OFFSET, PMBASE_SIZE);
+		goto out;
+	}
+out:
+	return err;
+}
+
+static void __exit amd_gpio_exit(void)
+{
+	int err = gpiochip_remove(&gp.chip);
+	WARN_ON(err);
+	ioport_unmap(gp.pm);
+	release_region(gp.pmbase + PMBASE_OFFSET, PMBASE_SIZE);
+}
+
+module_init(amd_gpio_init);
+module_exit(amd_gpio_exit);
+
+MODULE_AUTHOR("The Linux Kernel team");
+MODULE_DESCRIPTION("GPIO driver for AMD chipsets");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-arizona.c b/drivers/gpio/gpio-arizona.c
new file mode 100644
index 00000000000..8740d2eb06f
--- /dev/null
+++ b/drivers/gpio/gpio-arizona.c
@@ -0,0 +1,163 @@
+/*
+ * gpiolib support for Wolfson Arizona class devices
+ *
+ * Copyright 2012 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/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/pdata.h>
+#include <linux/mfd/arizona/registers.h>
+
+struct arizona_gpio {
+	struct arizona *arizona;
+	struct gpio_chip gpio_chip;
+};
+
+static inline struct arizona_gpio *to_arizona_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct arizona_gpio, gpio_chip);
+}
+
+static int arizona_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
+{
+	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona *arizona = arizona_gpio->arizona;
+
+	return regmap_update_bits(arizona->regmap, ARIZONA_GPIO1_CTRL + offset,
+				  ARIZONA_GPN_DIR, ARIZONA_GPN_DIR);
+}
+
+static int arizona_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona *arizona = arizona_gpio->arizona;
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(arizona->regmap, ARIZONA_GPIO1_CTRL + offset, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val & ARIZONA_GPN_LVL)
+		return 1;
+	else
+		return 0;
+}
+
+static int arizona_gpio_direction_out(struct gpio_chip *chip,
+				     unsigned offset, int value)
+{
+	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona *arizona = arizona_gpio->arizona;
+
+	if (value)
+		value = ARIZONA_GPN_LVL;
+
+	return regmap_update_bits(arizona->regmap, ARIZONA_GPIO1_CTRL + offset,
+				  ARIZONA_GPN_DIR | ARIZONA_GPN_LVL, value);
+}
+
+static void arizona_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona *arizona = arizona_gpio->arizona;
+
+	if (value)
+		value = ARIZONA_GPN_LVL;
+
+	regmap_update_bits(arizona->regmap, ARIZONA_GPIO1_CTRL + offset,
+			   ARIZONA_GPN_LVL, value);
+}
+
+static struct gpio_chip template_chip = {
+	.label			= "arizona",
+	.owner			= THIS_MODULE,
+	.direction_input	= arizona_gpio_direction_in,
+	.get			= arizona_gpio_get,
+	.direction_output	= arizona_gpio_direction_out,
+	.set			= arizona_gpio_set,
+	.can_sleep		= 1,
+};
+
+static int __devinit arizona_gpio_probe(struct platform_device *pdev)
+{
+	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
+	struct arizona_pdata *pdata = arizona->dev->platform_data;
+	struct arizona_gpio *arizona_gpio;
+	int ret;
+
+	arizona_gpio = devm_kzalloc(&pdev->dev, sizeof(*arizona_gpio),
+				    GFP_KERNEL);
+	if (arizona_gpio == NULL)
+		return -ENOMEM;
+
+	arizona_gpio->arizona = arizona;
+	arizona_gpio->gpio_chip = template_chip;
+	arizona_gpio->gpio_chip.dev = &pdev->dev;
+
+	switch (arizona->type) {
+	case WM5102:
+	case WM5110:
+		arizona_gpio->gpio_chip.ngpio = 5;
+		break;
+	default:
+		dev_err(&pdev->dev, "Unknown chip variant %d\n",
+			arizona->type);
+		return -EINVAL;
+	}
+
+	if (pdata && pdata->gpio_base)
+		arizona_gpio->gpio_chip.base = pdata->gpio_base;
+	else
+		arizona_gpio->gpio_chip.base = -1;
+
+	ret = gpiochip_add(&arizona_gpio->gpio_chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Could not register gpiochip, %d\n",
+			ret);
+		goto err;
+	}
+
+	platform_set_drvdata(pdev, arizona_gpio);
+
+	return ret;
+
+err:
+	return ret;
+}
+
+static int __devexit arizona_gpio_remove(struct platform_device *pdev)
+{
+	struct arizona_gpio *arizona_gpio = platform_get_drvdata(pdev);
+
+	return gpiochip_remove(&arizona_gpio->gpio_chip);
+}
+
+static struct platform_driver arizona_gpio_driver = {
+	.driver.name	= "arizona-gpio",
+	.driver.owner	= THIS_MODULE,
+	.probe		= arizona_gpio_probe,
+	.remove		= __devexit_p(arizona_gpio_remove),
+};
+
+module_platform_driver(arizona_gpio_driver);
+
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_DESCRIPTION("GPIO interface for Arizona devices");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:arizona-gpio");
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index c2199beca98..8a420f13905 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -1,5 +1,5 @@
 /*
- * arch/arm/mach-lpc32xx/gpiolib.c
+ * GPIO driver for LPC32xx SoC
  *
  * Author: Kevin Wells <kevin.wells@nxp.com>
  *
@@ -28,6 +28,7 @@
 #include <mach/hardware.h>
 #include <mach/platform.h>
 #include <mach/gpio-lpc32xx.h>
+#include <mach/irqs.h>
 
 #define LPC32XX_GPIO_P3_INP_STATE		_GPREG(0x000)
 #define LPC32XX_GPIO_P3_OUTP_SET		_GPREG(0x004)
@@ -367,6 +368,66 @@ static int lpc32xx_gpio_request(struct gpio_chip *chip, unsigned pin)
 	return -EINVAL;
 }
 
+static int lpc32xx_gpio_to_irq_p01(struct gpio_chip *chip, unsigned offset)
+{
+	return IRQ_LPC32XX_P0_P1_IRQ;
+}
+
+static const char lpc32xx_gpio_to_irq_gpio_p3_table[] = {
+	IRQ_LPC32XX_GPIO_00,
+	IRQ_LPC32XX_GPIO_01,
+	IRQ_LPC32XX_GPIO_02,
+	IRQ_LPC32XX_GPIO_03,
+	IRQ_LPC32XX_GPIO_04,
+	IRQ_LPC32XX_GPIO_05,
+};
+
+static int lpc32xx_gpio_to_irq_gpio_p3(struct gpio_chip *chip, unsigned offset)
+{
+	if (offset < ARRAY_SIZE(lpc32xx_gpio_to_irq_gpio_p3_table))
+		return lpc32xx_gpio_to_irq_gpio_p3_table[offset];
+	return -ENXIO;
+}
+
+static const char lpc32xx_gpio_to_irq_gpi_p3_table[] = {
+	IRQ_LPC32XX_GPI_00,
+	IRQ_LPC32XX_GPI_01,
+	IRQ_LPC32XX_GPI_02,
+	IRQ_LPC32XX_GPI_03,
+	IRQ_LPC32XX_GPI_04,
+	IRQ_LPC32XX_GPI_05,
+	IRQ_LPC32XX_GPI_06,
+	IRQ_LPC32XX_GPI_07,
+	IRQ_LPC32XX_GPI_08,
+	IRQ_LPC32XX_GPI_09,
+	-ENXIO, /* 10 */
+	-ENXIO, /* 11 */
+	-ENXIO, /* 12 */
+	-ENXIO, /* 13 */
+	-ENXIO, /* 14 */
+	-ENXIO, /* 15 */
+	-ENXIO, /* 16 */
+	-ENXIO, /* 17 */
+	-ENXIO, /* 18 */
+	IRQ_LPC32XX_GPI_19,
+	-ENXIO, /* 20 */
+	-ENXIO, /* 21 */
+	-ENXIO, /* 22 */
+	-ENXIO, /* 23 */
+	-ENXIO, /* 24 */
+	-ENXIO, /* 25 */
+	-ENXIO, /* 26 */
+	-ENXIO, /* 27 */
+	IRQ_LPC32XX_GPI_28,
+};
+
+static int lpc32xx_gpio_to_irq_gpi_p3(struct gpio_chip *chip, unsigned offset)
+{
+	if (offset < ARRAY_SIZE(lpc32xx_gpio_to_irq_gpi_p3_table))
+		return lpc32xx_gpio_to_irq_gpi_p3_table[offset];
+	return -ENXIO;
+}
+
 static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 	{
 		.chip = {
@@ -376,6 +437,7 @@ static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 			.direction_output	= lpc32xx_gpio_dir_output_p012,
 			.set			= lpc32xx_gpio_set_value_p012,
 			.request		= lpc32xx_gpio_request,
+			.to_irq			= lpc32xx_gpio_to_irq_p01,
 			.base			= LPC32XX_GPIO_P0_GRP,
 			.ngpio			= LPC32XX_GPIO_P0_MAX,
 			.names			= gpio_p0_names,
@@ -391,6 +453,7 @@ static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 			.direction_output	= lpc32xx_gpio_dir_output_p012,
 			.set			= lpc32xx_gpio_set_value_p012,
 			.request		= lpc32xx_gpio_request,
+			.to_irq			= lpc32xx_gpio_to_irq_p01,
 			.base			= LPC32XX_GPIO_P1_GRP,
 			.ngpio			= LPC32XX_GPIO_P1_MAX,
 			.names			= gpio_p1_names,
@@ -421,6 +484,7 @@ static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 			.direction_output	= lpc32xx_gpio_dir_output_p3,
 			.set			= lpc32xx_gpio_set_value_p3,
 			.request		= lpc32xx_gpio_request,
+			.to_irq			= lpc32xx_gpio_to_irq_gpio_p3,
 			.base			= LPC32XX_GPIO_P3_GRP,
 			.ngpio			= LPC32XX_GPIO_P3_MAX,
 			.names			= gpio_p3_names,
@@ -434,6 +498,7 @@ static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 			.direction_input	= lpc32xx_gpio_dir_in_always,
 			.get			= lpc32xx_gpi_get_value,
 			.request		= lpc32xx_gpio_request,
+			.to_irq			= lpc32xx_gpio_to_irq_gpi_p3,
 			.base			= LPC32XX_GPI_P3_GRP,
 			.ngpio			= LPC32XX_GPI_P3_MAX,
 			.names			= gpi_p3_names,
@@ -457,13 +522,6 @@ static struct lpc32xx_gpio_chip lpc32xx_gpiochip[] = {
 	},
 };
 
-/* Empty now, can be removed later when mach-lpc32xx is finally switched over
- * to DT support
- */
-void __init lpc32xx_gpio_init(void)
-{
-}
-
 static int lpc32xx_of_xlate(struct gpio_chip *gc,
 			    const struct of_phandle_args *gpiospec, u32 *flags)
 {
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index c89c4c1e668..4db460b6ecf 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -23,6 +23,7 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
@@ -33,12 +34,11 @@
 #include <asm-generic/bug.h>
 #include <asm/mach/irq.h>
 
-#define irq_to_gpio(irq)	((irq) - MXC_GPIO_IRQ_START)
-
 enum mxc_gpio_hwtype {
 	IMX1_GPIO,	/* runs on i.mx1 */
 	IMX21_GPIO,	/* runs on i.mx21 and i.mx27 */
-	IMX31_GPIO,	/* runs on all other i.mx */
+	IMX31_GPIO,	/* runs on i.mx31 */
+	IMX35_GPIO,	/* runs on all other i.mx */
 };
 
 /* device type dependent stuff */
@@ -50,6 +50,7 @@ struct mxc_gpio_hwdata {
 	unsigned icr2_reg;
 	unsigned imr_reg;
 	unsigned isr_reg;
+	int edge_sel_reg;
 	unsigned low_level;
 	unsigned high_level;
 	unsigned rise_edge;
@@ -61,7 +62,7 @@ struct mxc_gpio_port {
 	void __iomem *base;
 	int irq;
 	int irq_high;
-	int virtual_irq_start;
+	struct irq_domain *domain;
 	struct bgpio_chip bgc;
 	u32 both_edges;
 };
@@ -74,6 +75,7 @@ static struct mxc_gpio_hwdata imx1_imx21_gpio_hwdata = {
 	.icr2_reg	= 0x2c,
 	.imr_reg	= 0x30,
 	.isr_reg	= 0x34,
+	.edge_sel_reg	= -EINVAL,
 	.low_level	= 0x03,
 	.high_level	= 0x02,
 	.rise_edge	= 0x00,
@@ -88,6 +90,22 @@ static struct mxc_gpio_hwdata imx31_gpio_hwdata = {
 	.icr2_reg	= 0x10,
 	.imr_reg	= 0x14,
 	.isr_reg	= 0x18,
+	.edge_sel_reg	= -EINVAL,
+	.low_level	= 0x00,
+	.high_level	= 0x01,
+	.rise_edge	= 0x02,
+	.fall_edge	= 0x03,
+};
+
+static struct mxc_gpio_hwdata imx35_gpio_hwdata = {
+	.dr_reg		= 0x00,
+	.gdir_reg	= 0x04,
+	.psr_reg	= 0x08,
+	.icr1_reg	= 0x0c,
+	.icr2_reg	= 0x10,
+	.imr_reg	= 0x14,
+	.isr_reg	= 0x18,
+	.edge_sel_reg	= 0x1c,
 	.low_level	= 0x00,
 	.high_level	= 0x01,
 	.rise_edge	= 0x02,
@@ -104,12 +122,13 @@ static struct mxc_gpio_hwdata *mxc_gpio_hwdata;
 #define GPIO_ICR2		(mxc_gpio_hwdata->icr2_reg)
 #define GPIO_IMR		(mxc_gpio_hwdata->imr_reg)
 #define GPIO_ISR		(mxc_gpio_hwdata->isr_reg)
+#define GPIO_EDGE_SEL		(mxc_gpio_hwdata->edge_sel_reg)
 
 #define GPIO_INT_LOW_LEV	(mxc_gpio_hwdata->low_level)
 #define GPIO_INT_HIGH_LEV	(mxc_gpio_hwdata->high_level)
 #define GPIO_INT_RISE_EDGE	(mxc_gpio_hwdata->rise_edge)
 #define GPIO_INT_FALL_EDGE	(mxc_gpio_hwdata->fall_edge)
-#define GPIO_INT_NONE		0x4
+#define GPIO_INT_BOTH_EDGES	0x4
 
 static struct platform_device_id mxc_gpio_devtype[] = {
 	{
@@ -122,6 +141,9 @@ static struct platform_device_id mxc_gpio_devtype[] = {
 		.name = "imx31-gpio",
 		.driver_data = IMX31_GPIO,
 	}, {
+		.name = "imx35-gpio",
+		.driver_data = IMX35_GPIO,
+	}, {
 		/* sentinel */
 	}
 };
@@ -130,6 +152,7 @@ static const struct of_device_id mxc_gpio_dt_ids[] = {
 	{ .compatible = "fsl,imx1-gpio", .data = &mxc_gpio_devtype[IMX1_GPIO], },
 	{ .compatible = "fsl,imx21-gpio", .data = &mxc_gpio_devtype[IMX21_GPIO], },
 	{ .compatible = "fsl,imx31-gpio", .data = &mxc_gpio_devtype[IMX31_GPIO], },
+	{ .compatible = "fsl,imx35-gpio", .data = &mxc_gpio_devtype[IMX35_GPIO], },
 	{ /* sentinel */ }
 };
 
@@ -144,14 +167,15 @@ static LIST_HEAD(mxc_gpio_ports);
 
 static int gpio_set_irq_type(struct irq_data *d, u32 type)
 {
-	u32 gpio = irq_to_gpio(d->irq);
 	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
 	struct mxc_gpio_port *port = gc->private;
 	u32 bit, val;
+	u32 gpio_idx = d->hwirq;
+	u32 gpio = port->bgc.gc.base + gpio_idx;
 	int edge;
 	void __iomem *reg = port->base;
 
-	port->both_edges &= ~(1 << (gpio & 31));
+	port->both_edges &= ~(1 << gpio_idx);
 	switch (type) {
 	case IRQ_TYPE_EDGE_RISING:
 		edge = GPIO_INT_RISE_EDGE;
@@ -160,15 +184,19 @@ static int gpio_set_irq_type(struct irq_data *d, u32 type)
 		edge = GPIO_INT_FALL_EDGE;
 		break;
 	case IRQ_TYPE_EDGE_BOTH:
-		val = gpio_get_value(gpio);
-		if (val) {
-			edge = GPIO_INT_LOW_LEV;
-			pr_debug("mxc: set GPIO %d to low trigger\n", gpio);
+		if (GPIO_EDGE_SEL >= 0) {
+			edge = GPIO_INT_BOTH_EDGES;
 		} else {
-			edge = GPIO_INT_HIGH_LEV;
-			pr_debug("mxc: set GPIO %d to high trigger\n", gpio);
+			val = gpio_get_value(gpio);
+			if (val) {
+				edge = GPIO_INT_LOW_LEV;
+				pr_debug("mxc: set GPIO %d to low trigger\n", gpio);
+			} else {
+				edge = GPIO_INT_HIGH_LEV;
+				pr_debug("mxc: set GPIO %d to high trigger\n", gpio);
+			}
+			port->both_edges |= 1 << gpio_idx;
 		}
-		port->both_edges |= 1 << (gpio & 31);
 		break;
 	case IRQ_TYPE_LEVEL_LOW:
 		edge = GPIO_INT_LOW_LEV;
@@ -180,11 +208,24 @@ static int gpio_set_irq_type(struct irq_data *d, u32 type)
 		return -EINVAL;
 	}
 
-	reg += GPIO_ICR1 + ((gpio & 0x10) >> 2); /* lower or upper register */
-	bit = gpio & 0xf;
-	val = readl(reg) & ~(0x3 << (bit << 1));
-	writel(val | (edge << (bit << 1)), reg);
-	writel(1 << (gpio & 0x1f), port->base + GPIO_ISR);
+	if (GPIO_EDGE_SEL >= 0) {
+		val = readl(port->base + GPIO_EDGE_SEL);
+		if (edge == GPIO_INT_BOTH_EDGES)
+			writel(val | (1 << gpio_idx),
+				port->base + GPIO_EDGE_SEL);
+		else
+			writel(val & ~(1 << gpio_idx),
+				port->base + GPIO_EDGE_SEL);
+	}
+
+	if (edge != GPIO_INT_BOTH_EDGES) {
+		reg += GPIO_ICR1 + ((gpio_idx & 0x10) >> 2); /* lower or upper register */
+		bit = gpio_idx & 0xf;
+		val = readl(reg) & ~(0x3 << (bit << 1));
+		writel(val | (edge << (bit << 1)), reg);
+	}
+
+	writel(1 << gpio_idx, port->base + GPIO_ISR);
 
 	return 0;
 }
@@ -217,15 +258,13 @@ static void mxc_flip_edge(struct mxc_gpio_port *port, u32 gpio)
 /* handle 32 interrupts in one status register */
 static void mxc_gpio_irq_handler(struct mxc_gpio_port *port, u32 irq_stat)
 {
-	u32 gpio_irq_no_base = port->virtual_irq_start;
-
 	while (irq_stat != 0) {
 		int irqoffset = fls(irq_stat) - 1;
 
 		if (port->both_edges & (1 << irqoffset))
 			mxc_flip_edge(port, irqoffset);
 
-		generic_handle_irq(gpio_irq_no_base + irqoffset);
+		generic_handle_irq(irq_find_mapping(port->domain, irqoffset));
 
 		irq_stat &= ~(1 << irqoffset);
 	}
@@ -276,10 +315,9 @@ static void mx2_gpio_irq_handler(u32 irq, struct irq_desc *desc)
  */
 static int gpio_set_wake_irq(struct irq_data *d, u32 enable)
 {
-	u32 gpio = irq_to_gpio(d->irq);
-	u32 gpio_idx = gpio & 0x1F;
 	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
 	struct mxc_gpio_port *port = gc->private;
+	u32 gpio_idx = d->hwirq;
 
 	if (enable) {
 		if (port->irq_high && (gpio_idx >= 16))
@@ -296,12 +334,12 @@ static int gpio_set_wake_irq(struct irq_data *d, u32 enable)
 	return 0;
 }
 
-static void __init mxc_gpio_init_gc(struct mxc_gpio_port *port)
+static void __init mxc_gpio_init_gc(struct mxc_gpio_port *port, int irq_base)
 {
 	struct irq_chip_generic *gc;
 	struct irq_chip_type *ct;
 
-	gc = irq_alloc_generic_chip("gpio-mxc", 1, port->virtual_irq_start,
+	gc = irq_alloc_generic_chip("gpio-mxc", 1, irq_base,
 				    port->base, handle_level_irq);
 	gc->private = port;
 
@@ -338,7 +376,9 @@ static void __devinit mxc_gpio_get_hw(struct platform_device *pdev)
 		return;
 	}
 
-	if (hwtype == IMX31_GPIO)
+	if (hwtype == IMX35_GPIO)
+		mxc_gpio_hwdata = &imx35_gpio_hwdata;
+	else if (hwtype == IMX31_GPIO)
 		mxc_gpio_hwdata = &imx31_gpio_hwdata;
 	else
 		mxc_gpio_hwdata = &imx1_imx21_gpio_hwdata;
@@ -352,7 +392,7 @@ static int mxc_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 	struct mxc_gpio_port *port =
 		container_of(bgc, struct mxc_gpio_port, bgc);
 
-	return port->virtual_irq_start + offset;
+	return irq_find_mapping(port->domain, offset);
 }
 
 static int __devinit mxc_gpio_probe(struct platform_device *pdev)
@@ -360,6 +400,7 @@ static int __devinit mxc_gpio_probe(struct platform_device *pdev)
 	struct device_node *np = pdev->dev.of_node;
 	struct mxc_gpio_port *port;
 	struct resource *iores;
+	int irq_base;
 	int err;
 
 	mxc_gpio_get_hw(pdev);
@@ -432,20 +473,30 @@ static int __devinit mxc_gpio_probe(struct platform_device *pdev)
 	if (err)
 		goto out_bgpio_remove;
 
-	/*
-	 * In dt case, we use gpio number range dynamically
-	 * allocated by gpio core.
-	 */
-	port->virtual_irq_start = MXC_GPIO_IRQ_START + (np ? port->bgc.gc.base :
-							     pdev->id * 32);
+	irq_base = irq_alloc_descs(-1, 0, 32, numa_node_id());
+	if (irq_base < 0) {
+		err = irq_base;
+		goto out_gpiochip_remove;
+	}
+
+	port->domain = irq_domain_add_legacy(np, 32, irq_base, 0,
+					     &irq_domain_simple_ops, NULL);
+	if (!port->domain) {
+		err = -ENODEV;
+		goto out_irqdesc_free;
+	}
 
 	/* gpio-mxc can be a generic irq chip */
-	mxc_gpio_init_gc(port);
+	mxc_gpio_init_gc(port, irq_base);
 
 	list_add_tail(&port->node, &mxc_gpio_ports);
 
 	return 0;
 
+out_irqdesc_free:
+	irq_free_descs(irq_base, 32);
+out_gpiochip_remove:
+	WARN_ON(gpiochip_remove(&port->bgc.gc) < 0);
 out_bgpio_remove:
 	bgpio_remove(&port->bgc);
 out_iounmap:
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 4fbc208c32c..e6efd77668f 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -899,12 +899,6 @@ static int gpio_debounce(struct gpio_chip *chip, unsigned offset,
 
 	bank = container_of(chip, struct gpio_bank, chip);
 
-	if (!bank->dbck) {
-		bank->dbck = clk_get(bank->dev, "dbclk");
-		if (IS_ERR(bank->dbck))
-			dev_err(bank->dev, "Could not get gpio dbck\n");
-	}
-
 	spin_lock_irqsave(&bank->lock, flags);
 	_set_gpio_debounce(bank, offset, debounce);
 	spin_unlock_irqrestore(&bank->lock, flags);
@@ -976,6 +970,10 @@ static void omap_gpio_mod_init(struct gpio_bank *bank)
 	 /* Initialize interface clk ungated, module enabled */
 	if (bank->regs->ctrl)
 		__raw_writel(0, base + bank->regs->ctrl);
+
+	bank->dbck = clk_get(bank->dev, "dbclk");
+	if (IS_ERR(bank->dbck))
+		dev_err(bank->dev, "Could not get gpio dbck\n");
 }
 
 static __devinit void
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index 1c313c710be..9c693ae1795 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -78,10 +78,10 @@ struct pca953x_chip {
 
 #ifdef CONFIG_GPIO_PCA953X_IRQ
 	struct mutex irq_lock;
-	uint16_t irq_mask;
-	uint16_t irq_stat;
-	uint16_t irq_trig_raise;
-	uint16_t irq_trig_fall;
+	u32 irq_mask;
+	u32 irq_stat;
+	u32 irq_trig_raise;
+	u32 irq_trig_fall;
 	int	 irq_base;
 #endif
 
@@ -98,12 +98,11 @@ static int pca953x_write_reg(struct pca953x_chip *chip, int reg, u32 val)
 	if (chip->gpio_chip.ngpio <= 8)
 		ret = i2c_smbus_write_byte_data(chip->client, reg, val);
 	else if (chip->gpio_chip.ngpio == 24) {
-		ret = i2c_smbus_write_word_data(chip->client,
+		cpu_to_le32s(&val);
+		ret = i2c_smbus_write_i2c_block_data(chip->client,
 						(reg << 2) | REG_ADDR_AI,
-						val & 0xffff);
-		ret = i2c_smbus_write_byte_data(chip->client,
-						(reg << 2) + 2,
-						(val & 0xff0000) >> 16);
+						3,
+						(u8 *) &val);
 	}
 	else {
 		switch (chip->chip_type) {
@@ -135,22 +134,27 @@ static int pca953x_read_reg(struct pca953x_chip *chip, int reg, u32 *val)
 {
 	int ret;
 
-	if (chip->gpio_chip.ngpio <= 8)
+	if (chip->gpio_chip.ngpio <= 8) {
 		ret = i2c_smbus_read_byte_data(chip->client, reg);
-	else if (chip->gpio_chip.ngpio == 24) {
-		ret =  i2c_smbus_read_word_data(chip->client, reg << 2);
-		ret |= (i2c_smbus_read_byte_data(chip->client,
-						 (reg << 2) + 2)<<16);
+		*val = ret;
 	}
-	else
+	else if (chip->gpio_chip.ngpio == 24) {
+		*val = 0;
+		ret = i2c_smbus_read_i2c_block_data(chip->client,
+						(reg << 2) | REG_ADDR_AI,
+						3,
+						(u8 *) val);
+		le32_to_cpus(val);
+	} else {
 		ret = i2c_smbus_read_word_data(chip->client, reg << 1);
+		*val = ret;
+	}
 
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed reading register\n");
 		return ret;
 	}
 
-	*val = (u32)ret;
 	return 0;
 }
 
@@ -349,8 +353,8 @@ static void pca953x_irq_bus_lock(struct irq_data *d)
 static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 {
 	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
-	uint16_t new_irqs;
-	uint16_t level;
+	u32 new_irqs;
+	u32 level;
 
 	/* Look for any newly setup interrupt */
 	new_irqs = chip->irq_trig_fall | chip->irq_trig_raise;
@@ -368,8 +372,8 @@ static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 static int pca953x_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
-	uint16_t level = d->irq - chip->irq_base;
-	uint16_t mask = 1 << level;
+	u32 level = d->irq - chip->irq_base;
+	u32 mask = 1 << level;
 
 	if (!(type & IRQ_TYPE_EDGE_BOTH)) {
 		dev_err(&chip->client->dev, "irq %d: unsupported type %d\n",
@@ -399,12 +403,12 @@ static struct irq_chip pca953x_irq_chip = {
 	.irq_set_type		= pca953x_irq_set_type,
 };
 
-static uint16_t pca953x_irq_pending(struct pca953x_chip *chip)
+static u32 pca953x_irq_pending(struct pca953x_chip *chip)
 {
 	u32 cur_stat;
-	uint16_t old_stat;
-	uint16_t pending;
-	uint16_t trigger;
+	u32 old_stat;
+	u32 pending;
+	u32 trigger;
 	int ret, offset = 0;
 
 	switch (chip->chip_type) {
@@ -440,8 +444,8 @@ static uint16_t pca953x_irq_pending(struct pca953x_chip *chip)
 static irqreturn_t pca953x_irq_handler(int irq, void *devid)
 {
 	struct pca953x_chip *chip = devid;
-	uint16_t pending;
-	uint16_t level;
+	u32 pending;
+	u32 level;
 
 	pending = pca953x_irq_pending(chip);
 
@@ -564,7 +568,7 @@ static void pca953x_irq_teardown(struct pca953x_chip *chip)
  * WARNING: This is DEPRECATED and will be removed eventually!
  */
 static void
-pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, int *invert)
+pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, u32 *invert)
 {
 	struct device_node *node;
 	const __be32 *val;
@@ -592,13 +596,13 @@ pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, int *invert)
 }
 #else
 static void
-pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, int *invert)
+pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, u32 *invert)
 {
 	*gpio_base = -1;
 }
 #endif
 
-static int __devinit device_pca953x_init(struct pca953x_chip *chip, int invert)
+static int __devinit device_pca953x_init(struct pca953x_chip *chip, u32 invert)
 {
 	int ret;
 
@@ -617,7 +621,7 @@ out:
 	return ret;
 }
 
-static int __devinit device_pca957x_init(struct pca953x_chip *chip, int invert)
+static int __devinit device_pca957x_init(struct pca953x_chip *chip, u32 invert)
 {
 	int ret;
 	u32 val = 0;
@@ -653,8 +657,9 @@ static int __devinit pca953x_probe(struct i2c_client *client,
 {
 	struct pca953x_platform_data *pdata;
 	struct pca953x_chip *chip;
-	int irq_base=0, invert=0;
+	int irq_base = 0;
 	int ret;
+	u32 invert = 0;
 
 	chip = kzalloc(sizeof(struct pca953x_chip), GFP_KERNEL);
 	if (chip == NULL)
diff --git a/drivers/gpio/gpio-pcf857x.c b/drivers/gpio/gpio-pcf857x.c
index 2d1de9e7e9b..076e236d0da 100644
--- a/drivers/gpio/gpio-pcf857x.c
+++ b/drivers/gpio/gpio-pcf857x.c
@@ -61,61 +61,28 @@ struct pcf857x {
 	struct i2c_client	*client;
 	struct mutex		lock;		/* protect 'out' */
 	unsigned		out;		/* software latch */
+
+	int (*write)(struct i2c_client *client, unsigned data);
+	int (*read)(struct i2c_client *client);
 };
 
 /*-------------------------------------------------------------------------*/
 
 /* Talk to 8-bit I/O expander */
 
-static int pcf857x_input8(struct gpio_chip *chip, unsigned offset)
-{
-	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
-	int		status;
-
-	mutex_lock(&gpio->lock);
-	gpio->out |= (1 << offset);
-	status = i2c_smbus_write_byte(gpio->client, gpio->out);
-	mutex_unlock(&gpio->lock);
-
-	return status;
-}
-
-static int pcf857x_get8(struct gpio_chip *chip, unsigned offset)
-{
-	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
-	s32		value;
-
-	value = i2c_smbus_read_byte(gpio->client);
-	return (value < 0) ? 0 : (value & (1 << offset));
-}
-
-static int pcf857x_output8(struct gpio_chip *chip, unsigned offset, int value)
+static int i2c_write_le8(struct i2c_client *client, unsigned data)
 {
-	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
-	unsigned	bit = 1 << offset;
-	int		status;
-
-	mutex_lock(&gpio->lock);
-	if (value)
-		gpio->out |= bit;
-	else
-		gpio->out &= ~bit;
-	status = i2c_smbus_write_byte(gpio->client, gpio->out);
-	mutex_unlock(&gpio->lock);
-
-	return status;
+	return i2c_smbus_write_byte(client, data);
 }
 
-static void pcf857x_set8(struct gpio_chip *chip, unsigned offset, int value)
+static int i2c_read_le8(struct i2c_client *client)
 {
-	pcf857x_output8(chip, offset, value);
+	return (int)i2c_smbus_read_byte(client);
 }
 
-/*-------------------------------------------------------------------------*/
-
 /* Talk to 16-bit I/O expander */
 
-static int i2c_write_le16(struct i2c_client *client, u16 word)
+static int i2c_write_le16(struct i2c_client *client, unsigned word)
 {
 	u8 buf[2] = { word & 0xff, word >> 8, };
 	int status;
@@ -135,29 +102,31 @@ static int i2c_read_le16(struct i2c_client *client)
 	return (buf[1] << 8) | buf[0];
 }
 
-static int pcf857x_input16(struct gpio_chip *chip, unsigned offset)
+/*-------------------------------------------------------------------------*/
+
+static int pcf857x_input(struct gpio_chip *chip, unsigned offset)
 {
 	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
 	int		status;
 
 	mutex_lock(&gpio->lock);
 	gpio->out |= (1 << offset);
-	status = i2c_write_le16(gpio->client, gpio->out);
+	status = gpio->write(gpio->client, gpio->out);
 	mutex_unlock(&gpio->lock);
 
 	return status;
 }
 
-static int pcf857x_get16(struct gpio_chip *chip, unsigned offset)
+static int pcf857x_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
 	int		value;
 
-	value = i2c_read_le16(gpio->client);
+	value = gpio->read(gpio->client);
 	return (value < 0) ? 0 : (value & (1 << offset));
 }
 
-static int pcf857x_output16(struct gpio_chip *chip, unsigned offset, int value)
+static int pcf857x_output(struct gpio_chip *chip, unsigned offset, int value)
 {
 	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
 	unsigned	bit = 1 << offset;
@@ -168,15 +137,15 @@ static int pcf857x_output16(struct gpio_chip *chip, unsigned offset, int value)
 		gpio->out |= bit;
 	else
 		gpio->out &= ~bit;
-	status = i2c_write_le16(gpio->client, gpio->out);
+	status = gpio->write(gpio->client, gpio->out);
 	mutex_unlock(&gpio->lock);
 
 	return status;
 }
 
-static void pcf857x_set16(struct gpio_chip *chip, unsigned offset, int value)
+static void pcf857x_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	pcf857x_output16(chip, offset, value);
+	pcf857x_output(chip, offset, value);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -200,10 +169,15 @@ static int pcf857x_probe(struct i2c_client *client,
 
 	mutex_init(&gpio->lock);
 
-	gpio->chip.base = pdata ? pdata->gpio_base : -1;
-	gpio->chip.can_sleep = 1;
-	gpio->chip.dev = &client->dev;
-	gpio->chip.owner = THIS_MODULE;
+	gpio->chip.base			= pdata ? pdata->gpio_base : -1;
+	gpio->chip.can_sleep		= 1;
+	gpio->chip.dev			= &client->dev;
+	gpio->chip.owner		= THIS_MODULE;
+	gpio->chip.get			= pcf857x_get;
+	gpio->chip.set			= pcf857x_set;
+	gpio->chip.direction_input	= pcf857x_input;
+	gpio->chip.direction_output	= pcf857x_output;
+	gpio->chip.ngpio		= id->driver_data;
 
 	/* NOTE:  the OnSemi jlc1562b is also largely compatible with
 	 * these parts, notably for output.  It has a low-resolution
@@ -216,12 +190,9 @@ static int pcf857x_probe(struct i2c_client *client,
 	 *
 	 * NOTE: we don't distinguish here between *4 and *4a parts.
 	 */
-	gpio->chip.ngpio = id->driver_data;
 	if (gpio->chip.ngpio == 8) {
-		gpio->chip.direction_input = pcf857x_input8;
-		gpio->chip.get = pcf857x_get8;
-		gpio->chip.direction_output = pcf857x_output8;
-		gpio->chip.set = pcf857x_set8;
+		gpio->write	= i2c_write_le8;
+		gpio->read	= i2c_read_le8;
 
 		if (!i2c_check_functionality(client->adapter,
 				I2C_FUNC_SMBUS_BYTE))
@@ -238,10 +209,8 @@ static int pcf857x_probe(struct i2c_client *client,
 	 * NOTE: we don't distinguish here between '75 and '75c parts.
 	 */
 	} else if (gpio->chip.ngpio == 16) {
-		gpio->chip.direction_input = pcf857x_input16;
-		gpio->chip.get = pcf857x_get16;
-		gpio->chip.direction_output = pcf857x_output16;
-		gpio->chip.set = pcf857x_set16;
+		gpio->write	= i2c_write_le16;
+		gpio->read	= i2c_read_le16;
 
 		if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
 			status = -EIO;
diff --git a/drivers/gpio/gpio-samsung.c b/drivers/gpio/gpio-samsung.c
index b6453d0e44a..92f7b2bb79d 100644
--- a/drivers/gpio/gpio-samsung.c
+++ b/drivers/gpio/gpio-samsung.c
@@ -2681,11 +2681,14 @@ static int exynos_gpio_xlate(struct gpio_chip *gc,
 
 	if (s3c_gpio_cfgpin(pin, S3C_GPIO_SFN(gpiospec->args[1])))
 		pr_warn("gpio_xlate: failed to set pin function\n");
-	if (s3c_gpio_setpull(pin, gpiospec->args[2]))
+	if (s3c_gpio_setpull(pin, gpiospec->args[2] & 0xffff))
 		pr_warn("gpio_xlate: failed to set pin pull up/down\n");
 	if (s5p_gpio_set_drvstr(pin, gpiospec->args[3]))
 		pr_warn("gpio_xlate: failed to set pin drive strength\n");
 
+	if (flags)
+		*flags = gpiospec->args[2] >> 16;
+
 	return gpiospec->args[0];
 }
 
diff --git a/drivers/gpio/gpio-tps6586x.c b/drivers/gpio/gpio-tps6586x.c
new file mode 100644
index 00000000000..2526b3bb0fa
--- /dev/null
+++ b/drivers/gpio/gpio-tps6586x.c
@@ -0,0 +1,158 @@
+/*
+ * TI TPS6586x GPIO driver
+ *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * Based on tps6586x.c
+ * Copyright (c) 2010 CompuLab Ltd.
+ * Mike Rapoport <mike@compulab.co.il>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/errno.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mfd/tps6586x.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+/* GPIO control registers */
+#define TPS6586X_GPIOSET1	0x5d
+#define TPS6586X_GPIOSET2	0x5e
+
+struct tps6586x_gpio {
+	struct gpio_chip gpio_chip;
+	struct device *parent;
+};
+
+static inline struct tps6586x_gpio *to_tps6586x_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct tps6586x_gpio, gpio_chip);
+}
+
+static int tps6586x_gpio_get(struct gpio_chip *gc, unsigned offset)
+{
+	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+	uint8_t val;
+	int ret;
+
+	ret = tps6586x_read(tps6586x_gpio->parent, TPS6586X_GPIOSET2, &val);
+	if (ret)
+		return ret;
+
+	return !!(val & (1 << offset));
+}
+
+static void tps6586x_gpio_set(struct gpio_chip *gc, unsigned offset,
+			      int value)
+{
+	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+
+	tps6586x_update(tps6586x_gpio->parent, TPS6586X_GPIOSET2,
+			value << offset, 1 << offset);
+}
+
+static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
+				int value)
+{
+	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+	uint8_t val, mask;
+
+	tps6586x_gpio_set(gc, offset, value);
+
+	val = 0x1 << (offset * 2);
+	mask = 0x3 << (offset * 2);
+
+	return tps6586x_update(tps6586x_gpio->parent, TPS6586X_GPIOSET1,
+				val, mask);
+}
+
+static int __devinit tps6586x_gpio_probe(struct platform_device *pdev)
+{
+	struct tps6586x_platform_data *pdata;
+	struct tps6586x_gpio *tps6586x_gpio;
+	int ret;
+
+	pdata = dev_get_platdata(pdev->dev.parent);
+	tps6586x_gpio = devm_kzalloc(&pdev->dev,
+				sizeof(*tps6586x_gpio), GFP_KERNEL);
+	if (!tps6586x_gpio) {
+		dev_err(&pdev->dev, "Could not allocate tps6586x_gpio\n");
+		return -ENOMEM;
+	}
+
+	tps6586x_gpio->parent = pdev->dev.parent;
+
+	tps6586x_gpio->gpio_chip.owner = THIS_MODULE;
+	tps6586x_gpio->gpio_chip.label = pdev->name;
+	tps6586x_gpio->gpio_chip.dev = &pdev->dev;
+	tps6586x_gpio->gpio_chip.ngpio = 4;
+	tps6586x_gpio->gpio_chip.can_sleep = 1;
+
+	/* FIXME: add handling of GPIOs as dedicated inputs */
+	tps6586x_gpio->gpio_chip.direction_output = tps6586x_gpio_output;
+	tps6586x_gpio->gpio_chip.set	= tps6586x_gpio_set;
+	tps6586x_gpio->gpio_chip.get	= tps6586x_gpio_get;
+
+#ifdef CONFIG_OF_GPIO
+	tps6586x_gpio->gpio_chip.of_node = pdev->dev.parent->of_node;
+#endif
+	if (pdata && pdata->gpio_base)
+		tps6586x_gpio->gpio_chip.base = pdata->gpio_base;
+	else
+		tps6586x_gpio->gpio_chip.base = -1;
+
+	ret = gpiochip_add(&tps6586x_gpio->gpio_chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, tps6586x_gpio);
+
+	return ret;
+}
+
+static int __devexit tps6586x_gpio_remove(struct platform_device *pdev)
+{
+	struct tps6586x_gpio *tps6586x_gpio = platform_get_drvdata(pdev);
+
+	return gpiochip_remove(&tps6586x_gpio->gpio_chip);
+}
+
+static struct platform_driver tps6586x_gpio_driver = {
+	.driver.name	= "tps6586x-gpio",
+	.driver.owner	= THIS_MODULE,
+	.probe		= tps6586x_gpio_probe,
+	.remove		= __devexit_p(tps6586x_gpio_remove),
+};
+
+static int __init tps6586x_gpio_init(void)
+{
+	return platform_driver_register(&tps6586x_gpio_driver);
+}
+subsys_initcall(tps6586x_gpio_init);
+
+static void __exit tps6586x_gpio_exit(void)
+{
+	platform_driver_unregister(&tps6586x_gpio_driver);
+}
+module_exit(tps6586x_gpio_exit);
+
+MODULE_ALIAS("platform:tps6586x-gpio");
+MODULE_DESCRIPTION("GPIO interface for TPS6586X PMIC");
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-wm8994.c b/drivers/gpio/gpio-wm8994.c
index aa61ad2fcaa..1c764e779d8 100644
--- a/drivers/gpio/gpio-wm8994.c
+++ b/drivers/gpio/gpio-wm8994.c
@@ -19,6 +19,7 @@
 #include <linux/mfd/core.h>
 #include <linux/platform_device.h>
 #include <linux/seq_file.h>
+#include <linux/regmap.h>
 
 #include <linux/mfd/wm8994/core.h>
 #include <linux/mfd/wm8994/pdata.h>
@@ -112,10 +113,7 @@ static int wm8994_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 
-	if (!wm8994->irq_base)
-		return -EINVAL;
-
-	return wm8994->irq_base + offset;
+	return regmap_irq_get_virq(wm8994->irq_data, offset);
 }
 
 
@@ -254,7 +252,8 @@ static int __devinit wm8994_gpio_probe(struct platform_device *pdev)
 	struct wm8994_gpio *wm8994_gpio;
 	int ret;
 
-	wm8994_gpio = kzalloc(sizeof(*wm8994_gpio), GFP_KERNEL);
+	wm8994_gpio = devm_kzalloc(&pdev->dev, sizeof(*wm8994_gpio),
+				   GFP_KERNEL);
 	if (wm8994_gpio == NULL)
 		return -ENOMEM;
 
@@ -279,20 +278,14 @@ static int __devinit wm8994_gpio_probe(struct platform_device *pdev)
 	return ret;
 
 err:
-	kfree(wm8994_gpio);
 	return ret;
 }
 
 static int __devexit wm8994_gpio_remove(struct platform_device *pdev)
 {
 	struct wm8994_gpio *wm8994_gpio = platform_get_drvdata(pdev);
-	int ret;
 
-	ret = gpiochip_remove(&wm8994_gpio->gpio_chip);
-	if (ret == 0)
-		kfree(wm8994_gpio);
-
-	return ret;
+	return gpiochip_remove(&wm8994_gpio->gpio_chip);
 }
 
 static struct platform_driver wm8994_gpio_driver = {
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index d18068a9f3e..a18c4aa68b1 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -21,7 +21,7 @@
 #include <linux/of_gpio.h>
 #include <linux/slab.h>
 
-/* Private data structure for of_gpiochip_is_match */
+/* Private data structure for of_gpiochip_find_and_xlate */
 struct gg_data {
 	enum of_gpio_flags *flags;
 	struct of_phandle_args gpiospec;
@@ -62,7 +62,10 @@ static int of_gpiochip_find_and_xlate(struct gpio_chip *gc, void *data)
 int of_get_named_gpio_flags(struct device_node *np, const char *propname,
                            int index, enum of_gpio_flags *flags)
 {
-	struct gg_data gg_data = { .flags = flags, .out_gpio = -ENODEV };
+	/* Return -EPROBE_DEFER to support probe() functions to be called
+	 * later when the GPIO actually becomes available
+	 */
+	struct gg_data gg_data = { .flags = flags, .out_gpio = -EPROBE_DEFER };
 	int ret;
 
 	/* .of_xlate might decide to not fill in the flags, so clear it. */
@@ -73,7 +76,7 @@ int of_get_named_gpio_flags(struct device_node *np, const char *propname,
 					 &gg_data.gpiospec);
 	if (ret) {
 		pr_debug("%s: can't parse gpios property\n", __func__);
-		return -EINVAL;
+		return ret;
 	}
 
 	gpiochip_find(&gg_data, of_gpiochip_find_and_xlate);
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 120b2a0e316..de0213c9d11 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -1186,7 +1186,7 @@ int gpio_request(unsigned gpio, const char *label)
 {
 	struct gpio_desc	*desc;
 	struct gpio_chip	*chip;
-	int			status = -EINVAL;
+	int			status = -EPROBE_DEFER;
 	unsigned long		flags;
 
 	spin_lock_irqsave(&gpio_lock, flags);
diff --git a/drivers/gpu/drm/ast/ast_mode.c b/drivers/gpu/drm/ast/ast_mode.c
index 65f9d231af1..7282c081fb5 100644
--- a/drivers/gpu/drm/ast/ast_mode.c
+++ b/drivers/gpu/drm/ast/ast_mode.c
@@ -460,8 +460,8 @@ static void ast_crtc_dpms(struct drm_crtc *crtc, int mode)
 }
 
 static bool ast_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode)
+				const struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
 {
 	return true;
 }
@@ -680,7 +680,7 @@ static void ast_encoder_dpms(struct drm_encoder *encoder, int mode)
 }
 
 static bool ast_mode_fixup(struct drm_encoder *encoder,
-			   struct drm_display_mode *mode,
+			   const struct drm_display_mode *mode,
 			   struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
index 100f6308c50..a44d31aa4e3 100644
--- a/drivers/gpu/drm/cirrus/cirrus_mode.c
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -97,7 +97,7 @@ static void cirrus_crtc_dpms(struct drm_crtc *crtc, int mode)
  * to just pass that straight through, so this does nothing
  */
 static bool cirrus_crtc_mode_fixup(struct drm_crtc *crtc,
-				   struct drm_display_mode *mode,
+				   const struct drm_display_mode *mode,
 				   struct drm_display_mode *adjusted_mode)
 {
 	return true;
@@ -429,8 +429,8 @@ void cirrus_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
 
 
 static bool cirrus_encoder_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode)
+				      const struct drm_display_mode *mode,
+				      struct drm_display_mode *adjusted_mode)
 {
 	return true;
 }
diff --git a/drivers/gpu/drm/drm_bufs.c b/drivers/gpu/drm/drm_bufs.c
index 348b367debe..b356c719f2f 100644
--- a/drivers/gpu/drm/drm_bufs.c
+++ b/drivers/gpu/drm/drm_bufs.c
@@ -641,8 +641,6 @@ int drm_addbufs_agp(struct drm_device * dev, struct drm_buf_desc * request)
 
 	if (order < DRM_MIN_ORDER || order > DRM_MAX_ORDER)
 		return -EINVAL;
-	if (dev->queue_count)
-		return -EBUSY;	/* Not while in use */
 
 	/* Make sure buffers are located in AGP memory that we own */
 	valid = 0;
@@ -704,7 +702,6 @@ int drm_addbufs_agp(struct drm_device * dev, struct drm_buf_desc * request)
 		buf->next = NULL;
 		buf->waiting = 0;
 		buf->pending = 0;
-		init_waitqueue_head(&buf->dma_wait);
 		buf->file_priv = NULL;
 
 		buf->dev_priv_size = dev->driver->dev_priv_size;
@@ -796,13 +793,11 @@ int drm_addbufs_pci(struct drm_device * dev, struct drm_buf_desc * request)
 	order = drm_order(request->size);
 	size = 1 << order;
 
-	DRM_DEBUG("count=%d, size=%d (%d), order=%d, queue_count=%d\n",
-		  request->count, request->size, size, order, dev->queue_count);
+	DRM_DEBUG("count=%d, size=%d (%d), order=%d\n",
+		  request->count, request->size, size, order);
 
 	if (order < DRM_MIN_ORDER || order > DRM_MAX_ORDER)
 		return -EINVAL;
-	if (dev->queue_count)
-		return -EBUSY;	/* Not while in use */
 
 	alignment = (request->flags & _DRM_PAGE_ALIGN)
 	    ? PAGE_ALIGN(size) : size;
@@ -904,7 +899,6 @@ int drm_addbufs_pci(struct drm_device * dev, struct drm_buf_desc * request)
 			buf->next = NULL;
 			buf->waiting = 0;
 			buf->pending = 0;
-			init_waitqueue_head(&buf->dma_wait);
 			buf->file_priv = NULL;
 
 			buf->dev_priv_size = dev->driver->dev_priv_size;
@@ -1019,8 +1013,6 @@ static int drm_addbufs_sg(struct drm_device * dev, struct drm_buf_desc * request
 
 	if (order < DRM_MIN_ORDER || order > DRM_MAX_ORDER)
 		return -EINVAL;
-	if (dev->queue_count)
-		return -EBUSY;	/* Not while in use */
 
 	spin_lock(&dev->count_lock);
 	if (dev->buf_use) {
@@ -1071,7 +1063,6 @@ static int drm_addbufs_sg(struct drm_device * dev, struct drm_buf_desc * request
 		buf->next = NULL;
 		buf->waiting = 0;
 		buf->pending = 0;
-		init_waitqueue_head(&buf->dma_wait);
 		buf->file_priv = NULL;
 
 		buf->dev_priv_size = dev->driver->dev_priv_size;
@@ -1177,8 +1168,6 @@ static int drm_addbufs_fb(struct drm_device * dev, struct drm_buf_desc * request
 
 	if (order < DRM_MIN_ORDER || order > DRM_MAX_ORDER)
 		return -EINVAL;
-	if (dev->queue_count)
-		return -EBUSY;	/* Not while in use */
 
 	spin_lock(&dev->count_lock);
 	if (dev->buf_use) {
@@ -1228,7 +1217,6 @@ static int drm_addbufs_fb(struct drm_device * dev, struct drm_buf_desc * request
 		buf->next = NULL;
 		buf->waiting = 0;
 		buf->pending = 0;
-		init_waitqueue_head(&buf->dma_wait);
 		buf->file_priv = NULL;
 
 		buf->dev_priv_size = dev->driver->dev_priv_size;
diff --git a/drivers/gpu/drm/drm_debugfs.c b/drivers/gpu/drm/drm_debugfs.c
index 1c7a1c0d3ed..70b13fc1939 100644
--- a/drivers/gpu/drm/drm_debugfs.c
+++ b/drivers/gpu/drm/drm_debugfs.c
@@ -46,7 +46,6 @@ static struct drm_info_list drm_debugfs_list[] = {
 	{"name", drm_name_info, 0},
 	{"vm", drm_vm_info, 0},
 	{"clients", drm_clients_info, 0},
-	{"queues", drm_queues_info, 0},
 	{"bufs", drm_bufs_info, 0},
 	{"gem_names", drm_gem_name_info, DRIVER_GEM},
 #if DRM_DEBUG_CODE
diff --git a/drivers/gpu/drm/drm_dma.c b/drivers/gpu/drm/drm_dma.c
index cfb4e333ec0..08f5e5309b2 100644
--- a/drivers/gpu/drm/drm_dma.c
+++ b/drivers/gpu/drm/drm_dma.c
@@ -120,11 +120,6 @@ void drm_free_buffer(struct drm_device *dev, struct drm_buf * buf)
 	buf->pending = 0;
 	buf->file_priv = NULL;
 	buf->used = 0;
-
-	if (drm_core_check_feature(dev, DRIVER_DMA_QUEUE)
-	    && waitqueue_active(&buf->dma_wait)) {
-		wake_up_interruptible(&buf->dma_wait);
-	}
 }
 
 /**
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 8a9d0792e4e..9238de4009f 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -182,7 +182,6 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 int drm_lastclose(struct drm_device * dev)
 {
 	struct drm_vma_entry *vma, *vma_temp;
-	int i;
 
 	DRM_DEBUG("\n");
 
@@ -228,16 +227,6 @@ int drm_lastclose(struct drm_device * dev)
 		kfree(vma);
 	}
 
-	if (drm_core_check_feature(dev, DRIVER_DMA_QUEUE) && dev->queuelist) {
-		for (i = 0; i < dev->queue_count; i++) {
-			kfree(dev->queuelist[i]);
-			dev->queuelist[i] = NULL;
-		}
-		kfree(dev->queuelist);
-		dev->queuelist = NULL;
-	}
-	dev->queue_count = 0;
-
 	if (drm_core_check_feature(dev, DRIVER_HAVE_DMA) &&
 	    !drm_core_check_feature(dev, DRIVER_MODESET))
 		drm_dma_takedown(dev);
@@ -486,7 +475,7 @@ long drm_ioctl(struct file *filp,
 		kfree(kdata);
 	atomic_dec(&dev->ioctl_count);
 	if (retcode)
-		DRM_DEBUG("ret = %x\n", retcode);
+		DRM_DEBUG("ret = %d\n", retcode);
 	return retcode;
 }
 
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 5683b7fdd74..f546d1e8af8 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -228,7 +228,7 @@ bool drm_fb_helper_restore_fbdev_mode(struct drm_fb_helper *fb_helper)
 	int i, ret;
 	for (i = 0; i < fb_helper->crtc_count; i++) {
 		struct drm_mode_set *mode_set = &fb_helper->crtc_info[i].mode_set;
-		ret = drm_crtc_helper_set_config(mode_set);
+		ret = mode_set->crtc->funcs->set_config(mode_set);
 		if (ret)
 			error = true;
 	}
@@ -1353,7 +1353,7 @@ int drm_fb_helper_hotplug_event(struct drm_fb_helper *fb_helper)
 	struct drm_device *dev = fb_helper->dev;
 	int count = 0;
 	u32 max_width, max_height, bpp_sel;
-	bool bound = false, crtcs_bound = false;
+	int bound = 0, crtcs_bound = 0;
 	struct drm_crtc *crtc;
 
 	if (!fb_helper->fb)
@@ -1362,12 +1362,12 @@ int drm_fb_helper_hotplug_event(struct drm_fb_helper *fb_helper)
 	mutex_lock(&dev->mode_config.mutex);
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		if (crtc->fb)
-			crtcs_bound = true;
+			crtcs_bound++;
 		if (crtc->fb == fb_helper->fb)
-			bound = true;
+			bound++;
 	}
 
-	if (!bound && crtcs_bound) {
+	if (bound < crtcs_bound) {
 		fb_helper->delayed_hotplug = true;
 		mutex_unlock(&dev->mode_config.mutex);
 		return 0;
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 123de28f94e..5062eec673f 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -75,10 +75,6 @@ static int drm_setup(struct drm_device * dev)
 
 	dev->sigdata.lock = NULL;
 
-	dev->queue_count = 0;
-	dev->queue_reserved = 0;
-	dev->queue_slots = 0;
-	dev->queuelist = NULL;
 	dev->context_flag = 0;
 	dev->interrupt_flag = 0;
 	dev->dma_flag = 0;
@@ -144,12 +140,12 @@ int drm_open(struct inode *inode, struct file *filp)
 	}
 	if (!retcode) {
 		mutex_lock(&dev->struct_mutex);
-		if (minor->type == DRM_MINOR_LEGACY) {
-			if (dev->dev_mapping == NULL)
-				dev->dev_mapping = inode->i_mapping;
-			else if (dev->dev_mapping != inode->i_mapping)
-				retcode = -ENODEV;
-		}
+		if (dev->dev_mapping == NULL)
+			dev->dev_mapping = &inode->i_data;
+		/* ihold ensures nobody can remove inode with our i_data */
+		ihold(container_of(dev->dev_mapping, struct inode, i_data));
+		inode->i_mapping = dev->dev_mapping;
+		filp->f_mapping = dev->dev_mapping;
 		mutex_unlock(&dev->struct_mutex);
 	}
 
@@ -370,72 +366,16 @@ int drm_fasync(int fd, struct file *filp, int on)
 }
 EXPORT_SYMBOL(drm_fasync);
 
-/*
- * Reclaim locked buffers; note that this may be a bad idea if the current
- * context doesn't have the hw lock...
- */
-static void drm_reclaim_locked_buffers(struct drm_device *dev, struct file *f)
-{
-	struct drm_file *file_priv = f->private_data;
-
-	if (drm_i_have_hw_lock(dev, file_priv)) {
-		dev->driver->reclaim_buffers_locked(dev, file_priv);
-	} else {
-		unsigned long _end = jiffies + 3 * DRM_HZ;
-		int locked = 0;
-
-		drm_idlelock_take(&file_priv->master->lock);
-
-		/*
-		 * Wait for a while.
-		 */
-		do {
-			spin_lock_bh(&file_priv->master->lock.spinlock);
-			locked = file_priv->master->lock.idle_has_lock;
-			spin_unlock_bh(&file_priv->master->lock.spinlock);
-			if (locked)
-				break;
-			schedule();
-		} while (!time_after_eq(jiffies, _end));
-
-		if (!locked) {
-			DRM_ERROR("reclaim_buffers_locked() deadlock. Please rework this\n"
-				  "\tdriver to use reclaim_buffers_idlelocked() instead.\n"
-				  "\tI will go on reclaiming the buffers anyway.\n");
-		}
-
-		dev->driver->reclaim_buffers_locked(dev, file_priv);
-		drm_idlelock_release(&file_priv->master->lock);
-	}
-}
-
 static void drm_master_release(struct drm_device *dev, struct file *filp)
 {
 	struct drm_file *file_priv = filp->private_data;
 
-	if (dev->driver->reclaim_buffers_locked &&
-	    file_priv->master->lock.hw_lock)
-		drm_reclaim_locked_buffers(dev, filp);
-
-	if (dev->driver->reclaim_buffers_idlelocked &&
-	    file_priv->master->lock.hw_lock) {
-		drm_idlelock_take(&file_priv->master->lock);
-		dev->driver->reclaim_buffers_idlelocked(dev, file_priv);
-		drm_idlelock_release(&file_priv->master->lock);
-	}
-
-
 	if (drm_i_have_hw_lock(dev, file_priv)) {
 		DRM_DEBUG("File %p released, freeing lock for context %d\n",
 			  filp, _DRM_LOCKING_CONTEXT(file_priv->master->lock.hw_lock->lock));
 		drm_lock_free(&file_priv->master->lock,
 			      _DRM_LOCKING_CONTEXT(file_priv->master->lock.hw_lock->lock));
 	}
-
-	if (drm_core_check_feature(dev, DRIVER_HAVE_DMA) &&
-	    !dev->driver->reclaim_buffers_locked) {
-		dev->driver->reclaim_buffers(dev, file_priv);
-	}
 }
 
 static void drm_events_release(struct drm_file *file_priv)
@@ -505,6 +445,9 @@ int drm_release(struct inode *inode, struct file *filp)
 	if (file_priv->minor->master)
 		drm_master_release(dev, filp);
 
+	if (drm_core_check_feature(dev, DRIVER_HAVE_DMA))
+		drm_core_reclaim_buffers(dev, file_priv);
+
 	drm_events_release(file_priv);
 
 	if (dev->driver->driver_features & DRIVER_MODESET)
@@ -566,6 +509,9 @@ int drm_release(struct inode *inode, struct file *filp)
 		}
 	}
 
+	BUG_ON(dev->dev_mapping == NULL);
+	iput(container_of(dev->dev_mapping, struct inode, i_data));
+
 	/* drop the reference held my the file priv */
 	drm_master_put(&file_priv->master);
 	file_priv->is_master = 0;
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index d58e69da1fb..fbe0842038b 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -354,7 +354,7 @@ drm_gem_create_mmap_offset(struct drm_gem_object *obj)
 
 	/* Get a DRM GEM mmap offset allocated... */
 	list->file_offset_node = drm_mm_search_free(&mm->offset_manager,
-			obj->size / PAGE_SIZE, 0, 0);
+			obj->size / PAGE_SIZE, 0, false);
 
 	if (!list->file_offset_node) {
 		DRM_ERROR("failed to allocate offset for bo %d\n", obj->name);
diff --git a/drivers/gpu/drm/drm_info.c b/drivers/gpu/drm/drm_info.c
index ab1162da70f..8928edbb94c 100644
--- a/drivers/gpu/drm/drm_info.c
+++ b/drivers/gpu/drm/drm_info.c
@@ -110,42 +110,6 @@ int drm_vm_info(struct seq_file *m, void *data)
 }
 
 /**
- * Called when "/proc/dri/.../queues" is read.
- */
-int drm_queues_info(struct seq_file *m, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
-	int i;
-	struct drm_queue *q;
-
-	mutex_lock(&dev->struct_mutex);
-	seq_printf(m, "  ctx/flags   use   fin"
-		   "   blk/rw/rwf  wait    flushed	   queued"
-		   "      locks\n\n");
-	for (i = 0; i < dev->queue_count; i++) {
-		q = dev->queuelist[i];
-		atomic_inc(&q->use_count);
-		seq_printf(m,   "%5d/0x%03x %5d %5d"
-			   " %5d/%c%c/%c%c%c %5Zd\n",
-			   i,
-			   q->flags,
-			   atomic_read(&q->use_count),
-			   atomic_read(&q->finalization),
-			   atomic_read(&q->block_count),
-			   atomic_read(&q->block_read) ? 'r' : '-',
-			   atomic_read(&q->block_write) ? 'w' : '-',
-			   waitqueue_active(&q->read_queue) ? 'r' : '-',
-			   waitqueue_active(&q->write_queue) ? 'w' : '-',
-			   waitqueue_active(&q->flush_queue) ? 'f' : '-',
-			   DRM_BUFCOUNT(&q->waitlist));
-		atomic_dec(&q->use_count);
-	}
-	mutex_unlock(&dev->struct_mutex);
-	return 0;
-}
-
-/**
  * Called when "/proc/dri/.../bufs" is read.
  */
 int drm_bufs_info(struct seq_file *m, void *data)
@@ -235,7 +199,7 @@ int drm_clients_info(struct seq_file *m, void *data)
 }
 
 
-int drm_gem_one_name_info(int id, void *ptr, void *data)
+static int drm_gem_one_name_info(int id, void *ptr, void *data)
 {
 	struct drm_gem_object *obj = ptr;
 	struct seq_file *m = data;
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index c798eeae0a0..03f16f352fe 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -974,7 +974,6 @@ EXPORT_SYMBOL(drm_vblank_off);
  * drm_vblank_pre_modeset - account for vblanks across mode sets
  * @dev: DRM device
  * @crtc: CRTC in question
- * @post: post or pre mode set?
  *
  * Account for vblank events across mode setting events, which will likely
  * reset the hardware frame counter.
@@ -1037,6 +1036,10 @@ int drm_modeset_ctl(struct drm_device *dev, void *data,
 	if (!dev->num_crtcs)
 		return 0;
 
+	/* KMS drivers handle this internally */
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		return 0;
+
 	crtc = modeset->crtc;
 	if (crtc >= dev->num_crtcs)
 		return -EINVAL;
diff --git a/drivers/gpu/drm/drm_lock.c b/drivers/gpu/drm/drm_lock.c
index 52115204169..32039553e17 100644
--- a/drivers/gpu/drm/drm_lock.c
+++ b/drivers/gpu/drm/drm_lock.c
@@ -70,10 +70,6 @@ int drm_lock(struct drm_device *dev, void *data, struct drm_file *file_priv)
 		  lock->context, task_pid_nr(current),
 		  master->lock.hw_lock->lock, lock->flags);
 
-	if (drm_core_check_feature(dev, DRIVER_DMA_QUEUE))
-		if (lock->context < 0)
-			return -EINVAL;
-
 	add_wait_queue(&master->lock.lock_queue, &entry);
 	spin_lock_bh(&master->lock.spinlock);
 	master->lock.user_waiters++;
diff --git a/drivers/gpu/drm/drm_mm.c b/drivers/gpu/drm/drm_mm.c
index 961fb54f426..9bb82f7f006 100644
--- a/drivers/gpu/drm/drm_mm.c
+++ b/drivers/gpu/drm/drm_mm.c
@@ -118,45 +118,53 @@ static inline unsigned long drm_mm_hole_node_end(struct drm_mm_node *hole_node)
 
 static void drm_mm_insert_helper(struct drm_mm_node *hole_node,
 				 struct drm_mm_node *node,
-				 unsigned long size, unsigned alignment)
+				 unsigned long size, unsigned alignment,
+				 unsigned long color)
 {
 	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);
+	unsigned long adj_start = hole_start;
+	unsigned long adj_end = hole_end;
 
 	BUG_ON(!hole_node->hole_follows || node->allocated);
 
-	if (alignment)
-		tmp = hole_start % alignment;
+	if (mm->color_adjust)
+		mm->color_adjust(hole_node, color, &adj_start, &adj_end);
 
-	if (!tmp) {
+	if (alignment) {
+		unsigned tmp = adj_start % alignment;
+		if (tmp)
+			adj_start += alignment - tmp;
+	}
+
+	if (adj_start == hole_start) {
 		hole_node->hole_follows = 0;
-		list_del_init(&hole_node->hole_stack);
-	} else
-		wasted = alignment - tmp;
+		list_del(&hole_node->hole_stack);
+	}
 
-	node->start = hole_start + wasted;
+	node->start = adj_start;
 	node->size = size;
 	node->mm = mm;
+	node->color = color;
 	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 > adj_end);
 
+	node->hole_follows = 0;
 	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 *hole_node,
 					     unsigned long size,
 					     unsigned alignment,
+					     unsigned long color,
 					     int atomic)
 {
 	struct drm_mm_node *node;
@@ -165,7 +173,7 @@ struct drm_mm_node *drm_mm_get_block_generic(struct drm_mm_node *hole_node,
 	if (unlikely(node == NULL))
 		return NULL;
 
-	drm_mm_insert_helper(hole_node, node, size, alignment);
+	drm_mm_insert_helper(hole_node, node, size, alignment, color);
 
 	return node;
 }
@@ -181,11 +189,11 @@ int drm_mm_insert_node(struct drm_mm *mm, struct drm_mm_node *node,
 {
 	struct drm_mm_node *hole_node;
 
-	hole_node = drm_mm_search_free(mm, size, alignment, 0);
+	hole_node = drm_mm_search_free(mm, size, alignment, false);
 	if (!hole_node)
 		return -ENOSPC;
 
-	drm_mm_insert_helper(hole_node, node, size, alignment);
+	drm_mm_insert_helper(hole_node, node, size, alignment, 0);
 
 	return 0;
 }
@@ -194,50 +202,57 @@ EXPORT_SYMBOL(drm_mm_insert_node);
 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 color,
 				       unsigned long start, unsigned long end)
 {
 	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);
+	unsigned long adj_start = hole_start;
+	unsigned long adj_end = hole_end;
 
 	BUG_ON(!hole_node->hole_follows || node->allocated);
 
-	if (hole_start < start)
-		wasted += start - hole_start;
-	if (alignment)
-		tmp = (hole_start + wasted) % alignment;
+	if (mm->color_adjust)
+		mm->color_adjust(hole_node, color, &adj_start, &adj_end);
 
-	if (tmp)
-		wasted += alignment - tmp;
+	if (adj_start < start)
+		adj_start = start;
+
+	if (alignment) {
+		unsigned tmp = adj_start % alignment;
+		if (tmp)
+			adj_start += alignment - tmp;
+	}
 
-	if (!wasted) {
+	if (adj_start == hole_start) {
 		hole_node->hole_follows = 0;
-		list_del_init(&hole_node->hole_stack);
+		list_del(&hole_node->hole_stack);
 	}
 
-	node->start = hole_start + wasted;
+	node->start = adj_start;
 	node->size = size;
 	node->mm = mm;
+	node->color = color;
 	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 > adj_end);
 	BUG_ON(node->start + node->size > end);
 
+	node->hole_follows = 0;
 	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_range_generic(struct drm_mm_node *hole_node,
 						unsigned long size,
 						unsigned alignment,
+						unsigned long color,
 						unsigned long start,
 						unsigned long end,
 						int atomic)
@@ -248,7 +263,7 @@ struct drm_mm_node *drm_mm_get_block_range_generic(struct drm_mm_node *hole_node
 	if (unlikely(node == NULL))
 		return NULL;
 
-	drm_mm_insert_helper_range(hole_node, node, size, alignment,
+	drm_mm_insert_helper_range(hole_node, node, size, alignment, color,
 				   start, end);
 
 	return node;
@@ -267,11 +282,11 @@ int drm_mm_insert_node_in_range(struct drm_mm *mm, struct drm_mm_node *node,
 	struct drm_mm_node *hole_node;
 
 	hole_node = drm_mm_search_free_in_range(mm, size, alignment,
-						start, end, 0);
+						start, end, false);
 	if (!hole_node)
 		return -ENOSPC;
 
-	drm_mm_insert_helper_range(hole_node, node, size, alignment,
+	drm_mm_insert_helper_range(hole_node, node, size, alignment, 0,
 				   start, end);
 
 	return 0;
@@ -336,27 +351,23 @@ EXPORT_SYMBOL(drm_mm_put_block);
 static int check_free_hole(unsigned long start, unsigned long end,
 			   unsigned long size, unsigned alignment)
 {
-	unsigned wasted = 0;
-
 	if (end - start < size)
 		return 0;
 
 	if (alignment) {
 		unsigned tmp = start % alignment;
 		if (tmp)
-			wasted = alignment - tmp;
-	}
-
-	if (end >= start + size + wasted) {
-		return 1;
+			start += alignment - tmp;
 	}
 
-	return 0;
+	return end >= start + size;
 }
 
-struct drm_mm_node *drm_mm_search_free(const struct drm_mm *mm,
-				       unsigned long size,
-				       unsigned alignment, int best_match)
+struct drm_mm_node *drm_mm_search_free_generic(const struct drm_mm *mm,
+					       unsigned long size,
+					       unsigned alignment,
+					       unsigned long color,
+					       bool best_match)
 {
 	struct drm_mm_node *entry;
 	struct drm_mm_node *best;
@@ -368,10 +379,17 @@ struct drm_mm_node *drm_mm_search_free(const struct drm_mm *mm,
 	best_size = ~0UL;
 
 	list_for_each_entry(entry, &mm->hole_stack, hole_stack) {
+		unsigned long adj_start = drm_mm_hole_node_start(entry);
+		unsigned long adj_end = drm_mm_hole_node_end(entry);
+
+		if (mm->color_adjust) {
+			mm->color_adjust(entry, color, &adj_start, &adj_end);
+			if (adj_end <= adj_start)
+				continue;
+		}
+
 		BUG_ON(!entry->hole_follows);
-		if (!check_free_hole(drm_mm_hole_node_start(entry),
-				     drm_mm_hole_node_end(entry),
-				     size, alignment))
+		if (!check_free_hole(adj_start, adj_end, size, alignment))
 			continue;
 
 		if (!best_match)
@@ -385,14 +403,15 @@ struct drm_mm_node *drm_mm_search_free(const struct drm_mm *mm,
 
 	return best;
 }
-EXPORT_SYMBOL(drm_mm_search_free);
-
-struct drm_mm_node *drm_mm_search_free_in_range(const struct drm_mm *mm,
-						unsigned long size,
-						unsigned alignment,
-						unsigned long start,
-						unsigned long end,
-						int best_match)
+EXPORT_SYMBOL(drm_mm_search_free_generic);
+
+struct drm_mm_node *drm_mm_search_free_in_range_generic(const struct drm_mm *mm,
+							unsigned long size,
+							unsigned alignment,
+							unsigned long color,
+							unsigned long start,
+							unsigned long end,
+							bool best_match)
 {
 	struct drm_mm_node *entry;
 	struct drm_mm_node *best;
@@ -410,6 +429,13 @@ struct drm_mm_node *drm_mm_search_free_in_range(const struct drm_mm *mm,
 			end : drm_mm_hole_node_end(entry);
 
 		BUG_ON(!entry->hole_follows);
+
+		if (mm->color_adjust) {
+			mm->color_adjust(entry, color, &adj_start, &adj_end);
+			if (adj_end <= adj_start)
+				continue;
+		}
+
 		if (!check_free_hole(adj_start, adj_end, size, alignment))
 			continue;
 
@@ -424,7 +450,7 @@ struct drm_mm_node *drm_mm_search_free_in_range(const struct drm_mm *mm,
 
 	return best;
 }
-EXPORT_SYMBOL(drm_mm_search_free_in_range);
+EXPORT_SYMBOL(drm_mm_search_free_in_range_generic);
 
 /**
  * Moves an allocation. To be used with embedded struct drm_mm_node.
@@ -437,6 +463,7 @@ void drm_mm_replace_node(struct drm_mm_node *old, struct drm_mm_node *new)
 	new->mm = old->mm;
 	new->start = old->start;
 	new->size = old->size;
+	new->color = old->color;
 
 	old->allocated = 0;
 	new->allocated = 1;
@@ -452,9 +479,12 @@ EXPORT_SYMBOL(drm_mm_replace_node);
  * Warning: As long as the scan list is non-empty, no other operations than
  * adding/removing nodes to/from the scan list are allowed.
  */
-void drm_mm_init_scan(struct drm_mm *mm, unsigned long size,
-		      unsigned alignment)
+void drm_mm_init_scan(struct drm_mm *mm,
+		      unsigned long size,
+		      unsigned alignment,
+		      unsigned long color)
 {
+	mm->scan_color = color;
 	mm->scan_alignment = alignment;
 	mm->scan_size = size;
 	mm->scanned_blocks = 0;
@@ -474,11 +504,14 @@ EXPORT_SYMBOL(drm_mm_init_scan);
  * Warning: As long as the scan list is non-empty, no other operations than
  * adding/removing nodes to/from the scan list are allowed.
  */
-void drm_mm_init_scan_with_range(struct drm_mm *mm, unsigned long size,
+void drm_mm_init_scan_with_range(struct drm_mm *mm,
+				 unsigned long size,
 				 unsigned alignment,
+				 unsigned long color,
 				 unsigned long start,
 				 unsigned long end)
 {
+	mm->scan_color = color;
 	mm->scan_alignment = alignment;
 	mm->scan_size = size;
 	mm->scanned_blocks = 0;
@@ -522,17 +555,21 @@ int drm_mm_scan_add_block(struct drm_mm_node *node)
 
 	hole_start = drm_mm_hole_node_start(prev_node);
 	hole_end = drm_mm_hole_node_end(prev_node);
+
+	adj_start = hole_start;
+	adj_end = hole_end;
+
+	if (mm->color_adjust)
+		mm->color_adjust(prev_node, mm->scan_color, &adj_start, &adj_end);
+
 	if (mm->scan_check_range) {
-		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 = hole_start;
-		adj_end = hole_end;
+		if (adj_start < mm->scan_start)
+			adj_start = mm->scan_start;
+		if (adj_end > mm->scan_end)
+			adj_end = mm->scan_end;
 	}
 
-	if (check_free_hole(adj_start , adj_end,
+	if (check_free_hole(adj_start, adj_end,
 			    mm->scan_size, mm->scan_alignment)) {
 		mm->scan_hit_start = hole_start;
 		mm->scan_hit_size = hole_end;
@@ -616,6 +653,8 @@ int drm_mm_init(struct drm_mm * mm, unsigned long start, unsigned long size)
 	mm->head_node.size = start - mm->head_node.start;
 	list_add_tail(&mm->head_node.hole_stack, &mm->hole_stack);
 
+	mm->color_adjust = NULL;
+
 	return 0;
 }
 EXPORT_SYMBOL(drm_mm_init);
diff --git a/drivers/gpu/drm/drm_pci.c b/drivers/gpu/drm/drm_pci.c
index 13f3d936472..5320364582c 100644
--- a/drivers/gpu/drm/drm_pci.c
+++ b/drivers/gpu/drm/drm_pci.c
@@ -465,3 +465,52 @@ void drm_pci_exit(struct drm_driver *driver, struct pci_driver *pdriver)
 	DRM_INFO("Module unloaded\n");
 }
 EXPORT_SYMBOL(drm_pci_exit);
+
+int drm_pcie_get_speed_cap_mask(struct drm_device *dev, u32 *mask)
+{
+	struct pci_dev *root;
+	int pos;
+	u32 lnkcap, lnkcap2;
+
+	*mask = 0;
+	if (!dev->pdev)
+		return -EINVAL;
+
+	if (!pci_is_pcie(dev->pdev))
+		return -EINVAL;
+
+	root = dev->pdev->bus->self;
+
+	pos = pci_pcie_cap(root);
+	if (!pos)
+		return -EINVAL;
+
+	/* we've been informed via and serverworks don't make the cut */
+	if (root->vendor == PCI_VENDOR_ID_VIA ||
+	    root->vendor == PCI_VENDOR_ID_SERVERWORKS)
+		return -EINVAL;
+
+	pci_read_config_dword(root, pos + PCI_EXP_LNKCAP, &lnkcap);
+	pci_read_config_dword(root, pos + PCI_EXP_LNKCAP2, &lnkcap2);
+
+	lnkcap &= PCI_EXP_LNKCAP_SLS;
+	lnkcap2 &= 0xfe;
+
+	if (lnkcap2) { /* PCIE GEN 3.0 */
+		if (lnkcap2 & PCI_EXP_LNKCAP2_SLS_2_5GB)
+			*mask |= DRM_PCIE_SPEED_25;
+		if (lnkcap2 & PCI_EXP_LNKCAP2_SLS_5_0GB)
+			*mask |= DRM_PCIE_SPEED_50;
+		if (lnkcap2 & PCI_EXP_LNKCAP2_SLS_8_0GB)
+			*mask |= DRM_PCIE_SPEED_80;
+	} else {
+		if (lnkcap & 1)
+			*mask |= DRM_PCIE_SPEED_25;
+		if (lnkcap & 2)
+			*mask |= DRM_PCIE_SPEED_50;
+	}
+
+	DRM_INFO("probing gen 2 caps for device %x:%x = %x/%x\n", root->vendor, root->device, lnkcap, lnkcap2);
+	return 0;
+}
+EXPORT_SYMBOL(drm_pcie_get_speed_cap_mask);
diff --git a/drivers/gpu/drm/drm_proc.c b/drivers/gpu/drm/drm_proc.c
index fff87221f9e..371c695322d 100644
--- a/drivers/gpu/drm/drm_proc.c
+++ b/drivers/gpu/drm/drm_proc.c
@@ -53,7 +53,6 @@ static struct drm_info_list drm_proc_list[] = {
 	{"name", drm_name_info, 0},
 	{"vm", drm_vm_info, 0},
 	{"clients", drm_clients_info, 0},
-	{"queues", drm_queues_info, 0},
 	{"bufs", drm_bufs_info, 0},
 	{"gem_names", drm_gem_name_info, DRIVER_GEM},
 #if DRM_DEBUG_CODE
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 45cf1dd3eb9..45ac8d6c92b 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -134,6 +134,7 @@ void drm_sysfs_destroy(void)
 		return;
 	class_remove_file(drm_class, &class_attr_version.attr);
 	class_destroy(drm_class);
+	drm_class = NULL;
 }
 
 /**
@@ -554,6 +555,9 @@ void drm_sysfs_device_remove(struct drm_minor *minor)
 
 int drm_class_device_register(struct device *dev)
 {
+	if (!drm_class || IS_ERR(drm_class))
+		return -ENOENT;
+
 	dev->class = drm_class;
 	return device_register(dev);
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.c b/drivers/gpu/drm/exynos/exynos_drm_connector.c
index bf791fa0e50..d9568198c30 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_connector.c
@@ -196,7 +196,8 @@ static int exynos_drm_connector_mode_valid(struct drm_connector *connector,
 	return ret;
 }
 
-struct drm_encoder *exynos_drm_best_encoder(struct drm_connector *connector)
+static struct drm_encoder *exynos_drm_best_encoder(
+						struct drm_connector *connector)
 {
 	struct drm_device *dev = connector->dev;
 	struct exynos_drm_connector *exynos_connector =
diff --git a/drivers/gpu/drm/exynos/exynos_drm_core.c b/drivers/gpu/drm/exynos/exynos_drm_core.c
index eaf630dc5db..84dd099eae3 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_core.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_core.c
@@ -33,7 +33,6 @@
 #include "exynos_drm_fbdev.h"
 
 static LIST_HEAD(exynos_drm_subdrv_list);
-static struct drm_device *drm_dev;
 
 static int exynos_drm_subdrv_probe(struct drm_device *dev,
 					struct exynos_drm_subdrv *subdrv)
@@ -120,8 +119,6 @@ int exynos_drm_device_register(struct drm_device *dev)
 	if (!dev)
 		return -EINVAL;
 
-	drm_dev = dev;
-
 	list_for_each_entry_safe(subdrv, n, &exynos_drm_subdrv_list, list) {
 		subdrv->drm_dev = dev;
 		err = exynos_drm_subdrv_probe(dev, subdrv);
@@ -149,8 +146,6 @@ int exynos_drm_device_unregister(struct drm_device *dev)
 	list_for_each_entry(subdrv, &exynos_drm_subdrv_list, list)
 		exynos_drm_subdrv_remove(dev, subdrv);
 
-	drm_dev = NULL;
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(exynos_drm_device_unregister);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index 4afb625128d..abb1e2f8227 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -29,21 +29,23 @@
 #include "drmP.h"
 #include "drm_crtc_helper.h"
 
-#include "exynos_drm_crtc.h"
 #include "exynos_drm_drv.h"
-#include "exynos_drm_fb.h"
 #include "exynos_drm_encoder.h"
-#include "exynos_drm_gem.h"
+#include "exynos_drm_plane.h"
 
 #define to_exynos_crtc(x)	container_of(x, struct exynos_drm_crtc,\
 				drm_crtc)
 
+enum exynos_crtc_mode {
+	CRTC_MODE_NORMAL,	/* normal mode */
+	CRTC_MODE_BLANK,	/* The private plane of crtc is blank */
+};
+
 /*
  * Exynos specific crtc structure.
  *
  * @drm_crtc: crtc object.
- * @overlay: contain information common to display controller and hdmi and
- *	contents of this overlay object would be copied to sub driver size.
+ * @drm_plane: pointer of private plane object for this crtc
  * @pipe: a crtc index created at load() with a new crtc object creation
  *	and the crtc object would be set to private->crtc array
  *	to get a crtc object corresponding to this pipe from private->crtc
@@ -52,115 +54,16 @@
  *	we can refer to the crtc to current hardware interrupt occured through
  *	this pipe value.
  * @dpms: store the crtc dpms value
+ * @mode: store the crtc mode value
  */
 struct exynos_drm_crtc {
 	struct drm_crtc			drm_crtc;
-	struct exynos_drm_overlay	overlay;
+	struct drm_plane		*plane;
 	unsigned int			pipe;
 	unsigned int			dpms;
+	enum exynos_crtc_mode		mode;
 };
 
-static void exynos_drm_crtc_apply(struct drm_crtc *crtc)
-{
-	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
-	struct exynos_drm_overlay *overlay = &exynos_crtc->overlay;
-
-	exynos_drm_fn_encoder(crtc, overlay,
-			exynos_drm_encoder_crtc_mode_set);
-	exynos_drm_fn_encoder(crtc, &exynos_crtc->pipe,
-			exynos_drm_encoder_crtc_commit);
-}
-
-int exynos_drm_overlay_update(struct exynos_drm_overlay *overlay,
-			      struct drm_framebuffer *fb,
-			      struct drm_display_mode *mode,
-			      struct exynos_drm_crtc_pos *pos)
-{
-	struct exynos_drm_gem_buf *buffer;
-	unsigned int actual_w;
-	unsigned int actual_h;
-	int nr = exynos_drm_format_num_buffers(fb->pixel_format);
-	int i;
-
-	for (i = 0; i < nr; i++) {
-		buffer = exynos_drm_fb_buffer(fb, i);
-		if (!buffer) {
-			DRM_LOG_KMS("buffer is null\n");
-			return -EFAULT;
-		}
-
-		overlay->dma_addr[i] = buffer->dma_addr;
-		overlay->vaddr[i] = buffer->kvaddr;
-
-		DRM_DEBUG_KMS("buffer: %d, vaddr = 0x%lx, dma_addr = 0x%lx\n",
-				i, (unsigned long)overlay->vaddr[i],
-				(unsigned long)overlay->dma_addr[i]);
-	}
-
-	actual_w = min((mode->hdisplay - pos->crtc_x), pos->crtc_w);
-	actual_h = min((mode->vdisplay - pos->crtc_y), pos->crtc_h);
-
-	/* set drm framebuffer data. */
-	overlay->fb_x = pos->fb_x;
-	overlay->fb_y = pos->fb_y;
-	overlay->fb_width = fb->width;
-	overlay->fb_height = fb->height;
-	overlay->src_width = pos->src_w;
-	overlay->src_height = pos->src_h;
-	overlay->bpp = fb->bits_per_pixel;
-	overlay->pitch = fb->pitches[0];
-	overlay->pixel_format = fb->pixel_format;
-
-	/* set overlay range to be displayed. */
-	overlay->crtc_x = pos->crtc_x;
-	overlay->crtc_y = pos->crtc_y;
-	overlay->crtc_width = actual_w;
-	overlay->crtc_height = actual_h;
-
-	/* set drm mode data. */
-	overlay->mode_width = mode->hdisplay;
-	overlay->mode_height = mode->vdisplay;
-	overlay->refresh = mode->vrefresh;
-	overlay->scan_flag = mode->flags;
-
-	DRM_DEBUG_KMS("overlay : offset_x/y(%d,%d), width/height(%d,%d)",
-			overlay->crtc_x, overlay->crtc_y,
-			overlay->crtc_width, overlay->crtc_height);
-
-	return 0;
-}
-
-static int exynos_drm_crtc_update(struct drm_crtc *crtc)
-{
-	struct exynos_drm_crtc *exynos_crtc;
-	struct exynos_drm_overlay *overlay;
-	struct exynos_drm_crtc_pos pos;
-	struct drm_display_mode *mode = &crtc->mode;
-	struct drm_framebuffer *fb = crtc->fb;
-
-	if (!mode || !fb)
-		return -EINVAL;
-
-	exynos_crtc = to_exynos_crtc(crtc);
-	overlay = &exynos_crtc->overlay;
-
-	memset(&pos, 0, sizeof(struct exynos_drm_crtc_pos));
-
-	/* it means the offset of framebuffer to be displayed. */
-	pos.fb_x = crtc->x;
-	pos.fb_y = crtc->y;
-
-	/* OSD position to be displayed. */
-	pos.crtc_x = 0;
-	pos.crtc_y = 0;
-	pos.crtc_w = fb->width - crtc->x;
-	pos.crtc_h = fb->height - crtc->y;
-	pos.src_w = pos.crtc_w;
-	pos.src_h = pos.crtc_h;
-
-	return exynos_drm_overlay_update(overlay, crtc->fb, mode, &pos);
-}
-
 static void exynos_drm_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 	struct drm_device *dev = crtc->dev;
@@ -175,23 +78,8 @@ static void exynos_drm_crtc_dpms(struct drm_crtc *crtc, int mode)
 
 	mutex_lock(&dev->struct_mutex);
 
-	switch (mode) {
-	case DRM_MODE_DPMS_ON:
-		exynos_drm_fn_encoder(crtc, &mode,
-				exynos_drm_encoder_crtc_dpms);
-		exynos_crtc->dpms = mode;
-		break;
-	case DRM_MODE_DPMS_STANDBY:
-	case DRM_MODE_DPMS_SUSPEND:
-	case DRM_MODE_DPMS_OFF:
-		exynos_drm_fn_encoder(crtc, &mode,
-				exynos_drm_encoder_crtc_dpms);
-		exynos_crtc->dpms = mode;
-		break;
-	default:
-		DRM_ERROR("unspecified mode %d\n", mode);
-		break;
-	}
+	exynos_drm_fn_encoder(crtc, &mode, exynos_drm_encoder_crtc_dpms);
+	exynos_crtc->dpms = mode;
 
 	mutex_unlock(&dev->struct_mutex);
 }
@@ -209,35 +97,13 @@ static void exynos_drm_crtc_commit(struct drm_crtc *crtc)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	/*
-	 * when set_crtc is requested from user or at booting time,
-	 * crtc->commit would be called without dpms call so if dpms is
-	 * no power on then crtc->dpms should be called
-	 * with DRM_MODE_DPMS_ON for the hardware power to be on.
-	 */
-	if (exynos_crtc->dpms != DRM_MODE_DPMS_ON) {
-		int mode = DRM_MODE_DPMS_ON;
-
-		/*
-		 * enable hardware(power on) to all encoders hdmi connected
-		 * to current crtc.
-		 */
-		exynos_drm_crtc_dpms(crtc, mode);
-		/*
-		 * enable dma to all encoders connected to current crtc and
-		 * lcd panel.
-		 */
-		exynos_drm_fn_encoder(crtc, &mode,
-					exynos_drm_encoder_dpms_from_crtc);
-	}
-
-	exynos_drm_fn_encoder(crtc, &exynos_crtc->pipe,
-			exynos_drm_encoder_crtc_commit);
+	exynos_plane_commit(exynos_crtc->plane);
+	exynos_plane_dpms(exynos_crtc->plane, DRM_MODE_DPMS_ON);
 }
 
 static bool
 exynos_drm_crtc_mode_fixup(struct drm_crtc *crtc,
-			    struct drm_display_mode *mode,
+			    const struct drm_display_mode *mode,
 			    struct drm_display_mode *adjusted_mode)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -251,31 +117,61 @@ exynos_drm_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 			  struct drm_display_mode *adjusted_mode, int x, int y,
 			  struct drm_framebuffer *old_fb)
 {
+	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
+	struct drm_plane *plane = exynos_crtc->plane;
+	unsigned int crtc_w;
+	unsigned int crtc_h;
+	int pipe = exynos_crtc->pipe;
+	int ret;
+
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	exynos_drm_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
+
 	/*
 	 * copy the mode data adjusted by mode_fixup() into crtc->mode
 	 * so that hardware can be seet to proper mode.
 	 */
 	memcpy(&crtc->mode, adjusted_mode, sizeof(*adjusted_mode));
 
-	return exynos_drm_crtc_update(crtc);
+	crtc_w = crtc->fb->width - x;
+	crtc_h = crtc->fb->height - y;
+
+	ret = exynos_plane_mode_set(plane, crtc, crtc->fb, 0, 0, crtc_w, crtc_h,
+				    x, y, crtc_w, crtc_h);
+	if (ret)
+		return ret;
+
+	plane->crtc = crtc;
+	plane->fb = crtc->fb;
+
+	exynos_drm_fn_encoder(crtc, &pipe, exynos_drm_encoder_crtc_pipe);
+
+	return 0;
 }
 
 static int exynos_drm_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 					  struct drm_framebuffer *old_fb)
 {
+	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
+	struct drm_plane *plane = exynos_crtc->plane;
+	unsigned int crtc_w;
+	unsigned int crtc_h;
 	int ret;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	ret = exynos_drm_crtc_update(crtc);
+	crtc_w = crtc->fb->width - x;
+	crtc_h = crtc->fb->height - y;
+
+	ret = exynos_plane_mode_set(plane, crtc, crtc->fb, 0, 0, crtc_w, crtc_h,
+				    x, y, crtc_w, crtc_h);
 	if (ret)
 		return ret;
 
-	exynos_drm_crtc_apply(crtc);
+	exynos_drm_crtc_commit(crtc);
 
-	return ret;
+	return 0;
 }
 
 static void exynos_drm_crtc_load_lut(struct drm_crtc *crtc)
@@ -284,6 +180,16 @@ static void exynos_drm_crtc_load_lut(struct drm_crtc *crtc)
 	/* drm framework doesn't check NULL */
 }
 
+static void exynos_drm_crtc_disable(struct drm_crtc *crtc)
+{
+	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	exynos_plane_dpms(exynos_crtc->plane, DRM_MODE_DPMS_OFF);
+	exynos_drm_crtc_dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
 static struct drm_crtc_helper_funcs exynos_crtc_helper_funcs = {
 	.dpms		= exynos_drm_crtc_dpms,
 	.prepare	= exynos_drm_crtc_prepare,
@@ -292,6 +198,7 @@ static struct drm_crtc_helper_funcs exynos_crtc_helper_funcs = {
 	.mode_set	= exynos_drm_crtc_mode_set,
 	.mode_set_base	= exynos_drm_crtc_mode_set_base,
 	.load_lut	= exynos_drm_crtc_load_lut,
+	.disable	= exynos_drm_crtc_disable,
 };
 
 static int exynos_drm_crtc_page_flip(struct drm_crtc *crtc,
@@ -327,7 +234,8 @@ static int exynos_drm_crtc_page_flip(struct drm_crtc *crtc,
 				&dev_priv->pageflip_event_list);
 
 		crtc->fb = fb;
-		ret = exynos_drm_crtc_update(crtc);
+		ret = exynos_drm_crtc_mode_set_base(crtc, crtc->x, crtc->y,
+						    NULL);
 		if (ret) {
 			crtc->fb = old_fb;
 			drm_vblank_put(dev, exynos_crtc->pipe);
@@ -335,14 +243,6 @@ static int exynos_drm_crtc_page_flip(struct drm_crtc *crtc,
 
 			goto out;
 		}
-
-		/*
-		 * the values related to a buffer of the drm framebuffer
-		 * to be applied should be set at here. because these values
-		 * first, are set to shadow registers and then to
-		 * real registers at vsync front porch period.
-		 */
-		exynos_drm_crtc_apply(crtc);
 	}
 out:
 	mutex_unlock(&dev->struct_mutex);
@@ -362,18 +262,73 @@ static void exynos_drm_crtc_destroy(struct drm_crtc *crtc)
 	kfree(exynos_crtc);
 }
 
+static int exynos_drm_crtc_set_property(struct drm_crtc *crtc,
+					struct drm_property *property,
+					uint64_t val)
+{
+	struct drm_device *dev = crtc->dev;
+	struct exynos_drm_private *dev_priv = dev->dev_private;
+	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
+
+	DRM_DEBUG_KMS("%s\n", __func__);
+
+	if (property == dev_priv->crtc_mode_property) {
+		enum exynos_crtc_mode mode = val;
+
+		if (mode == exynos_crtc->mode)
+			return 0;
+
+		exynos_crtc->mode = mode;
+
+		switch (mode) {
+		case CRTC_MODE_NORMAL:
+			exynos_drm_crtc_commit(crtc);
+			break;
+		case CRTC_MODE_BLANK:
+			exynos_plane_dpms(exynos_crtc->plane,
+					  DRM_MODE_DPMS_OFF);
+			break;
+		default:
+			break;
+		}
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
 static struct drm_crtc_funcs exynos_crtc_funcs = {
 	.set_config	= drm_crtc_helper_set_config,
 	.page_flip	= exynos_drm_crtc_page_flip,
 	.destroy	= exynos_drm_crtc_destroy,
+	.set_property	= exynos_drm_crtc_set_property,
+};
+
+static const struct drm_prop_enum_list mode_names[] = {
+	{ CRTC_MODE_NORMAL, "normal" },
+	{ CRTC_MODE_BLANK, "blank" },
 };
 
-struct exynos_drm_overlay *get_exynos_drm_overlay(struct drm_device *dev,
-		struct drm_crtc *crtc)
+static void exynos_drm_crtc_attach_mode_property(struct drm_crtc *crtc)
 {
-	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
+	struct drm_device *dev = crtc->dev;
+	struct exynos_drm_private *dev_priv = dev->dev_private;
+	struct drm_property *prop;
 
-	return &exynos_crtc->overlay;
+	DRM_DEBUG_KMS("%s\n", __func__);
+
+	prop = dev_priv->crtc_mode_property;
+	if (!prop) {
+		prop = drm_property_create_enum(dev, 0, "mode", mode_names,
+						ARRAY_SIZE(mode_names));
+		if (!prop)
+			return;
+
+		dev_priv->crtc_mode_property = prop;
+	}
+
+	drm_object_attach_property(&crtc->base, prop, 0);
 }
 
 int exynos_drm_crtc_create(struct drm_device *dev, unsigned int nr)
@@ -392,7 +347,12 @@ int exynos_drm_crtc_create(struct drm_device *dev, unsigned int nr)
 
 	exynos_crtc->pipe = nr;
 	exynos_crtc->dpms = DRM_MODE_DPMS_OFF;
-	exynos_crtc->overlay.zpos = DEFAULT_ZPOS;
+	exynos_crtc->plane = exynos_plane_init(dev, 1 << nr, true);
+	if (!exynos_crtc->plane) {
+		kfree(exynos_crtc);
+		return -ENOMEM;
+	}
+
 	crtc = &exynos_crtc->drm_crtc;
 
 	private->crtc[nr] = crtc;
@@ -400,6 +360,8 @@ int exynos_drm_crtc_create(struct drm_device *dev, unsigned int nr)
 	drm_crtc_init(dev, crtc, &exynos_crtc_funcs);
 	drm_crtc_helper_add(crtc, &exynos_crtc_helper_funcs);
 
+	exynos_drm_crtc_attach_mode_property(crtc);
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.h b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
index 16b8e2195a0..6bae8d8c250 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
@@ -29,39 +29,8 @@
 #ifndef _EXYNOS_DRM_CRTC_H_
 #define _EXYNOS_DRM_CRTC_H_
 
-struct exynos_drm_overlay *get_exynos_drm_overlay(struct drm_device *dev,
-		struct drm_crtc *crtc);
 int exynos_drm_crtc_create(struct drm_device *dev, unsigned int nr);
 int exynos_drm_crtc_enable_vblank(struct drm_device *dev, int crtc);
 void exynos_drm_crtc_disable_vblank(struct drm_device *dev, int crtc);
 
-/*
- * Exynos specific crtc postion structure.
- *
- * @fb_x: offset x on a framebuffer to be displyed
- *	- the unit is screen coordinates.
- * @fb_y: offset y on a framebuffer to be displayed
- *	- the unit is screen coordinates.
- * @src_w: width of source area to be displayed from a framebuffer.
- * @src_h: height of source area to be displayed from a framebuffer.
- * @crtc_x: offset x on hardware screen.
- * @crtc_y: offset y on hardware screen.
- * @crtc_w: width of hardware screen.
- * @crtc_h: height of hardware screen.
- */
-struct exynos_drm_crtc_pos {
-	unsigned int fb_x;
-	unsigned int fb_y;
-	unsigned int src_w;
-	unsigned int src_h;
-	unsigned int crtc_x;
-	unsigned int crtc_y;
-	unsigned int crtc_w;
-	unsigned int crtc_h;
-};
-
-int exynos_drm_overlay_update(struct exynos_drm_overlay *overlay,
-			      struct drm_framebuffer *fb,
-			      struct drm_display_mode *mode,
-			      struct exynos_drm_crtc_pos *pos);
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
index 274909271c3..613bf8a5d9b 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
@@ -25,6 +25,7 @@
 
 #include "drmP.h"
 #include "drm.h"
+#include "exynos_drm.h"
 #include "exynos_drm_drv.h"
 #include "exynos_drm_gem.h"
 
@@ -86,6 +87,10 @@ static struct sg_table *
 	npages = buf->size / buf->page_size;
 
 	sgt = exynos_pages_to_sg(buf->pages, npages, buf->page_size);
+	if (!sgt) {
+		DRM_DEBUG_PRIME("exynos_pages_to_sg returned NULL!\n");
+		goto err_unlock;
+	}
 	nents = dma_map_sg(attach->dev, sgt->sgl, sgt->nents, dir);
 
 	DRM_DEBUG_PRIME("npages = %d buffer size = 0x%lx page_size = 0x%lx\n",
@@ -186,7 +191,7 @@ struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
 	struct exynos_drm_gem_obj *exynos_gem_obj;
 	struct exynos_drm_gem_buf *buffer;
 	struct page *page;
-	int ret, i = 0;
+	int ret;
 
 	DRM_DEBUG_PRIME("%s\n", __FILE__);
 
@@ -210,7 +215,7 @@ struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
 
 
 	sgt = dma_buf_map_attachment(attach, DMA_BIDIRECTIONAL);
-	if (IS_ERR(sgt)) {
+	if (IS_ERR_OR_NULL(sgt)) {
 		ret = PTR_ERR(sgt);
 		goto err_buf_detach;
 	}
@@ -236,13 +241,25 @@ struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
 	}
 
 	sgl = sgt->sgl;
-	buffer->dma_addr = sg_dma_address(sgl);
 
-	while (i < sgt->nents) {
-		buffer->pages[i] = sg_page(sgl);
-		buffer->size += sg_dma_len(sgl);
-		sgl = sg_next(sgl);
-		i++;
+	if (sgt->nents == 1) {
+		buffer->dma_addr = sg_dma_address(sgt->sgl);
+		buffer->size = sg_dma_len(sgt->sgl);
+
+		/* always physically continuous memory if sgt->nents is 1. */
+		exynos_gem_obj->flags |= EXYNOS_BO_CONTIG;
+	} else {
+		unsigned int i = 0;
+
+		buffer->dma_addr = sg_dma_address(sgl);
+		while (i < sgt->nents) {
+			buffer->pages[i] = sg_page(sgl);
+			buffer->size += sg_dma_len(sgl);
+			sgl = sg_next(sgl);
+			i++;
+		}
+
+		exynos_gem_obj->flags |= EXYNOS_BO_NONCONTIG;
 	}
 
 	exynos_gem_obj->buffer = buffer;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index d6de2e07fa0..ebacec6f1e4 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -85,8 +85,11 @@ static int exynos_drm_load(struct drm_device *dev, unsigned long flags)
 	}
 
 	for (nr = 0; nr < MAX_PLANE; nr++) {
-		ret = exynos_plane_init(dev, nr);
-		if (ret)
+		struct drm_plane *plane;
+		unsigned int possible_crtcs = (1 << MAX_CRTC) - 1;
+
+		plane = exynos_plane_init(dev, possible_crtcs, false);
+		if (!plane)
 			goto err_crtc;
 	}
 
@@ -221,8 +224,6 @@ static struct drm_ioctl_desc exynos_ioctls[] = {
 			exynos_drm_gem_mmap_ioctl, DRM_UNLOCKED | DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_GET,
 			exynos_drm_gem_get_ioctl, DRM_UNLOCKED),
-	DRM_IOCTL_DEF_DRV(EXYNOS_PLANE_SET_ZPOS, exynos_plane_set_zpos_ioctl,
-			DRM_UNLOCKED | DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_VIDI_CONNECTION,
 			vidi_connection_ioctl, DRM_UNLOCKED | DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_G2D_GET_VER,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index c82c90c443e..e22704b249d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -59,12 +59,14 @@ enum exynos_drm_output_type {
  *
  * @mode_set: copy drm overlay info to hw specific overlay info.
  * @commit: apply hardware specific overlay data to registers.
+ * @enable: enable hardware specific overlay.
  * @disable: disable hardware specific overlay.
  */
 struct exynos_drm_overlay_ops {
 	void (*mode_set)(struct device *subdrv_dev,
 			 struct exynos_drm_overlay *overlay);
 	void (*commit)(struct device *subdrv_dev, int zpos);
+	void (*enable)(struct device *subdrv_dev, int zpos);
 	void (*disable)(struct device *subdrv_dev, int zpos);
 };
 
@@ -174,7 +176,7 @@ struct exynos_drm_manager_ops {
 	void (*apply)(struct device *subdrv_dev);
 	void (*mode_fixup)(struct device *subdrv_dev,
 				struct drm_connector *connector,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode);
 	void (*mode_set)(struct device *subdrv_dev, void *mode);
 	void (*get_max_resol)(struct device *subdrv_dev, unsigned int *width,
@@ -235,6 +237,8 @@ struct exynos_drm_private {
 	 * this array is used to be aware of which crtc did it request vblank.
 	 */
 	struct drm_crtc *crtc[MAX_CRTC];
+	struct drm_property *plane_zpos_property;
+	struct drm_property *crtc_mode_property;
 };
 
 /*
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.c b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
index 23d5ad379f8..2c037cd7d2d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
@@ -30,7 +30,6 @@
 #include "drm_crtc_helper.h"
 
 #include "exynos_drm_drv.h"
-#include "exynos_drm_crtc.h"
 #include "exynos_drm_encoder.h"
 
 #define to_exynos_encoder(x)	container_of(x, struct exynos_drm_encoder,\
@@ -108,7 +107,7 @@ static void exynos_drm_encoder_dpms(struct drm_encoder *encoder, int mode)
 
 static bool
 exynos_drm_encoder_mode_fixup(struct drm_encoder *encoder,
-			       struct drm_display_mode *mode,
+			       const struct drm_display_mode *mode,
 			       struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
@@ -136,21 +135,16 @@ static void exynos_drm_encoder_mode_set(struct drm_encoder *encoder,
 	struct drm_connector *connector;
 	struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
 	struct exynos_drm_manager_ops *manager_ops = manager->ops;
-	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
-	struct exynos_drm_overlay *overlay = get_exynos_drm_overlay(dev,
-						encoder->crtc);
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	exynos_drm_encoder_dpms(encoder, DRM_MODE_DPMS_ON);
+
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (connector->encoder == encoder) {
+		if (connector->encoder == encoder)
 			if (manager_ops && manager_ops->mode_set)
 				manager_ops->mode_set(manager->dev,
 							adjusted_mode);
-
-			if (overlay_ops && overlay_ops->mode_set)
-				overlay_ops->mode_set(manager->dev, overlay);
-		}
 	}
 }
 
@@ -310,8 +304,8 @@ void exynos_drm_enable_vblank(struct drm_encoder *encoder, void *data)
 	struct exynos_drm_manager_ops *manager_ops = manager->ops;
 	int crtc = *(int *)data;
 
-	if (manager->pipe == -1)
-		manager->pipe = crtc;
+	if (manager->pipe != crtc)
+		return;
 
 	if (manager_ops->enable_vblank)
 		manager_ops->enable_vblank(manager->dev);
@@ -324,34 +318,41 @@ void exynos_drm_disable_vblank(struct drm_encoder *encoder, void *data)
 	struct exynos_drm_manager_ops *manager_ops = manager->ops;
 	int crtc = *(int *)data;
 
-	if (manager->pipe == -1)
-		manager->pipe = crtc;
+	if (manager->pipe != crtc)
+		return;
 
 	if (manager_ops->disable_vblank)
 		manager_ops->disable_vblank(manager->dev);
 }
 
-void exynos_drm_encoder_crtc_plane_commit(struct drm_encoder *encoder,
-					  void *data)
+void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data)
 {
-	struct exynos_drm_manager *manager =
-		to_exynos_encoder(encoder)->manager;
-	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
-	int zpos = DEFAULT_ZPOS;
+	struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
+	struct exynos_drm_manager *manager = exynos_encoder->manager;
+	struct exynos_drm_manager_ops *manager_ops = manager->ops;
+	int mode = *(int *)data;
 
-	if (data)
-		zpos = *(int *)data;
+	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (overlay_ops && overlay_ops->commit)
-		overlay_ops->commit(manager->dev, zpos);
+	if (manager_ops && manager_ops->dpms)
+		manager_ops->dpms(manager->dev, mode);
+
+	/*
+	 * if this condition is ok then it means that the crtc is already
+	 * detached from encoder and last function for detaching is properly
+	 * done, so clear pipe from manager to prevent repeated call.
+	 */
+	if (mode > DRM_MODE_DPMS_ON) {
+		if (!encoder->crtc)
+			manager->pipe = -1;
+	}
 }
 
-void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data)
+void exynos_drm_encoder_crtc_pipe(struct drm_encoder *encoder, void *data)
 {
 	struct exynos_drm_manager *manager =
 		to_exynos_encoder(encoder)->manager;
-	int crtc = *(int *)data;
-	int zpos = DEFAULT_ZPOS;
+	int pipe = *(int *)data;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -359,76 +360,62 @@ void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data)
 	 * when crtc is detached from encoder, this pipe is used
 	 * to select manager operation
 	 */
-	manager->pipe = crtc;
-
-	exynos_drm_encoder_crtc_plane_commit(encoder, &zpos);
+	manager->pipe = pipe;
 }
 
-void exynos_drm_encoder_dpms_from_crtc(struct drm_encoder *encoder, void *data)
+void exynos_drm_encoder_plane_mode_set(struct drm_encoder *encoder, void *data)
 {
-	struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
-	int mode = *(int *)data;
+	struct exynos_drm_manager *manager =
+		to_exynos_encoder(encoder)->manager;
+	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
+	struct exynos_drm_overlay *overlay = data;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	exynos_drm_encoder_dpms(encoder, mode);
-
-	exynos_encoder->dpms = mode;
+	if (overlay_ops && overlay_ops->mode_set)
+		overlay_ops->mode_set(manager->dev, overlay);
 }
 
-void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data)
+void exynos_drm_encoder_plane_commit(struct drm_encoder *encoder, void *data)
 {
-	struct drm_device *dev = encoder->dev;
-	struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
-	struct exynos_drm_manager *manager = exynos_encoder->manager;
-	struct exynos_drm_manager_ops *manager_ops = manager->ops;
-	struct drm_connector *connector;
-	int mode = *(int *)data;
+	struct exynos_drm_manager *manager =
+		to_exynos_encoder(encoder)->manager;
+	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
+	int zpos = DEFAULT_ZPOS;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (manager_ops && manager_ops->dpms)
-		manager_ops->dpms(manager->dev, mode);
-
-	/*
-	 * set current dpms mode to the connector connected to
-	 * current encoder. connector->dpms would be checked
-	 * at drm_helper_connector_dpms()
-	 */
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		if (connector->encoder == encoder)
-			connector->dpms = mode;
+	if (data)
+		zpos = *(int *)data;
 
-	/*
-	 * if this condition is ok then it means that the crtc is already
-	 * detached from encoder and last function for detaching is properly
-	 * done, so clear pipe from manager to prevent repeated call.
-	 */
-	if (mode > DRM_MODE_DPMS_ON) {
-		if (!encoder->crtc)
-			manager->pipe = -1;
-	}
+	if (overlay_ops && overlay_ops->commit)
+		overlay_ops->commit(manager->dev, zpos);
 }
 
-void exynos_drm_encoder_crtc_mode_set(struct drm_encoder *encoder, void *data)
+void exynos_drm_encoder_plane_enable(struct drm_encoder *encoder, void *data)
 {
 	struct exynos_drm_manager *manager =
 		to_exynos_encoder(encoder)->manager;
 	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
-	struct exynos_drm_overlay *overlay = data;
+	int zpos = DEFAULT_ZPOS;
 
-	if (overlay_ops && overlay_ops->mode_set)
-		overlay_ops->mode_set(manager->dev, overlay);
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (data)
+		zpos = *(int *)data;
+
+	if (overlay_ops && overlay_ops->enable)
+		overlay_ops->enable(manager->dev, zpos);
 }
 
-void exynos_drm_encoder_crtc_disable(struct drm_encoder *encoder, void *data)
+void exynos_drm_encoder_plane_disable(struct drm_encoder *encoder, void *data)
 {
 	struct exynos_drm_manager *manager =
 		to_exynos_encoder(encoder)->manager;
 	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
 	int zpos = DEFAULT_ZPOS;
 
-	DRM_DEBUG_KMS("\n");
+	DRM_DEBUG_KMS("%s\n", __FILE__);
 
 	if (data)
 		zpos = *(int *)data;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.h b/drivers/gpu/drm/exynos/exynos_drm_encoder.h
index eb7d2316847..6470d9ddf5a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.h
@@ -40,13 +40,11 @@ void exynos_drm_fn_encoder(struct drm_crtc *crtc, void *data,
 			    void (*fn)(struct drm_encoder *, void *));
 void exynos_drm_enable_vblank(struct drm_encoder *encoder, void *data);
 void exynos_drm_disable_vblank(struct drm_encoder *encoder, void *data);
-void exynos_drm_encoder_crtc_plane_commit(struct drm_encoder *encoder,
-					  void *data);
-void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data);
-void exynos_drm_encoder_dpms_from_crtc(struct drm_encoder *encoder,
-					void *data);
 void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data);
-void exynos_drm_encoder_crtc_mode_set(struct drm_encoder *encoder, void *data);
-void exynos_drm_encoder_crtc_disable(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_crtc_pipe(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_plane_mode_set(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_plane_commit(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_plane_enable(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_plane_disable(struct drm_encoder *encoder, void *data);
 
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 29fdbfeb43c..a68d2b313f0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -78,7 +78,6 @@ struct fimd_context {
 	struct drm_crtc			*crtc;
 	struct clk			*bus_clk;
 	struct clk			*lcd_clk;
-	struct resource			*regs_res;
 	void __iomem			*regs;
 	struct fimd_win_data		win_data[WINDOWS_NR];
 	unsigned int			clkdiv;
@@ -813,7 +812,7 @@ static int __devinit fimd_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
 		return -ENOMEM;
 
@@ -838,33 +837,26 @@ static int __devinit fimd_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
-	ctx->regs_res = request_mem_region(res->start, resource_size(res),
-					   dev_name(dev));
-	if (!ctx->regs_res) {
-		dev_err(dev, "failed to claim register region\n");
-		ret = -ENOENT;
-		goto err_clk;
-	}
-
-	ctx->regs = ioremap(res->start, resource_size(res));
+	ctx->regs = devm_request_and_ioremap(&pdev->dev, res);
 	if (!ctx->regs) {
 		dev_err(dev, "failed to map registers\n");
 		ret = -ENXIO;
-		goto err_req_region_io;
+		goto err_clk;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
 		dev_err(dev, "irq request failed.\n");
-		goto err_req_region_irq;
+		goto err_clk;
 	}
 
 	ctx->irq = res->start;
 
-	ret = request_irq(ctx->irq, fimd_irq_handler, 0, "drm_fimd", ctx);
-	if (ret < 0) {
+	ret = devm_request_irq(&pdev->dev, ctx->irq, fimd_irq_handler,
+							0, "drm_fimd", ctx);
+	if (ret) {
 		dev_err(dev, "irq request failed.\n");
-		goto err_req_irq;
+		goto err_clk;
 	}
 
 	ctx->vidcon0 = pdata->vidcon0;
@@ -899,14 +891,6 @@ static int __devinit fimd_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_req_irq:
-err_req_region_irq:
-	iounmap(ctx->regs);
-
-err_req_region_io:
-	release_resource(ctx->regs_res);
-	kfree(ctx->regs_res);
-
 err_clk:
 	clk_disable(ctx->lcd_clk);
 	clk_put(ctx->lcd_clk);
@@ -916,7 +900,6 @@ err_bus_clk:
 	clk_put(ctx->bus_clk);
 
 err_clk_get:
-	kfree(ctx);
 	return ret;
 }
 
@@ -944,13 +927,6 @@ out:
 	clk_put(ctx->lcd_clk);
 	clk_put(ctx->bus_clk);
 
-	iounmap(ctx->regs);
-	release_resource(ctx->regs_res);
-	kfree(ctx->regs_res);
-	free_irq(ctx->irq, ctx);
-
-	kfree(ctx);
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 5c8b683029e..f9efde40c09 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -99,25 +99,17 @@ out:
 struct page **exynos_gem_get_pages(struct drm_gem_object *obj,
 						gfp_t gfpmask)
 {
-	struct inode *inode;
-	struct address_space *mapping;
 	struct page *p, **pages;
 	int i, npages;
 
-	/* This is the shared memory object that backs the GEM resource */
-	inode = obj->filp->f_path.dentry->d_inode;
-	mapping = inode->i_mapping;
-
 	npages = obj->size >> PAGE_SHIFT;
 
 	pages = drm_malloc_ab(npages, sizeof(struct page *));
 	if (pages == NULL)
 		return ERR_PTR(-ENOMEM);
 
-	gfpmask |= mapping_gfp_mask(mapping);
-
 	for (i = 0; i < npages; i++) {
-		p = shmem_read_mapping_page_gfp(mapping, i, gfpmask);
+		p = alloc_page(gfpmask);
 		if (IS_ERR(p))
 			goto fail;
 		pages[i] = p;
@@ -126,31 +118,22 @@ struct page **exynos_gem_get_pages(struct drm_gem_object *obj,
 	return pages;
 
 fail:
-	while (i--)
-		page_cache_release(pages[i]);
+	while (--i)
+		__free_page(pages[i]);
 
 	drm_free_large(pages);
 	return ERR_PTR(PTR_ERR(p));
 }
 
 static void exynos_gem_put_pages(struct drm_gem_object *obj,
-					struct page **pages,
-					bool dirty, bool accessed)
+					struct page **pages)
 {
-	int i, npages;
+	int npages;
 
 	npages = obj->size >> PAGE_SHIFT;
 
-	for (i = 0; i < npages; i++) {
-		if (dirty)
-			set_page_dirty(pages[i]);
-
-		if (accessed)
-			mark_page_accessed(pages[i]);
-
-		/* Undo the reference we took when populating the table */
-		page_cache_release(pages[i]);
-	}
+	while (--npages >= 0)
+		__free_page(pages[npages]);
 
 	drm_free_large(pages);
 }
@@ -189,7 +172,7 @@ static int exynos_drm_gem_get_pages(struct drm_gem_object *obj)
 		return -EINVAL;
 	}
 
-	pages = exynos_gem_get_pages(obj, GFP_KERNEL);
+	pages = exynos_gem_get_pages(obj, GFP_HIGHUSER_MOVABLE);
 	if (IS_ERR(pages)) {
 		DRM_ERROR("failed to get pages.\n");
 		return PTR_ERR(pages);
@@ -230,7 +213,7 @@ err1:
 	kfree(buf->sgt);
 	buf->sgt = NULL;
 err:
-	exynos_gem_put_pages(obj, pages, true, false);
+	exynos_gem_put_pages(obj, pages);
 	return ret;
 
 }
@@ -248,7 +231,7 @@ static void exynos_drm_gem_put_pages(struct drm_gem_object *obj)
 	kfree(buf->sgt);
 	buf->sgt = NULL;
 
-	exynos_gem_put_pages(obj, buf->pages, true, false);
+	exynos_gem_put_pages(obj, buf->pages);
 	buf->pages = NULL;
 
 	/* add some codes for UNCACHED type here. TODO */
@@ -291,11 +274,21 @@ void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj)
 	if (!buf->pages)
 		return;
 
+	/*
+	 * do not release memory region from exporter.
+	 *
+	 * the region will be released by exporter
+	 * once dmabuf's refcount becomes 0.
+	 */
+	if (obj->import_attach)
+		goto out;
+
 	if (exynos_gem_obj->flags & EXYNOS_BO_NONCONTIG)
 		exynos_drm_gem_put_pages(obj);
 	else
 		exynos_drm_free_buf(obj->dev, exynos_gem_obj->flags, buf);
 
+out:
 	exynos_drm_fini_buf(obj->dev, buf);
 	exynos_gem_obj->buffer = NULL;
 
@@ -668,7 +661,7 @@ int exynos_drm_gem_dumb_create(struct drm_file *file_priv,
 	 *	with DRM_IOCTL_MODE_CREATE_DUMB command.
 	 */
 
-	args->pitch = args->width * args->bpp >> 3;
+	args->pitch = args->width * ((args->bpp + 7) / 8);
 	args->size = PAGE_ALIGN(args->pitch * args->height);
 
 	exynos_gem_obj = exynos_drm_gem_create(dev, args->flags, args->size);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index 14d038b6cb0..085b2a5d5f7 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -63,7 +63,8 @@ struct exynos_drm_gem_buf {
  *	by user request or at framebuffer creation.
  *	continuous memory region allocated by user request
  *	or at framebuffer creation.
- * @size: total memory size to physically non-continuous memory region.
+ * @size: size requested from user, in bytes and this size is aligned
+ *	in page unit.
  * @flags: indicate memory type to allocated buffer and cache attruibute.
  *
  * P.S. this object would be transfered to user as kms_bo.handle so
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
index 5d9d2c2f8f3..8ffcdf8b9e2 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
@@ -142,7 +142,7 @@ static void drm_hdmi_disable_vblank(struct device *subdrv_dev)
 
 static void drm_hdmi_mode_fixup(struct device *subdrv_dev,
 				struct drm_connector *connector,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
 	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
index bd8126996e5..a91c42088e4 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
@@ -51,7 +51,7 @@ struct exynos_hdmi_ops {
 
 	/* manager */
 	void (*mode_fixup)(void *ctx, struct drm_connector *connector,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode);
 	void (*mode_set)(void *ctx, void *mode);
 	void (*get_max_resol)(void *ctx, unsigned int *width,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index c4c6525d465..b89829e5043 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -12,9 +12,12 @@
 #include "drmP.h"
 
 #include "exynos_drm.h"
-#include "exynos_drm_crtc.h"
 #include "exynos_drm_drv.h"
 #include "exynos_drm_encoder.h"
+#include "exynos_drm_fb.h"
+#include "exynos_drm_gem.h"
+
+#define to_exynos_plane(x)	container_of(x, struct exynos_plane, base)
 
 struct exynos_plane {
 	struct drm_plane		base;
@@ -30,6 +33,108 @@ static const uint32_t formats[] = {
 	DRM_FORMAT_NV12MT,
 };
 
+int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
+			  struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+			  unsigned int crtc_w, unsigned int crtc_h,
+			  uint32_t src_x, uint32_t src_y,
+			  uint32_t src_w, uint32_t src_h)
+{
+	struct exynos_plane *exynos_plane = to_exynos_plane(plane);
+	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
+	unsigned int actual_w;
+	unsigned int actual_h;
+	int nr;
+	int i;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	nr = exynos_drm_format_num_buffers(fb->pixel_format);
+	for (i = 0; i < nr; i++) {
+		struct exynos_drm_gem_buf *buffer = exynos_drm_fb_buffer(fb, i);
+
+		if (!buffer) {
+			DRM_LOG_KMS("buffer is null\n");
+			return -EFAULT;
+		}
+
+		overlay->dma_addr[i] = buffer->dma_addr;
+		overlay->vaddr[i] = buffer->kvaddr;
+
+		DRM_DEBUG_KMS("buffer: %d, vaddr = 0x%lx, dma_addr = 0x%lx\n",
+				i, (unsigned long)overlay->vaddr[i],
+				(unsigned long)overlay->dma_addr[i]);
+	}
+
+	actual_w = min((unsigned)(crtc->mode.hdisplay - crtc_x), crtc_w);
+	actual_h = min((unsigned)(crtc->mode.vdisplay - crtc_y), crtc_h);
+
+	/* set drm framebuffer data. */
+	overlay->fb_x = src_x;
+	overlay->fb_y = src_y;
+	overlay->fb_width = fb->width;
+	overlay->fb_height = fb->height;
+	overlay->src_width = src_w;
+	overlay->src_height = src_h;
+	overlay->bpp = fb->bits_per_pixel;
+	overlay->pitch = fb->pitches[0];
+	overlay->pixel_format = fb->pixel_format;
+
+	/* set overlay range to be displayed. */
+	overlay->crtc_x = crtc_x;
+	overlay->crtc_y = crtc_y;
+	overlay->crtc_width = actual_w;
+	overlay->crtc_height = actual_h;
+
+	/* set drm mode data. */
+	overlay->mode_width = crtc->mode.hdisplay;
+	overlay->mode_height = crtc->mode.vdisplay;
+	overlay->refresh = crtc->mode.vrefresh;
+	overlay->scan_flag = crtc->mode.flags;
+
+	DRM_DEBUG_KMS("overlay : offset_x/y(%d,%d), width/height(%d,%d)",
+			overlay->crtc_x, overlay->crtc_y,
+			overlay->crtc_width, overlay->crtc_height);
+
+	exynos_drm_fn_encoder(crtc, overlay, exynos_drm_encoder_plane_mode_set);
+
+	return 0;
+}
+
+void exynos_plane_commit(struct drm_plane *plane)
+{
+	struct exynos_plane *exynos_plane = to_exynos_plane(plane);
+	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
+
+	exynos_drm_fn_encoder(plane->crtc, &overlay->zpos,
+			exynos_drm_encoder_plane_commit);
+}
+
+void exynos_plane_dpms(struct drm_plane *plane, int mode)
+{
+	struct exynos_plane *exynos_plane = to_exynos_plane(plane);
+	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (mode == DRM_MODE_DPMS_ON) {
+		if (exynos_plane->enabled)
+			return;
+
+		exynos_drm_fn_encoder(plane->crtc, &overlay->zpos,
+				exynos_drm_encoder_plane_enable);
+
+		exynos_plane->enabled = true;
+	} else {
+		if (!exynos_plane->enabled)
+			return;
+
+		exynos_drm_fn_encoder(plane->crtc, &overlay->zpos,
+				exynos_drm_encoder_plane_disable);
+
+		exynos_plane->enabled = false;
+	}
+}
+
 static int
 exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		     struct drm_framebuffer *fb, int crtc_x, int crtc_y,
@@ -37,64 +142,37 @@ exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		     uint32_t src_x, uint32_t src_y,
 		     uint32_t src_w, uint32_t src_h)
 {
-	struct exynos_plane *exynos_plane =
-		container_of(plane, struct exynos_plane, base);
-	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
-	struct exynos_drm_crtc_pos pos;
 	int ret;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	memset(&pos, 0, sizeof(struct exynos_drm_crtc_pos));
-	pos.crtc_x = crtc_x;
-	pos.crtc_y = crtc_y;
-	pos.crtc_w = crtc_w;
-	pos.crtc_h = crtc_h;
-
-	/* considering 16.16 fixed point of source values */
-	pos.fb_x = src_x >> 16;
-	pos.fb_y = src_y >> 16;
-	pos.src_w = src_w >> 16;
-	pos.src_h = src_h >> 16;
-
-	ret = exynos_drm_overlay_update(overlay, fb, &crtc->mode, &pos);
+	ret = exynos_plane_mode_set(plane, crtc, fb, crtc_x, crtc_y,
+			crtc_w, crtc_h, src_x >> 16, src_y >> 16,
+			src_w >> 16, src_h >> 16);
 	if (ret < 0)
 		return ret;
 
-	exynos_drm_fn_encoder(crtc, overlay,
-			exynos_drm_encoder_crtc_mode_set);
-	exynos_drm_fn_encoder(crtc, &overlay->zpos,
-			exynos_drm_encoder_crtc_plane_commit);
+	plane->crtc = crtc;
+	plane->fb = crtc->fb;
 
-	exynos_plane->enabled = true;
+	exynos_plane_commit(plane);
+	exynos_plane_dpms(plane, DRM_MODE_DPMS_ON);
 
 	return 0;
 }
 
 static int exynos_disable_plane(struct drm_plane *plane)
 {
-	struct exynos_plane *exynos_plane =
-		container_of(plane, struct exynos_plane, base);
-	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
-
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	if (!exynos_plane->enabled)
-		return 0;
-
-	exynos_drm_fn_encoder(plane->crtc, &overlay->zpos,
-			exynos_drm_encoder_crtc_disable);
-
-	exynos_plane->enabled = false;
-	exynos_plane->overlay.zpos = DEFAULT_ZPOS;
+	exynos_plane_dpms(plane, DRM_MODE_DPMS_OFF);
 
 	return 0;
 }
 
 static void exynos_plane_destroy(struct drm_plane *plane)
 {
-	struct exynos_plane *exynos_plane =
-		container_of(plane, struct exynos_plane, base);
+	struct exynos_plane *exynos_plane = to_exynos_plane(plane);
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
@@ -103,69 +181,79 @@ static void exynos_plane_destroy(struct drm_plane *plane)
 	kfree(exynos_plane);
 }
 
+static int exynos_plane_set_property(struct drm_plane *plane,
+				     struct drm_property *property,
+				     uint64_t val)
+{
+	struct drm_device *dev = plane->dev;
+	struct exynos_plane *exynos_plane = to_exynos_plane(plane);
+	struct exynos_drm_private *dev_priv = dev->dev_private;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (property == dev_priv->plane_zpos_property) {
+		exynos_plane->overlay.zpos = val;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
 static struct drm_plane_funcs exynos_plane_funcs = {
 	.update_plane	= exynos_update_plane,
 	.disable_plane	= exynos_disable_plane,
 	.destroy	= exynos_plane_destroy,
+	.set_property	= exynos_plane_set_property,
 };
 
-int exynos_plane_init(struct drm_device *dev, unsigned int nr)
+static void exynos_plane_attach_zpos_property(struct drm_plane *plane)
 {
-	struct exynos_plane *exynos_plane;
-	uint32_t possible_crtcs;
+	struct drm_device *dev = plane->dev;
+	struct exynos_drm_private *dev_priv = dev->dev_private;
+	struct drm_property *prop;
 
-	exynos_plane = kzalloc(sizeof(struct exynos_plane), GFP_KERNEL);
-	if (!exynos_plane)
-		return -ENOMEM;
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	/* all CRTCs are available */
-	possible_crtcs = (1 << MAX_CRTC) - 1;
+	prop = dev_priv->plane_zpos_property;
+	if (!prop) {
+		prop = drm_property_create_range(dev, 0, "zpos", 0,
+						 MAX_PLANE - 1);
+		if (!prop)
+			return;
 
-	exynos_plane->overlay.zpos = DEFAULT_ZPOS;
+		dev_priv->plane_zpos_property = prop;
+	}
 
-	return drm_plane_init(dev, &exynos_plane->base, possible_crtcs,
-			      &exynos_plane_funcs, formats, ARRAY_SIZE(formats),
-			      false);
+	drm_object_attach_property(&plane->base, prop, 0);
 }
 
-int exynos_plane_set_zpos_ioctl(struct drm_device *dev, void *data,
-				struct drm_file *file_priv)
+struct drm_plane *exynos_plane_init(struct drm_device *dev,
+				    unsigned int possible_crtcs, bool priv)
 {
-	struct drm_exynos_plane_set_zpos *zpos_req = data;
-	struct drm_mode_object *obj;
-	struct drm_plane *plane;
 	struct exynos_plane *exynos_plane;
-	int ret = 0;
+	int err;
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	if (!drm_core_check_feature(dev, DRIVER_MODESET))
-		return -EINVAL;
-
-	if (zpos_req->zpos < 0 || zpos_req->zpos >= MAX_PLANE) {
-		if (zpos_req->zpos != DEFAULT_ZPOS) {
-			DRM_ERROR("zpos not within limits\n");
-			return -EINVAL;
-		}
+	exynos_plane = kzalloc(sizeof(struct exynos_plane), GFP_KERNEL);
+	if (!exynos_plane) {
+		DRM_ERROR("failed to allocate plane\n");
+		return NULL;
 	}
 
-	mutex_lock(&dev->mode_config.mutex);
-
-	obj = drm_mode_object_find(dev, zpos_req->plane_id,
-			DRM_MODE_OBJECT_PLANE);
-	if (!obj) {
-		DRM_DEBUG_KMS("Unknown plane ID %d\n",
-			      zpos_req->plane_id);
-		ret = -EINVAL;
-		goto out;
+	err = drm_plane_init(dev, &exynos_plane->base, possible_crtcs,
+			      &exynos_plane_funcs, formats, ARRAY_SIZE(formats),
+			      priv);
+	if (err) {
+		DRM_ERROR("failed to initialize plane\n");
+		kfree(exynos_plane);
+		return NULL;
 	}
 
-	plane = obj_to_plane(obj);
-	exynos_plane = container_of(plane, struct exynos_plane, base);
-
-	exynos_plane->overlay.zpos = zpos_req->zpos;
+	if (priv)
+		exynos_plane->overlay.zpos = DEFAULT_ZPOS;
+	else
+		exynos_plane_attach_zpos_property(&exynos_plane->base);
 
-out:
-	mutex_unlock(&dev->mode_config.mutex);
-	return ret;
+	return &exynos_plane->base;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.h b/drivers/gpu/drm/exynos/exynos_drm_plane.h
index 16b71f8217e..88312458580 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.h
@@ -9,6 +9,12 @@
  *
  */
 
-int exynos_plane_init(struct drm_device *dev, unsigned int nr);
-int exynos_plane_set_zpos_ioctl(struct drm_device *dev, void *data,
-				struct drm_file *file_priv);
+int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
+			  struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+			  unsigned int crtc_w, unsigned int crtc_h,
+			  uint32_t src_x, uint32_t src_y,
+			  uint32_t src_w, uint32_t src_h);
+void exynos_plane_commit(struct drm_plane *plane);
+void exynos_plane_dpms(struct drm_plane *plane, int mode);
+struct drm_plane *exynos_plane_init(struct drm_device *dev,
+				    unsigned int possible_crtcs, bool priv);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 7b9c153dceb..bb1550c4dd5 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -85,8 +85,6 @@ static const char fake_edid_info[] = {
 	0x00, 0x00, 0x00, 0x06
 };
 
-static void vidi_fake_vblank_handler(struct work_struct *work);
-
 static bool vidi_display_is_connected(struct device *dev)
 {
 	struct vidi_context *ctx = get_vidi_context(dev);
@@ -531,6 +529,16 @@ static int vidi_store_connection(struct device *dev,
 	if (ctx->connected > 1)
 		return -EINVAL;
 
+	/* use fake edid data for test. */
+	if (!ctx->raw_edid)
+		ctx->raw_edid = (struct edid *)fake_edid_info;
+
+	/* if raw_edid isn't same as fake data then it can't be tested. */
+	if (ctx->raw_edid != (struct edid *)fake_edid_info) {
+		DRM_DEBUG_KMS("edid data is not fake data.\n");
+		return -EINVAL;
+	}
+
 	DRM_DEBUG_KMS("requested connection.\n");
 
 	drm_helper_hpd_irq_event(ctx->subdrv.drm_dev);
@@ -549,6 +557,8 @@ int vidi_connection_ioctl(struct drm_device *drm_dev, void *data,
 	struct exynos_drm_manager *manager;
 	struct exynos_drm_display_ops *display_ops;
 	struct drm_exynos_vidi_connection *vidi = data;
+	struct edid *raw_edid;
+	int edid_len;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -557,11 +567,6 @@ int vidi_connection_ioctl(struct drm_device *drm_dev, void *data,
 		return -EINVAL;
 	}
 
-	if (!vidi->edid) {
-		DRM_DEBUG_KMS("edid data is null.\n");
-		return -EINVAL;
-	}
-
 	if (vidi->connection > 1) {
 		DRM_DEBUG_KMS("connection should be 0 or 1.\n");
 		return -EINVAL;
@@ -588,8 +593,30 @@ int vidi_connection_ioctl(struct drm_device *drm_dev, void *data,
 		return -EINVAL;
 	}
 
-	if (vidi->connection)
-		ctx->raw_edid = (struct edid *)vidi->edid;
+	if (vidi->connection) {
+		if (!vidi->edid) {
+			DRM_DEBUG_KMS("edid data is null.\n");
+			return -EINVAL;
+		}
+		raw_edid = (struct edid *)(uint32_t)vidi->edid;
+		edid_len = (1 + raw_edid->extensions) * EDID_LENGTH;
+		ctx->raw_edid = kzalloc(edid_len, GFP_KERNEL);
+		if (!ctx->raw_edid) {
+			DRM_DEBUG_KMS("failed to allocate raw_edid.\n");
+			return -ENOMEM;
+		}
+		memcpy(ctx->raw_edid, raw_edid, edid_len);
+	} else {
+		/*
+		 * with connection = 0, free raw_edid
+		 * only if raw edid data isn't same as fake data.
+		 */
+		if (ctx->raw_edid && ctx->raw_edid !=
+				(struct edid *)fake_edid_info) {
+			kfree(ctx->raw_edid);
+			ctx->raw_edid = NULL;
+		}
+	}
 
 	ctx->connected = vidi->connection;
 	drm_helper_hpd_irq_event(ctx->subdrv.drm_dev);
@@ -614,9 +641,6 @@ static int __devinit vidi_probe(struct platform_device *pdev)
 
 	INIT_WORK(&ctx->work, vidi_fake_vblank_handler);
 
-	/* for test */
-	ctx->raw_edid = (struct edid *)fake_edid_info;
-
 	subdrv = &ctx->subdrv;
 	subdrv->dev = dev;
 	subdrv->manager = &vidi_manager;
@@ -644,6 +668,11 @@ static int __devexit vidi_remove(struct platform_device *pdev)
 
 	exynos_drm_subdrv_unregister(&ctx->subdrv);
 
+	if (ctx->raw_edid != (struct edid *)fake_edid_info) {
+		kfree(ctx->raw_edid);
+		ctx->raw_edid = NULL;
+	}
+
 	kfree(ctx);
 
 	return 0;
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index a137e9e39a3..409e2ec1207 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -63,7 +63,6 @@ struct hdmi_context {
 	bool				dvi_mode;
 	struct mutex			hdmi_mutex;
 
-	struct resource			*regs_res;
 	void __iomem			*regs;
 	unsigned int			external_irq;
 	unsigned int			internal_irq;
@@ -1940,7 +1939,7 @@ static void hdmi_conf_apply(struct hdmi_context *hdata)
 }
 
 static void hdmi_mode_fixup(void *ctx, struct drm_connector *connector,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
 	struct drm_display_mode *m;
@@ -2280,16 +2279,17 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	drm_hdmi_ctx = kzalloc(sizeof(*drm_hdmi_ctx), GFP_KERNEL);
+	drm_hdmi_ctx = devm_kzalloc(&pdev->dev, sizeof(*drm_hdmi_ctx),
+								GFP_KERNEL);
 	if (!drm_hdmi_ctx) {
 		DRM_ERROR("failed to allocate common hdmi context.\n");
 		return -ENOMEM;
 	}
 
-	hdata = kzalloc(sizeof(struct hdmi_context), GFP_KERNEL);
+	hdata = devm_kzalloc(&pdev->dev, sizeof(struct hdmi_context),
+								GFP_KERNEL);
 	if (!hdata) {
 		DRM_ERROR("out of memory\n");
-		kfree(drm_hdmi_ctx);
 		return -ENOMEM;
 	}
 
@@ -2318,26 +2318,18 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 		goto err_resource;
 	}
 
-	hdata->regs_res = request_mem_region(res->start, resource_size(res),
-					   dev_name(dev));
-	if (!hdata->regs_res) {
-		DRM_ERROR("failed to claim register region\n");
-		ret = -ENOENT;
-		goto err_resource;
-	}
-
-	hdata->regs = ioremap(res->start, resource_size(res));
+	hdata->regs = devm_request_and_ioremap(&pdev->dev, res);
 	if (!hdata->regs) {
 		DRM_ERROR("failed to map registers\n");
 		ret = -ENXIO;
-		goto err_req_region;
+		goto err_resource;
 	}
 
 	/* DDC i2c driver */
 	if (i2c_add_driver(&ddc_driver)) {
 		DRM_ERROR("failed to register ddc i2c driver\n");
 		ret = -ENOENT;
-		goto err_iomap;
+		goto err_resource;
 	}
 
 	hdata->ddc_port = hdmi_ddc;
@@ -2398,16 +2390,9 @@ err_hdmiphy:
 	i2c_del_driver(&hdmiphy_driver);
 err_ddc:
 	i2c_del_driver(&ddc_driver);
-err_iomap:
-	iounmap(hdata->regs);
-err_req_region:
-	release_mem_region(hdata->regs_res->start,
-			resource_size(hdata->regs_res));
 err_resource:
 	hdmi_resources_cleanup(hdata);
 err_data:
-	kfree(hdata);
-	kfree(drm_hdmi_ctx);
 	return ret;
 }
 
@@ -2425,18 +2410,11 @@ static int __devexit hdmi_remove(struct platform_device *pdev)
 
 	hdmi_resources_cleanup(hdata);
 
-	iounmap(hdata->regs);
-
-	release_mem_region(hdata->regs_res->start,
-			resource_size(hdata->regs_res));
-
 	/* hdmiphy i2c driver */
 	i2c_del_driver(&hdmiphy_driver);
 	/* DDC i2c driver */
 	i2c_del_driver(&ddc_driver);
 
-	kfree(hdata);
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index e2147a2ddce..30fcc12f81d 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -956,7 +956,8 @@ static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 
 	clk_set_parent(mixer_res->sclk_mixer, mixer_res->sclk_hdmi);
 
-	mixer_res->mixer_regs = ioremap(res->start, resource_size(res));
+	mixer_res->mixer_regs = devm_ioremap(&pdev->dev, res->start,
+							resource_size(res));
 	if (mixer_res->mixer_regs == NULL) {
 		dev_err(dev, "register mapping failed.\n");
 		ret = -ENXIO;
@@ -967,38 +968,34 @@ static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 	if (res == NULL) {
 		dev_err(dev, "get memory resource failed.\n");
 		ret = -ENXIO;
-		goto fail_mixer_regs;
+		goto fail;
 	}
 
-	mixer_res->vp_regs = ioremap(res->start, resource_size(res));
+	mixer_res->vp_regs = devm_ioremap(&pdev->dev, res->start,
+							resource_size(res));
 	if (mixer_res->vp_regs == NULL) {
 		dev_err(dev, "register mapping failed.\n");
 		ret = -ENXIO;
-		goto fail_mixer_regs;
+		goto fail;
 	}
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "irq");
 	if (res == NULL) {
 		dev_err(dev, "get interrupt resource failed.\n");
 		ret = -ENXIO;
-		goto fail_vp_regs;
+		goto fail;
 	}
 
-	ret = request_irq(res->start, mixer_irq_handler, 0, "drm_mixer", ctx);
+	ret = devm_request_irq(&pdev->dev, res->start, mixer_irq_handler,
+							0, "drm_mixer", ctx);
 	if (ret) {
 		dev_err(dev, "request interrupt failed.\n");
-		goto fail_vp_regs;
+		goto fail;
 	}
 	mixer_res->irq = res->start;
 
 	return 0;
 
-fail_vp_regs:
-	iounmap(mixer_res->vp_regs);
-
-fail_mixer_regs:
-	iounmap(mixer_res->mixer_regs);
-
 fail:
 	if (!IS_ERR_OR_NULL(mixer_res->sclk_dac))
 		clk_put(mixer_res->sclk_dac);
@@ -1013,16 +1010,6 @@ fail:
 	return ret;
 }
 
-static void mixer_resources_cleanup(struct mixer_context *ctx)
-{
-	struct mixer_resources *res = &ctx->mixer_res;
-
-	free_irq(res->irq, ctx);
-
-	iounmap(res->vp_regs);
-	iounmap(res->mixer_regs);
-}
-
 static int __devinit mixer_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -1032,16 +1019,16 @@ static int __devinit mixer_probe(struct platform_device *pdev)
 
 	dev_info(dev, "probe start\n");
 
-	drm_hdmi_ctx = kzalloc(sizeof(*drm_hdmi_ctx), GFP_KERNEL);
+	drm_hdmi_ctx = devm_kzalloc(&pdev->dev, sizeof(*drm_hdmi_ctx),
+								GFP_KERNEL);
 	if (!drm_hdmi_ctx) {
 		DRM_ERROR("failed to allocate common hdmi context.\n");
 		return -ENOMEM;
 	}
 
-	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
 	if (!ctx) {
 		DRM_ERROR("failed to alloc mixer context.\n");
-		kfree(drm_hdmi_ctx);
 		return -ENOMEM;
 	}
 
@@ -1072,17 +1059,10 @@ fail:
 
 static int mixer_remove(struct platform_device *pdev)
 {
-	struct device *dev = &pdev->dev;
-	struct exynos_drm_hdmi_context *drm_hdmi_ctx =
-					platform_get_drvdata(pdev);
-	struct mixer_context *ctx = drm_hdmi_ctx->ctx;
-
-	dev_info(dev, "remove successful\n");
+	dev_info(&pdev->dev, "remove successful\n");
 
 	pm_runtime_disable(&pdev->dev);
 
-	mixer_resources_cleanup(ctx);
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
index 9764045428c..b7e7b49d8f6 100644
--- a/drivers/gpu/drm/gma500/cdv_device.c
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -78,21 +78,6 @@ static int cdv_backlight_combination_mode(struct drm_device *dev)
 	return REG_READ(BLC_PWM_CTL2) & PWM_LEGACY_MODE;
 }
 
-static int cdv_get_brightness(struct backlight_device *bd)
-{
-	struct drm_device *dev = bl_get_data(bd);
-	u32 val = REG_READ(BLC_PWM_CTL) & BACKLIGHT_DUTY_CYCLE_MASK;
-
-	if (cdv_backlight_combination_mode(dev)) {
-		u8 lbpc;
-
-		val &= ~1;
-		pci_read_config_byte(dev->pdev, 0xF4, &lbpc);
-		val *= lbpc;
-	}
-	return val;
-}
-
 static u32 cdv_get_max_backlight(struct drm_device *dev)
 {
 	u32 max = REG_READ(BLC_PWM_CTL);
@@ -110,6 +95,22 @@ static u32 cdv_get_max_backlight(struct drm_device *dev)
 	return max;
 }
 
+static int cdv_get_brightness(struct backlight_device *bd)
+{
+	struct drm_device *dev = bl_get_data(bd);
+	u32 val = REG_READ(BLC_PWM_CTL) & BACKLIGHT_DUTY_CYCLE_MASK;
+
+	if (cdv_backlight_combination_mode(dev)) {
+		u8 lbpc;
+
+		val &= ~1;
+		pci_read_config_byte(dev->pdev, 0xF4, &lbpc);
+		val *= lbpc;
+	}
+	return (val * 100)/cdv_get_max_backlight(dev);
+
+}
+
 static int cdv_set_brightness(struct backlight_device *bd)
 {
 	struct drm_device *dev = bl_get_data(bd);
@@ -120,6 +121,9 @@ static int cdv_set_brightness(struct backlight_device *bd)
 	if (level < 1)
 		level = 1;
 
+	level *= cdv_get_max_backlight(dev);
+	level /= 100;
+
 	if (cdv_backlight_combination_mode(dev)) {
 		u32 max = cdv_get_max_backlight(dev);
 		u8 lbpc;
@@ -157,7 +161,6 @@ static int cdv_backlight_init(struct drm_device *dev)
 
 	cdv_backlight_device->props.brightness =
 			cdv_get_brightness(cdv_backlight_device);
-	cdv_backlight_device->props.max_brightness = cdv_get_max_backlight(dev);
 	backlight_update_status(cdv_backlight_device);
 	dev_priv->backlight_device = cdv_backlight_device;
 	return 0;
diff --git a/drivers/gpu/drm/gma500/cdv_intel_crt.c b/drivers/gpu/drm/gma500/cdv_intel_crt.c
index 18742201860..8c175345d85 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_crt.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_crt.c
@@ -82,7 +82,7 @@ static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
 }
 
 static bool cdv_intel_crt_mode_fixup(struct drm_encoder *encoder,
-				 struct drm_display_mode *mode,
+				 const struct drm_display_mode *mode,
 				 struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/cdv_intel_display.c b/drivers/gpu/drm/gma500/cdv_intel_display.c
index c3e9a0f701d..a68509ba22a 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_display.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_display.c
@@ -913,7 +913,7 @@ static void cdv_intel_crtc_commit(struct drm_crtc *crtc)
 }
 
 static bool cdv_intel_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
index 88b59d4a7b7..a86f87b9ddd 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
@@ -90,7 +90,7 @@ static void cdv_hdmi_mode_set(struct drm_encoder *encoder,
 }
 
 static bool cdv_hdmi_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/cdv_intel_lvds.c b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
index ff5b58eb878..c7f9468b74b 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
@@ -270,7 +270,7 @@ static int cdv_intel_lvds_mode_valid(struct drm_connector *connector,
 }
 
 static bool cdv_intel_lvds_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
diff --git a/drivers/gpu/drm/gma500/intel_bios.c b/drivers/gpu/drm/gma500/intel_bios.c
index 973d7f6d66b..8d7caf0f363 100644
--- a/drivers/gpu/drm/gma500/intel_bios.c
+++ b/drivers/gpu/drm/gma500/intel_bios.c
@@ -427,7 +427,7 @@ parse_device_mapping(struct drm_psb_private *dev_priv,
  *
  * Returns 0 on success, nonzero on failure.
  */
-bool psb_intel_init_bios(struct drm_device *dev)
+int psb_intel_init_bios(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct pci_dev *pdev = dev->pdev;
diff --git a/drivers/gpu/drm/gma500/intel_bios.h b/drivers/gpu/drm/gma500/intel_bios.h
index 0a738663eb5..2e95523b84b 100644
--- a/drivers/gpu/drm/gma500/intel_bios.h
+++ b/drivers/gpu/drm/gma500/intel_bios.h
@@ -431,7 +431,7 @@ struct bdb_driver_features {
 	u8 custom_vbt_version;
 } __attribute__((packed));
 
-extern bool psb_intel_init_bios(struct drm_device *dev);
+extern int psb_intel_init_bios(struct drm_device *dev);
 extern void psb_intel_destroy_bios(struct drm_device *dev);
 
 /*
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
index b34ff097b97..d4813e03f5e 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
@@ -684,7 +684,7 @@ void mdfld_dsi_dpi_dpms(struct drm_encoder *encoder, int mode)
 }
 
 bool mdfld_dsi_dpi_mode_fixup(struct drm_encoder *encoder,
-				     struct drm_display_mode *mode,
+				     const struct drm_display_mode *mode,
 				     struct drm_display_mode *adjusted_mode)
 {
 	struct mdfld_dsi_encoder *dsi_encoder = mdfld_dsi_encoder(encoder);
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.h b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.h
index 6f762478b95..2b40663e169 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.h
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.h
@@ -65,7 +65,7 @@ extern struct mdfld_dsi_encoder *mdfld_dsi_dpi_init(struct drm_device *dev,
 /* MDFLD DPI helper functions */
 extern void mdfld_dsi_dpi_dpms(struct drm_encoder *encoder, int mode);
 extern bool mdfld_dsi_dpi_mode_fixup(struct drm_encoder *encoder,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode);
 extern void mdfld_dsi_dpi_prepare(struct drm_encoder *encoder);
 extern void mdfld_dsi_dpi_commit(struct drm_encoder *encoder);
diff --git a/drivers/gpu/drm/gma500/mdfld_intel_display.c b/drivers/gpu/drm/gma500/mdfld_intel_display.c
index 3f3cd619c79..dec6a9aea3c 100644
--- a/drivers/gpu/drm/gma500/mdfld_intel_display.c
+++ b/drivers/gpu/drm/gma500/mdfld_intel_display.c
@@ -117,7 +117,7 @@ static void psb_intel_crtc_commit(struct drm_crtc *crtc)
 }
 
 static bool psb_intel_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/oaktrail_crtc.c b/drivers/gpu/drm/gma500/oaktrail_crtc.c
index f821c835ca9..cdafd2acc72 100644
--- a/drivers/gpu/drm/gma500/oaktrail_crtc.c
+++ b/drivers/gpu/drm/gma500/oaktrail_crtc.c
@@ -487,7 +487,7 @@ oaktrail_crtc_mode_set_exit:
 }
 
 static bool oaktrail_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi.c b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
index c10899c953b..2eb3dc4e9c9 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
@@ -191,7 +191,7 @@ static int oaktrail_hdmi_mode_valid(struct drm_connector *connector,
 }
 
 static bool oaktrail_hdmi_mode_fixup(struct drm_encoder *encoder,
-				 struct drm_display_mode *mode,
+				 const struct drm_display_mode *mode,
 				 struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/opregion.c b/drivers/gpu/drm/gma500/opregion.c
index 4f186eca3a3..c430bd42468 100644
--- a/drivers/gpu/drm/gma500/opregion.c
+++ b/drivers/gpu/drm/gma500/opregion.c
@@ -144,6 +144,8 @@ struct opregion_asle {
 
 #define ASLE_CBLV_VALID         (1<<31)
 
+static struct psb_intel_opregion *system_opregion;
+
 static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
@@ -205,7 +207,7 @@ void psb_intel_opregion_enable_asle(struct drm_device *dev)
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct opregion_asle *asle = dev_priv->opregion.asle;
 
-	if (asle) {
+	if (asle && system_opregion ) {
 		/* Don't do this on Medfield or other non PC like devices, they
 		   use the bit for something different altogether */
 		psb_enable_pipestat(dev_priv, 0, PIPE_LEGACY_BLC_EVENT_ENABLE);
@@ -221,7 +223,6 @@ void psb_intel_opregion_enable_asle(struct drm_device *dev)
 #define ACPI_EV_LID            (1<<1)
 #define ACPI_EV_DOCK           (1<<2)
 
-static struct psb_intel_opregion *system_opregion;
 
 static int psb_intel_opregion_video_event(struct notifier_block *nb,
 					  unsigned long val, void *data)
@@ -266,9 +267,6 @@ void psb_intel_opregion_init(struct drm_device *dev)
 		system_opregion = opregion;
 		register_acpi_notifier(&psb_intel_opregion_notifier);
 	}
-
-	if (opregion->asle)
-		psb_intel_opregion_enable_asle(dev);
 }
 
 void psb_intel_opregion_fini(struct drm_device *dev)
diff --git a/drivers/gpu/drm/gma500/opregion.h b/drivers/gpu/drm/gma500/opregion.h
index 72dc6b92126..4a90f8b0e16 100644
--- a/drivers/gpu/drm/gma500/opregion.h
+++ b/drivers/gpu/drm/gma500/opregion.h
@@ -27,6 +27,7 @@ extern void psb_intel_opregion_asle_intr(struct drm_device *dev);
 extern void psb_intel_opregion_init(struct drm_device *dev);
 extern void psb_intel_opregion_fini(struct drm_device *dev);
 extern int psb_intel_opregion_setup(struct drm_device *dev);
+extern void psb_intel_opregion_enable_asle(struct drm_device *dev);
 
 #else
 
@@ -46,4 +47,8 @@ extern inline int psb_intel_opregion_setup(struct drm_device *dev)
 {
 	return 0;
 }
+
+extern inline void psb_intel_opregion_enable_asle(struct drm_device *dev)
+{
+}
 #endif
diff --git a/drivers/gpu/drm/gma500/psb_device.c b/drivers/gpu/drm/gma500/psb_device.c
index eff039bf92d..5971bc82b76 100644
--- a/drivers/gpu/drm/gma500/psb_device.c
+++ b/drivers/gpu/drm/gma500/psb_device.c
@@ -144,6 +144,10 @@ static int psb_backlight_init(struct drm_device *dev)
 	psb_backlight_device->props.max_brightness = 100;
 	backlight_update_status(psb_backlight_device);
 	dev_priv->backlight_device = psb_backlight_device;
+
+	/* This must occur after the backlight is properly initialised */
+	psb_lid_timer_init(dev_priv);
+
 	return 0;
 }
 
@@ -354,13 +358,6 @@ static int psb_chip_setup(struct drm_device *dev)
 	return 0;
 }
 
-/* Not exactly an erratum more an irritation */
-static void psb_chip_errata(struct drm_device *dev)
-{
-	struct drm_psb_private *dev_priv = dev->dev_private;
-	psb_lid_timer_init(dev_priv);
-}
-
 static void psb_chip_teardown(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
@@ -379,7 +376,6 @@ const struct psb_ops psb_chip_ops = {
 	.sgx_offset = PSB_SGX_OFFSET,
 	.chip_setup = psb_chip_setup,
 	.chip_teardown = psb_chip_teardown,
-	.errata = psb_chip_errata,
 
 	.crtc_helper = &psb_intel_helper_funcs,
 	.crtc_funcs = &psb_intel_crtc_funcs,
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
index caba6e08693..0c473743853 100644
--- a/drivers/gpu/drm/gma500/psb_drv.c
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -374,6 +374,7 @@ static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
 
 	if (ret)
 		return ret;
+	psb_intel_opregion_enable_asle(dev);
 #if 0
 	/*enable runtime pm at last*/
 	pm_runtime_enable(&dev->pdev->dev);
@@ -632,7 +633,6 @@ static struct drm_driver driver = {
 	.open = psb_driver_open,
 	.preclose = psb_driver_preclose,
 	.postclose = psb_driver_close,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 
 	.gem_init_object = psb_gem_init_object,
 	.gem_free_object = psb_gem_free_object,
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
index 36c3c99612f..30dc22a7156 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.c
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -543,7 +543,7 @@ void psb_intel_encoder_destroy(struct drm_encoder *encoder)
 }
 
 static bool psb_intel_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
index 2515f83248c..ebe1a28f60e 100644
--- a/drivers/gpu/drm/gma500/psb_intel_drv.h
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -268,7 +268,7 @@ extern struct drm_framebuffer *psb_intel_framebuffer_create(struct drm_device
 							*mode_cmd,
 							void *mm_private);
 extern bool psb_intel_lvds_mode_fixup(struct drm_encoder *encoder,
-				      struct drm_display_mode *mode,
+				      const struct drm_display_mode *mode,
 				      struct drm_display_mode *adjusted_mode);
 extern int psb_intel_lvds_mode_valid(struct drm_connector *connector,
 				     struct drm_display_mode *mode);
diff --git a/drivers/gpu/drm/gma500/psb_intel_lvds.c b/drivers/gpu/drm/gma500/psb_intel_lvds.c
index c83f5b5d105..37adc9edf97 100644
--- a/drivers/gpu/drm/gma500/psb_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/psb_intel_lvds.c
@@ -375,7 +375,7 @@ int psb_intel_lvds_mode_valid(struct drm_connector *connector,
 }
 
 bool psb_intel_lvds_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
index d39b15be764..0466c7b985f 100644
--- a/drivers/gpu/drm/gma500/psb_intel_sdvo.c
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -901,7 +901,7 @@ static bool psb_intel_sdvo_set_tv_format(struct psb_intel_sdvo *psb_intel_sdvo)
 
 static bool
 psb_intel_sdvo_set_output_timings_from_mode(struct psb_intel_sdvo *psb_intel_sdvo,
-					struct drm_display_mode *mode)
+					const struct drm_display_mode *mode)
 {
 	struct psb_intel_sdvo_dtd output_dtd;
 
@@ -918,7 +918,7 @@ psb_intel_sdvo_set_output_timings_from_mode(struct psb_intel_sdvo *psb_intel_sdv
 
 static bool
 psb_intel_sdvo_set_input_timings_for_mode(struct psb_intel_sdvo *psb_intel_sdvo,
-					struct drm_display_mode *mode,
+					const struct drm_display_mode *mode,
 					struct drm_display_mode *adjusted_mode)
 {
 	/* Reset the input timing to the screen. Assume always input 0. */
@@ -942,7 +942,7 @@ psb_intel_sdvo_set_input_timings_for_mode(struct psb_intel_sdvo *psb_intel_sdvo,
 }
 
 static bool psb_intel_sdvo_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct psb_intel_sdvo *psb_intel_sdvo = to_psb_intel_sdvo(encoder);
diff --git a/drivers/gpu/drm/i2c/ch7006_drv.c b/drivers/gpu/drm/i2c/ch7006_drv.c
index d3f2e878501..36d952280c5 100644
--- a/drivers/gpu/drm/i2c/ch7006_drv.c
+++ b/drivers/gpu/drm/i2c/ch7006_drv.c
@@ -88,7 +88,7 @@ static void ch7006_encoder_restore(struct drm_encoder *encoder)
 }
 
 static bool ch7006_encoder_mode_fixup(struct drm_encoder *encoder,
-				      struct drm_display_mode *mode,
+				      const struct drm_display_mode *mode,
 				      struct drm_display_mode *adjusted_mode)
 {
 	struct ch7006_priv *priv = to_ch7006_priv(encoder);
diff --git a/drivers/gpu/drm/i2c/ch7006_mode.c b/drivers/gpu/drm/i2c/ch7006_mode.c
index c860f24a5af..9b83574141a 100644
--- a/drivers/gpu/drm/i2c/ch7006_mode.c
+++ b/drivers/gpu/drm/i2c/ch7006_mode.c
@@ -172,7 +172,7 @@ struct ch7006_mode ch7006_modes[] = {
 };
 
 struct ch7006_mode *ch7006_lookup_mode(struct drm_encoder *encoder,
-				       struct drm_display_mode *drm_mode)
+				       const struct drm_display_mode *drm_mode)
 {
 	struct ch7006_priv *priv = to_ch7006_priv(encoder);
 	struct ch7006_mode *mode;
diff --git a/drivers/gpu/drm/i2c/ch7006_priv.h b/drivers/gpu/drm/i2c/ch7006_priv.h
index 17667b7d57e..09599f4c0c9 100644
--- a/drivers/gpu/drm/i2c/ch7006_priv.h
+++ b/drivers/gpu/drm/i2c/ch7006_priv.h
@@ -111,7 +111,7 @@ extern struct ch7006_tv_norm_info ch7006_tv_norms[];
 extern struct ch7006_mode ch7006_modes[];
 
 struct ch7006_mode *ch7006_lookup_mode(struct drm_encoder *encoder,
-				       struct drm_display_mode *drm_mode);
+				       const struct drm_display_mode *drm_mode);
 
 void ch7006_setup_levels(struct drm_encoder *encoder);
 void ch7006_setup_subcarrier(struct drm_encoder *encoder);
diff --git a/drivers/gpu/drm/i2c/sil164_drv.c b/drivers/gpu/drm/i2c/sil164_drv.c
index b7d45ab4ba6..30b8ae5e5c4 100644
--- a/drivers/gpu/drm/i2c/sil164_drv.c
+++ b/drivers/gpu/drm/i2c/sil164_drv.c
@@ -254,7 +254,7 @@ sil164_encoder_restore(struct drm_encoder *encoder)
 
 static bool
 sil164_encoder_mode_fixup(struct drm_encoder *encoder,
-			  struct drm_display_mode *mode,
+			  const struct drm_display_mode *mode,
 			  struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/i810/i810_dma.c b/drivers/gpu/drm/i810/i810_dma.c
index fa9439159eb..57d892eaaa6 100644
--- a/drivers/gpu/drm/i810/i810_dma.c
+++ b/drivers/gpu/drm/i810/i810_dma.c
@@ -881,7 +881,7 @@ static int i810_flush_queue(struct drm_device *dev)
 }
 
 /* Must be called with the lock held */
-static void i810_reclaim_buffers(struct drm_device *dev,
+void i810_driver_reclaim_buffers(struct drm_device *dev,
 				 struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
@@ -1220,12 +1220,17 @@ void i810_driver_preclose(struct drm_device *dev, struct drm_file *file_priv)
 		if (dev_priv->page_flipping)
 			i810_do_cleanup_pageflip(dev);
 	}
-}
 
-void i810_driver_reclaim_buffers_locked(struct drm_device *dev,
-					struct drm_file *file_priv)
-{
-	i810_reclaim_buffers(dev, file_priv);
+	if (file_priv->master && file_priv->master->lock.hw_lock) {
+		drm_idlelock_take(&file_priv->master->lock);
+		i810_driver_reclaim_buffers(dev, file_priv);
+		drm_idlelock_release(&file_priv->master->lock);
+	} else {
+		/* master disappeared, clean up stuff anyway and hope nothing
+		 * goes wrong */
+		i810_driver_reclaim_buffers(dev, file_priv);
+	}
+
 }
 
 int i810_driver_dma_quiescent(struct drm_device *dev)
diff --git a/drivers/gpu/drm/i810/i810_drv.c b/drivers/gpu/drm/i810/i810_drv.c
index ec12f7dc717..f9924ad04d0 100644
--- a/drivers/gpu/drm/i810/i810_drv.c
+++ b/drivers/gpu/drm/i810/i810_drv.c
@@ -57,13 +57,12 @@ static const struct file_operations i810_driver_fops = {
 static struct drm_driver driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_REQUIRE_AGP | DRIVER_USE_MTRR |
-	    DRIVER_HAVE_DMA | DRIVER_DMA_QUEUE,
+	    DRIVER_HAVE_DMA,
 	.dev_priv_size = sizeof(drm_i810_buf_priv_t),
 	.load = i810_driver_load,
 	.lastclose = i810_driver_lastclose,
 	.preclose = i810_driver_preclose,
 	.device_is_agp = i810_driver_device_is_agp,
-	.reclaim_buffers_locked = i810_driver_reclaim_buffers_locked,
 	.dma_quiescent = i810_driver_dma_quiescent,
 	.ioctls = i810_ioctls,
 	.fops = &i810_driver_fops,
diff --git a/drivers/gpu/drm/i810/i810_drv.h b/drivers/gpu/drm/i810/i810_drv.h
index c9339f48179..6e0acad9e0f 100644
--- a/drivers/gpu/drm/i810/i810_drv.h
+++ b/drivers/gpu/drm/i810/i810_drv.h
@@ -116,14 +116,12 @@ typedef struct drm_i810_private {
 
 				/* i810_dma.c */
 extern int i810_driver_dma_quiescent(struct drm_device *dev);
-extern void i810_driver_reclaim_buffers_locked(struct drm_device *dev,
-					       struct drm_file *file_priv);
+void i810_driver_reclaim_buffers(struct drm_device *dev,
+			         struct drm_file *file_priv);
 extern int i810_driver_load(struct drm_device *, unsigned long flags);
 extern void i810_driver_lastclose(struct drm_device *dev);
 extern void i810_driver_preclose(struct drm_device *dev,
 				 struct drm_file *file_priv);
-extern void i810_driver_reclaim_buffers_locked(struct drm_device *dev,
-					       struct drm_file *file_priv);
 extern int i810_driver_device_is_agp(struct drm_device *dev);
 
 extern long i810_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index 2e9268da58d..b0bacdba6d7 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -7,6 +7,7 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o \
 	  i915_debugfs.o \
           i915_suspend.o \
 	  i915_gem.o \
+	  i915_gem_context.o \
 	  i915_gem_debug.o \
 	  i915_gem_evict.o \
 	  i915_gem_execbuffer.o \
diff --git a/drivers/gpu/drm/i915/dvo.h b/drivers/gpu/drm/i915/dvo.h
index 8c2ad014c47..58914691a77 100644
--- a/drivers/gpu/drm/i915/dvo.h
+++ b/drivers/gpu/drm/i915/dvo.h
@@ -86,7 +86,7 @@ struct intel_dvo_dev_ops {
 	 * buses with clock limitations.
 	 */
 	bool (*mode_fixup)(struct intel_dvo_device *dvo,
-			   struct drm_display_mode *mode,
+			   const struct drm_display_mode *mode,
 			   struct drm_display_mode *adjusted_mode);
 
 	/*
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 5363e9c66c2..359f6e8b9b0 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -676,6 +676,7 @@ static void i915_ring_error_state(struct seq_file *m,
 	seq_printf(m, "  INSTPM: 0x%08x\n", error->instpm[ring]);
 	seq_printf(m, "  FADDR: 0x%08x\n", error->faddr[ring]);
 	if (INTEL_INFO(dev)->gen >= 6) {
+		seq_printf(m, "  RC PSMI: 0x%08x\n", error->rc_psmi[ring]);
 		seq_printf(m, "  FAULT_REG: 0x%08x\n", error->fault_reg[ring]);
 		seq_printf(m, "  SYNC_0: 0x%08x\n",
 			   error->semaphore_mboxes[ring][0]);
@@ -713,6 +714,7 @@ static int i915_error_state(struct seq_file *m, void *unused)
 	seq_printf(m, "EIR: 0x%08x\n", error->eir);
 	seq_printf(m, "IER: 0x%08x\n", error->ier);
 	seq_printf(m, "PGTBL_ER: 0x%08x\n", error->pgtbl_er);
+	seq_printf(m, "CCID: 0x%08x\n", error->ccid);
 
 	for (i = 0; i < dev_priv->num_fence_regs; i++)
 		seq_printf(m, "  fence[%d] = %08llx\n", i, error->fence[i]);
@@ -1765,6 +1767,64 @@ static const struct file_operations i915_max_freq_fops = {
 };
 
 static ssize_t
+i915_min_freq_read(struct file *filp, char __user *ubuf, size_t max,
+		   loff_t *ppos)
+{
+	struct drm_device *dev = filp->private_data;
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	char buf[80];
+	int len;
+
+	len = snprintf(buf, sizeof(buf),
+		       "min freq: %d\n", dev_priv->min_delay * 50);
+
+	if (len > sizeof(buf))
+		len = sizeof(buf);
+
+	return simple_read_from_buffer(ubuf, max, ppos, buf, len);
+}
+
+static ssize_t
+i915_min_freq_write(struct file *filp, const char __user *ubuf, size_t cnt,
+		    loff_t *ppos)
+{
+	struct drm_device *dev = filp->private_data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	char buf[20];
+	int val = 1;
+
+	if (cnt > 0) {
+		if (cnt > sizeof(buf) - 1)
+			return -EINVAL;
+
+		if (copy_from_user(buf, ubuf, cnt))
+			return -EFAULT;
+		buf[cnt] = 0;
+
+		val = simple_strtoul(buf, NULL, 0);
+	}
+
+	DRM_DEBUG_DRIVER("Manually setting min freq to %d\n", val);
+
+	/*
+	 * Turbo will still be enabled, but won't go below the set value.
+	 */
+	dev_priv->min_delay = val / 50;
+
+	gen6_set_rps(dev, val / 50);
+
+	return cnt;
+}
+
+static const struct file_operations i915_min_freq_fops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = i915_min_freq_read,
+	.write = i915_min_freq_write,
+	.llseek = default_llseek,
+};
+
+static ssize_t
 i915_cache_sharing_read(struct file *filp,
 		   char __user *ubuf,
 		   size_t max,
@@ -1997,6 +2057,12 @@ int i915_debugfs_init(struct drm_minor *minor)
 		return ret;
 
 	ret = i915_debugfs_create(minor->debugfs_root, minor,
+				  "i915_min_freq",
+				  &i915_min_freq_fops);
+	if (ret)
+		return ret;
+
+	ret = i915_debugfs_create(minor->debugfs_root, minor,
 				  "i915_cache_sharing",
 				  &i915_cache_sharing_fops);
 	if (ret)
@@ -2028,6 +2094,8 @@ void i915_debugfs_cleanup(struct drm_minor *minor)
 				 1, minor);
 	drm_debugfs_remove_files((struct drm_info_list *) &i915_max_freq_fops,
 				 1, minor);
+	drm_debugfs_remove_files((struct drm_info_list *) &i915_min_freq_fops,
+				 1, minor);
 	drm_debugfs_remove_files((struct drm_info_list *) &i915_cache_sharing_fops,
 				 1, minor);
 	drm_debugfs_remove_files((struct drm_info_list *) &i915_ring_stop_fops,
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 36822b924eb..9cf7dfe022b 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1006,6 +1006,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_HAS_ALIASING_PPGTT:
 		value = dev_priv->mm.aliasing_ppgtt ? 1 : 0;
 		break;
+	case I915_PARAM_HAS_WAIT_TIMEOUT:
+		value = 1;
+		break;
 	default:
 		DRM_DEBUG_DRIVER("Unknown parameter %d\n",
 				 param->param);
@@ -1082,8 +1085,8 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 
 	ring->status_page.gfx_addr = hws->addr & (0x1ffff<<12);
 
-	dev_priv->dri1.gfx_hws_cpu_addr = ioremap_wc(dev->agp->base + hws->addr,
-						     4096);
+	dev_priv->dri1.gfx_hws_cpu_addr =
+		ioremap_wc(dev_priv->mm.gtt_base_addr + hws->addr, 4096);
 	if (dev_priv->dri1.gfx_hws_cpu_addr == NULL) {
 		i915_dma_cleanup(dev);
 		ring->status_page.gfx_addr = 0;
@@ -1411,7 +1414,7 @@ static void i915_kick_out_firmware_fb(struct drm_i915_private *dev_priv)
 	if (!ap)
 		return;
 
-	ap->ranges[0].base = dev_priv->dev->agp->base;
+	ap->ranges[0].base = dev_priv->mm.gtt->gma_bus_addr;
 	ap->ranges[0].size =
 		dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
 	primary =
@@ -1467,11 +1470,18 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto free_priv;
 	}
 
+	ret = intel_gmch_probe(dev_priv->bridge_dev, dev->pdev, NULL);
+	if (!ret) {
+		DRM_ERROR("failed to set up gmch\n");
+		ret = -EIO;
+		goto put_bridge;
+	}
+
 	dev_priv->mm.gtt = intel_gtt_get();
 	if (!dev_priv->mm.gtt) {
 		DRM_ERROR("Failed to initialize GTT\n");
 		ret = -ENODEV;
-		goto put_bridge;
+		goto put_gmch;
 	}
 
 	i915_kick_out_firmware_fb(dev_priv);
@@ -1498,19 +1508,22 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	if (!dev_priv->regs) {
 		DRM_ERROR("failed to map registers\n");
 		ret = -EIO;
-		goto put_bridge;
+		goto put_gmch;
 	}
 
 	aperture_size = dev_priv->mm.gtt->gtt_mappable_entries << PAGE_SHIFT;
+	dev_priv->mm.gtt_base_addr = dev_priv->mm.gtt->gma_bus_addr;
 
 	dev_priv->mm.gtt_mapping =
-		io_mapping_create_wc(dev->agp->base, aperture_size);
+		io_mapping_create_wc(dev_priv->mm.gtt_base_addr,
+				     aperture_size);
 	if (dev_priv->mm.gtt_mapping == NULL) {
 		ret = -EIO;
 		goto out_rmmap;
 	}
 
-	i915_mtrr_setup(dev_priv, dev->agp->base, aperture_size);
+	i915_mtrr_setup(dev_priv, dev_priv->mm.gtt_base_addr,
+			aperture_size);
 
 	/* The i915 workqueue is primarily used for batched retirement of
 	 * requests (and thus managing bo) once the task has been completed
@@ -1534,7 +1547,11 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 		goto out_mtrrfree;
 	}
 
+	/* This must be called before any calls to HAS_PCH_* */
+	intel_detect_pch(dev);
+
 	intel_irq_init(dev);
+	intel_gt_init(dev);
 
 	/* Try to make sure MCHBAR is enabled before poking at it */
 	intel_setup_mchbar(dev);
@@ -1567,7 +1584,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	if (!IS_I945G(dev) && !IS_I945GM(dev))
 		pci_enable_msi(dev->pdev);
 
-	spin_lock_init(&dev_priv->gt_lock);
 	spin_lock_init(&dev_priv->irq_lock);
 	spin_lock_init(&dev_priv->error_lock);
 	spin_lock_init(&dev_priv->rps_lock);
@@ -1586,8 +1602,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	/* Start out suspended */
 	dev_priv->mm.suspended = 1;
 
-	intel_detect_pch(dev);
-
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		ret = i915_load_modeset_init(dev);
 		if (ret < 0) {
@@ -1622,13 +1636,16 @@ out_gem_unload:
 	destroy_workqueue(dev_priv->wq);
 out_mtrrfree:
 	if (dev_priv->mm.gtt_mtrr >= 0) {
-		mtrr_del(dev_priv->mm.gtt_mtrr, dev->agp->base,
-			 dev->agp->agp_info.aper_size * 1024 * 1024);
+		mtrr_del(dev_priv->mm.gtt_mtrr,
+			 dev_priv->mm.gtt_base_addr,
+			 aperture_size);
 		dev_priv->mm.gtt_mtrr = -1;
 	}
 	io_mapping_free(dev_priv->mm.gtt_mapping);
 out_rmmap:
 	pci_iounmap(dev->pdev, dev_priv->regs);
+put_gmch:
+	intel_gmch_remove();
 put_bridge:
 	pci_dev_put(dev_priv->bridge_dev);
 free_priv:
@@ -1660,8 +1677,9 @@ int i915_driver_unload(struct drm_device *dev)
 
 	io_mapping_free(dev_priv->mm.gtt_mapping);
 	if (dev_priv->mm.gtt_mtrr >= 0) {
-		mtrr_del(dev_priv->mm.gtt_mtrr, dev->agp->base,
-			 dev->agp->agp_info.aper_size * 1024 * 1024);
+		mtrr_del(dev_priv->mm.gtt_mtrr,
+			 dev_priv->mm.gtt_base_addr,
+			 dev_priv->mm.gtt->gtt_mappable_entries * PAGE_SIZE);
 		dev_priv->mm.gtt_mtrr = -1;
 	}
 
@@ -1702,6 +1720,7 @@ int i915_driver_unload(struct drm_device *dev)
 		mutex_lock(&dev->struct_mutex);
 		i915_gem_free_all_phys_object(dev);
 		i915_gem_cleanup_ringbuffer(dev);
+		i915_gem_context_fini(dev);
 		mutex_unlock(&dev->struct_mutex);
 		i915_gem_cleanup_aliasing_ppgtt(dev);
 		i915_gem_cleanup_stolen(dev);
@@ -1741,6 +1760,8 @@ int i915_driver_open(struct drm_device *dev, struct drm_file *file)
 	spin_lock_init(&file_priv->mm.lock);
 	INIT_LIST_HEAD(&file_priv->mm.request_list);
 
+	idr_init(&file_priv->context_idr);
+
 	return 0;
 }
 
@@ -1760,7 +1781,13 @@ void i915_driver_lastclose(struct drm_device * dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
 
-	if (!dev_priv || drm_core_check_feature(dev, DRIVER_MODESET)) {
+	/* On gen6+ we refuse to init without kms enabled, but then the drm core
+	 * goes right around and calls lastclose. Check for this and don't clean
+	 * up anything. */
+	if (!dev_priv)
+		return;
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		intel_fb_restore_mode(dev);
 		vga_switcheroo_process_delayed_switch();
 		return;
@@ -1773,6 +1800,7 @@ void i915_driver_lastclose(struct drm_device * dev)
 
 void i915_driver_preclose(struct drm_device * dev, struct drm_file *file_priv)
 {
+	i915_gem_context_close(dev, file_priv);
 	i915_gem_release(dev, file_priv);
 }
 
@@ -1826,6 +1854,9 @@ struct drm_ioctl_desc i915_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(I915_OVERLAY_ATTRS, intel_overlay_attrs, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_SET_SPRITE_COLORKEY, intel_sprite_set_colorkey, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_GET_SPRITE_COLORKEY, intel_sprite_get_colorkey, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GEM_WAIT, i915_gem_wait_ioctl, DRM_AUTH|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GEM_CONTEXT_CREATE, i915_gem_context_create_ioctl, DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GEM_CONTEXT_DESTROY, i915_gem_context_destroy_ioctl, DRM_UNLOCKED),
 };
 
 int i915_max_ioctl = DRM_ARRAY_SIZE(i915_ioctls);
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 9fe9ebe52a7..ed22612bc84 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -32,6 +32,7 @@
 #include "drm.h"
 #include "i915_drm.h"
 #include "i915_drv.h"
+#include "i915_trace.h"
 #include "intel_drv.h"
 
 #include <linux/console.h>
@@ -215,7 +216,6 @@ static const struct intel_device_info intel_ironlake_d_info = {
 	.gen = 5,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_bsd_ring = 1,
-	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_ironlake_m_info = {
@@ -223,7 +223,6 @@ static const struct intel_device_info intel_ironlake_m_info = {
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.has_fbc = 1,
 	.has_bsd_ring = 1,
-	.has_pch_split = 1,
 };
 
 static const struct intel_device_info intel_sandybridge_d_info = {
@@ -232,7 +231,6 @@ static const struct intel_device_info intel_sandybridge_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
-	.has_pch_split = 1,
 	.has_force_wake = 1,
 };
 
@@ -243,7 +241,6 @@ static const struct intel_device_info intel_sandybridge_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
-	.has_pch_split = 1,
 	.has_force_wake = 1,
 };
 
@@ -253,7 +250,6 @@ static const struct intel_device_info intel_ivybridge_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
-	.has_pch_split = 1,
 	.has_force_wake = 1,
 };
 
@@ -264,7 +260,6 @@ static const struct intel_device_info intel_ivybridge_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
-	.has_pch_split = 1,
 	.has_force_wake = 1,
 };
 
@@ -292,7 +287,6 @@ static const struct intel_device_info intel_haswell_d_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
-	.has_pch_split = 1,
 	.has_force_wake = 1,
 };
 
@@ -302,7 +296,6 @@ static const struct intel_device_info intel_haswell_m_info = {
 	.has_bsd_ring = 1,
 	.has_blt_ring = 1,
 	.has_llc = 1,
-	.has_pch_split = 1,
 	.has_force_wake = 1,
 };
 
@@ -358,6 +351,9 @@ static const struct pci_device_id pciidlist[] = {		/* aka */
 	INTEL_VGA_DEVICE(0x0406, &intel_haswell_m_info), /* GT1 mobile */
 	INTEL_VGA_DEVICE(0x0416, &intel_haswell_m_info), /* GT2 mobile */
 	INTEL_VGA_DEVICE(0x0c16, &intel_haswell_d_info), /* SDV */
+	INTEL_VGA_DEVICE(0x0f30, &intel_valleyview_m_info),
+	INTEL_VGA_DEVICE(0x0157, &intel_valleyview_m_info),
+	INTEL_VGA_DEVICE(0x0155, &intel_valleyview_d_info),
 	{0, 0, 0}
 };
 
@@ -429,135 +425,6 @@ bool i915_semaphore_is_enabled(struct drm_device *dev)
 	return 1;
 }
 
-void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
-{
-	int count;
-
-	count = 0;
-	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_ACK) & 1))
-		udelay(10);
-
-	I915_WRITE_NOTRACE(FORCEWAKE, 1);
-	POSTING_READ(FORCEWAKE);
-
-	count = 0;
-	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_ACK) & 1) == 0)
-		udelay(10);
-}
-
-void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv)
-{
-	int count;
-
-	count = 0;
-	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_MT_ACK) & 1))
-		udelay(10);
-
-	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_ENABLE(1));
-	POSTING_READ(FORCEWAKE_MT);
-
-	count = 0;
-	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_MT_ACK) & 1) == 0)
-		udelay(10);
-}
-
-/*
- * Generally this is called implicitly by the register read function. However,
- * if some sequence requires the GT to not power down then this function should
- * be called at the beginning of the sequence followed by a call to
- * gen6_gt_force_wake_put() at the end of the sequence.
- */
-void gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
-{
-	unsigned long irqflags;
-
-	spin_lock_irqsave(&dev_priv->gt_lock, irqflags);
-	if (dev_priv->forcewake_count++ == 0)
-		dev_priv->display.force_wake_get(dev_priv);
-	spin_unlock_irqrestore(&dev_priv->gt_lock, irqflags);
-}
-
-static void gen6_gt_check_fifodbg(struct drm_i915_private *dev_priv)
-{
-	u32 gtfifodbg;
-	gtfifodbg = I915_READ_NOTRACE(GTFIFODBG);
-	if (WARN(gtfifodbg & GT_FIFO_CPU_ERROR_MASK,
-	     "MMIO read or write has been dropped %x\n", gtfifodbg))
-		I915_WRITE_NOTRACE(GTFIFODBG, GT_FIFO_CPU_ERROR_MASK);
-}
-
-void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
-{
-	I915_WRITE_NOTRACE(FORCEWAKE, 0);
-	/* The below doubles as a POSTING_READ */
-	gen6_gt_check_fifodbg(dev_priv);
-}
-
-void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv)
-{
-	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_DISABLE(1));
-	/* The below doubles as a POSTING_READ */
-	gen6_gt_check_fifodbg(dev_priv);
-}
-
-/*
- * see gen6_gt_force_wake_get()
- */
-void gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
-{
-	unsigned long irqflags;
-
-	spin_lock_irqsave(&dev_priv->gt_lock, irqflags);
-	if (--dev_priv->forcewake_count == 0)
-		dev_priv->display.force_wake_put(dev_priv);
-	spin_unlock_irqrestore(&dev_priv->gt_lock, irqflags);
-}
-
-int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
-{
-	int ret = 0;
-
-	if (dev_priv->gt_fifo_count < GT_FIFO_NUM_RESERVED_ENTRIES) {
-		int loop = 500;
-		u32 fifo = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
-		while (fifo <= GT_FIFO_NUM_RESERVED_ENTRIES && loop--) {
-			udelay(10);
-			fifo = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
-		}
-		if (WARN_ON(loop < 0 && fifo <= GT_FIFO_NUM_RESERVED_ENTRIES))
-			++ret;
-		dev_priv->gt_fifo_count = fifo;
-	}
-	dev_priv->gt_fifo_count--;
-
-	return ret;
-}
-
-void vlv_force_wake_get(struct drm_i915_private *dev_priv)
-{
-	int count;
-
-	count = 0;
-
-	/* Already awake? */
-	if ((I915_READ(0x130094) & 0xa1) == 0xa1)
-		return;
-
-	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffffffff);
-	POSTING_READ(FORCEWAKE_VLV);
-
-	count = 0;
-	while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & 1) == 0)
-		udelay(10);
-}
-
-void vlv_force_wake_put(struct drm_i915_private *dev_priv)
-{
-	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffff0000);
-	/* FIXME: confirm VLV behavior with Punit folks */
-	POSTING_READ(FORCEWAKE_VLV);
-}
-
 static int i915_drm_freeze(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -637,7 +504,7 @@ static int i915_drm_thaw(struct drm_device *dev)
 
 	/* KMS EnterVT equivalent */
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
-		if (HAS_PCH_SPLIT(dev))
+		if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
 			ironlake_init_pch_refclk(dev);
 
 		mutex_lock(&dev->struct_mutex);
@@ -794,9 +661,9 @@ static int gen6_do_reset(struct drm_device *dev)
 
 	/* If reset with a user forcewake, try to restore, otherwise turn it off */
 	if (dev_priv->forcewake_count)
-		dev_priv->display.force_wake_get(dev_priv);
+		dev_priv->gt.force_wake_get(dev_priv);
 	else
-		dev_priv->display.force_wake_put(dev_priv);
+		dev_priv->gt.force_wake_put(dev_priv);
 
 	/* Restore fifo count */
 	dev_priv->gt_fifo_count = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
@@ -805,7 +672,7 @@ static int gen6_do_reset(struct drm_device *dev)
 	return ret;
 }
 
-static int intel_gpu_reset(struct drm_device *dev)
+int intel_gpu_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret = -ENODEV;
@@ -863,10 +730,7 @@ int i915_reset(struct drm_device *dev)
 	if (!i915_try_reset)
 		return 0;
 
-	if (!mutex_trylock(&dev->struct_mutex))
-		return -EBUSY;
-
-	dev_priv->stop_rings = 0;
+	mutex_lock(&dev->struct_mutex);
 
 	i915_gem_reset(dev);
 
@@ -909,12 +773,16 @@ int i915_reset(struct drm_device *dev)
 		for_each_ring(ring, dev_priv, i)
 			ring->init(ring);
 
+		i915_gem_context_init(dev);
 		i915_gem_init_ppgtt(dev);
 
-		mutex_unlock(&dev->struct_mutex);
+		/*
+		 * It would make sense to re-init all the other hw state, at
+		 * least the rps/rc6/emon init done within modeset_init_hw. For
+		 * some unknown reason, this blows up my ilk, so don't.
+		 */
 
-		if (drm_core_check_feature(dev, DRIVER_MODESET))
-			intel_modeset_init_hw(dev);
+		mutex_unlock(&dev->struct_mutex);
 
 		drm_irq_uninstall(dev);
 		drm_irq_install(dev);
@@ -925,10 +793,12 @@ int i915_reset(struct drm_device *dev)
 	return 0;
 }
 
-
 static int __devinit
 i915_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
+	struct intel_device_info *intel_info =
+		(struct intel_device_info *) ent->driver_data;
+
 	/* Only bind to function 0 of the device. Early generations
 	 * used function 1 as a placeholder for multi-head. This causes
 	 * us confusion instead, especially on the systems where both
@@ -937,6 +807,18 @@ i915_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (PCI_FUNC(pdev->devfn))
 		return -ENODEV;
 
+	/* We've managed to ship a kms-enabled ddx that shipped with an XvMC
+	 * implementation for gen3 (and only gen3) that used legacy drm maps
+	 * (gasp!) to share buffers between X and the client. Hence we need to
+	 * keep around the fake agp stuff for gen3, even when kms is enabled. */
+	if (intel_info->gen != 3) {
+		driver.driver_features &=
+			~(DRIVER_USE_AGP | DRIVER_REQUIRE_AGP);
+	} else if (!intel_agp_enabled) {
+		DRM_ERROR("drm/i915 can't work without intel_agp module!\n");
+		return -ENODEV;
+	}
+
 	return drm_get_pci_dev(pdev, ent, &driver);
 }
 
@@ -1058,7 +940,6 @@ static struct drm_driver driver = {
 	.resume = i915_resume,
 
 	.device_is_agp = i915_driver_device_is_agp,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.master_create = i915_master_create,
 	.master_destroy = i915_master_destroy,
 #if defined(CONFIG_DEBUG_FS)
@@ -1097,11 +978,6 @@ static struct pci_driver i915_pci_driver = {
 
 static int __init i915_init(void)
 {
-	if (!intel_agp_enabled) {
-		DRM_ERROR("drm/i915 can't work without intel_agp module!\n");
-		return -ENODEV;
-	}
-
 	driver.num_ioctls = i915_max_ioctl;
 
 	/*
@@ -1149,6 +1025,84 @@ MODULE_LICENSE("GPL and additional rights");
 	 ((reg) < 0x40000) &&            \
 	 ((reg) != FORCEWAKE))
 
+static bool IS_DISPLAYREG(u32 reg)
+{
+	/*
+	 * This should make it easier to transition modules over to the
+	 * new register block scheme, since we can do it incrementally.
+	 */
+	if (reg >= 0x180000)
+		return false;
+
+	if (reg >= RENDER_RING_BASE &&
+	    reg < RENDER_RING_BASE + 0xff)
+		return false;
+	if (reg >= GEN6_BSD_RING_BASE &&
+	    reg < GEN6_BSD_RING_BASE + 0xff)
+		return false;
+	if (reg >= BLT_RING_BASE &&
+	    reg < BLT_RING_BASE + 0xff)
+		return false;
+
+	if (reg == PGTBL_ER)
+		return false;
+
+	if (reg >= IPEIR_I965 &&
+	    reg < HWSTAM)
+		return false;
+
+	if (reg == MI_MODE)
+		return false;
+
+	if (reg == GFX_MODE_GEN7)
+		return false;
+
+	if (reg == RENDER_HWS_PGA_GEN7 ||
+	    reg == BSD_HWS_PGA_GEN7 ||
+	    reg == BLT_HWS_PGA_GEN7)
+		return false;
+
+	if (reg == GEN6_BSD_SLEEP_PSMI_CONTROL ||
+	    reg == GEN6_BSD_RNCID)
+		return false;
+
+	if (reg == GEN6_BLITTER_ECOSKPD)
+		return false;
+
+	if (reg >= 0x4000c &&
+	    reg <= 0x4002c)
+		return false;
+
+	if (reg >= 0x4f000 &&
+	    reg <= 0x4f08f)
+		return false;
+
+	if (reg >= 0x4f100 &&
+	    reg <= 0x4f11f)
+		return false;
+
+	if (reg >= VLV_MASTER_IER &&
+	    reg <= GEN6_PMIER)
+		return false;
+
+	if (reg >= FENCE_REG_SANDYBRIDGE_0 &&
+	    reg < (FENCE_REG_SANDYBRIDGE_0 + (16*8)))
+		return false;
+
+	if (reg >= VLV_IIR_RW &&
+	    reg <= VLV_ISR)
+		return false;
+
+	if (reg == FORCEWAKE_VLV ||
+	    reg == FORCEWAKE_ACK_VLV)
+		return false;
+
+	if (reg == GEN6_GDRST)
+		return false;
+
+	return true;
+}
+
 #define __i915_read(x, y) \
 u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg) { \
 	u##x val = 0; \
@@ -1156,11 +1110,13 @@ u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg) { \
 		unsigned long irqflags; \
 		spin_lock_irqsave(&dev_priv->gt_lock, irqflags); \
 		if (dev_priv->forcewake_count == 0) \
-			dev_priv->display.force_wake_get(dev_priv); \
+			dev_priv->gt.force_wake_get(dev_priv); \
 		val = read##y(dev_priv->regs + reg); \
 		if (dev_priv->forcewake_count == 0) \
-			dev_priv->display.force_wake_put(dev_priv); \
+			dev_priv->gt.force_wake_put(dev_priv); \
 		spin_unlock_irqrestore(&dev_priv->gt_lock, irqflags); \
+	} else if (IS_VALLEYVIEW(dev_priv->dev) && IS_DISPLAYREG(reg)) { \
+		val = read##y(dev_priv->regs + reg + 0x180000);		\
 	} else { \
 		val = read##y(dev_priv->regs + reg); \
 	} \
@@ -1181,7 +1137,11 @@ void i915_write##x(struct drm_i915_private *dev_priv, u32 reg, u##x val) { \
 	if (NEEDS_FORCE_WAKE((dev_priv), (reg))) { \
 		__fifo_ret = __gen6_gt_wait_for_fifo(dev_priv); \
 	} \
-	write##y(val, dev_priv->regs + reg); \
+	if (IS_VALLEYVIEW(dev_priv->dev) && IS_DISPLAYREG(reg)) { \
+		write##y(val, dev_priv->regs + reg + 0x180000);		\
+	} else {							\
+		write##y(val, dev_priv->regs + reg);			\
+	}								\
 	if (unlikely(__fifo_ret)) { \
 		gen6_gt_check_fifodbg(dev_priv); \
 	} \
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index b0b676abde0..627fe35781b 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -79,6 +79,10 @@ enum port {
 
 #define for_each_pipe(p) for ((p) = 0; (p) < dev_priv->num_pipe; (p)++)
 
+#define for_each_encoder_on_crtc(dev, __crtc, intel_encoder) \
+	list_for_each_entry((intel_encoder), &(dev)->mode_config.encoder_list, base.head) \
+		if ((intel_encoder)->base.crtc == (__crtc))
+
 struct intel_pch_pll {
 	int refcount; /* count of number of CRTCs sharing this PLL */
 	int active; /* count of number of active CRTCs (i.e. DPMS on) */
@@ -176,6 +180,7 @@ struct drm_i915_error_state {
 	u32 eir;
 	u32 pgtbl_er;
 	u32 ier;
+	u32 ccid;
 	bool waiting[I915_NUM_RINGS];
 	u32 pipestat[I915_MAX_PIPES];
 	u32 tail[I915_NUM_RINGS];
@@ -185,6 +190,7 @@ struct drm_i915_error_state {
 	u32 instdone[I915_NUM_RINGS];
 	u32 acthd[I915_NUM_RINGS];
 	u32 semaphore_mboxes[I915_NUM_RINGS][I915_NUM_RINGS - 1];
+	u32 rc_psmi[I915_NUM_RINGS]; /* sleep state */
 	/* our own tracking of ring head and tail */
 	u32 cpu_ring_head[I915_NUM_RINGS];
 	u32 cpu_ring_tail[I915_NUM_RINGS];
@@ -261,8 +267,6 @@ struct drm_i915_display_funcs {
 			  struct drm_i915_gem_object *obj);
 	int (*update_plane)(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 			    int x, int y);
-	void (*force_wake_get)(struct drm_i915_private *dev_priv);
-	void (*force_wake_put)(struct drm_i915_private *dev_priv);
 	/* clock updates for mode set */
 	/* cursor updates */
 	/* render clock increase/decrease */
@@ -270,6 +274,11 @@ struct drm_i915_display_funcs {
 	/* pll clock increase/decrease */
 };
 
+struct drm_i915_gt_funcs {
+	void (*force_wake_get)(struct drm_i915_private *dev_priv);
+	void (*force_wake_put)(struct drm_i915_private *dev_priv);
+};
+
 struct intel_device_info {
 	u8 gen;
 	u8 is_mobile:1;
@@ -284,7 +293,6 @@ struct intel_device_info {
 	u8 is_crestline:1;
 	u8 is_ivybridge:1;
 	u8 is_valleyview:1;
-	u8 has_pch_split:1;
 	u8 has_force_wake:1;
 	u8 is_haswell:1;
 	u8 has_fbc:1;
@@ -309,6 +317,17 @@ struct i915_hw_ppgtt {
 	dma_addr_t scratch_page_dma_addr;
 };
 
+
+/* This must match up with the value previously used for execbuf2.rsvd1. */
+#define DEFAULT_CONTEXT_ID 0
+struct i915_hw_context {
+	int id;
+	bool is_initialized;
+	struct drm_i915_file_private *file_priv;
+	struct intel_ring_buffer *ring;
+	struct drm_i915_gem_object *obj;
+};
+
 enum no_fbc_reason {
 	FBC_NO_OUTPUT, /* no outputs enabled to compress */
 	FBC_STOLEN_TOO_SMALL, /* not enough space to hold compressed buffers */
@@ -321,6 +340,7 @@ enum no_fbc_reason {
 };
 
 enum intel_pch {
+	PCH_NONE = 0,	/* No PCH present */
 	PCH_IBX,	/* Ibexpeak PCH */
 	PCH_CPT,	/* Cougarpoint PCH */
 	PCH_LPT,	/* Lynxpoint PCH */
@@ -350,6 +370,8 @@ typedef struct drm_i915_private {
 	int relative_constants_mode;
 
 	void __iomem *regs;
+
+	struct drm_i915_gt_funcs gt;
 	/** gt_fifo_count and the subsequent register write are synchronized
 	 * with dev->struct_mutex. */
 	unsigned gt_fifo_count;
@@ -652,11 +674,14 @@ typedef struct drm_i915_private {
 		unsigned long gtt_end;
 
 		struct io_mapping *gtt_mapping;
+		phys_addr_t gtt_base_addr;
 		int gtt_mtrr;
 
 		/** PPGTT used for aliasing the PPGTT with the GTT */
 		struct i915_hw_ppgtt *aliasing_ppgtt;
 
+		u32 *l3_remap_info;
+
 		struct shrinker inactive_shrinker;
 
 		/**
@@ -817,6 +842,10 @@ typedef struct drm_i915_private {
 
 	struct drm_property *broadcast_rgb_property;
 	struct drm_property *force_audio_property;
+
+	struct work_struct parity_error_work;
+	bool hw_contexts_disabled;
+	uint32_t hw_context_size;
 } drm_i915_private_t;
 
 /* Iterate over initialised rings */
@@ -1026,6 +1055,7 @@ struct drm_i915_file_private {
 		struct spinlock lock;
 		struct list_head request_list;
 	} mm;
+	struct idr context_idr;
 };
 
 #define INTEL_INFO(dev)	(((struct drm_i915_private *) (dev)->dev_private)->info)
@@ -1071,7 +1101,8 @@ struct drm_i915_file_private {
 #define HAS_LLC(dev)            (INTEL_INFO(dev)->has_llc)
 #define I915_NEED_GFX_HWS(dev)	(INTEL_INFO(dev)->need_gfx_hws)
 
-#define HAS_ALIASING_PPGTT(dev)	(INTEL_INFO(dev)->gen >=6)
+#define HAS_HW_CONTEXTS(dev)	(INTEL_INFO(dev)->gen >= 6)
+#define HAS_ALIASING_PPGTT(dev)	(INTEL_INFO(dev)->gen >=6 && !IS_VALLEYVIEW(dev))
 
 #define HAS_OVERLAY(dev)		(INTEL_INFO(dev)->has_overlay)
 #define OVERLAY_NEEDS_PHYSICAL(dev)	(INTEL_INFO(dev)->overlay_needs_physical)
@@ -1094,13 +1125,13 @@ struct drm_i915_file_private {
 #define HAS_PIPE_CXSR(dev) (INTEL_INFO(dev)->has_pipe_cxsr)
 #define I915_HAS_FBC(dev) (INTEL_INFO(dev)->has_fbc)
 
-#define HAS_PCH_SPLIT(dev) (INTEL_INFO(dev)->has_pch_split)
 #define HAS_PIPE_CONTROL(dev) (INTEL_INFO(dev)->gen >= 5)
 
 #define INTEL_PCH_TYPE(dev) (((struct drm_i915_private *)(dev)->dev_private)->pch_type)
 #define HAS_PCH_LPT(dev) (INTEL_PCH_TYPE(dev) == PCH_LPT)
 #define HAS_PCH_CPT(dev) (INTEL_PCH_TYPE(dev) == PCH_CPT)
 #define HAS_PCH_IBX(dev) (INTEL_PCH_TYPE(dev) == PCH_IBX)
+#define HAS_PCH_SPLIT(dev) (INTEL_PCH_TYPE(dev) != PCH_NONE)
 
 #define HAS_FORCE_WAKE(dev) (INTEL_INFO(dev)->has_force_wake)
 
@@ -1166,6 +1197,7 @@ extern long i915_compat_ioctl(struct file *filp, unsigned int cmd,
 extern int i915_emit_box(struct drm_device *dev,
 			 struct drm_clip_rect *box,
 			 int DR1, int DR4);
+extern int intel_gpu_reset(struct drm_device *dev);
 extern int i915_reset(struct drm_device *dev);
 extern unsigned long i915_chipset_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_mch_val(struct drm_i915_private *dev_priv);
@@ -1178,6 +1210,7 @@ void i915_hangcheck_elapsed(unsigned long data);
 void i915_handle_error(struct drm_device *dev, bool wedged);
 
 extern void intel_irq_init(struct drm_device *dev);
+extern void intel_gt_init(struct drm_device *dev);
 
 void i915_error_state_free(struct kref *error_ref);
 
@@ -1237,6 +1270,8 @@ int i915_gem_get_tiling(struct drm_device *dev, void *data,
 			struct drm_file *file_priv);
 int i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 				struct drm_file *file_priv);
+int i915_gem_wait_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
 void i915_gem_load(struct drm_device *dev);
 int i915_gem_init_object(struct drm_gem_object *obj);
 int __must_check i915_gem_flush_ring(struct intel_ring_buffer *ring,
@@ -1306,6 +1341,8 @@ i915_gem_object_unpin_fence(struct drm_i915_gem_object *obj)
 
 void i915_gem_retire_requests(struct drm_device *dev);
 void i915_gem_retire_requests_ring(struct intel_ring_buffer *ring);
+int __must_check i915_gem_check_wedge(struct drm_i915_private *dev_priv,
+				      bool interruptible);
 
 void i915_gem_reset(struct drm_device *dev);
 void i915_gem_clflush_object(struct drm_i915_gem_object *obj);
@@ -1315,6 +1352,7 @@ int __must_check i915_gem_object_set_domain(struct drm_i915_gem_object *obj,
 int __must_check i915_gem_object_finish_gpu(struct drm_i915_gem_object *obj);
 int __must_check i915_gem_init(struct drm_device *dev);
 int __must_check i915_gem_init_hw(struct drm_device *dev);
+void i915_gem_l3_remap(struct drm_device *dev);
 void i915_gem_init_swizzling(struct drm_device *dev);
 void i915_gem_init_ppgtt(struct drm_device *dev);
 void i915_gem_cleanup_ringbuffer(struct drm_device *dev);
@@ -1323,8 +1361,8 @@ int __must_check i915_gem_idle(struct drm_device *dev);
 int __must_check i915_add_request(struct intel_ring_buffer *ring,
 				  struct drm_file *file,
 				  struct drm_i915_gem_request *request);
-int __must_check i915_wait_request(struct intel_ring_buffer *ring,
-				   uint32_t seqno);
+int __must_check i915_wait_seqno(struct intel_ring_buffer *ring,
+				 uint32_t seqno);
 int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 int __must_check
 i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj,
@@ -1358,6 +1396,16 @@ struct drm_gem_object *i915_gem_prime_import(struct drm_device *dev,
 struct dma_buf *i915_gem_prime_export(struct drm_device *dev,
 				struct drm_gem_object *gem_obj, int flags);
 
+/* i915_gem_context.c */
+void i915_gem_context_init(struct drm_device *dev);
+void i915_gem_context_fini(struct drm_device *dev);
+void i915_gem_context_close(struct drm_device *dev, struct drm_file *file);
+int i915_switch_context(struct intel_ring_buffer *ring,
+			struct drm_file *file, int to_id);
+int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
+				  struct drm_file *file);
+int i915_gem_context_destroy_ioctl(struct drm_device *dev, void *data,
+				   struct drm_file *file);
 
 /* i915_gem_gtt.c */
 int __must_check i915_gem_init_aliasing_ppgtt(struct drm_device *dev);
@@ -1475,20 +1523,12 @@ extern bool intel_fbc_enabled(struct drm_device *dev);
 extern void intel_disable_fbc(struct drm_device *dev);
 extern bool ironlake_set_drps(struct drm_device *dev, u8 val);
 extern void ironlake_init_pch_refclk(struct drm_device *dev);
-extern void ironlake_enable_rc6(struct drm_device *dev);
 extern void gen6_set_rps(struct drm_device *dev, u8 val);
 extern void intel_detect_pch(struct drm_device *dev);
 extern int intel_trans_dp_port_sel(struct drm_crtc *crtc);
 extern int intel_enable_rc6(const struct drm_device *dev);
 
 extern bool i915_semaphore_is_enabled(struct drm_device *dev);
-extern void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv);
-extern void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv);
-extern void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv);
-extern void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv);
-
-extern void vlv_force_wake_get(struct drm_i915_private *dev_priv);
-extern void vlv_force_wake_put(struct drm_i915_private *dev_priv);
 
 /* overlay */
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 288d7b8f49a..5c4657a54f9 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -96,9 +96,18 @@ i915_gem_wait_for_error(struct drm_device *dev)
 	if (!atomic_read(&dev_priv->mm.wedged))
 		return 0;
 
-	ret = wait_for_completion_interruptible(x);
-	if (ret)
+	/*
+	 * Only wait 10 seconds for the gpu reset to complete to avoid hanging
+	 * userspace. If it takes that long something really bad is going on and
+	 * we should simply try to bail out and fail as gracefully as possible.
+	 */
+	ret = wait_for_completion_interruptible_timeout(x, 10*HZ);
+	if (ret == 0) {
+		DRM_ERROR("Timed out waiting for the gpu reset to complete\n");
+		return -EIO;
+	} else if (ret < 0) {
 		return ret;
+	}
 
 	if (atomic_read(&dev_priv->mm.wedged)) {
 		/* GPU is hung, bump the completion count to account for
@@ -1122,7 +1131,7 @@ int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 
 	obj->fault_mappable = true;
 
-	pfn = ((dev->agp->base + obj->gtt_offset) >> PAGE_SHIFT) +
+	pfn = ((dev_priv->mm.gtt_base_addr + obj->gtt_offset) >> PAGE_SHIFT) +
 		page_offset;
 
 	/* Finally, remap it using the new GTT offset */
@@ -1132,6 +1141,11 @@ unlock:
 out:
 	switch (ret) {
 	case -EIO:
+		/* If this -EIO is due to a gpu hang, give the reset code a
+		 * chance to clean up the mess. Otherwise return the proper
+		 * SIGBUS. */
+		if (!atomic_read(&dev_priv->mm.wedged))
+			return VM_FAULT_SIGBUS;
 	case -EAGAIN:
 		/* Give the error handler a chance to run and move the
 		 * objects off the GPU active list. Next time we service the
@@ -1568,6 +1582,21 @@ i915_add_request(struct intel_ring_buffer *ring,
 	int was_empty;
 	int ret;
 
+	/*
+	 * Emit any outstanding flushes - execbuf can fail to emit the flush
+	 * after having emitted the batchbuffer command. Hence we need to fix
+	 * things up similar to emitting the lazy request. The difference here
+	 * is that the flush _must_ happen before the next request, no matter
+	 * what.
+	 */
+	if (ring->gpu_caches_dirty) {
+		ret = i915_gem_flush_ring(ring, 0, I915_GEM_GPU_DOMAINS);
+		if (ret)
+			return ret;
+
+		ring->gpu_caches_dirty = false;
+	}
+
 	BUG_ON(request == NULL);
 	seqno = i915_gem_next_request_seqno(ring);
 
@@ -1613,6 +1642,9 @@ i915_add_request(struct intel_ring_buffer *ring,
 			queue_delayed_work(dev_priv->wq,
 					   &dev_priv->mm.retire_work, HZ);
 	}
+
+	WARN_ON(!list_empty(&ring->gpu_write_list));
+
 	return 0;
 }
 
@@ -1827,14 +1859,11 @@ i915_gem_retire_work_handler(struct work_struct *work)
 	 */
 	idle = true;
 	for_each_ring(ring, dev_priv, i) {
-		if (!list_empty(&ring->gpu_write_list)) {
+		if (ring->gpu_caches_dirty) {
 			struct drm_i915_gem_request *request;
-			int ret;
 
-			ret = i915_gem_flush_ring(ring,
-						  0, I915_GEM_GPU_DOMAINS);
 			request = kzalloc(sizeof(*request), GFP_KERNEL);
-			if (ret || request == NULL ||
+			if (request == NULL ||
 			    i915_add_request(ring, NULL, request))
 			    kfree(request);
 		}
@@ -1848,11 +1877,10 @@ i915_gem_retire_work_handler(struct work_struct *work)
 	mutex_unlock(&dev->struct_mutex);
 }
 
-static int
-i915_gem_check_wedge(struct drm_i915_private *dev_priv)
+int
+i915_gem_check_wedge(struct drm_i915_private *dev_priv,
+		     bool interruptible)
 {
-	BUG_ON(!mutex_is_locked(&dev_priv->dev->struct_mutex));
-
 	if (atomic_read(&dev_priv->mm.wedged)) {
 		struct completion *x = &dev_priv->error_completion;
 		bool recovery_complete;
@@ -1863,7 +1891,16 @@ i915_gem_check_wedge(struct drm_i915_private *dev_priv)
 		recovery_complete = x->done > 0;
 		spin_unlock_irqrestore(&x->wait.lock, flags);
 
-		return recovery_complete ? -EIO : -EAGAIN;
+		/* Non-interruptible callers can't handle -EAGAIN, hence return
+		 * -EIO unconditionally for these. */
+		if (!interruptible)
+			return -EIO;
+
+		/* Recovery complete, but still wedged means reset failure. */
+		if (recovery_complete)
+			return -EIO;
+
+		return -EAGAIN;
 	}
 
 	return 0;
@@ -1899,34 +1936,85 @@ i915_gem_check_olr(struct intel_ring_buffer *ring, u32 seqno)
 	return ret;
 }
 
+/**
+ * __wait_seqno - wait until execution of seqno has finished
+ * @ring: the ring expected to report seqno
+ * @seqno: duh!
+ * @interruptible: do an interruptible wait (normally yes)
+ * @timeout: in - how long to wait (NULL forever); out - how much time remaining
+ *
+ * Returns 0 if the seqno was found within the alloted time. Else returns the
+ * errno with remaining time filled in timeout argument.
+ */
 static int __wait_seqno(struct intel_ring_buffer *ring, u32 seqno,
-			bool interruptible)
+			bool interruptible, struct timespec *timeout)
 {
 	drm_i915_private_t *dev_priv = ring->dev->dev_private;
-	int ret = 0;
+	struct timespec before, now, wait_time={1,0};
+	unsigned long timeout_jiffies;
+	long end;
+	bool wait_forever = true;
+	int ret;
 
 	if (i915_seqno_passed(ring->get_seqno(ring), seqno))
 		return 0;
 
 	trace_i915_gem_request_wait_begin(ring, seqno);
+
+	if (timeout != NULL) {
+		wait_time = *timeout;
+		wait_forever = false;
+	}
+
+	timeout_jiffies = timespec_to_jiffies(&wait_time);
+
 	if (WARN_ON(!ring->irq_get(ring)))
 		return -ENODEV;
 
+	/* Record current time in case interrupted by signal, or wedged * */
+	getrawmonotonic(&before);
+
 #define EXIT_COND \
 	(i915_seqno_passed(ring->get_seqno(ring), seqno) || \
 	atomic_read(&dev_priv->mm.wedged))
+	do {
+		if (interruptible)
+			end = wait_event_interruptible_timeout(ring->irq_queue,
+							       EXIT_COND,
+							       timeout_jiffies);
+		else
+			end = wait_event_timeout(ring->irq_queue, EXIT_COND,
+						 timeout_jiffies);
 
-	if (interruptible)
-		ret = wait_event_interruptible(ring->irq_queue,
-					       EXIT_COND);
-	else
-		wait_event(ring->irq_queue, EXIT_COND);
+		ret = i915_gem_check_wedge(dev_priv, interruptible);
+		if (ret)
+			end = ret;
+	} while (end == 0 && wait_forever);
+
+	getrawmonotonic(&now);
 
 	ring->irq_put(ring);
 	trace_i915_gem_request_wait_end(ring, seqno);
 #undef EXIT_COND
 
-	return ret;
+	if (timeout) {
+		struct timespec sleep_time = timespec_sub(now, before);
+		*timeout = timespec_sub(*timeout, sleep_time);
+	}
+
+	switch (end) {
+	case -EIO:
+	case -EAGAIN: /* Wedged */
+	case -ERESTARTSYS: /* Signal */
+		return (int)end;
+	case 0: /* Timeout */
+		if (timeout)
+			set_normalized_timespec(timeout, 0, 0);
+		return -ETIME;
+	default: /* Completed */
+		WARN_ON(end < 0); /* We're not aware of other errors */
+		return 0;
+	}
 }
 
 /**
@@ -1934,15 +2022,14 @@ static int __wait_seqno(struct intel_ring_buffer *ring, u32 seqno,
  * request and object lists appropriately for that event.
  */
 int
-i915_wait_request(struct intel_ring_buffer *ring,
-		  uint32_t seqno)
+i915_wait_seqno(struct intel_ring_buffer *ring, uint32_t seqno)
 {
 	drm_i915_private_t *dev_priv = ring->dev->dev_private;
 	int ret = 0;
 
 	BUG_ON(seqno == 0);
 
-	ret = i915_gem_check_wedge(dev_priv);
+	ret = i915_gem_check_wedge(dev_priv, dev_priv->mm.interruptible);
 	if (ret)
 		return ret;
 
@@ -1950,9 +2037,7 @@ i915_wait_request(struct intel_ring_buffer *ring,
 	if (ret)
 		return ret;
 
-	ret = __wait_seqno(ring, seqno, dev_priv->mm.interruptible);
-	if (atomic_read(&dev_priv->mm.wedged))
-		ret = -EAGAIN;
+	ret = __wait_seqno(ring, seqno, dev_priv->mm.interruptible, NULL);
 
 	return ret;
 }
@@ -1975,7 +2060,7 @@ i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj)
 	 * it.
 	 */
 	if (obj->active) {
-		ret = i915_wait_request(obj->ring, obj->last_rendering_seqno);
+		ret = i915_wait_seqno(obj->ring, obj->last_rendering_seqno);
 		if (ret)
 			return ret;
 		i915_gem_retire_requests_ring(obj->ring);
@@ -1985,6 +2070,115 @@ i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj)
 }
 
 /**
+ * Ensures that an object will eventually get non-busy by flushing any required
+ * write domains, emitting any outstanding lazy request and retiring and
+ * completed requests.
+ */
+static int
+i915_gem_object_flush_active(struct drm_i915_gem_object *obj)
+{
+	int ret;
+
+	if (obj->active) {
+		ret = i915_gem_object_flush_gpu_write_domain(obj);
+		if (ret)
+			return ret;
+
+		ret = i915_gem_check_olr(obj->ring,
+					 obj->last_rendering_seqno);
+		if (ret)
+			return ret;
+		i915_gem_retire_requests_ring(obj->ring);
+	}
+
+	return 0;
+}
+
+/**
+ * i915_gem_wait_ioctl - implements DRM_IOCTL_I915_GEM_WAIT
+ * @DRM_IOCTL_ARGS: standard ioctl arguments
+ *
+ * Returns 0 if successful, else an error is returned with the remaining time in
+ * the timeout parameter.
+ *  -ETIME: object is still busy after timeout
+ *  -ERESTARTSYS: signal interrupted the wait
+ *  -ENONENT: object doesn't exist
+ * Also possible, but rare:
+ *  -EAGAIN: GPU wedged
+ *  -ENOMEM: damn
+ *  -ENODEV: Internal IRQ fail
+ *  -E?: The add request failed
+ *
+ * The wait ioctl with a timeout of 0 reimplements the busy ioctl. With any
+ * non-zero timeout parameter the wait ioctl will wait for the given number of
+ * nanoseconds on an object becoming unbusy. Since the wait itself does so
+ * without holding struct_mutex the object may become re-busied before this
+ * function completes. A similar but shorter * race condition exists in the busy
+ * ioctl
+ */
+int
+i915_gem_wait_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
+{
+	struct drm_i915_gem_wait *args = data;
+	struct drm_i915_gem_object *obj;
+	struct intel_ring_buffer *ring = NULL;
+	struct timespec timeout_stack, *timeout = NULL;
+	u32 seqno = 0;
+	int ret = 0;
+
+	if (args->timeout_ns >= 0) {
+		timeout_stack = ns_to_timespec(args->timeout_ns);
+		timeout = &timeout_stack;
+	}
+
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		return ret;
+
+	obj = to_intel_bo(drm_gem_object_lookup(dev, file, args->bo_handle));
+	if (&obj->base == NULL) {
+		mutex_unlock(&dev->struct_mutex);
+		return -ENOENT;
+	}
+
+	/* Need to make sure the object gets inactive eventually. */
+	ret = i915_gem_object_flush_active(obj);
+	if (ret)
+		goto out;
+
+	if (obj->active) {
+		seqno = obj->last_rendering_seqno;
+		ring = obj->ring;
+	}
+
+	if (seqno == 0)
+		 goto out;
+
+	/* Do this after OLR check to make sure we make forward progress polling
+	 * on this IOCTL with a 0 timeout (like busy ioctl)
+	 */
+	if (!args->timeout_ns) {
+		ret = -ETIME;
+		goto out;
+	}
+
+	drm_gem_object_unreference(&obj->base);
+	mutex_unlock(&dev->struct_mutex);
+
+	ret = __wait_seqno(ring, seqno, true, timeout);
+	if (timeout) {
+		WARN_ON(!timespec_valid(timeout));
+		args->timeout_ns = timespec_to_ns(timeout);
+	}
+	return ret;
+
+out:
+	drm_gem_object_unreference(&obj->base);
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+}
+
+/**
  * i915_gem_object_sync - sync an object to a ring.
  *
  * @obj: object which may be in use on another ring.
@@ -2160,7 +2354,7 @@ static int i915_ring_idle(struct intel_ring_buffer *ring)
 			return ret;
 	}
 
-	return i915_wait_request(ring, i915_gem_next_request_seqno(ring));
+	return i915_wait_seqno(ring, i915_gem_next_request_seqno(ring));
 }
 
 int i915_gpu_idle(struct drm_device *dev)
@@ -2178,6 +2372,10 @@ int i915_gpu_idle(struct drm_device *dev)
 		/* Is the device fubar? */
 		if (WARN_ON(!list_empty(&ring->gpu_write_list)))
 			return -EBUSY;
+
+		ret = i915_switch_context(ring, NULL, DEFAULT_CONTEXT_ID);
+		if (ret)
+			return ret;
 	}
 
 	return 0;
@@ -2364,7 +2562,7 @@ i915_gem_object_flush_fence(struct drm_i915_gem_object *obj)
 	}
 
 	if (obj->last_fenced_seqno) {
-		ret = i915_wait_request(obj->ring, obj->last_fenced_seqno);
+		ret = i915_wait_seqno(obj->ring, obj->last_fenced_seqno);
 		if (ret)
 			return ret;
 
@@ -2551,8 +2749,8 @@ i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 	if (map_and_fenceable)
 		free_space =
 			drm_mm_search_free_in_range(&dev_priv->mm.gtt_space,
-						    size, alignment, 0,
-						    dev_priv->mm.gtt_mappable_end,
+						    size, alignment,
+						    0, dev_priv->mm.gtt_mappable_end,
 						    0);
 	else
 		free_space = drm_mm_search_free(&dev_priv->mm.gtt_space,
@@ -2563,7 +2761,7 @@ i915_gem_object_bind_to_gtt(struct drm_i915_gem_object *obj,
 			obj->gtt_space =
 				drm_mm_get_block_range_generic(free_space,
 							       size, alignment, 0,
-							       dev_priv->mm.gtt_mappable_end,
+							       0, dev_priv->mm.gtt_mappable_end,
 							       0);
 		else
 			obj->gtt_space =
@@ -3030,7 +3228,7 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 	if (seqno == 0)
 		return 0;
 
-	ret = __wait_seqno(ring, seqno, true);
+	ret = __wait_seqno(ring, seqno, true, NULL);
 	if (ret == 0)
 		queue_delayed_work(dev_priv->wq, &dev_priv->mm.retire_work, 0);
 
@@ -3199,30 +3397,9 @@ i915_gem_busy_ioctl(struct drm_device *dev, void *data,
 	 * become non-busy without any further actions, therefore emit any
 	 * necessary flushes here.
 	 */
-	args->busy = obj->active;
-	if (args->busy) {
-		/* Unconditionally flush objects, even when the gpu still uses this
-		 * object. Userspace calling this function indicates that it wants to
-		 * use this buffer rather sooner than later, so issuing the required
-		 * flush earlier is beneficial.
-		 */
-		if (obj->base.write_domain & I915_GEM_GPU_DOMAINS) {
-			ret = i915_gem_flush_ring(obj->ring,
-						  0, obj->base.write_domain);
-		} else {
-			ret = i915_gem_check_olr(obj->ring,
-						 obj->last_rendering_seqno);
-		}
+	ret = i915_gem_object_flush_active(obj);
 
-		/* Update the active list for the hardware's current position.
-		 * Otherwise this only updates on a delayed timer or when irqs
-		 * are actually unmasked, and our working set ends up being
-		 * larger than required.
-		 */
-		i915_gem_retire_requests_ring(obj->ring);
-
-		args->busy = obj->active;
-	}
+	args->busy = obj->active;
 
 	drm_gem_object_unreference(&obj->base);
 unlock:
@@ -3435,6 +3612,38 @@ i915_gem_idle(struct drm_device *dev)
 	return 0;
 }
 
+void i915_gem_l3_remap(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	u32 misccpctl;
+	int i;
+
+	if (!IS_IVYBRIDGE(dev))
+		return;
+
+	if (!dev_priv->mm.l3_remap_info)
+		return;
+
+	misccpctl = I915_READ(GEN7_MISCCPCTL);
+	I915_WRITE(GEN7_MISCCPCTL, misccpctl & ~GEN7_DOP_CLOCK_GATE_ENABLE);
+	POSTING_READ(GEN7_MISCCPCTL);
+
+	for (i = 0; i < GEN7_L3LOG_SIZE; i += 4) {
+		u32 remap = I915_READ(GEN7_L3LOG_BASE + i);
+		if (remap && remap != dev_priv->mm.l3_remap_info[i/4])
+			DRM_DEBUG("0x%x was already programmed to %x\n",
+				  GEN7_L3LOG_BASE + i, remap);
+		if (remap && !dev_priv->mm.l3_remap_info[i/4])
+			DRM_DEBUG_DRIVER("Clearing remapped register\n");
+		I915_WRITE(GEN7_L3LOG_BASE + i, dev_priv->mm.l3_remap_info[i/4]);
+	}
+
+	/* Make sure all the writes land before disabling dop clock gating */
+	POSTING_READ(GEN7_L3LOG_BASE);
+
+	I915_WRITE(GEN7_MISCCPCTL, misccpctl);
+}
+
 void i915_gem_init_swizzling(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
@@ -3518,12 +3727,33 @@ void i915_gem_init_ppgtt(struct drm_device *dev)
 	}
 }
 
+static bool
+intel_enable_blt(struct drm_device *dev)
+{
+	if (!HAS_BLT(dev))
+		return false;
+
+	/* The blitter was dysfunctional on early prototypes */
+	if (IS_GEN6(dev) && dev->pdev->revision < 8) {
+		DRM_INFO("BLT not supported on this pre-production hardware;"
+			 " graphics performance will be degraded.\n");
+		return false;
+	}
+
+	return true;
+}
+
 int
 i915_gem_init_hw(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = dev->dev_private;
 	int ret;
 
+	if (!intel_enable_gtt())
+		return -EIO;
+
+	i915_gem_l3_remap(dev);
+
 	i915_gem_init_swizzling(dev);
 
 	ret = intel_init_render_ring_buffer(dev);
@@ -3536,7 +3766,7 @@ i915_gem_init_hw(struct drm_device *dev)
 			goto cleanup_render_ring;
 	}
 
-	if (HAS_BLT(dev)) {
+	if (intel_enable_blt(dev)) {
 		ret = intel_init_blt_ring_buffer(dev);
 		if (ret)
 			goto cleanup_bsd_ring;
@@ -3544,6 +3774,11 @@ i915_gem_init_hw(struct drm_device *dev)
 
 	dev_priv->next_seqno = 1;
 
+	/*
+	 * XXX: There was some w/a described somewhere suggesting loading
+	 * contexts before PPGTT.
+	 */
+	i915_gem_context_init(dev);
 	i915_gem_init_ppgtt(dev);
 
 	return 0;
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
new file mode 100644
index 00000000000..da8b01fb1bf
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -0,0 +1,536 @@
+/*
+ * Copyright © 2011-2012 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Ben Widawsky <ben@bwidawsk.net>
+ *
+ */
+
+/*
+ * This file implements HW context support. On gen5+ a HW context consists of an
+ * opaque GPU object which is referenced at times of context saves and restores.
+ * With RC6 enabled, the context is also referenced as the GPU enters and exists
+ * from RC6 (GPU has it's own internal power context, except on gen5). Though
+ * something like a context does exist for the media ring, the code only
+ * supports contexts for the render ring.
+ *
+ * In software, there is a distinction between contexts created by the user,
+ * and the default HW context. The default HW context is used by GPU clients
+ * that do not request setup of their own hardware context. The default
+ * context's state is never restored to help prevent programming errors. This
+ * would happen if a client ran and piggy-backed off another clients GPU state.
+ * The default context only exists to give the GPU some offset to load as the
+ * current to invoke a save of the context we actually care about. In fact, the
+ * code could likely be constructed, albeit in a more complicated fashion, to
+ * never use the default context, though that limits the driver's ability to
+ * swap out, and/or destroy other contexts.
+ *
+ * All other contexts are created as a request by the GPU client. These contexts
+ * store GPU state, and thus allow GPU clients to not re-emit state (and
+ * potentially query certain state) at any time. The kernel driver makes
+ * certain that the appropriate commands are inserted.
+ *
+ * The context life cycle is semi-complicated in that context BOs may live
+ * longer than the context itself because of the way the hardware, and object
+ * tracking works. Below is a very crude representation of the state machine
+ * describing the context life.
+ *                                         refcount     pincount     active
+ * S0: initial state                          0            0           0
+ * S1: context created                        1            0           0
+ * S2: context is currently running           2            1           X
+ * S3: GPU referenced, but not current        2            0           1
+ * S4: context is current, but destroyed      1            1           0
+ * S5: like S3, but destroyed                 1            0           1
+ *
+ * The most common (but not all) transitions:
+ * S0->S1: client creates a context
+ * S1->S2: client submits execbuf with context
+ * S2->S3: other clients submits execbuf with context
+ * S3->S1: context object was retired
+ * S3->S2: clients submits another execbuf
+ * S2->S4: context destroy called with current context
+ * S3->S5->S0: destroy path
+ * S4->S5->S0: destroy path on current context
+ *
+ * There are two confusing terms used above:
+ *  The "current context" means the context which is currently running on the
+ *  GPU. The GPU has loaded it's state already and has stored away the gtt
+ *  offset of the BO. The GPU is not actively referencing the data at this
+ *  offset, but it will on the next context switch. The only way to avoid this
+ *  is to do a GPU reset.
+ *
+ *  An "active context' is one which was previously the "current context" and is
+ *  on the active list waiting for the next context switch to occur. Until this
+ *  happens, the object must remain at the same gtt offset. It is therefore
+ *  possible to destroy a context, but it is still active.
+ *
+ */
+
+#include "drmP.h"
+#include "i915_drm.h"
+#include "i915_drv.h"
+
+/* This is a HW constraint. The value below is the largest known requirement
+ * I've seen in a spec to date, and that was a workaround for a non-shipping
+ * part. It should be safe to decrease this, but it's more future proof as is.
+ */
+#define CONTEXT_ALIGN (64<<10)
+
+static struct i915_hw_context *
+i915_gem_context_get(struct drm_i915_file_private *file_priv, u32 id);
+static int do_switch(struct drm_i915_gem_object *from_obj,
+		     struct i915_hw_context *to, u32 seqno);
+
+static int get_context_size(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
+	u32 reg;
+
+	switch (INTEL_INFO(dev)->gen) {
+	case 6:
+		reg = I915_READ(CXT_SIZE);
+		ret = GEN6_CXT_TOTAL_SIZE(reg) * 64;
+		break;
+	case 7:
+		reg = I915_READ(GEN7_CXT_SIZE);
+		ret = GEN7_CXT_TOTAL_SIZE(reg) * 64;
+		break;
+	default:
+		BUG();
+	}
+
+	return ret;
+}
+
+static void do_destroy(struct i915_hw_context *ctx)
+{
+	struct drm_device *dev = ctx->obj->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (ctx->file_priv)
+		idr_remove(&ctx->file_priv->context_idr, ctx->id);
+	else
+		BUG_ON(ctx != dev_priv->ring[RCS].default_context);
+
+	drm_gem_object_unreference(&ctx->obj->base);
+	kfree(ctx);
+}
+
+static struct i915_hw_context *
+create_hw_context(struct drm_device *dev,
+		  struct drm_i915_file_private *file_priv)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct i915_hw_context *ctx;
+	int ret, id;
+
+	ctx = kzalloc(sizeof(struct drm_i915_file_private), GFP_KERNEL);
+	if (ctx == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	ctx->obj = i915_gem_alloc_object(dev, dev_priv->hw_context_size);
+	if (ctx->obj == NULL) {
+		kfree(ctx);
+		DRM_DEBUG_DRIVER("Context object allocated failed\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* The ring associated with the context object is handled by the normal
+	 * object tracking code. We give an initial ring value simple to pass an
+	 * assertion in the context switch code.
+	 */
+	ctx->ring = &dev_priv->ring[RCS];
+
+	/* Default context will never have a file_priv */
+	if (file_priv == NULL)
+		return ctx;
+
+	ctx->file_priv = file_priv;
+
+again:
+	if (idr_pre_get(&file_priv->context_idr, GFP_KERNEL) == 0) {
+		ret = -ENOMEM;
+		DRM_DEBUG_DRIVER("idr allocation failed\n");
+		goto err_out;
+	}
+
+	ret = idr_get_new_above(&file_priv->context_idr, ctx,
+				DEFAULT_CONTEXT_ID + 1, &id);
+	if (ret == 0)
+		ctx->id = id;
+
+	if (ret == -EAGAIN)
+		goto again;
+	else if (ret)
+		goto err_out;
+
+	return ctx;
+
+err_out:
+	do_destroy(ctx);
+	return ERR_PTR(ret);
+}
+
+static inline bool is_default_context(struct i915_hw_context *ctx)
+{
+	return (ctx == ctx->ring->default_context);
+}
+
+/**
+ * The default context needs to exist per ring that uses contexts. It stores the
+ * context state of the GPU for applications that don't utilize HW contexts, as
+ * well as an idle case.
+ */
+static int create_default_context(struct drm_i915_private *dev_priv)
+{
+	struct i915_hw_context *ctx;
+	int ret;
+
+	BUG_ON(!mutex_is_locked(&dev_priv->dev->struct_mutex));
+
+	ctx = create_hw_context(dev_priv->dev, NULL);
+	if (IS_ERR(ctx))
+		return PTR_ERR(ctx);
+
+	/* We may need to do things with the shrinker which require us to
+	 * immediately switch back to the default context. This can cause a
+	 * problem as pinning the default context also requires GTT space which
+	 * may not be available. To avoid this we always pin the
+	 * default context.
+	 */
+	dev_priv->ring[RCS].default_context = ctx;
+	ret = i915_gem_object_pin(ctx->obj, CONTEXT_ALIGN, false);
+	if (ret) {
+		do_destroy(ctx);
+		return ret;
+	}
+
+	ret = do_switch(NULL, ctx, 0);
+	if (ret) {
+		i915_gem_object_unpin(ctx->obj);
+		do_destroy(ctx);
+	} else {
+		DRM_DEBUG_DRIVER("Default HW context loaded\n");
+	}
+
+	return ret;
+}
+
+void i915_gem_context_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t ctx_size;
+
+	if (!HAS_HW_CONTEXTS(dev)) {
+		dev_priv->hw_contexts_disabled = true;
+		return;
+	}
+
+	/* If called from reset, or thaw... we've been here already */
+	if (dev_priv->hw_contexts_disabled ||
+	    dev_priv->ring[RCS].default_context)
+		return;
+
+	ctx_size = get_context_size(dev);
+	dev_priv->hw_context_size = get_context_size(dev);
+	dev_priv->hw_context_size = round_up(dev_priv->hw_context_size, 4096);
+
+	if (ctx_size <= 0 || ctx_size > (1<<20)) {
+		dev_priv->hw_contexts_disabled = true;
+		return;
+	}
+
+	if (create_default_context(dev_priv)) {
+		dev_priv->hw_contexts_disabled = true;
+		return;
+	}
+
+	DRM_DEBUG_DRIVER("HW context support initialized\n");
+}
+
+void i915_gem_context_fini(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->hw_contexts_disabled)
+		return;
+
+	/* The only known way to stop the gpu from accessing the hw context is
+	 * to reset it. Do this as the very last operation to avoid confusing
+	 * other code, leading to spurious errors. */
+	intel_gpu_reset(dev);
+
+	i915_gem_object_unpin(dev_priv->ring[RCS].default_context->obj);
+
+	do_destroy(dev_priv->ring[RCS].default_context);
+}
+
+static int context_idr_cleanup(int id, void *p, void *data)
+{
+	struct i915_hw_context *ctx = p;
+
+	BUG_ON(id == DEFAULT_CONTEXT_ID);
+
+	do_destroy(ctx);
+
+	return 0;
+}
+
+void i915_gem_context_close(struct drm_device *dev, struct drm_file *file)
+{
+	struct drm_i915_file_private *file_priv = file->driver_priv;
+
+	mutex_lock(&dev->struct_mutex);
+	idr_for_each(&file_priv->context_idr, context_idr_cleanup, NULL);
+	idr_destroy(&file_priv->context_idr);
+	mutex_unlock(&dev->struct_mutex);
+}
+
+static struct i915_hw_context *
+i915_gem_context_get(struct drm_i915_file_private *file_priv, u32 id)
+{
+	return (struct i915_hw_context *)idr_find(&file_priv->context_idr, id);
+}
+
+static inline int
+mi_set_context(struct intel_ring_buffer *ring,
+	       struct i915_hw_context *new_context,
+	       u32 hw_flags)
+{
+	int ret;
+
+	/* w/a: If Flush TLB Invalidation Mode is enabled, driver must do a TLB
+	 * invalidation prior to MI_SET_CONTEXT. On GEN6 we don't set the value
+	 * explicitly, so we rely on the value at ring init, stored in
+	 * itlb_before_ctx_switch.
+	 */
+	if (IS_GEN6(ring->dev) && ring->itlb_before_ctx_switch) {
+		ret = ring->flush(ring, 0, 0);
+		if (ret)
+			return ret;
+	}
+
+	ret = intel_ring_begin(ring, 6);
+	if (ret)
+		return ret;
+
+	if (IS_GEN7(ring->dev))
+		intel_ring_emit(ring, MI_ARB_ON_OFF | MI_ARB_DISABLE);
+	else
+		intel_ring_emit(ring, MI_NOOP);
+
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_emit(ring, MI_SET_CONTEXT);
+	intel_ring_emit(ring, new_context->obj->gtt_offset |
+			MI_MM_SPACE_GTT |
+			MI_SAVE_EXT_STATE_EN |
+			MI_RESTORE_EXT_STATE_EN |
+			hw_flags);
+	/* w/a: MI_SET_CONTEXT must always be followed by MI_NOOP */
+	intel_ring_emit(ring, MI_NOOP);
+
+	if (IS_GEN7(ring->dev))
+		intel_ring_emit(ring, MI_ARB_ON_OFF | MI_ARB_ENABLE);
+	else
+		intel_ring_emit(ring, MI_NOOP);
+
+	intel_ring_advance(ring);
+
+	return ret;
+}
+
+static int do_switch(struct drm_i915_gem_object *from_obj,
+		     struct i915_hw_context *to,
+		     u32 seqno)
+{
+	struct intel_ring_buffer *ring = NULL;
+	u32 hw_flags = 0;
+	int ret;
+
+	BUG_ON(to == NULL);
+	BUG_ON(from_obj != NULL && from_obj->pin_count == 0);
+
+	ret = i915_gem_object_pin(to->obj, CONTEXT_ALIGN, false);
+	if (ret)
+		return ret;
+
+	/* Clear this page out of any CPU caches for coherent swap-in/out. Note
+	 * that thanks to write = false in this call and us not setting any gpu
+	 * write domains when putting a context object onto the active list
+	 * (when switching away from it), this won't block.
+	 * XXX: We need a real interface to do this instead of trickery. */
+	ret = i915_gem_object_set_to_gtt_domain(to->obj, false);
+	if (ret) {
+		i915_gem_object_unpin(to->obj);
+		return ret;
+	}
+
+	if (!to->obj->has_global_gtt_mapping)
+		i915_gem_gtt_bind_object(to->obj, to->obj->cache_level);
+
+	if (!to->is_initialized || is_default_context(to))
+		hw_flags |= MI_RESTORE_INHIBIT;
+	else if (WARN_ON_ONCE(from_obj == to->obj)) /* not yet expected */
+		hw_flags |= MI_FORCE_RESTORE;
+
+	ring = to->ring;
+	ret = mi_set_context(ring, to, hw_flags);
+	if (ret) {
+		i915_gem_object_unpin(to->obj);
+		return ret;
+	}
+
+	/* The backing object for the context is done after switching to the
+	 * *next* context. Therefore we cannot retire the previous context until
+	 * the next context has already started running. In fact, the below code
+	 * is a bit suboptimal because the retiring can occur simply after the
+	 * MI_SET_CONTEXT instead of when the next seqno has completed.
+	 */
+	if (from_obj != NULL) {
+		from_obj->base.read_domains = I915_GEM_DOMAIN_INSTRUCTION;
+		i915_gem_object_move_to_active(from_obj, ring, seqno);
+		/* As long as MI_SET_CONTEXT is serializing, ie. it flushes the
+		 * whole damn pipeline, we don't need to explicitly mark the
+		 * object dirty. The only exception is that the context must be
+		 * correct in case the object gets swapped out. Ideally we'd be
+		 * able to defer doing this until we know the object would be
+		 * swapped, but there is no way to do that yet.
+		 */
+		from_obj->dirty = 1;
+		BUG_ON(from_obj->ring != to->ring);
+		i915_gem_object_unpin(from_obj);
+
+		drm_gem_object_unreference(&from_obj->base);
+	}
+
+	drm_gem_object_reference(&to->obj->base);
+	ring->last_context_obj = to->obj;
+	to->is_initialized = true;
+
+	return 0;
+}
+
+/**
+ * i915_switch_context() - perform a GPU context switch.
+ * @ring: ring for which we'll execute the context switch
+ * @file_priv: file_priv associated with the context, may be NULL
+ * @id: context id number
+ * @seqno: sequence number by which the new context will be switched to
+ * @flags:
+ *
+ * The context life cycle is simple. The context refcount is incremented and
+ * decremented by 1 and create and destroy. If the context is in use by the GPU,
+ * it will have a refoucnt > 1. This allows us to destroy the context abstract
+ * object while letting the normal object tracking destroy the backing BO.
+ */
+int i915_switch_context(struct intel_ring_buffer *ring,
+			struct drm_file *file,
+			int to_id)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	struct drm_i915_file_private *file_priv = NULL;
+	struct i915_hw_context *to;
+	struct drm_i915_gem_object *from_obj = ring->last_context_obj;
+	int ret;
+
+	if (dev_priv->hw_contexts_disabled)
+		return 0;
+
+	if (ring != &dev_priv->ring[RCS])
+		return 0;
+
+	if (file)
+		file_priv = file->driver_priv;
+
+	if (to_id == DEFAULT_CONTEXT_ID) {
+		to = ring->default_context;
+	} else {
+		to = i915_gem_context_get(file_priv, to_id);
+		if (to == NULL)
+			return -ENOENT;
+	}
+
+	if (from_obj == to->obj)
+		return 0;
+
+	return do_switch(from_obj, to, i915_gem_next_request_seqno(to->ring));
+}
+
+int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
+				  struct drm_file *file)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_gem_context_create *args = data;
+	struct drm_i915_file_private *file_priv = file->driver_priv;
+	struct i915_hw_context *ctx;
+	int ret;
+
+	if (!(dev->driver->driver_features & DRIVER_GEM))
+		return -ENODEV;
+
+	if (dev_priv->hw_contexts_disabled)
+		return -ENODEV;
+
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		return ret;
+
+	ctx = create_hw_context(dev, file_priv);
+	mutex_unlock(&dev->struct_mutex);
+	if (IS_ERR(ctx))
+		return PTR_ERR(ctx);
+
+	args->ctx_id = ctx->id;
+	DRM_DEBUG_DRIVER("HW context %d created\n", args->ctx_id);
+
+	return 0;
+}
+
+int i915_gem_context_destroy_ioctl(struct drm_device *dev, void *data,
+				   struct drm_file *file)
+{
+	struct drm_i915_gem_context_destroy *args = data;
+	struct drm_i915_file_private *file_priv = file->driver_priv;
+	struct i915_hw_context *ctx;
+	int ret;
+
+	if (!(dev->driver->driver_features & DRIVER_GEM))
+		return -ENODEV;
+
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		return ret;
+
+	ctx = i915_gem_context_get(file_priv, args->ctx_id);
+	if (!ctx) {
+		mutex_unlock(&dev->struct_mutex);
+		return -ENOENT;
+	}
+
+	do_destroy(ctx);
+
+	mutex_unlock(&dev->struct_mutex);
+
+	DRM_DEBUG_DRIVER("HW context %d destroyed\n", args->ctx_id);
+	return 0;
+}
diff --git a/drivers/gpu/drm/i915/i915_gem_debug.c b/drivers/gpu/drm/i915/i915_gem_debug.c
index a4f6aaabca9..bddf7bed183 100644
--- a/drivers/gpu/drm/i915/i915_gem_debug.c
+++ b/drivers/gpu/drm/i915/i915_gem_debug.c
@@ -132,7 +132,8 @@ i915_gem_object_check_coherency(struct drm_i915_gem_object *obj, int handle)
 		 __func__, obj, obj->gtt_offset, handle,
 		 obj->size / 1024);
 
-	gtt_mapping = ioremap(dev->agp->base + obj->gtt_offset, obj->base.size);
+	gtt_mapping = ioremap(dev_priv->mm.gtt_base_addr + obj->gtt_offset,
+			      obj->base.size);
 	if (gtt_mapping == NULL) {
 		DRM_ERROR("failed to map GTT space\n");
 		return;
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index ae7c24e12e5..eba0308f10e 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -78,11 +78,12 @@ i915_gem_evict_something(struct drm_device *dev, int min_size,
 
 	INIT_LIST_HEAD(&unwind_list);
 	if (mappable)
-		drm_mm_init_scan_with_range(&dev_priv->mm.gtt_space, min_size,
-					    alignment, 0,
-					    dev_priv->mm.gtt_mappable_end);
+		drm_mm_init_scan_with_range(&dev_priv->mm.gtt_space,
+					    min_size, alignment, 0,
+					    0, dev_priv->mm.gtt_mappable_end);
 	else
-		drm_mm_init_scan(&dev_priv->mm.gtt_space, min_size, alignment);
+		drm_mm_init_scan(&dev_priv->mm.gtt_space,
+				 min_size, alignment, 0);
 
 	/* First see if there is a large enough contiguous idle region... */
 	list_for_each_entry(obj, &dev_priv->mm.inactive_list, mm_list) {
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 974a9f1068a..5af631e788c 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -810,33 +810,16 @@ err:
 	return ret;
 }
 
-static int
+static void
 i915_gem_execbuffer_flush(struct drm_device *dev,
 			  uint32_t invalidate_domains,
-			  uint32_t flush_domains,
-			  uint32_t flush_rings)
+			  uint32_t flush_domains)
 {
-	drm_i915_private_t *dev_priv = dev->dev_private;
-	int i, ret;
-
 	if (flush_domains & I915_GEM_DOMAIN_CPU)
 		intel_gtt_chipset_flush();
 
 	if (flush_domains & I915_GEM_DOMAIN_GTT)
 		wmb();
-
-	if ((flush_domains | invalidate_domains) & I915_GEM_GPU_DOMAINS) {
-		for (i = 0; i < I915_NUM_RINGS; i++)
-			if (flush_rings & (1 << i)) {
-				ret = i915_gem_flush_ring(&dev_priv->ring[i],
-							  invalidate_domains,
-							  flush_domains);
-				if (ret)
-					return ret;
-			}
-	}
-
-	return 0;
 }
 
 static int
@@ -885,12 +868,9 @@ i915_gem_execbuffer_move_to_gpu(struct intel_ring_buffer *ring,
 		i915_gem_object_set_to_gpu_domain(obj, ring, &cd);
 
 	if (cd.invalidate_domains | cd.flush_domains) {
-		ret = i915_gem_execbuffer_flush(ring->dev,
-						cd.invalidate_domains,
-						cd.flush_domains,
-						cd.flush_rings);
-		if (ret)
-			return ret;
+		i915_gem_execbuffer_flush(ring->dev,
+					  cd.invalidate_domains,
+					  cd.flush_domains);
 	}
 
 	if (cd.flips) {
@@ -905,6 +885,16 @@ i915_gem_execbuffer_move_to_gpu(struct intel_ring_buffer *ring,
 			return ret;
 	}
 
+	/* Unconditionally invalidate gpu caches and ensure that we do flush
+	 * any residual writes from the previous batch.
+	 */
+	ret = i915_gem_flush_ring(ring,
+				  I915_GEM_GPU_DOMAINS,
+				  ring->gpu_caches_dirty ? I915_GEM_GPU_DOMAINS : 0);
+	if (ret)
+		return ret;
+
+	ring->gpu_caches_dirty = false;
 	return 0;
 }
 
@@ -983,26 +973,13 @@ i915_gem_execbuffer_retire_commands(struct drm_device *dev,
 				    struct intel_ring_buffer *ring)
 {
 	struct drm_i915_gem_request *request;
-	u32 invalidate;
 
-	/*
-	 * Ensure that the commands in the batch buffer are
-	 * finished before the interrupt fires.
-	 *
-	 * The sampler always gets flushed on i965 (sigh).
-	 */
-	invalidate = I915_GEM_DOMAIN_COMMAND;
-	if (INTEL_INFO(dev)->gen >= 4)
-		invalidate |= I915_GEM_DOMAIN_SAMPLER;
-	if (ring->flush(ring, invalidate, 0)) {
-		i915_gem_next_request_seqno(ring);
-		return;
-	}
+	/* Unconditionally force add_request to emit a full flush. */
+	ring->gpu_caches_dirty = true;
 
 	/* Add a breadcrumb for the completion of the batch buffer */
 	request = kzalloc(sizeof(*request), GFP_KERNEL);
 	if (request == NULL || i915_add_request(ring, file, request)) {
-		i915_gem_next_request_seqno(ring);
 		kfree(request);
 	}
 }
@@ -1044,6 +1021,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	struct drm_i915_gem_object *batch_obj;
 	struct drm_clip_rect *cliprects = NULL;
 	struct intel_ring_buffer *ring;
+	u32 ctx_id = i915_execbuffer2_get_context_id(*args);
 	u32 exec_start, exec_len;
 	u32 seqno;
 	u32 mask;
@@ -1065,9 +1043,19 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		break;
 	case I915_EXEC_BSD:
 		ring = &dev_priv->ring[VCS];
+		if (ctx_id != 0) {
+			DRM_DEBUG("Ring %s doesn't support contexts\n",
+				  ring->name);
+			return -EPERM;
+		}
 		break;
 	case I915_EXEC_BLT:
 		ring = &dev_priv->ring[BCS];
+		if (ctx_id != 0) {
+			DRM_DEBUG("Ring %s doesn't support contexts\n",
+				  ring->name);
+			return -EPERM;
+		}
 		break;
 	default:
 		DRM_DEBUG("execbuf with unknown ring: %d\n",
@@ -1240,6 +1228,10 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		}
 	}
 
+	ret = i915_switch_context(ring, file, ctx_id);
+	if (ret)
+		goto err;
+
 	if (ring == &dev_priv->ring[RCS] &&
 	    mode != dev_priv->relative_constants_mode) {
 		ret = intel_ring_begin(ring, 4);
@@ -1367,6 +1359,7 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
 	exec2.num_cliprects = args->num_cliprects;
 	exec2.cliprects_ptr = args->cliprects_ptr;
 	exec2.flags = I915_EXEC_RENDER;
+	i915_execbuffer2_set_context_id(exec2, 0);
 
 	ret = i915_gem_do_execbuffer(dev, data, file, &exec2, exec2_list);
 	if (!ret) {
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index ed3224c3742..8a3828528b9 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -375,6 +375,86 @@ static void gen6_pm_rps_work(struct work_struct *work)
 	mutex_unlock(&dev_priv->dev->struct_mutex);
 }
 
+
+/**
+ * ivybridge_parity_work - Workqueue called when a parity error interrupt
+ * occurred.
+ * @work: workqueue struct
+ *
+ * Doesn't actually do anything except notify userspace. As a consequence of
+ * this event, userspace should try to remap the bad rows since statistically
+ * it is likely the same row is more likely to go bad again.
+ */
+static void ivybridge_parity_work(struct work_struct *work)
+{
+	drm_i915_private_t *dev_priv = container_of(work, drm_i915_private_t,
+						    parity_error_work);
+	u32 error_status, row, bank, subbank;
+	char *parity_event[5];
+	uint32_t misccpctl;
+	unsigned long flags;
+
+	/* We must turn off DOP level clock gating to access the L3 registers.
+	 * In order to prevent a get/put style interface, acquire struct mutex
+	 * any time we access those registers.
+	 */
+	mutex_lock(&dev_priv->dev->struct_mutex);
+
+	misccpctl = I915_READ(GEN7_MISCCPCTL);
+	I915_WRITE(GEN7_MISCCPCTL, misccpctl & ~GEN7_DOP_CLOCK_GATE_ENABLE);
+	POSTING_READ(GEN7_MISCCPCTL);
+
+	error_status = I915_READ(GEN7_L3CDERRST1);
+	row = GEN7_PARITY_ERROR_ROW(error_status);
+	bank = GEN7_PARITY_ERROR_BANK(error_status);
+	subbank = GEN7_PARITY_ERROR_SUBBANK(error_status);
+
+	I915_WRITE(GEN7_L3CDERRST1, GEN7_PARITY_ERROR_VALID |
+				    GEN7_L3CDERRST1_ENABLE);
+	POSTING_READ(GEN7_L3CDERRST1);
+
+	I915_WRITE(GEN7_MISCCPCTL, misccpctl);
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	dev_priv->gt_irq_mask &= ~GT_GEN7_L3_PARITY_ERROR_INTERRUPT;
+	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	mutex_unlock(&dev_priv->dev->struct_mutex);
+
+	parity_event[0] = "L3_PARITY_ERROR=1";
+	parity_event[1] = kasprintf(GFP_KERNEL, "ROW=%d", row);
+	parity_event[2] = kasprintf(GFP_KERNEL, "BANK=%d", bank);
+	parity_event[3] = kasprintf(GFP_KERNEL, "SUBBANK=%d", subbank);
+	parity_event[4] = NULL;
+
+	kobject_uevent_env(&dev_priv->dev->primary->kdev.kobj,
+			   KOBJ_CHANGE, parity_event);
+
+	DRM_DEBUG("Parity error: Row = %d, Bank = %d, Sub bank = %d.\n",
+		  row, bank, subbank);
+
+	kfree(parity_event[3]);
+	kfree(parity_event[2]);
+	kfree(parity_event[1]);
+}
+
+static void ivybridge_handle_parity_error(struct drm_device *dev)
+{
+	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	unsigned long flags;
+
+	if (!IS_IVYBRIDGE(dev))
+		return;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	dev_priv->gt_irq_mask |= GT_GEN7_L3_PARITY_ERROR_INTERRUPT;
+	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	queue_work(dev_priv->wq, &dev_priv->parity_error_work);
+}
+
 static void snb_gt_irq_handler(struct drm_device *dev,
 			       struct drm_i915_private *dev_priv,
 			       u32 gt_iir)
@@ -394,6 +474,9 @@ static void snb_gt_irq_handler(struct drm_device *dev,
 		DRM_ERROR("GT error interrupt 0x%08x\n", gt_iir);
 		i915_handle_error(dev, false);
 	}
+
+	if (gt_iir & GT_GEN7_L3_PARITY_ERROR_INTERRUPT)
+		ivybridge_handle_parity_error(dev);
 }
 
 static void gen6_queue_rps_work(struct drm_i915_private *dev_priv,
@@ -429,15 +512,10 @@ static irqreturn_t valleyview_irq_handler(DRM_IRQ_ARGS)
 	unsigned long irqflags;
 	int pipe;
 	u32 pipe_stats[I915_MAX_PIPES];
-	u32 vblank_status;
-	int vblank = 0;
 	bool blc_event;
 
 	atomic_inc(&dev_priv->irq_received);
 
-	vblank_status = PIPE_START_VBLANK_INTERRUPT_STATUS |
-		PIPE_VBLANK_INTERRUPT_STATUS;
-
 	while (true) {
 		iir = I915_READ(VLV_IIR);
 		gt_iir = I915_READ(GTIIR);
@@ -467,6 +545,16 @@ static irqreturn_t valleyview_irq_handler(DRM_IRQ_ARGS)
 		}
 		spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
+		for_each_pipe(pipe) {
+			if (pipe_stats[pipe] & PIPE_VBLANK_INTERRUPT_STATUS)
+				drm_handle_vblank(dev, pipe);
+
+			if (pipe_stats[pipe] & PLANE_FLIPDONE_INT_STATUS_VLV) {
+				intel_prepare_page_flip(dev, pipe);
+				intel_finish_page_flip(dev, pipe);
+			}
+		}
+
 		/* Consume port.  Then clear IIR or we'll miss events */
 		if (iir & I915_DISPLAY_PORT_INTERRUPT) {
 			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
@@ -481,19 +569,6 @@ static irqreturn_t valleyview_irq_handler(DRM_IRQ_ARGS)
 			I915_READ(PORT_HOTPLUG_STAT);
 		}
 
-
-		if (iir & I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT) {
-			drm_handle_vblank(dev, 0);
-			vblank++;
-			intel_finish_page_flip(dev, 0);
-		}
-
-		if (iir & I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT) {
-			drm_handle_vblank(dev, 1);
-			vblank++;
-			intel_finish_page_flip(dev, 0);
-		}
-
 		if (pipe_stats[pipe] & PIPE_LEGACY_BLC_EVENT_STATUS)
 			blc_event = true;
 
@@ -991,6 +1066,7 @@ static void i915_record_ring_state(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (INTEL_INFO(dev)->gen >= 6) {
+		error->rc_psmi[ring->id] = I915_READ(ring->mmio_base + 0x50);
 		error->fault_reg[ring->id] = I915_READ(RING_FAULT_REG(ring));
 		error->semaphore_mboxes[ring->id][0]
 			= I915_READ(RING_SYNC_0(ring->mmio_base));
@@ -1104,6 +1180,7 @@ static void i915_capture_error_state(struct drm_device *dev)
 	kref_init(&error->ref);
 	error->eir = I915_READ(EIR);
 	error->pgtbl_er = I915_READ(PGTBL_ER);
+	error->ccid = I915_READ(CCID);
 
 	if (HAS_PCH_SPLIT(dev))
 		error->ier = I915_READ(DEIER) | I915_READ(GTIER);
@@ -1426,23 +1503,20 @@ static int valleyview_enable_vblank(struct drm_device *dev, int pipe)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	unsigned long irqflags;
-	u32 dpfl, imr;
+	u32 imr;
 
 	if (!i915_pipe_enabled(dev, pipe))
 		return -EINVAL;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	dpfl = I915_READ(VLV_DPFLIPSTAT);
 	imr = I915_READ(VLV_IMR);
-	if (pipe == 0) {
-		dpfl |= PIPEA_VBLANK_INT_EN;
+	if (pipe == 0)
 		imr &= ~I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT;
-	} else {
-		dpfl |= PIPEA_VBLANK_INT_EN;
+	else
 		imr &= ~I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
-	}
-	I915_WRITE(VLV_DPFLIPSTAT, dpfl);
 	I915_WRITE(VLV_IMR, imr);
+	i915_enable_pipestat(dev_priv, pipe,
+			     PIPE_START_VBLANK_INTERRUPT_ENABLE);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -1492,20 +1566,17 @@ static void valleyview_disable_vblank(struct drm_device *dev, int pipe)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 	unsigned long irqflags;
-	u32 dpfl, imr;
+	u32 imr;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	dpfl = I915_READ(VLV_DPFLIPSTAT);
+	i915_disable_pipestat(dev_priv, pipe,
+			      PIPE_START_VBLANK_INTERRUPT_ENABLE);
 	imr = I915_READ(VLV_IMR);
-	if (pipe == 0) {
-		dpfl &= ~PIPEA_VBLANK_INT_EN;
+	if (pipe == 0)
 		imr |= I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT;
-	} else {
-		dpfl &= ~PIPEB_VBLANK_INT_EN;
+	else
 		imr |= I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
-	}
 	I915_WRITE(VLV_IMR, imr);
-	I915_WRITE(VLV_DPFLIPSTAT, dpfl);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
@@ -1648,7 +1719,6 @@ static void ironlake_irq_preinstall(struct drm_device *dev)
 
 	atomic_set(&dev_priv->irq_received, 0);
 
-
 	I915_WRITE(HWSTAM, 0xeffe);
 
 	/* XXX hotplug from PCH */
@@ -1811,13 +1881,13 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 		   DE_PIPEA_VBLANK_IVB);
 	POSTING_READ(DEIER);
 
-	dev_priv->gt_irq_mask = ~0;
+	dev_priv->gt_irq_mask = ~GT_GEN7_L3_PARITY_ERROR_INTERRUPT;
 
 	I915_WRITE(GTIIR, I915_READ(GTIIR));
 	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
 
 	render_irqs = GT_USER_INTERRUPT | GEN6_BSD_USER_INTERRUPT |
-		GEN6_BLITTER_USER_INTERRUPT;
+		GEN6_BLITTER_USER_INTERRUPT | GT_GEN7_L3_PARITY_ERROR_INTERRUPT;
 	I915_WRITE(GTIER, render_irqs);
 	POSTING_READ(GTIER);
 
@@ -1840,16 +1910,24 @@ static int ivybridge_irq_postinstall(struct drm_device *dev)
 static int valleyview_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-	u32 render_irqs;
 	u32 enable_mask;
 	u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
+	u32 pipestat_enable = PLANE_FLIP_DONE_INT_EN_VLV;
 	u16 msid;
 
 	enable_mask = I915_DISPLAY_PORT_INTERRUPT;
-	enable_mask |= I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT |
+	enable_mask |= I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
+		I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT |
+		I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
 		I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
 
-	dev_priv->irq_mask = ~enable_mask;
+	/*
+	 *Leave vblank interrupts masked initially.  enable/disable will
+	 * toggle them based on usage.
+	 */
+	dev_priv->irq_mask = (~enable_mask) |
+		I915_DISPLAY_PIPE_A_VBLANK_INTERRUPT |
+		I915_DISPLAY_PIPE_B_VBLANK_INTERRUPT;
 
 	dev_priv->pipestat[0] = 0;
 	dev_priv->pipestat[1] = 0;
@@ -1868,26 +1946,27 @@ static int valleyview_irq_postinstall(struct drm_device *dev)
 	I915_WRITE(PIPESTAT(1), 0xffff);
 	POSTING_READ(VLV_IER);
 
+	i915_enable_pipestat(dev_priv, 0, pipestat_enable);
+	i915_enable_pipestat(dev_priv, 1, pipestat_enable);
+
 	I915_WRITE(VLV_IIR, 0xffffffff);
 	I915_WRITE(VLV_IIR, 0xffffffff);
 
-	render_irqs = GT_GEN6_BLT_FLUSHDW_NOTIFY_INTERRUPT |
-		GT_GEN6_BLT_CS_ERROR_INTERRUPT |
-		GT_GEN6_BLT_USER_INTERRUPT |
-		GT_GEN6_BSD_USER_INTERRUPT |
-		GT_GEN6_BSD_CS_ERROR_INTERRUPT |
-		GT_GEN7_L3_PARITY_ERROR_INTERRUPT |
-		GT_PIPE_NOTIFY |
-		GT_RENDER_CS_ERROR_INTERRUPT |
-		GT_SYNC_STATUS |
-		GT_USER_INTERRUPT;
-
-	dev_priv->gt_irq_mask = ~render_irqs;
+	dev_priv->gt_irq_mask = ~0;
 
 	I915_WRITE(GTIIR, I915_READ(GTIIR));
 	I915_WRITE(GTIIR, I915_READ(GTIIR));
-	I915_WRITE(GTIMR, 0);
-	I915_WRITE(GTIER, render_irqs);
+	I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
+	I915_WRITE(GTIER, GT_GEN6_BLT_FLUSHDW_NOTIFY_INTERRUPT |
+		   GT_GEN6_BLT_CS_ERROR_INTERRUPT |
+		   GT_GEN6_BLT_USER_INTERRUPT |
+		   GT_GEN6_BSD_USER_INTERRUPT |
+		   GT_GEN6_BSD_CS_ERROR_INTERRUPT |
+		   GT_GEN7_L3_PARITY_ERROR_INTERRUPT |
+		   GT_PIPE_NOTIFY |
+		   GT_RENDER_CS_ERROR_INTERRUPT |
+		   GT_SYNC_STATUS |
+		   GT_USER_INTERRUPT);
 	POSTING_READ(GTIER);
 
 	/* ack & enable invalid PTE error interrupts */
@@ -2166,9 +2245,9 @@ static int i915_irq_postinstall(struct drm_device *dev)
 			hotplug_en |= HDMIC_HOTPLUG_INT_EN;
 		if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
 			hotplug_en |= HDMID_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS)
+		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_I915)
 			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS)
+		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_I915)
 			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
 		if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
 			hotplug_en |= CRT_HOTPLUG_INT_EN;
@@ -2328,10 +2407,8 @@ static void i965_irq_preinstall(struct drm_device * dev)
 
 	atomic_set(&dev_priv->irq_received, 0);
 
-	if (I915_HAS_HOTPLUG(dev)) {
-		I915_WRITE(PORT_HOTPLUG_EN, 0);
-		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
-	}
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
 	I915_WRITE(HWSTAM, 0xeffe);
 	for_each_pipe(pipe)
@@ -2344,11 +2421,13 @@ static void i965_irq_preinstall(struct drm_device * dev)
 static int i965_irq_postinstall(struct drm_device *dev)
 {
 	drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+	u32 hotplug_en;
 	u32 enable_mask;
 	u32 error_mask;
 
 	/* Unmask the interrupts that we always want on. */
 	dev_priv->irq_mask = ~(I915_ASLE_INTERRUPT |
+			       I915_DISPLAY_PORT_INTERRUPT |
 			       I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
 			       I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
 			       I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
@@ -2364,13 +2443,6 @@ static int i965_irq_postinstall(struct drm_device *dev)
 	dev_priv->pipestat[0] = 0;
 	dev_priv->pipestat[1] = 0;
 
-	if (I915_HAS_HOTPLUG(dev)) {
-		/* Enable in IER... */
-		enable_mask |= I915_DISPLAY_PORT_INTERRUPT;
-		/* and unmask in IMR */
-		dev_priv->irq_mask &= ~I915_DISPLAY_PORT_INTERRUPT;
-	}
-
 	/*
 	 * Enable some error detection, note the instruction error mask
 	 * bit is reserved, so we leave it masked.
@@ -2390,36 +2462,40 @@ static int i965_irq_postinstall(struct drm_device *dev)
 	I915_WRITE(IER, enable_mask);
 	POSTING_READ(IER);
 
-	if (I915_HAS_HOTPLUG(dev)) {
-		u32 hotplug_en = I915_READ(PORT_HOTPLUG_EN);
-
-		/* Note HDMI and DP share bits */
-		if (dev_priv->hotplug_supported_mask & HDMIB_HOTPLUG_INT_STATUS)
-			hotplug_en |= HDMIB_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & HDMIC_HOTPLUG_INT_STATUS)
-			hotplug_en |= HDMIC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
-			hotplug_en |= HDMID_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS)
+	/* Note HDMI and DP share hotplug bits */
+	hotplug_en = 0;
+	if (dev_priv->hotplug_supported_mask & HDMIB_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMIB_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & HDMIC_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMIC_HOTPLUG_INT_EN;
+	if (dev_priv->hotplug_supported_mask & HDMID_HOTPLUG_INT_STATUS)
+		hotplug_en |= HDMID_HOTPLUG_INT_EN;
+	if (IS_G4X(dev)) {
+		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_G4X)
 			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS)
+		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_G4X)
 			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
-		if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
-			hotplug_en |= CRT_HOTPLUG_INT_EN;
+	} else {
+		if (dev_priv->hotplug_supported_mask & SDVOC_HOTPLUG_INT_STATUS_I965)
+			hotplug_en |= SDVOC_HOTPLUG_INT_EN;
+		if (dev_priv->hotplug_supported_mask & SDVOB_HOTPLUG_INT_STATUS_I965)
+			hotplug_en |= SDVOB_HOTPLUG_INT_EN;
+	}
+	if (dev_priv->hotplug_supported_mask & CRT_HOTPLUG_INT_STATUS) {
+		hotplug_en |= CRT_HOTPLUG_INT_EN;
 
-			/* Programming the CRT detection parameters tends
-			   to generate a spurious hotplug event about three
-			   seconds later.  So just do it once.
-			*/
-			if (IS_G4X(dev))
-				hotplug_en |= CRT_HOTPLUG_ACTIVATION_PERIOD_64;
-			hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
-		}
+		/* Programming the CRT detection parameters tends
+		   to generate a spurious hotplug event about three
+		   seconds later.  So just do it once.
+		   */
+		if (IS_G4X(dev))
+			hotplug_en |= CRT_HOTPLUG_ACTIVATION_PERIOD_64;
+		hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
+	}
 
-		/* Ignore TV since it's buggy */
+	/* Ignore TV since it's buggy */
 
-		I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
-	}
+	I915_WRITE(PORT_HOTPLUG_EN, hotplug_en);
 
 	intel_opregion_enable_asle(dev);
 
@@ -2477,8 +2553,7 @@ static irqreturn_t i965_irq_handler(DRM_IRQ_ARGS)
 		ret = IRQ_HANDLED;
 
 		/* Consume port.  Then clear IIR or we'll miss events */
-		if ((I915_HAS_HOTPLUG(dev)) &&
-		    (iir & I915_DISPLAY_PORT_INTERRUPT)) {
+		if (iir & I915_DISPLAY_PORT_INTERRUPT) {
 			u32 hotplug_status = I915_READ(PORT_HOTPLUG_STAT);
 
 			DRM_DEBUG_DRIVER("hotplug event received, stat 0x%08x\n",
@@ -2551,10 +2626,8 @@ static void i965_irq_uninstall(struct drm_device * dev)
 	if (!dev_priv)
 		return;
 
-	if (I915_HAS_HOTPLUG(dev)) {
-		I915_WRITE(PORT_HOTPLUG_EN, 0);
-		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
-	}
+	I915_WRITE(PORT_HOTPLUG_EN, 0);
+	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
 	I915_WRITE(HWSTAM, 0xffffffff);
 	for_each_pipe(pipe)
@@ -2575,6 +2648,7 @@ void intel_irq_init(struct drm_device *dev)
 	INIT_WORK(&dev_priv->hotplug_work, i915_hotplug_work_func);
 	INIT_WORK(&dev_priv->error_work, i915_error_work_func);
 	INIT_WORK(&dev_priv->rps_work, gen6_pm_rps_work);
+	INIT_WORK(&dev_priv->parity_error_work, ivybridge_parity_work);
 
 	dev->driver->get_vblank_counter = i915_get_vblank_counter;
 	dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 48d5e8e051c..acc99b21e0b 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -217,6 +217,9 @@
 #define   MI_DISPLAY_FLIP_IVB_SPRITE_B (3 << 19)
 #define   MI_DISPLAY_FLIP_IVB_PLANE_C  (4 << 19)
 #define   MI_DISPLAY_FLIP_IVB_SPRITE_C (5 << 19)
+#define MI_ARB_ON_OFF		MI_INSTR(0x08, 0)
+#define   MI_ARB_ENABLE			(1<<0)
+#define   MI_ARB_DISABLE		(0<<0)
 
 #define MI_SET_CONTEXT		MI_INSTR(0x18, 0)
 #define   MI_MM_SPACE_GTT		(1<<8)
@@ -299,6 +302,7 @@
 #define   DISPLAY_PLANE_B           (1<<20)
 #define GFX_OP_PIPE_CONTROL(len)	((0x3<<29)|(0x3<<27)|(0x2<<24)|(len-2))
 #define   PIPE_CONTROL_CS_STALL				(1<<20)
+#define   PIPE_CONTROL_TLB_INVALIDATE			(1<<18)
 #define   PIPE_CONTROL_QW_WRITE				(1<<14)
 #define   PIPE_CONTROL_DEPTH_STALL			(1<<13)
 #define   PIPE_CONTROL_WRITE_FLUSH			(1<<12)
@@ -686,10 +690,10 @@
 #define   GEN6_BLITTER_FBC_NOTIFY			(1<<3)
 
 #define GEN6_BSD_SLEEP_PSMI_CONTROL	0x12050
-#define   GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_MODIFY_MASK	(1 << 16)
-#define   GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_DISABLE		(1 << 0)
-#define   GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_ENABLE		0
-#define   GEN6_BSD_SLEEP_PSMI_CONTROL_IDLE_INDICATOR			(1 << 3)
+#define   GEN6_BSD_SLEEP_MSG_DISABLE	(1 << 0)
+#define   GEN6_BSD_SLEEP_FLUSH_DISABLE	(1 << 2)
+#define   GEN6_BSD_SLEEP_INDICATOR	(1 << 3)
+#define   GEN6_BSD_GO_INDICATOR		(1 << 4)
 
 #define GEN6_BSD_HWSTAM			0x12098
 #define GEN6_BSD_IMR			0x120a8
@@ -908,6 +912,7 @@
 #define   DPLL_P2_CLOCK_DIV_MASK	0x03000000 /* i915 */
 #define   DPLL_FPA01_P1_POST_DIV_MASK	0x00ff0000 /* i915 */
 #define   DPLL_FPA01_P1_POST_DIV_MASK_PINEVIEW	0x00ff8000 /* Pineview */
+#define   DPLL_LOCK_VLV			(1<<15)
 #define   DPLL_INTEGRATED_CLOCK_VLV	(1<<13)
 
 #define SRX_INDEX		0x3c4
@@ -1453,6 +1458,10 @@
 #define DDRMPLL1		0X12c20
 #define PEG_BAND_GAP_DATA	0x14d68
 
+#define GEN6_GT_THREAD_STATUS_REG 0x13805c
+#define GEN6_GT_THREAD_STATUS_CORE_MASK 0x7
+#define GEN6_GT_THREAD_STATUS_CORE_MASK_HSW (0x7 | (0x07 << 16))
+
 #define GEN6_GT_PERF_STATUS	0x145948
 #define GEN6_RP_STATE_LIMITS	0x145994
 #define GEN6_RP_STATE_CAP	0x145998
@@ -1462,6 +1471,31 @@
  */
 #define CCID			0x2180
 #define   CCID_EN		(1<<0)
+#define CXT_SIZE		0x21a0
+#define GEN6_CXT_POWER_SIZE(cxt_reg)	((cxt_reg >> 24) & 0x3f)
+#define GEN6_CXT_RING_SIZE(cxt_reg)	((cxt_reg >> 18) & 0x3f)
+#define GEN6_CXT_RENDER_SIZE(cxt_reg)	((cxt_reg >> 12) & 0x3f)
+#define GEN6_CXT_EXTENDED_SIZE(cxt_reg)	((cxt_reg >> 6) & 0x3f)
+#define GEN6_CXT_PIPELINE_SIZE(cxt_reg)	((cxt_reg >> 0) & 0x3f)
+#define GEN6_CXT_TOTAL_SIZE(cxt_reg)	(GEN6_CXT_POWER_SIZE(cxt_reg) + \
+					GEN6_CXT_RING_SIZE(cxt_reg) + \
+					GEN6_CXT_RENDER_SIZE(cxt_reg) + \
+					GEN6_CXT_EXTENDED_SIZE(cxt_reg) + \
+					GEN6_CXT_PIPELINE_SIZE(cxt_reg))
+#define GEN7_CXT_SIZE		0x21a8
+#define GEN7_CXT_POWER_SIZE(ctx_reg)	((ctx_reg >> 25) & 0x7f)
+#define GEN7_CXT_RING_SIZE(ctx_reg)	((ctx_reg >> 22) & 0x7)
+#define GEN7_CXT_RENDER_SIZE(ctx_reg)	((ctx_reg >> 16) & 0x3f)
+#define GEN7_CXT_EXTENDED_SIZE(ctx_reg)	((ctx_reg >> 9) & 0x7f)
+#define GEN7_CXT_GT1_SIZE(ctx_reg)	((ctx_reg >> 6) & 0x7)
+#define GEN7_CXT_VFSTATE_SIZE(ctx_reg)	((ctx_reg >> 0) & 0x3f)
+#define GEN7_CXT_TOTAL_SIZE(ctx_reg)	(GEN7_CXT_POWER_SIZE(ctx_reg) + \
+					 GEN7_CXT_RING_SIZE(ctx_reg) + \
+					 GEN7_CXT_RENDER_SIZE(ctx_reg) + \
+					 GEN7_CXT_EXTENDED_SIZE(ctx_reg) + \
+					 GEN7_CXT_GT1_SIZE(ctx_reg) + \
+					 GEN7_CXT_VFSTATE_SIZE(ctx_reg))
+
 /*
  * Overlay regs
  */
@@ -1566,20 +1600,34 @@
 #define CRT_HOTPLUG_DETECT_VOLTAGE_475MV	(1 << 2)
 
 #define PORT_HOTPLUG_STAT	0x61114
-#define   HDMIB_HOTPLUG_INT_STATUS		(1 << 29)
-#define   DPB_HOTPLUG_INT_STATUS		(1 << 29)
-#define   HDMIC_HOTPLUG_INT_STATUS		(1 << 28)
-#define   DPC_HOTPLUG_INT_STATUS		(1 << 28)
-#define   HDMID_HOTPLUG_INT_STATUS		(1 << 27)
-#define   DPD_HOTPLUG_INT_STATUS		(1 << 27)
+/* HDMI/DP bits are gen4+ */
+#define   DPB_HOTPLUG_LIVE_STATUS               (1 << 29)
+#define   DPC_HOTPLUG_LIVE_STATUS               (1 << 28)
+#define   DPD_HOTPLUG_LIVE_STATUS               (1 << 27)
+#define   DPD_HOTPLUG_INT_STATUS		(3 << 21)
+#define   DPC_HOTPLUG_INT_STATUS		(3 << 19)
+#define   DPB_HOTPLUG_INT_STATUS		(3 << 17)
+/* HDMI bits are shared with the DP bits */
+#define   HDMIB_HOTPLUG_LIVE_STATUS             (1 << 29)
+#define   HDMIC_HOTPLUG_LIVE_STATUS             (1 << 28)
+#define   HDMID_HOTPLUG_LIVE_STATUS             (1 << 27)
+#define   HDMID_HOTPLUG_INT_STATUS		(3 << 21)
+#define   HDMIC_HOTPLUG_INT_STATUS		(3 << 19)
+#define   HDMIB_HOTPLUG_INT_STATUS		(3 << 17)
+/* CRT/TV common between gen3+ */
 #define   CRT_HOTPLUG_INT_STATUS		(1 << 11)
 #define   TV_HOTPLUG_INT_STATUS			(1 << 10)
 #define   CRT_HOTPLUG_MONITOR_MASK		(3 << 8)
 #define   CRT_HOTPLUG_MONITOR_COLOR		(3 << 8)
 #define   CRT_HOTPLUG_MONITOR_MONO		(2 << 8)
 #define   CRT_HOTPLUG_MONITOR_NONE		(0 << 8)
-#define   SDVOC_HOTPLUG_INT_STATUS		(1 << 7)
-#define   SDVOB_HOTPLUG_INT_STATUS		(1 << 6)
+/* SDVO is different across gen3/4 */
+#define   SDVOC_HOTPLUG_INT_STATUS_G4X		(1 << 3)
+#define   SDVOB_HOTPLUG_INT_STATUS_G4X		(1 << 2)
+#define   SDVOC_HOTPLUG_INT_STATUS_I965		(3 << 4)
+#define   SDVOB_HOTPLUG_INT_STATUS_I965		(3 << 2)
+#define   SDVOC_HOTPLUG_INT_STATUS_I915		(1 << 7)
+#define   SDVOB_HOTPLUG_INT_STATUS_I915		(1 << 6)
 
 /* SDVO port control */
 #define SDVOB			0x61140
@@ -1711,8 +1759,10 @@
 #define   VIDEO_DIP_PORT_C		(2 << 29)
 #define   VIDEO_DIP_PORT_D		(3 << 29)
 #define   VIDEO_DIP_PORT_MASK		(3 << 29)
+#define   VIDEO_DIP_ENABLE_GCP		(1 << 25)
 #define   VIDEO_DIP_ENABLE_AVI		(1 << 21)
 #define   VIDEO_DIP_ENABLE_VENDOR	(2 << 21)
+#define   VIDEO_DIP_ENABLE_GAMUT	(4 << 21)
 #define   VIDEO_DIP_ENABLE_SPD		(8 << 21)
 #define   VIDEO_DIP_SELECT_AVI		(0 << 19)
 #define   VIDEO_DIP_SELECT_VENDOR	(1 << 19)
@@ -1723,7 +1773,11 @@
 #define   VIDEO_DIP_FREQ_2VSYNC		(2 << 16)
 #define   VIDEO_DIP_FREQ_MASK		(3 << 16)
 /* HSW and later: */
+#define   VIDEO_DIP_ENABLE_VSC_HSW	(1 << 20)
+#define   VIDEO_DIP_ENABLE_GCP_HSW	(1 << 16)
 #define   VIDEO_DIP_ENABLE_AVI_HSW	(1 << 12)
+#define   VIDEO_DIP_ENABLE_VS_HSW	(1 << 8)
+#define   VIDEO_DIP_ENABLE_GMP_HSW	(1 << 4)
 #define   VIDEO_DIP_ENABLE_SPD_HSW	(1 << 0)
 
 /* Panel power sequencing */
@@ -1795,18 +1849,35 @@
 #define PFIT_AUTO_RATIOS 0x61238
 
 /* Backlight control */
-#define BLC_PWM_CTL		0x61254
-#define   BACKLIGHT_MODULATION_FREQ_SHIFT		(17)
 #define BLC_PWM_CTL2		0x61250 /* 965+ only */
-#define   BLM_COMBINATION_MODE (1 << 30)
+#define   BLM_PWM_ENABLE		(1 << 31)
+#define   BLM_COMBINATION_MODE		(1 << 30) /* gen4 only */
+#define   BLM_PIPE_SELECT		(1 << 29)
+#define   BLM_PIPE_SELECT_IVB		(3 << 29)
+#define   BLM_PIPE_A			(0 << 29)
+#define   BLM_PIPE_B			(1 << 29)
+#define   BLM_PIPE_C			(2 << 29) /* ivb + */
+#define   BLM_PIPE(pipe)		((pipe) << 29)
+#define   BLM_POLARITY_I965		(1 << 28) /* gen4 only */
+#define   BLM_PHASE_IN_INTERUPT_STATUS	(1 << 26)
+#define   BLM_PHASE_IN_ENABLE		(1 << 25)
+#define   BLM_PHASE_IN_INTERUPT_ENABL	(1 << 24)
+#define   BLM_PHASE_IN_TIME_BASE_SHIFT	(16)
+#define   BLM_PHASE_IN_TIME_BASE_MASK	(0xff << 16)
+#define   BLM_PHASE_IN_COUNT_SHIFT	(8)
+#define   BLM_PHASE_IN_COUNT_MASK	(0xff << 8)
+#define   BLM_PHASE_IN_INCR_SHIFT	(0)
+#define   BLM_PHASE_IN_INCR_MASK	(0xff << 0)
+#define BLC_PWM_CTL		0x61254
 /*
  * This is the most significant 15 bits of the number of backlight cycles in a
  * complete cycle of the modulated backlight control.
  *
  * The actual value is this field multiplied by two.
  */
-#define   BACKLIGHT_MODULATION_FREQ_MASK		(0x7fff << 17)
-#define   BLM_LEGACY_MODE				(1 << 16)
+#define   BACKLIGHT_MODULATION_FREQ_SHIFT	(17)
+#define   BACKLIGHT_MODULATION_FREQ_MASK	(0x7fff << 17)
+#define   BLM_LEGACY_MODE			(1 << 16) /* gen2 only */
 /*
  * This is the number of cycles out of the backlight modulation cycle for which
  * the backlight is on.
@@ -1816,9 +1887,24 @@
  */
 #define   BACKLIGHT_DUTY_CYCLE_SHIFT		(0)
 #define   BACKLIGHT_DUTY_CYCLE_MASK		(0xffff)
+#define   BACKLIGHT_DUTY_CYCLE_MASK_PNV		(0xfffe)
+#define   BLM_POLARITY_PNV			(1 << 0) /* pnv only */
 
 #define BLC_HIST_CTL		0x61260
 
+/* New registers for PCH-split platforms. Safe where new bits show up, the
+ * register layout machtes with gen4 BLC_PWM_CTL[12]. */
+#define BLC_PWM_CPU_CTL2	0x48250
+#define BLC_PWM_CPU_CTL		0x48254
+
+/* PCH CTL1 is totally different, all but the below bits are reserved. CTL2 is
+ * like the normal CTL from gen4 and earlier. Hooray for confusing naming. */
+#define BLC_PWM_PCH_CTL1	0xc8250
+#define   BLM_PCH_PWM_ENABLE			(1 << 31)
+#define   BLM_PCH_OVERRIDE_ENABLE		(1 << 30)
+#define   BLM_PCH_POLARITY			(1 << 29)
+#define BLC_PWM_PCH_CTL2	0xc8254
+
 /* TV port control */
 #define TV_CTL			0x68000
 /** Enables the TV encoder */
@@ -2583,13 +2669,13 @@
 #define PIPESTAT(pipe) _PIPE(pipe, _PIPEASTAT, _PIPEBSTAT)
 
 #define VLV_DPFLIPSTAT				0x70028
-#define   PIPEB_LINE_COMPARE_STATUS		(1<<29)
+#define   PIPEB_LINE_COMPARE_INT_EN		(1<<29)
 #define   PIPEB_HLINE_INT_EN			(1<<28)
 #define   PIPEB_VBLANK_INT_EN			(1<<27)
 #define   SPRITED_FLIPDONE_INT_EN		(1<<26)
 #define   SPRITEC_FLIPDONE_INT_EN		(1<<25)
 #define   PLANEB_FLIPDONE_INT_EN		(1<<24)
-#define   PIPEA_LINE_COMPARE_STATUS		(1<<21)
+#define   PIPEA_LINE_COMPARE_INT_EN		(1<<21)
 #define   PIPEA_HLINE_INT_EN			(1<<20)
 #define   PIPEA_VBLANK_INT_EN			(1<<19)
 #define   SPRITEB_FLIPDONE_INT_EN		(1<<18)
@@ -2897,13 +2983,14 @@
 #define DSPSIZE(plane) _PIPE(plane, _DSPASIZE, _DSPBSIZE)
 #define DSPSURF(plane) _PIPE(plane, _DSPASURF, _DSPBSURF)
 #define DSPTILEOFF(plane) _PIPE(plane, _DSPATILEOFF, _DSPBTILEOFF)
+#define DSPLINOFF(plane) DSPADDR(plane)
 
 /* Display/Sprite base address macros */
 #define DISP_BASEADDR_MASK	(0xfffff000)
 #define I915_LO_DISPBASE(val)	(val & ~DISP_BASEADDR_MASK)
 #define I915_HI_DISPBASE(val)	(val & DISP_BASEADDR_MASK)
 #define I915_MODIFY_DISPBASE(reg, gfx_addr) \
-		(I915_WRITE(reg, gfx_addr | I915_LO_DISPBASE(I915_READ(reg))))
+		(I915_WRITE((reg), (gfx_addr) | I915_LO_DISPBASE(I915_READ(reg))))
 
 /* VBIOS flags */
 #define SWF00			0x71410
@@ -3771,6 +3858,9 @@
 #define _FDI_RXA_TUSIZE2         0xf0038
 #define _FDI_RXB_TUSIZE1         0xf1030
 #define _FDI_RXB_TUSIZE2         0xf1038
+#define  FDI_RX_TP1_TO_TP2_48	(2<<20)
+#define  FDI_RX_TP1_TO_TP2_64	(3<<20)
+#define  FDI_RX_FDI_DELAY_90	(0x90<<0)
 #define FDI_RX_MISC(pipe) _PIPE(pipe, _FDI_RXA_MISC, _FDI_RXB_MISC)
 #define FDI_RX_TUSIZE1(pipe) _PIPE(pipe, _FDI_RXA_TUSIZE1, _FDI_RXB_TUSIZE1)
 #define FDI_RX_TUSIZE2(pipe) _PIPE(pipe, _FDI_RXA_TUSIZE2, _FDI_RXB_TUSIZE2)
@@ -3824,7 +3914,6 @@
 #define  ADPA_CRT_HOTPLUG_FORCE_TRIGGER (1<<16)
 
 /* or SDVOB */
-#define VLV_HDMIB 0x61140
 #define HDMIB   0xe1140
 #define  PORT_ENABLE    (1 << 31)
 #define  TRANSCODER(pipe)       ((pipe) << 30)
@@ -3855,20 +3944,18 @@
 #define PCH_LVDS	0xe1180
 #define  LVDS_DETECTED	(1 << 1)
 
-#define BLC_PWM_CPU_CTL2	0x48250
-#define  PWM_ENABLE		(1 << 31)
-#define  PWM_PIPE_A		(0 << 29)
-#define  PWM_PIPE_B		(1 << 29)
-#define BLC_PWM_CPU_CTL		0x48254
+/* vlv has 2 sets of panel control regs. */
+#define PIPEA_PP_STATUS         0x61200
+#define PIPEA_PP_CONTROL        0x61204
+#define PIPEA_PP_ON_DELAYS      0x61208
+#define PIPEA_PP_OFF_DELAYS     0x6120c
+#define PIPEA_PP_DIVISOR        0x61210
 
-#define BLC_PWM_PCH_CTL1	0xc8250
-#define  PWM_PCH_ENABLE		(1 << 31)
-#define  PWM_POLARITY_ACTIVE_LOW	(1 << 29)
-#define  PWM_POLARITY_ACTIVE_HIGH	(0 << 29)
-#define  PWM_POLARITY_ACTIVE_LOW2	(1 << 28)
-#define  PWM_POLARITY_ACTIVE_HIGH2	(0 << 28)
-
-#define BLC_PWM_PCH_CTL2	0xc8254
+#define PIPEB_PP_STATUS         0x61300
+#define PIPEB_PP_CONTROL        0x61304
+#define PIPEB_PP_ON_DELAYS      0x61308
+#define PIPEB_PP_OFF_DELAYS     0x6130c
+#define PIPEB_PP_DIVISOR        0x61310
 
 #define PCH_PP_STATUS		0xc7200
 #define PCH_PP_CONTROL		0xc7204
@@ -3992,6 +4079,7 @@
 #define  FORCEWAKE				0xA18C
 #define  FORCEWAKE_VLV				0x1300b0
 #define  FORCEWAKE_ACK_VLV			0x1300b4
+#define  FORCEWAKE_ACK_HSW			0x130044
 #define  FORCEWAKE_ACK				0x130090
 #define  FORCEWAKE_MT				0xa188 /* multi-threaded */
 #define  FORCEWAKE_MT_ACK			0x130040
@@ -4012,10 +4100,15 @@
 # define GEN6_CSUNIT_CLOCK_GATE_DISABLE			(1 << 7)
 
 #define GEN6_UCGCTL2				0x9404
+# define GEN7_VDSUNIT_CLOCK_GATE_DISABLE		(1 << 30)
+# define GEN7_TDLUNIT_CLOCK_GATE_DISABLE		(1 << 22)
 # define GEN6_RCZUNIT_CLOCK_GATE_DISABLE		(1 << 13)
 # define GEN6_RCPBUNIT_CLOCK_GATE_DISABLE		(1 << 12)
 # define GEN6_RCCUNIT_CLOCK_GATE_DISABLE		(1 << 11)
 
+#define GEN7_UCGCTL4				0x940c
+#define  GEN7_L3BANK2X_CLOCK_GATE_DISABLE	(1<<25)
+
 #define GEN6_RPNSWREQ				0xA008
 #define   GEN6_TURBO_DISABLE			(1<<31)
 #define   GEN6_FREQUENCY(x)			((x)<<25)
@@ -4047,6 +4140,7 @@
 #define   GEN6_RP_UP_IDLE_MIN			(0x1<<3)
 #define   GEN6_RP_UP_BUSY_AVG			(0x2<<3)
 #define   GEN6_RP_UP_BUSY_CONT			(0x4<<3)
+#define   GEN7_RP_DOWN_IDLE_AVG			(0x2<<0)
 #define   GEN6_RP_DOWN_IDLE_CONT		(0x1<<0)
 #define GEN6_RP_UP_THRESHOLD			0xA02C
 #define GEN6_RP_DOWN_THRESHOLD			0xA030
@@ -4111,6 +4205,26 @@
 #define   GEN6_RC6			3
 #define   GEN6_RC7			4
 
+#define GEN7_MISCCPCTL			(0x9424)
+#define   GEN7_DOP_CLOCK_GATE_ENABLE	(1<<0)
+
+/* IVYBRIDGE DPF */
+#define GEN7_L3CDERRST1			0xB008 /* L3CD Error Status 1 */
+#define   GEN7_L3CDERRST1_ROW_MASK	(0x7ff<<14)
+#define   GEN7_PARITY_ERROR_VALID	(1<<13)
+#define   GEN7_L3CDERRST1_BANK_MASK	(3<<11)
+#define   GEN7_L3CDERRST1_SUBBANK_MASK	(7<<8)
+#define GEN7_PARITY_ERROR_ROW(reg) \
+		((reg & GEN7_L3CDERRST1_ROW_MASK) >> 14)
+#define GEN7_PARITY_ERROR_BANK(reg) \
+		((reg & GEN7_L3CDERRST1_BANK_MASK) >> 11)
+#define GEN7_PARITY_ERROR_SUBBANK(reg) \
+		((reg & GEN7_L3CDERRST1_SUBBANK_MASK) >> 8)
+#define   GEN7_L3CDERRST1_ENABLE	(1<<7)
+
+#define GEN7_L3LOG_BASE			0xB070
+#define GEN7_L3LOG_SIZE			0x80
+
 #define G4X_AUD_VID_DID			0x62020
 #define INTEL_AUDIO_DEVCL		0x808629FB
 #define INTEL_AUDIO_DEVBLC		0x80862801
@@ -4177,7 +4291,7 @@
 					PIPE_DDI_FUNC_CTL_B)
 #define  PIPE_DDI_FUNC_ENABLE		(1<<31)
 /* Those bits are ignored by pipe EDP since it can only connect to DDI A */
-#define  PIPE_DDI_PORT_MASK				(0xf<<28)
+#define  PIPE_DDI_PORT_MASK			(7<<28)
 #define  PIPE_DDI_SELECT_PORT(x)		((x)<<28)
 #define  PIPE_DDI_MODE_SELECT_HDMI		(0<<24)
 #define  PIPE_DDI_MODE_SELECT_DVI		(1<<24)
@@ -4335,7 +4449,7 @@
 #define PIPE_WM_LINETIME_B		0x45274
 #define PIPE_WM_LINETIME(pipe) _PIPE(pipe, \
 					PIPE_WM_LINETIME_A, \
-					PIPE_WM_LINETIME_A)
+					PIPE_WM_LINETIME_B)
 #define   PIPE_WM_LINETIME_MASK		(0x1ff)
 #define   PIPE_WM_LINETIME_TIME(x)			((x))
 #define   PIPE_WM_LINETIME_IPS_LINETIME_MASK	(0x1ff<<16)
@@ -4347,4 +4461,9 @@
 #define  SFUSE_STRAP_DDIC_DETECTED	(1<<1)
 #define  SFUSE_STRAP_DDID_DETECTED	(1<<0)
 
+#define WM_DBG				0x45280
+#define  WM_DBG_DISALLOW_MULTIPLE_LP	(1<<0)
+#define  WM_DBG_DISALLOW_MAXFIFO	(1<<1)
+#define  WM_DBG_DISALLOW_SPRITE		(1<<2)
+
 #endif /* _I915_REG_H_ */
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index a748e5cabe1..4776ccf1b3c 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -828,10 +828,7 @@ int i915_save_state(struct drm_device *dev)
 		dev_priv->saveIMR = I915_READ(IMR);
 	}
 
-	if (IS_IRONLAKE_M(dev))
-		ironlake_disable_drps(dev);
-	if (INTEL_INFO(dev)->gen >= 6)
-		gen6_disable_rps(dev);
+	intel_disable_gt_powersave(dev);
 
 	/* Cache mode state */
 	dev_priv->saveCACHE_MODE_0 = I915_READ(CACHE_MODE_0);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index 79f83445afa..2f5388af8df 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -29,6 +29,7 @@
 #include <linux/module.h>
 #include <linux/stat.h>
 #include <linux/sysfs.h>
+#include "intel_drv.h"
 #include "i915_drv.h"
 
 static u32 calc_residency(struct drm_device *dev, const u32 reg)
@@ -92,20 +93,134 @@ static struct attribute_group rc6_attr_group = {
 	.attrs =  rc6_attrs
 };
 
-void i915_setup_sysfs(struct drm_device *dev)
+static int l3_access_valid(struct drm_device *dev, loff_t offset)
+{
+	if (!IS_IVYBRIDGE(dev))
+		return -EPERM;
+
+	if (offset % 4 != 0)
+		return -EINVAL;
+
+	if (offset >= GEN7_L3LOG_SIZE)
+		return -ENXIO;
+
+	return 0;
+}
+
+static ssize_t
+i915_l3_read(struct file *filp, struct kobject *kobj,
+	     struct bin_attribute *attr, char *buf,
+	     loff_t offset, size_t count)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	struct drm_device *drm_dev = dminor->dev;
+	struct drm_i915_private *dev_priv = drm_dev->dev_private;
+	uint32_t misccpctl;
+	int i, ret;
+
+	ret = l3_access_valid(drm_dev, offset);
+	if (ret)
+		return ret;
+
+	ret = i915_mutex_lock_interruptible(drm_dev);
+	if (ret)
+		return ret;
+
+	misccpctl = I915_READ(GEN7_MISCCPCTL);
+	I915_WRITE(GEN7_MISCCPCTL, misccpctl & ~GEN7_DOP_CLOCK_GATE_ENABLE);
+
+	for (i = offset; count >= 4 && i < GEN7_L3LOG_SIZE; i += 4, count -= 4)
+		*((uint32_t *)(&buf[i])) = I915_READ(GEN7_L3LOG_BASE + i);
+
+	I915_WRITE(GEN7_MISCCPCTL, misccpctl);
+
+	mutex_unlock(&drm_dev->struct_mutex);
+
+	return i - offset;
+}
+
+static ssize_t
+i915_l3_write(struct file *filp, struct kobject *kobj,
+	      struct bin_attribute *attr, char *buf,
+	      loff_t offset, size_t count)
 {
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct drm_minor *dminor = container_of(dev, struct drm_minor, kdev);
+	struct drm_device *drm_dev = dminor->dev;
+	struct drm_i915_private *dev_priv = drm_dev->dev_private;
+	u32 *temp = NULL; /* Just here to make handling failures easy */
 	int ret;
 
-	/* ILK doesn't have any residency information */
-	if (INTEL_INFO(dev)->gen < 6)
-		return;
+	ret = l3_access_valid(drm_dev, offset);
+	if (ret)
+		return ret;
 
-	ret = sysfs_merge_group(&dev->primary->kdev.kobj, &rc6_attr_group);
+	ret = i915_mutex_lock_interruptible(drm_dev);
 	if (ret)
-		DRM_ERROR("sysfs setup failed\n");
+		return ret;
+
+	if (!dev_priv->mm.l3_remap_info) {
+		temp = kzalloc(GEN7_L3LOG_SIZE, GFP_KERNEL);
+		if (!temp) {
+			mutex_unlock(&drm_dev->struct_mutex);
+			return -ENOMEM;
+		}
+	}
+
+	ret = i915_gpu_idle(drm_dev);
+	if (ret) {
+		kfree(temp);
+		mutex_unlock(&drm_dev->struct_mutex);
+		return ret;
+	}
+
+	/* TODO: Ideally we really want a GPU reset here to make sure errors
+	 * aren't propagated. Since I cannot find a stable way to reset the GPU
+	 * at this point it is left as a TODO.
+	*/
+	if (temp)
+		dev_priv->mm.l3_remap_info = temp;
+
+	memcpy(dev_priv->mm.l3_remap_info + (offset/4),
+	       buf + (offset/4),
+	       count);
+
+	i915_gem_l3_remap(drm_dev);
+
+	mutex_unlock(&drm_dev->struct_mutex);
+
+	return count;
+}
+
+static struct bin_attribute dpf_attrs = {
+	.attr = {.name = "l3_parity", .mode = (S_IRUSR | S_IWUSR)},
+	.size = GEN7_L3LOG_SIZE,
+	.read = i915_l3_read,
+	.write = i915_l3_write,
+	.mmap = NULL
+};
+
+void i915_setup_sysfs(struct drm_device *dev)
+{
+	int ret;
+
+	if (INTEL_INFO(dev)->gen >= 6) {
+		ret = sysfs_merge_group(&dev->primary->kdev.kobj,
+					&rc6_attr_group);
+		if (ret)
+			DRM_ERROR("RC6 residency sysfs setup failed\n");
+	}
+
+	if (IS_IVYBRIDGE(dev)) {
+		ret = device_create_bin_file(&dev->primary->kdev, &dpf_attrs);
+		if (ret)
+			DRM_ERROR("l3 parity sysfs setup failed\n");
+	}
 }
 
 void i915_teardown_sysfs(struct drm_device *dev)
 {
+	device_remove_bin_file(&dev->primary->kdev,  &dpf_attrs);
 	sysfs_unmerge_group(&dev->primary->kdev.kobj, &rc6_attr_group);
 }
diff --git a/drivers/gpu/drm/i915/i915_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index dac7bba4d9d..fe90b3a84a6 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -311,9 +311,33 @@ DEFINE_EVENT(i915_gem_request, i915_gem_request_retire,
 	    TP_ARGS(ring, seqno)
 );
 
-DEFINE_EVENT(i915_gem_request, i915_gem_request_wait_begin,
+TRACE_EVENT(i915_gem_request_wait_begin,
 	    TP_PROTO(struct intel_ring_buffer *ring, u32 seqno),
-	    TP_ARGS(ring, seqno)
+	    TP_ARGS(ring, seqno),
+
+	    TP_STRUCT__entry(
+			     __field(u32, dev)
+			     __field(u32, ring)
+			     __field(u32, seqno)
+			     __field(bool, blocking)
+			     ),
+
+	    /* NB: the blocking information is racy since mutex_is_locked
+	     * doesn't check that the current thread holds the lock. The only
+	     * other option would be to pass the boolean information of whether
+	     * or not the class was blocking down through the stack which is
+	     * less desirable.
+	     */
+	    TP_fast_assign(
+			   __entry->dev = ring->dev->primary->index;
+			   __entry->ring = ring->id;
+			   __entry->seqno = seqno;
+			   __entry->blocking = mutex_is_locked(&ring->dev->struct_mutex);
+			   ),
+
+	    TP_printk("dev=%u, ring=%u, seqno=%u, blocking=%s",
+		      __entry->dev, __entry->ring, __entry->seqno,
+		      __entry->blocking ?  "yes (NB)" : "no")
 );
 
 DEFINE_EVENT(i915_gem_request, i915_gem_request_wait_end,
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index 353459362f6..8c6074154bf 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -692,7 +692,7 @@ static const struct dmi_system_id intel_no_opregion_vbt[] = {
  *
  * Returns 0 on success, nonzero on failure.
  */
-bool
+int
 intel_parse_bios(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
diff --git a/drivers/gpu/drm/i915/intel_bios.h b/drivers/gpu/drm/i915/intel_bios.h
index dbda6e3bdf0..31c2107e782 100644
--- a/drivers/gpu/drm/i915/intel_bios.h
+++ b/drivers/gpu/drm/i915/intel_bios.h
@@ -476,7 +476,7 @@ struct bdb_edp {
 } __attribute__ ((packed));
 
 void intel_setup_bios(struct drm_device *dev);
-bool intel_parse_bios(struct drm_device *dev);
+int intel_parse_bios(struct drm_device *dev);
 
 /*
  * Driver<->VBIOS interaction occurs through scratch bits in
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 75a70c46ef1..7ed4a41c396 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -88,6 +88,9 @@ static void gmch_crt_dpms(struct drm_encoder *encoder, int mode)
 	temp &= ~(ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE);
 	temp &= ~ADPA_DAC_ENABLE;
 
+	if (IS_VALLEYVIEW(dev) && mode != DRM_MODE_DPMS_ON)
+		mode = DRM_MODE_DPMS_OFF;
+
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
 		temp |= ADPA_DAC_ENABLE;
@@ -129,7 +132,7 @@ static int intel_crt_mode_valid(struct drm_connector *connector,
 }
 
 static bool intel_crt_mode_fixup(struct drm_encoder *encoder,
-				 struct drm_display_mode *mode,
+				 const struct drm_display_mode *mode,
 				 struct drm_display_mode *adjusted_mode)
 {
 	return true;
@@ -230,6 +233,42 @@ static bool intel_ironlake_crt_detect_hotplug(struct drm_connector *connector)
 	return ret;
 }
 
+static bool valleyview_crt_detect_hotplug(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 adpa;
+	bool ret;
+	u32 save_adpa;
+
+	save_adpa = adpa = I915_READ(ADPA);
+	DRM_DEBUG_KMS("trigger hotplug detect cycle: adpa=0x%x\n", adpa);
+
+	adpa |= ADPA_CRT_HOTPLUG_FORCE_TRIGGER;
+
+	I915_WRITE(ADPA, adpa);
+
+	if (wait_for((I915_READ(ADPA) & ADPA_CRT_HOTPLUG_FORCE_TRIGGER) == 0,
+		     1000)) {
+		DRM_DEBUG_KMS("timed out waiting for FORCE_TRIGGER");
+		I915_WRITE(ADPA, save_adpa);
+	}
+
+	/* Check the status to see if both blue and green are on now */
+	adpa = I915_READ(ADPA);
+	if ((adpa & ADPA_CRT_HOTPLUG_MONITOR_MASK) != 0)
+		ret = true;
+	else
+		ret = false;
+
+	DRM_DEBUG_KMS("valleyview hotplug adpa=0x%x, result %d\n", adpa, ret);
+
+	/* FIXME: debug force function and remove */
+	ret = true;
+
+	return ret;
+}
+
 /**
  * Uses CRT_HOTPLUG_EN and CRT_HOTPLUG_STAT to detect CRT presence.
  *
@@ -249,6 +288,9 @@ static bool intel_crt_detect_hotplug(struct drm_connector *connector)
 	if (HAS_PCH_SPLIT(dev))
 		return intel_ironlake_crt_detect_hotplug(connector);
 
+	if (IS_VALLEYVIEW(dev))
+		return valleyview_crt_detect_hotplug(connector);
+
 	/*
 	 * On 4 series desktop, CRT detect sequence need to be done twice
 	 * to get a reliable result.
@@ -288,39 +330,34 @@ static bool intel_crt_detect_ddc(struct drm_connector *connector)
 {
 	struct intel_crt *crt = intel_attached_crt(connector);
 	struct drm_i915_private *dev_priv = crt->base.base.dev->dev_private;
+	struct edid *edid;
+	struct i2c_adapter *i2c;
 
-	/* CRT should always be at 0, but check anyway */
-	if (crt->base.type != INTEL_OUTPUT_ANALOG)
-		return false;
+	BUG_ON(crt->base.type != INTEL_OUTPUT_ANALOG);
+
+	i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->crt_ddc_pin);
+	edid = drm_get_edid(connector, i2c);
 
-	if (intel_ddc_probe(&crt->base, dev_priv->crt_ddc_pin)) {
-		struct edid *edid;
-		bool is_digital = false;
-		struct i2c_adapter *i2c;
+	if (edid) {
+		bool is_digital = edid->input & DRM_EDID_INPUT_DIGITAL;
 
-		i2c = intel_gmbus_get_adapter(dev_priv, dev_priv->crt_ddc_pin);
-		edid = drm_get_edid(connector, i2c);
 		/*
 		 * This may be a DVI-I connector with a shared DDC
 		 * link between analog and digital outputs, so we
 		 * have to check the EDID input spec of the attached device.
-		 *
-		 * On the other hand, what should we do if it is a broken EDID?
 		 */
-		if (edid != NULL) {
-			is_digital = edid->input & DRM_EDID_INPUT_DIGITAL;
-			connector->display_info.raw_edid = NULL;
-			kfree(edid);
-		}
-
 		if (!is_digital) {
 			DRM_DEBUG_KMS("CRT detected via DDC:0x50 [EDID]\n");
 			return true;
-		} else {
-			DRM_DEBUG_KMS("CRT not detected via DDC:0x50 [EDID reports a digital panel]\n");
 		}
+
+		DRM_DEBUG_KMS("CRT not detected via DDC:0x50 [EDID reports a digital panel]\n");
+	} else {
+		DRM_DEBUG_KMS("CRT not detected via DDC:0x50 [no valid EDID found]\n");
 	}
 
+	kfree(edid);
+
 	return false;
 }
 
@@ -453,18 +490,27 @@ intel_crt_detect(struct drm_connector *connector, bool force)
 	struct intel_load_detect_pipe tmp;
 
 	if (I915_HAS_HOTPLUG(dev)) {
+		/* We can not rely on the HPD pin always being correctly wired
+		 * up, for example many KVM do not pass it through, and so
+		 * only trust an assertion that the monitor is connected.
+		 */
 		if (intel_crt_detect_hotplug(connector)) {
 			DRM_DEBUG_KMS("CRT detected via hotplug\n");
 			return connector_status_connected;
-		} else {
+		} else
 			DRM_DEBUG_KMS("CRT not detected via hotplug\n");
-			return connector_status_disconnected;
-		}
 	}
 
 	if (intel_crt_detect_ddc(connector))
 		return connector_status_connected;
 
+	/* Load detection is broken on HPD capable machines. Whoever wants a
+	 * broken monitor (without edid) to work behind a broken kvm (that fails
+	 * to have the right resistors for HP detection) needs to fix this up.
+	 * For now just bail out. */
+	if (I915_HAS_HOTPLUG(dev))
+		return connector_status_disconnected;
+
 	if (!force)
 		return connector->status;
 
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
index 46d1e886c69..933c7485917 100644
--- a/drivers/gpu/drm/i915/intel_ddi.c
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -170,6 +170,15 @@ void hsw_fdi_link_train(struct drm_crtc *crtc)
 
 		udelay(600);
 
+		/* We need to program FDI_RX_MISC with the default TP1 to TP2
+		 * values before enabling the receiver, and configure the delay
+		 * for the FDI timing generator to 90h. Luckily, all the other
+		 * bits are supposed to be zeroed, so we can write those values
+		 * directly.
+		 */
+		I915_WRITE(FDI_RX_MISC(pipe), FDI_RX_TP1_TO_TP2_48 |
+				FDI_RX_FDI_DELAY_90);
+
 		/* Enable CPU FDI Receiver with auto-training */
 		reg = FDI_RX_CTL(pipe);
 		I915_WRITE(reg,
@@ -726,8 +735,7 @@ void intel_ddi_mode_set(struct drm_encoder *encoder,
 
 	I915_WRITE(DDI_FUNC_CTL(pipe), temp);
 
-	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
-	intel_hdmi_set_spd_infoframe(encoder);
+	intel_hdmi->set_infoframes(encoder, adjusted_mode);
 }
 
 void intel_ddi_dpms(struct drm_encoder *encoder, int mode)
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index a8538ac0299..f6159765f1e 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -98,6 +98,11 @@ intel_find_pll_ironlake_dp(const intel_limit_t *, struct drm_crtc *crtc,
 			   int target, int refclk, intel_clock_t *match_clock,
 			   intel_clock_t *best_clock);
 
+static bool
+intel_vlv_find_best_pll(const intel_limit_t *limit, struct drm_crtc *crtc,
+			int target, int refclk, intel_clock_t *match_clock,
+			intel_clock_t *best_clock);
+
 static inline u32 /* units of 100MHz */
 intel_fdi_link_freq(struct drm_device *dev)
 {
@@ -359,6 +364,48 @@ static const intel_limit_t intel_limits_ironlake_display_port = {
 	.find_pll = intel_find_pll_ironlake_dp,
 };
 
+static const intel_limit_t intel_limits_vlv_dac = {
+	.dot = { .min = 25000, .max = 270000 },
+	.vco = { .min = 4000000, .max = 6000000 },
+	.n = { .min = 1, .max = 7 },
+	.m = { .min = 22, .max = 450 }, /* guess */
+	.m1 = { .min = 2, .max = 3 },
+	.m2 = { .min = 11, .max = 156 },
+	.p = { .min = 10, .max = 30 },
+	.p1 = { .min = 2, .max = 3 },
+	.p2 = { .dot_limit = 270000,
+		.p2_slow = 2, .p2_fast = 20 },
+	.find_pll = intel_vlv_find_best_pll,
+};
+
+static const intel_limit_t intel_limits_vlv_hdmi = {
+	.dot = { .min = 20000, .max = 165000 },
+	.vco = { .min = 5994000, .max = 4000000 },
+	.n = { .min = 1, .max = 7 },
+	.m = { .min = 60, .max = 300 }, /* guess */
+	.m1 = { .min = 2, .max = 3 },
+	.m2 = { .min = 11, .max = 156 },
+	.p = { .min = 10, .max = 30 },
+	.p1 = { .min = 2, .max = 3 },
+	.p2 = { .dot_limit = 270000,
+		.p2_slow = 2, .p2_fast = 20 },
+	.find_pll = intel_vlv_find_best_pll,
+};
+
+static const intel_limit_t intel_limits_vlv_dp = {
+	.dot = { .min = 162000, .max = 270000 },
+	.vco = { .min = 5994000, .max = 4000000 },
+	.n = { .min = 1, .max = 7 },
+	.m = { .min = 60, .max = 300 }, /* guess */
+	.m1 = { .min = 2, .max = 3 },
+	.m2 = { .min = 11, .max = 156 },
+	.p = { .min = 10, .max = 30 },
+	.p1 = { .min = 2, .max = 3 },
+	.p2 = { .dot_limit = 270000,
+		.p2_slow = 2, .p2_fast = 20 },
+	.find_pll = intel_vlv_find_best_pll,
+};
+
 u32 intel_dpio_read(struct drm_i915_private *dev_priv, int reg)
 {
 	unsigned long flags;
@@ -384,6 +431,28 @@ out_unlock:
 	return val;
 }
 
+static void intel_dpio_write(struct drm_i915_private *dev_priv, int reg,
+			     u32 val)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
+	if (wait_for_atomic_us((I915_READ(DPIO_PKT) & DPIO_BUSY) == 0, 100)) {
+		DRM_ERROR("DPIO idle wait timed out\n");
+		goto out_unlock;
+	}
+
+	I915_WRITE(DPIO_DATA, val);
+	I915_WRITE(DPIO_REG, reg);
+	I915_WRITE(DPIO_PKT, DPIO_RID | DPIO_OP_WRITE | DPIO_PORTID |
+		   DPIO_BYTE);
+	if (wait_for_atomic_us((I915_READ(DPIO_PKT) & DPIO_BUSY) == 0, 100))
+		DRM_ERROR("DPIO write wait timed out\n");
+
+out_unlock:
+       spin_unlock_irqrestore(&dev_priv->dpio_lock, flags);
+}
+
 static void vlv_init_dpio(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -434,7 +503,7 @@ static bool is_dual_link_lvds(struct drm_i915_private *dev_priv,
 		 * register is uninitialized.
 		 */
 		val = I915_READ(reg);
-		if (!(val & ~LVDS_DETECTED))
+		if (!(val & ~(LVDS_PIPE_MASK | LVDS_DETECTED)))
 			val = dev_priv->bios_lvds_val;
 		dev_priv->lvds_val = val;
 	}
@@ -510,6 +579,13 @@ static const intel_limit_t *intel_limit(struct drm_crtc *crtc, int refclk)
 			limit = &intel_limits_pineview_lvds;
 		else
 			limit = &intel_limits_pineview_sdvo;
+	} else if (IS_VALLEYVIEW(dev)) {
+		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_ANALOG))
+			limit = &intel_limits_vlv_dac;
+		else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI))
+			limit = &intel_limits_vlv_hdmi;
+		else
+			limit = &intel_limits_vlv_dp;
 	} else if (!IS_GEN2(dev)) {
 		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
 			limit = &intel_limits_i9xx_lvds;
@@ -551,11 +627,10 @@ static void intel_clock(struct drm_device *dev, int refclk, intel_clock_t *clock
 bool intel_pipe_has_type(struct drm_crtc *crtc, int type)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 
-	list_for_each_entry(encoder, &mode_config->encoder_list, base.head)
-		if (encoder->base.crtc == crtc && encoder->type == type)
+	for_each_encoder_on_crtc(dev, crtc, encoder)
+		if (encoder->type == type)
 			return true;
 
 	return false;
@@ -783,6 +858,73 @@ intel_find_pll_g4x_dp(const intel_limit_t *limit, struct drm_crtc *crtc,
 	memcpy(best_clock, &clock, sizeof(intel_clock_t));
 	return true;
 }
+static bool
+intel_vlv_find_best_pll(const intel_limit_t *limit, struct drm_crtc *crtc,
+			int target, int refclk, intel_clock_t *match_clock,
+			intel_clock_t *best_clock)
+{
+	u32 p1, p2, m1, m2, vco, bestn, bestm1, bestm2, bestp1, bestp2;
+	u32 m, n, fastclk;
+	u32 updrate, minupdate, fracbits, p;
+	unsigned long bestppm, ppm, absppm;
+	int dotclk, flag;
+
+	dotclk = target * 1000;
+	bestppm = 1000000;
+	ppm = absppm = 0;
+	fastclk = dotclk / (2*100);
+	updrate = 0;
+	minupdate = 19200;
+	fracbits = 1;
+	n = p = p1 = p2 = m = m1 = m2 = vco = bestn = 0;
+	bestm1 = bestm2 = bestp1 = bestp2 = 0;
+
+	/* based on hardware requirement, prefer smaller n to precision */
+	for (n = limit->n.min; n <= ((refclk) / minupdate); n++) {
+		updrate = refclk / n;
+		for (p1 = limit->p1.max; p1 > limit->p1.min; p1--) {
+			for (p2 = limit->p2.p2_fast+1; p2 > 0; p2--) {
+				if (p2 > 10)
+					p2 = p2 - 1;
+				p = p1 * p2;
+				/* based on hardware requirement, prefer bigger m1,m2 values */
+				for (m1 = limit->m1.min; m1 <= limit->m1.max; m1++) {
+					m2 = (((2*(fastclk * p * n / m1 )) +
+					       refclk) / (2*refclk));
+					m = m1 * m2;
+					vco = updrate * m;
+					if (vco >= limit->vco.min && vco < limit->vco.max) {
+						ppm = 1000000 * ((vco / p) - fastclk) / fastclk;
+						absppm = (ppm > 0) ? ppm : (-ppm);
+						if (absppm < 100 && ((p1 * p2) > (bestp1 * bestp2))) {
+							bestppm = 0;
+							flag = 1;
+						}
+						if (absppm < bestppm - 10) {
+							bestppm = absppm;
+							flag = 1;
+						}
+						if (flag) {
+							bestn = n;
+							bestm1 = m1;
+							bestm2 = m2;
+							bestp1 = p1;
+							bestp2 = p2;
+							flag = 0;
+						}
+					}
+				}
+			}
+		}
+	}
+	best_clock->n = bestn;
+	best_clock->m1 = bestm1;
+	best_clock->m2 = bestm2;
+	best_clock->p1 = bestp1;
+	best_clock->p2 = bestp2;
+
+	return true;
+}
 
 static void ironlake_wait_for_vblank(struct drm_device *dev, int pipe)
 {
@@ -1232,6 +1374,9 @@ static void assert_pch_dp_disabled(struct drm_i915_private *dev_priv,
 	WARN(dp_pipe_enabled(dev_priv, pipe, port_sel, val),
 	     "PCH DP (0x%08x) enabled on transcoder %c, should be disabled\n",
 	     reg, pipe_name(pipe));
+
+	WARN(HAS_PCH_IBX(dev_priv->dev) && (val & SDVO_PIPE_B_SELECT),
+	     "IBX PCH dp port still using transcoder B\n");
 }
 
 static void assert_pch_hdmi_disabled(struct drm_i915_private *dev_priv,
@@ -1241,6 +1386,9 @@ static void assert_pch_hdmi_disabled(struct drm_i915_private *dev_priv,
 	WARN(hdmi_pipe_enabled(dev_priv, val, pipe),
 	     "PCH HDMI (0x%08x) enabled on transcoder %c, should be disabled\n",
 	     reg, pipe_name(pipe));
+
+	WARN(HAS_PCH_IBX(dev_priv->dev) && (val & SDVO_PIPE_B_SELECT),
+	     "IBX PCH hdmi port still using transcoder B\n");
 }
 
 static void assert_pch_ports_disabled(struct drm_i915_private *dev_priv,
@@ -1287,7 +1435,7 @@ static void intel_enable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
 	u32 val;
 
 	/* No really, not for ILK+ */
-	BUG_ON(dev_priv->info->gen >= 5);
+	BUG_ON(!IS_VALLEYVIEW(dev_priv->dev) && dev_priv->info->gen >= 5);
 
 	/* PLL is protected by panel, make sure we can write it */
 	if (IS_MOBILE(dev_priv->dev) && !IS_I830(dev_priv->dev))
@@ -1344,7 +1492,7 @@ intel_sbi_write(struct drm_i915_private *dev_priv, u16 reg, u32 value)
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
-	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_READY) == 0,
+	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_BUSY) == 0,
 				100)) {
 		DRM_ERROR("timeout waiting for SBI to become ready\n");
 		goto out_unlock;
@@ -1358,7 +1506,7 @@ intel_sbi_write(struct drm_i915_private *dev_priv, u16 reg, u32 value)
 			SBI_BUSY |
 			SBI_CTL_OP_CRWR);
 
-	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_READY | SBI_RESPONSE_SUCCESS)) == 0,
+	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_BUSY | SBI_RESPONSE_FAIL)) == 0,
 				100)) {
 		DRM_ERROR("timeout waiting for SBI to complete write transaction\n");
 		goto out_unlock;
@@ -1372,10 +1520,10 @@ static u32
 intel_sbi_read(struct drm_i915_private *dev_priv, u16 reg)
 {
 	unsigned long flags;
-	u32 value;
+	u32 value = 0;
 
 	spin_lock_irqsave(&dev_priv->dpio_lock, flags);
-	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_READY) == 0,
+	if (wait_for((I915_READ(SBI_CTL_STAT) & SBI_BUSY) == 0,
 				100)) {
 		DRM_ERROR("timeout waiting for SBI to become ready\n");
 		goto out_unlock;
@@ -1387,7 +1535,7 @@ intel_sbi_read(struct drm_i915_private *dev_priv, u16 reg)
 			SBI_BUSY |
 			SBI_CTL_OP_CRRD);
 
-	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_READY | SBI_RESPONSE_SUCCESS)) == 0,
+	if (wait_for((I915_READ(SBI_CTL_STAT) & (SBI_BUSY | SBI_RESPONSE_FAIL)) == 0,
 				100)) {
 		DRM_ERROR("timeout waiting for SBI to complete read transaction\n");
 		goto out_unlock;
@@ -1824,6 +1972,22 @@ void intel_unpin_fb_obj(struct drm_i915_gem_object *obj)
 	i915_gem_object_unpin(obj);
 }
 
+/* Computes the linear offset to the base tile and adjusts x, y. bytes per pixel
+ * is assumed to be a power-of-two. */
+static unsigned long gen4_compute_dspaddr_offset_xtiled(int *x, int *y,
+							unsigned int bpp,
+							unsigned int pitch)
+{
+	int tile_rows, tiles;
+
+	tile_rows = *y / 8;
+	*y %= 8;
+	tiles = *x / (512/bpp);
+	*x %= 512/bpp;
+
+	return tile_rows * pitch * 8 + tiles * 4096;
+}
+
 static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 			     int x, int y)
 {
@@ -1833,7 +1997,7 @@ static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	struct intel_framebuffer *intel_fb;
 	struct drm_i915_gem_object *obj;
 	int plane = intel_crtc->plane;
-	unsigned long Start, Offset;
+	unsigned long linear_offset;
 	u32 dspcntr;
 	u32 reg;
 
@@ -1880,18 +2044,28 @@ static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 
 	I915_WRITE(reg, dspcntr);
 
-	Start = obj->gtt_offset;
-	Offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+	linear_offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
 
-	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
-		      Start, Offset, x, y, fb->pitches[0]);
+	if (INTEL_INFO(dev)->gen >= 4) {
+		intel_crtc->dspaddr_offset =
+			gen4_compute_dspaddr_offset_xtiled(&x, &y,
+							   fb->bits_per_pixel / 8,
+							   fb->pitches[0]);
+		linear_offset -= intel_crtc->dspaddr_offset;
+	} else {
+		intel_crtc->dspaddr_offset = linear_offset;
+	}
+
+	DRM_DEBUG_KMS("Writing base %08X %08lX %d %d %d\n",
+		      obj->gtt_offset, linear_offset, x, y, fb->pitches[0]);
 	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
 	if (INTEL_INFO(dev)->gen >= 4) {
-		I915_MODIFY_DISPBASE(DSPSURF(plane), Start);
+		I915_MODIFY_DISPBASE(DSPSURF(plane),
+				     obj->gtt_offset + intel_crtc->dspaddr_offset);
 		I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
-		I915_WRITE(DSPADDR(plane), Offset);
+		I915_WRITE(DSPLINOFF(plane), linear_offset);
 	} else
-		I915_WRITE(DSPADDR(plane), Start + Offset);
+		I915_WRITE(DSPADDR(plane), obj->gtt_offset + linear_offset);
 	POSTING_READ(reg);
 
 	return 0;
@@ -1906,7 +2080,7 @@ static int ironlake_update_plane(struct drm_crtc *crtc,
 	struct intel_framebuffer *intel_fb;
 	struct drm_i915_gem_object *obj;
 	int plane = intel_crtc->plane;
-	unsigned long Start, Offset;
+	unsigned long linear_offset;
 	u32 dspcntr;
 	u32 reg;
 
@@ -1961,15 +2135,20 @@ static int ironlake_update_plane(struct drm_crtc *crtc,
 
 	I915_WRITE(reg, dspcntr);
 
-	Start = obj->gtt_offset;
-	Offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+	linear_offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+	intel_crtc->dspaddr_offset =
+		gen4_compute_dspaddr_offset_xtiled(&x, &y,
+						   fb->bits_per_pixel / 8,
+						   fb->pitches[0]);
+	linear_offset -= intel_crtc->dspaddr_offset;
 
-	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
-		      Start, Offset, x, y, fb->pitches[0]);
+	DRM_DEBUG_KMS("Writing base %08X %08lX %d %d %d\n",
+		      obj->gtt_offset, linear_offset, x, y, fb->pitches[0]);
 	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
-	I915_MODIFY_DISPBASE(DSPSURF(plane), Start);
+	I915_MODIFY_DISPBASE(DSPSURF(plane),
+			     obj->gtt_offset + intel_crtc->dspaddr_offset);
 	I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
-	I915_WRITE(DSPADDR(plane), Offset);
+	I915_WRITE(DSPLINOFF(plane), linear_offset);
 	POSTING_READ(reg);
 
 	return 0;
@@ -2656,16 +2835,13 @@ static void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 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;
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
 
 		/* On Haswell, LPT PCH handles the VGA connection via FDI, and Haswell
 		 * CPU handles all others */
@@ -3397,7 +3573,7 @@ void intel_encoder_destroy(struct drm_encoder *encoder)
 }
 
 static bool intel_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = crtc->dev;
@@ -3554,16 +3730,12 @@ static bool intel_choose_pipe_bpp_dither(struct drm_crtc *crtc,
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_encoder *encoder;
 	struct drm_connector *connector;
+	struct intel_encoder *intel_encoder;
 	unsigned int display_bpc = UINT_MAX, bpc;
 
 	/* Walk the encoders & connectors on this crtc, get min bpc */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		struct intel_encoder *intel_encoder = to_intel_encoder(encoder);
-
-		if (encoder->crtc != crtc)
-			continue;
+	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
 
 		if (intel_encoder->type == INTEL_OUTPUT_LVDS) {
 			unsigned int lvds_bpc;
@@ -3595,7 +3767,7 @@ static bool intel_choose_pipe_bpp_dither(struct drm_crtc *crtc,
 		/* Not one of the known troublemakers, check the EDID */
 		list_for_each_entry(connector, &dev->mode_config.connector_list,
 				    head) {
-			if (connector->encoder != encoder)
+			if (connector->encoder != &intel_encoder->base)
 				continue;
 
 			/* Don't use an invalid EDID bpc value */
@@ -3666,13 +3838,37 @@ static bool intel_choose_pipe_bpp_dither(struct drm_crtc *crtc,
 	return display_bpc != bpc;
 }
 
+static int vlv_get_refclk(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int refclk = 27000; /* for DP & HDMI */
+
+	return 100000; /* only one validated so far */
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_ANALOG)) {
+		refclk = 96000;
+	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+		if (intel_panel_use_ssc(dev_priv))
+			refclk = 100000;
+		else
+			refclk = 96000;
+	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_EDP)) {
+		refclk = 100000;
+	}
+
+	return refclk;
+}
+
 static int i9xx_get_refclk(struct drm_crtc *crtc, int num_connectors)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int refclk;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	if (IS_VALLEYVIEW(dev)) {
+		refclk = vlv_get_refclk(crtc);
+	} else if (intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
 	    intel_panel_use_ssc(dev_priv) && num_connectors < 2) {
 		refclk = dev_priv->lvds_ssc_freq * 1000;
 		DRM_DEBUG_KMS("using SSC reference clock of %d MHz\n",
@@ -3787,6 +3983,72 @@ static void intel_update_lvds(struct drm_crtc *crtc, intel_clock_t *clock,
 	I915_WRITE(LVDS, temp);
 }
 
+static void vlv_update_pll(struct drm_crtc *crtc,
+			   struct drm_display_mode *mode,
+			   struct drm_display_mode *adjusted_mode,
+			   intel_clock_t *clock, intel_clock_t *reduced_clock,
+			   int refclk, int num_connectors)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	u32 dpll, mdiv, pdiv;
+	u32 bestn, bestm1, bestm2, bestp1, bestp2;
+	bool is_hdmi;
+
+	is_hdmi = intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI);
+
+	bestn = clock->n;
+	bestm1 = clock->m1;
+	bestm2 = clock->m2;
+	bestp1 = clock->p1;
+	bestp2 = clock->p2;
+
+	/* Enable DPIO clock input */
+	dpll = DPLL_EXT_BUFFER_ENABLE_VLV | DPLL_REFA_CLK_ENABLE_VLV |
+		DPLL_VGA_MODE_DIS | DPLL_INTEGRATED_CLOCK_VLV;
+	I915_WRITE(DPLL(pipe), dpll);
+	POSTING_READ(DPLL(pipe));
+
+	mdiv = ((bestm1 << DPIO_M1DIV_SHIFT) | (bestm2 & DPIO_M2DIV_MASK));
+	mdiv |= ((bestp1 << DPIO_P1_SHIFT) | (bestp2 << DPIO_P2_SHIFT));
+	mdiv |= ((bestn << DPIO_N_SHIFT));
+	mdiv |= (1 << DPIO_POST_DIV_SHIFT);
+	mdiv |= (1 << DPIO_K_SHIFT);
+	mdiv |= DPIO_ENABLE_CALIBRATION;
+	intel_dpio_write(dev_priv, DPIO_DIV(pipe), mdiv);
+
+	intel_dpio_write(dev_priv, DPIO_CORE_CLK(pipe), 0x01000000);
+
+	pdiv = DPIO_REFSEL_OVERRIDE | (5 << DPIO_PLL_MODESEL_SHIFT) |
+		(3 << DPIO_BIAS_CURRENT_CTL_SHIFT) | (1<<20) |
+		(8 << DPIO_DRIVER_CTL_SHIFT) | (5 << DPIO_CLK_BIAS_CTL_SHIFT);
+	intel_dpio_write(dev_priv, DPIO_REFSFR(pipe), pdiv);
+
+	intel_dpio_write(dev_priv, DPIO_LFP_COEFF(pipe), 0x009f0051);
+
+	dpll |= DPLL_VCO_ENABLE;
+	I915_WRITE(DPLL(pipe), dpll);
+	POSTING_READ(DPLL(pipe));
+	if (wait_for(((I915_READ(DPLL(pipe)) & DPLL_LOCK_VLV) == DPLL_LOCK_VLV), 1))
+		DRM_ERROR("DPLL %d failed to lock\n", pipe);
+
+	if (is_hdmi) {
+		u32 temp = intel_mode_get_pixel_multiplier(adjusted_mode);
+
+		if (temp > 1)
+			temp = (temp - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
+		else
+			temp = 0;
+
+		I915_WRITE(DPLL_MD(pipe), temp);
+		POSTING_READ(DPLL_MD(pipe));
+	}
+
+	intel_dpio_write(dev_priv, DPIO_FASTCLK_DISABLE, 0x641); /* ??? */
+}
+
 static void i9xx_update_pll(struct drm_crtc *crtc,
 			    struct drm_display_mode *mode,
 			    struct drm_display_mode *adjusted_mode,
@@ -3974,15 +4236,11 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	u32 dspcntr, pipeconf, vsyncshift;
 	bool ok, has_reduced_clock = false, is_sdvo = false;
 	bool is_lvds = false, is_tv = false, is_dp = false;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 	const intel_limit_t *limit;
 	int ret;
 
-	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
-		if (encoder->base.crtc != crtc)
-			continue;
-
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
 		switch (encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
@@ -4044,6 +4302,9 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 
 	if (IS_GEN2(dev))
 		i8xx_update_pll(crtc, adjusted_mode, &clock, num_connectors);
+	else if (IS_VALLEYVIEW(dev))
+		vlv_update_pll(crtc, mode,adjusted_mode, &clock, NULL,
+			       refclk, num_connectors);
 	else
 		i9xx_update_pll(crtc, mode, adjusted_mode, &clock,
 				has_reduced_clock ? &reduced_clock : NULL,
@@ -4282,15 +4543,11 @@ static int ironlake_get_refclk(struct drm_crtc *crtc)
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *encoder;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *edp_encoder = NULL;
 	int num_connectors = 0;
 	bool is_lvds = false;
 
-	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
-		if (encoder->base.crtc != crtc)
-			continue;
-
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
 		switch (encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
@@ -4327,7 +4584,6 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	u32 dpll, fp = 0, fp2 = 0, dspcntr, pipeconf;
 	bool ok, has_reduced_clock = false, is_sdvo = false;
 	bool is_crt = false, is_lvds = false, is_tv = false, is_dp = false;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder, *edp_encoder = NULL;
 	const intel_limit_t *limit;
 	int ret;
@@ -4338,10 +4594,7 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	bool dither;
 	bool is_cpu_edp = false, is_pch_edp = false;
 
-	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
-		if (encoder->base.crtc != crtc)
-			continue;
-
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
 		switch (encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			is_lvds = true;
@@ -4405,25 +4658,10 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 						    &clock,
 						    &reduced_clock);
 	}
-	/* SDVO TV has fixed PLL values depend on its clock range,
-	   this mirrors vbios setting. */
-	if (is_sdvo && is_tv) {
-		if (adjusted_mode->clock >= 100000
-		    && adjusted_mode->clock < 140500) {
-			clock.p1 = 2;
-			clock.p2 = 10;
-			clock.n = 3;
-			clock.m1 = 16;
-			clock.m2 = 8;
-		} else if (adjusted_mode->clock >= 140500
-			   && adjusted_mode->clock <= 200000) {
-			clock.p1 = 1;
-			clock.p2 = 10;
-			clock.n = 6;
-			clock.m1 = 12;
-			clock.m2 = 8;
-		}
-	}
+
+	if (is_sdvo && is_tv)
+		i9xx_adjust_sdvo_tv_clock(adjusted_mode, &clock);
+
 
 	/* FDI link */
 	pixel_multiplier = intel_mode_get_pixel_multiplier(adjusted_mode);
@@ -4431,16 +4669,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 	/* CPU eDP doesn't require FDI link, so just set DP M/N
 	   according to current link config */
 	if (is_cpu_edp) {
-		target_clock = mode->clock;
 		intel_edp_link_config(edp_encoder, &lane, &link_bw);
 	} else {
-		/* [e]DP over FDI requires target mode clock
-		   instead of link clock */
-		if (is_dp)
-			target_clock = mode->clock;
-		else
-			target_clock = adjusted_mode->clock;
-
 		/* FDI is a binary signal running at ~2.7GHz, encoding
 		 * each output octet as 10 bits. The actual frequency
 		 * is stored as a divider into a 100MHz clock, and the
@@ -4451,6 +4681,14 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		link_bw = intel_fdi_link_freq(dev) * MHz(100)/KHz(1)/10;
 	}
 
+	/* [e]DP over FDI requires target mode clock instead of link clock. */
+	if (edp_encoder)
+		target_clock = intel_edp_target_clock(edp_encoder, mode);
+	else if (is_dp)
+		target_clock = mode->clock;
+	else
+		target_clock = adjusted_mode->clock;
+
 	/* determine panel color depth */
 	temp = I915_READ(PIPECONF(pipe));
 	temp &= ~PIPE_BPC_MASK;
@@ -4662,16 +4900,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		if (is_lvds && has_reduced_clock && i915_powersave) {
 			I915_WRITE(intel_crtc->pch_pll->fp1_reg, fp2);
 			intel_crtc->lowfreq_avail = true;
-			if (HAS_PIPE_CXSR(dev)) {
-				DRM_DEBUG_KMS("enabling CxSR downclocking\n");
-				pipeconf |= PIPECONF_CXSR_DOWNCLOCK;
-			}
 		} else {
 			I915_WRITE(intel_crtc->pch_pll->fp1_reg, fp);
-			if (HAS_PIPE_CXSR(dev)) {
-				DRM_DEBUG_KMS("disabling CxSR downclocking\n");
-				pipeconf &= ~PIPECONF_CXSR_DOWNCLOCK;
-			}
 		}
 	}
 
@@ -5975,7 +6205,6 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	unsigned long offset;
 	u32 flip_mask;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
@@ -5984,9 +6213,6 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 	if (ret)
 		goto err;
 
-	/* Offset into the new buffer for cases of shared fbs between CRTCs */
-	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
-
 	ret = intel_ring_begin(ring, 6);
 	if (ret)
 		goto err_unpin;
@@ -6003,7 +6229,7 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 	intel_ring_emit(ring, MI_DISPLAY_FLIP |
 			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
 	intel_ring_emit(ring, fb->pitches[0]);
-	intel_ring_emit(ring, obj->gtt_offset + offset);
+	intel_ring_emit(ring, obj->gtt_offset + intel_crtc->dspaddr_offset);
 	intel_ring_emit(ring, 0); /* aux display base address, unused */
 	intel_ring_advance(ring);
 	return 0;
@@ -6021,7 +6247,6 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	unsigned long offset;
 	u32 flip_mask;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
 	int ret;
@@ -6030,9 +6255,6 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 	if (ret)
 		goto err;
 
-	/* Offset into the new buffer for cases of shared fbs between CRTCs */
-	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
-
 	ret = intel_ring_begin(ring, 6);
 	if (ret)
 		goto err_unpin;
@@ -6046,7 +6268,7 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 |
 			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
 	intel_ring_emit(ring, fb->pitches[0]);
-	intel_ring_emit(ring, obj->gtt_offset + offset);
+	intel_ring_emit(ring, obj->gtt_offset + intel_crtc->dspaddr_offset);
 	intel_ring_emit(ring, MI_NOOP);
 
 	intel_ring_advance(ring);
@@ -6084,7 +6306,9 @@ static int intel_gen4_queue_flip(struct drm_device *dev,
 	intel_ring_emit(ring, MI_DISPLAY_FLIP |
 			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
 	intel_ring_emit(ring, fb->pitches[0]);
-	intel_ring_emit(ring, obj->gtt_offset | obj->tiling_mode);
+	intel_ring_emit(ring,
+			(obj->gtt_offset + intel_crtc->dspaddr_offset) |
+			obj->tiling_mode);
 
 	/* XXX Enabling the panel-fitter across page-flip is so far
 	 * untested on non-native modes, so ignore it for now.
@@ -6124,7 +6348,7 @@ static int intel_gen6_queue_flip(struct drm_device *dev,
 	intel_ring_emit(ring, MI_DISPLAY_FLIP |
 			MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
 	intel_ring_emit(ring, fb->pitches[0] | obj->tiling_mode);
-	intel_ring_emit(ring, obj->gtt_offset);
+	intel_ring_emit(ring, obj->gtt_offset + intel_crtc->dspaddr_offset);
 
 	/* Contrary to the suggestions in the documentation,
 	 * "Enable Panel Fitter" does not seem to be required when page
@@ -6187,7 +6411,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 
 	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 | plane_bit);
 	intel_ring_emit(ring, (fb->pitches[0] | obj->tiling_mode));
-	intel_ring_emit(ring, (obj->gtt_offset));
+	intel_ring_emit(ring, obj->gtt_offset + intel_crtc->dspaddr_offset);
 	intel_ring_emit(ring, (MI_NOOP));
 	intel_ring_advance(ring);
 	return 0;
@@ -6219,6 +6443,19 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	unsigned long flags;
 	int ret;
 
+	/* Can't change pixel format via MI display flips. */
+	if (fb->pixel_format != crtc->fb->pixel_format)
+		return -EINVAL;
+
+	/*
+	 * TILEOFF/LINOFF registers can't be changed via MI display flips.
+	 * Note that pitch changes could also affect these register.
+	 */
+	if (INTEL_INFO(dev)->gen > 3 &&
+	    (fb->offsets[0] != crtc->fb->offsets[0] ||
+	     fb->pitches[0] != crtc->fb->pitches[0]))
+		return -EINVAL;
+
 	work = kzalloc(sizeof *work, GFP_KERNEL);
 	if (work == NULL)
 		return -ENOMEM;
@@ -6249,7 +6486,9 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	intel_fb = to_intel_framebuffer(fb);
 	obj = intel_fb->obj;
 
-	mutex_lock(&dev->struct_mutex);
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		goto cleanup;
 
 	/* Reference the objects for the scheduled work. */
 	drm_gem_object_reference(&work->old_fb_obj->base);
@@ -6284,6 +6523,7 @@ cleanup_pending:
 	drm_gem_object_unreference(&obj->base);
 	mutex_unlock(&dev->struct_mutex);
 
+cleanup:
 	spin_lock_irqsave(&dev->event_lock, flags);
 	intel_crtc->unpin_work = NULL;
 	spin_unlock_irqrestore(&dev->event_lock, flags);
@@ -6566,7 +6806,24 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (!dpd_is_edp && (I915_READ(PCH_DP_D) & DP_DETECTED))
 			intel_dp_init(dev, PCH_DP_D);
+	} else if (IS_VALLEYVIEW(dev)) {
+		int found;
 
+		if (I915_READ(SDVOB) & PORT_DETECTED) {
+			/* SDVOB multiplex with HDMIB */
+			found = intel_sdvo_init(dev, SDVOB, true);
+			if (!found)
+				intel_hdmi_init(dev, SDVOB);
+			if (!found && (I915_READ(DP_B) & DP_DETECTED))
+				intel_dp_init(dev, DP_B);
+		}
+
+		if (I915_READ(SDVOC) & PORT_DETECTED)
+			intel_hdmi_init(dev, SDVOC);
+
+		/* Shares lanes with HDMI on SDVOC */
+		if (I915_READ(DP_C) & DP_DETECTED)
+			intel_dp_init(dev, DP_C);
 	} else if (SUPPORTS_DIGITAL_OUTPUTS(dev)) {
 		bool found = false;
 
@@ -6623,7 +6880,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 	/* disable all the possible outputs/crtcs before entering KMS mode */
 	drm_helper_disable_unused_functions(dev);
 
-	if (HAS_PCH_SPLIT(dev))
+	if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev))
 		ironlake_init_pch_refclk(dev);
 }
 
@@ -6777,9 +7034,6 @@ static void intel_init_display(struct drm_device *dev)
 			dev_priv->display.write_eld = ironlake_write_eld;
 		} else
 			dev_priv->display.update_wm = NULL;
-	} else if (IS_VALLEYVIEW(dev)) {
-		dev_priv->display.force_wake_get = vlv_force_wake_get;
-		dev_priv->display.force_wake_put = vlv_force_wake_put;
 	} else if (IS_G4X(dev)) {
 		dev_priv->display.write_eld = g4x_write_eld;
 	}
@@ -6923,20 +7177,18 @@ static void i915_disable_vga(struct drm_device *dev)
 
 void intel_modeset_init_hw(struct drm_device *dev)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	/* We attempt to init the necessary power wells early in the initialization
+	 * time, so the subsystems that expect power to be enabled can work.
+	 */
+	intel_init_power_wells(dev);
 
-	intel_init_clock_gating(dev);
+	intel_prepare_ddi(dev);
 
-	if (IS_IRONLAKE_M(dev)) {
-		ironlake_enable_drps(dev);
-		ironlake_enable_rc6(dev);
-		intel_init_emon(dev);
-	}
+	intel_init_clock_gating(dev);
 
-	if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev)) {
-		gen6_enable_rps(dev_priv);
-		gen6_update_ring_freq(dev_priv);
-	}
+	mutex_lock(&dev->struct_mutex);
+	intel_enable_gt_powersave(dev);
+	mutex_unlock(&dev->struct_mutex);
 }
 
 void intel_modeset_init(struct drm_device *dev)
@@ -6958,8 +7210,6 @@ void intel_modeset_init(struct drm_device *dev)
 
 	intel_init_pm(dev);
 
-	intel_prepare_ddi(dev);
-
 	intel_init_display(dev);
 
 	if (IS_GEN2(dev)) {
@@ -6972,7 +7222,7 @@ void intel_modeset_init(struct drm_device *dev)
 		dev->mode_config.max_width = 8192;
 		dev->mode_config.max_height = 8192;
 	}
-	dev->mode_config.fb_base = dev->agp->base;
+	dev->mode_config.fb_base = dev_priv->mm.gtt_base_addr;
 
 	DRM_DEBUG_KMS("%d display pipe%s available.\n",
 		      dev_priv->num_pipe, dev_priv->num_pipe > 1 ? "s" : "");
@@ -7025,13 +7275,9 @@ void intel_modeset_cleanup(struct drm_device *dev)
 
 	intel_disable_fbc(dev);
 
-	if (IS_IRONLAKE_M(dev))
-		ironlake_disable_drps(dev);
-	if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev))
-		gen6_disable_rps(dev);
+	intel_disable_gt_powersave(dev);
 
-	if (IS_IRONLAKE_M(dev))
-		ironlake_disable_rc6(dev);
+	ironlake_teardown_rc6(dev);
 
 	if (IS_VALLEYVIEW(dev))
 		vlv_init_dpio(dev);
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index c0449324143..0a56b9ab0f5 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -155,6 +155,18 @@ intel_edp_link_config(struct intel_encoder *intel_encoder,
 		*link_bw = 270000;
 }
 
+int
+intel_edp_target_clock(struct intel_encoder *intel_encoder,
+		       struct drm_display_mode *mode)
+{
+	struct intel_dp *intel_dp = container_of(intel_encoder, struct intel_dp, base);
+
+	if (intel_dp->panel_fixed_mode)
+		return intel_dp->panel_fixed_mode->clock;
+	else
+		return mode->clock;
+}
+
 static int
 intel_dp_max_lane_count(struct intel_dp *intel_dp)
 {
@@ -225,7 +237,7 @@ intel_dp_max_data_rate(int max_link_clock, int max_lanes)
 static bool
 intel_dp_adjust_dithering(struct intel_dp *intel_dp,
 			  struct drm_display_mode *mode,
-			  struct drm_display_mode *adjusted_mode)
+			  bool adjust_mode)
 {
 	int max_link_clock = intel_dp_link_clock(intel_dp_max_link_bw(intel_dp));
 	int max_lanes = intel_dp_max_lane_count(intel_dp);
@@ -239,8 +251,8 @@ intel_dp_adjust_dithering(struct intel_dp *intel_dp,
 		if (mode_rate > max_rate)
 			return false;
 
-		if (adjusted_mode)
-			adjusted_mode->private_flags
+		if (adjust_mode)
+			mode->private_flags
 				|= INTEL_MODE_DP_FORCE_6BPC;
 
 		return true;
@@ -263,7 +275,7 @@ intel_dp_mode_valid(struct drm_connector *connector,
 			return MODE_PANEL;
 	}
 
-	if (!intel_dp_adjust_dithering(intel_dp, mode, NULL))
+	if (!intel_dp_adjust_dithering(intel_dp, mode, false))
 		return MODE_CLOCK_HIGH;
 
 	if (mode->clock < 10000)
@@ -691,7 +703,8 @@ intel_dp_i2c_init(struct intel_dp *intel_dp,
 }
 
 static bool
-intel_dp_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
+intel_dp_mode_fixup(struct drm_encoder *encoder,
+		    const struct drm_display_mode *mode,
 		    struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
@@ -706,28 +719,23 @@ intel_dp_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 		intel_fixed_panel_mode(intel_dp->panel_fixed_mode, adjusted_mode);
 		intel_pch_panel_fitting(dev, DRM_MODE_SCALE_FULLSCREEN,
 					mode, adjusted_mode);
-		/*
-		 * the mode->clock is used to calculate the Data&Link M/N
-		 * of the pipe. For the eDP the fixed clock should be used.
-		 */
-		mode->clock = intel_dp->panel_fixed_mode->clock;
 	}
 
-	if (mode->flags & DRM_MODE_FLAG_DBLCLK)
+	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK)
 		return false;
 
 	DRM_DEBUG_KMS("DP link computation with max lane count %i "
 		      "max bw %02x pixel clock %iKHz\n",
-		      max_lane_count, bws[max_clock], mode->clock);
+		      max_lane_count, bws[max_clock], adjusted_mode->clock);
 
-	if (!intel_dp_adjust_dithering(intel_dp, mode, adjusted_mode))
+	if (!intel_dp_adjust_dithering(intel_dp, adjusted_mode, true))
 		return false;
 
 	bpp = adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC ? 18 : 24;
-	mode_rate = intel_dp_link_required(mode->clock, bpp);
+	mode_rate = intel_dp_link_required(adjusted_mode->clock, bpp);
 
-	for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
-		for (clock = 0; clock <= max_clock; clock++) {
+	for (clock = 0; clock <= max_clock; clock++) {
+		for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
 			int link_avail = intel_dp_max_data_rate(intel_dp_link_clock(bws[clock]), lane_count);
 
 			if (mode_rate <= link_avail) {
@@ -786,8 +794,7 @@ intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
 		 struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_mode_config *mode_config = &dev->mode_config;
-	struct drm_encoder *encoder;
+	struct intel_encoder *encoder;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int lane_count = 4;
@@ -797,13 +804,9 @@ intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
 	/*
 	 * Find the lane count in the intel_encoder private
 	 */
-	list_for_each_entry(encoder, &mode_config->encoder_list, head) {
-		struct intel_dp *intel_dp;
-
-		if (encoder->crtc != crtc)
-			continue;
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
+		struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
 
-		intel_dp = enc_to_intel_dp(encoder);
 		if (intel_dp->base.type == INTEL_OUTPUT_DISPLAYPORT ||
 		    intel_dp->base.type == INTEL_OUTPUT_EDP)
 		{
@@ -1768,7 +1771,7 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
 		for (i = 0; i < intel_dp->lane_count; i++)
 			if ((intel_dp->train_set[i] & DP_TRAIN_MAX_SWING_REACHED) == 0)
 				break;
-		if (i == intel_dp->lane_count) {
+		if (i == intel_dp->lane_count && voltage_tries == 5) {
 			++loop_tries;
 			if (loop_tries == 5) {
 				DRM_DEBUG_KMS("too many full retries, give up\n");
@@ -1922,7 +1925,7 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 			DP |= DP_LINK_TRAIN_OFF;
 	}
 
-	if (!HAS_PCH_CPT(dev) &&
+	if (HAS_PCH_IBX(dev) &&
 	    I915_READ(intel_dp->output_reg) & DP_PIPEB_SELECT) {
 		struct drm_crtc *crtc = intel_dp->base.base.crtc;
 
@@ -2099,25 +2102,23 @@ g4x_dp_detect(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t temp, bit;
+	uint32_t bit;
 
 	switch (intel_dp->output_reg) {
 	case DP_B:
-		bit = DPB_HOTPLUG_INT_STATUS;
+		bit = DPB_HOTPLUG_LIVE_STATUS;
 		break;
 	case DP_C:
-		bit = DPC_HOTPLUG_INT_STATUS;
+		bit = DPC_HOTPLUG_LIVE_STATUS;
 		break;
 	case DP_D:
-		bit = DPD_HOTPLUG_INT_STATUS;
+		bit = DPD_HOTPLUG_LIVE_STATUS;
 		break;
 	default:
 		return connector_status_unknown;
 	}
 
-	temp = I915_READ(PORT_HOTPLUG_STAT);
-
-	if ((temp & bit) == 0)
+	if ((I915_READ(PORT_HOTPLUG_STAT) & bit) == 0)
 		return connector_status_disconnected;
 
 	return intel_dp_detect_dpcd(intel_dp);
@@ -2399,16 +2400,11 @@ int
 intel_trans_dp_port_sel(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_mode_config *mode_config = &dev->mode_config;
-	struct drm_encoder *encoder;
-
-	list_for_each_entry(encoder, &mode_config->encoder_list, head) {
-		struct intel_dp *intel_dp;
+	struct intel_encoder *encoder;
 
-		if (encoder->crtc != crtc)
-			continue;
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
+		struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
 
-		intel_dp = enc_to_intel_dp(encoder);
 		if (intel_dp->base.type == INTEL_OUTPUT_DISPLAYPORT ||
 		    intel_dp->base.type == INTEL_OUTPUT_EDP)
 			return intel_dp->output_reg;
@@ -2520,19 +2516,19 @@ intel_dp_init(struct drm_device *dev, int output_reg)
 		case DP_B:
 		case PCH_DP_B:
 			dev_priv->hotplug_supported_mask |=
-				HDMIB_HOTPLUG_INT_STATUS;
+				DPB_HOTPLUG_INT_STATUS;
 			name = "DPDDC-B";
 			break;
 		case DP_C:
 		case PCH_DP_C:
 			dev_priv->hotplug_supported_mask |=
-				HDMIC_HOTPLUG_INT_STATUS;
+				DPC_HOTPLUG_INT_STATUS;
 			name = "DPDDC-C";
 			break;
 		case DP_D:
 		case PCH_DP_D:
 			dev_priv->hotplug_supported_mask |=
-				HDMID_HOTPLUG_INT_STATUS;
+				DPD_HOTPLUG_INT_STATUS;
 			name = "DPDDC-D";
 			break;
 	}
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 3e0918834e7..84353559441 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -169,6 +169,7 @@ struct intel_crtc {
 	u8 lut_r[256], lut_g[256], lut_b[256];
 	int dpms_mode;
 	bool active; /* is the crtc on? independent of the dpms mode */
+	bool primary_disabled; /* is the crtc obscured by a plane? */
 	bool busy; /* is scanout buffer being updated frequently? */
 	struct timer_list idle_timer;
 	bool lowfreq_avail;
@@ -176,6 +177,11 @@ struct intel_crtc {
 	struct intel_unpin_work *unpin_work;
 	int fdi_lanes;
 
+	/* Display surface base address adjustement for pageflips. Note that on
+	 * gen4+ this only adjusts up to a tile, offsets within a tile are
+	 * handled in the hw itself (with the TILEOFF register). */
+	unsigned long dspaddr_offset;
+
 	struct drm_i915_gem_object *cursor_bo;
 	uint32_t cursor_addr;
 	int16_t cursor_x, cursor_y;
@@ -191,7 +197,6 @@ struct intel_plane {
 	struct drm_plane base;
 	enum pipe pipe;
 	struct drm_i915_gem_object *obj;
-	bool primary_disabled;
 	int max_downscale;
 	u32 lut_r[1024], lut_g[1024], lut_b[1024];
 	void (*update_plane)(struct drm_plane *plane,
@@ -301,6 +306,8 @@ struct intel_hdmi {
 	enum hdmi_force_audio force_audio;
 	void (*write_infoframe)(struct drm_encoder *encoder,
 				struct dip_infoframe *frame);
+	void (*set_infoframes)(struct drm_encoder *encoder,
+			       struct drm_display_mode *adjusted_mode);
 };
 
 static inline struct drm_crtc *
@@ -335,7 +342,6 @@ struct intel_fbc_work {
 };
 
 int intel_ddc_get_modes(struct drm_connector *c, struct i2c_adapter *adapter);
-extern bool intel_ddc_probe(struct intel_encoder *intel_encoder, int ddc_bus);
 
 extern void intel_attach_force_audio_property(struct drm_connector *connector);
 extern void intel_attach_broadcast_rgb_property(struct drm_connector *connector);
@@ -343,9 +349,6 @@ extern void intel_attach_broadcast_rgb_property(struct drm_connector *connector)
 extern void intel_crt_init(struct drm_device *dev);
 extern void intel_hdmi_init(struct drm_device *dev, int sdvox_reg);
 extern struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder);
-extern void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
-			    struct drm_display_mode *adjusted_mode);
-extern void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder);
 extern void intel_dip_infoframe_csum(struct dip_infoframe *avi_if);
 extern bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg,
 			    bool is_sdvob);
@@ -360,6 +363,8 @@ intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
 		 struct drm_display_mode *adjusted_mode);
 extern bool intel_dpd_is_edp(struct drm_device *dev);
 extern void intel_edp_link_config(struct intel_encoder *, int *, int *);
+extern int intel_edp_target_clock(struct intel_encoder *,
+				  struct drm_display_mode *mode);
 extern bool intel_encoder_is_pch_edp(struct drm_encoder *encoder);
 extern int intel_plane_init(struct drm_device *dev, enum pipe pipe);
 extern void intel_flush_display_plane(struct drm_i915_private *dev_priv,
@@ -372,13 +377,14 @@ extern void intel_fixed_panel_mode(struct drm_display_mode *fixed_mode,
 				   struct drm_display_mode *adjusted_mode);
 extern void intel_pch_panel_fitting(struct drm_device *dev,
 				    int fitting_mode,
-				    struct drm_display_mode *mode,
+				    const struct drm_display_mode *mode,
 				    struct drm_display_mode *adjusted_mode);
 extern u32 intel_panel_get_max_backlight(struct drm_device *dev);
 extern u32 intel_panel_get_backlight(struct drm_device *dev);
 extern void intel_panel_set_backlight(struct drm_device *dev, u32 level);
 extern int intel_panel_setup_backlight(struct drm_device *dev);
-extern void intel_panel_enable_backlight(struct drm_device *dev);
+extern void intel_panel_enable_backlight(struct drm_device *dev,
+					 enum pipe pipe);
 extern void intel_panel_disable_backlight(struct drm_device *dev);
 extern void intel_panel_destroy_backlight(struct drm_device *dev);
 extern enum drm_connector_status intel_panel_detect(struct drm_device *dev);
@@ -423,9 +429,6 @@ extern void intel_crtc_fb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
 extern void intel_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
 				    u16 *blue, int regno);
 extern void intel_enable_clock_gating(struct drm_device *dev);
-extern void ironlake_disable_rc6(struct drm_device *dev);
-extern void ironlake_enable_drps(struct drm_device *dev);
-extern void ironlake_disable_drps(struct drm_device *dev);
 
 extern int intel_pin_and_fence_fb_obj(struct drm_device *dev,
 				      struct drm_i915_gem_object *obj,
@@ -492,10 +495,11 @@ extern void intel_update_fbc(struct drm_device *dev);
 extern void intel_gpu_ips_init(struct drm_i915_private *dev_priv);
 extern void intel_gpu_ips_teardown(void);
 
-extern void gen6_enable_rps(struct drm_i915_private *dev_priv);
-extern void gen6_update_ring_freq(struct drm_i915_private *dev_priv);
-extern void gen6_disable_rps(struct drm_device *dev);
-extern void intel_init_emon(struct drm_device *dev);
+extern void intel_init_power_wells(struct drm_device *dev);
+extern void intel_enable_gt_powersave(struct drm_device *dev);
+extern void intel_disable_gt_powersave(struct drm_device *dev);
+extern void gen6_gt_check_fifodbg(struct drm_i915_private *dev_priv);
+extern void ironlake_teardown_rc6(struct drm_device *dev);
 
 extern void intel_ddi_dpms(struct drm_encoder *encoder, int mode);
 extern void intel_ddi_mode_set(struct drm_encoder *encoder,
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 60ba50b956f..36c542e5036 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -136,7 +136,7 @@ static int intel_dvo_mode_valid(struct drm_connector *connector,
 }
 
 static bool intel_dvo_mode_fixup(struct drm_encoder *encoder,
-				 struct drm_display_mode *mode,
+				 const struct drm_display_mode *mode,
 				 struct drm_display_mode *adjusted_mode)
 {
 	struct intel_dvo *intel_dvo = enc_to_intel_dvo(encoder);
diff --git a/drivers/gpu/drm/i915/intel_fb.c b/drivers/gpu/drm/i915/intel_fb.c
index bf8690720a0..97f673523b9 100644
--- a/drivers/gpu/drm/i915/intel_fb.c
+++ b/drivers/gpu/drm/i915/intel_fb.c
@@ -65,7 +65,7 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct fb_info *info;
 	struct drm_framebuffer *fb;
-	struct drm_mode_fb_cmd2 mode_cmd;
+	struct drm_mode_fb_cmd2 mode_cmd = {};
 	struct drm_i915_gem_object *obj;
 	struct device *device = &dev->pdev->dev;
 	int size, ret;
@@ -140,7 +140,9 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 	info->fix.smem_start = dev->mode_config.fb_base + obj->gtt_offset;
 	info->fix.smem_len = size;
 
-	info->screen_base = ioremap_wc(dev->agp->base + obj->gtt_offset, size);
+	info->screen_base =
+		ioremap_wc(dev_priv->mm.gtt_base_addr + obj->gtt_offset,
+			   size);
 	if (!info->screen_base) {
 		ret = -ENOSPC;
 		goto out_unpin;
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index 2ead3bf7c21..98f602427eb 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -37,6 +37,19 @@
 #include "i915_drm.h"
 #include "i915_drv.h"
 
+static void
+assert_hdmi_port_disabled(struct intel_hdmi *intel_hdmi)
+{
+	struct drm_device *dev = intel_hdmi->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t enabled_bits;
+
+	enabled_bits = IS_HASWELL(dev) ? DDI_BUF_CTL_ENABLE : SDVO_ENABLE;
+
+	WARN(I915_READ(intel_hdmi->sdvox_reg) & enabled_bits,
+	     "HDMI port enabled, expecting disabled\n");
+}
+
 struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder)
 {
 	return container_of(encoder, struct intel_hdmi, base.base);
@@ -121,36 +134,31 @@ static void g4x_write_infoframe(struct drm_encoder *encoder,
 	uint32_t *data = (uint32_t *)frame;
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	u32 val = I915_READ(VIDEO_DIP_CTL);
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
 
-	val &= ~VIDEO_DIP_PORT_MASK;
-	if (intel_hdmi->sdvox_reg == SDVOB)
-		val |= VIDEO_DIP_PORT_B;
-	else if (intel_hdmi->sdvox_reg == SDVOC)
-		val |= VIDEO_DIP_PORT_C;
-	else
-		return;
+	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
 	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
 	val |= g4x_infoframe_index(frame);
 
 	val &= ~g4x_infoframe_enable(frame);
-	val |= VIDEO_DIP_ENABLE;
 
 	I915_WRITE(VIDEO_DIP_CTL, val);
 
+	mmiowb();
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(VIDEO_DIP_DATA, *data);
 		data++;
 	}
+	mmiowb();
 
 	val |= g4x_infoframe_enable(frame);
 	val &= ~VIDEO_DIP_FREQ_MASK;
 	val |= VIDEO_DIP_FREQ_VSYNC;
 
 	I915_WRITE(VIDEO_DIP_CTL, val);
+	POSTING_READ(VIDEO_DIP_CTL);
 }
 
 static void ibx_write_infoframe(struct drm_encoder *encoder,
@@ -160,46 +168,32 @@ static void ibx_write_infoframe(struct drm_encoder *encoder,
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
 	u32 val = I915_READ(reg);
 
-	val &= ~VIDEO_DIP_PORT_MASK;
-	switch (intel_hdmi->sdvox_reg) {
-	case HDMIB:
-		val |= VIDEO_DIP_PORT_B;
-		break;
-	case HDMIC:
-		val |= VIDEO_DIP_PORT_C;
-		break;
-	case HDMID:
-		val |= VIDEO_DIP_PORT_D;
-		break;
-	default:
-		return;
-	}
-
-	intel_wait_for_vblank(dev, intel_crtc->pipe);
+	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
 	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
 	val |= g4x_infoframe_index(frame);
 
 	val &= ~g4x_infoframe_enable(frame);
-	val |= VIDEO_DIP_ENABLE;
 
 	I915_WRITE(reg, val);
 
+	mmiowb();
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
 		data++;
 	}
+	mmiowb();
 
 	val |= g4x_infoframe_enable(frame);
 	val &= ~VIDEO_DIP_FREQ_MASK;
 	val |= VIDEO_DIP_FREQ_VSYNC;
 
 	I915_WRITE(reg, val);
+	POSTING_READ(reg);
 }
 
 static void cpt_write_infoframe(struct drm_encoder *encoder,
@@ -213,32 +207,31 @@ static void cpt_write_infoframe(struct drm_encoder *encoder,
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
 	u32 val = I915_READ(reg);
 
-	intel_wait_for_vblank(dev, intel_crtc->pipe);
+	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
 	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
 	val |= g4x_infoframe_index(frame);
 
 	/* The DIP control register spec says that we need to update the AVI
 	 * infoframe without clearing its enable bit */
-	if (frame->type == DIP_TYPE_AVI)
-		val |= VIDEO_DIP_ENABLE_AVI;
-	else
+	if (frame->type != DIP_TYPE_AVI)
 		val &= ~g4x_infoframe_enable(frame);
 
-	val |= VIDEO_DIP_ENABLE;
-
 	I915_WRITE(reg, val);
 
+	mmiowb();
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
 		data++;
 	}
+	mmiowb();
 
 	val |= g4x_infoframe_enable(frame);
 	val &= ~VIDEO_DIP_FREQ_MASK;
 	val |= VIDEO_DIP_FREQ_VSYNC;
 
 	I915_WRITE(reg, val);
+	POSTING_READ(reg);
 }
 
 static void vlv_write_infoframe(struct drm_encoder *encoder,
@@ -252,26 +245,28 @@ static void vlv_write_infoframe(struct drm_encoder *encoder,
 	unsigned i, len = DIP_HEADER_SIZE + frame->len;
 	u32 val = I915_READ(reg);
 
-	intel_wait_for_vblank(dev, intel_crtc->pipe);
+	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
 	val &= ~(VIDEO_DIP_SELECT_MASK | 0xf); /* clear DIP data offset */
 	val |= g4x_infoframe_index(frame);
 
 	val &= ~g4x_infoframe_enable(frame);
-	val |= VIDEO_DIP_ENABLE;
 
 	I915_WRITE(reg, val);
 
+	mmiowb();
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(VLV_TVIDEO_DIP_DATA(intel_crtc->pipe), *data);
 		data++;
 	}
+	mmiowb();
 
 	val |= g4x_infoframe_enable(frame);
 	val &= ~VIDEO_DIP_FREQ_MASK;
 	val |= VIDEO_DIP_FREQ_VSYNC;
 
 	I915_WRITE(reg, val);
+	POSTING_READ(reg);
 }
 
 static void hsw_write_infoframe(struct drm_encoder *encoder,
@@ -289,18 +284,19 @@ static void hsw_write_infoframe(struct drm_encoder *encoder,
 	if (data_reg == 0)
 		return;
 
-	intel_wait_for_vblank(dev, intel_crtc->pipe);
-
 	val &= ~hsw_infoframe_enable(frame);
 	I915_WRITE(ctl_reg, val);
 
+	mmiowb();
 	for (i = 0; i < len; i += 4) {
 		I915_WRITE(data_reg + i, *data);
 		data++;
 	}
+	mmiowb();
 
 	val |= hsw_infoframe_enable(frame);
 	I915_WRITE(ctl_reg, val);
+	POSTING_READ(ctl_reg);
 }
 
 static void intel_set_infoframe(struct drm_encoder *encoder,
@@ -308,14 +304,11 @@ static void intel_set_infoframe(struct drm_encoder *encoder,
 {
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 
-	if (!intel_hdmi->has_hdmi_sink)
-		return;
-
 	intel_dip_infoframe_csum(frame);
 	intel_hdmi->write_infoframe(encoder, frame);
 }
 
-void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
+static void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
 					 struct drm_display_mode *adjusted_mode)
 {
 	struct dip_infoframe avi_if = {
@@ -330,7 +323,7 @@ void intel_hdmi_set_avi_infoframe(struct drm_encoder *encoder,
 	intel_set_infoframe(encoder, &avi_if);
 }
 
-void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
+static void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
 {
 	struct dip_infoframe spd_if;
 
@@ -345,6 +338,223 @@ void intel_hdmi_set_spd_infoframe(struct drm_encoder *encoder)
 	intel_set_infoframe(encoder, &spd_if);
 }
 
+static void g4x_set_infoframes(struct drm_encoder *encoder,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	u32 reg = VIDEO_DIP_CTL;
+	u32 val = I915_READ(reg);
+	u32 port;
+
+	assert_hdmi_port_disabled(intel_hdmi);
+
+	/* If the registers were not initialized yet, they might be zeroes,
+	 * which means we're selecting the AVI DIP and we're setting its
+	 * frequency to once. This seems to really confuse the HW and make
+	 * things stop working (the register spec says the AVI always needs to
+	 * be sent every VSync). So here we avoid writing to the register more
+	 * than we need and also explicitly select the AVI DIP and explicitly
+	 * set its frequency to every VSync. Avoiding to write it twice seems to
+	 * be enough to solve the problem, but being defensive shouldn't hurt us
+	 * either. */
+	val |= VIDEO_DIP_SELECT_AVI | VIDEO_DIP_FREQ_VSYNC;
+
+	if (!intel_hdmi->has_hdmi_sink) {
+		if (!(val & VIDEO_DIP_ENABLE))
+			return;
+		val &= ~VIDEO_DIP_ENABLE;
+		I915_WRITE(reg, val);
+		POSTING_READ(reg);
+		return;
+	}
+
+	switch (intel_hdmi->sdvox_reg) {
+	case SDVOB:
+		port = VIDEO_DIP_PORT_B;
+		break;
+	case SDVOC:
+		port = VIDEO_DIP_PORT_C;
+		break;
+	default:
+		return;
+	}
+
+	if (port != (val & VIDEO_DIP_PORT_MASK)) {
+		if (val & VIDEO_DIP_ENABLE) {
+			val &= ~VIDEO_DIP_ENABLE;
+			I915_WRITE(reg, val);
+			POSTING_READ(reg);
+		}
+		val &= ~VIDEO_DIP_PORT_MASK;
+		val |= port;
+	}
+
+	val |= VIDEO_DIP_ENABLE;
+	val &= ~VIDEO_DIP_ENABLE_VENDOR;
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
+static void ibx_set_infoframes(struct drm_encoder *encoder,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	u32 reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+	u32 port;
+
+	assert_hdmi_port_disabled(intel_hdmi);
+
+	/* See the big comment in g4x_set_infoframes() */
+	val |= VIDEO_DIP_SELECT_AVI | VIDEO_DIP_FREQ_VSYNC;
+
+	if (!intel_hdmi->has_hdmi_sink) {
+		if (!(val & VIDEO_DIP_ENABLE))
+			return;
+		val &= ~VIDEO_DIP_ENABLE;
+		I915_WRITE(reg, val);
+		POSTING_READ(reg);
+		return;
+	}
+
+	switch (intel_hdmi->sdvox_reg) {
+	case HDMIB:
+		port = VIDEO_DIP_PORT_B;
+		break;
+	case HDMIC:
+		port = VIDEO_DIP_PORT_C;
+		break;
+	case HDMID:
+		port = VIDEO_DIP_PORT_D;
+		break;
+	default:
+		return;
+	}
+
+	if (port != (val & VIDEO_DIP_PORT_MASK)) {
+		if (val & VIDEO_DIP_ENABLE) {
+			val &= ~VIDEO_DIP_ENABLE;
+			I915_WRITE(reg, val);
+			POSTING_READ(reg);
+		}
+		val &= ~VIDEO_DIP_PORT_MASK;
+		val |= port;
+	}
+
+	val |= VIDEO_DIP_ENABLE;
+	val &= ~(VIDEO_DIP_ENABLE_VENDOR | VIDEO_DIP_ENABLE_GAMUT |
+		 VIDEO_DIP_ENABLE_GCP);
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
+static void cpt_set_infoframes(struct drm_encoder *encoder,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	u32 reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+
+	assert_hdmi_port_disabled(intel_hdmi);
+
+	/* See the big comment in g4x_set_infoframes() */
+	val |= VIDEO_DIP_SELECT_AVI | VIDEO_DIP_FREQ_VSYNC;
+
+	if (!intel_hdmi->has_hdmi_sink) {
+		if (!(val & VIDEO_DIP_ENABLE))
+			return;
+		val &= ~(VIDEO_DIP_ENABLE | VIDEO_DIP_ENABLE_AVI);
+		I915_WRITE(reg, val);
+		POSTING_READ(reg);
+		return;
+	}
+
+	/* Set both together, unset both together: see the spec. */
+	val |= VIDEO_DIP_ENABLE | VIDEO_DIP_ENABLE_AVI;
+	val &= ~(VIDEO_DIP_ENABLE_VENDOR | VIDEO_DIP_ENABLE_GAMUT |
+		 VIDEO_DIP_ENABLE_GCP);
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
+static void vlv_set_infoframes(struct drm_encoder *encoder,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	u32 reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+
+	assert_hdmi_port_disabled(intel_hdmi);
+
+	/* See the big comment in g4x_set_infoframes() */
+	val |= VIDEO_DIP_SELECT_AVI | VIDEO_DIP_FREQ_VSYNC;
+
+	if (!intel_hdmi->has_hdmi_sink) {
+		if (!(val & VIDEO_DIP_ENABLE))
+			return;
+		val &= ~VIDEO_DIP_ENABLE;
+		I915_WRITE(reg, val);
+		POSTING_READ(reg);
+		return;
+	}
+
+	val |= VIDEO_DIP_ENABLE;
+	val &= ~(VIDEO_DIP_ENABLE_VENDOR | VIDEO_DIP_ENABLE_GAMUT |
+		 VIDEO_DIP_ENABLE_GCP);
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
+static void hsw_set_infoframes(struct drm_encoder *encoder,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
+	u32 reg = HSW_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	u32 val = I915_READ(reg);
+
+	assert_hdmi_port_disabled(intel_hdmi);
+
+	if (!intel_hdmi->has_hdmi_sink) {
+		I915_WRITE(reg, 0);
+		POSTING_READ(reg);
+		return;
+	}
+
+	val &= ~(VIDEO_DIP_ENABLE_VSC_HSW | VIDEO_DIP_ENABLE_GCP_HSW |
+		 VIDEO_DIP_ENABLE_VS_HSW | VIDEO_DIP_ENABLE_GMP_HSW);
+
+	I915_WRITE(reg, val);
+	POSTING_READ(reg);
+
+	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
+	intel_hdmi_set_spd_infoframe(encoder);
+}
+
 static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 				struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
@@ -355,7 +565,7 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	u32 sdvox;
 
-	sdvox = SDVO_ENCODING_HDMI | SDVO_BORDER_ENABLE;
+	sdvox = SDVO_ENCODING_HDMI;
 	if (!HAS_PCH_SPLIT(dev))
 		sdvox |= intel_hdmi->color_range;
 	if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
@@ -382,14 +592,13 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
 
 	if (HAS_PCH_CPT(dev))
 		sdvox |= PORT_TRANS_SEL_CPT(intel_crtc->pipe);
-	else if (intel_crtc->pipe == 1)
+	else if (intel_crtc->pipe == PIPE_B)
 		sdvox |= SDVO_PIPE_B_SELECT;
 
 	I915_WRITE(intel_hdmi->sdvox_reg, sdvox);
 	POSTING_READ(intel_hdmi->sdvox_reg);
 
-	intel_hdmi_set_avi_infoframe(encoder, adjusted_mode);
-	intel_hdmi_set_spd_infoframe(encoder);
+	intel_hdmi->set_infoframes(encoder, adjusted_mode);
 }
 
 static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
@@ -405,6 +614,36 @@ static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
 
 	temp = I915_READ(intel_hdmi->sdvox_reg);
 
+	/* HW workaround for IBX, we need to move the port to transcoder A
+	 * before disabling it. */
+	if (HAS_PCH_IBX(dev)) {
+		struct drm_crtc *crtc = encoder->crtc;
+		int pipe = crtc ? to_intel_crtc(crtc)->pipe : -1;
+
+		if (mode != DRM_MODE_DPMS_ON) {
+			if (temp & SDVO_PIPE_B_SELECT) {
+				temp &= ~SDVO_PIPE_B_SELECT;
+				I915_WRITE(intel_hdmi->sdvox_reg, temp);
+				POSTING_READ(intel_hdmi->sdvox_reg);
+
+				/* Again we need to write this twice. */
+				I915_WRITE(intel_hdmi->sdvox_reg, temp);
+				POSTING_READ(intel_hdmi->sdvox_reg);
+
+				/* Transcoder selection bits only update
+				 * effectively on vblank. */
+				if (crtc)
+					intel_wait_for_vblank(dev, pipe);
+				else
+					msleep(50);
+			}
+		} else {
+			/* Restore the transcoder select bit. */
+			if (pipe == PIPE_B)
+				enable_bits |= SDVO_PIPE_B_SELECT;
+		}
+	}
+
 	/* HW workaround, need to toggle enable bit off and on for 12bpc, but
 	 * we do this anyway which shows more stable in testing.
 	 */
@@ -446,12 +685,33 @@ static int intel_hdmi_mode_valid(struct drm_connector *connector,
 }
 
 static bool intel_hdmi_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	return true;
 }
 
+static bool g4x_hdmi_connected(struct intel_hdmi *intel_hdmi)
+{
+	struct drm_device *dev = intel_hdmi->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t bit;
+
+	switch (intel_hdmi->sdvox_reg) {
+	case SDVOB:
+		bit = HDMIB_HOTPLUG_LIVE_STATUS;
+		break;
+	case SDVOC:
+		bit = HDMIC_HOTPLUG_LIVE_STATUS;
+		break;
+	default:
+		bit = 0;
+		break;
+	}
+
+	return I915_READ(PORT_HOTPLUG_STAT) & bit;
+}
+
 static enum drm_connector_status
 intel_hdmi_detect(struct drm_connector *connector, bool force)
 {
@@ -460,6 +720,9 @@ intel_hdmi_detect(struct drm_connector *connector, bool force)
 	struct edid *edid;
 	enum drm_connector_status status = connector_status_disconnected;
 
+	if (IS_G4X(connector->dev) && !g4x_hdmi_connected(intel_hdmi))
+		return status;
+
 	intel_hdmi->has_hdmi_sink = false;
 	intel_hdmi->has_audio = false;
 	edid = drm_get_edid(connector,
@@ -633,7 +896,6 @@ void intel_hdmi_init(struct drm_device *dev, int sdvox_reg)
 	struct intel_encoder *intel_encoder;
 	struct intel_connector *intel_connector;
 	struct intel_hdmi *intel_hdmi;
-	int i;
 
 	intel_hdmi = kzalloc(sizeof(struct intel_hdmi), GFP_KERNEL);
 	if (!intel_hdmi)
@@ -710,26 +972,19 @@ void intel_hdmi_init(struct drm_device *dev, int sdvox_reg)
 
 	if (!HAS_PCH_SPLIT(dev)) {
 		intel_hdmi->write_infoframe = g4x_write_infoframe;
-		I915_WRITE(VIDEO_DIP_CTL, 0);
+		intel_hdmi->set_infoframes = g4x_set_infoframes;
 	} else if (IS_VALLEYVIEW(dev)) {
 		intel_hdmi->write_infoframe = vlv_write_infoframe;
-		for_each_pipe(i)
-			I915_WRITE(VLV_TVIDEO_DIP_CTL(i), 0);
+		intel_hdmi->set_infoframes = vlv_set_infoframes;
 	} else if (IS_HASWELL(dev)) {
-		/* FIXME: Haswell has a new set of DIP frame registers, but we are
-		 * just doing the minimal required for HDMI to work at this stage.
-		 */
 		intel_hdmi->write_infoframe = hsw_write_infoframe;
-		for_each_pipe(i)
-			I915_WRITE(HSW_TVIDEO_DIP_CTL(i), 0);
+		intel_hdmi->set_infoframes = hsw_set_infoframes;
 	} else if (HAS_PCH_IBX(dev)) {
 		intel_hdmi->write_infoframe = ibx_write_infoframe;
-		for_each_pipe(i)
-			I915_WRITE(TVIDEO_DIP_CTL(i), 0);
+		intel_hdmi->set_infoframes = ibx_set_infoframes;
 	} else {
 		intel_hdmi->write_infoframe = cpt_write_infoframe;
-		for_each_pipe(i)
-			I915_WRITE(TVIDEO_DIP_CTL(i), 0);
+		intel_hdmi->set_infoframes = cpt_set_infoframes;
 	}
 
 	if (IS_HASWELL(dev))
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 08eb04c787e..e05c0d3e344 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -71,6 +71,7 @@ static struct intel_lvds *intel_attached_lvds(struct drm_connector *connector)
 static void intel_lvds_enable(struct intel_lvds *intel_lvds)
 {
 	struct drm_device *dev = intel_lvds->base.base.dev;
+	struct intel_crtc *intel_crtc = to_intel_crtc(intel_lvds->base.base.crtc);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 ctl_reg, lvds_reg, stat_reg;
 
@@ -107,7 +108,7 @@ static void intel_lvds_enable(struct intel_lvds *intel_lvds)
 	if (wait_for((I915_READ(stat_reg) & PP_ON) != 0, 1000))
 		DRM_ERROR("timed out waiting for panel to power on\n");
 
-	intel_panel_enable_backlight(dev);
+	intel_panel_enable_backlight(dev, intel_crtc->pipe);
 }
 
 static void intel_lvds_disable(struct intel_lvds *intel_lvds)
@@ -228,14 +229,14 @@ static inline u32 panel_fitter_scaling(u32 source, u32 target)
 }
 
 static bool intel_lvds_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_lvds *intel_lvds = to_intel_lvds(encoder);
-	struct drm_encoder *tmp_encoder;
+	struct intel_encoder *tmp_encoder;
 	u32 pfit_control = 0, pfit_pgm_ratios = 0, border = 0;
 	int pipe;
 
@@ -246,8 +247,8 @@ static bool intel_lvds_mode_fixup(struct drm_encoder *encoder,
 	}
 
 	/* Should never happen!! */
-	list_for_each_entry(tmp_encoder, &dev->mode_config.encoder_list, head) {
-		if (tmp_encoder != encoder && tmp_encoder->crtc == encoder->crtc) {
+	for_each_encoder_on_crtc(dev, encoder->crtc, tmp_encoder) {
+		if (&tmp_encoder->base != encoder) {
 			DRM_ERROR("Can't enable LVDS and another "
 			       "encoder on the same pipe\n");
 			return false;
@@ -408,13 +409,7 @@ static void intel_lvds_prepare(struct drm_encoder *encoder)
 {
 	struct intel_lvds *intel_lvds = to_intel_lvds(encoder);
 
-	/*
-	 * Prior to Ironlake, we must disable the pipe if we want to adjust
-	 * the panel fitter. However at all other times we can just reset
-	 * the registers regardless.
-	 */
-	if (!HAS_PCH_SPLIT(encoder->dev) && intel_lvds->pfit_dirty)
-		intel_lvds_disable(intel_lvds);
+	intel_lvds_disable(intel_lvds);
 }
 
 static void intel_lvds_commit(struct drm_encoder *encoder)
@@ -777,6 +772,14 @@ static const struct dmi_system_id intel_no_lvds[] = {
 			DMI_MATCH(DMI_BOARD_NAME, "MS-7469"),
 		},
 	},
+	{
+		.callback = intel_no_lvds_dmi_callback,
+		.ident = "ZOTAC ZBOXSD-ID12/ID13",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ZOTAC"),
+			DMI_MATCH(DMI_BOARD_NAME, "ZBOXSD-ID12/ID13"),
+		},
+	},
 
 	{ }	/* terminating entry */
 };
@@ -967,6 +970,8 @@ bool intel_lvds_init(struct drm_device *dev)
 	intel_encoder->clone_mask = (1 << INTEL_LVDS_CLONE_BIT);
 	if (HAS_PCH_SPLIT(dev))
 		intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
+	else if (IS_GEN4(dev))
+		intel_encoder->crtc_mask = (1 << 0) | (1 << 1);
 	else
 		intel_encoder->crtc_mask = (1 << 1);
 
@@ -1074,35 +1079,14 @@ bool intel_lvds_init(struct drm_device *dev)
 		goto failed;
 
 out:
+	/*
+	 * Unlock registers and just
+	 * leave them unlocked
+	 */
 	if (HAS_PCH_SPLIT(dev)) {
-		u32 pwm;
-
-		pipe = (I915_READ(PCH_LVDS) & LVDS_PIPEB_SELECT) ? 1 : 0;
-
-		/* make sure PWM is enabled and locked to the LVDS pipe */
-		pwm = I915_READ(BLC_PWM_CPU_CTL2);
-		if (pipe == 0 && (pwm & PWM_PIPE_B))
-			I915_WRITE(BLC_PWM_CPU_CTL2, pwm & ~PWM_ENABLE);
-		if (pipe)
-			pwm |= PWM_PIPE_B;
-		else
-			pwm &= ~PWM_PIPE_B;
-		I915_WRITE(BLC_PWM_CPU_CTL2, pwm | PWM_ENABLE);
-
-		pwm = I915_READ(BLC_PWM_PCH_CTL1);
-		pwm |= PWM_PCH_ENABLE;
-		I915_WRITE(BLC_PWM_PCH_CTL1, pwm);
-		/*
-		 * Unlock registers and just
-		 * leave them unlocked
-		 */
 		I915_WRITE(PCH_PP_CONTROL,
 			   I915_READ(PCH_PP_CONTROL) | PANEL_UNLOCK_REGS);
 	} else {
-		/*
-		 * Unlock registers and just
-		 * leave them unlocked
-		 */
 		I915_WRITE(PP_CONTROL,
 			   I915_READ(PP_CONTROL) | PANEL_UNLOCK_REGS);
 	}
diff --git a/drivers/gpu/drm/i915/intel_modes.c b/drivers/gpu/drm/i915/intel_modes.c
index d67ec3a51e4..45848b9b670 100644
--- a/drivers/gpu/drm/i915/intel_modes.c
+++ b/drivers/gpu/drm/i915/intel_modes.c
@@ -33,34 +33,6 @@
 #include "i915_drv.h"
 
 /**
- * intel_ddc_probe
- *
- */
-bool intel_ddc_probe(struct intel_encoder *intel_encoder, int ddc_bus)
-{
-	struct drm_i915_private *dev_priv = intel_encoder->base.dev->dev_private;
-	u8 out_buf[] = { 0x0, 0x0};
-	u8 buf[2];
-	struct i2c_msg msgs[] = {
-		{
-			.addr = DDC_ADDR,
-			.flags = 0,
-			.len = 1,
-			.buf = out_buf,
-		},
-		{
-			.addr = DDC_ADDR,
-			.flags = I2C_M_RD,
-			.len = 1,
-			.buf = buf,
-		}
-	};
-
-	return i2c_transfer(intel_gmbus_get_adapter(dev_priv, ddc_bus),
-			    msgs, 2) == 2;
-}
-
-/**
  * intel_ddc_get_modes - get modelist from monitor
  * @connector: DRM connector device to use
  * @adapter: i2c adapter
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index 458743da377..830d0dd610e 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -226,7 +226,7 @@ static int intel_overlay_do_wait_request(struct intel_overlay *overlay,
 	}
 	overlay->last_flip_req = request->seqno;
 	overlay->flip_tail = tail;
-	ret = i915_wait_request(ring, overlay->last_flip_req);
+	ret = i915_wait_seqno(ring, overlay->last_flip_req);
 	if (ret)
 		return ret;
 	i915_gem_retire_requests(dev);
@@ -452,7 +452,7 @@ static int intel_overlay_recover_from_interrupt(struct intel_overlay *overlay)
 	if (overlay->last_flip_req == 0)
 		return 0;
 
-	ret = i915_wait_request(ring, overlay->last_flip_req);
+	ret = i915_wait_seqno(ring, overlay->last_flip_req);
 	if (ret)
 		return ret;
 	i915_gem_retire_requests(dev);
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 2a1625d84a6..10c7d39034e 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -56,7 +56,7 @@ intel_fixed_panel_mode(struct drm_display_mode *fixed_mode,
 void
 intel_pch_panel_fitting(struct drm_device *dev,
 			int fitting_mode,
-			struct drm_display_mode *mode,
+			const struct drm_display_mode *mode,
 			struct drm_display_mode *adjusted_mode)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -287,9 +287,24 @@ void intel_panel_disable_backlight(struct drm_device *dev)
 
 	dev_priv->backlight_enabled = false;
 	intel_panel_actually_set_backlight(dev, 0);
+
+	if (INTEL_INFO(dev)->gen >= 4) {
+		uint32_t reg, tmp;
+
+		reg = HAS_PCH_SPLIT(dev) ? BLC_PWM_CPU_CTL2 : BLC_PWM_CTL2;
+
+		I915_WRITE(reg, I915_READ(reg) & ~BLM_PWM_ENABLE);
+
+		if (HAS_PCH_SPLIT(dev)) {
+			tmp = I915_READ(BLC_PWM_PCH_CTL1);
+			tmp &= ~BLM_PCH_PWM_ENABLE;
+			I915_WRITE(BLC_PWM_PCH_CTL1, tmp);
+		}
+	}
 }
 
-void intel_panel_enable_backlight(struct drm_device *dev)
+void intel_panel_enable_backlight(struct drm_device *dev,
+				  enum pipe pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
@@ -298,6 +313,40 @@ void intel_panel_enable_backlight(struct drm_device *dev)
 
 	dev_priv->backlight_enabled = true;
 	intel_panel_actually_set_backlight(dev, dev_priv->backlight_level);
+
+	if (INTEL_INFO(dev)->gen >= 4) {
+		uint32_t reg, tmp;
+
+		reg = HAS_PCH_SPLIT(dev) ? BLC_PWM_CPU_CTL2 : BLC_PWM_CTL2;
+
+
+		tmp = I915_READ(reg);
+
+		/* Note that this can also get called through dpms changes. And
+		 * we don't track the backlight dpms state, hence check whether
+		 * we have to do anything first. */
+		if (tmp & BLM_PWM_ENABLE)
+			return;
+
+		if (dev_priv->num_pipe == 3)
+			tmp &= ~BLM_PIPE_SELECT_IVB;
+		else
+			tmp &= ~BLM_PIPE_SELECT;
+
+		tmp |= BLM_PIPE(pipe);
+		tmp &= ~BLM_PWM_ENABLE;
+
+		I915_WRITE(reg, tmp);
+		POSTING_READ(reg);
+		I915_WRITE(reg, tmp | BLM_PWM_ENABLE);
+
+		if (HAS_PCH_SPLIT(dev)) {
+			tmp = I915_READ(BLC_PWM_PCH_CTL1);
+			tmp |= BLM_PCH_PWM_ENABLE;
+			tmp &= ~BLM_PCH_OVERRIDE_ENABLE;
+			I915_WRITE(BLC_PWM_PCH_CTL1, tmp);
+		}
+	}
 }
 
 static void intel_panel_init_backlight(struct drm_device *dev)
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index d0ce2a5b1d3..94aabcaa3a6 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -387,8 +387,6 @@ void intel_update_fbc(struct drm_device *dev)
 	struct drm_i915_gem_object *obj;
 	int enable_fbc;
 
-	DRM_DEBUG_KMS("\n");
-
 	if (!i915_powersave)
 		return;
 
@@ -405,7 +403,9 @@ void intel_update_fbc(struct drm_device *dev)
 	 *   - going to an unsupported config (interlace, pixel multiply, etc.)
 	 */
 	list_for_each_entry(tmp_crtc, &dev->mode_config.crtc_list, head) {
-		if (tmp_crtc->enabled && tmp_crtc->fb) {
+		if (tmp_crtc->enabled &&
+		    !to_intel_crtc(tmp_crtc)->primary_disabled &&
+		    tmp_crtc->fb) {
 			if (crtc) {
 				DRM_DEBUG_KMS("more than one pipe active, disabling compression\n");
 				dev_priv->no_fbc_reason = FBC_MULTIPLE_PIPES;
@@ -2182,7 +2182,7 @@ bool ironlake_set_drps(struct drm_device *dev, u8 val)
 	return true;
 }
 
-void ironlake_enable_drps(struct drm_device *dev)
+static void ironlake_enable_drps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 rgvmodectl = I915_READ(MEMMODECTL);
@@ -2246,7 +2246,7 @@ void ironlake_enable_drps(struct drm_device *dev)
 	getrawmonotonic(&dev_priv->last_time2);
 }
 
-void ironlake_disable_drps(struct drm_device *dev)
+static void ironlake_disable_drps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u16 rgvswctl = I915_READ16(MEMSWCTL);
@@ -2299,10 +2299,11 @@ void gen6_set_rps(struct drm_device *dev, u8 val)
 	dev_priv->cur_delay = val;
 }
 
-void gen6_disable_rps(struct drm_device *dev)
+static void gen6_disable_rps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	I915_WRITE(GEN6_RC_CONTROL, 0);
 	I915_WRITE(GEN6_RPNSWREQ, 1 << 31);
 	I915_WRITE(GEN6_PMINTRMSK, 0xffffffff);
 	I915_WRITE(GEN6_PMIER, 0);
@@ -2332,9 +2333,11 @@ int intel_enable_rc6(const struct drm_device *dev)
 	if (INTEL_INFO(dev)->gen == 5)
 		return 0;
 
-	/* Sorry Haswell, no RC6 for you for now. */
+	/* On Haswell, only RC6 is available. So let's enable it by default to
+	 * provide better testing and coverage since the beginning.
+	 */
 	if (IS_HASWELL(dev))
-		return 0;
+		return INTEL_RC6_ENABLE;
 
 	/*
 	 * Disable rc6 on Sandybridge
@@ -2347,8 +2350,9 @@ int intel_enable_rc6(const struct drm_device *dev)
 	return (INTEL_RC6_ENABLE | INTEL_RC6p_ENABLE);
 }
 
-void gen6_enable_rps(struct drm_i915_private *dev_priv)
+static void gen6_enable_rps(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring;
 	u32 rp_state_cap;
 	u32 gt_perf_status;
@@ -2357,6 +2361,8 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	int rc6_mode;
 	int i;
 
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
 	/* Here begins a magic sequence of register writes to enable
 	 * auto-downclocking.
 	 *
@@ -2364,7 +2370,6 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	 * userspace...
 	 */
 	I915_WRITE(GEN6_RC_STATE, 0);
-	mutex_lock(&dev_priv->dev->struct_mutex);
 
 	/* Clear the DBG now so we don't confuse earlier errors */
 	if ((gtfifodbg = I915_READ(GTFIFODBG))) {
@@ -2400,20 +2405,24 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RC6p_THRESHOLD, 100000);
 	I915_WRITE(GEN6_RC6pp_THRESHOLD, 64000); /* unused */
 
+	/* Check if we are enabling RC6 */
 	rc6_mode = intel_enable_rc6(dev_priv->dev);
 	if (rc6_mode & INTEL_RC6_ENABLE)
 		rc6_mask |= GEN6_RC_CTL_RC6_ENABLE;
 
-	if (rc6_mode & INTEL_RC6p_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6p_ENABLE;
+	/* We don't use those on Haswell */
+	if (!IS_HASWELL(dev)) {
+		if (rc6_mode & INTEL_RC6p_ENABLE)
+			rc6_mask |= GEN6_RC_CTL_RC6p_ENABLE;
 
-	if (rc6_mode & INTEL_RC6pp_ENABLE)
-		rc6_mask |= GEN6_RC_CTL_RC6pp_ENABLE;
+		if (rc6_mode & INTEL_RC6pp_ENABLE)
+			rc6_mask |= GEN6_RC_CTL_RC6pp_ENABLE;
+	}
 
 	DRM_INFO("Enabling RC6 states: RC6 %s, RC6p %s, RC6pp %s\n",
-			(rc6_mode & INTEL_RC6_ENABLE) ? "on" : "off",
-			(rc6_mode & INTEL_RC6p_ENABLE) ? "on" : "off",
-			(rc6_mode & INTEL_RC6pp_ENABLE) ? "on" : "off");
+			(rc6_mask & GEN6_RC_CTL_RC6_ENABLE) ? "on" : "off",
+			(rc6_mask & GEN6_RC_CTL_RC6p_ENABLE) ? "on" : "off",
+			(rc6_mask & GEN6_RC_CTL_RC6pp_ENABLE) ? "on" : "off");
 
 	I915_WRITE(GEN6_RC_CONTROL,
 		   rc6_mask |
@@ -2431,10 +2440,19 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RP_INTERRUPT_LIMITS,
 		   dev_priv->max_delay << 24 |
 		   dev_priv->min_delay << 16);
-	I915_WRITE(GEN6_RP_UP_THRESHOLD, 10000);
-	I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 1000000);
-	I915_WRITE(GEN6_RP_UP_EI, 100000);
-	I915_WRITE(GEN6_RP_DOWN_EI, 5000000);
+
+	if (IS_HASWELL(dev)) {
+		I915_WRITE(GEN6_RP_UP_THRESHOLD, 59400);
+		I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 245000);
+		I915_WRITE(GEN6_RP_UP_EI, 66000);
+		I915_WRITE(GEN6_RP_DOWN_EI, 350000);
+	} else {
+		I915_WRITE(GEN6_RP_UP_THRESHOLD, 10000);
+		I915_WRITE(GEN6_RP_DOWN_THRESHOLD, 1000000);
+		I915_WRITE(GEN6_RP_UP_EI, 100000);
+		I915_WRITE(GEN6_RP_DOWN_EI, 5000000);
+	}
+
 	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
 	I915_WRITE(GEN6_RP_CONTROL,
 		   GEN6_RP_MEDIA_TURBO |
@@ -2442,7 +2460,7 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 		   GEN6_RP_MEDIA_IS_GFX |
 		   GEN6_RP_ENABLE |
 		   GEN6_RP_UP_BUSY_AVG |
-		   GEN6_RP_DOWN_IDLE_CONT);
+		   (IS_HASWELL(dev) ? GEN7_RP_DOWN_IDLE_AVG : GEN6_RP_DOWN_IDLE_CONT));
 
 	if (wait_for((I915_READ(GEN6_PCODE_MAILBOX) & GEN6_PCODE_READY) == 0,
 		     500))
@@ -2473,14 +2491,7 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	gen6_set_rps(dev_priv->dev, (gt_perf_status & 0xff00) >> 8);
 
 	/* requires MSI enabled */
-	I915_WRITE(GEN6_PMIER,
-		   GEN6_PM_MBOX_EVENT |
-		   GEN6_PM_THERMAL_EVENT |
-		   GEN6_PM_RP_DOWN_TIMEOUT |
-		   GEN6_PM_RP_UP_THRESHOLD |
-		   GEN6_PM_RP_DOWN_THRESHOLD |
-		   GEN6_PM_RP_UP_EI_EXPIRED |
-		   GEN6_PM_RP_DOWN_EI_EXPIRED);
+	I915_WRITE(GEN6_PMIER, GEN6_PM_DEFERRED_EVENTS);
 	spin_lock_irq(&dev_priv->rps_lock);
 	WARN_ON(dev_priv->pm_iir != 0);
 	I915_WRITE(GEN6_PMIMR, 0);
@@ -2489,15 +2500,17 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_PMINTRMSK, 0);
 
 	gen6_gt_force_wake_put(dev_priv);
-	mutex_unlock(&dev_priv->dev->struct_mutex);
 }
 
-void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
+static void gen6_update_ring_freq(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	int min_freq = 15;
 	int gpu_freq, ia_freq, max_ia_freq;
 	int scaling_factor = 180;
 
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
 	max_ia_freq = cpufreq_quick_get_max(0);
 	/*
 	 * Default to measured freq if none found, PCU will ensure we don't go
@@ -2509,8 +2522,6 @@ void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
 	/* Convert from kHz to MHz */
 	max_ia_freq /= 1000;
 
-	mutex_lock(&dev_priv->dev->struct_mutex);
-
 	/*
 	 * For each potential GPU frequency, load a ring frequency we'd like
 	 * to use for memory access.  We do this by specifying the IA frequency
@@ -2541,11 +2552,9 @@ void gen6_update_ring_freq(struct drm_i915_private *dev_priv)
 			continue;
 		}
 	}
-
-	mutex_unlock(&dev_priv->dev->struct_mutex);
 }
 
-static void ironlake_teardown_rc6(struct drm_device *dev)
+void ironlake_teardown_rc6(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
@@ -2562,7 +2571,7 @@ static void ironlake_teardown_rc6(struct drm_device *dev)
 	}
 }
 
-void ironlake_disable_rc6(struct drm_device *dev)
+static void ironlake_disable_rc6(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
@@ -2578,8 +2587,6 @@ void ironlake_disable_rc6(struct drm_device *dev)
 		I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
 		POSTING_READ(RSTDBYCTL);
 	}
-
-	ironlake_teardown_rc6(dev);
 }
 
 static int ironlake_setup_rc6(struct drm_device *dev)
@@ -2601,7 +2608,7 @@ static int ironlake_setup_rc6(struct drm_device *dev)
 	return 0;
 }
 
-void ironlake_enable_rc6(struct drm_device *dev)
+static void ironlake_enable_rc6(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_ring_buffer *ring = &dev_priv->ring[RCS];
@@ -2613,12 +2620,11 @@ void ironlake_enable_rc6(struct drm_device *dev)
 	if (!intel_enable_rc6(dev))
 		return;
 
-	mutex_lock(&dev->struct_mutex);
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
 	ret = ironlake_setup_rc6(dev);
-	if (ret) {
-		mutex_unlock(&dev->struct_mutex);
+	if (ret)
 		return;
-	}
 
 	/*
 	 * GPU can automatically power down the render unit if given a page
@@ -2627,7 +2633,6 @@ void ironlake_enable_rc6(struct drm_device *dev)
 	ret = intel_ring_begin(ring, 6);
 	if (ret) {
 		ironlake_teardown_rc6(dev);
-		mutex_unlock(&dev->struct_mutex);
 		return;
 	}
 
@@ -2652,13 +2657,11 @@ void ironlake_enable_rc6(struct drm_device *dev)
 	if (ret) {
 		DRM_ERROR("failed to enable ironlake power power savings\n");
 		ironlake_teardown_rc6(dev);
-		mutex_unlock(&dev->struct_mutex);
 		return;
 	}
 
 	I915_WRITE(PWRCTXA, dev_priv->pwrctx->gtt_offset | PWRCTX_EN);
 	I915_WRITE(RSTDBYCTL, I915_READ(RSTDBYCTL) & ~RCX_SW_EXIT);
-	mutex_unlock(&dev->struct_mutex);
 }
 
 static unsigned long intel_pxfreq(u32 vidfreq)
@@ -3154,8 +3157,7 @@ void intel_gpu_ips_teardown(void)
 	i915_mch_dev = NULL;
 	spin_unlock(&mchdev_lock);
 }
-
-void intel_init_emon(struct drm_device *dev)
+static void intel_init_emon(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 lcfuse;
@@ -3226,6 +3228,28 @@ void intel_init_emon(struct drm_device *dev)
 	dev_priv->corr = (lcfuse & LCFUSE_HIV_MASK);
 }
 
+void intel_disable_gt_powersave(struct drm_device *dev)
+{
+	if (IS_IRONLAKE_M(dev)) {
+		ironlake_disable_drps(dev);
+		ironlake_disable_rc6(dev);
+	} else if (INTEL_INFO(dev)->gen >= 6 && !IS_VALLEYVIEW(dev)) {
+		gen6_disable_rps(dev);
+	}
+}
+
+void intel_enable_gt_powersave(struct drm_device *dev)
+{
+	if (IS_IRONLAKE_M(dev)) {
+		ironlake_enable_drps(dev);
+		ironlake_enable_rc6(dev);
+		intel_init_emon(dev);
+	} else if ((IS_GEN6(dev) || IS_GEN7(dev)) && !IS_VALLEYVIEW(dev)) {
+		gen6_enable_rps(dev);
+		gen6_update_ring_freq(dev);
+	}
+}
+
 static void ironlake_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -3328,8 +3352,12 @@ static void gen6_init_clock_gating(struct drm_device *dev)
 	 *
 	 * According to the spec, bit 11 (RCCUNIT) must also be set,
 	 * but we didn't debug actual testcases to find it out.
+	 *
+	 * Also apply WaDisableVDSUnitClockGating and
+	 * WaDisableRCPBUnitClockGating.
 	 */
 	I915_WRITE(GEN6_UCGCTL2,
+		   GEN7_VDSUNIT_CLOCK_GATE_DISABLE |
 		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
 		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
 
@@ -3357,6 +3385,9 @@ static void gen6_init_clock_gating(struct drm_device *dev)
 		   ILK_DPARB_CLK_GATE  |
 		   ILK_DPFD_CLK_GATE);
 
+	I915_WRITE(GEN6_MBCTL, I915_READ(GEN6_MBCTL) |
+		   GEN6_MBCTL_ENABLE_BOOT_FETCH);
+
 	for_each_pipe(pipe) {
 		I915_WRITE(DSPCNTR(pipe),
 			   I915_READ(DSPCNTR(pipe)) |
@@ -3377,7 +3408,7 @@ static void gen7_setup_fixed_func_scheduler(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN7_FF_THREAD_MODE, reg);
 }
 
-static void ivybridge_init_clock_gating(struct drm_device *dev)
+static void haswell_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
@@ -3427,13 +3458,24 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 	/* WaDisable4x2SubspanOptimization */
 	I915_WRITE(CACHE_MODE_1,
 		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+
+	/* XXX: This is a workaround for early silicon revisions and should be
+	 * removed later.
+	 */
+	I915_WRITE(WM_DBG,
+			I915_READ(WM_DBG) |
+			WM_DBG_DISALLOW_MULTIPLE_LP |
+			WM_DBG_DISALLOW_SPRITE |
+			WM_DBG_DISALLOW_MAXFIFO);
+
 }
 
-static void valleyview_init_clock_gating(struct drm_device *dev)
+static void ivybridge_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
 	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+	uint32_t snpcr;
 
 	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
 
@@ -3441,10 +3483,77 @@ static void valleyview_init_clock_gating(struct drm_device *dev)
 	I915_WRITE(WM2_LP_ILK, 0);
 	I915_WRITE(WM1_LP_ILK, 0);
 
-	/* According to the spec, bit 13 (RCZUNIT) must be set on IVB.
+	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
+
+	I915_WRITE(IVB_CHICKEN3,
+		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
+		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
+
+	/* Apply the WaDisableRHWOOptimizationForRenderHang workaround. */
+	I915_WRITE(GEN7_COMMON_SLICE_CHICKEN1,
+		   GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC);
+
+	/* WaApplyL3ControlAndL3ChickenMode requires those two on Ivy Bridge */
+	I915_WRITE(GEN7_L3CNTLREG1,
+			GEN7_WA_FOR_GEN7_L3_CONTROL);
+	I915_WRITE(GEN7_L3_CHICKEN_MODE_REGISTER,
+			GEN7_WA_L3_CHICKEN_MODE);
+
+	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
+	 * gating disable must be set.  Failure to set it results in
+	 * flickering pixels due to Z write ordering failures after
+	 * some amount of runtime in the Mesa "fire" demo, and Unigine
+	 * Sanctuary and Tropics, and apparently anything else with
+	 * alpha test or pixel discard.
+	 *
+	 * According to the spec, bit 11 (RCCUNIT) must also be set,
+	 * but we didn't debug actual testcases to find it out.
+	 *
+	 * According to the spec, bit 13 (RCZUNIT) must be set on IVB.
 	 * This implements the WaDisableRCZUnitClockGating workaround.
 	 */
-	I915_WRITE(GEN6_UCGCTL2, GEN6_RCZUNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(GEN6_UCGCTL2,
+		   GEN6_RCZUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
+
+	/* This is required by WaCatErrorRejectionIssue */
+	I915_WRITE(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG,
+			I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
+			GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
+
+	for_each_pipe(pipe) {
+		I915_WRITE(DSPCNTR(pipe),
+			   I915_READ(DSPCNTR(pipe)) |
+			   DISPPLANE_TRICKLE_FEED_DISABLE);
+		intel_flush_display_plane(dev_priv, pipe);
+	}
+
+	I915_WRITE(GEN6_MBCTL, I915_READ(GEN6_MBCTL) |
+		   GEN6_MBCTL_ENABLE_BOOT_FETCH);
+
+	gen7_setup_fixed_func_scheduler(dev_priv);
+
+	/* WaDisable4x2SubspanOptimization */
+	I915_WRITE(CACHE_MODE_1,
+		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+
+	snpcr = I915_READ(GEN6_MBCUNIT_SNPCR);
+	snpcr &= ~GEN6_MBC_SNPCR_MASK;
+	snpcr |= GEN6_MBC_SNPCR_MED;
+	I915_WRITE(GEN6_MBCUNIT_SNPCR, snpcr);
+}
+
+static void valleyview_init_clock_gating(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe;
+	uint32_t dspclk_gate = VRHUNIT_CLOCK_GATE_DISABLE;
+
+	I915_WRITE(PCH_DSPCLK_GATE_D, dspclk_gate);
+
+	I915_WRITE(WM3_LP_ILK, 0);
+	I915_WRITE(WM2_LP_ILK, 0);
+	I915_WRITE(WM1_LP_ILK, 0);
 
 	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
 
@@ -3465,6 +3574,35 @@ static void valleyview_init_clock_gating(struct drm_device *dev)
 		   I915_READ(GEN7_SQ_CHICKEN_MBCUNIT_CONFIG) |
 		   GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB);
 
+	I915_WRITE(GEN6_MBCTL, I915_READ(GEN6_MBCTL) |
+		   GEN6_MBCTL_ENABLE_BOOT_FETCH);
+
+
+	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
+	 * gating disable must be set.  Failure to set it results in
+	 * flickering pixels due to Z write ordering failures after
+	 * some amount of runtime in the Mesa "fire" demo, and Unigine
+	 * Sanctuary and Tropics, and apparently anything else with
+	 * alpha test or pixel discard.
+	 *
+	 * According to the spec, bit 11 (RCCUNIT) must also be set,
+	 * but we didn't debug actual testcases to find it out.
+	 *
+	 * According to the spec, bit 13 (RCZUNIT) must be set on IVB.
+	 * This implements the WaDisableRCZUnitClockGating workaround.
+	 *
+	 * Also apply WaDisableVDSUnitClockGating and
+	 * WaDisableRCPBUnitClockGating.
+	 */
+	I915_WRITE(GEN6_UCGCTL2,
+		   GEN7_VDSUNIT_CLOCK_GATE_DISABLE |
+		   GEN7_TDLUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCZUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(GEN7_UCGCTL4, GEN7_L3BANK2X_CLOCK_GATE_DISABLE);
+
 	for_each_pipe(pipe) {
 		I915_WRITE(DSPCNTR(pipe),
 			   I915_READ(DSPCNTR(pipe)) |
@@ -3474,6 +3612,19 @@ static void valleyview_init_clock_gating(struct drm_device *dev)
 
 	I915_WRITE(CACHE_MODE_1,
 		   _MASKED_BIT_ENABLE(PIXEL_SUBSPAN_COLLECT_OPT_DISABLE));
+
+	/*
+	 * On ValleyView, the GUnit needs to signal the GT
+	 * when flip and other events complete.  So enable
+	 * all the GUnit->GT interrupts here
+	 */
+	I915_WRITE(VLV_DPFLIPSTAT, PIPEB_LINE_COMPARE_INT_EN |
+		   PIPEB_HLINE_INT_EN | PIPEB_VBLANK_INT_EN |
+		   SPRITED_FLIPDONE_INT_EN | SPRITEC_FLIPDONE_INT_EN |
+		   PLANEB_FLIPDONE_INT_EN | PIPEA_LINE_COMPARE_INT_EN |
+		   PIPEA_HLINE_INT_EN | PIPEA_VBLANK_INT_EN |
+		   SPRITEB_FLIPDONE_INT_EN | SPRITEA_FLIPDONE_INT_EN |
+		   PLANEA_FLIPDONE_INT_EN);
 }
 
 static void g4x_init_clock_gating(struct drm_device *dev)
@@ -3681,34 +3832,6 @@ void intel_init_pm(struct drm_device *dev)
 
 	/* For FIFO watermark updates */
 	if (HAS_PCH_SPLIT(dev)) {
-		dev_priv->display.force_wake_get = __gen6_gt_force_wake_get;
-		dev_priv->display.force_wake_put = __gen6_gt_force_wake_put;
-
-		/* IVB configs may use multi-threaded forcewake */
-		if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
-			u32	ecobus;
-
-			/* A small trick here - if the bios hasn't configured MT forcewake,
-			 * and if the device is in RC6, then force_wake_mt_get will not wake
-			 * the device and the ECOBUS read will return zero. Which will be
-			 * (correctly) interpreted by the test below as MT forcewake being
-			 * disabled.
-			 */
-			mutex_lock(&dev->struct_mutex);
-			__gen6_gt_force_wake_mt_get(dev_priv);
-			ecobus = I915_READ_NOTRACE(ECOBUS);
-			__gen6_gt_force_wake_mt_put(dev_priv);
-			mutex_unlock(&dev->struct_mutex);
-
-			if (ecobus & FORCEWAKE_MT_ENABLE) {
-				DRM_DEBUG_KMS("Using MT version of forcewake\n");
-				dev_priv->display.force_wake_get =
-					__gen6_gt_force_wake_mt_get;
-				dev_priv->display.force_wake_put =
-					__gen6_gt_force_wake_mt_put;
-			}
-		}
-
 		if (HAS_PCH_IBX(dev))
 			dev_priv->display.init_pch_clock_gating = ibx_init_clock_gating;
 		else if (HAS_PCH_CPT(dev))
@@ -3756,7 +3879,7 @@ void intel_init_pm(struct drm_device *dev)
 					      "Disable CxSR\n");
 				dev_priv->display.update_wm = NULL;
 			}
-			dev_priv->display.init_clock_gating = ivybridge_init_clock_gating;
+			dev_priv->display.init_clock_gating = haswell_init_clock_gating;
 			dev_priv->display.sanitize_pm = gen6_sanitize_pm;
 		} else
 			dev_priv->display.update_wm = NULL;
@@ -3764,8 +3887,6 @@ void intel_init_pm(struct drm_device *dev)
 		dev_priv->display.update_wm = valleyview_update_wm;
 		dev_priv->display.init_clock_gating =
 			valleyview_init_clock_gating;
-		dev_priv->display.force_wake_get = vlv_force_wake_get;
-		dev_priv->display.force_wake_put = vlv_force_wake_put;
 	} else if (IS_PINEVIEW(dev)) {
 		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
 					    dev_priv->is_ddr3,
@@ -3811,10 +3932,196 @@ void intel_init_pm(struct drm_device *dev)
 		else
 			dev_priv->display.get_fifo_size = i830_get_fifo_size;
 	}
+}
+
+static void __gen6_gt_wait_for_thread_c0(struct drm_i915_private *dev_priv)
+{
+	u32 gt_thread_status_mask;
+
+	if (IS_HASWELL(dev_priv->dev))
+		gt_thread_status_mask = GEN6_GT_THREAD_STATUS_CORE_MASK_HSW;
+	else
+		gt_thread_status_mask = GEN6_GT_THREAD_STATUS_CORE_MASK;
 
-	/* We attempt to init the necessary power wells early in the initialization
-	 * time, so the subsystems that expect power to be enabled can work.
+	/* w/a for a sporadic read returning 0 by waiting for the GT
+	 * thread to wake up.
 	 */
-	intel_init_power_wells(dev);
+	if (wait_for_atomic_us((I915_READ_NOTRACE(GEN6_GT_THREAD_STATUS_REG) & gt_thread_status_mask) == 0, 500))
+		DRM_ERROR("GT thread status wait timed out\n");
+}
+
+static void __gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
+{
+	u32 forcewake_ack;
+
+	if (IS_HASWELL(dev_priv->dev))
+		forcewake_ack = FORCEWAKE_ACK_HSW;
+	else
+		forcewake_ack = FORCEWAKE_ACK;
+
+	if (wait_for_atomic_us((I915_READ_NOTRACE(forcewake_ack) & 1) == 0, 500))
+		DRM_ERROR("Force wake wait timed out\n");
+
+	I915_WRITE_NOTRACE(FORCEWAKE, 1);
+
+	if (wait_for_atomic_us((I915_READ_NOTRACE(forcewake_ack) & 1), 500))
+		DRM_ERROR("Force wake wait timed out\n");
+
+	__gen6_gt_wait_for_thread_c0(dev_priv);
+}
+
+static void __gen6_gt_force_wake_mt_get(struct drm_i915_private *dev_priv)
+{
+	u32 forcewake_ack;
+
+	if (IS_HASWELL(dev_priv->dev))
+		forcewake_ack = FORCEWAKE_ACK_HSW;
+	else
+		forcewake_ack = FORCEWAKE_MT_ACK;
+
+	if (wait_for_atomic_us((I915_READ_NOTRACE(forcewake_ack) & 1) == 0, 500))
+		DRM_ERROR("Force wake wait timed out\n");
+
+	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_ENABLE(1));
+
+	if (wait_for_atomic_us((I915_READ_NOTRACE(forcewake_ack) & 1), 500))
+		DRM_ERROR("Force wake wait timed out\n");
+
+	__gen6_gt_wait_for_thread_c0(dev_priv);
+}
+
+/*
+ * Generally this is called implicitly by the register read function. However,
+ * if some sequence requires the GT to not power down then this function should
+ * be called at the beginning of the sequence followed by a call to
+ * gen6_gt_force_wake_put() at the end of the sequence.
+ */
+void gen6_gt_force_wake_get(struct drm_i915_private *dev_priv)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->gt_lock, irqflags);
+	if (dev_priv->forcewake_count++ == 0)
+		dev_priv->gt.force_wake_get(dev_priv);
+	spin_unlock_irqrestore(&dev_priv->gt_lock, irqflags);
+}
+
+void gen6_gt_check_fifodbg(struct drm_i915_private *dev_priv)
+{
+	u32 gtfifodbg;
+	gtfifodbg = I915_READ_NOTRACE(GTFIFODBG);
+	if (WARN(gtfifodbg & GT_FIFO_CPU_ERROR_MASK,
+	     "MMIO read or write has been dropped %x\n", gtfifodbg))
+		I915_WRITE_NOTRACE(GTFIFODBG, GT_FIFO_CPU_ERROR_MASK);
+}
+
+static void __gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
+{
+	I915_WRITE_NOTRACE(FORCEWAKE, 0);
+	/* The below doubles as a POSTING_READ */
+	gen6_gt_check_fifodbg(dev_priv);
+}
+
+static void __gen6_gt_force_wake_mt_put(struct drm_i915_private *dev_priv)
+{
+	I915_WRITE_NOTRACE(FORCEWAKE_MT, _MASKED_BIT_DISABLE(1));
+	/* The below doubles as a POSTING_READ */
+	gen6_gt_check_fifodbg(dev_priv);
+}
+
+/*
+ * see gen6_gt_force_wake_get()
+ */
+void gen6_gt_force_wake_put(struct drm_i915_private *dev_priv)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->gt_lock, irqflags);
+	if (--dev_priv->forcewake_count == 0)
+		dev_priv->gt.force_wake_put(dev_priv);
+	spin_unlock_irqrestore(&dev_priv->gt_lock, irqflags);
+}
+
+int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
+{
+	int ret = 0;
+
+	if (dev_priv->gt_fifo_count < GT_FIFO_NUM_RESERVED_ENTRIES) {
+		int loop = 500;
+		u32 fifo = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
+		while (fifo <= GT_FIFO_NUM_RESERVED_ENTRIES && loop--) {
+			udelay(10);
+			fifo = I915_READ_NOTRACE(GT_FIFO_FREE_ENTRIES);
+		}
+		if (WARN_ON(loop < 0 && fifo <= GT_FIFO_NUM_RESERVED_ENTRIES))
+			++ret;
+		dev_priv->gt_fifo_count = fifo;
+	}
+	dev_priv->gt_fifo_count--;
+
+	return ret;
+}
+
+static void vlv_force_wake_get(struct drm_i915_private *dev_priv)
+{
+	/* Already awake? */
+	if ((I915_READ(0x130094) & 0xa1) == 0xa1)
+		return;
+
+	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffffffff);
+	POSTING_READ(FORCEWAKE_VLV);
+
+	if (wait_for_atomic_us((I915_READ_NOTRACE(FORCEWAKE_ACK_VLV) & 1), 500))
+		DRM_ERROR("Force wake wait timed out\n");
+
+	__gen6_gt_wait_for_thread_c0(dev_priv);
+}
+
+static void vlv_force_wake_put(struct drm_i915_private *dev_priv)
+{
+	I915_WRITE_NOTRACE(FORCEWAKE_VLV, 0xffff0000);
+	/* FIXME: confirm VLV behavior with Punit folks */
+	POSTING_READ(FORCEWAKE_VLV);
+}
+
+void intel_gt_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	spin_lock_init(&dev_priv->gt_lock);
+
+	if (IS_VALLEYVIEW(dev)) {
+		dev_priv->gt.force_wake_get = vlv_force_wake_get;
+		dev_priv->gt.force_wake_put = vlv_force_wake_put;
+	} else if (INTEL_INFO(dev)->gen >= 6) {
+		dev_priv->gt.force_wake_get = __gen6_gt_force_wake_get;
+		dev_priv->gt.force_wake_put = __gen6_gt_force_wake_put;
+
+		/* IVB configs may use multi-threaded forcewake */
+		if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev)) {
+			u32 ecobus;
+
+			/* A small trick here - if the bios hasn't configured
+			 * MT forcewake, and if the device is in RC6, then
+			 * force_wake_mt_get will not wake the device and the
+			 * ECOBUS read will return zero. Which will be
+			 * (correctly) interpreted by the test below as MT
+			 * forcewake being disabled.
+			 */
+			mutex_lock(&dev->struct_mutex);
+			__gen6_gt_force_wake_mt_get(dev_priv);
+			ecobus = I915_READ_NOTRACE(ECOBUS);
+			__gen6_gt_force_wake_mt_put(dev_priv);
+			mutex_unlock(&dev->struct_mutex);
+
+			if (ecobus & FORCEWAKE_MT_ENABLE) {
+				DRM_DEBUG_KMS("Using MT version of forcewake\n");
+				dev_priv->gt.force_wake_get =
+					__gen6_gt_force_wake_mt_get;
+				dev_priv->gt.force_wake_put =
+					__gen6_gt_force_wake_mt_put;
+			}
+		}
+	}
 }
 
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index e5b84ff89ca..bf0195a96d5 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -219,19 +219,28 @@ gen6_render_ring_flush(struct intel_ring_buffer *ring,
 	int ret;
 
 	/* Force SNB workarounds for PIPE_CONTROL flushes */
-	intel_emit_post_sync_nonzero_flush(ring);
+	ret = intel_emit_post_sync_nonzero_flush(ring);
+	if (ret)
+		return ret;
 
 	/* Just flush everything.  Experiments have shown that reducing the
 	 * number of bits based on the write domains has little performance
 	 * impact.
 	 */
 	flags |= PIPE_CONTROL_RENDER_TARGET_CACHE_FLUSH;
+	flags |= PIPE_CONTROL_TLB_INVALIDATE;
 	flags |= PIPE_CONTROL_INSTRUCTION_CACHE_INVALIDATE;
 	flags |= PIPE_CONTROL_TEXTURE_CACHE_INVALIDATE;
 	flags |= PIPE_CONTROL_DEPTH_CACHE_FLUSH;
 	flags |= PIPE_CONTROL_VF_CACHE_INVALIDATE;
 	flags |= PIPE_CONTROL_CONST_CACHE_INVALIDATE;
 	flags |= PIPE_CONTROL_STATE_CACHE_INVALIDATE;
+	/*
+	 * Ensure that any following seqno writes only happen when the render
+	 * cache is indeed flushed (but only if the caller actually wants that).
+	 */
+	if (flush_domains)
+		flags |= PIPE_CONTROL_CS_STALL;
 
 	ret = intel_ring_begin(ring, 6);
 	if (ret)
@@ -433,11 +442,21 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 		 */
 		I915_WRITE(CACHE_MODE_0,
 			   _MASKED_BIT_DISABLE(CM0_STC_EVICT_DISABLE_LRA_SNB));
+
+		/* This is not explicitly set for GEN6, so read the register.
+		 * see intel_ring_mi_set_context() for why we care.
+		 * TODO: consider explicitly setting the bit for GEN5
+		 */
+		ring->itlb_before_ctx_switch =
+			!!(I915_READ(GFX_MODE) & GFX_TLB_INVALIDATE_ALWAYS);
 	}
 
 	if (INTEL_INFO(dev)->gen >= 6)
 		I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_FORCE_ORDERING));
 
+	if (IS_IVYBRIDGE(dev))
+		I915_WRITE_IMR(ring, ~GEN6_RENDER_L3_PARITY_ERROR);
+
 	return ret;
 }
 
@@ -825,7 +844,11 @@ gen6_ring_get_irq(struct intel_ring_buffer *ring)
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (ring->irq_refcount++ == 0) {
-		I915_WRITE_IMR(ring, ~ring->irq_enable_mask);
+		if (IS_IVYBRIDGE(dev) && ring->id == RCS)
+			I915_WRITE_IMR(ring, ~(ring->irq_enable_mask |
+						GEN6_RENDER_L3_PARITY_ERROR));
+		else
+			I915_WRITE_IMR(ring, ~ring->irq_enable_mask);
 		dev_priv->gt_irq_mask &= ~ring->irq_enable_mask;
 		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
 		POSTING_READ(GTIMR);
@@ -844,7 +867,10 @@ gen6_ring_put_irq(struct intel_ring_buffer *ring)
 
 	spin_lock_irqsave(&dev_priv->irq_lock, flags);
 	if (--ring->irq_refcount == 0) {
-		I915_WRITE_IMR(ring, ~0);
+		if (IS_IVYBRIDGE(dev) && ring->id == RCS)
+			I915_WRITE_IMR(ring, ~GEN6_RENDER_L3_PARITY_ERROR);
+		else
+			I915_WRITE_IMR(ring, ~0);
 		dev_priv->gt_irq_mask |= ring->irq_enable_mask;
 		I915_WRITE(GTIMR, dev_priv->gt_irq_mask);
 		POSTING_READ(GTIMR);
@@ -946,6 +972,7 @@ static int init_status_page(struct intel_ring_buffer *ring)
 	ring->status_page.gfx_addr = obj->gtt_offset;
 	ring->status_page.page_addr = kmap(obj->pages[0]);
 	if (ring->status_page.page_addr == NULL) {
+		ret = -ENOMEM;
 		goto err_unpin;
 	}
 	ring->status_page.obj = obj;
@@ -969,6 +996,7 @@ static int intel_init_ring_buffer(struct drm_device *dev,
 				  struct intel_ring_buffer *ring)
 {
 	struct drm_i915_gem_object *obj;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
 	ring->dev = dev;
@@ -1002,8 +1030,9 @@ static int intel_init_ring_buffer(struct drm_device *dev,
 	if (ret)
 		goto err_unpin;
 
-	ring->virtual_start = ioremap_wc(dev->agp->base + obj->gtt_offset,
-					 ring->size);
+	ring->virtual_start =
+		ioremap_wc(dev_priv->mm.gtt->gma_bus_addr + obj->gtt_offset,
+			   ring->size);
 	if (ring->virtual_start == NULL) {
 		DRM_ERROR("Failed to map ringbuffer.\n");
 		ret = -EINVAL;
@@ -1089,20 +1118,9 @@ static int intel_wrap_ring_buffer(struct intel_ring_buffer *ring)
 
 static int intel_ring_wait_seqno(struct intel_ring_buffer *ring, u32 seqno)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
-	bool was_interruptible;
 	int ret;
 
-	/* XXX As we have not yet audited all the paths to check that
-	 * they are ready for ERESTARTSYS from intel_ring_begin, do not
-	 * allow us to be interruptible by a signal.
-	 */
-	was_interruptible = dev_priv->mm.interruptible;
-	dev_priv->mm.interruptible = false;
-
-	ret = i915_wait_request(ring, seqno);
-
-	dev_priv->mm.interruptible = was_interruptible;
+	ret = i915_wait_seqno(ring, seqno);
 	if (!ret)
 		i915_gem_retire_requests_ring(ring);
 
@@ -1200,8 +1218,10 @@ int intel_wait_ring_buffer(struct intel_ring_buffer *ring, int n)
 		}
 
 		msleep(1);
-		if (atomic_read(&dev_priv->mm.wedged))
-			return -EAGAIN;
+
+		ret = i915_gem_check_wedge(dev_priv, dev_priv->mm.interruptible);
+		if (ret)
+			return ret;
 	} while (!time_after(jiffies, end));
 	trace_i915_ring_wait_end(ring);
 	return -EBUSY;
@@ -1210,12 +1230,13 @@ int intel_wait_ring_buffer(struct intel_ring_buffer *ring, int n)
 int intel_ring_begin(struct intel_ring_buffer *ring,
 		     int num_dwords)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	drm_i915_private_t *dev_priv = ring->dev->dev_private;
 	int n = 4*num_dwords;
 	int ret;
 
-	if (unlikely(atomic_read(&dev_priv->mm.wedged)))
-		return -EIO;
+	ret = i915_gem_check_wedge(dev_priv, dev_priv->mm.interruptible);
+	if (ret)
+		return ret;
 
 	if (unlikely(ring->tail + n > ring->effective_size)) {
 		ret = intel_wrap_ring_buffer(ring);
@@ -1250,20 +1271,31 @@ static void gen6_bsd_ring_write_tail(struct intel_ring_buffer *ring,
 	drm_i915_private_t *dev_priv = ring->dev->dev_private;
 
        /* Every tail move must follow the sequence below */
+
+	/* Disable notification that the ring is IDLE. The GT
+	 * will then assume that it is busy and bring it out of rc6.
+	 */
 	I915_WRITE(GEN6_BSD_SLEEP_PSMI_CONTROL,
-		GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_MODIFY_MASK |
-		GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_DISABLE);
-	I915_WRITE(GEN6_BSD_RNCID, 0x0);
+		   _MASKED_BIT_ENABLE(GEN6_BSD_SLEEP_MSG_DISABLE));
 
+	/* Clear the context id. Here be magic! */
+	I915_WRITE64(GEN6_BSD_RNCID, 0x0);
+
+	/* Wait for the ring not to be idle, i.e. for it to wake up. */
 	if (wait_for((I915_READ(GEN6_BSD_SLEEP_PSMI_CONTROL) &
-		GEN6_BSD_SLEEP_PSMI_CONTROL_IDLE_INDICATOR) == 0,
-		50))
-	DRM_ERROR("timed out waiting for IDLE Indicator\n");
+		      GEN6_BSD_SLEEP_INDICATOR) == 0,
+		     50))
+		DRM_ERROR("timed out waiting for the BSD ring to wake up\n");
 
+	/* Now that the ring is fully powered up, update the tail */
 	I915_WRITE_TAIL(ring, value);
+	POSTING_READ(RING_TAIL(ring->mmio_base));
+
+	/* Let the ring send IDLE messages to the GT again,
+	 * and so let it sleep to conserve power when idle.
+	 */
 	I915_WRITE(GEN6_BSD_SLEEP_PSMI_CONTROL,
-		GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_MODIFY_MASK |
-		GEN6_BSD_SLEEP_PSMI_CONTROL_RC_ILDL_MESSAGE_ENABLE);
+		   _MASKED_BIT_DISABLE(GEN6_BSD_SLEEP_MSG_DISABLE));
 }
 
 static int gen6_ring_flush(struct intel_ring_buffer *ring,
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 55d3da26bae..1d3c81fdad9 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -113,9 +113,17 @@ struct  intel_ring_buffer {
 	 * Do we have some not yet emitted requests outstanding?
 	 */
 	u32 outstanding_lazy_request;
+	bool gpu_caches_dirty;
 
 	wait_queue_head_t irq_queue;
 
+	/**
+	 * Do an explicit TLB flush before MI_SET_CONTEXT
+	 */
+	bool itlb_before_ctx_switch;
+	struct i915_hw_context *default_context;
+	struct drm_i915_gem_object *last_context_obj;
+
 	void *private;
 };
 
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index b6a9d45fc3c..26a6a4d0d07 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -140,9 +140,6 @@ struct intel_sdvo {
 
 	/* DDC bus used by this SDVO encoder */
 	uint8_t ddc_bus;
-
-	/* Input timings for adjusted_mode */
-	struct intel_sdvo_dtd input_dtd;
 };
 
 struct intel_sdvo_connector {
@@ -938,7 +935,7 @@ static bool intel_sdvo_set_tv_format(struct intel_sdvo *intel_sdvo)
 
 static bool
 intel_sdvo_set_output_timings_from_mode(struct intel_sdvo *intel_sdvo,
-					struct drm_display_mode *mode)
+					const struct drm_display_mode *mode)
 {
 	struct intel_sdvo_dtd output_dtd;
 
@@ -953,11 +950,15 @@ intel_sdvo_set_output_timings_from_mode(struct intel_sdvo *intel_sdvo,
 	return true;
 }
 
+/* Asks the sdvo controller for the preferred input mode given the output mode.
+ * Unfortunately we have to set up the full output mode to do that. */
 static bool
-intel_sdvo_set_input_timings_for_mode(struct intel_sdvo *intel_sdvo,
-					struct drm_display_mode *mode,
-					struct drm_display_mode *adjusted_mode)
+intel_sdvo_get_preferred_input_mode(struct intel_sdvo *intel_sdvo,
+				    const struct drm_display_mode *mode,
+				    struct drm_display_mode *adjusted_mode)
 {
+	struct intel_sdvo_dtd input_dtd;
+
 	/* Reset the input timing to the screen. Assume always input 0. */
 	if (!intel_sdvo_set_target_input(intel_sdvo))
 		return false;
@@ -969,16 +970,16 @@ intel_sdvo_set_input_timings_for_mode(struct intel_sdvo *intel_sdvo,
 		return false;
 
 	if (!intel_sdvo_get_preferred_input_timing(intel_sdvo,
-						   &intel_sdvo->input_dtd))
+						   &input_dtd))
 		return false;
 
-	intel_sdvo_get_mode_from_dtd(adjusted_mode, &intel_sdvo->input_dtd);
+	intel_sdvo_get_mode_from_dtd(adjusted_mode, &input_dtd);
 
 	return true;
 }
 
 static bool intel_sdvo_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct intel_sdvo *intel_sdvo = to_intel_sdvo(encoder);
@@ -993,17 +994,17 @@ static bool intel_sdvo_mode_fixup(struct drm_encoder *encoder,
 		if (!intel_sdvo_set_output_timings_from_mode(intel_sdvo, mode))
 			return false;
 
-		(void) intel_sdvo_set_input_timings_for_mode(intel_sdvo,
-							     mode,
-							     adjusted_mode);
+		(void) intel_sdvo_get_preferred_input_mode(intel_sdvo,
+							   mode,
+							   adjusted_mode);
 	} else if (intel_sdvo->is_lvds) {
 		if (!intel_sdvo_set_output_timings_from_mode(intel_sdvo,
 							     intel_sdvo->sdvo_lvds_fixed_mode))
 			return false;
 
-		(void) intel_sdvo_set_input_timings_for_mode(intel_sdvo,
-							     mode,
-							     adjusted_mode);
+		(void) intel_sdvo_get_preferred_input_mode(intel_sdvo,
+							   mode,
+							   adjusted_mode);
 	}
 
 	/* Make the CRTC code factor in the SDVO pixel multiplier.  The
@@ -1057,7 +1058,9 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 					     intel_sdvo->sdvo_lvds_fixed_mode);
 	else
 		intel_sdvo_get_dtd_from_mode(&output_dtd, mode);
-	(void) intel_sdvo_set_output_timing(intel_sdvo, &output_dtd);
+	if (!intel_sdvo_set_output_timing(intel_sdvo, &output_dtd))
+		DRM_INFO("Setting output timings on %s failed\n",
+			 SDVO_NAME(intel_sdvo));
 
 	/* Set the input timing to the screen. Assume always input 0. */
 	if (!intel_sdvo_set_target_input(intel_sdvo))
@@ -1079,7 +1082,9 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 	 * adjusted_mode.
 	 */
 	intel_sdvo_get_dtd_from_mode(&input_dtd, adjusted_mode);
-	(void) intel_sdvo_set_input_timing(intel_sdvo, &input_dtd);
+	if (!intel_sdvo_set_input_timing(intel_sdvo, &input_dtd))
+		DRM_INFO("Setting input timings on %s failed\n",
+			 SDVO_NAME(intel_sdvo));
 
 	switch (pixel_multiplier) {
 	default:
@@ -1376,7 +1381,7 @@ intel_sdvo_detect(struct drm_connector *connector, bool force)
 
 	/* add 30ms delay when the output type might be TV */
 	if (intel_sdvo->caps.output_flags & SDVO_TV_MASK)
-		mdelay(30);
+		msleep(30);
 
 	if (!intel_sdvo_read_response(intel_sdvo, &response, 2))
 		return connector_status_unknown;
@@ -2521,6 +2526,7 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *intel_encoder;
 	struct intel_sdvo *intel_sdvo;
+	u32 hotplug_mask;
 	int i;
 
 	intel_sdvo = kzalloc(sizeof(struct intel_sdvo), GFP_KERNEL);
@@ -2552,10 +2558,18 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 		}
 	}
 
-	if (intel_sdvo->is_sdvob)
-		dev_priv->hotplug_supported_mask |= SDVOB_HOTPLUG_INT_STATUS;
-	else
-		dev_priv->hotplug_supported_mask |= SDVOC_HOTPLUG_INT_STATUS;
+	hotplug_mask = 0;
+	if (IS_G4X(dev)) {
+		hotplug_mask = intel_sdvo->is_sdvob ?
+			SDVOB_HOTPLUG_INT_STATUS_G4X : SDVOC_HOTPLUG_INT_STATUS_G4X;
+	} else if (IS_GEN4(dev)) {
+		hotplug_mask = intel_sdvo->is_sdvob ?
+			SDVOB_HOTPLUG_INT_STATUS_I965 : SDVOC_HOTPLUG_INT_STATUS_I965;
+	} else {
+		hotplug_mask = intel_sdvo->is_sdvob ?
+			SDVOB_HOTPLUG_INT_STATUS_I915 : SDVOC_HOTPLUG_INT_STATUS_I915;
+	}
+	dev_priv->hotplug_supported_mask |= hotplug_mask;
 
 	drm_encoder_helper_add(&intel_encoder->base, &intel_sdvo_helper_funcs);
 
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 2a20fb0781d..cc8df4de2d9 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -56,6 +56,7 @@ ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	sprctl &= ~SPRITE_PIXFORMAT_MASK;
 	sprctl &= ~SPRITE_RGB_ORDER_RGBX;
 	sprctl &= ~SPRITE_YUV_BYTE_ORDER_MASK;
+	sprctl &= ~SPRITE_TILED;
 
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_XBGR8888:
@@ -84,7 +85,7 @@ ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 		break;
 	default:
 		DRM_DEBUG_DRIVER("bad pixel format, assuming RGBX888\n");
-		sprctl |= DVS_FORMAT_RGBX888;
+		sprctl |= SPRITE_FORMAT_RGBX888;
 		pixel_size = 4;
 		break;
 	}
@@ -233,6 +234,7 @@ ilk_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
 	dvscntr &= ~DVS_PIXFORMAT_MASK;
 	dvscntr &= ~DVS_RGB_ORDER_XBGR;
 	dvscntr &= ~DVS_YUV_BYTE_ORDER_MASK;
+	dvscntr &= ~DVS_TILED;
 
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_XBGR8888:
@@ -326,6 +328,12 @@ intel_enable_primary(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int reg = DSPCNTR(intel_crtc->plane);
 
+	if (!intel_crtc->primary_disabled)
+		return;
+
+	intel_crtc->primary_disabled = false;
+	intel_update_fbc(dev);
+
 	I915_WRITE(reg, I915_READ(reg) | DISPLAY_PLANE_ENABLE);
 }
 
@@ -337,7 +345,13 @@ intel_disable_primary(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int reg = DSPCNTR(intel_crtc->plane);
 
+	if (intel_crtc->primary_disabled)
+		return;
+
 	I915_WRITE(reg, I915_READ(reg) & ~DISPLAY_PLANE_ENABLE);
+
+	intel_crtc->primary_disabled = true;
+	intel_update_fbc(dev);
 }
 
 static int
@@ -485,18 +499,14 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	 * Be sure to re-enable the primary before the sprite is no longer
 	 * covering it fully.
 	 */
-	if (!disable_primary && intel_plane->primary_disabled) {
+	if (!disable_primary)
 		intel_enable_primary(crtc);
-		intel_plane->primary_disabled = false;
-	}
 
 	intel_plane->update_plane(plane, fb, obj, crtc_x, crtc_y,
 				  crtc_w, crtc_h, x, y, src_w, src_h);
 
-	if (disable_primary) {
+	if (disable_primary)
 		intel_disable_primary(crtc);
-		intel_plane->primary_disabled = true;
-	}
 
 	/* Unpin old obj after new one is active to avoid ugliness */
 	if (old_obj) {
@@ -527,11 +537,8 @@ intel_disable_plane(struct drm_plane *plane)
 	struct intel_plane *intel_plane = to_intel_plane(plane);
 	int ret = 0;
 
-	if (intel_plane->primary_disabled) {
+	if (plane->crtc)
 		intel_enable_primary(plane->crtc);
-		intel_plane->primary_disabled = false;
-	}
-
 	intel_plane->disable_plane(plane);
 
 	if (!intel_plane->obj)
@@ -685,6 +692,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe)
 		break;
 
 	default:
+		kfree(intel_plane);
 		return -ENODEV;
 	}
 
@@ -699,4 +707,3 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe)
 
 	return ret;
 }
-
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index a233a51fd7e..befce6c4970 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -891,24 +891,21 @@ intel_tv_mode_valid(struct drm_connector *connector,
 
 
 static bool
-intel_tv_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
+intel_tv_mode_fixup(struct drm_encoder *encoder,
+		    const struct drm_display_mode *mode,
 		    struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
-	struct drm_mode_config *drm_config = &dev->mode_config;
 	struct intel_tv *intel_tv = enc_to_intel_tv(encoder);
 	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
-	struct drm_encoder *other_encoder;
+	struct intel_encoder *other_encoder;
 
 	if (!tv_mode)
 		return false;
 
-	/* FIXME: lock encoder list */
-	list_for_each_entry(other_encoder, &drm_config->encoder_list, head) {
-		if (other_encoder != encoder &&
-		    other_encoder->crtc == encoder->crtc)
+	for_each_encoder_on_crtc(dev, encoder->crtc, other_encoder)
+		if (&other_encoder->base != encoder)
 			return false;
-	}
 
 	adjusted_mode->clock = tv_mode->clock;
 	return true;
diff --git a/drivers/gpu/drm/mga/mga_drv.c b/drivers/gpu/drm/mga/mga_drv.c
index f9a925d5881..b1bb46de3f5 100644
--- a/drivers/gpu/drm/mga/mga_drv.c
+++ b/drivers/gpu/drm/mga/mga_drv.c
@@ -75,7 +75,6 @@ static struct drm_driver driver = {
 	.irq_postinstall = mga_driver_irq_postinstall,
 	.irq_uninstall = mga_driver_irq_uninstall,
 	.irq_handler = mga_driver_irq_handler,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = mga_ioctls,
 	.dma_ioctl = mga_dma_buffers,
 	.fops = &mga_driver_fops,
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.c b/drivers/gpu/drm/mgag200/mgag200_drv.c
index 93e832d6c32..ea1024d7997 100644
--- a/drivers/gpu/drm/mgag200/mgag200_drv.c
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.c
@@ -47,6 +47,9 @@ static void mgag200_kick_out_firmware_fb(struct pci_dev *pdev)
 	bool primary = false;
 
 	ap = alloc_apertures(1);
+	if (!ap)
+		return;
+
 	ap->ranges[0].base = pci_resource_start(pdev, 0);
 	ap->ranges[0].size = pci_resource_len(pdev, 0);
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index d303061b251..a4d7c500c97 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -78,8 +78,8 @@ static inline void mga_wait_busy(struct mga_device *mdev)
  * to just pass that straight through, so this does nothing
  */
 static bool mga_crtc_mode_fixup(struct drm_crtc *crtc,
-				   struct drm_display_mode *mode,
-				   struct drm_display_mode *adjusted_mode)
+				const struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
 {
 	return true;
 }
@@ -1322,8 +1322,8 @@ void mga_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green,
  * to handle any encoder-specific limitations
  */
 static bool mga_encoder_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
-				  struct drm_display_mode *adjusted_mode)
+				   const struct drm_display_mode *mode,
+				   struct drm_display_mode *adjusted_mode)
 {
 	return true;
 }
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index fe5267d06ab..1cece6a78f3 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -4,7 +4,7 @@
 
 ccflags-y := -Iinclude/drm
 nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
-             nouveau_object.o nouveau_irq.o nouveau_notifier.o \
+             nouveau_gpuobj.o nouveau_irq.o nouveau_notifier.o \
              nouveau_sgdma.o nouveau_dma.o nouveau_util.o \
              nouveau_bo.o nouveau_fence.o nouveau_gem.o nouveau_ttm.o \
              nouveau_hw.o nouveau_calc.o nouveau_bios.o nouveau_i2c.o \
@@ -12,6 +12,7 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
              nouveau_hdmi.o nouveau_dp.o nouveau_ramht.o \
 	     nouveau_pm.o nouveau_volt.o nouveau_perf.o nouveau_temp.o \
 	     nouveau_mm.o nouveau_vm.o nouveau_mxm.o nouveau_gpio.o \
+	     nouveau_abi16.o \
              nv04_timer.o \
              nv04_mc.o nv40_mc.o nv50_mc.o \
              nv04_fb.o nv10_fb.o nv20_fb.o nv30_fb.o nv40_fb.o \
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
new file mode 100644
index 00000000000..ff23d88880e
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -0,0 +1,245 @@
+/*
+ * Copyright 2012 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "drmP.h"
+
+#include "nouveau_drv.h"
+#include "nouveau_dma.h"
+#include "nouveau_abi16.h"
+#include "nouveau_ramht.h"
+#include "nouveau_software.h"
+
+int
+nouveau_abi16_ioctl_getparam(ABI16_IOCTL_ARGS)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct drm_nouveau_getparam *getparam = data;
+
+	switch (getparam->param) {
+	case NOUVEAU_GETPARAM_CHIPSET_ID:
+		getparam->value = dev_priv->chipset;
+		break;
+	case NOUVEAU_GETPARAM_PCI_VENDOR:
+		getparam->value = dev->pci_vendor;
+		break;
+	case NOUVEAU_GETPARAM_PCI_DEVICE:
+		getparam->value = dev->pci_device;
+		break;
+	case NOUVEAU_GETPARAM_BUS_TYPE:
+		if (drm_pci_device_is_agp(dev))
+			getparam->value = 0;
+		else
+		if (!pci_is_pcie(dev->pdev))
+			getparam->value = 1;
+		else
+			getparam->value = 2;
+		break;
+	case NOUVEAU_GETPARAM_FB_SIZE:
+		getparam->value = dev_priv->fb_available_size;
+		break;
+	case NOUVEAU_GETPARAM_AGP_SIZE:
+		getparam->value = dev_priv->gart_info.aper_size;
+		break;
+	case NOUVEAU_GETPARAM_VM_VRAM_BASE:
+		getparam->value = 0; /* deprecated */
+		break;
+	case NOUVEAU_GETPARAM_PTIMER_TIME:
+		getparam->value = dev_priv->engine.timer.read(dev);
+		break;
+	case NOUVEAU_GETPARAM_HAS_BO_USAGE:
+		getparam->value = 1;
+		break;
+	case NOUVEAU_GETPARAM_HAS_PAGEFLIP:
+		getparam->value = 1;
+		break;
+	case NOUVEAU_GETPARAM_GRAPH_UNITS:
+		/* NV40 and NV50 versions are quite different, but register
+		 * address is the same. User is supposed to know the card
+		 * family anyway... */
+		if (dev_priv->chipset >= 0x40) {
+			getparam->value = nv_rd32(dev, NV40_PMC_GRAPH_UNITS);
+			break;
+		}
+		/* FALLTHRU */
+	default:
+		NV_DEBUG(dev, "unknown parameter %lld\n", getparam->param);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int
+nouveau_abi16_ioctl_setparam(ABI16_IOCTL_ARGS)
+{
+	return -EINVAL;
+}
+
+int
+nouveau_abi16_ioctl_channel_alloc(ABI16_IOCTL_ARGS)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct drm_nouveau_channel_alloc *init = data;
+	struct nouveau_channel *chan;
+	int ret;
+
+	if (!dev_priv->eng[NVOBJ_ENGINE_GR])
+		return -ENODEV;
+
+	if (init->fb_ctxdma_handle == ~0 || init->tt_ctxdma_handle == ~0)
+		return -EINVAL;
+
+	ret = nouveau_channel_alloc(dev, &chan, file_priv,
+				    init->fb_ctxdma_handle,
+				    init->tt_ctxdma_handle);
+	if (ret)
+		return ret;
+	init->channel  = chan->id;
+
+	if (nouveau_vram_pushbuf == 0) {
+		if (chan->dma.ib_max)
+			init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_VRAM |
+						NOUVEAU_GEM_DOMAIN_GART;
+		else if (chan->pushbuf_bo->bo.mem.mem_type == TTM_PL_VRAM)
+			init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_VRAM;
+		else
+			init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_GART;
+	} else {
+		init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_VRAM;
+	}
+
+	if (dev_priv->card_type < NV_C0) {
+		init->subchan[0].handle = 0x00000000;
+		init->subchan[0].grclass = 0x0000;
+		init->subchan[1].handle = NvSw;
+		init->subchan[1].grclass = NV_SW;
+		init->nr_subchan = 2;
+	}
+
+	/* Named memory object area */
+	ret = drm_gem_handle_create(file_priv, chan->notifier_bo->gem,
+				    &init->notifier_handle);
+
+	if (ret == 0)
+		atomic_inc(&chan->users); /* userspace reference */
+	nouveau_channel_put(&chan);
+	return ret;
+}
+
+int
+nouveau_abi16_ioctl_channel_free(ABI16_IOCTL_ARGS)
+{
+	struct drm_nouveau_channel_free *req = data;
+	struct nouveau_channel *chan;
+
+	chan = nouveau_channel_get(file_priv, req->channel);
+	if (IS_ERR(chan))
+		return PTR_ERR(chan);
+
+	list_del(&chan->list);
+	atomic_dec(&chan->users);
+	nouveau_channel_put(&chan);
+	return 0;
+}
+
+int
+nouveau_abi16_ioctl_grobj_alloc(ABI16_IOCTL_ARGS)
+{
+	struct drm_nouveau_grobj_alloc *init = data;
+	struct nouveau_channel *chan;
+	int ret;
+
+	if (init->handle == ~0)
+		return -EINVAL;
+
+	/* compatibility with userspace that assumes 506e for all chipsets */
+	if (init->class == 0x506e) {
+		init->class = nouveau_software_class(dev);
+		if (init->class == 0x906e)
+			return 0;
+	} else
+	if (init->class == 0x906e) {
+		NV_ERROR(dev, "906e not supported yet\n");
+		return -EINVAL;
+	}
+
+	chan = nouveau_channel_get(file_priv, init->channel);
+	if (IS_ERR(chan))
+		return PTR_ERR(chan);
+
+	if (nouveau_ramht_find(chan, init->handle)) {
+		ret = -EEXIST;
+		goto out;
+	}
+
+	ret = nouveau_gpuobj_gr_new(chan, init->handle, init->class);
+	if (ret) {
+		NV_ERROR(dev, "Error creating object: %d (%d/0x%08x)\n",
+			 ret, init->channel, init->handle);
+	}
+
+out:
+	nouveau_channel_put(&chan);
+	return ret;
+}
+
+int
+nouveau_abi16_ioctl_notifierobj_alloc(ABI16_IOCTL_ARGS)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct drm_nouveau_notifierobj_alloc *na = data;
+	struct nouveau_channel *chan;
+	int ret;
+
+	/* completely unnecessary for these chipsets... */
+	if (unlikely(dev_priv->card_type >= NV_C0))
+		return -EINVAL;
+
+	chan = nouveau_channel_get(file_priv, na->channel);
+	if (IS_ERR(chan))
+		return PTR_ERR(chan);
+
+	ret = nouveau_notifier_alloc(chan, na->handle, na->size, 0, 0x1000,
+				     &na->offset);
+	nouveau_channel_put(&chan);
+	return ret;
+}
+
+int
+nouveau_abi16_ioctl_gpuobj_free(ABI16_IOCTL_ARGS)
+{
+	struct drm_nouveau_gpuobj_free *objfree = data;
+	struct nouveau_channel *chan;
+	int ret;
+
+	chan = nouveau_channel_get(file_priv, objfree->channel);
+	if (IS_ERR(chan))
+		return PTR_ERR(chan);
+
+	/* Synchronize with the user channel */
+	nouveau_channel_idle(chan);
+
+	ret = nouveau_ramht_remove(chan, objfree->handle);
+	nouveau_channel_put(&chan);
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.h b/drivers/gpu/drm/nouveau/nouveau_abi16.h
new file mode 100644
index 00000000000..e6328b008a8
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.h
@@ -0,0 +1,83 @@
+#ifndef __NOUVEAU_ABI16_H__
+#define __NOUVEAU_ABI16_H__
+
+#define ABI16_IOCTL_ARGS                                                       \
+	struct drm_device *dev, void *data, struct drm_file *file_priv
+int nouveau_abi16_ioctl_getparam(ABI16_IOCTL_ARGS);
+int nouveau_abi16_ioctl_setparam(ABI16_IOCTL_ARGS);
+int nouveau_abi16_ioctl_channel_alloc(ABI16_IOCTL_ARGS);
+int nouveau_abi16_ioctl_channel_free(ABI16_IOCTL_ARGS);
+int nouveau_abi16_ioctl_grobj_alloc(ABI16_IOCTL_ARGS);
+int nouveau_abi16_ioctl_notifierobj_alloc(ABI16_IOCTL_ARGS);
+int nouveau_abi16_ioctl_gpuobj_free(ABI16_IOCTL_ARGS);
+
+struct drm_nouveau_channel_alloc {
+	uint32_t     fb_ctxdma_handle;
+	uint32_t     tt_ctxdma_handle;
+
+	int          channel;
+	uint32_t     pushbuf_domains;
+
+	/* Notifier memory */
+	uint32_t     notifier_handle;
+
+	/* DRM-enforced subchannel assignments */
+	struct {
+		uint32_t handle;
+		uint32_t grclass;
+	} subchan[8];
+	uint32_t nr_subchan;
+};
+
+struct drm_nouveau_channel_free {
+	int channel;
+};
+
+struct drm_nouveau_grobj_alloc {
+	int      channel;
+	uint32_t handle;
+	int      class;
+};
+
+struct drm_nouveau_notifierobj_alloc {
+	uint32_t channel;
+	uint32_t handle;
+	uint32_t size;
+	uint32_t offset;
+};
+
+struct drm_nouveau_gpuobj_free {
+	int      channel;
+	uint32_t handle;
+};
+
+#define NOUVEAU_GETPARAM_PCI_VENDOR      3
+#define NOUVEAU_GETPARAM_PCI_DEVICE      4
+#define NOUVEAU_GETPARAM_BUS_TYPE        5
+#define NOUVEAU_GETPARAM_FB_SIZE         8
+#define NOUVEAU_GETPARAM_AGP_SIZE        9
+#define NOUVEAU_GETPARAM_CHIPSET_ID      11
+#define NOUVEAU_GETPARAM_VM_VRAM_BASE    12
+#define NOUVEAU_GETPARAM_GRAPH_UNITS     13
+#define NOUVEAU_GETPARAM_PTIMER_TIME     14
+#define NOUVEAU_GETPARAM_HAS_BO_USAGE    15
+#define NOUVEAU_GETPARAM_HAS_PAGEFLIP    16
+struct drm_nouveau_getparam {
+	uint64_t param;
+	uint64_t value;
+};
+
+struct drm_nouveau_setparam {
+	uint64_t param;
+	uint64_t value;
+};
+
+#define DRM_IOCTL_NOUVEAU_GETPARAM           DRM_IOWR(DRM_COMMAND_BASE + DRM_NOUVEAU_GETPARAM, struct drm_nouveau_getparam)
+#define DRM_IOCTL_NOUVEAU_SETPARAM           DRM_IOWR(DRM_COMMAND_BASE + DRM_NOUVEAU_SETPARAM, struct drm_nouveau_setparam)
+#define DRM_IOCTL_NOUVEAU_CHANNEL_ALLOC      DRM_IOWR(DRM_COMMAND_BASE + DRM_NOUVEAU_CHANNEL_ALLOC, struct drm_nouveau_channel_alloc)
+#define DRM_IOCTL_NOUVEAU_CHANNEL_FREE       DRM_IOW (DRM_COMMAND_BASE + DRM_NOUVEAU_CHANNEL_FREE, struct drm_nouveau_channel_free)
+#define DRM_IOCTL_NOUVEAU_GROBJ_ALLOC        DRM_IOW (DRM_COMMAND_BASE + DRM_NOUVEAU_GROBJ_ALLOC, struct drm_nouveau_grobj_alloc)
+#define DRM_IOCTL_NOUVEAU_NOTIFIEROBJ_ALLOC  DRM_IOWR(DRM_COMMAND_BASE + DRM_NOUVEAU_NOTIFIEROBJ_ALLOC, struct drm_nouveau_notifierobj_alloc)
+#define DRM_IOCTL_NOUVEAU_GPUOBJ_FREE        DRM_IOW (DRM_COMMAND_BASE + DRM_NOUVEAU_GPUOBJ_FREE, struct drm_nouveau_gpuobj_free)
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index 2f11e16a81a..a0a3fe3c016 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -6091,6 +6091,18 @@ apply_dcb_encoder_quirks(struct drm_device *dev, int idx, u32 *conn, u32 *conf)
 		}
 	}
 
+	/* fdo#50830: connector indices for VGA and DVI-I are backwards */
+	if (nv_match_device(dev, 0x0421, 0x3842, 0xc793)) {
+		if (idx == 0 && *conn == 0x02000300)
+			*conn = 0x02011300;
+		else
+		if (idx == 1 && *conn == 0x04011310)
+			*conn = 0x04000310;
+		else
+		if (idx == 2 && *conn == 0x02011312)
+			*conn = 0x02000312;
+	}
+
 	return true;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_channel.c b/drivers/gpu/drm/nouveau/nouveau_channel.c
index 629d8a2df5b..debd90225a8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_channel.c
+++ b/drivers/gpu/drm/nouveau/nouveau_channel.c
@@ -395,98 +395,3 @@ nouveau_channel_cleanup(struct drm_device *dev, struct drm_file *file_priv)
 		nouveau_channel_put(&chan);
 	}
 }
-
-
-/***********************************
- * ioctls wrapping the functions
- ***********************************/
-
-static int
-nouveau_ioctl_fifo_alloc(struct drm_device *dev, void *data,
-			 struct drm_file *file_priv)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct drm_nouveau_channel_alloc *init = data;
-	struct nouveau_channel *chan;
-	int ret;
-
-	if (!dev_priv->eng[NVOBJ_ENGINE_GR])
-		return -ENODEV;
-
-	if (init->fb_ctxdma_handle == ~0 || init->tt_ctxdma_handle == ~0)
-		return -EINVAL;
-
-	ret = nouveau_channel_alloc(dev, &chan, file_priv,
-				    init->fb_ctxdma_handle,
-				    init->tt_ctxdma_handle);
-	if (ret)
-		return ret;
-	init->channel  = chan->id;
-
-	if (nouveau_vram_pushbuf == 0) {
-		if (chan->dma.ib_max)
-			init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_VRAM |
-						NOUVEAU_GEM_DOMAIN_GART;
-		else if (chan->pushbuf_bo->bo.mem.mem_type == TTM_PL_VRAM)
-			init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_VRAM;
-		else
-			init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_GART;
-	} else {
-		init->pushbuf_domains = NOUVEAU_GEM_DOMAIN_VRAM;
-	}
-
-	if (dev_priv->card_type < NV_C0) {
-		init->subchan[0].handle = 0x00000000;
-		init->subchan[0].grclass = 0x0000;
-		init->subchan[1].handle = NvSw;
-		init->subchan[1].grclass = NV_SW;
-		init->nr_subchan = 2;
-	}
-
-	/* Named memory object area */
-	ret = drm_gem_handle_create(file_priv, chan->notifier_bo->gem,
-				    &init->notifier_handle);
-
-	if (ret == 0)
-		atomic_inc(&chan->users); /* userspace reference */
-	nouveau_channel_put(&chan);
-	return ret;
-}
-
-static int
-nouveau_ioctl_fifo_free(struct drm_device *dev, void *data,
-			struct drm_file *file_priv)
-{
-	struct drm_nouveau_channel_free *req = data;
-	struct nouveau_channel *chan;
-
-	chan = nouveau_channel_get(file_priv, req->channel);
-	if (IS_ERR(chan))
-		return PTR_ERR(chan);
-
-	list_del(&chan->list);
-	atomic_dec(&chan->users);
-	nouveau_channel_put(&chan);
-	return 0;
-}
-
-/***********************************
- * finally, the ioctl table
- ***********************************/
-
-struct drm_ioctl_desc nouveau_ioctls[] = {
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GETPARAM, nouveau_ioctl_getparam, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_SETPARAM, nouveau_ioctl_setparam, DRM_UNLOCKED|DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_CHANNEL_ALLOC, nouveau_ioctl_fifo_alloc, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_CHANNEL_FREE, nouveau_ioctl_fifo_free, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GROBJ_ALLOC, nouveau_ioctl_grobj_alloc, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_NOTIFIEROBJ_ALLOC, nouveau_ioctl_notifier_alloc, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GPUOBJ_FREE, nouveau_ioctl_gpuobj_free, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_NEW, nouveau_gem_ioctl_new, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_PUSHBUF, nouveau_gem_ioctl_pushbuf, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_CPU_PREP, nouveau_gem_ioctl_cpu_prep, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_CPU_FINI, nouveau_gem_ioctl_cpu_fini, DRM_UNLOCKED|DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_INFO, nouveau_gem_ioctl_info, DRM_UNLOCKED|DRM_AUTH),
-};
-
-int nouveau_max_ioctl = DRM_ARRAY_SIZE(nouveau_ioctls);
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.c b/drivers/gpu/drm/nouveau/nouveau_drv.c
index cad254c8e38..9a36f5f39b0 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.c
@@ -29,6 +29,7 @@
 #include "drm.h"
 #include "drm_crtc_helper.h"
 #include "nouveau_drv.h"
+#include "nouveau_abi16.h"
 #include "nouveau_hw.h"
 #include "nouveau_fb.h"
 #include "nouveau_fbcon.h"
@@ -384,6 +385,21 @@ nouveau_pci_resume(struct pci_dev *pdev)
 	return 0;
 }
 
+static struct drm_ioctl_desc nouveau_ioctls[] = {
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GETPARAM, nouveau_abi16_ioctl_getparam, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_SETPARAM, nouveau_abi16_ioctl_setparam, DRM_UNLOCKED|DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_CHANNEL_ALLOC, nouveau_abi16_ioctl_channel_alloc, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_CHANNEL_FREE, nouveau_abi16_ioctl_channel_free, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GROBJ_ALLOC, nouveau_abi16_ioctl_grobj_alloc, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_NOTIFIEROBJ_ALLOC, nouveau_abi16_ioctl_notifierobj_alloc, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GPUOBJ_FREE, nouveau_abi16_ioctl_gpuobj_free, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_NEW, nouveau_gem_ioctl_new, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_PUSHBUF, nouveau_gem_ioctl_pushbuf, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_CPU_PREP, nouveau_gem_ioctl_cpu_prep, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_CPU_FINI, nouveau_gem_ioctl_cpu_fini, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(NOUVEAU_GEM_INFO, nouveau_gem_ioctl_info, DRM_UNLOCKED|DRM_AUTH),
+};
+
 static const struct file_operations nouveau_driver_fops = {
 	.owner = THIS_MODULE,
 	.open = drm_open,
@@ -422,7 +438,6 @@ static struct drm_driver driver = {
 	.get_vblank_counter = drm_vblank_count,
 	.enable_vblank = nouveau_vblank_enable,
 	.disable_vblank = nouveau_vblank_disable,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = nouveau_ioctls,
 	.fops = &nouveau_driver_fops,
 
@@ -463,7 +478,7 @@ static struct pci_driver nouveau_pci_driver = {
 
 static int __init nouveau_init(void)
 {
-	driver.num_ioctls = nouveau_max_ioctl;
+	driver.num_ioctls = ARRAY_SIZE(nouveau_ioctls);
 
 	if (nouveau_modeset == -1) {
 #ifdef CONFIG_VGA_CONSOLE
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.h b/drivers/gpu/drm/nouveau/nouveau_drv.h
index 8613cb23808..4f2cc95ce26 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.h
@@ -689,8 +689,6 @@ struct drm_nouveau_private {
 	void (*irq_handler[32])(struct drm_device *);
 	bool msi_enabled;
 
-	struct list_head vbl_waiting;
-
 	struct {
 		struct drm_global_reference mem_global_ref;
 		struct ttm_bo_global_ref bo_global_ref;
@@ -872,10 +870,6 @@ extern int  nouveau_load(struct drm_device *, unsigned long flags);
 extern int  nouveau_firstopen(struct drm_device *);
 extern void nouveau_lastclose(struct drm_device *);
 extern int  nouveau_unload(struct drm_device *);
-extern int  nouveau_ioctl_getparam(struct drm_device *, void *data,
-				   struct drm_file *);
-extern int  nouveau_ioctl_setparam(struct drm_device *, void *data,
-				   struct drm_file *);
 extern bool nouveau_wait_eq(struct drm_device *, uint64_t timeout,
 			    uint32_t reg, uint32_t mask, uint32_t val);
 extern bool nouveau_wait_ne(struct drm_device *, uint64_t timeout,
@@ -914,15 +908,8 @@ extern void nouveau_notifier_takedown_channel(struct nouveau_channel *);
 extern int  nouveau_notifier_alloc(struct nouveau_channel *, uint32_t handle,
 				   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 *);
-extern int  nouveau_ioctl_notifier_free(struct drm_device *, void *data,
-					struct drm_file *);
 
 /* nouveau_channel.c */
-extern struct drm_ioctl_desc nouveau_ioctls[];
-extern int nouveau_max_ioctl;
 extern void nouveau_channel_cleanup(struct drm_device *, struct drm_file *);
 extern int  nouveau_channel_alloc(struct drm_device *dev,
 				  struct nouveau_channel **chan,
@@ -938,7 +925,7 @@ extern void nouveau_channel_ref(struct nouveau_channel *chan,
 				struct nouveau_channel **pchan);
 extern int  nouveau_channel_idle(struct nouveau_channel *chan);
 
-/* nouveau_object.c */
+/* nouveau_gpuobj.c */
 #define NVOBJ_ENGINE_ADD(d, e, p) do {                                         \
 	struct drm_nouveau_private *dev_priv = (d)->dev_private;               \
 	dev_priv->eng[NVOBJ_ENGINE_##e] = (p);                                 \
@@ -993,10 +980,6 @@ extern int nv50_gpuobj_dma_new(struct nouveau_channel *, int class, u64 base,
 extern void nv50_gpuobj_dma_init(struct nouveau_gpuobj *, u32 offset,
 				 int class, u64 base, u64 size, int target,
 				 int access, u32 type, u32 comp);
-extern int nouveau_ioctl_grobj_alloc(struct drm_device *, void *data,
-				     struct drm_file *);
-extern int nouveau_ioctl_gpuobj_free(struct drm_device *, void *data,
-				     struct drm_file *);
 
 /* nouveau_irq.c */
 extern int         nouveau_irq_init(struct drm_device *);
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index 30f54231694..af7cfb82571 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -207,8 +207,7 @@ nouveau_gem_ioctl_new(struct drm_device *dev, void *data,
 	struct nouveau_bo *nvbo = NULL;
 	int ret = 0;
 
-	if (unlikely(dev_priv->ttm.bdev.dev_mapping == NULL))
-		dev_priv->ttm.bdev.dev_mapping = dev_priv->dev->dev_mapping;
+	dev_priv->ttm.bdev.dev_mapping = dev->dev_mapping;
 
 	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);
@@ -342,6 +341,7 @@ retry:
 		if (nvbo->reserved_by && nvbo->reserved_by == file_priv) {
 			NV_ERROR(dev, "multiple instances of buffer %d on "
 				      "validation list\n", b->handle);
+			drm_gem_object_unreference_unlocked(gem);
 			validate_fini(op, NULL);
 			return -EINVAL;
 		}
diff --git a/drivers/gpu/drm/nouveau/nouveau_object.c b/drivers/gpu/drm/nouveau/nouveau_gpuobj.c
index b190cc01c82..bd79fedb705 100644
--- a/drivers/gpu/drm/nouveau/nouveau_object.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gpuobj.c
@@ -758,66 +758,6 @@ nouveau_gpuobj_resume(struct drm_device *dev)
 	dev_priv->engine.instmem.flush(dev);
 }
 
-int nouveau_ioctl_grobj_alloc(struct drm_device *dev, void *data,
-			      struct drm_file *file_priv)
-{
-	struct drm_nouveau_grobj_alloc *init = data;
-	struct nouveau_channel *chan;
-	int ret;
-
-	if (init->handle == ~0)
-		return -EINVAL;
-
-	/* compatibility with userspace that assumes 506e for all chipsets */
-	if (init->class == 0x506e) {
-		init->class = nouveau_software_class(dev);
-		if (init->class == 0x906e)
-			return 0;
-	} else
-	if (init->class == 0x906e) {
-		NV_ERROR(dev, "906e not supported yet\n");
-		return -EINVAL;
-	}
-
-	chan = nouveau_channel_get(file_priv, init->channel);
-	if (IS_ERR(chan))
-		return PTR_ERR(chan);
-
-	if (nouveau_ramht_find(chan, init->handle)) {
-		ret = -EEXIST;
-		goto out;
-	}
-
-	ret = nouveau_gpuobj_gr_new(chan, init->handle, init->class);
-	if (ret) {
-		NV_ERROR(dev, "Error creating object: %d (%d/0x%08x)\n",
-			 ret, init->channel, init->handle);
-	}
-
-out:
-	nouveau_channel_put(&chan);
-	return ret;
-}
-
-int nouveau_ioctl_gpuobj_free(struct drm_device *dev, void *data,
-			      struct drm_file *file_priv)
-{
-	struct drm_nouveau_gpuobj_free *objfree = data;
-	struct nouveau_channel *chan;
-	int ret;
-
-	chan = nouveau_channel_get(file_priv, objfree->channel);
-	if (IS_ERR(chan))
-		return PTR_ERR(chan);
-
-	/* Synchronize with the user channel */
-	nouveau_channel_idle(chan);
-
-	ret = nouveau_ramht_remove(chan, objfree->handle);
-	nouveau_channel_put(&chan);
-	return ret;
-}
-
 u32
 nv_ro32(struct nouveau_gpuobj *gpuobj, u32 offset)
 {
diff --git a/drivers/gpu/drm/nouveau/nouveau_irq.c b/drivers/gpu/drm/nouveau/nouveau_irq.c
index 868c7fd7485..b2c2937531a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_irq.c
+++ b/drivers/gpu/drm/nouveau/nouveau_irq.c
@@ -41,12 +41,8 @@
 void
 nouveau_irq_preinstall(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-
 	/* Master disable */
 	nv_wr32(dev, NV03_PMC_INTR_EN_0, 0);
-
-	INIT_LIST_HEAD(&dev_priv->vbl_waiting);
 }
 
 int
diff --git a/drivers/gpu/drm/nouveau/nouveau_notifier.c b/drivers/gpu/drm/nouveau/nouveau_notifier.c
index 2ef883c4bbc..69c93b86451 100644
--- a/drivers/gpu/drm/nouveau/nouveau_notifier.c
+++ b/drivers/gpu/drm/nouveau/nouveau_notifier.c
@@ -161,44 +161,3 @@ nouveau_notifier_alloc(struct nouveau_channel *chan, uint32_t handle,
 	*b_offset = mem->start;
 	return 0;
 }
-
-int
-nouveau_notifier_offset(struct nouveau_gpuobj *nobj, uint32_t *poffset)
-{
-	if (!nobj || nobj->dtor != nouveau_notifier_gpuobj_dtor)
-		return -EINVAL;
-
-	if (poffset) {
-		struct drm_mm_node *mem = nobj->priv;
-
-		if (*poffset >= mem->size)
-			return false;
-
-		*poffset += mem->start;
-	}
-
-	return 0;
-}
-
-int
-nouveau_ioctl_notifier_alloc(struct drm_device *dev, void *data,
-			     struct drm_file *file_priv)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct drm_nouveau_notifierobj_alloc *na = data;
-	struct nouveau_channel *chan;
-	int ret;
-
-	/* completely unnecessary for these chipsets... */
-	if (unlikely(dev_priv->card_type >= NV_C0))
-		return -EINVAL;
-
-	chan = nouveau_channel_get(file_priv, na->channel);
-	if (IS_ERR(chan))
-		return PTR_ERR(chan);
-
-	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_software.h b/drivers/gpu/drm/nouveau/nouveau_software.h
index e60bc6ce900..709e5ac680e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_software.h
+++ b/drivers/gpu/drm/nouveau/nouveau_software.h
@@ -4,13 +4,15 @@
 struct nouveau_software_priv {
 	struct nouveau_exec_engine base;
 	struct list_head vblank;
+	spinlock_t peephole_lock;
 };
 
 struct nouveau_software_chan {
 	struct list_head flip;
 	struct {
 		struct list_head list;
-		struct nouveau_bo *bo;
+		u32 channel;
+		u32 ctxdma;
 		u32 offset;
 		u32 value;
 		u32 head;
@@ -18,32 +20,17 @@ struct nouveau_software_chan {
 };
 
 static inline void
-nouveau_software_vblank(struct drm_device *dev, int crtc)
-{
-	struct nouveau_software_priv *psw = nv_engine(dev, NVOBJ_ENGINE_SW);
-	struct nouveau_software_chan *pch, *tmp;
-
-	list_for_each_entry_safe(pch, tmp, &psw->vblank, vblank.list) {
-		if (pch->vblank.head != crtc)
-			continue;
-
-		nouveau_bo_wr32(pch->vblank.bo, pch->vblank.offset,
-						pch->vblank.value);
-		list_del(&pch->vblank.list);
-		drm_vblank_put(dev, crtc);
-	}
-}
-
-static inline void
 nouveau_software_context_new(struct nouveau_software_chan *pch)
 {
 	INIT_LIST_HEAD(&pch->flip);
+	INIT_LIST_HEAD(&pch->vblank.list);
 }
 
 static inline void
 nouveau_software_create(struct nouveau_software_priv *psw)
 {
 	INIT_LIST_HEAD(&psw->vblank);
+	spin_lock_init(&psw->peephole_lock);
 }
 
 static inline u16
diff --git a/drivers/gpu/drm/nouveau/nouveau_state.c b/drivers/gpu/drm/nouveau/nouveau_state.c
index 19706f0532e..1cdfd6e757c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_state.c
+++ b/drivers/gpu/drm/nouveau/nouveau_state.c
@@ -1234,80 +1234,6 @@ int nouveau_unload(struct drm_device *dev)
 	return 0;
 }
 
-int nouveau_ioctl_getparam(struct drm_device *dev, void *data,
-						struct drm_file *file_priv)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct drm_nouveau_getparam *getparam = data;
-
-	switch (getparam->param) {
-	case NOUVEAU_GETPARAM_CHIPSET_ID:
-		getparam->value = dev_priv->chipset;
-		break;
-	case NOUVEAU_GETPARAM_PCI_VENDOR:
-		getparam->value = dev->pci_vendor;
-		break;
-	case NOUVEAU_GETPARAM_PCI_DEVICE:
-		getparam->value = dev->pci_device;
-		break;
-	case NOUVEAU_GETPARAM_BUS_TYPE:
-		if (drm_pci_device_is_agp(dev))
-			getparam->value = NV_AGP;
-		else if (pci_is_pcie(dev->pdev))
-			getparam->value = NV_PCIE;
-		else
-			getparam->value = NV_PCI;
-		break;
-	case NOUVEAU_GETPARAM_FB_SIZE:
-		getparam->value = dev_priv->fb_available_size;
-		break;
-	case NOUVEAU_GETPARAM_AGP_SIZE:
-		getparam->value = dev_priv->gart_info.aper_size;
-		break;
-	case NOUVEAU_GETPARAM_VM_VRAM_BASE:
-		getparam->value = 0; /* deprecated */
-		break;
-	case NOUVEAU_GETPARAM_PTIMER_TIME:
-		getparam->value = dev_priv->engine.timer.read(dev);
-		break;
-	case NOUVEAU_GETPARAM_HAS_BO_USAGE:
-		getparam->value = 1;
-		break;
-	case NOUVEAU_GETPARAM_HAS_PAGEFLIP:
-		getparam->value = 1;
-		break;
-	case NOUVEAU_GETPARAM_GRAPH_UNITS:
-		/* NV40 and NV50 versions are quite different, but register
-		 * address is the same. User is supposed to know the card
-		 * family anyway... */
-		if (dev_priv->chipset >= 0x40) {
-			getparam->value = nv_rd32(dev, NV40_PMC_GRAPH_UNITS);
-			break;
-		}
-		/* FALLTHRU */
-	default:
-		NV_DEBUG(dev, "unknown parameter %lld\n", getparam->param);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-int
-nouveau_ioctl_setparam(struct drm_device *dev, void *data,
-		       struct drm_file *file_priv)
-{
-	struct drm_nouveau_setparam *setparam = data;
-
-	switch (setparam->param) {
-	default:
-		NV_DEBUG(dev, "unknown parameter %lld\n", setparam->param);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
 /* Wait until (value(reg) & mask) == val, up until timeout has hit */
 bool
 nouveau_wait_eq(struct drm_device *dev, uint64_t timeout,
diff --git a/drivers/gpu/drm/nouveau/nv04_crtc.c b/drivers/gpu/drm/nouveau/nv04_crtc.c
index 4c31c63e552..43accc11102 100644
--- a/drivers/gpu/drm/nouveau/nv04_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv04_crtc.c
@@ -215,7 +215,7 @@ nv_crtc_dpms(struct drm_crtc *crtc, int mode)
 }
 
 static bool
-nv_crtc_mode_fixup(struct drm_crtc *crtc, struct drm_display_mode *mode,
+nv_crtc_mode_fixup(struct drm_crtc *crtc, const struct drm_display_mode *mode,
 		   struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/nouveau/nv04_dac.c b/drivers/gpu/drm/nouveau/nv04_dac.c
index 8300266ffae..38f19479417 100644
--- a/drivers/gpu/drm/nouveau/nv04_dac.c
+++ b/drivers/gpu/drm/nouveau/nv04_dac.c
@@ -332,7 +332,7 @@ nv17_dac_detect(struct drm_encoder *encoder, struct drm_connector *connector)
 }
 
 static bool nv04_dac_mode_fixup(struct drm_encoder *encoder,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
 	if (nv04_dac_in_use(encoder))
diff --git a/drivers/gpu/drm/nouveau/nv04_dfp.c b/drivers/gpu/drm/nouveau/nv04_dfp.c
index 2258746016f..c2675623b7c 100644
--- a/drivers/gpu/drm/nouveau/nv04_dfp.c
+++ b/drivers/gpu/drm/nouveau/nv04_dfp.c
@@ -179,7 +179,7 @@ static struct drm_encoder *get_tmds_slave(struct drm_encoder *encoder)
 }
 
 static bool nv04_dfp_mode_fixup(struct drm_encoder *encoder,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
diff --git a/drivers/gpu/drm/nouveau/nv17_tv.c b/drivers/gpu/drm/nouveau/nv17_tv.c
index 696d7e7dc2a..67be5db021f 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv.c
+++ b/drivers/gpu/drm/nouveau/nv17_tv.c
@@ -338,7 +338,7 @@ static int nv17_tv_mode_valid(struct drm_encoder *encoder,
 }
 
 static bool nv17_tv_mode_fixup(struct drm_encoder *encoder,
-			       struct drm_display_mode *mode,
+			       const struct drm_display_mode *mode,
 			       struct drm_display_mode *adjusted_mode)
 {
 	struct nv17_tv_norm_params *tv_norm = get_tv_norm(encoder);
diff --git a/drivers/gpu/drm/nouveau/nv50_crtc.c b/drivers/gpu/drm/nouveau/nv50_crtc.c
index 97a477b3d52..22cebd5dd69 100644
--- a/drivers/gpu/drm/nouveau/nv50_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv50_crtc.c
@@ -527,7 +527,7 @@ nv50_crtc_commit(struct drm_crtc *crtc)
 }
 
 static bool
-nv50_crtc_mode_fixup(struct drm_crtc *crtc, struct drm_display_mode *mode,
+nv50_crtc_mode_fixup(struct drm_crtc *crtc, const struct drm_display_mode *mode,
 		     struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/nouveau/nv50_dac.c b/drivers/gpu/drm/nouveau/nv50_dac.c
index eb216a446b8..2c36a6b92c5 100644
--- a/drivers/gpu/drm/nouveau/nv50_dac.c
+++ b/drivers/gpu/drm/nouveau/nv50_dac.c
@@ -175,7 +175,8 @@ nv50_dac_restore(struct drm_encoder *encoder)
 }
 
 static bool
-nv50_dac_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
+nv50_dac_mode_fixup(struct drm_encoder *encoder,
+		    const struct drm_display_mode *mode,
 		    struct drm_display_mode *adjusted_mode)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 5c41612723b..b244d9968c5 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -646,7 +646,30 @@ nv50_display_script_select(struct drm_device *dev, struct dcb_entry *dcb,
 static void
 nv50_display_vblank_crtc_handler(struct drm_device *dev, int crtc)
 {
-	nouveau_software_vblank(dev, crtc);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_software_priv *psw = nv_engine(dev, NVOBJ_ENGINE_SW);
+	struct nouveau_software_chan *pch, *tmp;
+
+	list_for_each_entry_safe(pch, tmp, &psw->vblank, vblank.list) {
+		if (pch->vblank.head != crtc)
+			continue;
+
+		spin_lock(&psw->peephole_lock);
+		nv_wr32(dev, 0x001704, pch->vblank.channel);
+		nv_wr32(dev, 0x001710, 0x80000000 | pch->vblank.ctxdma);
+		if (dev_priv->chipset == 0x50) {
+			nv_wr32(dev, 0x001570, pch->vblank.offset);
+			nv_wr32(dev, 0x001574, pch->vblank.value);
+		} else {
+			nv_wr32(dev, 0x060010, pch->vblank.offset);
+			nv_wr32(dev, 0x060014, pch->vblank.value);
+		}
+		spin_unlock(&psw->peephole_lock);
+
+		list_del(&pch->vblank.list);
+		drm_vblank_put(dev, crtc);
+	}
+
 	drm_handle_vblank(dev, crtc);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv50_graph.c b/drivers/gpu/drm/nouveau/nv50_graph.c
index d9cc2f2638d..437608d1dfe 100644
--- a/drivers/gpu/drm/nouveau/nv50_graph.c
+++ b/drivers/gpu/drm/nouveau/nv50_graph.c
@@ -299,7 +299,7 @@ static struct nouveau_bitfield nv50_graph_trap_ccache[] = {
 
 /* There must be a *lot* of these. Will take some time to gather them up. */
 struct nouveau_enum nv50_data_error_names[] = {
-	{ 0x00000003, "INVALID_QUERY_OR_TEXTURE", NULL },
+	{ 0x00000003, "INVALID_OPERATION", NULL },
 	{ 0x00000004, "INVALID_VALUE", NULL },
 	{ 0x00000005, "INVALID_ENUM", NULL },
 	{ 0x00000008, "INVALID_OBJECT", NULL },
diff --git a/drivers/gpu/drm/nouveau/nv50_software.c b/drivers/gpu/drm/nouveau/nv50_software.c
index 114d2517d4a..df554d9dacb 100644
--- a/drivers/gpu/drm/nouveau/nv50_software.c
+++ b/drivers/gpu/drm/nouveau/nv50_software.c
@@ -36,9 +36,6 @@ struct nv50_software_priv {
 
 struct nv50_software_chan {
 	struct nouveau_software_chan base;
-	struct {
-		struct nouveau_gpuobj *object;
-	} vblank;
 };
 
 static int
@@ -51,11 +48,7 @@ mthd_dma_vblsem(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
 	if (!gpuobj)
 		return -ENOENT;
 
-	if (nouveau_notifier_offset(gpuobj, NULL))
-		return -EINVAL;
-
-	pch->vblank.object = gpuobj;
-	pch->base.vblank.offset = ~0;
+	pch->base.vblank.ctxdma = gpuobj->cinst >> 4;
 	return 0;
 }
 
@@ -63,11 +56,7 @@ static int
 mthd_vblsem_offset(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
 {
 	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
-
-	if (nouveau_notifier_offset(pch->vblank.object, &data))
-		return -ERANGE;
-
-	pch->base.vblank.offset = data >> 2;
+	pch->base.vblank.offset = data;
 	return 0;
 }
 
@@ -86,7 +75,7 @@ mthd_vblsem_release(struct nouveau_channel *chan, u32 class, u32 mthd, u32 data)
 	struct nv50_software_chan *pch = chan->engctx[NVOBJ_ENGINE_SW];
 	struct drm_device *dev = chan->dev;
 
-	if (!pch->vblank.object || pch->base.vblank.offset == ~0 || data > 1)
+	if (data > 1)
 		return -EINVAL;
 
 	drm_vblank_get(dev, data);
@@ -116,7 +105,7 @@ nv50_software_context_new(struct nouveau_channel *chan, int engine)
 		return -ENOMEM;
 
 	nouveau_software_context_new(&pch->base);
-	pch->base.vblank.bo = chan->notifier_bo;
+	pch->base.vblank.channel = chan->ramin->vinst >> 12;
 	chan->engctx[engine] = pch;
 
 	/* dma objects for display sync channel semaphore blocks */
diff --git a/drivers/gpu/drm/nouveau/nv50_sor.c b/drivers/gpu/drm/nouveau/nv50_sor.c
index a9514eaa74c..93240bde891 100644
--- a/drivers/gpu/drm/nouveau/nv50_sor.c
+++ b/drivers/gpu/drm/nouveau/nv50_sor.c
@@ -327,7 +327,8 @@ nv50_sor_restore(struct drm_encoder *encoder)
 }
 
 static bool
-nv50_sor_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
+nv50_sor_mode_fixup(struct drm_encoder *encoder,
+		    const struct drm_display_mode *mode,
 		    struct drm_display_mode *adjusted_mode)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
diff --git a/drivers/gpu/drm/nouveau/nv84_crypt.c b/drivers/gpu/drm/nouveau/nv84_crypt.c
index edece9c616e..bbfcc73b670 100644
--- a/drivers/gpu/drm/nouveau/nv84_crypt.c
+++ b/drivers/gpu/drm/nouveau/nv84_crypt.c
@@ -117,18 +117,30 @@ nv84_crypt_tlb_flush(struct drm_device *dev, int engine)
 	nv50_vm_flush_engine(dev, 0x0a);
 }
 
+static struct nouveau_bitfield nv84_crypt_intr[] = {
+	{ 0x00000001, "INVALID_STATE" },
+	{ 0x00000002, "ILLEGAL_MTHD" },
+	{ 0x00000004, "ILLEGAL_CLASS" },
+	{ 0x00000080, "QUERY" },
+	{ 0x00000100, "FAULT" },
+	{}
+};
+
 static void
 nv84_crypt_isr(struct drm_device *dev)
 {
 	u32 stat = nv_rd32(dev, 0x102130);
 	u32 mthd = nv_rd32(dev, 0x102190);
 	u32 data = nv_rd32(dev, 0x102194);
-	u32 inst = nv_rd32(dev, 0x102188) & 0x7fffffff;
+	u64 inst = (u64)(nv_rd32(dev, 0x102188) & 0x7fffffff) << 12;
 	int show = nouveau_ratelimit();
+	int chid = nv50_graph_isr_chid(dev, inst);
 
 	if (show) {
-		NV_INFO(dev, "PCRYPT_INTR: 0x%08x 0x%08x 0x%08x 0x%08x\n",
-			     stat, mthd, data, inst);
+		NV_INFO(dev, "PCRYPT:");
+		nouveau_bitfield_print(nv84_crypt_intr, stat);
+		printk(KERN_CONT " ch %d (0x%010llx) mthd 0x%04x data 0x%08x\n",
+			chid, inst, mthd, data);
 	}
 
 	nv_wr32(dev, 0x102130, stat);
diff --git a/drivers/gpu/drm/nouveau/nva3_copy.fuc b/drivers/gpu/drm/nouveau/nva3_copy.fuc
index abc36626fef..219850d5328 100644
--- a/drivers/gpu/drm/nouveau/nva3_copy.fuc
+++ b/drivers/gpu/drm/nouveau/nva3_copy.fuc
@@ -119,9 +119,9 @@ dispatch_dma:
 // mthd 0x030c-0x0340, various stuff
 .b16 0xc3 14
 .b32 #ctx_src_address_high           ~0x000000ff
-.b32 #ctx_src_address_low            ~0xfffffff0
+.b32 #ctx_src_address_low            ~0xffffffff
 .b32 #ctx_dst_address_high           ~0x000000ff
-.b32 #ctx_dst_address_low            ~0xfffffff0
+.b32 #ctx_dst_address_low            ~0xffffffff
 .b32 #ctx_src_pitch                  ~0x0007ffff
 .b32 #ctx_dst_pitch                  ~0x0007ffff
 .b32 #ctx_xcnt                       ~0x0000ffff
diff --git a/drivers/gpu/drm/nouveau/nva3_copy.fuc.h b/drivers/gpu/drm/nouveau/nva3_copy.fuc.h
index 1f33fbdc00b..37d6de3c9d6 100644
--- a/drivers/gpu/drm/nouveau/nva3_copy.fuc.h
+++ b/drivers/gpu/drm/nouveau/nva3_copy.fuc.h
@@ -1,37 +1,72 @@
-uint32_t nva3_pcopy_data[] = {
+u32 nva3_pcopy_data[] = {
+/* 0x0000: ctx_object */
 	0x00000000,
+/* 0x0004: ctx_dma */
+/* 0x0004: ctx_dma_query */
 	0x00000000,
+/* 0x0008: ctx_dma_src */
 	0x00000000,
+/* 0x000c: ctx_dma_dst */
 	0x00000000,
+/* 0x0010: ctx_query_address_high */
 	0x00000000,
+/* 0x0014: ctx_query_address_low */
 	0x00000000,
+/* 0x0018: ctx_query_counter */
 	0x00000000,
+/* 0x001c: ctx_src_address_high */
 	0x00000000,
+/* 0x0020: ctx_src_address_low */
 	0x00000000,
+/* 0x0024: ctx_src_pitch */
 	0x00000000,
+/* 0x0028: ctx_src_tile_mode */
 	0x00000000,
+/* 0x002c: ctx_src_xsize */
 	0x00000000,
+/* 0x0030: ctx_src_ysize */
 	0x00000000,
+/* 0x0034: ctx_src_zsize */
 	0x00000000,
+/* 0x0038: ctx_src_zoff */
 	0x00000000,
+/* 0x003c: ctx_src_xoff */
 	0x00000000,
+/* 0x0040: ctx_src_yoff */
 	0x00000000,
+/* 0x0044: ctx_src_cpp */
 	0x00000000,
+/* 0x0048: ctx_dst_address_high */
 	0x00000000,
+/* 0x004c: ctx_dst_address_low */
 	0x00000000,
+/* 0x0050: ctx_dst_pitch */
 	0x00000000,
+/* 0x0054: ctx_dst_tile_mode */
 	0x00000000,
+/* 0x0058: ctx_dst_xsize */
 	0x00000000,
+/* 0x005c: ctx_dst_ysize */
 	0x00000000,
+/* 0x0060: ctx_dst_zsize */
 	0x00000000,
+/* 0x0064: ctx_dst_zoff */
 	0x00000000,
+/* 0x0068: ctx_dst_xoff */
 	0x00000000,
+/* 0x006c: ctx_dst_yoff */
 	0x00000000,
+/* 0x0070: ctx_dst_cpp */
 	0x00000000,
+/* 0x0074: ctx_format */
 	0x00000000,
+/* 0x0078: ctx_swz_const0 */
 	0x00000000,
+/* 0x007c: ctx_swz_const1 */
 	0x00000000,
+/* 0x0080: ctx_xcnt */
 	0x00000000,
+/* 0x0084: ctx_ycnt */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -63,6 +98,7 @@ uint32_t nva3_pcopy_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+/* 0x0100: dispatch_table */
 	0x00010000,
 	0x00000000,
 	0x00000000,
@@ -73,6 +109,7 @@ uint32_t nva3_pcopy_data[] = {
 	0x00010162,
 	0x00000000,
 	0x00030060,
+/* 0x0128: dispatch_dma */
 	0x00010170,
 	0x00000000,
 	0x00010170,
@@ -118,11 +155,11 @@ uint32_t nva3_pcopy_data[] = {
 	0x0000001c,
 	0xffffff00,
 	0x00000020,
-	0x0000000f,
+	0x00000000,
 	0x00000048,
 	0xffffff00,
 	0x0000004c,
-	0x0000000f,
+	0x00000000,
 	0x00000024,
 	0xfff80000,
 	0x00000050,
@@ -146,7 +183,8 @@ uint32_t nva3_pcopy_data[] = {
 	0x00000800,
 };
 
-uint32_t nva3_pcopy_code[] = {
+u32 nva3_pcopy_code[] = {
+/* 0x0000: main */
 	0x04fe04bd,
 	0x3517f000,
 	0xf10010fe,
@@ -158,23 +196,31 @@ uint32_t nva3_pcopy_code[] = {
 	0x17f11031,
 	0x27f01200,
 	0x0012d003,
+/* 0x002f: spin */
 	0xf40031f4,
 	0x0ef40028,
+/* 0x0035: ih */
 	0x8001cffd,
 	0xf40812c4,
 	0x21f4060b,
+/* 0x0041: ih_no_chsw */
 	0x0412c472,
 	0xf4060bf4,
+/* 0x004a: ih_no_cmd */
 	0x11c4c321,
 	0x4001d00c,
+/* 0x0052: swctx */
 	0x47f101f8,
 	0x4bfe7700,
 	0x0007fe00,
 	0xf00204b9,
 	0x01f40643,
 	0x0604fa09,
+/* 0x006b: swctx_load */
 	0xfa060ef4,
+/* 0x006e: swctx_done */
 	0x03f80504,
+/* 0x0072: chsw */
 	0x27f100f8,
 	0x23cf1400,
 	0x1e3fc800,
@@ -183,18 +229,22 @@ uint32_t nva3_pcopy_code[] = {
 	0x1e3af052,
 	0xf00023d0,
 	0x24d00147,
+/* 0x0093: chsw_no_unload */
 	0xcf00f880,
 	0x3dc84023,
 	0x220bf41e,
 	0xf40131f4,
 	0x57f05221,
 	0x0367f004,
+/* 0x00a8: chsw_load_ctx_dma */
 	0xa07856bc,
 	0xb6018068,
 	0x87d00884,
 	0x0162b600,
+/* 0x00bb: chsw_finish_load */
 	0xf0f018f4,
 	0x23d00237,
+/* 0x00c3: dispatch */
 	0xf100f880,
 	0xcf190037,
 	0x33cf4032,
@@ -202,6 +252,7 @@ uint32_t nva3_pcopy_code[] = {
 	0x1024b607,
 	0x010057f1,
 	0x74bd64bd,
+/* 0x00dc: dispatch_loop */
 	0x58005658,
 	0x50b60157,
 	0x0446b804,
@@ -211,6 +262,7 @@ uint32_t nva3_pcopy_code[] = {
 	0xb60276bb,
 	0x57bb0374,
 	0xdf0ef400,
+/* 0x0100: dispatch_valid_mthd */
 	0xb60246bb,
 	0x45bb0344,
 	0x01459800,
@@ -220,31 +272,41 @@ uint32_t nva3_pcopy_code[] = {
 	0xb0014658,
 	0x1bf40064,
 	0x00538009,
+/* 0x0127: dispatch_cmd */
 	0xf4300ef4,
 	0x55f90132,
 	0xf40c01f4,
+/* 0x0132: dispatch_invalid_bitfield */
 	0x25f0250e,
+/* 0x0135: dispatch_illegal_mthd */
 	0x0125f002,
+/* 0x0138: dispatch_error */
 	0x100047f1,
 	0xd00042d0,
 	0x27f04043,
 	0x0002d040,
+/* 0x0148: hostirq_wait */
 	0xf08002cf,
 	0x24b04024,
 	0xf71bf400,
+/* 0x0154: dispatch_done */
 	0x1d0027f1,
 	0xd00137f0,
 	0x00f80023,
+/* 0x0160: cmd_nop */
+/* 0x0162: cmd_pm_trigger */
 	0x27f100f8,
 	0x34bd2200,
 	0xd00233f0,
 	0x00f80023,
+/* 0x0170: cmd_dma */
 	0x012842b7,
 	0xf00145b6,
 	0x43801e39,
 	0x0040b701,
 	0x0644b606,
 	0xf80043d0,
+/* 0x0189: cmd_exec_set_format */
 	0xf030f400,
 	0xb00001b0,
 	0x01b00101,
@@ -256,20 +318,26 @@ uint32_t nva3_pcopy_code[] = {
 	0x70b63847,
 	0x0232f401,
 	0x94bd84bd,
+/* 0x01b4: ncomp_loop */
 	0xb60f4ac4,
 	0xb4bd0445,
+/* 0x01bc: bpc_loop */
 	0xf404a430,
 	0xa5ff0f18,
 	0x00cbbbc0,
 	0xf40231f4,
+/* 0x01ce: cmp_c0 */
 	0x1bf4220e,
 	0x10c7f00c,
 	0xf400cbbb,
+/* 0x01da: cmp_c1 */
 	0xa430160e,
 	0x0c18f406,
 	0xbb14c7f0,
 	0x0ef400cb,
+/* 0x01e9: cmp_zero */
 	0x80c7f107,
+/* 0x01ed: bpc_next */
 	0x01c83800,
 	0xb60180b6,
 	0xb5b801b0,
@@ -280,6 +348,7 @@ uint32_t nva3_pcopy_code[] = {
 	0x98110680,
 	0x68fd2008,
 	0x0502f400,
+/* 0x0216: dst_xcnt */
 	0x75fd64bd,
 	0x1c078000,
 	0xf10078fd,
@@ -304,6 +373,7 @@ uint32_t nva3_pcopy_code[] = {
 	0x980056d0,
 	0x56d01f06,
 	0x1030f440,
+/* 0x0276: cmd_exec_set_surface_tiled */
 	0x579800f8,
 	0x6879c70a,
 	0xb66478c7,
@@ -311,9 +381,11 @@ uint32_t nva3_pcopy_code[] = {
 	0x0e76b060,
 	0xf0091bf4,
 	0x0ef40477,
+/* 0x0291: xtile64 */
 	0x027cf00f,
 	0xfd1170b6,
 	0x77f00947,
+/* 0x029d: xtileok */
 	0x0f5a9806,
 	0xfd115b98,
 	0xb7f000ab,
@@ -371,6 +443,7 @@ uint32_t nva3_pcopy_code[] = {
 	0x67d00600,
 	0x0060b700,
 	0x0068d004,
+/* 0x0382: cmd_exec_set_surface_linear */
 	0x6cf000f8,
 	0x0260b702,
 	0x0864b602,
@@ -381,13 +454,16 @@ uint32_t nva3_pcopy_code[] = {
 	0xb70067d0,
 	0x98040060,
 	0x67d00957,
+/* 0x03ab: cmd_exec_wait */
 	0xf900f800,
 	0xf110f900,
 	0xb6080007,
+/* 0x03b6: loop */
 	0x01cf0604,
 	0x0114f000,
 	0xfcfa1bf4,
 	0xf800fc10,
+/* 0x03c5: cmd_exec_query */
 	0x0d34c800,
 	0xf5701bf4,
 	0xf103ab21,
@@ -417,6 +493,7 @@ uint32_t nva3_pcopy_code[] = {
 	0x47f10153,
 	0x44b60800,
 	0x0045d006,
+/* 0x0438: query_counter */
 	0x03ab21f5,
 	0x080c47f1,
 	0x980644b6,
@@ -439,11 +516,13 @@ uint32_t nva3_pcopy_code[] = {
 	0x47f10153,
 	0x44b60800,
 	0x0045d006,
+/* 0x0492: cmd_exec */
 	0x21f500f8,
 	0x3fc803ab,
 	0x0e0bf400,
 	0x018921f5,
 	0x020047f1,
+/* 0x04a7: cmd_exec_no_format */
 	0xf11e0ef4,
 	0xb6081067,
 	0x77f00664,
@@ -451,19 +530,24 @@ uint32_t nva3_pcopy_code[] = {
 	0x981c0780,
 	0x67d02007,
 	0x4067d000,
+/* 0x04c2: cmd_exec_init_src_surface */
 	0x32f444bd,
 	0xc854bd02,
 	0x0bf4043f,
 	0x8221f50a,
 	0x0a0ef403,
+/* 0x04d4: src_tiled */
 	0x027621f5,
+/* 0x04db: cmd_exec_init_dst_surface */
 	0xf40749f0,
 	0x57f00231,
 	0x083fc82c,
 	0xf50a0bf4,
 	0xf4038221,
+/* 0x04ee: dst_tiled */
 	0x21f50a0e,
 	0x49f00276,
+/* 0x04f5: cmd_exec_kick */
 	0x0057f108,
 	0x0654b608,
 	0xd0210698,
@@ -473,6 +557,8 @@ uint32_t nva3_pcopy_code[] = {
 	0xc80054d0,
 	0x0bf40c3f,
 	0xc521f507,
+/* 0x0519: cmd_exec_done */
+/* 0x051b: cmd_wrcache_flush */
 	0xf100f803,
 	0xbd220027,
 	0x0133f034,
diff --git a/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h b/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h
index a8d17458ced..cd879f31bb3 100644
--- a/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h
+++ b/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h
@@ -1,34 +1,65 @@
-uint32_t nvc0_pcopy_data[] = {
+u32 nvc0_pcopy_data[] = {
+/* 0x0000: ctx_object */
 	0x00000000,
+/* 0x0004: ctx_query_address_high */
 	0x00000000,
+/* 0x0008: ctx_query_address_low */
 	0x00000000,
+/* 0x000c: ctx_query_counter */
 	0x00000000,
+/* 0x0010: ctx_src_address_high */
 	0x00000000,
+/* 0x0014: ctx_src_address_low */
 	0x00000000,
+/* 0x0018: ctx_src_pitch */
 	0x00000000,
+/* 0x001c: ctx_src_tile_mode */
 	0x00000000,
+/* 0x0020: ctx_src_xsize */
 	0x00000000,
+/* 0x0024: ctx_src_ysize */
 	0x00000000,
+/* 0x0028: ctx_src_zsize */
 	0x00000000,
+/* 0x002c: ctx_src_zoff */
 	0x00000000,
+/* 0x0030: ctx_src_xoff */
 	0x00000000,
+/* 0x0034: ctx_src_yoff */
 	0x00000000,
+/* 0x0038: ctx_src_cpp */
 	0x00000000,
+/* 0x003c: ctx_dst_address_high */
 	0x00000000,
+/* 0x0040: ctx_dst_address_low */
 	0x00000000,
+/* 0x0044: ctx_dst_pitch */
 	0x00000000,
+/* 0x0048: ctx_dst_tile_mode */
 	0x00000000,
+/* 0x004c: ctx_dst_xsize */
 	0x00000000,
+/* 0x0050: ctx_dst_ysize */
 	0x00000000,
+/* 0x0054: ctx_dst_zsize */
 	0x00000000,
+/* 0x0058: ctx_dst_zoff */
 	0x00000000,
+/* 0x005c: ctx_dst_xoff */
 	0x00000000,
+/* 0x0060: ctx_dst_yoff */
 	0x00000000,
+/* 0x0064: ctx_dst_cpp */
 	0x00000000,
+/* 0x0068: ctx_format */
 	0x00000000,
+/* 0x006c: ctx_swz_const0 */
 	0x00000000,
+/* 0x0070: ctx_swz_const1 */
 	0x00000000,
+/* 0x0074: ctx_xcnt */
 	0x00000000,
+/* 0x0078: ctx_ycnt */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -63,6 +94,7 @@ uint32_t nvc0_pcopy_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+/* 0x0100: dispatch_table */
 	0x00010000,
 	0x00000000,
 	0x00000000,
@@ -111,11 +143,11 @@ uint32_t nvc0_pcopy_data[] = {
 	0x00000010,
 	0xffffff00,
 	0x00000014,
-	0x0000000f,
+	0x00000000,
 	0x0000003c,
 	0xffffff00,
 	0x00000040,
-	0x0000000f,
+	0x00000000,
 	0x00000018,
 	0xfff80000,
 	0x00000044,
@@ -139,7 +171,8 @@ uint32_t nvc0_pcopy_data[] = {
 	0x00000800,
 };
 
-uint32_t nvc0_pcopy_code[] = {
+u32 nvc0_pcopy_code[] = {
+/* 0x0000: main */
 	0x04fe04bd,
 	0x3517f000,
 	0xf10010fe,
@@ -151,15 +184,20 @@ uint32_t nvc0_pcopy_code[] = {
 	0x17f11031,
 	0x27f01200,
 	0x0012d003,
+/* 0x002f: spin */
 	0xf40031f4,
 	0x0ef40028,
+/* 0x0035: ih */
 	0x8001cffd,
 	0xf40812c4,
 	0x21f4060b,
+/* 0x0041: ih_no_chsw */
 	0x0412c4ca,
 	0xf5070bf4,
+/* 0x004b: ih_no_cmd */
 	0xc4010221,
 	0x01d00c11,
+/* 0x0053: swctx */
 	0xf101f840,
 	0xfe770047,
 	0x47f1004b,
@@ -188,8 +226,11 @@ uint32_t nvc0_pcopy_code[] = {
 	0xf00204b9,
 	0x01f40643,
 	0x0604fa09,
+/* 0x00c3: swctx_load */
 	0xfa060ef4,
+/* 0x00c6: swctx_done */
 	0x03f80504,
+/* 0x00ca: chsw */
 	0x27f100f8,
 	0x23cf1400,
 	0x1e3fc800,
@@ -198,18 +239,22 @@ uint32_t nvc0_pcopy_code[] = {
 	0x1e3af053,
 	0xf00023d0,
 	0x24d00147,
+/* 0x00eb: chsw_no_unload */
 	0xcf00f880,
 	0x3dc84023,
 	0x090bf41e,
 	0xf40131f4,
+/* 0x00fa: chsw_finish_load */
 	0x37f05321,
 	0x8023d002,
+/* 0x0102: dispatch */
 	0x37f100f8,
 	0x32cf1900,
 	0x0033cf40,
 	0x07ff24e4,
 	0xf11024b6,
 	0xbd010057,
+/* 0x011b: dispatch_loop */
 	0x5874bd64,
 	0x57580056,
 	0x0450b601,
@@ -219,6 +264,7 @@ uint32_t nvc0_pcopy_code[] = {
 	0xbb0f08f4,
 	0x74b60276,
 	0x0057bb03,
+/* 0x013f: dispatch_valid_mthd */
 	0xbbdf0ef4,
 	0x44b60246,
 	0x0045bb03,
@@ -229,24 +275,33 @@ uint32_t nvc0_pcopy_code[] = {
 	0x64b00146,
 	0x091bf400,
 	0xf4005380,
+/* 0x0166: dispatch_cmd */
 	0x32f4300e,
 	0xf455f901,
 	0x0ef40c01,
+/* 0x0171: dispatch_invalid_bitfield */
 	0x0225f025,
+/* 0x0174: dispatch_illegal_mthd */
+/* 0x0177: dispatch_error */
 	0xf10125f0,
 	0xd0100047,
 	0x43d00042,
 	0x4027f040,
+/* 0x0187: hostirq_wait */
 	0xcf0002d0,
 	0x24f08002,
 	0x0024b040,
+/* 0x0193: dispatch_done */
 	0xf1f71bf4,
 	0xf01d0027,
 	0x23d00137,
+/* 0x019f: cmd_nop */
 	0xf800f800,
+/* 0x01a1: cmd_pm_trigger */
 	0x0027f100,
 	0xf034bd22,
 	0x23d00233,
+/* 0x01af: cmd_exec_set_format */
 	0xf400f800,
 	0x01b0f030,
 	0x0101b000,
@@ -258,20 +313,26 @@ uint32_t nvc0_pcopy_code[] = {
 	0x3847c701,
 	0xf40170b6,
 	0x84bd0232,
+/* 0x01da: ncomp_loop */
 	0x4ac494bd,
 	0x0445b60f,
+/* 0x01e2: bpc_loop */
 	0xa430b4bd,
 	0x0f18f404,
 	0xbbc0a5ff,
 	0x31f400cb,
 	0x220ef402,
+/* 0x01f4: cmp_c0 */
 	0xf00c1bf4,
 	0xcbbb10c7,
 	0x160ef400,
+/* 0x0200: cmp_c1 */
 	0xf406a430,
 	0xc7f00c18,
 	0x00cbbb14,
+/* 0x020f: cmp_zero */
 	0xf1070ef4,
+/* 0x0213: bpc_next */
 	0x380080c7,
 	0x80b601c8,
 	0x01b0b601,
@@ -283,6 +344,7 @@ uint32_t nvc0_pcopy_code[] = {
 	0x1d08980e,
 	0xf40068fd,
 	0x64bd0502,
+/* 0x023c: dst_xcnt */
 	0x800075fd,
 	0x78fd1907,
 	0x1057f100,
@@ -307,15 +369,18 @@ uint32_t nvc0_pcopy_code[] = {
 	0x1c069800,
 	0xf44056d0,
 	0x00f81030,
+/* 0x029c: cmd_exec_set_surface_tiled */
 	0xc7075798,
 	0x78c76879,
 	0x0380b664,
 	0xb06077c7,
 	0x1bf40e76,
 	0x0477f009,
+/* 0x02b7: xtile64 */
 	0xf00f0ef4,
 	0x70b6027c,
 	0x0947fd11,
+/* 0x02c3: xtileok */
 	0x980677f0,
 	0x5b980c5a,
 	0x00abfd0e,
@@ -374,6 +439,7 @@ uint32_t nvc0_pcopy_code[] = {
 	0xb70067d0,
 	0xd0040060,
 	0x00f80068,
+/* 0x03a8: cmd_exec_set_surface_linear */
 	0xb7026cf0,
 	0xb6020260,
 	0x57980864,
@@ -384,12 +450,15 @@ uint32_t nvc0_pcopy_code[] = {
 	0x0060b700,
 	0x06579804,
 	0xf80067d0,
+/* 0x03d1: cmd_exec_wait */
 	0xf900f900,
 	0x0007f110,
 	0x0604b608,
+/* 0x03dc: loop */
 	0xf00001cf,
 	0x1bf40114,
 	0xfc10fcfa,
+/* 0x03eb: cmd_exec_query */
 	0xc800f800,
 	0x1bf40d34,
 	0xd121f570,
@@ -419,6 +488,7 @@ uint32_t nvc0_pcopy_code[] = {
 	0x0153f026,
 	0x080047f1,
 	0xd00644b6,
+/* 0x045e: query_counter */
 	0x21f50045,
 	0x47f103d1,
 	0x44b6080c,
@@ -442,11 +512,13 @@ uint32_t nvc0_pcopy_code[] = {
 	0x080047f1,
 	0xd00644b6,
 	0x00f80045,
+/* 0x04b8: cmd_exec */
 	0x03d121f5,
 	0xf4003fc8,
 	0x21f50e0b,
 	0x47f101af,
 	0x0ef40200,
+/* 0x04cd: cmd_exec_no_format */
 	0x1067f11e,
 	0x0664b608,
 	0x800177f0,
@@ -454,18 +526,23 @@ uint32_t nvc0_pcopy_code[] = {
 	0x1d079819,
 	0xd00067d0,
 	0x44bd4067,
+/* 0x04e8: cmd_exec_init_src_surface */
 	0xbd0232f4,
 	0x043fc854,
 	0xf50a0bf4,
 	0xf403a821,
+/* 0x04fa: src_tiled */
 	0x21f50a0e,
 	0x49f0029c,
+/* 0x0501: cmd_exec_init_dst_surface */
 	0x0231f407,
 	0xc82c57f0,
 	0x0bf4083f,
 	0xa821f50a,
 	0x0a0ef403,
+/* 0x0514: dst_tiled */
 	0x029c21f5,
+/* 0x051b: cmd_exec_kick */
 	0xf10849f0,
 	0xb6080057,
 	0x06980654,
@@ -475,7 +552,9 @@ uint32_t nvc0_pcopy_code[] = {
 	0x54d00546,
 	0x0c3fc800,
 	0xf5070bf4,
+/* 0x053f: cmd_exec_done */
 	0xf803eb21,
+/* 0x0541: cmd_wrcache_flush */
 	0x0027f100,
 	0xf034bd22,
 	0x23d00133,
diff --git a/drivers/gpu/drm/nouveau/nvd0_display.c b/drivers/gpu/drm/nouveau/nvd0_display.c
index c486d3ce3c2..d0d60e1e7f9 100644
--- a/drivers/gpu/drm/nouveau/nvd0_display.c
+++ b/drivers/gpu/drm/nouveau/nvd0_display.c
@@ -607,7 +607,7 @@ nvd0_crtc_commit(struct drm_crtc *crtc)
 }
 
 static bool
-nvd0_crtc_mode_fixup(struct drm_crtc *crtc, struct drm_display_mode *mode,
+nvd0_crtc_mode_fixup(struct drm_crtc *crtc, const struct drm_display_mode *mode,
 		     struct drm_display_mode *adjusted_mode)
 {
 	return true;
@@ -938,7 +938,8 @@ nvd0_dac_dpms(struct drm_encoder *encoder, int mode)
 }
 
 static bool
-nvd0_dac_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
+nvd0_dac_mode_fixup(struct drm_encoder *encoder,
+		    const struct drm_display_mode *mode,
 		    struct drm_display_mode *adjusted_mode)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
@@ -1377,7 +1378,8 @@ nvd0_sor_dpms(struct drm_encoder *encoder, int mode)
 }
 
 static bool
-nvd0_sor_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
+nvd0_sor_mode_fixup(struct drm_encoder *encoder,
+		    const struct drm_display_mode *mode,
 		    struct drm_display_mode *adjusted_mode)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
diff --git a/drivers/gpu/drm/r128/r128_drv.c b/drivers/gpu/drm/r128/r128_drv.c
index 88718fad5d6..2666a5308ab 100644
--- a/drivers/gpu/drm/r128/r128_drv.c
+++ b/drivers/gpu/drm/r128/r128_drv.c
@@ -71,7 +71,6 @@ static struct drm_driver driver = {
 	.irq_postinstall = r128_driver_irq_postinstall,
 	.irq_uninstall = r128_driver_irq_uninstall,
 	.irq_handler = r128_driver_irq_handler,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = r128_ioctls,
 	.dma_ioctl = r128_cce_buffers,
 	.fops = &r128_driver_fops,
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index 3904d7964a4..9e6f76fec52 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -457,22 +457,18 @@ static void atombios_crtc_program_ss(struct radeon_device *rdev,
 		switch (pll_id) {
 		case ATOM_PPLL1:
 			args.v3.ucSpreadSpectrumType |= ATOM_PPLL_SS_TYPE_V3_P1PLL;
-			args.v3.usSpreadSpectrumAmount = cpu_to_le16(ss->amount);
-			args.v3.usSpreadSpectrumStep = cpu_to_le16(ss->step);
 			break;
 		case ATOM_PPLL2:
 			args.v3.ucSpreadSpectrumType |= ATOM_PPLL_SS_TYPE_V3_P2PLL;
-			args.v3.usSpreadSpectrumAmount = cpu_to_le16(ss->amount);
-			args.v3.usSpreadSpectrumStep = cpu_to_le16(ss->step);
 			break;
 		case ATOM_DCPLL:
 			args.v3.ucSpreadSpectrumType |= ATOM_PPLL_SS_TYPE_V3_DCPLL;
-			args.v3.usSpreadSpectrumAmount = cpu_to_le16(0);
-			args.v3.usSpreadSpectrumStep = cpu_to_le16(0);
 			break;
 		case ATOM_PPLL_INVALID:
 			return;
 		}
+		args.v3.usSpreadSpectrumAmount = cpu_to_le16(ss->amount);
+		args.v3.usSpreadSpectrumStep = cpu_to_le16(ss->step);
 		args.v3.ucEnable = enable;
 		if ((ss->percentage == 0) || (ss->type & ATOM_EXTERNAL_SS_MASK) || ASIC_IS_DCE61(rdev))
 			args.v3.ucEnable = ATOM_DISABLE;
@@ -482,22 +478,18 @@ static void atombios_crtc_program_ss(struct radeon_device *rdev,
 		switch (pll_id) {
 		case ATOM_PPLL1:
 			args.v2.ucSpreadSpectrumType |= ATOM_PPLL_SS_TYPE_V2_P1PLL;
-			args.v2.usSpreadSpectrumAmount = cpu_to_le16(ss->amount);
-			args.v2.usSpreadSpectrumStep = cpu_to_le16(ss->step);
 			break;
 		case ATOM_PPLL2:
 			args.v2.ucSpreadSpectrumType |= ATOM_PPLL_SS_TYPE_V2_P2PLL;
-			args.v2.usSpreadSpectrumAmount = cpu_to_le16(ss->amount);
-			args.v2.usSpreadSpectrumStep = cpu_to_le16(ss->step);
 			break;
 		case ATOM_DCPLL:
 			args.v2.ucSpreadSpectrumType |= ATOM_PPLL_SS_TYPE_V2_DCPLL;
-			args.v2.usSpreadSpectrumAmount = cpu_to_le16(0);
-			args.v2.usSpreadSpectrumStep = cpu_to_le16(0);
 			break;
 		case ATOM_PPLL_INVALID:
 			return;
 		}
+		args.v2.usSpreadSpectrumAmount = cpu_to_le16(ss->amount);
+		args.v2.usSpreadSpectrumStep = cpu_to_le16(ss->step);
 		args.v2.ucEnable = enable;
 		if ((ss->percentage == 0) || (ss->type & ATOM_EXTERNAL_SS_MASK) || ASIC_IS_DCE41(rdev))
 			args.v2.ucEnable = ATOM_DISABLE;
@@ -1539,7 +1531,11 @@ static int radeon_atom_pick_pll(struct drm_crtc *crtc)
 				 * crtc virtual pixel clock.
 				 */
 				if (ENCODER_MODE_IS_DP(atombios_get_encoder_mode(test_encoder))) {
-					if (ASIC_IS_DCE5(rdev) || rdev->clock.dp_extclk)
+					if (ASIC_IS_DCE5(rdev))
+						return ATOM_DCPLL;
+					else if (ASIC_IS_DCE6(rdev))
+						return ATOM_PPLL0;
+					else if (rdev->clock.dp_extclk)
 						return ATOM_PPLL_INVALID;
 				}
 			}
@@ -1628,7 +1624,7 @@ int atombios_crtc_mode_set(struct drm_crtc *crtc,
 }
 
 static bool atombios_crtc_mode_fixup(struct drm_crtc *crtc,
-				     struct drm_display_mode *mode,
+				     const struct drm_display_mode *mode,
 				     struct drm_display_mode *adjusted_mode)
 {
 	if (!radeon_crtc_scaling_mode_fixup(crtc, mode, adjusted_mode))
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index 5131b3b0f7d..7712cf5ab33 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -22,6 +22,7 @@
  *
  * Authors: Dave Airlie
  *          Alex Deucher
+ *          Jerome Glisse
  */
 #include "drmP.h"
 #include "radeon_drm.h"
@@ -608,7 +609,7 @@ int radeon_dp_get_panel_mode(struct drm_encoder *encoder,
 }
 
 void radeon_dp_set_link_config(struct drm_connector *connector,
-			       struct drm_display_mode *mode)
+			       const struct drm_display_mode *mode)
 {
 	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 	struct radeon_connector_atom_dig *dig_connector;
@@ -654,7 +655,6 @@ static bool radeon_dp_get_link_status(struct radeon_connector *radeon_connector,
 	ret = radeon_dp_aux_native_read(radeon_connector, DP_LANE0_1_STATUS,
 					link_status, DP_LINK_STATUS_SIZE, 100);
 	if (ret <= 0) {
-		DRM_ERROR("displayport link status failed\n");
 		return false;
 	}
 
@@ -833,8 +833,10 @@ static int radeon_dp_link_train_cr(struct radeon_dp_link_train_info *dp_info)
 		else
 			mdelay(dp_info->rd_interval * 4);
 
-		if (!radeon_dp_get_link_status(dp_info->radeon_connector, dp_info->link_status))
+		if (!radeon_dp_get_link_status(dp_info->radeon_connector, dp_info->link_status)) {
+			DRM_ERROR("displayport link status failed\n");
 			break;
+		}
 
 		if (dp_clock_recovery_ok(dp_info->link_status, dp_info->dp_lane_count)) {
 			clock_recovery = true;
@@ -896,8 +898,10 @@ static int radeon_dp_link_train_ce(struct radeon_dp_link_train_info *dp_info)
 		else
 			mdelay(dp_info->rd_interval * 4);
 
-		if (!radeon_dp_get_link_status(dp_info->radeon_connector, dp_info->link_status))
+		if (!radeon_dp_get_link_status(dp_info->radeon_connector, dp_info->link_status)) {
+			DRM_ERROR("displayport link status failed\n");
 			break;
+		}
 
 		if (dp_channel_eq_ok(dp_info->link_status, dp_info->dp_lane_count)) {
 			channel_eq = true;
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 486ccdf4aac..f9bc27fe269 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -58,7 +58,7 @@ static inline bool radeon_encoder_is_digital(struct drm_encoder *encoder)
 }
 
 static bool radeon_atom_mode_fixup(struct drm_encoder *encoder,
-				   struct drm_display_mode *mode,
+				   const struct drm_display_mode *mode,
 				   struct drm_display_mode *adjusted_mode)
 {
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
@@ -1392,10 +1392,18 @@ radeon_atom_encoder_dpms_dig(struct drm_encoder *encoder, int mode)
 	case DRM_MODE_DPMS_ON:
 		/* some early dce3.2 boards have a bug in their transmitter control table */
 		if ((rdev->family == CHIP_RV710) || (rdev->family == CHIP_RV730) ||
-		    ASIC_IS_DCE41(rdev) || ASIC_IS_DCE5(rdev))
+		    ASIC_IS_DCE41(rdev) || ASIC_IS_DCE5(rdev)) {
+			if (ASIC_IS_DCE6(rdev)) {
+				/* It seems we need to call ATOM_ENCODER_CMD_SETUP again
+				 * before reenabling encoder on DPMS ON, otherwise we never
+				 * get picture
+				 */
+				atombios_dig_encoder_setup(encoder, ATOM_ENCODER_CMD_SETUP, 0);
+			}
 			atombios_dig_transmitter_setup(encoder, ATOM_TRANSMITTER_ACTION_ENABLE, 0, 0);
-		else
+		} else {
 			atombios_dig_transmitter_setup(encoder, ATOM_TRANSMITTER_ACTION_ENABLE_OUTPUT, 0, 0);
+		}
 		if (ENCODER_MODE_IS_DP(atombios_get_encoder_mode(encoder)) && connector) {
 			if (connector->connector_type == DRM_MODE_CONNECTOR_eDP) {
 				atombios_set_edp_panel_power(connector,
@@ -2234,7 +2242,7 @@ radeon_atom_ext_dpms(struct drm_encoder *encoder, int mode)
 }
 
 static bool radeon_atom_ext_mode_fixup(struct drm_encoder *encoder,
-				       struct drm_display_mode *mode,
+				       const struct drm_display_mode *mode,
 				       struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 7fb3d2e0434..e585a3b947e 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -99,6 +99,14 @@ void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * dce4_wait_for_vblank - vblank wait asic callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to wait for vblank on
+ *
+ * Wait for vblank on the requested crtc (evergreen+).
+ */
 void dce4_wait_for_vblank(struct radeon_device *rdev, int crtc)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc];
@@ -118,18 +126,49 @@ void dce4_wait_for_vblank(struct radeon_device *rdev, int crtc)
 	}
 }
 
+/**
+ * radeon_irq_kms_pflip_irq_get - pre-pageflip callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to prepare for pageflip on
+ *
+ * Pre-pageflip callback (evergreen+).
+ * Enables the pageflip irq (vblank irq).
+ */
 void evergreen_pre_page_flip(struct radeon_device *rdev, int crtc)
 {
 	/* enable the pflip int */
 	radeon_irq_kms_pflip_irq_get(rdev, crtc);
 }
 
+/**
+ * evergreen_post_page_flip - pos-pageflip callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to cleanup pageflip on
+ *
+ * Post-pageflip callback (evergreen+).
+ * Disables the pageflip irq (vblank irq).
+ */
 void evergreen_post_page_flip(struct radeon_device *rdev, int crtc)
 {
 	/* disable the pflip int */
 	radeon_irq_kms_pflip_irq_put(rdev, crtc);
 }
 
+/**
+ * evergreen_page_flip - pageflip callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc_id: crtc to cleanup pageflip on
+ * @crtc_base: new address of the crtc (GPU MC address)
+ *
+ * Does the actual pageflip (evergreen+).
+ * During vblank we take the crtc lock and wait for the update_pending
+ * bit to go high, when it does, we release the lock, and allow the
+ * double buffered update to take place.
+ * Returns the current update pending status.
+ */
 u32 evergreen_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
@@ -214,6 +253,15 @@ int sumo_get_temp(struct radeon_device *rdev)
 	return actual_temp * 1000;
 }
 
+/**
+ * sumo_pm_init_profile - Initialize power profiles callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Initialize the power states used in profile mode
+ * (sumo, trinity, SI).
+ * Used for profile mode only.
+ */
 void sumo_pm_init_profile(struct radeon_device *rdev)
 {
 	int idx;
@@ -265,6 +313,14 @@ void sumo_pm_init_profile(struct radeon_device *rdev)
 		rdev->pm.power_state[idx].num_clock_modes - 1;
 }
 
+/**
+ * evergreen_pm_misc - set additional pm hw parameters callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Set non-clock parameters associated with a power state
+ * (voltage, etc.) (evergreen+).
+ */
 void evergreen_pm_misc(struct radeon_device *rdev)
 {
 	int req_ps_idx = rdev->pm.requested_power_state_index;
@@ -292,6 +348,13 @@ void evergreen_pm_misc(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * evergreen_pm_prepare - pre-power state change callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Prepare for a power state change (evergreen+).
+ */
 void evergreen_pm_prepare(struct radeon_device *rdev)
 {
 	struct drm_device *ddev = rdev->ddev;
@@ -310,6 +373,13 @@ void evergreen_pm_prepare(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * evergreen_pm_finish - post-power state change callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Clean up after a power state change (evergreen+).
+ */
 void evergreen_pm_finish(struct radeon_device *rdev)
 {
 	struct drm_device *ddev = rdev->ddev;
@@ -328,6 +398,15 @@ void evergreen_pm_finish(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * evergreen_hpd_sense - hpd sense callback.
+ *
+ * @rdev: radeon_device pointer
+ * @hpd: hpd (hotplug detect) pin
+ *
+ * Checks if a digital monitor is connected (evergreen+).
+ * Returns true if connected, false if not connected.
+ */
 bool evergreen_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd)
 {
 	bool connected = false;
@@ -364,6 +443,14 @@ bool evergreen_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd)
 	return connected;
 }
 
+/**
+ * evergreen_hpd_set_polarity - hpd set polarity callback.
+ *
+ * @rdev: radeon_device pointer
+ * @hpd: hpd (hotplug detect) pin
+ *
+ * Set the polarity of the hpd pin (evergreen+).
+ */
 void evergreen_hpd_set_polarity(struct radeon_device *rdev,
 				enum radeon_hpd_id hpd)
 {
@@ -424,10 +511,19 @@ void evergreen_hpd_set_polarity(struct radeon_device *rdev,
 	}
 }
 
+/**
+ * evergreen_hpd_init - hpd setup callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Setup the hpd pins used by the card (evergreen+).
+ * Enable the pin, set the polarity, and enable the hpd interrupts.
+ */
 void evergreen_hpd_init(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned enabled = 0;
 	u32 tmp = DC_HPDx_CONNECTION_TIMER(0x9c4) |
 		DC_HPDx_RX_INT_TIMER(0xfa) | DC_HPDx_EN;
 
@@ -436,73 +532,72 @@ void evergreen_hpd_init(struct radeon_device *rdev)
 		switch (radeon_connector->hpd.hpd) {
 		case RADEON_HPD_1:
 			WREG32(DC_HPD1_CONTROL, tmp);
-			rdev->irq.hpd[0] = true;
 			break;
 		case RADEON_HPD_2:
 			WREG32(DC_HPD2_CONTROL, tmp);
-			rdev->irq.hpd[1] = true;
 			break;
 		case RADEON_HPD_3:
 			WREG32(DC_HPD3_CONTROL, tmp);
-			rdev->irq.hpd[2] = true;
 			break;
 		case RADEON_HPD_4:
 			WREG32(DC_HPD4_CONTROL, tmp);
-			rdev->irq.hpd[3] = true;
 			break;
 		case RADEON_HPD_5:
 			WREG32(DC_HPD5_CONTROL, tmp);
-			rdev->irq.hpd[4] = true;
 			break;
 		case RADEON_HPD_6:
 			WREG32(DC_HPD6_CONTROL, tmp);
-			rdev->irq.hpd[5] = true;
 			break;
 		default:
 			break;
 		}
 		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
+		enabled |= 1 << radeon_connector->hpd.hpd;
 	}
-	if (rdev->irq.installed)
-		evergreen_irq_set(rdev);
+	radeon_irq_kms_enable_hpd(rdev, enabled);
 }
 
+/**
+ * evergreen_hpd_fini - hpd tear down callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Tear down the hpd pins used by the card (evergreen+).
+ * Disable the hpd interrupts.
+ */
 void evergreen_hpd_fini(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned disabled = 0;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 		switch (radeon_connector->hpd.hpd) {
 		case RADEON_HPD_1:
 			WREG32(DC_HPD1_CONTROL, 0);
-			rdev->irq.hpd[0] = false;
 			break;
 		case RADEON_HPD_2:
 			WREG32(DC_HPD2_CONTROL, 0);
-			rdev->irq.hpd[1] = false;
 			break;
 		case RADEON_HPD_3:
 			WREG32(DC_HPD3_CONTROL, 0);
-			rdev->irq.hpd[2] = false;
 			break;
 		case RADEON_HPD_4:
 			WREG32(DC_HPD4_CONTROL, 0);
-			rdev->irq.hpd[3] = false;
 			break;
 		case RADEON_HPD_5:
 			WREG32(DC_HPD5_CONTROL, 0);
-			rdev->irq.hpd[4] = false;
 			break;
 		case RADEON_HPD_6:
 			WREG32(DC_HPD6_CONTROL, 0);
-			rdev->irq.hpd[5] = false;
 			break;
 		default:
 			break;
 		}
+		disabled |= 1 << radeon_connector->hpd.hpd;
 	}
+	radeon_irq_kms_disable_hpd(rdev, disabled);
 }
 
 /* watermark setup */
@@ -933,6 +1028,14 @@ static void evergreen_program_watermarks(struct radeon_device *rdev,
 
 }
 
+/**
+ * evergreen_bandwidth_update - update display watermarks callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Update the display watermarks based on the requested mode(s)
+ * (evergreen+).
+ */
 void evergreen_bandwidth_update(struct radeon_device *rdev)
 {
 	struct drm_display_mode *mode0 = NULL;
@@ -956,6 +1059,15 @@ void evergreen_bandwidth_update(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * evergreen_mc_wait_for_idle - wait for MC idle callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Wait for the MC (memory controller) to be idle.
+ * (evergreen+).
+ * Returns 0 if the MC is idle, -1 if not.
+ */
 int evergreen_mc_wait_for_idle(struct radeon_device *rdev)
 {
 	unsigned i;
@@ -1371,12 +1483,28 @@ void evergreen_mc_program(struct radeon_device *rdev)
  */
 void evergreen_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	u32 next_rptr;
 
 	/* set to DX10/11 mode */
 	radeon_ring_write(ring, PACKET3(PACKET3_MODE_CONTROL, 0));
 	radeon_ring_write(ring, 1);
-	/* FIXME: implement */
+
+	if (ring->rptr_save_reg) {
+		next_rptr = ring->wptr + 3 + 4;
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, ((ring->rptr_save_reg - 
+					  PACKET3_SET_CONFIG_REG_START) >> 2));
+		radeon_ring_write(ring, next_rptr);
+	} else if (rdev->wb.enabled) {
+		next_rptr = ring->wptr + 5 + 4;
+		radeon_ring_write(ring, PACKET3(PACKET3_MEM_WRITE, 3));
+		radeon_ring_write(ring, ring->next_rptr_gpu_addr & 0xfffffffc);
+		radeon_ring_write(ring, (upper_32_bits(ring->next_rptr_gpu_addr) & 0xff) | (1 << 18));
+		radeon_ring_write(ring, next_rptr);
+		radeon_ring_write(ring, 0);
+	}
+
 	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
 	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
@@ -2188,6 +2316,14 @@ static int evergreen_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(GRBM_STATUS_SE1));
 	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
 		RREG32(SRBM_STATUS));
+	dev_info(rdev->dev, "  R_008674_CP_STALLED_STAT1 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT1));
+	dev_info(rdev->dev, "  R_008678_CP_STALLED_STAT2 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT2));
+	dev_info(rdev->dev, "  R_00867C_CP_BUSY_STAT     = 0x%08X\n",
+		RREG32(CP_BUSY_STAT));
+	dev_info(rdev->dev, "  R_008680_CP_STAT          = 0x%08X\n",
+		RREG32(CP_STAT));
 	evergreen_mc_stop(rdev, &save);
 	if (evergreen_mc_wait_for_idle(rdev)) {
 		dev_warn(rdev->dev, "Wait for MC idle timedout !\n");
@@ -2225,6 +2361,14 @@ static int evergreen_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(GRBM_STATUS_SE1));
 	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
 		RREG32(SRBM_STATUS));
+	dev_info(rdev->dev, "  R_008674_CP_STALLED_STAT1 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT1));
+	dev_info(rdev->dev, "  R_008678_CP_STALLED_STAT2 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT2));
+	dev_info(rdev->dev, "  R_00867C_CP_BUSY_STAT     = 0x%08X\n",
+		RREG32(CP_BUSY_STAT));
+	dev_info(rdev->dev, "  R_008680_CP_STAT          = 0x%08X\n",
+		RREG32(CP_STAT));
 	evergreen_mc_resume(rdev, &save);
 	return 0;
 }
@@ -2348,20 +2492,20 @@ int evergreen_irq_set(struct radeon_device *rdev)
 
 	if (rdev->family >= CHIP_CAYMAN) {
 		/* enable CP interrupts on all rings */
-		if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+		if (atomic_read(&rdev->irq.ring_int[RADEON_RING_TYPE_GFX_INDEX])) {
 			DRM_DEBUG("evergreen_irq_set: sw int gfx\n");
 			cp_int_cntl |= TIME_STAMP_INT_ENABLE;
 		}
-		if (rdev->irq.sw_int[CAYMAN_RING_TYPE_CP1_INDEX]) {
+		if (atomic_read(&rdev->irq.ring_int[CAYMAN_RING_TYPE_CP1_INDEX])) {
 			DRM_DEBUG("evergreen_irq_set: sw int cp1\n");
 			cp_int_cntl1 |= TIME_STAMP_INT_ENABLE;
 		}
-		if (rdev->irq.sw_int[CAYMAN_RING_TYPE_CP2_INDEX]) {
+		if (atomic_read(&rdev->irq.ring_int[CAYMAN_RING_TYPE_CP2_INDEX])) {
 			DRM_DEBUG("evergreen_irq_set: sw int cp2\n");
 			cp_int_cntl2 |= TIME_STAMP_INT_ENABLE;
 		}
 	} else {
-		if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+		if (atomic_read(&rdev->irq.ring_int[RADEON_RING_TYPE_GFX_INDEX])) {
 			DRM_DEBUG("evergreen_irq_set: sw int gfx\n");
 			cp_int_cntl |= RB_INT_ENABLE;
 			cp_int_cntl |= TIME_STAMP_INT_ENABLE;
@@ -2369,32 +2513,32 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	}
 
 	if (rdev->irq.crtc_vblank_int[0] ||
-	    rdev->irq.pflip[0]) {
+	    atomic_read(&rdev->irq.pflip[0])) {
 		DRM_DEBUG("evergreen_irq_set: vblank 0\n");
 		crtc1 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[1] ||
-	    rdev->irq.pflip[1]) {
+	    atomic_read(&rdev->irq.pflip[1])) {
 		DRM_DEBUG("evergreen_irq_set: vblank 1\n");
 		crtc2 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[2] ||
-	    rdev->irq.pflip[2]) {
+	    atomic_read(&rdev->irq.pflip[2])) {
 		DRM_DEBUG("evergreen_irq_set: vblank 2\n");
 		crtc3 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[3] ||
-	    rdev->irq.pflip[3]) {
+	    atomic_read(&rdev->irq.pflip[3])) {
 		DRM_DEBUG("evergreen_irq_set: vblank 3\n");
 		crtc4 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[4] ||
-	    rdev->irq.pflip[4]) {
+	    atomic_read(&rdev->irq.pflip[4])) {
 		DRM_DEBUG("evergreen_irq_set: vblank 4\n");
 		crtc5 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[5] ||
-	    rdev->irq.pflip[5]) {
+	    atomic_read(&rdev->irq.pflip[5])) {
 		DRM_DEBUG("evergreen_irq_set: vblank 5\n");
 		crtc6 |= VBLANK_INT_MASK;
 	}
@@ -2676,7 +2820,6 @@ int evergreen_irq_process(struct radeon_device *rdev)
 	u32 rptr;
 	u32 src_id, src_data;
 	u32 ring_index;
-	unsigned long flags;
 	bool queue_hotplug = false;
 	bool queue_hdmi = false;
 
@@ -2684,22 +2827,21 @@ int evergreen_irq_process(struct radeon_device *rdev)
 		return IRQ_NONE;
 
 	wptr = evergreen_get_ih_wptr(rdev);
+
+restart_ih:
+	/* is somebody else already processing irqs? */
+	if (atomic_xchg(&rdev->ih.lock, 1))
+		return IRQ_NONE;
+
 	rptr = rdev->ih.rptr;
 	DRM_DEBUG("r600_irq_process start: rptr %d, wptr %d\n", rptr, wptr);
 
-	spin_lock_irqsave(&rdev->ih.lock, flags);
-	if (rptr == wptr) {
-		spin_unlock_irqrestore(&rdev->ih.lock, flags);
-		return IRQ_NONE;
-	}
-restart_ih:
 	/* Order reading of wptr vs. reading of IH ring data */
 	rmb();
 
 	/* display interrupts */
 	evergreen_irq_ack(rdev);
 
-	rdev->ih.wptr = wptr;
 	while (rptr != wptr) {
 		/* wptr/rptr are in bytes! */
 		ring_index = rptr / 4;
@@ -2716,7 +2858,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[0])
+					if (atomic_read(&rdev->irq.pflip[0]))
 						radeon_crtc_handle_flip(rdev, 0);
 					rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D1 vblank\n");
@@ -2742,7 +2884,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[1])
+					if (atomic_read(&rdev->irq.pflip[1]))
 						radeon_crtc_handle_flip(rdev, 1);
 					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D2 vblank\n");
@@ -2768,7 +2910,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[2])
+					if (atomic_read(&rdev->irq.pflip[2]))
 						radeon_crtc_handle_flip(rdev, 2);
 					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D3 vblank\n");
@@ -2794,7 +2936,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[3])
+					if (atomic_read(&rdev->irq.pflip[3]))
 						radeon_crtc_handle_flip(rdev, 3);
 					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D4 vblank\n");
@@ -2820,7 +2962,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[4])
+					if (atomic_read(&rdev->irq.pflip[4]))
 						radeon_crtc_handle_flip(rdev, 4);
 					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D5 vblank\n");
@@ -2846,7 +2988,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[5])
+					if (atomic_read(&rdev->irq.pflip[5]))
 						radeon_crtc_handle_flip(rdev, 5);
 					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D6 vblank\n");
@@ -2986,7 +3128,6 @@ restart_ih:
 			break;
 		case 233: /* GUI IDLE */
 			DRM_DEBUG("IH: GUI idle\n");
-			rdev->pm.gui_idle = true;
 			wake_up(&rdev->irq.idle_queue);
 			break;
 		default:
@@ -2998,17 +3139,19 @@ restart_ih:
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
 	}
-	/* make sure wptr hasn't changed while processing */
-	wptr = evergreen_get_ih_wptr(rdev);
-	if (wptr != rdev->ih.wptr)
-		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
 	if (queue_hdmi)
 		schedule_work(&rdev->audio_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
-	spin_unlock_irqrestore(&rdev->ih.lock, flags);
+	atomic_set(&rdev->ih.lock, 0);
+
+	/* make sure wptr hasn't changed while processing */
+	wptr = evergreen_get_ih_wptr(rdev);
+	if (wptr != rptr)
+		goto restart_ih;
+
 	return IRQ_HANDLED;
 }
 
@@ -3096,13 +3239,11 @@ static int evergreen_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -3146,9 +3287,6 @@ int evergreen_suspend(struct radeon_device *rdev)
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 
 	r600_audio_fini(rdev);
-	/* FIXME: we should wait for ring to be empty */
-	radeon_ib_pool_suspend(rdev);
-	r600_blit_suspend(rdev);
 	r700_cp_stop(rdev);
 	ring->ready = false;
 	evergreen_irq_suspend(rdev);
@@ -3234,20 +3372,14 @@ int evergreen_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = evergreen_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		r700_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		evergreen_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
@@ -3274,7 +3406,7 @@ void evergreen_fini(struct radeon_device *rdev)
 	r700_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	evergreen_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
@@ -3289,7 +3421,8 @@ void evergreen_fini(struct radeon_device *rdev)
 
 void evergreen_pcie_gen2_enable(struct radeon_device *rdev)
 {
-	u32 link_width_cntl, speed_cntl;
+	u32 link_width_cntl, speed_cntl, mask;
+	int ret;
 
 	if (radeon_pcie_gen2 == 0)
 		return;
@@ -3304,6 +3437,15 @@ void evergreen_pcie_gen2_enable(struct radeon_device *rdev)
 	if (ASIC_IS_X2(rdev))
 		return;
 
+	ret = drm_pcie_get_speed_cap_mask(rdev->ddev, &mask);
+	if (ret != 0)
+		return;
+
+	if (!(mask & DRM_PCIE_SPEED_50))
+		return;
+
+	DRM_INFO("enabling PCIE gen 2 link speeds, disable with radeon.pcie_gen2=0\n");
+
 	speed_cntl = RREG32_PCIE_P(PCIE_LC_SPEED_CNTL);
 	if ((speed_cntl & LC_OTHER_SIDE_EVER_SENT_GEN2) ||
 	    (speed_cntl & LC_OTHER_SIDE_SUPPORTS_GEN2)) {
diff --git a/drivers/gpu/drm/radeon/evergreen_blit_kms.c b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
index 1e96bd458cf..89cb9feb565 100644
--- a/drivers/gpu/drm/radeon/evergreen_blit_kms.c
+++ b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
@@ -622,7 +622,8 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	rdev->r600_blit.primitives.draw_auto = draw_auto;
 	rdev->r600_blit.primitives.set_default_state = set_default_state;
 
-	rdev->r600_blit.ring_size_common = 55; /* shaders + def state */
+	rdev->r600_blit.ring_size_common = 8; /* sync semaphore */
+	rdev->r600_blit.ring_size_common += 55; /* shaders + def state */
 	rdev->r600_blit.ring_size_common += 16; /* fence emit for VB IB */
 	rdev->r600_blit.ring_size_common += 5; /* done copy */
 	rdev->r600_blit.ring_size_common += 16; /* fence emit for done copy */
@@ -633,10 +634,6 @@ int evergreen_blit_init(struct radeon_device *rdev)
 
 	rdev->r600_blit.max_dim = 16384;
 
-	/* pin copy shader into vram if already initialized */
-	if (rdev->r600_blit.shader_obj)
-		goto done;
-
 	rdev->r600_blit.state_offset = 0;
 
 	if (rdev->family < CHIP_CAYMAN)
@@ -667,11 +664,26 @@ int evergreen_blit_init(struct radeon_device *rdev)
 		obj_size += cayman_ps_size * 4;
 	obj_size = ALIGN(obj_size, 256);
 
-	r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-			     NULL, &rdev->r600_blit.shader_obj);
-	if (r) {
-		DRM_ERROR("evergreen failed to allocate shader\n");
-		return r;
+	/* pin copy shader into vram if not already initialized */
+	if (!rdev->r600_blit.shader_obj) {
+		r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true,
+				     RADEON_GEM_DOMAIN_VRAM,
+				     NULL, &rdev->r600_blit.shader_obj);
+		if (r) {
+			DRM_ERROR("evergreen failed to allocate shader\n");
+			return r;
+		}
+
+		r = radeon_bo_reserve(rdev->r600_blit.shader_obj, false);
+		if (unlikely(r != 0))
+			return r;
+		r = radeon_bo_pin(rdev->r600_blit.shader_obj, RADEON_GEM_DOMAIN_VRAM,
+				  &rdev->r600_blit.shader_gpu_addr);
+		radeon_bo_unreserve(rdev->r600_blit.shader_obj);
+		if (r) {
+			dev_err(rdev->dev, "(%d) pin blit object failed\n", r);
+			return r;
+		}
 	}
 
 	DRM_DEBUG("evergreen blit allocated bo %08x vs %08x ps %08x\n",
@@ -713,17 +725,6 @@ int evergreen_blit_init(struct radeon_device *rdev)
 	radeon_bo_kunmap(rdev->r600_blit.shader_obj);
 	radeon_bo_unreserve(rdev->r600_blit.shader_obj);
 
-done:
-	r = radeon_bo_reserve(rdev->r600_blit.shader_obj, false);
-	if (unlikely(r != 0))
-		return r;
-	r = radeon_bo_pin(rdev->r600_blit.shader_obj, RADEON_GEM_DOMAIN_VRAM,
-			  &rdev->r600_blit.shader_gpu_addr);
-	radeon_bo_unreserve(rdev->r600_blit.shader_obj);
-	if (r) {
-		dev_err(rdev->dev, "(%d) pin blit object failed\n", r);
-		return r;
-	}
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.real_vram_size);
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index b50b15c7049..d3bd098e4e1 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -88,6 +88,10 @@
 #define	CONFIG_MEMSIZE					0x5428
 
 #define	CP_COHER_BASE					0x85F8
+#define	CP_STALLED_STAT1			0x8674
+#define	CP_STALLED_STAT2			0x8678
+#define	CP_BUSY_STAT				0x867C
+#define	CP_STAT						0x8680
 #define CP_ME_CNTL					0x86D8
 #define		CP_ME_HALT					(1 << 28)
 #define		CP_PFP_HALT					(1 << 26)
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index b7bf18e4021..9945d86d900 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -850,11 +850,20 @@ void cayman_fence_ring_emit(struct radeon_device *rdev,
 
 void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+	struct radeon_ring *ring = &rdev->ring[ib->ring];
 
 	/* set to DX10/11 mode */
 	radeon_ring_write(ring, PACKET3(PACKET3_MODE_CONTROL, 0));
 	radeon_ring_write(ring, 1);
+
+	if (ring->rptr_save_reg) {
+		uint32_t next_rptr = ring->wptr + 3 + 4 + 8;
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, ((ring->rptr_save_reg - 
+					  PACKET3_SET_CONFIG_REG_START) >> 2));
+		radeon_ring_write(ring, next_rptr);
+	}
+
 	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
 	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
@@ -981,16 +990,41 @@ static int cayman_cp_start(struct radeon_device *rdev)
 
 static void cayman_cp_fini(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	cayman_cp_enable(rdev, false);
-	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
+	radeon_ring_fini(rdev, ring);
+	radeon_scratch_free(rdev, ring->rptr_save_reg);
 }
 
 int cayman_cp_resume(struct radeon_device *rdev)
 {
+	static const int ridx[] = {
+		RADEON_RING_TYPE_GFX_INDEX,
+		CAYMAN_RING_TYPE_CP1_INDEX,
+		CAYMAN_RING_TYPE_CP2_INDEX
+	};
+	static const unsigned cp_rb_cntl[] = {
+		CP_RB0_CNTL,
+		CP_RB1_CNTL,
+		CP_RB2_CNTL,
+	};
+	static const unsigned cp_rb_rptr_addr[] = {
+		CP_RB0_RPTR_ADDR,
+		CP_RB1_RPTR_ADDR,
+		CP_RB2_RPTR_ADDR
+	};
+	static const unsigned cp_rb_rptr_addr_hi[] = {
+		CP_RB0_RPTR_ADDR_HI,
+		CP_RB1_RPTR_ADDR_HI,
+		CP_RB2_RPTR_ADDR_HI
+	};
+	static const unsigned cp_rb_base[] = {
+		CP_RB0_BASE,
+		CP_RB1_BASE,
+		CP_RB2_BASE
+	};
 	struct radeon_ring *ring;
-	u32 tmp;
-	u32 rb_bufsz;
-	int r;
+	int i, r;
 
 	/* Reset cp; if cp is reset, then PA, SH, VGT also need to be reset */
 	WREG32(GRBM_SOFT_RESET, (SOFT_RESET_CP |
@@ -1012,91 +1046,47 @@ int cayman_cp_resume(struct radeon_device *rdev)
 
 	WREG32(CP_DEBUG, (1 << 27));
 
-	/* ring 0 - compute and gfx */
-	/* Set ring buffer size */
-	ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
-	rb_bufsz = drm_order(ring->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);
-	ring->wptr = 0;
-	WREG32(CP_RB0_WPTR, ring->wptr);
-
 	/* 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);
+	WREG32(SCRATCH_UMSK, 0xff);
 
-	if (rdev->wb.enabled)
-		WREG32(SCRATCH_UMSK, 0xff);
-	else {
-		tmp |= RB_NO_UPDATE;
-		WREG32(SCRATCH_UMSK, 0);
-	}
+	for (i = 0; i < 3; ++i) {
+		uint32_t rb_cntl;
+		uint64_t addr;
 
-	mdelay(1);
-	WREG32(CP_RB0_CNTL, tmp);
-
-	WREG32(CP_RB0_BASE, ring->gpu_addr >> 8);
-
-	ring->rptr = RREG32(CP_RB0_RPTR);
-
-	/* ring1  - compute only */
-	/* Set ring buffer size */
-	ring = &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX];
-	rb_bufsz = drm_order(ring->ring_size / 8);
-	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
+		/* Set ring buffer size */
+		ring = &rdev->ring[ridx[i]];
+		rb_cntl = drm_order(ring->ring_size / 8);
+		rb_cntl |= drm_order(RADEON_GPU_PAGE_SIZE/8) << 8;
 #ifdef __BIG_ENDIAN
-	tmp |= BUF_SWAP_32BIT;
+		rb_cntl |= 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);
-	ring->wptr = 0;
-	WREG32(CP_RB1_WPTR, ring->wptr);
-
-	/* 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_rb_cntl[i], rb_cntl);
 
-	WREG32(CP_RB1_BASE, ring->gpu_addr >> 8);
-
-	ring->rptr = RREG32(CP_RB1_RPTR);
-
-	/* ring2 - compute only */
-	/* Set ring buffer size */
-	ring = &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX];
-	rb_bufsz = drm_order(ring->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);
-	ring->wptr = 0;
-	WREG32(CP_RB2_WPTR, ring->wptr);
+		/* set the wb address wether it's enabled or not */
+		addr = rdev->wb.gpu_addr + RADEON_WB_CP_RPTR_OFFSET;
+		WREG32(cp_rb_rptr_addr[i], addr & 0xFFFFFFFC);
+		WREG32(cp_rb_rptr_addr_hi[i], upper_32_bits(addr) & 0xFF);
+	}
 
-	/* 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);
+	/* set the rb base addr, this causes an internal reset of ALL rings */
+	for (i = 0; i < 3; ++i) {
+		ring = &rdev->ring[ridx[i]];
+		WREG32(cp_rb_base[i], ring->gpu_addr >> 8);
+	}
 
-	mdelay(1);
-	WREG32(CP_RB2_CNTL, tmp);
+	for (i = 0; i < 3; ++i) {
+		/* Initialize the ring buffer's read and write pointers */
+		ring = &rdev->ring[ridx[i]];
+		WREG32_P(cp_rb_cntl[i], RB_RPTR_WR_ENA, ~RB_RPTR_WR_ENA);
 
-	WREG32(CP_RB2_BASE, ring->gpu_addr >> 8);
+		ring->rptr = ring->wptr = 0;
+		WREG32(ring->rptr_reg, ring->rptr);
+		WREG32(ring->wptr_reg, ring->wptr);
 
-	ring->rptr = RREG32(CP_RB2_RPTR);
+		mdelay(1);
+		WREG32_P(cp_rb_cntl[i], 0, ~RB_RPTR_WR_ENA);
+	}
 
 	/* start the rings */
 	cayman_cp_start(rdev);
@@ -1132,6 +1122,14 @@ static int cayman_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(GRBM_STATUS_SE1));
 	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
 		RREG32(SRBM_STATUS));
+	dev_info(rdev->dev, "  R_008674_CP_STALLED_STAT1 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT1));
+	dev_info(rdev->dev, "  R_008678_CP_STALLED_STAT2 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT2));
+	dev_info(rdev->dev, "  R_00867C_CP_BUSY_STAT     = 0x%08X\n",
+		RREG32(CP_BUSY_STAT));
+	dev_info(rdev->dev, "  R_008680_CP_STAT          = 0x%08X\n",
+		RREG32(CP_STAT));
 	dev_info(rdev->dev, "  VM_CONTEXT0_PROTECTION_FAULT_ADDR   0x%08X\n",
 		 RREG32(0x14F8));
 	dev_info(rdev->dev, "  VM_CONTEXT0_PROTECTION_FAULT_STATUS 0x%08X\n",
@@ -1180,6 +1178,14 @@ static int cayman_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(GRBM_STATUS_SE1));
 	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
 		RREG32(SRBM_STATUS));
+	dev_info(rdev->dev, "  R_008674_CP_STALLED_STAT1 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT1));
+	dev_info(rdev->dev, "  R_008678_CP_STALLED_STAT2 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT2));
+	dev_info(rdev->dev, "  R_00867C_CP_BUSY_STAT     = 0x%08X\n",
+		RREG32(CP_BUSY_STAT));
+	dev_info(rdev->dev, "  R_008680_CP_STAT          = 0x%08X\n",
+		RREG32(CP_STAT));
 	evergreen_mc_resume(rdev, &save);
 	return 0;
 }
@@ -1291,17 +1297,17 @@ static int cayman_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
-	r = radeon_vm_manager_start(rdev);
-	if (r)
+	r = radeon_vm_manager_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "vm manager initialization failed (%d).\n", r);
 		return r;
+	}
 
 	r = r600_audio_init(rdev);
 	if (r)
@@ -1334,10 +1340,6 @@ int cayman_resume(struct radeon_device *rdev)
 int cayman_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
-	/* FIXME: we should wait for ring to be empty */
-	radeon_ib_pool_suspend(rdev);
-	radeon_vm_manager_suspend(rdev);
-	r600_blit_suspend(rdev);
 	cayman_cp_enable(rdev, false);
 	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	evergreen_irq_suspend(rdev);
@@ -1413,17 +1415,7 @@ int cayman_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-	r = radeon_vm_manager_init(rdev);
-	if (r) {
-		dev_err(rdev->dev, "vm manager initialization failed (%d).\n", r);
-	}
-
 	r = cayman_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
@@ -1432,7 +1424,7 @@ int cayman_init(struct radeon_device *rdev)
 		if (rdev->flags & RADEON_IS_IGP)
 			si_rlc_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_vm_manager_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		cayman_pcie_gart_fini(rdev);
@@ -1463,7 +1455,7 @@ void cayman_fini(struct radeon_device *rdev)
 		si_rlc_fini(rdev);
 	radeon_wb_fini(rdev);
 	radeon_vm_manager_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	cayman_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/nid.h b/drivers/gpu/drm/radeon/nid.h
index a0b98066e20..870db340d37 100644
--- a/drivers/gpu/drm/radeon/nid.h
+++ b/drivers/gpu/drm/radeon/nid.h
@@ -236,6 +236,10 @@
 #define	CP_SEM_WAIT_TIMER				0x85BC
 #define	CP_SEM_INCOMPLETE_TIMER_CNTL			0x85C8
 #define	CP_COHER_CNTL2					0x85E8
+#define	CP_STALLED_STAT1			0x8674
+#define	CP_STALLED_STAT2			0x8678
+#define	CP_BUSY_STAT				0x867C
+#define	CP_STAT						0x8680
 #define CP_ME_CNTL					0x86D8
 #define		CP_ME_HALT					(1 << 28)
 #define		CP_PFP_HALT					(1 << 26)
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index fb44e7e4908..8acb34fd3fd 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -65,6 +65,19 @@ MODULE_FIRMWARE(FIRMWARE_R520);
 
 #include "r100_track.h"
 
+/* This files gather functions specifics to:
+ * r100,rv100,rs100,rv200,rs200,r200,rv250,rs300,rv280
+ * and others in some cases.
+ */
+
+/**
+ * r100_wait_for_vblank - vblank wait asic callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to wait for vblank on
+ *
+ * Wait for vblank on the requested crtc (r1xx-r4xx).
+ */
 void r100_wait_for_vblank(struct radeon_device *rdev, int crtc)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc];
@@ -99,128 +112,49 @@ void r100_wait_for_vblank(struct radeon_device *rdev, int crtc)
 	}
 }
 
-/* This files gather functions specifics to:
- * r100,rv100,rs100,rv200,rs200,r200,rv250,rs300,rv280
+/**
+ * r100_pre_page_flip - pre-pageflip callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to prepare for pageflip on
+ *
+ * Pre-pageflip callback (r1xx-r4xx).
+ * Enables the pageflip irq (vblank irq).
  */
-
-int r100_reloc_pitch_offset(struct radeon_cs_parser *p,
-			    struct radeon_cs_packet *pkt,
-			    unsigned idx,
-			    unsigned reg)
-{
-	int r;
-	u32 tile_flags = 0;
-	u32 tmp;
-	struct radeon_cs_reloc *reloc;
-	u32 value;
-
-	r = r100_cs_packet_next_reloc(p, &reloc);
-	if (r) {
-		DRM_ERROR("No reloc for ib[%d]=0x%04X\n",
-			  idx, reg);
-		r100_cs_dump_packet(p, pkt);
-		return r;
-	}
-
-	value = radeon_get_ib_value(p, idx);
-	tmp = value & 0x003fffff;
-	tmp += (((u32)reloc->lobj.gpu_offset) >> 10);
-
-	if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
-		if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO)
-			tile_flags |= RADEON_DST_TILE_MACRO;
-		if (reloc->lobj.tiling_flags & RADEON_TILING_MICRO) {
-			if (reg == RADEON_SRC_PITCH_OFFSET) {
-				DRM_ERROR("Cannot src blit from microtiled surface\n");
-				r100_cs_dump_packet(p, pkt);
-				return -EINVAL;
-			}
-			tile_flags |= RADEON_DST_TILE_MICRO;
-		}
-
-		tmp |= tile_flags;
-		p->ib.ptr[idx] = (value & 0x3fc00000) | tmp;
-	} else
-		p->ib.ptr[idx] = (value & 0xffc00000) | tmp;
-	return 0;
-}
-
-int r100_packet3_load_vbpntr(struct radeon_cs_parser *p,
-			     struct radeon_cs_packet *pkt,
-			     int idx)
-{
-	unsigned c, i;
-	struct radeon_cs_reloc *reloc;
-	struct r100_cs_track *track;
-	int r = 0;
-	volatile uint32_t *ib;
-	u32 idx_value;
-
-	ib = p->ib.ptr;
-	track = (struct r100_cs_track *)p->track;
-	c = radeon_get_ib_value(p, idx++) & 0x1F;
-	if (c > 16) {
-	    DRM_ERROR("Only 16 vertex buffers are allowed %d\n",
-		      pkt->opcode);
-	    r100_cs_dump_packet(p, pkt);
-	    return -EINVAL;
-	}
-	track->num_arrays = c;
-	for (i = 0; i < (c - 1); i+=2, idx+=3) {
-		r = r100_cs_packet_next_reloc(p, &reloc);
-		if (r) {
-			DRM_ERROR("No reloc for packet3 %d\n",
-				  pkt->opcode);
-			r100_cs_dump_packet(p, pkt);
-			return r;
-		}
-		idx_value = radeon_get_ib_value(p, idx);
-		ib[idx+1] = radeon_get_ib_value(p, idx + 1) + ((u32)reloc->lobj.gpu_offset);
-
-		track->arrays[i + 0].esize = idx_value >> 8;
-		track->arrays[i + 0].robj = reloc->robj;
-		track->arrays[i + 0].esize &= 0x7F;
-		r = r100_cs_packet_next_reloc(p, &reloc);
-		if (r) {
-			DRM_ERROR("No reloc for packet3 %d\n",
-				  pkt->opcode);
-			r100_cs_dump_packet(p, pkt);
-			return r;
-		}
-		ib[idx+2] = radeon_get_ib_value(p, idx + 2) + ((u32)reloc->lobj.gpu_offset);
-		track->arrays[i + 1].robj = reloc->robj;
-		track->arrays[i + 1].esize = idx_value >> 24;
-		track->arrays[i + 1].esize &= 0x7F;
-	}
-	if (c & 1) {
-		r = r100_cs_packet_next_reloc(p, &reloc);
-		if (r) {
-			DRM_ERROR("No reloc for packet3 %d\n",
-					  pkt->opcode);
-			r100_cs_dump_packet(p, pkt);
-			return r;
-		}
-		idx_value = radeon_get_ib_value(p, idx);
-		ib[idx+1] = radeon_get_ib_value(p, idx + 1) + ((u32)reloc->lobj.gpu_offset);
-		track->arrays[i + 0].robj = reloc->robj;
-		track->arrays[i + 0].esize = idx_value >> 8;
-		track->arrays[i + 0].esize &= 0x7F;
-	}
-	return r;
-}
-
 void r100_pre_page_flip(struct radeon_device *rdev, int crtc)
 {
 	/* enable the pflip int */
 	radeon_irq_kms_pflip_irq_get(rdev, crtc);
 }
 
+/**
+ * r100_post_page_flip - pos-pageflip callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc: crtc to cleanup pageflip on
+ *
+ * Post-pageflip callback (r1xx-r4xx).
+ * Disables the pageflip irq (vblank irq).
+ */
 void r100_post_page_flip(struct radeon_device *rdev, int crtc)
 {
 	/* disable the pflip int */
 	radeon_irq_kms_pflip_irq_put(rdev, crtc);
 }
 
+/**
+ * r100_page_flip - pageflip callback.
+ *
+ * @rdev: radeon_device pointer
+ * @crtc_id: crtc to cleanup pageflip on
+ * @crtc_base: new address of the crtc (GPU MC address)
+ *
+ * Does the actual pageflip (r1xx-r4xx).
+ * During vblank we take the crtc lock and wait for the update_pending
+ * bit to go high, when it does, we release the lock, and allow the
+ * double buffered update to take place.
+ * Returns the current update pending status.
+ */
 u32 r100_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
@@ -247,6 +181,15 @@ u32 r100_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 	return RREG32(RADEON_CRTC_OFFSET + radeon_crtc->crtc_offset) & RADEON_CRTC_OFFSET__GUI_TRIG_OFFSET;
 }
 
+/**
+ * r100_pm_get_dynpm_state - look up dynpm power state callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Look up the optimal power state based on the
+ * current state of the GPU (r1xx-r5xx).
+ * Used for dynpm only.
+ */
 void r100_pm_get_dynpm_state(struct radeon_device *rdev)
 {
 	int i;
@@ -329,6 +272,15 @@ void r100_pm_get_dynpm_state(struct radeon_device *rdev)
 		  pcie_lanes);
 }
 
+/**
+ * r100_pm_init_profile - Initialize power profiles callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Initialize the power states used in profile mode
+ * (r1xx-r3xx).
+ * Used for profile mode only.
+ */
 void r100_pm_init_profile(struct radeon_device *rdev)
 {
 	/* default */
@@ -368,6 +320,14 @@ void r100_pm_init_profile(struct radeon_device *rdev)
 	rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_cm_idx = 0;
 }
 
+/**
+ * r100_pm_misc - set additional pm hw parameters callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Set non-clock parameters associated with a power state
+ * (voltage, pcie lanes, etc.) (r1xx-r4xx).
+ */
 void r100_pm_misc(struct radeon_device *rdev)
 {
 	int requested_index = rdev->pm.requested_power_state_index;
@@ -459,6 +419,13 @@ void r100_pm_misc(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * r100_pm_prepare - pre-power state change callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Prepare for a power state change (r1xx-r4xx).
+ */
 void r100_pm_prepare(struct radeon_device *rdev)
 {
 	struct drm_device *ddev = rdev->ddev;
@@ -483,6 +450,13 @@ void r100_pm_prepare(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * r100_pm_finish - post-power state change callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Clean up after a power state change (r1xx-r4xx).
+ */
 void r100_pm_finish(struct radeon_device *rdev)
 {
 	struct drm_device *ddev = rdev->ddev;
@@ -507,6 +481,14 @@ void r100_pm_finish(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * r100_gui_idle - gui idle callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Check of the GUI (2D/3D engines) are idle (r1xx-r5xx).
+ * Returns true if idle, false if not.
+ */
 bool r100_gui_idle(struct radeon_device *rdev)
 {
 	if (RREG32(RADEON_RBBM_STATUS) & RADEON_RBBM_ACTIVE)
@@ -516,6 +498,15 @@ bool r100_gui_idle(struct radeon_device *rdev)
 }
 
 /* hpd for digital panel detect/disconnect */
+/**
+ * r100_hpd_sense - hpd sense callback.
+ *
+ * @rdev: radeon_device pointer
+ * @hpd: hpd (hotplug detect) pin
+ *
+ * Checks if a digital monitor is connected (r1xx-r4xx).
+ * Returns true if connected, false if not connected.
+ */
 bool r100_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd)
 {
 	bool connected = false;
@@ -535,6 +526,14 @@ bool r100_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd)
 	return connected;
 }
 
+/**
+ * r100_hpd_set_polarity - hpd set polarity callback.
+ *
+ * @rdev: radeon_device pointer
+ * @hpd: hpd (hotplug detect) pin
+ *
+ * Set the polarity of the hpd pin (r1xx-r4xx).
+ */
 void r100_hpd_set_polarity(struct radeon_device *rdev,
 			   enum radeon_hpd_id hpd)
 {
@@ -563,47 +562,47 @@ void r100_hpd_set_polarity(struct radeon_device *rdev,
 	}
 }
 
+/**
+ * r100_hpd_init - hpd setup callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Setup the hpd pins used by the card (r1xx-r4xx).
+ * Set the polarity, and enable the hpd interrupts.
+ */
 void r100_hpd_init(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned enable = 0;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
-		switch (radeon_connector->hpd.hpd) {
-		case RADEON_HPD_1:
-			rdev->irq.hpd[0] = true;
-			break;
-		case RADEON_HPD_2:
-			rdev->irq.hpd[1] = true;
-			break;
-		default:
-			break;
-		}
+		enable |= 1 << radeon_connector->hpd.hpd;
 		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
-	if (rdev->irq.installed)
-		r100_irq_set(rdev);
+	radeon_irq_kms_enable_hpd(rdev, enable);
 }
 
+/**
+ * r100_hpd_fini - hpd tear down callback.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Tear down the hpd pins used by the card (r1xx-r4xx).
+ * Disable the hpd interrupts.
+ */
 void r100_hpd_fini(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned disable = 0;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
-		switch (radeon_connector->hpd.hpd) {
-		case RADEON_HPD_1:
-			rdev->irq.hpd[0] = false;
-			break;
-		case RADEON_HPD_2:
-			rdev->irq.hpd[1] = false;
-			break;
-		default:
-			break;
-		}
+		disable |= 1 << radeon_connector->hpd.hpd;
 	}
+	radeon_irq_kms_disable_hpd(rdev, disable);
 }
 
 /*
@@ -635,15 +634,6 @@ int r100_pci_gart_init(struct radeon_device *rdev)
 	return radeon_gart_table_ram_alloc(rdev);
 }
 
-/* required on r1xx, r2xx, r300, r(v)350, r420/r481, rs400/rs480 */
-void r100_enable_bm(struct radeon_device *rdev)
-{
-	uint32_t tmp;
-	/* Enable bus mastering */
-	tmp = RREG32(RADEON_BUS_CNTL) & ~RADEON_BUS_MASTER_DIS;
-	WREG32(RADEON_BUS_CNTL, tmp);
-}
-
 int r100_pci_gart_enable(struct radeon_device *rdev)
 {
 	uint32_t tmp;
@@ -705,18 +695,18 @@ int r100_irq_set(struct radeon_device *rdev)
 		WREG32(R_000040_GEN_INT_CNTL, 0);
 		return -EINVAL;
 	}
-	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+	if (atomic_read(&rdev->irq.ring_int[RADEON_RING_TYPE_GFX_INDEX])) {
 		tmp |= RADEON_SW_INT_ENABLE;
 	}
 	if (rdev->irq.gui_idle) {
 		tmp |= RADEON_GUI_IDLE_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[0] ||
-	    rdev->irq.pflip[0]) {
+	    atomic_read(&rdev->irq.pflip[0])) {
 		tmp |= RADEON_CRTC_VBLANK_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[1] ||
-	    rdev->irq.pflip[1]) {
+	    atomic_read(&rdev->irq.pflip[1])) {
 		tmp |= RADEON_CRTC2_VBLANK_MASK;
 	}
 	if (rdev->irq.hpd[0]) {
@@ -782,7 +772,6 @@ int r100_irq_process(struct radeon_device *rdev)
 		/* gui idle interrupt */
 		if (status & RADEON_GUI_IDLE_STAT) {
 			rdev->irq.gui_idle_acked = true;
-			rdev->pm.gui_idle = true;
 			wake_up(&rdev->irq.idle_queue);
 		}
 		/* Vertical blank interrupts */
@@ -792,7 +781,7 @@ int r100_irq_process(struct radeon_device *rdev)
 				rdev->pm.vblank_sync = true;
 				wake_up(&rdev->irq.vblank_queue);
 			}
-			if (rdev->irq.pflip[0])
+			if (atomic_read(&rdev->irq.pflip[0]))
 				radeon_crtc_handle_flip(rdev, 0);
 		}
 		if (status & RADEON_CRTC2_VBLANK_STAT) {
@@ -801,7 +790,7 @@ int r100_irq_process(struct radeon_device *rdev)
 				rdev->pm.vblank_sync = true;
 				wake_up(&rdev->irq.vblank_queue);
 			}
-			if (rdev->irq.pflip[1])
+			if (atomic_read(&rdev->irq.pflip[1]))
 				radeon_crtc_handle_flip(rdev, 1);
 		}
 		if (status & RADEON_FP_DETECT_STAT) {
@@ -883,7 +872,7 @@ int r100_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset,
 		   uint64_t dst_offset,
 		   unsigned num_gpu_pages,
-		   struct radeon_fence *fence)
+		   struct radeon_fence **fence)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	uint32_t cur_pages;
@@ -947,7 +936,7 @@ int r100_copy_blit(struct radeon_device *rdev,
 			  RADEON_WAIT_HOST_IDLECLEAN |
 			  RADEON_WAIT_DMA_GUI_IDLE);
 	if (fence) {
-		r = radeon_fence_emit(rdev, fence);
+		r = radeon_fence_emit(rdev, fence, RADEON_RING_TYPE_GFX_INDEX);
 	}
 	radeon_ring_unlock_commit(rdev, ring);
 	return r;
@@ -1192,6 +1181,14 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	}
 	ring->ready = true;
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.real_vram_size);
+
+	if (radeon_ring_supports_scratch_reg(rdev, ring)) {
+		r = radeon_scratch_get(rdev, &ring->rptr_save_reg);
+		if (r) {
+			DRM_ERROR("failed to get scratch reg for rptr save (%d).\n", r);
+			ring->rptr_save_reg = 0;
+		}
+	}
 	return 0;
 }
 
@@ -1202,6 +1199,7 @@ void r100_cp_fini(struct radeon_device *rdev)
 	}
 	/* Disable ring */
 	r100_cp_disable(rdev);
+	radeon_scratch_free(rdev, rdev->ring[RADEON_RING_TYPE_GFX_INDEX].rptr_save_reg);
 	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 	DRM_INFO("radeon: cp finalized\n");
 }
@@ -1223,6 +1221,112 @@ void r100_cp_disable(struct radeon_device *rdev)
 /*
  * CS functions
  */
+int r100_reloc_pitch_offset(struct radeon_cs_parser *p,
+			    struct radeon_cs_packet *pkt,
+			    unsigned idx,
+			    unsigned reg)
+{
+	int r;
+	u32 tile_flags = 0;
+	u32 tmp;
+	struct radeon_cs_reloc *reloc;
+	u32 value;
+
+	r = r100_cs_packet_next_reloc(p, &reloc);
+	if (r) {
+		DRM_ERROR("No reloc for ib[%d]=0x%04X\n",
+			  idx, reg);
+		r100_cs_dump_packet(p, pkt);
+		return r;
+	}
+
+	value = radeon_get_ib_value(p, idx);
+	tmp = value & 0x003fffff;
+	tmp += (((u32)reloc->lobj.gpu_offset) >> 10);
+
+	if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
+		if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO)
+			tile_flags |= RADEON_DST_TILE_MACRO;
+		if (reloc->lobj.tiling_flags & RADEON_TILING_MICRO) {
+			if (reg == RADEON_SRC_PITCH_OFFSET) {
+				DRM_ERROR("Cannot src blit from microtiled surface\n");
+				r100_cs_dump_packet(p, pkt);
+				return -EINVAL;
+			}
+			tile_flags |= RADEON_DST_TILE_MICRO;
+		}
+
+		tmp |= tile_flags;
+		p->ib.ptr[idx] = (value & 0x3fc00000) | tmp;
+	} else
+		p->ib.ptr[idx] = (value & 0xffc00000) | tmp;
+	return 0;
+}
+
+int r100_packet3_load_vbpntr(struct radeon_cs_parser *p,
+			     struct radeon_cs_packet *pkt,
+			     int idx)
+{
+	unsigned c, i;
+	struct radeon_cs_reloc *reloc;
+	struct r100_cs_track *track;
+	int r = 0;
+	volatile uint32_t *ib;
+	u32 idx_value;
+
+	ib = p->ib.ptr;
+	track = (struct r100_cs_track *)p->track;
+	c = radeon_get_ib_value(p, idx++) & 0x1F;
+	if (c > 16) {
+	    DRM_ERROR("Only 16 vertex buffers are allowed %d\n",
+		      pkt->opcode);
+	    r100_cs_dump_packet(p, pkt);
+	    return -EINVAL;
+	}
+	track->num_arrays = c;
+	for (i = 0; i < (c - 1); i+=2, idx+=3) {
+		r = r100_cs_packet_next_reloc(p, &reloc);
+		if (r) {
+			DRM_ERROR("No reloc for packet3 %d\n",
+				  pkt->opcode);
+			r100_cs_dump_packet(p, pkt);
+			return r;
+		}
+		idx_value = radeon_get_ib_value(p, idx);
+		ib[idx+1] = radeon_get_ib_value(p, idx + 1) + ((u32)reloc->lobj.gpu_offset);
+
+		track->arrays[i + 0].esize = idx_value >> 8;
+		track->arrays[i + 0].robj = reloc->robj;
+		track->arrays[i + 0].esize &= 0x7F;
+		r = r100_cs_packet_next_reloc(p, &reloc);
+		if (r) {
+			DRM_ERROR("No reloc for packet3 %d\n",
+				  pkt->opcode);
+			r100_cs_dump_packet(p, pkt);
+			return r;
+		}
+		ib[idx+2] = radeon_get_ib_value(p, idx + 2) + ((u32)reloc->lobj.gpu_offset);
+		track->arrays[i + 1].robj = reloc->robj;
+		track->arrays[i + 1].esize = idx_value >> 24;
+		track->arrays[i + 1].esize &= 0x7F;
+	}
+	if (c & 1) {
+		r = r100_cs_packet_next_reloc(p, &reloc);
+		if (r) {
+			DRM_ERROR("No reloc for packet3 %d\n",
+					  pkt->opcode);
+			r100_cs_dump_packet(p, pkt);
+			return r;
+		}
+		idx_value = radeon_get_ib_value(p, idx);
+		ib[idx+1] = radeon_get_ib_value(p, idx + 1) + ((u32)reloc->lobj.gpu_offset);
+		track->arrays[i + 0].robj = reloc->robj;
+		track->arrays[i + 0].esize = idx_value >> 8;
+		track->arrays[i + 0].esize &= 0x7F;
+	}
+	return r;
+}
+
 int r100_cs_parse_packet0(struct radeon_cs_parser *p,
 			  struct radeon_cs_packet *pkt,
 			  const unsigned *auth, unsigned n,
@@ -2048,6 +2152,379 @@ int r100_cs_parse(struct radeon_cs_parser *p)
 	return 0;
 }
 
+static void r100_cs_track_texture_print(struct r100_cs_track_texture *t)
+{
+	DRM_ERROR("pitch                      %d\n", t->pitch);
+	DRM_ERROR("use_pitch                  %d\n", t->use_pitch);
+	DRM_ERROR("width                      %d\n", t->width);
+	DRM_ERROR("width_11                   %d\n", t->width_11);
+	DRM_ERROR("height                     %d\n", t->height);
+	DRM_ERROR("height_11                  %d\n", t->height_11);
+	DRM_ERROR("num levels                 %d\n", t->num_levels);
+	DRM_ERROR("depth                      %d\n", t->txdepth);
+	DRM_ERROR("bpp                        %d\n", t->cpp);
+	DRM_ERROR("coordinate type            %d\n", t->tex_coord_type);
+	DRM_ERROR("width round to power of 2  %d\n", t->roundup_w);
+	DRM_ERROR("height round to power of 2 %d\n", t->roundup_h);
+	DRM_ERROR("compress format            %d\n", t->compress_format);
+}
+
+static int r100_track_compress_size(int compress_format, int w, int h)
+{
+	int block_width, block_height, block_bytes;
+	int wblocks, hblocks;
+	int min_wblocks;
+	int sz;
+
+	block_width = 4;
+	block_height = 4;
+
+	switch (compress_format) {
+	case R100_TRACK_COMP_DXT1:
+		block_bytes = 8;
+		min_wblocks = 4;
+		break;
+	default:
+	case R100_TRACK_COMP_DXT35:
+		block_bytes = 16;
+		min_wblocks = 2;
+		break;
+	}
+
+	hblocks = (h + block_height - 1) / block_height;
+	wblocks = (w + block_width - 1) / block_width;
+	if (wblocks < min_wblocks)
+		wblocks = min_wblocks;
+	sz = wblocks * hblocks * block_bytes;
+	return sz;
+}
+
+static int r100_cs_track_cube(struct radeon_device *rdev,
+			      struct r100_cs_track *track, unsigned idx)
+{
+	unsigned face, w, h;
+	struct radeon_bo *cube_robj;
+	unsigned long size;
+	unsigned compress_format = track->textures[idx].compress_format;
+
+	for (face = 0; face < 5; face++) {
+		cube_robj = track->textures[idx].cube_info[face].robj;
+		w = track->textures[idx].cube_info[face].width;
+		h = track->textures[idx].cube_info[face].height;
+
+		if (compress_format) {
+			size = r100_track_compress_size(compress_format, w, h);
+		} else
+			size = w * h;
+		size *= track->textures[idx].cpp;
+
+		size += track->textures[idx].cube_info[face].offset;
+
+		if (size > radeon_bo_size(cube_robj)) {
+			DRM_ERROR("Cube texture offset greater than object size %lu %lu\n",
+				  size, radeon_bo_size(cube_robj));
+			r100_cs_track_texture_print(&track->textures[idx]);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+static int r100_cs_track_texture_check(struct radeon_device *rdev,
+				       struct r100_cs_track *track)
+{
+	struct radeon_bo *robj;
+	unsigned long size;
+	unsigned u, i, w, h, d;
+	int ret;
+
+	for (u = 0; u < track->num_texture; u++) {
+		if (!track->textures[u].enabled)
+			continue;
+		if (track->textures[u].lookup_disable)
+			continue;
+		robj = track->textures[u].robj;
+		if (robj == NULL) {
+			DRM_ERROR("No texture bound to unit %u\n", u);
+			return -EINVAL;
+		}
+		size = 0;
+		for (i = 0; i <= track->textures[u].num_levels; i++) {
+			if (track->textures[u].use_pitch) {
+				if (rdev->family < CHIP_R300)
+					w = (track->textures[u].pitch / track->textures[u].cpp) / (1 << i);
+				else
+					w = track->textures[u].pitch / (1 << i);
+			} else {
+				w = track->textures[u].width;
+				if (rdev->family >= CHIP_RV515)
+					w |= track->textures[u].width_11;
+				w = w / (1 << i);
+				if (track->textures[u].roundup_w)
+					w = roundup_pow_of_two(w);
+			}
+			h = track->textures[u].height;
+			if (rdev->family >= CHIP_RV515)
+				h |= track->textures[u].height_11;
+			h = h / (1 << i);
+			if (track->textures[u].roundup_h)
+				h = roundup_pow_of_two(h);
+			if (track->textures[u].tex_coord_type == 1) {
+				d = (1 << track->textures[u].txdepth) / (1 << i);
+				if (!d)
+					d = 1;
+			} else {
+				d = 1;
+			}
+			if (track->textures[u].compress_format) {
+
+				size += r100_track_compress_size(track->textures[u].compress_format, w, h) * d;
+				/* compressed textures are block based */
+			} else
+				size += w * h * d;
+		}
+		size *= track->textures[u].cpp;
+
+		switch (track->textures[u].tex_coord_type) {
+		case 0:
+		case 1:
+			break;
+		case 2:
+			if (track->separate_cube) {
+				ret = r100_cs_track_cube(rdev, track, u);
+				if (ret)
+					return ret;
+			} else
+				size *= 6;
+			break;
+		default:
+			DRM_ERROR("Invalid texture coordinate type %u for unit "
+				  "%u\n", track->textures[u].tex_coord_type, u);
+			return -EINVAL;
+		}
+		if (size > radeon_bo_size(robj)) {
+			DRM_ERROR("Texture of unit %u needs %lu bytes but is "
+				  "%lu\n", u, size, radeon_bo_size(robj));
+			r100_cs_track_texture_print(&track->textures[u]);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+int r100_cs_track_check(struct radeon_device *rdev, struct r100_cs_track *track)
+{
+	unsigned i;
+	unsigned long size;
+	unsigned prim_walk;
+	unsigned nverts;
+	unsigned num_cb = track->cb_dirty ? track->num_cb : 0;
+
+	if (num_cb && !track->zb_cb_clear && !track->color_channel_mask &&
+	    !track->blend_read_enable)
+		num_cb = 0;
+
+	for (i = 0; i < num_cb; i++) {
+		if (track->cb[i].robj == NULL) {
+			DRM_ERROR("[drm] No buffer for color buffer %d !\n", i);
+			return -EINVAL;
+		}
+		size = track->cb[i].pitch * track->cb[i].cpp * track->maxy;
+		size += track->cb[i].offset;
+		if (size > radeon_bo_size(track->cb[i].robj)) {
+			DRM_ERROR("[drm] Buffer too small for color buffer %d "
+				  "(need %lu have %lu) !\n", i, size,
+				  radeon_bo_size(track->cb[i].robj));
+			DRM_ERROR("[drm] color buffer %d (%u %u %u %u)\n",
+				  i, track->cb[i].pitch, track->cb[i].cpp,
+				  track->cb[i].offset, track->maxy);
+			return -EINVAL;
+		}
+	}
+	track->cb_dirty = false;
+
+	if (track->zb_dirty && track->z_enabled) {
+		if (track->zb.robj == NULL) {
+			DRM_ERROR("[drm] No buffer for z buffer !\n");
+			return -EINVAL;
+		}
+		size = track->zb.pitch * track->zb.cpp * track->maxy;
+		size += track->zb.offset;
+		if (size > radeon_bo_size(track->zb.robj)) {
+			DRM_ERROR("[drm] Buffer too small for z buffer "
+				  "(need %lu have %lu) !\n", size,
+				  radeon_bo_size(track->zb.robj));
+			DRM_ERROR("[drm] zbuffer (%u %u %u %u)\n",
+				  track->zb.pitch, track->zb.cpp,
+				  track->zb.offset, track->maxy);
+			return -EINVAL;
+		}
+	}
+	track->zb_dirty = false;
+
+	if (track->aa_dirty && track->aaresolve) {
+		if (track->aa.robj == NULL) {
+			DRM_ERROR("[drm] No buffer for AA resolve buffer %d !\n", i);
+			return -EINVAL;
+		}
+		/* I believe the format comes from colorbuffer0. */
+		size = track->aa.pitch * track->cb[0].cpp * track->maxy;
+		size += track->aa.offset;
+		if (size > radeon_bo_size(track->aa.robj)) {
+			DRM_ERROR("[drm] Buffer too small for AA resolve buffer %d "
+				  "(need %lu have %lu) !\n", i, size,
+				  radeon_bo_size(track->aa.robj));
+			DRM_ERROR("[drm] AA resolve buffer %d (%u %u %u %u)\n",
+				  i, track->aa.pitch, track->cb[0].cpp,
+				  track->aa.offset, track->maxy);
+			return -EINVAL;
+		}
+	}
+	track->aa_dirty = false;
+
+	prim_walk = (track->vap_vf_cntl >> 4) & 0x3;
+	if (track->vap_vf_cntl & (1 << 14)) {
+		nverts = track->vap_alt_nverts;
+	} else {
+		nverts = (track->vap_vf_cntl >> 16) & 0xFFFF;
+	}
+	switch (prim_walk) {
+	case 1:
+		for (i = 0; i < track->num_arrays; i++) {
+			size = track->arrays[i].esize * track->max_indx * 4;
+			if (track->arrays[i].robj == NULL) {
+				DRM_ERROR("(PW %u) Vertex array %u no buffer "
+					  "bound\n", prim_walk, i);
+				return -EINVAL;
+			}
+			if (size > radeon_bo_size(track->arrays[i].robj)) {
+				dev_err(rdev->dev, "(PW %u) Vertex array %u "
+					"need %lu dwords have %lu dwords\n",
+					prim_walk, i, size >> 2,
+					radeon_bo_size(track->arrays[i].robj)
+					>> 2);
+				DRM_ERROR("Max indices %u\n", track->max_indx);
+				return -EINVAL;
+			}
+		}
+		break;
+	case 2:
+		for (i = 0; i < track->num_arrays; i++) {
+			size = track->arrays[i].esize * (nverts - 1) * 4;
+			if (track->arrays[i].robj == NULL) {
+				DRM_ERROR("(PW %u) Vertex array %u no buffer "
+					  "bound\n", prim_walk, i);
+				return -EINVAL;
+			}
+			if (size > radeon_bo_size(track->arrays[i].robj)) {
+				dev_err(rdev->dev, "(PW %u) Vertex array %u "
+					"need %lu dwords have %lu dwords\n",
+					prim_walk, i, size >> 2,
+					radeon_bo_size(track->arrays[i].robj)
+					>> 2);
+				return -EINVAL;
+			}
+		}
+		break;
+	case 3:
+		size = track->vtx_size * nverts;
+		if (size != track->immd_dwords) {
+			DRM_ERROR("IMMD draw %u dwors but needs %lu dwords\n",
+				  track->immd_dwords, size);
+			DRM_ERROR("VAP_VF_CNTL.NUM_VERTICES %u, VTX_SIZE %u\n",
+				  nverts, track->vtx_size);
+			return -EINVAL;
+		}
+		break;
+	default:
+		DRM_ERROR("[drm] Invalid primitive walk %d for VAP_VF_CNTL\n",
+			  prim_walk);
+		return -EINVAL;
+	}
+
+	if (track->tex_dirty) {
+		track->tex_dirty = false;
+		return r100_cs_track_texture_check(rdev, track);
+	}
+	return 0;
+}
+
+void r100_cs_track_clear(struct radeon_device *rdev, struct r100_cs_track *track)
+{
+	unsigned i, face;
+
+	track->cb_dirty = true;
+	track->zb_dirty = true;
+	track->tex_dirty = true;
+	track->aa_dirty = true;
+
+	if (rdev->family < CHIP_R300) {
+		track->num_cb = 1;
+		if (rdev->family <= CHIP_RS200)
+			track->num_texture = 3;
+		else
+			track->num_texture = 6;
+		track->maxy = 2048;
+		track->separate_cube = 1;
+	} else {
+		track->num_cb = 4;
+		track->num_texture = 16;
+		track->maxy = 4096;
+		track->separate_cube = 0;
+		track->aaresolve = false;
+		track->aa.robj = NULL;
+	}
+
+	for (i = 0; i < track->num_cb; i++) {
+		track->cb[i].robj = NULL;
+		track->cb[i].pitch = 8192;
+		track->cb[i].cpp = 16;
+		track->cb[i].offset = 0;
+	}
+	track->z_enabled = true;
+	track->zb.robj = NULL;
+	track->zb.pitch = 8192;
+	track->zb.cpp = 4;
+	track->zb.offset = 0;
+	track->vtx_size = 0x7F;
+	track->immd_dwords = 0xFFFFFFFFUL;
+	track->num_arrays = 11;
+	track->max_indx = 0x00FFFFFFUL;
+	for (i = 0; i < track->num_arrays; i++) {
+		track->arrays[i].robj = NULL;
+		track->arrays[i].esize = 0x7F;
+	}
+	for (i = 0; i < track->num_texture; i++) {
+		track->textures[i].compress_format = R100_TRACK_COMP_NONE;
+		track->textures[i].pitch = 16536;
+		track->textures[i].width = 16536;
+		track->textures[i].height = 16536;
+		track->textures[i].width_11 = 1 << 11;
+		track->textures[i].height_11 = 1 << 11;
+		track->textures[i].num_levels = 12;
+		if (rdev->family <= CHIP_RS200) {
+			track->textures[i].tex_coord_type = 0;
+			track->textures[i].txdepth = 0;
+		} else {
+			track->textures[i].txdepth = 16;
+			track->textures[i].tex_coord_type = 1;
+		}
+		track->textures[i].cpp = 64;
+		track->textures[i].robj = NULL;
+		/* CS IB emission code makes sure texture unit are disabled */
+		track->textures[i].enabled = false;
+		track->textures[i].lookup_disable = false;
+		track->textures[i].roundup_w = true;
+		track->textures[i].roundup_h = true;
+		if (track->separate_cube)
+			for (face = 0; face < 5; face++) {
+				track->textures[i].cube_info[face].robj = NULL;
+				track->textures[i].cube_info[face].width = 16536;
+				track->textures[i].cube_info[face].height = 16536;
+				track->textures[i].cube_info[face].offset = 0;
+			}
+	}
+}
 
 /*
  * Global GPU functions
@@ -2175,6 +2652,15 @@ bool r100_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 	return radeon_ring_test_lockup(rdev, ring);
 }
 
+/* required on r1xx, r2xx, r300, r(v)350, r420/r481, rs400/rs480 */
+void r100_enable_bm(struct radeon_device *rdev)
+{
+	uint32_t tmp;
+	/* Enable bus mastering */
+	tmp = RREG32(RADEON_BUS_CNTL) & ~RADEON_BUS_MASTER_DIS;
+	WREG32(RADEON_BUS_CNTL, tmp);
+}
+
 void r100_bm_disable(struct radeon_device *rdev)
 {
 	u32 tmp;
@@ -3261,380 +3747,6 @@ void r100_bandwidth_update(struct radeon_device *rdev)
 	}
 }
 
-static void r100_cs_track_texture_print(struct r100_cs_track_texture *t)
-{
-	DRM_ERROR("pitch                      %d\n", t->pitch);
-	DRM_ERROR("use_pitch                  %d\n", t->use_pitch);
-	DRM_ERROR("width                      %d\n", t->width);
-	DRM_ERROR("width_11                   %d\n", t->width_11);
-	DRM_ERROR("height                     %d\n", t->height);
-	DRM_ERROR("height_11                  %d\n", t->height_11);
-	DRM_ERROR("num levels                 %d\n", t->num_levels);
-	DRM_ERROR("depth                      %d\n", t->txdepth);
-	DRM_ERROR("bpp                        %d\n", t->cpp);
-	DRM_ERROR("coordinate type            %d\n", t->tex_coord_type);
-	DRM_ERROR("width round to power of 2  %d\n", t->roundup_w);
-	DRM_ERROR("height round to power of 2 %d\n", t->roundup_h);
-	DRM_ERROR("compress format            %d\n", t->compress_format);
-}
-
-static int r100_track_compress_size(int compress_format, int w, int h)
-{
-	int block_width, block_height, block_bytes;
-	int wblocks, hblocks;
-	int min_wblocks;
-	int sz;
-
-	block_width = 4;
-	block_height = 4;
-
-	switch (compress_format) {
-	case R100_TRACK_COMP_DXT1:
-		block_bytes = 8;
-		min_wblocks = 4;
-		break;
-	default:
-	case R100_TRACK_COMP_DXT35:
-		block_bytes = 16;
-		min_wblocks = 2;
-		break;
-	}
-
-	hblocks = (h + block_height - 1) / block_height;
-	wblocks = (w + block_width - 1) / block_width;
-	if (wblocks < min_wblocks)
-		wblocks = min_wblocks;
-	sz = wblocks * hblocks * block_bytes;
-	return sz;
-}
-
-static int r100_cs_track_cube(struct radeon_device *rdev,
-			      struct r100_cs_track *track, unsigned idx)
-{
-	unsigned face, w, h;
-	struct radeon_bo *cube_robj;
-	unsigned long size;
-	unsigned compress_format = track->textures[idx].compress_format;
-
-	for (face = 0; face < 5; face++) {
-		cube_robj = track->textures[idx].cube_info[face].robj;
-		w = track->textures[idx].cube_info[face].width;
-		h = track->textures[idx].cube_info[face].height;
-
-		if (compress_format) {
-			size = r100_track_compress_size(compress_format, w, h);
-		} else
-			size = w * h;
-		size *= track->textures[idx].cpp;
-
-		size += track->textures[idx].cube_info[face].offset;
-
-		if (size > radeon_bo_size(cube_robj)) {
-			DRM_ERROR("Cube texture offset greater than object size %lu %lu\n",
-				  size, radeon_bo_size(cube_robj));
-			r100_cs_track_texture_print(&track->textures[idx]);
-			return -1;
-		}
-	}
-	return 0;
-}
-
-static int r100_cs_track_texture_check(struct radeon_device *rdev,
-				       struct r100_cs_track *track)
-{
-	struct radeon_bo *robj;
-	unsigned long size;
-	unsigned u, i, w, h, d;
-	int ret;
-
-	for (u = 0; u < track->num_texture; u++) {
-		if (!track->textures[u].enabled)
-			continue;
-		if (track->textures[u].lookup_disable)
-			continue;
-		robj = track->textures[u].robj;
-		if (robj == NULL) {
-			DRM_ERROR("No texture bound to unit %u\n", u);
-			return -EINVAL;
-		}
-		size = 0;
-		for (i = 0; i <= track->textures[u].num_levels; i++) {
-			if (track->textures[u].use_pitch) {
-				if (rdev->family < CHIP_R300)
-					w = (track->textures[u].pitch / track->textures[u].cpp) / (1 << i);
-				else
-					w = track->textures[u].pitch / (1 << i);
-			} else {
-				w = track->textures[u].width;
-				if (rdev->family >= CHIP_RV515)
-					w |= track->textures[u].width_11;
-				w = w / (1 << i);
-				if (track->textures[u].roundup_w)
-					w = roundup_pow_of_two(w);
-			}
-			h = track->textures[u].height;
-			if (rdev->family >= CHIP_RV515)
-				h |= track->textures[u].height_11;
-			h = h / (1 << i);
-			if (track->textures[u].roundup_h)
-				h = roundup_pow_of_two(h);
-			if (track->textures[u].tex_coord_type == 1) {
-				d = (1 << track->textures[u].txdepth) / (1 << i);
-				if (!d)
-					d = 1;
-			} else {
-				d = 1;
-			}
-			if (track->textures[u].compress_format) {
-
-				size += r100_track_compress_size(track->textures[u].compress_format, w, h) * d;
-				/* compressed textures are block based */
-			} else
-				size += w * h * d;
-		}
-		size *= track->textures[u].cpp;
-
-		switch (track->textures[u].tex_coord_type) {
-		case 0:
-		case 1:
-			break;
-		case 2:
-			if (track->separate_cube) {
-				ret = r100_cs_track_cube(rdev, track, u);
-				if (ret)
-					return ret;
-			} else
-				size *= 6;
-			break;
-		default:
-			DRM_ERROR("Invalid texture coordinate type %u for unit "
-				  "%u\n", track->textures[u].tex_coord_type, u);
-			return -EINVAL;
-		}
-		if (size > radeon_bo_size(robj)) {
-			DRM_ERROR("Texture of unit %u needs %lu bytes but is "
-				  "%lu\n", u, size, radeon_bo_size(robj));
-			r100_cs_track_texture_print(&track->textures[u]);
-			return -EINVAL;
-		}
-	}
-	return 0;
-}
-
-int r100_cs_track_check(struct radeon_device *rdev, struct r100_cs_track *track)
-{
-	unsigned i;
-	unsigned long size;
-	unsigned prim_walk;
-	unsigned nverts;
-	unsigned num_cb = track->cb_dirty ? track->num_cb : 0;
-
-	if (num_cb && !track->zb_cb_clear && !track->color_channel_mask &&
-	    !track->blend_read_enable)
-		num_cb = 0;
-
-	for (i = 0; i < num_cb; i++) {
-		if (track->cb[i].robj == NULL) {
-			DRM_ERROR("[drm] No buffer for color buffer %d !\n", i);
-			return -EINVAL;
-		}
-		size = track->cb[i].pitch * track->cb[i].cpp * track->maxy;
-		size += track->cb[i].offset;
-		if (size > radeon_bo_size(track->cb[i].robj)) {
-			DRM_ERROR("[drm] Buffer too small for color buffer %d "
-				  "(need %lu have %lu) !\n", i, size,
-				  radeon_bo_size(track->cb[i].robj));
-			DRM_ERROR("[drm] color buffer %d (%u %u %u %u)\n",
-				  i, track->cb[i].pitch, track->cb[i].cpp,
-				  track->cb[i].offset, track->maxy);
-			return -EINVAL;
-		}
-	}
-	track->cb_dirty = false;
-
-	if (track->zb_dirty && track->z_enabled) {
-		if (track->zb.robj == NULL) {
-			DRM_ERROR("[drm] No buffer for z buffer !\n");
-			return -EINVAL;
-		}
-		size = track->zb.pitch * track->zb.cpp * track->maxy;
-		size += track->zb.offset;
-		if (size > radeon_bo_size(track->zb.robj)) {
-			DRM_ERROR("[drm] Buffer too small for z buffer "
-				  "(need %lu have %lu) !\n", size,
-				  radeon_bo_size(track->zb.robj));
-			DRM_ERROR("[drm] zbuffer (%u %u %u %u)\n",
-				  track->zb.pitch, track->zb.cpp,
-				  track->zb.offset, track->maxy);
-			return -EINVAL;
-		}
-	}
-	track->zb_dirty = false;
-
-	if (track->aa_dirty && track->aaresolve) {
-		if (track->aa.robj == NULL) {
-			DRM_ERROR("[drm] No buffer for AA resolve buffer %d !\n", i);
-			return -EINVAL;
-		}
-		/* I believe the format comes from colorbuffer0. */
-		size = track->aa.pitch * track->cb[0].cpp * track->maxy;
-		size += track->aa.offset;
-		if (size > radeon_bo_size(track->aa.robj)) {
-			DRM_ERROR("[drm] Buffer too small for AA resolve buffer %d "
-				  "(need %lu have %lu) !\n", i, size,
-				  radeon_bo_size(track->aa.robj));
-			DRM_ERROR("[drm] AA resolve buffer %d (%u %u %u %u)\n",
-				  i, track->aa.pitch, track->cb[0].cpp,
-				  track->aa.offset, track->maxy);
-			return -EINVAL;
-		}
-	}
-	track->aa_dirty = false;
-
-	prim_walk = (track->vap_vf_cntl >> 4) & 0x3;
-	if (track->vap_vf_cntl & (1 << 14)) {
-		nverts = track->vap_alt_nverts;
-	} else {
-		nverts = (track->vap_vf_cntl >> 16) & 0xFFFF;
-	}
-	switch (prim_walk) {
-	case 1:
-		for (i = 0; i < track->num_arrays; i++) {
-			size = track->arrays[i].esize * track->max_indx * 4;
-			if (track->arrays[i].robj == NULL) {
-				DRM_ERROR("(PW %u) Vertex array %u no buffer "
-					  "bound\n", prim_walk, i);
-				return -EINVAL;
-			}
-			if (size > radeon_bo_size(track->arrays[i].robj)) {
-				dev_err(rdev->dev, "(PW %u) Vertex array %u "
-					"need %lu dwords have %lu dwords\n",
-					prim_walk, i, size >> 2,
-					radeon_bo_size(track->arrays[i].robj)
-					>> 2);
-				DRM_ERROR("Max indices %u\n", track->max_indx);
-				return -EINVAL;
-			}
-		}
-		break;
-	case 2:
-		for (i = 0; i < track->num_arrays; i++) {
-			size = track->arrays[i].esize * (nverts - 1) * 4;
-			if (track->arrays[i].robj == NULL) {
-				DRM_ERROR("(PW %u) Vertex array %u no buffer "
-					  "bound\n", prim_walk, i);
-				return -EINVAL;
-			}
-			if (size > radeon_bo_size(track->arrays[i].robj)) {
-				dev_err(rdev->dev, "(PW %u) Vertex array %u "
-					"need %lu dwords have %lu dwords\n",
-					prim_walk, i, size >> 2,
-					radeon_bo_size(track->arrays[i].robj)
-					>> 2);
-				return -EINVAL;
-			}
-		}
-		break;
-	case 3:
-		size = track->vtx_size * nverts;
-		if (size != track->immd_dwords) {
-			DRM_ERROR("IMMD draw %u dwors but needs %lu dwords\n",
-				  track->immd_dwords, size);
-			DRM_ERROR("VAP_VF_CNTL.NUM_VERTICES %u, VTX_SIZE %u\n",
-				  nverts, track->vtx_size);
-			return -EINVAL;
-		}
-		break;
-	default:
-		DRM_ERROR("[drm] Invalid primitive walk %d for VAP_VF_CNTL\n",
-			  prim_walk);
-		return -EINVAL;
-	}
-
-	if (track->tex_dirty) {
-		track->tex_dirty = false;
-		return r100_cs_track_texture_check(rdev, track);
-	}
-	return 0;
-}
-
-void r100_cs_track_clear(struct radeon_device *rdev, struct r100_cs_track *track)
-{
-	unsigned i, face;
-
-	track->cb_dirty = true;
-	track->zb_dirty = true;
-	track->tex_dirty = true;
-	track->aa_dirty = true;
-
-	if (rdev->family < CHIP_R300) {
-		track->num_cb = 1;
-		if (rdev->family <= CHIP_RS200)
-			track->num_texture = 3;
-		else
-			track->num_texture = 6;
-		track->maxy = 2048;
-		track->separate_cube = 1;
-	} else {
-		track->num_cb = 4;
-		track->num_texture = 16;
-		track->maxy = 4096;
-		track->separate_cube = 0;
-		track->aaresolve = false;
-		track->aa.robj = NULL;
-	}
-
-	for (i = 0; i < track->num_cb; i++) {
-		track->cb[i].robj = NULL;
-		track->cb[i].pitch = 8192;
-		track->cb[i].cpp = 16;
-		track->cb[i].offset = 0;
-	}
-	track->z_enabled = true;
-	track->zb.robj = NULL;
-	track->zb.pitch = 8192;
-	track->zb.cpp = 4;
-	track->zb.offset = 0;
-	track->vtx_size = 0x7F;
-	track->immd_dwords = 0xFFFFFFFFUL;
-	track->num_arrays = 11;
-	track->max_indx = 0x00FFFFFFUL;
-	for (i = 0; i < track->num_arrays; i++) {
-		track->arrays[i].robj = NULL;
-		track->arrays[i].esize = 0x7F;
-	}
-	for (i = 0; i < track->num_texture; i++) {
-		track->textures[i].compress_format = R100_TRACK_COMP_NONE;
-		track->textures[i].pitch = 16536;
-		track->textures[i].width = 16536;
-		track->textures[i].height = 16536;
-		track->textures[i].width_11 = 1 << 11;
-		track->textures[i].height_11 = 1 << 11;
-		track->textures[i].num_levels = 12;
-		if (rdev->family <= CHIP_RS200) {
-			track->textures[i].tex_coord_type = 0;
-			track->textures[i].txdepth = 0;
-		} else {
-			track->textures[i].txdepth = 16;
-			track->textures[i].tex_coord_type = 1;
-		}
-		track->textures[i].cpp = 64;
-		track->textures[i].robj = NULL;
-		/* CS IB emission code makes sure texture unit are disabled */
-		track->textures[i].enabled = false;
-		track->textures[i].lookup_disable = false;
-		track->textures[i].roundup_w = true;
-		track->textures[i].roundup_h = true;
-		if (track->separate_cube)
-			for (face = 0; face < 5; face++) {
-				track->textures[i].cube_info[face].robj = NULL;
-				track->textures[i].cube_info[face].width = 16536;
-				track->textures[i].cube_info[face].height = 16536;
-				track->textures[i].cube_info[face].offset = 0;
-			}
-	}
-}
-
 int r100_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	uint32_t scratch;
@@ -3679,6 +3791,12 @@ void r100_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 
+	if (ring->rptr_save_reg) {
+		u32 next_rptr = ring->wptr + 2 + 3;
+		radeon_ring_write(ring, PACKET0(ring->rptr_save_reg, 0));
+		radeon_ring_write(ring, next_rptr);
+	}
+
 	radeon_ring_write(ring, PACKET0(RADEON_CP_IB_BASE, 1));
 	radeon_ring_write(ring, ib->gpu_addr);
 	radeon_ring_write(ring, ib->length_dw);
@@ -3711,7 +3829,7 @@ int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[6] = PACKET2(0);
 	ib.ptr[7] = PACKET2(0);
 	ib.length_dw = 8;
-	r = radeon_ib_schedule(rdev, &ib);
+	r = radeon_ib_schedule(rdev, &ib, NULL);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
@@ -3740,12 +3858,6 @@ int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	return r;
 }
 
-void r100_ib_fini(struct radeon_device *rdev)
-{
-	radeon_ib_pool_suspend(rdev);
-	radeon_ib_pool_fini(rdev);
-}
-
 void r100_mc_stop(struct radeon_device *rdev, struct r100_mc_save *save)
 {
 	/* Shutdown CP we shouldn't need to do that but better be safe than
@@ -3905,13 +4017,11 @@ static int r100_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	return 0;
 }
@@ -3948,7 +4058,6 @@ int r100_resume(struct radeon_device *rdev)
 
 int r100_suspend(struct radeon_device *rdev)
 {
-	radeon_ib_pool_suspend(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
 	r100_irq_disable(rdev);
@@ -3961,7 +4070,7 @@ void r100_fini(struct radeon_device *rdev)
 {
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	if (rdev->flags & RADEON_IS_PCI)
 		r100_pci_gart_fini(rdev);
@@ -4068,20 +4177,14 @@ int r100_init(struct radeon_device *rdev)
 	}
 	r100_set_safe_registers(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = r100_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		if (rdev->flags & RADEON_IS_PCI)
 			r100_pci_gart_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r200.c b/drivers/gpu/drm/radeon/r200.c
index a26144d0120..f0889259eb0 100644
--- a/drivers/gpu/drm/radeon/r200.c
+++ b/drivers/gpu/drm/radeon/r200.c
@@ -85,7 +85,7 @@ int r200_copy_dma(struct radeon_device *rdev,
 		  uint64_t src_offset,
 		  uint64_t dst_offset,
 		  unsigned num_gpu_pages,
-		  struct radeon_fence *fence)
+		  struct radeon_fence **fence)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	uint32_t size;
@@ -120,7 +120,7 @@ int r200_copy_dma(struct radeon_device *rdev,
 	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
 	radeon_ring_write(ring, RADEON_WAIT_DMA_GUI_IDLE);
 	if (fence) {
-		r = radeon_fence_emit(rdev, fence);
+		r = radeon_fence_emit(rdev, fence, RADEON_RING_TYPE_GFX_INDEX);
 	}
 	radeon_ring_unlock_commit(rdev, ring);
 	return r;
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index 97722a33e51..646a1927dda 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -1391,13 +1391,11 @@ static int r300_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	return 0;
 }
@@ -1436,7 +1434,6 @@ int r300_resume(struct radeon_device *rdev)
 
 int r300_suspend(struct radeon_device *rdev)
 {
-	radeon_ib_pool_suspend(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
 	r100_irq_disable(rdev);
@@ -1451,7 +1448,7 @@ void r300_fini(struct radeon_device *rdev)
 {
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	if (rdev->flags & RADEON_IS_PCIE)
 		rv370_pcie_gart_fini(rdev);
@@ -1538,20 +1535,14 @@ int r300_init(struct radeon_device *rdev)
 	}
 	r300_set_reg_safe(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = r300_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		if (rdev->flags & RADEON_IS_PCIE)
 			rv370_pcie_gart_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index 99137be7a30..f2f5bf6d339 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -275,13 +275,11 @@ static int r420_startup(struct radeon_device *rdev)
 	}
 	r420_cp_errata_init(rdev);
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	return 0;
 }
@@ -324,7 +322,6 @@ int r420_resume(struct radeon_device *rdev)
 
 int r420_suspend(struct radeon_device *rdev)
 {
-	radeon_ib_pool_suspend(rdev);
 	r420_cp_errata_fini(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
@@ -340,7 +337,7 @@ void r420_fini(struct radeon_device *rdev)
 {
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	if (rdev->flags & RADEON_IS_PCIE)
 		rv370_pcie_gart_fini(rdev);
@@ -438,20 +435,14 @@ int r420_init(struct radeon_device *rdev)
 	}
 	r420_set_reg_safe(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = r420_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		if (rdev->flags & RADEON_IS_PCIE)
 			rv370_pcie_gart_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r520.c b/drivers/gpu/drm/radeon/r520.c
index b5cf8375cd2..079d3c52c08 100644
--- a/drivers/gpu/drm/radeon/r520.c
+++ b/drivers/gpu/drm/radeon/r520.c
@@ -203,13 +203,11 @@ static int r520_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	return 0;
 }
@@ -311,20 +309,14 @@ int r520_init(struct radeon_device *rdev)
 		return r;
 	rv515_set_safe_registers(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = r520_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rv370_pcie_gart_fini(rdev);
 		radeon_agp_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index bff62729381..637280f541a 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -709,6 +709,7 @@ void r600_hpd_init(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned enable = 0;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
@@ -729,28 +730,22 @@ void r600_hpd_init(struct radeon_device *rdev)
 			switch (radeon_connector->hpd.hpd) {
 			case RADEON_HPD_1:
 				WREG32(DC_HPD1_CONTROL, tmp);
-				rdev->irq.hpd[0] = true;
 				break;
 			case RADEON_HPD_2:
 				WREG32(DC_HPD2_CONTROL, tmp);
-				rdev->irq.hpd[1] = true;
 				break;
 			case RADEON_HPD_3:
 				WREG32(DC_HPD3_CONTROL, tmp);
-				rdev->irq.hpd[2] = true;
 				break;
 			case RADEON_HPD_4:
 				WREG32(DC_HPD4_CONTROL, tmp);
-				rdev->irq.hpd[3] = true;
 				break;
 				/* DCE 3.2 */
 			case RADEON_HPD_5:
 				WREG32(DC_HPD5_CONTROL, tmp);
-				rdev->irq.hpd[4] = true;
 				break;
 			case RADEON_HPD_6:
 				WREG32(DC_HPD6_CONTROL, tmp);
-				rdev->irq.hpd[5] = true;
 				break;
 			default:
 				break;
@@ -759,85 +754,73 @@ void r600_hpd_init(struct radeon_device *rdev)
 			switch (radeon_connector->hpd.hpd) {
 			case RADEON_HPD_1:
 				WREG32(DC_HOT_PLUG_DETECT1_CONTROL, DC_HOT_PLUG_DETECTx_EN);
-				rdev->irq.hpd[0] = true;
 				break;
 			case RADEON_HPD_2:
 				WREG32(DC_HOT_PLUG_DETECT2_CONTROL, DC_HOT_PLUG_DETECTx_EN);
-				rdev->irq.hpd[1] = true;
 				break;
 			case RADEON_HPD_3:
 				WREG32(DC_HOT_PLUG_DETECT3_CONTROL, DC_HOT_PLUG_DETECTx_EN);
-				rdev->irq.hpd[2] = true;
 				break;
 			default:
 				break;
 			}
 		}
+		enable |= 1 << radeon_connector->hpd.hpd;
 		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
-	if (rdev->irq.installed)
-		r600_irq_set(rdev);
+	radeon_irq_kms_enable_hpd(rdev, enable);
 }
 
 void r600_hpd_fini(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned disable = 0;
 
-	if (ASIC_IS_DCE3(rdev)) {
-		list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-			struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+		if (ASIC_IS_DCE3(rdev)) {
 			switch (radeon_connector->hpd.hpd) {
 			case RADEON_HPD_1:
 				WREG32(DC_HPD1_CONTROL, 0);
-				rdev->irq.hpd[0] = false;
 				break;
 			case RADEON_HPD_2:
 				WREG32(DC_HPD2_CONTROL, 0);
-				rdev->irq.hpd[1] = false;
 				break;
 			case RADEON_HPD_3:
 				WREG32(DC_HPD3_CONTROL, 0);
-				rdev->irq.hpd[2] = false;
 				break;
 			case RADEON_HPD_4:
 				WREG32(DC_HPD4_CONTROL, 0);
-				rdev->irq.hpd[3] = false;
 				break;
 				/* DCE 3.2 */
 			case RADEON_HPD_5:
 				WREG32(DC_HPD5_CONTROL, 0);
-				rdev->irq.hpd[4] = false;
 				break;
 			case RADEON_HPD_6:
 				WREG32(DC_HPD6_CONTROL, 0);
-				rdev->irq.hpd[5] = false;
 				break;
 			default:
 				break;
 			}
-		}
-	} else {
-		list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-			struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+		} else {
 			switch (radeon_connector->hpd.hpd) {
 			case RADEON_HPD_1:
 				WREG32(DC_HOT_PLUG_DETECT1_CONTROL, 0);
-				rdev->irq.hpd[0] = false;
 				break;
 			case RADEON_HPD_2:
 				WREG32(DC_HOT_PLUG_DETECT2_CONTROL, 0);
-				rdev->irq.hpd[1] = false;
 				break;
 			case RADEON_HPD_3:
 				WREG32(DC_HOT_PLUG_DETECT3_CONTROL, 0);
-				rdev->irq.hpd[2] = false;
 				break;
 			default:
 				break;
 			}
 		}
+		disable |= 1 << radeon_connector->hpd.hpd;
 	}
+	radeon_irq_kms_disable_hpd(rdev, disable);
 }
 
 /*
@@ -1306,6 +1289,14 @@ int r600_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(R_008014_GRBM_STATUS2));
 	dev_info(rdev->dev, "  R_000E50_SRBM_STATUS=0x%08X\n",
 		RREG32(R_000E50_SRBM_STATUS));
+	dev_info(rdev->dev, "  R_008674_CP_STALLED_STAT1 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT1));
+	dev_info(rdev->dev, "  R_008678_CP_STALLED_STAT2 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT2));
+	dev_info(rdev->dev, "  R_00867C_CP_BUSY_STAT     = 0x%08X\n",
+		RREG32(CP_BUSY_STAT));
+	dev_info(rdev->dev, "  R_008680_CP_STAT          = 0x%08X\n",
+		RREG32(CP_STAT));
 	rv515_mc_stop(rdev, &save);
 	if (r600_mc_wait_for_idle(rdev)) {
 		dev_warn(rdev->dev, "Wait for MC idle timedout !\n");
@@ -1349,6 +1340,14 @@ int r600_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(R_008014_GRBM_STATUS2));
 	dev_info(rdev->dev, "  R_000E50_SRBM_STATUS=0x%08X\n",
 		RREG32(R_000E50_SRBM_STATUS));
+	dev_info(rdev->dev, "  R_008674_CP_STALLED_STAT1 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT1));
+	dev_info(rdev->dev, "  R_008678_CP_STALLED_STAT2 = 0x%08X\n",
+		RREG32(CP_STALLED_STAT2));
+	dev_info(rdev->dev, "  R_00867C_CP_BUSY_STAT     = 0x%08X\n",
+		RREG32(CP_BUSY_STAT));
+	dev_info(rdev->dev, "  R_008680_CP_STAT          = 0x%08X\n",
+		RREG32(CP_STAT));
 	rv515_mc_resume(rdev, &save);
 	return 0;
 }
@@ -2172,18 +2171,29 @@ int r600_cp_resume(struct radeon_device *rdev)
 void r600_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ring_size)
 {
 	u32 rb_bufsz;
+	int r;
 
 	/* Align ring size */
 	rb_bufsz = drm_order(ring_size / 8);
 	ring_size = (1 << (rb_bufsz + 1)) * 4;
 	ring->ring_size = ring_size;
 	ring->align_mask = 16 - 1;
+
+	if (radeon_ring_supports_scratch_reg(rdev, ring)) {
+		r = radeon_scratch_get(rdev, &ring->rptr_save_reg);
+		if (r) {
+			DRM_ERROR("failed to get scratch reg for rptr save (%d).\n", r);
+			ring->rptr_save_reg = 0;
+		}
+	}
 }
 
 void r600_cp_fini(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	r600_cp_stop(rdev);
-	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
+	radeon_ring_fini(rdev, ring);
+	radeon_scratch_free(rdev, ring->rptr_save_reg);
 }
 
 
@@ -2206,7 +2216,7 @@ int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	uint32_t scratch;
 	uint32_t tmp = 0;
-	unsigned i, ridx = radeon_ring_index(rdev, ring);
+	unsigned i;
 	int r;
 
 	r = radeon_scratch_get(rdev, &scratch);
@@ -2217,7 +2227,7 @@ int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	WREG32(scratch, 0xCAFEDEAD);
 	r = radeon_ring_lock(rdev, ring, 3);
 	if (r) {
-		DRM_ERROR("radeon: cp failed to lock ring %d (%d).\n", ridx, r);
+		DRM_ERROR("radeon: cp failed to lock ring %d (%d).\n", ring->idx, r);
 		radeon_scratch_free(rdev, scratch);
 		return r;
 	}
@@ -2232,10 +2242,10 @@ int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		DRM_UDELAY(1);
 	}
 	if (i < rdev->usec_timeout) {
-		DRM_INFO("ring test on %d succeeded in %d usecs\n", ridx, i);
+		DRM_INFO("ring test on %d succeeded in %d usecs\n", ring->idx, i);
 	} else {
 		DRM_ERROR("radeon: ring %d test failed (scratch(0x%04X)=0x%08X)\n",
-			  ridx, scratch, tmp);
+			  ring->idx, scratch, tmp);
 		r = -EINVAL;
 	}
 	radeon_scratch_free(rdev, scratch);
@@ -2309,34 +2319,21 @@ int r600_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset,
 		   uint64_t dst_offset,
 		   unsigned num_gpu_pages,
-		   struct radeon_fence *fence)
+		   struct radeon_fence **fence)
 {
+	struct radeon_semaphore *sem = NULL;
 	struct radeon_sa_bo *vb = NULL;
 	int r;
 
-	r = r600_blit_prepare_copy(rdev, num_gpu_pages, &vb);
+	r = r600_blit_prepare_copy(rdev, num_gpu_pages, fence, &vb, &sem);
 	if (r) {
 		return r;
 	}
 	r600_kms_blit_copy(rdev, src_offset, dst_offset, num_gpu_pages, vb);
-	r600_blit_done_copy(rdev, fence, vb);
+	r600_blit_done_copy(rdev, fence, vb, sem);
 	return 0;
 }
 
-void r600_blit_suspend(struct radeon_device *rdev)
-{
-	int r;
-
-	/* unpin shaders bo */
-	if (rdev->r600_blit.shader_obj) {
-		r = radeon_bo_reserve(rdev->r600_blit.shader_obj, false);
-		if (!r) {
-			radeon_bo_unpin(rdev->r600_blit.shader_obj);
-			radeon_bo_unreserve(rdev->r600_blit.shader_obj);
-		}
-	}
-}
-
 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)
@@ -2419,13 +2416,11 @@ int r600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -2475,9 +2470,6 @@ int r600_resume(struct radeon_device *rdev)
 int r600_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
-	radeon_ib_pool_suspend(rdev);
-	r600_blit_suspend(rdev);
-	/* FIXME: we should wait for ring to be empty */
 	r600_cp_stop(rdev);
 	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	r600_irq_suspend(rdev);
@@ -2559,20 +2551,14 @@ int r600_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = r600_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		r600_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		r600_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
@@ -2588,7 +2574,7 @@ void r600_fini(struct radeon_device *rdev)
 	r600_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	r600_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
@@ -2607,9 +2593,24 @@ void r600_fini(struct radeon_device *rdev)
  */
 void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	u32 next_rptr;
+
+	if (ring->rptr_save_reg) {
+		next_rptr = ring->wptr + 3 + 4;
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, ((ring->rptr_save_reg -
+					 PACKET3_SET_CONFIG_REG_OFFSET) >> 2));
+		radeon_ring_write(ring, next_rptr);
+	} else if (rdev->wb.enabled) {
+		next_rptr = ring->wptr + 5 + 4;
+		radeon_ring_write(ring, PACKET3(PACKET3_MEM_WRITE, 3));
+		radeon_ring_write(ring, ring->next_rptr_gpu_addr & 0xfffffffc);
+		radeon_ring_write(ring, (upper_32_bits(ring->next_rptr_gpu_addr) & 0xff) | (1 << 18));
+		radeon_ring_write(ring, next_rptr);
+		radeon_ring_write(ring, 0);
+	}
 
-	/* FIXME: implement */
 	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
 	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
@@ -2627,7 +2628,6 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	uint32_t tmp = 0;
 	unsigned i;
 	int r;
-	int ring_index = radeon_ring_index(rdev, ring);
 
 	r = radeon_scratch_get(rdev, &scratch);
 	if (r) {
@@ -2635,7 +2635,7 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		return r;
 	}
 	WREG32(scratch, 0xCAFEDEAD);
-	r = radeon_ib_get(rdev, ring_index, &ib, 256);
+	r = radeon_ib_get(rdev, ring->idx, &ib, 256);
 	if (r) {
 		DRM_ERROR("radeon: failed to get ib (%d).\n", r);
 		return r;
@@ -2644,7 +2644,7 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[1] = ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
 	ib.ptr[2] = 0xDEADBEEF;
 	ib.length_dw = 3;
-	r = radeon_ib_schedule(rdev, &ib);
+	r = radeon_ib_schedule(rdev, &ib, NULL);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
@@ -2857,7 +2857,6 @@ void r600_disable_interrupts(struct radeon_device *rdev)
 	WREG32(IH_RB_RPTR, 0);
 	WREG32(IH_RB_WPTR, 0);
 	rdev->ih.enabled = false;
-	rdev->ih.wptr = 0;
 	rdev->ih.rptr = 0;
 }
 
@@ -3042,18 +3041,18 @@ int r600_irq_set(struct radeon_device *rdev)
 		hdmi1 = RREG32(HDMI1_AUDIO_PACKET_CONTROL) & ~HDMI0_AZ_FORMAT_WTRIG_MASK;
 	}
 
-	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+	if (atomic_read(&rdev->irq.ring_int[RADEON_RING_TYPE_GFX_INDEX])) {
 		DRM_DEBUG("r600_irq_set: sw int\n");
 		cp_int_cntl |= RB_INT_ENABLE;
 		cp_int_cntl |= TIME_STAMP_INT_ENABLE;
 	}
 	if (rdev->irq.crtc_vblank_int[0] ||
-	    rdev->irq.pflip[0]) {
+	    atomic_read(&rdev->irq.pflip[0])) {
 		DRM_DEBUG("r600_irq_set: vblank 0\n");
 		mode_int |= D1MODE_VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[1] ||
-	    rdev->irq.pflip[1]) {
+	    atomic_read(&rdev->irq.pflip[1])) {
 		DRM_DEBUG("r600_irq_set: vblank 1\n");
 		mode_int |= D2MODE_VBLANK_INT_MASK;
 	}
@@ -3309,7 +3308,6 @@ int r600_irq_process(struct radeon_device *rdev)
 	u32 rptr;
 	u32 src_id, src_data;
 	u32 ring_index;
-	unsigned long flags;
 	bool queue_hotplug = false;
 	bool queue_hdmi = false;
 
@@ -3321,24 +3319,21 @@ int r600_irq_process(struct radeon_device *rdev)
 		RREG32(IH_RB_WPTR);
 
 	wptr = r600_get_ih_wptr(rdev);
-	rptr = rdev->ih.rptr;
-	DRM_DEBUG("r600_irq_process start: rptr %d, wptr %d\n", rptr, wptr);
-
-	spin_lock_irqsave(&rdev->ih.lock, flags);
 
-	if (rptr == wptr) {
-		spin_unlock_irqrestore(&rdev->ih.lock, flags);
+restart_ih:
+	/* is somebody else already processing irqs? */
+	if (atomic_xchg(&rdev->ih.lock, 1))
 		return IRQ_NONE;
-	}
 
-restart_ih:
+	rptr = rdev->ih.rptr;
+	DRM_DEBUG("r600_irq_process start: rptr %d, wptr %d\n", rptr, wptr);
+
 	/* Order reading of wptr vs. reading of IH ring data */
 	rmb();
 
 	/* display interrupts */
 	r600_irq_ack(rdev);
 
-	rdev->ih.wptr = wptr;
 	while (rptr != wptr) {
 		/* wptr/rptr are in bytes! */
 		ring_index = rptr / 4;
@@ -3355,7 +3350,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[0])
+					if (atomic_read(&rdev->irq.pflip[0]))
 						radeon_crtc_handle_flip(rdev, 0);
 					rdev->irq.stat_regs.r600.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D1 vblank\n");
@@ -3381,7 +3376,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[1])
+					if (atomic_read(&rdev->irq.pflip[1]))
 						radeon_crtc_handle_flip(rdev, 1);
 					rdev->irq.stat_regs.r600.disp_int &= ~LB_D2_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D2 vblank\n");
@@ -3480,7 +3475,6 @@ restart_ih:
 			break;
 		case 233: /* GUI IDLE */
 			DRM_DEBUG("IH: GUI idle\n");
-			rdev->pm.gui_idle = true;
 			wake_up(&rdev->irq.idle_queue);
 			break;
 		default:
@@ -3492,17 +3486,19 @@ restart_ih:
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
 	}
-	/* make sure wptr hasn't changed while processing */
-	wptr = r600_get_ih_wptr(rdev);
-	if (wptr != rdev->ih.wptr)
-		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
 	if (queue_hdmi)
 		schedule_work(&rdev->audio_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
-	spin_unlock_irqrestore(&rdev->ih.lock, flags);
+	atomic_set(&rdev->ih.lock, 0);
+
+	/* make sure wptr hasn't changed while processing */
+	wptr = r600_get_ih_wptr(rdev);
+	if (wptr != rptr)
+		goto restart_ih;
+
 	return IRQ_HANDLED;
 }
 
@@ -3685,6 +3681,8 @@ static void r600_pcie_gen2_enable(struct radeon_device *rdev)
 {
 	u32 link_width_cntl, lanes, speed_cntl, training_cntl, tmp;
 	u16 link_cntl2;
+	u32 mask;
+	int ret;
 
 	if (radeon_pcie_gen2 == 0)
 		return;
@@ -3703,6 +3701,15 @@ static void r600_pcie_gen2_enable(struct radeon_device *rdev)
 	if (rdev->family <= CHIP_R600)
 		return;
 
+	ret = drm_pcie_get_speed_cap_mask(rdev->ddev, &mask);
+	if (ret != 0)
+		return;
+
+	if (!(mask & DRM_PCIE_SPEED_50))
+		return;
+
+	DRM_INFO("enabling PCIE gen 2 link speeds, disable with radeon.pcie_gen2=0\n");
+
 	/* 55 nm r6xx asics */
 	if ((rdev->family == CHIP_RV670) ||
 	    (rdev->family == CHIP_RV620) ||
diff --git a/drivers/gpu/drm/radeon/r600_blit_kms.c b/drivers/gpu/drm/radeon/r600_blit_kms.c
index 03b6e0d3d50..2bef8549ddf 100644
--- a/drivers/gpu/drm/radeon/r600_blit_kms.c
+++ b/drivers/gpu/drm/radeon/r600_blit_kms.c
@@ -512,7 +512,8 @@ int r600_blit_init(struct radeon_device *rdev)
 	rdev->r600_blit.primitives.draw_auto = draw_auto;
 	rdev->r600_blit.primitives.set_default_state = set_default_state;
 
-	rdev->r600_blit.ring_size_common = 40; /* shaders + def state */
+	rdev->r600_blit.ring_size_common = 8; /* sync semaphore */
+	rdev->r600_blit.ring_size_common += 40; /* shaders + def state */
 	rdev->r600_blit.ring_size_common += 5; /* done copy */
 	rdev->r600_blit.ring_size_common += 16; /* fence emit for done copy */
 
@@ -523,10 +524,6 @@ int r600_blit_init(struct radeon_device *rdev)
 
 	rdev->r600_blit.max_dim = 8192;
 
-	/* pin copy shader into vram if already initialized */
-	if (rdev->r600_blit.shader_obj)
-		goto done;
-
 	rdev->r600_blit.state_offset = 0;
 
 	if (rdev->family >= CHIP_RV770)
@@ -551,11 +548,26 @@ 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, obj_size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-			     NULL, &rdev->r600_blit.shader_obj);
-	if (r) {
-		DRM_ERROR("r600 failed to allocate shader\n");
-		return r;
+	/* pin copy shader into vram if not already initialized */
+	if (rdev->r600_blit.shader_obj == NULL) {
+		r = radeon_bo_create(rdev, obj_size, PAGE_SIZE, true,
+				     RADEON_GEM_DOMAIN_VRAM,
+				     NULL, &rdev->r600_blit.shader_obj);
+		if (r) {
+			DRM_ERROR("r600 failed to allocate shader\n");
+			return r;
+		}
+
+		r = radeon_bo_reserve(rdev->r600_blit.shader_obj, false);
+		if (unlikely(r != 0))
+			return r;
+		r = radeon_bo_pin(rdev->r600_blit.shader_obj, RADEON_GEM_DOMAIN_VRAM,
+				  &rdev->r600_blit.shader_gpu_addr);
+		radeon_bo_unreserve(rdev->r600_blit.shader_obj);
+		if (r) {
+			dev_err(rdev->dev, "(%d) pin blit object failed\n", r);
+			return r;
+		}
 	}
 
 	DRM_DEBUG("r6xx blit allocated bo %08x vs %08x ps %08x\n",
@@ -586,17 +598,6 @@ int r600_blit_init(struct radeon_device *rdev)
 	radeon_bo_kunmap(rdev->r600_blit.shader_obj);
 	radeon_bo_unreserve(rdev->r600_blit.shader_obj);
 
-done:
-	r = radeon_bo_reserve(rdev->r600_blit.shader_obj, false);
-	if (unlikely(r != 0))
-		return r;
-	r = radeon_bo_pin(rdev->r600_blit.shader_obj, RADEON_GEM_DOMAIN_VRAM,
-			  &rdev->r600_blit.shader_gpu_addr);
-	radeon_bo_unreserve(rdev->r600_blit.shader_obj);
-	if (r) {
-		dev_err(rdev->dev, "(%d) pin blit object failed\n", r);
-		return r;
-	}
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.real_vram_size);
 	return 0;
 }
@@ -666,7 +667,8 @@ static unsigned r600_blit_create_rect(unsigned num_gpu_pages,
 
 
 int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
-			   struct radeon_sa_bo **vb)
+			   struct radeon_fence **fence, struct radeon_sa_bo **vb,
+			   struct radeon_semaphore **sem)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
@@ -689,34 +691,50 @@ int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages,
 		return r;
 	}
 
+	r = radeon_semaphore_create(rdev, sem);
+	if (r) {
+		radeon_sa_bo_free(rdev, vb, NULL);
+		return r;
+	}
+
 	/* calculate number of loops correctly */
 	ring_size = num_loops * dwords_per_loop;
 	ring_size += rdev->r600_blit.ring_size_common;
 	r = radeon_ring_lock(rdev, ring, ring_size);
 	if (r) {
 		radeon_sa_bo_free(rdev, vb, NULL);
+		radeon_semaphore_free(rdev, sem, NULL);
 		return r;
 	}
 
+	if (radeon_fence_need_sync(*fence, RADEON_RING_TYPE_GFX_INDEX)) {
+		radeon_semaphore_sync_rings(rdev, *sem, (*fence)->ring,
+					    RADEON_RING_TYPE_GFX_INDEX);
+		radeon_fence_note_sync(*fence, RADEON_RING_TYPE_GFX_INDEX);
+	} else {
+		radeon_semaphore_free(rdev, sem, NULL);
+	}
+
 	rdev->r600_blit.primitives.set_default_state(rdev);
 	rdev->r600_blit.primitives.set_shaders(rdev);
 	return 0;
 }
 
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence,
-			 struct radeon_sa_bo *vb)
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence **fence,
+			 struct radeon_sa_bo *vb, struct radeon_semaphore *sem)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	r = radeon_fence_emit(rdev, fence);
+	r = radeon_fence_emit(rdev, fence, RADEON_RING_TYPE_GFX_INDEX);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		return;
 	}
 
 	radeon_ring_unlock_commit(rdev, ring);
-	radeon_sa_bo_free(rdev, &vb, fence);
+	radeon_sa_bo_free(rdev, &vb, *fence);
+	radeon_semaphore_free(rdev, &sem, *fence);
 }
 
 void r600_kms_blit_copy(struct radeon_device *rdev,
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index 82a0a4c919c..e3558c3ef24 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -519,8 +519,7 @@ void r600_hdmi_enable(struct drm_encoder *encoder)
 
 	if (rdev->irq.installed) {
 		/* if irq is available use it */
-		rdev->irq.afmt[dig->afmt->id] = true;
-		radeon_irq_set(rdev);
+		radeon_irq_kms_enable_afmt(rdev, dig->afmt->id);
 	}
 
 	dig->afmt->enabled = true;
@@ -556,8 +555,7 @@ void r600_hdmi_disable(struct drm_encoder *encoder)
 		  offset, radeon_encoder->encoder_id);
 
 	/* disable irq */
-	rdev->irq.afmt[dig->afmt->id] = false;
-	radeon_irq_set(rdev);
+	radeon_irq_kms_disable_afmt(rdev, dig->afmt->id);
 
 	/* Older chipsets not handled by AtomBIOS */
 	if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index 025fd5b6c08..4b116ae75fc 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -153,6 +153,9 @@
 
 #define	CONFIG_MEMSIZE					0x5428
 #define CONFIG_CNTL					0x5424
+#define	CP_STALLED_STAT1			0x8674
+#define	CP_STALLED_STAT2			0x8678
+#define	CP_BUSY_STAT				0x867C
 #define	CP_STAT						0x8680
 #define	CP_COHER_BASE					0x85F8
 #define	CP_DEBUG					0xC1FC
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index fefcca55c1e..5431af29240 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -113,7 +113,6 @@ extern int radeon_lockup_timeout;
 
 /* fence seq are set to this number when signaled */
 #define RADEON_FENCE_SIGNALED_SEQ		0LL
-#define RADEON_FENCE_NOTEMITED_SEQ		(~0LL)
 
 /* internal ring indices */
 /* r1xx+ has gfx CP ring */
@@ -160,48 +159,6 @@ static inline int radeon_atrm_get_bios_chunk(uint8_t *bios, int offset, int len)
 #endif
 bool radeon_get_bios(struct radeon_device *rdev);
 
-
-/*
- * Mutex which allows recursive locking from the same process.
- */
-struct radeon_mutex {
-	struct mutex		mutex;
-	struct task_struct	*owner;
-	int			level;
-};
-
-static inline void radeon_mutex_init(struct radeon_mutex *mutex)
-{
-	mutex_init(&mutex->mutex);
-	mutex->owner = NULL;
-	mutex->level = 0;
-}
-
-static inline void radeon_mutex_lock(struct radeon_mutex *mutex)
-{
-	if (mutex_trylock(&mutex->mutex)) {
-		/* The mutex was unlocked before, so it's ours now */
-		mutex->owner = current;
-	} else if (mutex->owner != current) {
-		/* Another process locked the mutex, take it */
-		mutex_lock(&mutex->mutex);
-		mutex->owner = current;
-	}
-	/* Otherwise the mutex was already locked by this process */
-
-	mutex->level++;
-}
-
-static inline void radeon_mutex_unlock(struct radeon_mutex *mutex)
-{
-	if (--mutex->level > 0)
-		return;
-
-	mutex->owner = NULL;
-	mutex_unlock(&mutex->mutex);
-}
-
-
 /*
  * Dummy page
  */
@@ -258,8 +215,8 @@ struct radeon_fence_driver {
 	uint32_t			scratch_reg;
 	uint64_t			gpu_addr;
 	volatile uint32_t		*cpu_addr;
-	/* seq is protected by ring emission lock */
-	uint64_t			seq;
+	/* sync_seq is protected by ring emission lock */
+	uint64_t			sync_seq[RADEON_NUM_RINGS];
 	atomic64_t			last_seq;
 	unsigned long			last_activity;
 	bool				initialized;
@@ -277,19 +234,39 @@ struct radeon_fence {
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
 int radeon_fence_driver_init(struct radeon_device *rdev);
 void radeon_fence_driver_fini(struct radeon_device *rdev);
-int radeon_fence_create(struct radeon_device *rdev, struct radeon_fence **fence, int ring);
-int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence);
+int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence **fence, int ring);
 void radeon_fence_process(struct radeon_device *rdev, int ring);
 bool radeon_fence_signaled(struct radeon_fence *fence);
 int radeon_fence_wait(struct radeon_fence *fence, bool interruptible);
 int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring);
-int radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring);
+void radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring);
 int radeon_fence_wait_any(struct radeon_device *rdev,
 			  struct radeon_fence **fences,
 			  bool intr);
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence);
 void radeon_fence_unref(struct radeon_fence **fence);
 unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring);
+bool radeon_fence_need_sync(struct radeon_fence *fence, int ring);
+void radeon_fence_note_sync(struct radeon_fence *fence, int ring);
+static inline struct radeon_fence *radeon_fence_later(struct radeon_fence *a,
+						      struct radeon_fence *b)
+{
+	if (!a) {
+		return b;
+	}
+
+	if (!b) {
+		return a;
+	}
+
+	BUG_ON(a->ring != b->ring);
+
+	if (a->seq > b->seq) {
+		return a;
+	} else {
+		return b;
+	}
+}
 
 /*
  * Tiling registers
@@ -385,7 +362,7 @@ struct radeon_bo_list {
  * alignment).
  */
 struct radeon_sa_manager {
-	spinlock_t		lock;
+	wait_queue_head_t	wq;
 	struct radeon_bo	*bo;
 	struct list_head	*hole;
 	struct list_head	flist[RADEON_NUM_RINGS];
@@ -451,10 +428,9 @@ void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 				struct radeon_semaphore *semaphore);
 int radeon_semaphore_sync_rings(struct radeon_device *rdev,
 				struct radeon_semaphore *semaphore,
-				bool sync_to[RADEON_NUM_RINGS],
-				int dst_ring);
+				int signaler, int waiter);
 void radeon_semaphore_free(struct radeon_device *rdev,
-			   struct radeon_semaphore *semaphore,
+			   struct radeon_semaphore **semaphore,
 			   struct radeon_fence *fence);
 
 /*
@@ -597,21 +573,18 @@ union radeon_irq_stat_regs {
 #define RADEON_MAX_AFMT_BLOCKS 6
 
 struct radeon_irq {
-	bool		installed;
-	bool		sw_int[RADEON_NUM_RINGS];
-	bool		crtc_vblank_int[RADEON_MAX_CRTCS];
-	bool		pflip[RADEON_MAX_CRTCS];
-	wait_queue_head_t	vblank_queue;
-	bool            hpd[RADEON_MAX_HPD_PINS];
-	bool            gui_idle;
-	bool            gui_idle_acked;
-	wait_queue_head_t	idle_queue;
-	bool		afmt[RADEON_MAX_AFMT_BLOCKS];
-	spinlock_t sw_lock;
-	int sw_refcount[RADEON_NUM_RINGS];
-	union radeon_irq_stat_regs stat_regs;
-	spinlock_t pflip_lock[RADEON_MAX_CRTCS];
-	int pflip_refcount[RADEON_MAX_CRTCS];
+	bool				installed;
+	spinlock_t			lock;
+	atomic_t			ring_int[RADEON_NUM_RINGS];
+	bool				crtc_vblank_int[RADEON_MAX_CRTCS];
+	atomic_t			pflip[RADEON_MAX_CRTCS];
+	wait_queue_head_t		vblank_queue;
+	bool				hpd[RADEON_MAX_HPD_PINS];
+	bool				gui_idle;
+	bool				gui_idle_acked;
+	wait_queue_head_t		idle_queue;
+	bool				afmt[RADEON_MAX_AFMT_BLOCKS];
+	union radeon_irq_stat_regs	stat_regs;
 };
 
 int radeon_irq_kms_init(struct radeon_device *rdev);
@@ -620,6 +593,11 @@ void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev, int ring);
 void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev, int ring);
 void radeon_irq_kms_pflip_irq_get(struct radeon_device *rdev, int crtc);
 void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc);
+void radeon_irq_kms_enable_afmt(struct radeon_device *rdev, int block);
+void radeon_irq_kms_disable_afmt(struct radeon_device *rdev, int block);
+void radeon_irq_kms_enable_hpd(struct radeon_device *rdev, unsigned hpd_mask);
+void radeon_irq_kms_disable_hpd(struct radeon_device *rdev, unsigned hpd_mask);
+int radeon_irq_kms_wait_gui_idle(struct radeon_device *rdev);
 
 /*
  * CP & rings.
@@ -630,9 +608,11 @@ struct radeon_ib {
 	uint32_t			length_dw;
 	uint64_t			gpu_addr;
 	uint32_t			*ptr;
+	int				ring;
 	struct radeon_fence		*fence;
 	unsigned			vm_id;
 	bool				is_const_ib;
+	struct radeon_fence		*sync_to[RADEON_NUM_RINGS];
 	struct radeon_semaphore		*semaphore;
 };
 
@@ -642,6 +622,9 @@ struct radeon_ring {
 	unsigned		rptr;
 	unsigned		rptr_offs;
 	unsigned		rptr_reg;
+	unsigned		rptr_save_reg;
+	u64			next_rptr_gpu_addr;
+	volatile u32		*next_rptr_cpu_addr;
 	unsigned		wptr;
 	unsigned		wptr_old;
 	unsigned		wptr_reg;
@@ -657,6 +640,7 @@ struct radeon_ring {
 	u32			ptr_reg_shift;
 	u32			ptr_reg_mask;
 	u32			nop;
+	u32			idx;
 };
 
 /*
@@ -690,6 +674,7 @@ struct radeon_vm_funcs {
 };
 
 struct radeon_vm_manager {
+	struct mutex			lock;
 	struct list_head		lru_vm;
 	uint32_t			use_bitmap;
 	struct radeon_sa_manager	sa_manager;
@@ -718,13 +703,10 @@ struct r600_ih {
 	struct radeon_bo	*ring_obj;
 	volatile uint32_t	*ring;
 	unsigned		rptr;
-	unsigned		rptr_offs;
-	unsigned		wptr;
-	unsigned		wptr_old;
 	unsigned		ring_size;
 	uint64_t		gpu_addr;
 	uint32_t		ptr_mask;
-	spinlock_t              lock;
+	atomic_t		lock;
 	bool                    enabled;
 };
 
@@ -757,8 +739,6 @@ struct r600_blit {
 	u32 state_len;
 };
 
-void r600_blit_suspend(struct radeon_device *rdev);
-
 /*
  * SI RLC stuff
  */
@@ -774,14 +754,14 @@ struct si_rlc {
 int radeon_ib_get(struct radeon_device *rdev, int ring,
 		  struct radeon_ib *ib, unsigned size);
 void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib);
-int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib);
+int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
+		       struct radeon_ib *const_ib);
 int radeon_ib_pool_init(struct radeon_device *rdev);
 void radeon_ib_pool_fini(struct radeon_device *rdev);
-int radeon_ib_pool_start(struct radeon_device *rdev);
-int radeon_ib_pool_suspend(struct radeon_device *rdev);
 int radeon_ib_ring_tests(struct radeon_device *rdev);
 /* Ring access between begin & end cannot sleep */
-int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *cp);
+bool radeon_ring_supports_scratch_reg(struct radeon_device *rdev,
+				      struct radeon_ring *ring);
 void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *cp);
 int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
 int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
@@ -793,6 +773,10 @@ int radeon_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
 void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *ring);
 void radeon_ring_lockup_update(struct radeon_ring *ring);
 bool radeon_ring_test_lockup(struct radeon_device *rdev, struct radeon_ring *ring);
+unsigned radeon_ring_backup(struct radeon_device *rdev, struct radeon_ring *ring,
+			    uint32_t **data);
+int radeon_ring_restore(struct radeon_device *rdev, struct radeon_ring *ring,
+			unsigned size, uint32_t *data);
 int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ring_size,
 		     unsigned rptr_offs, unsigned rptr_reg, unsigned wptr_reg,
 		     u32 ptr_reg_shift, u32 ptr_reg_mask, u32 nop);
@@ -891,6 +875,7 @@ struct radeon_wb {
 };
 
 #define RADEON_WB_SCRATCH_OFFSET 0
+#define RADEON_WB_RING0_NEXT_RPTR 256
 #define RADEON_WB_CP_RPTR_OFFSET 1024
 #define RADEON_WB_CP1_RPTR_OFFSET 1280
 #define RADEON_WB_CP2_RPTR_OFFSET 1536
@@ -1039,11 +1024,12 @@ struct radeon_power_state {
 
 struct radeon_pm {
 	struct mutex		mutex;
+	/* write locked while reprogramming mclk */
+	struct rw_semaphore	mclk_lock;
 	u32			active_crtcs;
 	int			active_crtc_count;
 	int			req_vblank;
 	bool			vblank_sync;
-	bool			gui_idle;
 	fixed20_12		max_bandwidth;
 	fixed20_12		igp_sideport_mclk;
 	fixed20_12		igp_system_mclk;
@@ -1192,20 +1178,20 @@ struct radeon_asic {
 			    uint64_t src_offset,
 			    uint64_t dst_offset,
 			    unsigned num_gpu_pages,
-			    struct radeon_fence *fence);
+			    struct radeon_fence **fence);
 		u32 blit_ring_index;
 		int (*dma)(struct radeon_device *rdev,
 			   uint64_t src_offset,
 			   uint64_t dst_offset,
 			   unsigned num_gpu_pages,
-			   struct radeon_fence *fence);
+			   struct radeon_fence **fence);
 		u32 dma_ring_index;
 		/* method used for bo copy */
 		int (*copy)(struct radeon_device *rdev,
 			    uint64_t src_offset,
 			    uint64_t dst_offset,
 			    unsigned num_gpu_pages,
-			    struct radeon_fence *fence);
+			    struct radeon_fence **fence);
 		/* ring used for bo copies */
 		u32 copy_ring_index;
 	} copy;
@@ -1467,6 +1453,7 @@ struct radeon_device {
 	struct device			*dev;
 	struct drm_device		*ddev;
 	struct pci_dev			*pdev;
+	struct rw_semaphore		exclusive_lock;
 	/* ASIC */
 	union radeon_asic_config	config;
 	enum radeon_family		family;
@@ -1512,7 +1499,6 @@ struct radeon_device {
 	struct radeon_gem		gem;
 	struct radeon_pm		pm;
 	uint32_t			bios_scratch[RADEON_BIOS_NUM_SCRATCH];
-	struct radeon_mutex		cs_mutex;
 	struct radeon_wb		wb;
 	struct radeon_dummy_page	dummy_page;
 	bool				shutdown;
@@ -1534,7 +1520,6 @@ struct radeon_device {
 	struct work_struct audio_work;
 	int num_crtc; /* number of crtcs */
 	struct mutex dc_hw_i2c_mutex; /* display controller hw i2c mutex */
-	struct mutex vram_mutex;
 	bool audio_enabled;
 	struct r600_audio audio_status; /* audio stuff */
 	struct notifier_block acpi_nb;
@@ -1785,8 +1770,6 @@ extern void radeon_ttm_set_active_vram_size(struct radeon_device *rdev, u64 size
  */
 int radeon_vm_manager_init(struct radeon_device *rdev);
 void radeon_vm_manager_fini(struct radeon_device *rdev);
-int radeon_vm_manager_start(struct radeon_device *rdev);
-int radeon_vm_manager_suspend(struct radeon_device *rdev);
 int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm);
 void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm);
 int radeon_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm);
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index f533df5f7d5..973417c4b01 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -40,6 +40,16 @@
 /*
  * Registers accessors functions.
  */
+/**
+ * radeon_invalid_rreg - dummy reg read function
+ *
+ * @rdev: radeon device pointer
+ * @reg: offset of register
+ *
+ * Dummy register read function.  Used for register blocks
+ * that certain asics don't have (all asics).
+ * Returns the value in the register.
+ */
 static uint32_t radeon_invalid_rreg(struct radeon_device *rdev, uint32_t reg)
 {
 	DRM_ERROR("Invalid callback to read register 0x%04X\n", reg);
@@ -47,6 +57,16 @@ static uint32_t radeon_invalid_rreg(struct radeon_device *rdev, uint32_t reg)
 	return 0;
 }
 
+/**
+ * radeon_invalid_wreg - dummy reg write function
+ *
+ * @rdev: radeon device pointer
+ * @reg: offset of register
+ * @v: value to write to the register
+ *
+ * Dummy register read function.  Used for register blocks
+ * that certain asics don't have (all asics).
+ */
 static void radeon_invalid_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v)
 {
 	DRM_ERROR("Invalid callback to write register 0x%04X with 0x%08X\n",
@@ -54,6 +74,14 @@ static void radeon_invalid_wreg(struct radeon_device *rdev, uint32_t reg, uint32
 	BUG_ON(1);
 }
 
+/**
+ * radeon_register_accessor_init - sets up the register accessor callbacks
+ *
+ * @rdev: radeon device pointer
+ *
+ * Sets up the register accessor callbacks for various register
+ * apertures.  Not all asics have all apertures (all asics).
+ */
 static void radeon_register_accessor_init(struct radeon_device *rdev)
 {
 	rdev->mc_rreg = &radeon_invalid_rreg;
@@ -102,6 +130,14 @@ static void radeon_register_accessor_init(struct radeon_device *rdev)
 
 
 /* helper to disable agp */
+/**
+ * radeon_agp_disable - AGP disable helper function
+ *
+ * @rdev: radeon device pointer
+ *
+ * Removes AGP flags and changes the gart callbacks on AGP
+ * cards when using the internal gart rather than AGP (all asics).
+ */
 void radeon_agp_disable(struct radeon_device *rdev)
 {
 	rdev->flags &= ~RADEON_IS_AGP;
@@ -1608,6 +1644,16 @@ static struct radeon_asic si_asic = {
 	},
 };
 
+/**
+ * radeon_asic_init - register asic specific callbacks
+ *
+ * @rdev: radeon device pointer
+ *
+ * Registers the appropriate asic specific callbacks for each
+ * chip family.  Also sets other asics specific info like the number
+ * of crtcs and the register aperture accessors (all asics).
+ * Returns 0 for success.
+ */
 int radeon_asic_init(struct radeon_device *rdev)
 {
 	radeon_register_accessor_init(rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index e76a941ef14..f4af2431043 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -79,7 +79,7 @@ int r100_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset,
 		   uint64_t dst_offset,
 		   unsigned num_gpu_pages,
-		   struct radeon_fence *fence);
+		   struct radeon_fence **fence);
 int r100_set_surface_reg(struct radeon_device *rdev, int reg,
 			 uint32_t tiling_flags, uint32_t pitch,
 			 uint32_t offset, uint32_t obj_size);
@@ -103,7 +103,6 @@ int r100_pci_gart_enable(struct radeon_device *rdev);
 void r100_pci_gart_disable(struct radeon_device *rdev);
 int r100_debugfs_mc_info_init(struct radeon_device *rdev);
 int r100_gui_wait_for_idle(struct radeon_device *rdev);
-void r100_ib_fini(struct radeon_device *rdev);
 int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 void r100_irq_disable(struct radeon_device *rdev);
 void r100_mc_stop(struct radeon_device *rdev, struct r100_mc_save *save);
@@ -144,7 +143,7 @@ extern int r200_copy_dma(struct radeon_device *rdev,
 			 uint64_t src_offset,
 			 uint64_t dst_offset,
 			 unsigned num_gpu_pages,
-			 struct radeon_fence *fence);
+			 struct radeon_fence **fence);
 void r200_set_safe_registers(struct radeon_device *rdev);
 
 /*
@@ -318,7 +317,7 @@ void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
 int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
 int r600_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset, uint64_t dst_offset,
-		   unsigned num_gpu_pages, struct radeon_fence *fence);
+		   unsigned num_gpu_pages, struct radeon_fence **fence);
 void r600_hpd_init(struct radeon_device *rdev);
 void r600_hpd_fini(struct radeon_device *rdev);
 bool r600_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd);
@@ -363,9 +362,10 @@ 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, unsigned num_gpu_pages,
-			   struct radeon_sa_bo **vb);
-void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence,
-			 struct radeon_sa_bo *vb);
+			   struct radeon_fence **fence, struct radeon_sa_bo **vb,
+			   struct radeon_semaphore **sem);
+void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence **fence,
+			 struct radeon_sa_bo *vb, struct radeon_semaphore *sem);
 void r600_kms_blit_copy(struct radeon_device *rdev,
 			u64 src_gpu_addr, u64 dst_gpu_addr,
 			unsigned num_gpu_pages,
diff --git a/drivers/gpu/drm/radeon/radeon_benchmark.c b/drivers/gpu/drm/radeon/radeon_benchmark.c
index 364f5b1a04b..bedda9caadd 100644
--- a/drivers/gpu/drm/radeon/radeon_benchmark.c
+++ b/drivers/gpu/drm/radeon/radeon_benchmark.c
@@ -45,20 +45,14 @@ static int radeon_benchmark_do_move(struct radeon_device *rdev, unsigned size,
 	for (i = 0; i < n; i++) {
 		switch (flag) {
 		case RADEON_BENCHMARK_COPY_DMA:
-			r = radeon_fence_create(rdev, &fence, radeon_copy_dma_ring_index(rdev));
-			if (r)
-				return r;
 			r = radeon_copy_dma(rdev, saddr, daddr,
 					    size / RADEON_GPU_PAGE_SIZE,
-					    fence);
+					    &fence);
 			break;
 		case RADEON_BENCHMARK_COPY_BLIT:
-			r = radeon_fence_create(rdev, &fence, radeon_copy_blit_ring_index(rdev));
-			if (r)
-				return r;
 			r = radeon_copy_blit(rdev, saddr, daddr,
 					     size / RADEON_GPU_PAGE_SIZE,
-					     fence);
+					     &fence);
 			break;
 		default:
 			DRM_ERROR("Unknown copy method\n");
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 2914c5761cf..895e628b60f 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -64,14 +64,33 @@ void radeon_connector_hotplug(struct drm_connector *connector)
 
 	/* just deal with DP (not eDP) here. */
 	if (connector->connector_type == DRM_MODE_CONNECTOR_DisplayPort) {
-		int saved_dpms = connector->dpms;
-
-		/* Only turn off the display it it's physically disconnected */
-		if (!radeon_hpd_sense(rdev, radeon_connector->hpd.hpd))
-			drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
-		else if (radeon_dp_needs_link_train(radeon_connector))
-			drm_helper_connector_dpms(connector, DRM_MODE_DPMS_ON);
-		connector->dpms = saved_dpms;
+		struct radeon_connector_atom_dig *dig_connector =
+			radeon_connector->con_priv;
+
+		/* if existing sink type was not DP no need to retrain */
+		if (dig_connector->dp_sink_type != CONNECTOR_OBJECT_ID_DISPLAYPORT)
+			return;
+
+		/* first get sink type as it may be reset after (un)plug */
+		dig_connector->dp_sink_type = radeon_dp_getsinktype(radeon_connector);
+		/* don't do anything if sink is not display port, i.e.,
+		 * passive dp->(dvi|hdmi) adaptor
+		 */
+		if (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) {
+			int saved_dpms = connector->dpms;
+			/* Only turn off the display if it's physically disconnected */
+			if (!radeon_hpd_sense(rdev, radeon_connector->hpd.hpd)) {
+				drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
+			} else if (radeon_dp_needs_link_train(radeon_connector)) {
+				/* set it to OFF so that drm_helper_connector_dpms()
+				 * won't return immediately since the current state
+				 * is ON at this point.
+				 */
+				connector->dpms = DRM_MODE_DPMS_OFF;
+				drm_helper_connector_dpms(connector, DRM_MODE_DPMS_ON);
+			}
+			connector->dpms = saved_dpms;
+		}
 	}
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 142f89462aa..8a4c49ef0cc 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -115,36 +115,20 @@ static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority
 	return 0;
 }
 
-static int radeon_cs_sync_rings(struct radeon_cs_parser *p)
+static void radeon_cs_sync_rings(struct radeon_cs_parser *p)
 {
-	bool sync_to_ring[RADEON_NUM_RINGS] = { };
-	bool need_sync = false;
-	int i, r;
+	int i;
 
 	for (i = 0; i < p->nrelocs; i++) {
-		struct radeon_fence *fence;
+		struct radeon_fence *a, *b;
 
 		if (!p->relocs[i].robj || !p->relocs[i].robj->tbo.sync_obj)
 			continue;
 
-		fence = p->relocs[i].robj->tbo.sync_obj;
-		if (fence->ring != p->ring && !radeon_fence_signaled(fence)) {
-			sync_to_ring[fence->ring] = true;
-			need_sync = true;
-		}
-	}
-
-	if (!need_sync) {
-		return 0;
-	}
-
-	r = radeon_semaphore_create(p->rdev, &p->ib.semaphore);
-	if (r) {
-		return r;
+		a = p->relocs[i].robj->tbo.sync_obj;
+		b = p->ib.sync_to[a->ring];
+		p->ib.sync_to[a->ring] = radeon_fence_later(a, b);
 	}
-
-	return radeon_semaphore_sync_rings(p->rdev, p->ib.semaphore,
-					   sync_to_ring, p->ring);
 }
 
 /* XXX: note that this is called from the legacy UMS CS ioctl as well */
@@ -368,16 +352,13 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 		DRM_ERROR("Invalid command stream !\n");
 		return r;
 	}
-	r = radeon_cs_sync_rings(parser);
-	if (r) {
-		DRM_ERROR("Failed to synchronize rings !\n");
-	}
+	radeon_cs_sync_rings(parser);
 	parser->ib.vm_id = 0;
-	r = radeon_ib_schedule(rdev, &parser->ib);
+	r = radeon_ib_schedule(rdev, &parser->ib, NULL);
 	if (r) {
 		DRM_ERROR("Failed to schedule IB !\n");
 	}
-	return 0;
+	return r;
 }
 
 static int radeon_bo_vm_update_pte(struct radeon_cs_parser *parser,
@@ -459,6 +440,7 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 		return r;
 	}
 
+	mutex_lock(&rdev->vm_manager.lock);
 	mutex_lock(&vm->mutex);
 	r = radeon_vm_bind(rdev, vm);
 	if (r) {
@@ -468,30 +450,26 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 	if (r) {
 		goto out;
 	}
-	r = radeon_cs_sync_rings(parser);
-	if (r) {
-		DRM_ERROR("Failed to synchronize rings !\n");
-	}
+	radeon_cs_sync_rings(parser);
+
+	parser->ib.vm_id = vm->id;
+	/* ib pool is bind at 0 in virtual address space,
+	 * so gpu_addr is the offset inside the pool bo
+	 */
+	parser->ib.gpu_addr = parser->ib.sa_bo->soffset;
 
 	if ((rdev->family >= CHIP_TAHITI) &&
 	    (parser->chunk_const_ib_idx != -1)) {
 		parser->const_ib.vm_id = vm->id;
-		/* ib pool is bind at 0 in virtual address space to gpu_addr is the
-		 * offset inside the pool bo
+		/* ib pool is bind at 0 in virtual address space,
+		 * so gpu_addr is the offset inside the pool bo
 		 */
 		parser->const_ib.gpu_addr = parser->const_ib.sa_bo->soffset;
-		r = radeon_ib_schedule(rdev, &parser->const_ib);
-		if (r)
-			goto out;
+		r = radeon_ib_schedule(rdev, &parser->ib, &parser->const_ib);
+	} else {
+		r = radeon_ib_schedule(rdev, &parser->ib, NULL);
 	}
 
-	parser->ib.vm_id = vm->id;
-	/* ib pool is bind at 0 in virtual address space to gpu_addr is the
-	 * offset inside the pool bo
-	 */
-	parser->ib.gpu_addr = parser->ib.sa_bo->soffset;
-	parser->ib.is_const_ib = false;
-	r = radeon_ib_schedule(rdev, &parser->ib);
 out:
 	if (!r) {
 		if (vm->fence) {
@@ -499,7 +477,8 @@ out:
 		}
 		vm->fence = radeon_fence_ref(parser->ib.fence);
 	}
-	mutex_unlock(&fpriv->vm.mutex);
+	mutex_unlock(&vm->mutex);
+	mutex_unlock(&rdev->vm_manager.lock);
 	return r;
 }
 
@@ -519,9 +498,9 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	struct radeon_cs_parser parser;
 	int r;
 
-	radeon_mutex_lock(&rdev->cs_mutex);
+	down_read(&rdev->exclusive_lock);
 	if (!rdev->accel_working) {
-		radeon_mutex_unlock(&rdev->cs_mutex);
+		up_read(&rdev->exclusive_lock);
 		return -EBUSY;
 	}
 	/* initialize parser */
@@ -534,8 +513,8 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	if (r) {
 		DRM_ERROR("Failed to initialize parser !\n");
 		radeon_cs_parser_fini(&parser, r);
+		up_read(&rdev->exclusive_lock);
 		r = radeon_cs_handle_lockup(rdev, r);
-		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
 	r = radeon_cs_parser_relocs(&parser);
@@ -543,8 +522,8 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		if (r != -ERESTARTSYS)
 			DRM_ERROR("Failed to parse relocation %d!\n", r);
 		radeon_cs_parser_fini(&parser, r);
+		up_read(&rdev->exclusive_lock);
 		r = radeon_cs_handle_lockup(rdev, r);
-		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
 	r = radeon_cs_ib_chunk(rdev, &parser);
@@ -557,8 +536,8 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 	}
 out:
 	radeon_cs_parser_fini(&parser, r);
+	up_read(&rdev->exclusive_lock);
 	r = radeon_cs_handle_lockup(rdev, r);
-	radeon_mutex_unlock(&rdev->cs_mutex);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_cursor.c b/drivers/gpu/drm/radeon/radeon_cursor.c
index 42acc6449dd..711e95ad39b 100644
--- a/drivers/gpu/drm/radeon/radeon_cursor.c
+++ b/drivers/gpu/drm/radeon/radeon_cursor.c
@@ -262,8 +262,14 @@ int radeon_crtc_cursor_move(struct drm_crtc *crtc,
 				if (!(cursor_end & 0x7f))
 					w--;
 			}
-			if (w <= 0)
+			if (w <= 0) {
 				w = 1;
+				cursor_end = x - xorigin + w;
+				if (!(cursor_end & 0x7f)) {
+					x--;
+					WARN_ON_ONCE(x < 0);
+				}
+			}
 		}
 	}
 
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 066c98b888a..742af8244e8 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -96,8 +96,12 @@ static const char radeon_family_name[][16] = {
 	"LAST",
 };
 
-/*
- * Clear GPU surface registers.
+/**
+ * radeon_surface_init - Clear GPU surface registers.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Clear GPU surface registers (r1xx-r5xx).
  */
 void radeon_surface_init(struct radeon_device *rdev)
 {
@@ -119,6 +123,13 @@ void radeon_surface_init(struct radeon_device *rdev)
 /*
  * GPU scratch registers helpers function.
  */
+/**
+ * radeon_scratch_init - Init scratch register driver information.
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Init CP scratch register driver information (r1xx-r5xx)
+ */
 void radeon_scratch_init(struct radeon_device *rdev)
 {
 	int i;
@@ -136,6 +147,15 @@ void radeon_scratch_init(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * radeon_scratch_get - Allocate a scratch register
+ *
+ * @rdev: radeon_device pointer
+ * @reg: scratch register mmio offset
+ *
+ * Allocate a CP scratch register for use by the driver (all asics).
+ * Returns 0 on success or -EINVAL on failure.
+ */
 int radeon_scratch_get(struct radeon_device *rdev, uint32_t *reg)
 {
 	int i;
@@ -150,6 +170,14 @@ int radeon_scratch_get(struct radeon_device *rdev, uint32_t *reg)
 	return -EINVAL;
 }
 
+/**
+ * radeon_scratch_free - Free a scratch register
+ *
+ * @rdev: radeon_device pointer
+ * @reg: scratch register mmio offset
+ *
+ * Free a CP scratch register allocated for use by the driver (all asics)
+ */
 void radeon_scratch_free(struct radeon_device *rdev, uint32_t reg)
 {
 	int i;
@@ -162,6 +190,20 @@ void radeon_scratch_free(struct radeon_device *rdev, uint32_t reg)
 	}
 }
 
+/*
+ * radeon_wb_*()
+ * Writeback is the the method by which the the GPU updates special pages
+ * in memory with the status of certain GPU events (fences, ring pointers,
+ * etc.).
+ */
+
+/**
+ * radeon_wb_disable - Disable Writeback
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Disables Writeback (all asics).  Used for suspend.
+ */
 void radeon_wb_disable(struct radeon_device *rdev)
 {
 	int r;
@@ -177,6 +219,14 @@ void radeon_wb_disable(struct radeon_device *rdev)
 	rdev->wb.enabled = false;
 }
 
+/**
+ * radeon_wb_fini - Disable Writeback and free memory
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Disables Writeback and frees the Writeback memory (all asics).
+ * Used at driver shutdown.
+ */
 void radeon_wb_fini(struct radeon_device *rdev)
 {
 	radeon_wb_disable(rdev);
@@ -187,6 +237,15 @@ void radeon_wb_fini(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * radeon_wb_init- Init Writeback driver info and allocate memory
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Disables Writeback and frees the Writeback memory (all asics).
+ * Used at driver startup.
+ * Returns 0 on success or an -error on failure.
+ */
 int radeon_wb_init(struct radeon_device *rdev)
 {
 	int r;
@@ -355,6 +414,15 @@ void radeon_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc)
 /*
  * GPU helpers function.
  */
+/**
+ * radeon_card_posted - check if the hw has already been initialized
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Check if the asic has been initialized (all asics).
+ * Used at driver startup.
+ * Returns true if initialized or false if not.
+ */
 bool radeon_card_posted(struct radeon_device *rdev)
 {
 	uint32_t reg;
@@ -404,6 +472,14 @@ bool radeon_card_posted(struct radeon_device *rdev)
 
 }
 
+/**
+ * radeon_update_bandwidth_info - update display bandwidth params
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Used when sclk/mclk are switched or display modes are set.
+ * params are used to calculate display watermarks (all asics)
+ */
 void radeon_update_bandwidth_info(struct radeon_device *rdev)
 {
 	fixed20_12 a;
@@ -424,6 +500,15 @@ void radeon_update_bandwidth_info(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * radeon_boot_test_post_card - check and possibly initialize the hw
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Check if the asic is initialized and if not, attempt to initialize
+ * it (all asics).
+ * Returns true if initialized or false if not.
+ */
 bool radeon_boot_test_post_card(struct radeon_device *rdev)
 {
 	if (radeon_card_posted(rdev))
@@ -442,6 +527,16 @@ bool radeon_boot_test_post_card(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * radeon_dummy_page_init - init dummy page used by the driver
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Allocate the dummy page used by the driver (all asics).
+ * This dummy page is used by the driver as a filler for gart entries
+ * when pages are taken out of the GART
+ * Returns 0 on sucess, -ENOMEM on failure.
+ */
 int radeon_dummy_page_init(struct radeon_device *rdev)
 {
 	if (rdev->dummy_page.page)
@@ -460,6 +555,13 @@ int radeon_dummy_page_init(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_dummy_page_fini - free dummy page used by the driver
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Frees the dummy page used by the driver (all asics).
+ */
 void radeon_dummy_page_fini(struct radeon_device *rdev)
 {
 	if (rdev->dummy_page.page == NULL)
@@ -472,6 +574,23 @@ void radeon_dummy_page_fini(struct radeon_device *rdev)
 
 
 /* ATOM accessor methods */
+/*
+ * ATOM is an interpreted byte code stored in tables in the vbios.  The
+ * driver registers callbacks to access registers and the interpreter
+ * in the driver parses the tables and executes then to program specific
+ * actions (set display modes, asic init, etc.).  See radeon_atombios.c,
+ * atombios.h, and atom.c
+ */
+
+/**
+ * cail_pll_read - read PLL register
+ *
+ * @info: atom card_info pointer
+ * @reg: PLL register offset
+ *
+ * Provides a PLL register accessor for the atom interpreter (r4xx+).
+ * Returns the value of the PLL register.
+ */
 static uint32_t cail_pll_read(struct card_info *info, uint32_t reg)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -481,6 +600,15 @@ static uint32_t cail_pll_read(struct card_info *info, uint32_t reg)
 	return r;
 }
 
+/**
+ * cail_pll_write - write PLL register
+ *
+ * @info: atom card_info pointer
+ * @reg: PLL register offset
+ * @val: value to write to the pll register
+ *
+ * Provides a PLL register accessor for the atom interpreter (r4xx+).
+ */
 static void cail_pll_write(struct card_info *info, uint32_t reg, uint32_t val)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -488,6 +616,15 @@ static void cail_pll_write(struct card_info *info, uint32_t reg, uint32_t val)
 	rdev->pll_wreg(rdev, reg, val);
 }
 
+/**
+ * cail_mc_read - read MC (Memory Controller) register
+ *
+ * @info: atom card_info pointer
+ * @reg: MC register offset
+ *
+ * Provides an MC register accessor for the atom interpreter (r4xx+).
+ * Returns the value of the MC register.
+ */
 static uint32_t cail_mc_read(struct card_info *info, uint32_t reg)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -497,6 +634,15 @@ static uint32_t cail_mc_read(struct card_info *info, uint32_t reg)
 	return r;
 }
 
+/**
+ * cail_mc_write - write MC (Memory Controller) register
+ *
+ * @info: atom card_info pointer
+ * @reg: MC register offset
+ * @val: value to write to the pll register
+ *
+ * Provides a MC register accessor for the atom interpreter (r4xx+).
+ */
 static void cail_mc_write(struct card_info *info, uint32_t reg, uint32_t val)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -504,6 +650,15 @@ static void cail_mc_write(struct card_info *info, uint32_t reg, uint32_t val)
 	rdev->mc_wreg(rdev, reg, val);
 }
 
+/**
+ * cail_reg_write - write MMIO register
+ *
+ * @info: atom card_info pointer
+ * @reg: MMIO register offset
+ * @val: value to write to the pll register
+ *
+ * Provides a MMIO register accessor for the atom interpreter (r4xx+).
+ */
 static void cail_reg_write(struct card_info *info, uint32_t reg, uint32_t val)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -511,6 +666,15 @@ static void cail_reg_write(struct card_info *info, uint32_t reg, uint32_t val)
 	WREG32(reg*4, val);
 }
 
+/**
+ * cail_reg_read - read MMIO register
+ *
+ * @info: atom card_info pointer
+ * @reg: MMIO register offset
+ *
+ * Provides an MMIO register accessor for the atom interpreter (r4xx+).
+ * Returns the value of the MMIO register.
+ */
 static uint32_t cail_reg_read(struct card_info *info, uint32_t reg)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -520,6 +684,15 @@ static uint32_t cail_reg_read(struct card_info *info, uint32_t reg)
 	return r;
 }
 
+/**
+ * cail_ioreg_write - write IO register
+ *
+ * @info: atom card_info pointer
+ * @reg: IO register offset
+ * @val: value to write to the pll register
+ *
+ * Provides a IO register accessor for the atom interpreter (r4xx+).
+ */
 static void cail_ioreg_write(struct card_info *info, uint32_t reg, uint32_t val)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -527,6 +700,15 @@ static void cail_ioreg_write(struct card_info *info, uint32_t reg, uint32_t val)
 	WREG32_IO(reg*4, val);
 }
 
+/**
+ * cail_ioreg_read - read IO register
+ *
+ * @info: atom card_info pointer
+ * @reg: IO register offset
+ *
+ * Provides an IO register accessor for the atom interpreter (r4xx+).
+ * Returns the value of the IO register.
+ */
 static uint32_t cail_ioreg_read(struct card_info *info, uint32_t reg)
 {
 	struct radeon_device *rdev = info->dev->dev_private;
@@ -536,6 +718,16 @@ static uint32_t cail_ioreg_read(struct card_info *info, uint32_t reg)
 	return r;
 }
 
+/**
+ * radeon_atombios_init - init the driver info and callbacks for atombios
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Initializes the driver info and register access callbacks for the
+ * ATOM interpreter (r4xx+).
+ * Returns 0 on sucess, -ENOMEM on failure.
+ * Called at driver startup.
+ */
 int radeon_atombios_init(struct radeon_device *rdev)
 {
 	struct card_info *atom_card_info =
@@ -569,6 +761,15 @@ int radeon_atombios_init(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_atombios_fini - free the driver info and callbacks for atombios
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Frees the driver info and register access callbacks for the ATOM
+ * interpreter (r4xx+).
+ * Called at driver shutdown.
+ */
 void radeon_atombios_fini(struct radeon_device *rdev)
 {
 	if (rdev->mode_info.atom_context) {
@@ -578,17 +779,50 @@ void radeon_atombios_fini(struct radeon_device *rdev)
 	kfree(rdev->mode_info.atom_card_info);
 }
 
+/* COMBIOS */
+/*
+ * COMBIOS is the bios format prior to ATOM. It provides
+ * command tables similar to ATOM, but doesn't have a unified
+ * parser.  See radeon_combios.c
+ */
+
+/**
+ * radeon_combios_init - init the driver info for combios
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Initializes the driver info for combios (r1xx-r3xx).
+ * Returns 0 on sucess.
+ * Called at driver startup.
+ */
 int radeon_combios_init(struct radeon_device *rdev)
 {
 	radeon_combios_initialize_bios_scratch_regs(rdev->ddev);
 	return 0;
 }
 
+/**
+ * radeon_combios_fini - free the driver info for combios
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Frees the driver info for combios (r1xx-r3xx).
+ * Called at driver shutdown.
+ */
 void radeon_combios_fini(struct radeon_device *rdev)
 {
 }
 
-/* if we get transitioned to only one device, tak VGA back */
+/* if we get transitioned to only one device, take VGA back */
+/**
+ * radeon_vga_set_decode - enable/disable vga decode
+ *
+ * @cookie: radeon_device pointer
+ * @state: enable/disable vga decode
+ *
+ * Enable/disable vga decode (all asics).
+ * Returns VGA resource flags.
+ */
 static unsigned int radeon_vga_set_decode(void *cookie, bool state)
 {
 	struct radeon_device *rdev = cookie;
@@ -600,6 +834,14 @@ static unsigned int radeon_vga_set_decode(void *cookie, bool state)
 		return VGA_RSRC_NORMAL_IO | VGA_RSRC_NORMAL_MEM;
 }
 
+/**
+ * radeon_check_arguments - validate module params
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Validates certain module parameters and updates
+ * the associated values used by the driver (all asics).
+ */
 void radeon_check_arguments(struct radeon_device *rdev)
 {
 	/* vramlimit must be a power of two */
@@ -666,6 +908,15 @@ void radeon_check_arguments(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * radeon_switcheroo_set_state - set switcheroo state
+ *
+ * @pdev: pci dev pointer
+ * @state: vga switcheroo state
+ *
+ * Callback for the switcheroo driver.  Suspends or resumes the
+ * the asics before or after it is powered up using ACPI methods.
+ */
 static void radeon_switcheroo_set_state(struct pci_dev *pdev, enum vga_switcheroo_state state)
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
@@ -686,6 +937,15 @@ static void radeon_switcheroo_set_state(struct pci_dev *pdev, enum vga_switchero
 	}
 }
 
+/**
+ * radeon_switcheroo_can_switch - see if switcheroo state can change
+ *
+ * @pdev: pci dev pointer
+ *
+ * Callback for the switcheroo driver.  Check of the switcheroo
+ * state can be changed.
+ * Returns true if the state can be changed, false if not.
+ */
 static bool radeon_switcheroo_can_switch(struct pci_dev *pdev)
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
@@ -703,6 +963,18 @@ static const struct vga_switcheroo_client_ops radeon_switcheroo_ops = {
 	.can_switch = radeon_switcheroo_can_switch,
 };
 
+/**
+ * radeon_device_init - initialize the driver
+ *
+ * @rdev: radeon_device pointer
+ * @pdev: drm dev pointer
+ * @pdev: pci dev pointer
+ * @flags: driver flags
+ *
+ * Initializes the driver info and hw (all asics).
+ * Returns 0 for success or an error on failure.
+ * Called at driver startup.
+ */
 int radeon_device_init(struct radeon_device *rdev,
 		       struct drm_device *ddev,
 		       struct pci_dev *pdev,
@@ -721,6 +993,10 @@ int radeon_device_init(struct radeon_device *rdev,
 	rdev->usec_timeout = RADEON_MAX_USEC_TIMEOUT;
 	rdev->mc.gtt_size = radeon_gart_size * 1024 * 1024;
 	rdev->accel_working = false;
+	/* set up ring ids */
+	for (i = 0; i < RADEON_NUM_RINGS; i++) {
+		rdev->ring[i].idx = i;
+	}
 
 	DRM_INFO("initializing kernel modesetting (%s 0x%04X:0x%04X 0x%04X:0x%04X).\n",
 		radeon_family_name[rdev->family], pdev->vendor, pdev->device,
@@ -728,20 +1004,20 @@ int radeon_device_init(struct radeon_device *rdev,
 
 	/* mutex initialization are all done here so we
 	 * can recall function without having locking issues */
-	radeon_mutex_init(&rdev->cs_mutex);
 	mutex_init(&rdev->ring_lock);
 	mutex_init(&rdev->dc_hw_i2c_mutex);
-	if (rdev->family >= CHIP_R600)
-		spin_lock_init(&rdev->ih.lock);
+	atomic_set(&rdev->ih.lock, 0);
 	mutex_init(&rdev->gem.mutex);
 	mutex_init(&rdev->pm.mutex);
-	mutex_init(&rdev->vram_mutex);
+	init_rwsem(&rdev->pm.mclk_lock);
+	init_rwsem(&rdev->exclusive_lock);
 	init_waitqueue_head(&rdev->irq.vblank_queue);
 	init_waitqueue_head(&rdev->irq.idle_queue);
 	r = radeon_gem_init(rdev);
 	if (r)
 		return r;
 	/* initialize vm here */
+	mutex_init(&rdev->vm_manager.lock);
 	rdev->vm_manager.use_bitmap = 1;
 	rdev->vm_manager.max_pfn = 1 << 20;
 	INIT_LIST_HEAD(&rdev->vm_manager.lru_vm);
@@ -822,6 +1098,10 @@ int radeon_device_init(struct radeon_device *rdev,
 	if (r)
 		return r;
 
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
+		DRM_ERROR("ib ring test failed (%d).\n", r);
+
 	if (rdev->flags & RADEON_IS_AGP && !rdev->accel_working) {
 		/* Acceleration not working on AGP card try again
 		 * with fallback to PCI or PCIE GART
@@ -847,6 +1127,14 @@ int radeon_device_init(struct radeon_device *rdev,
 
 static void radeon_debugfs_remove_files(struct radeon_device *rdev);
 
+/**
+ * radeon_device_fini - tear down the driver
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Tear down the driver info (all asics).
+ * Called at driver shutdown.
+ */
 void radeon_device_fini(struct radeon_device *rdev)
 {
 	DRM_INFO("radeon: finishing device.\n");
@@ -868,6 +1156,16 @@ void radeon_device_fini(struct radeon_device *rdev)
 /*
  * Suspend & resume.
  */
+/**
+ * radeon_suspend_kms - initiate device suspend
+ *
+ * @pdev: drm dev pointer
+ * @state: suspend state
+ *
+ * Puts the hw in the suspend state (all asics).
+ * Returns 0 for success or an error on failure.
+ * Called at driver suspend.
+ */
 int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 {
 	struct radeon_device *rdev;
@@ -942,10 +1240,20 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 	return 0;
 }
 
+/**
+ * radeon_resume_kms - initiate device resume
+ *
+ * @pdev: drm dev pointer
+ *
+ * Bring the hw back to operating state (all asics).
+ * Returns 0 for success or an error on failure.
+ * Called at driver resume.
+ */
 int radeon_resume_kms(struct drm_device *dev)
 {
 	struct drm_connector *connector;
 	struct radeon_device *rdev = dev->dev_private;
+	int r;
 
 	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
@@ -960,6 +1268,11 @@ int radeon_resume_kms(struct drm_device *dev)
 	/* resume AGP if in use */
 	radeon_agp_resume(rdev);
 	radeon_resume(rdev);
+
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
+		DRM_ERROR("ib ring test failed (%d).\n", r);
+
 	radeon_pm_resume(rdev);
 	radeon_restore_bios_scratch_regs(rdev);
 
@@ -984,30 +1297,77 @@ int radeon_resume_kms(struct drm_device *dev)
 	return 0;
 }
 
+/**
+ * radeon_gpu_reset - reset the asic
+ *
+ * @rdev: radeon device pointer
+ *
+ * Attempt the reset the GPU if it has hung (all asics).
+ * Returns 0 for success or an error on failure.
+ */
 int radeon_gpu_reset(struct radeon_device *rdev)
 {
-	int r;
+	unsigned ring_sizes[RADEON_NUM_RINGS];
+	uint32_t *ring_data[RADEON_NUM_RINGS];
+
+	bool saved = false;
+
+	int i, r;
 	int resched;
 
+	down_write(&rdev->exclusive_lock);
 	radeon_save_bios_scratch_regs(rdev);
 	/* block TTM */
 	resched = ttm_bo_lock_delayed_workqueue(&rdev->mman.bdev);
 	radeon_suspend(rdev);
 
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		ring_sizes[i] = radeon_ring_backup(rdev, &rdev->ring[i],
+						   &ring_data[i]);
+		if (ring_sizes[i]) {
+			saved = true;
+			dev_info(rdev->dev, "Saved %d dwords of commands "
+				 "on ring %d.\n", ring_sizes[i], i);
+		}
+	}
+
+retry:
 	r = radeon_asic_reset(rdev);
 	if (!r) {
-		dev_info(rdev->dev, "GPU reset succeed\n");
+		dev_info(rdev->dev, "GPU reset succeeded, trying to resume\n");
 		radeon_resume(rdev);
-		radeon_restore_bios_scratch_regs(rdev);
-		drm_helper_resume_force_mode(rdev->ddev);
-		ttm_bo_unlock_delayed_workqueue(&rdev->mman.bdev, resched);
 	}
 
+	radeon_restore_bios_scratch_regs(rdev);
+	drm_helper_resume_force_mode(rdev->ddev);
+
+	if (!r) {
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			radeon_ring_restore(rdev, &rdev->ring[i],
+					    ring_sizes[i], ring_data[i]);
+		}
+
+		r = radeon_ib_ring_tests(rdev);
+		if (r) {
+			dev_err(rdev->dev, "ib ring test failed (%d).\n", r);
+			if (saved) {
+				radeon_suspend(rdev);
+				goto retry;
+			}
+		}
+	} else {
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			kfree(ring_data[i]);
+		}
+	}
+
+	ttm_bo_unlock_delayed_workqueue(&rdev->mman.bdev, resched);
 	if (r) {
 		/* bad news, how to tell it to userspace ? */
 		dev_info(rdev->dev, "GPU reset failed\n");
 	}
 
+	up_write(&rdev->exclusive_lock);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 64a008d1449..7ddef8f30d0 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -1401,7 +1401,7 @@ void radeon_modeset_fini(struct radeon_device *rdev)
 	radeon_i2c_fini(rdev);
 }
 
-static bool is_hdtv_mode(struct drm_display_mode *mode)
+static bool is_hdtv_mode(const struct drm_display_mode *mode)
 {
 	/* try and guess if this is a tv or a monitor */
 	if ((mode->vdisplay == 480 && mode->hdisplay == 720) || /* 480p */
@@ -1414,7 +1414,7 @@ static bool is_hdtv_mode(struct drm_display_mode *mode)
 }
 
 bool radeon_crtc_scaling_mode_fixup(struct drm_crtc *crtc,
-				struct drm_display_mode *mode,
+				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = crtc->dev;
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 2c4d53fd20c..dcea6f01ae4 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -133,7 +133,7 @@ int radeon_tv = 1;
 int radeon_audio = 0;
 int radeon_disp_priority = 0;
 int radeon_hw_i2c = 0;
-int radeon_pcie_gen2 = 0;
+int radeon_pcie_gen2 = -1;
 int radeon_msi = -1;
 int radeon_lockup_timeout = 10000;
 
@@ -179,7 +179,7 @@ module_param_named(disp_priority, radeon_disp_priority, int, 0444);
 MODULE_PARM_DESC(hw_i2c, "hw i2c engine enable (0 = disable)");
 module_param_named(hw_i2c, radeon_hw_i2c, int, 0444);
 
-MODULE_PARM_DESC(pcie_gen2, "PCIE Gen2 mode (1 = enable)");
+MODULE_PARM_DESC(pcie_gen2, "PCIE Gen2 mode (-1 = auto, 0 = disable, 1 = enable)");
 module_param_named(pcie_gen2, radeon_pcie_gen2, int, 0444);
 
 MODULE_PARM_DESC(msi, "MSI support (1 = enable, 0 = disable, -1 = auto)");
@@ -262,7 +262,6 @@ static struct drm_driver driver_old = {
 	.irq_postinstall = radeon_driver_irq_postinstall,
 	.irq_uninstall = radeon_driver_irq_uninstall,
 	.irq_handler = radeon_driver_irq_handler,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = radeon_ioctls,
 	.dma_ioctl = radeon_cp_buffers,
 	.fops = &radeon_driver_old_fops,
@@ -365,7 +364,6 @@ static struct drm_driver kms_driver = {
 	.irq_postinstall = radeon_driver_irq_postinstall_kms,
 	.irq_uninstall = radeon_driver_irq_uninstall_kms,
 	.irq_handler = radeon_driver_irq_handler_kms,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = radeon_ioctls_kms,
 	.gem_init_object = radeon_gem_object_init,
 	.gem_free_object = radeon_gem_object_free,
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 11f5f402d22..7b737b9339a 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -40,39 +40,95 @@
 #include "radeon.h"
 #include "radeon_trace.h"
 
+/*
+ * Fences
+ * Fences mark an event in the GPUs pipeline and are used
+ * for GPU/CPU synchronization.  When the fence is written,
+ * it is expected that all buffers associated with that fence
+ * are no longer in use by the associated ring on the GPU and
+ * that the the relevant GPU caches have been flushed.  Whether
+ * we use a scratch register or memory location depends on the asic
+ * and whether writeback is enabled.
+ */
+
+/**
+ * radeon_fence_write - write a fence value
+ *
+ * @rdev: radeon_device pointer
+ * @seq: sequence number to write
+ * @ring: ring index the fence is associated with
+ *
+ * Writes a fence value to memory or a scratch register (all asics).
+ */
 static void radeon_fence_write(struct radeon_device *rdev, u32 seq, int ring)
 {
-	if (rdev->wb.enabled) {
-		*rdev->fence_drv[ring].cpu_addr = cpu_to_le32(seq);
+	struct radeon_fence_driver *drv = &rdev->fence_drv[ring];
+	if (likely(rdev->wb.enabled || !drv->scratch_reg)) {
+		*drv->cpu_addr = cpu_to_le32(seq);
 	} else {
-		WREG32(rdev->fence_drv[ring].scratch_reg, seq);
+		WREG32(drv->scratch_reg, seq);
 	}
 }
 
+/**
+ * radeon_fence_read - read a fence value
+ *
+ * @rdev: radeon_device pointer
+ * @ring: ring index the fence is associated with
+ *
+ * Reads a fence value from memory or a scratch register (all asics).
+ * Returns the value of the fence read from memory or register.
+ */
 static u32 radeon_fence_read(struct radeon_device *rdev, int ring)
 {
+	struct radeon_fence_driver *drv = &rdev->fence_drv[ring];
 	u32 seq = 0;
 
-	if (rdev->wb.enabled) {
-		seq = le32_to_cpu(*rdev->fence_drv[ring].cpu_addr);
+	if (likely(rdev->wb.enabled || !drv->scratch_reg)) {
+		seq = le32_to_cpu(*drv->cpu_addr);
 	} else {
-		seq = RREG32(rdev->fence_drv[ring].scratch_reg);
+		seq = RREG32(drv->scratch_reg);
 	}
 	return seq;
 }
 
-int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence)
+/**
+ * radeon_fence_emit - emit a fence on the requested ring
+ *
+ * @rdev: radeon_device pointer
+ * @fence: radeon fence object
+ * @ring: ring index the fence is associated with
+ *
+ * Emits a fence command on the requested ring (all asics).
+ * Returns 0 on success, -ENOMEM on failure.
+ */
+int radeon_fence_emit(struct radeon_device *rdev,
+		      struct radeon_fence **fence,
+		      int ring)
 {
 	/* we are protected by the ring emission mutex */
-	if (fence->seq && fence->seq < RADEON_FENCE_NOTEMITED_SEQ) {
-		return 0;
+	*fence = kmalloc(sizeof(struct radeon_fence), GFP_KERNEL);
+	if ((*fence) == NULL) {
+		return -ENOMEM;
 	}
-	fence->seq = ++rdev->fence_drv[fence->ring].seq;
-	radeon_fence_ring_emit(rdev, fence->ring, fence);
-	trace_radeon_fence_emit(rdev->ddev, fence->seq);
+	kref_init(&((*fence)->kref));
+	(*fence)->rdev = rdev;
+	(*fence)->seq = ++rdev->fence_drv[ring].sync_seq[ring];
+	(*fence)->ring = ring;
+	radeon_fence_ring_emit(rdev, ring, *fence);
+	trace_radeon_fence_emit(rdev->ddev, (*fence)->seq);
 	return 0;
 }
 
+/**
+ * radeon_fence_process - process a fence
+ *
+ * @rdev: radeon_device pointer
+ * @ring: ring index the fence is associated with
+ *
+ * Checks the current fence value and wakes the fence queue
+ * if the sequence number has increased (all asics).
+ */
 void radeon_fence_process(struct radeon_device *rdev, int ring)
 {
 	uint64_t seq, last_seq;
@@ -133,30 +189,35 @@ void radeon_fence_process(struct radeon_device *rdev, int ring)
 	}
 }
 
+/**
+ * radeon_fence_destroy - destroy a fence
+ *
+ * @kref: fence kref
+ *
+ * Frees the fence object (all asics).
+ */
 static void radeon_fence_destroy(struct kref *kref)
 {
 	struct radeon_fence *fence;
 
 	fence = container_of(kref, struct radeon_fence, kref);
-	fence->seq = RADEON_FENCE_NOTEMITED_SEQ;
 	kfree(fence);
 }
 
-int radeon_fence_create(struct radeon_device *rdev,
-			struct radeon_fence **fence,
-			int ring)
-{
-	*fence = kmalloc(sizeof(struct radeon_fence), GFP_KERNEL);
-	if ((*fence) == NULL) {
-		return -ENOMEM;
-	}
-	kref_init(&((*fence)->kref));
-	(*fence)->rdev = rdev;
-	(*fence)->seq = RADEON_FENCE_NOTEMITED_SEQ;
-	(*fence)->ring = ring;
-	return 0;
-}
-
+/**
+ * radeon_fence_seq_signaled - check if a fence sequeuce number has signaled
+ *
+ * @rdev: radeon device pointer
+ * @seq: sequence number
+ * @ring: ring index the fence is associated with
+ *
+ * Check if the last singled fence sequnce number is >= the requested
+ * sequence number (all asics).
+ * Returns true if the fence has signaled (current fence value
+ * is >= requested value) or false if it has not (current fence
+ * value is < the requested value.  Helper function for
+ * radeon_fence_signaled().
+ */
 static bool radeon_fence_seq_signaled(struct radeon_device *rdev,
 				      u64 seq, unsigned ring)
 {
@@ -171,15 +232,19 @@ static bool radeon_fence_seq_signaled(struct radeon_device *rdev,
 	return false;
 }
 
+/**
+ * radeon_fence_signaled - check if a fence has signaled
+ *
+ * @fence: radeon fence object
+ *
+ * Check if the requested fence has signaled (all asics).
+ * Returns true if the fence has signaled or false if it has not.
+ */
 bool radeon_fence_signaled(struct radeon_fence *fence)
 {
 	if (!fence) {
 		return true;
 	}
-	if (fence->seq == RADEON_FENCE_NOTEMITED_SEQ) {
-		WARN(1, "Querying an unemitted fence : %p !\n", fence);
-		return true;
-	}
 	if (fence->seq == RADEON_FENCE_SIGNALED_SEQ) {
 		return true;
 	}
@@ -190,6 +255,24 @@ bool radeon_fence_signaled(struct radeon_fence *fence)
 	return false;
 }
 
+/**
+ * radeon_fence_wait_seq - wait for a specific sequence number
+ *
+ * @rdev: radeon device pointer
+ * @target_seq: sequence number we want to wait for
+ * @ring: ring index the fence is associated with
+ * @intr: use interruptable sleep
+ * @lock_ring: whether the ring should be locked or not
+ *
+ * Wait for the requested sequence number to be written (all asics).
+ * @intr selects whether to use interruptable (true) or non-interruptable
+ * (false) sleep when waiting for the sequence number.  Helper function
+ * for radeon_fence_wait(), et al.
+ * Returns 0 if the sequence number has passed, error for all other cases.
+ * -EDEADLK is returned when a GPU lockup has been detected and the ring is
+ * marked as not ready so no further jobs get scheduled until a successful
+ * reset.
+ */
 static int radeon_fence_wait_seq(struct radeon_device *rdev, u64 target_seq,
 				 unsigned ring, bool intr, bool lock_ring)
 {
@@ -285,6 +368,17 @@ static int radeon_fence_wait_seq(struct radeon_device *rdev, u64 target_seq,
 	return 0;
 }
 
+/**
+ * radeon_fence_wait - wait for a fence to signal
+ *
+ * @fence: radeon fence object
+ * @intr: use interruptable sleep
+ *
+ * Wait for the requested fence to signal (all asics).
+ * @intr selects whether to use interruptable (true) or non-interruptable
+ * (false) sleep when waiting for the fence.
+ * Returns 0 if the fence has passed, error for all other cases.
+ */
 int radeon_fence_wait(struct radeon_fence *fence, bool intr)
 {
 	int r;
@@ -315,6 +409,20 @@ bool radeon_fence_any_seq_signaled(struct radeon_device *rdev, u64 *seq)
 	return false;
 }
 
+/**
+ * radeon_fence_wait_any_seq - wait for a sequence number on any ring
+ *
+ * @rdev: radeon device pointer
+ * @target_seq: sequence number(s) we want to wait for
+ * @intr: use interruptable sleep
+ *
+ * Wait for the requested sequence number(s) to be written by any ring
+ * (all asics).  Sequnce number array is indexed by ring id.
+ * @intr selects whether to use interruptable (true) or non-interruptable
+ * (false) sleep when waiting for the sequence number.  Helper function
+ * for radeon_fence_wait_any(), et al.
+ * Returns 0 if the sequence number has passed, error for all other cases.
+ */
 static int radeon_fence_wait_any_seq(struct radeon_device *rdev,
 				     u64 *target_seq, bool intr)
 {
@@ -343,7 +451,7 @@ static int radeon_fence_wait_any_seq(struct radeon_device *rdev,
 
 	/* nothing to wait for ? */
 	if (ring == RADEON_NUM_RINGS) {
-		return 0;
+		return -ENOENT;
 	}
 
 	while (!radeon_fence_any_seq_signaled(rdev, target_seq)) {
@@ -424,6 +532,19 @@ static int radeon_fence_wait_any_seq(struct radeon_device *rdev,
 	return 0;
 }
 
+/**
+ * radeon_fence_wait_any - wait for a fence to signal on any ring
+ *
+ * @rdev: radeon device pointer
+ * @fences: radeon fence object(s)
+ * @intr: use interruptable sleep
+ *
+ * Wait for any requested fence to signal (all asics).  Fence
+ * array is indexed by ring id.  @intr selects whether to use
+ * interruptable (true) or non-interruptable (false) sleep when
+ * waiting for the fences. Used by the suballocator.
+ * Returns 0 if any fence has passed, error for all other cases.
+ */
 int radeon_fence_wait_any(struct radeon_device *rdev,
 			  struct radeon_fence **fences,
 			  bool intr)
@@ -444,9 +565,7 @@ int radeon_fence_wait_any(struct radeon_device *rdev,
 			return 0;
 		}
 
-		if (fences[i]->seq < RADEON_FENCE_NOTEMITED_SEQ) {
-			seq[i] = fences[i]->seq;
-		}
+		seq[i] = fences[i]->seq;
 	}
 
 	r = radeon_fence_wait_any_seq(rdev, seq, intr);
@@ -456,16 +575,22 @@ int radeon_fence_wait_any(struct radeon_device *rdev,
 	return 0;
 }
 
+/**
+ * radeon_fence_wait_next_locked - wait for the next fence to signal
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring index the fence is associated with
+ *
+ * Wait for the next fence on the requested ring to signal (all asics).
+ * Returns 0 if the next fence has passed, error for all other cases.
+ * Caller must hold ring lock.
+ */
 int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring)
 {
 	uint64_t seq;
 
-	/* We are not protected by ring lock when reading current seq but
-	 * it's ok as worst case is we return to early while we could have
-	 * wait.
-	 */
 	seq = atomic64_read(&rdev->fence_drv[ring].last_seq) + 1ULL;
-	if (seq >= rdev->fence_drv[ring].seq) {
+	if (seq >= rdev->fence_drv[ring].sync_seq[ring]) {
 		/* nothing to wait for, last_seq is
 		   already the last emited fence */
 		return -ENOENT;
@@ -473,23 +598,59 @@ int radeon_fence_wait_next_locked(struct radeon_device *rdev, int ring)
 	return radeon_fence_wait_seq(rdev, seq, ring, false, false);
 }
 
-int radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring)
+/**
+ * radeon_fence_wait_empty_locked - wait for all fences to signal
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring index the fence is associated with
+ *
+ * Wait for all fences on the requested ring to signal (all asics).
+ * Returns 0 if the fences have passed, error for all other cases.
+ * Caller must hold ring lock.
+ */
+void radeon_fence_wait_empty_locked(struct radeon_device *rdev, int ring)
 {
-	/* We are not protected by ring lock when reading current seq
-	 * but it's ok as wait empty is call from place where no more
-	 * activity can be scheduled so there won't be concurrent access
-	 * to seq value.
-	 */
-	return radeon_fence_wait_seq(rdev, rdev->fence_drv[ring].seq,
-				     ring, false, false);
+	uint64_t seq = rdev->fence_drv[ring].sync_seq[ring];
+
+	while(1) {
+		int r;
+		r = radeon_fence_wait_seq(rdev, seq, ring, false, false);
+		if (r == -EDEADLK) {
+			mutex_unlock(&rdev->ring_lock);
+			r = radeon_gpu_reset(rdev);
+			mutex_lock(&rdev->ring_lock);
+			if (!r)
+				continue;
+		}
+		if (r) {
+			dev_err(rdev->dev, "error waiting for ring to become"
+				" idle (%d)\n", r);
+		}
+		return;
+	}
 }
 
+/**
+ * radeon_fence_ref - take a ref on a fence
+ *
+ * @fence: radeon fence object
+ *
+ * Take a reference on a fence (all asics).
+ * Returns the fence.
+ */
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence)
 {
 	kref_get(&fence->kref);
 	return fence;
 }
 
+/**
+ * radeon_fence_unref - remove a ref on a fence
+ *
+ * @fence: radeon fence object
+ *
+ * Remove a reference on a fence (all asics).
+ */
 void radeon_fence_unref(struct radeon_fence **fence)
 {
 	struct radeon_fence *tmp = *fence;
@@ -500,6 +661,16 @@ void radeon_fence_unref(struct radeon_fence **fence)
 	}
 }
 
+/**
+ * radeon_fence_count_emitted - get the count of emitted fences
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring index the fence is associated with
+ *
+ * Get the number of fences emitted on the requested ring (all asics).
+ * Returns the number of emitted fences on the ring.  Used by the
+ * dynpm code to ring track activity.
+ */
 unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
 {
 	uint64_t emitted;
@@ -508,7 +679,8 @@ unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
 	 * but it's ok to report slightly wrong fence count here.
 	 */
 	radeon_fence_process(rdev, ring);
-	emitted = rdev->fence_drv[ring].seq - atomic64_read(&rdev->fence_drv[ring].last_seq);
+	emitted = rdev->fence_drv[ring].sync_seq[ring]
+		- atomic64_read(&rdev->fence_drv[ring].last_seq);
 	/* to avoid 32bits warp around */
 	if (emitted > 0x10000000) {
 		emitted = 0x10000000;
@@ -516,6 +688,83 @@ unsigned radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
 	return (unsigned)emitted;
 }
 
+/**
+ * radeon_fence_need_sync - do we need a semaphore
+ *
+ * @fence: radeon fence object
+ * @dst_ring: which ring to check against
+ *
+ * Check if the fence needs to be synced against another ring
+ * (all asics).  If so, we need to emit a semaphore.
+ * Returns true if we need to sync with another ring, false if
+ * not.
+ */
+bool radeon_fence_need_sync(struct radeon_fence *fence, int dst_ring)
+{
+	struct radeon_fence_driver *fdrv;
+
+	if (!fence) {
+		return false;
+	}
+
+	if (fence->ring == dst_ring) {
+		return false;
+	}
+
+	/* we are protected by the ring mutex */
+	fdrv = &fence->rdev->fence_drv[dst_ring];
+	if (fence->seq <= fdrv->sync_seq[fence->ring]) {
+		return false;
+	}
+
+	return true;
+}
+
+/**
+ * radeon_fence_note_sync - record the sync point
+ *
+ * @fence: radeon fence object
+ * @dst_ring: which ring to check against
+ *
+ * Note the sequence number at which point the fence will
+ * be synced with the requested ring (all asics).
+ */
+void radeon_fence_note_sync(struct radeon_fence *fence, int dst_ring)
+{
+	struct radeon_fence_driver *dst, *src;
+	unsigned i;
+
+	if (!fence) {
+		return;
+	}
+
+	if (fence->ring == dst_ring) {
+		return;
+	}
+
+	/* we are protected by the ring mutex */
+	src = &fence->rdev->fence_drv[fence->ring];
+	dst = &fence->rdev->fence_drv[dst_ring];
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (i == dst_ring) {
+			continue;
+		}
+		dst->sync_seq[i] = max(dst->sync_seq[i], src->sync_seq[i]);
+	}
+}
+
+/**
+ * radeon_fence_driver_start_ring - make the fence driver
+ * ready for use on the requested ring.
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring index to start the fence driver on
+ *
+ * Make the fence driver ready for processing (all asics).
+ * Not all asics have all rings, so each asic will only
+ * start the fence driver on the rings it has.
+ * Returns 0 for success, errors for failure.
+ */
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 {
 	uint64_t index;
@@ -537,24 +786,49 @@ int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 	}
 	rdev->fence_drv[ring].cpu_addr = &rdev->wb.wb[index/4];
 	rdev->fence_drv[ring].gpu_addr = rdev->wb.gpu_addr + index;
-	radeon_fence_write(rdev, rdev->fence_drv[ring].seq, ring);
+	radeon_fence_write(rdev, atomic64_read(&rdev->fence_drv[ring].last_seq), ring);
 	rdev->fence_drv[ring].initialized = true;
 	dev_info(rdev->dev, "fence driver on ring %d use gpu addr 0x%016llx and cpu addr 0x%p\n",
 		 ring, rdev->fence_drv[ring].gpu_addr, rdev->fence_drv[ring].cpu_addr);
 	return 0;
 }
 
+/**
+ * radeon_fence_driver_init_ring - init the fence driver
+ * for the requested ring.
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring index to start the fence driver on
+ *
+ * Init the fence driver for the requested ring (all asics).
+ * Helper function for radeon_fence_driver_init().
+ */
 static void radeon_fence_driver_init_ring(struct radeon_device *rdev, int ring)
 {
+	int i;
+
 	rdev->fence_drv[ring].scratch_reg = -1;
 	rdev->fence_drv[ring].cpu_addr = NULL;
 	rdev->fence_drv[ring].gpu_addr = 0;
-	rdev->fence_drv[ring].seq = 0;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i)
+		rdev->fence_drv[ring].sync_seq[i] = 0;
 	atomic64_set(&rdev->fence_drv[ring].last_seq, 0);
 	rdev->fence_drv[ring].last_activity = jiffies;
 	rdev->fence_drv[ring].initialized = false;
 }
 
+/**
+ * radeon_fence_driver_init - init the fence driver
+ * for all possible rings.
+ *
+ * @rdev: radeon device pointer
+ *
+ * Init the fence driver for all possible rings (all asics).
+ * Not all asics have all rings, so each asic will only
+ * start the fence driver on the rings it has using
+ * radeon_fence_driver_start_ring().
+ * Returns 0 for success.
+ */
 int radeon_fence_driver_init(struct radeon_device *rdev)
 {
 	int ring;
@@ -569,6 +843,14 @@ int radeon_fence_driver_init(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_fence_driver_fini - tear down the fence driver
+ * for all possible rings.
+ *
+ * @rdev: radeon device pointer
+ *
+ * Tear down the fence driver for all possible rings (all asics).
+ */
 void radeon_fence_driver_fini(struct radeon_device *rdev)
 {
 	int ring;
@@ -595,7 +877,7 @@ static int radeon_debugfs_fence_info(struct seq_file *m, void *data)
 	struct drm_info_node *node = (struct drm_info_node *)m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct radeon_device *rdev = dev->dev_private;
-	int i;
+	int i, j;
 
 	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
 		if (!rdev->fence_drv[i].initialized)
@@ -604,8 +886,14 @@ static int radeon_debugfs_fence_info(struct seq_file *m, void *data)
 		seq_printf(m, "--- ring %d ---\n", i);
 		seq_printf(m, "Last signaled fence 0x%016llx\n",
 			   (unsigned long long)atomic64_read(&rdev->fence_drv[i].last_seq));
-		seq_printf(m, "Last emitted  0x%016llx\n",
-			   rdev->fence_drv[i].seq);
+		seq_printf(m, "Last emitted        0x%016llx\n",
+			   rdev->fence_drv[i].sync_seq[i]);
+
+		for (j = 0; j < RADEON_NUM_RINGS; ++j) {
+			if (i != j && rdev->fence_drv[j].initialized)
+				seq_printf(m, "Last sync to ring %d 0x%016llx\n",
+					   j, rdev->fence_drv[i].sync_seq[j]);
+		}
 	}
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index 84b648a7ddd..b3720054614 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -31,8 +31,38 @@
 #include "radeon_reg.h"
 
 /*
+ * GART
+ * The GART (Graphics Aperture Remapping Table) is an aperture
+ * in the GPU's address space.  System pages can be mapped into
+ * the aperture and look like contiguous pages from the GPU's
+ * perspective.  A page table maps the pages in the aperture
+ * to the actual backing pages in system memory.
+ *
+ * Radeon GPUs support both an internal GART, as described above,
+ * and AGP.  AGP works similarly, but the GART table is configured
+ * and maintained by the northbridge rather than the driver.
+ * Radeon hw has a separate AGP aperture that is programmed to
+ * point to the AGP aperture provided by the northbridge and the
+ * requests are passed through to the northbridge aperture.
+ * Both AGP and internal GART can be used at the same time, however
+ * that is not currently supported by the driver.
+ *
+ * This file handles the common internal GART management.
+ */
+
+/*
  * Common GART table functions.
  */
+/**
+ * radeon_gart_table_ram_alloc - allocate system ram for gart page table
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Allocate system memory for GART page table
+ * (r1xx-r3xx, non-pcie r4xx, rs400).  These asics require the
+ * gart table to be in system memory.
+ * Returns 0 for success, -ENOMEM for failure.
+ */
 int radeon_gart_table_ram_alloc(struct radeon_device *rdev)
 {
 	void *ptr;
@@ -54,6 +84,15 @@ int radeon_gart_table_ram_alloc(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_gart_table_ram_free - free system ram for gart page table
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Free system memory for GART page table
+ * (r1xx-r3xx, non-pcie r4xx, rs400).  These asics require the
+ * gart table to be in system memory.
+ */
 void radeon_gart_table_ram_free(struct radeon_device *rdev)
 {
 	if (rdev->gart.ptr == NULL) {
@@ -73,6 +112,16 @@ void radeon_gart_table_ram_free(struct radeon_device *rdev)
 	rdev->gart.table_addr = 0;
 }
 
+/**
+ * radeon_gart_table_vram_alloc - allocate vram for gart page table
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Allocate video memory for GART page table
+ * (pcie r4xx, r5xx+).  These asics require the
+ * gart table to be in video memory.
+ * Returns 0 for success, error for failure.
+ */
 int radeon_gart_table_vram_alloc(struct radeon_device *rdev)
 {
 	int r;
@@ -88,6 +137,16 @@ int radeon_gart_table_vram_alloc(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_gart_table_vram_pin - pin gart page table in vram
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Pin the GART page table in vram so it will not be moved
+ * by the memory manager (pcie r4xx, r5xx+).  These asics require the
+ * gart table to be in video memory.
+ * Returns 0 for success, error for failure.
+ */
 int radeon_gart_table_vram_pin(struct radeon_device *rdev)
 {
 	uint64_t gpu_addr;
@@ -110,6 +169,14 @@ int radeon_gart_table_vram_pin(struct radeon_device *rdev)
 	return r;
 }
 
+/**
+ * radeon_gart_table_vram_unpin - unpin gart page table in vram
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Unpin the GART page table in vram (pcie r4xx, r5xx+).
+ * These asics require the gart table to be in video memory.
+ */
 void radeon_gart_table_vram_unpin(struct radeon_device *rdev)
 {
 	int r;
@@ -126,6 +193,15 @@ void radeon_gart_table_vram_unpin(struct radeon_device *rdev)
 	}
 }
 
+/**
+ * radeon_gart_table_vram_free - free gart page table vram
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Free the video memory used for the GART page table
+ * (pcie r4xx, r5xx+).  These asics require the gart table to
+ * be in video memory.
+ */
 void radeon_gart_table_vram_free(struct radeon_device *rdev)
 {
 	if (rdev->gart.robj == NULL) {
@@ -135,12 +211,19 @@ void radeon_gart_table_vram_free(struct radeon_device *rdev)
 	radeon_bo_unref(&rdev->gart.robj);
 }
 
-
-
-
 /*
  * Common gart functions.
  */
+/**
+ * radeon_gart_unbind - unbind pages from the gart page table
+ *
+ * @rdev: radeon_device pointer
+ * @offset: offset into the GPU's gart aperture
+ * @pages: number of pages to unbind
+ *
+ * Unbinds the requested pages from the gart page table and
+ * replaces them with the dummy page (all asics).
+ */
 void radeon_gart_unbind(struct radeon_device *rdev, unsigned offset,
 			int pages)
 {
@@ -172,6 +255,19 @@ void radeon_gart_unbind(struct radeon_device *rdev, unsigned offset,
 	radeon_gart_tlb_flush(rdev);
 }
 
+/**
+ * radeon_gart_bind - bind pages into the gart page table
+ *
+ * @rdev: radeon_device pointer
+ * @offset: offset into the GPU's gart aperture
+ * @pages: number of pages to bind
+ * @pagelist: pages to bind
+ * @dma_addr: DMA addresses of pages
+ *
+ * Binds the requested pages to the gart page table
+ * (all asics).
+ * Returns 0 for success, -EINVAL for failure.
+ */
 int radeon_gart_bind(struct radeon_device *rdev, unsigned offset,
 		     int pages, struct page **pagelist, dma_addr_t *dma_addr)
 {
@@ -203,6 +299,14 @@ int radeon_gart_bind(struct radeon_device *rdev, unsigned offset,
 	return 0;
 }
 
+/**
+ * radeon_gart_restore - bind all pages in the gart page table
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Binds all pages in the gart page table (all asics).
+ * Used to rebuild the gart table on device startup or resume.
+ */
 void radeon_gart_restore(struct radeon_device *rdev)
 {
 	int i, j, t;
@@ -222,6 +326,14 @@ void radeon_gart_restore(struct radeon_device *rdev)
 	radeon_gart_tlb_flush(rdev);
 }
 
+/**
+ * radeon_gart_init - init the driver info for managing the gart
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Allocate the dummy page and init the gart driver info (all asics).
+ * Returns 0 for success, error for failure.
+ */
 int radeon_gart_init(struct radeon_device *rdev)
 {
 	int r, i;
@@ -262,6 +374,13 @@ int radeon_gart_init(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_gart_fini - tear down the driver info for managing the gart
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Tear down the gart driver info and free the dummy page (all asics).
+ */
 void radeon_gart_fini(struct radeon_device *rdev)
 {
 	if (rdev->gart.pages && rdev->gart.pages_addr && rdev->gart.ready) {
@@ -278,35 +397,104 @@ void radeon_gart_fini(struct radeon_device *rdev)
 }
 
 /*
+ * GPUVM
+ * GPUVM is similar to the legacy gart on older asics, however
+ * rather than there being a single global gart table
+ * for the entire GPU, there are multiple VM page tables active
+ * at any given time.  The VM page tables can contain a mix
+ * vram pages and system memory pages and system memory pages
+ * can be mapped as snooped (cached system pages) or unsnooped
+ * (uncached system pages).
+ * Each VM has an ID associated with it and there is a page table
+ * associated with each VMID.  When execting a command buffer,
+ * the kernel tells the the ring what VMID to use for that command
+ * buffer.  VMIDs are allocated dynamically as commands are submitted.
+ * The userspace drivers maintain their own address space and the kernel
+ * sets up their pages tables accordingly when they submit their
+ * command buffers and a VMID is assigned.
+ * Cayman/Trinity support up to 8 active VMs at any given time;
+ * SI supports 16.
+ */
+
+/*
  * vm helpers
  *
  * TODO bind a default page at vm initialization for default address
  */
+
+/**
+ * radeon_vm_manager_init - init the vm manager
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Init the vm manager (cayman+).
+ * Returns 0 for success, error for failure.
+ */
 int radeon_vm_manager_init(struct radeon_device *rdev)
 {
+	struct radeon_vm *vm;
+	struct radeon_bo_va *bo_va;
 	int r;
 
-	rdev->vm_manager.enabled = false;
+	if (!rdev->vm_manager.enabled) {
+		/* mark first vm as always in use, it's the system one */
+		/* allocate enough for 2 full VM pts */
+		r = radeon_sa_bo_manager_init(rdev, &rdev->vm_manager.sa_manager,
+					      rdev->vm_manager.max_pfn * 8 * 2,
+					      RADEON_GEM_DOMAIN_VRAM);
+		if (r) {
+			dev_err(rdev->dev, "failed to allocate vm bo (%dKB)\n",
+				(rdev->vm_manager.max_pfn * 8) >> 10);
+			return r;
+		}
 
-	/* mark first vm as always in use, it's the system one */
-	/* allocate enough for 2 full VM pts */
-	r = radeon_sa_bo_manager_init(rdev, &rdev->vm_manager.sa_manager,
-				      rdev->vm_manager.max_pfn * 8 * 2,
-				      RADEON_GEM_DOMAIN_VRAM);
-	if (r) {
-		dev_err(rdev->dev, "failed to allocate vm bo (%dKB)\n",
-			(rdev->vm_manager.max_pfn * 8) >> 10);
-		return r;
+		r = rdev->vm_manager.funcs->init(rdev);
+		if (r)
+			return r;
+	
+		rdev->vm_manager.enabled = true;
+
+		r = radeon_sa_bo_manager_start(rdev, &rdev->vm_manager.sa_manager);
+		if (r)
+			return r;
 	}
 
-	r = rdev->vm_manager.funcs->init(rdev);
-	if (r == 0)
-		rdev->vm_manager.enabled = true;
+	/* restore page table */
+	list_for_each_entry(vm, &rdev->vm_manager.lru_vm, list) {
+		if (vm->id == -1)
+			continue;
 
-	return r;
+		list_for_each_entry(bo_va, &vm->va, vm_list) {
+			struct ttm_mem_reg *mem = NULL;
+			if (bo_va->valid)
+				mem = &bo_va->bo->tbo.mem;
+
+			bo_va->valid = false;
+			r = radeon_vm_bo_update_pte(rdev, vm, bo_va->bo, mem);
+			if (r) {
+				DRM_ERROR("Failed to update pte for vm %d!\n", vm->id);
+			}
+		}
+
+		r = rdev->vm_manager.funcs->bind(rdev, vm, vm->id);
+		if (r) {
+			DRM_ERROR("Failed to bind vm %d!\n", vm->id);
+		}
+	}
+	return 0;
 }
 
-/* cs mutex must be lock */
+/* global mutex must be lock */
+/**
+ * radeon_vm_unbind_locked - unbind a specific vm
+ *
+ * @rdev: radeon_device pointer
+ * @vm: vm to unbind
+ *
+ * Unbind the requested vm (cayman+).
+ * Wait for use of the VM to finish, then unbind the page table,
+ * and free the page table memory.
+ */
 static void radeon_vm_unbind_locked(struct radeon_device *rdev,
 				    struct radeon_vm *vm)
 {
@@ -317,10 +505,21 @@ static void radeon_vm_unbind_locked(struct radeon_device *rdev,
 	}
 
 	/* wait for vm use to end */
-	if (vm->fence) {
-		radeon_fence_wait(vm->fence, false);
-		radeon_fence_unref(&vm->fence);
+	while (vm->fence) {
+		int r;
+		r = radeon_fence_wait(vm->fence, false);
+		if (r)
+			DRM_ERROR("error while waiting for fence: %d\n", r);
+		if (r == -EDEADLK) {
+			mutex_unlock(&rdev->vm_manager.lock);
+			r = radeon_gpu_reset(rdev);
+			mutex_lock(&rdev->vm_manager.lock);
+			if (!r)
+				continue;
+		}
+		break;
 	}
+	radeon_fence_unref(&vm->fence);
 
 	/* hw unbind */
 	rdev->vm_manager.funcs->unbind(rdev, vm);
@@ -335,39 +534,42 @@ static void radeon_vm_unbind_locked(struct radeon_device *rdev,
 	}
 }
 
+/**
+ * radeon_vm_manager_fini - tear down the vm manager
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Tear down the VM manager (cayman+).
+ */
 void radeon_vm_manager_fini(struct radeon_device *rdev)
 {
-	if (rdev->vm_manager.sa_manager.bo == NULL)
-		return;
-	radeon_vm_manager_suspend(rdev);
-	rdev->vm_manager.funcs->fini(rdev);
-	radeon_sa_bo_manager_fini(rdev, &rdev->vm_manager.sa_manager);
-	rdev->vm_manager.enabled = false;
-}
-
-int radeon_vm_manager_start(struct radeon_device *rdev)
-{
-	if (rdev->vm_manager.sa_manager.bo == NULL) {
-		return -EINVAL;
-	}
-	return radeon_sa_bo_manager_start(rdev, &rdev->vm_manager.sa_manager);
-}
-
-int radeon_vm_manager_suspend(struct radeon_device *rdev)
-{
 	struct radeon_vm *vm, *tmp;
 
-	radeon_mutex_lock(&rdev->cs_mutex);
+	if (!rdev->vm_manager.enabled)
+		return;
+
+	mutex_lock(&rdev->vm_manager.lock);
 	/* unbind all active vm */
 	list_for_each_entry_safe(vm, tmp, &rdev->vm_manager.lru_vm, list) {
 		radeon_vm_unbind_locked(rdev, vm);
 	}
 	rdev->vm_manager.funcs->fini(rdev);
-	radeon_mutex_unlock(&rdev->cs_mutex);
-	return radeon_sa_bo_manager_suspend(rdev, &rdev->vm_manager.sa_manager);
+	mutex_unlock(&rdev->vm_manager.lock);
+
+	radeon_sa_bo_manager_suspend(rdev, &rdev->vm_manager.sa_manager);
+	radeon_sa_bo_manager_fini(rdev, &rdev->vm_manager.sa_manager);
+	rdev->vm_manager.enabled = false;
 }
 
-/* cs mutex must be lock */
+/* global mutex must be locked */
+/**
+ * radeon_vm_unbind - locked version of unbind
+ *
+ * @rdev: radeon_device pointer
+ * @vm: vm to unbind
+ *
+ * Locked version that wraps radeon_vm_unbind_locked (cayman+).
+ */
 void radeon_vm_unbind(struct radeon_device *rdev, struct radeon_vm *vm)
 {
 	mutex_lock(&vm->mutex);
@@ -375,7 +577,19 @@ void radeon_vm_unbind(struct radeon_device *rdev, struct radeon_vm *vm)
 	mutex_unlock(&vm->mutex);
 }
 
-/* cs mutex must be lock & vm mutex must be lock */
+/* global and local mutex must be locked */
+/**
+ * radeon_vm_bind - bind a page table to a VMID
+ *
+ * @rdev: radeon_device pointer
+ * @vm: vm to bind
+ *
+ * Bind the requested vm (cayman+).
+ * Suballocate memory for the page table, allocate a VMID
+ * and bind the page table to it, and finally start to populate
+ * the page table.
+ * Returns 0 for success, error for failure.
+ */
 int radeon_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm)
 {
 	struct radeon_vm *vm_evict;
@@ -438,6 +652,20 @@ retry_id:
 }
 
 /* object have to be reserved */
+/**
+ * radeon_vm_bo_add - add a bo to a specific vm
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ * @bo: radeon buffer object
+ * @offset: requested offset of the buffer in the VM address space
+ * @flags: attributes of pages (read/write/valid/etc.)
+ *
+ * Add @bo into the requested vm (cayman+).
+ * Add @bo to the list of bos associated with the vm and validate
+ * the offset requested within the vm address space.
+ * Returns 0 for success, error for failure.
+ */
 int radeon_vm_bo_add(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
 		     struct radeon_bo *bo,
@@ -479,7 +707,7 @@ int radeon_vm_bo_add(struct radeon_device *rdev,
 	if (last_pfn > vm->last_pfn) {
 		/* release mutex and lock in right order */
 		mutex_unlock(&vm->mutex);
-		radeon_mutex_lock(&rdev->cs_mutex);
+		mutex_lock(&rdev->vm_manager.lock);
 		mutex_lock(&vm->mutex);
 		/* and check again */
 		if (last_pfn > vm->last_pfn) {
@@ -488,7 +716,7 @@ int radeon_vm_bo_add(struct radeon_device *rdev,
 			radeon_vm_unbind_locked(rdev, vm);
 			vm->last_pfn = (last_pfn + align) & ~align;
 		}
-		radeon_mutex_unlock(&rdev->cs_mutex);
+		mutex_unlock(&rdev->vm_manager.lock);
 	}
 	head = &vm->va;
 	last_offset = 0;
@@ -515,6 +743,17 @@ int radeon_vm_bo_add(struct radeon_device *rdev,
 	return 0;
 }
 
+/**
+ * radeon_vm_get_addr - get the physical address of the page
+ *
+ * @rdev: radeon_device pointer
+ * @mem: ttm mem
+ * @pfn: pfn
+ *
+ * Look up the physical address of the page that the pte resolves
+ * to (cayman+).
+ * Returns the physical address of the page.
+ */
 static u64 radeon_vm_get_addr(struct radeon_device *rdev,
 			      struct ttm_mem_reg *mem,
 			      unsigned pfn)
@@ -543,7 +782,18 @@ static u64 radeon_vm_get_addr(struct radeon_device *rdev,
 	return addr;
 }
 
-/* object have to be reserved & cs mutex took & vm mutex took */
+/* object have to be reserved & global and local mutex must be locked */
+/**
+ * radeon_vm_bo_update_pte - map a bo into the vm page table
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ * @bo: radeon buffer object
+ * @mem: ttm mem
+ *
+ * Fill in the page table entries for @bo (cayman+).
+ * Returns 0 for success, -EINVAL for failure.
+ */
 int radeon_vm_bo_update_pte(struct radeon_device *rdev,
 			    struct radeon_vm *vm,
 			    struct radeon_bo *bo,
@@ -592,6 +842,18 @@ int radeon_vm_bo_update_pte(struct radeon_device *rdev,
 }
 
 /* object have to be reserved */
+/**
+ * radeon_vm_bo_rmv - remove a bo to a specific vm
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ * @bo: radeon buffer object
+ *
+ * Remove @bo from the requested vm (cayman+).
+ * Remove @bo from the list of bos associated with the vm and
+ * remove the ptes for @bo in the page table.
+ * Returns 0 for success.
+ */
 int radeon_vm_bo_rmv(struct radeon_device *rdev,
 		     struct radeon_vm *vm,
 		     struct radeon_bo *bo)
@@ -602,10 +864,10 @@ int radeon_vm_bo_rmv(struct radeon_device *rdev,
 	if (bo_va == NULL)
 		return 0;
 
-	radeon_mutex_lock(&rdev->cs_mutex);
+	mutex_lock(&rdev->vm_manager.lock);
 	mutex_lock(&vm->mutex);
 	radeon_vm_bo_update_pte(rdev, vm, bo, NULL);
-	radeon_mutex_unlock(&rdev->cs_mutex);
+	mutex_unlock(&rdev->vm_manager.lock);
 	list_del(&bo_va->vm_list);
 	mutex_unlock(&vm->mutex);
 	list_del(&bo_va->bo_list);
@@ -614,6 +876,15 @@ int radeon_vm_bo_rmv(struct radeon_device *rdev,
 	return 0;
 }
 
+/**
+ * radeon_vm_bo_invalidate - mark the bo as invalid
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ * @bo: radeon buffer object
+ *
+ * Mark @bo as invalid (cayman+).
+ */
 void radeon_vm_bo_invalidate(struct radeon_device *rdev,
 			     struct radeon_bo *bo)
 {
@@ -625,6 +896,17 @@ void radeon_vm_bo_invalidate(struct radeon_device *rdev,
 	}
 }
 
+/**
+ * radeon_vm_init - initialize a vm instance
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ *
+ * Init @vm (cayman+).
+ * Map the IB pool and any other shared objects into the VM
+ * by default as it's used by all VMs.
+ * Returns 0 for success, error for failure.
+ */
 int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 {
 	int r;
@@ -651,15 +933,24 @@ int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 	return r;
 }
 
+/**
+ * radeon_vm_init - tear down a vm instance
+ *
+ * @rdev: radeon_device pointer
+ * @vm: requested vm
+ *
+ * Tear down @vm (cayman+).
+ * Unbind the VM and remove all bos from the vm bo list
+ */
 void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
 {
 	struct radeon_bo_va *bo_va, *tmp;
 	int r;
 
-	radeon_mutex_lock(&rdev->cs_mutex);
+	mutex_lock(&rdev->vm_manager.lock);
 	mutex_lock(&vm->mutex);
 	radeon_vm_unbind_locked(rdev, vm);
-	radeon_mutex_unlock(&rdev->cs_mutex);
+	mutex_unlock(&rdev->vm_manager.lock);
 
 	/* remove all bo */
 	r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index 21ec9f5653c..84d04524573 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -159,11 +159,9 @@ void radeon_gem_object_close(struct drm_gem_object *obj,
 static int radeon_gem_handle_lockup(struct radeon_device *rdev, int r)
 {
 	if (r == -EDEADLK) {
-		radeon_mutex_lock(&rdev->cs_mutex);
 		r = radeon_gpu_reset(rdev);
 		if (!r)
 			r = -EAGAIN;
-		radeon_mutex_unlock(&rdev->cs_mutex);
 	}
 	return r;
 }
@@ -217,12 +215,14 @@ int radeon_gem_create_ioctl(struct drm_device *dev, void *data,
 	uint32_t handle;
 	int r;
 
+	down_read(&rdev->exclusive_lock);
 	/* create a gem object to contain this object in */
 	args->size = roundup(args->size, PAGE_SIZE);
 	r = radeon_gem_object_create(rdev, args->size, args->alignment,
 					args->initial_domain, false,
 					false, &gobj);
 	if (r) {
+		up_read(&rdev->exclusive_lock);
 		r = radeon_gem_handle_lockup(rdev, r);
 		return r;
 	}
@@ -230,10 +230,12 @@ int radeon_gem_create_ioctl(struct drm_device *dev, void *data,
 	/* drop reference from allocate - handle holds it now */
 	drm_gem_object_unreference_unlocked(gobj);
 	if (r) {
+		up_read(&rdev->exclusive_lock);
 		r = radeon_gem_handle_lockup(rdev, r);
 		return r;
 	}
 	args->handle = handle;
+	up_read(&rdev->exclusive_lock);
 	return 0;
 }
 
@@ -242,6 +244,7 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 {
 	/* transition the BO to a domain -
 	 * just validate the BO into a certain domain */
+	struct radeon_device *rdev = dev->dev_private;
 	struct drm_radeon_gem_set_domain *args = data;
 	struct drm_gem_object *gobj;
 	struct radeon_bo *robj;
@@ -249,10 +252,12 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 
 	/* for now if someone requests domain CPU -
 	 * just make sure the buffer is finished with */
+	down_read(&rdev->exclusive_lock);
 
 	/* just do a BO wait for now */
 	gobj = drm_gem_object_lookup(dev, filp, args->handle);
 	if (gobj == NULL) {
+		up_read(&rdev->exclusive_lock);
 		return -ENOENT;
 	}
 	robj = gem_to_radeon_bo(gobj);
@@ -260,6 +265,7 @@ int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	r = radeon_gem_set_domain(gobj, args->read_domains, args->write_domain);
 
 	drm_gem_object_unreference_unlocked(gobj);
+	up_read(&rdev->exclusive_lock);
 	r = radeon_gem_handle_lockup(robj->rdev, r);
 	return r;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 5df58d1aba0..afaa1727abd 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -32,6 +32,17 @@
 #include "radeon.h"
 #include "atom.h"
 
+#define RADEON_WAIT_IDLE_TIMEOUT 200
+
+/**
+ * radeon_driver_irq_handler_kms - irq handler for KMS
+ *
+ * @DRM_IRQ_ARGS: args
+ *
+ * This is the irq handler for the radeon KMS driver (all asics).
+ * radeon_irq_process is a macro that points to the per-asic
+ * irq handler callback.
+ */
 irqreturn_t radeon_driver_irq_handler_kms(DRM_IRQ_ARGS)
 {
 	struct drm_device *dev = (struct drm_device *) arg;
@@ -43,6 +54,17 @@ irqreturn_t radeon_driver_irq_handler_kms(DRM_IRQ_ARGS)
 /*
  * Handle hotplug events outside the interrupt handler proper.
  */
+/**
+ * radeon_hotplug_work_func - display hotplug work handler
+ *
+ * @work: work struct
+ *
+ * This is the hot plug event work handler (all asics).
+ * The work gets scheduled from the irq handler if there
+ * was a hot plug interrupt.  It walks the connector table
+ * and calls the hotplug handler for each one, then sends
+ * a drm hotplug event to alert userspace.
+ */
 static void radeon_hotplug_work_func(struct work_struct *work)
 {
 	struct radeon_device *rdev = container_of(work, struct radeon_device,
@@ -59,61 +81,94 @@ static void radeon_hotplug_work_func(struct work_struct *work)
 	drm_helper_hpd_irq_event(dev);
 }
 
+/**
+ * radeon_driver_irq_preinstall_kms - drm irq preinstall callback
+ *
+ * @dev: drm dev pointer
+ *
+ * Gets the hw ready to enable irqs (all asics).
+ * This function disables all interrupt sources on the GPU.
+ */
 void radeon_driver_irq_preinstall_kms(struct drm_device *dev)
 {
 	struct radeon_device *rdev = dev->dev_private;
+	unsigned long irqflags;
 	unsigned i;
 
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
 	/* Disable *all* interrupts */
 	for (i = 0; i < RADEON_NUM_RINGS; i++)
-		rdev->irq.sw_int[i] = false;
+		atomic_set(&rdev->irq.ring_int[i], 0);
 	rdev->irq.gui_idle = false;
 	for (i = 0; i < RADEON_MAX_HPD_PINS; i++)
 		rdev->irq.hpd[i] = false;
 	for (i = 0; i < RADEON_MAX_CRTCS; i++) {
 		rdev->irq.crtc_vblank_int[i] = false;
-		rdev->irq.pflip[i] = false;
+		atomic_set(&rdev->irq.pflip[i], 0);
 		rdev->irq.afmt[i] = false;
 	}
 	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 	/* Clear bits */
 	radeon_irq_process(rdev);
 }
 
+/**
+ * radeon_driver_irq_postinstall_kms - drm irq preinstall callback
+ *
+ * @dev: drm dev pointer
+ *
+ * Handles stuff to be done after enabling irqs (all asics).
+ * Returns 0 on success.
+ */
 int radeon_driver_irq_postinstall_kms(struct drm_device *dev)
 {
-	struct radeon_device *rdev = dev->dev_private;
-	unsigned i;
-
 	dev->max_vblank_count = 0x001fffff;
-	for (i = 0; i < RADEON_NUM_RINGS; i++)
-		rdev->irq.sw_int[i] = true;
-	radeon_irq_set(rdev);
 	return 0;
 }
 
+/**
+ * radeon_driver_irq_uninstall_kms - drm irq uninstall callback
+ *
+ * @dev: drm dev pointer
+ *
+ * This function disables all interrupt sources on the GPU (all asics).
+ */
 void radeon_driver_irq_uninstall_kms(struct drm_device *dev)
 {
 	struct radeon_device *rdev = dev->dev_private;
+	unsigned long irqflags;
 	unsigned i;
 
 	if (rdev == NULL) {
 		return;
 	}
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
 	/* Disable *all* interrupts */
 	for (i = 0; i < RADEON_NUM_RINGS; i++)
-		rdev->irq.sw_int[i] = false;
+		atomic_set(&rdev->irq.ring_int[i], 0);
 	rdev->irq.gui_idle = false;
 	for (i = 0; i < RADEON_MAX_HPD_PINS; i++)
 		rdev->irq.hpd[i] = false;
 	for (i = 0; i < RADEON_MAX_CRTCS; i++) {
 		rdev->irq.crtc_vblank_int[i] = false;
-		rdev->irq.pflip[i] = false;
+		atomic_set(&rdev->irq.pflip[i], 0);
 		rdev->irq.afmt[i] = false;
 	}
 	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 }
 
+/**
+ * radeon_msi_ok - asic specific msi checks
+ *
+ * @rdev: radeon device pointer
+ *
+ * Handles asic specific MSI checks to determine if
+ * MSIs should be enabled on a particular chip (all asics).
+ * Returns true if MSIs should be enabled, false if MSIs
+ * should not be enabled.
+ */
 static bool radeon_msi_ok(struct radeon_device *rdev)
 {
 	/* RV370/RV380 was first asic with MSI support */
@@ -166,17 +221,22 @@ static bool radeon_msi_ok(struct radeon_device *rdev)
 	return true;
 }
 
+/**
+ * radeon_irq_kms_init - init driver interrupt info
+ *
+ * @rdev: radeon device pointer
+ *
+ * Sets up the work irq handlers, vblank init, MSIs, etc. (all asics).
+ * Returns 0 for success, error for failure.
+ */
 int radeon_irq_kms_init(struct radeon_device *rdev)
 {
-	int i;
 	int r = 0;
 
 	INIT_WORK(&rdev->hotplug_work, radeon_hotplug_work_func);
 	INIT_WORK(&rdev->audio_work, r600_audio_update_hdmi);
 
-	spin_lock_init(&rdev->irq.sw_lock);
-	for (i = 0; i < rdev->num_crtc; i++)
-		spin_lock_init(&rdev->irq.pflip_lock[i]);
+	spin_lock_init(&rdev->irq.lock);
 	r = drm_vblank_init(rdev->ddev, rdev->num_crtc);
 	if (r) {
 		return r;
@@ -201,6 +261,13 @@ int radeon_irq_kms_init(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_irq_kms_fini - tear down driver interrrupt info
+ *
+ * @rdev: radeon device pointer
+ *
+ * Tears down the work irq handlers, vblank handlers, MSIs, etc. (all asics).
+ */
 void radeon_irq_kms_fini(struct radeon_device *rdev)
 {
 	drm_vblank_cleanup(rdev->ddev);
@@ -213,31 +280,63 @@ void radeon_irq_kms_fini(struct radeon_device *rdev)
 	flush_work_sync(&rdev->hotplug_work);
 }
 
+/**
+ * radeon_irq_kms_sw_irq_get - enable software interrupt
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring whose interrupt you want to enable
+ *
+ * Enables the software interrupt for a specific ring (all asics).
+ * The software interrupt is generally used to signal a fence on
+ * a particular ring.
+ */
 void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev, int ring)
 {
 	unsigned long irqflags;
 
-	spin_lock_irqsave(&rdev->irq.sw_lock, irqflags);
-	if (rdev->ddev->irq_enabled && (++rdev->irq.sw_refcount[ring] == 1)) {
-		rdev->irq.sw_int[ring] = true;
+	if (!rdev->ddev->irq_enabled)
+		return;
+
+	if (atomic_inc_return(&rdev->irq.ring_int[ring]) == 1) {
+		spin_lock_irqsave(&rdev->irq.lock, irqflags);
 		radeon_irq_set(rdev);
+		spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 	}
-	spin_unlock_irqrestore(&rdev->irq.sw_lock, irqflags);
 }
 
+/**
+ * radeon_irq_kms_sw_irq_put - disable software interrupt
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring whose interrupt you want to disable
+ *
+ * Disables the software interrupt for a specific ring (all asics).
+ * The software interrupt is generally used to signal a fence on
+ * a particular ring.
+ */
 void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev, int ring)
 {
 	unsigned long irqflags;
 
-	spin_lock_irqsave(&rdev->irq.sw_lock, irqflags);
-	BUG_ON(rdev->ddev->irq_enabled && rdev->irq.sw_refcount[ring] <= 0);
-	if (rdev->ddev->irq_enabled && (--rdev->irq.sw_refcount[ring] == 0)) {
-		rdev->irq.sw_int[ring] = false;
+	if (!rdev->ddev->irq_enabled)
+		return;
+
+	if (atomic_dec_and_test(&rdev->irq.ring_int[ring])) {
+		spin_lock_irqsave(&rdev->irq.lock, irqflags);
 		radeon_irq_set(rdev);
+		spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 	}
-	spin_unlock_irqrestore(&rdev->irq.sw_lock, irqflags);
 }
 
+/**
+ * radeon_irq_kms_pflip_irq_get - enable pageflip interrupt
+ *
+ * @rdev: radeon device pointer
+ * @crtc: crtc whose interrupt you want to enable
+ *
+ * Enables the pageflip interrupt for a specific crtc (all asics).
+ * For pageflips we use the vblank interrupt source.
+ */
 void radeon_irq_kms_pflip_irq_get(struct radeon_device *rdev, int crtc)
 {
 	unsigned long irqflags;
@@ -245,14 +344,25 @@ void radeon_irq_kms_pflip_irq_get(struct radeon_device *rdev, int crtc)
 	if (crtc < 0 || crtc >= rdev->num_crtc)
 		return;
 
-	spin_lock_irqsave(&rdev->irq.pflip_lock[crtc], irqflags);
-	if (rdev->ddev->irq_enabled && (++rdev->irq.pflip_refcount[crtc] == 1)) {
-		rdev->irq.pflip[crtc] = true;
+	if (!rdev->ddev->irq_enabled)
+		return;
+
+	if (atomic_inc_return(&rdev->irq.pflip[crtc]) == 1) {
+		spin_lock_irqsave(&rdev->irq.lock, irqflags);
 		radeon_irq_set(rdev);
+		spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 	}
-	spin_unlock_irqrestore(&rdev->irq.pflip_lock[crtc], irqflags);
 }
 
+/**
+ * radeon_irq_kms_pflip_irq_put - disable pageflip interrupt
+ *
+ * @rdev: radeon device pointer
+ * @crtc: crtc whose interrupt you want to disable
+ *
+ * Disables the pageflip interrupt for a specific crtc (all asics).
+ * For pageflips we use the vblank interrupt source.
+ */
 void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc)
 {
 	unsigned long irqflags;
@@ -260,12 +370,121 @@ void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc)
 	if (crtc < 0 || crtc >= rdev->num_crtc)
 		return;
 
-	spin_lock_irqsave(&rdev->irq.pflip_lock[crtc], irqflags);
-	BUG_ON(rdev->ddev->irq_enabled && rdev->irq.pflip_refcount[crtc] <= 0);
-	if (rdev->ddev->irq_enabled && (--rdev->irq.pflip_refcount[crtc] == 0)) {
-		rdev->irq.pflip[crtc] = false;
+	if (!rdev->ddev->irq_enabled)
+		return;
+
+	if (atomic_dec_and_test(&rdev->irq.pflip[crtc])) {
+		spin_lock_irqsave(&rdev->irq.lock, irqflags);
 		radeon_irq_set(rdev);
+		spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 	}
-	spin_unlock_irqrestore(&rdev->irq.pflip_lock[crtc], irqflags);
 }
 
+/**
+ * radeon_irq_kms_enable_afmt - enable audio format change interrupt
+ *
+ * @rdev: radeon device pointer
+ * @block: afmt block whose interrupt you want to enable
+ *
+ * Enables the afmt change interrupt for a specific afmt block (all asics).
+ */
+void radeon_irq_kms_enable_afmt(struct radeon_device *rdev, int block)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
+	rdev->irq.afmt[block] = true;
+	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+
+}
+
+/**
+ * radeon_irq_kms_disable_afmt - disable audio format change interrupt
+ *
+ * @rdev: radeon device pointer
+ * @block: afmt block whose interrupt you want to disable
+ *
+ * Disables the afmt change interrupt for a specific afmt block (all asics).
+ */
+void radeon_irq_kms_disable_afmt(struct radeon_device *rdev, int block)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
+	rdev->irq.afmt[block] = false;
+	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+}
+
+/**
+ * radeon_irq_kms_enable_hpd - enable hotplug detect interrupt
+ *
+ * @rdev: radeon device pointer
+ * @hpd_mask: mask of hpd pins you want to enable.
+ *
+ * Enables the hotplug detect interrupt for a specific hpd pin (all asics).
+ */
+void radeon_irq_kms_enable_hpd(struct radeon_device *rdev, unsigned hpd_mask)
+{
+	unsigned long irqflags;
+	int i;
+
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
+	for (i = 0; i < RADEON_MAX_HPD_PINS; ++i)
+		rdev->irq.hpd[i] |= !!(hpd_mask & (1 << i));
+	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+}
+
+/**
+ * radeon_irq_kms_disable_hpd - disable hotplug detect interrupt
+ *
+ * @rdev: radeon device pointer
+ * @hpd_mask: mask of hpd pins you want to disable.
+ *
+ * Disables the hotplug detect interrupt for a specific hpd pin (all asics).
+ */
+void radeon_irq_kms_disable_hpd(struct radeon_device *rdev, unsigned hpd_mask)
+{
+	unsigned long irqflags;
+	int i;
+
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
+	for (i = 0; i < RADEON_MAX_HPD_PINS; ++i)
+		rdev->irq.hpd[i] &= !(hpd_mask & (1 << i));
+	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+}
+
+/**
+ * radeon_irq_kms_wait_gui_idle - waits for drawing engine to be idle
+ *
+ * @rdev: radeon device pointer
+ *
+ * Enabled the GUI idle interrupt and waits for it to fire (r6xx+).
+ * This is currently used to make sure the 3D engine is idle for power
+ * management, but should be replaces with proper fence waits.
+ * GUI idle interrupts don't work very well on pre-r6xx hw and it also
+ * does not take into account other aspects of the chip that may be busy.
+ * DO NOT USE GOING FORWARD.
+ */
+int radeon_irq_kms_wait_gui_idle(struct radeon_device *rdev)
+{
+	unsigned long irqflags;
+	int r;
+
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
+	rdev->irq.gui_idle = true;
+	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+
+	r = wait_event_timeout(rdev->irq.idle_queue, radeon_gui_idle(rdev),
+			       msecs_to_jiffies(RADEON_WAIT_IDLE_TIMEOUT));
+
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
+	rdev->irq.gui_idle = false;
+	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+	return r;
+}
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 5c58d7d90cb..1d73f16b5d9 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -33,6 +33,17 @@
 #include <linux/vga_switcheroo.h>
 #include <linux/slab.h>
 
+/**
+ * radeon_driver_unload_kms - Main unload function for KMS.
+ *
+ * @dev: drm dev pointer
+ *
+ * This is the main unload function for KMS (all asics).
+ * It calls radeon_modeset_fini() to tear down the
+ * displays, and radeon_device_fini() to tear down
+ * the rest of the device (CP, writeback, etc.).
+ * Returns 0 on success.
+ */
 int radeon_driver_unload_kms(struct drm_device *dev)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -46,6 +57,19 @@ int radeon_driver_unload_kms(struct drm_device *dev)
 	return 0;
 }
 
+/**
+ * radeon_driver_load_kms - Main load function for KMS.
+ *
+ * @dev: drm dev pointer
+ * @flags: device flags
+ *
+ * This is the main load function for KMS (all asics).
+ * It calls radeon_device_init() to set up the non-display
+ * parts of the chip (asic init, CP, writeback, etc.), and
+ * radeon_modeset_init() to set up the display parts
+ * (crtcs, encoders, hotplug detect, etc.).
+ * Returns 0 on success, error on failure.
+ */
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 {
 	struct radeon_device *rdev;
@@ -96,6 +120,16 @@ out:
 	return r;
 }
 
+/**
+ * radeon_set_filp_rights - Set filp right.
+ *
+ * @dev: drm dev pointer
+ * @owner: drm file
+ * @applier: drm file
+ * @value: value
+ *
+ * Sets the filp rights for the device (all asics).
+ */
 static void radeon_set_filp_rights(struct drm_device *dev,
 				   struct drm_file **owner,
 				   struct drm_file *applier,
@@ -118,6 +152,18 @@ static void radeon_set_filp_rights(struct drm_device *dev,
 /*
  * Userspace get information ioctl
  */
+/**
+ * radeon_info_ioctl - answer a device specific request.
+ *
+ * @rdev: radeon device pointer
+ * @data: request object
+ * @filp: drm filp
+ *
+ * This function is used to pass device specific parameters to the userspace
+ * drivers.  Examples include: pci device id, pipeline parms, tiling params,
+ * etc. (all asics).
+ * Returns 0 on success, -EINVAL on failure.
+ */
 int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -301,16 +347,40 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 /*
  * Outdated mess for old drm with Xorg being in charge (void function now).
  */
+/**
+ * radeon_driver_firstopen_kms - drm callback for first open
+ *
+ * @dev: drm dev pointer
+ *
+ * Nothing to be done for KMS (all asics).
+ * Returns 0 on success.
+ */
 int radeon_driver_firstopen_kms(struct drm_device *dev)
 {
 	return 0;
 }
 
+/**
+ * radeon_driver_firstopen_kms - drm callback for last close
+ *
+ * @dev: drm dev pointer
+ *
+ * Switch vga switcheroo state after last close (all asics).
+ */
 void radeon_driver_lastclose_kms(struct drm_device *dev)
 {
 	vga_switcheroo_process_delayed_switch();
 }
 
+/**
+ * radeon_driver_open_kms - drm callback for open
+ *
+ * @dev: drm dev pointer
+ * @file_priv: drm file
+ *
+ * On device open, init vm on cayman+ (all asics).
+ * Returns 0 on success, error on failure.
+ */
 int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -339,6 +409,14 @@ int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
 	return 0;
 }
 
+/**
+ * radeon_driver_postclose_kms - drm callback for post close
+ *
+ * @dev: drm dev pointer
+ * @file_priv: drm file
+ *
+ * On device post close, tear down vm on cayman+ (all asics).
+ */
 void radeon_driver_postclose_kms(struct drm_device *dev,
 				 struct drm_file *file_priv)
 {
@@ -354,6 +432,15 @@ void radeon_driver_postclose_kms(struct drm_device *dev,
 	}
 }
 
+/**
+ * radeon_driver_preclose_kms - drm callback for pre close
+ *
+ * @dev: drm dev pointer
+ * @file_priv: drm file
+ *
+ * On device pre close, tear down hyperz and cmask filps on r1xx-r5xx
+ * (all asics).
+ */
 void radeon_driver_preclose_kms(struct drm_device *dev,
 				struct drm_file *file_priv)
 {
@@ -367,6 +454,15 @@ void radeon_driver_preclose_kms(struct drm_device *dev,
 /*
  * VBlank related functions.
  */
+/**
+ * radeon_get_vblank_counter_kms - get frame count
+ *
+ * @dev: drm dev pointer
+ * @crtc: crtc to get the frame count from
+ *
+ * Gets the frame count on the requested crtc (all asics).
+ * Returns frame count on success, -EINVAL on failure.
+ */
 u32 radeon_get_vblank_counter_kms(struct drm_device *dev, int crtc)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -379,34 +475,70 @@ u32 radeon_get_vblank_counter_kms(struct drm_device *dev, int crtc)
 	return radeon_get_vblank_counter(rdev, crtc);
 }
 
+/**
+ * radeon_enable_vblank_kms - enable vblank interrupt
+ *
+ * @dev: drm dev pointer
+ * @crtc: crtc to enable vblank interrupt for
+ *
+ * Enable the interrupt on the requested crtc (all asics).
+ * Returns 0 on success, -EINVAL on failure.
+ */
 int radeon_enable_vblank_kms(struct drm_device *dev, int crtc)
 {
 	struct radeon_device *rdev = dev->dev_private;
+	unsigned long irqflags;
+	int r;
 
 	if (crtc < 0 || crtc >= rdev->num_crtc) {
 		DRM_ERROR("Invalid crtc %d\n", crtc);
 		return -EINVAL;
 	}
 
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
 	rdev->irq.crtc_vblank_int[crtc] = true;
-
-	return radeon_irq_set(rdev);
+	r = radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+	return r;
 }
 
+/**
+ * radeon_disable_vblank_kms - disable vblank interrupt
+ *
+ * @dev: drm dev pointer
+ * @crtc: crtc to disable vblank interrupt for
+ *
+ * Disable the interrupt on the requested crtc (all asics).
+ */
 void radeon_disable_vblank_kms(struct drm_device *dev, int crtc)
 {
 	struct radeon_device *rdev = dev->dev_private;
+	unsigned long irqflags;
 
 	if (crtc < 0 || crtc >= rdev->num_crtc) {
 		DRM_ERROR("Invalid crtc %d\n", crtc);
 		return;
 	}
 
+	spin_lock_irqsave(&rdev->irq.lock, irqflags);
 	rdev->irq.crtc_vblank_int[crtc] = false;
-
 	radeon_irq_set(rdev);
+	spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
 }
 
+/**
+ * radeon_get_vblank_timestamp_kms - get vblank timestamp
+ *
+ * @dev: drm dev pointer
+ * @crtc: crtc to get the timestamp for
+ * @max_error: max error
+ * @vblank_time: time value
+ * @flags: flags passed to the driver
+ *
+ * Gets the timestamp on the requested crtc based on the
+ * scanout position.  (all asics).
+ * Returns postive status flags on success, negative error on failure.
+ */
 int radeon_get_vblank_timestamp_kms(struct drm_device *dev, int crtc,
 				    int *max_error,
 				    struct timeval *vblank_time,
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index 210317c7045..d5fd615897e 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -990,7 +990,7 @@ static void radeon_set_pll(struct drm_crtc *crtc, struct drm_display_mode *mode)
 }
 
 static bool radeon_crtc_mode_fixup(struct drm_crtc *crtc,
-				   struct drm_display_mode *mode,
+				   const struct drm_display_mode *mode,
 				   struct drm_display_mode *adjusted_mode)
 {
 	if (!radeon_crtc_scaling_mode_fixup(crtc, mode, adjusted_mode))
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
index a0c82229e8f..670e9910f86 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
@@ -244,7 +244,7 @@ static void radeon_legacy_lvds_mode_set(struct drm_encoder *encoder,
 }
 
 static bool radeon_legacy_mode_fixup(struct drm_encoder *encoder,
-				     struct drm_display_mode *mode,
+				     const struct drm_display_mode *mode,
 				     struct drm_display_mode *adjusted_mode)
 {
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 5b10ffd7bb2..f380d59c576 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -488,7 +488,7 @@ extern void radeon_connector_hotplug(struct drm_connector *connector);
 extern int radeon_dp_mode_valid_helper(struct drm_connector *connector,
 				       struct drm_display_mode *mode);
 extern void radeon_dp_set_link_config(struct drm_connector *connector,
-				      struct drm_display_mode *mode);
+				      const struct drm_display_mode *mode);
 extern void radeon_dp_link_train(struct drm_encoder *encoder,
 				 struct drm_connector *connector);
 extern bool radeon_dp_needs_link_train(struct radeon_connector *radeon_connector);
@@ -678,7 +678,7 @@ void radeon_enc_destroy(struct drm_encoder *encoder);
 void radeon_copy_fb(struct drm_device *dev, struct drm_gem_object *dst_obj);
 void radeon_combios_asic_init(struct drm_device *dev);
 bool radeon_crtc_scaling_mode_fixup(struct drm_crtc *crtc,
-					struct drm_display_mode *mode,
+					const struct drm_display_mode *mode,
 					struct drm_display_mode *adjusted_mode);
 void radeon_panel_mode_fixup(struct drm_encoder *encoder,
 			     struct drm_display_mode *adjusted_mode);
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 830f1a7b486..1f1a4c803c1 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -115,9 +115,7 @@ int radeon_bo_create(struct radeon_device *rdev,
 
 	size = ALIGN(size, PAGE_SIZE);
 
-	if (unlikely(rdev->mman.bdev.dev_mapping == NULL)) {
-		rdev->mman.bdev.dev_mapping = rdev->ddev->dev_mapping;
-	}
+	rdev->mman.bdev.dev_mapping = rdev->ddev->dev_mapping;
 	if (kernel) {
 		type = ttm_bo_type_kernel;
 	} else if (sg) {
@@ -138,7 +136,6 @@ int radeon_bo_create(struct radeon_device *rdev,
 	acc_size = ttm_bo_dma_acc_size(&rdev->mman.bdev, size,
 				       sizeof(struct radeon_bo));
 
-retry:
 	bo = kzalloc(sizeof(struct radeon_bo), GFP_KERNEL);
 	if (bo == NULL)
 		return -ENOMEM;
@@ -152,13 +149,15 @@ retry:
 	bo->surface_reg = -1;
 	INIT_LIST_HEAD(&bo->list);
 	INIT_LIST_HEAD(&bo->va);
+
+retry:
 	radeon_ttm_placement_from_domain(bo, domain);
 	/* Kernel allocation are uninterruptible */
-	mutex_lock(&rdev->vram_mutex);
+	down_read(&rdev->pm.mclk_lock);
 	r = ttm_bo_init(&rdev->mman.bdev, &bo->tbo, size, type,
 			&bo->placement, page_align, 0, !kernel, NULL,
 			acc_size, sg, &radeon_ttm_bo_destroy);
-	mutex_unlock(&rdev->vram_mutex);
+	up_read(&rdev->pm.mclk_lock);
 	if (unlikely(r != 0)) {
 		if (r != -ERESTARTSYS) {
 			if (domain == RADEON_GEM_DOMAIN_VRAM) {
@@ -219,9 +218,9 @@ void radeon_bo_unref(struct radeon_bo **bo)
 		return;
 	rdev = (*bo)->rdev;
 	tbo = &((*bo)->tbo);
-	mutex_lock(&rdev->vram_mutex);
+	down_read(&rdev->pm.mclk_lock);
 	ttm_bo_unref(&tbo);
-	mutex_unlock(&rdev->vram_mutex);
+	up_read(&rdev->pm.mclk_lock);
 	if (tbo == NULL)
 		*bo = NULL;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index 5b37e283ec3..7ae60660010 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -34,7 +34,6 @@
 #define RADEON_IDLE_LOOP_MS 100
 #define RADEON_RECLOCK_DELAY_MS 200
 #define RADEON_WAIT_VBLANK_TIMEOUT 200
-#define RADEON_WAIT_IDLE_TIMEOUT 200
 
 static const char *radeon_pm_state_type_name[5] = {
 	"Default",
@@ -251,21 +250,14 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 		return;
 
 	mutex_lock(&rdev->ddev->struct_mutex);
-	mutex_lock(&rdev->vram_mutex);
+	down_write(&rdev->pm.mclk_lock);
 	mutex_lock(&rdev->ring_lock);
 
 	/* gui idle int has issues on older chips it seems */
 	if (rdev->family >= CHIP_R600) {
 		if (rdev->irq.installed) {
-			/* wait for GPU idle */
-			rdev->pm.gui_idle = false;
-			rdev->irq.gui_idle = true;
-			radeon_irq_set(rdev);
-			wait_event_interruptible_timeout(
-				rdev->irq.idle_queue, rdev->pm.gui_idle,
-				msecs_to_jiffies(RADEON_WAIT_IDLE_TIMEOUT));
-			rdev->irq.gui_idle = false;
-			radeon_irq_set(rdev);
+			/* wait for GPU to become idle */
+			radeon_irq_kms_wait_gui_idle(rdev);
 		}
 	} else {
 		struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
@@ -303,7 +295,7 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 	rdev->pm.dynpm_planned_action = DYNPM_ACTION_NONE;
 
 	mutex_unlock(&rdev->ring_lock);
-	mutex_unlock(&rdev->vram_mutex);
+	up_write(&rdev->pm.mclk_lock);
 	mutex_unlock(&rdev->ddev->struct_mutex);
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index 983658c9135..ec79b375043 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -35,47 +35,97 @@
 #include "atom.h"
 
 /*
- * IB.
+ * IB
+ * IBs (Indirect Buffers) and areas of GPU accessible memory where
+ * commands are stored.  You can put a pointer to the IB in the
+ * command ring and the hw will fetch the commands from the IB
+ * and execute them.  Generally userspace acceleration drivers
+ * produce command buffers which are send to the kernel and
+ * put in IBs for execution by the requested ring.
  */
 int radeon_debugfs_sa_init(struct radeon_device *rdev);
 
+/**
+ * radeon_ib_get - request an IB (Indirect Buffer)
+ *
+ * @rdev: radeon_device pointer
+ * @ring: ring index the IB is associated with
+ * @ib: IB object returned
+ * @size: requested IB size
+ *
+ * Request an IB (all asics).  IBs are allocated using the
+ * suballocator.
+ * Returns 0 on success, error on failure.
+ */
 int radeon_ib_get(struct radeon_device *rdev, int ring,
 		  struct radeon_ib *ib, unsigned size)
 {
-	int r;
+	int i, r;
 
 	r = radeon_sa_bo_new(rdev, &rdev->ring_tmp_bo, &ib->sa_bo, size, 256, true);
 	if (r) {
 		dev_err(rdev->dev, "failed to get a new IB (%d)\n", r);
 		return r;
 	}
-	r = radeon_fence_create(rdev, &ib->fence, ring);
+
+	r = radeon_semaphore_create(rdev, &ib->semaphore);
 	if (r) {
-		dev_err(rdev->dev, "failed to create fence for new IB (%d)\n", r);
-		radeon_sa_bo_free(rdev, &ib->sa_bo, NULL);
 		return r;
 	}
 
+	ib->ring = ring;
+	ib->fence = NULL;
 	ib->ptr = radeon_sa_bo_cpu_addr(ib->sa_bo);
 	ib->gpu_addr = radeon_sa_bo_gpu_addr(ib->sa_bo);
 	ib->vm_id = 0;
 	ib->is_const_ib = false;
-	ib->semaphore = NULL;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i)
+		ib->sync_to[i] = NULL;
 
 	return 0;
 }
 
+/**
+ * radeon_ib_free - free an IB (Indirect Buffer)
+ *
+ * @rdev: radeon_device pointer
+ * @ib: IB object to free
+ *
+ * Free an IB (all asics).
+ */
 void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	radeon_semaphore_free(rdev, ib->semaphore, ib->fence);
+	radeon_semaphore_free(rdev, &ib->semaphore, ib->fence);
 	radeon_sa_bo_free(rdev, &ib->sa_bo, ib->fence);
 	radeon_fence_unref(&ib->fence);
 }
 
-int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
+/**
+ * radeon_ib_schedule - schedule an IB (Indirect Buffer) on the ring
+ *
+ * @rdev: radeon_device pointer
+ * @ib: IB object to schedule
+ * @const_ib: Const IB to schedule (SI only)
+ *
+ * Schedule an IB on the associated ring (all asics).
+ * Returns 0 on success, error on failure.
+ *
+ * On SI, there are two parallel engines fed from the primary ring,
+ * the CE (Constant Engine) and the DE (Drawing Engine).  Since
+ * resource descriptors have moved to memory, the CE allows you to
+ * prime the caches while the DE is updating register state so that
+ * the resource descriptors will be already in cache when the draw is
+ * processed.  To accomplish this, the userspace driver submits two
+ * IBs, one for the CE and one for the DE.  If there is a CE IB (called
+ * a CONST_IB), it will be put on the ring prior to the DE IB.  Prior
+ * to SI there was just a DE IB.
+ */
+int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
+		       struct radeon_ib *const_ib)
 {
-	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
-	int r = 0;
+	struct radeon_ring *ring = &rdev->ring[ib->ring];
+	bool need_sync = false;
+	int i, r = 0;
 
 	if (!ib->length_dw || !ring->ready) {
 		/* TODO: Nothings in the ib we should report. */
@@ -84,17 +134,51 @@ 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, ring, 64);
+	r = radeon_ring_lock(rdev, ring, 64 + RADEON_NUM_RINGS * 8);
 	if (r) {
 		dev_err(rdev->dev, "scheduling IB failed (%d).\n", r);
 		return r;
 	}
-	radeon_ring_ib_execute(rdev, ib->fence->ring, ib);
-	radeon_fence_emit(rdev, ib->fence);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_fence *fence = ib->sync_to[i];
+		if (radeon_fence_need_sync(fence, ib->ring)) {
+			need_sync = true;
+			radeon_semaphore_sync_rings(rdev, ib->semaphore,
+						    fence->ring, ib->ring);
+			radeon_fence_note_sync(fence, ib->ring);
+		}
+	}
+	/* immediately free semaphore when we don't need to sync */
+	if (!need_sync) {
+		radeon_semaphore_free(rdev, &ib->semaphore, NULL);
+	}
+	if (const_ib) {
+		radeon_ring_ib_execute(rdev, const_ib->ring, const_ib);
+		radeon_semaphore_free(rdev, &const_ib->semaphore, NULL);
+	}
+	radeon_ring_ib_execute(rdev, ib->ring, ib);
+	r = radeon_fence_emit(rdev, &ib->fence, ib->ring);
+	if (r) {
+		dev_err(rdev->dev, "failed to emit fence for new IB (%d)\n", r);
+		radeon_ring_unlock_undo(rdev, ring);
+		return r;
+	}
+	if (const_ib) {
+		const_ib->fence = radeon_fence_ref(ib->fence);
+	}
 	radeon_ring_unlock_commit(rdev, ring);
 	return 0;
 }
 
+/**
+ * radeon_ib_pool_init - Init the IB (Indirect Buffer) pool
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Initialize the suballocator to manage a pool of memory
+ * for use as IBs (all asics).
+ * Returns 0 on success, error on failure.
+ */
 int radeon_ib_pool_init(struct radeon_device *rdev)
 {
 	int r;
@@ -108,6 +192,12 @@ int radeon_ib_pool_init(struct radeon_device *rdev)
 	if (r) {
 		return r;
 	}
+
+	r = radeon_sa_bo_manager_start(rdev, &rdev->ring_tmp_bo);
+	if (r) {
+		return r;
+	}
+
 	rdev->ib_pool_ready = true;
 	if (radeon_debugfs_sa_init(rdev)) {
 		dev_err(rdev->dev, "failed to register debugfs file for SA\n");
@@ -115,24 +205,33 @@ int radeon_ib_pool_init(struct radeon_device *rdev)
 	return 0;
 }
 
+/**
+ * radeon_ib_pool_fini - Free the IB (Indirect Buffer) pool
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Tear down the suballocator managing the pool of memory
+ * for use as IBs (all asics).
+ */
 void radeon_ib_pool_fini(struct radeon_device *rdev)
 {
 	if (rdev->ib_pool_ready) {
+		radeon_sa_bo_manager_suspend(rdev, &rdev->ring_tmp_bo);
 		radeon_sa_bo_manager_fini(rdev, &rdev->ring_tmp_bo);
 		rdev->ib_pool_ready = false;
 	}
 }
 
-int radeon_ib_pool_start(struct radeon_device *rdev)
-{
-	return radeon_sa_bo_manager_start(rdev, &rdev->ring_tmp_bo);
-}
-
-int radeon_ib_pool_suspend(struct radeon_device *rdev)
-{
-	return radeon_sa_bo_manager_suspend(rdev, &rdev->ring_tmp_bo);
-}
-
+/**
+ * radeon_ib_ring_tests - test IBs on the rings
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Test an IB (Indirect Buffer) on each ring.
+ * If the test fails, disable the ring.
+ * Returns 0 on success, error if the primary GFX ring
+ * IB test fails.
+ */
 int radeon_ib_ring_tests(struct radeon_device *rdev)
 {
 	unsigned i;
@@ -164,10 +263,28 @@ int radeon_ib_ring_tests(struct radeon_device *rdev)
 }
 
 /*
- * Ring.
+ * Rings
+ * Most engines on the GPU are fed via ring buffers.  Ring
+ * buffers are areas of GPU accessible memory that the host
+ * writes commands into and the GPU reads commands out of.
+ * There is a rptr (read pointer) that determines where the
+ * GPU is currently reading, and a wptr (write pointer)
+ * which determines where the host has written.  When the
+ * pointers are equal, the ring is idle.  When the host
+ * writes commands to the ring buffer, it increments the
+ * wptr.  The GPU then starts fetching commands and executes
+ * them until the pointers are equal again.
  */
 int radeon_debugfs_ring_init(struct radeon_device *rdev, struct radeon_ring *ring);
 
+/**
+ * radeon_ring_write - write a value to the ring
+ *
+ * @ring: radeon_ring structure holding ring information
+ * @v: dword (dw) value to write
+ *
+ * Write a value to the requested ring buffer (all asics).
+ */
 void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 {
 #if DRM_DEBUG_CODE
@@ -181,21 +298,37 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 	ring->ring_free_dw--;
 }
 
-int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *ring)
+/**
+ * radeon_ring_supports_scratch_reg - check if the ring supports
+ * writing to scratch registers
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Check if a specific ring supports writing to scratch registers (all asics).
+ * Returns true if the ring supports writing to scratch regs, false if not.
+ */
+bool radeon_ring_supports_scratch_reg(struct radeon_device *rdev,
+				      struct radeon_ring *ring)
 {
-	/* r1xx-r5xx only has CP ring */
-	if (rdev->family < CHIP_R600)
-		return RADEON_RING_TYPE_GFX_INDEX;
-
-	if (rdev->family >= CHIP_CAYMAN) {
-		if (ring == &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX])
-			return CAYMAN_RING_TYPE_CP1_INDEX;
-		else if (ring == &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX])
-			return CAYMAN_RING_TYPE_CP2_INDEX;
+	switch (ring->idx) {
+	case RADEON_RING_TYPE_GFX_INDEX:
+	case CAYMAN_RING_TYPE_CP1_INDEX:
+	case CAYMAN_RING_TYPE_CP2_INDEX:
+		return true;
+	default:
+		return false;
 	}
-	return RADEON_RING_TYPE_GFX_INDEX;
 }
 
+/**
+ * radeon_ring_free_size - update the free size
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Update the free dw slots in the ring buffer (all asics).
+ */
 void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 rptr;
@@ -214,7 +347,16 @@ void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *ring)
 	}
 }
 
-
+/**
+ * radeon_ring_alloc - allocate space on the ring buffer
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ * @ndw: number of dwords to allocate in the ring buffer
+ *
+ * Allocate @ndw dwords in the ring buffer (all asics).
+ * Returns 0 on success, error on failure.
+ */
 int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ndw)
 {
 	int r;
@@ -227,7 +369,7 @@ int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *ring, unsi
 		if (ndw < ring->ring_free_dw) {
 			break;
 		}
-		r = radeon_fence_wait_next_locked(rdev, radeon_ring_index(rdev, ring));
+		r = radeon_fence_wait_next_locked(rdev, ring->idx);
 		if (r)
 			return r;
 	}
@@ -236,6 +378,17 @@ int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *ring, unsi
 	return 0;
 }
 
+/**
+ * radeon_ring_lock - lock the ring and allocate space on it
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ * @ndw: number of dwords to allocate in the ring buffer
+ *
+ * Lock the ring and allocate @ndw dwords in the ring buffer
+ * (all asics).
+ * Returns 0 on success, error on failure.
+ */
 int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ndw)
 {
 	int r;
@@ -249,15 +402,20 @@ int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	return 0;
 }
 
+/**
+ * radeon_ring_commit - tell the GPU to execute the new
+ * commands on the ring buffer
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Update the wptr (write pointer) to tell the GPU to
+ * execute new commands on the ring buffer (all asics).
+ */
 void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	unsigned count_dw_pad;
-	unsigned i;
-
 	/* We pad to match fetch size */
-	count_dw_pad = (ring->align_mask + 1) -
-		       (ring->wptr & ring->align_mask);
-	for (i = 0; i < count_dw_pad; i++) {
+	while (ring->wptr & ring->align_mask) {
 		radeon_ring_write(ring, ring->nop);
 	}
 	DRM_MEMORYBARRIER();
@@ -265,23 +423,55 @@ void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 	(void)RREG32(ring->wptr_reg);
 }
 
+/**
+ * radeon_ring_unlock_commit - tell the GPU to execute the new
+ * commands on the ring buffer and unlock it
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Call radeon_ring_commit() then unlock the ring (all asics).
+ */
 void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	radeon_ring_commit(rdev, ring);
 	mutex_unlock(&rdev->ring_lock);
 }
 
+/**
+ * radeon_ring_undo - reset the wptr
+ *
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Reset the driver's copy of the wtpr (all asics).
+ */
 void radeon_ring_undo(struct radeon_ring *ring)
 {
 	ring->wptr = ring->wptr_old;
 }
 
+/**
+ * radeon_ring_unlock_undo - reset the wptr and unlock the ring
+ *
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Call radeon_ring_undo() then unlock the ring (all asics).
+ */
 void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	radeon_ring_undo(ring);
 	mutex_unlock(&rdev->ring_lock);
 }
 
+/**
+ * radeon_ring_force_activity - add some nop packets to the ring
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Add some nop packets to the ring to force activity (all asics).
+ * Used for lockup detection to see if the rptr is advancing.
+ */
 void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	int r;
@@ -296,6 +486,13 @@ void radeon_ring_force_activity(struct radeon_device *rdev, struct radeon_ring *
 	}
 }
 
+/**
+ * radeon_ring_force_activity - update lockup variables
+ *
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Update the last rptr value and timestamp (all asics).
+ */
 void radeon_ring_lockup_update(struct radeon_ring *ring)
 {
 	ring->last_rptr = ring->rptr;
@@ -349,6 +546,116 @@ bool radeon_ring_test_lockup(struct radeon_device *rdev, struct radeon_ring *rin
 	return false;
 }
 
+/**
+ * radeon_ring_backup - Back up the content of a ring
+ *
+ * @rdev: radeon_device pointer
+ * @ring: the ring we want to back up
+ *
+ * Saves all unprocessed commits from a ring, returns the number of dwords saved.
+ */
+unsigned radeon_ring_backup(struct radeon_device *rdev, struct radeon_ring *ring,
+			    uint32_t **data)
+{
+	unsigned size, ptr, i;
+
+	/* just in case lock the ring */
+	mutex_lock(&rdev->ring_lock);
+	*data = NULL;
+
+	if (ring->ring_obj == NULL) {
+		mutex_unlock(&rdev->ring_lock);
+		return 0;
+	}
+
+	/* it doesn't make sense to save anything if all fences are signaled */
+	if (!radeon_fence_count_emitted(rdev, ring->idx)) {
+		mutex_unlock(&rdev->ring_lock);
+		return 0;
+	}
+
+	/* calculate the number of dw on the ring */
+	if (ring->rptr_save_reg)
+		ptr = RREG32(ring->rptr_save_reg);
+	else if (rdev->wb.enabled)
+		ptr = le32_to_cpu(*ring->next_rptr_cpu_addr);
+	else {
+		/* no way to read back the next rptr */
+		mutex_unlock(&rdev->ring_lock);
+		return 0;
+	}
+
+	size = ring->wptr + (ring->ring_size / 4);
+	size -= ptr;
+	size &= ring->ptr_mask;
+	if (size == 0) {
+		mutex_unlock(&rdev->ring_lock);
+		return 0;
+	}
+
+	/* and then save the content of the ring */
+	*data = kmalloc_array(size, sizeof(uint32_t), GFP_KERNEL);
+	if (!*data) {
+		mutex_unlock(&rdev->ring_lock);
+		return 0;
+	}
+	for (i = 0; i < size; ++i) {
+		(*data)[i] = ring->ring[ptr++];
+		ptr &= ring->ptr_mask;
+	}
+
+	mutex_unlock(&rdev->ring_lock);
+	return size;
+}
+
+/**
+ * radeon_ring_restore - append saved commands to the ring again
+ *
+ * @rdev: radeon_device pointer
+ * @ring: ring to append commands to
+ * @size: number of dwords we want to write
+ * @data: saved commands
+ *
+ * Allocates space on the ring and restore the previously saved commands.
+ */
+int radeon_ring_restore(struct radeon_device *rdev, struct radeon_ring *ring,
+			unsigned size, uint32_t *data)
+{
+	int i, r;
+
+	if (!size || !data)
+		return 0;
+
+	/* restore the saved ring content */
+	r = radeon_ring_lock(rdev, ring, size);
+	if (r)
+		return r;
+
+	for (i = 0; i < size; ++i) {
+		radeon_ring_write(ring, data[i]);
+	}
+
+	radeon_ring_unlock_commit(rdev, ring);
+	kfree(data);
+	return 0;
+}
+
+/**
+ * radeon_ring_init - init driver ring struct.
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ * @ring_size: size of the ring
+ * @rptr_offs: offset of the rptr writeback location in the WB buffer
+ * @rptr_reg: MMIO offset of the rptr register
+ * @wptr_reg: MMIO offset of the wptr register
+ * @ptr_reg_shift: bit offset of the rptr/wptr values
+ * @ptr_reg_mask: bit mask of the rptr/wptr values
+ * @nop: nop packet for this ring
+ *
+ * Initialize the driver information for the selected ring (all asics).
+ * Returns 0 on success, error on failure.
+ */
 int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ring_size,
 		     unsigned rptr_offs, unsigned rptr_reg, unsigned wptr_reg,
 		     u32 ptr_reg_shift, u32 ptr_reg_mask, u32 nop)
@@ -391,12 +698,25 @@ int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	}
 	ring->ptr_mask = (ring->ring_size / 4) - 1;
 	ring->ring_free_dw = ring->ring_size / 4;
+	if (rdev->wb.enabled) {
+		u32 index = RADEON_WB_RING0_NEXT_RPTR + (ring->idx * 4);
+		ring->next_rptr_gpu_addr = rdev->wb.gpu_addr + index;
+		ring->next_rptr_cpu_addr = &rdev->wb.wb[index/4];
+	}
 	if (radeon_debugfs_ring_init(rdev, ring)) {
 		DRM_ERROR("Failed to register debugfs file for rings !\n");
 	}
 	return 0;
 }
 
+/**
+ * radeon_ring_fini - tear down the driver ring struct.
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon_ring structure holding ring information
+ *
+ * Tear down the driver information for the selected ring (all asics).
+ */
 void radeon_ring_fini(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	int r;
@@ -438,6 +758,10 @@ static int radeon_debugfs_ring_info(struct seq_file *m, void *data)
 	count = (ring->ring_size / 4) - ring->ring_free_dw;
 	seq_printf(m, "wptr(0x%04x): 0x%08x\n", ring->wptr_reg, RREG32(ring->wptr_reg));
 	seq_printf(m, "rptr(0x%04x): 0x%08x\n", ring->rptr_reg, RREG32(ring->rptr_reg));
+	if (ring->rptr_save_reg) {
+		seq_printf(m, "rptr next(0x%04x): 0x%08x\n", ring->rptr_save_reg,
+			   RREG32(ring->rptr_save_reg));
+	}
 	seq_printf(m, "driver's copy of the wptr: 0x%08x\n", ring->wptr);
 	seq_printf(m, "driver's copy of the rptr: 0x%08x\n", ring->rptr);
 	seq_printf(m, "%u free dwords in ring\n", ring->ring_free_dw);
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
index 32059b74572..4e771240fdd 100644
--- a/drivers/gpu/drm/radeon/radeon_sa.c
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -54,7 +54,7 @@ int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 {
 	int i, r;
 
-	spin_lock_init(&sa_manager->lock);
+	init_waitqueue_head(&sa_manager->wq);
 	sa_manager->bo = NULL;
 	sa_manager->size = size;
 	sa_manager->domain = domain;
@@ -211,6 +211,39 @@ static bool radeon_sa_bo_try_alloc(struct radeon_sa_manager *sa_manager,
 	return false;
 }
 
+/**
+ * radeon_sa_event - Check if we can stop waiting
+ *
+ * @sa_manager: pointer to the sa_manager
+ * @size: number of bytes we want to allocate
+ * @align: alignment we need to match
+ *
+ * Check if either there is a fence we can wait for or
+ * enough free memory to satisfy the allocation directly
+ */
+static bool radeon_sa_event(struct radeon_sa_manager *sa_manager,
+			    unsigned size, unsigned align)
+{
+	unsigned soffset, eoffset, wasted;
+	int i;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!list_empty(&sa_manager->flist[i])) {
+			return true;
+		}
+	}
+
+	soffset = radeon_sa_bo_hole_soffset(sa_manager);
+	eoffset = radeon_sa_bo_hole_eoffset(sa_manager);
+	wasted = (align - (soffset % align)) % align;
+
+	if ((eoffset - soffset) >= (size + wasted)) {
+		return true;
+	}
+
+	return false;
+}
+
 static bool radeon_sa_bo_next_hole(struct radeon_sa_manager *sa_manager,
 				   struct radeon_fence **fences,
 				   unsigned *tries)
@@ -297,8 +330,8 @@ int radeon_sa_bo_new(struct radeon_device *rdev,
 	INIT_LIST_HEAD(&(*sa_bo)->olist);
 	INIT_LIST_HEAD(&(*sa_bo)->flist);
 
-	spin_lock(&sa_manager->lock);
-	do {
+	spin_lock(&sa_manager->wq.lock);
+	while(1) {
 		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
 			fences[i] = NULL;
 			tries[i] = 0;
@@ -309,30 +342,34 @@ int radeon_sa_bo_new(struct radeon_device *rdev,
 
 			if (radeon_sa_bo_try_alloc(sa_manager, *sa_bo,
 						   size, align)) {
-				spin_unlock(&sa_manager->lock);
+				spin_unlock(&sa_manager->wq.lock);
 				return 0;
 			}
 
 			/* see if we can skip over some allocations */
 		} while (radeon_sa_bo_next_hole(sa_manager, fences, tries));
 
-		if (block) {
-			spin_unlock(&sa_manager->lock);
-			r = radeon_fence_wait_any(rdev, fences, false);
-			spin_lock(&sa_manager->lock);
-			if (r) {
-				/* if we have nothing to wait for we
-				   are practically out of memory */
-				if (r == -ENOENT) {
-					r = -ENOMEM;
-				}
-				goto out_err;
-			}
+		if (!block) {
+			break;
+		}
+
+		spin_unlock(&sa_manager->wq.lock);
+		r = radeon_fence_wait_any(rdev, fences, false);
+		spin_lock(&sa_manager->wq.lock);
+		/* if we have nothing to wait for block */
+		if (r == -ENOENT) {
+			r = wait_event_interruptible_locked(
+				sa_manager->wq, 
+				radeon_sa_event(sa_manager, size, align)
+			);
+		}
+		if (r) {
+			goto out_err;
 		}
-	} while (block);
+	};
 
 out_err:
-	spin_unlock(&sa_manager->lock);
+	spin_unlock(&sa_manager->wq.lock);
 	kfree(*sa_bo);
 	*sa_bo = NULL;
 	return r;
@@ -348,15 +385,16 @@ void radeon_sa_bo_free(struct radeon_device *rdev, struct radeon_sa_bo **sa_bo,
 	}
 
 	sa_manager = (*sa_bo)->manager;
-	spin_lock(&sa_manager->lock);
-	if (fence && fence->seq && fence->seq < RADEON_FENCE_NOTEMITED_SEQ) {
+	spin_lock(&sa_manager->wq.lock);
+	if (fence && !radeon_fence_signaled(fence)) {
 		(*sa_bo)->fence = radeon_fence_ref(fence);
 		list_add_tail(&(*sa_bo)->flist,
 			      &sa_manager->flist[fence->ring]);
 	} else {
 		radeon_sa_bo_remove_locked(*sa_bo);
 	}
-	spin_unlock(&sa_manager->lock);
+	wake_up_all_locked(&sa_manager->wq);
+	spin_unlock(&sa_manager->wq.lock);
 	*sa_bo = NULL;
 }
 
@@ -366,7 +404,7 @@ void radeon_sa_bo_dump_debug_info(struct radeon_sa_manager *sa_manager,
 {
 	struct radeon_sa_bo *i;
 
-	spin_lock(&sa_manager->lock);
+	spin_lock(&sa_manager->wq.lock);
 	list_for_each_entry(i, &sa_manager->olist, olist) {
 		if (&i->olist == sa_manager->hole) {
 			seq_printf(m, ">");
@@ -381,6 +419,6 @@ void radeon_sa_bo_dump_debug_info(struct radeon_sa_manager *sa_manager,
 		}
 		seq_printf(m, "\n");
 	}
-	spin_unlock(&sa_manager->lock);
+	spin_unlock(&sa_manager->wq.lock);
 }
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_semaphore.c b/drivers/gpu/drm/radeon/radeon_semaphore.c
index e2ace5dce11..7cc78de6ddc 100644
--- a/drivers/gpu/drm/radeon/radeon_semaphore.c
+++ b/drivers/gpu/drm/radeon/radeon_semaphore.c
@@ -68,70 +68,49 @@ void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, true);
 }
 
+/* caller must hold ring lock */
 int radeon_semaphore_sync_rings(struct radeon_device *rdev,
 				struct radeon_semaphore *semaphore,
-				bool sync_to[RADEON_NUM_RINGS],
-				int dst_ring)
+				int signaler, int waiter)
 {
-	int i = 0, r;
+	int r;
 
-	mutex_lock(&rdev->ring_lock);
-	r = radeon_ring_alloc(rdev, &rdev->ring[dst_ring], RADEON_NUM_RINGS * 8);
-	if (r) {
-		goto error;
+	/* no need to signal and wait on the same ring */
+	if (signaler == waiter) {
+		return 0;
 	}
 
-	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-		/* no need to sync to our own or unused rings */
-		if (!sync_to[i] || i == dst_ring)
-			continue;
-
-		/* prevent GPU deadlocks */
-		if (!rdev->ring[i].ready) {
-			dev_err(rdev->dev, "Trying to sync to a disabled ring!");
-			r = -EINVAL;
-			goto error;
-		}
-
-		r = radeon_ring_alloc(rdev, &rdev->ring[i], 8);
-		if (r) {
-			goto error;
-		}
-
-		radeon_semaphore_emit_signal(rdev, i, semaphore);
-		radeon_semaphore_emit_wait(rdev, dst_ring, semaphore);
+	/* prevent GPU deadlocks */
+	if (!rdev->ring[signaler].ready) {
+		dev_err(rdev->dev, "Trying to sync to a disabled ring!");
+		return -EINVAL;
+	}
 
-		radeon_ring_commit(rdev, &rdev->ring[i]);
+	r = radeon_ring_alloc(rdev, &rdev->ring[signaler], 8);
+	if (r) {
+		return r;
 	}
+	radeon_semaphore_emit_signal(rdev, signaler, semaphore);
+	radeon_ring_commit(rdev, &rdev->ring[signaler]);
 
-	radeon_ring_commit(rdev, &rdev->ring[dst_ring]);
-	mutex_unlock(&rdev->ring_lock);
+	/* we assume caller has already allocated space on waiters ring */
+	radeon_semaphore_emit_wait(rdev, waiter, semaphore);
 
 	return 0;
-
-error:
-	/* unlock all locks taken so far */
-	for (--i; i >= 0; --i) {
-		if (sync_to[i] || i == dst_ring) {
-			radeon_ring_undo(&rdev->ring[i]);
-		}
-	}
-	radeon_ring_undo(&rdev->ring[dst_ring]);
-	mutex_unlock(&rdev->ring_lock);
-	return r;
 }
 
 void radeon_semaphore_free(struct radeon_device *rdev,
-			   struct radeon_semaphore *semaphore,
+			   struct radeon_semaphore **semaphore,
 			   struct radeon_fence *fence)
 {
-	if (semaphore == NULL) {
+	if (semaphore == NULL || *semaphore == NULL) {
 		return;
 	}
-	if (semaphore->waiters > 0) {
+	if ((*semaphore)->waiters > 0) {
 		dev_err(rdev->dev, "semaphore %p has more waiters than signalers,"
-			" hardware lockup imminent!\n", semaphore);
+			" hardware lockup imminent!\n", *semaphore);
 	}
-	radeon_sa_bo_free(rdev, &semaphore->sa_bo, fence);
-	kfree(semaphore);
+	radeon_sa_bo_free(rdev, &(*semaphore)->sa_bo, fence);
+	kfree(*semaphore);
+	*semaphore = NULL;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index efff929ea49..7c16540c10f 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -106,13 +106,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 
 		radeon_bo_kunmap(gtt_obj[i]);
 
-		r = radeon_fence_create(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
-		if (r) {
-			DRM_ERROR("Failed to create GTT->VRAM fence %d\n", i);
-			goto out_cleanup;
-		}
-
-		r = radeon_copy(rdev, gtt_addr, vram_addr, size / RADEON_GPU_PAGE_SIZE, fence);
+		r = radeon_copy(rdev, gtt_addr, vram_addr, size / RADEON_GPU_PAGE_SIZE, &fence);
 		if (r) {
 			DRM_ERROR("Failed GTT->VRAM copy %d\n", i);
 			goto out_cleanup;
@@ -155,13 +149,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 
 		radeon_bo_kunmap(vram_obj);
 
-		r = radeon_fence_create(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
-		if (r) {
-			DRM_ERROR("Failed to create VRAM->GTT fence %d\n", i);
-			goto out_cleanup;
-		}
-
-		r = radeon_copy(rdev, vram_addr, gtt_addr, size / RADEON_GPU_PAGE_SIZE, fence);
+		r = radeon_copy(rdev, vram_addr, gtt_addr, size / RADEON_GPU_PAGE_SIZE, &fence);
 		if (r) {
 			DRM_ERROR("Failed VRAM->GTT copy %d\n", i);
 			goto out_cleanup;
@@ -241,36 +229,33 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 {
 	struct radeon_fence *fence1 = NULL, *fence2 = NULL;
 	struct radeon_semaphore *semaphore = NULL;
-	int ridxA = radeon_ring_index(rdev, ringA);
-	int ridxB = radeon_ring_index(rdev, ringB);
 	int r;
 
-	r = radeon_fence_create(rdev, &fence1, ridxA);
+	r = radeon_semaphore_create(rdev, &semaphore);
 	if (r) {
-		DRM_ERROR("Failed to create sync fence 1\n");
+		DRM_ERROR("Failed to create semaphore\n");
 		goto out_cleanup;
 	}
-	r = radeon_fence_create(rdev, &fence2, ridxA);
+
+	r = radeon_ring_lock(rdev, ringA, 64);
 	if (r) {
-		DRM_ERROR("Failed to create sync fence 2\n");
+		DRM_ERROR("Failed to lock ring A %d\n", ringA->idx);
 		goto out_cleanup;
 	}
-
-	r = radeon_semaphore_create(rdev, &semaphore);
+	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
+	r = radeon_fence_emit(rdev, &fence1, ringA->idx);
 	if (r) {
-		DRM_ERROR("Failed to create semaphore\n");
+		DRM_ERROR("Failed to emit fence 1\n");
+		radeon_ring_unlock_undo(rdev, ringA);
 		goto out_cleanup;
 	}
-
-	r = radeon_ring_lock(rdev, ringA, 64);
+	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
+	r = radeon_fence_emit(rdev, &fence2, ringA->idx);
 	if (r) {
-		DRM_ERROR("Failed to lock ring A %d\n", ridxA);
+		DRM_ERROR("Failed to emit fence 2\n");
+		radeon_ring_unlock_undo(rdev, ringA);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_wait(rdev, ridxA, semaphore);
-	radeon_fence_emit(rdev, fence1);
-	radeon_semaphore_emit_wait(rdev, ridxA, semaphore);
-	radeon_fence_emit(rdev, fence2);
 	radeon_ring_unlock_commit(rdev, ringA);
 
 	mdelay(1000);
@@ -285,7 +270,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		DRM_ERROR("Failed to lock ring B %p\n", ringB);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_signal(rdev, ridxB, semaphore);
+	radeon_semaphore_emit_signal(rdev, ringB->idx, semaphore);
 	radeon_ring_unlock_commit(rdev, ringB);
 
 	r = radeon_fence_wait(fence1, false);
@@ -306,7 +291,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		DRM_ERROR("Failed to lock ring B %p\n", ringB);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_signal(rdev, ridxB, semaphore);
+	radeon_semaphore_emit_signal(rdev, ringB->idx, semaphore);
 	radeon_ring_unlock_commit(rdev, ringB);
 
 	r = radeon_fence_wait(fence2, false);
@@ -316,8 +301,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 	}
 
 out_cleanup:
-	if (semaphore)
-		radeon_semaphore_free(rdev, semaphore, NULL);
+	radeon_semaphore_free(rdev, &semaphore, NULL);
 
 	if (fence1)
 		radeon_fence_unref(&fence1);
@@ -336,23 +320,9 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 {
 	struct radeon_fence *fenceA = NULL, *fenceB = NULL;
 	struct radeon_semaphore *semaphore = NULL;
-	int ridxA = radeon_ring_index(rdev, ringA);
-	int ridxB = radeon_ring_index(rdev, ringB);
-	int ridxC = radeon_ring_index(rdev, ringC);
 	bool sigA, sigB;
 	int i, r;
 
-	r = radeon_fence_create(rdev, &fenceA, ridxA);
-	if (r) {
-		DRM_ERROR("Failed to create sync fence 1\n");
-		goto out_cleanup;
-	}
-	r = radeon_fence_create(rdev, &fenceB, ridxB);
-	if (r) {
-		DRM_ERROR("Failed to create sync fence 2\n");
-		goto out_cleanup;
-	}
-
 	r = radeon_semaphore_create(rdev, &semaphore);
 	if (r) {
 		DRM_ERROR("Failed to create semaphore\n");
@@ -361,20 +331,30 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 
 	r = radeon_ring_lock(rdev, ringA, 64);
 	if (r) {
-		DRM_ERROR("Failed to lock ring A %d\n", ridxA);
+		DRM_ERROR("Failed to lock ring A %d\n", ringA->idx);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
+	r = radeon_fence_emit(rdev, &fenceA, ringA->idx);
+	if (r) {
+		DRM_ERROR("Failed to emit sync fence 1\n");
+		radeon_ring_unlock_undo(rdev, ringA);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_wait(rdev, ridxA, semaphore);
-	radeon_fence_emit(rdev, fenceA);
 	radeon_ring_unlock_commit(rdev, ringA);
 
 	r = radeon_ring_lock(rdev, ringB, 64);
 	if (r) {
-		DRM_ERROR("Failed to lock ring B %d\n", ridxB);
+		DRM_ERROR("Failed to lock ring B %d\n", ringB->idx);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_wait(rdev, ringB->idx, semaphore);
+	r = radeon_fence_emit(rdev, &fenceB, ringB->idx);
+	if (r) {
+		DRM_ERROR("Failed to create sync fence 2\n");
+		radeon_ring_unlock_undo(rdev, ringB);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_wait(rdev, ridxB, semaphore);
-	radeon_fence_emit(rdev, fenceB);
 	radeon_ring_unlock_commit(rdev, ringB);
 
 	mdelay(1000);
@@ -393,7 +373,7 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 		DRM_ERROR("Failed to lock ring B %p\n", ringC);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_signal(rdev, ridxC, semaphore);
+	radeon_semaphore_emit_signal(rdev, ringC->idx, semaphore);
 	radeon_ring_unlock_commit(rdev, ringC);
 
 	for (i = 0; i < 30; ++i) {
@@ -419,7 +399,7 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 		DRM_ERROR("Failed to lock ring B %p\n", ringC);
 		goto out_cleanup;
 	}
-	radeon_semaphore_emit_signal(rdev, ridxC, semaphore);
+	radeon_semaphore_emit_signal(rdev, ringC->idx, semaphore);
 	radeon_ring_unlock_commit(rdev, ringC);
 
 	mdelay(1000);
@@ -436,8 +416,7 @@ void radeon_test_ring_sync2(struct radeon_device *rdev,
 	}
 
 out_cleanup:
-	if (semaphore)
-		radeon_semaphore_free(rdev, semaphore, NULL);
+	radeon_semaphore_free(rdev, &semaphore, NULL);
 
 	if (fenceA)
 		radeon_fence_unref(&fenceA);
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index c94a2257761..5b71c716d83 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -222,15 +222,11 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 {
 	struct radeon_device *rdev;
 	uint64_t old_start, new_start;
-	struct radeon_fence *fence, *old_fence;
-	struct radeon_semaphore *sem = NULL;
-	int r;
+	struct radeon_fence *fence;
+	int r, ridx;
 
 	rdev = radeon_get_rdev(bo->bdev);
-	r = radeon_fence_create(rdev, &fence, radeon_copy_ring_index(rdev));
-	if (unlikely(r)) {
-		return r;
-	}
+	ridx = radeon_copy_ring_index(rdev);
 	old_start = old_mem->start << PAGE_SHIFT;
 	new_start = new_mem->start << PAGE_SHIFT;
 
@@ -243,7 +239,6 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		break;
 	default:
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
-		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 	switch (new_mem->mem_type) {
@@ -255,46 +250,23 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		break;
 	default:
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
-		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
-	if (!rdev->ring[radeon_copy_ring_index(rdev)].ready) {
+	if (!rdev->ring[ridx].ready) {
 		DRM_ERROR("Trying to move memory with ring turned off.\n");
-		radeon_fence_unref(&fence);
 		return -EINVAL;
 	}
 
 	BUILD_BUG_ON((PAGE_SIZE % RADEON_GPU_PAGE_SIZE) != 0);
 
 	/* sync other rings */
-	old_fence = bo->sync_obj;
-	if (old_fence && old_fence->ring != fence->ring
-	    && !radeon_fence_signaled(old_fence)) {
-		bool sync_to_ring[RADEON_NUM_RINGS] = { };
-		sync_to_ring[old_fence->ring] = true;
-
-		r = radeon_semaphore_create(rdev, &sem);
-		if (r) {
-			radeon_fence_unref(&fence);
-			return r;
-		}
-
-		r = radeon_semaphore_sync_rings(rdev, sem,
-						sync_to_ring, fence->ring);
-		if (r) {
-			radeon_semaphore_free(rdev, sem, NULL);
-			radeon_fence_unref(&fence);
-			return r;
-		}
-	}
-
+	fence = bo->sync_obj;
 	r = radeon_copy(rdev, old_start, new_start,
 			new_mem->num_pages * (PAGE_SIZE / RADEON_GPU_PAGE_SIZE), /* GPU pages */
-			fence);
+			&fence);
 	/* FIXME: handle copy error */
 	r = ttm_bo_move_accel_cleanup(bo, (void *)fence, NULL,
 				      evict, no_wait_reserve, no_wait_gpu, new_mem);
-	radeon_semaphore_free(rdev, sem, fence);
 	radeon_fence_unref(&fence);
 	return r;
 }
@@ -762,9 +734,7 @@ int radeon_ttm_init(struct radeon_device *rdev)
 	}
 	DRM_INFO("radeon: %uM of GTT memory ready.\n",
 		 (unsigned)(rdev->mc.gtt_size / (1024 * 1024)));
-	if (unlikely(rdev->mman.bdev.dev_mapping == NULL)) {
-		rdev->mman.bdev.dev_mapping = rdev->ddev->dev_mapping;
-	}
+	rdev->mman.bdev.dev_mapping = rdev->ddev->dev_mapping;
 
 	r = radeon_ttm_debugfs_init(rdev);
 	if (r) {
@@ -825,9 +795,9 @@ static int radeon_ttm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 		return VM_FAULT_NOPAGE;
 	}
 	rdev = radeon_get_rdev(bo->bdev);
-	mutex_lock(&rdev->vram_mutex);
+	down_read(&rdev->pm.mclk_lock);
 	r = ttm_vm_ops->fault(vma, vmf);
-	mutex_unlock(&rdev->vram_mutex);
+	up_read(&rdev->pm.mclk_lock);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/rs400.c b/drivers/gpu/drm/radeon/rs400.c
index a464eb5e2df..2752f7f7823 100644
--- a/drivers/gpu/drm/radeon/rs400.c
+++ b/drivers/gpu/drm/radeon/rs400.c
@@ -426,13 +426,11 @@ static int rs400_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	return 0;
 }
@@ -470,7 +468,6 @@ int rs400_resume(struct radeon_device *rdev)
 
 int rs400_suspend(struct radeon_device *rdev)
 {
-	radeon_ib_pool_suspend(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
 	r100_irq_disable(rdev);
@@ -482,7 +479,7 @@ void rs400_fini(struct radeon_device *rdev)
 {
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	rs400_gart_fini(rdev);
 	radeon_irq_kms_fini(rdev);
@@ -550,20 +547,14 @@ int rs400_init(struct radeon_device *rdev)
 		return r;
 	r300_set_reg_safe(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = rs400_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		rs400_gart_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rdev->accel_working = false;
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index e95c5e61d4e..5301b3df846 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -294,6 +294,7 @@ void rs600_hpd_init(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned enable = 0;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
@@ -301,26 +302,25 @@ void rs600_hpd_init(struct radeon_device *rdev)
 		case RADEON_HPD_1:
 			WREG32(R_007D00_DC_HOT_PLUG_DETECT1_CONTROL,
 			       S_007D00_DC_HOT_PLUG_DETECT1_EN(1));
-			rdev->irq.hpd[0] = true;
 			break;
 		case RADEON_HPD_2:
 			WREG32(R_007D10_DC_HOT_PLUG_DETECT2_CONTROL,
 			       S_007D10_DC_HOT_PLUG_DETECT2_EN(1));
-			rdev->irq.hpd[1] = true;
 			break;
 		default:
 			break;
 		}
+		enable |= 1 << radeon_connector->hpd.hpd;
 		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
-	if (rdev->irq.installed)
-		rs600_irq_set(rdev);
+	radeon_irq_kms_enable_hpd(rdev, enable);
 }
 
 void rs600_hpd_fini(struct radeon_device *rdev)
 {
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
+	unsigned disable = 0;
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
@@ -328,17 +328,17 @@ void rs600_hpd_fini(struct radeon_device *rdev)
 		case RADEON_HPD_1:
 			WREG32(R_007D00_DC_HOT_PLUG_DETECT1_CONTROL,
 			       S_007D00_DC_HOT_PLUG_DETECT1_EN(0));
-			rdev->irq.hpd[0] = false;
 			break;
 		case RADEON_HPD_2:
 			WREG32(R_007D10_DC_HOT_PLUG_DETECT2_CONTROL,
 			       S_007D10_DC_HOT_PLUG_DETECT2_EN(0));
-			rdev->irq.hpd[1] = false;
 			break;
 		default:
 			break;
 		}
+		disable |= 1 << radeon_connector->hpd.hpd;
 	}
+	radeon_irq_kms_disable_hpd(rdev, disable);
 }
 
 int rs600_asic_reset(struct radeon_device *rdev)
@@ -564,18 +564,18 @@ int rs600_irq_set(struct radeon_device *rdev)
 		WREG32(R_000040_GEN_INT_CNTL, 0);
 		return -EINVAL;
 	}
-	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+	if (atomic_read(&rdev->irq.ring_int[RADEON_RING_TYPE_GFX_INDEX])) {
 		tmp |= S_000040_SW_INT_EN(1);
 	}
 	if (rdev->irq.gui_idle) {
 		tmp |= S_000040_GUI_IDLE(1);
 	}
 	if (rdev->irq.crtc_vblank_int[0] ||
-	    rdev->irq.pflip[0]) {
+	    atomic_read(&rdev->irq.pflip[0])) {
 		mode_int |= S_006540_D1MODE_VBLANK_INT_MASK(1);
 	}
 	if (rdev->irq.crtc_vblank_int[1] ||
-	    rdev->irq.pflip[1]) {
+	    atomic_read(&rdev->irq.pflip[1])) {
 		mode_int |= S_006540_D2MODE_VBLANK_INT_MASK(1);
 	}
 	if (rdev->irq.hpd[0]) {
@@ -686,7 +686,6 @@ int rs600_irq_process(struct radeon_device *rdev)
 		/* GUI idle */
 		if (G_000040_GUI_IDLE(status)) {
 			rdev->irq.gui_idle_acked = true;
-			rdev->pm.gui_idle = true;
 			wake_up(&rdev->irq.idle_queue);
 		}
 		/* Vertical blank interrupts */
@@ -696,7 +695,7 @@ int rs600_irq_process(struct radeon_device *rdev)
 				rdev->pm.vblank_sync = true;
 				wake_up(&rdev->irq.vblank_queue);
 			}
-			if (rdev->irq.pflip[0])
+			if (atomic_read(&rdev->irq.pflip[0]))
 				radeon_crtc_handle_flip(rdev, 0);
 		}
 		if (G_007EDC_LB_D2_VBLANK_INTERRUPT(rdev->irq.stat_regs.r500.disp_int)) {
@@ -705,7 +704,7 @@ int rs600_irq_process(struct radeon_device *rdev)
 				rdev->pm.vblank_sync = true;
 				wake_up(&rdev->irq.vblank_queue);
 			}
-			if (rdev->irq.pflip[1])
+			if (atomic_read(&rdev->irq.pflip[1]))
 				radeon_crtc_handle_flip(rdev, 1);
 		}
 		if (G_007EDC_DC_HOT_PLUG_DETECT1_INTERRUPT(rdev->irq.stat_regs.r500.disp_int)) {
@@ -908,13 +907,11 @@ static int rs600_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -956,7 +953,6 @@ int rs600_resume(struct radeon_device *rdev)
 
 int rs600_suspend(struct radeon_device *rdev)
 {
-	radeon_ib_pool_suspend(rdev);
 	r600_audio_fini(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
@@ -970,7 +966,7 @@ void rs600_fini(struct radeon_device *rdev)
 	r600_audio_fini(rdev);
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	rs600_gart_fini(rdev);
 	radeon_irq_kms_fini(rdev);
@@ -1038,20 +1034,14 @@ int rs600_init(struct radeon_device *rdev)
 		return r;
 	rs600_set_safe_registers(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = rs600_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		rs600_gart_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rdev->accel_working = false;
diff --git a/drivers/gpu/drm/radeon/rs690.c b/drivers/gpu/drm/radeon/rs690.c
index 159b6a43fda..3b663fcfe06 100644
--- a/drivers/gpu/drm/radeon/rs690.c
+++ b/drivers/gpu/drm/radeon/rs690.c
@@ -637,13 +637,11 @@ static int rs690_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -685,7 +683,6 @@ int rs690_resume(struct radeon_device *rdev)
 
 int rs690_suspend(struct radeon_device *rdev)
 {
-	radeon_ib_pool_suspend(rdev);
 	r600_audio_fini(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
@@ -699,7 +696,7 @@ void rs690_fini(struct radeon_device *rdev)
 	r600_audio_fini(rdev);
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	rs400_gart_fini(rdev);
 	radeon_irq_kms_fini(rdev);
@@ -768,20 +765,14 @@ int rs690_init(struct radeon_device *rdev)
 		return r;
 	rs600_set_safe_registers(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = rs690_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		rs400_gart_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rdev->accel_working = false;
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index 7f08cedb533..a12fbcc8ccb 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -408,13 +408,11 @@ static int rv515_startup(struct radeon_device *rdev)
 		return r;
 	}
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	return 0;
 }
@@ -469,7 +467,7 @@ void rv515_fini(struct radeon_device *rdev)
 {
 	r100_cp_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_gem_fini(rdev);
 	rv370_pcie_gart_fini(rdev);
 	radeon_agp_fini(rdev);
@@ -543,20 +541,14 @@ int rv515_init(struct radeon_device *rdev)
 		return r;
 	rv515_set_safe_registers(rdev);
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = rv515_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
 		dev_err(rdev->dev, "Disabling GPU acceleration\n");
 		r100_cp_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rv370_pcie_gart_fini(rdev);
 		radeon_agp_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index b4f51c569c3..ca8ffec10ff 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -358,8 +358,10 @@ static int rv770_cp_load_microcode(struct radeon_device *rdev)
 
 void r700_cp_fini(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	r700_cp_stop(rdev);
-	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
+	radeon_ring_fini(rdev, ring);
+	radeon_scratch_free(rdev, ring->rptr_save_reg);
 }
 
 /*
@@ -951,13 +953,11 @@ static int rv770_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
+	r = radeon_ib_pool_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
+	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -994,9 +994,6 @@ int rv770_resume(struct radeon_device *rdev)
 int rv770_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
-	radeon_ib_pool_suspend(rdev);
-	r600_blit_suspend(rdev);
-	/* FIXME: we should wait for ring to be empty */
 	r700_cp_stop(rdev);
 	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	r600_irq_suspend(rdev);
@@ -1076,20 +1073,14 @@ int rv770_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-
 	r = rv770_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		r700_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rv770_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
@@ -1104,7 +1095,7 @@ void rv770_fini(struct radeon_device *rdev)
 	r700_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	rv770_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
@@ -1121,6 +1112,8 @@ static void rv770_pcie_gen2_enable(struct radeon_device *rdev)
 {
 	u32 link_width_cntl, lanes, speed_cntl, tmp;
 	u16 link_cntl2;
+	u32 mask;
+	int ret;
 
 	if (radeon_pcie_gen2 == 0)
 		return;
@@ -1135,6 +1128,15 @@ static void rv770_pcie_gen2_enable(struct radeon_device *rdev)
 	if (ASIC_IS_X2(rdev))
 		return;
 
+	ret = drm_pcie_get_speed_cap_mask(rdev->ddev, &mask);
+	if (ret != 0)
+		return;
+
+	if (!(mask & DRM_PCIE_SPEED_50))
+		return;
+
+	DRM_INFO("enabling PCIE gen 2 link speeds, disable with radeon.pcie_gen2=0\n");
+
 	/* advertise upconfig capability */
 	link_width_cntl = RREG32_PCIE_P(PCIE_LC_LINK_WIDTH_CNTL);
 	link_width_cntl &= ~LC_UPCONFIGURE_DIS;
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 0b0279291a7..c053f819377 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -1762,13 +1762,34 @@ void si_fence_ring_emit(struct radeon_device *rdev,
  */
 void si_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+	struct radeon_ring *ring = &rdev->ring[ib->ring];
 	u32 header;
 
-	if (ib->is_const_ib)
+	if (ib->is_const_ib) {
+		/* set switch buffer packet before const IB */
+		radeon_ring_write(ring, PACKET3(PACKET3_SWITCH_BUFFER, 0));
+		radeon_ring_write(ring, 0);
+
 		header = PACKET3(PACKET3_INDIRECT_BUFFER_CONST, 2);
-	else
+	} else {
+		u32 next_rptr;
+		if (ring->rptr_save_reg) {
+			next_rptr = ring->wptr + 3 + 4 + 8;
+			radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+			radeon_ring_write(ring, ((ring->rptr_save_reg -
+						  PACKET3_SET_CONFIG_REG_START) >> 2));
+			radeon_ring_write(ring, next_rptr);
+		} else if (rdev->wb.enabled) {
+			next_rptr = ring->wptr + 5 + 4 + 8;
+			radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
+			radeon_ring_write(ring, (1 << 8));
+			radeon_ring_write(ring, ring->next_rptr_gpu_addr & 0xfffffffc);
+			radeon_ring_write(ring, upper_32_bits(ring->next_rptr_gpu_addr) & 0xffffffff);
+			radeon_ring_write(ring, next_rptr);
+		}
+
 		header = PACKET3(PACKET3_INDIRECT_BUFFER, 2);
+	}
 
 	radeon_ring_write(ring, header);
 	radeon_ring_write(ring,
@@ -1779,18 +1800,20 @@ void si_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 	radeon_ring_write(ring, upper_32_bits(ib->gpu_addr) & 0xFFFF);
 	radeon_ring_write(ring, ib->length_dw | (ib->vm_id << 24));
 
-	/* flush read cache over gart for this vmid */
-	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-	radeon_ring_write(ring, (CP_COHER_CNTL2 - PACKET3_SET_CONFIG_REG_START) >> 2);
-	radeon_ring_write(ring, ib->vm_id);
-	radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
-	radeon_ring_write(ring, PACKET3_TCL1_ACTION_ENA |
-			  PACKET3_TC_ACTION_ENA |
-			  PACKET3_SH_KCACHE_ACTION_ENA |
-			  PACKET3_SH_ICACHE_ACTION_ENA);
-	radeon_ring_write(ring, 0xFFFFFFFF);
-	radeon_ring_write(ring, 0);
-	radeon_ring_write(ring, 10); /* poll interval */
+	if (!ib->is_const_ib) {
+		/* flush read cache over gart for this vmid */
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, (CP_COHER_CNTL2 - PACKET3_SET_CONFIG_REG_START) >> 2);
+		radeon_ring_write(ring, ib->vm_id);
+		radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+		radeon_ring_write(ring, PACKET3_TCL1_ACTION_ENA |
+				  PACKET3_TC_ACTION_ENA |
+				  PACKET3_SH_KCACHE_ACTION_ENA |
+				  PACKET3_SH_ICACHE_ACTION_ENA);
+		radeon_ring_write(ring, 0xFFFFFFFF);
+		radeon_ring_write(ring, 0);
+		radeon_ring_write(ring, 10); /* poll interval */
+	}
 }
 
 /*
@@ -1917,10 +1940,20 @@ static int si_cp_start(struct radeon_device *rdev)
 
 static void si_cp_fini(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring;
 	si_cp_enable(rdev, false);
-	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	radeon_ring_fini(rdev, &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX]);
-	radeon_ring_fini(rdev, &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX]);
+
+	ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	radeon_ring_fini(rdev, ring);
+	radeon_scratch_free(rdev, ring->rptr_save_reg);
+
+	ring = &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX];
+	radeon_ring_fini(rdev, ring);
+	radeon_scratch_free(rdev, ring->rptr_save_reg);
+
+	ring = &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX];
+	radeon_ring_fini(rdev, ring);
+	radeon_scratch_free(rdev, ring->rptr_save_reg);
 }
 
 static int si_cp_resume(struct radeon_device *rdev)
@@ -2702,7 +2735,7 @@ int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
 			if (ib->is_const_ib)
 				ret = si_vm_packet3_ce_check(rdev, ib->ptr, &pkt);
 			else {
-				switch (ib->fence->ring) {
+				switch (ib->ring) {
 				case RADEON_RING_TYPE_GFX_INDEX:
 					ret = si_vm_packet3_gfx_check(rdev, ib->ptr, &pkt);
 					break;
@@ -2711,7 +2744,7 @@ int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
 					ret = si_vm_packet3_compute_check(rdev, ib->ptr, &pkt);
 					break;
 				default:
-					dev_err(rdev->dev, "Non-PM4 ring %d !\n", ib->fence->ring);
+					dev_err(rdev->dev, "Non-PM4 ring %d !\n", ib->ring);
 					ret = -EINVAL;
 					break;
 				}
@@ -2942,7 +2975,6 @@ static void si_disable_interrupts(struct radeon_device *rdev)
 	WREG32(IH_RB_RPTR, 0);
 	WREG32(IH_RB_WPTR, 0);
 	rdev->ih.enabled = false;
-	rdev->ih.wptr = 0;
 	rdev->ih.rptr = 0;
 }
 
@@ -3093,45 +3125,45 @@ int si_irq_set(struct radeon_device *rdev)
 	hpd6 = RREG32(DC_HPD6_INT_CONTROL) & ~DC_HPDx_INT_EN;
 
 	/* enable CP interrupts on all rings */
-	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+	if (atomic_read(&rdev->irq.ring_int[RADEON_RING_TYPE_GFX_INDEX])) {
 		DRM_DEBUG("si_irq_set: sw int gfx\n");
 		cp_int_cntl |= TIME_STAMP_INT_ENABLE;
 	}
-	if (rdev->irq.sw_int[CAYMAN_RING_TYPE_CP1_INDEX]) {
+	if (atomic_read(&rdev->irq.ring_int[CAYMAN_RING_TYPE_CP1_INDEX])) {
 		DRM_DEBUG("si_irq_set: sw int cp1\n");
 		cp_int_cntl1 |= TIME_STAMP_INT_ENABLE;
 	}
-	if (rdev->irq.sw_int[CAYMAN_RING_TYPE_CP2_INDEX]) {
+	if (atomic_read(&rdev->irq.ring_int[CAYMAN_RING_TYPE_CP2_INDEX])) {
 		DRM_DEBUG("si_irq_set: sw int cp2\n");
 		cp_int_cntl2 |= TIME_STAMP_INT_ENABLE;
 	}
 	if (rdev->irq.crtc_vblank_int[0] ||
-	    rdev->irq.pflip[0]) {
+	    atomic_read(&rdev->irq.pflip[0])) {
 		DRM_DEBUG("si_irq_set: vblank 0\n");
 		crtc1 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[1] ||
-	    rdev->irq.pflip[1]) {
+	    atomic_read(&rdev->irq.pflip[1])) {
 		DRM_DEBUG("si_irq_set: vblank 1\n");
 		crtc2 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[2] ||
-	    rdev->irq.pflip[2]) {
+	    atomic_read(&rdev->irq.pflip[2])) {
 		DRM_DEBUG("si_irq_set: vblank 2\n");
 		crtc3 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[3] ||
-	    rdev->irq.pflip[3]) {
+	    atomic_read(&rdev->irq.pflip[3])) {
 		DRM_DEBUG("si_irq_set: vblank 3\n");
 		crtc4 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[4] ||
-	    rdev->irq.pflip[4]) {
+	    atomic_read(&rdev->irq.pflip[4])) {
 		DRM_DEBUG("si_irq_set: vblank 4\n");
 		crtc5 |= VBLANK_INT_MASK;
 	}
 	if (rdev->irq.crtc_vblank_int[5] ||
-	    rdev->irq.pflip[5]) {
+	    atomic_read(&rdev->irq.pflip[5])) {
 		DRM_DEBUG("si_irq_set: vblank 5\n");
 		crtc6 |= VBLANK_INT_MASK;
 	}
@@ -3359,29 +3391,27 @@ int si_irq_process(struct radeon_device *rdev)
 	u32 rptr;
 	u32 src_id, src_data, ring_id;
 	u32 ring_index;
-	unsigned long flags;
 	bool queue_hotplug = false;
 
 	if (!rdev->ih.enabled || rdev->shutdown)
 		return IRQ_NONE;
 
 	wptr = si_get_ih_wptr(rdev);
+
+restart_ih:
+	/* is somebody else already processing irqs? */
+	if (atomic_xchg(&rdev->ih.lock, 1))
+		return IRQ_NONE;
+
 	rptr = rdev->ih.rptr;
 	DRM_DEBUG("si_irq_process start: rptr %d, wptr %d\n", rptr, wptr);
 
-	spin_lock_irqsave(&rdev->ih.lock, flags);
-	if (rptr == wptr) {
-		spin_unlock_irqrestore(&rdev->ih.lock, flags);
-		return IRQ_NONE;
-	}
-restart_ih:
 	/* Order reading of wptr vs. reading of IH ring data */
 	rmb();
 
 	/* display interrupts */
 	si_irq_ack(rdev);
 
-	rdev->ih.wptr = wptr;
 	while (rptr != wptr) {
 		/* wptr/rptr are in bytes! */
 		ring_index = rptr / 4;
@@ -3399,7 +3429,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[0])
+					if (atomic_read(&rdev->irq.pflip[0]))
 						radeon_crtc_handle_flip(rdev, 0);
 					rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D1 vblank\n");
@@ -3425,7 +3455,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[1])
+					if (atomic_read(&rdev->irq.pflip[1]))
 						radeon_crtc_handle_flip(rdev, 1);
 					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D2 vblank\n");
@@ -3451,7 +3481,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[2])
+					if (atomic_read(&rdev->irq.pflip[2]))
 						radeon_crtc_handle_flip(rdev, 2);
 					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D3 vblank\n");
@@ -3477,7 +3507,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[3])
+					if (atomic_read(&rdev->irq.pflip[3]))
 						radeon_crtc_handle_flip(rdev, 3);
 					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D4 vblank\n");
@@ -3503,7 +3533,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[4])
+					if (atomic_read(&rdev->irq.pflip[4]))
 						radeon_crtc_handle_flip(rdev, 4);
 					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D5 vblank\n");
@@ -3529,7 +3559,7 @@ restart_ih:
 						rdev->pm.vblank_sync = true;
 						wake_up(&rdev->irq.vblank_queue);
 					}
-					if (rdev->irq.pflip[5])
+					if (atomic_read(&rdev->irq.pflip[5]))
 						radeon_crtc_handle_flip(rdev, 5);
 					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
 					DRM_DEBUG("IH: D6 vblank\n");
@@ -3620,7 +3650,6 @@ restart_ih:
 			break;
 		case 233: /* GUI IDLE */
 			DRM_DEBUG("IH: GUI idle\n");
-			rdev->pm.gui_idle = true;
 			wake_up(&rdev->irq.idle_queue);
 			break;
 		default:
@@ -3632,15 +3661,17 @@ restart_ih:
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
 	}
-	/* make sure wptr hasn't changed while processing */
-	wptr = si_get_ih_wptr(rdev);
-	if (wptr != rdev->ih.wptr)
-		goto restart_ih;
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
 	rdev->ih.rptr = rptr;
 	WREG32(IH_RB_RPTR, rdev->ih.rptr);
-	spin_unlock_irqrestore(&rdev->ih.lock, flags);
+	atomic_set(&rdev->ih.lock, 0);
+
+	/* make sure wptr hasn't changed while processing */
+	wptr = si_get_ih_wptr(rdev);
+	if (wptr != rptr)
+		goto restart_ih;
+
 	return IRQ_HANDLED;
 }
 
@@ -3752,35 +3783,18 @@ static int si_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_start(rdev);
-	if (r)
-		return r;
-
-	r = radeon_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d) on CP ring 0\n", r);
-		rdev->accel_working = false;
-		return r;
-	}
-
-	r = radeon_ib_test(rdev, CAYMAN_RING_TYPE_CP1_INDEX, &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX]);
+	r = radeon_ib_pool_init(rdev);
 	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d) on CP ring 1\n", r);
-		rdev->accel_working = false;
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
 		return r;
 	}
 
-	r = radeon_ib_test(rdev, CAYMAN_RING_TYPE_CP2_INDEX, &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX]);
+	r = radeon_vm_manager_init(rdev);
 	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d) on CP ring 2\n", r);
-		rdev->accel_working = false;
+		dev_err(rdev->dev, "vm manager initialization failed (%d).\n", r);
 		return r;
 	}
 
-	r = radeon_vm_manager_start(rdev);
-	if (r)
-		return r;
-
 	return 0;
 }
 
@@ -3809,12 +3823,6 @@ int si_resume(struct radeon_device *rdev)
 
 int si_suspend(struct radeon_device *rdev)
 {
-	/* FIXME: we should wait for ring to be empty */
-	radeon_ib_pool_suspend(rdev);
-	radeon_vm_manager_suspend(rdev);
-#if 0
-	r600_blit_suspend(rdev);
-#endif
 	si_cp_enable(rdev, false);
 	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX].ready = false;
@@ -3903,17 +3911,7 @@ int si_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
-	if (r) {
-		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-		rdev->accel_working = false;
-	}
-	r = radeon_vm_manager_init(rdev);
-	if (r) {
-		dev_err(rdev->dev, "vm manager initialization failed (%d).\n", r);
-	}
-
 	r = si_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
@@ -3921,7 +3919,7 @@ int si_init(struct radeon_device *rdev)
 		si_irq_fini(rdev);
 		si_rlc_fini(rdev);
 		radeon_wb_fini(rdev);
-		r100_ib_fini(rdev);
+		radeon_ib_pool_fini(rdev);
 		radeon_vm_manager_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		si_pcie_gart_fini(rdev);
@@ -3950,7 +3948,7 @@ void si_fini(struct radeon_device *rdev)
 	si_rlc_fini(rdev);
 	radeon_wb_fini(rdev);
 	radeon_vm_manager_fini(rdev);
-	r100_ib_fini(rdev);
+	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	si_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index db406796286..7869089e876 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -901,5 +901,6 @@
 #define	PACKET3_WAIT_ON_DE_COUNTER_DIFF			0x88
 #define	PACKET3_SET_CE_DE_COUNTERS			0x89
 #define	PACKET3_WAIT_ON_AVAIL_BUFFER			0x8A
+#define	PACKET3_SWITCH_BUFFER				0x8B
 
 #endif
diff --git a/drivers/gpu/drm/savage/savage_bci.c b/drivers/gpu/drm/savage/savage_bci.c
index 6eb507a5d13..1efbb907583 100644
--- a/drivers/gpu/drm/savage/savage_bci.c
+++ b/drivers/gpu/drm/savage/savage_bci.c
@@ -1050,6 +1050,7 @@ void savage_reclaim_buffers(struct drm_device *dev, struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
 	drm_savage_private_t *dev_priv = dev->dev_private;
+	int release_idlelock = 0;
 	int i;
 
 	if (!dma)
@@ -1059,7 +1060,10 @@ void savage_reclaim_buffers(struct drm_device *dev, struct drm_file *file_priv)
 	if (!dma->buflist)
 		return;
 
-	/*i830_flush_queue(dev); */
+	if (file_priv->master && file_priv->master->lock.hw_lock) {
+		drm_idlelock_take(&file_priv->master->lock);
+		release_idlelock = 1;
+	}
 
 	for (i = 0; i < dma->buf_count; i++) {
 		struct drm_buf *buf = dma->buflist[i];
@@ -1075,7 +1079,8 @@ void savage_reclaim_buffers(struct drm_device *dev, struct drm_file *file_priv)
 		}
 	}
 
-	drm_core_reclaim_buffers(dev, file_priv);
+	if (release_idlelock)
+		drm_idlelock_release(&file_priv->master->lock);
 }
 
 struct drm_ioctl_desc savage_ioctls[] = {
diff --git a/drivers/gpu/drm/savage/savage_drv.c b/drivers/gpu/drm/savage/savage_drv.c
index 89afe0b8364..d31d4cca9a4 100644
--- a/drivers/gpu/drm/savage/savage_drv.c
+++ b/drivers/gpu/drm/savage/savage_drv.c
@@ -52,9 +52,9 @@ static struct drm_driver driver = {
 	.dev_priv_size = sizeof(drm_savage_buf_priv_t),
 	.load = savage_driver_load,
 	.firstopen = savage_driver_firstopen,
+	.preclose = savage_reclaim_buffers,
 	.lastclose = savage_driver_lastclose,
 	.unload = savage_driver_unload,
-	.reclaim_buffers = savage_reclaim_buffers,
 	.ioctls = savage_ioctls,
 	.dma_ioctl = savage_bci_buffers,
 	.fops = &savage_driver_fops,
diff --git a/drivers/gpu/drm/sis/sis_drv.c b/drivers/gpu/drm/sis/sis_drv.c
index dd14cd1a003..7f119870147 100644
--- a/drivers/gpu/drm/sis/sis_drv.c
+++ b/drivers/gpu/drm/sis/sis_drv.c
@@ -105,10 +105,9 @@ static struct drm_driver driver = {
 	.load = sis_driver_load,
 	.unload = sis_driver_unload,
 	.open = sis_driver_open,
+	.preclose = sis_reclaim_buffers_locked,
 	.postclose = sis_driver_postclose,
 	.dma_quiescent = sis_idle,
-	.reclaim_buffers = NULL,
-	.reclaim_buffers_idlelocked = sis_reclaim_buffers_locked,
 	.lastclose = sis_lastclose,
 	.ioctls = sis_ioctls,
 	.fops = &sis_driver_fops,
diff --git a/drivers/gpu/drm/sis/sis_mm.c b/drivers/gpu/drm/sis/sis_mm.c
index dd4a316c3d7..2c231070d25 100644
--- a/drivers/gpu/drm/sis/sis_mm.c
+++ b/drivers/gpu/drm/sis/sis_mm.c
@@ -74,7 +74,7 @@ static int sis_fb_init(struct drm_device *dev, void *data, struct drm_file *file
 	dev_priv->vram_offset = fb->offset;
 
 	mutex_unlock(&dev->struct_mutex);
-	DRM_DEBUG("offset = %u, size = %u\n", fb->offset, fb->size);
+	DRM_DEBUG("offset = %lu, size = %lu\n", fb->offset, fb->size);
 
 	return 0;
 }
@@ -161,7 +161,7 @@ fail_alloc:
 	mem->size = 0;
 	mem->free = 0;
 
-	DRM_DEBUG("alloc %d, size = %d, offset = %d\n", pool, mem->size,
+	DRM_DEBUG("alloc %d, size = %ld, offset = %ld\n", pool, mem->size,
 		  mem->offset);
 
 	return retval;
@@ -215,7 +215,7 @@ static int sis_ioctl_agp_init(struct drm_device *dev, void *data,
 	dev_priv->agp_offset = agp->offset;
 	mutex_unlock(&dev->struct_mutex);
 
-	DRM_DEBUG("offset = %u, size = %u\n", agp->offset, agp->size);
+	DRM_DEBUG("offset = %lu, size = %lu\n", agp->offset, agp->size);
 	return 0;
 }
 
@@ -321,14 +321,20 @@ void sis_reclaim_buffers_locked(struct drm_device *dev,
 	struct sis_file_private *file_priv = file->driver_priv;
 	struct sis_memblock *entry, *next;
 
+	if (!(file->minor->master && file->master->lock.hw_lock))
+		return;
+
+	drm_idlelock_take(&file->master->lock);
+
 	mutex_lock(&dev->struct_mutex);
 	if (list_empty(&file_priv->obj_list)) {
 		mutex_unlock(&dev->struct_mutex);
+		drm_idlelock_release(&file->master->lock);
+
 		return;
 	}
 
-	if (dev->driver->dma_quiescent)
-		dev->driver->dma_quiescent(dev);
+	sis_idle(dev);
 
 
 	list_for_each_entry_safe(entry, next, &file_priv->obj_list,
@@ -343,6 +349,9 @@ void sis_reclaim_buffers_locked(struct drm_device *dev,
 		kfree(entry);
 	}
 	mutex_unlock(&dev->struct_mutex);
+
+	drm_idlelock_release(&file->master->lock);
+
 	return;
 }
 
diff --git a/drivers/gpu/drm/tdfx/tdfx_drv.c b/drivers/gpu/drm/tdfx/tdfx_drv.c
index 1613c78544c..90f6b13acfa 100644
--- a/drivers/gpu/drm/tdfx/tdfx_drv.c
+++ b/drivers/gpu/drm/tdfx/tdfx_drv.c
@@ -54,7 +54,6 @@ static const struct file_operations tdfx_driver_fops = {
 
 static struct drm_driver driver = {
 	.driver_features = DRIVER_USE_MTRR,
-	.reclaim_buffers = drm_core_reclaim_buffers,
 	.fops = &tdfx_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
diff --git a/drivers/gpu/drm/udl/udl_encoder.c b/drivers/gpu/drm/udl/udl_encoder.c
index 56e75f0f1df..0731ab2e6c0 100644
--- a/drivers/gpu/drm/udl/udl_encoder.c
+++ b/drivers/gpu/drm/udl/udl_encoder.c
@@ -27,7 +27,7 @@ static void udl_encoder_disable(struct drm_encoder *encoder)
 }
 
 static bool udl_mode_fixup(struct drm_encoder *encoder,
-			   struct drm_display_mode *mode,
+			   const struct drm_display_mode *mode,
 			   struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/gpu/drm/udl/udl_modeset.c b/drivers/gpu/drm/udl/udl_modeset.c
index 0d7816789da..f5dd89e891d 100644
--- a/drivers/gpu/drm/udl/udl_modeset.c
+++ b/drivers/gpu/drm/udl/udl_modeset.c
@@ -45,12 +45,25 @@ static char *udl_vidreg_unlock(char *buf)
  *  0x01 H and V sync off (screen blank but powered)
  *  0x07 DPMS powerdown (requires modeset to come back)
  */
-static char *udl_enable_hvsync(char *buf, bool enable)
+static char *udl_set_blank(char *buf, int dpms_mode)
 {
-	if (enable)
-		return udl_set_register(buf, 0x1F, 0x00);
-	else
-		return udl_set_register(buf, 0x1F, 0x07);
+	u8 reg;
+	switch (dpms_mode) {
+	case DRM_MODE_DPMS_OFF:
+		reg = 0x07;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		reg = 0x05;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		reg = 0x01;
+		break;
+	case DRM_MODE_DPMS_ON:
+		reg = 0x00;
+		break;
+	}
+
+	return udl_set_register(buf, 0x1f, reg);
 }
 
 static char *udl_set_color_depth(char *buf, u8 selection)
@@ -199,6 +212,20 @@ static char *udl_set_vid_cmds(char *wrptr, struct drm_display_mode *mode)
 	return wrptr;
 }
 
+static char *udl_dummy_render(char *wrptr)
+{
+	*wrptr++ = 0xAF;
+	*wrptr++ = 0x6A; /* copy */
+	*wrptr++ = 0x00; /* from addr */
+	*wrptr++ = 0x00;
+	*wrptr++ = 0x00;
+	*wrptr++ = 0x01; /* one pixel */
+	*wrptr++ = 0x00; /* to address */
+	*wrptr++ = 0x00;
+	*wrptr++ = 0x00;
+	return wrptr;
+}
+
 static int udl_crtc_write_mode_to_hw(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -235,9 +262,10 @@ static void udl_crtc_dpms(struct drm_crtc *crtc, int mode)
 
 		buf = (char *)urb->transfer_buffer;
 		buf = udl_vidreg_lock(buf);
-		buf = udl_enable_hvsync(buf, false);
+		buf = udl_set_blank(buf, mode);
 		buf = udl_vidreg_unlock(buf);
 
+		buf = udl_dummy_render(buf);
 		retval = udl_submit_urb(dev, urb, buf - (char *)
 					urb->transfer_buffer);
 	} else {
@@ -251,7 +279,7 @@ static void udl_crtc_dpms(struct drm_crtc *crtc, int mode)
 }
 
 static bool udl_crtc_mode_fixup(struct drm_crtc *crtc,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 
 {
@@ -306,9 +334,11 @@ static int udl_crtc_mode_set(struct drm_crtc *crtc,
 	wrptr = udl_set_base8bpp(wrptr, 2 * mode->vdisplay * mode->hdisplay);
 
 	wrptr = udl_set_vid_cmds(wrptr, adjusted_mode);
-	wrptr = udl_enable_hvsync(wrptr, true);
+	wrptr = udl_set_blank(wrptr, DRM_MODE_DPMS_ON);
 	wrptr = udl_vidreg_unlock(wrptr);
 
+	wrptr = udl_dummy_render(wrptr);
+
 	ufb->active_16 = true;
 	if (old_fb) {
 		struct udl_framebuffer *uold_fb = to_udl_fb(old_fb);
diff --git a/drivers/gpu/drm/via/via_drv.c b/drivers/gpu/drm/via/via_drv.c
index 02661f35f7a..e927b4c052f 100644
--- a/drivers/gpu/drm/via/via_drv.c
+++ b/drivers/gpu/drm/via/via_drv.c
@@ -75,6 +75,7 @@ static struct drm_driver driver = {
 	.load = via_driver_load,
 	.unload = via_driver_unload,
 	.open = via_driver_open,
+	.preclose = via_reclaim_buffers_locked,
 	.postclose = via_driver_postclose,
 	.context_dtor = via_final_context,
 	.get_vblank_counter = via_get_vblank_counter,
@@ -85,9 +86,6 @@ static struct drm_driver driver = {
 	.irq_uninstall = via_driver_irq_uninstall,
 	.irq_handler = via_driver_irq_handler,
 	.dma_quiescent = via_driver_dma_quiescent,
-	.reclaim_buffers = drm_core_reclaim_buffers,
-	.reclaim_buffers_locked = NULL,
-	.reclaim_buffers_idlelocked = via_reclaim_buffers_locked,
 	.lastclose = via_lastclose,
 	.ioctls = via_ioctls,
 	.fops = &via_driver_fops,
diff --git a/drivers/gpu/drm/via/via_mm.c b/drivers/gpu/drm/via/via_mm.c
index a3574d09a07..acfcb358e7b 100644
--- a/drivers/gpu/drm/via/via_mm.c
+++ b/drivers/gpu/drm/via/via_mm.c
@@ -215,14 +215,20 @@ void via_reclaim_buffers_locked(struct drm_device *dev,
 	struct via_file_private *file_priv = file->driver_priv;
 	struct via_memblock *entry, *next;
 
+	if (!(file->minor->master && file->master->lock.hw_lock))
+		return;
+
+	drm_idlelock_take(&file->master->lock);
+
 	mutex_lock(&dev->struct_mutex);
 	if (list_empty(&file_priv->obj_list)) {
 		mutex_unlock(&dev->struct_mutex);
+		drm_idlelock_release(&file->master->lock);
+
 		return;
 	}
 
-	if (dev->driver->dma_quiescent)
-		dev->driver->dma_quiescent(dev);
+	via_driver_dma_quiescent(dev);
 
 	list_for_each_entry_safe(entry, next, &file_priv->obj_list,
 				 owner_list) {
@@ -231,5 +237,8 @@ void via_reclaim_buffers_locked(struct drm_device *dev,
 		kfree(entry);
 	}
 	mutex_unlock(&dev->struct_mutex);
+
+	drm_idlelock_release(&file->master->lock);
+
 	return;
 }
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index ee24d216aa8..4d9edead01a 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -769,10 +769,7 @@ static int vmw_driver_open(struct drm_device *dev, struct drm_file *file_priv)
 		goto out_no_tfile;
 
 	file_priv->driver_priv = vmw_fp;
-
-	if (unlikely(dev_priv->bdev.dev_mapping == NULL))
-		dev_priv->bdev.dev_mapping =
-			file_priv->filp->f_path.dentry->d_inode->i_mapping;
+	dev_priv->bdev.dev_mapping = dev->dev_mapping;
 
 	return 0;
 
@@ -1147,7 +1144,6 @@ static struct drm_driver driver = {
 	.get_vblank_counter = vmw_get_vblank_counter,
 	.enable_vblank = vmw_enable_vblank,
 	.disable_vblank = vmw_disable_vblank,
-	.reclaim_buffers_locked = NULL,
 	.ioctls = vmw_ioctls,
 	.num_ioctls = DRM_ARRAY_SIZE(vmw_ioctls),
 	.dma_quiescent = NULL,	/*vmw_dma_quiescent, */
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index bef04c19276..fbf49503508 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -53,6 +53,27 @@ config HIDRAW
 
 	If unsure, say Y.
 
+config UHID
+	tristate "User-space I/O driver support for HID subsystem"
+	depends on HID
+	default n
+	---help---
+	Say Y here if you want to provide HID I/O Drivers from user-space.
+	This allows to write I/O drivers in user-space and feed the data from
+	the device into the kernel. The kernel parses the HID reports, loads the
+	corresponding HID Device Driver or provides input devices on top of your
+	user-space device.
+
+	This driver cannot be used to parse HID-reports in user-space and write
+	special HID-drivers. You should use hidraw for that.
+	Instead, this driver allows to write the transport-layer driver in
+	user-space like USB-HID and Bluetooth-HID do in kernel-space.
+
+	If unsure, say N.
+
+	To compile this driver as a module, choose M here: the
+	module will be called uhid.
+
 config HID_GENERIC
 	tristate "Generic HID driver"
 	depends on HID
@@ -193,10 +214,12 @@ config HID_EZKEY
 	Support for Ezkey BTC 8193 keyboard.
 
 config HID_HOLTEK
-	tristate "Holtek On Line Grip based game controller support"
+	tristate "Holtek HID devices"
 	depends on USB_HID
 	---help---
-	  Say Y here if you have a Holtek On Line Grip based game controller.
+	Support for Holtek based devices:
+	  - Holtek On Line Grip based game controller
+	  - Trust GXT 18 Gaming Keyboard
 
 config HOLTEK_FF
 	bool "Holtek On Line Grip force feedback support"
@@ -261,6 +284,19 @@ config HID_LCPOWER
 	---help---
 	Support for LC-Power RC1000MCE RF remote control.
 
+config HID_LENOVO_TPKBD
+	tristate "Lenovo ThinkPad USB Keyboard with TrackPoint"
+	depends on USB_HID
+	select NEW_LEDS
+	select LEDS_CLASS
+	---help---
+	Support for the Lenovo ThinkPad USB Keyboard with TrackPoint.
+
+	Say Y here if you have a Lenovo ThinkPad USB Keyboard with TrackPoint
+	and would like to use device-specific features like changing the
+	sensitivity of the trackpoint, using the microphone mute button or
+	controlling the mute and microphone mute LEDs.
+
 config HID_LOGITECH
 	tristate "Logitech devices" if EXPERT
 	depends on USB_HID
@@ -386,6 +422,7 @@ config HID_MULTITOUCH
 	  - Unitec Panels
 	  - XAT optical touch panels
 	  - Xiroku optical touch panels
+	  - Zytronic touch panels
 
 	  If unsure, say N.
 
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index ca6cc9f0485..f975485f88b 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -8,6 +8,7 @@ ifdef CONFIG_DEBUG_FS
 endif
 
 obj-$(CONFIG_HID)		+= hid.o
+obj-$(CONFIG_UHID)		+= uhid.o
 
 obj-$(CONFIG_HID_GENERIC)	+= hid-generic.o
 
@@ -48,12 +49,14 @@ obj-$(CONFIG_HID_EMS_FF)	+= hid-emsff.o
 obj-$(CONFIG_HID_ELECOM)	+= hid-elecom.o
 obj-$(CONFIG_HID_EZKEY)		+= hid-ezkey.o
 obj-$(CONFIG_HID_GYRATION)	+= hid-gyration.o
+obj-$(CONFIG_HID_HOLTEK)	+= hid-holtek-kbd.o
 obj-$(CONFIG_HID_HOLTEK)	+= hid-holtekff.o
 obj-$(CONFIG_HID_HYPERV_MOUSE)	+= hid-hyperv.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_LENOVO_TPKBD)	+= hid-lenovo-tpkbd.o
 obj-$(CONFIG_HID_LOGITECH)	+= hid-logitech.o
 obj-$(CONFIG_HID_LOGITECH_DJ)	+= hid-logitech-dj.o
 obj-$(CONFIG_HID_MAGICMOUSE)    += hid-magicmouse.o
@@ -69,7 +72,8 @@ obj-$(CONFIG_HID_PICOLCD)	+= hid-picolcd.o
 obj-$(CONFIG_HID_PRIMAX)	+= hid-primax.o
 obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o hid-roccat-common.o \
 	hid-roccat-arvo.o hid-roccat-isku.o hid-roccat-kone.o \
-	hid-roccat-koneplus.o hid-roccat-kovaplus.o hid-roccat-pyra.o
+	hid-roccat-koneplus.o hid-roccat-kovaplus.o hid-roccat-pyra.o \
+	hid-roccat-savu.o
 obj-$(CONFIG_HID_SAITEK)	+= hid-saitek.o
 obj-$(CONFIG_HID_SAMSUNG)	+= hid-samsung.o
 obj-$(CONFIG_HID_SMARTJOYPLUS)	+= hid-sjoy.o
diff --git a/drivers/hid/hid-chicony.c b/drivers/hid/hid-chicony.c
index b99af346fdf..a2abb8e1572 100644
--- a/drivers/hid/hid-chicony.c
+++ b/drivers/hid/hid-chicony.c
@@ -60,6 +60,7 @@ static int ch_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 static const struct hid_device_id ch_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_TACTICAL_PAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_AK1D) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, ch_devices);
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 4c87276c8dd..60ea284407c 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -1194,8 +1194,10 @@ int hid_report_raw_event(struct hid_device *hid, int type, u8 *data, int size,
 			goto out;
 	}
 
-	for (a = 0; a < report->maxfield; a++)
-		hid_input_field(hid, report->field[a], cdata, interrupt);
+	if (hid->claimed != HID_CLAIMED_HIDRAW) {
+		for (a = 0; a < report->maxfield; a++)
+			hid_input_field(hid, report->field[a], cdata, interrupt);
+	}
 
 	if (hid->claimed & HID_CLAIMED_INPUT)
 		hidinput_report_event(hid, report);
@@ -1243,6 +1245,10 @@ int hid_input_report(struct hid_device *hid, int type, u8 *data, int size, int i
 		goto unlock;
 	}
 
+	/* Avoid unnecessary overhead if debugfs is disabled */
+	if (list_empty(&hid->debug_list))
+		goto nomem;
+
 	buf = kmalloc(sizeof(char) * HID_DEBUG_BUFSIZE, GFP_ATOMIC);
 
 	if (!buf)
@@ -1373,8 +1379,10 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 	if ((connect_mask & HID_CONNECT_HIDRAW) && !hidraw_connect(hdev))
 		hdev->claimed |= HID_CLAIMED_HIDRAW;
 
-	if (!hdev->claimed) {
-		hid_err(hdev, "claimed by neither input, hiddev nor hidraw\n");
+	/* Drivers with the ->raw_event callback set are not required to connect
+	 * to any other listener. */
+	if (!hdev->claimed && !hdev->driver->raw_event) {
+		hid_err(hdev, "device has no listeners, quitting\n");
 		return -ENODEV;
 	}
 
@@ -1521,10 +1529,12 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_TACTICAL_PAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_WIRELESS2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_AK1D) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CREATIVELABS, USB_DEVICE_ID_PRODIKEYS_PCMIDI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_3) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_4) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_MOUSE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0006) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0011) },
@@ -1539,6 +1549,7 @@ 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_HOLTEK, USB_DEVICE_ID_HOLTEK_ON_LINE_GRIP) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_KEYBOARD) },
 	{ 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) },
@@ -1547,6 +1558,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_EASYPEN_M610X) },
 	{ 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_LENOVO, USB_DEVICE_ID_LENOVO_TPKBD) },
 	{ 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) },
@@ -1620,6 +1632,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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_ROCCAT, USB_DEVICE_ID_ROCCAT_PYRA_WIRELESS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_SAVU) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SAITEK, USB_DEVICE_ID_SAITEK_PS1000) },
 	{ 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) },
@@ -1915,6 +1928,7 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GRETAGMACBETH, USB_DEVICE_ID_GRETAGMACBETH_HUEY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GRIFFIN, USB_DEVICE_ID_POWERMATE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GRIFFIN, USB_DEVICE_ID_SOUNDKNOB) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_GRIFFIN, USB_DEVICE_ID_RADIOSHARK) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GTCO, USB_DEVICE_ID_GTCO_90) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GTCO, USB_DEVICE_ID_GTCO_100) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GTCO, USB_DEVICE_ID_GTCO_101) },
diff --git a/drivers/hid/hid-cypress.c b/drivers/hid/hid-cypress.c
index 2f0be4c66af..9e43aaca977 100644
--- a/drivers/hid/hid-cypress.c
+++ b/drivers/hid/hid-cypress.c
@@ -129,6 +129,8 @@ static const struct hid_device_id cp_devices[] = {
 		.driver_data = CP_RDESC_SWAPPED_MIN_MAX },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_3),
 		.driver_data = CP_RDESC_SWAPPED_MIN_MAX },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_BARCODE_4),
+		.driver_data = CP_RDESC_SWAPPED_MIN_MAX },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_MOUSE),
 		.driver_data = CP_2WHEEL_MOUSE_HACK },
 	{ }
diff --git a/drivers/hid/hid-holtek-kbd.c b/drivers/hid/hid-holtek-kbd.c
new file mode 100644
index 00000000000..e0a5d1739fc
--- /dev/null
+++ b/drivers/hid/hid-holtek-kbd.c
@@ -0,0 +1,183 @@
+/*
+ * HID driver for Holtek keyboard
+ * Copyright (c) 2012 Tom Harwood
+*/
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the 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 <linux/usb.h>
+
+#include "hid-ids.h"
+#include "usbhid/usbhid.h"
+
+/* Holtek based keyboards (USB ID 04d9:a055) have the following issues:
+ * - The report descriptor specifies an excessively large number of consumer
+ *   usages (2^15), which is more than HID_MAX_USAGES. This prevents proper
+ *   parsing of the report descriptor.
+ * - The report descriptor reports on caps/scroll/num lock key presses, but
+ *   doesn't have an LED output usage block.
+ *
+ * The replacement descriptor below fixes the number of consumer usages,
+ * and provides an LED output usage block. LED output events are redirected
+ * to the boot interface.
+ */
+
+static __u8 holtek_kbd_rdesc_fixed[] = {
+	/* Original report descriptor, with reduced number of consumer usages */
+	0x05, 0x01,         /*  Usage Page (Desktop),                         */
+	0x09, 0x80,         /*  Usage (Sys Control),                          */
+	0xA1, 0x01,         /*  Collection (Application),                     */
+	0x85, 0x01,         /*      Report ID (1),                            */
+	0x19, 0x81,         /*      Usage Minimum (Sys Power Down),           */
+	0x29, 0x83,         /*      Usage Maximum (Sys Wake Up),              */
+	0x15, 0x00,         /*      Logical Minimum (0),                      */
+	0x25, 0x01,         /*      Logical Maximum (1),                      */
+	0x95, 0x03,         /*      Report Count (3),                         */
+	0x75, 0x01,         /*      Report Size (1),                          */
+	0x81, 0x02,         /*      Input (Variable),                         */
+	0x95, 0x01,         /*      Report Count (1),                         */
+	0x75, 0x05,         /*      Report Size (5),                          */
+	0x81, 0x01,         /*      Input (Constant),                         */
+	0xC0,               /*  End Collection,                               */
+	0x05, 0x0C,         /*  Usage Page (Consumer),                        */
+	0x09, 0x01,         /*  Usage (Consumer Control),                     */
+	0xA1, 0x01,         /*  Collection (Application),                     */
+	0x85, 0x02,         /*      Report ID (2),                            */
+	0x19, 0x00,         /*      Usage Minimum (00h),                      */
+	0x2A, 0xFF, 0x2F,   /*      Usage Maximum (0x2FFF), previously 0x7FFF */
+	0x15, 0x00,         /*      Logical Minimum (0),                      */
+	0x26, 0xFF, 0x2F,   /*      Logical Maximum (0x2FFF),previously 0x7FFF*/
+	0x95, 0x01,         /*      Report Count (1),                         */
+	0x75, 0x10,         /*      Report Size (16),                         */
+	0x81, 0x00,         /*      Input,                                    */
+	0xC0,               /*  End Collection,                               */
+	0x05, 0x01,         /*  Usage Page (Desktop),                         */
+	0x09, 0x06,         /*  Usage (Keyboard),                             */
+	0xA1, 0x01,         /*  Collection (Application),                     */
+	0x85, 0x03,         /*      Report ID (3),                            */
+	0x95, 0x38,         /*      Report Count (56),                        */
+	0x75, 0x01,         /*      Report Size (1),                          */
+	0x15, 0x00,         /*      Logical Minimum (0),                      */
+	0x25, 0x01,         /*      Logical Maximum (1),                      */
+	0x05, 0x07,         /*      Usage Page (Keyboard),                    */
+	0x19, 0xE0,         /*      Usage Minimum (KB Leftcontrol),           */
+	0x29, 0xE7,         /*      Usage Maximum (KB Right GUI),             */
+	0x19, 0x00,         /*      Usage Minimum (None),                     */
+	0x29, 0x2F,         /*      Usage Maximum (KB Lboxbracket And Lbrace),*/
+	0x81, 0x02,         /*      Input (Variable),                         */
+	0xC0,               /*  End Collection,                               */
+	0x05, 0x01,         /*  Usage Page (Desktop),                         */
+	0x09, 0x06,         /*  Usage (Keyboard),                             */
+	0xA1, 0x01,         /*  Collection (Application),                     */
+	0x85, 0x04,         /*      Report ID (4),                            */
+	0x95, 0x38,         /*      Report Count (56),                        */
+	0x75, 0x01,         /*      Report Size (1),                          */
+	0x15, 0x00,         /*      Logical Minimum (0),                      */
+	0x25, 0x01,         /*      Logical Maximum (1),                      */
+	0x05, 0x07,         /*      Usage Page (Keyboard),                    */
+	0x19, 0x30,         /*      Usage Minimum (KB Rboxbracket And Rbrace),*/
+	0x29, 0x67,         /*      Usage Maximum (KP Equals),                */
+	0x81, 0x02,         /*      Input (Variable),                         */
+	0xC0,               /*  End Collection                                */
+
+	/* LED usage for the boot protocol interface */
+	0x05, 0x01,         /*  Usage Page (Desktop),                         */
+	0x09, 0x06,         /*  Usage (Keyboard),                             */
+	0xA1, 0x01,         /*  Collection (Application),                     */
+	0x05, 0x08,         /*      Usage Page (LED),                         */
+	0x19, 0x01,         /*      Usage Minimum (01h),                      */
+	0x29, 0x03,         /*      Usage Maximum (03h),                      */
+	0x15, 0x00,         /*      Logical Minimum (0),                      */
+	0x25, 0x01,         /*      Logical Maximum (1),                      */
+	0x75, 0x01,         /*      Report Size (1),                          */
+	0x95, 0x03,         /*      Report Count (3),                         */
+	0x91, 0x02,         /*      Output (Variable),                        */
+	0x95, 0x05,         /*      Report Count (5),                         */
+	0x91, 0x01,         /*      Output (Constant),                        */
+	0xC0,               /*  End Collection                                */
+};
+
+static __u8 *holtek_kbd_report_fixup(struct hid_device *hdev, __u8 *rdesc,
+		unsigned int *rsize)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+
+	if (intf->cur_altsetting->desc.bInterfaceNumber == 1) {
+		rdesc = holtek_kbd_rdesc_fixed;
+		*rsize = sizeof(holtek_kbd_rdesc_fixed);
+	}
+	return rdesc;
+}
+
+static int holtek_kbd_input_event(struct input_dev *dev, unsigned int type,
+		unsigned int code,
+		int value)
+{
+	struct hid_device *hid = input_get_drvdata(dev);
+	struct usb_device *usb_dev = hid_to_usb_dev(hid);
+
+	/* Locate the boot interface, to receive the LED change events */
+	struct usb_interface *boot_interface = usb_ifnum_to_if(usb_dev, 0);
+
+	struct hid_device *boot_hid = usb_get_intfdata(boot_interface);
+	struct hid_input *boot_hid_input = list_first_entry(&boot_hid->inputs,
+		struct hid_input, list);
+
+	return boot_hid_input->input->event(boot_hid_input->input, type, code,
+			value);
+}
+
+static int holtek_kbd_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	int ret = hid_parse(hdev);
+
+	if (!ret)
+		ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+
+	if (!ret && intf->cur_altsetting->desc.bInterfaceNumber == 1) {
+		struct hid_input *hidinput;
+		list_for_each_entry(hidinput, &hdev->inputs, list) {
+			hidinput->input->event = holtek_kbd_input_event;
+		}
+	}
+
+	return ret;
+}
+
+static const struct hid_device_id holtek_kbd_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT,
+			USB_DEVICE_ID_HOLTEK_ALT_KEYBOARD) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, holtek_kbd_devices);
+
+static struct hid_driver holtek_kbd_driver = {
+	.name = "holtek_kbd",
+	.id_table = holtek_kbd_devices,
+	.report_fixup = holtek_kbd_report_fixup,
+	.probe = holtek_kbd_probe
+};
+
+static int __init holtek_kbd_init(void)
+{
+	return hid_register_driver(&holtek_kbd_driver);
+}
+
+static void __exit holtek_kbd_exit(void)
+{
+	hid_unregister_driver(&holtek_kbd_driver);
+}
+
+module_exit(holtek_kbd_exit);
+module_init(holtek_kbd_init);
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 875ff451842..1dcb76ff51e 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -208,6 +208,7 @@
 #define USB_DEVICE_ID_CHICONY_MULTI_TOUCH	0xb19d
 #define USB_DEVICE_ID_CHICONY_WIRELESS	0x0618
 #define USB_DEVICE_ID_CHICONY_WIRELESS2	0x1123
+#define USB_DEVICE_ID_CHICONY_AK1D	0x1125
 
 #define USB_VENDOR_ID_CHUNGHWAT		0x2247
 #define USB_DEVICE_ID_CHUNGHWAT_MULTITOUCH	0x0001
@@ -237,6 +238,7 @@
 #define USB_DEVICE_ID_CYPRESS_BARCODE_1	0xde61
 #define USB_DEVICE_ID_CYPRESS_BARCODE_2	0xde64
 #define USB_DEVICE_ID_CYPRESS_BARCODE_3	0xbca1
+#define USB_DEVICE_ID_CYPRESS_BARCODE_4	0xed81
 #define USB_DEVICE_ID_CYPRESS_TRUETOUCH	0xc001
 
 #define USB_VENDOR_ID_DEALEXTREAME	0x10c5
@@ -331,6 +333,7 @@
 #define USB_VENDOR_ID_GRIFFIN		0x077d
 #define USB_DEVICE_ID_POWERMATE		0x0410
 #define USB_DEVICE_ID_SOUNDKNOB		0x04AA
+#define USB_DEVICE_ID_RADIOSHARK	0x627a
 
 #define USB_VENDOR_ID_GTCO		0x078c
 #define USB_DEVICE_ID_GTCO_90		0x0090
@@ -410,6 +413,9 @@
 #define USB_VENDOR_ID_HOLTEK		0x1241
 #define USB_DEVICE_ID_HOLTEK_ON_LINE_GRIP	0x5015
 
+#define USB_VENDOR_ID_HOLTEK_ALT		0x04d9
+#define USB_DEVICE_ID_HOLTEK_ALT_KEYBOARD	0xa055
+
 #define USB_VENDOR_ID_IMATION		0x0718
 #define USB_DEVICE_ID_DISC_STAKKA	0xd000
 
@@ -479,6 +485,9 @@
 #define USB_DEVICE_ID_LD_HYBRID		0x2090
 #define USB_DEVICE_ID_LD_HEATCONTROL	0x20A0
 
+#define USB_VENDOR_ID_LENOVO		0x17ef
+#define USB_DEVICE_ID_LENOVO_TPKBD	0x6009
+
 #define USB_VENDOR_ID_LG		0x1fd2
 #define USB_DEVICE_ID_LG_MULTITOUCH	0x0064
 
@@ -573,6 +582,9 @@
 #define USB_VENDOR_ID_NINTENDO		0x057e
 #define USB_DEVICE_ID_NINTENDO_WIIMOTE	0x0306
 
+#define USB_VENDOR_ID_NOVATEK		0x0603
+#define USB_DEVICE_ID_NOVATEK_PCT	0x0600
+
 #define USB_VENDOR_ID_NTRIG		0x1b96
 #define USB_DEVICE_ID_NTRIG_TOUCH_SCREEN   0x0001
 #define USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_1   0x0003
@@ -650,6 +662,7 @@
 #define USB_DEVICE_ID_ROCCAT_KOVAPLUS	0x2d50
 #define USB_DEVICE_ID_ROCCAT_PYRA_WIRED	0x2c24
 #define USB_DEVICE_ID_ROCCAT_PYRA_WIRELESS	0x2cf6
+#define USB_DEVICE_ID_ROCCAT_SAVU	0x2d5a
 
 #define USB_VENDOR_ID_SAITEK		0x06a3
 #define USB_DEVICE_ID_SAITEK_RUMBLEPAD	0xff17
@@ -659,6 +672,9 @@
 #define USB_DEVICE_ID_SAMSUNG_IR_REMOTE	0x0001
 #define USB_DEVICE_ID_SAMSUNG_WIRELESS_KBD_MOUSE	0x0600
 
+#define USB_VENDOR_ID_SENNHEISER	0x1395
+#define USB_DEVICE_ID_SENNHEISER_BTD500USB	0x002c
+
 #define USB_VENDOR_ID_SIGMA_MICRO	0x1c4f
 #define USB_DEVICE_ID_SIGMA_MICRO_KEYBOARD	0x0002
 
@@ -808,6 +824,9 @@
 #define USB_VENDOR_ID_ZYDACRON	0x13EC
 #define USB_DEVICE_ID_ZYDACRON_REMOTE_CONTROL	0x0006
 
+#define USB_VENDOR_ID_ZYTRONIC		0x14c8
+#define USB_DEVICE_ID_ZYTRONIC_ZXY100	0x0005
+
 #define USB_VENDOR_ID_PRIMAX	0x0461
 #define USB_DEVICE_ID_PRIMAX_KEYBOARD	0x4e05
 
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 132b0019365..811bfad6460 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -301,6 +301,9 @@ static const struct hid_device_id hid_battery_quirks[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE,
 			       USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ANSI),
 	  HID_BATTERY_QUIRK_PERCENT | HID_BATTERY_QUIRK_FEATURE },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE,
+		USB_DEVICE_ID_APPLE_ALU_WIRELESS_ANSI),
+	  HID_BATTERY_QUIRK_PERCENT | HID_BATTERY_QUIRK_FEATURE },
 	{}
 };
 
@@ -834,6 +837,15 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		}
 		break;
 
+	case HID_UP_HPVENDOR2:
+		set_bit(EV_REP, input->evbit);
+		switch (usage->hid & HID_USAGE) {
+		case 0x003: map_key_clear(KEY_BRIGHTNESSDOWN);	break;
+		case 0x004: map_key_clear(KEY_BRIGHTNESSUP);	break;
+		default:    goto ignore;
+		}
+		break;
+
 	case HID_UP_MSVENDOR:
 		goto ignore;
 
diff --git a/drivers/hid/hid-lenovo-tpkbd.c b/drivers/hid/hid-lenovo-tpkbd.c
new file mode 100644
index 00000000000..77d2df04c97
--- /dev/null
+++ b/drivers/hid/hid-lenovo-tpkbd.c
@@ -0,0 +1,564 @@
+/*
+ *  HID driver for Lenovo ThinkPad USB Keyboard with TrackPoint
+ *
+ *  Copyright (c) 2012 Bernhard Seibold
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the 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/sysfs.h>
+#include <linux/device.h>
+#include <linux/usb.h>
+#include <linux/hid.h>
+#include <linux/input.h>
+#include <linux/leds.h>
+#include "usbhid/usbhid.h"
+
+#include "hid-ids.h"
+
+/* This is only used for the trackpoint part of the driver, hence _tp */
+struct tpkbd_data_pointer {
+	int led_state;
+	struct led_classdev led_mute;
+	struct led_classdev led_micmute;
+	int press_to_select;
+	int dragging;
+	int release_to_select;
+	int select_right;
+	int sensitivity;
+	int press_speed;
+};
+
+#define map_key_clear(c) hid_map_usage_clear(hi, usage, bit, max, EV_KEY, (c))
+
+static int tpkbd_input_mapping(struct hid_device *hdev,
+		struct hid_input *hi, struct hid_field *field,
+		struct hid_usage *usage, unsigned long **bit, int *max)
+{
+	struct usbhid_device *uhdev;
+
+	uhdev = (struct usbhid_device *) hdev->driver_data;
+	if (uhdev->ifnum == 1 && usage->hid == (HID_UP_BUTTON | 0x0010)) {
+		map_key_clear(KEY_MICMUTE);
+		return 1;
+	}
+	return 0;
+}
+
+#undef map_key_clear
+
+static int tpkbd_features_set(struct hid_device *hdev)
+{
+	struct hid_report *report;
+	struct tpkbd_data_pointer *data_pointer;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+	report = hdev->report_enum[HID_FEATURE_REPORT].report_id_hash[4];
+
+	report->field[0]->value[0]  = data_pointer->press_to_select   ? 0x01 : 0x02;
+	report->field[0]->value[0] |= data_pointer->dragging          ? 0x04 : 0x08;
+	report->field[0]->value[0] |= data_pointer->release_to_select ? 0x10 : 0x20;
+	report->field[0]->value[0] |= data_pointer->select_right      ? 0x80 : 0x40;
+	report->field[1]->value[0] = 0x03; // unknown setting, imitate windows driver
+	report->field[2]->value[0] = data_pointer->sensitivity;
+	report->field[3]->value[0] = data_pointer->press_speed;
+
+	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+	return 0;
+}
+
+static ssize_t pointer_press_to_select_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->press_to_select);
+}
+
+static ssize_t pointer_press_to_select_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf,
+		size_t count)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int value;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (kstrtoint(buf, 10, &value))
+		return -EINVAL;
+	if (value < 0 || value > 1)
+		return -EINVAL;
+
+	data_pointer->press_to_select = value;
+	tpkbd_features_set(hdev);
+
+	return count;
+}
+
+static ssize_t pointer_dragging_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->dragging);
+}
+
+static ssize_t pointer_dragging_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf,
+		size_t count)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int value;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (kstrtoint(buf, 10, &value))
+		return -EINVAL;
+	if (value < 0 || value > 1)
+		return -EINVAL;
+
+	data_pointer->dragging = value;
+	tpkbd_features_set(hdev);
+
+	return count;
+}
+
+static ssize_t pointer_release_to_select_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->release_to_select);
+}
+
+static ssize_t pointer_release_to_select_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf,
+		size_t count)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int value;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (kstrtoint(buf, 10, &value))
+		return -EINVAL;
+	if (value < 0 || value > 1)
+		return -EINVAL;
+
+	data_pointer->release_to_select = value;
+	tpkbd_features_set(hdev);
+
+	return count;
+}
+
+static ssize_t pointer_select_right_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->select_right);
+}
+
+static ssize_t pointer_select_right_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf,
+		size_t count)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int value;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (kstrtoint(buf, 10, &value))
+		return -EINVAL;
+	if (value < 0 || value > 1)
+		return -EINVAL;
+
+	data_pointer->select_right = value;
+	tpkbd_features_set(hdev);
+
+	return count;
+}
+
+static ssize_t pointer_sensitivity_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+		data_pointer->sensitivity);
+}
+
+static ssize_t pointer_sensitivity_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf,
+		size_t count)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int value;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (kstrtoint(buf, 10, &value) || value < 1 || value > 255)
+		return -EINVAL;
+
+	data_pointer->sensitivity = value;
+	tpkbd_features_set(hdev);
+
+	return count;
+}
+
+static ssize_t pointer_press_speed_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+		data_pointer->press_speed);
+}
+
+static ssize_t pointer_press_speed_store(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf,
+		size_t count)
+{
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int value;
+
+	hdev = container_of(dev, struct hid_device, dev);
+	if (hdev == NULL)
+		return -ENODEV;
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (kstrtoint(buf, 10, &value) || value < 1 || value > 255)
+		return -EINVAL;
+
+	data_pointer->press_speed = value;
+	tpkbd_features_set(hdev);
+
+	return count;
+}
+
+static struct device_attribute dev_attr_pointer_press_to_select =
+	__ATTR(press_to_select, S_IWUSR | S_IRUGO,
+			pointer_press_to_select_show,
+			pointer_press_to_select_store);
+
+static struct device_attribute dev_attr_pointer_dragging =
+	__ATTR(dragging, S_IWUSR | S_IRUGO,
+			pointer_dragging_show,
+			pointer_dragging_store);
+
+static struct device_attribute dev_attr_pointer_release_to_select =
+	__ATTR(release_to_select, S_IWUSR | S_IRUGO,
+			pointer_release_to_select_show,
+			pointer_release_to_select_store);
+
+static struct device_attribute dev_attr_pointer_select_right =
+	__ATTR(select_right, S_IWUSR | S_IRUGO,
+			pointer_select_right_show,
+			pointer_select_right_store);
+
+static struct device_attribute dev_attr_pointer_sensitivity =
+	__ATTR(sensitivity, S_IWUSR | S_IRUGO,
+			pointer_sensitivity_show,
+			pointer_sensitivity_store);
+
+static struct device_attribute dev_attr_pointer_press_speed =
+	__ATTR(press_speed, S_IWUSR | S_IRUGO,
+			pointer_press_speed_show,
+			pointer_press_speed_store);
+
+static struct attribute *tpkbd_attributes_pointer[] = {
+	&dev_attr_pointer_press_to_select.attr,
+	&dev_attr_pointer_dragging.attr,
+	&dev_attr_pointer_release_to_select.attr,
+	&dev_attr_pointer_select_right.attr,
+	&dev_attr_pointer_sensitivity.attr,
+	&dev_attr_pointer_press_speed.attr,
+	NULL
+};
+
+static const struct attribute_group tpkbd_attr_group_pointer = {
+	.attrs = tpkbd_attributes_pointer,
+};
+
+static enum led_brightness tpkbd_led_brightness_get(
+			struct led_classdev *led_cdev)
+{
+	struct device *dev;
+	struct hid_device *hdev;
+	struct tpkbd_data_pointer *data_pointer;
+	int led_nr = 0;
+
+	dev = led_cdev->dev->parent;
+	hdev = container_of(dev, struct hid_device, dev);
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (led_cdev == &data_pointer->led_micmute)
+		led_nr = 1;
+
+	return data_pointer->led_state & (1 << led_nr)
+				? LED_FULL
+				: LED_OFF;
+}
+
+static void tpkbd_led_brightness_set(struct led_classdev *led_cdev,
+			enum led_brightness value)
+{
+	struct device *dev;
+	struct hid_device *hdev;
+	struct hid_report *report;
+	struct tpkbd_data_pointer *data_pointer;
+	int led_nr = 0;
+
+	dev = led_cdev->dev->parent;
+	hdev = container_of(dev, struct hid_device, dev);
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	if (led_cdev == &data_pointer->led_micmute)
+		led_nr = 1;
+
+	if (value == LED_OFF)
+		data_pointer->led_state &= ~(1 << led_nr);
+	else
+		data_pointer->led_state |= 1 << led_nr;
+
+	report = hdev->report_enum[HID_OUTPUT_REPORT].report_id_hash[3];
+	report->field[0]->value[0] = (data_pointer->led_state >> 0) & 1;
+	report->field[0]->value[1] = (data_pointer->led_state >> 1) & 1;
+	usbhid_submit_report(hdev, report, USB_DIR_OUT);
+}
+
+static int tpkbd_probe_tp(struct hid_device *hdev)
+{
+	struct device *dev = &hdev->dev;
+	struct tpkbd_data_pointer *data_pointer;
+	size_t name_sz = strlen(dev_name(dev)) + 16;
+	char *name_mute, *name_micmute;
+	int ret;
+
+	if (sysfs_create_group(&hdev->dev.kobj,
+				&tpkbd_attr_group_pointer)) {
+		hid_warn(hdev, "Could not create sysfs group\n");
+	}
+
+	data_pointer = kzalloc(sizeof(struct tpkbd_data_pointer), GFP_KERNEL);
+	if (data_pointer == NULL) {
+		hid_err(hdev, "Could not allocate memory for driver data\n");
+		return -ENOMEM;
+	}
+
+	// set same default values as windows driver
+	data_pointer->sensitivity = 0xa0;
+	data_pointer->press_speed = 0x38;
+
+	name_mute = kzalloc(name_sz, GFP_KERNEL);
+	if (name_mute == NULL) {
+		hid_err(hdev, "Could not allocate memory for led data\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+	snprintf(name_mute, name_sz, "%s:amber:mute", dev_name(dev));
+
+	name_micmute = kzalloc(name_sz, GFP_KERNEL);
+	if (name_micmute == NULL) {
+		hid_err(hdev, "Could not allocate memory for led data\n");
+		ret = -ENOMEM;
+		goto err2;
+	}
+	snprintf(name_micmute, name_sz, "%s:amber:micmute", dev_name(dev));
+
+	hid_set_drvdata(hdev, data_pointer);
+
+	data_pointer->led_mute.name = name_mute;
+	data_pointer->led_mute.brightness_get = tpkbd_led_brightness_get;
+	data_pointer->led_mute.brightness_set = tpkbd_led_brightness_set;
+	data_pointer->led_mute.dev = dev;
+	led_classdev_register(dev, &data_pointer->led_mute);
+
+	data_pointer->led_micmute.name = name_micmute;
+	data_pointer->led_micmute.brightness_get = tpkbd_led_brightness_get;
+	data_pointer->led_micmute.brightness_set = tpkbd_led_brightness_set;
+	data_pointer->led_micmute.dev = dev;
+	led_classdev_register(dev, &data_pointer->led_micmute);
+
+	tpkbd_features_set(hdev);
+
+	return 0;
+
+err2:
+	kfree(name_mute);
+err:
+	kfree(data_pointer);
+	return ret;
+}
+
+static int tpkbd_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int ret;
+	struct usbhid_device *uhdev;
+
+	ret = hid_parse(hdev);
+	if (ret) {
+		hid_err(hdev, "hid_parse failed\n");
+		goto err_free;
+	}
+
+	ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (ret) {
+		hid_err(hdev, "hid_hw_start failed\n");
+		goto err_free;
+	}
+
+	uhdev = (struct usbhid_device *) hdev->driver_data;
+
+	if (uhdev->ifnum == 1)
+		return tpkbd_probe_tp(hdev);
+
+	return 0;
+err_free:
+	return ret;
+}
+
+static void tpkbd_remove_tp(struct hid_device *hdev)
+{
+	struct tpkbd_data_pointer *data_pointer;
+
+	sysfs_remove_group(&hdev->dev.kobj,
+			&tpkbd_attr_group_pointer);
+
+	data_pointer = (struct tpkbd_data_pointer *) hid_get_drvdata(hdev);
+
+	led_classdev_unregister(&data_pointer->led_micmute);
+	led_classdev_unregister(&data_pointer->led_mute);
+
+	hid_set_drvdata(hdev, NULL);
+	kfree(data_pointer);
+}
+
+static void tpkbd_remove(struct hid_device *hdev)
+{
+	struct usbhid_device *uhdev;
+
+	uhdev = (struct usbhid_device *) hdev->driver_data;
+	if (uhdev->ifnum == 1)
+		tpkbd_remove_tp(hdev);
+
+	hid_hw_stop(hdev);
+}
+
+static const struct hid_device_id tpkbd_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPKBD) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, tpkbd_devices);
+
+static struct hid_driver tpkbd_driver = {
+	.name = "lenovo_tpkbd",
+	.id_table = tpkbd_devices,
+	.input_mapping = tpkbd_input_mapping,
+	.probe = tpkbd_probe,
+	.remove = tpkbd_remove,
+};
+
+static int __init tpkbd_init(void)
+{
+	return hid_register_driver(&tpkbd_driver);
+}
+
+static void __exit tpkbd_exit(void)
+{
+	hid_unregister_driver(&tpkbd_driver);
+}
+
+module_init(tpkbd_init);
+module_exit(tpkbd_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index 40ac6654f1d..73647266daa 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -16,6 +16,7 @@
 
 #include <linux/device.h>
 #include <linux/hid.h>
+#include <linux/input/mt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/usb.h>
@@ -48,10 +49,6 @@ static bool scroll_acceleration = false;
 module_param(scroll_acceleration, bool, 0644);
 MODULE_PARM_DESC(scroll_acceleration, "Accelerate sequential scroll events");
 
-static bool report_touches = true;
-module_param(report_touches, bool, 0644);
-MODULE_PARM_DESC(report_touches, "Emit touch records (otherwise, only use them for emulation)");
-
 static bool report_undeciphered;
 module_param(report_undeciphered, bool, 0644);
 MODULE_PARM_DESC(report_undeciphered, "Report undeciphered multi-touch state field using a MSC_RAW event");
@@ -72,15 +69,6 @@ MODULE_PARM_DESC(report_undeciphered, "Report undeciphered multi-touch state fie
 
 #define SCROLL_ACCEL_DEFAULT 7
 
-/* Single touch emulation should only begin when no touches are currently down.
- * This is true when single_touch_id is equal to NO_TOUCHES. If multiple touches
- * are down and the touch providing for single touch emulation is lifted,
- * single_touch_id is equal to SINGLE_TOUCH_UP. While single touch emulation is
- * occurring, single_touch_id corresponds with the tracking id of the touch used.
- */
-#define NO_TOUCHES -1
-#define SINGLE_TOUCH_UP -2
-
 /* Touch surface information. Dimension is in hundredths of a mm, min and max
  * are in units. */
 #define MOUSE_DIMENSION_X (float)9056
@@ -129,7 +117,6 @@ struct magicmouse_sc {
 		u8 size;
 	} touches[16];
 	int tracking_ids[16];
-	int single_touch_id;
 };
 
 static int magicmouse_firm_touch(struct magicmouse_sc *msc)
@@ -268,16 +255,14 @@ static void magicmouse_emit_touch(struct magicmouse_sc *msc, int raw_id, u8 *tda
 		}
 	}
 
-	if (down) {
+	if (down)
 		msc->ntouches++;
-		if (msc->single_touch_id == NO_TOUCHES)
-			msc->single_touch_id = id;
-	} else if (msc->single_touch_id == id)
-		msc->single_touch_id = SINGLE_TOUCH_UP;
+
+	input_mt_slot(input, id);
+	input_mt_report_slot_state(input, MT_TOOL_FINGER, down);
 
 	/* Generate the input events for this touch. */
-	if (report_touches && down) {
-		input_report_abs(input, ABS_MT_TRACKING_ID, id);
+	if (down) {
 		input_report_abs(input, ABS_MT_TOUCH_MAJOR, touch_major << 2);
 		input_report_abs(input, ABS_MT_TOUCH_MINOR, touch_minor << 2);
 		input_report_abs(input, ABS_MT_ORIENTATION, -orientation);
@@ -290,8 +275,6 @@ static void magicmouse_emit_touch(struct magicmouse_sc *msc, int raw_id, u8 *tda
 			else /* USB_DEVICE_ID_APPLE_MAGICTRACKPAD */
 				input_event(input, EV_MSC, MSC_RAW, tdata[8]);
 		}
-
-		input_mt_sync(input);
 	}
 }
 
@@ -312,12 +295,6 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 		for (ii = 0; ii < npoints; ii++)
 			magicmouse_emit_touch(msc, ii, data + ii * 9 + 4);
 
-		/* We don't need an MT sync here because trackpad emits a
-		 * BTN_TOUCH event in a new frame when all touches are released.
-		 */
-		if (msc->ntouches == 0)
-			msc->single_touch_id = NO_TOUCHES;
-
 		clicks = data[1];
 
 		/* The following bits provide a device specific timestamp. They
@@ -335,9 +312,6 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 		for (ii = 0; ii < npoints; ii++)
 			magicmouse_emit_touch(msc, ii, data + ii * 8 + 6);
 
-		if (report_touches && msc->ntouches == 0)
-			input_mt_sync(input);
-
 		/* When emulating three-button mode, it is important
 		 * to have the current touch information before
 		 * generating a click event.
@@ -370,25 +344,17 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 		input_report_rel(input, REL_Y, y);
 	} else { /* USB_DEVICE_ID_APPLE_MAGICTRACKPAD */
 		input_report_key(input, BTN_MOUSE, clicks & 1);
-		input_report_key(input, BTN_TOUCH, msc->ntouches > 0);
-		input_report_key(input, BTN_TOOL_FINGER, msc->ntouches == 1);
-		input_report_key(input, BTN_TOOL_DOUBLETAP, msc->ntouches == 2);
-		input_report_key(input, BTN_TOOL_TRIPLETAP, msc->ntouches == 3);
-		input_report_key(input, BTN_TOOL_QUADTAP, msc->ntouches == 4);
-		if (msc->single_touch_id >= 0) {
-			input_report_abs(input, ABS_X,
-				msc->touches[msc->single_touch_id].x);
-			input_report_abs(input, ABS_Y,
-				msc->touches[msc->single_touch_id].y);
-		}
+		input_mt_report_pointer_emulation(input, true);
 	}
 
 	input_sync(input);
 	return 1;
 }
 
-static void magicmouse_setup_input(struct input_dev *input, struct hid_device *hdev)
+static int magicmouse_setup_input(struct input_dev *input, struct hid_device *hdev)
 {
+	int error;
+
 	__set_bit(EV_KEY, input->evbit);
 
 	if (input->id.product == USB_DEVICE_ID_APPLE_MAGICMOUSE) {
@@ -417,62 +383,66 @@ static void magicmouse_setup_input(struct input_dev *input, struct hid_device *h
 		__set_bit(BTN_TOOL_DOUBLETAP, input->keybit);
 		__set_bit(BTN_TOOL_TRIPLETAP, input->keybit);
 		__set_bit(BTN_TOOL_QUADTAP, input->keybit);
+		__set_bit(BTN_TOOL_QUINTTAP, input->keybit);
 		__set_bit(BTN_TOUCH, input->keybit);
 		__set_bit(INPUT_PROP_POINTER, input->propbit);
 		__set_bit(INPUT_PROP_BUTTONPAD, input->propbit);
 	}
 
-	if (report_touches) {
-		__set_bit(EV_ABS, input->evbit);
-
-		input_set_abs_params(input, ABS_MT_TRACKING_ID, 0, 15, 0, 0);
-		input_set_abs_params(input, ABS_MT_TOUCH_MAJOR, 0, 255 << 2,
-				     4, 0);
-		input_set_abs_params(input, ABS_MT_TOUCH_MINOR, 0, 255 << 2,
-				     4, 0);
-		input_set_abs_params(input, ABS_MT_ORIENTATION, -31, 32, 1, 0);
-
-		/* Note: Touch Y position from the device is inverted relative
-		 * to how pointer motion is reported (and relative to how USB
-		 * HID recommends the coordinates work).  This driver keeps
-		 * the origin at the same position, and just uses the additive
-		 * inverse of the reported Y.
-		 */
-		if (input->id.product == USB_DEVICE_ID_APPLE_MAGICMOUSE) {
-			input_set_abs_params(input, ABS_MT_POSITION_X,
-				MOUSE_MIN_X, MOUSE_MAX_X, 4, 0);
-			input_set_abs_params(input, ABS_MT_POSITION_Y,
-				MOUSE_MIN_Y, MOUSE_MAX_Y, 4, 0);
-
-			input_abs_set_res(input, ABS_MT_POSITION_X,
-				MOUSE_RES_X);
-			input_abs_set_res(input, ABS_MT_POSITION_Y,
-				MOUSE_RES_Y);
-		} else { /* USB_DEVICE_ID_APPLE_MAGICTRACKPAD */
-			input_set_abs_params(input, ABS_X, TRACKPAD_MIN_X,
-				TRACKPAD_MAX_X, 4, 0);
-			input_set_abs_params(input, ABS_Y, TRACKPAD_MIN_Y,
-				TRACKPAD_MAX_Y, 4, 0);
-			input_set_abs_params(input, ABS_MT_POSITION_X,
-				TRACKPAD_MIN_X, TRACKPAD_MAX_X, 4, 0);
-			input_set_abs_params(input, ABS_MT_POSITION_Y,
-				TRACKPAD_MIN_Y, TRACKPAD_MAX_Y, 4, 0);
-
-			input_abs_set_res(input, ABS_X, TRACKPAD_RES_X);
-			input_abs_set_res(input, ABS_Y, TRACKPAD_RES_Y);
-			input_abs_set_res(input, ABS_MT_POSITION_X,
-				TRACKPAD_RES_X);
-			input_abs_set_res(input, ABS_MT_POSITION_Y,
-				TRACKPAD_RES_Y);
-		}
 
-		input_set_events_per_packet(input, 60);
+	__set_bit(EV_ABS, input->evbit);
+
+	error = input_mt_init_slots(input, 16);
+	if (error)
+		return error;
+	input_set_abs_params(input, ABS_MT_TOUCH_MAJOR, 0, 255 << 2,
+			     4, 0);
+	input_set_abs_params(input, ABS_MT_TOUCH_MINOR, 0, 255 << 2,
+			     4, 0);
+	input_set_abs_params(input, ABS_MT_ORIENTATION, -31, 32, 1, 0);
+
+	/* Note: Touch Y position from the device is inverted relative
+	 * to how pointer motion is reported (and relative to how USB
+	 * HID recommends the coordinates work).  This driver keeps
+	 * the origin at the same position, and just uses the additive
+	 * inverse of the reported Y.
+	 */
+	if (input->id.product == USB_DEVICE_ID_APPLE_MAGICMOUSE) {
+		input_set_abs_params(input, ABS_MT_POSITION_X,
+				     MOUSE_MIN_X, MOUSE_MAX_X, 4, 0);
+		input_set_abs_params(input, ABS_MT_POSITION_Y,
+				     MOUSE_MIN_Y, MOUSE_MAX_Y, 4, 0);
+
+		input_abs_set_res(input, ABS_MT_POSITION_X,
+				  MOUSE_RES_X);
+		input_abs_set_res(input, ABS_MT_POSITION_Y,
+				  MOUSE_RES_Y);
+	} else { /* USB_DEVICE_ID_APPLE_MAGICTRACKPAD */
+		input_set_abs_params(input, ABS_X, TRACKPAD_MIN_X,
+				     TRACKPAD_MAX_X, 4, 0);
+		input_set_abs_params(input, ABS_Y, TRACKPAD_MIN_Y,
+				     TRACKPAD_MAX_Y, 4, 0);
+		input_set_abs_params(input, ABS_MT_POSITION_X,
+				     TRACKPAD_MIN_X, TRACKPAD_MAX_X, 4, 0);
+		input_set_abs_params(input, ABS_MT_POSITION_Y,
+				     TRACKPAD_MIN_Y, TRACKPAD_MAX_Y, 4, 0);
+
+		input_abs_set_res(input, ABS_X, TRACKPAD_RES_X);
+		input_abs_set_res(input, ABS_Y, TRACKPAD_RES_Y);
+		input_abs_set_res(input, ABS_MT_POSITION_X,
+				  TRACKPAD_RES_X);
+		input_abs_set_res(input, ABS_MT_POSITION_Y,
+				  TRACKPAD_RES_Y);
 	}
 
+	input_set_events_per_packet(input, 60);
+
 	if (report_undeciphered) {
 		__set_bit(EV_MSC, input->evbit);
 		__set_bit(MSC_RAW, input->mscbit);
 	}
+
+	return 0;
 }
 
 static int magicmouse_input_mapping(struct hid_device *hdev,
@@ -511,8 +481,6 @@ static int magicmouse_probe(struct hid_device *hdev,
 	msc->quirks = id->driver_data;
 	hid_set_drvdata(hdev, msc);
 
-	msc->single_touch_id = NO_TOUCHES;
-
 	ret = hid_parse(hdev);
 	if (ret) {
 		hid_err(hdev, "magicmouse hid parse failed\n");
@@ -528,8 +496,13 @@ static int magicmouse_probe(struct hid_device *hdev,
 	/* We do this after hid-input is done parsing reports so that
 	 * hid-input uses the most natural button and axis IDs.
 	 */
-	if (msc->input)
-		magicmouse_setup_input(msc->input, hdev);
+	if (msc->input) {
+		ret = magicmouse_setup_input(msc->input, hdev);
+		if (ret) {
+			hid_err(hdev, "magicmouse setup input failed (%d)\n", ret);
+			goto err_stop_hw;
+		}
+	}
 
 	if (id->product == USB_DEVICE_ID_APPLE_MAGICMOUSE)
 		report = hid_register_report(hdev, HID_INPUT_REPORT,
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 6e3332a9997..59c8b5c1d2d 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -83,6 +83,7 @@ struct mt_device {
 	unsigned last_field_index;	/* last field index of the report */
 	unsigned last_slot_field;	/* the last field of a slot */
 	__s8 inputmode;		/* InputMode HID feature, -1 if non-existent */
+	__s8 inputmode_index;	/* InputMode HID feature index in the report */
 	__s8 maxcontact_report_id;	/* Maximum Contact Number HID feature,
 				   -1 if non-existent */
 	__u8 num_received;	/* how many contacts we received */
@@ -260,10 +261,20 @@ static void mt_feature_mapping(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage)
 {
 	struct mt_device *td = hid_get_drvdata(hdev);
+	int i;
 
 	switch (usage->hid) {
 	case HID_DG_INPUTMODE:
 		td->inputmode = field->report->id;
+		td->inputmode_index = 0; /* has to be updated below */
+
+		for (i=0; i < field->maxusage; i++) {
+			if (field->usage[i].hid == usage->hid) {
+				td->inputmode_index = i;
+				break;
+			}
+		}
+
 		break;
 	case HID_DG_CONTACTMAX:
 		td->maxcontact_report_id = field->report->id;
@@ -618,7 +629,7 @@ static void mt_set_input_mode(struct hid_device *hdev)
 	re = &(hdev->report_enum[HID_FEATURE_REPORT]);
 	r = re->report_id_hash[td->inputmode];
 	if (r) {
-		r->field[0]->value[0] = 0x02;
+		r->field[0]->value[td->inputmode_index] = 0x02;
 		usbhid_submit_report(hdev, r, USB_DIR_OUT);
 	}
 }
@@ -951,6 +962,11 @@ static const struct hid_device_id mt_devices[] = {
 		MT_USB_DEVICE(USB_VENDOR_ID_PANASONIC,
 			USB_DEVICE_ID_PANABOARD_UBT880) },
 
+	/* Novatek Panel */
+	{ .driver_data = MT_CLS_DEFAULT,
+		MT_USB_DEVICE(USB_VENDOR_ID_NOVATEK,
+			USB_DEVICE_ID_NOVATEK_PCT) },
+
 	/* PenMount panels */
 	{ .driver_data = MT_CLS_CONFIDENCE,
 		MT_USB_DEVICE(USB_VENDOR_ID_PENMOUNT,
@@ -1048,6 +1064,11 @@ static const struct hid_device_id mt_devices[] = {
 		MT_USB_DEVICE(USB_VENDOR_ID_XIROKU,
 			USB_DEVICE_ID_XIROKU_CSR2) },
 
+	/* Zytronic panels */
+	{ .driver_data = MT_CLS_SERIAL,
+		MT_USB_DEVICE(USB_VENDOR_ID_ZYTRONIC,
+			USB_DEVICE_ID_ZYTRONIC_ZXY100) },
+
 	/* Generic MT device */
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_MULTITOUCH, HID_ANY_ID, HID_ANY_ID) },
 	{ }
diff --git a/drivers/hid/hid-picolcd.c b/drivers/hid/hid-picolcd.c
index 45c3433f798..27c8ebdfad0 100644
--- a/drivers/hid/hid-picolcd.c
+++ b/drivers/hid/hid-picolcd.c
@@ -1846,7 +1846,7 @@ static void picolcd_debug_out_report(struct picolcd_data *data,
 #define BUFF_SZ 256
 
 	/* Avoid unnecessary overhead if debugfs is disabled */
-	if (!hdev->debug_events)
+	if (list_empty(&hdev->debug_list))
 		return;
 
 	buff = kmalloc(BUFF_SZ, GFP_ATOMIC);
@@ -2613,11 +2613,7 @@ static int picolcd_probe(struct hid_device *hdev,
 		goto err_cleanup_data;
 	}
 
-	/* We don't use hidinput but hid_hw_start() fails if nothing is
-	 * claimed. So spoof claimed input. */
-	hdev->claimed = HID_CLAIMED_INPUT;
 	error = hid_hw_start(hdev, 0);
-	hdev->claimed = 0;
 	if (error) {
 		hid_err(hdev, "hardware start failed\n");
 		goto err_cleanup_data;
diff --git a/drivers/hid/hid-roccat-arvo.c b/drivers/hid/hid-roccat-arvo.c
index 093bfad00b0..327f9b8ed1f 100644
--- a/drivers/hid/hid-roccat-arvo.c
+++ b/drivers/hid/hid-roccat-arvo.c
@@ -39,7 +39,7 @@ static ssize_t arvo_sysfs_show_mode_key(struct device *dev,
 	int retval;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_receive(usb_dev, ARVO_COMMAND_MODE_KEY,
+	retval = roccat_common2_receive(usb_dev, ARVO_COMMAND_MODE_KEY,
 			&temp_buf, sizeof(struct arvo_mode_key));
 	mutex_unlock(&arvo->arvo_lock);
 	if (retval)
@@ -67,7 +67,7 @@ static ssize_t arvo_sysfs_set_mode_key(struct device *dev,
 	temp_buf.state = state;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_send(usb_dev, ARVO_COMMAND_MODE_KEY,
+	retval = roccat_common2_send(usb_dev, ARVO_COMMAND_MODE_KEY,
 			&temp_buf, sizeof(struct arvo_mode_key));
 	mutex_unlock(&arvo->arvo_lock);
 	if (retval)
@@ -87,7 +87,7 @@ static ssize_t arvo_sysfs_show_key_mask(struct device *dev,
 	int retval;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_receive(usb_dev, ARVO_COMMAND_KEY_MASK,
+	retval = roccat_common2_receive(usb_dev, ARVO_COMMAND_KEY_MASK,
 			&temp_buf, sizeof(struct arvo_key_mask));
 	mutex_unlock(&arvo->arvo_lock);
 	if (retval)
@@ -115,7 +115,7 @@ static ssize_t arvo_sysfs_set_key_mask(struct device *dev,
 	temp_buf.key_mask = key_mask;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_send(usb_dev, ARVO_COMMAND_KEY_MASK,
+	retval = roccat_common2_send(usb_dev, ARVO_COMMAND_KEY_MASK,
 			&temp_buf, sizeof(struct arvo_key_mask));
 	mutex_unlock(&arvo->arvo_lock);
 	if (retval)
@@ -130,7 +130,7 @@ 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_COMMAND_ACTUAL_PROFILE,
+	retval = roccat_common2_receive(usb_dev, ARVO_COMMAND_ACTUAL_PROFILE,
 			&temp_buf, sizeof(struct arvo_actual_profile));
 
 	if (retval)
@@ -170,7 +170,7 @@ static ssize_t arvo_sysfs_set_actual_profile(struct device *dev,
 	temp_buf.actual_profile = profile;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_send(usb_dev, ARVO_COMMAND_ACTUAL_PROFILE,
+	retval = roccat_common2_send(usb_dev, ARVO_COMMAND_ACTUAL_PROFILE,
 			&temp_buf, sizeof(struct arvo_actual_profile));
 	if (!retval) {
 		arvo->actual_profile = profile;
@@ -194,7 +194,7 @@ static ssize_t arvo_sysfs_write(struct file *fp,
 		return -EINVAL;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_send(usb_dev, command, buf, real_size);
+	retval = roccat_common2_send(usb_dev, command, buf, real_size);
 	mutex_unlock(&arvo->arvo_lock);
 
 	return (retval ? retval : real_size);
@@ -217,7 +217,7 @@ static ssize_t arvo_sysfs_read(struct file *fp,
 		return -EINVAL;
 
 	mutex_lock(&arvo->arvo_lock);
-	retval = roccat_common_receive(usb_dev, command, buf, real_size);
+	retval = roccat_common2_receive(usb_dev, command, buf, real_size);
 	mutex_unlock(&arvo->arvo_lock);
 
 	return (retval ? retval : real_size);
diff --git a/drivers/hid/hid-roccat-common.c b/drivers/hid/hid-roccat-common.c
index a6d93992c75..74f70403262 100644
--- a/drivers/hid/hid-roccat-common.c
+++ b/drivers/hid/hid-roccat-common.c
@@ -16,12 +16,12 @@
 #include <linux/module.h>
 #include "hid-roccat-common.h"
 
-static inline uint16_t roccat_common_feature_report(uint8_t report_id)
+static inline uint16_t roccat_common2_feature_report(uint8_t report_id)
 {
 	return 0x300 | report_id;
 }
 
-int roccat_common_receive(struct usb_device *usb_dev, uint report_id,
+int roccat_common2_receive(struct usb_device *usb_dev, uint report_id,
 		void *data, uint size)
 {
 	char *buf;
@@ -34,16 +34,16 @@ int roccat_common_receive(struct usb_device *usb_dev, uint report_id,
 	len = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
 			HID_REQ_GET_REPORT,
 			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_IN,
-			roccat_common_feature_report(report_id),
+			roccat_common2_feature_report(report_id),
 			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);
+EXPORT_SYMBOL_GPL(roccat_common2_receive);
 
-int roccat_common_send(struct usb_device *usb_dev, uint report_id,
+int roccat_common2_send(struct usb_device *usb_dev, uint report_id,
 		void const *data, uint size)
 {
 	char *buf;
@@ -56,13 +56,71 @@ int roccat_common_send(struct usb_device *usb_dev, uint report_id,
 	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
 			HID_REQ_SET_REPORT,
 			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
-			roccat_common_feature_report(report_id),
+			roccat_common2_feature_report(report_id),
 			0, buf, size, USB_CTRL_SET_TIMEOUT);
 
 	kfree(buf);
 	return ((len < 0) ? len : ((len != size) ? -EIO : 0));
 }
-EXPORT_SYMBOL_GPL(roccat_common_send);
+EXPORT_SYMBOL_GPL(roccat_common2_send);
+
+enum roccat_common2_control_states {
+	ROCCAT_COMMON_CONTROL_STATUS_OVERLOAD = 0,
+	ROCCAT_COMMON_CONTROL_STATUS_OK = 1,
+	ROCCAT_COMMON_CONTROL_STATUS_INVALID = 2,
+	ROCCAT_COMMON_CONTROL_STATUS_WAIT = 3,
+};
+
+static int roccat_common2_receive_control_status(struct usb_device *usb_dev)
+{
+	int retval;
+	struct roccat_common2_control control;
+
+	do {
+		msleep(50);
+		retval = roccat_common2_receive(usb_dev,
+				ROCCAT_COMMON_COMMAND_CONTROL,
+				&control, sizeof(struct roccat_common2_control));
+
+		if (retval)
+			return retval;
+
+		switch (control.value) {
+		case ROCCAT_COMMON_CONTROL_STATUS_OK:
+			return 0;
+		case ROCCAT_COMMON_CONTROL_STATUS_WAIT:
+			msleep(500);
+			continue;
+		case ROCCAT_COMMON_CONTROL_STATUS_INVALID:
+
+		case ROCCAT_COMMON_CONTROL_STATUS_OVERLOAD:
+			/* seems to be critical - replug necessary */
+			return -EINVAL;
+		default:
+			dev_err(&usb_dev->dev,
+					"roccat_common2_receive_control_status: "
+					"unknown response value 0x%x\n",
+					control.value);
+			return -EINVAL;
+		}
+
+	} while (1);
+}
+
+int roccat_common2_send_with_status(struct usb_device *usb_dev,
+		uint command, void const *buf, uint size)
+{
+	int retval;
+
+	retval = roccat_common2_send(usb_dev, command, buf, size);
+	if (retval)
+		return retval;
+
+	msleep(100);
+
+	return roccat_common2_receive_control_status(usb_dev);
+}
+EXPORT_SYMBOL_GPL(roccat_common2_send_with_status);
 
 MODULE_AUTHOR("Stefan Achatz");
 MODULE_DESCRIPTION("USB Roccat common driver");
diff --git a/drivers/hid/hid-roccat-common.h b/drivers/hid/hid-roccat-common.h
index 9a5bc61f969..a97746a63b7 100644
--- a/drivers/hid/hid-roccat-common.h
+++ b/drivers/hid/hid-roccat-common.h
@@ -15,9 +15,21 @@
 #include <linux/usb.h>
 #include <linux/types.h>
 
-int roccat_common_receive(struct usb_device *usb_dev, uint report_id,
+enum roccat_common2_commands {
+	ROCCAT_COMMON_COMMAND_CONTROL = 0x4,
+};
+
+struct roccat_common2_control {
+	uint8_t command;
+	uint8_t value;
+	uint8_t request; /* always 0 on requesting write check */
+} __packed;
+
+int roccat_common2_receive(struct usb_device *usb_dev, uint report_id,
 		void *data, uint size);
-int roccat_common_send(struct usb_device *usb_dev, uint report_id,
+int roccat_common2_send(struct usb_device *usb_dev, uint report_id,
 		void const *data, uint size);
+int roccat_common2_send_with_status(struct usb_device *usb_dev,
+		uint command, void const *buf, uint size);
 
 #endif
diff --git a/drivers/hid/hid-roccat-isku.c b/drivers/hid/hid-roccat-isku.c
index 0e4a0ab4714..5669916c294 100644
--- a/drivers/hid/hid-roccat-isku.c
+++ b/drivers/hid/hid-roccat-isku.c
@@ -36,51 +36,7 @@ static void isku_profile_activated(struct isku_device *isku, uint new_profile)
 static int isku_receive(struct usb_device *usb_dev, uint command,
 		void *buf, uint size)
 {
-	return roccat_common_receive(usb_dev, command, buf, size);
-}
-
-static int isku_receive_control_status(struct usb_device *usb_dev)
-{
-	int retval;
-	struct isku_control control;
-
-	do {
-		msleep(50);
-		retval = isku_receive(usb_dev, ISKU_COMMAND_CONTROL,
-				&control, sizeof(struct isku_control));
-
-		if (retval)
-			return retval;
-
-		switch (control.value) {
-		case ISKU_CONTROL_VALUE_STATUS_OK:
-			return 0;
-		case ISKU_CONTROL_VALUE_STATUS_WAIT:
-			continue;
-		case ISKU_CONTROL_VALUE_STATUS_INVALID:
-		/* seems to be critical - replug necessary */
-		case ISKU_CONTROL_VALUE_STATUS_OVERLOAD:
-			return -EINVAL;
-		default:
-			hid_err(usb_dev, "isku_receive_control_status: "
-					"unknown response value 0x%x\n",
-					control.value);
-			return -EINVAL;
-		}
-
-	} while (1);
-}
-
-static int isku_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 isku_receive_control_status(usb_dev);
+	return roccat_common2_receive(usb_dev, command, buf, size);
 }
 
 static int isku_get_actual_profile(struct usb_device *usb_dev)
@@ -100,7 +56,8 @@ static int isku_set_actual_profile(struct usb_device *usb_dev, int new_profile)
 	buf.command = ISKU_COMMAND_ACTUAL_PROFILE;
 	buf.size = sizeof(struct isku_actual_profile);
 	buf.actual_profile = new_profile;
-	return isku_send(usb_dev, ISKU_COMMAND_ACTUAL_PROFILE, &buf,
+	return roccat_common2_send_with_status(usb_dev,
+			ISKU_COMMAND_ACTUAL_PROFILE, &buf,
 			sizeof(struct isku_actual_profile));
 }
 
@@ -197,7 +154,8 @@ static ssize_t isku_sysfs_write(struct file *fp, struct kobject *kobj,
 		return -EINVAL;
 
 	mutex_lock(&isku->isku_lock);
-	retval = isku_send(usb_dev, command, (void *)buf, real_size);
+	retval = roccat_common2_send_with_status(usb_dev, command,
+			(void *)buf, real_size);
 	mutex_unlock(&isku->isku_lock);
 
 	return retval ? retval : real_size;
diff --git a/drivers/hid/hid-roccat-isku.h b/drivers/hid/hid-roccat-isku.h
index 075f6efaec5..605b3ce2163 100644
--- a/drivers/hid/hid-roccat-isku.h
+++ b/drivers/hid/hid-roccat-isku.h
@@ -25,13 +25,6 @@ struct isku_control {
 	uint8_t request;
 } __packed;
 
-enum isku_control_values {
-	ISKU_CONTROL_VALUE_STATUS_OVERLOAD = 0,
-	ISKU_CONTROL_VALUE_STATUS_OK = 1,
-	ISKU_CONTROL_VALUE_STATUS_INVALID = 2,
-	ISKU_CONTROL_VALUE_STATUS_WAIT = 3,
-};
-
 struct isku_actual_profile {
 	uint8_t command; /* ISKU_COMMAND_ACTUAL_PROFILE */
 	uint8_t size; /* always 3 */
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index 40090d60215..9ce2d0b615a 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -138,7 +138,7 @@ static int kone_check_write(struct usb_device *usb_dev)
 		return 0;
 
 	/* unknown answer */
-	hid_err(usb_dev, "got retval %d when checking write\n", data);
+	dev_err(&usb_dev->dev, "got retval %d when checking write\n", data);
 	return -EIO;
 }
 
@@ -503,7 +503,7 @@ static ssize_t kone_sysfs_set_tcu(struct device *dev,
 
 		retval = kone_set_settings(usb_dev, &kone->settings);
 		if (retval) {
-			hid_err(usb_dev, "couldn't set tcu state\n");
+			dev_err(&usb_dev->dev, "couldn't set tcu state\n");
 			/*
 			 * try to reread valid settings into buffer overwriting
 			 * first error code
@@ -519,7 +519,7 @@ static ssize_t kone_sysfs_set_tcu(struct device *dev,
 
 	retval = size;
 exit_no_settings:
-	hid_err(usb_dev, "couldn't read settings\n");
+	dev_err(&usb_dev->dev, "couldn't read settings\n");
 exit_unlock:
 	mutex_unlock(&kone->kone_lock);
 	return retval;
diff --git a/drivers/hid/hid-roccat-koneplus.c b/drivers/hid/hid-roccat-koneplus.c
index 59e47770fa1..f5602fec486 100644
--- a/drivers/hid/hid-roccat-koneplus.c
+++ b/drivers/hid/hid-roccat-koneplus.c
@@ -39,88 +39,26 @@ 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)
 {
-	struct koneplus_control control;
+	struct roccat_common2_control control;
 
 	if ((request == KONEPLUS_CONTROL_REQUEST_PROFILE_SETTINGS ||
 			request == KONEPLUS_CONTROL_REQUEST_PROFILE_BUTTONS) &&
 			value > 4)
 		return -EINVAL;
 
-	control.command = KONEPLUS_COMMAND_CONTROL;
+	control.command = ROCCAT_COMMON_COMMAND_CONTROL;
 	control.value = value;
 	control.request = request;
 
-	return roccat_common_send(usb_dev, KONEPLUS_COMMAND_CONTROL,
-			&control, sizeof(struct koneplus_control));
-}
-
-static int koneplus_receive_control_status(struct usb_device *usb_dev)
-{
-	int retval;
-	struct koneplus_control control;
-
-	do {
-		retval = roccat_common_receive(usb_dev, KONEPLUS_COMMAND_CONTROL,
-				&control, sizeof(struct koneplus_control));
-
-		/* check if we get a completely wrong answer */
-		if (retval)
-			return retval;
-
-		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) {
-			msleep(500); /* windows driver uses 1000 */
-			continue;
-		}
-
-		/* seems to be critical - replug necessary */
-		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);
-}
-
-static int koneplus_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 koneplus_receive_control_status(usb_dev);
-}
-
-static int koneplus_select_profile(struct usb_device *usb_dev, uint number,
-		enum koneplus_control_requests request)
-{
-	int retval;
-
-	retval = koneplus_send_control(usb_dev, number, request);
-	if (retval)
-		return retval;
-
-	/* allow time to settle things - windows driver uses 500 */
-	msleep(100);
-
-	retval = koneplus_receive_control_status(usb_dev);
-	if (retval)
-		return retval;
-
-	return 0;
+	return roccat_common2_send_with_status(usb_dev,
+			ROCCAT_COMMON_COMMAND_CONTROL,
+			&control, sizeof(struct roccat_common2_control));
 }
 
 static int koneplus_get_info(struct usb_device *usb_dev,
 		struct koneplus_info *buf)
 {
-	return roccat_common_receive(usb_dev, KONEPLUS_COMMAND_INFO,
+	return roccat_common2_receive(usb_dev, KONEPLUS_COMMAND_INFO,
 			buf, sizeof(struct koneplus_info));
 }
 
@@ -129,19 +67,20 @@ static int koneplus_get_profile_settings(struct usb_device *usb_dev,
 {
 	int retval;
 
-	retval = koneplus_select_profile(usb_dev, number,
+	retval = koneplus_send_control(usb_dev, number,
 			KONEPLUS_CONTROL_REQUEST_PROFILE_SETTINGS);
 	if (retval)
 		return retval;
 
-	return roccat_common_receive(usb_dev, KONEPLUS_COMMAND_PROFILE_SETTINGS,
+	return roccat_common2_receive(usb_dev, KONEPLUS_COMMAND_PROFILE_SETTINGS,
 			buf, sizeof(struct koneplus_profile_settings));
 }
 
 static int koneplus_set_profile_settings(struct usb_device *usb_dev,
 		struct koneplus_profile_settings const *settings)
 {
-	return koneplus_send(usb_dev, KONEPLUS_COMMAND_PROFILE_SETTINGS,
+	return roccat_common2_send_with_status(usb_dev,
+			KONEPLUS_COMMAND_PROFILE_SETTINGS,
 			settings, sizeof(struct koneplus_profile_settings));
 }
 
@@ -150,19 +89,20 @@ static int koneplus_get_profile_buttons(struct usb_device *usb_dev,
 {
 	int retval;
 
-	retval = koneplus_select_profile(usb_dev, number,
+	retval = koneplus_send_control(usb_dev, number,
 			KONEPLUS_CONTROL_REQUEST_PROFILE_BUTTONS);
 	if (retval)
 		return retval;
 
-	return roccat_common_receive(usb_dev, KONEPLUS_COMMAND_PROFILE_BUTTONS,
+	return roccat_common2_receive(usb_dev, KONEPLUS_COMMAND_PROFILE_BUTTONS,
 			buf, sizeof(struct koneplus_profile_buttons));
 }
 
 static int koneplus_set_profile_buttons(struct usb_device *usb_dev,
 		struct koneplus_profile_buttons const *buttons)
 {
-	return koneplus_send(usb_dev, KONEPLUS_COMMAND_PROFILE_BUTTONS,
+	return roccat_common2_send_with_status(usb_dev,
+			KONEPLUS_COMMAND_PROFILE_BUTTONS,
 			buttons, sizeof(struct koneplus_profile_buttons));
 }
 
@@ -172,7 +112,7 @@ static int koneplus_get_actual_profile(struct usb_device *usb_dev)
 	struct koneplus_actual_profile buf;
 	int retval;
 
-	retval = roccat_common_receive(usb_dev, KONEPLUS_COMMAND_ACTUAL_PROFILE,
+	retval = roccat_common2_receive(usb_dev, KONEPLUS_COMMAND_ACTUAL_PROFILE,
 			&buf, sizeof(struct koneplus_actual_profile));
 
 	return retval ? retval : buf.actual_profile;
@@ -187,7 +127,8 @@ static int koneplus_set_actual_profile(struct usb_device *usb_dev,
 	buf.size = sizeof(struct koneplus_actual_profile);
 	buf.actual_profile = new_profile;
 
-	return koneplus_send(usb_dev, KONEPLUS_COMMAND_ACTUAL_PROFILE,
+	return roccat_common2_send_with_status(usb_dev,
+			KONEPLUS_COMMAND_ACTUAL_PROFILE,
 			&buf, sizeof(struct koneplus_actual_profile));
 }
 
@@ -208,7 +149,7 @@ static ssize_t koneplus_sysfs_read(struct file *fp, struct kobject *kobj,
 		return -EINVAL;
 
 	mutex_lock(&koneplus->koneplus_lock);
-	retval = roccat_common_receive(usb_dev, command, buf, real_size);
+	retval = roccat_common2_receive(usb_dev, command, buf, real_size);
 	mutex_unlock(&koneplus->koneplus_lock);
 
 	if (retval)
@@ -231,7 +172,8 @@ 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, buf, real_size);
+	retval = roccat_common2_send_with_status(usb_dev, command,
+			buf, real_size);
 	mutex_unlock(&koneplus->koneplus_lock);
 
 	if (retval)
diff --git a/drivers/hid/hid-roccat-koneplus.h b/drivers/hid/hid-roccat-koneplus.h
index c03332a4fa9..7074b2a4b94 100644
--- a/drivers/hid/hid-roccat-koneplus.h
+++ b/drivers/hid/hid-roccat-koneplus.h
@@ -20,32 +20,11 @@ struct koneplus_talk {
 	uint8_t data[14];
 } __packed;
 
-/*
- * case 1: writes request 80 and reads value 1
- *
- */
-struct koneplus_control {
-	uint8_t command; /* KONEPLUS_COMMAND_CONTROL */
-	/*
-	 * value is profile number in range 0-4 for requesting settings and buttons
-	 * 1 if status ok for requesting status
-	 */
-	uint8_t value;
-	uint8_t request;
-} __attribute__ ((__packed__));
-
 enum koneplus_control_requests {
-	KONEPLUS_CONTROL_REQUEST_STATUS = 0x00,
 	KONEPLUS_CONTROL_REQUEST_PROFILE_SETTINGS = 0x80,
 	KONEPLUS_CONTROL_REQUEST_PROFILE_BUTTONS = 0x90,
 };
 
-enum koneplus_control_values {
-	KONEPLUS_CONTROL_REQUEST_STATUS_OVERLOAD = 0,
-	KONEPLUS_CONTROL_REQUEST_STATUS_OK = 1,
-	KONEPLUS_CONTROL_REQUEST_STATUS_WAIT = 3,
-};
-
 struct koneplus_actual_profile {
 	uint8_t command; /* KONEPLUS_COMMAND_ACTUAL_PROFILE */
 	uint8_t size; /* always 3 */
@@ -137,7 +116,6 @@ struct koneplus_tcu_image {
 } __attribute__ ((__packed__));
 
 enum koneplus_commands {
-	KONEPLUS_COMMAND_CONTROL = 0x4,
 	KONEPLUS_COMMAND_ACTUAL_PROFILE = 0x5,
 	KONEPLUS_COMMAND_PROFILE_SETTINGS = 0x6,
 	KONEPLUS_COMMAND_PROFILE_BUTTONS = 0x7,
diff --git a/drivers/hid/hid-roccat-kovaplus.c b/drivers/hid/hid-roccat-kovaplus.c
index 112d934132c..ca6527ac655 100644
--- a/drivers/hid/hid-roccat-kovaplus.c
+++ b/drivers/hid/hid-roccat-kovaplus.c
@@ -47,69 +47,23 @@ static int kovaplus_send_control(struct usb_device *usb_dev, uint value,
 		enum kovaplus_control_requests request)
 {
 	int retval;
-	struct kovaplus_control control;
+	struct roccat_common2_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.command = ROCCAT_COMMON_COMMAND_CONTROL;
 	control.value = value;
 	control.request = request;
 
-	retval = roccat_common_send(usb_dev, KOVAPLUS_COMMAND_CONTROL,
-			&control, sizeof(struct kovaplus_control));
+	retval = roccat_common2_send(usb_dev, ROCCAT_COMMON_COMMAND_CONTROL,
+			&control, sizeof(struct roccat_common2_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_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, "roccat_common_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)
 {
@@ -119,7 +73,7 @@ static int kovaplus_select_profile(struct usb_device *usb_dev, uint number,
 static int kovaplus_get_info(struct usb_device *usb_dev,
 		struct kovaplus_info *buf)
 {
-	return roccat_common_receive(usb_dev, KOVAPLUS_COMMAND_INFO,
+	return roccat_common2_receive(usb_dev, KOVAPLUS_COMMAND_INFO,
 			buf, sizeof(struct kovaplus_info));
 }
 
@@ -133,14 +87,15 @@ static int kovaplus_get_profile_settings(struct usb_device *usb_dev,
 	if (retval)
 		return retval;
 
-	return roccat_common_receive(usb_dev, KOVAPLUS_COMMAND_PROFILE_SETTINGS,
+	return roccat_common2_receive(usb_dev, KOVAPLUS_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_COMMAND_PROFILE_SETTINGS,
+	return roccat_common2_send_with_status(usb_dev,
+			KOVAPLUS_COMMAND_PROFILE_SETTINGS,
 			settings, sizeof(struct kovaplus_profile_settings));
 }
 
@@ -154,14 +109,15 @@ static int kovaplus_get_profile_buttons(struct usb_device *usb_dev,
 	if (retval)
 		return retval;
 
-	return roccat_common_receive(usb_dev, KOVAPLUS_COMMAND_PROFILE_BUTTONS,
+	return roccat_common2_receive(usb_dev, KOVAPLUS_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_COMMAND_PROFILE_BUTTONS,
+	return roccat_common2_send_with_status(usb_dev,
+			KOVAPLUS_COMMAND_PROFILE_BUTTONS,
 			buttons, sizeof(struct kovaplus_profile_buttons));
 }
 
@@ -171,7 +127,7 @@ 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_COMMAND_ACTUAL_PROFILE,
+	retval = roccat_common2_receive(usb_dev, KOVAPLUS_COMMAND_ACTUAL_PROFILE,
 			&buf, sizeof(struct kovaplus_actual_profile));
 
 	return retval ? retval : buf.actual_profile;
@@ -186,7 +142,8 @@ static int kovaplus_set_actual_profile(struct usb_device *usb_dev,
 	buf.size = sizeof(struct kovaplus_actual_profile);
 	buf.actual_profile = new_profile;
 
-	return kovaplus_send(usb_dev, KOVAPLUS_COMMAND_ACTUAL_PROFILE,
+	return roccat_common2_send_with_status(usb_dev,
+			KOVAPLUS_COMMAND_ACTUAL_PROFILE,
 			&buf, sizeof(struct kovaplus_actual_profile));
 }
 
diff --git a/drivers/hid/hid-roccat-kovaplus.h b/drivers/hid/hid-roccat-kovaplus.h
index fb2aed44a8e..f82daa1cdcb 100644
--- a/drivers/hid/hid-roccat-kovaplus.h
+++ b/drivers/hid/hid-roccat-kovaplus.h
@@ -14,27 +14,13 @@
 
 #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 */
@@ -75,7 +61,6 @@ struct kovaplus_a {
 } __packed;
 
 enum kovaplus_commands {
-	KOVAPLUS_COMMAND_CONTROL = 0x4,
 	KOVAPLUS_COMMAND_ACTUAL_PROFILE = 0x5,
 	KOVAPLUS_COMMAND_PROFILE_SETTINGS = 0x6,
 	KOVAPLUS_COMMAND_PROFILE_BUTTONS = 0x7,
diff --git a/drivers/hid/hid-roccat-pyra.c b/drivers/hid/hid-roccat-pyra.c
index df05c1b1064..1317c177a3e 100644
--- a/drivers/hid/hid-roccat-pyra.c
+++ b/drivers/hid/hid-roccat-pyra.c
@@ -42,43 +42,19 @@ 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)
 {
-	struct pyra_control control;
+	struct roccat_common2_control control;
 
 	if ((request == PYRA_CONTROL_REQUEST_PROFILE_SETTINGS ||
 			request == PYRA_CONTROL_REQUEST_PROFILE_BUTTONS) &&
 			(value < 0 || value > 4))
 		return -EINVAL;
 
-	control.command = PYRA_COMMAND_CONTROL;
+	control.command = ROCCAT_COMMON_COMMAND_CONTROL;
 	control.value = value;
 	control.request = request;
 
-	return roccat_common_send(usb_dev, PYRA_COMMAND_CONTROL,
-			&control, sizeof(struct pyra_control));
-}
-
-static int pyra_receive_control_status(struct usb_device *usb_dev)
-{
-	int retval;
-	struct pyra_control control;
-
-	do {
-		msleep(10);
-		retval = roccat_common_receive(usb_dev, PYRA_COMMAND_CONTROL,
-				&control, sizeof(struct pyra_control));
-
-		/* requested too early, try again */
-	} while (retval == -EPROTO);
-
-	if (!retval && control.command == PYRA_COMMAND_CONTROL &&
-			control.request == PYRA_CONTROL_REQUEST_STATUS &&
-			control.value == 1)
-		return 0;
-	else {
-		hid_err(usb_dev, "receive control status: unknown response 0x%x 0x%x\n",
-			control.request, control.value);
-		return retval ? retval : -EINVAL;
-	}
+	return roccat_common2_send(usb_dev, ROCCAT_COMMON_COMMAND_CONTROL,
+			&control, sizeof(struct roccat_common2_control));
 }
 
 static int pyra_get_profile_settings(struct usb_device *usb_dev,
@@ -89,7 +65,7 @@ static int pyra_get_profile_settings(struct usb_device *usb_dev,
 			PYRA_CONTROL_REQUEST_PROFILE_SETTINGS);
 	if (retval)
 		return retval;
-	return roccat_common_receive(usb_dev, PYRA_COMMAND_PROFILE_SETTINGS,
+	return roccat_common2_receive(usb_dev, PYRA_COMMAND_PROFILE_SETTINGS,
 			buf, sizeof(struct pyra_profile_settings));
 }
 
@@ -101,51 +77,44 @@ static int pyra_get_profile_buttons(struct usb_device *usb_dev,
 			PYRA_CONTROL_REQUEST_PROFILE_BUTTONS);
 	if (retval)
 		return retval;
-	return roccat_common_receive(usb_dev, PYRA_COMMAND_PROFILE_BUTTONS,
+	return roccat_common2_receive(usb_dev, PYRA_COMMAND_PROFILE_BUTTONS,
 			buf, sizeof(struct pyra_profile_buttons));
 }
 
 static int pyra_get_settings(struct usb_device *usb_dev,
 		struct pyra_settings *buf)
 {
-	return roccat_common_receive(usb_dev, PYRA_COMMAND_SETTINGS,
+	return roccat_common2_receive(usb_dev, PYRA_COMMAND_SETTINGS,
 			buf, sizeof(struct pyra_settings));
 }
 
 static int pyra_get_info(struct usb_device *usb_dev, struct pyra_info *buf)
 {
-	return roccat_common_receive(usb_dev, PYRA_COMMAND_INFO,
+	return roccat_common2_receive(usb_dev, PYRA_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)
 {
-	return pyra_send(usb_dev, PYRA_COMMAND_PROFILE_SETTINGS, settings,
+	return roccat_common2_send_with_status(usb_dev,
+			PYRA_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)
 {
-	return pyra_send(usb_dev, PYRA_COMMAND_PROFILE_BUTTONS, buttons,
+	return roccat_common2_send_with_status(usb_dev,
+			PYRA_COMMAND_PROFILE_BUTTONS, buttons,
 			sizeof(struct pyra_profile_buttons));
 }
 
 static int pyra_set_settings(struct usb_device *usb_dev,
 		struct pyra_settings const *settings)
 {
-	return pyra_send(usb_dev, PYRA_COMMAND_SETTINGS, settings,
+	return roccat_common2_send_with_status(usb_dev,
+			PYRA_COMMAND_SETTINGS, settings,
 			sizeof(struct pyra_settings));
 }
 
diff --git a/drivers/hid/hid-roccat-pyra.h b/drivers/hid/hid-roccat-pyra.h
index 0442d7fa2dc..eada7830fa9 100644
--- a/drivers/hid/hid-roccat-pyra.h
+++ b/drivers/hid/hid-roccat-pyra.h
@@ -20,18 +20,7 @@ struct pyra_b {
 	uint8_t unknown; /* 1 */
 } __attribute__ ((__packed__));
 
-struct pyra_control {
-	uint8_t command; /* PYRA_COMMAND_CONTROL */
-	/*
-	 * value is profile number for request_settings and request_buttons
-	 * 1 if status ok for request_status
-	 */
-	uint8_t value; /* Range 0-4 */
-	uint8_t request;
-} __attribute__ ((__packed__));
-
 enum pyra_control_requests {
-	PYRA_CONTROL_REQUEST_STATUS = 0x00,
 	PYRA_CONTROL_REQUEST_PROFILE_SETTINGS = 0x10,
 	PYRA_CONTROL_REQUEST_PROFILE_BUTTONS = 0x20
 };
@@ -75,7 +64,6 @@ struct pyra_info {
 } __attribute__ ((__packed__));
 
 enum pyra_commands {
-	PYRA_COMMAND_CONTROL = 0x4,
 	PYRA_COMMAND_SETTINGS = 0x5,
 	PYRA_COMMAND_PROFILE_SETTINGS = 0x6,
 	PYRA_COMMAND_PROFILE_BUTTONS = 0x7,
diff --git a/drivers/hid/hid-roccat-savu.c b/drivers/hid/hid-roccat-savu.c
new file mode 100644
index 00000000000..014afba407e
--- /dev/null
+++ b/drivers/hid/hid-roccat-savu.c
@@ -0,0 +1,316 @@
+/*
+ * Roccat Savu driver for Linux
+ *
+ * Copyright (c) 2012 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 Savu is a gamer mouse with 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-savu.h"
+
+static struct class *savu_class;
+
+static ssize_t savu_sysfs_read(struct file *fp, struct kobject *kobj,
+		char *buf, loff_t off, size_t count,
+		size_t real_size, uint command)
+{
+	struct device *dev =
+			container_of(kobj, struct device, kobj)->parent->parent;
+	struct savu_device *savu = 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(&savu->savu_lock);
+	retval = roccat_common2_receive(usb_dev, command, buf, real_size);
+	mutex_unlock(&savu->savu_lock);
+
+	return retval ? retval : real_size;
+}
+
+static ssize_t savu_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 savu_device *savu = 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(&savu->savu_lock);
+	retval = roccat_common2_send_with_status(usb_dev, command,
+			(void *)buf, real_size);
+	mutex_unlock(&savu->savu_lock);
+
+	return retval ? retval : real_size;
+}
+
+#define SAVU_SYSFS_W(thingy, THINGY) \
+static ssize_t savu_sysfs_write_ ## thingy(struct file *fp, \
+		struct kobject *kobj, struct bin_attribute *attr, char *buf, \
+		loff_t off, size_t count) \
+{ \
+	return savu_sysfs_write(fp, kobj, buf, off, count, \
+			SAVU_SIZE_ ## THINGY, SAVU_COMMAND_ ## THINGY); \
+}
+
+#define SAVU_SYSFS_R(thingy, THINGY) \
+static ssize_t savu_sysfs_read_ ## thingy(struct file *fp, \
+		struct kobject *kobj, struct bin_attribute *attr, char *buf, \
+		loff_t off, size_t count) \
+{ \
+	return savu_sysfs_read(fp, kobj, buf, off, count, \
+			SAVU_SIZE_ ## THINGY, SAVU_COMMAND_ ## THINGY); \
+}
+
+#define SAVU_SYSFS_RW(thingy, THINGY) \
+SAVU_SYSFS_W(thingy, THINGY) \
+SAVU_SYSFS_R(thingy, THINGY)
+
+#define SAVU_BIN_ATTRIBUTE_RW(thingy, THINGY) \
+{ \
+	.attr = { .name = #thingy, .mode = 0660 }, \
+	.size = SAVU_SIZE_ ## THINGY, \
+	.read = savu_sysfs_read_ ## thingy, \
+	.write = savu_sysfs_write_ ## thingy \
+}
+
+#define SAVU_BIN_ATTRIBUTE_R(thingy, THINGY) \
+{ \
+	.attr = { .name = #thingy, .mode = 0440 }, \
+	.size = SAVU_SIZE_ ## THINGY, \
+	.read = savu_sysfs_read_ ## thingy, \
+}
+
+#define SAVU_BIN_ATTRIBUTE_W(thingy, THINGY) \
+{ \
+	.attr = { .name = #thingy, .mode = 0220 }, \
+	.size = SAVU_SIZE_ ## THINGY, \
+	.write = savu_sysfs_write_ ## thingy \
+}
+
+SAVU_SYSFS_W(control, CONTROL)
+SAVU_SYSFS_RW(profile, PROFILE)
+SAVU_SYSFS_RW(general, GENERAL)
+SAVU_SYSFS_RW(buttons, BUTTONS)
+SAVU_SYSFS_RW(macro, MACRO)
+SAVU_SYSFS_R(info, INFO)
+SAVU_SYSFS_RW(sensor, SENSOR)
+
+static struct bin_attribute savu_bin_attributes[] = {
+	SAVU_BIN_ATTRIBUTE_W(control, CONTROL),
+	SAVU_BIN_ATTRIBUTE_RW(profile, PROFILE),
+	SAVU_BIN_ATTRIBUTE_RW(general, GENERAL),
+	SAVU_BIN_ATTRIBUTE_RW(buttons, BUTTONS),
+	SAVU_BIN_ATTRIBUTE_RW(macro, MACRO),
+	SAVU_BIN_ATTRIBUTE_R(info, INFO),
+	SAVU_BIN_ATTRIBUTE_RW(sensor, SENSOR),
+	__ATTR_NULL
+};
+
+static int savu_init_savu_device_struct(struct usb_device *usb_dev,
+		struct savu_device *savu)
+{
+	mutex_init(&savu->savu_lock);
+
+	return 0;
+}
+
+static int savu_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 savu_device *savu;
+	int retval;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE) {
+		hid_set_drvdata(hdev, NULL);
+		return 0;
+	}
+
+	savu = kzalloc(sizeof(*savu), GFP_KERNEL);
+	if (!savu) {
+		hid_err(hdev, "can't alloc device descriptor\n");
+		return -ENOMEM;
+	}
+	hid_set_drvdata(hdev, savu);
+
+	retval = savu_init_savu_device_struct(usb_dev, savu);
+	if (retval) {
+		hid_err(hdev, "couldn't init struct savu_device\n");
+		goto exit_free;
+	}
+
+	retval = roccat_connect(savu_class, hdev,
+			sizeof(struct savu_roccat_report));
+	if (retval < 0) {
+		hid_err(hdev, "couldn't init char dev\n");
+	} else {
+		savu->chrdev_minor = retval;
+		savu->roccat_claimed = 1;
+	}
+
+	return 0;
+exit_free:
+	kfree(savu);
+	return retval;
+}
+
+static void savu_remove_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct savu_device *savu;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE)
+		return;
+
+	savu = hid_get_drvdata(hdev);
+	if (savu->roccat_claimed)
+		roccat_disconnect(savu->chrdev_minor);
+	kfree(savu);
+}
+
+static int savu_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 = savu_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 savu_remove(struct hid_device *hdev)
+{
+	savu_remove_specials(hdev);
+	hid_hw_stop(hdev);
+}
+
+static void savu_report_to_chrdev(struct savu_device const *savu,
+		u8 const *data)
+{
+	struct savu_roccat_report roccat_report;
+	struct savu_mouse_report_special const *special_report;
+
+	if (data[0] != SAVU_MOUSE_REPORT_NUMBER_SPECIAL)
+		return;
+
+	special_report = (struct savu_mouse_report_special const *)data;
+
+	roccat_report.type = special_report->type;
+	roccat_report.data[0] = special_report->data[0];
+	roccat_report.data[1] = special_report->data[1];
+	roccat_report_event(savu->chrdev_minor,
+			(uint8_t const *)&roccat_report);
+}
+
+static int savu_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 savu_device *savu = hid_get_drvdata(hdev);
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= USB_INTERFACE_PROTOCOL_MOUSE)
+		return 0;
+
+	if (savu == NULL)
+		return 0;
+
+	if (savu->roccat_claimed)
+		savu_report_to_chrdev(savu, data);
+
+	return 0;
+}
+
+static const struct hid_device_id savu_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_SAVU) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, savu_devices);
+
+static struct hid_driver savu_driver = {
+		.name = "savu",
+		.id_table = savu_devices,
+		.probe = savu_probe,
+		.remove = savu_remove,
+		.raw_event = savu_raw_event
+};
+
+static int __init savu_init(void)
+{
+	int retval;
+
+	savu_class = class_create(THIS_MODULE, "savu");
+	if (IS_ERR(savu_class))
+		return PTR_ERR(savu_class);
+	savu_class->dev_bin_attrs = savu_bin_attributes;
+
+	retval = hid_register_driver(&savu_driver);
+	if (retval)
+		class_destroy(savu_class);
+	return retval;
+}
+
+static void __exit savu_exit(void)
+{
+	hid_unregister_driver(&savu_driver);
+	class_destroy(savu_class);
+}
+
+module_init(savu_init);
+module_exit(savu_exit);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat Savu driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-savu.h b/drivers/hid/hid-roccat-savu.h
new file mode 100644
index 00000000000..9120ba72087
--- /dev/null
+++ b/drivers/hid/hid-roccat-savu.h
@@ -0,0 +1,87 @@
+#ifndef __HID_ROCCAT_SAVU_H
+#define __HID_ROCCAT_SAVU_H
+
+/*
+ * Copyright (c) 2012 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>
+
+enum {
+	SAVU_SIZE_CONTROL = 0x03,
+	SAVU_SIZE_PROFILE = 0x03,
+	SAVU_SIZE_GENERAL = 0x10,
+	SAVU_SIZE_BUTTONS = 0x2f,
+	SAVU_SIZE_MACRO = 0x0823,
+	SAVU_SIZE_INFO = 0x08,
+	SAVU_SIZE_SENSOR = 0x04,
+};
+
+enum savu_control_requests {
+	SAVU_CONTROL_REQUEST_GENERAL = 0x80,
+	SAVU_CONTROL_REQUEST_BUTTONS = 0x90,
+};
+
+enum savu_commands {
+	SAVU_COMMAND_CONTROL = 0x4,
+	SAVU_COMMAND_PROFILE = 0x5,
+	SAVU_COMMAND_GENERAL = 0x6,
+	SAVU_COMMAND_BUTTONS = 0x7,
+	SAVU_COMMAND_MACRO = 0x8,
+	SAVU_COMMAND_INFO = 0x9,
+	SAVU_COMMAND_SENSOR = 0xc,
+};
+
+struct savu_mouse_report_special {
+	uint8_t report_number; /* always 3 */
+	uint8_t zero;
+	uint8_t type;
+	uint8_t data[2];
+} __packed;
+
+enum {
+	SAVU_MOUSE_REPORT_NUMBER_SPECIAL = 3,
+};
+
+enum savu_mouse_report_button_types {
+	/* data1 = new profile range 1-5 */
+	SAVU_MOUSE_REPORT_BUTTON_TYPE_PROFILE = 0x20,
+
+	/* data1 = button number range 1-24; data2 = action */
+	SAVU_MOUSE_REPORT_BUTTON_TYPE_QUICKLAUNCH = 0x60,
+
+	/* data1 = button number range 1-24; data2 = action */
+	SAVU_MOUSE_REPORT_BUTTON_TYPE_TIMER = 0x80,
+
+	/* data1 = setting number range 1-5 */
+	SAVU_MOUSE_REPORT_BUTTON_TYPE_CPI = 0xb0,
+
+	/* data1 and data2 = range 0x1-0xb */
+	SAVU_MOUSE_REPORT_BUTTON_TYPE_SENSITIVITY = 0xc0,
+
+	/* data1 = 22 = next track...
+	 * data2 = action
+	 */
+	SAVU_MOUSE_REPORT_BUTTON_TYPE_MULTIMEDIA = 0xf0,
+};
+
+struct savu_roccat_report {
+	uint8_t type;
+	uint8_t data[2];
+} __packed;
+
+struct savu_device {
+	int roccat_claimed;
+	int chrdev_minor;
+
+	struct mutex savu_lock;
+};
+
+#endif
diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
index aa958706c0e..0a1805c9b0e 100644
--- a/drivers/hid/hid-wiimote-ext.c
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -77,7 +77,7 @@ static __u16 wiiext_keymap[] = {
 	BTN_TR,		/* WIIEXT_KEY_RT */
 };
 
-/* diable all extensions */
+/* disable all extensions */
 static void ext_disable(struct wiimote_ext *ext)
 {
 	unsigned long flags;
diff --git a/drivers/hid/hidraw.c b/drivers/hid/hidraw.c
index 36fa77b40ff..3b6f7bf5a77 100644
--- a/drivers/hid/hidraw.c
+++ b/drivers/hid/hidraw.c
@@ -96,6 +96,7 @@ static ssize_t hidraw_read(struct file *file, char __user *buffer, size_t count,
 		}
 
 		kfree(list->buffer[list->tail].value);
+		list->buffer[list->tail].value = NULL;
 		list->tail = (list->tail + 1) & (HIDRAW_BUFFER_SIZE - 1);
 	}
 out:
@@ -300,6 +301,7 @@ static int hidraw_release(struct inode * inode, struct file * file)
 	struct hidraw *dev;
 	struct hidraw_list *list = file->private_data;
 	int ret;
+	int i;
 
 	mutex_lock(&minors_lock);
 	if (!hidraw_table[minor]) {
@@ -317,6 +319,9 @@ static int hidraw_release(struct inode * inode, struct file * file)
 			kfree(list->hidraw);
 		}
 	}
+
+	for (i = 0; i < HIDRAW_BUFFER_SIZE; ++i)
+		kfree(list->buffer[i].value);
 	kfree(list);
 	ret = 0;
 unlock:
@@ -446,12 +451,17 @@ int hidraw_report_event(struct hid_device *hid, u8 *data, int len)
 	int ret = 0;
 
 	list_for_each_entry(list, &dev->list, node) {
+		int new_head = (list->head + 1) & (HIDRAW_BUFFER_SIZE - 1);
+
+		if (new_head == list->tail)
+			continue;
+
 		if (!(list->buffer[list->head].value = kmemdup(data, len, GFP_ATOMIC))) {
 			ret = -ENOMEM;
 			break;
 		}
 		list->buffer[list->head].len = len;
-		list->head = (list->head + 1) & (HIDRAW_BUFFER_SIZE - 1);
+		list->head = new_head;
 		kill_fasync(&list->fasync, SIGIO, POLL_IN);
 	}
 
diff --git a/drivers/hid/uhid.c b/drivers/hid/uhid.c
new file mode 100644
index 00000000000..714cd8cc957
--- /dev/null
+++ b/drivers/hid/uhid.c
@@ -0,0 +1,572 @@
+/*
+ * User-space I/O driver support for HID subsystem
+ * Copyright (c) 2012 David Herrmann
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/atomic.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/hid.h>
+#include <linux/input.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/uhid.h>
+#include <linux/wait.h>
+
+#define UHID_NAME	"uhid"
+#define UHID_BUFSIZE	32
+
+struct uhid_device {
+	struct mutex devlock;
+	bool running;
+
+	__u8 *rd_data;
+	uint rd_size;
+
+	struct hid_device *hid;
+	struct uhid_event input_buf;
+
+	wait_queue_head_t waitq;
+	spinlock_t qlock;
+	__u8 head;
+	__u8 tail;
+	struct uhid_event *outq[UHID_BUFSIZE];
+
+	struct mutex report_lock;
+	wait_queue_head_t report_wait;
+	atomic_t report_done;
+	atomic_t report_id;
+	struct uhid_event report_buf;
+};
+
+static struct miscdevice uhid_misc;
+
+static void uhid_queue(struct uhid_device *uhid, struct uhid_event *ev)
+{
+	__u8 newhead;
+
+	newhead = (uhid->head + 1) % UHID_BUFSIZE;
+
+	if (newhead != uhid->tail) {
+		uhid->outq[uhid->head] = ev;
+		uhid->head = newhead;
+		wake_up_interruptible(&uhid->waitq);
+	} else {
+		hid_warn(uhid->hid, "Output queue is full\n");
+		kfree(ev);
+	}
+}
+
+static int uhid_queue_event(struct uhid_device *uhid, __u32 event)
+{
+	unsigned long flags;
+	struct uhid_event *ev;
+
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->type = event;
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+	uhid_queue(uhid, ev);
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+
+	return 0;
+}
+
+static int uhid_hid_start(struct hid_device *hid)
+{
+	struct uhid_device *uhid = hid->driver_data;
+
+	return uhid_queue_event(uhid, UHID_START);
+}
+
+static void uhid_hid_stop(struct hid_device *hid)
+{
+	struct uhid_device *uhid = hid->driver_data;
+
+	hid->claimed = 0;
+	uhid_queue_event(uhid, UHID_STOP);
+}
+
+static int uhid_hid_open(struct hid_device *hid)
+{
+	struct uhid_device *uhid = hid->driver_data;
+
+	return uhid_queue_event(uhid, UHID_OPEN);
+}
+
+static void uhid_hid_close(struct hid_device *hid)
+{
+	struct uhid_device *uhid = hid->driver_data;
+
+	uhid_queue_event(uhid, UHID_CLOSE);
+}
+
+static int uhid_hid_input(struct input_dev *input, unsigned int type,
+			  unsigned int code, int value)
+{
+	struct hid_device *hid = input_get_drvdata(input);
+	struct uhid_device *uhid = hid->driver_data;
+	unsigned long flags;
+	struct uhid_event *ev;
+
+	ev = kzalloc(sizeof(*ev), GFP_ATOMIC);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->type = UHID_OUTPUT_EV;
+	ev->u.output_ev.type = type;
+	ev->u.output_ev.code = code;
+	ev->u.output_ev.value = value;
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+	uhid_queue(uhid, ev);
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+
+	return 0;
+}
+
+static int uhid_hid_parse(struct hid_device *hid)
+{
+	struct uhid_device *uhid = hid->driver_data;
+
+	return hid_parse_report(hid, uhid->rd_data, uhid->rd_size);
+}
+
+static int uhid_hid_get_raw(struct hid_device *hid, unsigned char rnum,
+			    __u8 *buf, size_t count, unsigned char rtype)
+{
+	struct uhid_device *uhid = hid->driver_data;
+	__u8 report_type;
+	struct uhid_event *ev;
+	unsigned long flags;
+	int ret;
+	size_t uninitialized_var(len);
+	struct uhid_feature_answer_req *req;
+
+	if (!uhid->running)
+		return -EIO;
+
+	switch (rtype) {
+	case HID_FEATURE_REPORT:
+		report_type = UHID_FEATURE_REPORT;
+		break;
+	case HID_OUTPUT_REPORT:
+		report_type = UHID_OUTPUT_REPORT;
+		break;
+	case HID_INPUT_REPORT:
+		report_type = UHID_INPUT_REPORT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = mutex_lock_interruptible(&uhid->report_lock);
+	if (ret)
+		return ret;
+
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+	ev->type = UHID_FEATURE;
+	ev->u.feature.id = atomic_inc_return(&uhid->report_id);
+	ev->u.feature.rnum = rnum;
+	ev->u.feature.rtype = report_type;
+
+	atomic_set(&uhid->report_done, 0);
+	uhid_queue(uhid, ev);
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+
+	ret = wait_event_interruptible_timeout(uhid->report_wait,
+				atomic_read(&uhid->report_done), 5 * HZ);
+
+	/*
+	 * Make sure "uhid->running" is cleared on shutdown before
+	 * "uhid->report_done" is set.
+	 */
+	smp_rmb();
+	if (!ret || !uhid->running) {
+		ret = -EIO;
+	} else if (ret < 0) {
+		ret = -ERESTARTSYS;
+	} else {
+		spin_lock_irqsave(&uhid->qlock, flags);
+		req = &uhid->report_buf.u.feature_answer;
+
+		if (req->err) {
+			ret = -EIO;
+		} else {
+			ret = 0;
+			len = min(count,
+				min_t(size_t, req->size, UHID_DATA_MAX));
+			memcpy(buf, req->data, len);
+		}
+
+		spin_unlock_irqrestore(&uhid->qlock, flags);
+	}
+
+	atomic_set(&uhid->report_done, 1);
+
+unlock:
+	mutex_unlock(&uhid->report_lock);
+	return ret ? ret : len;
+}
+
+static int uhid_hid_output_raw(struct hid_device *hid, __u8 *buf, size_t count,
+			       unsigned char report_type)
+{
+	struct uhid_device *uhid = hid->driver_data;
+	__u8 rtype;
+	unsigned long flags;
+	struct uhid_event *ev;
+
+	switch (report_type) {
+	case HID_FEATURE_REPORT:
+		rtype = UHID_FEATURE_REPORT;
+		break;
+	case HID_OUTPUT_REPORT:
+		rtype = UHID_OUTPUT_REPORT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (count < 1 || count > UHID_DATA_MAX)
+		return -EINVAL;
+
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->type = UHID_OUTPUT;
+	ev->u.output.size = count;
+	ev->u.output.rtype = rtype;
+	memcpy(ev->u.output.data, buf, count);
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+	uhid_queue(uhid, ev);
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+
+	return count;
+}
+
+static struct hid_ll_driver uhid_hid_driver = {
+	.start = uhid_hid_start,
+	.stop = uhid_hid_stop,
+	.open = uhid_hid_open,
+	.close = uhid_hid_close,
+	.hidinput_input_event = uhid_hid_input,
+	.parse = uhid_hid_parse,
+};
+
+static int uhid_dev_create(struct uhid_device *uhid,
+			   const struct uhid_event *ev)
+{
+	struct hid_device *hid;
+	int ret;
+
+	if (uhid->running)
+		return -EALREADY;
+
+	uhid->rd_size = ev->u.create.rd_size;
+	if (uhid->rd_size <= 0 || uhid->rd_size > HID_MAX_DESCRIPTOR_SIZE)
+		return -EINVAL;
+
+	uhid->rd_data = kmalloc(uhid->rd_size, GFP_KERNEL);
+	if (!uhid->rd_data)
+		return -ENOMEM;
+
+	if (copy_from_user(uhid->rd_data, ev->u.create.rd_data,
+			   uhid->rd_size)) {
+		ret = -EFAULT;
+		goto err_free;
+	}
+
+	hid = hid_allocate_device();
+	if (IS_ERR(hid)) {
+		ret = PTR_ERR(hid);
+		goto err_free;
+	}
+
+	strncpy(hid->name, ev->u.create.name, 127);
+	hid->name[127] = 0;
+	strncpy(hid->phys, ev->u.create.phys, 63);
+	hid->phys[63] = 0;
+	strncpy(hid->uniq, ev->u.create.uniq, 63);
+	hid->uniq[63] = 0;
+
+	hid->ll_driver = &uhid_hid_driver;
+	hid->hid_get_raw_report = uhid_hid_get_raw;
+	hid->hid_output_raw_report = uhid_hid_output_raw;
+	hid->bus = ev->u.create.bus;
+	hid->vendor = ev->u.create.vendor;
+	hid->product = ev->u.create.product;
+	hid->version = ev->u.create.version;
+	hid->country = ev->u.create.country;
+	hid->driver_data = uhid;
+	hid->dev.parent = uhid_misc.this_device;
+
+	uhid->hid = hid;
+	uhid->running = true;
+
+	ret = hid_add_device(hid);
+	if (ret) {
+		hid_err(hid, "Cannot register HID device\n");
+		goto err_hid;
+	}
+
+	return 0;
+
+err_hid:
+	hid_destroy_device(hid);
+	uhid->hid = NULL;
+	uhid->running = false;
+err_free:
+	kfree(uhid->rd_data);
+	return ret;
+}
+
+static int uhid_dev_destroy(struct uhid_device *uhid)
+{
+	if (!uhid->running)
+		return -EINVAL;
+
+	/* clear "running" before setting "report_done" */
+	uhid->running = false;
+	smp_wmb();
+	atomic_set(&uhid->report_done, 1);
+	wake_up_interruptible(&uhid->report_wait);
+
+	hid_destroy_device(uhid->hid);
+	kfree(uhid->rd_data);
+
+	return 0;
+}
+
+static int uhid_dev_input(struct uhid_device *uhid, struct uhid_event *ev)
+{
+	if (!uhid->running)
+		return -EINVAL;
+
+	hid_input_report(uhid->hid, HID_INPUT_REPORT, ev->u.input.data,
+			 min_t(size_t, ev->u.input.size, UHID_DATA_MAX), 0);
+
+	return 0;
+}
+
+static int uhid_dev_feature_answer(struct uhid_device *uhid,
+				   struct uhid_event *ev)
+{
+	unsigned long flags;
+
+	if (!uhid->running)
+		return -EINVAL;
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+
+	/* id for old report; drop it silently */
+	if (atomic_read(&uhid->report_id) != ev->u.feature_answer.id)
+		goto unlock;
+	if (atomic_read(&uhid->report_done))
+		goto unlock;
+
+	memcpy(&uhid->report_buf, ev, sizeof(*ev));
+	atomic_set(&uhid->report_done, 1);
+	wake_up_interruptible(&uhid->report_wait);
+
+unlock:
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+	return 0;
+}
+
+static int uhid_char_open(struct inode *inode, struct file *file)
+{
+	struct uhid_device *uhid;
+
+	uhid = kzalloc(sizeof(*uhid), GFP_KERNEL);
+	if (!uhid)
+		return -ENOMEM;
+
+	mutex_init(&uhid->devlock);
+	mutex_init(&uhid->report_lock);
+	spin_lock_init(&uhid->qlock);
+	init_waitqueue_head(&uhid->waitq);
+	init_waitqueue_head(&uhid->report_wait);
+	uhid->running = false;
+	atomic_set(&uhid->report_done, 1);
+
+	file->private_data = uhid;
+	nonseekable_open(inode, file);
+
+	return 0;
+}
+
+static int uhid_char_release(struct inode *inode, struct file *file)
+{
+	struct uhid_device *uhid = file->private_data;
+	unsigned int i;
+
+	uhid_dev_destroy(uhid);
+
+	for (i = 0; i < UHID_BUFSIZE; ++i)
+		kfree(uhid->outq[i]);
+
+	kfree(uhid);
+
+	return 0;
+}
+
+static ssize_t uhid_char_read(struct file *file, char __user *buffer,
+				size_t count, loff_t *ppos)
+{
+	struct uhid_device *uhid = file->private_data;
+	int ret;
+	unsigned long flags;
+	size_t len;
+
+	/* they need at least the "type" member of uhid_event */
+	if (count < sizeof(__u32))
+		return -EINVAL;
+
+try_again:
+	if (file->f_flags & O_NONBLOCK) {
+		if (uhid->head == uhid->tail)
+			return -EAGAIN;
+	} else {
+		ret = wait_event_interruptible(uhid->waitq,
+						uhid->head != uhid->tail);
+		if (ret)
+			return ret;
+	}
+
+	ret = mutex_lock_interruptible(&uhid->devlock);
+	if (ret)
+		return ret;
+
+	if (uhid->head == uhid->tail) {
+		mutex_unlock(&uhid->devlock);
+		goto try_again;
+	} else {
+		len = min(count, sizeof(**uhid->outq));
+		if (copy_to_user(buffer, uhid->outq[uhid->tail], len)) {
+			ret = -EFAULT;
+		} else {
+			kfree(uhid->outq[uhid->tail]);
+			uhid->outq[uhid->tail] = NULL;
+
+			spin_lock_irqsave(&uhid->qlock, flags);
+			uhid->tail = (uhid->tail + 1) % UHID_BUFSIZE;
+			spin_unlock_irqrestore(&uhid->qlock, flags);
+		}
+	}
+
+	mutex_unlock(&uhid->devlock);
+	return ret ? ret : len;
+}
+
+static ssize_t uhid_char_write(struct file *file, const char __user *buffer,
+				size_t count, loff_t *ppos)
+{
+	struct uhid_device *uhid = file->private_data;
+	int ret;
+	size_t len;
+
+	/* we need at least the "type" member of uhid_event */
+	if (count < sizeof(__u32))
+		return -EINVAL;
+
+	ret = mutex_lock_interruptible(&uhid->devlock);
+	if (ret)
+		return ret;
+
+	memset(&uhid->input_buf, 0, sizeof(uhid->input_buf));
+	len = min(count, sizeof(uhid->input_buf));
+	if (copy_from_user(&uhid->input_buf, buffer, len)) {
+		ret = -EFAULT;
+		goto unlock;
+	}
+
+	switch (uhid->input_buf.type) {
+	case UHID_CREATE:
+		ret = uhid_dev_create(uhid, &uhid->input_buf);
+		break;
+	case UHID_DESTROY:
+		ret = uhid_dev_destroy(uhid);
+		break;
+	case UHID_INPUT:
+		ret = uhid_dev_input(uhid, &uhid->input_buf);
+		break;
+	case UHID_FEATURE_ANSWER:
+		ret = uhid_dev_feature_answer(uhid, &uhid->input_buf);
+		break;
+	default:
+		ret = -EOPNOTSUPP;
+	}
+
+unlock:
+	mutex_unlock(&uhid->devlock);
+
+	/* return "count" not "len" to not confuse the caller */
+	return ret ? ret : count;
+}
+
+static unsigned int uhid_char_poll(struct file *file, poll_table *wait)
+{
+	struct uhid_device *uhid = file->private_data;
+
+	poll_wait(file, &uhid->waitq, wait);
+
+	if (uhid->head != uhid->tail)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+static const struct file_operations uhid_fops = {
+	.owner		= THIS_MODULE,
+	.open		= uhid_char_open,
+	.release	= uhid_char_release,
+	.read		= uhid_char_read,
+	.write		= uhid_char_write,
+	.poll		= uhid_char_poll,
+	.llseek		= no_llseek,
+};
+
+static struct miscdevice uhid_misc = {
+	.fops		= &uhid_fops,
+	.minor		= MISC_DYNAMIC_MINOR,
+	.name		= UHID_NAME,
+};
+
+static int __init uhid_init(void)
+{
+	return misc_register(&uhid_misc);
+}
+
+static void __exit uhid_exit(void)
+{
+	misc_deregister(&uhid_misc);
+}
+
+module_init(uhid_init);
+module_exit(uhid_exit);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("David Herrmann <dh.herrmann@gmail.com>");
+MODULE_DESCRIPTION("User-space I/O driver support for HID subsystem");
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index 482f936fc29..dedd8e4e5c6 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -84,7 +84,7 @@ static int hid_start_in(struct hid_device *hid)
 	spin_lock_irqsave(&usbhid->lock, flags);
 	if (hid->open > 0 &&
 			!test_bit(HID_DISCONNECTED, &usbhid->iofl) &&
-			!test_bit(HID_REPORTED_IDLE, &usbhid->iofl) &&
+			!test_bit(HID_SUSPENDED, &usbhid->iofl) &&
 			!test_and_set_bit(HID_IN_RUNNING, &usbhid->iofl)) {
 		rc = usb_submit_urb(usbhid->urbin, GFP_ATOMIC);
 		if (rc != 0) {
@@ -207,15 +207,27 @@ static int usbhid_restart_out_queue(struct usbhid_device *usbhid)
 	int kicked;
 	int r;
 
-	if (!hid)
+	if (!hid || test_bit(HID_RESET_PENDING, &usbhid->iofl) ||
+			test_bit(HID_SUSPENDED, &usbhid->iofl))
 		return 0;
 
 	if ((kicked = (usbhid->outhead != usbhid->outtail))) {
 		hid_dbg(hid, "Kicking head %d tail %d", usbhid->outhead, usbhid->outtail);
 
+		/* Try to wake up from autosuspend... */
 		r = usb_autopm_get_interface_async(usbhid->intf);
 		if (r < 0)
 			return r;
+
+		/*
+		 * If still suspended, don't submit.  Submission will
+		 * occur if/when resume drains the queue.
+		 */
+		if (test_bit(HID_SUSPENDED, &usbhid->iofl)) {
+			usb_autopm_put_interface_no_suspend(usbhid->intf);
+			return r;
+		}
+
 		/* Asynchronously flush queue. */
 		set_bit(HID_OUT_RUNNING, &usbhid->iofl);
 		if (hid_submit_out(hid)) {
@@ -234,15 +246,27 @@ static int usbhid_restart_ctrl_queue(struct usbhid_device *usbhid)
 	int r;
 
 	WARN_ON(hid == NULL);
-	if (!hid)
+	if (!hid || test_bit(HID_RESET_PENDING, &usbhid->iofl) ||
+			test_bit(HID_SUSPENDED, &usbhid->iofl))
 		return 0;
 
 	if ((kicked = (usbhid->ctrlhead != usbhid->ctrltail))) {
 		hid_dbg(hid, "Kicking head %d tail %d", usbhid->ctrlhead, usbhid->ctrltail);
 
+		/* Try to wake up from autosuspend... */
 		r = usb_autopm_get_interface_async(usbhid->intf);
 		if (r < 0)
 			return r;
+
+		/*
+		 * If still suspended, don't submit.  Submission will
+		 * occur if/when resume drains the queue.
+		 */
+		if (test_bit(HID_SUSPENDED, &usbhid->iofl)) {
+			usb_autopm_put_interface_no_suspend(usbhid->intf);
+			return r;
+		}
+
 		/* Asynchronously flush queue. */
 		set_bit(HID_CTRL_RUNNING, &usbhid->iofl);
 		if (hid_submit_ctrl(hid)) {
@@ -331,9 +355,12 @@ static int hid_submit_out(struct hid_device *hid)
 	usbhid->urbout->transfer_buffer_length = ((report->size - 1) >> 3) +
 						 1 + (report->id > 0);
 	usbhid->urbout->dev = hid_to_usb_dev(hid);
-	memcpy(usbhid->outbuf, raw_report,
-	       usbhid->urbout->transfer_buffer_length);
-	kfree(raw_report);
+	if (raw_report) {
+		memcpy(usbhid->outbuf, raw_report,
+				usbhid->urbout->transfer_buffer_length);
+		kfree(raw_report);
+		usbhid->out[usbhid->outtail].raw_report = NULL;
+	}
 
 	dbg_hid("submitting out urb\n");
 
@@ -362,8 +389,11 @@ static int hid_submit_ctrl(struct hid_device *hid)
 	if (dir == USB_DIR_OUT) {
 		usbhid->urbctrl->pipe = usb_sndctrlpipe(hid_to_usb_dev(hid), 0);
 		usbhid->urbctrl->transfer_buffer_length = len;
-		memcpy(usbhid->ctrlbuf, raw_report, len);
-		kfree(raw_report);
+		if (raw_report) {
+			memcpy(usbhid->ctrlbuf, raw_report, len);
+			kfree(raw_report);
+			usbhid->ctrl[usbhid->ctrltail].raw_report = NULL;
+		}
 	} else {
 		int maxpacket, padlen;
 
@@ -407,16 +437,6 @@ static int hid_submit_ctrl(struct hid_device *hid)
  * Output interrupt completion handler.
  */
 
-static int irq_out_pump_restart(struct hid_device *hid)
-{
-	struct usbhid_device *usbhid = hid->driver_data;
-
-	if (usbhid->outhead != usbhid->outtail)
-		return hid_submit_out(hid);
-	else
-		return -1;
-}
-
 static void hid_irq_out(struct urb *urb)
 {
 	struct hid_device *hid = urb->context;
@@ -441,15 +461,17 @@ static void hid_irq_out(struct urb *urb)
 
 	spin_lock_irqsave(&usbhid->lock, flags);
 
-	if (unplug)
+	if (unplug) {
 		usbhid->outtail = usbhid->outhead;
-	else
+	} else {
 		usbhid->outtail = (usbhid->outtail + 1) & (HID_OUTPUT_FIFO_SIZE - 1);
 
-	if (!irq_out_pump_restart(hid)) {
-		/* Successfully submitted next urb in queue */
-		spin_unlock_irqrestore(&usbhid->lock, flags);
-		return;
+		if (usbhid->outhead != usbhid->outtail &&
+				hid_submit_out(hid) == 0) {
+			/* Successfully submitted next urb in queue */
+			spin_unlock_irqrestore(&usbhid->lock, flags);
+			return;
+		}
 	}
 
 	clear_bit(HID_OUT_RUNNING, &usbhid->iofl);
@@ -461,15 +483,6 @@ static void hid_irq_out(struct urb *urb)
 /*
  * Control pipe completion handler.
  */
-static int ctrl_pump_restart(struct hid_device *hid)
-{
-	struct usbhid_device *usbhid = hid->driver_data;
-
-	if (usbhid->ctrlhead != usbhid->ctrltail)
-		return hid_submit_ctrl(hid);
-	else
-		return -1;
-}
 
 static void hid_ctrl(struct urb *urb)
 {
@@ -498,15 +511,17 @@ static void hid_ctrl(struct urb *urb)
 		hid_warn(urb->dev, "ctrl urb status %d received\n", status);
 	}
 
-	if (unplug)
+	if (unplug) {
 		usbhid->ctrltail = usbhid->ctrlhead;
-	else
+	} else {
 		usbhid->ctrltail = (usbhid->ctrltail + 1) & (HID_CONTROL_FIFO_SIZE - 1);
 
-	if (!ctrl_pump_restart(hid)) {
-		/* Successfully submitted next urb in queue */
-		spin_unlock(&usbhid->lock);
-		return;
+		if (usbhid->ctrlhead != usbhid->ctrltail &&
+				hid_submit_ctrl(hid) == 0) {
+			/* Successfully submitted next urb in queue */
+			spin_unlock(&usbhid->lock);
+			return;
+		}
 	}
 
 	clear_bit(HID_CTRL_RUNNING, &usbhid->iofl);
@@ -540,49 +555,36 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 		usbhid->out[usbhid->outhead].report = report;
 		usbhid->outhead = head;
 
-		/* Try to awake from autosuspend... */
-		if (usb_autopm_get_interface_async(usbhid->intf) < 0)
-			return;
+		/* If the queue isn't running, restart it */
+		if (!test_bit(HID_OUT_RUNNING, &usbhid->iofl)) {
+			usbhid_restart_out_queue(usbhid);
 
-		/*
-		 * But if still suspended, leave urb enqueued, don't submit.
-		 * Submission will occur if/when resume() drains the queue.
-		 */
-		if (test_bit(HID_REPORTED_IDLE, &usbhid->iofl))
-			return;
+		/* Otherwise see if an earlier request has timed out */
+		} else if (time_after(jiffies, usbhid->last_out + HZ * 5)) {
+
+			/* Prevent autosuspend following the unlink */
+			usb_autopm_get_interface_no_resume(usbhid->intf);
 
-		if (!test_and_set_bit(HID_OUT_RUNNING, &usbhid->iofl)) {
-			if (hid_submit_out(hid)) {
-				clear_bit(HID_OUT_RUNNING, &usbhid->iofl);
-				usb_autopm_put_interface_async(usbhid->intf);
-			}
-			wake_up(&usbhid->wait);
-		} else {
 			/*
-			 * the queue is known to run
-			 * but an earlier request may be stuck
-			 * we may need to time out
-			 * no race because the URB is blocked under
-			 * spinlock
+			 * Prevent resubmission in case the URB completes
+			 * before we can unlink it.  We don't want to cancel
+			 * the wrong transfer!
 			 */
-			if (time_after(jiffies, usbhid->last_out + HZ * 5)) {
-				usb_block_urb(usbhid->urbout);
-				/* drop lock to not deadlock if the callback is called */
-				spin_unlock(&usbhid->lock);
-				usb_unlink_urb(usbhid->urbout);
-				spin_lock(&usbhid->lock);
-				usb_unblock_urb(usbhid->urbout);
-				/*
-				 * if the unlinking has already completed
-				 * the pump will have been stopped
-				 * it must be restarted now
-				 */
-				if (!test_bit(HID_OUT_RUNNING, &usbhid->iofl))
-					if (!irq_out_pump_restart(hid))
-						set_bit(HID_OUT_RUNNING, &usbhid->iofl);
+			usb_block_urb(usbhid->urbout);
 
+			/* Drop lock to avoid deadlock if the callback runs */
+			spin_unlock(&usbhid->lock);
 
-			}
+			usb_unlink_urb(usbhid->urbout);
+			spin_lock(&usbhid->lock);
+			usb_unblock_urb(usbhid->urbout);
+
+			/* Unlink might have stopped the queue */
+			if (!test_bit(HID_OUT_RUNNING, &usbhid->iofl))
+				usbhid_restart_out_queue(usbhid);
+
+			/* Now we can allow autosuspend again */
+			usb_autopm_put_interface_async(usbhid->intf);
 		}
 		return;
 	}
@@ -604,47 +606,36 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 	usbhid->ctrl[usbhid->ctrlhead].dir = dir;
 	usbhid->ctrlhead = head;
 
-	/* Try to awake from autosuspend... */
-	if (usb_autopm_get_interface_async(usbhid->intf) < 0)
-		return;
+	/* If the queue isn't running, restart it */
+	if (!test_bit(HID_CTRL_RUNNING, &usbhid->iofl)) {
+		usbhid_restart_ctrl_queue(usbhid);
 
-	/*
-	 * If already suspended, leave urb enqueued, but don't submit.
-	 * Submission will occur if/when resume() drains the queue.
-	 */
-	if (test_bit(HID_REPORTED_IDLE, &usbhid->iofl))
-		return;
+	/* Otherwise see if an earlier request has timed out */
+	} else if (time_after(jiffies, usbhid->last_ctrl + HZ * 5)) {
+
+		/* Prevent autosuspend following the unlink */
+		usb_autopm_get_interface_no_resume(usbhid->intf);
 
-	if (!test_and_set_bit(HID_CTRL_RUNNING, &usbhid->iofl)) {
-		if (hid_submit_ctrl(hid)) {
-			clear_bit(HID_CTRL_RUNNING, &usbhid->iofl);
-			usb_autopm_put_interface_async(usbhid->intf);
-		}
-		wake_up(&usbhid->wait);
-	} else {
 		/*
-		 * the queue is known to run
-		 * but an earlier request may be stuck
-		 * we may need to time out
-		 * no race because the URB is blocked under
-		 * spinlock
+		 * Prevent resubmission in case the URB completes
+		 * before we can unlink it.  We don't want to cancel
+		 * the wrong transfer!
 		 */
-		if (time_after(jiffies, usbhid->last_ctrl + HZ * 5)) {
-			usb_block_urb(usbhid->urbctrl);
-			/* drop lock to not deadlock if the callback is called */
-			spin_unlock(&usbhid->lock);
-			usb_unlink_urb(usbhid->urbctrl);
-			spin_lock(&usbhid->lock);
-			usb_unblock_urb(usbhid->urbctrl);
-			/*
-			 * if the unlinking has already completed
-			 * the pump will have been stopped
-			 * it must be restarted now
-			 */
-			if (!test_bit(HID_CTRL_RUNNING, &usbhid->iofl))
-				if (!ctrl_pump_restart(hid))
-					set_bit(HID_CTRL_RUNNING, &usbhid->iofl);
-		}
+		usb_block_urb(usbhid->urbctrl);
+
+		/* Drop lock to avoid deadlock if the callback runs */
+		spin_unlock(&usbhid->lock);
+
+		usb_unlink_urb(usbhid->urbctrl);
+		spin_lock(&usbhid->lock);
+		usb_unblock_urb(usbhid->urbctrl);
+
+		/* Unlink might have stopped the queue */
+		if (!test_bit(HID_CTRL_RUNNING, &usbhid->iofl))
+			usbhid_restart_ctrl_queue(usbhid);
+
+		/* Now we can allow autosuspend again */
+		usb_autopm_put_interface_async(usbhid->intf);
 	}
 }
 
@@ -1002,9 +993,10 @@ static int usbhid_output_raw_report(struct hid_device *hid, __u8 *buf, size_t co
 
 static void usbhid_restart_queues(struct usbhid_device *usbhid)
 {
-	if (usbhid->urbout)
+	if (usbhid->urbout && !test_bit(HID_OUT_RUNNING, &usbhid->iofl))
 		usbhid_restart_out_queue(usbhid);
-	usbhid_restart_ctrl_queue(usbhid);
+	if (!test_bit(HID_CTRL_RUNNING, &usbhid->iofl))
+		usbhid_restart_ctrl_queue(usbhid);
 }
 
 static void hid_free_buffers(struct usb_device *dev, struct hid_device *hid)
@@ -1471,11 +1463,38 @@ void usbhid_put_power(struct hid_device *hid)
 
 
 #ifdef CONFIG_PM
+static int hid_resume_common(struct hid_device *hid, bool driver_suspended)
+{
+	struct usbhid_device *usbhid = hid->driver_data;
+	int status;
+
+	spin_lock_irq(&usbhid->lock);
+	clear_bit(HID_SUSPENDED, &usbhid->iofl);
+	usbhid_mark_busy(usbhid);
+
+	if (test_bit(HID_CLEAR_HALT, &usbhid->iofl) ||
+			test_bit(HID_RESET_PENDING, &usbhid->iofl))
+		schedule_work(&usbhid->reset_work);
+	usbhid->retry_delay = 0;
+
+	usbhid_restart_queues(usbhid);
+	spin_unlock_irq(&usbhid->lock);
+
+	status = hid_start_in(hid);
+	if (status < 0)
+		hid_io_error(hid);
+
+	if (driver_suspended && hid->driver && hid->driver->resume)
+		status = hid->driver->resume(hid);
+	return status;
+}
+
 static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct hid_device *hid = usb_get_intfdata(intf);
 	struct usbhid_device *usbhid = hid->driver_data;
 	int status;
+	bool driver_suspended = false;
 
 	if (PMSG_IS_AUTO(message)) {
 		spin_lock_irq(&usbhid->lock);	/* Sync with error handler */
@@ -1486,13 +1505,14 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 		    && !test_bit(HID_KEYS_PRESSED, &usbhid->iofl)
 		    && (!usbhid->ledcount || ignoreled))
 		{
-			set_bit(HID_REPORTED_IDLE, &usbhid->iofl);
+			set_bit(HID_SUSPENDED, &usbhid->iofl);
 			spin_unlock_irq(&usbhid->lock);
 			if (hid->driver && hid->driver->suspend) {
 				status = hid->driver->suspend(hid, message);
 				if (status < 0)
-					return status;
+					goto failed;
 			}
+			driver_suspended = true;
 		} else {
 			usbhid_mark_busy(usbhid);
 			spin_unlock_irq(&usbhid->lock);
@@ -1505,11 +1525,14 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 			if (status < 0)
 				return status;
 		}
+		driver_suspended = true;
 		spin_lock_irq(&usbhid->lock);
-		set_bit(HID_REPORTED_IDLE, &usbhid->iofl);
+		set_bit(HID_SUSPENDED, &usbhid->iofl);
 		spin_unlock_irq(&usbhid->lock);
-		if (usbhid_wait_io(hid) < 0)
-			return -EIO;
+		if (usbhid_wait_io(hid) < 0) {
+			status = -EIO;
+			goto failed;
+		}
 	}
 
 	hid_cancel_delayed_stuff(usbhid);
@@ -1517,14 +1540,15 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 
 	if (PMSG_IS_AUTO(message) && test_bit(HID_KEYS_PRESSED, &usbhid->iofl)) {
 		/* lost race against keypresses */
-		status = hid_start_in(hid);
-		if (status < 0)
-			hid_io_error(hid);
-		usbhid_mark_busy(usbhid);
-		return -EBUSY;
+		status = -EBUSY;
+		goto failed;
 	}
 	dev_dbg(&intf->dev, "suspend\n");
 	return 0;
+
+ failed:
+	hid_resume_common(hid, driver_suspended);
+	return status;
 }
 
 static int hid_resume(struct usb_interface *intf)
@@ -1536,23 +1560,7 @@ static int hid_resume(struct usb_interface *intf)
 	if (!test_bit(HID_STARTED, &usbhid->iofl))
 		return 0;
 
-	clear_bit(HID_REPORTED_IDLE, &usbhid->iofl);
-	usbhid_mark_busy(usbhid);
-
-	if (test_bit(HID_CLEAR_HALT, &usbhid->iofl) ||
-	    test_bit(HID_RESET_PENDING, &usbhid->iofl))
-		schedule_work(&usbhid->reset_work);
-	usbhid->retry_delay = 0;
-	status = hid_start_in(hid);
-	if (status < 0)
-		hid_io_error(hid);
-	usbhid_restart_queues(usbhid);
-
-	if (status >= 0 && hid->driver && hid->driver->resume) {
-		int ret = hid->driver->resume(hid);
-		if (ret < 0)
-			status = ret;
-	}
+	status = hid_resume_common(hid, true);
 	dev_dbg(&intf->dev, "resume status %d\n", status);
 	return 0;
 }
@@ -1563,7 +1571,7 @@ static int hid_reset_resume(struct usb_interface *intf)
 	struct usbhid_device *usbhid = hid->driver_data;
 	int status;
 
-	clear_bit(HID_REPORTED_IDLE, &usbhid->iofl);
+	clear_bit(HID_SUSPENDED, &usbhid->iofl);
 	status = hid_post_reset(intf);
 	if (status >= 0 && hid->driver && hid->driver->reset_resume) {
 		int ret = hid->driver->reset_resume(hid);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 0597ee604f6..903eef3d3e1 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -76,6 +76,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_PRODIGE, USB_DEVICE_ID_PRODIGE_CORDLESS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_SENNHEISER, USB_DEVICE_ID_SENNHEISER_BTD500USB, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SUN, USB_DEVICE_ID_RARITAN_KVM_DONGLE, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SYMBOL, USB_DEVICE_ID_SYMBOL_SCANNER_1, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SYMBOL, USB_DEVICE_ID_SYMBOL_SCANNER_2, HID_QUIRK_NOGET },
diff --git a/drivers/hid/usbhid/usbhid.h b/drivers/hid/usbhid/usbhid.h
index 1883d7b9487..bd87a61e530 100644
--- a/drivers/hid/usbhid/usbhid.h
+++ b/drivers/hid/usbhid/usbhid.h
@@ -53,7 +53,6 @@ struct usb_interface *usbhid_find_interface(int minor);
 #define HID_CLEAR_HALT		6
 #define HID_DISCONNECTED	7
 #define HID_STARTED		8
-#define HID_REPORTED_IDLE	9
 #define HID_KEYS_PRESSED	10
 #define HID_NO_BANDWIDTH	11
 
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index b9426a6592e..0614ff3a7d7 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -411,7 +411,7 @@ enum {
 #define HV_PRESENT_BIT			0x80000000
 
 #define HV_LINUX_GUEST_ID_LO		0x00000000
-#define HV_LINUX_GUEST_ID_HI		0xB16B00B5
+#define HV_LINUX_GUEST_ID_HI		2976579765
 #define HV_LINUX_GUEST_ID		(((u64)HV_LINUX_GUEST_ID_HI << 32) | \
 					   HV_LINUX_GUEST_ID_LO)
 
diff --git a/drivers/hv/vmbus_drv.c b/drivers/hv/vmbus_drv.c
index a220e5746d6..4748086eaaf 100644
--- a/drivers/hv/vmbus_drv.c
+++ b/drivers/hv/vmbus_drv.c
@@ -545,8 +545,7 @@ static int vmbus_bus_init(int irq)
 	if (ret)
 		goto err_cleanup;
 
-	ret = request_irq(irq, vmbus_isr, IRQF_SAMPLE_RANDOM,
-			driver_name, hv_acpi_dev);
+	ret = request_irq(irq, vmbus_isr, 0, driver_name, hv_acpi_dev);
 
 	if (ret != 0) {
 		pr_err("Unable to request IRQ %d\n",
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 6f1d167cb1e..b0a2e4c37e1 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -314,6 +314,16 @@ config SENSORS_DS1621
 	  This driver can also be built as a module.  If so, the module
 	  will be called ds1621.
 
+config SENSORS_DA9052_ADC
+	tristate "Dialog DA9052/DA9053 ADC"
+	depends on PMIC_DA9052
+	help
+	  Say y here to support the ADC found on Dialog Semiconductor
+	  DA9052-BC and DA9053-AA/Bx PMICs.
+
+	  This driver can also be built as module. If so, the module
+	  will be called da9052-hwmon.
+
 config SENSORS_EXYNOS4_TMU
 	tristate "Temperature sensor on Samsung EXYNOS4"
 	depends on ARCH_EXYNOS4
@@ -433,6 +443,16 @@ config SENSORS_GPIO_FAN
 	  This driver can also be built as a module.  If so, the module
 	  will be called gpio-fan.
 
+config SENSORS_HIH6130
+	tristate "Honeywell Humidicon HIH-6130 humidity/temperature sensor"
+	depends on I2C && EXPERIMENTAL
+	help
+	  If you say yes here you get support for Honeywell Humidicon
+	  HIH-6130 and HIH-6131 Humidicon humidity sensors.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called hih6130.
+
 config SENSORS_CORETEMP
 	tristate "Intel Core/Core2/Atom temperature sensor"
 	depends on X86 && PCI && EXPERIMENTAL
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index e1eeac13b85..7aa98119c4a 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -42,6 +42,7 @@ obj-$(CONFIG_SENSORS_APPLESMC)	+= applesmc.o
 obj-$(CONFIG_SENSORS_ASC7621)	+= asc7621.o
 obj-$(CONFIG_SENSORS_ATXP1)	+= atxp1.o
 obj-$(CONFIG_SENSORS_CORETEMP)	+= coretemp.o
+obj-$(CONFIG_SENSORS_DA9052_ADC)+= da9052-hwmon.o
 obj-$(CONFIG_SENSORS_DME1737)	+= dme1737.o
 obj-$(CONFIG_SENSORS_DS620)	+= ds620.o
 obj-$(CONFIG_SENSORS_DS1621)	+= ds1621.o
@@ -58,6 +59,7 @@ obj-$(CONFIG_SENSORS_G760A)	+= g760a.o
 obj-$(CONFIG_SENSORS_GL518SM)	+= gl518sm.o
 obj-$(CONFIG_SENSORS_GL520SM)	+= gl520sm.o
 obj-$(CONFIG_SENSORS_GPIO_FAN)	+= gpio-fan.o
+obj-$(CONFIG_SENSORS_HIH6130)	+= hih6130.o
 obj-$(CONFIG_SENSORS_ULTRA45)	+= ultra45_env.o
 obj-$(CONFIG_SENSORS_I5K_AMB)	+= i5k_amb.o
 obj-$(CONFIG_SENSORS_IBMAEM)	+= ibmaem.o
diff --git a/drivers/hwmon/abituguru.c b/drivers/hwmon/abituguru.c
index a72bf25601a..d4419b47f3d 100644
--- a/drivers/hwmon/abituguru.c
+++ b/drivers/hwmon/abituguru.c
@@ -1513,10 +1513,10 @@ LEAVE_UPDATE:
 		return NULL;
 }
 
-#ifdef CONFIG_PM
-static int abituguru_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int abituguru_suspend(struct device *dev)
 {
-	struct abituguru_data *data = platform_get_drvdata(pdev);
+	struct abituguru_data *data = dev_get_drvdata(dev);
 	/*
 	 * make sure all communications with the uguru are done and no new
 	 * ones are started
@@ -1525,29 +1525,30 @@ static int abituguru_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int abituguru_resume(struct platform_device *pdev)
+static int abituguru_resume(struct device *dev)
 {
-	struct abituguru_data *data = platform_get_drvdata(pdev);
+	struct abituguru_data *data = dev_get_drvdata(dev);
 	/* See if the uGuru is still ready */
 	if (inb_p(data->addr + ABIT_UGURU_DATA) != ABIT_UGURU_STATUS_INPUT)
 		data->uguru_ready = 0;
 	mutex_unlock(&data->update_lock);
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(abituguru_pm, abituguru_suspend, abituguru_resume);
+#define ABIT_UGURU_PM	&abituguru_pm
 #else
-#define abituguru_suspend	NULL
-#define abituguru_resume	NULL
+#define ABIT_UGURU_PM	NULL
 #endif /* CONFIG_PM */
 
 static struct platform_driver abituguru_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= ABIT_UGURU_NAME,
+		.pm	= ABIT_UGURU_PM,
 	},
 	.probe		= abituguru_probe,
 	.remove		= __devexit_p(abituguru_remove),
-	.suspend	= abituguru_suspend,
-	.resume		= abituguru_resume,
 };
 
 static int __init abituguru_detect(void)
diff --git a/drivers/hwmon/abituguru3.c b/drivers/hwmon/abituguru3.c
index a5bc4287daa..5d582aebff8 100644
--- a/drivers/hwmon/abituguru3.c
+++ b/drivers/hwmon/abituguru3.c
@@ -1141,10 +1141,10 @@ LEAVE_UPDATE:
 		return NULL;
 }
 
-#ifdef CONFIG_PM
-static int abituguru3_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int abituguru3_suspend(struct device *dev)
 {
-	struct abituguru3_data *data = platform_get_drvdata(pdev);
+	struct abituguru3_data *data = dev_get_drvdata(dev);
 	/*
 	 * make sure all communications with the uguru3 are done and no new
 	 * ones are started
@@ -1153,26 +1153,27 @@ static int abituguru3_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int abituguru3_resume(struct platform_device *pdev)
+static int abituguru3_resume(struct device *dev)
 {
-	struct abituguru3_data *data = platform_get_drvdata(pdev);
+	struct abituguru3_data *data = dev_get_drvdata(dev);
 	mutex_unlock(&data->update_lock);
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(abituguru3_pm, abituguru3_suspend, abituguru3_resume);
+#define ABIT_UGURU3_PM	&abituguru3_pm
 #else
-#define abituguru3_suspend	NULL
-#define abituguru3_resume	NULL
+#define ABIT_UGURU3_PM	NULL
 #endif /* CONFIG_PM */
 
 static struct platform_driver abituguru3_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= ABIT_UGURU3_NAME,
+		.pm	= ABIT_UGURU3_PM
 	},
 	.probe	= abituguru3_probe,
 	.remove	= __devexit_p(abituguru3_remove),
-	.suspend = abituguru3_suspend,
-	.resume = abituguru3_resume
 };
 
 static int __init abituguru3_dmi_detect(void)
diff --git a/drivers/hwmon/acpi_power_meter.c b/drivers/hwmon/acpi_power_meter.c
index 34ad5a27a7e..23ab3c496b0 100644
--- a/drivers/hwmon/acpi_power_meter.c
+++ b/drivers/hwmon/acpi_power_meter.c
@@ -101,7 +101,7 @@ struct acpi_power_meter_resource {
 	unsigned long		sensors_last_updated;
 	struct sensor_device_attribute	sensors[NUM_SENSORS];
 	int			num_sensors;
-	int			trip[2];
+	s64			trip[2];
 	int			num_domain_devices;
 	struct acpi_device	**domain_devices;
 	struct kobject		*holders_dir;
@@ -237,7 +237,7 @@ static ssize_t set_cap(struct device *dev, struct device_attribute *devattr,
 	if (res)
 		return res;
 
-	temp /= 1000;
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	if (temp > resource->caps.max_cap || temp < resource->caps.min_cap)
 		return -EINVAL;
 	arg0.integer.value = temp;
@@ -307,9 +307,7 @@ static ssize_t set_trip(struct device *dev, struct device_attribute *devattr,
 	if (res)
 		return res;
 
-	temp /= 1000;
-	if (temp < 0)
-		return -EINVAL;
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 
 	mutex_lock(&resource->lock);
 	resource->trip[attr->index - 7] = temp;
@@ -929,20 +927,29 @@ static int acpi_power_meter_remove(struct acpi_device *device, int type)
 	return 0;
 }
 
-static int acpi_power_meter_resume(struct acpi_device *device)
+#ifdef CONFIG_PM_SLEEP
+
+static int acpi_power_meter_resume(struct device *dev)
 {
 	struct acpi_power_meter_resource *resource;
 
-	if (!device || !acpi_driver_data(device))
+	if (!dev)
+		return -EINVAL;
+
+	resource = acpi_driver_data(to_acpi_device(dev));
+	if (!resource)
 		return -EINVAL;
 
-	resource = acpi_driver_data(device);
 	free_capabilities(resource);
 	read_capabilities(resource);
 
 	return 0;
 }
 
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(acpi_power_meter_pm, NULL, acpi_power_meter_resume);
+
 static struct acpi_driver acpi_power_meter_driver = {
 	.name = "power_meter",
 	.class = ACPI_POWER_METER_CLASS,
@@ -950,9 +957,9 @@ static struct acpi_driver acpi_power_meter_driver = {
 	.ops = {
 		.add = acpi_power_meter_add,
 		.remove = acpi_power_meter_remove,
-		.resume = acpi_power_meter_resume,
 		.notify = acpi_power_meter_notify,
 		},
+	.drv.pm = &acpi_power_meter_pm,
 };
 
 /* Module init/exit routines */
diff --git a/drivers/hwmon/adm1021.c b/drivers/hwmon/adm1021.c
index 4394e7e99c4..fd1d1b15854 100644
--- a/drivers/hwmon/adm1021.c
+++ b/drivers/hwmon/adm1021.c
@@ -366,11 +366,11 @@ static int adm1021_probe(struct i2c_client *client,
 	struct adm1021_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct adm1021_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct adm1021_data),
+			    GFP_KERNEL);
 	if (!data) {
-		pr_debug("adm1021: detect failed, kzalloc failed!\n");
-		err = -ENOMEM;
-		goto error0;
+		pr_debug("adm1021: detect failed, devm_kzalloc failed!\n");
+		return -ENOMEM;
 	}
 
 	i2c_set_clientdata(client, data);
@@ -384,21 +384,18 @@ static int adm1021_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &adm1021_group);
 	if (err)
-		goto error1;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
-		goto error3;
+		goto error;
 	}
 
 	return 0;
 
-error3:
+error:
 	sysfs_remove_group(&client->dev.kobj, &adm1021_group);
-error1:
-	kfree(data);
-error0:
 	return err;
 }
 
@@ -418,7 +415,6 @@ static int adm1021_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &adm1021_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/adm1025.c b/drivers/hwmon/adm1025.c
index b8557f9857d..7e16e5d07bc 100644
--- a/drivers/hwmon/adm1025.c
+++ b/drivers/hwmon/adm1025.c
@@ -477,11 +477,10 @@ static int adm1025_probe(struct i2c_client *client,
 	int err;
 	u8 config;
 
-	data = kzalloc(sizeof(struct adm1025_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct adm1025_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -492,7 +491,7 @@ static int adm1025_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &adm1025_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	/* Pin 11 is either in4 (+12V) or VID4 */
 	config = i2c_smbus_read_byte_data(client, ADM1025_REG_CONFIG);
@@ -513,9 +512,6 @@ static int adm1025_probe(struct i2c_client *client,
 exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &adm1025_group);
 	sysfs_remove_group(&client->dev.kobj, &adm1025_group_in4);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -569,7 +565,6 @@ static int adm1025_remove(struct i2c_client *client)
 	sysfs_remove_group(&client->dev.kobj, &adm1025_group);
 	sysfs_remove_group(&client->dev.kobj, &adm1025_group_in4);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/adm1026.c b/drivers/hwmon/adm1026.c
index 1003219b9f9..0f068e7297e 100644
--- a/drivers/hwmon/adm1026.c
+++ b/drivers/hwmon/adm1026.c
@@ -1834,11 +1834,10 @@ static int adm1026_probe(struct i2c_client *client,
 	struct adm1026_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct adm1026_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct adm1026_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -1852,7 +1851,7 @@ static int adm1026_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &adm1026_group);
 	if (err)
-		goto exitfree;
+		return err;
 	if (data->config1 & CFG1_AIN8_9)
 		err = sysfs_create_group(&client->dev.kobj,
 					 &adm1026_group_in8_9);
@@ -1877,9 +1876,6 @@ exitremove:
 		sysfs_remove_group(&client->dev.kobj, &adm1026_group_in8_9);
 	else
 		sysfs_remove_group(&client->dev.kobj, &adm1026_group_temp3);
-exitfree:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -1892,7 +1888,6 @@ static int adm1026_remove(struct i2c_client *client)
 		sysfs_remove_group(&client->dev.kobj, &adm1026_group_in8_9);
 	else
 		sysfs_remove_group(&client->dev.kobj, &adm1026_group_temp3);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/adm1031.c b/drivers/hwmon/adm1031.c
index 44e1fd7f3d8..c6a4631e833 100644
--- a/drivers/hwmon/adm1031.c
+++ b/drivers/hwmon/adm1031.c
@@ -954,11 +954,10 @@ static int adm1031_probe(struct i2c_client *client,
 	struct adm1031_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct adm1031_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct adm1031_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->chip_type = id->driver_data;
@@ -975,7 +974,7 @@ static int adm1031_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &adm1031_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	if (data->chip_type == adm1031) {
 		err = sysfs_create_group(&client->dev.kobj, &adm1031_group_opt);
@@ -994,9 +993,6 @@ static int adm1031_probe(struct i2c_client *client,
 exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &adm1031_group);
 	sysfs_remove_group(&client->dev.kobj, &adm1031_group_opt);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -1007,7 +1003,6 @@ static int adm1031_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &adm1031_group);
 	sysfs_remove_group(&client->dev.kobj, &adm1031_group_opt);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/adm9240.c b/drivers/hwmon/adm9240.c
index c3c2865a896..5a78d102a0f 100644
--- a/drivers/hwmon/adm9240.c
+++ b/drivers/hwmon/adm9240.c
@@ -650,11 +650,9 @@ static int adm9240_probe(struct i2c_client *new_client,
 	struct adm9240_data *data;
 	int err;
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&new_client->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(new_client, data);
 	mutex_init(&data->update_lock);
@@ -664,7 +662,7 @@ static int adm9240_probe(struct i2c_client *new_client,
 	/* populate sysfs filesystem */
 	err = sysfs_create_group(&new_client->dev.kobj, &adm9240_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&new_client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -676,9 +674,6 @@ static int adm9240_probe(struct i2c_client *new_client,
 
 exit_remove:
 	sysfs_remove_group(&new_client->dev.kobj, &adm9240_group);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -689,7 +684,6 @@ static int adm9240_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &adm9240_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/adt7475.c b/drivers/hwmon/adt7475.c
index df29d13a534..861c756e953 100644
--- a/drivers/hwmon/adt7475.c
+++ b/drivers/hwmon/adt7475.c
@@ -1260,7 +1260,7 @@ static int adt7475_probe(struct i2c_client *client,
 	int i, ret = 0, revision;
 	u8 config2, config3;
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
 	if (data == NULL)
 		return -ENOMEM;
 
@@ -1344,7 +1344,7 @@ static int adt7475_probe(struct i2c_client *client,
 
 	ret = sysfs_create_group(&client->dev.kobj, &adt7475_attr_group);
 	if (ret)
-		goto efree;
+		return ret;
 
 	/* Features that can be disabled individually */
 	if (data->has_fan4) {
@@ -1410,8 +1410,6 @@ static int adt7475_probe(struct i2c_client *client,
 
 eremove:
 	adt7475_remove_files(client, data);
-efree:
-	kfree(data);
 	return ret;
 }
 
@@ -1421,7 +1419,6 @@ static int adt7475_remove(struct i2c_client *client)
 
 	hwmon_device_unregister(data->hwmon_dev);
 	adt7475_remove_files(client, data);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 2cde9ecf773..28270886051 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -54,10 +54,10 @@
 #define APPLESMC_MAX_DATA_LENGTH 32
 
 /* wait up to 32 ms for a status change. */
-#define APPLESMC_MIN_WAIT	0x0040
+#define APPLESMC_MIN_WAIT	0x0010
+#define APPLESMC_RETRY_WAIT	0x0100
 #define APPLESMC_MAX_WAIT	0x8000
 
-#define APPLESMC_STATUS_MASK	0x0f
 #define APPLESMC_READ_CMD	0x10
 #define APPLESMC_WRITE_CMD	0x11
 #define APPLESMC_GET_KEY_BY_INDEX_CMD	0x12
@@ -80,6 +80,8 @@
 #define FANS_MANUAL		"FS! " /* r-w ui16 */
 #define FAN_ID_FMT		"F%dID" /* r-o char[16] */
 
+#define TEMP_SENSOR_TYPE	"sp78"
+
 /* List of keys used to read/write fan speeds */
 static const char *const fan_speed_fmt[] = {
 	"F%dAc",		/* actual speed */
@@ -96,10 +98,6 @@ static const char *const fan_speed_fmt[] = {
 #define APPLESMC_INPUT_FUZZ	4	/* input event threshold */
 #define APPLESMC_INPUT_FLAT	4
 
-#define SENSOR_X 0
-#define SENSOR_Y 1
-#define SENSOR_Z 2
-
 #define to_index(attr) (to_sensor_dev_attr(attr)->index & 0xffff)
 #define to_option(attr) (to_sensor_dev_attr(attr)->index >> 16)
 
@@ -135,11 +133,13 @@ static struct applesmc_registers {
 	unsigned int temp_count;	/* number of temperature registers */
 	unsigned int temp_begin;	/* temperature lower index bound */
 	unsigned int temp_end;		/* temperature upper index bound */
+	unsigned int index_count;	/* size of temperature index array */
 	int num_light_sensors;		/* number of light sensors */
 	bool has_accelerometer;		/* has motion sensor */
 	bool has_key_backlight;		/* has keyboard backlight */
 	bool init_complete;		/* true when fully initialized */
 	struct applesmc_entry *cache;	/* cached key entries */
+	const char **index;		/* temperature key index */
 } smcreg = {
 	.mutex = __MUTEX_INITIALIZER(smcreg.mutex),
 };
@@ -162,51 +162,68 @@ static unsigned int key_at_index;
 static struct workqueue_struct *applesmc_led_wq;
 
 /*
- * __wait_status - Wait up to 32ms for the status port to get a certain value
- * (masked with 0x0f), returning zero if the value is obtained.  Callers must
+ * wait_read - Wait for a byte to appear on SMC port. Callers must
  * hold applesmc_lock.
  */
-static int __wait_status(u8 val)
+static int wait_read(void)
 {
+	u8 status;
 	int us;
-
-	val = val & APPLESMC_STATUS_MASK;
-
 	for (us = APPLESMC_MIN_WAIT; us < APPLESMC_MAX_WAIT; us <<= 1) {
 		udelay(us);
-		if ((inb(APPLESMC_CMD_PORT) & APPLESMC_STATUS_MASK) == val)
+		status = inb(APPLESMC_CMD_PORT);
+		/* read: wait for smc to settle */
+		if (status & 0x01)
 			return 0;
 	}
 
+	pr_warn("wait_read() fail: 0x%02x\n", status);
 	return -EIO;
 }
 
 /*
- * special treatment of command port - on newer macbooks, it seems necessary
- * to resend the command byte before polling the status again. Callers must
- * hold applesmc_lock.
+ * send_byte - Write to SMC port, retrying when necessary. Callers
+ * must hold applesmc_lock.
  */
-static int send_command(u8 cmd)
+static int send_byte(u8 cmd, u16 port)
 {
+	u8 status;
 	int us;
+
+	outb(cmd, port);
 	for (us = APPLESMC_MIN_WAIT; us < APPLESMC_MAX_WAIT; us <<= 1) {
-		outb(cmd, APPLESMC_CMD_PORT);
 		udelay(us);
-		if ((inb(APPLESMC_CMD_PORT) & APPLESMC_STATUS_MASK) == 0x0c)
+		status = inb(APPLESMC_CMD_PORT);
+		/* write: wait for smc to settle */
+		if (status & 0x02)
+			continue;
+		/* ready: cmd accepted, return */
+		if (status & 0x04)
 			return 0;
+		/* timeout: give up */
+		if (us << 1 == APPLESMC_MAX_WAIT)
+			break;
+		/* busy: long wait and resend */
+		udelay(APPLESMC_RETRY_WAIT);
+		outb(cmd, port);
 	}
+
+	pr_warn("send_byte(0x%02x, 0x%04x) fail: 0x%02x\n", cmd, port, status);
 	return -EIO;
 }
 
+static int send_command(u8 cmd)
+{
+	return send_byte(cmd, APPLESMC_CMD_PORT);
+}
+
 static int send_argument(const char *key)
 {
 	int i;
 
-	for (i = 0; i < 4; i++) {
-		outb(key[i], APPLESMC_DATA_PORT);
-		if (__wait_status(0x04))
+	for (i = 0; i < 4; i++)
+		if (send_byte(key[i], APPLESMC_DATA_PORT))
 			return -EIO;
-	}
 	return 0;
 }
 
@@ -219,11 +236,14 @@ static int read_smc(u8 cmd, const char *key, u8 *buffer, u8 len)
 		return -EIO;
 	}
 
-	outb(len, APPLESMC_DATA_PORT);
+	if (send_byte(len, APPLESMC_DATA_PORT)) {
+		pr_warn("%.4s: read len fail\n", key);
+		return -EIO;
+	}
 
 	for (i = 0; i < len; i++) {
-		if (__wait_status(0x05)) {
-			pr_warn("%.4s: read data fail\n", key);
+		if (wait_read()) {
+			pr_warn("%.4s: read data[%d] fail\n", key, i);
 			return -EIO;
 		}
 		buffer[i] = inb(APPLESMC_DATA_PORT);
@@ -241,14 +261,16 @@ static int write_smc(u8 cmd, const char *key, const u8 *buffer, u8 len)
 		return -EIO;
 	}
 
-	outb(len, APPLESMC_DATA_PORT);
+	if (send_byte(len, APPLESMC_DATA_PORT)) {
+		pr_warn("%.4s: write len fail\n", key);
+		return -EIO;
+	}
 
 	for (i = 0; i < len; i++) {
-		if (__wait_status(0x04)) {
+		if (send_byte(buffer[i], APPLESMC_DATA_PORT)) {
 			pr_warn("%s: write data fail\n", key);
 			return -EIO;
 		}
-		outb(buffer[i], APPLESMC_DATA_PORT);
 	}
 
 	return 0;
@@ -432,30 +454,19 @@ static int applesmc_has_key(const char *key, bool *value)
 }
 
 /*
- * applesmc_read_motion_sensor - Read motion sensor (X, Y or Z).
+ * applesmc_read_s16 - Read 16-bit signed big endian register
  */
-static int applesmc_read_motion_sensor(int index, s16 *value)
+static int applesmc_read_s16(const char *key, s16 *value)
 {
 	u8 buffer[2];
 	int ret;
 
-	switch (index) {
-	case SENSOR_X:
-		ret = applesmc_read_key(MOTION_SENSOR_X_KEY, buffer, 2);
-		break;
-	case SENSOR_Y:
-		ret = applesmc_read_key(MOTION_SENSOR_Y_KEY, buffer, 2);
-		break;
-	case SENSOR_Z:
-		ret = applesmc_read_key(MOTION_SENSOR_Z_KEY, buffer, 2);
-		break;
-	default:
-		ret = -EINVAL;
-	}
+	ret = applesmc_read_key(key, buffer, 2);
+	if (ret)
+		return ret;
 
 	*value = ((s16)buffer[0] << 8) | buffer[1];
-
-	return ret;
+	return 0;
 }
 
 /*
@@ -482,6 +493,30 @@ static void applesmc_device_init(void)
 	pr_warn("failed to init the device\n");
 }
 
+static int applesmc_init_index(struct applesmc_registers *s)
+{
+	const struct applesmc_entry *entry;
+	unsigned int i;
+
+	if (s->index)
+		return 0;
+
+	s->index = kcalloc(s->temp_count, sizeof(s->index[0]), GFP_KERNEL);
+	if (!s->index)
+		return -ENOMEM;
+
+	for (i = s->temp_begin; i < s->temp_end; i++) {
+		entry = applesmc_get_entry_by_index(i);
+		if (IS_ERR(entry))
+			continue;
+		if (strcmp(entry->type, TEMP_SENSOR_TYPE))
+			continue;
+		s->index[s->index_count++] = entry->key;
+	}
+
+	return 0;
+}
+
 /*
  * applesmc_init_smcreg_try - Try to initialize register cache. Idempotent.
  */
@@ -517,6 +552,10 @@ static int applesmc_init_smcreg_try(void)
 		return ret;
 	s->temp_count = s->temp_end - s->temp_begin;
 
+	ret = applesmc_init_index(s);
+	if (ret)
+		return ret;
+
 	ret = applesmc_has_key(LIGHT_SENSOR_LEFT_KEY, &left_light_sensor);
 	if (ret)
 		return ret;
@@ -533,8 +572,8 @@ static int applesmc_init_smcreg_try(void)
 	s->num_light_sensors = left_light_sensor + right_light_sensor;
 	s->init_complete = true;
 
-	pr_info("key=%d fan=%d temp=%d acc=%d lux=%d kbd=%d\n",
-	       s->key_count, s->fan_count, s->temp_count,
+	pr_info("key=%d fan=%d temp=%d index=%d acc=%d lux=%d kbd=%d\n",
+	       s->key_count, s->fan_count, s->temp_count, s->index_count,
 	       s->has_accelerometer,
 	       s->num_light_sensors,
 	       s->has_key_backlight);
@@ -542,6 +581,15 @@ static int applesmc_init_smcreg_try(void)
 	return 0;
 }
 
+static void applesmc_destroy_smcreg(void)
+{
+	kfree(smcreg.index);
+	smcreg.index = NULL;
+	kfree(smcreg.cache);
+	smcreg.cache = NULL;
+	smcreg.init_complete = false;
+}
+
 /*
  * applesmc_init_smcreg - Initialize register cache.
  *
@@ -562,19 +610,11 @@ static int applesmc_init_smcreg(void)
 		msleep(INIT_WAIT_MSECS);
 	}
 
-	kfree(smcreg.cache);
-	smcreg.cache = NULL;
+	applesmc_destroy_smcreg();
 
 	return ret;
 }
 
-static void applesmc_destroy_smcreg(void)
-{
-	kfree(smcreg.cache);
-	smcreg.cache = NULL;
-	smcreg.init_complete = false;
-}
-
 /* Device model stuff */
 static int applesmc_probe(struct platform_device *dev)
 {
@@ -624,8 +664,8 @@ static struct platform_driver applesmc_driver = {
  */
 static void applesmc_calibrate(void)
 {
-	applesmc_read_motion_sensor(SENSOR_X, &rest_x);
-	applesmc_read_motion_sensor(SENSOR_Y, &rest_y);
+	applesmc_read_s16(MOTION_SENSOR_X_KEY, &rest_x);
+	applesmc_read_s16(MOTION_SENSOR_Y_KEY, &rest_y);
 	rest_x = -rest_x;
 }
 
@@ -634,9 +674,9 @@ static void applesmc_idev_poll(struct input_polled_dev *dev)
 	struct input_dev *idev = dev->input;
 	s16 x, y;
 
-	if (applesmc_read_motion_sensor(SENSOR_X, &x))
+	if (applesmc_read_s16(MOTION_SENSOR_X_KEY, &x))
 		return;
-	if (applesmc_read_motion_sensor(SENSOR_Y, &y))
+	if (applesmc_read_s16(MOTION_SENSOR_Y_KEY, &y))
 		return;
 
 	x = -x;
@@ -659,13 +699,13 @@ static ssize_t applesmc_position_show(struct device *dev,
 	int ret;
 	s16 x, y, z;
 
-	ret = applesmc_read_motion_sensor(SENSOR_X, &x);
+	ret = applesmc_read_s16(MOTION_SENSOR_X_KEY, &x);
 	if (ret)
 		goto out;
-	ret = applesmc_read_motion_sensor(SENSOR_Y, &y);
+	ret = applesmc_read_s16(MOTION_SENSOR_Y_KEY, &y);
 	if (ret)
 		goto out;
-	ret = applesmc_read_motion_sensor(SENSOR_Z, &z);
+	ret = applesmc_read_s16(MOTION_SENSOR_Z_KEY, &z);
 	if (ret)
 		goto out;
 
@@ -718,44 +758,27 @@ out:
 static ssize_t applesmc_show_sensor_label(struct device *dev,
 			struct device_attribute *devattr, char *sysfsbuf)
 {
-	int index = smcreg.temp_begin + to_index(devattr);
-	const struct applesmc_entry *entry;
-
-	entry = applesmc_get_entry_by_index(index);
-	if (IS_ERR(entry))
-		return PTR_ERR(entry);
+	const char *key = smcreg.index[to_index(devattr)];
 
-	return snprintf(sysfsbuf, PAGE_SIZE, "%s\n", entry->key);
+	return snprintf(sysfsbuf, PAGE_SIZE, "%s\n", key);
 }
 
 /* Displays degree Celsius * 1000 */
 static ssize_t applesmc_show_temperature(struct device *dev,
 			struct device_attribute *devattr, char *sysfsbuf)
 {
-	int index = smcreg.temp_begin + to_index(devattr);
-	const struct applesmc_entry *entry;
+	const char *key = smcreg.index[to_index(devattr)];
 	int ret;
-	u8 buffer[2];
-	unsigned int temp;
-
-	entry = applesmc_get_entry_by_index(index);
-	if (IS_ERR(entry))
-		return PTR_ERR(entry);
-	if (entry->len > 2)
-		return -EINVAL;
+	s16 value;
+	int temp;
 
-	ret = applesmc_read_entry(entry, buffer, entry->len);
+	ret = applesmc_read_s16(key, &value);
 	if (ret)
 		return ret;
 
-	if (entry->len == 2) {
-		temp = buffer[0] * 1000;
-		temp += (buffer[1] >> 6) * 250;
-	} else {
-		temp = buffer[0] * 4000;
-	}
+	temp = 250 * (value >> 6);
 
-	return snprintf(sysfsbuf, PAGE_SIZE, "%u\n", temp);
+	return snprintf(sysfsbuf, PAGE_SIZE, "%d\n", temp);
 }
 
 static ssize_t applesmc_show_fan_speed(struct device *dev,
@@ -1265,7 +1288,7 @@ static int __init applesmc_init(void)
 	if (ret)
 		goto out_info;
 
-	ret = applesmc_create_nodes(temp_group, smcreg.temp_count);
+	ret = applesmc_create_nodes(temp_group, smcreg.index_count);
 	if (ret)
 		goto out_fans;
 
diff --git a/drivers/hwmon/asc7621.c b/drivers/hwmon/asc7621.c
index 7caa242915a..b867aab7804 100644
--- a/drivers/hwmon/asc7621.c
+++ b/drivers/hwmon/asc7621.c
@@ -1109,7 +1109,8 @@ asc7621_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -EIO;
 
-	data = kzalloc(sizeof(struct asc7621_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct asc7621_data),
+			    GFP_KERNEL);
 	if (data == NULL)
 		return -ENOMEM;
 
@@ -1143,7 +1144,6 @@ exit_remove:
 				   &(asc7621_params[i].sda.dev_attr));
 	}
 
-	kfree(data);
 	return err;
 }
 
@@ -1192,7 +1192,6 @@ static int asc7621_remove(struct i2c_client *client)
 				   &(asc7621_params[i].sda.dev_attr));
 	}
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/atxp1.c b/drivers/hwmon/atxp1.c
index 58af6aa9353..aecb9ea7beb 100644
--- a/drivers/hwmon/atxp1.c
+++ b/drivers/hwmon/atxp1.c
@@ -345,11 +345,10 @@ static int atxp1_probe(struct i2c_client *new_client,
 	struct atxp1_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct atxp1_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&new_client->dev, sizeof(struct atxp1_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	/* Get VRM */
 	data->vrm = vid_which_vrm();
@@ -362,7 +361,7 @@ static int atxp1_probe(struct i2c_client *new_client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&new_client->dev.kobj, &atxp1_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&new_client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -377,9 +376,6 @@ static int atxp1_probe(struct i2c_client *new_client,
 
 exit_remove_files:
 	sysfs_remove_group(&new_client->dev.kobj, &atxp1_group);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 };
 
@@ -390,8 +386,6 @@ static int atxp1_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &atxp1_group);
 
-	kfree(data);
-
 	return 0;
 };
 
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 637c51c11b4..faa16f80db9 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -793,7 +793,7 @@ static struct notifier_block coretemp_cpu_notifier __refdata = {
 	.notifier_call = coretemp_cpu_callback,
 };
 
-static const struct x86_cpu_id coretemp_ids[] = {
+static const struct x86_cpu_id __initconst coretemp_ids[] = {
 	{ X86_VENDOR_INTEL, X86_FAMILY_ANY, X86_MODEL_ANY, X86_FEATURE_DTHERM },
 	{}
 };
diff --git a/drivers/hwmon/da9052-hwmon.c b/drivers/hwmon/da9052-hwmon.c
new file mode 100644
index 00000000000..fc65f2d3ec9
--- /dev/null
+++ b/drivers/hwmon/da9052-hwmon.c
@@ -0,0 +1,344 @@
+/*
+ * HWMON Driver for Dialog DA9052
+ *
+ * Copyright(c) 2012 Dialog Semiconductor Ltd.
+ *
+ * Author: David Dajun Chen <dchen@diasemi.com>
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include <linux/mfd/da9052/da9052.h>
+#include <linux/mfd/da9052/reg.h>
+
+struct da9052_hwmon {
+	struct da9052	*da9052;
+	struct device	*class_device;
+	struct mutex	hwmon_lock;
+};
+
+static const char * const input_names[] = {
+	[DA9052_ADC_VDDOUT]	=	"VDDOUT",
+	[DA9052_ADC_ICH]	=	"CHARGING CURRENT",
+	[DA9052_ADC_TBAT]	=	"BATTERY TEMP",
+	[DA9052_ADC_VBAT]	=	"BATTERY VOLTAGE",
+	[DA9052_ADC_IN4]	=	"ADC IN4",
+	[DA9052_ADC_IN5]	=	"ADC IN5",
+	[DA9052_ADC_IN6]	=	"ADC IN6",
+	[DA9052_ADC_TJUNC]	=	"BATTERY JUNCTION TEMP",
+	[DA9052_ADC_VBBAT]	=	"BACK-UP BATTERY VOLTAGE",
+};
+
+/* Conversion function for VDDOUT and VBAT */
+static inline int volt_reg_to_mV(int value)
+{
+	return DIV_ROUND_CLOSEST(value * 1000, 512) + 2500;
+}
+
+/* Conversion function for ADC channels 4, 5 and 6 */
+static inline int input_reg_to_mV(int value)
+{
+	return DIV_ROUND_CLOSEST(value * 2500, 1023);
+}
+
+/* Conversion function for VBBAT */
+static inline int vbbat_reg_to_mV(int value)
+{
+	return DIV_ROUND_CLOSEST(value * 2500, 512);
+}
+
+static int da9052_enable_vddout_channel(struct da9052 *da9052)
+{
+	int ret;
+
+	ret = da9052_reg_read(da9052, DA9052_ADC_CONT_REG);
+	if (ret < 0)
+		return ret;
+
+	ret |= DA9052_ADCCONT_AUTOVDDEN;
+
+	return da9052_reg_write(da9052, DA9052_ADC_CONT_REG, ret);
+}
+
+static int da9052_disable_vddout_channel(struct da9052 *da9052)
+{
+	int ret;
+
+	ret = da9052_reg_read(da9052, DA9052_ADC_CONT_REG);
+	if (ret < 0)
+		return ret;
+
+	ret &= ~DA9052_ADCCONT_AUTOVDDEN;
+
+	return da9052_reg_write(da9052, DA9052_ADC_CONT_REG, ret);
+}
+
+static ssize_t da9052_read_vddout(struct device *dev,
+				  struct device_attribute *devattr, char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+	int ret, vdd;
+
+	mutex_lock(&hwmon->hwmon_lock);
+
+	ret = da9052_enable_vddout_channel(hwmon->da9052);
+	if (ret < 0)
+		goto hwmon_err;
+
+	vdd = da9052_reg_read(hwmon->da9052, DA9052_VDD_RES_REG);
+	if (vdd < 0) {
+		ret = vdd;
+		goto hwmon_err_release;
+	}
+
+	ret = da9052_disable_vddout_channel(hwmon->da9052);
+	if (ret < 0)
+		goto hwmon_err;
+
+	mutex_unlock(&hwmon->hwmon_lock);
+	return sprintf(buf, "%d\n", volt_reg_to_mV(vdd));
+
+hwmon_err_release:
+	da9052_disable_vddout_channel(hwmon->da9052);
+hwmon_err:
+	mutex_unlock(&hwmon->hwmon_lock);
+	return ret;
+}
+
+static ssize_t da9052_read_ich(struct device *dev,
+			       struct device_attribute *devattr, char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+	int ret;
+
+	ret = da9052_reg_read(hwmon->da9052, DA9052_ICHG_AV_REG);
+	if (ret < 0)
+		return ret;
+
+	/* Equivalent to 3.9mA/bit in register ICHG_AV */
+	return sprintf(buf, "%d\n", DIV_ROUND_CLOSEST(ret * 39, 10));
+}
+
+static ssize_t da9052_read_tbat(struct device *dev,
+				struct device_attribute *devattr, char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", da9052_adc_read_temp(hwmon->da9052));
+}
+
+static ssize_t da9052_read_vbat(struct device *dev,
+				struct device_attribute *devattr, char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+	int ret;
+
+	ret = da9052_adc_manual_read(hwmon->da9052, DA9052_ADC_VBAT);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", volt_reg_to_mV(ret));
+}
+
+static ssize_t da9052_read_misc_channel(struct device *dev,
+					struct device_attribute *devattr,
+					char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+	int channel = to_sensor_dev_attr(devattr)->index;
+	int ret;
+
+	ret = da9052_adc_manual_read(hwmon->da9052, channel);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", input_reg_to_mV(ret));
+}
+
+static ssize_t da9052_read_tjunc(struct device *dev,
+				 struct device_attribute *devattr, char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+	int tjunc;
+	int toffset;
+
+	tjunc = da9052_reg_read(hwmon->da9052, DA9052_TJUNC_RES_REG);
+	if (tjunc < 0)
+		return tjunc;
+
+	toffset = da9052_reg_read(hwmon->da9052, DA9052_T_OFFSET_REG);
+	if (toffset < 0)
+		return toffset;
+
+	/*
+	 * Degrees celsius = 1.708 * (TJUNC_RES - T_OFFSET) - 108.8
+	 * T_OFFSET is a trim value used to improve accuracy of the result
+	 */
+	return sprintf(buf, "%d\n", 1708 * (tjunc - toffset) - 108800);
+}
+
+static ssize_t da9052_read_vbbat(struct device *dev,
+				 struct device_attribute *devattr, char *buf)
+{
+	struct da9052_hwmon *hwmon = dev_get_drvdata(dev);
+	int ret;
+
+	ret = da9052_adc_manual_read(hwmon->da9052, DA9052_ADC_VBBAT);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", vbbat_reg_to_mV(ret));
+}
+
+static ssize_t da9052_hwmon_show_name(struct device *dev,
+				      struct device_attribute *devattr,
+				      char *buf)
+{
+	return sprintf(buf, "da9052-hwmon\n");
+}
+
+static ssize_t show_label(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	return sprintf(buf, "%s\n",
+		       input_names[to_sensor_dev_attr(devattr)->index]);
+}
+
+static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, da9052_read_vddout, NULL,
+			  DA9052_ADC_VDDOUT);
+static SENSOR_DEVICE_ATTR(in0_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_VDDOUT);
+static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, da9052_read_vbat, NULL,
+			  DA9052_ADC_VBAT);
+static SENSOR_DEVICE_ATTR(in3_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_VBAT);
+static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, da9052_read_misc_channel, NULL,
+			  DA9052_ADC_IN4);
+static SENSOR_DEVICE_ATTR(in4_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_IN4);
+static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, da9052_read_misc_channel, NULL,
+			  DA9052_ADC_IN5);
+static SENSOR_DEVICE_ATTR(in5_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_IN5);
+static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, da9052_read_misc_channel, NULL,
+			  DA9052_ADC_IN6);
+static SENSOR_DEVICE_ATTR(in6_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_IN6);
+static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, da9052_read_vbbat, NULL,
+			  DA9052_ADC_VBBAT);
+static SENSOR_DEVICE_ATTR(in9_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_VBBAT);
+
+static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, da9052_read_ich, NULL,
+			  DA9052_ADC_ICH);
+static SENSOR_DEVICE_ATTR(curr1_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_ICH);
+
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, da9052_read_tbat, NULL,
+			  DA9052_ADC_TBAT);
+static SENSOR_DEVICE_ATTR(temp2_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_TBAT);
+static SENSOR_DEVICE_ATTR(temp8_input, S_IRUGO, da9052_read_tjunc, NULL,
+			  DA9052_ADC_TJUNC);
+static SENSOR_DEVICE_ATTR(temp8_label, S_IRUGO, show_label, NULL,
+			  DA9052_ADC_TJUNC);
+
+static DEVICE_ATTR(name, S_IRUGO, da9052_hwmon_show_name, NULL);
+
+static struct attribute *da9052_attr[] = {
+	&dev_attr_name.attr,
+	&sensor_dev_attr_in0_input.dev_attr.attr,
+	&sensor_dev_attr_in0_label.dev_attr.attr,
+	&sensor_dev_attr_in3_input.dev_attr.attr,
+	&sensor_dev_attr_in3_label.dev_attr.attr,
+	&sensor_dev_attr_in4_input.dev_attr.attr,
+	&sensor_dev_attr_in4_label.dev_attr.attr,
+	&sensor_dev_attr_in5_input.dev_attr.attr,
+	&sensor_dev_attr_in5_label.dev_attr.attr,
+	&sensor_dev_attr_in6_input.dev_attr.attr,
+	&sensor_dev_attr_in6_label.dev_attr.attr,
+	&sensor_dev_attr_in9_input.dev_attr.attr,
+	&sensor_dev_attr_in9_label.dev_attr.attr,
+	&sensor_dev_attr_curr1_input.dev_attr.attr,
+	&sensor_dev_attr_curr1_label.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_label.dev_attr.attr,
+	&sensor_dev_attr_temp8_input.dev_attr.attr,
+	&sensor_dev_attr_temp8_label.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group da9052_attr_group = {.attrs = da9052_attr};
+
+static int __devinit da9052_hwmon_probe(struct platform_device *pdev)
+{
+	struct da9052_hwmon *hwmon;
+	int ret;
+
+	hwmon = devm_kzalloc(&pdev->dev, sizeof(struct da9052_hwmon),
+			     GFP_KERNEL);
+	if (!hwmon)
+		return -ENOMEM;
+
+	mutex_init(&hwmon->hwmon_lock);
+	hwmon->da9052 = dev_get_drvdata(pdev->dev.parent);
+
+	platform_set_drvdata(pdev, hwmon);
+
+	ret = sysfs_create_group(&pdev->dev.kobj, &da9052_attr_group);
+	if (ret)
+		goto err_mem;
+
+	hwmon->class_device = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(hwmon->class_device)) {
+		ret = PTR_ERR(hwmon->class_device);
+		goto err_sysfs;
+	}
+
+	return 0;
+
+err_sysfs:
+	sysfs_remove_group(&pdev->dev.kobj, &da9052_attr_group);
+err_mem:
+	return ret;
+}
+
+static int __devexit da9052_hwmon_remove(struct platform_device *pdev)
+{
+	struct da9052_hwmon *hwmon = platform_get_drvdata(pdev);
+
+	hwmon_device_unregister(hwmon->class_device);
+	sysfs_remove_group(&pdev->dev.kobj, &da9052_attr_group);
+
+	return 0;
+}
+
+static struct platform_driver da9052_hwmon_driver = {
+	.probe = da9052_hwmon_probe,
+	.remove = __devexit_p(da9052_hwmon_remove),
+	.driver = {
+		.name = "da9052-hwmon",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_platform_driver(da9052_hwmon_driver);
+
+MODULE_AUTHOR("David Dajun Chen <dchen@diasemi.com>");
+MODULE_DESCRIPTION("DA9052 HWMON driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9052-hwmon");
diff --git a/drivers/hwmon/ds1621.c b/drivers/hwmon/ds1621.c
index f647a3307eb..1c568736baf 100644
--- a/drivers/hwmon/ds1621.c
+++ b/drivers/hwmon/ds1621.c
@@ -249,11 +249,10 @@ static int ds1621_probe(struct i2c_client *client,
 	struct ds1621_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct ds1621_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct ds1621_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -264,7 +263,7 @@ static int ds1621_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &ds1621_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -276,9 +275,6 @@ static int ds1621_probe(struct i2c_client *client,
 
  exit_remove_files:
 	sysfs_remove_group(&client->dev.kobj, &ds1621_group);
- exit_free:
-	kfree(data);
- exit:
 	return err;
 }
 
@@ -289,8 +285,6 @@ static int ds1621_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &ds1621_group);
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/emc2103.c b/drivers/hwmon/emc2103.c
index e7d234b5931..7bb8e888692 100644
--- a/drivers/hwmon/emc2103.c
+++ b/drivers/hwmon/emc2103.c
@@ -732,6 +732,6 @@ static struct i2c_driver emc2103_driver = {
 
 module_i2c_driver(emc2103_driver);
 
-MODULE_AUTHOR("Steve Glendinning <steve.glendinning@smsc.com>");
+MODULE_AUTHOR("Steve Glendinning <steve.glendinning@shawell.net>");
 MODULE_DESCRIPTION("SMSC EMC2103 hwmon driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/emc6w201.c b/drivers/hwmon/emc6w201.c
index 840f5112e60..ada12a98a97 100644
--- a/drivers/hwmon/emc6w201.c
+++ b/drivers/hwmon/emc6w201.c
@@ -492,11 +492,10 @@ static int emc6w201_probe(struct i2c_client *client,
 	struct emc6w201_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct emc6w201_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct emc6w201_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -504,7 +503,7 @@ static int emc6w201_probe(struct i2c_client *client,
 	/* Create sysfs attribute */
 	err = sysfs_create_group(&client->dev.kobj, &emc6w201_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	/* Expose as a hwmon device */
 	data->hwmon_dev = hwmon_device_register(&client->dev);
@@ -517,9 +516,6 @@ static int emc6w201_probe(struct i2c_client *client,
 
  exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &emc6w201_group);
- exit_free:
-	kfree(data);
- exit:
 	return err;
 }
 
@@ -529,7 +525,6 @@ static int emc6w201_remove(struct i2c_client *client)
 
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &emc6w201_group);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/hwmon/exynos4_tmu.c b/drivers/hwmon/exynos4_tmu.c
index f2359a0093b..e912059140c 100644
--- a/drivers/hwmon/exynos4_tmu.c
+++ b/drivers/hwmon/exynos4_tmu.c
@@ -475,35 +475,39 @@ static int __devexit exynos4_tmu_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int exynos4_tmu_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int exynos4_tmu_suspend(struct device *dev)
 {
-	exynos4_tmu_control(pdev, false);
+	exynos4_tmu_control(to_platform_device(dev), false);
 
 	return 0;
 }
 
-static int exynos4_tmu_resume(struct platform_device *pdev)
+static int exynos4_tmu_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	exynos4_tmu_initialize(pdev);
 	exynos4_tmu_control(pdev, true);
 
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(exynos4_tmu_pm,
+			 exynos4_tmu_suspend, exynos4_tmu_resume);
+#define EXYNOS4_TMU_PM	&exynos4_tmu_pm
 #else
-#define exynos4_tmu_suspend NULL
-#define exynos4_tmu_resume NULL
+#define EXYNOS4_TMU_PM	NULL
 #endif
 
 static struct platform_driver exynos4_tmu_driver = {
 	.driver = {
 		.name   = "exynos4-tmu",
 		.owner  = THIS_MODULE,
+		.pm     = EXYNOS4_TMU_PM,
 	},
 	.probe = exynos4_tmu_probe,
 	.remove	= __devexit_p(exynos4_tmu_remove),
-	.suspend = exynos4_tmu_suspend,
-	.resume = exynos4_tmu_resume,
 };
 
 module_platform_driver(exynos4_tmu_driver);
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index 3e4da620e9c..4dd7723d257 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -1386,20 +1386,20 @@ static int __devinit f71805f_probe(struct platform_device *pdev)
 		"f71872f",
 	};
 
-	data = kzalloc(sizeof(struct f71805f_data), GFP_KERNEL);
+	data = devm_kzalloc(&pdev->dev, sizeof(struct f71805f_data),
+			    GFP_KERNEL);
 	if (!data) {
-		err = -ENOMEM;
 		pr_err("Out of memory\n");
-		goto exit;
+		return -ENOMEM;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start + ADDR_REG_OFFSET, 2, DRVNAME)) {
-		err = -EBUSY;
+	if (!devm_request_region(&pdev->dev, res->start + ADDR_REG_OFFSET, 2,
+				 DRVNAME)) {
 		dev_err(&pdev->dev, "Failed to request region 0x%lx-0x%lx\n",
 			(unsigned long)(res->start + ADDR_REG_OFFSET),
 			(unsigned long)(res->start + ADDR_REG_OFFSET + 1));
-		goto exit_free;
+		return -EBUSY;
 	}
 	data->addr = res->start;
 	data->name = names[sio_data->kind];
@@ -1427,7 +1427,7 @@ static int __devinit f71805f_probe(struct platform_device *pdev)
 	/* Register sysfs interface files */
 	err = sysfs_create_group(&pdev->dev.kobj, &f71805f_group);
 	if (err)
-		goto exit_release_region;
+		return err;
 	if (data->has_in & (1 << 4)) { /* in4 */
 		err = sysfs_create_group(&pdev->dev.kobj,
 					 &f71805f_group_optin[0]);
@@ -1487,19 +1487,12 @@ exit_remove_files:
 	for (i = 0; i < 4; i++)
 		sysfs_remove_group(&pdev->dev.kobj, &f71805f_group_optin[i]);
 	sysfs_remove_group(&pdev->dev.kobj, &f71805f_group_pwm_freq);
-exit_release_region:
-	release_region(res->start + ADDR_REG_OFFSET, 2);
-exit_free:
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-exit:
 	return err;
 }
 
 static int __devexit f71805f_remove(struct platform_device *pdev)
 {
 	struct f71805f_data *data = platform_get_drvdata(pdev);
-	struct resource *res;
 	int i;
 
 	hwmon_device_unregister(data->hwmon_dev);
@@ -1507,11 +1500,6 @@ static int __devexit f71805f_remove(struct platform_device *pdev)
 	for (i = 0; i < 4; i++)
 		sysfs_remove_group(&pdev->dev.kobj, &f71805f_group_optin[i]);
 	sysfs_remove_group(&pdev->dev.kobj, &f71805f_group_pwm_freq);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
-	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	release_region(res->start + ADDR_REG_OFFSET, 2);
 
 	return 0;
 }
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index 6b13f1a4dc2..2764b78a784 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -67,7 +67,8 @@ static ssize_t show_power(struct device *dev,
 				  REG_TDP_LIMIT3, &val);
 
 	tdp_limit = val >> 16;
-	curr_pwr_watts = (tdp_limit + data->base_tdp) << running_avg_range;
+	curr_pwr_watts = ((u64)(tdp_limit +
+				data->base_tdp)) << running_avg_range;
 	curr_pwr_watts -= running_avg_capture;
 	curr_pwr_watts *= data->tdp_to_watts;
 
diff --git a/drivers/hwmon/gl518sm.c b/drivers/hwmon/gl518sm.c
index 764a083ac7a..2c74673f48e 100644
--- a/drivers/hwmon/gl518sm.c
+++ b/drivers/hwmon/gl518sm.c
@@ -544,11 +544,10 @@ static int gl518_probe(struct i2c_client *client,
 	struct gl518_data *data;
 	int err, revision;
 
-	data = kzalloc(sizeof(struct gl518_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct gl518_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	revision = gl518_read_value(client, GL518_REG_REVISION);
@@ -562,7 +561,7 @@ static int gl518_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &gl518_group);
 	if (err)
-		goto exit_free;
+		return err;
 	if (data->type == gl518sm_r80) {
 		err = sysfs_create_group(&client->dev.kobj, &gl518_group_r80);
 		if (err)
@@ -581,9 +580,6 @@ exit_remove_files:
 	sysfs_remove_group(&client->dev.kobj, &gl518_group);
 	if (data->type == gl518sm_r80)
 		sysfs_remove_group(&client->dev.kobj, &gl518_group_r80);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -617,7 +613,6 @@ static int gl518_remove(struct i2c_client *client)
 	if (data->type == gl518sm_r80)
 		sysfs_remove_group(&client->dev.kobj, &gl518_group_r80);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/gl520sm.c b/drivers/hwmon/gl520sm.c
index 5ff452b6a4d..a21ff252f2f 100644
--- a/drivers/hwmon/gl520sm.c
+++ b/drivers/hwmon/gl520sm.c
@@ -779,11 +779,10 @@ static int gl520_probe(struct i2c_client *client,
 	struct gl520_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct gl520_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct gl520_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -794,7 +793,7 @@ static int gl520_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &gl520_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	if (data->two_temps)
 		err = sysfs_create_group(&client->dev.kobj, &gl520_group_temp2);
@@ -816,9 +815,6 @@ exit_remove_files:
 	sysfs_remove_group(&client->dev.kobj, &gl520_group);
 	sysfs_remove_group(&client->dev.kobj, &gl520_group_in4);
 	sysfs_remove_group(&client->dev.kobj, &gl520_group_temp2);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -870,7 +866,6 @@ static int gl520_remove(struct i2c_client *client)
 	sysfs_remove_group(&client->dev.kobj, &gl520_group_in4);
 	sysfs_remove_group(&client->dev.kobj, &gl520_group_temp2);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 2ce8c44a0e0..2f4b01bda87 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -41,7 +41,7 @@ struct gpio_fan_data {
 	int			num_speed;
 	struct gpio_fan_speed	*speed;
 	int			speed_index;
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	int			resume_speed;
 #endif
 	bool			pwm_enable;
@@ -95,17 +95,17 @@ static int fan_alarm_init(struct gpio_fan_data *fan_data,
 
 	fan_data->alarm = alarm;
 
-	err = gpio_request(alarm->gpio, "GPIO fan alarm");
+	err = devm_gpio_request(&pdev->dev, alarm->gpio, "GPIO fan alarm");
 	if (err)
 		return err;
 
 	err = gpio_direction_input(alarm->gpio);
 	if (err)
-		goto err_free_gpio;
+		return err;
 
 	err = device_create_file(&pdev->dev, &dev_attr_fan1_alarm);
 	if (err)
-		goto err_free_gpio;
+		return err;
 
 	/*
 	 * If the alarm GPIO don't support interrupts, just leave
@@ -117,8 +117,8 @@ static int fan_alarm_init(struct gpio_fan_data *fan_data,
 
 	INIT_WORK(&fan_data->alarm_work, fan_alarm_notify);
 	irq_set_irq_type(alarm_irq, IRQ_TYPE_EDGE_BOTH);
-	err = request_irq(alarm_irq, fan_alarm_irq_handler, IRQF_SHARED,
-			  "GPIO fan alarm", fan_data);
+	err = devm_request_irq(&pdev->dev, alarm_irq, fan_alarm_irq_handler,
+			       IRQF_SHARED, "GPIO fan alarm", fan_data);
 	if (err)
 		goto err_free_sysfs;
 
@@ -126,21 +126,14 @@ static int fan_alarm_init(struct gpio_fan_data *fan_data,
 
 err_free_sysfs:
 	device_remove_file(&pdev->dev, &dev_attr_fan1_alarm);
-err_free_gpio:
-	gpio_free(alarm->gpio);
-
 	return err;
 }
 
 static void fan_alarm_free(struct gpio_fan_data *fan_data)
 {
 	struct platform_device *pdev = fan_data->pdev;
-	int alarm_irq = gpio_to_irq(fan_data->alarm->gpio);
 
-	if (alarm_irq >= 0)
-		free_irq(alarm_irq, fan_data);
 	device_remove_file(&pdev->dev, &dev_attr_fan1_alarm);
-	gpio_free(fan_data->alarm->gpio);
 }
 
 /*
@@ -365,15 +358,14 @@ static int fan_ctrl_init(struct gpio_fan_data *fan_data,
 	int i, err;
 
 	for (i = 0; i < num_ctrl; i++) {
-		err = gpio_request(ctrl[i], "GPIO fan control");
+		err = devm_gpio_request(&pdev->dev, ctrl[i],
+					"GPIO fan control");
 		if (err)
-			goto err_free_gpio;
+			return err;
 
 		err = gpio_direction_output(ctrl[i], gpio_get_value(ctrl[i]));
-		if (err) {
-			gpio_free(ctrl[i]);
-			goto err_free_gpio;
-		}
+		if (err)
+			return err;
 	}
 
 	fan_data->num_ctrl = num_ctrl;
@@ -382,32 +374,18 @@ static int fan_ctrl_init(struct gpio_fan_data *fan_data,
 	fan_data->speed = pdata->speed;
 	fan_data->pwm_enable = true; /* Enable manual fan speed control. */
 	fan_data->speed_index = get_fan_speed_index(fan_data);
-	if (fan_data->speed_index < 0) {
-		err = -ENODEV;
-		goto err_free_gpio;
-	}
+	if (fan_data->speed_index < 0)
+		return -ENODEV;
 
 	err = sysfs_create_group(&pdev->dev.kobj, &gpio_fan_ctrl_group);
-	if (err)
-		goto err_free_gpio;
-
-	return 0;
-
-err_free_gpio:
-	for (i = i - 1; i >= 0; i--)
-		gpio_free(ctrl[i]);
-
 	return err;
 }
 
 static void fan_ctrl_free(struct gpio_fan_data *fan_data)
 {
 	struct platform_device *pdev = fan_data->pdev;
-	int i;
 
 	sysfs_remove_group(&pdev->dev.kobj, &gpio_fan_ctrl_group);
-	for (i = 0; i < fan_data->num_ctrl; i++)
-		gpio_free(fan_data->ctrl[i]);
 }
 
 /*
@@ -431,7 +409,8 @@ static int __devinit gpio_fan_probe(struct platform_device *pdev)
 	if (!pdata)
 		return -EINVAL;
 
-	fan_data = kzalloc(sizeof(struct gpio_fan_data), GFP_KERNEL);
+	fan_data = devm_kzalloc(&pdev->dev, sizeof(struct gpio_fan_data),
+				GFP_KERNEL);
 	if (!fan_data)
 		return -ENOMEM;
 
@@ -443,7 +422,7 @@ static int __devinit gpio_fan_probe(struct platform_device *pdev)
 	if (pdata->alarm) {
 		err = fan_alarm_init(fan_data, pdata->alarm);
 		if (err)
-			goto err_free_data;
+			return err;
 	}
 
 	/* Configure control GPIOs if available. */
@@ -480,10 +459,6 @@ err_free_ctrl:
 err_free_alarm:
 	if (fan_data->alarm)
 		fan_alarm_free(fan_data);
-err_free_data:
-	platform_set_drvdata(pdev, NULL);
-	kfree(fan_data);
-
 	return err;
 }
 
@@ -497,15 +472,14 @@ static int __devexit gpio_fan_remove(struct platform_device *pdev)
 		fan_alarm_free(fan_data);
 	if (fan_data->ctrl)
 		fan_ctrl_free(fan_data);
-	kfree(fan_data);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int gpio_fan_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int gpio_fan_suspend(struct device *dev)
 {
-	struct gpio_fan_data *fan_data = platform_get_drvdata(pdev);
+	struct gpio_fan_data *fan_data = dev_get_drvdata(dev);
 
 	if (fan_data->ctrl) {
 		fan_data->resume_speed = fan_data->speed_index;
@@ -515,27 +489,28 @@ static int gpio_fan_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int gpio_fan_resume(struct platform_device *pdev)
+static int gpio_fan_resume(struct device *dev)
 {
-	struct gpio_fan_data *fan_data = platform_get_drvdata(pdev);
+	struct gpio_fan_data *fan_data = dev_get_drvdata(dev);
 
 	if (fan_data->ctrl)
 		set_fan_speed(fan_data, fan_data->resume_speed);
 
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(gpio_fan_pm, gpio_fan_suspend, gpio_fan_resume);
+#define GPIO_FAN_PM	&gpio_fan_pm
 #else
-#define gpio_fan_suspend NULL
-#define gpio_fan_resume NULL
+#define GPIO_FAN_PM	NULL
 #endif
 
 static struct platform_driver gpio_fan_driver = {
 	.probe		= gpio_fan_probe,
 	.remove		= __devexit_p(gpio_fan_remove),
-	.suspend	= gpio_fan_suspend,
-	.resume		= gpio_fan_resume,
 	.driver	= {
 		.name	= "gpio-fan",
+		.pm	= GPIO_FAN_PM,
 	},
 };
 
diff --git a/drivers/hwmon/hih6130.c b/drivers/hwmon/hih6130.c
new file mode 100644
index 00000000000..e8ee75f5547
--- /dev/null
+++ b/drivers/hwmon/hih6130.c
@@ -0,0 +1,293 @@
+/* Honeywell HIH-6130/HIH-6131 humidity and temperature sensor driver
+ *
+ * Copyright (C) 2012 Iain Paton <ipaton0@gmail.com>
+ *
+ * heavily based on the sht21 driver
+ * Copyright (C) 2010 Urs Fleisch <urs.fleisch@sensirion.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., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * Data sheets available (2012-06-22) at
+ * http://sensing.honeywell.com/index.php?ci_id=3106&la_id=1&defId=44872
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+
+/**
+ * struct hih6130 - HIH-6130 device specific data
+ * @hwmon_dev: device registered with hwmon
+ * @lock: mutex to protect measurement values
+ * @valid: only false before first measurement is taken
+ * @last_update: time of last update (jiffies)
+ * @temperature: cached temperature measurement value
+ * @humidity: cached humidity measurement value
+ */
+struct hih6130 {
+	struct device *hwmon_dev;
+	struct mutex lock;
+	bool valid;
+	unsigned long last_update;
+	int temperature;
+	int humidity;
+};
+
+/**
+ * hih6130_temp_ticks_to_millicelsius() - convert raw temperature ticks to
+ * milli celsius
+ * @ticks: temperature ticks value received from sensor
+ */
+static inline int hih6130_temp_ticks_to_millicelsius(int ticks)
+{
+
+	ticks = ticks >> 2;
+	/*
+	 * from data sheet section 5.0
+	 * Formula T = ( ticks / ( 2^14 - 2 ) ) * 165 -40
+	 */
+	return (DIV_ROUND_CLOSEST(ticks * 1650, 16382) - 400) * 100;
+}
+
+/**
+ * hih6130_rh_ticks_to_per_cent_mille() - convert raw humidity ticks to
+ * one-thousandths of a percent relative humidity
+ * @ticks: humidity ticks value received from sensor
+ */
+static inline int hih6130_rh_ticks_to_per_cent_mille(int ticks)
+{
+
+	ticks &= ~0xC000; /* clear status bits */
+	/*
+	 * from data sheet section 4.0
+	 * Formula RH = ( ticks / ( 2^14 -2 ) ) * 100
+	 */
+	return DIV_ROUND_CLOSEST(ticks * 1000, 16382) * 100;
+}
+
+/**
+ * hih6130_update_measurements() - get updated measurements from device
+ * @client: I2C client device
+ *
+ * Returns 0 on success, else negative errno.
+ */
+static int hih6130_update_measurements(struct i2c_client *client)
+{
+	int ret = 0;
+	int t;
+	struct hih6130 *hih6130 = i2c_get_clientdata(client);
+	unsigned char tmp[4];
+	struct i2c_msg msgs[1] = {
+		{
+			.addr = client->addr,
+			.flags = I2C_M_RD,
+			.len = 4,
+			.buf = tmp,
+		}
+	};
+
+	mutex_lock(&hih6130->lock);
+
+	/*
+	 * While the measurement can be completed in ~40ms the sensor takes
+	 * much longer to react to a change in external conditions. How quickly
+	 * it reacts depends on airflow and other factors outwith our control.
+	 * The datasheet specifies maximum 'Response time' for humidity at 8s
+	 * and temperature at 30s under specified conditions.
+	 * We therefore choose to only read the sensor at most once per second.
+	 * This trades off pointless activity polling the sensor much faster
+	 * than it can react against better response times in conditions more
+	 * favourable than specified in the datasheet.
+	 */
+	if (time_after(jiffies, hih6130->last_update + HZ) || !hih6130->valid) {
+
+		/* write to slave address, no data, to request a measurement */
+		ret = i2c_master_send(client, tmp, 0);
+		if (ret < 0)
+			goto out;
+
+		/* measurement cycle time is ~36.65msec */
+		msleep(40);
+
+		ret = i2c_transfer(client->adapter, msgs, 1);
+		if (ret < 0)
+			goto out;
+
+		if ((tmp[0] & 0xC0) != 0) {
+			dev_err(&client->dev, "Error while reading measurement result\n");
+			ret = -EIO;
+			goto out;
+		}
+
+		t = (tmp[0] << 8) + tmp[1];
+		hih6130->humidity = hih6130_rh_ticks_to_per_cent_mille(t);
+
+		t = (tmp[2] << 8) + tmp[3];
+		hih6130->temperature = hih6130_temp_ticks_to_millicelsius(t);
+
+		hih6130->last_update = jiffies;
+		hih6130->valid = true;
+	}
+out:
+	mutex_unlock(&hih6130->lock);
+
+	return ret >= 0 ? 0 : ret;
+}
+
+/**
+ * hih6130_show_temperature() - show temperature measurement value in sysfs
+ * @dev: device
+ * @attr: device attribute
+ * @buf: sysfs buffer (PAGE_SIZE) where measurement values are written to
+ *
+ * Will be called on read access to temp1_input sysfs attribute.
+ * Returns number of bytes written into buffer, negative errno on error.
+ */
+static ssize_t hih6130_show_temperature(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct hih6130 *hih6130 = i2c_get_clientdata(client);
+	int ret = hih6130_update_measurements(client);
+	if (ret < 0)
+		return ret;
+	return sprintf(buf, "%d\n", hih6130->temperature);
+}
+
+/**
+ * hih6130_show_humidity() - show humidity measurement value in sysfs
+ * @dev: device
+ * @attr: device attribute
+ * @buf: sysfs buffer (PAGE_SIZE) where measurement values are written to
+ *
+ * Will be called on read access to humidity1_input sysfs attribute.
+ * Returns number of bytes written into buffer, negative errno on error.
+ */
+static ssize_t hih6130_show_humidity(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct hih6130 *hih6130 = i2c_get_clientdata(client);
+	int ret = hih6130_update_measurements(client);
+	if (ret < 0)
+		return ret;
+	return sprintf(buf, "%d\n", hih6130->humidity);
+}
+
+/* sysfs attributes */
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, hih6130_show_temperature,
+	NULL, 0);
+static SENSOR_DEVICE_ATTR(humidity1_input, S_IRUGO, hih6130_show_humidity,
+	NULL, 0);
+
+static struct attribute *hih6130_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_humidity1_input.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group hih6130_attr_group = {
+	.attrs = hih6130_attributes,
+};
+
+/**
+ * hih6130_probe() - probe device
+ * @client: I2C client device
+ * @id: device ID
+ *
+ * Called by the I2C core when an entry in the ID table matches a
+ * device's name.
+ * Returns 0 on success.
+ */
+static int __devinit hih6130_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	struct hih6130 *hih6130;
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "adapter does not support true I2C\n");
+		return -ENODEV;
+	}
+
+	hih6130 = devm_kzalloc(&client->dev, sizeof(*hih6130), GFP_KERNEL);
+	if (!hih6130)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, hih6130);
+
+	mutex_init(&hih6130->lock);
+
+	err = sysfs_create_group(&client->dev.kobj, &hih6130_attr_group);
+	if (err) {
+		dev_dbg(&client->dev, "could not create sysfs files\n");
+		return err;
+	}
+
+	hih6130->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(hih6130->hwmon_dev)) {
+		dev_dbg(&client->dev, "unable to register hwmon device\n");
+		err = PTR_ERR(hih6130->hwmon_dev);
+		goto fail_remove_sysfs;
+	}
+
+	return 0;
+
+fail_remove_sysfs:
+	sysfs_remove_group(&client->dev.kobj, &hih6130_attr_group);
+	return err;
+}
+
+/**
+ * hih6130_remove() - remove device
+ * @client: I2C client device
+ */
+static int __devexit hih6130_remove(struct i2c_client *client)
+{
+	struct hih6130 *hih6130 = i2c_get_clientdata(client);
+
+	hwmon_device_unregister(hih6130->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &hih6130_attr_group);
+
+	return 0;
+}
+
+/* Device ID table */
+static const struct i2c_device_id hih6130_id[] = {
+	{ "hih6130", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, hih6130_id);
+
+static struct i2c_driver hih6130_driver = {
+	.driver.name = "hih6130",
+	.probe       = hih6130_probe,
+	.remove      = __devexit_p(hih6130_remove),
+	.id_table    = hih6130_id,
+};
+
+module_i2c_driver(hih6130_driver);
+
+MODULE_AUTHOR("Iain Paton <ipaton0@gmail.com>");
+MODULE_DESCRIPTION("Honeywell HIH-6130 humidity and temperature sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/jc42.c b/drivers/hwmon/jc42.c
index e72ba5d2a82..e21e43c1315 100644
--- a/drivers/hwmon/jc42.c
+++ b/drivers/hwmon/jc42.c
@@ -57,7 +57,7 @@ static const unsigned short normal_i2c[] = {
 #define JC42_CFG_EVENT_LOCK	(1 << 7)
 #define JC42_CFG_SHUTDOWN	(1 << 8)
 #define JC42_CFG_HYST_SHIFT	9
-#define JC42_CFG_HYST_MASK	0x03
+#define JC42_CFG_HYST_MASK	(0x03 << 9)
 
 /* Capabilities */
 #define JC42_CAP_RANGE		(1 << 2)
@@ -287,8 +287,8 @@ static ssize_t show_temp_crit_hyst(struct device *dev,
 		return PTR_ERR(data);
 
 	temp = jc42_temp_from_reg(data->temp_crit);
-	hyst = jc42_hysteresis[(data->config >> JC42_CFG_HYST_SHIFT)
-			       & JC42_CFG_HYST_MASK];
+	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
+			       >> JC42_CFG_HYST_SHIFT];
 	return sprintf(buf, "%d\n", temp - hyst);
 }
 
@@ -302,8 +302,8 @@ static ssize_t show_temp_max_hyst(struct device *dev,
 		return PTR_ERR(data);
 
 	temp = jc42_temp_from_reg(data->temp_max);
-	hyst = jc42_hysteresis[(data->config >> JC42_CFG_HYST_SHIFT)
-			       & JC42_CFG_HYST_MASK];
+	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
+			       >> JC42_CFG_HYST_SHIFT];
 	return sprintf(buf, "%d\n", temp - hyst);
 }
 
@@ -362,8 +362,7 @@ static ssize_t set_temp_crit_hyst(struct device *dev,
 	}
 
 	mutex_lock(&data->update_lock);
-	data->config = (data->config
-			& ~(JC42_CFG_HYST_MASK << JC42_CFG_HYST_SHIFT))
+	data->config = (data->config & ~JC42_CFG_HYST_MASK)
 	  | (hyst << JC42_CFG_HYST_SHIFT);
 	err = i2c_smbus_write_word_swapped(client, JC42_REG_CONFIG,
 					   data->config);
@@ -535,9 +534,16 @@ static int jc42_remove(struct i2c_client *client)
 	struct jc42_data *data = i2c_get_clientdata(client);
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &jc42_group);
-	if (data->config != data->orig_config)
-		i2c_smbus_write_word_swapped(client, JC42_REG_CONFIG,
-					     data->orig_config);
+
+	/* Restore original configuration except hysteresis */
+	if ((data->config & ~JC42_CFG_HYST_MASK) !=
+	    (data->orig_config & ~JC42_CFG_HYST_MASK)) {
+		int config;
+
+		config = (data->orig_config & ~JC42_CFG_HYST_MASK)
+		  | (data->config & JC42_CFG_HYST_MASK);
+		i2c_smbus_write_word_swapped(client, JC42_REG_CONFIG, config);
+	}
 	return 0;
 }
 
diff --git a/drivers/hwmon/k10temp.c b/drivers/hwmon/k10temp.c
index 7356b5ec8f6..f2fe8078633 100644
--- a/drivers/hwmon/k10temp.c
+++ b/drivers/hwmon/k10temp.c
@@ -33,9 +33,6 @@ static bool force;
 module_param(force, bool, 0444);
 MODULE_PARM_DESC(force, "force loading on processors with erratum 319");
 
-/* PCI-IDs for Northbridge devices not used anywhere else */
-#define PCI_DEVICE_ID_AMD_15H_M10H_NB_F3	0x1403
-
 /* CPUID function 0x80000001, ebx */
 #define CPUID_PKGTYPE_MASK	0xf0000000
 #define CPUID_PKGTYPE_F		0x00000000
@@ -213,7 +210,7 @@ static DEFINE_PCI_DEVICE_TABLE(k10temp_id_table) = {
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_11H_NB_MISC) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_CNB17H_F3) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_NB_F3) },
-	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M10H_NB_F3) },
+	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M10H_F3) },
 	{}
 };
 MODULE_DEVICE_TABLE(pci, k10temp_id_table);
diff --git a/drivers/hwmon/k8temp.c b/drivers/hwmon/k8temp.c
index 35aac82ee8e..49a69c5b3b8 100644
--- a/drivers/hwmon/k8temp.c
+++ b/drivers/hwmon/k8temp.c
@@ -183,21 +183,17 @@ static int __devinit k8temp_probe(struct pci_dev *pdev,
 	u8 model, stepping;
 	struct k8temp_data *data;
 
-	data = kzalloc(sizeof(struct k8temp_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&pdev->dev, sizeof(struct k8temp_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	model = boot_cpu_data.x86_model;
 	stepping = boot_cpu_data.x86_mask;
 
 	/* feature available since SH-C0, exclude older revisions */
-	if (((model == 4) && (stepping == 0)) ||
-	    ((model == 5) && (stepping <= 1))) {
-		err = -ENODEV;
-		goto exit_free;
-	}
+	if ((model == 4 && stepping == 0) ||
+	    (model == 5 && stepping <= 1))
+		return -ENODEV;
 
 	/*
 	 * AMD NPT family 0fh, i.e. RevF and RevG:
@@ -224,8 +220,7 @@ static int __devinit k8temp_probe(struct pci_dev *pdev,
 
 	if (scfg & (SEL_PLACE | SEL_CORE)) {
 		dev_err(&pdev->dev, "Configuration bit(s) stuck at 1!\n");
-		err = -ENODEV;
-		goto exit_free;
+		return -ENODEV;
 	}
 
 	scfg |= (SEL_PLACE | SEL_CORE);
@@ -307,10 +302,6 @@ exit_remove:
 	device_remove_file(&pdev->dev,
 			   &sensor_dev_attr_temp4_input.dev_attr);
 	device_remove_file(&pdev->dev, &dev_attr_name);
-exit_free:
-	pci_set_drvdata(pdev, NULL);
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -328,8 +319,6 @@ static void __devexit k8temp_remove(struct pci_dev *pdev)
 	device_remove_file(&pdev->dev,
 			   &sensor_dev_attr_temp4_input.dev_attr);
 	device_remove_file(&pdev->dev, &dev_attr_name);
-	pci_set_drvdata(pdev, NULL);
-	kfree(data);
 }
 
 static struct pci_driver k8temp_driver = {
diff --git a/drivers/hwmon/lm63.c b/drivers/hwmon/lm63.c
index 602a0f0b0de..eed4d940178 100644
--- a/drivers/hwmon/lm63.c
+++ b/drivers/hwmon/lm63.c
@@ -1108,11 +1108,9 @@ static int lm63_probe(struct i2c_client *client,
 	struct lm63_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm63_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct lm63_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->valid = 0;
@@ -1129,7 +1127,7 @@ static int lm63_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &lm63_group);
 	if (err)
-		goto exit_free;
+		return err;
 	if (data->config & 0x04) { /* tachometer enabled */
 		err = sysfs_create_group(&client->dev.kobj, &lm63_group_fan1);
 		if (err)
@@ -1161,9 +1159,6 @@ exit_remove_files:
 		device_remove_file(&client->dev, &dev_attr_temp2_type);
 		sysfs_remove_group(&client->dev.kobj, &lm63_group_extra_lut);
 	}
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -1179,7 +1174,6 @@ static int lm63_remove(struct i2c_client *client)
 		sysfs_remove_group(&client->dev.kobj, &lm63_group_extra_lut);
 	}
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index a83f206af24..291edfff55b 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -156,7 +156,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 			I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA))
 		return -EIO;
 
-	data = kzalloc(sizeof(struct lm75_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct lm75_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -174,7 +174,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	status = lm75_read_value(client, LM75_REG_CONF);
 	if (status < 0) {
 		dev_dbg(&client->dev, "Can't read config? %d\n", status);
-		goto exit_free;
+		return status;
 	}
 	data->orig_conf = status;
 	new = status & ~clr_mask;
@@ -186,7 +186,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* Register sysfs hooks */
 	status = sysfs_create_group(&client->dev.kobj, &lm75_group);
 	if (status)
-		goto exit_free;
+		return status;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -201,8 +201,6 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &lm75_group);
-exit_free:
-	kfree(data);
 	return status;
 }
 
@@ -213,7 +211,6 @@ static int lm75_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &lm75_group);
 	lm75_write_value(client, LM75_REG_CONF, data->orig_conf);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm77.c b/drivers/hwmon/lm77.c
index 0fca8613e7d..f82acf67acf 100644
--- a/drivers/hwmon/lm77.c
+++ b/drivers/hwmon/lm77.c
@@ -267,10 +267,9 @@ static const struct attribute_group lm77_group = {
 };
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
-static int lm77_detect(struct i2c_client *new_client,
-		       struct i2c_board_info *info)
+static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info)
 {
-	struct i2c_adapter *adapter = new_client->adapter;
+	struct i2c_adapter *adapter = client->adapter;
 	int i, cur, conf, hyst, crit, min, max;
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA |
@@ -292,18 +291,18 @@ static int lm77_detect(struct i2c_client *new_client,
 	 */
 
 	/* addresses cycling */
-	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);
-	crit = i2c_smbus_read_word_data(new_client, 3);
-	min = i2c_smbus_read_word_data(new_client, 4);
-	max = i2c_smbus_read_word_data(new_client, 5);
+	cur = i2c_smbus_read_word_data(client, 0);
+	conf = i2c_smbus_read_byte_data(client, 1);
+	hyst = i2c_smbus_read_word_data(client, 2);
+	crit = i2c_smbus_read_word_data(client, 3);
+	min = i2c_smbus_read_word_data(client, 4);
+	max = i2c_smbus_read_word_data(client, 5);
 	for (i = 8; i <= 0xff; i += 8) {
-		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) != crit
-		 || i2c_smbus_read_word_data(new_client, i + 4) != min
-		 || i2c_smbus_read_word_data(new_client, i + 5) != max)
+		if (i2c_smbus_read_byte_data(client, i + 1) != conf
+		 || i2c_smbus_read_word_data(client, i + 2) != hyst
+		 || i2c_smbus_read_word_data(client, i + 3) != crit
+		 || i2c_smbus_read_word_data(client, i + 4) != min
+		 || i2c_smbus_read_word_data(client, i + 5) != max)
 			return -ENODEV;
 	}
 
@@ -320,17 +319,17 @@ static int lm77_detect(struct i2c_client *new_client,
 		return -ENODEV;
 
 	/* 0x06 and 0x07 return the last read value */
-	cur = i2c_smbus_read_word_data(new_client, 0);
-	if (i2c_smbus_read_word_data(new_client, 6) != cur
-	 || i2c_smbus_read_word_data(new_client, 7) != cur)
+	cur = i2c_smbus_read_word_data(client, 0);
+	if (i2c_smbus_read_word_data(client, 6) != cur
+	 || i2c_smbus_read_word_data(client, 7) != cur)
 		return -ENODEV;
-	hyst = i2c_smbus_read_word_data(new_client, 2);
-	if (i2c_smbus_read_word_data(new_client, 6) != hyst
-	 || i2c_smbus_read_word_data(new_client, 7) != hyst)
+	hyst = i2c_smbus_read_word_data(client, 2);
+	if (i2c_smbus_read_word_data(client, 6) != hyst
+	 || i2c_smbus_read_word_data(client, 7) != hyst)
 		return -ENODEV;
-	min = i2c_smbus_read_word_data(new_client, 4);
-	if (i2c_smbus_read_word_data(new_client, 6) != min
-	 || i2c_smbus_read_word_data(new_client, 7) != min)
+	min = i2c_smbus_read_word_data(client, 4);
+	if (i2c_smbus_read_word_data(client, 6) != min
+	 || i2c_smbus_read_word_data(client, 7) != min)
 		return -ENODEV;
 
 	strlcpy(info->type, "lm77", I2C_NAME_SIZE);
@@ -338,31 +337,29 @@ static int lm77_detect(struct i2c_client *new_client,
 	return 0;
 }
 
-static int lm77_probe(struct i2c_client *new_client,
-		      const struct i2c_device_id *id)
+static int lm77_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
 	struct lm77_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm77_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(dev, sizeof(struct lm77_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	i2c_set_clientdata(new_client, data);
+	i2c_set_clientdata(client, data);
 	data->valid = 0;
 	mutex_init(&data->update_lock);
 
 	/* Initialize the LM77 chip */
-	lm77_init_client(new_client);
+	lm77_init_client(client);
 
 	/* Register sysfs hooks */
-	err = sysfs_create_group(&new_client->dev.kobj, &lm77_group);
+	err = sysfs_create_group(&dev->kobj, &lm77_group);
 	if (err)
-		goto exit_free;
+		return err;
 
-	data->hwmon_dev = hwmon_device_register(&new_client->dev);
+	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
 		goto exit_remove;
@@ -371,10 +368,7 @@ static int lm77_probe(struct i2c_client *new_client,
 	return 0;
 
 exit_remove:
-	sysfs_remove_group(&new_client->dev.kobj, &lm77_group);
-exit_free:
-	kfree(data);
-exit:
+	sysfs_remove_group(&dev->kobj, &lm77_group);
 	return err;
 }
 
@@ -383,7 +377,6 @@ static int lm77_remove(struct i2c_client *client)
 	struct lm77_data *data = i2c_get_clientdata(client);
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &lm77_group);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm78.c b/drivers/hwmon/lm78.c
index f6bc414e1e9..c6ffafe600a 100644
--- a/drivers/hwmon/lm78.c
+++ b/drivers/hwmon/lm78.c
@@ -660,7 +660,7 @@ static int lm78_i2c_probe(struct i2c_client *client,
 	struct lm78_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm78_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct lm78_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -674,20 +674,18 @@ static int lm78_i2c_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &lm78_group);
 	if (err)
-		goto ERROR3;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
-		goto ERROR4;
+		goto error;
 	}
 
 	return 0;
 
-ERROR4:
+error:
 	sysfs_remove_group(&client->dev.kobj, &lm78_group);
-ERROR3:
-	kfree(data);
 	return err;
 }
 
@@ -697,7 +695,6 @@ static int lm78_i2c_remove(struct i2c_client *client)
 
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &lm78_group);
-	kfree(data);
 
 	return 0;
 }
@@ -844,16 +841,14 @@ static int __devinit lm78_isa_probe(struct platform_device *pdev)
 
 	/* Reserve the ISA region */
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start + LM78_ADDR_REG_OFFSET, 2, "lm78")) {
-		err = -EBUSY;
-		goto exit;
-	}
+	if (!devm_request_region(&pdev->dev, res->start + LM78_ADDR_REG_OFFSET,
+				 2, "lm78"))
+		return -EBUSY;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(struct lm78_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	data = kzalloc(sizeof(struct lm78_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit_release_region;
-	}
 	mutex_init(&data->lock);
 	data->isa_addr = res->start;
 	platform_set_drvdata(pdev, data);
@@ -888,25 +883,16 @@ static int __devinit lm78_isa_probe(struct platform_device *pdev)
  exit_remove_files:
 	sysfs_remove_group(&pdev->dev.kobj, &lm78_group);
 	device_remove_file(&pdev->dev, &dev_attr_name);
-	kfree(data);
- exit_release_region:
-	release_region(res->start + LM78_ADDR_REG_OFFSET, 2);
- exit:
 	return err;
 }
 
 static int __devexit lm78_isa_remove(struct platform_device *pdev)
 {
 	struct lm78_data *data = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&pdev->dev.kobj, &lm78_group);
 	device_remove_file(&pdev->dev, &dev_attr_name);
-	kfree(data);
-
-	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	release_region(res->start + LM78_ADDR_REG_OFFSET, 2);
 
 	return 0;
 }
diff --git a/drivers/hwmon/lm80.c b/drivers/hwmon/lm80.c
index e2c43e1774b..28a8b71f457 100644
--- a/drivers/hwmon/lm80.c
+++ b/drivers/hwmon/lm80.c
@@ -543,11 +543,9 @@ static int lm80_probe(struct i2c_client *client,
 	struct lm80_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm80_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct lm80_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -562,7 +560,7 @@ static int lm80_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &lm80_group);
 	if (err)
-		goto error_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -574,9 +572,6 @@ static int lm80_probe(struct i2c_client *client,
 
 error_remove:
 	sysfs_remove_group(&client->dev.kobj, &lm80_group);
-error_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -587,7 +582,6 @@ static int lm80_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &lm80_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm83.c b/drivers/hwmon/lm83.c
index cd45b9d8558..e998034f1f1 100644
--- a/drivers/hwmon/lm83.c
+++ b/drivers/hwmon/lm83.c
@@ -343,11 +343,10 @@ static int lm83_probe(struct i2c_client *new_client,
 	struct lm83_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm83_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&new_client->dev, sizeof(struct lm83_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(new_client, data);
 	data->valid = 0;
@@ -362,7 +361,7 @@ static int lm83_probe(struct i2c_client *new_client,
 
 	err = sysfs_create_group(&new_client->dev.kobj, &lm83_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	if (id->driver_data == lm83) {
 		err = sysfs_create_group(&new_client->dev.kobj,
@@ -382,9 +381,6 @@ static int lm83_probe(struct i2c_client *new_client,
 exit_remove_files:
 	sysfs_remove_group(&new_client->dev.kobj, &lm83_group);
 	sysfs_remove_group(&new_client->dev.kobj, &lm83_group_opt);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -396,7 +392,6 @@ static int lm83_remove(struct i2c_client *client)
 	sysfs_remove_group(&client->dev.kobj, &lm83_group);
 	sysfs_remove_group(&client->dev.kobj, &lm83_group_opt);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm85.c b/drivers/hwmon/lm85.c
index 864c7d999e0..9f2dd77e1e0 100644
--- a/drivers/hwmon/lm85.c
+++ b/drivers/hwmon/lm85.c
@@ -1387,7 +1387,7 @@ static int lm85_probe(struct i2c_client *client,
 	struct lm85_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm85_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct lm85_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -1419,7 +1419,7 @@ static int lm85_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &lm85_group);
 	if (err)
-		goto err_kfree;
+		return err;
 
 	/* minctl and temp_off exist on all chips except emc6d103s */
 	if (data->type != emc6d103s) {
@@ -1466,8 +1466,6 @@ static int lm85_probe(struct i2c_client *client,
 	/* Error out and cleanup code */
  err_remove_files:
 	lm85_remove_files(client, data);
- err_kfree:
-	kfree(data);
 	return err;
 }
 
@@ -1476,7 +1474,6 @@ static int lm85_remove(struct i2c_client *client)
 	struct lm85_data *data = i2c_get_clientdata(client);
 	hwmon_device_unregister(data->hwmon_dev);
 	lm85_remove_files(client, data);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm87.c b/drivers/hwmon/lm87.c
index 314d147bf1a..16e45d70215 100644
--- a/drivers/hwmon/lm87.c
+++ b/drivers/hwmon/lm87.c
@@ -898,11 +898,9 @@ static int lm87_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	struct lm87_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm87_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct lm87_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->valid = 0;
@@ -923,7 +921,7 @@ static int lm87_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &lm87_group);
 	if (err)
-		goto exit_free;
+		goto exit_stop;
 
 	if (data->channel & CHAN_NO_FAN(0)) {
 		err = sysfs_create_group(&client->dev.kobj, &lm87_group_in6);
@@ -972,10 +970,8 @@ static int lm87_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 exit_remove:
 	lm87_remove_files(client);
-exit_free:
+exit_stop:
 	lm87_write_value(client, LM87_REG_CONFIG, data->config);
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -987,7 +983,6 @@ static int lm87_remove(struct i2c_client *client)
 	lm87_remove_files(client);
 
 	lm87_write_value(client, LM87_REG_CONFIG, data->config);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm90.c b/drivers/hwmon/lm90.c
index 22b14a68e35..863412a02bd 100644
--- a/drivers/hwmon/lm90.c
+++ b/drivers/hwmon/lm90.c
@@ -1399,11 +1399,10 @@ static int lm90_probe(struct i2c_client *client,
 	struct lm90_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm90_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct lm90_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
 
@@ -1474,8 +1473,6 @@ exit_remove_files:
 	lm90_remove_files(client, data);
 exit_restore:
 	lm90_restore_conf(client, data);
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -1487,7 +1484,6 @@ static int lm90_remove(struct i2c_client *client)
 	lm90_remove_files(client, data);
 	lm90_restore_conf(client, data);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm92.c b/drivers/hwmon/lm92.c
index fdc691a4028..2282d77e83e 100644
--- a/drivers/hwmon/lm92.c
+++ b/drivers/hwmon/lm92.c
@@ -373,11 +373,10 @@ static int lm92_probe(struct i2c_client *new_client,
 	struct lm92_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct lm92_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&new_client->dev, sizeof(struct lm92_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(new_client, data);
 	data->valid = 0;
@@ -389,7 +388,7 @@ static int lm92_probe(struct i2c_client *new_client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&new_client->dev.kobj, &lm92_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&new_client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -401,9 +400,6 @@ static int lm92_probe(struct i2c_client *new_client,
 
 exit_remove:
 	sysfs_remove_group(&new_client->dev.kobj, &lm92_group);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -414,7 +410,6 @@ static int lm92_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &lm92_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/lm93.c b/drivers/hwmon/lm93.c
index 67e8fe256e0..bf946187bd3 100644
--- a/drivers/hwmon/lm93.c
+++ b/drivers/hwmon/lm93.c
@@ -2738,15 +2738,13 @@ static int lm93_probe(struct i2c_client *client,
 	} else {
 		dev_dbg(&client->dev, "detect failed, "
 			"smbus byte and/or word data not supported!\n");
-		err = -ENODEV;
-		goto err_out;
+		return -ENODEV;
 	}
 
-	data = kzalloc(sizeof(struct lm93_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct lm93_data), GFP_KERNEL);
 	if (!data) {
 		dev_dbg(&client->dev, "out of memory!\n");
-		err = -ENOMEM;
-		goto err_out;
+		return -ENOMEM;
 	}
 	i2c_set_clientdata(client, data);
 
@@ -2760,7 +2758,7 @@ static int lm93_probe(struct i2c_client *client,
 
 	err = sysfs_create_group(&client->dev.kobj, &lm93_attr_grp);
 	if (err)
-		goto err_free;
+		return err;
 
 	/* Register hwmon driver class */
 	data->hwmon_dev = hwmon_device_register(&client->dev);
@@ -2770,9 +2768,6 @@ static int lm93_probe(struct i2c_client *client,
 	err = PTR_ERR(data->hwmon_dev);
 	dev_err(&client->dev, "error registering hwmon device.\n");
 	sysfs_remove_group(&client->dev.kobj, &lm93_attr_grp);
-err_free:
-	kfree(data);
-err_out:
 	return err;
 }
 
@@ -2783,7 +2778,6 @@ static int lm93_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &lm93_attr_grp);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/max1111.c b/drivers/hwmon/max1111.c
index 362a40eb612..f3978a46e84 100644
--- a/drivers/hwmon/max1111.c
+++ b/drivers/hwmon/max1111.c
@@ -168,7 +168,7 @@ static int __devinit max1111_probe(struct spi_device *spi)
 	if (err < 0)
 		return err;
 
-	data = kzalloc(sizeof(struct max1111_data), GFP_KERNEL);
+	data = devm_kzalloc(&spi->dev, sizeof(struct max1111_data), GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&spi->dev, "failed to allocate memory\n");
 		return -ENOMEM;
@@ -176,7 +176,7 @@ static int __devinit max1111_probe(struct spi_device *spi)
 
 	err = setup_transfer(data);
 	if (err)
-		goto err_free_data;
+		return err;
 
 	mutex_init(&data->drvdata_lock);
 
@@ -186,7 +186,7 @@ static int __devinit max1111_probe(struct spi_device *spi)
 	err = sysfs_create_group(&spi->dev.kobj, &max1111_attr_group);
 	if (err) {
 		dev_err(&spi->dev, "failed to create attribute group\n");
-		goto err_free_data;
+		return err;
 	}
 
 	data->hwmon_dev = hwmon_device_register(&spi->dev);
@@ -203,8 +203,6 @@ static int __devinit max1111_probe(struct spi_device *spi)
 
 err_remove:
 	sysfs_remove_group(&spi->dev.kobj, &max1111_attr_group);
-err_free_data:
-	kfree(data);
 	return err;
 }
 
@@ -215,7 +213,6 @@ static int __devexit max1111_remove(struct spi_device *spi)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&spi->dev.kobj, &max1111_attr_group);
 	mutex_destroy(&data->drvdata_lock);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/max1619.c b/drivers/hwmon/max1619.c
index ecac04a7b7d..6c11ec21407 100644
--- a/drivers/hwmon/max1619.c
+++ b/drivers/hwmon/max1619.c
@@ -267,11 +267,10 @@ static int max1619_probe(struct i2c_client *new_client,
 	struct max1619_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct max1619_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&new_client->dev, sizeof(struct max1619_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(new_client, data);
 	data->valid = 0;
@@ -283,7 +282,7 @@ static int max1619_probe(struct i2c_client *new_client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&new_client->dev.kobj, &max1619_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&new_client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -295,9 +294,6 @@ static int max1619_probe(struct i2c_client *new_client,
 
 exit_remove_files:
 	sysfs_remove_group(&new_client->dev.kobj, &max1619_group);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -323,7 +319,6 @@ static int max1619_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &max1619_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/max6639.c b/drivers/hwmon/max6639.c
index de8f7adaccb..6e60036abfa 100644
--- a/drivers/hwmon/max6639.c
+++ b/drivers/hwmon/max6639.c
@@ -548,11 +548,10 @@ static int max6639_probe(struct i2c_client *client,
 	struct max6639_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct max6639_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct max6639_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -560,12 +559,12 @@ static int max6639_probe(struct i2c_client *client,
 	/* Initialize the max6639 chip */
 	err = max6639_init_client(client);
 	if (err < 0)
-		goto error_free;
+		return err;
 
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &max6639_group);
 	if (err)
-		goto error_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -579,9 +578,6 @@ static int max6639_probe(struct i2c_client *client,
 
 error_remove:
 	sysfs_remove_group(&client->dev.kobj, &max6639_group);
-error_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -592,7 +588,6 @@ static int max6639_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &max6639_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/max6642.c b/drivers/hwmon/max6642.c
index 4298909a41f..bf236c0782b 100644
--- a/drivers/hwmon/max6642.c
+++ b/drivers/hwmon/max6642.c
@@ -286,11 +286,10 @@ static int max6642_probe(struct i2c_client *new_client,
 	struct max6642_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct max6642_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&new_client->dev, sizeof(struct max6642_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(new_client, data);
 	mutex_init(&data->update_lock);
@@ -301,7 +300,7 @@ static int max6642_probe(struct i2c_client *new_client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&new_client->dev.kobj, &max6642_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&new_client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -313,9 +312,6 @@ static int max6642_probe(struct i2c_client *new_client,
 
 exit_remove_files:
 	sysfs_remove_group(&new_client->dev.kobj, &max6642_group);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -326,7 +322,6 @@ static int max6642_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &max6642_group);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/max6650.c b/drivers/hwmon/max6650.c
index 33a8a7f15e1..f739f83bafb 100644
--- a/drivers/hwmon/max6650.c
+++ b/drivers/hwmon/max6650.c
@@ -545,7 +545,8 @@ static int max6650_probe(struct i2c_client *client,
 	struct max6650_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct max6650_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct max6650_data),
+			    GFP_KERNEL);
 	if (!data) {
 		dev_err(&client->dev, "out of memory.\n");
 		return -ENOMEM;
@@ -560,11 +561,11 @@ static int max6650_probe(struct i2c_client *client,
 	 */
 	err = max6650_init_client(client);
 	if (err)
-		goto err_free;
+		return err;
 
 	err = sysfs_create_group(&client->dev.kobj, &max6650_attr_grp);
 	if (err)
-		goto err_free;
+		return err;
 	/* 3 additional fan inputs for the MAX6651 */
 	if (data->nr_fans == 4) {
 		err = sysfs_create_group(&client->dev.kobj, &max6651_attr_grp);
@@ -582,8 +583,6 @@ static int max6650_probe(struct i2c_client *client,
 		sysfs_remove_group(&client->dev.kobj, &max6651_attr_grp);
 err_remove:
 	sysfs_remove_group(&client->dev.kobj, &max6650_attr_grp);
-err_free:
-	kfree(data);
 	return err;
 }
 
@@ -595,7 +594,6 @@ static int max6650_remove(struct i2c_client *client)
 	if (data->nr_fans == 4)
 		sysfs_remove_group(&client->dev.kobj, &max6651_attr_grp);
 	sysfs_remove_group(&client->dev.kobj, &max6650_attr_grp);
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/mc13783-adc.c b/drivers/hwmon/mc13783-adc.c
index ce86c5e3c2c..cf47a59657a 100644
--- a/drivers/hwmon/mc13783-adc.c
+++ b/drivers/hwmon/mc13783-adc.c
@@ -179,7 +179,7 @@ static int __init mc13783_adc_probe(struct platform_device *pdev)
 	const struct platform_device_id *id = platform_get_device_id(pdev);
 	char *dash;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -194,7 +194,7 @@ static int __init mc13783_adc_probe(struct platform_device *pdev)
 	/* Register sysfs hooks */
 	ret = sysfs_create_group(&pdev->dev.kobj, &mc13783_group_base);
 	if (ret)
-		goto out_err_create_base;
+		return ret;
 
 	if (id->driver_data & MC13783_ADC_16CHANS) {
 		ret = sysfs_create_group(&pdev->dev.kobj,
@@ -230,11 +230,6 @@ out_err_create_ts:
 out_err_create_16chans:
 
 	sysfs_remove_group(&pdev->dev.kobj, &mc13783_group_base);
-out_err_create_base:
-
-	platform_set_drvdata(pdev, NULL);
-	kfree(priv);
-
 	return ret;
 }
 
@@ -253,9 +248,6 @@ static int __devexit mc13783_adc_remove(struct platform_device *pdev)
 
 	sysfs_remove_group(&pdev->dev.kobj, &mc13783_group_base);
 
-	platform_set_drvdata(pdev, NULL);
-	kfree(priv);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 6da9696e182..74a6c58d021 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -351,7 +351,7 @@ static int __devinit ntc_thermistor_probe(struct platform_device *pdev)
 
 	data->dev = &pdev->dev;
 	data->pdata = pdata;
-	strncpy(data->name, pdev->id_entry->name, PLATFORM_NAME_SIZE);
+	strlcpy(data->name, pdev->id_entry->name, sizeof(data->name));
 
 	switch (pdev->id_entry->driver_data) {
 	case TYPE_NCPXXWB473:
diff --git a/drivers/hwmon/pc87360.c b/drivers/hwmon/pc87360.c
index 79ba48c8c11..91d5b2a21dd 100644
--- a/drivers/hwmon/pc87360.c
+++ b/drivers/hwmon/pc87360.c
@@ -1230,7 +1230,7 @@ static int __devinit pc87360_probe(struct platform_device *pdev)
 	int use_thermistors = 0;
 	struct device *dev = &pdev->dev;
 
-	data = kzalloc(sizeof(struct pc87360_data), GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct pc87360_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -1269,15 +1269,12 @@ static int __devinit pc87360_probe(struct platform_device *pdev)
 	for (i = 0; i < LDNI_MAX; i++) {
 		data->address[i] = extra_isa[i];
 		if (data->address[i]
-		 && !request_region(extra_isa[i], PC87360_EXTENT,
-				    pc87360_driver.driver.name)) {
+		 && !devm_request_region(dev, extra_isa[i], PC87360_EXTENT,
+					 pc87360_driver.driver.name)) {
 			dev_err(dev, "Region 0x%x-0x%x already "
 				"in use!\n", extra_isa[i],
 				extra_isa[i]+PC87360_EXTENT-1);
-			for (i--; i >= 0; i--)
-				release_region(extra_isa[i], PC87360_EXTENT);
-			err = -EBUSY;
-			goto ERROR1;
+			return -EBUSY;
 		}
 	}
 
@@ -1325,13 +1322,13 @@ static int __devinit pc87360_probe(struct platform_device *pdev)
 	if (data->innr) {
 		err = sysfs_create_group(&dev->kobj, &pc8736x_vin_group);
 		if (err)
-			goto ERROR3;
+			goto error;
 	}
 
 	if (data->innr == 14) {
 		err = sysfs_create_group(&dev->kobj, &pc8736x_therm_group);
 		if (err)
-			goto ERROR3;
+			goto error;
 	}
 
 	/* create device attr-files for varying sysfs groups */
@@ -1341,11 +1338,11 @@ static int __devinit pc87360_probe(struct platform_device *pdev)
 			err = sysfs_create_group(&dev->kobj,
 						 &pc8736x_temp_attr_group[i]);
 			if (err)
-				goto ERROR3;
+				goto error;
 		}
 		err = device_create_file(dev, &dev_attr_alarms_temp);
 		if (err)
-			goto ERROR3;
+			goto error;
 	}
 
 	for (i = 0; i < data->fannr; i++) {
@@ -1353,49 +1350,37 @@ static int __devinit pc87360_probe(struct platform_device *pdev)
 			err = sysfs_create_group(&dev->kobj,
 						 &pc8736x_fan_attr_group[i]);
 			if (err)
-				goto ERROR3;
+				goto error;
 		}
 		if (FAN_CONFIG_CONTROL(data->fan_conf, i)) {
 			err = device_create_file(dev, &pwm[i].dev_attr);
 			if (err)
-				goto ERROR3;
+				goto error;
 		}
 	}
 
 	err = device_create_file(dev, &dev_attr_name);
 	if (err)
-		goto ERROR3;
+		goto error;
 
 	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
-		goto ERROR3;
+		goto error;
 	}
 	return 0;
 
-ERROR3:
+error:
 	pc87360_remove_files(dev);
-	for (i = 0; i < 3; i++) {
-		if (data->address[i])
-			release_region(data->address[i], PC87360_EXTENT);
-	}
-ERROR1:
-	kfree(data);
 	return err;
 }
 
 static int __devexit pc87360_remove(struct platform_device *pdev)
 {
 	struct pc87360_data *data = platform_get_drvdata(pdev);
-	int i;
 
 	hwmon_device_unregister(data->hwmon_dev);
 	pc87360_remove_files(&pdev->dev);
-	for (i = 0; i < 3; i++) {
-		if (data->address[i])
-			release_region(data->address[i], PC87360_EXTENT);
-	}
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/hwmon/pc87427.c b/drivers/hwmon/pc87427.c
index 37059a3755e..f185b1fa53e 100644
--- a/drivers/hwmon/pc87427.c
+++ b/drivers/hwmon/pc87427.c
@@ -956,44 +956,28 @@ static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
  * Device detection, attach and detach
  */
 
-static void pc87427_release_regions(struct platform_device *pdev, int count)
-{
-	struct resource *res;
-	int i;
-
-	for (i = 0; i < count; i++) {
-		res = platform_get_resource(pdev, IORESOURCE_IO, i);
-		release_region(res->start, resource_size(res));
-	}
-}
-
 static int __devinit pc87427_request_regions(struct platform_device *pdev,
 					     int count)
 {
 	struct resource *res;
-	int i, err = 0;
+	int i;
 
 	for (i = 0; i < count; i++) {
 		res = platform_get_resource(pdev, IORESOURCE_IO, i);
 		if (!res) {
-			err = -ENOENT;
 			dev_err(&pdev->dev, "Missing resource #%d\n", i);
-			break;
+			return -ENOENT;
 		}
-		if (!request_region(res->start, resource_size(res), DRVNAME)) {
-			err = -EBUSY;
+		if (!devm_request_region(&pdev->dev, res->start,
+					 resource_size(res), DRVNAME)) {
 			dev_err(&pdev->dev,
 				"Failed to request region 0x%lx-0x%lx\n",
 				(unsigned long)res->start,
 				(unsigned long)res->end);
-			break;
+			return -EBUSY;
 		}
 	}
-
-	if (err && i)
-		pc87427_release_regions(pdev, i);
-
-	return err;
+	return 0;
 }
 
 static void __devinit pc87427_init_device(struct device *dev)
@@ -1094,11 +1078,11 @@ static int __devinit pc87427_probe(struct platform_device *pdev)
 	struct pc87427_data *data;
 	int i, err, res_count;
 
-	data = kzalloc(sizeof(struct pc87427_data), GFP_KERNEL);
+	data = devm_kzalloc(&pdev->dev, sizeof(struct pc87427_data),
+			    GFP_KERNEL);
 	if (!data) {
-		err = -ENOMEM;
 		pr_err("Out of memory\n");
-		goto exit;
+		return -ENOMEM;
 	}
 
 	data->address[0] = sio_data->address[0];
@@ -1107,7 +1091,7 @@ static int __devinit pc87427_probe(struct platform_device *pdev)
 
 	err = pc87427_request_regions(pdev, res_count);
 	if (err)
-		goto exit_kfree;
+		return err;
 
 	mutex_init(&data->lock);
 	data->name = "pc87427";
@@ -1117,7 +1101,7 @@ static int __devinit pc87427_probe(struct platform_device *pdev)
 	/* Register sysfs hooks */
 	err = device_create_file(&pdev->dev, &dev_attr_name);
 	if (err)
-		goto exit_release_region;
+		return err;
 	for (i = 0; i < 8; i++) {
 		if (!(data->fan_enabled & (1 << i)))
 			continue;
@@ -1154,28 +1138,15 @@ static int __devinit pc87427_probe(struct platform_device *pdev)
 
 exit_remove_files:
 	pc87427_remove_files(&pdev->dev);
-exit_release_region:
-	pc87427_release_regions(pdev, res_count);
-exit_kfree:
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-exit:
 	return err;
 }
 
 static int __devexit pc87427_remove(struct platform_device *pdev)
 {
 	struct pc87427_data *data = platform_get_drvdata(pdev);
-	int res_count;
-
-	res_count = (data->address[0] != 0) + (data->address[1] != 0);
 
 	hwmon_device_unregister(data->hwmon_dev);
 	pc87427_remove_files(&pdev->dev);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
-	pc87427_release_regions(pdev, res_count);
 
 	return 0;
 }
diff --git a/drivers/hwmon/pcf8591.c b/drivers/hwmon/pcf8591.c
index 4174c7463d7..825883d2900 100644
--- a/drivers/hwmon/pcf8591.c
+++ b/drivers/hwmon/pcf8591.c
@@ -200,11 +200,10 @@ static int pcf8591_probe(struct i2c_client *client,
 	struct pcf8591_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct pcf8591_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct pcf8591_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
@@ -215,7 +214,7 @@ static int pcf8591_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &pcf8591_attr_group);
 	if (err)
-		goto exit_kfree;
+		return err;
 
 	/* Register input2 if not in "two differential inputs" mode */
 	if (input_mode != 3) {
@@ -242,9 +241,6 @@ static int pcf8591_probe(struct i2c_client *client,
 exit_sysfs_remove:
 	sysfs_remove_group(&client->dev.kobj, &pcf8591_attr_group_opt);
 	sysfs_remove_group(&client->dev.kobj, &pcf8591_attr_group);
-exit_kfree:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -255,7 +251,6 @@ static int pcf8591_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &pcf8591_attr_group_opt);
 	sysfs_remove_group(&client->dev.kobj, &pcf8591_attr_group);
-	kfree(i2c_get_clientdata(client));
 	return 0;
 }
 
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index f6c26d19f52..b7975f858cf 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -288,7 +288,7 @@ static int __devinit s3c_hwmon_probe(struct platform_device *dev)
 		return -EINVAL;
 	}
 
-	hwmon = kzalloc(sizeof(struct s3c_hwmon), GFP_KERNEL);
+	hwmon = devm_kzalloc(&dev->dev, sizeof(struct s3c_hwmon), GFP_KERNEL);
 	if (hwmon == NULL) {
 		dev_err(&dev->dev, "no memory\n");
 		return -ENOMEM;
@@ -303,8 +303,7 @@ static int __devinit s3c_hwmon_probe(struct platform_device *dev)
 	hwmon->client = s3c_adc_register(dev, NULL, NULL, 0);
 	if (IS_ERR(hwmon->client)) {
 		dev_err(&dev->dev, "cannot register adc\n");
-		ret = PTR_ERR(hwmon->client);
-		goto err_mem;
+		return PTR_ERR(hwmon->client);
 	}
 
 	/* add attributes for our adc devices. */
@@ -363,8 +362,6 @@ static int __devinit s3c_hwmon_probe(struct platform_device *dev)
  err_registered:
 	s3c_adc_release(hwmon->client);
 
- err_mem:
-	kfree(hwmon);
 	return ret;
 }
 
diff --git a/drivers/hwmon/sis5595.c b/drivers/hwmon/sis5595.c
index 6c4d8eb9b7c..8275f0e14eb 100644
--- a/drivers/hwmon/sis5595.c
+++ b/drivers/hwmon/sis5595.c
@@ -593,17 +593,14 @@ static int __devinit sis5595_probe(struct platform_device *pdev)
 
 	/* Reserve the ISA region */
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start, SIS5595_EXTENT,
-			    sis5595_driver.driver.name)) {
-		err = -EBUSY;
-		goto exit;
-	}
+	if (!devm_request_region(&pdev->dev, res->start, SIS5595_EXTENT,
+				 sis5595_driver.driver.name))
+		return -EBUSY;
 
-	data = kzalloc(sizeof(struct sis5595_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit_release;
-	}
+	data = devm_kzalloc(&pdev->dev, sizeof(struct sis5595_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	mutex_init(&data->lock);
 	mutex_init(&data->update_lock);
@@ -636,7 +633,7 @@ static int __devinit sis5595_probe(struct platform_device *pdev)
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&pdev->dev.kobj, &sis5595_group);
 	if (err)
-		goto exit_free;
+		return err;
 	if (data->maxins == 4) {
 		err = sysfs_create_group(&pdev->dev.kobj, &sis5595_group_in4);
 		if (err)
@@ -659,11 +656,6 @@ exit_remove_files:
 	sysfs_remove_group(&pdev->dev.kobj, &sis5595_group);
 	sysfs_remove_group(&pdev->dev.kobj, &sis5595_group_in4);
 	sysfs_remove_group(&pdev->dev.kobj, &sis5595_group_temp1);
-exit_free:
-	kfree(data);
-exit_release:
-	release_region(res->start, SIS5595_EXTENT);
-exit:
 	return err;
 }
 
@@ -676,10 +668,6 @@ static int __devexit sis5595_remove(struct platform_device *pdev)
 	sysfs_remove_group(&pdev->dev.kobj, &sis5595_group_in4);
 	sysfs_remove_group(&pdev->dev.kobj, &sis5595_group_temp1);
 
-	release_region(data->addr, SIS5595_EXTENT);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/smsc47b397.c b/drivers/hwmon/smsc47b397.c
index c5f6be478ba..65b07de11a0 100644
--- a/drivers/hwmon/smsc47b397.c
+++ b/drivers/hwmon/smsc47b397.c
@@ -231,13 +231,9 @@ static const struct attribute_group smsc47b397_group = {
 static int __devexit smsc47b397_remove(struct platform_device *pdev)
 {
 	struct smsc47b397_data *data = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&pdev->dev.kobj, &smsc47b397_group);
-	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	release_region(res->start, SMSC_EXTENT);
-	kfree(data);
 
 	return 0;
 }
@@ -261,19 +257,17 @@ static int __devinit smsc47b397_probe(struct platform_device *pdev)
 	int err = 0;
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start, SMSC_EXTENT,
-			    smsc47b397_driver.driver.name)) {
+	if (!devm_request_region(dev, res->start, SMSC_EXTENT,
+				 smsc47b397_driver.driver.name)) {
 		dev_err(dev, "Region 0x%lx-0x%lx already in use!\n",
 			(unsigned long)res->start,
 			(unsigned long)res->start + SMSC_EXTENT - 1);
 		return -EBUSY;
 	}
 
-	data = kzalloc(sizeof(struct smsc47b397_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto error_release;
-	}
+	data = devm_kzalloc(dev, sizeof(struct smsc47b397_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	data->addr = res->start;
 	data->name = "smsc47b397";
@@ -283,7 +277,7 @@ static int __devinit smsc47b397_probe(struct platform_device *pdev)
 
 	err = sysfs_create_group(&dev->kobj, &smsc47b397_group);
 	if (err)
-		goto error_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -295,10 +289,6 @@ static int __devinit smsc47b397_probe(struct platform_device *pdev)
 
 error_remove:
 	sysfs_remove_group(&dev->kobj, &smsc47b397_group);
-error_free:
-	kfree(data);
-error_release:
-	release_region(res->start, SMSC_EXTENT);
 	return err;
 }
 
diff --git a/drivers/hwmon/smsc47m1.c b/drivers/hwmon/smsc47m1.c
index b5aa38dd7ab..dba0c567e7a 100644
--- a/drivers/hwmon/smsc47m1.c
+++ b/drivers/hwmon/smsc47m1.c
@@ -584,18 +584,17 @@ static void smsc47m1_restore(const struct smsc47m1_sio_data *sio_data)
 
 #define CHECK		1
 #define REQUEST		2
-#define RELEASE		3
 
 /*
  * This function can be used to:
  *  - test for resource conflicts with ACPI
  *  - request the resources
- *  - release the resources
  * We only allocate the I/O ports we really need, to minimize the risk of
  * conflicts with ACPI or with other drivers.
  */
-static int smsc47m1_handle_resources(unsigned short address, enum chips type,
-				     int action, struct device *dev)
+static int __init smsc47m1_handle_resources(unsigned short address,
+					    enum chips type, int action,
+					    struct device *dev)
 {
 	static const u8 ports_m1[] = {
 		/* register, region length */
@@ -642,21 +641,13 @@ static int smsc47m1_handle_resources(unsigned short address, enum chips type,
 			break;
 		case REQUEST:
 			/* Request the resources */
-			if (!request_region(start, len, DRVNAME)) {
-				dev_err(dev, "Region 0x%hx-0x%hx already in "
-					"use!\n", start, start + len);
-
-				/* Undo all requests */
-				for (i -= 2; i >= 0; i -= 2)
-					release_region(address + ports[i],
-						       ports[i + 1]);
+			if (!devm_request_region(dev, start, len, DRVNAME)) {
+				dev_err(dev,
+					"Region 0x%hx-0x%hx already in use!\n",
+					start, start + len);
 				return -EBUSY;
 			}
 			break;
-		case RELEASE:
-			/* Release the resources */
-			release_region(start, len);
-			break;
 		}
 	}
 
@@ -694,11 +685,9 @@ static int __init smsc47m1_probe(struct platform_device *pdev)
 	if (err < 0)
 		return err;
 
-	data = kzalloc(sizeof(struct smsc47m1_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto error_release;
-	}
+	data = devm_kzalloc(dev, sizeof(struct smsc47m1_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	data->addr = res->start;
 	data->type = sio_data->type;
@@ -733,8 +722,7 @@ static int __init smsc47m1_probe(struct platform_device *pdev)
 	}
 	if (!(fan1 || fan2 || fan3 || pwm1 || pwm2 || pwm3)) {
 		dev_warn(dev, "Device not configured, will not use\n");
-		err = -ENODEV;
-		goto error_free;
+		return -ENODEV;
 	}
 
 	/*
@@ -810,27 +798,16 @@ static int __init smsc47m1_probe(struct platform_device *pdev)
 
 error_remove_files:
 	smsc47m1_remove_files(dev);
-error_free:
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-error_release:
-	smsc47m1_handle_resources(res->start, sio_data->type, RELEASE, dev);
 	return err;
 }
 
 static int __exit smsc47m1_remove(struct platform_device *pdev)
 {
 	struct smsc47m1_data *data = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	hwmon_device_unregister(data->hwmon_dev);
 	smsc47m1_remove_files(&pdev->dev);
 
-	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	smsc47m1_handle_resources(res->start, data->type, RELEASE, &pdev->dev);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/smsc47m192.c b/drivers/hwmon/smsc47m192.c
index 4705a8bf11c..36a3478d079 100644
--- a/drivers/hwmon/smsc47m192.c
+++ b/drivers/hwmon/smsc47m192.c
@@ -554,11 +554,10 @@ static int smsc47m192_probe(struct i2c_client *client,
 	int config;
 	int err;
 
-	data = kzalloc(sizeof(struct smsc47m192_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct smsc47m192_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->vrm = vid_which_vrm();
@@ -570,7 +569,7 @@ static int smsc47m192_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &smsc47m192_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	/* Pin 110 is either in4 (+12V) or VID4 */
 	config = i2c_smbus_read_byte_data(client, SMSC47M192_REG_CONFIG);
@@ -592,9 +591,6 @@ static int smsc47m192_probe(struct i2c_client *client,
 exit_remove_files:
 	sysfs_remove_group(&client->dev.kobj, &smsc47m192_group);
 	sysfs_remove_group(&client->dev.kobj, &smsc47m192_group_in4);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -606,8 +602,6 @@ static int smsc47m192_remove(struct i2c_client *client)
 	sysfs_remove_group(&client->dev.kobj, &smsc47m192_group);
 	sysfs_remove_group(&client->dev.kobj, &smsc47m192_group_in4);
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/thmc50.c b/drivers/hwmon/thmc50.c
index add9f019b24..080c2637048 100644
--- a/drivers/hwmon/thmc50.c
+++ b/drivers/hwmon/thmc50.c
@@ -361,12 +361,10 @@ static int thmc50_probe(struct i2c_client *client,
 	struct thmc50_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct thmc50_data), GFP_KERNEL);
-	if (!data) {
-		pr_debug("thmc50: detect failed, kzalloc failed!\n");
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct thmc50_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->type = id->driver_data;
@@ -377,7 +375,7 @@ static int thmc50_probe(struct i2c_client *client,
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&client->dev.kobj, &thmc50_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	/* Register ADM1022 sysfs hooks */
 	if (data->has_temp3) {
@@ -400,9 +398,6 @@ exit_remove_sysfs:
 		sysfs_remove_group(&client->dev.kobj, &temp3_group);
 exit_remove_sysfs_thmc50:
 	sysfs_remove_group(&client->dev.kobj, &thmc50_group);
-exit_free:
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -415,8 +410,6 @@ static int thmc50_remove(struct i2c_client *client)
 	if (data->has_temp3)
 		sysfs_remove_group(&client->dev.kobj, &temp3_group);
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 0d466b9d890..4e1ff82c63e 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -159,17 +159,16 @@ static int __devinit tmp102_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	tmp102 = kzalloc(sizeof(*tmp102), GFP_KERNEL);
-	if (!tmp102) {
-		dev_dbg(&client->dev, "kzalloc failed\n");
+	tmp102 = devm_kzalloc(&client->dev, sizeof(*tmp102), GFP_KERNEL);
+	if (!tmp102)
 		return -ENOMEM;
-	}
+
 	i2c_set_clientdata(client, tmp102);
 
 	status = i2c_smbus_read_word_swapped(client, TMP102_CONF_REG);
 	if (status < 0) {
 		dev_err(&client->dev, "error reading config register\n");
-		goto fail_free;
+		return status;
 	}
 	tmp102->config_orig = status;
 	status = i2c_smbus_write_word_swapped(client, TMP102_CONF_REG,
@@ -213,9 +212,6 @@ fail_remove_sysfs:
 fail_restore_config:
 	i2c_smbus_write_word_swapped(client, TMP102_CONF_REG,
 				     tmp102->config_orig);
-fail_free:
-	kfree(tmp102);
-
 	return status;
 }
 
@@ -236,8 +232,6 @@ static int __devexit tmp102_remove(struct i2c_client *client)
 						     config | TMP102_CONF_SD);
 	}
 
-	kfree(tmp102);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/tmp401.c b/drivers/hwmon/tmp401.c
index ea54c338467..e6205487516 100644
--- a/drivers/hwmon/tmp401.c
+++ b/drivers/hwmon/tmp401.c
@@ -594,7 +594,6 @@ static int tmp401_remove(struct i2c_client *client)
 					   &tmp411_attr[i].dev_attr);
 	}
 
-	kfree(data);
 	return 0;
 }
 
@@ -605,7 +604,8 @@ static int tmp401_probe(struct i2c_client *client,
 	struct tmp401_data *data;
 	const char *names[] = { "TMP401", "TMP411" };
 
-	data = kzalloc(sizeof(struct tmp401_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct tmp401_data),
+			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -646,7 +646,7 @@ static int tmp401_probe(struct i2c_client *client,
 	return 0;
 
 exit_remove:
-	tmp401_remove(client); /* will also free data for us */
+	tmp401_remove(client);
 	return err;
 }
 
diff --git a/drivers/hwmon/tmp421.c b/drivers/hwmon/tmp421.c
index 8fac87a3854..6a8ded29f1e 100644
--- a/drivers/hwmon/tmp421.c
+++ b/drivers/hwmon/tmp421.c
@@ -267,7 +267,8 @@ static int tmp421_probe(struct i2c_client *client,
 	struct tmp421_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct tmp421_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct tmp421_data),
+			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -277,11 +278,11 @@ static int tmp421_probe(struct i2c_client *client,
 
 	err = tmp421_init_client(client);
 	if (err)
-		goto exit_free;
+		return err;
 
 	err = sysfs_create_group(&client->dev.kobj, &tmp421_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&client->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -293,10 +294,6 @@ static int tmp421_probe(struct i2c_client *client,
 
 exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &tmp421_group);
-
-exit_free:
-	kfree(data);
-
 	return err;
 }
 
@@ -307,8 +304,6 @@ static int tmp421_remove(struct i2c_client *client)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &tmp421_group);
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/via-cputemp.c b/drivers/hwmon/via-cputemp.c
index 8689664ef03..ee4ebc198a9 100644
--- a/drivers/hwmon/via-cputemp.c
+++ b/drivers/hwmon/via-cputemp.c
@@ -309,7 +309,7 @@ static struct notifier_block via_cputemp_cpu_notifier __refdata = {
 	.notifier_call = via_cputemp_cpu_callback,
 };
 
-static const struct x86_cpu_id cputemp_ids[] = {
+static const struct x86_cpu_id __initconst cputemp_ids[] = {
 	{ X86_VENDOR_CENTAUR, 6, 0xa, }, /* C7 A */
 	{ X86_VENDOR_CENTAUR, 6, 0xd, }, /* C7 D */
 	{ X86_VENDOR_CENTAUR, 6, 0xf, }, /* Nano */
diff --git a/drivers/hwmon/via686a.c b/drivers/hwmon/via686a.c
index 288135d85e1..299399aa30f 100644
--- a/drivers/hwmon/via686a.c
+++ b/drivers/hwmon/via686a.c
@@ -690,18 +690,17 @@ static int __devinit via686a_probe(struct platform_device *pdev)
 
 	/* Reserve the ISA region */
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start, VIA686A_EXTENT,
-			    via686a_driver.driver.name)) {
+	if (!devm_request_region(&pdev->dev, res->start, VIA686A_EXTENT,
+				 via686a_driver.driver.name)) {
 		dev_err(&pdev->dev, "Region 0x%lx-0x%lx already in use!\n",
 			(unsigned long)res->start, (unsigned long)res->end);
 		return -ENODEV;
 	}
 
-	data = kzalloc(sizeof(struct via686a_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit_release;
-	}
+	data = devm_kzalloc(&pdev->dev, sizeof(struct via686a_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	platform_set_drvdata(pdev, data);
 	data->addr = res->start;
@@ -714,7 +713,7 @@ static int __devinit via686a_probe(struct platform_device *pdev)
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&pdev->dev.kobj, &via686a_group);
 	if (err)
-		goto exit_free;
+		return err;
 
 	data->hwmon_dev = hwmon_device_register(&pdev->dev);
 	if (IS_ERR(data->hwmon_dev)) {
@@ -726,10 +725,6 @@ static int __devinit via686a_probe(struct platform_device *pdev)
 
 exit_remove_files:
 	sysfs_remove_group(&pdev->dev.kobj, &via686a_group);
-exit_free:
-	kfree(data);
-exit_release:
-	release_region(res->start, VIA686A_EXTENT);
 	return err;
 }
 
@@ -740,10 +735,6 @@ static int __devexit via686a_remove(struct platform_device *pdev)
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&pdev->dev.kobj, &via686a_group);
 
-	release_region(data->addr, VIA686A_EXTENT);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/vt1211.c b/drivers/hwmon/vt1211.c
index c2c5c72fb8f..f2c61153dba 100644
--- a/drivers/hwmon/vt1211.c
+++ b/drivers/hwmon/vt1211.c
@@ -1148,19 +1148,18 @@ static int __devinit vt1211_probe(struct platform_device *pdev)
 	struct resource *res;
 	int i, err;
 
-	data = kzalloc(sizeof(struct vt1211_data), GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct vt1211_data), GFP_KERNEL);
 	if (!data) {
-		err = -ENOMEM;
 		dev_err(dev, "Out of memory\n");
-		goto EXIT;
+		return -ENOMEM;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start, resource_size(res), DRVNAME)) {
-		err = -EBUSY;
+	if (!devm_request_region(dev, res->start, resource_size(res),
+				 DRVNAME)) {
 		dev_err(dev, "Failed to request region 0x%lx-0x%lx\n",
 			(unsigned long)res->start, (unsigned long)res->end);
-		goto EXIT_KFREE;
+		return -EBUSY;
 	}
 	data->addr = res->start;
 	data->name = DRVNAME;
@@ -1215,26 +1214,15 @@ EXIT_DEV_REMOVE:
 	dev_err(dev, "Sysfs interface creation failed (%d)\n", err);
 EXIT_DEV_REMOVE_SILENT:
 	vt1211_remove_sysfs(pdev);
-	release_region(res->start, resource_size(res));
-EXIT_KFREE:
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-EXIT:
 	return err;
 }
 
 static int __devexit vt1211_remove(struct platform_device *pdev)
 {
 	struct vt1211_data *data = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	hwmon_device_unregister(data->hwmon_dev);
 	vt1211_remove_sysfs(pdev);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
-	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	release_region(res->start, resource_size(res));
 
 	return 0;
 }
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index 54922ed1297..1821b7423d5 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -599,6 +599,7 @@ static void nct6775_write_fan_div(struct w83627ehf_data *data, int nr)
 		reg = (w83627ehf_read_value(data, NCT6775_REG_FANDIV1) & 0x7)
 		    | ((data->fan_div[1] << 4) & 0x70);
 		w83627ehf_write_value(data, NCT6775_REG_FANDIV1, reg);
+		break;
 	case 2:
 		reg = (w83627ehf_read_value(data, NCT6775_REG_FANDIV2) & 0x70)
 		    | (data->fan_div[2] & 0x7);
diff --git a/drivers/hwmon/w83627hf.c b/drivers/hwmon/w83627hf.c
index 5ce54a29724..ab4825205a9 100644
--- a/drivers/hwmon/w83627hf.c
+++ b/drivers/hwmon/w83627hf.c
@@ -1359,19 +1359,17 @@ static int __devinit w83627hf_probe(struct platform_device *pdev)
 	};
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start, WINB_REGION_SIZE, DRVNAME)) {
+	if (!devm_request_region(dev, res->start, WINB_REGION_SIZE, DRVNAME)) {
 		dev_err(dev, "Failed to request region 0x%lx-0x%lx\n",
 			(unsigned long)res->start,
 			(unsigned long)(res->start + WINB_REGION_SIZE - 1));
-		err = -EBUSY;
-		goto ERROR0;
+		return -EBUSY;
 	}
 
-	data = kzalloc(sizeof(struct w83627hf_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto ERROR1;
-	}
+	data = devm_kzalloc(dev, sizeof(struct w83627hf_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
 	data->addr = res->start;
 	data->type = sio_data->type;
 	data->name = names[sio_data->type];
@@ -1391,7 +1389,7 @@ static int __devinit w83627hf_probe(struct platform_device *pdev)
 	/* Register common device attributes */
 	err = sysfs_create_group(&dev->kobj, &w83627hf_group);
 	if (err)
-		goto ERROR3;
+		return err;
 
 	/* Register chip-specific device attributes */
 	if (data->type == w83627hf || data->type == w83697hf)
@@ -1419,7 +1417,7 @@ static int __devinit w83627hf_probe(struct platform_device *pdev)
 				&sensor_dev_attr_pwm1_freq.dev_attr))
 		 || (err = device_create_file(dev,
 				&sensor_dev_attr_pwm2_freq.dev_attr)))
-			goto ERROR4;
+			goto error;
 
 	if (data->type != w83697hf)
 		if ((err = device_create_file(dev,
@@ -1454,7 +1452,7 @@ static int __devinit w83627hf_probe(struct platform_device *pdev)
 				&sensor_dev_attr_temp3_beep.dev_attr))
 		 || (err = device_create_file(dev,
 				&sensor_dev_attr_temp3_type.dev_attr)))
-			goto ERROR4;
+			goto error;
 
 	if (data->type != w83697hf && data->vid != 0xff) {
 		/* Convert VID to voltage based on VRM */
@@ -1462,14 +1460,14 @@ static int __devinit w83627hf_probe(struct platform_device *pdev)
 
 		if ((err = device_create_file(dev, &dev_attr_cpu0_vid))
 		 || (err = device_create_file(dev, &dev_attr_vrm)))
-			goto ERROR4;
+			goto error;
 	}
 
 	if (data->type == w83627thf || data->type == w83637hf
 	    || data->type == w83687thf) {
 		err = device_create_file(dev, &sensor_dev_attr_pwm3.dev_attr);
 		if (err)
-			goto ERROR4;
+			goto error;
 	}
 
 	if (data->type == w83637hf || data->type == w83687thf)
@@ -1479,57 +1477,45 @@ static int __devinit w83627hf_probe(struct platform_device *pdev)
 				&sensor_dev_attr_pwm2_freq.dev_attr))
 		 || (err = device_create_file(dev,
 				&sensor_dev_attr_pwm3_freq.dev_attr)))
-			goto ERROR4;
+			goto error;
 
 	if (data->type != w83627hf)
 		if ((err = device_create_file(dev,
 				&sensor_dev_attr_pwm1_enable.dev_attr))
 		 || (err = device_create_file(dev,
 				&sensor_dev_attr_pwm2_enable.dev_attr)))
-			goto ERROR4;
+			goto error;
 
 	if (data->type == w83627thf || data->type == w83637hf
 	    || data->type == w83687thf) {
 		err = device_create_file(dev,
 					 &sensor_dev_attr_pwm3_enable.dev_attr);
 		if (err)
-			goto ERROR4;
+			goto error;
 	}
 
 	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
-		goto ERROR4;
+		goto error;
 	}
 
 	return 0;
 
-      ERROR4:
+ error:
 	sysfs_remove_group(&dev->kobj, &w83627hf_group);
 	sysfs_remove_group(&dev->kobj, &w83627hf_group_opt);
-      ERROR3:
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-      ERROR1:
-	release_region(res->start, WINB_REGION_SIZE);
-      ERROR0:
 	return err;
 }
 
 static int __devexit w83627hf_remove(struct platform_device *pdev)
 {
 	struct w83627hf_data *data = platform_get_drvdata(pdev);
-	struct resource *res;
 
 	hwmon_device_unregister(data->hwmon_dev);
 
 	sysfs_remove_group(&pdev->dev.kobj, &w83627hf_group);
 	sysfs_remove_group(&pdev->dev.kobj, &w83627hf_group_opt);
-	platform_set_drvdata(pdev, NULL);
-	kfree(data);
-
-	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	release_region(res->start, WINB_REGION_SIZE);
 
 	return 0;
 }
diff --git a/drivers/hwmon/w83781d.c b/drivers/hwmon/w83781d.c
index b03d54a799e..5a5046d94c3 100644
--- a/drivers/hwmon/w83781d.c
+++ b/drivers/hwmon/w83781d.c
@@ -867,6 +867,7 @@ w83781d_detect_subclients(struct i2c_client *new_client)
 	struct i2c_adapter *adapter = new_client->adapter;
 	struct w83781d_data *data = i2c_get_clientdata(new_client);
 	enum chips kind = data->type;
+	int num_sc = 1;
 
 	id = i2c_adapter_id(adapter);
 
@@ -891,6 +892,7 @@ w83781d_detect_subclients(struct i2c_client *new_client)
 	}
 
 	if (kind != w83783s) {
+		num_sc = 2;
 		if (force_subclients[0] == id &&
 		    force_subclients[1] == address) {
 			sc_addr[1] = force_subclients[3];
@@ -906,7 +908,7 @@ w83781d_detect_subclients(struct i2c_client *new_client)
 		}
 	}
 
-	for (i = 0; i <= 1; i++) {
+	for (i = 0; i < num_sc; i++) {
 		data->lm75[i] = i2c_new_dummy(adapter, sc_addr[i]);
 		if (!data->lm75[i]) {
 			dev_err(&new_client->dev, "Subclient %d "
@@ -917,8 +919,6 @@ w83781d_detect_subclients(struct i2c_client *new_client)
 				goto ERROR_SC_3;
 			goto ERROR_SC_2;
 		}
-		if (kind == w83783s)
-			break;
 	}
 
 	return 0;
@@ -1213,11 +1213,9 @@ w83781d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	struct w83781d_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct w83781d_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto ERROR1;
-	}
+	data = devm_kzalloc(dev, sizeof(struct w83781d_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->lock);
@@ -1229,7 +1227,7 @@ w83781d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* attach secondary i2c lm75-like clients */
 	err = w83781d_detect_subclients(client);
 	if (err)
-		goto ERROR3;
+		return err;
 
 	/* Initialize the chip */
 	w83781d_init_device(dev);
@@ -1237,25 +1235,22 @@ w83781d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* Register sysfs hooks */
 	err = w83781d_create_files(dev, data->type, 0);
 	if (err)
-		goto ERROR4;
+		goto exit_remove_files;
 
 	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
-		goto ERROR4;
+		goto exit_remove_files;
 	}
 
 	return 0;
 
-ERROR4:
+ exit_remove_files:
 	w83781d_remove_files(dev);
 	if (data->lm75[0])
 		i2c_unregister_device(data->lm75[0]);
 	if (data->lm75[1])
 		i2c_unregister_device(data->lm75[1]);
-ERROR3:
-	kfree(data);
-ERROR1:
 	return err;
 }
 
@@ -1273,8 +1268,6 @@ w83781d_remove(struct i2c_client *client)
 	if (data->lm75[1])
 		i2c_unregister_device(data->lm75[1]);
 
-	kfree(data);
-
 	return 0;
 }
 
@@ -1780,17 +1773,16 @@ w83781d_isa_probe(struct platform_device *pdev)
 
 	/* Reserve the ISA region */
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
-	if (!request_region(res->start + W83781D_ADDR_REG_OFFSET, 2,
-			    "w83781d")) {
-		err = -EBUSY;
-		goto exit;
-	}
+	if (!devm_request_region(&pdev->dev,
+				 res->start + W83781D_ADDR_REG_OFFSET, 2,
+				 "w83781d"))
+		return -EBUSY;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(struct w83781d_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	data = kzalloc(sizeof(struct w83781d_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit_release_region;
-	}
 	mutex_init(&data->lock);
 	data->isa_addr = res->start;
 	platform_set_drvdata(pdev, data);
@@ -1829,10 +1821,6 @@ w83781d_isa_probe(struct platform_device *pdev)
  exit_remove_files:
 	w83781d_remove_files(&pdev->dev);
 	device_remove_file(&pdev->dev, &dev_attr_name);
-	kfree(data);
- exit_release_region:
-	release_region(res->start + W83781D_ADDR_REG_OFFSET, 2);
- exit:
 	return err;
 }
 
@@ -1844,8 +1832,6 @@ w83781d_isa_remove(struct platform_device *pdev)
 	hwmon_device_unregister(data->hwmon_dev);
 	w83781d_remove_files(&pdev->dev);
 	device_remove_file(&pdev->dev, &dev_attr_name);
-	release_region(data->isa_addr + W83781D_ADDR_REG_OFFSET, 2);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/hwmon/w83791d.c b/drivers/hwmon/w83791d.c
index 2f446f92acf..9ade4d4e218 100644
--- a/drivers/hwmon/w83791d.c
+++ b/drivers/hwmon/w83791d.c
@@ -1384,18 +1384,17 @@ static int w83791d_probe(struct i2c_client *client,
 			(val1 >> 5) & 0x07, (val1 >> 1) & 0x0f, val1);
 #endif
 
-	data = kzalloc(sizeof(struct w83791d_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto error0;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct w83791d_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
 
 	err = w83791d_detect_subclients(client);
 	if (err)
-		goto error1;
+		return err;
 
 	/* Initialize the chip */
 	w83791d_init_client(client);
@@ -1440,9 +1439,6 @@ error3:
 		i2c_unregister_device(data->lm75[0]);
 	if (data->lm75[1] != NULL)
 		i2c_unregister_device(data->lm75[1]);
-error1:
-	kfree(data);
-error0:
 	return err;
 }
 
@@ -1458,7 +1454,6 @@ static int w83791d_remove(struct i2c_client *client)
 	if (data->lm75[1] != NULL)
 		i2c_unregister_device(data->lm75[1]);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/w83792d.c b/drivers/hwmon/w83792d.c
index ffb5fdfecf0..0ba5a2bd562 100644
--- a/drivers/hwmon/w83792d.c
+++ b/drivers/hwmon/w83792d.c
@@ -1422,11 +1422,9 @@ w83792d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	struct device *dev = &client->dev;
 	int i, val1, err;
 
-	data = kzalloc(sizeof(struct w83792d_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto ERROR0;
-	}
+	data = devm_kzalloc(dev, sizeof(struct w83792d_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->valid = 0;
@@ -1434,7 +1432,7 @@ w83792d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 
 	err = w83792d_detect_subclients(client);
 	if (err)
-		goto ERROR1;
+		return err;
 
 	/* Initialize the chip */
 	w83792d_init_client(client);
@@ -1448,7 +1446,7 @@ w83792d_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* Register sysfs hooks */
 	err = sysfs_create_group(&dev->kobj, &w83792d_group);
 	if (err)
-		goto ERROR3;
+		goto exit_i2c_unregister;
 
 	/*
 	 * Read GPIO enable register to check if pins for fan 4,5 are used as
@@ -1493,14 +1491,11 @@ exit_remove_files:
 	sysfs_remove_group(&dev->kobj, &w83792d_group);
 	for (i = 0; i < ARRAY_SIZE(w83792d_group_fan); i++)
 		sysfs_remove_group(&dev->kobj, &w83792d_group_fan[i]);
-ERROR3:
+exit_i2c_unregister:
 	if (data->lm75[0] != NULL)
 		i2c_unregister_device(data->lm75[0]);
 	if (data->lm75[1] != NULL)
 		i2c_unregister_device(data->lm75[1]);
-ERROR1:
-	kfree(data);
-ERROR0:
 	return err;
 }
 
@@ -1521,7 +1516,6 @@ w83792d_remove(struct i2c_client *client)
 	if (data->lm75[1] != NULL)
 		i2c_unregister_device(data->lm75[1]);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/w83795.c b/drivers/hwmon/w83795.c
index d887cb3b72e..b813c646c7c 100644
--- a/drivers/hwmon/w83795.c
+++ b/drivers/hwmon/w83795.c
@@ -2157,11 +2157,9 @@ static int w83795_probe(struct i2c_client *client,
 	struct w83795_data *data;
 	int err;
 
-	data = kzalloc(sizeof(struct w83795_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(dev, sizeof(struct w83795_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, data);
 	data->chip_type = id->driver_data;
@@ -2247,8 +2245,6 @@ static int w83795_probe(struct i2c_client *client,
 
 exit_remove:
 	w83795_handle_files(dev, device_remove_file_wrapper);
-	kfree(data);
-exit:
 	return err;
 }
 
@@ -2258,7 +2254,6 @@ static int w83795_remove(struct i2c_client *client)
 
 	hwmon_device_unregister(data->hwmon_dev);
 	w83795_handle_files(&client->dev, device_remove_file_wrapper);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/hwmon/w83l785ts.c b/drivers/hwmon/w83l785ts.c
index 5f14e389705..39dbe990dc1 100644
--- a/drivers/hwmon/w83l785ts.c
+++ b/drivers/hwmon/w83l785ts.c
@@ -176,19 +176,18 @@ static int w83l785ts_detect(struct i2c_client *client,
 	return 0;
 }
 
-static int w83l785ts_probe(struct i2c_client *new_client,
+static int w83l785ts_probe(struct i2c_client *client,
 			   const struct i2c_device_id *id)
 {
 	struct w83l785ts_data *data;
-	int err = 0;
+	struct device *dev = &client->dev;
+	int err;
 
-	data = kzalloc(sizeof(struct w83l785ts_data), GFP_KERNEL);
-	if (!data) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(dev, sizeof(struct w83l785ts_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	i2c_set_clientdata(new_client, data);
+	i2c_set_clientdata(client, data);
 	data->valid = 0;
 	mutex_init(&data->update_lock);
 
@@ -200,18 +199,16 @@ static int w83l785ts_probe(struct i2c_client *new_client,
 	 * Nothing yet, assume it is already started.
 	 */
 
-	err = device_create_file(&new_client->dev,
-				 &sensor_dev_attr_temp1_input.dev_attr);
+	err = device_create_file(dev, &sensor_dev_attr_temp1_input.dev_attr);
 	if (err)
-		goto exit_remove;
+		return err;
 
-	err = device_create_file(&new_client->dev,
-				 &sensor_dev_attr_temp1_max.dev_attr);
+	err = device_create_file(dev, &sensor_dev_attr_temp1_max.dev_attr);
 	if (err)
 		goto exit_remove;
 
 	/* Register sysfs hooks */
-	data->hwmon_dev = hwmon_device_register(&new_client->dev);
+	data->hwmon_dev = hwmon_device_register(dev);
 	if (IS_ERR(data->hwmon_dev)) {
 		err = PTR_ERR(data->hwmon_dev);
 		goto exit_remove;
@@ -220,12 +217,8 @@ static int w83l785ts_probe(struct i2c_client *new_client,
 	return 0;
 
 exit_remove:
-	device_remove_file(&new_client->dev,
-			   &sensor_dev_attr_temp1_input.dev_attr);
-	device_remove_file(&new_client->dev,
-			   &sensor_dev_attr_temp1_max.dev_attr);
-	kfree(data);
-exit:
+	device_remove_file(dev, &sensor_dev_attr_temp1_input.dev_attr);
+	device_remove_file(dev, &sensor_dev_attr_temp1_max.dev_attr);
 	return err;
 }
 
@@ -239,7 +232,6 @@ static int w83l785ts_remove(struct i2c_client *client)
 	device_remove_file(&client->dev,
 			   &sensor_dev_attr_temp1_max.dev_attr);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/hwmon/wm831x-hwmon.c b/drivers/hwmon/wm831x-hwmon.c
index 07cb25ae69b..d0db1f2738f 100644
--- a/drivers/hwmon/wm831x-hwmon.c
+++ b/drivers/hwmon/wm831x-hwmon.c
@@ -163,7 +163,8 @@ static int __devinit wm831x_hwmon_probe(struct platform_device *pdev)
 	struct wm831x_hwmon *hwmon;
 	int ret;
 
-	hwmon = kzalloc(sizeof(struct wm831x_hwmon), GFP_KERNEL);
+	hwmon = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_hwmon),
+			     GFP_KERNEL);
 	if (!hwmon)
 		return -ENOMEM;
 
@@ -171,7 +172,7 @@ static int __devinit wm831x_hwmon_probe(struct platform_device *pdev)
 
 	ret = sysfs_create_group(&pdev->dev.kobj, &wm831x_attr_group);
 	if (ret)
-		goto err;
+		return ret;
 
 	hwmon->classdev = hwmon_device_register(&pdev->dev);
 	if (IS_ERR(hwmon->classdev)) {
@@ -185,8 +186,6 @@ static int __devinit wm831x_hwmon_probe(struct platform_device *pdev)
 
 err_sysfs:
 	sysfs_remove_group(&pdev->dev.kobj, &wm831x_attr_group);
-err:
-	kfree(hwmon);
 	return ret;
 }
 
@@ -196,8 +195,6 @@ static int __devexit wm831x_hwmon_remove(struct platform_device *pdev)
 
 	hwmon_device_unregister(hwmon->classdev);
 	sysfs_remove_group(&pdev->dev.kobj, &wm831x_attr_group);
-	platform_set_drvdata(pdev, NULL);
-	kfree(hwmon);
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 7244c8be606..b4aaa1bd672 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -133,7 +133,7 @@ config I2C_PIIX4
 	    ATI IXP300
 	    ATI IXP400
 	    ATI SB600
-	    ATI SB700
+	    ATI SB700/SP5100
 	    ATI SB800
 	    AMD Hudson-2
 	    Serverworks OSB4
@@ -143,6 +143,10 @@ config I2C_PIIX4
 	    Serverworks HT-1100
 	    SMSC Victory66
 
+	  Some AMD chipsets contain two PIIX4-compatible SMBus
+	  controllers. This driver will attempt to use both controllers
+	  on the SB700/SP5100, if they have been initialized by the BIOS.
+
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-piix4.
 
@@ -458,7 +462,7 @@ config I2C_MPC
 
 config I2C_MV64XXX
 	tristate "Marvell mv64xxx I2C Controller"
-	depends on (MV64X60 || PLAT_ORION) && EXPERIMENTAL
+	depends on (MV64X60 || PLAT_ORION)
 	help
 	  If you say yes to this option, support will be included for the
 	  built-in I2C interface on the Marvell 64xxx line of host bridges.
@@ -479,10 +483,11 @@ config I2C_MXS
 
 config I2C_NOMADIK
 	tristate "ST-Ericsson Nomadik/Ux500 I2C Controller"
-	depends on PLAT_NOMADIK
+	depends on ARM_AMBA
 	help
 	  If you say yes to this option, support will be included for the
-	  I2C interface from ST-Ericsson's Nomadik and Ux500 architectures.
+	  I2C interface from ST-Ericsson's Nomadik and Ux500 architectures,
+	  as well as the STA2X11 PCIe I/O HUB.
 
 config I2C_NUC900
 	tristate "NUC900 I2C Driver"
diff --git a/drivers/i2c/busses/i2c-ali1535.c b/drivers/i2c/busses/i2c-ali1535.c
index e66d248fc12..125cd8e0ad2 100644
--- a/drivers/i2c/busses/i2c-ali1535.c
+++ b/drivers/i2c/busses/i2c-ali1535.c
@@ -531,15 +531,7 @@ static struct pci_driver ali1535_driver = {
 	.remove		= __devexit_p(ali1535_remove),
 };
 
-static int __init i2c_ali1535_init(void)
-{
-	return pci_register_driver(&ali1535_driver);
-}
-
-static void __exit i2c_ali1535_exit(void)
-{
-	pci_unregister_driver(&ali1535_driver);
-}
+module_pci_driver(ali1535_driver);
 
 MODULE_AUTHOR("Frodo Looijaard <frodol@dds.nl>, "
 	      "Philip Edelbrock <phil@netroedge.com>, "
@@ -547,6 +539,3 @@ MODULE_AUTHOR("Frodo Looijaard <frodol@dds.nl>, "
 	      "and Dan Eaton <dan.eaton@rocketlogix.com>");
 MODULE_DESCRIPTION("ALI1535 SMBus driver");
 MODULE_LICENSE("GPL");
-
-module_init(i2c_ali1535_init);
-module_exit(i2c_ali1535_exit);
diff --git a/drivers/i2c/busses/i2c-ali1563.c b/drivers/i2c/busses/i2c-ali1563.c
index 47ae0091e02..e02d9f86c6a 100644
--- a/drivers/i2c/busses/i2c-ali1563.c
+++ b/drivers/i2c/busses/i2c-ali1563.c
@@ -431,18 +431,6 @@ static struct pci_driver ali1563_pci_driver = {
 	.remove		= __devexit_p(ali1563_remove),
 };
 
-static int __init ali1563_init(void)
-{
-	return pci_register_driver(&ali1563_pci_driver);
-}
-
-module_init(ali1563_init);
-
-static void __exit ali1563_exit(void)
-{
-	pci_unregister_driver(&ali1563_pci_driver);
-}
-
-module_exit(ali1563_exit);
+module_pci_driver(ali1563_pci_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-ali15x3.c b/drivers/i2c/busses/i2c-ali15x3.c
index 087ea9caa74..ce8d26d053a 100644
--- a/drivers/i2c/busses/i2c-ali15x3.c
+++ b/drivers/i2c/busses/i2c-ali15x3.c
@@ -513,21 +513,10 @@ static struct pci_driver ali15x3_driver = {
 	.remove		= __devexit_p(ali15x3_remove),
 };
 
-static int __init i2c_ali15x3_init(void)
-{
-	return pci_register_driver(&ali15x3_driver);
-}
-
-static void __exit i2c_ali15x3_exit(void)
-{
-	pci_unregister_driver(&ali15x3_driver);
-}
+module_pci_driver(ali15x3_driver);
 
 MODULE_AUTHOR ("Frodo Looijaard <frodol@dds.nl>, "
 		"Philip Edelbrock <phil@netroedge.com>, "
 		"and Mark D. Studebaker <mdsxyz123@yahoo.com>");
 MODULE_DESCRIPTION("ALI15X3 SMBus driver");
 MODULE_LICENSE("GPL");
-
-module_init(i2c_ali15x3_init);
-module_exit(i2c_ali15x3_exit);
diff --git a/drivers/i2c/busses/i2c-amd756.c b/drivers/i2c/busses/i2c-amd756.c
index eb778bf15c1..304aa03b57b 100644
--- a/drivers/i2c/busses/i2c-amd756.c
+++ b/drivers/i2c/busses/i2c-amd756.c
@@ -410,21 +410,10 @@ static struct pci_driver amd756_driver = {
 	.remove		= __devexit_p(amd756_remove),
 };
 
-static int __init amd756_init(void)
-{
-	return pci_register_driver(&amd756_driver);
-}
-
-static void __exit amd756_exit(void)
-{
-	pci_unregister_driver(&amd756_driver);
-}
+module_pci_driver(amd756_driver);
 
 MODULE_AUTHOR("Merlin Hughes <merlin@merlin.org>");
 MODULE_DESCRIPTION("AMD756/766/768/8111 and nVidia nForce SMBus driver");
 MODULE_LICENSE("GPL");
 
 EXPORT_SYMBOL(amd756_smbus);
-
-module_init(amd756_init)
-module_exit(amd756_exit)
diff --git a/drivers/i2c/busses/i2c-amd8111.c b/drivers/i2c/busses/i2c-amd8111.c
index e5ac53b99b0..0919ac1d99a 100644
--- a/drivers/i2c/busses/i2c-amd8111.c
+++ b/drivers/i2c/busses/i2c-amd8111.c
@@ -491,15 +491,4 @@ static struct pci_driver amd8111_driver = {
 	.remove		= __devexit_p(amd8111_remove),
 };
 
-static int __init i2c_amd8111_init(void)
-{
-	return pci_register_driver(&amd8111_driver);
-}
-
-static void __exit i2c_amd8111_exit(void)
-{
-	pci_unregister_driver(&amd8111_driver);
-}
-
-module_init(i2c_amd8111_init);
-module_exit(i2c_amd8111_exit);
+module_pci_driver(amd8111_driver);
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 1679deef9c8..e24484beef0 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -279,30 +279,31 @@ static int __devexit at91_i2c_remove(struct platform_device *pdev)
 
 /* NOTE: could save a few mA by keeping clock off outside of at91_xfer... */
 
-static int at91_i2c_suspend(struct platform_device *pdev, pm_message_t mesg)
+static int at91_i2c_suspend(struct device *dev)
 {
 	clk_disable(twi_clk);
 	return 0;
 }
 
-static int at91_i2c_resume(struct platform_device *pdev)
+static int at91_i2c_resume(struct device *dev)
 {
 	return clk_enable(twi_clk);
 }
 
+static SIMPLE_DEV_PM_OPS(at91_i2c_pm, at91_i2c_suspend, at91_i2c_resume);
+#define AT91_I2C_PM	(&at91_i2c_pm)
+
 #else
-#define at91_i2c_suspend	NULL
-#define at91_i2c_resume		NULL
+#define AT91_I2C_PM	NULL
 #endif
 
 static struct platform_driver at91_i2c_driver = {
 	.probe		= at91_i2c_probe,
 	.remove		= __devexit_p(at91_i2c_remove),
-	.suspend	= at91_i2c_suspend,
-	.resume		= at91_i2c_resume,
 	.driver		= {
 		.name	= "at91_i2c",
 		.owner	= THIS_MODULE,
+		.pm	= AT91_I2C_PM,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-bfin-twi.c b/drivers/i2c/busses/i2c-bfin-twi.c
index cdb59e5b23f..0cf780fd6ef 100644
--- a/drivers/i2c/busses/i2c-bfin-twi.c
+++ b/drivers/i2c/busses/i2c-bfin-twi.c
@@ -25,6 +25,7 @@
 #include <asm/blackfin.h>
 #include <asm/portmux.h>
 #include <asm/irq.h>
+#include <asm/bfin_twi.h>
 
 /* SMBus mode*/
 #define TWI_I2C_MODE_STANDARD		1
@@ -32,56 +33,6 @@
 #define TWI_I2C_MODE_COMBINED		3
 #define TWI_I2C_MODE_REPEAT		4
 
-struct bfin_twi_iface {
-	int			irq;
-	spinlock_t		lock;
-	char			read_write;
-	u8			command;
-	u8			*transPtr;
-	int			readNum;
-	int			writeNum;
-	int			cur_mode;
-	int			manual_stop;
-	int			result;
-	struct i2c_adapter	adap;
-	struct completion	complete;
-	struct i2c_msg 		*pmsg;
-	int			msg_num;
-	int			cur_msg;
-	u16			saved_clkdiv;
-	u16			saved_control;
-	void __iomem		*regs_base;
-};
-
-
-#define DEFINE_TWI_REG(reg, off) \
-static inline u16 read_##reg(struct bfin_twi_iface *iface) \
-	{ return bfin_read16(iface->regs_base + (off)); } \
-static inline void write_##reg(struct bfin_twi_iface *iface, u16 v) \
-	{ bfin_write16(iface->regs_base + (off), v); }
-
-DEFINE_TWI_REG(CLKDIV, 0x00)
-DEFINE_TWI_REG(CONTROL, 0x04)
-DEFINE_TWI_REG(SLAVE_CTL, 0x08)
-DEFINE_TWI_REG(SLAVE_STAT, 0x0C)
-DEFINE_TWI_REG(SLAVE_ADDR, 0x10)
-DEFINE_TWI_REG(MASTER_CTL, 0x14)
-DEFINE_TWI_REG(MASTER_STAT, 0x18)
-DEFINE_TWI_REG(MASTER_ADDR, 0x1C)
-DEFINE_TWI_REG(INT_STAT, 0x20)
-DEFINE_TWI_REG(INT_MASK, 0x24)
-DEFINE_TWI_REG(FIFO_CTL, 0x28)
-DEFINE_TWI_REG(FIFO_STAT, 0x2C)
-DEFINE_TWI_REG(XMT_DATA8, 0x80)
-DEFINE_TWI_REG(XMT_DATA16, 0x84)
-DEFINE_TWI_REG(RCV_DATA8, 0x88)
-DEFINE_TWI_REG(RCV_DATA16, 0x8C)
-
-static const u16 pin_req[2][3] = {
-	{P_TWI0_SCL, P_TWI0_SDA, 0},
-	{P_TWI1_SCL, P_TWI1_SDA, 0},
-};
-
 static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 					unsigned short twi_int_status)
 {
@@ -99,7 +50,7 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 		 */
 		else if (iface->cur_mode == TWI_I2C_MODE_COMBINED)
 			write_MASTER_CTL(iface,
-				read_MASTER_CTL(iface) | MDIR | RSTART);
+				read_MASTER_CTL(iface) | MDIR);
 		else if (iface->manual_stop)
 			write_MASTER_CTL(iface,
 				read_MASTER_CTL(iface) | STOP);
@@ -107,10 +58,10 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 		         iface->cur_msg + 1 < iface->msg_num) {
 			if (iface->pmsg[iface->cur_msg + 1].flags & I2C_M_RD)
 				write_MASTER_CTL(iface,
-					read_MASTER_CTL(iface) | RSTART | MDIR);
+					read_MASTER_CTL(iface) | MDIR);
 			else
 				write_MASTER_CTL(iface,
-					(read_MASTER_CTL(iface) | RSTART) & ~MDIR);
+					read_MASTER_CTL(iface) & ~MDIR);
 		}
 	}
 	if (twi_int_status & RCVSERV) {
@@ -130,17 +81,25 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 			}
 			iface->transPtr++;
 			iface->readNum--;
-		} else if (iface->manual_stop) {
-			write_MASTER_CTL(iface,
-				read_MASTER_CTL(iface) | STOP);
-		} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
-		           iface->cur_msg + 1 < iface->msg_num) {
-			if (iface->pmsg[iface->cur_msg + 1].flags & I2C_M_RD)
-				write_MASTER_CTL(iface,
-					read_MASTER_CTL(iface) | RSTART | MDIR);
-			else
+		}
+
+		if (iface->readNum == 0) {
+			if (iface->manual_stop) {
+				/* Temporary workaround to avoid possible bus stall -
+				 * Flush FIFO before issuing the STOP condition
+				 */
+				read_RCV_DATA16(iface);
 				write_MASTER_CTL(iface,
-					(read_MASTER_CTL(iface) | RSTART) & ~MDIR);
+					read_MASTER_CTL(iface) | STOP);
+			} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
+					iface->cur_msg + 1 < iface->msg_num) {
+				if (iface->pmsg[iface->cur_msg + 1].flags & I2C_M_RD)
+					write_MASTER_CTL(iface,
+						read_MASTER_CTL(iface) | MDIR);
+				else
+					write_MASTER_CTL(iface,
+						read_MASTER_CTL(iface) & ~MDIR);
+			}
 		}
 	}
 	if (twi_int_status & MERR) {
@@ -193,7 +152,8 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 		return;
 	}
 	if (twi_int_status & MCOMP) {
-		if ((read_MASTER_CTL(iface) & MEN) == 0 &&
+		if (twi_int_status & (XMTSERV | RCVSERV) &&
+			(read_MASTER_CTL(iface) & MEN) == 0 &&
 			(iface->cur_mode == TWI_I2C_MODE_REPEAT ||
 			iface->cur_mode == TWI_I2C_MODE_COMBINED)) {
 			iface->result = -1;
@@ -221,7 +181,7 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 			write_MASTER_CTL(iface,
 				read_MASTER_CTL(iface) & ~RSTART);
 		} else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
-				iface->cur_msg+1 < iface->msg_num) {
+				iface->cur_msg + 1 < iface->msg_num) {
 			iface->cur_msg++;
 			iface->transPtr = iface->pmsg[iface->cur_msg].buf;
 			iface->writeNum = iface->readNum =
@@ -241,27 +201,29 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
 				}
 			}
 
-			if (iface->pmsg[iface->cur_msg].len <= 255)
-					write_MASTER_CTL(iface,
+			if (iface->pmsg[iface->cur_msg].len <= 255) {
+				write_MASTER_CTL(iface,
 					(read_MASTER_CTL(iface) &
 					(~(0xff << 6))) |
-				(iface->pmsg[iface->cur_msg].len << 6));
-			else {
+					(iface->pmsg[iface->cur_msg].len << 6));
+				iface->manual_stop = 0;
+			} else {
 				write_MASTER_CTL(iface,
 					(read_MASTER_CTL(iface) |
 					(0xff << 6)));
 				iface->manual_stop = 1;
 			}
-			/* remove restart bit and enable master receive */
-			write_MASTER_CTL(iface,
-				read_MASTER_CTL(iface) & ~RSTART);
+			/* remove restart bit before last message */
+			if (iface->cur_msg + 1 == iface->msg_num)
+				write_MASTER_CTL(iface,
+					read_MASTER_CTL(iface) & ~RSTART);
 		} else {
 			iface->result = 1;
 			write_INT_MASK(iface, 0);
 			write_MASTER_CTL(iface, 0);
 		}
+		complete(&iface->complete);
 	}
-	complete(&iface->complete);
 }
 
 /* Interrupt handler */
@@ -298,8 +260,8 @@ static int bfin_twi_do_master_xfer(struct i2c_adapter *adap,
 	if (!(read_CONTROL(iface) & TWI_ENA))
 		return -ENXIO;
 
-	while (read_MASTER_STAT(iface) & BUSBUSY)
-		yield();
+	if (read_MASTER_STAT(iface) & BUSBUSY)
+		return -EAGAIN;
 
 	iface->pmsg = msgs;
 	iface->msg_num = num;
@@ -311,7 +273,8 @@ static int bfin_twi_do_master_xfer(struct i2c_adapter *adap,
 		return -EINVAL;
 	}
 
-	iface->cur_mode = TWI_I2C_MODE_REPEAT;
+	if (iface->msg_num > 1)
+		iface->cur_mode = TWI_I2C_MODE_REPEAT;
 	iface->manual_stop = 0;
 	iface->transPtr = pmsg->buf;
 	iface->writeNum = iface->readNum = pmsg->len;
@@ -356,6 +319,7 @@ static int bfin_twi_do_master_xfer(struct i2c_adapter *adap,
 
 	/* Master enable */
 	write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
+		(iface->msg_num > 1 ? RSTART : 0) |
 		((iface->read_write == I2C_SMBUS_READ) ? MDIR : 0) |
 		((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ > 100) ? FAST : 0));
 	SSYNC();
@@ -398,8 +362,8 @@ int bfin_twi_do_smbus_xfer(struct i2c_adapter *adap, u16 addr,
 	if (!(read_CONTROL(iface) & TWI_ENA))
 		return -ENXIO;
 
-	while (read_MASTER_STAT(iface) & BUSBUSY)
-		yield();
+	if (read_MASTER_STAT(iface) & BUSBUSY)
+		return -EAGAIN;
 
 	iface->writeNum = 0;
 	iface->readNum = 0;
@@ -520,7 +484,7 @@ int bfin_twi_do_smbus_xfer(struct i2c_adapter *adap, u16 addr,
 		else
 			write_MASTER_CTL(iface, 0x1 << 6);
 		/* Master enable */
-		write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN |
+		write_MASTER_CTL(iface, read_MASTER_CTL(iface) | MEN | RSTART |
 			((CONFIG_I2C_BLACKFIN_TWI_CLK_KHZ>100) ? FAST : 0));
 		break;
 	default:
@@ -611,9 +575,9 @@ static struct i2c_algorithm bfin_twi_algorithm = {
 	.functionality = bfin_twi_functionality,
 };
 
-static int i2c_bfin_twi_suspend(struct platform_device *pdev, pm_message_t state)
+static int i2c_bfin_twi_suspend(struct device *dev)
 {
-	struct bfin_twi_iface *iface = platform_get_drvdata(pdev);
+	struct bfin_twi_iface *iface = dev_get_drvdata(dev);
 
 	iface->saved_clkdiv = read_CLKDIV(iface);
 	iface->saved_control = read_CONTROL(iface);
@@ -626,14 +590,14 @@ static int i2c_bfin_twi_suspend(struct platform_device *pdev, pm_message_t state
 	return 0;
 }
 
-static int i2c_bfin_twi_resume(struct platform_device *pdev)
+static int i2c_bfin_twi_resume(struct device *dev)
 {
-	struct bfin_twi_iface *iface = platform_get_drvdata(pdev);
+	struct bfin_twi_iface *iface = dev_get_drvdata(dev);
 
 	int rc = request_irq(iface->irq, bfin_twi_interrupt_entry,
-		0, pdev->name, iface);
+		0, to_platform_device(dev)->name, iface);
 	if (rc) {
-		dev_err(&pdev->dev, "Can't get IRQ %d !\n", iface->irq);
+		dev_err(dev, "Can't get IRQ %d !\n", iface->irq);
 		return -ENODEV;
 	}
 
@@ -646,6 +610,9 @@ static int i2c_bfin_twi_resume(struct platform_device *pdev)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(i2c_bfin_twi_pm,
+			 i2c_bfin_twi_suspend, i2c_bfin_twi_resume);
+
 static int i2c_bfin_twi_probe(struct platform_device *pdev)
 {
 	struct bfin_twi_iface *iface;
@@ -695,7 +662,8 @@ static int i2c_bfin_twi_probe(struct platform_device *pdev)
 	p_adap->timeout = 5 * HZ;
 	p_adap->retries = 3;
 
-	rc = peripheral_request_list(pin_req[pdev->id], "i2c-bfin-twi");
+	rc = peripheral_request_list((unsigned short *)pdev->dev.platform_data,
+					"i2c-bfin-twi");
 	if (rc) {
 		dev_err(&pdev->dev, "Can't setup pin mux!\n");
 		goto out_error_pin_mux;
@@ -742,7 +710,7 @@ out_error_add_adapter:
 	free_irq(iface->irq, iface);
 out_error_req_irq:
 out_error_no_irq:
-	peripheral_free_list(pin_req[pdev->id]);
+	peripheral_free_list((unsigned short *)pdev->dev.platform_data);
 out_error_pin_mux:
 	iounmap(iface->regs_base);
 out_error_ioremap:
@@ -760,7 +728,7 @@ static int i2c_bfin_twi_remove(struct platform_device *pdev)
 
 	i2c_del_adapter(&(iface->adap));
 	free_irq(iface->irq, iface);
-	peripheral_free_list(pin_req[pdev->id]);
+	peripheral_free_list((unsigned short *)pdev->dev.platform_data);
 	iounmap(iface->regs_base);
 	kfree(iface);
 
@@ -770,11 +738,10 @@ static int i2c_bfin_twi_remove(struct platform_device *pdev)
 static struct platform_driver i2c_bfin_twi_driver = {
 	.probe		= i2c_bfin_twi_probe,
 	.remove		= i2c_bfin_twi_remove,
-	.suspend	= i2c_bfin_twi_suspend,
-	.resume		= i2c_bfin_twi_resume,
 	.driver		= {
 		.name	= "i2c-bfin-twi",
 		.owner	= THIS_MODULE,
+		.pm	= &i2c_bfin_twi_pm,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-designware-pcidrv.c b/drivers/i2c/busses/i2c-designware-pcidrv.c
index 00e8f213f56..92a1e2c15ba 100644
--- a/drivers/i2c/busses/i2c-designware-pcidrv.c
+++ b/drivers/i2c/busses/i2c-designware-pcidrv.c
@@ -374,17 +374,7 @@ static struct pci_driver dw_i2c_driver = {
 	},
 };
 
-static int __init dw_i2c_init_driver(void)
-{
-	return  pci_register_driver(&dw_i2c_driver);
-}
-module_init(dw_i2c_init_driver);
-
-static void __exit dw_i2c_exit_driver(void)
-{
-	pci_unregister_driver(&dw_i2c_driver);
-}
-module_exit(dw_i2c_exit_driver);
+module_pci_driver(dw_i2c_driver);
 
 MODULE_AUTHOR("Baruch Siach <baruch@tkos.co.il>");
 MODULE_DESCRIPTION("Synopsys DesignWare PCI I2C bus adapter");
diff --git a/drivers/i2c/busses/i2c-diolan-u2c.c b/drivers/i2c/busses/i2c-diolan-u2c.c
index 7eb19a5222f..aedb94f34bf 100644
--- a/drivers/i2c/busses/i2c-diolan-u2c.c
+++ b/drivers/i2c/busses/i2c-diolan-u2c.c
@@ -517,6 +517,6 @@ static struct usb_driver diolan_u2c_driver = {
 
 module_usb_driver(diolan_u2c_driver);
 
-MODULE_AUTHOR("Guenter Roeck <guenter.roeck@ericsson.com>");
+MODULE_AUTHOR("Guenter Roeck <linux@roeck-us.net>");
 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 2f74ae872e1..259f7697bf2 100644
--- a/drivers/i2c/busses/i2c-eg20t.c
+++ b/drivers/i2c/busses/i2c-eg20t.c
@@ -953,17 +953,7 @@ static struct pci_driver pch_pcidriver = {
 	.resume = pch_i2c_resume
 };
 
-static int __init pch_pci_init(void)
-{
-	return pci_register_driver(&pch_pcidriver);
-}
-module_init(pch_pci_init);
-
-static void __exit pch_pci_exit(void)
-{
-	pci_unregister_driver(&pch_pcidriver);
-}
-module_exit(pch_pci_exit);
+module_pci_driver(pch_pcidriver);
 
 MODULE_DESCRIPTION("Intel EG20T PCH/LAPIS Semico ML7213/ML7223/ML7831 IOH I2C");
 MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-hydra.c b/drivers/i2c/busses/i2c-hydra.c
index c527de17db4..c9f95e1666a 100644
--- a/drivers/i2c/busses/i2c-hydra.c
+++ b/drivers/i2c/busses/i2c-hydra.c
@@ -156,23 +156,8 @@ static struct pci_driver hydra_driver = {
 	.remove		= __devexit_p(hydra_remove),
 };
 
-static int __init i2c_hydra_init(void)
-{
-	return pci_register_driver(&hydra_driver);
-}
-
-
-static void __exit i2c_hydra_exit(void)
-{
-	pci_unregister_driver(&hydra_driver);
-}
-
-
+module_pci_driver(hydra_driver);
 
 MODULE_AUTHOR("Geert Uytterhoeven <geert@linux-m68k.org>");
 MODULE_DESCRIPTION("i2c for Apple Hydra Mac I/O");
 MODULE_LICENSE("GPL");
-
-module_init(i2c_hydra_init);
-module_exit(i2c_hydra_exit);
-
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index ae2945a5e00..898dcf9c7ad 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -60,10 +60,12 @@
   Block process call transaction   no
   I2C block read transaction       yes  (doesn't use the block buffer)
   Slave mode                       no
+  Interrupt processing             yes
 
   See the file Documentation/i2c/busses/i2c-i801 for details.
 */
 
+#include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/kernel.h>
@@ -76,6 +78,7 @@
 #include <linux/io.h>
 #include <linux/dmi.h>
 #include <linux/slab.h>
+#include <linux/wait.h>
 
 /* I801 SMBus address offsets */
 #define SMBHSTSTS(p)	(0 + (p)->smba)
@@ -91,8 +94,12 @@
 
 /* PCI Address Constants */
 #define SMBBAR		4
+#define SMBPCISTS	0x006
 #define SMBHSTCFG	0x040
 
+/* Host status bits for SMBPCISTS */
+#define SMBPCISTS_INTS		0x08
+
 /* Host configuration bits for SMBHSTCFG */
 #define SMBHSTCFG_HST_EN	1
 #define SMBHSTCFG_SMB_SMI_EN	2
@@ -102,12 +109,8 @@
 #define SMBAUXCTL_CRC		1
 #define SMBAUXCTL_E32B		2
 
-/* kill bit for SMBHSTCNT */
-#define SMBHSTCNT_KILL		2
-
 /* Other settings */
 #define MAX_RETRIES		400
-#define ENABLE_INT9		0	/* set to 0x01 to enable - untested */
 
 /* I801 command constants */
 #define I801_QUICK		0x00
@@ -117,10 +120,13 @@
 #define I801_PROC_CALL		0x10	/* unimplemented */
 #define I801_BLOCK_DATA		0x14
 #define I801_I2C_BLOCK_DATA	0x18	/* ICH5 and later */
-#define I801_BLOCK_LAST		0x34
-#define I801_I2C_BLOCK_LAST	0x38	/* ICH5 and later */
-#define I801_START		0x40
-#define I801_PEC_EN		0x80	/* ICH3 and later */
+
+/* I801 Host Control register bits */
+#define SMBHSTCNT_INTREN	0x01
+#define SMBHSTCNT_KILL		0x02
+#define SMBHSTCNT_LAST_BYTE	0x20
+#define SMBHSTCNT_START		0x40
+#define SMBHSTCNT_PEC_EN	0x80	/* ICH3 and later */
 
 /* I801 Hosts Status register bits */
 #define SMBHSTSTS_BYTE_DONE	0x80
@@ -132,9 +138,11 @@
 #define SMBHSTSTS_INTR		0x02
 #define SMBHSTSTS_HOST_BUSY	0x01
 
-#define STATUS_FLAGS		(SMBHSTSTS_BYTE_DONE | SMBHSTSTS_FAILED | \
-				 SMBHSTSTS_BUS_ERR | SMBHSTSTS_DEV_ERR | \
-				 SMBHSTSTS_INTR)
+#define STATUS_ERROR_FLAGS	(SMBHSTSTS_FAILED | SMBHSTSTS_BUS_ERR | \
+				 SMBHSTSTS_DEV_ERR)
+
+#define STATUS_FLAGS		(SMBHSTSTS_BYTE_DONE | SMBHSTSTS_INTR | \
+				 STATUS_ERROR_FLAGS)
 
 /* Older devices have their ID defined in <linux/pci_ids.h> */
 #define PCI_DEVICE_ID_INTEL_COUGARPOINT_SMBUS	0x1c22
@@ -154,6 +162,17 @@ struct i801_priv {
 	unsigned char original_hstcfg;
 	struct pci_dev *pci_dev;
 	unsigned int features;
+
+	/* isr processing */
+	wait_queue_head_t waitq;
+	u8 status;
+
+	/* Command state used by isr for byte-by-byte block transactions */
+	u8 cmd;
+	bool is_read;
+	int count;
+	int len;
+	u8 *data;
 };
 
 static struct pci_driver i801_driver;
@@ -162,6 +181,7 @@ static struct pci_driver i801_driver;
 #define FEATURE_BLOCK_BUFFER	(1 << 1)
 #define FEATURE_BLOCK_PROC	(1 << 2)
 #define FEATURE_I2C_BLOCK_READ	(1 << 3)
+#define FEATURE_IRQ		(1 << 4)
 /* Not really a feature, but it's convenient to handle it as such */
 #define FEATURE_IDF		(1 << 15)
 
@@ -170,6 +190,7 @@ static const char *i801_feature_names[] = {
 	"Block buffer",
 	"Block process call",
 	"I2C block read",
+	"Interrupt",
 };
 
 static unsigned int disable_features;
@@ -205,13 +226,22 @@ static int i801_check_pre(struct i801_priv *priv)
 	return 0;
 }
 
-/* Convert the status register to an error code, and clear it. */
-static int i801_check_post(struct i801_priv *priv, int status, int timeout)
+/*
+ * Convert the status register to an error code, and clear it.
+ * Note that status only contains the bits we want to clear, not the
+ * actual register value.
+ */
+static int i801_check_post(struct i801_priv *priv, int status)
 {
 	int result = 0;
 
-	/* If the SMBus is still busy, we give up */
-	if (timeout) {
+	/*
+	 * If the SMBus is still busy, we give up
+	 * Note: This timeout condition only happens when using polling
+	 * transactions.  For interrupt operation, NAK/timeout is indicated by
+	 * DEV_ERR.
+	 */
+	if (unlikely(status < 0)) {
 		dev_err(&priv->pci_dev->dev, "Transaction timeout\n");
 		/* try to stop the current command */
 		dev_dbg(&priv->pci_dev->dev, "Terminating the current operation\n");
@@ -244,64 +274,76 @@ static int i801_check_post(struct i801_priv *priv, int status, int timeout)
 		dev_dbg(&priv->pci_dev->dev, "Lost arbitration\n");
 	}
 
-	if (result) {
-		/* Clear error flags */
-		outb_p(status & STATUS_FLAGS, SMBHSTSTS(priv));
-		status = inb_p(SMBHSTSTS(priv)) & STATUS_FLAGS;
-		if (status) {
-			dev_warn(&priv->pci_dev->dev, "Failed clearing status "
-				 "flags at end of transaction (%02x)\n",
-				 status);
-		}
-	}
+	/* Clear status flags except BYTE_DONE, to be cleared by caller */
+	outb_p(status, SMBHSTSTS(priv));
 
 	return result;
 }
 
-static int i801_transaction(struct i801_priv *priv, int xact)
+/* Wait for BUSY being cleared and either INTR or an error flag being set */
+static int i801_wait_intr(struct i801_priv *priv)
 {
-	int status;
-	int result;
 	int timeout = 0;
-
-	result = i801_check_pre(priv);
-	if (result < 0)
-		return result;
-
-	/* the current contents of SMBHSTCNT can be overwritten, since PEC,
-	 * INTREN, SMBSCMD are passed in xact */
-	outb_p(xact | I801_START, SMBHSTCNT(priv));
+	int status;
 
 	/* We will always wait for a fraction of a second! */
 	do {
 		usleep_range(250, 500);
 		status = inb_p(SMBHSTSTS(priv));
-	} while ((status & SMBHSTSTS_HOST_BUSY) && (timeout++ < MAX_RETRIES));
+	} while (((status & SMBHSTSTS_HOST_BUSY) ||
+		  !(status & (STATUS_ERROR_FLAGS | SMBHSTSTS_INTR))) &&
+		 (timeout++ < MAX_RETRIES));
 
-	result = i801_check_post(priv, status, timeout > MAX_RETRIES);
-	if (result < 0)
-		return result;
-
-	outb_p(SMBHSTSTS_INTR, SMBHSTSTS(priv));
-	return 0;
+	if (timeout > MAX_RETRIES) {
+		dev_dbg(&priv->pci_dev->dev, "INTR Timeout!\n");
+		return -ETIMEDOUT;
+	}
+	return status & (STATUS_ERROR_FLAGS | SMBHSTSTS_INTR);
 }
 
-/* wait for INTR bit as advised by Intel */
-static void i801_wait_hwpec(struct i801_priv *priv)
+/* Wait for either BYTE_DONE or an error flag being set */
+static int i801_wait_byte_done(struct i801_priv *priv)
 {
 	int timeout = 0;
 	int status;
 
+	/* We will always wait for a fraction of a second! */
 	do {
 		usleep_range(250, 500);
 		status = inb_p(SMBHSTSTS(priv));
-	} while ((!(status & SMBHSTSTS_INTR))
-		 && (timeout++ < MAX_RETRIES));
+	} while (!(status & (STATUS_ERROR_FLAGS | SMBHSTSTS_BYTE_DONE)) &&
+		 (timeout++ < MAX_RETRIES));
 
-	if (timeout > MAX_RETRIES)
-		dev_dbg(&priv->pci_dev->dev, "PEC Timeout!\n");
+	if (timeout > MAX_RETRIES) {
+		dev_dbg(&priv->pci_dev->dev, "BYTE_DONE Timeout!\n");
+		return -ETIMEDOUT;
+	}
+	return status & STATUS_ERROR_FLAGS;
+}
 
-	outb_p(status, SMBHSTSTS(priv));
+static int i801_transaction(struct i801_priv *priv, int xact)
+{
+	int status;
+	int result;
+
+	result = i801_check_pre(priv);
+	if (result < 0)
+		return result;
+
+	if (priv->features & FEATURE_IRQ) {
+		outb_p(xact | SMBHSTCNT_INTREN | SMBHSTCNT_START,
+		       SMBHSTCNT(priv));
+		wait_event(priv->waitq, (status = priv->status));
+		priv->status = 0;
+		return i801_check_post(priv, status);
+	}
+
+	/* the current contents of SMBHSTCNT can be overwritten, since PEC,
+	 * SMBSCMD are passed in xact */
+	outb_p(xact | SMBHSTCNT_START, SMBHSTCNT(priv));
+
+	status = i801_wait_intr(priv);
+	return i801_check_post(priv, status);
 }
 
 static int i801_block_transaction_by_block(struct i801_priv *priv,
@@ -321,8 +363,8 @@ static int i801_block_transaction_by_block(struct i801_priv *priv,
 			outb_p(data->block[i+1], SMBBLKDAT(priv));
 	}
 
-	status = i801_transaction(priv, I801_BLOCK_DATA | ENABLE_INT9 |
-				  I801_PEC_EN * hwpec);
+	status = i801_transaction(priv, I801_BLOCK_DATA |
+				  (hwpec ? SMBHSTCNT_PEC_EN : 0));
 	if (status)
 		return status;
 
@@ -338,6 +380,98 @@ static int i801_block_transaction_by_block(struct i801_priv *priv,
 	return 0;
 }
 
+static void i801_isr_byte_done(struct i801_priv *priv)
+{
+	if (priv->is_read) {
+		/* For SMBus block reads, length is received with first byte */
+		if (((priv->cmd & 0x1c) == I801_BLOCK_DATA) &&
+		    (priv->count == 0)) {
+			priv->len = inb_p(SMBHSTDAT0(priv));
+			if (priv->len < 1 || priv->len > I2C_SMBUS_BLOCK_MAX) {
+				dev_err(&priv->pci_dev->dev,
+					"Illegal SMBus block read size %d\n",
+					priv->len);
+				/* FIXME: Recover */
+				priv->len = I2C_SMBUS_BLOCK_MAX;
+			} else {
+				dev_dbg(&priv->pci_dev->dev,
+					"SMBus block read size is %d\n",
+					priv->len);
+			}
+			priv->data[-1] = priv->len;
+		}
+
+		/* Read next byte */
+		if (priv->count < priv->len)
+			priv->data[priv->count++] = inb(SMBBLKDAT(priv));
+		else
+			dev_dbg(&priv->pci_dev->dev,
+				"Discarding extra byte on block read\n");
+
+		/* Set LAST_BYTE for last byte of read transaction */
+		if (priv->count == priv->len - 1)
+			outb_p(priv->cmd | SMBHSTCNT_LAST_BYTE,
+			       SMBHSTCNT(priv));
+	} else if (priv->count < priv->len - 1) {
+		/* Write next byte, except for IRQ after last byte */
+		outb_p(priv->data[++priv->count], SMBBLKDAT(priv));
+	}
+
+	/* Clear BYTE_DONE to continue with next byte */
+	outb_p(SMBHSTSTS_BYTE_DONE, SMBHSTSTS(priv));
+}
+
+/*
+ * There are two kinds of interrupts:
+ *
+ * 1) i801 signals transaction completion with one of these interrupts:
+ *      INTR - Success
+ *      DEV_ERR - Invalid command, NAK or communication timeout
+ *      BUS_ERR - SMI# transaction collision
+ *      FAILED - transaction was canceled due to a KILL request
+ *    When any of these occur, update ->status and wake up the waitq.
+ *    ->status must be cleared before kicking off the next transaction.
+ *
+ * 2) For byte-by-byte (I2C read/write) transactions, one BYTE_DONE interrupt
+ *    occurs for each byte of a byte-by-byte to prepare the next byte.
+ */
+static irqreturn_t i801_isr(int irq, void *dev_id)
+{
+	struct i801_priv *priv = dev_id;
+	u16 pcists;
+	u8 status;
+
+	/* Confirm this is our interrupt */
+	pci_read_config_word(priv->pci_dev, SMBPCISTS, &pcists);
+	if (!(pcists & SMBPCISTS_INTS))
+		return IRQ_NONE;
+
+	status = inb_p(SMBHSTSTS(priv));
+	if (status != 0x42)
+		dev_dbg(&priv->pci_dev->dev, "irq: status = %02x\n", status);
+
+	if (status & SMBHSTSTS_BYTE_DONE)
+		i801_isr_byte_done(priv);
+
+	/*
+	 * Clear irq sources and report transaction result.
+	 * ->status must be cleared before the next transaction is started.
+	 */
+	status &= SMBHSTSTS_INTR | STATUS_ERROR_FLAGS;
+	if (status) {
+		outb_p(status, SMBHSTSTS(priv));
+		priv->status |= status;
+		wake_up(&priv->waitq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * For "byte-by-byte" block transactions:
+ *   I2C write uses cmd=I801_BLOCK_DATA, I2C_EN=1
+ *   I2C read uses cmd=I801_I2C_BLOCK_DATA
+ */
 static int i801_block_transaction_byte_by_byte(struct i801_priv *priv,
 					       union i2c_smbus_data *data,
 					       char read_write, int command,
@@ -347,7 +481,6 @@ static int i801_block_transaction_byte_by_byte(struct i801_priv *priv,
 	int smbcmd;
 	int status;
 	int result;
-	int timeout;
 
 	result = i801_check_pre(priv);
 	if (result < 0)
@@ -360,36 +493,39 @@ static int i801_block_transaction_byte_by_byte(struct i801_priv *priv,
 		outb_p(data->block[1], SMBBLKDAT(priv));
 	}
 
+	if (command == I2C_SMBUS_I2C_BLOCK_DATA &&
+	    read_write == I2C_SMBUS_READ)
+		smbcmd = I801_I2C_BLOCK_DATA;
+	else
+		smbcmd = I801_BLOCK_DATA;
+
+	if (priv->features & FEATURE_IRQ) {
+		priv->is_read = (read_write == I2C_SMBUS_READ);
+		if (len == 1 && priv->is_read)
+			smbcmd |= SMBHSTCNT_LAST_BYTE;
+		priv->cmd = smbcmd | SMBHSTCNT_INTREN;
+		priv->len = len;
+		priv->count = 0;
+		priv->data = &data->block[1];
+
+		outb_p(priv->cmd | SMBHSTCNT_START, SMBHSTCNT(priv));
+		wait_event(priv->waitq, (status = priv->status));
+		priv->status = 0;
+		return i801_check_post(priv, status);
+	}
+
 	for (i = 1; i <= len; i++) {
-		if (i == len && read_write == I2C_SMBUS_READ) {
-			if (command == I2C_SMBUS_I2C_BLOCK_DATA)
-				smbcmd = I801_I2C_BLOCK_LAST;
-			else
-				smbcmd = I801_BLOCK_LAST;
-		} else {
-			if (command == I2C_SMBUS_I2C_BLOCK_DATA
-			 && read_write == I2C_SMBUS_READ)
-				smbcmd = I801_I2C_BLOCK_DATA;
-			else
-				smbcmd = I801_BLOCK_DATA;
-		}
-		outb_p(smbcmd | ENABLE_INT9, SMBHSTCNT(priv));
+		if (i == len && read_write == I2C_SMBUS_READ)
+			smbcmd |= SMBHSTCNT_LAST_BYTE;
+		outb_p(smbcmd, SMBHSTCNT(priv));
 
 		if (i == 1)
-			outb_p(inb(SMBHSTCNT(priv)) | I801_START,
+			outb_p(inb(SMBHSTCNT(priv)) | SMBHSTCNT_START,
 			       SMBHSTCNT(priv));
 
-		/* We will always wait for a fraction of a second! */
-		timeout = 0;
-		do {
-			usleep_range(250, 500);
-			status = inb_p(SMBHSTSTS(priv));
-		} while ((!(status & SMBHSTSTS_BYTE_DONE))
-			 && (timeout++ < MAX_RETRIES));
-
-		result = i801_check_post(priv, status, timeout > MAX_RETRIES);
-		if (result < 0)
-			return result;
+		status = i801_wait_byte_done(priv);
+		if (status)
+			goto exit;
 
 		if (i == 1 && read_write == I2C_SMBUS_READ
 		 && command != I2C_SMBUS_I2C_BLOCK_DATA) {
@@ -416,10 +552,12 @@ static int i801_block_transaction_byte_by_byte(struct i801_priv *priv,
 			outb_p(data->block[i+1], SMBBLKDAT(priv));
 
 		/* signals SMBBLKDAT ready */
-		outb_p(SMBHSTSTS_BYTE_DONE | SMBHSTSTS_INTR, SMBHSTSTS(priv));
+		outb_p(SMBHSTSTS_BYTE_DONE, SMBHSTSTS(priv));
 	}
 
-	return 0;
+	status = i801_wait_intr(priv);
+exit:
+	return i801_check_post(priv, status);
 }
 
 static int i801_set_block_buffer_mode(struct i801_priv *priv)
@@ -474,9 +612,6 @@ static int i801_block_transaction(struct i801_priv *priv,
 							     read_write,
 							     command, hwpec);
 
-	if (result == 0 && hwpec)
-		i801_wait_hwpec(priv);
-
 	if (command == I2C_SMBUS_I2C_BLOCK_DATA
 	 && read_write == I2C_SMBUS_WRITE) {
 		/* restore saved configuration register value */
@@ -564,7 +699,7 @@ static s32 i801_access(struct i2c_adapter *adap, u16 addr,
 		ret = i801_block_transaction(priv, data, read_write, size,
 					     hwpec);
 	else
-		ret = i801_transaction(priv, xact | ENABLE_INT9);
+		ret = i801_transaction(priv, xact);
 
 	/* Some BIOSes don't like it when PEC is enabled at reboot or resume
 	   time, so we forcibly disable it after every transaction. Turn off
@@ -799,6 +934,16 @@ static int __devinit i801_probe(struct pci_dev *dev,
 		break;
 	}
 
+	/* IRQ processing tested on CougarPoint PCH, ICH5, ICH7-M and ICH10 */
+	if (dev->device == PCI_DEVICE_ID_INTEL_COUGARPOINT_SMBUS ||
+	    dev->device == PCI_DEVICE_ID_INTEL_82801EB_3 ||
+	    dev->device == PCI_DEVICE_ID_INTEL_ICH7_17 ||
+	    dev->device == PCI_DEVICE_ID_INTEL_ICH8_5 ||
+	    dev->device == PCI_DEVICE_ID_INTEL_ICH9_6 ||
+	    dev->device == PCI_DEVICE_ID_INTEL_ICH10_4 ||
+	    dev->device == PCI_DEVICE_ID_INTEL_ICH10_5)
+		priv->features |= FEATURE_IRQ;
+
 	/* Disable features on user request */
 	for (i = 0; i < ARRAY_SIZE(i801_feature_names); i++) {
 		if (priv->features & disable_features & (1 << i))
@@ -846,16 +991,30 @@ static int __devinit i801_probe(struct pci_dev *dev,
 	}
 	pci_write_config_byte(priv->pci_dev, SMBHSTCFG, temp);
 
-	if (temp & SMBHSTCFG_SMB_SMI_EN)
+	if (temp & SMBHSTCFG_SMB_SMI_EN) {
 		dev_dbg(&dev->dev, "SMBus using interrupt SMI#\n");
-	else
-		dev_dbg(&dev->dev, "SMBus using PCI Interrupt\n");
+		/* Disable SMBus interrupt feature if SMBus using SMI# */
+		priv->features &= ~FEATURE_IRQ;
+	}
 
 	/* Clear special mode bits */
 	if (priv->features & (FEATURE_SMBUS_PEC | FEATURE_BLOCK_BUFFER))
 		outb_p(inb_p(SMBAUXCTL(priv)) &
 		       ~(SMBAUXCTL_CRC | SMBAUXCTL_E32B), SMBAUXCTL(priv));
 
+	if (priv->features & FEATURE_IRQ) {
+		init_waitqueue_head(&priv->waitq);
+
+		err = request_irq(dev->irq, i801_isr, IRQF_SHARED,
+				  i801_driver.name, priv);
+		if (err) {
+			dev_err(&dev->dev, "Failed to allocate irq %d: %d\n",
+				dev->irq, err);
+			goto exit_release;
+		}
+		dev_info(&dev->dev, "SMBus using PCI Interrupt\n");
+	}
+
 	/* set up the sysfs linkage to our parent device */
 	priv->adapter.dev.parent = &dev->dev;
 
@@ -867,14 +1026,18 @@ static int __devinit i801_probe(struct pci_dev *dev,
 	err = i2c_add_adapter(&priv->adapter);
 	if (err) {
 		dev_err(&dev->dev, "Failed to add SMBus adapter\n");
-		goto exit_release;
+		goto exit_free_irq;
 	}
 
 	i801_probe_optional_slaves(priv);
 
 	pci_set_drvdata(dev, priv);
+
 	return 0;
 
+exit_free_irq:
+	if (priv->features & FEATURE_IRQ)
+		free_irq(dev->irq, priv);
 exit_release:
 	pci_release_region(dev, SMBBAR);
 exit:
@@ -888,7 +1051,11 @@ static void __devexit i801_remove(struct pci_dev *dev)
 
 	i2c_del_adapter(&priv->adapter);
 	pci_write_config_byte(dev, SMBHSTCFG, priv->original_hstcfg);
+
+	if (priv->features & FEATURE_IRQ)
+		free_irq(dev->irq, priv);
 	pci_release_region(dev, SMBBAR);
+
 	pci_set_drvdata(dev, NULL);
 	kfree(priv);
 	/*
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 8d6b504d65c..0722f869465 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -53,7 +53,6 @@
 #include <linux/of_i2c.h>
 #include <linux/pinctrl/consumer.h>
 
-#include <mach/irqs.h>
 #include <mach/hardware.h>
 #include <mach/i2c.h>
 
@@ -118,10 +117,8 @@ static u16 __initdata i2c_clk_div[50][2] = {
 
 struct imx_i2c_struct {
 	struct i2c_adapter	adapter;
-	struct resource		*res;
 	struct clk		*clk;
 	void __iomem		*base;
-	int			irq;
 	wait_queue_head_t	queue;
 	unsigned long		i2csr;
 	unsigned int 		disable_delay;
@@ -473,9 +470,8 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	struct imxi2c_platform_data *pdata = pdev->dev.platform_data;
 	struct pinctrl *pinctrl;
 	void __iomem *base;
-	resource_size_t res_size;
-	int irq, bitrate;
-	int ret;
+	int irq, ret;
+	u32 bitrate;
 
 	dev_dbg(&pdev->dev, "<%s>\n", __func__);
 
@@ -490,25 +486,15 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	res_size = resource_size(res);
-
-	if (!request_mem_region(res->start, res_size, DRIVER_NAME)) {
-		dev_err(&pdev->dev, "request_mem_region failed\n");
+	base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!base)
 		return -EBUSY;
-	}
-
-	base = ioremap(res->start, res_size);
-	if (!base) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -EIO;
-		goto fail1;
-	}
 
-	i2c_imx = kzalloc(sizeof(struct imx_i2c_struct), GFP_KERNEL);
+	i2c_imx = devm_kzalloc(&pdev->dev, sizeof(struct imx_i2c_struct),
+				GFP_KERNEL);
 	if (!i2c_imx) {
 		dev_err(&pdev->dev, "can't allocate interface\n");
-		ret = -ENOMEM;
-		goto fail2;
+		return -ENOMEM;
 	}
 
 	/* Setup i2c_imx driver structure */
@@ -518,29 +504,27 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	i2c_imx->adapter.dev.parent	= &pdev->dev;
 	i2c_imx->adapter.nr 		= pdev->id;
 	i2c_imx->adapter.dev.of_node	= pdev->dev.of_node;
-	i2c_imx->irq			= irq;
 	i2c_imx->base			= base;
-	i2c_imx->res			= res;
 
 	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
 	if (IS_ERR(pinctrl)) {
-		ret = PTR_ERR(pinctrl);
-		goto fail3;
+		dev_err(&pdev->dev, "can't get/select pinctrl\n");
+		return PTR_ERR(pinctrl);
 	}
 
 	/* Get I2C clock */
-	i2c_imx->clk = clk_get(&pdev->dev, "i2c_clk");
+	i2c_imx->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(i2c_imx->clk)) {
-		ret = PTR_ERR(i2c_imx->clk);
 		dev_err(&pdev->dev, "can't get I2C clock\n");
-		goto fail3;
+		return PTR_ERR(i2c_imx->clk);
 	}
 
 	/* Request IRQ */
-	ret = request_irq(i2c_imx->irq, i2c_imx_isr, 0, pdev->name, i2c_imx);
+	ret = devm_request_irq(&pdev->dev, irq, i2c_imx_isr, 0,
+				pdev->name, i2c_imx);
 	if (ret) {
-		dev_err(&pdev->dev, "can't claim irq %d\n", i2c_imx->irq);
-		goto fail4;
+		dev_err(&pdev->dev, "can't claim irq %d\n", irq);
+		return ret;
 	}
 
 	/* Init queue */
@@ -565,7 +549,7 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	ret = i2c_add_numbered_adapter(&i2c_imx->adapter);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "registration failed\n");
-		goto fail5;
+		return ret;
 	}
 
 	of_i2c_register_devices(&i2c_imx->adapter);
@@ -573,28 +557,16 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	/* Set up platform driver data */
 	platform_set_drvdata(pdev, i2c_imx);
 
-	dev_dbg(&i2c_imx->adapter.dev, "claimed irq %d\n", i2c_imx->irq);
+	dev_dbg(&i2c_imx->adapter.dev, "claimed irq %d\n", irq);
 	dev_dbg(&i2c_imx->adapter.dev, "device resources from 0x%x to 0x%x\n",
-		i2c_imx->res->start, i2c_imx->res->end);
-	dev_dbg(&i2c_imx->adapter.dev, "allocated %d bytes at 0x%x \n",
-		res_size, i2c_imx->res->start);
+		res->start, res->end);
+	dev_dbg(&i2c_imx->adapter.dev, "allocated %d bytes at 0x%x\n",
+		resource_size(res), res->start);
 	dev_dbg(&i2c_imx->adapter.dev, "adapter name: \"%s\"\n",
 		i2c_imx->adapter.name);
 	dev_dbg(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
 
 	return 0;   /* Return OK */
-
-fail5:
-	free_irq(i2c_imx->irq, i2c_imx);
-fail4:
-	clk_put(i2c_imx->clk);
-fail3:
-	kfree(i2c_imx);
-fail2:
-	iounmap(base);
-fail1:
-	release_mem_region(res->start, resource_size(res));
-	return ret; /* Return error number */
 }
 
 static int __exit i2c_imx_remove(struct platform_device *pdev)
@@ -606,20 +578,12 @@ static int __exit i2c_imx_remove(struct platform_device *pdev)
 	i2c_del_adapter(&i2c_imx->adapter);
 	platform_set_drvdata(pdev, NULL);
 
-	/* free interrupt */
-	free_irq(i2c_imx->irq, i2c_imx);
-
 	/* setup chip registers to defaults */
 	writeb(0, i2c_imx->base + IMX_I2C_IADR);
 	writeb(0, i2c_imx->base + IMX_I2C_IFDR);
 	writeb(0, i2c_imx->base + IMX_I2C_I2CR);
 	writeb(0, i2c_imx->base + IMX_I2C_I2SR);
 
-	clk_put(i2c_imx->clk);
-
-	iounmap(i2c_imx->base);
-	release_mem_region(i2c_imx->res->start, resource_size(i2c_imx->res));
-	kfree(i2c_imx);
 	return 0;
 }
 
diff --git a/drivers/i2c/busses/i2c-intel-mid.c b/drivers/i2c/busses/i2c-intel-mid.c
index 365bad5b890..7c28f10f95c 100644
--- a/drivers/i2c/busses/i2c-intel-mid.c
+++ b/drivers/i2c/busses/i2c-intel-mid.c
@@ -1116,18 +1116,7 @@ static struct pci_driver intel_mid_i2c_driver = {
 	.remove		= __devexit_p(intel_mid_i2c_remove),
 };
 
-static int __init intel_mid_i2c_init(void)
-{
-	return pci_register_driver(&intel_mid_i2c_driver);
-}
-
-static void __exit intel_mid_i2c_exit(void)
-{
-	pci_unregister_driver(&intel_mid_i2c_driver);
-}
-
-module_init(intel_mid_i2c_init);
-module_exit(intel_mid_i2c_exit);
+module_pci_driver(intel_mid_i2c_driver);
 
 MODULE_AUTHOR("Ba Zheng <zheng.ba@intel.com>");
 MODULE_DESCRIPTION("I2C driver for Moorestown Platform");
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 4f44a33017b..2e9d56719e9 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -18,6 +18,11 @@
 #include <linux/mv643xx_i2c.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_i2c.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 
 /* Register defines */
 #define	MV64XXX_I2C_REG_SLAVE_ADDR			0x00
@@ -98,6 +103,9 @@ struct mv64xxx_i2c_data {
 	int			rc;
 	u32			freq_m;
 	u32			freq_n;
+#if defined(CONFIG_HAVE_CLK)
+	struct clk              *clk;
+#endif
 	wait_queue_head_t	waitq;
 	spinlock_t		lock;
 	struct i2c_msg		*msg;
@@ -521,6 +529,82 @@ mv64xxx_i2c_unmap_regs(struct mv64xxx_i2c_data *drv_data)
 	drv_data->reg_base_p = 0;
 }
 
+#ifdef CONFIG_OF
+static int __devinit
+mv64xxx_calc_freq(const int tclk, const int n, const int m)
+{
+	return tclk / (10 * (m + 1) * (2 << n));
+}
+
+static bool __devinit
+mv64xxx_find_baud_factors(const u32 req_freq, const u32 tclk, u32 *best_n,
+			  u32 *best_m)
+{
+	int freq, delta, best_delta = INT_MAX;
+	int m, n;
+
+	for (n = 0; n <= 7; n++)
+		for (m = 0; m <= 15; m++) {
+			freq = mv64xxx_calc_freq(tclk, n, m);
+			delta = req_freq - freq;
+			if (delta >= 0 && delta < best_delta) {
+				*best_m = m;
+				*best_n = n;
+				best_delta = delta;
+			}
+			if (best_delta == 0)
+				return true;
+		}
+	if (best_delta == INT_MAX)
+		return false;
+	return true;
+}
+
+static int __devinit
+mv64xxx_of_config(struct mv64xxx_i2c_data *drv_data,
+		  struct device_node *np)
+{
+	u32 bus_freq, tclk;
+	int rc = 0;
+
+	/* CLK is mandatory when using DT to describe the i2c bus. We
+	 * need to know tclk in order to calculate bus clock
+	 * factors.
+	 */
+#if !defined(CONFIG_HAVE_CLK)
+	/* Have OF but no CLK */
+	return -ENODEV;
+#else
+	if (IS_ERR(drv_data->clk)) {
+		rc = -ENODEV;
+		goto out;
+	}
+	tclk = clk_get_rate(drv_data->clk);
+	of_property_read_u32(np, "clock-frequency", &bus_freq);
+	if (!mv64xxx_find_baud_factors(bus_freq, tclk,
+				       &drv_data->freq_n, &drv_data->freq_m)) {
+		rc = -EINVAL;
+		goto out;
+	}
+	drv_data->irq = irq_of_parse_and_map(np, 0);
+
+	/* Its not yet defined how timeouts will be specified in device tree.
+	 * So hard code the value to 1 second.
+	 */
+	drv_data->adapter.timeout = HZ;
+out:
+	return rc;
+#endif
+}
+#else /* CONFIG_OF */
+static int __devinit
+mv64xxx_of_config(struct mv64xxx_i2c_data *drv_data,
+		  struct device_node *np)
+{
+	return -ENODEV;
+}
+#endif /* CONFIG_OF */
+
 static int __devinit
 mv64xxx_i2c_probe(struct platform_device *pd)
 {
@@ -528,7 +612,7 @@ mv64xxx_i2c_probe(struct platform_device *pd)
 	struct mv64xxx_i2c_pdata	*pdata = pd->dev.platform_data;
 	int	rc;
 
-	if ((pd->id != 0) || !pdata)
+	if ((!pdata && !pd->dev.of_node))
 		return -ENODEV;
 
 	drv_data = kzalloc(sizeof(struct mv64xxx_i2c_data), GFP_KERNEL);
@@ -546,19 +630,35 @@ mv64xxx_i2c_probe(struct platform_device *pd)
 	init_waitqueue_head(&drv_data->waitq);
 	spin_lock_init(&drv_data->lock);
 
-	drv_data->freq_m = pdata->freq_m;
-	drv_data->freq_n = pdata->freq_n;
-	drv_data->irq = platform_get_irq(pd, 0);
+#if defined(CONFIG_HAVE_CLK)
+	/* Not all platforms have a clk */
+	drv_data->clk = clk_get(&pd->dev, NULL);
+	if (!IS_ERR(drv_data->clk)) {
+		clk_prepare(drv_data->clk);
+		clk_enable(drv_data->clk);
+	}
+#endif
+	if (pdata) {
+		drv_data->freq_m = pdata->freq_m;
+		drv_data->freq_n = pdata->freq_n;
+		drv_data->irq = platform_get_irq(pd, 0);
+		drv_data->adapter.timeout = msecs_to_jiffies(pdata->timeout);
+	} else if (pd->dev.of_node) {
+		rc = mv64xxx_of_config(drv_data, pd->dev.of_node);
+		if (rc)
+			goto exit_unmap_regs;
+	}
 	if (drv_data->irq < 0) {
 		rc = -ENXIO;
 		goto exit_unmap_regs;
 	}
+
 	drv_data->adapter.dev.parent = &pd->dev;
 	drv_data->adapter.algo = &mv64xxx_i2c_algo;
 	drv_data->adapter.owner = THIS_MODULE;
 	drv_data->adapter.class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
-	drv_data->adapter.timeout = msecs_to_jiffies(pdata->timeout);
 	drv_data->adapter.nr = pd->id;
+	drv_data->adapter.dev.of_node = pd->dev.of_node;
 	platform_set_drvdata(pd, drv_data);
 	i2c_set_adapdata(&drv_data->adapter, drv_data);
 
@@ -577,11 +677,20 @@ mv64xxx_i2c_probe(struct platform_device *pd)
 		goto exit_free_irq;
 	}
 
+	of_i2c_register_devices(&drv_data->adapter);
+
 	return 0;
 
 	exit_free_irq:
 		free_irq(drv_data->irq, drv_data);
 	exit_unmap_regs:
+#if defined(CONFIG_HAVE_CLK)
+	/* Not all platforms have a clk */
+	if (!IS_ERR(drv_data->clk)) {
+		clk_disable(drv_data->clk);
+		clk_unprepare(drv_data->clk);
+	}
+#endif
 		mv64xxx_i2c_unmap_regs(drv_data);
 	exit_kfree:
 		kfree(drv_data);
@@ -597,17 +706,31 @@ mv64xxx_i2c_remove(struct platform_device *dev)
 	rc = i2c_del_adapter(&drv_data->adapter);
 	free_irq(drv_data->irq, drv_data);
 	mv64xxx_i2c_unmap_regs(drv_data);
+#if defined(CONFIG_HAVE_CLK)
+	/* Not all platforms have a clk */
+	if (!IS_ERR(drv_data->clk)) {
+		clk_disable(drv_data->clk);
+		clk_unprepare(drv_data->clk);
+	}
+#endif
 	kfree(drv_data);
 
 	return rc;
 }
 
+static const struct of_device_id mv64xxx_i2c_of_match_table[] __devinitdata = {
+	{ .compatible = "marvell,mv64xxx-i2c", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, mv64xxx_i2c_of_match_table);
+
 static struct platform_driver mv64xxx_i2c_driver = {
 	.probe	= mv64xxx_i2c_probe,
 	.remove	= __devexit_p(mv64xxx_i2c_remove),
 	.driver	= {
 		.owner	= THIS_MODULE,
 		.name	= MV64XXX_I2C_CTLR_NAME,
+		.of_match_table = of_match_ptr(mv64xxx_i2c_of_match_table),
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 04eb441b6ce..088c5c1ed17 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -46,6 +46,10 @@
 #define MXS_I2C_CTRL0_DIRECTION			0x00010000
 #define MXS_I2C_CTRL0_XFER_COUNT(v)		((v) & 0x0000FFFF)
 
+#define MXS_I2C_TIMING0		(0x10)
+#define MXS_I2C_TIMING1		(0x20)
+#define MXS_I2C_TIMING2		(0x30)
+
 #define MXS_I2C_CTRL1		(0x40)
 #define MXS_I2C_CTRL1_SET	(0x44)
 #define MXS_I2C_CTRL1_CLR	(0x48)
@@ -97,6 +101,35 @@
 #define MXS_CMD_I2C_READ	(MXS_I2C_CTRL0_SEND_NAK_ON_LAST | \
 				 MXS_I2C_CTRL0_MASTER_MODE)
 
+struct mxs_i2c_speed_config {
+	uint32_t	timing0;
+	uint32_t	timing1;
+	uint32_t	timing2;
+};
+
+/*
+ * Timing values for the default 24MHz clock supplied into the i2c block.
+ *
+ * The bus can operate at 95kHz or at 400kHz with the following timing
+ * register configurations. The 100kHz mode isn't present because it's
+ * values are not stated in the i.MX233/i.MX28 datasheet. The 95kHz mode
+ * shall be close enough replacement. Therefore when the bus is configured
+ * for 100kHz operation, 95kHz timing settings are actually loaded.
+ *
+ * For details, see i.MX233 [25.4.2 - 25.4.4] and i.MX28 [27.5.2 - 27.5.4].
+ */
+static const struct mxs_i2c_speed_config mxs_i2c_95kHz_config = {
+	.timing0	= 0x00780030,
+	.timing1	= 0x00800030,
+	.timing2	= 0x00300030,
+};
+
+static const struct mxs_i2c_speed_config mxs_i2c_400kHz_config = {
+	.timing0	= 0x000f0007,
+	.timing1	= 0x001f000f,
+	.timing2	= 0x00300030,
+};
+
 /**
  * struct mxs_i2c_dev - per device, private MXS-I2C data
  *
@@ -112,11 +145,17 @@ struct mxs_i2c_dev {
 	struct completion cmd_complete;
 	u32 cmd_err;
 	struct i2c_adapter adapter;
+	const struct mxs_i2c_speed_config *speed;
 };
 
 static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
 {
 	stmp_reset_block(i2c->regs);
+
+	writel(i2c->speed->timing0, i2c->regs + MXS_I2C_TIMING0);
+	writel(i2c->speed->timing1, i2c->regs + MXS_I2C_TIMING1);
+	writel(i2c->speed->timing2, i2c->regs + MXS_I2C_TIMING2);
+
 	writel(MXS_I2C_IRQ_MASK << 8, i2c->regs + MXS_I2C_CTRL1_SET);
 	writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
 			i2c->regs + MXS_I2C_QUEUECTRL_SET);
@@ -193,7 +232,7 @@ static int mxs_i2c_wait_for_data(struct mxs_i2c_dev *i2c)
 
 static int mxs_i2c_finish_read(struct mxs_i2c_dev *i2c, u8 *buf, int len)
 {
-	u32 data;
+	u32 uninitialized_var(data);
 	int i;
 
 	for (i = 0; i < len; i++) {
@@ -319,6 +358,28 @@ static const struct i2c_algorithm mxs_i2c_algo = {
 	.functionality = mxs_i2c_func,
 };
 
+static int mxs_i2c_get_ofdata(struct mxs_i2c_dev *i2c)
+{
+	uint32_t speed;
+	struct device *dev = i2c->dev;
+	struct device_node *node = dev->of_node;
+	int ret;
+
+	if (!node)
+		return -EINVAL;
+
+	i2c->speed = &mxs_i2c_95kHz_config;
+	ret = of_property_read_u32(node, "clock-frequency", &speed);
+	if (ret)
+		dev_warn(dev, "No I2C speed selected, using 100kHz\n");
+	else if (speed == 400000)
+		i2c->speed = &mxs_i2c_400kHz_config;
+	else if (speed != 100000)
+		dev_warn(dev, "Unsupported I2C speed selected, using 100kHz\n");
+
+	return 0;
+}
+
 static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -358,6 +419,11 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 		return err;
 
 	i2c->dev = dev;
+
+	err = mxs_i2c_get_ofdata(i2c);
+	if (err)
+		return err;
+
 	platform_set_drvdata(pdev, i2c);
 
 	/* Do reset to enforce correct startup after pinmuxing */
diff --git a/drivers/i2c/busses/i2c-nforce2.c b/drivers/i2c/busses/i2c-nforce2.c
index 43a96a12392..392303b4be0 100644
--- a/drivers/i2c/busses/i2c-nforce2.c
+++ b/drivers/i2c/busses/i2c-nforce2.c
@@ -453,16 +453,4 @@ static struct pci_driver nforce2_driver = {
 	.remove		= __devexit_p(nforce2_remove),
 };
 
-static int __init nforce2_init(void)
-{
-	return pci_register_driver(&nforce2_driver);
-}
-
-static void __exit nforce2_exit(void)
-{
-	pci_unregister_driver(&nforce2_driver);
-}
-
-module_init(nforce2_init);
-module_exit(nforce2_exit);
-
+module_pci_driver(nforce2_driver);
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 5267ab93d55..5e6f1eed4f8 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -14,7 +14,8 @@
  */
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/platform_device.h>
+#include <linux/amba/bus.h>
+#include <linux/atomic.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/i2c.h>
@@ -23,8 +24,7 @@
 #include <linux/io.h>
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
-
-#include <plat/i2c.h>
+#include <linux/platform_data/i2c-nomadik.h>
 
 #define DRIVER_NAME "nmk-i2c"
 
@@ -136,7 +136,7 @@ struct i2c_nmk_client {
 
 /**
  * struct nmk_i2c_dev - private data structure of the controller.
- * @pdev: parent platform device.
+ * @adev: parent amba device.
  * @adap: corresponding I2C adapter.
  * @irq: interrupt line for the controller.
  * @virtbase: virtual io memory area.
@@ -150,7 +150,7 @@ struct i2c_nmk_client {
  * @busy: Busy doing transfer.
  */
 struct nmk_i2c_dev {
-	struct platform_device		*pdev;
+	struct amba_device		*adev;
 	struct i2c_adapter		adap;
 	int				irq;
 	void __iomem			*virtbase;
@@ -217,7 +217,7 @@ static int flush_i2c_fifo(struct nmk_i2c_dev *dev)
 		}
 	}
 
-	dev_err(&dev->pdev->dev,
+	dev_err(&dev->adev->dev,
 		"flushing operation timed out giving up after %d attempts",
 		LOOP_ATTEMPTS);
 
@@ -276,15 +276,32 @@ exit:
 /**
  * load_i2c_mcr_reg() - load the MCR register
  * @dev: private data of controller
+ * @flags: message flags
  */
-static u32 load_i2c_mcr_reg(struct nmk_i2c_dev *dev)
+static u32 load_i2c_mcr_reg(struct nmk_i2c_dev *dev, u16 flags)
 {
 	u32 mcr = 0;
+	unsigned short slave_adr_3msb_bits;
 
-	/* 7-bit address transaction */
-	mcr |= GEN_MASK(1, I2C_MCR_AM, 12);
 	mcr |= GEN_MASK(dev->cli.slave_adr, I2C_MCR_A7, 1);
 
+	if (unlikely(flags & I2C_M_TEN)) {
+		/* 10-bit address transaction */
+		mcr |= GEN_MASK(2, I2C_MCR_AM, 12);
+		/*
+		 * Get the top 3 bits.
+		 * EA10 represents extended address in MCR. This includes
+		 * the extension (MSB bits) of the 7 bit address loaded
+		 * in A7
+		 */
+		slave_adr_3msb_bits = (dev->cli.slave_adr >> 7) & 0x7;
+
+		mcr |= GEN_MASK(slave_adr_3msb_bits, I2C_MCR_EA10, 8);
+	} else {
+		/* 7-bit address transaction */
+		mcr |= GEN_MASK(1, I2C_MCR_AM, 12);
+	}
+
 	/* start byte procedure not applied */
 	mcr |= GEN_MASK(0, I2C_MCR_SB, 11);
 
@@ -364,7 +381,7 @@ static void setup_i2c_controller(struct nmk_i2c_dev *dev)
 	 * and high speed (up to 3.4 Mb/s)
 	 */
 	if (dev->cfg.sm > I2C_FREQ_MODE_FAST) {
-		dev_err(&dev->pdev->dev,
+		dev_err(&dev->adev->dev,
 			"do not support this mode defaulting to std. mode\n");
 		brcr2 = i2c_clk/(100000 * 2) & 0xffff;
 		writel((brcr1 | brcr2), dev->virtbase + I2C_BRCR);
@@ -381,19 +398,20 @@ static void setup_i2c_controller(struct nmk_i2c_dev *dev)
 /**
  * read_i2c() - Read from I2C client device
  * @dev: private data of I2C Driver
+ * @flags: message flags
  *
  * This function reads from i2c client device when controller is in
  * master mode. There is a completion timeout. If there is no transfer
  * before timeout error is returned.
  */
-static int read_i2c(struct nmk_i2c_dev *dev)
+static int read_i2c(struct nmk_i2c_dev *dev, u16 flags)
 {
 	u32 status = 0;
 	u32 mcr;
 	u32 irq_mask = 0;
 	int timeout;
 
-	mcr = load_i2c_mcr_reg(dev);
+	mcr = load_i2c_mcr_reg(dev, flags);
 	writel(mcr, dev->virtbase + I2C_MCR);
 
 	/* load the current CR value */
@@ -423,7 +441,7 @@ static int read_i2c(struct nmk_i2c_dev *dev)
 		&dev->xfer_complete, dev->adap.timeout);
 
 	if (timeout < 0) {
-		dev_err(&dev->pdev->dev,
+		dev_err(&dev->adev->dev,
 			"wait_for_completion_timeout "
 			"returned %d waiting for event\n", timeout);
 		status = timeout;
@@ -431,7 +449,7 @@ static int read_i2c(struct nmk_i2c_dev *dev)
 
 	if (timeout == 0) {
 		/* Controller timed out */
-		dev_err(&dev->pdev->dev, "read from slave 0x%x timed out\n",
+		dev_err(&dev->adev->dev, "read from slave 0x%x timed out\n",
 				dev->cli.slave_adr);
 		status = -ETIMEDOUT;
 	}
@@ -459,17 +477,18 @@ static void fill_tx_fifo(struct nmk_i2c_dev *dev, int no_bytes)
 /**
  * write_i2c() - Write data to I2C client.
  * @dev: private data of I2C Driver
+ * @flags: message flags
  *
  * This function writes data to I2C client
  */
-static int write_i2c(struct nmk_i2c_dev *dev)
+static int write_i2c(struct nmk_i2c_dev *dev, u16 flags)
 {
 	u32 status = 0;
 	u32 mcr;
 	u32 irq_mask = 0;
 	int timeout;
 
-	mcr = load_i2c_mcr_reg(dev);
+	mcr = load_i2c_mcr_reg(dev, flags);
 
 	writel(mcr, dev->virtbase + I2C_MCR);
 
@@ -510,7 +529,7 @@ static int write_i2c(struct nmk_i2c_dev *dev)
 		&dev->xfer_complete, dev->adap.timeout);
 
 	if (timeout < 0) {
-		dev_err(&dev->pdev->dev,
+		dev_err(&dev->adev->dev,
 			"wait_for_completion_timeout "
 			"returned %d waiting for event\n", timeout);
 		status = timeout;
@@ -518,7 +537,7 @@ static int write_i2c(struct nmk_i2c_dev *dev)
 
 	if (timeout == 0) {
 		/* Controller timed out */
-		dev_err(&dev->pdev->dev, "write to slave 0x%x timed out\n",
+		dev_err(&dev->adev->dev, "write to slave 0x%x timed out\n",
 				dev->cli.slave_adr);
 		status = -ETIMEDOUT;
 	}
@@ -538,11 +557,11 @@ static int nmk_i2c_xfer_one(struct nmk_i2c_dev *dev, u16 flags)
 	if (flags & I2C_M_RD) {
 		/* read operation */
 		dev->cli.operation = I2C_READ;
-		status = read_i2c(dev);
+		status = read_i2c(dev, flags);
 	} else {
 		/* write operation */
 		dev->cli.operation = I2C_WRITE;
-		status = write_i2c(dev);
+		status = write_i2c(dev, flags);
 	}
 
 	if (status || (dev->result)) {
@@ -557,7 +576,7 @@ static int nmk_i2c_xfer_one(struct nmk_i2c_dev *dev, u16 flags)
 		if (((i2c_sr >> 2) & 0x3) == 0x3) {
 			/* get the abort cause */
 			cause =	(i2c_sr >> 4) & 0x7;
-			dev_err(&dev->pdev->dev, "%s\n",
+			dev_err(&dev->adev->dev, "%s\n",
 				cause >= ARRAY_SIZE(abort_causes) ?
 				"unknown reason" :
 				abort_causes[cause]);
@@ -630,7 +649,7 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 
 	if (dev->regulator)
 		regulator_enable(dev->regulator);
-	pm_runtime_get_sync(&dev->pdev->dev);
+	pm_runtime_get_sync(&dev->adev->dev);
 
 	clk_enable(dev->clk);
 
@@ -644,13 +663,6 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 		setup_i2c_controller(dev);
 
 		for (i = 0; i < num_msgs; i++) {
-			if (unlikely(msgs[i].flags & I2C_M_TEN)) {
-				dev_err(&dev->pdev->dev,
-					"10 bit addressing not supported\n");
-
-				status = -EINVAL;
-				goto out;
-			}
 			dev->cli.slave_adr	= msgs[i].addr;
 			dev->cli.buffer		= msgs[i].buf;
 			dev->cli.count		= msgs[i].len;
@@ -667,7 +679,7 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 
 out:
 	clk_disable(dev->clk);
-	pm_runtime_put_sync(&dev->pdev->dev);
+	pm_runtime_put_sync(&dev->adev->dev);
 	if (dev->regulator)
 		regulator_disable(dev->regulator);
 
@@ -790,7 +802,7 @@ static irqreturn_t i2c_irq_handler(int irq, void *arg)
 
 		if (dev->cli.count) {
 			dev->result = -EIO;
-			dev_err(&dev->pdev->dev,
+			dev_err(&dev->adev->dev,
 				"%lu bytes still remain to be xfered\n",
 				dev->cli.count);
 			(void) init_hw(dev);
@@ -834,7 +846,7 @@ static irqreturn_t i2c_irq_handler(int irq, void *arg)
 		dev->result = -EIO;
 		(void) init_hw(dev);
 
-		dev_err(&dev->pdev->dev, "Tx Fifo Over run\n");
+		dev_err(&dev->adev->dev, "Tx Fifo Over run\n");
 		complete(&dev->xfer_complete);
 
 		break;
@@ -847,10 +859,10 @@ static irqreturn_t i2c_irq_handler(int irq, void *arg)
 	case I2C_IT_RFSE:
 	case I2C_IT_WTSR:
 	case I2C_IT_STD:
-		dev_err(&dev->pdev->dev, "unhandled Interrupt\n");
+		dev_err(&dev->adev->dev, "unhandled Interrupt\n");
 		break;
 	default:
-		dev_err(&dev->pdev->dev, "spurious Interrupt..\n");
+		dev_err(&dev->adev->dev, "spurious Interrupt..\n");
 		break;
 	}
 
@@ -861,8 +873,8 @@ static irqreturn_t i2c_irq_handler(int irq, void *arg)
 #ifdef CONFIG_PM
 static int nmk_i2c_suspend(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct nmk_i2c_dev *nmk_i2c = platform_get_drvdata(pdev);
+	struct amba_device *adev = to_amba_device(dev);
+	struct nmk_i2c_dev *nmk_i2c = amba_get_drvdata(adev);
 
 	if (nmk_i2c->busy)
 		return -EBUSY;
@@ -891,7 +903,7 @@ static const struct dev_pm_ops nmk_i2c_pm = {
 
 static unsigned int nmk_i2c_functionality(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR;
 }
 
 static const struct i2c_algorithm nmk_i2c_algo = {
@@ -899,79 +911,69 @@ static const struct i2c_algorithm nmk_i2c_algo = {
 	.functionality	= nmk_i2c_functionality
 };
 
-static int __devinit nmk_i2c_probe(struct platform_device *pdev)
+static atomic_t adapter_id = ATOMIC_INIT(0);
+
+static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	int ret = 0;
-	struct resource *res;
 	struct nmk_i2c_controller *pdata =
-			pdev->dev.platform_data;
+			adev->dev.platform_data;
 	struct nmk_i2c_dev	*dev;
 	struct i2c_adapter *adap;
 
+	if (!pdata) {
+		dev_warn(&adev->dev, "no platform data\n");
+		return -ENODEV;
+	}
 	dev = kzalloc(sizeof(struct nmk_i2c_dev), GFP_KERNEL);
 	if (!dev) {
-		dev_err(&pdev->dev, "cannot allocate memory\n");
+		dev_err(&adev->dev, "cannot allocate memory\n");
 		ret = -ENOMEM;
 		goto err_no_mem;
 	}
 	dev->busy = false;
-	dev->pdev = pdev;
-	platform_set_drvdata(pdev, dev);
+	dev->adev = adev;
+	amba_set_drvdata(adev, dev);
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENOENT;
-		goto err_no_resource;
-	}
-
-	if (request_mem_region(res->start, resource_size(res),
-		DRIVER_NAME "I/O region") == NULL) {
-		ret = -EBUSY;
-		goto err_no_region;
-	}
-
-	dev->virtbase = ioremap(res->start, resource_size(res));
+	dev->virtbase = ioremap(adev->res.start, resource_size(&adev->res));
 	if (!dev->virtbase) {
 		ret = -ENOMEM;
 		goto err_no_ioremap;
 	}
 
-	dev->irq = platform_get_irq(pdev, 0);
+	dev->irq = adev->irq[0];
 	ret = request_irq(dev->irq, i2c_irq_handler, 0,
 				DRIVER_NAME, dev);
 	if (ret) {
-		dev_err(&pdev->dev, "cannot claim the irq %d\n", dev->irq);
+		dev_err(&adev->dev, "cannot claim the irq %d\n", dev->irq);
 		goto err_irq;
 	}
 
-	dev->regulator = regulator_get(&pdev->dev, "v-i2c");
+	dev->regulator = regulator_get(&adev->dev, "v-i2c");
 	if (IS_ERR(dev->regulator)) {
-		dev_warn(&pdev->dev, "could not get i2c regulator\n");
+		dev_warn(&adev->dev, "could not get i2c regulator\n");
 		dev->regulator = NULL;
 	}
 
-	pm_suspend_ignore_children(&pdev->dev, true);
-	pm_runtime_enable(&pdev->dev);
+	pm_suspend_ignore_children(&adev->dev, true);
 
-	dev->clk = clk_get(&pdev->dev, NULL);
+	dev->clk = clk_get(&adev->dev, NULL);
 	if (IS_ERR(dev->clk)) {
-		dev_err(&pdev->dev, "could not get i2c clock\n");
+		dev_err(&adev->dev, "could not get i2c clock\n");
 		ret = PTR_ERR(dev->clk);
 		goto err_no_clk;
 	}
 
 	adap = &dev->adap;
-	adap->dev.parent = &pdev->dev;
+	adap->dev.parent = &adev->dev;
 	adap->owner	= THIS_MODULE;
 	adap->class	= I2C_CLASS_HWMON | I2C_CLASS_SPD;
 	adap->algo	= &nmk_i2c_algo;
-	adap->timeout	= pdata->timeout ? msecs_to_jiffies(pdata->timeout) :
-		msecs_to_jiffies(20000);
+	adap->timeout	= msecs_to_jiffies(pdata->timeout);
+	adap->nr = atomic_read(&adapter_id);
 	snprintf(adap->name, sizeof(adap->name),
-		 "Nomadik I2C%d at %lx", pdev->id, (unsigned long)res->start);
-
-	/* fetch the controller id */
-	adap->nr	= pdev->id;
+		 "Nomadik I2C%d at %pR", adap->nr, &adev->res);
+	atomic_inc(&adapter_id);
 
 	/* fetch the controller configuration from machine */
 	dev->cfg.clk_freq = pdata->clk_freq;
@@ -982,16 +984,18 @@ static int __devinit nmk_i2c_probe(struct platform_device *pdev)
 
 	i2c_set_adapdata(adap, dev);
 
-	dev_info(&pdev->dev,
+	dev_info(&adev->dev,
 		 "initialize %s on virtual base %p\n",
 		 adap->name, dev->virtbase);
 
 	ret = i2c_add_numbered_adapter(adap);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to add adapter\n");
+		dev_err(&adev->dev, "failed to add adapter\n");
 		goto err_add_adap;
 	}
 
+	pm_runtime_put(&adev->dev);
+
 	return 0;
 
  err_add_adap:
@@ -999,25 +1003,21 @@ static int __devinit nmk_i2c_probe(struct platform_device *pdev)
  err_no_clk:
 	if (dev->regulator)
 		regulator_put(dev->regulator);
-	pm_runtime_disable(&pdev->dev);
 	free_irq(dev->irq, dev);
  err_irq:
 	iounmap(dev->virtbase);
  err_no_ioremap:
-	release_mem_region(res->start, resource_size(res));
- err_no_region:
-	platform_set_drvdata(pdev, NULL);
- err_no_resource:
+	amba_set_drvdata(adev, NULL);
 	kfree(dev);
  err_no_mem:
 
 	return ret;
 }
 
-static int __devexit nmk_i2c_remove(struct platform_device *pdev)
+static int nmk_i2c_remove(struct amba_device *adev)
 {
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	struct nmk_i2c_dev *dev = platform_get_drvdata(pdev);
+	struct resource *res = &adev->res;
+	struct nmk_i2c_dev *dev = amba_get_drvdata(adev);
 
 	i2c_del_adapter(&dev->adap);
 	flush_i2c_fifo(dev);
@@ -1032,31 +1032,46 @@ static int __devexit nmk_i2c_remove(struct platform_device *pdev)
 	clk_put(dev->clk);
 	if (dev->regulator)
 		regulator_put(dev->regulator);
-	pm_runtime_disable(&pdev->dev);
-	platform_set_drvdata(pdev, NULL);
+	pm_runtime_disable(&adev->dev);
+	amba_set_drvdata(adev, NULL);
 	kfree(dev);
 
 	return 0;
 }
 
-static struct platform_driver nmk_i2c_driver = {
-	.driver = {
+static struct amba_id nmk_i2c_ids[] = {
+	{
+		.id	= 0x00180024,
+		.mask	= 0x00ffffff,
+	},
+	{
+		.id	= 0x00380024,
+		.mask	= 0x00ffffff,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(amba, nmk_i2c_ids);
+
+static struct amba_driver nmk_i2c_driver = {
+	.drv = {
 		.owner = THIS_MODULE,
 		.name = DRIVER_NAME,
 		.pm = &nmk_i2c_pm,
 	},
+	.id_table = nmk_i2c_ids,
 	.probe = nmk_i2c_probe,
-	.remove = __devexit_p(nmk_i2c_remove),
+	.remove = nmk_i2c_remove,
 };
 
 static int __init nmk_i2c_init(void)
 {
-	return platform_driver_register(&nmk_i2c_driver);
+	return amba_driver_register(&nmk_i2c_driver);
 }
 
 static void __exit nmk_i2c_exit(void)
 {
-	platform_driver_unregister(&nmk_i2c_driver);
+	amba_driver_unregister(&nmk_i2c_driver);
 }
 
 subsys_initcall(nmk_i2c_init);
@@ -1065,4 +1080,3 @@ module_exit(nmk_i2c_exit);
 MODULE_AUTHOR("Sachin Verma, Srinidhi KASAGAR");
 MODULE_DESCRIPTION("Nomadik/Ux500 I2C 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 75194c579b6..bffd5501ac2 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -10,40 +10,9 @@
  */
 
 /*
- * Device tree configuration:
- *
- * Required properties:
- * - compatible      : "opencores,i2c-ocores"
- * - reg             : bus address start and address range size of device
- * - interrupts      : interrupt number
- * - regstep         : size of device registers in bytes
- * - clock-frequency : frequency of bus clock in Hz
- * 
- * Example:
- *
- *  i2c0: ocores@a0000000 {
- *              compatible = "opencores,i2c-ocores";
- *              reg = <0xa0000000 0x8>;
- *              interrupts = <10>;
- *
- *              regstep = <1>;
- *              clock-frequency = <20000000>;
- *
- * -- Devices connected on this I2C bus get
- * -- defined here; address- and size-cells
- * -- apply to these child devices
- *
- *              #address-cells = <1>;
- *              #size-cells = <0>;
- *
- *              dummy@60 {
- *                     compatible = "dummy";
- *                     reg = <60>;
- *              };
- *  };
- *
+ * This driver can be used from the device tree, see
+ *     Documentation/devicetree/bindings/i2c/ocore-i2c.txt
  */
-
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -56,10 +25,12 @@
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/of_i2c.h>
+#include <linux/log2.h>
 
 struct ocores_i2c {
 	void __iomem *base;
-	int regstep;
+	u32 reg_shift;
+	u32 reg_io_width;
 	wait_queue_head_t wait;
 	struct i2c_adapter adap;
 	struct i2c_msg *msg;
@@ -102,12 +73,22 @@ struct ocores_i2c {
 
 static inline void oc_setreg(struct ocores_i2c *i2c, int reg, u8 value)
 {
-	iowrite8(value, i2c->base + reg * i2c->regstep);
+	if (i2c->reg_io_width == 4)
+		iowrite32(value, i2c->base + (reg << i2c->reg_shift));
+	else if (i2c->reg_io_width == 2)
+		iowrite16(value, i2c->base + (reg << i2c->reg_shift));
+	else
+		iowrite8(value, i2c->base + (reg << i2c->reg_shift));
 }
 
 static inline u8 oc_getreg(struct ocores_i2c *i2c, int reg)
 {
-	return ioread8(i2c->base + reg * i2c->regstep);
+	if (i2c->reg_io_width == 4)
+		return ioread32(i2c->base + (reg << i2c->reg_shift));
+	else if (i2c->reg_io_width == 2)
+		return ioread16(i2c->base + (reg << i2c->reg_shift));
+	else
+		return ioread8(i2c->base + (reg << i2c->reg_shift));
 }
 
 static void ocores_process(struct ocores_i2c *i2c)
@@ -247,26 +228,35 @@ static struct i2c_adapter ocores_adapter = {
 };
 
 #ifdef CONFIG_OF
-static int ocores_i2c_of_probe(struct platform_device* pdev,
-				struct ocores_i2c* i2c)
+static int ocores_i2c_of_probe(struct platform_device *pdev,
+				struct ocores_i2c *i2c)
 {
-	const __be32* val;
-
-	val = of_get_property(pdev->dev.of_node, "regstep", NULL);
-	if (!val) {
-		dev_err(&pdev->dev, "Missing required parameter 'regstep'");
-		return -ENODEV;
+	struct device_node *np = pdev->dev.of_node;
+	u32 val;
+
+	if (of_property_read_u32(np, "reg-shift", &i2c->reg_shift)) {
+		/* no 'reg-shift', check for deprecated 'regstep' */
+		if (!of_property_read_u32(np, "regstep", &val)) {
+			if (!is_power_of_2(val)) {
+				dev_err(&pdev->dev, "invalid regstep %d\n",
+					val);
+				return -EINVAL;
+			}
+			i2c->reg_shift = ilog2(val);
+			dev_warn(&pdev->dev,
+				"regstep property deprecated, use reg-shift\n");
+		}
 	}
-	i2c->regstep = be32_to_cpup(val);
 
-	val = of_get_property(pdev->dev.of_node, "clock-frequency", NULL);
-	if (!val) {
+	if (of_property_read_u32(np, "clock-frequency", &val)) {
 		dev_err(&pdev->dev,
-			"Missing required parameter 'clock-frequency'");
+			"Missing required parameter 'clock-frequency'\n");
 		return -ENODEV;
 	}
-	i2c->clock_khz = be32_to_cpup(val) / 1000;
+	i2c->clock_khz = val / 1000;
 
+	of_property_read_u32(pdev->dev.of_node, "reg-io-width",
+				&i2c->reg_io_width);
 	return 0;
 }
 #else
@@ -308,7 +298,8 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
 
 	pdata = pdev->dev.platform_data;
 	if (pdata) {
-		i2c->regstep = pdata->regstep;
+		i2c->reg_shift = pdata->reg_shift;
+		i2c->reg_io_width = pdata->reg_io_width;
 		i2c->clock_khz = pdata->clock_khz;
 	} else {
 		ret = ocores_i2c_of_probe(pdev, i2c);
@@ -316,6 +307,9 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
 			return ret;
 	}
 
+	if (i2c->reg_io_width == 0)
+		i2c->reg_io_width = 1; /* Set to default value */
+
 	ocores_init(i2c);
 
 	init_waitqueue_head(&i2c->wait);
@@ -351,7 +345,7 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __devexit ocores_i2c_remove(struct platform_device* pdev)
+static int __devexit ocores_i2c_remove(struct platform_device *pdev)
 {
 	struct ocores_i2c *i2c = platform_get_drvdata(pdev);
 
@@ -367,9 +361,9 @@ static int __devexit ocores_i2c_remove(struct platform_device* pdev)
 }
 
 #ifdef CONFIG_PM
-static int ocores_i2c_suspend(struct platform_device *pdev, pm_message_t state)
+static int ocores_i2c_suspend(struct device *dev)
 {
-	struct ocores_i2c *i2c = platform_get_drvdata(pdev);
+	struct ocores_i2c *i2c = dev_get_drvdata(dev);
 	u8 ctrl = oc_getreg(i2c, OCI2C_CONTROL);
 
 	/* make sure the device is disabled */
@@ -378,17 +372,19 @@ static int ocores_i2c_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int ocores_i2c_resume(struct platform_device *pdev)
+static int ocores_i2c_resume(struct device *dev)
 {
-	struct ocores_i2c *i2c = platform_get_drvdata(pdev);
+	struct ocores_i2c *i2c = dev_get_drvdata(dev);
 
 	ocores_init(i2c);
 
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(ocores_i2c_pm, ocores_i2c_suspend, ocores_i2c_resume);
+#define OCORES_I2C_PM	(&ocores_i2c_pm)
 #else
-#define ocores_i2c_suspend	NULL
-#define ocores_i2c_resume	NULL
+#define OCORES_I2C_PM	NULL
 #endif
 
 static struct of_device_id ocores_i2c_match[] = {
@@ -400,12 +396,11 @@ MODULE_DEVICE_TABLE(of, ocores_i2c_match);
 static struct platform_driver ocores_i2c_driver = {
 	.probe   = ocores_i2c_probe,
 	.remove  = __devexit_p(ocores_i2c_remove),
-	.suspend = ocores_i2c_suspend,
-	.resume  = ocores_i2c_resume,
 	.driver  = {
 		.owner = THIS_MODULE,
 		.name = "ocores-i2c",
 		.of_match_table = ocores_i2c_match,
+		.pm = OCORES_I2C_PM,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-octeon.c b/drivers/i2c/busses/i2c-octeon.c
index ee139a59881..f44c83549fe 100644
--- a/drivers/i2c/busses/i2c-octeon.c
+++ b/drivers/i2c/busses/i2c-octeon.c
@@ -2,7 +2,7 @@
  * (C) Copyright 2009-2010
  * Nokia Siemens Networks, michael.lawnick.ext@nsn.com
  *
- * Portions Copyright (C) 2010 Cavium Networks, Inc.
+ * Portions Copyright (C) 2010, 2011 Cavium Networks, Inc.
  *
  * This is a driver for the i2c adapter in Cavium Networks' OCTEON processors.
  *
@@ -11,17 +11,18 @@
  * warranty of any kind, whether express or implied.
  */
 
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of_i2c.h>
+#include <linux/delay.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/init.h>
-
-#include <linux/io.h>
 #include <linux/i2c.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/of.h>
 
 #include <asm/octeon/octeon.h>
 
@@ -65,7 +66,7 @@ struct octeon_i2c {
 	wait_queue_head_t queue;
 	struct i2c_adapter adap;
 	int irq;
-	int twsi_freq;
+	u32 twsi_freq;
 	int sys_freq;
 	resource_size_t twsi_phys;
 	void __iomem *twsi_base;
@@ -121,10 +122,8 @@ static u8 octeon_i2c_read_sw(struct octeon_i2c *i2c, u64 eop_reg)
  */
 static void octeon_i2c_write_int(struct octeon_i2c *i2c, u64 data)
 {
-	u64 tmp;
-
 	__raw_writeq(data, i2c->twsi_base + TWSI_INT);
-	tmp = __raw_readq(i2c->twsi_base + TWSI_INT);
+	__raw_readq(i2c->twsi_base + TWSI_INT);
 }
 
 /**
@@ -515,7 +514,6 @@ static int __devinit octeon_i2c_probe(struct platform_device *pdev)
 {
 	int irq, result = 0;
 	struct octeon_i2c *i2c;
-	struct octeon_i2c_data *i2c_data;
 	struct resource *res_mem;
 
 	/* All adaptors have an irq.  */
@@ -523,86 +521,90 @@ static int __devinit octeon_i2c_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	i2c = kzalloc(sizeof(*i2c), GFP_KERNEL);
+	i2c = devm_kzalloc(&pdev->dev, sizeof(*i2c), GFP_KERNEL);
 	if (!i2c) {
 		dev_err(&pdev->dev, "kzalloc failed\n");
 		result = -ENOMEM;
 		goto out;
 	}
 	i2c->dev = &pdev->dev;
-	i2c_data = pdev->dev.platform_data;
 
 	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	if (res_mem == NULL) {
 		dev_err(i2c->dev, "found no memory resource\n");
 		result = -ENXIO;
-		goto fail_region;
+		goto out;
 	}
+	i2c->twsi_phys = res_mem->start;
+	i2c->regsize = resource_size(res_mem);
 
-	if (i2c_data == NULL) {
-		dev_err(i2c->dev, "no I2C frequency data\n");
+	/*
+	 * "clock-rate" is a legacy binding, the official binding is
+	 * "clock-frequency".  Try the official one first and then
+	 * fall back if it doesn't exist.
+	 */
+	if (of_property_read_u32(pdev->dev.of_node,
+				 "clock-frequency", &i2c->twsi_freq) &&
+	    of_property_read_u32(pdev->dev.of_node,
+				 "clock-rate", &i2c->twsi_freq)) {
+		dev_err(i2c->dev,
+			"no I2C 'clock-rate' or 'clock-frequency' property\n");
 		result = -ENXIO;
-		goto fail_region;
+		goto out;
 	}
 
-	i2c->twsi_phys = res_mem->start;
-	i2c->regsize = resource_size(res_mem);
-	i2c->twsi_freq = i2c_data->i2c_freq;
-	i2c->sys_freq = i2c_data->sys_freq;
+	i2c->sys_freq = octeon_get_io_clock_rate();
 
-	if (!request_mem_region(i2c->twsi_phys, i2c->regsize, res_mem->name)) {
+	if (!devm_request_mem_region(&pdev->dev, i2c->twsi_phys, i2c->regsize,
+				      res_mem->name)) {
 		dev_err(i2c->dev, "request_mem_region failed\n");
-		goto fail_region;
+		goto out;
 	}
-	i2c->twsi_base = ioremap(i2c->twsi_phys, i2c->regsize);
+	i2c->twsi_base = devm_ioremap(&pdev->dev, i2c->twsi_phys, i2c->regsize);
 
 	init_waitqueue_head(&i2c->queue);
 
 	i2c->irq = irq;
 
-	result = request_irq(i2c->irq, octeon_i2c_isr, 0, DRV_NAME, i2c);
+	result = devm_request_irq(&pdev->dev, i2c->irq,
+				  octeon_i2c_isr, 0, DRV_NAME, i2c);
 	if (result < 0) {
 		dev_err(i2c->dev, "failed to attach interrupt\n");
-		goto fail_irq;
+		goto out;
 	}
 
 	result = octeon_i2c_initlowlevel(i2c);
 	if (result) {
 		dev_err(i2c->dev, "init low level failed\n");
-		goto  fail_add;
+		goto  out;
 	}
 
 	result = octeon_i2c_setclock(i2c);
 	if (result) {
 		dev_err(i2c->dev, "clock init failed\n");
-		goto  fail_add;
+		goto  out;
 	}
 
 	i2c->adap = octeon_i2c_ops;
 	i2c->adap.dev.parent = &pdev->dev;
-	i2c->adap.nr = pdev->id >= 0 ? pdev->id : 0;
+	i2c->adap.dev.of_node = pdev->dev.of_node;
 	i2c_set_adapdata(&i2c->adap, i2c);
 	platform_set_drvdata(pdev, i2c);
 
-	result = i2c_add_numbered_adapter(&i2c->adap);
+	result = i2c_add_adapter(&i2c->adap);
 	if (result < 0) {
 		dev_err(i2c->dev, "failed to add adapter\n");
 		goto fail_add;
 	}
-
 	dev_info(i2c->dev, "version %s\n", DRV_VERSION);
 
-	return result;
+	of_i2c_register_devices(&i2c->adap);
+
+	return 0;
 
 fail_add:
 	platform_set_drvdata(pdev, NULL);
-	free_irq(i2c->irq, i2c);
-fail_irq:
-	iounmap(i2c->twsi_base);
-	release_mem_region(i2c->twsi_phys, i2c->regsize);
-fail_region:
-	kfree(i2c);
 out:
 	return result;
 };
@@ -613,19 +615,24 @@ static int __devexit octeon_i2c_remove(struct platform_device *pdev)
 
 	i2c_del_adapter(&i2c->adap);
 	platform_set_drvdata(pdev, NULL);
-	free_irq(i2c->irq, i2c);
-	iounmap(i2c->twsi_base);
-	release_mem_region(i2c->twsi_phys, i2c->regsize);
-	kfree(i2c);
 	return 0;
 };
 
+static struct of_device_id octeon_i2c_match[] = {
+	{
+		.compatible = "cavium,octeon-3860-twsi",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, octeon_i2c_match);
+
 static struct platform_driver octeon_i2c_driver = {
 	.probe		= octeon_i2c_probe,
 	.remove		= __devexit_p(octeon_i2c_remove),
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= DRV_NAME,
+		.of_match_table = octeon_i2c_match,
 	},
 };
 
@@ -635,4 +642,3 @@ MODULE_AUTHOR("Michael Lawnick <michael.lawnick.ext@nsn.com>");
 MODULE_DESCRIPTION("I2C-Bus adapter for Cavium OCTEON processors");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
-MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 801df6000e9..6849635b268 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -49,8 +49,8 @@
 
 /* I2C controller revisions present on specific hardware */
 #define OMAP_I2C_REV_ON_2430		0x36
-#define OMAP_I2C_REV_ON_3430		0x3C
-#define OMAP_I2C_REV_ON_3530_4430	0x40
+#define OMAP_I2C_REV_ON_3430_3530	0x3C
+#define OMAP_I2C_REV_ON_3630_4430	0x40
 
 /* timeout waiting for the controller to respond */
 #define OMAP_I2C_TIMEOUT (msecs_to_jiffies(1000))
@@ -173,7 +173,7 @@ enum {
 
 /* Errata definitions */
 #define I2C_OMAP_ERRATA_I207		(1 << 0)
-#define I2C_OMAP3_1P153			(1 << 1)
+#define I2C_OMAP_ERRATA_I462		(1 << 1)
 
 struct omap_i2c_dev {
 	struct device		*dev;
@@ -269,47 +269,6 @@ static inline u16 omap_i2c_read_reg(struct omap_i2c_dev *i2c_dev, int reg)
 				(i2c_dev->regs[reg] << i2c_dev->reg_shift));
 }
 
-static void omap_i2c_unidle(struct omap_i2c_dev *dev)
-{
-	if (dev->flags & OMAP_I2C_FLAG_RESET_REGS_POSTIDLE) {
-		omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
-		omap_i2c_write_reg(dev, OMAP_I2C_PSC_REG, dev->pscstate);
-		omap_i2c_write_reg(dev, OMAP_I2C_SCLL_REG, dev->scllstate);
-		omap_i2c_write_reg(dev, OMAP_I2C_SCLH_REG, dev->sclhstate);
-		omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, dev->bufstate);
-		omap_i2c_write_reg(dev, OMAP_I2C_SYSC_REG, dev->syscstate);
-		omap_i2c_write_reg(dev, OMAP_I2C_WE_REG, dev->westate);
-		omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, OMAP_I2C_CON_EN);
-	}
-
-	/*
-	 * Don't write to this register if the IE state is 0 as it can
-	 * cause deadlock.
-	 */
-	if (dev->iestate)
-		omap_i2c_write_reg(dev, OMAP_I2C_IE_REG, dev->iestate);
-}
-
-static void omap_i2c_idle(struct omap_i2c_dev *dev)
-{
-	u16 iv;
-
-	dev->iestate = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
-	if (dev->dtrev == OMAP_I2C_IP_VERSION_2)
-		omap_i2c_write_reg(dev, OMAP_I2C_IP_V2_IRQENABLE_CLR, 1);
-	else
-		omap_i2c_write_reg(dev, OMAP_I2C_IE_REG, 0);
-
-	if (dev->rev < OMAP_I2C_OMAP1_REV_2) {
-		iv = omap_i2c_read_reg(dev, OMAP_I2C_IV_REG); /* Read clears */
-	} else {
-		omap_i2c_write_reg(dev, OMAP_I2C_STAT_REG, dev->iestate);
-
-		/* Flush posted write */
-		omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
-	}
-}
-
 static int omap_i2c_init(struct omap_i2c_dev *dev)
 {
 	u16 psc = 0, scll = 0, sclh = 0, buf = 0;
@@ -346,7 +305,7 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
 			omap_i2c_write_reg(dev, OMAP_I2C_SYSC_REG,
 					   SYSC_AUTOIDLE_MASK);
 
-		} else if (dev->rev >= OMAP_I2C_REV_ON_3430) {
+		} else if (dev->rev >= OMAP_I2C_REV_ON_3430_3530) {
 			dev->syscstate = SYSC_AUTOIDLE_MASK;
 			dev->syscstate |= SYSC_ENAWAKEUP_MASK;
 			dev->syscstate |= (SYSC_IDLEMODE_SMART <<
@@ -468,11 +427,6 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
 	/* Take the I2C module out of reset: */
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, OMAP_I2C_CON_EN);
 
-	dev->errata = 0;
-
-	if (dev->flags & OMAP_I2C_FLAG_APPLY_ERRATA_I207)
-		dev->errata |= I2C_OMAP_ERRATA_I207;
-
 	/* Enable interrupts */
 	dev->iestate = (OMAP_I2C_IE_XRDY | OMAP_I2C_IE_RRDY |
 			OMAP_I2C_IE_ARDY | OMAP_I2C_IE_NACK |
@@ -514,7 +468,7 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 			     struct i2c_msg *msg, int stop)
 {
 	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
-	int r;
+	unsigned long timeout;
 	u16 w;
 
 	dev_dbg(dev->dev, "addr: 0x%04x, len: %d, flags: 0x%x, stop: %d\n",
@@ -536,7 +490,7 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 	w |= OMAP_I2C_BUF_RXFIF_CLR | OMAP_I2C_BUF_TXFIF_CLR;
 	omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, w);
 
-	init_completion(&dev->cmd_complete);
+	INIT_COMPLETION(dev->cmd_complete);
 	dev->cmd_err = 0;
 
 	w = OMAP_I2C_CON_EN | OMAP_I2C_CON_MST | OMAP_I2C_CON_STT;
@@ -545,6 +499,8 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 	if (dev->speed > 400)
 		w |= OMAP_I2C_CON_OPMODE_HS;
 
+	if (msg->flags & I2C_M_STOP)
+		stop = 1;
 	if (msg->flags & I2C_M_TEN)
 		w |= OMAP_I2C_CON_XA;
 	if (!(msg->flags & I2C_M_RD))
@@ -582,12 +538,10 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 	 * REVISIT: We should abort the transfer on signals, but the bus goes
 	 * into arbitration and we're currently unable to recover from it.
 	 */
-	r = wait_for_completion_timeout(&dev->cmd_complete,
-					OMAP_I2C_TIMEOUT);
+	timeout = wait_for_completion_timeout(&dev->cmd_complete,
+						OMAP_I2C_TIMEOUT);
 	dev->buf_len = 0;
-	if (r < 0)
-		return r;
-	if (r == 0) {
+	if (timeout == 0) {
 		dev_err(dev->dev, "controller timed out\n");
 		omap_i2c_init(dev);
 		return -ETIMEDOUT;
@@ -628,7 +582,9 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	int i;
 	int r;
 
-	pm_runtime_get_sync(dev->dev);
+	r = pm_runtime_get_sync(dev->dev);
+	if (IS_ERR_VALUE(r))
+		return r;
 
 	r = omap_i2c_wait_for_bb(dev);
 	if (r < 0)
@@ -658,7 +614,8 @@ out:
 static u32
 omap_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) |
+	       I2C_FUNC_PROTOCOL_MANGLING;
 }
 
 static inline void
@@ -764,11 +721,11 @@ omap_i2c_omap1_isr(int this_irq, void *dev_id)
 #endif
 
 /*
- * OMAP3430 Errata 1.153: When an XRDY/XDR is hit, wait for XUDF before writing
+ * OMAP3430 Errata i462: When an XRDY/XDR is hit, wait for XUDF before writing
  * data to DATA_REG. Otherwise some data bytes can be lost while transferring
  * them from the memory to the I2C interface.
  */
-static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
+static int errata_omap3_i462(struct omap_i2c_dev *dev, u16 *stat, int *err)
 {
 	unsigned long timeout = 10000;
 
@@ -776,7 +733,6 @@ static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
 		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
 			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
 							OMAP_I2C_STAT_XDR));
-			*err |= OMAP_I2C_STAT_XUDF;
 			return -ETIMEDOUT;
 		}
 
@@ -789,6 +745,7 @@ static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
 		return 0;
 	}
 
+	*err |= OMAP_I2C_STAT_XUDF;
 	return 0;
 }
 
@@ -927,8 +884,8 @@ complete:
 					break;
 				}
 
-				if ((dev->errata & I2C_OMAP3_1P153) &&
-				    errata_omap3_1p153(dev, &stat, &err))
+				if ((dev->errata & I2C_OMAP_ERRATA_I462) &&
+				    errata_omap3_i462(dev, &stat, &err))
 					goto complete;
 
 				omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
@@ -1045,6 +1002,7 @@ omap_i2c_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, dev);
+	init_completion(&dev->cmd_complete);
 
 	dev->reg_shift = (dev->flags >> OMAP_I2C_FLAG_BUS_SHIFT__SHIFT) & 3;
 
@@ -1054,12 +1012,19 @@ omap_i2c_probe(struct platform_device *pdev)
 		dev->regs = (u8 *)reg_map_ip_v1;
 
 	pm_runtime_enable(dev->dev);
-	pm_runtime_get_sync(dev->dev);
+	r = pm_runtime_get_sync(dev->dev);
+	if (IS_ERR_VALUE(r))
+		goto err_free_mem;
 
 	dev->rev = omap_i2c_read_reg(dev, OMAP_I2C_REV_REG) & 0xff;
 
-	if (dev->rev <= OMAP_I2C_REV_ON_3430)
-		dev->errata |= I2C_OMAP3_1P153;
+	dev->errata = 0;
+
+	if (dev->flags & OMAP_I2C_FLAG_APPLY_ERRATA_I207)
+		dev->errata |= I2C_OMAP_ERRATA_I207;
+
+	if (dev->rev <= OMAP_I2C_REV_ON_3430_3530)
+		dev->errata |= I2C_OMAP_ERRATA_I462;
 
 	if (!(dev->flags & OMAP_I2C_FLAG_NO_FIFO)) {
 		u16 s;
@@ -1076,7 +1041,7 @@ omap_i2c_probe(struct platform_device *pdev)
 
 		dev->fifo_size = (dev->fifo_size / 2);
 
-		if (dev->rev >= OMAP_I2C_REV_ON_3530_4430)
+		if (dev->rev >= OMAP_I2C_REV_ON_3630_4430)
 			dev->b_hw = 0; /* Disable hardware fixes */
 		else
 			dev->b_hw = 1; /* Enable hardware fixes */
@@ -1092,7 +1057,7 @@ omap_i2c_probe(struct platform_device *pdev)
 
 	isr = (dev->rev < OMAP_I2C_OMAP1_REV_2) ? omap_i2c_omap1_isr :
 								   omap_i2c_isr;
-	r = request_irq(dev->irq, isr, 0, pdev->name, dev);
+	r = request_irq(dev->irq, isr, IRQF_NO_SUSPEND, pdev->name, dev);
 
 	if (r) {
 		dev_err(dev->dev, "failure requesting irq %i\n", dev->irq);
@@ -1102,8 +1067,6 @@ omap_i2c_probe(struct platform_device *pdev)
 	dev_info(dev->dev, "bus %d rev%d.%d.%d at %d kHz\n", pdev->id,
 		 dev->dtrev, dev->rev >> 4, dev->rev & 0xf, dev->speed);
 
-	pm_runtime_put(dev->dev);
-
 	adap = &dev->adapter;
 	i2c_set_adapdata(adap, dev);
 	adap->owner = THIS_MODULE;
@@ -1123,6 +1086,8 @@ omap_i2c_probe(struct platform_device *pdev)
 
 	of_i2c_register_devices(adap);
 
+	pm_runtime_put(dev->dev);
+
 	return 0;
 
 err_free_irq:
@@ -1131,6 +1096,7 @@ err_unuse_clocks:
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
 	pm_runtime_put(dev->dev);
 	iounmap(dev->base);
+	pm_runtime_disable(&pdev->dev);
 err_free_mem:
 	platform_set_drvdata(pdev, NULL);
 	kfree(dev);
@@ -1140,17 +1106,23 @@ err_release_region:
 	return r;
 }
 
-static int
-omap_i2c_remove(struct platform_device *pdev)
+static int __devexit omap_i2c_remove(struct platform_device *pdev)
 {
 	struct omap_i2c_dev	*dev = platform_get_drvdata(pdev);
 	struct resource		*mem;
+	int ret;
 
 	platform_set_drvdata(pdev, NULL);
 
 	free_irq(dev->irq, dev);
 	i2c_del_adapter(&dev->adapter);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	iounmap(dev->base);
 	kfree(dev);
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1158,13 +1130,26 @@ omap_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
 #ifdef CONFIG_PM_RUNTIME
 static int omap_i2c_runtime_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct omap_i2c_dev *_dev = platform_get_drvdata(pdev);
+	u16 iv;
+
+	_dev->iestate = omap_i2c_read_reg(_dev, OMAP_I2C_IE_REG);
+
+	omap_i2c_write_reg(_dev, OMAP_I2C_IE_REG, 0);
 
-	omap_i2c_idle(_dev);
+	if (_dev->rev < OMAP_I2C_OMAP1_REV_2) {
+		iv = omap_i2c_read_reg(_dev, OMAP_I2C_IV_REG); /* Read clears */
+	} else {
+		omap_i2c_write_reg(_dev, OMAP_I2C_STAT_REG, _dev->iestate);
+
+		/* Flush posted write */
+		omap_i2c_read_reg(_dev, OMAP_I2C_STAT_REG);
+	}
 
 	return 0;
 }
@@ -1174,23 +1159,40 @@ static int omap_i2c_runtime_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct omap_i2c_dev *_dev = platform_get_drvdata(pdev);
 
-	omap_i2c_unidle(_dev);
+	if (_dev->flags & OMAP_I2C_FLAG_RESET_REGS_POSTIDLE) {
+		omap_i2c_write_reg(_dev, OMAP_I2C_CON_REG, 0);
+		omap_i2c_write_reg(_dev, OMAP_I2C_PSC_REG, _dev->pscstate);
+		omap_i2c_write_reg(_dev, OMAP_I2C_SCLL_REG, _dev->scllstate);
+		omap_i2c_write_reg(_dev, OMAP_I2C_SCLH_REG, _dev->sclhstate);
+		omap_i2c_write_reg(_dev, OMAP_I2C_BUF_REG, _dev->bufstate);
+		omap_i2c_write_reg(_dev, OMAP_I2C_SYSC_REG, _dev->syscstate);
+		omap_i2c_write_reg(_dev, OMAP_I2C_WE_REG, _dev->westate);
+		omap_i2c_write_reg(_dev, OMAP_I2C_CON_REG, OMAP_I2C_CON_EN);
+	}
+
+	/*
+	 * Don't write to this register if the IE state is 0 as it can
+	 * cause deadlock.
+	 */
+	if (_dev->iestate)
+		omap_i2c_write_reg(_dev, OMAP_I2C_IE_REG, _dev->iestate);
 
 	return 0;
 }
+#endif /* CONFIG_PM_RUNTIME */
 
 static struct dev_pm_ops omap_i2c_pm_ops = {
-	.runtime_suspend = omap_i2c_runtime_suspend,
-	.runtime_resume = omap_i2c_runtime_resume,
+	SET_RUNTIME_PM_OPS(omap_i2c_runtime_suspend,
+			   omap_i2c_runtime_resume, NULL)
 };
 #define OMAP_I2C_PM_OPS (&omap_i2c_pm_ops)
 #else
 #define OMAP_I2C_PM_OPS NULL
-#endif
+#endif /* CONFIG_PM */
 
 static struct platform_driver omap_i2c_driver = {
 	.probe		= omap_i2c_probe,
-	.remove		= omap_i2c_remove,
+	.remove		= __devexit_p(omap_i2c_remove),
 	.driver		= {
 		.name	= "omap_i2c",
 		.owner	= THIS_MODULE,
diff --git a/drivers/i2c/busses/i2c-pasemi.c b/drivers/i2c/busses/i2c-pasemi.c
index eaaea73209c..12edefd4183 100644
--- a/drivers/i2c/busses/i2c-pasemi.c
+++ b/drivers/i2c/busses/i2c-pasemi.c
@@ -415,19 +415,8 @@ static struct pci_driver pasemi_smb_driver = {
 	.remove		= __devexit_p(pasemi_smb_remove),
 };
 
-static int __init pasemi_smb_init(void)
-{
-	return pci_register_driver(&pasemi_smb_driver);
-}
-
-static void __exit pasemi_smb_exit(void)
-{
-	pci_unregister_driver(&pasemi_smb_driver);
-}
+module_pci_driver(pasemi_smb_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR ("Olof Johansson <olof@lixom.net>");
 MODULE_DESCRIPTION("PA Semi PWRficient SMBus driver");
-
-module_init(pasemi_smb_init);
-module_exit(pasemi_smb_exit);
diff --git a/drivers/i2c/busses/i2c-piix4.c b/drivers/i2c/busses/i2c-piix4.c
index c14d48dd601..ef511df2c96 100644
--- a/drivers/i2c/busses/i2c-piix4.c
+++ b/drivers/i2c/busses/i2c-piix4.c
@@ -21,11 +21,12 @@
    Supports:
 	Intel PIIX4, 440MX
 	Serverworks OSB4, CSB5, CSB6, HT-1000, HT-1100
-	ATI IXP200, IXP300, IXP400, SB600, SB700, SB800
+	ATI IXP200, IXP300, IXP400, SB600, SB700/SP5100, SB800
 	AMD Hudson-2
 	SMSC Victory66
 
-   Note: we assume there can only be one device, with one SMBus interface.
+   Note: we assume there can only be one device, with one or more
+   SMBus interfaces.
 */
 
 #include <linux/module.h>
@@ -94,10 +95,8 @@ MODULE_PARM_DESC(force_addr,
 		 "Forcibly enable the PIIX4 at the given address. "
 		 "EXTREMELY DANGEROUS!");
 
-static unsigned short piix4_smba;
 static int srvrworks_csb5_delay;
 static struct pci_driver piix4_driver;
-static struct i2c_adapter piix4_adapter;
 
 static struct dmi_system_id __devinitdata piix4_dmi_blacklist[] = {
 	{
@@ -127,10 +126,15 @@ static struct dmi_system_id __devinitdata piix4_dmi_ibm[] = {
 	{ },
 };
 
+struct i2c_piix4_adapdata {
+	unsigned short smba;
+};
+
 static int __devinit piix4_setup(struct pci_dev *PIIX4_dev,
 				const struct pci_device_id *id)
 {
 	unsigned char temp;
+	unsigned short piix4_smba;
 
 	if ((PIIX4_dev->vendor == PCI_VENDOR_ID_SERVERWORKS) &&
 	    (PIIX4_dev->device == PCI_DEVICE_ID_SERVERWORKS_CSB5))
@@ -206,7 +210,6 @@ static int __devinit piix4_setup(struct pci_dev *PIIX4_dev,
 			dev_err(&PIIX4_dev->dev,
 				"Host SMBus controller not enabled!\n");
 			release_region(piix4_smba, SMBIOSIZE);
-			piix4_smba = 0;
 			return -ENODEV;
 		}
 	}
@@ -224,12 +227,13 @@ static int __devinit piix4_setup(struct pci_dev *PIIX4_dev,
 		 "SMBus Host Controller at 0x%x, revision %d\n",
 		 piix4_smba, temp);
 
-	return 0;
+	return piix4_smba;
 }
 
 static int __devinit piix4_setup_sb800(struct pci_dev *PIIX4_dev,
 				const struct pci_device_id *id)
 {
+	unsigned short piix4_smba;
 	unsigned short smba_idx = 0xcd6;
 	u8 smba_en_lo, smba_en_hi, i2ccfg, i2ccfg_offset = 0x10, smb_en = 0x2c;
 
@@ -273,7 +277,6 @@ static int __devinit piix4_setup_sb800(struct pci_dev *PIIX4_dev,
 		dev_err(&PIIX4_dev->dev, "SMBus I2C bus config region "
 			"0x%x already in use!\n", piix4_smba + i2ccfg_offset);
 		release_region(piix4_smba, SMBIOSIZE);
-		piix4_smba = 0;
 		return -EBUSY;
 	}
 	i2ccfg = inb_p(piix4_smba + i2ccfg_offset);
@@ -288,30 +291,72 @@ static int __devinit piix4_setup_sb800(struct pci_dev *PIIX4_dev,
 		 "SMBus Host Controller at 0x%x, revision %d\n",
 		 piix4_smba, i2ccfg >> 4);
 
-	return 0;
+	return piix4_smba;
+}
+
+static int __devinit piix4_setup_aux(struct pci_dev *PIIX4_dev,
+				const struct pci_device_id *id,
+				unsigned short base_reg_addr)
+{
+	/* Set up auxiliary SMBus controllers found on some
+	 * AMD chipsets e.g. SP5100 (SB700 derivative) */
+
+	unsigned short piix4_smba;
+
+	/* Read address of auxiliary SMBus controller */
+	pci_read_config_word(PIIX4_dev, base_reg_addr, &piix4_smba);
+	if ((piix4_smba & 1) == 0) {
+		dev_dbg(&PIIX4_dev->dev,
+			"Auxiliary SMBus controller not enabled\n");
+		return -ENODEV;
+	}
+
+	piix4_smba &= 0xfff0;
+	if (piix4_smba == 0) {
+		dev_dbg(&PIIX4_dev->dev,
+			"Auxiliary SMBus base address uninitialized\n");
+		return -ENODEV;
+	}
+
+	if (acpi_check_region(piix4_smba, SMBIOSIZE, piix4_driver.name))
+		return -ENODEV;
+
+	if (!request_region(piix4_smba, SMBIOSIZE, piix4_driver.name)) {
+		dev_err(&PIIX4_dev->dev, "Auxiliary SMBus region 0x%x "
+			"already in use!\n", piix4_smba);
+		return -EBUSY;
+	}
+
+	dev_info(&PIIX4_dev->dev,
+		 "Auxiliary SMBus Host Controller at 0x%x\n",
+		 piix4_smba);
+
+	return piix4_smba;
 }
 
-static int piix4_transaction(void)
+static int piix4_transaction(struct i2c_adapter *piix4_adapter)
 {
+	struct i2c_piix4_adapdata *adapdata = i2c_get_adapdata(piix4_adapter);
+	unsigned short piix4_smba = adapdata->smba;
 	int temp;
 	int result = 0;
 	int timeout = 0;
 
-	dev_dbg(&piix4_adapter.dev, "Transaction (pre): CNT=%02x, CMD=%02x, "
+	dev_dbg(&piix4_adapter->dev, "Transaction (pre): CNT=%02x, CMD=%02x, "
 		"ADD=%02x, DAT0=%02x, DAT1=%02x\n", inb_p(SMBHSTCNT),
 		inb_p(SMBHSTCMD), inb_p(SMBHSTADD), inb_p(SMBHSTDAT0),
 		inb_p(SMBHSTDAT1));
 
 	/* Make sure the SMBus host is ready to start transmitting */
 	if ((temp = inb_p(SMBHSTSTS)) != 0x00) {
-		dev_dbg(&piix4_adapter.dev, "SMBus busy (%02x). "
+		dev_dbg(&piix4_adapter->dev, "SMBus busy (%02x). "
 			"Resetting...\n", temp);
 		outb_p(temp, SMBHSTSTS);
 		if ((temp = inb_p(SMBHSTSTS)) != 0x00) {
-			dev_err(&piix4_adapter.dev, "Failed! (%02x)\n", temp);
+			dev_err(&piix4_adapter->dev, "Failed! (%02x)\n", temp);
 			return -EBUSY;
 		} else {
-			dev_dbg(&piix4_adapter.dev, "Successful!\n");
+			dev_dbg(&piix4_adapter->dev, "Successful!\n");
 		}
 	}
 
@@ -330,35 +375,35 @@ static int piix4_transaction(void)
 
 	/* If the SMBus is still busy, we give up */
 	if (timeout == MAX_TIMEOUT) {
-		dev_err(&piix4_adapter.dev, "SMBus Timeout!\n");
+		dev_err(&piix4_adapter->dev, "SMBus Timeout!\n");
 		result = -ETIMEDOUT;
 	}
 
 	if (temp & 0x10) {
 		result = -EIO;
-		dev_err(&piix4_adapter.dev, "Error: Failed bus transaction\n");
+		dev_err(&piix4_adapter->dev, "Error: Failed bus transaction\n");
 	}
 
 	if (temp & 0x08) {
 		result = -EIO;
-		dev_dbg(&piix4_adapter.dev, "Bus collision! SMBus may be "
+		dev_dbg(&piix4_adapter->dev, "Bus collision! SMBus may be "
 			"locked until next hard reset. (sorry!)\n");
 		/* Clock stops and slave is stuck in mid-transmission */
 	}
 
 	if (temp & 0x04) {
 		result = -ENXIO;
-		dev_dbg(&piix4_adapter.dev, "Error: no response!\n");
+		dev_dbg(&piix4_adapter->dev, "Error: no response!\n");
 	}
 
 	if (inb_p(SMBHSTSTS) != 0x00)
 		outb_p(inb(SMBHSTSTS), SMBHSTSTS);
 
 	if ((temp = inb_p(SMBHSTSTS)) != 0x00) {
-		dev_err(&piix4_adapter.dev, "Failed reset at end of "
+		dev_err(&piix4_adapter->dev, "Failed reset at end of "
 			"transaction (%02x)\n", temp);
 	}
-	dev_dbg(&piix4_adapter.dev, "Transaction (post): CNT=%02x, CMD=%02x, "
+	dev_dbg(&piix4_adapter->dev, "Transaction (post): CNT=%02x, CMD=%02x, "
 		"ADD=%02x, DAT0=%02x, DAT1=%02x\n", inb_p(SMBHSTCNT),
 		inb_p(SMBHSTCMD), inb_p(SMBHSTADD), inb_p(SMBHSTDAT0),
 		inb_p(SMBHSTDAT1));
@@ -370,6 +415,8 @@ static s32 piix4_access(struct i2c_adapter * adap, u16 addr,
 		 unsigned short flags, char read_write,
 		 u8 command, int size, union i2c_smbus_data * data)
 {
+	struct i2c_piix4_adapdata *adapdata = i2c_get_adapdata(adap);
+	unsigned short piix4_smba = adapdata->smba;
 	int i, len;
 	int status;
 
@@ -426,7 +473,7 @@ static s32 piix4_access(struct i2c_adapter * adap, u16 addr,
 
 	outb_p((size & 0x1C) + (ENABLE_INT9 & 1), SMBHSTCNT);
 
-	status = piix4_transaction();
+	status = piix4_transaction(adap);
 	if (status)
 		return status;
 
@@ -466,12 +513,6 @@ static const struct i2c_algorithm smbus_algorithm = {
 	.functionality	= piix4_func,
 };
 
-static struct i2c_adapter piix4_adapter = {
-	.owner		= THIS_MODULE,
-	.class		= I2C_CLASS_HWMON | I2C_CLASS_SPD,
-	.algo		= &smbus_algorithm,
-};
-
 static DEFINE_PCI_DEVICE_TABLE(piix4_ids) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82371AB_3) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82443MX_3) },
@@ -496,6 +537,57 @@ static DEFINE_PCI_DEVICE_TABLE(piix4_ids) = {
 
 MODULE_DEVICE_TABLE (pci, piix4_ids);
 
+static struct i2c_adapter *piix4_main_adapter;
+static struct i2c_adapter *piix4_aux_adapter;
+
+static int __devinit piix4_add_adapter(struct pci_dev *dev,
+					unsigned short smba,
+					struct i2c_adapter **padap)
+{
+	struct i2c_adapter *adap;
+	struct i2c_piix4_adapdata *adapdata;
+	int retval;
+
+	adap = kzalloc(sizeof(*adap), GFP_KERNEL);
+	if (adap == NULL) {
+		release_region(smba, SMBIOSIZE);
+		return -ENOMEM;
+	}
+
+	adap->owner = THIS_MODULE;
+	adap->class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
+	adap->algo = &smbus_algorithm;
+
+	adapdata = kzalloc(sizeof(*adapdata), GFP_KERNEL);
+	if (adapdata == NULL) {
+		kfree(adap);
+		release_region(smba, SMBIOSIZE);
+		return -ENOMEM;
+	}
+
+	adapdata->smba = smba;
+
+	/* set up the sysfs linkage to our parent device */
+	adap->dev.parent = &dev->dev;
+
+	snprintf(adap->name, sizeof(adap->name),
+		"SMBus PIIX4 adapter at %04x", smba);
+
+	i2c_set_adapdata(adap, adapdata);
+
+	retval = i2c_add_adapter(adap);
+	if (retval) {
+		dev_err(&dev->dev, "Couldn't register adapter!\n");
+		kfree(adapdata);
+		kfree(adap);
+		release_region(smba, SMBIOSIZE);
+		return retval;
+	}
+
+	*padap = adap;
+	return 0;
+}
+
 static int __devinit piix4_probe(struct pci_dev *dev,
 				const struct pci_device_id *id)
 {
@@ -510,30 +602,52 @@ static int __devinit piix4_probe(struct pci_dev *dev,
 	else
 		retval = piix4_setup(dev, id);
 
-	if (retval)
+	/* If no main SMBus found, give up */
+	if (retval < 0)
 		return retval;
 
-	/* set up the sysfs linkage to our parent device */
-	piix4_adapter.dev.parent = &dev->dev;
-
-	snprintf(piix4_adapter.name, sizeof(piix4_adapter.name),
-		"SMBus PIIX4 adapter at %04x", piix4_smba);
+	/* Try to register main SMBus adapter, give up if we can't */
+	retval = piix4_add_adapter(dev, retval, &piix4_main_adapter);
+	if (retval < 0)
+		return retval;
 
-	if ((retval = i2c_add_adapter(&piix4_adapter))) {
-		dev_err(&dev->dev, "Couldn't register adapter!\n");
-		release_region(piix4_smba, SMBIOSIZE);
-		piix4_smba = 0;
+	/* Check for auxiliary SMBus on some AMD chipsets */
+	if (dev->vendor == PCI_VENDOR_ID_ATI &&
+	    dev->device == PCI_DEVICE_ID_ATI_SBX00_SMBUS &&
+	    dev->revision < 0x40) {
+		retval = piix4_setup_aux(dev, id, 0x58);
+		if (retval > 0) {
+			/* Try to add the aux adapter if it exists,
+			 * piix4_add_adapter will clean up if this fails */
+			piix4_add_adapter(dev, retval, &piix4_aux_adapter);
+		}
 	}
 
-	return retval;
+	return 0;
+}
+
+static void __devexit piix4_adap_remove(struct i2c_adapter *adap)
+{
+	struct i2c_piix4_adapdata *adapdata = i2c_get_adapdata(adap);
+
+	if (adapdata->smba) {
+		i2c_del_adapter(adap);
+		release_region(adapdata->smba, SMBIOSIZE);
+		kfree(adapdata);
+		kfree(adap);
+	}
 }
 
 static void __devexit piix4_remove(struct pci_dev *dev)
 {
-	if (piix4_smba) {
-		i2c_del_adapter(&piix4_adapter);
-		release_region(piix4_smba, SMBIOSIZE);
-		piix4_smba = 0;
+	if (piix4_main_adapter) {
+		piix4_adap_remove(piix4_main_adapter);
+		piix4_main_adapter = NULL;
+	}
+
+	if (piix4_aux_adapter) {
+		piix4_adap_remove(piix4_aux_adapter);
+		piix4_aux_adapter = NULL;
 	}
 }
 
@@ -544,20 +658,9 @@ static struct pci_driver piix4_driver = {
 	.remove		= __devexit_p(piix4_remove),
 };
 
-static int __init i2c_piix4_init(void)
-{
-	return pci_register_driver(&piix4_driver);
-}
-
-static void __exit i2c_piix4_exit(void)
-{
-	pci_unregister_driver(&piix4_driver);
-}
+module_pci_driver(piix4_driver);
 
 MODULE_AUTHOR("Frodo Looijaard <frodol@dds.nl> and "
 		"Philip Edelbrock <phil@netroedge.com>");
 MODULE_DESCRIPTION("PIIX4 SMBus driver");
 MODULE_LICENSE("GPL");
-
-module_init(i2c_piix4_init);
-module_exit(i2c_piix4_exit);
diff --git a/drivers/i2c/busses/i2c-pmcmsp.c b/drivers/i2c/busses/i2c-pmcmsp.c
index 07b7447ecbc..3d71395ae1f 100644
--- a/drivers/i2c/busses/i2c-pmcmsp.c
+++ b/drivers/i2c/busses/i2c-pmcmsp.c
@@ -306,8 +306,7 @@ static int __devinit pmcmsptwi_probe(struct platform_device *pldev)
 	pmcmsptwi_data.irq = platform_get_irq(pldev, 0);
 	if (pmcmsptwi_data.irq) {
 		rc = request_irq(pmcmsptwi_data.irq, &pmcmsptwi_interrupt,
-			IRQF_SHARED | IRQF_SAMPLE_RANDOM,
-			pldev->name, &pmcmsptwi_data);
+				 IRQF_SHARED, pldev->name, &pmcmsptwi_data);
 		if (rc == 0) {
 			/*
 			 * Enable 'DONE' interrupt only.
diff --git a/drivers/i2c/busses/i2c-pnx.c b/drivers/i2c/busses/i2c-pnx.c
index 99389d2eae5..5d54416770b 100644
--- a/drivers/i2c/busses/i2c-pnx.c
+++ b/drivers/i2c/busses/i2c-pnx.c
@@ -587,25 +587,27 @@ static struct i2c_algorithm pnx_algorithm = {
 };
 
 #ifdef CONFIG_PM
-static int i2c_pnx_controller_suspend(struct platform_device *pdev,
-				      pm_message_t state)
+static int i2c_pnx_controller_suspend(struct device *dev)
 {
-	struct i2c_pnx_algo_data *alg_data = platform_get_drvdata(pdev);
+	struct i2c_pnx_algo_data *alg_data = dev_get_drvdata(dev);
 
 	clk_disable(alg_data->clk);
 
 	return 0;
 }
 
-static int i2c_pnx_controller_resume(struct platform_device *pdev)
+static int i2c_pnx_controller_resume(struct device *dev)
 {
-	struct i2c_pnx_algo_data *alg_data = platform_get_drvdata(pdev);
+	struct i2c_pnx_algo_data *alg_data = dev_get_drvdata(dev);
 
 	return clk_enable(alg_data->clk);
 }
+
+static SIMPLE_DEV_PM_OPS(i2c_pnx_pm,
+			 i2c_pnx_controller_suspend, i2c_pnx_controller_resume);
+#define PNX_I2C_PM	(&i2c_pnx_pm)
 #else
-#define i2c_pnx_controller_suspend	NULL
-#define i2c_pnx_controller_resume	NULL
+#define PNX_I2C_PM	NULL
 #endif
 
 static int __devinit i2c_pnx_probe(struct platform_device *pdev)
@@ -783,11 +785,10 @@ static struct platform_driver i2c_pnx_driver = {
 		.name = "pnx-i2c",
 		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(i2c_pnx_of_match),
+		.pm = PNX_I2C_PM,
 	},
 	.probe = i2c_pnx_probe,
 	.remove = __devexit_p(i2c_pnx_remove),
-	.suspend = i2c_pnx_controller_suspend,
-	.resume = i2c_pnx_controller_resume,
 };
 
 static int __init i2c_adap_pnx_init(void)
diff --git a/drivers/i2c/busses/i2c-powermac.c b/drivers/i2c/busses/i2c-powermac.c
index 31c47e18d83..5285f8565de 100644
--- a/drivers/i2c/busses/i2c-powermac.c
+++ b/drivers/i2c/busses/i2c-powermac.c
@@ -227,28 +227,138 @@ static int __devexit i2c_powermac_remove(struct platform_device *dev)
 	return 0;
 }
 
+static u32 __devinit i2c_powermac_get_addr(struct i2c_adapter *adap,
+					   struct pmac_i2c_bus *bus,
+					   struct device_node *node)
+{
+	const __be32 *prop;
+	int len;
+
+	/* First check for valid "reg" */
+	prop = of_get_property(node, "reg", &len);
+	if (prop && (len >= sizeof(int)))
+		return (be32_to_cpup(prop) & 0xff) >> 1;
+
+	/* Then check old-style "i2c-address" */
+	prop = of_get_property(node, "i2c-address", &len);
+	if (prop && (len >= sizeof(int)))
+		return (be32_to_cpup(prop) & 0xff) >> 1;
+
+	/* Now handle some devices with missing "reg" properties */
+	if (!strcmp(node->name, "cereal"))
+		return 0x60;
+	else if (!strcmp(node->name, "deq"))
+		return 0x34;
+
+	dev_warn(&adap->dev, "No i2c address for %s\n", node->full_name);
+
+	return 0xffffffff;
+}
+
+static void __devinit i2c_powermac_create_one(struct i2c_adapter *adap,
+					      const char *type,
+					      u32 addr)
+{
+	struct i2c_board_info info = {};
+	struct i2c_client *newdev;
+
+	strncpy(info.type, type, sizeof(info.type));
+	info.addr = addr;
+	newdev = i2c_new_device(adap, &info);
+	if (!newdev)
+		dev_err(&adap->dev,
+			"i2c-powermac: Failure to register missing %s\n",
+			type);
+}
+
+static void __devinit i2c_powermac_add_missing(struct i2c_adapter *adap,
+					       struct pmac_i2c_bus *bus,
+					       bool found_onyx)
+{
+	struct device_node *busnode = pmac_i2c_get_bus_node(bus);
+	int rc;
+
+	/* Check for the onyx audio codec */
+#define ONYX_REG_CONTROL		67
+	if (of_device_is_compatible(busnode, "k2-i2c") && !found_onyx) {
+		union i2c_smbus_data data;
+
+		rc = i2c_smbus_xfer(adap, 0x46, 0, I2C_SMBUS_READ,
+				    ONYX_REG_CONTROL, I2C_SMBUS_BYTE_DATA,
+				    &data);
+		if (rc >= 0)
+			i2c_powermac_create_one(adap, "MAC,pcm3052", 0x46);
+
+		rc = i2c_smbus_xfer(adap, 0x47, 0, I2C_SMBUS_READ,
+				    ONYX_REG_CONTROL, I2C_SMBUS_BYTE_DATA,
+				    &data);
+		if (rc >= 0)
+			i2c_powermac_create_one(adap, "MAC,pcm3052", 0x47);
+	}
+}
+
+static bool __devinit i2c_powermac_get_type(struct i2c_adapter *adap,
+					    struct device_node *node,
+					    u32 addr, char *type, int type_size)
+{
+	char tmp[16];
+
+	/* Note: we to _NOT_ want the standard
+	 * i2c drivers to match with any of our powermac stuff
+	 * unless they have been specifically modified to handle
+	 * it on a case by case basis. For example, for thermal
+	 * control, things like lm75 etc... shall match with their
+	 * corresponding windfarm drivers, _NOT_ the generic ones,
+	 * so we force a prefix of AAPL, onto the modalias to
+	 * make that happen
+	 */
+
+	/* First try proper modalias */
+	if (of_modalias_node(node, tmp, sizeof(tmp)) >= 0) {
+		snprintf(type, type_size, "MAC,%s", tmp);
+		return true;
+	}
+
+	/* Now look for known workarounds */
+	if (!strcmp(node->name, "deq")) {
+		/* Apple uses address 0x34 for TAS3001 and 0x35 for TAS3004 */
+		if (addr == 0x34) {
+			snprintf(type, type_size, "MAC,tas3001");
+			return true;
+		} else if (addr == 0x35) {
+			snprintf(type, type_size, "MAC,tas3004");
+			return true;
+		}
+	}
+
+	dev_err(&adap->dev, "i2c-powermac: modalias failure"
+		" on %s\n", node->full_name);
+	return false;
+}
+
 static void __devinit i2c_powermac_register_devices(struct i2c_adapter *adap,
 						    struct pmac_i2c_bus *bus)
 {
 	struct i2c_client *newdev;
 	struct device_node *node;
+	bool found_onyx = 0;
+
+	/*
+	 * In some cases we end up with the via-pmu node itself, in this
+	 * case we skip this function completely as the device-tree will
+	 * not contain anything useful.
+	 */
+	if (!strcmp(adap->dev.of_node->name, "via-pmu"))
+		return;
 
 	for_each_child_of_node(adap->dev.of_node, node) {
 		struct i2c_board_info info = {};
-		struct dev_archdata dev_ad = {};
-		const __be32 *reg;
-		char tmp[16];
 		u32 addr;
-		int len;
 
 		/* Get address & channel */
-		reg = of_get_property(node, "reg", &len);
-		if (!reg || (len < sizeof(int))) {
-			dev_err(&adap->dev, "i2c-powermac: invalid reg on %s\n",
-				node->full_name);
+		addr = i2c_powermac_get_addr(adap, bus, node);
+		if (addr == 0xffffffff)
 			continue;
-		}
-		addr = be32_to_cpup(reg);
 
 		/* Multibus setup, check channel */
 		if (!pmac_i2c_match_adapter(node, adap))
@@ -257,27 +367,23 @@ static void __devinit i2c_powermac_register_devices(struct i2c_adapter *adap,
 		dev_dbg(&adap->dev, "i2c-powermac: register %s\n",
 			node->full_name);
 
-		/* Make up a modalias. Note: we to _NOT_ want the standard
-		 * i2c drivers to match with any of our powermac stuff
-		 * unless they have been specifically modified to handle
-		 * it on a case by case basis. For example, for thermal
-		 * control, things like lm75 etc... shall match with their
-		 * corresponding windfarm drivers, _NOT_ the generic ones,
-		 * so we force a prefix of AAPL, onto the modalias to
-		 * make that happen
+		/*
+		 * Keep track of some device existence to handle
+		 * workarounds later.
 		 */
-		if (of_modalias_node(node, tmp, sizeof(tmp)) < 0) {
-			dev_err(&adap->dev, "i2c-powermac: modalias failure"
-				" on %s\n", node->full_name);
+		if (of_device_is_compatible(node, "pcm3052"))
+			found_onyx = true;
+
+		/* Make up a modalias */
+		if (!i2c_powermac_get_type(adap, node, addr,
+					   info.type, sizeof(info.type))) {
 			continue;
 		}
-		snprintf(info.type, sizeof(info.type), "MAC,%s", tmp);
 
 		/* Fill out the rest of the info structure */
-		info.addr = (addr & 0xff) >> 1;
+		info.addr = addr;
 		info.irq = irq_of_parse_and_map(node, 0);
 		info.of_node = of_node_get(node);
-		info.archdata = &dev_ad;
 
 		newdev = i2c_new_device(adap, &info);
 		if (!newdev) {
@@ -292,6 +398,9 @@ static void __devinit i2c_powermac_register_devices(struct i2c_adapter *adap,
 			continue;
 		}
 	}
+
+	/* Additional workarounds */
+	i2c_powermac_add_missing(adap, bus, found_onyx);
 }
 
 static int __devinit i2c_powermac_probe(struct platform_device *dev)
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
index 93709fbe30e..d8515be00b9 100644
--- a/drivers/i2c/busses/i2c-puv3.c
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -254,7 +254,7 @@ static int __devexit puv3_i2c_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-static int puv3_i2c_suspend(struct platform_device *dev, pm_message_t state)
+static int puv3_i2c_suspend(struct device *dev)
 {
 	int poll_count;
 	/* Disable the IIC */
@@ -267,23 +267,20 @@ static int puv3_i2c_suspend(struct platform_device *dev, pm_message_t state)
 	return 0;
 }
 
-static int puv3_i2c_resume(struct platform_device *dev)
-{
-	return 0 ;
-}
+static SIMPLE_DEV_PM_OPS(puv3_i2c_pm, puv3_i2c_suspend, NULL);
+#define PUV3_I2C_PM	(&puv3_i2c_pm)
+
 #else
-#define puv3_i2c_suspend NULL
-#define puv3_i2c_resume NULL
+#define PUV3_I2C_PM	NULL
 #endif
 
 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,
+		.pm	= PUV3_I2C_PM,
 	}
 };
 
diff --git a/drivers/i2c/busses/i2c-pxa-pci.c b/drivers/i2c/busses/i2c-pxa-pci.c
index a0581798055..4dc9bef17d7 100644
--- a/drivers/i2c/busses/i2c-pxa-pci.c
+++ b/drivers/i2c/busses/i2c-pxa-pci.c
@@ -163,17 +163,7 @@ static struct pci_driver ce4100_i2c_driver = {
 	.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_pci_driver(ce4100_i2c_driver);
 
 MODULE_DESCRIPTION("CE4100 PCI-I2C glue code for PXA's driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-pxa.c b/drivers/i2c/busses/i2c-pxa.c
index a997c7d3f95..1034d93fb83 100644
--- a/drivers/i2c/busses/i2c-pxa.c
+++ b/drivers/i2c/busses/i2c-pxa.c
@@ -41,13 +41,6 @@
 
 #include <asm/irq.h>
 
-#ifndef CONFIG_HAVE_CLK
-#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;
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 01959154572..5ae3b0236bd 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -122,7 +122,7 @@ static inline unsigned int s3c24xx_get_device_quirks(struct platform_device *pde
 {
 	if (pdev->dev.of_node) {
 		const struct of_device_id *match;
-		match = of_match_node(&s3c24xx_i2c_match, pdev->dev.of_node);
+		match = of_match_node(s3c24xx_i2c_match, pdev->dev.of_node);
 		return (unsigned int)match->data;
 	}
 
@@ -609,7 +609,7 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 
 		if (ret != -EAGAIN) {
 			clk_disable(i2c->clk);
-			pm_runtime_put_sync(&adap->dev);
+			pm_runtime_put(&adap->dev);
 			return ret;
 		}
 
@@ -619,7 +619,7 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 	}
 
 	clk_disable(i2c->clk);
-	pm_runtime_put_sync(&adap->dev);
+	pm_runtime_put(&adap->dev);
 	return -EREMOTEIO;
 }
 
diff --git a/drivers/i2c/busses/i2c-sis630.c b/drivers/i2c/busses/i2c-sis630.c
index 15cf78f65ce..5d6723b7525 100644
--- a/drivers/i2c/busses/i2c-sis630.c
+++ b/drivers/i2c/busses/i2c-sis630.c
@@ -513,21 +513,8 @@ static struct pci_driver sis630_driver = {
 	.remove		= __devexit_p(sis630_remove),
 };
 
-static int __init i2c_sis630_init(void)
-{
-	return pci_register_driver(&sis630_driver);
-}
-
-
-static void __exit i2c_sis630_exit(void)
-{
-	pci_unregister_driver(&sis630_driver);
-}
-
+module_pci_driver(sis630_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Alexander Malysh <amalysh@web.de>");
 MODULE_DESCRIPTION("SIS630 SMBus driver");
-
-module_init(i2c_sis630_init);
-module_exit(i2c_sis630_exit);
diff --git a/drivers/i2c/busses/i2c-sis96x.c b/drivers/i2c/busses/i2c-sis96x.c
index cc5d149413f..7b72614a9bc 100644
--- a/drivers/i2c/busses/i2c-sis96x.c
+++ b/drivers/i2c/busses/i2c-sis96x.c
@@ -324,21 +324,8 @@ static struct pci_driver sis96x_driver = {
 	.remove		= __devexit_p(sis96x_remove),
 };
 
-static int __init i2c_sis96x_init(void)
-{
-	return pci_register_driver(&sis96x_driver);
-}
-
-static void __exit i2c_sis96x_exit(void)
-{
-	pci_unregister_driver(&sis96x_driver);
-}
+module_pci_driver(sis96x_driver);
 
 MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
 MODULE_DESCRIPTION("SiS96x SMBus driver");
 MODULE_LICENSE("GPL");
-
-/* Register initialization functions using helper macros */
-module_init(i2c_sis96x_init);
-module_exit(i2c_sis96x_exit);
-
diff --git a/drivers/i2c/busses/i2c-stu300.c b/drivers/i2c/busses/i2c-stu300.c
index 4d44af181f3..580a0c04cb4 100644
--- a/drivers/i2c/busses/i2c-stu300.c
+++ b/drivers/i2c/busses/i2c-stu300.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2007-2009 ST-Ericsson AB
+ * Copyright (C) 2007-2012 ST-Ericsson AB
  * License terms: GNU General Public License (GPL) version 2
  * ST DDC I2C master mode driver, used in e.g. U300 series platforms.
  * Author: Linus Walleij <linus.walleij@stericsson.com>
@@ -139,8 +139,6 @@ module_param(scl_frequency, uint,  0644);
  * struct stu300_dev - the stu300 driver state holder
  * @pdev: parent platform device
  * @adapter: corresponding I2C adapter
- * @phybase: location of I/O area in memory
- * @physize: size of I/O area in memory
  * @clk: hardware block clock
  * @irq: assigned interrupt line
  * @cmd_issue_lock: this locks the following cmd_ variables
@@ -155,8 +153,6 @@ module_param(scl_frequency, uint,  0644);
 struct stu300_dev {
 	struct platform_device	*pdev;
 	struct i2c_adapter	adapter;
-	resource_size_t		phybase;
-	resource_size_t		physize;
 	void __iomem		*virtbase;
 	struct clk		*clk;
 	int			irq;
@@ -873,64 +869,44 @@ stu300_probe(struct platform_device *pdev)
 	int ret = 0;
 	char clk_name[] = "I2C0";
 
-	dev = kzalloc(sizeof(struct stu300_dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(struct stu300_dev), GFP_KERNEL);
 	if (!dev) {
 		dev_err(&pdev->dev, "could not allocate device struct\n");
-		ret = -ENOMEM;
-		goto err_no_devmem;
+		return -ENOMEM;
 	}
 
 	bus_nr = pdev->id;
 	clk_name[3] += (char)bus_nr;
-	dev->clk = clk_get(&pdev->dev, clk_name);
+	dev->clk = devm_clk_get(&pdev->dev, clk_name);
 	if (IS_ERR(dev->clk)) {
-		ret = PTR_ERR(dev->clk);
 		dev_err(&pdev->dev, "could not retrieve i2c bus clock\n");
-		goto err_no_clk;
+		return PTR_ERR(dev->clk);
 	}
 
 	dev->pdev = pdev;
-	platform_set_drvdata(pdev, dev);
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENOENT;
-		goto err_no_resource;
-	}
-
-	dev->phybase = res->start;
-	dev->physize = resource_size(res);
-
-	if (request_mem_region(dev->phybase, dev->physize,
-			       NAME " I/O Area") == NULL) {
-		ret = -EBUSY;
-		goto err_no_ioregion;
-	}
+	if (!res)
+		return -ENOENT;
 
-	dev->virtbase = ioremap(dev->phybase, dev->physize);
+	dev->virtbase = devm_request_and_ioremap(&pdev->dev, res);
 	dev_dbg(&pdev->dev, "initialize bus device I2C%d on virtual "
 		"base %p\n", bus_nr, dev->virtbase);
-	if (!dev->virtbase) {
-		ret = -ENOMEM;
-		goto err_no_ioremap;
-	}
+	if (!dev->virtbase)
+		return -ENOMEM;
 
 	dev->irq = platform_get_irq(pdev, 0);
-	if (request_irq(dev->irq, stu300_irh, 0,
-			NAME, dev)) {
-		ret = -EIO;
-		goto err_no_irq;
-	}
+	ret = devm_request_irq(&pdev->dev, dev->irq, stu300_irh, 0, NAME, dev);
+	if (ret < 0)
+		return ret;
 
 	dev->speed = scl_frequency;
 
-	clk_enable(dev->clk);
+	clk_prepare_enable(dev->clk);
 	ret = stu300_init_hw(dev);
 	clk_disable(dev->clk);
-
 	if (ret != 0) {
 		dev_err(&dev->pdev->dev, "error initializing hardware.\n");
-		goto err_init_hw;
+		return -EIO;
 	}
 
 	/* IRQ event handling initialization */
@@ -952,57 +928,43 @@ stu300_probe(struct platform_device *pdev)
 	/* i2c device drivers may be active on return from add_adapter() */
 	ret = i2c_add_numbered_adapter(adap);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "failure adding ST Micro DDC "
+		dev_err(&pdev->dev, "failure adding ST Micro DDC "
 		       "I2C adapter\n");
-		goto err_add_adapter;
+		return ret;
 	}
-	return 0;
 
- err_add_adapter:
- err_init_hw:
-	free_irq(dev->irq, dev);
- err_no_irq:
-	iounmap(dev->virtbase);
- err_no_ioremap:
-	release_mem_region(dev->phybase, dev->physize);
- err_no_ioregion:
-	platform_set_drvdata(pdev, NULL);
- err_no_resource:
-	clk_put(dev->clk);
- err_no_clk:
-	kfree(dev);
- err_no_devmem:
-	dev_err(&pdev->dev, "failed to add " NAME " adapter: %d\n",
-		pdev->id);
-	return ret;
+	platform_set_drvdata(pdev, dev);
+	return 0;
 }
 
 #ifdef CONFIG_PM
-static int stu300_suspend(struct platform_device *pdev, pm_message_t state)
+static int stu300_suspend(struct device *device)
 {
-	struct stu300_dev *dev = platform_get_drvdata(pdev);
+	struct stu300_dev *dev = dev_get_drvdata(device);
 
 	/* Turn off everything */
 	stu300_wr8(0x00, dev->virtbase + I2C_CR);
 	return 0;
 }
 
-static int stu300_resume(struct platform_device *pdev)
+static int stu300_resume(struct device *device)
 {
 	int ret = 0;
-	struct stu300_dev *dev = platform_get_drvdata(pdev);
+	struct stu300_dev *dev = dev_get_drvdata(device);
 
 	clk_enable(dev->clk);
 	ret = stu300_init_hw(dev);
 	clk_disable(dev->clk);
 
 	if (ret != 0)
-		dev_err(&pdev->dev, "error re-initializing hardware.\n");
+		dev_err(device, "error re-initializing hardware.\n");
 	return ret;
 }
+
+static SIMPLE_DEV_PM_OPS(stu300_pm, stu300_suspend, stu300_resume);
+#define STU300_I2C_PM	(&stu300_pm)
 #else
-#define stu300_suspend NULL
-#define stu300_resume NULL
+#define STU300_I2C_PM	NULL
 #endif
 
 static int __exit
@@ -1013,12 +975,7 @@ stu300_remove(struct platform_device *pdev)
 	i2c_del_adapter(&dev->adapter);
 	/* Turn off everything */
 	stu300_wr8(0x00, dev->virtbase + I2C_CR);
-	free_irq(dev->irq, dev);
-	iounmap(dev->virtbase);
-	release_mem_region(dev->phybase, dev->physize);
-	clk_put(dev->clk);
 	platform_set_drvdata(pdev, NULL);
-	kfree(dev);
 	return 0;
 }
 
@@ -1026,10 +983,9 @@ static struct platform_driver stu300_i2c_driver = {
 	.driver = {
 		.name	= NAME,
 		.owner	= THIS_MODULE,
+		.pm	= STU300_I2C_PM,
 	},
 	.remove		= __exit_p(stu300_remove),
-	.suspend        = stu300_suspend,
-	.resume         = stu300_resume,
 
 };
 
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 8b2e555a956..66eb53fac20 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -97,8 +97,21 @@
 #define I2C_HEADER_10BIT_ADDR			(1<<18)
 #define I2C_HEADER_IE_ENABLE			(1<<17)
 #define I2C_HEADER_REPEAT_START			(1<<16)
+#define I2C_HEADER_CONTINUE_XFER		(1<<15)
 #define I2C_HEADER_MASTER_ADDR_SHIFT		12
 #define I2C_HEADER_SLAVE_ADDR_SHIFT		1
+/*
+ * msg_end_type: The bus control which need to be send at end of transfer.
+ * @MSG_END_STOP: Send stop pulse at end of transfer.
+ * @MSG_END_REPEAT_START: Send repeat start at end of transfer.
+ * @MSG_END_CONTINUE: The following on message is coming and so do not send
+ *		stop or repeat start.
+ */
+enum msg_end_type {
+	MSG_END_STOP,
+	MSG_END_REPEAT_START,
+	MSG_END_CONTINUE,
+};
 
 /**
  * struct tegra_i2c_dev	- per device i2c context
@@ -106,7 +119,6 @@
  * @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
@@ -124,7 +136,6 @@ struct tegra_i2c_dev {
 	struct i2c_adapter adapter;
 	struct clk *clk;
 	struct clk *i2c_clk;
-	struct resource *iomem;
 	void __iomem *base;
 	int cont_id;
 	int irq;
@@ -165,6 +176,10 @@ 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));
+
+	/* Read back register to make sure that register writes completed */
+	if (reg != I2C_TX_FIFO)
+		readl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
 }
 
 static u32 i2c_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
@@ -341,7 +356,7 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 	u32 val;
 	int err = 0;
 
-	clk_enable(i2c_dev->clk);
+	clk_prepare_enable(i2c_dev->clk);
 
 	tegra_periph_reset_assert(i2c_dev->clk);
 	udelay(2);
@@ -372,7 +387,7 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 	if (tegra_i2c_flush_fifos(i2c_dev))
 		err = -ETIMEDOUT;
 
-	clk_disable(i2c_dev->clk);
+	clk_disable_unprepare(i2c_dev->clk);
 
 	if (i2c_dev->irq_disabled) {
 		i2c_dev->irq_disabled = 0;
@@ -449,7 +464,7 @@ err:
 }
 
 static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
-	struct i2c_msg *msg, int stop)
+	struct i2c_msg *msg, enum msg_end_type end_state)
 {
 	u32 packet_header;
 	u32 int_mask;
@@ -476,7 +491,9 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
 
 	packet_header = I2C_HEADER_IE_ENABLE;
-	if (!stop)
+	if (end_state == MSG_END_CONTINUE)
+		packet_header |= I2C_HEADER_CONTINUE_XFER;
+	else if (end_state == MSG_END_REPEAT_START)
 		packet_header |= I2C_HEADER_REPEAT_START;
 	if (msg->flags & I2C_M_TEN) {
 		packet_header |= msg->addr;
@@ -546,20 +563,27 @@ static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
 	if (i2c_dev->is_suspended)
 		return -EBUSY;
 
-	clk_enable(i2c_dev->clk);
+	clk_prepare_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);
+		enum msg_end_type end_type = MSG_END_STOP;
+		if (i < (num - 1)) {
+			if (msgs[i + 1].flags & I2C_M_NOSTART)
+				end_type = MSG_END_CONTINUE;
+			else
+				end_type = MSG_END_REPEAT_START;
+		}
+		ret = tegra_i2c_xfer_msg(i2c_dev, &msgs[i], end_type);
 		if (ret)
 			break;
 	}
-	clk_disable(i2c_dev->clk);
+	clk_disable_unprepare(i2c_dev->clk);
 	return ret ?: i;
 }
 
 static u32 tegra_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR |
+		I2C_FUNC_PROTOCOL_MANGLING | I2C_FUNC_NOSTART;
 }
 
 static const struct i2c_algorithm tegra_i2c_algo = {
@@ -572,7 +596,6 @@ static int __devinit 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;
 	const unsigned int *prop;
@@ -585,50 +608,41 @@ static int __devinit tegra_i2c_probe(struct platform_device *pdev)
 		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));
+	base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!base) {
-		dev_err(&pdev->dev, "Cannot ioremap I2C region\n");
-		return -ENOMEM;
+		dev_err(&pdev->dev, "Cannot request/ioremap I2C registers\n");
+		return -EADDRNOTAVAIL;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no irq resource\n");
-		ret = -EINVAL;
-		goto err_iounmap;
+		return -EINVAL;
 	}
 	irq = res->start;
 
-	clk = clk_get(&pdev->dev, NULL);
+	clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
 		dev_err(&pdev->dev, "missing controller clock");
-		ret = PTR_ERR(clk);
-		goto err_release_region;
+		return PTR_ERR(clk);
 	}
 
-	i2c_clk = clk_get(&pdev->dev, "i2c");
+	i2c_clk = devm_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;
+		return PTR_ERR(i2c_clk);
 	}
 
-	i2c_dev = kzalloc(sizeof(struct tegra_i2c_dev), GFP_KERNEL);
+	i2c_dev = devm_kzalloc(&pdev->dev, sizeof(*i2c_dev), GFP_KERNEL);
 	if (!i2c_dev) {
-		ret = -ENOMEM;
-		goto err_i2c_clk_put;
+		dev_err(&pdev->dev, "Could not allocate struct tegra_i2c_dev");
+		return -ENOMEM;
 	}
 
 	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;
@@ -657,16 +671,17 @@ static int __devinit tegra_i2c_probe(struct platform_device *pdev)
 	ret = tegra_i2c_init(i2c_dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to initialize i2c controller");
-		goto err_free;
+		return ret;
 	}
 
-	ret = request_irq(i2c_dev->irq, tegra_i2c_isr, 0, pdev->name, i2c_dev);
+	ret = devm_request_irq(&pdev->dev, 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;
+		return ret;
 	}
 
-	clk_enable(i2c_dev->i2c_clk);
+	clk_prepare_enable(i2c_dev->i2c_clk);
 
 	i2c_set_adapdata(&i2c_dev->adapter, i2c_dev);
 	i2c_dev->adapter.owner = THIS_MODULE;
@@ -681,45 +696,26 @@ static int __devinit tegra_i2c_probe(struct platform_device *pdev)
 	ret = i2c_add_numbered_adapter(&i2c_dev->adapter);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to add I2C adapter\n");
-		goto err_free_irq;
+		clk_disable_unprepare(i2c_dev->i2c_clk);
+		return ret;
 	}
 
 	of_i2c_register_devices(&i2c_dev->adapter);
 
 	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 __devexit 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)
+static int tegra_i2c_suspend(struct device *dev)
 {
-	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+	struct tegra_i2c_dev *i2c_dev = dev_get_drvdata(dev);
 
 	i2c_lock_adapter(&i2c_dev->adapter);
 	i2c_dev->is_suspended = true;
@@ -728,9 +724,9 @@ static int tegra_i2c_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int tegra_i2c_resume(struct platform_device *pdev)
+static int tegra_i2c_resume(struct device *dev)
 {
-	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
+	struct tegra_i2c_dev *i2c_dev = dev_get_drvdata(dev);
 	int ret;
 
 	i2c_lock_adapter(&i2c_dev->adapter);
@@ -748,6 +744,11 @@ static int tegra_i2c_resume(struct platform_device *pdev)
 
 	return 0;
 }
+
+static SIMPLE_DEV_PM_OPS(tegra_i2c_pm, tegra_i2c_suspend, tegra_i2c_resume);
+#define TEGRA_I2C_PM	(&tegra_i2c_pm)
+#else
+#define TEGRA_I2C_PM	NULL
 #endif
 
 #if defined(CONFIG_OF)
@@ -758,21 +759,16 @@ static const struct of_device_id tegra_i2c_of_match[] __devinitconst = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, tegra_i2c_of_match);
-#else
-#define tegra_i2c_of_match NULL
 #endif
 
 static struct platform_driver tegra_i2c_driver = {
 	.probe   = tegra_i2c_probe,
 	.remove  = __devexit_p(tegra_i2c_remove),
-#ifdef CONFIG_PM
-	.suspend = tegra_i2c_suspend,
-	.resume  = tegra_i2c_resume,
-#endif
 	.driver  = {
 		.name  = "tegra-i2c",
 		.owner = THIS_MODULE,
-		.of_match_table = tegra_i2c_of_match,
+		.of_match_table = of_match_ptr(tegra_i2c_of_match),
+		.pm    = TEGRA_I2C_PM,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-tiny-usb.c b/drivers/i2c/busses/i2c-tiny-usb.c
index f07307ff360..05106368d40 100644
--- a/drivers/i2c/busses/i2c-tiny-usb.c
+++ b/drivers/i2c/busses/i2c-tiny-usb.c
@@ -143,6 +143,7 @@ static const struct i2c_algorithm usb_algorithm = {
 static const struct usb_device_id i2c_tiny_usb_table[] = {
 	{ USB_DEVICE(0x0403, 0xc631) },   /* FTDI */
 	{ USB_DEVICE(0x1c40, 0x0534) },   /* EZPrototypes */
+	{ USB_DEVICE(0x1964, 0x0001) },   /* Robofuzz OSIF */
 	{ }                               /* Terminating entry */
 };
 
diff --git a/drivers/i2c/busses/i2c-via.c b/drivers/i2c/busses/i2c-via.c
index 713d31ade26..7ffee71ca19 100644
--- a/drivers/i2c/busses/i2c-via.c
+++ b/drivers/i2c/busses/i2c-via.c
@@ -161,20 +161,8 @@ static struct pci_driver vt586b_driver = {
 	.remove		= __devexit_p(vt586b_remove),
 };
 
-static int __init i2c_vt586b_init(void)
-{
-	return pci_register_driver(&vt586b_driver);
-}
-
-static void __exit i2c_vt586b_exit(void)
-{
-	pci_unregister_driver(&vt586b_driver);
-}
-
+module_pci_driver(vt586b_driver);
 
 MODULE_AUTHOR("Kyösti Mälkki <kmalkki@cc.hut.fi>");
 MODULE_DESCRIPTION("i2c for Via vt82c586b southbridge");
 MODULE_LICENSE("GPL");
-
-module_init(i2c_vt586b_init);
-module_exit(i2c_vt586b_exit);
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index a6ad32bc0a9..2efa56c5ff2 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -1312,6 +1312,37 @@ module_exit(i2c_exit);
  */
 
 /**
+ * __i2c_transfer - unlocked flavor of i2c_transfer
+ * @adap: Handle to I2C bus
+ * @msgs: One or more messages to execute before STOP is issued to
+ *	terminate the operation; each message begins with a START.
+ * @num: Number of messages to be executed.
+ *
+ * Returns negative errno, else the number of messages executed.
+ *
+ * Adapter lock must be held when calling this function. No debug logging
+ * takes place. adap->algo->master_xfer existence isn't checked.
+ */
+int __i2c_transfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
+{
+	unsigned long orig_jiffies;
+	int ret, try;
+
+	/* Retry automatically on arbitration loss */
+	orig_jiffies = jiffies;
+	for (ret = 0, try = 0; try <= adap->retries; try++) {
+		ret = adap->algo->master_xfer(adap, msgs, num);
+		if (ret != -EAGAIN)
+			break;
+		if (time_after(jiffies, orig_jiffies + adap->timeout))
+			break;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(__i2c_transfer);
+
+/**
  * i2c_transfer - execute a single or combined I2C message
  * @adap: Handle to I2C bus
  * @msgs: One or more messages to execute before STOP is issued to
@@ -1325,8 +1356,7 @@ module_exit(i2c_exit);
  */
 int i2c_transfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 {
-	unsigned long orig_jiffies;
-	int ret, try;
+	int ret;
 
 	/* REVISIT the fault reporting model here is weak:
 	 *
@@ -1364,15 +1394,7 @@ int i2c_transfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 			i2c_lock_adapter(adap);
 		}
 
-		/* Retry automatically on arbitration loss */
-		orig_jiffies = jiffies;
-		for (ret = 0, try = 0; try <= adap->retries; try++) {
-			ret = adap->algo->master_xfer(adap, msgs, num);
-			if (ret != -EAGAIN)
-				break;
-			if (time_after(jiffies, orig_jiffies + adap->timeout))
-				break;
-		}
+		ret = __i2c_transfer(adap, msgs, num);
 		i2c_unlock_adapter(adap);
 
 		return ret;
@@ -2122,7 +2144,7 @@ s32 i2c_smbus_xfer(struct i2c_adapter *adapter, u16 addr, unsigned short flags,
 	int try;
 	s32 res;
 
-	flags &= I2C_M_TEN | I2C_CLIENT_PEC;
+	flags &= I2C_M_TEN | I2C_CLIENT_PEC | I2C_CLIENT_SCCB;
 
 	if (adapter->algo->smbus_xfer) {
 		i2c_lock_adapter(adapter);
@@ -2140,11 +2162,17 @@ s32 i2c_smbus_xfer(struct i2c_adapter *adapter, u16 addr, unsigned short flags,
 				break;
 		}
 		i2c_unlock_adapter(adapter);
-	} else
-		res = i2c_smbus_xfer_emulated(adapter, addr, flags, read_write,
-					      command, protocol, data);
 
-	return res;
+		if (res != -EOPNOTSUPP || !adapter->algo->master_xfer)
+			return res;
+		/*
+		 * Fall back to i2c_smbus_xfer_emulated if the adapter doesn't
+		 * implement native support for the SMBus operation.
+		 */
+	}
+
+	return i2c_smbus_xfer_emulated(adapter, addr, flags, read_write,
+				       command, protocol, data);
 }
 EXPORT_SYMBOL(i2c_smbus_xfer);
 
diff --git a/drivers/i2c/i2c-smbus.c b/drivers/i2c/i2c-smbus.c
index 9836d08f7a7..df3e0bf31eb 100644
--- a/drivers/i2c/i2c-smbus.c
+++ b/drivers/i2c/i2c-smbus.c
@@ -245,18 +245,7 @@ int i2c_handle_smbus_alert(struct i2c_client *ara)
 }
 EXPORT_SYMBOL_GPL(i2c_handle_smbus_alert);
 
-static int __init i2c_smbus_init(void)
-{
-	return i2c_add_driver(&smbalert_driver);
-}
-
-static void __exit i2c_smbus_exit(void)
-{
-	i2c_del_driver(&smbalert_driver);
-}
-
-module_init(i2c_smbus_init);
-module_exit(i2c_smbus_exit);
+module_i2c_driver(smbalert_driver);
 
 MODULE_AUTHOR("Jean Delvare <khali@linux-fr.org>");
 MODULE_DESCRIPTION("SMBus protocol extensions support");
diff --git a/drivers/i2c/muxes/i2c-mux-pca9541.c b/drivers/i2c/muxes/i2c-mux-pca9541.c
index 8aacde1516a..f8f72f39e0b 100644
--- a/drivers/i2c/muxes/i2c-mux-pca9541.c
+++ b/drivers/i2c/muxes/i2c-mux-pca9541.c
@@ -396,6 +396,6 @@ static struct i2c_driver pca9541_driver = {
 
 module_i2c_driver(pca9541_driver);
 
-MODULE_AUTHOR("Guenter Roeck <guenter.roeck@ericsson.com>");
+MODULE_AUTHOR("Guenter Roeck <linux@roeck-us.net>");
 MODULE_DESCRIPTION("PCA9541 I2C master selector driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index d0f59c3f87e..f559088869f 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -96,6 +96,7 @@ static const struct idle_cpu *icpu;
 static struct cpuidle_device __percpu *intel_idle_cpuidle_devices;
 static int intel_idle(struct cpuidle_device *dev,
 			struct cpuidle_driver *drv, int index);
+static int intel_idle_cpu_init(int cpu);
 
 static struct cpuidle_state *cpuidle_state_table;
 
@@ -169,6 +170,38 @@ static struct cpuidle_state snb_cstates[MWAIT_MAX_NUM_CSTATES] = {
 		.enter = &intel_idle },
 };
 
+static struct cpuidle_state ivb_cstates[MWAIT_MAX_NUM_CSTATES] = {
+	{ /* MWAIT C0 */ },
+	{ /* MWAIT C1 */
+		.name = "C1-IVB",
+		.desc = "MWAIT 0x00",
+		.flags = CPUIDLE_FLAG_TIME_VALID,
+		.exit_latency = 1,
+		.target_residency = 1,
+		.enter = &intel_idle },
+	{ /* MWAIT C2 */
+		.name = "C3-IVB",
+		.desc = "MWAIT 0x10",
+		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 59,
+		.target_residency = 156,
+		.enter = &intel_idle },
+	{ /* MWAIT C3 */
+		.name = "C6-IVB",
+		.desc = "MWAIT 0x20",
+		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 80,
+		.target_residency = 300,
+		.enter = &intel_idle },
+	{ /* MWAIT C4 */
+		.name = "C7-IVB",
+		.desc = "MWAIT 0x30",
+		.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
+		.exit_latency = 87,
+		.target_residency = 300,
+		.enter = &intel_idle },
+};
+
 static struct cpuidle_state atom_cstates[MWAIT_MAX_NUM_CSTATES] = {
 	{ /* MWAIT C0 */ },
 	{ /* MWAIT C1 */
@@ -302,22 +335,35 @@ static void __setup_broadcast_timer(void *arg)
 	clockevents_notify(reason, &cpu);
 }
 
-static int setup_broadcast_cpuhp_notify(struct notifier_block *n,
-		unsigned long action, void *hcpu)
+static int cpu_hotplug_notify(struct notifier_block *n,
+			      unsigned long action, void *hcpu)
 {
 	int hotcpu = (unsigned long)hcpu;
+	struct cpuidle_device *dev;
 
 	switch (action & 0xf) {
 	case CPU_ONLINE:
-		smp_call_function_single(hotcpu, __setup_broadcast_timer,
-			(void *)true, 1);
+
+		if (lapic_timer_reliable_states != LAPIC_TIMER_ALWAYS_RELIABLE)
+			smp_call_function_single(hotcpu, __setup_broadcast_timer,
+						 (void *)true, 1);
+
+		/*
+		 * Some systems can hotplug a cpu at runtime after
+		 * the kernel has booted, we have to initialize the
+		 * driver in this case
+		 */
+		dev = per_cpu_ptr(intel_idle_cpuidle_devices, hotcpu);
+		if (!dev->registered)
+			intel_idle_cpu_init(hotcpu);
+
 		break;
 	}
 	return NOTIFY_OK;
 }
 
-static struct notifier_block setup_broadcast_notifier = {
-	.notifier_call = setup_broadcast_cpuhp_notify,
+static struct notifier_block cpu_hotplug_notifier = {
+	.notifier_call = cpu_hotplug_notify,
 };
 
 static void auto_demotion_disable(void *dummy)
@@ -347,6 +393,10 @@ static const struct idle_cpu idle_cpu_snb = {
 	.state_table = snb_cstates,
 };
 
+static const struct idle_cpu idle_cpu_ivb = {
+	.state_table = ivb_cstates,
+};
+
 #define ICPU(model, cpu) \
 	{ X86_VENDOR_INTEL, 6, model, X86_FEATURE_MWAIT, (unsigned long)&cpu }
 
@@ -362,6 +412,7 @@ static const struct x86_cpu_id intel_idle_ids[] = {
 	ICPU(0x2f, idle_cpu_nehalem),
 	ICPU(0x2a, idle_cpu_snb),
 	ICPU(0x2d, idle_cpu_snb),
+	ICPU(0x3a, idle_cpu_ivb),
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_idle_ids);
@@ -405,10 +456,10 @@ static int intel_idle_probe(void)
 
 	if (boot_cpu_has(X86_FEATURE_ARAT))	/* Always Reliable APIC Timer */
 		lapic_timer_reliable_states = LAPIC_TIMER_ALWAYS_RELIABLE;
-	else {
+	else
 		on_each_cpu(__setup_broadcast_timer, (void *)true, 1);
-		register_cpu_notifier(&setup_broadcast_notifier);
-	}
+
+	register_cpu_notifier(&cpu_hotplug_notifier);
 
 	pr_debug(PREFIX "v" INTEL_IDLE_VERSION
 		" model 0x%X\n", boot_cpu_data.x86_model);
@@ -494,7 +545,7 @@ static int intel_idle_cpuidle_driver_init(void)
  * allocate, initialize, register cpuidle_devices
  * @cpu: cpu/core to initialize
  */
-int intel_idle_cpu_init(int cpu)
+static int intel_idle_cpu_init(int cpu)
 {
 	int cstate;
 	struct cpuidle_device *dev;
@@ -539,7 +590,6 @@ int intel_idle_cpu_init(int cpu)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(intel_idle_cpu_init);
 
 static int __init intel_idle_init(void)
 {
@@ -581,10 +631,10 @@ static void __exit intel_idle_exit(void)
 	intel_idle_cpuidle_devices_uninit();
 	cpuidle_unregister_driver(&intel_idle_driver);
 
-	if (lapic_timer_reliable_states != LAPIC_TIMER_ALWAYS_RELIABLE) {
+
+	if (lapic_timer_reliable_states != LAPIC_TIMER_ALWAYS_RELIABLE)
 		on_each_cpu(__setup_broadcast_timer, (void *)false, 1);
-		unregister_cpu_notifier(&setup_broadcast_notifier);
-	}
+	unregister_cpu_notifier(&cpu_hotplug_notifier);
 
 	return;
 }
diff --git a/drivers/ieee802154/Kconfig b/drivers/ieee802154/Kconfig
index 15c06407370..1fc4eefc20e 100644
--- a/drivers/ieee802154/Kconfig
+++ b/drivers/ieee802154/Kconfig
@@ -19,6 +19,7 @@ config IEEE802154_FAKEHARD
 
           This driver can also be built as a module. To do so say M here.
 	  The module will be called 'fakehard'.
+
 config IEEE802154_FAKELB
 	depends on IEEE802154_DRIVERS && MAC802154
 	tristate "IEEE 802.15.4 loopback driver"
@@ -28,3 +29,8 @@ config IEEE802154_FAKELB
 
 	  This driver can also be built as a module. To do so say M here.
 	  The module will be called 'fakelb'.
+
+config IEEE802154_AT86RF230
+        depends on IEEE802154_DRIVERS && MAC802154
+        tristate "AT86RF230/231 transceiver driver"
+        depends on SPI
diff --git a/drivers/ieee802154/Makefile b/drivers/ieee802154/Makefile
index ea784ea6f0f..4f4371d3aa7 100644
--- a/drivers/ieee802154/Makefile
+++ b/drivers/ieee802154/Makefile
@@ -1,2 +1,3 @@
 obj-$(CONFIG_IEEE802154_FAKEHARD) += fakehard.o
 obj-$(CONFIG_IEEE802154_FAKELB) += fakelb.o
+obj-$(CONFIG_IEEE802154_AT86RF230) += at86rf230.o
diff --git a/drivers/ieee802154/at86rf230.c b/drivers/ieee802154/at86rf230.c
new file mode 100644
index 00000000000..5d309408395
--- /dev/null
+++ b/drivers/ieee802154/at86rf230.c
@@ -0,0 +1,968 @@
+/*
+ * AT86RF230/RF231 driver
+ *
+ * Copyright (C) 2009-2012 Siemens AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ * Written by:
+ * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
+ * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/at86rf230.h>
+#include <linux/skbuff.h>
+
+#include <net/mac802154.h>
+#include <net/wpan-phy.h>
+
+struct at86rf230_local {
+	struct spi_device *spi;
+	int rstn, slp_tr, dig2;
+
+	u8 part;
+	u8 vers;
+
+	u8 buf[2];
+	struct mutex bmux;
+
+	struct work_struct irqwork;
+	struct completion tx_complete;
+
+	struct ieee802154_dev *dev;
+
+	spinlock_t lock;
+	bool irq_disabled;
+	bool is_tx;
+};
+
+#define	RG_TRX_STATUS	(0x01)
+#define	SR_TRX_STATUS		0x01, 0x1f, 0
+#define	SR_RESERVED_01_3	0x01, 0x20, 5
+#define	SR_CCA_STATUS		0x01, 0x40, 6
+#define	SR_CCA_DONE		0x01, 0x80, 7
+#define	RG_TRX_STATE	(0x02)
+#define	SR_TRX_CMD		0x02, 0x1f, 0
+#define	SR_TRAC_STATUS		0x02, 0xe0, 5
+#define	RG_TRX_CTRL_0	(0x03)
+#define	SR_CLKM_CTRL		0x03, 0x07, 0
+#define	SR_CLKM_SHA_SEL		0x03, 0x08, 3
+#define	SR_PAD_IO_CLKM		0x03, 0x30, 4
+#define	SR_PAD_IO		0x03, 0xc0, 6
+#define	RG_TRX_CTRL_1	(0x04)
+#define	SR_IRQ_POLARITY		0x04, 0x01, 0
+#define	SR_IRQ_MASK_MODE	0x04, 0x02, 1
+#define	SR_SPI_CMD_MODE		0x04, 0x0c, 2
+#define	SR_RX_BL_CTRL		0x04, 0x10, 4
+#define	SR_TX_AUTO_CRC_ON	0x04, 0x20, 5
+#define	SR_IRQ_2_EXT_EN		0x04, 0x40, 6
+#define	SR_PA_EXT_EN		0x04, 0x80, 7
+#define	RG_PHY_TX_PWR	(0x05)
+#define	SR_TX_PWR		0x05, 0x0f, 0
+#define	SR_PA_LT		0x05, 0x30, 4
+#define	SR_PA_BUF_LT		0x05, 0xc0, 6
+#define	RG_PHY_RSSI	(0x06)
+#define	SR_RSSI			0x06, 0x1f, 0
+#define	SR_RND_VALUE		0x06, 0x60, 5
+#define	SR_RX_CRC_VALID		0x06, 0x80, 7
+#define	RG_PHY_ED_LEVEL	(0x07)
+#define	SR_ED_LEVEL		0x07, 0xff, 0
+#define	RG_PHY_CC_CCA	(0x08)
+#define	SR_CHANNEL		0x08, 0x1f, 0
+#define	SR_CCA_MODE		0x08, 0x60, 5
+#define	SR_CCA_REQUEST		0x08, 0x80, 7
+#define	RG_CCA_THRES	(0x09)
+#define	SR_CCA_ED_THRES		0x09, 0x0f, 0
+#define	SR_RESERVED_09_1	0x09, 0xf0, 4
+#define	RG_RX_CTRL	(0x0a)
+#define	SR_PDT_THRES		0x0a, 0x0f, 0
+#define	SR_RESERVED_0a_1	0x0a, 0xf0, 4
+#define	RG_SFD_VALUE	(0x0b)
+#define	SR_SFD_VALUE		0x0b, 0xff, 0
+#define	RG_TRX_CTRL_2	(0x0c)
+#define	SR_OQPSK_DATA_RATE	0x0c, 0x03, 0
+#define	SR_RESERVED_0c_2	0x0c, 0x7c, 2
+#define	SR_RX_SAFE_MODE		0x0c, 0x80, 7
+#define	RG_ANT_DIV	(0x0d)
+#define	SR_ANT_CTRL		0x0d, 0x03, 0
+#define	SR_ANT_EXT_SW_EN	0x0d, 0x04, 2
+#define	SR_ANT_DIV_EN		0x0d, 0x08, 3
+#define	SR_RESERVED_0d_2	0x0d, 0x70, 4
+#define	SR_ANT_SEL		0x0d, 0x80, 7
+#define	RG_IRQ_MASK	(0x0e)
+#define	SR_IRQ_MASK		0x0e, 0xff, 0
+#define	RG_IRQ_STATUS	(0x0f)
+#define	SR_IRQ_0_PLL_LOCK	0x0f, 0x01, 0
+#define	SR_IRQ_1_PLL_UNLOCK	0x0f, 0x02, 1
+#define	SR_IRQ_2_RX_START	0x0f, 0x04, 2
+#define	SR_IRQ_3_TRX_END	0x0f, 0x08, 3
+#define	SR_IRQ_4_CCA_ED_DONE	0x0f, 0x10, 4
+#define	SR_IRQ_5_AMI		0x0f, 0x20, 5
+#define	SR_IRQ_6_TRX_UR		0x0f, 0x40, 6
+#define	SR_IRQ_7_BAT_LOW	0x0f, 0x80, 7
+#define	RG_VREG_CTRL	(0x10)
+#define	SR_RESERVED_10_6	0x10, 0x03, 0
+#define	SR_DVDD_OK		0x10, 0x04, 2
+#define	SR_DVREG_EXT		0x10, 0x08, 3
+#define	SR_RESERVED_10_3	0x10, 0x30, 4
+#define	SR_AVDD_OK		0x10, 0x40, 6
+#define	SR_AVREG_EXT		0x10, 0x80, 7
+#define	RG_BATMON	(0x11)
+#define	SR_BATMON_VTH		0x11, 0x0f, 0
+#define	SR_BATMON_HR		0x11, 0x10, 4
+#define	SR_BATMON_OK		0x11, 0x20, 5
+#define	SR_RESERVED_11_1	0x11, 0xc0, 6
+#define	RG_XOSC_CTRL	(0x12)
+#define	SR_XTAL_TRIM		0x12, 0x0f, 0
+#define	SR_XTAL_MODE		0x12, 0xf0, 4
+#define	RG_RX_SYN	(0x15)
+#define	SR_RX_PDT_LEVEL		0x15, 0x0f, 0
+#define	SR_RESERVED_15_2	0x15, 0x70, 4
+#define	SR_RX_PDT_DIS		0x15, 0x80, 7
+#define	RG_XAH_CTRL_1	(0x17)
+#define	SR_RESERVED_17_8	0x17, 0x01, 0
+#define	SR_AACK_PROM_MODE	0x17, 0x02, 1
+#define	SR_AACK_ACK_TIME	0x17, 0x04, 2
+#define	SR_RESERVED_17_5	0x17, 0x08, 3
+#define	SR_AACK_UPLD_RES_FT	0x17, 0x10, 4
+#define	SR_AACK_FLTR_RES_FT	0x17, 0x20, 5
+#define	SR_RESERVED_17_2	0x17, 0x40, 6
+#define	SR_RESERVED_17_1	0x17, 0x80, 7
+#define	RG_FTN_CTRL	(0x18)
+#define	SR_RESERVED_18_2	0x18, 0x7f, 0
+#define	SR_FTN_START		0x18, 0x80, 7
+#define	RG_PLL_CF	(0x1a)
+#define	SR_RESERVED_1a_2	0x1a, 0x7f, 0
+#define	SR_PLL_CF_START		0x1a, 0x80, 7
+#define	RG_PLL_DCU	(0x1b)
+#define	SR_RESERVED_1b_3	0x1b, 0x3f, 0
+#define	SR_RESERVED_1b_2	0x1b, 0x40, 6
+#define	SR_PLL_DCU_START	0x1b, 0x80, 7
+#define	RG_PART_NUM	(0x1c)
+#define	SR_PART_NUM		0x1c, 0xff, 0
+#define	RG_VERSION_NUM	(0x1d)
+#define	SR_VERSION_NUM		0x1d, 0xff, 0
+#define	RG_MAN_ID_0	(0x1e)
+#define	SR_MAN_ID_0		0x1e, 0xff, 0
+#define	RG_MAN_ID_1	(0x1f)
+#define	SR_MAN_ID_1		0x1f, 0xff, 0
+#define	RG_SHORT_ADDR_0	(0x20)
+#define	SR_SHORT_ADDR_0		0x20, 0xff, 0
+#define	RG_SHORT_ADDR_1	(0x21)
+#define	SR_SHORT_ADDR_1		0x21, 0xff, 0
+#define	RG_PAN_ID_0	(0x22)
+#define	SR_PAN_ID_0		0x22, 0xff, 0
+#define	RG_PAN_ID_1	(0x23)
+#define	SR_PAN_ID_1		0x23, 0xff, 0
+#define	RG_IEEE_ADDR_0	(0x24)
+#define	SR_IEEE_ADDR_0		0x24, 0xff, 0
+#define	RG_IEEE_ADDR_1	(0x25)
+#define	SR_IEEE_ADDR_1		0x25, 0xff, 0
+#define	RG_IEEE_ADDR_2	(0x26)
+#define	SR_IEEE_ADDR_2		0x26, 0xff, 0
+#define	RG_IEEE_ADDR_3	(0x27)
+#define	SR_IEEE_ADDR_3		0x27, 0xff, 0
+#define	RG_IEEE_ADDR_4	(0x28)
+#define	SR_IEEE_ADDR_4		0x28, 0xff, 0
+#define	RG_IEEE_ADDR_5	(0x29)
+#define	SR_IEEE_ADDR_5		0x29, 0xff, 0
+#define	RG_IEEE_ADDR_6	(0x2a)
+#define	SR_IEEE_ADDR_6		0x2a, 0xff, 0
+#define	RG_IEEE_ADDR_7	(0x2b)
+#define	SR_IEEE_ADDR_7		0x2b, 0xff, 0
+#define	RG_XAH_CTRL_0	(0x2c)
+#define	SR_SLOTTED_OPERATION	0x2c, 0x01, 0
+#define	SR_MAX_CSMA_RETRIES	0x2c, 0x0e, 1
+#define	SR_MAX_FRAME_RETRIES	0x2c, 0xf0, 4
+#define	RG_CSMA_SEED_0	(0x2d)
+#define	SR_CSMA_SEED_0		0x2d, 0xff, 0
+#define	RG_CSMA_SEED_1	(0x2e)
+#define	SR_CSMA_SEED_1		0x2e, 0x07, 0
+#define	SR_AACK_I_AM_COORD	0x2e, 0x08, 3
+#define	SR_AACK_DIS_ACK		0x2e, 0x10, 4
+#define	SR_AACK_SET_PD		0x2e, 0x20, 5
+#define	SR_AACK_FVN_MODE	0x2e, 0xc0, 6
+#define	RG_CSMA_BE	(0x2f)
+#define	SR_MIN_BE		0x2f, 0x0f, 0
+#define	SR_MAX_BE		0x2f, 0xf0, 4
+
+#define CMD_REG		0x80
+#define CMD_REG_MASK	0x3f
+#define CMD_WRITE	0x40
+#define CMD_FB		0x20
+
+#define IRQ_BAT_LOW	(1 << 7)
+#define IRQ_TRX_UR	(1 << 6)
+#define IRQ_AMI		(1 << 5)
+#define IRQ_CCA_ED	(1 << 4)
+#define IRQ_TRX_END	(1 << 3)
+#define IRQ_RX_START	(1 << 2)
+#define IRQ_PLL_UNL	(1 << 1)
+#define IRQ_PLL_LOCK	(1 << 0)
+
+#define STATE_P_ON		0x00	/* BUSY */
+#define STATE_BUSY_RX		0x01
+#define STATE_BUSY_TX		0x02
+#define STATE_FORCE_TRX_OFF	0x03
+#define STATE_FORCE_TX_ON	0x04	/* IDLE */
+/* 0x05 */				/* INVALID_PARAMETER */
+#define STATE_RX_ON		0x06
+/* 0x07 */				/* SUCCESS */
+#define STATE_TRX_OFF		0x08
+#define STATE_TX_ON		0x09
+/* 0x0a - 0x0e */			/* 0x0a - UNSUPPORTED_ATTRIBUTE */
+#define STATE_SLEEP		0x0F
+#define STATE_BUSY_RX_AACK	0x11
+#define STATE_BUSY_TX_ARET	0x12
+#define STATE_BUSY_RX_AACK_ON	0x16
+#define STATE_BUSY_TX_ARET_ON	0x19
+#define STATE_RX_ON_NOCLK	0x1C
+#define STATE_RX_AACK_ON_NOCLK	0x1D
+#define STATE_BUSY_RX_AACK_NOCLK 0x1E
+#define STATE_TRANSITION_IN_PROGRESS 0x1F
+
+static int
+__at86rf230_write(struct at86rf230_local *lp, u8 addr, u8 data)
+{
+	u8 *buf = lp->buf;
+	int status;
+	struct spi_message msg;
+	struct spi_transfer xfer = {
+		.len	= 2,
+		.tx_buf	= buf,
+	};
+
+	buf[0] = (addr & CMD_REG_MASK) | CMD_REG | CMD_WRITE;
+	buf[1] = data;
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	dev_vdbg(&lp->spi->dev, "buf[1] = %02x\n", buf[1]);
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	status = spi_sync(lp->spi, &msg);
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	if (msg.status)
+		status = msg.status;
+
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	dev_vdbg(&lp->spi->dev, "buf[1] = %02x\n", buf[1]);
+
+	return status;
+}
+
+static int
+__at86rf230_read_subreg(struct at86rf230_local *lp,
+			u8 addr, u8 mask, int shift, u8 *data)
+{
+	u8 *buf = lp->buf;
+	int status;
+	struct spi_message msg;
+	struct spi_transfer xfer = {
+		.len	= 2,
+		.tx_buf	= buf,
+		.rx_buf	= buf,
+	};
+
+	buf[0] = (addr & CMD_REG_MASK) | CMD_REG;
+	buf[1] = 0xff;
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	status = spi_sync(lp->spi, &msg);
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	if (msg.status)
+		status = msg.status;
+
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	dev_vdbg(&lp->spi->dev, "buf[1] = %02x\n", buf[1]);
+
+	if (status == 0)
+		*data = buf[1];
+
+	return status;
+}
+
+static int
+at86rf230_read_subreg(struct at86rf230_local *lp,
+		      u8 addr, u8 mask, int shift, u8 *data)
+{
+	int status;
+
+	mutex_lock(&lp->bmux);
+	status = __at86rf230_read_subreg(lp, addr, mask, shift, data);
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int
+at86rf230_write_subreg(struct at86rf230_local *lp,
+		       u8 addr, u8 mask, int shift, u8 data)
+{
+	int status;
+	u8 val;
+
+	mutex_lock(&lp->bmux);
+	status = __at86rf230_read_subreg(lp, addr, 0xff, 0, &val);
+	if (status)
+		goto out;
+
+	val &= ~mask;
+	val |= (data << shift) & mask;
+
+	status = __at86rf230_write(lp, addr, val);
+out:
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int
+at86rf230_write_fbuf(struct at86rf230_local *lp, u8 *data, u8 len)
+{
+	u8 *buf = lp->buf;
+	int status;
+	struct spi_message msg;
+	struct spi_transfer xfer_head = {
+		.len		= 2,
+		.tx_buf		= buf,
+
+	};
+	struct spi_transfer xfer_buf = {
+		.len		= len,
+		.tx_buf		= data,
+	};
+
+	mutex_lock(&lp->bmux);
+	buf[0] = CMD_WRITE | CMD_FB;
+	buf[1] = len + 2; /* 2 bytes for CRC that isn't written */
+
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	dev_vdbg(&lp->spi->dev, "buf[1] = %02x\n", buf[1]);
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer_head, &msg);
+	spi_message_add_tail(&xfer_buf, &msg);
+
+	status = spi_sync(lp->spi, &msg);
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	if (msg.status)
+		status = msg.status;
+
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	dev_vdbg(&lp->spi->dev, "buf[1] = %02x\n", buf[1]);
+
+	mutex_unlock(&lp->bmux);
+	return status;
+}
+
+static int
+at86rf230_read_fbuf(struct at86rf230_local *lp, u8 *data, u8 *len, u8 *lqi)
+{
+	u8 *buf = lp->buf;
+	int status;
+	struct spi_message msg;
+	struct spi_transfer xfer_head = {
+		.len		= 2,
+		.tx_buf		= buf,
+		.rx_buf		= buf,
+	};
+	struct spi_transfer xfer_head1 = {
+		.len		= 2,
+		.tx_buf		= buf,
+		.rx_buf		= buf,
+	};
+	struct spi_transfer xfer_buf = {
+		.len		= 0,
+		.rx_buf		= data,
+	};
+
+	mutex_lock(&lp->bmux);
+
+	buf[0] = CMD_FB;
+	buf[1] = 0x00;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer_head, &msg);
+
+	status = spi_sync(lp->spi, &msg);
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+
+	xfer_buf.len = *(buf + 1) + 1;
+	*len = buf[1];
+
+	buf[0] = CMD_FB;
+	buf[1] = 0x00;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer_head1, &msg);
+	spi_message_add_tail(&xfer_buf, &msg);
+
+	status = spi_sync(lp->spi, &msg);
+
+	if (msg.status)
+		status = msg.status;
+
+	dev_vdbg(&lp->spi->dev, "status = %d\n", status);
+	dev_vdbg(&lp->spi->dev, "buf[0] = %02x\n", buf[0]);
+	dev_vdbg(&lp->spi->dev, "buf[1] = %02x\n", buf[1]);
+
+	if (status) {
+		if (lqi && (*len > lp->buf[1]))
+			*lqi = data[lp->buf[1]];
+	}
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int
+at86rf230_ed(struct ieee802154_dev *dev, u8 *level)
+{
+	might_sleep();
+	BUG_ON(!level);
+	*level = 0xbe;
+	return 0;
+}
+
+static int
+at86rf230_state(struct ieee802154_dev *dev, int state)
+{
+	struct at86rf230_local *lp = dev->priv;
+	int rc;
+	u8 val;
+	u8 desired_status;
+
+	might_sleep();
+
+	if (state == STATE_FORCE_TX_ON)
+		desired_status = STATE_TX_ON;
+	else if (state == STATE_FORCE_TRX_OFF)
+		desired_status = STATE_TRX_OFF;
+	else
+		desired_status = state;
+
+	do {
+		rc = at86rf230_read_subreg(lp, SR_TRX_STATUS, &val);
+		if (rc)
+			goto err;
+	} while (val == STATE_TRANSITION_IN_PROGRESS);
+
+	if (val == desired_status)
+		return 0;
+
+	/* state is equal to phy states */
+	rc = at86rf230_write_subreg(lp, SR_TRX_CMD, state);
+	if (rc)
+		goto err;
+
+	do {
+		rc = at86rf230_read_subreg(lp, SR_TRX_STATUS, &val);
+		if (rc)
+			goto err;
+	} while (val == STATE_TRANSITION_IN_PROGRESS);
+
+
+	if (val == desired_status)
+		return 0;
+
+	pr_err("unexpected state change: %d, asked for %d\n", val, state);
+	return -EBUSY;
+
+err:
+	pr_err("error: %d\n", rc);
+	return rc;
+}
+
+static int
+at86rf230_start(struct ieee802154_dev *dev)
+{
+	struct at86rf230_local *lp = dev->priv;
+	u8 rc;
+
+	rc = at86rf230_write_subreg(lp, SR_RX_SAFE_MODE, 1);
+	if (rc)
+		return rc;
+
+	return at86rf230_state(dev, STATE_RX_ON);
+}
+
+static void
+at86rf230_stop(struct ieee802154_dev *dev)
+{
+	at86rf230_state(dev, STATE_FORCE_TRX_OFF);
+}
+
+static int
+at86rf230_channel(struct ieee802154_dev *dev, int page, int channel)
+{
+	struct at86rf230_local *lp = dev->priv;
+	int rc;
+
+	might_sleep();
+
+	if (page != 0 || channel < 11 || channel > 26) {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	rc = at86rf230_write_subreg(lp, SR_CHANNEL, channel);
+	msleep(1); /* Wait for PLL */
+	dev->phy->current_channel = channel;
+
+	return 0;
+}
+
+static int
+at86rf230_xmit(struct ieee802154_dev *dev, struct sk_buff *skb)
+{
+	struct at86rf230_local *lp = dev->priv;
+	int rc;
+	unsigned long flags;
+
+	spin_lock(&lp->lock);
+	if  (lp->irq_disabled) {
+		spin_unlock(&lp->lock);
+		return -EBUSY;
+	}
+	spin_unlock(&lp->lock);
+
+	might_sleep();
+
+	rc = at86rf230_state(dev, STATE_FORCE_TX_ON);
+	if (rc)
+		goto err;
+
+	spin_lock_irqsave(&lp->lock, flags);
+	lp->is_tx = 1;
+	INIT_COMPLETION(lp->tx_complete);
+	spin_unlock_irqrestore(&lp->lock, flags);
+
+	rc = at86rf230_write_fbuf(lp, skb->data, skb->len);
+	if (rc)
+		goto err_rx;
+
+	rc = at86rf230_write_subreg(lp, SR_TRX_CMD, STATE_BUSY_TX);
+	if (rc)
+		goto err_rx;
+
+	rc = wait_for_completion_interruptible(&lp->tx_complete);
+	if (rc < 0)
+		goto err_rx;
+
+	rc = at86rf230_start(dev);
+
+	return rc;
+
+err_rx:
+	at86rf230_start(dev);
+err:
+	pr_err("error: %d\n", rc);
+
+	spin_lock_irqsave(&lp->lock, flags);
+	lp->is_tx = 0;
+	spin_unlock_irqrestore(&lp->lock, flags);
+
+	return rc;
+}
+
+static int at86rf230_rx(struct at86rf230_local *lp)
+{
+	u8 len = 128, lqi = 0;
+	struct sk_buff *skb;
+
+	skb = alloc_skb(len, GFP_KERNEL);
+
+	if (!skb)
+		return -ENOMEM;
+
+	if (at86rf230_read_fbuf(lp, skb_put(skb, len), &len, &lqi))
+		goto err;
+
+	if (len < 2)
+		goto err;
+
+	skb_trim(skb, len - 2); /* We do not put CRC into the frame */
+
+	ieee802154_rx_irqsafe(lp->dev, skb, lqi);
+
+	dev_dbg(&lp->spi->dev, "READ_FBUF: %d %x\n", len, lqi);
+
+	return 0;
+err:
+	pr_debug("received frame is too small\n");
+
+	kfree_skb(skb);
+	return -EINVAL;
+}
+
+static struct ieee802154_ops at86rf230_ops = {
+	.owner = THIS_MODULE,
+	.xmit = at86rf230_xmit,
+	.ed = at86rf230_ed,
+	.set_channel = at86rf230_channel,
+	.start = at86rf230_start,
+	.stop = at86rf230_stop,
+};
+
+static void at86rf230_irqwork(struct work_struct *work)
+{
+	struct at86rf230_local *lp =
+		container_of(work, struct at86rf230_local, irqwork);
+	u8 status = 0, val;
+	int rc;
+	unsigned long flags;
+
+	rc = at86rf230_read_subreg(lp, RG_IRQ_STATUS, 0xff, 0, &val);
+	status |= val;
+
+	status &= ~IRQ_PLL_LOCK; /* ignore */
+	status &= ~IRQ_RX_START; /* ignore */
+	status &= ~IRQ_AMI; /* ignore */
+	status &= ~IRQ_TRX_UR; /* FIXME: possibly handle ???*/
+
+	if (status & IRQ_TRX_END) {
+		spin_lock_irqsave(&lp->lock, flags);
+		status &= ~IRQ_TRX_END;
+		if (lp->is_tx) {
+			lp->is_tx = 0;
+			spin_unlock_irqrestore(&lp->lock, flags);
+			complete(&lp->tx_complete);
+		} else {
+			spin_unlock_irqrestore(&lp->lock, flags);
+			at86rf230_rx(lp);
+		}
+	}
+
+	spin_lock_irqsave(&lp->lock, flags);
+	lp->irq_disabled = 0;
+	spin_unlock_irqrestore(&lp->lock, flags);
+
+	enable_irq(lp->spi->irq);
+}
+
+static irqreturn_t at86rf230_isr(int irq, void *data)
+{
+	struct at86rf230_local *lp = data;
+
+	disable_irq_nosync(irq);
+
+	spin_lock(&lp->lock);
+	lp->irq_disabled = 1;
+	spin_unlock(&lp->lock);
+
+	schedule_work(&lp->irqwork);
+
+	return IRQ_HANDLED;
+}
+
+
+static int at86rf230_hw_init(struct at86rf230_local *lp)
+{
+	u8 status;
+	int rc;
+
+	rc = at86rf230_read_subreg(lp, SR_TRX_STATUS, &status);
+	if (rc)
+		return rc;
+
+	dev_info(&lp->spi->dev, "Status: %02x\n", status);
+	if (status == STATE_P_ON) {
+		rc = at86rf230_write_subreg(lp, SR_TRX_CMD, STATE_TRX_OFF);
+		if (rc)
+			return rc;
+		msleep(1);
+		rc = at86rf230_read_subreg(lp, SR_TRX_STATUS, &status);
+		if (rc)
+			return rc;
+		dev_info(&lp->spi->dev, "Status: %02x\n", status);
+	}
+
+	rc = at86rf230_write_subreg(lp, SR_IRQ_MASK, 0xff); /* IRQ_TRX_UR |
+							     * IRQ_CCA_ED |
+							     * IRQ_TRX_END |
+							     * IRQ_PLL_UNL |
+							     * IRQ_PLL_LOCK
+							     */
+	if (rc)
+		return rc;
+
+	/* CLKM changes are applied immediately */
+	rc = at86rf230_write_subreg(lp, SR_CLKM_SHA_SEL, 0x00);
+	if (rc)
+		return rc;
+
+	/* Turn CLKM Off */
+	rc = at86rf230_write_subreg(lp, SR_CLKM_CTRL, 0x00);
+	if (rc)
+		return rc;
+	/* Wait the next SLEEP cycle */
+	msleep(100);
+
+	rc = at86rf230_write_subreg(lp, SR_TRX_CMD, STATE_TX_ON);
+	if (rc)
+		return rc;
+	msleep(1);
+
+	rc = at86rf230_read_subreg(lp, SR_TRX_STATUS, &status);
+	if (rc)
+		return rc;
+	dev_info(&lp->spi->dev, "Status: %02x\n", status);
+
+	rc = at86rf230_read_subreg(lp, SR_DVDD_OK, &status);
+	if (rc)
+		return rc;
+	if (!status) {
+		dev_err(&lp->spi->dev, "DVDD error\n");
+		return -EINVAL;
+	}
+
+	rc = at86rf230_read_subreg(lp, SR_AVDD_OK, &status);
+	if (rc)
+		return rc;
+	if (!status) {
+		dev_err(&lp->spi->dev, "AVDD error\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int at86rf230_suspend(struct spi_device *spi, pm_message_t message)
+{
+	return 0;
+}
+
+static int at86rf230_resume(struct spi_device *spi)
+{
+	return 0;
+}
+
+static int at86rf230_fill_data(struct spi_device *spi)
+{
+	struct at86rf230_local *lp = spi_get_drvdata(spi);
+	struct at86rf230_platform_data *pdata = spi->dev.platform_data;
+
+	if (!pdata) {
+		dev_err(&spi->dev, "no platform_data\n");
+		return -EINVAL;
+	}
+
+	lp->rstn = pdata->rstn;
+	lp->slp_tr = pdata->slp_tr;
+	lp->dig2 = pdata->dig2;
+
+	return 0;
+}
+
+static int __devinit at86rf230_probe(struct spi_device *spi)
+{
+	struct ieee802154_dev *dev;
+	struct at86rf230_local *lp;
+	u8 man_id_0, man_id_1;
+	int rc;
+	const char *chip;
+	int supported = 0;
+
+	if (!spi->irq) {
+		dev_err(&spi->dev, "no IRQ specified\n");
+		return -EINVAL;
+	}
+
+	dev = ieee802154_alloc_device(sizeof(*lp), &at86rf230_ops);
+	if (!dev)
+		return -ENOMEM;
+
+	lp = dev->priv;
+	lp->dev = dev;
+
+	lp->spi = spi;
+
+	dev->priv = lp;
+	dev->parent = &spi->dev;
+	dev->extra_tx_headroom = 0;
+	/* We do support only 2.4 Ghz */
+	dev->phy->channels_supported[0] = 0x7FFF800;
+	dev->flags = IEEE802154_HW_OMIT_CKSUM;
+
+	mutex_init(&lp->bmux);
+	INIT_WORK(&lp->irqwork, at86rf230_irqwork);
+	spin_lock_init(&lp->lock);
+	init_completion(&lp->tx_complete);
+
+	spi_set_drvdata(spi, lp);
+
+	rc = at86rf230_fill_data(spi);
+	if (rc)
+		goto err_fill;
+
+	rc = gpio_request(lp->rstn, "rstn");
+	if (rc)
+		goto err_rstn;
+
+	if (gpio_is_valid(lp->slp_tr)) {
+		rc = gpio_request(lp->slp_tr, "slp_tr");
+		if (rc)
+			goto err_slp_tr;
+	}
+
+	rc = gpio_direction_output(lp->rstn, 1);
+	if (rc)
+		goto err_gpio_dir;
+
+	if (gpio_is_valid(lp->slp_tr)) {
+		rc = gpio_direction_output(lp->slp_tr, 0);
+		if (rc)
+			goto err_gpio_dir;
+	}
+
+	/* Reset */
+	msleep(1);
+	gpio_set_value(lp->rstn, 0);
+	msleep(1);
+	gpio_set_value(lp->rstn, 1);
+	msleep(1);
+
+	rc = at86rf230_read_subreg(lp, SR_MAN_ID_0, &man_id_0);
+	if (rc)
+		goto err_gpio_dir;
+	rc = at86rf230_read_subreg(lp, SR_MAN_ID_1, &man_id_1);
+	if (rc)
+		goto err_gpio_dir;
+
+	if (man_id_1 != 0x00 || man_id_0 != 0x1f) {
+		dev_err(&spi->dev, "Non-Atmel dev found (MAN_ID %02x %02x)\n",
+			man_id_1, man_id_0);
+		rc = -EINVAL;
+		goto err_gpio_dir;
+	}
+
+	rc = at86rf230_read_subreg(lp, SR_PART_NUM, &lp->part);
+	if (rc)
+		goto err_gpio_dir;
+
+	rc = at86rf230_read_subreg(lp, SR_VERSION_NUM, &lp->vers);
+	if (rc)
+		goto err_gpio_dir;
+
+	switch (lp->part) {
+	case 2:
+		chip = "at86rf230";
+		/* supported = 1;  FIXME: should be easy to support; */
+		break;
+	case 3:
+		chip = "at86rf231";
+		supported = 1;
+		break;
+	default:
+		chip = "UNKNOWN";
+		break;
+	}
+
+	dev_info(&spi->dev, "Detected %s chip version %d\n", chip, lp->vers);
+	if (!supported) {
+		rc = -ENOTSUPP;
+		goto err_gpio_dir;
+	}
+
+	rc = at86rf230_hw_init(lp);
+	if (rc)
+		goto err_gpio_dir;
+
+	rc = request_irq(spi->irq, at86rf230_isr, IRQF_SHARED,
+			 dev_name(&spi->dev), lp);
+	if (rc)
+		goto err_gpio_dir;
+
+	rc = ieee802154_register_device(lp->dev);
+	if (rc)
+		goto err_irq;
+
+	return rc;
+
+	ieee802154_unregister_device(lp->dev);
+err_irq:
+	free_irq(spi->irq, lp);
+	flush_work(&lp->irqwork);
+err_gpio_dir:
+	if (gpio_is_valid(lp->slp_tr))
+		gpio_free(lp->slp_tr);
+err_slp_tr:
+	gpio_free(lp->rstn);
+err_rstn:
+err_fill:
+	spi_set_drvdata(spi, NULL);
+	mutex_destroy(&lp->bmux);
+	ieee802154_free_device(lp->dev);
+	return rc;
+}
+
+static int __devexit at86rf230_remove(struct spi_device *spi)
+{
+	struct at86rf230_local *lp = spi_get_drvdata(spi);
+
+	ieee802154_unregister_device(lp->dev);
+
+	free_irq(spi->irq, lp);
+	flush_work(&lp->irqwork);
+
+	if (gpio_is_valid(lp->slp_tr))
+		gpio_free(lp->slp_tr);
+	gpio_free(lp->rstn);
+
+	spi_set_drvdata(spi, NULL);
+	mutex_destroy(&lp->bmux);
+	ieee802154_free_device(lp->dev);
+
+	dev_dbg(&spi->dev, "unregistered at86rf230\n");
+	return 0;
+}
+
+static struct spi_driver at86rf230_driver = {
+	.driver = {
+		.name	= "at86rf230",
+		.owner	= THIS_MODULE,
+	},
+	.probe      = at86rf230_probe,
+	.remove     = __devexit_p(at86rf230_remove),
+	.suspend    = at86rf230_suspend,
+	.resume     = at86rf230_resume,
+};
+
+static int __init at86rf230_init(void)
+{
+	return spi_register_driver(&at86rf230_driver);
+}
+module_init(at86rf230_init);
+
+static void __exit at86rf230_exit(void)
+{
+	spi_unregister_driver(&at86rf230_driver);
+}
+module_exit(at86rf230_exit);
+
+MODULE_DESCRIPTION("AT86RF230 Transceiver Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index 2ec93da41e2..d4984c8be97 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -29,6 +29,13 @@ config IIO_KFIFO_BUF
 	  no buffer events so it is up to userspace to work out how
 	  often to read from the buffer.
 
+config IIO_TRIGGERED_BUFFER
+	tristate
+	select IIO_TRIGGER
+	select IIO_KFIFO_BUF
+	help
+	  Provides helper functions for setting up triggered buffers.
+
 endif # IIO_BUFFER
 
 config IIO_TRIGGER
@@ -49,5 +56,8 @@ config IIO_CONSUMERS_PER_TRIGGER
 
 source "drivers/iio/adc/Kconfig"
 source "drivers/iio/amplifiers/Kconfig"
+source "drivers/iio/light/Kconfig"
+source "drivers/iio/frequency/Kconfig"
+source "drivers/iio/dac/Kconfig"
 
 endif # IIO
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index e425afd1480..34309abb797 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -7,7 +7,11 @@ industrialio-y := industrialio-core.o industrialio-event.o inkern.o
 industrialio-$(CONFIG_IIO_BUFFER) += industrialio-buffer.o
 industrialio-$(CONFIG_IIO_TRIGGER) += industrialio-trigger.o
 
+obj-$(CONFIG_IIO_TRIGGERED_BUFFER) += industrialio-triggered-buffer.o
 obj-$(CONFIG_IIO_KFIFO_BUF) += kfifo_buf.o
 
 obj-y += adc/
 obj-y += amplifiers/
+obj-y += light/
+obj-y += frequency/
+obj-y += dac/
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 9a0df8123cc..8a78b4f3ef5 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -3,12 +3,21 @@
 #
 menu "Analog to digital converters"
 
+config AD7266
+	tristate "Analog Devices AD7265/AD7266 ADC driver"
+	depends on SPI_MASTER
+	select IIO_BUFFER
+	select IIO_TRIGGER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Analog Devices AD7265 and AD7266
+	  ADCs.
+
 config AT91_ADC
 	tristate "Atmel AT91 ADC"
 	depends on ARCH_AT91
 	select IIO_BUFFER
-	select IIO_KFIFO_BUF
-	select IIO_TRIGGER
+	select IIO_TRIGGERED_BUFFER
 	select SYSFS
 	help
 	  Say yes here to build support for Atmel AT91 ADC.
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 175c8d41ea9..52eec254c38 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -2,4 +2,5 @@
 # Makefile for IIO ADC drivers
 #
 
+obj-$(CONFIG_AD7266) += ad7266.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
diff --git a/drivers/iio/adc/ad7266.c b/drivers/iio/adc/ad7266.c
new file mode 100644
index 00000000000..5c3f1ba5a06
--- /dev/null
+++ b/drivers/iio/adc/ad7266.c
@@ -0,0 +1,536 @@
+/*
+ * AD7266/65 SPI ADC driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+
+#include <linux/interrupt.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#include <linux/platform_data/ad7266.h>
+
+struct ad7266_state {
+	struct spi_device	*spi;
+	struct regulator	*reg;
+	unsigned long		vref_uv;
+
+	struct spi_transfer	single_xfer[3];
+	struct spi_message	single_msg;
+
+	enum ad7266_range	range;
+	enum ad7266_mode	mode;
+	bool			fixed_addr;
+	struct gpio		gpios[3];
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 * The buffer needs to be large enough to hold two samples (4 bytes) and
+	 * the naturally aligned timestamp (8 bytes).
+	 */
+	uint8_t data[ALIGN(4, sizeof(s64)) + sizeof(s64)] ____cacheline_aligned;
+};
+
+static int ad7266_wakeup(struct ad7266_state *st)
+{
+	/* Any read with >= 2 bytes will wake the device */
+	return spi_read(st->spi, st->data, 2);
+}
+
+static int ad7266_powerdown(struct ad7266_state *st)
+{
+	/* Any read with < 2 bytes will powerdown the device */
+	return spi_read(st->spi, st->data, 1);
+}
+
+static int ad7266_preenable(struct iio_dev *indio_dev)
+{
+	struct ad7266_state *st = iio_priv(indio_dev);
+	int ret;
+
+	ret = ad7266_wakeup(st);
+	if (ret)
+		return ret;
+
+	ret = iio_sw_buffer_preenable(indio_dev);
+	if (ret)
+		ad7266_powerdown(st);
+
+	return ret;
+}
+
+static int ad7266_postdisable(struct iio_dev *indio_dev)
+{
+	struct ad7266_state *st = iio_priv(indio_dev);
+	return ad7266_powerdown(st);
+}
+
+static const struct iio_buffer_setup_ops iio_triggered_buffer_setup_ops = {
+	.preenable = &ad7266_preenable,
+	.postenable = &iio_triggered_buffer_postenable,
+	.predisable = &iio_triggered_buffer_predisable,
+	.postdisable = &ad7266_postdisable,
+};
+
+static irqreturn_t ad7266_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct iio_buffer *buffer = indio_dev->buffer;
+	struct ad7266_state *st = iio_priv(indio_dev);
+	int ret;
+
+	ret = spi_read(st->spi, st->data, 4);
+	if (ret == 0) {
+		if (indio_dev->scan_timestamp)
+			((s64 *)st->data)[1] = pf->timestamp;
+		iio_push_to_buffer(buffer, (u8 *)st->data, pf->timestamp);
+	}
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static void ad7266_select_input(struct ad7266_state *st, unsigned int nr)
+{
+	unsigned int i;
+
+	if (st->fixed_addr)
+		return;
+
+	switch (st->mode) {
+	case AD7266_MODE_SINGLE_ENDED:
+		nr >>= 1;
+		break;
+	case AD7266_MODE_PSEUDO_DIFF:
+		nr |= 1;
+		break;
+	case AD7266_MODE_DIFF:
+		nr &= ~1;
+		break;
+	}
+
+	for (i = 0; i < 3; ++i)
+		gpio_set_value(st->gpios[i].gpio, (bool)(nr & BIT(i)));
+}
+
+static int ad7266_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *scan_mask)
+{
+	struct ad7266_state *st = iio_priv(indio_dev);
+	unsigned int nr = find_first_bit(scan_mask, indio_dev->masklength);
+
+	ad7266_select_input(st, nr);
+
+	return 0;
+}
+
+static int ad7266_read_single(struct ad7266_state *st, int *val,
+	unsigned int address)
+{
+	int ret;
+
+	ad7266_select_input(st, address);
+
+	ret = spi_sync(st->spi, &st->single_msg);
+	*val = be16_to_cpu(st->data[address % 2]);
+
+	return ret;
+}
+
+static int ad7266_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long m)
+{
+	struct ad7266_state *st = iio_priv(indio_dev);
+	unsigned long scale_uv;
+	int ret;
+
+	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
+
+		ret = ad7266_read_single(st, val, chan->address);
+		if (ret)
+			return ret;
+
+		*val = (*val >> 2) & 0xfff;
+		if (chan->scan_type.sign == 's')
+			*val = sign_extend32(*val, 11);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		scale_uv = (st->vref_uv * 100);
+		if (st->mode == AD7266_MODE_DIFF)
+			scale_uv *= 2;
+		if (st->range == AD7266_RANGE_2VREF)
+			scale_uv *= 2;
+
+		scale_uv >>= chan->scan_type.realbits;
+		*val =  scale_uv / 100000;
+		*val2 = (scale_uv % 100000) * 10;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_OFFSET:
+		if (st->range == AD7266_RANGE_2VREF &&
+			st->mode != AD7266_MODE_DIFF)
+			*val = 2048;
+		else
+			*val = 0;
+		return IIO_VAL_INT;
+	}
+	return -EINVAL;
+}
+
+#define AD7266_CHAN(_chan, _sign) {			\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.channel = (_chan),				\
+	.address = (_chan),				\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT	\
+		| IIO_CHAN_INFO_SCALE_SHARED_BIT	\
+		| IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
+	.scan_index = (_chan),				\
+	.scan_type = {					\
+		.sign = (_sign),			\
+		.realbits = 12,				\
+		.storagebits = 16,			\
+		.shift = 2,				\
+		.endianness = IIO_BE,			\
+	},						\
+}
+
+#define AD7266_DECLARE_SINGLE_ENDED_CHANNELS(_name, _sign) \
+const struct iio_chan_spec ad7266_channels_##_name[] = { \
+	AD7266_CHAN(0, (_sign)), \
+	AD7266_CHAN(1, (_sign)), \
+	AD7266_CHAN(2, (_sign)), \
+	AD7266_CHAN(3, (_sign)), \
+	AD7266_CHAN(4, (_sign)), \
+	AD7266_CHAN(5, (_sign)), \
+	AD7266_CHAN(6, (_sign)), \
+	AD7266_CHAN(7, (_sign)), \
+	AD7266_CHAN(8, (_sign)), \
+	AD7266_CHAN(9, (_sign)), \
+	AD7266_CHAN(10, (_sign)), \
+	AD7266_CHAN(11, (_sign)), \
+	IIO_CHAN_SOFT_TIMESTAMP(13), \
+}
+
+#define AD7266_DECLARE_SINGLE_ENDED_CHANNELS_FIXED(_name, _sign) \
+const struct iio_chan_spec ad7266_channels_##_name##_fixed[] = { \
+	AD7266_CHAN(0, (_sign)), \
+	AD7266_CHAN(1, (_sign)), \
+	IIO_CHAN_SOFT_TIMESTAMP(2), \
+}
+
+static AD7266_DECLARE_SINGLE_ENDED_CHANNELS(u, 'u');
+static AD7266_DECLARE_SINGLE_ENDED_CHANNELS(s, 's');
+static AD7266_DECLARE_SINGLE_ENDED_CHANNELS_FIXED(u, 'u');
+static AD7266_DECLARE_SINGLE_ENDED_CHANNELS_FIXED(s, 's');
+
+#define AD7266_CHAN_DIFF(_chan, _sign) {			\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.channel = (_chan) * 2,				\
+	.channel2 = (_chan) * 2 + 1,			\
+	.address = (_chan),				\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT	\
+		| IIO_CHAN_INFO_SCALE_SHARED_BIT	\
+		| IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
+	.scan_index = (_chan),				\
+	.scan_type = {					\
+		.sign = _sign,			\
+		.realbits = 12,				\
+		.storagebits = 16,			\
+		.shift = 2,				\
+		.endianness = IIO_BE,			\
+	},						\
+	.differential = 1,				\
+}
+
+#define AD7266_DECLARE_DIFF_CHANNELS(_name, _sign) \
+const struct iio_chan_spec ad7266_channels_diff_##_name[] = { \
+	AD7266_CHAN_DIFF(0, (_sign)), \
+	AD7266_CHAN_DIFF(1, (_sign)), \
+	AD7266_CHAN_DIFF(2, (_sign)), \
+	AD7266_CHAN_DIFF(3, (_sign)), \
+	AD7266_CHAN_DIFF(4, (_sign)), \
+	AD7266_CHAN_DIFF(5, (_sign)), \
+	IIO_CHAN_SOFT_TIMESTAMP(6), \
+}
+
+static AD7266_DECLARE_DIFF_CHANNELS(s, 's');
+static AD7266_DECLARE_DIFF_CHANNELS(u, 'u');
+
+#define AD7266_DECLARE_DIFF_CHANNELS_FIXED(_name, _sign) \
+const struct iio_chan_spec ad7266_channels_diff_fixed_##_name[] = { \
+	AD7266_CHAN_DIFF(0, (_sign)), \
+	AD7266_CHAN_DIFF(1, (_sign)), \
+	IIO_CHAN_SOFT_TIMESTAMP(2), \
+}
+
+static AD7266_DECLARE_DIFF_CHANNELS_FIXED(s, 's');
+static AD7266_DECLARE_DIFF_CHANNELS_FIXED(u, 'u');
+
+static const struct iio_info ad7266_info = {
+	.read_raw = &ad7266_read_raw,
+	.update_scan_mode = &ad7266_update_scan_mode,
+	.driver_module = THIS_MODULE,
+};
+
+static unsigned long ad7266_available_scan_masks[] = {
+	0x003,
+	0x00c,
+	0x030,
+	0x0c0,
+	0x300,
+	0xc00,
+	0x000,
+};
+
+static unsigned long ad7266_available_scan_masks_diff[] = {
+	0x003,
+	0x00c,
+	0x030,
+	0x000,
+};
+
+static unsigned long ad7266_available_scan_masks_fixed[] = {
+	0x003,
+	0x000,
+};
+
+struct ad7266_chan_info {
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
+	unsigned long *scan_masks;
+};
+
+#define AD7266_CHAN_INFO_INDEX(_differential, _signed, _fixed) \
+	(((_differential) << 2) | ((_signed) << 1) | ((_fixed) << 0))
+
+static const struct ad7266_chan_info ad7266_chan_infos[] = {
+	[AD7266_CHAN_INFO_INDEX(0, 0, 0)] = {
+		.channels = ad7266_channels_u,
+		.num_channels = ARRAY_SIZE(ad7266_channels_u),
+		.scan_masks = ad7266_available_scan_masks,
+	},
+	[AD7266_CHAN_INFO_INDEX(0, 0, 1)] = {
+		.channels = ad7266_channels_u_fixed,
+		.num_channels = ARRAY_SIZE(ad7266_channels_u_fixed),
+		.scan_masks = ad7266_available_scan_masks_fixed,
+	},
+	[AD7266_CHAN_INFO_INDEX(0, 1, 0)] = {
+		.channels = ad7266_channels_s,
+		.num_channels = ARRAY_SIZE(ad7266_channels_s),
+		.scan_masks = ad7266_available_scan_masks,
+	},
+	[AD7266_CHAN_INFO_INDEX(0, 1, 1)] = {
+		.channels = ad7266_channels_s_fixed,
+		.num_channels = ARRAY_SIZE(ad7266_channels_s_fixed),
+		.scan_masks = ad7266_available_scan_masks_fixed,
+	},
+	[AD7266_CHAN_INFO_INDEX(1, 0, 0)] = {
+		.channels = ad7266_channels_diff_u,
+		.num_channels = ARRAY_SIZE(ad7266_channels_diff_u),
+		.scan_masks = ad7266_available_scan_masks_diff,
+	},
+	[AD7266_CHAN_INFO_INDEX(1, 0, 1)] = {
+		.channels = ad7266_channels_diff_fixed_u,
+		.num_channels = ARRAY_SIZE(ad7266_channels_diff_fixed_u),
+		.scan_masks = ad7266_available_scan_masks_fixed,
+	},
+	[AD7266_CHAN_INFO_INDEX(1, 1, 0)] = {
+		.channels = ad7266_channels_diff_s,
+		.num_channels = ARRAY_SIZE(ad7266_channels_diff_s),
+		.scan_masks = ad7266_available_scan_masks_diff,
+	},
+	[AD7266_CHAN_INFO_INDEX(1, 1, 1)] = {
+		.channels = ad7266_channels_diff_fixed_s,
+		.num_channels = ARRAY_SIZE(ad7266_channels_diff_fixed_s),
+		.scan_masks = ad7266_available_scan_masks_fixed,
+	},
+};
+
+static void __devinit ad7266_init_channels(struct iio_dev *indio_dev)
+{
+	struct ad7266_state *st = iio_priv(indio_dev);
+	bool is_differential, is_signed;
+	const struct ad7266_chan_info *chan_info;
+	int i;
+
+	is_differential = st->mode != AD7266_MODE_SINGLE_ENDED;
+	is_signed = (st->range == AD7266_RANGE_2VREF) |
+		    (st->mode == AD7266_MODE_DIFF);
+
+	i = AD7266_CHAN_INFO_INDEX(is_differential, is_signed, st->fixed_addr);
+	chan_info = &ad7266_chan_infos[i];
+
+	indio_dev->channels = chan_info->channels;
+	indio_dev->num_channels = chan_info->num_channels;
+	indio_dev->available_scan_masks = chan_info->scan_masks;
+	indio_dev->masklength = chan_info->num_channels - 1;
+}
+
+static const char * const ad7266_gpio_labels[] = {
+	"AD0", "AD1", "AD2",
+};
+
+static int __devinit ad7266_probe(struct spi_device *spi)
+{
+	struct ad7266_platform_data *pdata = spi->dev.platform_data;
+	struct iio_dev *indio_dev;
+	struct ad7266_state *st;
+	unsigned int i;
+	int ret;
+
+	indio_dev = iio_device_alloc(sizeof(*st));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->reg = regulator_get(&spi->dev, "vref");
+	if (!IS_ERR_OR_NULL(st->reg)) {
+		ret = regulator_enable(st->reg);
+		if (ret)
+			goto error_put_reg;
+
+		st->vref_uv = regulator_get_voltage(st->reg);
+	} else {
+		/* Use internal reference */
+		st->vref_uv = 2500000;
+	}
+
+	if (pdata) {
+		st->fixed_addr = pdata->fixed_addr;
+		st->mode = pdata->mode;
+		st->range = pdata->range;
+
+		if (!st->fixed_addr) {
+			for (i = 0; i < ARRAY_SIZE(st->gpios); ++i) {
+				st->gpios[i].gpio = pdata->addr_gpios[i];
+				st->gpios[i].flags = GPIOF_OUT_INIT_LOW;
+				st->gpios[i].label = ad7266_gpio_labels[i];
+			}
+			ret = gpio_request_array(st->gpios,
+				ARRAY_SIZE(st->gpios));
+			if (ret)
+				goto error_disable_reg;
+		}
+	} else {
+		st->fixed_addr = true;
+		st->range = AD7266_RANGE_VREF;
+		st->mode = AD7266_MODE_DIFF;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+	st->spi = spi;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &ad7266_info;
+
+	ad7266_init_channels(indio_dev);
+
+	/* wakeup */
+	st->single_xfer[0].rx_buf = &st->data;
+	st->single_xfer[0].len = 2;
+	st->single_xfer[0].cs_change = 1;
+	/* conversion */
+	st->single_xfer[1].rx_buf = &st->data;
+	st->single_xfer[1].len = 4;
+	st->single_xfer[1].cs_change = 1;
+	/* powerdown */
+	st->single_xfer[2].tx_buf = &st->data;
+	st->single_xfer[2].len = 1;
+
+	spi_message_init(&st->single_msg);
+	spi_message_add_tail(&st->single_xfer[0], &st->single_msg);
+	spi_message_add_tail(&st->single_xfer[1], &st->single_msg);
+	spi_message_add_tail(&st->single_xfer[2], &st->single_msg);
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		&ad7266_trigger_handler, &iio_triggered_buffer_setup_ops);
+	if (ret)
+		goto error_free_gpios;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_buffer_cleanup;
+
+	return 0;
+
+error_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+error_free_gpios:
+	if (!st->fixed_addr)
+		gpio_free_array(st->gpios, ARRAY_SIZE(st->gpios));
+error_disable_reg:
+	if (!IS_ERR_OR_NULL(st->reg))
+		regulator_disable(st->reg);
+error_put_reg:
+	if (!IS_ERR_OR_NULL(st->reg))
+		regulator_put(st->reg);
+
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad7266_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad7266_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	if (!st->fixed_addr)
+		gpio_free_array(st->gpios, ARRAY_SIZE(st->gpios));
+	if (!IS_ERR_OR_NULL(st->reg)) {
+		regulator_disable(st->reg);
+		regulator_put(st->reg);
+	}
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad7266_id[] = {
+	{"ad7265", 0},
+	{"ad7266", 0},
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, ad7266_id);
+
+static struct spi_driver ad7266_driver = {
+	.driver = {
+		.name	= "ad7266",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ad7266_probe,
+	.remove		= __devexit_p(ad7266_remove),
+	.id_table	= ad7266_id,
+};
+module_spi_driver(ad7266_driver);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices AD7266/65 ADC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index f18a95d8025..f61780a0237 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -26,9 +26,9 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include <mach/at91_adc.h>
 
@@ -318,58 +318,15 @@ static void at91_adc_trigger_remove(struct iio_dev *idev)
 	}
 }
 
-static const struct iio_buffer_setup_ops at91_adc_buffer_ops = {
-	.preenable = &iio_sw_buffer_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-};
-
 static int at91_adc_buffer_init(struct iio_dev *idev)
 {
-	int ret;
-
-	idev->buffer = iio_kfifo_allocate(idev);
-	if (!idev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-
-	idev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
-					    &at91_adc_trigger_handler,
-					    IRQF_ONESHOT,
-					    idev,
-					    "%s-consumer%d",
-					    idev->name,
-					    idev->id);
-	if (idev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_pollfunc;
-	}
-
-	idev->setup_ops = &at91_adc_buffer_ops;
-	idev->modes |= INDIO_BUFFER_TRIGGERED;
-
-	ret = iio_buffer_register(idev,
-				  idev->channels,
-				  idev->num_channels);
-	if (ret)
-		goto error_register;
-
-	return 0;
-
-error_register:
-	iio_dealloc_pollfunc(idev->pollfunc);
-error_pollfunc:
-	iio_kfifo_free(idev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(idev, &iio_pollfunc_store_time,
+		&at91_adc_trigger_handler, NULL);
 }
 
 static void at91_adc_buffer_remove(struct iio_dev *idev)
 {
-	iio_buffer_unregister(idev);
-	iio_dealloc_pollfunc(idev->pollfunc);
-	iio_kfifo_free(idev->buffer);
+	iio_triggered_buffer_cleanup(idev);
 }
 
 static int at91_adc_read_raw(struct iio_dev *idev,
@@ -392,9 +349,11 @@ static int at91_adc_read_raw(struct iio_dev *idev,
 						       st->done,
 						       msecs_to_jiffies(1000));
 		if (ret == 0)
-			return -ETIMEDOUT;
-		else if (ret < 0)
+			ret = -ETIMEDOUT;
+		if (ret < 0) {
+			mutex_unlock(&st->lock);
 			return ret;
+		}
 
 		*val = st->last_value;
 
diff --git a/drivers/staging/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index a626f03871e..1be15fa9d61 100644
--- a/drivers/staging/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -4,12 +4,12 @@
 menu "Digital to analog converters"
 
 config AD5064
-	tristate "Analog Devices AD5064/64-1/65/44/45/24/25, AD5628/48/66/68 DAC driver"
-	depends on SPI
+	tristate "Analog Devices AD5064 and similar multi-channel DAC driver"
+	depends on (SPI_MASTER || I2C)
 	help
 	  Say yes here to build support for Analog Devices AD5024, AD5025, AD5044,
-	  AD5045, AD5064, AD5064-1, AD5065, AD5628, AD5648, AD5666, AD5668 Digital
-	  to Analog Converter.
+	  AD5045, AD5064, AD5064-1, AD5065, AD5628, AD5629R, AD5648, AD5666, AD5668,
+	  AD5669R Digital to Analog Converter.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5064.
@@ -59,9 +59,9 @@ config AD5446
 	tristate "Analog Devices AD5446 and similar single channel DACs driver"
 	depends on SPI
 	help
-	  Say yes here to build support for Analog Devices AD5444, AD5446,
-	  AD5512A, AD5541A, AD5542A, AD5543, AD5553, AD5601, AD5611, AD5620,
-	  AD5621, AD5640, AD5660, AD5662 DACs.
+	  Say yes here to build support for Analog Devices AD5444, AD5446, AD5450,
+	  AD5451, AD5452, AD5453, AD5512A, AD5541A, AD5542A, AD5543, AD5553, AD5601,
+	  AD5611, AD5620, AD5621, AD5640, AD5660, AD5662 DACs.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5446.
@@ -118,4 +118,15 @@ config MAX517
 	  This driver can also be built as a module.  If so, the module
 	  will be called max517.
 
+config MCP4725
+	tristate "MCP4725 DAC driver"
+	depends on I2C
+	---help---
+	  Say Y here if you want to build a driver for the Microchip
+	  MCP 4725 12-bit digital-to-analog converter (DAC) with I2C
+	  interface.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mcp4725.
+
 endmenu
diff --git a/drivers/staging/iio/dac/Makefile b/drivers/iio/dac/Makefile
index 8ab1d264aab..9ea3ceeefc0 100644
--- a/drivers/staging/iio/dac/Makefile
+++ b/drivers/iio/dac/Makefile
@@ -13,3 +13,4 @@ obj-$(CONFIG_AD5764) += ad5764.o
 obj-$(CONFIG_AD5791) += ad5791.o
 obj-$(CONFIG_AD5686) += ad5686.o
 obj-$(CONFIG_MAX517) += max517.o
+obj-$(CONFIG_MCP4725) += mcp4725.o
diff --git a/drivers/staging/iio/dac/ad5064.c b/drivers/iio/dac/ad5064.c
index 047148aa66b..eb281a2c295 100644
--- a/drivers/staging/iio/dac/ad5064.c
+++ b/drivers/iio/dac/ad5064.c
@@ -1,6 +1,6 @@
 /*
- * AD5024, AD5025, AD5044, AD5045, AD5064, AD5064-1, AD5065, AD5628, AD5648,
- * AD5666, AD5668 Digital to analog converters driver
+ * AD5024, AD5025, AD5044, AD5045, AD5064, AD5064-1, AD5065, AD5628, AD5629R,
+ * AD5648, AD5666, AD5668, AD5669R Digital to analog converters driver
  *
  * Copyright 2011 Analog Devices Inc.
  *
@@ -12,13 +12,14 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/spi/spi.h>
+#include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 #include <linux/regulator/consumer.h>
+#include <asm/unaligned.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
 
 #define AD5064_MAX_DAC_CHANNELS			8
 #define AD5064_MAX_VREFS			4
@@ -63,9 +64,14 @@ struct ad5064_chip_info {
 	unsigned int num_channels;
 };
 
+struct ad5064_state;
+
+typedef int (*ad5064_write_func)(struct ad5064_state *st, unsigned int cmd,
+		unsigned int addr, unsigned int val);
+
 /**
  * struct ad5064_state - driver instance specific data
- * @spi:		spi_device
+ * @dev:		the device for this driver instance
  * @chip_info:		chip model specific constants, available modes etc
  * @vref_reg:		vref supply regulators
  * @pwr_down:		whether channel is powered down
@@ -73,11 +79,12 @@ struct ad5064_chip_info {
  * @dac_cache:		current DAC raw value (chip does not support readback)
  * @use_internal_vref:	set to true if the internal reference voltage should be
  *			used.
- * @data:		spi transfer buffers
+ * @write:		register write callback
+ * @data:		i2c/spi transfer buffers
  */
 
 struct ad5064_state {
-	struct spi_device		*spi;
+	struct device			*dev;
 	const struct ad5064_chip_info	*chip_info;
 	struct regulator_bulk_data	vref_reg[AD5064_MAX_VREFS];
 	bool				pwr_down[AD5064_MAX_DAC_CHANNELS];
@@ -85,11 +92,16 @@ struct ad5064_state {
 	unsigned int			dac_cache[AD5064_MAX_DAC_CHANNELS];
 	bool				use_internal_vref;
 
+	ad5064_write_func		write;
+
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
 	 * transfer buffers to live in their own cache lines.
 	 */
-	__be32 data ____cacheline_aligned;
+	union {
+		u8 i2c[3];
+		__be32 spi;
+	} data ____cacheline_aligned;
 };
 
 enum ad5064_type {
@@ -110,14 +122,12 @@ enum ad5064_type {
 	ID_AD5668_2,
 };
 
-static int ad5064_spi_write(struct ad5064_state *st, unsigned int cmd,
+static int ad5064_write(struct ad5064_state *st, unsigned int cmd,
 	unsigned int addr, unsigned int val, unsigned int shift)
 {
 	val <<= shift;
 
-	st->data = cpu_to_be32(AD5064_CMD(cmd) | AD5064_ADDR(addr) | val);
-
-	return spi_write(st->spi, &st->data, sizeof(st->data));
+	return st->write(st, cmd, addr, val);
 }
 
 static int ad5064_sync_powerdown_mode(struct ad5064_state *st,
@@ -131,62 +141,47 @@ static int ad5064_sync_powerdown_mode(struct ad5064_state *st,
 	if (st->pwr_down[channel])
 		val |= st->pwr_down_mode[channel] << 8;
 
-	ret = ad5064_spi_write(st, AD5064_CMD_POWERDOWN_DAC, 0, val, 0);
+	ret = ad5064_write(st, AD5064_CMD_POWERDOWN_DAC, 0, val, 0);
 
 	return ret;
 }
 
-static const char ad5064_powerdown_modes[][15] = {
-	[AD5064_LDAC_PWRDN_NONE]	= "",
-	[AD5064_LDAC_PWRDN_1K]		= "1kohm_to_gnd",
-	[AD5064_LDAC_PWRDN_100K]	= "100kohm_to_gnd",
-	[AD5064_LDAC_PWRDN_3STATE]	= "three_state",
+static const char * const ad5064_powerdown_modes[] = {
+	"1kohm_to_gnd",
+	"100kohm_to_gnd",
+	"three_state",
 };
 
-static ssize_t ad5064_read_powerdown_mode_available(struct iio_dev *indio_dev,
-	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
-{
-	return sprintf(buf, "%s %s %s\n", ad5064_powerdown_modes[1],
-		ad5064_powerdown_modes[2], ad5064_powerdown_modes[3]);
-}
-
-static ssize_t ad5064_read_powerdown_mode(struct iio_dev *indio_dev,
-	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
+static int ad5064_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
 
-	return sprintf(buf, "%s\n",
-		ad5064_powerdown_modes[st->pwr_down_mode[chan->channel]]);
+	return st->pwr_down_mode[chan->channel] - 1;
 }
 
-static ssize_t ad5064_write_powerdown_mode(struct iio_dev *indio_dev,
-	uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
-	size_t len)
+static int ad5064_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
-	unsigned int mode, i;
 	int ret;
 
-	mode = 0;
-
-	for (i = 1; i < ARRAY_SIZE(ad5064_powerdown_modes); ++i) {
-		if (sysfs_streq(buf, ad5064_powerdown_modes[i])) {
-			mode = i;
-			break;
-		}
-	}
-	if (mode == 0)
-		return  -EINVAL;
-
 	mutex_lock(&indio_dev->mlock);
-	st->pwr_down_mode[chan->channel] = mode;
+	st->pwr_down_mode[chan->channel] = mode + 1;
 
 	ret = ad5064_sync_powerdown_mode(st, chan->channel);
 	mutex_unlock(&indio_dev->mlock);
 
-	return ret ? ret : len;
+	return ret;
 }
 
+static const struct iio_enum ad5064_powerdown_mode_enum = {
+	.items = ad5064_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5064_powerdown_modes),
+	.get = ad5064_get_powerdown_mode,
+	.set = ad5064_set_powerdown_mode,
+};
+
 static ssize_t ad5064_read_dac_powerdown(struct iio_dev *indio_dev,
 	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
@@ -267,7 +262,7 @@ static int ad5064_write_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 
 		mutex_lock(&indio_dev->mlock);
-		ret = ad5064_spi_write(st, AD5064_CMD_WRITE_INPUT_N_UPDATE_N,
+		ret = ad5064_write(st, AD5064_CMD_WRITE_INPUT_N_UPDATE_N,
 				chan->address, val, chan->scan_type.shift);
 		if (ret == 0)
 			st->dac_cache[chan->channel] = val;
@@ -286,22 +281,14 @@ static const struct iio_info ad5064_info = {
 	.driver_module = THIS_MODULE,
 };
 
-static struct iio_chan_spec_ext_info ad5064_ext_info[] = {
+static const struct iio_chan_spec_ext_info ad5064_ext_info[] = {
 	{
 		.name = "powerdown",
 		.read = ad5064_read_dac_powerdown,
 		.write = ad5064_write_dac_powerdown,
 	},
-	{
-		.name = "powerdown_mode",
-		.read = ad5064_read_powerdown_mode,
-		.write = ad5064_write_powerdown_mode,
-	},
-	{
-		.name = "powerdown_mode_available",
-		.shared = true,
-		.read = ad5064_read_powerdown_mode_available,
-	},
+	IIO_ENUM("powerdown_mode", false, &ad5064_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5064_powerdown_mode_enum),
 	{ },
 };
 
@@ -437,9 +424,9 @@ static const char * const ad5064_vref_name(struct ad5064_state *st,
 	return st->chip_info->shared_vref ? "vref" : ad5064_vref_names[vref];
 }
 
-static int __devinit ad5064_probe(struct spi_device *spi)
+static int __devinit ad5064_probe(struct device *dev, enum ad5064_type type,
+	const char *name, ad5064_write_func write)
 {
-	enum ad5064_type type = spi_get_device_id(spi)->driver_data;
 	struct iio_dev *indio_dev;
 	struct ad5064_state *st;
 	unsigned int i;
@@ -450,24 +437,25 @@ static int __devinit ad5064_probe(struct spi_device *spi)
 		return  -ENOMEM;
 
 	st = iio_priv(indio_dev);
-	spi_set_drvdata(spi, indio_dev);
+	dev_set_drvdata(dev, indio_dev);
 
 	st->chip_info = &ad5064_chip_info_tbl[type];
-	st->spi = spi;
+	st->dev = dev;
+	st->write = write;
 
 	for (i = 0; i < ad5064_num_vref(st); ++i)
 		st->vref_reg[i].supply = ad5064_vref_name(st, i);
 
-	ret = regulator_bulk_get(&st->spi->dev, ad5064_num_vref(st),
+	ret = regulator_bulk_get(dev, ad5064_num_vref(st),
 		st->vref_reg);
 	if (ret) {
 		if (!st->chip_info->internal_vref)
 			goto error_free;
 		st->use_internal_vref = true;
-		ret = ad5064_spi_write(st, AD5064_CMD_CONFIG, 0,
+		ret = ad5064_write(st, AD5064_CMD_CONFIG, 0,
 			AD5064_CONFIG_INT_VREF_ENABLE, 0);
 		if (ret) {
-			dev_err(&spi->dev, "Failed to enable internal vref: %d\n",
+			dev_err(dev, "Failed to enable internal vref: %d\n",
 				ret);
 			goto error_free;
 		}
@@ -482,8 +470,8 @@ static int __devinit ad5064_probe(struct spi_device *spi)
 		st->dac_cache[i] = 0x8000;
 	}
 
-	indio_dev->dev.parent = &spi->dev;
-	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->dev.parent = dev;
+	indio_dev->name = name;
 	indio_dev->info = &ad5064_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = st->chip_info->channels;
@@ -507,10 +495,9 @@ error_free:
 	return ret;
 }
 
-
-static int __devexit ad5064_remove(struct spi_device *spi)
+static int __devexit ad5064_remove(struct device *dev)
 {
-	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5064_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
@@ -525,7 +512,31 @@ static int __devexit ad5064_remove(struct spi_device *spi)
 	return 0;
 }
 
-static const struct spi_device_id ad5064_id[] = {
+#if IS_ENABLED(CONFIG_SPI_MASTER)
+
+static int ad5064_spi_write(struct ad5064_state *st, unsigned int cmd,
+	unsigned int addr, unsigned int val)
+{
+	struct spi_device *spi = to_spi_device(st->dev);
+
+	st->data.spi = cpu_to_be32(AD5064_CMD(cmd) | AD5064_ADDR(addr) | val);
+	return spi_write(spi, &st->data.spi, sizeof(st->data.spi));
+}
+
+static int __devinit ad5064_spi_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
+
+	return ad5064_probe(&spi->dev, id->driver_data, id->name,
+				ad5064_spi_write);
+}
+
+static int __devexit ad5064_spi_remove(struct spi_device *spi)
+{
+	return ad5064_remove(&spi->dev);
+}
+
+static const struct spi_device_id ad5064_spi_ids[] = {
 	{"ad5024", ID_AD5024},
 	{"ad5025", ID_AD5025},
 	{"ad5044", ID_AD5044},
@@ -544,19 +555,122 @@ static const struct spi_device_id ad5064_id[] = {
 	{"ad5668-3", ID_AD5668_2}, /* similar enough to ad5668-2 */
 	{}
 };
-MODULE_DEVICE_TABLE(spi, ad5064_id);
+MODULE_DEVICE_TABLE(spi, ad5064_spi_ids);
+
+static struct spi_driver ad5064_spi_driver = {
+	.driver = {
+		   .name = "ad5064",
+		   .owner = THIS_MODULE,
+	},
+	.probe = ad5064_spi_probe,
+	.remove = __devexit_p(ad5064_spi_remove),
+	.id_table = ad5064_spi_ids,
+};
+
+static int __init ad5064_spi_register_driver(void)
+{
+	return spi_register_driver(&ad5064_spi_driver);
+}
+
+static void ad5064_spi_unregister_driver(void)
+{
+	spi_unregister_driver(&ad5064_spi_driver);
+}
+
+#else
+
+static inline int ad5064_spi_register_driver(void) { return 0; }
+static inline void ad5064_spi_unregister_driver(void) { }
+
+#endif
+
+#if IS_ENABLED(CONFIG_I2C)
+
+static int ad5064_i2c_write(struct ad5064_state *st, unsigned int cmd,
+	unsigned int addr, unsigned int val)
+{
+	struct i2c_client *i2c = to_i2c_client(st->dev);
+
+	st->data.i2c[0] = (cmd << 4) | addr;
+	put_unaligned_be16(val, &st->data.i2c[1]);
+	return i2c_master_send(i2c, st->data.i2c, 3);
+}
+
+static int __devinit ad5064_i2c_probe(struct i2c_client *i2c,
+	const struct i2c_device_id *id)
+{
+	return ad5064_probe(&i2c->dev, id->driver_data, id->name,
+						ad5064_i2c_write);
+}
+
+static int __devexit ad5064_i2c_remove(struct i2c_client *i2c)
+{
+	return ad5064_remove(&i2c->dev);
+}
+
+static const struct i2c_device_id ad5064_i2c_ids[] = {
+	{"ad5629-1", ID_AD5628_1},
+	{"ad5629-2", ID_AD5628_2},
+	{"ad5629-3", ID_AD5628_2}, /* similar enough to ad5629-2 */
+	{"ad5669-1", ID_AD5668_1},
+	{"ad5669-2", ID_AD5668_2},
+	{"ad5669-3", ID_AD5668_2}, /* similar enough to ad5669-2 */
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ad5064_i2c_ids);
 
-static struct spi_driver ad5064_driver = {
+static struct i2c_driver ad5064_i2c_driver = {
 	.driver = {
 		   .name = "ad5064",
 		   .owner = THIS_MODULE,
 	},
-	.probe = ad5064_probe,
-	.remove = __devexit_p(ad5064_remove),
-	.id_table = ad5064_id,
+	.probe = ad5064_i2c_probe,
+	.remove = __devexit_p(ad5064_i2c_remove),
+	.id_table = ad5064_i2c_ids,
 };
-module_spi_driver(ad5064_driver);
+
+static int __init ad5064_i2c_register_driver(void)
+{
+	return i2c_add_driver(&ad5064_i2c_driver);
+}
+
+static void __exit ad5064_i2c_unregister_driver(void)
+{
+	i2c_del_driver(&ad5064_i2c_driver);
+}
+
+#else
+
+static inline int ad5064_i2c_register_driver(void) { return 0; }
+static inline void ad5064_i2c_unregister_driver(void) { }
+
+#endif
+
+static int __init ad5064_init(void)
+{
+	int ret;
+
+	ret = ad5064_spi_register_driver();
+	if (ret)
+		return ret;
+
+	ret = ad5064_i2c_register_driver();
+	if (ret) {
+		ad5064_spi_unregister_driver();
+		return ret;
+	}
+
+	return 0;
+}
+module_init(ad5064_init);
+
+static void __exit ad5064_exit(void)
+{
+	ad5064_i2c_unregister_driver();
+	ad5064_spi_unregister_driver();
+}
+module_exit(ad5064_exit);
 
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
-MODULE_DESCRIPTION("Analog Devices AD5024/25/44/45/64/64-1/65, AD5628/48/66/68 DAC");
+MODULE_DESCRIPTION("Analog Devices AD5024 and similar multi-channel DACs");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/dac/ad5360.c b/drivers/iio/dac/ad5360.c
index 38660efca78..8fce84fe70b 100644
--- a/drivers/staging/iio/dac/ad5360.c
+++ b/drivers/iio/dac/ad5360.c
@@ -18,7 +18,6 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
 
 #define AD5360_CMD(x)				((x) << 22)
 #define AD5360_ADDR(x)				((x) << 16)
diff --git a/drivers/staging/iio/dac/ad5380.c b/drivers/iio/dac/ad5380.c
index 370d2842190..14991ac55f2 100644
--- a/drivers/staging/iio/dac/ad5380.c
+++ b/drivers/iio/dac/ad5380.c
@@ -20,8 +20,6 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
-
 
 #define AD5380_REG_DATA(x)	(((x) << 2) | 3)
 #define AD5380_REG_OFFSET(x)	(((x) << 2) | 2)
@@ -81,103 +79,18 @@ enum ad5380_type {
 	ID_AD5392_5,
 };
 
-#define AD5380_CHANNEL(_bits) {					\
-	.type = IIO_VOLTAGE,					\
-	.indexed = 1,						\
-	.output = 1,						\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
-		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
-	.scan_type = IIO_ST('u', (_bits), 16, 14 - (_bits))	\
-}
-
-static const struct ad5380_chip_info ad5380_chip_info_tbl[] = {
-	[ID_AD5380_3] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 40,
-		.int_vref = 1250000,
-	},
-	[ID_AD5380_5] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 40,
-		.int_vref = 2500000,
-	},
-	[ID_AD5381_3] = {
-		.channel_template = AD5380_CHANNEL(12),
-		.num_channels = 16,
-		.int_vref = 1250000,
-	},
-	[ID_AD5381_5] = {
-		.channel_template = AD5380_CHANNEL(12),
-		.num_channels = 16,
-		.int_vref = 2500000,
-	},
-	[ID_AD5382_3] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 32,
-		.int_vref = 1250000,
-	},
-	[ID_AD5382_5] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 32,
-		.int_vref = 2500000,
-	},
-	[ID_AD5383_3] = {
-		.channel_template = AD5380_CHANNEL(12),
-		.num_channels = 32,
-		.int_vref = 1250000,
-	},
-	[ID_AD5383_5] = {
-		.channel_template = AD5380_CHANNEL(12),
-		.num_channels = 32,
-		.int_vref = 2500000,
-	},
-	[ID_AD5390_3] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 16,
-		.int_vref = 1250000,
-	},
-	[ID_AD5390_5] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 16,
-		.int_vref = 2500000,
-	},
-	[ID_AD5391_3] = {
-		.channel_template = AD5380_CHANNEL(12),
-		.num_channels = 16,
-		.int_vref = 1250000,
-	},
-	[ID_AD5391_5] = {
-		.channel_template = AD5380_CHANNEL(12),
-		.num_channels = 16,
-		.int_vref = 2500000,
-	},
-	[ID_AD5392_3] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 8,
-		.int_vref = 1250000,
-	},
-	[ID_AD5392_5] = {
-		.channel_template = AD5380_CHANNEL(14),
-		.num_channels = 8,
-		.int_vref = 2500000,
-	},
-};
-
-static ssize_t ad5380_read_dac_powerdown(struct device *dev,
-	struct device_attribute *attr, char *buf)
+static ssize_t ad5380_read_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", st->pwr_down);
 }
 
-static ssize_t ad5380_write_dac_powerdown(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t len)
+static ssize_t ad5380_write_dac_powerdown(struct iio_dev *indio_dev,
+	 uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	 size_t len)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 	bool pwr_down;
 	int ret;
@@ -200,20 +113,14 @@ static ssize_t ad5380_write_dac_powerdown(struct device *dev,
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage_powerdown,
-			S_IRUGO | S_IWUSR,
-			ad5380_read_dac_powerdown,
-			ad5380_write_dac_powerdown, 0);
-
-static const char ad5380_powerdown_modes[][15] = {
-	[0]	= "100kohm_to_gnd",
-	[1]	= "three_state",
+static const char * const ad5380_powerdown_modes[] = {
+	"100kohm_to_gnd",
+	"three_state",
 };
 
-static ssize_t ad5380_read_powerdown_mode(struct device *dev,
-	struct device_attribute *attr, char *buf)
+static int ad5380_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
 	unsigned int mode;
 	int ret;
@@ -224,49 +131,27 @@ static ssize_t ad5380_read_powerdown_mode(struct device *dev,
 
 	mode = (mode >> AD5380_CTRL_PWR_DOWN_MODE_OFFSET) & 1;
 
-	return sprintf(buf, "%s\n", ad5380_powerdown_modes[mode]);
+	return mode;
 }
 
-static ssize_t ad5380_write_powerdown_mode(struct device *dev,
-	struct device_attribute *attr, const char *buf, size_t len)
+static int ad5380_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5380_state *st = iio_priv(indio_dev);
-	unsigned int i;
 	int ret;
 
-	for (i = 0; i < ARRAY_SIZE(ad5380_powerdown_modes); ++i) {
-		if (sysfs_streq(buf, ad5380_powerdown_modes[i]))
-			break;
-	}
-
-	if (i == ARRAY_SIZE(ad5380_powerdown_modes))
-		return -EINVAL;
-
 	ret = regmap_update_bits(st->regmap, AD5380_REG_SF_CTRL,
 		1 << AD5380_CTRL_PWR_DOWN_MODE_OFFSET,
-		i << AD5380_CTRL_PWR_DOWN_MODE_OFFSET);
+		mode << AD5380_CTRL_PWR_DOWN_MODE_OFFSET);
 
-	return ret ? ret : len;
+	return ret;
 }
 
-static IIO_DEVICE_ATTR(out_voltage_powerdown_mode,
-			S_IRUGO | S_IWUSR,
-			ad5380_read_powerdown_mode,
-			ad5380_write_powerdown_mode, 0);
-
-static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
-			"100kohm_to_gnd three_state");
-
-static struct attribute *ad5380_attributes[] = {
-	&iio_dev_attr_out_voltage_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5380_attribute_group = {
-	.attrs = ad5380_attributes,
+static const struct iio_enum ad5380_powerdown_mode_enum = {
+	.items = ad5380_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5380_powerdown_modes),
+	.get = ad5380_get_powerdown_mode,
+	.set = ad5380_set_powerdown_mode,
 };
 
 static unsigned int ad5380_info_to_reg(struct iio_chan_spec const *chan,
@@ -354,10 +239,105 @@ static int ad5380_read_raw(struct iio_dev *indio_dev,
 static const struct iio_info ad5380_info = {
 	.read_raw = ad5380_read_raw,
 	.write_raw = ad5380_write_raw,
-	.attrs = &ad5380_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
+static struct iio_chan_spec_ext_info ad5380_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = ad5380_read_dac_powerdown,
+		.write = ad5380_write_dac_powerdown,
+	},
+	IIO_ENUM("powerdown_mode", true, &ad5380_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5380_powerdown_mode_enum),
+	{ },
+};
+
+#define AD5380_CHANNEL(_bits) {					\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.output = 1,						\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.scan_type = IIO_ST('u', (_bits), 16, 14 - (_bits)),	\
+	.ext_info = ad5380_ext_info,				\
+}
+
+static const struct ad5380_chip_info ad5380_chip_info_tbl[] = {
+	[ID_AD5380_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 40,
+		.int_vref = 1250000,
+	},
+	[ID_AD5380_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 40,
+		.int_vref = 2500000,
+	},
+	[ID_AD5381_3] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 1250000,
+	},
+	[ID_AD5381_5] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 2500000,
+	},
+	[ID_AD5382_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 32,
+		.int_vref = 1250000,
+	},
+	[ID_AD5382_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 32,
+		.int_vref = 2500000,
+	},
+	[ID_AD5383_3] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 32,
+		.int_vref = 1250000,
+	},
+	[ID_AD5383_5] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 32,
+		.int_vref = 2500000,
+	},
+	[ID_AD5390_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 16,
+		.int_vref = 1250000,
+	},
+	[ID_AD5390_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 16,
+		.int_vref = 2500000,
+	},
+	[ID_AD5391_3] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 1250000,
+	},
+	[ID_AD5391_5] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 2500000,
+	},
+	[ID_AD5392_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 8,
+		.int_vref = 1250000,
+	},
+	[ID_AD5392_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 8,
+		.int_vref = 2500000,
+	},
+};
+
 static int __devinit ad5380_alloc_channels(struct iio_dev *indio_dev)
 {
 	struct ad5380_state *st = iio_priv(indio_dev);
@@ -393,7 +373,7 @@ static int __devinit ad5380_probe(struct device *dev, struct regmap *regmap,
 	if (indio_dev == NULL) {
 		dev_err(dev, "Failed to allocate iio device\n");
 		ret = -ENOMEM;
-		goto error_regmap_exit;
+		goto error_out;
 	}
 
 	st = iio_priv(indio_dev);
@@ -456,8 +436,7 @@ error_free_reg:
 	kfree(indio_dev->channels);
 error_free:
 	iio_device_free(indio_dev);
-error_regmap_exit:
-	regmap_exit(regmap);
+error_out:
 
 	return ret;
 }
@@ -476,7 +455,6 @@ static int __devexit ad5380_remove(struct device *dev)
 		regulator_put(st->vref_reg);
 	}
 
-	regmap_exit(st->regmap);
 	iio_device_free(indio_dev);
 
 	return 0;
@@ -505,7 +483,7 @@ static int __devinit ad5380_spi_probe(struct spi_device *spi)
 	const struct spi_device_id *id = spi_get_device_id(spi);
 	struct regmap *regmap;
 
-	regmap = regmap_init_spi(spi, &ad5380_regmap_config);
+	regmap = devm_regmap_init_spi(spi, &ad5380_regmap_config);
 
 	if (IS_ERR(regmap))
 		return PTR_ERR(regmap);
@@ -579,7 +557,7 @@ static int __devinit ad5380_i2c_probe(struct i2c_client *i2c,
 {
 	struct regmap *regmap;
 
-	regmap = regmap_init_i2c(i2c, &ad5380_regmap_config);
+	regmap = devm_regmap_init_i2c(i2c, &ad5380_regmap_config);
 
 	if (IS_ERR(regmap))
 		return PTR_ERR(regmap);
diff --git a/drivers/staging/iio/dac/ad5421.c b/drivers/iio/dac/ad5421.c
index ffbd4c234f5..cdbc5bf25c3 100644
--- a/drivers/staging/iio/dac/ad5421.c
+++ b/drivers/iio/dac/ad5421.c
@@ -19,8 +19,7 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/events.h>
-#include "dac.h"
-#include "ad5421.h"
+#include <linux/iio/dac/ad5421.h>
 
 
 #define AD5421_REG_DAC_DATA		0x1
diff --git a/drivers/staging/iio/dac/ad5446.c b/drivers/iio/dac/ad5446.c
index daa65b384c1..2ca5059ef89 100644
--- a/drivers/staging/iio/dac/ad5446.c
+++ b/drivers/iio/dac/ad5446.c
@@ -20,7 +20,6 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
 
 #include "ad5446.h"
 
@@ -42,47 +41,34 @@ static int ad5660_write(struct ad5446_state *st, unsigned val)
 }
 
 static const char * const ad5446_powerdown_modes[] = {
-	"", "1kohm_to_gnd", "100kohm_to_gnd", "three_state"
+	"1kohm_to_gnd", "100kohm_to_gnd", "three_state"
 };
 
-static ssize_t ad5446_read_powerdown_mode_available(struct iio_dev *indio_dev,
-	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
-{
-	return sprintf(buf, "%s %s %s\n", ad5446_powerdown_modes[1],
-		ad5446_powerdown_modes[2], ad5446_powerdown_modes[3]);
-}
-
-static ssize_t ad5446_write_powerdown_mode(struct iio_dev *indio_dev,
-					    uintptr_t private,
-					    const struct iio_chan_spec *chan,
-					    const char *buf, size_t len)
+static int ad5446_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
 	struct ad5446_state *st = iio_priv(indio_dev);
-	int i;
-
-	for (i = 1; i < ARRAY_SIZE(ad5446_powerdown_modes); i++) {
-		if (sysfs_streq(buf, ad5446_powerdown_modes[i])) {
-			st->pwr_down_mode = i;
-			break;
-		}
-	}
 
-	if (i == ARRAY_SIZE(ad5446_powerdown_modes))
-		return -EINVAL;
+	st->pwr_down_mode = mode + 1;
 
-	return len;
+	return 0;
 }
 
-static ssize_t ad5446_read_powerdown_mode(struct iio_dev *indio_dev,
-					   uintptr_t private,
-					   const struct iio_chan_spec *chan,
-					   char *buf)
+static int ad5446_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
 	struct ad5446_state *st = iio_priv(indio_dev);
 
-	return sprintf(buf, "%s\n", ad5446_powerdown_modes[st->pwr_down_mode]);
+	return st->pwr_down_mode - 1;
 }
 
+static const struct iio_enum ad5446_powerdown_mode_enum = {
+	.items = ad5446_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5446_powerdown_modes),
+	.get = ad5446_get_powerdown_mode,
+	.set = ad5446_set_powerdown_mode,
+};
+
 static ssize_t ad5446_read_dac_powerdown(struct iio_dev *indio_dev,
 					   uintptr_t private,
 					   const struct iio_chan_spec *chan,
@@ -129,15 +115,9 @@ static const struct iio_chan_spec_ext_info ad5064_ext_info_powerdown[] = {
 		.name = "powerdown",
 		.read = ad5446_read_dac_powerdown,
 		.write = ad5446_write_dac_powerdown,
-	}, {
-		.name = "powerdown_mode",
-		.read = ad5446_read_powerdown_mode,
-		.write = ad5446_write_powerdown_mode,
-	}, {
-		.name = "powerdown_mode_available",
-		.shared = true,
-		.read = ad5446_read_powerdown_mode_available,
 	},
+	IIO_ENUM("powerdown_mode", false, &ad5446_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5446_powerdown_mode_enum),
 	{ },
 };
 
@@ -167,6 +147,14 @@ static const struct ad5446_chip_info ad5446_chip_info_tbl[] = {
 		.channel = AD5446_CHANNEL(14, 16, 0),
 		.write = ad5446_write,
 	},
+	[ID_AD5450] = {
+		.channel = AD5446_CHANNEL(8, 16, 6),
+		.write = ad5446_write,
+	},
+	[ID_AD5451] = {
+		.channel = AD5446_CHANNEL(10, 16, 4),
+		.write = ad5446_write,
+	},
 	[ID_AD5541A] = {
 		.channel = AD5446_CHANNEL(16, 16, 0),
 		.write = ad5446_write,
@@ -321,6 +309,8 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 	indio_dev->channels = &st->chip_info->channel;
 	indio_dev->num_channels = 1;
 
+	st->pwr_down_mode = MODE_PWRDWN_1k;
+
 	if (st->chip_info->int_vref_mv)
 		st->vref_mv = st->chip_info->int_vref_mv;
 	else if (voltage_uv)
@@ -364,6 +354,10 @@ static int ad5446_remove(struct spi_device *spi)
 static const struct spi_device_id ad5446_id[] = {
 	{"ad5444", ID_AD5444},
 	{"ad5446", ID_AD5446},
+	{"ad5450", ID_AD5450},
+	{"ad5451", ID_AD5451},
+	{"ad5452", ID_AD5444}, /* ad5452 is compatible to the ad5444 */
+	{"ad5453", ID_AD5446}, /* ad5453 is compatible to the ad5446 */
 	{"ad5512a", ID_AD5512A},
 	{"ad5541a", ID_AD5541A},
 	{"ad5542a", ID_AD5541A}, /* ad5541a and ad5542a are compatible */
diff --git a/drivers/staging/iio/dac/ad5446.h b/drivers/iio/dac/ad5446.h
index dfd68ce7427..2934269a56d 100644
--- a/drivers/staging/iio/dac/ad5446.h
+++ b/drivers/iio/dac/ad5446.h
@@ -71,6 +71,8 @@ struct ad5446_chip_info {
 enum ad5446_supported_device_ids {
 	ID_AD5444,
 	ID_AD5446,
+	ID_AD5450,
+	ID_AD5451,
 	ID_AD5541A,
 	ID_AD5512A,
 	ID_AD5553,
diff --git a/drivers/staging/iio/dac/ad5504.c b/drivers/iio/dac/ad5504.c
index 019cf15cd24..242bdc7d004 100644
--- a/drivers/staging/iio/dac/ad5504.c
+++ b/drivers/iio/dac/ad5504.c
@@ -19,25 +19,51 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/events.h>
-#include "dac.h"
-#include "ad5504.h"
+#include <linux/iio/dac/ad5504.h>
+
+#define AD5505_BITS			12
+#define AD5504_RES_MASK			((1 << (AD5505_BITS)) - 1)
+
+#define AD5504_CMD_READ			(1 << 15)
+#define AD5504_CMD_WRITE		(0 << 15)
+#define AD5504_ADDR(addr)		((addr) << 12)
+
+/* Registers */
+#define AD5504_ADDR_NOOP		0
+#define AD5504_ADDR_DAC(x)		((x) + 1)
+#define AD5504_ADDR_ALL_DAC		5
+#define AD5504_ADDR_CTRL		7
+
+/* Control Register */
+#define AD5504_DAC_PWR(ch)		((ch) << 2)
+#define AD5504_DAC_PWRDWN_MODE(mode)	((mode) << 6)
+#define AD5504_DAC_PWRDN_20K		0
+#define AD5504_DAC_PWRDN_3STATE		1
+
+/**
+ * struct ad5446_state - driver instance specific data
+ * @us:			spi_device
+ * @reg:		supply regulator
+ * @vref_mv:		actual reference voltage used
+ * @pwr_down_mask	power down mask
+ * @pwr_down_mode	current power down mode
+ */
 
-#define AD5504_CHANNEL(_chan) { \
-	.type = IIO_VOLTAGE, \
-	.indexed = 1, \
-	.output = 1, \
-	.channel = (_chan), \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
-	.address = AD5504_ADDR_DAC(_chan), \
-	.scan_type = IIO_ST('u', 12, 16, 0), \
-}
+struct ad5504_state {
+	struct spi_device		*spi;
+	struct regulator		*reg;
+	unsigned short			vref_mv;
+	unsigned			pwr_down_mask;
+	unsigned			pwr_down_mode;
+};
 
-static const struct iio_chan_spec ad5504_channels[] = {
-	AD5504_CHANNEL(0),
-	AD5504_CHANNEL(1),
-	AD5504_CHANNEL(2),
-	AD5504_CHANNEL(3),
+/**
+ * ad5504_supported_device_ids:
+ */
+
+enum ad5504_supported_device_ids {
+	ID_AD5504,
+	ID_AD5501,
 };
 
 static int ad5504_spi_write(struct spi_device *spi, u8 addr, u16 val)
@@ -122,67 +148,61 @@ static int ad5504_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static ssize_t ad5504_read_powerdown_mode(struct device *dev,
-				      struct device_attribute *attr, char *buf)
+static const char * const ad5504_powerdown_modes[] = {
+	"20kohm_to_gnd",
+	"three_state",
+};
+
+static int ad5504_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
 
-	const char mode[][14] = {"20kohm_to_gnd", "three_state"};
-
-	return sprintf(buf, "%s\n", mode[st->pwr_down_mode]);
+	return st->pwr_down_mode;
 }
 
-static ssize_t ad5504_write_powerdown_mode(struct device *dev,
-				       struct device_attribute *attr,
-				       const char *buf, size_t len)
+static int ad5504_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
-	int ret;
 
-	if (sysfs_streq(buf, "20kohm_to_gnd"))
-		st->pwr_down_mode = AD5504_DAC_PWRDN_20K;
-	else if (sysfs_streq(buf, "three_state"))
-		st->pwr_down_mode = AD5504_DAC_PWRDN_3STATE;
-	else
-		ret = -EINVAL;
+	st->pwr_down_mode = mode;
 
-	return ret ? ret : len;
+	return 0;
 }
 
-static ssize_t ad5504_read_dac_powerdown(struct device *dev,
-					   struct device_attribute *attr,
-					   char *buf)
+static const struct iio_enum ad5504_powerdown_mode_enum = {
+	.items = ad5504_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5504_powerdown_modes),
+	.get = ad5504_get_powerdown_mode,
+	.set = ad5504_set_powerdown_mode,
+};
+
+static ssize_t ad5504_read_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
 	return sprintf(buf, "%d\n",
-			!(st->pwr_down_mask & (1 << this_attr->address)));
+			!(st->pwr_down_mask & (1 << chan->channel)));
 }
 
-static ssize_t ad5504_write_dac_powerdown(struct device *dev,
-					    struct device_attribute *attr,
-					    const char *buf, size_t len)
+static ssize_t ad5504_write_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	size_t len)
 {
-	long readin;
+	bool pwr_down;
 	int ret;
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
-	ret = strict_strtol(buf, 10, &readin);
+	ret = strtobool(buf, &pwr_down);
 	if (ret)
 		return ret;
 
-	if (readin == 0)
-		st->pwr_down_mask |= (1 << this_attr->address);
-	else if (readin == 1)
-		st->pwr_down_mask &= ~(1 << this_attr->address);
+	if (pwr_down)
+		st->pwr_down_mask |= (1 << chan->channel);
 	else
-		ret = -EINVAL;
+		st->pwr_down_mask &= ~(1 << chan->channel);
 
 	ret = ad5504_spi_write(st->spi, AD5504_ADDR_CTRL,
 				AD5504_DAC_PWRDWN_MODE(st->pwr_down_mode) |
@@ -194,50 +214,6 @@ static ssize_t ad5504_write_dac_powerdown(struct device *dev,
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage_powerdown_mode, S_IRUGO |
-			S_IWUSR, ad5504_read_powerdown_mode,
-			ad5504_write_powerdown_mode, 0);
-
-static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
-			"20kohm_to_gnd three_state");
-
-#define IIO_DEV_ATTR_DAC_POWERDOWN(_num, _show, _store, _addr)		\
-	IIO_DEVICE_ATTR(out_voltage##_num##_powerdown,			\
-			S_IRUGO | S_IWUSR, _show, _store, _addr)
-static IIO_DEV_ATTR_DAC_POWERDOWN(0, ad5504_read_dac_powerdown,
-				   ad5504_write_dac_powerdown, 0);
-static IIO_DEV_ATTR_DAC_POWERDOWN(1, ad5504_read_dac_powerdown,
-				   ad5504_write_dac_powerdown, 1);
-static IIO_DEV_ATTR_DAC_POWERDOWN(2, ad5504_read_dac_powerdown,
-				   ad5504_write_dac_powerdown, 2);
-static IIO_DEV_ATTR_DAC_POWERDOWN(3, ad5504_read_dac_powerdown,
-				   ad5504_write_dac_powerdown, 3);
-
-static struct attribute *ad5504_attributes[] = {
-	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage3_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5504_attribute_group = {
-	.attrs = ad5504_attributes,
-};
-
-static struct attribute *ad5501_attributes[] = {
-	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5501_attribute_group = {
-	.attrs = ad5501_attributes,
-};
-
 static IIO_CONST_ATTR(temp0_thresh_rising_value, "110000");
 static IIO_CONST_ATTR(temp0_thresh_rising_en, "1");
 
@@ -267,17 +243,38 @@ static irqreturn_t ad5504_event_handler(int irq, void *private)
 static const struct iio_info ad5504_info = {
 	.write_raw = ad5504_write_raw,
 	.read_raw = ad5504_read_raw,
-	.attrs = &ad5504_attribute_group,
 	.event_attrs = &ad5504_ev_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
-static const struct iio_info ad5501_info = {
-	.write_raw = ad5504_write_raw,
-	.read_raw = ad5504_read_raw,
-	.attrs = &ad5501_attribute_group,
-	.event_attrs = &ad5504_ev_attribute_group,
-	.driver_module = THIS_MODULE,
+static const struct iio_chan_spec_ext_info ad5504_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = ad5504_read_dac_powerdown,
+		.write = ad5504_write_dac_powerdown,
+	},
+	IIO_ENUM("powerdown_mode", true, &ad5504_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5504_powerdown_mode_enum),
+	{ },
+};
+
+#define AD5504_CHANNEL(_chan) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.output = 1, \
+	.channel = (_chan), \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
+		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.address = AD5504_ADDR_DAC(_chan), \
+	.scan_type = IIO_ST('u', 12, 16, 0), \
+	.ext_info = ad5504_ext_info, \
+}
+
+static const struct iio_chan_spec ad5504_channels[] = {
+	AD5504_CHANNEL(0),
+	AD5504_CHANNEL(1),
+	AD5504_CHANNEL(2),
+	AD5504_CHANNEL(3),
 };
 
 static int __devinit ad5504_probe(struct spi_device *spi)
@@ -315,13 +312,11 @@ static int __devinit ad5504_probe(struct spi_device *spi)
 	st->spi = spi;
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(st->spi)->name;
-	if (spi_get_device_id(st->spi)->driver_data == ID_AD5501) {
-		indio_dev->info = &ad5501_info;
+	indio_dev->info = &ad5504_info;
+	if (spi_get_device_id(st->spi)->driver_data == ID_AD5501)
 		indio_dev->num_channels = 1;
-	} else {
-		indio_dev->info = &ad5504_info;
+	else
 		indio_dev->num_channels = 4;
-	}
 	indio_dev->channels = ad5504_channels;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
@@ -343,7 +338,8 @@ static int __devinit ad5504_probe(struct spi_device *spi)
 	return 0;
 
 error_free_irq:
-	free_irq(spi->irq, indio_dev);
+	if (spi->irq)
+		free_irq(spi->irq, indio_dev);
 error_disable_reg:
 	if (!IS_ERR(reg))
 		regulator_disable(reg);
diff --git a/drivers/staging/iio/dac/ad5624r.h b/drivers/iio/dac/ad5624r.h
index 5dca3028cdf..5dca3028cdf 100644
--- a/drivers/staging/iio/dac/ad5624r.h
+++ b/drivers/iio/dac/ad5624r.h
diff --git a/drivers/staging/iio/dac/ad5624r_spi.c b/drivers/iio/dac/ad5624r_spi.c
index 42ff644ac43..6a7d6a48cc6 100644
--- a/drivers/staging/iio/dac/ad5624r_spi.c
+++ b/drivers/iio/dac/ad5624r_spi.c
@@ -18,58 +18,8 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
-#include "ad5624r.h"
-
-#define AD5624R_CHANNEL(_chan, _bits) { \
-	.type = IIO_VOLTAGE, \
-	.indexed = 1, \
-	.output = 1, \
-	.channel = (_chan), \
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
-		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
-	.address = (_chan), \
-	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)), \
-}
-
-#define DECLARE_AD5624R_CHANNELS(_name, _bits) \
-	const struct iio_chan_spec _name##_channels[] = { \
-		AD5624R_CHANNEL(0, _bits), \
-		AD5624R_CHANNEL(1, _bits), \
-		AD5624R_CHANNEL(2, _bits), \
-		AD5624R_CHANNEL(3, _bits), \
-}
 
-static DECLARE_AD5624R_CHANNELS(ad5624r, 12);
-static DECLARE_AD5624R_CHANNELS(ad5644r, 14);
-static DECLARE_AD5624R_CHANNELS(ad5664r, 16);
-
-static const struct ad5624r_chip_info ad5624r_chip_info_tbl[] = {
-	[ID_AD5624R3] = {
-		.channels = ad5624r_channels,
-		.int_vref_mv = 1250,
-	},
-	[ID_AD5624R5] = {
-		.channels = ad5624r_channels,
-		.int_vref_mv = 2500,
-	},
-	[ID_AD5644R3] = {
-		.channels = ad5644r_channels,
-		.int_vref_mv = 1250,
-	},
-	[ID_AD5644R5] = {
-		.channels = ad5644r_channels,
-		.int_vref_mv = 2500,
-	},
-	[ID_AD5664R3] = {
-		.channels = ad5664r_channels,
-		.int_vref_mv = 1250,
-	},
-	[ID_AD5664R5] = {
-		.channels = ad5664r_channels,
-		.int_vref_mv = 2500,
-	},
-};
+#include "ad5624r.h"
 
 static int ad5624r_spi_write(struct spi_device *spi,
 			     u8 cmd, u8 addr, u16 val, u8 len)
@@ -138,69 +88,62 @@ static int ad5624r_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static ssize_t ad5624r_read_powerdown_mode(struct device *dev,
-				      struct device_attribute *attr, char *buf)
+static const char * const ad5624r_powerdown_modes[] = {
+	"1kohm_to_gnd",
+	"100kohm_to_gnd",
+	"three_state"
+};
+
+static int ad5624r_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
 
-	char mode[][15] = {"", "1kohm_to_gnd", "100kohm_to_gnd", "three_state"};
-
-	return sprintf(buf, "%s\n", mode[st->pwr_down_mode]);
+	return st->pwr_down_mode;
 }
 
-static ssize_t ad5624r_write_powerdown_mode(struct device *dev,
-				       struct device_attribute *attr,
-				       const char *buf, size_t len)
+static int ad5624r_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
-	int ret;
 
-	if (sysfs_streq(buf, "1kohm_to_gnd"))
-		st->pwr_down_mode = AD5624R_LDAC_PWRDN_1K;
-	else if (sysfs_streq(buf, "100kohm_to_gnd"))
-		st->pwr_down_mode = AD5624R_LDAC_PWRDN_100K;
-	else if (sysfs_streq(buf, "three_state"))
-		st->pwr_down_mode = AD5624R_LDAC_PWRDN_3STATE;
-	else
-		ret = -EINVAL;
+	st->pwr_down_mode = mode;
 
-	return ret ? ret : len;
+	return 0;
 }
 
-static ssize_t ad5624r_read_dac_powerdown(struct device *dev,
-					   struct device_attribute *attr,
-					   char *buf)
+static const struct iio_enum ad5624r_powerdown_mode_enum = {
+	.items = ad5624r_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5624r_powerdown_modes),
+	.get = ad5624r_get_powerdown_mode,
+	.set = ad5624r_set_powerdown_mode,
+};
+
+static ssize_t ad5624r_read_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
 	return sprintf(buf, "%d\n",
-			!!(st->pwr_down_mask & (1 << this_attr->address)));
+			!!(st->pwr_down_mask & (1 << chan->channel)));
 }
 
-static ssize_t ad5624r_write_dac_powerdown(struct device *dev,
-					    struct device_attribute *attr,
-					    const char *buf, size_t len)
+static ssize_t ad5624r_write_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	size_t len)
 {
-	long readin;
+	bool pwr_down;
 	int ret;
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
-	ret = strict_strtol(buf, 10, &readin);
+	ret = strtobool(buf, &pwr_down);
 	if (ret)
 		return ret;
 
-	if (readin == 1)
-		st->pwr_down_mask |= (1 << this_attr->address);
-	else if (!readin)
-		st->pwr_down_mask &= ~(1 << this_attr->address);
+	if (pwr_down)
+		st->pwr_down_mask |= (1 << chan->channel);
 	else
-		ret = -EINVAL;
+		st->pwr_down_mask &= ~(1 << chan->channel);
 
 	ret = ad5624r_spi_write(st->us, AD5624R_CMD_POWERDOWN_DAC, 0,
 				(st->pwr_down_mode << 4) |
@@ -209,47 +152,74 @@ static ssize_t ad5624r_write_dac_powerdown(struct device *dev,
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage_powerdown_mode, S_IRUGO |
-			S_IWUSR, ad5624r_read_powerdown_mode,
-			ad5624r_write_powerdown_mode, 0);
-
-static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
-			"1kohm_to_gnd 100kohm_to_gnd three_state");
-
-#define IIO_DEV_ATTR_DAC_POWERDOWN(_num, _show, _store, _addr)		\
-	IIO_DEVICE_ATTR(out_voltage##_num##_powerdown,			\
-			S_IRUGO | S_IWUSR, _show, _store, _addr)
-
-static IIO_DEV_ATTR_DAC_POWERDOWN(0, ad5624r_read_dac_powerdown,
-				   ad5624r_write_dac_powerdown, 0);
-static IIO_DEV_ATTR_DAC_POWERDOWN(1, ad5624r_read_dac_powerdown,
-				   ad5624r_write_dac_powerdown, 1);
-static IIO_DEV_ATTR_DAC_POWERDOWN(2, ad5624r_read_dac_powerdown,
-				   ad5624r_write_dac_powerdown, 2);
-static IIO_DEV_ATTR_DAC_POWERDOWN(3, ad5624r_read_dac_powerdown,
-				   ad5624r_write_dac_powerdown, 3);
-
-static struct attribute *ad5624r_attributes[] = {
-	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage3_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5624r_attribute_group = {
-	.attrs = ad5624r_attributes,
-};
-
 static const struct iio_info ad5624r_info = {
 	.write_raw = ad5624r_write_raw,
 	.read_raw = ad5624r_read_raw,
-	.attrs = &ad5624r_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
+static const struct iio_chan_spec_ext_info ad5624r_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = ad5624r_read_dac_powerdown,
+		.write = ad5624r_write_dac_powerdown,
+	},
+	IIO_ENUM("powerdown_mode", true, &ad5624r_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5624r_powerdown_mode_enum),
+	{ },
+};
+
+#define AD5624R_CHANNEL(_chan, _bits) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.output = 1, \
+	.channel = (_chan), \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
+		     IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.address = (_chan), \
+	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)), \
+	.ext_info = ad5624r_ext_info, \
+}
+
+#define DECLARE_AD5624R_CHANNELS(_name, _bits) \
+	const struct iio_chan_spec _name##_channels[] = { \
+		AD5624R_CHANNEL(0, _bits), \
+		AD5624R_CHANNEL(1, _bits), \
+		AD5624R_CHANNEL(2, _bits), \
+		AD5624R_CHANNEL(3, _bits), \
+}
+
+static DECLARE_AD5624R_CHANNELS(ad5624r, 12);
+static DECLARE_AD5624R_CHANNELS(ad5644r, 14);
+static DECLARE_AD5624R_CHANNELS(ad5664r, 16);
+
+static const struct ad5624r_chip_info ad5624r_chip_info_tbl[] = {
+	[ID_AD5624R3] = {
+		.channels = ad5624r_channels,
+		.int_vref_mv = 1250,
+	},
+	[ID_AD5624R5] = {
+		.channels = ad5624r_channels,
+		.int_vref_mv = 2500,
+	},
+	[ID_AD5644R3] = {
+		.channels = ad5644r_channels,
+		.int_vref_mv = 1250,
+	},
+	[ID_AD5644R5] = {
+		.channels = ad5644r_channels,
+		.int_vref_mv = 2500,
+	},
+	[ID_AD5664R3] = {
+		.channels = ad5664r_channels,
+		.int_vref_mv = 1250,
+	},
+	[ID_AD5664R5] = {
+		.channels = ad5664r_channels,
+		.int_vref_mv = 2500,
+	},
+};
+
 static int __devinit ad5624r_probe(struct spi_device *spi)
 {
 	struct ad5624r_state *st;
diff --git a/drivers/staging/iio/dac/ad5686.c b/drivers/iio/dac/ad5686.c
index c1e903ebc7b..6948d75e103 100644
--- a/drivers/staging/iio/dac/ad5686.c
+++ b/drivers/iio/dac/ad5686.c
@@ -18,7 +18,6 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
 
 #define AD5686_DAC_CHANNELS			4
 
@@ -93,40 +92,6 @@ enum ad5686_supported_device_ids {
 	ID_AD5685,
 	ID_AD5686,
 };
-#define AD5868_CHANNEL(chan, bits, shift) {			\
-		.type = IIO_VOLTAGE,				\
-		.indexed = 1,					\
-		.output = 1,					\
-		.channel = chan,				\
-		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
-		.address = AD5686_ADDR_DAC(chan),			\
-		.scan_type = IIO_ST('u', bits, 16, shift)	\
-}
-static const struct ad5686_chip_info ad5686_chip_info_tbl[] = {
-	[ID_AD5684] = {
-		.channel[0] = AD5868_CHANNEL(0, 12, 4),
-		.channel[1] = AD5868_CHANNEL(1, 12, 4),
-		.channel[2] = AD5868_CHANNEL(2, 12, 4),
-		.channel[3] = AD5868_CHANNEL(3, 12, 4),
-		.int_vref_mv = 2500,
-	},
-	[ID_AD5685] = {
-		.channel[0] = AD5868_CHANNEL(0, 14, 2),
-		.channel[1] = AD5868_CHANNEL(1, 14, 2),
-		.channel[2] = AD5868_CHANNEL(2, 14, 2),
-		.channel[3] = AD5868_CHANNEL(3, 14, 2),
-		.int_vref_mv = 2500,
-	},
-	[ID_AD5686] = {
-		.channel[0] = AD5868_CHANNEL(0, 16, 0),
-		.channel[1] = AD5868_CHANNEL(1, 16, 0),
-		.channel[2] = AD5868_CHANNEL(2, 16, 0),
-		.channel[3] = AD5868_CHANNEL(3, 16, 0),
-		.int_vref_mv = 2500,
-	},
-};
-
 static int ad5686_spi_write(struct ad5686_state *st,
 			     u8 cmd, u8 addr, u16 val, u8 shift)
 {
@@ -170,73 +135,63 @@ static int ad5686_spi_read(struct ad5686_state *st, u8 addr)
 	return be32_to_cpu(st->data[2].d32);
 }
 
-static ssize_t ad5686_read_powerdown_mode(struct device *dev,
-				      struct device_attribute *attr, char *buf)
+static const char * const ad5686_powerdown_modes[] = {
+	"1kohm_to_gnd",
+	"100kohm_to_gnd",
+	"three_state"
+};
+
+static int ad5686_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
-	char mode[][15] = {"", "1kohm_to_gnd", "100kohm_to_gnd", "three_state"};
-
-	return sprintf(buf, "%s\n", mode[(st->pwr_down_mode >>
-					 (this_attr->address * 2)) & 0x3]);
+	return ((st->pwr_down_mode >> (chan->channel * 2)) & 0x3) - 1;
 }
 
-static ssize_t ad5686_write_powerdown_mode(struct device *dev,
-				       struct device_attribute *attr,
-				       const char *buf, size_t len)
+static int ad5686_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	unsigned mode;
-
-	if (sysfs_streq(buf, "1kohm_to_gnd"))
-		mode = AD5686_LDAC_PWRDN_1K;
-	else if (sysfs_streq(buf, "100kohm_to_gnd"))
-		mode = AD5686_LDAC_PWRDN_100K;
-	else if (sysfs_streq(buf, "three_state"))
-		mode = AD5686_LDAC_PWRDN_3STATE;
-	else
-		return  -EINVAL;
 
-	st->pwr_down_mode &= ~(0x3 << (this_attr->address * 2));
-	st->pwr_down_mode |= (mode << (this_attr->address * 2));
+	st->pwr_down_mode &= ~(0x3 << (chan->channel * 2));
+	st->pwr_down_mode |= ((mode + 1) << (chan->channel * 2));
 
-	return len;
+	return 0;
 }
 
-static ssize_t ad5686_read_dac_powerdown(struct device *dev,
-					   struct device_attribute *attr,
-					   char *buf)
+static const struct iio_enum ad5686_powerdown_mode_enum = {
+	.items = ad5686_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5686_powerdown_modes),
+	.get = ad5686_get_powerdown_mode,
+	.set = ad5686_set_powerdown_mode,
+};
+
+static ssize_t ad5686_read_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
 	return sprintf(buf, "%d\n", !!(st->pwr_down_mask &
-			(0x3 << (this_attr->address * 2))));
+			(0x3 << (chan->channel * 2))));
 }
 
-static ssize_t ad5686_write_dac_powerdown(struct device *dev,
-					    struct device_attribute *attr,
-					    const char *buf, size_t len)
+static ssize_t ad5686_write_dac_powerdown(struct iio_dev *indio_dev,
+	 uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	 size_t len)
 {
 	bool readin;
 	int ret;
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5686_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 
 	ret = strtobool(buf, &readin);
 	if (ret)
 		return ret;
 
 	if (readin == true)
-		st->pwr_down_mask |= (0x3 << (this_attr->address * 2));
+		st->pwr_down_mask |= (0x3 << (chan->channel * 2));
 	else
-		st->pwr_down_mask &= ~(0x3 << (this_attr->address * 2));
+		st->pwr_down_mask &= ~(0x3 << (chan->channel * 2));
 
 	ret = ad5686_spi_write(st, AD5686_CMD_POWERDOWN_DAC, 0,
 			       st->pwr_down_mask & st->pwr_down_mode, 0);
@@ -244,48 +199,6 @@ static ssize_t ad5686_write_dac_powerdown(struct device *dev,
 	return ret ? ret : len;
 }
 
-static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
-			"1kohm_to_gnd 100kohm_to_gnd three_state");
-
-#define IIO_DEV_ATTR_DAC_POWERDOWN_MODE(_num)				\
-	IIO_DEVICE_ATTR(out_voltage##_num##_powerdown_mode,		\
-			S_IRUGO | S_IWUSR,				\
-			ad5686_read_powerdown_mode,			\
-			ad5686_write_powerdown_mode, _num)
-
-static IIO_DEV_ATTR_DAC_POWERDOWN_MODE(0);
-static IIO_DEV_ATTR_DAC_POWERDOWN_MODE(1);
-static IIO_DEV_ATTR_DAC_POWERDOWN_MODE(2);
-static IIO_DEV_ATTR_DAC_POWERDOWN_MODE(3);
-
-#define IIO_DEV_ATTR_DAC_POWERDOWN(_num)				\
-	IIO_DEVICE_ATTR(out_voltage##_num##_powerdown,			\
-			S_IRUGO | S_IWUSR,				\
-			ad5686_read_dac_powerdown,			\
-			ad5686_write_dac_powerdown, _num)
-
-static IIO_DEV_ATTR_DAC_POWERDOWN(0);
-static IIO_DEV_ATTR_DAC_POWERDOWN(1);
-static IIO_DEV_ATTR_DAC_POWERDOWN(2);
-static IIO_DEV_ATTR_DAC_POWERDOWN(3);
-
-static struct attribute *ad5686_attributes[] = {
-	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage3_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage0_powerdown_mode.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_powerdown_mode.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_powerdown_mode.dev_attr.attr,
-	&iio_dev_attr_out_voltage3_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5686_attribute_group = {
-	.attrs = ad5686_attributes,
-};
-
 static int ad5686_read_raw(struct iio_dev *indio_dev,
 			   struct iio_chan_spec const *chan,
 			   int *val,
@@ -349,10 +262,57 @@ static int ad5686_write_raw(struct iio_dev *indio_dev,
 static const struct iio_info ad5686_info = {
 	.read_raw = ad5686_read_raw,
 	.write_raw = ad5686_write_raw,
-	.attrs = &ad5686_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
+static const struct iio_chan_spec_ext_info ad5686_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = ad5686_read_dac_powerdown,
+		.write = ad5686_write_dac_powerdown,
+	},
+	IIO_ENUM("powerdown_mode", false, &ad5686_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5686_powerdown_mode_enum),
+	{ },
+};
+
+#define AD5868_CHANNEL(chan, bits, shift) {			\
+		.type = IIO_VOLTAGE,				\
+		.indexed = 1,					\
+		.output = 1,					\
+		.channel = chan,				\
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
+		.address = AD5686_ADDR_DAC(chan),			\
+		.scan_type = IIO_ST('u', bits, 16, shift),	\
+		.ext_info = ad5686_ext_info,			\
+}
+
+static const struct ad5686_chip_info ad5686_chip_info_tbl[] = {
+	[ID_AD5684] = {
+		.channel[0] = AD5868_CHANNEL(0, 12, 4),
+		.channel[1] = AD5868_CHANNEL(1, 12, 4),
+		.channel[2] = AD5868_CHANNEL(2, 12, 4),
+		.channel[3] = AD5868_CHANNEL(3, 12, 4),
+		.int_vref_mv = 2500,
+	},
+	[ID_AD5685] = {
+		.channel[0] = AD5868_CHANNEL(0, 14, 2),
+		.channel[1] = AD5868_CHANNEL(1, 14, 2),
+		.channel[2] = AD5868_CHANNEL(2, 14, 2),
+		.channel[3] = AD5868_CHANNEL(3, 14, 2),
+		.int_vref_mv = 2500,
+	},
+	[ID_AD5686] = {
+		.channel[0] = AD5868_CHANNEL(0, 16, 0),
+		.channel[1] = AD5868_CHANNEL(1, 16, 0),
+		.channel[2] = AD5868_CHANNEL(2, 16, 0),
+		.channel[3] = AD5868_CHANNEL(3, 16, 0),
+		.int_vref_mv = 2500,
+	},
+};
+
+
 static int __devinit ad5686_probe(struct spi_device *spi)
 {
 	struct ad5686_state *st;
@@ -385,6 +345,9 @@ static int __devinit ad5686_probe(struct spi_device *spi)
 
 	st->spi = spi;
 
+	/* Set all the power down mode for all channels to 1K pulldown */
+	st->pwr_down_mode = 0x55;
+
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(spi)->name;
 	indio_dev->info = &ad5686_info;
diff --git a/drivers/staging/iio/dac/ad5764.c b/drivers/iio/dac/ad5764.c
index 03dbd937b08..ffce3044744 100644
--- a/drivers/staging/iio/dac/ad5764.c
+++ b/drivers/iio/dac/ad5764.c
@@ -18,7 +18,6 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
 
 #define AD5764_REG_SF_NOP			0x0
 #define AD5764_REG_SF_CONFIG			0x1
diff --git a/drivers/staging/iio/dac/ad5791.c b/drivers/iio/dac/ad5791.c
index 13d8b5bb1ce..2bd2e37280f 100644
--- a/drivers/staging/iio/dac/ad5791.c
+++ b/drivers/iio/dac/ad5791.c
@@ -19,8 +19,90 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
-#include "ad5791.h"
+#include <linux/iio/dac/ad5791.h>
+
+#define AD5791_RES_MASK(x)		((1 << (x)) - 1)
+#define AD5791_DAC_MASK			AD5791_RES_MASK(20)
+#define AD5791_DAC_MSB			(1 << 19)
+
+#define AD5791_CMD_READ			(1 << 23)
+#define AD5791_CMD_WRITE		(0 << 23)
+#define AD5791_ADDR(addr)		((addr) << 20)
+
+/* Registers */
+#define AD5791_ADDR_NOOP		0
+#define AD5791_ADDR_DAC0		1
+#define AD5791_ADDR_CTRL		2
+#define AD5791_ADDR_CLRCODE		3
+#define AD5791_ADDR_SW_CTRL		4
+
+/* Control Register */
+#define AD5791_CTRL_RBUF		(1 << 1)
+#define AD5791_CTRL_OPGND		(1 << 2)
+#define AD5791_CTRL_DACTRI		(1 << 3)
+#define AD5791_CTRL_BIN2SC		(1 << 4)
+#define AD5791_CTRL_SDODIS		(1 << 5)
+#define AD5761_CTRL_LINCOMP(x)		((x) << 6)
+
+#define AD5791_LINCOMP_0_10		0
+#define AD5791_LINCOMP_10_12		1
+#define AD5791_LINCOMP_12_16		2
+#define AD5791_LINCOMP_16_19		3
+#define AD5791_LINCOMP_19_20		12
+
+#define AD5780_LINCOMP_0_10		0
+#define AD5780_LINCOMP_10_20		12
+
+/* Software Control Register */
+#define AD5791_SWCTRL_LDAC		(1 << 0)
+#define AD5791_SWCTRL_CLR		(1 << 1)
+#define AD5791_SWCTRL_RESET		(1 << 2)
+
+#define AD5791_DAC_PWRDN_6K		0
+#define AD5791_DAC_PWRDN_3STATE		1
+
+/**
+ * struct ad5791_chip_info - chip specific information
+ * @get_lin_comp:	function pointer to the device specific function
+ */
+
+struct ad5791_chip_info {
+	int (*get_lin_comp)	(unsigned int span);
+};
+
+/**
+ * struct ad5791_state - driver instance specific data
+ * @us:			spi_device
+ * @reg_vdd:		positive supply regulator
+ * @reg_vss:		negative supply regulator
+ * @chip_info:		chip model specific constants
+ * @vref_mv:		actual reference voltage used
+ * @vref_neg_mv:	voltage of the negative supply
+ * @pwr_down_mode	current power down mode
+ */
+
+struct ad5791_state {
+	struct spi_device		*spi;
+	struct regulator		*reg_vdd;
+	struct regulator		*reg_vss;
+	const struct ad5791_chip_info	*chip_info;
+	unsigned short			vref_mv;
+	unsigned int			vref_neg_mv;
+	unsigned			ctrl;
+	unsigned			pwr_down_mode;
+	bool				pwr_down;
+};
+
+/**
+ * ad5791_supported_device_ids:
+ */
+
+enum ad5791_supported_device_ids {
+	ID_AD5760,
+	ID_AD5780,
+	ID_AD5781,
+	ID_AD5791,
+};
 
 static int ad5791_spi_write(struct spi_device *spi, u8 addr, u32 val)
 {
@@ -72,119 +154,71 @@ static int ad5791_spi_read(struct spi_device *spi, u8 addr, u32 *val)
 	return ret;
 }
 
-#define AD5791_CHAN(bits, shift) {			\
-	.type = IIO_VOLTAGE,				\
-	.output = 1,					\
-	.indexed = 1,					\
-	.address = AD5791_ADDR_DAC0,			\
-	.channel = 0,					\
-	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
-		IIO_CHAN_INFO_SCALE_SHARED_BIT |	\
-		IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
-	.scan_type = IIO_ST('u', bits, 24, shift)	\
-}
-
-static const struct iio_chan_spec ad5791_channels[] = {
-	[ID_AD5760] = AD5791_CHAN(16, 4),
-	[ID_AD5780] = AD5791_CHAN(18, 2),
-	[ID_AD5781] = AD5791_CHAN(18, 2),
-	[ID_AD5791] = AD5791_CHAN(20, 0)
+static const char * const ad5791_powerdown_modes[] = {
+	"6kohm_to_gnd",
+	"three_state",
 };
 
-static ssize_t ad5791_read_powerdown_mode(struct device *dev,
-				      struct device_attribute *attr, char *buf)
+static int ad5791_get_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 
-	const char mode[][14] = {"6kohm_to_gnd", "three_state"};
-
-	return sprintf(buf, "%s\n", mode[st->pwr_down_mode]);
+	return st->pwr_down_mode;
 }
 
-static ssize_t ad5791_write_powerdown_mode(struct device *dev,
-				       struct device_attribute *attr,
-				       const char *buf, size_t len)
+static int ad5791_set_powerdown_mode(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, unsigned int mode)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
-	int ret;
 
-	if (sysfs_streq(buf, "6kohm_to_gnd"))
-		st->pwr_down_mode = AD5791_DAC_PWRDN_6K;
-	else if (sysfs_streq(buf, "three_state"))
-		st->pwr_down_mode = AD5791_DAC_PWRDN_3STATE;
-	else
-		ret = -EINVAL;
+	st->pwr_down_mode = mode;
 
-	return ret ? ret : len;
+	return 0;
 }
 
-static ssize_t ad5791_read_dac_powerdown(struct device *dev,
-					   struct device_attribute *attr,
-					   char *buf)
+static const struct iio_enum ad5791_powerdown_mode_enum = {
+	.items = ad5791_powerdown_modes,
+	.num_items = ARRAY_SIZE(ad5791_powerdown_modes),
+	.get = ad5791_get_powerdown_mode,
+	.set = ad5791_set_powerdown_mode,
+};
+
+static ssize_t ad5791_read_dac_powerdown(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "%d\n", st->pwr_down);
 }
 
-static ssize_t ad5791_write_dac_powerdown(struct device *dev,
-					    struct device_attribute *attr,
-					    const char *buf, size_t len)
+static ssize_t ad5791_write_dac_powerdown(struct iio_dev *indio_dev,
+	 uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	 size_t len)
 {
-	long readin;
+	bool pwr_down;
 	int ret;
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad5791_state *st = iio_priv(indio_dev);
 
-	ret = strict_strtol(buf, 10, &readin);
+	ret = strtobool(buf, &pwr_down);
 	if (ret)
 		return ret;
 
-	if (readin == 0) {
-		st->pwr_down = false;
+	if (!pwr_down) {
 		st->ctrl &= ~(AD5791_CTRL_OPGND | AD5791_CTRL_DACTRI);
-	} else if (readin == 1) {
-		st->pwr_down = true;
+	} else {
 		if (st->pwr_down_mode == AD5791_DAC_PWRDN_6K)
 			st->ctrl |= AD5791_CTRL_OPGND;
 		else if (st->pwr_down_mode == AD5791_DAC_PWRDN_3STATE)
 			st->ctrl |= AD5791_CTRL_DACTRI;
-	} else
-		ret = -EINVAL;
+	}
+	st->pwr_down = pwr_down;
 
 	ret = ad5791_spi_write(st->spi, AD5791_ADDR_CTRL, st->ctrl);
 
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage_powerdown_mode, S_IRUGO |
-			S_IWUSR, ad5791_read_powerdown_mode,
-			ad5791_write_powerdown_mode, 0);
-
-static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
-			"6kohm_to_gnd three_state");
-
-#define IIO_DEV_ATTR_DAC_POWERDOWN(_num, _show, _store, _addr)		\
-	IIO_DEVICE_ATTR(out_voltage##_num##_powerdown,			\
-			S_IRUGO | S_IWUSR, _show, _store, _addr)
-
-static IIO_DEV_ATTR_DAC_POWERDOWN(0, ad5791_read_dac_powerdown,
-				   ad5791_write_dac_powerdown, 0);
-
-static struct attribute *ad5791_attributes[] = {
-	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
-	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
-	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5791_attribute_group = {
-	.attrs = ad5791_attributes,
-};
-
 static int ad5791_get_lin_comp(unsigned int span)
 {
 	if (span <= 10000)
@@ -254,6 +288,37 @@ static int ad5791_read_raw(struct iio_dev *indio_dev,
 
 };
 
+static const struct iio_chan_spec_ext_info ad5791_ext_info[] = {
+	{
+		.name = "powerdown",
+		.shared = true,
+		.read = ad5791_read_dac_powerdown,
+		.write = ad5791_write_dac_powerdown,
+	},
+	IIO_ENUM("powerdown_mode", true, &ad5791_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &ad5791_powerdown_mode_enum),
+	{ },
+};
+
+#define AD5791_CHAN(bits, shift) {			\
+	.type = IIO_VOLTAGE,				\
+	.output = 1,					\
+	.indexed = 1,					\
+	.address = AD5791_ADDR_DAC0,			\
+	.channel = 0,					\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |	\
+		IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
+	.scan_type = IIO_ST('u', bits, 24, shift),	\
+	.ext_info = ad5791_ext_info,			\
+}
+
+static const struct iio_chan_spec ad5791_channels[] = {
+	[ID_AD5760] = AD5791_CHAN(16, 4),
+	[ID_AD5780] = AD5791_CHAN(18, 2),
+	[ID_AD5781] = AD5791_CHAN(18, 2),
+	[ID_AD5791] = AD5791_CHAN(20, 0)
+};
 
 static int ad5791_write_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan,
@@ -278,7 +343,6 @@ static int ad5791_write_raw(struct iio_dev *indio_dev,
 static const struct iio_info ad5791_info = {
 	.read_raw = &ad5791_read_raw,
 	.write_raw = &ad5791_write_raw,
-	.attrs = &ad5791_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/dac/max517.c b/drivers/iio/dac/max517.c
index 5287cad1f3a..c3d748c2593 100644
--- a/drivers/staging/iio/dac/max517.c
+++ b/drivers/iio/dac/max517.c
@@ -27,9 +27,7 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
-#include "dac.h"
-
-#include "max517.h"
+#include <linux/iio/dac/max517.h>
 
 #define MAX517_DRV_NAME	"max517"
 
@@ -45,7 +43,6 @@ enum max517_device_ids {
 };
 
 struct max517_data {
-	struct iio_dev		*indio_dev;
 	struct i2c_client	*client;
 	unsigned short		vref_mv[2];
 };
@@ -55,129 +52,67 @@ struct max517_data {
  *          bit 1: channel 2
  * (this way, it's possible to set both channels at once)
  */
-static ssize_t max517_set_value(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf, size_t count, int channel)
+static int max517_set_value(struct iio_dev *indio_dev,
+	long val, int channel)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct max517_data *data = iio_priv(indio_dev);
 	struct i2c_client *client = data->client;
-	u8 outbuf[4]; /* 1x or 2x command + value */
-	int outbuf_size = 0;
+	u8 outbuf[2];
 	int res;
-	long val;
-
-	res = strict_strtol(buf, 10, &val);
-
-	if (res)
-		return res;
 
 	if (val < 0 || val > 255)
 		return -EINVAL;
 
-	if (channel & 1) {
-		outbuf[outbuf_size++] = COMMAND_CHANNEL0;
-		outbuf[outbuf_size++] = val;
-	}
-	if (channel & 2) {
-		outbuf[outbuf_size++] = COMMAND_CHANNEL1;
-		outbuf[outbuf_size++] = val;
-	}
-
-	/*
-	 * At this point, there are always 1 or 2 two-byte commands in
-	 * outbuf. With 2 commands, the device can set two outputs
-	 * simultaneously, latching the values upon the end of the I2C
-	 * transfer.
-	 */
+	outbuf[0] = channel;
+	outbuf[1] = val;
 
-	res = i2c_master_send(client, outbuf, outbuf_size);
+	res = i2c_master_send(client, outbuf, 2);
 	if (res < 0)
 		return res;
-
-	return count;
-}
-
-static ssize_t max517_set_value_1(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf, size_t count)
-{
-	return max517_set_value(dev, attr, buf, count, 1);
-}
-static IIO_DEV_ATTR_OUT_RAW(1, max517_set_value_1, 0);
-
-static ssize_t max517_set_value_2(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf, size_t count)
-{
-	return max517_set_value(dev, attr, buf, count, 2);
-}
-static IIO_DEV_ATTR_OUT_RAW(2, max517_set_value_2, 1);
-
-static ssize_t max517_set_value_both(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf, size_t count)
-{
-	return max517_set_value(dev, attr, buf, count, 3);
+	else if (res != 2)
+		return -EIO;
+	else
+		return 0;
 }
-static IIO_DEVICE_ATTR_NAMED(out_voltage1and2_raw,
-			     out_voltage1&2_raw, S_IWUSR, NULL,
-			     max517_set_value_both, -1);
 
-static ssize_t max517_show_scale(struct device *dev,
-				struct device_attribute *attr,
-				char *buf, int channel)
+static int max517_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct max517_data *data = iio_priv(indio_dev);
-	/* Corresponds to Vref / 2^(bits) */
-	unsigned int scale_uv = (data->vref_mv[channel - 1] * 1000) >> 8;
-
-	return sprintf(buf, "%d.%03d\n", scale_uv / 1000, scale_uv % 1000);
+	unsigned int scale_uv;
+
+	switch (m) {
+	case IIO_CHAN_INFO_SCALE:
+		/* Corresponds to Vref / 2^(bits) */
+		scale_uv = (data->vref_mv[chan->channel] * 1000) >> 8;
+		*val =  scale_uv / 1000000;
+		*val2 = scale_uv % 1000000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		break;
+	}
+	return -EINVAL;
 }
 
-static ssize_t max517_show_scale1(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
+static int max517_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long mask)
 {
-	return max517_show_scale(dev, attr, buf, 1);
-}
-static IIO_DEVICE_ATTR(out_voltage1_scale, S_IRUGO,
-		       max517_show_scale1, NULL, 0);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = max517_set_value(indio_dev, val, chan->channel);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
 
-static ssize_t max517_show_scale2(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	return max517_show_scale(dev, attr, buf, 2);
+	return ret;
 }
-static IIO_DEVICE_ATTR(out_voltage2_scale, S_IRUGO,
-		       max517_show_scale2, NULL, 0);
-
-/* On MAX517 variant, we have one output */
-static struct attribute *max517_attributes[] = {
-	&iio_dev_attr_out_voltage1_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_scale.dev_attr.attr,
-	NULL
-};
-
-static struct attribute_group max517_attribute_group = {
-	.attrs = max517_attributes,
-};
-
-/* On MAX518 and MAX519 variant, we have two outputs */
-static struct attribute *max518_attributes[] = {
-	&iio_dev_attr_out_voltage1_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_scale.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_scale.dev_attr.attr,
-	&iio_dev_attr_out_voltage1and2_raw.dev_attr.attr,
-	NULL
-};
-
-static struct attribute_group max518_attribute_group = {
-	.attrs = max518_attributes,
-};
 
 #ifdef CONFIG_PM_SLEEP
 static int max517_suspend(struct device *dev)
@@ -201,16 +136,27 @@ static SIMPLE_DEV_PM_OPS(max517_pm_ops, max517_suspend, max517_resume);
 #endif
 
 static const struct iio_info max517_info = {
-	.attrs = &max517_attribute_group,
+	.read_raw = max517_read_raw,
+	.write_raw = max517_write_raw,
 	.driver_module = THIS_MODULE,
 };
 
-static const struct iio_info max518_info = {
-	.attrs = &max518_attribute_group,
-	.driver_module = THIS_MODULE,
+#define MAX517_CHANNEL(chan) {				\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.output = 1,					\
+	.channel = (chan),				\
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |	\
+	IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
+	.scan_type = IIO_ST('u', 8, 8, 0),		\
+}
+
+static const struct iio_chan_spec max517_channels[] = {
+	MAX517_CHANNEL(0),
+	MAX517_CHANNEL(1)
 };
 
-static int max517_probe(struct i2c_client *client,
+static int __devinit max517_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct max517_data *data;
@@ -230,12 +176,14 @@ static int max517_probe(struct i2c_client *client,
 	/* establish that the iio_dev is a child of the i2c device */
 	indio_dev->dev.parent = &client->dev;
 
-	/* reduced attribute set for MAX517 */
+	/* reduced channel set for MAX517 */
 	if (id->driver_data == ID_MAX517)
-		indio_dev->info = &max517_info;
+		indio_dev->num_channels = 1;
 	else
-		indio_dev->info = &max518_info;
+		indio_dev->num_channels = 2;
+	indio_dev->channels = max517_channels;
 	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &max517_info;
 
 	/*
 	 * Reference voltage on MAX518 and default is 5V, else take vref_mv
@@ -262,7 +210,7 @@ exit:
 	return err;
 }
 
-static int max517_remove(struct i2c_client *client)
+static int __devexit max517_remove(struct i2c_client *client)
 {
 	iio_device_unregister(i2c_get_clientdata(client));
 	iio_device_free(i2c_get_clientdata(client));
@@ -284,7 +232,7 @@ static struct i2c_driver max517_driver = {
 		.pm		= MAX517_PM_OPS,
 	},
 	.probe		= max517_probe,
-	.remove		= max517_remove,
+	.remove		=  __devexit_p(max517_remove),
 	.id_table	= max517_id,
 };
 module_i2c_driver(max517_driver);
diff --git a/drivers/iio/dac/mcp4725.c b/drivers/iio/dac/mcp4725.c
new file mode 100644
index 00000000000..e0e168bd5b4
--- /dev/null
+++ b/drivers/iio/dac/mcp4725.c
@@ -0,0 +1,227 @@
+/*
+ * mcp4725.c - Support for Microchip MCP4725
+ *
+ * Copyright (C) 2012 Peter Meerwald <pmeerw@pmeerw.net>
+ *
+ * Based on max517 by Roland Stigge <stigge@antcom.de>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * driver for the Microchip I2C 12-bit digital-to-analog converter (DAC)
+ * (7-bit I2C slave address 0x60, the three LSBs can be configured in
+ * hardware)
+ *
+ * writing the DAC value to EEPROM is not supported
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#include <linux/iio/dac/mcp4725.h>
+
+#define MCP4725_DRV_NAME "mcp4725"
+
+struct mcp4725_data {
+	struct i2c_client *client;
+	u16 vref_mv;
+	u16 dac_value;
+};
+
+#ifdef CONFIG_PM_SLEEP
+static int mcp4725_suspend(struct device *dev)
+{
+	u8 outbuf[2];
+
+	outbuf[0] = 0x3 << 4; /* power-down bits, 500 kOhm resistor */
+	outbuf[1] = 0;
+
+	return i2c_master_send(to_i2c_client(dev), outbuf, 2);
+}
+
+static int mcp4725_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct mcp4725_data *data = iio_priv(indio_dev);
+	u8 outbuf[2];
+
+	/* restore previous DAC value */
+	outbuf[0] = (data->dac_value >> 8) & 0xf;
+	outbuf[1] = data->dac_value & 0xff;
+
+	return i2c_master_send(to_i2c_client(dev), outbuf, 2);
+}
+
+static SIMPLE_DEV_PM_OPS(mcp4725_pm_ops, mcp4725_suspend, mcp4725_resume);
+#define MCP4725_PM_OPS (&mcp4725_pm_ops)
+#else
+#define MCP4725_PM_OPS NULL
+#endif
+
+static const struct iio_chan_spec mcp4725_channel = {
+	.type		= IIO_VOLTAGE,
+	.indexed	= 1,
+	.output		= 1,
+	.channel	= 0,
+	.info_mask	= IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			  IIO_CHAN_INFO_SCALE_SHARED_BIT,
+	.scan_type	= IIO_ST('u', 12, 16, 0),
+};
+
+static int mcp4725_set_value(struct iio_dev *indio_dev, int val)
+{
+	struct mcp4725_data *data = iio_priv(indio_dev);
+	u8 outbuf[2];
+	int ret;
+
+	if (val >= (1 << 12) || val < 0)
+		return -EINVAL;
+
+	outbuf[0] = (val >> 8) & 0xf;
+	outbuf[1] = val & 0xff;
+
+	ret = i2c_master_send(data->client, outbuf, 2);
+	if (ret < 0)
+		return ret;
+	else if (ret != 2)
+		return -EIO;
+	else
+		return 0;
+}
+
+static int mcp4725_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	struct mcp4725_data *data = iio_priv(indio_dev);
+	unsigned long scale_uv;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		*val = data->dac_value;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		scale_uv = (data->vref_mv * 1000) >> 12;
+		*val =  scale_uv / 1000000;
+		*val2 = scale_uv % 1000000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+	return -EINVAL;
+}
+
+static int mcp4725_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val, int val2, long mask)
+{
+	struct mcp4725_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = mcp4725_set_value(indio_dev, val);
+		data->dac_value = val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct iio_info mcp4725_info = {
+	.read_raw = mcp4725_read_raw,
+	.write_raw = mcp4725_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit mcp4725_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct mcp4725_data *data;
+	struct iio_dev *indio_dev;
+	struct mcp4725_platform_data *platform_data = client->dev.platform_data;
+	u8 inbuf[3];
+	int err;
+
+	if (!platform_data || !platform_data->vref_mv) {
+		dev_err(&client->dev, "invalid platform data");
+		err = -EINVAL;
+		goto exit;
+	}
+
+	indio_dev = iio_device_alloc(sizeof(*data));
+	if (indio_dev == NULL) {
+		err = -ENOMEM;
+		goto exit;
+	}
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &mcp4725_info;
+	indio_dev->channels = &mcp4725_channel;
+	indio_dev->num_channels = 1;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	data->vref_mv = platform_data->vref_mv;
+
+	/* read current DAC value */
+	err = i2c_master_recv(client, inbuf, 3);
+	if (err < 0) {
+		dev_err(&client->dev, "failed to read DAC value");
+		goto exit_free_device;
+	}
+	data->dac_value = (inbuf[1] << 4) | (inbuf[2] >> 4);
+
+	err = iio_device_register(indio_dev);
+	if (err)
+		goto exit_free_device;
+
+	dev_info(&client->dev, "MCP4725 DAC registered\n");
+
+	return 0;
+
+exit_free_device:
+	iio_device_free(indio_dev);
+exit:
+	return err;
+}
+
+static int __devexit mcp4725_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id mcp4725_id[] = {
+	{ "mcp4725", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mcp4725_id);
+
+static struct i2c_driver mcp4725_driver = {
+	.driver = {
+		.name	= MCP4725_DRV_NAME,
+		.pm	= MCP4725_PM_OPS,
+	},
+	.probe		= mcp4725_probe,
+	.remove		= __devexit_p(mcp4725_remove),
+	.id_table	= mcp4725_id,
+};
+module_i2c_driver(mcp4725_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("MCP4725 12-bit DAC");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/frequency/Kconfig b/drivers/iio/frequency/Kconfig
new file mode 100644
index 00000000000..6aaa33ef454
--- /dev/null
+++ b/drivers/iio/frequency/Kconfig
@@ -0,0 +1,41 @@
+#
+# Frequency
+#	Direct Digital Synthesis drivers (DDS)
+#	Clock Distribution device drivers
+#	Phase-Locked Loop (PLL) frequency synthesizers
+#
+
+menu "Frequency Synthesizers DDS/PLL"
+
+menu "Clock Generator/Distribution"
+
+config AD9523
+	tristate "Analog Devices AD9523 Low Jitter Clock Generator"
+	depends on SPI
+	help
+	  Say yes here to build support for Analog Devices AD9523 Low Jitter
+	  Clock Generator. The driver provides direct access via sysfs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad9523.
+
+endmenu
+
+#
+# Phase-Locked Loop (PLL) frequency synthesizers
+#
+
+menu "Phase-Locked Loop (PLL) frequency synthesizers"
+
+config ADF4350
+	tristate "Analog Devices ADF4350/ADF4351 Wideband Synthesizers"
+	depends on SPI
+	help
+	  Say yes here to build support for Analog Devices  ADF4350/ADF4351
+	  Wideband Synthesizers. The driver provides direct access via sysfs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adf4350.
+
+endmenu
+endmenu
diff --git a/drivers/iio/frequency/Makefile b/drivers/iio/frequency/Makefile
new file mode 100644
index 00000000000..00d26e5d1dc
--- /dev/null
+++ b/drivers/iio/frequency/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile iio/frequency
+#
+
+obj-$(CONFIG_AD9523) += ad9523.o
+obj-$(CONFIG_ADF4350) += adf4350.o
diff --git a/drivers/iio/frequency/ad9523.c b/drivers/iio/frequency/ad9523.c
new file mode 100644
index 00000000000..b737c64a402
--- /dev/null
+++ b/drivers/iio/frequency/ad9523.c
@@ -0,0 +1,1059 @@
+/*
+ * AD9523 SPI Low Jitter Clock Generator
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/frequency/ad9523.h>
+
+#define AD9523_READ	(1 << 15)
+#define AD9523_WRITE	(0 << 15)
+#define AD9523_CNT(x)	(((x) - 1) << 13)
+#define AD9523_ADDR(x)	((x) & 0xFFF)
+
+#define AD9523_R1B	(1 << 16)
+#define AD9523_R2B	(2 << 16)
+#define AD9523_R3B	(3 << 16)
+#define AD9523_TRANSF_LEN(x)			((x) >> 16)
+
+#define AD9523_SERIAL_PORT_CONFIG		(AD9523_R1B | 0x0)
+#define AD9523_VERSION_REGISTER			(AD9523_R1B | 0x2)
+#define AD9523_PART_REGISTER			(AD9523_R1B | 0x3)
+#define AD9523_READBACK_CTRL			(AD9523_R1B | 0x4)
+
+#define AD9523_EEPROM_CUSTOMER_VERSION_ID	(AD9523_R2B | 0x6)
+
+#define AD9523_PLL1_REF_A_DIVIDER		(AD9523_R2B | 0x11)
+#define AD9523_PLL1_REF_B_DIVIDER		(AD9523_R2B | 0x13)
+#define AD9523_PLL1_REF_TEST_DIVIDER		(AD9523_R1B | 0x14)
+#define AD9523_PLL1_FEEDBACK_DIVIDER		(AD9523_R2B | 0x17)
+#define AD9523_PLL1_CHARGE_PUMP_CTRL		(AD9523_R2B | 0x19)
+#define AD9523_PLL1_INPUT_RECEIVERS_CTRL	(AD9523_R1B | 0x1A)
+#define AD9523_PLL1_REF_CTRL			(AD9523_R1B | 0x1B)
+#define AD9523_PLL1_MISC_CTRL			(AD9523_R1B | 0x1C)
+#define AD9523_PLL1_LOOP_FILTER_CTRL		(AD9523_R1B | 0x1D)
+
+#define AD9523_PLL2_CHARGE_PUMP			(AD9523_R1B | 0xF0)
+#define AD9523_PLL2_FEEDBACK_DIVIDER_AB		(AD9523_R1B | 0xF1)
+#define AD9523_PLL2_CTRL			(AD9523_R1B | 0xF2)
+#define AD9523_PLL2_VCO_CTRL			(AD9523_R1B | 0xF3)
+#define AD9523_PLL2_VCO_DIVIDER			(AD9523_R1B | 0xF4)
+#define AD9523_PLL2_LOOP_FILTER_CTRL		(AD9523_R2B | 0xF6)
+#define AD9523_PLL2_R2_DIVIDER			(AD9523_R1B | 0xF7)
+
+#define AD9523_CHANNEL_CLOCK_DIST(ch)		(AD9523_R3B | (0x192 + 3 * ch))
+
+#define AD9523_PLL1_OUTPUT_CTRL			(AD9523_R1B | 0x1BA)
+#define AD9523_PLL1_OUTPUT_CHANNEL_CTRL		(AD9523_R1B | 0x1BB)
+
+#define AD9523_READBACK_0			(AD9523_R1B | 0x22C)
+#define AD9523_READBACK_1			(AD9523_R1B | 0x22D)
+
+#define AD9523_STATUS_SIGNALS			(AD9523_R3B | 0x232)
+#define AD9523_POWER_DOWN_CTRL			(AD9523_R1B | 0x233)
+#define AD9523_IO_UPDATE			(AD9523_R1B | 0x234)
+
+#define AD9523_EEPROM_DATA_XFER_STATUS		(AD9523_R1B | 0xB00)
+#define AD9523_EEPROM_ERROR_READBACK		(AD9523_R1B | 0xB01)
+#define AD9523_EEPROM_CTRL1			(AD9523_R1B | 0xB02)
+#define AD9523_EEPROM_CTRL2			(AD9523_R1B | 0xB03)
+
+/* AD9523_SERIAL_PORT_CONFIG */
+
+#define AD9523_SER_CONF_SDO_ACTIVE		(1 << 7)
+#define AD9523_SER_CONF_SOFT_RESET		(1 << 5)
+
+/* AD9523_READBACK_CTRL */
+#define AD9523_READBACK_CTRL_READ_BUFFERED	(1 << 0)
+
+/* AD9523_PLL1_CHARGE_PUMP_CTRL */
+#define AD9523_PLL1_CHARGE_PUMP_CURRENT_nA(x)	(((x) / 500) & 0x7F)
+#define AD9523_PLL1_CHARGE_PUMP_TRISTATE	(1 << 7)
+#define AD9523_PLL1_CHARGE_PUMP_MODE_NORMAL	(3 << 8)
+#define AD9523_PLL1_CHARGE_PUMP_MODE_PUMP_DOWN	(2 << 8)
+#define AD9523_PLL1_CHARGE_PUMP_MODE_PUMP_UP	(1 << 8)
+#define AD9523_PLL1_CHARGE_PUMP_MODE_TRISTATE	(0 << 8)
+#define AD9523_PLL1_BACKLASH_PW_MIN		(0 << 10)
+#define AD9523_PLL1_BACKLASH_PW_LOW		(1 << 10)
+#define AD9523_PLL1_BACKLASH_PW_HIGH		(2 << 10)
+#define AD9523_PLL1_BACKLASH_PW_MAX		(3 << 10)
+
+/* AD9523_PLL1_INPUT_RECEIVERS_CTRL */
+#define AD9523_PLL1_REF_TEST_RCV_EN		(1 << 7)
+#define AD9523_PLL1_REFB_DIFF_RCV_EN		(1 << 6)
+#define AD9523_PLL1_REFA_DIFF_RCV_EN		(1 << 5)
+#define AD9523_PLL1_REFB_RCV_EN			(1 << 4)
+#define AD9523_PLL1_REFA_RCV_EN			(1 << 3)
+#define AD9523_PLL1_REFA_REFB_PWR_CTRL_EN	(1 << 2)
+#define AD9523_PLL1_OSC_IN_CMOS_NEG_INP_EN	(1 << 1)
+#define AD9523_PLL1_OSC_IN_DIFF_EN		(1 << 0)
+
+/* AD9523_PLL1_REF_CTRL */
+#define AD9523_PLL1_BYPASS_REF_TEST_DIV_EN	(1 << 7)
+#define AD9523_PLL1_BYPASS_FEEDBACK_DIV_EN	(1 << 6)
+#define AD9523_PLL1_ZERO_DELAY_MODE_INT		(1 << 5)
+#define AD9523_PLL1_ZERO_DELAY_MODE_EXT		(0 << 5)
+#define AD9523_PLL1_OSC_IN_PLL_FEEDBACK_EN	(1 << 4)
+#define AD9523_PLL1_ZD_IN_CMOS_NEG_INP_EN	(1 << 3)
+#define AD9523_PLL1_ZD_IN_DIFF_EN		(1 << 2)
+#define AD9523_PLL1_REFB_CMOS_NEG_INP_EN	(1 << 1)
+#define AD9523_PLL1_REFA_CMOS_NEG_INP_EN	(1 << 0)
+
+/* AD9523_PLL1_MISC_CTRL */
+#define AD9523_PLL1_REFB_INDEP_DIV_CTRL_EN	(1 << 7)
+#define AD9523_PLL1_OSC_CTRL_FAIL_VCC_BY2_EN	(1 << 6)
+#define AD9523_PLL1_REF_MODE(x)			((x) << 2)
+#define AD9523_PLL1_BYPASS_REFB_DIV		(1 << 1)
+#define AD9523_PLL1_BYPASS_REFA_DIV		(1 << 0)
+
+/* AD9523_PLL1_LOOP_FILTER_CTRL */
+#define AD9523_PLL1_LOOP_FILTER_RZERO(x)	((x) & 0xF)
+
+/* AD9523_PLL2_CHARGE_PUMP */
+#define AD9523_PLL2_CHARGE_PUMP_CURRENT_nA(x)	((x) / 3500)
+
+/* AD9523_PLL2_FEEDBACK_DIVIDER_AB */
+#define AD9523_PLL2_FB_NDIV_A_CNT(x)		(((x) & 0x3) << 6)
+#define AD9523_PLL2_FB_NDIV_B_CNT(x)		(((x) & 0x3F) << 0)
+#define AD9523_PLL2_FB_NDIV(a, b)		(4 * (b) + (a))
+
+/* AD9523_PLL2_CTRL */
+#define AD9523_PLL2_CHARGE_PUMP_MODE_NORMAL	(3 << 0)
+#define AD9523_PLL2_CHARGE_PUMP_MODE_PUMP_DOWN	(2 << 0)
+#define AD9523_PLL2_CHARGE_PUMP_MODE_PUMP_UP	(1 << 0)
+#define AD9523_PLL2_CHARGE_PUMP_MODE_TRISTATE	(0 << 0)
+#define AD9523_PLL2_BACKLASH_PW_MIN		(0 << 2)
+#define AD9523_PLL2_BACKLASH_PW_LOW		(1 << 2)
+#define AD9523_PLL2_BACKLASH_PW_HIGH		(2 << 2)
+#define AD9523_PLL2_BACKLASH_PW_MAX		(3 << 1)
+#define AD9523_PLL2_BACKLASH_CTRL_EN		(1 << 4)
+#define AD9523_PLL2_FREQ_DOUBLER_EN		(1 << 5)
+#define AD9523_PLL2_LOCK_DETECT_PWR_DOWN_EN	(1 << 7)
+
+/* AD9523_PLL2_VCO_CTRL */
+#define AD9523_PLL2_VCO_CALIBRATE		(1 << 1)
+#define AD9523_PLL2_FORCE_VCO_MIDSCALE		(1 << 2)
+#define AD9523_PLL2_FORCE_REFERENCE_VALID	(1 << 3)
+#define AD9523_PLL2_FORCE_RELEASE_SYNC		(1 << 4)
+
+/* AD9523_PLL2_VCO_DIVIDER */
+#define AD9523_PLL2_VCO_DIV_M1(x)		((((x) - 3) & 0x3) << 0)
+#define AD9523_PLL2_VCO_DIV_M2(x)		((((x) - 3) & 0x3) << 4)
+#define AD9523_PLL2_VCO_DIV_M1_PWR_DOWN_EN	(1 << 2)
+#define AD9523_PLL2_VCO_DIV_M2_PWR_DOWN_EN	(1 << 6)
+
+/* AD9523_PLL2_LOOP_FILTER_CTRL */
+#define AD9523_PLL2_LOOP_FILTER_CPOLE1(x)	(((x) & 0x7) << 0)
+#define AD9523_PLL2_LOOP_FILTER_RZERO(x)	(((x) & 0x7) << 3)
+#define AD9523_PLL2_LOOP_FILTER_RPOLE2(x)	(((x) & 0x7) << 6)
+#define AD9523_PLL2_LOOP_FILTER_RZERO_BYPASS_EN	(1 << 8)
+
+/* AD9523_PLL2_R2_DIVIDER */
+#define AD9523_PLL2_R2_DIVIDER_VAL(x)		(((x) & 0x1F) << 0)
+
+/* AD9523_CHANNEL_CLOCK_DIST */
+#define AD9523_CLK_DIST_DIV_PHASE(x)		(((x) & 0x3F) << 18)
+#define AD9523_CLK_DIST_DIV_PHASE_REV(x)	((ret >> 18) & 0x3F)
+#define AD9523_CLK_DIST_DIV(x)			((((x) - 1) & 0x3FF) << 8)
+#define AD9523_CLK_DIST_DIV_REV(x)		(((ret >> 8) & 0x3FF) + 1)
+#define AD9523_CLK_DIST_INV_DIV_OUTPUT_EN	(1 << 7)
+#define AD9523_CLK_DIST_IGNORE_SYNC_EN		(1 << 6)
+#define AD9523_CLK_DIST_PWR_DOWN_EN		(1 << 5)
+#define AD9523_CLK_DIST_LOW_PWR_MODE_EN		(1 << 4)
+#define AD9523_CLK_DIST_DRIVER_MODE(x)		(((x) & 0xF) << 0)
+
+/* AD9523_PLL1_OUTPUT_CTRL */
+#define AD9523_PLL1_OUTP_CTRL_VCO_DIV_SEL_CH6_M2	(1 << 7)
+#define AD9523_PLL1_OUTP_CTRL_VCO_DIV_SEL_CH5_M2	(1 << 6)
+#define AD9523_PLL1_OUTP_CTRL_VCO_DIV_SEL_CH4_M2	(1 << 5)
+#define AD9523_PLL1_OUTP_CTRL_CMOS_DRV_WEAK		(1 << 4)
+#define AD9523_PLL1_OUTP_CTRL_OUTPUT_DIV_1		(0 << 0)
+#define AD9523_PLL1_OUTP_CTRL_OUTPUT_DIV_2		(1 << 0)
+#define AD9523_PLL1_OUTP_CTRL_OUTPUT_DIV_4		(2 << 0)
+#define AD9523_PLL1_OUTP_CTRL_OUTPUT_DIV_8		(4 << 0)
+#define AD9523_PLL1_OUTP_CTRL_OUTPUT_DIV_16		(8 << 0)
+
+/* AD9523_PLL1_OUTPUT_CHANNEL_CTRL */
+#define AD9523_PLL1_OUTP_CH_CTRL_OUTPUT_PWR_DOWN_EN	(1 << 7)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCO_DIV_SEL_CH9_M2	(1 << 6)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCO_DIV_SEL_CH8_M2	(1 << 5)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCO_DIV_SEL_CH7_M2	(1 << 4)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCXO_SRC_SEL_CH3	(1 << 3)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCXO_SRC_SEL_CH2	(1 << 2)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCXO_SRC_SEL_CH1	(1 << 1)
+#define AD9523_PLL1_OUTP_CH_CTRL_VCXO_SRC_SEL_CH0	(1 << 0)
+
+/* AD9523_READBACK_0 */
+#define AD9523_READBACK_0_STAT_PLL2_REF_CLK		(1 << 7)
+#define AD9523_READBACK_0_STAT_PLL2_FB_CLK		(1 << 6)
+#define AD9523_READBACK_0_STAT_VCXO			(1 << 5)
+#define AD9523_READBACK_0_STAT_REF_TEST			(1 << 4)
+#define AD9523_READBACK_0_STAT_REFB			(1 << 3)
+#define AD9523_READBACK_0_STAT_REFA			(1 << 2)
+#define AD9523_READBACK_0_STAT_PLL2_LD			(1 << 1)
+#define AD9523_READBACK_0_STAT_PLL1_LD			(1 << 0)
+
+/* AD9523_READBACK_1 */
+#define AD9523_READBACK_1_HOLDOVER_ACTIVE		(1 << 3)
+#define AD9523_READBACK_1_AUTOMODE_SEL_REFB		(1 << 2)
+#define AD9523_READBACK_1_VCO_CALIB_IN_PROGRESS		(1 << 0)
+
+/* AD9523_STATUS_SIGNALS */
+#define AD9523_STATUS_SIGNALS_SYNC_MAN_CTRL		(1 << 16)
+#define AD9523_STATUS_MONITOR_01_PLL12_LOCKED		(0x302)
+/* AD9523_POWER_DOWN_CTRL */
+#define AD9523_POWER_DOWN_CTRL_PLL1_PWR_DOWN		(1 << 2)
+#define AD9523_POWER_DOWN_CTRL_PLL2_PWR_DOWN		(1 << 1)
+#define AD9523_POWER_DOWN_CTRL_DIST_PWR_DOWN		(1 << 0)
+
+/* AD9523_IO_UPDATE */
+#define AD9523_IO_UPDATE_EN				(1 << 0)
+
+/* AD9523_EEPROM_DATA_XFER_STATUS */
+#define AD9523_EEPROM_DATA_XFER_IN_PROGRESS		(1 << 0)
+
+/* AD9523_EEPROM_ERROR_READBACK */
+#define AD9523_EEPROM_ERROR_READBACK_FAIL		(1 << 0)
+
+/* AD9523_EEPROM_CTRL1 */
+#define AD9523_EEPROM_CTRL1_SOFT_EEPROM			(1 << 1)
+#define AD9523_EEPROM_CTRL1_EEPROM_WRITE_PROT_DIS	(1 << 0)
+
+/* AD9523_EEPROM_CTRL2 */
+#define AD9523_EEPROM_CTRL2_REG2EEPROM			(1 << 0)
+
+#define AD9523_NUM_CHAN					14
+#define AD9523_NUM_CHAN_ALT_CLK_SRC			10
+
+/* Helpers to avoid excess line breaks */
+#define AD_IFE(_pde, _a, _b) ((pdata->_pde) ? _a : _b)
+#define AD_IF(_pde, _a) AD_IFE(_pde, _a, 0)
+
+enum {
+	AD9523_STAT_PLL1_LD,
+	AD9523_STAT_PLL2_LD,
+	AD9523_STAT_REFA,
+	AD9523_STAT_REFB,
+	AD9523_STAT_REF_TEST,
+	AD9523_STAT_VCXO,
+	AD9523_STAT_PLL2_FB_CLK,
+	AD9523_STAT_PLL2_REF_CLK,
+	AD9523_SYNC,
+	AD9523_EEPROM,
+};
+
+enum {
+	AD9523_VCO1,
+	AD9523_VCO2,
+	AD9523_VCXO,
+	AD9523_NUM_CLK_SRC,
+};
+
+struct ad9523_state {
+	struct spi_device		*spi;
+	struct regulator		*reg;
+	struct ad9523_platform_data	*pdata;
+	struct iio_chan_spec		ad9523_channels[AD9523_NUM_CHAN];
+
+	unsigned long		vcxo_freq;
+	unsigned long		vco_freq;
+	unsigned long		vco_out_freq[AD9523_NUM_CLK_SRC];
+	unsigned char		vco_out_map[AD9523_NUM_CHAN_ALT_CLK_SRC];
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	union {
+		__be32 d32;
+		u8 d8[4];
+	} data[2] ____cacheline_aligned;
+};
+
+static int ad9523_read(struct iio_dev *indio_dev, unsigned addr)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	struct spi_message m;
+	int ret;
+
+	/* We encode the register size 1..3 bytes into the register address.
+	 * On transfer we get the size from the register datum, and make sure
+	 * the result is properly aligned.
+	 */
+
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = &st->data[0].d8[2],
+			.len = 2,
+		}, {
+			.rx_buf = &st->data[1].d8[4 - AD9523_TRANSF_LEN(addr)],
+			.len = AD9523_TRANSF_LEN(addr),
+		},
+	};
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+
+	st->data[0].d32 = cpu_to_be32(AD9523_READ |
+				      AD9523_CNT(AD9523_TRANSF_LEN(addr)) |
+				      AD9523_ADDR(addr));
+
+	ret = spi_sync(st->spi, &m);
+	if (ret < 0)
+		dev_err(&indio_dev->dev, "read failed (%d)", ret);
+	else
+		ret = be32_to_cpu(st->data[1].d32) & (0xFFFFFF >>
+				  (8 * (3 - AD9523_TRANSF_LEN(addr))));
+
+	return ret;
+};
+
+static int ad9523_write(struct iio_dev *indio_dev, unsigned addr, unsigned val)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	struct spi_message m;
+	int ret;
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = &st->data[0].d8[2],
+			.len = 2,
+		}, {
+			.tx_buf = &st->data[1].d8[4 - AD9523_TRANSF_LEN(addr)],
+			.len = AD9523_TRANSF_LEN(addr),
+		},
+	};
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+
+	st->data[0].d32 = cpu_to_be32(AD9523_WRITE |
+				      AD9523_CNT(AD9523_TRANSF_LEN(addr)) |
+				      AD9523_ADDR(addr));
+	st->data[1].d32 = cpu_to_be32(val);
+
+	ret = spi_sync(st->spi, &m);
+
+	if (ret < 0)
+		dev_err(&indio_dev->dev, "write failed (%d)", ret);
+
+	return ret;
+}
+
+static int ad9523_io_update(struct iio_dev *indio_dev)
+{
+	return ad9523_write(indio_dev, AD9523_IO_UPDATE, AD9523_IO_UPDATE_EN);
+}
+
+static int ad9523_vco_out_map(struct iio_dev *indio_dev,
+			      unsigned ch, unsigned out)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	int ret;
+	unsigned mask;
+
+	switch (ch) {
+	case 0 ... 3:
+		ret = ad9523_read(indio_dev, AD9523_PLL1_OUTPUT_CHANNEL_CTRL);
+		if (ret < 0)
+			break;
+		mask = AD9523_PLL1_OUTP_CH_CTRL_VCXO_SRC_SEL_CH0 << ch;
+		if (out) {
+			ret |= mask;
+			out = 2;
+		} else {
+			ret &= ~mask;
+		}
+		ret = ad9523_write(indio_dev,
+				   AD9523_PLL1_OUTPUT_CHANNEL_CTRL, ret);
+		break;
+	case 4 ... 6:
+		ret = ad9523_read(indio_dev, AD9523_PLL1_OUTPUT_CTRL);
+		if (ret < 0)
+			break;
+		mask = AD9523_PLL1_OUTP_CTRL_VCO_DIV_SEL_CH4_M2 << (ch - 4);
+		if (out)
+			ret |= mask;
+		else
+			ret &= ~mask;
+		ret = ad9523_write(indio_dev, AD9523_PLL1_OUTPUT_CTRL, ret);
+		break;
+	case 7 ... 9:
+		ret = ad9523_read(indio_dev, AD9523_PLL1_OUTPUT_CHANNEL_CTRL);
+		if (ret < 0)
+			break;
+		mask = AD9523_PLL1_OUTP_CH_CTRL_VCO_DIV_SEL_CH7_M2 << (ch - 7);
+		if (out)
+			ret |= mask;
+		else
+			ret &= ~mask;
+		ret = ad9523_write(indio_dev,
+				   AD9523_PLL1_OUTPUT_CHANNEL_CTRL, ret);
+		break;
+	default:
+		return 0;
+	}
+
+	st->vco_out_map[ch] = out;
+
+	return ret;
+}
+
+static int ad9523_set_clock_provider(struct iio_dev *indio_dev,
+			      unsigned ch, unsigned long freq)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	long tmp1, tmp2;
+	bool use_alt_clk_src;
+
+	switch (ch) {
+	case 0 ... 3:
+		use_alt_clk_src = (freq == st->vco_out_freq[AD9523_VCXO]);
+		break;
+	case 4 ... 9:
+		tmp1 = st->vco_out_freq[AD9523_VCO1] / freq;
+		tmp2 = st->vco_out_freq[AD9523_VCO2] / freq;
+		tmp1 *= freq;
+		tmp2 *= freq;
+		use_alt_clk_src = (abs(tmp1 - freq) > abs(tmp2 - freq));
+		break;
+	default:
+		/* Ch 10..14: No action required, return success */
+		return 0;
+	}
+
+	return ad9523_vco_out_map(indio_dev, ch, use_alt_clk_src);
+}
+
+static int ad9523_store_eeprom(struct iio_dev *indio_dev)
+{
+	int ret, tmp;
+
+	ret = ad9523_write(indio_dev, AD9523_EEPROM_CTRL1,
+			   AD9523_EEPROM_CTRL1_EEPROM_WRITE_PROT_DIS);
+	if (ret < 0)
+		return ret;
+	ret = ad9523_write(indio_dev, AD9523_EEPROM_CTRL2,
+			   AD9523_EEPROM_CTRL2_REG2EEPROM);
+	if (ret < 0)
+		return ret;
+
+	tmp = 4;
+	do {
+		msleep(16);
+		ret = ad9523_read(indio_dev,
+				  AD9523_EEPROM_DATA_XFER_STATUS);
+		if (ret < 0)
+			return ret;
+	} while ((ret & AD9523_EEPROM_DATA_XFER_IN_PROGRESS) && tmp--);
+
+	ret = ad9523_write(indio_dev, AD9523_EEPROM_CTRL1, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_read(indio_dev, AD9523_EEPROM_ERROR_READBACK);
+	if (ret < 0)
+		return ret;
+
+	if (ret & AD9523_EEPROM_ERROR_READBACK_FAIL) {
+		dev_err(&indio_dev->dev, "Verify EEPROM failed");
+		ret = -EIO;
+	}
+
+	return ret;
+}
+
+static int ad9523_sync(struct iio_dev *indio_dev)
+{
+	int ret, tmp;
+
+	ret = ad9523_read(indio_dev, AD9523_STATUS_SIGNALS);
+	if (ret < 0)
+		return ret;
+
+	tmp = ret;
+	tmp |= AD9523_STATUS_SIGNALS_SYNC_MAN_CTRL;
+
+	ret = ad9523_write(indio_dev, AD9523_STATUS_SIGNALS, tmp);
+	if (ret < 0)
+		return ret;
+
+	ad9523_io_update(indio_dev);
+	tmp &= ~AD9523_STATUS_SIGNALS_SYNC_MAN_CTRL;
+
+	ret = ad9523_write(indio_dev, AD9523_STATUS_SIGNALS, tmp);
+	if (ret < 0)
+		return ret;
+
+	return ad9523_io_update(indio_dev);
+}
+
+static ssize_t ad9523_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	bool state;
+	int ret;
+
+	ret = strtobool(buf, &state);
+	if (ret < 0)
+		return ret;
+
+	if (!state)
+		return 0;
+
+	mutex_lock(&indio_dev->mlock);
+	switch ((u32)this_attr->address) {
+	case AD9523_SYNC:
+		ret = ad9523_sync(indio_dev);
+		break;
+	case AD9523_EEPROM:
+		ret = ad9523_store_eeprom(indio_dev);
+		break;
+	default:
+		ret = -ENODEV;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret ? ret : len;
+}
+
+static ssize_t ad9523_show(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	ret = ad9523_read(indio_dev, AD9523_READBACK_0);
+	if (ret >= 0) {
+		ret = sprintf(buf, "%d\n", !!(ret & (1 <<
+			(u32)this_attr->address)));
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static IIO_DEVICE_ATTR(pll1_locked, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_PLL1_LD);
+
+static IIO_DEVICE_ATTR(pll2_locked, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_PLL2_LD);
+
+static IIO_DEVICE_ATTR(pll1_reference_clk_a_present, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_REFA);
+
+static IIO_DEVICE_ATTR(pll1_reference_clk_b_present, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_REFB);
+
+static IIO_DEVICE_ATTR(pll1_reference_clk_test_present, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_REF_TEST);
+
+static IIO_DEVICE_ATTR(vcxo_clk_present, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_VCXO);
+
+static IIO_DEVICE_ATTR(pll2_feedback_clk_present, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_PLL2_FB_CLK);
+
+static IIO_DEVICE_ATTR(pll2_reference_clk_present, S_IRUGO,
+			ad9523_show,
+			NULL,
+			AD9523_STAT_PLL2_REF_CLK);
+
+static IIO_DEVICE_ATTR(sync_dividers, S_IWUSR,
+			NULL,
+			ad9523_store,
+			AD9523_SYNC);
+
+static IIO_DEVICE_ATTR(store_eeprom, S_IWUSR,
+			NULL,
+			ad9523_store,
+			AD9523_EEPROM);
+
+static struct attribute *ad9523_attributes[] = {
+	&iio_dev_attr_sync_dividers.dev_attr.attr,
+	&iio_dev_attr_store_eeprom.dev_attr.attr,
+	&iio_dev_attr_pll2_feedback_clk_present.dev_attr.attr,
+	&iio_dev_attr_pll2_reference_clk_present.dev_attr.attr,
+	&iio_dev_attr_pll1_reference_clk_a_present.dev_attr.attr,
+	&iio_dev_attr_pll1_reference_clk_b_present.dev_attr.attr,
+	&iio_dev_attr_pll1_reference_clk_test_present.dev_attr.attr,
+	&iio_dev_attr_vcxo_clk_present.dev_attr.attr,
+	&iio_dev_attr_pll1_locked.dev_attr.attr,
+	&iio_dev_attr_pll2_locked.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad9523_attribute_group = {
+	.attrs = ad9523_attributes,
+};
+
+static int ad9523_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	unsigned code;
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	ret = ad9523_read(indio_dev, AD9523_CHANNEL_CLOCK_DIST(chan->channel));
+	mutex_unlock(&indio_dev->mlock);
+
+	if (ret < 0)
+		return ret;
+
+	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		*val = !(ret & AD9523_CLK_DIST_PWR_DOWN_EN);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_FREQUENCY:
+		*val = st->vco_out_freq[st->vco_out_map[chan->channel]] /
+			AD9523_CLK_DIST_DIV_REV(ret);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_PHASE:
+		code = (AD9523_CLK_DIST_DIV_PHASE_REV(ret) * 3141592) /
+			AD9523_CLK_DIST_DIV_REV(ret);
+		*val = code / 1000000;
+		*val2 = (code % 1000000) * 10;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+};
+
+static int ad9523_write_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int val,
+			    int val2,
+			    long mask)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	unsigned reg;
+	int ret, tmp, code;
+
+	mutex_lock(&indio_dev->mlock);
+	ret = ad9523_read(indio_dev, AD9523_CHANNEL_CLOCK_DIST(chan->channel));
+	if (ret < 0)
+		goto out;
+
+	reg = ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (val)
+			reg &= ~AD9523_CLK_DIST_PWR_DOWN_EN;
+		else
+			reg |= AD9523_CLK_DIST_PWR_DOWN_EN;
+		break;
+	case IIO_CHAN_INFO_FREQUENCY:
+		if (val <= 0) {
+			ret = -EINVAL;
+			goto out;
+		}
+		ret = ad9523_set_clock_provider(indio_dev, chan->channel, val);
+		if (ret < 0)
+			goto out;
+		tmp = st->vco_out_freq[st->vco_out_map[chan->channel]] / val;
+		tmp = clamp(tmp, 1, 1024);
+		reg &= ~(0x3FF << 8);
+		reg |= AD9523_CLK_DIST_DIV(tmp);
+		break;
+	case IIO_CHAN_INFO_PHASE:
+		code = val * 1000000 + val2 % 1000000;
+		tmp = (code * AD9523_CLK_DIST_DIV_REV(ret)) / 3141592;
+		tmp = clamp(tmp, 0, 63);
+		reg &= ~AD9523_CLK_DIST_DIV_PHASE(~0);
+		reg |= AD9523_CLK_DIST_DIV_PHASE(tmp);
+		break;
+	default:
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = ad9523_write(indio_dev, AD9523_CHANNEL_CLOCK_DIST(chan->channel),
+			   reg);
+	if (ret < 0)
+		goto out;
+
+	ad9523_io_update(indio_dev);
+out:
+	mutex_unlock(&indio_dev->mlock);
+	return ret;
+}
+
+static int ad9523_reg_access(struct iio_dev *indio_dev,
+			      unsigned reg, unsigned writeval,
+			      unsigned *readval)
+{
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	if (readval == NULL) {
+		ret = ad9523_write(indio_dev, reg | AD9523_R1B, writeval);
+		ad9523_io_update(indio_dev);
+	} else {
+		ret = ad9523_read(indio_dev, reg | AD9523_R1B);
+		if (ret < 0)
+			goto out_unlock;
+		*readval = ret;
+		ret = 0;
+	}
+
+out_unlock:
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static const struct iio_info ad9523_info = {
+	.read_raw = &ad9523_read_raw,
+	.write_raw = &ad9523_write_raw,
+	.debugfs_reg_access = &ad9523_reg_access,
+	.attrs = &ad9523_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
+static int ad9523_setup(struct iio_dev *indio_dev)
+{
+	struct ad9523_state *st = iio_priv(indio_dev);
+	struct ad9523_platform_data *pdata = st->pdata;
+	struct ad9523_channel_spec *chan;
+	unsigned long active_mask = 0;
+	int ret, i;
+
+	ret = ad9523_write(indio_dev, AD9523_SERIAL_PORT_CONFIG,
+			   AD9523_SER_CONF_SOFT_RESET |
+			  (st->spi->mode & SPI_3WIRE ? 0 :
+			  AD9523_SER_CONF_SDO_ACTIVE));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_READBACK_CTRL,
+			  AD9523_READBACK_CTRL_READ_BUFFERED);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_io_update(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * PLL1 Setup
+	 */
+	ret = ad9523_write(indio_dev, AD9523_PLL1_REF_A_DIVIDER,
+		pdata->refa_r_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_REF_B_DIVIDER,
+		pdata->refb_r_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_FEEDBACK_DIVIDER,
+		pdata->pll1_feedback_div);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_CHARGE_PUMP_CTRL,
+		AD9523_PLL1_CHARGE_PUMP_CURRENT_nA(pdata->
+			pll1_charge_pump_current_nA) |
+		AD9523_PLL1_CHARGE_PUMP_MODE_NORMAL |
+		AD9523_PLL1_BACKLASH_PW_MIN);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_INPUT_RECEIVERS_CTRL,
+		AD_IF(refa_diff_rcv_en, AD9523_PLL1_REFA_RCV_EN) |
+		AD_IF(refb_diff_rcv_en, AD9523_PLL1_REFB_RCV_EN) |
+		AD_IF(osc_in_diff_en, AD9523_PLL1_OSC_IN_DIFF_EN) |
+		AD_IF(osc_in_cmos_neg_inp_en,
+		      AD9523_PLL1_OSC_IN_CMOS_NEG_INP_EN) |
+		AD_IF(refa_diff_rcv_en, AD9523_PLL1_REFA_DIFF_RCV_EN) |
+		AD_IF(refb_diff_rcv_en, AD9523_PLL1_REFB_DIFF_RCV_EN));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_REF_CTRL,
+		AD_IF(zd_in_diff_en, AD9523_PLL1_ZD_IN_DIFF_EN) |
+		AD_IF(zd_in_cmos_neg_inp_en,
+		      AD9523_PLL1_ZD_IN_CMOS_NEG_INP_EN) |
+		AD_IF(zero_delay_mode_internal_en,
+		      AD9523_PLL1_ZERO_DELAY_MODE_INT) |
+		AD_IF(osc_in_feedback_en, AD9523_PLL1_OSC_IN_PLL_FEEDBACK_EN) |
+		AD_IF(refa_cmos_neg_inp_en, AD9523_PLL1_REFA_CMOS_NEG_INP_EN) |
+		AD_IF(refb_cmos_neg_inp_en, AD9523_PLL1_REFB_CMOS_NEG_INP_EN));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_MISC_CTRL,
+		AD9523_PLL1_REFB_INDEP_DIV_CTRL_EN |
+		AD9523_PLL1_REF_MODE(pdata->ref_mode));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL1_LOOP_FILTER_CTRL,
+		AD9523_PLL1_LOOP_FILTER_RZERO(pdata->pll1_loop_filter_rzero));
+	if (ret < 0)
+		return ret;
+	/*
+	 * PLL2 Setup
+	 */
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_CHARGE_PUMP,
+		AD9523_PLL2_CHARGE_PUMP_CURRENT_nA(pdata->
+			pll2_charge_pump_current_nA));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_FEEDBACK_DIVIDER_AB,
+		AD9523_PLL2_FB_NDIV_A_CNT(pdata->pll2_ndiv_a_cnt) |
+		AD9523_PLL2_FB_NDIV_B_CNT(pdata->pll2_ndiv_b_cnt));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_CTRL,
+		AD9523_PLL2_CHARGE_PUMP_MODE_NORMAL |
+		AD9523_PLL2_BACKLASH_CTRL_EN |
+		AD_IF(pll2_freq_doubler_en, AD9523_PLL2_FREQ_DOUBLER_EN));
+	if (ret < 0)
+		return ret;
+
+	st->vco_freq = (pdata->vcxo_freq * (pdata->pll2_freq_doubler_en ? 2 : 1)
+			/ pdata->pll2_r2_div) * AD9523_PLL2_FB_NDIV(pdata->
+			pll2_ndiv_a_cnt, pdata->pll2_ndiv_b_cnt);
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_VCO_CTRL,
+		AD9523_PLL2_VCO_CALIBRATE);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_VCO_DIVIDER,
+		AD9523_PLL2_VCO_DIV_M1(pdata->pll2_vco_diff_m1) |
+		AD9523_PLL2_VCO_DIV_M2(pdata->pll2_vco_diff_m2) |
+		AD_IFE(pll2_vco_diff_m1, 0,
+		       AD9523_PLL2_VCO_DIV_M1_PWR_DOWN_EN) |
+		AD_IFE(pll2_vco_diff_m2, 0,
+		       AD9523_PLL2_VCO_DIV_M2_PWR_DOWN_EN));
+	if (ret < 0)
+		return ret;
+
+	if (pdata->pll2_vco_diff_m1)
+		st->vco_out_freq[AD9523_VCO1] =
+			st->vco_freq / pdata->pll2_vco_diff_m1;
+
+	if (pdata->pll2_vco_diff_m2)
+		st->vco_out_freq[AD9523_VCO2] =
+			st->vco_freq / pdata->pll2_vco_diff_m2;
+
+	st->vco_out_freq[AD9523_VCXO] = pdata->vcxo_freq;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_R2_DIVIDER,
+		AD9523_PLL2_R2_DIVIDER_VAL(pdata->pll2_r2_div));
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_PLL2_LOOP_FILTER_CTRL,
+		AD9523_PLL2_LOOP_FILTER_CPOLE1(pdata->cpole1) |
+		AD9523_PLL2_LOOP_FILTER_RZERO(pdata->rzero) |
+		AD9523_PLL2_LOOP_FILTER_RPOLE2(pdata->rpole2) |
+		AD_IF(rzero_bypass_en,
+		      AD9523_PLL2_LOOP_FILTER_RZERO_BYPASS_EN));
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < pdata->num_channels; i++) {
+		chan = &pdata->channels[i];
+		if (chan->channel_num < AD9523_NUM_CHAN) {
+			__set_bit(chan->channel_num, &active_mask);
+			ret = ad9523_write(indio_dev,
+				AD9523_CHANNEL_CLOCK_DIST(chan->channel_num),
+				AD9523_CLK_DIST_DRIVER_MODE(chan->driver_mode) |
+				AD9523_CLK_DIST_DIV(chan->channel_divider) |
+				AD9523_CLK_DIST_DIV_PHASE(chan->divider_phase) |
+				(chan->sync_ignore_en ?
+					AD9523_CLK_DIST_IGNORE_SYNC_EN : 0) |
+				(chan->divider_output_invert_en ?
+					AD9523_CLK_DIST_INV_DIV_OUTPUT_EN : 0) |
+				(chan->low_power_mode_en ?
+					AD9523_CLK_DIST_LOW_PWR_MODE_EN : 0) |
+				(chan->output_dis ?
+					AD9523_CLK_DIST_PWR_DOWN_EN : 0));
+			if (ret < 0)
+				return ret;
+
+			ret = ad9523_vco_out_map(indio_dev, chan->channel_num,
+					   chan->use_alt_clock_src);
+			if (ret < 0)
+				return ret;
+
+			st->ad9523_channels[i].type = IIO_ALTVOLTAGE;
+			st->ad9523_channels[i].output = 1;
+			st->ad9523_channels[i].indexed = 1;
+			st->ad9523_channels[i].channel = chan->channel_num;
+			st->ad9523_channels[i].extend_name =
+				chan->extended_name;
+			st->ad9523_channels[i].info_mask =
+				IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_PHASE_SEPARATE_BIT |
+				IIO_CHAN_INFO_FREQUENCY_SEPARATE_BIT;
+		}
+	}
+
+	for_each_clear_bit(i, &active_mask, AD9523_NUM_CHAN)
+		ad9523_write(indio_dev,
+			     AD9523_CHANNEL_CLOCK_DIST(i),
+			     AD9523_CLK_DIST_DRIVER_MODE(TRISTATE) |
+			     AD9523_CLK_DIST_PWR_DOWN_EN);
+
+	ret = ad9523_write(indio_dev, AD9523_POWER_DOWN_CTRL, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_write(indio_dev, AD9523_STATUS_SIGNALS,
+			   AD9523_STATUS_MONITOR_01_PLL12_LOCKED);
+	if (ret < 0)
+		return ret;
+
+	ret = ad9523_io_update(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int __devinit ad9523_probe(struct spi_device *spi)
+{
+	struct ad9523_platform_data *pdata = spi->dev.platform_data;
+	struct iio_dev *indio_dev;
+	struct ad9523_state *st;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&spi->dev, "no platform data?\n");
+		return -EINVAL;
+	}
+
+	indio_dev = iio_device_alloc(sizeof(*st));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->reg = regulator_get(&spi->dev, "vcc");
+	if (!IS_ERR(st->reg)) {
+		ret = regulator_enable(st->reg);
+		if (ret)
+			goto error_put_reg;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+	st->spi = spi;
+	st->pdata = pdata;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = (pdata->name[0] != 0) ? pdata->name :
+			  spi_get_device_id(spi)->name;
+	indio_dev->info = &ad9523_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = st->ad9523_channels;
+	indio_dev->num_channels = pdata->num_channels;
+
+	ret = ad9523_setup(indio_dev);
+	if (ret < 0)
+		goto error_disable_reg;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_disable_reg;
+
+	dev_info(&spi->dev, "probed %s\n", indio_dev->name);
+
+	return 0;
+
+error_disable_reg:
+	if (!IS_ERR(st->reg))
+		regulator_disable(st->reg);
+error_put_reg:
+	if (!IS_ERR(st->reg))
+		regulator_put(st->reg);
+
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad9523_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad9523_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (!IS_ERR(st->reg)) {
+		regulator_disable(st->reg);
+		regulator_put(st->reg);
+	}
+
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad9523_id[] = {
+	{"ad9523-1", 9523},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad9523_id);
+
+static struct spi_driver ad9523_driver = {
+	.driver = {
+		.name	= "ad9523",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ad9523_probe,
+	.remove		= __devexit_p(ad9523_remove),
+	.id_table	= ad9523_id,
+};
+module_spi_driver(ad9523_driver);
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("Analog Devices AD9523 CLOCKDIST/PLL");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/frequency/adf4350.c b/drivers/iio/frequency/adf4350.c
new file mode 100644
index 00000000000..59fbb3ae40e
--- /dev/null
+++ b/drivers/iio/frequency/adf4350.c
@@ -0,0 +1,480 @@
+/*
+ * ADF4350/ADF4351 SPI Wideband Synthesizer driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/gcd.h>
+#include <linux/gpio.h>
+#include <asm/div64.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/frequency/adf4350.h>
+
+enum {
+	ADF4350_FREQ,
+	ADF4350_FREQ_REFIN,
+	ADF4350_FREQ_RESOLUTION,
+	ADF4350_PWRDOWN,
+};
+
+struct adf4350_state {
+	struct spi_device		*spi;
+	struct regulator		*reg;
+	struct adf4350_platform_data	*pdata;
+	unsigned long			clkin;
+	unsigned long			chspc; /* Channel Spacing */
+	unsigned long			fpfd; /* Phase Frequency Detector */
+	unsigned long			min_out_freq;
+	unsigned			r0_fract;
+	unsigned			r0_int;
+	unsigned			r1_mod;
+	unsigned			r4_rf_div_sel;
+	unsigned long			regs[6];
+	unsigned long			regs_hw[6];
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	__be32				val ____cacheline_aligned;
+};
+
+static struct adf4350_platform_data default_pdata = {
+	.clkin = 122880000,
+	.channel_spacing = 10000,
+	.r2_user_settings = ADF4350_REG2_PD_POLARITY_POS |
+			    ADF4350_REG2_CHARGE_PUMP_CURR_uA(2500),
+	.r3_user_settings = ADF4350_REG3_12BIT_CLKDIV_MODE(0),
+	.r4_user_settings = ADF4350_REG4_OUTPUT_PWR(3) |
+			    ADF4350_REG4_MUTE_TILL_LOCK_EN,
+	.gpio_lock_detect = -1,
+};
+
+static int adf4350_sync_config(struct adf4350_state *st)
+{
+	int ret, i, doublebuf = 0;
+
+	for (i = ADF4350_REG5; i >= ADF4350_REG0; i--) {
+		if ((st->regs_hw[i] != st->regs[i]) ||
+			((i == ADF4350_REG0) && doublebuf)) {
+
+			switch (i) {
+			case ADF4350_REG1:
+			case ADF4350_REG4:
+				doublebuf = 1;
+				break;
+			}
+
+			st->val  = cpu_to_be32(st->regs[i] | i);
+			ret = spi_write(st->spi, &st->val, 4);
+			if (ret < 0)
+				return ret;
+			st->regs_hw[i] = st->regs[i];
+			dev_dbg(&st->spi->dev, "[%d] 0x%X\n",
+				i, (u32)st->regs[i] | i);
+		}
+	}
+	return 0;
+}
+
+static int adf4350_reg_access(struct iio_dev *indio_dev,
+			      unsigned reg, unsigned writeval,
+			      unsigned *readval)
+{
+	struct adf4350_state *st = iio_priv(indio_dev);
+	int ret;
+
+	if (reg > ADF4350_REG5)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+	if (readval == NULL) {
+		st->regs[reg] = writeval & ~(BIT(0) | BIT(1) | BIT(2));
+		ret = adf4350_sync_config(st);
+	} else {
+		*readval =  st->regs_hw[reg];
+		ret = 0;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int adf4350_tune_r_cnt(struct adf4350_state *st, unsigned short r_cnt)
+{
+	struct adf4350_platform_data *pdata = st->pdata;
+
+	do {
+		r_cnt++;
+		st->fpfd = (st->clkin * (pdata->ref_doubler_en ? 2 : 1)) /
+			   (r_cnt * (pdata->ref_div2_en ? 2 : 1));
+	} while (st->fpfd > ADF4350_MAX_FREQ_PFD);
+
+	return r_cnt;
+}
+
+static int adf4350_set_freq(struct adf4350_state *st, unsigned long long freq)
+{
+	struct adf4350_platform_data *pdata = st->pdata;
+	u64 tmp;
+	u32 div_gcd, prescaler;
+	u16 mdiv, r_cnt = 0;
+	u8 band_sel_div;
+
+	if (freq > ADF4350_MAX_OUT_FREQ || freq < st->min_out_freq)
+		return -EINVAL;
+
+	if (freq > ADF4350_MAX_FREQ_45_PRESC) {
+		prescaler = ADF4350_REG1_PRESCALER;
+		mdiv = 75;
+	} else {
+		prescaler = 0;
+		mdiv = 23;
+	}
+
+	st->r4_rf_div_sel = 0;
+
+	while (freq < ADF4350_MIN_VCO_FREQ) {
+		freq <<= 1;
+		st->r4_rf_div_sel++;
+	}
+
+	/*
+	 * Allow a predefined reference division factor
+	 * if not set, compute our own
+	 */
+	if (pdata->ref_div_factor)
+		r_cnt = pdata->ref_div_factor - 1;
+
+	do  {
+		r_cnt = adf4350_tune_r_cnt(st, r_cnt);
+
+		st->r1_mod = st->fpfd / st->chspc;
+		while (st->r1_mod > ADF4350_MAX_MODULUS) {
+			r_cnt = adf4350_tune_r_cnt(st, r_cnt);
+			st->r1_mod = st->fpfd / st->chspc;
+		}
+
+		tmp = freq * (u64)st->r1_mod + (st->fpfd > 1);
+		do_div(tmp, st->fpfd); /* Div round closest (n + d/2)/d */
+		st->r0_fract = do_div(tmp, st->r1_mod);
+		st->r0_int = tmp;
+	} while (mdiv > st->r0_int);
+
+	band_sel_div = DIV_ROUND_UP(st->fpfd, ADF4350_MAX_BANDSEL_CLK);
+
+	if (st->r0_fract && st->r1_mod) {
+		div_gcd = gcd(st->r1_mod, st->r0_fract);
+		st->r1_mod /= div_gcd;
+		st->r0_fract /= div_gcd;
+	} else {
+		st->r0_fract = 0;
+		st->r1_mod = 1;
+	}
+
+	dev_dbg(&st->spi->dev, "VCO: %llu Hz, PFD %lu Hz\n"
+		"REF_DIV %d, R0_INT %d, R0_FRACT %d\n"
+		"R1_MOD %d, RF_DIV %d\nPRESCALER %s, BAND_SEL_DIV %d\n",
+		freq, st->fpfd, r_cnt, st->r0_int, st->r0_fract, st->r1_mod,
+		1 << st->r4_rf_div_sel, prescaler ? "8/9" : "4/5",
+		band_sel_div);
+
+	st->regs[ADF4350_REG0] = ADF4350_REG0_INT(st->r0_int) |
+				 ADF4350_REG0_FRACT(st->r0_fract);
+
+	st->regs[ADF4350_REG1] = ADF4350_REG1_PHASE(0) |
+				 ADF4350_REG1_MOD(st->r1_mod) |
+				 prescaler;
+
+	st->regs[ADF4350_REG2] =
+		ADF4350_REG2_10BIT_R_CNT(r_cnt) |
+		ADF4350_REG2_DOUBLE_BUFF_EN |
+		(pdata->ref_doubler_en ? ADF4350_REG2_RMULT2_EN : 0) |
+		(pdata->ref_div2_en ? ADF4350_REG2_RDIV2_EN : 0) |
+		(pdata->r2_user_settings & (ADF4350_REG2_PD_POLARITY_POS |
+		ADF4350_REG2_LDP_6ns | ADF4350_REG2_LDF_INT_N |
+		ADF4350_REG2_CHARGE_PUMP_CURR_uA(5000) |
+		ADF4350_REG2_MUXOUT(0x7) | ADF4350_REG2_NOISE_MODE(0x9)));
+
+	st->regs[ADF4350_REG3] = pdata->r3_user_settings &
+				 (ADF4350_REG3_12BIT_CLKDIV(0xFFF) |
+				 ADF4350_REG3_12BIT_CLKDIV_MODE(0x3) |
+				 ADF4350_REG3_12BIT_CSR_EN |
+				 ADF4351_REG3_CHARGE_CANCELLATION_EN |
+				 ADF4351_REG3_ANTI_BACKLASH_3ns_EN |
+				 ADF4351_REG3_BAND_SEL_CLOCK_MODE_HIGH);
+
+	st->regs[ADF4350_REG4] =
+		ADF4350_REG4_FEEDBACK_FUND |
+		ADF4350_REG4_RF_DIV_SEL(st->r4_rf_div_sel) |
+		ADF4350_REG4_8BIT_BAND_SEL_CLKDIV(band_sel_div) |
+		ADF4350_REG4_RF_OUT_EN |
+		(pdata->r4_user_settings &
+		(ADF4350_REG4_OUTPUT_PWR(0x3) |
+		ADF4350_REG4_AUX_OUTPUT_PWR(0x3) |
+		ADF4350_REG4_AUX_OUTPUT_EN |
+		ADF4350_REG4_AUX_OUTPUT_FUND |
+		ADF4350_REG4_MUTE_TILL_LOCK_EN));
+
+	st->regs[ADF4350_REG5] = ADF4350_REG5_LD_PIN_MODE_DIGITAL;
+
+	return adf4350_sync_config(st);
+}
+
+static ssize_t adf4350_write(struct iio_dev *indio_dev,
+				    uintptr_t private,
+				    const struct iio_chan_spec *chan,
+				    const char *buf, size_t len)
+{
+	struct adf4350_state *st = iio_priv(indio_dev);
+	unsigned long long readin;
+	int ret;
+
+	ret = kstrtoull(buf, 10, &readin);
+	if (ret)
+		return ret;
+
+	mutex_lock(&indio_dev->mlock);
+	switch ((u32)private) {
+	case ADF4350_FREQ:
+		ret = adf4350_set_freq(st, readin);
+		break;
+	case ADF4350_FREQ_REFIN:
+		if (readin > ADF4350_MAX_FREQ_REFIN)
+			ret = -EINVAL;
+		else
+			st->clkin = readin;
+		break;
+	case ADF4350_FREQ_RESOLUTION:
+		if (readin == 0)
+			ret = -EINVAL;
+		else
+			st->chspc = readin;
+		break;
+	case ADF4350_PWRDOWN:
+		if (readin)
+			st->regs[ADF4350_REG2] |= ADF4350_REG2_POWER_DOWN_EN;
+		else
+			st->regs[ADF4350_REG2] &= ~ADF4350_REG2_POWER_DOWN_EN;
+
+		adf4350_sync_config(st);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret ? ret : len;
+}
+
+static ssize_t adf4350_read(struct iio_dev *indio_dev,
+				   uintptr_t private,
+				   const struct iio_chan_spec *chan,
+				   char *buf)
+{
+	struct adf4350_state *st = iio_priv(indio_dev);
+	unsigned long long val;
+	int ret = 0;
+
+	mutex_lock(&indio_dev->mlock);
+	switch ((u32)private) {
+	case ADF4350_FREQ:
+		val = (u64)((st->r0_int * st->r1_mod) + st->r0_fract) *
+			(u64)st->fpfd;
+		do_div(val, st->r1_mod * (1 << st->r4_rf_div_sel));
+		/* PLL unlocked? return error */
+		if (gpio_is_valid(st->pdata->gpio_lock_detect))
+			if (!gpio_get_value(st->pdata->gpio_lock_detect)) {
+				dev_dbg(&st->spi->dev, "PLL un-locked\n");
+				ret = -EBUSY;
+			}
+		break;
+	case ADF4350_FREQ_REFIN:
+		val = st->clkin;
+		break;
+	case ADF4350_FREQ_RESOLUTION:
+		val = st->chspc;
+		break;
+	case ADF4350_PWRDOWN:
+		val = !!(st->regs[ADF4350_REG2] & ADF4350_REG2_POWER_DOWN_EN);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret < 0 ? ret : sprintf(buf, "%llu\n", val);
+}
+
+#define _ADF4350_EXT_INFO(_name, _ident) { \
+	.name = _name, \
+	.read = adf4350_read, \
+	.write = adf4350_write, \
+	.private = _ident, \
+}
+
+static const struct iio_chan_spec_ext_info adf4350_ext_info[] = {
+	/* Ideally we use IIO_CHAN_INFO_FREQUENCY, but there are
+	 * values > 2^32 in order to support the entire frequency range
+	 * in Hz. Using scale is a bit ugly.
+	 */
+	_ADF4350_EXT_INFO("frequency", ADF4350_FREQ),
+	_ADF4350_EXT_INFO("frequency_resolution", ADF4350_FREQ_RESOLUTION),
+	_ADF4350_EXT_INFO("refin_frequency", ADF4350_FREQ_REFIN),
+	_ADF4350_EXT_INFO("powerdown", ADF4350_PWRDOWN),
+	{ },
+};
+
+static const struct iio_chan_spec adf4350_chan = {
+	.type = IIO_ALTVOLTAGE,
+	.indexed = 1,
+	.output = 1,
+	.ext_info = adf4350_ext_info,
+};
+
+static const struct iio_info adf4350_info = {
+	.debugfs_reg_access = &adf4350_reg_access,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit adf4350_probe(struct spi_device *spi)
+{
+	struct adf4350_platform_data *pdata = spi->dev.platform_data;
+	struct iio_dev *indio_dev;
+	struct adf4350_state *st;
+	int ret;
+
+	if (!pdata) {
+		dev_warn(&spi->dev, "no platform data? using default\n");
+
+		pdata = &default_pdata;
+	}
+
+	indio_dev = iio_device_alloc(sizeof(*st));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->reg = regulator_get(&spi->dev, "vcc");
+	if (!IS_ERR(st->reg)) {
+		ret = regulator_enable(st->reg);
+		if (ret)
+			goto error_put_reg;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+	st->spi = spi;
+	st->pdata = pdata;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = (pdata->name[0] != 0) ? pdata->name :
+		spi_get_device_id(spi)->name;
+
+	indio_dev->info = &adf4350_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = &adf4350_chan;
+	indio_dev->num_channels = 1;
+
+	st->chspc = pdata->channel_spacing;
+	st->clkin = pdata->clkin;
+
+	st->min_out_freq = spi_get_device_id(spi)->driver_data == 4351 ?
+		ADF4351_MIN_OUT_FREQ : ADF4350_MIN_OUT_FREQ;
+
+	memset(st->regs_hw, 0xFF, sizeof(st->regs_hw));
+
+	if (gpio_is_valid(pdata->gpio_lock_detect)) {
+		ret = gpio_request(pdata->gpio_lock_detect, indio_dev->name);
+		if (ret) {
+			dev_err(&spi->dev, "fail to request lock detect GPIO-%d",
+				pdata->gpio_lock_detect);
+			goto error_disable_reg;
+		}
+		gpio_direction_input(pdata->gpio_lock_detect);
+	}
+
+	if (pdata->power_up_frequency) {
+		ret = adf4350_set_freq(st, pdata->power_up_frequency);
+		if (ret)
+			goto error_free_gpio;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_free_gpio;
+
+	return 0;
+
+error_free_gpio:
+	if (gpio_is_valid(pdata->gpio_lock_detect))
+		gpio_free(pdata->gpio_lock_detect);
+
+error_disable_reg:
+	if (!IS_ERR(st->reg))
+		regulator_disable(st->reg);
+error_put_reg:
+	if (!IS_ERR(st->reg))
+		regulator_put(st->reg);
+
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit adf4350_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct adf4350_state *st = iio_priv(indio_dev);
+	struct regulator *reg = st->reg;
+
+	st->regs[ADF4350_REG2] |= ADF4350_REG2_POWER_DOWN_EN;
+	adf4350_sync_config(st);
+
+	iio_device_unregister(indio_dev);
+
+	if (!IS_ERR(reg)) {
+		regulator_disable(reg);
+		regulator_put(reg);
+	}
+
+	if (gpio_is_valid(st->pdata->gpio_lock_detect))
+		gpio_free(st->pdata->gpio_lock_detect);
+
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id adf4350_id[] = {
+	{"adf4350", 4350},
+	{"adf4351", 4351},
+	{}
+};
+
+static struct spi_driver adf4350_driver = {
+	.driver = {
+		.name	= "adf4350",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= adf4350_probe,
+	.remove		= __devexit_p(adf4350_remove),
+	.id_table	= adf4350_id,
+};
+module_spi_driver(adf4350_driver);
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("Analog Devices ADF4350/ADF4351 PLL");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index ac185b8694b..4add9bb40ee 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -285,11 +285,14 @@ int iio_buffer_register(struct iio_dev *indio_dev,
 	if (channels) {
 		/* new magic */
 		for (i = 0; i < num_channels; i++) {
+			if (channels[i].scan_index < 0)
+				continue;
+
 			/* Establish necessary mask length */
 			if (channels[i].scan_index >
 			    (int)indio_dev->masklength - 1)
 				indio_dev->masklength
-					= indio_dev->channels[i].scan_index + 1;
+					= channels[i].scan_index + 1;
 
 			ret = iio_buffer_add_channel_sysfs(indio_dev,
 							 &channels[i]);
@@ -553,6 +556,10 @@ int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
 					    buffer->scan_mask);
 	else
 		indio_dev->active_scan_mask = buffer->scan_mask;
+
+	if (indio_dev->active_scan_mask == NULL)
+		return -EINVAL;
+
 	iio_update_demux(indio_dev);
 
 	if (indio_dev->info->update_scan_mode)
@@ -564,6 +571,31 @@ int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
 EXPORT_SYMBOL(iio_sw_buffer_preenable);
 
 /**
+ * iio_validate_scan_mask_onehot() - Validates that exactly one channel is selected
+ * @indio_dev: the iio device
+ * @mask: scan mask to be checked
+ *
+ * Return true if exactly one bit is set in the scan mask, false otherwise. It
+ * can be used for devices where only one channel can be active for sampling at
+ * a time.
+ */
+bool iio_validate_scan_mask_onehot(struct iio_dev *indio_dev,
+	const unsigned long *mask)
+{
+	return bitmap_weight(mask, indio_dev->masklength) == 1;
+}
+EXPORT_SYMBOL_GPL(iio_validate_scan_mask_onehot);
+
+static bool iio_validate_scan_mask(struct iio_dev *indio_dev,
+	const unsigned long *mask)
+{
+	if (!indio_dev->setup_ops->validate_scan_mask)
+		return true;
+
+	return indio_dev->setup_ops->validate_scan_mask(indio_dev, mask);
+}
+
+/**
  * iio_scan_mask_set() - set particular bit in the scan mask
  * @buffer: the buffer whose scan mask we are interested in
  * @bit: the bit to be set.
@@ -582,27 +614,31 @@ int iio_scan_mask_set(struct iio_dev *indio_dev,
 		return -ENOMEM;
 	if (!indio_dev->masklength) {
 		WARN_ON("trying to set scanmask prior to registering buffer\n");
-		kfree(trialmask);
-		return -EINVAL;
+		goto err_invalid_mask;
 	}
 	bitmap_copy(trialmask, buffer->scan_mask, indio_dev->masklength);
 	set_bit(bit, trialmask);
 
+	if (!iio_validate_scan_mask(indio_dev, trialmask))
+		goto err_invalid_mask;
+
 	if (indio_dev->available_scan_masks) {
 		mask = iio_scan_mask_match(indio_dev->available_scan_masks,
 					   indio_dev->masklength,
 					   trialmask);
-		if (!mask) {
-			kfree(trialmask);
-			return -EINVAL;
-		}
+		if (!mask)
+			goto err_invalid_mask;
 	}
 	bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
 
 	kfree(trialmask);
 
 	return 0;
-};
+
+err_invalid_mask:
+	kfree(trialmask);
+	return -EINVAL;
+}
 EXPORT_SYMBOL_GPL(iio_scan_mask_set);
 
 int iio_scan_mask_query(struct iio_dev *indio_dev,
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 4f947e4377e..2ec266ef41a 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -64,14 +64,21 @@ static const char * const iio_chan_type_name_spec[] = {
 	[IIO_TIMESTAMP] = "timestamp",
 	[IIO_CAPACITANCE] = "capacitance",
 	[IIO_ALTVOLTAGE] = "altvoltage",
+	[IIO_CCT] = "cct",
 };
 
 static const char * const iio_modifier_names[] = {
 	[IIO_MOD_X] = "x",
 	[IIO_MOD_Y] = "y",
 	[IIO_MOD_Z] = "z",
+	[IIO_MOD_ROOT_SUM_SQUARED_X_Y] = "sqrt(x^2+y^2)",
+	[IIO_MOD_SUM_SQUARED_X_Y_Z] = "x^2+y^2+z^2",
 	[IIO_MOD_LIGHT_BOTH] = "both",
 	[IIO_MOD_LIGHT_IR] = "ir",
+	[IIO_MOD_LIGHT_CLEAR] = "clear",
+	[IIO_MOD_LIGHT_RED] = "red",
+	[IIO_MOD_LIGHT_GREEN] = "green",
+	[IIO_MOD_LIGHT_BLUE] = "blue",
 };
 
 /* relies on pairs of these shared then separate */
@@ -289,6 +296,69 @@ static ssize_t iio_write_channel_ext_info(struct device *dev,
 			       this_attr->c, buf, len);
 }
 
+ssize_t iio_enum_available_read(struct iio_dev *indio_dev,
+	uintptr_t priv, const struct iio_chan_spec *chan, char *buf)
+{
+	const struct iio_enum *e = (const struct iio_enum *)priv;
+	unsigned int i;
+	size_t len = 0;
+
+	if (!e->num_items)
+		return 0;
+
+	for (i = 0; i < e->num_items; ++i)
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%s ", e->items[i]);
+
+	/* replace last space with a newline */
+	buf[len - 1] = '\n';
+
+	return len;
+}
+EXPORT_SYMBOL_GPL(iio_enum_available_read);
+
+ssize_t iio_enum_read(struct iio_dev *indio_dev,
+	uintptr_t priv, const struct iio_chan_spec *chan, char *buf)
+{
+	const struct iio_enum *e = (const struct iio_enum *)priv;
+	int i;
+
+	if (!e->get)
+		return -EINVAL;
+
+	i = e->get(indio_dev, chan);
+	if (i < 0)
+		return i;
+	else if (i >= e->num_items)
+		return -EINVAL;
+
+	return sprintf(buf, "%s\n", e->items[i]);
+}
+EXPORT_SYMBOL_GPL(iio_enum_read);
+
+ssize_t iio_enum_write(struct iio_dev *indio_dev,
+	uintptr_t priv, const struct iio_chan_spec *chan, const char *buf,
+	size_t len)
+{
+	const struct iio_enum *e = (const struct iio_enum *)priv;
+	unsigned int i;
+	int ret;
+
+	if (!e->set)
+		return -EINVAL;
+
+	for (i = 0; i < e->num_items; i++) {
+		if (sysfs_streq(buf, e->items[i]))
+			break;
+	}
+
+	if (i == e->num_items)
+		return -EINVAL;
+
+	ret = e->set(indio_dev, chan, i);
+	return ret ? ret : len;
+}
+EXPORT_SYMBOL_GPL(iio_enum_write);
+
 static ssize_t iio_read_channel_info(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index b49059de5d0..fa6543bf673 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -345,7 +345,6 @@ static inline int __iio_add_event_config_attrs(struct iio_dev *indio_dev)
 {
 	int j, ret, attrcount = 0;
 
-	INIT_LIST_HEAD(&indio_dev->event_interface->dev_attr_list);
 	/* Dynically created from the channels array */
 	for (j = 0; j < indio_dev->num_channels; j++) {
 		ret = iio_device_add_event_sysfs(indio_dev,
@@ -396,6 +395,8 @@ int iio_device_register_eventset(struct iio_dev *indio_dev)
 		goto error_ret;
 	}
 
+	INIT_LIST_HEAD(&indio_dev->event_interface->dev_attr_list);
+
 	iio_setup_ev_int(indio_dev->event_interface);
 	if (indio_dev->info->event_attrs != NULL) {
 		attr = indio_dev->info->event_attrs->attrs;
diff --git a/drivers/iio/industrialio-trigger.c b/drivers/iio/industrialio-trigger.c
index 0f582df75a1..4fe0ead8421 100644
--- a/drivers/iio/industrialio-trigger.c
+++ b/drivers/iio/industrialio-trigger.c
@@ -45,31 +45,25 @@ static ssize_t iio_trigger_read_name(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct iio_trigger *trig = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	return sprintf(buf, "%s\n", trig->name);
 }
 
 static DEVICE_ATTR(name, S_IRUGO, iio_trigger_read_name, NULL);
 
-/**
- * iio_trigger_register_sysfs() - create a device for this trigger
- * @trig_info:	the trigger
- *
- * Also adds any control attribute registered by the trigger driver
- **/
-static int iio_trigger_register_sysfs(struct iio_trigger *trig_info)
-{
-	return sysfs_add_file_to_group(&trig_info->dev.kobj,
-				       &dev_attr_name.attr,
-				       NULL);
-}
+static struct attribute *iio_trig_dev_attrs[] = {
+	&dev_attr_name.attr,
+	NULL,
+};
 
-static void iio_trigger_unregister_sysfs(struct iio_trigger *trig_info)
-{
-	sysfs_remove_file_from_group(&trig_info->dev.kobj,
-					   &dev_attr_name.attr,
-					   NULL);
-}
+static struct attribute_group iio_trig_attr_group = {
+	.attrs	= iio_trig_dev_attrs,
+};
+
+static const struct attribute_group *iio_trig_attr_groups[] = {
+	&iio_trig_attr_group,
+	NULL
+};
 
 int iio_trigger_register(struct iio_trigger *trig_info)
 {
@@ -88,10 +82,6 @@ int iio_trigger_register(struct iio_trigger *trig_info)
 	if (ret)
 		goto error_unregister_id;
 
-	ret = iio_trigger_register_sysfs(trig_info);
-	if (ret)
-		goto error_device_del;
-
 	/* Add to list of available triggers held by the IIO core */
 	mutex_lock(&iio_trigger_list_lock);
 	list_add_tail(&trig_info->list, &iio_trigger_list);
@@ -99,8 +89,6 @@ int iio_trigger_register(struct iio_trigger *trig_info)
 
 	return 0;
 
-error_device_del:
-	device_del(&trig_info->dev);
 error_unregister_id:
 	ida_simple_remove(&iio_trigger_ida, trig_info->id);
 error_ret:
@@ -114,7 +102,6 @@ void iio_trigger_unregister(struct iio_trigger *trig_info)
 	list_del(&trig_info->list);
 	mutex_unlock(&iio_trigger_list_lock);
 
-	iio_trigger_unregister_sysfs(trig_info);
 	ida_simple_remove(&iio_trigger_ida, trig_info->id);
 	/* Possible issue in here */
 	device_unregister(&trig_info->dev);
@@ -234,7 +221,7 @@ static int iio_trigger_attach_poll_func(struct iio_trigger *trig,
 	return ret;
 }
 
-static int iio_trigger_dettach_poll_func(struct iio_trigger *trig,
+static int iio_trigger_detach_poll_func(struct iio_trigger *trig,
 					 struct iio_poll_func *pf)
 {
 	int ret = 0;
@@ -406,6 +393,7 @@ static void iio_trig_release(struct device *device)
 
 static struct device_type iio_trig_type = {
 	.release = iio_trig_release,
+	.groups = iio_trig_attr_groups,
 };
 
 static void iio_trig_subirqmask(struct irq_data *d)
@@ -436,7 +424,6 @@ struct iio_trigger *iio_trigger_alloc(const char *fmt, ...)
 		trig->dev.type = &iio_trig_type;
 		trig->dev.bus = &iio_bus_type;
 		device_initialize(&trig->dev);
-		dev_set_drvdata(&trig->dev, (void *)trig);
 
 		mutex_init(&trig->pool_lock);
 		trig->subirq_base
@@ -503,7 +490,7 @@ EXPORT_SYMBOL(iio_triggered_buffer_postenable);
 
 int iio_triggered_buffer_predisable(struct iio_dev *indio_dev)
 {
-	return iio_trigger_dettach_poll_func(indio_dev->trig,
+	return iio_trigger_detach_poll_func(indio_dev->trig,
 					     indio_dev->pollfunc);
 }
 EXPORT_SYMBOL(iio_triggered_buffer_predisable);
diff --git a/drivers/iio/industrialio-triggered-buffer.c b/drivers/iio/industrialio-triggered-buffer.c
new file mode 100644
index 00000000000..46c619b0d8c
--- /dev/null
+++ b/drivers/iio/industrialio-triggered-buffer.c
@@ -0,0 +1,110 @@
+ /*
+ * Copyright (c) 2012 Analog Devices, Inc.
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+static const struct iio_buffer_setup_ops iio_triggered_buffer_setup_ops = {
+	.preenable = &iio_sw_buffer_preenable,
+	.postenable = &iio_triggered_buffer_postenable,
+	.predisable = &iio_triggered_buffer_predisable,
+};
+
+/**
+ * iio_triggered_buffer_setup() - Setup triggered buffer and pollfunc
+ * @indio_dev:		IIO device structure
+ * @pollfunc_bh:	Function which will be used as pollfunc bottom half
+ * @pollfunc_th:	Function which will be used as pollfunc top half
+ * @setup_ops:		Buffer setup functions to use for this device.
+ *			If NULL the default setup functions for triggered
+ *			buffers will be used.
+ *
+ * This function combines some common tasks which will normally be performed
+ * when setting up a triggered buffer. It will allocate the buffer and the
+ * pollfunc, as well as register the buffer with the IIO core.
+ *
+ * Before calling this function the indio_dev structure should already be
+ * completely initialized, but not yet registered. In practice this means that
+ * this function should be called right before iio_device_register().
+ *
+ * To free the resources allocated by this function call
+ * iio_triggered_buffer_cleanup().
+ */
+int iio_triggered_buffer_setup(struct iio_dev *indio_dev,
+	irqreturn_t (*pollfunc_bh)(int irq, void *p),
+	irqreturn_t (*pollfunc_th)(int irq, void *p),
+	const struct iio_buffer_setup_ops *setup_ops)
+{
+	int ret;
+
+	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
+	if (!indio_dev->buffer) {
+		ret = -ENOMEM;
+		goto error_ret;
+	}
+
+	indio_dev->pollfunc = iio_alloc_pollfunc(pollfunc_bh,
+						 pollfunc_th,
+						 IRQF_ONESHOT,
+						 indio_dev,
+						 "%s_consumer%d",
+						 indio_dev->name,
+						 indio_dev->id);
+	if (indio_dev->pollfunc == NULL) {
+		ret = -ENOMEM;
+		goto error_kfifo_free;
+	}
+
+	/* Ring buffer functions - here trigger setup related */
+	if (setup_ops)
+		indio_dev->setup_ops = setup_ops;
+	else
+		indio_dev->setup_ops = &iio_triggered_buffer_setup_ops;
+
+	/* Flag that polled ring buffering is possible */
+	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
+
+	ret = iio_buffer_register(indio_dev,
+				  indio_dev->channels,
+				  indio_dev->num_channels);
+	if (ret)
+		goto error_dealloc_pollfunc;
+
+	return 0;
+
+error_dealloc_pollfunc:
+	iio_dealloc_pollfunc(indio_dev->pollfunc);
+error_kfifo_free:
+	iio_kfifo_free(indio_dev->buffer);
+error_ret:
+	return ret;
+}
+EXPORT_SYMBOL(iio_triggered_buffer_setup);
+
+/**
+ * iio_triggered_buffer_cleanup() - Free resources allocated by iio_triggered_buffer_setup()
+ * @indio_dev: IIO device structure
+ */
+void iio_triggered_buffer_cleanup(struct iio_dev *indio_dev)
+{
+	iio_buffer_unregister(indio_dev);
+	iio_dealloc_pollfunc(indio_dev->pollfunc);
+	iio_kfifo_free(indio_dev->buffer);
+}
+EXPORT_SYMBOL(iio_triggered_buffer_cleanup);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("IIO helper functions for setting up triggered buffers");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index 922645893dc..b5afc2ff34f 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -92,8 +92,7 @@ error_ret:
 EXPORT_SYMBOL_GPL(iio_map_array_unregister);
 
 static const struct iio_chan_spec
-*iio_chan_spec_from_name(const struct iio_dev *indio_dev,
-			 const char *name)
+*iio_chan_spec_from_name(const struct iio_dev *indio_dev, const char *name)
 {
 	int i;
 	const struct iio_chan_spec *chan = NULL;
@@ -108,8 +107,7 @@ static const struct iio_chan_spec
 }
 
 
-struct iio_channel *iio_st_channel_get(const char *name,
-				       const char *channel_name)
+struct iio_channel *iio_channel_get(const char *name, const char *channel_name)
 {
 	struct iio_map_internal *c_i = NULL, *c = NULL;
 	struct iio_channel *channel;
@@ -125,7 +123,7 @@ struct iio_channel *iio_st_channel_get(const char *name,
 		     strcmp(channel_name, c_i->map->consumer_channel) != 0))
 			continue;
 		c = c_i;
-		get_device(&c->indio_dev->dev);
+		iio_device_get(c->indio_dev);
 		break;
 	}
 	mutex_unlock(&iio_map_list_lock);
@@ -145,16 +143,16 @@ struct iio_channel *iio_st_channel_get(const char *name,
 
 	return channel;
 }
-EXPORT_SYMBOL_GPL(iio_st_channel_get);
+EXPORT_SYMBOL_GPL(iio_channel_get);
 
-void iio_st_channel_release(struct iio_channel *channel)
+void iio_channel_release(struct iio_channel *channel)
 {
-	put_device(&channel->indio_dev->dev);
+	iio_device_put(channel->indio_dev);
 	kfree(channel);
 }
-EXPORT_SYMBOL_GPL(iio_st_channel_release);
+EXPORT_SYMBOL_GPL(iio_channel_release);
 
-struct iio_channel *iio_st_channel_get_all(const char *name)
+struct iio_channel *iio_channel_get_all(const char *name)
 {
 	struct iio_channel *chans;
 	struct iio_map_internal *c = NULL;
@@ -195,44 +193,43 @@ struct iio_channel *iio_st_channel_get_all(const char *name)
 						c->map->adc_channel_label);
 		if (chans[mapind].channel == NULL) {
 			ret = -EINVAL;
-			put_device(&chans[mapind].indio_dev->dev);
 			goto error_free_chans;
 		}
-		get_device(&chans[mapind].indio_dev->dev);
+		iio_device_get(chans[mapind].indio_dev);
 		mapind++;
 	}
-	mutex_unlock(&iio_map_list_lock);
 	if (mapind == 0) {
 		ret = -ENODEV;
 		goto error_free_chans;
 	}
+	mutex_unlock(&iio_map_list_lock);
+
 	return chans;
 
 error_free_chans:
 	for (i = 0; i < nummaps; i++)
-		if (chans[i].indio_dev)
-			put_device(&chans[i].indio_dev->dev);
+		iio_device_put(chans[i].indio_dev);
 	kfree(chans);
 error_ret:
 	mutex_unlock(&iio_map_list_lock);
 
 	return ERR_PTR(ret);
 }
-EXPORT_SYMBOL_GPL(iio_st_channel_get_all);
+EXPORT_SYMBOL_GPL(iio_channel_get_all);
 
-void iio_st_channel_release_all(struct iio_channel *channels)
+void iio_channel_release_all(struct iio_channel *channels)
 {
 	struct iio_channel *chan = &channels[0];
 
 	while (chan->indio_dev) {
-		put_device(&chan->indio_dev->dev);
+		iio_device_put(chan->indio_dev);
 		chan++;
 	}
 	kfree(channels);
 }
-EXPORT_SYMBOL_GPL(iio_st_channel_release_all);
+EXPORT_SYMBOL_GPL(iio_channel_release_all);
 
-int iio_st_read_channel_raw(struct iio_channel *chan, int *val)
+int iio_read_channel_raw(struct iio_channel *chan, int *val)
 {
 	int val2, ret;
 
@@ -249,9 +246,9 @@ err_unlock:
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(iio_st_read_channel_raw);
+EXPORT_SYMBOL_GPL(iio_read_channel_raw);
 
-int iio_st_read_channel_scale(struct iio_channel *chan, int *val, int *val2)
+int iio_read_channel_scale(struct iio_channel *chan, int *val, int *val2)
 {
 	int ret;
 
@@ -270,10 +267,9 @@ err_unlock:
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(iio_st_read_channel_scale);
+EXPORT_SYMBOL_GPL(iio_read_channel_scale);
 
-int iio_st_get_channel_type(struct iio_channel *chan,
-			    enum iio_chan_type *type)
+int iio_get_channel_type(struct iio_channel *chan, enum iio_chan_type *type)
 {
 	int ret = 0;
 	/* Need to verify underlying driver has not gone away */
@@ -290,4 +286,4 @@ err_unlock:
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(iio_st_get_channel_type);
+EXPORT_SYMBOL_GPL(iio_get_channel_type);
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
new file mode 100644
index 00000000000..91d15d2f694
--- /dev/null
+++ b/drivers/iio/light/Kconfig
@@ -0,0 +1,45 @@
+#
+# Light sensors
+#
+menu "Light sensors"
+
+config ADJD_S311
+	tristate "ADJD-S311-CR999 digital color sensor"
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	depends on I2C
+	help
+	 If you say yes here you get support for the Avago ADJD-S311-CR999
+	 digital color light sensor.
+
+	 This driver can also be built as a module.  If so, the module
+	 will be called adjd_s311.
+
+config SENSORS_LM3533
+	tristate "LM3533 ambient light sensor"
+	depends on MFD_LM3533
+	help
+	  If you say yes here you get support for the ambient light sensor
+	  interface on National Semiconductor / TI LM3533 Lighting Power
+	  chips.
+
+	  The sensor interface can be used to control the LEDs and backlights
+	  of the chip through defining five light zones and three sets of
+	  corresponding output-current values.
+
+	  The driver provides raw and mean adc readings along with the current
+	  light zone through sysfs. A threshold event can be generated on zone
+	  changes. The ALS-control output values can be set per zone for the
+	  three current output channels.
+
+config VCNL4000
+	tristate "VCNL4000 combined ALS and proximity sensor"
+	depends on I2C
+	help
+	 Say Y here if you want to build a driver for the Vishay VCNL4000
+	 combined ambient light and proximity sensor.
+
+	 To compile this driver as a module, choose M here: the
+	 module will be called vcnl4000.
+
+endmenu
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
new file mode 100644
index 00000000000..13f8a782d29
--- /dev/null
+++ b/drivers/iio/light/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for IIO Light sensors
+#
+
+obj-$(CONFIG_ADJD_S311)		+= adjd_s311.o
+obj-$(CONFIG_SENSORS_LM3533)	+= lm3533-als.o
+obj-$(CONFIG_VCNL4000)		+= vcnl4000.o
diff --git a/drivers/iio/light/adjd_s311.c b/drivers/iio/light/adjd_s311.c
new file mode 100644
index 00000000000..1cbb449b319
--- /dev/null
+++ b/drivers/iio/light/adjd_s311.c
@@ -0,0 +1,364 @@
+/*
+ * adjd_s311.c - Support for ADJD-S311-CR999 digital color sensor
+ *
+ * Copyright (C) 2012 Peter Meerwald <pmeerw@pmeerw.net>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * driver for ADJD-S311-CR999 digital color sensor (10-bit channels for
+ * red, green, blue, clear); 7-bit I2C slave address 0x74
+ *
+ * limitations: no calibration, no offset mode, no sleep mode
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/bitmap.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#define ADJD_S311_DRV_NAME "adjd_s311"
+
+#define ADJD_S311_CTRL		0x00
+#define ADJD_S311_CONFIG	0x01
+#define ADJD_S311_CAP_RED	0x06
+#define ADJD_S311_CAP_GREEN	0x07
+#define ADJD_S311_CAP_BLUE	0x08
+#define ADJD_S311_CAP_CLEAR	0x09
+#define ADJD_S311_INT_RED_LO	0x0a
+#define ADJD_S311_INT_RED_HI	0x0b
+#define ADJD_S311_INT_GREEN_LO	0x0c
+#define ADJD_S311_INT_GREEN_HI	0x0d
+#define ADJD_S311_INT_BLUE_LO	0x0e
+#define ADJD_S311_INT_BLUE_HI	0x0f
+#define ADJD_S311_INT_CLEAR_LO	0x10
+#define ADJD_S311_INT_CLEAR_HI	0x11
+#define ADJD_S311_DATA_RED_LO	0x40
+#define ADJD_S311_DATA_RED_HI	0x41
+#define ADJD_S311_DATA_GREEN_LO	0x42
+#define ADJD_S311_DATA_GREEN_HI	0x43
+#define ADJD_S311_DATA_BLUE_LO	0x44
+#define ADJD_S311_DATA_BLUE_HI	0x45
+#define ADJD_S311_DATA_CLEAR_LO	0x46
+#define ADJD_S311_DATA_CLEAR_HI	0x47
+#define ADJD_S311_OFFSET_RED	0x48
+#define ADJD_S311_OFFSET_GREEN	0x49
+#define ADJD_S311_OFFSET_BLUE	0x4a
+#define ADJD_S311_OFFSET_CLEAR	0x4b
+
+#define ADJD_S311_CTRL_GOFS	0x02
+#define ADJD_S311_CTRL_GSSR	0x01
+#define ADJD_S311_CAP_MASK	0x0f
+#define ADJD_S311_INT_MASK	0x0fff
+#define ADJD_S311_DATA_MASK	0x03ff
+
+struct adjd_s311_data {
+	struct i2c_client *client;
+	u16 *buffer;
+};
+
+enum adjd_s311_channel_idx {
+	IDX_RED, IDX_GREEN, IDX_BLUE, IDX_CLEAR
+};
+
+#define ADJD_S311_DATA_REG(chan) (ADJD_S311_DATA_RED_LO + (chan) * 2)
+#define ADJD_S311_INT_REG(chan) (ADJD_S311_INT_RED_LO + (chan) * 2)
+#define ADJD_S311_CAP_REG(chan) (ADJD_S311_CAP_RED + (chan))
+
+static int adjd_s311_req_data(struct iio_dev *indio_dev)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	int tries = 10;
+
+	int ret = i2c_smbus_write_byte_data(data->client, ADJD_S311_CTRL,
+		ADJD_S311_CTRL_GSSR);
+	if (ret < 0)
+		return ret;
+
+	while (tries--) {
+		ret = i2c_smbus_read_byte_data(data->client, ADJD_S311_CTRL);
+		if (ret < 0)
+			return ret;
+		if (!(ret & ADJD_S311_CTRL_GSSR))
+			break;
+		msleep(20);
+	}
+
+	if (tries < 0) {
+		dev_err(&data->client->dev,
+			"adjd_s311_req_data() failed, data not ready\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int adjd_s311_read_data(struct iio_dev *indio_dev, u8 reg, int *val)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+
+	int ret = adjd_s311_req_data(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_read_word_data(data->client, reg);
+	if (ret < 0)
+		return ret;
+
+	*val = ret & ADJD_S311_DATA_MASK;
+
+	return 0;
+}
+
+static ssize_t adjd_s311_read_int_time(struct iio_dev *indio_dev,
+	uintptr_t private, const struct iio_chan_spec *chan, char *buf)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	s32 ret;
+
+	ret = i2c_smbus_read_word_data(data->client,
+		ADJD_S311_INT_REG(chan->address));
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", ret & ADJD_S311_INT_MASK);
+}
+
+static ssize_t adjd_s311_write_int_time(struct iio_dev *indio_dev,
+	 uintptr_t private, const struct iio_chan_spec *chan, const char *buf,
+	 size_t len)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	unsigned long int_time;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &int_time);
+	if (ret)
+		return ret;
+
+	if (int_time > ADJD_S311_INT_MASK)
+		return -EINVAL;
+
+	ret = i2c_smbus_write_word_data(data->client,
+		ADJD_S311_INT_REG(chan->address), int_time);
+	if (ret < 0)
+		return ret;
+
+	return len;
+}
+
+static irqreturn_t adjd_s311_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	struct iio_buffer *buffer = indio_dev->buffer;
+	s64 time_ns = iio_get_time_ns();
+	int len = 0;
+	int i, j = 0;
+
+	int ret = adjd_s311_req_data(indio_dev);
+	if (ret < 0)
+		goto done;
+
+	for_each_set_bit(i, indio_dev->active_scan_mask,
+		indio_dev->masklength) {
+		ret = i2c_smbus_read_word_data(data->client,
+			ADJD_S311_DATA_REG(i));
+		if (ret < 0)
+			goto done;
+
+		data->buffer[j++] = ret & ADJD_S311_DATA_MASK;
+		len += 2;
+	}
+
+	if (indio_dev->scan_timestamp)
+		*(s64 *)((u8 *)data->buffer + ALIGN(len, sizeof(s64)))
+			= time_ns;
+	iio_push_to_buffer(buffer, (u8 *)data->buffer, time_ns);
+
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_chan_spec_ext_info adjd_s311_ext_info[] = {
+	{
+		.name = "integration_time",
+		.read = adjd_s311_read_int_time,
+		.write = adjd_s311_write_int_time,
+	},
+	{ }
+};
+
+#define ADJD_S311_CHANNEL(_color, _scan_idx) { \
+	.type = IIO_INTENSITY, \
+	.modified = 1, \
+	.address = (IDX_##_color), \
+	.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT | \
+		IIO_CHAN_INFO_HARDWAREGAIN_SEPARATE_BIT, \
+	.channel2 = (IIO_MOD_LIGHT_##_color), \
+	.scan_index = (_scan_idx), \
+	.scan_type = IIO_ST('u', 10, 16, 0), \
+	.ext_info = adjd_s311_ext_info, \
+}
+
+static const struct iio_chan_spec adjd_s311_channels[] = {
+	ADJD_S311_CHANNEL(RED, 0),
+	ADJD_S311_CHANNEL(GREEN, 1),
+	ADJD_S311_CHANNEL(BLUE, 2),
+	ADJD_S311_CHANNEL(CLEAR, 3),
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static int adjd_s311_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = adjd_s311_read_data(indio_dev, chan->address, val);
+		if (ret < 0)
+			return ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		ret = i2c_smbus_read_byte_data(data->client,
+			ADJD_S311_CAP_REG(chan->address));
+		if (ret < 0)
+			return ret;
+		*val = ret & ADJD_S311_CAP_MASK;
+		return IIO_VAL_INT;
+	}
+	return -EINVAL;
+}
+
+static int adjd_s311_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val, int val2, long mask)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		if (val < 0 || val > ADJD_S311_CAP_MASK)
+			return -EINVAL;
+
+		ret = i2c_smbus_write_byte_data(data->client,
+			ADJD_S311_CAP_REG(chan->address), val);
+		return ret;
+	}
+	return -EINVAL;
+}
+
+static int adjd_s311_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *scan_mask)
+{
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+	data->buffer = krealloc(data->buffer, indio_dev->scan_bytes,
+				GFP_KERNEL);
+	if (!data->buffer)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static const struct iio_info adjd_s311_info = {
+	.read_raw = adjd_s311_read_raw,
+	.write_raw = adjd_s311_write_raw,
+	.update_scan_mode = adjd_s311_update_scan_mode,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit adjd_s311_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct adjd_s311_data *data;
+	struct iio_dev *indio_dev;
+	int err;
+
+	indio_dev = iio_device_alloc(sizeof(*data));
+	if (indio_dev == NULL) {
+		err = -ENOMEM;
+		goto exit;
+	}
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &adjd_s311_info;
+	indio_dev->name = ADJD_S311_DRV_NAME;
+	indio_dev->channels = adjd_s311_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adjd_s311_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	err = iio_triggered_buffer_setup(indio_dev, NULL,
+		adjd_s311_trigger_handler, NULL);
+	if (err < 0)
+		goto exit_free_device;
+
+	err = iio_device_register(indio_dev);
+	if (err)
+		goto exit_unreg_buffer;
+
+	dev_info(&client->dev, "ADJD-S311 color sensor registered\n");
+
+	return 0;
+
+exit_unreg_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+exit_free_device:
+	iio_device_free(indio_dev);
+exit:
+	return err;
+}
+
+static int __devexit adjd_s311_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct adjd_s311_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	kfree(data->buffer);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id adjd_s311_id[] = {
+	{ "adjd_s311", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, adjd_s311_id);
+
+static struct i2c_driver adjd_s311_driver = {
+	.driver = {
+		.name	= ADJD_S311_DRV_NAME,
+	},
+	.probe		= adjd_s311_probe,
+	.remove		= __devexit_p(adjd_s311_remove),
+	.id_table	= adjd_s311_id,
+};
+module_i2c_driver(adjd_s311_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("ADJD-S311 color sensor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/light/lm3533-als.c b/drivers/iio/light/lm3533-als.c
new file mode 100644
index 00000000000..c3e7bac1312
--- /dev/null
+++ b/drivers/iio/light/lm3533-als.c
@@ -0,0 +1,932 @@
+/*
+ * lm3533-als.c -- LM3533 Ambient Light Sensor driver
+ *
+ * Copyright (C) 2011-2012 Texas Instruments
+ *
+ * Author: Johan Hovold <jhovold@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under  the terms of the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/atomic.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iio/events.h>
+#include <linux/iio/iio.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/mfd/core.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include <linux/mfd/lm3533.h>
+
+
+#define LM3533_ALS_RESISTOR_MIN			1
+#define LM3533_ALS_RESISTOR_MAX			127
+#define LM3533_ALS_CHANNEL_CURRENT_MAX		2
+#define LM3533_ALS_THRESH_MAX			3
+#define LM3533_ALS_ZONE_MAX			4
+
+#define LM3533_REG_ALS_RESISTOR_SELECT		0x30
+#define LM3533_REG_ALS_CONF			0x31
+#define LM3533_REG_ALS_ZONE_INFO		0x34
+#define LM3533_REG_ALS_READ_ADC_RAW		0x37
+#define LM3533_REG_ALS_READ_ADC_AVERAGE		0x38
+#define LM3533_REG_ALS_BOUNDARY_BASE		0x50
+#define LM3533_REG_ALS_TARGET_BASE		0x60
+
+#define LM3533_ALS_ENABLE_MASK			0x01
+#define LM3533_ALS_INPUT_MODE_MASK		0x02
+#define LM3533_ALS_INT_ENABLE_MASK		0x01
+
+#define LM3533_ALS_ZONE_SHIFT			2
+#define LM3533_ALS_ZONE_MASK			0x1c
+
+#define LM3533_ALS_FLAG_INT_ENABLED		1
+
+
+struct lm3533_als {
+	struct lm3533 *lm3533;
+	struct platform_device *pdev;
+
+	unsigned long flags;
+	int irq;
+
+	atomic_t zone;
+	struct mutex thresh_mutex;
+};
+
+
+static int lm3533_als_get_adc(struct iio_dev *indio_dev, bool average,
+								int *adc)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 reg;
+	u8 val;
+	int ret;
+
+	if (average)
+		reg = LM3533_REG_ALS_READ_ADC_AVERAGE;
+	else
+		reg = LM3533_REG_ALS_READ_ADC_RAW;
+
+	ret = lm3533_read(als->lm3533, reg, &val);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to read adc\n");
+		return ret;
+	}
+
+	*adc = val;
+
+	return 0;
+}
+
+static int _lm3533_als_get_zone(struct iio_dev *indio_dev, u8 *zone)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 val;
+	int ret;
+
+	ret = lm3533_read(als->lm3533, LM3533_REG_ALS_ZONE_INFO, &val);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to read zone\n");
+		return ret;
+	}
+
+	val = (val & LM3533_ALS_ZONE_MASK) >> LM3533_ALS_ZONE_SHIFT;
+	*zone = min_t(u8, val, LM3533_ALS_ZONE_MAX);
+
+	return 0;
+}
+
+static int lm3533_als_get_zone(struct iio_dev *indio_dev, u8 *zone)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	int ret;
+
+	if (test_bit(LM3533_ALS_FLAG_INT_ENABLED, &als->flags)) {
+		*zone = atomic_read(&als->zone);
+	} else {
+		ret = _lm3533_als_get_zone(indio_dev, zone);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * channel	output channel 0..2
+ * zone		zone 0..4
+ */
+static inline u8 lm3533_als_get_target_reg(unsigned channel, unsigned zone)
+{
+	return LM3533_REG_ALS_TARGET_BASE + 5 * channel + zone;
+}
+
+static int lm3533_als_get_target(struct iio_dev *indio_dev, unsigned channel,
+							unsigned zone, u8 *val)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 reg;
+	int ret;
+
+	if (channel > LM3533_ALS_CHANNEL_CURRENT_MAX)
+		return -EINVAL;
+
+	if (zone > LM3533_ALS_ZONE_MAX)
+		return -EINVAL;
+
+	reg = lm3533_als_get_target_reg(channel, zone);
+	ret = lm3533_read(als->lm3533, reg, val);
+	if (ret)
+		dev_err(&indio_dev->dev, "failed to get target current\n");
+
+	return ret;
+}
+
+static int lm3533_als_set_target(struct iio_dev *indio_dev, unsigned channel,
+							unsigned zone, u8 val)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 reg;
+	int ret;
+
+	if (channel > LM3533_ALS_CHANNEL_CURRENT_MAX)
+		return -EINVAL;
+
+	if (zone > LM3533_ALS_ZONE_MAX)
+		return -EINVAL;
+
+	reg = lm3533_als_get_target_reg(channel, zone);
+	ret = lm3533_write(als->lm3533, reg, val);
+	if (ret)
+		dev_err(&indio_dev->dev, "failed to set target current\n");
+
+	return ret;
+}
+
+static int lm3533_als_get_current(struct iio_dev *indio_dev, unsigned channel,
+								int *val)
+{
+	u8 zone;
+	u8 target;
+	int ret;
+
+	ret = lm3533_als_get_zone(indio_dev, &zone);
+	if (ret)
+		return ret;
+
+	ret = lm3533_als_get_target(indio_dev, channel, zone, &target);
+	if (ret)
+		return ret;
+
+	*val = target;
+
+	return 0;
+}
+
+static int lm3533_als_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	int ret;
+
+	switch (mask) {
+	case 0:
+		switch (chan->type) {
+		case IIO_LIGHT:
+			ret = lm3533_als_get_adc(indio_dev, false, val);
+			break;
+		case IIO_CURRENT:
+			ret = lm3533_als_get_current(indio_dev, chan->channel,
+									val);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case IIO_CHAN_INFO_AVERAGE_RAW:
+		ret = lm3533_als_get_adc(indio_dev, true, val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (ret)
+		return ret;
+
+	return IIO_VAL_INT;
+}
+
+#define CHANNEL_CURRENT(_channel)					\
+	{								\
+		.type		= IIO_CURRENT,				\
+		.channel	= _channel,				\
+		.indexed	= true,					\
+		.output		= true,					\
+		.info_mask	= IIO_CHAN_INFO_RAW_SEPARATE_BIT,	\
+	}
+
+static const struct iio_chan_spec lm3533_als_channels[] = {
+	{
+		.type		= IIO_LIGHT,
+		.channel	= 0,
+		.indexed	= true,
+		.info_mask	= (IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT |
+				   IIO_CHAN_INFO_RAW_SEPARATE_BIT),
+	},
+	CHANNEL_CURRENT(0),
+	CHANNEL_CURRENT(1),
+	CHANNEL_CURRENT(2),
+};
+
+static irqreturn_t lm3533_als_isr(int irq, void *dev_id)
+{
+
+	struct iio_dev *indio_dev = dev_id;
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 zone;
+	int ret;
+
+	/* Clear interrupt by reading the ALS zone register. */
+	ret = _lm3533_als_get_zone(indio_dev, &zone);
+	if (ret)
+		goto out;
+
+	atomic_set(&als->zone, zone);
+
+	iio_push_event(indio_dev,
+		       IIO_UNMOD_EVENT_CODE(IIO_LIGHT,
+					    0,
+					    IIO_EV_TYPE_THRESH,
+					    IIO_EV_DIR_EITHER),
+		       iio_get_time_ns());
+out:
+	return IRQ_HANDLED;
+}
+
+static int lm3533_als_set_int_mode(struct iio_dev *indio_dev, int enable)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 mask = LM3533_ALS_INT_ENABLE_MASK;
+	u8 val;
+	int ret;
+
+	if (enable)
+		val = mask;
+	else
+		val = 0;
+
+	ret = lm3533_update(als->lm3533, LM3533_REG_ALS_ZONE_INFO, val, mask);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to set int mode %d\n",
+								enable);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int lm3533_als_get_int_mode(struct iio_dev *indio_dev, int *enable)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 mask = LM3533_ALS_INT_ENABLE_MASK;
+	u8 val;
+	int ret;
+
+	ret = lm3533_read(als->lm3533, LM3533_REG_ALS_ZONE_INFO, &val);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to get int mode\n");
+		return ret;
+	}
+
+	*enable = !!(val & mask);
+
+	return 0;
+}
+
+static inline u8 lm3533_als_get_threshold_reg(unsigned nr, bool raising)
+{
+	u8 offset = !raising;
+
+	return LM3533_REG_ALS_BOUNDARY_BASE + 2 * nr + offset;
+}
+
+static int lm3533_als_get_threshold(struct iio_dev *indio_dev, unsigned nr,
+							bool raising, u8 *val)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 reg;
+	int ret;
+
+	if (nr > LM3533_ALS_THRESH_MAX)
+		return -EINVAL;
+
+	reg = lm3533_als_get_threshold_reg(nr, raising);
+	ret = lm3533_read(als->lm3533, reg, val);
+	if (ret)
+		dev_err(&indio_dev->dev, "failed to get threshold\n");
+
+	return ret;
+}
+
+static int lm3533_als_set_threshold(struct iio_dev *indio_dev, unsigned nr,
+							bool raising, u8 val)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 val2;
+	u8 reg, reg2;
+	int ret;
+
+	if (nr > LM3533_ALS_THRESH_MAX)
+		return -EINVAL;
+
+	reg = lm3533_als_get_threshold_reg(nr, raising);
+	reg2 = lm3533_als_get_threshold_reg(nr, !raising);
+
+	mutex_lock(&als->thresh_mutex);
+	ret = lm3533_read(als->lm3533, reg2, &val2);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to get threshold\n");
+		goto out;
+	}
+	/*
+	 * This device does not allow negative hysteresis (in fact, it uses
+	 * whichever value is smaller as the lower bound) so we need to make
+	 * sure that thresh_falling <= thresh_raising.
+	 */
+	if ((raising && (val < val2)) || (!raising && (val > val2))) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = lm3533_write(als->lm3533, reg, val);
+	if (ret) {
+		dev_err(&indio_dev->dev, "failed to set threshold\n");
+		goto out;
+	}
+out:
+	mutex_unlock(&als->thresh_mutex);
+
+	return ret;
+}
+
+static int lm3533_als_get_hysteresis(struct iio_dev *indio_dev, unsigned nr,
+								u8 *val)
+{
+	struct lm3533_als *als = iio_priv(indio_dev);
+	u8 falling;
+	u8 raising;
+	int ret;
+
+	if (nr > LM3533_ALS_THRESH_MAX)
+		return -EINVAL;
+
+	mutex_lock(&als->thresh_mutex);
+	ret = lm3533_als_get_threshold(indio_dev, nr, false, &falling);
+	if (ret)
+		goto out;
+	ret = lm3533_als_get_threshold(indio_dev, nr, true, &raising);
+	if (ret)
+		goto out;
+
+	*val = raising - falling;
+out:
+	mutex_unlock(&als->thresh_mutex);
+
+	return ret;
+}
+
+static int show_thresh_either_en(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct lm3533_als *als = iio_priv(indio_dev);
+	int enable;
+	int ret;
+
+	if (als->irq) {
+		ret = lm3533_als_get_int_mode(indio_dev, &enable);
+		if (ret)
+			return ret;
+	} else {
+		enable = 0;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", enable);
+}
+
+static int store_thresh_either_en(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct lm3533_als *als = iio_priv(indio_dev);
+	unsigned long enable;
+	bool int_enabled;
+	u8 zone;
+	int ret;
+
+	if (!als->irq)
+		return -EBUSY;
+
+	if (kstrtoul(buf, 0, &enable))
+		return -EINVAL;
+
+	int_enabled = test_bit(LM3533_ALS_FLAG_INT_ENABLED, &als->flags);
+
+	if (enable && !int_enabled) {
+		ret = lm3533_als_get_zone(indio_dev, &zone);
+		if (ret)
+			return ret;
+
+		atomic_set(&als->zone, zone);
+
+		set_bit(LM3533_ALS_FLAG_INT_ENABLED, &als->flags);
+	}
+
+	ret = lm3533_als_set_int_mode(indio_dev, enable);
+	if (ret) {
+		if (!int_enabled)
+			clear_bit(LM3533_ALS_FLAG_INT_ENABLED, &als->flags);
+
+		return ret;
+	}
+
+	if (!enable)
+		clear_bit(LM3533_ALS_FLAG_INT_ENABLED, &als->flags);
+
+	return len;
+}
+
+static ssize_t show_zone(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	u8 zone;
+	int ret;
+
+	ret = lm3533_als_get_zone(indio_dev, &zone);
+	if (ret)
+		return ret;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", zone);
+}
+
+enum lm3533_als_attribute_type {
+	LM3533_ATTR_TYPE_HYSTERESIS,
+	LM3533_ATTR_TYPE_TARGET,
+	LM3533_ATTR_TYPE_THRESH_FALLING,
+	LM3533_ATTR_TYPE_THRESH_RAISING,
+};
+
+struct lm3533_als_attribute {
+	struct device_attribute dev_attr;
+	enum lm3533_als_attribute_type type;
+	u8 val1;
+	u8 val2;
+};
+
+static inline struct lm3533_als_attribute *
+to_lm3533_als_attr(struct device_attribute *attr)
+{
+	return container_of(attr, struct lm3533_als_attribute, dev_attr);
+}
+
+static ssize_t show_als_attr(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct lm3533_als_attribute *als_attr = to_lm3533_als_attr(attr);
+	u8 val;
+	int ret;
+
+	switch (als_attr->type) {
+	case LM3533_ATTR_TYPE_HYSTERESIS:
+		ret = lm3533_als_get_hysteresis(indio_dev, als_attr->val1,
+									&val);
+		break;
+	case LM3533_ATTR_TYPE_TARGET:
+		ret = lm3533_als_get_target(indio_dev, als_attr->val1,
+							als_attr->val2, &val);
+		break;
+	case LM3533_ATTR_TYPE_THRESH_FALLING:
+		ret = lm3533_als_get_threshold(indio_dev, als_attr->val1,
+								false, &val);
+		break;
+	case LM3533_ATTR_TYPE_THRESH_RAISING:
+		ret = lm3533_als_get_threshold(indio_dev, als_attr->val1,
+								true, &val);
+		break;
+	default:
+		ret = -ENXIO;
+	}
+
+	if (ret)
+		return ret;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", val);
+}
+
+static ssize_t store_als_attr(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct lm3533_als_attribute *als_attr = to_lm3533_als_attr(attr);
+	u8 val;
+	int ret;
+
+	if (kstrtou8(buf, 0, &val))
+		return -EINVAL;
+
+	switch (als_attr->type) {
+	case LM3533_ATTR_TYPE_TARGET:
+		ret = lm3533_als_set_target(indio_dev, als_attr->val1,
+							als_attr->val2, val);
+		break;
+	case LM3533_ATTR_TYPE_THRESH_FALLING:
+		ret = lm3533_als_set_threshold(indio_dev, als_attr->val1,
+								false, val);
+		break;
+	case LM3533_ATTR_TYPE_THRESH_RAISING:
+		ret = lm3533_als_set_threshold(indio_dev, als_attr->val1,
+								true, val);
+		break;
+	default:
+		ret = -ENXIO;
+	}
+
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+#define ALS_ATTR(_name, _mode, _show, _store, _type, _val1, _val2)	\
+	{ .dev_attr	= __ATTR(_name, _mode, _show, _store),		\
+	  .type		= _type,					\
+	  .val1		= _val1,					\
+	  .val2		= _val2 }
+
+#define LM3533_ALS_ATTR(_name, _mode, _show, _store, _type, _val1, _val2) \
+	struct lm3533_als_attribute lm3533_als_attr_##_name =		  \
+		ALS_ATTR(_name, _mode, _show, _store, _type, _val1, _val2)
+
+#define ALS_TARGET_ATTR_RW(_channel, _zone)				\
+	LM3533_ALS_ATTR(out_current##_channel##_current##_zone##_raw,	\
+				S_IRUGO | S_IWUSR,			\
+				show_als_attr, store_als_attr,		\
+				LM3533_ATTR_TYPE_TARGET, _channel, _zone)
+/*
+ * ALS output current values (ALS mapper targets)
+ *
+ * out_current[0-2]_current[0-4]_raw		0-255
+ */
+static ALS_TARGET_ATTR_RW(0, 0);
+static ALS_TARGET_ATTR_RW(0, 1);
+static ALS_TARGET_ATTR_RW(0, 2);
+static ALS_TARGET_ATTR_RW(0, 3);
+static ALS_TARGET_ATTR_RW(0, 4);
+
+static ALS_TARGET_ATTR_RW(1, 0);
+static ALS_TARGET_ATTR_RW(1, 1);
+static ALS_TARGET_ATTR_RW(1, 2);
+static ALS_TARGET_ATTR_RW(1, 3);
+static ALS_TARGET_ATTR_RW(1, 4);
+
+static ALS_TARGET_ATTR_RW(2, 0);
+static ALS_TARGET_ATTR_RW(2, 1);
+static ALS_TARGET_ATTR_RW(2, 2);
+static ALS_TARGET_ATTR_RW(2, 3);
+static ALS_TARGET_ATTR_RW(2, 4);
+
+#define ALS_THRESH_FALLING_ATTR_RW(_nr)					\
+	LM3533_ALS_ATTR(in_illuminance0_thresh##_nr##_falling_value,	\
+			S_IRUGO | S_IWUSR,				\
+			show_als_attr, store_als_attr,		\
+			LM3533_ATTR_TYPE_THRESH_FALLING, _nr, 0)
+
+#define ALS_THRESH_RAISING_ATTR_RW(_nr)					\
+	LM3533_ALS_ATTR(in_illuminance0_thresh##_nr##_raising_value,	\
+			S_IRUGO | S_IWUSR,				\
+			show_als_attr, store_als_attr,			\
+			LM3533_ATTR_TYPE_THRESH_RAISING, _nr, 0)
+/*
+ * ALS Zone thresholds (boundaries)
+ *
+ * in_illuminance0_thresh[0-3]_falling_value	0-255
+ * in_illuminance0_thresh[0-3]_raising_value	0-255
+ */
+static ALS_THRESH_FALLING_ATTR_RW(0);
+static ALS_THRESH_FALLING_ATTR_RW(1);
+static ALS_THRESH_FALLING_ATTR_RW(2);
+static ALS_THRESH_FALLING_ATTR_RW(3);
+
+static ALS_THRESH_RAISING_ATTR_RW(0);
+static ALS_THRESH_RAISING_ATTR_RW(1);
+static ALS_THRESH_RAISING_ATTR_RW(2);
+static ALS_THRESH_RAISING_ATTR_RW(3);
+
+#define ALS_HYSTERESIS_ATTR_RO(_nr)					\
+	LM3533_ALS_ATTR(in_illuminance0_thresh##_nr##_hysteresis,	\
+			S_IRUGO, show_als_attr, NULL,			\
+			LM3533_ATTR_TYPE_HYSTERESIS, _nr, 0)
+/*
+ * ALS Zone threshold hysteresis
+ *
+ * threshY_hysteresis = threshY_raising - threshY_falling
+ *
+ * in_illuminance0_thresh[0-3]_hysteresis	0-255
+ * in_illuminance0_thresh[0-3]_hysteresis	0-255
+ */
+static ALS_HYSTERESIS_ATTR_RO(0);
+static ALS_HYSTERESIS_ATTR_RO(1);
+static ALS_HYSTERESIS_ATTR_RO(2);
+static ALS_HYSTERESIS_ATTR_RO(3);
+
+#define ILLUMINANCE_ATTR_RO(_name) \
+	DEVICE_ATTR(in_illuminance0_##_name, S_IRUGO, show_##_name, NULL)
+#define ILLUMINANCE_ATTR_RW(_name) \
+	DEVICE_ATTR(in_illuminance0_##_name, S_IRUGO | S_IWUSR , \
+						show_##_name, store_##_name)
+/*
+ * ALS Zone threshold-event enable
+ *
+ * in_illuminance0_thresh_either_en		0,1
+ */
+static ILLUMINANCE_ATTR_RW(thresh_either_en);
+
+/*
+ * ALS Current Zone
+ *
+ * in_illuminance0_zone		0-4
+ */
+static ILLUMINANCE_ATTR_RO(zone);
+
+static struct attribute *lm3533_als_event_attributes[] = {
+	&dev_attr_in_illuminance0_thresh_either_en.attr,
+	&lm3533_als_attr_in_illuminance0_thresh0_falling_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh0_hysteresis.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh0_raising_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh1_falling_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh1_hysteresis.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh1_raising_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh2_falling_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh2_hysteresis.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh2_raising_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh3_falling_value.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh3_hysteresis.dev_attr.attr,
+	&lm3533_als_attr_in_illuminance0_thresh3_raising_value.dev_attr.attr,
+	NULL
+};
+
+static struct attribute_group lm3533_als_event_attribute_group = {
+	.attrs = lm3533_als_event_attributes
+};
+
+static struct attribute *lm3533_als_attributes[] = {
+	&dev_attr_in_illuminance0_zone.attr,
+	&lm3533_als_attr_out_current0_current0_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current0_current1_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current0_current2_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current0_current3_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current0_current4_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current1_current0_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current1_current1_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current1_current2_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current1_current3_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current1_current4_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current2_current0_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current2_current1_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current2_current2_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current2_current3_raw.dev_attr.attr,
+	&lm3533_als_attr_out_current2_current4_raw.dev_attr.attr,
+	NULL
+};
+
+static struct attribute_group lm3533_als_attribute_group = {
+	.attrs = lm3533_als_attributes
+};
+
+static int __devinit lm3533_als_set_input_mode(struct lm3533_als *als,
+								bool pwm_mode)
+{
+	u8 mask = LM3533_ALS_INPUT_MODE_MASK;
+	u8 val;
+	int ret;
+
+	if (pwm_mode)
+		val = mask;	/* pwm input */
+	else
+		val = 0;	/* analog input */
+
+	ret = lm3533_update(als->lm3533, LM3533_REG_ALS_CONF, val, mask);
+	if (ret) {
+		dev_err(&als->pdev->dev, "failed to set input mode %d\n",
+								pwm_mode);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devinit lm3533_als_set_resistor(struct lm3533_als *als, u8 val)
+{
+	int ret;
+
+	if (val < LM3533_ALS_RESISTOR_MIN || val > LM3533_ALS_RESISTOR_MAX)
+		return -EINVAL;
+
+	ret = lm3533_write(als->lm3533, LM3533_REG_ALS_RESISTOR_SELECT, val);
+	if (ret) {
+		dev_err(&als->pdev->dev, "failed to set resistor\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devinit lm3533_als_setup(struct lm3533_als *als,
+					struct lm3533_als_platform_data *pdata)
+{
+	int ret;
+
+	ret = lm3533_als_set_input_mode(als, pdata->pwm_mode);
+	if (ret)
+		return ret;
+
+	/* ALS input is always high impedance in PWM-mode. */
+	if (!pdata->pwm_mode) {
+		ret = lm3533_als_set_resistor(als, pdata->r_select);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int __devinit lm3533_als_setup_irq(struct lm3533_als *als, void *dev)
+{
+	u8 mask = LM3533_ALS_INT_ENABLE_MASK;
+	int ret;
+
+	/* Make sure interrupts are disabled. */
+	ret = lm3533_update(als->lm3533, LM3533_REG_ALS_ZONE_INFO, 0, mask);
+	if (ret) {
+		dev_err(&als->pdev->dev, "failed to disable interrupts\n");
+		return ret;
+	}
+
+	ret = request_threaded_irq(als->irq, NULL, lm3533_als_isr,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					dev_name(&als->pdev->dev), dev);
+	if (ret) {
+		dev_err(&als->pdev->dev, "failed to request irq %d\n",
+								als->irq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devinit lm3533_als_enable(struct lm3533_als *als)
+{
+	u8 mask = LM3533_ALS_ENABLE_MASK;
+	int ret;
+
+	ret = lm3533_update(als->lm3533, LM3533_REG_ALS_CONF, mask, mask);
+	if (ret)
+		dev_err(&als->pdev->dev, "failed to enable ALS\n");
+
+	return ret;
+}
+
+static int lm3533_als_disable(struct lm3533_als *als)
+{
+	u8 mask = LM3533_ALS_ENABLE_MASK;
+	int ret;
+
+	ret = lm3533_update(als->lm3533, LM3533_REG_ALS_CONF, 0, mask);
+	if (ret)
+		dev_err(&als->pdev->dev, "failed to disable ALS\n");
+
+	return ret;
+}
+
+static const struct iio_info lm3533_als_info = {
+	.attrs		= &lm3533_als_attribute_group,
+	.event_attrs	= &lm3533_als_event_attribute_group,
+	.driver_module	= THIS_MODULE,
+	.read_raw	= &lm3533_als_read_raw,
+};
+
+static int __devinit lm3533_als_probe(struct platform_device *pdev)
+{
+	struct lm3533 *lm3533;
+	struct lm3533_als_platform_data *pdata;
+	struct lm3533_als *als;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	lm3533 = dev_get_drvdata(pdev->dev.parent);
+	if (!lm3533)
+		return -EINVAL;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	indio_dev = iio_device_alloc(sizeof(*als));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	indio_dev->info = &lm3533_als_info;
+	indio_dev->channels = lm3533_als_channels;
+	indio_dev->num_channels = ARRAY_SIZE(lm3533_als_channels);
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = pdev->dev.parent;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	als = iio_priv(indio_dev);
+	als->lm3533 = lm3533;
+	als->pdev = pdev;
+	als->irq = lm3533->irq;
+	atomic_set(&als->zone, 0);
+	mutex_init(&als->thresh_mutex);
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	if (als->irq) {
+		ret = lm3533_als_setup_irq(als, indio_dev);
+		if (ret)
+			goto err_free_dev;
+	}
+
+	ret = lm3533_als_setup(als, pdata);
+	if (ret)
+		goto err_free_irq;
+
+	ret = lm3533_als_enable(als);
+	if (ret)
+		goto err_free_irq;
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register ALS\n");
+		goto err_disable;
+	}
+
+	return 0;
+
+err_disable:
+	lm3533_als_disable(als);
+err_free_irq:
+	if (als->irq)
+		free_irq(als->irq, indio_dev);
+err_free_dev:
+	iio_device_free(indio_dev);
+
+	return ret;
+}
+
+static int __devexit lm3533_als_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct lm3533_als *als = iio_priv(indio_dev);
+
+	lm3533_als_set_int_mode(indio_dev, false);
+	iio_device_unregister(indio_dev);
+	lm3533_als_disable(als);
+	if (als->irq)
+		free_irq(als->irq, indio_dev);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct platform_driver lm3533_als_driver = {
+	.driver	= {
+		.name	= "lm3533-als",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= lm3533_als_probe,
+	.remove		= __devexit_p(lm3533_als_remove),
+};
+module_platform_driver(lm3533_als_driver);
+
+MODULE_AUTHOR("Johan Hovold <jhovold@gmail.com>");
+MODULE_DESCRIPTION("LM3533 Ambient Light Sensor driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lm3533-als");
diff --git a/drivers/iio/light/vcnl4000.c b/drivers/iio/light/vcnl4000.c
new file mode 100644
index 00000000000..e49cb9784a6
--- /dev/null
+++ b/drivers/iio/light/vcnl4000.c
@@ -0,0 +1,217 @@
+/*
+ * vcnl4000.c - Support for Vishay VCNL4000 combined ambient light and
+ * proximity sensor
+ *
+ * Copyright 2012 Peter Meerwald <pmeerw@pmeerw.net>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for VCNL4000 (7-bit I2C slave address 0x13)
+ *
+ * TODO:
+ *   allow to adjust IR current
+ *   proximity threshold and event handling
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define VCNL4000_DRV_NAME "vcnl4000"
+
+#define VCNL4000_COMMAND	0x80 /* Command register */
+#define VCNL4000_PROD_REV	0x81 /* Product ID and Revision ID */
+#define VCNL4000_LED_CURRENT	0x83 /* IR LED current for proximity mode */
+#define VCNL4000_AL_PARAM	0x84 /* Ambient light parameter register */
+#define VCNL4000_AL_RESULT_HI	0x85 /* Ambient light result register, MSB */
+#define VCNL4000_AL_RESULT_LO	0x86 /* Ambient light result register, LSB */
+#define VCNL4000_PS_RESULT_HI	0x87 /* Proximity result register, MSB */
+#define VCNL4000_PS_RESULT_LO	0x88 /* Proximity result register, LSB */
+#define VCNL4000_PS_MEAS_FREQ	0x89 /* Proximity test signal frequency */
+#define VCNL4000_PS_MOD_ADJ	0x8a /* Proximity modulator timing adjustment */
+
+/* Bit masks for COMMAND register */
+#define VCNL4000_AL_RDY		0x40 /* ALS data ready? */
+#define VCNL4000_PS_RDY		0x20 /* proximity data ready? */
+#define VCNL4000_AL_OD		0x10 /* start on-demand ALS measurement */
+#define VCNL4000_PS_OD		0x08 /* start on-demand proximity measurement */
+
+struct vcnl4000_data {
+	struct i2c_client *client;
+};
+
+static const struct i2c_device_id vcnl4000_id[] = {
+	{ "vcnl4000", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, vcnl4000_id);
+
+static int vcnl4000_measure(struct vcnl4000_data *data, u8 req_mask,
+				u8 rdy_mask, u8 data_reg, int *val)
+{
+	int tries = 20;
+	u16 buf;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4000_COMMAND,
+					req_mask);
+	if (ret < 0)
+		return ret;
+
+	/* wait for data to become ready */
+	while (tries--) {
+		ret = i2c_smbus_read_byte_data(data->client, VCNL4000_COMMAND);
+		if (ret < 0)
+			return ret;
+		if (ret & rdy_mask)
+			break;
+		msleep(20); /* measurement takes up to 100 ms */
+	}
+
+	if (tries < 0) {
+		dev_err(&data->client->dev,
+			"vcnl4000_measure() failed, data not ready\n");
+		return -EIO;
+	}
+
+	ret = i2c_smbus_read_i2c_block_data(data->client,
+		data_reg, sizeof(buf), (u8 *) &buf);
+	if (ret < 0)
+		return ret;
+
+	*val = be16_to_cpu(buf);
+
+	return 0;
+}
+
+static const struct iio_chan_spec vcnl4000_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+	}, {
+		.type = IIO_PROXIMITY,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT,
+	}
+};
+
+static int vcnl4000_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	int ret = -EINVAL;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_LIGHT:
+			ret = vcnl4000_measure(data,
+				VCNL4000_AL_OD, VCNL4000_AL_RDY,
+				VCNL4000_AL_RESULT_HI, val);
+			if (ret < 0)
+				return ret;
+			ret = IIO_VAL_INT;
+			break;
+		case IIO_PROXIMITY:
+			ret = vcnl4000_measure(data,
+				VCNL4000_PS_OD, VCNL4000_PS_RDY,
+				VCNL4000_PS_RESULT_HI, val);
+			if (ret < 0)
+				return ret;
+			ret = IIO_VAL_INT;
+			break;
+		default:
+			break;
+		}
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type == IIO_LIGHT) {
+			*val = 0;
+			*val2 = 250000;
+			ret = IIO_VAL_INT_PLUS_MICRO;
+		}
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static const struct iio_info vcnl4000_info = {
+	.read_raw = vcnl4000_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit vcnl4000_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct vcnl4000_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = iio_device_alloc(sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	ret = i2c_smbus_read_byte_data(data->client, VCNL4000_PROD_REV);
+	if (ret < 0)
+		goto error_free_dev;
+
+	dev_info(&client->dev, "VCNL4000 Ambient light/proximity sensor, Prod %02x, Rev: %02x\n",
+		ret >> 4, ret & 0xf);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &vcnl4000_info;
+	indio_dev->channels = vcnl4000_channels;
+	indio_dev->num_channels = ARRAY_SIZE(vcnl4000_channels);
+	indio_dev->name = VCNL4000_DRV_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto error_free_dev;
+
+	return 0;
+
+error_free_dev:
+	iio_device_free(indio_dev);
+	return ret;
+}
+
+static int __devexit vcnl4000_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	iio_device_free(indio_dev);
+
+	return 0;
+}
+
+static struct i2c_driver vcnl4000_driver = {
+	.driver = {
+		.name   = VCNL4000_DRV_NAME,
+		.owner  = THIS_MODULE,
+	},
+	.probe  = vcnl4000_probe,
+	.remove = __devexit_p(vcnl4000_remove),
+	.id_table = vcnl4000_id,
+};
+
+module_i2c_driver(vcnl4000_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Vishay VCNL4000 proximity/ambient light sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 6ef660c1332..28058ae33d3 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -129,7 +129,7 @@ int rdma_translate_ip(struct sockaddr *addr, struct rdma_dev_addr *dev_addr)
 		dev_put(dev);
 		break;
 
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+#if IS_ENABLED(CONFIG_IPV6)
 	case AF_INET6:
 		rcu_read_lock();
 		for_each_netdev_rcu(&init_net, dev) {
@@ -243,7 +243,7 @@ out:
 	return ret;
 }
 
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+#if IS_ENABLED(CONFIG_IPV6)
 static int addr6_resolve(struct sockaddr_in6 *src_in,
 			 struct sockaddr_in6 *dst_in,
 			 struct rdma_dev_addr *addr)
diff --git a/drivers/infiniband/core/cm.c b/drivers/infiniband/core/cm.c
index c889aaef341..d67999f6e34 100644
--- a/drivers/infiniband/core/cm.c
+++ b/drivers/infiniband/core/cm.c
@@ -3848,24 +3848,28 @@ static int __init ib_cm_init(void)
 	INIT_LIST_HEAD(&cm.timewait_list);
 
 	ret = class_register(&cm_class);
-	if (ret)
-		return -ENOMEM;
+	if (ret) {
+		ret = -ENOMEM;
+		goto error1;
+	}
 
 	cm.wq = create_workqueue("ib_cm");
 	if (!cm.wq) {
 		ret = -ENOMEM;
-		goto error1;
+		goto error2;
 	}
 
 	ret = ib_register_client(&cm_client);
 	if (ret)
-		goto error2;
+		goto error3;
 
 	return 0;
-error2:
+error3:
 	destroy_workqueue(cm.wq);
-error1:
+error2:
 	class_unregister(&cm_class);
+error1:
+	idr_destroy(&cm.local_id_table);
 	return ret;
 }
 
diff --git a/drivers/infiniband/core/cm_msgs.h b/drivers/infiniband/core/cm_msgs.h
index 7da9b210234..be068f47e47 100644
--- a/drivers/infiniband/core/cm_msgs.h
+++ b/drivers/infiniband/core/cm_msgs.h
@@ -44,18 +44,6 @@
 
 #define IB_CM_CLASS_VERSION	2 /* IB specification 1.2 */
 
-#define CM_REQ_ATTR_ID		cpu_to_be16(0x0010)
-#define CM_MRA_ATTR_ID		cpu_to_be16(0x0011)
-#define CM_REJ_ATTR_ID		cpu_to_be16(0x0012)
-#define CM_REP_ATTR_ID		cpu_to_be16(0x0013)
-#define CM_RTU_ATTR_ID		cpu_to_be16(0x0014)
-#define CM_DREQ_ATTR_ID		cpu_to_be16(0x0015)
-#define CM_DREP_ATTR_ID		cpu_to_be16(0x0016)
-#define CM_SIDR_REQ_ATTR_ID	cpu_to_be16(0x0017)
-#define CM_SIDR_REP_ATTR_ID	cpu_to_be16(0x0018)
-#define CM_LAP_ATTR_ID		cpu_to_be16(0x0019)
-#define CM_APR_ATTR_ID		cpu_to_be16(0x001A)
-
 enum cm_msg_sequence {
 	CM_MSG_SEQUENCE_REQ,
 	CM_MSG_SEQUENCE_LAP,
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 2e826f9702c..7172559ce0c 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -99,6 +99,10 @@ struct rdma_bind_list {
 	unsigned short		port;
 };
 
+enum {
+	CMA_OPTION_AFONLY,
+};
+
 /*
  * Device removal can occur at anytime, so we need extra handling to
  * serialize notifying the user of device removal with other callbacks.
@@ -137,9 +141,11 @@ struct rdma_id_private {
 	u32			qkey;
 	u32			qp_num;
 	pid_t			owner;
+	u32			options;
 	u8			srq;
 	u8			tos;
 	u8			reuseaddr;
+	u8			afonly;
 };
 
 struct cma_multicast {
@@ -1297,8 +1303,10 @@ static void cma_set_compare_data(enum rdma_port_space ps, struct sockaddr *addr,
 		} else {
 			cma_set_ip_ver(cma_data, 4);
 			cma_set_ip_ver(cma_mask, 0xF);
-			cma_data->dst_addr.ip4.addr = ip4_addr;
-			cma_mask->dst_addr.ip4.addr = htonl(~0);
+			if (!cma_any_addr(addr)) {
+				cma_data->dst_addr.ip4.addr = ip4_addr;
+				cma_mask->dst_addr.ip4.addr = htonl(~0);
+			}
 		}
 		break;
 	case AF_INET6:
@@ -1312,9 +1320,11 @@ static void cma_set_compare_data(enum rdma_port_space ps, struct sockaddr *addr,
 		} else {
 			cma_set_ip_ver(cma_data, 6);
 			cma_set_ip_ver(cma_mask, 0xF);
-			cma_data->dst_addr.ip6 = ip6_addr;
-			memset(&cma_mask->dst_addr.ip6, 0xFF,
-			       sizeof cma_mask->dst_addr.ip6);
+			if (!cma_any_addr(addr)) {
+				cma_data->dst_addr.ip6 = ip6_addr;
+				memset(&cma_mask->dst_addr.ip6, 0xFF,
+				       sizeof cma_mask->dst_addr.ip6);
+			}
 		}
 		break;
 	default:
@@ -1499,7 +1509,7 @@ static int cma_ib_listen(struct rdma_id_private *id_priv)
 
 	addr = (struct sockaddr *) &id_priv->id.route.addr.src_addr;
 	svc_id = cma_get_service_id(id_priv->id.ps, addr);
-	if (cma_any_addr(addr))
+	if (cma_any_addr(addr) && !id_priv->afonly)
 		ret = ib_cm_listen(id_priv->cm_id.ib, svc_id, 0, NULL);
 	else {
 		cma_set_compare_data(id_priv->id.ps, addr, &compare_data);
@@ -1573,6 +1583,7 @@ static void cma_listen_on_dev(struct rdma_id_private *id_priv,
 	list_add_tail(&dev_id_priv->listen_list, &id_priv->listen_list);
 	atomic_inc(&id_priv->refcount);
 	dev_id_priv->internal_id = 1;
+	dev_id_priv->afonly = id_priv->afonly;
 
 	ret = rdma_listen(id, id_priv->backlog);
 	if (ret)
@@ -2098,6 +2109,26 @@ int rdma_set_reuseaddr(struct rdma_cm_id *id, int reuse)
 }
 EXPORT_SYMBOL(rdma_set_reuseaddr);
 
+int rdma_set_afonly(struct rdma_cm_id *id, int afonly)
+{
+	struct rdma_id_private *id_priv;
+	unsigned long flags;
+	int ret;
+
+	id_priv = container_of(id, struct rdma_id_private, id);
+	spin_lock_irqsave(&id_priv->lock, flags);
+	if (id_priv->state == RDMA_CM_IDLE || id_priv->state == RDMA_CM_ADDR_BOUND) {
+		id_priv->options |= (1 << CMA_OPTION_AFONLY);
+		id_priv->afonly = afonly;
+		ret = 0;
+	} else {
+		ret = -EINVAL;
+	}
+	spin_unlock_irqrestore(&id_priv->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(rdma_set_afonly);
+
 static void cma_bind_port(struct rdma_bind_list *bind_list,
 			  struct rdma_id_private *id_priv)
 {
@@ -2187,22 +2218,24 @@ static int cma_check_port(struct rdma_bind_list *bind_list,
 	struct hlist_node *node;
 
 	addr = (struct sockaddr *) &id_priv->id.route.addr.src_addr;
-	if (cma_any_addr(addr) && !reuseaddr)
-		return -EADDRNOTAVAIL;
-
 	hlist_for_each_entry(cur_id, node, &bind_list->owners, node) {
 		if (id_priv == cur_id)
 			continue;
 
-		if ((cur_id->state == RDMA_CM_LISTEN) ||
-		    !reuseaddr || !cur_id->reuseaddr) {
-			cur_addr = (struct sockaddr *) &cur_id->id.route.addr.src_addr;
-			if (cma_any_addr(cur_addr))
-				return -EADDRNOTAVAIL;
+		if ((cur_id->state != RDMA_CM_LISTEN) && reuseaddr &&
+		    cur_id->reuseaddr)
+			continue;
 
-			if (!cma_addr_cmp(addr, cur_addr))
-				return -EADDRINUSE;
-		}
+		cur_addr = (struct sockaddr *) &cur_id->id.route.addr.src_addr;
+		if (id_priv->afonly && cur_id->afonly &&
+		    (addr->sa_family != cur_addr->sa_family))
+			continue;
+
+		if (cma_any_addr(addr) || cma_any_addr(cur_addr))
+			return -EADDRNOTAVAIL;
+
+		if (!cma_addr_cmp(addr, cur_addr))
+			return -EADDRINUSE;
 	}
 	return 0;
 }
@@ -2278,7 +2311,7 @@ static int cma_get_port(struct rdma_id_private *id_priv)
 static int cma_check_linklocal(struct rdma_dev_addr *dev_addr,
 			       struct sockaddr *addr)
 {
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+#if IS_ENABLED(CONFIG_IPV6)
 	struct sockaddr_in6 *sin6;
 
 	if (addr->sa_family != AF_INET6)
@@ -2371,6 +2404,14 @@ int rdma_bind_addr(struct rdma_cm_id *id, struct sockaddr *addr)
 	}
 
 	memcpy(&id->route.addr.src_addr, addr, ip_addr_size(addr));
+	if (!(id_priv->options & (1 << CMA_OPTION_AFONLY))) {
+		if (addr->sa_family == AF_INET)
+			id_priv->afonly = 1;
+#if IS_ENABLED(CONFIG_IPV6)
+		else if (addr->sa_family == AF_INET6)
+			id_priv->afonly = init_net.ipv6.sysctl.bindv6only;
+#endif
+	}
 	ret = cma_get_port(id_priv);
 	if (ret)
 		goto err2;
@@ -3023,10 +3064,7 @@ static int cma_join_ib_multicast(struct rdma_id_private *id_priv,
 						id_priv->id.port_num, &rec,
 						comp_mask, GFP_KERNEL,
 						cma_ib_mc_handler, mc);
-	if (IS_ERR(mc->multicast.ib))
-		return PTR_ERR(mc->multicast.ib);
-
-	return 0;
+	return PTR_RET(mc->multicast.ib);
 }
 
 static void iboe_mcast_work_handler(struct work_struct *work)
diff --git a/drivers/infiniband/core/netlink.c b/drivers/infiniband/core/netlink.c
index e497dfbee43..3ae2bfd3101 100644
--- a/drivers/infiniband/core/netlink.c
+++ b/drivers/infiniband/core/netlink.c
@@ -108,12 +108,14 @@ void *ibnl_put_msg(struct sk_buff *skb, struct nlmsghdr **nlh, int seq,
 	unsigned char *prev_tail;
 
 	prev_tail = skb_tail_pointer(skb);
-	*nlh = NLMSG_NEW(skb, 0, seq, RDMA_NL_GET_TYPE(client, op),
-			len, NLM_F_MULTI);
+	*nlh = nlmsg_put(skb, 0, seq, RDMA_NL_GET_TYPE(client, op),
+			 len, NLM_F_MULTI);
+	if (!*nlh)
+		goto out_nlmsg_trim;
 	(*nlh)->nlmsg_len = skb_tail_pointer(skb) - prev_tail;
-	return NLMSG_DATA(*nlh);
+	return nlmsg_data(*nlh);
 
-nlmsg_failure:
+out_nlmsg_trim:
 	nlmsg_trim(skb, prev_tail);
 	return NULL;
 }
@@ -171,8 +173,11 @@ static void ibnl_rcv(struct sk_buff *skb)
 
 int __init ibnl_init(void)
 {
-	nls = netlink_kernel_create(&init_net, NETLINK_RDMA, 0, ibnl_rcv,
-				    NULL, THIS_MODULE);
+	struct netlink_kernel_cfg cfg = {
+		.input	= ibnl_rcv,
+	};
+
+	nls = netlink_kernel_create(&init_net, NETLINK_RDMA, THIS_MODULE, &cfg);
 	if (!nls) {
 		pr_warn("Failed to create netlink socket\n");
 		return -ENOMEM;
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index fbbfa24cf57..a8905abc56e 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -94,6 +94,12 @@ struct ib_sa_path_query {
 	struct ib_sa_query sa_query;
 };
 
+struct ib_sa_guidinfo_query {
+	void (*callback)(int, struct ib_sa_guidinfo_rec *, void *);
+	void *context;
+	struct ib_sa_query sa_query;
+};
+
 struct ib_sa_mcmember_query {
 	void (*callback)(int, struct ib_sa_mcmember_rec *, void *);
 	void *context;
@@ -347,6 +353,34 @@ static const struct ib_field service_rec_table[] = {
 	  .size_bits    = 2*64 },
 };
 
+#define GUIDINFO_REC_FIELD(field) \
+	.struct_offset_bytes = offsetof(struct ib_sa_guidinfo_rec, field),	\
+	.struct_size_bytes   = sizeof((struct ib_sa_guidinfo_rec *) 0)->field,	\
+	.field_name          = "sa_guidinfo_rec:" #field
+
+static const struct ib_field guidinfo_rec_table[] = {
+	{ GUIDINFO_REC_FIELD(lid),
+	  .offset_words = 0,
+	  .offset_bits  = 0,
+	  .size_bits    = 16 },
+	{ GUIDINFO_REC_FIELD(block_num),
+	  .offset_words = 0,
+	  .offset_bits  = 16,
+	  .size_bits    = 8 },
+	{ GUIDINFO_REC_FIELD(res1),
+	  .offset_words = 0,
+	  .offset_bits  = 24,
+	  .size_bits    = 8 },
+	{ GUIDINFO_REC_FIELD(res2),
+	  .offset_words = 1,
+	  .offset_bits  = 0,
+	  .size_bits    = 32 },
+	{ GUIDINFO_REC_FIELD(guid_info_list),
+	  .offset_words = 2,
+	  .offset_bits  = 0,
+	  .size_bits    = 512 },
+};
+
 static void free_sm_ah(struct kref *kref)
 {
 	struct ib_sa_sm_ah *sm_ah = container_of(kref, struct ib_sa_sm_ah, ref);
@@ -945,6 +979,105 @@ err1:
 	return ret;
 }
 
+/* Support GuidInfoRecord */
+static void ib_sa_guidinfo_rec_callback(struct ib_sa_query *sa_query,
+					int status,
+					struct ib_sa_mad *mad)
+{
+	struct ib_sa_guidinfo_query *query =
+		container_of(sa_query, struct ib_sa_guidinfo_query, sa_query);
+
+	if (mad) {
+		struct ib_sa_guidinfo_rec rec;
+
+		ib_unpack(guidinfo_rec_table, ARRAY_SIZE(guidinfo_rec_table),
+			  mad->data, &rec);
+		query->callback(status, &rec, query->context);
+	} else
+		query->callback(status, NULL, query->context);
+}
+
+static void ib_sa_guidinfo_rec_release(struct ib_sa_query *sa_query)
+{
+	kfree(container_of(sa_query, struct ib_sa_guidinfo_query, sa_query));
+}
+
+int ib_sa_guid_info_rec_query(struct ib_sa_client *client,
+			      struct ib_device *device, u8 port_num,
+			      struct ib_sa_guidinfo_rec *rec,
+			      ib_sa_comp_mask comp_mask, u8 method,
+			      int timeout_ms, gfp_t gfp_mask,
+			      void (*callback)(int status,
+					       struct ib_sa_guidinfo_rec *resp,
+					       void *context),
+			      void *context,
+			      struct ib_sa_query **sa_query)
+{
+	struct ib_sa_guidinfo_query *query;
+	struct ib_sa_device *sa_dev = ib_get_client_data(device, &sa_client);
+	struct ib_sa_port *port;
+	struct ib_mad_agent *agent;
+	struct ib_sa_mad *mad;
+	int ret;
+
+	if (!sa_dev)
+		return -ENODEV;
+
+	if (method != IB_MGMT_METHOD_GET &&
+	    method != IB_MGMT_METHOD_SET &&
+	    method != IB_SA_METHOD_DELETE) {
+		return -EINVAL;
+	}
+
+	port  = &sa_dev->port[port_num - sa_dev->start_port];
+	agent = port->agent;
+
+	query = kmalloc(sizeof *query, gfp_mask);
+	if (!query)
+		return -ENOMEM;
+
+	query->sa_query.port = port;
+	ret = alloc_mad(&query->sa_query, gfp_mask);
+	if (ret)
+		goto err1;
+
+	ib_sa_client_get(client);
+	query->sa_query.client = client;
+	query->callback        = callback;
+	query->context         = context;
+
+	mad = query->sa_query.mad_buf->mad;
+	init_mad(mad, agent);
+
+	query->sa_query.callback = callback ? ib_sa_guidinfo_rec_callback : NULL;
+	query->sa_query.release  = ib_sa_guidinfo_rec_release;
+
+	mad->mad_hdr.method	 = method;
+	mad->mad_hdr.attr_id	 = cpu_to_be16(IB_SA_ATTR_GUID_INFO_REC);
+	mad->sa_hdr.comp_mask	 = comp_mask;
+
+	ib_pack(guidinfo_rec_table, ARRAY_SIZE(guidinfo_rec_table), rec,
+		mad->data);
+
+	*sa_query = &query->sa_query;
+
+	ret = send_mad(&query->sa_query, timeout_ms, gfp_mask);
+	if (ret < 0)
+		goto err2;
+
+	return ret;
+
+err2:
+	*sa_query = NULL;
+	ib_sa_client_put(query->sa_query.client);
+	free_mad(&query->sa_query);
+
+err1:
+	kfree(query);
+	return ret;
+}
+EXPORT_SYMBOL(ib_sa_guid_info_rec_query);
+
 static void send_handler(struct ib_mad_agent *agent,
 			 struct ib_mad_send_wc *mad_send_wc)
 {
diff --git a/drivers/infiniband/core/ucma.c b/drivers/infiniband/core/ucma.c
index 8002ae642cf..6bf85042289 100644
--- a/drivers/infiniband/core/ucma.c
+++ b/drivers/infiniband/core/ucma.c
@@ -909,6 +909,13 @@ static int ucma_set_option_id(struct ucma_context *ctx, int optname,
 		}
 		ret = rdma_set_reuseaddr(ctx->cm_id, *((int *) optval) ? 1 : 0);
 		break;
+	case RDMA_OPTION_ID_AFONLY:
+		if (optlen != sizeof(int)) {
+			ret = -EINVAL;
+			break;
+		}
+		ret = rdma_set_afonly(ctx->cm_id, *((int *) optval) ? 1 : 0);
+		break;
 	default:
 		ret = -ENOSYS;
 	}
@@ -995,23 +1002,18 @@ static ssize_t ucma_set_option(struct ucma_file *file, const char __user *inbuf,
 	if (IS_ERR(ctx))
 		return PTR_ERR(ctx);
 
-	optval = kmalloc(cmd.optlen, GFP_KERNEL);
-	if (!optval) {
-		ret = -ENOMEM;
-		goto out1;
-	}
-
-	if (copy_from_user(optval, (void __user *) (unsigned long) cmd.optval,
-			   cmd.optlen)) {
-		ret = -EFAULT;
-		goto out2;
+	optval = memdup_user((void __user *) (unsigned long) cmd.optval,
+			     cmd.optlen);
+	if (IS_ERR(optval)) {
+		ret = PTR_ERR(optval);
+		goto out;
 	}
 
 	ret = ucma_set_option_level(ctx, cmd.level, cmd.optname, optval,
 				    cmd.optlen);
-out2:
 	kfree(optval);
-out1:
+
+out:
 	ucma_put_ctx(ctx);
 	return ret;
 }
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index 740dcc065cf..77b6b182778 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -1374,7 +1374,7 @@ static int pass_accept_req(struct t3cdev *tdev, struct sk_buff *skb, void *ctx)
 		goto reject;
 	}
 	dst = &rt->dst;
-	l2t = t3_l2t_get(tdev, dst, NULL);
+	l2t = t3_l2t_get(tdev, dst, NULL, &req->peer_ip);
 	if (!l2t) {
 		printk(KERN_ERR MOD "%s - failed to allocate l2t entry!\n",
 		       __func__);
@@ -1942,7 +1942,8 @@ int iwch_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		goto fail3;
 	}
 	ep->dst = &rt->dst;
-	ep->l2t = t3_l2t_get(ep->com.tdev, ep->dst, NULL);
+	ep->l2t = t3_l2t_get(ep->com.tdev, ep->dst, NULL,
+			     &cm_id->remote_addr.sin_addr.s_addr);
 	if (!ep->l2t) {
 		printk(KERN_ERR MOD "%s - cannot alloc l2e.\n", __func__);
 		err = -ENOMEM;
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index b18870c455a..51f42061dae 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -548,8 +548,8 @@ static void send_mpa_req(struct c4iw_ep *ep, struct sk_buff *skb,
 	}
 
 	if (mpa_rev_to_use == 2) {
-		mpa->private_data_size +=
-			htons(sizeof(struct mpa_v2_conn_params));
+		mpa->private_data_size = htons(ntohs(mpa->private_data_size) +
+					       sizeof (struct mpa_v2_conn_params));
 		mpa_v2_params.ird = htons((u16)ep->ird);
 		mpa_v2_params.ord = htons((u16)ep->ord);
 
@@ -635,8 +635,8 @@ static int send_mpa_reject(struct c4iw_ep *ep, const void *pdata, u8 plen)
 
 	if (ep->mpa_attr.version == 2 && ep->mpa_attr.enhanced_rdma_conn) {
 		mpa->flags |= MPA_ENHANCED_RDMA_CONN;
-		mpa->private_data_size +=
-			htons(sizeof(struct mpa_v2_conn_params));
+		mpa->private_data_size = htons(ntohs(mpa->private_data_size) +
+					       sizeof (struct mpa_v2_conn_params));
 		mpa_v2_params.ird = htons(((u16)ep->ird) |
 					  (peer2peer ? MPA_V2_PEER2PEER_MODEL :
 					   0));
@@ -715,8 +715,8 @@ static int send_mpa_reply(struct c4iw_ep *ep, const void *pdata, u8 plen)
 
 	if (ep->mpa_attr.version == 2 && ep->mpa_attr.enhanced_rdma_conn) {
 		mpa->flags |= MPA_ENHANCED_RDMA_CONN;
-		mpa->private_data_size +=
-			htons(sizeof(struct mpa_v2_conn_params));
+		mpa->private_data_size = htons(ntohs(mpa->private_data_size) +
+					       sizeof (struct mpa_v2_conn_params));
 		mpa_v2_params.ird = htons((u16)ep->ird);
 		mpa_v2_params.ord = htons((u16)ep->ord);
 		if (peer2peer && (ep->mpa_attr.p2p_type !=
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index 259b0670b51..c27141fef1a 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -147,47 +147,51 @@ static void update_sm_ah(struct mlx4_ib_dev *dev, u8 port_num, u16 lid, u8 sl)
 }
 
 /*
- * Snoop SM MADs for port info and P_Key table sets, so we can
- * synthesize LID change and P_Key change events.
+ * Snoop SM MADs for port info, GUID info, and  P_Key table sets, so we can
+ * synthesize LID change, Client-Rereg, GID change, and P_Key change events.
  */
 static void smp_snoop(struct ib_device *ibdev, u8 port_num, struct ib_mad *mad,
-				u16 prev_lid)
+		      u16 prev_lid)
 {
-	struct ib_event event;
+	struct ib_port_info *pinfo;
+	u16 lid;
 
+	struct mlx4_ib_dev *dev = to_mdev(ibdev);
 	if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED ||
 	     mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) &&
-	    mad->mad_hdr.method == IB_MGMT_METHOD_SET) {
-		if (mad->mad_hdr.attr_id == IB_SMP_ATTR_PORT_INFO) {
-			struct ib_port_info *pinfo =
-				(struct ib_port_info *) ((struct ib_smp *) mad)->data;
-			u16 lid = be16_to_cpu(pinfo->lid);
+	    mad->mad_hdr.method == IB_MGMT_METHOD_SET)
+		switch (mad->mad_hdr.attr_id) {
+		case IB_SMP_ATTR_PORT_INFO:
+			pinfo = (struct ib_port_info *) ((struct ib_smp *) mad)->data;
+			lid = be16_to_cpu(pinfo->lid);
 
-			update_sm_ah(to_mdev(ibdev), port_num,
+			update_sm_ah(dev, port_num,
 				     be16_to_cpu(pinfo->sm_lid),
 				     pinfo->neighbormtu_mastersmsl & 0xf);
 
-			event.device	       = ibdev;
-			event.element.port_num = port_num;
+			if (pinfo->clientrereg_resv_subnetto & 0x80)
+				mlx4_ib_dispatch_event(dev, port_num,
+						       IB_EVENT_CLIENT_REREGISTER);
 
-			if (pinfo->clientrereg_resv_subnetto & 0x80) {
-				event.event    = IB_EVENT_CLIENT_REREGISTER;
-				ib_dispatch_event(&event);
-			}
+			if (prev_lid != lid)
+				mlx4_ib_dispatch_event(dev, port_num,
+						       IB_EVENT_LID_CHANGE);
+			break;
 
-			if (prev_lid != lid) {
-				event.event    = IB_EVENT_LID_CHANGE;
-				ib_dispatch_event(&event);
-			}
-		}
+		case IB_SMP_ATTR_PKEY_TABLE:
+			mlx4_ib_dispatch_event(dev, port_num,
+					       IB_EVENT_PKEY_CHANGE);
+			break;
 
-		if (mad->mad_hdr.attr_id == IB_SMP_ATTR_PKEY_TABLE) {
-			event.device	       = ibdev;
-			event.event	       = IB_EVENT_PKEY_CHANGE;
-			event.element.port_num = port_num;
-			ib_dispatch_event(&event);
+		case IB_SMP_ATTR_GUID_INFO:
+			/* paravirtualized master's guid is guid 0 -- does not change */
+			if (!mlx4_is_master(dev->dev))
+				mlx4_ib_dispatch_event(dev, port_num,
+						       IB_EVENT_GID_CHANGE);
+			break;
+		default:
+			break;
 		}
-	}
 }
 
 static void node_desc_override(struct ib_device *dev,
@@ -242,6 +246,25 @@ static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 	int err;
 	struct ib_port_attr pattr;
 
+	if (in_wc && in_wc->qp->qp_num) {
+		pr_debug("received MAD: slid:%d sqpn:%d "
+			"dlid_bits:%d dqpn:%d wc_flags:0x%x, cls %x, mtd %x, atr %x\n",
+			in_wc->slid, in_wc->src_qp,
+			in_wc->dlid_path_bits,
+			in_wc->qp->qp_num,
+			in_wc->wc_flags,
+			in_mad->mad_hdr.mgmt_class, in_mad->mad_hdr.method,
+			be16_to_cpu(in_mad->mad_hdr.attr_id));
+		if (in_wc->wc_flags & IB_WC_GRH) {
+			pr_debug("sgid_hi:0x%016llx sgid_lo:0x%016llx\n",
+				 be64_to_cpu(in_grh->sgid.global.subnet_prefix),
+				 be64_to_cpu(in_grh->sgid.global.interface_id));
+			pr_debug("dgid_hi:0x%016llx dgid_lo:0x%016llx\n",
+				 be64_to_cpu(in_grh->dgid.global.subnet_prefix),
+				 be64_to_cpu(in_grh->dgid.global.interface_id));
+		}
+	}
+
 	slid = in_wc ? in_wc->slid : be16_to_cpu(IB_LID_PERMISSIVE);
 
 	if (in_mad->mad_hdr.method == IB_MGMT_METHOD_TRAP && slid == 0) {
@@ -286,7 +309,8 @@ static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 		return IB_MAD_RESULT_FAILURE;
 
 	if (!out_mad->mad_hdr.status) {
-		smp_snoop(ibdev, port_num, in_mad, prev_lid);
+		if (!(to_mdev(ibdev)->dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV))
+			smp_snoop(ibdev, port_num, in_mad, prev_lid);
 		node_desc_override(ibdev, out_mad);
 	}
 
@@ -427,3 +451,64 @@ void mlx4_ib_mad_cleanup(struct mlx4_ib_dev *dev)
 			ib_destroy_ah(dev->sm_ah[p]);
 	}
 }
+
+void handle_port_mgmt_change_event(struct work_struct *work)
+{
+	struct ib_event_work *ew = container_of(work, struct ib_event_work, work);
+	struct mlx4_ib_dev *dev = ew->ib_dev;
+	struct mlx4_eqe *eqe = &(ew->ib_eqe);
+	u8 port = eqe->event.port_mgmt_change.port;
+	u32 changed_attr;
+
+	switch (eqe->subtype) {
+	case MLX4_DEV_PMC_SUBTYPE_PORT_INFO:
+		changed_attr = be32_to_cpu(eqe->event.port_mgmt_change.params.port_info.changed_attr);
+
+		/* Update the SM ah - This should be done before handling
+		   the other changed attributes so that MADs can be sent to the SM */
+		if (changed_attr & MSTR_SM_CHANGE_MASK) {
+			u16 lid = be16_to_cpu(eqe->event.port_mgmt_change.params.port_info.mstr_sm_lid);
+			u8 sl = eqe->event.port_mgmt_change.params.port_info.mstr_sm_sl & 0xf;
+			update_sm_ah(dev, port, lid, sl);
+		}
+
+		/* Check if it is a lid change event */
+		if (changed_attr & MLX4_EQ_PORT_INFO_LID_CHANGE_MASK)
+			mlx4_ib_dispatch_event(dev, port, IB_EVENT_LID_CHANGE);
+
+		/* Generate GUID changed event */
+		if (changed_attr & MLX4_EQ_PORT_INFO_GID_PFX_CHANGE_MASK)
+			mlx4_ib_dispatch_event(dev, port, IB_EVENT_GID_CHANGE);
+
+		if (changed_attr & MLX4_EQ_PORT_INFO_CLIENT_REREG_MASK)
+			mlx4_ib_dispatch_event(dev, port,
+					       IB_EVENT_CLIENT_REREGISTER);
+		break;
+
+	case MLX4_DEV_PMC_SUBTYPE_PKEY_TABLE:
+		mlx4_ib_dispatch_event(dev, port, IB_EVENT_PKEY_CHANGE);
+		break;
+	case MLX4_DEV_PMC_SUBTYPE_GUID_INFO:
+		/* paravirtualized master's guid is guid 0 -- does not change */
+		if (!mlx4_is_master(dev->dev))
+			mlx4_ib_dispatch_event(dev, port, IB_EVENT_GID_CHANGE);
+		break;
+	default:
+		pr_warn("Unsupported subtype 0x%x for "
+			"Port Management Change event\n", eqe->subtype);
+	}
+
+	kfree(ew);
+}
+
+void mlx4_ib_dispatch_event(struct mlx4_ib_dev *dev, u8 port_num,
+			    enum ib_event_type type)
+{
+	struct ib_event event;
+
+	event.device		= &dev->ib_dev;
+	event.element.port_num	= port_num;
+	event.event		= type;
+
+	ib_dispatch_event(&event);
+}
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 3530c41fcd1..fe2088cfa6e 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -50,7 +50,7 @@
 #include "mlx4_ib.h"
 #include "user.h"
 
-#define DRV_NAME	"mlx4_ib"
+#define DRV_NAME	MLX4_IB_DRV_NAME
 #define DRV_VERSION	"1.0"
 #define DRV_RELDATE	"April 4, 2008"
 
@@ -157,7 +157,7 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 	props->local_ca_ack_delay  = dev->dev->caps.local_ca_ack_delay;
 	props->atomic_cap	   = dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_ATOMIC ?
 		IB_ATOMIC_HCA : IB_ATOMIC_NONE;
-	props->masked_atomic_cap   = IB_ATOMIC_HCA;
+	props->masked_atomic_cap   = props->atomic_cap;
 	props->max_pkeys	   = dev->dev->caps.pkey_table_len[1];
 	props->max_mcast_grp	   = dev->dev->caps.num_mgms + dev->dev->caps.num_amgms;
 	props->max_mcast_qp_attach = dev->dev->caps.num_qp_per_mgm;
@@ -718,26 +718,53 @@ int mlx4_ib_add_mc(struct mlx4_ib_dev *mdev, struct mlx4_ib_qp *mqp,
 	return ret;
 }
 
+struct mlx4_ib_steering {
+	struct list_head list;
+	u64 reg_id;
+	union ib_gid gid;
+};
+
 static int mlx4_ib_mcg_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 {
 	int err;
 	struct mlx4_ib_dev *mdev = to_mdev(ibqp->device);
 	struct mlx4_ib_qp *mqp = to_mqp(ibqp);
+	u64 reg_id;
+	struct mlx4_ib_steering *ib_steering = NULL;
+
+	if (mdev->dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		ib_steering = kmalloc(sizeof(*ib_steering), GFP_KERNEL);
+		if (!ib_steering)
+			return -ENOMEM;
+	}
 
-	err = mlx4_multicast_attach(mdev->dev, &mqp->mqp, gid->raw,
-				    !!(mqp->flags & MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK),
-				    MLX4_PROT_IB_IPV6);
+	err = mlx4_multicast_attach(mdev->dev, &mqp->mqp, gid->raw, mqp->port,
+				    !!(mqp->flags &
+				       MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK),
+				    MLX4_PROT_IB_IPV6, &reg_id);
 	if (err)
-		return err;
+		goto err_malloc;
 
 	err = add_gid_entry(ibqp, gid);
 	if (err)
 		goto err_add;
 
+	if (ib_steering) {
+		memcpy(ib_steering->gid.raw, gid->raw, 16);
+		ib_steering->reg_id = reg_id;
+		mutex_lock(&mqp->mutex);
+		list_add(&ib_steering->list, &mqp->steering_rules);
+		mutex_unlock(&mqp->mutex);
+	}
 	return 0;
 
 err_add:
-	mlx4_multicast_detach(mdev->dev, &mqp->mqp, gid->raw, MLX4_PROT_IB_IPV6);
+	mlx4_multicast_detach(mdev->dev, &mqp->mqp, gid->raw,
+			      MLX4_PROT_IB_IPV6, reg_id);
+err_malloc:
+	kfree(ib_steering);
+
 	return err;
 }
 
@@ -765,9 +792,30 @@ static int mlx4_ib_mcg_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 	u8 mac[6];
 	struct net_device *ndev;
 	struct mlx4_ib_gid_entry *ge;
+	u64 reg_id = 0;
+
+	if (mdev->dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		struct mlx4_ib_steering *ib_steering;
+
+		mutex_lock(&mqp->mutex);
+		list_for_each_entry(ib_steering, &mqp->steering_rules, list) {
+			if (!memcmp(ib_steering->gid.raw, gid->raw, 16)) {
+				list_del(&ib_steering->list);
+				break;
+			}
+		}
+		mutex_unlock(&mqp->mutex);
+		if (&ib_steering->list == &mqp->steering_rules) {
+			pr_err("Couldn't find reg_id for mgid. Steering rule is left attached\n");
+			return -EINVAL;
+		}
+		reg_id = ib_steering->reg_id;
+		kfree(ib_steering);
+	}
 
-	err = mlx4_multicast_detach(mdev->dev,
-				    &mqp->mqp, gid->raw, MLX4_PROT_IB_IPV6);
+	err = mlx4_multicast_detach(mdev->dev, &mqp->mqp, gid->raw,
+				    MLX4_PROT_IB_IPV6, reg_id);
 	if (err)
 		return err;
 
@@ -898,7 +946,6 @@ static void update_gids_task(struct work_struct *work)
 	union ib_gid *gids;
 	int err;
 	struct mlx4_dev	*dev = gw->dev->dev;
-	struct ib_event event;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox)) {
@@ -916,10 +963,7 @@ static void update_gids_task(struct work_struct *work)
 		pr_warn("set port command failed\n");
 	else {
 		memcpy(gw->dev->iboe.gid_table[gw->port - 1], gw->gids, sizeof gw->gids);
-		event.device = &gw->dev->ib_dev;
-		event.element.port_num = gw->port;
-		event.event    = IB_EVENT_GID_CHANGE;
-		ib_dispatch_event(&event);
+		mlx4_ib_dispatch_event(gw->dev, gw->port, IB_EVENT_GID_CHANGE);
 	}
 
 	mlx4_free_cmd_mailbox(dev, mailbox);
@@ -1111,7 +1155,8 @@ static void mlx4_ib_alloc_eqs(struct mlx4_dev *dev, struct mlx4_ib_dev *ibdev)
 			sprintf(name, "mlx4-ib-%d-%d@%s",
 				i, j, dev->pdev->bus->name);
 			/* Set IRQ for specific name (per ring) */
-			if (mlx4_assign_eq(dev, name, &ibdev->eq_table[eq])) {
+			if (mlx4_assign_eq(dev, name, NULL,
+					   &ibdev->eq_table[eq])) {
 				/* Use legacy (same as mlx4_en driver) */
 				pr_warn("Can't allocate EQ %d; reverting to legacy\n", eq);
 				ibdev->eq_table[eq] =
@@ -1383,10 +1428,18 @@ static void mlx4_ib_remove(struct mlx4_dev *dev, void *ibdev_ptr)
 }
 
 static void mlx4_ib_event(struct mlx4_dev *dev, void *ibdev_ptr,
-			  enum mlx4_dev_event event, int port)
+			  enum mlx4_dev_event event, unsigned long param)
 {
 	struct ib_event ibev;
 	struct mlx4_ib_dev *ibdev = to_mdev((struct ib_device *) ibdev_ptr);
+	struct mlx4_eqe *eqe = NULL;
+	struct ib_event_work *ew;
+	int port = 0;
+
+	if (event == MLX4_DEV_EVENT_PORT_MGMT_CHANGE)
+		eqe = (struct mlx4_eqe *)param;
+	else
+		port = (u8)param;
 
 	if (port > ibdev->num_ports)
 		return;
@@ -1405,6 +1458,19 @@ static void mlx4_ib_event(struct mlx4_dev *dev, void *ibdev_ptr,
 		ibev.event = IB_EVENT_DEVICE_FATAL;
 		break;
 
+	case MLX4_DEV_EVENT_PORT_MGMT_CHANGE:
+		ew = kmalloc(sizeof *ew, GFP_ATOMIC);
+		if (!ew) {
+			pr_err("failed to allocate memory for events work\n");
+			break;
+		}
+
+		INIT_WORK(&ew->work, handle_port_mgmt_change_event);
+		memcpy(&ew->ib_eqe, eqe, sizeof *eqe);
+		ew->ib_dev = ibdev;
+		handle_port_mgmt_change_event(&ew->work);
+		return;
+
 	default:
 		return;
 	}
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index ff36655d23d..c136bb618e2 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -44,6 +44,16 @@
 #include <linux/mlx4/device.h>
 #include <linux/mlx4/doorbell.h>
 
+#define MLX4_IB_DRV_NAME	"mlx4_ib"
+
+#ifdef pr_fmt
+#undef pr_fmt
+#endif
+#define pr_fmt(fmt)	"<" MLX4_IB_DRV_NAME "> %s: " fmt, __func__
+
+#define mlx4_ib_warn(ibdev, format, arg...) \
+	dev_warn((ibdev)->dma_device, MLX4_IB_DRV_NAME ": " format, ## arg)
+
 enum {
 	MLX4_IB_SQ_MIN_WQE_SHIFT = 6,
 	MLX4_IB_MAX_HEADROOM	 = 2048
@@ -163,6 +173,7 @@ struct mlx4_ib_qp {
 	u8			state;
 	int			mlx_type;
 	struct list_head	gid_list;
+	struct list_head	steering_rules;
 };
 
 struct mlx4_ib_srq {
@@ -214,6 +225,12 @@ struct mlx4_ib_dev {
 	int			eq_added;
 };
 
+struct ib_event_work {
+	struct work_struct	work;
+	struct mlx4_ib_dev	*ib_dev;
+	struct mlx4_eqe		ib_eqe;
+};
+
 static inline struct mlx4_ib_dev *to_mdev(struct ib_device *ibdev)
 {
 	return container_of(ibdev, struct mlx4_ib_dev, ib_dev);
@@ -371,4 +388,7 @@ static inline int mlx4_ib_ah_grh_present(struct mlx4_ib_ah *ah)
 int mlx4_ib_add_mc(struct mlx4_ib_dev *mdev, struct mlx4_ib_qp *mqp,
 		   union ib_gid *gid);
 
+void mlx4_ib_dispatch_event(struct mlx4_ib_dev *dev, u8 port_num,
+			    enum ib_event_type type);
+
 #endif /* MLX4_IB_H */
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 8d4ed24aef9..a6d8ea060ea 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -495,6 +495,7 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 	spin_lock_init(&qp->sq.lock);
 	spin_lock_init(&qp->rq.lock);
 	INIT_LIST_HEAD(&qp->gid_list);
+	INIT_LIST_HEAD(&qp->steering_rules);
 
 	qp->state	 = IB_QPS_RESET;
 	if (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR)
@@ -1335,11 +1336,21 @@ int mlx4_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	cur_state = attr_mask & IB_QP_CUR_STATE ? attr->cur_qp_state : qp->state;
 	new_state = attr_mask & IB_QP_STATE ? attr->qp_state : cur_state;
 
-	if (!ib_modify_qp_is_ok(cur_state, new_state, ibqp->qp_type, attr_mask))
+	if (!ib_modify_qp_is_ok(cur_state, new_state, ibqp->qp_type, attr_mask)) {
+		pr_debug("qpn 0x%x: invalid attribute mask specified "
+			 "for transition %d to %d. qp_type %d,"
+			 " attr_mask 0x%x\n",
+			 ibqp->qp_num, cur_state, new_state,
+			 ibqp->qp_type, attr_mask);
 		goto out;
+	}
 
 	if ((attr_mask & IB_QP_PORT) &&
 	    (attr->port_num == 0 || attr->port_num > dev->dev->caps.num_ports)) {
+		pr_debug("qpn 0x%x: invalid port number (%d) specified "
+			 "for transition %d to %d. qp_type %d\n",
+			 ibqp->qp_num, attr->port_num, cur_state,
+			 new_state, ibqp->qp_type);
 		goto out;
 	}
 
@@ -1350,17 +1361,30 @@ int mlx4_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 
 	if (attr_mask & IB_QP_PKEY_INDEX) {
 		int p = attr_mask & IB_QP_PORT ? attr->port_num : qp->port;
-		if (attr->pkey_index >= dev->dev->caps.pkey_table_len[p])
+		if (attr->pkey_index >= dev->dev->caps.pkey_table_len[p]) {
+			pr_debug("qpn 0x%x: invalid pkey index (%d) specified "
+				 "for transition %d to %d. qp_type %d\n",
+				 ibqp->qp_num, attr->pkey_index, cur_state,
+				 new_state, ibqp->qp_type);
 			goto out;
+		}
 	}
 
 	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC &&
 	    attr->max_rd_atomic > dev->dev->caps.max_qp_init_rdma) {
+		pr_debug("qpn 0x%x: max_rd_atomic (%d) too large. "
+			 "Transition %d to %d. qp_type %d\n",
+			 ibqp->qp_num, attr->max_rd_atomic, cur_state,
+			 new_state, ibqp->qp_type);
 		goto out;
 	}
 
 	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC &&
 	    attr->max_dest_rd_atomic > dev->dev->caps.max_qp_dest_rdma) {
+		pr_debug("qpn 0x%x: max_dest_rd_atomic (%d) too large. "
+			 "Transition %d to %d. qp_type %d\n",
+			 ibqp->qp_num, attr->max_dest_rd_atomic, cur_state,
+			 new_state, ibqp->qp_type);
 		goto out;
 	}
 
diff --git a/drivers/infiniband/hw/mthca/mthca_qp.c b/drivers/infiniband/hw/mthca/mthca_qp.c
index 9601049e14d..26a68453610 100644
--- a/drivers/infiniband/hw/mthca/mthca_qp.c
+++ b/drivers/infiniband/hw/mthca/mthca_qp.c
@@ -247,7 +247,8 @@ void mthca_qp_event(struct mthca_dev *dev, u32 qpn,
 	spin_unlock(&dev->qp_table.lock);
 
 	if (!qp) {
-		mthca_warn(dev, "Async event for bogus QP %08x\n", qpn);
+		mthca_warn(dev, "Async event %d for bogus QP %08x\n",
+			   event_type, qpn);
 		return;
 	}
 
@@ -501,6 +502,7 @@ done:
 	qp_attr->cap.max_inline_data = qp->max_inline_data;
 
 	qp_init_attr->cap	     = qp_attr->cap;
+	qp_init_attr->sq_sig_type    = qp->sq_policy;
 
 out_mailbox:
 	mthca_free_mailbox(dev, mailbox);
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_main.c b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
index b050e629e9c..5a044526e4f 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_main.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
@@ -202,8 +202,7 @@ static int ocrdma_build_sgid_tbl(struct ocrdma_dev *dev)
 	return 0;
 }
 
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE) || \
-defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_IPV6) || IS_ENABLED(CONFIG_VLAN_8021Q)
 
 static int ocrdma_inet6addr_event(struct notifier_block *notifier,
 				  unsigned long event, void *ptr)
@@ -549,7 +548,7 @@ static struct ocrdma_driver ocrdma_drv = {
 
 static void ocrdma_unregister_inet6addr_notifier(void)
 {
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+#if IS_ENABLED(CONFIG_IPV6)
 	unregister_inet6addr_notifier(&ocrdma_inet6addr_notifier);
 #endif
 }
@@ -558,7 +557,7 @@ static int __init ocrdma_init_module(void)
 {
 	int status;
 
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+#if IS_ENABLED(CONFIG_IPV6)
 	status = register_inet6addr_notifier(&ocrdma_inet6addr_notifier);
 	if (status)
 		return status;
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index 2e2e7aecc99..cb5b7f7d4d3 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -97,7 +97,7 @@ int ocrdma_query_device(struct ib_device *ibdev, struct ib_device_attr *attr)
 	    min(dev->attr.max_ord_per_qp, dev->attr.max_ird_per_qp);
 	attr->max_qp_init_rd_atom = dev->attr.max_ord_per_qp;
 	attr->max_srq = (dev->attr.max_qp - 1);
-	attr->max_srq_sge = attr->max_srq_sge;
+	attr->max_srq_sge = dev->attr.max_srq_sge;
 	attr->max_srq_wr = dev->attr.max_rqe;
 	attr->local_ca_ack_delay = dev->attr.local_ca_ack_delay;
 	attr->max_fast_reg_page_list_len = 0;
@@ -893,7 +893,9 @@ static int ocrdma_check_qp_params(struct ib_pd *ibpd, struct ocrdma_dev *dev,
 	/* verify consumer QPs are not trying to use GSI QP's CQ */
 	if ((attrs->qp_type != IB_QPT_GSI) && (dev->gsi_qp_created)) {
 		if ((dev->gsi_sqcq == get_ocrdma_cq(attrs->send_cq)) ||
-		    (dev->gsi_sqcq == get_ocrdma_cq(attrs->send_cq))) {
+		    (dev->gsi_sqcq == get_ocrdma_cq(attrs->recv_cq)) ||
+		    (dev->gsi_rqcq == get_ocrdma_cq(attrs->send_cq)) ||
+		    (dev->gsi_rqcq == get_ocrdma_cq(attrs->recv_cq))) {
 			ocrdma_err("%s(%d) Consumer QP cannot use GSI CQs.\n",
 				   __func__, dev->id);
 			return -EINVAL;
diff --git a/drivers/infiniband/hw/qib/qib.h b/drivers/infiniband/hw/qib/qib.h
index 7e62f413714..7b1b8669002 100644
--- a/drivers/infiniband/hw/qib/qib.h
+++ b/drivers/infiniband/hw/qib/qib.h
@@ -1,8 +1,8 @@
 #ifndef _QIB_KERNEL_H
 #define _QIB_KERNEL_H
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -519,6 +519,7 @@ struct qib_pportdata {
 	struct qib_devdata *dd;
 	struct qib_chippport_specific *cpspec; /* chip-specific per-port */
 	struct kobject pport_kobj;
+	struct kobject pport_cc_kobj;
 	struct kobject sl2vl_kobj;
 	struct kobject diagc_kobj;
 
@@ -544,6 +545,7 @@ struct qib_pportdata {
 
 	/* read mostly */
 	struct qib_sdma_desc *sdma_descq;
+	struct workqueue_struct *qib_wq;
 	struct qib_sdma_state sdma_state;
 	dma_addr_t       sdma_descq_phys;
 	volatile __le64 *sdma_head_dma; /* DMA'ed by chip */
@@ -637,6 +639,39 @@ struct qib_pportdata {
 	struct timer_list led_override_timer;
 	struct xmit_wait cong_stats;
 	struct timer_list symerr_clear_timer;
+
+	/* Synchronize access between driver writes and sysfs reads */
+	spinlock_t cc_shadow_lock
+		____cacheline_aligned_in_smp;
+
+	/* Shadow copy of the congestion control table */
+	struct cc_table_shadow *ccti_entries_shadow;
+
+	/* Shadow copy of the congestion control entries */
+	struct ib_cc_congestion_setting_attr_shadow *congestion_entries_shadow;
+
+	/* List of congestion control table entries */
+	struct ib_cc_table_entry_shadow *ccti_entries;
+
+	/* 16 congestion entries with each entry corresponding to a SL */
+	struct ib_cc_congestion_entry_shadow *congestion_entries;
+
+	/* Maximum number of congestion control entries that the agent expects
+	 * the manager to send.
+	 */
+	u16 cc_supported_table_entries;
+
+	/* Total number of congestion control table entries */
+	u16 total_cct_entry;
+
+	/* Bit map identifying service level */
+	u16 cc_sl_control_map;
+
+	/* maximum congestion control table index */
+	u16 ccti_limit;
+
+	/* CA's max number of 64 entry units in the congestion control table */
+	u8 cc_max_table_entries;
 };
 
 /* Observers. Not to be taken lightly, possibly not to ship. */
@@ -1077,6 +1112,7 @@ extern u32 qib_cpulist_count;
 extern unsigned long *qib_cpulist;
 
 extern unsigned qib_wc_pat;
+extern unsigned qib_cc_table_size;
 int qib_init(struct qib_devdata *, int);
 int init_chip_wc_pat(struct qib_devdata *dd, u32);
 int qib_enable_wc(struct qib_devdata *dd);
@@ -1267,6 +1303,11 @@ int qib_sdma_verbs_send(struct qib_pportdata *, struct qib_sge_state *,
 /* ppd->sdma_lock should be locked before calling this. */
 int qib_sdma_make_progress(struct qib_pportdata *dd);
 
+static inline int qib_sdma_empty(const struct qib_pportdata *ppd)
+{
+	return ppd->sdma_descq_added == ppd->sdma_descq_removed;
+}
+
 /* must be called under qib_sdma_lock */
 static inline u16 qib_sdma_descq_freecnt(const struct qib_pportdata *ppd)
 {
diff --git a/drivers/infiniband/hw/qib/qib_diag.c b/drivers/infiniband/hw/qib/qib_diag.c
index 9892456a434..1686fd4bda8 100644
--- a/drivers/infiniband/hw/qib/qib_diag.c
+++ b/drivers/infiniband/hw/qib/qib_diag.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2010 QLogic Corporation. All rights reserved.
- * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -53,6 +53,9 @@
 #include "qib.h"
 #include "qib_common.h"
 
+#undef pr_fmt
+#define pr_fmt(fmt) QIB_DRV_NAME ": " fmt
+
 /*
  * Each client that opens the diag device must read then write
  * offset 0, to prevent lossage from random cat or od. diag_state
@@ -598,8 +601,8 @@ static ssize_t qib_diagpkt_write(struct file *fp,
 	}
 	tmpbuf = vmalloc(plen);
 	if (!tmpbuf) {
-		qib_devinfo(dd->pcidev, "Unable to allocate tmp buffer, "
-			 "failing\n");
+		qib_devinfo(dd->pcidev,
+			"Unable to allocate tmp buffer, failing\n");
 		ret = -ENOMEM;
 		goto bail;
 	}
@@ -693,7 +696,7 @@ int qib_register_observer(struct qib_devdata *dd,
 	ret = -ENOMEM;
 	olp = vmalloc(sizeof *olp);
 	if (!olp) {
-		printk(KERN_ERR QIB_DRV_NAME ": vmalloc for observer failed\n");
+		pr_err("vmalloc for observer failed\n");
 		goto bail;
 	}
 	if (olp) {
diff --git a/drivers/infiniband/hw/qib/qib_driver.c b/drivers/infiniband/hw/qib/qib_driver.c
index 8895cfec501..e41e7f7fc76 100644
--- a/drivers/infiniband/hw/qib/qib_driver.c
+++ b/drivers/infiniband/hw/qib/qib_driver.c
@@ -764,8 +764,9 @@ int qib_reset_device(int unit)
 	qib_devinfo(dd->pcidev, "Reset on unit %u requested\n", unit);
 
 	if (!dd->kregbase || !(dd->flags & QIB_PRESENT)) {
-		qib_devinfo(dd->pcidev, "Invalid unit number %u or "
-			    "not initialized or not present\n", unit);
+		qib_devinfo(dd->pcidev,
+			"Invalid unit number %u or not initialized or not present\n",
+			unit);
 		ret = -ENXIO;
 		goto bail;
 	}
@@ -802,11 +803,13 @@ int qib_reset_device(int unit)
 	else
 		ret = -EAGAIN;
 	if (ret)
-		qib_dev_err(dd, "Reinitialize unit %u after "
-			    "reset failed with %d\n", unit, ret);
+		qib_dev_err(dd,
+			"Reinitialize unit %u after reset failed with %d\n",
+			unit, ret);
 	else
-		qib_devinfo(dd->pcidev, "Reinitialized unit %u after "
-			    "resetting\n", unit);
+		qib_devinfo(dd->pcidev,
+			"Reinitialized unit %u after resetting\n",
+			unit);
 
 bail:
 	return ret;
diff --git a/drivers/infiniband/hw/qib/qib_eeprom.c b/drivers/infiniband/hw/qib/qib_eeprom.c
index 92d9cfe98a6..4d5d71aaa2b 100644
--- a/drivers/infiniband/hw/qib/qib_eeprom.c
+++ b/drivers/infiniband/hw/qib/qib_eeprom.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -160,10 +161,9 @@ void qib_get_eeprom_info(struct qib_devdata *dd)
 		if (oguid > bguid[7]) {
 			if (bguid[6] == 0xff) {
 				if (bguid[5] == 0xff) {
-					qib_dev_err(dd, "Can't set %s GUID"
-						    " from base, wraps to"
-						    " OUI!\n",
-						    qib_get_unit_name(t));
+					qib_dev_err(dd,
+						"Can't set %s GUID from base, wraps to OUI!\n",
+						qib_get_unit_name(t));
 					dd->base_guid = 0;
 					goto bail;
 				}
@@ -182,8 +182,9 @@ void qib_get_eeprom_info(struct qib_devdata *dd)
 	len = sizeof(struct qib_flash);
 	buf = vmalloc(len);
 	if (!buf) {
-		qib_dev_err(dd, "Couldn't allocate memory to read %u "
-			    "bytes from eeprom for GUID\n", len);
+		qib_dev_err(dd,
+			"Couldn't allocate memory to read %u bytes from eeprom for GUID\n",
+			len);
 		goto bail;
 	}
 
@@ -201,23 +202,25 @@ void qib_get_eeprom_info(struct qib_devdata *dd)
 
 	csum = flash_csum(ifp, 0);
 	if (csum != ifp->if_csum) {
-		qib_devinfo(dd->pcidev, "Bad I2C flash checksum: "
-			 "0x%x, not 0x%x\n", csum, ifp->if_csum);
+		qib_devinfo(dd->pcidev,
+			"Bad I2C flash checksum: 0x%x, not 0x%x\n",
+			csum, ifp->if_csum);
 		goto done;
 	}
 	if (*(__be64 *) ifp->if_guid == cpu_to_be64(0) ||
 	    *(__be64 *) ifp->if_guid == ~cpu_to_be64(0)) {
-		qib_dev_err(dd, "Invalid GUID %llx from flash; ignoring\n",
-			    *(unsigned long long *) ifp->if_guid);
+		qib_dev_err(dd,
+			"Invalid GUID %llx from flash; ignoring\n",
+			*(unsigned long long *) ifp->if_guid);
 		/* don't allow GUID if all 0 or all 1's */
 		goto done;
 	}
 
 	/* complain, but allow it */
 	if (*(u64 *) ifp->if_guid == 0x100007511000000ULL)
-		qib_devinfo(dd->pcidev, "Warning, GUID %llx is "
-			 "default, probably not correct!\n",
-			 *(unsigned long long *) ifp->if_guid);
+		qib_devinfo(dd->pcidev,
+			"Warning, GUID %llx is default, probably not correct!\n",
+			*(unsigned long long *) ifp->if_guid);
 
 	bguid = ifp->if_guid;
 	if (!bguid[0] && !bguid[1] && !bguid[2]) {
@@ -260,8 +263,9 @@ void qib_get_eeprom_info(struct qib_devdata *dd)
 		memcpy(dd->serial, ifp->if_serial,
 		       sizeof ifp->if_serial);
 	if (!strstr(ifp->if_comment, "Tested successfully"))
-		qib_dev_err(dd, "Board SN %s did not pass functional "
-			    "test: %s\n", dd->serial, ifp->if_comment);
+		qib_dev_err(dd,
+			"Board SN %s did not pass functional test: %s\n",
+			dd->serial, ifp->if_comment);
 
 	memcpy(&dd->eep_st_errs, &ifp->if_errcntp, QIB_EEP_LOG_CNT);
 	/*
@@ -323,8 +327,9 @@ int qib_update_eeprom_log(struct qib_devdata *dd)
 	buf = vmalloc(len);
 	ret = 1;
 	if (!buf) {
-		qib_dev_err(dd, "Couldn't allocate memory to read %u "
-			    "bytes from eeprom for logging\n", len);
+		qib_dev_err(dd,
+			"Couldn't allocate memory to read %u bytes from eeprom for logging\n",
+			len);
 		goto bail;
 	}
 
diff --git a/drivers/infiniband/hw/qib/qib_file_ops.c b/drivers/infiniband/hw/qib/qib_file_ops.c
index a7403248d83..faa44cb0807 100644
--- a/drivers/infiniband/hw/qib/qib_file_ops.c
+++ b/drivers/infiniband/hw/qib/qib_file_ops.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -49,6 +49,9 @@
 #include "qib_common.h"
 #include "qib_user_sdma.h"
 
+#undef pr_fmt
+#define pr_fmt(fmt) QIB_DRV_NAME ": " fmt
+
 static int qib_open(struct inode *, struct file *);
 static int qib_close(struct inode *, struct file *);
 static ssize_t qib_write(struct file *, const char __user *, size_t, loff_t *);
@@ -315,8 +318,9 @@ static int qib_tid_update(struct qib_ctxtdata *rcd, struct file *fp,
 	}
 	if (cnt > tidcnt) {
 		/* make sure it all fits in tid_pg_list */
-		qib_devinfo(dd->pcidev, "Process tried to allocate %u "
-			 "TIDs, only trying max (%u)\n", cnt, tidcnt);
+		qib_devinfo(dd->pcidev,
+			"Process tried to allocate %u TIDs, only trying max (%u)\n",
+			cnt, tidcnt);
 		cnt = tidcnt;
 	}
 	pagep = (struct page **) rcd->tid_pg_list;
@@ -750,9 +754,9 @@ static int qib_mmap_mem(struct vm_area_struct *vma, struct qib_ctxtdata *rcd,
 	ret = remap_pfn_range(vma, vma->vm_start, pfn,
 			      len, vma->vm_page_prot);
 	if (ret)
-		qib_devinfo(dd->pcidev, "%s ctxt%u mmap of %lx, %x "
-			 "bytes failed: %d\n", what, rcd->ctxt,
-			 pfn, len, ret);
+		qib_devinfo(dd->pcidev,
+			"%s ctxt%u mmap of %lx, %x bytes failed: %d\n",
+			what, rcd->ctxt, pfn, len, ret);
 bail:
 	return ret;
 }
@@ -771,8 +775,9 @@ static int mmap_ureg(struct vm_area_struct *vma, struct qib_devdata *dd,
 	 */
 	sz = dd->flags & QIB_HAS_HDRSUPP ? 2 * PAGE_SIZE : PAGE_SIZE;
 	if ((vma->vm_end - vma->vm_start) > sz) {
-		qib_devinfo(dd->pcidev, "FAIL mmap userreg: reqlen "
-			 "%lx > PAGE\n", vma->vm_end - vma->vm_start);
+		qib_devinfo(dd->pcidev,
+			"FAIL mmap userreg: reqlen %lx > PAGE\n",
+			vma->vm_end - vma->vm_start);
 		ret = -EFAULT;
 	} else {
 		phys = dd->physaddr + ureg;
@@ -802,8 +807,8 @@ static int mmap_piobufs(struct vm_area_struct *vma,
 	 * for it.
 	 */
 	if ((vma->vm_end - vma->vm_start) > (piocnt * dd->palign)) {
-		qib_devinfo(dd->pcidev, "FAIL mmap piobufs: "
-			 "reqlen %lx > PAGE\n",
+		qib_devinfo(dd->pcidev,
+			"FAIL mmap piobufs: reqlen %lx > PAGE\n",
 			 vma->vm_end - vma->vm_start);
 		ret = -EINVAL;
 		goto bail;
@@ -847,8 +852,8 @@ static int mmap_rcvegrbufs(struct vm_area_struct *vma,
 	size = rcd->rcvegrbuf_size;
 	total_size = rcd->rcvegrbuf_chunks * size;
 	if ((vma->vm_end - vma->vm_start) > total_size) {
-		qib_devinfo(dd->pcidev, "FAIL on egr bufs: "
-			 "reqlen %lx > actual %lx\n",
+		qib_devinfo(dd->pcidev,
+			"FAIL on egr bufs: reqlen %lx > actual %lx\n",
 			 vma->vm_end - vma->vm_start,
 			 (unsigned long) total_size);
 		ret = -EINVAL;
@@ -856,8 +861,9 @@ static int mmap_rcvegrbufs(struct vm_area_struct *vma,
 	}
 
 	if (vma->vm_flags & VM_WRITE) {
-		qib_devinfo(dd->pcidev, "Can't map eager buffers as "
-			 "writable (flags=%lx)\n", vma->vm_flags);
+		qib_devinfo(dd->pcidev,
+			"Can't map eager buffers as writable (flags=%lx)\n",
+			vma->vm_flags);
 		ret = -EPERM;
 		goto bail;
 	}
@@ -1270,8 +1276,8 @@ static int setup_ctxt(struct qib_pportdata *ppd, int ctxt,
 			       GFP_KERNEL);
 
 	if (!rcd || !ptmp) {
-		qib_dev_err(dd, "Unable to allocate ctxtdata "
-			    "memory, failing open\n");
+		qib_dev_err(dd,
+			"Unable to allocate ctxtdata memory, failing open\n");
 		ret = -ENOMEM;
 		goto bailerr;
 	}
@@ -1560,10 +1566,10 @@ done_chk_sdma:
 		} else if (weight == 1 &&
 			test_bit(cpumask_first(tsk_cpus_allowed(current)),
 				 qib_cpulist))
-			qib_devinfo(dd->pcidev, "%s PID %u affinity "
-				    "set to cpu %d; already allocated\n",
-				    current->comm, current->pid,
-				    cpumask_first(tsk_cpus_allowed(current)));
+			qib_devinfo(dd->pcidev,
+				"%s PID %u affinity set to cpu %d; already allocated\n",
+				current->comm, current->pid,
+				cpumask_first(tsk_cpus_allowed(current)));
 	}
 
 	mutex_unlock(&qib_mutex);
@@ -2185,8 +2191,7 @@ int qib_cdev_init(int minor, const char *name,
 
 	cdev = cdev_alloc();
 	if (!cdev) {
-		printk(KERN_ERR QIB_DRV_NAME
-		       ": Could not allocate cdev for minor %d, %s\n",
+		pr_err("Could not allocate cdev for minor %d, %s\n",
 		       minor, name);
 		ret = -ENOMEM;
 		goto done;
@@ -2198,8 +2203,7 @@ int qib_cdev_init(int minor, const char *name,
 
 	ret = cdev_add(cdev, dev, 1);
 	if (ret < 0) {
-		printk(KERN_ERR QIB_DRV_NAME
-		       ": Could not add cdev for minor %d, %s (err %d)\n",
+		pr_err("Could not add cdev for minor %d, %s (err %d)\n",
 		       minor, name, -ret);
 		goto err_cdev;
 	}
@@ -2209,8 +2213,7 @@ int qib_cdev_init(int minor, const char *name,
 		goto done;
 	ret = PTR_ERR(device);
 	device = NULL;
-	printk(KERN_ERR QIB_DRV_NAME ": Could not create "
-	       "device for minor %d, %s (err %d)\n",
+	pr_err("Could not create device for minor %d, %s (err %d)\n",
 	       minor, name, -ret);
 err_cdev:
 	cdev_del(cdev);
@@ -2245,16 +2248,14 @@ int __init qib_dev_init(void)
 
 	ret = alloc_chrdev_region(&qib_dev, 0, QIB_NMINORS, QIB_DRV_NAME);
 	if (ret < 0) {
-		printk(KERN_ERR QIB_DRV_NAME ": Could not allocate "
-		       "chrdev region (err %d)\n", -ret);
+		pr_err("Could not allocate chrdev region (err %d)\n", -ret);
 		goto done;
 	}
 
 	qib_class = class_create(THIS_MODULE, "ipath");
 	if (IS_ERR(qib_class)) {
 		ret = PTR_ERR(qib_class);
-		printk(KERN_ERR QIB_DRV_NAME ": Could not create "
-		       "device class (err %d)\n", -ret);
+		pr_err("Could not create device class (err %d)\n", -ret);
 		unregister_chrdev_region(qib_dev, QIB_NMINORS);
 	}
 
diff --git a/drivers/infiniband/hw/qib/qib_fs.c b/drivers/infiniband/hw/qib/qib_fs.c
index 05e0f17c5b4..cff8a6c3216 100644
--- a/drivers/infiniband/hw/qib/qib_fs.c
+++ b/drivers/infiniband/hw/qib/qib_fs.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -382,7 +383,7 @@ static int add_cntr_files(struct super_block *sb, struct qib_devdata *dd)
 	ret = create_file(unit, S_IFDIR|S_IRUGO|S_IXUGO, sb->s_root, &dir,
 			  &simple_dir_operations, dd);
 	if (ret) {
-		printk(KERN_ERR "create_file(%s) failed: %d\n", unit, ret);
+		pr_err("create_file(%s) failed: %d\n", unit, ret);
 		goto bail;
 	}
 
@@ -390,21 +391,21 @@ static int add_cntr_files(struct super_block *sb, struct qib_devdata *dd)
 	ret = create_file("counters", S_IFREG|S_IRUGO, dir, &tmp,
 			  &cntr_ops[0], dd);
 	if (ret) {
-		printk(KERN_ERR "create_file(%s/counters) failed: %d\n",
+		pr_err("create_file(%s/counters) failed: %d\n",
 		       unit, ret);
 		goto bail;
 	}
 	ret = create_file("counter_names", S_IFREG|S_IRUGO, dir, &tmp,
 			  &cntr_ops[1], dd);
 	if (ret) {
-		printk(KERN_ERR "create_file(%s/counter_names) failed: %d\n",
+		pr_err("create_file(%s/counter_names) failed: %d\n",
 		       unit, ret);
 		goto bail;
 	}
 	ret = create_file("portcounter_names", S_IFREG|S_IRUGO, dir, &tmp,
 			  &portcntr_ops[0], dd);
 	if (ret) {
-		printk(KERN_ERR "create_file(%s/%s) failed: %d\n",
+		pr_err("create_file(%s/%s) failed: %d\n",
 		       unit, "portcounter_names", ret);
 		goto bail;
 	}
@@ -416,7 +417,7 @@ static int add_cntr_files(struct super_block *sb, struct qib_devdata *dd)
 		ret = create_file(fname, S_IFREG|S_IRUGO, dir, &tmp,
 				  &portcntr_ops[i], dd);
 		if (ret) {
-			printk(KERN_ERR "create_file(%s/%s) failed: %d\n",
+			pr_err("create_file(%s/%s) failed: %d\n",
 				unit, fname, ret);
 			goto bail;
 		}
@@ -426,7 +427,7 @@ static int add_cntr_files(struct super_block *sb, struct qib_devdata *dd)
 		ret = create_file(fname, S_IFREG|S_IRUGO, dir, &tmp,
 				  &qsfp_ops[i - 1], dd);
 		if (ret) {
-			printk(KERN_ERR "create_file(%s/%s) failed: %d\n",
+			pr_err("create_file(%s/%s) failed: %d\n",
 				unit, fname, ret);
 			goto bail;
 		}
@@ -435,7 +436,7 @@ static int add_cntr_files(struct super_block *sb, struct qib_devdata *dd)
 	ret = create_file("flash", S_IFREG|S_IWUSR|S_IRUGO, dir, &tmp,
 			  &flash_ops, dd);
 	if (ret)
-		printk(KERN_ERR "create_file(%s/flash) failed: %d\n",
+		pr_err("create_file(%s/flash) failed: %d\n",
 			unit, ret);
 bail:
 	return ret;
@@ -486,7 +487,7 @@ static int remove_device_files(struct super_block *sb,
 
 	if (IS_ERR(dir)) {
 		ret = PTR_ERR(dir);
-		printk(KERN_ERR "Lookup of %s failed\n", unit);
+		pr_err("Lookup of %s failed\n", unit);
 		goto bail;
 	}
 
@@ -532,7 +533,7 @@ static int qibfs_fill_super(struct super_block *sb, void *data, int silent)
 
 	ret = simple_fill_super(sb, QIBFS_MAGIC, files);
 	if (ret) {
-		printk(KERN_ERR "simple_fill_super failed: %d\n", ret);
+		pr_err("simple_fill_super failed: %d\n", ret);
 		goto bail;
 	}
 
diff --git a/drivers/infiniband/hw/qib/qib_iba6120.c b/drivers/infiniband/hw/qib/qib_iba6120.c
index 4d352b90750..a099ac171e2 100644
--- a/drivers/infiniband/hw/qib/qib_iba6120.c
+++ b/drivers/infiniband/hw/qib/qib_iba6120.c
@@ -753,8 +753,8 @@ static void qib_handle_6120_hwerrors(struct qib_devdata *dd, char *msg,
 	if (!hwerrs)
 		return;
 	if (hwerrs == ~0ULL) {
-		qib_dev_err(dd, "Read of hardware error status failed "
-			    "(all bits set); ignoring\n");
+		qib_dev_err(dd,
+			"Read of hardware error status failed (all bits set); ignoring\n");
 		return;
 	}
 	qib_stats.sps_hwerrs++;
@@ -779,13 +779,14 @@ static void qib_handle_6120_hwerrors(struct qib_devdata *dd, char *msg,
 	 * or it's occurred within the last 5 seconds.
 	 */
 	if (hwerrs & ~(TXE_PIO_PARITY | RXEMEMPARITYERR_EAGERTID))
-		qib_devinfo(dd->pcidev, "Hardware error: hwerr=0x%llx "
-			 "(cleared)\n", (unsigned long long) hwerrs);
+		qib_devinfo(dd->pcidev,
+			"Hardware error: hwerr=0x%llx (cleared)\n",
+			(unsigned long long) hwerrs);
 
 	if (hwerrs & ~IB_HWE_BITSEXTANT)
-		qib_dev_err(dd, "hwerror interrupt with unknown errors "
-			    "%llx set\n", (unsigned long long)
-			    (hwerrs & ~IB_HWE_BITSEXTANT));
+		qib_dev_err(dd,
+			"hwerror interrupt with unknown errors %llx set\n",
+			(unsigned long long)(hwerrs & ~IB_HWE_BITSEXTANT));
 
 	ctrl = qib_read_kreg32(dd, kr_control);
 	if ((ctrl & QLOGIC_IB_C_FREEZEMODE) && !dd->diag_client) {
@@ -815,8 +816,9 @@ static void qib_handle_6120_hwerrors(struct qib_devdata *dd, char *msg,
 
 	if (hwerrs & HWE_MASK(PowerOnBISTFailed)) {
 		isfatal = 1;
-		strlcat(msg, "[Memory BIST test failed, InfiniPath hardware"
-			" unusable]", msgl);
+		strlcat(msg,
+			"[Memory BIST test failed, InfiniPath hardware unusable]",
+			msgl);
 		/* ignore from now on, so disable until driver reloaded */
 		dd->cspec->hwerrmask &= ~HWE_MASK(PowerOnBISTFailed);
 		qib_write_kreg(dd, kr_hwerrmask, dd->cspec->hwerrmask);
@@ -868,8 +870,9 @@ static void qib_handle_6120_hwerrors(struct qib_devdata *dd, char *msg,
 		*msg = 0; /* recovered from all of them */
 
 	if (isfatal && !dd->diag_client) {
-		qib_dev_err(dd, "Fatal Hardware Error, no longer"
-			    " usable, SN %.16s\n", dd->serial);
+		qib_dev_err(dd,
+			"Fatal Hardware Error, no longer usable, SN %.16s\n",
+			dd->serial);
 		/*
 		 * for /sys status file and user programs to print; if no
 		 * trailing brace is copied, we'll know it was truncated.
@@ -1017,9 +1020,9 @@ static void handle_6120_errors(struct qib_devdata *dd, u64 errs)
 				qib_inc_eeprom_err(dd, log_idx, 1);
 
 	if (errs & ~IB_E_BITSEXTANT)
-		qib_dev_err(dd, "error interrupt with unknown errors "
-			    "%llx set\n",
-			    (unsigned long long) (errs & ~IB_E_BITSEXTANT));
+		qib_dev_err(dd,
+			"error interrupt with unknown errors %llx set\n",
+			(unsigned long long) (errs & ~IB_E_BITSEXTANT));
 
 	if (errs & E_SUM_ERRS) {
 		qib_disarm_6120_senderrbufs(ppd);
@@ -1089,8 +1092,8 @@ static void handle_6120_errors(struct qib_devdata *dd, u64 errs)
 	}
 
 	if (errs & ERR_MASK(ResetNegated)) {
-		qib_dev_err(dd, "Got reset, requires re-init "
-			      "(unload and reload driver)\n");
+		qib_dev_err(dd,
+			"Got reset, requires re-init (unload and reload driver)\n");
 		dd->flags &= ~QIB_INITTED;  /* needs re-init */
 		/* mark as having had error */
 		*dd->devstatusp |= QIB_STATUS_HWERROR;
@@ -1541,8 +1544,9 @@ static noinline void unlikely_6120_intr(struct qib_devdata *dd, u64 istat)
 		qib_stats.sps_errints++;
 		estat = qib_read_kreg64(dd, kr_errstatus);
 		if (!estat)
-			qib_devinfo(dd->pcidev, "error interrupt (%Lx), "
-				 "but no error bits set!\n", istat);
+			qib_devinfo(dd->pcidev,
+				"error interrupt (%Lx), but no error bits set!\n",
+				istat);
 		handle_6120_errors(dd, estat);
 	}
 
@@ -1715,16 +1719,16 @@ static void qib_setup_6120_interrupt(struct qib_devdata *dd)
 	}
 
 	if (!dd->cspec->irq)
-		qib_dev_err(dd, "irq is 0, BIOS error?  Interrupts won't "
-			    "work\n");
+		qib_dev_err(dd,
+			"irq is 0, BIOS error?  Interrupts won't work\n");
 	else {
 		int ret;
 		ret = request_irq(dd->cspec->irq, qib_6120intr, 0,
 				  QIB_DRV_NAME, dd);
 		if (ret)
-			qib_dev_err(dd, "Couldn't setup interrupt "
-				    "(irq=%d): %d\n", dd->cspec->irq,
-				    ret);
+			qib_dev_err(dd,
+				"Couldn't setup interrupt (irq=%d): %d\n",
+				dd->cspec->irq, ret);
 	}
 }
 
@@ -1759,8 +1763,9 @@ static void pe_boardname(struct qib_devdata *dd)
 		snprintf(dd->boardname, namelen, "%s", n);
 
 	if (dd->majrev != 4 || !dd->minrev || dd->minrev > 2)
-		qib_dev_err(dd, "Unsupported InfiniPath hardware revision "
-			    "%u.%u!\n", dd->majrev, dd->minrev);
+		qib_dev_err(dd,
+			"Unsupported InfiniPath hardware revision %u.%u!\n",
+			dd->majrev, dd->minrev);
 
 	snprintf(dd->boardversion, sizeof(dd->boardversion),
 		 "ChipABI %u.%u, %s, InfiniPath%u %u.%u, SW Compat %u\n",
@@ -1833,8 +1838,8 @@ static int qib_6120_setup_reset(struct qib_devdata *dd)
 bail:
 	if (ret) {
 		if (qib_pcie_params(dd, dd->lbus_width, NULL, NULL))
-			qib_dev_err(dd, "Reset failed to setup PCIe or "
-				    "interrupts; continuing anyway\n");
+			qib_dev_err(dd,
+				"Reset failed to setup PCIe or interrupts; continuing anyway\n");
 		/* clear the reset error, init error/hwerror mask */
 		qib_6120_init_hwerrors(dd);
 		/* for Rev2 error interrupts; nop for rev 1 */
@@ -1876,8 +1881,9 @@ static void qib_6120_put_tid(struct qib_devdata *dd, u64 __iomem *tidptr,
 		}
 		pa >>= 11;
 		if (pa & ~QLOGIC_IB_RT_ADDR_MASK) {
-			qib_dev_err(dd, "Physical page address 0x%lx "
-				    "larger than supported\n", pa);
+			qib_dev_err(dd,
+				"Physical page address 0x%lx larger than supported\n",
+				pa);
 			return;
 		}
 
@@ -1941,8 +1947,9 @@ static void qib_6120_put_tid_2(struct qib_devdata *dd, u64 __iomem *tidptr,
 		}
 		pa >>= 11;
 		if (pa & ~QLOGIC_IB_RT_ADDR_MASK) {
-			qib_dev_err(dd, "Physical page address 0x%lx "
-				    "larger than supported\n", pa);
+			qib_dev_err(dd,
+				"Physical page address 0x%lx larger than supported\n",
+				pa);
 			return;
 		}
 
@@ -2928,8 +2935,9 @@ static int qib_6120_set_loopback(struct qib_pportdata *ppd, const char *what)
 			 ppd->dd->unit, ppd->port);
 	} else if (!strncmp(what, "off", 3)) {
 		ppd->dd->cspec->ibcctrl &= ~SYM_MASK(IBCCtrl, Loopback);
-		qib_devinfo(ppd->dd->pcidev, "Disabling IB%u:%u IBC loopback "
-			    "(normal)\n", ppd->dd->unit, ppd->port);
+		qib_devinfo(ppd->dd->pcidev,
+			"Disabling IB%u:%u IBC loopback (normal)\n",
+			ppd->dd->unit, ppd->port);
 	} else
 		ret = -EINVAL;
 	if (!ret) {
@@ -3186,11 +3194,10 @@ static int qib_late_6120_initreg(struct qib_devdata *dd)
 	qib_write_kreg(dd, kr_sendpioavailaddr, dd->pioavailregs_phys);
 	val = qib_read_kreg64(dd, kr_sendpioavailaddr);
 	if (val != dd->pioavailregs_phys) {
-		qib_dev_err(dd, "Catastrophic software error, "
-			    "SendPIOAvailAddr written as %lx, "
-			    "read back as %llx\n",
-			    (unsigned long) dd->pioavailregs_phys,
-			    (unsigned long long) val);
+		qib_dev_err(dd,
+			"Catastrophic software error, SendPIOAvailAddr written as %lx, read back as %llx\n",
+			(unsigned long) dd->pioavailregs_phys,
+			(unsigned long long) val);
 		ret = -EINVAL;
 	}
 	return ret;
@@ -3218,8 +3225,8 @@ static int init_6120_variables(struct qib_devdata *dd)
 	dd->revision = readq(&dd->kregbase[kr_revision]);
 
 	if ((dd->revision & 0xffffffffU) == 0xffffffffU) {
-		qib_dev_err(dd, "Revision register read failure, "
-			    "giving up initialization\n");
+		qib_dev_err(dd,
+			"Revision register read failure, giving up initialization\n");
 		ret = -ENODEV;
 		goto bail;
 	}
@@ -3551,8 +3558,8 @@ struct qib_devdata *qib_init_iba6120_funcs(struct pci_dev *pdev,
 		goto bail;
 
 	if (qib_pcie_params(dd, 8, NULL, NULL))
-		qib_dev_err(dd, "Failed to setup PCIe or interrupts; "
-			    "continuing anyway\n");
+		qib_dev_err(dd,
+			"Failed to setup PCIe or interrupts; continuing anyway\n");
 	dd->cspec->irq = pdev->irq; /* save IRQ */
 
 	/* clear diagctrl register, in case diags were running and crashed */
diff --git a/drivers/infiniband/hw/qib/qib_iba7220.c b/drivers/infiniband/hw/qib/qib_iba7220.c
index 86a0ba7ca0c..64d0ecb90cd 100644
--- a/drivers/infiniband/hw/qib/qib_iba7220.c
+++ b/drivers/infiniband/hw/qib/qib_iba7220.c
@@ -1111,9 +1111,9 @@ static void handle_7220_errors(struct qib_devdata *dd, u64 errs)
 		sdma_7220_errors(ppd, errs);
 
 	if (errs & ~IB_E_BITSEXTANT)
-		qib_dev_err(dd, "error interrupt with unknown errors "
-			    "%llx set\n", (unsigned long long)
-			    (errs & ~IB_E_BITSEXTANT));
+		qib_dev_err(dd,
+			"error interrupt with unknown errors %llx set\n",
+			(unsigned long long) (errs & ~IB_E_BITSEXTANT));
 
 	if (errs & E_SUM_ERRS) {
 		qib_disarm_7220_senderrbufs(ppd);
@@ -1192,8 +1192,8 @@ static void handle_7220_errors(struct qib_devdata *dd, u64 errs)
 	}
 
 	if (errs & ERR_MASK(ResetNegated)) {
-		qib_dev_err(dd, "Got reset, requires re-init "
-			    "(unload and reload driver)\n");
+		qib_dev_err(dd,
+			"Got reset, requires re-init (unload and reload driver)\n");
 		dd->flags &= ~QIB_INITTED;  /* needs re-init */
 		/* mark as having had error */
 		*dd->devstatusp |= QIB_STATUS_HWERROR;
@@ -1305,8 +1305,8 @@ static void qib_7220_handle_hwerrors(struct qib_devdata *dd, char *msg,
 	if (!hwerrs)
 		goto bail;
 	if (hwerrs == ~0ULL) {
-		qib_dev_err(dd, "Read of hardware error status failed "
-			    "(all bits set); ignoring\n");
+		qib_dev_err(dd,
+			"Read of hardware error status failed (all bits set); ignoring\n");
 		goto bail;
 	}
 	qib_stats.sps_hwerrs++;
@@ -1329,13 +1329,14 @@ static void qib_7220_handle_hwerrors(struct qib_devdata *dd, char *msg,
 			qib_inc_eeprom_err(dd, log_idx, 1);
 	if (hwerrs & ~(TXEMEMPARITYERR_PIOBUF | TXEMEMPARITYERR_PIOPBC |
 		       RXE_PARITY))
-		qib_devinfo(dd->pcidev, "Hardware error: hwerr=0x%llx "
-			 "(cleared)\n", (unsigned long long) hwerrs);
+		qib_devinfo(dd->pcidev,
+			"Hardware error: hwerr=0x%llx (cleared)\n",
+			(unsigned long long) hwerrs);
 
 	if (hwerrs & ~IB_HWE_BITSEXTANT)
-		qib_dev_err(dd, "hwerror interrupt with unknown errors "
-			    "%llx set\n", (unsigned long long)
-			    (hwerrs & ~IB_HWE_BITSEXTANT));
+		qib_dev_err(dd,
+			"hwerror interrupt with unknown errors %llx set\n",
+			(unsigned long long) (hwerrs & ~IB_HWE_BITSEXTANT));
 
 	if (hwerrs & QLOGIC_IB_HWE_IB_UC_MEMORYPARITYERR)
 		qib_sd7220_clr_ibpar(dd);
@@ -1362,8 +1363,9 @@ static void qib_7220_handle_hwerrors(struct qib_devdata *dd, char *msg,
 
 	if (hwerrs & HWE_MASK(PowerOnBISTFailed)) {
 		isfatal = 1;
-		strlcat(msg, "[Memory BIST test failed, "
-			"InfiniPath hardware unusable]", msgl);
+		strlcat(msg,
+			"[Memory BIST test failed, InfiniPath hardware unusable]",
+			msgl);
 		/* ignore from now on, so disable until driver reloaded */
 		dd->cspec->hwerrmask &= ~HWE_MASK(PowerOnBISTFailed);
 		qib_write_kreg(dd, kr_hwerrmask, dd->cspec->hwerrmask);
@@ -1409,8 +1411,9 @@ static void qib_7220_handle_hwerrors(struct qib_devdata *dd, char *msg,
 	qib_dev_err(dd, "%s hardware error\n", msg);
 
 	if (isfatal && !dd->diag_client) {
-		qib_dev_err(dd, "Fatal Hardware Error, no longer"
-			    " usable, SN %.16s\n", dd->serial);
+		qib_dev_err(dd,
+			"Fatal Hardware Error, no longer usable, SN %.16s\n",
+			dd->serial);
 		/*
 		 * For /sys status file and user programs to print; if no
 		 * trailing brace is copied, we'll know it was truncated.
@@ -1918,8 +1921,9 @@ static noinline void unlikely_7220_intr(struct qib_devdata *dd, u64 istat)
 		qib_stats.sps_errints++;
 		estat = qib_read_kreg64(dd, kr_errstatus);
 		if (!estat)
-			qib_devinfo(dd->pcidev, "error interrupt (%Lx), "
-				 "but no error bits set!\n", istat);
+			qib_devinfo(dd->pcidev,
+				"error interrupt (%Lx), but no error bits set!\n",
+				istat);
 		else
 			handle_7220_errors(dd, estat);
 	}
@@ -2023,17 +2027,18 @@ bail:
 static void qib_setup_7220_interrupt(struct qib_devdata *dd)
 {
 	if (!dd->cspec->irq)
-		qib_dev_err(dd, "irq is 0, BIOS error?  Interrupts won't "
-			    "work\n");
+		qib_dev_err(dd,
+			"irq is 0, BIOS error?  Interrupts won't work\n");
 	else {
 		int ret = request_irq(dd->cspec->irq, qib_7220intr,
 			dd->msi_lo ? 0 : IRQF_SHARED,
 			QIB_DRV_NAME, dd);
 
 		if (ret)
-			qib_dev_err(dd, "Couldn't setup %s interrupt "
-				    "(irq=%d): %d\n", dd->msi_lo ?
-				    "MSI" : "INTx", dd->cspec->irq, ret);
+			qib_dev_err(dd,
+				"Couldn't setup %s interrupt (irq=%d): %d\n",
+				dd->msi_lo ?  "MSI" : "INTx",
+				dd->cspec->irq, ret);
 	}
 }
 
@@ -2072,9 +2077,9 @@ static void qib_7220_boardname(struct qib_devdata *dd)
 		snprintf(dd->boardname, namelen, "%s", n);
 
 	if (dd->majrev != 5 || !dd->minrev || dd->minrev > 2)
-		qib_dev_err(dd, "Unsupported InfiniPath hardware "
-			    "revision %u.%u!\n",
-			    dd->majrev, dd->minrev);
+		qib_dev_err(dd,
+			"Unsupported InfiniPath hardware revision %u.%u!\n",
+			dd->majrev, dd->minrev);
 
 	snprintf(dd->boardversion, sizeof(dd->boardversion),
 		 "ChipABI %u.%u, %s, InfiniPath%u %u.%u, SW Compat %u\n",
@@ -2146,8 +2151,8 @@ static int qib_setup_7220_reset(struct qib_devdata *dd)
 bail:
 	if (ret) {
 		if (qib_pcie_params(dd, dd->lbus_width, NULL, NULL))
-			qib_dev_err(dd, "Reset failed to setup PCIe or "
-				    "interrupts; continuing anyway\n");
+			qib_dev_err(dd,
+				"Reset failed to setup PCIe or interrupts; continuing anyway\n");
 
 		/* hold IBC in reset, no sends, etc till later */
 		qib_write_kreg(dd, kr_control, 0ULL);
@@ -2187,8 +2192,9 @@ static void qib_7220_put_tid(struct qib_devdata *dd, u64 __iomem *tidptr,
 			return;
 		}
 		if (chippa >= (1UL << IBA7220_TID_SZ_SHIFT)) {
-			qib_dev_err(dd, "Physical page address 0x%lx "
-				"larger than supported\n", pa);
+			qib_dev_err(dd,
+				"Physical page address 0x%lx larger than supported\n",
+				pa);
 			return;
 		}
 
@@ -2706,8 +2712,9 @@ static int qib_7220_set_loopback(struct qib_pportdata *ppd, const char *what)
 		ppd->cpspec->ibcctrl &= ~SYM_MASK(IBCCtrl, Loopback);
 		/* enable heart beat again */
 		val = IBA7220_IBC_HRTBT_MASK << IBA7220_IBC_HRTBT_SHIFT;
-		qib_devinfo(ppd->dd->pcidev, "Disabling IB%u:%u IBC loopback "
-			    "(normal)\n", ppd->dd->unit, ppd->port);
+		qib_devinfo(ppd->dd->pcidev,
+			"Disabling IB%u:%u IBC loopback (normal)\n",
+			ppd->dd->unit, ppd->port);
 	} else
 		ret = -EINVAL;
 	if (!ret) {
@@ -3307,8 +3314,8 @@ static int qib_7220_intr_fallback(struct qib_devdata *dd)
 	if (!dd->msi_lo)
 		return 0;
 
-	qib_devinfo(dd->pcidev, "MSI interrupt not detected,"
-		 " trying INTx interrupts\n");
+	qib_devinfo(dd->pcidev,
+		"MSI interrupt not detected, trying INTx interrupts\n");
 	qib_7220_free_irq(dd);
 	qib_enable_intx(dd->pcidev);
 	/*
@@ -3980,11 +3987,10 @@ static int qib_late_7220_initreg(struct qib_devdata *dd)
 	qib_write_kreg(dd, kr_sendpioavailaddr, dd->pioavailregs_phys);
 	val = qib_read_kreg64(dd, kr_sendpioavailaddr);
 	if (val != dd->pioavailregs_phys) {
-		qib_dev_err(dd, "Catastrophic software error, "
-			    "SendPIOAvailAddr written as %lx, "
-			    "read back as %llx\n",
-			    (unsigned long) dd->pioavailregs_phys,
-			    (unsigned long long) val);
+		qib_dev_err(dd,
+			"Catastrophic software error, SendPIOAvailAddr written as %lx, read back as %llx\n",
+			(unsigned long) dd->pioavailregs_phys,
+			(unsigned long long) val);
 		ret = -EINVAL;
 	}
 	qib_register_observer(dd, &sendctrl_observer);
@@ -4014,8 +4020,8 @@ static int qib_init_7220_variables(struct qib_devdata *dd)
 	dd->revision = readq(&dd->kregbase[kr_revision]);
 
 	if ((dd->revision & 0xffffffffU) == 0xffffffffU) {
-		qib_dev_err(dd, "Revision register read failure, "
-			    "giving up initialization\n");
+		qib_dev_err(dd,
+			"Revision register read failure, giving up initialization\n");
 		ret = -ENODEV;
 		goto bail;
 	}
@@ -4613,8 +4619,8 @@ struct qib_devdata *qib_init_iba7220_funcs(struct pci_dev *pdev,
 		break;
 	}
 	if (qib_pcie_params(dd, minwidth, NULL, NULL))
-		qib_dev_err(dd, "Failed to setup PCIe or interrupts; "
-			    "continuing anyway\n");
+		qib_dev_err(dd,
+			"Failed to setup PCIe or interrupts; continuing anyway\n");
 
 	/* save IRQ for possible later use */
 	dd->cspec->irq = pdev->irq;
diff --git a/drivers/infiniband/hw/qib/qib_iba7322.c b/drivers/infiniband/hw/qib/qib_iba7322.c
index c881e744c09..0d7280af99b 100644
--- a/drivers/infiniband/hw/qib/qib_iba7322.c
+++ b/drivers/infiniband/hw/qib/qib_iba7322.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2008, 2009, 2010 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2008 - 2012 QLogic Corporation. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -49,6 +50,10 @@
 #include "qib_qsfp.h"
 
 #include "qib_mad.h"
+#include "qib_verbs.h"
+
+#undef pr_fmt
+#define pr_fmt(fmt) QIB_DRV_NAME " " fmt
 
 static void qib_setup_7322_setextled(struct qib_pportdata *, u32);
 static void qib_7322_handle_hwerrors(struct qib_devdata *, char *, size_t);
@@ -1575,8 +1580,8 @@ static noinline void handle_7322_errors(struct qib_devdata *dd)
 	qib_stats.sps_errints++;
 	errs = qib_read_kreg64(dd, kr_errstatus);
 	if (!errs) {
-		qib_devinfo(dd->pcidev, "device error interrupt, "
-			 "but no error bits set!\n");
+		qib_devinfo(dd->pcidev,
+			"device error interrupt, but no error bits set!\n");
 		goto done;
 	}
 
@@ -1622,8 +1627,8 @@ static noinline void handle_7322_errors(struct qib_devdata *dd)
 	if (errs & QIB_E_RESET) {
 		int pidx;
 
-		qib_dev_err(dd, "Got reset, requires re-init "
-			    "(unload and reload driver)\n");
+		qib_dev_err(dd,
+			"Got reset, requires re-init (unload and reload driver)\n");
 		dd->flags &= ~QIB_INITTED;  /* needs re-init */
 		/* mark as having had error */
 		*dd->devstatusp |= QIB_STATUS_HWERROR;
@@ -1760,9 +1765,9 @@ static void handle_serdes_issues(struct qib_pportdata *ppd, u64 ibcst)
 					    ppd->dd->cspec->r1 ?
 					    QDR_STATIC_ADAPT_DOWN_R1 :
 					    QDR_STATIC_ADAPT_DOWN);
-			printk(KERN_INFO QIB_DRV_NAME
-				" IB%u:%u re-enabled QDR adaptation "
-				"ibclt %x\n", ppd->dd->unit, ppd->port, ibclt);
+			pr_info(
+				"IB%u:%u re-enabled QDR adaptation ibclt %x\n",
+				ppd->dd->unit, ppd->port, ibclt);
 		}
 	}
 }
@@ -1804,9 +1809,9 @@ static noinline void handle_7322_p_errors(struct qib_pportdata *ppd)
 		if (!*msg)
 			snprintf(msg, sizeof ppd->cpspec->epmsgbuf,
 				 "no others");
-		qib_dev_porterr(dd, ppd->port, "error interrupt with unknown"
-				" errors 0x%016Lx set (and %s)\n",
-				(errs & ~QIB_E_P_BITSEXTANT), msg);
+		qib_dev_porterr(dd, ppd->port,
+			"error interrupt with unknown errors 0x%016Lx set (and %s)\n",
+			(errs & ~QIB_E_P_BITSEXTANT), msg);
 		*msg = '\0';
 	}
 
@@ -2024,8 +2029,8 @@ static void qib_7322_handle_hwerrors(struct qib_devdata *dd, char *msg,
 	if (!hwerrs)
 		goto bail;
 	if (hwerrs == ~0ULL) {
-		qib_dev_err(dd, "Read of hardware error status failed "
-			    "(all bits set); ignoring\n");
+		qib_dev_err(dd,
+			"Read of hardware error status failed (all bits set); ignoring\n");
 		goto bail;
 	}
 	qib_stats.sps_hwerrs++;
@@ -2039,8 +2044,9 @@ static void qib_7322_handle_hwerrors(struct qib_devdata *dd, char *msg,
 	/* no EEPROM logging, yet */
 
 	if (hwerrs)
-		qib_devinfo(dd->pcidev, "Hardware error: hwerr=0x%llx "
-			    "(cleared)\n", (unsigned long long) hwerrs);
+		qib_devinfo(dd->pcidev,
+			"Hardware error: hwerr=0x%llx (cleared)\n",
+			(unsigned long long) hwerrs);
 
 	ctrl = qib_read_kreg32(dd, kr_control);
 	if ((ctrl & SYM_MASK(Control, FreezeMode)) && !dd->diag_client) {
@@ -2064,8 +2070,9 @@ static void qib_7322_handle_hwerrors(struct qib_devdata *dd, char *msg,
 
 	if (hwerrs & HWE_MASK(PowerOnBISTFailed)) {
 		isfatal = 1;
-		strlcpy(msg, "[Memory BIST test failed, "
-			"InfiniPath hardware unusable]", msgl);
+		strlcpy(msg,
+			"[Memory BIST test failed, InfiniPath hardware unusable]",
+			msgl);
 		/* ignore from now on, so disable until driver reloaded */
 		dd->cspec->hwerrmask &= ~HWE_MASK(PowerOnBISTFailed);
 		qib_write_kreg(dd, kr_hwerrmask, dd->cspec->hwerrmask);
@@ -2078,8 +2085,9 @@ static void qib_7322_handle_hwerrors(struct qib_devdata *dd, char *msg,
 	qib_dev_err(dd, "%s hardware error\n", msg);
 
 	if (isfatal && !dd->diag_client) {
-		qib_dev_err(dd, "Fatal Hardware Error, no longer"
-			    " usable, SN %.16s\n", dd->serial);
+		qib_dev_err(dd,
+			"Fatal Hardware Error, no longer usable, SN %.16s\n",
+			dd->serial);
 		/*
 		 * for /sys status file and user programs to print; if no
 		 * trailing brace is copied, we'll know it was truncated.
@@ -2667,8 +2675,9 @@ static noinline void unknown_7322_ibits(struct qib_devdata *dd, u64 istat)
 	char msg[128];
 
 	kills = istat & ~QIB_I_BITSEXTANT;
-	qib_dev_err(dd, "Clearing reserved interrupt(s) 0x%016llx:"
-		    " %s\n", (unsigned long long) kills, msg);
+	qib_dev_err(dd,
+		"Clearing reserved interrupt(s) 0x%016llx: %s\n",
+		(unsigned long long) kills, msg);
 	qib_write_kreg(dd, kr_intmask, (dd->cspec->int_enable_mask & ~kills));
 }
 
@@ -3101,16 +3110,16 @@ static void qib_setup_7322_interrupt(struct qib_devdata *dd, int clearpend)
 		/* Try to get INTx interrupt */
 try_intx:
 		if (!dd->pcidev->irq) {
-			qib_dev_err(dd, "irq is 0, BIOS error?  "
-				    "Interrupts won't work\n");
+			qib_dev_err(dd,
+				"irq is 0, BIOS error?  Interrupts won't work\n");
 			goto bail;
 		}
 		ret = request_irq(dd->pcidev->irq, qib_7322intr,
 				  IRQF_SHARED, QIB_DRV_NAME, dd);
 		if (ret) {
-			qib_dev_err(dd, "Couldn't setup INTx "
-				    "interrupt (irq=%d): %d\n",
-				    dd->pcidev->irq, ret);
+			qib_dev_err(dd,
+				"Couldn't setup INTx interrupt (irq=%d): %d\n",
+				dd->pcidev->irq, ret);
 			goto bail;
 		}
 		dd->cspec->irq = dd->pcidev->irq;
@@ -3185,8 +3194,9 @@ try_intx:
 			 * Shouldn't happen since the enable said we could
 			 * have as many as we are trying to setup here.
 			 */
-			qib_dev_err(dd, "Couldn't setup MSIx "
-				"interrupt (vec=%d, irq=%d): %d\n", msixnum,
+			qib_dev_err(dd,
+				"Couldn't setup MSIx interrupt (vec=%d, irq=%d): %d\n",
+				msixnum,
 				dd->cspec->msix_entries[msixnum].msix.vector,
 				ret);
 			qib_7322_nomsix(dd);
@@ -3305,8 +3315,9 @@ static unsigned qib_7322_boardname(struct qib_devdata *dd)
 		 (unsigned)SYM_FIELD(dd->revision, Revision_R, SW));
 
 	if (qib_singleport && (features >> PORT_SPD_CAP_SHIFT) & PORT_SPD_CAP) {
-		qib_devinfo(dd->pcidev, "IB%u: Forced to single port mode"
-			    " by module parameter\n", dd->unit);
+		qib_devinfo(dd->pcidev,
+			"IB%u: Forced to single port mode by module parameter\n",
+			dd->unit);
 		features &= PORT_SPD_CAP;
 	}
 
@@ -3400,8 +3411,8 @@ static int qib_do_7322_reset(struct qib_devdata *dd)
 		if (val == dd->revision)
 			break;
 		if (i == 5) {
-			qib_dev_err(dd, "Failed to initialize after reset, "
-				    "unusable\n");
+			qib_dev_err(dd,
+				"Failed to initialize after reset, unusable\n");
 			ret = 0;
 			goto  bail;
 		}
@@ -3432,8 +3443,8 @@ static int qib_do_7322_reset(struct qib_devdata *dd)
 	if (qib_pcie_params(dd, dd->lbus_width,
 			    &dd->cspec->num_msix_entries,
 			    dd->cspec->msix_entries))
-		qib_dev_err(dd, "Reset failed to setup PCIe or interrupts; "
-				"continuing anyway\n");
+		qib_dev_err(dd,
+			"Reset failed to setup PCIe or interrupts; continuing anyway\n");
 
 	qib_setup_7322_interrupt(dd, 1);
 
@@ -3474,8 +3485,9 @@ static void qib_7322_put_tid(struct qib_devdata *dd, u64 __iomem *tidptr,
 			return;
 		}
 		if (chippa >= (1UL << IBA7322_TID_SZ_SHIFT)) {
-			qib_dev_err(dd, "Physical page address 0x%lx "
-				"larger than supported\n", pa);
+			qib_dev_err(dd,
+				"Physical page address 0x%lx larger than supported\n",
+				pa);
 			return;
 		}
 
@@ -4029,8 +4041,9 @@ static int qib_7322_set_loopback(struct qib_pportdata *ppd, const char *what)
 							Loopback);
 		/* enable heart beat again */
 		val = IBA7322_IBC_HRTBT_RMASK << IBA7322_IBC_HRTBT_LSB;
-		qib_devinfo(ppd->dd->pcidev, "Disabling IB%u:%u IBC loopback "
-			    "(normal)\n", ppd->dd->unit, ppd->port);
+		qib_devinfo(ppd->dd->pcidev,
+			"Disabling IB%u:%u IBC loopback (normal)\n",
+			ppd->dd->unit, ppd->port);
 	} else
 		ret = -EINVAL;
 	if (!ret) {
@@ -4714,8 +4727,8 @@ static void init_7322_cntrnames(struct qib_devdata *dd)
 		dd->pport[i].cpspec->portcntrs = kmalloc(dd->cspec->nportcntrs
 			* sizeof(u64), GFP_KERNEL);
 		if (!dd->pport[i].cpspec->portcntrs)
-			qib_dev_err(dd, "Failed allocation for"
-				    " portcounters\n");
+			qib_dev_err(dd,
+				"Failed allocation for portcounters\n");
 	}
 }
 
@@ -4865,8 +4878,8 @@ static int qib_7322_intr_fallback(struct qib_devdata *dd)
 	if (!dd->cspec->num_msix_entries)
 		return 0; /* already using INTx */
 
-	qib_devinfo(dd->pcidev, "MSIx interrupt not detected,"
-		 " trying INTx interrupts\n");
+	qib_devinfo(dd->pcidev,
+		"MSIx interrupt not detected, trying INTx interrupts\n");
 	qib_7322_nomsix(dd);
 	qib_enable_intx(dd->pcidev);
 	qib_setup_7322_interrupt(dd, 0);
@@ -5151,15 +5164,11 @@ static void try_7322_ipg(struct qib_pportdata *ppd)
 		goto retry;
 
 	if (!ibp->smi_ah) {
-		struct ib_ah_attr attr;
 		struct ib_ah *ah;
 
-		memset(&attr, 0, sizeof attr);
-		attr.dlid = be16_to_cpu(IB_LID_PERMISSIVE);
-		attr.port_num = ppd->port;
-		ah = ib_create_ah(ibp->qp0->ibqp.pd, &attr);
+		ah = qib_create_qp0_ah(ibp, be16_to_cpu(IB_LID_PERMISSIVE));
 		if (IS_ERR(ah))
-			ret = -EINVAL;
+			ret = PTR_ERR(ah);
 		else {
 			send_buf->ah = ah;
 			ibp->smi_ah = to_iah(ah);
@@ -5844,22 +5853,21 @@ static int setup_txselect(const char *str, struct kernel_param *kp)
 {
 	struct qib_devdata *dd;
 	unsigned long val;
-	char *n;
+	int ret;
+
 	if (strlen(str) >= MAX_ATTEN_LEN) {
-		printk(KERN_INFO QIB_DRV_NAME " txselect_values string "
-		       "too long\n");
+		pr_info("txselect_values string too long\n");
 		return -ENOSPC;
 	}
-	val = simple_strtoul(str, &n, 0);
-	if (n == str || val >= (TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ +
+	ret = kstrtoul(str, 0, &val);
+	if (ret || val >= (TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ +
 				TXDDS_MFG_SZ)) {
-		printk(KERN_INFO QIB_DRV_NAME
-		       "txselect_values must start with a number < %d\n",
+		pr_info("txselect_values must start with a number < %d\n",
 			TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ + TXDDS_MFG_SZ);
-		return -EINVAL;
+		return ret ? ret : -EINVAL;
 	}
-	strcpy(txselect_list, str);
 
+	strcpy(txselect_list, str);
 	list_for_each_entry(dd, &qib_dev_list, list)
 		if (dd->deviceid == PCI_DEVICE_ID_QLOGIC_IB_7322)
 			set_no_qsfp_atten(dd, 1);
@@ -5882,11 +5890,10 @@ static int qib_late_7322_initreg(struct qib_devdata *dd)
 	qib_write_kreg(dd, kr_sendpioavailaddr, dd->pioavailregs_phys);
 	val = qib_read_kreg64(dd, kr_sendpioavailaddr);
 	if (val != dd->pioavailregs_phys) {
-		qib_dev_err(dd, "Catastrophic software error, "
-			    "SendPIOAvailAddr written as %lx, "
-			    "read back as %llx\n",
-			    (unsigned long) dd->pioavailregs_phys,
-			    (unsigned long long) val);
+		qib_dev_err(dd,
+			"Catastrophic software error, SendPIOAvailAddr written as %lx, read back as %llx\n",
+			(unsigned long) dd->pioavailregs_phys,
+			(unsigned long long) val);
 		ret = -EINVAL;
 	}
 
@@ -6098,8 +6105,8 @@ static int qib_init_7322_variables(struct qib_devdata *dd)
 	dd->revision = readq(&dd->kregbase[kr_revision]);
 
 	if ((dd->revision & 0xffffffffU) == 0xffffffffU) {
-		qib_dev_err(dd, "Revision register read failure, "
-			    "giving up initialization\n");
+		qib_dev_err(dd,
+			"Revision register read failure, giving up initialization\n");
 		ret = -ENODEV;
 		goto bail;
 	}
@@ -6265,9 +6272,9 @@ static int qib_init_7322_variables(struct qib_devdata *dd)
 		 */
 		if (!(dd->flags & QIB_HAS_QSFP)) {
 			if (!IS_QMH(dd) && !IS_QME(dd))
-				qib_devinfo(dd->pcidev, "IB%u:%u: "
-					    "Unknown mezzanine card type\n",
-					    dd->unit, ppd->port);
+				qib_devinfo(dd->pcidev,
+					"IB%u:%u: Unknown mezzanine card type\n",
+					dd->unit, ppd->port);
 			cp->h1_val = IS_QMH(dd) ? H1_FORCE_QMH : H1_FORCE_QME;
 			/*
 			 * Choose center value as default tx serdes setting
@@ -6922,8 +6929,8 @@ struct qib_devdata *qib_init_iba7322_funcs(struct pci_dev *pdev,
 		dd->cspec->msix_entries[i].msix.entry = i;
 
 	if (qib_pcie_params(dd, 8, &tabsize, dd->cspec->msix_entries))
-		qib_dev_err(dd, "Failed to setup PCIe or interrupts; "
-			    "continuing anyway\n");
+		qib_dev_err(dd,
+			"Failed to setup PCIe or interrupts; continuing anyway\n");
 	/* may be less than we wanted, if not enough available */
 	dd->cspec->num_msix_entries = tabsize;
 
@@ -7276,8 +7283,7 @@ static void find_best_ent(struct qib_pportdata *ppd,
 		   ppd->cpspec->no_eep < (TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ +
 					  TXDDS_MFG_SZ)) {
 		idx = ppd->cpspec->no_eep - (TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ);
-		printk(KERN_INFO QIB_DRV_NAME
-			" IB%u:%u use idx %u into txdds_mfg\n",
+		pr_info("IB%u:%u use idx %u into txdds_mfg\n",
 			ppd->dd->unit, ppd->port, idx);
 		*sdr_dds = &txdds_extra_mfg[idx];
 		*ddr_dds = &txdds_extra_mfg[idx];
@@ -7432,11 +7438,11 @@ static void serdes_7322_los_enable(struct qib_pportdata *ppd, int enable)
 	u8 state = SYM_FIELD(data, IBSerdesCtrl_0, RXLOSEN);
 
 	if (enable && !state) {
-		printk(KERN_INFO QIB_DRV_NAME " IB%u:%u Turning LOS on\n",
+		pr_info("IB%u:%u Turning LOS on\n",
 			ppd->dd->unit, ppd->port);
 		data |= SYM_MASK(IBSerdesCtrl_0, RXLOSEN);
 	} else if (!enable && state) {
-		printk(KERN_INFO QIB_DRV_NAME " IB%u:%u Turning LOS off\n",
+		pr_info("IB%u:%u Turning LOS off\n",
 			ppd->dd->unit, ppd->port);
 		data &= ~SYM_MASK(IBSerdesCtrl_0, RXLOSEN);
 	}
@@ -7672,8 +7678,7 @@ static int serdes_7322_init_new(struct qib_pportdata *ppd)
 		}
 	}
 	if (chan_done) {
-		printk(KERN_INFO QIB_DRV_NAME
-			 " Serdes %d calibration not done after .5 sec: 0x%x\n",
+		pr_info("Serdes %d calibration not done after .5 sec: 0x%x\n",
 			 IBSD(ppd->hw_pidx), chan_done);
 	} else {
 		for (chan = 0; chan < SERDES_CHANS; ++chan) {
@@ -7681,9 +7686,8 @@ static int serdes_7322_init_new(struct qib_pportdata *ppd)
 					    (chan + (chan >> 1)),
 					    25, 0, 0);
 			if ((~rxcaldone & (u32)BMASK(10, 10)) == 0)
-				printk(KERN_INFO QIB_DRV_NAME
-					 " Serdes %d chan %d calibration "
-					 "failed\n", IBSD(ppd->hw_pidx), chan);
+				pr_info("Serdes %d chan %d calibration failed\n",
+					IBSD(ppd->hw_pidx), chan);
 		}
 	}
 
diff --git a/drivers/infiniband/hw/qib/qib_init.c b/drivers/infiniband/hw/qib/qib_init.c
index dc14e100a7f..4443adfcd9e 100644
--- a/drivers/infiniband/hw/qib/qib_init.c
+++ b/drivers/infiniband/hw/qib/qib_init.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -38,9 +38,14 @@
 #include <linux/delay.h>
 #include <linux/idr.h>
 #include <linux/module.h>
+#include <linux/printk.h>
 
 #include "qib.h"
 #include "qib_common.h"
+#include "qib_mad.h"
+
+#undef pr_fmt
+#define pr_fmt(fmt) QIB_DRV_NAME ": " fmt
 
 /*
  * min buffers we want to have per context, after driver
@@ -71,6 +76,9 @@ unsigned qib_n_krcv_queues;
 module_param_named(krcvqs, qib_n_krcv_queues, uint, S_IRUGO);
 MODULE_PARM_DESC(krcvqs, "number of kernel receive queues per IB port");
 
+unsigned qib_cc_table_size;
+module_param_named(cc_table_size, qib_cc_table_size, uint, S_IRUGO);
+MODULE_PARM_DESC(cc_table_size, "Congestion control table entries 0 (CCA disabled - default), min = 128, max = 1984");
 /*
  * qib_wc_pat parameter:
  *      0 is WC via MTRR
@@ -120,8 +128,8 @@ int qib_create_ctxts(struct qib_devdata *dd)
 	 */
 	dd->rcd = kzalloc(sizeof(*dd->rcd) * dd->ctxtcnt, GFP_KERNEL);
 	if (!dd->rcd) {
-		qib_dev_err(dd, "Unable to allocate ctxtdata array, "
-			    "failing\n");
+		qib_dev_err(dd,
+			"Unable to allocate ctxtdata array, failing\n");
 		ret = -ENOMEM;
 		goto done;
 	}
@@ -137,8 +145,8 @@ int qib_create_ctxts(struct qib_devdata *dd)
 		ppd = dd->pport + (i % dd->num_pports);
 		rcd = qib_create_ctxtdata(ppd, i);
 		if (!rcd) {
-			qib_dev_err(dd, "Unable to allocate ctxtdata"
-				    " for Kernel ctxt, failing\n");
+			qib_dev_err(dd,
+				"Unable to allocate ctxtdata for Kernel ctxt, failing\n");
 			ret = -ENOMEM;
 			goto done;
 		}
@@ -199,6 +207,7 @@ struct qib_ctxtdata *qib_create_ctxtdata(struct qib_pportdata *ppd, u32 ctxt)
 void qib_init_pportdata(struct qib_pportdata *ppd, struct qib_devdata *dd,
 			u8 hw_pidx, u8 port)
 {
+	int size;
 	ppd->dd = dd;
 	ppd->hw_pidx = hw_pidx;
 	ppd->port = port; /* IB port number, not index */
@@ -210,6 +219,83 @@ void qib_init_pportdata(struct qib_pportdata *ppd, struct qib_devdata *dd,
 	init_timer(&ppd->symerr_clear_timer);
 	ppd->symerr_clear_timer.function = qib_clear_symerror_on_linkup;
 	ppd->symerr_clear_timer.data = (unsigned long)ppd;
+
+	ppd->qib_wq = NULL;
+
+	spin_lock_init(&ppd->cc_shadow_lock);
+
+	if (qib_cc_table_size < IB_CCT_MIN_ENTRIES)
+		goto bail;
+
+	ppd->cc_supported_table_entries = min(max_t(int, qib_cc_table_size,
+		IB_CCT_MIN_ENTRIES), IB_CCT_ENTRIES*IB_CC_TABLE_CAP_DEFAULT);
+
+	ppd->cc_max_table_entries =
+		ppd->cc_supported_table_entries/IB_CCT_ENTRIES;
+
+	size = IB_CC_TABLE_CAP_DEFAULT * sizeof(struct ib_cc_table_entry)
+		* IB_CCT_ENTRIES;
+	ppd->ccti_entries = kzalloc(size, GFP_KERNEL);
+	if (!ppd->ccti_entries) {
+		qib_dev_err(dd,
+		  "failed to allocate congestion control table for port %d!\n",
+		  port);
+		goto bail;
+	}
+
+	size = IB_CC_CCS_ENTRIES * sizeof(struct ib_cc_congestion_entry);
+	ppd->congestion_entries = kzalloc(size, GFP_KERNEL);
+	if (!ppd->congestion_entries) {
+		qib_dev_err(dd,
+		 "failed to allocate congestion setting list for port %d!\n",
+		 port);
+		goto bail_1;
+	}
+
+	size = sizeof(struct cc_table_shadow);
+	ppd->ccti_entries_shadow = kzalloc(size, GFP_KERNEL);
+	if (!ppd->ccti_entries_shadow) {
+		qib_dev_err(dd,
+		 "failed to allocate shadow ccti list for port %d!\n",
+		 port);
+		goto bail_2;
+	}
+
+	size = sizeof(struct ib_cc_congestion_setting_attr);
+	ppd->congestion_entries_shadow = kzalloc(size, GFP_KERNEL);
+	if (!ppd->congestion_entries_shadow) {
+		qib_dev_err(dd,
+		 "failed to allocate shadow congestion setting list for port %d!\n",
+		 port);
+		goto bail_3;
+	}
+
+	return;
+
+bail_3:
+	kfree(ppd->ccti_entries_shadow);
+	ppd->ccti_entries_shadow = NULL;
+bail_2:
+	kfree(ppd->congestion_entries);
+	ppd->congestion_entries = NULL;
+bail_1:
+	kfree(ppd->ccti_entries);
+	ppd->ccti_entries = NULL;
+bail:
+	/* User is intentionally disabling the congestion control agent */
+	if (!qib_cc_table_size)
+		return;
+
+	if (qib_cc_table_size < IB_CCT_MIN_ENTRIES) {
+		qib_cc_table_size = 0;
+		qib_dev_err(dd,
+		 "Congestion Control table size %d less than minimum %d for port %d\n",
+		 qib_cc_table_size, IB_CCT_MIN_ENTRIES, port);
+	}
+
+	qib_dev_err(dd, "Congestion Control Agent disabled for port %d\n",
+		port);
+	return;
 }
 
 static int init_pioavailregs(struct qib_devdata *dd)
@@ -221,8 +307,8 @@ static int init_pioavailregs(struct qib_devdata *dd)
 		&dd->pcidev->dev, PAGE_SIZE, &dd->pioavailregs_phys,
 		GFP_KERNEL);
 	if (!dd->pioavailregs_dma) {
-		qib_dev_err(dd, "failed to allocate PIOavail reg area "
-			    "in memory\n");
+		qib_dev_err(dd,
+			"failed to allocate PIOavail reg area in memory\n");
 		ret = -ENOMEM;
 		goto done;
 	}
@@ -277,15 +363,15 @@ static void init_shadow_tids(struct qib_devdata *dd)
 
 	pages = vzalloc(dd->cfgctxts * dd->rcvtidcnt * sizeof(struct page *));
 	if (!pages) {
-		qib_dev_err(dd, "failed to allocate shadow page * "
-			    "array, no expected sends!\n");
+		qib_dev_err(dd,
+			"failed to allocate shadow page * array, no expected sends!\n");
 		goto bail;
 	}
 
 	addrs = vzalloc(dd->cfgctxts * dd->rcvtidcnt * sizeof(dma_addr_t));
 	if (!addrs) {
-		qib_dev_err(dd, "failed to allocate shadow dma handle "
-			    "array, no expected sends!\n");
+		qib_dev_err(dd,
+			"failed to allocate shadow dma handle array, no expected sends!\n");
 		goto bail_free;
 	}
 
@@ -309,13 +395,13 @@ static int loadtime_init(struct qib_devdata *dd)
 
 	if (((dd->revision >> QLOGIC_IB_R_SOFTWARE_SHIFT) &
 	     QLOGIC_IB_R_SOFTWARE_MASK) != QIB_CHIP_SWVERSION) {
-		qib_dev_err(dd, "Driver only handles version %d, "
-			    "chip swversion is %d (%llx), failng\n",
-			    QIB_CHIP_SWVERSION,
-			    (int)(dd->revision >>
+		qib_dev_err(dd,
+			"Driver only handles version %d, chip swversion is %d (%llx), failng\n",
+			QIB_CHIP_SWVERSION,
+			(int)(dd->revision >>
 				QLOGIC_IB_R_SOFTWARE_SHIFT) &
-			    QLOGIC_IB_R_SOFTWARE_MASK,
-			    (unsigned long long) dd->revision);
+				QLOGIC_IB_R_SOFTWARE_MASK,
+			(unsigned long long) dd->revision);
 		ret = -ENOSYS;
 		goto done;
 	}
@@ -419,8 +505,8 @@ static void verify_interrupt(unsigned long opaque)
 	 */
 	if (dd->int_counter == 0) {
 		if (!dd->f_intr_fallback(dd))
-			dev_err(&dd->pcidev->dev, "No interrupts detected, "
-				"not usable.\n");
+			dev_err(&dd->pcidev->dev,
+				"No interrupts detected, not usable.\n");
 		else /* re-arm the timer to see if fallback works */
 			mod_timer(&dd->intrchk_timer, jiffies + HZ/2);
 	}
@@ -483,6 +569,41 @@ static void init_piobuf_state(struct qib_devdata *dd)
 }
 
 /**
+ * qib_create_workqueues - create per port workqueues
+ * @dd: the qlogic_ib device
+ */
+static int qib_create_workqueues(struct qib_devdata *dd)
+{
+	int pidx;
+	struct qib_pportdata *ppd;
+
+	for (pidx = 0; pidx < dd->num_pports; ++pidx) {
+		ppd = dd->pport + pidx;
+		if (!ppd->qib_wq) {
+			char wq_name[8]; /* 3 + 2 + 1 + 1 + 1 */
+			snprintf(wq_name, sizeof(wq_name), "qib%d_%d",
+				dd->unit, pidx);
+			ppd->qib_wq =
+				create_singlethread_workqueue(wq_name);
+			if (!ppd->qib_wq)
+				goto wq_error;
+		}
+	}
+	return 0;
+wq_error:
+	pr_err("create_singlethread_workqueue failed for port %d\n",
+		pidx + 1);
+	for (pidx = 0; pidx < dd->num_pports; ++pidx) {
+		ppd = dd->pport + pidx;
+		if (ppd->qib_wq) {
+			destroy_workqueue(ppd->qib_wq);
+			ppd->qib_wq = NULL;
+		}
+	}
+	return -ENOMEM;
+}
+
+/**
  * qib_init - do the actual initialization sequence on the chip
  * @dd: the qlogic_ib device
  * @reinit: reinitializing, so don't allocate new memory
@@ -547,8 +668,8 @@ int qib_init(struct qib_devdata *dd, int reinit)
 		if (!lastfail)
 			lastfail = qib_setup_eagerbufs(rcd);
 		if (lastfail) {
-			qib_dev_err(dd, "failed to allocate kernel ctxt's "
-				    "rcvhdrq and/or egr bufs\n");
+			qib_dev_err(dd,
+				"failed to allocate kernel ctxt's rcvhdrq and/or egr bufs\n");
 			continue;
 		}
 	}
@@ -764,6 +885,11 @@ static void qib_shutdown_device(struct qib_devdata *dd)
 		 * We can't count on interrupts since we are stopping.
 		 */
 		dd->f_quiet_serdes(ppd);
+
+		if (ppd->qib_wq) {
+			destroy_workqueue(ppd->qib_wq);
+			ppd->qib_wq = NULL;
+		}
 	}
 
 	qib_update_eeprom_log(dd);
@@ -893,8 +1019,7 @@ static void qib_verify_pioperf(struct qib_devdata *dd)
 	/* 1 GiB/sec, slightly over IB SDR line rate */
 	if (lcnt < (emsecs * 1024U))
 		qib_dev_err(dd,
-			    "Performance problem: bandwidth to PIO buffers is "
-			    "only %u MiB/sec\n",
+			    "Performance problem: bandwidth to PIO buffers is only %u MiB/sec\n",
 			    lcnt / (u32) emsecs);
 
 	preempt_enable();
@@ -967,8 +1092,8 @@ struct qib_devdata *qib_alloc_devdata(struct pci_dev *pdev, size_t extra)
 		if (qib_cpulist)
 			qib_cpulist_count = count;
 		else
-			qib_early_err(&pdev->dev, "Could not alloc cpulist "
-				      "info, cpu affinity might be wrong\n");
+			qib_early_err(&pdev->dev,
+				"Could not alloc cpulist info, cpu affinity might be wrong\n");
 	}
 
 bail:
@@ -1057,21 +1182,20 @@ static int __init qlogic_ib_init(void)
 	 */
 	idr_init(&qib_unit_table);
 	if (!idr_pre_get(&qib_unit_table, GFP_KERNEL)) {
-		printk(KERN_ERR QIB_DRV_NAME ": idr_pre_get() failed\n");
+		pr_err("idr_pre_get() failed\n");
 		ret = -ENOMEM;
 		goto bail_cq_wq;
 	}
 
 	ret = pci_register_driver(&qib_driver);
 	if (ret < 0) {
-		printk(KERN_ERR QIB_DRV_NAME
-		       ": Unable to register driver: error %d\n", -ret);
+		pr_err("Unable to register driver: error %d\n", -ret);
 		goto bail_unit;
 	}
 
 	/* not fatal if it doesn't work */
 	if (qib_init_qibfs())
-		printk(KERN_ERR QIB_DRV_NAME ": Unable to register ipathfs\n");
+		pr_err("Unable to register ipathfs\n");
 	goto bail; /* all OK */
 
 bail_unit:
@@ -1095,9 +1219,9 @@ static void __exit qlogic_ib_cleanup(void)
 
 	ret = qib_exit_qibfs();
 	if (ret)
-		printk(KERN_ERR QIB_DRV_NAME ": "
-			"Unable to cleanup counter filesystem: "
-			"error %d\n", -ret);
+		pr_err(
+			"Unable to cleanup counter filesystem: error %d\n",
+			-ret);
 
 	pci_unregister_driver(&qib_driver);
 
@@ -1121,10 +1245,24 @@ static void cleanup_device_data(struct qib_devdata *dd)
 	unsigned long flags;
 
 	/* users can't do anything more with chip */
-	for (pidx = 0; pidx < dd->num_pports; ++pidx)
+	for (pidx = 0; pidx < dd->num_pports; ++pidx) {
 		if (dd->pport[pidx].statusp)
 			*dd->pport[pidx].statusp &= ~QIB_STATUS_CHIP_PRESENT;
 
+		spin_lock(&dd->pport[pidx].cc_shadow_lock);
+
+		kfree(dd->pport[pidx].congestion_entries);
+		dd->pport[pidx].congestion_entries = NULL;
+		kfree(dd->pport[pidx].ccti_entries);
+		dd->pport[pidx].ccti_entries = NULL;
+		kfree(dd->pport[pidx].ccti_entries_shadow);
+		dd->pport[pidx].ccti_entries_shadow = NULL;
+		kfree(dd->pport[pidx].congestion_entries_shadow);
+		dd->pport[pidx].congestion_entries_shadow = NULL;
+
+		spin_unlock(&dd->pport[pidx].cc_shadow_lock);
+	}
+
 	if (!qib_wc_pat)
 		qib_disable_wc(dd);
 
@@ -1223,9 +1361,9 @@ static int __devinit qib_init_one(struct pci_dev *pdev,
 #ifdef CONFIG_PCI_MSI
 		dd = qib_init_iba6120_funcs(pdev, ent);
 #else
-		qib_early_err(&pdev->dev, "QLogic PCIE device 0x%x cannot "
-		      "work if CONFIG_PCI_MSI is not enabled\n",
-		      ent->device);
+		qib_early_err(&pdev->dev,
+			"QLogic PCIE device 0x%x cannot work if CONFIG_PCI_MSI is not enabled\n",
+			ent->device);
 		dd = ERR_PTR(-ENODEV);
 #endif
 		break;
@@ -1239,8 +1377,9 @@ static int __devinit qib_init_one(struct pci_dev *pdev,
 		break;
 
 	default:
-		qib_early_err(&pdev->dev, "Failing on unknown QLogic "
-			      "deviceid 0x%x\n", ent->device);
+		qib_early_err(&pdev->dev,
+			"Failing on unknown QLogic deviceid 0x%x\n",
+			ent->device);
 		ret = -ENODEV;
 	}
 
@@ -1249,6 +1388,10 @@ static int __devinit qib_init_one(struct pci_dev *pdev,
 	if (ret)
 		goto bail; /* error already printed */
 
+	ret = qib_create_workqueues(dd);
+	if (ret)
+		goto bail;
+
 	/* do the generic initialization */
 	initfail = qib_init(dd, 0);
 
@@ -1293,9 +1436,9 @@ static int __devinit qib_init_one(struct pci_dev *pdev,
 	if (!qib_wc_pat) {
 		ret = qib_enable_wc(dd);
 		if (ret) {
-			qib_dev_err(dd, "Write combining not enabled "
-				    "(err %d): performance may be poor\n",
-				    -ret);
+			qib_dev_err(dd,
+				"Write combining not enabled (err %d): performance may be poor\n",
+				-ret);
 			ret = 0;
 		}
 	}
@@ -1361,9 +1504,9 @@ int qib_create_rcvhdrq(struct qib_devdata *dd, struct qib_ctxtdata *rcd)
 			gfp_flags | __GFP_COMP);
 
 		if (!rcd->rcvhdrq) {
-			qib_dev_err(dd, "attempt to allocate %d bytes "
-				    "for ctxt %u rcvhdrq failed\n",
-				    amt, rcd->ctxt);
+			qib_dev_err(dd,
+				"attempt to allocate %d bytes for ctxt %u rcvhdrq failed\n",
+				amt, rcd->ctxt);
 			goto bail;
 		}
 
@@ -1392,8 +1535,9 @@ int qib_create_rcvhdrq(struct qib_devdata *dd, struct qib_ctxtdata *rcd)
 	return 0;
 
 bail_free:
-	qib_dev_err(dd, "attempt to allocate 1 page for ctxt %u "
-		    "rcvhdrqtailaddr failed\n", rcd->ctxt);
+	qib_dev_err(dd,
+		"attempt to allocate 1 page for ctxt %u rcvhdrqtailaddr failed\n",
+		rcd->ctxt);
 	vfree(rcd->user_event_mask);
 	rcd->user_event_mask = NULL;
 bail_free_hdrq:
diff --git a/drivers/infiniband/hw/qib/qib_intr.c b/drivers/infiniband/hw/qib/qib_intr.c
index 6ae57d23004..f4918f2165e 100644
--- a/drivers/infiniband/hw/qib/qib_intr.c
+++ b/drivers/infiniband/hw/qib/qib_intr.c
@@ -224,15 +224,15 @@ void qib_bad_intrstatus(struct qib_devdata *dd)
 	 * We print the message and disable interrupts, in hope of
 	 * having a better chance of debugging the problem.
 	 */
-	qib_dev_err(dd, "Read of chip interrupt status failed"
-		    " disabling interrupts\n");
+	qib_dev_err(dd,
+		"Read of chip interrupt status failed disabling interrupts\n");
 	if (allbits++) {
 		/* disable interrupt delivery, something is very wrong */
 		if (allbits == 2)
 			dd->f_set_intr_state(dd, 0);
 		if (allbits == 3) {
-			qib_dev_err(dd, "2nd bad interrupt status, "
-				    "unregistering interrupts\n");
+			qib_dev_err(dd,
+				"2nd bad interrupt status, unregistering interrupts\n");
 			dd->flags |= QIB_BADINTR;
 			dd->flags &= ~QIB_INITTED;
 			dd->f_free_irq(dd);
diff --git a/drivers/infiniband/hw/qib/qib_keys.c b/drivers/infiniband/hw/qib/qib_keys.c
index 8fd19a47df0..e9486c74c22 100644
--- a/drivers/infiniband/hw/qib/qib_keys.c
+++ b/drivers/infiniband/hw/qib/qib_keys.c
@@ -35,21 +35,41 @@
 
 /**
  * qib_alloc_lkey - allocate an lkey
- * @rkt: lkey table in which to allocate the lkey
  * @mr: memory region that this lkey protects
+ * @dma_region: 0->normal key, 1->restricted DMA key
+ *
+ * Returns 0 if successful, otherwise returns -errno.
+ *
+ * Increments mr reference count as required.
+ *
+ * Sets the lkey field mr for non-dma regions.
  *
- * Returns 1 if successful, otherwise returns 0.
  */
 
-int qib_alloc_lkey(struct qib_lkey_table *rkt, struct qib_mregion *mr)
+int qib_alloc_lkey(struct qib_mregion *mr, int dma_region)
 {
 	unsigned long flags;
 	u32 r;
 	u32 n;
-	int ret;
+	int ret = 0;
+	struct qib_ibdev *dev = to_idev(mr->pd->device);
+	struct qib_lkey_table *rkt = &dev->lk_table;
 
 	spin_lock_irqsave(&rkt->lock, flags);
 
+	/* special case for dma_mr lkey == 0 */
+	if (dma_region) {
+		struct qib_mregion *tmr;
+
+		tmr = rcu_dereference(dev->dma_mr);
+		if (!tmr) {
+			qib_get_mr(mr);
+			rcu_assign_pointer(dev->dma_mr, mr);
+			mr->lkey_published = 1;
+		}
+		goto success;
+	}
+
 	/* Find the next available LKEY */
 	r = rkt->next;
 	n = r;
@@ -57,11 +77,8 @@ int qib_alloc_lkey(struct qib_lkey_table *rkt, struct qib_mregion *mr)
 		if (rkt->table[r] == NULL)
 			break;
 		r = (r + 1) & (rkt->max - 1);
-		if (r == n) {
-			spin_unlock_irqrestore(&rkt->lock, flags);
-			ret = 0;
+		if (r == n)
 			goto bail;
-		}
 	}
 	rkt->next = (r + 1) & (rkt->max - 1);
 	/*
@@ -76,57 +93,58 @@ int qib_alloc_lkey(struct qib_lkey_table *rkt, struct qib_mregion *mr)
 		mr->lkey |= 1 << 8;
 		rkt->gen++;
 	}
-	rkt->table[r] = mr;
+	qib_get_mr(mr);
+	rcu_assign_pointer(rkt->table[r], mr);
+	mr->lkey_published = 1;
+success:
 	spin_unlock_irqrestore(&rkt->lock, flags);
-
-	ret = 1;
-
-bail:
+out:
 	return ret;
+bail:
+	spin_unlock_irqrestore(&rkt->lock, flags);
+	ret = -ENOMEM;
+	goto out;
 }
 
 /**
  * qib_free_lkey - free an lkey
- * @rkt: table from which to free the lkey
- * @lkey: lkey id to free
+ * @mr: mr to free from tables
  */
-int qib_free_lkey(struct qib_ibdev *dev, struct qib_mregion *mr)
+void qib_free_lkey(struct qib_mregion *mr)
 {
 	unsigned long flags;
 	u32 lkey = mr->lkey;
 	u32 r;
-	int ret;
+	struct qib_ibdev *dev = to_idev(mr->pd->device);
+	struct qib_lkey_table *rkt = &dev->lk_table;
 
-	spin_lock_irqsave(&dev->lk_table.lock, flags);
-	if (lkey == 0) {
-		if (dev->dma_mr && dev->dma_mr == mr) {
-			ret = atomic_read(&dev->dma_mr->refcount);
-			if (!ret)
-				dev->dma_mr = NULL;
-		} else
-			ret = 0;
-	} else {
+	spin_lock_irqsave(&rkt->lock, flags);
+	if (!mr->lkey_published)
+		goto out;
+	if (lkey == 0)
+		rcu_assign_pointer(dev->dma_mr, NULL);
+	else {
 		r = lkey >> (32 - ib_qib_lkey_table_size);
-		ret = atomic_read(&dev->lk_table.table[r]->refcount);
-		if (!ret)
-			dev->lk_table.table[r] = NULL;
+		rcu_assign_pointer(rkt->table[r], NULL);
 	}
-	spin_unlock_irqrestore(&dev->lk_table.lock, flags);
-
-	if (ret)
-		ret = -EBUSY;
-	return ret;
+	qib_put_mr(mr);
+	mr->lkey_published = 0;
+out:
+	spin_unlock_irqrestore(&rkt->lock, flags);
 }
 
 /**
  * qib_lkey_ok - check IB SGE for validity and initialize
  * @rkt: table containing lkey to check SGE against
+ * @pd: protection domain
  * @isge: outgoing internal SGE
  * @sge: SGE to check
  * @acc: access flags
  *
  * Return 1 if valid and successful, otherwise returns 0.
  *
+ * increments the reference count upon success
+ *
  * Check the IB SGE for validity and initialize our internal version
  * of it.
  */
@@ -136,24 +154,25 @@ int qib_lkey_ok(struct qib_lkey_table *rkt, struct qib_pd *pd,
 	struct qib_mregion *mr;
 	unsigned n, m;
 	size_t off;
-	unsigned long flags;
 
 	/*
 	 * We use LKEY == zero for kernel virtual addresses
 	 * (see qib_get_dma_mr and qib_dma.c).
 	 */
-	spin_lock_irqsave(&rkt->lock, flags);
+	rcu_read_lock();
 	if (sge->lkey == 0) {
 		struct qib_ibdev *dev = to_idev(pd->ibpd.device);
 
 		if (pd->user)
 			goto bail;
-		if (!dev->dma_mr)
+		mr = rcu_dereference(dev->dma_mr);
+		if (!mr)
 			goto bail;
-		atomic_inc(&dev->dma_mr->refcount);
-		spin_unlock_irqrestore(&rkt->lock, flags);
+		if (unlikely(!atomic_inc_not_zero(&mr->refcount)))
+			goto bail;
+		rcu_read_unlock();
 
-		isge->mr = dev->dma_mr;
+		isge->mr = mr;
 		isge->vaddr = (void *) sge->addr;
 		isge->length = sge->length;
 		isge->sge_length = sge->length;
@@ -161,18 +180,18 @@ int qib_lkey_ok(struct qib_lkey_table *rkt, struct qib_pd *pd,
 		isge->n = 0;
 		goto ok;
 	}
-	mr = rkt->table[(sge->lkey >> (32 - ib_qib_lkey_table_size))];
-	if (unlikely(mr == NULL || mr->lkey != sge->lkey ||
-		     mr->pd != &pd->ibpd))
+	mr = rcu_dereference(
+		rkt->table[(sge->lkey >> (32 - ib_qib_lkey_table_size))]);
+	if (unlikely(!mr || mr->lkey != sge->lkey || mr->pd != &pd->ibpd))
 		goto bail;
 
 	off = sge->addr - mr->user_base;
-	if (unlikely(sge->addr < mr->user_base ||
-		     off + sge->length > mr->length ||
-		     (mr->access_flags & acc) != acc))
+	if (unlikely(sge->addr < mr->iova || off + sge->length > mr->length ||
+		     (mr->access_flags & acc) == 0))
 		goto bail;
-	atomic_inc(&mr->refcount);
-	spin_unlock_irqrestore(&rkt->lock, flags);
+	if (unlikely(!atomic_inc_not_zero(&mr->refcount)))
+		goto bail;
+	rcu_read_unlock();
 
 	off += mr->offset;
 	if (mr->page_shift) {
@@ -208,20 +227,22 @@ int qib_lkey_ok(struct qib_lkey_table *rkt, struct qib_pd *pd,
 ok:
 	return 1;
 bail:
-	spin_unlock_irqrestore(&rkt->lock, flags);
+	rcu_read_unlock();
 	return 0;
 }
 
 /**
  * qib_rkey_ok - check the IB virtual address, length, and RKEY
- * @dev: infiniband device
- * @ss: SGE state
+ * @qp: qp for validation
+ * @sge: SGE state
  * @len: length of data
  * @vaddr: virtual address to place data
  * @rkey: rkey to check
  * @acc: access flags
  *
  * Return 1 if successful, otherwise 0.
+ *
+ * increments the reference count upon success
  */
 int qib_rkey_ok(struct qib_qp *qp, struct qib_sge *sge,
 		u32 len, u64 vaddr, u32 rkey, int acc)
@@ -230,25 +251,26 @@ int qib_rkey_ok(struct qib_qp *qp, struct qib_sge *sge,
 	struct qib_mregion *mr;
 	unsigned n, m;
 	size_t off;
-	unsigned long flags;
 
 	/*
 	 * We use RKEY == zero for kernel virtual addresses
 	 * (see qib_get_dma_mr and qib_dma.c).
 	 */
-	spin_lock_irqsave(&rkt->lock, flags);
+	rcu_read_lock();
 	if (rkey == 0) {
 		struct qib_pd *pd = to_ipd(qp->ibqp.pd);
 		struct qib_ibdev *dev = to_idev(pd->ibpd.device);
 
 		if (pd->user)
 			goto bail;
-		if (!dev->dma_mr)
+		mr = rcu_dereference(dev->dma_mr);
+		if (!mr)
 			goto bail;
-		atomic_inc(&dev->dma_mr->refcount);
-		spin_unlock_irqrestore(&rkt->lock, flags);
+		if (unlikely(!atomic_inc_not_zero(&mr->refcount)))
+			goto bail;
+		rcu_read_unlock();
 
-		sge->mr = dev->dma_mr;
+		sge->mr = mr;
 		sge->vaddr = (void *) vaddr;
 		sge->length = len;
 		sge->sge_length = len;
@@ -257,16 +279,18 @@ int qib_rkey_ok(struct qib_qp *qp, struct qib_sge *sge,
 		goto ok;
 	}
 
-	mr = rkt->table[(rkey >> (32 - ib_qib_lkey_table_size))];
-	if (unlikely(mr == NULL || mr->lkey != rkey || qp->ibqp.pd != mr->pd))
+	mr = rcu_dereference(
+		rkt->table[(rkey >> (32 - ib_qib_lkey_table_size))]);
+	if (unlikely(!mr || mr->lkey != rkey || qp->ibqp.pd != mr->pd))
 		goto bail;
 
 	off = vaddr - mr->iova;
 	if (unlikely(vaddr < mr->iova || off + len > mr->length ||
 		     (mr->access_flags & acc) == 0))
 		goto bail;
-	atomic_inc(&mr->refcount);
-	spin_unlock_irqrestore(&rkt->lock, flags);
+	if (unlikely(!atomic_inc_not_zero(&mr->refcount)))
+		goto bail;
+	rcu_read_unlock();
 
 	off += mr->offset;
 	if (mr->page_shift) {
@@ -302,7 +326,7 @@ int qib_rkey_ok(struct qib_qp *qp, struct qib_sge *sge,
 ok:
 	return 1;
 bail:
-	spin_unlock_irqrestore(&rkt->lock, flags);
+	rcu_read_unlock();
 	return 0;
 }
 
@@ -325,7 +349,9 @@ int qib_fast_reg_mr(struct qib_qp *qp, struct ib_send_wr *wr)
 	if (pd->user || rkey == 0)
 		goto bail;
 
-	mr = rkt->table[(rkey >> (32 - ib_qib_lkey_table_size))];
+	mr = rcu_dereference_protected(
+		rkt->table[(rkey >> (32 - ib_qib_lkey_table_size))],
+		lockdep_is_held(&rkt->lock));
 	if (unlikely(mr == NULL || qp->ibqp.pd != mr->pd))
 		goto bail;
 
diff --git a/drivers/infiniband/hw/qib/qib_mad.c b/drivers/infiniband/hw/qib/qib_mad.c
index 43390217a02..19f1e6c45fb 100644
--- a/drivers/infiniband/hw/qib/qib_mad.c
+++ b/drivers/infiniband/hw/qib/qib_mad.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -49,6 +49,18 @@ static int reply(struct ib_smp *smp)
 	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
 }
 
+static int reply_failure(struct ib_smp *smp)
+{
+	/*
+	 * The verbs framework will handle the directed/LID route
+	 * packet changes.
+	 */
+	smp->method = IB_MGMT_METHOD_GET_RESP;
+	if (smp->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
+		smp->status |= IB_SMP_DIRECTION;
+	return IB_MAD_RESULT_FAILURE | IB_MAD_RESULT_REPLY;
+}
+
 static void qib_send_trap(struct qib_ibport *ibp, void *data, unsigned len)
 {
 	struct ib_mad_send_buf *send_buf;
@@ -90,14 +102,10 @@ static void qib_send_trap(struct qib_ibport *ibp, void *data, unsigned len)
 	if (!ibp->sm_ah) {
 		if (ibp->sm_lid != be16_to_cpu(IB_LID_PERMISSIVE)) {
 			struct ib_ah *ah;
-			struct ib_ah_attr attr;
 
-			memset(&attr, 0, sizeof attr);
-			attr.dlid = ibp->sm_lid;
-			attr.port_num = ppd_from_ibp(ibp)->port;
-			ah = ib_create_ah(ibp->qp0->ibqp.pd, &attr);
+			ah = qib_create_qp0_ah(ibp, ibp->sm_lid);
 			if (IS_ERR(ah))
-				ret = -EINVAL;
+				ret = PTR_ERR(ah);
 			else {
 				send_buf->ah = ah;
 				ibp->sm_ah = to_iah(ah);
@@ -2051,6 +2059,298 @@ bail:
 	return ret;
 }
 
+static int cc_get_classportinfo(struct ib_cc_mad *ccp,
+				struct ib_device *ibdev)
+{
+	struct ib_cc_classportinfo_attr *p =
+		(struct ib_cc_classportinfo_attr *)ccp->mgmt_data;
+
+	memset(ccp->mgmt_data, 0, sizeof(ccp->mgmt_data));
+
+	p->base_version = 1;
+	p->class_version = 1;
+	p->cap_mask = 0;
+
+	/*
+	 * Expected response time is 4.096 usec. * 2^18 == 1.073741824 sec.
+	 */
+	p->resp_time_value = 18;
+
+	return reply((struct ib_smp *) ccp);
+}
+
+static int cc_get_congestion_info(struct ib_cc_mad *ccp,
+				struct ib_device *ibdev, u8 port)
+{
+	struct ib_cc_info_attr *p =
+		(struct ib_cc_info_attr *)ccp->mgmt_data;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+
+	memset(ccp->mgmt_data, 0, sizeof(ccp->mgmt_data));
+
+	p->congestion_info = 0;
+	p->control_table_cap = ppd->cc_max_table_entries;
+
+	return reply((struct ib_smp *) ccp);
+}
+
+static int cc_get_congestion_setting(struct ib_cc_mad *ccp,
+				struct ib_device *ibdev, u8 port)
+{
+	int i;
+	struct ib_cc_congestion_setting_attr *p =
+		(struct ib_cc_congestion_setting_attr *)ccp->mgmt_data;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+	struct ib_cc_congestion_entry_shadow *entries;
+
+	memset(ccp->mgmt_data, 0, sizeof(ccp->mgmt_data));
+
+	spin_lock(&ppd->cc_shadow_lock);
+
+	entries = ppd->congestion_entries_shadow->entries;
+	p->port_control = cpu_to_be16(
+		ppd->congestion_entries_shadow->port_control);
+	p->control_map = cpu_to_be16(
+		ppd->congestion_entries_shadow->control_map);
+	for (i = 0; i < IB_CC_CCS_ENTRIES; i++) {
+		p->entries[i].ccti_increase = entries[i].ccti_increase;
+		p->entries[i].ccti_timer = cpu_to_be16(entries[i].ccti_timer);
+		p->entries[i].trigger_threshold = entries[i].trigger_threshold;
+		p->entries[i].ccti_min = entries[i].ccti_min;
+	}
+
+	spin_unlock(&ppd->cc_shadow_lock);
+
+	return reply((struct ib_smp *) ccp);
+}
+
+static int cc_get_congestion_control_table(struct ib_cc_mad *ccp,
+				struct ib_device *ibdev, u8 port)
+{
+	struct ib_cc_table_attr *p =
+		(struct ib_cc_table_attr *)ccp->mgmt_data;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+	u32 cct_block_index = be32_to_cpu(ccp->attr_mod);
+	u32 max_cct_block;
+	u32 cct_entry;
+	struct ib_cc_table_entry_shadow *entries;
+	int i;
+
+	/* Is the table index more than what is supported? */
+	if (cct_block_index > IB_CC_TABLE_CAP_DEFAULT - 1)
+		goto bail;
+
+	memset(ccp->mgmt_data, 0, sizeof(ccp->mgmt_data));
+
+	spin_lock(&ppd->cc_shadow_lock);
+
+	max_cct_block =
+		(ppd->ccti_entries_shadow->ccti_last_entry + 1)/IB_CCT_ENTRIES;
+	max_cct_block = max_cct_block ? max_cct_block - 1 : 0;
+
+	if (cct_block_index > max_cct_block) {
+		spin_unlock(&ppd->cc_shadow_lock);
+		goto bail;
+	}
+
+	ccp->attr_mod = cpu_to_be32(cct_block_index);
+
+	cct_entry = IB_CCT_ENTRIES * (cct_block_index + 1);
+
+	cct_entry--;
+
+	p->ccti_limit = cpu_to_be16(cct_entry);
+
+	entries = &ppd->ccti_entries_shadow->
+			entries[IB_CCT_ENTRIES * cct_block_index];
+	cct_entry %= IB_CCT_ENTRIES;
+
+	for (i = 0; i <= cct_entry; i++)
+		p->ccti_entries[i].entry = cpu_to_be16(entries[i].entry);
+
+	spin_unlock(&ppd->cc_shadow_lock);
+
+	return reply((struct ib_smp *) ccp);
+
+bail:
+	return reply_failure((struct ib_smp *) ccp);
+}
+
+static int cc_set_congestion_setting(struct ib_cc_mad *ccp,
+				struct ib_device *ibdev, u8 port)
+{
+	struct ib_cc_congestion_setting_attr *p =
+		(struct ib_cc_congestion_setting_attr *)ccp->mgmt_data;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+	int i;
+
+	ppd->cc_sl_control_map = be16_to_cpu(p->control_map);
+
+	for (i = 0; i < IB_CC_CCS_ENTRIES; i++) {
+		ppd->congestion_entries[i].ccti_increase =
+			p->entries[i].ccti_increase;
+
+		ppd->congestion_entries[i].ccti_timer =
+			be16_to_cpu(p->entries[i].ccti_timer);
+
+		ppd->congestion_entries[i].trigger_threshold =
+			p->entries[i].trigger_threshold;
+
+		ppd->congestion_entries[i].ccti_min =
+			p->entries[i].ccti_min;
+	}
+
+	return reply((struct ib_smp *) ccp);
+}
+
+static int cc_set_congestion_control_table(struct ib_cc_mad *ccp,
+				struct ib_device *ibdev, u8 port)
+{
+	struct ib_cc_table_attr *p =
+		(struct ib_cc_table_attr *)ccp->mgmt_data;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+	u32 cct_block_index = be32_to_cpu(ccp->attr_mod);
+	u32 cct_entry;
+	struct ib_cc_table_entry_shadow *entries;
+	int i;
+
+	/* Is the table index more than what is supported? */
+	if (cct_block_index > IB_CC_TABLE_CAP_DEFAULT - 1)
+		goto bail;
+
+	/* If this packet is the first in the sequence then
+	 * zero the total table entry count.
+	 */
+	if (be16_to_cpu(p->ccti_limit) < IB_CCT_ENTRIES)
+		ppd->total_cct_entry = 0;
+
+	cct_entry = (be16_to_cpu(p->ccti_limit))%IB_CCT_ENTRIES;
+
+	/* ccti_limit is 0 to 63 */
+	ppd->total_cct_entry += (cct_entry + 1);
+
+	if (ppd->total_cct_entry > ppd->cc_supported_table_entries)
+		goto bail;
+
+	ppd->ccti_limit = be16_to_cpu(p->ccti_limit);
+
+	entries = ppd->ccti_entries + (IB_CCT_ENTRIES * cct_block_index);
+
+	for (i = 0; i <= cct_entry; i++)
+		entries[i].entry = be16_to_cpu(p->ccti_entries[i].entry);
+
+	spin_lock(&ppd->cc_shadow_lock);
+
+	ppd->ccti_entries_shadow->ccti_last_entry = ppd->total_cct_entry - 1;
+	memcpy(ppd->ccti_entries_shadow->entries, ppd->ccti_entries,
+		(ppd->total_cct_entry * sizeof(struct ib_cc_table_entry)));
+
+	ppd->congestion_entries_shadow->port_control = IB_CC_CCS_PC_SL_BASED;
+	ppd->congestion_entries_shadow->control_map = ppd->cc_sl_control_map;
+	memcpy(ppd->congestion_entries_shadow->entries, ppd->congestion_entries,
+		IB_CC_CCS_ENTRIES * sizeof(struct ib_cc_congestion_entry));
+
+	spin_unlock(&ppd->cc_shadow_lock);
+
+	return reply((struct ib_smp *) ccp);
+
+bail:
+	return reply_failure((struct ib_smp *) ccp);
+}
+
+static int check_cc_key(struct qib_ibport *ibp,
+			struct ib_cc_mad *ccp, int mad_flags)
+{
+	return 0;
+}
+
+static int process_cc(struct ib_device *ibdev, int mad_flags,
+			u8 port, struct ib_mad *in_mad,
+			struct ib_mad *out_mad)
+{
+	struct ib_cc_mad *ccp = (struct ib_cc_mad *)out_mad;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	int ret;
+
+	*out_mad = *in_mad;
+
+	if (ccp->class_version != 2) {
+		ccp->status |= IB_SMP_UNSUP_VERSION;
+		ret = reply((struct ib_smp *)ccp);
+		goto bail;
+	}
+
+	ret = check_cc_key(ibp, ccp, mad_flags);
+	if (ret)
+		goto bail;
+
+	switch (ccp->method) {
+	case IB_MGMT_METHOD_GET:
+		switch (ccp->attr_id) {
+		case IB_CC_ATTR_CLASSPORTINFO:
+			ret = cc_get_classportinfo(ccp, ibdev);
+			goto bail;
+
+		case IB_CC_ATTR_CONGESTION_INFO:
+			ret = cc_get_congestion_info(ccp, ibdev, port);
+			goto bail;
+
+		case IB_CC_ATTR_CA_CONGESTION_SETTING:
+			ret = cc_get_congestion_setting(ccp, ibdev, port);
+			goto bail;
+
+		case IB_CC_ATTR_CONGESTION_CONTROL_TABLE:
+			ret = cc_get_congestion_control_table(ccp, ibdev, port);
+			goto bail;
+
+			/* FALLTHROUGH */
+		default:
+			ccp->status |= IB_SMP_UNSUP_METH_ATTR;
+			ret = reply((struct ib_smp *) ccp);
+			goto bail;
+		}
+
+	case IB_MGMT_METHOD_SET:
+		switch (ccp->attr_id) {
+		case IB_CC_ATTR_CA_CONGESTION_SETTING:
+			ret = cc_set_congestion_setting(ccp, ibdev, port);
+			goto bail;
+
+		case IB_CC_ATTR_CONGESTION_CONTROL_TABLE:
+			ret = cc_set_congestion_control_table(ccp, ibdev, port);
+			goto bail;
+
+			/* FALLTHROUGH */
+		default:
+			ccp->status |= IB_SMP_UNSUP_METH_ATTR;
+			ret = reply((struct ib_smp *) ccp);
+			goto bail;
+		}
+
+	case IB_MGMT_METHOD_GET_RESP:
+		/*
+		 * The ib_mad module will call us to process responses
+		 * before checking for other consumers.
+		 * Just tell the caller to process it normally.
+		 */
+		ret = IB_MAD_RESULT_SUCCESS;
+		goto bail;
+
+	case IB_MGMT_METHOD_TRAP:
+	default:
+		ccp->status |= IB_SMP_UNSUP_METHOD;
+		ret = reply((struct ib_smp *) ccp);
+	}
+
+bail:
+	return ret;
+}
+
 /**
  * qib_process_mad - process an incoming MAD packet
  * @ibdev: the infiniband device this packet came in on
@@ -2075,6 +2375,8 @@ int qib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port,
 		    struct ib_mad *in_mad, struct ib_mad *out_mad)
 {
 	int ret;
+	struct qib_ibport *ibp = to_iport(ibdev, port);
+	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
 
 	switch (in_mad->mad_hdr.mgmt_class) {
 	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
@@ -2086,6 +2388,15 @@ int qib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port,
 		ret = process_perf(ibdev, port, in_mad, out_mad);
 		goto bail;
 
+	case IB_MGMT_CLASS_CONG_MGMT:
+		if (!ppd->congestion_entries_shadow ||
+			 !qib_cc_table_size) {
+			ret = IB_MAD_RESULT_SUCCESS;
+			goto bail;
+		}
+		ret = process_cc(ibdev, mad_flags, port, in_mad, out_mad);
+		goto bail;
+
 	default:
 		ret = IB_MAD_RESULT_SUCCESS;
 	}
diff --git a/drivers/infiniband/hw/qib/qib_mad.h b/drivers/infiniband/hw/qib/qib_mad.h
index ecc416cdbaa..57bd3fa016b 100644
--- a/drivers/infiniband/hw/qib/qib_mad.h
+++ b/drivers/infiniband/hw/qib/qib_mad.h
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -31,6 +31,8 @@
  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
+#ifndef _QIB_MAD_H
+#define _QIB_MAD_H
 
 #include <rdma/ib_pma.h>
 
@@ -223,6 +225,198 @@ struct ib_pma_portcounters_cong {
 #define IB_PMA_SEL_CONG_ROUTING                 0x08
 
 /*
+ * Congestion control class attributes
+ */
+#define IB_CC_ATTR_CLASSPORTINFO			cpu_to_be16(0x0001)
+#define IB_CC_ATTR_NOTICE				cpu_to_be16(0x0002)
+#define IB_CC_ATTR_CONGESTION_INFO			cpu_to_be16(0x0011)
+#define IB_CC_ATTR_CONGESTION_KEY_INFO			cpu_to_be16(0x0012)
+#define IB_CC_ATTR_CONGESTION_LOG			cpu_to_be16(0x0013)
+#define IB_CC_ATTR_SWITCH_CONGESTION_SETTING		cpu_to_be16(0x0014)
+#define IB_CC_ATTR_SWITCH_PORT_CONGESTION_SETTING	cpu_to_be16(0x0015)
+#define IB_CC_ATTR_CA_CONGESTION_SETTING		cpu_to_be16(0x0016)
+#define IB_CC_ATTR_CONGESTION_CONTROL_TABLE		cpu_to_be16(0x0017)
+#define IB_CC_ATTR_TIME_STAMP				cpu_to_be16(0x0018)
+
+/* generalizations for threshold values */
+#define IB_CC_THRESHOLD_NONE 0x0
+#define IB_CC_THRESHOLD_MIN  0x1
+#define IB_CC_THRESHOLD_MAX  0xf
+
+/* CCA MAD header constants */
+#define IB_CC_MAD_LOGDATA_LEN 32
+#define IB_CC_MAD_MGMTDATA_LEN 192
+
+struct ib_cc_mad {
+	u8	base_version;
+	u8	mgmt_class;
+	u8	class_version;
+	u8	method;
+	__be16	status;
+	__be16	class_specific;
+	__be64	tid;
+	__be16	attr_id;
+	__be16	resv;
+	__be32	attr_mod;
+	__be64 cckey;
+
+	/* For CongestionLog attribute only */
+	u8 log_data[IB_CC_MAD_LOGDATA_LEN];
+
+	u8 mgmt_data[IB_CC_MAD_MGMTDATA_LEN];
+} __packed;
+
+/*
+ * Congestion Control class portinfo capability mask bits
+ */
+#define IB_CC_CPI_CM_TRAP_GEN		cpu_to_be16(1 << 0)
+#define IB_CC_CPI_CM_GET_SET_NOTICE	cpu_to_be16(1 << 1)
+#define IB_CC_CPI_CM_CAP2		cpu_to_be16(1 << 2)
+#define IB_CC_CPI_CM_ENHANCEDPORT0_CC	cpu_to_be16(1 << 8)
+
+struct ib_cc_classportinfo_attr {
+	u8 base_version;
+	u8 class_version;
+	__be16 cap_mask;
+	u8 reserved[3];
+	u8 resp_time_value;     /* only lower 5 bits */
+	union ib_gid redirect_gid;
+	__be32 redirect_tc_sl_fl;       /* 8, 4, 20 bits respectively */
+	__be16 redirect_lid;
+	__be16 redirect_pkey;
+	__be32 redirect_qp;     /* only lower 24 bits */
+	__be32 redirect_qkey;
+	union ib_gid trap_gid;
+	__be32 trap_tc_sl_fl;   /* 8, 4, 20 bits respectively */
+	__be16 trap_lid;
+	__be16 trap_pkey;
+	__be32 trap_hl_qp;      /* 8, 24 bits respectively */
+	__be32 trap_qkey;
+} __packed;
+
+/* Congestion control traps */
+#define IB_CC_TRAP_KEY_VIOLATION 0x0000
+
+struct ib_cc_trap_key_violation_attr {
+	__be16 source_lid;
+	u8 method;
+	u8 reserved1;
+	__be16 attrib_id;
+	__be32 attrib_mod;
+	__be32 qp;
+	__be64 cckey;
+	u8 sgid[16];
+	u8 padding[24];
+} __packed;
+
+/* Congestion info flags */
+#define IB_CC_CI_FLAGS_CREDIT_STARVATION 0x1
+#define IB_CC_TABLE_CAP_DEFAULT 31
+
+struct ib_cc_info_attr {
+	__be16 congestion_info;
+	u8  control_table_cap; /* Multiple of 64 entry unit CCTs */
+} __packed;
+
+struct ib_cc_key_info_attr {
+	__be64 cckey;
+	u8  protect;
+	__be16 lease_period;
+	__be16 violations;
+} __packed;
+
+#define IB_CC_CL_CA_LOGEVENTS_LEN 208
+
+struct ib_cc_log_attr {
+	u8 log_type;
+	u8 congestion_flags;
+	__be16 threshold_event_counter;
+	__be16 threshold_congestion_event_map;
+	__be16 current_time_stamp;
+	u8 log_events[IB_CC_CL_CA_LOGEVENTS_LEN];
+} __packed;
+
+#define IB_CC_CLEC_SERVICETYPE_RC 0x0
+#define IB_CC_CLEC_SERVICETYPE_UC 0x1
+#define IB_CC_CLEC_SERVICETYPE_RD 0x2
+#define IB_CC_CLEC_SERVICETYPE_UD 0x3
+
+struct ib_cc_log_event {
+	u8 local_qp_cn_entry;
+	u8 remote_qp_number_cn_entry[3];
+	u8  sl_cn_entry:4;
+	u8  service_type_cn_entry:4;
+	__be32 remote_lid_cn_entry;
+	__be32 timestamp_cn_entry;
+} __packed;
+
+/* Sixteen congestion entries */
+#define IB_CC_CCS_ENTRIES 16
+
+/* Port control flags */
+#define IB_CC_CCS_PC_SL_BASED 0x01
+
+struct ib_cc_congestion_entry {
+	u8 ccti_increase;
+	__be16 ccti_timer;
+	u8 trigger_threshold;
+	u8 ccti_min; /* min CCTI for cc table */
+} __packed;
+
+struct ib_cc_congestion_entry_shadow {
+	u8 ccti_increase;
+	u16 ccti_timer;
+	u8 trigger_threshold;
+	u8 ccti_min; /* min CCTI for cc table */
+} __packed;
+
+struct ib_cc_congestion_setting_attr {
+	__be16 port_control;
+	__be16 control_map;
+	struct ib_cc_congestion_entry entries[IB_CC_CCS_ENTRIES];
+} __packed;
+
+struct ib_cc_congestion_setting_attr_shadow {
+	u16 port_control;
+	u16 control_map;
+	struct ib_cc_congestion_entry_shadow entries[IB_CC_CCS_ENTRIES];
+} __packed;
+
+#define IB_CC_TABLE_ENTRY_INCREASE_DEFAULT 1
+#define IB_CC_TABLE_ENTRY_TIMER_DEFAULT 1
+
+/* 64 Congestion Control table entries in a single MAD */
+#define IB_CCT_ENTRIES 64
+#define IB_CCT_MIN_ENTRIES (IB_CCT_ENTRIES * 2)
+
+struct ib_cc_table_entry {
+	__be16 entry; /* shift:2, multiplier:14 */
+};
+
+struct ib_cc_table_entry_shadow {
+	u16 entry; /* shift:2, multiplier:14 */
+};
+
+struct ib_cc_table_attr {
+	__be16 ccti_limit; /* max CCTI for cc table */
+	struct ib_cc_table_entry ccti_entries[IB_CCT_ENTRIES];
+} __packed;
+
+struct ib_cc_table_attr_shadow {
+	u16 ccti_limit; /* max CCTI for cc table */
+	struct ib_cc_table_entry_shadow ccti_entries[IB_CCT_ENTRIES];
+} __packed;
+
+#define CC_TABLE_SHADOW_MAX \
+	(IB_CC_TABLE_CAP_DEFAULT * IB_CCT_ENTRIES)
+
+struct cc_table_shadow {
+	u16 ccti_last_entry;
+	struct ib_cc_table_entry_shadow entries[CC_TABLE_SHADOW_MAX];
+} __packed;
+
+#endif				/* _QIB_MAD_H */
+/*
  * The PortSamplesControl.CounterMasks field is an array of 3 bit fields
  * which specify the N'th counter's capabilities. See ch. 16.1.3.2.
  * We support 5 counters which only count the mandatory quantities.
diff --git a/drivers/infiniband/hw/qib/qib_mr.c b/drivers/infiniband/hw/qib/qib_mr.c
index 08944e2ee33..e6687ded821 100644
--- a/drivers/infiniband/hw/qib/qib_mr.c
+++ b/drivers/infiniband/hw/qib/qib_mr.c
@@ -47,6 +47,43 @@ static inline struct qib_fmr *to_ifmr(struct ib_fmr *ibfmr)
 	return container_of(ibfmr, struct qib_fmr, ibfmr);
 }
 
+static int init_qib_mregion(struct qib_mregion *mr, struct ib_pd *pd,
+	int count)
+{
+	int m, i = 0;
+	int rval = 0;
+
+	m = (count + QIB_SEGSZ - 1) / QIB_SEGSZ;
+	for (; i < m; i++) {
+		mr->map[i] = kzalloc(sizeof *mr->map[0], GFP_KERNEL);
+		if (!mr->map[i])
+			goto bail;
+	}
+	mr->mapsz = m;
+	init_completion(&mr->comp);
+	/* count returning the ptr to user */
+	atomic_set(&mr->refcount, 1);
+	mr->pd = pd;
+	mr->max_segs = count;
+out:
+	return rval;
+bail:
+	while (i)
+		kfree(mr->map[--i]);
+	rval = -ENOMEM;
+	goto out;
+}
+
+static void deinit_qib_mregion(struct qib_mregion *mr)
+{
+	int i = mr->mapsz;
+
+	mr->mapsz = 0;
+	while (i)
+		kfree(mr->map[--i]);
+}
+
+
 /**
  * qib_get_dma_mr - get a DMA memory region
  * @pd: protection domain for this memory region
@@ -58,10 +95,9 @@ static inline struct qib_fmr *to_ifmr(struct ib_fmr *ibfmr)
  */
 struct ib_mr *qib_get_dma_mr(struct ib_pd *pd, int acc)
 {
-	struct qib_ibdev *dev = to_idev(pd->device);
-	struct qib_mr *mr;
+	struct qib_mr *mr = NULL;
 	struct ib_mr *ret;
-	unsigned long flags;
+	int rval;
 
 	if (to_ipd(pd)->user) {
 		ret = ERR_PTR(-EPERM);
@@ -74,61 +110,64 @@ struct ib_mr *qib_get_dma_mr(struct ib_pd *pd, int acc)
 		goto bail;
 	}
 
-	mr->mr.access_flags = acc;
-	atomic_set(&mr->mr.refcount, 0);
+	rval = init_qib_mregion(&mr->mr, pd, 0);
+	if (rval) {
+		ret = ERR_PTR(rval);
+		goto bail;
+	}
+
 
-	spin_lock_irqsave(&dev->lk_table.lock, flags);
-	if (!dev->dma_mr)
-		dev->dma_mr = &mr->mr;
-	spin_unlock_irqrestore(&dev->lk_table.lock, flags);
+	rval = qib_alloc_lkey(&mr->mr, 1);
+	if (rval) {
+		ret = ERR_PTR(rval);
+		goto bail_mregion;
+	}
 
+	mr->mr.access_flags = acc;
 	ret = &mr->ibmr;
+done:
+	return ret;
 
+bail_mregion:
+	deinit_qib_mregion(&mr->mr);
 bail:
-	return ret;
+	kfree(mr);
+	goto done;
 }
 
-static struct qib_mr *alloc_mr(int count, struct qib_lkey_table *lk_table)
+static struct qib_mr *alloc_mr(int count, struct ib_pd *pd)
 {
 	struct qib_mr *mr;
-	int m, i = 0;
+	int rval = -ENOMEM;
+	int m;
 
 	/* Allocate struct plus pointers to first level page tables. */
 	m = (count + QIB_SEGSZ - 1) / QIB_SEGSZ;
-	mr = kmalloc(sizeof *mr + m * sizeof mr->mr.map[0], GFP_KERNEL);
+	mr = kzalloc(sizeof *mr + m * sizeof mr->mr.map[0], GFP_KERNEL);
 	if (!mr)
-		goto done;
-
-	/* Allocate first level page tables. */
-	for (; i < m; i++) {
-		mr->mr.map[i] = kmalloc(sizeof *mr->mr.map[0], GFP_KERNEL);
-		if (!mr->mr.map[i])
-			goto bail;
-	}
-	mr->mr.mapsz = m;
-	mr->mr.page_shift = 0;
-	mr->mr.max_segs = count;
+		goto bail;
 
+	rval = init_qib_mregion(&mr->mr, pd, count);
+	if (rval)
+		goto bail;
 	/*
 	 * ib_reg_phys_mr() will initialize mr->ibmr except for
 	 * lkey and rkey.
 	 */
-	if (!qib_alloc_lkey(lk_table, &mr->mr))
-		goto bail;
+	rval = qib_alloc_lkey(&mr->mr, 0);
+	if (rval)
+		goto bail_mregion;
 	mr->ibmr.lkey = mr->mr.lkey;
 	mr->ibmr.rkey = mr->mr.lkey;
+done:
+	return mr;
 
-	atomic_set(&mr->mr.refcount, 0);
-	goto done;
-
+bail_mregion:
+	deinit_qib_mregion(&mr->mr);
 bail:
-	while (i)
-		kfree(mr->mr.map[--i]);
 	kfree(mr);
-	mr = NULL;
-
-done:
-	return mr;
+	mr = ERR_PTR(rval);
+	goto done;
 }
 
 /**
@@ -148,19 +187,15 @@ struct ib_mr *qib_reg_phys_mr(struct ib_pd *pd,
 	int n, m, i;
 	struct ib_mr *ret;
 
-	mr = alloc_mr(num_phys_buf, &to_idev(pd->device)->lk_table);
-	if (mr == NULL) {
-		ret = ERR_PTR(-ENOMEM);
+	mr = alloc_mr(num_phys_buf, pd);
+	if (IS_ERR(mr)) {
+		ret = (struct ib_mr *)mr;
 		goto bail;
 	}
 
-	mr->mr.pd = pd;
 	mr->mr.user_base = *iova_start;
 	mr->mr.iova = *iova_start;
-	mr->mr.length = 0;
-	mr->mr.offset = 0;
 	mr->mr.access_flags = acc;
-	mr->umem = NULL;
 
 	m = 0;
 	n = 0;
@@ -186,7 +221,6 @@ bail:
  * @pd: protection domain for this memory region
  * @start: starting userspace address
  * @length: length of region to register
- * @virt_addr: virtual address to use (from HCA's point of view)
  * @mr_access_flags: access flags for this memory region
  * @udata: unused by the QLogic_IB driver
  *
@@ -216,14 +250,13 @@ struct ib_mr *qib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 	list_for_each_entry(chunk, &umem->chunk_list, list)
 		n += chunk->nents;
 
-	mr = alloc_mr(n, &to_idev(pd->device)->lk_table);
-	if (!mr) {
-		ret = ERR_PTR(-ENOMEM);
+	mr = alloc_mr(n, pd);
+	if (IS_ERR(mr)) {
+		ret = (struct ib_mr *)mr;
 		ib_umem_release(umem);
 		goto bail;
 	}
 
-	mr->mr.pd = pd;
 	mr->mr.user_base = start;
 	mr->mr.iova = virt_addr;
 	mr->mr.length = length;
@@ -271,21 +304,25 @@ bail:
 int qib_dereg_mr(struct ib_mr *ibmr)
 {
 	struct qib_mr *mr = to_imr(ibmr);
-	struct qib_ibdev *dev = to_idev(ibmr->device);
-	int ret;
-	int i;
-
-	ret = qib_free_lkey(dev, &mr->mr);
-	if (ret)
-		return ret;
-
-	i = mr->mr.mapsz;
-	while (i)
-		kfree(mr->mr.map[--i]);
+	int ret = 0;
+	unsigned long timeout;
+
+	qib_free_lkey(&mr->mr);
+
+	qib_put_mr(&mr->mr); /* will set completion if last */
+	timeout = wait_for_completion_timeout(&mr->mr.comp,
+		5 * HZ);
+	if (!timeout) {
+		qib_get_mr(&mr->mr);
+		ret = -EBUSY;
+		goto out;
+	}
+	deinit_qib_mregion(&mr->mr);
 	if (mr->umem)
 		ib_umem_release(mr->umem);
 	kfree(mr);
-	return 0;
+out:
+	return ret;
 }
 
 /*
@@ -298,17 +335,9 @@ struct ib_mr *qib_alloc_fast_reg_mr(struct ib_pd *pd, int max_page_list_len)
 {
 	struct qib_mr *mr;
 
-	mr = alloc_mr(max_page_list_len, &to_idev(pd->device)->lk_table);
-	if (mr == NULL)
-		return ERR_PTR(-ENOMEM);
-
-	mr->mr.pd = pd;
-	mr->mr.user_base = 0;
-	mr->mr.iova = 0;
-	mr->mr.length = 0;
-	mr->mr.offset = 0;
-	mr->mr.access_flags = 0;
-	mr->umem = NULL;
+	mr = alloc_mr(max_page_list_len, pd);
+	if (IS_ERR(mr))
+		return (struct ib_mr *)mr;
 
 	return &mr->ibmr;
 }
@@ -322,11 +351,11 @@ qib_alloc_fast_reg_page_list(struct ib_device *ibdev, int page_list_len)
 	if (size > PAGE_SIZE)
 		return ERR_PTR(-EINVAL);
 
-	pl = kmalloc(sizeof *pl, GFP_KERNEL);
+	pl = kzalloc(sizeof *pl, GFP_KERNEL);
 	if (!pl)
 		return ERR_PTR(-ENOMEM);
 
-	pl->page_list = kmalloc(size, GFP_KERNEL);
+	pl->page_list = kzalloc(size, GFP_KERNEL);
 	if (!pl->page_list)
 		goto err_free;
 
@@ -355,57 +384,47 @@ struct ib_fmr *qib_alloc_fmr(struct ib_pd *pd, int mr_access_flags,
 			     struct ib_fmr_attr *fmr_attr)
 {
 	struct qib_fmr *fmr;
-	int m, i = 0;
+	int m;
 	struct ib_fmr *ret;
+	int rval = -ENOMEM;
 
 	/* Allocate struct plus pointers to first level page tables. */
 	m = (fmr_attr->max_pages + QIB_SEGSZ - 1) / QIB_SEGSZ;
-	fmr = kmalloc(sizeof *fmr + m * sizeof fmr->mr.map[0], GFP_KERNEL);
+	fmr = kzalloc(sizeof *fmr + m * sizeof fmr->mr.map[0], GFP_KERNEL);
 	if (!fmr)
 		goto bail;
 
-	/* Allocate first level page tables. */
-	for (; i < m; i++) {
-		fmr->mr.map[i] = kmalloc(sizeof *fmr->mr.map[0],
-					 GFP_KERNEL);
-		if (!fmr->mr.map[i])
-			goto bail;
-	}
-	fmr->mr.mapsz = m;
+	rval = init_qib_mregion(&fmr->mr, pd, fmr_attr->max_pages);
+	if (rval)
+		goto bail;
 
 	/*
 	 * ib_alloc_fmr() will initialize fmr->ibfmr except for lkey &
 	 * rkey.
 	 */
-	if (!qib_alloc_lkey(&to_idev(pd->device)->lk_table, &fmr->mr))
-		goto bail;
+	rval = qib_alloc_lkey(&fmr->mr, 0);
+	if (rval)
+		goto bail_mregion;
 	fmr->ibfmr.rkey = fmr->mr.lkey;
 	fmr->ibfmr.lkey = fmr->mr.lkey;
 	/*
 	 * Resources are allocated but no valid mapping (RKEY can't be
 	 * used).
 	 */
-	fmr->mr.pd = pd;
-	fmr->mr.user_base = 0;
-	fmr->mr.iova = 0;
-	fmr->mr.length = 0;
-	fmr->mr.offset = 0;
 	fmr->mr.access_flags = mr_access_flags;
 	fmr->mr.max_segs = fmr_attr->max_pages;
 	fmr->mr.page_shift = fmr_attr->page_shift;
 
-	atomic_set(&fmr->mr.refcount, 0);
 	ret = &fmr->ibfmr;
-	goto done;
+done:
+	return ret;
 
+bail_mregion:
+	deinit_qib_mregion(&fmr->mr);
 bail:
-	while (i)
-		kfree(fmr->mr.map[--i]);
 	kfree(fmr);
-	ret = ERR_PTR(-ENOMEM);
-
-done:
-	return ret;
+	ret = ERR_PTR(rval);
+	goto done;
 }
 
 /**
@@ -428,7 +447,8 @@ int qib_map_phys_fmr(struct ib_fmr *ibfmr, u64 *page_list,
 	u32 ps;
 	int ret;
 
-	if (atomic_read(&fmr->mr.refcount))
+	i = atomic_read(&fmr->mr.refcount);
+	if (i > 2)
 		return -EBUSY;
 
 	if (list_len > fmr->mr.max_segs) {
@@ -490,16 +510,27 @@ int qib_unmap_fmr(struct list_head *fmr_list)
 int qib_dealloc_fmr(struct ib_fmr *ibfmr)
 {
 	struct qib_fmr *fmr = to_ifmr(ibfmr);
-	int ret;
-	int i;
+	int ret = 0;
+	unsigned long timeout;
+
+	qib_free_lkey(&fmr->mr);
+	qib_put_mr(&fmr->mr); /* will set completion if last */
+	timeout = wait_for_completion_timeout(&fmr->mr.comp,
+		5 * HZ);
+	if (!timeout) {
+		qib_get_mr(&fmr->mr);
+		ret = -EBUSY;
+		goto out;
+	}
+	deinit_qib_mregion(&fmr->mr);
+	kfree(fmr);
+out:
+	return ret;
+}
 
-	ret = qib_free_lkey(to_idev(ibfmr->device), &fmr->mr);
-	if (ret)
-		return ret;
+void mr_rcu_callback(struct rcu_head *list)
+{
+	struct qib_mregion *mr = container_of(list, struct qib_mregion, list);
 
-	i = fmr->mr.mapsz;
-	while (i)
-		kfree(fmr->mr.map[--i]);
-	kfree(fmr);
-	return 0;
+	complete(&mr->comp);
 }
diff --git a/drivers/infiniband/hw/qib/qib_pcie.c b/drivers/infiniband/hw/qib/qib_pcie.c
index 790646ef510..062c301ebf5 100644
--- a/drivers/infiniband/hw/qib/qib_pcie.c
+++ b/drivers/infiniband/hw/qib/qib_pcie.c
@@ -224,8 +224,9 @@ static void qib_msix_setup(struct qib_devdata *dd, int pos, u32 *msixcnt,
 	}
 do_intx:
 	if (ret) {
-		qib_dev_err(dd, "pci_enable_msix %d vectors failed: %d, "
-			    "falling back to INTx\n", tabsize, ret);
+		qib_dev_err(dd,
+			"pci_enable_msix %d vectors failed: %d, falling back to INTx\n",
+			tabsize, ret);
 		tabsize = 0;
 	}
 	for (i = 0; i < tabsize; i++)
@@ -251,8 +252,9 @@ static int qib_msi_setup(struct qib_devdata *dd, int pos)
 
 	ret = pci_enable_msi(pdev);
 	if (ret)
-		qib_dev_err(dd, "pci_enable_msi failed: %d, "
-			    "interrupts may not work\n", ret);
+		qib_dev_err(dd,
+			"pci_enable_msi failed: %d, interrupts may not work\n",
+			ret);
 	/* continue even if it fails, we may still be OK... */
 
 	pci_read_config_dword(pdev, pos + PCI_MSI_ADDRESS_LO,
@@ -358,8 +360,8 @@ int qib_reinit_intr(struct qib_devdata *dd)
 
 	pos = pci_find_capability(dd->pcidev, PCI_CAP_ID_MSI);
 	if (!pos) {
-		qib_dev_err(dd, "Can't find MSI capability, "
-			    "can't restore MSI settings\n");
+		qib_dev_err(dd,
+			"Can't find MSI capability, can't restore MSI settings\n");
 		ret = 0;
 		/* nothing special for MSIx, just MSI */
 		goto bail;
@@ -471,8 +473,8 @@ void qib_pcie_reenable(struct qib_devdata *dd, u16 cmd, u8 iline, u8 cline)
 	pci_write_config_byte(dd->pcidev, PCI_CACHE_LINE_SIZE, cline);
 	r = pci_enable_device(dd->pcidev);
 	if (r)
-		qib_dev_err(dd, "pci_enable_device failed after "
-			    "reset: %d\n", r);
+		qib_dev_err(dd,
+			"pci_enable_device failed after reset: %d\n", r);
 }
 
 /* code to adjust PCIe capabilities. */
@@ -717,15 +719,16 @@ qib_pci_mmio_enabled(struct pci_dev *pdev)
 		if (words == ~0ULL)
 			ret = PCI_ERS_RESULT_NEED_RESET;
 	}
-	qib_devinfo(pdev, "QIB mmio_enabled function called, "
-		 "read wordscntr %Lx, returning %d\n", words, ret);
+	qib_devinfo(pdev,
+		"QIB mmio_enabled function called, read wordscntr %Lx, returning %d\n",
+		words, ret);
 	return  ret;
 }
 
 static pci_ers_result_t
 qib_pci_slot_reset(struct pci_dev *pdev)
 {
-	qib_devinfo(pdev, "QIB link_reset function called, ignored\n");
+	qib_devinfo(pdev, "QIB slot_reset function called, ignored\n");
 	return PCI_ERS_RESULT_CAN_RECOVER;
 }
 
diff --git a/drivers/infiniband/hw/qib/qib_qp.c b/drivers/infiniband/hw/qib/qib_qp.c
index 1ce56b51ab1..4850d03870c 100644
--- a/drivers/infiniband/hw/qib/qib_qp.c
+++ b/drivers/infiniband/hw/qib/qib_qp.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation.  * All rights reserved.
  * Copyright (c) 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -250,23 +250,33 @@ static void remove_qp(struct qib_ibdev *dev, struct qib_qp *qp)
 
 	spin_lock_irqsave(&dev->qpt_lock, flags);
 
-	if (ibp->qp0 == qp) {
+	if (rcu_dereference_protected(ibp->qp0,
+			lockdep_is_held(&dev->qpt_lock)) == qp) {
 		atomic_dec(&qp->refcount);
 		rcu_assign_pointer(ibp->qp0, NULL);
-	} else if (ibp->qp1 == qp) {
+	} else if (rcu_dereference_protected(ibp->qp1,
+			lockdep_is_held(&dev->qpt_lock)) == qp) {
 		atomic_dec(&qp->refcount);
 		rcu_assign_pointer(ibp->qp1, NULL);
 	} else {
-		struct qib_qp *q, **qpp;
+		struct qib_qp *q;
+		struct qib_qp __rcu **qpp;
 
 		qpp = &dev->qp_table[n];
-		for (; (q = *qpp) != NULL; qpp = &q->next)
+		q = rcu_dereference_protected(*qpp,
+			lockdep_is_held(&dev->qpt_lock));
+		for (; q; qpp = &q->next) {
 			if (q == qp) {
 				atomic_dec(&qp->refcount);
-				rcu_assign_pointer(*qpp, qp->next);
-				qp->next = NULL;
+				*qpp = qp->next;
+				rcu_assign_pointer(qp->next, NULL);
+				q = rcu_dereference_protected(*qpp,
+					lockdep_is_held(&dev->qpt_lock));
 				break;
 			}
+			q = rcu_dereference_protected(*qpp,
+				lockdep_is_held(&dev->qpt_lock));
+		}
 	}
 
 	spin_unlock_irqrestore(&dev->qpt_lock, flags);
@@ -302,10 +312,12 @@ unsigned qib_free_all_qps(struct qib_devdata *dd)
 
 	spin_lock_irqsave(&dev->qpt_lock, flags);
 	for (n = 0; n < dev->qp_table_size; n++) {
-		qp = dev->qp_table[n];
+		qp = rcu_dereference_protected(dev->qp_table[n],
+			lockdep_is_held(&dev->qpt_lock));
 		rcu_assign_pointer(dev->qp_table[n], NULL);
 
-		for (; qp; qp = qp->next)
+		for (; qp; qp = rcu_dereference_protected(qp->next,
+					lockdep_is_held(&dev->qpt_lock)))
 			qp_inuse++;
 	}
 	spin_unlock_irqrestore(&dev->qpt_lock, flags);
@@ -337,7 +349,8 @@ struct qib_qp *qib_lookup_qpn(struct qib_ibport *ibp, u32 qpn)
 		unsigned n = qpn_hash(dev, qpn);
 
 		rcu_read_lock();
-		for (qp = dev->qp_table[n]; rcu_dereference(qp); qp = qp->next)
+		for (qp = rcu_dereference(dev->qp_table[n]); qp;
+			qp = rcu_dereference(qp->next))
 			if (qp->ibqp.qp_num == qpn)
 				break;
 	}
@@ -406,18 +419,9 @@ static void clear_mr_refs(struct qib_qp *qp, int clr_sends)
 	unsigned n;
 
 	if (test_and_clear_bit(QIB_R_REWIND_SGE, &qp->r_aflags))
-		while (qp->s_rdma_read_sge.num_sge) {
-			atomic_dec(&qp->s_rdma_read_sge.sge.mr->refcount);
-			if (--qp->s_rdma_read_sge.num_sge)
-				qp->s_rdma_read_sge.sge =
-					*qp->s_rdma_read_sge.sg_list++;
-		}
+		qib_put_ss(&qp->s_rdma_read_sge);
 
-	while (qp->r_sge.num_sge) {
-		atomic_dec(&qp->r_sge.sge.mr->refcount);
-		if (--qp->r_sge.num_sge)
-			qp->r_sge.sge = *qp->r_sge.sg_list++;
-	}
+	qib_put_ss(&qp->r_sge);
 
 	if (clr_sends) {
 		while (qp->s_last != qp->s_head) {
@@ -427,7 +431,7 @@ static void clear_mr_refs(struct qib_qp *qp, int clr_sends)
 			for (i = 0; i < wqe->wr.num_sge; i++) {
 				struct qib_sge *sge = &wqe->sg_list[i];
 
-				atomic_dec(&sge->mr->refcount);
+				qib_put_mr(sge->mr);
 			}
 			if (qp->ibqp.qp_type == IB_QPT_UD ||
 			    qp->ibqp.qp_type == IB_QPT_SMI ||
@@ -437,7 +441,7 @@ static void clear_mr_refs(struct qib_qp *qp, int clr_sends)
 				qp->s_last = 0;
 		}
 		if (qp->s_rdma_mr) {
-			atomic_dec(&qp->s_rdma_mr->refcount);
+			qib_put_mr(qp->s_rdma_mr);
 			qp->s_rdma_mr = NULL;
 		}
 	}
@@ -450,7 +454,7 @@ static void clear_mr_refs(struct qib_qp *qp, int clr_sends)
 
 		if (e->opcode == IB_OPCODE_RC_RDMA_READ_REQUEST &&
 		    e->rdma_sge.mr) {
-			atomic_dec(&e->rdma_sge.mr->refcount);
+			qib_put_mr(e->rdma_sge.mr);
 			e->rdma_sge.mr = NULL;
 		}
 	}
@@ -495,7 +499,7 @@ int qib_error_qp(struct qib_qp *qp, enum ib_wc_status err)
 	if (!(qp->s_flags & QIB_S_BUSY)) {
 		qp->s_hdrwords = 0;
 		if (qp->s_rdma_mr) {
-			atomic_dec(&qp->s_rdma_mr->refcount);
+			qib_put_mr(qp->s_rdma_mr);
 			qp->s_rdma_mr = NULL;
 		}
 		if (qp->s_tx) {
diff --git a/drivers/infiniband/hw/qib/qib_rc.c b/drivers/infiniband/hw/qib/qib_rc.c
index b641416148e..3ab341320ea 100644
--- a/drivers/infiniband/hw/qib/qib_rc.c
+++ b/drivers/infiniband/hw/qib/qib_rc.c
@@ -95,7 +95,7 @@ static int qib_make_rc_ack(struct qib_ibdev *dev, struct qib_qp *qp,
 	case OP(RDMA_READ_RESPONSE_ONLY):
 		e = &qp->s_ack_queue[qp->s_tail_ack_queue];
 		if (e->rdma_sge.mr) {
-			atomic_dec(&e->rdma_sge.mr->refcount);
+			qib_put_mr(e->rdma_sge.mr);
 			e->rdma_sge.mr = NULL;
 		}
 		/* FALLTHROUGH */
@@ -133,7 +133,7 @@ static int qib_make_rc_ack(struct qib_ibdev *dev, struct qib_qp *qp,
 			/* Copy SGE state in case we need to resend */
 			qp->s_rdma_mr = e->rdma_sge.mr;
 			if (qp->s_rdma_mr)
-				atomic_inc(&qp->s_rdma_mr->refcount);
+				qib_get_mr(qp->s_rdma_mr);
 			qp->s_ack_rdma_sge.sge = e->rdma_sge;
 			qp->s_ack_rdma_sge.num_sge = 1;
 			qp->s_cur_sge = &qp->s_ack_rdma_sge;
@@ -172,7 +172,7 @@ static int qib_make_rc_ack(struct qib_ibdev *dev, struct qib_qp *qp,
 		qp->s_cur_sge = &qp->s_ack_rdma_sge;
 		qp->s_rdma_mr = qp->s_ack_rdma_sge.sge.mr;
 		if (qp->s_rdma_mr)
-			atomic_inc(&qp->s_rdma_mr->refcount);
+			qib_get_mr(qp->s_rdma_mr);
 		len = qp->s_ack_rdma_sge.sge.sge_length;
 		if (len > pmtu)
 			len = pmtu;
@@ -1012,7 +1012,7 @@ void qib_rc_send_complete(struct qib_qp *qp, struct qib_ib_header *hdr)
 		for (i = 0; i < wqe->wr.num_sge; i++) {
 			struct qib_sge *sge = &wqe->sg_list[i];
 
-			atomic_dec(&sge->mr->refcount);
+			qib_put_mr(sge->mr);
 		}
 		/* Post a send completion queue entry if requested. */
 		if (!(qp->s_flags & QIB_S_SIGNAL_REQ_WR) ||
@@ -1068,7 +1068,7 @@ static struct qib_swqe *do_rc_completion(struct qib_qp *qp,
 		for (i = 0; i < wqe->wr.num_sge; i++) {
 			struct qib_sge *sge = &wqe->sg_list[i];
 
-			atomic_dec(&sge->mr->refcount);
+			qib_put_mr(sge->mr);
 		}
 		/* Post a send completion queue entry if requested. */
 		if (!(qp->s_flags & QIB_S_SIGNAL_REQ_WR) ||
@@ -1730,7 +1730,7 @@ static int qib_rc_rcv_error(struct qib_other_headers *ohdr,
 		if (unlikely(offset + len != e->rdma_sge.sge_length))
 			goto unlock_done;
 		if (e->rdma_sge.mr) {
-			atomic_dec(&e->rdma_sge.mr->refcount);
+			qib_put_mr(e->rdma_sge.mr);
 			e->rdma_sge.mr = NULL;
 		}
 		if (len != 0) {
@@ -2024,11 +2024,7 @@ send_last:
 		if (unlikely(wc.byte_len > qp->r_len))
 			goto nack_inv;
 		qib_copy_sge(&qp->r_sge, data, tlen, 1);
-		while (qp->r_sge.num_sge) {
-			atomic_dec(&qp->r_sge.sge.mr->refcount);
-			if (--qp->r_sge.num_sge)
-				qp->r_sge.sge = *qp->r_sge.sg_list++;
-		}
+		qib_put_ss(&qp->r_sge);
 		qp->r_msn++;
 		if (!test_and_clear_bit(QIB_R_WRID_VALID, &qp->r_aflags))
 			break;
@@ -2116,7 +2112,7 @@ send_last:
 		}
 		e = &qp->s_ack_queue[qp->r_head_ack_queue];
 		if (e->opcode == OP(RDMA_READ_REQUEST) && e->rdma_sge.mr) {
-			atomic_dec(&e->rdma_sge.mr->refcount);
+			qib_put_mr(e->rdma_sge.mr);
 			e->rdma_sge.mr = NULL;
 		}
 		reth = &ohdr->u.rc.reth;
@@ -2188,7 +2184,7 @@ send_last:
 		}
 		e = &qp->s_ack_queue[qp->r_head_ack_queue];
 		if (e->opcode == OP(RDMA_READ_REQUEST) && e->rdma_sge.mr) {
-			atomic_dec(&e->rdma_sge.mr->refcount);
+			qib_put_mr(e->rdma_sge.mr);
 			e->rdma_sge.mr = NULL;
 		}
 		ateth = &ohdr->u.atomic_eth;
@@ -2210,7 +2206,7 @@ send_last:
 			(u64) cmpxchg((u64 *) qp->r_sge.sge.vaddr,
 				      be64_to_cpu(ateth->compare_data),
 				      sdata);
-		atomic_dec(&qp->r_sge.sge.mr->refcount);
+		qib_put_mr(qp->r_sge.sge.mr);
 		qp->r_sge.num_sge = 0;
 		e->opcode = opcode;
 		e->sent = 0;
diff --git a/drivers/infiniband/hw/qib/qib_ruc.c b/drivers/infiniband/hw/qib/qib_ruc.c
index c0ee7e095d8..357b6cfcd46 100644
--- a/drivers/infiniband/hw/qib/qib_ruc.c
+++ b/drivers/infiniband/hw/qib/qib_ruc.c
@@ -110,7 +110,7 @@ bad_lkey:
 	while (j) {
 		struct qib_sge *sge = --j ? &ss->sg_list[j - 1] : &ss->sge;
 
-		atomic_dec(&sge->mr->refcount);
+		qib_put_mr(sge->mr);
 	}
 	ss->num_sge = 0;
 	memset(&wc, 0, sizeof(wc));
@@ -501,7 +501,7 @@ again:
 			(u64) atomic64_add_return(sdata, maddr) - sdata :
 			(u64) cmpxchg((u64 *) qp->r_sge.sge.vaddr,
 				      sdata, wqe->wr.wr.atomic.swap);
-		atomic_dec(&qp->r_sge.sge.mr->refcount);
+		qib_put_mr(qp->r_sge.sge.mr);
 		qp->r_sge.num_sge = 0;
 		goto send_comp;
 
@@ -525,7 +525,7 @@ again:
 		sge->sge_length -= len;
 		if (sge->sge_length == 0) {
 			if (!release)
-				atomic_dec(&sge->mr->refcount);
+				qib_put_mr(sge->mr);
 			if (--sqp->s_sge.num_sge)
 				*sge = *sqp->s_sge.sg_list++;
 		} else if (sge->length == 0 && sge->mr->lkey) {
@@ -542,11 +542,7 @@ again:
 		sqp->s_len -= len;
 	}
 	if (release)
-		while (qp->r_sge.num_sge) {
-			atomic_dec(&qp->r_sge.sge.mr->refcount);
-			if (--qp->r_sge.num_sge)
-				qp->r_sge.sge = *qp->r_sge.sg_list++;
-		}
+		qib_put_ss(&qp->r_sge);
 
 	if (!test_and_clear_bit(QIB_R_WRID_VALID, &qp->r_aflags))
 		goto send_comp;
@@ -782,7 +778,7 @@ void qib_send_complete(struct qib_qp *qp, struct qib_swqe *wqe,
 	for (i = 0; i < wqe->wr.num_sge; i++) {
 		struct qib_sge *sge = &wqe->sg_list[i];
 
-		atomic_dec(&sge->mr->refcount);
+		qib_put_mr(sge->mr);
 	}
 	if (qp->ibqp.qp_type == IB_QPT_UD ||
 	    qp->ibqp.qp_type == IB_QPT_SMI ||
diff --git a/drivers/infiniband/hw/qib/qib_sd7220.c b/drivers/infiniband/hw/qib/qib_sd7220.c
index ac065dd6b69..a322d5171a2 100644
--- a/drivers/infiniband/hw/qib/qib_sd7220.c
+++ b/drivers/infiniband/hw/qib/qib_sd7220.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -342,15 +342,17 @@ static void qib_sd_trimdone_monitor(struct qib_devdata *dd,
 		ret = qib_sd7220_reg_mod(dd, IB_7220_SERDES,
 			IB_CTRL2(chn), 0, 0);
 		if (ret < 0)
-			qib_dev_err(dd, "Failed checking TRIMDONE, chn %d"
-				    " (%s)\n", chn, where);
+			qib_dev_err(dd,
+				"Failed checking TRIMDONE, chn %d (%s)\n",
+				chn, where);
 
 		if (!(ret & 0x10)) {
 			int probe;
 
 			baduns |= (1 << chn);
-			qib_dev_err(dd, "TRIMDONE cleared on chn %d (%02X)."
-				" (%s)\n", chn, ret, where);
+			qib_dev_err(dd,
+				"TRIMDONE cleared on chn %d (%02X). (%s)\n",
+				chn, ret, where);
 			probe = qib_sd7220_reg_mod(dd, IB_7220_SERDES,
 				IB_PGUDP(0), 0, 0);
 			qib_dev_err(dd, "probe is %d (%02X)\n",
@@ -375,8 +377,8 @@ static void qib_sd_trimdone_monitor(struct qib_devdata *dd,
 			ret = qib_sd7220_reg_mod(dd, IB_7220_SERDES,
 				IB_CTRL2(chn), 0x10, 0x10);
 			if (ret < 0)
-				qib_dev_err(dd, "Failed re-setting "
-					"TRIMDONE, chn %d (%s)\n",
+				qib_dev_err(dd,
+					"Failed re-setting TRIMDONE, chn %d (%s)\n",
 					chn, where);
 		}
 	}
@@ -1144,10 +1146,10 @@ static int ibsd_mod_allchnls(struct qib_devdata *dd, int loc, int val,
 			if (ret < 0) {
 				int sloc = loc >> EPB_ADDR_SHF;
 
-				qib_dev_err(dd, "pre-read failed: elt %d,"
-					    " addr 0x%X, chnl %d\n",
-					    (sloc & 0xF),
-					    (sloc >> 9) & 0x3f, chnl);
+				qib_dev_err(dd,
+					"pre-read failed: elt %d, addr 0x%X, chnl %d\n",
+					(sloc & 0xF),
+					(sloc >> 9) & 0x3f, chnl);
 				return ret;
 			}
 			val = (ret & ~mask) | (val & mask);
@@ -1157,9 +1159,9 @@ static int ibsd_mod_allchnls(struct qib_devdata *dd, int loc, int val,
 		if (ret < 0) {
 			int sloc = loc >> EPB_ADDR_SHF;
 
-			qib_dev_err(dd, "Global WR failed: elt %d,"
-				    " addr 0x%X, val %02X\n",
-				    (sloc & 0xF), (sloc >> 9) & 0x3f, val);
+			qib_dev_err(dd,
+				"Global WR failed: elt %d, addr 0x%X, val %02X\n",
+				(sloc & 0xF), (sloc >> 9) & 0x3f, val);
 		}
 		return ret;
 	}
@@ -1173,11 +1175,10 @@ static int ibsd_mod_allchnls(struct qib_devdata *dd, int loc, int val,
 		if (ret < 0) {
 			int sloc = loc >> EPB_ADDR_SHF;
 
-			qib_dev_err(dd, "Write failed: elt %d,"
-				    " addr 0x%X, chnl %d, val 0x%02X,"
-				    " mask 0x%02X\n",
-				    (sloc & 0xF), (sloc >> 9) & 0x3f, chnl,
-				    val & 0xFF, mask & 0xFF);
+			qib_dev_err(dd,
+				"Write failed: elt %d, addr 0x%X, chnl %d, val 0x%02X, mask 0x%02X\n",
+				(sloc & 0xF), (sloc >> 9) & 0x3f, chnl,
+				val & 0xFF, mask & 0xFF);
 			break;
 		}
 	}
diff --git a/drivers/infiniband/hw/qib/qib_sdma.c b/drivers/infiniband/hw/qib/qib_sdma.c
index 12a9604310d..3fc51443121 100644
--- a/drivers/infiniband/hw/qib/qib_sdma.c
+++ b/drivers/infiniband/hw/qib/qib_sdma.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2007, 2008, 2009, 2010 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2007 - 2012 QLogic Corporation. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -276,8 +277,8 @@ static int alloc_sdma(struct qib_pportdata *ppd)
 		GFP_KERNEL);
 
 	if (!ppd->sdma_descq) {
-		qib_dev_err(ppd->dd, "failed to allocate SendDMA descriptor "
-			    "FIFO memory\n");
+		qib_dev_err(ppd->dd,
+			"failed to allocate SendDMA descriptor FIFO memory\n");
 		goto bail;
 	}
 
@@ -285,8 +286,8 @@ static int alloc_sdma(struct qib_pportdata *ppd)
 	ppd->sdma_head_dma = dma_alloc_coherent(&ppd->dd->pcidev->dev,
 		PAGE_SIZE, &ppd->sdma_head_phys, GFP_KERNEL);
 	if (!ppd->sdma_head_dma) {
-		qib_dev_err(ppd->dd, "failed to allocate SendDMA "
-			    "head memory\n");
+		qib_dev_err(ppd->dd,
+			"failed to allocate SendDMA head memory\n");
 		goto cleanup_descq;
 	}
 	ppd->sdma_head_dma[0] = 0;
diff --git a/drivers/infiniband/hw/qib/qib_sysfs.c b/drivers/infiniband/hw/qib/qib_sysfs.c
index dd9cd49d097..034cc821de5 100644
--- a/drivers/infiniband/hw/qib/qib_sysfs.c
+++ b/drivers/infiniband/hw/qib/qib_sysfs.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -33,41 +34,7 @@
 #include <linux/ctype.h>
 
 #include "qib.h"
-
-/**
- * qib_parse_ushort - parse an unsigned short value in an arbitrary base
- * @str: the string containing the number
- * @valp: where to put the result
- *
- * Returns the number of bytes consumed, or negative value on error.
- */
-static int qib_parse_ushort(const char *str, unsigned short *valp)
-{
-	unsigned long val;
-	char *end;
-	int ret;
-
-	if (!isdigit(str[0])) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	val = simple_strtoul(str, &end, 0);
-
-	if (val > 0xffff) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	*valp = val;
-
-	ret = end + 1 - str;
-	if (ret == 0)
-		ret = -EINVAL;
-
-bail:
-	return ret;
-}
+#include "qib_mad.h"
 
 /* start of per-port functions */
 /*
@@ -90,7 +57,11 @@ static ssize_t store_hrtbt_enb(struct qib_pportdata *ppd, const char *buf,
 	int ret;
 	u16 val;
 
-	ret = qib_parse_ushort(buf, &val);
+	ret = kstrtou16(buf, 0, &val);
+	if (ret) {
+		qib_dev_err(dd, "attempt to set invalid Heartbeat enable\n");
+		return ret;
+	}
 
 	/*
 	 * Set the "intentional" heartbeat enable per either of
@@ -99,10 +70,7 @@ static ssize_t store_hrtbt_enb(struct qib_pportdata *ppd, const char *buf,
 	 * because entering loopback mode overrides it and automatically
 	 * disables heartbeat.
 	 */
-	if (ret >= 0)
-		ret = dd->f_set_ib_cfg(ppd, QIB_IB_CFG_HRTBT, val);
-	if (ret < 0)
-		qib_dev_err(dd, "attempt to set invalid Heartbeat enable\n");
+	ret = dd->f_set_ib_cfg(ppd, QIB_IB_CFG_HRTBT, val);
 	return ret < 0 ? ret : count;
 }
 
@@ -126,12 +94,14 @@ static ssize_t store_led_override(struct qib_pportdata *ppd, const char *buf,
 	int ret;
 	u16 val;
 
-	ret = qib_parse_ushort(buf, &val);
-	if (ret > 0)
-		qib_set_led_override(ppd, val);
-	else
+	ret = kstrtou16(buf, 0, &val);
+	if (ret) {
 		qib_dev_err(dd, "attempt to set invalid LED override\n");
-	return ret < 0 ? ret : count;
+		return ret;
+	}
+
+	qib_set_led_override(ppd, val);
+	return count;
 }
 
 static ssize_t show_status(struct qib_pportdata *ppd, char *buf)
@@ -231,6 +201,98 @@ static struct attribute *port_default_attributes[] = {
 	NULL
 };
 
+/*
+ * Start of per-port congestion control structures and support code
+ */
+
+/*
+ * Congestion control table size followed by table entries
+ */
+static ssize_t read_cc_table_bin(struct file *filp, struct kobject *kobj,
+		struct bin_attribute *bin_attr,
+		char *buf, loff_t pos, size_t count)
+{
+	int ret;
+	struct qib_pportdata *ppd =
+		container_of(kobj, struct qib_pportdata, pport_cc_kobj);
+
+	if (!qib_cc_table_size || !ppd->ccti_entries_shadow)
+		return -EINVAL;
+
+	ret = ppd->total_cct_entry * sizeof(struct ib_cc_table_entry_shadow)
+		 + sizeof(__be16);
+
+	if (pos > ret)
+		return -EINVAL;
+
+	if (count > ret - pos)
+		count = ret - pos;
+
+	if (!count)
+		return count;
+
+	spin_lock(&ppd->cc_shadow_lock);
+	memcpy(buf, ppd->ccti_entries_shadow, count);
+	spin_unlock(&ppd->cc_shadow_lock);
+
+	return count;
+}
+
+static void qib_port_release(struct kobject *kobj)
+{
+	/* nothing to do since memory is freed by qib_free_devdata() */
+}
+
+static struct kobj_type qib_port_cc_ktype = {
+	.release = qib_port_release,
+};
+
+static struct bin_attribute cc_table_bin_attr = {
+	.attr = {.name = "cc_table_bin", .mode = 0444},
+	.read = read_cc_table_bin,
+	.size = PAGE_SIZE,
+};
+
+/*
+ * Congestion settings: port control, control map and an array of 16
+ * entries for the congestion entries - increase, timer, event log
+ * trigger threshold and the minimum injection rate delay.
+ */
+static ssize_t read_cc_setting_bin(struct file *filp, struct kobject *kobj,
+		struct bin_attribute *bin_attr,
+		char *buf, loff_t pos, size_t count)
+{
+	int ret;
+	struct qib_pportdata *ppd =
+		container_of(kobj, struct qib_pportdata, pport_cc_kobj);
+
+	if (!qib_cc_table_size || !ppd->congestion_entries_shadow)
+		return -EINVAL;
+
+	ret = sizeof(struct ib_cc_congestion_setting_attr_shadow);
+
+	if (pos > ret)
+		return -EINVAL;
+	if (count > ret - pos)
+		count = ret - pos;
+
+	if (!count)
+		return count;
+
+	spin_lock(&ppd->cc_shadow_lock);
+	memcpy(buf, ppd->congestion_entries_shadow, count);
+	spin_unlock(&ppd->cc_shadow_lock);
+
+	return count;
+}
+
+static struct bin_attribute cc_setting_bin_attr = {
+	.attr = {.name = "cc_settings_bin", .mode = 0444},
+	.read = read_cc_setting_bin,
+	.size = PAGE_SIZE,
+};
+
+
 static ssize_t qib_portattr_show(struct kobject *kobj,
 	struct attribute *attr, char *buf)
 {
@@ -253,10 +315,6 @@ static ssize_t qib_portattr_store(struct kobject *kobj,
 	return pattr->store(ppd, buf, len);
 }
 
-static void qib_port_release(struct kobject *kobj)
-{
-	/* nothing to do since memory is freed by qib_free_devdata() */
-}
 
 static const struct sysfs_ops qib_port_ops = {
 	.show = qib_portattr_show,
@@ -411,12 +469,12 @@ static ssize_t diagc_attr_store(struct kobject *kobj, struct attribute *attr,
 	struct qib_pportdata *ppd =
 		container_of(kobj, struct qib_pportdata, diagc_kobj);
 	struct qib_ibport *qibp = &ppd->ibport_data;
-	char *endp;
-	long val = simple_strtol(buf, &endp, 0);
-
-	if (val < 0 || endp == buf)
-		return -EINVAL;
+	u32 val;
+	int ret;
 
+	ret = kstrtou32(buf, 0, &val);
+	if (ret)
+		return ret;
 	*(u32 *)((char *) qibp + dattr->counter) = val;
 	return size;
 }
@@ -649,8 +707,9 @@ int qib_create_port_files(struct ib_device *ibdev, u8 port_num,
 	int ret;
 
 	if (!port_num || port_num > dd->num_pports) {
-		qib_dev_err(dd, "Skipping infiniband class with "
-			    "invalid port %u\n", port_num);
+		qib_dev_err(dd,
+			"Skipping infiniband class with invalid port %u\n",
+			port_num);
 		ret = -ENODEV;
 		goto bail;
 	}
@@ -659,8 +718,9 @@ int qib_create_port_files(struct ib_device *ibdev, u8 port_num,
 	ret = kobject_init_and_add(&ppd->pport_kobj, &qib_port_ktype, kobj,
 				   "linkcontrol");
 	if (ret) {
-		qib_dev_err(dd, "Skipping linkcontrol sysfs info, "
-			    "(err %d) port %u\n", ret, port_num);
+		qib_dev_err(dd,
+			"Skipping linkcontrol sysfs info, (err %d) port %u\n",
+			ret, port_num);
 		goto bail;
 	}
 	kobject_uevent(&ppd->pport_kobj, KOBJ_ADD);
@@ -668,26 +728,70 @@ int qib_create_port_files(struct ib_device *ibdev, u8 port_num,
 	ret = kobject_init_and_add(&ppd->sl2vl_kobj, &qib_sl2vl_ktype, kobj,
 				   "sl2vl");
 	if (ret) {
-		qib_dev_err(dd, "Skipping sl2vl sysfs info, "
-			    "(err %d) port %u\n", ret, port_num);
-		goto bail_sl;
+		qib_dev_err(dd,
+			"Skipping sl2vl sysfs info, (err %d) port %u\n",
+			ret, port_num);
+		goto bail_link;
 	}
 	kobject_uevent(&ppd->sl2vl_kobj, KOBJ_ADD);
 
 	ret = kobject_init_and_add(&ppd->diagc_kobj, &qib_diagc_ktype, kobj,
 				   "diag_counters");
 	if (ret) {
-		qib_dev_err(dd, "Skipping diag_counters sysfs info, "
-			    "(err %d) port %u\n", ret, port_num);
-		goto bail_diagc;
+		qib_dev_err(dd,
+			"Skipping diag_counters sysfs info, (err %d) port %u\n",
+			ret, port_num);
+		goto bail_sl;
 	}
 	kobject_uevent(&ppd->diagc_kobj, KOBJ_ADD);
 
+	if (!qib_cc_table_size || !ppd->congestion_entries_shadow)
+		return 0;
+
+	ret = kobject_init_and_add(&ppd->pport_cc_kobj, &qib_port_cc_ktype,
+				kobj, "CCMgtA");
+	if (ret) {
+		qib_dev_err(dd,
+		 "Skipping Congestion Control sysfs info, (err %d) port %u\n",
+		 ret, port_num);
+		goto bail_diagc;
+	}
+
+	kobject_uevent(&ppd->pport_cc_kobj, KOBJ_ADD);
+
+	ret = sysfs_create_bin_file(&ppd->pport_cc_kobj,
+				&cc_setting_bin_attr);
+	if (ret) {
+		qib_dev_err(dd,
+		 "Skipping Congestion Control setting sysfs info, (err %d) port %u\n",
+		 ret, port_num);
+		goto bail_cc;
+	}
+
+	ret = sysfs_create_bin_file(&ppd->pport_cc_kobj,
+				&cc_table_bin_attr);
+	if (ret) {
+		qib_dev_err(dd,
+		 "Skipping Congestion Control table sysfs info, (err %d) port %u\n",
+		 ret, port_num);
+		goto bail_cc_entry_bin;
+	}
+
+	qib_devinfo(dd->pcidev,
+		"IB%u: Congestion Control Agent enabled for port %d\n",
+		dd->unit, port_num);
+
 	return 0;
 
+bail_cc_entry_bin:
+	sysfs_remove_bin_file(&ppd->pport_cc_kobj, &cc_setting_bin_attr);
+bail_cc:
+	kobject_put(&ppd->pport_cc_kobj);
 bail_diagc:
-	kobject_put(&ppd->sl2vl_kobj);
+	kobject_put(&ppd->diagc_kobj);
 bail_sl:
+	kobject_put(&ppd->sl2vl_kobj);
+bail_link:
 	kobject_put(&ppd->pport_kobj);
 bail:
 	return ret;
@@ -720,7 +824,15 @@ void qib_verbs_unregister_sysfs(struct qib_devdata *dd)
 
 	for (i = 0; i < dd->num_pports; i++) {
 		ppd = &dd->pport[i];
-		kobject_put(&ppd->pport_kobj);
+		if (qib_cc_table_size &&
+			ppd->congestion_entries_shadow) {
+			sysfs_remove_bin_file(&ppd->pport_cc_kobj,
+				&cc_setting_bin_attr);
+			sysfs_remove_bin_file(&ppd->pport_cc_kobj,
+				&cc_table_bin_attr);
+			kobject_put(&ppd->pport_cc_kobj);
+		}
 		kobject_put(&ppd->sl2vl_kobj);
+		kobject_put(&ppd->pport_kobj);
 	}
 }
diff --git a/drivers/infiniband/hw/qib/qib_twsi.c b/drivers/infiniband/hw/qib/qib_twsi.c
index ddde72e11ed..647f7beb1b0 100644
--- a/drivers/infiniband/hw/qib/qib_twsi.c
+++ b/drivers/infiniband/hw/qib/qib_twsi.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -449,8 +450,9 @@ int qib_twsi_blk_wr(struct qib_devdata *dd, int dev, int addr,
 				goto failed_write;
 			ret = qib_twsi_wr(dd, addr, 0);
 			if (ret) {
-				qib_dev_err(dd, "Failed to write interface"
-					    " write addr %02X\n", addr);
+				qib_dev_err(dd,
+					"Failed to write interface write addr %02X\n",
+					addr);
 				goto failed_write;
 			}
 		}
diff --git a/drivers/infiniband/hw/qib/qib_uc.c b/drivers/infiniband/hw/qib/qib_uc.c
index ce7387ff5d9..aa3a8035bb6 100644
--- a/drivers/infiniband/hw/qib/qib_uc.c
+++ b/drivers/infiniband/hw/qib/qib_uc.c
@@ -281,11 +281,7 @@ inv:
 			set_bit(QIB_R_REWIND_SGE, &qp->r_aflags);
 			qp->r_sge.num_sge = 0;
 		} else
-			while (qp->r_sge.num_sge) {
-				atomic_dec(&qp->r_sge.sge.mr->refcount);
-				if (--qp->r_sge.num_sge)
-					qp->r_sge.sge = *qp->r_sge.sg_list++;
-			}
+			qib_put_ss(&qp->r_sge);
 		qp->r_state = OP(SEND_LAST);
 		switch (opcode) {
 		case OP(SEND_FIRST):
@@ -403,14 +399,9 @@ send_last:
 		if (unlikely(wc.byte_len > qp->r_len))
 			goto rewind;
 		wc.opcode = IB_WC_RECV;
-last_imm:
 		qib_copy_sge(&qp->r_sge, data, tlen, 0);
-		while (qp->s_rdma_read_sge.num_sge) {
-			atomic_dec(&qp->s_rdma_read_sge.sge.mr->refcount);
-			if (--qp->s_rdma_read_sge.num_sge)
-				qp->s_rdma_read_sge.sge =
-					*qp->s_rdma_read_sge.sg_list++;
-		}
+		qib_put_ss(&qp->s_rdma_read_sge);
+last_imm:
 		wc.wr_id = qp->r_wr_id;
 		wc.status = IB_WC_SUCCESS;
 		wc.qp = &qp->ibqp;
@@ -493,13 +484,7 @@ rdma_last_imm:
 		if (unlikely(tlen + qp->r_rcv_len != qp->r_len))
 			goto drop;
 		if (test_and_clear_bit(QIB_R_REWIND_SGE, &qp->r_aflags))
-			while (qp->s_rdma_read_sge.num_sge) {
-				atomic_dec(&qp->s_rdma_read_sge.sge.mr->
-					   refcount);
-				if (--qp->s_rdma_read_sge.num_sge)
-					qp->s_rdma_read_sge.sge =
-						*qp->s_rdma_read_sge.sg_list++;
-			}
+			qib_put_ss(&qp->s_rdma_read_sge);
 		else {
 			ret = qib_get_rwqe(qp, 1);
 			if (ret < 0)
@@ -509,6 +494,8 @@ rdma_last_imm:
 		}
 		wc.byte_len = qp->r_len;
 		wc.opcode = IB_WC_RECV_RDMA_WITH_IMM;
+		qib_copy_sge(&qp->r_sge, data, tlen, 1);
+		qib_put_ss(&qp->r_sge);
 		goto last_imm;
 
 	case OP(RDMA_WRITE_LAST):
@@ -524,11 +511,7 @@ rdma_last:
 		if (unlikely(tlen + qp->r_rcv_len != qp->r_len))
 			goto drop;
 		qib_copy_sge(&qp->r_sge, data, tlen, 1);
-		while (qp->r_sge.num_sge) {
-			atomic_dec(&qp->r_sge.sge.mr->refcount);
-			if (--qp->r_sge.num_sge)
-				qp->r_sge.sge = *qp->r_sge.sg_list++;
-		}
+		qib_put_ss(&qp->r_sge);
 		break;
 
 	default:
diff --git a/drivers/infiniband/hw/qib/qib_ud.c b/drivers/infiniband/hw/qib/qib_ud.c
index a468bf2d446..d6c7fe7f88d 100644
--- a/drivers/infiniband/hw/qib/qib_ud.c
+++ b/drivers/infiniband/hw/qib/qib_ud.c
@@ -194,11 +194,7 @@ static void qib_ud_loopback(struct qib_qp *sqp, struct qib_swqe *swqe)
 		}
 		length -= len;
 	}
-	while (qp->r_sge.num_sge) {
-		atomic_dec(&qp->r_sge.sge.mr->refcount);
-		if (--qp->r_sge.num_sge)
-			qp->r_sge.sge = *qp->r_sge.sg_list++;
-	}
+	qib_put_ss(&qp->r_sge);
 	if (!test_and_clear_bit(QIB_R_WRID_VALID, &qp->r_aflags))
 		goto bail_unlock;
 	wc.wr_id = qp->r_wr_id;
@@ -556,11 +552,7 @@ void qib_ud_rcv(struct qib_ibport *ibp, struct qib_ib_header *hdr,
 	} else
 		qib_skip_sge(&qp->r_sge, sizeof(struct ib_grh), 1);
 	qib_copy_sge(&qp->r_sge, data, wc.byte_len - sizeof(struct ib_grh), 1);
-	while (qp->r_sge.num_sge) {
-		atomic_dec(&qp->r_sge.sge.mr->refcount);
-		if (--qp->r_sge.num_sge)
-			qp->r_sge.sge = *qp->r_sge.sg_list++;
-	}
+	qib_put_ss(&qp->r_sge);
 	if (!test_and_clear_bit(QIB_R_WRID_VALID, &qp->r_aflags))
 		return;
 	wc.wr_id = qp->r_wr_id;
diff --git a/drivers/infiniband/hw/qib/qib_verbs.c b/drivers/infiniband/hw/qib/qib_verbs.c
index 7b6c3bffa9d..fc9b205c241 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.c
+++ b/drivers/infiniband/hw/qib/qib_verbs.c
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -183,7 +183,7 @@ void qib_copy_sge(struct qib_sge_state *ss, void *data, u32 length, int release)
 		sge->sge_length -= len;
 		if (sge->sge_length == 0) {
 			if (release)
-				atomic_dec(&sge->mr->refcount);
+				qib_put_mr(sge->mr);
 			if (--ss->num_sge)
 				*sge = *ss->sg_list++;
 		} else if (sge->length == 0 && sge->mr->lkey) {
@@ -224,7 +224,7 @@ void qib_skip_sge(struct qib_sge_state *ss, u32 length, int release)
 		sge->sge_length -= len;
 		if (sge->sge_length == 0) {
 			if (release)
-				atomic_dec(&sge->mr->refcount);
+				qib_put_mr(sge->mr);
 			if (--ss->num_sge)
 				*sge = *ss->sg_list++;
 		} else if (sge->length == 0 && sge->mr->lkey) {
@@ -333,7 +333,8 @@ static void qib_copy_from_sge(void *data, struct qib_sge_state *ss, u32 length)
  * @qp: the QP to post on
  * @wr: the work request to send
  */
-static int qib_post_one_send(struct qib_qp *qp, struct ib_send_wr *wr)
+static int qib_post_one_send(struct qib_qp *qp, struct ib_send_wr *wr,
+	int *scheduled)
 {
 	struct qib_swqe *wqe;
 	u32 next;
@@ -435,11 +436,17 @@ bail_inval_free:
 	while (j) {
 		struct qib_sge *sge = &wqe->sg_list[--j];
 
-		atomic_dec(&sge->mr->refcount);
+		qib_put_mr(sge->mr);
 	}
 bail_inval:
 	ret = -EINVAL;
 bail:
+	if (!ret && !wr->next &&
+	 !qib_sdma_empty(
+	   dd_from_ibdev(qp->ibqp.device)->pport + qp->port_num - 1)) {
+		qib_schedule_send(qp);
+		*scheduled = 1;
+	}
 	spin_unlock_irqrestore(&qp->s_lock, flags);
 	return ret;
 }
@@ -457,9 +464,10 @@ static int qib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 {
 	struct qib_qp *qp = to_iqp(ibqp);
 	int err = 0;
+	int scheduled = 0;
 
 	for (; wr; wr = wr->next) {
-		err = qib_post_one_send(qp, wr);
+		err = qib_post_one_send(qp, wr, &scheduled);
 		if (err) {
 			*bad_wr = wr;
 			goto bail;
@@ -467,7 +475,8 @@ static int qib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 	}
 
 	/* Try to do the send work in the caller's context. */
-	qib_do_send(&qp->s_work);
+	if (!scheduled)
+		qib_do_send(&qp->s_work);
 
 bail:
 	return err;
@@ -978,7 +987,7 @@ void qib_put_txreq(struct qib_verbs_txreq *tx)
 	if (atomic_dec_and_test(&qp->refcount))
 		wake_up(&qp->wait);
 	if (tx->mr) {
-		atomic_dec(&tx->mr->refcount);
+		qib_put_mr(tx->mr);
 		tx->mr = NULL;
 	}
 	if (tx->txreq.flags & QIB_SDMA_TXREQ_F_FREEBUF) {
@@ -1336,7 +1345,7 @@ done:
 	}
 	qib_sendbuf_done(dd, pbufn);
 	if (qp->s_rdma_mr) {
-		atomic_dec(&qp->s_rdma_mr->refcount);
+		qib_put_mr(qp->s_rdma_mr);
 		qp->s_rdma_mr = NULL;
 	}
 	if (qp->s_wqe) {
@@ -1845,6 +1854,23 @@ bail:
 	return ret;
 }
 
+struct ib_ah *qib_create_qp0_ah(struct qib_ibport *ibp, u16 dlid)
+{
+	struct ib_ah_attr attr;
+	struct ib_ah *ah = ERR_PTR(-EINVAL);
+	struct qib_qp *qp0;
+
+	memset(&attr, 0, sizeof attr);
+	attr.dlid = dlid;
+	attr.port_num = ppd_from_ibp(ibp)->port;
+	rcu_read_lock();
+	qp0 = rcu_dereference(ibp->qp0);
+	if (qp0)
+		ah = ib_create_ah(qp0->ibqp.pd, &attr);
+	rcu_read_unlock();
+	return ah;
+}
+
 /**
  * qib_destroy_ah - destroy an address handle
  * @ibah: the AH to destroy
@@ -2060,13 +2086,15 @@ int qib_register_ib_device(struct qib_devdata *dd)
 	spin_lock_init(&dev->lk_table.lock);
 	dev->lk_table.max = 1 << ib_qib_lkey_table_size;
 	lk_tab_size = dev->lk_table.max * sizeof(*dev->lk_table.table);
-	dev->lk_table.table = (struct qib_mregion **)
+	dev->lk_table.table = (struct qib_mregion __rcu **)
 		__get_free_pages(GFP_KERNEL, get_order(lk_tab_size));
 	if (dev->lk_table.table == NULL) {
 		ret = -ENOMEM;
 		goto err_lk;
 	}
-	memset(dev->lk_table.table, 0, lk_tab_size);
+	RCU_INIT_POINTER(dev->dma_mr, NULL);
+	for (i = 0; i < dev->lk_table.max; i++)
+		RCU_INIT_POINTER(dev->lk_table.table[i], NULL);
 	INIT_LIST_HEAD(&dev->pending_mmaps);
 	spin_lock_init(&dev->pending_lock);
 	dev->mmap_offset = PAGE_SIZE;
@@ -2289,3 +2317,17 @@ void qib_unregister_ib_device(struct qib_devdata *dd)
 		   get_order(lk_tab_size));
 	kfree(dev->qp_table);
 }
+
+/*
+ * This must be called with s_lock held.
+ */
+void qib_schedule_send(struct qib_qp *qp)
+{
+	if (qib_send_ok(qp)) {
+		struct qib_ibport *ibp =
+			to_iport(qp->ibqp.device, qp->port_num);
+		struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+
+		queue_work(ppd->qib_wq, &qp->s_work);
+	}
+}
diff --git a/drivers/infiniband/hw/qib/qib_verbs.h b/drivers/infiniband/hw/qib/qib_verbs.h
index 48760602465..aff8b2c1788 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.h
+++ b/drivers/infiniband/hw/qib/qib_verbs.h
@@ -1,6 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009, 2010 QLogic Corporation.
- * All rights reserved.
+ * Copyright (c) 2012 Intel Corporation.  All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -41,6 +41,7 @@
 #include <linux/interrupt.h>
 #include <linux/kref.h>
 #include <linux/workqueue.h>
+#include <linux/completion.h>
 #include <rdma/ib_pack.h>
 #include <rdma/ib_user_verbs.h>
 
@@ -302,6 +303,9 @@ struct qib_mregion {
 	u32 max_segs;           /* number of qib_segs in all the arrays */
 	u32 mapsz;              /* size of the map array */
 	u8  page_shift;         /* 0 - non unform/non powerof2 sizes */
+	u8  lkey_published;     /* in global table */
+	struct completion comp; /* complete when refcount goes to zero */
+	struct rcu_head list;
 	atomic_t refcount;
 	struct qib_segarray *map[0];    /* the segments */
 };
@@ -416,7 +420,7 @@ struct qib_qp {
 	/* read mostly fields above and below */
 	struct ib_ah_attr remote_ah_attr;
 	struct ib_ah_attr alt_ah_attr;
-	struct qib_qp *next;            /* link list for QPN hash table */
+	struct qib_qp __rcu *next;            /* link list for QPN hash table */
 	struct qib_swqe *s_wq;  /* send work queue */
 	struct qib_mmap_info *ip;
 	struct qib_ib_header *s_hdr;     /* next packet header to send */
@@ -646,7 +650,7 @@ struct qib_lkey_table {
 	u32 next;               /* next unused index (speeds search) */
 	u32 gen;                /* generation count */
 	u32 max;                /* size of the table */
-	struct qib_mregion **table;
+	struct qib_mregion __rcu **table;
 };
 
 struct qib_opcode_stats {
@@ -655,8 +659,8 @@ struct qib_opcode_stats {
 };
 
 struct qib_ibport {
-	struct qib_qp *qp0;
-	struct qib_qp *qp1;
+	struct qib_qp __rcu *qp0;
+	struct qib_qp __rcu *qp1;
 	struct ib_mad_agent *send_agent;	/* agent for SMI (traps) */
 	struct qib_ah *sm_ah;
 	struct qib_ah *smi_ah;
@@ -723,12 +727,13 @@ struct qib_ibport {
 	struct qib_opcode_stats opstats[128];
 };
 
+
 struct qib_ibdev {
 	struct ib_device ibdev;
 	struct list_head pending_mmaps;
 	spinlock_t mmap_offset_lock; /* protect mmap_offset */
 	u32 mmap_offset;
-	struct qib_mregion *dma_mr;
+	struct qib_mregion __rcu *dma_mr;
 
 	/* QP numbers are shared by all IB ports */
 	struct qib_qpn_table qpn_table;
@@ -739,7 +744,7 @@ struct qib_ibdev {
 	struct list_head memwait;       /* list for wait kernel memory */
 	struct list_head txreq_free;
 	struct timer_list mem_timer;
-	struct qib_qp **qp_table;
+	struct qib_qp __rcu **qp_table;
 	struct qib_pio_header *pio_hdrs;
 	dma_addr_t pio_hdrs_phys;
 	/* list of QPs waiting for RNR timer */
@@ -832,11 +837,7 @@ extern struct workqueue_struct *qib_cq_wq;
 /*
  * This must be called with s_lock held.
  */
-static inline void qib_schedule_send(struct qib_qp *qp)
-{
-	if (qib_send_ok(qp))
-		queue_work(ib_wq, &qp->s_work);
-}
+void qib_schedule_send(struct qib_qp *qp);
 
 static inline int qib_pkey_ok(u16 pkey1, u16 pkey2)
 {
@@ -933,6 +934,8 @@ void qib_rc_rcv(struct qib_ctxtdata *rcd, struct qib_ib_header *hdr,
 
 int qib_check_ah(struct ib_device *ibdev, struct ib_ah_attr *ah_attr);
 
+struct ib_ah *qib_create_qp0_ah(struct qib_ibport *ibp, u16 dlid);
+
 void qib_rc_rnr_retry(unsigned long arg);
 
 void qib_rc_send_complete(struct qib_qp *qp, struct qib_ib_header *hdr);
@@ -944,9 +947,9 @@ int qib_post_ud_send(struct qib_qp *qp, struct ib_send_wr *wr);
 void qib_ud_rcv(struct qib_ibport *ibp, struct qib_ib_header *hdr,
 		int has_grh, void *data, u32 tlen, struct qib_qp *qp);
 
-int qib_alloc_lkey(struct qib_lkey_table *rkt, struct qib_mregion *mr);
+int qib_alloc_lkey(struct qib_mregion *mr, int dma_region);
 
-int qib_free_lkey(struct qib_ibdev *dev, struct qib_mregion *mr);
+void qib_free_lkey(struct qib_mregion *mr);
 
 int qib_lkey_ok(struct qib_lkey_table *rkt, struct qib_pd *pd,
 		struct qib_sge *isge, struct ib_sge *sge, int acc);
@@ -1014,6 +1017,29 @@ int qib_unmap_fmr(struct list_head *fmr_list);
 
 int qib_dealloc_fmr(struct ib_fmr *ibfmr);
 
+static inline void qib_get_mr(struct qib_mregion *mr)
+{
+	atomic_inc(&mr->refcount);
+}
+
+void mr_rcu_callback(struct rcu_head *list);
+
+static inline void qib_put_mr(struct qib_mregion *mr)
+{
+	if (unlikely(atomic_dec_and_test(&mr->refcount)))
+		call_rcu(&mr->list, mr_rcu_callback);
+}
+
+static inline void qib_put_ss(struct qib_sge_state *ss)
+{
+	while (ss->num_sge) {
+		qib_put_mr(ss->sge.mr);
+		if (--ss->num_sge)
+			ss->sge = *ss->sg_list++;
+	}
+}
+
+
 void qib_release_mmap_info(struct kref *ref);
 
 struct qib_mmap_info *qib_create_mmap_info(struct qib_ibdev *dev, u32 size,
diff --git a/drivers/infiniband/hw/qib/qib_wc_x86_64.c b/drivers/infiniband/hw/qib/qib_wc_x86_64.c
index 561b8bca406..1d7281c5a02 100644
--- a/drivers/infiniband/hw/qib/qib_wc_x86_64.c
+++ b/drivers/infiniband/hw/qib/qib_wc_x86_64.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2006, 2007, 2008, 2009 QLogic Corporation. All rights reserved.
+ * Copyright (c) 2012 Intel Corporation. All rights reserved.
+ * Copyright (c) 2006 - 2012 QLogic Corporation. All rights reserved.
  * Copyright (c) 2003, 2004, 2005, 2006 PathScale, Inc. All rights reserved.
  *
  * This software is available to you under a choice of one of two
@@ -102,10 +103,10 @@ int qib_enable_wc(struct qib_devdata *dd)
 		u64 atmp;
 		atmp = pioaddr & ~(piolen - 1);
 		if (atmp < addr || (atmp + piolen) > (addr + len)) {
-			qib_dev_err(dd, "No way to align address/size "
-				    "(%llx/%llx), no WC mtrr\n",
-				    (unsigned long long) atmp,
-				    (unsigned long long) piolen << 1);
+			qib_dev_err(dd,
+				"No way to align address/size (%llx/%llx), no WC mtrr\n",
+				(unsigned long long) atmp,
+				(unsigned long long) piolen << 1);
 			ret = -ENODEV;
 		} else {
 			pioaddr = atmp;
@@ -120,8 +121,7 @@ int qib_enable_wc(struct qib_devdata *dd)
 		if (cookie < 0) {
 			{
 				qib_devinfo(dd->pcidev,
-					 "mtrr_add()  WC for PIO bufs "
-					 "failed (%d)\n",
+					 "mtrr_add()  WC for PIO bufs failed (%d)\n",
 					 cookie);
 				ret = -EINVAL;
 			}
diff --git a/drivers/infiniband/ulp/ipoib/ipoib.h b/drivers/infiniband/ulp/ipoib/ipoib.h
index 86df632ea61..ca43901ed86 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib.h
+++ b/drivers/infiniband/ulp/ipoib/ipoib.h
@@ -92,6 +92,8 @@ enum {
 	IPOIB_STOP_REAPER	  = 7,
 	IPOIB_FLAG_ADMIN_CM	  = 9,
 	IPOIB_FLAG_UMCAST	  = 10,
+	IPOIB_STOP_NEIGH_GC	  = 11,
+	IPOIB_NEIGH_TBL_FLUSH	  = 12,
 
 	IPOIB_MAX_BACKOFF_SECONDS = 16,
 
@@ -260,6 +262,20 @@ struct ipoib_ethtool_st {
 	u16     max_coalesced_frames;
 };
 
+struct ipoib_neigh_hash {
+	struct ipoib_neigh __rcu      **buckets;
+	struct rcu_head			rcu;
+	u32				mask;
+	u32				size;
+};
+
+struct ipoib_neigh_table {
+	struct ipoib_neigh_hash __rcu  *htbl;
+	rwlock_t			rwlock;
+	atomic_t			entries;
+	struct completion		flushed;
+};
+
 /*
  * Device private locking: network stack tx_lock protects members used
  * in TX fast path, lock protects everything else.  lock nests inside
@@ -279,6 +295,8 @@ struct ipoib_dev_priv {
 	struct rb_root  path_tree;
 	struct list_head path_list;
 
+	struct ipoib_neigh_table ntbl;
+
 	struct ipoib_mcast *broadcast;
 	struct list_head multicast_list;
 	struct rb_root multicast_tree;
@@ -291,7 +309,7 @@ struct ipoib_dev_priv {
 	struct work_struct flush_heavy;
 	struct work_struct restart_task;
 	struct delayed_work ah_reap_task;
-
+	struct delayed_work neigh_reap_task;
 	struct ib_device *ca;
 	u8		  port;
 	u16		  pkey;
@@ -377,13 +395,16 @@ struct ipoib_neigh {
 #ifdef CONFIG_INFINIBAND_IPOIB_CM
 	struct ipoib_cm_tx *cm;
 #endif
-	union ib_gid	    dgid;
+	u8     daddr[INFINIBAND_ALEN];
 	struct sk_buff_head queue;
 
-	struct neighbour   *neighbour;
 	struct net_device *dev;
 
 	struct list_head    list;
+	struct ipoib_neigh __rcu *hnext;
+	struct rcu_head     rcu;
+	atomic_t	    refcnt;
+	unsigned long       alive;
 };
 
 #define IPOIB_UD_MTU(ib_mtu)		(ib_mtu - IPOIB_ENCAP_LEN)
@@ -394,21 +415,17 @@ static inline int ipoib_ud_need_sg(unsigned int ib_mtu)
 	return IPOIB_UD_BUF_SIZE(ib_mtu) > PAGE_SIZE;
 }
 
-/*
- * We stash a pointer to our private neighbour information after our
- * hardware address in neigh->ha.  The ALIGN() expression here makes
- * sure that this pointer is stored aligned so that an unaligned
- * load is not needed to dereference it.
- */
-static inline struct ipoib_neigh **to_ipoib_neigh(struct neighbour *neigh)
+void ipoib_neigh_dtor(struct ipoib_neigh *neigh);
+static inline void ipoib_neigh_put(struct ipoib_neigh *neigh)
 {
-	return (void*) neigh + ALIGN(offsetof(struct neighbour, ha) +
-				     INFINIBAND_ALEN, sizeof(void *));
+	if (atomic_dec_and_test(&neigh->refcnt))
+		ipoib_neigh_dtor(neigh);
 }
-
-struct ipoib_neigh *ipoib_neigh_alloc(struct neighbour *neigh,
+struct ipoib_neigh *ipoib_neigh_get(struct net_device *dev, u8 *daddr);
+struct ipoib_neigh *ipoib_neigh_alloc(u8 *daddr,
 				      struct net_device *dev);
-void ipoib_neigh_free(struct net_device *dev, struct ipoib_neigh *neigh);
+void ipoib_neigh_free(struct ipoib_neigh *neigh);
+void ipoib_del_neighs_by_gid(struct net_device *dev, u8 *gid);
 
 extern struct workqueue_struct *ipoib_workqueue;
 
@@ -425,7 +442,6 @@ static inline void ipoib_put_ah(struct ipoib_ah *ah)
 {
 	kref_put(&ah->ref, ipoib_free_ah);
 }
-
 int ipoib_open(struct net_device *dev);
 int ipoib_add_pkey_attr(struct net_device *dev);
 int ipoib_add_umcast_attr(struct net_device *dev);
@@ -455,7 +471,7 @@ void ipoib_dev_cleanup(struct net_device *dev);
 
 void ipoib_mcast_join_task(struct work_struct *work);
 void ipoib_mcast_carrier_on_task(struct work_struct *work);
-void ipoib_mcast_send(struct net_device *dev, void *mgid, struct sk_buff *skb);
+void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb);
 
 void ipoib_mcast_restart_task(struct work_struct *work);
 int ipoib_mcast_start_thread(struct net_device *dev);
@@ -517,10 +533,10 @@ static inline int ipoib_cm_admin_enabled(struct net_device *dev)
 		test_bit(IPOIB_FLAG_ADMIN_CM, &priv->flags);
 }
 
-static inline int ipoib_cm_enabled(struct net_device *dev, struct neighbour *n)
+static inline int ipoib_cm_enabled(struct net_device *dev, u8 *hwaddr)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
-	return IPOIB_CM_SUPPORTED(n->ha) &&
+	return IPOIB_CM_SUPPORTED(hwaddr) &&
 		test_bit(IPOIB_FLAG_ADMIN_CM, &priv->flags);
 }
 
@@ -575,7 +591,7 @@ static inline int ipoib_cm_admin_enabled(struct net_device *dev)
 {
 	return 0;
 }
-static inline int ipoib_cm_enabled(struct net_device *dev, struct neighbour *n)
+static inline int ipoib_cm_enabled(struct net_device *dev, u8 *hwaddr)
 
 {
 	return 0;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_cm.c b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
index 014504d8e43..95ecf4eadf5 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_cm.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
@@ -811,9 +811,7 @@ void ipoib_cm_handle_tx_wc(struct net_device *dev, struct ib_wc *wc)
 		if (neigh) {
 			neigh->cm = NULL;
 			list_del(&neigh->list);
-			if (neigh->ah)
-				ipoib_put_ah(neigh->ah);
-			ipoib_neigh_free(dev, neigh);
+			ipoib_neigh_free(neigh);
 
 			tx->neigh = NULL;
 		}
@@ -1230,9 +1228,7 @@ static int ipoib_cm_tx_handler(struct ib_cm_id *cm_id,
 		if (neigh) {
 			neigh->cm = NULL;
 			list_del(&neigh->list);
-			if (neigh->ah)
-				ipoib_put_ah(neigh->ah);
-			ipoib_neigh_free(dev, neigh);
+			ipoib_neigh_free(neigh);
 
 			tx->neigh = NULL;
 		}
@@ -1279,7 +1275,7 @@ void ipoib_cm_destroy_tx(struct ipoib_cm_tx *tx)
 		list_move(&tx->list, &priv->cm.reap_list);
 		queue_work(ipoib_workqueue, &priv->cm.reap_task);
 		ipoib_dbg(priv, "Reap connection for gid %pI6\n",
-			  tx->neigh->dgid.raw);
+			  tx->neigh->daddr + 4);
 		tx->neigh = NULL;
 	}
 }
@@ -1304,7 +1300,7 @@ static void ipoib_cm_tx_start(struct work_struct *work)
 		p = list_entry(priv->cm.start_list.next, typeof(*p), list);
 		list_del_init(&p->list);
 		neigh = p->neigh;
-		qpn = IPOIB_QPN(neigh->neighbour->ha);
+		qpn = IPOIB_QPN(neigh->daddr);
 		memcpy(&pathrec, &p->path->pathrec, sizeof pathrec);
 
 		spin_unlock_irqrestore(&priv->lock, flags);
@@ -1320,9 +1316,7 @@ static void ipoib_cm_tx_start(struct work_struct *work)
 			if (neigh) {
 				neigh->cm = NULL;
 				list_del(&neigh->list);
-				if (neigh->ah)
-					ipoib_put_ah(neigh->ah);
-				ipoib_neigh_free(dev, neigh);
+				ipoib_neigh_free(neigh);
 			}
 			list_del(&p->list);
 			kfree(p);
@@ -1376,7 +1370,7 @@ static void ipoib_cm_skb_reap(struct work_struct *work)
 
 		if (skb->protocol == htons(ETH_P_IP))
 			icmp_send(skb, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED, htonl(mtu));
-#if defined(CONFIG_IPV6) || defined(CONFIG_IPV6_MODULE)
+#if IS_ENABLED(CONFIG_IPV6)
 		else if (skb->protocol == htons(ETH_P_IPV6))
 			icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu);
 #endif
@@ -1397,7 +1391,7 @@ void ipoib_cm_skb_too_long(struct net_device *dev, struct sk_buff *skb,
 	int e = skb_queue_empty(&priv->cm.skb_queue);
 
 	if (skb_dst(skb))
-		skb_dst(skb)->ops->update_pmtu(skb_dst(skb), mtu);
+		skb_dst(skb)->ops->update_pmtu(skb_dst(skb), NULL, skb, mtu);
 
 	skb_queue_tail(&priv->cm.skb_queue, skb);
 	if (e)
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_ib.c b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
index 5c1bc995e56..f10221f4080 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_ib.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
@@ -123,7 +123,7 @@ static void ipoib_ud_skb_put_frags(struct ipoib_dev_priv *priv,
 
 		skb_frag_size_set(frag, size);
 		skb->data_len += size;
-		skb->truesize += size;
+		skb->truesize += PAGE_SIZE;
 	} else
 		skb_put(skb, length);
 
@@ -156,14 +156,18 @@ static struct sk_buff *ipoib_alloc_rx_skb(struct net_device *dev, int id)
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct sk_buff *skb;
 	int buf_size;
+	int tailroom;
 	u64 *mapping;
 
-	if (ipoib_ud_need_sg(priv->max_ib_mtu))
+	if (ipoib_ud_need_sg(priv->max_ib_mtu)) {
 		buf_size = IPOIB_UD_HEAD_SIZE;
-	else
+		tailroom = 128; /* reserve some tailroom for IP/TCP headers */
+	} else {
 		buf_size = IPOIB_UD_BUF_SIZE(priv->max_ib_mtu);
+		tailroom = 0;
+	}
 
-	skb = dev_alloc_skb(buf_size + 4);
+	skb = dev_alloc_skb(buf_size + tailroom + 4);
 	if (unlikely(!skb))
 		return NULL;
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 3974c290b66..97920b77a5d 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -46,7 +46,8 @@
 #include <linux/ip.h>
 #include <linux/in.h>
 
-#include <net/dst.h>
+#include <linux/jhash.h>
+#include <net/arp.h>
 
 MODULE_AUTHOR("Roland Dreier");
 MODULE_DESCRIPTION("IP-over-InfiniBand net driver");
@@ -84,6 +85,7 @@ struct ib_sa_client ipoib_sa_client;
 
 static void ipoib_add_one(struct ib_device *device);
 static void ipoib_remove_one(struct ib_device *device);
+static void ipoib_neigh_reclaim(struct rcu_head *rp);
 
 static struct ib_client ipoib_client = {
 	.name   = "ipoib",
@@ -264,30 +266,15 @@ static int __path_add(struct net_device *dev, struct ipoib_path *path)
 
 static void path_free(struct net_device *dev, struct ipoib_path *path)
 {
-	struct ipoib_dev_priv *priv = netdev_priv(dev);
-	struct ipoib_neigh *neigh, *tn;
 	struct sk_buff *skb;
-	unsigned long flags;
 
 	while ((skb = __skb_dequeue(&path->queue)))
 		dev_kfree_skb_irq(skb);
 
-	spin_lock_irqsave(&priv->lock, flags);
-
-	list_for_each_entry_safe(neigh, tn, &path->neigh_list, list) {
-		/*
-		 * It's safe to call ipoib_put_ah() inside priv->lock
-		 * here, because we know that path->ah will always
-		 * hold one more reference, so ipoib_put_ah() will
-		 * never do more than decrement the ref count.
-		 */
-		if (neigh->ah)
-			ipoib_put_ah(neigh->ah);
-
-		ipoib_neigh_free(dev, neigh);
-	}
+	ipoib_dbg(netdev_priv(dev), "path_free\n");
 
-	spin_unlock_irqrestore(&priv->lock, flags);
+	/* remove all neigh connected to this path */
+	ipoib_del_neighs_by_gid(dev, path->pathrec.dgid.raw);
 
 	if (path->ah)
 		ipoib_put_ah(path->ah);
@@ -458,19 +445,15 @@ static void path_rec_completion(int status,
 			}
 			kref_get(&path->ah->ref);
 			neigh->ah = path->ah;
-			memcpy(&neigh->dgid.raw, &path->pathrec.dgid.raw,
-			       sizeof(union ib_gid));
 
-			if (ipoib_cm_enabled(dev, neigh->neighbour)) {
+			if (ipoib_cm_enabled(dev, neigh->daddr)) {
 				if (!ipoib_cm_get(neigh))
 					ipoib_cm_set(neigh, ipoib_cm_create_tx(dev,
 									       path,
 									       neigh));
 				if (!ipoib_cm_get(neigh)) {
 					list_del(&neigh->list);
-					if (neigh->ah)
-						ipoib_put_ah(neigh->ah);
-					ipoib_neigh_free(dev, neigh);
+					ipoib_neigh_free(neigh);
 					continue;
 				}
 			}
@@ -555,15 +538,15 @@ static int path_rec_start(struct net_device *dev,
 	return 0;
 }
 
-/* called with rcu_read_lock */
-static void neigh_add_path(struct sk_buff *skb, struct neighbour *n, struct net_device *dev)
+static void neigh_add_path(struct sk_buff *skb, u8 *daddr,
+			   struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct ipoib_path *path;
 	struct ipoib_neigh *neigh;
 	unsigned long flags;
 
-	neigh = ipoib_neigh_alloc(n, skb->dev);
+	neigh = ipoib_neigh_alloc(daddr, dev);
 	if (!neigh) {
 		++dev->stats.tx_dropped;
 		dev_kfree_skb_any(skb);
@@ -572,9 +555,9 @@ static void neigh_add_path(struct sk_buff *skb, struct neighbour *n, struct net_
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	path = __path_find(dev, n->ha + 4);
+	path = __path_find(dev, daddr + 4);
 	if (!path) {
-		path = path_rec_create(dev, n->ha + 4);
+		path = path_rec_create(dev, daddr + 4);
 		if (!path)
 			goto err_path;
 
@@ -586,17 +569,13 @@ static void neigh_add_path(struct sk_buff *skb, struct neighbour *n, struct net_
 	if (path->ah) {
 		kref_get(&path->ah->ref);
 		neigh->ah = path->ah;
-		memcpy(&neigh->dgid.raw, &path->pathrec.dgid.raw,
-		       sizeof(union ib_gid));
 
-		if (ipoib_cm_enabled(dev, neigh->neighbour)) {
+		if (ipoib_cm_enabled(dev, neigh->daddr)) {
 			if (!ipoib_cm_get(neigh))
 				ipoib_cm_set(neigh, ipoib_cm_create_tx(dev, path, neigh));
 			if (!ipoib_cm_get(neigh)) {
 				list_del(&neigh->list);
-				if (neigh->ah)
-					ipoib_put_ah(neigh->ah);
-				ipoib_neigh_free(dev, neigh);
+				ipoib_neigh_free(neigh);
 				goto err_drop;
 			}
 			if (skb_queue_len(&neigh->queue) < IPOIB_MAX_PATH_REC_QUEUE)
@@ -608,7 +587,8 @@ static void neigh_add_path(struct sk_buff *skb, struct neighbour *n, struct net_
 			}
 		} else {
 			spin_unlock_irqrestore(&priv->lock, flags);
-			ipoib_send(dev, skb, path->ah, IPOIB_QPN(n->ha));
+			ipoib_send(dev, skb, path->ah, IPOIB_QPN(daddr));
+			ipoib_neigh_put(neigh);
 			return;
 		}
 	} else {
@@ -621,35 +601,20 @@ static void neigh_add_path(struct sk_buff *skb, struct neighbour *n, struct net_
 	}
 
 	spin_unlock_irqrestore(&priv->lock, flags);
+	ipoib_neigh_put(neigh);
 	return;
 
 err_list:
 	list_del(&neigh->list);
 
 err_path:
-	ipoib_neigh_free(dev, neigh);
+	ipoib_neigh_free(neigh);
 err_drop:
 	++dev->stats.tx_dropped;
 	dev_kfree_skb_any(skb);
 
 	spin_unlock_irqrestore(&priv->lock, flags);
-}
-
-/* called with rcu_read_lock */
-static void ipoib_path_lookup(struct sk_buff *skb, struct neighbour *n, struct net_device *dev)
-{
-	struct ipoib_dev_priv *priv = netdev_priv(skb->dev);
-
-	/* Look up path record for unicasts */
-	if (n->ha[4] != 0xff) {
-		neigh_add_path(skb, n, dev);
-		return;
-	}
-
-	/* Add in the P_Key for multicasts */
-	n->ha[8] = (priv->pkey >> 8) & 0xff;
-	n->ha[9] = priv->pkey & 0xff;
-	ipoib_mcast_send(dev, n->ha + 4, skb);
+	ipoib_neigh_put(neigh);
 }
 
 static void unicast_arp_send(struct sk_buff *skb, struct net_device *dev,
@@ -710,94 +675,80 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct ipoib_neigh *neigh;
-	struct neighbour *n = NULL;
+	struct ipoib_cb *cb = (struct ipoib_cb *) skb->cb;
+	struct ipoib_header *header;
 	unsigned long flags;
 
-	rcu_read_lock();
-	if (likely(skb_dst(skb))) {
-		n = dst_get_neighbour_noref(skb_dst(skb));
-		if (!n) {
+	header = (struct ipoib_header *) skb->data;
+
+	if (unlikely(cb->hwaddr[4] == 0xff)) {
+		/* multicast, arrange "if" according to probability */
+		if ((header->proto != htons(ETH_P_IP)) &&
+		    (header->proto != htons(ETH_P_IPV6)) &&
+		    (header->proto != htons(ETH_P_ARP)) &&
+		    (header->proto != htons(ETH_P_RARP))) {
+			/* ethertype not supported by IPoIB */
 			++dev->stats.tx_dropped;
 			dev_kfree_skb_any(skb);
-			goto unlock;
+			return NETDEV_TX_OK;
 		}
+		/* Add in the P_Key for multicast*/
+		cb->hwaddr[8] = (priv->pkey >> 8) & 0xff;
+		cb->hwaddr[9] = priv->pkey & 0xff;
+
+		neigh = ipoib_neigh_get(dev, cb->hwaddr);
+		if (likely(neigh))
+			goto send_using_neigh;
+		ipoib_mcast_send(dev, cb->hwaddr, skb);
+		return NETDEV_TX_OK;
 	}
-	if (likely(n)) {
-		if (unlikely(!*to_ipoib_neigh(n))) {
-			ipoib_path_lookup(skb, n, dev);
-			goto unlock;
-		}
-
-		neigh = *to_ipoib_neigh(n);
 
-		if (unlikely((memcmp(&neigh->dgid.raw,
-				     n->ha + 4,
-				     sizeof(union ib_gid))) ||
-			     (neigh->dev != dev))) {
-			spin_lock_irqsave(&priv->lock, flags);
-			/*
-			 * It's safe to call ipoib_put_ah() inside
-			 * priv->lock here, because we know that
-			 * path->ah will always hold one more reference,
-			 * so ipoib_put_ah() will never do more than
-			 * decrement the ref count.
-			 */
-			if (neigh->ah)
-				ipoib_put_ah(neigh->ah);
-			list_del(&neigh->list);
-			ipoib_neigh_free(dev, neigh);
-			spin_unlock_irqrestore(&priv->lock, flags);
-			ipoib_path_lookup(skb, n, dev);
-			goto unlock;
+	/* unicast, arrange "switch" according to probability */
+	switch (header->proto) {
+	case htons(ETH_P_IP):
+	case htons(ETH_P_IPV6):
+		neigh = ipoib_neigh_get(dev, cb->hwaddr);
+		if (unlikely(!neigh)) {
+			neigh_add_path(skb, cb->hwaddr, dev);
+			return NETDEV_TX_OK;
 		}
+		break;
+	case htons(ETH_P_ARP):
+	case htons(ETH_P_RARP):
+		/* for unicast ARP and RARP should always perform path find */
+		unicast_arp_send(skb, dev, cb);
+		return NETDEV_TX_OK;
+	default:
+		/* ethertype not supported by IPoIB */
+		++dev->stats.tx_dropped;
+		dev_kfree_skb_any(skb);
+		return NETDEV_TX_OK;
+	}
 
-		if (ipoib_cm_get(neigh)) {
-			if (ipoib_cm_up(neigh)) {
-				ipoib_cm_send(dev, skb, ipoib_cm_get(neigh));
-				goto unlock;
-			}
-		} else if (neigh->ah) {
-			ipoib_send(dev, skb, neigh->ah, IPOIB_QPN(n->ha));
-			goto unlock;
+send_using_neigh:
+	/* note we now hold a ref to neigh */
+	if (ipoib_cm_get(neigh)) {
+		if (ipoib_cm_up(neigh)) {
+			ipoib_cm_send(dev, skb, ipoib_cm_get(neigh));
+			goto unref;
 		}
+	} else if (neigh->ah) {
+		ipoib_send(dev, skb, neigh->ah, IPOIB_QPN(cb->hwaddr));
+		goto unref;
+	}
 
-		if (skb_queue_len(&neigh->queue) < IPOIB_MAX_PATH_REC_QUEUE) {
-			spin_lock_irqsave(&priv->lock, flags);
-			__skb_queue_tail(&neigh->queue, skb);
-			spin_unlock_irqrestore(&priv->lock, flags);
-		} else {
-			++dev->stats.tx_dropped;
-			dev_kfree_skb_any(skb);
-		}
+	if (skb_queue_len(&neigh->queue) < IPOIB_MAX_PATH_REC_QUEUE) {
+		spin_lock_irqsave(&priv->lock, flags);
+		__skb_queue_tail(&neigh->queue, skb);
+		spin_unlock_irqrestore(&priv->lock, flags);
 	} else {
-		struct ipoib_cb *cb = (struct ipoib_cb *) skb->cb;
-
-		if (cb->hwaddr[4] == 0xff) {
-			/* Add in the P_Key for multicast*/
-			cb->hwaddr[8] = (priv->pkey >> 8) & 0xff;
-			cb->hwaddr[9] = priv->pkey & 0xff;
+		++dev->stats.tx_dropped;
+		dev_kfree_skb_any(skb);
+	}
 
-			ipoib_mcast_send(dev, cb->hwaddr + 4, skb);
-		} else {
-			/* unicast GID -- should be ARP or RARP reply */
-
-			if ((be16_to_cpup((__be16 *) skb->data) != ETH_P_ARP) &&
-			    (be16_to_cpup((__be16 *) skb->data) != ETH_P_RARP)) {
-				ipoib_warn(priv, "Unicast, no %s: type %04x, QPN %06x %pI6\n",
-					   skb_dst(skb) ? "neigh" : "dst",
-					   be16_to_cpup((__be16 *) skb->data),
-					   IPOIB_QPN(cb->hwaddr),
-					   cb->hwaddr + 4);
-				dev_kfree_skb_any(skb);
-				++dev->stats.tx_dropped;
-				goto unlock;
-			}
+unref:
+	ipoib_neigh_put(neigh);
 
-			unicast_arp_send(skb, dev, cb);
-		}
-	}
-unlock:
-	rcu_read_unlock();
 	return NETDEV_TX_OK;
 }
 
@@ -819,6 +770,7 @@ static int ipoib_hard_header(struct sk_buff *skb,
 			     const void *daddr, const void *saddr, unsigned len)
 {
 	struct ipoib_header *header;
+	struct ipoib_cb *cb = (struct ipoib_cb *) skb->cb;
 
 	header = (struct ipoib_header *) skb_push(skb, sizeof *header);
 
@@ -826,14 +778,11 @@ static int ipoib_hard_header(struct sk_buff *skb,
 	header->reserved = 0;
 
 	/*
-	 * If we don't have a dst_entry structure, stuff the
+	 * we don't rely on dst_entry structure,  always stuff the
 	 * destination address into skb->cb so we can figure out where
 	 * to send the packet later.
 	 */
-	if (!skb_dst(skb)) {
-		struct ipoib_cb *cb = (struct ipoib_cb *) skb->cb;
-		memcpy(cb->hwaddr, daddr, INFINIBAND_ALEN);
-	}
+	memcpy(cb->hwaddr, daddr, INFINIBAND_ALEN);
 
 	return 0;
 }
@@ -850,86 +799,438 @@ static void ipoib_set_mcast_list(struct net_device *dev)
 	queue_work(ipoib_workqueue, &priv->restart_task);
 }
 
-static void ipoib_neigh_cleanup(struct neighbour *n)
+static u32 ipoib_addr_hash(struct ipoib_neigh_hash *htbl, u8 *daddr)
 {
-	struct ipoib_neigh *neigh;
-	struct ipoib_dev_priv *priv = netdev_priv(n->dev);
+	/*
+	 * Use only the address parts that contributes to spreading
+	 * The subnet prefix is not used as one can not connect to
+	 * same remote port (GUID) using the same remote QPN via two
+	 * different subnets.
+	 */
+	 /* qpn octets[1:4) & port GUID octets[12:20) */
+	u32 *daddr_32 = (u32 *) daddr;
+	u32 hv;
+
+	hv = jhash_3words(daddr_32[3], daddr_32[4], 0xFFFFFF & daddr_32[0], 0);
+	return hv & htbl->mask;
+}
+
+struct ipoib_neigh *ipoib_neigh_get(struct net_device *dev, u8 *daddr)
+{
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	struct ipoib_neigh *neigh = NULL;
+	u32 hash_val;
+
+	rcu_read_lock_bh();
+
+	htbl = rcu_dereference_bh(ntbl->htbl);
+
+	if (!htbl)
+		goto out_unlock;
+
+	hash_val = ipoib_addr_hash(htbl, daddr);
+	for (neigh = rcu_dereference_bh(htbl->buckets[hash_val]);
+	     neigh != NULL;
+	     neigh = rcu_dereference_bh(neigh->hnext)) {
+		if (memcmp(daddr, neigh->daddr, INFINIBAND_ALEN) == 0) {
+			/* found, take one ref on behalf of the caller */
+			if (!atomic_inc_not_zero(&neigh->refcnt)) {
+				/* deleted */
+				neigh = NULL;
+				goto out_unlock;
+			}
+			neigh->alive = jiffies;
+			goto out_unlock;
+		}
+	}
+
+out_unlock:
+	rcu_read_unlock_bh();
+	return neigh;
+}
+
+static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
+{
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	unsigned long neigh_obsolete;
+	unsigned long dt;
 	unsigned long flags;
-	struct ipoib_ah *ah = NULL;
+	int i;
 
-	neigh = *to_ipoib_neigh(n);
-	if (neigh)
-		priv = netdev_priv(neigh->dev);
-	else
+	if (test_bit(IPOIB_STOP_NEIGH_GC, &priv->flags))
 		return;
-	ipoib_dbg(priv,
-		  "neigh_cleanup for %06x %pI6\n",
-		  IPOIB_QPN(n->ha),
-		  n->ha + 4);
 
-	spin_lock_irqsave(&priv->lock, flags);
+	write_lock_bh(&ntbl->rwlock);
+
+	htbl = rcu_dereference_protected(ntbl->htbl,
+					 lockdep_is_held(&ntbl->rwlock));
+
+	if (!htbl)
+		goto out_unlock;
+
+	/* neigh is obsolete if it was idle for two GC periods */
+	dt = 2 * arp_tbl.gc_interval;
+	neigh_obsolete = jiffies - dt;
+	/* handle possible race condition */
+	if (test_bit(IPOIB_STOP_NEIGH_GC, &priv->flags))
+		goto out_unlock;
+
+	for (i = 0; i < htbl->size; i++) {
+		struct ipoib_neigh *neigh;
+		struct ipoib_neigh __rcu **np = &htbl->buckets[i];
+
+		while ((neigh = rcu_dereference_protected(*np,
+							  lockdep_is_held(&ntbl->rwlock))) != NULL) {
+			/* was the neigh idle for two GC periods */
+			if (time_after(neigh_obsolete, neigh->alive)) {
+				rcu_assign_pointer(*np,
+						   rcu_dereference_protected(neigh->hnext,
+									     lockdep_is_held(&ntbl->rwlock)));
+				/* remove from path/mc list */
+				spin_lock_irqsave(&priv->lock, flags);
+				list_del(&neigh->list);
+				spin_unlock_irqrestore(&priv->lock, flags);
+				call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
+			} else {
+				np = &neigh->hnext;
+			}
 
-	if (neigh->ah)
-		ah = neigh->ah;
-	list_del(&neigh->list);
-	ipoib_neigh_free(n->dev, neigh);
+		}
+	}
 
-	spin_unlock_irqrestore(&priv->lock, flags);
+out_unlock:
+	write_unlock_bh(&ntbl->rwlock);
+}
 
-	if (ah)
-		ipoib_put_ah(ah);
+static void ipoib_reap_neigh(struct work_struct *work)
+{
+	struct ipoib_dev_priv *priv =
+		container_of(work, struct ipoib_dev_priv, neigh_reap_task.work);
+
+	__ipoib_reap_neigh(priv);
+
+	if (!test_bit(IPOIB_STOP_NEIGH_GC, &priv->flags))
+		queue_delayed_work(ipoib_workqueue, &priv->neigh_reap_task,
+				   arp_tbl.gc_interval);
 }
 
-struct ipoib_neigh *ipoib_neigh_alloc(struct neighbour *neighbour,
+
+static struct ipoib_neigh *ipoib_neigh_ctor(u8 *daddr,
 				      struct net_device *dev)
 {
 	struct ipoib_neigh *neigh;
 
-	neigh = kmalloc(sizeof *neigh, GFP_ATOMIC);
+	neigh = kzalloc(sizeof *neigh, GFP_ATOMIC);
 	if (!neigh)
 		return NULL;
 
-	neigh->neighbour = neighbour;
 	neigh->dev = dev;
-	memset(&neigh->dgid.raw, 0, sizeof (union ib_gid));
-	*to_ipoib_neigh(neighbour) = neigh;
+	memcpy(&neigh->daddr, daddr, sizeof(neigh->daddr));
 	skb_queue_head_init(&neigh->queue);
+	INIT_LIST_HEAD(&neigh->list);
 	ipoib_cm_set(neigh, NULL);
+	/* one ref on behalf of the caller */
+	atomic_set(&neigh->refcnt, 1);
+
+	return neigh;
+}
+
+struct ipoib_neigh *ipoib_neigh_alloc(u8 *daddr,
+				      struct net_device *dev)
+{
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	struct ipoib_neigh *neigh;
+	u32 hash_val;
+
+	write_lock_bh(&ntbl->rwlock);
+
+	htbl = rcu_dereference_protected(ntbl->htbl,
+					 lockdep_is_held(&ntbl->rwlock));
+	if (!htbl) {
+		neigh = NULL;
+		goto out_unlock;
+	}
+
+	/* need to add a new neigh, but maybe some other thread succeeded?
+	 * recalc hash, maybe hash resize took place so we do a search
+	 */
+	hash_val = ipoib_addr_hash(htbl, daddr);
+	for (neigh = rcu_dereference_protected(htbl->buckets[hash_val],
+					       lockdep_is_held(&ntbl->rwlock));
+	     neigh != NULL;
+	     neigh = rcu_dereference_protected(neigh->hnext,
+					       lockdep_is_held(&ntbl->rwlock))) {
+		if (memcmp(daddr, neigh->daddr, INFINIBAND_ALEN) == 0) {
+			/* found, take one ref on behalf of the caller */
+			if (!atomic_inc_not_zero(&neigh->refcnt)) {
+				/* deleted */
+				neigh = NULL;
+				break;
+			}
+			neigh->alive = jiffies;
+			goto out_unlock;
+		}
+	}
+
+	neigh = ipoib_neigh_ctor(daddr, dev);
+	if (!neigh)
+		goto out_unlock;
+
+	/* one ref on behalf of the hash table */
+	atomic_inc(&neigh->refcnt);
+	neigh->alive = jiffies;
+	/* put in hash */
+	rcu_assign_pointer(neigh->hnext,
+			   rcu_dereference_protected(htbl->buckets[hash_val],
+						     lockdep_is_held(&ntbl->rwlock)));
+	rcu_assign_pointer(htbl->buckets[hash_val], neigh);
+	atomic_inc(&ntbl->entries);
+
+out_unlock:
+	write_unlock_bh(&ntbl->rwlock);
 
 	return neigh;
 }
 
-void ipoib_neigh_free(struct net_device *dev, struct ipoib_neigh *neigh)
+void ipoib_neigh_dtor(struct ipoib_neigh *neigh)
 {
+	/* neigh reference count was dropprd to zero */
+	struct net_device *dev = neigh->dev;
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct sk_buff *skb;
-	*to_ipoib_neigh(neigh->neighbour) = NULL;
+	if (neigh->ah)
+		ipoib_put_ah(neigh->ah);
 	while ((skb = __skb_dequeue(&neigh->queue))) {
 		++dev->stats.tx_dropped;
 		dev_kfree_skb_any(skb);
 	}
 	if (ipoib_cm_get(neigh))
 		ipoib_cm_destroy_tx(ipoib_cm_get(neigh));
+	ipoib_dbg(netdev_priv(dev),
+		  "neigh free for %06x %pI6\n",
+		  IPOIB_QPN(neigh->daddr),
+		  neigh->daddr + 4);
 	kfree(neigh);
+	if (atomic_dec_and_test(&priv->ntbl.entries)) {
+		if (test_bit(IPOIB_NEIGH_TBL_FLUSH, &priv->flags))
+			complete(&priv->ntbl.flushed);
+	}
+}
+
+static void ipoib_neigh_reclaim(struct rcu_head *rp)
+{
+	/* Called as a result of removal from hash table */
+	struct ipoib_neigh *neigh = container_of(rp, struct ipoib_neigh, rcu);
+	/* note TX context may hold another ref */
+	ipoib_neigh_put(neigh);
 }
 
-static int ipoib_neigh_setup_dev(struct net_device *dev, struct neigh_parms *parms)
+void ipoib_neigh_free(struct ipoib_neigh *neigh)
 {
-	parms->neigh_cleanup = ipoib_neigh_cleanup;
+	struct net_device *dev = neigh->dev;
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	struct ipoib_neigh __rcu **np;
+	struct ipoib_neigh *n;
+	u32 hash_val;
+
+	write_lock_bh(&ntbl->rwlock);
+
+	htbl = rcu_dereference_protected(ntbl->htbl,
+					lockdep_is_held(&ntbl->rwlock));
+	if (!htbl)
+		goto out_unlock;
+
+	hash_val = ipoib_addr_hash(htbl, neigh->daddr);
+	np = &htbl->buckets[hash_val];
+	for (n = rcu_dereference_protected(*np,
+					    lockdep_is_held(&ntbl->rwlock));
+	     n != NULL;
+	     n = rcu_dereference_protected(neigh->hnext,
+					lockdep_is_held(&ntbl->rwlock))) {
+		if (n == neigh) {
+			/* found */
+			rcu_assign_pointer(*np,
+					   rcu_dereference_protected(neigh->hnext,
+								     lockdep_is_held(&ntbl->rwlock)));
+			call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
+			goto out_unlock;
+		} else {
+			np = &n->hnext;
+		}
+	}
+
+out_unlock:
+	write_unlock_bh(&ntbl->rwlock);
+
+}
+
+static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
+{
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	struct ipoib_neigh **buckets;
+	u32 size;
+
+	clear_bit(IPOIB_NEIGH_TBL_FLUSH, &priv->flags);
+	ntbl->htbl = NULL;
+	rwlock_init(&ntbl->rwlock);
+	htbl = kzalloc(sizeof(*htbl), GFP_KERNEL);
+	if (!htbl)
+		return -ENOMEM;
+	set_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
+	size = roundup_pow_of_two(arp_tbl.gc_thresh3);
+	buckets = kzalloc(size * sizeof(*buckets), GFP_KERNEL);
+	if (!buckets) {
+		kfree(htbl);
+		return -ENOMEM;
+	}
+	htbl->size = size;
+	htbl->mask = (size - 1);
+	htbl->buckets = buckets;
+	ntbl->htbl = htbl;
+	atomic_set(&ntbl->entries, 0);
+
+	/* start garbage collection */
+	clear_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
+	queue_delayed_work(ipoib_workqueue, &priv->neigh_reap_task,
+			   arp_tbl.gc_interval);
 
 	return 0;
 }
 
+static void neigh_hash_free_rcu(struct rcu_head *head)
+{
+	struct ipoib_neigh_hash *htbl = container_of(head,
+						    struct ipoib_neigh_hash,
+						    rcu);
+	struct ipoib_neigh __rcu **buckets = htbl->buckets;
+
+	kfree(buckets);
+	kfree(htbl);
+}
+
+void ipoib_del_neighs_by_gid(struct net_device *dev, u8 *gid)
+{
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	unsigned long flags;
+	int i;
+
+	/* remove all neigh connected to a given path or mcast */
+	write_lock_bh(&ntbl->rwlock);
+
+	htbl = rcu_dereference_protected(ntbl->htbl,
+					 lockdep_is_held(&ntbl->rwlock));
+
+	if (!htbl)
+		goto out_unlock;
+
+	for (i = 0; i < htbl->size; i++) {
+		struct ipoib_neigh *neigh;
+		struct ipoib_neigh __rcu **np = &htbl->buckets[i];
+
+		while ((neigh = rcu_dereference_protected(*np,
+							  lockdep_is_held(&ntbl->rwlock))) != NULL) {
+			/* delete neighs belong to this parent */
+			if (!memcmp(gid, neigh->daddr + 4, sizeof (union ib_gid))) {
+				rcu_assign_pointer(*np,
+						   rcu_dereference_protected(neigh->hnext,
+									     lockdep_is_held(&ntbl->rwlock)));
+				/* remove from parent list */
+				spin_lock_irqsave(&priv->lock, flags);
+				list_del(&neigh->list);
+				spin_unlock_irqrestore(&priv->lock, flags);
+				call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
+			} else {
+				np = &neigh->hnext;
+			}
+
+		}
+	}
+out_unlock:
+	write_unlock_bh(&ntbl->rwlock);
+}
+
+static void ipoib_flush_neighs(struct ipoib_dev_priv *priv)
+{
+	struct ipoib_neigh_table *ntbl = &priv->ntbl;
+	struct ipoib_neigh_hash *htbl;
+	unsigned long flags;
+	int i;
+
+	write_lock_bh(&ntbl->rwlock);
+
+	htbl = rcu_dereference_protected(ntbl->htbl,
+					lockdep_is_held(&ntbl->rwlock));
+	if (!htbl)
+		goto out_unlock;
+
+	for (i = 0; i < htbl->size; i++) {
+		struct ipoib_neigh *neigh;
+		struct ipoib_neigh __rcu **np = &htbl->buckets[i];
+
+		while ((neigh = rcu_dereference_protected(*np,
+							  lockdep_is_held(&ntbl->rwlock))) != NULL) {
+			rcu_assign_pointer(*np,
+					   rcu_dereference_protected(neigh->hnext,
+								     lockdep_is_held(&ntbl->rwlock)));
+			/* remove from path/mc list */
+			spin_lock_irqsave(&priv->lock, flags);
+			list_del(&neigh->list);
+			spin_unlock_irqrestore(&priv->lock, flags);
+			call_rcu(&neigh->rcu, ipoib_neigh_reclaim);
+		}
+	}
+
+	rcu_assign_pointer(ntbl->htbl, NULL);
+	call_rcu(&htbl->rcu, neigh_hash_free_rcu);
+
+out_unlock:
+	write_unlock_bh(&ntbl->rwlock);
+}
+
+static void ipoib_neigh_hash_uninit(struct net_device *dev)
+{
+	struct ipoib_dev_priv *priv = netdev_priv(dev);
+	int stopped;
+
+	ipoib_dbg(priv, "ipoib_neigh_hash_uninit\n");
+	init_completion(&priv->ntbl.flushed);
+	set_bit(IPOIB_NEIGH_TBL_FLUSH, &priv->flags);
+
+	/* Stop GC if called at init fail need to cancel work */
+	stopped = test_and_set_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
+	if (!stopped)
+		cancel_delayed_work(&priv->neigh_reap_task);
+
+	if (atomic_read(&priv->ntbl.entries)) {
+		ipoib_flush_neighs(priv);
+		wait_for_completion(&priv->ntbl.flushed);
+	}
+}
+
+
 int ipoib_dev_init(struct net_device *dev, struct ib_device *ca, int port)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 
+	if (ipoib_neigh_hash_init(priv) < 0)
+		goto out;
 	/* Allocate RX/TX "rings" to hold queued skbs */
 	priv->rx_ring =	kzalloc(ipoib_recvq_size * sizeof *priv->rx_ring,
 				GFP_KERNEL);
 	if (!priv->rx_ring) {
 		printk(KERN_WARNING "%s: failed to allocate RX ring (%d entries)\n",
 		       ca->name, ipoib_recvq_size);
-		goto out;
+		goto out_neigh_hash_cleanup;
 	}
 
 	priv->tx_ring = vzalloc(ipoib_sendq_size * sizeof *priv->tx_ring);
@@ -952,6 +1253,8 @@ out_tx_ring_cleanup:
 out_rx_ring_cleanup:
 	kfree(priv->rx_ring);
 
+out_neigh_hash_cleanup:
+	ipoib_neigh_hash_uninit(dev);
 out:
 	return -ENOMEM;
 }
@@ -964,6 +1267,9 @@ void ipoib_dev_cleanup(struct net_device *dev)
 
 	/* Delete any child interfaces first */
 	list_for_each_entry_safe(cpriv, tcpriv, &priv->child_intfs, list) {
+		/* Stop GC on child */
+		set_bit(IPOIB_STOP_NEIGH_GC, &cpriv->flags);
+		cancel_delayed_work(&cpriv->neigh_reap_task);
 		unregister_netdev(cpriv->dev);
 		ipoib_dev_cleanup(cpriv->dev);
 		free_netdev(cpriv->dev);
@@ -976,6 +1282,8 @@ void ipoib_dev_cleanup(struct net_device *dev)
 
 	priv->rx_ring = NULL;
 	priv->tx_ring = NULL;
+
+	ipoib_neigh_hash_uninit(dev);
 }
 
 static const struct header_ops ipoib_header_ops = {
@@ -990,7 +1298,6 @@ static const struct net_device_ops ipoib_netdev_ops = {
 	.ndo_start_xmit	 	 = ipoib_start_xmit,
 	.ndo_tx_timeout		 = ipoib_timeout,
 	.ndo_set_rx_mode	 = ipoib_set_mcast_list,
-	.ndo_neigh_setup	 = ipoib_neigh_setup_dev,
 };
 
 static void ipoib_setup(struct net_device *dev)
@@ -1039,6 +1346,7 @@ static void ipoib_setup(struct net_device *dev)
 	INIT_WORK(&priv->flush_heavy,   ipoib_ib_dev_flush_heavy);
 	INIT_WORK(&priv->restart_task, ipoib_mcast_restart_task);
 	INIT_DELAYED_WORK(&priv->ah_reap_task, ipoib_reap_ah);
+	INIT_DELAYED_WORK(&priv->neigh_reap_task, ipoib_reap_neigh);
 }
 
 struct ipoib_dev_priv *ipoib_intf_alloc(const char *name)
@@ -1279,6 +1587,9 @@ sysfs_failed:
 
 register_failed:
 	ib_unregister_event_handler(&priv->event_handler);
+	/* Stop GC if started before flush */
+	set_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
+	cancel_delayed_work(&priv->neigh_reap_task);
 	flush_workqueue(ipoib_workqueue);
 
 event_failed:
@@ -1345,6 +1656,9 @@ static void ipoib_remove_one(struct ib_device *device)
 		dev_change_flags(priv->dev, priv->dev->flags & ~IFF_UP);
 		rtnl_unlock();
 
+		/* Stop GC */
+		set_bit(IPOIB_STOP_NEIGH_GC, &priv->flags);
+		cancel_delayed_work(&priv->neigh_reap_task);
 		flush_workqueue(ipoib_workqueue);
 
 		unregister_netdev(priv->dev);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
index 20ebc6fd1bb..13f4aa7593c 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
@@ -69,28 +69,13 @@ struct ipoib_mcast_iter {
 static void ipoib_mcast_free(struct ipoib_mcast *mcast)
 {
 	struct net_device *dev = mcast->dev;
-	struct ipoib_dev_priv *priv = netdev_priv(dev);
-	struct ipoib_neigh *neigh, *tmp;
 	int tx_dropped = 0;
 
 	ipoib_dbg_mcast(netdev_priv(dev), "deleting multicast group %pI6\n",
 			mcast->mcmember.mgid.raw);
 
-	spin_lock_irq(&priv->lock);
-
-	list_for_each_entry_safe(neigh, tmp, &mcast->neigh_list, list) {
-		/*
-		 * It's safe to call ipoib_put_ah() inside priv->lock
-		 * here, because we know that mcast->ah will always
-		 * hold one more reference, so ipoib_put_ah() will
-		 * never do more than decrement the ref count.
-		 */
-		if (neigh->ah)
-			ipoib_put_ah(neigh->ah);
-		ipoib_neigh_free(dev, neigh);
-	}
-
-	spin_unlock_irq(&priv->lock);
+	/* remove all neigh connected to this mcast */
+	ipoib_del_neighs_by_gid(dev, mcast->mcmember.mgid.raw);
 
 	if (mcast->ah)
 		ipoib_put_ah(mcast->ah);
@@ -655,11 +640,12 @@ static int ipoib_mcast_leave(struct net_device *dev, struct ipoib_mcast *mcast)
 	return 0;
 }
 
-void ipoib_mcast_send(struct net_device *dev, void *mgid, struct sk_buff *skb)
+void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct ipoib_mcast *mcast;
 	unsigned long flags;
+	void *mgid = daddr + 4;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
@@ -715,25 +701,25 @@ void ipoib_mcast_send(struct net_device *dev, void *mgid, struct sk_buff *skb)
 
 out:
 	if (mcast && mcast->ah) {
-		struct dst_entry *dst = skb_dst(skb);
-		struct neighbour *n = NULL;
-
-		rcu_read_lock();
-		if (dst)
-			n = dst_get_neighbour_noref(dst);
-		if (n && !*to_ipoib_neigh(n)) {
-			struct ipoib_neigh *neigh = ipoib_neigh_alloc(n,
-								      skb->dev);
+		struct ipoib_neigh *neigh;
 
+		spin_unlock_irqrestore(&priv->lock, flags);
+		neigh = ipoib_neigh_get(dev, daddr);
+		spin_lock_irqsave(&priv->lock, flags);
+		if (!neigh) {
+			spin_unlock_irqrestore(&priv->lock, flags);
+			neigh = ipoib_neigh_alloc(daddr, dev);
+			spin_lock_irqsave(&priv->lock, flags);
 			if (neigh) {
 				kref_get(&mcast->ah->ref);
 				neigh->ah	= mcast->ah;
 				list_add_tail(&neigh->list, &mcast->neigh_list);
 			}
 		}
-		rcu_read_unlock();
 		spin_unlock_irqrestore(&priv->lock, flags);
 		ipoib_send(dev, skb, mcast->ah, IB_MULTICAST_QPN);
+		if (neigh)
+			ipoib_neigh_put(neigh);
 		return;
 	}
 
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index 5f6b7f63cde..7a0ce8d4288 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -1377,10 +1377,14 @@ static int srpt_abort_cmd(struct srpt_send_ioctx *ioctx)
 		break;
 	case SRPT_STATE_NEED_DATA:
 		/* DMA_TO_DEVICE (write) - RDMA read error. */
+
+		/* XXX(hch): this is a horrible layering violation.. */
 		spin_lock_irqsave(&ioctx->cmd.t_state_lock, flags);
 		ioctx->cmd.transport_state |= CMD_T_LUN_STOP;
+		ioctx->cmd.transport_state &= ~CMD_T_ACTIVE;
 		spin_unlock_irqrestore(&ioctx->cmd.t_state_lock, flags);
-		transport_generic_handle_data(&ioctx->cmd);
+
+		complete(&ioctx->cmd.transport_lun_stop_comp);
 		break;
 	case SRPT_STATE_CMD_RSP_SENT:
 		/*
@@ -1463,9 +1467,10 @@ static void srpt_handle_send_comp(struct srpt_rdma_ch *ch,
 /**
  * srpt_handle_rdma_comp() - Process an IB RDMA completion notification.
  *
- * Note: transport_generic_handle_data() is asynchronous so unmapping the
- * data that has been transferred via IB RDMA must be postponed until the
- * check_stop_free() callback.
+ * XXX: what is now target_execute_cmd used to be asynchronous, and unmapping
+ * the data that has been transferred via IB RDMA had to be postponed until the
+ * check_stop_free() callback.  None of this is nessecary anymore and needs to
+ * be cleaned up.
  */
 static void srpt_handle_rdma_comp(struct srpt_rdma_ch *ch,
 				  struct srpt_send_ioctx *ioctx,
@@ -1477,7 +1482,7 @@ static void srpt_handle_rdma_comp(struct srpt_rdma_ch *ch,
 	if (opcode == SRPT_RDMA_READ_LAST) {
 		if (srpt_test_and_set_cmd_state(ioctx, SRPT_STATE_NEED_DATA,
 						SRPT_STATE_DATA_IN))
-			transport_generic_handle_data(&ioctx->cmd);
+			target_execute_cmd(&ioctx->cmd);
 		else
 			printk(KERN_ERR "%s[%d]: wrong state = %d\n", __func__,
 			       __LINE__, srpt_get_cmd_state(ioctx));
diff --git a/drivers/input/ff-memless.c b/drivers/input/ff-memless.c
index 5f558851d64..b107922514f 100644
--- a/drivers/input/ff-memless.c
+++ b/drivers/input/ff-memless.c
@@ -176,7 +176,7 @@ static int apply_envelope(struct ml_effect_state *state, int value,
 			 value, envelope->attack_level);
 		time_from_level = jiffies_to_msecs(now - state->play_at);
 		time_of_envelope = envelope->attack_length;
-		envelope_level = min_t(__s16, envelope->attack_level, 0x7fff);
+		envelope_level = min_t(u16, envelope->attack_level, 0x7fff);
 
 	} else if (envelope->fade_length && effect->replay.length &&
 		   time_after(now,
@@ -184,7 +184,7 @@ static int apply_envelope(struct ml_effect_state *state, int value,
 		   time_before(now, state->stop_at)) {
 		time_from_level = jiffies_to_msecs(state->stop_at - now);
 		time_of_envelope = envelope->fade_length;
-		envelope_level = min_t(__s16, envelope->fade_level, 0x7fff);
+		envelope_level = min_t(u16, envelope->fade_level, 0x7fff);
 	} else
 		return value;
 
diff --git a/drivers/input/input-mt.c b/drivers/input/input-mt.c
index f658086fbbe..70a16c7da8c 100644
--- a/drivers/input/input-mt.c
+++ b/drivers/input/input-mt.c
@@ -135,7 +135,7 @@ EXPORT_SYMBOL(input_mt_report_finger_count);
  */
 void input_mt_report_pointer_emulation(struct input_dev *dev, bool use_count)
 {
-	struct input_mt_slot *oldest = 0;
+	struct input_mt_slot *oldest = NULL;
 	int oldid = dev->trkid;
 	int count = 0;
 	int i;
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index c0e11ecc646..c50fa75416f 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -332,6 +332,16 @@ config KEYBOARD_LOCOMO
 	  To compile this driver as a module, choose M here: the
 	  module will be called locomokbd.
 
+config KEYBOARD_LPC32XX
+	tristate "LPC32XX matrix key scanner support"
+	depends on ARCH_LPC32XX && OF
+	help
+	  Say Y here if you want to use NXP LPC32XX SoC key scanner interface,
+	  connected to a key matrix.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called lpc32xx-keys.
+
 config KEYBOARD_MAPLE
 	tristate "Maple bus keyboard"
 	depends on SH_DREAMCAST && MAPLE
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index b03b02456a8..44e76002f54 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -26,6 +26,7 @@ obj-$(CONFIG_KEYBOARD_LKKBD)		+= lkkbd.o
 obj-$(CONFIG_KEYBOARD_LM8323)		+= lm8323.o
 obj-$(CONFIG_KEYBOARD_LM8333)		+= lm8333.o
 obj-$(CONFIG_KEYBOARD_LOCOMO)		+= locomokbd.o
+obj-$(CONFIG_KEYBOARD_LPC32XX)		+= lpc32xx-keys.o
 obj-$(CONFIG_KEYBOARD_MAPLE)		+= maple_keyb.o
 obj-$(CONFIG_KEYBOARD_MATRIX)		+= matrix_keypad.o
 obj-$(CONFIG_KEYBOARD_MAX7359)		+= max7359_keypad.o
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 62bfce468f9..cbb1add43d5 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -559,7 +559,6 @@ static int gpio_keys_get_devtree_pdata(struct device *dev,
 	pdata->rep = !!of_get_property(node, "autorepeat", NULL);
 
 	/* First count the subnodes */
-	pdata->nbuttons = 0;
 	pp = NULL;
 	while ((pp = of_get_next_child(node, pp)))
 		pdata->nbuttons++;
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index 6ee7421e232..ff4c0a87a25 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -378,20 +378,24 @@ static void imx_keypad_close(struct input_dev *dev)
 	imx_keypad_inhibit(keypad);
 
 	/* Disable clock unit */
-	clk_disable(keypad->clk);
+	clk_disable_unprepare(keypad->clk);
 }
 
 static int imx_keypad_open(struct input_dev *dev)
 {
 	struct imx_keypad *keypad = input_get_drvdata(dev);
+	int error;
 
 	dev_dbg(&dev->dev, ">%s\n", __func__);
 
+	/* Enable the kpp clock */
+	error = clk_prepare_enable(keypad->clk);
+	if (error)
+		return error;
+
 	/* We became active from now */
 	keypad->enabled = true;
 
-	/* Enable the kpp clock */
-	clk_enable(keypad->clk);
 	imx_keypad_config(keypad);
 
 	/* Sanity control, not all the rows must be actived now. */
@@ -467,7 +471,7 @@ static int __devinit imx_keypad_probe(struct platform_device *pdev)
 		goto failed_free_priv;
 	}
 
-	keypad->clk = clk_get(&pdev->dev, "kpp");
+	keypad->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(keypad->clk)) {
 		dev_err(&pdev->dev, "failed to get keypad clock\n");
 		error = PTR_ERR(keypad->clk);
@@ -581,7 +585,7 @@ static int imx_kbd_suspend(struct device *dev)
 	mutex_lock(&input_dev->mutex);
 
 	if (input_dev->users)
-		clk_disable(kbd->clk);
+		clk_disable_unprepare(kbd->clk);
 
 	mutex_unlock(&input_dev->mutex);
 
@@ -596,18 +600,23 @@ static int imx_kbd_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct imx_keypad *kbd = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = kbd->input_dev;
+	int ret = 0;
 
 	if (device_may_wakeup(&pdev->dev))
 		disable_irq_wake(kbd->irq);
 
 	mutex_lock(&input_dev->mutex);
 
-	if (input_dev->users)
-		clk_enable(kbd->clk);
+	if (input_dev->users) {
+		ret = clk_prepare_enable(kbd->clk);
+		if (ret)
+			goto err_clk;
+	}
 
+err_clk:
 	mutex_unlock(&input_dev->mutex);
 
-	return 0;
+	return ret;
 }
 #endif
 
diff --git a/drivers/input/keyboard/lm8333.c b/drivers/input/keyboard/lm8333.c
index ca168a6679d..081fd9effa8 100644
--- a/drivers/input/keyboard/lm8333.c
+++ b/drivers/input/keyboard/lm8333.c
@@ -91,7 +91,7 @@ static void lm8333_key_handler(struct lm8333 *lm8333)
 		return;
 	}
 
-	for (i = 0; keys[i] && i < LM8333_FIFO_TRANSFER_SIZE; i++) {
+	for (i = 0; i < LM8333_FIFO_TRANSFER_SIZE && keys[i]; i++) {
 		pressed = keys[i] & 0x80;
 		code = keys[i] & 0x7f;
 
diff --git a/drivers/input/keyboard/lpc32xx-keys.c b/drivers/input/keyboard/lpc32xx-keys.c
new file mode 100644
index 00000000000..dd786c8a758
--- /dev/null
+++ b/drivers/input/keyboard/lpc32xx-keys.c
@@ -0,0 +1,394 @@
+/*
+ * NXP LPC32xx SoC Key Scan Interface
+ *
+ * Authors:
+ *    Kevin Wells <kevin.wells@nxp.com>
+ *    Roland Stigge <stigge@antcom.de>
+ *
+ * Copyright (C) 2010 NXP Semiconductors
+ * Copyright (C) 2012 Roland Stigge
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ *
+ * This controller supports square key matrices from 1x1 up to 8x8
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/input/matrix_keypad.h>
+
+#define DRV_NAME				"lpc32xx_keys"
+
+/*
+ * Key scanner register offsets
+ */
+#define LPC32XX_KS_DEB(x)			((x) + 0x00)
+#define LPC32XX_KS_STATE_COND(x)		((x) + 0x04)
+#define LPC32XX_KS_IRQ(x)			((x) + 0x08)
+#define LPC32XX_KS_SCAN_CTL(x)			((x) + 0x0C)
+#define LPC32XX_KS_FAST_TST(x)			((x) + 0x10)
+#define LPC32XX_KS_MATRIX_DIM(x)		((x) + 0x14) /* 1..8 */
+#define LPC32XX_KS_DATA(x, y)			((x) + 0x40 + ((y) << 2))
+
+#define LPC32XX_KSCAN_DEB_NUM_DEB_PASS(n)	((n) & 0xFF)
+
+#define LPC32XX_KSCAN_SCOND_IN_IDLE		0x0
+#define LPC32XX_KSCAN_SCOND_IN_SCANONCE		0x1
+#define LPC32XX_KSCAN_SCOND_IN_IRQGEN		0x2
+#define LPC32XX_KSCAN_SCOND_IN_SCAN_MATRIX	0x3
+
+#define LPC32XX_KSCAN_IRQ_PENDING_CLR		0x1
+
+#define LPC32XX_KSCAN_SCTRL_SCAN_DELAY(n)	((n) & 0xFF)
+
+#define LPC32XX_KSCAN_FTST_FORCESCANONCE	0x1
+#define LPC32XX_KSCAN_FTST_USE32K_CLK		0x2
+
+#define LPC32XX_KSCAN_MSEL_SELECT(n)		((n) & 0xF)
+
+struct lpc32xx_kscan_drv {
+	struct input_dev *input;
+	struct clk *clk;
+	struct resource *iores;
+	void __iomem *kscan_base;
+	unsigned int irq;
+
+	u32 matrix_sz;		/* Size of matrix in XxY, ie. 3 = 3x3 */
+	u32 deb_clks;		/* Debounce clocks (based on 32KHz clock) */
+	u32 scan_delay;		/* Scan delay (based on 32KHz clock) */
+
+	unsigned short *keymap;	/* Pointer to key map for the scan matrix */
+	unsigned int row_shift;
+
+	u8 lastkeystates[8];
+};
+
+static void lpc32xx_mod_states(struct lpc32xx_kscan_drv *kscandat, int col)
+{
+	struct input_dev *input = kscandat->input;
+	unsigned row, changed, scancode, keycode;
+	u8 key;
+
+	key = readl(LPC32XX_KS_DATA(kscandat->kscan_base, col));
+	changed = key ^ kscandat->lastkeystates[col];
+	kscandat->lastkeystates[col] = key;
+
+	for (row = 0; changed; row++, changed >>= 1) {
+		if (changed & 1) {
+			/* Key state changed, signal an event */
+			scancode = MATRIX_SCAN_CODE(row, col,
+						    kscandat->row_shift);
+			keycode = kscandat->keymap[scancode];
+			input_event(input, EV_MSC, MSC_SCAN, scancode);
+			input_report_key(input, keycode, key & (1 << row));
+		}
+	}
+}
+
+static irqreturn_t lpc32xx_kscan_irq(int irq, void *dev_id)
+{
+	struct lpc32xx_kscan_drv *kscandat = dev_id;
+	int i;
+
+	for (i = 0; i < kscandat->matrix_sz; i++)
+		lpc32xx_mod_states(kscandat, i);
+
+	writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
+
+	input_sync(kscandat->input);
+
+	return IRQ_HANDLED;
+}
+
+static int lpc32xx_kscan_open(struct input_dev *dev)
+{
+	struct lpc32xx_kscan_drv *kscandat = input_get_drvdata(dev);
+	int error;
+
+	error = clk_prepare_enable(kscandat->clk);
+	if (error)
+		return error;
+
+	writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
+
+	return 0;
+}
+
+static void lpc32xx_kscan_close(struct input_dev *dev)
+{
+	struct lpc32xx_kscan_drv *kscandat = input_get_drvdata(dev);
+
+	writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
+	clk_disable_unprepare(kscandat->clk);
+}
+
+static int __devinit lpc32xx_parse_dt(struct device *dev,
+				      struct lpc32xx_kscan_drv *kscandat)
+{
+	struct device_node *np = dev->of_node;
+	u32 rows = 0, columns = 0;
+
+	of_property_read_u32(np, "keypad,num-rows", &rows);
+	of_property_read_u32(np, "keypad,num-columns", &columns);
+	if (!rows || rows != columns) {
+		dev_err(dev,
+			"rows and columns must be specified and be equal!\n");
+		return -EINVAL;
+	}
+
+	kscandat->matrix_sz = rows;
+	kscandat->row_shift = get_count_order(columns);
+
+	of_property_read_u32(np, "nxp,debounce-delay-ms", &kscandat->deb_clks);
+	of_property_read_u32(np, "nxp,scan-delay-ms", &kscandat->scan_delay);
+	if (!kscandat->deb_clks || !kscandat->scan_delay) {
+		dev_err(dev, "debounce or scan delay not specified\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __devinit lpc32xx_kscan_probe(struct platform_device *pdev)
+{
+	struct lpc32xx_kscan_drv *kscandat;
+	struct input_dev *input;
+	struct resource *res;
+	size_t keymap_size;
+	int error;
+	int irq;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get platform I/O memory\n");
+		return -EINVAL;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0 || irq >= NR_IRQS) {
+		dev_err(&pdev->dev, "failed to get platform irq\n");
+		return -EINVAL;
+	}
+
+	kscandat = kzalloc(sizeof(struct lpc32xx_kscan_drv), GFP_KERNEL);
+	if (!kscandat) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	error = lpc32xx_parse_dt(&pdev->dev, kscandat);
+	if (error) {
+		dev_err(&pdev->dev, "failed to parse device tree\n");
+		goto err_free_mem;
+	}
+
+	keymap_size = sizeof(kscandat->keymap[0]) *
+				(kscandat->matrix_sz << kscandat->row_shift);
+	kscandat->keymap = kzalloc(keymap_size, GFP_KERNEL);
+	if (!kscandat->keymap) {
+		dev_err(&pdev->dev, "could not allocate memory for keymap\n");
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	kscandat->input = input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		error = -ENOMEM;
+		goto err_free_keymap;
+	}
+
+	/* Setup key input */
+	input->name		= pdev->name;
+	input->phys		= "lpc32xx/input0";
+	input->id.vendor	= 0x0001;
+	input->id.product	= 0x0001;
+	input->id.version	= 0x0100;
+	input->open		= lpc32xx_kscan_open;
+	input->close		= lpc32xx_kscan_close;
+	input->dev.parent	= &pdev->dev;
+
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+
+	error = matrix_keypad_build_keymap(NULL, NULL,
+					   kscandat->matrix_sz,
+					   kscandat->matrix_sz,
+					   kscandat->keymap, kscandat->input);
+	if (error) {
+		dev_err(&pdev->dev, "failed to build keymap\n");
+		goto err_free_input;
+	}
+
+	input_set_drvdata(kscandat->input, kscandat);
+
+	kscandat->iores = request_mem_region(res->start, resource_size(res),
+					     pdev->name);
+	if (!kscandat->iores) {
+		dev_err(&pdev->dev, "failed to request I/O memory\n");
+		error = -EBUSY;
+		goto err_free_input;
+	}
+
+	kscandat->kscan_base = ioremap(kscandat->iores->start,
+				       resource_size(kscandat->iores));
+	if (!kscandat->kscan_base) {
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
+		error = -EBUSY;
+		goto err_release_memregion;
+	}
+
+	/* Get the key scanner clock */
+	kscandat->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(kscandat->clk)) {
+		dev_err(&pdev->dev, "failed to get clock\n");
+		error = PTR_ERR(kscandat->clk);
+		goto err_unmap;
+	}
+
+	/* Configure the key scanner */
+	error = clk_prepare_enable(kscandat->clk);
+	if (error)
+		goto err_clk_put;
+
+	writel(kscandat->deb_clks, LPC32XX_KS_DEB(kscandat->kscan_base));
+	writel(kscandat->scan_delay, LPC32XX_KS_SCAN_CTL(kscandat->kscan_base));
+	writel(LPC32XX_KSCAN_FTST_USE32K_CLK,
+	       LPC32XX_KS_FAST_TST(kscandat->kscan_base));
+	writel(kscandat->matrix_sz,
+	       LPC32XX_KS_MATRIX_DIM(kscandat->kscan_base));
+	writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
+	clk_disable_unprepare(kscandat->clk);
+
+	error = request_irq(irq, lpc32xx_kscan_irq, 0, pdev->name, kscandat);
+	if (error) {
+		dev_err(&pdev->dev, "failed to request irq\n");
+		goto err_clk_put;
+	}
+
+	error = input_register_device(kscandat->input);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		goto err_free_irq;
+	}
+
+	platform_set_drvdata(pdev, kscandat);
+	return 0;
+
+err_free_irq:
+	free_irq(irq, kscandat);
+err_clk_put:
+	clk_put(kscandat->clk);
+err_unmap:
+	iounmap(kscandat->kscan_base);
+err_release_memregion:
+	release_mem_region(kscandat->iores->start,
+			   resource_size(kscandat->iores));
+err_free_input:
+	input_free_device(kscandat->input);
+err_free_keymap:
+	kfree(kscandat->keymap);
+err_free_mem:
+	kfree(kscandat);
+
+	return error;
+}
+
+static int __devexit lpc32xx_kscan_remove(struct platform_device *pdev)
+{
+	struct lpc32xx_kscan_drv *kscandat = platform_get_drvdata(pdev);
+
+	free_irq(platform_get_irq(pdev, 0), kscandat);
+	clk_put(kscandat->clk);
+	iounmap(kscandat->kscan_base);
+	release_mem_region(kscandat->iores->start,
+			   resource_size(kscandat->iores));
+	input_unregister_device(kscandat->input);
+	kfree(kscandat->keymap);
+	kfree(kscandat);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int lpc32xx_kscan_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lpc32xx_kscan_drv *kscandat = platform_get_drvdata(pdev);
+	struct input_dev *input = kscandat->input;
+
+	mutex_lock(&input->mutex);
+
+	if (input->users) {
+		/* Clear IRQ and disable clock */
+		writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
+		clk_disable_unprepare(kscandat->clk);
+	}
+
+	mutex_unlock(&input->mutex);
+	return 0;
+}
+
+static int lpc32xx_kscan_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lpc32xx_kscan_drv *kscandat = platform_get_drvdata(pdev);
+	struct input_dev *input = kscandat->input;
+	int retval = 0;
+
+	mutex_lock(&input->mutex);
+
+	if (input->users) {
+		/* Enable clock and clear IRQ */
+		retval = clk_prepare_enable(kscandat->clk);
+		if (retval == 0)
+			writel(1, LPC32XX_KS_IRQ(kscandat->kscan_base));
+	}
+
+	mutex_unlock(&input->mutex);
+	return retval;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(lpc32xx_kscan_pm_ops, lpc32xx_kscan_suspend,
+			 lpc32xx_kscan_resume);
+
+static const struct of_device_id lpc32xx_kscan_match[] = {
+	{ .compatible = "nxp,lpc3220-key" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_kscan_match);
+
+static struct platform_driver lpc32xx_kscan_driver = {
+	.probe		= lpc32xx_kscan_probe,
+	.remove		= __devexit_p(lpc32xx_kscan_remove),
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+		.pm	= &lpc32xx_kscan_pm_ops,
+		.of_match_table = of_match_ptr(lpc32xx_kscan_match),
+	}
+};
+
+module_platform_driver(lpc32xx_kscan_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Kevin Wells <kevin.wells@nxp.com>");
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("Key scanner driver for LPC32XX devices");
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 4ea4341a68c..a880e741420 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -49,6 +49,7 @@
 #define SKE_ASR3	0x2C
 
 #define SKE_NUM_ASRX_REGISTERS	(4)
+#define	KEY_PRESSED_DELAY	10
 
 /**
  * struct ske_keypad  - data structure used by keypad driver
@@ -92,7 +93,7 @@ static void ske_keypad_set_bits(struct ske_keypad *keypad, u16 addr,
 static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 {
 	u32 value;
-	int timeout = 50;
+	int timeout = keypad->board->debounce_ms;
 
 	/* check SKE_RIS to be 0 */
 	while ((readl(keypad->reg_base + SKE_RIS) != 0x00000000) && timeout--)
@@ -135,12 +136,37 @@ static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 	return 0;
 }
 
-static void ske_keypad_read_data(struct ske_keypad *keypad)
+static void ske_keypad_report(struct ske_keypad *keypad, u8 status, int col)
 {
+	int row = 0, code, pos;
 	struct input_dev *input = keypad->input;
-	u16 status;
-	int col = 0, row = 0, code;
-	int ske_asr, ske_ris, key_pressed, i;
+	u32 ske_ris;
+	int key_pressed;
+	int num_of_rows;
+
+	/* find out the row */
+	num_of_rows = hweight8(status);
+	do {
+		pos = __ffs(status);
+		row = pos;
+		status &= ~(1 << pos);
+
+		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
+		ske_ris = readl(keypad->reg_base + SKE_RIS);
+		key_pressed = ske_ris & SKE_KPRISA;
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, keypad->keymap[code], key_pressed);
+		input_sync(input);
+		num_of_rows--;
+	} while (num_of_rows);
+}
+
+static void ske_keypad_read_data(struct ske_keypad *keypad)
+{
+	u8 status;
+	int col = 0;
+	int ske_asr, i;
 
 	/*
 	 * Read the auto scan registers
@@ -154,44 +180,38 @@ static void ske_keypad_read_data(struct ske_keypad *keypad)
 		if (!ske_asr)
 			continue;
 
-		/* now that ASRx is zero, find out the column x and row y*/
-		if (ske_asr & 0xff) {
+		/* now that ASRx is zero, find out the coloumn x and row y */
+		status = ske_asr & 0xff;
+		if (status) {
 			col = i * 2;
-			status = ske_asr & 0xff;
-		} else {
+			ske_keypad_report(keypad, status, col);
+		}
+		status = (ske_asr & 0xff00) >> 8;
+		if (status) {
 			col = (i * 2) + 1;
-			status = (ske_asr & 0xff00) >> 8;
+			ske_keypad_report(keypad, status, col);
 		}
-
-		/* find out the row */
-		row = __ffs(status);
-
-		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
-		ske_ris = readl(keypad->reg_base + SKE_RIS);
-		key_pressed = ske_ris & SKE_KPRISA;
-
-		input_event(input, EV_MSC, MSC_SCAN, code);
-		input_report_key(input, keypad->keymap[code], key_pressed);
-		input_sync(input);
 	}
 }
 
 static irqreturn_t ske_keypad_irq(int irq, void *dev_id)
 {
 	struct ske_keypad *keypad = dev_id;
-	int retries = 20;
+	int timeout = keypad->board->debounce_ms;
 
 	/* disable auto scan interrupt; mask the interrupt generated */
 	ske_keypad_set_bits(keypad, SKE_IMSC, ~SKE_KPIMA, 0x0);
 	ske_keypad_set_bits(keypad, SKE_ICR, 0x0, SKE_KPICA);
 
-	while ((readl(keypad->reg_base + SKE_CR) & SKE_KPASON) && --retries)
-		msleep(5);
+	while ((readl(keypad->reg_base + SKE_CR) & SKE_KPASON) && --timeout)
+		cpu_relax();
 
-	if (retries) {
-		/* SKEx registers are stable and can be read */
-		ske_keypad_read_data(keypad);
-	}
+	/* SKEx registers are stable and can be read */
+	ske_keypad_read_data(keypad);
+
+	/* wait until raw interrupt is clear */
+	while ((readl(keypad->reg_base + SKE_RIS)) && --timeout)
+		msleep(KEY_PRESSED_DELAY);
 
 	/* enable auto scan interrupts */
 	ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index aed5f6999ce..c05f98c4141 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -27,6 +27,7 @@
 #include <linux/platform_device.h>
 #include <linux/errno.h>
 #include <linux/io.h>
+#include <linux/of.h>
 #include <linux/input.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
@@ -84,8 +85,9 @@ struct omap4_keypad {
 	u32 reg_offset;
 	u32 irqreg_offset;
 	unsigned int row_shift;
+	bool no_autorepeat;
 	unsigned char key_state[8];
-	unsigned short keymap[];
+	unsigned short *keymap;
 };
 
 static int kbd_readl(struct omap4_keypad *keypad_data, u32 offset)
@@ -208,25 +210,51 @@ static void omap4_keypad_close(struct input_dev *input)
 	pm_runtime_put_sync(input->dev.parent);
 }
 
+#ifdef CONFIG_OF
+static int __devinit omap4_keypad_parse_dt(struct device *dev,
+					   struct omap4_keypad *keypad_data)
+{
+	struct device_node *np = dev->of_node;
+
+	if (!np) {
+		dev_err(dev, "missing DT data");
+		return -EINVAL;
+	}
+
+	of_property_read_u32(np, "keypad,num-rows", &keypad_data->rows);
+	of_property_read_u32(np, "keypad,num-columns", &keypad_data->cols);
+	if (!keypad_data->rows || !keypad_data->cols) {
+		dev_err(dev, "number of keypad rows/columns not specified\n");
+		return -EINVAL;
+	}
+
+	if (of_get_property(np, "linux,input-no-autorepeat", NULL))
+		keypad_data->no_autorepeat = true;
+
+	return 0;
+}
+#else
+static inline int omap4_keypad_parse_dt(struct device *dev,
+					struct omap4_keypad *keypad_data)
+{
+	return -ENOSYS;
+}
+#endif
+
 static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 {
-	const struct omap4_keypad_platform_data *pdata;
+	const struct omap4_keypad_platform_data *pdata =
+				dev_get_platdata(&pdev->dev);
+	const struct matrix_keymap_data *keymap_data =
+				pdata ? pdata->keymap_data : NULL;
 	struct omap4_keypad *keypad_data;
 	struct input_dev *input_dev;
 	struct resource *res;
-	resource_size_t size;
-	unsigned int row_shift, max_keys;
+	unsigned int max_keys;
 	int rev;
 	int irq;
 	int error;
 
-	/* platform data */
-	pdata = pdev->dev.platform_data;
-	if (!pdata) {
-		dev_err(&pdev->dev, "no platform data defined\n");
-		return -EINVAL;
-	}
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no base address specified\n");
@@ -239,25 +267,24 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	if (!pdata->keymap_data) {
-		dev_err(&pdev->dev, "no keymap data defined\n");
-		return -EINVAL;
-	}
-
-	row_shift = get_count_order(pdata->cols);
-	max_keys = pdata->rows << row_shift;
-
-	keypad_data = kzalloc(sizeof(struct omap4_keypad) +
-				max_keys * sizeof(keypad_data->keymap[0]),
-			      GFP_KERNEL);
+	keypad_data = kzalloc(sizeof(struct omap4_keypad), GFP_KERNEL);
 	if (!keypad_data) {
 		dev_err(&pdev->dev, "keypad_data memory allocation failed\n");
 		return -ENOMEM;
 	}
 
-	size = resource_size(res);
+	keypad_data->irq = irq;
+
+	if (pdata) {
+		keypad_data->rows = pdata->rows;
+		keypad_data->cols = pdata->cols;
+	} else {
+		error = omap4_keypad_parse_dt(&pdev->dev, keypad_data);
+		if (error)
+			return error;
+	}
 
-	res = request_mem_region(res->start, size, pdev->name);
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
 	if (!res) {
 		dev_err(&pdev->dev, "can't request mem region\n");
 		error = -EBUSY;
@@ -271,15 +298,11 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 		goto err_release_mem;
 	}
 
-	keypad_data->irq = irq;
-	keypad_data->row_shift = row_shift;
-	keypad_data->rows = pdata->rows;
-	keypad_data->cols = pdata->cols;
 
 	/*
-	* Enable clocks for the keypad module so that we can read
-	* revision register.
-	*/
+	 * Enable clocks for the keypad module so that we can read
+	 * revision register.
+	 */
 	pm_runtime_enable(&pdev->dev);
 	error = pm_runtime_get_sync(&pdev->dev);
 	if (error) {
@@ -322,19 +345,30 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 	input_dev->open = omap4_keypad_open;
 	input_dev->close = omap4_keypad_close;
 
-	error = matrix_keypad_build_keymap(pdata->keymap_data, NULL,
-					   pdata->rows, pdata->cols,
+	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
+	if (!keypad_data->no_autorepeat)
+		__set_bit(EV_REP, input_dev->evbit);
+
+	input_set_drvdata(input_dev, keypad_data);
+
+	keypad_data->row_shift = get_count_order(keypad_data->cols);
+	max_keys = keypad_data->rows << keypad_data->row_shift;
+	keypad_data->keymap = kzalloc(max_keys * sizeof(keypad_data->keymap[0]),
+				      GFP_KERNEL);
+	if (!keypad_data->keymap) {
+		dev_err(&pdev->dev, "Not enough memory for keymap\n");
+		error = -ENOMEM;
+		goto err_free_input;
+	}
+
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   keypad_data->rows, keypad_data->cols,
 					   keypad_data->keymap, input_dev);
 	if (error) {
 		dev_err(&pdev->dev, "failed to build keymap\n");
-		goto err_free_input;
+		goto err_free_keymap;
 	}
 
-	__set_bit(EV_REP, input_dev->evbit);
-	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
-
-	input_set_drvdata(input_dev, keypad_data);
-
 	error = request_irq(keypad_data->irq, omap4_keypad_interrupt,
 			     IRQF_TRIGGER_RISING,
 			     "omap4-keypad", keypad_data);
@@ -357,6 +391,8 @@ static int __devinit omap4_keypad_probe(struct platform_device *pdev)
 err_pm_disable:
 	pm_runtime_disable(&pdev->dev);
 	free_irq(keypad_data->irq, keypad_data);
+err_free_keymap:
+	kfree(keypad_data->keymap);
 err_free_input:
 	input_free_device(input_dev);
 err_pm_put_sync:
@@ -364,7 +400,7 @@ err_pm_put_sync:
 err_unmap:
 	iounmap(keypad_data->base);
 err_release_mem:
-	release_mem_region(res->start, size);
+	release_mem_region(res->start, resource_size(res));
 err_free_keypad:
 	kfree(keypad_data);
 	return error;
@@ -386,18 +422,29 @@ static int __devexit omap4_keypad_remove(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
 
+	kfree(keypad_data->keymap);
 	kfree(keypad_data);
+
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id omap_keypad_dt_match[] = {
+	{ .compatible = "ti,omap4-keypad" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_keypad_dt_match);
+#endif
+
 static struct platform_driver omap4_keypad_driver = {
 	.probe		= omap4_keypad_probe,
 	.remove		= __devexit_p(omap4_keypad_remove),
 	.driver		= {
 		.name	= "omap4-keypad",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(omap_keypad_dt_match),
 	},
 };
 module_platform_driver(omap4_keypad_driver);
diff --git a/drivers/input/keyboard/spear-keyboard.c b/drivers/input/keyboard/spear-keyboard.c
index 6f287f7e153..72ef01be336 100644
--- a/drivers/input/keyboard/spear-keyboard.c
+++ b/drivers/input/keyboard/spear-keyboard.c
@@ -27,33 +27,31 @@
 #include <plat/keyboard.h>
 
 /* Keyboard Registers */
-#define MODE_REG	0x00	/* 16 bit reg */
-#define STATUS_REG	0x0C	/* 2 bit reg */
-#define DATA_REG	0x10	/* 8 bit reg */
+#define MODE_CTL_REG	0x00
+#define STATUS_REG	0x0C
+#define DATA_REG	0x10
 #define INTR_MASK	0x54
 
 /* Register Values */
-/*
- * pclk freq mask = (APB FEQ -1)= 82 MHZ.Programme bit 15-9 in mode
- * control register as 1010010(82MHZ)
- */
-#define PCLK_FREQ_MSK	0xA400	/* 82 MHz */
-#define START_SCAN	0x0100
-#define SCAN_RATE_10	0x0000
-#define SCAN_RATE_20	0x0004
-#define SCAN_RATE_40	0x0008
-#define SCAN_RATE_80	0x000C
-#define MODE_KEYBOARD	0x0002
-#define DATA_AVAIL	0x2
-
-#define KEY_MASK	0xFF000000
-#define KEY_VALUE	0x00FFFFFF
-#define ROW_MASK	0xF0
-#define COLUMN_MASK	0x0F
 #define NUM_ROWS	16
 #define NUM_COLS	16
+#define MODE_CTL_PCLK_FREQ_SHIFT	9
+#define MODE_CTL_PCLK_FREQ_MSK		0x7F
+
+#define MODE_CTL_KEYBOARD	(0x2 << 0)
+#define MODE_CTL_SCAN_RATE_10	(0x0 << 2)
+#define MODE_CTL_SCAN_RATE_20	(0x1 << 2)
+#define MODE_CTL_SCAN_RATE_40	(0x2 << 2)
+#define MODE_CTL_SCAN_RATE_80	(0x3 << 2)
+#define MODE_CTL_KEYNUM_SHIFT	6
+#define MODE_CTL_START_SCAN	(0x1 << 8)
 
-#define KEY_MATRIX_SHIFT	6
+#define STATUS_DATA_AVAIL	(0x1 << 1)
+
+#define DATA_ROW_MASK		0xF0
+#define DATA_COLUMN_MASK	0x0F
+
+#define ROW_SHIFT		4
 
 struct spear_kbd {
 	struct input_dev *input;
@@ -65,6 +63,8 @@ struct spear_kbd {
 	unsigned short last_key;
 	unsigned short keycodes[NUM_ROWS * NUM_COLS];
 	bool rep;
+	unsigned int suspended_rate;
+	u32 mode_ctl_reg;
 };
 
 static irqreturn_t spear_kbd_interrupt(int irq, void *dev_id)
@@ -72,10 +72,10 @@ static irqreturn_t spear_kbd_interrupt(int irq, void *dev_id)
 	struct spear_kbd *kbd = dev_id;
 	struct input_dev *input = kbd->input;
 	unsigned int key;
-	u8 sts, val;
+	u32 sts, val;
 
-	sts = readb(kbd->io_base + STATUS_REG);
-	if (!(sts & DATA_AVAIL))
+	sts = readl_relaxed(kbd->io_base + STATUS_REG);
+	if (!(sts & STATUS_DATA_AVAIL))
 		return IRQ_NONE;
 
 	if (kbd->last_key != KEY_RESERVED) {
@@ -84,7 +84,8 @@ static irqreturn_t spear_kbd_interrupt(int irq, void *dev_id)
 	}
 
 	/* following reads active (row, col) pair */
-	val = readb(kbd->io_base + DATA_REG);
+	val = readl_relaxed(kbd->io_base + DATA_REG) &
+		(DATA_ROW_MASK | DATA_COLUMN_MASK);
 	key = kbd->keycodes[val];
 
 	input_event(input, EV_MSC, MSC_SCAN, val);
@@ -94,7 +95,7 @@ static irqreturn_t spear_kbd_interrupt(int irq, void *dev_id)
 	kbd->last_key = key;
 
 	/* clear interrupt */
-	writeb(0, kbd->io_base + STATUS_REG);
+	writel_relaxed(0, kbd->io_base + STATUS_REG);
 
 	return IRQ_HANDLED;
 }
@@ -103,7 +104,7 @@ static int spear_kbd_open(struct input_dev *dev)
 {
 	struct spear_kbd *kbd = input_get_drvdata(dev);
 	int error;
-	u16 val;
+	u32 val;
 
 	kbd->last_key = KEY_RESERVED;
 
@@ -111,16 +112,20 @@ static int spear_kbd_open(struct input_dev *dev)
 	if (error)
 		return error;
 
+	/* keyboard rate to be programmed is input clock (in MHz) - 1 */
+	val = clk_get_rate(kbd->clk) / 1000000 - 1;
+	val = (val & MODE_CTL_PCLK_FREQ_MSK) << MODE_CTL_PCLK_FREQ_SHIFT;
+
 	/* program keyboard */
-	val = SCAN_RATE_80 | MODE_KEYBOARD | PCLK_FREQ_MSK |
-		(kbd->mode << KEY_MATRIX_SHIFT);
-	writew(val, kbd->io_base + MODE_REG);
-	writeb(1, kbd->io_base + STATUS_REG);
+	val = MODE_CTL_SCAN_RATE_80 | MODE_CTL_KEYBOARD | val |
+		(kbd->mode << MODE_CTL_KEYNUM_SHIFT);
+	writel_relaxed(val, kbd->io_base + MODE_CTL_REG);
+	writel_relaxed(1, kbd->io_base + STATUS_REG);
 
 	/* start key scan */
-	val = readw(kbd->io_base + MODE_REG);
-	val |= START_SCAN;
-	writew(val, kbd->io_base + MODE_REG);
+	val = readl_relaxed(kbd->io_base + MODE_CTL_REG);
+	val |= MODE_CTL_START_SCAN;
+	writel_relaxed(val, kbd->io_base + MODE_CTL_REG);
 
 	return 0;
 }
@@ -128,12 +133,12 @@ static int spear_kbd_open(struct input_dev *dev)
 static void spear_kbd_close(struct input_dev *dev)
 {
 	struct spear_kbd *kbd = input_get_drvdata(dev);
-	u16 val;
+	u32 val;
 
 	/* stop key scan */
-	val = readw(kbd->io_base + MODE_REG);
-	val &= ~START_SCAN;
-	writew(val, kbd->io_base + MODE_REG);
+	val = readl_relaxed(kbd->io_base + MODE_CTL_REG);
+	val &= ~MODE_CTL_START_SCAN;
+	writel_relaxed(val, kbd->io_base + MODE_CTL_REG);
 
 	clk_disable(kbd->clk);
 
@@ -146,7 +151,7 @@ static int __devinit spear_kbd_parse_dt(struct platform_device *pdev,
 {
 	struct device_node *np = pdev->dev.of_node;
 	int error;
-	u32 val;
+	u32 val, suspended_rate;
 
 	if (!np) {
 		dev_err(&pdev->dev, "Missing DT data\n");
@@ -156,6 +161,9 @@ static int __devinit spear_kbd_parse_dt(struct platform_device *pdev,
 	if (of_property_read_bool(np, "autorepeat"))
 		kbd->rep = true;
 
+	if (of_property_read_u32(np, "suspended_rate", &suspended_rate))
+		kbd->suspended_rate = suspended_rate;
+
 	error = of_property_read_u32(np, "st,mode", &val);
 	if (error) {
 		dev_err(&pdev->dev, "DT: Invalid or missing mode\n");
@@ -213,6 +221,7 @@ static int __devinit spear_kbd_probe(struct platform_device *pdev)
 	} else {
 		kbd->mode = pdata->mode;
 		kbd->rep = pdata->rep;
+		kbd->suspended_rate = pdata->suspended_rate;
 	}
 
 	kbd->res = request_mem_region(res->start, resource_size(res),
@@ -302,7 +311,7 @@ static int __devexit spear_kbd_remove(struct platform_device *pdev)
 	release_mem_region(kbd->res->start, resource_size(kbd->res));
 	kfree(kbd);
 
-	device_init_wakeup(&pdev->dev, 1);
+	device_init_wakeup(&pdev->dev, 0);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
@@ -314,15 +323,48 @@ static int spear_kbd_suspend(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct spear_kbd *kbd = platform_get_drvdata(pdev);
 	struct input_dev *input_dev = kbd->input;
+	unsigned int rate = 0, mode_ctl_reg, val;
 
 	mutex_lock(&input_dev->mutex);
 
-	if (input_dev->users)
-		clk_enable(kbd->clk);
+	/* explicitly enable clock as we may program device */
+	clk_enable(kbd->clk);
 
-	if (device_may_wakeup(&pdev->dev))
+	mode_ctl_reg = readl_relaxed(kbd->io_base + MODE_CTL_REG);
+
+	if (device_may_wakeup(&pdev->dev)) {
 		enable_irq_wake(kbd->irq);
 
+		/*
+		 * reprogram the keyboard operating frequency as on some
+		 * platform it may change during system suspended
+		 */
+		if (kbd->suspended_rate)
+			rate = kbd->suspended_rate / 1000000 - 1;
+		else
+			rate = clk_get_rate(kbd->clk) / 1000000 - 1;
+
+		val = mode_ctl_reg &
+			~(MODE_CTL_PCLK_FREQ_MSK << MODE_CTL_PCLK_FREQ_SHIFT);
+		val |= (rate & MODE_CTL_PCLK_FREQ_MSK)
+			<< MODE_CTL_PCLK_FREQ_SHIFT;
+		writel_relaxed(val, kbd->io_base + MODE_CTL_REG);
+
+	} else {
+		if (input_dev->users) {
+			writel_relaxed(mode_ctl_reg & ~MODE_CTL_START_SCAN,
+					kbd->io_base + MODE_CTL_REG);
+			clk_disable(kbd->clk);
+		}
+	}
+
+	/* store current configuration */
+	if (input_dev->users)
+		kbd->mode_ctl_reg = mode_ctl_reg;
+
+	/* restore previous clk state */
+	clk_disable(kbd->clk);
+
 	mutex_unlock(&input_dev->mutex);
 
 	return 0;
@@ -336,11 +378,16 @@ static int spear_kbd_resume(struct device *dev)
 
 	mutex_lock(&input_dev->mutex);
 
-	if (device_may_wakeup(&pdev->dev))
+	if (device_may_wakeup(&pdev->dev)) {
 		disable_irq_wake(kbd->irq);
+	} else {
+		if (input_dev->users)
+			clk_enable(kbd->clk);
+	}
 
+	/* restore current configuration */
 	if (input_dev->users)
-		clk_enable(kbd->clk);
+		writel_relaxed(kbd->mode_ctl_reg, kbd->io_base + MODE_CTL_REG);
 
 	mutex_unlock(&input_dev->mutex);
 
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index 4ffe64d5310..2c1c9ed1bd9 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -492,7 +492,7 @@ static int tegra_kbc_start(struct tegra_kbc *kbc)
 	unsigned int debounce_cnt;
 	u32 val = 0;
 
-	clk_enable(kbc->clk);
+	clk_prepare_enable(kbc->clk);
 
 	/* Reset the KBC controller to clear all previous status.*/
 	tegra_periph_reset_assert(kbc->clk);
@@ -556,7 +556,7 @@ static void tegra_kbc_stop(struct tegra_kbc *kbc)
 	disable_irq(kbc->irq);
 	del_timer_sync(&kbc->timer);
 
-	clk_disable(kbc->clk);
+	clk_disable_unprepare(kbc->clk);
 }
 
 static int tegra_kbc_open(struct input_dev *dev)
diff --git a/drivers/input/misc/88pm80x_onkey.c b/drivers/input/misc/88pm80x_onkey.c
new file mode 100644
index 00000000000..7f26e7b6c22
--- /dev/null
+++ b/drivers/input/misc/88pm80x_onkey.c
@@ -0,0 +1,168 @@
+/*
+ * Marvell 88PM80x ONKEY driver
+ *
+ * Copyright (C) 2012 Marvell International Ltd.
+ * Haojian Zhuang <haojian.zhuang@marvell.com>
+ * Qiao Zhou <zhouqiao@marvell.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License. See the file "COPYING" in the main directory of this
+ * archive for more details.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/mfd/88pm80x.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#define PM800_LONG_ONKEY_EN		(1 << 0)
+#define PM800_LONG_KEY_DELAY		(8)	/* 1 .. 16 seconds */
+#define PM800_LONKEY_PRESS_TIME		((PM800_LONG_KEY_DELAY-1) << 4)
+#define PM800_LONKEY_PRESS_TIME_MASK	(0xF0)
+#define PM800_SW_PDOWN			(1 << 5)
+
+struct pm80x_onkey_info {
+	struct input_dev *idev;
+	struct pm80x_chip *pm80x;
+	struct regmap *map;
+	int irq;
+};
+
+/* 88PM80x gives us an interrupt when ONKEY is held */
+static irqreturn_t pm80x_onkey_handler(int irq, void *data)
+{
+	struct pm80x_onkey_info *info = data;
+	int ret = 0;
+	unsigned int val;
+
+	ret = regmap_read(info->map, PM800_STATUS_1, &val);
+	if (ret < 0) {
+		dev_err(info->idev->dev.parent, "failed to read status: %d\n", ret);
+		return IRQ_NONE;
+	}
+	val &= PM800_ONKEY_STS1;
+
+	input_report_key(info->idev, KEY_POWER, val);
+	input_sync(info->idev);
+
+	return IRQ_HANDLED;
+}
+
+static SIMPLE_DEV_PM_OPS(pm80x_onkey_pm_ops, pm80x_dev_suspend,
+			 pm80x_dev_resume);
+
+static int __devinit pm80x_onkey_probe(struct platform_device *pdev)
+{
+
+	struct pm80x_chip *chip = dev_get_drvdata(pdev->dev.parent);
+	struct pm80x_onkey_info *info;
+	int err;
+
+	info = kzalloc(sizeof(struct pm80x_onkey_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->pm80x = chip;
+
+	info->irq = platform_get_irq(pdev, 0);
+	if (info->irq < 0) {
+		dev_err(&pdev->dev, "No IRQ resource!\n");
+		err = -EINVAL;
+		goto out;
+	}
+
+	info->map = info->pm80x->regmap;
+	if (!info->map) {
+		dev_err(&pdev->dev, "no regmap!\n");
+		err = -EINVAL;
+		goto out;
+	}
+
+	info->idev = input_allocate_device();
+	if (!info->idev) {
+		dev_err(&pdev->dev, "Failed to allocate input dev\n");
+		err = -ENOMEM;
+		goto out;
+	}
+
+	info->idev->name = "88pm80x_on";
+	info->idev->phys = "88pm80x_on/input0";
+	info->idev->id.bustype = BUS_I2C;
+	info->idev->dev.parent = &pdev->dev;
+	info->idev->evbit[0] = BIT_MASK(EV_KEY);
+	__set_bit(KEY_POWER, info->idev->keybit);
+
+	err = pm80x_request_irq(info->pm80x, info->irq, pm80x_onkey_handler,
+					    IRQF_ONESHOT, "onkey", info);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Failed to request IRQ: #%d: %d\n",
+			info->irq, err);
+		goto out_reg;
+	}
+
+	err = input_register_device(info->idev);
+	if (err) {
+		dev_err(&pdev->dev, "Can't register input device: %d\n", err);
+		goto out_irq;
+	}
+
+	platform_set_drvdata(pdev, info);
+
+	/* Enable long onkey detection */
+	regmap_update_bits(info->map, PM800_RTC_MISC4, PM800_LONG_ONKEY_EN,
+			   PM800_LONG_ONKEY_EN);
+	/* Set 8-second interval */
+	regmap_update_bits(info->map, PM800_RTC_MISC3,
+			   PM800_LONKEY_PRESS_TIME_MASK,
+			   PM800_LONKEY_PRESS_TIME);
+
+	device_init_wakeup(&pdev->dev, 1);
+	return 0;
+
+out_irq:
+	pm80x_free_irq(info->pm80x, info->irq, info);
+out_reg:
+	input_free_device(info->idev);
+out:
+	kfree(info);
+	return err;
+}
+
+static int __devexit pm80x_onkey_remove(struct platform_device *pdev)
+{
+	struct pm80x_onkey_info *info = platform_get_drvdata(pdev);
+
+	device_init_wakeup(&pdev->dev, 0);
+	pm80x_free_irq(info->pm80x, info->irq, info);
+	input_unregister_device(info->idev);
+	kfree(info);
+	return 0;
+}
+
+static struct platform_driver pm80x_onkey_driver = {
+	.driver = {
+		   .name = "88pm80x-onkey",
+		   .owner = THIS_MODULE,
+		   .pm = &pm80x_onkey_pm_ops,
+		   },
+	.probe = pm80x_onkey_probe,
+	.remove = __devexit_p(pm80x_onkey_remove),
+};
+
+module_platform_driver(pm80x_onkey_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Marvell 88PM80x ONKEY driver");
+MODULE_AUTHOR("Qiao Zhou <zhouqiao@marvell.com>");
+MODULE_ALIAS("platform:88pm80x-onkey");
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 7faf4a7fcaa..7c0f1ecfdd7 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -22,6 +22,16 @@ config INPUT_88PM860X_ONKEY
 	  To compile this driver as a module, choose M here: the module
 	  will be called 88pm860x_onkey.
 
+config INPUT_88PM80X_ONKEY
+	tristate "88PM80x ONKEY support"
+	depends on MFD_88PM800
+	help
+	  Support the ONKEY of Marvell 88PM80x PMICs as an input device
+	  reporting power button status.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called 88pm80x_onkey.
+
 config INPUT_AB8500_PONKEY
 	tristate "AB8500 Pon (PowerOn) Key"
 	depends on AB8500_CORE
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index f55cdf4916f..83fe6f5b77d 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -5,6 +5,7 @@
 # Each configuration option enables a list of files.
 
 obj-$(CONFIG_INPUT_88PM860X_ONKEY)	+= 88pm860x_onkey.o
+obj-$(CONFIG_INPUT_88PM80X_ONKEY)	+= 88pm80x_onkey.o
 obj-$(CONFIG_INPUT_AB8500_PONKEY)	+= ab8500-ponkey.o
 obj-$(CONFIG_INPUT_AD714X)		+= ad714x.o
 obj-$(CONFIG_INPUT_AD714X_I2C)		+= ad714x-i2c.o
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index 350fd0c385d..f06231b7cab 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -13,6 +13,7 @@
 #include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/mfd/abx500/ab8500.h>
+#include <linux/of.h>
 #include <linux/slab.h>
 
 /**
@@ -73,8 +74,8 @@ static int __devinit ab8500_ponkey_probe(struct platform_device *pdev)
 
 	ponkey->idev = input;
 	ponkey->ab8500 = ab8500;
-	ponkey->irq_dbf = irq_dbf;
-	ponkey->irq_dbr = irq_dbr;
+	ponkey->irq_dbf = ab8500_irq_get_virq(ab8500, irq_dbf);
+	ponkey->irq_dbr = ab8500_irq_get_virq(ab8500, irq_dbr);
 
 	input->name = "AB8500 POn(PowerOn) Key";
 	input->dev.parent = &pdev->dev;
@@ -131,10 +132,18 @@ static int __devexit ab8500_ponkey_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id ab8500_ponkey_match[] = {
+	{ .compatible = "stericsson,ab8500-ponkey", },
+	{}
+};
+#endif
+
 static struct platform_driver ab8500_ponkey_driver = {
 	.driver		= {
 		.name	= "ab8500-poweron-key",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(ab8500_ponkey_match),
 	},
 	.probe		= ab8500_ponkey_probe,
 	.remove		= __devexit_p(ab8500_ponkey_remove),
diff --git a/drivers/input/misc/cma3000_d0x.c b/drivers/input/misc/cma3000_d0x.c
index a3735a01e9f..df9b756594f 100644
--- a/drivers/input/misc/cma3000_d0x.c
+++ b/drivers/input/misc/cma3000_d0x.c
@@ -58,7 +58,7 @@
 
 /*
  * Bit weights in mg for bit 0, other bits need
- * multipy factor 2^n. Eight bit is the sign bit.
+ * multiply factor 2^n. Eight bit is the sign bit.
  */
 #define BIT_TO_2G  18
 #define BIT_TO_8G  71
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index c34f6c0371c..c8a288ae1d5 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -251,7 +251,6 @@ static int twl6040_vibra_suspend(struct device *dev)
 
 	return 0;
 }
-
 #endif
 
 static SIMPLE_DEV_PM_OPS(twl6040_vibra_pm_ops, twl6040_vibra_suspend, NULL);
@@ -259,13 +258,19 @@ static SIMPLE_DEV_PM_OPS(twl6040_vibra_pm_ops, twl6040_vibra_suspend, NULL);
 static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 {
 	struct twl6040_vibra_data *pdata = pdev->dev.platform_data;
-	struct device_node *node = pdev->dev.of_node;
+	struct device *twl6040_core_dev = pdev->dev.parent;
+	struct device_node *twl6040_core_node = NULL;
 	struct vibra_info *info;
 	int vddvibl_uV = 0;
 	int vddvibr_uV = 0;
 	int ret;
 
-	if (!pdata && !node) {
+#ifdef CONFIG_OF
+	twl6040_core_node = of_find_node_by_name(twl6040_core_dev->of_node,
+						 "vibra");
+#endif
+
+	if (!pdata && !twl6040_core_node) {
 		dev_err(&pdev->dev, "platform_data not available\n");
 		return -EINVAL;
 	}
@@ -287,14 +292,18 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 		vddvibl_uV = pdata->vddvibl_uV;
 		vddvibr_uV = pdata->vddvibr_uV;
 	} else {
-		of_property_read_u32(node, "vibldrv_res", &info->vibldrv_res);
-		of_property_read_u32(node, "vibrdrv_res", &info->vibrdrv_res);
-		of_property_read_u32(node, "viblmotor_res",
+		of_property_read_u32(twl6040_core_node, "ti,vibldrv-res",
+				     &info->vibldrv_res);
+		of_property_read_u32(twl6040_core_node, "ti,vibrdrv-res",
+				     &info->vibrdrv_res);
+		of_property_read_u32(twl6040_core_node, "ti,viblmotor-res",
 				     &info->viblmotor_res);
-		of_property_read_u32(node, "vibrmotor_res",
+		of_property_read_u32(twl6040_core_node, "ti,vibrmotor-res",
 				     &info->vibrmotor_res);
-		of_property_read_u32(node, "vddvibl_uV", &vddvibl_uV);
-		of_property_read_u32(node, "vddvibr_uV", &vddvibr_uV);
+		of_property_read_u32(twl6040_core_node, "ti,vddvibl-uV",
+				     &vddvibl_uV);
+		of_property_read_u32(twl6040_core_node, "ti,vddvibr-uV",
+				     &vddvibr_uV);
 	}
 
 	if ((!info->vibldrv_res && !info->viblmotor_res) ||
@@ -351,8 +360,12 @@ static int __devinit twl6040_vibra_probe(struct platform_device *pdev)
 
 	info->supplies[0].supply = "vddvibl";
 	info->supplies[1].supply = "vddvibr";
-	ret = regulator_bulk_get(info->dev, ARRAY_SIZE(info->supplies),
-				 info->supplies);
+	/*
+	 * When booted with Device tree the regulators are attached to the
+	 * parent device (twl6040 MFD core)
+	 */
+	ret = regulator_bulk_get(pdata ? info->dev : twl6040_core_dev,
+				 ARRAY_SIZE(info->supplies), info->supplies);
 	if (ret) {
 		dev_err(info->dev, "couldn't get regulators %d\n", ret);
 		goto err_regulator;
@@ -418,12 +431,6 @@ static int __devexit twl6040_vibra_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id twl6040_vibra_of_match[] = {
-	{.compatible = "ti,twl6040-vibra", },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, twl6040_vibra_of_match);
-
 static struct platform_driver twl6040_vibra_driver = {
 	.probe		= twl6040_vibra_probe,
 	.remove		= __devexit_p(twl6040_vibra_remove),
@@ -431,7 +438,6 @@ static struct platform_driver twl6040_vibra_driver = {
 		.name	= "twl6040-vibra",
 		.owner	= THIS_MODULE,
 		.pm	= &twl6040_vibra_pm_ops,
-		.of_match_table = twl6040_vibra_of_match,
 	},
 };
 module_platform_driver(twl6040_vibra_driver);
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index c703d53be3a..14eaecea2b7 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -40,11 +40,27 @@
  * Note that newer firmware allows querying device for maximum useable
  * coordinates.
  */
+#define XMIN 0
+#define XMAX 6143
+#define YMIN 0
+#define YMAX 6143
 #define XMIN_NOMINAL 1472
 #define XMAX_NOMINAL 5472
 #define YMIN_NOMINAL 1408
 #define YMAX_NOMINAL 4448
 
+/* Size in bits of absolute position values reported by the hardware */
+#define ABS_POS_BITS 13
+
+/*
+ * Any position values from the hardware above the following limits are
+ * treated as "wrapped around negative" values that have been truncated to
+ * the 13-bit reporting range of the hardware. These are just reasonable
+ * guesses and can be adjusted if hardware is found that operates outside
+ * of these parameters.
+ */
+#define X_MAX_POSITIVE (((1 << ABS_POS_BITS) + XMAX) / 2)
+#define Y_MAX_POSITIVE (((1 << ABS_POS_BITS) + YMAX) / 2)
 
 /*****************************************************************************
  *	Stuff we need even when we do not want native Synaptics support
@@ -139,6 +155,35 @@ static int synaptics_model_id(struct psmouse *psmouse)
 }
 
 /*
+ * Read the board id from the touchpad
+ * The board id is encoded in the "QUERY MODES" response
+ */
+static int synaptics_board_id(struct psmouse *psmouse)
+{
+	struct synaptics_data *priv = psmouse->private;
+	unsigned char bid[3];
+
+	if (synaptics_send_cmd(psmouse, SYN_QUE_MODES, bid))
+		return -1;
+	priv->board_id = ((bid[0] & 0xfc) << 6) | bid[1];
+	return 0;
+}
+
+/*
+ * Read the firmware id from the touchpad
+ */
+static int synaptics_firmware_id(struct psmouse *psmouse)
+{
+	struct synaptics_data *priv = psmouse->private;
+	unsigned char fwid[3];
+
+	if (synaptics_send_cmd(psmouse, SYN_QUE_FIRMWARE_ID, fwid))
+		return -1;
+	priv->firmware_id = (fwid[0] << 16) | (fwid[1] << 8) | fwid[2];
+	return 0;
+}
+
+/*
  * Read the capability-bits from the touchpad
  * see also the SYN_CAP_* macros
  */
@@ -261,6 +306,10 @@ static int synaptics_query_hardware(struct psmouse *psmouse)
 		return -1;
 	if (synaptics_model_id(psmouse))
 		return -1;
+	if (synaptics_firmware_id(psmouse))
+		return -1;
+	if (synaptics_board_id(psmouse))
+		return -1;
 	if (synaptics_capability(psmouse))
 		return -1;
 	if (synaptics_resolution(psmouse))
@@ -555,6 +604,12 @@ static int synaptics_parse_hw_state(const unsigned char buf[],
 		hw->right = (buf[0] & 0x02) ? 1 : 0;
 	}
 
+	/* Convert wrap-around values to negative */
+	if (hw->x > X_MAX_POSITIVE)
+		hw->x -= 1 << ABS_POS_BITS;
+	if (hw->y > Y_MAX_POSITIVE)
+		hw->y -= 1 << ABS_POS_BITS;
+
 	return 0;
 }
 
@@ -1435,11 +1490,12 @@ static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
 	priv->pkt_type = SYN_MODEL_NEWABS(priv->model_id) ? SYN_NEWABS : SYN_OLDABS;
 
 	psmouse_info(psmouse,
-		     "Touchpad model: %ld, fw: %ld.%ld, id: %#lx, caps: %#lx/%#lx/%#lx\n",
+		     "Touchpad model: %ld, fw: %ld.%ld, id: %#lx, caps: %#lx/%#lx/%#lx, board id: %lu, fw id: %lu\n",
 		     SYN_ID_MODEL(priv->identity),
 		     SYN_ID_MAJOR(priv->identity), SYN_ID_MINOR(priv->identity),
 		     priv->model_id,
-		     priv->capabilities, priv->ext_cap, priv->ext_cap_0c);
+		     priv->capabilities, priv->ext_cap, priv->ext_cap_0c,
+		     priv->board_id, priv->firmware_id);
 
 	set_input_params(psmouse->dev, priv);
 
diff --git a/drivers/input/mouse/synaptics.h b/drivers/input/mouse/synaptics.h
index fd26ccca13d..e594af0b264 100644
--- a/drivers/input/mouse/synaptics.h
+++ b/drivers/input/mouse/synaptics.h
@@ -18,6 +18,7 @@
 #define SYN_QUE_SERIAL_NUMBER_SUFFIX	0x07
 #define SYN_QUE_RESOLUTION		0x08
 #define SYN_QUE_EXT_CAPAB		0x09
+#define SYN_QUE_FIRMWARE_ID		0x0a
 #define SYN_QUE_EXT_CAPAB_0C		0x0c
 #define SYN_QUE_EXT_MAX_COORDS		0x0d
 #define SYN_QUE_EXT_MIN_COORDS		0x0f
@@ -148,6 +149,8 @@ struct synaptics_hw_state {
 struct synaptics_data {
 	/* Data read from the touchpad */
 	unsigned long int model_id;		/* Model-ID */
+	unsigned long int firmware_id;		/* Firmware-ID */
+	unsigned long int board_id;		/* Board-ID */
 	unsigned long int capabilities;		/* Capabilities */
 	unsigned long int ext_cap;		/* Extended Capabilities */
 	unsigned long int ext_cap_0c;		/* Ext Caps from 0x0c query */
diff --git a/drivers/input/mouse/synaptics_usb.c b/drivers/input/mouse/synaptics_usb.c
index 3c5eaaa5d15..64cf34ea760 100644
--- a/drivers/input/mouse/synaptics_usb.c
+++ b/drivers/input/mouse/synaptics_usb.c
@@ -364,7 +364,7 @@ static int synusb_probe(struct usb_interface *intf,
 			 le16_to_cpu(udev->descriptor.idProduct));
 
 	if (synusb->flags & SYNUSB_STICK)
-		strlcat(synusb->name, " (Stick) ", sizeof(synusb->name));
+		strlcat(synusb->name, " (Stick)", sizeof(synusb->name));
 
 	usb_make_path(udev, synusb->phys, sizeof(synusb->phys));
 	strlcat(synusb->phys, "/input0", sizeof(synusb->phys));
diff --git a/drivers/input/serio/hp_sdc.c b/drivers/input/serio/hp_sdc.c
index 09a089996de..d7a7e54f646 100644
--- a/drivers/input/serio/hp_sdc.c
+++ b/drivers/input/serio/hp_sdc.c
@@ -878,7 +878,7 @@ static int __init hp_sdc_init(void)
 #endif
 
 	errstr = "IRQ not available for";
-	if (request_irq(hp_sdc.irq, &hp_sdc_isr, IRQF_SHARED|IRQF_SAMPLE_RANDOM,
+	if (request_irq(hp_sdc.irq, &hp_sdc_isr, IRQF_SHARED,
 			"HP SDC", &hp_sdc))
 		goto err1;
 
diff --git a/drivers/input/tablet/hanwang.c b/drivers/input/tablet/hanwang.c
index b2db3cfe308..5cc04124995 100644
--- a/drivers/input/tablet/hanwang.c
+++ b/drivers/input/tablet/hanwang.c
@@ -63,6 +63,7 @@ MODULE_LICENSE(DRIVER_LICENSE);
 enum hanwang_tablet_type {
 	HANWANG_ART_MASTER_III,
 	HANWANG_ART_MASTER_HD,
+	HANWANG_ART_MASTER_II,
 };
 
 struct hanwang {
@@ -99,6 +100,8 @@ static const struct hanwang_features features_array[] = {
 	  ART_MASTER_PKGLEN_MAX, 0x7f00, 0x4f60, 0x3f, 0x7f, 2048 },
 	{ 0x8401, "Hanwang Art Master HD 5012", HANWANG_ART_MASTER_HD,
 	  ART_MASTER_PKGLEN_MAX, 0x678e, 0x4150, 0x3f, 0x7f, 1024 },
+	{ 0x8503, "Hanwang Art Master II", HANWANG_ART_MASTER_II,
+	  ART_MASTER_PKGLEN_MAX, 0x27de, 0x1cfe, 0x3f, 0x7f, 1024 },
 };
 
 static const int hw_eventtypes[] = {
@@ -127,14 +130,30 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 	struct usb_device *dev = hanwang->usbdev;
 	enum hanwang_tablet_type type = hanwang->features->type;
 	int i;
-	u16 x, y, p;
+	u16 p;
+
+	if (type == HANWANG_ART_MASTER_II) {
+		hanwang->current_tool = BTN_TOOL_PEN;
+		hanwang->current_id = STYLUS_DEVICE_ID;
+	}
 
 	switch (data[0]) {
 	case 0x02:	/* data packet */
 		switch (data[1]) {
 		case 0x80:	/* tool prox out */
-			hanwang->current_id = 0;
-			input_report_key(input_dev, hanwang->current_tool, 0);
+			if (type != HANWANG_ART_MASTER_II) {
+				hanwang->current_id = 0;
+				input_report_key(input_dev,
+						 hanwang->current_tool, 0);
+			}
+			break;
+
+		case 0x00:	/* artmaster ii pen leave */
+			if (type == HANWANG_ART_MASTER_II) {
+				hanwang->current_id = 0;
+				input_report_key(input_dev,
+						 hanwang->current_tool, 0);
+			}
 			break;
 
 		case 0xc2:	/* first time tool prox in */
@@ -154,15 +173,12 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 			default:
 				hanwang->current_id = 0;
 				dev_dbg(&dev->dev,
-					"unknown tablet tool %02x ", data[0]);
+					"unknown tablet tool %02x\n", data[0]);
 				break;
 			}
 			break;
 
 		default:	/* tool data packet */
-			x = (data[2] << 8) | data[3];
-			y = (data[4] << 8) | data[5];
-
 			switch (type) {
 			case HANWANG_ART_MASTER_III:
 				p = (data[6] << 3) |
@@ -171,6 +187,7 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 				break;
 
 			case HANWANG_ART_MASTER_HD:
+			case HANWANG_ART_MASTER_II:
 				p = (data[7] >> 6) | (data[6] << 2);
 				break;
 
@@ -180,17 +197,23 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 			}
 
 			input_report_abs(input_dev, ABS_X,
-						le16_to_cpup((__le16 *)&x));
+					 be16_to_cpup((__be16 *)&data[2]));
 			input_report_abs(input_dev, ABS_Y,
-						le16_to_cpup((__le16 *)&y));
-			input_report_abs(input_dev, ABS_PRESSURE,
-						le16_to_cpup((__le16 *)&p));
+					 be16_to_cpup((__be16 *)&data[4]));
+			input_report_abs(input_dev, ABS_PRESSURE, p);
 			input_report_abs(input_dev, ABS_TILT_X, data[7] & 0x3f);
 			input_report_abs(input_dev, ABS_TILT_Y, data[8] & 0x7f);
 			input_report_key(input_dev, BTN_STYLUS, data[1] & 0x02);
-			input_report_key(input_dev, BTN_STYLUS2, data[1] & 0x04);
+
+			if (type != HANWANG_ART_MASTER_II)
+				input_report_key(input_dev, BTN_STYLUS2,
+						 data[1] & 0x04);
+			else
+				input_report_key(input_dev, BTN_TOOL_PEN, 1);
+
 			break;
 		}
+
 		input_report_abs(input_dev, ABS_MISC, hanwang->current_id);
 		input_event(input_dev, EV_MSC, MSC_SERIAL,
 				hanwang->features->pid);
@@ -202,8 +225,8 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 
 		switch (type) {
 		case HANWANG_ART_MASTER_III:
-			input_report_key(input_dev, BTN_TOOL_FINGER, data[1] ||
-							data[2] || data[3]);
+			input_report_key(input_dev, BTN_TOOL_FINGER,
+					 data[1] || data[2] || data[3]);
 			input_report_abs(input_dev, ABS_WHEEL, data[1]);
 			input_report_key(input_dev, BTN_0, data[2]);
 			for (i = 0; i < 8; i++)
@@ -227,6 +250,10 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 					 BTN_5 + i, data[6] & (1 << i));
 			}
 			break;
+
+		case HANWANG_ART_MASTER_II:
+			dev_dbg(&dev->dev, "error packet  %02x\n", data[0]);
+			return;
 		}
 
 		input_report_abs(input_dev, ABS_MISC, hanwang->current_id);
@@ -234,7 +261,7 @@ static void hanwang_parse_packet(struct hanwang *hanwang)
 		break;
 
 	default:
-		dev_dbg(&dev->dev, "error packet  %02x ", data[0]);
+		dev_dbg(&dev->dev, "error packet  %02x\n", data[0]);
 		break;
 	}
 
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 8b31473a81f..0d3219f2974 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -445,8 +445,7 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 	/* ask to report Wacom data */
 	if (features->device_type == BTN_TOOL_FINGER) {
 		/* if it is an MT Tablet PC touch */
-		if (features->type == TABLETPC2FG ||
-		    features->type == MTSCREEN) {
+		if (features->type > TABLETPC) {
 			do {
 				rep_data[0] = 3;
 				rep_data[1] = 4;
@@ -465,7 +464,7 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 			} while ((error < 0 || rep_data[1] != 4) &&
 				 limit++ < WAC_MSG_RETRIES);
 		}
-	} else if (features->type != TABLETPC &&
+	} else if (features->type <= BAMBOO_PT &&
 		   features->type != WIRELESS &&
 		   features->device_type == BTN_TOOL_PEN) {
 		do {
@@ -509,16 +508,13 @@ static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
 		if (intf->cur_altsetting->desc.bInterfaceNumber == 0) {
 			features->device_type = 0;
 		} else if (intf->cur_altsetting->desc.bInterfaceNumber == 2) {
-			features->device_type = BTN_TOOL_DOUBLETAP;
+			features->device_type = BTN_TOOL_FINGER;
 			features->pktlen = WACOM_PKGLEN_BBTOUCH3;
 		}
 	}
 
 	/* only devices that support touch need to retrieve the info */
-	if (features->type != TABLETPC &&
-	    features->type != TABLETPC2FG &&
-	    features->type != BAMBOO_PT &&
-	    features->type != MTSCREEN) {
+	if (features->type < BAMBOO_PT) {
 		goto out;
 	}
 
@@ -860,6 +856,7 @@ static int wacom_initialize_leds(struct wacom *wacom)
 
 	/* Initialize default values */
 	switch (wacom->wacom_wac.features.type) {
+	case INTUOS4S:
 	case INTUOS4:
 	case INTUOS4L:
 		wacom->led.select[0] = 0;
@@ -913,6 +910,7 @@ static int wacom_initialize_leds(struct wacom *wacom)
 static void wacom_destroy_leds(struct wacom *wacom)
 {
 	switch (wacom->wacom_wac.features.type) {
+	case INTUOS4S:
 	case INTUOS4:
 	case INTUOS4L:
 		sysfs_remove_group(&wacom->intf->dev.kobj,
@@ -972,6 +970,10 @@ static int wacom_initialize_battery(struct wacom *wacom)
 
 		error = power_supply_register(&wacom->usbdev->dev,
 					      &wacom->battery);
+
+		if (!error)
+			power_supply_powers(&wacom->battery,
+					    &wacom->usbdev->dev);
 	}
 
 	return error;
@@ -979,8 +981,11 @@ static int wacom_initialize_battery(struct wacom *wacom)
 
 static void wacom_destroy_battery(struct wacom *wacom)
 {
-	if (wacom->wacom_wac.features.quirks & WACOM_QUIRK_MONITOR)
+	if (wacom->wacom_wac.features.quirks & WACOM_QUIRK_MONITOR &&
+	    wacom->battery.dev) {
 		power_supply_unregister(&wacom->battery);
+		wacom->battery.dev = NULL;
+	}
 }
 
 static int wacom_register_input(struct wacom *wacom)
@@ -1027,23 +1032,30 @@ static void wacom_wireless_work(struct work_struct *work)
 	struct wacom *wacom = container_of(work, struct wacom, work);
 	struct usb_device *usbdev = wacom->usbdev;
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct wacom *wacom1, *wacom2;
+	struct wacom_wac *wacom_wac1, *wacom_wac2;
+	int error;
 
 	/*
 	 * Regardless if this is a disconnect or a new tablet,
-	 * remove any existing input devices.
+	 * remove any existing input and battery devices.
 	 */
 
+	wacom_destroy_battery(wacom);
+
 	/* Stylus interface */
-	wacom = usb_get_intfdata(usbdev->config->interface[1]);
-	if (wacom->wacom_wac.input)
-		input_unregister_device(wacom->wacom_wac.input);
-	wacom->wacom_wac.input = NULL;
+	wacom1 = usb_get_intfdata(usbdev->config->interface[1]);
+	wacom_wac1 = &(wacom1->wacom_wac);
+	if (wacom_wac1->input)
+		input_unregister_device(wacom_wac1->input);
+	wacom_wac1->input = NULL;
 
 	/* Touch interface */
-	wacom = usb_get_intfdata(usbdev->config->interface[2]);
-	if (wacom->wacom_wac.input)
-		input_unregister_device(wacom->wacom_wac.input);
-	wacom->wacom_wac.input = NULL;
+	wacom2 = usb_get_intfdata(usbdev->config->interface[2]);
+	wacom_wac2 = &(wacom2->wacom_wac);
+	if (wacom_wac2->input)
+		input_unregister_device(wacom_wac2->input);
+	wacom_wac2->input = NULL;
 
 	if (wacom_wac->pid == 0) {
 		dev_info(&wacom->intf->dev, "wireless tablet disconnected\n");
@@ -1068,24 +1080,39 @@ static void wacom_wireless_work(struct work_struct *work)
 		}
 
 		/* Stylus interface */
-		wacom = usb_get_intfdata(usbdev->config->interface[1]);
-		wacom_wac = &wacom->wacom_wac;
-		wacom_wac->features =
+		wacom_wac1->features =
 			*((struct wacom_features *)id->driver_info);
-		wacom_wac->features.device_type = BTN_TOOL_PEN;
-		wacom_register_input(wacom);
+		wacom_wac1->features.device_type = BTN_TOOL_PEN;
+		error = wacom_register_input(wacom1);
+		if (error)
+			goto fail1;
 
 		/* Touch interface */
-		wacom = usb_get_intfdata(usbdev->config->interface[2]);
-		wacom_wac = &wacom->wacom_wac;
-		wacom_wac->features =
+		wacom_wac2->features =
 			*((struct wacom_features *)id->driver_info);
-		wacom_wac->features.pktlen = WACOM_PKGLEN_BBTOUCH3;
-		wacom_wac->features.device_type = BTN_TOOL_FINGER;
-		wacom_set_phy_from_res(&wacom_wac->features);
-		wacom_wac->features.x_max = wacom_wac->features.y_max = 4096;
-		wacom_register_input(wacom);
+		wacom_wac2->features.pktlen = WACOM_PKGLEN_BBTOUCH3;
+		wacom_wac2->features.device_type = BTN_TOOL_FINGER;
+		wacom_set_phy_from_res(&wacom_wac2->features);
+		wacom_wac2->features.x_max = wacom_wac2->features.y_max = 4096;
+		error = wacom_register_input(wacom2);
+		if (error)
+			goto fail2;
+
+		error = wacom_initialize_battery(wacom);
+		if (error)
+			goto fail3;
 	}
+
+	return;
+
+fail3:
+	input_unregister_device(wacom_wac2->input);
+	wacom_wac2->input = NULL;
+fail2:
+	input_unregister_device(wacom_wac1->input);
+	wacom_wac1->input = NULL;
+fail1:
+	return;
 }
 
 static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *id)
@@ -1149,10 +1176,7 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
 			features->device_type = BTN_TOOL_FINGER;
 			features->pktlen = WACOM_PKGLEN_BBTOUCH3;
 
-			features->x_phy =
-				(features->x_max * 100) / features->x_resolution;
-			features->y_phy =
-				(features->y_max * 100) / features->y_resolution;
+			wacom_set_phy_from_res(features);
 
 			features->x_max = 4096;
 			features->y_max = 4096;
@@ -1188,14 +1212,10 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
 	if (error)
 		goto fail4;
 
-	error = wacom_initialize_battery(wacom);
-	if (error)
-		goto fail5;
-
 	if (!(features->quirks & WACOM_QUIRK_NO_INPUT)) {
 		error = wacom_register_input(wacom);
 		if (error)
-			goto fail6;
+			goto fail5;
 	}
 
 	/* Note that if query fails it is not a hard failure */
@@ -1210,7 +1230,6 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
 
 	return 0;
 
- fail6: wacom_destroy_battery(wacom);
  fail5: wacom_destroy_leds(wacom);
  fail4:	wacom_remove_shared_data(wacom_wac);
  fail3:	usb_free_urb(wacom->irq);
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 004bc1bb154..002041975de 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -248,7 +248,7 @@ static int wacom_graphire_irq(struct wacom_wac *wacom)
 		input_report_abs(input, ABS_X, le16_to_cpup((__le16 *)&data[2]));
 		input_report_abs(input, ABS_Y, le16_to_cpup((__le16 *)&data[4]));
 		if (wacom->tool[0] != BTN_TOOL_MOUSE) {
-			input_report_abs(input, ABS_PRESSURE, data[6] | ((data[7] & 0x01) << 8));
+			input_report_abs(input, ABS_PRESSURE, data[6] | ((data[7] & 0x03) << 8));
 			input_report_key(input, BTN_TOUCH, data[1] & 0x01);
 			input_report_key(input, BTN_STYLUS, data[1] & 0x02);
 			input_report_key(input, BTN_STYLUS2, data[1] & 0x04);
@@ -464,7 +464,7 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 		t = (data[6] << 2) | ((data[7] >> 6) & 3);
 		if ((features->type >= INTUOS4S && features->type <= INTUOS4L) ||
                     (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
-		    features->type == WACOM_21UX2 || features->type == WACOM_24HD) {
+		    (features->type >= WACOM_21UX2 && features->type <= WACOM_24HD)) {
 			t = (t << 1) | (data[1] & 1);
 		}
 		input_report_abs(input, ABS_PRESSURE, t);
@@ -614,7 +614,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				input_report_abs(input, ABS_MISC, 0);
 			}
 		} else {
-			if (features->type == WACOM_21UX2) {
+			if (features->type == WACOM_21UX2 || features->type == WACOM_22HD) {
 				input_report_key(input, BTN_0, (data[5] & 0x01));
 				input_report_key(input, BTN_1, (data[6] & 0x01));
 				input_report_key(input, BTN_2, (data[6] & 0x02));
@@ -633,6 +633,12 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				input_report_key(input, BTN_Z, (data[8] & 0x20));
 				input_report_key(input, BTN_BASE, (data[8] & 0x40));
 				input_report_key(input, BTN_BASE2, (data[8] & 0x80));
+
+				if (features->type == WACOM_22HD) {
+					input_report_key(input, KEY_PROG1, data[9] & 0x01);
+					input_report_key(input, KEY_PROG2, data[9] & 0x02);
+					input_report_key(input, KEY_PROG3, data[9] & 0x04);
+				}
 			} else {
 				input_report_key(input, BTN_0, (data[5] & 0x01));
 				input_report_key(input, BTN_1, (data[5] & 0x02));
@@ -888,7 +894,7 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 			prox = data[0] & 0x01;
 			x = get_unaligned_le16(&data[1]);
 			y = get_unaligned_le16(&data[3]);
-		} else { /* with capacity */
+		} else {
 			prox = data[1] & 0x01;
 			x = le16_to_cpup((__le16 *)&data[2]);
 			y = le16_to_cpup((__le16 *)&data[4]);
@@ -961,6 +967,7 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 		case WACOM_REPORT_TPC1FG:
 		case WACOM_REPORT_TPCHID:
 		case WACOM_REPORT_TPCST:
+		case WACOM_REPORT_TPC1FGE:
 			return wacom_tpc_single_touch(wacom, len);
 
 		case WACOM_REPORT_TPCMT:
@@ -1230,6 +1237,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 	case CINTIQ:
 	case WACOM_BEE:
 	case WACOM_21UX2:
+	case WACOM_22HD:
 	case WACOM_24HD:
 		sync = wacom_intuos_irq(wacom_wac);
 		break;
@@ -1244,6 +1252,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 		break;
 
 	case TABLETPC:
+	case TABLETPCE:
 	case TABLETPC2FG:
 	case MTSCREEN:
 		sync = wacom_tpc_irq(wacom_wac, len);
@@ -1317,10 +1326,8 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 	}
 
 	/* these device have multiple inputs */
-	if (features->type == TABLETPC || features->type == TABLETPC2FG ||
-	    features->type == BAMBOO_PT || features->type == WIRELESS ||
-	    (features->type >= INTUOS5S && features->type <= INTUOS5L) ||
-	    features->type == MTSCREEN)
+	if (features->type >= WIRELESS ||
+	    (features->type >= INTUOS5S && features->type <= INTUOS5L))
 		features->quirks |= WACOM_QUIRK_MULTI_INPUT;
 
 	/* quirk for bamboo touch with 2 low res touches */
@@ -1432,6 +1439,12 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 		wacom_setup_cintiq(wacom_wac);
 		break;
 
+	case WACOM_22HD:
+		__set_bit(KEY_PROG1, input_dev->keybit);
+		__set_bit(KEY_PROG2, input_dev->keybit);
+		__set_bit(KEY_PROG3, input_dev->keybit);
+		/* fall through */
+
 	case WACOM_21UX2:
 		__set_bit(BTN_A, input_dev->keybit);
 		__set_bit(BTN_B, input_dev->keybit);
@@ -1547,10 +1560,8 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
 		break;
 
-	case TABLETPC2FG:
 	case MTSCREEN:
 		if (features->device_type == BTN_TOOL_FINGER) {
-
 			wacom_wac->slots = kmalloc(features->touch_max *
 							sizeof(int),
 						   GFP_KERNEL);
@@ -1559,7 +1570,11 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 			for (i = 0; i < features->touch_max; i++)
 				wacom_wac->slots[i] = -1;
+		}
+		/* fall through */
 
+	case TABLETPC2FG:
+		if (features->device_type == BTN_TOOL_FINGER) {
 			input_mt_init_slots(input_dev, features->touch_max);
 			input_set_abs_params(input_dev, ABS_MT_TOOL_TYPE,
 					0, MT_TOOL_MAX, 0, 0);
@@ -1571,6 +1586,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 		/* fall through */
 
 	case TABLETPC:
+	case TABLETPCE:
 		__clear_bit(ABS_MISC, input_dev->absbit);
 
 		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
@@ -1855,6 +1871,9 @@ static const struct wacom_features wacom_features_0xF0 =
 static const struct wacom_features wacom_features_0xCC =
 	{ "Wacom Cintiq 21UX2",   WACOM_PKGLEN_INTUOS,    87200, 65600, 2047,
 	  63, WACOM_21UX2, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+static const struct wacom_features wacom_features_0xFA =
+	{ "Wacom Cintiq 22HD",    WACOM_PKGLEN_INTUOS,    95840, 54260, 2047,
+	  63, WACOM_22HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0x90 =
 	{ "Wacom ISDv4 90",       WACOM_PKGLEN_GRAPHIRE,  26202, 16325,  255,
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1888,6 +1907,12 @@ static const struct wacom_features wacom_features_0xE6 =
 static const struct wacom_features wacom_features_0xEC =
 	{ "Wacom ISDv4 EC",       WACOM_PKGLEN_GRAPHIRE,  25710, 14500,  255,
 	  0, TABLETPC,    WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0xED =
+	{ "Wacom ISDv4 ED",       WACOM_PKGLEN_GRAPHIRE,  26202, 16325,  255,
+	  0, TABLETPCE, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0xEF =
+	{ "Wacom ISDv4 EF",       WACOM_PKGLEN_GRAPHIRE,  26202, 16325,  255,
+	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x47 =
 	{ "Wacom Intuos2 6x8",    WACOM_PKGLEN_INTUOS,    20320, 16240, 1023,
 	  31, INTUOS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -2062,8 +2087,11 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0xE5) },
 	{ USB_DEVICE_WACOM(0xE6) },
 	{ USB_DEVICE_WACOM(0xEC) },
+	{ USB_DEVICE_WACOM(0xED) },
+	{ USB_DEVICE_WACOM(0xEF) },
 	{ USB_DEVICE_WACOM(0x47) },
 	{ USB_DEVICE_WACOM(0xF4) },
+	{ USB_DEVICE_WACOM(0xFA) },
 	{ USB_DEVICE_LENOVO(0x6004) },
 	{ }
 };
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index 78fbd3f4200..96c185cc301 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -48,6 +48,7 @@
 #define WACOM_REPORT_TPCMT		13
 #define WACOM_REPORT_TPCHID		15
 #define WACOM_REPORT_TPCST		16
+#define WACOM_REPORT_TPC1FGE		18
 
 /* device quirks */
 #define WACOM_QUIRK_MULTI_INPUT		0x0001
@@ -62,8 +63,6 @@ enum {
 	PTU,
 	PL,
 	DTU,
-	BAMBOO_PT,
-	WIRELESS,
 	INTUOS,
 	INTUOS3S,
 	INTUOS3,
@@ -74,12 +73,16 @@ enum {
 	INTUOS5S,
 	INTUOS5,
 	INTUOS5L,
-	WACOM_24HD,
 	WACOM_21UX2,
+	WACOM_22HD,
+	WACOM_24HD,
 	CINTIQ,
 	WACOM_BEE,
 	WACOM_MO,
-	TABLETPC,
+	WIRELESS,
+	BAMBOO_PT,
+	TABLETPC,   /* add new TPC below */
+	TABLETPCE,
 	TABLETPC2FG,
 	MTSCREEN,
 	MAX_TYPE
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 98d263504ee..1ba232cbc09 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -369,6 +369,18 @@ config TOUCHSCREEN_MCS5000
 	  To compile this driver as a module, choose M here: the
 	  module will be called mcs5000_ts.
 
+config TOUCHSCREEN_MMS114
+	tristate "MELFAS MMS114 touchscreen"
+	depends on I2C
+	help
+	  Say Y here if you have the MELFAS MMS114 touchscreen controller
+	  chip in your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mms114.
+
 config TOUCHSCREEN_MTOUCH
 	tristate "MicroTouch serial touchscreens"
 	select SERIO
@@ -460,6 +472,19 @@ config TOUCHSCREEN_PENMOUNT
 	  To compile this driver as a module, choose M here: the
 	  module will be called penmount.
 
+config TOUCHSCREEN_EDT_FT5X06
+	tristate "EDT FocalTech FT5x06 I2C Touchscreen support"
+	depends on I2C
+	help
+	  Say Y here if you have an EDT "Polytouch" touchscreen based
+	  on the FocalTech FT5x06 family of controllers connected to
+	  your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called edt-ft5x06.
+
 config TOUCHSCREEN_MIGOR
 	tristate "Renesas MIGO-R touchscreen"
 	depends on SH_MIGOR && I2C
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index eb8bfe1c1a4..178eb128d90 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_TOUCHSCREEN_CYTTSP_SPI)	+= cyttsp_spi.o
 obj-$(CONFIG_TOUCHSCREEN_DA9034)	+= da9034-ts.o
 obj-$(CONFIG_TOUCHSCREEN_DA9052)	+= da9052_tsi.o
 obj-$(CONFIG_TOUCHSCREEN_DYNAPRO)	+= dynapro.o
+obj-$(CONFIG_TOUCHSCREEN_EDT_FT5X06)	+= edt-ft5x06.o
 obj-$(CONFIG_TOUCHSCREEN_HAMPSHIRE)	+= hampshire.o
 obj-$(CONFIG_TOUCHSCREEN_GUNZE)		+= gunze.o
 obj-$(CONFIG_TOUCHSCREEN_EETI)		+= eeti_ts.o
@@ -38,6 +39,7 @@ obj-$(CONFIG_TOUCHSCREEN_MAX11801)	+= max11801_ts.o
 obj-$(CONFIG_TOUCHSCREEN_MC13783)	+= mc13783_ts.o
 obj-$(CONFIG_TOUCHSCREEN_MCS5000)	+= mcs5000_ts.o
 obj-$(CONFIG_TOUCHSCREEN_MIGOR)		+= migor_ts.o
+obj-$(CONFIG_TOUCHSCREEN_MMS114)	+= mms114.o
 obj-$(CONFIG_TOUCHSCREEN_MTOUCH)	+= mtouch.o
 obj-$(CONFIG_TOUCHSCREEN_MK712)		+= mk712.o
 obj-$(CONFIG_TOUCHSCREEN_HP600)		+= hp680_ts_input.o
diff --git a/drivers/input/touchscreen/ad7879.c b/drivers/input/touchscreen/ad7879.c
index bd4eb427769..facd3057b62 100644
--- a/drivers/input/touchscreen/ad7879.c
+++ b/drivers/input/touchscreen/ad7879.c
@@ -118,6 +118,7 @@ struct ad7879 {
 	unsigned int		irq;
 	bool			disabled;	/* P: input->mutex */
 	bool			suspended;	/* P: input->mutex */
+	bool			swap_xy;
 	u16			conversion_data[AD7879_NR_SENSE];
 	char			phys[32];
 	u8			first_conversion_delay;
@@ -161,6 +162,9 @@ static int ad7879_report(struct ad7879 *ts)
 	z1 = ts->conversion_data[AD7879_SEQ_Z1] & MAX_12BIT;
 	z2 = ts->conversion_data[AD7879_SEQ_Z2] & MAX_12BIT;
 
+	if (ts->swap_xy)
+		swap(x, y);
+
 	/*
 	 * The samples processed here are already preprocessed by the AD7879.
 	 * The preprocessing function consists of a median and an averaging
@@ -520,6 +524,7 @@ struct ad7879 *ad7879_probe(struct device *dev, u8 devid, unsigned int irq,
 	ts->dev = dev;
 	ts->input = input_dev;
 	ts->irq = irq;
+	ts->swap_xy = pdata->swap_xy;
 
 	setup_timer(&ts->timer, ad7879_timer, (unsigned long) ts);
 
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 25fd0561a17..4623cc69fc6 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -36,6 +36,7 @@
 #define MXT_FW_NAME		"maxtouch.fw"
 
 /* Registers */
+#define MXT_INFO		0x00
 #define MXT_FAMILY_ID		0x00
 #define MXT_VARIANT_ID		0x01
 #define MXT_VERSION		0x02
@@ -194,6 +195,7 @@
 #define MXT_BOOT_STATUS_MASK	0x3f
 
 /* Touch status */
+#define MXT_UNGRIP		(1 << 0)
 #define MXT_SUPPRESS		(1 << 1)
 #define MXT_AMP			(1 << 2)
 #define MXT_VECTOR		(1 << 3)
@@ -210,8 +212,6 @@
 /* Touchscreen absolute values */
 #define MXT_MAX_AREA		0xff
 
-#define MXT_MAX_FINGER		10
-
 struct mxt_info {
 	u8 family_id;
 	u8 variant_id;
@@ -225,44 +225,37 @@ struct mxt_info {
 struct mxt_object {
 	u8 type;
 	u16 start_address;
-	u8 size;
-	u8 instances;
+	u8 size;		/* Size of each instance - 1 */
+	u8 instances;		/* Number of instances - 1 */
 	u8 num_report_ids;
-
-	/* to map object and message */
-	u8 max_reportid;
-};
+} __packed;
 
 struct mxt_message {
 	u8 reportid;
 	u8 message[7];
 };
 
-struct mxt_finger {
-	int status;
-	int x;
-	int y;
-	int area;
-	int pressure;
-};
-
 /* Each client has this additional data */
 struct mxt_data {
 	struct i2c_client *client;
 	struct input_dev *input_dev;
+	char phys[64];		/* device physical location */
 	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;
 	unsigned int max_x;
 	unsigned int max_y;
+
+	/* Cached parameters from object table */
+	u8 T6_reportid;
+	u8 T9_reportid_min;
+	u8 T9_reportid_max;
 };
 
 static bool mxt_object_readable(unsigned int type)
 {
 	switch (type) {
-	case MXT_GEN_MESSAGE_T5:
 	case MXT_GEN_COMMAND_T6:
 	case MXT_GEN_POWER_T7:
 	case MXT_GEN_ACQUIRE_T8:
@@ -396,6 +389,7 @@ static int __mxt_read_reg(struct i2c_client *client,
 {
 	struct i2c_msg xfer[2];
 	u8 buf[2];
+	int ret;
 
 	buf[0] = reg & 0xff;
 	buf[1] = (reg >> 8) & 0xff;
@@ -412,12 +406,17 @@ static int __mxt_read_reg(struct i2c_client *client,
 	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;
+	ret = i2c_transfer(client->adapter, xfer, 2);
+	if (ret == 2) {
+		ret = 0;
+	} else {
+		if (ret >= 0)
+			ret = -EIO;
+		dev_err(&client->dev, "%s: i2c transfer failed (%d)\n",
+			__func__, ret);
 	}
 
-	return 0;
+	return ret;
 }
 
 static int mxt_read_reg(struct i2c_client *client, u16 reg, u8 *val)
@@ -425,27 +424,39 @@ 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)
+static int __mxt_write_reg(struct i2c_client *client, u16 reg, u16 len,
+			   const void *val)
 {
-	u8 buf[3];
+	u8 *buf;
+	size_t count;
+	int ret;
+
+	count = len + 2;
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 
 	buf[0] = reg & 0xff;
 	buf[1] = (reg >> 8) & 0xff;
-	buf[2] = val;
+	memcpy(&buf[2], val, len);
 
-	if (i2c_master_send(client, buf, 3) != 3) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
+	ret = i2c_master_send(client, buf, count);
+	if (ret == count) {
+		ret = 0;
+	} else {
+		if (ret >= 0)
+			ret = -EIO;
+		dev_err(&client->dev, "%s: i2c send failed (%d)\n",
+			__func__, ret);
 	}
 
-	return 0;
+	kfree(buf);
+	return ret;
 }
 
-static int mxt_read_object_table(struct i2c_client *client,
-				      u16 reg, u8 *object_buf)
+static int mxt_write_reg(struct i2c_client *client, u16 reg, u8 val)
 {
-	return __mxt_read_reg(client, reg, MXT_OBJECT_SIZE,
-				   object_buf);
+	return __mxt_write_reg(client, reg, 1, &val);
 }
 
 static struct mxt_object *
@@ -479,20 +490,6 @@ static int mxt_read_message(struct mxt_data *data,
 			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)
 {
@@ -507,75 +504,17 @@ static int mxt_write_object(struct mxt_data *data,
 	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_mt_slot(input_dev, id);
-		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER,
-				finger[id].status != MXT_RELEASE);
-
-		if (finger[id].status != MXT_RELEASE) {
-			finger_num++;
-			input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR,
-					finger[id].area);
-			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_report_abs(input_dev, ABS_MT_PRESSURE,
-					finger[id].pressure);
-		} else {
-			finger[id].status = 0;
-		}
-	}
-
-	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_report_abs(input_dev,
-				 ABS_PRESSURE, finger[single_id].pressure);
-	}
-
-	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];
+	struct input_dev *input_dev = data->input_dev;
 	int x;
 	int y;
 	int area;
 	int pressure;
 
-	/* 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] << 4) | ((message->message[3] >> 4) & 0xf);
 	y = (message->message[2] << 4) | ((message->message[3] & 0xf));
 	if (data->max_x < 1024)
@@ -586,30 +525,50 @@ static void mxt_input_touchevent(struct mxt_data *data,
 	area = message->message[4];
 	pressure = message->message[5];
 
-	dev_dbg(dev, "[%d] %s x: %d, y: %d, area: %d\n", id,
-		status & MXT_MOVE ? "moved" : "pressed",
-		x, y, area);
+	dev_dbg(dev,
+		"[%u] %c%c%c%c%c%c%c%c x: %5u y: %5u area: %3u amp: %3u\n",
+		id,
+		(status & MXT_DETECT) ? 'D' : '.',
+		(status & MXT_PRESS) ? 'P' : '.',
+		(status & MXT_RELEASE) ? 'R' : '.',
+		(status & MXT_MOVE) ? 'M' : '.',
+		(status & MXT_VECTOR) ? 'V' : '.',
+		(status & MXT_AMP) ? 'A' : '.',
+		(status & MXT_SUPPRESS) ? 'S' : '.',
+		(status & MXT_UNGRIP) ? 'U' : '.',
+		x, y, area, pressure);
+
+	input_mt_slot(input_dev, id);
+	input_mt_report_slot_state(input_dev, MT_TOOL_FINGER,
+				   status & MXT_DETECT);
+
+	if (status & MXT_DETECT) {
+		input_report_abs(input_dev, ABS_MT_POSITION_X, x);
+		input_report_abs(input_dev, ABS_MT_POSITION_Y, y);
+		input_report_abs(input_dev, ABS_MT_PRESSURE, pressure);
+		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR, area);
+	}
+}
 
-	finger[id].status = status & MXT_MOVE ?
-				MXT_MOVE : MXT_PRESS;
-	finger[id].x = x;
-	finger[id].y = y;
-	finger[id].area = area;
-	finger[id].pressure = pressure;
+static unsigned mxt_extract_T6_csum(const u8 *csum)
+{
+	return csum[0] | (csum[1] << 8) | (csum[2] << 16);
+}
 
-	mxt_input_report(data, id);
+static bool mxt_is_T9_message(struct mxt_data *data, struct mxt_message *msg)
+{
+	u8 id = msg->reportid;
+	return (id >= data->T9_reportid_min && id <= data->T9_reportid_max);
 }
 
 static irqreturn_t mxt_interrupt(int irq, void *dev_id)
 {
 	struct mxt_data *data = dev_id;
 	struct mxt_message message;
-	struct mxt_object *object;
+	const u8 *payload = &message.message[0];
 	struct device *dev = &data->client->dev;
-	int id;
 	u8 reportid;
-	u8 max_reportid;
-	u8 min_reportid;
+	bool update_input = false;
 
 	do {
 		if (mxt_read_message(data, &message)) {
@@ -619,21 +578,25 @@ static irqreturn_t mxt_interrupt(int irq, void *dev_id)
 
 		reportid = message.reportid;
 
-		/* whether reportid is thing of MXT_TOUCH_MULTI_T9 */
-		object = mxt_get_object(data, MXT_TOUCH_MULTI_T9);
-		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)
+		if (reportid == data->T6_reportid) {
+			u8 status = payload[0];
+			unsigned csum = mxt_extract_T6_csum(&payload[1]);
+			dev_dbg(dev, "Status: %02x Config Checksum: %06x\n",
+				status, csum);
+		} else if (mxt_is_T9_message(data, &message)) {
+			int id = reportid - data->T9_reportid_min;
 			mxt_input_touchevent(data, &message, id);
-		else
+			update_input = true;
+		} else {
 			mxt_dump_message(dev, &message);
+		}
 	} while (reportid != 0xff);
 
+	if (update_input) {
+		input_mt_report_pointer_emulation(data->input_dev, false);
+		input_sync(data->input_dev);
+	}
+
 end:
 	return IRQ_HANDLED;
 }
@@ -644,7 +607,8 @@ static int mxt_check_reg_init(struct mxt_data *data)
 	struct mxt_object *object;
 	struct device *dev = &data->client->dev;
 	int index = 0;
-	int i, j, config_offset;
+	int i, size;
+	int ret;
 
 	if (!pdata->config) {
 		dev_dbg(dev, "No cfg data defined, skipping reg init\n");
@@ -657,18 +621,17 @@ static int mxt_check_reg_init(struct mxt_data *data)
 		if (!mxt_object_writable(object->type))
 			continue;
 
-		for (j = 0;
-		     j < (object->size + 1) * (object->instances + 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]);
+		size = (object->size + 1) * (object->instances + 1);
+		if (index + size > pdata->config_length) {
+			dev_err(dev, "Not enough config data!\n");
+			return -EINVAL;
 		}
-		index += (object->size + 1) * (object->instances + 1);
+
+		ret = __mxt_write_reg(data->client, object->start_address,
+				size, &pdata->config[index]);
+		if (ret)
+			return ret;
+		index += size;
 	}
 
 	return 0;
@@ -749,68 +712,76 @@ 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);
+	/* Read 7-byte info block starting at address 0 */
+	error = __mxt_read_reg(client, MXT_INFO, sizeof(*info), info);
 	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)
 {
+	struct i2c_client *client = data->client;
+	size_t table_size;
 	int error;
 	int i;
-	u16 reg;
-	u8 reportid = 0;
-	u8 buf[MXT_OBJECT_SIZE];
+	u8 reportid;
+
+	table_size = data->info.object_num * sizeof(struct mxt_object);
+	error = __mxt_read_reg(client, MXT_OBJECT_START, table_size,
+			data->object_table);
+	if (error)
+		return error;
 
+	/* Valid Report IDs start counting from 1 */
+	reportid = 1;
 	for (i = 0; i < data->info.object_num; i++) {
 		struct mxt_object *object = data->object_table + i;
+		u8 min_id, max_id;
 
-		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];
+		le16_to_cpus(&object->start_address);
 
 		if (object->num_report_ids) {
+			min_id = reportid;
 			reportid += object->num_report_ids *
 					(object->instances + 1);
-			object->max_reportid = reportid;
+			max_id = reportid - 1;
+		} else {
+			min_id = 0;
+			max_id = 0;
+		}
+
+		dev_dbg(&data->client->dev,
+			"Type %2d Start %3d Size %3d Instances %2d ReportIDs %3u : %3u\n",
+			object->type, object->start_address, object->size + 1,
+			object->instances + 1, min_id, max_id);
+
+		switch (object->type) {
+		case MXT_GEN_COMMAND_T6:
+			data->T6_reportid = min_id;
+			break;
+		case MXT_TOUCH_MULTI_T9:
+			data->T9_reportid_min = min_id;
+			data->T9_reportid_max = max_id;
+			break;
 		}
 	}
 
 	return 0;
 }
 
+static void mxt_free_object_table(struct mxt_data *data)
+{
+	kfree(data->object_table);
+	data->object_table = NULL;
+	data->T6_reportid = 0;
+	data->T9_reportid_min = 0;
+	data->T9_reportid_max = 0;
+
+}
+
 static int mxt_initialize(struct mxt_data *data)
 {
 	struct i2c_client *client = data->client;
@@ -833,12 +804,12 @@ static int mxt_initialize(struct mxt_data *data)
 	/* Get object table information */
 	error = mxt_get_object_table(data);
 	if (error)
-		return error;
+		goto err_free_object_table;
 
 	/* Check register init values */
 	error = mxt_check_reg_init(data);
 	if (error)
-		return error;
+		goto err_free_object_table;
 
 	mxt_handle_pdata(data);
 
@@ -856,25 +827,29 @@ static int mxt_initialize(struct mxt_data *data)
 	/* Update matrix size at info struct */
 	error = mxt_read_reg(client, MXT_MATRIX_X_SIZE, &val);
 	if (error)
-		return error;
+		goto err_free_object_table;
 	info->matrix_xsize = val;
 
 	error = mxt_read_reg(client, MXT_MATRIX_Y_SIZE, &val);
 	if (error)
-		return error;
+		goto err_free_object_table;
 	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);
+			"Family ID: %u Variant ID: %u Major.Minor.Build: %u.%u.%02X\n",
+			info->family_id, info->variant_id, info->version >> 4,
+			info->version & 0xf, info->build);
 
 	dev_info(&client->dev,
-			"Matrix X Size: %d Matrix Y Size: %d Object Num: %d\n",
+			"Matrix X Size: %u Matrix Y Size: %u Object Num: %u\n",
 			info->matrix_xsize, info->matrix_ysize,
 			info->object_num);
 
 	return 0;
+
+err_free_object_table:
+	mxt_free_object_table(data);
+	return error;
 }
 
 static void mxt_calc_resolution(struct mxt_data *data)
@@ -891,6 +866,44 @@ static void mxt_calc_resolution(struct mxt_data *data)
 	}
 }
 
+/* Firmware Version is returned as Major.Minor.Build */
+static ssize_t mxt_fw_version_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	struct mxt_info *info = &data->info;
+	return scnprintf(buf, PAGE_SIZE, "%u.%u.%02X\n",
+			 info->version >> 4, info->version & 0xf, info->build);
+}
+
+/* Hardware Version is returned as FamilyID.VariantID */
+static ssize_t mxt_hw_version_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	struct mxt_info *info = &data->info;
+	return scnprintf(buf, PAGE_SIZE, "%u.%u\n",
+			 info->family_id, info->variant_id);
+}
+
+static ssize_t mxt_show_instance(char *buf, int count,
+				 struct mxt_object *object, int instance,
+				 const u8 *val)
+{
+	int i;
+
+	if (object->instances > 0)
+		count += scnprintf(buf + count, PAGE_SIZE - count,
+				   "Instance %u\n", instance);
+
+	for (i = 0; i < object->size + 1; i++)
+		count += scnprintf(buf + count, PAGE_SIZE - count,
+				"\t[%2u]: %02x (%d)\n", i, val[i], val[i]);
+	count += scnprintf(buf + count, PAGE_SIZE - count, "\n");
+
+	return count;
+}
+
 static ssize_t mxt_object_show(struct device *dev,
 				    struct device_attribute *attr, char *buf)
 {
@@ -899,43 +912,38 @@ static ssize_t mxt_object_show(struct device *dev,
 	int count = 0;
 	int i, j;
 	int error;
-	u8 val;
+	u8 *obuf;
 
+	/* Pre-allocate buffer large enough to hold max sized object. */
+	obuf = kmalloc(256, GFP_KERNEL);
+	if (!obuf)
+		return -ENOMEM;
+
+	error = 0;
 	for (i = 0; i < data->info.object_num; i++) {
 		object = data->object_table + i;
 
-		count += snprintf(buf + count, PAGE_SIZE - count,
-				"Object[%d] (Type %d)\n",
-				i + 1, object->type);
-		if (count >= PAGE_SIZE)
-			return PAGE_SIZE - 1;
-
-		if (!mxt_object_readable(object->type)) {
-			count += snprintf(buf + count, PAGE_SIZE - count,
-					"\n");
-			if (count >= PAGE_SIZE)
-				return PAGE_SIZE - 1;
+		if (!mxt_object_readable(object->type))
 			continue;
-		}
 
-		for (j = 0; j < object->size + 1; j++) {
-			error = mxt_read_object(data,
-						object->type, j, &val);
+		count += scnprintf(buf + count, PAGE_SIZE - count,
+				"T%u:\n", object->type);
+
+		for (j = 0; j < object->instances + 1; j++) {
+			u16 size = object->size + 1;
+			u16 addr = object->start_address + j * size;
+
+			error = __mxt_read_reg(data->client, addr, size, obuf);
 			if (error)
-				return error;
+				goto done;
 
-			count += snprintf(buf + count, PAGE_SIZE - count,
-					"\t[%2d]: %02x (%d)\n", j, val, val);
-			if (count >= PAGE_SIZE)
-				return PAGE_SIZE - 1;
+			count = mxt_show_instance(buf, count, object, j, obuf);
 		}
-
-		count += snprintf(buf + count, PAGE_SIZE - count, "\n");
-		if (count >= PAGE_SIZE)
-			return PAGE_SIZE - 1;
 	}
 
-	return count;
+done:
+	kfree(obuf);
+	return error ?: count;
 }
 
 static int mxt_load_fw(struct device *dev, const char *fn)
@@ -1028,8 +1036,7 @@ static ssize_t mxt_update_fw_store(struct device *dev,
 		/* Wait for reset */
 		msleep(MXT_FWRESET_TIME);
 
-		kfree(data->object_table);
-		data->object_table = NULL;
+		mxt_free_object_table(data);
 
 		mxt_initialize(data);
 	}
@@ -1043,10 +1050,14 @@ static ssize_t mxt_update_fw_store(struct device *dev,
 	return count;
 }
 
+static DEVICE_ATTR(fw_version, S_IRUGO, mxt_fw_version_show, NULL);
+static DEVICE_ATTR(hw_version, S_IRUGO, mxt_hw_version_show, NULL);
 static DEVICE_ATTR(object, S_IRUGO, mxt_object_show, NULL);
 static DEVICE_ATTR(update_fw, S_IWUSR, NULL, mxt_update_fw_store);
 
 static struct attribute *mxt_attrs[] = {
+	&dev_attr_fw_version.attr,
+	&dev_attr_hw_version.attr,
 	&dev_attr_object.attr,
 	&dev_attr_update_fw.attr,
 	NULL
@@ -1093,6 +1104,7 @@ static int __devinit mxt_probe(struct i2c_client *client,
 	struct mxt_data *data;
 	struct input_dev *input_dev;
 	int error;
+	unsigned int num_mt_slots;
 
 	if (!pdata)
 		return -EINVAL;
@@ -1106,6 +1118,10 @@ static int __devinit mxt_probe(struct i2c_client *client,
 	}
 
 	input_dev->name = "Atmel maXTouch Touchscreen";
+	snprintf(data->phys, sizeof(data->phys), "i2c-%u-%04x/input0",
+		 client->adapter->nr, client->addr);
+	input_dev->phys = data->phys;
+
 	input_dev->id.bustype = BUS_I2C;
 	input_dev->dev.parent = &client->dev;
 	input_dev->open = mxt_input_open;
@@ -1118,6 +1134,10 @@ static int __devinit mxt_probe(struct i2c_client *client,
 
 	mxt_calc_resolution(data);
 
+	error = mxt_initialize(data);
+	if (error)
+		goto err_free_mem;
+
 	__set_bit(EV_ABS, input_dev->evbit);
 	__set_bit(EV_KEY, input_dev->evbit);
 	__set_bit(BTN_TOUCH, input_dev->keybit);
@@ -1131,7 +1151,10 @@ static int __devinit mxt_probe(struct i2c_client *client,
 			     0, 255, 0, 0);
 
 	/* For multi touch */
-	input_mt_init_slots(input_dev, MXT_MAX_FINGER);
+	num_mt_slots = data->T9_reportid_max - data->T9_reportid_min + 1;
+	error = input_mt_init_slots(input_dev, num_mt_slots);
+	if (error)
+		goto err_free_object;
 	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,
@@ -1144,13 +1167,9 @@ static int __devinit mxt_probe(struct i2c_client *client,
 	input_set_drvdata(input_dev, data);
 	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 | IRQF_ONESHOT,
-				     client->dev.driver->name, data);
+				     client->name, data);
 	if (error) {
 		dev_err(&client->dev, "Failed to register interrupt\n");
 		goto err_free_object;
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
new file mode 100644
index 00000000000..9afc777a40a
--- /dev/null
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -0,0 +1,898 @@
+/*
+ * Copyright (C) 2012 Simon Budig, <simon.budig@kernelconcepts.de>
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+/*
+ * This is a driver for the EDT "Polytouch" family of touch controllers
+ * based on the FocalTech FT5x06 line of chips.
+ *
+ * Development of this driver has been sponsored by Glyn:
+ *    http://www.glyn.com/Products/Displays
+ */
+
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/i2c.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/input/mt.h>
+#include <linux/input/edt-ft5x06.h>
+
+#define MAX_SUPPORT_POINTS		5
+
+#define WORK_REGISTER_THRESHOLD		0x00
+#define WORK_REGISTER_REPORT_RATE	0x08
+#define WORK_REGISTER_GAIN		0x30
+#define WORK_REGISTER_OFFSET		0x31
+#define WORK_REGISTER_NUM_X		0x33
+#define WORK_REGISTER_NUM_Y		0x34
+
+#define WORK_REGISTER_OPMODE		0x3c
+#define FACTORY_REGISTER_OPMODE		0x01
+
+#define TOUCH_EVENT_DOWN		0x00
+#define TOUCH_EVENT_UP			0x01
+#define TOUCH_EVENT_ON			0x02
+#define TOUCH_EVENT_RESERVED		0x03
+
+#define EDT_NAME_LEN			23
+#define EDT_SWITCH_MODE_RETRIES		10
+#define EDT_SWITCH_MODE_DELAY		5 /* msec */
+#define EDT_RAW_DATA_RETRIES		100
+#define EDT_RAW_DATA_DELAY		1 /* msec */
+
+struct edt_ft5x06_ts_data {
+	struct i2c_client *client;
+	struct input_dev *input;
+	u16 num_x;
+	u16 num_y;
+
+#if defined(CONFIG_DEBUG_FS)
+	struct dentry *debug_dir;
+	u8 *raw_buffer;
+	size_t raw_bufsize;
+#endif
+
+	struct mutex mutex;
+	bool factory_mode;
+	int threshold;
+	int gain;
+	int offset;
+	int report_rate;
+
+	char name[EDT_NAME_LEN];
+};
+
+static int edt_ft5x06_ts_readwrite(struct i2c_client *client,
+				   u16 wr_len, u8 *wr_buf,
+				   u16 rd_len, u8 *rd_buf)
+{
+	struct i2c_msg wrmsg[2];
+	int i = 0;
+	int ret;
+
+	if (wr_len) {
+		wrmsg[i].addr  = client->addr;
+		wrmsg[i].flags = 0;
+		wrmsg[i].len = wr_len;
+		wrmsg[i].buf = wr_buf;
+		i++;
+	}
+	if (rd_len) {
+		wrmsg[i].addr  = client->addr;
+		wrmsg[i].flags = I2C_M_RD;
+		wrmsg[i].len = rd_len;
+		wrmsg[i].buf = rd_buf;
+		i++;
+	}
+
+	ret = i2c_transfer(client->adapter, wrmsg, i);
+	if (ret < 0)
+		return ret;
+	if (ret != i)
+		return -EIO;
+
+	return 0;
+}
+
+static bool edt_ft5x06_ts_check_crc(struct edt_ft5x06_ts_data *tsdata,
+				    u8 *buf, int buflen)
+{
+	int i;
+	u8 crc = 0;
+
+	for (i = 0; i < buflen - 1; i++)
+		crc ^= buf[i];
+
+	if (crc != buf[buflen-1]) {
+		dev_err_ratelimited(&tsdata->client->dev,
+				    "crc error: 0x%02x expected, got 0x%02x\n",
+				    crc, buf[buflen-1]);
+		return false;
+	}
+
+	return true;
+}
+
+static irqreturn_t edt_ft5x06_ts_isr(int irq, void *dev_id)
+{
+	struct edt_ft5x06_ts_data *tsdata = dev_id;
+	struct device *dev = &tsdata->client->dev;
+	u8 cmd = 0xf9;
+	u8 rdbuf[26];
+	int i, type, x, y, id;
+	int error;
+
+	memset(rdbuf, 0, sizeof(rdbuf));
+
+	error = edt_ft5x06_ts_readwrite(tsdata->client,
+					sizeof(cmd), &cmd,
+					sizeof(rdbuf), rdbuf);
+	if (error) {
+		dev_err_ratelimited(dev, "Unable to fetch data, error: %d\n",
+				    error);
+		goto out;
+	}
+
+	if (rdbuf[0] != 0xaa || rdbuf[1] != 0xaa || rdbuf[2] != 26) {
+		dev_err_ratelimited(dev, "Unexpected header: %02x%02x%02x!\n",
+				    rdbuf[0], rdbuf[1], rdbuf[2]);
+		goto out;
+	}
+
+	if (!edt_ft5x06_ts_check_crc(tsdata, rdbuf, 26))
+		goto out;
+
+	for (i = 0; i < MAX_SUPPORT_POINTS; i++) {
+		u8 *buf = &rdbuf[i * 4 + 5];
+		bool down;
+
+		type = buf[0] >> 6;
+		/* ignore Reserved events */
+		if (type == TOUCH_EVENT_RESERVED)
+			continue;
+
+		x = ((buf[0] << 8) | buf[1]) & 0x0fff;
+		y = ((buf[2] << 8) | buf[3]) & 0x0fff;
+		id = (buf[2] >> 4) & 0x0f;
+		down = (type != TOUCH_EVENT_UP);
+
+		input_mt_slot(tsdata->input, id);
+		input_mt_report_slot_state(tsdata->input, MT_TOOL_FINGER, down);
+
+		if (!down)
+			continue;
+
+		input_report_abs(tsdata->input, ABS_MT_POSITION_X, x);
+		input_report_abs(tsdata->input, ABS_MT_POSITION_Y, y);
+	}
+
+	input_mt_report_pointer_emulation(tsdata->input, true);
+	input_sync(tsdata->input);
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int edt_ft5x06_register_write(struct edt_ft5x06_ts_data *tsdata,
+				     u8 addr, u8 value)
+{
+	u8 wrbuf[4];
+
+	wrbuf[0] = tsdata->factory_mode ? 0xf3 : 0xfc;
+	wrbuf[1] = tsdata->factory_mode ? addr & 0x7f : addr & 0x3f;
+	wrbuf[2] = value;
+	wrbuf[3] = wrbuf[0] ^ wrbuf[1] ^ wrbuf[2];
+
+	return edt_ft5x06_ts_readwrite(tsdata->client, 4, wrbuf, 0, NULL);
+}
+
+static int edt_ft5x06_register_read(struct edt_ft5x06_ts_data *tsdata,
+				    u8 addr)
+{
+	u8 wrbuf[2], rdbuf[2];
+	int error;
+
+	wrbuf[0] = tsdata->factory_mode ? 0xf3 : 0xfc;
+	wrbuf[1] = tsdata->factory_mode ? addr & 0x7f : addr & 0x3f;
+	wrbuf[1] |= tsdata->factory_mode ? 0x80 : 0x40;
+
+	error = edt_ft5x06_ts_readwrite(tsdata->client, 2, wrbuf, 2, rdbuf);
+	if (error)
+		return error;
+
+	if ((wrbuf[0] ^ wrbuf[1] ^ rdbuf[0]) != rdbuf[1]) {
+		dev_err(&tsdata->client->dev,
+			"crc error: 0x%02x expected, got 0x%02x\n",
+			wrbuf[0] ^ wrbuf[1] ^ rdbuf[0], rdbuf[1]);
+		return -EIO;
+	}
+
+	return rdbuf[0];
+}
+
+struct edt_ft5x06_attribute {
+	struct device_attribute dattr;
+	size_t field_offset;
+	u8 limit_low;
+	u8 limit_high;
+	u8 addr;
+};
+
+#define EDT_ATTR(_field, _mode, _addr, _limit_low, _limit_high)		\
+	struct edt_ft5x06_attribute edt_ft5x06_attr_##_field = {	\
+		.dattr = __ATTR(_field, _mode,				\
+				edt_ft5x06_setting_show,		\
+				edt_ft5x06_setting_store),		\
+		.field_offset =						\
+			offsetof(struct edt_ft5x06_ts_data, _field),	\
+		.limit_low = _limit_low,				\
+		.limit_high = _limit_high,				\
+		.addr = _addr,						\
+	}
+
+static ssize_t edt_ft5x06_setting_show(struct device *dev,
+				       struct device_attribute *dattr,
+				       char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct edt_ft5x06_ts_data *tsdata = i2c_get_clientdata(client);
+	struct edt_ft5x06_attribute *attr =
+			container_of(dattr, struct edt_ft5x06_attribute, dattr);
+	u8 *field = (u8 *)((char *)tsdata + attr->field_offset);
+	int val;
+	size_t count = 0;
+	int error = 0;
+
+	mutex_lock(&tsdata->mutex);
+
+	if (tsdata->factory_mode) {
+		error = -EIO;
+		goto out;
+	}
+
+	val = edt_ft5x06_register_read(tsdata, attr->addr);
+	if (val < 0) {
+		error = val;
+		dev_err(&tsdata->client->dev,
+			"Failed to fetch attribute %s, error %d\n",
+			dattr->attr.name, error);
+		goto out;
+	}
+
+	if (val != *field) {
+		dev_warn(&tsdata->client->dev,
+			 "%s: read (%d) and stored value (%d) differ\n",
+			 dattr->attr.name, val, *field);
+		*field = val;
+	}
+
+	count = scnprintf(buf, PAGE_SIZE, "%d\n", val);
+out:
+	mutex_unlock(&tsdata->mutex);
+	return error ?: count;
+}
+
+static ssize_t edt_ft5x06_setting_store(struct device *dev,
+					struct device_attribute *dattr,
+					const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct edt_ft5x06_ts_data *tsdata = i2c_get_clientdata(client);
+	struct edt_ft5x06_attribute *attr =
+			container_of(dattr, struct edt_ft5x06_attribute, dattr);
+	u8 *field = (u8 *)((char *)tsdata + attr->field_offset);
+	unsigned int val;
+	int error;
+
+	mutex_lock(&tsdata->mutex);
+
+	if (tsdata->factory_mode) {
+		error = -EIO;
+		goto out;
+	}
+
+	error = kstrtouint(buf, 0, &val);
+	if (error)
+		goto out;
+
+	if (val < attr->limit_low || val > attr->limit_high) {
+		error = -ERANGE;
+		goto out;
+	}
+
+	error = edt_ft5x06_register_write(tsdata, attr->addr, val);
+	if (error) {
+		dev_err(&tsdata->client->dev,
+			"Failed to update attribute %s, error: %d\n",
+			dattr->attr.name, error);
+		goto out;
+	}
+
+	*field = val;
+
+out:
+	mutex_unlock(&tsdata->mutex);
+	return error ?: count;
+}
+
+static EDT_ATTR(gain, S_IWUSR | S_IRUGO, WORK_REGISTER_GAIN, 0, 31);
+static EDT_ATTR(offset, S_IWUSR | S_IRUGO, WORK_REGISTER_OFFSET, 0, 31);
+static EDT_ATTR(threshold, S_IWUSR | S_IRUGO,
+		WORK_REGISTER_THRESHOLD, 20, 80);
+static EDT_ATTR(report_rate, S_IWUSR | S_IRUGO,
+		WORK_REGISTER_REPORT_RATE, 3, 14);
+
+static struct attribute *edt_ft5x06_attrs[] = {
+	&edt_ft5x06_attr_gain.dattr.attr,
+	&edt_ft5x06_attr_offset.dattr.attr,
+	&edt_ft5x06_attr_threshold.dattr.attr,
+	&edt_ft5x06_attr_report_rate.dattr.attr,
+	NULL
+};
+
+static const struct attribute_group edt_ft5x06_attr_group = {
+	.attrs = edt_ft5x06_attrs,
+};
+
+#ifdef CONFIG_DEBUG_FS
+static int edt_ft5x06_factory_mode(struct edt_ft5x06_ts_data *tsdata)
+{
+	struct i2c_client *client = tsdata->client;
+	int retries = EDT_SWITCH_MODE_RETRIES;
+	int ret;
+	int error;
+
+	disable_irq(client->irq);
+
+	if (!tsdata->raw_buffer) {
+		tsdata->raw_bufsize = tsdata->num_x * tsdata->num_y *
+				      sizeof(u16);
+		tsdata->raw_buffer = kzalloc(tsdata->raw_bufsize, GFP_KERNEL);
+		if (!tsdata->raw_buffer) {
+			error = -ENOMEM;
+			goto err_out;
+		}
+	}
+
+	/* mode register is 0x3c when in the work mode */
+	error = edt_ft5x06_register_write(tsdata, WORK_REGISTER_OPMODE, 0x03);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to switch to factory mode, error %d\n", error);
+		goto err_out;
+	}
+
+	tsdata->factory_mode = true;
+	do {
+		mdelay(EDT_SWITCH_MODE_DELAY);
+		/* mode register is 0x01 when in factory mode */
+		ret = edt_ft5x06_register_read(tsdata, FACTORY_REGISTER_OPMODE);
+		if (ret == 0x03)
+			break;
+	} while (--retries > 0);
+
+	if (retries == 0) {
+		dev_err(&client->dev, "not in factory mode after %dms.\n",
+			EDT_SWITCH_MODE_RETRIES * EDT_SWITCH_MODE_DELAY);
+		error = -EIO;
+		goto err_out;
+	}
+
+	return 0;
+
+err_out:
+	kfree(tsdata->raw_buffer);
+	tsdata->raw_buffer = NULL;
+	tsdata->factory_mode = false;
+	enable_irq(client->irq);
+
+	return error;
+}
+
+static int edt_ft5x06_work_mode(struct edt_ft5x06_ts_data *tsdata)
+{
+	struct i2c_client *client = tsdata->client;
+	int retries = EDT_SWITCH_MODE_RETRIES;
+	int ret;
+	int error;
+
+	/* mode register is 0x01 when in the factory mode */
+	error = edt_ft5x06_register_write(tsdata, FACTORY_REGISTER_OPMODE, 0x1);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to switch to work mode, error: %d\n", error);
+		return error;
+	}
+
+	tsdata->factory_mode = false;
+
+	do {
+		mdelay(EDT_SWITCH_MODE_DELAY);
+		/* mode register is 0x01 when in factory mode */
+		ret = edt_ft5x06_register_read(tsdata, WORK_REGISTER_OPMODE);
+		if (ret == 0x01)
+			break;
+	} while (--retries > 0);
+
+	if (retries == 0) {
+		dev_err(&client->dev, "not in work mode after %dms.\n",
+			EDT_SWITCH_MODE_RETRIES * EDT_SWITCH_MODE_DELAY);
+		tsdata->factory_mode = true;
+		return -EIO;
+	}
+
+	if (tsdata->raw_buffer)
+		kfree(tsdata->raw_buffer);
+	tsdata->raw_buffer = NULL;
+
+	/* restore parameters */
+	edt_ft5x06_register_write(tsdata, WORK_REGISTER_THRESHOLD,
+				  tsdata->threshold);
+	edt_ft5x06_register_write(tsdata, WORK_REGISTER_GAIN,
+				  tsdata->gain);
+	edt_ft5x06_register_write(tsdata, WORK_REGISTER_OFFSET,
+				  tsdata->offset);
+	edt_ft5x06_register_write(tsdata, WORK_REGISTER_REPORT_RATE,
+				  tsdata->report_rate);
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static int edt_ft5x06_debugfs_mode_get(void *data, u64 *mode)
+{
+	struct edt_ft5x06_ts_data *tsdata = data;
+
+	*mode = tsdata->factory_mode;
+
+	return 0;
+};
+
+static int edt_ft5x06_debugfs_mode_set(void *data, u64 mode)
+{
+	struct edt_ft5x06_ts_data *tsdata = data;
+	int retval = 0;
+
+	if (mode > 1)
+		return -ERANGE;
+
+	mutex_lock(&tsdata->mutex);
+
+	if (mode != tsdata->factory_mode) {
+		retval = mode ? edt_ft5x06_factory_mode(tsdata) :
+			        edt_ft5x06_work_mode(tsdata);
+	}
+
+	mutex_unlock(&tsdata->mutex);
+
+	return retval;
+};
+
+DEFINE_SIMPLE_ATTRIBUTE(debugfs_mode_fops, edt_ft5x06_debugfs_mode_get,
+			edt_ft5x06_debugfs_mode_set, "%llu\n");
+
+static int edt_ft5x06_debugfs_raw_data_open(struct inode *inode,
+					    struct file *file)
+{
+	file->private_data = inode->i_private;
+
+	return 0;
+}
+
+static ssize_t edt_ft5x06_debugfs_raw_data_read(struct file *file,
+				char __user *buf, size_t count, loff_t *off)
+{
+	struct edt_ft5x06_ts_data *tsdata = file->private_data;
+	struct i2c_client *client = tsdata->client;
+	int retries  = EDT_RAW_DATA_RETRIES;
+	int val, i, error;
+	size_t read = 0;
+	int colbytes;
+	char wrbuf[3];
+	u8 *rdbuf;
+
+	if (*off < 0 || *off >= tsdata->raw_bufsize)
+		return 0;
+
+	mutex_lock(&tsdata->mutex);
+
+	if (!tsdata->factory_mode || !tsdata->raw_buffer) {
+		error = -EIO;
+		goto out;
+	}
+
+	error = edt_ft5x06_register_write(tsdata, 0x08, 0x01);
+	if (error) {
+		dev_dbg(&client->dev,
+			"failed to write 0x08 register, error %d\n", error);
+		goto out;
+	}
+
+	do {
+		msleep(EDT_RAW_DATA_DELAY);
+		val = edt_ft5x06_register_read(tsdata, 0x08);
+		if (val < 1)
+			break;
+	} while (--retries > 0);
+
+	if (val < 0) {
+		error = val;
+		dev_dbg(&client->dev,
+			"failed to read 0x08 register, error %d\n", error);
+		goto out;
+	}
+
+	if (retries == 0) {
+		dev_dbg(&client->dev,
+			"timed out waiting for register to settle\n");
+		error = -ETIMEDOUT;
+		goto out;
+	}
+
+	rdbuf = tsdata->raw_buffer;
+	colbytes = tsdata->num_y * sizeof(u16);
+
+	wrbuf[0] = 0xf5;
+	wrbuf[1] = 0x0e;
+	for (i = 0; i < tsdata->num_x; i++) {
+		wrbuf[2] = i;  /* column index */
+		error = edt_ft5x06_ts_readwrite(tsdata->client,
+						sizeof(wrbuf), wrbuf,
+						colbytes, rdbuf);
+		if (error)
+			goto out;
+
+		rdbuf += colbytes;
+	}
+
+	read = min_t(size_t, count, tsdata->raw_bufsize - *off);
+	error = copy_to_user(buf, tsdata->raw_buffer + *off, read);
+	if (!error)
+		*off += read;
+out:
+	mutex_unlock(&tsdata->mutex);
+	return error ?: read;
+};
+
+
+static const struct file_operations debugfs_raw_data_fops = {
+	.open = edt_ft5x06_debugfs_raw_data_open,
+	.read = edt_ft5x06_debugfs_raw_data_read,
+};
+
+static void __devinit
+edt_ft5x06_ts_prepare_debugfs(struct edt_ft5x06_ts_data *tsdata,
+			      const char *debugfs_name)
+{
+	tsdata->debug_dir = debugfs_create_dir(debugfs_name, NULL);
+	if (!tsdata->debug_dir)
+		return;
+
+	debugfs_create_u16("num_x", S_IRUSR, tsdata->debug_dir, &tsdata->num_x);
+	debugfs_create_u16("num_y", S_IRUSR, tsdata->debug_dir, &tsdata->num_y);
+
+	debugfs_create_file("mode", S_IRUSR | S_IWUSR,
+			    tsdata->debug_dir, tsdata, &debugfs_mode_fops);
+	debugfs_create_file("raw_data", S_IRUSR,
+			    tsdata->debug_dir, tsdata, &debugfs_raw_data_fops);
+}
+
+static void __devexit
+edt_ft5x06_ts_teardown_debugfs(struct edt_ft5x06_ts_data *tsdata)
+{
+	if (tsdata->debug_dir)
+		debugfs_remove_recursive(tsdata->debug_dir);
+}
+
+#else
+
+static inline void
+edt_ft5x06_ts_prepare_debugfs(struct edt_ft5x06_ts_data *tsdata,
+			      const char *debugfs_name)
+{
+}
+
+static inline void
+edt_ft5x06_ts_teardown_debugfs(struct edt_ft5x06_ts_data *tsdata)
+{
+}
+
+#endif /* CONFIG_DEBUGFS */
+
+
+
+static int __devinit edt_ft5x06_ts_reset(struct i2c_client *client,
+					 int reset_pin)
+{
+	int error;
+
+	if (gpio_is_valid(reset_pin)) {
+		/* this pulls reset down, enabling the low active reset */
+		error = gpio_request_one(reset_pin, GPIOF_OUT_INIT_LOW,
+					 "edt-ft5x06 reset");
+		if (error) {
+			dev_err(&client->dev,
+				"Failed to request GPIO %d as reset pin, error %d\n",
+				reset_pin, error);
+			return error;
+		}
+
+		mdelay(50);
+		gpio_set_value(reset_pin, 1);
+		mdelay(100);
+	}
+
+	return 0;
+}
+
+static int __devinit edt_ft5x06_ts_identify(struct i2c_client *client,
+					    char *model_name,
+					    char *fw_version)
+{
+	u8 rdbuf[EDT_NAME_LEN];
+	char *p;
+	int error;
+
+	error = edt_ft5x06_ts_readwrite(client, 1, "\xbb",
+					EDT_NAME_LEN - 1, rdbuf);
+	if (error)
+		return error;
+
+	/* remove last '$' end marker */
+	rdbuf[EDT_NAME_LEN - 1] = '\0';
+	if (rdbuf[EDT_NAME_LEN - 2] == '$')
+		rdbuf[EDT_NAME_LEN - 2] = '\0';
+
+	/* look for Model/Version separator */
+	p = strchr(rdbuf, '*');
+	if (p)
+		*p++ = '\0';
+
+	strlcpy(model_name, rdbuf + 1, EDT_NAME_LEN);
+	strlcpy(fw_version, p ? p : "", EDT_NAME_LEN);
+
+	return 0;
+}
+
+#define EDT_ATTR_CHECKSET(name, reg) \
+	if (pdata->name >= edt_ft5x06_attr_##name.limit_low &&		\
+	    pdata->name <= edt_ft5x06_attr_##name.limit_high)		\
+		edt_ft5x06_register_write(tsdata, reg, pdata->name)
+
+static void __devinit
+edt_ft5x06_ts_get_defaults(struct edt_ft5x06_ts_data *tsdata,
+			   const struct edt_ft5x06_platform_data *pdata)
+{
+	if (!pdata->use_parameters)
+		return;
+
+	/* pick up defaults from the platform data */
+	EDT_ATTR_CHECKSET(threshold, WORK_REGISTER_THRESHOLD);
+	EDT_ATTR_CHECKSET(gain, WORK_REGISTER_GAIN);
+	EDT_ATTR_CHECKSET(offset, WORK_REGISTER_OFFSET);
+	EDT_ATTR_CHECKSET(report_rate, WORK_REGISTER_REPORT_RATE);
+}
+
+static void __devinit
+edt_ft5x06_ts_get_parameters(struct edt_ft5x06_ts_data *tsdata)
+{
+	tsdata->threshold = edt_ft5x06_register_read(tsdata,
+						     WORK_REGISTER_THRESHOLD);
+	tsdata->gain = edt_ft5x06_register_read(tsdata, WORK_REGISTER_GAIN);
+	tsdata->offset = edt_ft5x06_register_read(tsdata, WORK_REGISTER_OFFSET);
+	tsdata->report_rate = edt_ft5x06_register_read(tsdata,
+						WORK_REGISTER_REPORT_RATE);
+	tsdata->num_x = edt_ft5x06_register_read(tsdata, WORK_REGISTER_NUM_X);
+	tsdata->num_y = edt_ft5x06_register_read(tsdata, WORK_REGISTER_NUM_Y);
+}
+
+static int __devinit edt_ft5x06_ts_probe(struct i2c_client *client,
+					 const struct i2c_device_id *id)
+{
+	const struct edt_ft5x06_platform_data *pdata =
+						client->dev.platform_data;
+	struct edt_ft5x06_ts_data *tsdata;
+	struct input_dev *input;
+	int error;
+	char fw_version[EDT_NAME_LEN];
+
+	dev_dbg(&client->dev, "probing for EDT FT5x06 I2C\n");
+
+	if (!pdata) {
+		dev_err(&client->dev, "no platform data?\n");
+		return -EINVAL;
+	}
+
+	error = edt_ft5x06_ts_reset(client, pdata->reset_pin);
+	if (error)
+		return error;
+
+	if (gpio_is_valid(pdata->irq_pin)) {
+		error = gpio_request_one(pdata->irq_pin,
+					 GPIOF_IN, "edt-ft5x06 irq");
+		if (error) {
+			dev_err(&client->dev,
+				"Failed to request GPIO %d, error %d\n",
+				pdata->irq_pin, error);
+			return error;
+		}
+	}
+
+	tsdata = kzalloc(sizeof(*tsdata), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!tsdata || !input) {
+		dev_err(&client->dev, "failed to allocate driver data.\n");
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	mutex_init(&tsdata->mutex);
+	tsdata->client = client;
+	tsdata->input = input;
+	tsdata->factory_mode = false;
+
+	error = edt_ft5x06_ts_identify(client, tsdata->name, fw_version);
+	if (error) {
+		dev_err(&client->dev, "touchscreen probe failed\n");
+		goto err_free_mem;
+	}
+
+	edt_ft5x06_ts_get_defaults(tsdata, pdata);
+	edt_ft5x06_ts_get_parameters(tsdata);
+
+	dev_dbg(&client->dev,
+		"Model \"%s\", Rev. \"%s\", %dx%d sensors\n",
+		tsdata->name, fw_version, tsdata->num_x, tsdata->num_y);
+
+	input->name = tsdata->name;
+	input->id.bustype = BUS_I2C;
+	input->dev.parent = &client->dev;
+
+	__set_bit(EV_SYN, input->evbit);
+	__set_bit(EV_KEY, input->evbit);
+	__set_bit(EV_ABS, input->evbit);
+	__set_bit(BTN_TOUCH, input->keybit);
+	input_set_abs_params(input, ABS_X, 0, tsdata->num_x * 64 - 1, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, tsdata->num_y * 64 - 1, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_X,
+			     0, tsdata->num_x * 64 - 1, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_Y,
+			     0, tsdata->num_y * 64 - 1, 0, 0);
+	error = input_mt_init_slots(input, MAX_SUPPORT_POINTS);
+	if (error) {
+		dev_err(&client->dev, "Unable to init MT slots.\n");
+		goto err_free_mem;
+	}
+
+	input_set_drvdata(input, tsdata);
+	i2c_set_clientdata(client, tsdata);
+
+	error = request_threaded_irq(client->irq, NULL, edt_ft5x06_ts_isr,
+				     IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				     client->name, tsdata);
+	if (error) {
+		dev_err(&client->dev, "Unable to request touchscreen IRQ.\n");
+		goto err_free_mem;
+	}
+
+	error = sysfs_create_group(&client->dev.kobj, &edt_ft5x06_attr_group);
+	if (error)
+		goto err_free_irq;
+
+	error = input_register_device(input);
+	if (error)
+		goto err_remove_attrs;
+
+	edt_ft5x06_ts_prepare_debugfs(tsdata, dev_driver_string(&client->dev));
+	device_init_wakeup(&client->dev, 1);
+
+	dev_dbg(&client->dev,
+		"EDT FT5x06 initialized: IRQ pin %d, Reset pin %d.\n",
+		pdata->irq_pin, pdata->reset_pin);
+
+	return 0;
+
+err_remove_attrs:
+	sysfs_remove_group(&client->dev.kobj, &edt_ft5x06_attr_group);
+err_free_irq:
+	free_irq(client->irq, tsdata);
+err_free_mem:
+	input_free_device(input);
+	kfree(tsdata);
+
+	if (gpio_is_valid(pdata->irq_pin))
+		gpio_free(pdata->irq_pin);
+
+	return error;
+}
+
+static int __devexit edt_ft5x06_ts_remove(struct i2c_client *client)
+{
+	const struct edt_ft5x06_platform_data *pdata =
+						dev_get_platdata(&client->dev);
+	struct edt_ft5x06_ts_data *tsdata = i2c_get_clientdata(client);
+
+	edt_ft5x06_ts_teardown_debugfs(tsdata);
+	sysfs_remove_group(&client->dev.kobj, &edt_ft5x06_attr_group);
+
+	free_irq(client->irq, tsdata);
+	input_unregister_device(tsdata->input);
+
+	if (gpio_is_valid(pdata->irq_pin))
+		gpio_free(pdata->irq_pin);
+	if (gpio_is_valid(pdata->reset_pin))
+		gpio_free(pdata->reset_pin);
+
+	kfree(tsdata->raw_buffer);
+	kfree(tsdata);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int edt_ft5x06_ts_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(client->irq);
+
+	return 0;
+}
+
+static int edt_ft5x06_ts_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(client->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(edt_ft5x06_ts_pm_ops,
+			 edt_ft5x06_ts_suspend, edt_ft5x06_ts_resume);
+
+static const struct i2c_device_id edt_ft5x06_ts_id[] = {
+	{ "edt-ft5x06", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, edt_ft5x06_ts_id);
+
+static struct i2c_driver edt_ft5x06_ts_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "edt_ft5x06",
+		.pm = &edt_ft5x06_ts_pm_ops,
+	},
+	.id_table = edt_ft5x06_ts_id,
+	.probe    = edt_ft5x06_ts_probe,
+	.remove   = __devexit_p(edt_ft5x06_ts_remove),
+};
+
+module_i2c_driver(edt_ft5x06_ts_driver);
+
+MODULE_AUTHOR("Simon Budig <simon.budig@kernelconcepts.de>");
+MODULE_DESCRIPTION("EDT FT5x06 I2C Touchscreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/jornada720_ts.c b/drivers/input/touchscreen/jornada720_ts.c
index d9be6eac99b..7f03d1bd916 100644
--- a/drivers/input/touchscreen/jornada720_ts.c
+++ b/drivers/input/touchscreen/jornada720_ts.c
@@ -19,6 +19,7 @@
 #include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/io.h>
 
 #include <mach/hardware.h>
 #include <mach/jornada720.h>
diff --git a/drivers/input/touchscreen/mms114.c b/drivers/input/touchscreen/mms114.c
new file mode 100644
index 00000000000..49c44bbf548
--- /dev/null
+++ b/drivers/input/touchscreen/mms114.c
@@ -0,0 +1,544 @@
+/*
+ * Copyright (C) 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/i2c/mms114.h>
+#include <linux/input/mt.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+/* Write only registers */
+#define MMS114_MODE_CONTROL		0x01
+#define MMS114_OPERATION_MODE_MASK	0xE
+#define MMS114_ACTIVE			(1 << 1)
+
+#define MMS114_XY_RESOLUTION_H		0x02
+#define MMS114_X_RESOLUTION		0x03
+#define MMS114_Y_RESOLUTION		0x04
+#define MMS114_CONTACT_THRESHOLD	0x05
+#define MMS114_MOVING_THRESHOLD		0x06
+
+/* Read only registers */
+#define MMS114_PACKET_SIZE		0x0F
+#define MMS114_INFOMATION		0x10
+#define MMS114_TSP_REV			0xF0
+
+/* Minimum delay time is 50us between stop and start signal of i2c */
+#define MMS114_I2C_DELAY		50
+
+/* 200ms needs after power on */
+#define MMS114_POWERON_DELAY		200
+
+/* Touchscreen absolute values */
+#define MMS114_MAX_AREA			0xff
+
+#define MMS114_MAX_TOUCH		10
+#define MMS114_PACKET_NUM		8
+
+/* Touch type */
+#define MMS114_TYPE_NONE		0
+#define MMS114_TYPE_TOUCHSCREEN		1
+#define MMS114_TYPE_TOUCHKEY		2
+
+struct mms114_data {
+	struct i2c_client	*client;
+	struct input_dev	*input_dev;
+	struct regulator	*core_reg;
+	struct regulator	*io_reg;
+	const struct mms114_platform_data	*pdata;
+
+	/* Use cache data for mode control register(write only) */
+	u8			cache_mode_control;
+};
+
+struct mms114_touch {
+	u8 id:4, reserved_bit4:1, type:2, pressed:1;
+	u8 x_hi:4, y_hi:4;
+	u8 x_lo;
+	u8 y_lo;
+	u8 width;
+	u8 strength;
+	u8 reserved[2];
+} __packed;
+
+static int __mms114_read_reg(struct mms114_data *data, unsigned int reg,
+			     unsigned int len, u8 *val)
+{
+	struct i2c_client *client = data->client;
+	struct i2c_msg xfer[2];
+	u8 buf = reg & 0xff;
+	int error;
+
+	if (reg <= MMS114_MODE_CONTROL && reg + len > MMS114_MODE_CONTROL)
+		BUG();
+
+	/* Write register: use repeated start */
+	xfer[0].addr = client->addr;
+	xfer[0].flags = I2C_M_TEN | I2C_M_NOSTART;
+	xfer[0].len = 1;
+	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;
+
+	error = i2c_transfer(client->adapter, xfer, 2);
+	if (error != 2) {
+		dev_err(&client->dev,
+			"%s: i2c transfer failed (%d)\n", __func__, error);
+		return error < 0 ? error : -EIO;
+	}
+	udelay(MMS114_I2C_DELAY);
+
+	return 0;
+}
+
+static int mms114_read_reg(struct mms114_data *data, unsigned int reg)
+{
+	u8 val;
+	int error;
+
+	if (reg == MMS114_MODE_CONTROL)
+		return data->cache_mode_control;
+
+	error = __mms114_read_reg(data, reg, 1, &val);
+	return error < 0 ? error : val;
+}
+
+static int mms114_write_reg(struct mms114_data *data, unsigned int reg,
+			    unsigned int val)
+{
+	struct i2c_client *client = data->client;
+	u8 buf[2];
+	int error;
+
+	buf[0] = reg & 0xff;
+	buf[1] = val & 0xff;
+
+	error = i2c_master_send(client, buf, 2);
+	if (error != 2) {
+		dev_err(&client->dev,
+			"%s: i2c send failed (%d)\n", __func__, error);
+		return error < 0 ? error : -EIO;
+	}
+	udelay(MMS114_I2C_DELAY);
+
+	if (reg == MMS114_MODE_CONTROL)
+		data->cache_mode_control = val;
+
+	return 0;
+}
+
+static void mms114_process_mt(struct mms114_data *data, struct mms114_touch *touch)
+{
+	const struct mms114_platform_data *pdata = data->pdata;
+	struct i2c_client *client = data->client;
+	struct input_dev *input_dev = data->input_dev;
+	unsigned int id;
+	unsigned int x;
+	unsigned int y;
+
+	if (touch->id > MMS114_MAX_TOUCH) {
+		dev_err(&client->dev, "Wrong touch id (%d)\n", touch->id);
+		return;
+	}
+
+	if (touch->type != MMS114_TYPE_TOUCHSCREEN) {
+		dev_err(&client->dev, "Wrong touch type (%d)\n", touch->type);
+		return;
+	}
+
+	id = touch->id - 1;
+	x = touch->x_lo | touch->x_hi << 8;
+	y = touch->y_lo | touch->y_hi << 8;
+	if (x > pdata->x_size || y > pdata->y_size) {
+		dev_dbg(&client->dev,
+			"Wrong touch coordinates (%d, %d)\n", x, y);
+		return;
+	}
+
+	if (pdata->x_invert)
+		x = pdata->x_size - x;
+	if (pdata->y_invert)
+		y = pdata->y_size - y;
+
+	dev_dbg(&client->dev,
+		"id: %d, type: %d, pressed: %d, x: %d, y: %d, width: %d, strength: %d\n",
+		id, touch->type, touch->pressed,
+		x, y, touch->width, touch->strength);
+
+	input_mt_slot(input_dev, id);
+	input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, touch->pressed);
+
+	if (touch->pressed) {
+		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR, touch->width);
+		input_report_abs(input_dev, ABS_MT_POSITION_X, x);
+		input_report_abs(input_dev, ABS_MT_POSITION_Y, y);
+		input_report_abs(input_dev, ABS_MT_PRESSURE, touch->strength);
+	}
+}
+
+static irqreturn_t mms114_interrupt(int irq, void *dev_id)
+{
+	struct mms114_data *data = dev_id;
+	struct input_dev *input_dev = data->input_dev;
+	struct mms114_touch touch[MMS114_MAX_TOUCH];
+	int packet_size;
+	int touch_size;
+	int index;
+	int error;
+
+	mutex_lock(&input_dev->mutex);
+	if (!input_dev->users) {
+		mutex_unlock(&input_dev->mutex);
+		goto out;
+	}
+	mutex_unlock(&input_dev->mutex);
+
+	packet_size = mms114_read_reg(data, MMS114_PACKET_SIZE);
+	if (packet_size <= 0)
+		goto out;
+
+	touch_size = packet_size / MMS114_PACKET_NUM;
+
+	error = __mms114_read_reg(data, MMS114_INFOMATION, packet_size,
+			(u8 *)touch);
+	if (error < 0)
+		goto out;
+
+	for (index = 0; index < touch_size; index++)
+		mms114_process_mt(data, touch + index);
+
+	input_mt_report_pointer_emulation(data->input_dev, true);
+	input_sync(data->input_dev);
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int mms114_set_active(struct mms114_data *data, bool active)
+{
+	int val;
+
+	val = mms114_read_reg(data, MMS114_MODE_CONTROL);
+	if (val < 0)
+		return val;
+
+	val &= ~MMS114_OPERATION_MODE_MASK;
+
+	/* If active is false, sleep mode */
+	if (active)
+		val |= MMS114_ACTIVE;
+
+	return mms114_write_reg(data, MMS114_MODE_CONTROL, val);
+}
+
+static int mms114_get_version(struct mms114_data *data)
+{
+	struct device *dev = &data->client->dev;
+	u8 buf[6];
+	int error;
+
+	error = __mms114_read_reg(data, MMS114_TSP_REV, 6, buf);
+	if (error < 0)
+		return error;
+
+	dev_info(dev, "TSP Rev: 0x%x, HW Rev: 0x%x, Firmware Ver: 0x%x\n",
+		 buf[0], buf[1], buf[3]);
+
+	return 0;
+}
+
+static int mms114_setup_regs(struct mms114_data *data)
+{
+	const struct mms114_platform_data *pdata = data->pdata;
+	int val;
+	int error;
+
+	error = mms114_get_version(data);
+	if (error < 0)
+		return error;
+
+	error = mms114_set_active(data, true);
+	if (error < 0)
+		return error;
+
+	val = (pdata->x_size >> 8) & 0xf;
+	val |= ((pdata->y_size >> 8) & 0xf) << 4;
+	error = mms114_write_reg(data, MMS114_XY_RESOLUTION_H, val);
+	if (error < 0)
+		return error;
+
+	val = pdata->x_size & 0xff;
+	error = mms114_write_reg(data, MMS114_X_RESOLUTION, val);
+	if (error < 0)
+		return error;
+
+	val = pdata->y_size & 0xff;
+	error = mms114_write_reg(data, MMS114_Y_RESOLUTION, val);
+	if (error < 0)
+		return error;
+
+	if (pdata->contact_threshold) {
+		error = mms114_write_reg(data, MMS114_CONTACT_THRESHOLD,
+				pdata->contact_threshold);
+		if (error < 0)
+			return error;
+	}
+
+	if (pdata->moving_threshold) {
+		error = mms114_write_reg(data, MMS114_MOVING_THRESHOLD,
+				pdata->moving_threshold);
+		if (error < 0)
+			return error;
+	}
+
+	return 0;
+}
+
+static int mms114_start(struct mms114_data *data)
+{
+	struct i2c_client *client = data->client;
+	int error;
+
+	if (data->core_reg)
+		regulator_enable(data->core_reg);
+	if (data->io_reg)
+		regulator_enable(data->io_reg);
+	mdelay(MMS114_POWERON_DELAY);
+
+	error = mms114_setup_regs(data);
+	if (error < 0)
+		return error;
+
+	if (data->pdata->cfg_pin)
+		data->pdata->cfg_pin(true);
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static void mms114_stop(struct mms114_data *data)
+{
+	struct i2c_client *client = data->client;
+
+	disable_irq(client->irq);
+
+	if (data->pdata->cfg_pin)
+		data->pdata->cfg_pin(false);
+
+	if (data->io_reg)
+		regulator_disable(data->io_reg);
+	if (data->core_reg)
+		regulator_disable(data->core_reg);
+}
+
+static int mms114_input_open(struct input_dev *dev)
+{
+	struct mms114_data *data = input_get_drvdata(dev);
+
+	return mms114_start(data);
+}
+
+static void mms114_input_close(struct input_dev *dev)
+{
+	struct mms114_data *data = input_get_drvdata(dev);
+
+	mms114_stop(data);
+}
+
+static int __devinit mms114_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct mms114_data *data;
+	struct input_dev *input_dev;
+	int error;
+
+	if (!client->dev.platform_data) {
+		dev_err(&client->dev, "Need platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(client->adapter,
+				I2C_FUNC_PROTOCOL_MANGLING)) {
+		dev_err(&client->dev,
+			"Need i2c bus that supports protocol mangling\n");
+		return -ENODEV;
+	}
+
+	data = kzalloc(sizeof(struct mms114_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;
+	}
+
+	data->client = client;
+	data->input_dev = input_dev;
+	data->pdata = client->dev.platform_data;
+
+	input_dev->name = "MELPAS MMS114 Touchscreen";
+	input_dev->id.bustype = BUS_I2C;
+	input_dev->dev.parent = &client->dev;
+	input_dev->open = mms114_input_open;
+	input_dev->close = mms114_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, data->pdata->x_size, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, data->pdata->y_size, 0, 0);
+
+	/* For multi touch */
+	input_mt_init_slots(input_dev, MMS114_MAX_TOUCH);
+	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
+			     0, MMS114_MAX_AREA, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
+			     0, data->pdata->x_size, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
+			     0, data->pdata->y_size, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_PRESSURE, 0, 255, 0, 0);
+
+	input_set_drvdata(input_dev, data);
+	i2c_set_clientdata(client, data);
+
+	data->core_reg = regulator_get(&client->dev, "avdd");
+	if (IS_ERR(data->core_reg)) {
+		error = PTR_ERR(data->core_reg);
+		dev_err(&client->dev,
+			"Unable to get the Core regulator (%d)\n", error);
+		goto err_free_mem;
+	}
+
+	data->io_reg = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(data->io_reg)) {
+		error = PTR_ERR(data->io_reg);
+		dev_err(&client->dev,
+			"Unable to get the IO regulator (%d)\n", error);
+		goto err_core_reg;
+	}
+
+	error = request_threaded_irq(client->irq, NULL, mms114_interrupt,
+			IRQF_TRIGGER_FALLING | IRQF_ONESHOT, "mms114", data);
+	if (error) {
+		dev_err(&client->dev, "Failed to register interrupt\n");
+		goto err_io_reg;
+	}
+	disable_irq(client->irq);
+
+	error = input_register_device(data->input_dev);
+	if (error)
+		goto err_free_irq;
+
+	return 0;
+
+err_free_irq:
+	free_irq(client->irq, data);
+err_io_reg:
+	regulator_put(data->io_reg);
+err_core_reg:
+	regulator_put(data->core_reg);
+err_free_mem:
+	input_free_device(input_dev);
+	kfree(data);
+	return error;
+}
+
+static int __devexit mms114_remove(struct i2c_client *client)
+{
+	struct mms114_data *data = i2c_get_clientdata(client);
+
+	free_irq(client->irq, data);
+	regulator_put(data->io_reg);
+	regulator_put(data->core_reg);
+	input_unregister_device(data->input_dev);
+	kfree(data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mms114_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mms114_data *data = i2c_get_clientdata(client);
+	struct input_dev *input_dev = data->input_dev;
+	int id;
+
+	/* Release all touch */
+	for (id = 0; id < MMS114_MAX_TOUCH; id++) {
+		input_mt_slot(input_dev, id);
+		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, false);
+	}
+
+	input_mt_report_pointer_emulation(input_dev, true);
+	input_sync(input_dev);
+
+	mutex_lock(&input_dev->mutex);
+	if (input_dev->users)
+		mms114_stop(data);
+	mutex_unlock(&input_dev->mutex);
+
+	return 0;
+}
+
+static int mms114_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct mms114_data *data = i2c_get_clientdata(client);
+	struct input_dev *input_dev = data->input_dev;
+	int error;
+
+	mutex_lock(&input_dev->mutex);
+	if (input_dev->users) {
+		error = mms114_start(data);
+		if (error < 0) {
+			mutex_unlock(&input_dev->mutex);
+			return error;
+		}
+	}
+	mutex_unlock(&input_dev->mutex);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(mms114_pm_ops, mms114_suspend, mms114_resume);
+
+static const struct i2c_device_id mms114_id[] = {
+	{ "mms114", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mms114_id);
+
+static struct i2c_driver mms114_driver = {
+	.driver = {
+		.name	= "mms114",
+		.owner	= THIS_MODULE,
+		.pm	= &mms114_pm_ops,
+	},
+	.probe		= mms114_probe,
+	.remove		= __devexit_p(mms114_remove),
+	.id_table	= mms114_id,
+};
+
+module_i2c_driver(mms114_driver);
+
+/* Module information */
+MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
+MODULE_DESCRIPTION("MELFAS mms114 Touchscreen driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/wacom_i2c.c b/drivers/input/touchscreen/wacom_i2c.c
index 35572575d34..0c01657132f 100644
--- a/drivers/input/touchscreen/wacom_i2c.c
+++ b/drivers/input/touchscreen/wacom_i2c.c
@@ -149,7 +149,7 @@ static int __devinit wacom_i2c_probe(struct i2c_client *client,
 {
 	struct wacom_i2c *wac_i2c;
 	struct input_dev *input;
-	struct wacom_features features;
+	struct wacom_features features = { 0 };
 	int error;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 34089372753..9f69b561f5d 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -13,6 +13,10 @@ menuconfig IOMMU_SUPPORT
 
 if IOMMU_SUPPORT
 
+config OF_IOMMU
+       def_bool y
+       depends on OF
+
 # MSM IOMMU support
 config MSM_IOMMU
 	bool "MSM IOMMU Support"
@@ -154,7 +158,7 @@ config TEGRA_IOMMU_GART
 
 config TEGRA_IOMMU_SMMU
 	bool "Tegra SMMU IOMMU Support"
-	depends on ARCH_TEGRA_3x_SOC
+	depends on ARCH_TEGRA_3x_SOC && TEGRA_AHB
 	select IOMMU_API
 	help
 	  Enables support for remapping discontiguous physical memory
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 76e54ef796d..14a4d5fc94f 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_IOMMU_API) += iommu.o
+obj-$(CONFIG_OF_IOMMU)	+= of_iommu.o
 obj-$(CONFIG_MSM_IOMMU) += msm_iommu.o msm_iommu_dev.o
 obj-$(CONFIG_AMD_IOMMU) += amd_iommu.o amd_iommu_init.o
 obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 625626391f2..6d1cbdfc9b2 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -256,11 +256,21 @@ static bool check_device(struct device *dev)
 	return true;
 }
 
+static void swap_pci_ref(struct pci_dev **from, struct pci_dev *to)
+{
+	pci_dev_put(*from);
+	*from = to;
+}
+
+#define REQ_ACS_FLAGS	(PCI_ACS_SV | PCI_ACS_RR | PCI_ACS_CR | PCI_ACS_UF)
+
 static int iommu_init_device(struct device *dev)
 {
-	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_dev *dma_pdev, *pdev = to_pci_dev(dev);
 	struct iommu_dev_data *dev_data;
+	struct iommu_group *group;
 	u16 alias;
+	int ret;
 
 	if (dev->archdata.iommu)
 		return 0;
@@ -281,8 +291,43 @@ static int iommu_init_device(struct device *dev)
 			return -ENOTSUPP;
 		}
 		dev_data->alias_data = alias_data;
+
+		dma_pdev = pci_get_bus_and_slot(alias >> 8, alias & 0xff);
+	} else
+		dma_pdev = pci_dev_get(pdev);
+
+	swap_pci_ref(&dma_pdev, pci_get_dma_source(dma_pdev));
+
+	if (dma_pdev->multifunction &&
+	    !pci_acs_enabled(dma_pdev, REQ_ACS_FLAGS))
+		swap_pci_ref(&dma_pdev,
+			     pci_get_slot(dma_pdev->bus,
+					  PCI_DEVFN(PCI_SLOT(dma_pdev->devfn),
+					  0)));
+
+	while (!pci_is_root_bus(dma_pdev->bus)) {
+		if (pci_acs_path_enabled(dma_pdev->bus->self,
+					 NULL, REQ_ACS_FLAGS))
+			break;
+
+		swap_pci_ref(&dma_pdev, pci_dev_get(dma_pdev->bus->self));
+	}
+
+	group = iommu_group_get(&dma_pdev->dev);
+	pci_dev_put(dma_pdev);
+	if (!group) {
+		group = iommu_group_alloc();
+		if (IS_ERR(group))
+			return PTR_ERR(group);
 	}
 
+	ret = iommu_group_add_device(group, dev);
+
+	iommu_group_put(group);
+
+	if (ret)
+		return ret;
+
 	if (pci_iommuv2_capable(pdev)) {
 		struct amd_iommu *iommu;
 
@@ -311,6 +356,8 @@ static void iommu_ignore_device(struct device *dev)
 
 static void iommu_uninit_device(struct device *dev)
 {
+	iommu_group_remove_device(dev);
+
 	/*
 	 * Nothing to do here - we keep dev_data around for unplugged devices
 	 * and reuse it when the device is re-plugged - not doing so would
@@ -384,7 +431,6 @@ DECLARE_STATS_COUNTER(invalidate_iotlb);
 DECLARE_STATS_COUNTER(invalidate_iotlb_all);
 DECLARE_STATS_COUNTER(pri_requests);
 
-
 static struct dentry *stats_dir;
 static struct dentry *de_fflush;
 
@@ -2073,7 +2119,7 @@ out_err:
 /* FIXME: Move this to PCI code */
 #define PCI_PRI_TLP_OFF		(1 << 15)
 
-bool pci_pri_tlp_required(struct pci_dev *pdev)
+static bool pci_pri_tlp_required(struct pci_dev *pdev)
 {
 	u16 status;
 	int pos;
@@ -2254,6 +2300,18 @@ static int device_change_notifier(struct notifier_block *nb,
 
 		iommu_init_device(dev);
 
+		/*
+		 * dev_data is still NULL and
+		 * got initialized in iommu_init_device
+		 */
+		dev_data = get_dev_data(dev);
+
+		if (iommu_pass_through || dev_data->iommu_v2) {
+			dev_data->passthrough = true;
+			attach_device(dev, pt_domain);
+			break;
+		}
+
 		domain = domain_for_device(dev);
 
 		/* allocate a protection domain if a device is added */
@@ -2271,10 +2329,7 @@ static int device_change_notifier(struct notifier_block *nb,
 
 		dev_data = get_dev_data(dev);
 
-		if (!dev_data->passthrough)
-			dev->archdata.dma_ops = &amd_iommu_dma_ops;
-		else
-			dev->archdata.dma_ops = &nommu_dma_ops;
+		dev->archdata.dma_ops = &amd_iommu_dma_ops;
 
 		break;
 	case BUS_NOTIFY_DEL_DEVICE:
@@ -2972,6 +3027,11 @@ int __init amd_iommu_init_dma_ops(void)
 
 	amd_iommu_stats_init();
 
+	if (amd_iommu_unmap_flush)
+		pr_info("AMD-Vi: IO/TLB flush on unmap enabled\n");
+	else
+		pr_info("AMD-Vi: Lazy IO/TLB flushing enabled\n");
+
 	return 0;
 
 free_domains:
@@ -3078,6 +3138,10 @@ static int amd_iommu_domain_init(struct iommu_domain *dom)
 
 	dom->priv = domain;
 
+	dom->geometry.aperture_start = 0;
+	dom->geometry.aperture_end   = ~0ULL;
+	dom->geometry.force_aperture = true;
+
 	return 0;
 
 out_free:
@@ -3236,26 +3300,6 @@ static int amd_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
-static int amd_iommu_device_group(struct device *dev, unsigned int *groupid)
-{
-	struct iommu_dev_data *dev_data = dev->archdata.iommu;
-	struct pci_dev *pdev = to_pci_dev(dev);
-	u16 devid;
-
-	if (!dev_data)
-		return -ENODEV;
-
-	if (pdev->is_virtfn || !iommu_group_mf)
-		devid = dev_data->devid;
-	else
-		devid = calc_devid(pdev->bus->number,
-				   PCI_DEVFN(PCI_SLOT(pdev->devfn), 0));
-
-	*groupid = amd_iommu_alias_table[devid];
-
-	return 0;
-}
-
 static struct iommu_ops amd_iommu_ops = {
 	.domain_init = amd_iommu_domain_init,
 	.domain_destroy = amd_iommu_domain_destroy,
@@ -3265,7 +3309,6 @@ static struct iommu_ops amd_iommu_ops = {
 	.unmap = amd_iommu_unmap,
 	.iova_to_phys = amd_iommu_iova_to_phys,
 	.domain_has_cap = amd_iommu_domain_has_cap,
-	.device_group = amd_iommu_device_group,
 	.pgsize_bitmap	= AMD_IOMMU_PGSIZES,
 };
 
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index a33612f3206..500e7f15f5c 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -26,6 +26,8 @@
 #include <linux/msi.h>
 #include <linux/amd-iommu.h>
 #include <linux/export.h>
+#include <linux/acpi.h>
+#include <acpi/acpi.h>
 #include <asm/pci-direct.h>
 #include <asm/iommu.h>
 #include <asm/gart.h>
@@ -122,7 +124,7 @@ struct ivmd_header {
 
 bool amd_iommu_dump;
 
-static int __initdata amd_iommu_detected;
+static bool amd_iommu_detected;
 static bool __initdata amd_iommu_disabled;
 
 u16 amd_iommu_last_bdf;			/* largest PCI device id we have
@@ -149,11 +151,6 @@ bool amd_iommu_v2_present __read_mostly;
 bool amd_iommu_force_isolation __read_mostly;
 
 /*
- * The ACPI table parsing functions set this variable on an error
- */
-static int __initdata amd_iommu_init_err;
-
-/*
  * List of protection domains - used during resume
  */
 LIST_HEAD(amd_iommu_pd_list);
@@ -190,13 +187,23 @@ static u32 dev_table_size;	/* size of the device table */
 static u32 alias_table_size;	/* size of the alias table */
 static u32 rlookup_table_size;	/* size if the rlookup table */
 
-/*
- * This function flushes all internal caches of
- * the IOMMU used by this driver.
- */
-extern void iommu_flush_all_caches(struct amd_iommu *iommu);
+enum iommu_init_state {
+	IOMMU_START_STATE,
+	IOMMU_IVRS_DETECTED,
+	IOMMU_ACPI_FINISHED,
+	IOMMU_ENABLED,
+	IOMMU_PCI_INIT,
+	IOMMU_INTERRUPTS_EN,
+	IOMMU_DMA_OPS,
+	IOMMU_INITIALIZED,
+	IOMMU_NOT_FOUND,
+	IOMMU_INIT_ERROR,
+};
+
+static enum iommu_init_state init_state = IOMMU_START_STATE;
 
 static int amd_iommu_enable_interrupts(void);
+static int __init iommu_go_to_state(enum iommu_init_state state);
 
 static inline void update_last_devid(u16 devid)
 {
@@ -321,23 +328,6 @@ static void iommu_set_inv_tlb_timeout(struct amd_iommu *iommu, int timeout)
 /* Function to enable the hardware */
 static void iommu_enable(struct amd_iommu *iommu)
 {
-	static const char * const feat_str[] = {
-		"PreF", "PPR", "X2APIC", "NX", "GT", "[5]",
-		"IA", "GA", "HE", "PC", NULL
-	};
-	int i;
-
-	printk(KERN_INFO "AMD-Vi: Enabling IOMMU at %s cap 0x%hx",
-	       dev_name(&iommu->dev->dev), iommu->cap_ptr);
-
-	if (iommu->cap & (1 << IOMMU_CAP_EFR)) {
-		printk(KERN_CONT " extended features: ");
-		for (i = 0; feat_str[i]; ++i)
-			if (iommu_feature(iommu, (1ULL << i)))
-				printk(KERN_CONT " %s", feat_str[i]);
-	}
-	printk(KERN_CONT "\n");
-
 	iommu_feature_enable(iommu, CONTROL_IOMMU_EN);
 }
 
@@ -358,7 +348,7 @@ static void iommu_disable(struct amd_iommu *iommu)
  * mapping and unmapping functions for the IOMMU MMIO space. Each AMD IOMMU in
  * the system has one.
  */
-static u8 * __init iommu_map_mmio_space(u64 address)
+static u8 __iomem * __init iommu_map_mmio_space(u64 address)
 {
 	if (!request_mem_region(address, MMIO_REGION_LENGTH, "amd_iommu")) {
 		pr_err("AMD-Vi: Can not reserve memory region %llx for mmio\n",
@@ -367,7 +357,7 @@ static u8 * __init iommu_map_mmio_space(u64 address)
 		return NULL;
 	}
 
-	return ioremap_nocache(address, MMIO_REGION_LENGTH);
+	return (u8 __iomem *)ioremap_nocache(address, MMIO_REGION_LENGTH);
 }
 
 static void __init iommu_unmap_mmio_space(struct amd_iommu *iommu)
@@ -463,11 +453,9 @@ static int __init find_last_devid_acpi(struct acpi_table_header *table)
 	 */
 	for (i = 0; i < table->length; ++i)
 		checksum += p[i];
-	if (checksum != 0) {
+	if (checksum != 0)
 		/* ACPI table corrupt */
-		amd_iommu_init_err = -ENODEV;
-		return 0;
-	}
+		return -ENODEV;
 
 	p += IVRS_HEADER_LENGTH;
 
@@ -726,90 +714,6 @@ static void __init set_device_exclusion_range(u16 devid, struct ivmd_header *m)
 }
 
 /*
- * This function reads some important data from the IOMMU PCI space and
- * initializes the driver data structure with it. It reads the hardware
- * capabilities and the first/last device entries
- */
-static void __init init_iommu_from_pci(struct amd_iommu *iommu)
-{
-	int cap_ptr = iommu->cap_ptr;
-	u32 range, misc, low, high;
-	int i, j;
-
-	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_CAP_HDR_OFFSET,
-			      &iommu->cap);
-	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_RANGE_OFFSET,
-			      &range);
-	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_MISC_OFFSET,
-			      &misc);
-
-	iommu->first_device = calc_devid(MMIO_GET_BUS(range),
-					 MMIO_GET_FD(range));
-	iommu->last_device = calc_devid(MMIO_GET_BUS(range),
-					MMIO_GET_LD(range));
-	iommu->evt_msi_num = MMIO_MSI_NUM(misc);
-
-	if (!(iommu->cap & (1 << IOMMU_CAP_IOTLB)))
-		amd_iommu_iotlb_sup = false;
-
-	/* read extended feature bits */
-	low  = readl(iommu->mmio_base + MMIO_EXT_FEATURES);
-	high = readl(iommu->mmio_base + MMIO_EXT_FEATURES + 4);
-
-	iommu->features = ((u64)high << 32) | low;
-
-	if (iommu_feature(iommu, FEATURE_GT)) {
-		int glxval;
-		u32 pasids;
-		u64 shift;
-
-		shift   = iommu->features & FEATURE_PASID_MASK;
-		shift >>= FEATURE_PASID_SHIFT;
-		pasids  = (1 << shift);
-
-		amd_iommu_max_pasids = min(amd_iommu_max_pasids, pasids);
-
-		glxval   = iommu->features & FEATURE_GLXVAL_MASK;
-		glxval >>= FEATURE_GLXVAL_SHIFT;
-
-		if (amd_iommu_max_glx_val == -1)
-			amd_iommu_max_glx_val = glxval;
-		else
-			amd_iommu_max_glx_val = min(amd_iommu_max_glx_val, glxval);
-	}
-
-	if (iommu_feature(iommu, FEATURE_GT) &&
-	    iommu_feature(iommu, FEATURE_PPR)) {
-		iommu->is_iommu_v2   = true;
-		amd_iommu_v2_present = true;
-	}
-
-	if (!is_rd890_iommu(iommu->dev))
-		return;
-
-	/*
-	 * Some rd890 systems may not be fully reconfigured by the BIOS, so
-	 * it's necessary for us to store this information so it can be
-	 * reprogrammed on resume
-	 */
-
-	pci_read_config_dword(iommu->dev, iommu->cap_ptr + 4,
-			      &iommu->stored_addr_lo);
-	pci_read_config_dword(iommu->dev, iommu->cap_ptr + 8,
-			      &iommu->stored_addr_hi);
-
-	/* Low bit locks writes to configuration space */
-	iommu->stored_addr_lo &= ~1;
-
-	for (i = 0; i < 6; i++)
-		for (j = 0; j < 0x12; j++)
-			iommu->stored_l1[i][j] = iommu_read_l1(iommu, i, j);
-
-	for (i = 0; i < 0x83; i++)
-		iommu->stored_l2[i] = iommu_read_l2(iommu, i);
-}
-
-/*
  * Takes a pointer to an AMD IOMMU entry in the ACPI table and
  * initializes the hardware and our data structures with it.
  */
@@ -1025,13 +929,7 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 	/*
 	 * Copy data from ACPI table entry to the iommu struct
 	 */
-	iommu->dev = pci_get_bus_and_slot(PCI_BUS(h->devid), h->devid & 0xff);
-	if (!iommu->dev)
-		return 1;
-
-	iommu->root_pdev = pci_get_bus_and_slot(iommu->dev->bus->number,
-						PCI_DEVFN(0, 0));
-
+	iommu->devid   = h->devid;
 	iommu->cap_ptr = h->cap_ptr;
 	iommu->pci_seg = h->pci_seg;
 	iommu->mmio_phys = h->mmio_phys;
@@ -1049,20 +947,10 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 
 	iommu->int_enabled = false;
 
-	init_iommu_from_pci(iommu);
 	init_iommu_from_acpi(iommu, h);
 	init_iommu_devices(iommu);
 
-	if (iommu_feature(iommu, FEATURE_PPR)) {
-		iommu->ppr_log = alloc_ppr_log(iommu);
-		if (!iommu->ppr_log)
-			return -ENOMEM;
-	}
-
-	if (iommu->cap & (1UL << IOMMU_CAP_NPCACHE))
-		amd_iommu_np_cache = true;
-
-	return pci_enable_device(iommu->dev);
+	return 0;
 }
 
 /*
@@ -1093,16 +981,12 @@ static int __init init_iommu_all(struct acpi_table_header *table)
 				    h->mmio_phys);
 
 			iommu = kzalloc(sizeof(struct amd_iommu), GFP_KERNEL);
-			if (iommu == NULL) {
-				amd_iommu_init_err = -ENOMEM;
-				return 0;
-			}
+			if (iommu == NULL)
+				return -ENOMEM;
 
 			ret = init_iommu_one(iommu, h);
-			if (ret) {
-				amd_iommu_init_err = ret;
-				return 0;
-			}
+			if (ret)
+				return ret;
 			break;
 		default:
 			break;
@@ -1115,6 +999,148 @@ static int __init init_iommu_all(struct acpi_table_header *table)
 	return 0;
 }
 
+static int iommu_init_pci(struct amd_iommu *iommu)
+{
+	int cap_ptr = iommu->cap_ptr;
+	u32 range, misc, low, high;
+
+	iommu->dev = pci_get_bus_and_slot(PCI_BUS(iommu->devid),
+					  iommu->devid & 0xff);
+	if (!iommu->dev)
+		return -ENODEV;
+
+	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_CAP_HDR_OFFSET,
+			      &iommu->cap);
+	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_RANGE_OFFSET,
+			      &range);
+	pci_read_config_dword(iommu->dev, cap_ptr + MMIO_MISC_OFFSET,
+			      &misc);
+
+	iommu->first_device = calc_devid(MMIO_GET_BUS(range),
+					 MMIO_GET_FD(range));
+	iommu->last_device = calc_devid(MMIO_GET_BUS(range),
+					MMIO_GET_LD(range));
+
+	if (!(iommu->cap & (1 << IOMMU_CAP_IOTLB)))
+		amd_iommu_iotlb_sup = false;
+
+	/* read extended feature bits */
+	low  = readl(iommu->mmio_base + MMIO_EXT_FEATURES);
+	high = readl(iommu->mmio_base + MMIO_EXT_FEATURES + 4);
+
+	iommu->features = ((u64)high << 32) | low;
+
+	if (iommu_feature(iommu, FEATURE_GT)) {
+		int glxval;
+		u32 pasids;
+		u64 shift;
+
+		shift   = iommu->features & FEATURE_PASID_MASK;
+		shift >>= FEATURE_PASID_SHIFT;
+		pasids  = (1 << shift);
+
+		amd_iommu_max_pasids = min(amd_iommu_max_pasids, pasids);
+
+		glxval   = iommu->features & FEATURE_GLXVAL_MASK;
+		glxval >>= FEATURE_GLXVAL_SHIFT;
+
+		if (amd_iommu_max_glx_val == -1)
+			amd_iommu_max_glx_val = glxval;
+		else
+			amd_iommu_max_glx_val = min(amd_iommu_max_glx_val, glxval);
+	}
+
+	if (iommu_feature(iommu, FEATURE_GT) &&
+	    iommu_feature(iommu, FEATURE_PPR)) {
+		iommu->is_iommu_v2   = true;
+		amd_iommu_v2_present = true;
+	}
+
+	if (iommu_feature(iommu, FEATURE_PPR)) {
+		iommu->ppr_log = alloc_ppr_log(iommu);
+		if (!iommu->ppr_log)
+			return -ENOMEM;
+	}
+
+	if (iommu->cap & (1UL << IOMMU_CAP_NPCACHE))
+		amd_iommu_np_cache = true;
+
+	if (is_rd890_iommu(iommu->dev)) {
+		int i, j;
+
+		iommu->root_pdev = pci_get_bus_and_slot(iommu->dev->bus->number,
+				PCI_DEVFN(0, 0));
+
+		/*
+		 * Some rd890 systems may not be fully reconfigured by the
+		 * BIOS, so it's necessary for us to store this information so
+		 * it can be reprogrammed on resume
+		 */
+		pci_read_config_dword(iommu->dev, iommu->cap_ptr + 4,
+				&iommu->stored_addr_lo);
+		pci_read_config_dword(iommu->dev, iommu->cap_ptr + 8,
+				&iommu->stored_addr_hi);
+
+		/* Low bit locks writes to configuration space */
+		iommu->stored_addr_lo &= ~1;
+
+		for (i = 0; i < 6; i++)
+			for (j = 0; j < 0x12; j++)
+				iommu->stored_l1[i][j] = iommu_read_l1(iommu, i, j);
+
+		for (i = 0; i < 0x83; i++)
+			iommu->stored_l2[i] = iommu_read_l2(iommu, i);
+	}
+
+	return pci_enable_device(iommu->dev);
+}
+
+static void print_iommu_info(void)
+{
+	static const char * const feat_str[] = {
+		"PreF", "PPR", "X2APIC", "NX", "GT", "[5]",
+		"IA", "GA", "HE", "PC"
+	};
+	struct amd_iommu *iommu;
+
+	for_each_iommu(iommu) {
+		int i;
+
+		pr_info("AMD-Vi: Found IOMMU at %s cap 0x%hx\n",
+			dev_name(&iommu->dev->dev), iommu->cap_ptr);
+
+		if (iommu->cap & (1 << IOMMU_CAP_EFR)) {
+			pr_info("AMD-Vi:  Extended features: ");
+			for (i = 0; ARRAY_SIZE(feat_str); ++i) {
+				if (iommu_feature(iommu, (1ULL << i)))
+					pr_cont(" %s", feat_str[i]);
+			}
+		}
+		pr_cont("\n");
+	}
+}
+
+static int __init amd_iommu_init_pci(void)
+{
+	struct amd_iommu *iommu;
+	int ret = 0;
+
+	for_each_iommu(iommu) {
+		ret = iommu_init_pci(iommu);
+		if (ret)
+			break;
+	}
+
+	/* Make sure ACS will be enabled */
+	pci_request_acs();
+
+	ret = amd_iommu_init_devices();
+
+	print_iommu_info();
+
+	return ret;
+}
+
 /****************************************************************************
  *
  * The following functions initialize the MSI interrupts for all IOMMUs
@@ -1217,7 +1243,7 @@ static int __init init_exclusion_range(struct ivmd_header *m)
 /* called for unity map ACPI definition */
 static int __init init_unity_map_range(struct ivmd_header *m)
 {
-	struct unity_map_entry *e = 0;
+	struct unity_map_entry *e = NULL;
 	char *s;
 
 	e = kzalloc(sizeof(*e), GFP_KERNEL);
@@ -1369,7 +1395,7 @@ static void iommu_apply_resume_quirks(struct amd_iommu *iommu)
  * This function finally enables all IOMMUs found in the system after
  * they have been initialized
  */
-static void enable_iommus(void)
+static void early_enable_iommus(void)
 {
 	struct amd_iommu *iommu;
 
@@ -1379,14 +1405,29 @@ static void enable_iommus(void)
 		iommu_set_device_table(iommu);
 		iommu_enable_command_buffer(iommu);
 		iommu_enable_event_buffer(iommu);
-		iommu_enable_ppr_log(iommu);
-		iommu_enable_gt(iommu);
 		iommu_set_exclusion_range(iommu);
 		iommu_enable(iommu);
 		iommu_flush_all_caches(iommu);
 	}
 }
 
+static void enable_iommus_v2(void)
+{
+	struct amd_iommu *iommu;
+
+	for_each_iommu(iommu) {
+		iommu_enable_ppr_log(iommu);
+		iommu_enable_gt(iommu);
+	}
+}
+
+static void enable_iommus(void)
+{
+	early_enable_iommus();
+
+	enable_iommus_v2();
+}
+
 static void disable_iommus(void)
 {
 	struct amd_iommu *iommu;
@@ -1481,16 +1522,23 @@ static void __init free_on_init_error(void)
  * After everything is set up the IOMMUs are enabled and the necessary
  * hotplug and suspend notifiers are registered.
  */
-int __init amd_iommu_init_hardware(void)
+static int __init early_amd_iommu_init(void)
 {
+	struct acpi_table_header *ivrs_base;
+	acpi_size ivrs_size;
+	acpi_status status;
 	int i, ret = 0;
 
 	if (!amd_iommu_detected)
 		return -ENODEV;
 
-	if (amd_iommu_dev_table != NULL) {
-		/* Hardware already initialized */
-		return 0;
+	status = acpi_get_table_with_size("IVRS", 0, &ivrs_base, &ivrs_size);
+	if (status == AE_NOT_FOUND)
+		return -ENODEV;
+	else if (ACPI_FAILURE(status)) {
+		const char *err = acpi_format_exception(status);
+		pr_err("AMD-Vi: IVRS table error: %s\n", err);
+		return -EINVAL;
 	}
 
 	/*
@@ -1498,10 +1546,7 @@ int __init amd_iommu_init_hardware(void)
 	 * we need to handle. Upon this information the shared data
 	 * structures for the IOMMUs in the system will be allocated
 	 */
-	if (acpi_table_parse("IVRS", find_last_devid_acpi) != 0)
-		return -ENODEV;
-
-	ret = amd_iommu_init_err;
+	ret = find_last_devid_acpi(ivrs_base);
 	if (ret)
 		goto out;
 
@@ -1523,20 +1568,20 @@ int __init amd_iommu_init_hardware(void)
 	amd_iommu_alias_table = (void *)__get_free_pages(GFP_KERNEL,
 			get_order(alias_table_size));
 	if (amd_iommu_alias_table == NULL)
-		goto free;
+		goto out;
 
 	/* IOMMU rlookup table - find the IOMMU for a specific device */
 	amd_iommu_rlookup_table = (void *)__get_free_pages(
 			GFP_KERNEL | __GFP_ZERO,
 			get_order(rlookup_table_size));
 	if (amd_iommu_rlookup_table == NULL)
-		goto free;
+		goto out;
 
 	amd_iommu_pd_alloc_bitmap = (void *)__get_free_pages(
 					    GFP_KERNEL | __GFP_ZERO,
 					    get_order(MAX_DOMAIN_ID/8));
 	if (amd_iommu_pd_alloc_bitmap == NULL)
-		goto free;
+		goto out;
 
 	/* init the device table */
 	init_device_table();
@@ -1559,38 +1604,18 @@ int __init amd_iommu_init_hardware(void)
 	 * now the data structures are allocated and basically initialized
 	 * start the real acpi table scan
 	 */
-	ret = -ENODEV;
-	if (acpi_table_parse("IVRS", init_iommu_all) != 0)
-		goto free;
-
-	if (amd_iommu_init_err) {
-		ret = amd_iommu_init_err;
-		goto free;
-	}
-
-	if (acpi_table_parse("IVRS", init_memory_definitions) != 0)
-		goto free;
-
-	if (amd_iommu_init_err) {
-		ret = amd_iommu_init_err;
-		goto free;
-	}
-
-	ret = amd_iommu_init_devices();
+	ret = init_iommu_all(ivrs_base);
 	if (ret)
-		goto free;
-
-	enable_iommus();
-
-	amd_iommu_init_notifier();
+		goto out;
 
-	register_syscore_ops(&amd_iommu_syscore_ops);
+	ret = init_memory_definitions(ivrs_base);
+	if (ret)
+		goto out;
 
 out:
-	return ret;
-
-free:
-	free_on_init_error();
+	/* Don't leak any ACPI memory */
+	early_acpi_os_unmap_memory((char __iomem *)ivrs_base, ivrs_size);
+	ivrs_base = NULL;
 
 	return ret;
 }
@@ -1610,26 +1635,29 @@ out:
 	return ret;
 }
 
-/*
- * This is the core init function for AMD IOMMU hardware in the system.
- * This function is called from the generic x86 DMA layer initialization
- * code.
- *
- * The function calls amd_iommu_init_hardware() to setup and enable the
- * IOMMU hardware if this has not happened yet. After that the driver
- * registers for the DMA-API and for the IOMMU-API as necessary.
- */
-static int __init amd_iommu_init(void)
+static bool detect_ivrs(void)
 {
-	int ret = 0;
+	struct acpi_table_header *ivrs_base;
+	acpi_size ivrs_size;
+	acpi_status status;
 
-	ret = amd_iommu_init_hardware();
-	if (ret)
-		goto out;
+	status = acpi_get_table_with_size("IVRS", 0, &ivrs_base, &ivrs_size);
+	if (status == AE_NOT_FOUND)
+		return false;
+	else if (ACPI_FAILURE(status)) {
+		const char *err = acpi_format_exception(status);
+		pr_err("AMD-Vi: IVRS table error: %s\n", err);
+		return false;
+	}
 
-	ret = amd_iommu_enable_interrupts();
-	if (ret)
-		goto free;
+	early_acpi_os_unmap_memory((char __iomem *)ivrs_base, ivrs_size);
+
+	return true;
+}
+
+static int amd_iommu_init_dma(void)
+{
+	int ret;
 
 	if (iommu_pass_through)
 		ret = amd_iommu_init_passthrough();
@@ -1637,29 +1665,108 @@ static int __init amd_iommu_init(void)
 		ret = amd_iommu_init_dma_ops();
 
 	if (ret)
-		goto free;
+		return ret;
 
 	amd_iommu_init_api();
 
-	x86_platform.iommu_shutdown = disable_iommus;
+	amd_iommu_init_notifier();
 
-	if (iommu_pass_through)
-		goto out;
+	return 0;
+}
 
-	if (amd_iommu_unmap_flush)
-		printk(KERN_INFO "AMD-Vi: IO/TLB flush on unmap enabled\n");
-	else
-		printk(KERN_INFO "AMD-Vi: Lazy IO/TLB flushing enabled\n");
+/****************************************************************************
+ *
+ * AMD IOMMU Initialization State Machine
+ *
+ ****************************************************************************/
+
+static int __init state_next(void)
+{
+	int ret = 0;
+
+	switch (init_state) {
+	case IOMMU_START_STATE:
+		if (!detect_ivrs()) {
+			init_state	= IOMMU_NOT_FOUND;
+			ret		= -ENODEV;
+		} else {
+			init_state	= IOMMU_IVRS_DETECTED;
+		}
+		break;
+	case IOMMU_IVRS_DETECTED:
+		ret = early_amd_iommu_init();
+		init_state = ret ? IOMMU_INIT_ERROR : IOMMU_ACPI_FINISHED;
+		break;
+	case IOMMU_ACPI_FINISHED:
+		early_enable_iommus();
+		register_syscore_ops(&amd_iommu_syscore_ops);
+		x86_platform.iommu_shutdown = disable_iommus;
+		init_state = IOMMU_ENABLED;
+		break;
+	case IOMMU_ENABLED:
+		ret = amd_iommu_init_pci();
+		init_state = ret ? IOMMU_INIT_ERROR : IOMMU_PCI_INIT;
+		enable_iommus_v2();
+		break;
+	case IOMMU_PCI_INIT:
+		ret = amd_iommu_enable_interrupts();
+		init_state = ret ? IOMMU_INIT_ERROR : IOMMU_INTERRUPTS_EN;
+		break;
+	case IOMMU_INTERRUPTS_EN:
+		ret = amd_iommu_init_dma();
+		init_state = ret ? IOMMU_INIT_ERROR : IOMMU_DMA_OPS;
+		break;
+	case IOMMU_DMA_OPS:
+		init_state = IOMMU_INITIALIZED;
+		break;
+	case IOMMU_INITIALIZED:
+		/* Nothing to do */
+		break;
+	case IOMMU_NOT_FOUND:
+	case IOMMU_INIT_ERROR:
+		/* Error states => do nothing */
+		ret = -EINVAL;
+		break;
+	default:
+		/* Unknown state */
+		BUG();
+	}
 
-out:
 	return ret;
+}
 
-free:
-	disable_iommus();
+static int __init iommu_go_to_state(enum iommu_init_state state)
+{
+	int ret = 0;
+
+	while (init_state != state) {
+		ret = state_next();
+		if (init_state == IOMMU_NOT_FOUND ||
+		    init_state == IOMMU_INIT_ERROR)
+			break;
+	}
+
+	return ret;
+}
+
+
+
+/*
+ * This is the core init function for AMD IOMMU hardware in the system.
+ * This function is called from the generic x86 DMA layer initialization
+ * code.
+ */
+static int __init amd_iommu_init(void)
+{
+	int ret;
 
-	free_on_init_error();
+	ret = iommu_go_to_state(IOMMU_INITIALIZED);
+	if (ret) {
+		disable_iommus();
+		free_on_init_error();
+	}
 
-	goto out;
+	return ret;
 }
 
 /****************************************************************************
@@ -1669,29 +1776,25 @@ free:
  * IOMMUs
  *
  ****************************************************************************/
-static int __init early_amd_iommu_detect(struct acpi_table_header *table)
-{
-	return 0;
-}
-
 int __init amd_iommu_detect(void)
 {
+	int ret;
+
 	if (no_iommu || (iommu_detected && !gart_iommu_aperture))
 		return -ENODEV;
 
 	if (amd_iommu_disabled)
 		return -ENODEV;
 
-	if (acpi_table_parse("IVRS", early_amd_iommu_detect) == 0) {
-		iommu_detected = 1;
-		amd_iommu_detected = 1;
-		x86_init.iommu.iommu_init = amd_iommu_init;
+	ret = iommu_go_to_state(IOMMU_IVRS_DETECTED);
+	if (ret)
+		return ret;
 
-		/* Make sure ACS will be enabled */
-		pci_request_acs();
-		return 1;
-	}
-	return -ENODEV;
+	amd_iommu_detected = true;
+	iommu_detected = 1;
+	x86_init.iommu.iommu_init = amd_iommu_init;
+
+	return 0;
 }
 
 /****************************************************************************
@@ -1727,8 +1830,8 @@ __setup("amd_iommu=", parse_amd_iommu_options);
 
 IOMMU_INIT_FINISH(amd_iommu_detect,
 		  gart_iommu_hole_init,
-		  0,
-		  0);
+		  NULL,
+		  NULL);
 
 bool amd_iommu_v2_supported(void)
 {
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index c1b1d489817..d0dab865a8b 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -487,7 +487,7 @@ struct amd_iommu {
 	/* physical address of MMIO space */
 	u64 mmio_phys;
 	/* virtual address of MMIO space */
-	u8 *mmio_base;
+	u8 __iomem *mmio_base;
 
 	/* capabilities of that IOMMU read from ACPI */
 	u32 cap;
@@ -501,6 +501,9 @@ struct amd_iommu {
 	/* IOMMUv2 */
 	bool is_iommu_v2;
 
+	/* PCI device id of the IOMMU device */
+	u16 devid;
+
 	/*
 	 * Capability pointer. There could be more than one IOMMU per PCI
 	 * device function if there are more than one AMD IOMMU capability
@@ -530,8 +533,6 @@ struct amd_iommu {
 	u32 evt_buf_size;
 	/* event buffer virtual address */
 	u8 *evt_buf;
-	/* MSI number for event interrupt */
-	u16 evt_msi_num;
 
 	/* Base of the PPR log, if present */
 	u8 *ppr_log;
@@ -664,6 +665,12 @@ extern bool amd_iommu_force_isolation;
 /* Max levels of glxval supported */
 extern int amd_iommu_max_glx_val;
 
+/*
+ * This function flushes all internal caches of
+ * the IOMMU used by this driver.
+ */
+extern void iommu_flush_all_caches(struct amd_iommu *iommu);
+
 /* takes bus and device/function and returns the device id
  * FIXME: should that be in generic PCI code? */
 static inline u16 calc_devid(u8 bus, u8 devfn)
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
index 036fe9bf157..5208828792e 100644
--- a/drivers/iommu/amd_iommu_v2.c
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -81,7 +81,7 @@ struct fault {
 	u16 flags;
 };
 
-struct device_state **state_table;
+static struct device_state **state_table;
 static spinlock_t state_lock;
 
 /* List and lock for all pasid_states */
@@ -681,6 +681,8 @@ int amd_iommu_bind_pasid(struct pci_dev *pdev, int pasid,
 
 	atomic_set(&pasid_state->count, 1);
 	init_waitqueue_head(&pasid_state->wq);
+	spin_lock_init(&pasid_state->lock);
+
 	pasid_state->task         = task;
 	pasid_state->mm           = get_task_mm(task);
 	pasid_state->device_state = dev_state;
@@ -924,7 +926,7 @@ static int __init amd_iommu_v2_init(void)
 	pr_info("AMD IOMMUv2 driver by Joerg Roedel <joerg.roedel@amd.com>\n");
 
 	if (!amd_iommu_v2_supported()) {
-		pr_info("AMD IOMMUv2 functionality not available on this sytem\n");
+		pr_info("AMD IOMMUv2 functionality not available on this system\n");
 		/*
 		 * Load anyway to provide the symbols to other modules
 		 * which may use AMD IOMMUv2 optionally.
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 3a74e4410fc..86e2f4a62b9 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -26,6 +26,8 @@
  * These routines are used by both DMA-remapping and Interrupt-remapping
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt /* has to precede printk.h */
+
 #include <linux/pci.h>
 #include <linux/dmar.h>
 #include <linux/iova.h>
@@ -39,8 +41,6 @@
 #include <asm/irq_remapping.h>
 #include <asm/iommu_table.h>
 
-#define PREFIX "DMAR: "
-
 /* No locks are needed as DMA remapping hardware unit
  * list is constructed at boot time and hotplug of
  * these units are not supported by the architecture.
@@ -83,16 +83,12 @@ static int __init dmar_parse_one_dev_scope(struct acpi_dmar_device_scope *scope,
 		 * ignore it
 		 */
 		if (!bus) {
-			printk(KERN_WARNING
-			PREFIX "Device scope bus [%d] not found\n",
-			scope->bus);
+			pr_warn("Device scope bus [%d] not found\n", scope->bus);
 			break;
 		}
 		pdev = pci_get_slot(bus, PCI_DEVFN(path->dev, path->fn));
 		if (!pdev) {
-			printk(KERN_WARNING PREFIX
-			"Device scope device [%04x:%02x:%02x.%02x] not found\n",
-				segment, bus->number, path->dev, path->fn);
+			/* warning will be printed below */
 			break;
 		}
 		path ++;
@@ -100,9 +96,8 @@ static int __init dmar_parse_one_dev_scope(struct acpi_dmar_device_scope *scope,
 		bus = pdev->subordinate;
 	}
 	if (!pdev) {
-		printk(KERN_WARNING PREFIX
-		"Device scope device [%04x:%02x:%02x.%02x] not found\n",
-		segment, scope->bus, path->dev, path->fn);
+		pr_warn("Device scope device [%04x:%02x:%02x.%02x] not found\n",
+			segment, scope->bus, path->dev, path->fn);
 		*dev = NULL;
 		return 0;
 	}
@@ -110,9 +105,8 @@ static int __init dmar_parse_one_dev_scope(struct acpi_dmar_device_scope *scope,
 			pdev->subordinate) || (scope->entry_type == \
 			ACPI_DMAR_SCOPE_TYPE_BRIDGE && !pdev->subordinate)) {
 		pci_dev_put(pdev);
-		printk(KERN_WARNING PREFIX
-			"Device scope type does not match for %s\n",
-			 pci_name(pdev));
+		pr_warn("Device scope type does not match for %s\n",
+			pci_name(pdev));
 		return -EINVAL;
 	}
 	*dev = pdev;
@@ -134,8 +128,7 @@ int __init dmar_parse_dev_scope(void *start, void *end, int *cnt,
 		    scope->entry_type == ACPI_DMAR_SCOPE_TYPE_BRIDGE)
 			(*cnt)++;
 		else if (scope->entry_type != ACPI_DMAR_SCOPE_TYPE_IOAPIC) {
-			printk(KERN_WARNING PREFIX
-			       "Unsupported device scope\n");
+			pr_warn("Unsupported device scope\n");
 		}
 		start += scope->length;
 	}
@@ -261,25 +254,23 @@ dmar_table_print_dmar_entry(struct acpi_dmar_header *header)
 	case ACPI_DMAR_TYPE_HARDWARE_UNIT:
 		drhd = container_of(header, struct acpi_dmar_hardware_unit,
 				    header);
-		printk (KERN_INFO PREFIX
-			"DRHD base: %#016Lx flags: %#x\n",
+		pr_info("DRHD base: %#016Lx flags: %#x\n",
 			(unsigned long long)drhd->address, drhd->flags);
 		break;
 	case ACPI_DMAR_TYPE_RESERVED_MEMORY:
 		rmrr = container_of(header, struct acpi_dmar_reserved_memory,
 				    header);
-		printk (KERN_INFO PREFIX
-			"RMRR base: %#016Lx end: %#016Lx\n",
+		pr_info("RMRR base: %#016Lx end: %#016Lx\n",
 			(unsigned long long)rmrr->base_address,
 			(unsigned long long)rmrr->end_address);
 		break;
 	case ACPI_DMAR_TYPE_ATSR:
 		atsr = container_of(header, struct acpi_dmar_atsr, header);
-		printk(KERN_INFO PREFIX "ATSR flags: %#x\n", atsr->flags);
+		pr_info("ATSR flags: %#x\n", atsr->flags);
 		break;
 	case ACPI_DMAR_HARDWARE_AFFINITY:
 		rhsa = container_of(header, struct acpi_dmar_rhsa, header);
-		printk(KERN_INFO PREFIX "RHSA base: %#016Lx proximity domain: %#x\n",
+		pr_info("RHSA base: %#016Lx proximity domain: %#x\n",
 		       (unsigned long long)rhsa->base_address,
 		       rhsa->proximity_domain);
 		break;
@@ -299,7 +290,7 @@ static int __init dmar_table_detect(void)
 				&dmar_tbl_size);
 
 	if (ACPI_SUCCESS(status) && !dmar_tbl) {
-		printk (KERN_WARNING PREFIX "Unable to map DMAR\n");
+		pr_warn("Unable to map DMAR\n");
 		status = AE_NOT_FOUND;
 	}
 
@@ -333,20 +324,18 @@ parse_dmar_table(void)
 		return -ENODEV;
 
 	if (dmar->width < PAGE_SHIFT - 1) {
-		printk(KERN_WARNING PREFIX "Invalid DMAR haw\n");
+		pr_warn("Invalid DMAR haw\n");
 		return -EINVAL;
 	}
 
-	printk (KERN_INFO PREFIX "Host address width %d\n",
-		dmar->width + 1);
+	pr_info("Host address width %d\n", dmar->width + 1);
 
 	entry_header = (struct acpi_dmar_header *)(dmar + 1);
 	while (((unsigned long)entry_header) <
 			(((unsigned long)dmar) + dmar_tbl->length)) {
 		/* Avoid looping forever on bad ACPI tables */
 		if (entry_header->length == 0) {
-			printk(KERN_WARNING PREFIX
-				"Invalid 0-length structure\n");
+			pr_warn("Invalid 0-length structure\n");
 			ret = -EINVAL;
 			break;
 		}
@@ -369,8 +358,7 @@ parse_dmar_table(void)
 #endif
 			break;
 		default:
-			printk(KERN_WARNING PREFIX
-				"Unknown DMAR structure type %d\n",
+			pr_warn("Unknown DMAR structure type %d\n",
 				entry_header->type);
 			ret = 0; /* for forward compatibility */
 			break;
@@ -469,12 +457,12 @@ int __init dmar_table_init(void)
 	ret = parse_dmar_table();
 	if (ret) {
 		if (ret != -ENODEV)
-			printk(KERN_INFO PREFIX "parse DMAR table failure.\n");
+			pr_info("parse DMAR table failure.\n");
 		return ret;
 	}
 
 	if (list_empty(&dmar_drhd_units)) {
-		printk(KERN_INFO PREFIX "No DMAR devices found\n");
+		pr_info("No DMAR devices found\n");
 		return -ENODEV;
 	}
 
@@ -506,8 +494,7 @@ int __init check_zero_address(void)
 			(((unsigned long)dmar) + dmar_tbl->length)) {
 		/* Avoid looping forever on bad ACPI tables */
 		if (entry_header->length == 0) {
-			printk(KERN_WARNING PREFIX
-				"Invalid 0-length structure\n");
+			pr_warn("Invalid 0-length structure\n");
 			return 0;
 		}
 
@@ -558,8 +545,7 @@ int __init detect_intel_iommu(void)
 
 		if (ret && irq_remapping_enabled && cpu_has_x2apic &&
 		    dmar->flags & 0x1)
-			printk(KERN_INFO
-			       "Queued invalidation will be enabled to support x2apic and Intr-remapping.\n");
+			pr_info("Queued invalidation will be enabled to support x2apic and Intr-remapping.\n");
 
 		if (ret && !no_iommu && !iommu_detected && !dmar_disabled) {
 			iommu_detected = 1;
@@ -579,14 +565,89 @@ int __init detect_intel_iommu(void)
 }
 
 
+static void unmap_iommu(struct intel_iommu *iommu)
+{
+	iounmap(iommu->reg);
+	release_mem_region(iommu->reg_phys, iommu->reg_size);
+}
+
+/**
+ * map_iommu: map the iommu's registers
+ * @iommu: the iommu to map
+ * @phys_addr: the physical address of the base resgister
+ *
+ * Memory map the iommu's registers.  Start w/ a single page, and
+ * possibly expand if that turns out to be insufficent.
+ */
+static int map_iommu(struct intel_iommu *iommu, u64 phys_addr)
+{
+	int map_size, err=0;
+
+	iommu->reg_phys = phys_addr;
+	iommu->reg_size = VTD_PAGE_SIZE;
+
+	if (!request_mem_region(iommu->reg_phys, iommu->reg_size, iommu->name)) {
+		pr_err("IOMMU: can't reserve memory\n");
+		err = -EBUSY;
+		goto out;
+	}
+
+	iommu->reg = ioremap(iommu->reg_phys, iommu->reg_size);
+	if (!iommu->reg) {
+		pr_err("IOMMU: can't map the region\n");
+		err = -ENOMEM;
+		goto release;
+	}
+
+	iommu->cap = dmar_readq(iommu->reg + DMAR_CAP_REG);
+	iommu->ecap = dmar_readq(iommu->reg + DMAR_ECAP_REG);
+
+	if (iommu->cap == (uint64_t)-1 && iommu->ecap == (uint64_t)-1) {
+		err = -EINVAL;
+		warn_invalid_dmar(phys_addr, " returns all ones");
+		goto unmap;
+	}
+
+	/* the registers might be more than one page */
+	map_size = max_t(int, ecap_max_iotlb_offset(iommu->ecap),
+			 cap_max_fault_reg_offset(iommu->cap));
+	map_size = VTD_PAGE_ALIGN(map_size);
+	if (map_size > iommu->reg_size) {
+		iounmap(iommu->reg);
+		release_mem_region(iommu->reg_phys, iommu->reg_size);
+		iommu->reg_size = map_size;
+		if (!request_mem_region(iommu->reg_phys, iommu->reg_size,
+					iommu->name)) {
+			pr_err("IOMMU: can't reserve memory\n");
+			err = -EBUSY;
+			goto out;
+		}
+		iommu->reg = ioremap(iommu->reg_phys, iommu->reg_size);
+		if (!iommu->reg) {
+			pr_err("IOMMU: can't map the region\n");
+			err = -ENOMEM;
+			goto release;
+		}
+	}
+	err = 0;
+	goto out;
+
+unmap:
+	iounmap(iommu->reg);
+release:
+	release_mem_region(iommu->reg_phys, iommu->reg_size);
+out:
+	return err;
+}
+
 int alloc_iommu(struct dmar_drhd_unit *drhd)
 {
 	struct intel_iommu *iommu;
-	int map_size;
 	u32 ver;
 	static int iommu_allocated = 0;
 	int agaw = 0;
 	int msagaw = 0;
+	int err;
 
 	if (!drhd->reg_base_addr) {
 		warn_invalid_dmar(0, "");
@@ -600,30 +661,22 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 	iommu->seq_id = iommu_allocated++;
 	sprintf (iommu->name, "dmar%d", iommu->seq_id);
 
-	iommu->reg = ioremap(drhd->reg_base_addr, VTD_PAGE_SIZE);
-	if (!iommu->reg) {
-		printk(KERN_ERR "IOMMU: can't map the region\n");
+	err = map_iommu(iommu, drhd->reg_base_addr);
+	if (err) {
+		pr_err("IOMMU: failed to map %s\n", iommu->name);
 		goto error;
 	}
-	iommu->cap = dmar_readq(iommu->reg + DMAR_CAP_REG);
-	iommu->ecap = dmar_readq(iommu->reg + DMAR_ECAP_REG);
-
-	if (iommu->cap == (uint64_t)-1 && iommu->ecap == (uint64_t)-1) {
-		warn_invalid_dmar(drhd->reg_base_addr, " returns all ones");
-		goto err_unmap;
-	}
 
+	err = -EINVAL;
 	agaw = iommu_calculate_agaw(iommu);
 	if (agaw < 0) {
-		printk(KERN_ERR
-		       "Cannot get a valid agaw for iommu (seq_id = %d)\n",
-		       iommu->seq_id);
+		pr_err("Cannot get a valid agaw for iommu (seq_id = %d)\n",
+			iommu->seq_id);
 		goto err_unmap;
 	}
 	msagaw = iommu_calculate_max_sagaw(iommu);
 	if (msagaw < 0) {
-		printk(KERN_ERR
-			"Cannot get a valid max agaw for iommu (seq_id = %d)\n",
+		pr_err("Cannot get a valid max agaw for iommu (seq_id = %d)\n",
 			iommu->seq_id);
 		goto err_unmap;
 	}
@@ -632,19 +685,6 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 
 	iommu->node = -1;
 
-	/* the registers might be more than one page */
-	map_size = max_t(int, ecap_max_iotlb_offset(iommu->ecap),
-		cap_max_fault_reg_offset(iommu->cap));
-	map_size = VTD_PAGE_ALIGN(map_size);
-	if (map_size > VTD_PAGE_SIZE) {
-		iounmap(iommu->reg);
-		iommu->reg = ioremap(drhd->reg_base_addr, map_size);
-		if (!iommu->reg) {
-			printk(KERN_ERR "IOMMU: can't map the region\n");
-			goto error;
-		}
-	}
-
 	ver = readl(iommu->reg + DMAR_VER_REG);
 	pr_info("IOMMU %d: reg_base_addr %llx ver %d:%d cap %llx ecap %llx\n",
 		iommu->seq_id,
@@ -659,10 +699,10 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 	return 0;
 
  err_unmap:
-	iounmap(iommu->reg);
+	unmap_iommu(iommu);
  error:
 	kfree(iommu);
-	return -1;
+	return err;
 }
 
 void free_iommu(struct intel_iommu *iommu)
@@ -673,7 +713,8 @@ void free_iommu(struct intel_iommu *iommu)
 	free_dmar_iommu(iommu);
 
 	if (iommu->reg)
-		iounmap(iommu->reg);
+		unmap_iommu(iommu);
+
 	kfree(iommu);
 }
 
@@ -710,7 +751,7 @@ static int qi_check_fault(struct intel_iommu *iommu, int index)
 	if (fault & DMA_FSTS_IQE) {
 		head = readl(iommu->reg + DMAR_IQH_REG);
 		if ((head >> DMAR_IQ_SHIFT) == index) {
-			printk(KERN_ERR "VT-d detected invalid descriptor: "
+			pr_err("VT-d detected invalid descriptor: "
 				"low=%llx, high=%llx\n",
 				(unsigned long long)qi->desc[index].low,
 				(unsigned long long)qi->desc[index].high);
@@ -1129,15 +1170,14 @@ static int dmar_fault_do_one(struct intel_iommu *iommu, int type,
 	reason = dmar_get_fault_reason(fault_reason, &fault_type);
 
 	if (fault_type == INTR_REMAP)
-		printk(KERN_ERR "INTR-REMAP: Request device [[%02x:%02x.%d] "
+		pr_err("INTR-REMAP: Request device [[%02x:%02x.%d] "
 		       "fault index %llx\n"
 			"INTR-REMAP:[fault reason %02d] %s\n",
 			(source_id >> 8), PCI_SLOT(source_id & 0xFF),
 			PCI_FUNC(source_id & 0xFF), addr >> 48,
 			fault_reason, reason);
 	else
-		printk(KERN_ERR
-		       "DMAR:[%s] Request device [%02x:%02x.%d] "
+		pr_err("DMAR:[%s] Request device [%02x:%02x.%d] "
 		       "fault addr %llx \n"
 		       "DMAR:[fault reason %02d] %s\n",
 		       (type ? "DMA Read" : "DMA Write"),
@@ -1157,8 +1197,7 @@ irqreturn_t dmar_fault(int irq, void *dev_id)
 	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	fault_status = readl(iommu->reg + DMAR_FSTS_REG);
 	if (fault_status)
-		printk(KERN_ERR "DRHD: handling fault status reg %x\n",
-		       fault_status);
+		pr_err("DRHD: handling fault status reg %x\n", fault_status);
 
 	/* TBD: ignore advanced fault log currently */
 	if (!(fault_status & DMA_FSTS_PPF))
@@ -1224,7 +1263,7 @@ int dmar_set_interrupt(struct intel_iommu *iommu)
 
 	irq = create_irq();
 	if (!irq) {
-		printk(KERN_ERR "IOMMU: no free vectors\n");
+		pr_err("IOMMU: no free vectors\n");
 		return -EINVAL;
 	}
 
@@ -1241,7 +1280,7 @@ int dmar_set_interrupt(struct intel_iommu *iommu)
 
 	ret = request_irq(irq, dmar_fault, IRQF_NO_THREAD, iommu->name, iommu);
 	if (ret)
-		printk(KERN_ERR "IOMMU: can't request irq\n");
+		pr_err("IOMMU: can't request irq\n");
 	return ret;
 }
 
@@ -1258,8 +1297,7 @@ int __init enable_drhd_fault_handling(void)
 		ret = dmar_set_interrupt(iommu);
 
 		if (ret) {
-			printk(KERN_ERR "DRHD %Lx: failed to enable fault, "
-			       " interrupt, ret %d\n",
+			pr_err("DRHD %Lx: failed to enable fault, interrupt, ret %d\n",
 			       (unsigned long long)drhd->reg_base_addr, ret);
 			return -1;
 		}
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 9a114b9ff17..45350ff5e93 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -317,7 +317,7 @@ static int default_fault_handler(enum exynos_sysmmu_inttype itype,
 	if ((itype >= SYSMMU_FAULTS_NUM) || (itype < SYSMMU_PAGEFAULT))
 		itype = SYSMMU_FAULT_UNKNOWN;
 
-	pr_err("%s occured at 0x%lx(Page table base: 0x%lx)\n",
+	pr_err("%s occurred at 0x%lx(Page table base: 0x%lx)\n",
 			sysmmu_fault_name[itype], fault_addr, pgtable_base);
 
 	ent = section_entry(__va(pgtable_base), fault_addr);
@@ -732,6 +732,10 @@ static int exynos_iommu_domain_init(struct iommu_domain *domain)
 	spin_lock_init(&priv->pgtablelock);
 	INIT_LIST_HEAD(&priv->clients);
 
+	dom->geometry.aperture_start = 0;
+	dom->geometry.aperture_end   = ~0UL;
+	dom->geometry.force_aperture = true;
+
 	domain->priv = priv;
 	return 0;
 
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index b12af2ff8c5..7469b534664 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -661,7 +661,7 @@ static struct intel_iommu *device_to_iommu(int segment, u8 bus, u8 devfn)
 			if (drhd->devices[i] &&
 			    drhd->devices[i]->subordinate &&
 			    drhd->devices[i]->subordinate->number <= bus &&
-			    drhd->devices[i]->subordinate->subordinate >= bus)
+			    drhd->devices[i]->subordinate->busn_res.end >= bus)
 				return drhd->iommu;
 		}
 
@@ -3932,6 +3932,10 @@ static int intel_iommu_domain_init(struct iommu_domain *domain)
 	domain_update_iommu_cap(dmar_domain);
 	domain->priv = dmar_domain;
 
+	domain->geometry.aperture_start = 0;
+	domain->geometry.aperture_end   = __DOMAIN_MAX_ADDR(dmar_domain->gaw);
+	domain->geometry.force_aperture = true;
+
 	return 0;
 }
 
@@ -4090,52 +4094,70 @@ static int intel_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
-/*
- * Group numbers are arbitrary.  Device with the same group number
- * indicate the iommu cannot differentiate between them.  To avoid
- * tracking used groups we just use the seg|bus|devfn of the lowest
- * level we're able to differentiate devices
- */
-static int intel_iommu_device_group(struct device *dev, unsigned int *groupid)
+static void swap_pci_ref(struct pci_dev **from, struct pci_dev *to)
 {
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct pci_dev *bridge;
-	union {
-		struct {
-			u8 devfn;
-			u8 bus;
-			u16 segment;
-		} pci;
-		u32 group;
-	} id;
+	pci_dev_put(*from);
+	*from = to;
+}
 
-	if (iommu_no_mapping(dev))
-		return -ENODEV;
+#define REQ_ACS_FLAGS	(PCI_ACS_SV | PCI_ACS_RR | PCI_ACS_CR | PCI_ACS_UF)
 
-	id.pci.segment = pci_domain_nr(pdev->bus);
-	id.pci.bus = pdev->bus->number;
-	id.pci.devfn = pdev->devfn;
+static int intel_iommu_add_device(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_dev *bridge, *dma_pdev;
+	struct iommu_group *group;
+	int ret;
 
-	if (!device_to_iommu(id.pci.segment, id.pci.bus, id.pci.devfn))
+	if (!device_to_iommu(pci_domain_nr(pdev->bus),
+			     pdev->bus->number, pdev->devfn))
 		return -ENODEV;
 
 	bridge = pci_find_upstream_pcie_bridge(pdev);
 	if (bridge) {
-		if (pci_is_pcie(bridge)) {
-			id.pci.bus = bridge->subordinate->number;
-			id.pci.devfn = 0;
-		} else {
-			id.pci.bus = bridge->bus->number;
-			id.pci.devfn = bridge->devfn;
-		}
+		if (pci_is_pcie(bridge))
+			dma_pdev = pci_get_domain_bus_and_slot(
+						pci_domain_nr(pdev->bus),
+						bridge->subordinate->number, 0);
+		else
+			dma_pdev = pci_dev_get(bridge);
+	} else
+		dma_pdev = pci_dev_get(pdev);
+
+	swap_pci_ref(&dma_pdev, pci_get_dma_source(dma_pdev));
+
+	if (dma_pdev->multifunction &&
+	    !pci_acs_enabled(dma_pdev, REQ_ACS_FLAGS))
+		swap_pci_ref(&dma_pdev,
+			     pci_get_slot(dma_pdev->bus,
+					  PCI_DEVFN(PCI_SLOT(dma_pdev->devfn),
+					  0)));
+
+	while (!pci_is_root_bus(dma_pdev->bus)) {
+		if (pci_acs_path_enabled(dma_pdev->bus->self,
+					 NULL, REQ_ACS_FLAGS))
+			break;
+
+		swap_pci_ref(&dma_pdev, pci_dev_get(dma_pdev->bus->self));
+	}
+
+	group = iommu_group_get(&dma_pdev->dev);
+	pci_dev_put(dma_pdev);
+	if (!group) {
+		group = iommu_group_alloc();
+		if (IS_ERR(group))
+			return PTR_ERR(group);
 	}
 
-	if (!pdev->is_virtfn && iommu_group_mf)
-		id.pci.devfn = PCI_DEVFN(PCI_SLOT(id.pci.devfn), 0);
+	ret = iommu_group_add_device(group, dev);
 
-	*groupid = id.group;
+	iommu_group_put(group);
+	return ret;
+}
 
-	return 0;
+static void intel_iommu_remove_device(struct device *dev)
+{
+	iommu_group_remove_device(dev);
 }
 
 static struct iommu_ops intel_iommu_ops = {
@@ -4147,7 +4169,8 @@ static struct iommu_ops intel_iommu_ops = {
 	.unmap		= intel_iommu_unmap,
 	.iova_to_phys	= intel_iommu_iova_to_phys,
 	.domain_has_cap = intel_iommu_domain_has_cap,
-	.device_group	= intel_iommu_device_group,
+	.add_device	= intel_iommu_add_device,
+	.remove_device	= intel_iommu_remove_device,
 	.pgsize_bitmap	= INTEL_IOMMU_PGSIZES,
 };
 
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index 6d347064b8b..e0b18f3ae9a 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -902,7 +902,6 @@ static int intel_setup_ioapic_entry(int irq,
 	return 0;
 }
 
-#ifdef CONFIG_SMP
 /*
  * Migrate the IO-APIC irq in the presence of intr-remapping.
  *
@@ -924,6 +923,10 @@ intel_ioapic_set_affinity(struct irq_data *data, const struct cpumask *mask,
 	struct irq_cfg *cfg = data->chip_data;
 	unsigned int dest, irq = data->irq;
 	struct irte irte;
+	int err;
+
+	if (!config_enabled(CONFIG_SMP))
+		return -EINVAL;
 
 	if (!cpumask_intersects(mask, cpu_online_mask))
 		return -EINVAL;
@@ -931,10 +934,16 @@ intel_ioapic_set_affinity(struct irq_data *data, const struct cpumask *mask,
 	if (get_irte(irq, &irte))
 		return -EBUSY;
 
-	if (assign_irq_vector(irq, cfg, mask))
-		return -EBUSY;
+	err = assign_irq_vector(irq, cfg, mask);
+	if (err)
+		return err;
 
-	dest = apic->cpu_mask_to_apicid_and(cfg->domain, mask);
+	err = apic->cpu_mask_to_apicid_and(cfg->domain, mask, &dest);
+	if (err) {
+		if (assign_irq_vector(irq, cfg, data->affinity))
+			pr_err("Failed to recover vector for irq %d\n", irq);
+		return err;
+	}
 
 	irte.vector = cfg->vector;
 	irte.dest_id = IRTE_DEST(dest);
@@ -956,7 +965,6 @@ intel_ioapic_set_affinity(struct irq_data *data, const struct cpumask *mask,
 	cpumask_copy(data->affinity, mask);
 	return 0;
 }
-#endif
 
 static void intel_compose_msi_msg(struct pci_dev *pdev,
 				  unsigned int irq, unsigned int dest,
@@ -1058,9 +1066,7 @@ struct irq_remap_ops intel_irq_remap_ops = {
 	.reenable		= reenable_irq_remapping,
 	.enable_faulting	= enable_drhd_fault_handling,
 	.setup_ioapic_entry	= intel_setup_ioapic_entry,
-#ifdef CONFIG_SMP
 	.set_affinity		= intel_ioapic_set_affinity,
-#endif
 	.free_irq		= free_irte,
 	.compose_msi_msg	= intel_compose_msi_msg,
 	.msi_alloc_irq		= intel_msi_alloc_irq,
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 8b9ded88e6f..ddbdacad776 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -26,60 +26,535 @@
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/iommu.h>
+#include <linux/idr.h>
+#include <linux/notifier.h>
+#include <linux/err.h>
+
+static struct kset *iommu_group_kset;
+static struct ida iommu_group_ida;
+static struct mutex iommu_group_mutex;
+
+struct iommu_group {
+	struct kobject kobj;
+	struct kobject *devices_kobj;
+	struct list_head devices;
+	struct mutex mutex;
+	struct blocking_notifier_head notifier;
+	void *iommu_data;
+	void (*iommu_data_release)(void *iommu_data);
+	char *name;
+	int id;
+};
+
+struct iommu_device {
+	struct list_head list;
+	struct device *dev;
+	char *name;
+};
+
+struct iommu_group_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct iommu_group *group, char *buf);
+	ssize_t (*store)(struct iommu_group *group,
+			 const char *buf, size_t count);
+};
+
+#define IOMMU_GROUP_ATTR(_name, _mode, _show, _store)		\
+struct iommu_group_attribute iommu_group_attr_##_name =		\
+	__ATTR(_name, _mode, _show, _store)
+
+#define to_iommu_group_attr(_attr)	\
+	container_of(_attr, struct iommu_group_attribute, attr)
+#define to_iommu_group(_kobj)		\
+	container_of(_kobj, struct iommu_group, kobj)
 
-static ssize_t show_iommu_group(struct device *dev,
-				struct device_attribute *attr, char *buf)
+static ssize_t iommu_group_attr_show(struct kobject *kobj,
+				     struct attribute *__attr, char *buf)
 {
-	unsigned int groupid;
+	struct iommu_group_attribute *attr = to_iommu_group_attr(__attr);
+	struct iommu_group *group = to_iommu_group(kobj);
+	ssize_t ret = -EIO;
 
-	if (iommu_device_group(dev, &groupid))
-		return 0;
+	if (attr->show)
+		ret = attr->show(group, buf);
+	return ret;
+}
+
+static ssize_t iommu_group_attr_store(struct kobject *kobj,
+				      struct attribute *__attr,
+				      const char *buf, size_t count)
+{
+	struct iommu_group_attribute *attr = to_iommu_group_attr(__attr);
+	struct iommu_group *group = to_iommu_group(kobj);
+	ssize_t ret = -EIO;
+
+	if (attr->store)
+		ret = attr->store(group, buf, count);
+	return ret;
+}
+
+static const struct sysfs_ops iommu_group_sysfs_ops = {
+	.show = iommu_group_attr_show,
+	.store = iommu_group_attr_store,
+};
 
-	return sprintf(buf, "%u", groupid);
+static int iommu_group_create_file(struct iommu_group *group,
+				   struct iommu_group_attribute *attr)
+{
+	return sysfs_create_file(&group->kobj, &attr->attr);
 }
-static DEVICE_ATTR(iommu_group, S_IRUGO, show_iommu_group, NULL);
 
-static int add_iommu_group(struct device *dev, void *data)
+static void iommu_group_remove_file(struct iommu_group *group,
+				    struct iommu_group_attribute *attr)
+{
+	sysfs_remove_file(&group->kobj, &attr->attr);
+}
+
+static ssize_t iommu_group_show_name(struct iommu_group *group, char *buf)
+{
+	return sprintf(buf, "%s\n", group->name);
+}
+
+static IOMMU_GROUP_ATTR(name, S_IRUGO, iommu_group_show_name, NULL);
+
+static void iommu_group_release(struct kobject *kobj)
+{
+	struct iommu_group *group = to_iommu_group(kobj);
+
+	if (group->iommu_data_release)
+		group->iommu_data_release(group->iommu_data);
+
+	mutex_lock(&iommu_group_mutex);
+	ida_remove(&iommu_group_ida, group->id);
+	mutex_unlock(&iommu_group_mutex);
+
+	kfree(group->name);
+	kfree(group);
+}
+
+static struct kobj_type iommu_group_ktype = {
+	.sysfs_ops = &iommu_group_sysfs_ops,
+	.release = iommu_group_release,
+};
+
+/**
+ * iommu_group_alloc - Allocate a new group
+ * @name: Optional name to associate with group, visible in sysfs
+ *
+ * This function is called by an iommu driver to allocate a new iommu
+ * group.  The iommu group represents the minimum granularity of the iommu.
+ * Upon successful return, the caller holds a reference to the supplied
+ * group in order to hold the group until devices are added.  Use
+ * iommu_group_put() to release this extra reference count, allowing the
+ * group to be automatically reclaimed once it has no devices or external
+ * references.
+ */
+struct iommu_group *iommu_group_alloc(void)
+{
+	struct iommu_group *group;
+	int ret;
+
+	group = kzalloc(sizeof(*group), GFP_KERNEL);
+	if (!group)
+		return ERR_PTR(-ENOMEM);
+
+	group->kobj.kset = iommu_group_kset;
+	mutex_init(&group->mutex);
+	INIT_LIST_HEAD(&group->devices);
+	BLOCKING_INIT_NOTIFIER_HEAD(&group->notifier);
+
+	mutex_lock(&iommu_group_mutex);
+
+again:
+	if (unlikely(0 == ida_pre_get(&iommu_group_ida, GFP_KERNEL))) {
+		kfree(group);
+		mutex_unlock(&iommu_group_mutex);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	if (-EAGAIN == ida_get_new(&iommu_group_ida, &group->id))
+		goto again;
+
+	mutex_unlock(&iommu_group_mutex);
+
+	ret = kobject_init_and_add(&group->kobj, &iommu_group_ktype,
+				   NULL, "%d", group->id);
+	if (ret) {
+		mutex_lock(&iommu_group_mutex);
+		ida_remove(&iommu_group_ida, group->id);
+		mutex_unlock(&iommu_group_mutex);
+		kfree(group);
+		return ERR_PTR(ret);
+	}
+
+	group->devices_kobj = kobject_create_and_add("devices", &group->kobj);
+	if (!group->devices_kobj) {
+		kobject_put(&group->kobj); /* triggers .release & free */
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/*
+	 * The devices_kobj holds a reference on the group kobject, so
+	 * as long as that exists so will the group.  We can therefore
+	 * use the devices_kobj for reference counting.
+	 */
+	kobject_put(&group->kobj);
+
+	return group;
+}
+EXPORT_SYMBOL_GPL(iommu_group_alloc);
+
+/**
+ * iommu_group_get_iommudata - retrieve iommu_data registered for a group
+ * @group: the group
+ *
+ * iommu drivers can store data in the group for use when doing iommu
+ * operations.  This function provides a way to retrieve it.  Caller
+ * should hold a group reference.
+ */
+void *iommu_group_get_iommudata(struct iommu_group *group)
+{
+	return group->iommu_data;
+}
+EXPORT_SYMBOL_GPL(iommu_group_get_iommudata);
+
+/**
+ * iommu_group_set_iommudata - set iommu_data for a group
+ * @group: the group
+ * @iommu_data: new data
+ * @release: release function for iommu_data
+ *
+ * iommu drivers can store data in the group for use when doing iommu
+ * operations.  This function provides a way to set the data after
+ * the group has been allocated.  Caller should hold a group reference.
+ */
+void iommu_group_set_iommudata(struct iommu_group *group, void *iommu_data,
+			       void (*release)(void *iommu_data))
 {
-	unsigned int groupid;
+	group->iommu_data = iommu_data;
+	group->iommu_data_release = release;
+}
+EXPORT_SYMBOL_GPL(iommu_group_set_iommudata);
 
-	if (iommu_device_group(dev, &groupid) == 0)
-		return device_create_file(dev, &dev_attr_iommu_group);
+/**
+ * iommu_group_set_name - set name for a group
+ * @group: the group
+ * @name: name
+ *
+ * Allow iommu driver to set a name for a group.  When set it will
+ * appear in a name attribute file under the group in sysfs.
+ */
+int iommu_group_set_name(struct iommu_group *group, const char *name)
+{
+	int ret;
+
+	if (group->name) {
+		iommu_group_remove_file(group, &iommu_group_attr_name);
+		kfree(group->name);
+		group->name = NULL;
+		if (!name)
+			return 0;
+	}
+
+	group->name = kstrdup(name, GFP_KERNEL);
+	if (!group->name)
+		return -ENOMEM;
+
+	ret = iommu_group_create_file(group, &iommu_group_attr_name);
+	if (ret) {
+		kfree(group->name);
+		group->name = NULL;
+		return ret;
+	}
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(iommu_group_set_name);
+
+/**
+ * iommu_group_add_device - add a device to an iommu group
+ * @group: the group into which to add the device (reference should be held)
+ * @dev: the device
+ *
+ * This function is called by an iommu driver to add a device into a
+ * group.  Adding a device increments the group reference count.
+ */
+int iommu_group_add_device(struct iommu_group *group, struct device *dev)
+{
+	int ret, i = 0;
+	struct iommu_device *device;
+
+	device = kzalloc(sizeof(*device), GFP_KERNEL);
+	if (!device)
+		return -ENOMEM;
+
+	device->dev = dev;
+
+	ret = sysfs_create_link(&dev->kobj, &group->kobj, "iommu_group");
+	if (ret) {
+		kfree(device);
+		return ret;
+	}
+
+	device->name = kasprintf(GFP_KERNEL, "%s", kobject_name(&dev->kobj));
+rename:
+	if (!device->name) {
+		sysfs_remove_link(&dev->kobj, "iommu_group");
+		kfree(device);
+		return -ENOMEM;
+	}
+
+	ret = sysfs_create_link_nowarn(group->devices_kobj,
+				       &dev->kobj, device->name);
+	if (ret) {
+		kfree(device->name);
+		if (ret == -EEXIST && i >= 0) {
+			/*
+			 * Account for the slim chance of collision
+			 * and append an instance to the name.
+			 */
+			device->name = kasprintf(GFP_KERNEL, "%s.%d",
+						 kobject_name(&dev->kobj), i++);
+			goto rename;
+		}
+
+		sysfs_remove_link(&dev->kobj, "iommu_group");
+		kfree(device);
+		return ret;
+	}
+
+	kobject_get(group->devices_kobj);
+
+	dev->iommu_group = group;
+
+	mutex_lock(&group->mutex);
+	list_add_tail(&device->list, &group->devices);
+	mutex_unlock(&group->mutex);
+
+	/* Notify any listeners about change to group. */
+	blocking_notifier_call_chain(&group->notifier,
+				     IOMMU_GROUP_NOTIFY_ADD_DEVICE, dev);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iommu_group_add_device);
+
+/**
+ * iommu_group_remove_device - remove a device from it's current group
+ * @dev: device to be removed
+ *
+ * This function is called by an iommu driver to remove the device from
+ * it's current group.  This decrements the iommu group reference count.
+ */
+void iommu_group_remove_device(struct device *dev)
+{
+	struct iommu_group *group = dev->iommu_group;
+	struct iommu_device *tmp_device, *device = NULL;
+
+	/* Pre-notify listeners that a device is being removed. */
+	blocking_notifier_call_chain(&group->notifier,
+				     IOMMU_GROUP_NOTIFY_DEL_DEVICE, dev);
+
+	mutex_lock(&group->mutex);
+	list_for_each_entry(tmp_device, &group->devices, list) {
+		if (tmp_device->dev == dev) {
+			device = tmp_device;
+			list_del(&device->list);
+			break;
+		}
+	}
+	mutex_unlock(&group->mutex);
+
+	if (!device)
+		return;
+
+	sysfs_remove_link(group->devices_kobj, device->name);
+	sysfs_remove_link(&dev->kobj, "iommu_group");
+
+	kfree(device->name);
+	kfree(device);
+	dev->iommu_group = NULL;
+	kobject_put(group->devices_kobj);
+}
+EXPORT_SYMBOL_GPL(iommu_group_remove_device);
+
+/**
+ * iommu_group_for_each_dev - iterate over each device in the group
+ * @group: the group
+ * @data: caller opaque data to be passed to callback function
+ * @fn: caller supplied callback function
+ *
+ * This function is called by group users to iterate over group devices.
+ * Callers should hold a reference count to the group during callback.
+ * The group->mutex is held across callbacks, which will block calls to
+ * iommu_group_add/remove_device.
+ */
+int iommu_group_for_each_dev(struct iommu_group *group, void *data,
+			     int (*fn)(struct device *, void *))
+{
+	struct iommu_device *device;
+	int ret = 0;
+
+	mutex_lock(&group->mutex);
+	list_for_each_entry(device, &group->devices, list) {
+		ret = fn(device->dev, data);
+		if (ret)
+			break;
+	}
+	mutex_unlock(&group->mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iommu_group_for_each_dev);
+
+/**
+ * iommu_group_get - Return the group for a device and increment reference
+ * @dev: get the group that this device belongs to
+ *
+ * This function is called by iommu drivers and users to get the group
+ * for the specified device.  If found, the group is returned and the group
+ * reference in incremented, else NULL.
+ */
+struct iommu_group *iommu_group_get(struct device *dev)
+{
+	struct iommu_group *group = dev->iommu_group;
+
+	if (group)
+		kobject_get(group->devices_kobj);
+
+	return group;
+}
+EXPORT_SYMBOL_GPL(iommu_group_get);
+
+/**
+ * iommu_group_put - Decrement group reference
+ * @group: the group to use
+ *
+ * This function is called by iommu drivers and users to release the
+ * iommu group.  Once the reference count is zero, the group is released.
+ */
+void iommu_group_put(struct iommu_group *group)
+{
+	if (group)
+		kobject_put(group->devices_kobj);
+}
+EXPORT_SYMBOL_GPL(iommu_group_put);
+
+/**
+ * iommu_group_register_notifier - Register a notifier for group changes
+ * @group: the group to watch
+ * @nb: notifier block to signal
+ *
+ * This function allows iommu group users to track changes in a group.
+ * See include/linux/iommu.h for actions sent via this notifier.  Caller
+ * should hold a reference to the group throughout notifier registration.
+ */
+int iommu_group_register_notifier(struct iommu_group *group,
+				  struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&group->notifier, nb);
+}
+EXPORT_SYMBOL_GPL(iommu_group_register_notifier);
+
+/**
+ * iommu_group_unregister_notifier - Unregister a notifier
+ * @group: the group to watch
+ * @nb: notifier block to signal
+ *
+ * Unregister a previously registered group notifier block.
+ */
+int iommu_group_unregister_notifier(struct iommu_group *group,
+				    struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&group->notifier, nb);
+}
+EXPORT_SYMBOL_GPL(iommu_group_unregister_notifier);
+
+/**
+ * iommu_group_id - Return ID for a group
+ * @group: the group to ID
+ *
+ * Return the unique ID for the group matching the sysfs group number.
+ */
+int iommu_group_id(struct iommu_group *group)
+{
+	return group->id;
+}
+EXPORT_SYMBOL_GPL(iommu_group_id);
 
-static int remove_iommu_group(struct device *dev)
+static int add_iommu_group(struct device *dev, void *data)
 {
-	unsigned int groupid;
+	struct iommu_ops *ops = data;
+
+	if (!ops->add_device)
+		return -ENODEV;
 
-	if (iommu_device_group(dev, &groupid) == 0)
-		device_remove_file(dev, &dev_attr_iommu_group);
+	WARN_ON(dev->iommu_group);
+
+	ops->add_device(dev);
 
 	return 0;
 }
 
-static int iommu_device_notifier(struct notifier_block *nb,
-				 unsigned long action, void *data)
+static int iommu_bus_notifier(struct notifier_block *nb,
+			      unsigned long action, void *data)
 {
 	struct device *dev = data;
+	struct iommu_ops *ops = dev->bus->iommu_ops;
+	struct iommu_group *group;
+	unsigned long group_action = 0;
+
+	/*
+	 * ADD/DEL call into iommu driver ops if provided, which may
+	 * result in ADD/DEL notifiers to group->notifier
+	 */
+	if (action == BUS_NOTIFY_ADD_DEVICE) {
+		if (ops->add_device)
+			return ops->add_device(dev);
+	} else if (action == BUS_NOTIFY_DEL_DEVICE) {
+		if (ops->remove_device && dev->iommu_group) {
+			ops->remove_device(dev);
+			return 0;
+		}
+	}
+
+	/*
+	 * Remaining BUS_NOTIFYs get filtered and republished to the
+	 * group, if anyone is listening
+	 */
+	group = iommu_group_get(dev);
+	if (!group)
+		return 0;
+
+	switch (action) {
+	case BUS_NOTIFY_BIND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_BIND_DRIVER;
+		break;
+	case BUS_NOTIFY_BOUND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_BOUND_DRIVER;
+		break;
+	case BUS_NOTIFY_UNBIND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_UNBIND_DRIVER;
+		break;
+	case BUS_NOTIFY_UNBOUND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_UNBOUND_DRIVER;
+		break;
+	}
 
-	if (action == BUS_NOTIFY_ADD_DEVICE)
-		return add_iommu_group(dev, NULL);
-	else if (action == BUS_NOTIFY_DEL_DEVICE)
-		return remove_iommu_group(dev);
+	if (group_action)
+		blocking_notifier_call_chain(&group->notifier,
+					     group_action, dev);
 
+	iommu_group_put(group);
 	return 0;
 }
 
-static struct notifier_block iommu_device_nb = {
-	.notifier_call = iommu_device_notifier,
+static struct notifier_block iommu_bus_nb = {
+	.notifier_call = iommu_bus_notifier,
 };
 
 static void iommu_bus_init(struct bus_type *bus, struct iommu_ops *ops)
 {
-	bus_register_notifier(bus, &iommu_device_nb);
-	bus_for_each_dev(bus, NULL, NULL, add_iommu_group);
+	bus_register_notifier(bus, &iommu_bus_nb);
+	bus_for_each_dev(bus, NULL, ops, add_iommu_group);
 }
 
 /**
@@ -192,6 +667,45 @@ void iommu_detach_device(struct iommu_domain *domain, struct device *dev)
 }
 EXPORT_SYMBOL_GPL(iommu_detach_device);
 
+/*
+ * IOMMU groups are really the natrual working unit of the IOMMU, but
+ * the IOMMU API works on domains and devices.  Bridge that gap by
+ * iterating over the devices in a group.  Ideally we'd have a single
+ * device which represents the requestor ID of the group, but we also
+ * allow IOMMU drivers to create policy defined minimum sets, where
+ * the physical hardware may be able to distiguish members, but we
+ * wish to group them at a higher level (ex. untrusted multi-function
+ * PCI devices).  Thus we attach each device.
+ */
+static int iommu_group_do_attach_device(struct device *dev, void *data)
+{
+	struct iommu_domain *domain = data;
+
+	return iommu_attach_device(domain, dev);
+}
+
+int iommu_attach_group(struct iommu_domain *domain, struct iommu_group *group)
+{
+	return iommu_group_for_each_dev(group, domain,
+					iommu_group_do_attach_device);
+}
+EXPORT_SYMBOL_GPL(iommu_attach_group);
+
+static int iommu_group_do_detach_device(struct device *dev, void *data)
+{
+	struct iommu_domain *domain = data;
+
+	iommu_detach_device(domain, dev);
+
+	return 0;
+}
+
+void iommu_detach_group(struct iommu_domain *domain, struct iommu_group *group)
+{
+	iommu_group_for_each_dev(group, domain, iommu_group_do_detach_device);
+}
+EXPORT_SYMBOL_GPL(iommu_detach_group);
+
 phys_addr_t iommu_iova_to_phys(struct iommu_domain *domain,
 			       unsigned long iova)
 {
@@ -336,11 +850,48 @@ size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
 }
 EXPORT_SYMBOL_GPL(iommu_unmap);
 
-int iommu_device_group(struct device *dev, unsigned int *groupid)
+static int __init iommu_init(void)
 {
-	if (iommu_present(dev->bus) && dev->bus->iommu_ops->device_group)
-		return dev->bus->iommu_ops->device_group(dev, groupid);
+	iommu_group_kset = kset_create_and_add("iommu_groups",
+					       NULL, kernel_kobj);
+	ida_init(&iommu_group_ida);
+	mutex_init(&iommu_group_mutex);
+
+	BUG_ON(!iommu_group_kset);
+
+	return 0;
+}
+subsys_initcall(iommu_init);
+
+int iommu_domain_get_attr(struct iommu_domain *domain,
+			  enum iommu_attr attr, void *data)
+{
+	struct iommu_domain_geometry *geometry;
+	int ret = 0;
+
+	switch (attr) {
+	case DOMAIN_ATTR_GEOMETRY:
+		geometry  = data;
+		*geometry = domain->geometry;
+
+		break;
+	default:
+		if (!domain->ops->domain_get_attr)
+			return -EINVAL;
+
+		ret = domain->ops->domain_get_attr(domain, attr, data);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iommu_domain_get_attr);
+
+int iommu_domain_set_attr(struct iommu_domain *domain,
+			  enum iommu_attr attr, void *data)
+{
+	if (!domain->ops->domain_set_attr)
+		return -EINVAL;
 
-	return -ENODEV;
+	return domain->ops->domain_set_attr(domain, attr, data);
 }
-EXPORT_SYMBOL_GPL(iommu_device_group);
+EXPORT_SYMBOL_GPL(iommu_domain_set_attr);
diff --git a/drivers/iommu/iova.c b/drivers/iommu/iova.c
index c5c274ab5c5..67da6cff74e 100644
--- a/drivers/iommu/iova.c
+++ b/drivers/iommu/iova.c
@@ -198,10 +198,10 @@ iova_insert_rbtree(struct rb_root *root, struct iova *iova)
 
 /**
  * alloc_iova - allocates an iova
- * @iovad - iova domain in question
- * @size - size of page frames to allocate
- * @limit_pfn - max limit address
- * @size_aligned - set if size_aligned address range is required
+ * @iovad: - iova domain in question
+ * @size: - size of page frames to allocate
+ * @limit_pfn: - max limit address
+ * @size_aligned: - set if size_aligned address range is required
  * This function allocates an iova in the range limit_pfn to IOVA_START_PFN
  * looking from limit_pfn instead from IOVA_START_PFN. If the size_aligned
  * flag is set then the allocated address iova->pfn_lo will be naturally
@@ -238,8 +238,8 @@ alloc_iova(struct iova_domain *iovad, unsigned long size,
 
 /**
  * find_iova - find's an iova for a given pfn
- * @iovad - iova domain in question.
- * pfn - page frame number
+ * @iovad: - iova domain in question.
+ * @pfn: - page frame number
  * This function finds and returns an iova belonging to the
  * given doamin which matches the given pfn.
  */
@@ -260,7 +260,7 @@ struct iova *find_iova(struct iova_domain *iovad, unsigned long pfn)
 			/* We are not holding the lock while this iova
 			 * is referenced by the caller as the same thread
 			 * which called this function also calls __free_iova()
-			 * and it is by desing that only one thread can possibly
+			 * and it is by design that only one thread can possibly
 			 * reference a particular iova and hence no conflict.
 			 */
 			return iova;
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 40cda8e98d8..151690db692 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -1,6 +1,11 @@
 #include <linux/kernel.h>
 #include <linux/string.h>
+#include <linux/cpumask.h>
 #include <linux/errno.h>
+#include <linux/msi.h>
+
+#include <asm/hw_irq.h>
+#include <asm/irq_remapping.h>
 
 #include "irq_remapping.h"
 
@@ -111,16 +116,15 @@ int setup_ioapic_remapped_entry(int irq,
 					     vector, attr);
 }
 
-#ifdef CONFIG_SMP
 int set_remapped_irq_affinity(struct irq_data *data, const struct cpumask *mask,
 			      bool force)
 {
-	if (!remap_ops || !remap_ops->set_affinity)
+	if (!config_enabled(CONFIG_SMP) || !remap_ops ||
+	    !remap_ops->set_affinity)
 		return 0;
 
 	return remap_ops->set_affinity(data, mask, force);
 }
-#endif
 
 void free_remapped_irq(int irq)
 {
diff --git a/drivers/iommu/irq_remapping.h b/drivers/iommu/irq_remapping.h
index be9d72950c5..b12974cc1df 100644
--- a/drivers/iommu/irq_remapping.h
+++ b/drivers/iommu/irq_remapping.h
@@ -59,11 +59,9 @@ struct irq_remap_ops {
 				  unsigned int, int,
 				  struct io_apic_irq_attr *);
 
-#ifdef CONFIG_SMP
 	/* Set the CPU affinity of a remapped interrupt */
 	int (*set_affinity)(struct irq_data *data, const struct cpumask *mask,
 			    bool force);
-#endif
 
 	/* Free an IRQ */
 	int (*free_irq)(int);
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index cee307e8660..6a8870a3166 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -226,6 +226,11 @@ static int msm_iommu_domain_init(struct iommu_domain *domain)
 
 	memset(priv->pgtable, 0, SZ_16K);
 	domain->priv = priv;
+
+	domain->geometry.aperture_start = 0;
+	domain->geometry.aperture_end   = (1ULL << 32) - 1;
+	domain->geometry.force_aperture = true;
+
 	return 0;
 
 fail_nomem:
diff --git a/drivers/iommu/of_iommu.c b/drivers/iommu/of_iommu.c
new file mode 100644
index 00000000000..ee249bc959f
--- /dev/null
+++ b/drivers/iommu/of_iommu.c
@@ -0,0 +1,90 @@
+/*
+ * OF helpers for IOMMU
+ *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/export.h>
+#include <linux/limits.h>
+#include <linux/of.h>
+
+/**
+ * of_get_dma_window - Parse *dma-window property and returns 0 if found.
+ *
+ * @dn: device node
+ * @prefix: prefix for property name if any
+ * @index: index to start to parse
+ * @busno: Returns busno if supported. Otherwise pass NULL
+ * @addr: Returns address that DMA starts
+ * @size: Returns the range that DMA can handle
+ *
+ * This supports different formats flexibly. "prefix" can be
+ * configured if any. "busno" and "index" are optionally
+ * specified. Set 0(or NULL) if not used.
+ */
+int of_get_dma_window(struct device_node *dn, const char *prefix, int index,
+		      unsigned long *busno, dma_addr_t *addr, size_t *size)
+{
+	const __be32 *dma_window, *end;
+	int bytes, cur_index = 0;
+	char propname[NAME_MAX], addrname[NAME_MAX], sizename[NAME_MAX];
+
+	if (!dn || !addr || !size)
+		return -EINVAL;
+
+	if (!prefix)
+		prefix = "";
+
+	snprintf(propname, sizeof(propname), "%sdma-window", prefix);
+	snprintf(addrname, sizeof(addrname), "%s#dma-address-cells", prefix);
+	snprintf(sizename, sizeof(sizename), "%s#dma-size-cells", prefix);
+
+	dma_window = of_get_property(dn, propname, &bytes);
+	if (!dma_window)
+		return -ENODEV;
+	end = dma_window + bytes / sizeof(*dma_window);
+
+	while (dma_window < end) {
+		u32 cells;
+		const void *prop;
+
+		/* busno is one cell if supported */
+		if (busno)
+			*busno = be32_to_cpup(dma_window++);
+
+		prop = of_get_property(dn, addrname, NULL);
+		if (!prop)
+			prop = of_get_property(dn, "#address-cells", NULL);
+
+		cells = prop ? be32_to_cpup(prop) : of_n_addr_cells(dn);
+		if (!cells)
+			return -EINVAL;
+		*addr = of_read_number(dma_window, cells);
+		dma_window += cells;
+
+		prop = of_get_property(dn, sizename, NULL);
+		cells = prop ? be32_to_cpup(prop) : of_n_size_cells(dn);
+		if (!cells)
+			return -EINVAL;
+		*size = of_read_number(dma_window, cells);
+		dma_window += cells;
+
+		if (cur_index++ == index)
+			break;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(of_get_dma_window);
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index e70ee2b59df..d0b1234581b 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -1148,6 +1148,10 @@ static int omap_iommu_domain_init(struct iommu_domain *domain)
 
 	domain->priv = omap_domain;
 
+	domain->geometry.aperture_start = 0;
+	domain->geometry.aperture_end   = (1ULL << 32) - 1;
+	domain->geometry.force_aperture = true;
+
 	return 0;
 
 fail_nomem:
diff --git a/drivers/iommu/tegra-gart.c b/drivers/iommu/tegra-gart.c
index 0c0a3779221..c16e8fc8a4b 100644
--- a/drivers/iommu/tegra-gart.c
+++ b/drivers/iommu/tegra-gart.c
@@ -165,6 +165,11 @@ static int gart_iommu_attach_dev(struct iommu_domain *domain,
 		return -EINVAL;
 	domain->priv = gart;
 
+	domain->geometry.aperture_start = gart->iovmm_base;
+	domain->geometry.aperture_end   = gart->iovmm_base +
+					gart->page_count * GART_PAGE_SIZE - 1;
+	domain->geometry.force_aperture = true;
+
 	client = devm_kzalloc(gart->dev, sizeof(*c), GFP_KERNEL);
 	if (!client)
 		return -ENOMEM;
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index 3f3d09d560e..4ba325ab626 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -30,12 +30,15 @@
 #include <linux/sched.h>
 #include <linux/iommu.h>
 #include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_iommu.h>
 
 #include <asm/page.h>
 #include <asm/cacheflush.h>
 
 #include <mach/iomap.h>
 #include <mach/smmu.h>
+#include <mach/tegra-ahb.h>
 
 /* bitmap of the page sizes currently supported */
 #define SMMU_IOMMU_PGSIZES	(SZ_4K)
@@ -111,12 +114,6 @@
 
 #define SMMU_PDE_NEXT_SHIFT		28
 
-/* AHB Arbiter Registers */
-#define AHB_XBAR_CTRL				0xe0
-#define AHB_XBAR_CTRL_SMMU_INIT_DONE_DONE	1
-#define AHB_XBAR_CTRL_SMMU_INIT_DONE_SHIFT	17
-
-#define SMMU_NUM_ASIDS				4
 #define SMMU_TLB_FLUSH_VA_SECTION__MASK		0xffc00000
 #define SMMU_TLB_FLUSH_VA_SECTION__SHIFT	12 /* right shift */
 #define SMMU_TLB_FLUSH_VA_GROUP__MASK		0xffffc000
@@ -136,6 +133,7 @@
 
 #define SMMU_PAGE_SHIFT 12
 #define SMMU_PAGE_SIZE	(1 << SMMU_PAGE_SHIFT)
+#define SMMU_PAGE_MASK	((1 << SMMU_PAGE_SHIFT) - 1)
 
 #define SMMU_PDIR_COUNT	1024
 #define SMMU_PDIR_SIZE	(sizeof(unsigned long) * SMMU_PDIR_COUNT)
@@ -177,6 +175,8 @@
 #define SMMU_ASID_DISABLE	0
 #define SMMU_ASID_ASID(n)	((n) & ~SMMU_ASID_ENABLE(0))
 
+#define NUM_SMMU_REG_BANKS	3
+
 #define smmu_client_enable_hwgrp(c, m)	smmu_client_set_hwgrp(c, m, 1)
 #define smmu_client_disable_hwgrp(c)	smmu_client_set_hwgrp(c, 0, 0)
 #define __smmu_client_enable_hwgrp(c, m) __smmu_client_set_hwgrp(c, m, 1)
@@ -235,14 +235,12 @@ struct smmu_as {
  * Per SMMU device - IOMMU device
  */
 struct smmu_device {
-	void __iomem	*regs, *regs_ahbarb;
+	void __iomem	*regs[NUM_SMMU_REG_BANKS];
 	unsigned long	iovmm_base;	/* remappable base address */
 	unsigned long	page_count;	/* total remappable size */
 	spinlock_t	lock;
 	char		*name;
 	struct device	*dev;
-	int		num_as;
-	struct smmu_as	*as;		/* Run-time allocated array */
 	struct page *avp_vector_page;	/* dummy page shared by all AS's */
 
 	/*
@@ -252,29 +250,50 @@ struct smmu_device {
 	unsigned long translation_enable_1;
 	unsigned long translation_enable_2;
 	unsigned long asid_security;
+
+	struct device_node *ahb;
+
+	int		num_as;
+	struct smmu_as	as[0];		/* Run-time allocated array */
 };
 
 static struct smmu_device *smmu_handle; /* unique for a system */
 
 /*
- *	SMMU/AHB register accessors
+ *	SMMU register accessors
  */
 static inline u32 smmu_read(struct smmu_device *smmu, size_t offs)
 {
-	return readl(smmu->regs + offs);
-}
-static inline void smmu_write(struct smmu_device *smmu, u32 val, size_t offs)
-{
-	writel(val, smmu->regs + offs);
+	BUG_ON(offs < 0x10);
+	if (offs < 0x3c)
+		return readl(smmu->regs[0] + offs - 0x10);
+	BUG_ON(offs < 0x1f0);
+	if (offs < 0x200)
+		return readl(smmu->regs[1] + offs - 0x1f0);
+	BUG_ON(offs < 0x228);
+	if (offs < 0x284)
+		return readl(smmu->regs[2] + offs - 0x228);
+	BUG();
 }
 
-static inline u32 ahb_read(struct smmu_device *smmu, size_t offs)
-{
-	return readl(smmu->regs_ahbarb + offs);
-}
-static inline void ahb_write(struct smmu_device *smmu, u32 val, size_t offs)
+static inline void smmu_write(struct smmu_device *smmu, u32 val, size_t offs)
 {
-	writel(val, smmu->regs_ahbarb + offs);
+	BUG_ON(offs < 0x10);
+	if (offs < 0x3c) {
+		writel(val, smmu->regs[0] + offs - 0x10);
+		return;
+	}
+	BUG_ON(offs < 0x1f0);
+	if (offs < 0x200) {
+		writel(val, smmu->regs[1] + offs - 0x1f0);
+		return;
+	}
+	BUG_ON(offs < 0x228);
+	if (offs < 0x284) {
+		writel(val, smmu->regs[2] + offs - 0x228);
+		return;
+	}
+	BUG();
 }
 
 #define VA_PAGE_TO_PA(va, page)	\
@@ -370,7 +389,7 @@ static void smmu_flush_regs(struct smmu_device *smmu, int enable)
 	FLUSH_SMMU_REGS(smmu);
 }
 
-static void smmu_setup_regs(struct smmu_device *smmu)
+static int smmu_setup_regs(struct smmu_device *smmu)
 {
 	int i;
 	u32 val;
@@ -398,10 +417,7 @@ static void smmu_setup_regs(struct smmu_device *smmu)
 
 	smmu_flush_regs(smmu, 1);
 
-	val = ahb_read(smmu, AHB_XBAR_CTRL);
-	val |= AHB_XBAR_CTRL_SMMU_INIT_DONE_DONE <<
-		AHB_XBAR_CTRL_SMMU_INIT_DONE_SHIFT;
-	ahb_write(smmu, val, AHB_XBAR_CTRL);
+	return tegra_ahb_enable_smmu(smmu->ahb);
 }
 
 static void flush_ptc_and_tlb(struct smmu_device *smmu,
@@ -537,33 +553,42 @@ static inline void put_signature(struct smmu_as *as,
 #endif
 
 /*
- * Caller must lock/unlock as
+ * Caller must not hold as->lock
  */
 static int alloc_pdir(struct smmu_as *as)
 {
-	unsigned long *pdir;
-	int pdn;
+	unsigned long *pdir, flags;
+	int pdn, err = 0;
 	u32 val;
 	struct smmu_device *smmu = as->smmu;
+	struct page *page;
+	unsigned int *cnt;
+
+	/*
+	 * do the allocation, then grab as->lock
+	 */
+	cnt = devm_kzalloc(smmu->dev,
+			   sizeof(cnt[0]) * SMMU_PDIR_COUNT,
+			   GFP_KERNEL);
+	page = alloc_page(GFP_KERNEL | __GFP_DMA);
 
-	if (as->pdir_page)
-		return 0;
+	spin_lock_irqsave(&as->lock, flags);
 
-	as->pte_count = devm_kzalloc(smmu->dev,
-		     sizeof(as->pte_count[0]) * SMMU_PDIR_COUNT, GFP_ATOMIC);
-	if (!as->pte_count) {
-		dev_err(smmu->dev,
-			"failed to allocate smmu_device PTE cunters\n");
-		return -ENOMEM;
+	if (as->pdir_page) {
+		/* We raced, free the redundant */
+		err = -EAGAIN;
+		goto err_out;
 	}
-	as->pdir_page = alloc_page(GFP_ATOMIC | __GFP_DMA);
-	if (!as->pdir_page) {
-		dev_err(smmu->dev,
-			"failed to allocate smmu_device page directory\n");
-		devm_kfree(smmu->dev, as->pte_count);
-		as->pte_count = NULL;
-		return -ENOMEM;
+
+	if (!page || !cnt) {
+		dev_err(smmu->dev, "failed to allocate at %s\n", __func__);
+		err = -ENOMEM;
+		goto err_out;
 	}
+
+	as->pdir_page = page;
+	as->pte_count = cnt;
+
 	SetPageReserved(as->pdir_page);
 	pdir = page_address(as->pdir_page);
 
@@ -579,7 +604,17 @@ static int alloc_pdir(struct smmu_as *as)
 	smmu_write(smmu, val, SMMU_TLB_FLUSH);
 	FLUSH_SMMU_REGS(as->smmu);
 
+	spin_unlock_irqrestore(&as->lock, flags);
+
 	return 0;
+
+err_out:
+	spin_unlock_irqrestore(&as->lock, flags);
+
+	devm_kfree(smmu->dev, cnt);
+	if (page)
+		__free_page(page);
+	return err;
 }
 
 static void __smmu_iommu_unmap(struct smmu_as *as, dma_addr_t iova)
@@ -771,30 +806,28 @@ out:
 
 static int smmu_iommu_domain_init(struct iommu_domain *domain)
 {
-	int i;
+	int i, err = -ENODEV;
 	unsigned long flags;
 	struct smmu_as *as;
 	struct smmu_device *smmu = smmu_handle;
 
 	/* Look for a free AS with lock held */
 	for  (i = 0; i < smmu->num_as; i++) {
-		struct smmu_as *tmp = &smmu->as[i];
-
-		spin_lock_irqsave(&tmp->lock, flags);
-		if (!tmp->pdir_page) {
-			as = tmp;
-			goto found;
+		as = &smmu->as[i];
+		if (!as->pdir_page) {
+			err = alloc_pdir(as);
+			if (!err)
+				goto found;
 		}
-		spin_unlock_irqrestore(&tmp->lock, flags);
+		if (err != -EAGAIN)
+			break;
 	}
-	dev_err(smmu->dev, "no free AS\n");
-	return -ENODEV;
+	if (i == smmu->num_as)
+		dev_err(smmu->dev,  "no free AS\n");
+	return err;
 
 found:
-	if (alloc_pdir(as) < 0)
-		goto err_alloc_pdir;
-
-	spin_lock(&smmu->lock);
+	spin_lock_irqsave(&smmu->lock, flags);
 
 	/* Update PDIR register */
 	smmu_write(smmu, SMMU_PTB_ASID_CUR(as->asid), SMMU_PTB_ASID);
@@ -802,17 +835,18 @@ found:
 		   SMMU_MK_PDIR(as->pdir_page, as->pdir_attr), SMMU_PTB_DATA);
 	FLUSH_SMMU_REGS(smmu);
 
-	spin_unlock(&smmu->lock);
+	spin_unlock_irqrestore(&smmu->lock, flags);
 
-	spin_unlock_irqrestore(&as->lock, flags);
 	domain->priv = as;
 
+	domain->geometry.aperture_start = smmu->iovmm_base;
+	domain->geometry.aperture_end   = smmu->iovmm_base +
+		smmu->page_count * SMMU_PAGE_SIZE - 1;
+	domain->geometry.force_aperture = true;
+
 	dev_dbg(smmu->dev, "smmu_as@%p\n", as);
-	return 0;
 
-err_alloc_pdir:
-	spin_unlock_irqrestore(&as->lock, flags);
-	return -ENODEV;
+	return 0;
 }
 
 static void smmu_iommu_domain_destroy(struct iommu_domain *domain)
@@ -873,65 +907,73 @@ static int tegra_smmu_resume(struct device *dev)
 {
 	struct smmu_device *smmu = dev_get_drvdata(dev);
 	unsigned long flags;
+	int err;
 
 	spin_lock_irqsave(&smmu->lock, flags);
-	smmu_setup_regs(smmu);
+	err = smmu_setup_regs(smmu);
 	spin_unlock_irqrestore(&smmu->lock, flags);
-	return 0;
+	return err;
 }
 
 static int tegra_smmu_probe(struct platform_device *pdev)
 {
 	struct smmu_device *smmu;
-	struct resource *regs, *regs2, *window;
 	struct device *dev = &pdev->dev;
-	int i, err = 0;
+	int i, asids, err = 0;
+	dma_addr_t uninitialized_var(base);
+	size_t bytes, uninitialized_var(size);
 
 	if (smmu_handle)
 		return -EIO;
 
 	BUILD_BUG_ON(PAGE_SHIFT != SMMU_PAGE_SHIFT);
 
-	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	regs2 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	window = platform_get_resource(pdev, IORESOURCE_MEM, 2);
-	if (!regs || !regs2 || !window) {
-		dev_err(dev, "No SMMU resources\n");
+	if (of_property_read_u32(dev->of_node, "nvidia,#asids", &asids))
 		return -ENODEV;
-	}
 
-	smmu = devm_kzalloc(dev, sizeof(*smmu), GFP_KERNEL);
+	bytes = sizeof(*smmu) + asids * sizeof(*smmu->as);
+	smmu = devm_kzalloc(dev, bytes, GFP_KERNEL);
 	if (!smmu) {
 		dev_err(dev, "failed to allocate smmu_device\n");
 		return -ENOMEM;
 	}
 
-	smmu->dev = dev;
-	smmu->num_as = SMMU_NUM_ASIDS;
-	smmu->iovmm_base = (unsigned long)window->start;
-	smmu->page_count = resource_size(window) >> SMMU_PAGE_SHIFT;
-	smmu->regs = devm_ioremap(dev, regs->start, resource_size(regs));
-	smmu->regs_ahbarb = devm_ioremap(dev, regs2->start,
-					 resource_size(regs2));
-	if (!smmu->regs || !smmu->regs_ahbarb) {
-		dev_err(dev, "failed to remap SMMU registers\n");
-		err = -ENXIO;
-		goto fail;
+	for (i = 0; i < ARRAY_SIZE(smmu->regs); i++) {
+		struct resource *res;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res)
+			return -ENODEV;
+		smmu->regs[i] = devm_request_and_ioremap(&pdev->dev, res);
+		if (!smmu->regs[i])
+			return -EBUSY;
 	}
 
+	err = of_get_dma_window(dev->of_node, NULL, 0, NULL, &base, &size);
+	if (err)
+		return -ENODEV;
+
+	if (size & SMMU_PAGE_MASK)
+		return -EINVAL;
+
+	size >>= SMMU_PAGE_SHIFT;
+	if (!size)
+		return -EINVAL;
+
+	smmu->ahb = of_parse_phandle(dev->of_node, "nvidia,ahb", 0);
+	if (!smmu->ahb)
+		return -ENODEV;
+
+	smmu->dev = dev;
+	smmu->num_as = asids;
+	smmu->iovmm_base = base;
+	smmu->page_count = size;
+
 	smmu->translation_enable_0 = ~0;
 	smmu->translation_enable_1 = ~0;
 	smmu->translation_enable_2 = ~0;
 	smmu->asid_security = 0;
 
-	smmu->as = devm_kzalloc(dev,
-			sizeof(smmu->as[0]) * smmu->num_as, GFP_KERNEL);
-	if (!smmu->as) {
-		dev_err(dev, "failed to allocate smmu_as\n");
-		err = -ENOMEM;
-		goto fail;
-	}
-
 	for (i = 0; i < smmu->num_as; i++) {
 		struct smmu_as *as = &smmu->as[i];
 
@@ -945,57 +987,28 @@ static int tegra_smmu_probe(struct platform_device *pdev)
 		INIT_LIST_HEAD(&as->client);
 	}
 	spin_lock_init(&smmu->lock);
-	smmu_setup_regs(smmu);
+	err = smmu_setup_regs(smmu);
+	if (err)
+		return err;
 	platform_set_drvdata(pdev, smmu);
 
 	smmu->avp_vector_page = alloc_page(GFP_KERNEL);
 	if (!smmu->avp_vector_page)
-		goto fail;
+		return -ENOMEM;
 
 	smmu_handle = smmu;
 	return 0;
-
-fail:
-	if (smmu->avp_vector_page)
-		__free_page(smmu->avp_vector_page);
-	if (smmu->regs)
-		devm_iounmap(dev, smmu->regs);
-	if (smmu->regs_ahbarb)
-		devm_iounmap(dev, smmu->regs_ahbarb);
-	if (smmu && smmu->as) {
-		for (i = 0; i < smmu->num_as; i++) {
-			if (smmu->as[i].pdir_page) {
-				ClearPageReserved(smmu->as[i].pdir_page);
-				__free_page(smmu->as[i].pdir_page);
-			}
-		}
-		devm_kfree(dev, smmu->as);
-	}
-	devm_kfree(dev, smmu);
-	return err;
 }
 
 static int tegra_smmu_remove(struct platform_device *pdev)
 {
 	struct smmu_device *smmu = platform_get_drvdata(pdev);
-	struct device *dev = smmu->dev;
+	int i;
 
 	smmu_write(smmu, SMMU_CONFIG_DISABLE, SMMU_CONFIG);
-	platform_set_drvdata(pdev, NULL);
-	if (smmu->as) {
-		int i;
-
-		for (i = 0; i < smmu->num_as; i++)
-			free_pdir(&smmu->as[i]);
-		devm_kfree(dev, smmu->as);
-	}
-	if (smmu->avp_vector_page)
-		__free_page(smmu->avp_vector_page);
-	if (smmu->regs)
-		devm_iounmap(dev, smmu->regs);
-	if (smmu->regs_ahbarb)
-		devm_iounmap(dev, smmu->regs_ahbarb);
-	devm_kfree(dev, smmu);
+	for (i = 0; i < smmu->num_as; i++)
+		free_pdir(&smmu->as[i]);
+	__free_page(smmu->avp_vector_page);
 	smmu_handle = NULL;
 	return 0;
 }
@@ -1005,6 +1018,14 @@ const struct dev_pm_ops tegra_smmu_pm_ops = {
 	.resume		= tegra_smmu_resume,
 };
 
+#ifdef CONFIG_OF
+static struct of_device_id tegra_smmu_of_match[] __devinitdata = {
+	{ .compatible = "nvidia,tegra30-smmu", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tegra_smmu_of_match);
+#endif
+
 static struct platform_driver tegra_smmu_driver = {
 	.probe		= tegra_smmu_probe,
 	.remove		= tegra_smmu_remove,
@@ -1012,6 +1033,7 @@ static struct platform_driver tegra_smmu_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "tegra-smmu",
 		.pm	= &tegra_smmu_pm_ops,
+		.of_match_table = of_match_ptr(tegra_smmu_of_match),
 	},
 };
 
@@ -1031,4 +1053,5 @@ module_exit(tegra_smmu_exit);
 
 MODULE_DESCRIPTION("IOMMU API for SMMU in Tegra30");
 MODULE_AUTHOR("Hiroshi DOYU <hdoyu@nvidia.com>");
+MODULE_ALIAS("platform:tegra-smmu");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/isdn/gigaset/capi.c b/drivers/isdn/gigaset/capi.c
index 27e4a3e21d6..68452b768da 100644
--- a/drivers/isdn/gigaset/capi.c
+++ b/drivers/isdn/gigaset/capi.c
@@ -288,6 +288,7 @@ static inline void dump_rawmsg(enum debuglevel level, const char *tag,
  * format CAPI IE as string
  */
 
+#ifdef CONFIG_GIGASET_DEBUG
 static const char *format_ie(const char *ie)
 {
 	static char result[3 * MAX_FMT_IE_LEN];
@@ -313,6 +314,7 @@ static const char *format_ie(const char *ie)
 	*--pout = 0;
 	return result;
 }
+#endif
 
 /*
  * emit DATA_B3_CONF message
diff --git a/drivers/isdn/hardware/mISDN/avmfritz.c b/drivers/isdn/hardware/mISDN/avmfritz.c
index c08fc605e56..fa6ca473372 100644
--- a/drivers/isdn/hardware/mISDN/avmfritz.c
+++ b/drivers/isdn/hardware/mISDN/avmfritz.c
@@ -449,7 +449,8 @@ hdlc_fill_fifo(struct bchannel *bch)
 {
 	struct fritzcard *fc = bch->hw;
 	struct hdlc_hw *hdlc;
-	int count, fs, cnt = 0, idx, fillempty = 0;
+	int count, fs, cnt = 0, idx;
+	bool fillempty = false;
 	u8 *p;
 	u32 *ptr, val, addr;
 
@@ -462,7 +463,7 @@ hdlc_fill_fifo(struct bchannel *bch)
 			return;
 		count = fs;
 		p = bch->fill;
-		fillempty = 1;
+		fillempty = true;
 	} else {
 		count = bch->tx_skb->len - bch->tx_idx;
 		if (count <= 0)
@@ -477,7 +478,7 @@ hdlc_fill_fifo(struct bchannel *bch)
 			hdlc->ctrl.sr.cmd |= HDLC_CMD_XME;
 	}
 	ptr = (u32 *)p;
-	if (fillempty) {
+	if (!fillempty) {
 		pr_debug("%s.B%d: %d/%d/%d", fc->name, bch->nr, count,
 			 bch->tx_idx, bch->tx_skb->len);
 		bch->tx_idx += count;
diff --git a/drivers/isdn/hardware/mISDN/hfcsusb.c b/drivers/isdn/hardware/mISDN/hfcsusb.c
index c65c3440cd7..114f3bcba1b 100644
--- a/drivers/isdn/hardware/mISDN/hfcsusb.c
+++ b/drivers/isdn/hardware/mISDN/hfcsusb.c
@@ -2084,13 +2084,21 @@ hfcsusb_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	/* create the control pipes needed for register access */
 	hw->ctrl_in_pipe = usb_rcvctrlpipe(hw->dev, 0);
 	hw->ctrl_out_pipe = usb_sndctrlpipe(hw->dev, 0);
+
+	driver_info = (struct hfcsusb_vdata *)
+		      hfcsusb_idtab[vend_idx].driver_info;
+
 	hw->ctrl_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!hw->ctrl_urb) {
+		pr_warn("%s: No memory for control urb\n",
+			driver_info->vend_name);
+		kfree(hw);
+		return -ENOMEM;
+	}
 
-	driver_info =
-		(struct hfcsusb_vdata *)hfcsusb_idtab[vend_idx].driver_info;
-	printk(KERN_DEBUG "%s: %s: detected \"%s\" (%s, if=%d alt=%d)\n",
-	       hw->name, __func__, driver_info->vend_name,
-	       conf_str[small_match], ifnum, alt_used);
+	pr_info("%s: %s: detected \"%s\" (%s, if=%d alt=%d)\n",
+		hw->name, __func__, driver_info->vend_name,
+		conf_str[small_match], ifnum, alt_used);
 
 	if (setup_instance(hw, dev->dev.parent))
 		return -EIO;
diff --git a/drivers/isdn/hisax/hfc_usb.c b/drivers/isdn/hisax/hfc_usb.c
index 84f9c810307..849a8075268 100644
--- a/drivers/isdn/hisax/hfc_usb.c
+++ b/drivers/isdn/hisax/hfc_usb.c
@@ -1483,13 +1483,21 @@ hfc_usb_probe(struct usb_interface *intf, const struct usb_device_id *id)
 				usb_rcvctrlpipe(context->dev, 0);
 			context->ctrl_out_pipe =
 				usb_sndctrlpipe(context->dev, 0);
+
+			driver_info = (hfcsusb_vdata *)
+				      hfcusb_idtab[vend_idx].driver_info;
+
 			context->ctrl_urb = usb_alloc_urb(0, GFP_KERNEL);
 
-			driver_info =
-				(hfcsusb_vdata *) hfcusb_idtab[vend_idx].
-				driver_info;
-			printk(KERN_INFO "HFC-S USB: detected \"%s\"\n",
-			       driver_info->vend_name);
+			if (!context->ctrl_urb) {
+				pr_warn("%s: No memory for control urb\n",
+					driver_info->vend_name);
+				kfree(context);
+				return -ENOMEM;
+			}
+
+			pr_info("HFC-S USB: detected \"%s\"\n",
+				driver_info->vend_name);
 
 			DBG(HFCUSB_DBG_INIT,
 			    "HFC-S USB: Endpoint-Config: %s (if=%d alt=%d), E-Channel(%d)",
diff --git a/drivers/isdn/hisax/isurf.c b/drivers/isdn/hisax/isurf.c
index ea271721529..c1530fe248c 100644
--- a/drivers/isdn/hisax/isurf.c
+++ b/drivers/isdn/hisax/isurf.c
@@ -231,6 +231,11 @@ setup_isurf(struct IsdnCard *card)
 				}
 				pnp_disable_dev(pnp_d);
 				err = pnp_activate_dev(pnp_d);
+				if (err < 0) {
+					pr_warn("%s: pnp_activate_dev ret=%d\n",
+						__func__, err);
+					return 0;
+				}
 				cs->hw.isurf.reset = pnp_port_start(pnp_d, 0);
 				cs->hw.isurf.phymem = pnp_mem_start(pnp_d, 1);
 				cs->irq = pnp_irq(pnp_d, 0);
diff --git a/drivers/isdn/mISDN/stack.c b/drivers/isdn/mISDN/stack.c
index 1a0ae4445ff..5f21f629b7a 100644
--- a/drivers/isdn/mISDN/stack.c
+++ b/drivers/isdn/mISDN/stack.c
@@ -135,8 +135,8 @@ send_layer2(struct mISDNstack *st, struct sk_buff *skb)
 			skb = NULL;
 		else if (*debug & DEBUG_SEND_ERR)
 			printk(KERN_DEBUG
-			       "%s ch%d mgr prim(%x) addr(%x) err %d\n",
-			       __func__, ch->nr, hh->prim, ch->addr, ret);
+			       "%s mgr prim(%x) err %d\n",
+			       __func__, hh->prim, ret);
 	}
 out:
 	mutex_unlock(&st->lmutex);
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 12b2b55c519..c96bbaadeeb 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -200,6 +200,13 @@ config LEDS_LP5523
 	  Driver provides direct control via LED class and interface for
 	  programming the engines.
 
+config LEDS_LP8788
+	tristate "LED support for the TI LP8788 PMIC"
+	depends on LEDS_CLASS
+	depends on MFD_LP8788
+	help
+	  This option enables support for the Keyboard LEDs on the LP8788 PMIC.
+
 config LEDS_CLEVO_MAIL
 	tristate "Mail LED on Clevo notebook"
 	depends on LEDS_CLASS
@@ -415,6 +422,14 @@ config LEDS_MAX8997
 	  This option enables support for on-chip LED drivers on
 	  MAXIM MAX8997 PMIC.
 
+config LEDS_LM3556
+	tristate "LED support for LM3556 Chip"
+	depends on LEDS_CLASS && I2C
+	select REGMAP_I2C
+	help
+	  This option enables support for LEDs connected to LM3556.
+	  LM3556 includes Torch, Flash and Indicator functions.
+
 config LEDS_OT200
 	tristate "LED support for the Bachmann OT200"
 	depends on LEDS_CLASS && HAS_IOMEM
@@ -422,6 +437,14 @@ config LEDS_OT200
 	  This option enables support for the LEDs on the Bachmann OT200.
 	  Say Y to enable LEDs on the Bachmann OT200.
 
+config LEDS_BLINKM
+	tristate "LED support for the BlinkM I2C RGB LED"
+	depends on LEDS_CLASS
+	depends on I2C
+	help
+	  This option enables support for the BlinkM RGB LED connected
+	  through I2C. Say Y to enable support for the BlinkM LED.
+
 config LEDS_TRIGGERS
 	bool "LED Trigger support"
 	depends on LEDS_CLASS
@@ -443,6 +466,20 @@ config LEDS_TRIGGER_TIMER
 
 	  If unsure, say Y.
 
+config LEDS_TRIGGER_ONESHOT
+	tristate "LED One-shot Trigger"
+	depends on LEDS_TRIGGERS
+	help
+	  This allows LEDs to blink in one-shot pulses with parameters
+	  controlled via sysfs.  It's useful to notify the user on
+	  sporadic events, when there are no clear begin and end trap points,
+	  or on dense events, where this blinks the LED at constant rate if
+	  rearmed continuously.
+
+	  It also shows how to use the led_blink_set_oneshot() function.
+
+	  If unsure, say Y.
+
 config LEDS_TRIGGER_IDE_DISK
 	bool "LED IDE Disk Trigger"
 	depends on IDE_GD_ATA
@@ -497,7 +534,7 @@ config LEDS_TRIGGER_TRANSIENT
 	depends on LEDS_TRIGGERS
 	help
 	  This allows one time activation of a transient state on
-	  GPIO/PWM based hadrware.
+	  GPIO/PWM based hardware.
 	  If unsure, say Y.
 
 endif # NEW_LEDS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index f8958cd6cf6..a4429a9217b 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_LEDS_GPIO)			+= leds-gpio.o
 obj-$(CONFIG_LEDS_LP3944)		+= leds-lp3944.o
 obj-$(CONFIG_LEDS_LP5521)		+= leds-lp5521.o
 obj-$(CONFIG_LEDS_LP5523)		+= leds-lp5523.o
+obj-$(CONFIG_LEDS_LP8788)		+= leds-lp8788.o
 obj-$(CONFIG_LEDS_TCA6507)		+= leds-tca6507.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
 obj-$(CONFIG_LEDS_HP6XX)		+= leds-hp6xx.o
@@ -47,12 +48,15 @@ obj-$(CONFIG_LEDS_NETXBIG)		+= leds-netxbig.o
 obj-$(CONFIG_LEDS_ASIC3)		+= leds-asic3.o
 obj-$(CONFIG_LEDS_RENESAS_TPU)		+= leds-renesas-tpu.o
 obj-$(CONFIG_LEDS_MAX8997)		+= leds-max8997.o
+obj-$(CONFIG_LEDS_LM3556)		+= leds-lm3556.o
+obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
 
 # LED Triggers
 obj-$(CONFIG_LEDS_TRIGGER_TIMER)	+= ledtrig-timer.o
+obj-$(CONFIG_LEDS_TRIGGER_ONESHOT)	+= ledtrig-oneshot.o
 obj-$(CONFIG_LEDS_TRIGGER_IDE_DISK)	+= ledtrig-ide-disk.o
 obj-$(CONFIG_LEDS_TRIGGER_HEARTBEAT)	+= ledtrig-heartbeat.o
 obj-$(CONFIG_LEDS_TRIGGER_BACKLIGHT)	+= ledtrig-backlight.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index e663e6f413e..c599095bc00 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -53,7 +53,7 @@ static ssize_t led_brightness_store(struct device *dev,
 
 	if (state == LED_OFF)
 		led_trigger_remove(led_cdev);
-	led_set_brightness(led_cdev, state);
+	__led_set_brightness(led_cdev, state);
 
 	return size;
 }
@@ -82,7 +82,12 @@ static void led_timer_function(unsigned long data)
 	unsigned long delay;
 
 	if (!led_cdev->blink_delay_on || !led_cdev->blink_delay_off) {
-		led_set_brightness(led_cdev, LED_OFF);
+		__led_set_brightness(led_cdev, LED_OFF);
+		return;
+	}
+
+	if (led_cdev->flags & LED_BLINK_ONESHOT_STOP) {
+		led_cdev->flags &= ~LED_BLINK_ONESHOT_STOP;
 		return;
 	}
 
@@ -100,7 +105,21 @@ static void led_timer_function(unsigned long data)
 		delay = led_cdev->blink_delay_off;
 	}
 
-	led_set_brightness(led_cdev, brightness);
+	__led_set_brightness(led_cdev, brightness);
+
+	/* Return in next iteration if led is in one-shot mode and we are in
+	 * the final blink state so that the led is toggled each delay_on +
+	 * delay_off milliseconds in worst case.
+	 */
+	if (led_cdev->flags & LED_BLINK_ONESHOT) {
+		if (led_cdev->flags & LED_BLINK_INVERT) {
+			if (brightness)
+				led_cdev->flags |= LED_BLINK_ONESHOT_STOP;
+		} else {
+			if (!brightness)
+				led_cdev->flags |= LED_BLINK_ONESHOT_STOP;
+		}
+	}
 
 	mod_timer(&led_cdev->blink_timer, jiffies + msecs_to_jiffies(delay));
 }
@@ -203,7 +222,7 @@ void led_classdev_unregister(struct led_classdev *led_cdev)
 #endif
 
 	/* Stop blinking */
-	led_brightness_set(led_cdev, LED_OFF);
+	led_set_brightness(led_cdev, LED_OFF);
 
 	device_unregister(led_cdev->dev);
 
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index d65353d8d3f..2ab05af3de3 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -24,14 +24,6 @@ EXPORT_SYMBOL_GPL(leds_list_lock);
 LIST_HEAD(leds_list);
 EXPORT_SYMBOL_GPL(leds_list);
 
-static void led_stop_software_blink(struct led_classdev *led_cdev)
-{
-	/* deactivate previous settings */
-	del_timer_sync(&led_cdev->blink_timer);
-	led_cdev->blink_delay_on = 0;
-	led_cdev->blink_delay_off = 0;
-}
-
 static void led_set_software_blink(struct led_classdev *led_cdev,
 				   unsigned long delay_on,
 				   unsigned long delay_off)
@@ -53,7 +45,7 @@ static void led_set_software_blink(struct led_classdev *led_cdev,
 
 	/* never off - just set to brightness */
 	if (!delay_off) {
-		led_set_brightness(led_cdev, led_cdev->blink_brightness);
+		__led_set_brightness(led_cdev, led_cdev->blink_brightness);
 		return;
 	}
 
@@ -61,13 +53,12 @@ static void led_set_software_blink(struct led_classdev *led_cdev,
 }
 
 
-void led_blink_set(struct led_classdev *led_cdev,
-		   unsigned long *delay_on,
-		   unsigned long *delay_off)
+static void led_blink_setup(struct led_classdev *led_cdev,
+		     unsigned long *delay_on,
+		     unsigned long *delay_off)
 {
-	del_timer_sync(&led_cdev->blink_timer);
-
-	if (led_cdev->blink_set &&
+	if (!(led_cdev->flags & LED_BLINK_ONESHOT) &&
+	    led_cdev->blink_set &&
 	    !led_cdev->blink_set(led_cdev, delay_on, delay_off))
 		return;
 
@@ -77,12 +68,49 @@ void led_blink_set(struct led_classdev *led_cdev,
 
 	led_set_software_blink(led_cdev, *delay_on, *delay_off);
 }
+
+void led_blink_set(struct led_classdev *led_cdev,
+		   unsigned long *delay_on,
+		   unsigned long *delay_off)
+{
+	del_timer_sync(&led_cdev->blink_timer);
+
+	led_cdev->flags &= ~LED_BLINK_ONESHOT;
+	led_cdev->flags &= ~LED_BLINK_ONESHOT_STOP;
+
+	led_blink_setup(led_cdev, delay_on, delay_off);
+}
 EXPORT_SYMBOL(led_blink_set);
 
-void led_brightness_set(struct led_classdev *led_cdev,
+void led_blink_set_oneshot(struct led_classdev *led_cdev,
+			   unsigned long *delay_on,
+			   unsigned long *delay_off,
+			   int invert)
+{
+	if ((led_cdev->flags & LED_BLINK_ONESHOT) &&
+	     timer_pending(&led_cdev->blink_timer))
+		return;
+
+	led_cdev->flags |= LED_BLINK_ONESHOT;
+	led_cdev->flags &= ~LED_BLINK_ONESHOT_STOP;
+
+	if (invert)
+		led_cdev->flags |= LED_BLINK_INVERT;
+	else
+		led_cdev->flags &= ~LED_BLINK_INVERT;
+
+	led_blink_setup(led_cdev, delay_on, delay_off);
+}
+EXPORT_SYMBOL(led_blink_set_oneshot);
+
+void led_set_brightness(struct led_classdev *led_cdev,
 			enum led_brightness brightness)
 {
-	led_stop_software_blink(led_cdev);
-	led_cdev->brightness_set(led_cdev, brightness);
+	/* stop and clear soft-blink timer */
+	del_timer_sync(&led_cdev->blink_timer);
+	led_cdev->blink_delay_on = 0;
+	led_cdev->blink_delay_off = 0;
+
+	__led_set_brightness(led_cdev, brightness);
 }
-EXPORT_SYMBOL(led_brightness_set);
+EXPORT_SYMBOL(led_set_brightness);
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index 46b4c766335..6157cbbf411 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -99,7 +99,7 @@ ssize_t led_trigger_show(struct device *dev, struct device_attribute *attr,
 EXPORT_SYMBOL_GPL(led_trigger_show);
 
 /* Caller must ensure led_cdev->trigger_lock held */
-void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trigger)
+void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 {
 	unsigned long flags;
 
@@ -112,15 +112,15 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trigger)
 		if (led_cdev->trigger->deactivate)
 			led_cdev->trigger->deactivate(led_cdev);
 		led_cdev->trigger = NULL;
-		led_brightness_set(led_cdev, LED_OFF);
+		led_set_brightness(led_cdev, LED_OFF);
 	}
-	if (trigger) {
-		write_lock_irqsave(&trigger->leddev_list_lock, flags);
-		list_add_tail(&led_cdev->trig_list, &trigger->led_cdevs);
-		write_unlock_irqrestore(&trigger->leddev_list_lock, flags);
-		led_cdev->trigger = trigger;
-		if (trigger->activate)
-			trigger->activate(led_cdev);
+	if (trig) {
+		write_lock_irqsave(&trig->leddev_list_lock, flags);
+		list_add_tail(&led_cdev->trig_list, &trig->led_cdevs);
+		write_unlock_irqrestore(&trig->leddev_list_lock, flags);
+		led_cdev->trigger = trig;
+		if (trig->activate)
+			trig->activate(led_cdev);
 	}
 }
 EXPORT_SYMBOL_GPL(led_trigger_set);
@@ -153,24 +153,24 @@ EXPORT_SYMBOL_GPL(led_trigger_set_default);
 
 /* LED Trigger Interface */
 
-int led_trigger_register(struct led_trigger *trigger)
+int led_trigger_register(struct led_trigger *trig)
 {
 	struct led_classdev *led_cdev;
-	struct led_trigger *trig;
+	struct led_trigger *_trig;
 
-	rwlock_init(&trigger->leddev_list_lock);
-	INIT_LIST_HEAD(&trigger->led_cdevs);
+	rwlock_init(&trig->leddev_list_lock);
+	INIT_LIST_HEAD(&trig->led_cdevs);
 
 	down_write(&triggers_list_lock);
 	/* Make sure the trigger's name isn't already in use */
-	list_for_each_entry(trig, &trigger_list, next_trig) {
-		if (!strcmp(trig->name, trigger->name)) {
+	list_for_each_entry(_trig, &trigger_list, next_trig) {
+		if (!strcmp(_trig->name, trig->name)) {
 			up_write(&triggers_list_lock);
 			return -EEXIST;
 		}
 	}
 	/* Add to the list of led triggers */
-	list_add_tail(&trigger->next_trig, &trigger_list);
+	list_add_tail(&trig->next_trig, &trigger_list);
 	up_write(&triggers_list_lock);
 
 	/* Register with any LEDs that have this as a default trigger */
@@ -178,8 +178,8 @@ int led_trigger_register(struct led_trigger *trigger)
 	list_for_each_entry(led_cdev, &leds_list, node) {
 		down_write(&led_cdev->trigger_lock);
 		if (!led_cdev->trigger && led_cdev->default_trigger &&
-			    !strcmp(led_cdev->default_trigger, trigger->name))
-			led_trigger_set(led_cdev, trigger);
+			    !strcmp(led_cdev->default_trigger, trig->name))
+			led_trigger_set(led_cdev, trig);
 		up_write(&led_cdev->trigger_lock);
 	}
 	up_read(&leds_list_lock);
@@ -188,20 +188,20 @@ int led_trigger_register(struct led_trigger *trigger)
 }
 EXPORT_SYMBOL_GPL(led_trigger_register);
 
-void led_trigger_unregister(struct led_trigger *trigger)
+void led_trigger_unregister(struct led_trigger *trig)
 {
 	struct led_classdev *led_cdev;
 
 	/* Remove from the list of led triggers */
 	down_write(&triggers_list_lock);
-	list_del(&trigger->next_trig);
+	list_del(&trig->next_trig);
 	up_write(&triggers_list_lock);
 
 	/* Remove anyone actively using this trigger */
 	down_read(&leds_list_lock);
 	list_for_each_entry(led_cdev, &leds_list, node) {
 		down_write(&led_cdev->trigger_lock);
-		if (led_cdev->trigger == trigger)
+		if (led_cdev->trigger == trig)
 			led_trigger_set(led_cdev, NULL);
 		up_write(&led_cdev->trigger_lock);
 	}
@@ -211,58 +211,80 @@ EXPORT_SYMBOL_GPL(led_trigger_unregister);
 
 /* Simple LED Tigger Interface */
 
-void led_trigger_event(struct led_trigger *trigger,
+void led_trigger_event(struct led_trigger *trig,
 			enum led_brightness brightness)
 {
 	struct list_head *entry;
 
-	if (!trigger)
+	if (!trig)
 		return;
 
-	read_lock(&trigger->leddev_list_lock);
-	list_for_each(entry, &trigger->led_cdevs) {
+	read_lock(&trig->leddev_list_lock);
+	list_for_each(entry, &trig->led_cdevs) {
 		struct led_classdev *led_cdev;
 
 		led_cdev = list_entry(entry, struct led_classdev, trig_list);
 		led_set_brightness(led_cdev, brightness);
 	}
-	read_unlock(&trigger->leddev_list_lock);
+	read_unlock(&trig->leddev_list_lock);
 }
 EXPORT_SYMBOL_GPL(led_trigger_event);
 
-void led_trigger_blink(struct led_trigger *trigger,
-		       unsigned long *delay_on,
-		       unsigned long *delay_off)
+static void led_trigger_blink_setup(struct led_trigger *trig,
+			     unsigned long *delay_on,
+			     unsigned long *delay_off,
+			     int oneshot,
+			     int invert)
 {
 	struct list_head *entry;
 
-	if (!trigger)
+	if (!trig)
 		return;
 
-	read_lock(&trigger->leddev_list_lock);
-	list_for_each(entry, &trigger->led_cdevs) {
+	read_lock(&trig->leddev_list_lock);
+	list_for_each(entry, &trig->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);
+		if (oneshot)
+			led_blink_set_oneshot(led_cdev, delay_on, delay_off,
+					      invert);
+		else
+			led_blink_set(led_cdev, delay_on, delay_off);
 	}
-	read_unlock(&trigger->leddev_list_lock);
+	read_unlock(&trig->leddev_list_lock);
+}
+
+void led_trigger_blink(struct led_trigger *trig,
+		       unsigned long *delay_on,
+		       unsigned long *delay_off)
+{
+	led_trigger_blink_setup(trig, delay_on, delay_off, 0, 0);
 }
 EXPORT_SYMBOL_GPL(led_trigger_blink);
 
+void led_trigger_blink_oneshot(struct led_trigger *trig,
+			       unsigned long *delay_on,
+			       unsigned long *delay_off,
+			       int invert)
+{
+	led_trigger_blink_setup(trig, delay_on, delay_off, 1, invert);
+}
+EXPORT_SYMBOL_GPL(led_trigger_blink_oneshot);
+
 void led_trigger_register_simple(const char *name, struct led_trigger **tp)
 {
-	struct led_trigger *trigger;
+	struct led_trigger *trig;
 	int err;
 
-	trigger = kzalloc(sizeof(struct led_trigger), GFP_KERNEL);
+	trig = kzalloc(sizeof(struct led_trigger), GFP_KERNEL);
 
-	if (trigger) {
-		trigger->name = name;
-		err = led_trigger_register(trigger);
+	if (trig) {
+		trig->name = name;
+		err = led_trigger_register(trig);
 		if (err < 0) {
-			kfree(trigger);
-			trigger = NULL;
+			kfree(trig);
+			trig = NULL;
 			printk(KERN_WARNING "LED trigger %s failed to register"
 				" (%d)\n", name, err);
 		}
@@ -270,15 +292,15 @@ void led_trigger_register_simple(const char *name, struct led_trigger **tp)
 		printk(KERN_WARNING "LED trigger %s failed to register"
 			" (no memory)\n", name);
 
-	*tp = trigger;
+	*tp = trig;
 }
 EXPORT_SYMBOL_GPL(led_trigger_register_simple);
 
-void led_trigger_unregister_simple(struct led_trigger *trigger)
+void led_trigger_unregister_simple(struct led_trigger *trig)
 {
-	if (trigger)
-		led_trigger_unregister(trigger);
-	kfree(trigger);
+	if (trig)
+		led_trigger_unregister(trig);
+	kfree(trig);
 }
 EXPORT_SYMBOL_GPL(led_trigger_unregister_simple);
 
diff --git a/drivers/leds/leds-88pm860x.c b/drivers/leds/leds-88pm860x.c
index 5b61aaf7ac0..61897cfeeda 100644
--- a/drivers/leds/leds-88pm860x.c
+++ b/drivers/leds/leds-88pm860x.c
@@ -209,7 +209,7 @@ static int pm860x_led_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	data = kzalloc(sizeof(struct pm860x_led), GFP_KERNEL);
+	data = devm_kzalloc(&pdev->dev, sizeof(struct pm860x_led), GFP_KERNEL);
 	if (data == NULL)
 		return -ENOMEM;
 	strncpy(data->name, res->name, MFD_NAME_SIZE - 1);
@@ -220,7 +220,6 @@ static int pm860x_led_probe(struct platform_device *pdev)
 	data->port = pdata->flags;
 	if (data->port < 0) {
 		dev_err(&pdev->dev, "check device failed\n");
-		kfree(data);
 		return -EINVAL;
 	}
 
@@ -233,13 +232,10 @@ static int pm860x_led_probe(struct platform_device *pdev)
 	ret = led_classdev_register(chip->dev, &data->cdev);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to register LED: %d\n", ret);
-		goto out;
+		return ret;
 	}
 	pm860x_led_set(&data->cdev, 0);
 	return 0;
-out:
-	kfree(data);
-	return ret;
 }
 
 static int pm860x_led_remove(struct platform_device *pdev)
@@ -247,7 +243,6 @@ static int pm860x_led_remove(struct platform_device *pdev)
 	struct pm860x_led *data = platform_get_drvdata(pdev);
 
 	led_classdev_unregister(&data->cdev);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-adp5520.c b/drivers/leds/leds-adp5520.c
index b1400db3f83..aa56a867693 100644
--- a/drivers/leds/leds-adp5520.c
+++ b/drivers/leds/leds-adp5520.c
@@ -119,7 +119,8 @@ static int __devinit adp5520_led_probe(struct platform_device *pdev)
 		return -EFAULT;
 	}
 
-	led = kzalloc(sizeof(*led) * pdata->num_leds, GFP_KERNEL);
+	led = devm_kzalloc(&pdev->dev, sizeof(*led) * pdata->num_leds,
+				GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&pdev->dev, "failed to alloc memory\n");
 		return -ENOMEM;
@@ -129,7 +130,7 @@ static int __devinit adp5520_led_probe(struct platform_device *pdev)
 
 	if (ret) {
 		dev_err(&pdev->dev, "failed to write\n");
-		goto err_free;
+		return ret;
 	}
 
 	for (i = 0; i < pdata->num_leds; ++i) {
@@ -179,8 +180,6 @@ err:
 		}
 	}
 
-err_free:
-	kfree(led);
 	return ret;
 }
 
@@ -200,7 +199,6 @@ static int __devexit adp5520_led_remove(struct platform_device *pdev)
 		cancel_work_sync(&led[i].work);
 	}
 
-	kfree(led);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-asic3.c b/drivers/leds/leds-asic3.c
index 525a9249283..5de74ff90dc 100644
--- a/drivers/leds/leds-asic3.c
+++ b/drivers/leds/leds-asic3.c
@@ -99,12 +99,13 @@ static int __devinit asic3_led_probe(struct platform_device *pdev)
 
 	ret = mfd_cell_enable(pdev);
 	if (ret < 0)
-		goto ret0;
+		return ret;
 
-	led->cdev = kzalloc(sizeof(struct led_classdev), GFP_KERNEL);
+	led->cdev = devm_kzalloc(&pdev->dev, sizeof(struct led_classdev),
+				GFP_KERNEL);
 	if (!led->cdev) {
 		ret = -ENOMEM;
-		goto ret1;
+		goto out;
 	}
 
 	led->cdev->name = led->name;
@@ -115,15 +116,12 @@ static int __devinit asic3_led_probe(struct platform_device *pdev)
 
 	ret = led_classdev_register(&pdev->dev, led->cdev);
 	if (ret < 0)
-		goto ret2;
+		goto out;
 
 	return 0;
 
-ret2:
-	kfree(led->cdev);
-ret1:
+out:
 	(void) mfd_cell_disable(pdev);
-ret0:
 	return ret;
 }
 
@@ -133,8 +131,6 @@ static int __devexit asic3_led_remove(struct platform_device *pdev)
 
 	led_classdev_unregister(led->cdev);
 
-	kfree(led->cdev);
-
 	return mfd_cell_disable(pdev);
 }
 
diff --git a/drivers/leds/leds-atmel-pwm.c b/drivers/leds/leds-atmel-pwm.c
index 64ad702a2ec..45430632faa 100644
--- a/drivers/leds/leds-atmel-pwm.c
+++ b/drivers/leds/leds-atmel-pwm.c
@@ -46,7 +46,8 @@ static int __devinit pwmled_probe(struct platform_device *pdev)
 	if (!pdata || pdata->num_leds < 1)
 		return -ENODEV;
 
-	leds = kcalloc(pdata->num_leds, sizeof(*leds), GFP_KERNEL);
+	leds = devm_kzalloc(&pdev->dev, pdata->num_leds * sizeof(*leds),
+			GFP_KERNEL);
 	if (!leds)
 		return -ENOMEM;
 
@@ -108,7 +109,6 @@ err:
 			pwm_channel_free(&leds[i].pwmc);
 		}
 	}
-	kfree(leds);
 
 	return status;
 }
@@ -129,7 +129,6 @@ static int __exit pwmled_remove(struct platform_device *pdev)
 		pwm_channel_free(&led->pwmc);
 	}
 
-	kfree(leds);
 	platform_set_drvdata(pdev, NULL);
 	return 0;
 }
diff --git a/drivers/leds/leds-bd2802.c b/drivers/leds/leds-bd2802.c
index 591cbdf5a04..89ca6a2a19d 100644
--- a/drivers/leds/leds-bd2802.c
+++ b/drivers/leds/leds-bd2802.c
@@ -677,7 +677,7 @@ static int __devinit bd2802_probe(struct i2c_client *client,
 	struct bd2802_led_platform_data *pdata;
 	int ret, i;
 
-	led = kzalloc(sizeof(struct bd2802_led), GFP_KERNEL);
+	led = devm_kzalloc(&client->dev, sizeof(struct bd2802_led), GFP_KERNEL);
 	if (!led) {
 		dev_err(&client->dev, "failed to allocate driver data\n");
 		return -ENOMEM;
@@ -697,7 +697,7 @@ static int __devinit bd2802_probe(struct i2c_client *client,
 	ret = bd2802_write_byte(client, BD2802_REG_CLKSETUP, 0x00);
 	if (ret < 0) {
 		dev_err(&client->dev, "failed to detect device\n");
-		goto failed_free;
+		return ret;
 	} else
 		dev_info(&client->dev, "return 0x%02x\n", ret);
 
@@ -729,9 +729,6 @@ static int __devinit bd2802_probe(struct i2c_client *client,
 failed_unregister_dev_file:
 	for (i--; i >= 0; i--)
 		device_remove_file(&led->client->dev, bd2802_attributes[i]);
-failed_free:
-	kfree(led);
-
 	return ret;
 }
 
@@ -746,7 +743,6 @@ static int __exit bd2802_remove(struct i2c_client *client)
 		bd2802_disable_adv_conf(led);
 	for (i = 0; i < ARRAY_SIZE(bd2802_attributes); i++)
 		device_remove_file(&led->client->dev, bd2802_attributes[i]);
-	kfree(led);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-blinkm.c b/drivers/leds/leds-blinkm.c
new file mode 100644
index 00000000000..f7c3d7f1ec5
--- /dev/null
+++ b/drivers/leds/leds-blinkm.c
@@ -0,0 +1,815 @@
+/*
+ *  leds-blinkm.c
+ *  (c) Jan-Simon Möller (dl9pf@gmx.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.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/sysfs.h>
+#include <linux/printk.h>
+#include <linux/pm_runtime.h>
+#include <linux/leds.h>
+#include <linux/delay.h>
+
+/* Addresses to scan - BlinkM is on 0x09 by default*/
+static const unsigned short normal_i2c[] = { 0x09, I2C_CLIENT_END };
+
+static int blinkm_transfer_hw(struct i2c_client *client, int cmd);
+static int blinkm_test_run(struct i2c_client *client);
+
+struct blinkm_led {
+	struct i2c_client *i2c_client;
+	struct led_classdev led_cdev;
+	int id;
+	atomic_t active;
+};
+
+struct blinkm_work {
+	struct blinkm_led *blinkm_led;
+	struct work_struct work;
+};
+
+#define cdev_to_blmled(c)          container_of(c, struct blinkm_led, led_cdev)
+#define work_to_blmwork(c)         container_of(c, struct blinkm_work, work)
+
+struct blinkm_data {
+	struct i2c_client *i2c_client;
+	struct mutex update_lock;
+	/* used for led class interface */
+	struct blinkm_led blinkm_leds[3];
+	/* used for "blinkm" sysfs interface */
+	u8 red;			/* color red */
+	u8 green;		/* color green */
+	u8 blue;		/* color blue */
+	/* next values to use for transfer */
+	u8 next_red;			/* color red */
+	u8 next_green;		/* color green */
+	u8 next_blue;		/* color blue */
+	/* internal use */
+	u8 args[7];		/* set of args for transmission */
+	u8 i2c_addr;		/* i2c addr */
+	u8 fw_ver;		/* firmware version */
+	/* used, but not from userspace */
+	u8 hue;			/* HSB  hue */
+	u8 saturation;		/* HSB  saturation */
+	u8 brightness;		/* HSB  brightness */
+	u8 next_hue;			/* HSB  hue */
+	u8 next_saturation;		/* HSB  saturation */
+	u8 next_brightness;		/* HSB  brightness */
+	/* currently unused / todo */
+	u8 fade_speed;		/* fade speed     1 - 255 */
+	s8 time_adjust;		/* time adjust -128 - 127 */
+	u8 fade:1;		/* fade on = 1, off = 0 */
+	u8 rand:1;		/* rand fade mode on = 1 */
+	u8 script_id;		/* script ID */
+	u8 script_repeats;	/* repeats of script */
+	u8 script_startline;	/* line to start */
+};
+
+/* Colors */
+#define RED   0
+#define GREEN 1
+#define BLUE  2
+
+/* mapping command names to cmd chars - see datasheet */
+#define BLM_GO_RGB            0
+#define BLM_FADE_RGB          1
+#define BLM_FADE_HSB          2
+#define BLM_FADE_RAND_RGB     3
+#define BLM_FADE_RAND_HSB     4
+#define BLM_PLAY_SCRIPT       5
+#define BLM_STOP_SCRIPT       6
+#define BLM_SET_FADE_SPEED    7
+#define BLM_SET_TIME_ADJ      8
+#define BLM_GET_CUR_RGB       9
+#define BLM_WRITE_SCRIPT_LINE 10
+#define BLM_READ_SCRIPT_LINE  11
+#define BLM_SET_SCRIPT_LR     12	/* Length & Repeats */
+#define BLM_SET_ADDR          13
+#define BLM_GET_ADDR          14
+#define BLM_GET_FW_VER        15
+#define BLM_SET_STARTUP_PARAM 16
+
+/* BlinkM Commands
+ *  as extracted out of the datasheet:
+ *
+ *  cmdchar = command (ascii)
+ *  cmdbyte = command in hex
+ *  nr_args = number of arguments (to send)
+ *  nr_ret  = number of return values (to read)
+ *  dir = direction (0 = read, 1 = write, 2 = both)
+ *
+ */
+static const struct {
+	char cmdchar;
+	u8 cmdbyte;
+	u8 nr_args;
+	u8 nr_ret;
+	u8 dir:2;
+} blinkm_cmds[17] = {
+  /* cmdchar, cmdbyte, nr_args, nr_ret,  dir */
+	{ 'n', 0x6e, 3, 0, 1},
+	{ 'c', 0x63, 3, 0, 1},
+	{ 'h', 0x68, 3, 0, 1},
+	{ 'C', 0x43, 3, 0, 1},
+	{ 'H', 0x48, 3, 0, 1},
+	{ 'p', 0x70, 3, 0, 1},
+	{ 'o', 0x6f, 0, 0, 1},
+	{ 'f', 0x66, 1, 0, 1},
+	{ 't', 0x74, 1, 0, 1},
+	{ 'g', 0x67, 0, 3, 0},
+	{ 'W', 0x57, 7, 0, 1},
+	{ 'R', 0x52, 2, 5, 2},
+	{ 'L', 0x4c, 3, 0, 1},
+	{ 'A', 0x41, 4, 0, 1},
+	{ 'a', 0x61, 0, 1, 0},
+	{ 'Z', 0x5a, 0, 1, 0},
+	{ 'B', 0x42, 5, 0, 1},
+};
+
+static ssize_t show_color_common(struct device *dev, char *buf, int color)
+{
+	struct i2c_client *client;
+	struct blinkm_data *data;
+	int ret;
+
+	client = to_i2c_client(dev);
+	data = i2c_get_clientdata(client);
+
+	ret = blinkm_transfer_hw(client, BLM_GET_CUR_RGB);
+	if (ret < 0)
+		return ret;
+	switch (color) {
+	case RED:
+		return scnprintf(buf, PAGE_SIZE, "%02X\n", data->red);
+		break;
+	case GREEN:
+		return scnprintf(buf, PAGE_SIZE, "%02X\n", data->green);
+		break;
+	case BLUE:
+		return scnprintf(buf, PAGE_SIZE, "%02X\n", data->blue);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return -EINVAL;
+}
+
+static int store_color_common(struct device *dev, const char *buf, int color)
+{
+	struct i2c_client *client;
+	struct blinkm_data *data;
+	int ret;
+	u8 value;
+
+	client = to_i2c_client(dev);
+	data = i2c_get_clientdata(client);
+
+	ret = kstrtou8(buf, 10, &value);
+	if (ret < 0) {
+		dev_err(dev, "BlinkM: value too large!\n");
+		return ret;
+	}
+
+	switch (color) {
+	case RED:
+		data->next_red = value;
+		break;
+	case GREEN:
+		data->next_green = value;
+		break;
+	case BLUE:
+		data->next_blue = value;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "next_red = %d, next_green = %d, next_blue = %d\n",
+			data->next_red, data->next_green, data->next_blue);
+
+	/* if mode ... */
+	ret = blinkm_transfer_hw(client, BLM_GO_RGB);
+	if (ret < 0) {
+		dev_err(dev, "BlinkM: can't set RGB\n");
+		return ret;
+	}
+	return 0;
+}
+
+static ssize_t show_red(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	return show_color_common(dev, buf, RED);
+}
+
+static ssize_t store_red(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	int ret;
+
+	ret = store_color_common(dev, buf, RED);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
+static DEVICE_ATTR(red, S_IRUGO | S_IWUSR, show_red, store_red);
+
+static ssize_t show_green(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	return show_color_common(dev, buf, GREEN);
+}
+
+static ssize_t store_green(struct device *dev, struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+
+	int ret;
+
+	ret = store_color_common(dev, buf, GREEN);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
+static DEVICE_ATTR(green, S_IRUGO | S_IWUSR, show_green, store_green);
+
+static ssize_t show_blue(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	return show_color_common(dev, buf, BLUE);
+}
+
+static ssize_t store_blue(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	int ret;
+
+	ret = store_color_common(dev, buf, BLUE);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
+static DEVICE_ATTR(blue, S_IRUGO | S_IWUSR, show_blue, store_blue);
+
+static ssize_t show_test(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	return scnprintf(buf, PAGE_SIZE,
+			 "#Write into test to start test sequence!#\n");
+}
+
+static ssize_t store_test(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+
+	struct i2c_client *client;
+	int ret;
+	client = to_i2c_client(dev);
+
+	/*test */
+	ret = blinkm_test_run(client);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+static DEVICE_ATTR(test, S_IRUGO | S_IWUSR, show_test, store_test);
+
+/* TODO: HSB, fade, timeadj, script ... */
+
+static struct attribute *blinkm_attrs[] = {
+	&dev_attr_red.attr,
+	&dev_attr_green.attr,
+	&dev_attr_blue.attr,
+	&dev_attr_test.attr,
+	NULL,
+};
+
+static struct attribute_group blinkm_group = {
+	.name = "blinkm",
+	.attrs = blinkm_attrs,
+};
+
+static int blinkm_write(struct i2c_client *client, int cmd, u8 *arg)
+{
+	int result;
+	int i;
+	int arglen = blinkm_cmds[cmd].nr_args;
+	/* write out cmd to blinkm - always / default step */
+	result = i2c_smbus_write_byte(client, blinkm_cmds[cmd].cmdbyte);
+	if (result < 0)
+		return result;
+	/* no args to write out */
+	if (arglen == 0)
+		return 0;
+
+	for (i = 0; i < arglen; i++) {
+		/* repeat for arglen */
+		result = i2c_smbus_write_byte(client, arg[i]);
+		if (result < 0)
+			return result;
+	}
+	return 0;
+}
+
+static int blinkm_read(struct i2c_client *client, int cmd, u8 *arg)
+{
+	int result;
+	int i;
+	int retlen = blinkm_cmds[cmd].nr_ret;
+	for (i = 0; i < retlen; i++) {
+		/* repeat for retlen */
+		result = i2c_smbus_read_byte(client);
+		if (result < 0)
+			return result;
+		arg[i] = result;
+	}
+
+	return 0;
+}
+
+static int blinkm_transfer_hw(struct i2c_client *client, int cmd)
+{
+	/* the protocol is simple but non-standard:
+	 * e.g.  cmd 'g' (= 0x67) for "get device address"
+	 * - which defaults to 0x09 - would be the sequence:
+	 *   a) write 0x67 to the device (byte write)
+	 *   b) read the value (0x09) back right after (byte read)
+	 *
+	 * Watch out for "unfinished" sequences (i.e. not enough reads
+	 * or writes after a command. It will make the blinkM misbehave.
+	 * Sequence is key here.
+	 */
+
+	/* args / return are in private data struct */
+	struct blinkm_data *data = i2c_get_clientdata(client);
+
+	/* We start hardware transfers which are not to be
+	 * mixed with other commands. Aquire a lock now. */
+	if (mutex_lock_interruptible(&data->update_lock) < 0)
+		return -EAGAIN;
+
+	/* switch cmd - usually write before reads */
+	switch (cmd) {
+	case BLM_FADE_RAND_RGB:
+	case BLM_GO_RGB:
+	case BLM_FADE_RGB:
+		data->args[0] = data->next_red;
+		data->args[1] = data->next_green;
+		data->args[2] = data->next_blue;
+		blinkm_write(client, cmd, data->args);
+		data->red = data->args[0];
+		data->green = data->args[1];
+		data->blue = data->args[2];
+		break;
+	case BLM_FADE_HSB:
+	case BLM_FADE_RAND_HSB:
+		data->args[0] = data->next_hue;
+		data->args[1] = data->next_saturation;
+		data->args[2] = data->next_brightness;
+		blinkm_write(client, cmd, data->args);
+		data->hue = data->next_hue;
+		data->saturation = data->next_saturation;
+		data->brightness = data->next_brightness;
+		break;
+	case BLM_PLAY_SCRIPT:
+		data->args[0] = data->script_id;
+		data->args[1] = data->script_repeats;
+		data->args[2] = data->script_startline;
+		blinkm_write(client, cmd, data->args);
+		break;
+	case BLM_STOP_SCRIPT:
+		blinkm_write(client, cmd, NULL);
+		break;
+	case BLM_GET_CUR_RGB:
+		data->args[0] = data->red;
+		data->args[1] = data->green;
+		data->args[2] = data->blue;
+		blinkm_write(client, cmd, NULL);
+		blinkm_read(client, cmd, data->args);
+		data->red = data->args[0];
+		data->green = data->args[1];
+		data->blue = data->args[2];
+		break;
+	case BLM_GET_ADDR:
+		data->args[0] = data->i2c_addr;
+		blinkm_write(client, cmd, NULL);
+		blinkm_read(client, cmd, data->args);
+		data->i2c_addr = data->args[0];
+		break;
+	case BLM_SET_TIME_ADJ:
+	case BLM_SET_FADE_SPEED:
+	case BLM_READ_SCRIPT_LINE:
+	case BLM_WRITE_SCRIPT_LINE:
+	case BLM_SET_SCRIPT_LR:
+	case BLM_SET_ADDR:
+	case BLM_GET_FW_VER:
+	case BLM_SET_STARTUP_PARAM:
+		dev_err(&client->dev,
+				"BlinkM: cmd %d not implemented yet.\n", cmd);
+		break;
+	default:
+		dev_err(&client->dev, "BlinkM: unknown command %d\n", cmd);
+		mutex_unlock(&data->update_lock);
+		return -EINVAL;
+	}			/* end switch(cmd) */
+
+	/* transfers done, unlock */
+	mutex_unlock(&data->update_lock);
+	return 0;
+}
+
+static void led_work(struct work_struct *work)
+{
+	int ret;
+	struct blinkm_led *led;
+	struct blinkm_data *data ;
+	struct blinkm_work *blm_work = work_to_blmwork(work);
+
+	led = blm_work->blinkm_led;
+	data = i2c_get_clientdata(led->i2c_client);
+	ret = blinkm_transfer_hw(led->i2c_client, BLM_GO_RGB);
+	atomic_dec(&led->active);
+	dev_dbg(&led->i2c_client->dev,
+			"# DONE # next_red = %d, next_green = %d,"
+			" next_blue = %d, active = %d\n",
+			data->next_red, data->next_green,
+			data->next_blue, atomic_read(&led->active));
+	kfree(blm_work);
+}
+
+static int blinkm_led_common_set(struct led_classdev *led_cdev,
+				 enum led_brightness value, int color)
+{
+	/* led_brightness is 0, 127 or 255 - we just use it here as-is */
+	struct blinkm_led *led = cdev_to_blmled(led_cdev);
+	struct blinkm_data *data = i2c_get_clientdata(led->i2c_client);
+	struct blinkm_work *bl_work;
+
+	switch (color) {
+	case RED:
+		/* bail out if there's no change */
+		if (data->next_red == (u8) value)
+			return 0;
+		/* we assume a quite fast sequence here ([off]->on->off)
+		 * think of network led trigger - we cannot blink that fast, so
+		 * in case we already have a off->on->off transition queued up,
+		 * we refuse to queue up more.
+		 * Revisit: fast-changing brightness. */
+		if (atomic_read(&led->active) > 1)
+			return 0;
+		data->next_red = (u8) value;
+		break;
+	case GREEN:
+		/* bail out if there's no change */
+		if (data->next_green == (u8) value)
+			return 0;
+		/* we assume a quite fast sequence here ([off]->on->off)
+		 * Revisit: fast-changing brightness. */
+		if (atomic_read(&led->active) > 1)
+			return 0;
+		data->next_green = (u8) value;
+		break;
+	case BLUE:
+		/* bail out if there's no change */
+		if (data->next_blue == (u8) value)
+			return 0;
+		/* we assume a quite fast sequence here ([off]->on->off)
+		 * Revisit: fast-changing brightness. */
+		if (atomic_read(&led->active) > 1)
+			return 0;
+		data->next_blue = (u8) value;
+		break;
+
+	default:
+		dev_err(&led->i2c_client->dev, "BlinkM: unknown color.\n");
+		return -EINVAL;
+	}
+
+	bl_work = kzalloc(sizeof(*bl_work), GFP_ATOMIC);
+	if (!bl_work)
+		return -ENOMEM;
+
+	atomic_inc(&led->active);
+	dev_dbg(&led->i2c_client->dev,
+			"#TO_SCHED# next_red = %d, next_green = %d,"
+			" next_blue = %d, active = %d\n",
+			data->next_red, data->next_green,
+			data->next_blue, atomic_read(&led->active));
+
+	/* a fresh work _item_ for each change */
+	bl_work->blinkm_led = led;
+	INIT_WORK(&bl_work->work, led_work);
+	/* queue work in own queue for easy sync on exit*/
+	schedule_work(&bl_work->work);
+
+	return 0;
+}
+
+static void blinkm_led_red_set(struct led_classdev *led_cdev,
+			       enum led_brightness value)
+{
+	blinkm_led_common_set(led_cdev, value, RED);
+}
+
+static void blinkm_led_green_set(struct led_classdev *led_cdev,
+				 enum led_brightness value)
+{
+	blinkm_led_common_set(led_cdev, value, GREEN);
+}
+
+static void blinkm_led_blue_set(struct led_classdev *led_cdev,
+				enum led_brightness value)
+{
+	blinkm_led_common_set(led_cdev, value, BLUE);
+}
+
+static void blinkm_init_hw(struct i2c_client *client)
+{
+	int ret;
+	ret = blinkm_transfer_hw(client, BLM_STOP_SCRIPT);
+	ret = blinkm_transfer_hw(client, BLM_GO_RGB);
+}
+
+static int blinkm_test_run(struct i2c_client *client)
+{
+	int ret;
+	struct blinkm_data *data = i2c_get_clientdata(client);
+
+	data->next_red = 0x01;
+	data->next_green = 0x05;
+	data->next_blue = 0x10;
+	ret = blinkm_transfer_hw(client, BLM_GO_RGB);
+	if (ret < 0)
+		return ret;
+	msleep(2000);
+
+	data->next_red = 0x25;
+	data->next_green = 0x10;
+	data->next_blue = 0x31;
+	ret = blinkm_transfer_hw(client, BLM_FADE_RGB);
+	if (ret < 0)
+		return ret;
+	msleep(2000);
+
+	data->next_hue = 0x50;
+	data->next_saturation = 0x10;
+	data->next_brightness = 0x20;
+	ret = blinkm_transfer_hw(client, BLM_FADE_HSB);
+	if (ret < 0)
+		return ret;
+	msleep(2000);
+
+	return 0;
+}
+
+/* Return 0 if detection is successful, -ENODEV otherwise */
+static int blinkm_detect(struct i2c_client *client, struct i2c_board_info *info)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	int ret;
+	int count = 99;
+	u8 tmpargs[7];
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA
+				     | I2C_FUNC_SMBUS_WORD_DATA
+				     | I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -ENODEV;
+
+	/* Now, we do the remaining detection. Simple for now. */
+	/* We might need more guards to protect other i2c slaves */
+
+	/* make sure the blinkM is balanced (read/writes) */
+	while (count > 0) {
+		ret = blinkm_write(client, BLM_GET_ADDR, NULL);
+		usleep_range(5000, 10000);
+		ret = blinkm_read(client, BLM_GET_ADDR, tmpargs);
+		usleep_range(5000, 10000);
+		if (tmpargs[0] == 0x09)
+			count = 0;
+		count--;
+	}
+
+	/* Step 1: Read BlinkM address back  -  cmd_char 'a' */
+	ret = blinkm_write(client, BLM_GET_ADDR, NULL);
+	if (ret < 0)
+		return ret;
+	usleep_range(20000, 30000);	/* allow a small delay */
+	ret = blinkm_read(client, BLM_GET_ADDR, tmpargs);
+	if (ret < 0)
+		return ret;
+
+	if (tmpargs[0] != 0x09) {
+		dev_err(&client->dev, "enodev DEV ADDR = 0x%02X\n", tmpargs[0]);
+		return -ENODEV;
+	}
+
+	strlcpy(info->type, "blinkm", I2C_NAME_SIZE);
+	return 0;
+}
+
+static int __devinit blinkm_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct blinkm_data *data;
+	struct blinkm_led *led[3];
+	int err, i;
+	char blinkm_led_name[28];
+
+	data = devm_kzalloc(&client->dev,
+			sizeof(struct blinkm_data), GFP_KERNEL);
+	if (!data) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	data->i2c_addr = 0x09;
+	data->i2c_addr = 0x08;
+	/* i2c addr  - use fake addr of 0x08 initially (real is 0x09) */
+	data->fw_ver = 0xfe;
+	/* firmware version - use fake until we read real value
+	 * (currently broken - BlinkM confused!) */
+	data->script_id = 0x01;
+	data->i2c_client = client;
+
+	i2c_set_clientdata(client, data);
+	mutex_init(&data->update_lock);
+
+	/* Register sysfs hooks */
+	err = sysfs_create_group(&client->dev.kobj, &blinkm_group);
+	if (err < 0) {
+		dev_err(&client->dev, "couldn't register sysfs group\n");
+		goto exit;
+	}
+
+	for (i = 0; i < 3; i++) {
+		/* RED = 0, GREEN = 1, BLUE = 2 */
+		led[i] = &data->blinkm_leds[i];
+		led[i]->i2c_client = client;
+		led[i]->id = i;
+		led[i]->led_cdev.max_brightness = 255;
+		led[i]->led_cdev.flags = LED_CORE_SUSPENDRESUME;
+		atomic_set(&led[i]->active, 0);
+		switch (i) {
+		case RED:
+			snprintf(blinkm_led_name, sizeof(blinkm_led_name),
+					 "blinkm-%d-%d-red",
+					 client->adapter->nr,
+					 client->addr);
+			led[i]->led_cdev.name = blinkm_led_name;
+			led[i]->led_cdev.brightness_set = blinkm_led_red_set;
+			err = led_classdev_register(&client->dev,
+						    &led[i]->led_cdev);
+			if (err < 0) {
+				dev_err(&client->dev,
+					"couldn't register LED %s\n",
+					led[i]->led_cdev.name);
+				goto failred;
+			}
+			break;
+		case GREEN:
+			snprintf(blinkm_led_name, sizeof(blinkm_led_name),
+					 "blinkm-%d-%d-green",
+					 client->adapter->nr,
+					 client->addr);
+			led[i]->led_cdev.name = blinkm_led_name;
+			led[i]->led_cdev.brightness_set = blinkm_led_green_set;
+			err = led_classdev_register(&client->dev,
+						    &led[i]->led_cdev);
+			if (err < 0) {
+				dev_err(&client->dev,
+					"couldn't register LED %s\n",
+					led[i]->led_cdev.name);
+				goto failgreen;
+			}
+			break;
+		case BLUE:
+			snprintf(blinkm_led_name, sizeof(blinkm_led_name),
+					 "blinkm-%d-%d-blue",
+					 client->adapter->nr,
+					 client->addr);
+			led[i]->led_cdev.name = blinkm_led_name;
+			led[i]->led_cdev.brightness_set = blinkm_led_blue_set;
+			err = led_classdev_register(&client->dev,
+						    &led[i]->led_cdev);
+			if (err < 0) {
+				dev_err(&client->dev,
+					"couldn't register LED %s\n",
+					led[i]->led_cdev.name);
+				goto failblue;
+			}
+			break;
+		}		/* end switch */
+	}			/* end for */
+
+	/* Initialize the blinkm */
+	blinkm_init_hw(client);
+
+	return 0;
+
+failblue:
+	led_classdev_unregister(&led[GREEN]->led_cdev);
+
+failgreen:
+	led_classdev_unregister(&led[RED]->led_cdev);
+
+failred:
+	sysfs_remove_group(&client->dev.kobj, &blinkm_group);
+exit:
+	return err;
+}
+
+static int __devexit blinkm_remove(struct i2c_client *client)
+{
+	struct blinkm_data *data = i2c_get_clientdata(client);
+	int ret = 0;
+	int i;
+
+	/* make sure no workqueue entries are pending */
+	for (i = 0; i < 3; i++) {
+		flush_scheduled_work();
+		led_classdev_unregister(&data->blinkm_leds[i].led_cdev);
+	}
+
+	/* reset rgb */
+	data->next_red = 0x00;
+	data->next_green = 0x00;
+	data->next_blue = 0x00;
+	ret = blinkm_transfer_hw(client, BLM_FADE_RGB);
+	if (ret < 0)
+		dev_err(&client->dev, "Failure in blinkm_remove ignored. Continuing.\n");
+
+	/* reset hsb */
+	data->next_hue = 0x00;
+	data->next_saturation = 0x00;
+	data->next_brightness = 0x00;
+	ret = blinkm_transfer_hw(client, BLM_FADE_HSB);
+	if (ret < 0)
+		dev_err(&client->dev, "Failure in blinkm_remove ignored. Continuing.\n");
+
+	/* red fade to off */
+	data->next_red = 0xff;
+	ret = blinkm_transfer_hw(client, BLM_GO_RGB);
+	if (ret < 0)
+		dev_err(&client->dev, "Failure in blinkm_remove ignored. Continuing.\n");
+
+	/* off */
+	data->next_red = 0x00;
+	ret = blinkm_transfer_hw(client, BLM_FADE_RGB);
+	if (ret < 0)
+		dev_err(&client->dev, "Failure in blinkm_remove ignored. Continuing.\n");
+
+	sysfs_remove_group(&client->dev.kobj, &blinkm_group);
+	return 0;
+}
+
+static const struct i2c_device_id blinkm_id[] = {
+	{"blinkm", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, blinkm_id);
+
+  /* This is the driver that will be inserted */
+static struct i2c_driver blinkm_driver = {
+	.class = I2C_CLASS_HWMON,
+	.driver = {
+		   .name = "blinkm",
+		   },
+	.probe = blinkm_probe,
+	.remove = __devexit_p(blinkm_remove),
+	.id_table = blinkm_id,
+	.detect = blinkm_detect,
+	.address_list = normal_i2c,
+};
+
+module_i2c_driver(blinkm_driver);
+
+MODULE_AUTHOR("Jan-Simon Moeller <dl9pf@gmx.de>");
+MODULE_DESCRIPTION("BlinkM RGB LED driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/leds/leds-da903x.c b/drivers/leds/leds-da903x.c
index d9cd73ebd6c..cc77c9d9261 100644
--- a/drivers/leds/leds-da903x.c
+++ b/drivers/leds/leds-da903x.c
@@ -108,7 +108,7 @@ static int __devinit da903x_led_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	led = kzalloc(sizeof(struct da903x_led), GFP_KERNEL);
+	led = devm_kzalloc(&pdev->dev, sizeof(struct da903x_led), GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&pdev->dev, "failed to alloc memory for LED%d\n", id);
 		return -ENOMEM;
@@ -129,15 +129,11 @@ static int __devinit da903x_led_probe(struct platform_device *pdev)
 	ret = led_classdev_register(led->master, &led->cdev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register LED %d\n", id);
-		goto err;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, led);
 	return 0;
-
-err:
-	kfree(led);
-	return ret;
 }
 
 static int __devexit da903x_led_remove(struct platform_device *pdev)
@@ -145,7 +141,6 @@ static int __devexit da903x_led_remove(struct platform_device *pdev)
 	struct da903x_led *led = platform_get_drvdata(pdev);
 
 	led_classdev_unregister(&led->cdev);
-	kfree(led);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-dac124s085.c b/drivers/leds/leds-dac124s085.c
index d56c14269ff..1f9d8e62d37 100644
--- a/drivers/leds/leds-dac124s085.c
+++ b/drivers/leds/leds-dac124s085.c
@@ -69,7 +69,7 @@ static int dac124s085_probe(struct spi_device *spi)
 	struct dac124s085_led	*led;
 	int i, ret;
 
-	dac = kzalloc(sizeof(*dac), GFP_KERNEL);
+	dac = devm_kzalloc(&spi->dev, sizeof(*dac), GFP_KERNEL);
 	if (!dac)
 		return -ENOMEM;
 
@@ -102,7 +102,6 @@ eledcr:
 		led_classdev_unregister(&dac->leds[i].ldev);
 
 	spi_set_drvdata(spi, NULL);
-	kfree(dac);
 	return ret;
 }
 
@@ -117,7 +116,6 @@ static int dac124s085_remove(struct spi_device *spi)
 	}
 
 	spi_set_drvdata(spi, NULL);
-	kfree(dac);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index f4c470a3bc8..c032b218034 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -178,7 +178,8 @@ static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_dev
 	if (!count)
 		return NULL;
 
-	priv = kzalloc(sizeof_gpio_leds_priv(count), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof_gpio_leds_priv(count),
+			GFP_KERNEL);
 	if (!priv)
 		return NULL;
 
@@ -215,7 +216,6 @@ static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_dev
 err:
 	for (count = priv->num_leds - 2; count >= 0; count--)
 		delete_gpio_led(&priv->leds[count]);
-	kfree(priv);
 	return NULL;
 }
 
@@ -239,8 +239,9 @@ static int __devinit gpio_led_probe(struct platform_device *pdev)
 	int i, ret = 0;
 
 	if (pdata && pdata->num_leds) {
-		priv = kzalloc(sizeof_gpio_leds_priv(pdata->num_leds),
-				GFP_KERNEL);
+		priv = devm_kzalloc(&pdev->dev,
+				sizeof_gpio_leds_priv(pdata->num_leds),
+					GFP_KERNEL);
 		if (!priv)
 			return -ENOMEM;
 
@@ -253,7 +254,6 @@ static int __devinit gpio_led_probe(struct platform_device *pdev)
 				/* On failure: unwind the led creations */
 				for (i = i - 1; i >= 0; i--)
 					delete_gpio_led(&priv->leds[i]);
-				kfree(priv);
 				return ret;
 			}
 		}
@@ -277,7 +277,6 @@ static int __devexit gpio_led_remove(struct platform_device *pdev)
 		delete_gpio_led(&priv->leds[i]);
 
 	dev_set_drvdata(&pdev->dev, NULL);
-	kfree(priv);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 84ba6de8039..23637bdb275 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -386,28 +386,24 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 
 	if (pdata == NULL) {
 		dev_err(&client->dev, "platform data required\n");
-		err = -ENODEV;
-		goto err_out;
+		return -ENODEV;
 	}
 
 	/* BL mode */
 	if (pdata->mode > LM3530_BL_MODE_PWM) {
 		dev_err(&client->dev, "Illegal Mode request\n");
-		err = -EINVAL;
-		goto err_out;
+		return -EINVAL;
 	}
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
 		dev_err(&client->dev, "I2C_FUNC_I2C not supported\n");
-		err = -EIO;
-		goto err_out;
+		return -EIO;
 	}
 
-	drvdata = kzalloc(sizeof(struct lm3530_data), GFP_KERNEL);
-	if (drvdata == NULL) {
-		err = -ENOMEM;
-		goto err_out;
-	}
+	drvdata = devm_kzalloc(&client->dev, sizeof(struct lm3530_data),
+				GFP_KERNEL);
+	if (drvdata == NULL)
+		return -ENOMEM;
 
 	drvdata->mode = pdata->mode;
 	drvdata->client = client;
@@ -425,7 +421,7 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 		dev_err(&client->dev, "regulator get failed\n");
 		err = PTR_ERR(drvdata->regulator);
 		drvdata->regulator = NULL;
-		goto err_regulator_get;
+		return err;
 	}
 
 	if (drvdata->pdata->brt_val) {
@@ -458,9 +454,6 @@ err_create_file:
 err_class_register:
 err_reg_init:
 	regulator_put(drvdata->regulator);
-err_regulator_get:
-	kfree(drvdata);
-err_out:
 	return err;
 }
 
@@ -474,7 +467,6 @@ static int __devexit lm3530_remove(struct i2c_client *client)
 		regulator_disable(drvdata->regulator);
 	regulator_put(drvdata->regulator);
 	led_classdev_unregister(&drvdata->led_dev);
-	kfree(drvdata);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-lm3556.c b/drivers/leds/leds-lm3556.c
new file mode 100644
index 00000000000..3062abd9a53
--- /dev/null
+++ b/drivers/leds/leds-lm3556.c
@@ -0,0 +1,512 @@
+/*
+ * Simple driver for Texas Instruments LM3556 LED Flash driver chip (Rev0x03)
+ * Copyright (C) 2012 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.
+ *
+ * Please refer Documentation/leds/leds-lm3556.txt file.
+ */
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/regmap.h>
+#include <linux/platform_data/leds-lm3556.h>
+
+#define REG_FILT_TIME			(0x0)
+#define REG_IVFM_MODE			(0x1)
+#define REG_NTC				(0x2)
+#define REG_INDIC_TIME			(0x3)
+#define REG_INDIC_BLINK			(0x4)
+#define REG_INDIC_PERIOD		(0x5)
+#define REG_TORCH_TIME			(0x6)
+#define REG_CONF			(0x7)
+#define REG_FLASH			(0x8)
+#define REG_I_CTRL			(0x9)
+#define REG_ENABLE			(0xA)
+#define REG_FLAG			(0xB)
+#define REG_MAX				(0xB)
+
+#define IVFM_FILTER_TIME_SHIFT		(3)
+#define UVLO_EN_SHIFT			(7)
+#define HYSTERSIS_SHIFT			(5)
+#define IVM_D_TH_SHIFT			(2)
+#define IVFM_ADJ_MODE_SHIFT		(0)
+#define NTC_EVENT_LVL_SHIFT		(5)
+#define NTC_TRIP_TH_SHIFT		(2)
+#define NTC_BIAS_I_LVL_SHIFT		(0)
+#define INDIC_RAMP_UP_TIME_SHIFT	(3)
+#define INDIC_RAMP_DN_TIME_SHIFT	(0)
+#define INDIC_N_BLANK_SHIFT		(4)
+#define INDIC_PULSE_TIME_SHIFT		(0)
+#define INDIC_N_PERIOD_SHIFT		(0)
+#define TORCH_RAMP_UP_TIME_SHIFT	(3)
+#define TORCH_RAMP_DN_TIME_SHIFT	(0)
+#define STROBE_USUAGE_SHIFT		(7)
+#define STROBE_PIN_POLARITY_SHIFT	(6)
+#define TORCH_PIN_POLARITY_SHIFT	(5)
+#define TX_PIN_POLARITY_SHIFT		(4)
+#define TX_EVENT_LVL_SHIFT		(3)
+#define IVFM_EN_SHIFT			(2)
+#define NTC_MODE_SHIFT			(1)
+#define INDIC_MODE_SHIFT		(0)
+#define INDUCTOR_I_LIMIT_SHIFT		(6)
+#define FLASH_RAMP_TIME_SHIFT		(3)
+#define FLASH_TOUT_TIME_SHIFT		(0)
+#define TORCH_I_SHIFT			(4)
+#define FLASH_I_SHIFT			(0)
+#define NTC_EN_SHIFT			(7)
+#define TX_PIN_EN_SHIFT			(6)
+#define STROBE_PIN_EN_SHIFT		(5)
+#define TORCH_PIN_EN_SHIFT		(4)
+#define PRECHG_MODE_EN_SHIFT		(3)
+#define PASS_MODE_ONLY_EN_SHIFT		(2)
+#define MODE_BITS_SHIFT			(0)
+
+#define IVFM_FILTER_TIME_MASK		(0x3)
+#define UVLO_EN_MASK			(0x1)
+#define HYSTERSIS_MASK			(0x3)
+#define IVM_D_TH_MASK			(0x7)
+#define IVFM_ADJ_MODE_MASK		(0x3)
+#define NTC_EVENT_LVL_MASK		(0x1)
+#define NTC_TRIP_TH_MASK		(0x7)
+#define NTC_BIAS_I_LVL_MASK		(0x3)
+#define INDIC_RAMP_UP_TIME_MASK		(0x7)
+#define INDIC_RAMP_DN_TIME_MASK		(0x7)
+#define INDIC_N_BLANK_MASK		(0x7)
+#define INDIC_PULSE_TIME_MASK		(0x7)
+#define INDIC_N_PERIOD_MASK		(0x7)
+#define TORCH_RAMP_UP_TIME_MASK		(0x7)
+#define TORCH_RAMP_DN_TIME_MASK		(0x7)
+#define STROBE_USUAGE_MASK		(0x1)
+#define STROBE_PIN_POLARITY_MASK	(0x1)
+#define TORCH_PIN_POLARITY_MASK		(0x1)
+#define TX_PIN_POLARITY_MASK		(0x1)
+#define TX_EVENT_LVL_MASK		(0x1)
+#define IVFM_EN_MASK			(0x1)
+#define NTC_MODE_MASK			(0x1)
+#define INDIC_MODE_MASK			(0x1)
+#define INDUCTOR_I_LIMIT_MASK		(0x3)
+#define FLASH_RAMP_TIME_MASK		(0x7)
+#define FLASH_TOUT_TIME_MASK		(0x7)
+#define TORCH_I_MASK			(0x7)
+#define FLASH_I_MASK			(0xF)
+#define NTC_EN_MASK			(0x1)
+#define TX_PIN_EN_MASK			(0x1)
+#define STROBE_PIN_EN_MASK		(0x1)
+#define TORCH_PIN_EN_MASK		(0x1)
+#define PRECHG_MODE_EN_MASK		(0x1)
+#define PASS_MODE_ONLY_EN_MASK		(0x1)
+#define MODE_BITS_MASK			(0x13)
+#define EX_PIN_CONTROL_MASK		(0xF1)
+#define EX_PIN_ENABLE_MASK		(0x70)
+
+enum lm3556_indic_pulse_time {
+	PULSE_TIME_0_MS = 0,
+	PULSE_TIME_32_MS,
+	PULSE_TIME_64_MS,
+	PULSE_TIME_92_MS,
+	PULSE_TIME_128_MS,
+	PULSE_TIME_160_MS,
+	PULSE_TIME_196_MS,
+	PULSE_TIME_224_MS,
+	PULSE_TIME_256_MS,
+	PULSE_TIME_288_MS,
+	PULSE_TIME_320_MS,
+	PULSE_TIME_352_MS,
+	PULSE_TIME_384_MS,
+	PULSE_TIME_416_MS,
+	PULSE_TIME_448_MS,
+	PULSE_TIME_480_MS,
+};
+
+enum lm3556_indic_n_blank {
+	INDIC_N_BLANK_0 = 0,
+	INDIC_N_BLANK_1,
+	INDIC_N_BLANK_2,
+	INDIC_N_BLANK_3,
+	INDIC_N_BLANK_4,
+	INDIC_N_BLANK_5,
+	INDIC_N_BLANK_6,
+	INDIC_N_BLANK_7,
+	INDIC_N_BLANK_8,
+	INDIC_N_BLANK_9,
+	INDIC_N_BLANK_10,
+	INDIC_N_BLANK_11,
+	INDIC_N_BLANK_12,
+	INDIC_N_BLANK_13,
+	INDIC_N_BLANK_14,
+	INDIC_N_BLANK_15,
+};
+
+enum lm3556_indic_period {
+	INDIC_PERIOD_0 = 0,
+	INDIC_PERIOD_1,
+	INDIC_PERIOD_2,
+	INDIC_PERIOD_3,
+	INDIC_PERIOD_4,
+	INDIC_PERIOD_5,
+	INDIC_PERIOD_6,
+	INDIC_PERIOD_7,
+};
+
+enum lm3556_mode {
+	MODES_STASNDBY = 0,
+	MODES_INDIC,
+	MODES_TORCH,
+	MODES_FLASH
+};
+
+#define INDIC_PATTERN_SIZE 4
+
+struct indicator {
+	u8 blinking;
+	u8 period_cnt;
+};
+
+struct lm3556_chip_data {
+	struct device *dev;
+
+	struct led_classdev cdev_flash;
+	struct led_classdev cdev_torch;
+	struct led_classdev cdev_indicator;
+
+	struct lm3556_platform_data *pdata;
+	struct regmap *regmap;
+	struct mutex lock;
+
+	unsigned int last_flag;
+};
+
+/* indicator pattern */
+static struct indicator indicator_pattern[INDIC_PATTERN_SIZE] = {
+	[0] = {(INDIC_N_BLANK_1 << INDIC_N_BLANK_SHIFT)
+	       | PULSE_TIME_32_MS, INDIC_PERIOD_1},
+	[1] = {(INDIC_N_BLANK_15 << INDIC_N_BLANK_SHIFT)
+	       | PULSE_TIME_32_MS, INDIC_PERIOD_2},
+	[2] = {(INDIC_N_BLANK_10 << INDIC_N_BLANK_SHIFT)
+	       | PULSE_TIME_32_MS, INDIC_PERIOD_4},
+	[3] = {(INDIC_N_BLANK_5 << INDIC_N_BLANK_SHIFT)
+	       | PULSE_TIME_32_MS, INDIC_PERIOD_7},
+};
+
+/* chip initialize */
+static int __devinit lm3556_chip_init(struct lm3556_chip_data *chip)
+{
+	unsigned int reg_val;
+	int ret;
+	struct lm3556_platform_data *pdata = chip->pdata;
+
+	/* set config register */
+	ret = regmap_read(chip->regmap, REG_CONF, &reg_val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read REG_CONF Register\n");
+		goto out;
+	}
+
+	reg_val &= (~EX_PIN_CONTROL_MASK);
+	reg_val |= ((pdata->torch_pin_polarity & 0x01)
+		    << TORCH_PIN_POLARITY_SHIFT);
+	reg_val |= ((pdata->strobe_usuage & 0x01) << STROBE_USUAGE_SHIFT);
+	reg_val |= ((pdata->strobe_pin_polarity & 0x01)
+		    << STROBE_PIN_POLARITY_SHIFT);
+	reg_val |= ((pdata->tx_pin_polarity & 0x01) << TX_PIN_POLARITY_SHIFT);
+	reg_val |= ((pdata->indicator_mode & 0x01) << INDIC_MODE_SHIFT);
+
+	ret = regmap_write(chip->regmap, REG_CONF, reg_val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to write REG_CONF Regisgter\n");
+		goto out;
+	}
+
+	/* set enable register */
+	ret = regmap_read(chip->regmap, REG_ENABLE, &reg_val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read REG_ENABLE Register\n");
+		goto out;
+	}
+
+	reg_val &= (~EX_PIN_ENABLE_MASK);
+	reg_val |= ((pdata->torch_pin_en & 0x01) << TORCH_PIN_EN_SHIFT);
+	reg_val |= ((pdata->strobe_pin_en & 0x01) << STROBE_PIN_EN_SHIFT);
+	reg_val |= ((pdata->tx_pin_en & 0x01) << TX_PIN_EN_SHIFT);
+
+	ret = regmap_write(chip->regmap, REG_ENABLE, reg_val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+/* chip control */
+static int lm3556_control(struct lm3556_chip_data *chip,
+			  u8 brightness, enum lm3556_mode opmode)
+{
+	int ret;
+	struct lm3556_platform_data *pdata = chip->pdata;
+
+	ret = regmap_read(chip->regmap, REG_FLAG, &chip->last_flag);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read REG_FLAG Register\n");
+		goto out;
+	}
+
+	if (chip->last_flag)
+		dev_info(chip->dev, "Last FLAG is 0x%x\n", chip->last_flag);
+
+	/* brightness 0 means off-state */
+	if (!brightness)
+		opmode = MODES_STASNDBY;
+
+	switch (opmode) {
+	case MODES_TORCH:
+		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+					 TORCH_I_MASK << TORCH_I_SHIFT,
+					 (brightness - 1) << TORCH_I_SHIFT);
+
+		if (pdata->torch_pin_en)
+			opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
+		break;
+
+	case MODES_FLASH:
+		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+					 FLASH_I_MASK << FLASH_I_SHIFT,
+					 (brightness - 1) << FLASH_I_SHIFT);
+		break;
+
+	case MODES_INDIC:
+		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+					 TORCH_I_MASK << TORCH_I_SHIFT,
+					 (brightness - 1) << TORCH_I_SHIFT);
+		break;
+
+	case MODES_STASNDBY:
+		if (pdata->torch_pin_en)
+			opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
+		break;
+
+	default:
+		return ret;
+	}
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to write REG_I_CTRL Register\n");
+		goto out;
+	}
+	ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+				 MODE_BITS_MASK << MODE_BITS_SHIFT,
+				 opmode << MODE_BITS_SHIFT);
+
+out:
+	return ret;
+}
+
+/* torch */
+static void lm3556_torch_brightness_set(struct led_classdev *cdev,
+					enum led_brightness brightness)
+{
+	struct lm3556_chip_data *chip =
+	    container_of(cdev, struct lm3556_chip_data, cdev_torch);
+
+	mutex_lock(&chip->lock);
+	lm3556_control(chip, brightness, MODES_TORCH);
+	mutex_unlock(&chip->lock);
+}
+
+/* flash */
+static void lm3556_strobe_brightness_set(struct led_classdev *cdev,
+					 enum led_brightness brightness)
+{
+	struct lm3556_chip_data *chip =
+	    container_of(cdev, struct lm3556_chip_data, cdev_flash);
+
+	mutex_lock(&chip->lock);
+	lm3556_control(chip, brightness, MODES_FLASH);
+	mutex_unlock(&chip->lock);
+}
+
+/* indicator */
+static void lm3556_indicator_brightness_set(struct led_classdev *cdev,
+					    enum led_brightness brightness)
+{
+	struct lm3556_chip_data *chip =
+	    container_of(cdev, struct lm3556_chip_data, cdev_indicator);
+
+	mutex_lock(&chip->lock);
+	lm3556_control(chip, brightness, MODES_INDIC);
+	mutex_unlock(&chip->lock);
+}
+
+/* indicator pattern */
+static ssize_t lm3556_indicator_pattern_store(struct device *dev,
+					      struct device_attribute *devAttr,
+					      const char *buf, size_t size)
+{
+	ssize_t ret;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3556_chip_data *chip =
+	    container_of(led_cdev, struct lm3556_chip_data, cdev_indicator);
+	unsigned int state;
+
+	ret = kstrtouint(buf, 10, &state);
+	if (ret)
+		goto out;
+	if (state > INDIC_PATTERN_SIZE - 1)
+		state = INDIC_PATTERN_SIZE - 1;
+
+	ret = regmap_write(chip->regmap, REG_INDIC_BLINK,
+			   indicator_pattern[state].blinking);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
+		goto out;
+	}
+
+	ret = regmap_write(chip->regmap, REG_INDIC_PERIOD,
+			   indicator_pattern[state].period_cnt);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
+		goto out;
+	}
+
+	return size;
+out:
+	dev_err(chip->dev, "Indicator pattern doesn't saved\n");
+	return size;
+}
+
+static DEVICE_ATTR(pattern, 0666, NULL, lm3556_indicator_pattern_store);
+
+static const struct regmap_config lm3556_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = REG_MAX,
+};
+
+/* module initialize */
+static int __devinit lm3556_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct lm3556_platform_data *pdata = client->dev.platform_data;
+	struct lm3556_chip_data *chip;
+
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c functionality check fail.\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "Needs Platform Data.\n");
+		return -ENODATA;
+	}
+
+	chip =
+	    devm_kzalloc(&client->dev, sizeof(struct lm3556_chip_data),
+			 GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = &client->dev;
+	chip->pdata = pdata;
+
+	chip->regmap = devm_regmap_init_i2c(client, &lm3556_regmap);
+	if (IS_ERR(chip->regmap)) {
+		err = PTR_ERR(chip->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			err);
+		return err;
+	}
+
+	mutex_init(&chip->lock);
+	i2c_set_clientdata(client, chip);
+
+	err = lm3556_chip_init(chip);
+	if (err < 0)
+		goto err_out;
+
+	/* flash */
+	chip->cdev_flash.name = "flash";
+	chip->cdev_flash.max_brightness = 16;
+	chip->cdev_flash.brightness_set = lm3556_strobe_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_flash);
+	if (err < 0)
+		goto err_out;
+	/* torch */
+	chip->cdev_torch.name = "torch";
+	chip->cdev_torch.max_brightness = 8;
+	chip->cdev_torch.brightness_set = lm3556_torch_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_torch);
+	if (err < 0)
+		goto err_create_torch_file;
+	/* indicator */
+	chip->cdev_indicator.name = "indicator";
+	chip->cdev_indicator.max_brightness = 8;
+	chip->cdev_indicator.brightness_set = lm3556_indicator_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_indicator);
+	if (err < 0)
+		goto err_create_indicator_file;
+
+	err = device_create_file(chip->cdev_indicator.dev, &dev_attr_pattern);
+	if (err < 0)
+		goto err_create_pattern_file;
+
+	dev_info(&client->dev, "LM3556 is initialized\n");
+	return 0;
+
+err_create_pattern_file:
+	led_classdev_unregister(&chip->cdev_indicator);
+err_create_indicator_file:
+	led_classdev_unregister(&chip->cdev_torch);
+err_create_torch_file:
+	led_classdev_unregister(&chip->cdev_flash);
+err_out:
+	return err;
+}
+
+static int __devexit lm3556_remove(struct i2c_client *client)
+{
+	struct lm3556_chip_data *chip = i2c_get_clientdata(client);
+
+	device_remove_file(chip->cdev_indicator.dev, &dev_attr_pattern);
+	led_classdev_unregister(&chip->cdev_indicator);
+	led_classdev_unregister(&chip->cdev_torch);
+	led_classdev_unregister(&chip->cdev_flash);
+	regmap_write(chip->regmap, REG_ENABLE, 0);
+	return 0;
+}
+
+static const struct i2c_device_id lm3556_id[] = {
+	{LM3556_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm3556_id);
+
+static struct i2c_driver lm3556_i2c_driver = {
+	.driver = {
+		   .name = LM3556_NAME,
+		   .owner = THIS_MODULE,
+		   .pm = NULL,
+		   },
+	.probe = lm3556_probe,
+	.remove = __devexit_p(lm3556_remove),
+	.id_table = lm3556_id,
+};
+
+module_i2c_driver(lm3556_i2c_driver);
+
+MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM3556");
+MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
+MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lp3944.c b/drivers/leds/leds-lp3944.c
index b8f9f0a5d43..c298f7d9f53 100644
--- a/drivers/leds/leds-lp3944.c
+++ b/drivers/leds/leds-lp3944.c
@@ -393,7 +393,8 @@ static int __devinit lp3944_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	data = kzalloc(sizeof(struct lp3944_data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(struct lp3944_data),
+			GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -403,10 +404,8 @@ static int __devinit lp3944_probe(struct i2c_client *client,
 	mutex_init(&data->lock);
 
 	err = lp3944_configure(client, data, lp3944_pdata);
-	if (err < 0) {
-		kfree(data);
+	if (err < 0)
 		return err;
-	}
 
 	dev_info(&client->dev, "lp3944 enabled\n");
 	return 0;
@@ -431,8 +430,6 @@ static int __devexit lp3944_remove(struct i2c_client *client)
 			break;
 		}
 
-	kfree(data);
-
 	return 0;
 }
 
diff --git a/drivers/leds/leds-lp5521.c b/drivers/leds/leds-lp5521.c
index 23815624f35..2064aefedc0 100644
--- a/drivers/leds/leds-lp5521.c
+++ b/drivers/leds/leds-lp5521.c
@@ -744,7 +744,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 	int ret, i, led;
 	u8 buf;
 
-	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	chip = devm_kzalloc(&client->dev, sizeof(*chip), GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
@@ -755,8 +755,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 
 	if (!pdata) {
 		dev_err(&client->dev, "no platform data\n");
-		ret = -EINVAL;
-		goto fail1;
+		return -EINVAL;
 	}
 
 	mutex_init(&chip->lock);
@@ -766,7 +765,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 	if (pdata->setup_resources) {
 		ret = pdata->setup_resources();
 		if (ret < 0)
-			goto fail1;
+			return ret;
 	}
 
 	if (pdata->enable) {
@@ -807,7 +806,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 	ret = lp5521_configure(client);
 	if (ret < 0) {
 		dev_err(&client->dev, "error configuring chip\n");
-		goto fail2;
+		goto fail1;
 	}
 
 	/* Initialize leds */
@@ -822,7 +821,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 		ret = lp5521_init_led(&chip->leds[led], client, i, pdata);
 		if (ret) {
 			dev_err(&client->dev, "error initializing leds\n");
-			goto fail3;
+			goto fail2;
 		}
 		chip->num_leds++;
 
@@ -840,21 +839,19 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 	ret = lp5521_register_sysfs(client);
 	if (ret) {
 		dev_err(&client->dev, "registering sysfs failed\n");
-		goto fail3;
+		goto fail2;
 	}
 	return ret;
-fail3:
+fail2:
 	for (i = 0; i < chip->num_leds; i++) {
 		led_classdev_unregister(&chip->leds[i].cdev);
 		cancel_work_sync(&chip->leds[i].brightness_work);
 	}
-fail2:
+fail1:
 	if (pdata->enable)
 		pdata->enable(0);
 	if (pdata->release_resources)
 		pdata->release_resources();
-fail1:
-	kfree(chip);
 	return ret;
 }
 
@@ -875,7 +872,6 @@ static int __devexit lp5521_remove(struct i2c_client *client)
 		chip->pdata->enable(0);
 	if (chip->pdata->release_resources)
 		chip->pdata->release_resources();
-	kfree(chip);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index 857a3e15f2d..fbc12acada9 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -877,7 +877,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 	struct lp5523_platform_data	*pdata;
 	int ret, i, led;
 
-	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	chip = devm_kzalloc(&client->dev, sizeof(*chip), GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
@@ -888,8 +888,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 
 	if (!pdata) {
 		dev_err(&client->dev, "no platform data\n");
-		ret = -EINVAL;
-		goto fail1;
+		return -EINVAL;
 	}
 
 	mutex_init(&chip->lock);
@@ -899,7 +898,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 	if (pdata->setup_resources) {
 		ret = pdata->setup_resources();
 		if (ret < 0)
-			goto fail1;
+			return ret;
 	}
 
 	if (pdata->enable) {
@@ -916,7 +915,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 				     */
 	ret = lp5523_detect(client);
 	if (ret)
-		goto fail2;
+		goto fail1;
 
 	dev_info(&client->dev, "LP5523 Programmable led chip found\n");
 
@@ -925,13 +924,13 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 		ret = lp5523_init_engine(&chip->engines[i], i + 1);
 		if (ret) {
 			dev_err(&client->dev, "error initializing engine\n");
-			goto fail2;
+			goto fail1;
 		}
 	}
 	ret = lp5523_configure(client);
 	if (ret < 0) {
 		dev_err(&client->dev, "error configuring chip\n");
-		goto fail2;
+		goto fail1;
 	}
 
 	/* Initialize leds */
@@ -943,10 +942,13 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 		if (pdata->led_config[i].led_current == 0)
 			continue;
 
+		INIT_WORK(&chip->leds[led].brightness_work,
+			lp5523_led_brightness_work);
+
 		ret = lp5523_init_led(&chip->leds[led], &client->dev, i, pdata);
 		if (ret) {
 			dev_err(&client->dev, "error initializing leds\n");
-			goto fail3;
+			goto fail2;
 		}
 		chip->num_leds++;
 
@@ -956,30 +958,25 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 			  LP5523_REG_LED_CURRENT_BASE + chip->leds[led].chan_nr,
 			  chip->leds[led].led_current);
 
-		INIT_WORK(&(chip->leds[led].brightness_work),
-			lp5523_led_brightness_work);
-
 		led++;
 	}
 
 	ret = lp5523_register_sysfs(client);
 	if (ret) {
 		dev_err(&client->dev, "registering sysfs failed\n");
-		goto fail3;
+		goto fail2;
 	}
 	return ret;
-fail3:
+fail2:
 	for (i = 0; i < chip->num_leds; i++) {
 		led_classdev_unregister(&chip->leds[i].cdev);
 		cancel_work_sync(&chip->leds[i].brightness_work);
 	}
-fail2:
+fail1:
 	if (pdata->enable)
 		pdata->enable(0);
 	if (pdata->release_resources)
 		pdata->release_resources();
-fail1:
-	kfree(chip);
 	return ret;
 }
 
@@ -999,7 +996,6 @@ static int lp5523_remove(struct i2c_client *client)
 		chip->pdata->enable(0);
 	if (chip->pdata->release_resources)
 		chip->pdata->release_resources();
-	kfree(chip);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-lp8788.c b/drivers/leds/leds-lp8788.c
new file mode 100644
index 00000000000..53bd136f1ef
--- /dev/null
+++ b/drivers/leds/leds-lp8788.c
@@ -0,0 +1,193 @@
+/*
+ * TI LP8788 MFD - keyled driver
+ *
+ * Copyright 2012 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/mutex.h>
+#include <linux/mfd/lp8788.h>
+#include <linux/mfd/lp8788-isink.h>
+
+#define MAX_BRIGHTNESS			LP8788_ISINK_MAX_PWM
+#define DEFAULT_LED_NAME		"keyboard-backlight"
+
+struct lp8788_led {
+	struct lp8788 *lp;
+	struct mutex lock;
+	struct work_struct work;
+	struct led_classdev led_dev;
+	enum lp8788_isink_number isink_num;
+	enum led_brightness brightness;
+	int on;
+};
+
+struct lp8788_led_config {
+	enum lp8788_isink_scale scale;
+	enum lp8788_isink_number num;
+	int iout;
+};
+
+static struct lp8788_led_config default_led_config = {
+	.scale = LP8788_ISINK_SCALE_100mA,
+	.num   = LP8788_ISINK_3,
+	.iout  = 0,
+};
+
+static int lp8788_led_init_device(struct lp8788_led *led,
+				struct lp8788_led_platform_data *pdata)
+{
+	struct lp8788_led_config *cfg = &default_led_config;
+	u8 addr, mask, val;
+	int ret;
+
+	if (pdata) {
+		cfg->scale = pdata->scale;
+		cfg->num = pdata->num;
+		cfg->iout = pdata->iout_code;
+	}
+
+	led->isink_num = cfg->num;
+
+	/* scale configuration */
+	addr = LP8788_ISINK_CTRL;
+	mask = 1 << (cfg->num + LP8788_ISINK_SCALE_OFFSET);
+	val = cfg->scale << cfg->num;
+	ret = lp8788_update_bits(led->lp, addr, mask, val);
+	if (ret)
+		return ret;
+
+	/* current configuration */
+	addr = lp8788_iout_addr[cfg->num];
+	mask = lp8788_iout_mask[cfg->num];
+	val = cfg->iout;
+
+	return lp8788_update_bits(led->lp, addr, mask, val);
+}
+
+static void lp8788_led_enable(struct lp8788_led *led,
+			enum lp8788_isink_number num, int on)
+{
+	u8 mask = 1 << num;
+	u8 val = on << num;
+
+	if (lp8788_update_bits(led->lp, LP8788_ISINK_CTRL, mask, val))
+		return;
+
+	led->on = on;
+}
+
+static void lp8788_led_work(struct work_struct *work)
+{
+	struct lp8788_led *led = container_of(work, struct lp8788_led, work);
+	enum lp8788_isink_number num = led->isink_num;
+	int enable;
+	u8 val = led->brightness;
+
+	mutex_lock(&led->lock);
+
+	switch (num) {
+	case LP8788_ISINK_1:
+	case LP8788_ISINK_2:
+	case LP8788_ISINK_3:
+		lp8788_write_byte(led->lp, lp8788_pwm_addr[num], val);
+		break;
+	default:
+		mutex_unlock(&led->lock);
+		return;
+	}
+
+	enable = (val > 0) ? 1 : 0;
+	if (enable != led->on)
+		lp8788_led_enable(led, num, enable);
+
+	mutex_unlock(&led->lock);
+}
+
+static void lp8788_brightness_set(struct led_classdev *led_cdev,
+				enum led_brightness brt_val)
+{
+	struct lp8788_led *led =
+			container_of(led_cdev, struct lp8788_led, led_dev);
+
+	led->brightness = brt_val;
+	schedule_work(&led->work);
+}
+
+static __devinit int lp8788_led_probe(struct platform_device *pdev)
+{
+	struct lp8788 *lp = dev_get_drvdata(pdev->dev.parent);
+	struct lp8788_led_platform_data *led_pdata;
+	struct lp8788_led *led;
+	int ret;
+
+	led = devm_kzalloc(lp->dev, sizeof(struct lp8788_led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->lp = lp;
+	led->led_dev.max_brightness = MAX_BRIGHTNESS;
+	led->led_dev.brightness_set = lp8788_brightness_set;
+
+	led_pdata = lp->pdata ? lp->pdata->led_pdata : NULL;
+
+	if (!led_pdata || !led_pdata->name)
+		led->led_dev.name = DEFAULT_LED_NAME;
+	else
+		led->led_dev.name = led_pdata->name;
+
+	mutex_init(&led->lock);
+	INIT_WORK(&led->work, lp8788_led_work);
+
+	platform_set_drvdata(pdev, led);
+
+	ret = lp8788_led_init_device(led, led_pdata);
+	if (ret) {
+		dev_err(lp->dev, "led init device err: %d\n", ret);
+		return ret;
+	}
+
+	ret = led_classdev_register(lp->dev, &led->led_dev);
+	if (ret) {
+		dev_err(lp->dev, "led register err: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit lp8788_led_remove(struct platform_device *pdev)
+{
+	struct lp8788_led *led = platform_get_drvdata(pdev);
+
+	led_classdev_unregister(&led->led_dev);
+	flush_work_sync(&led->work);
+
+	return 0;
+}
+
+static struct platform_driver lp8788_led_driver = {
+	.probe = lp8788_led_probe,
+	.remove = __devexit_p(lp8788_led_remove),
+	.driver = {
+		.name = LP8788_DEV_KEYLED,
+		.owner = THIS_MODULE,
+	},
+};
+module_platform_driver(lp8788_led_driver);
+
+MODULE_DESCRIPTION("Texas Instruments LP8788 Keyboard LED Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lp8788-keyled");
diff --git a/drivers/leds/leds-lt3593.c b/drivers/leds/leds-lt3593.c
index e311a96c446..09a732217f6 100644
--- a/drivers/leds/leds-lt3593.c
+++ b/drivers/leds/leds-lt3593.c
@@ -149,8 +149,9 @@ static int __devinit lt3593_led_probe(struct platform_device *pdev)
 	if (!pdata)
 		return -EBUSY;
 
-	leds_data = kzalloc(sizeof(struct lt3593_led_data) * pdata->num_leds,
-				GFP_KERNEL);
+	leds_data = devm_kzalloc(&pdev->dev,
+			sizeof(struct lt3593_led_data) * pdata->num_leds,
+			GFP_KERNEL);
 	if (!leds_data)
 		return -ENOMEM;
 
@@ -169,8 +170,6 @@ err:
 	for (i = i - 1; i >= 0; i--)
 		delete_lt3593_led(&leds_data[i]);
 
-	kfree(leds_data);
-
 	return ret;
 }
 
@@ -185,8 +184,6 @@ static int __devexit lt3593_led_remove(struct platform_device *pdev)
 	for (i = 0; i < pdata->num_leds; i++)
 		delete_lt3593_led(&leds_data[i]);
 
-	kfree(leds_data);
-
 	return 0;
 }
 
diff --git a/drivers/leds/leds-max8997.c b/drivers/leds/leds-max8997.c
index f4c0e37fad1..569e36de37d 100644
--- a/drivers/leds/leds-max8997.c
+++ b/drivers/leds/leds-max8997.c
@@ -49,71 +49,37 @@ struct max8997_led {
 	struct mutex mutex;
 };
 
-static void max8997_led_clear_mode(struct max8997_led *led,
-			enum max8997_led_mode mode)
-{
-	struct i2c_client *client = led->iodev->i2c;
-	u8 val = 0, mask = 0;
-	int ret;
-
-	switch (mode) {
-	case MAX8997_FLASH_MODE:
-		mask = led->id ?
-		      MAX8997_LED1_FLASH_MASK : MAX8997_LED0_FLASH_MASK;
-		break;
-	case MAX8997_MOVIE_MODE:
-		mask = led->id ?
-		      MAX8997_LED1_MOVIE_MASK : MAX8997_LED0_MOVIE_MASK;
-		break;
-	case MAX8997_FLASH_PIN_CONTROL_MODE:
-		mask = led->id ?
-		      MAX8997_LED1_FLASH_PIN_MASK : MAX8997_LED0_FLASH_PIN_MASK;
-		break;
-	case MAX8997_MOVIE_PIN_CONTROL_MODE:
-		mask = led->id ?
-		      MAX8997_LED1_MOVIE_PIN_MASK : MAX8997_LED0_MOVIE_PIN_MASK;
-		break;
-	default:
-		break;
-	}
-
-	if (mask) {
-		ret = max8997_update_reg(client,
-				MAX8997_REG_LEN_CNTL, val, mask);
-		if (ret)
-			dev_err(led->iodev->dev,
-				"failed to update register(%d)\n", ret);
-	}
-}
-
 static void max8997_led_set_mode(struct max8997_led *led,
 			enum max8997_led_mode mode)
 {
 	int ret;
 	struct i2c_client *client = led->iodev->i2c;
-	u8 mask = 0;
-
-	/* First, clear the previous mode */
-	max8997_led_clear_mode(led, led->led_mode);
+	u8 mask = 0, val;
 
 	switch (mode) {
 	case MAX8997_FLASH_MODE:
-		mask = led->id ?
+		mask = MAX8997_LED1_FLASH_MASK | MAX8997_LED0_FLASH_MASK;
+		val = led->id ?
 		      MAX8997_LED1_FLASH_MASK : MAX8997_LED0_FLASH_MASK;
 		led->cdev.max_brightness = MAX8997_LED_FLASH_MAX_BRIGHTNESS;
 		break;
 	case MAX8997_MOVIE_MODE:
-		mask = led->id ?
+		mask = MAX8997_LED1_MOVIE_MASK | MAX8997_LED0_MOVIE_MASK;
+		val = led->id ?
 		      MAX8997_LED1_MOVIE_MASK : MAX8997_LED0_MOVIE_MASK;
 		led->cdev.max_brightness = MAX8997_LED_MOVIE_MAX_BRIGHTNESS;
 		break;
 	case MAX8997_FLASH_PIN_CONTROL_MODE:
-		mask = led->id ?
+		mask = MAX8997_LED1_FLASH_PIN_MASK |
+		       MAX8997_LED0_FLASH_PIN_MASK;
+		val = led->id ?
 		      MAX8997_LED1_FLASH_PIN_MASK : MAX8997_LED0_FLASH_PIN_MASK;
 		led->cdev.max_brightness = MAX8997_LED_FLASH_MAX_BRIGHTNESS;
 		break;
 	case MAX8997_MOVIE_PIN_CONTROL_MODE:
-		mask = led->id ?
+		mask = MAX8997_LED1_MOVIE_PIN_MASK |
+		       MAX8997_LED0_MOVIE_PIN_MASK;
+		val = led->id ?
 		      MAX8997_LED1_MOVIE_PIN_MASK : MAX8997_LED0_MOVIE_PIN_MASK;
 		led->cdev.max_brightness = MAX8997_LED_MOVIE_MAX_BRIGHTNESS;
 		break;
@@ -123,8 +89,8 @@ static void max8997_led_set_mode(struct max8997_led *led,
 	}
 
 	if (mask) {
-		ret = max8997_update_reg(client,
-				MAX8997_REG_LEN_CNTL, mask, mask);
+		ret = max8997_update_reg(client, MAX8997_REG_LEN_CNTL, val,
+					 mask);
 		if (ret)
 			dev_err(led->iodev->dev,
 				"failed to update register(%d)\n", ret);
@@ -276,11 +242,9 @@ static int __devinit max8997_led_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	led = kzalloc(sizeof(*led), GFP_KERNEL);
-	if (led == NULL) {
-		ret = -ENOMEM;
-		goto err_mem;
-	}
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
+	if (led == NULL)
+		return -ENOMEM;
 
 	led->id = pdev->id;
 	snprintf(name, sizeof(name), "max8997-led%d", pdev->id);
@@ -315,23 +279,17 @@ static int __devinit max8997_led_probe(struct platform_device *pdev)
 
 	ret = led_classdev_register(&pdev->dev, &led->cdev);
 	if (ret < 0)
-		goto err_led;
+		return ret;
 
 	ret = device_create_file(led->cdev.dev, &dev_attr_mode);
 	if (ret != 0) {
 		dev_err(&pdev->dev,
 			"failed to create file: %d\n", ret);
-		goto err_file;
+		led_classdev_unregister(&led->cdev);
+		return ret;
 	}
 
 	return 0;
-
-err_file:
-	led_classdev_unregister(&led->cdev);
-err_led:
-	kfree(led);
-err_mem:
-	return ret;
 }
 
 static int __devexit max8997_led_remove(struct platform_device *pdev)
@@ -340,7 +298,6 @@ static int __devexit max8997_led_remove(struct platform_device *pdev)
 
 	device_remove_file(led->cdev.dev, &dev_attr_mode);
 	led_classdev_unregister(&led->cdev);
-	kfree(led);
 
 	return 0;
 }
@@ -354,17 +311,7 @@ static struct platform_driver max8997_led_driver = {
 	.remove = __devexit_p(max8997_led_remove),
 };
 
-static int __init max8997_led_init(void)
-{
-	return platform_driver_register(&max8997_led_driver);
-}
-module_init(max8997_led_init);
-
-static void __exit max8997_led_exit(void)
-{
-	platform_driver_unregister(&max8997_led_driver);
-}
-module_exit(max8997_led_exit);
+module_platform_driver(max8997_led_driver);
 
 MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
 MODULE_DESCRIPTION("MAX8997 LED driver");
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index 4cc6a2e3df3..2a5d4340067 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -280,7 +280,8 @@ static int __devinit mc13783_led_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	led = kcalloc(pdata->num_leds, sizeof(*led), GFP_KERNEL);
+	led = devm_kzalloc(&pdev->dev, pdata->num_leds * sizeof(*led),
+				GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&pdev->dev, "failed to alloc memory\n");
 		return -ENOMEM;
@@ -289,7 +290,7 @@ static int __devinit mc13783_led_probe(struct platform_device *pdev)
 	ret = mc13783_leds_prepare(pdev);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to init led driver\n");
-		goto err_free;
+		return ret;
 	}
 
 	for (i = 0; i < pdata->num_leds; i++) {
@@ -344,8 +345,6 @@ err_register:
 		cancel_work_sync(&led[i].work);
 	}
 
-err_free:
-	kfree(led);
 	return ret;
 }
 
@@ -372,7 +371,7 @@ static int __devexit mc13783_led_remove(struct platform_device *pdev)
 
 	mc13xxx_unlock(dev);
 
-	kfree(led);
+	platform_set_drvdata(pdev, NULL);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-netxbig.c b/drivers/leds/leds-netxbig.c
index 73973fdbd8b..e37618e363c 100644
--- a/drivers/leds/leds-netxbig.c
+++ b/drivers/leds/leds-netxbig.c
@@ -362,14 +362,14 @@ static int __devinit netxbig_led_probe(struct platform_device *pdev)
 	if (!pdata)
 		return -EINVAL;
 
-	leds_data = kzalloc(sizeof(struct netxbig_led_data) * pdata->num_leds,
-			    GFP_KERNEL);
+	leds_data = devm_kzalloc(&pdev->dev,
+		sizeof(struct netxbig_led_data) * pdata->num_leds, GFP_KERNEL);
 	if (!leds_data)
 		return -ENOMEM;
 
 	ret = gpio_ext_init(pdata->gpio_ext);
 	if (ret < 0)
-		goto err_free_data;
+		return ret;
 
 	for (i = 0; i < pdata->num_leds; i++) {
 		ret = create_netxbig_led(pdev, &leds_data[i], &pdata->leds[i]);
@@ -386,9 +386,6 @@ err_free_leds:
 		delete_netxbig_led(&leds_data[i]);
 
 	gpio_ext_free(pdata->gpio_ext);
-err_free_data:
-	kfree(leds_data);
-
 	return ret;
 }
 
@@ -404,7 +401,6 @@ static int __devexit netxbig_led_remove(struct platform_device *pdev)
 		delete_netxbig_led(&leds_data[i]);
 
 	gpio_ext_free(pdata->gpio_ext);
-	kfree(leds_data);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-ns2.c b/drivers/leds/leds-ns2.c
index 01cf89ec694..10528dafb04 100644
--- a/drivers/leds/leds-ns2.c
+++ b/drivers/leds/leds-ns2.c
@@ -273,29 +273,23 @@ static int __devinit ns2_led_probe(struct platform_device *pdev)
 	if (!pdata)
 		return -EINVAL;
 
-	leds_data = kzalloc(sizeof(struct ns2_led_data) *
+	leds_data = devm_kzalloc(&pdev->dev, sizeof(struct ns2_led_data) *
 			    pdata->num_leds, GFP_KERNEL);
 	if (!leds_data)
 		return -ENOMEM;
 
 	for (i = 0; i < pdata->num_leds; i++) {
 		ret = create_ns2_led(pdev, &leds_data[i], &pdata->leds[i]);
-		if (ret < 0)
-			goto err;
-
+		if (ret < 0) {
+			for (i = i - 1; i >= 0; i--)
+				delete_ns2_led(&leds_data[i]);
+			return ret;
+		}
 	}
 
 	platform_set_drvdata(pdev, leds_data);
 
 	return 0;
-
-err:
-	for (i = i - 1; i >= 0; i--)
-		delete_ns2_led(&leds_data[i]);
-
-	kfree(leds_data);
-
-	return ret;
 }
 
 static int __devexit ns2_led_remove(struct platform_device *pdev)
@@ -309,7 +303,6 @@ static int __devexit ns2_led_remove(struct platform_device *pdev)
 	for (i = 0; i < pdata->num_leds; i++)
 		delete_ns2_led(&leds_data[i]);
 
-	kfree(leds_data);
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
diff --git a/drivers/leds/leds-pca9532.c b/drivers/leds/leds-pca9532.c
index ceccab44b5b..cee8a5b483a 100644
--- a/drivers/leds/leds-pca9532.c
+++ b/drivers/leds/leds-pca9532.c
@@ -449,7 +449,6 @@ static int pca9532_probe(struct i2c_client *client,
 {
 	struct pca9532_data *data = i2c_get_clientdata(client);
 	struct pca9532_platform_data *pca9532_pdata = client->dev.platform_data;
-	int err;
 
 	if (!pca9532_pdata)
 		return -EIO;
@@ -458,7 +457,7 @@ static int pca9532_probe(struct i2c_client *client,
 		I2C_FUNC_SMBUS_BYTE_DATA))
 		return -EIO;
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
@@ -469,11 +468,7 @@ static int pca9532_probe(struct i2c_client *client,
 	data->client = client;
 	mutex_init(&data->update_lock);
 
-	err = pca9532_configure(client, data, pca9532_pdata);
-	if (err)
-		kfree(data);
-
-	return err;
+	return pca9532_configure(client, data, pca9532_pdata);
 }
 
 static int pca9532_remove(struct i2c_client *client)
@@ -485,7 +480,6 @@ static int pca9532_remove(struct i2c_client *client)
 	if (err)
 		return err;
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
index 5f462dbf0db..aef3cf0432f 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -293,15 +293,14 @@ static int __devinit pca955x_probe(struct i2c_client *client,
 		}
 	}
 
-	pca955x = kzalloc(sizeof(*pca955x), GFP_KERNEL);
+	pca955x = devm_kzalloc(&client->dev, sizeof(*pca955x), GFP_KERNEL);
 	if (!pca955x)
 		return -ENOMEM;
 
-	pca955x->leds = kzalloc(sizeof(*pca955x_led) * chip->bits, GFP_KERNEL);
-	if (!pca955x->leds) {
-		err = -ENOMEM;
-		goto exit_nomem;
-	}
+	pca955x->leds = devm_kzalloc(&client->dev,
+			sizeof(*pca955x_led) * chip->bits, GFP_KERNEL);
+	if (!pca955x->leds)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, pca955x);
 
@@ -361,10 +360,6 @@ exit:
 		cancel_work_sync(&pca955x->leds[i].work);
 	}
 
-	kfree(pca955x->leds);
-exit_nomem:
-	kfree(pca955x);
-
 	return err;
 }
 
@@ -378,9 +373,6 @@ static int __devexit pca955x_remove(struct i2c_client *client)
 		cancel_work_sync(&pca955x->leds[i].work);
 	}
 
-	kfree(pca955x->leds);
-	kfree(pca955x);
-
 	return 0;
 }
 
diff --git a/drivers/leds/leds-pca9633.c b/drivers/leds/leds-pca9633.c
index d8926fd031a..edcd706c563 100644
--- a/drivers/leds/leds-pca9633.c
+++ b/drivers/leds/leds-pca9633.c
@@ -108,7 +108,7 @@ static int __devinit pca9633_probe(struct i2c_client *client,
 		}
 	}
 
-	pca9633 = kcalloc(4, sizeof(*pca9633), GFP_KERNEL);
+	pca9633 = devm_kzalloc(&client->dev, 4 * sizeof(*pca9633), GFP_KERNEL);
 	if (!pca9633)
 		return -ENOMEM;
 
@@ -156,8 +156,6 @@ exit:
 		cancel_work_sync(&pca9633[i].work);
 	}
 
-	kfree(pca9633);
-
 	return err;
 }
 
@@ -171,8 +169,6 @@ static int __devexit pca9633_remove(struct i2c_client *client)
 		cancel_work_sync(&pca9633[i].work);
 	}
 
-	kfree(pca9633);
-
 	return 0;
 }
 
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index 3ed92f34bd4..f2e44c71943 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -57,7 +57,8 @@ static int led_pwm_probe(struct platform_device *pdev)
 	if (!pdata)
 		return -EBUSY;
 
-	leds_data = kzalloc(sizeof(struct led_pwm_data) * pdata->num_leds,
+	leds_data = devm_kzalloc(&pdev->dev,
+			sizeof(struct led_pwm_data) * pdata->num_leds,
 				GFP_KERNEL);
 	if (!leds_data)
 		return -ENOMEM;
@@ -103,8 +104,6 @@ err:
 		}
 	}
 
-	kfree(leds_data);
-
 	return ret;
 }
 
@@ -121,8 +120,6 @@ static int __devexit led_pwm_remove(struct platform_device *pdev)
 		pwm_free(leds_data[i].pwm);
 	}
 
-	kfree(leds_data);
-
 	return 0;
 }
 
diff --git a/drivers/leds/leds-regulator.c b/drivers/leds/leds-regulator.c
index df7e963bddd..25d382d60fa 100644
--- a/drivers/leds/leds-regulator.c
+++ b/drivers/leds/leds-regulator.c
@@ -158,7 +158,7 @@ static int __devinit regulator_led_probe(struct platform_device *pdev)
 		return PTR_ERR(vcc);
 	}
 
-	led = kzalloc(sizeof(*led), GFP_KERNEL);
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
 	if (led == NULL) {
 		ret = -ENOMEM;
 		goto err_vcc;
@@ -169,7 +169,7 @@ static int __devinit regulator_led_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Invalid default brightness %d\n",
 				pdata->brightness);
 		ret = -EINVAL;
-		goto err_led;
+		goto err_vcc;
 	}
 	led->value = pdata->brightness;
 
@@ -190,7 +190,7 @@ static int __devinit regulator_led_probe(struct platform_device *pdev)
 	ret = led_classdev_register(&pdev->dev, &led->cdev);
 	if (ret < 0) {
 		cancel_work_sync(&led->work);
-		goto err_led;
+		goto err_vcc;
 	}
 
 	/* to expose the default value to userspace */
@@ -201,8 +201,6 @@ static int __devinit regulator_led_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_led:
-	kfree(led);
 err_vcc:
 	regulator_put(vcc);
 	return ret;
@@ -216,7 +214,6 @@ static int __devexit regulator_led_remove(struct platform_device *pdev)
 	cancel_work_sync(&led->work);
 	regulator_led_disable(led);
 	regulator_put(led->vcc);
-	kfree(led);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-renesas-tpu.c b/drivers/leds/leds-renesas-tpu.c
index 32fe337d5c6..9ee12c28059 100644
--- a/drivers/leds/leds-renesas-tpu.c
+++ b/drivers/leds/leds-renesas-tpu.c
@@ -243,31 +243,30 @@ static int __devinit r_tpu_probe(struct platform_device *pdev)
 	struct led_renesas_tpu_config *cfg = pdev->dev.platform_data;
 	struct r_tpu_priv *p;
 	struct resource *res;
-	int ret = -ENXIO;
+	int ret;
 
 	if (!cfg) {
 		dev_err(&pdev->dev, "missing platform data\n");
 		goto err0;
 	}
 
-	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
 	if (p == NULL) {
 		dev_err(&pdev->dev, "failed to allocate driver data\n");
-		ret = -ENOMEM;
-		goto err0;
+		return -ENOMEM;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "failed to get I/O memory\n");
-		goto err1;
+		return -ENXIO;
 	}
 
 	/* map memory, let mapbase point to our channel */
 	p->mapbase = ioremap_nocache(res->start, resource_size(res));
 	if (p->mapbase == NULL) {
 		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		goto err1;
+		return -ENXIO;
 	}
 
 	/* get hold of clock */
@@ -275,7 +274,7 @@ static int __devinit r_tpu_probe(struct platform_device *pdev)
 	if (IS_ERR(p->clk)) {
 		dev_err(&pdev->dev, "cannot get clock\n");
 		ret = PTR_ERR(p->clk);
-		goto err2;
+		goto err0;
 	}
 
 	p->pdev = pdev;
@@ -294,7 +293,7 @@ static int __devinit r_tpu_probe(struct platform_device *pdev)
 	p->ldev.flags |= LED_CORE_SUSPENDRESUME;
 	ret = led_classdev_register(&pdev->dev, &p->ldev);
 	if (ret < 0)
-		goto err3;
+		goto err1;
 
 	/* max_brightness may be updated by the LED core code */
 	p->min_rate = p->ldev.max_brightness * p->refresh_rate;
@@ -302,14 +301,11 @@ static int __devinit r_tpu_probe(struct platform_device *pdev)
 	pm_runtime_enable(&pdev->dev);
 	return 0;
 
- err3:
+ err1:
 	r_tpu_set_pin(p, R_TPU_PIN_UNUSED, LED_OFF);
 	clk_put(p->clk);
- err2:
-	iounmap(p->mapbase);
- err1:
-	kfree(p);
  err0:
+	iounmap(p->mapbase);
 	return ret;
 }
 
@@ -327,7 +323,6 @@ static int __devexit r_tpu_remove(struct platform_device *pdev)
 	clk_put(p->clk);
 
 	iounmap(p->mapbase);
-	kfree(p);
 	return 0;
 }
 
diff --git a/drivers/leds/leds-s3c24xx.c b/drivers/leds/leds-s3c24xx.c
index bd0a5ed49c4..942f0ea1817 100644
--- a/drivers/leds/leds-s3c24xx.c
+++ b/drivers/leds/leds-s3c24xx.c
@@ -45,17 +45,19 @@ static void s3c24xx_led_set(struct led_classdev *led_cdev,
 {
 	struct s3c24xx_gpio_led *led = to_gpio(led_cdev);
 	struct s3c24xx_led_platdata *pd = led->pdata;
+	int state = (value ? 1 : 0) ^ (pd->flags & S3C24XX_LEDF_ACTLOW);
 
 	/* there will be a short delay between setting the output and
 	 * going from output to input when using tristate. */
 
-	s3c2410_gpio_setpin(pd->gpio, (value ? 1 : 0) ^
-			    (pd->flags & S3C24XX_LEDF_ACTLOW));
-
-	if (pd->flags & S3C24XX_LEDF_TRISTATE)
-		s3c2410_gpio_cfgpin(pd->gpio,
-			value ? S3C2410_GPIO_OUTPUT : S3C2410_GPIO_INPUT);
+	gpio_set_value(pd->gpio, state);
 
+	if (pd->flags & S3C24XX_LEDF_TRISTATE) {
+		if (value)
+			gpio_direction_output(pd->gpio, state);
+		else
+			gpio_direction_input(pd->gpio);
+	}
 }
 
 static int s3c24xx_led_remove(struct platform_device *dev)
@@ -63,7 +65,6 @@ static int s3c24xx_led_remove(struct platform_device *dev)
 	struct s3c24xx_gpio_led *led = pdev_to_gpio(dev);
 
 	led_classdev_unregister(&led->cdev);
-	kfree(led);
 
 	return 0;
 }
@@ -74,7 +75,8 @@ static int s3c24xx_led_probe(struct platform_device *dev)
 	struct s3c24xx_gpio_led *led;
 	int ret;
 
-	led = kzalloc(sizeof(struct s3c24xx_gpio_led), GFP_KERNEL);
+	led = devm_kzalloc(&dev->dev, sizeof(struct s3c24xx_gpio_led),
+			   GFP_KERNEL);
 	if (led == NULL) {
 		dev_err(&dev->dev, "No memory for device\n");
 		return -ENOMEM;
@@ -89,27 +91,27 @@ static int s3c24xx_led_probe(struct platform_device *dev)
 
 	led->pdata = pdata;
 
+	ret = devm_gpio_request(&dev->dev, pdata->gpio, "S3C24XX_LED");
+	if (ret < 0)
+		return ret;
+
 	/* no point in having a pull-up if we are always driving */
 
-	if (pdata->flags & S3C24XX_LEDF_TRISTATE) {
-		s3c2410_gpio_setpin(pdata->gpio, 0);
-		s3c2410_gpio_cfgpin(pdata->gpio, S3C2410_GPIO_INPUT);
-	} else {
-		s3c2410_gpio_pullup(pdata->gpio, 0);
-		s3c2410_gpio_setpin(pdata->gpio, 0);
-		s3c2410_gpio_cfgpin(pdata->gpio, S3C2410_GPIO_OUTPUT);
-	}
+	s3c_gpio_setpull(pdata->gpio, S3C_GPIO_PULL_NONE);
+
+	if (pdata->flags & S3C24XX_LEDF_TRISTATE)
+		gpio_direction_input(pdata->gpio);
+	else
+		gpio_direction_output(pdata->gpio,
+			pdata->flags & S3C24XX_LEDF_ACTLOW ? 1 : 0);
 
 	/* register our new led device */
 
 	ret = led_classdev_register(&dev->dev, &led->cdev);
-	if (ret < 0) {
+	if (ret < 0)
 		dev_err(&dev->dev, "led_classdev_register failed\n");
-		kfree(led);
-		return ret;
-	}
 
-	return 0;
+	return ret;
 }
 
 static struct platform_driver s3c24xx_led_driver = {
diff --git a/drivers/leds/leds-sunfire.c b/drivers/leds/leds-sunfire.c
index 1757396b20b..134d9a4b34f 100644
--- a/drivers/leds/leds-sunfire.c
+++ b/drivers/leds/leds-sunfire.c
@@ -132,15 +132,13 @@ static int __devinit sunfire_led_generic_probe(struct platform_device *pdev,
 	if (pdev->num_resources != 1) {
 		printk(KERN_ERR PFX "Wrong number of resources %d, should be 1\n",
 		       pdev->num_resources);
-		err = -EINVAL;
-		goto out;
+		return -EINVAL;
 	}
 
-	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
 		printk(KERN_ERR PFX "Could not allocate struct sunfire_drvdata\n");
-		err = -ENOMEM;
-		goto out;
+		return -ENOMEM;
 	}
 
 	for (i = 0; i < NUM_LEDS_PER_BOARD; i++) {
@@ -156,20 +154,15 @@ static int __devinit sunfire_led_generic_probe(struct platform_device *pdev,
 		if (err) {
 			printk(KERN_ERR PFX "Could not register %s LED\n",
 			       lp->name);
-			goto out_unregister_led_cdevs;
+			for (i--; i >= 0; i--)
+				led_classdev_unregister(&p->leds[i].led_cdev);
+			return err;
 		}
 	}
 
 	dev_set_drvdata(&pdev->dev, p);
 
 	return 0;
-
-out_unregister_led_cdevs:
-	for (i--; i >= 0; i--)
-		led_classdev_unregister(&p->leds[i].led_cdev);
-	kfree(p);
-out:
-	return err;
 }
 
 static int __devexit sunfire_led_generic_remove(struct platform_device *pdev)
@@ -180,8 +173,6 @@ static int __devexit sunfire_led_generic_remove(struct platform_device *pdev)
 	for (i = 0; i < NUM_LEDS_PER_BOARD; i++)
 		led_classdev_unregister(&p->leds[i].led_cdev);
 
-	kfree(p);
-
 	return 0;
 }
 
diff --git a/drivers/leds/leds-tca6507.c b/drivers/leds/leds-tca6507.c
index 6c1c14f3163..dabcf7ae8d0 100644
--- a/drivers/leds/leds-tca6507.c
+++ b/drivers/leds/leds-tca6507.c
@@ -687,7 +687,7 @@ static int __devinit tca6507_probe(struct i2c_client *client,
 			NUM_LEDS);
 		return -ENODEV;
 	}
-	tca = kzalloc(sizeof(*tca), GFP_KERNEL);
+	tca = devm_kzalloc(&client->dev, sizeof(*tca), GFP_KERNEL);
 	if (!tca)
 		return -ENOMEM;
 
@@ -727,7 +727,6 @@ exit:
 		if (tca->leds[i].led_cdev.name)
 			led_classdev_unregister(&tca->leds[i].led_cdev);
 	}
-	kfree(tca);
 	return err;
 }
 
@@ -743,7 +742,6 @@ static int __devexit tca6507_remove(struct i2c_client *client)
 	}
 	tca6507_remove_gpio(tca);
 	cancel_work_sync(&tca->work);
-	kfree(tca);
 
 	return 0;
 }
@@ -758,18 +756,7 @@ static struct i2c_driver tca6507_driver = {
 	.id_table = tca6507_id,
 };
 
-static int __init tca6507_leds_init(void)
-{
-	return i2c_add_driver(&tca6507_driver);
-}
-
-static void __exit tca6507_leds_exit(void)
-{
-	i2c_del_driver(&tca6507_driver);
-}
-
-module_init(tca6507_leds_init);
-module_exit(tca6507_leds_exit);
+module_i2c_driver(tca6507_driver);
 
 MODULE_AUTHOR("NeilBrown <neilb@suse.de>");
 MODULE_DESCRIPTION("TCA6507 LED/GPO driver");
diff --git a/drivers/leds/leds.h b/drivers/leds/leds.h
index e77c7f8dcdd..d02acd49612 100644
--- a/drivers/leds/leds.h
+++ b/drivers/leds/leds.h
@@ -17,7 +17,7 @@
 #include <linux/rwsem.h>
 #include <linux/leds.h>
 
-static inline void led_set_brightness(struct led_classdev *led_cdev,
+static inline void __led_set_brightness(struct led_classdev *led_cdev,
 					enum led_brightness value)
 {
 	if (value > led_cdev->max_brightness)
diff --git a/drivers/leds/ledtrig-backlight.c b/drivers/leds/ledtrig-backlight.c
index e2726867c5d..b941685f222 100644
--- a/drivers/leds/ledtrig-backlight.c
+++ b/drivers/leds/ledtrig-backlight.c
@@ -46,9 +46,9 @@ static int fb_notifier_callback(struct notifier_block *p,
 
 		if ((n->old_status == UNBLANK) ^ n->invert) {
 			n->brightness = led->brightness;
-			led_set_brightness(led, LED_OFF);
+			__led_set_brightness(led, LED_OFF);
 		} else {
-			led_set_brightness(led, n->brightness);
+			__led_set_brightness(led, n->brightness);
 		}
 
 		n->old_status = new_status;
@@ -87,9 +87,9 @@ static ssize_t bl_trig_invert_store(struct device *dev,
 
 	/* After inverting, we need to update the LED. */
 	if ((n->old_status == BLANK) ^ n->invert)
-		led_set_brightness(led, LED_OFF);
+		__led_set_brightness(led, LED_OFF);
 	else
-		led_set_brightness(led, n->brightness);
+		__led_set_brightness(led, n->brightness);
 
 	return num;
 }
diff --git a/drivers/leds/ledtrig-default-on.c b/drivers/leds/ledtrig-default-on.c
index a4ef54b9d50..eac1f1b1ada 100644
--- a/drivers/leds/ledtrig-default-on.c
+++ b/drivers/leds/ledtrig-default-on.c
@@ -19,7 +19,7 @@
 
 static void defon_trig_activate(struct led_classdev *led_cdev)
 {
-	led_set_brightness(led_cdev, led_cdev->max_brightness);
+	__led_set_brightness(led_cdev, led_cdev->max_brightness);
 }
 
 static struct led_trigger defon_led_trigger = {
diff --git a/drivers/leds/ledtrig-gpio.c b/drivers/leds/ledtrig-gpio.c
index f057c101b89..ba215dc42f9 100644
--- a/drivers/leds/ledtrig-gpio.c
+++ b/drivers/leds/ledtrig-gpio.c
@@ -54,12 +54,12 @@ static void gpio_trig_work(struct work_struct *work)
 
 	if (tmp) {
 		if (gpio_data->desired_brightness)
-			led_set_brightness(gpio_data->led,
+			__led_set_brightness(gpio_data->led,
 					   gpio_data->desired_brightness);
 		else
-			led_set_brightness(gpio_data->led, LED_FULL);
+			__led_set_brightness(gpio_data->led, LED_FULL);
 	} else {
-		led_set_brightness(gpio_data->led, LED_OFF);
+		__led_set_brightness(gpio_data->led, LED_OFF);
 	}
 }
 
diff --git a/drivers/leds/ledtrig-heartbeat.c b/drivers/leds/ledtrig-heartbeat.c
index a019fbb7088..1edc7463ce8 100644
--- a/drivers/leds/ledtrig-heartbeat.c
+++ b/drivers/leds/ledtrig-heartbeat.c
@@ -74,7 +74,7 @@ static void led_heartbeat_function(unsigned long data)
 		break;
 	}
 
-	led_set_brightness(led_cdev, brightness);
+	__led_set_brightness(led_cdev, brightness);
 	mod_timer(&heartbeat_data->timer, jiffies + delay);
 }
 
diff --git a/drivers/leds/ledtrig-ide-disk.c b/drivers/leds/ledtrig-ide-disk.c
index ec099fcbcb0..2cd7c0cf592 100644
--- a/drivers/leds/ledtrig-ide-disk.c
+++ b/drivers/leds/ledtrig-ide-disk.c
@@ -12,39 +12,22 @@
  */
 
 #include <linux/module.h>
-#include <linux/jiffies.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
-#include <linux/timer.h>
 #include <linux/leds.h>
 
-static void ledtrig_ide_timerfunc(unsigned long data);
+#define BLINK_DELAY 30
 
 DEFINE_LED_TRIGGER(ledtrig_ide);
-static DEFINE_TIMER(ledtrig_ide_timer, ledtrig_ide_timerfunc, 0, 0);
-static int ide_activity;
-static int ide_lastactivity;
+static unsigned long ide_blink_delay = BLINK_DELAY;
 
 void ledtrig_ide_activity(void)
 {
-	ide_activity++;
-	if (!timer_pending(&ledtrig_ide_timer))
-		mod_timer(&ledtrig_ide_timer, jiffies + msecs_to_jiffies(10));
+	led_trigger_blink_oneshot(ledtrig_ide,
+				  &ide_blink_delay, &ide_blink_delay, 0);
 }
 EXPORT_SYMBOL(ledtrig_ide_activity);
 
-static void ledtrig_ide_timerfunc(unsigned long data)
-{
-	if (ide_lastactivity != ide_activity) {
-		ide_lastactivity = ide_activity;
-		/* INT_MAX will set each LED to its maximum brightness */
-		led_trigger_event(ledtrig_ide, INT_MAX);
-		mod_timer(&ledtrig_ide_timer, jiffies + msecs_to_jiffies(10));
-	} else {
-		led_trigger_event(ledtrig_ide, LED_OFF);
-	}
-}
-
 static int __init ledtrig_ide_init(void)
 {
 	led_trigger_register_simple("ide-disk", &ledtrig_ide);
diff --git a/drivers/leds/ledtrig-oneshot.c b/drivers/leds/ledtrig-oneshot.c
new file mode 100644
index 00000000000..2c029aa5c4f
--- /dev/null
+++ b/drivers/leds/ledtrig-oneshot.c
@@ -0,0 +1,204 @@
+/*
+ * One-shot LED Trigger
+ *
+ * Copyright 2012, Fabio Baltieri <fabio.baltieri@gmail.com>
+ *
+ * Based on ledtrig-timer.c by Richard Purdie <rpurdie@openedhand.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/ctype.h>
+#include <linux/slab.h>
+#include <linux/leds.h>
+#include "leds.h"
+
+#define DEFAULT_DELAY 100
+
+struct oneshot_trig_data {
+	unsigned int invert;
+};
+
+static ssize_t led_shot(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct oneshot_trig_data *oneshot_data = led_cdev->trigger_data;
+
+	led_blink_set_oneshot(led_cdev,
+			&led_cdev->blink_delay_on, &led_cdev->blink_delay_off,
+			oneshot_data->invert);
+
+	/* content is ignored */
+	return size;
+}
+static ssize_t led_invert_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct oneshot_trig_data *oneshot_data = led_cdev->trigger_data;
+
+	return sprintf(buf, "%u\n", oneshot_data->invert);
+}
+
+static ssize_t led_invert_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct oneshot_trig_data *oneshot_data = led_cdev->trigger_data;
+	unsigned long state;
+	int ret;
+
+	ret = kstrtoul(buf, 0, &state);
+	if (ret)
+		return ret;
+
+	oneshot_data->invert = !!state;
+
+	if (oneshot_data->invert)
+		__led_set_brightness(led_cdev, LED_FULL);
+	else
+		__led_set_brightness(led_cdev, LED_OFF);
+
+	return size;
+}
+
+static ssize_t led_delay_on_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lu\n", led_cdev->blink_delay_on);
+}
+
+static ssize_t led_delay_on_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	unsigned long state;
+	int ret;
+
+	ret = kstrtoul(buf, 0, &state);
+	if (ret)
+		return ret;
+
+	led_cdev->blink_delay_on = state;
+
+	return size;
+}
+static ssize_t led_delay_off_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lu\n", led_cdev->blink_delay_off);
+}
+
+static ssize_t led_delay_off_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	unsigned long state;
+	int ret;
+
+	ret = kstrtoul(buf, 0, &state);
+	if (ret)
+		return ret;
+
+	led_cdev->blink_delay_off = state;
+
+	return size;
+}
+
+static DEVICE_ATTR(delay_on, 0644, led_delay_on_show, led_delay_on_store);
+static DEVICE_ATTR(delay_off, 0644, led_delay_off_show, led_delay_off_store);
+static DEVICE_ATTR(invert, 0644, led_invert_show, led_invert_store);
+static DEVICE_ATTR(shot, 0200, NULL, led_shot);
+
+static void oneshot_trig_activate(struct led_classdev *led_cdev)
+{
+	struct oneshot_trig_data *oneshot_data;
+	int rc;
+
+	oneshot_data = kzalloc(sizeof(*oneshot_data), GFP_KERNEL);
+	if (!oneshot_data)
+		return;
+
+	led_cdev->trigger_data = oneshot_data;
+
+	rc = device_create_file(led_cdev->dev, &dev_attr_delay_on);
+	if (rc)
+		goto err_out_trig_data;
+	rc = device_create_file(led_cdev->dev, &dev_attr_delay_off);
+	if (rc)
+		goto err_out_delayon;
+	rc = device_create_file(led_cdev->dev, &dev_attr_invert);
+	if (rc)
+		goto err_out_delayoff;
+	rc = device_create_file(led_cdev->dev, &dev_attr_shot);
+	if (rc)
+		goto err_out_invert;
+
+	led_cdev->blink_delay_on = DEFAULT_DELAY;
+	led_cdev->blink_delay_off = DEFAULT_DELAY;
+
+	led_cdev->activated = true;
+
+	return;
+
+err_out_invert:
+	device_remove_file(led_cdev->dev, &dev_attr_invert);
+err_out_delayoff:
+	device_remove_file(led_cdev->dev, &dev_attr_delay_off);
+err_out_delayon:
+	device_remove_file(led_cdev->dev, &dev_attr_delay_on);
+err_out_trig_data:
+	kfree(led_cdev->trigger_data);
+}
+
+static void oneshot_trig_deactivate(struct led_classdev *led_cdev)
+{
+	struct oneshot_trig_data *oneshot_data = led_cdev->trigger_data;
+
+	if (led_cdev->activated) {
+		device_remove_file(led_cdev->dev, &dev_attr_delay_on);
+		device_remove_file(led_cdev->dev, &dev_attr_delay_off);
+		device_remove_file(led_cdev->dev, &dev_attr_invert);
+		device_remove_file(led_cdev->dev, &dev_attr_shot);
+		kfree(oneshot_data);
+		led_cdev->activated = false;
+	}
+
+	/* Stop blinking */
+	led_set_brightness(led_cdev, LED_OFF);
+}
+
+static struct led_trigger oneshot_led_trigger = {
+	.name     = "oneshot",
+	.activate = oneshot_trig_activate,
+	.deactivate = oneshot_trig_deactivate,
+};
+
+static int __init oneshot_trig_init(void)
+{
+	return led_trigger_register(&oneshot_led_trigger);
+}
+
+static void __exit oneshot_trig_exit(void)
+{
+	led_trigger_unregister(&oneshot_led_trigger);
+}
+
+module_init(oneshot_trig_init);
+module_exit(oneshot_trig_exit);
+
+MODULE_AUTHOR("Fabio Baltieri <fabio.baltieri@gmail.com>");
+MODULE_DESCRIPTION("One-shot LED trigger");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/ledtrig-timer.c b/drivers/leds/ledtrig-timer.c
index 9010f7abaf2..f774d059220 100644
--- a/drivers/leds/ledtrig-timer.c
+++ b/drivers/leds/ledtrig-timer.c
@@ -104,7 +104,7 @@ static void timer_trig_deactivate(struct led_classdev *led_cdev)
 	}
 
 	/* Stop blinking */
-	led_brightness_set(led_cdev, LED_OFF);
+	led_set_brightness(led_cdev, LED_OFF);
 }
 
 static struct led_trigger timer_led_trigger = {
diff --git a/drivers/leds/ledtrig-transient.c b/drivers/leds/ledtrig-transient.c
index 83179f435e1..398f1042c43 100644
--- a/drivers/leds/ledtrig-transient.c
+++ b/drivers/leds/ledtrig-transient.c
@@ -41,7 +41,7 @@ static void transient_timer_function(unsigned long data)
 	struct transient_trig_data *transient_data = led_cdev->trigger_data;
 
 	transient_data->activate = 0;
-	led_set_brightness(led_cdev, transient_data->restore_state);
+	__led_set_brightness(led_cdev, transient_data->restore_state);
 }
 
 static ssize_t transient_activate_show(struct device *dev,
@@ -72,7 +72,7 @@ static ssize_t transient_activate_store(struct device *dev,
 	if (state == 0 && transient_data->activate == 1) {
 		del_timer(&transient_data->timer);
 		transient_data->activate = state;
-		led_set_brightness(led_cdev, transient_data->restore_state);
+		__led_set_brightness(led_cdev, transient_data->restore_state);
 		return size;
 	}
 
@@ -80,7 +80,7 @@ static ssize_t transient_activate_store(struct device *dev,
 	if (state == 1 && transient_data->activate == 0 &&
 	    transient_data->duration != 0) {
 		transient_data->activate = state;
-		led_set_brightness(led_cdev, transient_data->state);
+		__led_set_brightness(led_cdev, transient_data->state);
 		transient_data->restore_state =
 		    (transient_data->state == LED_FULL) ? LED_OFF : LED_FULL;
 		mod_timer(&transient_data->timer,
@@ -203,7 +203,7 @@ static void transient_trig_deactivate(struct led_classdev *led_cdev)
 
 	if (led_cdev->activated) {
 		del_timer_sync(&transient_data->timer);
-		led_set_brightness(led_cdev, transient_data->restore_state);
+		__led_set_brightness(led_cdev, transient_data->restore_state);
 		device_remove_file(led_cdev->dev, &dev_attr_activate);
 		device_remove_file(led_cdev->dev, &dev_attr_duration);
 		device_remove_file(led_cdev->dev, &dev_attr_state);
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index 10f122a3a85..d949b781f6f 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -260,15 +260,6 @@ config DM_DEBUG_BLOCK_STACK_TRACING
 
 	  If unsure, say N.
 
-config DM_DEBUG_SPACE_MAPS
-	boolean "Extra validation for thin provisioning space maps"
-	depends on DM_THIN_PROVISIONING
-	---help---
-	  Enable this for messages that may help debug problems with the
-	  space maps used by thin provisioning.
-
-          If unsure, say N.
-
 config DM_MIRROR
        tristate "Mirror target"
        depends on BLK_DEV_DM
@@ -277,13 +268,14 @@ config DM_MIRROR
          needed for live data migration tools such as 'pvmove'.
 
 config DM_RAID
-       tristate "RAID 1/4/5/6 target"
+       tristate "RAID 1/4/5/6/10 target"
        depends on BLK_DEV_DM
        select MD_RAID1
+       select MD_RAID10
        select MD_RAID456
        select BLK_DEV_MD
        ---help---
-	 A dm target that supports RAID1, RAID4, RAID5 and RAID6 mappings
+	 A dm target that supports RAID1, RAID10, RAID4, RAID5 and RAID6 mappings
 
 	 A RAID-5 set of N drives with a capacity of C MB per drive provides
 	 the capacity of C * (N - 1) MB, and protects against a failure
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 15dbe03117e..94e7f6ba2e1 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -1305,7 +1305,7 @@ int bitmap_startwrite(struct bitmap *bitmap, sector_t offset, unsigned long sect
 			prepare_to_wait(&bitmap->overflow_wait, &__wait,
 					TASK_UNINTERRUPTIBLE);
 			spin_unlock_irq(&bitmap->counts.lock);
-			io_schedule();
+			schedule();
 			finish_wait(&bitmap->overflow_wait, &__wait);
 			continue;
 		}
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 3f06df59fd8..664743d6a6c 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -42,21 +42,21 @@ struct convert_context {
 	unsigned int offset_out;
 	unsigned int idx_in;
 	unsigned int idx_out;
-	sector_t sector;
-	atomic_t pending;
+	sector_t cc_sector;
+	atomic_t cc_pending;
 };
 
 /*
  * per bio private data
  */
 struct dm_crypt_io {
-	struct dm_target *target;
+	struct crypt_config *cc;
 	struct bio *base_bio;
 	struct work_struct work;
 
 	struct convert_context ctx;
 
-	atomic_t pending;
+	atomic_t io_pending;
 	int error;
 	sector_t sector;
 	struct dm_crypt_io *base_io;
@@ -109,9 +109,6 @@ enum flags { DM_CRYPT_SUSPENDED, DM_CRYPT_KEY_VALID };
  */
 struct crypt_cpu {
 	struct ablkcipher_request *req;
-	/* ESSIV: struct crypto_cipher *essiv_tfm */
-	void *iv_private;
-	struct crypto_ablkcipher *tfms[0];
 };
 
 /*
@@ -151,6 +148,10 @@ struct crypt_config {
 	 * per_cpu_ptr() only.
 	 */
 	struct crypt_cpu __percpu *cpu;
+
+	/* ESSIV: struct crypto_cipher *essiv_tfm */
+	void *iv_private;
+	struct crypto_ablkcipher **tfms;
 	unsigned tfms_count;
 
 	/*
@@ -193,7 +194,7 @@ static struct crypt_cpu *this_crypt_config(struct crypt_config *cc)
  */
 static struct crypto_ablkcipher *any_tfm(struct crypt_config *cc)
 {
-	return __this_cpu_ptr(cc->cpu)->tfms[0];
+	return cc->tfms[0];
 }
 
 /*
@@ -258,7 +259,7 @@ static int crypt_iv_essiv_init(struct crypt_config *cc)
 	struct hash_desc desc;
 	struct scatterlist sg;
 	struct crypto_cipher *essiv_tfm;
-	int err, cpu;
+	int err;
 
 	sg_init_one(&sg, cc->key, cc->key_size);
 	desc.tfm = essiv->hash_tfm;
@@ -268,14 +269,12 @@ static int crypt_iv_essiv_init(struct crypt_config *cc)
 	if (err)
 		return err;
 
-	for_each_possible_cpu(cpu) {
-		essiv_tfm = per_cpu_ptr(cc->cpu, cpu)->iv_private,
+	essiv_tfm = cc->iv_private;
 
-		err = crypto_cipher_setkey(essiv_tfm, essiv->salt,
-				    crypto_hash_digestsize(essiv->hash_tfm));
-		if (err)
-			return err;
-	}
+	err = crypto_cipher_setkey(essiv_tfm, essiv->salt,
+			    crypto_hash_digestsize(essiv->hash_tfm));
+	if (err)
+		return err;
 
 	return 0;
 }
@@ -286,16 +285,14 @@ static int crypt_iv_essiv_wipe(struct crypt_config *cc)
 	struct iv_essiv_private *essiv = &cc->iv_gen_private.essiv;
 	unsigned salt_size = crypto_hash_digestsize(essiv->hash_tfm);
 	struct crypto_cipher *essiv_tfm;
-	int cpu, r, err = 0;
+	int r, err = 0;
 
 	memset(essiv->salt, 0, salt_size);
 
-	for_each_possible_cpu(cpu) {
-		essiv_tfm = per_cpu_ptr(cc->cpu, cpu)->iv_private;
-		r = crypto_cipher_setkey(essiv_tfm, essiv->salt, salt_size);
-		if (r)
-			err = r;
-	}
+	essiv_tfm = cc->iv_private;
+	r = crypto_cipher_setkey(essiv_tfm, essiv->salt, salt_size);
+	if (r)
+		err = r;
 
 	return err;
 }
@@ -335,8 +332,6 @@ static struct crypto_cipher *setup_essiv_cpu(struct crypt_config *cc,
 
 static void crypt_iv_essiv_dtr(struct crypt_config *cc)
 {
-	int cpu;
-	struct crypt_cpu *cpu_cc;
 	struct crypto_cipher *essiv_tfm;
 	struct iv_essiv_private *essiv = &cc->iv_gen_private.essiv;
 
@@ -346,15 +341,12 @@ static void crypt_iv_essiv_dtr(struct crypt_config *cc)
 	kzfree(essiv->salt);
 	essiv->salt = NULL;
 
-	for_each_possible_cpu(cpu) {
-		cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-		essiv_tfm = cpu_cc->iv_private;
+	essiv_tfm = cc->iv_private;
 
-		if (essiv_tfm)
-			crypto_free_cipher(essiv_tfm);
+	if (essiv_tfm)
+		crypto_free_cipher(essiv_tfm);
 
-		cpu_cc->iv_private = NULL;
-	}
+	cc->iv_private = NULL;
 }
 
 static int crypt_iv_essiv_ctr(struct crypt_config *cc, struct dm_target *ti,
@@ -363,7 +355,7 @@ static int crypt_iv_essiv_ctr(struct crypt_config *cc, struct dm_target *ti,
 	struct crypto_cipher *essiv_tfm = NULL;
 	struct crypto_hash *hash_tfm = NULL;
 	u8 *salt = NULL;
-	int err, cpu;
+	int err;
 
 	if (!opts) {
 		ti->error = "Digest algorithm missing for ESSIV mode";
@@ -388,15 +380,13 @@ static int crypt_iv_essiv_ctr(struct crypt_config *cc, struct dm_target *ti,
 	cc->iv_gen_private.essiv.salt = salt;
 	cc->iv_gen_private.essiv.hash_tfm = hash_tfm;
 
-	for_each_possible_cpu(cpu) {
-		essiv_tfm = setup_essiv_cpu(cc, ti, salt,
-					crypto_hash_digestsize(hash_tfm));
-		if (IS_ERR(essiv_tfm)) {
-			crypt_iv_essiv_dtr(cc);
-			return PTR_ERR(essiv_tfm);
-		}
-		per_cpu_ptr(cc->cpu, cpu)->iv_private = essiv_tfm;
+	essiv_tfm = setup_essiv_cpu(cc, ti, salt,
+				crypto_hash_digestsize(hash_tfm));
+	if (IS_ERR(essiv_tfm)) {
+		crypt_iv_essiv_dtr(cc);
+		return PTR_ERR(essiv_tfm);
 	}
+	cc->iv_private = essiv_tfm;
 
 	return 0;
 
@@ -410,7 +400,7 @@ bad:
 static int crypt_iv_essiv_gen(struct crypt_config *cc, u8 *iv,
 			      struct dm_crypt_request *dmreq)
 {
-	struct crypto_cipher *essiv_tfm = this_crypt_config(cc)->iv_private;
+	struct crypto_cipher *essiv_tfm = cc->iv_private;
 
 	memset(iv, 0, cc->iv_size);
 	*(__le64 *)iv = cpu_to_le64(dmreq->iv_sector);
@@ -664,7 +654,7 @@ static void crypt_convert_init(struct crypt_config *cc,
 	ctx->offset_out = 0;
 	ctx->idx_in = bio_in ? bio_in->bi_idx : 0;
 	ctx->idx_out = bio_out ? bio_out->bi_idx : 0;
-	ctx->sector = sector + cc->iv_offset;
+	ctx->cc_sector = sector + cc->iv_offset;
 	init_completion(&ctx->restart);
 }
 
@@ -695,12 +685,12 @@ static int crypt_convert_block(struct crypt_config *cc,
 	struct bio_vec *bv_out = bio_iovec_idx(ctx->bio_out, ctx->idx_out);
 	struct dm_crypt_request *dmreq;
 	u8 *iv;
-	int r = 0;
+	int r;
 
 	dmreq = dmreq_of_req(cc, req);
 	iv = iv_of_dmreq(cc, dmreq);
 
-	dmreq->iv_sector = ctx->sector;
+	dmreq->iv_sector = ctx->cc_sector;
 	dmreq->ctx = ctx;
 	sg_init_table(&dmreq->sg_in, 1);
 	sg_set_page(&dmreq->sg_in, bv_in->bv_page, 1 << SECTOR_SHIFT,
@@ -749,12 +739,12 @@ static void crypt_alloc_req(struct crypt_config *cc,
 			    struct convert_context *ctx)
 {
 	struct crypt_cpu *this_cc = this_crypt_config(cc);
-	unsigned key_index = ctx->sector & (cc->tfms_count - 1);
+	unsigned key_index = ctx->cc_sector & (cc->tfms_count - 1);
 
 	if (!this_cc->req)
 		this_cc->req = mempool_alloc(cc->req_pool, GFP_NOIO);
 
-	ablkcipher_request_set_tfm(this_cc->req, this_cc->tfms[key_index]);
+	ablkcipher_request_set_tfm(this_cc->req, cc->tfms[key_index]);
 	ablkcipher_request_set_callback(this_cc->req,
 	    CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
 	    kcryptd_async_done, dmreq_of_req(cc, this_cc->req));
@@ -769,14 +759,14 @@ static int crypt_convert(struct crypt_config *cc,
 	struct crypt_cpu *this_cc = this_crypt_config(cc);
 	int r;
 
-	atomic_set(&ctx->pending, 1);
+	atomic_set(&ctx->cc_pending, 1);
 
 	while(ctx->idx_in < ctx->bio_in->bi_vcnt &&
 	      ctx->idx_out < ctx->bio_out->bi_vcnt) {
 
 		crypt_alloc_req(cc, ctx);
 
-		atomic_inc(&ctx->pending);
+		atomic_inc(&ctx->cc_pending);
 
 		r = crypt_convert_block(cc, ctx, this_cc->req);
 
@@ -788,19 +778,19 @@ static int crypt_convert(struct crypt_config *cc,
 			/* fall through*/
 		case -EINPROGRESS:
 			this_cc->req = NULL;
-			ctx->sector++;
+			ctx->cc_sector++;
 			continue;
 
 		/* sync */
 		case 0:
-			atomic_dec(&ctx->pending);
-			ctx->sector++;
+			atomic_dec(&ctx->cc_pending);
+			ctx->cc_sector++;
 			cond_resched();
 			continue;
 
 		/* error */
 		default:
-			atomic_dec(&ctx->pending);
+			atomic_dec(&ctx->cc_pending);
 			return r;
 		}
 	}
@@ -811,7 +801,7 @@ static int crypt_convert(struct crypt_config *cc,
 static void dm_crypt_bio_destructor(struct bio *bio)
 {
 	struct dm_crypt_io *io = bio->bi_private;
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 
 	bio_free(bio, cc->bs);
 }
@@ -825,7 +815,7 @@ static void dm_crypt_bio_destructor(struct bio *bio)
 static struct bio *crypt_alloc_buffer(struct dm_crypt_io *io, unsigned size,
 				      unsigned *out_of_pages)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 	struct bio *clone;
 	unsigned int nr_iovecs = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
 	gfp_t gfp_mask = GFP_NOIO | __GFP_HIGHMEM;
@@ -884,26 +874,25 @@ static void crypt_free_buffer_pages(struct crypt_config *cc, struct bio *clone)
 	}
 }
 
-static struct dm_crypt_io *crypt_io_alloc(struct dm_target *ti,
+static struct dm_crypt_io *crypt_io_alloc(struct crypt_config *cc,
 					  struct bio *bio, sector_t sector)
 {
-	struct crypt_config *cc = ti->private;
 	struct dm_crypt_io *io;
 
 	io = mempool_alloc(cc->io_pool, GFP_NOIO);
-	io->target = ti;
+	io->cc = cc;
 	io->base_bio = bio;
 	io->sector = sector;
 	io->error = 0;
 	io->base_io = NULL;
-	atomic_set(&io->pending, 0);
+	atomic_set(&io->io_pending, 0);
 
 	return io;
 }
 
 static void crypt_inc_pending(struct dm_crypt_io *io)
 {
-	atomic_inc(&io->pending);
+	atomic_inc(&io->io_pending);
 }
 
 /*
@@ -913,12 +902,12 @@ static void crypt_inc_pending(struct dm_crypt_io *io)
  */
 static void crypt_dec_pending(struct dm_crypt_io *io)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 	struct bio *base_bio = io->base_bio;
 	struct dm_crypt_io *base_io = io->base_io;
 	int error = io->error;
 
-	if (!atomic_dec_and_test(&io->pending))
+	if (!atomic_dec_and_test(&io->io_pending))
 		return;
 
 	mempool_free(io, cc->io_pool);
@@ -952,7 +941,7 @@ static void crypt_dec_pending(struct dm_crypt_io *io)
 static void crypt_endio(struct bio *clone, int error)
 {
 	struct dm_crypt_io *io = clone->bi_private;
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 	unsigned rw = bio_data_dir(clone);
 
 	if (unlikely(!bio_flagged(clone, BIO_UPTODATE) && !error))
@@ -979,7 +968,7 @@ static void crypt_endio(struct bio *clone, int error)
 
 static void clone_init(struct dm_crypt_io *io, struct bio *clone)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 
 	clone->bi_private = io;
 	clone->bi_end_io  = crypt_endio;
@@ -990,7 +979,7 @@ static void clone_init(struct dm_crypt_io *io, struct bio *clone)
 
 static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 	struct bio *base_bio = io->base_bio;
 	struct bio *clone;
 
@@ -1038,7 +1027,7 @@ static void kcryptd_io(struct work_struct *work)
 
 static void kcryptd_queue_io(struct dm_crypt_io *io)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 
 	INIT_WORK(&io->work, kcryptd_io);
 	queue_work(cc->io_queue, &io->work);
@@ -1047,7 +1036,7 @@ static void kcryptd_queue_io(struct dm_crypt_io *io)
 static void kcryptd_crypt_write_io_submit(struct dm_crypt_io *io, int async)
 {
 	struct bio *clone = io->ctx.bio_out;
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 
 	if (unlikely(io->error < 0)) {
 		crypt_free_buffer_pages(cc, clone);
@@ -1069,7 +1058,7 @@ static void kcryptd_crypt_write_io_submit(struct dm_crypt_io *io, int async)
 
 static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 	struct bio *clone;
 	struct dm_crypt_io *new_io;
 	int crypt_finished;
@@ -1107,7 +1096,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 		if (r < 0)
 			io->error = -EIO;
 
-		crypt_finished = atomic_dec_and_test(&io->ctx.pending);
+		crypt_finished = atomic_dec_and_test(&io->ctx.cc_pending);
 
 		/* Encryption was already finished, submit io now */
 		if (crypt_finished) {
@@ -1135,7 +1124,7 @@ static void kcryptd_crypt_write_convert(struct dm_crypt_io *io)
 		 * between fragments, so switch to a new dm_crypt_io structure.
 		 */
 		if (unlikely(!crypt_finished && remaining)) {
-			new_io = crypt_io_alloc(io->target, io->base_bio,
+			new_io = crypt_io_alloc(io->cc, io->base_bio,
 						sector);
 			crypt_inc_pending(new_io);
 			crypt_convert_init(cc, &new_io->ctx, NULL,
@@ -1169,7 +1158,7 @@ static void kcryptd_crypt_read_done(struct dm_crypt_io *io)
 
 static void kcryptd_crypt_read_convert(struct dm_crypt_io *io)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 	int r = 0;
 
 	crypt_inc_pending(io);
@@ -1181,7 +1170,7 @@ static void kcryptd_crypt_read_convert(struct dm_crypt_io *io)
 	if (r < 0)
 		io->error = -EIO;
 
-	if (atomic_dec_and_test(&io->ctx.pending))
+	if (atomic_dec_and_test(&io->ctx.cc_pending))
 		kcryptd_crypt_read_done(io);
 
 	crypt_dec_pending(io);
@@ -1193,7 +1182,7 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 	struct dm_crypt_request *dmreq = async_req->data;
 	struct convert_context *ctx = dmreq->ctx;
 	struct dm_crypt_io *io = container_of(ctx, struct dm_crypt_io, ctx);
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 
 	if (error == -EINPROGRESS) {
 		complete(&ctx->restart);
@@ -1208,7 +1197,7 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 
 	mempool_free(req_of_dmreq(cc, dmreq), cc->req_pool);
 
-	if (!atomic_dec_and_test(&ctx->pending))
+	if (!atomic_dec_and_test(&ctx->cc_pending))
 		return;
 
 	if (bio_data_dir(io->base_bio) == READ)
@@ -1229,7 +1218,7 @@ static void kcryptd_crypt(struct work_struct *work)
 
 static void kcryptd_queue_crypt(struct dm_crypt_io *io)
 {
-	struct crypt_config *cc = io->target->private;
+	struct crypt_config *cc = io->cc;
 
 	INIT_WORK(&io->work, kcryptd_crypt);
 	queue_work(cc->crypt_queue, &io->work);
@@ -1241,7 +1230,6 @@ static void kcryptd_queue_crypt(struct dm_crypt_io *io)
 static int crypt_decode_key(u8 *key, char *hex, unsigned int size)
 {
 	char buffer[3];
-	char *endp;
 	unsigned int i;
 
 	buffer[2] = '\0';
@@ -1250,9 +1238,7 @@ static int crypt_decode_key(u8 *key, char *hex, unsigned int size)
 		buffer[0] = *hex++;
 		buffer[1] = *hex++;
 
-		key[i] = (u8)simple_strtoul(buffer, &endp, 16);
-
-		if (endp != &buffer[2])
+		if (kstrtou8(buffer, 16, &key[i]))
 			return -EINVAL;
 	}
 
@@ -1276,29 +1262,38 @@ static void crypt_encode_key(char *hex, u8 *key, unsigned int size)
 	}
 }
 
-static void crypt_free_tfms(struct crypt_config *cc, int cpu)
+static void crypt_free_tfms(struct crypt_config *cc)
 {
-	struct crypt_cpu *cpu_cc = per_cpu_ptr(cc->cpu, cpu);
 	unsigned i;
 
+	if (!cc->tfms)
+		return;
+
 	for (i = 0; i < cc->tfms_count; i++)
-		if (cpu_cc->tfms[i] && !IS_ERR(cpu_cc->tfms[i])) {
-			crypto_free_ablkcipher(cpu_cc->tfms[i]);
-			cpu_cc->tfms[i] = NULL;
+		if (cc->tfms[i] && !IS_ERR(cc->tfms[i])) {
+			crypto_free_ablkcipher(cc->tfms[i]);
+			cc->tfms[i] = NULL;
 		}
+
+	kfree(cc->tfms);
+	cc->tfms = NULL;
 }
 
-static int crypt_alloc_tfms(struct crypt_config *cc, int cpu, char *ciphermode)
+static int crypt_alloc_tfms(struct crypt_config *cc, char *ciphermode)
 {
-	struct crypt_cpu *cpu_cc = per_cpu_ptr(cc->cpu, cpu);
 	unsigned i;
 	int err;
 
+	cc->tfms = kmalloc(cc->tfms_count * sizeof(struct crypto_ablkcipher *),
+			   GFP_KERNEL);
+	if (!cc->tfms)
+		return -ENOMEM;
+
 	for (i = 0; i < cc->tfms_count; i++) {
-		cpu_cc->tfms[i] = crypto_alloc_ablkcipher(ciphermode, 0, 0);
-		if (IS_ERR(cpu_cc->tfms[i])) {
-			err = PTR_ERR(cpu_cc->tfms[i]);
-			crypt_free_tfms(cc, cpu);
+		cc->tfms[i] = crypto_alloc_ablkcipher(ciphermode, 0, 0);
+		if (IS_ERR(cc->tfms[i])) {
+			err = PTR_ERR(cc->tfms[i]);
+			crypt_free_tfms(cc);
 			return err;
 		}
 	}
@@ -1309,15 +1304,14 @@ static int crypt_alloc_tfms(struct crypt_config *cc, int cpu, char *ciphermode)
 static int crypt_setkey_allcpus(struct crypt_config *cc)
 {
 	unsigned subkey_size = cc->key_size >> ilog2(cc->tfms_count);
-	int cpu, err = 0, i, r;
-
-	for_each_possible_cpu(cpu) {
-		for (i = 0; i < cc->tfms_count; i++) {
-			r = crypto_ablkcipher_setkey(per_cpu_ptr(cc->cpu, cpu)->tfms[i],
-						     cc->key + (i * subkey_size), subkey_size);
-			if (r)
-				err = r;
-		}
+	int err = 0, i, r;
+
+	for (i = 0; i < cc->tfms_count; i++) {
+		r = crypto_ablkcipher_setkey(cc->tfms[i],
+					     cc->key + (i * subkey_size),
+					     subkey_size);
+		if (r)
+			err = r;
 	}
 
 	return err;
@@ -1379,9 +1373,10 @@ static void crypt_dtr(struct dm_target *ti)
 			cpu_cc = per_cpu_ptr(cc->cpu, cpu);
 			if (cpu_cc->req)
 				mempool_free(cpu_cc->req, cc->req_pool);
-			crypt_free_tfms(cc, cpu);
 		}
 
+	crypt_free_tfms(cc);
+
 	if (cc->bs)
 		bioset_free(cc->bs);
 
@@ -1414,7 +1409,7 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	struct crypt_config *cc = ti->private;
 	char *tmp, *cipher, *chainmode, *ivmode, *ivopts, *keycount;
 	char *cipher_api = NULL;
-	int cpu, ret = -EINVAL;
+	int ret = -EINVAL;
 	char dummy;
 
 	/* Convert to crypto api definition? */
@@ -1455,8 +1450,7 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	if (tmp)
 		DMWARN("Ignoring unexpected additional cipher options");
 
-	cc->cpu = __alloc_percpu(sizeof(*(cc->cpu)) +
-				 cc->tfms_count * sizeof(*(cc->cpu->tfms)),
+	cc->cpu = __alloc_percpu(sizeof(*(cc->cpu)),
 				 __alignof__(struct crypt_cpu));
 	if (!cc->cpu) {
 		ti->error = "Cannot allocate per cpu state";
@@ -1489,12 +1483,10 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	}
 
 	/* Allocate cipher */
-	for_each_possible_cpu(cpu) {
-		ret = crypt_alloc_tfms(cc, cpu, cipher_api);
-		if (ret < 0) {
-			ti->error = "Error allocating crypto tfm";
-			goto bad;
-		}
+	ret = crypt_alloc_tfms(cc, cipher_api);
+	if (ret < 0) {
+		ti->error = "Error allocating crypto tfm";
+		goto bad;
 	}
 
 	/* Initialize and set key */
@@ -1702,7 +1694,7 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	}
 
 	ti->num_flush_requests = 1;
-	ti->discard_zeroes_data_unsupported = 1;
+	ti->discard_zeroes_data_unsupported = true;
 
 	return 0;
 
@@ -1715,7 +1707,7 @@ static int crypt_map(struct dm_target *ti, struct bio *bio,
 		     union map_info *map_context)
 {
 	struct dm_crypt_io *io;
-	struct crypt_config *cc;
+	struct crypt_config *cc = ti->private;
 
 	/*
 	 * If bio is REQ_FLUSH or REQ_DISCARD, just bypass crypt queues.
@@ -1723,14 +1715,13 @@ static int crypt_map(struct dm_target *ti, struct bio *bio,
 	 * - for REQ_DISCARD caller must use flush if IO ordering matters
 	 */
 	if (unlikely(bio->bi_rw & (REQ_FLUSH | REQ_DISCARD))) {
-		cc = ti->private;
 		bio->bi_bdev = cc->dev->bdev;
 		if (bio_sectors(bio))
 			bio->bi_sector = cc->start + dm_target_offset(ti, bio->bi_sector);
 		return DM_MAPIO_REMAPPED;
 	}
 
-	io = crypt_io_alloc(ti, bio, dm_target_offset(ti, bio->bi_sector));
+	io = crypt_io_alloc(cc, bio, dm_target_offset(ti, bio->bi_sector));
 
 	if (bio_data_dir(io->base_bio) == READ) {
 		if (kcryptd_io_read(io, GFP_NOWAIT))
@@ -1742,7 +1733,7 @@ static int crypt_map(struct dm_target *ti, struct bio *bio,
 }
 
 static int crypt_status(struct dm_target *ti, status_type_t type,
-			char *result, unsigned int maxlen)
+			unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct crypt_config *cc = ti->private;
 	unsigned int sz = 0;
diff --git a/drivers/md/dm-delay.c b/drivers/md/dm-delay.c
index 2dc22dddb2a..f53846f9ab5 100644
--- a/drivers/md/dm-delay.c
+++ b/drivers/md/dm-delay.c
@@ -295,7 +295,7 @@ static int delay_map(struct dm_target *ti, struct bio *bio,
 }
 
 static int delay_status(struct dm_target *ti, status_type_t type,
-			char *result, unsigned maxlen)
+			unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct delay_c *dc = ti->private;
 	int sz = 0;
diff --git a/drivers/md/dm-exception-store.c b/drivers/md/dm-exception-store.c
index aa70f7d43a1..ebaa4f803ee 100644
--- a/drivers/md/dm-exception-store.c
+++ b/drivers/md/dm-exception-store.c
@@ -142,24 +142,19 @@ EXPORT_SYMBOL(dm_exception_store_type_unregister);
 static int set_chunk_size(struct dm_exception_store *store,
 			  const char *chunk_size_arg, char **error)
 {
-	unsigned long chunk_size_ulong;
-	char *value;
+	unsigned chunk_size;
 
-	chunk_size_ulong = simple_strtoul(chunk_size_arg, &value, 10);
-	if (*chunk_size_arg == '\0' || *value != '\0' ||
-	    chunk_size_ulong > UINT_MAX) {
+	if (kstrtouint(chunk_size_arg, 10, &chunk_size)) {
 		*error = "Invalid chunk size";
 		return -EINVAL;
 	}
 
-	if (!chunk_size_ulong) {
+	if (!chunk_size) {
 		store->chunk_size = store->chunk_mask = store->chunk_shift = 0;
 		return 0;
 	}
 
-	return dm_exception_store_set_chunk_size(store,
-						 (unsigned) chunk_size_ulong,
-						 error);
+	return dm_exception_store_set_chunk_size(store, chunk_size, error);
 }
 
 int dm_exception_store_set_chunk_size(struct dm_exception_store *store,
diff --git a/drivers/md/dm-flakey.c b/drivers/md/dm-flakey.c
index ac49c01f1a4..cc15543a6ad 100644
--- a/drivers/md/dm-flakey.c
+++ b/drivers/md/dm-flakey.c
@@ -333,7 +333,7 @@ static int flakey_end_io(struct dm_target *ti, struct bio *bio,
 }
 
 static int flakey_status(struct dm_target *ti, status_type_t type,
-			 char *result, unsigned int maxlen)
+			 unsigned status_flags, char *result, unsigned maxlen)
 {
 	unsigned sz = 0;
 	struct flakey_c *fc = ti->private;
diff --git a/drivers/md/dm-ioctl.c b/drivers/md/dm-ioctl.c
index a1a3e6df17b..afd95986d09 100644
--- a/drivers/md/dm-ioctl.c
+++ b/drivers/md/dm-ioctl.c
@@ -1054,6 +1054,7 @@ static void retrieve_status(struct dm_table *table,
 	char *outbuf, *outptr;
 	status_type_t type;
 	size_t remaining, len, used = 0;
+	unsigned status_flags = 0;
 
 	outptr = outbuf = get_result_buffer(param, param_size, &len);
 
@@ -1090,7 +1091,9 @@ static void retrieve_status(struct dm_table *table,
 
 		/* Get the status/table string from the target driver */
 		if (ti->type->status) {
-			if (ti->type->status(ti, type, outptr, remaining)) {
+			if (param->flags & DM_NOFLUSH_FLAG)
+				status_flags |= DM_STATUS_NOFLUSH_FLAG;
+			if (ti->type->status(ti, type, status_flags, outptr, remaining)) {
 				param->flags |= DM_BUFFER_FULL_FLAG;
 				break;
 			}
diff --git a/drivers/md/dm-linear.c b/drivers/md/dm-linear.c
index 3639eeab604..1bf19a93eef 100644
--- a/drivers/md/dm-linear.c
+++ b/drivers/md/dm-linear.c
@@ -96,7 +96,7 @@ static int linear_map(struct dm_target *ti, struct bio *bio,
 }
 
 static int linear_status(struct dm_target *ti, status_type_t type,
-			 char *result, unsigned int maxlen)
+			 unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct linear_c *lc = (struct linear_c *) ti->private;
 
diff --git a/drivers/md/dm-log.c b/drivers/md/dm-log.c
index 65ebaebf502..627d19186d5 100644
--- a/drivers/md/dm-log.c
+++ b/drivers/md/dm-log.c
@@ -571,16 +571,6 @@ static void disk_dtr(struct dm_dirty_log *log)
 	destroy_log_context(lc);
 }
 
-static int count_bits32(uint32_t *addr, unsigned size)
-{
-	int count = 0, i;
-
-	for (i = 0; i < size; i++) {
-		count += hweight32(*(addr+i));
-	}
-	return count;
-}
-
 static void fail_log_device(struct log_c *lc)
 {
 	if (lc->log_dev_failed)
@@ -629,7 +619,8 @@ static int disk_resume(struct dm_dirty_log *log)
 
 	/* copy clean across to sync */
 	memcpy(lc->sync_bits, lc->clean_bits, size);
-	lc->sync_count = count_bits32(lc->clean_bits, lc->bitset_uint32_count);
+	lc->sync_count = memweight(lc->clean_bits,
+				lc->bitset_uint32_count * sizeof(uint32_t));
 	lc->sync_search = 0;
 
 	/* set the correct number of regions in the header */
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index 638dae048b4..d8abb90a6c2 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -85,6 +85,7 @@ struct multipath {
 	unsigned queue_io:1;		/* Must we queue all I/O? */
 	unsigned queue_if_no_path:1;	/* Queue I/O if last path fails? */
 	unsigned saved_queue_if_no_path:1; /* Saved state during suspension */
+	unsigned retain_attached_hw_handler:1; /* If there's already a hw_handler present, don't change it. */
 
 	unsigned pg_init_retries;	/* Number of times to retry pg_init */
 	unsigned pg_init_count;		/* Number of times pg_init called */
@@ -568,6 +569,8 @@ static struct pgpath *parse_path(struct dm_arg_set *as, struct path_selector *ps
 	int r;
 	struct pgpath *p;
 	struct multipath *m = ti->private;
+	struct request_queue *q = NULL;
+	const char *attached_handler_name;
 
 	/* we need at least a path arg */
 	if (as->argc < 1) {
@@ -586,13 +589,37 @@ static struct pgpath *parse_path(struct dm_arg_set *as, struct path_selector *ps
 		goto bad;
 	}
 
-	if (m->hw_handler_name) {
-		struct request_queue *q = bdev_get_queue(p->path.dev->bdev);
+	if (m->retain_attached_hw_handler || m->hw_handler_name)
+		q = bdev_get_queue(p->path.dev->bdev);
+
+	if (m->retain_attached_hw_handler) {
+		attached_handler_name = scsi_dh_attached_handler_name(q, GFP_KERNEL);
+		if (attached_handler_name) {
+			/*
+			 * Reset hw_handler_name to match the attached handler
+			 * and clear any hw_handler_params associated with the
+			 * ignored handler.
+			 *
+			 * NB. This modifies the table line to show the actual
+			 * handler instead of the original table passed in.
+			 */
+			kfree(m->hw_handler_name);
+			m->hw_handler_name = attached_handler_name;
+
+			kfree(m->hw_handler_params);
+			m->hw_handler_params = NULL;
+		}
+	}
 
+	if (m->hw_handler_name) {
+		/*
+		 * Increments scsi_dh reference, even when using an
+		 * already-attached handler.
+		 */
 		r = scsi_dh_attach(q, m->hw_handler_name);
 		if (r == -EBUSY) {
 			/*
-			 * Already attached to different hw_handler,
+			 * Already attached to different hw_handler:
 			 * try to reattach with correct one.
 			 */
 			scsi_dh_detach(q);
@@ -760,7 +787,7 @@ static int parse_features(struct dm_arg_set *as, struct multipath *m)
 	const char *arg_name;
 
 	static struct dm_arg _args[] = {
-		{0, 5, "invalid number of feature args"},
+		{0, 6, "invalid number of feature args"},
 		{1, 50, "pg_init_retries must be between 1 and 50"},
 		{0, 60000, "pg_init_delay_msecs must be between 0 and 60000"},
 	};
@@ -781,6 +808,11 @@ static int parse_features(struct dm_arg_set *as, struct multipath *m)
 			continue;
 		}
 
+		if (!strcasecmp(arg_name, "retain_attached_hw_handler")) {
+			m->retain_attached_hw_handler = 1;
+			continue;
+		}
+
 		if (!strcasecmp(arg_name, "pg_init_retries") &&
 		    (argc >= 1)) {
 			r = dm_read_arg(_args + 1, as, &m->pg_init_retries, &ti->error);
@@ -1346,7 +1378,7 @@ static void multipath_resume(struct dm_target *ti)
  *      num_paths num_selector_args [path_dev [selector_args]* ]+ ]+
  */
 static int multipath_status(struct dm_target *ti, status_type_t type,
-			    char *result, unsigned int maxlen)
+			    unsigned status_flags, char *result, unsigned maxlen)
 {
 	int sz = 0;
 	unsigned long flags;
@@ -1364,13 +1396,16 @@ static int multipath_status(struct dm_target *ti, status_type_t type,
 	else {
 		DMEMIT("%u ", m->queue_if_no_path +
 			      (m->pg_init_retries > 0) * 2 +
-			      (m->pg_init_delay_msecs != DM_PG_INIT_DELAY_DEFAULT) * 2);
+			      (m->pg_init_delay_msecs != DM_PG_INIT_DELAY_DEFAULT) * 2 +
+			      m->retain_attached_hw_handler);
 		if (m->queue_if_no_path)
 			DMEMIT("queue_if_no_path ");
 		if (m->pg_init_retries)
 			DMEMIT("pg_init_retries %u ", m->pg_init_retries);
 		if (m->pg_init_delay_msecs != DM_PG_INIT_DELAY_DEFAULT)
 			DMEMIT("pg_init_delay_msecs %u ", m->pg_init_delay_msecs);
+		if (m->retain_attached_hw_handler)
+			DMEMIT("retain_attached_hw_handler ");
 	}
 
 	if (!m->hw_handler_name || type == STATUSTYPE_INFO)
@@ -1656,7 +1691,7 @@ out:
  *---------------------------------------------------------------*/
 static struct target_type multipath_target = {
 	.name = "multipath",
-	.version = {1, 4, 0},
+	.version = {1, 5, 0},
 	.module = THIS_MODULE,
 	.ctr = multipath_ctr,
 	.dtr = multipath_dtr,
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 017c34d78d6..982e3e390c4 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -11,6 +11,7 @@
 #include "md.h"
 #include "raid1.h"
 #include "raid5.h"
+#include "raid10.h"
 #include "bitmap.h"
 
 #include <linux/device-mapper.h>
@@ -52,7 +53,10 @@ struct raid_dev {
 #define DMPF_MAX_RECOVERY_RATE 0x20
 #define DMPF_MAX_WRITE_BEHIND  0x40
 #define DMPF_STRIPE_CACHE      0x80
-#define DMPF_REGION_SIZE       0X100
+#define DMPF_REGION_SIZE       0x100
+#define DMPF_RAID10_COPIES     0x200
+#define DMPF_RAID10_FORMAT     0x400
+
 struct raid_set {
 	struct dm_target *ti;
 
@@ -76,6 +80,7 @@ static struct raid_type {
 	const unsigned algorithm;	/* RAID algorithm. */
 } raid_types[] = {
 	{"raid1",    "RAID1 (mirroring)",               0, 2, 1, 0 /* NONE */},
+	{"raid10",   "RAID10 (striped mirrors)",        0, 2, 10, UINT_MAX /* Varies */},
 	{"raid4",    "RAID4 (dedicated parity disk)",	1, 2, 5, ALGORITHM_PARITY_0},
 	{"raid5_la", "RAID5 (left asymmetric)",		1, 2, 5, ALGORITHM_LEFT_ASYMMETRIC},
 	{"raid5_ra", "RAID5 (right asymmetric)",	1, 2, 5, ALGORITHM_RIGHT_ASYMMETRIC},
@@ -86,6 +91,17 @@ static struct raid_type {
 	{"raid6_nc", "RAID6 (N continue)",		2, 4, 6, ALGORITHM_ROTATING_N_CONTINUE}
 };
 
+static unsigned raid10_md_layout_to_copies(int layout)
+{
+	return layout & 0xFF;
+}
+
+static int raid10_format_to_md_layout(char *format, unsigned copies)
+{
+	/* 1 "far" copy, and 'copies' "near" copies */
+	return (1 << 8) | (copies & 0xFF);
+}
+
 static struct raid_type *get_raid_type(char *name)
 {
 	int i;
@@ -101,20 +117,12 @@ static struct raid_set *context_alloc(struct dm_target *ti, struct raid_type *ra
 {
 	unsigned i;
 	struct raid_set *rs;
-	sector_t sectors_per_dev;
 
 	if (raid_devs <= raid_type->parity_devs) {
 		ti->error = "Insufficient number of devices";
 		return ERR_PTR(-EINVAL);
 	}
 
-	sectors_per_dev = ti->len;
-	if ((raid_type->level > 1) &&
-	    sector_div(sectors_per_dev, (raid_devs - raid_type->parity_devs))) {
-		ti->error = "Target length not divisible by number of data devices";
-		return ERR_PTR(-EINVAL);
-	}
-
 	rs = kzalloc(sizeof(*rs) + raid_devs * sizeof(rs->dev[0]), GFP_KERNEL);
 	if (!rs) {
 		ti->error = "Cannot allocate raid context";
@@ -128,7 +136,6 @@ static struct raid_set *context_alloc(struct dm_target *ti, struct raid_type *ra
 	rs->md.raid_disks = raid_devs;
 	rs->md.level = raid_type->level;
 	rs->md.new_level = rs->md.level;
-	rs->md.dev_sectors = sectors_per_dev;
 	rs->md.layout = raid_type->algorithm;
 	rs->md.new_layout = rs->md.layout;
 	rs->md.delta_disks = 0;
@@ -143,6 +150,7 @@ static struct raid_set *context_alloc(struct dm_target *ti, struct raid_type *ra
 	 *  rs->md.external
 	 *  rs->md.chunk_sectors
 	 *  rs->md.new_chunk_sectors
+	 *  rs->md.dev_sectors
 	 */
 
 	return rs;
@@ -347,12 +355,20 @@ static int validate_region_size(struct raid_set *rs, unsigned long region_size)
  *    [max_write_behind <sectors>]	See '-write-behind=' (man mdadm)
  *    [stripe_cache <sectors>]		Stripe cache size for higher RAIDs
  *    [region_size <sectors>]           Defines granularity of bitmap
+ *
+ * RAID10-only options:
+ *    [raid10_copies <# copies>]        Number of copies.  (Default: 2)
+ *    [raid10_format <near>]            Layout algorithm.  (Default: near)
  */
 static int parse_raid_params(struct raid_set *rs, char **argv,
 			     unsigned num_raid_params)
 {
+	char *raid10_format = "near";
+	unsigned raid10_copies = 2;
 	unsigned i, rebuild_cnt = 0;
 	unsigned long value, region_size = 0;
+	sector_t sectors_per_dev = rs->ti->len;
+	sector_t max_io_len;
 	char *key;
 
 	/*
@@ -422,20 +438,53 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 		}
 
 		key = argv[i++];
+
+		/* Parameters that take a string value are checked here. */
+		if (!strcasecmp(key, "raid10_format")) {
+			if (rs->raid_type->level != 10) {
+				rs->ti->error = "'raid10_format' is an invalid parameter for this RAID type";
+				return -EINVAL;
+			}
+			if (strcmp("near", argv[i])) {
+				rs->ti->error = "Invalid 'raid10_format' value given";
+				return -EINVAL;
+			}
+			raid10_format = argv[i];
+			rs->print_flags |= DMPF_RAID10_FORMAT;
+			continue;
+		}
+
 		if (strict_strtoul(argv[i], 10, &value) < 0) {
 			rs->ti->error = "Bad numerical argument given in raid params";
 			return -EINVAL;
 		}
 
+		/* Parameters that take a numeric value are checked here */
 		if (!strcasecmp(key, "rebuild")) {
 			rebuild_cnt++;
-			if (((rs->raid_type->level != 1) &&
-			     (rebuild_cnt > rs->raid_type->parity_devs)) ||
-			    ((rs->raid_type->level == 1) &&
-			     (rebuild_cnt > (rs->md.raid_disks - 1)))) {
-				rs->ti->error = "Too many rebuild devices specified for given RAID type";
+
+			switch (rs->raid_type->level) {
+			case 1:
+				if (rebuild_cnt >= rs->md.raid_disks) {
+					rs->ti->error = "Too many rebuild devices specified";
+					return -EINVAL;
+				}
+				break;
+			case 4:
+			case 5:
+			case 6:
+				if (rebuild_cnt > rs->raid_type->parity_devs) {
+					rs->ti->error = "Too many rebuild devices specified for given RAID type";
+					return -EINVAL;
+				}
+				break;
+			case 10:
+			default:
+				DMERR("The rebuild parameter is not supported for %s", rs->raid_type->name);
+				rs->ti->error = "Rebuild not supported for this RAID type";
 				return -EINVAL;
 			}
+
 			if (value > rs->md.raid_disks) {
 				rs->ti->error = "Invalid rebuild index given";
 				return -EINVAL;
@@ -486,7 +535,8 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 			 */
 			value /= 2;
 
-			if (rs->raid_type->level < 5) {
+			if ((rs->raid_type->level != 5) &&
+			    (rs->raid_type->level != 6)) {
 				rs->ti->error = "Inappropriate argument: stripe_cache";
 				return -EINVAL;
 			}
@@ -511,6 +561,14 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 		} else if (!strcasecmp(key, "region_size")) {
 			rs->print_flags |= DMPF_REGION_SIZE;
 			region_size = value;
+		} else if (!strcasecmp(key, "raid10_copies") &&
+			   (rs->raid_type->level == 10)) {
+			if ((value < 2) || (value > 0xFF)) {
+				rs->ti->error = "Bad value for 'raid10_copies'";
+				return -EINVAL;
+			}
+			rs->print_flags |= DMPF_RAID10_COPIES;
+			raid10_copies = value;
 		} else {
 			DMERR("Unable to parse RAID parameter: %s", key);
 			rs->ti->error = "Unable to parse RAID parameters";
@@ -522,14 +580,33 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 		return -EINVAL;
 
 	if (rs->md.chunk_sectors)
-		rs->ti->split_io = rs->md.chunk_sectors;
+		max_io_len = rs->md.chunk_sectors;
 	else
-		rs->ti->split_io = region_size;
+		max_io_len = region_size;
 
-	if (rs->md.chunk_sectors)
-		rs->ti->split_io = rs->md.chunk_sectors;
-	else
-		rs->ti->split_io = region_size;
+	if (dm_set_target_max_io_len(rs->ti, max_io_len))
+		return -EINVAL;
+
+	if (rs->raid_type->level == 10) {
+		if (raid10_copies > rs->md.raid_disks) {
+			rs->ti->error = "Not enough devices to satisfy specification";
+			return -EINVAL;
+		}
+
+		/* (Len * #mirrors) / #devices */
+		sectors_per_dev = rs->ti->len * raid10_copies;
+		sector_div(sectors_per_dev, rs->md.raid_disks);
+
+		rs->md.layout = raid10_format_to_md_layout(raid10_format,
+							   raid10_copies);
+		rs->md.new_layout = rs->md.layout;
+	} else if ((rs->raid_type->level > 1) &&
+		   sector_div(sectors_per_dev,
+			      (rs->md.raid_disks - rs->raid_type->parity_devs))) {
+		rs->ti->error = "Target length not divisible by number of data devices";
+		return -EINVAL;
+	}
+	rs->md.dev_sectors = sectors_per_dev;
 
 	/* Assume there are no metadata devices until the drives are parsed */
 	rs->md.persistent = 0;
@@ -552,6 +629,9 @@ static int raid_is_congested(struct dm_target_callbacks *cb, int bits)
 	if (rs->raid_type->level == 1)
 		return md_raid1_congested(&rs->md, bits);
 
+	if (rs->raid_type->level == 10)
+		return md_raid10_congested(&rs->md, bits);
+
 	return md_raid5_congested(&rs->md, bits);
 }
 
@@ -870,6 +950,9 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 	case 6:
 		redundancy = rs->raid_type->parity_devs;
 		break;
+	case 10:
+		redundancy = raid10_md_layout_to_copies(mddev->layout) - 1;
+		break;
 	default:
 		ti->error = "Unknown RAID type";
 		return -EINVAL;
@@ -1035,12 +1118,19 @@ static int raid_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		goto bad;
 	}
 
+	if (ti->len != rs->md.array_sectors) {
+		ti->error = "Array size does not match requested target length";
+		ret = -EINVAL;
+		goto size_mismatch;
+	}
 	rs->callbacks.congested_fn = raid_is_congested;
 	dm_table_add_target_callbacks(ti->table, &rs->callbacks);
 
 	mddev_suspend(&rs->md);
 	return 0;
 
+size_mismatch:
+	md_stop(&rs->md);
 bad:
 	context_free(rs);
 
@@ -1067,7 +1157,7 @@ static int raid_map(struct dm_target *ti, struct bio *bio, union map_info *map_c
 }
 
 static int raid_status(struct dm_target *ti, status_type_t type,
-		       char *result, unsigned maxlen)
+		       unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct raid_set *rs = ti->private;
 	unsigned raid_param_cnt = 1; /* at least 1 for chunksize */
@@ -1189,6 +1279,13 @@ static int raid_status(struct dm_target *ti, status_type_t type,
 			DMEMIT(" region_size %lu",
 			       rs->md.bitmap_info.chunksize >> 9);
 
+		if (rs->print_flags & DMPF_RAID10_COPIES)
+			DMEMIT(" raid10_copies %u",
+			       raid10_md_layout_to_copies(rs->md.layout));
+
+		if (rs->print_flags & DMPF_RAID10_FORMAT)
+			DMEMIT(" raid10_format near");
+
 		DMEMIT(" %d", rs->md.raid_disks);
 		for (i = 0; i < rs->md.raid_disks; i++) {
 			if (rs->dev[i].meta_dev)
@@ -1263,7 +1360,7 @@ static void raid_resume(struct dm_target *ti)
 
 static struct target_type raid_target = {
 	.name = "raid",
-	.version = {1, 2, 0},
+	.version = {1, 3, 0},
 	.module = THIS_MODULE,
 	.ctr = raid_ctr,
 	.dtr = raid_dtr,
@@ -1290,6 +1387,8 @@ module_init(dm_raid_init);
 module_exit(dm_raid_exit);
 
 MODULE_DESCRIPTION(DM_NAME " raid4/5/6 target");
+MODULE_ALIAS("dm-raid1");
+MODULE_ALIAS("dm-raid10");
 MODULE_ALIAS("dm-raid4");
 MODULE_ALIAS("dm-raid5");
 MODULE_ALIAS("dm-raid6");
diff --git a/drivers/md/dm-raid1.c b/drivers/md/dm-raid1.c
index d039de8322f..bc5ddba8045 100644
--- a/drivers/md/dm-raid1.c
+++ b/drivers/md/dm-raid1.c
@@ -1081,9 +1081,14 @@ static int mirror_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	}
 
 	ti->private = ms;
-	ti->split_io = dm_rh_get_region_size(ms->rh);
+
+	r = dm_set_target_max_io_len(ti, dm_rh_get_region_size(ms->rh));
+	if (r)
+		goto err_free_context;
+
 	ti->num_flush_requests = 1;
 	ti->num_discard_requests = 1;
+	ti->discard_zeroes_data_unsupported = true;
 
 	ms->kmirrord_wq = alloc_workqueue("kmirrord",
 					  WQ_NON_REENTRANT | WQ_MEM_RECLAIM, 0);
@@ -1214,7 +1219,7 @@ static int mirror_end_io(struct dm_target *ti, struct bio *bio,
 	 * We need to dec pending if this was a write.
 	 */
 	if (rw == WRITE) {
-		if (!(bio->bi_rw & REQ_FLUSH))
+		if (!(bio->bi_rw & (REQ_FLUSH | REQ_DISCARD)))
 			dm_rh_dec(ms->rh, map_context->ll);
 		return error;
 	}
@@ -1362,7 +1367,7 @@ static char device_status_char(struct mirror *m)
 
 
 static int mirror_status(struct dm_target *ti, status_type_t type,
-			 char *result, unsigned int maxlen)
+			 unsigned status_flags, char *result, unsigned maxlen)
 {
 	unsigned int m, sz = 0;
 	struct mirror_set *ms = (struct mirror_set *) ti->private;
diff --git a/drivers/md/dm-region-hash.c b/drivers/md/dm-region-hash.c
index 7771ed21218..69732e03eb3 100644
--- a/drivers/md/dm-region-hash.c
+++ b/drivers/md/dm-region-hash.c
@@ -404,6 +404,9 @@ void dm_rh_mark_nosync(struct dm_region_hash *rh, struct bio *bio)
 		return;
 	}
 
+	if (bio->bi_rw & REQ_DISCARD)
+		return;
+
 	/* We must inform the log that the sync count has changed. */
 	log->type->set_region_sync(log, region, 0);
 
@@ -524,7 +527,7 @@ void dm_rh_inc_pending(struct dm_region_hash *rh, struct bio_list *bios)
 	struct bio *bio;
 
 	for (bio = bios->head; bio; bio = bio->bi_next) {
-		if (bio->bi_rw & REQ_FLUSH)
+		if (bio->bi_rw & (REQ_FLUSH | REQ_DISCARD))
 			continue;
 		rh_inc(rh, dm_rh_bio_to_region(rh, bio));
 	}
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index 6f758870fc1..a143921feaf 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -691,7 +691,7 @@ static int dm_add_exception(void *context, chunk_t old, chunk_t new)
  * Return a minimum chunk size of all snapshots that have the specified origin.
  * Return zero if the origin has no snapshots.
  */
-static sector_t __minimum_chunk_size(struct origin *o)
+static uint32_t __minimum_chunk_size(struct origin *o)
 {
 	struct dm_snapshot *snap;
 	unsigned chunk_size = 0;
@@ -701,7 +701,7 @@ static sector_t __minimum_chunk_size(struct origin *o)
 			chunk_size = min_not_zero(chunk_size,
 						  snap->store->chunk_size);
 
-	return chunk_size;
+	return (uint32_t) chunk_size;
 }
 
 /*
@@ -1172,7 +1172,10 @@ static int snapshot_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 		ti->error = "Chunk size not set";
 		goto bad_read_metadata;
 	}
-	ti->split_io = s->store->chunk_size;
+
+	r = dm_set_target_max_io_len(ti, s->store->chunk_size);
+	if (r)
+		goto bad_read_metadata;
 
 	return 0;
 
@@ -1239,7 +1242,7 @@ static void __handover_exceptions(struct dm_snapshot *snap_src,
 	snap_dest->store->snap = snap_dest;
 	snap_src->store->snap = snap_src;
 
-	snap_dest->ti->split_io = snap_dest->store->chunk_size;
+	snap_dest->ti->max_io_len = snap_dest->store->chunk_size;
 	snap_dest->valid = snap_src->valid;
 
 	/*
@@ -1817,9 +1820,9 @@ static void snapshot_resume(struct dm_target *ti)
 	up_write(&s->lock);
 }
 
-static sector_t get_origin_minimum_chunksize(struct block_device *bdev)
+static uint32_t get_origin_minimum_chunksize(struct block_device *bdev)
 {
-	sector_t min_chunksize;
+	uint32_t min_chunksize;
 
 	down_read(&_origins_lock);
 	min_chunksize = __minimum_chunk_size(__lookup_origin(bdev));
@@ -1838,15 +1841,15 @@ static void snapshot_merge_resume(struct dm_target *ti)
 	snapshot_resume(ti);
 
 	/*
-	 * snapshot-merge acts as an origin, so set ti->split_io
+	 * snapshot-merge acts as an origin, so set ti->max_io_len
 	 */
-	ti->split_io = get_origin_minimum_chunksize(s->origin->bdev);
+	ti->max_io_len = get_origin_minimum_chunksize(s->origin->bdev);
 
 	start_merge(s);
 }
 
 static int snapshot_status(struct dm_target *ti, status_type_t type,
-			   char *result, unsigned int maxlen)
+			   unsigned status_flags, char *result, unsigned maxlen)
 {
 	unsigned sz = 0;
 	struct dm_snapshot *snap = ti->private;
@@ -2073,12 +2076,12 @@ static int origin_write_extent(struct dm_snapshot *merging_snap,
 	struct origin *o;
 
 	/*
-	 * The origin's __minimum_chunk_size() got stored in split_io
+	 * The origin's __minimum_chunk_size() got stored in max_io_len
 	 * by snapshot_merge_resume().
 	 */
 	down_read(&_origins_lock);
 	o = __lookup_origin(merging_snap->origin->bdev);
-	for (n = 0; n < size; n += merging_snap->ti->split_io)
+	for (n = 0; n < size; n += merging_snap->ti->max_io_len)
 		if (__origin_write(&o->snapshots, sector + n, NULL) ==
 		    DM_MAPIO_SUBMITTED)
 			must_wait = 1;
@@ -2138,18 +2141,18 @@ static int origin_map(struct dm_target *ti, struct bio *bio,
 }
 
 /*
- * Set the target "split_io" field to the minimum of all the snapshots'
+ * Set the target "max_io_len" field to the minimum of all the snapshots'
  * chunk sizes.
  */
 static void origin_resume(struct dm_target *ti)
 {
 	struct dm_dev *dev = ti->private;
 
-	ti->split_io = get_origin_minimum_chunksize(dev->bdev);
+	ti->max_io_len = get_origin_minimum_chunksize(dev->bdev);
 }
 
-static int origin_status(struct dm_target *ti, status_type_t type, char *result,
-			 unsigned int maxlen)
+static int origin_status(struct dm_target *ti, status_type_t type,
+			 unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct dm_dev *dev = ti->private;
 
@@ -2176,7 +2179,6 @@ static int origin_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
 		return max_size;
 
 	bvm->bi_bdev = dev->bdev;
-	bvm->bi_sector = bvm->bi_sector;
 
 	return min(max_size, q->merge_bvec_fn(q, bvm, biovec));
 }
diff --git a/drivers/md/dm-stripe.c b/drivers/md/dm-stripe.c
index 35c94ff24ad..a087bf2a8d6 100644
--- a/drivers/md/dm-stripe.c
+++ b/drivers/md/dm-stripe.c
@@ -26,14 +26,12 @@ struct stripe {
 struct stripe_c {
 	uint32_t stripes;
 	int stripes_shift;
-	sector_t stripes_mask;
 
 	/* The size of this target / num. stripes */
 	sector_t stripe_width;
 
-	/* stripe chunk size */
-	uint32_t chunk_shift;
-	sector_t chunk_mask;
+	uint32_t chunk_size;
+	int chunk_size_shift;
 
 	/* Needed for handling events */
 	struct dm_target *ti;
@@ -91,7 +89,7 @@ static int get_stripe(struct dm_target *ti, struct stripe_c *sc,
 
 /*
  * Construct a striped mapping.
- * <number of stripes> <chunk size (2^^n)> [<dev_path> <offset>]+
+ * <number of stripes> <chunk size> [<dev_path> <offset>]+
  */
 static int stripe_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 {
@@ -99,7 +97,6 @@ static int stripe_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	sector_t width;
 	uint32_t stripes;
 	uint32_t chunk_size;
-	char *end;
 	int r;
 	unsigned int i;
 
@@ -108,34 +105,23 @@ static int stripe_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 		return -EINVAL;
 	}
 
-	stripes = simple_strtoul(argv[0], &end, 10);
-	if (!stripes || *end) {
+	if (kstrtouint(argv[0], 10, &stripes) || !stripes) {
 		ti->error = "Invalid stripe count";
 		return -EINVAL;
 	}
 
-	chunk_size = simple_strtoul(argv[1], &end, 10);
-	if (*end) {
+	if (kstrtouint(argv[1], 10, &chunk_size) || !chunk_size) {
 		ti->error = "Invalid chunk_size";
 		return -EINVAL;
 	}
 
-	/*
-	 * chunk_size is a power of two
-	 */
-	if (!is_power_of_2(chunk_size) ||
-	    (chunk_size < (PAGE_SIZE >> SECTOR_SHIFT))) {
-		ti->error = "Invalid chunk size";
-		return -EINVAL;
-	}
-
-	if (ti->len & (chunk_size - 1)) {
+	width = ti->len;
+	if (sector_div(width, chunk_size)) {
 		ti->error = "Target length not divisible by "
 		    "chunk size";
 		return -EINVAL;
 	}
 
-	width = ti->len;
 	if (sector_div(width, stripes)) {
 		ti->error = "Target length not divisible by "
 		    "number of stripes";
@@ -167,17 +153,21 @@ static int stripe_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 
 	if (stripes & (stripes - 1))
 		sc->stripes_shift = -1;
-	else {
-		sc->stripes_shift = ffs(stripes) - 1;
-		sc->stripes_mask = ((sector_t) stripes) - 1;
-	}
+	else
+		sc->stripes_shift = __ffs(stripes);
+
+	r = dm_set_target_max_io_len(ti, chunk_size);
+	if (r)
+		return r;
 
-	ti->split_io = chunk_size;
 	ti->num_flush_requests = stripes;
 	ti->num_discard_requests = stripes;
 
-	sc->chunk_shift = ffs(chunk_size) - 1;
-	sc->chunk_mask = ((sector_t) chunk_size) - 1;
+	sc->chunk_size = chunk_size;
+	if (chunk_size & (chunk_size - 1))
+		sc->chunk_size_shift = -1;
+	else
+		sc->chunk_size_shift = __ffs(chunk_size);
 
 	/*
 	 * Get the stripe destinations.
@@ -216,17 +206,29 @@ static void stripe_dtr(struct dm_target *ti)
 static void stripe_map_sector(struct stripe_c *sc, sector_t sector,
 			      uint32_t *stripe, sector_t *result)
 {
-	sector_t offset = dm_target_offset(sc->ti, sector);
-	sector_t chunk = offset >> sc->chunk_shift;
+	sector_t chunk = dm_target_offset(sc->ti, sector);
+	sector_t chunk_offset;
+
+	if (sc->chunk_size_shift < 0)
+		chunk_offset = sector_div(chunk, sc->chunk_size);
+	else {
+		chunk_offset = chunk & (sc->chunk_size - 1);
+		chunk >>= sc->chunk_size_shift;
+	}
 
 	if (sc->stripes_shift < 0)
 		*stripe = sector_div(chunk, sc->stripes);
 	else {
-		*stripe = chunk & sc->stripes_mask;
+		*stripe = chunk & (sc->stripes - 1);
 		chunk >>= sc->stripes_shift;
 	}
 
-	*result = (chunk << sc->chunk_shift) | (offset & sc->chunk_mask);
+	if (sc->chunk_size_shift < 0)
+		chunk *= sc->chunk_size;
+	else
+		chunk <<= sc->chunk_size_shift;
+
+	*result = chunk + chunk_offset;
 }
 
 static void stripe_map_range_sector(struct stripe_c *sc, sector_t sector,
@@ -237,9 +239,16 @@ static void stripe_map_range_sector(struct stripe_c *sc, sector_t sector,
 	stripe_map_sector(sc, sector, &stripe, result);
 	if (stripe == target_stripe)
 		return;
-	*result &= ~sc->chunk_mask;			/* round down */
+
+	/* round down */
+	sector = *result;
+	if (sc->chunk_size_shift < 0)
+		*result -= sector_div(sector, sc->chunk_size);
+	else
+		*result = sector & ~(sector_t)(sc->chunk_size - 1);
+
 	if (target_stripe < stripe)
-		*result += sc->chunk_mask + 1;		/* next chunk */
+		*result += sc->chunk_size;		/* next chunk */
 }
 
 static int stripe_map_discard(struct stripe_c *sc, struct bio *bio,
@@ -302,8 +311,8 @@ static int stripe_map(struct dm_target *ti, struct bio *bio,
  *
  */
 
-static int stripe_status(struct dm_target *ti,
-			 status_type_t type, char *result, unsigned int maxlen)
+static int stripe_status(struct dm_target *ti, status_type_t type,
+			 unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct stripe_c *sc = (struct stripe_c *) ti->private;
 	char buffer[sc->stripes + 1];
@@ -324,7 +333,7 @@ static int stripe_status(struct dm_target *ti,
 
 	case STATUSTYPE_TABLE:
 		DMEMIT("%d %llu", sc->stripes,
-			(unsigned long long)sc->chunk_mask + 1);
+			(unsigned long long)sc->chunk_size);
 		for (i = 0; i < sc->stripes; i++)
 			DMEMIT(" %s %llu", sc->stripe[i].dev->name,
 			    (unsigned long long)sc->stripe[i].physical_start);
@@ -391,7 +400,7 @@ static void stripe_io_hints(struct dm_target *ti,
 			    struct queue_limits *limits)
 {
 	struct stripe_c *sc = ti->private;
-	unsigned chunk_size = (sc->chunk_mask + 1) << 9;
+	unsigned chunk_size = sc->chunk_size << SECTOR_SHIFT;
 
 	blk_limits_io_min(limits, chunk_size);
 	blk_limits_io_opt(limits, chunk_size * sc->stripes);
@@ -419,7 +428,7 @@ static int stripe_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
 
 static struct target_type stripe_target = {
 	.name   = "striped",
-	.version = {1, 4, 0},
+	.version = {1, 5, 0},
 	.module = THIS_MODULE,
 	.ctr    = stripe_ctr,
 	.dtr    = stripe_dtr,
diff --git a/drivers/md/dm-table.c b/drivers/md/dm-table.c
index 2e227fbf162..f90069029aa 100644
--- a/drivers/md/dm-table.c
+++ b/drivers/md/dm-table.c
@@ -1319,6 +1319,9 @@ static bool dm_table_supports_flush(struct dm_table *t, unsigned flush)
 		if (!ti->num_flush_requests)
 			continue;
 
+		if (ti->flush_supported)
+			return 1;
+
 		if (ti->type->iterate_devices &&
 		    ti->type->iterate_devices(ti, device_flush_capable, &flush))
 			return 1;
diff --git a/drivers/md/dm-thin-metadata.c b/drivers/md/dm-thin-metadata.c
index 3e2907f0bc4..693e149e972 100644
--- a/drivers/md/dm-thin-metadata.c
+++ b/drivers/md/dm-thin-metadata.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Red Hat, Inc.
+ * Copyright (C) 2011-2012 Red Hat, Inc.
  *
  * This file is released under the GPL.
  */
@@ -80,6 +80,12 @@
 #define THIN_METADATA_CACHE_SIZE 64
 #define SECTOR_TO_BLOCK_SHIFT 3
 
+/*
+ *  3 for btree insert +
+ *  2 for btree lookup used within space map
+ */
+#define THIN_MAX_CONCURRENT_LOCKS 5
+
 /* This should be plenty */
 #define SPACE_MAP_ROOT_SIZE 128
 
@@ -172,13 +178,20 @@ struct dm_pool_metadata {
 
 	struct rw_semaphore root_lock;
 	uint32_t time;
-	int need_commit;
 	dm_block_t root;
 	dm_block_t details_root;
 	struct list_head thin_devices;
 	uint64_t trans_id;
 	unsigned long flags;
 	sector_t data_block_size;
+	bool read_only:1;
+
+	/*
+	 * Set if a transaction has to be aborted but the attempt to roll back
+	 * to the previous (good) transaction failed.  The only pool metadata
+	 * operation possible in this state is the closing of the device.
+	 */
+	bool fail_io:1;
 };
 
 struct dm_thin_device {
@@ -187,7 +200,8 @@ struct dm_thin_device {
 	dm_thin_id id;
 
 	int open_count;
-	int changed;
+	bool changed:1;
+	bool aborted_with_changes:1;
 	uint64_t mapped_blocks;
 	uint64_t transaction_id;
 	uint32_t creation_time;
@@ -338,7 +352,21 @@ static int subtree_equal(void *context, void *value1_le, void *value2_le)
 
 /*----------------------------------------------------------------*/
 
-static int superblock_all_zeroes(struct dm_block_manager *bm, int *result)
+static int superblock_lock_zero(struct dm_pool_metadata *pmd,
+				struct dm_block **sblock)
+{
+	return dm_bm_write_lock_zero(pmd->bm, THIN_SUPERBLOCK_LOCATION,
+				     &sb_validator, sblock);
+}
+
+static int superblock_lock(struct dm_pool_metadata *pmd,
+			   struct dm_block **sblock)
+{
+	return dm_bm_write_lock(pmd->bm, THIN_SUPERBLOCK_LOCATION,
+				&sb_validator, sblock);
+}
+
+static int __superblock_all_zeroes(struct dm_block_manager *bm, int *result)
 {
 	int r;
 	unsigned i;
@@ -365,72 +393,9 @@ static int superblock_all_zeroes(struct dm_block_manager *bm, int *result)
 	return dm_bm_unlock(b);
 }
 
-static int init_pmd(struct dm_pool_metadata *pmd,
-		    struct dm_block_manager *bm,
-		    dm_block_t nr_blocks, int create)
+static void __setup_btree_details(struct dm_pool_metadata *pmd)
 {
-	int r;
-	struct dm_space_map *sm, *data_sm;
-	struct dm_transaction_manager *tm;
-	struct dm_block *sblock;
-
-	if (create) {
-		r = dm_tm_create_with_sm(bm, THIN_SUPERBLOCK_LOCATION,
-					 &sb_validator, &tm, &sm, &sblock);
-		if (r < 0) {
-			DMERR("tm_create_with_sm failed");
-			return r;
-		}
-
-		data_sm = dm_sm_disk_create(tm, nr_blocks);
-		if (IS_ERR(data_sm)) {
-			DMERR("sm_disk_create failed");
-			dm_tm_unlock(tm, sblock);
-			r = PTR_ERR(data_sm);
-			goto bad;
-		}
-	} else {
-		struct thin_disk_superblock *disk_super = NULL;
-		size_t space_map_root_offset =
-			offsetof(struct thin_disk_superblock, metadata_space_map_root);
-
-		r = dm_tm_open_with_sm(bm, THIN_SUPERBLOCK_LOCATION,
-				       &sb_validator, space_map_root_offset,
-				       SPACE_MAP_ROOT_SIZE, &tm, &sm, &sblock);
-		if (r < 0) {
-			DMERR("tm_open_with_sm failed");
-			return r;
-		}
-
-		disk_super = dm_block_data(sblock);
-		data_sm = dm_sm_disk_open(tm, disk_super->data_space_map_root,
-					  sizeof(disk_super->data_space_map_root));
-		if (IS_ERR(data_sm)) {
-			DMERR("sm_disk_open failed");
-			r = PTR_ERR(data_sm);
-			goto bad;
-		}
-	}
-
-
-	r = dm_tm_unlock(tm, sblock);
-	if (r < 0) {
-		DMERR("couldn't unlock superblock");
-		goto bad_data_sm;
-	}
-
-	pmd->bm = bm;
-	pmd->metadata_sm = sm;
-	pmd->data_sm = data_sm;
-	pmd->tm = tm;
-	pmd->nb_tm = dm_tm_create_non_blocking_clone(tm);
-	if (!pmd->nb_tm) {
-		DMERR("could not create clone tm");
-		r = -ENOMEM;
-		goto bad_data_sm;
-	}
-
-	pmd->info.tm = tm;
+	pmd->info.tm = pmd->tm;
 	pmd->info.levels = 2;
 	pmd->info.value_type.context = pmd->data_sm;
 	pmd->info.value_type.size = sizeof(__le64);
@@ -441,7 +406,7 @@ static int init_pmd(struct dm_pool_metadata *pmd,
 	memcpy(&pmd->nb_info, &pmd->info, sizeof(pmd->nb_info));
 	pmd->nb_info.tm = pmd->nb_tm;
 
-	pmd->tl_info.tm = tm;
+	pmd->tl_info.tm = pmd->tm;
 	pmd->tl_info.levels = 1;
 	pmd->tl_info.value_type.context = &pmd->info;
 	pmd->tl_info.value_type.size = sizeof(__le64);
@@ -449,7 +414,7 @@ static int init_pmd(struct dm_pool_metadata *pmd,
 	pmd->tl_info.value_type.dec = subtree_dec;
 	pmd->tl_info.value_type.equal = subtree_equal;
 
-	pmd->bl_info.tm = tm;
+	pmd->bl_info.tm = pmd->tm;
 	pmd->bl_info.levels = 1;
 	pmd->bl_info.value_type.context = pmd->data_sm;
 	pmd->bl_info.value_type.size = sizeof(__le64);
@@ -457,48 +422,266 @@ static int init_pmd(struct dm_pool_metadata *pmd,
 	pmd->bl_info.value_type.dec = data_block_dec;
 	pmd->bl_info.value_type.equal = data_block_equal;
 
-	pmd->details_info.tm = tm;
+	pmd->details_info.tm = pmd->tm;
 	pmd->details_info.levels = 1;
 	pmd->details_info.value_type.context = NULL;
 	pmd->details_info.value_type.size = sizeof(struct disk_device_details);
 	pmd->details_info.value_type.inc = NULL;
 	pmd->details_info.value_type.dec = NULL;
 	pmd->details_info.value_type.equal = NULL;
+}
 
-	pmd->root = 0;
+static int __write_initial_superblock(struct dm_pool_metadata *pmd)
+{
+	int r;
+	struct dm_block *sblock;
+	size_t metadata_len, data_len;
+	struct thin_disk_superblock *disk_super;
+	sector_t bdev_size = i_size_read(pmd->bdev->bd_inode) >> SECTOR_SHIFT;
 
-	init_rwsem(&pmd->root_lock);
-	pmd->time = 0;
-	pmd->need_commit = 0;
-	pmd->details_root = 0;
-	pmd->trans_id = 0;
-	pmd->flags = 0;
-	INIT_LIST_HEAD(&pmd->thin_devices);
+	if (bdev_size > THIN_METADATA_MAX_SECTORS)
+		bdev_size = THIN_METADATA_MAX_SECTORS;
+
+	r = dm_sm_root_size(pmd->metadata_sm, &metadata_len);
+	if (r < 0)
+		return r;
+
+	r = dm_sm_root_size(pmd->data_sm, &data_len);
+	if (r < 0)
+		return r;
+
+	r = dm_sm_commit(pmd->data_sm);
+	if (r < 0)
+		return r;
+
+	r = dm_tm_pre_commit(pmd->tm);
+	if (r < 0)
+		return r;
+
+	r = superblock_lock_zero(pmd, &sblock);
+	if (r)
+		return r;
+
+	disk_super = dm_block_data(sblock);
+	disk_super->flags = 0;
+	memset(disk_super->uuid, 0, sizeof(disk_super->uuid));
+	disk_super->magic = cpu_to_le64(THIN_SUPERBLOCK_MAGIC);
+	disk_super->version = cpu_to_le32(THIN_VERSION);
+	disk_super->time = 0;
+	disk_super->trans_id = 0;
+	disk_super->held_root = 0;
+
+	r = dm_sm_copy_root(pmd->metadata_sm, &disk_super->metadata_space_map_root,
+			    metadata_len);
+	if (r < 0)
+		goto bad_locked;
+
+	r = dm_sm_copy_root(pmd->data_sm, &disk_super->data_space_map_root,
+			    data_len);
+	if (r < 0)
+		goto bad_locked;
+
+	disk_super->data_mapping_root = cpu_to_le64(pmd->root);
+	disk_super->device_details_root = cpu_to_le64(pmd->details_root);
+	disk_super->metadata_block_size = cpu_to_le32(THIN_METADATA_BLOCK_SIZE >> SECTOR_SHIFT);
+	disk_super->metadata_nr_blocks = cpu_to_le64(bdev_size >> SECTOR_TO_BLOCK_SHIFT);
+	disk_super->data_block_size = cpu_to_le32(pmd->data_block_size);
+
+	return dm_tm_commit(pmd->tm, sblock);
+
+bad_locked:
+	dm_bm_unlock(sblock);
+	return r;
+}
+
+static int __format_metadata(struct dm_pool_metadata *pmd)
+{
+	int r;
+
+	r = dm_tm_create_with_sm(pmd->bm, THIN_SUPERBLOCK_LOCATION,
+				 &pmd->tm, &pmd->metadata_sm);
+	if (r < 0) {
+		DMERR("tm_create_with_sm failed");
+		return r;
+	}
+
+	pmd->data_sm = dm_sm_disk_create(pmd->tm, 0);
+	if (IS_ERR(pmd->data_sm)) {
+		DMERR("sm_disk_create failed");
+		r = PTR_ERR(pmd->data_sm);
+		goto bad_cleanup_tm;
+	}
+
+	pmd->nb_tm = dm_tm_create_non_blocking_clone(pmd->tm);
+	if (!pmd->nb_tm) {
+		DMERR("could not create non-blocking clone tm");
+		r = -ENOMEM;
+		goto bad_cleanup_data_sm;
+	}
+
+	__setup_btree_details(pmd);
+
+	r = dm_btree_empty(&pmd->info, &pmd->root);
+	if (r < 0)
+		goto bad_cleanup_nb_tm;
+
+	r = dm_btree_empty(&pmd->details_info, &pmd->details_root);
+	if (r < 0) {
+		DMERR("couldn't create devices root");
+		goto bad_cleanup_nb_tm;
+	}
+
+	r = __write_initial_superblock(pmd);
+	if (r)
+		goto bad_cleanup_nb_tm;
 
 	return 0;
 
-bad_data_sm:
-	dm_sm_destroy(data_sm);
-bad:
-	dm_tm_destroy(tm);
-	dm_sm_destroy(sm);
+bad_cleanup_nb_tm:
+	dm_tm_destroy(pmd->nb_tm);
+bad_cleanup_data_sm:
+	dm_sm_destroy(pmd->data_sm);
+bad_cleanup_tm:
+	dm_tm_destroy(pmd->tm);
+	dm_sm_destroy(pmd->metadata_sm);
+
+	return r;
+}
+
+static int __check_incompat_features(struct thin_disk_superblock *disk_super,
+				     struct dm_pool_metadata *pmd)
+{
+	uint32_t features;
+
+	features = le32_to_cpu(disk_super->incompat_flags) & ~THIN_FEATURE_INCOMPAT_SUPP;
+	if (features) {
+		DMERR("could not access metadata due to unsupported optional features (%lx).",
+		      (unsigned long)features);
+		return -EINVAL;
+	}
+
+	/*
+	 * Check for read-only metadata to skip the following RDWR checks.
+	 */
+	if (get_disk_ro(pmd->bdev->bd_disk))
+		return 0;
+
+	features = le32_to_cpu(disk_super->compat_ro_flags) & ~THIN_FEATURE_COMPAT_RO_SUPP;
+	if (features) {
+		DMERR("could not access metadata RDWR due to unsupported optional features (%lx).",
+		      (unsigned long)features);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __open_metadata(struct dm_pool_metadata *pmd)
+{
+	int r;
+	struct dm_block *sblock;
+	struct thin_disk_superblock *disk_super;
+
+	r = dm_bm_read_lock(pmd->bm, THIN_SUPERBLOCK_LOCATION,
+			    &sb_validator, &sblock);
+	if (r < 0) {
+		DMERR("couldn't read superblock");
+		return r;
+	}
+
+	disk_super = dm_block_data(sblock);
+
+	r = __check_incompat_features(disk_super, pmd);
+	if (r < 0)
+		goto bad_unlock_sblock;
+
+	r = dm_tm_open_with_sm(pmd->bm, THIN_SUPERBLOCK_LOCATION,
+			       disk_super->metadata_space_map_root,
+			       sizeof(disk_super->metadata_space_map_root),
+			       &pmd->tm, &pmd->metadata_sm);
+	if (r < 0) {
+		DMERR("tm_open_with_sm failed");
+		goto bad_unlock_sblock;
+	}
+
+	pmd->data_sm = dm_sm_disk_open(pmd->tm, disk_super->data_space_map_root,
+				       sizeof(disk_super->data_space_map_root));
+	if (IS_ERR(pmd->data_sm)) {
+		DMERR("sm_disk_open failed");
+		r = PTR_ERR(pmd->data_sm);
+		goto bad_cleanup_tm;
+	}
+
+	pmd->nb_tm = dm_tm_create_non_blocking_clone(pmd->tm);
+	if (!pmd->nb_tm) {
+		DMERR("could not create non-blocking clone tm");
+		r = -ENOMEM;
+		goto bad_cleanup_data_sm;
+	}
+
+	__setup_btree_details(pmd);
+	return dm_bm_unlock(sblock);
+
+bad_cleanup_data_sm:
+	dm_sm_destroy(pmd->data_sm);
+bad_cleanup_tm:
+	dm_tm_destroy(pmd->tm);
+	dm_sm_destroy(pmd->metadata_sm);
+bad_unlock_sblock:
+	dm_bm_unlock(sblock);
+
+	return r;
+}
+
+static int __open_or_format_metadata(struct dm_pool_metadata *pmd, bool format_device)
+{
+	int r, unformatted;
+
+	r = __superblock_all_zeroes(pmd->bm, &unformatted);
+	if (r)
+		return r;
+
+	if (unformatted)
+		return format_device ? __format_metadata(pmd) : -EPERM;
+
+	return __open_metadata(pmd);
+}
+
+static int __create_persistent_data_objects(struct dm_pool_metadata *pmd, bool format_device)
+{
+	int r;
+
+	pmd->bm = dm_block_manager_create(pmd->bdev, THIN_METADATA_BLOCK_SIZE,
+					  THIN_METADATA_CACHE_SIZE,
+					  THIN_MAX_CONCURRENT_LOCKS);
+	if (IS_ERR(pmd->bm)) {
+		DMERR("could not create block manager");
+		return PTR_ERR(pmd->bm);
+	}
+
+	r = __open_or_format_metadata(pmd, format_device);
+	if (r)
+		dm_block_manager_destroy(pmd->bm);
 
 	return r;
 }
 
+static void __destroy_persistent_data_objects(struct dm_pool_metadata *pmd)
+{
+	dm_sm_destroy(pmd->data_sm);
+	dm_sm_destroy(pmd->metadata_sm);
+	dm_tm_destroy(pmd->nb_tm);
+	dm_tm_destroy(pmd->tm);
+	dm_block_manager_destroy(pmd->bm);
+}
+
 static int __begin_transaction(struct dm_pool_metadata *pmd)
 {
 	int r;
-	u32 features;
 	struct thin_disk_superblock *disk_super;
 	struct dm_block *sblock;
 
 	/*
-	 * __maybe_commit_transaction() resets these
-	 */
-	WARN_ON(pmd->need_commit);
-
-	/*
 	 * We re-read the superblock every time.  Shouldn't need to do this
 	 * really.
 	 */
@@ -515,32 +698,8 @@ static int __begin_transaction(struct dm_pool_metadata *pmd)
 	pmd->flags = le32_to_cpu(disk_super->flags);
 	pmd->data_block_size = le32_to_cpu(disk_super->data_block_size);
 
-	features = le32_to_cpu(disk_super->incompat_flags) & ~THIN_FEATURE_INCOMPAT_SUPP;
-	if (features) {
-		DMERR("could not access metadata due to "
-		      "unsupported optional features (%lx).",
-		      (unsigned long)features);
-		r = -EINVAL;
-		goto out;
-	}
-
-	/*
-	 * Check for read-only metadata to skip the following RDWR checks.
-	 */
-	if (get_disk_ro(pmd->bdev->bd_disk))
-		goto out;
-
-	features = le32_to_cpu(disk_super->compat_ro_flags) & ~THIN_FEATURE_COMPAT_RO_SUPP;
-	if (features) {
-		DMERR("could not access metadata RDWR due to "
-		      "unsupported optional features (%lx).",
-		      (unsigned long)features);
-		r = -EINVAL;
-	}
-
-out:
 	dm_bm_unlock(sblock);
-	return r;
+	return 0;
 }
 
 static int __write_changed_details(struct dm_pool_metadata *pmd)
@@ -573,8 +732,6 @@ static int __write_changed_details(struct dm_pool_metadata *pmd)
 			list_del(&td->list);
 			kfree(td);
 		}
-
-		pmd->need_commit = 1;
 	}
 
 	return 0;
@@ -582,9 +739,6 @@ static int __write_changed_details(struct dm_pool_metadata *pmd)
 
 static int __commit_transaction(struct dm_pool_metadata *pmd)
 {
-	/*
-	 * FIXME: Associated pool should be made read-only on failure.
-	 */
 	int r;
 	size_t metadata_len, data_len;
 	struct thin_disk_superblock *disk_super;
@@ -597,31 +751,27 @@ static int __commit_transaction(struct dm_pool_metadata *pmd)
 
 	r = __write_changed_details(pmd);
 	if (r < 0)
-		goto out;
-
-	if (!pmd->need_commit)
-		goto out;
+		return r;
 
 	r = dm_sm_commit(pmd->data_sm);
 	if (r < 0)
-		goto out;
+		return r;
 
 	r = dm_tm_pre_commit(pmd->tm);
 	if (r < 0)
-		goto out;
+		return r;
 
 	r = dm_sm_root_size(pmd->metadata_sm, &metadata_len);
 	if (r < 0)
-		goto out;
+		return r;
 
 	r = dm_sm_root_size(pmd->data_sm, &data_len);
 	if (r < 0)
-		goto out;
+		return r;
 
-	r = dm_bm_write_lock(pmd->bm, THIN_SUPERBLOCK_LOCATION,
-			     &sb_validator, &sblock);
+	r = superblock_lock(pmd, &sblock);
 	if (r)
-		goto out;
+		return r;
 
 	disk_super = dm_block_data(sblock);
 	disk_super->time = cpu_to_le32(pmd->time);
@@ -640,12 +790,7 @@ static int __commit_transaction(struct dm_pool_metadata *pmd)
 	if (r < 0)
 		goto out_locked;
 
-	r = dm_tm_commit(pmd->tm, sblock);
-	if (!r)
-		pmd->need_commit = 0;
-
-out:
-	return r;
+	return dm_tm_commit(pmd->tm, sblock);
 
 out_locked:
 	dm_bm_unlock(sblock);
@@ -653,15 +798,11 @@ out_locked:
 }
 
 struct dm_pool_metadata *dm_pool_metadata_open(struct block_device *bdev,
-					       sector_t data_block_size)
+					       sector_t data_block_size,
+					       bool format_device)
 {
 	int r;
-	struct thin_disk_superblock *disk_super;
 	struct dm_pool_metadata *pmd;
-	sector_t bdev_size = i_size_read(bdev->bd_inode) >> SECTOR_SHIFT;
-	struct dm_block_manager *bm;
-	int create;
-	struct dm_block *sblock;
 
 	pmd = kmalloc(sizeof(*pmd), GFP_KERNEL);
 	if (!pmd) {
@@ -669,90 +810,28 @@ struct dm_pool_metadata *dm_pool_metadata_open(struct block_device *bdev,
 		return ERR_PTR(-ENOMEM);
 	}
 
-	/*
-	 * Max hex locks:
-	 *  3 for btree insert +
-	 *  2 for btree lookup used within space map
-	 */
-	bm = dm_block_manager_create(bdev, THIN_METADATA_BLOCK_SIZE,
-				     THIN_METADATA_CACHE_SIZE, 5);
-	if (!bm) {
-		DMERR("could not create block manager");
-		kfree(pmd);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	r = superblock_all_zeroes(bm, &create);
-	if (r) {
-		dm_block_manager_destroy(bm);
-		kfree(pmd);
-		return ERR_PTR(r);
-	}
-
+	init_rwsem(&pmd->root_lock);
+	pmd->time = 0;
+	INIT_LIST_HEAD(&pmd->thin_devices);
+	pmd->read_only = false;
+	pmd->fail_io = false;
+	pmd->bdev = bdev;
+	pmd->data_block_size = data_block_size;
 
-	r = init_pmd(pmd, bm, 0, create);
+	r = __create_persistent_data_objects(pmd, format_device);
 	if (r) {
-		dm_block_manager_destroy(bm);
 		kfree(pmd);
 		return ERR_PTR(r);
 	}
-	pmd->bdev = bdev;
-
-	if (!create) {
-		r = __begin_transaction(pmd);
-		if (r < 0)
-			goto bad;
-		return pmd;
-	}
-
-	/*
-	 * Create.
-	 */
-	r = dm_bm_write_lock(pmd->bm, THIN_SUPERBLOCK_LOCATION,
-			     &sb_validator, &sblock);
-	if (r)
-		goto bad;
-
-	if (bdev_size > THIN_METADATA_MAX_SECTORS)
-		bdev_size = THIN_METADATA_MAX_SECTORS;
-
-	disk_super = dm_block_data(sblock);
-	disk_super->magic = cpu_to_le64(THIN_SUPERBLOCK_MAGIC);
-	disk_super->version = cpu_to_le32(THIN_VERSION);
-	disk_super->time = 0;
-	disk_super->metadata_block_size = cpu_to_le32(THIN_METADATA_BLOCK_SIZE >> SECTOR_SHIFT);
-	disk_super->metadata_nr_blocks = cpu_to_le64(bdev_size >> SECTOR_TO_BLOCK_SHIFT);
-	disk_super->data_block_size = cpu_to_le32(data_block_size);
-
-	r = dm_bm_unlock(sblock);
-	if (r < 0)
-		goto bad;
-
-	r = dm_btree_empty(&pmd->info, &pmd->root);
-	if (r < 0)
-		goto bad;
-
-	r = dm_btree_empty(&pmd->details_info, &pmd->details_root);
-	if (r < 0) {
-		DMERR("couldn't create devices root");
-		goto bad;
-	}
 
-	pmd->flags = 0;
-	pmd->need_commit = 1;
-	r = dm_pool_commit_metadata(pmd);
+	r = __begin_transaction(pmd);
 	if (r < 0) {
-		DMERR("%s: dm_pool_commit_metadata() failed, error = %d",
-		      __func__, r);
-		goto bad;
+		if (dm_pool_metadata_close(pmd) < 0)
+			DMWARN("%s: dm_pool_metadata_close() failed.", __func__);
+		return ERR_PTR(r);
 	}
 
 	return pmd;
-
-bad:
-	if (dm_pool_metadata_close(pmd) < 0)
-		DMWARN("%s: dm_pool_metadata_close() failed.", __func__);
-	return ERR_PTR(r);
 }
 
 int dm_pool_metadata_close(struct dm_pool_metadata *pmd)
@@ -778,18 +857,17 @@ int dm_pool_metadata_close(struct dm_pool_metadata *pmd)
 		return -EBUSY;
 	}
 
-	r = __commit_transaction(pmd);
-	if (r < 0)
-		DMWARN("%s: __commit_transaction() failed, error = %d",
-		       __func__, r);
+	if (!pmd->read_only && !pmd->fail_io) {
+		r = __commit_transaction(pmd);
+		if (r < 0)
+			DMWARN("%s: __commit_transaction() failed, error = %d",
+			       __func__, r);
+	}
 
-	dm_tm_destroy(pmd->tm);
-	dm_tm_destroy(pmd->nb_tm);
-	dm_block_manager_destroy(pmd->bm);
-	dm_sm_destroy(pmd->metadata_sm);
-	dm_sm_destroy(pmd->data_sm);
-	kfree(pmd);
+	if (!pmd->fail_io)
+		__destroy_persistent_data_objects(pmd);
 
+	kfree(pmd);
 	return 0;
 }
 
@@ -850,6 +928,7 @@ static int __open_device(struct dm_pool_metadata *pmd,
 	(*td)->id = dev;
 	(*td)->open_count = 1;
 	(*td)->changed = changed;
+	(*td)->aborted_with_changes = false;
 	(*td)->mapped_blocks = le64_to_cpu(details_le.mapped_blocks);
 	(*td)->transaction_id = le64_to_cpu(details_le.transaction_id);
 	(*td)->creation_time = le32_to_cpu(details_le.creation_time);
@@ -911,10 +990,11 @@ static int __create_thin(struct dm_pool_metadata *pmd,
 
 int dm_pool_create_thin(struct dm_pool_metadata *pmd, dm_thin_id dev)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __create_thin(pmd, dev);
+	if (!pmd->fail_io)
+		r = __create_thin(pmd, dev);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1001,10 +1081,11 @@ int dm_pool_create_snap(struct dm_pool_metadata *pmd,
 				 dm_thin_id dev,
 				 dm_thin_id origin)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __create_snap(pmd, dev, origin);
+	if (!pmd->fail_io)
+		r = __create_snap(pmd, dev, origin);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1037,18 +1118,17 @@ static int __delete_device(struct dm_pool_metadata *pmd, dm_thin_id dev)
 	if (r)
 		return r;
 
-	pmd->need_commit = 1;
-
 	return 0;
 }
 
 int dm_pool_delete_thin_device(struct dm_pool_metadata *pmd,
 			       dm_thin_id dev)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __delete_device(pmd, dev);
+	if (!pmd->fail_io)
+		r = __delete_device(pmd, dev);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1058,28 +1138,40 @@ int dm_pool_set_metadata_transaction_id(struct dm_pool_metadata *pmd,
 					uint64_t current_id,
 					uint64_t new_id)
 {
+	int r = -EINVAL;
+
 	down_write(&pmd->root_lock);
+
+	if (pmd->fail_io)
+		goto out;
+
 	if (pmd->trans_id != current_id) {
-		up_write(&pmd->root_lock);
 		DMERR("mismatched transaction id");
-		return -EINVAL;
+		goto out;
 	}
 
 	pmd->trans_id = new_id;
-	pmd->need_commit = 1;
+	r = 0;
+
+out:
 	up_write(&pmd->root_lock);
 
-	return 0;
+	return r;
 }
 
 int dm_pool_get_metadata_transaction_id(struct dm_pool_metadata *pmd,
 					uint64_t *result)
 {
+	int r = -EINVAL;
+
 	down_read(&pmd->root_lock);
-	*result = pmd->trans_id;
+	if (!pmd->fail_io) {
+		*result = pmd->trans_id;
+		r = 0;
+	}
 	up_read(&pmd->root_lock);
 
-	return 0;
+	return r;
 }
 
 static int __reserve_metadata_snap(struct dm_pool_metadata *pmd)
@@ -1108,8 +1200,6 @@ static int __reserve_metadata_snap(struct dm_pool_metadata *pmd)
 
 		dm_tm_dec(pmd->tm, held_root);
 		dm_tm_unlock(pmd->tm, copy);
-		pmd->need_commit = 1;
-
 		return -EBUSY;
 	}
 
@@ -1131,29 +1221,25 @@ static int __reserve_metadata_snap(struct dm_pool_metadata *pmd)
 	/*
 	 * Write the held root into the superblock.
 	 */
-	r = dm_bm_write_lock(pmd->bm, THIN_SUPERBLOCK_LOCATION,
-			     &sb_validator, &sblock);
+	r = superblock_lock(pmd, &sblock);
 	if (r) {
 		dm_tm_dec(pmd->tm, held_root);
-		pmd->need_commit = 1;
 		return r;
 	}
 
 	disk_super = dm_block_data(sblock);
 	disk_super->held_root = cpu_to_le64(held_root);
 	dm_bm_unlock(sblock);
-
-	pmd->need_commit = 1;
-
 	return 0;
 }
 
 int dm_pool_reserve_metadata_snap(struct dm_pool_metadata *pmd)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __reserve_metadata_snap(pmd);
+	if (!pmd->fail_io)
+		r = __reserve_metadata_snap(pmd);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1166,15 +1252,13 @@ static int __release_metadata_snap(struct dm_pool_metadata *pmd)
 	struct dm_block *sblock, *copy;
 	dm_block_t held_root;
 
-	r = dm_bm_write_lock(pmd->bm, THIN_SUPERBLOCK_LOCATION,
-			     &sb_validator, &sblock);
+	r = superblock_lock(pmd, &sblock);
 	if (r)
 		return r;
 
 	disk_super = dm_block_data(sblock);
 	held_root = le64_to_cpu(disk_super->held_root);
 	disk_super->held_root = cpu_to_le64(0);
-	pmd->need_commit = 1;
 
 	dm_bm_unlock(sblock);
 
@@ -1197,10 +1281,11 @@ static int __release_metadata_snap(struct dm_pool_metadata *pmd)
 
 int dm_pool_release_metadata_snap(struct dm_pool_metadata *pmd)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __release_metadata_snap(pmd);
+	if (!pmd->fail_io)
+		r = __release_metadata_snap(pmd);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1227,10 +1312,11 @@ static int __get_metadata_snap(struct dm_pool_metadata *pmd,
 int dm_pool_get_metadata_snap(struct dm_pool_metadata *pmd,
 			      dm_block_t *result)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_read(&pmd->root_lock);
-	r = __get_metadata_snap(pmd, result);
+	if (!pmd->fail_io)
+		r = __get_metadata_snap(pmd, result);
 	up_read(&pmd->root_lock);
 
 	return r;
@@ -1239,10 +1325,11 @@ int dm_pool_get_metadata_snap(struct dm_pool_metadata *pmd,
 int dm_pool_open_thin_device(struct dm_pool_metadata *pmd, dm_thin_id dev,
 			     struct dm_thin_device **td)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __open_device(pmd, dev, 0, td);
+	if (!pmd->fail_io)
+		r = __open_device(pmd, dev, 0, td);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1262,7 +1349,7 @@ dm_thin_id dm_thin_dev_id(struct dm_thin_device *td)
 	return td->id;
 }
 
-static int __snapshotted_since(struct dm_thin_device *td, uint32_t time)
+static bool __snapshotted_since(struct dm_thin_device *td, uint32_t time)
 {
 	return td->snapshotted_time > time;
 }
@@ -1270,28 +1357,31 @@ static int __snapshotted_since(struct dm_thin_device *td, uint32_t time)
 int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
 		       int can_block, struct dm_thin_lookup_result *result)
 {
-	int r;
+	int r = -EINVAL;
 	uint64_t block_time = 0;
 	__le64 value;
 	struct dm_pool_metadata *pmd = td->pmd;
 	dm_block_t keys[2] = { td->id, block };
+	struct dm_btree_info *info;
 
 	if (can_block) {
 		down_read(&pmd->root_lock);
-		r = dm_btree_lookup(&pmd->info, pmd->root, keys, &value);
-		if (!r)
-			block_time = le64_to_cpu(value);
-		up_read(&pmd->root_lock);
-
-	} else if (down_read_trylock(&pmd->root_lock)) {
-		r = dm_btree_lookup(&pmd->nb_info, pmd->root, keys, &value);
-		if (!r)
-			block_time = le64_to_cpu(value);
-		up_read(&pmd->root_lock);
-
-	} else
+		info = &pmd->info;
+	} else if (down_read_trylock(&pmd->root_lock))
+		info = &pmd->nb_info;
+	else
 		return -EWOULDBLOCK;
 
+	if (pmd->fail_io)
+		goto out;
+
+	r = dm_btree_lookup(info, pmd->root, keys, &value);
+	if (!r)
+		block_time = le64_to_cpu(value);
+
+out:
+	up_read(&pmd->root_lock);
+
 	if (!r) {
 		dm_block_t exception_block;
 		uint32_t exception_time;
@@ -1312,7 +1402,6 @@ static int __insert(struct dm_thin_device *td, dm_block_t block,
 	struct dm_pool_metadata *pmd = td->pmd;
 	dm_block_t keys[2] = { td->id, block };
 
-	pmd->need_commit = 1;
 	value = cpu_to_le64(pack_block_time(data_block, pmd->time));
 	__dm_bless_for_disk(&value);
 
@@ -1321,10 +1410,9 @@ static int __insert(struct dm_thin_device *td, dm_block_t block,
 	if (r)
 		return r;
 
-	if (inserted) {
+	td->changed = 1;
+	if (inserted)
 		td->mapped_blocks++;
-		td->changed = 1;
-	}
 
 	return 0;
 }
@@ -1332,10 +1420,11 @@ static int __insert(struct dm_thin_device *td, dm_block_t block,
 int dm_thin_insert_block(struct dm_thin_device *td, dm_block_t block,
 			 dm_block_t data_block)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&td->pmd->root_lock);
-	r = __insert(td, block, data_block);
+	if (!td->pmd->fail_io)
+		r = __insert(td, block, data_block);
 	up_write(&td->pmd->root_lock);
 
 	return r;
@@ -1353,31 +1442,51 @@ static int __remove(struct dm_thin_device *td, dm_block_t block)
 
 	td->mapped_blocks--;
 	td->changed = 1;
-	pmd->need_commit = 1;
 
 	return 0;
 }
 
 int dm_thin_remove_block(struct dm_thin_device *td, dm_block_t block)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&td->pmd->root_lock);
-	r = __remove(td, block);
+	if (!td->pmd->fail_io)
+		r = __remove(td, block);
 	up_write(&td->pmd->root_lock);
 
 	return r;
 }
 
-int dm_pool_alloc_data_block(struct dm_pool_metadata *pmd, dm_block_t *result)
+bool dm_thin_changed_this_transaction(struct dm_thin_device *td)
 {
 	int r;
 
-	down_write(&pmd->root_lock);
+	down_read(&td->pmd->root_lock);
+	r = td->changed;
+	up_read(&td->pmd->root_lock);
 
-	r = dm_sm_new_block(pmd->data_sm, result);
-	pmd->need_commit = 1;
+	return r;
+}
+
+bool dm_thin_aborted_changes(struct dm_thin_device *td)
+{
+	bool r;
 
+	down_read(&td->pmd->root_lock);
+	r = td->aborted_with_changes;
+	up_read(&td->pmd->root_lock);
+
+	return r;
+}
+
+int dm_pool_alloc_data_block(struct dm_pool_metadata *pmd, dm_block_t *result)
+{
+	int r = -EINVAL;
+
+	down_write(&pmd->root_lock);
+	if (!pmd->fail_io)
+		r = dm_sm_new_block(pmd->data_sm, result);
 	up_write(&pmd->root_lock);
 
 	return r;
@@ -1385,9 +1494,11 @@ int dm_pool_alloc_data_block(struct dm_pool_metadata *pmd, dm_block_t *result)
 
 int dm_pool_commit_metadata(struct dm_pool_metadata *pmd)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
+	if (pmd->fail_io)
+		goto out;
 
 	r = __commit_transaction(pmd);
 	if (r <= 0)
@@ -1402,12 +1513,41 @@ out:
 	return r;
 }
 
+static void __set_abort_with_changes_flags(struct dm_pool_metadata *pmd)
+{
+	struct dm_thin_device *td;
+
+	list_for_each_entry(td, &pmd->thin_devices, list)
+		td->aborted_with_changes = td->changed;
+}
+
+int dm_pool_abort_metadata(struct dm_pool_metadata *pmd)
+{
+	int r = -EINVAL;
+
+	down_write(&pmd->root_lock);
+	if (pmd->fail_io)
+		goto out;
+
+	__set_abort_with_changes_flags(pmd);
+	__destroy_persistent_data_objects(pmd);
+	r = __create_persistent_data_objects(pmd, false);
+	if (r)
+		pmd->fail_io = true;
+
+out:
+	up_write(&pmd->root_lock);
+
+	return r;
+}
+
 int dm_pool_get_free_block_count(struct dm_pool_metadata *pmd, dm_block_t *result)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_read(&pmd->root_lock);
-	r = dm_sm_get_nr_free(pmd->data_sm, result);
+	if (!pmd->fail_io)
+		r = dm_sm_get_nr_free(pmd->data_sm, result);
 	up_read(&pmd->root_lock);
 
 	return r;
@@ -1416,10 +1556,11 @@ int dm_pool_get_free_block_count(struct dm_pool_metadata *pmd, dm_block_t *resul
 int dm_pool_get_free_metadata_block_count(struct dm_pool_metadata *pmd,
 					  dm_block_t *result)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_read(&pmd->root_lock);
-	r = dm_sm_get_nr_free(pmd->metadata_sm, result);
+	if (!pmd->fail_io)
+		r = dm_sm_get_nr_free(pmd->metadata_sm, result);
 	up_read(&pmd->root_lock);
 
 	return r;
@@ -1428,10 +1569,11 @@ int dm_pool_get_free_metadata_block_count(struct dm_pool_metadata *pmd,
 int dm_pool_get_metadata_dev_size(struct dm_pool_metadata *pmd,
 				  dm_block_t *result)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_read(&pmd->root_lock);
-	r = dm_sm_get_nr_blocks(pmd->metadata_sm, result);
+	if (!pmd->fail_io)
+		r = dm_sm_get_nr_blocks(pmd->metadata_sm, result);
 	up_read(&pmd->root_lock);
 
 	return r;
@@ -1448,10 +1590,11 @@ int dm_pool_get_data_block_size(struct dm_pool_metadata *pmd, sector_t *result)
 
 int dm_pool_get_data_dev_size(struct dm_pool_metadata *pmd, dm_block_t *result)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_read(&pmd->root_lock);
-	r = dm_sm_get_nr_blocks(pmd->data_sm, result);
+	if (!pmd->fail_io)
+		r = dm_sm_get_nr_blocks(pmd->data_sm, result);
 	up_read(&pmd->root_lock);
 
 	return r;
@@ -1459,13 +1602,17 @@ int dm_pool_get_data_dev_size(struct dm_pool_metadata *pmd, dm_block_t *result)
 
 int dm_thin_get_mapped_count(struct dm_thin_device *td, dm_block_t *result)
 {
+	int r = -EINVAL;
 	struct dm_pool_metadata *pmd = td->pmd;
 
 	down_read(&pmd->root_lock);
-	*result = td->mapped_blocks;
+	if (!pmd->fail_io) {
+		*result = td->mapped_blocks;
+		r = 0;
+	}
 	up_read(&pmd->root_lock);
 
-	return 0;
+	return r;
 }
 
 static int __highest_block(struct dm_thin_device *td, dm_block_t *result)
@@ -1487,11 +1634,12 @@ static int __highest_block(struct dm_thin_device *td, dm_block_t *result)
 int dm_thin_get_highest_mapped_block(struct dm_thin_device *td,
 				     dm_block_t *result)
 {
-	int r;
+	int r = -EINVAL;
 	struct dm_pool_metadata *pmd = td->pmd;
 
 	down_read(&pmd->root_lock);
-	r = __highest_block(td, result);
+	if (!pmd->fail_io)
+		r = __highest_block(td, result);
 	up_read(&pmd->root_lock);
 
 	return r;
@@ -1514,20 +1662,25 @@ static int __resize_data_dev(struct dm_pool_metadata *pmd, dm_block_t new_count)
 		return -EINVAL;
 	}
 
-	r = dm_sm_extend(pmd->data_sm, new_count - old_count);
-	if (!r)
-		pmd->need_commit = 1;
-
-	return r;
+	return dm_sm_extend(pmd->data_sm, new_count - old_count);
 }
 
 int dm_pool_resize_data_dev(struct dm_pool_metadata *pmd, dm_block_t new_count)
 {
-	int r;
+	int r = -EINVAL;
 
 	down_write(&pmd->root_lock);
-	r = __resize_data_dev(pmd, new_count);
+	if (!pmd->fail_io)
+		r = __resize_data_dev(pmd, new_count);
 	up_write(&pmd->root_lock);
 
 	return r;
 }
+
+void dm_pool_metadata_read_only(struct dm_pool_metadata *pmd)
+{
+	down_write(&pmd->root_lock);
+	pmd->read_only = true;
+	dm_bm_set_read_only(pmd->bm);
+	up_write(&pmd->root_lock);
+}
diff --git a/drivers/md/dm-thin-metadata.h b/drivers/md/dm-thin-metadata.h
index b88918ccdaf..0cecc370288 100644
--- a/drivers/md/dm-thin-metadata.h
+++ b/drivers/md/dm-thin-metadata.h
@@ -38,7 +38,8 @@ typedef uint64_t dm_thin_id;
  * Reopens or creates a new, empty metadata volume.
  */
 struct dm_pool_metadata *dm_pool_metadata_open(struct block_device *bdev,
-					       sector_t data_block_size);
+					       sector_t data_block_size,
+					       bool format_device);
 
 int dm_pool_metadata_close(struct dm_pool_metadata *pmd);
 
@@ -79,6 +80,16 @@ int dm_pool_delete_thin_device(struct dm_pool_metadata *pmd,
 int dm_pool_commit_metadata(struct dm_pool_metadata *pmd);
 
 /*
+ * Discards all uncommitted changes.  Rereads the superblock, rolling back
+ * to the last good transaction.  Thin devices remain open.
+ * dm_thin_aborted_changes() tells you if they had uncommitted changes.
+ *
+ * If this call fails it's only useful to call dm_pool_metadata_close().
+ * All other methods will fail with -EINVAL.
+ */
+int dm_pool_abort_metadata(struct dm_pool_metadata *pmd);
+
+/*
  * Set/get userspace transaction id.
  */
 int dm_pool_set_metadata_transaction_id(struct dm_pool_metadata *pmd,
@@ -119,7 +130,7 @@ dm_thin_id dm_thin_dev_id(struct dm_thin_device *td);
 
 struct dm_thin_lookup_result {
 	dm_block_t block;
-	int shared;
+	unsigned shared:1;
 };
 
 /*
@@ -147,6 +158,10 @@ int dm_thin_remove_block(struct dm_thin_device *td, dm_block_t block);
 /*
  * Queries.
  */
+bool dm_thin_changed_this_transaction(struct dm_thin_device *td);
+
+bool dm_thin_aborted_changes(struct dm_thin_device *td);
+
 int dm_thin_get_highest_mapped_block(struct dm_thin_device *td,
 				     dm_block_t *highest_mapped);
 
@@ -171,6 +186,12 @@ int dm_pool_get_data_dev_size(struct dm_pool_metadata *pmd, dm_block_t *result);
  */
 int dm_pool_resize_data_dev(struct dm_pool_metadata *pmd, dm_block_t new_size);
 
+/*
+ * Flicks the underlying block manager into read only mode, so you know
+ * that nothing is changing.
+ */
+void dm_pool_metadata_read_only(struct dm_pool_metadata *pmd);
+
 /*----------------------------------------------------------------*/
 
 #endif
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index ce59824fb41..af1fc3b2c2a 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -1,10 +1,11 @@
 /*
- * Copyright (C) 2011 Red Hat UK.
+ * Copyright (C) 2011-2012 Red Hat UK.
  *
  * This file is released under the GPL.
  */
 
 #include "dm-thin-metadata.h"
+#include "dm.h"
 
 #include <linux/device-mapper.h>
 #include <linux/dm-io.h>
@@ -19,7 +20,7 @@
 /*
  * Tunable constants
  */
-#define ENDIO_HOOK_POOL_SIZE 10240
+#define ENDIO_HOOK_POOL_SIZE 1024
 #define DEFERRED_SET_SIZE 64
 #define MAPPING_POOL_SIZE 1024
 #define PRISON_CELLS 1024
@@ -496,12 +497,27 @@ static void build_virtual_key(struct dm_thin_device *td, dm_block_t b,
  */
 struct dm_thin_new_mapping;
 
+/*
+ * The pool runs in 3 modes.  Ordered in degraded order for comparisons.
+ */
+enum pool_mode {
+	PM_WRITE,		/* metadata may be changed */
+	PM_READ_ONLY,		/* metadata may not be changed */
+	PM_FAIL,		/* all I/O fails */
+};
+
 struct pool_features {
+	enum pool_mode mode;
+
 	unsigned zero_new_blocks:1;
 	unsigned discard_enabled:1;
 	unsigned discard_passdown:1;
 };
 
+struct thin_c;
+typedef void (*process_bio_fn)(struct thin_c *tc, struct bio *bio);
+typedef void (*process_mapping_fn)(struct dm_thin_new_mapping *m);
+
 struct pool {
 	struct list_head list;
 	struct dm_target *ti;	/* Only set if a pool target is bound */
@@ -510,10 +526,9 @@ struct pool {
 	struct block_device *md_dev;
 	struct dm_pool_metadata *pmd;
 
-	uint32_t sectors_per_block;
-	unsigned block_shift;
-	dm_block_t offset_mask;
 	dm_block_t low_water_blocks;
+	uint32_t sectors_per_block;
+	int sectors_per_block_shift;
 
 	struct pool_features pf;
 	unsigned low_water_triggered:1;	/* A dm event has been sent */
@@ -526,8 +541,8 @@ struct pool {
 	struct work_struct worker;
 	struct delayed_work waker;
 
-	unsigned ref_count;
 	unsigned long last_commit_jiffies;
+	unsigned ref_count;
 
 	spinlock_t lock;
 	struct bio_list deferred_bios;
@@ -543,8 +558,17 @@ struct pool {
 	struct dm_thin_new_mapping *next_mapping;
 	mempool_t *mapping_pool;
 	mempool_t *endio_hook_pool;
+
+	process_bio_fn process_bio;
+	process_bio_fn process_discard;
+
+	process_mapping_fn process_prepared_mapping;
+	process_mapping_fn process_prepared_discard;
 };
 
+static enum pool_mode get_pool_mode(struct pool *pool);
+static void set_pool_mode(struct pool *pool, enum pool_mode mode);
+
 /*
  * Target context for a pool.
  */
@@ -679,16 +703,28 @@ static void requeue_io(struct thin_c *tc)
 
 static dm_block_t get_bio_block(struct thin_c *tc, struct bio *bio)
 {
-	return bio->bi_sector >> tc->pool->block_shift;
+	sector_t block_nr = bio->bi_sector;
+
+	if (tc->pool->sectors_per_block_shift < 0)
+		(void) sector_div(block_nr, tc->pool->sectors_per_block);
+	else
+		block_nr >>= tc->pool->sectors_per_block_shift;
+
+	return block_nr;
 }
 
 static void remap(struct thin_c *tc, struct bio *bio, dm_block_t block)
 {
 	struct pool *pool = tc->pool;
+	sector_t bi_sector = bio->bi_sector;
 
 	bio->bi_bdev = tc->pool_dev->bdev;
-	bio->bi_sector = (block << pool->block_shift) +
-		(bio->bi_sector & pool->offset_mask);
+	if (tc->pool->sectors_per_block_shift < 0)
+		bio->bi_sector = (block * pool->sectors_per_block) +
+				 sector_div(bi_sector, pool->sectors_per_block);
+	else
+		bio->bi_sector = (block << pool->sectors_per_block_shift) |
+				(bi_sector & (pool->sectors_per_block - 1));
 }
 
 static void remap_to_origin(struct thin_c *tc, struct bio *bio)
@@ -696,21 +732,39 @@ static void remap_to_origin(struct thin_c *tc, struct bio *bio)
 	bio->bi_bdev = tc->origin_dev->bdev;
 }
 
+static int bio_triggers_commit(struct thin_c *tc, struct bio *bio)
+{
+	return (bio->bi_rw & (REQ_FLUSH | REQ_FUA)) &&
+		dm_thin_changed_this_transaction(tc->td);
+}
+
 static void issue(struct thin_c *tc, struct bio *bio)
 {
 	struct pool *pool = tc->pool;
 	unsigned long flags;
 
+	if (!bio_triggers_commit(tc, bio)) {
+		generic_make_request(bio);
+		return;
+	}
+
 	/*
-	 * Batch together any FUA/FLUSH bios we find and then issue
-	 * a single commit for them in process_deferred_bios().
+	 * Complete bio with an error if earlier I/O caused changes to
+	 * the metadata that can't be committed e.g, due to I/O errors
+	 * on the metadata device.
 	 */
-	if (bio->bi_rw & (REQ_FLUSH | REQ_FUA)) {
-		spin_lock_irqsave(&pool->lock, flags);
-		bio_list_add(&pool->deferred_flush_bios, bio);
-		spin_unlock_irqrestore(&pool->lock, flags);
-	} else
-		generic_make_request(bio);
+	if (dm_thin_aborted_changes(tc->td)) {
+		bio_io_error(bio);
+		return;
+	}
+
+	/*
+	 * Batch together any bios that trigger commits and then issue a
+	 * single commit for them in process_deferred_bios().
+	 */
+	spin_lock_irqsave(&pool->lock, flags);
+	bio_list_add(&pool->deferred_flush_bios, bio);
+	spin_unlock_irqrestore(&pool->lock, flags);
 }
 
 static void remap_to_origin_and_issue(struct thin_c *tc, struct bio *bio)
@@ -847,6 +901,14 @@ static void cell_defer_except(struct thin_c *tc, struct dm_bio_prison_cell *cell
 	wake_worker(pool);
 }
 
+static void process_prepared_mapping_fail(struct dm_thin_new_mapping *m)
+{
+	if (m->bio)
+		m->bio->bi_end_io = m->saved_bi_end_io;
+	cell_error(m->cell);
+	list_del(&m->list);
+	mempool_free(m, m->tc->pool->mapping_pool);
+}
 static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 {
 	struct thin_c *tc = m->tc;
@@ -859,7 +921,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 
 	if (m->err) {
 		cell_error(m->cell);
-		return;
+		goto out;
 	}
 
 	/*
@@ -871,7 +933,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 	if (r) {
 		DMERR("dm_thin_insert_block() failed");
 		cell_error(m->cell);
-		return;
+		goto out;
 	}
 
 	/*
@@ -886,22 +948,25 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 	} else
 		cell_defer(tc, m->cell, m->data_block);
 
+out:
 	list_del(&m->list);
 	mempool_free(m, tc->pool->mapping_pool);
 }
 
-static void process_prepared_discard(struct dm_thin_new_mapping *m)
+static void process_prepared_discard_fail(struct dm_thin_new_mapping *m)
 {
-	int r;
 	struct thin_c *tc = m->tc;
 
-	r = dm_thin_remove_block(tc->td, m->virt_block);
-	if (r)
-		DMERR("dm_thin_remove_block() failed");
+	bio_io_error(m->bio);
+	cell_defer_except(tc, m->cell);
+	cell_defer_except(tc, m->cell2);
+	mempool_free(m, tc->pool->mapping_pool);
+}
+
+static void process_prepared_discard_passdown(struct dm_thin_new_mapping *m)
+{
+	struct thin_c *tc = m->tc;
 
-	/*
-	 * Pass the discard down to the underlying device?
-	 */
 	if (m->pass_discard)
 		remap_and_issue(tc, m->bio, m->data_block);
 	else
@@ -912,8 +977,20 @@ static void process_prepared_discard(struct dm_thin_new_mapping *m)
 	mempool_free(m, tc->pool->mapping_pool);
 }
 
+static void process_prepared_discard(struct dm_thin_new_mapping *m)
+{
+	int r;
+	struct thin_c *tc = m->tc;
+
+	r = dm_thin_remove_block(tc->td, m->virt_block);
+	if (r)
+		DMERR("dm_thin_remove_block() failed");
+
+	process_prepared_discard_passdown(m);
+}
+
 static void process_prepared(struct pool *pool, struct list_head *head,
-			     void (*fn)(struct dm_thin_new_mapping *))
+			     process_mapping_fn *fn)
 {
 	unsigned long flags;
 	struct list_head maps;
@@ -925,7 +1002,7 @@ static void process_prepared(struct pool *pool, struct list_head *head,
 	spin_unlock_irqrestore(&pool->lock, flags);
 
 	list_for_each_entry_safe(m, tmp, &maps, list)
-		fn(m);
+		(*fn)(m);
 }
 
 /*
@@ -933,9 +1010,7 @@ static void process_prepared(struct pool *pool, struct list_head *head,
  */
 static int io_overlaps_block(struct pool *pool, struct bio *bio)
 {
-	return !(bio->bi_sector & pool->offset_mask) &&
-		(bio->bi_size == (pool->sectors_per_block << SECTOR_SHIFT));
-
+	return bio->bi_size == (pool->sectors_per_block << SECTOR_SHIFT);
 }
 
 static int io_overwrites_block(struct pool *pool, struct bio *bio)
@@ -1093,6 +1168,35 @@ static void schedule_zero(struct thin_c *tc, dm_block_t virt_block,
 	}
 }
 
+static int commit(struct pool *pool)
+{
+	int r;
+
+	r = dm_pool_commit_metadata(pool->pmd);
+	if (r)
+		DMERR("commit failed, error = %d", r);
+
+	return r;
+}
+
+/*
+ * A non-zero return indicates read_only or fail_io mode.
+ * Many callers don't care about the return value.
+ */
+static int commit_or_fallback(struct pool *pool)
+{
+	int r;
+
+	if (get_pool_mode(pool) != PM_WRITE)
+		return -EINVAL;
+
+	r = commit(pool);
+	if (r)
+		set_pool_mode(pool, PM_READ_ONLY);
+
+	return r;
+}
+
 static int alloc_data_block(struct thin_c *tc, dm_block_t *result)
 {
 	int r;
@@ -1121,12 +1225,7 @@ static int alloc_data_block(struct thin_c *tc, dm_block_t *result)
 			 * Try to commit to see if that will free up some
 			 * more space.
 			 */
-			r = dm_pool_commit_metadata(pool->pmd);
-			if (r) {
-				DMERR("%s: dm_pool_commit_metadata() failed, error = %d",
-				      __func__, r);
-				return r;
-			}
+			(void) commit_or_fallback(pool);
 
 			r = dm_pool_get_free_block_count(pool->pmd, &free_blocks);
 			if (r)
@@ -1218,7 +1317,7 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 			 */
 			m = get_next_mapping(pool);
 			m->tc = tc;
-			m->pass_discard = (!lookup_result.shared) & pool->pf.discard_passdown;
+			m->pass_discard = (!lookup_result.shared) && pool->pf.discard_passdown;
 			m->virt_block = block;
 			m->data_block = lookup_result.block;
 			m->cell = cell;
@@ -1234,18 +1333,16 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 			}
 		} else {
 			/*
-			 * This path is hit if people are ignoring
-			 * limits->discard_granularity.  It ignores any
-			 * part of the discard that is in a subsequent
-			 * block.
+			 * The DM core makes sure that the discard doesn't span
+			 * a block boundary.  So we submit the discard of a
+			 * partial block appropriately.
 			 */
-			sector_t offset = bio->bi_sector - (block << pool->block_shift);
-			unsigned remaining = (pool->sectors_per_block - offset) << 9;
-			bio->bi_size = min(bio->bi_size, remaining);
-
 			cell_release_singleton(cell, bio);
 			cell_release_singleton(cell2, bio);
-			remap_and_issue(tc, bio, lookup_result.block);
+			if ((!lookup_result.shared) && pool->pf.discard_passdown)
+				remap_and_issue(tc, bio, lookup_result.block);
+			else
+				bio_endio(bio, 0);
 		}
 		break;
 
@@ -1307,7 +1404,7 @@ static void process_shared_bio(struct thin_c *tc, struct bio *bio,
 	if (bio_detain(pool->prison, &key, bio, &cell))
 		return;
 
-	if (bio_data_dir(bio) == WRITE)
+	if (bio_data_dir(bio) == WRITE && bio->bi_size)
 		break_sharing(tc, bio, block, &key, lookup_result, cell);
 	else {
 		struct dm_thin_endio_hook *h = dm_get_mapinfo(bio)->ptr;
@@ -1359,6 +1456,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
 
 	default:
 		DMERR("%s: alloc_data_block() failed, error = %d", __func__, r);
+		set_pool_mode(tc->pool, PM_READ_ONLY);
 		cell_error(cell);
 		break;
 	}
@@ -1416,6 +1514,49 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 	}
 }
 
+static void process_bio_read_only(struct thin_c *tc, struct bio *bio)
+{
+	int r;
+	int rw = bio_data_dir(bio);
+	dm_block_t block = get_bio_block(tc, bio);
+	struct dm_thin_lookup_result lookup_result;
+
+	r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
+	switch (r) {
+	case 0:
+		if (lookup_result.shared && (rw == WRITE) && bio->bi_size)
+			bio_io_error(bio);
+		else
+			remap_and_issue(tc, bio, lookup_result.block);
+		break;
+
+	case -ENODATA:
+		if (rw != READ) {
+			bio_io_error(bio);
+			break;
+		}
+
+		if (tc->origin_dev) {
+			remap_to_origin_and_issue(tc, bio);
+			break;
+		}
+
+		zero_fill_bio(bio);
+		bio_endio(bio, 0);
+		break;
+
+	default:
+		DMERR("dm_thin_find_block() failed, error = %d", r);
+		bio_io_error(bio);
+		break;
+	}
+}
+
+static void process_bio_fail(struct thin_c *tc, struct bio *bio)
+{
+	bio_io_error(bio);
+}
+
 static int need_commit_due_to_time(struct pool *pool)
 {
 	return jiffies < pool->last_commit_jiffies ||
@@ -1427,7 +1568,6 @@ static void process_deferred_bios(struct pool *pool)
 	unsigned long flags;
 	struct bio *bio;
 	struct bio_list bios;
-	int r;
 
 	bio_list_init(&bios);
 
@@ -1454,9 +1594,9 @@ static void process_deferred_bios(struct pool *pool)
 		}
 
 		if (bio->bi_rw & REQ_DISCARD)
-			process_discard(tc, bio);
+			pool->process_discard(tc, bio);
 		else
-			process_bio(tc, bio);
+			pool->process_bio(tc, bio);
 	}
 
 	/*
@@ -1472,10 +1612,7 @@ static void process_deferred_bios(struct pool *pool)
 	if (bio_list_empty(&bios) && !need_commit_due_to_time(pool))
 		return;
 
-	r = dm_pool_commit_metadata(pool->pmd);
-	if (r) {
-		DMERR("%s: dm_pool_commit_metadata() failed, error = %d",
-		      __func__, r);
+	if (commit_or_fallback(pool)) {
 		while ((bio = bio_list_pop(&bios)))
 			bio_io_error(bio);
 		return;
@@ -1490,8 +1627,8 @@ static void do_worker(struct work_struct *ws)
 {
 	struct pool *pool = container_of(ws, struct pool, worker);
 
-	process_prepared(pool, &pool->prepared_mappings, process_prepared_mapping);
-	process_prepared(pool, &pool->prepared_discards, process_prepared_discard);
+	process_prepared(pool, &pool->prepared_mappings, &pool->process_prepared_mapping);
+	process_prepared(pool, &pool->prepared_discards, &pool->process_prepared_discard);
 	process_deferred_bios(pool);
 }
 
@@ -1508,6 +1645,52 @@ static void do_waker(struct work_struct *ws)
 
 /*----------------------------------------------------------------*/
 
+static enum pool_mode get_pool_mode(struct pool *pool)
+{
+	return pool->pf.mode;
+}
+
+static void set_pool_mode(struct pool *pool, enum pool_mode mode)
+{
+	int r;
+
+	pool->pf.mode = mode;
+
+	switch (mode) {
+	case PM_FAIL:
+		DMERR("switching pool to failure mode");
+		pool->process_bio = process_bio_fail;
+		pool->process_discard = process_bio_fail;
+		pool->process_prepared_mapping = process_prepared_mapping_fail;
+		pool->process_prepared_discard = process_prepared_discard_fail;
+		break;
+
+	case PM_READ_ONLY:
+		DMERR("switching pool to read-only mode");
+		r = dm_pool_abort_metadata(pool->pmd);
+		if (r) {
+			DMERR("aborting transaction failed");
+			set_pool_mode(pool, PM_FAIL);
+		} else {
+			dm_pool_metadata_read_only(pool->pmd);
+			pool->process_bio = process_bio_read_only;
+			pool->process_discard = process_discard;
+			pool->process_prepared_mapping = process_prepared_mapping_fail;
+			pool->process_prepared_discard = process_prepared_discard_passdown;
+		}
+		break;
+
+	case PM_WRITE:
+		pool->process_bio = process_bio;
+		pool->process_discard = process_discard;
+		pool->process_prepared_mapping = process_prepared_mapping;
+		pool->process_prepared_discard = process_prepared_discard;
+		break;
+	}
+}
+
+/*----------------------------------------------------------------*/
+
 /*
  * Mapping functions.
  */
@@ -1553,6 +1736,12 @@ static int thin_bio_map(struct dm_target *ti, struct bio *bio,
 	struct dm_thin_lookup_result result;
 
 	map_context->ptr = thin_hook_bio(tc, bio);
+
+	if (get_pool_mode(tc->pool) == PM_FAIL) {
+		bio_io_error(bio);
+		return DM_MAPIO_SUBMITTED;
+	}
+
 	if (bio->bi_rw & (REQ_DISCARD | REQ_FLUSH | REQ_FUA)) {
 		thin_defer_bio(tc, bio);
 		return DM_MAPIO_SUBMITTED;
@@ -1589,14 +1778,35 @@ static int thin_bio_map(struct dm_target *ti, struct bio *bio,
 		break;
 
 	case -ENODATA:
+		if (get_pool_mode(tc->pool) == PM_READ_ONLY) {
+			/*
+			 * This block isn't provisioned, and we have no way
+			 * of doing so.  Just error it.
+			 */
+			bio_io_error(bio);
+			r = DM_MAPIO_SUBMITTED;
+			break;
+		}
+		/* fall through */
+
+	case -EWOULDBLOCK:
 		/*
 		 * In future, the failed dm_thin_find_block above could
 		 * provide the hint to load the metadata into cache.
 		 */
-	case -EWOULDBLOCK:
 		thin_defer_bio(tc, bio);
 		r = DM_MAPIO_SUBMITTED;
 		break;
+
+	default:
+		/*
+		 * Must always call bio_io_error on failure.
+		 * dm_thin_find_block can fail with -EINVAL if the
+		 * pool is switched to fail-io mode.
+		 */
+		bio_io_error(bio);
+		r = DM_MAPIO_SUBMITTED;
+		break;
 	}
 
 	return r;
@@ -1633,15 +1843,26 @@ static int bind_control_target(struct pool *pool, struct dm_target *ti)
 {
 	struct pool_c *pt = ti->private;
 
+	/*
+	 * We want to make sure that degraded pools are never upgraded.
+	 */
+	enum pool_mode old_mode = pool->pf.mode;
+	enum pool_mode new_mode = pt->pf.mode;
+
+	if (old_mode > new_mode)
+		new_mode = old_mode;
+
 	pool->ti = ti;
 	pool->low_water_blocks = pt->low_water_blocks;
 	pool->pf = pt->pf;
+	set_pool_mode(pool, new_mode);
 
 	/*
 	 * If discard_passdown was enabled verify that the data device
 	 * supports discards.  Disable discard_passdown if not; otherwise
 	 * -EOPNOTSUPP will be returned.
 	 */
+	/* FIXME: pull this out into a sep fn. */
 	if (pt->pf.discard_passdown) {
 		struct request_queue *q = bdev_get_queue(pt->data_dev->bdev);
 		if (!q || !blk_queue_discard(q)) {
@@ -1667,6 +1888,7 @@ static void unbind_control_target(struct pool *pool, struct dm_target *ti)
 /* Initialize pool features. */
 static void pool_features_init(struct pool_features *pf)
 {
+	pf->mode = PM_WRITE;
 	pf->zero_new_blocks = 1;
 	pf->discard_enabled = 1;
 	pf->discard_passdown = 1;
@@ -1697,14 +1919,16 @@ static struct kmem_cache *_endio_hook_cache;
 
 static struct pool *pool_create(struct mapped_device *pool_md,
 				struct block_device *metadata_dev,
-				unsigned long block_size, char **error)
+				unsigned long block_size,
+				int read_only, char **error)
 {
 	int r;
 	void *err_p;
 	struct pool *pool;
 	struct dm_pool_metadata *pmd;
+	bool format_device = read_only ? false : true;
 
-	pmd = dm_pool_metadata_open(metadata_dev, block_size);
+	pmd = dm_pool_metadata_open(metadata_dev, block_size, format_device);
 	if (IS_ERR(pmd)) {
 		*error = "Error creating metadata object";
 		return (struct pool *)pmd;
@@ -1719,8 +1943,10 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 
 	pool->pmd = pmd;
 	pool->sectors_per_block = block_size;
-	pool->block_shift = ffs(block_size) - 1;
-	pool->offset_mask = block_size - 1;
+	if (block_size & (block_size - 1))
+		pool->sectors_per_block_shift = -1;
+	else
+		pool->sectors_per_block_shift = __ffs(block_size);
 	pool->low_water_blocks = 0;
 	pool_features_init(&pool->pf);
 	pool->prison = prison_create(PRISON_CELLS);
@@ -1819,25 +2045,29 @@ static void __pool_dec(struct pool *pool)
 
 static struct pool *__pool_find(struct mapped_device *pool_md,
 				struct block_device *metadata_dev,
-				unsigned long block_size, char **error,
-				int *created)
+				unsigned long block_size, int read_only,
+				char **error, int *created)
 {
 	struct pool *pool = __pool_table_lookup_metadata_dev(metadata_dev);
 
 	if (pool) {
-		if (pool->pool_md != pool_md)
+		if (pool->pool_md != pool_md) {
+			*error = "metadata device already in use by a pool";
 			return ERR_PTR(-EBUSY);
+		}
 		__pool_inc(pool);
 
 	} else {
 		pool = __pool_table_lookup(pool_md);
 		if (pool) {
-			if (pool->md_dev != metadata_dev)
+			if (pool->md_dev != metadata_dev) {
+				*error = "different pool cannot replace a pool";
 				return ERR_PTR(-EINVAL);
+			}
 			__pool_inc(pool);
 
 		} else {
-			pool = pool_create(pool_md, metadata_dev, block_size, error);
+			pool = pool_create(pool_md, metadata_dev, block_size, read_only, error);
 			*created = 1;
 		}
 	}
@@ -1888,19 +2118,23 @@ static int parse_pool_features(struct dm_arg_set *as, struct pool_features *pf,
 		arg_name = dm_shift_arg(as);
 		argc--;
 
-		if (!strcasecmp(arg_name, "skip_block_zeroing")) {
+		if (!strcasecmp(arg_name, "skip_block_zeroing"))
 			pf->zero_new_blocks = 0;
-			continue;
-		} else if (!strcasecmp(arg_name, "ignore_discard")) {
+
+		else if (!strcasecmp(arg_name, "ignore_discard"))
 			pf->discard_enabled = 0;
-			continue;
-		} else if (!strcasecmp(arg_name, "no_discard_passdown")) {
+
+		else if (!strcasecmp(arg_name, "no_discard_passdown"))
 			pf->discard_passdown = 0;
-			continue;
-		}
 
-		ti->error = "Unrecognised pool feature requested";
-		r = -EINVAL;
+		else if (!strcasecmp(arg_name, "read_only"))
+			pf->mode = PM_READ_ONLY;
+
+		else {
+			ti->error = "Unrecognised pool feature requested";
+			r = -EINVAL;
+			break;
+		}
 	}
 
 	return r;
@@ -1964,7 +2198,7 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	if (kstrtoul(argv[2], 10, &block_size) || !block_size ||
 	    block_size < DATA_DEV_BLOCK_SIZE_MIN_SECTORS ||
 	    block_size > DATA_DEV_BLOCK_SIZE_MAX_SECTORS ||
-	    !is_power_of_2(block_size)) {
+	    block_size & (DATA_DEV_BLOCK_SIZE_MIN_SECTORS - 1)) {
 		ti->error = "Invalid block size";
 		r = -EINVAL;
 		goto out;
@@ -1993,7 +2227,7 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	}
 
 	pool = __pool_find(dm_table_get_md(ti->table), metadata_dev->bdev,
-			   block_size, &ti->error, &pool_created);
+			   block_size, pf.mode == PM_READ_ONLY, &ti->error, &pool_created);
 	if (IS_ERR(pool)) {
 		r = PTR_ERR(pool);
 		goto out_free_pt;
@@ -2011,6 +2245,15 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		goto out_flags_changed;
 	}
 
+	/*
+	 * The block layer requires discard_granularity to be a power of 2.
+	 */
+	if (pf.discard_enabled && !is_power_of_2(block_size)) {
+		ti->error = "Discard support must be disabled when the block size is not a power of 2";
+		r = -EINVAL;
+		goto out_flags_changed;
+	}
+
 	pt->pool = pool;
 	pt->ti = ti;
 	pt->metadata_dev = metadata_dev;
@@ -2030,7 +2273,7 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		 * stacking of discard limits (this keeps the pool and
 		 * thin devices' discard limits consistent).
 		 */
-		ti->discards_supported = 1;
+		ti->discards_supported = true;
 	}
 	ti->private = pt;
 
@@ -2090,7 +2333,8 @@ static int pool_preresume(struct dm_target *ti)
 	int r;
 	struct pool_c *pt = ti->private;
 	struct pool *pool = pt->pool;
-	dm_block_t data_size, sb_data_size;
+	sector_t data_size = ti->len;
+	dm_block_t sb_data_size;
 
 	/*
 	 * Take control of the pool object.
@@ -2099,7 +2343,8 @@ static int pool_preresume(struct dm_target *ti)
 	if (r)
 		return r;
 
-	data_size = ti->len >> pool->block_shift;
+	(void) sector_div(data_size, pool->sectors_per_block);
+
 	r = dm_pool_get_data_dev_size(pool->pmd, &sb_data_size);
 	if (r) {
 		DMERR("failed to retrieve data device size");
@@ -2108,22 +2353,19 @@ static int pool_preresume(struct dm_target *ti)
 
 	if (data_size < sb_data_size) {
 		DMERR("pool target too small, is %llu blocks (expected %llu)",
-		      data_size, sb_data_size);
+		      (unsigned long long)data_size, sb_data_size);
 		return -EINVAL;
 
 	} else if (data_size > sb_data_size) {
 		r = dm_pool_resize_data_dev(pool->pmd, data_size);
 		if (r) {
 			DMERR("failed to resize data device");
+			/* FIXME Stricter than necessary: Rollback transaction instead here */
+			set_pool_mode(pool, PM_READ_ONLY);
 			return r;
 		}
 
-		r = dm_pool_commit_metadata(pool->pmd);
-		if (r) {
-			DMERR("%s: dm_pool_commit_metadata() failed, error = %d",
-			      __func__, r);
-			return r;
-		}
+		(void) commit_or_fallback(pool);
 	}
 
 	return 0;
@@ -2146,19 +2388,12 @@ static void pool_resume(struct dm_target *ti)
 
 static void pool_postsuspend(struct dm_target *ti)
 {
-	int r;
 	struct pool_c *pt = ti->private;
 	struct pool *pool = pt->pool;
 
 	cancel_delayed_work(&pool->waker);
 	flush_workqueue(pool->wq);
-
-	r = dm_pool_commit_metadata(pool->pmd);
-	if (r < 0) {
-		DMERR("%s: dm_pool_commit_metadata() failed, error = %d",
-		      __func__, r);
-		/* FIXME: invalidate device? error the next FUA or FLUSH bio ?*/
-	}
+	(void) commit_or_fallback(pool);
 }
 
 static int check_arg_count(unsigned argc, unsigned args_required)
@@ -2292,12 +2527,7 @@ static int process_reserve_metadata_snap_mesg(unsigned argc, char **argv, struct
 	if (r)
 		return r;
 
-	r = dm_pool_commit_metadata(pool->pmd);
-	if (r) {
-		DMERR("%s: dm_pool_commit_metadata() failed, error = %d",
-		      __func__, r);
-		return r;
-	}
+	(void) commit_or_fallback(pool);
 
 	r = dm_pool_reserve_metadata_snap(pool->pmd);
 	if (r)
@@ -2358,25 +2588,41 @@ static int pool_message(struct dm_target *ti, unsigned argc, char **argv)
 	else
 		DMWARN("Unrecognised thin pool target message received: %s", argv[0]);
 
-	if (!r) {
-		r = dm_pool_commit_metadata(pool->pmd);
-		if (r)
-			DMERR("%s message: dm_pool_commit_metadata() failed, error = %d",
-			      argv[0], r);
-	}
+	if (!r)
+		(void) commit_or_fallback(pool);
 
 	return r;
 }
 
+static void emit_flags(struct pool_features *pf, char *result,
+		       unsigned sz, unsigned maxlen)
+{
+	unsigned count = !pf->zero_new_blocks + !pf->discard_enabled +
+		!pf->discard_passdown + (pf->mode == PM_READ_ONLY);
+	DMEMIT("%u ", count);
+
+	if (!pf->zero_new_blocks)
+		DMEMIT("skip_block_zeroing ");
+
+	if (!pf->discard_enabled)
+		DMEMIT("ignore_discard ");
+
+	if (!pf->discard_passdown)
+		DMEMIT("no_discard_passdown ");
+
+	if (pf->mode == PM_READ_ONLY)
+		DMEMIT("read_only ");
+}
+
 /*
  * Status line is:
  *    <transaction id> <used metadata sectors>/<total metadata sectors>
  *    <used data sectors>/<total data sectors> <held metadata root>
  */
 static int pool_status(struct dm_target *ti, status_type_t type,
-		       char *result, unsigned maxlen)
+		       unsigned status_flags, char *result, unsigned maxlen)
 {
-	int r, count;
+	int r;
 	unsigned sz = 0;
 	uint64_t transaction_id;
 	dm_block_t nr_free_blocks_data;
@@ -2391,6 +2637,15 @@ static int pool_status(struct dm_target *ti, status_type_t type,
 
 	switch (type) {
 	case STATUSTYPE_INFO:
+		if (get_pool_mode(pool) == PM_FAIL) {
+			DMEMIT("Fail");
+			break;
+		}
+
+		/* Commit to ensure statistics aren't out-of-date */
+		if (!(status_flags & DM_STATUS_NOFLUSH_FLAG) && !dm_suspended(ti))
+			(void) commit_or_fallback(pool);
+
 		r = dm_pool_get_metadata_transaction_id(pool->pmd,
 							&transaction_id);
 		if (r)
@@ -2426,9 +2681,19 @@ static int pool_status(struct dm_target *ti, status_type_t type,
 		       (unsigned long long)nr_blocks_data);
 
 		if (held_root)
-			DMEMIT("%llu", held_root);
+			DMEMIT("%llu ", held_root);
+		else
+			DMEMIT("- ");
+
+		if (pool->pf.mode == PM_READ_ONLY)
+			DMEMIT("ro ");
+		else
+			DMEMIT("rw ");
+
+		if (pool->pf.discard_enabled && pool->pf.discard_passdown)
+			DMEMIT("discard_passdown");
 		else
-			DMEMIT("-");
+			DMEMIT("no_discard_passdown");
 
 		break;
 
@@ -2438,20 +2703,7 @@ static int pool_status(struct dm_target *ti, status_type_t type,
 		       format_dev_t(buf2, pt->data_dev->bdev->bd_dev),
 		       (unsigned long)pool->sectors_per_block,
 		       (unsigned long long)pt->low_water_blocks);
-
-		count = !pool->pf.zero_new_blocks + !pool->pf.discard_enabled +
-			!pt->pf.discard_passdown;
-		DMEMIT("%u ", count);
-
-		if (!pool->pf.zero_new_blocks)
-			DMEMIT("skip_block_zeroing ");
-
-		if (!pool->pf.discard_enabled)
-			DMEMIT("ignore_discard ");
-
-		if (!pt->pf.discard_passdown)
-			DMEMIT("no_discard_passdown ");
-
+		emit_flags(&pt->pf, result, sz, maxlen);
 		break;
 	}
 
@@ -2489,7 +2741,8 @@ static void set_discard_limits(struct pool *pool, struct queue_limits *limits)
 
 	/*
 	 * This is just a hint, and not enforced.  We have to cope with
-	 * bios that overlap 2 blocks.
+	 * bios that cover a block partially.  A discard that spans a block
+	 * boundary is not sent to this target.
 	 */
 	limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
 	limits->discard_zeroes_data = pool->pf.zero_new_blocks;
@@ -2510,7 +2763,7 @@ static struct target_type pool_target = {
 	.name = "thin-pool",
 	.features = DM_TARGET_SINGLETON | DM_TARGET_ALWAYS_WRITEABLE |
 		    DM_TARGET_IMMUTABLE,
-	.version = {1, 2, 0},
+	.version = {1, 3, 0},
 	.module = THIS_MODULE,
 	.ctr = pool_ctr,
 	.dtr = pool_dtr,
@@ -2615,19 +2868,31 @@ static int thin_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	}
 	__pool_inc(tc->pool);
 
+	if (get_pool_mode(tc->pool) == PM_FAIL) {
+		ti->error = "Couldn't open thin device, Pool is in fail mode";
+		goto bad_thin_open;
+	}
+
 	r = dm_pool_open_thin_device(tc->pool->pmd, tc->dev_id, &tc->td);
 	if (r) {
 		ti->error = "Couldn't open thin internal device";
 		goto bad_thin_open;
 	}
 
-	ti->split_io = tc->pool->sectors_per_block;
+	r = dm_set_target_max_io_len(ti, tc->pool->sectors_per_block);
+	if (r)
+		goto bad_thin_open;
+
 	ti->num_flush_requests = 1;
+	ti->flush_supported = true;
 
 	/* In case the pool supports discards, pass them on. */
 	if (tc->pool->pf.discard_enabled) {
-		ti->discards_supported = 1;
+		ti->discards_supported = true;
 		ti->num_discard_requests = 1;
+		ti->discard_zeroes_data_unsupported = true;
+		/* Discard requests must be split on a block boundary */
+		ti->split_discard_requests = true;
 	}
 
 	dm_put(pool_md);
@@ -2708,7 +2973,7 @@ static void thin_postsuspend(struct dm_target *ti)
  * <nr mapped sectors> <highest mapped sector>
  */
 static int thin_status(struct dm_target *ti, status_type_t type,
-		       char *result, unsigned maxlen)
+		       unsigned status_flags, char *result, unsigned maxlen)
 {
 	int r;
 	ssize_t sz = 0;
@@ -2716,6 +2981,11 @@ static int thin_status(struct dm_target *ti, status_type_t type,
 	char buf[BDEVNAME_SIZE];
 	struct thin_c *tc = ti->private;
 
+	if (get_pool_mode(tc->pool) == PM_FAIL) {
+		DMEMIT("Fail");
+		return 0;
+	}
+
 	if (!tc->td)
 		DMEMIT("-");
 	else {
@@ -2753,19 +3023,21 @@ static int thin_status(struct dm_target *ti, status_type_t type,
 static int thin_iterate_devices(struct dm_target *ti,
 				iterate_devices_callout_fn fn, void *data)
 {
-	dm_block_t blocks;
+	sector_t blocks;
 	struct thin_c *tc = ti->private;
+	struct pool *pool = tc->pool;
 
 	/*
 	 * We can't call dm_pool_get_data_dev_size() since that blocks.  So
 	 * we follow a more convoluted path through to the pool's target.
 	 */
-	if (!tc->pool->ti)
+	if (!pool->ti)
 		return 0;	/* nothing is bound */
 
-	blocks = tc->pool->ti->len >> tc->pool->block_shift;
+	blocks = pool->ti->len;
+	(void) sector_div(blocks, pool->sectors_per_block);
 	if (blocks)
-		return fn(ti, tc->pool_dev, 0, tc->pool->sectors_per_block * blocks, data);
+		return fn(ti, tc->pool_dev, 0, pool->sectors_per_block * blocks, data);
 
 	return 0;
 }
@@ -2782,7 +3054,7 @@ static void thin_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 static struct target_type thin_target = {
 	.name = "thin",
-	.version = {1, 1, 0},
+	.version = {1, 3, 0},
 	.module	= THIS_MODULE,
 	.ctr = thin_ctr,
 	.dtr = thin_dtr,
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index fa365d39b61..254d19268ad 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -515,7 +515,7 @@ static int verity_map(struct dm_target *ti, struct bio *bio,
  * Status: V (valid) or C (corruption found)
  */
 static int verity_status(struct dm_target *ti, status_type_t type,
-			 char *result, unsigned maxlen)
+			 unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct dm_verity *v = ti->private;
 	unsigned sz = 0;
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index e24143cc204..4e09b6ff5b4 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -968,22 +968,41 @@ static sector_t max_io_len_target_boundary(sector_t sector, struct dm_target *ti
 static sector_t max_io_len(sector_t sector, struct dm_target *ti)
 {
 	sector_t len = max_io_len_target_boundary(sector, ti);
+	sector_t offset, max_len;
 
 	/*
-	 * Does the target need to split even further ?
+	 * Does the target need to split even further?
 	 */
-	if (ti->split_io) {
-		sector_t boundary;
-		sector_t offset = dm_target_offset(ti, sector);
-		boundary = ((offset + ti->split_io) & ~(ti->split_io - 1))
-			   - offset;
-		if (len > boundary)
-			len = boundary;
+	if (ti->max_io_len) {
+		offset = dm_target_offset(ti, sector);
+		if (unlikely(ti->max_io_len & (ti->max_io_len - 1)))
+			max_len = sector_div(offset, ti->max_io_len);
+		else
+			max_len = offset & (ti->max_io_len - 1);
+		max_len = ti->max_io_len - max_len;
+
+		if (len > max_len)
+			len = max_len;
 	}
 
 	return len;
 }
 
+int dm_set_target_max_io_len(struct dm_target *ti, sector_t len)
+{
+	if (len > UINT_MAX) {
+		DMERR("Specified maximum size of target IO (%llu) exceeds limit (%u)",
+		      (unsigned long long)len, UINT_MAX);
+		ti->error = "Maximum size of target IO is too large";
+		return -EINVAL;
+	}
+
+	ti->max_io_len = (uint32_t) len;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dm_set_target_max_io_len);
+
 static void __map_bio(struct dm_target *ti, struct bio *clone,
 		      struct dm_target_io *tio)
 {
@@ -1196,7 +1215,10 @@ static int __clone_and_map_discard(struct clone_info *ci)
 		if (!ti->num_discard_requests)
 			return -EOPNOTSUPP;
 
-		len = min(ci->sector_count, max_io_len_target_boundary(ci->sector, ti));
+		if (!ti->split_discard_requests)
+			len = min(ci->sector_count, max_io_len_target_boundary(ci->sector, ti));
+		else
+			len = min(ci->sector_count, max_io_len(ci->sector, ti));
 
 		__issue_target_requests(ci, ti, ti->num_discard_requests, len);
 
diff --git a/drivers/md/dm.h b/drivers/md/dm.h
index b7dacd59d8d..52eef493d26 100644
--- a/drivers/md/dm.h
+++ b/drivers/md/dm.h
@@ -23,6 +23,11 @@
 #define DM_SUSPEND_NOFLUSH_FLAG		(1 << 1)
 
 /*
+ * Status feature flags
+ */
+#define DM_STATUS_NOFLUSH_FLAG		(1 << 0)
+
+/*
  * Type of table and mapped_device's mempool
  */
 #define DM_TYPE_NONE		0
diff --git a/drivers/md/md.c b/drivers/md/md.c
index a4c219e3c85..fcd098794d3 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -498,61 +498,13 @@ void md_flush_request(struct mddev *mddev, struct bio *bio)
 }
 EXPORT_SYMBOL(md_flush_request);
 
-/* Support for plugging.
- * This mirrors the plugging support in request_queue, but does not
- * require having a whole queue or request structures.
- * We allocate an md_plug_cb for each md device and each thread it gets
- * plugged on.  This links tot the private plug_handle structure in the
- * personality data where we keep a count of the number of outstanding
- * plugs so other code can see if a plug is active.
- */
-struct md_plug_cb {
-	struct blk_plug_cb cb;
-	struct mddev *mddev;
-};
-
-static void plugger_unplug(struct blk_plug_cb *cb)
-{
-	struct md_plug_cb *mdcb = container_of(cb, struct md_plug_cb, cb);
-	if (atomic_dec_and_test(&mdcb->mddev->plug_cnt))
-		md_wakeup_thread(mdcb->mddev->thread);
-	kfree(mdcb);
-}
-
-/* Check that an unplug wakeup will come shortly.
- * If not, wakeup the md thread immediately
- */
-int mddev_check_plugged(struct mddev *mddev)
+void md_unplug(struct blk_plug_cb *cb, bool from_schedule)
 {
-	struct blk_plug *plug = current->plug;
-	struct md_plug_cb *mdcb;
-
-	if (!plug)
-		return 0;
-
-	list_for_each_entry(mdcb, &plug->cb_list, cb.list) {
-		if (mdcb->cb.callback == plugger_unplug &&
-		    mdcb->mddev == mddev) {
-			/* Already on the list, move to top */
-			if (mdcb != list_first_entry(&plug->cb_list,
-						    struct md_plug_cb,
-						    cb.list))
-				list_move(&mdcb->cb.list, &plug->cb_list);
-			return 1;
-		}
-	}
-	/* Not currently on the callback list */
-	mdcb = kmalloc(sizeof(*mdcb), GFP_ATOMIC);
-	if (!mdcb)
-		return 0;
-
-	mdcb->mddev = mddev;
-	mdcb->cb.callback = plugger_unplug;
-	atomic_inc(&mddev->plug_cnt);
-	list_add(&mdcb->cb.list, &plug->cb_list);
-	return 1;
+	struct mddev *mddev = cb->data;
+	md_wakeup_thread(mddev->thread);
+	kfree(cb);
 }
-EXPORT_SYMBOL_GPL(mddev_check_plugged);
+EXPORT_SYMBOL(md_unplug);
 
 static inline struct mddev *mddev_get(struct mddev *mddev)
 {
@@ -602,7 +554,6 @@ void mddev_init(struct mddev *mddev)
 	atomic_set(&mddev->active, 1);
 	atomic_set(&mddev->openers, 0);
 	atomic_set(&mddev->active_io, 0);
-	atomic_set(&mddev->plug_cnt, 0);
 	spin_lock_init(&mddev->write_lock);
 	atomic_set(&mddev->flush_pending, 0);
 	init_waitqueue_head(&mddev->sb_wait);
@@ -2931,6 +2882,7 @@ offset_store(struct md_rdev *rdev, const char *buf, size_t len)
 		 * can be sane */
 		return -EBUSY;
 	rdev->data_offset = offset;
+	rdev->new_data_offset = offset;
 	return len;
 }
 
@@ -3926,8 +3878,8 @@ array_state_show(struct mddev *mddev, char *page)
 	return sprintf(page, "%s\n", array_states[st]);
 }
 
-static int do_md_stop(struct mddev * mddev, int ro, int is_open);
-static int md_set_readonly(struct mddev * mddev, int is_open);
+static int do_md_stop(struct mddev * mddev, int ro, struct block_device *bdev);
+static int md_set_readonly(struct mddev * mddev, struct block_device *bdev);
 static int do_md_run(struct mddev * mddev);
 static int restart_array(struct mddev *mddev);
 
@@ -3941,24 +3893,20 @@ array_state_store(struct mddev *mddev, const char *buf, size_t len)
 		break;
 	case clear:
 		/* stopping an active array */
-		if (atomic_read(&mddev->openers) > 0)
-			return -EBUSY;
-		err = do_md_stop(mddev, 0, 0);
+		err = do_md_stop(mddev, 0, NULL);
 		break;
 	case inactive:
 		/* stopping an active array */
-		if (mddev->pers) {
-			if (atomic_read(&mddev->openers) > 0)
-				return -EBUSY;
-			err = do_md_stop(mddev, 2, 0);
-		} else
+		if (mddev->pers)
+			err = do_md_stop(mddev, 2, NULL);
+		else
 			err = 0; /* already inactive */
 		break;
 	case suspended:
 		break; /* not supported yet */
 	case readonly:
 		if (mddev->pers)
-			err = md_set_readonly(mddev, 0);
+			err = md_set_readonly(mddev, NULL);
 		else {
 			mddev->ro = 1;
 			set_disk_ro(mddev->gendisk, 1);
@@ -3968,7 +3916,7 @@ array_state_store(struct mddev *mddev, const char *buf, size_t len)
 	case read_auto:
 		if (mddev->pers) {
 			if (mddev->ro == 0)
-				err = md_set_readonly(mddev, 0);
+				err = md_set_readonly(mddev, NULL);
 			else if (mddev->ro == 1)
 				err = restart_array(mddev);
 			if (err == 0) {
@@ -5351,15 +5299,17 @@ void md_stop(struct mddev *mddev)
 }
 EXPORT_SYMBOL_GPL(md_stop);
 
-static int md_set_readonly(struct mddev *mddev, int is_open)
+static int md_set_readonly(struct mddev *mddev, struct block_device *bdev)
 {
 	int err = 0;
 	mutex_lock(&mddev->open_mutex);
-	if (atomic_read(&mddev->openers) > is_open) {
+	if (atomic_read(&mddev->openers) > !!bdev) {
 		printk("md: %s still in use.\n",mdname(mddev));
 		err = -EBUSY;
 		goto out;
 	}
+	if (bdev)
+		sync_blockdev(bdev);
 	if (mddev->pers) {
 		__md_stop_writes(mddev);
 
@@ -5381,18 +5331,26 @@ out:
  *   0 - completely stop and dis-assemble array
  *   2 - stop but do not disassemble array
  */
-static int do_md_stop(struct mddev * mddev, int mode, int is_open)
+static int do_md_stop(struct mddev * mddev, int mode,
+		      struct block_device *bdev)
 {
 	struct gendisk *disk = mddev->gendisk;
 	struct md_rdev *rdev;
 
 	mutex_lock(&mddev->open_mutex);
-	if (atomic_read(&mddev->openers) > is_open ||
+	if (atomic_read(&mddev->openers) > !!bdev ||
 	    mddev->sysfs_active) {
 		printk("md: %s still in use.\n",mdname(mddev));
 		mutex_unlock(&mddev->open_mutex);
 		return -EBUSY;
 	}
+	if (bdev)
+		/* It is possible IO was issued on some other
+		 * open file which was closed before we took ->open_mutex.
+		 * As that was not the last close __blkdev_put will not
+		 * have called sync_blockdev, so we must.
+		 */
+		sync_blockdev(bdev);
 
 	if (mddev->pers) {
 		if (mddev->ro)
@@ -5466,7 +5424,7 @@ static void autorun_array(struct mddev *mddev)
 	err = do_md_run(mddev);
 	if (err) {
 		printk(KERN_WARNING "md: do_md_run() returned %d\n", err);
-		do_md_stop(mddev, 0, 0);
+		do_md_stop(mddev, 0, NULL);
 	}
 }
 
@@ -6481,11 +6439,11 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			goto done_unlock;
 
 		case STOP_ARRAY:
-			err = do_md_stop(mddev, 0, 1);
+			err = do_md_stop(mddev, 0, bdev);
 			goto done_unlock;
 
 		case STOP_ARRAY_RO:
-			err = md_set_readonly(mddev, 1);
+			err = md_set_readonly(mddev, bdev);
 			goto done_unlock;
 
 		case BLKROSET:
diff --git a/drivers/md/md.h b/drivers/md/md.h
index 7b4a3c318ca..f385b038589 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -266,9 +266,6 @@ struct mddev {
 	int				new_chunk_sectors;
 	int				reshape_backwards;
 
-	atomic_t			plug_cnt;	/* If device is expecting
-							 * more bios soon.
-							 */
 	struct md_thread		*thread;	/* management thread */
 	struct md_thread		*sync_thread;	/* doing resync or reconstruct */
 	sector_t			curr_resync;	/* last block scheduled */
@@ -630,6 +627,12 @@ extern struct bio *bio_clone_mddev(struct bio *bio, gfp_t gfp_mask,
 				   struct mddev *mddev);
 extern struct bio *bio_alloc_mddev(gfp_t gfp_mask, int nr_iovecs,
 				   struct mddev *mddev);
-extern int mddev_check_plugged(struct mddev *mddev);
 extern void md_trim_bio(struct bio *bio, int offset, int size);
+
+extern void md_unplug(struct blk_plug_cb *cb, bool from_schedule);
+static inline int mddev_check_plugged(struct mddev *mddev)
+{
+	return !!blk_check_plugged(md_unplug, mddev,
+				   sizeof(struct blk_plug_cb));
+}
 #endif /* _MD_MD_H */
diff --git a/drivers/md/persistent-data/Makefile b/drivers/md/persistent-data/Makefile
index cfa95f66223..d8e7cb767c1 100644
--- a/drivers/md/persistent-data/Makefile
+++ b/drivers/md/persistent-data/Makefile
@@ -1,7 +1,6 @@
 obj-$(CONFIG_DM_PERSISTENT_DATA) += dm-persistent-data.o
 dm-persistent-data-objs := \
 	dm-block-manager.o \
-	dm-space-map-checker.o \
 	dm-space-map-common.o \
 	dm-space-map-disk.o \
 	dm-space-map-metadata.o \
diff --git a/drivers/md/persistent-data/dm-block-manager.c b/drivers/md/persistent-data/dm-block-manager.c
index 0317ecdc6e5..5ba277768d9 100644
--- a/drivers/md/persistent-data/dm-block-manager.c
+++ b/drivers/md/persistent-data/dm-block-manager.c
@@ -325,11 +325,6 @@ static struct dm_buffer *to_buffer(struct dm_block *b)
 	return (struct dm_buffer *) b;
 }
 
-static struct dm_bufio_client *to_bufio(struct dm_block_manager *bm)
-{
-	return (struct dm_bufio_client *) bm;
-}
-
 dm_block_t dm_block_location(struct dm_block *b)
 {
 	return dm_bufio_get_block_number(to_buffer(b));
@@ -367,34 +362,60 @@ static void dm_block_manager_write_callback(struct dm_buffer *buf)
 /*----------------------------------------------------------------
  * Public interface
  *--------------------------------------------------------------*/
+struct dm_block_manager {
+	struct dm_bufio_client *bufio;
+	bool read_only:1;
+};
+
 struct dm_block_manager *dm_block_manager_create(struct block_device *bdev,
 						 unsigned block_size,
 						 unsigned cache_size,
 						 unsigned max_held_per_thread)
 {
-	return (struct dm_block_manager *)
-		dm_bufio_client_create(bdev, block_size, max_held_per_thread,
-				       sizeof(struct buffer_aux),
-				       dm_block_manager_alloc_callback,
-				       dm_block_manager_write_callback);
+	int r;
+	struct dm_block_manager *bm;
+
+	bm = kmalloc(sizeof(*bm), GFP_KERNEL);
+	if (!bm) {
+		r = -ENOMEM;
+		goto bad;
+	}
+
+	bm->bufio = dm_bufio_client_create(bdev, block_size, max_held_per_thread,
+					   sizeof(struct buffer_aux),
+					   dm_block_manager_alloc_callback,
+					   dm_block_manager_write_callback);
+	if (IS_ERR(bm->bufio)) {
+		r = PTR_ERR(bm->bufio);
+		kfree(bm);
+		goto bad;
+	}
+
+	bm->read_only = false;
+
+	return bm;
+
+bad:
+	return ERR_PTR(r);
 }
 EXPORT_SYMBOL_GPL(dm_block_manager_create);
 
 void dm_block_manager_destroy(struct dm_block_manager *bm)
 {
-	return dm_bufio_client_destroy(to_bufio(bm));
+	dm_bufio_client_destroy(bm->bufio);
+	kfree(bm);
 }
 EXPORT_SYMBOL_GPL(dm_block_manager_destroy);
 
 unsigned dm_bm_block_size(struct dm_block_manager *bm)
 {
-	return dm_bufio_get_block_size(to_bufio(bm));
+	return dm_bufio_get_block_size(bm->bufio);
 }
 EXPORT_SYMBOL_GPL(dm_bm_block_size);
 
 dm_block_t dm_bm_nr_blocks(struct dm_block_manager *bm)
 {
-	return dm_bufio_get_device_size(to_bufio(bm));
+	return dm_bufio_get_device_size(bm->bufio);
 }
 
 static int dm_bm_validate_buffer(struct dm_block_manager *bm,
@@ -406,7 +427,7 @@ static int dm_bm_validate_buffer(struct dm_block_manager *bm,
 		int r;
 		if (!v)
 			return 0;
-		r = v->check(v, (struct dm_block *) buf, dm_bufio_get_block_size(to_bufio(bm)));
+		r = v->check(v, (struct dm_block *) buf, dm_bufio_get_block_size(bm->bufio));
 		if (unlikely(r))
 			return r;
 		aux->validator = v;
@@ -430,7 +451,7 @@ int dm_bm_read_lock(struct dm_block_manager *bm, dm_block_t b,
 	void *p;
 	int r;
 
-	p = dm_bufio_read(to_bufio(bm), b, (struct dm_buffer **) result);
+	p = dm_bufio_read(bm->bufio, b, (struct dm_buffer **) result);
 	if (unlikely(IS_ERR(p)))
 		return PTR_ERR(p);
 
@@ -463,7 +484,10 @@ int dm_bm_write_lock(struct dm_block_manager *bm,
 	void *p;
 	int r;
 
-	p = dm_bufio_read(to_bufio(bm), b, (struct dm_buffer **) result);
+	if (bm->read_only)
+		return -EPERM;
+
+	p = dm_bufio_read(bm->bufio, b, (struct dm_buffer **) result);
 	if (unlikely(IS_ERR(p)))
 		return PTR_ERR(p);
 
@@ -496,7 +520,7 @@ int dm_bm_read_try_lock(struct dm_block_manager *bm,
 	void *p;
 	int r;
 
-	p = dm_bufio_get(to_bufio(bm), b, (struct dm_buffer **) result);
+	p = dm_bufio_get(bm->bufio, b, (struct dm_buffer **) result);
 	if (unlikely(IS_ERR(p)))
 		return PTR_ERR(p);
 	if (unlikely(!p))
@@ -529,7 +553,10 @@ int dm_bm_write_lock_zero(struct dm_block_manager *bm,
 	struct buffer_aux *aux;
 	void *p;
 
-	p = dm_bufio_new(to_bufio(bm), b, (struct dm_buffer **) result);
+	if (bm->read_only)
+		return -EPERM;
+
+	p = dm_bufio_new(bm->bufio, b, (struct dm_buffer **) result);
 	if (unlikely(IS_ERR(p)))
 		return PTR_ERR(p);
 
@@ -547,6 +574,7 @@ int dm_bm_write_lock_zero(struct dm_block_manager *bm,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(dm_bm_write_lock_zero);
 
 int dm_bm_unlock(struct dm_block *b)
 {
@@ -565,45 +593,30 @@ int dm_bm_unlock(struct dm_block *b)
 }
 EXPORT_SYMBOL_GPL(dm_bm_unlock);
 
-int dm_bm_unlock_move(struct dm_block *b, dm_block_t n)
-{
-	struct buffer_aux *aux;
-
-	aux = dm_bufio_get_aux_data(to_buffer(b));
-
-	if (aux->write_locked) {
-		dm_bufio_mark_buffer_dirty(to_buffer(b));
-		bl_up_write(&aux->lock);
-	} else
-		bl_up_read(&aux->lock);
-
-	dm_bufio_release_move(to_buffer(b), n);
-	return 0;
-}
-
 int dm_bm_flush_and_unlock(struct dm_block_manager *bm,
 			   struct dm_block *superblock)
 {
 	int r;
 
-	r = dm_bufio_write_dirty_buffers(to_bufio(bm));
-	if (unlikely(r))
-		return r;
-	r = dm_bufio_issue_flush(to_bufio(bm));
-	if (unlikely(r))
+	if (bm->read_only)
+		return -EPERM;
+
+	r = dm_bufio_write_dirty_buffers(bm->bufio);
+	if (unlikely(r)) {
+		dm_bm_unlock(superblock);
 		return r;
+	}
 
 	dm_bm_unlock(superblock);
 
-	r = dm_bufio_write_dirty_buffers(to_bufio(bm));
-	if (unlikely(r))
-		return r;
-	r = dm_bufio_issue_flush(to_bufio(bm));
-	if (unlikely(r))
-		return r;
+	return dm_bufio_write_dirty_buffers(bm->bufio);
+}
 
-	return 0;
+void dm_bm_set_read_only(struct dm_block_manager *bm)
+{
+	bm->read_only = true;
 }
+EXPORT_SYMBOL_GPL(dm_bm_set_read_only);
 
 u32 dm_bm_checksum(const void *data, size_t len, u32 init_xor)
 {
diff --git a/drivers/md/persistent-data/dm-block-manager.h b/drivers/md/persistent-data/dm-block-manager.h
index 924833d2dfa..be5bff61be2 100644
--- a/drivers/md/persistent-data/dm-block-manager.h
+++ b/drivers/md/persistent-data/dm-block-manager.h
@@ -97,14 +97,6 @@ int dm_bm_write_lock_zero(struct dm_block_manager *bm, dm_block_t b,
 int dm_bm_unlock(struct dm_block *b);
 
 /*
- * An optimisation; we often want to copy a block's contents to a new
- * block.  eg, as part of the shadowing operation.  It's far better for
- * bufio to do this move behind the scenes than hold 2 locks and memcpy the
- * data.
- */
-int dm_bm_unlock_move(struct dm_block *b, dm_block_t n);
-
-/*
  * It's a common idiom to have a superblock that should be committed last.
  *
  * @superblock should be write-locked on entry. It will be unlocked during
@@ -116,6 +108,19 @@ int dm_bm_unlock_move(struct dm_block *b, dm_block_t n);
 int dm_bm_flush_and_unlock(struct dm_block_manager *bm,
 			   struct dm_block *superblock);
 
+/*
+ * Switches the bm to a read only mode.  Once read-only mode
+ * has been entered the following functions will return -EPERM.
+ *
+ *   dm_bm_write_lock
+ *   dm_bm_write_lock_zero
+ *   dm_bm_flush_and_unlock
+ *
+ * Additionally you should not use dm_bm_unlock_move, however no error will
+ * be returned if you do.
+ */
+void dm_bm_set_read_only(struct dm_block_manager *bm);
+
 u32 dm_bm_checksum(const void *data, size_t len, u32 init_xor);
 
 /*----------------------------------------------------------------*/
diff --git a/drivers/md/persistent-data/dm-space-map-checker.c b/drivers/md/persistent-data/dm-space-map-checker.c
deleted file mode 100644
index fc90c11620a..00000000000
--- a/drivers/md/persistent-data/dm-space-map-checker.c
+++ /dev/null
@@ -1,446 +0,0 @@
-/*
- * Copyright (C) 2011 Red Hat, Inc.
- *
- * This file is released under the GPL.
- */
-
-#include "dm-space-map-checker.h"
-
-#include <linux/device-mapper.h>
-#include <linux/export.h>
-#include <linux/vmalloc.h>
-
-#ifdef CONFIG_DM_DEBUG_SPACE_MAPS
-
-#define DM_MSG_PREFIX "space map checker"
-
-/*----------------------------------------------------------------*/
-
-struct count_array {
-	dm_block_t nr;
-	dm_block_t nr_free;
-
-	uint32_t *counts;
-};
-
-static int ca_get_count(struct count_array *ca, dm_block_t b, uint32_t *count)
-{
-	if (b >= ca->nr)
-		return -EINVAL;
-
-	*count = ca->counts[b];
-	return 0;
-}
-
-static int ca_count_more_than_one(struct count_array *ca, dm_block_t b, int *r)
-{
-	if (b >= ca->nr)
-		return -EINVAL;
-
-	*r = ca->counts[b] > 1;
-	return 0;
-}
-
-static int ca_set_count(struct count_array *ca, dm_block_t b, uint32_t count)
-{
-	uint32_t old_count;
-
-	if (b >= ca->nr)
-		return -EINVAL;
-
-	old_count = ca->counts[b];
-
-	if (!count && old_count)
-		ca->nr_free++;
-
-	else if (count && !old_count)
-		ca->nr_free--;
-
-	ca->counts[b] = count;
-	return 0;
-}
-
-static int ca_inc_block(struct count_array *ca, dm_block_t b)
-{
-	if (b >= ca->nr)
-		return -EINVAL;
-
-	ca_set_count(ca, b, ca->counts[b] + 1);
-	return 0;
-}
-
-static int ca_dec_block(struct count_array *ca, dm_block_t b)
-{
-	if (b >= ca->nr)
-		return -EINVAL;
-
-	BUG_ON(ca->counts[b] == 0);
-	ca_set_count(ca, b, ca->counts[b] - 1);
-	return 0;
-}
-
-static int ca_create(struct count_array *ca, struct dm_space_map *sm)
-{
-	int r;
-	dm_block_t nr_blocks;
-
-	r = dm_sm_get_nr_blocks(sm, &nr_blocks);
-	if (r)
-		return r;
-
-	ca->nr = nr_blocks;
-	ca->nr_free = nr_blocks;
-
-	if (!nr_blocks)
-		ca->counts = NULL;
-	else {
-		ca->counts = vzalloc(sizeof(*ca->counts) * nr_blocks);
-		if (!ca->counts)
-			return -ENOMEM;
-	}
-
-	return 0;
-}
-
-static void ca_destroy(struct count_array *ca)
-{
-	vfree(ca->counts);
-}
-
-static int ca_load(struct count_array *ca, struct dm_space_map *sm)
-{
-	int r;
-	uint32_t count;
-	dm_block_t nr_blocks, i;
-
-	r = dm_sm_get_nr_blocks(sm, &nr_blocks);
-	if (r)
-		return r;
-
-	BUG_ON(ca->nr != nr_blocks);
-
-	DMWARN("Loading debug space map from disk.  This may take some time");
-	for (i = 0; i < nr_blocks; i++) {
-		r = dm_sm_get_count(sm, i, &count);
-		if (r) {
-			DMERR("load failed");
-			return r;
-		}
-
-		ca_set_count(ca, i, count);
-	}
-	DMWARN("Load complete");
-
-	return 0;
-}
-
-static int ca_extend(struct count_array *ca, dm_block_t extra_blocks)
-{
-	dm_block_t nr_blocks = ca->nr + extra_blocks;
-	uint32_t *counts = vzalloc(sizeof(*counts) * nr_blocks);
-	if (!counts)
-		return -ENOMEM;
-
-	if (ca->counts) {
-		memcpy(counts, ca->counts, sizeof(*counts) * ca->nr);
-		ca_destroy(ca);
-	}
-	ca->nr = nr_blocks;
-	ca->nr_free += extra_blocks;
-	ca->counts = counts;
-	return 0;
-}
-
-static int ca_commit(struct count_array *old, struct count_array *new)
-{
-	if (old->nr != new->nr) {
-		BUG_ON(old->nr > new->nr);
-		ca_extend(old, new->nr - old->nr);
-	}
-
-	BUG_ON(old->nr != new->nr);
-	old->nr_free = new->nr_free;
-	memcpy(old->counts, new->counts, sizeof(*old->counts) * old->nr);
-	return 0;
-}
-
-/*----------------------------------------------------------------*/
-
-struct sm_checker {
-	struct dm_space_map sm;
-
-	struct count_array old_counts;
-	struct count_array counts;
-
-	struct dm_space_map *real_sm;
-};
-
-static void sm_checker_destroy(struct dm_space_map *sm)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-
-	dm_sm_destroy(smc->real_sm);
-	ca_destroy(&smc->old_counts);
-	ca_destroy(&smc->counts);
-	kfree(smc);
-}
-
-static int sm_checker_get_nr_blocks(struct dm_space_map *sm, dm_block_t *count)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r = dm_sm_get_nr_blocks(smc->real_sm, count);
-	if (!r)
-		BUG_ON(smc->old_counts.nr != *count);
-	return r;
-}
-
-static int sm_checker_get_nr_free(struct dm_space_map *sm, dm_block_t *count)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r = dm_sm_get_nr_free(smc->real_sm, count);
-	if (!r) {
-		/*
-		 * Slow, but we know it's correct.
-		 */
-		dm_block_t b, n = 0;
-		for (b = 0; b < smc->old_counts.nr; b++)
-			if (smc->old_counts.counts[b] == 0 &&
-			    smc->counts.counts[b] == 0)
-				n++;
-
-		if (n != *count)
-			DMERR("free block counts differ, checker %u, sm-disk:%u",
-			      (unsigned) n, (unsigned) *count);
-	}
-	return r;
-}
-
-static int sm_checker_new_block(struct dm_space_map *sm, dm_block_t *b)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r = dm_sm_new_block(smc->real_sm, b);
-
-	if (!r) {
-		BUG_ON(*b >= smc->old_counts.nr);
-		BUG_ON(smc->old_counts.counts[*b] != 0);
-		BUG_ON(*b >= smc->counts.nr);
-		BUG_ON(smc->counts.counts[*b] != 0);
-		ca_set_count(&smc->counts, *b, 1);
-	}
-
-	return r;
-}
-
-static int sm_checker_inc_block(struct dm_space_map *sm, dm_block_t b)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r = dm_sm_inc_block(smc->real_sm, b);
-	int r2 = ca_inc_block(&smc->counts, b);
-	BUG_ON(r != r2);
-	return r;
-}
-
-static int sm_checker_dec_block(struct dm_space_map *sm, dm_block_t b)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r = dm_sm_dec_block(smc->real_sm, b);
-	int r2 = ca_dec_block(&smc->counts, b);
-	BUG_ON(r != r2);
-	return r;
-}
-
-static int sm_checker_get_count(struct dm_space_map *sm, dm_block_t b, uint32_t *result)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	uint32_t result2 = 0;
-	int r = dm_sm_get_count(smc->real_sm, b, result);
-	int r2 = ca_get_count(&smc->counts, b, &result2);
-
-	BUG_ON(r != r2);
-	if (!r)
-		BUG_ON(*result != result2);
-	return r;
-}
-
-static int sm_checker_count_more_than_one(struct dm_space_map *sm, dm_block_t b, int *result)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int result2 = 0;
-	int r = dm_sm_count_is_more_than_one(smc->real_sm, b, result);
-	int r2 = ca_count_more_than_one(&smc->counts, b, &result2);
-
-	BUG_ON(r != r2);
-	if (!r)
-		BUG_ON(!(*result) && result2);
-	return r;
-}
-
-static int sm_checker_set_count(struct dm_space_map *sm, dm_block_t b, uint32_t count)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	uint32_t old_rc;
-	int r = dm_sm_set_count(smc->real_sm, b, count);
-	int r2;
-
-	BUG_ON(b >= smc->counts.nr);
-	old_rc = smc->counts.counts[b];
-	r2 = ca_set_count(&smc->counts, b, count);
-	BUG_ON(r != r2);
-
-	return r;
-}
-
-static int sm_checker_commit(struct dm_space_map *sm)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r;
-
-	r = dm_sm_commit(smc->real_sm);
-	if (r)
-		return r;
-
-	r = ca_commit(&smc->old_counts, &smc->counts);
-	if (r)
-		return r;
-
-	return 0;
-}
-
-static int sm_checker_extend(struct dm_space_map *sm, dm_block_t extra_blocks)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	int r = dm_sm_extend(smc->real_sm, extra_blocks);
-	if (r)
-		return r;
-
-	return ca_extend(&smc->counts, extra_blocks);
-}
-
-static int sm_checker_root_size(struct dm_space_map *sm, size_t *result)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	return dm_sm_root_size(smc->real_sm, result);
-}
-
-static int sm_checker_copy_root(struct dm_space_map *sm, void *copy_to_here_le, size_t len)
-{
-	struct sm_checker *smc = container_of(sm, struct sm_checker, sm);
-	return dm_sm_copy_root(smc->real_sm, copy_to_here_le, len);
-}
-
-/*----------------------------------------------------------------*/
-
-static struct dm_space_map ops_ = {
-	.destroy = sm_checker_destroy,
-	.get_nr_blocks = sm_checker_get_nr_blocks,
-	.get_nr_free = sm_checker_get_nr_free,
-	.inc_block = sm_checker_inc_block,
-	.dec_block = sm_checker_dec_block,
-	.new_block = sm_checker_new_block,
-	.get_count = sm_checker_get_count,
-	.count_is_more_than_one = sm_checker_count_more_than_one,
-	.set_count = sm_checker_set_count,
-	.commit = sm_checker_commit,
-	.extend = sm_checker_extend,
-	.root_size = sm_checker_root_size,
-	.copy_root = sm_checker_copy_root
-};
-
-struct dm_space_map *dm_sm_checker_create(struct dm_space_map *sm)
-{
-	int r;
-	struct sm_checker *smc;
-
-	if (IS_ERR_OR_NULL(sm))
-		return ERR_PTR(-EINVAL);
-
-	smc = kmalloc(sizeof(*smc), GFP_KERNEL);
-	if (!smc)
-		return ERR_PTR(-ENOMEM);
-
-	memcpy(&smc->sm, &ops_, sizeof(smc->sm));
-	r = ca_create(&smc->old_counts, sm);
-	if (r) {
-		kfree(smc);
-		return ERR_PTR(r);
-	}
-
-	r = ca_create(&smc->counts, sm);
-	if (r) {
-		ca_destroy(&smc->old_counts);
-		kfree(smc);
-		return ERR_PTR(r);
-	}
-
-	smc->real_sm = sm;
-
-	r = ca_load(&smc->counts, sm);
-	if (r) {
-		ca_destroy(&smc->counts);
-		ca_destroy(&smc->old_counts);
-		kfree(smc);
-		return ERR_PTR(r);
-	}
-
-	r = ca_commit(&smc->old_counts, &smc->counts);
-	if (r) {
-		ca_destroy(&smc->counts);
-		ca_destroy(&smc->old_counts);
-		kfree(smc);
-		return ERR_PTR(r);
-	}
-
-	return &smc->sm;
-}
-EXPORT_SYMBOL_GPL(dm_sm_checker_create);
-
-struct dm_space_map *dm_sm_checker_create_fresh(struct dm_space_map *sm)
-{
-	int r;
-	struct sm_checker *smc;
-
-	if (IS_ERR_OR_NULL(sm))
-		return ERR_PTR(-EINVAL);
-
-	smc = kmalloc(sizeof(*smc), GFP_KERNEL);
-	if (!smc)
-		return ERR_PTR(-ENOMEM);
-
-	memcpy(&smc->sm, &ops_, sizeof(smc->sm));
-	r = ca_create(&smc->old_counts, sm);
-	if (r) {
-		kfree(smc);
-		return ERR_PTR(r);
-	}
-
-	r = ca_create(&smc->counts, sm);
-	if (r) {
-		ca_destroy(&smc->old_counts);
-		kfree(smc);
-		return ERR_PTR(r);
-	}
-
-	smc->real_sm = sm;
-	return &smc->sm;
-}
-EXPORT_SYMBOL_GPL(dm_sm_checker_create_fresh);
-
-/*----------------------------------------------------------------*/
-
-#else
-
-struct dm_space_map *dm_sm_checker_create(struct dm_space_map *sm)
-{
-	return sm;
-}
-EXPORT_SYMBOL_GPL(dm_sm_checker_create);
-
-struct dm_space_map *dm_sm_checker_create_fresh(struct dm_space_map *sm)
-{
-	return sm;
-}
-EXPORT_SYMBOL_GPL(dm_sm_checker_create_fresh);
-
-/*----------------------------------------------------------------*/
-
-#endif
diff --git a/drivers/md/persistent-data/dm-space-map-checker.h b/drivers/md/persistent-data/dm-space-map-checker.h
deleted file mode 100644
index 444dccf6688..00000000000
--- a/drivers/md/persistent-data/dm-space-map-checker.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * Copyright (C) 2011 Red Hat, Inc.
- *
- * This file is released under the GPL.
- */
-
-#ifndef SNAPSHOTS_SPACE_MAP_CHECKER_H
-#define SNAPSHOTS_SPACE_MAP_CHECKER_H
-
-#include "dm-space-map.h"
-
-/*----------------------------------------------------------------*/
-
-/*
- * This space map wraps a real on-disk space map, and verifies all of its
- * operations.  It uses a lot of memory, so only use if you have a specific
- * problem that you're debugging.
- *
- * Ownership of @sm passes.
- */
-struct dm_space_map *dm_sm_checker_create(struct dm_space_map *sm);
-struct dm_space_map *dm_sm_checker_create_fresh(struct dm_space_map *sm);
-
-/*----------------------------------------------------------------*/
-
-#endif
diff --git a/drivers/md/persistent-data/dm-space-map-common.c b/drivers/md/persistent-data/dm-space-map-common.c
index ff3beed6ad2..d77602d63c8 100644
--- a/drivers/md/persistent-data/dm-space-map-common.c
+++ b/drivers/md/persistent-data/dm-space-map-common.c
@@ -224,6 +224,7 @@ static int sm_ll_init(struct ll_disk *ll, struct dm_transaction_manager *tm)
 	ll->nr_blocks = 0;
 	ll->bitmap_root = 0;
 	ll->ref_count_root = 0;
+	ll->bitmap_index_changed = false;
 
 	return 0;
 }
@@ -476,7 +477,15 @@ int sm_ll_dec(struct ll_disk *ll, dm_block_t b, enum allocation_event *ev)
 
 int sm_ll_commit(struct ll_disk *ll)
 {
-	return ll->commit(ll);
+	int r = 0;
+
+	if (ll->bitmap_index_changed) {
+		r = ll->commit(ll);
+		if (!r)
+			ll->bitmap_index_changed = false;
+	}
+
+	return r;
 }
 
 /*----------------------------------------------------------------*/
@@ -491,6 +500,7 @@ static int metadata_ll_load_ie(struct ll_disk *ll, dm_block_t index,
 static int metadata_ll_save_ie(struct ll_disk *ll, dm_block_t index,
 			       struct disk_index_entry *ie)
 {
+	ll->bitmap_index_changed = true;
 	memcpy(ll->mi_le.index + index, ie, sizeof(*ie));
 	return 0;
 }
diff --git a/drivers/md/persistent-data/dm-space-map-common.h b/drivers/md/persistent-data/dm-space-map-common.h
index 8f220821a9a..b3078d5eda0 100644
--- a/drivers/md/persistent-data/dm-space-map-common.h
+++ b/drivers/md/persistent-data/dm-space-map-common.h
@@ -78,6 +78,7 @@ struct ll_disk {
 	open_index_fn open_index;
 	max_index_entries_fn max_entries;
 	commit_fn commit;
+	bool bitmap_index_changed:1;
 };
 
 struct disk_sm_root {
diff --git a/drivers/md/persistent-data/dm-space-map-disk.c b/drivers/md/persistent-data/dm-space-map-disk.c
index 3d0ed533288..f6d29e614ab 100644
--- a/drivers/md/persistent-data/dm-space-map-disk.c
+++ b/drivers/md/persistent-data/dm-space-map-disk.c
@@ -4,7 +4,6 @@
  * This file is released under the GPL.
  */
 
-#include "dm-space-map-checker.h"
 #include "dm-space-map-common.h"
 #include "dm-space-map-disk.h"
 #include "dm-space-map.h"
@@ -252,9 +251,8 @@ static struct dm_space_map ops = {
 	.copy_root = sm_disk_copy_root
 };
 
-static struct dm_space_map *dm_sm_disk_create_real(
-	struct dm_transaction_manager *tm,
-	dm_block_t nr_blocks)
+struct dm_space_map *dm_sm_disk_create(struct dm_transaction_manager *tm,
+				       dm_block_t nr_blocks)
 {
 	int r;
 	struct sm_disk *smd;
@@ -285,27 +283,10 @@ bad:
 	kfree(smd);
 	return ERR_PTR(r);
 }
-
-struct dm_space_map *dm_sm_disk_create(struct dm_transaction_manager *tm,
-				       dm_block_t nr_blocks)
-{
-	struct dm_space_map *sm = dm_sm_disk_create_real(tm, nr_blocks);
-	struct dm_space_map *smc;
-
-	if (IS_ERR_OR_NULL(sm))
-		return sm;
-
-	smc = dm_sm_checker_create_fresh(sm);
-	if (IS_ERR(smc))
-		dm_sm_destroy(sm);
-
-	return smc;
-}
 EXPORT_SYMBOL_GPL(dm_sm_disk_create);
 
-static struct dm_space_map *dm_sm_disk_open_real(
-	struct dm_transaction_manager *tm,
-	void *root_le, size_t len)
+struct dm_space_map *dm_sm_disk_open(struct dm_transaction_manager *tm,
+				     void *root_le, size_t len)
 {
 	int r;
 	struct sm_disk *smd;
@@ -332,13 +313,6 @@ bad:
 	kfree(smd);
 	return ERR_PTR(r);
 }
-
-struct dm_space_map *dm_sm_disk_open(struct dm_transaction_manager *tm,
-				     void *root_le, size_t len)
-{
-	return dm_sm_checker_create(
-		dm_sm_disk_open_real(tm, root_le, len));
-}
 EXPORT_SYMBOL_GPL(dm_sm_disk_open);
 
 /*----------------------------------------------------------------*/
diff --git a/drivers/md/persistent-data/dm-transaction-manager.c b/drivers/md/persistent-data/dm-transaction-manager.c
index e5604b32d91..d247a35da3c 100644
--- a/drivers/md/persistent-data/dm-transaction-manager.c
+++ b/drivers/md/persistent-data/dm-transaction-manager.c
@@ -5,7 +5,6 @@
  */
 #include "dm-transaction-manager.h"
 #include "dm-space-map.h"
-#include "dm-space-map-checker.h"
 #include "dm-space-map-disk.h"
 #include "dm-space-map-metadata.h"
 #include "dm-persistent-data-internal.h"
@@ -220,13 +219,24 @@ static int __shadow_block(struct dm_transaction_manager *tm, dm_block_t orig,
 	if (r < 0)
 		return r;
 
-	r = dm_bm_unlock_move(orig_block, new);
-	if (r < 0) {
+	/*
+	 * It would be tempting to use dm_bm_unlock_move here, but some
+	 * code, such as the space maps, keeps using the old data structures
+	 * secure in the knowledge they won't be changed until the next
+	 * transaction.  Using unlock_move would force a synchronous read
+	 * since the old block would no longer be in the cache.
+	 */
+	r = dm_bm_write_lock_zero(tm->bm, new, v, result);
+	if (r) {
 		dm_bm_unlock(orig_block);
 		return r;
 	}
 
-	return dm_bm_write_lock(tm->bm, new, v, result);
+	memcpy(dm_block_data(*result), dm_block_data(orig_block),
+	       dm_bm_block_size(tm->bm));
+
+	dm_bm_unlock(orig_block);
+	return r;
 }
 
 int dm_tm_shadow_block(struct dm_transaction_manager *tm, dm_block_t orig,
@@ -311,98 +321,61 @@ struct dm_block_manager *dm_tm_get_bm(struct dm_transaction_manager *tm)
 
 static int dm_tm_create_internal(struct dm_block_manager *bm,
 				 dm_block_t sb_location,
-				 struct dm_block_validator *sb_validator,
-				 size_t root_offset, size_t root_max_len,
 				 struct dm_transaction_manager **tm,
 				 struct dm_space_map **sm,
-				 struct dm_block **sblock,
-				 int create)
+				 int create,
+				 void *sm_root, size_t sm_len)
 {
 	int r;
-	struct dm_space_map *inner;
 
-	inner = dm_sm_metadata_init();
-	if (IS_ERR(inner))
-		return PTR_ERR(inner);
+	*sm = dm_sm_metadata_init();
+	if (IS_ERR(*sm))
+		return PTR_ERR(*sm);
 
-	*tm = dm_tm_create(bm, inner);
+	*tm = dm_tm_create(bm, *sm);
 	if (IS_ERR(*tm)) {
-		dm_sm_destroy(inner);
+		dm_sm_destroy(*sm);
 		return PTR_ERR(*tm);
 	}
 
 	if (create) {
-		r = dm_bm_write_lock_zero(dm_tm_get_bm(*tm), sb_location,
-					  sb_validator, sblock);
-		if (r < 0) {
-			DMERR("couldn't lock superblock");
-			goto bad1;
-		}
-
-		r = dm_sm_metadata_create(inner, *tm, dm_bm_nr_blocks(bm),
+		r = dm_sm_metadata_create(*sm, *tm, dm_bm_nr_blocks(bm),
 					  sb_location);
 		if (r) {
 			DMERR("couldn't create metadata space map");
-			goto bad2;
-		}
-
-		*sm = dm_sm_checker_create(inner);
-		if (IS_ERR(*sm)) {
-			r = PTR_ERR(*sm);
-			goto bad2;
+			goto bad;
 		}
 
 	} else {
-		r = dm_bm_write_lock(dm_tm_get_bm(*tm), sb_location,
-				     sb_validator, sblock);
-		if (r < 0) {
-			DMERR("couldn't lock superblock");
-			goto bad1;
-		}
-
-		r = dm_sm_metadata_open(inner, *tm,
-					dm_block_data(*sblock) + root_offset,
-					root_max_len);
+		r = dm_sm_metadata_open(*sm, *tm, sm_root, sm_len);
 		if (r) {
 			DMERR("couldn't open metadata space map");
-			goto bad2;
-		}
-
-		*sm = dm_sm_checker_create(inner);
-		if (IS_ERR(*sm)) {
-			r = PTR_ERR(*sm);
-			goto bad2;
+			goto bad;
 		}
 	}
 
 	return 0;
 
-bad2:
-	dm_tm_unlock(*tm, *sblock);
-bad1:
+bad:
 	dm_tm_destroy(*tm);
-	dm_sm_destroy(inner);
+	dm_sm_destroy(*sm);
 	return r;
 }
 
 int dm_tm_create_with_sm(struct dm_block_manager *bm, dm_block_t sb_location,
-			 struct dm_block_validator *sb_validator,
 			 struct dm_transaction_manager **tm,
-			 struct dm_space_map **sm, struct dm_block **sblock)
+			 struct dm_space_map **sm)
 {
-	return dm_tm_create_internal(bm, sb_location, sb_validator,
-				     0, 0, tm, sm, sblock, 1);
+	return dm_tm_create_internal(bm, sb_location, tm, sm, 1, NULL, 0);
 }
 EXPORT_SYMBOL_GPL(dm_tm_create_with_sm);
 
 int dm_tm_open_with_sm(struct dm_block_manager *bm, dm_block_t sb_location,
-		       struct dm_block_validator *sb_validator,
-		       size_t root_offset, size_t root_max_len,
+		       void *sm_root, size_t root_len,
 		       struct dm_transaction_manager **tm,
-		       struct dm_space_map **sm, struct dm_block **sblock)
+		       struct dm_space_map **sm)
 {
-	return dm_tm_create_internal(bm, sb_location, sb_validator, root_offset,
-				     root_max_len, tm, sm, sblock, 0);
+	return dm_tm_create_internal(bm, sb_location, tm, sm, 0, sm_root, root_len);
 }
 EXPORT_SYMBOL_GPL(dm_tm_open_with_sm);
 
diff --git a/drivers/md/persistent-data/dm-transaction-manager.h b/drivers/md/persistent-data/dm-transaction-manager.h
index 6da784871db..b5b139076ca 100644
--- a/drivers/md/persistent-data/dm-transaction-manager.h
+++ b/drivers/md/persistent-data/dm-transaction-manager.h
@@ -115,16 +115,17 @@ struct dm_block_manager *dm_tm_get_bm(struct dm_transaction_manager *tm);
  *
  * Returns a tm that has an open transaction to write the new disk sm.
  * Caller should store the new sm root and commit.
+ *
+ * The superblock location is passed so the metadata space map knows it
+ * shouldn't be used.
  */
 int dm_tm_create_with_sm(struct dm_block_manager *bm, dm_block_t sb_location,
-			 struct dm_block_validator *sb_validator,
 			 struct dm_transaction_manager **tm,
-			 struct dm_space_map **sm, struct dm_block **sblock);
+			 struct dm_space_map **sm);
 
 int dm_tm_open_with_sm(struct dm_block_manager *bm, dm_block_t sb_location,
-		       struct dm_block_validator *sb_validator,
-		       size_t root_offset, size_t root_max_len,
+		       void *sm_root, size_t root_len,
 		       struct dm_transaction_manager **tm,
-		       struct dm_space_map **sm, struct dm_block **sblock);
+		       struct dm_space_map **sm);
 
 #endif	/* _LINUX_DM_TRANSACTION_MANAGER_H */
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 240ff312504..611b5f79761 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -46,6 +46,20 @@
  */
 #define	NR_RAID1_BIOS 256
 
+/* when we get a read error on a read-only array, we redirect to another
+ * device without failing the first device, or trying to over-write to
+ * correct the read error.  To keep track of bad blocks on a per-bio
+ * level, we store IO_BLOCKED in the appropriate 'bios' pointer
+ */
+#define IO_BLOCKED ((struct bio *)1)
+/* When we successfully write to a known bad-block, we need to remove the
+ * bad-block marking which must be done from process context.  So we record
+ * the success by setting devs[n].bio to IO_MADE_GOOD
+ */
+#define IO_MADE_GOOD ((struct bio *)2)
+
+#define BIO_SPECIAL(bio) ((unsigned long)bio <= 2)
+
 /* When there are this many requests queue to be written by
  * the raid1 thread, we become 'congested' to provide back-pressure
  * for writeback.
@@ -483,12 +497,14 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 	const sector_t this_sector = r1_bio->sector;
 	int sectors;
 	int best_good_sectors;
-	int start_disk;
-	int best_disk;
-	int i;
+	int best_disk, best_dist_disk, best_pending_disk;
+	int has_nonrot_disk;
+	int disk;
 	sector_t best_dist;
+	unsigned int min_pending;
 	struct md_rdev *rdev;
 	int choose_first;
+	int choose_next_idle;
 
 	rcu_read_lock();
 	/*
@@ -499,26 +515,26 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
  retry:
 	sectors = r1_bio->sectors;
 	best_disk = -1;
+	best_dist_disk = -1;
 	best_dist = MaxSector;
+	best_pending_disk = -1;
+	min_pending = UINT_MAX;
 	best_good_sectors = 0;
+	has_nonrot_disk = 0;
+	choose_next_idle = 0;
 
 	if (conf->mddev->recovery_cp < MaxSector &&
-	    (this_sector + sectors >= conf->next_resync)) {
+	    (this_sector + sectors >= conf->next_resync))
 		choose_first = 1;
-		start_disk = 0;
-	} else {
+	else
 		choose_first = 0;
-		start_disk = conf->last_used;
-	}
 
-	for (i = 0 ; i < conf->raid_disks * 2 ; i++) {
+	for (disk = 0 ; disk < conf->raid_disks * 2 ; disk++) {
 		sector_t dist;
 		sector_t first_bad;
 		int bad_sectors;
-
-		int disk = start_disk + i;
-		if (disk >= conf->raid_disks * 2)
-			disk -= conf->raid_disks * 2;
+		unsigned int pending;
+		bool nonrot;
 
 		rdev = rcu_dereference(conf->mirrors[disk].rdev);
 		if (r1_bio->bios[disk] == IO_BLOCKED
@@ -577,22 +593,77 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 		} else
 			best_good_sectors = sectors;
 
+		nonrot = blk_queue_nonrot(bdev_get_queue(rdev->bdev));
+		has_nonrot_disk |= nonrot;
+		pending = atomic_read(&rdev->nr_pending);
 		dist = abs(this_sector - conf->mirrors[disk].head_position);
-		if (choose_first
-		    /* Don't change to another disk for sequential reads */
-		    || conf->next_seq_sect == this_sector
-		    || dist == 0
-		    /* If device is idle, use it */
-		    || atomic_read(&rdev->nr_pending) == 0) {
+		if (choose_first) {
+			best_disk = disk;
+			break;
+		}
+		/* Don't change to another disk for sequential reads */
+		if (conf->mirrors[disk].next_seq_sect == this_sector
+		    || dist == 0) {
+			int opt_iosize = bdev_io_opt(rdev->bdev) >> 9;
+			struct raid1_info *mirror = &conf->mirrors[disk];
+
+			best_disk = disk;
+			/*
+			 * If buffered sequential IO size exceeds optimal
+			 * iosize, check if there is idle disk. If yes, choose
+			 * the idle disk. read_balance could already choose an
+			 * idle disk before noticing it's a sequential IO in
+			 * this disk. This doesn't matter because this disk
+			 * will idle, next time it will be utilized after the
+			 * first disk has IO size exceeds optimal iosize. In
+			 * this way, iosize of the first disk will be optimal
+			 * iosize at least. iosize of the second disk might be
+			 * small, but not a big deal since when the second disk
+			 * starts IO, the first disk is likely still busy.
+			 */
+			if (nonrot && opt_iosize > 0 &&
+			    mirror->seq_start != MaxSector &&
+			    mirror->next_seq_sect > opt_iosize &&
+			    mirror->next_seq_sect - opt_iosize >=
+			    mirror->seq_start) {
+				choose_next_idle = 1;
+				continue;
+			}
+			break;
+		}
+		/* If device is idle, use it */
+		if (pending == 0) {
 			best_disk = disk;
 			break;
 		}
+
+		if (choose_next_idle)
+			continue;
+
+		if (min_pending > pending) {
+			min_pending = pending;
+			best_pending_disk = disk;
+		}
+
 		if (dist < best_dist) {
 			best_dist = dist;
-			best_disk = disk;
+			best_dist_disk = disk;
 		}
 	}
 
+	/*
+	 * If all disks are rotational, choose the closest disk. If any disk is
+	 * non-rotational, choose the disk with less pending request even the
+	 * disk is rotational, which might/might not be optimal for raids with
+	 * mixed ratation/non-rotational disks depending on workload.
+	 */
+	if (best_disk == -1) {
+		if (has_nonrot_disk)
+			best_disk = best_pending_disk;
+		else
+			best_disk = best_dist_disk;
+	}
+
 	if (best_disk >= 0) {
 		rdev = rcu_dereference(conf->mirrors[best_disk].rdev);
 		if (!rdev)
@@ -606,8 +677,11 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 			goto retry;
 		}
 		sectors = best_good_sectors;
-		conf->next_seq_sect = this_sector + sectors;
-		conf->last_used = best_disk;
+
+		if (conf->mirrors[best_disk].next_seq_sect != this_sector)
+			conf->mirrors[best_disk].seq_start = this_sector;
+
+		conf->mirrors[best_disk].next_seq_sect = this_sector + sectors;
 	}
 	rcu_read_unlock();
 	*max_sectors = sectors;
@@ -870,10 +944,48 @@ do_sync_io:
 	pr_debug("%dB behind alloc failed, doing sync I/O\n", bio->bi_size);
 }
 
+struct raid1_plug_cb {
+	struct blk_plug_cb	cb;
+	struct bio_list		pending;
+	int			pending_cnt;
+};
+
+static void raid1_unplug(struct blk_plug_cb *cb, bool from_schedule)
+{
+	struct raid1_plug_cb *plug = container_of(cb, struct raid1_plug_cb,
+						  cb);
+	struct mddev *mddev = plug->cb.data;
+	struct r1conf *conf = mddev->private;
+	struct bio *bio;
+
+	if (from_schedule) {
+		spin_lock_irq(&conf->device_lock);
+		bio_list_merge(&conf->pending_bio_list, &plug->pending);
+		conf->pending_count += plug->pending_cnt;
+		spin_unlock_irq(&conf->device_lock);
+		md_wakeup_thread(mddev->thread);
+		kfree(plug);
+		return;
+	}
+
+	/* we aren't scheduling, so we can do the write-out directly. */
+	bio = bio_list_get(&plug->pending);
+	bitmap_unplug(mddev->bitmap);
+	wake_up(&conf->wait_barrier);
+
+	while (bio) { /* submit pending writes */
+		struct bio *next = bio->bi_next;
+		bio->bi_next = NULL;
+		generic_make_request(bio);
+		bio = next;
+	}
+	kfree(plug);
+}
+
 static void make_request(struct mddev *mddev, struct bio * bio)
 {
 	struct r1conf *conf = mddev->private;
-	struct mirror_info *mirror;
+	struct raid1_info *mirror;
 	struct r1bio *r1_bio;
 	struct bio *read_bio;
 	int i, disks;
@@ -883,6 +995,8 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
 	const unsigned long do_flush_fua = (bio->bi_rw & (REQ_FLUSH | REQ_FUA));
 	struct md_rdev *blocked_rdev;
+	struct blk_plug_cb *cb;
+	struct raid1_plug_cb *plug = NULL;
 	int first_clone;
 	int sectors_handled;
 	int max_sectors;
@@ -1185,11 +1299,22 @@ read_again:
 		mbio->bi_private = r1_bio;
 
 		atomic_inc(&r1_bio->remaining);
+
+		cb = blk_check_plugged(raid1_unplug, mddev, sizeof(*plug));
+		if (cb)
+			plug = container_of(cb, struct raid1_plug_cb, cb);
+		else
+			plug = NULL;
 		spin_lock_irqsave(&conf->device_lock, flags);
-		bio_list_add(&conf->pending_bio_list, mbio);
-		conf->pending_count++;
+		if (plug) {
+			bio_list_add(&plug->pending, mbio);
+			plug->pending_cnt++;
+		} else {
+			bio_list_add(&conf->pending_bio_list, mbio);
+			conf->pending_count++;
+		}
 		spin_unlock_irqrestore(&conf->device_lock, flags);
-		if (!mddev_check_plugged(mddev))
+		if (!plug)
 			md_wakeup_thread(mddev->thread);
 	}
 	/* Mustn't call r1_bio_write_done before this next test,
@@ -1364,7 +1489,7 @@ static int raid1_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 	struct r1conf *conf = mddev->private;
 	int err = -EEXIST;
 	int mirror = 0;
-	struct mirror_info *p;
+	struct raid1_info *p;
 	int first = 0;
 	int last = conf->raid_disks - 1;
 	struct request_queue *q = bdev_get_queue(rdev->bdev);
@@ -1433,7 +1558,7 @@ static int raid1_remove_disk(struct mddev *mddev, struct md_rdev *rdev)
 	struct r1conf *conf = mddev->private;
 	int err = 0;
 	int number = rdev->raid_disk;
-	struct mirror_info *p = conf->mirrors+ number;
+	struct raid1_info *p = conf->mirrors + number;
 
 	if (rdev != p->rdev)
 		p = conf->mirrors + conf->raid_disks + number;
@@ -1818,8 +1943,14 @@ static void sync_request_write(struct mddev *mddev, struct r1bio *r1_bio)
 
 	if (atomic_dec_and_test(&r1_bio->remaining)) {
 		/* if we're here, all write(s) have completed, so clean up */
-		md_done_sync(mddev, r1_bio->sectors, 1);
-		put_buf(r1_bio);
+		int s = r1_bio->sectors;
+		if (test_bit(R1BIO_MadeGood, &r1_bio->state) ||
+		    test_bit(R1BIO_WriteError, &r1_bio->state))
+			reschedule_retry(r1_bio);
+		else {
+			put_buf(r1_bio);
+			md_done_sync(mddev, s, 1);
+		}
 	}
 }
 
@@ -2167,8 +2298,7 @@ static void raid1d(struct mddev *mddev)
 	blk_start_plug(&plug);
 	for (;;) {
 
-		if (atomic_read(&mddev->plug_cnt) == 0)
-			flush_pending_writes(conf);
+		flush_pending_writes(conf);
 
 		spin_lock_irqsave(&conf->device_lock, flags);
 		if (list_empty(head)) {
@@ -2365,6 +2495,18 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 				bio->bi_rw = READ;
 				bio->bi_end_io = end_sync_read;
 				read_targets++;
+			} else if (!test_bit(WriteErrorSeen, &rdev->flags) &&
+				test_bit(MD_RECOVERY_SYNC, &mddev->recovery) &&
+				!test_bit(MD_RECOVERY_CHECK, &mddev->recovery)) {
+				/*
+				 * The device is suitable for reading (InSync),
+				 * but has bad block(s) here. Let's try to correct them,
+				 * if we are doing resync or repair. Otherwise, leave
+				 * this device alone for this sync request.
+				 */
+				bio->bi_rw = WRITE;
+				bio->bi_end_io = end_sync_write;
+				write_targets++;
 			}
 		}
 		if (bio->bi_end_io) {
@@ -2422,7 +2564,10 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 		/* There is nowhere to write, so all non-sync
 		 * drives must be failed - so we are finished
 		 */
-		sector_t rv = max_sector - sector_nr;
+		sector_t rv;
+		if (min_bad > 0)
+			max_sector = sector_nr + min_bad;
+		rv = max_sector - sector_nr;
 		*skipped = 1;
 		put_buf(r1_bio);
 		return rv;
@@ -2515,7 +2660,7 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 {
 	struct r1conf *conf;
 	int i;
-	struct mirror_info *disk;
+	struct raid1_info *disk;
 	struct md_rdev *rdev;
 	int err = -ENOMEM;
 
@@ -2523,7 +2668,7 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 	if (!conf)
 		goto abort;
 
-	conf->mirrors = kzalloc(sizeof(struct mirror_info)
+	conf->mirrors = kzalloc(sizeof(struct raid1_info)
 				* mddev->raid_disks * 2,
 				 GFP_KERNEL);
 	if (!conf->mirrors)
@@ -2566,6 +2711,7 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 			mddev->merge_check_needed = 1;
 
 		disk->head_position = 0;
+		disk->seq_start = MaxSector;
 	}
 	conf->raid_disks = mddev->raid_disks;
 	conf->mddev = mddev;
@@ -2579,7 +2725,6 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 	conf->recovery_disabled = mddev->recovery_disabled - 1;
 
 	err = -EIO;
-	conf->last_used = -1;
 	for (i = 0; i < conf->raid_disks * 2; i++) {
 
 		disk = conf->mirrors + i;
@@ -2605,19 +2750,9 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 			if (disk->rdev &&
 			    (disk->rdev->saved_raid_disk < 0))
 				conf->fullsync = 1;
-		} else if (conf->last_used < 0)
-			/*
-			 * The first working device is used as a
-			 * starting point to read balancing.
-			 */
-			conf->last_used = i;
+		}
 	}
 
-	if (conf->last_used < 0) {
-		printk(KERN_ERR "md/raid1:%s: no operational mirrors\n",
-		       mdname(mddev));
-		goto abort;
-	}
 	err = -ENOMEM;
 	conf->thread = md_register_thread(raid1d, mddev, "raid1");
 	if (!conf->thread) {
@@ -2792,7 +2927,7 @@ static int raid1_reshape(struct mddev *mddev)
 	 */
 	mempool_t *newpool, *oldpool;
 	struct pool_info *newpoolinfo;
-	struct mirror_info *newmirrors;
+	struct raid1_info *newmirrors;
 	struct r1conf *conf = mddev->private;
 	int cnt, raid_disks;
 	unsigned long flags;
@@ -2835,7 +2970,7 @@ static int raid1_reshape(struct mddev *mddev)
 		kfree(newpoolinfo);
 		return -ENOMEM;
 	}
-	newmirrors = kzalloc(sizeof(struct mirror_info) * raid_disks * 2,
+	newmirrors = kzalloc(sizeof(struct raid1_info) * raid_disks * 2,
 			     GFP_KERNEL);
 	if (!newmirrors) {
 		kfree(newpoolinfo);
@@ -2874,7 +3009,6 @@ static int raid1_reshape(struct mddev *mddev)
 	conf->raid_disks = mddev->raid_disks = raid_disks;
 	mddev->delta_disks = 0;
 
-	conf->last_used = 0; /* just make sure it is in-range */
 	lower_barrier(conf);
 
 	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
diff --git a/drivers/md/raid1.h b/drivers/md/raid1.h
index 80ded139314..0ff3715fb7e 100644
--- a/drivers/md/raid1.h
+++ b/drivers/md/raid1.h
@@ -1,9 +1,15 @@
 #ifndef _RAID1_H
 #define _RAID1_H
 
-struct mirror_info {
+struct raid1_info {
 	struct md_rdev	*rdev;
 	sector_t	head_position;
+
+	/* When choose the best device for a read (read_balance())
+	 * we try to keep sequential reads one the same device
+	 */
+	sector_t	next_seq_sect;
+	sector_t	seq_start;
 };
 
 /*
@@ -24,17 +30,11 @@ struct pool_info {
 
 struct r1conf {
 	struct mddev		*mddev;
-	struct mirror_info	*mirrors;	/* twice 'raid_disks' to
+	struct raid1_info	*mirrors;	/* twice 'raid_disks' to
 						 * allow for replacements.
 						 */
 	int			raid_disks;
 
-	/* When choose the best device for a read (read_balance())
-	 * we try to keep sequential reads one the same device
-	 * using 'last_used' and 'next_seq_sect'
-	 */
-	int			last_used;
-	sector_t		next_seq_sect;
 	/* During resync, read_balancing is only allowed on the part
 	 * of the array that has been resynced.  'next_resync' tells us
 	 * where that is.
@@ -135,20 +135,6 @@ struct r1bio {
 	/* DO NOT PUT ANY NEW FIELDS HERE - bios array is contiguously alloced*/
 };
 
-/* when we get a read error on a read-only array, we redirect to another
- * device without failing the first device, or trying to over-write to
- * correct the read error.  To keep track of bad blocks on a per-bio
- * level, we store IO_BLOCKED in the appropriate 'bios' pointer
- */
-#define IO_BLOCKED ((struct bio *)1)
-/* When we successfully write to a known bad-block, we need to remove the
- * bad-block marking which must be done from process context.  So we record
- * the success by setting bios[n] to IO_MADE_GOOD
- */
-#define IO_MADE_GOOD ((struct bio *)2)
-
-#define BIO_SPECIAL(bio) ((unsigned long)bio <= 2)
-
 /* bits for r1bio.state */
 #define	R1BIO_Uptodate	0
 #define	R1BIO_IsSync	1
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 8da6282254c..de5ed6fd880 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -60,7 +60,21 @@
  */
 #define	NR_RAID10_BIOS 256
 
-/* When there are this many requests queue to be written by
+/* when we get a read error on a read-only array, we redirect to another
+ * device without failing the first device, or trying to over-write to
+ * correct the read error.  To keep track of bad blocks on a per-bio
+ * level, we store IO_BLOCKED in the appropriate 'bios' pointer
+ */
+#define IO_BLOCKED ((struct bio *)1)
+/* When we successfully write to a known bad-block, we need to remove the
+ * bad-block marking which must be done from process context.  So we record
+ * the success by setting devs[n].bio to IO_MADE_GOOD
+ */
+#define IO_MADE_GOOD ((struct bio *)2)
+
+#define BIO_SPECIAL(bio) ((unsigned long)bio <= 2)
+
+/* When there are this many requests queued to be written by
  * the raid10 thread, we become 'congested' to provide back-pressure
  * for writeback.
  */
@@ -717,7 +731,7 @@ static struct md_rdev *read_balance(struct r10conf *conf,
 	int sectors = r10_bio->sectors;
 	int best_good_sectors;
 	sector_t new_distance, best_dist;
-	struct md_rdev *rdev, *best_rdev;
+	struct md_rdev *best_rdev, *rdev = NULL;
 	int do_balance;
 	int best_slot;
 	struct geom *geo = &conf->geo;
@@ -839,9 +853,8 @@ retry:
 	return rdev;
 }
 
-static int raid10_congested(void *data, int bits)
+int md_raid10_congested(struct mddev *mddev, int bits)
 {
-	struct mddev *mddev = data;
 	struct r10conf *conf = mddev->private;
 	int i, ret = 0;
 
@@ -849,8 +862,6 @@ static int raid10_congested(void *data, int bits)
 	    conf->pending_count >= max_queued_requests)
 		return 1;
 
-	if (mddev_congested(mddev, bits))
-		return 1;
 	rcu_read_lock();
 	for (i = 0;
 	     (i < conf->geo.raid_disks || i < conf->prev.raid_disks)
@@ -866,6 +877,15 @@ static int raid10_congested(void *data, int bits)
 	rcu_read_unlock();
 	return ret;
 }
+EXPORT_SYMBOL_GPL(md_raid10_congested);
+
+static int raid10_congested(void *data, int bits)
+{
+	struct mddev *mddev = data;
+
+	return mddev_congested(mddev, bits) ||
+		md_raid10_congested(mddev, bits);
+}
 
 static void flush_pending_writes(struct r10conf *conf)
 {
@@ -1546,7 +1566,7 @@ static void error(struct mddev *mddev, struct md_rdev *rdev)
 static void print_conf(struct r10conf *conf)
 {
 	int i;
-	struct mirror_info *tmp;
+	struct raid10_info *tmp;
 
 	printk(KERN_DEBUG "RAID10 conf printout:\n");
 	if (!conf) {
@@ -1580,7 +1600,7 @@ static int raid10_spare_active(struct mddev *mddev)
 {
 	int i;
 	struct r10conf *conf = mddev->private;
-	struct mirror_info *tmp;
+	struct raid10_info *tmp;
 	int count = 0;
 	unsigned long flags;
 
@@ -1655,7 +1675,7 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 	else
 		mirror = first;
 	for ( ; mirror <= last ; mirror++) {
-		struct mirror_info *p = &conf->mirrors[mirror];
+		struct raid10_info *p = &conf->mirrors[mirror];
 		if (p->recovery_disabled == mddev->recovery_disabled)
 			continue;
 		if (p->rdev) {
@@ -1709,7 +1729,7 @@ static int raid10_remove_disk(struct mddev *mddev, struct md_rdev *rdev)
 	int err = 0;
 	int number = rdev->raid_disk;
 	struct md_rdev **rdevp;
-	struct mirror_info *p = conf->mirrors + number;
+	struct raid10_info *p = conf->mirrors + number;
 
 	print_conf(conf);
 	if (rdev == p->rdev)
@@ -2660,8 +2680,7 @@ static void raid10d(struct mddev *mddev)
 	blk_start_plug(&plug);
 	for (;;) {
 
-		if (atomic_read(&mddev->plug_cnt) == 0)
-			flush_pending_writes(conf);
+		flush_pending_writes(conf);
 
 		spin_lock_irqsave(&conf->device_lock, flags);
 		if (list_empty(head)) {
@@ -2876,7 +2895,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 			sector_t sect;
 			int must_sync;
 			int any_working;
-			struct mirror_info *mirror = &conf->mirrors[i];
+			struct raid10_info *mirror = &conf->mirrors[i];
 
 			if ((mirror->rdev == NULL ||
 			     test_bit(In_sync, &mirror->rdev->flags))
@@ -3388,7 +3407,7 @@ static struct r10conf *setup_conf(struct mddev *mddev)
 		goto out;
 
 	/* FIXME calc properly */
-	conf->mirrors = kzalloc(sizeof(struct mirror_info)*(mddev->raid_disks +
+	conf->mirrors = kzalloc(sizeof(struct raid10_info)*(mddev->raid_disks +
 							    max(0,mddev->delta_disks)),
 				GFP_KERNEL);
 	if (!conf->mirrors)
@@ -3452,7 +3471,7 @@ static int run(struct mddev *mddev)
 {
 	struct r10conf *conf;
 	int i, disk_idx, chunk_size;
-	struct mirror_info *disk;
+	struct raid10_info *disk;
 	struct md_rdev *rdev;
 	sector_t size;
 	sector_t min_offset_diff = 0;
@@ -3472,12 +3491,14 @@ static int run(struct mddev *mddev)
 	conf->thread = NULL;
 
 	chunk_size = mddev->chunk_sectors << 9;
-	blk_queue_io_min(mddev->queue, chunk_size);
-	if (conf->geo.raid_disks % conf->geo.near_copies)
-		blk_queue_io_opt(mddev->queue, chunk_size * conf->geo.raid_disks);
-	else
-		blk_queue_io_opt(mddev->queue, chunk_size *
-				 (conf->geo.raid_disks / conf->geo.near_copies));
+	if (mddev->queue) {
+		blk_queue_io_min(mddev->queue, chunk_size);
+		if (conf->geo.raid_disks % conf->geo.near_copies)
+			blk_queue_io_opt(mddev->queue, chunk_size * conf->geo.raid_disks);
+		else
+			blk_queue_io_opt(mddev->queue, chunk_size *
+					 (conf->geo.raid_disks / conf->geo.near_copies));
+	}
 
 	rdev_for_each(rdev, mddev) {
 		long long diff;
@@ -3511,8 +3532,9 @@ static int run(struct mddev *mddev)
 		if (first || diff < min_offset_diff)
 			min_offset_diff = diff;
 
-		disk_stack_limits(mddev->gendisk, rdev->bdev,
-				  rdev->data_offset << 9);
+		if (mddev->gendisk)
+			disk_stack_limits(mddev->gendisk, rdev->bdev,
+					  rdev->data_offset << 9);
 
 		disk->head_position = 0;
 	}
@@ -3575,22 +3597,22 @@ static int run(struct mddev *mddev)
 	md_set_array_sectors(mddev, size);
 	mddev->resync_max_sectors = size;
 
-	mddev->queue->backing_dev_info.congested_fn = raid10_congested;
-	mddev->queue->backing_dev_info.congested_data = mddev;
-
-	/* Calculate max read-ahead size.
-	 * We need to readahead at least twice a whole stripe....
-	 * maybe...
-	 */
-	{
+	if (mddev->queue) {
 		int stripe = conf->geo.raid_disks *
 			((mddev->chunk_sectors << 9) / PAGE_SIZE);
+		mddev->queue->backing_dev_info.congested_fn = raid10_congested;
+		mddev->queue->backing_dev_info.congested_data = mddev;
+
+		/* Calculate max read-ahead size.
+		 * We need to readahead at least twice a whole stripe....
+		 * maybe...
+		 */
 		stripe /= conf->geo.near_copies;
 		if (mddev->queue->backing_dev_info.ra_pages < 2 * stripe)
 			mddev->queue->backing_dev_info.ra_pages = 2 * stripe;
+		blk_queue_merge_bvec(mddev->queue, raid10_mergeable_bvec);
 	}
 
-	blk_queue_merge_bvec(mddev->queue, raid10_mergeable_bvec);
 
 	if (md_integrity_register(mddev))
 		goto out_free_conf;
@@ -3641,7 +3663,10 @@ static int stop(struct mddev *mddev)
 	lower_barrier(conf);
 
 	md_unregister_thread(&mddev->thread);
-	blk_sync_queue(mddev->queue); /* the unplug fn references 'conf'*/
+	if (mddev->queue)
+		/* the unplug fn references 'conf'*/
+		blk_sync_queue(mddev->queue);
+
 	if (conf->r10bio_pool)
 		mempool_destroy(conf->r10bio_pool);
 	kfree(conf->mirrors);
@@ -3805,7 +3830,7 @@ static int raid10_check_reshape(struct mddev *mddev)
 	if (mddev->delta_disks > 0) {
 		/* allocate new 'mirrors' list */
 		conf->mirrors_new = kzalloc(
-			sizeof(struct mirror_info)
+			sizeof(struct raid10_info)
 			*(mddev->raid_disks +
 			  mddev->delta_disks),
 			GFP_KERNEL);
@@ -3930,7 +3955,7 @@ static int raid10_start_reshape(struct mddev *mddev)
 	spin_lock_irq(&conf->device_lock);
 	if (conf->mirrors_new) {
 		memcpy(conf->mirrors_new, conf->mirrors,
-		       sizeof(struct mirror_info)*conf->prev.raid_disks);
+		       sizeof(struct raid10_info)*conf->prev.raid_disks);
 		smp_mb();
 		kfree(conf->mirrors_old); /* FIXME and elsewhere */
 		conf->mirrors_old = conf->mirrors;
diff --git a/drivers/md/raid10.h b/drivers/md/raid10.h
index 135b1b0a155..007c2c68dd8 100644
--- a/drivers/md/raid10.h
+++ b/drivers/md/raid10.h
@@ -1,7 +1,7 @@
 #ifndef _RAID10_H
 #define _RAID10_H
 
-struct mirror_info {
+struct raid10_info {
 	struct md_rdev	*rdev, *replacement;
 	sector_t	head_position;
 	int		recovery_disabled;	/* matches
@@ -13,8 +13,8 @@ struct mirror_info {
 
 struct r10conf {
 	struct mddev		*mddev;
-	struct mirror_info	*mirrors;
-	struct mirror_info	*mirrors_new, *mirrors_old;
+	struct raid10_info	*mirrors;
+	struct raid10_info	*mirrors_new, *mirrors_old;
 	spinlock_t		device_lock;
 
 	/* geometry */
@@ -123,20 +123,6 @@ struct r10bio {
 	} devs[0];
 };
 
-/* when we get a read error on a read-only array, we redirect to another
- * device without failing the first device, or trying to over-write to
- * correct the read error.  To keep track of bad blocks on a per-bio
- * level, we store IO_BLOCKED in the appropriate 'bios' pointer
- */
-#define IO_BLOCKED ((struct bio*)1)
-/* When we successfully write to a known bad-block, we need to remove the
- * bad-block marking which must be done from process context.  So we record
- * the success by setting devs[n].bio to IO_MADE_GOOD
- */
-#define IO_MADE_GOOD ((struct bio *)2)
-
-#define BIO_SPECIAL(bio) ((unsigned long)bio <= 2)
-
 /* bits for r10bio.state */
 enum r10bio_state {
 	R10BIO_Uptodate,
@@ -159,4 +145,7 @@ enum r10bio_state {
  */
 	R10BIO_Previous,
 };
+
+extern int md_raid10_congested(struct mddev *mddev, int bits);
+
 #endif
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 04348d76bb3..adda94df5eb 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -99,34 +99,40 @@ static inline struct bio *r5_next_bio(struct bio *bio, sector_t sector)
  * We maintain a biased count of active stripes in the bottom 16 bits of
  * bi_phys_segments, and a count of processed stripes in the upper 16 bits
  */
-static inline int raid5_bi_phys_segments(struct bio *bio)
+static inline int raid5_bi_processed_stripes(struct bio *bio)
 {
-	return bio->bi_phys_segments & 0xffff;
+	atomic_t *segments = (atomic_t *)&bio->bi_phys_segments;
+	return (atomic_read(segments) >> 16) & 0xffff;
 }
 
-static inline int raid5_bi_hw_segments(struct bio *bio)
+static inline int raid5_dec_bi_active_stripes(struct bio *bio)
 {
-	return (bio->bi_phys_segments >> 16) & 0xffff;
+	atomic_t *segments = (atomic_t *)&bio->bi_phys_segments;
+	return atomic_sub_return(1, segments) & 0xffff;
 }
 
-static inline int raid5_dec_bi_phys_segments(struct bio *bio)
+static inline void raid5_inc_bi_active_stripes(struct bio *bio)
 {
-	--bio->bi_phys_segments;
-	return raid5_bi_phys_segments(bio);
+	atomic_t *segments = (atomic_t *)&bio->bi_phys_segments;
+	atomic_inc(segments);
 }
 
-static inline int raid5_dec_bi_hw_segments(struct bio *bio)
+static inline void raid5_set_bi_processed_stripes(struct bio *bio,
+	unsigned int cnt)
 {
-	unsigned short val = raid5_bi_hw_segments(bio);
+	atomic_t *segments = (atomic_t *)&bio->bi_phys_segments;
+	int old, new;
 
-	--val;
-	bio->bi_phys_segments = (val << 16) | raid5_bi_phys_segments(bio);
-	return val;
+	do {
+		old = atomic_read(segments);
+		new = (old & 0xffff) | (cnt << 16);
+	} while (atomic_cmpxchg(segments, old, new) != old);
 }
 
-static inline void raid5_set_bi_hw_segments(struct bio *bio, unsigned int cnt)
+static inline void raid5_set_bi_stripes(struct bio *bio, unsigned int cnt)
 {
-	bio->bi_phys_segments = raid5_bi_phys_segments(bio) | (cnt << 16);
+	atomic_t *segments = (atomic_t *)&bio->bi_phys_segments;
+	atomic_set(segments, cnt);
 }
 
 /* Find first data disk in a raid6 stripe */
@@ -190,49 +196,56 @@ static int stripe_operations_active(struct stripe_head *sh)
 	       test_bit(STRIPE_COMPUTE_RUN, &sh->state);
 }
 
-static void __release_stripe(struct r5conf *conf, struct stripe_head *sh)
+static void do_release_stripe(struct r5conf *conf, struct stripe_head *sh)
 {
-	if (atomic_dec_and_test(&sh->count)) {
-		BUG_ON(!list_empty(&sh->lru));
-		BUG_ON(atomic_read(&conf->active_stripes)==0);
-		if (test_bit(STRIPE_HANDLE, &sh->state)) {
-			if (test_bit(STRIPE_DELAYED, &sh->state) &&
-			    !test_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
-				list_add_tail(&sh->lru, &conf->delayed_list);
-			else if (test_bit(STRIPE_BIT_DELAY, &sh->state) &&
-				   sh->bm_seq - conf->seq_write > 0)
-				list_add_tail(&sh->lru, &conf->bitmap_list);
-			else {
-				clear_bit(STRIPE_DELAYED, &sh->state);
-				clear_bit(STRIPE_BIT_DELAY, &sh->state);
-				list_add_tail(&sh->lru, &conf->handle_list);
-			}
-			md_wakeup_thread(conf->mddev->thread);
-		} else {
-			BUG_ON(stripe_operations_active(sh));
-			if (test_and_clear_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
-				if (atomic_dec_return(&conf->preread_active_stripes)
-				    < IO_THRESHOLD)
-					md_wakeup_thread(conf->mddev->thread);
-			atomic_dec(&conf->active_stripes);
-			if (!test_bit(STRIPE_EXPANDING, &sh->state)) {
-				list_add_tail(&sh->lru, &conf->inactive_list);
-				wake_up(&conf->wait_for_stripe);
-				if (conf->retry_read_aligned)
-					md_wakeup_thread(conf->mddev->thread);
-			}
+	BUG_ON(!list_empty(&sh->lru));
+	BUG_ON(atomic_read(&conf->active_stripes)==0);
+	if (test_bit(STRIPE_HANDLE, &sh->state)) {
+		if (test_bit(STRIPE_DELAYED, &sh->state) &&
+		    !test_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
+			list_add_tail(&sh->lru, &conf->delayed_list);
+		else if (test_bit(STRIPE_BIT_DELAY, &sh->state) &&
+			   sh->bm_seq - conf->seq_write > 0)
+			list_add_tail(&sh->lru, &conf->bitmap_list);
+		else {
+			clear_bit(STRIPE_DELAYED, &sh->state);
+			clear_bit(STRIPE_BIT_DELAY, &sh->state);
+			list_add_tail(&sh->lru, &conf->handle_list);
+		}
+		md_wakeup_thread(conf->mddev->thread);
+	} else {
+		BUG_ON(stripe_operations_active(sh));
+		if (test_and_clear_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
+			if (atomic_dec_return(&conf->preread_active_stripes)
+			    < IO_THRESHOLD)
+				md_wakeup_thread(conf->mddev->thread);
+		atomic_dec(&conf->active_stripes);
+		if (!test_bit(STRIPE_EXPANDING, &sh->state)) {
+			list_add_tail(&sh->lru, &conf->inactive_list);
+			wake_up(&conf->wait_for_stripe);
+			if (conf->retry_read_aligned)
+				md_wakeup_thread(conf->mddev->thread);
 		}
 	}
 }
 
+static void __release_stripe(struct r5conf *conf, struct stripe_head *sh)
+{
+	if (atomic_dec_and_test(&sh->count))
+		do_release_stripe(conf, sh);
+}
+
 static void release_stripe(struct stripe_head *sh)
 {
 	struct r5conf *conf = sh->raid_conf;
 	unsigned long flags;
 
-	spin_lock_irqsave(&conf->device_lock, flags);
-	__release_stripe(conf, sh);
-	spin_unlock_irqrestore(&conf->device_lock, flags);
+	local_irq_save(flags);
+	if (atomic_dec_and_lock(&sh->count, &conf->device_lock)) {
+		do_release_stripe(conf, sh);
+		spin_unlock(&conf->device_lock);
+	}
+	local_irq_restore(flags);
 }
 
 static inline void remove_hash(struct stripe_head *sh)
@@ -471,7 +484,8 @@ get_active_stripe(struct r5conf *conf, sector_t sector,
 		} else {
 			if (atomic_read(&sh->count)) {
 				BUG_ON(!list_empty(&sh->lru)
-				    && !test_bit(STRIPE_EXPANDING, &sh->state));
+				    && !test_bit(STRIPE_EXPANDING, &sh->state)
+				    && !test_bit(STRIPE_ON_UNPLUG_LIST, &sh->state));
 			} else {
 				if (!test_bit(STRIPE_HANDLE, &sh->state))
 					atomic_inc(&conf->active_stripes);
@@ -640,6 +654,9 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 			else
 				bi->bi_sector = (sh->sector
 						 + rdev->data_offset);
+			if (test_bit(R5_ReadNoMerge, &sh->dev[i].flags))
+				bi->bi_rw |= REQ_FLUSH;
+
 			bi->bi_flags = 1 << BIO_UPTODATE;
 			bi->bi_idx = 0;
 			bi->bi_io_vec[0].bv_len = STRIPE_SIZE;
@@ -749,14 +766,12 @@ static void ops_complete_biofill(void *stripe_head_ref)
 {
 	struct stripe_head *sh = stripe_head_ref;
 	struct bio *return_bi = NULL;
-	struct r5conf *conf = sh->raid_conf;
 	int i;
 
 	pr_debug("%s: stripe %llu\n", __func__,
 		(unsigned long long)sh->sector);
 
 	/* clear completed biofills */
-	spin_lock_irq(&conf->device_lock);
 	for (i = sh->disks; i--; ) {
 		struct r5dev *dev = &sh->dev[i];
 
@@ -774,7 +789,7 @@ static void ops_complete_biofill(void *stripe_head_ref)
 			while (rbi && rbi->bi_sector <
 				dev->sector + STRIPE_SECTORS) {
 				rbi2 = r5_next_bio(rbi, dev->sector);
-				if (!raid5_dec_bi_phys_segments(rbi)) {
+				if (!raid5_dec_bi_active_stripes(rbi)) {
 					rbi->bi_next = return_bi;
 					return_bi = rbi;
 				}
@@ -782,7 +797,6 @@ static void ops_complete_biofill(void *stripe_head_ref)
 			}
 		}
 	}
-	spin_unlock_irq(&conf->device_lock);
 	clear_bit(STRIPE_BIOFILL_RUN, &sh->state);
 
 	return_io(return_bi);
@@ -794,7 +808,6 @@ static void ops_complete_biofill(void *stripe_head_ref)
 static void ops_run_biofill(struct stripe_head *sh)
 {
 	struct dma_async_tx_descriptor *tx = NULL;
-	struct r5conf *conf = sh->raid_conf;
 	struct async_submit_ctl submit;
 	int i;
 
@@ -805,10 +818,10 @@ static void ops_run_biofill(struct stripe_head *sh)
 		struct r5dev *dev = &sh->dev[i];
 		if (test_bit(R5_Wantfill, &dev->flags)) {
 			struct bio *rbi;
-			spin_lock_irq(&conf->device_lock);
+			spin_lock_irq(&sh->stripe_lock);
 			dev->read = rbi = dev->toread;
 			dev->toread = NULL;
-			spin_unlock_irq(&conf->device_lock);
+			spin_unlock_irq(&sh->stripe_lock);
 			while (rbi && rbi->bi_sector <
 				dev->sector + STRIPE_SECTORS) {
 				tx = async_copy_data(0, rbi, dev->page,
@@ -1144,12 +1157,12 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
 		if (test_and_clear_bit(R5_Wantdrain, &dev->flags)) {
 			struct bio *wbi;
 
-			spin_lock_irq(&sh->raid_conf->device_lock);
+			spin_lock_irq(&sh->stripe_lock);
 			chosen = dev->towrite;
 			dev->towrite = NULL;
 			BUG_ON(dev->written);
 			wbi = dev->written = chosen;
-			spin_unlock_irq(&sh->raid_conf->device_lock);
+			spin_unlock_irq(&sh->stripe_lock);
 
 			while (wbi && wbi->bi_sector <
 				dev->sector + STRIPE_SECTORS) {
@@ -1454,6 +1467,8 @@ static int grow_one_stripe(struct r5conf *conf)
 	init_waitqueue_head(&sh->ops.wait_for_ops);
 	#endif
 
+	spin_lock_init(&sh->stripe_lock);
+
 	if (grow_buffers(sh)) {
 		shrink_buffers(sh);
 		kmem_cache_free(conf->slab_cache, sh);
@@ -1739,7 +1754,9 @@ static void raid5_end_read_request(struct bio * bi, int error)
 			atomic_add(STRIPE_SECTORS, &rdev->corrected_errors);
 			clear_bit(R5_ReadError, &sh->dev[i].flags);
 			clear_bit(R5_ReWrite, &sh->dev[i].flags);
-		}
+		} else if (test_bit(R5_ReadNoMerge, &sh->dev[i].flags))
+			clear_bit(R5_ReadNoMerge, &sh->dev[i].flags);
+
 		if (atomic_read(&rdev->read_errors))
 			atomic_set(&rdev->read_errors, 0);
 	} else {
@@ -1784,7 +1801,11 @@ static void raid5_end_read_request(struct bio * bi, int error)
 		else
 			retry = 1;
 		if (retry)
-			set_bit(R5_ReadError, &sh->dev[i].flags);
+			if (test_bit(R5_ReadNoMerge, &sh->dev[i].flags)) {
+				set_bit(R5_ReadError, &sh->dev[i].flags);
+				clear_bit(R5_ReadNoMerge, &sh->dev[i].flags);
+			} else
+				set_bit(R5_ReadNoMerge, &sh->dev[i].flags);
 		else {
 			clear_bit(R5_ReadError, &sh->dev[i].flags);
 			clear_bit(R5_ReWrite, &sh->dev[i].flags);
@@ -2340,11 +2361,18 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 		(unsigned long long)bi->bi_sector,
 		(unsigned long long)sh->sector);
 
-
-	spin_lock_irq(&conf->device_lock);
+	/*
+	 * If several bio share a stripe. The bio bi_phys_segments acts as a
+	 * reference count to avoid race. The reference count should already be
+	 * increased before this function is called (for example, in
+	 * make_request()), so other bio sharing this stripe will not free the
+	 * stripe. If a stripe is owned by one stripe, the stripe lock will
+	 * protect it.
+	 */
+	spin_lock_irq(&sh->stripe_lock);
 	if (forwrite) {
 		bip = &sh->dev[dd_idx].towrite;
-		if (*bip == NULL && sh->dev[dd_idx].written == NULL)
+		if (*bip == NULL)
 			firstwrite = 1;
 	} else
 		bip = &sh->dev[dd_idx].toread;
@@ -2360,7 +2388,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 	if (*bip)
 		bi->bi_next = *bip;
 	*bip = bi;
-	bi->bi_phys_segments++;
+	raid5_inc_bi_active_stripes(bi);
 
 	if (forwrite) {
 		/* check if page is covered */
@@ -2375,7 +2403,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 		if (sector >= sh->dev[dd_idx].sector + STRIPE_SECTORS)
 			set_bit(R5_OVERWRITE, &sh->dev[dd_idx].flags);
 	}
-	spin_unlock_irq(&conf->device_lock);
+	spin_unlock_irq(&sh->stripe_lock);
 
 	pr_debug("added bi b#%llu to stripe s#%llu, disk %d.\n",
 		(unsigned long long)(*bip)->bi_sector,
@@ -2391,7 +2419,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 
  overlap:
 	set_bit(R5_Overlap, &sh->dev[dd_idx].flags);
-	spin_unlock_irq(&conf->device_lock);
+	spin_unlock_irq(&sh->stripe_lock);
 	return 0;
 }
 
@@ -2441,10 +2469,11 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 				rdev_dec_pending(rdev, conf->mddev);
 			}
 		}
-		spin_lock_irq(&conf->device_lock);
+		spin_lock_irq(&sh->stripe_lock);
 		/* fail all writes first */
 		bi = sh->dev[i].towrite;
 		sh->dev[i].towrite = NULL;
+		spin_unlock_irq(&sh->stripe_lock);
 		if (bi) {
 			s->to_write--;
 			bitmap_end = 1;
@@ -2457,13 +2486,17 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 			sh->dev[i].sector + STRIPE_SECTORS) {
 			struct bio *nextbi = r5_next_bio(bi, sh->dev[i].sector);
 			clear_bit(BIO_UPTODATE, &bi->bi_flags);
-			if (!raid5_dec_bi_phys_segments(bi)) {
+			if (!raid5_dec_bi_active_stripes(bi)) {
 				md_write_end(conf->mddev);
 				bi->bi_next = *return_bi;
 				*return_bi = bi;
 			}
 			bi = nextbi;
 		}
+		if (bitmap_end)
+			bitmap_endwrite(conf->mddev->bitmap, sh->sector,
+				STRIPE_SECTORS, 0, 0);
+		bitmap_end = 0;
 		/* and fail all 'written' */
 		bi = sh->dev[i].written;
 		sh->dev[i].written = NULL;
@@ -2472,7 +2505,7 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 		       sh->dev[i].sector + STRIPE_SECTORS) {
 			struct bio *bi2 = r5_next_bio(bi, sh->dev[i].sector);
 			clear_bit(BIO_UPTODATE, &bi->bi_flags);
-			if (!raid5_dec_bi_phys_segments(bi)) {
+			if (!raid5_dec_bi_active_stripes(bi)) {
 				md_write_end(conf->mddev);
 				bi->bi_next = *return_bi;
 				*return_bi = bi;
@@ -2496,14 +2529,13 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 				struct bio *nextbi =
 					r5_next_bio(bi, sh->dev[i].sector);
 				clear_bit(BIO_UPTODATE, &bi->bi_flags);
-				if (!raid5_dec_bi_phys_segments(bi)) {
+				if (!raid5_dec_bi_active_stripes(bi)) {
 					bi->bi_next = *return_bi;
 					*return_bi = bi;
 				}
 				bi = nextbi;
 			}
 		}
-		spin_unlock_irq(&conf->device_lock);
 		if (bitmap_end)
 			bitmap_endwrite(conf->mddev->bitmap, sh->sector,
 					STRIPE_SECTORS, 0, 0);
@@ -2707,30 +2739,23 @@ static void handle_stripe_clean_event(struct r5conf *conf,
 				test_bit(R5_UPTODATE, &dev->flags)) {
 				/* We can return any write requests */
 				struct bio *wbi, *wbi2;
-				int bitmap_end = 0;
 				pr_debug("Return write for disc %d\n", i);
-				spin_lock_irq(&conf->device_lock);
 				wbi = dev->written;
 				dev->written = NULL;
 				while (wbi && wbi->bi_sector <
 					dev->sector + STRIPE_SECTORS) {
 					wbi2 = r5_next_bio(wbi, dev->sector);
-					if (!raid5_dec_bi_phys_segments(wbi)) {
+					if (!raid5_dec_bi_active_stripes(wbi)) {
 						md_write_end(conf->mddev);
 						wbi->bi_next = *return_bi;
 						*return_bi = wbi;
 					}
 					wbi = wbi2;
 				}
-				if (dev->towrite == NULL)
-					bitmap_end = 1;
-				spin_unlock_irq(&conf->device_lock);
-				if (bitmap_end)
-					bitmap_endwrite(conf->mddev->bitmap,
-							sh->sector,
-							STRIPE_SECTORS,
+				bitmap_endwrite(conf->mddev->bitmap, sh->sector,
+						STRIPE_SECTORS,
 					 !test_bit(STRIPE_DEGRADED, &sh->state),
-							0);
+						0);
 			}
 		}
 
@@ -3182,7 +3207,6 @@ static void analyse_stripe(struct stripe_head *sh, struct stripe_head_state *s)
 
 	/* Now to look around and see what can be done */
 	rcu_read_lock();
-	spin_lock_irq(&conf->device_lock);
 	for (i=disks; i--; ) {
 		struct md_rdev *rdev;
 		sector_t first_bad;
@@ -3328,7 +3352,6 @@ static void analyse_stripe(struct stripe_head *sh, struct stripe_head_state *s)
 				do_recovery = 1;
 		}
 	}
-	spin_unlock_irq(&conf->device_lock);
 	if (test_bit(STRIPE_SYNCING, &sh->state)) {
 		/* If there is a failed device being replaced,
 		 *     we must be recovering.
@@ -3791,7 +3814,7 @@ static struct bio *remove_bio_from_retry(struct r5conf *conf)
 		 * this sets the active strip count to 1 and the processed
 		 * strip count to zero (upper 8 bits)
 		 */
-		bi->bi_phys_segments = 1; /* biased count of active stripes */
+		raid5_set_bi_stripes(bi, 1); /* biased count of active stripes */
 	}
 
 	return bi;
@@ -3988,6 +4011,62 @@ static struct stripe_head *__get_priority_stripe(struct r5conf *conf)
 	return sh;
 }
 
+struct raid5_plug_cb {
+	struct blk_plug_cb	cb;
+	struct list_head	list;
+};
+
+static void raid5_unplug(struct blk_plug_cb *blk_cb, bool from_schedule)
+{
+	struct raid5_plug_cb *cb = container_of(
+		blk_cb, struct raid5_plug_cb, cb);
+	struct stripe_head *sh;
+	struct mddev *mddev = cb->cb.data;
+	struct r5conf *conf = mddev->private;
+
+	if (cb->list.next && !list_empty(&cb->list)) {
+		spin_lock_irq(&conf->device_lock);
+		while (!list_empty(&cb->list)) {
+			sh = list_first_entry(&cb->list, struct stripe_head, lru);
+			list_del_init(&sh->lru);
+			/*
+			 * avoid race release_stripe_plug() sees
+			 * STRIPE_ON_UNPLUG_LIST clear but the stripe
+			 * is still in our list
+			 */
+			smp_mb__before_clear_bit();
+			clear_bit(STRIPE_ON_UNPLUG_LIST, &sh->state);
+			__release_stripe(conf, sh);
+		}
+		spin_unlock_irq(&conf->device_lock);
+	}
+	kfree(cb);
+}
+
+static void release_stripe_plug(struct mddev *mddev,
+				struct stripe_head *sh)
+{
+	struct blk_plug_cb *blk_cb = blk_check_plugged(
+		raid5_unplug, mddev,
+		sizeof(struct raid5_plug_cb));
+	struct raid5_plug_cb *cb;
+
+	if (!blk_cb) {
+		release_stripe(sh);
+		return;
+	}
+
+	cb = container_of(blk_cb, struct raid5_plug_cb, cb);
+
+	if (cb->list.next == NULL)
+		INIT_LIST_HEAD(&cb->list);
+
+	if (!test_and_set_bit(STRIPE_ON_UNPLUG_LIST, &sh->state))
+		list_add_tail(&sh->lru, &cb->list);
+	else
+		release_stripe(sh);
+}
+
 static void make_request(struct mddev *mddev, struct bio * bi)
 {
 	struct r5conf *conf = mddev->private;
@@ -4113,11 +4192,10 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 			finish_wait(&conf->wait_for_overlap, &w);
 			set_bit(STRIPE_HANDLE, &sh->state);
 			clear_bit(STRIPE_DELAYED, &sh->state);
-			if ((bi->bi_rw & REQ_SYNC) &&
+			if ((bi->bi_rw & REQ_NOIDLE) &&
 			    !test_and_set_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
 				atomic_inc(&conf->preread_active_stripes);
-			mddev_check_plugged(mddev);
-			release_stripe(sh);
+			release_stripe_plug(mddev, sh);
 		} else {
 			/* cannot get stripe for read-ahead, just give-up */
 			clear_bit(BIO_UPTODATE, &bi->bi_flags);
@@ -4126,9 +4204,7 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 		}
 	}
 
-	spin_lock_irq(&conf->device_lock);
-	remaining = raid5_dec_bi_phys_segments(bi);
-	spin_unlock_irq(&conf->device_lock);
+	remaining = raid5_dec_bi_active_stripes(bi);
 	if (remaining == 0) {
 
 		if ( rw == WRITE )
@@ -4484,7 +4560,7 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 		     sector += STRIPE_SECTORS,
 		     scnt++) {
 
-		if (scnt < raid5_bi_hw_segments(raid_bio))
+		if (scnt < raid5_bi_processed_stripes(raid_bio))
 			/* already done this stripe */
 			continue;
 
@@ -4492,25 +4568,24 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 
 		if (!sh) {
 			/* failed to get a stripe - must wait */
-			raid5_set_bi_hw_segments(raid_bio, scnt);
+			raid5_set_bi_processed_stripes(raid_bio, scnt);
 			conf->retry_read_aligned = raid_bio;
 			return handled;
 		}
 
 		if (!add_stripe_bio(sh, raid_bio, dd_idx, 0)) {
 			release_stripe(sh);
-			raid5_set_bi_hw_segments(raid_bio, scnt);
+			raid5_set_bi_processed_stripes(raid_bio, scnt);
 			conf->retry_read_aligned = raid_bio;
 			return handled;
 		}
 
+		set_bit(R5_ReadNoMerge, &sh->dev[dd_idx].flags);
 		handle_stripe(sh);
 		release_stripe(sh);
 		handled++;
 	}
-	spin_lock_irq(&conf->device_lock);
-	remaining = raid5_dec_bi_phys_segments(raid_bio);
-	spin_unlock_irq(&conf->device_lock);
+	remaining = raid5_dec_bi_active_stripes(raid_bio);
 	if (remaining == 0)
 		bio_endio(raid_bio, 0);
 	if (atomic_dec_and_test(&conf->active_aligned_reads))
@@ -4518,6 +4593,30 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 	return handled;
 }
 
+#define MAX_STRIPE_BATCH 8
+static int handle_active_stripes(struct r5conf *conf)
+{
+	struct stripe_head *batch[MAX_STRIPE_BATCH], *sh;
+	int i, batch_size = 0;
+
+	while (batch_size < MAX_STRIPE_BATCH &&
+			(sh = __get_priority_stripe(conf)) != NULL)
+		batch[batch_size++] = sh;
+
+	if (batch_size == 0)
+		return batch_size;
+	spin_unlock_irq(&conf->device_lock);
+
+	for (i = 0; i < batch_size; i++)
+		handle_stripe(batch[i]);
+
+	cond_resched();
+
+	spin_lock_irq(&conf->device_lock);
+	for (i = 0; i < batch_size; i++)
+		__release_stripe(conf, batch[i]);
+	return batch_size;
+}
 
 /*
  * This is our raid5 kernel thread.
@@ -4528,7 +4627,6 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
  */
 static void raid5d(struct mddev *mddev)
 {
-	struct stripe_head *sh;
 	struct r5conf *conf = mddev->private;
 	int handled;
 	struct blk_plug plug;
@@ -4542,8 +4640,9 @@ static void raid5d(struct mddev *mddev)
 	spin_lock_irq(&conf->device_lock);
 	while (1) {
 		struct bio *bio;
+		int batch_size;
 
-		if (atomic_read(&mddev->plug_cnt) == 0 &&
+		if (
 		    !list_empty(&conf->bitmap_list)) {
 			/* Now is a good time to flush some bitmap updates */
 			conf->seq_flush++;
@@ -4553,8 +4652,7 @@ static void raid5d(struct mddev *mddev)
 			conf->seq_write = conf->seq_flush;
 			activate_bit_delay(conf);
 		}
-		if (atomic_read(&mddev->plug_cnt) == 0)
-			raid5_activate_delayed(conf);
+		raid5_activate_delayed(conf);
 
 		while ((bio = remove_bio_from_retry(conf))) {
 			int ok;
@@ -4566,21 +4664,16 @@ static void raid5d(struct mddev *mddev)
 			handled++;
 		}
 
-		sh = __get_priority_stripe(conf);
-
-		if (!sh)
+		batch_size = handle_active_stripes(conf);
+		if (!batch_size)
 			break;
-		spin_unlock_irq(&conf->device_lock);
-		
-		handled++;
-		handle_stripe(sh);
-		release_stripe(sh);
-		cond_resched();
+		handled += batch_size;
 
-		if (mddev->flags & ~(1<<MD_CHANGE_PENDING))
+		if (mddev->flags & ~(1<<MD_CHANGE_PENDING)) {
+			spin_unlock_irq(&conf->device_lock);
 			md_check_recovery(mddev);
-
-		spin_lock_irq(&conf->device_lock);
+			spin_lock_irq(&conf->device_lock);
+		}
 	}
 	pr_debug("%d stripes handled\n", handled);
 
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index 2164021f3b5..a9fc24901ed 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -210,6 +210,7 @@ struct stripe_head {
 	int			disks;		/* disks in stripe */
 	enum check_states	check_state;
 	enum reconstruct_states reconstruct_state;
+	spinlock_t		stripe_lock;
 	/**
 	 * struct stripe_operations
 	 * @target - STRIPE_OP_COMPUTE_BLK target
@@ -273,6 +274,7 @@ enum r5dev_flags {
 	R5_Wantwrite,
 	R5_Overlap,	/* There is a pending overlapping request
 			 * on this block */
+	R5_ReadNoMerge, /* prevent bio from merging in block-layer */
 	R5_ReadError,	/* seen a read error here recently */
 	R5_ReWrite,	/* have tried to over-write the readerror */
 
@@ -319,6 +321,7 @@ enum {
 	STRIPE_BIOFILL_RUN,
 	STRIPE_COMPUTE_RUN,
 	STRIPE_OPS_REQ_PENDING,
+	STRIPE_ON_UNPLUG_LIST,
 };
 
 /*
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 9575db429df..d941581ab92 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -6,20 +6,82 @@ menuconfig MEDIA_SUPPORT
 	tristate "Multimedia support"
 	depends on HAS_IOMEM
 	help
-	  If you want to use Video for Linux, DVB for Linux, or DAB adapters,
+	  If you want to use Webcams, Video grabber devices and/or TV devices
 	  enable this option and other options below.
+	  Additional info and docs are available on the web at
+	  <http://linuxtv.org>
 
 if MEDIA_SUPPORT
 
 comment "Multimedia core support"
 
 #
+# Multimedia support - automatically enable V4L2 and DVB core
+#
+config MEDIA_CAMERA_SUPPORT
+	bool "Cameras/video grabbers support"
+	---help---
+	  Enable support for webcams and video grabbers.
+
+	  Say Y when you have a webcam or a video capture grabber board.
+
+config MEDIA_ANALOG_TV_SUPPORT
+	bool "Analog TV support"
+	---help---
+	  Enable analog TV support.
+
+	  Say Y when you have a TV board with analog support or with a
+	  hybrid analog/digital TV chipset.
+
+	  Note: There are several DVB cards that are based on chips that
+		support both analog and digital TV. Disabling this option
+		will disable support for them.
+
+config MEDIA_DIGITAL_TV_SUPPORT
+	bool "Digital TV support"
+	---help---
+	  Enable digital TV support.
+
+	  Say Y when you have a board with digital support or a board with
+	  hybrid digital TV and analog TV.
+
+config MEDIA_RADIO_SUPPORT
+	bool "AM/FM radio receivers/transmitters support"
+	---help---
+	  Enable AM/FM radio support.
+
+	  Additional info and docs are available on the web at
+	  <http://linuxtv.org>
+
+	  Say Y when you have a board with radio support.
+
+	  Note: There are several TV cards that are based on chips that
+		support radio reception. Disabling this option will
+		disable support for them.
+
+config MEDIA_RC_SUPPORT
+	bool "Remote Controller support"
+	depends on INPUT
+	---help---
+	  Enable support for Remote Controllers on Linux. This is
+	  needed in order to support several video capture adapters,
+	  standalone IR receivers/transmitters, and RF receivers.
+
+	  Enable this option if you have a video capture board even
+	  if you don't need IR, as otherwise, you may not be able to
+	  compile the driver for your adapter.
+
+	  Say Y when you have a TV or an IR device.
+
+#
 # Media controller
+#	Selectable only for webcam/grabbers, as other drivers don't use it
 #
 
 config MEDIA_CONTROLLER
 	bool "Media Controller API (EXPERIMENTAL)"
 	depends on EXPERIMENTAL
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  Enable the media controller API used to query media devices internal
 	  topology and configure it dynamically.
@@ -27,26 +89,15 @@ config MEDIA_CONTROLLER
 	  This API is mostly used by camera interfaces in embedded platforms.
 
 #
-# V4L core and enabled API's
+# Video4Linux support
+#	Only enables if one of the V4L2 types (ATV, webcam, radio) is selected
 #
 
 config VIDEO_DEV
-	tristate "Video For Linux"
-	---help---
-	  V4L core support for video capture and overlay devices, webcams and
-	  AM/FM radio cards.
-
-	  This kernel includes support for the new Video for Linux Two API,
-	  (V4L2).
-
-	  Additional info and docs are available on the web at
-	  <http://linuxtv.org>
-
-	  Documentation for V4L2 is also available on the web at
-	  <http://bytesex.org/v4l/>.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called videodev.
+	tristate
+	depends on MEDIA_SUPPORT
+	depends on MEDIA_CAMERA_SUPPORT || MEDIA_ANALOG_TV_SUPPORT || MEDIA_RADIO_SUPPORT
+	default y
 
 config VIDEO_V4L2_COMMON
 	tristate
@@ -64,25 +115,15 @@ config VIDEO_V4L2_SUBDEV_API
 
 #
 # DVB Core
+#	Only enables if one of DTV is selected
 #
 
 config DVB_CORE
-	tristate "DVB for Linux"
+	tristate
+	depends on MEDIA_SUPPORT
+	depends on MEDIA_DIGITAL_TV_SUPPORT
+	default y
 	select CRC32
-	help
-	  DVB core utility functions for device handling, software fallbacks etc.
-
-	  Enable this if you own a DVB/ATSC adapter and want to use it or if
-	  you compile Linux for a digital SetTopBox.
-
-	  Say Y when you have a DVB or an ATSC card and want to use it.
-
-	  API specs and user tools are available from <http://www.linuxtv.org/>.
-
-	  Please report problems regarding this support to the LinuxDVB
-	  mailing list.
-
-	  If unsure say N.
 
 config DVB_NET
 	bool "DVB Network Support"
@@ -97,12 +138,7 @@ config DVB_NET
 	  You may want to disable the network support on embedded devices. If
 	  unsure say Y.
 
-config VIDEO_MEDIA
-	tristate
-	default (DVB_CORE && (VIDEO_DEV = n)) || (VIDEO_DEV && (DVB_CORE = n)) || (DVB_CORE && VIDEO_DEV)
-
-comment "Multimedia drivers"
-
+comment "Media drivers"
 source "drivers/media/common/Kconfig"
 source "drivers/media/rc/Kconfig"
 
diff --git a/drivers/media/common/tuners/Kconfig b/drivers/media/common/tuners/Kconfig
index bbf4945149a..94c6ff7a5da 100644
--- a/drivers/media/common/tuners/Kconfig
+++ b/drivers/media/common/tuners/Kconfig
@@ -1,7 +1,8 @@
 config MEDIA_ATTACH
 	bool "Load and attach frontend and tuner driver modules as needed"
-	depends on VIDEO_MEDIA
+	depends on MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT || MEDIA_RADIO_SUPPORT
 	depends on MODULES
+	default y if !EXPERT
 	help
 	  Remove the static dependency of DVB card drivers on all
 	  frontend modules for all possible card variants. Instead,
@@ -19,15 +20,15 @@ config MEDIA_ATTACH
 
 config MEDIA_TUNER
 	tristate
-	default VIDEO_MEDIA && I2C
-	depends on VIDEO_MEDIA && I2C
+	depends on (MEDIA_ANALOG_TV_SUPPORT || MEDIA_RADIO_SUPPORT) && I2C
+	default y
 	select MEDIA_TUNER_XC2028 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_XC5000 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_XC4000 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_MT20XX if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_TDA8290 if !MEDIA_TUNER_CUSTOMISE
-	select MEDIA_TUNER_TEA5761 if !MEDIA_TUNER_CUSTOMISE && EXPERIMENTAL
-	select MEDIA_TUNER_TEA5767 if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_TEA5761 if !MEDIA_TUNER_CUSTOMISE && MEDIA_RADIO_SUPPORT && EXPERIMENTAL
+	select MEDIA_TUNER_TEA5767 if !MEDIA_TUNER_CUSTOMISE && MEDIA_RADIO_SUPPORT
 	select MEDIA_TUNER_SIMPLE if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_TDA9887 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_MC44S803 if !MEDIA_TUNER_CUSTOMISE
@@ -47,10 +48,11 @@ config MEDIA_TUNER_CUSTOMISE
 
 menu "Customize TV tuners"
 	visible if MEDIA_TUNER_CUSTOMISE
+	depends on MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT || MEDIA_RADIO_SUPPORT
 
 config MEDIA_TUNER_SIMPLE
 	tristate "Simple tuner support"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	select MEDIA_TUNER_TDA9887
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
@@ -58,7 +60,7 @@ config MEDIA_TUNER_SIMPLE
 
 config MEDIA_TUNER_TDA8290
 	tristate "TDA 8290/8295 + 8275(a)/18271 tuner combo"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	select MEDIA_TUNER_TDA827X
 	select MEDIA_TUNER_TDA18271
 	default m if MEDIA_TUNER_CUSTOMISE
@@ -67,21 +69,21 @@ config MEDIA_TUNER_TDA8290
 
 config MEDIA_TUNER_TDA827X
 	tristate "Philips TDA827X silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A DVB-T silicon tuner module. Say Y when you want to support this tuner.
 
 config MEDIA_TUNER_TDA18271
 	tristate "NXP TDA18271 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A silicon tuner module. Say Y when you want to support this tuner.
 
 config MEDIA_TUNER_TDA9887
 	tristate "TDA 9885/6/7 analog IF demodulator"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to include support for Philips TDA9885/6/7
@@ -89,7 +91,7 @@ config MEDIA_TUNER_TDA9887
 
 config MEDIA_TUNER_TEA5761
 	tristate "TEA 5761 radio tuner (EXPERIMENTAL)"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	depends on EXPERIMENTAL
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
@@ -97,63 +99,63 @@ config MEDIA_TUNER_TEA5761
 
 config MEDIA_TUNER_TEA5767
 	tristate "TEA 5767 radio tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to include support for the Philips TEA5767 radio tuner.
 
 config MEDIA_TUNER_MT20XX
 	tristate "Microtune 2032 / 2050 tuners"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to include support for the MT2032 / MT2050 tuner.
 
 config MEDIA_TUNER_MT2060
 	tristate "Microtune MT2060 silicon IF tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon IF tuner MT2060 from Microtune.
 
 config MEDIA_TUNER_MT2063
 	tristate "Microtune MT2063 silicon IF tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon IF tuner MT2063 from Microtune.
 
 config MEDIA_TUNER_MT2266
 	tristate "Microtune MT2266 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon baseband tuner MT2266 from Microtune.
 
 config MEDIA_TUNER_MT2131
 	tristate "Microtune MT2131 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon baseband tuner MT2131 from Microtune.
 
 config MEDIA_TUNER_QT1010
 	tristate "Quantek QT1010 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon tuner QT1010 from Quantek.
 
 config MEDIA_TUNER_XC2028
 	tristate "XCeive xc2028/xc3028 tuners"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to include support for the xc2028/xc3028 tuners.
 
 config MEDIA_TUNER_XC5000
 	tristate "Xceive XC5000 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon tuner XC5000 from Xceive.
@@ -162,7 +164,7 @@ config MEDIA_TUNER_XC5000
 
 config MEDIA_TUNER_XC4000
 	tristate "Xceive XC4000 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon tuner XC4000 from Xceive.
@@ -171,70 +173,70 @@ config MEDIA_TUNER_XC4000
 
 config MEDIA_TUNER_MXL5005S
 	tristate "MaxLinear MSL5005S silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon tuner MXL5005S from MaxLinear.
 
 config MEDIA_TUNER_MXL5007T
 	tristate "MaxLinear MxL5007T silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon tuner MxL5007T from MaxLinear.
 
 config MEDIA_TUNER_MC44S803
 	tristate "Freescale MC44S803 Low Power CMOS Broadband tuners"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to support the Freescale MC44S803 based tuners
 
 config MEDIA_TUNER_MAX2165
 	tristate "Maxim MAX2165 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  A driver for the silicon tuner MAX2165 from Maxim.
 
 config MEDIA_TUNER_TDA18218
 	tristate "NXP TDA18218 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  NXP TDA18218 silicon tuner driver.
 
 config MEDIA_TUNER_FC0011
 	tristate "Fitipower FC0011 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Fitipower FC0011 silicon tuner driver.
 
 config MEDIA_TUNER_FC0012
 	tristate "Fitipower FC0012 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Fitipower FC0012 silicon tuner driver.
 
 config MEDIA_TUNER_FC0013
 	tristate "Fitipower FC0013 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Fitipower FC0013 silicon tuner driver.
 
 config MEDIA_TUNER_TDA18212
 	tristate "NXP TDA18212 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  NXP TDA18212 silicon tuner driver.
 
 config MEDIA_TUNER_TUA9001
 	tristate "Infineon TUA 9001 silicon tuner"
-	depends on VIDEO_MEDIA && I2C
+	depends on MEDIA_SUPPORT && I2C
 	default m if MEDIA_TUNER_CUSTOMISE
 	help
 	  Infineon TUA 9001 silicon tuner driver.
diff --git a/drivers/media/common/tuners/tuner-xc2028.c b/drivers/media/common/tuners/tuner-xc2028.c
index b5ee3ebfcfc..ea0550eafe7 100644
--- a/drivers/media/common/tuners/tuner-xc2028.c
+++ b/drivers/media/common/tuners/tuner-xc2028.c
@@ -90,11 +90,22 @@ struct firmware_properties {
 	int 		scode_nr;
 };
 
+enum xc2028_state {
+	XC2028_NO_FIRMWARE = 0,
+	XC2028_WAITING_FIRMWARE,
+	XC2028_ACTIVE,
+	XC2028_SLEEP,
+	XC2028_NODEV,
+};
+
 struct xc2028_data {
 	struct list_head        hybrid_tuner_instance_list;
 	struct tuner_i2c_props  i2c_props;
 	__u32			frequency;
 
+	enum xc2028_state	state;
+	const char		*fname;
+
 	struct firmware_description *firm;
 	int			firm_size;
 	__u16			firm_version;
@@ -255,6 +266,21 @@ static  v4l2_std_id parse_audio_std_option(void)
 	return 0;
 }
 
+static int check_device_status(struct xc2028_data *priv)
+{
+	switch (priv->state) {
+	case XC2028_NO_FIRMWARE:
+	case XC2028_WAITING_FIRMWARE:
+		return -EAGAIN;
+	case XC2028_ACTIVE:
+	case XC2028_SLEEP:
+		return 0;
+	case XC2028_NODEV:
+		return -ENODEV;
+	}
+	return 0;
+}
+
 static void free_firmware(struct xc2028_data *priv)
 {
 	int i;
@@ -270,45 +296,28 @@ static void free_firmware(struct xc2028_data *priv)
 
 	priv->firm = NULL;
 	priv->firm_size = 0;
+	priv->state = XC2028_NO_FIRMWARE;
 
 	memset(&priv->cur_fw, 0, sizeof(priv->cur_fw));
 }
 
-static int load_all_firmwares(struct dvb_frontend *fe)
+static int load_all_firmwares(struct dvb_frontend *fe,
+			      const struct firmware *fw)
 {
 	struct xc2028_data    *priv = fe->tuner_priv;
-	const struct firmware *fw   = NULL;
 	const unsigned char   *p, *endp;
 	int                   rc = 0;
 	int		      n, n_array;
 	char		      name[33];
-	char		      *fname;
 
 	tuner_dbg("%s called\n", __func__);
 
-	if (!firmware_name[0])
-		fname = priv->ctrl.fname;
-	else
-		fname = firmware_name;
-
-	tuner_dbg("Reading firmware %s\n", fname);
-	rc = request_firmware(&fw, fname, priv->i2c_props.adap->dev.parent);
-	if (rc < 0) {
-		if (rc == -ENOENT)
-			tuner_err("Error: firmware %s not found.\n",
-				   fname);
-		else
-			tuner_err("Error %d while requesting firmware %s \n",
-				   rc, fname);
-
-		return rc;
-	}
 	p = fw->data;
 	endp = p + fw->size;
 
 	if (fw->size < sizeof(name) - 1 + 2 + 2) {
 		tuner_err("Error: firmware file %s has invalid size!\n",
-			  fname);
+			  priv->fname);
 		goto corrupt;
 	}
 
@@ -323,7 +332,7 @@ static int load_all_firmwares(struct dvb_frontend *fe)
 	p += 2;
 
 	tuner_info("Loading %d firmware images from %s, type: %s, ver %d.%d\n",
-		   n_array, fname, name,
+		   n_array, priv->fname, name,
 		   priv->firm_version >> 8, priv->firm_version & 0xff);
 
 	priv->firm = kcalloc(n_array, sizeof(*priv->firm), GFP_KERNEL);
@@ -417,9 +426,10 @@ err:
 	free_firmware(priv);
 
 done:
-	release_firmware(fw);
 	if (rc == 0)
 		tuner_dbg("Firmware files loaded.\n");
+	else
+		priv->state = XC2028_NODEV;
 
 	return rc;
 }
@@ -707,22 +717,15 @@ 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, retry_count = 0;
+	int			   rc, retry_count = 0;
 	u16			   version, hwmodel;
 	v4l2_std_id		   std0;
 
 	tuner_dbg("%s called\n", __func__);
 
-	if (!priv->firm) {
-		if (!priv->ctrl.fname) {
-			tuner_info("xc2028/3028 firmware name not set!\n");
-			return -EINVAL;
-		}
-
-		rc = load_all_firmwares(fe);
-		if (rc < 0)
-			return rc;
-	}
+	rc = check_device_status(priv);
+	if (rc < 0)
+		return rc;
 
 	if (priv->ctrl.mts && !(type & FM))
 		type |= MTS;
@@ -749,9 +752,13 @@ retry:
 		printk("scode_nr %d\n", new_fw.scode_nr);
 	}
 
-	/* No need to reload base firmware if it matches */
-	if (((BASE | new_fw.type) & BASE_TYPES) ==
-	    (priv->cur_fw.type & BASE_TYPES)) {
+	/*
+	 * No need to reload base firmware if it matches and if the tuner
+	 * is not at sleep mode
+	 */
+	if ((priv->state == XC2028_ACTIVE) &&
+	    (((BASE | new_fw.type) & BASE_TYPES) ==
+	    (priv->cur_fw.type & BASE_TYPES))) {
 		tuner_dbg("BASE firmware not changed.\n");
 		goto skip_base;
 	}
@@ -872,10 +879,13 @@ read_not_reliable:
 	 * 2. Tell whether BASE firmware was just changed the next time through.
 	 */
 	priv->cur_fw.type |= BASE;
+	priv->state = XC2028_ACTIVE;
 
 	return 0;
 
 fail:
+	priv->state = XC2028_SLEEP;
+
 	memset(&priv->cur_fw, 0, sizeof(priv->cur_fw));
 	if (retry_count < 8) {
 		msleep(50);
@@ -893,28 +903,39 @@ static int xc2028_signal(struct dvb_frontend *fe, u16 *strength)
 {
 	struct xc2028_data *priv = fe->tuner_priv;
 	u16                 frq_lock, signal = 0;
-	int                 rc;
+	int                 rc, i;
 
 	tuner_dbg("%s called\n", __func__);
 
+	rc = check_device_status(priv);
+	if (rc < 0)
+		return rc;
+
 	mutex_lock(&priv->lock);
 
 	/* Sync Lock Indicator */
-	rc = xc2028_get_reg(priv, XREG_LOCK, &frq_lock);
-	if (rc < 0)
-		goto ret;
+	for (i = 0; i < 3; i++) {
+		rc = xc2028_get_reg(priv, XREG_LOCK, &frq_lock);
+		if (rc < 0)
+			goto ret;
 
-	/* Frequency is locked */
-	if (frq_lock == 1)
-		signal = 1 << 11;
+		if (frq_lock)
+			break;
+		msleep(6);
+	}
+
+	/* Frequency didn't lock */
+	if (frq_lock == 2)
+		goto ret;
 
 	/* Get SNR of the video signal */
 	rc = xc2028_get_reg(priv, XREG_SNR, &signal);
 	if (rc < 0)
 		goto ret;
 
-	/* Use both frq_lock and signal to generate the result */
-	signal = signal || ((signal & 0x07) << 12);
+	/* Signal level is 3 bits only */
+
+	signal = ((1 << 12) - 1) | ((signal & 0x07) << 12);
 
 ret:
 	mutex_unlock(&priv->lock);
@@ -926,6 +947,49 @@ ret:
 	return rc;
 }
 
+static int xc2028_get_afc(struct dvb_frontend *fe, s32 *afc)
+{
+	struct xc2028_data *priv = fe->tuner_priv;
+	int i, rc;
+	u16 frq_lock = 0;
+	s16 afc_reg = 0;
+
+	rc = check_device_status(priv);
+	if (rc < 0)
+		return rc;
+
+	mutex_lock(&priv->lock);
+
+	/* Sync Lock Indicator */
+	for (i = 0; i < 3; i++) {
+		rc = xc2028_get_reg(priv, XREG_LOCK, &frq_lock);
+		if (rc < 0)
+			goto ret;
+
+		if (frq_lock)
+			break;
+		msleep(6);
+	}
+
+	/* Frequency didn't lock */
+	if (frq_lock == 2)
+		goto ret;
+
+	/* Get AFC */
+	rc = xc2028_get_reg(priv, XREG_FREQ_ERROR, &afc_reg);
+	if (rc < 0)
+		goto ret;
+
+	*afc = afc_reg * 15625; /* Hz */
+
+	tuner_dbg("AFC is %d Hz\n", *afc);
+
+ret:
+	mutex_unlock(&priv->lock);
+
+	return rc;
+}
+
 #define DIV 15625
 
 static int generic_set_freq(struct dvb_frontend *fe, u32 freq /* in HZ */,
@@ -1111,11 +1175,16 @@ static int xc2028_set_params(struct dvb_frontend *fe)
 	u32 delsys = c->delivery_system;
 	u32 bw = c->bandwidth_hz;
 	struct xc2028_data *priv = fe->tuner_priv;
-	unsigned int       type=0;
+	int rc;
+	unsigned int       type = 0;
 	u16                demod = 0;
 
 	tuner_dbg("%s called\n", __func__);
 
+	rc = check_device_status(priv);
+	if (rc < 0)
+		return rc;
+
 	switch (delsys) {
 	case SYS_DVBT:
 	case SYS_DVBT2:
@@ -1201,7 +1270,11 @@ static int xc2028_set_params(struct dvb_frontend *fe)
 static int xc2028_sleep(struct dvb_frontend *fe)
 {
 	struct xc2028_data *priv = fe->tuner_priv;
-	int rc = 0;
+	int rc;
+
+	rc = check_device_status(priv);
+	if (rc < 0)
+		return rc;
 
 	/* Avoid firmware reload on slow devices or if PM disabled */
 	if (no_poweroff || priv->ctrl.disable_power_mgmt)
@@ -1220,7 +1293,7 @@ static int xc2028_sleep(struct dvb_frontend *fe)
 	else
 		rc = send_seq(priv, {0x80, XREG_POWER_DOWN, 0x00, 0x00});
 
-	priv->cur_fw.type = 0;	/* need firmware reload */
+	priv->state = XC2028_SLEEP;
 
 	mutex_unlock(&priv->lock);
 
@@ -1237,8 +1310,9 @@ static int xc2028_dvb_release(struct dvb_frontend *fe)
 
 	/* only perform final cleanup if this is the last instance */
 	if (hybrid_tuner_report_instance_count(priv) == 1) {
-		kfree(priv->ctrl.fname);
 		free_firmware(priv);
+		kfree(priv->ctrl.fname);
+		priv->ctrl.fname = NULL;
 	}
 
 	if (priv)
@@ -1254,14 +1328,42 @@ static int xc2028_dvb_release(struct dvb_frontend *fe)
 static int xc2028_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct xc2028_data *priv = fe->tuner_priv;
+	int rc;
 
 	tuner_dbg("%s called\n", __func__);
 
+	rc = check_device_status(priv);
+	if (rc < 0)
+		return rc;
+
 	*frequency = priv->frequency;
 
 	return 0;
 }
 
+static void load_firmware_cb(const struct firmware *fw,
+			     void *context)
+{
+	struct dvb_frontend *fe = context;
+	struct xc2028_data *priv = fe->tuner_priv;
+	int rc;
+
+	tuner_dbg("request_firmware_nowait(): %s\n", fw ? "OK" : "error");
+	if (!fw) {
+		tuner_err("Could not load firmware %s.\n", priv->fname);
+		priv->state = XC2028_NODEV;
+		return;
+	}
+
+	rc = load_all_firmwares(fe, fw);
+
+	release_firmware(fw);
+
+	if (rc < 0)
+		return;
+	priv->state = XC2028_SLEEP;
+}
+
 static int xc2028_set_config(struct dvb_frontend *fe, void *priv_cfg)
 {
 	struct xc2028_data *priv = fe->tuner_priv;
@@ -1272,21 +1374,49 @@ static int xc2028_set_config(struct dvb_frontend *fe, void *priv_cfg)
 
 	mutex_lock(&priv->lock);
 
+	/*
+	 * Copy the config data.
+	 * For the firmware name, keep a local copy of the string,
+	 * in order to avoid troubles during device release.
+	 */
+	if (priv->ctrl.fname)
+		kfree(priv->ctrl.fname);
 	memcpy(&priv->ctrl, p, sizeof(priv->ctrl));
-	if (priv->ctrl.max_len < 9)
-		priv->ctrl.max_len = 13;
-
 	if (p->fname) {
-		if (priv->ctrl.fname && strcmp(p->fname, priv->ctrl.fname)) {
-			kfree(priv->ctrl.fname);
-			free_firmware(priv);
-		}
-
 		priv->ctrl.fname = kstrdup(p->fname, GFP_KERNEL);
 		if (priv->ctrl.fname == NULL)
 			rc = -ENOMEM;
 	}
 
+	/*
+	 * If firmware name changed, frees firmware. As free_firmware will
+	 * reset the status to NO_FIRMWARE, this forces a new request_firmware
+	 */
+	if (!firmware_name[0] && p->fname &&
+	    priv->fname && strcmp(p->fname, priv->fname))
+		free_firmware(priv);
+
+	if (priv->ctrl.max_len < 9)
+		priv->ctrl.max_len = 13;
+
+	if (priv->state == XC2028_NO_FIRMWARE) {
+		if (!firmware_name[0])
+			priv->fname = priv->ctrl.fname;
+		else
+			priv->fname = firmware_name;
+
+		rc = request_firmware_nowait(THIS_MODULE, 1,
+					     priv->fname,
+					     priv->i2c_props.adap->dev.parent,
+					     GFP_KERNEL,
+					     fe, load_firmware_cb);
+		if (rc < 0) {
+			tuner_err("Failed to request firmware %s\n",
+				  priv->fname);
+			priv->state = XC2028_NODEV;
+		}
+		priv->state = XC2028_WAITING_FIRMWARE;
+	}
 	mutex_unlock(&priv->lock);
 
 	return rc;
@@ -1305,6 +1435,7 @@ static const struct dvb_tuner_ops xc2028_dvb_tuner_ops = {
 	.release           = xc2028_dvb_release,
 	.get_frequency     = xc2028_get_frequency,
 	.get_rf_strength   = xc2028_signal,
+	.get_afc           = xc2028_get_afc,
 	.set_params        = xc2028_set_params,
 	.sleep             = xc2028_sleep,
 };
@@ -1375,3 +1506,5 @@ MODULE_DESCRIPTION("Xceive xc2028/xc3028 tuner driver");
 MODULE_AUTHOR("Michel Ludwig <michel.ludwig@gmail.com>");
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(XC2028_DEFAULT_FIRMWARE);
+MODULE_FIRMWARE(XC3028L_DEFAULT_FIRMWARE);
diff --git a/drivers/media/common/tuners/xc5000.c b/drivers/media/common/tuners/xc5000.c
index dcca42ca57b..362a8d7c973 100644
--- a/drivers/media/common/tuners/xc5000.c
+++ b/drivers/media/common/tuners/xc5000.c
@@ -210,13 +210,15 @@ struct xc5000_fw_cfg {
 	u16 size;
 };
 
+#define XC5000A_FIRMWARE "dvb-fe-xc5000-1.6.114.fw"
 static const struct xc5000_fw_cfg xc5000a_1_6_114 = {
-	.name = "dvb-fe-xc5000-1.6.114.fw",
+	.name = XC5000A_FIRMWARE,
 	.size = 12401,
 };
 
+#define XC5000C_FIRMWARE "dvb-fe-xc5000c-41.024.5.fw"
 static const struct xc5000_fw_cfg xc5000c_41_024_5 = {
-	.name = "dvb-fe-xc5000c-41.024.5.fw",
+	.name = XC5000C_FIRMWARE,
 	.size = 16497,
 };
 
@@ -717,6 +719,12 @@ static int xc5000_set_params(struct dvb_frontend *fe)
 		priv->freq_hz = freq - 1750000;
 		priv->video_standard = DTV6;
 		break;
+	case SYS_ISDBT:
+		/* All ISDB-T are currently for 6 MHz bw */
+		if (!bw)
+			bw = 6000000;
+		/* fall to OFDM handling */
+	case SYS_DMBTH:
 	case SYS_DVBT:
 	case SYS_DVBT2:
 		dprintk(1, "%s() OFDM\n", __func__);
@@ -1253,3 +1261,5 @@ EXPORT_SYMBOL(xc5000_attach);
 MODULE_AUTHOR("Steven Toth");
 MODULE_DESCRIPTION("Xceive xc5000 silicon tuner driver");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(XC5000A_FIRMWARE);
+MODULE_FIRMWARE(XC5000C_FIRMWARE);
diff --git a/drivers/media/dvb/ddbridge/ddbridge-core.c b/drivers/media/dvb/ddbridge/ddbridge-core.c
index 131b938e9e8..ebf3f05839d 100644
--- a/drivers/media/dvb/ddbridge/ddbridge-core.c
+++ b/drivers/media/dvb/ddbridge/ddbridge-core.c
@@ -578,6 +578,7 @@ static int demod_attach_drxk(struct ddb_input *input)
 
 	memset(&config, 0, sizeof(config));
 	config.microcode_name = "drxk_a3.mc";
+	config.qam_demod_parameter_count = 4;
 	config.adr = 0x29 + (input->nr & 1);
 
 	fe = input->fe = dvb_attach(drxk_attach, &config, i2c);
diff --git a/drivers/media/dvb/dvb-core/dvb_frontend.h b/drivers/media/dvb/dvb-core/dvb_frontend.h
index e929d5697b8..7c64c09103a 100644
--- a/drivers/media/dvb/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb/dvb-core/dvb_frontend.h
@@ -220,6 +220,7 @@ struct dvb_tuner_ops {
 #define TUNER_STATUS_STEREO 2
 	int (*get_status)(struct dvb_frontend *fe, u32 *status);
 	int (*get_rf_strength)(struct dvb_frontend *fe, u16 *strength);
+	int (*get_afc)(struct dvb_frontend *fe, s32 *afc);
 
 	/** These are provided separately from set_params in order to facilitate silicon
 	 * tuners which require sophisticated tuning loops, controlling each parameter separately. */
diff --git a/drivers/media/dvb/dvb-usb/Kconfig b/drivers/media/dvb/dvb-usb/Kconfig
index a26949336b3..c2161565023 100644
--- a/drivers/media/dvb/dvb-usb/Kconfig
+++ b/drivers/media/dvb/dvb-usb/Kconfig
@@ -418,9 +418,12 @@ config DVB_USB_RTL28XXU
 	tristate "Realtek RTL28xxU DVB USB support"
 	depends on DVB_USB && EXPERIMENTAL
 	select DVB_RTL2830
+	select DVB_RTL2832
 	select MEDIA_TUNER_QT1010 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_MT2060 if !MEDIA_TUNER_CUSTOMISE
 	select MEDIA_TUNER_MXL5005S if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_FC0012 if !MEDIA_TUNER_CUSTOMISE
+	select MEDIA_TUNER_FC0013 if !MEDIA_TUNER_CUSTOMISE
 	help
 	  Say Y here to support the Realtek RTL28xxU DVB USB receiver.
 
diff --git a/drivers/media/dvb/dvb-usb/az6007.c b/drivers/media/dvb/dvb-usb/az6007.c
index 4008b9c50fb..86861e6f86d 100644
--- a/drivers/media/dvb/dvb-usb/az6007.c
+++ b/drivers/media/dvb/dvb-usb/az6007.c
@@ -590,12 +590,10 @@ static int az6007_read_mac_addr(struct dvb_usb_device *d, u8 mac[6])
 	int ret;
 
 	ret = az6007_read(d, AZ6007_READ_DATA, 6, 0, st->data, 6);
-	memcpy(mac, st->data, sizeof(mac));
+	memcpy(mac, st->data, 6);
 
 	if (ret > 0)
-		deb_info("%s: mac is %02x:%02x:%02x:%02x:%02x:%02x\n",
-			 __func__, mac[0], mac[1], mac[2],
-			 mac[3], mac[4], mac[5]);
+		deb_info("%s: mac is %pM\n", __func__, mac);
 
 	return ret;
 }
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
index 7a6160bf54b..26c44818a5a 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
@@ -100,6 +100,7 @@
 #define USB_PID_CONCEPTRONIC_CTVDIGRCU			0xe397
 #define USB_PID_CONEXANT_D680_DMB			0x86d6
 #define USB_PID_CREATIX_CTX1921				0x1921
+#define USB_PID_DELOCK_USB2_DVBT			0xb803
 #define USB_PID_DIBCOM_HOOK_DEFAULT			0x0064
 #define USB_PID_DIBCOM_HOOK_DEFAULT_REENUM		0x0065
 #define USB_PID_DIBCOM_MOD3000_COLD			0x0bb8
@@ -160,6 +161,7 @@
 #define USB_PID_TERRATEC_CINERGY_T_STICK		0x0093
 #define USB_PID_TERRATEC_CINERGY_T_STICK_RC		0x0097
 #define USB_PID_TERRATEC_CINERGY_T_STICK_DUAL_RC	0x0099
+#define USB_PID_TERRATEC_CINERGY_T_STICK_BLACK_REV1	0x00a9
 #define USB_PID_TWINHAN_VP7041_COLD			0x3201
 #define USB_PID_TWINHAN_VP7041_WARM			0x3202
 #define USB_PID_TWINHAN_VP7020_COLD			0x3203
@@ -245,6 +247,7 @@
 #define USB_PID_TERRATEC_H7_2				0x10a3
 #define USB_PID_TERRATEC_T3				0x10a0
 #define USB_PID_TERRATEC_T5				0x10a1
+#define USB_PID_NOXON_DAB_STICK				0x00b3
 #define USB_PID_PINNACLE_EXPRESSCARD_320CX		0x022e
 #define USB_PID_PINNACLE_PCTV2000E			0x022c
 #define USB_PID_PINNACLE_PCTV_DVB_T_FLASH		0x0228
diff --git a/drivers/media/dvb/dvb-usb/rtl28xxu.c b/drivers/media/dvb/dvb-usb/rtl28xxu.c
index 41e1f5537f4..6bd0bd79243 100644
--- a/drivers/media/dvb/dvb-usb/rtl28xxu.c
+++ b/drivers/media/dvb/dvb-usb/rtl28xxu.c
@@ -3,6 +3,7 @@
  *
  * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
  * Copyright (C) 2011 Antti Palosaari <crope@iki.fi>
+ * Copyright (C) 2012 Thomas Mair <thomas.mair86@googlemail.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
@@ -22,10 +23,13 @@
 #include "rtl28xxu.h"
 
 #include "rtl2830.h"
+#include "rtl2832.h"
 
 #include "qt1010.h"
 #include "mt2060.h"
 #include "mxl5005s.h"
+#include "fc0012.h"
+#include "fc0013.h"
 
 /* debug */
 static int dvb_usb_rtl28xxu_debug;
@@ -80,7 +84,7 @@ err:
 	return ret;
 }
 
-static int rtl2831_wr_regs(struct dvb_usb_device *d, u16 reg, u8 *val, int len)
+static int rtl28xx_wr_regs(struct dvb_usb_device *d, u16 reg, u8 *val, int len)
 {
 	struct rtl28xxu_req req;
 
@@ -116,12 +120,12 @@ static int rtl2831_rd_regs(struct dvb_usb_device *d, u16 reg, u8 *val, int len)
 	return rtl28xxu_ctrl_msg(d, &req);
 }
 
-static int rtl2831_wr_reg(struct dvb_usb_device *d, u16 reg, u8 val)
+static int rtl28xx_wr_reg(struct dvb_usb_device *d, u16 reg, u8 val)
 {
-	return rtl2831_wr_regs(d, reg, &val, 1);
+	return rtl28xx_wr_regs(d, reg, &val, 1);
 }
 
-static int rtl2831_rd_reg(struct dvb_usb_device *d, u16 reg, u8 *val)
+static int rtl28xx_rd_reg(struct dvb_usb_device *d, u16 reg, u8 *val)
 {
 	return rtl2831_rd_regs(d, reg, val, 1);
 }
@@ -308,12 +312,12 @@ static int rtl2831u_frontend_attach(struct dvb_usb_adapter *adap)
 	 */
 
 	/* GPIO direction */
-	ret = rtl2831_wr_reg(adap->dev, SYS_GPIO_DIR, 0x0a);
+	ret = rtl28xx_wr_reg(adap->dev, SYS_GPIO_DIR, 0x0a);
 	if (ret)
 		goto err;
 
 	/* enable as output GPIO0, GPIO2, GPIO4 */
-	ret = rtl2831_wr_reg(adap->dev, SYS_GPIO_OUT_EN, 0x15);
+	ret = rtl28xx_wr_reg(adap->dev, SYS_GPIO_OUT_EN, 0x15);
 	if (ret)
 		goto err;
 
@@ -381,34 +385,159 @@ err:
 	return ret;
 }
 
+static struct rtl2832_config rtl28xxu_rtl2832_fc0012_config = {
+	.i2c_addr = 0x10, /* 0x20 */
+	.xtal = 28800000,
+	.if_dvbt = 0,
+	.tuner = TUNER_RTL2832_FC0012
+};
+
+static struct rtl2832_config rtl28xxu_rtl2832_fc0013_config = {
+	.i2c_addr = 0x10, /* 0x20 */
+	.xtal = 28800000,
+	.if_dvbt = 0,
+	.tuner = TUNER_RTL2832_FC0013
+};
+
+static int rtl2832u_fc0012_tuner_callback(struct dvb_usb_device *d,
+		int cmd, int arg)
+{
+	int ret;
+	u8 val;
+
+	deb_info("%s cmd=%d arg=%d\n", __func__, cmd, arg);
+	switch (cmd) {
+	case FC_FE_CALLBACK_VHF_ENABLE:
+		/* set output values */
+		ret = rtl28xx_rd_reg(d, SYS_GPIO_OUT_VAL, &val);
+		if (ret)
+			goto err;
+
+		if (arg)
+			val &= 0xbf; /* set GPIO6 low */
+		else
+			val |= 0x40; /* set GPIO6 high */
+
+
+		ret = rtl28xx_wr_reg(d, SYS_GPIO_OUT_VAL, val);
+		if (ret)
+			goto err;
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+	return 0;
+
+err:
+	err("%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+
+static int rtl2832u_fc0013_tuner_callback(struct dvb_usb_device *d,
+		int cmd, int arg)
+{
+	/* TODO implement*/
+	return 0;
+}
+
+static int rtl2832u_tuner_callback(struct dvb_usb_device *d, int cmd, int arg)
+{
+	struct rtl28xxu_priv *priv = d->priv;
+
+	switch (priv->tuner) {
+	case TUNER_RTL2832_FC0012:
+		return rtl2832u_fc0012_tuner_callback(d, cmd, arg);
+
+	case TUNER_RTL2832_FC0013:
+		return rtl2832u_fc0013_tuner_callback(d, cmd, arg);
+	default:
+		break;
+	}
+
+	return -ENODEV;
+}
+
+static int rtl2832u_frontend_callback(void *adapter_priv, int component,
+				    int cmd, int arg)
+{
+	struct i2c_adapter *adap = adapter_priv;
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+
+	switch (component) {
+	case DVB_FRONTEND_COMPONENT_TUNER:
+		return rtl2832u_tuner_callback(d, cmd, arg);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+
+
+
 static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 {
 	int ret;
 	struct rtl28xxu_priv *priv = adap->dev->priv;
-	u8 buf[1];
+	struct rtl2832_config *rtl2832_config;
+
+	u8 buf[2], val;
 	/* open RTL2832U/RTL2832 I2C gate */
 	struct rtl28xxu_req req_gate_open = {0x0120, 0x0011, 0x0001, "\x18"};
 	/* close RTL2832U/RTL2832 I2C gate */
 	struct rtl28xxu_req req_gate_close = {0x0120, 0x0011, 0x0001, "\x10"};
+	/* for FC0012 tuner probe */
+	struct rtl28xxu_req req_fc0012 = {0x00c6, CMD_I2C_RD, 1, buf};
+	/* for FC0013 tuner probe */
+	struct rtl28xxu_req req_fc0013 = {0x00c6, CMD_I2C_RD, 1, buf};
+	/* for MT2266 tuner probe */
+	struct rtl28xxu_req req_mt2266 = {0x00c0, CMD_I2C_RD, 1, buf};
 	/* for FC2580 tuner probe */
 	struct rtl28xxu_req req_fc2580 = {0x01ac, CMD_I2C_RD, 1, buf};
+	/* for MT2063 tuner probe */
+	struct rtl28xxu_req req_mt2063 = {0x00c0, CMD_I2C_RD, 1, buf};
+	/* for MAX3543 tuner probe */
+	struct rtl28xxu_req req_max3543 = {0x00c0, CMD_I2C_RD, 1, buf};
+	/* for TUA9001 tuner probe */
+	struct rtl28xxu_req req_tua9001 = {0x7ec0, CMD_I2C_RD, 2, buf};
+	/* for MXL5007T tuner probe */
+	struct rtl28xxu_req req_mxl5007t = {0xd9c0, CMD_I2C_RD, 1, buf};
+	/* for E4000 tuner probe */
+	struct rtl28xxu_req req_e4000 = {0x02c8, CMD_I2C_RD, 1, buf};
+	/* for TDA18272 tuner probe */
+	struct rtl28xxu_req req_tda18272 = {0x00c0, CMD_I2C_RD, 2, buf};
 
 	deb_info("%s:\n", __func__);
 
-	/* GPIO direction */
-	ret = rtl2831_wr_reg(adap->dev, SYS_GPIO_DIR, 0x0a);
+
+	ret = rtl28xx_rd_reg(adap->dev, SYS_GPIO_DIR, &val);
 	if (ret)
 		goto err;
 
-	/* enable as output GPIO0, GPIO2, GPIO4 */
-	ret = rtl2831_wr_reg(adap->dev, SYS_GPIO_OUT_EN, 0x15);
+	val &= 0xbf;
+
+	ret = rtl28xx_wr_reg(adap->dev, SYS_GPIO_DIR, val);
 	if (ret)
 		goto err;
 
-	ret = rtl2831_wr_reg(adap->dev, SYS_DEMOD_CTL, 0xe8);
+
+	/* enable as output GPIO3 and GPIO6*/
+	ret = rtl28xx_rd_reg(adap->dev, SYS_GPIO_OUT_EN, &val);
 	if (ret)
 		goto err;
 
+	val |= 0x48;
+
+	ret = rtl28xx_wr_reg(adap->dev, SYS_GPIO_OUT_EN, val);
+	if (ret)
+		goto err;
+
+
+
 	/*
 	 * Probe used tuner. We need to know used tuner before demod attach
 	 * since there is some demod params needed to set according to tuner.
@@ -419,25 +548,108 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 	if (ret)
 		goto err;
 
+	priv->tuner = TUNER_NONE;
+
+	/* check FC0012 ID register; reg=00 val=a1 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_fc0012);
+	if (ret == 0 && buf[0] == 0xa1) {
+		priv->tuner = TUNER_RTL2832_FC0012;
+		rtl2832_config = &rtl28xxu_rtl2832_fc0012_config;
+		info("%s: FC0012 tuner found", __func__);
+		goto found;
+	}
+
+	/* check FC0013 ID register; reg=00 val=a3 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_fc0013);
+	if (ret == 0 && buf[0] == 0xa3) {
+		priv->tuner = TUNER_RTL2832_FC0013;
+		rtl2832_config = &rtl28xxu_rtl2832_fc0013_config;
+		info("%s: FC0013 tuner found", __func__);
+		goto found;
+	}
+
+	/* check MT2266 ID register; reg=00 val=85 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_mt2266);
+	if (ret == 0 && buf[0] == 0x85) {
+		priv->tuner = TUNER_RTL2832_MT2266;
+		/* TODO implement tuner */
+		info("%s: MT2266 tuner found", __func__);
+		goto unsupported;
+	}
+
 	/* check FC2580 ID register; reg=01 val=56 */
 	ret = rtl28xxu_ctrl_msg(adap->dev, &req_fc2580);
 	if (ret == 0 && buf[0] == 0x56) {
 		priv->tuner = TUNER_RTL2832_FC2580;
-		deb_info("%s: FC2580\n", __func__);
-		goto found;
-	} else {
-		deb_info("%s: FC2580 probe failed=%d - %02x\n",
-			__func__, ret, buf[0]);
+		/* TODO implement tuner */
+		info("%s: FC2580 tuner found", __func__);
+		goto unsupported;
 	}
 
+	/* check MT2063 ID register; reg=00 val=9e || 9c */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_mt2063);
+	if (ret == 0 && (buf[0] == 0x9e || buf[0] == 0x9c)) {
+		priv->tuner = TUNER_RTL2832_MT2063;
+		/* TODO implement tuner */
+		info("%s: MT2063 tuner found", __func__);
+		goto unsupported;
+	}
+
+	/* check MAX3543 ID register; reg=00 val=38 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_max3543);
+	if (ret == 0 && buf[0] == 0x38) {
+		priv->tuner = TUNER_RTL2832_MAX3543;
+		/* TODO implement tuner */
+		info("%s: MAX3534 tuner found", __func__);
+		goto unsupported;
+	}
+
+	/* check TUA9001 ID register; reg=7e val=2328 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_tua9001);
+	if (ret == 0 && buf[0] == 0x23 && buf[1] == 0x28) {
+		priv->tuner = TUNER_RTL2832_TUA9001;
+		/* TODO implement tuner */
+		info("%s: TUA9001 tuner found", __func__);
+		goto unsupported;
+	}
+
+	/* check MXL5007R ID register; reg=d9 val=14 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_mxl5007t);
+	if (ret == 0 && buf[0] == 0x14) {
+		priv->tuner = TUNER_RTL2832_MXL5007T;
+		/* TODO implement tuner */
+		info("%s: MXL5007T tuner found", __func__);
+		goto unsupported;
+	}
+
+	/* check E4000 ID register; reg=02 val=40 */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_e4000);
+	if (ret == 0 && buf[0] == 0x40) {
+		priv->tuner = TUNER_RTL2832_E4000;
+		/* TODO implement tuner */
+		info("%s: E4000 tuner found", __func__);
+		goto unsupported;
+	}
+
+	/* check TDA18272 ID register; reg=00 val=c760  */
+	ret = rtl28xxu_ctrl_msg(adap->dev, &req_tda18272);
+	if (ret == 0 && (buf[0] == 0xc7 || buf[1] == 0x60)) {
+		priv->tuner = TUNER_RTL2832_TDA18272;
+		/* TODO implement tuner */
+		info("%s: TDA18272 tuner found", __func__);
+		goto unsupported;
+	}
+
+unsupported:
 	/* close demod I2C gate */
 	ret = rtl28xxu_ctrl_msg(adap->dev, &req_gate_close);
 	if (ret)
 		goto err;
 
 	/* tuner not found */
+	deb_info("No compatible tuner found");
 	ret = -ENODEV;
-	goto err;
+	return ret;
 
 found:
 	/* close demod I2C gate */
@@ -446,9 +658,18 @@ found:
 		goto err;
 
 	/* attach demodulator */
-	/* TODO: */
+	adap->fe_adap[0].fe = dvb_attach(rtl2832_attach, rtl2832_config,
+		&adap->dev->i2c_adap);
+		if (adap->fe_adap[0].fe == NULL) {
+			ret = -ENODEV;
+			goto err;
+		}
+
+	/* set fe callbacks */
+	adap->fe_adap[0].fe->callback = rtl2832u_frontend_callback;
 
 	return ret;
+
 err:
 	deb_info("%s: failed=%d\n", __func__, ret);
 	return ret;
@@ -531,10 +752,24 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 	deb_info("%s:\n", __func__);
 
 	switch (priv->tuner) {
-	case TUNER_RTL2832_FC2580:
-		/* TODO: */
-		fe = NULL;
+	case TUNER_RTL2832_FC0012:
+		fe = dvb_attach(fc0012_attach, adap->fe_adap[0].fe,
+			&adap->dev->i2c_adap, 0xc6>>1, 0, FC_XTAL_28_8_MHZ);
+
+		/* since fc0012 includs reading the signal strength delegate
+		 * that to the tuner driver */
+		adap->fe_adap[0].fe->ops.read_signal_strength = adap->fe_adap[0].
+				fe->ops.tuner_ops.get_rf_strength;
+		return 0;
 		break;
+	case TUNER_RTL2832_FC0013:
+		fe = dvb_attach(fc0013_attach, adap->fe_adap[0].fe,
+			&adap->dev->i2c_adap, 0xc6>>1, 0, FC_XTAL_28_8_MHZ);
+
+		/* fc0013 also supports signal strength reading */
+		adap->fe_adap[0].fe->ops.read_signal_strength = adap->fe_adap[0]
+			.fe->ops.tuner_ops.get_rf_strength;
+		return 0;
 	default:
 		fe = NULL;
 		err("unknown tuner=%d", priv->tuner);
@@ -551,14 +786,14 @@ err:
 	return ret;
 }
 
-static int rtl28xxu_streaming_ctrl(struct dvb_usb_adapter *adap , int onoff)
+static int rtl2831u_streaming_ctrl(struct dvb_usb_adapter *adap , int onoff)
 {
 	int ret;
 	u8 buf[2], gpio;
 
 	deb_info("%s: onoff=%d\n", __func__, onoff);
 
-	ret = rtl2831_rd_reg(adap->dev, SYS_GPIO_OUT_VAL, &gpio);
+	ret = rtl28xx_rd_reg(adap->dev, SYS_GPIO_OUT_VAL, &gpio);
 	if (ret)
 		goto err;
 
@@ -572,11 +807,37 @@ static int rtl28xxu_streaming_ctrl(struct dvb_usb_adapter *adap , int onoff)
 		gpio &= (~0x04); /* LED off */
 	}
 
-	ret = rtl2831_wr_reg(adap->dev, SYS_GPIO_OUT_VAL, gpio);
+	ret = rtl28xx_wr_reg(adap->dev, SYS_GPIO_OUT_VAL, gpio);
 	if (ret)
 		goto err;
 
-	ret = rtl2831_wr_regs(adap->dev, USB_EPA_CTL, buf, 2);
+	ret = rtl28xx_wr_regs(adap->dev, USB_EPA_CTL, buf, 2);
+	if (ret)
+		goto err;
+
+	return ret;
+err:
+	deb_info("%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static int rtl2832u_streaming_ctrl(struct dvb_usb_adapter *adap , int onoff)
+{
+	int ret;
+	u8 buf[2];
+
+	deb_info("%s: onoff=%d\n", __func__, onoff);
+
+
+	if (onoff) {
+		buf[0] = 0x00;
+		buf[1] = 0x00;
+	} else {
+		buf[0] = 0x10; /* stall EPA */
+		buf[1] = 0x02; /* reset EPA */
+	}
+
+	ret = rtl28xx_wr_regs(adap->dev, USB_EPA_CTL, buf, 2);
 	if (ret)
 		goto err;
 
@@ -586,7 +847,7 @@ err:
 	return ret;
 }
 
-static int rtl28xxu_power_ctrl(struct dvb_usb_device *d, int onoff)
+static int rtl2831u_power_ctrl(struct dvb_usb_device *d, int onoff)
 {
 	int ret;
 	u8 gpio, sys0;
@@ -594,12 +855,12 @@ static int rtl28xxu_power_ctrl(struct dvb_usb_device *d, int onoff)
 	deb_info("%s: onoff=%d\n", __func__, onoff);
 
 	/* demod adc */
-	ret = rtl2831_rd_reg(d, SYS_SYS0, &sys0);
+	ret = rtl28xx_rd_reg(d, SYS_SYS0, &sys0);
 	if (ret)
 		goto err;
 
 	/* tuner power, read GPIOs */
-	ret = rtl2831_rd_reg(d, SYS_GPIO_OUT_VAL, &gpio);
+	ret = rtl28xx_rd_reg(d, SYS_GPIO_OUT_VAL, &gpio);
 	if (ret)
 		goto err;
 
@@ -619,12 +880,12 @@ static int rtl28xxu_power_ctrl(struct dvb_usb_device *d, int onoff)
 	deb_info("%s: WR SYS0=%02x GPIO_OUT_VAL=%02x\n", __func__, sys0, gpio);
 
 	/* demod adc */
-	ret = rtl2831_wr_reg(d, SYS_SYS0, sys0);
+	ret = rtl28xx_wr_reg(d, SYS_SYS0, sys0);
 	if (ret)
 		goto err;
 
 	/* tuner power, write GPIOs */
-	ret = rtl2831_wr_reg(d, SYS_GPIO_OUT_VAL, gpio);
+	ret = rtl28xx_wr_reg(d, SYS_GPIO_OUT_VAL, gpio);
 	if (ret)
 		goto err;
 
@@ -634,6 +895,128 @@ err:
 	return ret;
 }
 
+static int rtl2832u_power_ctrl(struct dvb_usb_device *d, int onoff)
+{
+	int ret;
+	u8 val;
+
+	deb_info("%s: onoff=%d\n", __func__, onoff);
+
+	if (onoff) {
+		/* set output values */
+		ret = rtl28xx_rd_reg(d, SYS_GPIO_OUT_VAL, &val);
+		if (ret)
+			goto err;
+
+		val |= 0x08;
+		val &= 0xef;
+
+		ret = rtl28xx_wr_reg(d, SYS_GPIO_OUT_VAL, val);
+		if (ret)
+			goto err;
+
+		/* demod_ctl_1 */
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL1, &val);
+		if (ret)
+			goto err;
+
+		val &= 0xef;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL1, val);
+		if (ret)
+			goto err;
+
+		/* demod control */
+		/* PLL enable */
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL, &val);
+		if (ret)
+			goto err;
+
+		/* bit 7 to 1 */
+		val |= 0x80;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL, val);
+		if (ret)
+			goto err;
+
+		/* demod HW reset */
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL, &val);
+		if (ret)
+			goto err;
+		/* bit 5 to 0 */
+		val &= 0xdf;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL, val);
+		if (ret)
+			goto err;
+
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL, &val);
+		if (ret)
+			goto err;
+
+		val |= 0x20;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL, val);
+		if (ret)
+			goto err;
+
+		mdelay(5);
+
+		/*enable ADC_Q and ADC_I */
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL, &val);
+		if (ret)
+			goto err;
+
+		val |= 0x48;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL, val);
+		if (ret)
+			goto err;
+
+
+	} else {
+		/* demod_ctl_1 */
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL1, &val);
+		if (ret)
+			goto err;
+
+		val |= 0x0c;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL1, val);
+		if (ret)
+			goto err;
+
+		/* set output values */
+		ret = rtl28xx_rd_reg(d, SYS_GPIO_OUT_VAL, &val);
+		if (ret)
+				goto err;
+
+		val |= 0x10;
+
+		ret = rtl28xx_wr_reg(d, SYS_GPIO_OUT_VAL, val);
+		if (ret)
+			goto err;
+
+		/* demod control */
+		ret = rtl28xx_rd_reg(d, SYS_DEMOD_CTL, &val);
+		if (ret)
+			goto err;
+
+		val &= 0x37;
+
+		ret = rtl28xx_wr_reg(d, SYS_DEMOD_CTL, val);
+		if (ret)
+			goto err;
+
+	}
+
+	return ret;
+err:
+	deb_info("%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+
 static int rtl2831u_rc_query(struct dvb_usb_device *d)
 {
 	int ret, i;
@@ -660,7 +1043,7 @@ static int rtl2831u_rc_query(struct dvb_usb_device *d)
 	/* init remote controller */
 	if (!priv->rc_active) {
 		for (i = 0; i < ARRAY_SIZE(rc_nec_tab); i++) {
-			ret = rtl2831_wr_reg(d, rc_nec_tab[i].reg,
+			ret = rtl28xx_wr_reg(d, rc_nec_tab[i].reg,
 					rc_nec_tab[i].val);
 			if (ret)
 				goto err;
@@ -690,12 +1073,12 @@ static int rtl2831u_rc_query(struct dvb_usb_device *d)
 
 		rc_keydown(d->rc_dev, rc_code, 0);
 
-		ret = rtl2831_wr_reg(d, SYS_IRRC_SR, 1);
+		ret = rtl28xx_wr_reg(d, SYS_IRRC_SR, 1);
 		if (ret)
 			goto err;
 
 		/* repeated intentionally to avoid extra keypress */
-		ret = rtl2831_wr_reg(d, SYS_IRRC_SR, 1);
+		ret = rtl28xx_wr_reg(d, SYS_IRRC_SR, 1);
 		if (ret)
 			goto err;
 	}
@@ -732,7 +1115,7 @@ static int rtl2832u_rc_query(struct dvb_usb_device *d)
 	/* init remote controller */
 	if (!priv->rc_active) {
 		for (i = 0; i < ARRAY_SIZE(rc_nec_tab); i++) {
-			ret = rtl2831_wr_reg(d, rc_nec_tab[i].reg,
+			ret = rtl28xx_wr_reg(d, rc_nec_tab[i].reg,
 					rc_nec_tab[i].val);
 			if (ret)
 				goto err;
@@ -740,14 +1123,14 @@ static int rtl2832u_rc_query(struct dvb_usb_device *d)
 		priv->rc_active = true;
 	}
 
-	ret = rtl2831_rd_reg(d, IR_RX_IF, &buf[0]);
+	ret = rtl28xx_rd_reg(d, IR_RX_IF, &buf[0]);
 	if (ret)
 		goto err;
 
 	if (buf[0] != 0x83)
 		goto exit;
 
-	ret = rtl2831_rd_reg(d, IR_RX_BC, &buf[0]);
+	ret = rtl28xx_rd_reg(d, IR_RX_BC, &buf[0]);
 	if (ret)
 		goto err;
 
@@ -756,9 +1139,9 @@ static int rtl2832u_rc_query(struct dvb_usb_device *d)
 
 	/* TODO: pass raw IR to Kernel IR decoder */
 
-	ret = rtl2831_wr_reg(d, IR_RX_IF, 0x03);
-	ret = rtl2831_wr_reg(d, IR_RX_BUF_CTRL, 0x80);
-	ret = rtl2831_wr_reg(d, IR_RX_CTRL, 0x80);
+	ret = rtl28xx_wr_reg(d, IR_RX_IF, 0x03);
+	ret = rtl28xx_wr_reg(d, IR_RX_BUF_CTRL, 0x80);
+	ret = rtl28xx_wr_reg(d, IR_RX_CTRL, 0x80);
 
 exit:
 	return ret;
@@ -771,6 +1154,9 @@ enum rtl28xxu_usb_table_entry {
 	RTL2831U_0BDA_2831,
 	RTL2831U_14AA_0160,
 	RTL2831U_14AA_0161,
+	RTL2832U_0CCD_00A9,
+	RTL2832U_1F4D_B803,
+	RTL2832U_0CCD_00B3,
 };
 
 static struct usb_device_id rtl28xxu_table[] = {
@@ -783,6 +1169,12 @@ static struct usb_device_id rtl28xxu_table[] = {
 		USB_DEVICE(USB_VID_WIDEVIEW, USB_PID_FREECOM_DVBT_2)},
 
 	/* RTL2832U */
+	[RTL2832U_0CCD_00A9] = {
+		USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_T_STICK_BLACK_REV1)},
+	[RTL2832U_1F4D_B803] = {
+		USB_DEVICE(USB_VID_GTEK, USB_PID_DELOCK_USB2_DVBT)},
+	[RTL2832U_0CCD_00B3] = {
+		USB_DEVICE(USB_VID_TERRATEC, USB_PID_NOXON_DAB_STICK)},
 	{} /* terminating entry */
 };
 
@@ -805,7 +1197,7 @@ static struct dvb_usb_device_properties rtl28xxu_properties[] = {
 					{
 						.frontend_attach = rtl2831u_frontend_attach,
 						.tuner_attach    = rtl2831u_tuner_attach,
-						.streaming_ctrl  = rtl28xxu_streaming_ctrl,
+						.streaming_ctrl  = rtl2831u_streaming_ctrl,
 						.stream = {
 							.type = USB_BULK,
 							.count = 6,
@@ -821,7 +1213,7 @@ static struct dvb_usb_device_properties rtl28xxu_properties[] = {
 			}
 		},
 
-		.power_ctrl = rtl28xxu_power_ctrl,
+		.power_ctrl = rtl2831u_power_ctrl,
 
 		.rc.core = {
 			.protocol       = RC_TYPE_NEC,
@@ -867,7 +1259,7 @@ static struct dvb_usb_device_properties rtl28xxu_properties[] = {
 					{
 						.frontend_attach = rtl2832u_frontend_attach,
 						.tuner_attach    = rtl2832u_tuner_attach,
-						.streaming_ctrl  = rtl28xxu_streaming_ctrl,
+						.streaming_ctrl  = rtl2832u_streaming_ctrl,
 						.stream = {
 							.type = USB_BULK,
 							.count = 6,
@@ -883,7 +1275,7 @@ static struct dvb_usb_device_properties rtl28xxu_properties[] = {
 			}
 		},
 
-		.power_ctrl = rtl28xxu_power_ctrl,
+		.power_ctrl = rtl2832u_power_ctrl,
 
 		.rc.core = {
 			.protocol       = RC_TYPE_NEC,
@@ -896,10 +1288,25 @@ static struct dvb_usb_device_properties rtl28xxu_properties[] = {
 
 		.i2c_algo = &rtl28xxu_i2c_algo,
 
-		.num_device_descs = 0, /* disabled as no support for RTL2832 */
+		.num_device_descs = 3,
 		.devices = {
 			{
-				.name = "Realtek RTL2832U reference design",
+				.name = "Terratec Cinergy T Stick Black",
+				.warm_ids = {
+					&rtl28xxu_table[RTL2832U_0CCD_00A9],
+				},
+			},
+			{
+				.name = "G-Tek Electronics Group Lifeview LV5TDLX DVB-T",
+				.warm_ids = {
+					&rtl28xxu_table[RTL2832U_1F4D_B803],
+				},
+			},
+			{
+				.name = "NOXON DAB/DAB+ USB dongle",
+				.warm_ids = {
+					&rtl28xxu_table[RTL2832U_0CCD_00B3],
+				},
 			},
 		}
 	},
@@ -910,6 +1317,7 @@ static int rtl28xxu_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
 	int ret, i;
+	u8 val;
 	int properties_count = ARRAY_SIZE(rtl28xxu_properties);
 	struct dvb_usb_device *d;
 	struct usb_device *udev;
@@ -954,16 +1362,25 @@ static int rtl28xxu_probe(struct usb_interface *intf,
 	if (ret)
 		goto err;
 
+
 	/* init USB endpoints */
-	ret = rtl2831_wr_reg(d, USB_SYSCTL_0, 0x09);
+	ret = rtl28xx_rd_reg(d, USB_SYSCTL_0, &val);
+	if (ret)
+			goto err;
+
+	/* enable DMA and Full Packet Mode*/
+	val |= 0x09;
+	ret = rtl28xx_wr_reg(d, USB_SYSCTL_0, val);
 	if (ret)
 		goto err;
 
-	ret = rtl2831_wr_regs(d, USB_EPA_MAXPKT, "\x00\x02\x00\x00", 4);
+	/* set EPA maximum packet size to 0x0200 */
+	ret = rtl28xx_wr_regs(d, USB_EPA_MAXPKT, "\x00\x02\x00\x00", 4);
 	if (ret)
 		goto err;
 
-	ret = rtl2831_wr_regs(d, USB_EPA_FIFO_CFG, "\x14\x00\x00\x00", 4);
+	/* change EPA FIFO length */
+	ret = rtl28xx_wr_regs(d, USB_EPA_FIFO_CFG, "\x14\x00\x00\x00", 4);
 	if (ret)
 		goto err;
 
@@ -1007,4 +1424,5 @@ module_exit(rtl28xxu_module_exit);
 
 MODULE_DESCRIPTION("Realtek RTL28xxU DVB USB driver");
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_AUTHOR("Thomas Mair <thomas.mair86@googlemail.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb/frontends/Kconfig b/drivers/media/dvb/frontends/Kconfig
index b98ebb264e2..a08c2152d0e 100644
--- a/drivers/media/dvb/frontends/Kconfig
+++ b/drivers/media/dvb/frontends/Kconfig
@@ -1,6 +1,7 @@
 config DVB_FE_CUSTOMISE
 	bool "Customise the frontend modules to build"
 	depends on DVB_CORE
+	depends on EXPERT
 	default y if EXPERT
 	help
 	  This allows the user to select/deselect frontend drivers for their
@@ -432,6 +433,13 @@ config DVB_RTL2830
 	help
 	  Say Y when you want to support this frontend.
 
+config DVB_RTL2832
+	tristate "Realtek RTL2832 DVB-T"
+	depends on DVB_CORE && I2C
+	default m if DVB_FE_CUSTOMISE
+	help
+	  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 cd1ac2fd577..185bb8b5195 100644
--- a/drivers/media/dvb/frontends/Makefile
+++ b/drivers/media/dvb/frontends/Makefile
@@ -99,6 +99,7 @@ obj-$(CONFIG_DVB_IT913X_FE) += it913x-fe.o
 obj-$(CONFIG_DVB_A8293) += a8293.o
 obj-$(CONFIG_DVB_TDA10071) += tda10071.o
 obj-$(CONFIG_DVB_RTL2830) += rtl2830.o
+obj-$(CONFIG_DVB_RTL2832) += rtl2832.o
 obj-$(CONFIG_DVB_M88RS2000) += m88rs2000.o
 obj-$(CONFIG_DVB_AF9033) += af9033.o
 
diff --git a/drivers/media/dvb/frontends/a8293.c b/drivers/media/dvb/frontends/a8293.c
index bb56497e940..cff44a389b4 100644
--- a/drivers/media/dvb/frontends/a8293.c
+++ b/drivers/media/dvb/frontends/a8293.c
@@ -21,24 +21,6 @@
 #include "dvb_frontend.h"
 #include "a8293.h"
 
-static int debug;
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug, "Turn on/off debugging (default:off).");
-
-#define LOG_PREFIX "a8293"
-
-#undef dbg
-#define dbg(f, arg...) \
-	if (debug) \
-		printk(KERN_INFO   LOG_PREFIX": " f "\n" , ## arg)
-#undef err
-#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
-#undef info
-#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
-#undef warn
-#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
-
-
 struct a8293_priv {
 	struct i2c_adapter *i2c;
 	const struct a8293_config *cfg;
@@ -65,7 +47,8 @@ static int a8293_i2c(struct a8293_priv *priv, u8 *val, int len, bool rd)
 	if (ret == 1) {
 		ret = 0;
 	} else {
-		warn("i2c failed=%d rd=%d", ret, rd);
+		dev_warn(&priv->i2c->dev, "%s: i2c failed=%d rd=%d\n",
+				KBUILD_MODNAME, ret, rd);
 		ret = -EREMOTEIO;
 	}
 
@@ -88,7 +71,8 @@ static int a8293_set_voltage(struct dvb_frontend *fe,
 	struct a8293_priv *priv = fe->sec_priv;
 	int ret;
 
-	dbg("%s: fe_sec_voltage=%d", __func__, fe_sec_voltage);
+	dev_dbg(&priv->i2c->dev, "%s: fe_sec_voltage=%d\n", __func__,
+			fe_sec_voltage);
 
 	switch (fe_sec_voltage) {
 	case SEC_VOLTAGE_OFF:
@@ -114,14 +98,12 @@ static int a8293_set_voltage(struct dvb_frontend *fe,
 
 	return ret;
 err:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
 static void a8293_release_sec(struct dvb_frontend *fe)
 {
-	dbg("%s:", __func__);
-
 	a8293_set_voltage(fe, SEC_VOLTAGE_OFF);
 
 	kfree(fe->sec_priv);
@@ -154,7 +136,7 @@ struct dvb_frontend *a8293_attach(struct dvb_frontend *fe,
 
 	/* ENB=0 */
 	priv->reg[0] = 0x10;
-	ret = a8293_wr(priv, &priv->reg[1], 1);
+	ret = a8293_wr(priv, &priv->reg[0], 1);
 	if (ret)
 		goto err;
 
@@ -164,16 +146,17 @@ struct dvb_frontend *a8293_attach(struct dvb_frontend *fe,
 	if (ret)
 		goto err;
 
-	info("Allegro A8293 SEC attached.");
-
 	fe->ops.release_sec = a8293_release_sec;
 
 	/* override frontend ops */
 	fe->ops.set_voltage = a8293_set_voltage;
 
+	dev_info(&priv->i2c->dev, "%s: Allegro A8293 SEC attached\n",
+			KBUILD_MODNAME);
+
 	return fe;
 err:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
 	kfree(priv);
 	return NULL;
 }
diff --git a/drivers/media/dvb/frontends/dib8000.c b/drivers/media/dvb/frontends/dib8000.c
index 9ca34f49500..1f3bcb5a1de 100644
--- a/drivers/media/dvb/frontends/dib8000.c
+++ b/drivers/media/dvb/frontends/dib8000.c
@@ -2680,12 +2680,14 @@ static int dib8000_tune(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	int ret = 0;
-	u16 lock, value, mode = fft_to_mode(state);
+	u16 lock, value, mode;
 
 	// we are already tuned - just resuming from suspend
 	if (state == NULL)
 		return -EINVAL;
 
+	mode = fft_to_mode(state);
+
 	dib8000_set_bandwidth(fe, state->fe[0]->dtv_property_cache.bandwidth_hz / 1000);
 	dib8000_set_channel(state, 0, 0);
 
diff --git a/drivers/media/dvb/frontends/drxk.h b/drivers/media/dvb/frontends/drxk.h
index 9d64e4fea06..d615d7d055a 100644
--- a/drivers/media/dvb/frontends/drxk.h
+++ b/drivers/media/dvb/frontends/drxk.h
@@ -20,6 +20,14 @@
  *			means that 1=DVBC, 0 = DVBT. Zero means the opposite.
  * @mpeg_out_clk_strength: DRXK Mpeg output clock drive strength.
  * @microcode_name:	Name of the firmware file with the microcode
+ * @qam_demod_parameter_count:	The number of parameters used for the command
+ *				to set the demodulator parameters. All
+ *				firmwares are using the 2-parameter commmand.
+ *				An exception is the "drxk_a3.mc" firmware,
+ *				which uses the 4-parameter command.
+ *				A value of 0 (default) or lower indicates that
+ *				the correct number of parameters will be
+ *				automatically detected.
  *
  * On the *_gpio vars, bit 0 is UIO-1, bit 1 is UIO-2 and bit 2 is
  * UIO-3.
@@ -38,7 +46,8 @@ struct drxk_config {
 	u8	mpeg_out_clk_strength;
 	int	chunk_size;
 
-	const char *microcode_name;
+	const char	*microcode_name;
+	int		 qam_demod_parameter_count;
 };
 
 #if defined(CONFIG_DVB_DRXK) || (defined(CONFIG_DVB_DRXK_MODULE) \
diff --git a/drivers/media/dvb/frontends/drxk_hard.c b/drivers/media/dvb/frontends/drxk_hard.c
index 60b868faeac..1ab8154542d 100644
--- a/drivers/media/dvb/frontends/drxk_hard.c
+++ b/drivers/media/dvb/frontends/drxk_hard.c
@@ -28,6 +28,7 @@
 #include <linux/delay.h>
 #include <linux/firmware.h>
 #include <linux/i2c.h>
+#include <linux/hardirq.h>
 #include <asm/div64.h>
 
 #include "dvb_frontend.h"
@@ -308,16 +309,42 @@ static u32 Log10Times100(u32 x)
 /* I2C **********************************************************************/
 /****************************************************************************/
 
-static int i2c_read1(struct i2c_adapter *adapter, u8 adr, u8 *val)
+static int drxk_i2c_lock(struct drxk_state *state)
+{
+	i2c_lock_adapter(state->i2c);
+	state->drxk_i2c_exclusive_lock = true;
+
+	return 0;
+}
+
+static void drxk_i2c_unlock(struct drxk_state *state)
+{
+	if (!state->drxk_i2c_exclusive_lock)
+		return;
+
+	i2c_unlock_adapter(state->i2c);
+	state->drxk_i2c_exclusive_lock = false;
+}
+
+static int drxk_i2c_transfer(struct drxk_state *state, struct i2c_msg *msgs,
+			     unsigned len)
+{
+	if (state->drxk_i2c_exclusive_lock)
+		return __i2c_transfer(state->i2c, msgs, len);
+	else
+		return i2c_transfer(state->i2c, msgs, len);
+}
+
+static int i2c_read1(struct drxk_state *state, u8 adr, u8 *val)
 {
 	struct i2c_msg msgs[1] = { {.addr = adr, .flags = I2C_M_RD,
 				    .buf = val, .len = 1}
 	};
 
-	return i2c_transfer(adapter, msgs, 1);
+	return drxk_i2c_transfer(state, msgs, 1);
 }
 
-static int i2c_write(struct i2c_adapter *adap, u8 adr, u8 *data, int len)
+static int i2c_write(struct drxk_state *state, u8 adr, u8 *data, int len)
 {
 	int status;
 	struct i2c_msg msg = {
@@ -330,7 +357,7 @@ static int i2c_write(struct i2c_adapter *adap, u8 adr, u8 *data, int len)
 			printk(KERN_CONT " %02x", data[i]);
 		printk(KERN_CONT "\n");
 	}
-	status = i2c_transfer(adap, &msg, 1);
+	status = drxk_i2c_transfer(state, &msg, 1);
 	if (status >= 0 && status != 1)
 		status = -EIO;
 
@@ -340,7 +367,7 @@ static int i2c_write(struct i2c_adapter *adap, u8 adr, u8 *data, int len)
 	return status;
 }
 
-static int i2c_read(struct i2c_adapter *adap,
+static int i2c_read(struct drxk_state *state,
 		    u8 adr, u8 *msg, int len, u8 *answ, int alen)
 {
 	int status;
@@ -351,7 +378,7 @@ static int i2c_read(struct i2c_adapter *adap,
 		 .buf = answ, .len = alen}
 	};
 
-	status = i2c_transfer(adap, msgs, 2);
+	status = drxk_i2c_transfer(state, msgs, 2);
 	if (status != 2) {
 		if (debug > 2)
 			printk(KERN_CONT ": ERROR!\n");
@@ -394,7 +421,7 @@ static int read16_flags(struct drxk_state *state, u32 reg, u16 *data, u8 flags)
 		len = 2;
 	}
 	dprintk(2, "(0x%08x, 0x%02x)\n", reg, flags);
-	status = i2c_read(state->i2c, adr, mm1, len, mm2, 2);
+	status = i2c_read(state, adr, mm1, len, mm2, 2);
 	if (status < 0)
 		return status;
 	if (data)
@@ -428,7 +455,7 @@ static int read32_flags(struct drxk_state *state, u32 reg, u32 *data, u8 flags)
 		len = 2;
 	}
 	dprintk(2, "(0x%08x, 0x%02x)\n", reg, flags);
-	status = i2c_read(state->i2c, adr, mm1, len, mm2, 4);
+	status = i2c_read(state, adr, mm1, len, mm2, 4);
 	if (status < 0)
 		return status;
 	if (data)
@@ -464,7 +491,7 @@ static int write16_flags(struct drxk_state *state, u32 reg, u16 data, u8 flags)
 	mm[len + 1] = (data >> 8) & 0xff;
 
 	dprintk(2, "(0x%08x, 0x%04x, 0x%02x)\n", reg, data, flags);
-	return i2c_write(state->i2c, adr, mm, len + 2);
+	return i2c_write(state, adr, mm, len + 2);
 }
 
 static int write16(struct drxk_state *state, u32 reg, u16 data)
@@ -495,7 +522,7 @@ static int write32_flags(struct drxk_state *state, u32 reg, u32 data, u8 flags)
 	mm[len + 3] = (data >> 24) & 0xff;
 	dprintk(2, "(0x%08x, 0x%08x, 0x%02x)\n", reg, data, flags);
 
-	return i2c_write(state->i2c, adr, mm, len + 4);
+	return i2c_write(state, adr, mm, len + 4);
 }
 
 static int write32(struct drxk_state *state, u32 reg, u32 data)
@@ -542,7 +569,7 @@ static int write_block(struct drxk_state *state, u32 Address,
 					printk(KERN_CONT " %02x", pBlock[i]);
 			printk(KERN_CONT "\n");
 		}
-		status = i2c_write(state->i2c, state->demod_address,
+		status = i2c_write(state, state->demod_address,
 				   &state->Chunk[0], Chunk + AdrLength);
 		if (status < 0) {
 			printk(KERN_ERR "drxk: %s: i2c write error at addr 0x%02x\n",
@@ -568,17 +595,17 @@ int PowerUpDevice(struct drxk_state *state)
 
 	dprintk(1, "\n");
 
-	status = i2c_read1(state->i2c, state->demod_address, &data);
+	status = i2c_read1(state, state->demod_address, &data);
 	if (status < 0) {
 		do {
 			data = 0;
-			status = i2c_write(state->i2c, state->demod_address,
+			status = i2c_write(state, state->demod_address,
 					   &data, 1);
 			msleep(10);
 			retryCount++;
 			if (status < 0)
 				continue;
-			status = i2c_read1(state->i2c, state->demod_address,
+			status = i2c_read1(state, state->demod_address,
 					   &data);
 		} while (status < 0 &&
 			 (retryCount < DRXK_MAX_RETRIES_POWERUP));
@@ -932,7 +959,7 @@ static int GetDeviceCapabilities(struct drxk_state *state)
 	if (status < 0)
 		goto error;
 
-printk(KERN_ERR "drxk: status = 0x%08x\n", sioTopJtagidLo);
+	printk(KERN_INFO "drxk: status = 0x%08x\n", sioTopJtagidLo);
 
 	/* driver 0.9.0 */
 	switch ((sioTopJtagidLo >> 29) & 0xF) {
@@ -2824,7 +2851,7 @@ static int ConfigureI2CBridge(struct drxk_state *state, bool bEnableBridge)
 	dprintk(1, "\n");
 
 	if (state->m_DrxkState == DRXK_UNINITIALIZED)
-		goto error;
+		return 0;
 	if (state->m_DrxkState == DRXK_POWERED_DOWN)
 		goto error;
 
@@ -2977,7 +3004,7 @@ static int ADCSynchronization(struct drxk_state *state)
 		status = read16(state, IQM_AF_CLKNEG__A, &clkNeg);
 		if (status < 0)
 			goto error;
-		if ((clkNeg | IQM_AF_CLKNEG_CLKNEGDATA__M) ==
+		if ((clkNeg & IQM_AF_CLKNEG_CLKNEGDATA__M) ==
 			IQM_AF_CLKNEG_CLKNEGDATA_CLK_ADC_DATA_POS) {
 			clkNeg &= (~(IQM_AF_CLKNEG_CLKNEGDATA__M));
 			clkNeg |=
@@ -5361,7 +5388,7 @@ static int GetQAMLockStatus(struct drxk_state *state, u32 *pLockStatus)
 			SCU_RAM_COMMAND_CMD_DEMOD_GET_LOCK, 0, NULL, 2,
 			Result);
 	if (status < 0)
-		printk(KERN_ERR "drxk: %s status = %08x\n", __func__, status);
+		printk(KERN_ERR "drxk: Error %d on %s\n", status, __func__);
 
 	if (Result[1] < SCU_RAM_QAM_LOCKED_LOCKED_DEMOD_LOCKED) {
 		/* 0x0000 NOT LOCKED */
@@ -5388,12 +5415,67 @@ static int GetQAMLockStatus(struct drxk_state *state, u32 *pLockStatus)
 #define QAM_LOCKRANGE__M      0x10
 #define QAM_LOCKRANGE_NORMAL  0x10
 
+static int QAMDemodulatorCommand(struct drxk_state *state,
+				 int numberOfParameters)
+{
+	int status;
+	u16 cmdResult;
+	u16 setParamParameters[4] = { 0, 0, 0, 0 };
+
+	setParamParameters[0] = state->m_Constellation;	/* modulation     */
+	setParamParameters[1] = DRXK_QAM_I12_J17;	/* interleave mode   */
+
+	if (numberOfParameters == 2) {
+		u16 setEnvParameters[1] = { 0 };
+
+		if (state->m_OperationMode == OM_QAM_ITU_C)
+			setEnvParameters[0] = QAM_TOP_ANNEX_C;
+		else
+			setEnvParameters[0] = QAM_TOP_ANNEX_A;
+
+		status = scu_command(state,
+				     SCU_RAM_COMMAND_STANDARD_QAM | SCU_RAM_COMMAND_CMD_DEMOD_SET_ENV,
+				     1, setEnvParameters, 1, &cmdResult);
+		if (status < 0)
+			goto error;
+
+		status = scu_command(state,
+				     SCU_RAM_COMMAND_STANDARD_QAM | SCU_RAM_COMMAND_CMD_DEMOD_SET_PARAM,
+				     numberOfParameters, setParamParameters,
+				     1, &cmdResult);
+	} else if (numberOfParameters == 4) {
+		if (state->m_OperationMode == OM_QAM_ITU_C)
+			setParamParameters[2] = QAM_TOP_ANNEX_C;
+		else
+			setParamParameters[2] = QAM_TOP_ANNEX_A;
+
+		setParamParameters[3] |= (QAM_MIRROR_AUTO_ON);
+		/* Env parameters */
+		/* check for LOCKRANGE Extented */
+		/* setParamParameters[3] |= QAM_LOCKRANGE_NORMAL; */
+
+		status = scu_command(state,
+				     SCU_RAM_COMMAND_STANDARD_QAM | SCU_RAM_COMMAND_CMD_DEMOD_SET_PARAM,
+				     numberOfParameters, setParamParameters,
+				     1, &cmdResult);
+	} else {
+		printk(KERN_WARNING "drxk: Unknown QAM demodulator parameter "
+			"count %d\n", numberOfParameters);
+	}
+
+error:
+	if (status < 0)
+		printk(KERN_WARNING "drxk: Warning %d on %s\n",
+		       status, __func__);
+	return status;
+}
+
 static int SetQAM(struct drxk_state *state, u16 IntermediateFreqkHz,
 		  s32 tunerFreqOffset)
 {
 	int status;
-	u16 setParamParameters[4] = { 0, 0, 0, 0 };
 	u16 cmdResult;
+	int qamDemodParamCount = state->qam_demod_parameter_count;
 
 	dprintk(1, "\n");
 	/*
@@ -5445,34 +5527,42 @@ static int SetQAM(struct drxk_state *state, u16 IntermediateFreqkHz,
 	}
 	if (status < 0)
 		goto error;
-	setParamParameters[0] = state->m_Constellation;	/* modulation     */
-	setParamParameters[1] = DRXK_QAM_I12_J17;	/* interleave mode   */
-	if (state->m_OperationMode == OM_QAM_ITU_C)
-		setParamParameters[2] = QAM_TOP_ANNEX_C;
-	else
-		setParamParameters[2] = QAM_TOP_ANNEX_A;
-	setParamParameters[3] |= (QAM_MIRROR_AUTO_ON);
-	/* Env parameters */
-	/* check for LOCKRANGE Extented */
-	/* setParamParameters[3] |= QAM_LOCKRANGE_NORMAL; */
 
-	status = scu_command(state, SCU_RAM_COMMAND_STANDARD_QAM | SCU_RAM_COMMAND_CMD_DEMOD_SET_PARAM, 4, setParamParameters, 1, &cmdResult);
-	if (status < 0) {
-		/* Fall-back to the simpler call */
-		if (state->m_OperationMode == OM_QAM_ITU_C)
-			setParamParameters[0] = QAM_TOP_ANNEX_C;
-		else
-			setParamParameters[0] = QAM_TOP_ANNEX_A;
-		status = scu_command(state, SCU_RAM_COMMAND_STANDARD_QAM | SCU_RAM_COMMAND_CMD_DEMOD_SET_ENV, 1, setParamParameters, 1, &cmdResult);
-		if (status < 0)
-			goto error;
+	/* Use the 4-parameter if it's requested or we're probing for
+	 * the correct command. */
+	if (state->qam_demod_parameter_count == 4
+		|| !state->qam_demod_parameter_count) {
+		qamDemodParamCount = 4;
+		status = QAMDemodulatorCommand(state, qamDemodParamCount);
+	}
 
-		setParamParameters[0] = state->m_Constellation; /* modulation     */
-		setParamParameters[1] = DRXK_QAM_I12_J17;       /* interleave mode   */
-		status = scu_command(state, SCU_RAM_COMMAND_STANDARD_QAM | SCU_RAM_COMMAND_CMD_DEMOD_SET_PARAM, 2, setParamParameters, 1, &cmdResult);
+	/* Use the 2-parameter command if it was requested or if we're
+	 * probing for the correct command and the 4-parameter command
+	 * failed. */
+	if (state->qam_demod_parameter_count == 2
+		|| (!state->qam_demod_parameter_count && status < 0)) {
+		qamDemodParamCount = 2;
+		status = QAMDemodulatorCommand(state, qamDemodParamCount);
 	}
-	if (status < 0)
+
+	if (status < 0) {
+		dprintk(1, "Could not set demodulator parameters. Make "
+			"sure qam_demod_parameter_count (%d) is correct for "
+			"your firmware (%s).\n",
+			state->qam_demod_parameter_count,
+			state->microcode_name);
 		goto error;
+	} else if (!state->qam_demod_parameter_count) {
+		dprintk(1, "Auto-probing the correct QAM demodulator command "
+			"parameters was successful - using %d parameters.\n",
+			qamDemodParamCount);
+
+		/*
+		 * One of our commands was successful. We don't need to
+		 * auto-probe anymore, now that we got the correct command.
+		 */
+		state->qam_demod_parameter_count = qamDemodParamCount;
+	}
 
 	/*
 	 * STEP 3: enable the system in a mode where the ADC provides valid
@@ -5968,34 +6058,15 @@ error:
 	return status;
 }
 
-static int load_microcode(struct drxk_state *state, const char *mc_name)
-{
-	const struct firmware *fw = NULL;
-	int err = 0;
-
-	dprintk(1, "\n");
-
-	err = request_firmware(&fw, mc_name, state->i2c->dev.parent);
-	if (err < 0) {
-		printk(KERN_ERR
-		       "drxk: Could not load firmware file %s.\n", mc_name);
-		printk(KERN_INFO
-		       "drxk: Copy %s to your hotplug directory!\n", mc_name);
-		return err;
-	}
-	err = DownloadMicrocode(state, fw->data, fw->size);
-	release_firmware(fw);
-	return err;
-}
-
 static int init_drxk(struct drxk_state *state)
 {
-	int status = 0;
+	int status = 0, n = 0;
 	enum DRXPowerMode powerMode = DRXK_POWER_DOWN_OFDM;
 	u16 driverVersion;
 
 	dprintk(1, "\n");
 	if ((state->m_DrxkState == DRXK_UNINITIALIZED)) {
+		drxk_i2c_lock(state);
 		status = PowerUpDevice(state);
 		if (status < 0)
 			goto error;
@@ -6073,8 +6144,12 @@ static int init_drxk(struct drxk_state *state)
 		if (status < 0)
 			goto error;
 
-		if (state->microcode_name)
-			load_microcode(state, state->microcode_name);
+		if (state->fw) {
+			status = DownloadMicrocode(state, state->fw->data,
+						   state->fw->size);
+			if (status < 0)
+				goto error;
+		}
 
 		/* disable token-ring bus through OFDM block for possible ucode upload */
 		status = write16(state, SIO_OFDM_SH_OFDM_RING_ENABLE__A, SIO_OFDM_SH_OFDM_RING_ENABLE_OFF);
@@ -6167,19 +6242,71 @@ static int init_drxk(struct drxk_state *state)
 			state->m_DrxkState = DRXK_POWERED_DOWN;
 		} else
 			state->m_DrxkState = DRXK_STOPPED;
+
+		/* Initialize the supported delivery systems */
+		n = 0;
+		if (state->m_hasDVBC) {
+			state->frontend.ops.delsys[n++] = SYS_DVBC_ANNEX_A;
+			state->frontend.ops.delsys[n++] = SYS_DVBC_ANNEX_C;
+			strlcat(state->frontend.ops.info.name, " DVB-C",
+				sizeof(state->frontend.ops.info.name));
+		}
+		if (state->m_hasDVBT) {
+			state->frontend.ops.delsys[n++] = SYS_DVBT;
+			strlcat(state->frontend.ops.info.name, " DVB-T",
+				sizeof(state->frontend.ops.info.name));
+		}
+		drxk_i2c_unlock(state);
 	}
 error:
-	if (status < 0)
+	if (status < 0) {
+		state->m_DrxkState = DRXK_NO_DEV;
+		drxk_i2c_unlock(state);
 		printk(KERN_ERR "drxk: Error %d on %s\n", status, __func__);
+	}
 
 	return status;
 }
 
+static void load_firmware_cb(const struct firmware *fw,
+			     void *context)
+{
+	struct drxk_state *state = context;
+
+	dprintk(1, ": %s\n", fw ? "firmware loaded" : "firmware not loaded");
+	if (!fw) {
+		printk(KERN_ERR
+		       "drxk: Could not load firmware file %s.\n",
+			state->microcode_name);
+		printk(KERN_INFO
+		       "drxk: Copy %s to your hotplug directory!\n",
+			state->microcode_name);
+		state->microcode_name = NULL;
+
+		/*
+		 * As firmware is now load asynchronous, it is not possible
+		 * anymore to fail at frontend attach. We might silently
+		 * return here, and hope that the driver won't crash.
+		 * We might also change all DVB callbacks to return -ENODEV
+		 * if the device is not initialized.
+		 * As the DRX-K devices have their own internal firmware,
+		 * let's just hope that it will match a firmware revision
+		 * compatible with this driver and proceed.
+		 */
+	}
+	state->fw = fw;
+
+	init_drxk(state);
+}
+
 static void drxk_release(struct dvb_frontend *fe)
 {
 	struct drxk_state *state = fe->demodulator_priv;
 
 	dprintk(1, "\n");
+	if (state->fw)
+		release_firmware(state->fw);
+
 	kfree(state);
 }
 
@@ -6188,6 +6315,12 @@ static int drxk_sleep(struct dvb_frontend *fe)
 	struct drxk_state *state = fe->demodulator_priv;
 
 	dprintk(1, "\n");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return 0;
+
 	ShutDown(state);
 	return 0;
 }
@@ -6196,7 +6329,11 @@ static int drxk_gate_ctrl(struct dvb_frontend *fe, int enable)
 {
 	struct drxk_state *state = fe->demodulator_priv;
 
-	dprintk(1, "%s\n", enable ? "enable" : "disable");
+	dprintk(1, ": %s\n", enable ? "enable" : "disable");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+
 	return ConfigureI2CBridge(state, enable ? true : false);
 }
 
@@ -6209,6 +6346,12 @@ static int drxk_set_parameters(struct dvb_frontend *fe)
 
 	dprintk(1, "\n");
 
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	if (!fe->ops.tuner_ops.get_if_frequency) {
 		printk(KERN_ERR
 		       "drxk: Error: get_if_frequency() not defined at tuner. Can't work without it!\n");
@@ -6262,6 +6405,12 @@ static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
 	u32 stat;
 
 	dprintk(1, "\n");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	*status = 0;
 	GetLockStatus(state, &stat, 0);
 	if (stat == MPEG_LOCK)
@@ -6275,8 +6424,15 @@ static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 static int drxk_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
+	struct drxk_state *state = fe->demodulator_priv;
+
 	dprintk(1, "\n");
 
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	*ber = 0;
 	return 0;
 }
@@ -6288,6 +6444,12 @@ static int drxk_read_signal_strength(struct dvb_frontend *fe,
 	u32 val = 0;
 
 	dprintk(1, "\n");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	ReadIFAgc(state, &val);
 	*strength = val & 0xffff;
 	return 0;
@@ -6299,6 +6461,12 @@ static int drxk_read_snr(struct dvb_frontend *fe, u16 *snr)
 	s32 snr2;
 
 	dprintk(1, "\n");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	GetSignalToNoise(state, &snr2);
 	*snr = snr2 & 0xffff;
 	return 0;
@@ -6310,6 +6478,12 @@ static int drxk_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 	u16 err;
 
 	dprintk(1, "\n");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	DVBTQAMGetAccPktErr(state, &err);
 	*ucblocks = (u32) err;
 	return 0;
@@ -6318,9 +6492,16 @@ static int drxk_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 static int drxk_get_tune_settings(struct dvb_frontend *fe, struct dvb_frontend_tune_settings
 				    *sets)
 {
+	struct drxk_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 
 	dprintk(1, "\n");
+
+	if (state->m_DrxkState == DRXK_NO_DEV)
+		return -ENODEV;
+	if (state->m_DrxkState == DRXK_UNINITIALIZED)
+		return -EAGAIN;
+
 	switch (p->delivery_system) {
 	case SYS_DVBC_ANNEX_A:
 	case SYS_DVBC_ANNEX_C:
@@ -6371,10 +6552,9 @@ static struct dvb_frontend_ops drxk_ops = {
 struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 				 struct i2c_adapter *i2c)
 {
-	int n;
-
 	struct drxk_state *state = NULL;
 	u8 adr = config->adr;
+	int status;
 
 	dprintk(1, "\n");
 	state = kzalloc(sizeof(struct drxk_state), GFP_KERNEL);
@@ -6385,6 +6565,7 @@ struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 	state->demod_address = adr;
 	state->single_master = config->single_master;
 	state->microcode_name = config->microcode_name;
+	state->qam_demod_parameter_count = config->qam_demod_parameter_count;
 	state->no_i2c_bridge = config->no_i2c_bridge;
 	state->antenna_gpio = config->antenna_gpio;
 	state->antenna_dvbt = config->antenna_dvbt;
@@ -6425,22 +6606,21 @@ struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 	state->frontend.demodulator_priv = state;
 
 	init_state(state);
-	if (init_drxk(state) < 0)
-		goto error;
 
-	/* Initialize the supported delivery systems */
-	n = 0;
-	if (state->m_hasDVBC) {
-		state->frontend.ops.delsys[n++] = SYS_DVBC_ANNEX_A;
-		state->frontend.ops.delsys[n++] = SYS_DVBC_ANNEX_C;
-		strlcat(state->frontend.ops.info.name, " DVB-C",
-			sizeof(state->frontend.ops.info.name));
-	}
-	if (state->m_hasDVBT) {
-		state->frontend.ops.delsys[n++] = SYS_DVBT;
-		strlcat(state->frontend.ops.info.name, " DVB-T",
-			sizeof(state->frontend.ops.info.name));
-	}
+	/* Load firmware and initialize DRX-K */
+	if (state->microcode_name) {
+		status = request_firmware_nowait(THIS_MODULE, 1,
+					      state->microcode_name,
+					      state->i2c->dev.parent,
+					      GFP_KERNEL,
+					      state, load_firmware_cb);
+		if (status < 0) {
+			printk(KERN_ERR
+			"drxk: failed to request a firmware\n");
+			return NULL;
+		}
+	} else if (init_drxk(state) < 0)
+		goto error;
 
 	printk(KERN_INFO "drxk: frontend initialized.\n");
 	return &state->frontend;
diff --git a/drivers/media/dvb/frontends/drxk_hard.h b/drivers/media/dvb/frontends/drxk_hard.h
index 4bbf841de83..6bb9fc4a7b9 100644
--- a/drivers/media/dvb/frontends/drxk_hard.h
+++ b/drivers/media/dvb/frontends/drxk_hard.h
@@ -94,7 +94,15 @@ enum DRXPowerMode {
 
 
 enum AGC_CTRL_MODE { DRXK_AGC_CTRL_AUTO = 0, DRXK_AGC_CTRL_USER, DRXK_AGC_CTRL_OFF };
-enum EDrxkState { DRXK_UNINITIALIZED = 0, DRXK_STOPPED, DRXK_DTV_STARTED, DRXK_ATV_STARTED, DRXK_POWERED_DOWN };
+enum EDrxkState {
+	DRXK_UNINITIALIZED = 0,
+	DRXK_STOPPED,
+	DRXK_DTV_STARTED,
+	DRXK_ATV_STARTED,
+	DRXK_POWERED_DOWN,
+	DRXK_NO_DEV			/* If drxk init failed */
+};
+
 enum EDrxkCoefArrayIndex {
 	DRXK_COEF_IDX_MN = 0,
 	DRXK_COEF_IDX_FM    ,
@@ -325,6 +333,9 @@ struct drxk_state {
 
 	enum DRXPowerMode m_currentPowerMode;
 
+	/* when true, avoids other devices to use the I2C bus */
+	bool		  drxk_i2c_exclusive_lock;
+
 	/*
 	 * Configurable parameters at the driver. They stores the values found
 	 * at struct drxk_config.
@@ -338,7 +349,11 @@ struct drxk_state {
 	bool	antenna_dvbt;
 	u16	antenna_gpio;
 
+	/* Firmware */
 	const char *microcode_name;
+	struct completion fw_wait_load;
+	const struct firmware *fw;
+	int qam_demod_parameter_count;
 };
 
 #define NEVER_LOCK 0
diff --git a/drivers/media/dvb/frontends/lgs8gxx.c b/drivers/media/dvb/frontends/lgs8gxx.c
index 568363a10a3..c2ea2749ebe 100644
--- a/drivers/media/dvb/frontends/lgs8gxx.c
+++ b/drivers/media/dvb/frontends/lgs8gxx.c
@@ -40,6 +40,8 @@
 static int debug;
 static int fake_signal_str = 1;
 
+#define LGS8GXX_FIRMWARE "lgs8g75.fw"
+
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Turn on/off frontend debugging (default:off).");
 
@@ -592,7 +594,7 @@ static int lgs8g75_init_data(struct lgs8gxx_state *priv)
 	int rc;
 	int i;
 
-	rc = request_firmware(&fw, "lgs8g75.fw", &priv->i2c->dev);
+	rc = request_firmware(&fw, LGS8GXX_FIRMWARE, &priv->i2c->dev);
 	if (rc)
 		return rc;
 
@@ -1070,3 +1072,4 @@ EXPORT_SYMBOL(lgs8gxx_attach);
 MODULE_DESCRIPTION("Legend Silicon LGS8913/LGS8GXX DMB-TH demodulator driver");
 MODULE_AUTHOR("David T. L. Wong <davidtlwong@gmail.com>");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(LGS8GXX_FIRMWARE);
diff --git a/drivers/media/dvb/frontends/rtl2832.c b/drivers/media/dvb/frontends/rtl2832.c
new file mode 100644
index 00000000000..28269ccaeab
--- /dev/null
+++ b/drivers/media/dvb/frontends/rtl2832.c
@@ -0,0 +1,789 @@
+/*
+ * Realtek RTL2832 DVB-T demodulator driver
+ *
+ * Copyright (C) 2012 Thomas Mair <thomas.mair86@gmail.com>
+ *
+ *	This program is free software; you can redistribute it and/or modify
+ *	it under the terms of the GNU General Public License as published by
+ *	the Free Software Foundation; either version 2 of the License, or
+ *	(at your option) any later version.
+ *
+ *	This program is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *	GNU General Public License for more details.
+ *
+ *	You should have received a copy of the GNU General Public License along
+ *	with this program; if not, write to the Free Software Foundation, Inc.,
+ *	51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include "rtl2832_priv.h"
+#include <linux/bitops.h>
+
+int rtl2832_debug;
+module_param_named(debug, rtl2832_debug, int, 0644);
+MODULE_PARM_DESC(debug, "Turn on/off frontend debugging (default:off).");
+
+#define REG_MASK(b) (BIT(b + 1) - 1)
+
+static const struct rtl2832_reg_entry registers[] = {
+	[DVBT_SOFT_RST]		= {0x1, 0x1,   2, 2},
+	[DVBT_IIC_REPEAT]	= {0x1, 0x1,   3, 3},
+	[DVBT_TR_WAIT_MIN_8K]	= {0x1, 0x88, 11, 2},
+	[DVBT_RSD_BER_FAIL_VAL]	= {0x1, 0x8f, 15, 0},
+	[DVBT_EN_BK_TRK]	= {0x1, 0xa6,  7, 7},
+	[DVBT_AD_EN_REG]	= {0x0, 0x8,   7, 7},
+	[DVBT_AD_EN_REG1]	= {0x0, 0x8,   6, 6},
+	[DVBT_EN_BBIN]		= {0x1, 0xb1,  0, 0},
+	[DVBT_MGD_THD0]		= {0x1, 0x95,  7, 0},
+	[DVBT_MGD_THD1]		= {0x1, 0x96,  7, 0},
+	[DVBT_MGD_THD2]		= {0x1, 0x97,  7, 0},
+	[DVBT_MGD_THD3]		= {0x1, 0x98,  7, 0},
+	[DVBT_MGD_THD4]		= {0x1, 0x99,  7, 0},
+	[DVBT_MGD_THD5]		= {0x1, 0x9a,  7, 0},
+	[DVBT_MGD_THD6]		= {0x1, 0x9b,  7, 0},
+	[DVBT_MGD_THD7]		= {0x1, 0x9c,  7, 0},
+	[DVBT_EN_CACQ_NOTCH]	= {0x1, 0x61,  4, 4},
+	[DVBT_AD_AV_REF]	= {0x0, 0x9,   6, 0},
+	[DVBT_REG_PI]		= {0x0, 0xa,   2, 0},
+	[DVBT_PIP_ON]		= {0x0, 0x21,  3, 3},
+	[DVBT_SCALE1_B92]	= {0x2, 0x92,  7, 0},
+	[DVBT_SCALE1_B93]	= {0x2, 0x93,  7, 0},
+	[DVBT_SCALE1_BA7]	= {0x2, 0xa7,  7, 0},
+	[DVBT_SCALE1_BA9]	= {0x2, 0xa9,  7, 0},
+	[DVBT_SCALE1_BAA]	= {0x2, 0xaa,  7, 0},
+	[DVBT_SCALE1_BAB]	= {0x2, 0xab,  7, 0},
+	[DVBT_SCALE1_BAC]	= {0x2, 0xac,  7, 0},
+	[DVBT_SCALE1_BB0]	= {0x2, 0xb0,  7, 0},
+	[DVBT_SCALE1_BB1]	= {0x2, 0xb1,  7, 0},
+	[DVBT_KB_P1]		= {0x1, 0x64,  3, 1},
+	[DVBT_KB_P2]		= {0x1, 0x64,  6, 4},
+	[DVBT_KB_P3]		= {0x1, 0x65,  2, 0},
+	[DVBT_OPT_ADC_IQ]	= {0x0, 0x6,   5, 4},
+	[DVBT_AD_AVI]		= {0x0, 0x9,   1, 0},
+	[DVBT_AD_AVQ]		= {0x0, 0x9,   3, 2},
+	[DVBT_K1_CR_STEP12]	= {0x2, 0xad,  9, 4},
+	[DVBT_TRK_KS_P2]	= {0x1, 0x6f,  2, 0},
+	[DVBT_TRK_KS_I2]	= {0x1, 0x70,  5, 3},
+	[DVBT_TR_THD_SET2]	= {0x1, 0x72,  3, 0},
+	[DVBT_TRK_KC_P2]	= {0x1, 0x73,  5, 3},
+	[DVBT_TRK_KC_I2]	= {0x1, 0x75,  2, 0},
+	[DVBT_CR_THD_SET2]	= {0x1, 0x76,  7, 6},
+	[DVBT_PSET_IFFREQ]	= {0x1, 0x19, 21, 0},
+	[DVBT_SPEC_INV]		= {0x1, 0x15,  0, 0},
+	[DVBT_RSAMP_RATIO]	= {0x1, 0x9f, 27, 2},
+	[DVBT_CFREQ_OFF_RATIO]	= {0x1, 0x9d, 23, 4},
+	[DVBT_FSM_STAGE]	= {0x3, 0x51,  6, 3},
+	[DVBT_RX_CONSTEL]	= {0x3, 0x3c,  3, 2},
+	[DVBT_RX_HIER]		= {0x3, 0x3c,  6, 4},
+	[DVBT_RX_C_RATE_LP]	= {0x3, 0x3d,  2, 0},
+	[DVBT_RX_C_RATE_HP]	= {0x3, 0x3d,  5, 3},
+	[DVBT_GI_IDX]		= {0x3, 0x51,  1, 0},
+	[DVBT_FFT_MODE_IDX]	= {0x3, 0x51,  2, 2},
+	[DVBT_RSD_BER_EST]	= {0x3, 0x4e, 15, 0},
+	[DVBT_CE_EST_EVM]	= {0x4, 0xc,  15, 0},
+	[DVBT_RF_AGC_VAL]	= {0x3, 0x5b, 13, 0},
+	[DVBT_IF_AGC_VAL]	= {0x3, 0x59, 13, 0},
+	[DVBT_DAGC_VAL]		= {0x3, 0x5,   7, 0},
+	[DVBT_SFREQ_OFF]	= {0x3, 0x18, 13, 0},
+	[DVBT_CFREQ_OFF]	= {0x3, 0x5f, 17, 0},
+	[DVBT_POLAR_RF_AGC]	= {0x0, 0xe,   1, 1},
+	[DVBT_POLAR_IF_AGC]	= {0x0, 0xe,   0, 0},
+	[DVBT_AAGC_HOLD]	= {0x1, 0x4,   5, 5},
+	[DVBT_EN_RF_AGC]	= {0x1, 0x4,   6, 6},
+	[DVBT_EN_IF_AGC]	= {0x1, 0x4,   7, 7},
+	[DVBT_IF_AGC_MIN]	= {0x1, 0x8,   7, 0},
+	[DVBT_IF_AGC_MAX]	= {0x1, 0x9,   7, 0},
+	[DVBT_RF_AGC_MIN]	= {0x1, 0xa,   7, 0},
+	[DVBT_RF_AGC_MAX]	= {0x1, 0xb,   7, 0},
+	[DVBT_IF_AGC_MAN]	= {0x1, 0xc,   6, 6},
+	[DVBT_IF_AGC_MAN_VAL]	= {0x1, 0xc,  13, 0},
+	[DVBT_RF_AGC_MAN]	= {0x1, 0xe,   6, 6},
+	[DVBT_RF_AGC_MAN_VAL]	= {0x1, 0xe,  13, 0},
+	[DVBT_DAGC_TRG_VAL]	= {0x1, 0x12,  7, 0},
+	[DVBT_AGC_TARG_VAL_0]	= {0x1, 0x2,   0, 0},
+	[DVBT_AGC_TARG_VAL_8_1]	= {0x1, 0x3,   7, 0},
+	[DVBT_AAGC_LOOP_GAIN]	= {0x1, 0xc7,  5, 1},
+	[DVBT_LOOP_GAIN2_3_0]	= {0x1, 0x4,   4, 1},
+	[DVBT_LOOP_GAIN2_4]	= {0x1, 0x5,   7, 7},
+	[DVBT_LOOP_GAIN3]	= {0x1, 0xc8,  4, 0},
+	[DVBT_VTOP1]		= {0x1, 0x6,   5, 0},
+	[DVBT_VTOP2]		= {0x1, 0xc9,  5, 0},
+	[DVBT_VTOP3]		= {0x1, 0xca,  5, 0},
+	[DVBT_KRF1]		= {0x1, 0xcb,  7, 0},
+	[DVBT_KRF2]		= {0x1, 0x7,   7, 0},
+	[DVBT_KRF3]		= {0x1, 0xcd,  7, 0},
+	[DVBT_KRF4]		= {0x1, 0xce,  7, 0},
+	[DVBT_EN_GI_PGA]	= {0x1, 0xe5,  0, 0},
+	[DVBT_THD_LOCK_UP]	= {0x1, 0xd9,  8, 0},
+	[DVBT_THD_LOCK_DW]	= {0x1, 0xdb,  8, 0},
+	[DVBT_THD_UP1]		= {0x1, 0xdd,  7, 0},
+	[DVBT_THD_DW1]		= {0x1, 0xde,  7, 0},
+	[DVBT_INTER_CNT_LEN]	= {0x1, 0xd8,  3, 0},
+	[DVBT_GI_PGA_STATE]	= {0x1, 0xe6,  3, 3},
+	[DVBT_EN_AGC_PGA]	= {0x1, 0xd7,  0, 0},
+	[DVBT_CKOUTPAR]		= {0x1, 0x7b,  5, 5},
+	[DVBT_CKOUT_PWR]	= {0x1, 0x7b,  6, 6},
+	[DVBT_SYNC_DUR]		= {0x1, 0x7b,  7, 7},
+	[DVBT_ERR_DUR]		= {0x1, 0x7c,  0, 0},
+	[DVBT_SYNC_LVL]		= {0x1, 0x7c,  1, 1},
+	[DVBT_ERR_LVL]		= {0x1, 0x7c,  2, 2},
+	[DVBT_VAL_LVL]		= {0x1, 0x7c,  3, 3},
+	[DVBT_SERIAL]		= {0x1, 0x7c,  4, 4},
+	[DVBT_SER_LSB]		= {0x1, 0x7c,  5, 5},
+	[DVBT_CDIV_PH0]		= {0x1, 0x7d,  3, 0},
+	[DVBT_CDIV_PH1]		= {0x1, 0x7d,  7, 4},
+	[DVBT_MPEG_IO_OPT_2_2]	= {0x0, 0x6,   7, 7},
+	[DVBT_MPEG_IO_OPT_1_0]	= {0x0, 0x7,   7, 6},
+	[DVBT_CKOUTPAR_PIP]	= {0x0, 0xb7,  4, 4},
+	[DVBT_CKOUT_PWR_PIP]	= {0x0, 0xb7,  3, 3},
+	[DVBT_SYNC_LVL_PIP]	= {0x0, 0xb7,  2, 2},
+	[DVBT_ERR_LVL_PIP]	= {0x0, 0xb7,  1, 1},
+	[DVBT_VAL_LVL_PIP]	= {0x0, 0xb7,  0, 0},
+	[DVBT_CKOUTPAR_PID]	= {0x0, 0xb9,  4, 4},
+	[DVBT_CKOUT_PWR_PID]	= {0x0, 0xb9,  3, 3},
+	[DVBT_SYNC_LVL_PID]	= {0x0, 0xb9,  2, 2},
+	[DVBT_ERR_LVL_PID]	= {0x0, 0xb9,  1, 1},
+	[DVBT_VAL_LVL_PID]	= {0x0, 0xb9,  0, 0},
+	[DVBT_SM_PASS]		= {0x1, 0x93, 11, 0},
+	[DVBT_AD7_SETTING]	= {0x0, 0x11, 15, 0},
+	[DVBT_RSSI_R]		= {0x3, 0x1,   6, 0},
+	[DVBT_ACI_DET_IND]	= {0x3, 0x12,  0, 0},
+	[DVBT_REG_MON]		= {0x0, 0xd,   1, 0},
+	[DVBT_REG_MONSEL]	= {0x0, 0xd,   2, 2},
+	[DVBT_REG_GPE]		= {0x0, 0xd,   7, 7},
+	[DVBT_REG_GPO]		= {0x0, 0x10,  0, 0},
+	[DVBT_REG_4MSEL]	= {0x0, 0x13,  0, 0},
+};
+
+/* write multiple hardware registers */
+static int rtl2832_wr(struct rtl2832_priv *priv, u8 reg, u8 *val, int len)
+{
+	int ret;
+	u8 buf[1+len];
+	struct i2c_msg msg[1] = {
+		{
+			.addr = priv->cfg.i2c_addr,
+			.flags = 0,
+			.len = 1+len,
+			.buf = buf,
+		}
+	};
+
+	buf[0] = reg;
+	memcpy(&buf[1], val, len);
+
+	ret = i2c_transfer(priv->i2c, msg, 1);
+	if (ret == 1) {
+		ret = 0;
+	} else {
+		warn("i2c wr failed=%d reg=%02x len=%d", ret, reg, len);
+		ret = -EREMOTEIO;
+	}
+	return ret;
+}
+
+/* read multiple hardware registers */
+static int rtl2832_rd(struct rtl2832_priv *priv, u8 reg, u8 *val, int len)
+{
+	int ret;
+	struct i2c_msg msg[2] = {
+		{
+			.addr = priv->cfg.i2c_addr,
+			.flags = 0,
+			.len = 1,
+			.buf = &reg,
+		}, {
+			.addr = priv->cfg.i2c_addr,
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = val,
+		}
+	};
+
+	ret = i2c_transfer(priv->i2c, msg, 2);
+	if (ret == 2) {
+		ret = 0;
+	} else {
+		warn("i2c rd failed=%d reg=%02x len=%d", ret, reg, len);
+		ret = -EREMOTEIO;
+}
+return ret;
+}
+
+/* write multiple registers */
+static int rtl2832_wr_regs(struct rtl2832_priv *priv, u8 reg, u8 page, u8 *val,
+	int len)
+{
+	int ret;
+
+
+	/* switch bank if needed */
+	if (page != priv->page) {
+		ret = rtl2832_wr(priv, 0x00, &page, 1);
+		if (ret)
+			return ret;
+
+		priv->page = page;
+}
+
+return rtl2832_wr(priv, reg, val, len);
+}
+
+/* read multiple registers */
+static int rtl2832_rd_regs(struct rtl2832_priv *priv, u8 reg, u8 page, u8 *val,
+	int len)
+{
+	int ret;
+
+	/* switch bank if needed */
+	if (page != priv->page) {
+		ret = rtl2832_wr(priv, 0x00, &page, 1);
+		if (ret)
+			return ret;
+
+		priv->page = page;
+	}
+
+	return rtl2832_rd(priv, reg, val, len);
+}
+
+#if 0 /* currently not used */
+/* write single register */
+static int rtl2832_wr_reg(struct rtl2832_priv *priv, u8 reg, u8 page, u8 val)
+{
+	return rtl2832_wr_regs(priv, reg, page, &val, 1);
+}
+#endif
+
+/* read single register */
+static int rtl2832_rd_reg(struct rtl2832_priv *priv, u8 reg, u8 page, u8 *val)
+{
+	return rtl2832_rd_regs(priv, reg, page, val, 1);
+}
+
+int rtl2832_rd_demod_reg(struct rtl2832_priv *priv, int reg, u32 *val)
+{
+	int ret;
+
+	u8 reg_start_addr;
+	u8 msb, lsb;
+	u8 page;
+	u8 reading[4];
+	u32 reading_tmp;
+	int i;
+
+	u8 len;
+	u32 mask;
+
+	reg_start_addr = registers[reg].start_address;
+	msb = registers[reg].msb;
+	lsb = registers[reg].lsb;
+	page = registers[reg].page;
+
+	len = (msb >> 3) + 1;
+	mask = REG_MASK(msb - lsb);
+
+	ret = rtl2832_rd_regs(priv, reg_start_addr, page, &reading[0], len);
+	if (ret)
+		goto err;
+
+	reading_tmp = 0;
+	for (i = 0; i < len; i++)
+		reading_tmp |= reading[i] << ((len - 1 - i) * 8);
+
+	*val = (reading_tmp >> lsb) & mask;
+
+	return ret;
+
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
+
+}
+
+int rtl2832_wr_demod_reg(struct rtl2832_priv *priv, int reg, u32 val)
+{
+	int ret, i;
+	u8 len;
+	u8 reg_start_addr;
+	u8 msb, lsb;
+	u8 page;
+	u32 mask;
+
+
+	u8 reading[4];
+	u8 writing[4];
+	u32 reading_tmp;
+	u32 writing_tmp;
+
+
+	reg_start_addr = registers[reg].start_address;
+	msb = registers[reg].msb;
+	lsb = registers[reg].lsb;
+	page = registers[reg].page;
+
+	len = (msb >> 3) + 1;
+	mask = REG_MASK(msb - lsb);
+
+
+	ret = rtl2832_rd_regs(priv, reg_start_addr, page, &reading[0], len);
+	if (ret)
+		goto err;
+
+	reading_tmp = 0;
+	for (i = 0; i < len; i++)
+		reading_tmp |= reading[i] << ((len - 1 - i) * 8);
+
+	writing_tmp = reading_tmp & ~(mask << lsb);
+	writing_tmp |= ((val & mask) << lsb);
+
+
+	for (i = 0; i < len; i++)
+		writing[i] = (writing_tmp >> ((len - 1 - i) * 8)) & 0xff;
+
+	ret = rtl2832_wr_regs(priv, reg_start_addr, page, &writing[0], len);
+	if (ret)
+		goto err;
+
+	return ret;
+
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
+
+}
+
+
+static int rtl2832_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
+{
+	int ret;
+	struct rtl2832_priv *priv = fe->demodulator_priv;
+
+	dbg("%s: enable=%d", __func__, enable);
+
+	/* gate already open or close */
+	if (priv->i2c_gate_state == enable)
+		return 0;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_IIC_REPEAT, (enable ? 0x1 : 0x0));
+	if (ret)
+		goto err;
+
+	priv->i2c_gate_state = enable;
+
+	return ret;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
+}
+
+
+
+static int rtl2832_init(struct dvb_frontend *fe)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
+	int i, ret;
+
+	u8 en_bbin;
+	u64 pset_iffreq;
+
+	/* initialization values for the demodulator registers */
+	struct rtl2832_reg_value rtl2832_initial_regs[] = {
+		{DVBT_AD_EN_REG,		0x1},
+		{DVBT_AD_EN_REG1,		0x1},
+		{DVBT_RSD_BER_FAIL_VAL,		0x2800},
+		{DVBT_MGD_THD0,			0x10},
+		{DVBT_MGD_THD1,			0x20},
+		{DVBT_MGD_THD2,			0x20},
+		{DVBT_MGD_THD3,			0x40},
+		{DVBT_MGD_THD4,			0x22},
+		{DVBT_MGD_THD5,			0x32},
+		{DVBT_MGD_THD6,			0x37},
+		{DVBT_MGD_THD7,			0x39},
+		{DVBT_EN_BK_TRK,		0x0},
+		{DVBT_EN_CACQ_NOTCH,		0x0},
+		{DVBT_AD_AV_REF,		0x2a},
+		{DVBT_REG_PI,			0x6},
+		{DVBT_PIP_ON,			0x0},
+		{DVBT_CDIV_PH0,			0x8},
+		{DVBT_CDIV_PH1,			0x8},
+		{DVBT_SCALE1_B92,		0x4},
+		{DVBT_SCALE1_B93,		0xb0},
+		{DVBT_SCALE1_BA7,		0x78},
+		{DVBT_SCALE1_BA9,		0x28},
+		{DVBT_SCALE1_BAA,		0x59},
+		{DVBT_SCALE1_BAB,		0x83},
+		{DVBT_SCALE1_BAC,		0xd4},
+		{DVBT_SCALE1_BB0,		0x65},
+		{DVBT_SCALE1_BB1,		0x43},
+		{DVBT_KB_P1,			0x1},
+		{DVBT_KB_P2,			0x4},
+		{DVBT_KB_P3,			0x7},
+		{DVBT_K1_CR_STEP12,		0xa},
+		{DVBT_REG_GPE,			0x1},
+		{DVBT_SERIAL,			0x0},
+		{DVBT_CDIV_PH0,			0x9},
+		{DVBT_CDIV_PH1,			0x9},
+		{DVBT_MPEG_IO_OPT_2_2,		0x0},
+		{DVBT_MPEG_IO_OPT_1_0,		0x0},
+		{DVBT_TRK_KS_P2,		0x4},
+		{DVBT_TRK_KS_I2,		0x7},
+		{DVBT_TR_THD_SET2,		0x6},
+		{DVBT_TRK_KC_I2,		0x5},
+		{DVBT_CR_THD_SET2,		0x1},
+		{DVBT_SPEC_INV,			0x0},
+		{DVBT_DAGC_TRG_VAL,		0x5a},
+		{DVBT_AGC_TARG_VAL_0,		0x0},
+		{DVBT_AGC_TARG_VAL_8_1,		0x5a},
+		{DVBT_AAGC_LOOP_GAIN,		0x16},
+		{DVBT_LOOP_GAIN2_3_0,		0x6},
+		{DVBT_LOOP_GAIN2_4,		0x1},
+		{DVBT_LOOP_GAIN3,		0x16},
+		{DVBT_VTOP1,			0x35},
+		{DVBT_VTOP2,			0x21},
+		{DVBT_VTOP3,			0x21},
+		{DVBT_KRF1,			0x0},
+		{DVBT_KRF2,			0x40},
+		{DVBT_KRF3,			0x10},
+		{DVBT_KRF4,			0x10},
+		{DVBT_IF_AGC_MIN,		0x80},
+		{DVBT_IF_AGC_MAX,		0x7f},
+		{DVBT_RF_AGC_MIN,		0x80},
+		{DVBT_RF_AGC_MAX,		0x7f},
+		{DVBT_POLAR_RF_AGC,		0x0},
+		{DVBT_POLAR_IF_AGC,		0x0},
+		{DVBT_AD7_SETTING,		0xe9bf},
+		{DVBT_EN_GI_PGA,		0x0},
+		{DVBT_THD_LOCK_UP,		0x0},
+		{DVBT_THD_LOCK_DW,		0x0},
+		{DVBT_THD_UP1,			0x11},
+		{DVBT_THD_DW1,			0xef},
+		{DVBT_INTER_CNT_LEN,		0xc},
+		{DVBT_GI_PGA_STATE,		0x0},
+		{DVBT_EN_AGC_PGA,		0x1},
+		{DVBT_IF_AGC_MAN,		0x0},
+	};
+
+
+	dbg("%s", __func__);
+
+	en_bbin = (priv->cfg.if_dvbt == 0 ? 0x1 : 0x0);
+
+	/*
+	* PSET_IFFREQ = - floor((IfFreqHz % CrystalFreqHz) * pow(2, 22)
+	*		/ CrystalFreqHz)
+	*/
+	pset_iffreq = priv->cfg.if_dvbt % priv->cfg.xtal;
+	pset_iffreq *= 0x400000;
+	pset_iffreq = div_u64(pset_iffreq, priv->cfg.xtal);
+	pset_iffreq = pset_iffreq & 0x3fffff;
+
+
+
+	for (i = 0; i < ARRAY_SIZE(rtl2832_initial_regs); i++) {
+		ret = rtl2832_wr_demod_reg(priv, rtl2832_initial_regs[i].reg,
+			rtl2832_initial_regs[i].value);
+		if (ret)
+			goto err;
+	}
+
+	/* if frequency settings */
+	ret = rtl2832_wr_demod_reg(priv, DVBT_EN_BBIN, en_bbin);
+		if (ret)
+			goto err;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_PSET_IFFREQ, pset_iffreq);
+		if (ret)
+			goto err;
+
+	priv->sleeping = false;
+
+	return ret;
+
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	return ret;
+}
+
+static int rtl2832_sleep(struct dvb_frontend *fe)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
+
+	dbg("%s", __func__);
+	priv->sleeping = true;
+	return 0;
+}
+
+int rtl2832_get_tune_settings(struct dvb_frontend *fe,
+	struct dvb_frontend_tune_settings *s)
+{
+	dbg("%s", __func__);
+	s->min_delay_ms = 1000;
+	s->step_size = fe->ops.info.frequency_stepsize * 2;
+	s->max_drift = (fe->ops.info.frequency_stepsize * 2) + 1;
+	return 0;
+}
+
+static int rtl2832_set_frontend(struct dvb_frontend *fe)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret, i, j;
+	u64 bw_mode, num, num2;
+	u32 resamp_ratio, cfreq_off_ratio;
+
+
+	static u8 bw_params[3][32] = {
+	/* 6 MHz bandwidth */
+		{
+		0xf5, 0xff, 0x15, 0x38, 0x5d, 0x6d, 0x52, 0x07, 0xfa, 0x2f,
+		0x53, 0xf5, 0x3f, 0xca, 0x0b, 0x91, 0xea, 0x30, 0x63, 0xb2,
+		0x13, 0xda, 0x0b, 0xc4, 0x18, 0x7e, 0x16, 0x66, 0x08, 0x67,
+		0x19, 0xe0,
+		},
+
+	/*  7 MHz bandwidth */
+		{
+		0xe7, 0xcc, 0xb5, 0xba, 0xe8, 0x2f, 0x67, 0x61, 0x00, 0xaf,
+		0x86, 0xf2, 0xbf, 0x59, 0x04, 0x11, 0xb6, 0x33, 0xa4, 0x30,
+		0x15, 0x10, 0x0a, 0x42, 0x18, 0xf8, 0x17, 0xd9, 0x07, 0x22,
+		0x19, 0x10,
+		},
+
+	/*  8 MHz bandwidth */
+		{
+		0x09, 0xf6, 0xd2, 0xa7, 0x9a, 0xc9, 0x27, 0x77, 0x06, 0xbf,
+		0xec, 0xf4, 0x4f, 0x0b, 0xfc, 0x01, 0x63, 0x35, 0x54, 0xa7,
+		0x16, 0x66, 0x08, 0xb4, 0x19, 0x6e, 0x19, 0x65, 0x05, 0xc8,
+		0x19, 0xe0,
+		},
+	};
+
+
+	dbg("%s: frequency=%d bandwidth_hz=%d inversion=%d", __func__,
+		c->frequency, c->bandwidth_hz, c->inversion);
+
+
+	/* program tuner */
+	if (fe->ops.tuner_ops.set_params)
+		fe->ops.tuner_ops.set_params(fe);
+
+
+	switch (c->bandwidth_hz) {
+	case 6000000:
+		i = 0;
+		bw_mode = 48000000;
+		break;
+	case 7000000:
+		i = 1;
+		bw_mode = 56000000;
+		break;
+	case 8000000:
+		i = 2;
+		bw_mode = 64000000;
+		break;
+	default:
+		dbg("invalid bandwidth");
+		return -EINVAL;
+	}
+
+	for (j = 0; j < sizeof(bw_params[0]); j++) {
+		ret = rtl2832_wr_regs(priv, 0x1c+j, 1, &bw_params[i][j], 1);
+		if (ret)
+			goto err;
+	}
+
+	/* calculate and set resample ratio
+	* RSAMP_RATIO = floor(CrystalFreqHz * 7 * pow(2, 22)
+	*	/ ConstWithBandwidthMode)
+	*/
+	num = priv->cfg.xtal * 7;
+	num *= 0x400000;
+	num = div_u64(num, bw_mode);
+	resamp_ratio =  num & 0x3ffffff;
+	ret = rtl2832_wr_demod_reg(priv, DVBT_RSAMP_RATIO, resamp_ratio);
+	if (ret)
+		goto err;
+
+	/* calculate and set cfreq off ratio
+	* CFREQ_OFF_RATIO = - floor(ConstWithBandwidthMode * pow(2, 20)
+	*	/ (CrystalFreqHz * 7))
+	*/
+	num = bw_mode << 20;
+	num2 = priv->cfg.xtal * 7;
+	num = div_u64(num, num2);
+	num = -num;
+	cfreq_off_ratio = num & 0xfffff;
+	ret = rtl2832_wr_demod_reg(priv, DVBT_CFREQ_OFF_RATIO, cfreq_off_ratio);
+	if (ret)
+		goto err;
+
+
+	/* soft reset */
+	ret = rtl2832_wr_demod_reg(priv, DVBT_SOFT_RST, 0x1);
+	if (ret)
+		goto err;
+
+	ret = rtl2832_wr_demod_reg(priv, DVBT_SOFT_RST, 0x0);
+	if (ret)
+		goto err;
+
+	return ret;
+err:
+	info("%s: failed=%d", __func__, ret);
+	return ret;
+}
+
+static int rtl2832_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
+	int ret;
+	u32 tmp;
+	*status = 0;
+
+
+	dbg("%s", __func__);
+	if (priv->sleeping)
+		return 0;
+
+	ret = rtl2832_rd_demod_reg(priv, DVBT_FSM_STAGE, &tmp);
+	if (ret)
+		goto err;
+
+	if (tmp == 11) {
+		*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
+				FE_HAS_VITERBI | FE_HAS_SYNC | FE_HAS_LOCK;
+	}
+	/* TODO find out if this is also true for rtl2832? */
+	/*else if (tmp == 10) {
+		*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER |
+				FE_HAS_VITERBI;
+	}*/
+
+	return ret;
+err:
+	info("%s: failed=%d", __func__, ret);
+	return ret;
+}
+
+static int rtl2832_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	*snr = 0;
+	return 0;
+}
+
+static int rtl2832_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	*ber = 0;
+	return 0;
+}
+
+static int rtl2832_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	*ucblocks = 0;
+	return 0;
+}
+
+
+static int rtl2832_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
+{
+	*strength = 0;
+	return 0;
+}
+
+static struct dvb_frontend_ops rtl2832_ops;
+
+static void rtl2832_release(struct dvb_frontend *fe)
+{
+	struct rtl2832_priv *priv = fe->demodulator_priv;
+
+	dbg("%s", __func__);
+	kfree(priv);
+}
+
+struct dvb_frontend *rtl2832_attach(const struct rtl2832_config *cfg,
+	struct i2c_adapter *i2c)
+{
+	struct rtl2832_priv *priv = NULL;
+	int ret = 0;
+	u8 tmp;
+
+	dbg("%s", __func__);
+
+	/* allocate memory for the internal state */
+	priv = kzalloc(sizeof(struct rtl2832_priv), GFP_KERNEL);
+	if (priv == NULL)
+		goto err;
+
+	/* setup the priv */
+	priv->i2c = i2c;
+	priv->tuner = cfg->tuner;
+	memcpy(&priv->cfg, cfg, sizeof(struct rtl2832_config));
+
+	/* check if the demod is there */
+	ret = rtl2832_rd_reg(priv, 0x00, 0x0, &tmp);
+	if (ret)
+		goto err;
+
+	/* create dvb_frontend */
+	memcpy(&priv->fe.ops, &rtl2832_ops, sizeof(struct dvb_frontend_ops));
+	priv->fe.demodulator_priv = priv;
+
+	/* TODO implement sleep mode */
+	priv->sleeping = true;
+
+	return &priv->fe;
+err:
+	dbg("%s: failed=%d", __func__, ret);
+	kfree(priv);
+	return NULL;
+}
+EXPORT_SYMBOL(rtl2832_attach);
+
+static struct dvb_frontend_ops rtl2832_ops = {
+	.delsys = { SYS_DVBT },
+	.info = {
+		.name = "Realtek RTL2832 (DVB-T)",
+		.frequency_min	  = 174000000,
+		.frequency_max	  = 862000000,
+		.frequency_stepsize = 166667,
+		.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_AUTO |
+			FE_CAN_TRANSMISSION_MODE_AUTO |
+			FE_CAN_GUARD_INTERVAL_AUTO |
+			FE_CAN_HIERARCHY_AUTO |
+			FE_CAN_RECOVER |
+			FE_CAN_MUTE_TS
+	 },
+
+	.release = rtl2832_release,
+
+	.init = rtl2832_init,
+	.sleep = rtl2832_sleep,
+
+	.get_tune_settings = rtl2832_get_tune_settings,
+
+	.set_frontend = rtl2832_set_frontend,
+
+	.read_status = rtl2832_read_status,
+	.read_snr = rtl2832_read_snr,
+	.read_ber = rtl2832_read_ber,
+	.read_ucblocks = rtl2832_read_ucblocks,
+	.read_signal_strength = rtl2832_read_signal_strength,
+	.i2c_gate_ctrl = rtl2832_i2c_gate_ctrl,
+};
+
+MODULE_AUTHOR("Thomas Mair <mair.thomas86@gmail.com>");
+MODULE_DESCRIPTION("Realtek RTL2832 DVB-T demodulator driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.5");
diff --git a/drivers/media/dvb/frontends/rtl2832.h b/drivers/media/dvb/frontends/rtl2832.h
new file mode 100644
index 00000000000..d94dc9a3fa6
--- /dev/null
+++ b/drivers/media/dvb/frontends/rtl2832.h
@@ -0,0 +1,74 @@
+/*
+ * Realtek RTL2832 DVB-T demodulator driver
+ *
+ * Copyright (C) 2012 Thomas Mair <thomas.mair86@gmail.com>
+ *
+ *	This program is free software; you can redistribute it and/or modify
+ *	it under the terms of the GNU General Public License as published by
+ *	the Free Software Foundation; either version 2 of the License, or
+ *	(at your option) any later version.
+ *
+ *	This program is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *	GNU General Public License for more details.
+ *
+ *	You should have received a copy of the GNU General Public License along
+ *	with this program; if not, write to the Free Software Foundation, Inc.,
+ *	51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef RTL2832_H
+#define RTL2832_H
+
+#include <linux/dvb/frontend.h>
+
+struct rtl2832_config {
+	/*
+	 * Demodulator I2C address.
+	 */
+	u8 i2c_addr;
+
+	/*
+	 * Xtal frequency.
+	 * Hz
+	 * 4000000, 16000000, 25000000, 28800000
+	 */
+	u32 xtal;
+
+	/*
+	 * IFs for all used modes.
+	 * Hz
+	 * 4570000, 4571429, 36000000, 36125000, 36166667, 44000000
+	 */
+	u32 if_dvbt;
+
+	/*
+	 */
+	u8 tuner;
+};
+
+
+#if defined(CONFIG_DVB_RTL2832) || \
+	(defined(CONFIG_DVB_RTL2832_MODULE) && defined(MODULE))
+extern struct dvb_frontend *rtl2832_attach(
+	const struct rtl2832_config *cfg,
+	struct i2c_adapter *i2c
+);
+
+extern struct i2c_adapter *rtl2832_get_tuner_i2c_adapter(
+	struct dvb_frontend *fe
+);
+#else
+static inline struct dvb_frontend *rtl2832_attach(
+	const struct rtl2832_config *config,
+	struct i2c_adapter *i2c
+)
+{
+	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+
+#endif /* RTL2832_H */
diff --git a/drivers/media/dvb/frontends/rtl2832_priv.h b/drivers/media/dvb/frontends/rtl2832_priv.h
new file mode 100644
index 00000000000..0ce9502da8b
--- /dev/null
+++ b/drivers/media/dvb/frontends/rtl2832_priv.h
@@ -0,0 +1,260 @@
+/*
+ * Realtek RTL2832 DVB-T demodulator driver
+ *
+ * Copyright (C) 2012 Thomas Mair <thomas.mair86@gmail.com>
+ *
+ *	This program is free software; you can redistribute it and/or modify
+ *	it under the terms of the GNU General Public License as published by
+ *	the Free Software Foundation; either version 2 of the License, or
+ *	(at your option) any later version.
+ *
+ *	This program is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *	GNU General Public License for more details.
+ *
+ *	You should have received a copy of the GNU General Public License along
+ *	with this program; if not, write to the Free Software Foundation, Inc.,
+ *	51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef RTL2832_PRIV_H
+#define RTL2832_PRIV_H
+
+#include "dvb_frontend.h"
+#include "rtl2832.h"
+
+#define LOG_PREFIX "rtl2832"
+
+#undef dbg
+#define dbg(f, arg...) \
+do { \
+	if (rtl2832_debug)  \
+		printk(KERN_INFO     LOG_PREFIX": " f "\n" , ## arg); \
+} while (0)
+#undef err
+#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
+#undef info
+#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
+#undef warn
+#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
+
+struct rtl2832_priv {
+	struct i2c_adapter *i2c;
+	struct dvb_frontend fe;
+	struct rtl2832_config cfg;
+
+	bool i2c_gate_state;
+	bool sleeping;
+
+	u8 tuner;
+	u8 page; /* active register page */
+};
+
+struct rtl2832_reg_entry {
+	u8 page;
+	u8 start_address;
+	u8 msb;
+	u8 lsb;
+};
+
+struct rtl2832_reg_value {
+	int reg;
+	u32 value;
+};
+
+
+/* Demod register bit names */
+enum DVBT_REG_BIT_NAME {
+	DVBT_SOFT_RST,
+	DVBT_IIC_REPEAT,
+	DVBT_TR_WAIT_MIN_8K,
+	DVBT_RSD_BER_FAIL_VAL,
+	DVBT_EN_BK_TRK,
+	DVBT_REG_PI,
+	DVBT_REG_PFREQ_1_0,
+	DVBT_PD_DA8,
+	DVBT_LOCK_TH,
+	DVBT_BER_PASS_SCAL,
+	DVBT_CE_FFSM_BYPASS,
+	DVBT_ALPHAIIR_N,
+	DVBT_ALPHAIIR_DIF,
+	DVBT_EN_TRK_SPAN,
+	DVBT_LOCK_TH_LEN,
+	DVBT_CCI_THRE,
+	DVBT_CCI_MON_SCAL,
+	DVBT_CCI_M0,
+	DVBT_CCI_M1,
+	DVBT_CCI_M2,
+	DVBT_CCI_M3,
+	DVBT_SPEC_INIT_0,
+	DVBT_SPEC_INIT_1,
+	DVBT_SPEC_INIT_2,
+	DVBT_AD_EN_REG,
+	DVBT_AD_EN_REG1,
+	DVBT_EN_BBIN,
+	DVBT_MGD_THD0,
+	DVBT_MGD_THD1,
+	DVBT_MGD_THD2,
+	DVBT_MGD_THD3,
+	DVBT_MGD_THD4,
+	DVBT_MGD_THD5,
+	DVBT_MGD_THD6,
+	DVBT_MGD_THD7,
+	DVBT_EN_CACQ_NOTCH,
+	DVBT_AD_AV_REF,
+	DVBT_PIP_ON,
+	DVBT_SCALE1_B92,
+	DVBT_SCALE1_B93,
+	DVBT_SCALE1_BA7,
+	DVBT_SCALE1_BA9,
+	DVBT_SCALE1_BAA,
+	DVBT_SCALE1_BAB,
+	DVBT_SCALE1_BAC,
+	DVBT_SCALE1_BB0,
+	DVBT_SCALE1_BB1,
+	DVBT_KB_P1,
+	DVBT_KB_P2,
+	DVBT_KB_P3,
+	DVBT_OPT_ADC_IQ,
+	DVBT_AD_AVI,
+	DVBT_AD_AVQ,
+	DVBT_K1_CR_STEP12,
+	DVBT_TRK_KS_P2,
+	DVBT_TRK_KS_I2,
+	DVBT_TR_THD_SET2,
+	DVBT_TRK_KC_P2,
+	DVBT_TRK_KC_I2,
+	DVBT_CR_THD_SET2,
+	DVBT_PSET_IFFREQ,
+	DVBT_SPEC_INV,
+	DVBT_BW_INDEX,
+	DVBT_RSAMP_RATIO,
+	DVBT_CFREQ_OFF_RATIO,
+	DVBT_FSM_STAGE,
+	DVBT_RX_CONSTEL,
+	DVBT_RX_HIER,
+	DVBT_RX_C_RATE_LP,
+	DVBT_RX_C_RATE_HP,
+	DVBT_GI_IDX,
+	DVBT_FFT_MODE_IDX,
+	DVBT_RSD_BER_EST,
+	DVBT_CE_EST_EVM,
+	DVBT_RF_AGC_VAL,
+	DVBT_IF_AGC_VAL,
+	DVBT_DAGC_VAL,
+	DVBT_SFREQ_OFF,
+	DVBT_CFREQ_OFF,
+	DVBT_POLAR_RF_AGC,
+	DVBT_POLAR_IF_AGC,
+	DVBT_AAGC_HOLD,
+	DVBT_EN_RF_AGC,
+	DVBT_EN_IF_AGC,
+	DVBT_IF_AGC_MIN,
+	DVBT_IF_AGC_MAX,
+	DVBT_RF_AGC_MIN,
+	DVBT_RF_AGC_MAX,
+	DVBT_IF_AGC_MAN,
+	DVBT_IF_AGC_MAN_VAL,
+	DVBT_RF_AGC_MAN,
+	DVBT_RF_AGC_MAN_VAL,
+	DVBT_DAGC_TRG_VAL,
+	DVBT_AGC_TARG_VAL,
+	DVBT_LOOP_GAIN_3_0,
+	DVBT_LOOP_GAIN_4,
+	DVBT_VTOP,
+	DVBT_KRF,
+	DVBT_AGC_TARG_VAL_0,
+	DVBT_AGC_TARG_VAL_8_1,
+	DVBT_AAGC_LOOP_GAIN,
+	DVBT_LOOP_GAIN2_3_0,
+	DVBT_LOOP_GAIN2_4,
+	DVBT_LOOP_GAIN3,
+	DVBT_VTOP1,
+	DVBT_VTOP2,
+	DVBT_VTOP3,
+	DVBT_KRF1,
+	DVBT_KRF2,
+	DVBT_KRF3,
+	DVBT_KRF4,
+	DVBT_EN_GI_PGA,
+	DVBT_THD_LOCK_UP,
+	DVBT_THD_LOCK_DW,
+	DVBT_THD_UP1,
+	DVBT_THD_DW1,
+	DVBT_INTER_CNT_LEN,
+	DVBT_GI_PGA_STATE,
+	DVBT_EN_AGC_PGA,
+	DVBT_CKOUTPAR,
+	DVBT_CKOUT_PWR,
+	DVBT_SYNC_DUR,
+	DVBT_ERR_DUR,
+	DVBT_SYNC_LVL,
+	DVBT_ERR_LVL,
+	DVBT_VAL_LVL,
+	DVBT_SERIAL,
+	DVBT_SER_LSB,
+	DVBT_CDIV_PH0,
+	DVBT_CDIV_PH1,
+	DVBT_MPEG_IO_OPT_2_2,
+	DVBT_MPEG_IO_OPT_1_0,
+	DVBT_CKOUTPAR_PIP,
+	DVBT_CKOUT_PWR_PIP,
+	DVBT_SYNC_LVL_PIP,
+	DVBT_ERR_LVL_PIP,
+	DVBT_VAL_LVL_PIP,
+	DVBT_CKOUTPAR_PID,
+	DVBT_CKOUT_PWR_PID,
+	DVBT_SYNC_LVL_PID,
+	DVBT_ERR_LVL_PID,
+	DVBT_VAL_LVL_PID,
+	DVBT_SM_PASS,
+	DVBT_UPDATE_REG_2,
+	DVBT_BTHD_P3,
+	DVBT_BTHD_D3,
+	DVBT_FUNC4_REG0,
+	DVBT_FUNC4_REG1,
+	DVBT_FUNC4_REG2,
+	DVBT_FUNC4_REG3,
+	DVBT_FUNC4_REG4,
+	DVBT_FUNC4_REG5,
+	DVBT_FUNC4_REG6,
+	DVBT_FUNC4_REG7,
+	DVBT_FUNC4_REG8,
+	DVBT_FUNC4_REG9,
+	DVBT_FUNC4_REG10,
+	DVBT_FUNC5_REG0,
+	DVBT_FUNC5_REG1,
+	DVBT_FUNC5_REG2,
+	DVBT_FUNC5_REG3,
+	DVBT_FUNC5_REG4,
+	DVBT_FUNC5_REG5,
+	DVBT_FUNC5_REG6,
+	DVBT_FUNC5_REG7,
+	DVBT_FUNC5_REG8,
+	DVBT_FUNC5_REG9,
+	DVBT_FUNC5_REG10,
+	DVBT_FUNC5_REG11,
+	DVBT_FUNC5_REG12,
+	DVBT_FUNC5_REG13,
+	DVBT_FUNC5_REG14,
+	DVBT_FUNC5_REG15,
+	DVBT_FUNC5_REG16,
+	DVBT_FUNC5_REG17,
+	DVBT_FUNC5_REG18,
+	DVBT_AD7_SETTING,
+	DVBT_RSSI_R,
+	DVBT_ACI_DET_IND,
+	DVBT_REG_MON,
+	DVBT_REG_MONSEL,
+	DVBT_REG_GPE,
+	DVBT_REG_GPO,
+	DVBT_REG_4MSEL,
+	DVBT_TEST_REG_1,
+	DVBT_TEST_REG_2,
+	DVBT_TEST_REG_3,
+	DVBT_TEST_REG_4,
+	DVBT_REG_BIT_NAME_ITEM_TERMINATOR,
+};
+
+#endif /* RTL2832_PRIV_H */
diff --git a/drivers/media/dvb/frontends/s5h1420.c b/drivers/media/dvb/frontends/s5h1420.c
index 2322257c69a..e2fec9ebf94 100644
--- a/drivers/media/dvb/frontends/s5h1420.c
+++ b/drivers/media/dvb/frontends/s5h1420.c
@@ -634,7 +634,6 @@ static int s5h1420_set_frontend(struct dvb_frontend *fe)
 	struct s5h1420_state* state = fe->demodulator_priv;
 	int frequency_delta;
 	struct dvb_frontend_tune_settings fesettings;
-	uint8_t clock_setting;
 
 	dprintk("enter %s\n", __func__);
 
@@ -679,25 +678,6 @@ static int s5h1420_set_frontend(struct dvb_frontend *fe)
 	else
 		state->fclk = 44000000;
 
-	/* Clock */
-	switch (state->fclk) {
-	default:
-	case 88000000:
-		clock_setting = 80;
-		break;
-	case 86000000:
-		clock_setting = 78;
-		break;
-	case 80000000:
-		clock_setting = 72;
-		break;
-	case 59000000:
-		clock_setting = 51;
-		break;
-	case 44000000:
-		clock_setting = 36;
-		break;
-	}
 	dprintk("pll01: %d, ToneFreq: %d\n", state->fclk/1000000 - 8, (state->fclk + (TONE_FREQ * 32) - 1) / (TONE_FREQ * 32));
 	s5h1420_writereg(state, PLL01, state->fclk/1000000 - 8);
 	s5h1420_writereg(state, PLL02, 0x40);
diff --git a/drivers/media/dvb/frontends/stb0899_drv.c b/drivers/media/dvb/frontends/stb0899_drv.c
index 8b0dc74a329..5d7f8a9b451 100644
--- a/drivers/media/dvb/frontends/stb0899_drv.c
+++ b/drivers/media/dvb/frontends/stb0899_drv.c
@@ -1129,7 +1129,6 @@ static int stb0899_read_ber(struct dvb_frontend *fe, u32 *ber)
 	struct stb0899_internal *internal	= &state->internal;
 
 	u8  lsb, msb;
-	u32 i;
 
 	*ber = 0;
 
@@ -1137,14 +1136,9 @@ static int stb0899_read_ber(struct dvb_frontend *fe, u32 *ber)
 	case SYS_DVBS:
 	case SYS_DSS:
 		if (internal->lock) {
-			/* average 5 BER values	*/
-			for (i = 0; i < 5; i++) {
-				msleep(100);
-				lsb = stb0899_read_reg(state, STB0899_ECNT1L);
-				msb = stb0899_read_reg(state, STB0899_ECNT1M);
-				*ber += MAKEWORD16(msb, lsb);
-			}
-			*ber /= 5;
+			lsb = stb0899_read_reg(state, STB0899_ECNT1L);
+			msb = stb0899_read_reg(state, STB0899_ECNT1M);
+			*ber = MAKEWORD16(msb, lsb);
 			/* Viterbi Check	*/
 			if (STB0899_GETFIELD(VSTATUS_PRFVIT, internal->v_status)) {
 				/* Error Rate		*/
@@ -1157,13 +1151,9 @@ static int stb0899_read_ber(struct dvb_frontend *fe, u32 *ber)
 		break;
 	case SYS_DVBS2:
 		if (internal->lock) {
-			/* Average 5 PER values	*/
-			for (i = 0; i < 5; i++) {
-				msleep(100);
-				lsb = stb0899_read_reg(state, STB0899_ECNT1L);
-				msb = stb0899_read_reg(state, STB0899_ECNT1M);
-				*ber += MAKEWORD16(msb, lsb);
-			}
+			lsb = stb0899_read_reg(state, STB0899_ECNT1L);
+			msb = stb0899_read_reg(state, STB0899_ECNT1M);
+			*ber = MAKEWORD16(msb, lsb);
 			/* ber = ber * 10 ^ 7	*/
 			*ber *= 10000000;
 			*ber /= (-1 + (1 << (4 + 2 * STB0899_GETFIELD(NOE, internal->err_ctrl))));
diff --git a/drivers/media/dvb/frontends/stv0367.c b/drivers/media/dvb/frontends/stv0367.c
index fdd20c7737b..2a8aaeb1112 100644
--- a/drivers/media/dvb/frontends/stv0367.c
+++ b/drivers/media/dvb/frontends/stv0367.c
@@ -1584,7 +1584,7 @@ static int stv0367ter_algo(struct dvb_frontend *fe)
 	struct stv0367ter_state *ter_state = state->ter_state;
 	int offset = 0, tempo = 0;
 	u8 u_var;
-	u8 /*constell,*/ counter, tps_rcvd[2];
+	u8 /*constell,*/ counter;
 	s8 step;
 	s32 timing_offset = 0;
 	u32 trl_nomrate = 0, InternalFreq = 0, temp = 0;
@@ -1709,9 +1709,6 @@ static int stv0367ter_algo(struct dvb_frontend *fe)
 		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);
diff --git a/drivers/media/dvb/frontends/stv090x.c b/drivers/media/dvb/frontends/stv090x.c
index d79e69f65cb..ea86a5603e5 100644
--- a/drivers/media/dvb/frontends/stv090x.c
+++ b/drivers/media/dvb/frontends/stv090x.c
@@ -3172,7 +3172,7 @@ static enum stv090x_signal_state stv090x_algo(struct stv090x_state *state)
 	enum stv090x_signal_state signal_state = STV090x_NOCARRIER;
 	u32 reg;
 	s32 agc1_power, power_iq = 0, i;
-	int lock = 0, low_sr = 0, no_signal = 0;
+	int lock = 0, low_sr = 0;
 
 	reg = STV090x_READ_DEMOD(state, TSCFGH);
 	STV090x_SETFIELD_Px(reg, RST_HWARE_FIELD, 1); /* Stop path 1 stream merger */
@@ -3413,7 +3413,7 @@ static enum stv090x_signal_state stv090x_algo(struct stv090x_state *state)
 				goto err;
 		} else {
 			signal_state = STV090x_NODATA;
-			no_signal = stv090x_chk_signal(state);
+			stv090x_chk_signal(state);
 		}
 	}
 	return signal_state;
diff --git a/drivers/media/dvb/frontends/tda10071.c b/drivers/media/dvb/frontends/tda10071.c
index c21bc92d281..703c3d05f9f 100644
--- a/drivers/media/dvb/frontends/tda10071.c
+++ b/drivers/media/dvb/frontends/tda10071.c
@@ -20,10 +20,6 @@
 
 #include "tda10071_priv.h"
 
-int tda10071_debug;
-module_param_named(debug, tda10071_debug, int, 0644);
-MODULE_PARM_DESC(debug, "Turn on/off frontend debugging (default:off).");
-
 static struct dvb_frontend_ops tda10071_ops;
 
 /* write multiple registers */
@@ -48,7 +44,8 @@ static int tda10071_wr_regs(struct tda10071_priv *priv, u8 reg, u8 *val,
 	if (ret == 1) {
 		ret = 0;
 	} else {
-		warn("i2c wr failed=%d reg=%02x len=%d", ret, reg, len);
+		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d reg=%02x " \
+				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
 		ret = -EREMOTEIO;
 	}
 	return ret;
@@ -79,7 +76,8 @@ static int tda10071_rd_regs(struct tda10071_priv *priv, u8 reg, u8 *val,
 		memcpy(val, buf, len);
 		ret = 0;
 	} else {
-		warn("i2c rd failed=%d reg=%02x len=%d", ret, reg, len);
+		dev_warn(&priv->i2c->dev, "%s: i2c rd failed=%d reg=%02x " \
+				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
 		ret = -EREMOTEIO;
 	}
 	return ret;
@@ -170,7 +168,7 @@ static int tda10071_cmd_execute(struct tda10071_priv *priv,
 		usleep_range(200, 5000);
 	}
 
-	dbg("%s: loop=%d", __func__, i);
+	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
 
 	if (i == 0) {
 		ret = -ETIMEDOUT;
@@ -179,7 +177,7 @@ static int tda10071_cmd_execute(struct tda10071_priv *priv,
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -196,7 +194,8 @@ static int tda10071_set_tone(struct dvb_frontend *fe,
 		goto error;
 	}
 
-	dbg("%s: tone_mode=%d", __func__, fe_sec_tone_mode);
+	dev_dbg(&priv->i2c->dev, "%s: tone_mode=%d\n", __func__,
+			fe_sec_tone_mode);
 
 	switch (fe_sec_tone_mode) {
 	case SEC_TONE_ON:
@@ -206,24 +205,25 @@ static int tda10071_set_tone(struct dvb_frontend *fe,
 		tone = 0;
 		break;
 	default:
-		dbg("%s: invalid fe_sec_tone_mode", __func__);
+		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_tone_mode\n",
+				__func__);
 		ret = -EINVAL;
 		goto error;
 	}
 
-	cmd.args[0x00] = CMD_LNB_PCB_CONFIG;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = 0x00;
-	cmd.args[0x03] = 0x00;
-	cmd.args[0x04] = tone;
-	cmd.len = 0x05;
+	cmd.args[0] = CMD_LNB_PCB_CONFIG;
+	cmd.args[1] = 0;
+	cmd.args[2] = 0x00;
+	cmd.args[3] = 0x00;
+	cmd.args[4] = tone;
+	cmd.len = 5;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -240,7 +240,7 @@ static int tda10071_set_voltage(struct dvb_frontend *fe,
 		goto error;
 	}
 
-	dbg("%s: voltage=%d", __func__, fe_sec_voltage);
+	dev_dbg(&priv->i2c->dev, "%s: voltage=%d\n", __func__, fe_sec_voltage);
 
 	switch (fe_sec_voltage) {
 	case SEC_VOLTAGE_13:
@@ -253,22 +253,23 @@ static int tda10071_set_voltage(struct dvb_frontend *fe,
 		voltage = 0;
 		break;
 	default:
-		dbg("%s: invalid fe_sec_voltage", __func__);
+		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_voltage\n",
+				__func__);
 		ret = -EINVAL;
 		goto error;
 	};
 
-	cmd.args[0x00] = CMD_LNB_SET_DC_LEVEL;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = voltage;
-	cmd.len = 0x03;
+	cmd.args[0] = CMD_LNB_SET_DC_LEVEL;
+	cmd.args[1] = 0;
+	cmd.args[2] = voltage;
+	cmd.len = 3;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -285,9 +286,10 @@ static int tda10071_diseqc_send_master_cmd(struct dvb_frontend *fe,
 		goto error;
 	}
 
-	dbg("%s: msg_len=%d", __func__, diseqc_cmd->msg_len);
+	dev_dbg(&priv->i2c->dev, "%s: msg_len=%d\n", __func__,
+			diseqc_cmd->msg_len);
 
-	if (diseqc_cmd->msg_len < 3 || diseqc_cmd->msg_len > 16) {
+	if (diseqc_cmd->msg_len < 3 || diseqc_cmd->msg_len > 6) {
 		ret = -EINVAL;
 		goto error;
 	}
@@ -301,7 +303,7 @@ static int tda10071_diseqc_send_master_cmd(struct dvb_frontend *fe,
 		usleep_range(10000, 20000);
 	}
 
-	dbg("%s: loop=%d", __func__, i);
+	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
 
 	if (i == 0) {
 		ret = -ETIMEDOUT;
@@ -312,22 +314,22 @@ static int tda10071_diseqc_send_master_cmd(struct dvb_frontend *fe,
 	if (ret)
 		goto error;
 
-	cmd.args[0x00] = CMD_LNB_SEND_DISEQC;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = 0;
-	cmd.args[0x03] = 0;
-	cmd.args[0x04] = 2;
-	cmd.args[0x05] = 0;
-	cmd.args[0x06] = diseqc_cmd->msg_len;
-	memcpy(&cmd.args[0x07], diseqc_cmd->msg, diseqc_cmd->msg_len);
-	cmd.len = 0x07 + diseqc_cmd->msg_len;
+	cmd.args[0] = CMD_LNB_SEND_DISEQC;
+	cmd.args[1] = 0;
+	cmd.args[2] = 0;
+	cmd.args[3] = 0;
+	cmd.args[4] = 2;
+	cmd.args[5] = 0;
+	cmd.args[6] = diseqc_cmd->msg_len;
+	memcpy(&cmd.args[7], diseqc_cmd->msg, diseqc_cmd->msg_len);
+	cmd.len = 7 + diseqc_cmd->msg_len;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -344,7 +346,7 @@ static int tda10071_diseqc_recv_slave_reply(struct dvb_frontend *fe,
 		goto error;
 	}
 
-	dbg("%s:", __func__);
+	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
 	/* wait LNB RX */
 	for (i = 500, tmp = 0; i && !tmp; i--) {
@@ -355,7 +357,7 @@ static int tda10071_diseqc_recv_slave_reply(struct dvb_frontend *fe,
 		usleep_range(10000, 20000);
 	}
 
-	dbg("%s: loop=%d", __func__, i);
+	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
 
 	if (i == 0) {
 		ret = -ETIMEDOUT;
@@ -372,9 +374,9 @@ static int tda10071_diseqc_recv_slave_reply(struct dvb_frontend *fe,
 		reply->msg_len = sizeof(reply->msg); /* truncate API max */
 
 	/* read reply */
-	cmd.args[0x00] = CMD_LNB_UPDATE_REPLY;
-	cmd.args[0x01] = 0;
-	cmd.len = 0x02;
+	cmd.args[0] = CMD_LNB_UPDATE_REPLY;
+	cmd.args[1] = 0;
+	cmd.len = 2;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
@@ -385,7 +387,7 @@ static int tda10071_diseqc_recv_slave_reply(struct dvb_frontend *fe,
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -402,7 +404,8 @@ static int tda10071_diseqc_send_burst(struct dvb_frontend *fe,
 		goto error;
 	}
 
-	dbg("%s: fe_sec_mini_cmd=%d", __func__, fe_sec_mini_cmd);
+	dev_dbg(&priv->i2c->dev, "%s: fe_sec_mini_cmd=%d\n", __func__,
+			fe_sec_mini_cmd);
 
 	switch (fe_sec_mini_cmd) {
 	case SEC_MINI_A:
@@ -412,7 +415,8 @@ static int tda10071_diseqc_send_burst(struct dvb_frontend *fe,
 		burst = 1;
 		break;
 	default:
-		dbg("%s: invalid fe_sec_mini_cmd", __func__);
+		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_mini_cmd\n",
+				__func__);
 		ret = -EINVAL;
 		goto error;
 	}
@@ -426,7 +430,7 @@ static int tda10071_diseqc_send_burst(struct dvb_frontend *fe,
 		usleep_range(10000, 20000);
 	}
 
-	dbg("%s: loop=%d", __func__, i);
+	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
 
 	if (i == 0) {
 		ret = -ETIMEDOUT;
@@ -437,17 +441,17 @@ static int tda10071_diseqc_send_burst(struct dvb_frontend *fe,
 	if (ret)
 		goto error;
 
-	cmd.args[0x00] = CMD_LNB_SEND_TONEBURST;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = burst;
-	cmd.len = 0x03;
+	cmd.args[0] = CMD_LNB_SEND_TONEBURST;
+	cmd.args[1] = 0;
+	cmd.args[2] = burst;
+	cmd.len = 3;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -481,7 +485,7 @@ static int tda10071_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -506,7 +510,7 @@ static int tda10071_read_snr(struct dvb_frontend *fe, u16 *snr)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -523,9 +527,9 @@ static int tda10071_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 		goto error;
 	}
 
-	cmd.args[0x00] = CMD_GET_AGCACC;
-	cmd.args[0x01] = 0;
-	cmd.len = 0x02;
+	cmd.args[0] = CMD_GET_AGCACC;
+	cmd.args[1] = 0;
+	cmd.len = 2;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
@@ -545,7 +549,7 @@ static int tda10071_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -583,17 +587,18 @@ static int tda10071_read_ber(struct dvb_frontend *fe, u32 *ber)
 		goto error;
 
 	if (priv->meas_count[i] == tmp) {
-		dbg("%s: meas not ready=%02x", __func__, tmp);
+		dev_dbg(&priv->i2c->dev, "%s: meas not ready=%02x\n", __func__,
+				tmp);
 		*ber = priv->ber;
 		return 0;
 	} else {
 		priv->meas_count[i] = tmp;
 	}
 
-	cmd.args[0x00] = CMD_BER_UPDATE_COUNTERS;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = i;
-	cmd.len = 0x03;
+	cmd.args[0] = CMD_BER_UPDATE_COUNTERS;
+	cmd.args[1] = 0;
+	cmd.args[2] = i;
+	cmd.len = 3;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
@@ -612,7 +617,7 @@ static int tda10071_read_ber(struct dvb_frontend *fe, u32 *ber)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -632,7 +637,7 @@ static int tda10071_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -644,10 +649,11 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 	int ret, i;
 	u8 mode, rolloff, pilot, inversion, div;
 
-	dbg("%s: delivery_system=%d modulation=%d frequency=%d " \
-		"symbol_rate=%d inversion=%d pilot=%d rolloff=%d", __func__,
-		c->delivery_system, c->modulation, c->frequency,
-		c->symbol_rate, c->inversion, c->pilot, c->rolloff);
+	dev_dbg(&priv->i2c->dev, "%s: delivery_system=%d modulation=%d " \
+		"frequency=%d symbol_rate=%d inversion=%d pilot=%d " \
+		"rolloff=%d\n", __func__, c->delivery_system, c->modulation,
+		c->frequency, c->symbol_rate, c->inversion, c->pilot,
+		c->rolloff);
 
 	priv->delivery_system = SYS_UNDEFINED;
 
@@ -669,7 +675,7 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 		inversion = 3;
 		break;
 	default:
-		dbg("%s: invalid inversion", __func__);
+		dev_dbg(&priv->i2c->dev, "%s: invalid inversion\n", __func__);
 		ret = -EINVAL;
 		goto error;
 	}
@@ -692,7 +698,8 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 			break;
 		case ROLLOFF_AUTO:
 		default:
-			dbg("%s: invalid rolloff", __func__);
+			dev_dbg(&priv->i2c->dev, "%s: invalid rolloff\n",
+					__func__);
 			ret = -EINVAL;
 			goto error;
 		}
@@ -708,13 +715,15 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 			pilot = 2;
 			break;
 		default:
-			dbg("%s: invalid pilot", __func__);
+			dev_dbg(&priv->i2c->dev, "%s: invalid pilot\n",
+					__func__);
 			ret = -EINVAL;
 			goto error;
 		}
 		break;
 	default:
-		dbg("%s: invalid delivery_system", __func__);
+		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
+				__func__);
 		ret = -EINVAL;
 		goto error;
 	}
@@ -724,13 +733,15 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 			c->modulation == TDA10071_MODCOD[i].modulation &&
 			c->fec_inner == TDA10071_MODCOD[i].fec) {
 			mode = TDA10071_MODCOD[i].val;
-			dbg("%s: mode found=%02x", __func__, mode);
+			dev_dbg(&priv->i2c->dev, "%s: mode found=%02x\n",
+					__func__, mode);
 			break;
 		}
 	}
 
 	if (mode == 0xff) {
-		dbg("%s: invalid parameter combination", __func__);
+		dev_dbg(&priv->i2c->dev, "%s: invalid parameter combination\n",
+				__func__);
 		ret = -EINVAL;
 		goto error;
 	}
@@ -748,22 +759,22 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 	if (ret)
 		goto error;
 
-	cmd.args[0x00] = CMD_CHANGE_CHANNEL;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = mode;
-	cmd.args[0x03] = (c->frequency >> 16) & 0xff;
-	cmd.args[0x04] = (c->frequency >>  8) & 0xff;
-	cmd.args[0x05] = (c->frequency >>  0) & 0xff;
-	cmd.args[0x06] = ((c->symbol_rate / 1000) >> 8) & 0xff;
-	cmd.args[0x07] = ((c->symbol_rate / 1000) >> 0) & 0xff;
-	cmd.args[0x08] = (tda10071_ops.info.frequency_tolerance >> 8) & 0xff;
-	cmd.args[0x09] = (tda10071_ops.info.frequency_tolerance >> 0) & 0xff;
-	cmd.args[0x0a] = rolloff;
-	cmd.args[0x0b] = inversion;
-	cmd.args[0x0c] = pilot;
-	cmd.args[0x0d] = 0x00;
-	cmd.args[0x0e] = 0x00;
-	cmd.len = 0x0f;
+	cmd.args[0] = CMD_CHANGE_CHANNEL;
+	cmd.args[1] = 0;
+	cmd.args[2] = mode;
+	cmd.args[3] = (c->frequency >> 16) & 0xff;
+	cmd.args[4] = (c->frequency >>  8) & 0xff;
+	cmd.args[5] = (c->frequency >>  0) & 0xff;
+	cmd.args[6] = ((c->symbol_rate / 1000) >> 8) & 0xff;
+	cmd.args[7] = ((c->symbol_rate / 1000) >> 0) & 0xff;
+	cmd.args[8] = (tda10071_ops.info.frequency_tolerance >> 8) & 0xff;
+	cmd.args[9] = (tda10071_ops.info.frequency_tolerance >> 0) & 0xff;
+	cmd.args[10] = rolloff;
+	cmd.args[11] = inversion;
+	cmd.args[12] = pilot;
+	cmd.args[13] = 0x00;
+	cmd.args[14] = 0x00;
+	cmd.len = 15;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
@@ -772,7 +783,7 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -829,7 +840,7 @@ static int tda10071_get_frontend(struct dvb_frontend *fe)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -915,10 +926,10 @@ static int tda10071_init(struct dvb_frontend *fe)
 				goto error;
 		}
 
-		cmd.args[0x00] = CMD_SET_SLEEP_MODE;
-		cmd.args[0x01] = 0;
-		cmd.args[0x02] = 0;
-		cmd.len = 0x03;
+		cmd.args[0] = CMD_SET_SLEEP_MODE;
+		cmd.args[1] = 0;
+		cmd.args[2] = 0;
+		cmd.len = 3;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
@@ -929,10 +940,11 @@ static int tda10071_init(struct dvb_frontend *fe)
 		/* request the firmware, this will block and timeout */
 		ret = request_firmware(&fw, fw_file, priv->i2c->dev.parent);
 		if (ret) {
-			err("did not find the firmware file. (%s) "
-				"Please see linux/Documentation/dvb/ for more" \
-				" details on firmware-problems. (%d)",
-				fw_file, ret);
+			dev_err(&priv->i2c->dev, "%s: did not find the " \
+					"firmware file. (%s) Please see " \
+					"linux/Documentation/dvb/ for more " \
+					"details on firmware-problems. (%d)\n",
+					KBUILD_MODNAME, fw_file, ret);
 			goto error;
 		}
 
@@ -961,10 +973,11 @@ static int tda10071_init(struct dvb_frontend *fe)
 		if (ret)
 			goto error_release_firmware;
 
-		info("found a '%s' in cold state, will try to load a firmware",
-			tda10071_ops.info.name);
-
-		info("downloading firmware from file '%s'", fw_file);
+		dev_info(&priv->i2c->dev, "%s: found a '%s' in cold state, " \
+				"will try to load a firmware\n", KBUILD_MODNAME,
+				tda10071_ops.info.name);
+		dev_info(&priv->i2c->dev, "%s: downloading firmware from " \
+				"file '%s'\n", KBUILD_MODNAME, fw_file);
 
 		/* do not download last byte */
 		fw_size = fw->size - 1;
@@ -978,7 +991,9 @@ static int tda10071_init(struct dvb_frontend *fe)
 			ret = tda10071_wr_regs(priv, 0xfa,
 				(u8 *) &fw->data[fw_size - remaining], len);
 			if (ret) {
-				err("firmware download failed=%d", ret);
+				dev_err(&priv->i2c->dev, "%s: firmware " \
+						"download failed=%d\n",
+						KBUILD_MODNAME, ret);
 				if (ret)
 					goto error_release_firmware;
 			}
@@ -1002,15 +1017,16 @@ static int tda10071_init(struct dvb_frontend *fe)
 			goto error;
 
 		if (tmp) {
-			info("firmware did not run");
+			dev_info(&priv->i2c->dev, "%s: firmware did not run\n",
+					KBUILD_MODNAME);
 			ret = -EFAULT;
 			goto error;
 		} else {
 			priv->warm = 1;
 		}
 
-		cmd.args[0x00] = CMD_GET_FW_VERSION;
-		cmd.len = 0x01;
+		cmd.args[0] = CMD_GET_FW_VERSION;
+		cmd.len = 1;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
@@ -1019,54 +1035,55 @@ static int tda10071_init(struct dvb_frontend *fe)
 		if (ret)
 			goto error;
 
-		info("firmware version %d.%d.%d.%d",
-			buf[0], buf[1], buf[2], buf[3]);
-		info("found a '%s' in warm state.", tda10071_ops.info.name);
+		dev_info(&priv->i2c->dev, "%s: firmware version %d.%d.%d.%d\n",
+				KBUILD_MODNAME, buf[0], buf[1], buf[2], buf[3]);
+		dev_info(&priv->i2c->dev, "%s: found a '%s' in warm state\n",
+				KBUILD_MODNAME, tda10071_ops.info.name);
 
 		ret = tda10071_rd_regs(priv, 0x81, buf, 2);
 		if (ret)
 			goto error;
 
-		cmd.args[0x00] = CMD_DEMOD_INIT;
-		cmd.args[0x01] = ((priv->cfg.xtal / 1000) >> 8) & 0xff;
-		cmd.args[0x02] = ((priv->cfg.xtal / 1000) >> 0) & 0xff;
-		cmd.args[0x03] = buf[0];
-		cmd.args[0x04] = buf[1];
-		cmd.args[0x05] = priv->cfg.pll_multiplier;
-		cmd.args[0x06] = priv->cfg.spec_inv;
-		cmd.args[0x07] = 0x00;
-		cmd.len = 0x08;
+		cmd.args[0] = CMD_DEMOD_INIT;
+		cmd.args[1] = ((priv->cfg.xtal / 1000) >> 8) & 0xff;
+		cmd.args[2] = ((priv->cfg.xtal / 1000) >> 0) & 0xff;
+		cmd.args[3] = buf[0];
+		cmd.args[4] = buf[1];
+		cmd.args[5] = priv->cfg.pll_multiplier;
+		cmd.args[6] = priv->cfg.spec_inv;
+		cmd.args[7] = 0x00;
+		cmd.len = 8;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
 
-		cmd.args[0x00] = CMD_TUNER_INIT;
-		cmd.args[0x01] = 0x00;
-		cmd.args[0x02] = 0x00;
-		cmd.args[0x03] = 0x00;
-		cmd.args[0x04] = 0x00;
-		cmd.args[0x05] = 0x14;
-		cmd.args[0x06] = 0x00;
-		cmd.args[0x07] = 0x03;
-		cmd.args[0x08] = 0x02;
-		cmd.args[0x09] = 0x02;
-		cmd.args[0x0a] = 0x00;
-		cmd.args[0x0b] = 0x00;
-		cmd.args[0x0c] = 0x00;
-		cmd.args[0x0d] = 0x00;
-		cmd.args[0x0e] = 0x00;
-		cmd.len = 0x0f;
+		cmd.args[0] = CMD_TUNER_INIT;
+		cmd.args[1] = 0x00;
+		cmd.args[2] = 0x00;
+		cmd.args[3] = 0x00;
+		cmd.args[4] = 0x00;
+		cmd.args[5] = 0x14;
+		cmd.args[6] = 0x00;
+		cmd.args[7] = 0x03;
+		cmd.args[8] = 0x02;
+		cmd.args[9] = 0x02;
+		cmd.args[10] = 0x00;
+		cmd.args[11] = 0x00;
+		cmd.args[12] = 0x00;
+		cmd.args[13] = 0x00;
+		cmd.args[14] = 0x00;
+		cmd.len = 15;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
 
-		cmd.args[0x00] = CMD_MPEG_CONFIG;
-		cmd.args[0x01] = 0;
-		cmd.args[0x02] = priv->cfg.ts_mode;
-		cmd.args[0x03] = 0x00;
-		cmd.args[0x04] = 0x04;
-		cmd.args[0x05] = 0x00;
-		cmd.len = 0x06;
+		cmd.args[0] = CMD_MPEG_CONFIG;
+		cmd.args[1] = 0;
+		cmd.args[2] = priv->cfg.ts_mode;
+		cmd.args[3] = 0x00;
+		cmd.args[4] = 0x04;
+		cmd.args[5] = 0x00;
+		cmd.len = 6;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
@@ -1075,27 +1092,27 @@ static int tda10071_init(struct dvb_frontend *fe)
 		if (ret)
 			goto error;
 
-		cmd.args[0x00] = CMD_LNB_CONFIG;
-		cmd.args[0x01] = 0;
-		cmd.args[0x02] = 150;
-		cmd.args[0x03] = 3;
-		cmd.args[0x04] = 22;
-		cmd.args[0x05] = 1;
-		cmd.args[0x06] = 1;
-		cmd.args[0x07] = 30;
-		cmd.args[0x08] = 30;
-		cmd.args[0x09] = 30;
-		cmd.args[0x0a] = 30;
-		cmd.len = 0x0b;
+		cmd.args[0] = CMD_LNB_CONFIG;
+		cmd.args[1] = 0;
+		cmd.args[2] = 150;
+		cmd.args[3] = 3;
+		cmd.args[4] = 22;
+		cmd.args[5] = 1;
+		cmd.args[6] = 1;
+		cmd.args[7] = 30;
+		cmd.args[8] = 30;
+		cmd.args[9] = 30;
+		cmd.args[10] = 30;
+		cmd.len = 11;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
 
-		cmd.args[0x00] = CMD_BER_CONTROL;
-		cmd.args[0x01] = 0;
-		cmd.args[0x02] = 14;
-		cmd.args[0x03] = 14;
-		cmd.len = 0x04;
+		cmd.args[0] = CMD_BER_CONTROL;
+		cmd.args[1] = 0;
+		cmd.args[2] = 14;
+		cmd.args[3] = 14;
+		cmd.len = 4;
 		ret = tda10071_cmd_execute(priv, &cmd);
 		if (ret)
 			goto error;
@@ -1105,7 +1122,7 @@ static int tda10071_init(struct dvb_frontend *fe)
 error_release_firmware:
 	release_firmware(fw);
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -1132,10 +1149,10 @@ static int tda10071_sleep(struct dvb_frontend *fe)
 		goto error;
 	}
 
-	cmd.args[0x00] = CMD_SET_SLEEP_MODE;
-	cmd.args[0x01] = 0;
-	cmd.args[0x02] = 1;
-	cmd.len = 0x03;
+	cmd.args[0] = CMD_SET_SLEEP_MODE;
+	cmd.args[1] = 0;
+	cmd.args[2] = 1;
+	cmd.len = 3;
 	ret = tda10071_cmd_execute(priv, &cmd);
 	if (ret)
 		goto error;
@@ -1149,7 +1166,7 @@ static int tda10071_sleep(struct dvb_frontend *fe)
 
 	return ret;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
 	return ret;
 }
 
@@ -1208,7 +1225,7 @@ struct dvb_frontend *tda10071_attach(const struct tda10071_config *config,
 
 	return &priv->fe;
 error:
-	dbg("%s: failed=%d", __func__, ret);
+	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
 	kfree(priv);
 	return NULL;
 }
diff --git a/drivers/media/dvb/frontends/tda10071_priv.h b/drivers/media/dvb/frontends/tda10071_priv.h
index 93c5e6317f0..0fa85cfa70c 100644
--- a/drivers/media/dvb/frontends/tda10071_priv.h
+++ b/drivers/media/dvb/frontends/tda10071_priv.h
@@ -25,19 +25,6 @@
 #include "tda10071.h"
 #include <linux/firmware.h>
 
-#define LOG_PREFIX "tda10071"
-
-#undef dbg
-#define dbg(f, arg...) \
-	if (tda10071_debug) \
-		printk(KERN_INFO   LOG_PREFIX": " f "\n" , ## arg)
-#undef err
-#define err(f, arg...)  printk(KERN_ERR     LOG_PREFIX": " f "\n" , ## arg)
-#undef info
-#define info(f, arg...) printk(KERN_INFO    LOG_PREFIX": " f "\n" , ## arg)
-#undef warn
-#define warn(f, arg...) printk(KERN_WARNING LOG_PREFIX": " f "\n" , ## arg)
-
 struct tda10071_priv {
 	struct i2c_adapter *i2c;
 	struct dvb_frontend fe;
@@ -112,7 +99,7 @@ struct tda10071_reg_val_mask {
 #define CMD_BER_UPDATE_COUNTERS 0x3f
 
 /* firmare command struct */
-#define TDA10071_ARGLEN      0x1e
+#define TDA10071_ARGLEN      30
 struct tda10071_cmd {
 	u8 args[TDA10071_ARGLEN];
 	u8 len;
diff --git a/drivers/media/dvb/ngene/ngene-cards.c b/drivers/media/dvb/ngene/ngene-cards.c
index 7539a5d7102..72ee8de0226 100644
--- a/drivers/media/dvb/ngene/ngene-cards.c
+++ b/drivers/media/dvb/ngene/ngene-cards.c
@@ -217,6 +217,7 @@ static int demod_attach_drxk(struct ngene_channel *chan,
 
 	memset(&config, 0, sizeof(config));
 	config.microcode_name = "drxk_a3.mc";
+	config.qam_demod_parameter_count = 4;
 	config.adr = 0x29 + (chan->number ^ 2);
 
 	chan->fe = dvb_attach(drxk_attach, &config, i2c);
diff --git a/drivers/media/dvb/siano/smscoreapi.c b/drivers/media/dvb/siano/smscoreapi.c
index 7331e8450d1..9cc55546cc3 100644
--- a/drivers/media/dvb/siano/smscoreapi.c
+++ b/drivers/media/dvb/siano/smscoreapi.c
@@ -276,16 +276,13 @@ static void smscore_notify_clients(struct smscore_device_t *coredev)
 static int smscore_notify_callbacks(struct smscore_device_t *coredev,
 				    struct device *device, int arrival)
 {
-	struct list_head *next, *first;
+	struct smscore_device_notifyee_t *elem;
 	int rc = 0;
 
 	/* note: must be called under g_deviceslock */
 
-	first = &g_smscore_notifyees;
-
-	for (next = first->next; next != first; next = next->next) {
-		rc = ((struct smscore_device_notifyee_t *) next)->
-				hotplug(coredev, device, arrival);
+	list_for_each_entry(elem, &g_smscore_notifyees, entry) {
+		rc = elem->hotplug(coredev, device, arrival);
 		if (rc < 0)
 			break;
 	}
@@ -940,29 +937,25 @@ static struct
 smscore_client_t *smscore_find_client(struct smscore_device_t *coredev,
 				      int data_type, int id)
 {
-	struct smscore_client_t *client = NULL;
-	struct list_head *next, *first;
+	struct list_head *first;
+	struct smscore_client_t *client;
 	unsigned long flags;
-	struct list_head *firstid, *nextid;
-
+	struct list_head *firstid;
+	struct smscore_idlist_t *client_id;
 
 	spin_lock_irqsave(&coredev->clientslock, flags);
 	first = &coredev->clients;
-	for (next = first->next;
-	     (next != first) && !client;
-	     next = next->next) {
-		firstid = &((struct smscore_client_t *)next)->idlist;
-		for (nextid = firstid->next;
-		     nextid != firstid;
-		     nextid = nextid->next) {
-			if ((((struct smscore_idlist_t *)nextid)->id == id) &&
-			    (((struct smscore_idlist_t *)nextid)->data_type == data_type ||
-			    (((struct smscore_idlist_t *)nextid)->data_type == 0))) {
-				client = (struct smscore_client_t *) next;
-				break;
-			}
+	list_for_each_entry(client, first, entry) {
+		firstid = &client->idlist;
+		list_for_each_entry(client_id, firstid, entry) {
+			if ((client_id->id == id) &&
+			    (client_id->data_type == data_type ||
+			    (client_id->data_type == 0)))
+				goto found;
 		}
 	}
+	client = NULL;
+found:
 	spin_unlock_irqrestore(&coredev->clientslock, flags);
 	return client;
 }
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index c257da13d76..8090b87b306 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -5,6 +5,7 @@
 menuconfig RADIO_ADAPTERS
 	bool "Radio Adapters"
 	depends on VIDEO_V4L2
+	depends on MEDIA_RADIO_SUPPORT
 	default y
 	---help---
 	  Say Y here to enable selecting AM/FM radio adapters.
@@ -56,6 +57,39 @@ config RADIO_MAXIRADIO
 	  To compile this driver as a module, choose M here: the
 	  module will be called radio-maxiradio.
 
+config RADIO_SHARK
+	tristate "Griffin radioSHARK USB radio receiver"
+	depends on USB && SND
+	---help---
+	  Choose Y here if you have this radio receiver.
+
+	  There are 2 versions of this device, this driver is for version 1,
+	  which is white.
+
+	  In order to control your radio card, you will need to use programs
+	  that are compatible with the Video For Linux API.  Information on
+	  this API and pointers to "v4l" programs may be found at
+	  <file:Documentation/video4linux/API.html>.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-shark.
+
+config RADIO_SHARK2
+	tristate "Griffin radioSHARK2 USB radio receiver"
+	depends on USB
+	---help---
+	  Choose Y here if you have this radio receiver.
+
+	  There are 2 versions of this device, this driver is for version 2,
+	  which is black.
+
+	  In order to control your radio card, you will need to use programs
+	  that are compatible with the Video For Linux API.  Information on
+	  this API and pointers to "v4l" programs may be found at
+	  <file:Documentation/video4linux/API.html>.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called radio-shark2.
 
 config I2C_SI4713
 	tristate "I2C driver for Silicon Labs Si4713 device"
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index ca8c7d134b9..c03ce4fe74e 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -11,6 +11,8 @@ obj-$(CONFIG_RADIO_CADET) += radio-cadet.o
 obj-$(CONFIG_RADIO_TYPHOON) += radio-typhoon.o
 obj-$(CONFIG_RADIO_TERRATEC) += radio-terratec.o
 obj-$(CONFIG_RADIO_MAXIRADIO) += radio-maxiradio.o
+obj-$(CONFIG_RADIO_SHARK) += radio-shark.o
+obj-$(CONFIG_RADIO_SHARK2) += shark2.o
 obj-$(CONFIG_RADIO_RTRACK) += radio-aimslab.o
 obj-$(CONFIG_RADIO_ZOLTRIX) += radio-zoltrix.o
 obj-$(CONFIG_RADIO_GEMTEK) += radio-gemtek.o
@@ -29,4 +31,6 @@ obj-$(CONFIG_RADIO_TIMBERDALE) += radio-timb.o
 obj-$(CONFIG_RADIO_WL1273) += radio-wl1273.o
 obj-$(CONFIG_RADIO_WL128X) += wl128x/
 
+shark2-objs := radio-shark2.o radio-tea5777.o
+
 ccflags-y += -Isound
diff --git a/drivers/media/radio/lm7000.h b/drivers/media/radio/lm7000.h
new file mode 100644
index 00000000000..139cd6b6882
--- /dev/null
+++ b/drivers/media/radio/lm7000.h
@@ -0,0 +1,43 @@
+#ifndef __LM7000_H
+#define __LM7000_H
+
+/* Sanyo LM7000 tuner chip control
+ *
+ * Copyright 2012 Ondrej Zary <linux@rainbow-software.org>
+ * based on radio-aimslab.c by M. Kirkwood
+ * and radio-sf16fmi.c by M. Kirkwood and Petr Vandrovec
+ */
+
+#define LM7000_DATA	(1 << 0)
+#define LM7000_CLK	(1 << 1)
+#define LM7000_CE	(1 << 2)
+
+#define LM7000_FM_100	(0 << 20)
+#define LM7000_FM_50	(1 << 20)
+#define LM7000_FM_25	(2 << 20)
+#define LM7000_BIT_FM	(1 << 23)
+
+static inline void lm7000_set_freq(u32 freq, void *handle,
+				void (*set_pins)(void *handle, u8 pins))
+{
+	int i;
+	u8 data;
+	u32 val;
+
+	freq += 171200;		/* Add 10.7 MHz IF */
+	freq /= 400;		/* Convert to 25 kHz units */
+	val = freq | LM7000_FM_25 | LM7000_BIT_FM;
+	/* write the 24-bit register, starting with LSB */
+	for (i = 0; i < 24; i++) {
+		data = val & (1 << i) ? LM7000_DATA : 0;
+		set_pins(handle, data | LM7000_CE);
+		udelay(2);
+		set_pins(handle, data | LM7000_CE | LM7000_CLK);
+		udelay(2);
+		set_pins(handle, data | LM7000_CE);
+		udelay(2);
+	}
+	set_pins(handle, 0);
+}
+
+#endif /* __LM7000_H */
diff --git a/drivers/media/radio/radio-aimslab.c b/drivers/media/radio/radio-aimslab.c
index 98e0c8c2031..12c70e876f5 100644
--- a/drivers/media/radio/radio-aimslab.c
+++ b/drivers/media/radio/radio-aimslab.c
@@ -37,6 +37,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
 #include "radio-isa.h"
+#include "lm7000.h"
 
 MODULE_AUTHOR("M. Kirkwood");
 MODULE_DESCRIPTION("A driver for the RadioTrack/RadioReveal radio card.");
@@ -72,55 +73,38 @@ static struct radio_isa_card *rtrack_alloc(void)
 	return rt ? &rt->isa : NULL;
 }
 
-/* The 128+64 on these outb's is to keep the volume stable while tuning.
- * Without them, the volume _will_ creep up with each frequency change
- * and bit 4 (+16) is to keep the signal strength meter enabled.
- */
+#define AIMS_BIT_TUN_CE		(1 << 0)
+#define AIMS_BIT_TUN_CLK	(1 << 1)
+#define AIMS_BIT_TUN_DATA	(1 << 2)
+#define AIMS_BIT_VOL_CE		(1 << 3)
+#define AIMS_BIT_TUN_STRQ	(1 << 4)
+/* bit 5 is not connected */
+#define AIMS_BIT_VOL_UP		(1 << 6)	/* active low */
+#define AIMS_BIT_VOL_DN		(1 << 7)	/* active low */
 
-static void send_0_byte(struct radio_isa_card *isa, int on)
+void rtrack_set_pins(void *handle, u8 pins)
 {
-	outb_p(128+64+16+on+1, isa->io);	/* wr-enable + data low */
-	outb_p(128+64+16+on+2+1, isa->io);	/* clock */
-	msleep(1);
-}
+	struct radio_isa_card *isa = handle;
+	struct rtrack *rt = container_of(isa, struct rtrack, isa);
+	u8 bits = AIMS_BIT_VOL_DN | AIMS_BIT_VOL_UP | AIMS_BIT_TUN_STRQ;
 
-static void send_1_byte(struct radio_isa_card *isa, int on)
-{
-	outb_p(128+64+16+on+4+1, isa->io);	/* wr-enable+data high */
-	outb_p(128+64+16+on+4+2+1, isa->io);	/* clock */
-	msleep(1);
+	if (!v4l2_ctrl_g_ctrl(rt->isa.mute))
+		bits |= AIMS_BIT_VOL_CE;
+
+	if (pins & LM7000_DATA)
+		bits |= AIMS_BIT_TUN_DATA;
+	if (pins & LM7000_CLK)
+		bits |= AIMS_BIT_TUN_CLK;
+	if (pins & LM7000_CE)
+		bits |= AIMS_BIT_TUN_CE;
+
+	outb_p(bits, rt->isa.io);
 }
 
 static int rtrack_s_frequency(struct radio_isa_card *isa, u32 freq)
 {
-	int on = v4l2_ctrl_g_ctrl(isa->mute) ? 0 : 8;
-	int i;
-
-	freq += 171200;			/* Add 10.7 MHz IF 		*/
-	freq /= 800;			/* Convert to 50 kHz units	*/
-
-	send_0_byte(isa, on);		/*  0: LSB of frequency		*/
-
-	for (i = 0; i < 13; i++)	/*   : frequency bits (1-13)	*/
-		if (freq & (1 << i))
-			send_1_byte(isa, on);
-		else
-			send_0_byte(isa, on);
-
-	send_0_byte(isa, on);		/* 14: test bit - always 0    */
-	send_0_byte(isa, on);		/* 15: test bit - always 0    */
-
-	send_0_byte(isa, on);		/* 16: band data 0 - always 0 */
-	send_0_byte(isa, on);		/* 17: band data 1 - always 0 */
-	send_0_byte(isa, on);		/* 18: band data 2 - always 0 */
-	send_0_byte(isa, on);		/* 19: time base - always 0   */
-
-	send_0_byte(isa, on);		/* 20: spacing (0 = 25 kHz)   */
-	send_1_byte(isa, on);		/* 21: spacing (1 = 25 kHz)   */
-	send_0_byte(isa, on);		/* 22: spacing (0 = 25 kHz)   */
-	send_1_byte(isa, on);		/* 23: AM/FM (FM = 1, always) */
+	lm7000_set_freq(freq, isa, rtrack_set_pins);
 
-	outb(0xd0 + on, isa->io);	/* volume steady + sigstr */
 	return 0;
 }
 
diff --git a/drivers/media/radio/radio-cadet.c b/drivers/media/radio/radio-cadet.c
index 16a089fad90..697a421c994 100644
--- a/drivers/media/radio/radio-cadet.c
+++ b/drivers/media/radio/radio-cadet.c
@@ -41,6 +41,9 @@
 #include <linux/io.h>		/* outb, outb_p			*/
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 
 MODULE_AUTHOR("Fred Gleason, Russell Kroll, Quay Lu, Donald Song, Jason Lewis, Scott McGrath, William McGrath");
 MODULE_DESCRIPTION("A driver for the ADS Cadet AM/FM/RDS radio card.");
@@ -61,14 +64,15 @@ module_param(radio_nr, int, 0);
 struct cadet {
 	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
+	struct v4l2_ctrl_handler ctrl_handler;
 	int io;
-	int users;
-	int curtuner;
+	bool is_fm_band;
+	u32 curfreq;
 	int tunestat;
 	int sigstrength;
 	wait_queue_head_t read_queue;
 	struct timer_list readtimer;
-	__u8 rdsin, rdsout, rdsstat;
+	u8 rdsin, rdsout, rdsstat;
 	unsigned char rdsbuf[RDS_BUFFER];
 	struct mutex lock;
 	int reading;
@@ -81,9 +85,9 @@ static struct cadet cadet_card;
  * The V4L API spec does not define any particular unit for the signal
  * strength value.  These values are in microvolts of RF at the tuner's input.
  */
-static __u16 sigtable[2][4] = {
-	{  5, 10, 30,  150 },
-	{ 28, 40, 63, 1000 }
+static u16 sigtable[2][4] = {
+	{ 1835, 2621,  4128, 65535 },
+	{ 2185, 4369, 13107, 65535 },
 };
 
 
@@ -91,14 +95,12 @@ static int cadet_getstereo(struct cadet *dev)
 {
 	int ret = V4L2_TUNER_SUB_MONO;
 
-	if (dev->curtuner != 0)	/* Only FM has stereo capability! */
+	if (!dev->is_fm_band)	/* Only FM has stereo capability! */
 		return V4L2_TUNER_SUB_MONO;
 
-	mutex_lock(&dev->lock);
 	outb(7, dev->io);          /* Select tuner control */
 	if ((inb(dev->io + 1) & 0x40) == 0)
 		ret = V4L2_TUNER_SUB_STEREO;
-	mutex_unlock(&dev->lock);
 	return ret;
 }
 
@@ -111,8 +113,6 @@ static unsigned cadet_gettune(struct cadet *dev)
 	 * Prepare for read
 	 */
 
-	mutex_lock(&dev->lock);
-
 	outb(7, dev->io);       /* Select tuner control */
 	curvol = inb(dev->io + 1); /* Save current volume/mute setting */
 	outb(0x00, dev->io + 1);  /* Ensure WRITE-ENABLE is LOW */
@@ -134,8 +134,6 @@ static unsigned cadet_gettune(struct cadet *dev)
 	 * Restore volume/mute setting
 	 */
 	outb(curvol, dev->io + 1);
-	mutex_unlock(&dev->lock);
-
 	return fifo;
 }
 
@@ -152,20 +150,18 @@ static unsigned cadet_getfreq(struct cadet *dev)
 	/*
 	 * Convert to actual frequency
 	 */
-	if (dev->curtuner == 0) {    /* FM */
-		test = 12500;
-		for (i = 0; i < 14; i++) {
-			if ((fifo & 0x01) != 0)
-				freq += test;
-			test = test << 1;
-			fifo = fifo >> 1;
-		}
-		freq -= 10700000;           /* IF frequency is 10.7 MHz */
-		freq = (freq * 16) / 1000000;   /* Make it 1/16 MHz */
+	if (!dev->is_fm_band)    /* AM */
+		return ((fifo & 0x7fff) - 450) * 16;
+
+	test = 12500;
+	for (i = 0; i < 14; i++) {
+		if ((fifo & 0x01) != 0)
+			freq += test;
+		test = test << 1;
+		fifo = fifo >> 1;
 	}
-	if (dev->curtuner == 1)    /* AM */
-		freq = ((fifo & 0x7fff) - 2010) * 16;
-
+	freq -= 10700000;           /* IF frequency is 10.7 MHz */
+	freq = (freq * 16) / 1000;   /* Make it 1/16 kHz */
 	return freq;
 }
 
@@ -174,8 +170,6 @@ static void cadet_settune(struct cadet *dev, unsigned fifo)
 	int i;
 	unsigned test;
 
-	mutex_lock(&dev->lock);
-
 	outb(7, dev->io);                /* Select tuner control */
 	/*
 	 * Write the shift register
@@ -194,7 +188,6 @@ static void cadet_settune(struct cadet *dev, unsigned fifo)
 		test = 0x1c | ((fifo >> 23) & 0x02);
 		outb(test, dev->io + 1);
 	}
-	mutex_unlock(&dev->lock);
 }
 
 static void cadet_setfreq(struct cadet *dev, unsigned freq)
@@ -203,13 +196,14 @@ static void cadet_setfreq(struct cadet *dev, unsigned freq)
 	int i, j, test;
 	int curvol;
 
+	dev->curfreq = freq;
 	/*
 	 * Formulate a fifo command
 	 */
 	fifo = 0;
-	if (dev->curtuner == 0) {    /* FM */
+	if (dev->is_fm_band) {    /* FM */
 		test = 102400;
-		freq = (freq * 1000) / 16;       /* Make it kHz */
+		freq = freq / 16;       /* Make it kHz */
 		freq += 10700;               /* IF is 10700 kHz */
 		for (i = 0; i < 14; i++) {
 			fifo = fifo << 1;
@@ -219,20 +213,17 @@ static void cadet_setfreq(struct cadet *dev, unsigned freq)
 			}
 			test = test >> 1;
 		}
-	}
-	if (dev->curtuner == 1) {    /* AM */
-		fifo = (freq / 16) + 2010;            /* Make it kHz */
-		fifo |= 0x100000;            /* Select AM Band */
+	} else {	/* AM */
+		fifo = (freq / 16) + 450;	/* Make it kHz */
+		fifo |= 0x100000;		/* Select AM Band */
 	}
 
 	/*
 	 * Save current volume/mute setting
 	 */
 
-	mutex_lock(&dev->lock);
 	outb(7, dev->io);                /* Select tuner control */
 	curvol = inb(dev->io + 1);
-	mutex_unlock(&dev->lock);
 
 	/*
 	 * Tune the card
@@ -240,49 +231,24 @@ static void cadet_setfreq(struct cadet *dev, unsigned freq)
 	for (j = 3; j > -1; j--) {
 		cadet_settune(dev, fifo | (j << 16));
 
-		mutex_lock(&dev->lock);
 		outb(7, dev->io);         /* Select tuner control */
 		outb(curvol, dev->io + 1);
-		mutex_unlock(&dev->lock);
 
 		msleep(100);
 
 		cadet_gettune(dev);
 		if ((dev->tunestat & 0x40) == 0) {   /* Tuned */
-			dev->sigstrength = sigtable[dev->curtuner][j];
-			return;
+			dev->sigstrength = sigtable[dev->is_fm_band][j];
+			goto reset_rds;
 		}
 	}
 	dev->sigstrength = 0;
+reset_rds:
+	outb(3, dev->io);
+	outb(inb(dev->io + 1) & 0x7f, dev->io + 1);
 }
 
 
-static int cadet_getvol(struct cadet *dev)
-{
-	int ret = 0;
-
-	mutex_lock(&dev->lock);
-
-	outb(7, dev->io);                /* Select tuner control */
-	if ((inb(dev->io + 1) & 0x20) != 0)
-		ret = 0xffff;
-
-	mutex_unlock(&dev->lock);
-	return ret;
-}
-
-
-static void cadet_setvol(struct cadet *dev, int vol)
-{
-	mutex_lock(&dev->lock);
-	outb(7, dev->io);                /* Select tuner control */
-	if (vol > 0)
-		outb(0x20, dev->io + 1);
-	else
-		outb(0x00, dev->io + 1);
-	mutex_unlock(&dev->lock);
-}
-
 static void cadet_handler(unsigned long data)
 {
 	struct cadet *dev = (void *)data;
@@ -295,7 +261,7 @@ static void cadet_handler(unsigned long data)
 		outb(0x80, dev->io);      /* Select RDS fifo */
 		while ((inb(dev->io) & 0x80) != 0) {
 			dev->rdsbuf[dev->rdsin] = inb(dev->io + 1);
-			if (dev->rdsin == dev->rdsout)
+			if (dev->rdsin + 1 == dev->rdsout)
 				printk(KERN_WARNING "cadet: RDS buffer overflow\n");
 			else
 				dev->rdsin++;
@@ -314,11 +280,21 @@ static void cadet_handler(unsigned long data)
 	 */
 	init_timer(&dev->readtimer);
 	dev->readtimer.function = cadet_handler;
-	dev->readtimer.data = (unsigned long)0;
+	dev->readtimer.data = data;
 	dev->readtimer.expires = jiffies + msecs_to_jiffies(50);
 	add_timer(&dev->readtimer);
 }
 
+static void cadet_start_rds(struct cadet *dev)
+{
+	dev->rdsstat = 1;
+	outb(0x80, dev->io);        /* Select RDS fifo */
+	init_timer(&dev->readtimer);
+	dev->readtimer.function = cadet_handler;
+	dev->readtimer.data = (unsigned long)dev;
+	dev->readtimer.expires = jiffies + msecs_to_jiffies(50);
+	add_timer(&dev->readtimer);
+}
 
 static ssize_t cadet_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 {
@@ -327,28 +303,24 @@ static ssize_t cadet_read(struct file *file, char __user *data, size_t count, lo
 	int i = 0;
 
 	mutex_lock(&dev->lock);
-	if (dev->rdsstat == 0) {
-		dev->rdsstat = 1;
-		outb(0x80, dev->io);        /* Select RDS fifo */
-		init_timer(&dev->readtimer);
-		dev->readtimer.function = cadet_handler;
-		dev->readtimer.data = (unsigned long)dev;
-		dev->readtimer.expires = jiffies + msecs_to_jiffies(50);
-		add_timer(&dev->readtimer);
-	}
+	if (dev->rdsstat == 0)
+		cadet_start_rds(dev);
 	if (dev->rdsin == dev->rdsout) {
+		if (file->f_flags & O_NONBLOCK) {
+			i = -EWOULDBLOCK;
+			goto unlock;
+		}
 		mutex_unlock(&dev->lock);
-		if (file->f_flags & O_NONBLOCK)
-			return -EWOULDBLOCK;
 		interruptible_sleep_on(&dev->read_queue);
 		mutex_lock(&dev->lock);
 	}
 	while (i < count && dev->rdsin != dev->rdsout)
 		readbuf[i++] = dev->rdsbuf[dev->rdsout++];
-	mutex_unlock(&dev->lock);
 
-	if (copy_to_user(data, readbuf, i))
-		return -EFAULT;
+	if (i && copy_to_user(data, readbuf, i))
+		i = -EFAULT;
+unlock:
+	mutex_unlock(&dev->lock);
 	return i;
 }
 
@@ -359,48 +331,58 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strlcpy(v->driver, "ADS Cadet", sizeof(v->driver));
 	strlcpy(v->card, "ADS Cadet", sizeof(v->card));
 	strlcpy(v->bus_info, "ISA", sizeof(v->bus_info));
-	v->capabilities = V4L2_CAP_TUNER | V4L2_CAP_RADIO |
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO |
 			  V4L2_CAP_READWRITE | V4L2_CAP_RDS_CAPTURE;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
+static const struct v4l2_frequency_band bands[] = {
+	{
+		.index = 0,
+		.type = V4L2_TUNER_RADIO,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow = 8320,      /* 520 kHz */
+		.rangehigh = 26400,    /* 1650 kHz */
+		.modulation = V4L2_BAND_MODULATION_AM,
+	}, {
+		.index = 1,
+		.type = V4L2_TUNER_RADIO,
+		.capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
+			V4L2_TUNER_CAP_RDS_BLOCK_IO | V4L2_TUNER_CAP_LOW |
+			V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow = 1400000,   /* 87.5 MHz */
+		.rangehigh = 1728000,  /* 108.0 MHz */
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+};
+
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
 	struct cadet *dev = video_drvdata(file);
 
+	if (v->index)
+		return -EINVAL;
 	v->type = V4L2_TUNER_RADIO;
-	switch (v->index) {
-	case 0:
-		strlcpy(v->name, "FM", sizeof(v->name));
-		v->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
-			V4L2_TUNER_CAP_RDS_BLOCK_IO;
-		v->rangelow = 1400;     /* 87.5 MHz */
-		v->rangehigh = 1728;    /* 108.0 MHz */
+	strlcpy(v->name, "Radio", sizeof(v->name));
+	v->capability = bands[0].capability | bands[1].capability;
+	v->rangelow = bands[0].rangelow;	   /* 520 kHz (start of AM band) */
+	v->rangehigh = bands[1].rangehigh;    /* 108.0 MHz (end of FM band) */
+	if (dev->is_fm_band) {
 		v->rxsubchans = cadet_getstereo(dev);
-		switch (v->rxsubchans) {
-		case V4L2_TUNER_SUB_MONO:
-			v->audmode = V4L2_TUNER_MODE_MONO;
-			break;
-		case V4L2_TUNER_SUB_STEREO:
-			v->audmode = V4L2_TUNER_MODE_STEREO;
-			break;
-		default:
-			break;
-		}
-		v->rxsubchans |= V4L2_TUNER_SUB_RDS;
-		break;
-	case 1:
-		strlcpy(v->name, "AM", sizeof(v->name));
-		v->capability = V4L2_TUNER_CAP_LOW;
+		outb(3, dev->io);
+		outb(inb(dev->io + 1) & 0x7f, dev->io + 1);
+		mdelay(100);
+		outb(3, dev->io);
+		if (inb(dev->io + 1) & 0x80)
+			v->rxsubchans |= V4L2_TUNER_SUB_RDS;
+	} else {
 		v->rangelow = 8320;      /* 520 kHz */
 		v->rangehigh = 26400;    /* 1650 kHz */
 		v->rxsubchans = V4L2_TUNER_SUB_MONO;
-		v->audmode = V4L2_TUNER_MODE_MONO;
-		break;
-	default:
-		return -EINVAL;
 	}
+	v->audmode = V4L2_TUNER_MODE_STEREO;
 	v->signal = dev->sigstrength; /* We might need to modify scaling of this */
 	return 0;
 }
@@ -408,11 +390,17 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 static int vidioc_s_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *v)
 {
-	struct cadet *dev = video_drvdata(file);
+	return v->index ? -EINVAL : 0;
+}
 
-	if (v->index != 0 && v->index != 1)
+static int vidioc_enum_freq_bands(struct file *file, void *priv,
+				struct v4l2_frequency_band *band)
+{
+	if (band->tuner)
+		return -EINVAL;
+	if (band->index >= ARRAY_SIZE(bands))
 		return -EINVAL;
-	dev->curtuner = v->index;
+	*band = bands[band->index];
 	return 0;
 }
 
@@ -421,9 +409,10 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 {
 	struct cadet *dev = video_drvdata(file);
 
-	f->tuner = dev->curtuner;
+	if (f->tuner)
+		return -EINVAL;
 	f->type = V4L2_TUNER_RADIO;
-	f->frequency = cadet_getfreq(dev);
+	f->frequency = dev->curfreq;
 	return 0;
 }
 
@@ -433,103 +422,46 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 {
 	struct cadet *dev = video_drvdata(file);
 
-	if (f->type != V4L2_TUNER_RADIO)
-		return -EINVAL;
-	if (dev->curtuner == 0 && (f->frequency < 1400 || f->frequency > 1728))
-		return -EINVAL;
-	if (dev->curtuner == 1 && (f->frequency < 8320 || f->frequency > 26400))
+	if (f->tuner)
 		return -EINVAL;
+	dev->is_fm_band =
+		f->frequency >= (bands[0].rangehigh + bands[1].rangelow) / 2;
+	clamp(f->frequency, bands[dev->is_fm_band].rangelow,
+			    bands[dev->is_fm_band].rangehigh);
 	cadet_setfreq(dev, f->frequency);
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *qc)
+static int cadet_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (qc->id) {
-	case V4L2_CID_AUDIO_MUTE:
-		return v4l2_ctrl_query_fill(qc, 0, 1, 1, 1);
-	case V4L2_CID_AUDIO_VOLUME:
-		return v4l2_ctrl_query_fill(qc, 0, 0xff, 1, 0xff);
-	}
-	return -EINVAL;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct cadet *dev = video_drvdata(file);
+	struct cadet *dev = container_of(ctrl->handler, struct cadet, ctrl_handler);
 
 	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_MUTE: /* TODO: Handle this correctly */
-		ctrl->value = (cadet_getvol(dev) == 0);
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-		ctrl->value = cadet_getvol(dev);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctrl)
-{
-	struct cadet *dev = video_drvdata(file);
-
-	switch (ctrl->id){
-	case V4L2_CID_AUDIO_MUTE: /* TODO: Handle this correctly */
-		if (ctrl->value)
-			cadet_setvol(dev, 0);
+	case V4L2_CID_AUDIO_MUTE:
+		outb(7, dev->io);                /* Select tuner control */
+		if (ctrl->val)
+			outb(0x00, dev->io + 1);
 		else
-			cadet_setvol(dev, 0xffff);
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-		cadet_setvol(dev, ctrl->value);
-		break;
-	default:
-		return -EINVAL;
+			outb(0x20, dev->io + 1);
+		return 0;
 	}
-	return 0;
-}
-
-static int vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int vidioc_s_input(struct file *filp, void *priv, unsigned int i)
-{
-	return i ? -EINVAL : 0;
-}
-
-static int vidioc_g_audio(struct file *file, void *priv,
-				struct v4l2_audio *a)
-{
-	a->index = 0;
-	strlcpy(a->name, "Radio", sizeof(a->name));
-	a->capability = V4L2_AUDCAP_STEREO;
-	return 0;
-}
-
-static int vidioc_s_audio(struct file *file, void *priv,
-				struct v4l2_audio *a)
-{
-	return a->index ? -EINVAL : 0;
+	return -EINVAL;
 }
 
 static int cadet_open(struct file *file)
 {
 	struct cadet *dev = video_drvdata(file);
+	int err;
 
 	mutex_lock(&dev->lock);
-	dev->users++;
-	if (1 == dev->users)
+	err = v4l2_fh_open(file);
+	if (err)
+		goto fail;
+	if (v4l2_fh_is_singular_file(file))
 		init_waitqueue_head(&dev->read_queue);
+fail:
 	mutex_unlock(&dev->lock);
-	return 0;
+	return err;
 }
 
 static int cadet_release(struct file *file)
@@ -537,11 +469,11 @@ static int cadet_release(struct file *file)
 	struct cadet *dev = video_drvdata(file);
 
 	mutex_lock(&dev->lock);
-	dev->users--;
-	if (0 == dev->users) {
+	if (v4l2_fh_is_singular_file(file) && dev->rdsstat) {
 		del_timer_sync(&dev->readtimer);
 		dev->rdsstat = 0;
 	}
+	v4l2_fh_release(file);
 	mutex_unlock(&dev->lock);
 	return 0;
 }
@@ -549,11 +481,19 @@ static int cadet_release(struct file *file)
 static unsigned int cadet_poll(struct file *file, struct poll_table_struct *wait)
 {
 	struct cadet *dev = video_drvdata(file);
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int res = v4l2_ctrl_poll(file, wait);
 
 	poll_wait(file, &dev->read_queue, wait);
+	if (dev->rdsstat == 0 && (req_events & (POLLIN | POLLRDNORM))) {
+		mutex_lock(&dev->lock);
+		if (dev->rdsstat == 0)
+			cadet_start_rds(dev);
+		mutex_unlock(&dev->lock);
+	}
 	if (dev->rdsin != dev->rdsout)
-		return POLLIN | POLLRDNORM;
-	return 0;
+		res |= POLLIN | POLLRDNORM;
+	return res;
 }
 
 
@@ -572,13 +512,14 @@ static const struct v4l2_ioctl_ops cadet_ioctl_ops = {
 	.vidioc_s_tuner     = vidioc_s_tuner,
 	.vidioc_g_frequency = vidioc_g_frequency,
 	.vidioc_s_frequency = vidioc_s_frequency,
-	.vidioc_queryctrl   = vidioc_queryctrl,
-	.vidioc_g_ctrl      = vidioc_g_ctrl,
-	.vidioc_s_ctrl      = vidioc_s_ctrl,
-	.vidioc_g_audio     = vidioc_g_audio,
-	.vidioc_s_audio     = vidioc_s_audio,
-	.vidioc_g_input     = vidioc_g_input,
-	.vidioc_s_input     = vidioc_s_input,
+	.vidioc_enum_freq_bands = vidioc_enum_freq_bands,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_ctrl_ops cadet_ctrl_ops = {
+	.s_ctrl = cadet_s_ctrl,
 };
 
 #ifdef CONFIG_PNP
@@ -628,8 +569,8 @@ static void cadet_probe(struct cadet *dev)
 	for (i = 0; i < 8; i++) {
 		dev->io = iovals[i];
 		if (request_region(dev->io, 2, "cadet-probe")) {
-			cadet_setfreq(dev, 1410);
-			if (cadet_getfreq(dev) == 1410) {
+			cadet_setfreq(dev, bands[1].rangelow);
+			if (cadet_getfreq(dev) == bands[1].rangelow) {
 				release_region(dev->io, 2);
 				return;
 			}
@@ -648,7 +589,8 @@ static int __init cadet_init(void)
 {
 	struct cadet *dev = &cadet_card;
 	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
-	int res;
+	struct v4l2_ctrl_handler *hdl;
+	int res = -ENODEV;
 
 	strlcpy(v4l2_dev->name, "cadet", sizeof(v4l2_dev->name));
 	mutex_init(&dev->lock);
@@ -680,23 +622,40 @@ static int __init cadet_init(void)
 		goto fail;
 	}
 
+	hdl = &dev->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 2);
+	v4l2_ctrl_new_std(hdl, &cadet_ctrl_ops,
+			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	v4l2_dev->ctrl_handler = hdl;
+	if (hdl->error) {
+		res = hdl->error;
+		v4l2_err(v4l2_dev, "Could not register controls\n");
+		goto err_hdl;
+	}
+
+	dev->is_fm_band = true;
+	dev->curfreq = bands[dev->is_fm_band].rangelow;
+	cadet_setfreq(dev, dev->curfreq);
 	strlcpy(dev->vdev.name, v4l2_dev->name, sizeof(dev->vdev.name));
 	dev->vdev.v4l2_dev = v4l2_dev;
 	dev->vdev.fops = &cadet_fops;
 	dev->vdev.ioctl_ops = &cadet_ioctl_ops;
 	dev->vdev.release = video_device_release_empty;
+	dev->vdev.lock = &dev->lock;
+	set_bit(V4L2_FL_USE_FH_PRIO, &dev->vdev.flags);
 	video_set_drvdata(&dev->vdev, dev);
 
-	if (video_register_device(&dev->vdev, VFL_TYPE_RADIO, radio_nr) < 0) {
-		v4l2_device_unregister(v4l2_dev);
-		release_region(dev->io, 2);
-		goto fail;
-	}
+	if (video_register_device(&dev->vdev, VFL_TYPE_RADIO, radio_nr) < 0)
+		goto err_hdl;
 	v4l2_info(v4l2_dev, "ADS Cadet Radio Card at 0x%x\n", dev->io);
 	return 0;
+err_hdl:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(v4l2_dev);
+	release_region(dev->io, 2);
 fail:
 	pnp_unregister_driver(&cadet_pnp_driver);
-	return -ENODEV;
+	return res;
 }
 
 static void __exit cadet_exit(void)
@@ -704,7 +663,10 @@ static void __exit cadet_exit(void)
 	struct cadet *dev = &cadet_card;
 
 	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 	v4l2_device_unregister(&dev->v4l2_dev);
+	outb(7, dev->io);	/* Mute */
+	outb(0x00, dev->io + 1);
 	release_region(dev->io, 2);
 	pnp_unregister_driver(&cadet_pnp_driver);
 }
diff --git a/drivers/media/radio/radio-mr800.c b/drivers/media/radio/radio-mr800.c
index 94cb6bc690f..3182b26d6ef 100644
--- a/drivers/media/radio/radio-mr800.c
+++ b/drivers/media/radio/radio-mr800.c
@@ -295,7 +295,8 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 	v->type = V4L2_TUNER_RADIO;
 	v->rangelow = FREQ_MIN * FREQ_MUL;
 	v->rangehigh = FREQ_MAX * FREQ_MUL;
-	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+		V4L2_TUNER_CAP_HWSEEK_WRAP;
 	v->rxsubchans = is_stereo ? V4L2_TUNER_SUB_STEREO : V4L2_TUNER_SUB_MONO;
 	v->audmode = radio->stereo ?
 		V4L2_TUNER_MODE_STEREO : V4L2_TUNER_MODE_MONO;
@@ -372,7 +373,7 @@ static int vidioc_s_hw_freq_seek(struct file *file, void *priv,
 	timeout = jiffies + msecs_to_jiffies(30000);
 	for (;;) {
 		if (time_after(jiffies, timeout)) {
-			retval = -EAGAIN;
+			retval = -ENODATA;
 			break;
 		}
 		if (schedule_timeout_interruptible(msecs_to_jiffies(10))) {
diff --git a/drivers/media/radio/radio-sf16fmi.c b/drivers/media/radio/radio-sf16fmi.c
index a81d723b8c7..8185d5fbfa8 100644
--- a/drivers/media/radio/radio-sf16fmi.c
+++ b/drivers/media/radio/radio-sf16fmi.c
@@ -27,6 +27,7 @@
 #include <linux/io.h>		/* outb, outb_p			*/
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include "lm7000.h"
 
 MODULE_AUTHOR("Petr Vandrovec, vandrove@vc.cvut.cz and M. Kirkwood");
 MODULE_DESCRIPTION("A driver for the SF16-FMI, SF16-FMP and SF16-FMD radio.");
@@ -54,31 +55,33 @@ static struct fmi fmi_card;
 static struct pnp_dev *dev;
 bool pnp_attached;
 
-/* freq is in 1/16 kHz to internal number, hw precision is 50 kHz */
-/* It is only useful to give freq in interval of 800 (=0.05Mhz),
- * other bits will be truncated, e.g 92.7400016 -> 92.7, but
- * 92.7400017 -> 92.75
- */
-#define RSF16_ENCODE(x)	((x) / 800 + 214)
 #define RSF16_MINFREQ (87 * 16000)
 #define RSF16_MAXFREQ (108 * 16000)
 
-static void outbits(int bits, unsigned int data, int io)
+#define FMI_BIT_TUN_CE		(1 << 0)
+#define FMI_BIT_TUN_CLK		(1 << 1)
+#define FMI_BIT_TUN_DATA	(1 << 2)
+#define FMI_BIT_VOL_SW		(1 << 3)
+#define FMI_BIT_TUN_STRQ	(1 << 4)
+
+void fmi_set_pins(void *handle, u8 pins)
 {
-	while (bits--) {
-		if (data & 1) {
-			outb(5, io);
-			udelay(6);
-			outb(7, io);
-			udelay(6);
-		} else {
-			outb(1, io);
-			udelay(6);
-			outb(3, io);
-			udelay(6);
-		}
-		data >>= 1;
-	}
+	struct fmi *fmi = handle;
+	u8 bits = FMI_BIT_TUN_STRQ;
+
+	if (!fmi->mute)
+		bits |= FMI_BIT_VOL_SW;
+
+	if (pins & LM7000_DATA)
+		bits |= FMI_BIT_TUN_DATA;
+	if (pins & LM7000_CLK)
+		bits |= FMI_BIT_TUN_CLK;
+	if (pins & LM7000_CE)
+		bits |= FMI_BIT_TUN_CE;
+
+	mutex_lock(&fmi->lock);
+	outb_p(bits, fmi->io);
+	mutex_unlock(&fmi->lock);
 }
 
 static inline void fmi_mute(struct fmi *fmi)
@@ -95,20 +98,6 @@ static inline void fmi_unmute(struct fmi *fmi)
 	mutex_unlock(&fmi->lock);
 }
 
-static inline int fmi_setfreq(struct fmi *fmi, unsigned long freq)
-{
-	mutex_lock(&fmi->lock);
-	fmi->curfreq = freq;
-
-	outbits(16, RSF16_ENCODE(freq), fmi->io);
-	outbits(8, 0xC0, fmi->io);
-	msleep(143);		/* was schedule_timeout(HZ/7) */
-	mutex_unlock(&fmi->lock);
-	if (!fmi->mute)
-		fmi_unmute(fmi);
-	return 0;
-}
-
 static inline int fmi_getsigstr(struct fmi *fmi)
 {
 	int val;
@@ -173,7 +162,7 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 		return -EINVAL;
 	/* rounding in steps of 800 to match the freq
 	   that will be used */
-	fmi_setfreq(fmi, (f->frequency / 800) * 800);
+	lm7000_set_freq((f->frequency / 800) * 800, fmi, fmi_set_pins);
 	return 0;
 }
 
diff --git a/drivers/media/radio/radio-shark.c b/drivers/media/radio/radio-shark.c
new file mode 100644
index 00000000000..d0b6bb50763
--- /dev/null
+++ b/drivers/media/radio/radio-shark.c
@@ -0,0 +1,376 @@
+/*
+ * Linux V4L2 radio driver for the Griffin radioSHARK USB radio receiver
+ *
+ * Note the radioSHARK offers the audio through a regular USB audio device,
+ * this driver only handles the tuning.
+ *
+ * The info necessary to drive the shark was taken from the small userspace
+ * shark.c program by Michael Rolig, which he kindly placed in the Public
+ * Domain.
+ *
+ * Copyright (c) 2012 Hans de Goede <hdegoede@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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/init.h>
+#include <linux/kernel.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-device.h>
+#include <sound/tea575x-tuner.h>
+
+/*
+ * Version Information
+ */
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_DESCRIPTION("Griffin radioSHARK, USB radio receiver driver");
+MODULE_LICENSE("GPL");
+
+#define SHARK_IN_EP		0x83
+#define SHARK_OUT_EP		0x05
+
+#define TEA575X_BIT_MONO	(1<<22)		/* 0 = stereo, 1 = mono */
+#define TEA575X_BIT_BAND_MASK	(3<<20)
+#define TEA575X_BIT_BAND_FM	(0<<20)
+
+#define TB_LEN 6
+#define DRV_NAME "radioshark"
+
+#define v4l2_dev_to_shark(d) container_of(d, struct shark_device, v4l2_dev)
+
+enum { BLUE_LED, BLUE_PULSE_LED, RED_LED, NO_LEDS };
+
+static void shark_led_set_blue(struct led_classdev *led_cdev,
+			       enum led_brightness value);
+static void shark_led_set_blue_pulse(struct led_classdev *led_cdev,
+				     enum led_brightness value);
+static void shark_led_set_red(struct led_classdev *led_cdev,
+			      enum led_brightness value);
+
+static const struct led_classdev shark_led_templates[NO_LEDS] = {
+	[BLUE_LED] = {
+		.name		= "%s:blue:",
+		.brightness	= LED_OFF,
+		.max_brightness = 127,
+		.brightness_set = shark_led_set_blue,
+	},
+	[BLUE_PULSE_LED] = {
+		.name		= "%s:blue-pulse:",
+		.brightness	= LED_OFF,
+		.max_brightness = 255,
+		.brightness_set = shark_led_set_blue_pulse,
+	},
+	[RED_LED] = {
+		.name		= "%s:red:",
+		.brightness	= LED_OFF,
+		.max_brightness = 1,
+		.brightness_set = shark_led_set_red,
+	},
+};
+
+struct shark_device {
+	struct usb_device *usbdev;
+	struct v4l2_device v4l2_dev;
+	struct snd_tea575x tea;
+
+	struct work_struct led_work;
+	struct led_classdev leds[NO_LEDS];
+	char led_names[NO_LEDS][32];
+	atomic_t brightness[NO_LEDS];
+	unsigned long brightness_new;
+
+	u8 *transfer_buffer;
+	u32 last_val;
+};
+
+static atomic_t shark_instance = ATOMIC_INIT(0);
+
+static void shark_write_val(struct snd_tea575x *tea, u32 val)
+{
+	struct shark_device *shark = tea->private_data;
+	int i, res, actual_len;
+
+	/* Avoid unnecessary (slow) USB transfers */
+	if (shark->last_val == val)
+		return;
+
+	memset(shark->transfer_buffer, 0, TB_LEN);
+	shark->transfer_buffer[0] = 0xc0; /* Write shift register command */
+	for (i = 0; i < 4; i++)
+		shark->transfer_buffer[i] |= (val >> (24 - i * 8)) & 0xff;
+
+	res = usb_interrupt_msg(shark->usbdev,
+				usb_sndintpipe(shark->usbdev, SHARK_OUT_EP),
+				shark->transfer_buffer, TB_LEN,
+				&actual_len, 1000);
+	if (res >= 0)
+		shark->last_val = val;
+	else
+		v4l2_err(&shark->v4l2_dev, "set-freq error: %d\n", res);
+}
+
+static u32 shark_read_val(struct snd_tea575x *tea)
+{
+	struct shark_device *shark = tea->private_data;
+	int i, res, actual_len;
+	u32 val = 0;
+
+	memset(shark->transfer_buffer, 0, TB_LEN);
+	shark->transfer_buffer[0] = 0x80;
+	res = usb_interrupt_msg(shark->usbdev,
+				usb_sndintpipe(shark->usbdev, SHARK_OUT_EP),
+				shark->transfer_buffer, TB_LEN,
+				&actual_len, 1000);
+	if (res < 0) {
+		v4l2_err(&shark->v4l2_dev, "request-status error: %d\n", res);
+		return shark->last_val;
+	}
+
+	res = usb_interrupt_msg(shark->usbdev,
+				usb_rcvintpipe(shark->usbdev, SHARK_IN_EP),
+				shark->transfer_buffer, TB_LEN,
+				&actual_len, 1000);
+	if (res < 0) {
+		v4l2_err(&shark->v4l2_dev, "get-status error: %d\n", res);
+		return shark->last_val;
+	}
+
+	for (i = 0; i < 4; i++)
+		val |= shark->transfer_buffer[i] << (24 - i * 8);
+
+	shark->last_val = val;
+
+	/*
+	 * The shark does not allow actually reading the stereo / mono pin :(
+	 * So assume that when we're tuned to an FM station and mono has not
+	 * been requested, that we're receiving stereo.
+	 */
+	if (((val & TEA575X_BIT_BAND_MASK) == TEA575X_BIT_BAND_FM) &&
+	    !(val & TEA575X_BIT_MONO))
+		shark->tea.stereo = true;
+	else
+		shark->tea.stereo = false;
+
+	return val;
+}
+
+static struct snd_tea575x_ops shark_tea_ops = {
+	.write_val = shark_write_val,
+	.read_val  = shark_read_val,
+};
+
+static void shark_led_work(struct work_struct *work)
+{
+	struct shark_device *shark =
+		container_of(work, struct shark_device, led_work);
+	int i, res, brightness, actual_len;
+
+	/*
+	 * We use the v4l2_dev lock and registered bit to ensure the device
+	 * does not get unplugged and unreffed while we're running.
+	 */
+	mutex_lock(&shark->tea.mutex);
+	if (!video_is_registered(&shark->tea.vd))
+		goto leave;
+
+	for (i = 0; i < 3; i++) {
+		if (!test_and_clear_bit(i, &shark->brightness_new))
+			continue;
+
+		brightness = atomic_read(&shark->brightness[i]);
+		memset(shark->transfer_buffer, 0, TB_LEN);
+		if (i != RED_LED) {
+			shark->transfer_buffer[0] = 0xA0 + i;
+			shark->transfer_buffer[1] = brightness;
+		} else
+			shark->transfer_buffer[0] = brightness ? 0xA9 : 0xA8;
+		res = usb_interrupt_msg(shark->usbdev,
+					usb_sndintpipe(shark->usbdev, 0x05),
+					shark->transfer_buffer, TB_LEN,
+					&actual_len, 1000);
+		if (res < 0)
+			v4l2_err(&shark->v4l2_dev, "set LED %s error: %d\n",
+				 shark->led_names[i], res);
+	}
+leave:
+	mutex_unlock(&shark->tea.mutex);
+}
+
+static void shark_led_set_blue(struct led_classdev *led_cdev,
+			       enum led_brightness value)
+{
+	struct shark_device *shark =
+		container_of(led_cdev, struct shark_device, leds[BLUE_LED]);
+
+	atomic_set(&shark->brightness[BLUE_LED], value);
+	set_bit(BLUE_LED, &shark->brightness_new);
+	schedule_work(&shark->led_work);
+}
+
+static void shark_led_set_blue_pulse(struct led_classdev *led_cdev,
+				     enum led_brightness value)
+{
+	struct shark_device *shark = container_of(led_cdev,
+				struct shark_device, leds[BLUE_PULSE_LED]);
+
+	atomic_set(&shark->brightness[BLUE_PULSE_LED], 256 - value);
+	set_bit(BLUE_PULSE_LED, &shark->brightness_new);
+	schedule_work(&shark->led_work);
+}
+
+static void shark_led_set_red(struct led_classdev *led_cdev,
+			      enum led_brightness value)
+{
+	struct shark_device *shark =
+		container_of(led_cdev, struct shark_device, leds[RED_LED]);
+
+	atomic_set(&shark->brightness[RED_LED], value);
+	set_bit(RED_LED, &shark->brightness_new);
+	schedule_work(&shark->led_work);
+}
+
+static void usb_shark_disconnect(struct usb_interface *intf)
+{
+	struct v4l2_device *v4l2_dev = usb_get_intfdata(intf);
+	struct shark_device *shark = v4l2_dev_to_shark(v4l2_dev);
+	int i;
+
+	mutex_lock(&shark->tea.mutex);
+	v4l2_device_disconnect(&shark->v4l2_dev);
+	snd_tea575x_exit(&shark->tea);
+	mutex_unlock(&shark->tea.mutex);
+
+	for (i = 0; i < NO_LEDS; i++)
+		led_classdev_unregister(&shark->leds[i]);
+
+	v4l2_device_put(&shark->v4l2_dev);
+}
+
+static void usb_shark_release(struct v4l2_device *v4l2_dev)
+{
+	struct shark_device *shark = v4l2_dev_to_shark(v4l2_dev);
+
+	cancel_work_sync(&shark->led_work);
+	v4l2_device_unregister(&shark->v4l2_dev);
+	kfree(shark->transfer_buffer);
+	kfree(shark);
+}
+
+static int usb_shark_probe(struct usb_interface *intf,
+			   const struct usb_device_id *id)
+{
+	struct shark_device *shark;
+	int i, retval = -ENOMEM;
+
+	shark = kzalloc(sizeof(struct shark_device), GFP_KERNEL);
+	if (!shark)
+		return retval;
+
+	shark->transfer_buffer = kmalloc(TB_LEN, GFP_KERNEL);
+	if (!shark->transfer_buffer)
+		goto err_alloc_buffer;
+
+	/*
+	 * Work around a bug in usbhid/hid-core.c, where it leaves a dangling
+	 * pointer in intfdata causing v4l2-device.c to not set it. Which
+	 * results in usb_shark_disconnect() referencing the dangling pointer
+	 *
+	 * REMOVE (as soon as the above bug is fixed, patch submitted)
+	 */
+	usb_set_intfdata(intf, NULL);
+
+	shark->v4l2_dev.release = usb_shark_release;
+	v4l2_device_set_name(&shark->v4l2_dev, DRV_NAME, &shark_instance);
+	retval = v4l2_device_register(&intf->dev, &shark->v4l2_dev);
+	if (retval) {
+		v4l2_err(&shark->v4l2_dev, "couldn't register v4l2_device\n");
+		goto err_reg_dev;
+	}
+
+	shark->usbdev = interface_to_usbdev(intf);
+	shark->tea.v4l2_dev = &shark->v4l2_dev;
+	shark->tea.private_data = shark;
+	shark->tea.radio_nr = -1;
+	shark->tea.ops = &shark_tea_ops;
+	shark->tea.cannot_mute = true;
+	strlcpy(shark->tea.card, "Griffin radioSHARK",
+		sizeof(shark->tea.card));
+	usb_make_path(shark->usbdev, shark->tea.bus_info,
+		sizeof(shark->tea.bus_info));
+
+	retval = snd_tea575x_init(&shark->tea, THIS_MODULE);
+	if (retval) {
+		v4l2_err(&shark->v4l2_dev, "couldn't init tea5757\n");
+		goto err_init_tea;
+	}
+
+	INIT_WORK(&shark->led_work, shark_led_work);
+	for (i = 0; i < NO_LEDS; i++) {
+		shark->leds[i] = shark_led_templates[i];
+		snprintf(shark->led_names[i], sizeof(shark->led_names[0]),
+			 shark->leds[i].name, shark->v4l2_dev.name);
+		shark->leds[i].name = shark->led_names[i];
+		/*
+		 * We don't fail the probe if we fail to register the leds,
+		 * because once we've called snd_tea575x_init, the /dev/radio0
+		 * node may be opened from userspace holding a reference to us!
+		 *
+		 * Note we cannot register the leds first instead as
+		 * shark_led_work depends on the v4l2 mutex and registered bit.
+		 */
+		retval = led_classdev_register(&intf->dev, &shark->leds[i]);
+		if (retval)
+			v4l2_err(&shark->v4l2_dev,
+				 "couldn't register led: %s\n",
+				 shark->led_names[i]);
+	}
+
+	return 0;
+
+err_init_tea:
+	v4l2_device_unregister(&shark->v4l2_dev);
+err_reg_dev:
+	kfree(shark->transfer_buffer);
+err_alloc_buffer:
+	kfree(shark);
+
+	return retval;
+}
+
+/* Specify the bcdDevice value, as the radioSHARK and radioSHARK2 share ids */
+static struct usb_device_id usb_shark_device_table[] = {
+	{ .match_flags = USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION |
+			 USB_DEVICE_ID_MATCH_INT_CLASS,
+	  .idVendor     = 0x077d,
+	  .idProduct    = 0x627a,
+	  .bcdDevice_lo = 0x0001,
+	  .bcdDevice_hi = 0x0001,
+	  .bInterfaceClass = 3,
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, usb_shark_device_table);
+
+static struct usb_driver usb_shark_driver = {
+	.name			= DRV_NAME,
+	.probe			= usb_shark_probe,
+	.disconnect		= usb_shark_disconnect,
+	.id_table		= usb_shark_device_table,
+};
+module_usb_driver(usb_shark_driver);
diff --git a/drivers/media/radio/radio-shark2.c b/drivers/media/radio/radio-shark2.c
new file mode 100644
index 00000000000..b9575de3e7e
--- /dev/null
+++ b/drivers/media/radio/radio-shark2.c
@@ -0,0 +1,348 @@
+/*
+ * Linux V4L2 radio driver for the Griffin radioSHARK2 USB radio receiver
+ *
+ * Note the radioSHARK2 offers the audio through a regular USB audio device,
+ * this driver only handles the tuning.
+ *
+ * The info necessary to drive the shark2 was taken from the small userspace
+ * shark2.c program by Hisaaki Shibata, which he kindly placed in the Public
+ * Domain.
+ *
+ * Copyright (c) 2012 Hans de Goede <hdegoede@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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/init.h>
+#include <linux/kernel.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-device.h>
+#include "radio-tea5777.h"
+
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_DESCRIPTION("Griffin radioSHARK2, USB radio receiver driver");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+
+#define SHARK_IN_EP		0x83
+#define SHARK_OUT_EP		0x05
+
+#define TB_LEN 7
+#define DRV_NAME "radioshark2"
+
+#define v4l2_dev_to_shark(d) container_of(d, struct shark_device, v4l2_dev)
+
+enum { BLUE_LED, RED_LED, NO_LEDS };
+
+static void shark_led_set_blue(struct led_classdev *led_cdev,
+			       enum led_brightness value);
+static void shark_led_set_red(struct led_classdev *led_cdev,
+			      enum led_brightness value);
+
+static const struct led_classdev shark_led_templates[NO_LEDS] = {
+	[BLUE_LED] = {
+		.name		= "%s:blue:",
+		.brightness	= LED_OFF,
+		.max_brightness = 127,
+		.brightness_set = shark_led_set_blue,
+	},
+	[RED_LED] = {
+		.name		= "%s:red:",
+		.brightness	= LED_OFF,
+		.max_brightness = 1,
+		.brightness_set = shark_led_set_red,
+	},
+};
+
+struct shark_device {
+	struct usb_device *usbdev;
+	struct v4l2_device v4l2_dev;
+	struct radio_tea5777 tea;
+
+	struct work_struct led_work;
+	struct led_classdev leds[NO_LEDS];
+	char led_names[NO_LEDS][32];
+	atomic_t brightness[NO_LEDS];
+	unsigned long brightness_new;
+
+	u8 *transfer_buffer;
+};
+
+static atomic_t shark_instance = ATOMIC_INIT(0);
+
+static int shark_write_reg(struct radio_tea5777 *tea, u64 reg)
+{
+	struct shark_device *shark = tea->private_data;
+	int i, res, actual_len;
+
+	memset(shark->transfer_buffer, 0, TB_LEN);
+	shark->transfer_buffer[0] = 0x81; /* Write register command */
+	for (i = 0; i < 6; i++)
+		shark->transfer_buffer[i + 1] = (reg >> (40 - i * 8)) & 0xff;
+
+	v4l2_dbg(1, debug, tea->v4l2_dev,
+		 "shark2-write: %02x %02x %02x %02x %02x %02x %02x\n",
+		 shark->transfer_buffer[0], shark->transfer_buffer[1],
+		 shark->transfer_buffer[2], shark->transfer_buffer[3],
+		 shark->transfer_buffer[4], shark->transfer_buffer[5],
+		 shark->transfer_buffer[6]);
+
+	res = usb_interrupt_msg(shark->usbdev,
+				usb_sndintpipe(shark->usbdev, SHARK_OUT_EP),
+				shark->transfer_buffer, TB_LEN,
+				&actual_len, 1000);
+	if (res < 0) {
+		v4l2_err(tea->v4l2_dev, "write error: %d\n", res);
+		return res;
+	}
+
+	return 0;
+}
+
+static int shark_read_reg(struct radio_tea5777 *tea, u32 *reg_ret)
+{
+	struct shark_device *shark = tea->private_data;
+	int i, res, actual_len;
+	u32 reg = 0;
+
+	memset(shark->transfer_buffer, 0, TB_LEN);
+	shark->transfer_buffer[0] = 0x82;
+	res = usb_interrupt_msg(shark->usbdev,
+				usb_sndintpipe(shark->usbdev, SHARK_OUT_EP),
+				shark->transfer_buffer, TB_LEN,
+				&actual_len, 1000);
+	if (res < 0) {
+		v4l2_err(tea->v4l2_dev, "request-read error: %d\n", res);
+		return res;
+	}
+
+	res = usb_interrupt_msg(shark->usbdev,
+				usb_rcvintpipe(shark->usbdev, SHARK_IN_EP),
+				shark->transfer_buffer, TB_LEN,
+				&actual_len, 1000);
+	if (res < 0) {
+		v4l2_err(tea->v4l2_dev, "read error: %d\n", res);
+		return res;
+	}
+
+	for (i = 0; i < 3; i++)
+		reg |= shark->transfer_buffer[i] << (16 - i * 8);
+
+	v4l2_dbg(1, debug, tea->v4l2_dev, "shark2-read: %02x %02x %02x\n",
+		 shark->transfer_buffer[0], shark->transfer_buffer[1],
+		 shark->transfer_buffer[2]);
+
+	*reg_ret = reg;
+	return 0;
+}
+
+static struct radio_tea5777_ops shark_tea_ops = {
+	.write_reg = shark_write_reg,
+	.read_reg  = shark_read_reg,
+};
+
+static void shark_led_work(struct work_struct *work)
+{
+	struct shark_device *shark =
+		container_of(work, struct shark_device, led_work);
+	int i, res, brightness, actual_len;
+	/*
+	 * We use the v4l2_dev lock and registered bit to ensure the device
+	 * does not get unplugged and unreffed while we're running.
+	 */
+	mutex_lock(&shark->tea.mutex);
+	if (!video_is_registered(&shark->tea.vd))
+		goto leave;
+
+	for (i = 0; i < 2; i++) {
+		if (!test_and_clear_bit(i, &shark->brightness_new))
+			continue;
+
+		brightness = atomic_read(&shark->brightness[i]);
+		memset(shark->transfer_buffer, 0, TB_LEN);
+		shark->transfer_buffer[0] = 0x83 + i;
+		shark->transfer_buffer[1] = brightness;
+		res = usb_interrupt_msg(shark->usbdev,
+					usb_sndintpipe(shark->usbdev,
+						       SHARK_OUT_EP),
+					shark->transfer_buffer, TB_LEN,
+					&actual_len, 1000);
+		if (res < 0)
+			v4l2_err(&shark->v4l2_dev, "set LED %s error: %d\n",
+				 shark->led_names[i], res);
+	}
+leave:
+	mutex_unlock(&shark->tea.mutex);
+}
+
+static void shark_led_set_blue(struct led_classdev *led_cdev,
+			       enum led_brightness value)
+{
+	struct shark_device *shark =
+		container_of(led_cdev, struct shark_device, leds[BLUE_LED]);
+
+	atomic_set(&shark->brightness[BLUE_LED], value);
+	set_bit(BLUE_LED, &shark->brightness_new);
+	schedule_work(&shark->led_work);
+}
+
+static void shark_led_set_red(struct led_classdev *led_cdev,
+			      enum led_brightness value)
+{
+	struct shark_device *shark =
+		container_of(led_cdev, struct shark_device, leds[RED_LED]);
+
+	atomic_set(&shark->brightness[RED_LED], value);
+	set_bit(RED_LED, &shark->brightness_new);
+	schedule_work(&shark->led_work);
+}
+
+static void usb_shark_disconnect(struct usb_interface *intf)
+{
+	struct v4l2_device *v4l2_dev = usb_get_intfdata(intf);
+	struct shark_device *shark = v4l2_dev_to_shark(v4l2_dev);
+	int i;
+
+	mutex_lock(&shark->tea.mutex);
+	v4l2_device_disconnect(&shark->v4l2_dev);
+	radio_tea5777_exit(&shark->tea);
+	mutex_unlock(&shark->tea.mutex);
+
+	for (i = 0; i < NO_LEDS; i++)
+		led_classdev_unregister(&shark->leds[i]);
+
+	v4l2_device_put(&shark->v4l2_dev);
+}
+
+static void usb_shark_release(struct v4l2_device *v4l2_dev)
+{
+	struct shark_device *shark = v4l2_dev_to_shark(v4l2_dev);
+
+	cancel_work_sync(&shark->led_work);
+	v4l2_device_unregister(&shark->v4l2_dev);
+	kfree(shark->transfer_buffer);
+	kfree(shark);
+}
+
+static int usb_shark_probe(struct usb_interface *intf,
+			   const struct usb_device_id *id)
+{
+	struct shark_device *shark;
+	int i, retval = -ENOMEM;
+
+	shark = kzalloc(sizeof(struct shark_device), GFP_KERNEL);
+	if (!shark)
+		return retval;
+
+	shark->transfer_buffer = kmalloc(TB_LEN, GFP_KERNEL);
+	if (!shark->transfer_buffer)
+		goto err_alloc_buffer;
+
+	/*
+	 * Work around a bug in usbhid/hid-core.c, where it leaves a dangling
+	 * pointer in intfdata causing v4l2-device.c to not set it. Which
+	 * results in usb_shark_disconnect() referencing the dangling pointer
+	 *
+	 * REMOVE (as soon as the above bug is fixed, patch submitted)
+	 */
+	usb_set_intfdata(intf, NULL);
+
+	shark->v4l2_dev.release = usb_shark_release;
+	v4l2_device_set_name(&shark->v4l2_dev, DRV_NAME, &shark_instance);
+	retval = v4l2_device_register(&intf->dev, &shark->v4l2_dev);
+	if (retval) {
+		v4l2_err(&shark->v4l2_dev, "couldn't register v4l2_device\n");
+		goto err_reg_dev;
+	}
+
+	shark->usbdev = interface_to_usbdev(intf);
+	shark->tea.v4l2_dev = &shark->v4l2_dev;
+	shark->tea.private_data = shark;
+	shark->tea.ops = &shark_tea_ops;
+	shark->tea.has_am = true;
+	shark->tea.write_before_read = true;
+	strlcpy(shark->tea.card, "Griffin radioSHARK2",
+		sizeof(shark->tea.card));
+	usb_make_path(shark->usbdev, shark->tea.bus_info,
+		sizeof(shark->tea.bus_info));
+
+	retval = radio_tea5777_init(&shark->tea, THIS_MODULE);
+	if (retval) {
+		v4l2_err(&shark->v4l2_dev, "couldn't init tea5777\n");
+		goto err_init_tea;
+	}
+
+	INIT_WORK(&shark->led_work, shark_led_work);
+	for (i = 0; i < NO_LEDS; i++) {
+		shark->leds[i] = shark_led_templates[i];
+		snprintf(shark->led_names[i], sizeof(shark->led_names[0]),
+			 shark->leds[i].name, shark->v4l2_dev.name);
+		shark->leds[i].name = shark->led_names[i];
+		/*
+		 * We don't fail the probe if we fail to register the leds,
+		 * because once we've called radio_tea5777_init, the /dev/radio0
+		 * node may be opened from userspace holding a reference to us!
+		 *
+		 * Note we cannot register the leds first instead as
+		 * shark_led_work depends on the v4l2 mutex and registered bit.
+		 */
+		retval = led_classdev_register(&intf->dev, &shark->leds[i]);
+		if (retval)
+			v4l2_err(&shark->v4l2_dev,
+				 "couldn't register led: %s\n",
+				 shark->led_names[i]);
+	}
+
+	return 0;
+
+err_init_tea:
+	v4l2_device_unregister(&shark->v4l2_dev);
+err_reg_dev:
+	kfree(shark->transfer_buffer);
+err_alloc_buffer:
+	kfree(shark);
+
+	return retval;
+}
+
+/* Specify the bcdDevice value, as the radioSHARK and radioSHARK2 share ids */
+static struct usb_device_id usb_shark_device_table[] = {
+	{ .match_flags = USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION |
+			 USB_DEVICE_ID_MATCH_INT_CLASS,
+	  .idVendor     = 0x077d,
+	  .idProduct    = 0x627a,
+	  .bcdDevice_lo = 0x0010,
+	  .bcdDevice_hi = 0x0010,
+	  .bInterfaceClass = 3,
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, usb_shark_device_table);
+
+static struct usb_driver usb_shark_driver = {
+	.name			= DRV_NAME,
+	.probe			= usb_shark_probe,
+	.disconnect		= usb_shark_disconnect,
+	.id_table		= usb_shark_device_table,
+};
+module_usb_driver(usb_shark_driver);
diff --git a/drivers/media/radio/radio-tea5777.c b/drivers/media/radio/radio-tea5777.c
new file mode 100644
index 00000000000..5bc9fa62720
--- /dev/null
+++ b/drivers/media/radio/radio-tea5777.c
@@ -0,0 +1,491 @@
+/*
+ *   v4l2 driver for TEA5777 Philips AM/FM radio tuner chips
+ *
+ *	Copyright (c) 2012 Hans de Goede <hdegoede@redhat.com>
+ *
+ *   Based on the ALSA driver for TEA5757/5759 Philips AM/FM radio tuner chips:
+ *
+ *	Copyright (c) 2004 Jaroslav Kysela <perex@perex.cz>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You 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 <linux/init.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <asm/div64.h>
+#include "radio-tea5777.h"
+
+MODULE_AUTHOR("Hans de Goede <perex@perex.cz>");
+MODULE_DESCRIPTION("Routines for control of TEA5777 Philips AM/FM radio tuner chips");
+MODULE_LICENSE("GPL");
+
+/* Fixed FM only band for now, will implement multi-band support when the
+   VIDIOC_ENUM_FREQ_BANDS API is upstream */
+#define TEA5777_FM_RANGELOW		(76000 * 16)
+#define TEA5777_FM_RANGEHIGH		(108000 * 16)
+
+#define TEA5777_FM_IF			150 /* kHz */
+#define TEA5777_FM_FREQ_STEP		50 /* kHz */
+
+/* Write reg, common bits */
+#define TEA5777_W_MUTE_MASK		(1LL << 47)
+#define TEA5777_W_MUTE_SHIFT		47
+#define TEA5777_W_AM_FM_MASK		(1LL << 46)
+#define TEA5777_W_AM_FM_SHIFT		46
+#define TEA5777_W_STB_MASK		(1LL << 45)
+#define TEA5777_W_STB_SHIFT		45
+
+#define TEA5777_W_IFCE_MASK		(1LL << 29)
+#define TEA5777_W_IFCE_SHIFT		29
+#define TEA5777_W_IFW_MASK		(1LL << 28)
+#define TEA5777_W_IFW_SHIFT		28
+#define TEA5777_W_HILO_MASK		(1LL << 27)
+#define TEA5777_W_HILO_SHIFT		27
+#define TEA5777_W_DBUS_MASK		(1LL << 26)
+#define TEA5777_W_DBUS_SHIFT		26
+
+#define TEA5777_W_INTEXT_MASK		(1LL << 24)
+#define TEA5777_W_INTEXT_SHIFT		24
+#define TEA5777_W_P1_MASK		(1LL << 23)
+#define TEA5777_W_P1_SHIFT		23
+#define TEA5777_W_P0_MASK		(1LL << 22)
+#define TEA5777_W_P0_SHIFT		22
+#define TEA5777_W_PEN1_MASK		(1LL << 21)
+#define TEA5777_W_PEN1_SHIFT		21
+#define TEA5777_W_PEN0_MASK		(1LL << 20)
+#define TEA5777_W_PEN0_SHIFT		20
+
+#define TEA5777_W_CHP0_MASK		(1LL << 18)
+#define TEA5777_W_CHP0_SHIFT		18
+#define TEA5777_W_DEEM_MASK		(1LL << 17)
+#define TEA5777_W_DEEM_SHIFT		17
+
+#define TEA5777_W_SEARCH_MASK		(1LL << 7)
+#define TEA5777_W_SEARCH_SHIFT		7
+#define TEA5777_W_PROGBLIM_MASK		(1LL << 6)
+#define TEA5777_W_PROGBLIM_SHIFT	6
+#define TEA5777_W_UPDWN_MASK		(1LL << 5)
+#define TEA5777_W_UPDWN_SHIFT		5
+#define TEA5777_W_SLEV_MASK		(3LL << 3)
+#define TEA5777_W_SLEV_SHIFT		3
+
+/* Write reg, FM specific bits */
+#define TEA5777_W_FM_PLL_MASK		(0x1fffLL << 32)
+#define TEA5777_W_FM_PLL_SHIFT		32
+#define TEA5777_W_FM_FREF_MASK		(0x03LL << 30)
+#define TEA5777_W_FM_FREF_SHIFT		30
+#define TEA5777_W_FM_FREF_VALUE		0 /* 50 kHz tune steps, 150 kHz IF */
+
+#define TEA5777_W_FM_FORCEMONO_MASK	(1LL << 15)
+#define TEA5777_W_FM_FORCEMONO_SHIFT	15
+#define TEA5777_W_FM_SDSOFF_MASK	(1LL << 14)
+#define TEA5777_W_FM_SDSOFF_SHIFT	14
+#define TEA5777_W_FM_DOFF_MASK		(1LL << 13)
+#define TEA5777_W_FM_DOFF_SHIFT		13
+
+#define TEA5777_W_FM_STEP_MASK		(3LL << 1)
+#define TEA5777_W_FM_STEP_SHIFT		1
+
+/* Write reg, AM specific bits */
+#define TEA5777_W_AM_PLL_MASK		(0x7ffLL << 34)
+#define TEA5777_W_AM_PLL_SHIFT		34
+#define TEA5777_W_AM_AGCRF_MASK		(1LL << 33)
+#define TEA5777_W_AM_AGCRF_SHIFT	33
+#define TEA5777_W_AM_AGCIF_MASK		(1LL << 32)
+#define TEA5777_W_AM_AGCIF_SHIFT	32
+#define TEA5777_W_AM_MWLW_MASK		(1LL << 31)
+#define TEA5777_W_AM_MWLW_SHIFT		31
+#define TEA5777_W_AM_LNA_MASK		(1LL << 30)
+#define TEA5777_W_AM_LNA_SHIFT		30
+
+#define TEA5777_W_AM_PEAK_MASK		(1LL << 25)
+#define TEA5777_W_AM_PEAK_SHIFT		25
+
+#define TEA5777_W_AM_RFB_MASK		(1LL << 16)
+#define TEA5777_W_AM_RFB_SHIFT		16
+#define TEA5777_W_AM_CALLIGN_MASK	(1LL << 15)
+#define TEA5777_W_AM_CALLIGN_SHIFT	15
+#define TEA5777_W_AM_CBANK_MASK		(0x7fLL << 8)
+#define TEA5777_W_AM_CBANK_SHIFT	8
+
+#define TEA5777_W_AM_DELAY_MASK		(1LL << 2)
+#define TEA5777_W_AM_DELAY_SHIFT	2
+#define TEA5777_W_AM_STEP_MASK		(1LL << 1)
+#define TEA5777_W_AM_STEP_SHIFT		1
+
+/* Read reg, common bits */
+#define TEA5777_R_LEVEL_MASK		(0x0f << 17)
+#define TEA5777_R_LEVEL_SHIFT		17
+#define TEA5777_R_SFOUND_MASK		(0x01 << 16)
+#define TEA5777_R_SFOUND_SHIFT		16
+#define TEA5777_R_BLIM_MASK		(0x01 << 15)
+#define TEA5777_R_BLIM_SHIFT		15
+
+/* Read reg, FM specific bits */
+#define TEA5777_R_FM_STEREO_MASK	(0x01 << 21)
+#define TEA5777_R_FM_STEREO_SHIFT	21
+#define TEA5777_R_FM_PLL_MASK		0x1fff
+#define TEA5777_R_FM_PLL_SHIFT		0
+
+static u32 tea5777_freq_to_v4l2_freq(struct radio_tea5777 *tea, u32 freq)
+{
+	return (freq * TEA5777_FM_FREQ_STEP + TEA5777_FM_IF) * 16;
+}
+
+static int radio_tea5777_set_freq(struct radio_tea5777 *tea)
+{
+	u64 freq;
+	int res;
+
+	freq = clamp_t(u32, tea->freq,
+		       TEA5777_FM_RANGELOW, TEA5777_FM_RANGEHIGH) + 8;
+	do_div(freq, 16); /* to kHz */
+
+	freq -= TEA5777_FM_IF;
+	do_div(freq, TEA5777_FM_FREQ_STEP);
+
+	tea->write_reg &= ~(TEA5777_W_FM_PLL_MASK | TEA5777_W_FM_FREF_MASK);
+	tea->write_reg |= freq << TEA5777_W_FM_PLL_SHIFT;
+	tea->write_reg |= TEA5777_W_FM_FREF_VALUE << TEA5777_W_FM_FREF_SHIFT;
+
+	res = tea->ops->write_reg(tea, tea->write_reg);
+	if (res)
+		return res;
+
+	tea->needs_write = false;
+	tea->read_reg = -1;
+	tea->freq = tea5777_freq_to_v4l2_freq(tea, freq);
+
+	return 0;
+}
+
+static int radio_tea5777_update_read_reg(struct radio_tea5777 *tea, int wait)
+{
+	int res;
+
+	if (tea->read_reg != -1)
+		return 0;
+
+	if (tea->write_before_read && tea->needs_write) {
+		res = radio_tea5777_set_freq(tea);
+		if (res)
+			return res;
+	}
+
+	if (wait) {
+		if (schedule_timeout_interruptible(msecs_to_jiffies(wait)))
+			return -ERESTARTSYS;
+	}
+
+	res = tea->ops->read_reg(tea, &tea->read_reg);
+	if (res)
+		return res;
+
+	tea->needs_write = true;
+	return 0;
+}
+
+/*
+ * Linux Video interface
+ */
+
+static int vidioc_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *v)
+{
+	struct radio_tea5777 *tea = video_drvdata(file);
+
+	strlcpy(v->driver, tea->v4l2_dev->name, sizeof(v->driver));
+	strlcpy(v->card, tea->card, sizeof(v->card));
+	strlcat(v->card, " TEA5777", sizeof(v->card));
+	strlcpy(v->bus_info, tea->bus_info, sizeof(v->bus_info));
+	v->device_caps = V4L2_CAP_TUNER | V4L2_CAP_RADIO;
+	v->device_caps |= V4L2_CAP_HW_FREQ_SEEK;
+	v->capabilities = v->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+					struct v4l2_tuner *v)
+{
+	struct radio_tea5777 *tea = video_drvdata(file);
+	int res;
+
+	if (v->index > 0)
+		return -EINVAL;
+
+	res = radio_tea5777_update_read_reg(tea, 0);
+	if (res)
+		return res;
+
+	memset(v, 0, sizeof(*v));
+	if (tea->has_am)
+		strlcpy(v->name, "AM/FM", sizeof(v->name));
+	else
+		strlcpy(v->name, "FM", sizeof(v->name));
+	v->type = V4L2_TUNER_RADIO;
+	v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+	v->rangelow   = TEA5777_FM_RANGELOW;
+	v->rangehigh  = TEA5777_FM_RANGEHIGH;
+	v->rxsubchans = (tea->read_reg & TEA5777_R_FM_STEREO_MASK) ?
+			V4L2_TUNER_SUB_STEREO : V4L2_TUNER_SUB_MONO;
+	v->audmode = (tea->write_reg & TEA5777_W_FM_FORCEMONO_MASK) ?
+		V4L2_TUNER_MODE_MONO : V4L2_TUNER_MODE_STEREO;
+	/* shift - 12 to convert 4-bits (0-15) scale to 16-bits (0-65535) */
+	v->signal = (tea->read_reg & TEA5777_R_LEVEL_MASK) >>
+		    (TEA5777_R_LEVEL_SHIFT - 12);
+
+	/* Invalidate read_reg, so that next call we return up2date signal */
+	tea->read_reg = -1;
+
+	return 0;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+					struct v4l2_tuner *v)
+{
+	struct radio_tea5777 *tea = video_drvdata(file);
+
+	if (v->index)
+		return -EINVAL;
+
+	if (v->audmode == V4L2_TUNER_MODE_MONO)
+		tea->write_reg |= TEA5777_W_FM_FORCEMONO_MASK;
+	else
+		tea->write_reg &= ~TEA5777_W_FM_FORCEMONO_MASK;
+
+	return radio_tea5777_set_freq(tea);
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+					struct v4l2_frequency *f)
+{
+	struct radio_tea5777 *tea = video_drvdata(file);
+
+	if (f->tuner != 0)
+		return -EINVAL;
+	f->type = V4L2_TUNER_RADIO;
+	f->frequency = tea->freq;
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+					struct v4l2_frequency *f)
+{
+	struct radio_tea5777 *tea = video_drvdata(file);
+
+	if (f->tuner != 0 || f->type != V4L2_TUNER_RADIO)
+		return -EINVAL;
+
+	tea->freq = f->frequency;
+	return radio_tea5777_set_freq(tea);
+}
+
+static int vidioc_s_hw_freq_seek(struct file *file, void *fh,
+					struct v4l2_hw_freq_seek *a)
+{
+	struct radio_tea5777 *tea = video_drvdata(file);
+	u32 orig_freq = tea->freq;
+	unsigned long timeout;
+	int res, spacing = 200 * 16; /* 200 kHz */
+	/* These are fixed *for now* */
+	const u32 seek_rangelow  = TEA5777_FM_RANGELOW;
+	const u32 seek_rangehigh = TEA5777_FM_RANGEHIGH;
+
+	if (a->tuner || a->wrap_around)
+		return -EINVAL;
+
+	tea->write_reg |= TEA5777_W_PROGBLIM_MASK;
+	if (seek_rangelow != tea->seek_rangelow) {
+		tea->write_reg &= ~TEA5777_W_UPDWN_MASK;
+		tea->freq = seek_rangelow;
+		res = radio_tea5777_set_freq(tea);
+		if (res)
+			goto leave;
+		tea->seek_rangelow = tea->freq;
+	}
+	if (seek_rangehigh != tea->seek_rangehigh) {
+		tea->write_reg |= TEA5777_W_UPDWN_MASK;
+		tea->freq = seek_rangehigh;
+		res = radio_tea5777_set_freq(tea);
+		if (res)
+			goto leave;
+		tea->seek_rangehigh = tea->freq;
+	}
+	tea->write_reg &= ~TEA5777_W_PROGBLIM_MASK;
+
+	tea->write_reg |= TEA5777_W_SEARCH_MASK;
+	if (a->seek_upward) {
+		tea->write_reg |= TEA5777_W_UPDWN_MASK;
+		tea->freq = orig_freq + spacing;
+	} else {
+		tea->write_reg &= ~TEA5777_W_UPDWN_MASK;
+		tea->freq = orig_freq - spacing;
+	}
+	res = radio_tea5777_set_freq(tea);
+	if (res)
+		goto leave;
+
+	timeout = jiffies + msecs_to_jiffies(5000);
+	for (;;) {
+		if (time_after(jiffies, timeout)) {
+			res = -ENODATA;
+			break;
+		}
+
+		res = radio_tea5777_update_read_reg(tea, 100);
+		if (res)
+			break;
+
+		/*
+		 * Note we use tea->freq to track how far we've searched sofar
+		 * this is necessary to ensure we continue seeking at the right
+		 * point, in the write_before_read case.
+		 */
+		tea->freq = (tea->read_reg & TEA5777_R_FM_PLL_MASK);
+		tea->freq = tea5777_freq_to_v4l2_freq(tea, tea->freq);
+
+		if ((tea->read_reg & TEA5777_R_SFOUND_MASK)) {
+			tea->write_reg &= ~TEA5777_W_SEARCH_MASK;
+			return 0;
+		}
+
+		if (tea->read_reg & TEA5777_R_BLIM_MASK) {
+			res = -ENODATA;
+			break;
+		}
+
+		/* Force read_reg update */
+		tea->read_reg = -1;
+	}
+leave:
+	tea->write_reg &= ~TEA5777_W_PROGBLIM_MASK;
+	tea->write_reg &= ~TEA5777_W_SEARCH_MASK;
+	tea->freq = orig_freq;
+	radio_tea5777_set_freq(tea);
+	return res;
+}
+
+static int tea575x_s_ctrl(struct v4l2_ctrl *c)
+{
+	struct radio_tea5777 *tea =
+		container_of(c->handler, struct radio_tea5777, ctrl_handler);
+
+	switch (c->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		if (c->val)
+			tea->write_reg |= TEA5777_W_MUTE_MASK;
+		else
+			tea->write_reg &= ~TEA5777_W_MUTE_MASK;
+
+		return radio_tea5777_set_freq(tea);
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_file_operations tea575x_fops = {
+	.unlocked_ioctl	= video_ioctl2,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
+};
+
+static const struct v4l2_ioctl_ops tea575x_ioctl_ops = {
+	.vidioc_querycap    = vidioc_querycap,
+	.vidioc_g_tuner     = vidioc_g_tuner,
+	.vidioc_s_tuner     = vidioc_s_tuner,
+	.vidioc_g_frequency = vidioc_g_frequency,
+	.vidioc_s_frequency = vidioc_s_frequency,
+	.vidioc_s_hw_freq_seek = vidioc_s_hw_freq_seek,
+	.vidioc_log_status  = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static const struct video_device tea575x_radio = {
+	.ioctl_ops	= &tea575x_ioctl_ops,
+	.release	= video_device_release_empty,
+};
+
+static const struct v4l2_ctrl_ops tea575x_ctrl_ops = {
+	.s_ctrl = tea575x_s_ctrl,
+};
+
+int radio_tea5777_init(struct radio_tea5777 *tea, struct module *owner)
+{
+	int res;
+
+	tea->write_reg = (1LL << TEA5777_W_IFCE_SHIFT) |
+			 (1LL << TEA5777_W_IFW_SHIFT) |
+			 (1LL << TEA5777_W_INTEXT_SHIFT) |
+			 (1LL << TEA5777_W_CHP0_SHIFT) |
+			 (2LL << TEA5777_W_SLEV_SHIFT);
+	tea->freq = 90500 * 16;	/* 90.5Mhz default */
+	res = radio_tea5777_set_freq(tea);
+	if (res) {
+		v4l2_err(tea->v4l2_dev, "can't set initial freq (%d)\n", res);
+		return res;
+	}
+
+	tea->vd = tea575x_radio;
+	video_set_drvdata(&tea->vd, tea);
+	mutex_init(&tea->mutex);
+	strlcpy(tea->vd.name, tea->v4l2_dev->name, sizeof(tea->vd.name));
+	tea->vd.lock = &tea->mutex;
+	tea->vd.v4l2_dev = tea->v4l2_dev;
+	tea->fops = tea575x_fops;
+	tea->fops.owner = owner;
+	tea->vd.fops = &tea->fops;
+	set_bit(V4L2_FL_USE_FH_PRIO, &tea->vd.flags);
+
+	tea->vd.ctrl_handler = &tea->ctrl_handler;
+	v4l2_ctrl_handler_init(&tea->ctrl_handler, 1);
+	v4l2_ctrl_new_std(&tea->ctrl_handler, &tea575x_ctrl_ops,
+			  V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
+	res = tea->ctrl_handler.error;
+	if (res) {
+		v4l2_err(tea->v4l2_dev, "can't initialize controls\n");
+		v4l2_ctrl_handler_free(&tea->ctrl_handler);
+		return res;
+	}
+	v4l2_ctrl_handler_setup(&tea->ctrl_handler);
+
+	res = video_register_device(&tea->vd, VFL_TYPE_RADIO, -1);
+	if (res) {
+		v4l2_err(tea->v4l2_dev, "can't register video device!\n");
+		v4l2_ctrl_handler_free(tea->vd.ctrl_handler);
+		return res;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(radio_tea5777_init);
+
+void radio_tea5777_exit(struct radio_tea5777 *tea)
+{
+	video_unregister_device(&tea->vd);
+	v4l2_ctrl_handler_free(tea->vd.ctrl_handler);
+}
+EXPORT_SYMBOL_GPL(radio_tea5777_exit);
diff --git a/drivers/media/radio/radio-tea5777.h b/drivers/media/radio/radio-tea5777.h
new file mode 100644
index 00000000000..55cbd78df5e
--- /dev/null
+++ b/drivers/media/radio/radio-tea5777.h
@@ -0,0 +1,87 @@
+#ifndef __RADIO_TEA5777_H
+#define __RADIO_TEA5777_H
+
+/*
+ *   v4l2 driver for TEA5777 Philips AM/FM radio tuner chips
+ *
+ *	Copyright (c) 2012 Hans de Goede <hdegoede@redhat.com>
+ *
+ *   Based on the ALSA driver for TEA5757/5759 Philips AM/FM radio tuner chips:
+ *
+ *	Copyright (c) 2004 Jaroslav Kysela <perex@perex.cz>
+ *	Copyright (c) 2012 Hans de Goede <hdegoede@redhat.com>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You 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/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+
+#define TEA575X_FMIF	10700
+#define TEA575X_AMIF	  450
+
+struct radio_tea5777;
+
+struct radio_tea5777_ops {
+	/*
+	 * Write the 6 bytes large write register of the tea5777
+	 *
+	 * val represents the 6 write registers, with byte 1 from the
+	 * datasheet being the most significant byte (so byte 5 of the u64),
+	 * and byte 6 from the datasheet being the least significant byte.
+	 *
+	 * returns 0 on success.
+	 */
+	int (*write_reg)(struct radio_tea5777 *tea, u64 val);
+	/*
+	 * Read the 3 bytes large read register of the tea5777
+	 *
+	 * The read value gets returned in val, akin to write_reg, byte 1 from
+	 * the datasheet is stored as the most significant byte (so byte 2 of
+	 * the u32), and byte 3 from the datasheet gets stored as the least
+	 * significant byte (iow byte 0 of the u32).
+	 *
+	 * returns 0 on success.
+	 */
+	int (*read_reg)(struct radio_tea5777 *tea, u32 *val);
+};
+
+struct radio_tea5777 {
+	struct v4l2_device *v4l2_dev;
+	struct v4l2_file_operations fops;
+	struct video_device vd;		/* video device */
+	bool has_am;			/* Device can tune to AM freqs */
+	bool write_before_read;		/* must write before read quirk */
+	bool needs_write;		/* for write before read quirk */
+	u32 freq;			/* current frequency */
+	u32 seek_rangelow;		/* current hwseek limits */
+	u32 seek_rangehigh;
+	u32 read_reg;
+	u64 write_reg;
+	struct mutex mutex;
+	struct radio_tea5777_ops *ops;
+	void *private_data;
+	u8 card[32];
+	u8 bus_info[32];
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+int radio_tea5777_init(struct radio_tea5777 *tea, struct module *owner);
+void radio_tea5777_exit(struct radio_tea5777 *tea);
+
+#endif /* __RADIO_TEA5777_H */
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index f1b607099b6..e8428f573cc 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -1514,7 +1514,8 @@ static int wl1273_fm_vidioc_g_tuner(struct file *file, void *priv,
 	tuner->rangehigh = WL1273_FREQ(WL1273_BAND_OTHER_HIGH);
 
 	tuner->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_RDS |
-		V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS_BLOCK_IO;
+		V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS_BLOCK_IO |
+		V4L2_TUNER_CAP_HWSEEK_BOUNDED | V4L2_TUNER_CAP_HWSEEK_WRAP;
 
 	if (radio->stereo)
 		tuner->audmode = V4L2_TUNER_MODE_STEREO;
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 969cf494d85..9e38132afec 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -4,6 +4,7 @@
  *  Driver for radios with Silicon Labs Si470x FM Radio Receivers
  *
  *  Copyright (c) 2009 Tobias Lorenz <tobias.lorenz@gmx.net>
+ *  Copyright (c) 2012 Hans de Goede <hdegoede@redhat.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -127,14 +128,6 @@ static unsigned short space = 2;
 module_param(space, ushort, 0444);
 MODULE_PARM_DESC(space, "Spacing: 0=200kHz 1=100kHz *2=50kHz*");
 
-/* Bottom of Band (MHz) */
-/* 0: 87.5 - 108 MHz (USA, Europe)*/
-/* 1: 76   - 108 MHz (Japan wide band) */
-/* 2: 76   -  90 MHz (Japan) */
-static unsigned short band = 1;
-module_param(band, ushort, 0444);
-MODULE_PARM_DESC(band, "Band: 0=87.5..108MHz *1=76..108MHz* 2=76..90MHz");
-
 /* De-emphasis */
 /* 0: 75 us (USA) */
 /* 1: 50 us (Europe, Australia, Japan) */
@@ -152,19 +145,66 @@ static unsigned int seek_timeout = 5000;
 module_param(seek_timeout, uint, 0644);
 MODULE_PARM_DESC(seek_timeout, "Seek timeout: *5000*");
 
-
+static const struct v4l2_frequency_band bands[] = {
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			    V4L2_TUNER_CAP_RDS | V4L2_TUNER_CAP_RDS_BLOCK_IO |
+			    V4L2_TUNER_CAP_HWSEEK_BOUNDED |
+			    V4L2_TUNER_CAP_HWSEEK_WRAP,
+		.rangelow   =  87500 * 16,
+		.rangehigh  = 108000 * 16,
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			    V4L2_TUNER_CAP_RDS | V4L2_TUNER_CAP_RDS_BLOCK_IO |
+			    V4L2_TUNER_CAP_HWSEEK_BOUNDED |
+			    V4L2_TUNER_CAP_HWSEEK_WRAP,
+		.rangelow   =  76000 * 16,
+		.rangehigh  = 108000 * 16,
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 2,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			    V4L2_TUNER_CAP_RDS | V4L2_TUNER_CAP_RDS_BLOCK_IO |
+			    V4L2_TUNER_CAP_HWSEEK_BOUNDED |
+			    V4L2_TUNER_CAP_HWSEEK_WRAP,
+		.rangelow   =  76000 * 16,
+		.rangehigh  =  90000 * 16,
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+};
 
 /**************************************************************************
  * Generic Functions
  **************************************************************************/
 
 /*
+ * si470x_set_band - set the band
+ */
+static int si470x_set_band(struct si470x_device *radio, int band)
+{
+	if (radio->band == band)
+		return 0;
+
+	radio->band = band;
+	radio->registers[SYSCONFIG2] &= ~SYSCONFIG2_BAND;
+	radio->registers[SYSCONFIG2] |= radio->band << 6;
+	return si470x_set_register(radio, SYSCONFIG2);
+}
+
+/*
  * si470x_set_chan - set the channel
  */
 static int si470x_set_chan(struct si470x_device *radio, unsigned short chan)
 {
 	int retval;
-	unsigned long timeout;
 	bool timed_out = 0;
 
 	/* start tuning */
@@ -174,26 +214,12 @@ static int si470x_set_chan(struct si470x_device *radio, unsigned short chan)
 	if (retval < 0)
 		goto done;
 
-	/* currently I2C driver only uses interrupt way to tune */
-	if (radio->stci_enabled) {
-		INIT_COMPLETION(radio->completion);
-
-		/* wait till tune operation has completed */
-		retval = wait_for_completion_timeout(&radio->completion,
-				msecs_to_jiffies(tune_timeout));
-		if (!retval)
-			timed_out = true;
-	} else {
-		/* wait till tune operation has completed */
-		timeout = jiffies + msecs_to_jiffies(tune_timeout);
-		do {
-			retval = si470x_get_register(radio, STATUSRSSI);
-			if (retval < 0)
-				goto stop;
-			timed_out = time_after(jiffies, timeout);
-		} while (((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
-				&& (!timed_out));
-	}
+	/* wait till tune operation has completed */
+	INIT_COMPLETION(radio->completion);
+	retval = wait_for_completion_timeout(&radio->completion,
+			msecs_to_jiffies(tune_timeout));
+	if (!retval)
+		timed_out = true;
 
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
 		dev_warn(&radio->videodev.dev, "tune does not complete\n");
@@ -201,7 +227,6 @@ static int si470x_set_chan(struct si470x_device *radio, unsigned short chan)
 		dev_warn(&radio->videodev.dev,
 			"tune timed out after %u ms\n", tune_timeout);
 
-stop:
 	/* stop tuning */
 	radio->registers[CHANNEL] &= ~CHANNEL_TUNE;
 	retval = si470x_set_register(radio, CHANNEL);
@@ -210,48 +235,39 @@ done:
 	return retval;
 }
 
-
 /*
- * si470x_get_freq - get the frequency
+ * si470x_get_step - get channel spacing
  */
-static int si470x_get_freq(struct si470x_device *radio, unsigned int *freq)
+static unsigned int si470x_get_step(struct si470x_device *radio)
 {
-	unsigned int spacing, band_bottom;
-	unsigned short chan;
-	int retval;
-
 	/* Spacing (kHz) */
 	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_SPACE) >> 4) {
 	/* 0: 200 kHz (USA, Australia) */
 	case 0:
-		spacing = 0.200 * FREQ_MUL; break;
+		return 200 * 16;
 	/* 1: 100 kHz (Europe, Japan) */
 	case 1:
-		spacing = 0.100 * FREQ_MUL; break;
+		return 100 * 16;
 	/* 2:  50 kHz */
 	default:
-		spacing = 0.050 * FREQ_MUL; break;
+		return 50 * 16;
 	};
+}
 
-	/* Bottom of Band (MHz) */
-	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_BAND) >> 6) {
-	/* 0: 87.5 - 108 MHz (USA, Europe) */
-	case 0:
-		band_bottom = 87.5 * FREQ_MUL; break;
-	/* 1: 76   - 108 MHz (Japan wide band) */
-	default:
-		band_bottom = 76   * FREQ_MUL; break;
-	/* 2: 76   -  90 MHz (Japan) */
-	case 2:
-		band_bottom = 76   * FREQ_MUL; break;
-	};
+
+/*
+ * si470x_get_freq - get the frequency
+ */
+static int si470x_get_freq(struct si470x_device *radio, unsigned int *freq)
+{
+	int chan, retval;
 
 	/* read channel */
 	retval = si470x_get_register(radio, READCHAN);
 	chan = radio->registers[READCHAN] & READCHAN_READCHAN;
 
 	/* Frequency (MHz) = Spacing (kHz) x Channel + Bottom of Band (MHz) */
-	*freq = chan * spacing + band_bottom;
+	*freq = chan * si470x_get_step(radio) + bands[radio->band].rangelow;
 
 	return retval;
 }
@@ -262,44 +278,12 @@ static int si470x_get_freq(struct si470x_device *radio, unsigned int *freq)
  */
 int si470x_set_freq(struct si470x_device *radio, unsigned int freq)
 {
-	unsigned int spacing, band_bottom, band_top;
 	unsigned short chan;
 
-	/* Spacing (kHz) */
-	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_SPACE) >> 4) {
-	/* 0: 200 kHz (USA, Australia) */
-	case 0:
-		spacing = 0.200 * FREQ_MUL; break;
-	/* 1: 100 kHz (Europe, Japan) */
-	case 1:
-		spacing = 0.100 * FREQ_MUL; break;
-	/* 2:  50 kHz */
-	default:
-		spacing = 0.050 * FREQ_MUL; break;
-	};
-
-	/* Bottom/Top of Band (MHz) */
-	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_BAND) >> 6) {
-	/* 0: 87.5 - 108 MHz (USA, Europe) */
-	case 0:
-		band_bottom = 87.5 * FREQ_MUL;
-		band_top = 108 * FREQ_MUL;
-		break;
-	/* 1: 76   - 108 MHz (Japan wide band) */
-	default:
-		band_bottom = 76 * FREQ_MUL;
-		band_top = 108 * FREQ_MUL;
-		break;
-	/* 2: 76   -  90 MHz (Japan) */
-	case 2:
-		band_bottom = 76 * FREQ_MUL;
-		band_top = 90 * FREQ_MUL;
-		break;
-	};
-
-	freq = clamp(freq, band_bottom, band_top);
+	freq = clamp(freq, bands[radio->band].rangelow,
+			   bands[radio->band].rangehigh);
 	/* Chan = [ Freq (Mhz) - Bottom of Band (MHz) ] / Spacing (kHz) */
-	chan = (freq - band_bottom) / spacing;
+	chan = (freq - bands[radio->band].rangelow) / si470x_get_step(radio);
 
 	return si470x_set_chan(radio, chan);
 }
@@ -309,19 +293,43 @@ int si470x_set_freq(struct si470x_device *radio, unsigned int freq)
  * si470x_set_seek - set seek
  */
 static int si470x_set_seek(struct si470x_device *radio,
-		unsigned int wrap_around, unsigned int seek_upward)
+			   struct v4l2_hw_freq_seek *seek)
 {
-	int retval = 0;
-	unsigned long timeout;
+	int band, retval;
+	unsigned int freq;
 	bool timed_out = 0;
 
+	/* set band */
+	if (seek->rangelow || seek->rangehigh) {
+		for (band = 0; band < ARRAY_SIZE(bands); band++) {
+			if (bands[band].rangelow  == seek->rangelow &&
+			    bands[band].rangehigh == seek->rangehigh)
+				break;
+		}
+		if (band == ARRAY_SIZE(bands))
+			return -EINVAL; /* No matching band found */
+	} else
+		band = 1; /* If nothing is specified seek 76 - 108 Mhz */
+
+	if (radio->band != band) {
+		retval = si470x_get_freq(radio, &freq);
+		if (retval)
+			return retval;
+		retval = si470x_set_band(radio, band);
+		if (retval)
+			return retval;
+		retval = si470x_set_freq(radio, freq);
+		if (retval)
+			return retval;
+	}
+
 	/* start seeking */
 	radio->registers[POWERCFG] |= POWERCFG_SEEK;
-	if (wrap_around == 1)
+	if (seek->wrap_around)
 		radio->registers[POWERCFG] &= ~POWERCFG_SKMODE;
 	else
 		radio->registers[POWERCFG] |= POWERCFG_SKMODE;
-	if (seek_upward == 1)
+	if (seek->seek_upward)
 		radio->registers[POWERCFG] |= POWERCFG_SEEKUP;
 	else
 		radio->registers[POWERCFG] &= ~POWERCFG_SEEKUP;
@@ -329,26 +337,12 @@ static int si470x_set_seek(struct si470x_device *radio,
 	if (retval < 0)
 		return retval;
 
-	/* currently I2C driver only uses interrupt way to seek */
-	if (radio->stci_enabled) {
-		INIT_COMPLETION(radio->completion);
-
-		/* wait till seek operation has completed */
-		retval = wait_for_completion_timeout(&radio->completion,
-				msecs_to_jiffies(seek_timeout));
-		if (!retval)
-			timed_out = true;
-	} else {
-		/* wait till seek operation has completed */
-		timeout = jiffies + msecs_to_jiffies(seek_timeout);
-		do {
-			retval = si470x_get_register(radio, STATUSRSSI);
-			if (retval < 0)
-				goto stop;
-			timed_out = time_after(jiffies, timeout);
-		} while (((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
-				&& (!timed_out));
-	}
+	/* wait till tune operation has completed */
+	INIT_COMPLETION(radio->completion);
+	retval = wait_for_completion_timeout(&radio->completion,
+			msecs_to_jiffies(seek_timeout));
+	if (!retval)
+		timed_out = true;
 
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_STC) == 0)
 		dev_warn(&radio->videodev.dev, "seek does not complete\n");
@@ -356,14 +350,13 @@ static int si470x_set_seek(struct si470x_device *radio,
 		dev_warn(&radio->videodev.dev,
 			"seek failed / band limit reached\n");
 
-stop:
 	/* stop seeking */
 	radio->registers[POWERCFG] &= ~POWERCFG_SEEK;
 	retval = si470x_set_register(radio, POWERCFG);
 
 	/* try again, if timed out */
 	if (retval == 0 && timed_out)
-		return -EAGAIN;
+		return -ENODATA;
 	return retval;
 }
 
@@ -391,8 +384,8 @@ int si470x_start(struct si470x_device *radio)
 
 	/* sysconfig 2 */
 	radio->registers[SYSCONFIG2] =
-		(0x3f  << 8) |				/* SEEKTH */
-		((band  << 6) & SYSCONFIG2_BAND)  |	/* BAND */
+		(0x1f  << 8) |				/* SEEKTH */
+		((radio->band << 6) & SYSCONFIG2_BAND) |/* BAND */
 		((space << 4) & SYSCONFIG2_SPACE) |	/* SPACE */
 		15;					/* VOLUME (max) */
 	retval = si470x_set_register(radio, SYSCONFIG2);
@@ -583,39 +576,26 @@ static int si470x_vidioc_g_tuner(struct file *file, void *priv,
 		struct v4l2_tuner *tuner)
 {
 	struct si470x_device *radio = video_drvdata(file);
-	int retval;
+	int retval = 0;
 
 	if (tuner->index != 0)
 		return -EINVAL;
 
-	retval = si470x_get_register(radio, STATUSRSSI);
-	if (retval < 0)
-		return retval;
+	if (!radio->status_rssi_auto_update) {
+		retval = si470x_get_register(radio, STATUSRSSI);
+		if (retval < 0)
+			return retval;
+	}
 
 	/* driver constants */
 	strcpy(tuner->name, "FM");
 	tuner->type = V4L2_TUNER_RADIO;
 	tuner->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
-			    V4L2_TUNER_CAP_RDS | V4L2_TUNER_CAP_RDS_BLOCK_IO;
-
-	/* range limits */
-	switch ((radio->registers[SYSCONFIG2] & SYSCONFIG2_BAND) >> 6) {
-	/* 0: 87.5 - 108 MHz (USA, Europe, default) */
-	default:
-		tuner->rangelow  =  87.5 * FREQ_MUL;
-		tuner->rangehigh = 108   * FREQ_MUL;
-		break;
-	/* 1: 76   - 108 MHz (Japan wide band) */
-	case 1:
-		tuner->rangelow  =  76   * FREQ_MUL;
-		tuner->rangehigh = 108   * FREQ_MUL;
-		break;
-	/* 2: 76   -  90 MHz (Japan) */
-	case 2:
-		tuner->rangelow  =  76   * FREQ_MUL;
-		tuner->rangehigh =  90   * FREQ_MUL;
-		break;
-	};
+			    V4L2_TUNER_CAP_RDS | V4L2_TUNER_CAP_RDS_BLOCK_IO |
+			    V4L2_TUNER_CAP_HWSEEK_BOUNDED |
+			    V4L2_TUNER_CAP_HWSEEK_WRAP;
+	tuner->rangelow  =  76 * FREQ_MUL;
+	tuner->rangehigh = 108 * FREQ_MUL;
 
 	/* stereo indicator == stereo (instead of mono) */
 	if ((radio->registers[STATUSRSSI] & STATUSRSSI_ST) == 0)
@@ -698,10 +678,18 @@ static int si470x_vidioc_s_frequency(struct file *file, void *priv,
 		struct v4l2_frequency *freq)
 {
 	struct si470x_device *radio = video_drvdata(file);
+	int retval;
 
 	if (freq->tuner != 0)
 		return -EINVAL;
 
+	if (freq->frequency < bands[radio->band].rangelow ||
+	    freq->frequency > bands[radio->band].rangehigh) {
+		/* Switch to band 1 which covers everything we support */
+		retval = si470x_set_band(radio, 1);
+		if (retval)
+			return retval;
+	}
 	return si470x_set_freq(radio, freq->frequency);
 }
 
@@ -717,7 +705,21 @@ static int si470x_vidioc_s_hw_freq_seek(struct file *file, void *priv,
 	if (seek->tuner != 0)
 		return -EINVAL;
 
-	return si470x_set_seek(radio, seek->wrap_around, seek->seek_upward);
+	return si470x_set_seek(radio, seek);
+}
+
+/*
+ * si470x_vidioc_enum_freq_bands - enumerate supported bands
+ */
+static int si470x_vidioc_enum_freq_bands(struct file *file, void *priv,
+					 struct v4l2_frequency_band *band)
+{
+	if (band->tuner != 0)
+		return -EINVAL;
+	if (band->index >= ARRAY_SIZE(bands))
+		return -EINVAL;
+	*band = bands[band->index];
+	return 0;
 }
 
 const struct v4l2_ctrl_ops si470x_ctrl_ops = {
@@ -734,6 +736,7 @@ static const struct v4l2_ioctl_ops si470x_ioctl_ops = {
 	.vidioc_g_frequency	= si470x_vidioc_g_frequency,
 	.vidioc_s_frequency	= si470x_vidioc_s_frequency,
 	.vidioc_s_hw_freq_seek	= si470x_vidioc_s_hw_freq_seek,
+	.vidioc_enum_freq_bands = si470x_vidioc_enum_freq_bands,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index a80044c5874..643a6ff7c5d 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -350,7 +350,9 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 	}
 
 	radio->client = client;
+	radio->band = 1; /* Default to 76 - 108 MHz */
 	mutex_init(&radio->lock);
+	init_completion(&radio->completion);
 
 	/* video device initialization */
 	radio->videodev = si470x_viddev_template;
@@ -406,10 +408,6 @@ static int __devinit si470x_i2c_probe(struct i2c_client *client,
 	radio->rd_index = 0;
 	init_waitqueue_head(&radio->read_queue);
 
-	/* mark Seek/Tune Complete Interrupt enabled */
-	radio->stci_enabled = true;
-	init_completion(&radio->completion);
-
 	retval = request_threaded_irq(client->irq, NULL, si470x_i2c_interrupt,
 			IRQF_TRIGGER_FALLING, DRIVER_NAME, radio);
 	if (retval) {
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index f412f7ab270..146be4263ea 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -143,7 +143,7 @@ MODULE_PARM_DESC(max_rds_errors, "RDS maximum block errors: *1*");
  * Software/Hardware Versions from Scratch Page
  **************************************************************************/
 #define RADIO_SW_VERSION_NOT_BOOTLOADABLE	6
-#define RADIO_SW_VERSION			7
+#define RADIO_SW_VERSION			1
 #define RADIO_HW_VERSION			1
 
 
@@ -399,12 +399,19 @@ static void si470x_int_in_callback(struct urb *urb)
 		}
 	}
 
-	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS) == 0)
+	/* Sometimes the device returns len 0 packets */
+	if (urb->actual_length != RDS_REPORT_SIZE)
 		goto resubmit;
 
-	if (urb->actual_length > 0) {
+	radio->registers[STATUSRSSI] =
+		get_unaligned_be16(&radio->int_in_buffer[1]);
+
+	if (radio->registers[STATUSRSSI] & STATUSRSSI_STC)
+		complete(&radio->completion);
+
+	if ((radio->registers[SYSCONFIG1] & SYSCONFIG1_RDS)) {
 		/* Update RDS registers with URB data */
-		for (regnr = 0; regnr < RDS_REGISTER_NUM; regnr++)
+		for (regnr = 1; regnr < RDS_REGISTER_NUM; regnr++)
 			radio->registers[STATUSRSSI + regnr] =
 			    get_unaligned_be16(&radio->int_in_buffer[
 				regnr * RADIO_REGISTER_SIZE + 1]);
@@ -480,6 +487,7 @@ resubmit:
 			radio->int_in_running = 0;
 		}
 	}
+	radio->status_rssi_auto_update = radio->int_in_running;
 }
 
 
@@ -534,13 +542,6 @@ static int si470x_start_usb(struct si470x_device *radio)
 {
 	int retval;
 
-	/* start radio */
-	retval = si470x_start(radio);
-	if (retval < 0)
-		return retval;
-
-	v4l2_ctrl_handler_setup(&radio->hdl);
-
 	/* initialize interrupt urb */
 	usb_fill_int_urb(radio->int_in_urb, radio->usbdev,
 			usb_rcvintpipe(radio->usbdev,
@@ -560,6 +561,15 @@ static int si470x_start_usb(struct si470x_device *radio)
 				"submitting int urb failed (%d)\n", retval);
 		radio->int_in_running = 0;
 	}
+	radio->status_rssi_auto_update = radio->int_in_running;
+
+	/* start radio */
+	retval = si470x_start(radio);
+	if (retval < 0)
+		return retval;
+
+	v4l2_ctrl_handler_setup(&radio->hdl);
+
 	return retval;
 }
 
@@ -587,7 +597,9 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	}
 	radio->usbdev = interface_to_usbdev(intf);
 	radio->intf = intf;
+	radio->band = 1; /* Default to 76 - 108 MHz */
 	mutex_init(&radio->lock);
+	init_completion(&radio->completion);
 
 	iface_desc = intf->cur_altsetting;
 
@@ -698,9 +710,6 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 			"linux-media@vger.kernel.org\n");
 	}
 
-	/* set initial frequency */
-	si470x_set_freq(radio, 87.5 * FREQ_MUL); /* available in all regions */
-
 	/* set led to connect state */
 	si470x_set_led_state(radio, BLINK_GREEN_LED);
 
@@ -723,6 +732,9 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	if (retval < 0)
 		goto err_all;
 
+	/* set initial frequency */
+	si470x_set_freq(radio, 87.5 * FREQ_MUL); /* available in all regions */
+
 	/* register video device */
 	retval = video_register_device(&radio->videodev, VFL_TYPE_RADIO,
 			radio_nr);
@@ -781,11 +793,16 @@ static int si470x_usb_driver_suspend(struct usb_interface *intf,
 static int si470x_usb_driver_resume(struct usb_interface *intf)
 {
 	struct si470x_device *radio = usb_get_intfdata(intf);
+	int ret;
 
 	dev_info(&intf->dev, "resuming now...\n");
 
 	/* start radio */
-	return si470x_start_usb(radio);
+	ret = si470x_start_usb(radio);
+	if (ret == 0)
+		v4l2_ctrl_handler_setup(&radio->hdl);
+
+	return ret;
 }
 
 
diff --git a/drivers/media/radio/si470x/radio-si470x.h b/drivers/media/radio/si470x/radio-si470x.h
index 4921cab8e0f..2f089b4252d 100644
--- a/drivers/media/radio/si470x/radio-si470x.h
+++ b/drivers/media/radio/si470x/radio-si470x.h
@@ -87,7 +87,7 @@
 
 #define SYSCONFIG2		5	/* System Configuration 2 */
 #define SYSCONFIG2_SEEKTH	0xff00	/* bits 15..08: RSSI Seek Threshold */
-#define SYSCONFIG2_BAND		0x0080	/* bits 07..06: Band Select */
+#define SYSCONFIG2_BAND		0x00c0	/* bits 07..06: Band Select */
 #define SYSCONFIG2_SPACE	0x0030	/* bits 05..04: Channel Spacing */
 #define SYSCONFIG2_VOLUME	0x000f	/* bits 03..00: Volume */
 
@@ -147,6 +147,7 @@ struct si470x_device {
 	struct v4l2_device v4l2_dev;
 	struct video_device videodev;
 	struct v4l2_ctrl_handler hdl;
+	int band;
 
 	/* Silabs internal registers (0..15) */
 	unsigned short registers[RADIO_REGISTER_NUM];
@@ -160,7 +161,7 @@ struct si470x_device {
 	unsigned int wr_index;
 
 	struct completion completion;
-	bool stci_enabled;		/* Seek/Tune Complete Interrupt */
+	bool status_rssi_auto_update;	/* Does RSSI get updated automatic? */
 
 #if defined(CONFIG_USB_SI470X) || defined(CONFIG_USB_SI470X_MODULE)
 	/* reference to USB and video device */
@@ -189,7 +190,7 @@ struct si470x_device {
  * Firmware Versions
  **************************************************************************/
 
-#define RADIO_FW_VERSION	15
+#define RADIO_FW_VERSION	12
 
 
 
diff --git a/drivers/media/radio/wl128x/fmdrv_rx.c b/drivers/media/radio/wl128x/fmdrv_rx.c
index 43fb72291be..3dd9fc097c4 100644
--- a/drivers/media/radio/wl128x/fmdrv_rx.c
+++ b/drivers/media/radio/wl128x/fmdrv_rx.c
@@ -251,7 +251,7 @@ again:
 	if (!timeleft) {
 		fmerr("Timeout(%d sec),didn't get tune ended int\n",
 			   jiffies_to_msecs(FM_DRV_RX_SEEK_TIMEOUT) / 1000);
-		return -ETIMEDOUT;
+		return -ENODATA;
 	}
 
 	int_reason = fmdev->irq_info.flag & (FM_TUNE_COMPLETE | FM_BAND_LIMIT);
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
index 080b96a61f1..49a11ec1f44 100644
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.c
+++ b/drivers/media/radio/wl128x/fmdrv_v4l2.c
@@ -285,7 +285,9 @@ static int fm_v4l2_vidioc_g_tuner(struct file *file, void *priv,
 	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;
+			    V4L2_TUNER_CAP_LOW |
+			    V4L2_TUNER_CAP_HWSEEK_BOUNDED |
+			    V4L2_TUNER_CAP_HWSEEK_WRAP;
 	tuner->audmode = (stereo_mono_mode ?
 			  V4L2_TUNER_MODE_MONO : V4L2_TUNER_MODE_STEREO);
 
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index f97eeb87045..5180390be7a 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -1,21 +1,20 @@
-menuconfig RC_CORE
-	tristate "Remote Controller adapters"
+config RC_CORE
+	tristate
+	depends on MEDIA_RC_SUPPORT
 	depends on INPUT
-	default INPUT
-	---help---
-	  Enable support for Remote Controllers on Linux. This is
-	  needed in order to support several video capture adapters,
-	  standalone IR receivers/transmitters, and RF receivers.
+	default y
 
-	  Enable this option if you have a video capture board even
-	  if you don't need IR, as otherwise, you may not be able to
-	  compile the driver for your adapter.
+source "drivers/media/rc/keymaps/Kconfig"
 
-if RC_CORE
+menuconfig RC_DECODERS
+        bool "Remote controller decoders"
+	depends on RC_CORE
+	default y
 
+if RC_DECODERS
 config LIRC
-	tristate
-	default y
+	tristate "LIRC interface driver"
+	depends on RC_CORE
 
 	---help---
 	   Enable this option to build the Linux Infrared Remote
@@ -24,7 +23,16 @@ config LIRC
 	   LIRC daemon handles protocol decoding for IR reception and
 	   encoding for IR transmitting (aka "blasting").
 
-source "drivers/media/rc/keymaps/Kconfig"
+config IR_LIRC_CODEC
+	tristate "Enable IR to LIRC bridge"
+	depends on RC_CORE
+	depends on LIRC
+	default y
+
+	---help---
+	   Enable this option to pass raw IR to and from userspace via
+	   the LIRC interface.
+
 
 config IR_NEC_DECODER
 	tristate "Enable IR raw decoder for the NEC protocol"
@@ -108,16 +116,13 @@ config IR_MCE_KBD_DECODER
 	   Enable this option if you have a Microsoft Remote Keyboard for
 	   Windows Media Center Edition, which you would like to use with
 	   a raw IR receiver in your system.
+endif #RC_DECODERS
 
-config IR_LIRC_CODEC
-	tristate "Enable IR to LIRC bridge"
+menuconfig RC_DEVICES
+	bool "Remote Controller devices"
 	depends on RC_CORE
-	depends on LIRC
-	default y
 
-	---help---
-	   Enable this option to pass raw IR to and from userspace via
-	   the LIRC interface.
+if RC_DEVICES
 
 config RC_ATI_REMOTE
 	tristate "ATI / X10 based USB RF remote controls"
@@ -254,6 +259,17 @@ config IR_WINBOND_CIR
 	   To compile this driver as a module, choose M here: the module will
 	   be called winbond_cir.
 
+config IR_IGUANA
+	tristate "IguanaWorks USB IR Transceiver"
+	depends on RC_CORE
+	select USB
+	---help---
+	   Say Y here if you want to use the IgaunaWorks USB IR Transceiver.
+	   Both infrared receive and send are supported.
+
+	   To compile this driver as a module, choose M here: the module will
+	   be called iguanair.
+
 config RC_LOOPBACK
 	tristate "Remote Control Loopback Driver"
 	depends on RC_CORE
@@ -276,4 +292,4 @@ config IR_GPIO_CIR
 	   To compile this driver as a module, choose M here: the module will
 	   be called gpio-ir-recv.
 
-endif #RC_CORE
+endif #RC_DEVICES
diff --git a/drivers/media/rc/Makefile b/drivers/media/rc/Makefile
index 29f364f88a9..f871d1986c2 100644
--- a/drivers/media/rc/Makefile
+++ b/drivers/media/rc/Makefile
@@ -27,3 +27,4 @@ obj-$(CONFIG_IR_STREAMZAP) += streamzap.o
 obj-$(CONFIG_IR_WINBOND_CIR) += winbond-cir.o
 obj-$(CONFIG_RC_LOOPBACK) += rc-loopback.o
 obj-$(CONFIG_IR_GPIO_CIR) += gpio-ir-recv.o
+obj-$(CONFIG_IR_IGUANA) += iguanair.o
diff --git a/drivers/media/rc/ati_remote.c b/drivers/media/rc/ati_remote.c
index 7be377fc1be..8fa72e2dacb 100644
--- a/drivers/media/rc/ati_remote.c
+++ b/drivers/media/rc/ati_remote.c
@@ -147,7 +147,8 @@ static bool mouse = true;
 module_param(mouse, bool, 0444);
 MODULE_PARM_DESC(mouse, "Enable mouse device, default = yes");
 
-#define dbginfo(dev, format, arg...) do { if (debug) dev_info(dev , format , ## arg); } while (0)
+#define dbginfo(dev, format, arg...) \
+	do { if (debug) dev_info(dev , format , ## arg); } while (0)
 #undef err
 #define err(format, arg...) printk(KERN_ERR format , ## arg)
 
@@ -191,17 +192,41 @@ static const char *get_medion_keymap(struct usb_interface *interface)
 	return RC_MAP_MEDION_X10;
 }
 
-static const struct ati_receiver_type type_ati		= { .default_keymap = RC_MAP_ATI_X10 };
-static const struct ati_receiver_type type_medion	= { .get_default_keymap = get_medion_keymap };
-static const struct ati_receiver_type type_firefly	= { .default_keymap = RC_MAP_SNAPSTREAM_FIREFLY };
+static const struct ati_receiver_type type_ati		= {
+	.default_keymap = RC_MAP_ATI_X10
+};
+static const struct ati_receiver_type type_medion	= {
+	.get_default_keymap = get_medion_keymap
+};
+static const struct ati_receiver_type type_firefly	= {
+	.default_keymap = RC_MAP_SNAPSTREAM_FIREFLY
+};
 
 static struct usb_device_id ati_remote_table[] = {
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA2_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, ATI_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, NVIDIA_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_ati },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, MEDION_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_medion },
-	{ USB_DEVICE(ATI_REMOTE_VENDOR_ID, FIREFLY_REMOTE_PRODUCT_ID),	.driver_info = (unsigned long)&type_firefly },
+	{
+		USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA_REMOTE_PRODUCT_ID),
+		.driver_info = (unsigned long)&type_ati
+	},
+	{
+		USB_DEVICE(ATI_REMOTE_VENDOR_ID, LOLA2_REMOTE_PRODUCT_ID),
+		.driver_info = (unsigned long)&type_ati
+	},
+	{
+		USB_DEVICE(ATI_REMOTE_VENDOR_ID, ATI_REMOTE_PRODUCT_ID),
+		.driver_info = (unsigned long)&type_ati
+	},
+	{
+		USB_DEVICE(ATI_REMOTE_VENDOR_ID, NVIDIA_REMOTE_PRODUCT_ID),
+		.driver_info = (unsigned long)&type_ati
+	},
+	{
+		USB_DEVICE(ATI_REMOTE_VENDOR_ID, MEDION_REMOTE_PRODUCT_ID),
+		.driver_info = (unsigned long)&type_medion
+	},
+	{
+		USB_DEVICE(ATI_REMOTE_VENDOR_ID, FIREFLY_REMOTE_PRODUCT_ID),
+		.driver_info = (unsigned long)&type_firefly
+	},
 	{}	/* Terminating entry */
 };
 
@@ -296,25 +321,8 @@ static const struct {
 	{KIND_END, 0x00, EV_MAX + 1, 0, 0}
 };
 
-/* Local function prototypes */
-static int ati_remote_sendpacket	(struct ati_remote *ati_remote, u16 cmd, unsigned char *data);
-static void ati_remote_irq_out		(struct urb *urb);
-static void ati_remote_irq_in		(struct urb *urb);
-static void ati_remote_input_report	(struct urb *urb);
-static int ati_remote_initialize	(struct ati_remote *ati_remote);
-static int ati_remote_probe		(struct usb_interface *interface, const struct usb_device_id *id);
-static void ati_remote_disconnect	(struct usb_interface *interface);
-
-/* usb specific object to register with the usb subsystem */
-static struct usb_driver ati_remote_driver = {
-	.name         = "ati_remote",
-	.probe        = ati_remote_probe,
-	.disconnect   = ati_remote_disconnect,
-	.id_table     = ati_remote_table,
-};
-
 /*
- *	ati_remote_dump_input
+ * ati_remote_dump_input
  */
 static void ati_remote_dump(struct device *dev, unsigned char *data,
 			    unsigned int len)
@@ -326,12 +334,14 @@ static void ati_remote_dump(struct device *dev, unsigned char *data,
 		dev_warn(dev, "Weird key %02x %02x %02x %02x\n",
 		     data[0], data[1], data[2], data[3]);
 	else
-		dev_warn(dev, "Weird data, len=%d %02x %02x %02x %02x %02x %02x ...\n",
-		     len, data[0], data[1], data[2], data[3], data[4], data[5]);
+		dev_warn(dev,
+			"Weird data, len=%d %02x %02x %02x %02x %02x %02x ...\n",
+			len, data[0], data[1], data[2], data[3], data[4],
+			data[5]);
 }
 
 /*
- *	ati_remote_open
+ * ati_remote_open
  */
 static int ati_remote_open(struct ati_remote *ati_remote)
 {
@@ -355,7 +365,7 @@ out:	mutex_unlock(&ati_remote->open_mutex);
 }
 
 /*
- *	ati_remote_close
+ * ati_remote_close
  */
 static void ati_remote_close(struct ati_remote *ati_remote)
 {
@@ -390,7 +400,7 @@ static void ati_remote_rc_close(struct rc_dev *rdev)
 }
 
 /*
- *		ati_remote_irq_out
+ * ati_remote_irq_out
  */
 static void ati_remote_irq_out(struct urb *urb)
 {
@@ -408,11 +418,12 @@ static void ati_remote_irq_out(struct urb *urb)
 }
 
 /*
- *	ati_remote_sendpacket
+ * ati_remote_sendpacket
  *
- *	Used to send device initialization strings
+ * Used to send device initialization strings
  */
-static int ati_remote_sendpacket(struct ati_remote *ati_remote, u16 cmd, unsigned char *data)
+static int ati_remote_sendpacket(struct ati_remote *ati_remote, u16 cmd,
+	unsigned char *data)
 {
 	int retval = 0;
 
@@ -441,7 +452,7 @@ static int ati_remote_sendpacket(struct ati_remote *ati_remote, u16 cmd, unsigne
 }
 
 /*
- *	ati_remote_compute_accel
+ * ati_remote_compute_accel
  *
  * Implements acceleration curve for directional control pad
  * If elapsed time since last event is > 1/4 second, user "stopped",
@@ -478,7 +489,7 @@ static int ati_remote_compute_accel(struct ati_remote *ati_remote)
 }
 
 /*
- *	ati_remote_report_input
+ * ati_remote_report_input
  */
 static void ati_remote_input_report(struct urb *urb)
 {
@@ -518,7 +529,8 @@ static void ati_remote_input_report(struct urb *urb)
 	remote_num = (data[3] >> 4) & 0x0f;
 	if (channel_mask & (1 << (remote_num + 1))) {
 		dbginfo(&ati_remote->interface->dev,
-			"Masked input from channel 0x%02x: data %02x,%02x, mask= 0x%02lx\n",
+			"Masked input from channel 0x%02x: data %02x,%02x, "
+			"mask= 0x%02lx\n",
 			remote_num, data[1], data[2], channel_mask);
 		return;
 	}
@@ -546,7 +558,9 @@ static void ati_remote_input_report(struct urb *urb)
 		if (wheel_keycode == KEY_RESERVED) {
 			/* scrollwheel was not mapped, assume mouse */
 
-			/* Look up event code index in the mouse translation table. */
+			/* Look up event code index in the mouse translation
+			 * table.
+			 */
 			for (i = 0; ati_remote_tbl[i].kind != KIND_END; i++) {
 				if (scancode == ati_remote_tbl[i].data) {
 					index = i;
@@ -630,9 +644,9 @@ static void ati_remote_input_report(struct urb *urb)
 	} else {
 
 		/*
-		 * Other event kinds are from the directional control pad, and have an
-		 * acceleration factor applied to them.  Without this acceleration, the
-		 * control pad is mostly unusable.
+		 * Other event kinds are from the directional control pad, and
+		 * have an acceleration factor applied to them.  Without this
+		 * acceleration, the control pad is mostly unusable.
 		 */
 		acc = ati_remote_compute_accel(ati_remote);
 
@@ -659,7 +673,8 @@ static void ati_remote_input_report(struct urb *urb)
 			input_report_rel(dev, REL_Y, acc);
 			break;
 		default:
-			dev_dbg(&ati_remote->interface->dev, "ati_remote kind=%d\n",
+			dev_dbg(&ati_remote->interface->dev,
+				"ati_remote kind=%d\n",
 				ati_remote_tbl[index].kind);
 		}
 		input_sync(dev);
@@ -670,7 +685,7 @@ static void ati_remote_input_report(struct urb *urb)
 }
 
 /*
- *	ati_remote_irq_in
+ * ati_remote_irq_in
  */
 static void ati_remote_irq_in(struct urb *urb)
 {
@@ -684,22 +699,25 @@ static void ati_remote_irq_in(struct urb *urb)
 	case -ECONNRESET:	/* unlink */
 	case -ENOENT:
 	case -ESHUTDOWN:
-		dev_dbg(&ati_remote->interface->dev, "%s: urb error status, unlink? \n",
+		dev_dbg(&ati_remote->interface->dev,
+			"%s: urb error status, unlink?\n",
 			__func__);
 		return;
 	default:		/* error */
-		dev_dbg(&ati_remote->interface->dev, "%s: Nonzero urb status %d\n",
+		dev_dbg(&ati_remote->interface->dev,
+			"%s: Nonzero urb status %d\n",
 			__func__, urb->status);
 	}
 
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
-		dev_err(&ati_remote->interface->dev, "%s: usb_submit_urb()=%d\n",
+		dev_err(&ati_remote->interface->dev,
+			"%s: usb_submit_urb()=%d\n",
 			__func__, retval);
 }
 
 /*
- *	ati_remote_alloc_buffers
+ * ati_remote_alloc_buffers
  */
 static int ati_remote_alloc_buffers(struct usb_device *udev,
 				    struct ati_remote *ati_remote)
@@ -726,7 +744,7 @@ static int ati_remote_alloc_buffers(struct usb_device *udev,
 }
 
 /*
- *	ati_remote_free_buffers
+ * ati_remote_free_buffers
  */
 static void ati_remote_free_buffers(struct ati_remote *ati_remote)
 {
@@ -825,9 +843,10 @@ static int ati_remote_initialize(struct ati_remote *ati_remote)
 }
 
 /*
- *	ati_remote_probe
+ * ati_remote_probe
  */
-static int ati_remote_probe(struct usb_interface *interface, const struct usb_device_id *id)
+static int ati_remote_probe(struct usb_interface *interface,
+	const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
 	struct usb_host_interface *iface_host = interface->cur_altsetting;
@@ -949,7 +968,7 @@ static int ati_remote_probe(struct usb_interface *interface, const struct usb_de
 }
 
 /*
- *	ati_remote_disconnect
+ * ati_remote_disconnect
  */
 static void ati_remote_disconnect(struct usb_interface *interface)
 {
@@ -971,6 +990,14 @@ static void ati_remote_disconnect(struct usb_interface *interface)
 	kfree(ati_remote);
 }
 
+/* usb specific object to register with the usb subsystem */
+static struct usb_driver ati_remote_driver = {
+	.name         = "ati_remote",
+	.probe        = ati_remote_probe,
+	.disconnect   = ati_remote_disconnect,
+	.id_table     = ati_remote_table,
+};
+
 module_usb_driver(ati_remote_driver);
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index bef5296173c..647dd951b0e 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -1018,6 +1018,8 @@ static int ene_probe(struct pnp_dev *pnp_dev, const struct pnp_device_id *id)
 
 	spin_lock_init(&dev->hw_lock);
 
+	dev->hw_io = pnp_port_start(pnp_dev, 0);
+
 	pnp_set_drvdata(pnp_dev, dev);
 	dev->pnp_dev = pnp_dev;
 
@@ -1072,7 +1074,6 @@ static int ene_probe(struct pnp_dev *pnp_dev, const struct pnp_device_id *id)
 
 	/* claim the resources */
 	error = -EBUSY;
-	dev->hw_io = pnp_port_start(pnp_dev, 0);
 	if (!request_region(dev->hw_io, ENE_IO_SIZE, ENE_DRIVER_NAME)) {
 		dev->hw_io = -1;
 		dev->irq = -1;
diff --git a/drivers/media/rc/fintek-cir.c b/drivers/media/rc/fintek-cir.c
index 6aabf7ae3a3..ab30c64f812 100644
--- a/drivers/media/rc/fintek-cir.c
+++ b/drivers/media/rc/fintek-cir.c
@@ -23,6 +23,8 @@
  * USA
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/pnp.h>
@@ -110,30 +112,32 @@ static u8 fintek_cir_reg_read(struct fintek_dev *fintek, u8 offset)
 	return val;
 }
 
-#define pr_reg(text, ...) \
-	printk(KERN_INFO KBUILD_MODNAME ": " text, ## __VA_ARGS__)
-
 /* dump current cir register contents */
 static void cir_dump_regs(struct fintek_dev *fintek)
 {
 	fintek_config_mode_enable(fintek);
 	fintek_select_logical_dev(fintek, fintek->logical_dev_cir);
 
-	pr_reg("%s: Dump CIR logical device registers:\n", FINTEK_DRIVER_NAME);
-	pr_reg(" * CR CIR BASE ADDR: 0x%x\n",
-	       (fintek_cr_read(fintek, CIR_CR_BASE_ADDR_HI) << 8) |
+	pr_info("%s: Dump CIR logical device registers:\n", FINTEK_DRIVER_NAME);
+	pr_info(" * CR CIR BASE ADDR: 0x%x\n",
+		(fintek_cr_read(fintek, CIR_CR_BASE_ADDR_HI) << 8) |
 		fintek_cr_read(fintek, CIR_CR_BASE_ADDR_LO));
-	pr_reg(" * CR CIR IRQ NUM:   0x%x\n",
-	       fintek_cr_read(fintek, CIR_CR_IRQ_SEL));
+	pr_info(" * CR CIR IRQ NUM:   0x%x\n",
+		fintek_cr_read(fintek, CIR_CR_IRQ_SEL));
 
 	fintek_config_mode_disable(fintek);
 
-	pr_reg("%s: Dump CIR registers:\n", FINTEK_DRIVER_NAME);
-	pr_reg(" * STATUS:     0x%x\n", fintek_cir_reg_read(fintek, CIR_STATUS));
-	pr_reg(" * CONTROL:    0x%x\n", fintek_cir_reg_read(fintek, CIR_CONTROL));
-	pr_reg(" * RX_DATA:    0x%x\n", fintek_cir_reg_read(fintek, CIR_RX_DATA));
-	pr_reg(" * TX_CONTROL: 0x%x\n", fintek_cir_reg_read(fintek, CIR_TX_CONTROL));
-	pr_reg(" * TX_DATA:    0x%x\n", fintek_cir_reg_read(fintek, CIR_TX_DATA));
+	pr_info("%s: Dump CIR registers:\n", FINTEK_DRIVER_NAME);
+	pr_info(" * STATUS:     0x%x\n",
+		fintek_cir_reg_read(fintek, CIR_STATUS));
+	pr_info(" * CONTROL:    0x%x\n",
+		fintek_cir_reg_read(fintek, CIR_CONTROL));
+	pr_info(" * RX_DATA:    0x%x\n",
+		fintek_cir_reg_read(fintek, CIR_RX_DATA));
+	pr_info(" * TX_CONTROL: 0x%x\n",
+		fintek_cir_reg_read(fintek, CIR_TX_CONTROL));
+	pr_info(" * TX_DATA:    0x%x\n",
+		fintek_cir_reg_read(fintek, CIR_TX_DATA));
 }
 
 /* detect hardware features */
diff --git a/drivers/media/rc/gpio-ir-recv.c b/drivers/media/rc/gpio-ir-recv.c
index 0d875450c5c..04cb272db16 100644
--- a/drivers/media/rc/gpio-ir-recv.c
+++ b/drivers/media/rc/gpio-ir-recv.c
@@ -82,12 +82,21 @@ static int __devinit gpio_ir_recv_probe(struct platform_device *pdev)
 		goto err_allocate_device;
 	}
 
+	rcdev->priv = gpio_dev;
 	rcdev->driver_type = RC_DRIVER_IR_RAW;
-	rcdev->allowed_protos = RC_TYPE_ALL;
 	rcdev->input_name = GPIO_IR_DEVICE_NAME;
+	rcdev->input_phys = GPIO_IR_DEVICE_NAME "/input0";
 	rcdev->input_id.bustype = BUS_HOST;
+	rcdev->input_id.vendor = 0x0001;
+	rcdev->input_id.product = 0x0001;
+	rcdev->input_id.version = 0x0100;
+	rcdev->dev.parent = &pdev->dev;
 	rcdev->driver_name = GPIO_IR_DRIVER_NAME;
-	rcdev->map_name = RC_MAP_EMPTY;
+	if (pdata->allowed_protos)
+		rcdev->allowed_protos = pdata->allowed_protos;
+	else
+		rcdev->allowed_protos = RC_TYPE_ALL;
+	rcdev->map_name = pdata->map_name ?: RC_MAP_EMPTY;
 
 	gpio_dev->rcdev = rcdev;
 	gpio_dev->gpio_nr = pdata->gpio_nr;
@@ -188,18 +197,7 @@ static struct platform_driver gpio_ir_recv_driver = {
 #endif
 	},
 };
-
-static int __init gpio_ir_recv_init(void)
-{
-	return platform_driver_register(&gpio_ir_recv_driver);
-}
-module_init(gpio_ir_recv_init);
-
-static void __exit gpio_ir_recv_exit(void)
-{
-	platform_driver_unregister(&gpio_ir_recv_driver);
-}
-module_exit(gpio_ir_recv_exit);
+module_platform_driver(gpio_ir_recv_driver);
 
 MODULE_DESCRIPTION("GPIO IR Receiver driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/rc/iguanair.c b/drivers/media/rc/iguanair.c
new file mode 100644
index 00000000000..5e2eaf8ba73
--- /dev/null
+++ b/drivers/media/rc/iguanair.c
@@ -0,0 +1,639 @@
+/*
+ * IguanaWorks USB IR Transceiver support
+ *
+ * Copyright (C) 2012 Sean Young <sean@mess.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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/device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/usb/input.h>
+#include <linux/slab.h>
+#include <linux/completion.h>
+#include <media/rc-core.h>
+
+#define DRIVER_NAME "iguanair"
+
+struct iguanair {
+	struct rc_dev *rc;
+
+	struct device *dev;
+	struct usb_device *udev;
+
+	int pipe_in, pipe_out;
+	uint8_t bufsize;
+	uint8_t version[2];
+
+	struct mutex lock;
+
+	/* receiver support */
+	bool receiver_on;
+	dma_addr_t dma_in;
+	uint8_t *buf_in;
+	struct urb *urb_in;
+	struct completion completion;
+
+	/* transmit support */
+	bool tx_overflow;
+	uint32_t carrier;
+	uint8_t cycle_overhead;
+	uint8_t channels;
+	uint8_t busy4;
+	uint8_t busy7;
+
+	char name[64];
+	char phys[64];
+};
+
+#define CMD_GET_VERSION		0x01
+#define CMD_GET_BUFSIZE		0x11
+#define CMD_GET_FEATURES	0x10
+#define CMD_SEND		0x15
+#define CMD_EXECUTE		0x1f
+#define CMD_RX_OVERFLOW		0x31
+#define CMD_TX_OVERFLOW		0x32
+#define CMD_RECEIVER_ON		0x12
+#define CMD_RECEIVER_OFF	0x14
+
+#define DIR_IN			0xdc
+#define DIR_OUT			0xcd
+
+#define MAX_PACKET_SIZE		8u
+#define TIMEOUT			1000
+
+struct packet {
+	uint16_t start;
+	uint8_t direction;
+	uint8_t cmd;
+};
+
+struct response_packet {
+	struct packet header;
+	uint8_t data[4];
+};
+
+struct send_packet {
+	struct packet header;
+	uint8_t length;
+	uint8_t channels;
+	uint8_t busy7;
+	uint8_t busy4;
+	uint8_t payload[0];
+};
+
+static void process_ir_data(struct iguanair *ir, unsigned len)
+{
+	if (len >= 4 && ir->buf_in[0] == 0 && ir->buf_in[1] == 0) {
+		switch (ir->buf_in[3]) {
+		case CMD_TX_OVERFLOW:
+			ir->tx_overflow = true;
+		case CMD_RECEIVER_OFF:
+		case CMD_RECEIVER_ON:
+		case CMD_SEND:
+			complete(&ir->completion);
+			break;
+		case CMD_RX_OVERFLOW:
+			dev_warn(ir->dev, "receive overflow\n");
+			break;
+		default:
+			dev_warn(ir->dev, "control code %02x received\n",
+							ir->buf_in[3]);
+			break;
+		}
+	} else if (len >= 7) {
+		DEFINE_IR_RAW_EVENT(rawir);
+		unsigned i;
+
+		init_ir_raw_event(&rawir);
+
+		for (i = 0; i < 7; i++) {
+			if (ir->buf_in[i] == 0x80) {
+				rawir.pulse = false;
+				rawir.duration = US_TO_NS(21845);
+			} else {
+				rawir.pulse = (ir->buf_in[i] & 0x80) == 0;
+				rawir.duration = ((ir->buf_in[i] & 0x7f) + 1) *
+									 21330;
+			}
+
+			ir_raw_event_store_with_filter(ir->rc, &rawir);
+		}
+
+		ir_raw_event_handle(ir->rc);
+	}
+}
+
+static void iguanair_rx(struct urb *urb)
+{
+	struct iguanair *ir;
+
+	if (!urb)
+		return;
+
+	ir = urb->context;
+	if (!ir) {
+		usb_unlink_urb(urb);
+		return;
+	}
+
+	switch (urb->status) {
+	case 0:
+		process_ir_data(ir, urb->actual_length);
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		usb_unlink_urb(urb);
+		return;
+	case -EPIPE:
+	default:
+		dev_dbg(ir->dev, "Error: urb status = %d\n", urb->status);
+		break;
+	}
+
+	usb_submit_urb(urb, GFP_ATOMIC);
+}
+
+static int iguanair_send(struct iguanair *ir, void *data, unsigned size,
+			struct response_packet *response, unsigned *res_len)
+{
+	unsigned offset, len;
+	int rc, transferred;
+
+	for (offset = 0; offset < size; offset += MAX_PACKET_SIZE) {
+		len = min(size - offset, MAX_PACKET_SIZE);
+
+		if (ir->tx_overflow)
+			return -EOVERFLOW;
+
+		rc = usb_interrupt_msg(ir->udev, ir->pipe_out, data + offset,
+						len, &transferred, TIMEOUT);
+		if (rc)
+			return rc;
+
+		if (transferred != len)
+			return -EIO;
+	}
+
+	if (response) {
+		rc = usb_interrupt_msg(ir->udev, ir->pipe_in, response,
+					sizeof(*response), res_len, TIMEOUT);
+	}
+
+	return rc;
+}
+
+static int iguanair_get_features(struct iguanair *ir)
+{
+	struct packet packet;
+	struct response_packet response;
+	int rc, len;
+
+	packet.start = 0;
+	packet.direction = DIR_OUT;
+	packet.cmd = CMD_GET_VERSION;
+
+	rc = iguanair_send(ir, &packet, sizeof(packet), &response, &len);
+	if (rc) {
+		dev_info(ir->dev, "failed to get version\n");
+		goto out;
+	}
+
+	if (len != 6) {
+		dev_info(ir->dev, "failed to get version\n");
+		rc = -EIO;
+		goto out;
+	}
+
+	ir->version[0] = response.data[0];
+	ir->version[1] = response.data[1];
+	ir->bufsize = 150;
+	ir->cycle_overhead = 65;
+
+	packet.cmd = CMD_GET_BUFSIZE;
+
+	rc = iguanair_send(ir, &packet, sizeof(packet), &response, &len);
+	if (rc) {
+		dev_info(ir->dev, "failed to get buffer size\n");
+		goto out;
+	}
+
+	if (len != 5) {
+		dev_info(ir->dev, "failed to get buffer size\n");
+		rc = -EIO;
+		goto out;
+	}
+
+	ir->bufsize = response.data[0];
+
+	if (ir->version[0] == 0 || ir->version[1] == 0)
+		goto out;
+
+	packet.cmd = CMD_GET_FEATURES;
+
+	rc = iguanair_send(ir, &packet, sizeof(packet), &response, &len);
+	if (rc) {
+		dev_info(ir->dev, "failed to get features\n");
+		goto out;
+	}
+
+	if (len < 5) {
+		dev_info(ir->dev, "failed to get features\n");
+		rc = -EIO;
+		goto out;
+	}
+
+	if (len > 5 && ir->version[0] >= 4)
+		ir->cycle_overhead = response.data[1];
+
+out:
+	return rc;
+}
+
+static int iguanair_receiver(struct iguanair *ir, bool enable)
+{
+	struct packet packet = { 0, DIR_OUT, enable ?
+				CMD_RECEIVER_ON : CMD_RECEIVER_OFF };
+	int rc;
+
+	INIT_COMPLETION(ir->completion);
+
+	rc = iguanair_send(ir, &packet, sizeof(packet), NULL, NULL);
+	if (rc)
+		return rc;
+
+	wait_for_completion_timeout(&ir->completion, TIMEOUT);
+
+	return 0;
+}
+
+/*
+ * The iguana ir creates the carrier by busy spinning after each pulse or
+ * space. This is counted in CPU cycles, with the CPU running at 24MHz. It is
+ * broken down into 7-cycles and 4-cyles delays, with a preference for
+ * 4-cycle delays.
+ */
+static int iguanair_set_tx_carrier(struct rc_dev *dev, uint32_t carrier)
+{
+	struct iguanair *ir = dev->priv;
+
+	if (carrier < 25000 || carrier > 150000)
+		return -EINVAL;
+
+	mutex_lock(&ir->lock);
+
+	if (carrier != ir->carrier) {
+		uint32_t cycles, fours, sevens;
+
+		ir->carrier = carrier;
+
+		cycles = DIV_ROUND_CLOSEST(24000000, carrier * 2) -
+							ir->cycle_overhead;
+
+		/*  make up the the remainer of 4-cycle blocks */
+		switch (cycles & 3) {
+		case 0:
+			sevens = 0;
+			break;
+		case 1:
+			sevens = 3;
+			break;
+		case 2:
+			sevens = 2;
+			break;
+		case 3:
+			sevens = 1;
+			break;
+		}
+
+		fours = (cycles - sevens * 7) / 4;
+
+		/* magic happens here */
+		ir->busy7 = (4 - sevens) * 2;
+		ir->busy4 = 110 - fours;
+	}
+
+	mutex_unlock(&ir->lock);
+
+	return carrier;
+}
+
+static int iguanair_set_tx_mask(struct rc_dev *dev, uint32_t mask)
+{
+	struct iguanair *ir = dev->priv;
+
+	if (mask > 15)
+		return 4;
+
+	mutex_lock(&ir->lock);
+	ir->channels = mask;
+	mutex_unlock(&ir->lock);
+
+	return 0;
+}
+
+static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
+{
+	struct iguanair *ir = dev->priv;
+	uint8_t space, *payload;
+	unsigned i, size, rc;
+	struct send_packet *packet;
+
+	mutex_lock(&ir->lock);
+
+	/* convert from us to carrier periods */
+	for (i = size = 0; i < count; i++) {
+		txbuf[i] = DIV_ROUND_CLOSEST(txbuf[i] * ir->carrier, 1000000);
+		size += (txbuf[i] + 126) / 127;
+	}
+
+	packet = kmalloc(sizeof(*packet) + size, GFP_KERNEL);
+	if (!packet) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	if (size > ir->bufsize) {
+		rc = -E2BIG;
+		goto out;
+	}
+
+	packet->header.start = 0;
+	packet->header.direction = DIR_OUT;
+	packet->header.cmd = CMD_SEND;
+	packet->length = size;
+	packet->channels = ir->channels << 4;
+	packet->busy7 = ir->busy7;
+	packet->busy4 = ir->busy4;
+
+	space = 0;
+	payload = packet->payload;
+
+	for (i = 0; i < count; i++) {
+		unsigned periods = txbuf[i];
+
+		while (periods > 127) {
+			*payload++ = 127 | space;
+			periods -= 127;
+		}
+
+		*payload++ = periods | space;
+		space ^= 0x80;
+	}
+
+	if (ir->receiver_on) {
+		rc = iguanair_receiver(ir, false);
+		if (rc) {
+			dev_warn(ir->dev, "disable receiver before transmit failed\n");
+			goto out;
+		}
+	}
+
+	ir->tx_overflow = false;
+
+	INIT_COMPLETION(ir->completion);
+
+	rc = iguanair_send(ir, packet, size + 8, NULL, NULL);
+
+	if (rc == 0) {
+		wait_for_completion_timeout(&ir->completion, TIMEOUT);
+		if (ir->tx_overflow)
+			rc = -EOVERFLOW;
+	}
+
+	ir->tx_overflow = false;
+
+	if (ir->receiver_on) {
+		if (iguanair_receiver(ir, true))
+			dev_warn(ir->dev, "re-enable receiver after transmit failed\n");
+	}
+
+out:
+	mutex_unlock(&ir->lock);
+	kfree(packet);
+
+	return rc;
+}
+
+static int iguanair_open(struct rc_dev *rdev)
+{
+	struct iguanair *ir = rdev->priv;
+	int rc;
+
+	mutex_lock(&ir->lock);
+
+	usb_submit_urb(ir->urb_in, GFP_KERNEL);
+
+	BUG_ON(ir->receiver_on);
+
+	rc = iguanair_receiver(ir, true);
+	if (rc == 0)
+		ir->receiver_on = true;
+
+	mutex_unlock(&ir->lock);
+
+	return rc;
+}
+
+static void iguanair_close(struct rc_dev *rdev)
+{
+	struct iguanair *ir = rdev->priv;
+	int rc;
+
+	mutex_lock(&ir->lock);
+
+	rc = iguanair_receiver(ir, false);
+	ir->receiver_on = false;
+	if (rc)
+		dev_warn(ir->dev, "failed to disable receiver: %d\n", rc);
+
+	usb_kill_urb(ir->urb_in);
+
+	mutex_unlock(&ir->lock);
+}
+
+static int __devinit iguanair_probe(struct usb_interface *intf,
+						const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct iguanair *ir;
+	struct rc_dev *rc;
+	int ret;
+	struct usb_host_interface *idesc;
+
+	ir = kzalloc(sizeof(*ir), GFP_KERNEL);
+	rc = rc_allocate_device();
+	if (!ir || !rc) {
+		ret = ENOMEM;
+		goto out;
+	}
+
+	ir->buf_in = usb_alloc_coherent(udev, MAX_PACKET_SIZE, GFP_ATOMIC,
+								&ir->dma_in);
+	ir->urb_in = usb_alloc_urb(0, GFP_KERNEL);
+
+	if (!ir->buf_in || !ir->urb_in) {
+		ret = ENOMEM;
+		goto out;
+	}
+
+	idesc = intf->altsetting;
+
+	if (idesc->desc.bNumEndpoints < 2) {
+		ret = -ENODEV;
+		goto out;
+	}
+
+	ir->rc = rc;
+	ir->dev = &intf->dev;
+	ir->udev = udev;
+	ir->pipe_in = usb_rcvintpipe(udev,
+				idesc->endpoint[0].desc.bEndpointAddress);
+	ir->pipe_out = usb_sndintpipe(udev,
+				idesc->endpoint[1].desc.bEndpointAddress);
+	mutex_init(&ir->lock);
+	init_completion(&ir->completion);
+
+	ret = iguanair_get_features(ir);
+	if (ret) {
+		dev_warn(&intf->dev, "failed to get device features");
+		goto out;
+	}
+
+	usb_fill_int_urb(ir->urb_in, ir->udev, ir->pipe_in, ir->buf_in,
+		MAX_PACKET_SIZE, iguanair_rx, ir,
+		idesc->endpoint[0].desc.bInterval);
+	ir->urb_in->transfer_dma = ir->dma_in;
+	ir->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+	snprintf(ir->name, sizeof(ir->name),
+		"IguanaWorks USB IR Transceiver version %d.%d",
+		ir->version[0], ir->version[1]);
+
+	usb_make_path(ir->udev, ir->phys, sizeof(ir->phys));
+
+	rc->input_name = ir->name;
+	rc->input_phys = ir->phys;
+	usb_to_input_id(ir->udev, &rc->input_id);
+	rc->dev.parent = &intf->dev;
+	rc->driver_type = RC_DRIVER_IR_RAW;
+	rc->allowed_protos = RC_TYPE_ALL;
+	rc->priv = ir;
+	rc->open = iguanair_open;
+	rc->close = iguanair_close;
+	rc->s_tx_mask = iguanair_set_tx_mask;
+	rc->s_tx_carrier = iguanair_set_tx_carrier;
+	rc->tx_ir = iguanair_tx;
+	rc->driver_name = DRIVER_NAME;
+	rc->map_name = RC_MAP_EMPTY;
+
+	iguanair_set_tx_carrier(rc, 38000);
+
+	ret = rc_register_device(rc);
+	if (ret < 0) {
+		dev_err(&intf->dev, "failed to register rc device %d", ret);
+		goto out;
+	}
+
+	usb_set_intfdata(intf, ir);
+
+	dev_info(&intf->dev, "Registered %s", ir->name);
+
+	return 0;
+out:
+	if (ir) {
+		usb_free_urb(ir->urb_in);
+		usb_free_coherent(udev, MAX_PACKET_SIZE, ir->buf_in,
+								ir->dma_in);
+	}
+	rc_free_device(rc);
+	kfree(ir);
+	return ret;
+}
+
+static void __devexit iguanair_disconnect(struct usb_interface *intf)
+{
+	struct iguanair *ir = usb_get_intfdata(intf);
+
+	usb_set_intfdata(intf, NULL);
+
+	usb_kill_urb(ir->urb_in);
+	usb_free_urb(ir->urb_in);
+	usb_free_coherent(ir->udev, MAX_PACKET_SIZE, ir->buf_in, ir->dma_in);
+	rc_unregister_device(ir->rc);
+	kfree(ir);
+}
+
+static int iguanair_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct iguanair *ir = usb_get_intfdata(intf);
+	int rc = 0;
+
+	mutex_lock(&ir->lock);
+
+	if (ir->receiver_on) {
+		rc = iguanair_receiver(ir, false);
+		if (rc)
+			dev_warn(ir->dev, "failed to disable receiver for suspend\n");
+	}
+
+	mutex_unlock(&ir->lock);
+
+	return rc;
+}
+
+static int iguanair_resume(struct usb_interface *intf)
+{
+	struct iguanair *ir = usb_get_intfdata(intf);
+	int rc = 0;
+
+	mutex_lock(&ir->lock);
+
+	if (ir->receiver_on) {
+		rc = iguanair_receiver(ir, true);
+		if (rc)
+			dev_warn(ir->dev, "failed to enable receiver after resume\n");
+	}
+
+	mutex_unlock(&ir->lock);
+
+	return rc;
+}
+
+static const struct usb_device_id iguanair_table[] = {
+	{ USB_DEVICE(0x1781, 0x0938) },
+	{ }
+};
+
+static struct usb_driver iguanair_driver = {
+	.name =	DRIVER_NAME,
+	.probe = iguanair_probe,
+	.disconnect = __devexit_p(iguanair_disconnect),
+	.suspend = iguanair_suspend,
+	.resume = iguanair_resume,
+	.reset_resume = iguanair_resume,
+	.id_table = iguanair_table
+};
+
+module_usb_driver(iguanair_driver);
+
+MODULE_DESCRIPTION("IguanaWorks USB IR Transceiver");
+MODULE_AUTHOR("Sean Young <sean@mess.org>");
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(usb, iguanair_table);
+
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 84e06d3aa69..f38d9a8c688 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -199,6 +199,7 @@ static bool debug;
 #define VENDOR_REALTEK		0x0bda
 #define VENDOR_TIVO		0x105a
 #define VENDOR_CONEXANT		0x0572
+#define VENDOR_TWISTEDMELON	0x2596
 
 enum mceusb_model_type {
 	MCE_GEN2 = 0,		/* Most boards */
@@ -391,6 +392,12 @@ static struct usb_device_id mceusb_dev_table[] = {
 	/* Conexant Hybrid TV RDU253S Polaris */
 	{ USB_DEVICE(VENDOR_CONEXANT, 0x58a5),
 	  .driver_info = CX_HYBRID_TV },
+	/* Twisted Melon Inc. - Manta Mini Receiver */
+	{ USB_DEVICE(VENDOR_TWISTEDMELON, 0x8008) },
+	/* Twisted Melon Inc. - Manta Pico Receiver */
+	{ USB_DEVICE(VENDOR_TWISTEDMELON, 0x8016) },
+	/* Twisted Melon Inc. - Manta Transceiver */
+	{ USB_DEVICE(VENDOR_TWISTEDMELON, 0x8042) },
 	/* Terminating entry */
 	{ }
 };
@@ -410,14 +417,12 @@ struct mceusb_dev {
 	/* usb */
 	struct usb_device *usbdev;
 	struct urb *urb_in;
-	struct usb_endpoint_descriptor *usb_ep_in;
 	struct usb_endpoint_descriptor *usb_ep_out;
 
 	/* buffers and dma */
 	unsigned char *buf_in;
 	unsigned int len_in;
 	dma_addr_t dma_in;
-	dma_addr_t dma_out;
 
 	enum {
 		CMD_HEADER = 0,
@@ -686,7 +691,7 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, char *buf,
 		dev_info(dev, "Raw IR data, %d pulse/space samples\n", ir->rem);
 }
 
-static void mce_async_callback(struct urb *urb, struct pt_regs *regs)
+static void mce_async_callback(struct urb *urb)
 {
 	struct mceusb_dev *ir;
 	int len;
@@ -733,7 +738,7 @@ static void mce_request_packet(struct mceusb_dev *ir, unsigned char *data,
 		pipe = usb_sndintpipe(ir->usbdev,
 				      ir->usb_ep_out->bEndpointAddress);
 		usb_fill_int_urb(async_urb, ir->usbdev, pipe,
-			async_buf, size, (usb_complete_t)mce_async_callback,
+			async_buf, size, mce_async_callback,
 			ir, ir->usb_ep_out->bInterval);
 		memcpy(async_buf, data, size);
 
@@ -1031,7 +1036,7 @@ static void mceusb_process_ir_data(struct mceusb_dev *ir, int buf_len)
 	ir_raw_event_handle(ir->rc);
 }
 
-static void mceusb_dev_recv(struct urb *urb, struct pt_regs *regs)
+static void mceusb_dev_recv(struct urb *urb)
 {
 	struct mceusb_dev *ir;
 	int buf_len;
@@ -1331,7 +1336,6 @@ static int __devinit mceusb_dev_probe(struct usb_interface *intf,
 	ir->model = model;
 
 	/* Saving usb interface data for use by the transmitter routine */
-	ir->usb_ep_in = ep_in;
 	ir->usb_ep_out = ep_out;
 
 	if (dev->descriptor.iManufacturer
@@ -1349,8 +1353,8 @@ static int __devinit mceusb_dev_probe(struct usb_interface *intf,
 		goto rc_dev_fail;
 
 	/* wire up inbound data handler */
-	usb_fill_int_urb(ir->urb_in, dev, pipe, ir->buf_in,
-		maxp, (usb_complete_t) mceusb_dev_recv, ir, ep_in->bInterval);
+	usb_fill_int_urb(ir->urb_in, dev, pipe, ir->buf_in, maxp,
+				mceusb_dev_recv, ir, ep_in->bInterval);
 	ir->urb_in->transfer_dma = ir->dma_in;
 	ir->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index dc8a7dddccd..699eef39128 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -25,6 +25,8 @@
  * USA
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/pnp.h>
@@ -123,43 +125,40 @@ static u8 nvt_cir_wake_reg_read(struct nvt_dev *nvt, u8 offset)
 	return val;
 }
 
-#define pr_reg(text, ...) \
-	printk(KERN_INFO KBUILD_MODNAME ": " text, ## __VA_ARGS__)
-
 /* dump current cir register contents */
 static void cir_dump_regs(struct nvt_dev *nvt)
 {
 	nvt_efm_enable(nvt);
 	nvt_select_logical_dev(nvt, LOGICAL_DEV_CIR);
 
-	pr_reg("%s: Dump CIR logical device registers:\n", NVT_DRIVER_NAME);
-	pr_reg(" * CR CIR ACTIVE :   0x%x\n",
-	       nvt_cr_read(nvt, CR_LOGICAL_DEV_EN));
-	pr_reg(" * CR CIR BASE ADDR: 0x%x\n",
-	       (nvt_cr_read(nvt, CR_CIR_BASE_ADDR_HI) << 8) |
+	pr_info("%s: Dump CIR logical device registers:\n", NVT_DRIVER_NAME);
+	pr_info(" * CR CIR ACTIVE :   0x%x\n",
+		nvt_cr_read(nvt, CR_LOGICAL_DEV_EN));
+	pr_info(" * CR CIR BASE ADDR: 0x%x\n",
+		(nvt_cr_read(nvt, CR_CIR_BASE_ADDR_HI) << 8) |
 		nvt_cr_read(nvt, CR_CIR_BASE_ADDR_LO));
-	pr_reg(" * CR CIR IRQ NUM:   0x%x\n",
-	       nvt_cr_read(nvt, CR_CIR_IRQ_RSRC));
+	pr_info(" * CR CIR IRQ NUM:   0x%x\n",
+		nvt_cr_read(nvt, CR_CIR_IRQ_RSRC));
 
 	nvt_efm_disable(nvt);
 
-	pr_reg("%s: Dump CIR registers:\n", NVT_DRIVER_NAME);
-	pr_reg(" * IRCON:     0x%x\n", nvt_cir_reg_read(nvt, CIR_IRCON));
-	pr_reg(" * IRSTS:     0x%x\n", nvt_cir_reg_read(nvt, CIR_IRSTS));
-	pr_reg(" * IREN:      0x%x\n", nvt_cir_reg_read(nvt, CIR_IREN));
-	pr_reg(" * RXFCONT:   0x%x\n", nvt_cir_reg_read(nvt, CIR_RXFCONT));
-	pr_reg(" * CP:        0x%x\n", nvt_cir_reg_read(nvt, CIR_CP));
-	pr_reg(" * CC:        0x%x\n", nvt_cir_reg_read(nvt, CIR_CC));
-	pr_reg(" * SLCH:      0x%x\n", nvt_cir_reg_read(nvt, CIR_SLCH));
-	pr_reg(" * SLCL:      0x%x\n", nvt_cir_reg_read(nvt, CIR_SLCL));
-	pr_reg(" * FIFOCON:   0x%x\n", nvt_cir_reg_read(nvt, CIR_FIFOCON));
-	pr_reg(" * IRFIFOSTS: 0x%x\n", nvt_cir_reg_read(nvt, CIR_IRFIFOSTS));
-	pr_reg(" * SRXFIFO:   0x%x\n", nvt_cir_reg_read(nvt, CIR_SRXFIFO));
-	pr_reg(" * TXFCONT:   0x%x\n", nvt_cir_reg_read(nvt, CIR_TXFCONT));
-	pr_reg(" * STXFIFO:   0x%x\n", nvt_cir_reg_read(nvt, CIR_STXFIFO));
-	pr_reg(" * FCCH:      0x%x\n", nvt_cir_reg_read(nvt, CIR_FCCH));
-	pr_reg(" * FCCL:      0x%x\n", nvt_cir_reg_read(nvt, CIR_FCCL));
-	pr_reg(" * IRFSM:     0x%x\n", nvt_cir_reg_read(nvt, CIR_IRFSM));
+	pr_info("%s: Dump CIR registers:\n", NVT_DRIVER_NAME);
+	pr_info(" * IRCON:     0x%x\n", nvt_cir_reg_read(nvt, CIR_IRCON));
+	pr_info(" * IRSTS:     0x%x\n", nvt_cir_reg_read(nvt, CIR_IRSTS));
+	pr_info(" * IREN:      0x%x\n", nvt_cir_reg_read(nvt, CIR_IREN));
+	pr_info(" * RXFCONT:   0x%x\n", nvt_cir_reg_read(nvt, CIR_RXFCONT));
+	pr_info(" * CP:        0x%x\n", nvt_cir_reg_read(nvt, CIR_CP));
+	pr_info(" * CC:        0x%x\n", nvt_cir_reg_read(nvt, CIR_CC));
+	pr_info(" * SLCH:      0x%x\n", nvt_cir_reg_read(nvt, CIR_SLCH));
+	pr_info(" * SLCL:      0x%x\n", nvt_cir_reg_read(nvt, CIR_SLCL));
+	pr_info(" * FIFOCON:   0x%x\n", nvt_cir_reg_read(nvt, CIR_FIFOCON));
+	pr_info(" * IRFIFOSTS: 0x%x\n", nvt_cir_reg_read(nvt, CIR_IRFIFOSTS));
+	pr_info(" * SRXFIFO:   0x%x\n", nvt_cir_reg_read(nvt, CIR_SRXFIFO));
+	pr_info(" * TXFCONT:   0x%x\n", nvt_cir_reg_read(nvt, CIR_TXFCONT));
+	pr_info(" * STXFIFO:   0x%x\n", nvt_cir_reg_read(nvt, CIR_STXFIFO));
+	pr_info(" * FCCH:      0x%x\n", nvt_cir_reg_read(nvt, CIR_FCCH));
+	pr_info(" * FCCL:      0x%x\n", nvt_cir_reg_read(nvt, CIR_FCCL));
+	pr_info(" * IRFSM:     0x%x\n", nvt_cir_reg_read(nvt, CIR_IRFSM));
 }
 
 /* dump current cir wake register contents */
@@ -170,59 +169,59 @@ static void cir_wake_dump_regs(struct nvt_dev *nvt)
 	nvt_efm_enable(nvt);
 	nvt_select_logical_dev(nvt, LOGICAL_DEV_CIR_WAKE);
 
-	pr_reg("%s: Dump CIR WAKE logical device registers:\n",
-	       NVT_DRIVER_NAME);
-	pr_reg(" * CR CIR WAKE ACTIVE :   0x%x\n",
-	       nvt_cr_read(nvt, CR_LOGICAL_DEV_EN));
-	pr_reg(" * CR CIR WAKE BASE ADDR: 0x%x\n",
-	       (nvt_cr_read(nvt, CR_CIR_BASE_ADDR_HI) << 8) |
+	pr_info("%s: Dump CIR WAKE logical device registers:\n",
+		NVT_DRIVER_NAME);
+	pr_info(" * CR CIR WAKE ACTIVE :   0x%x\n",
+		nvt_cr_read(nvt, CR_LOGICAL_DEV_EN));
+	pr_info(" * CR CIR WAKE BASE ADDR: 0x%x\n",
+		(nvt_cr_read(nvt, CR_CIR_BASE_ADDR_HI) << 8) |
 		nvt_cr_read(nvt, CR_CIR_BASE_ADDR_LO));
-	pr_reg(" * CR CIR WAKE IRQ NUM:   0x%x\n",
-	       nvt_cr_read(nvt, CR_CIR_IRQ_RSRC));
+	pr_info(" * CR CIR WAKE IRQ NUM:   0x%x\n",
+		nvt_cr_read(nvt, CR_CIR_IRQ_RSRC));
 
 	nvt_efm_disable(nvt);
 
-	pr_reg("%s: Dump CIR WAKE registers\n", NVT_DRIVER_NAME);
-	pr_reg(" * IRCON:          0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRCON));
-	pr_reg(" * IRSTS:          0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRSTS));
-	pr_reg(" * IREN:           0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_IREN));
-	pr_reg(" * FIFO CMP DEEP:  0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_CMP_DEEP));
-	pr_reg(" * FIFO CMP TOL:   0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_CMP_TOL));
-	pr_reg(" * FIFO COUNT:     0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_COUNT));
-	pr_reg(" * SLCH:           0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_SLCH));
-	pr_reg(" * SLCL:           0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_SLCL));
-	pr_reg(" * FIFOCON:        0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFOCON));
-	pr_reg(" * SRXFSTS:        0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_SRXFSTS));
-	pr_reg(" * SAMPLE RX FIFO: 0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_SAMPLE_RX_FIFO));
-	pr_reg(" * WR FIFO DATA:   0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_WR_FIFO_DATA));
-	pr_reg(" * RD FIFO ONLY:   0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_RD_FIFO_ONLY));
-	pr_reg(" * RD FIFO ONLY IDX: 0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_RD_FIFO_ONLY_IDX));
-	pr_reg(" * FIFO IGNORE:    0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_IGNORE));
-	pr_reg(" * IRFSM:          0x%x\n",
-	       nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRFSM));
+	pr_info("%s: Dump CIR WAKE registers\n", NVT_DRIVER_NAME);
+	pr_info(" * IRCON:          0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRCON));
+	pr_info(" * IRSTS:          0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRSTS));
+	pr_info(" * IREN:           0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_IREN));
+	pr_info(" * FIFO CMP DEEP:  0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_CMP_DEEP));
+	pr_info(" * FIFO CMP TOL:   0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_CMP_TOL));
+	pr_info(" * FIFO COUNT:     0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_COUNT));
+	pr_info(" * SLCH:           0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_SLCH));
+	pr_info(" * SLCL:           0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_SLCL));
+	pr_info(" * FIFOCON:        0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFOCON));
+	pr_info(" * SRXFSTS:        0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_SRXFSTS));
+	pr_info(" * SAMPLE RX FIFO: 0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_SAMPLE_RX_FIFO));
+	pr_info(" * WR FIFO DATA:   0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_WR_FIFO_DATA));
+	pr_info(" * RD FIFO ONLY:   0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_RD_FIFO_ONLY));
+	pr_info(" * RD FIFO ONLY IDX: 0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_RD_FIFO_ONLY_IDX));
+	pr_info(" * FIFO IGNORE:    0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_IGNORE));
+	pr_info(" * IRFSM:          0x%x\n",
+		nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRFSM));
 
 	fifo_len = nvt_cir_wake_reg_read(nvt, CIR_WAKE_FIFO_COUNT);
-	pr_reg("%s: Dump CIR WAKE FIFO (len %d)\n", NVT_DRIVER_NAME, fifo_len);
-	pr_reg("* Contents = ");
+	pr_info("%s: Dump CIR WAKE FIFO (len %d)\n", NVT_DRIVER_NAME, fifo_len);
+	pr_info("* Contents =");
 	for (i = 0; i < fifo_len; i++)
-		printk(KERN_CONT "%02x ",
-		       nvt_cir_wake_reg_read(nvt, CIR_WAKE_RD_FIFO_ONLY));
-	printk(KERN_CONT "\n");
+		pr_cont(" %02x",
+			nvt_cir_wake_reg_read(nvt, CIR_WAKE_RD_FIFO_ONLY));
+	pr_cont("\n");
 }
 
 /* detect hardware features */
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 6e16b09c24a..cabc19c1051 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -775,10 +775,11 @@ static ssize_t show_protocols(struct device *device,
 	if (dev->driver_type == RC_DRIVER_SCANCODE) {
 		enabled = dev->rc_map.rc_type;
 		allowed = dev->allowed_protos;
-	} else {
+	} else if (dev->raw) {
 		enabled = dev->raw->enabled_protocols;
 		allowed = ir_raw_get_allowed_protocols();
-	}
+	} else
+		return -ENODEV;
 
 	IR_dprintk(1, "allowed - 0x%llx, enabled - 0x%llx\n",
 		   (long long)allowed,
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index 99937c94d7d..c128fac0ce2 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -5,7 +5,7 @@
 config VIDEO_V4L2
 	tristate
 	depends on VIDEO_DEV && VIDEO_V4L2_COMMON
-	default VIDEO_DEV && VIDEO_V4L2_COMMON
+	default y
 
 config VIDEOBUF_GEN
 	tristate
@@ -73,6 +73,7 @@ config VIDEOBUF2_DMA_SG
 menuconfig VIDEO_CAPTURE_DRIVERS
 	bool "Video capture adapters"
 	depends on VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT || MEDIA_ANALOG_TV_SUPPORT
 	default y
 	---help---
 	  Say Y here to enable selecting the video adapters for
@@ -461,6 +462,15 @@ config VIDEO_ADV7343
 	  To compile this driver as a module, choose M here: the
 	  module will be called adv7343.
 
+config VIDEO_ADV7393
+	tristate "ADV7393 video encoder"
+	depends on I2C
+	help
+	  Support for Analog Devices I2C bus based ADV7393 encoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7393.
+
 config VIDEO_AK881X
 	tristate "AK8813/AK8814 video encoders"
 	depends on I2C
@@ -478,6 +488,7 @@ config VIDEO_SMIAPP_PLL
 config VIDEO_OV7670
 	tristate "OmniVision OV7670 sensor support"
 	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the OmniVision
 	  OV7670 VGA camera.  It currently only works with the M88ALP01
@@ -486,6 +497,7 @@ config VIDEO_OV7670
 config VIDEO_VS6624
 	tristate "ST VS6624 sensor support"
 	depends on VIDEO_V4L2 && I2C
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the ST VS6624
 	  camera.
@@ -496,6 +508,7 @@ config VIDEO_VS6624
 config VIDEO_MT9M032
 	tristate "MT9M032 camera sensor support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
 	select VIDEO_APTINA_PLL
 	---help---
 	  This driver supports MT9M032 camera sensors from Aptina, monochrome
@@ -504,6 +517,7 @@ config VIDEO_MT9M032
 config VIDEO_MT9P031
 	tristate "Aptina MT9P031 support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
 	select VIDEO_APTINA_PLL
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the Aptina
@@ -512,6 +526,7 @@ config VIDEO_MT9P031
 config VIDEO_MT9T001
 	tristate "Aptina MT9T001 support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the Aptina
 	  (Micron) mt0t001 3 Mpixel camera.
@@ -519,6 +534,7 @@ config VIDEO_MT9T001
 config VIDEO_MT9V011
 	tristate "Micron mt9v011 sensor support"
 	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the Micron
 	  mt0v011 1.3 Mpixel camera.  It currently only works with the
@@ -527,6 +543,7 @@ config VIDEO_MT9V011
 config VIDEO_MT9V032
 	tristate "Micron MT9V032 sensor support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a Video4Linux2 sensor-level driver for the Micron
 	  MT9V032 752x480 CMOS sensor.
@@ -534,6 +551,7 @@ config VIDEO_MT9V032
 config VIDEO_TCM825X
 	tristate "TCM825x camera sensor support"
 	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a driver for the Toshiba TCM825x VGA camera sensor.
 	  It is used for example in Nokia N800.
@@ -541,12 +559,14 @@ config VIDEO_TCM825X
 config VIDEO_SR030PC30
 	tristate "Siliconfile SR030PC30 sensor support"
 	depends on I2C && VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This driver supports SR030PC30 VGA camera from Siliconfile
 
 config VIDEO_NOON010PC30
 	tristate "Siliconfile NOON010PC30 sensor support"
 	depends on I2C && VIDEO_V4L2 && EXPERIMENTAL && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This driver supports NOON010PC30 CIF camera from Siliconfile
 
@@ -554,6 +574,7 @@ source "drivers/media/video/m5mols/Kconfig"
 
 config VIDEO_S5K6AA
 	tristate "Samsung S5K6AAFX sensor support"
+	depends on MEDIA_CAMERA_SUPPORT
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	---help---
 	  This is a V4L2 sensor-level driver for Samsung S5K6AA(FX) 1.3M
@@ -566,6 +587,7 @@ comment "Flash devices"
 config VIDEO_ADP1653
 	tristate "ADP1653 flash support"
 	depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a driver for the ADP1653 flash controller. It is used for
 	  example in Nokia N900.
@@ -573,6 +595,7 @@ config VIDEO_ADP1653
 config VIDEO_AS3645A
 	tristate "AS3645A flash driver support"
 	depends on I2C && VIDEO_V4L2 && MEDIA_CONTROLLER
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This is a driver for the AS3645A and LM3555 flash controllers. It has
 	  build in control for flash, torch and indicator LEDs.
@@ -647,30 +670,14 @@ menuconfig V4L_USB_DRIVERS
 	depends on USB
 	default y
 
-if V4L_USB_DRIVERS
+if V4L_USB_DRIVERS && MEDIA_CAMERA_SUPPORT
 
-source "drivers/media/video/au0828/Kconfig"
+	comment "Webcam devices"
 
 source "drivers/media/video/uvc/Kconfig"
 
 source "drivers/media/video/gspca/Kconfig"
 
-source "drivers/media/video/pvrusb2/Kconfig"
-
-source "drivers/media/video/hdpvr/Kconfig"
-
-source "drivers/media/video/em28xx/Kconfig"
-
-source "drivers/media/video/tlg2300/Kconfig"
-
-source "drivers/media/video/cx231xx/Kconfig"
-
-source "drivers/media/video/tm6000/Kconfig"
-
-source "drivers/media/video/usbvision/Kconfig"
-
-source "drivers/media/video/sn9c102/Kconfig"
-
 source "drivers/media/video/pwc/Kconfig"
 
 source "drivers/media/video/cpia2/Kconfig"
@@ -711,15 +718,46 @@ config USB_S2255
 	  Say Y here if you want support for the Sensoray 2255 USB device.
 	  This driver can be compiled as a module, called s2255drv.
 
+source "drivers/media/video/sn9c102/Kconfig"
+
+endif # V4L_USB_DRIVERS && MEDIA_CAMERA_SUPPORT
+
+if V4L_USB_DRIVERS
+
+	comment "Webcam and/or TV USB devices"
+
+source "drivers/media/video/em28xx/Kconfig"
+
+endif
+
+if V4L_USB_DRIVERS && MEDIA_ANALOG_TV_SUPPORT
+
+	comment "TV USB devices"
+
+source "drivers/media/video/au0828/Kconfig"
+
+source "drivers/media/video/pvrusb2/Kconfig"
+
+source "drivers/media/video/hdpvr/Kconfig"
+
+source "drivers/media/video/tlg2300/Kconfig"
+
+source "drivers/media/video/cx231xx/Kconfig"
+
+source "drivers/media/video/tm6000/Kconfig"
+
+source "drivers/media/video/usbvision/Kconfig"
+
 endif # V4L_USB_DRIVERS
 
 #
-# PCI drivers configuration
+# PCI drivers configuration - No devices here are for webcams
 #
 
 menuconfig V4L_PCI_DRIVERS
 	bool "V4L PCI(e) devices"
 	depends on PCI
+	depends on MEDIA_ANALOG_TV_SUPPORT
 	default y
 	---help---
 	  Say Y here to enable support for these PCI(e) drivers.
@@ -814,11 +852,13 @@ endif # V4L_PCI_DRIVERS
 
 #
 # ISA & parallel port drivers configuration
+#	All devices here are webcam or grabber devices
 #
 
 menuconfig V4L_ISA_PARPORT_DRIVERS
 	bool "V4L ISA and parallel port devices"
 	depends on ISA || PARPORT
+	depends on MEDIA_CAMERA_SUPPORT
 	default n
 	---help---
 	  Say Y here to enable support for these ISA and parallel port drivers.
@@ -871,8 +911,13 @@ config VIDEO_W9966
 
 endif # V4L_ISA_PARPORT_DRIVERS
 
+#
+# Platform drivers
+#	All drivers here are currently for webcam support
+
 menuconfig V4L_PLATFORM_DRIVERS
 	bool "V4L platform devices"
+	depends on MEDIA_CAMERA_SUPPORT
 	default n
 	---help---
 	  Say Y here to enable support for platform-specific V4L drivers.
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index d209de0e0ca..b7da9faa3b0 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -45,6 +45,7 @@ obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
 obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
 obj-$(CONFIG_VIDEO_ADV7183) += adv7183.o
 obj-$(CONFIG_VIDEO_ADV7343) += adv7343.o
+obj-$(CONFIG_VIDEO_ADV7393) += adv7393.o
 obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o
 obj-$(CONFIG_VIDEO_VS6624)  += vs6624.o
 obj-$(CONFIG_VIDEO_BT819) += bt819.o
diff --git a/drivers/media/video/adv7180.c b/drivers/media/video/adv7180.c
index 174bffacf11..45ecf8db1ea 100644
--- a/drivers/media/video/adv7180.c
+++ b/drivers/media/video/adv7180.c
@@ -26,11 +26,10 @@
 #include <media/v4l2-ioctl.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-chip-ident.h>
 #include <linux/mutex.h>
 
-#define DRIVER_NAME "adv7180"
-
 #define ADV7180_INPUT_CONTROL_REG			0x00
 #define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM	0x00
 #define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM_PED 0x10
@@ -55,21 +54,21 @@
 
 #define ADV7180_AUTODETECT_ENABLE_REG			0x07
 #define ADV7180_AUTODETECT_DEFAULT			0x7f
-
+/* Contrast */
 #define ADV7180_CON_REG		0x08	/*Unsigned */
-#define CON_REG_MIN		0
-#define CON_REG_DEF		128
-#define CON_REG_MAX		255
-
+#define ADV7180_CON_MIN		0
+#define ADV7180_CON_DEF		128
+#define ADV7180_CON_MAX		255
+/* Brightness*/
 #define ADV7180_BRI_REG		0x0a	/*Signed */
-#define BRI_REG_MIN		-128
-#define BRI_REG_DEF		0
-#define BRI_REG_MAX		127
-
+#define ADV7180_BRI_MIN		-128
+#define ADV7180_BRI_DEF		0
+#define ADV7180_BRI_MAX		127
+/* Hue */
 #define ADV7180_HUE_REG		0x0b	/*Signed, inverted */
-#define HUE_REG_MIN		-127
-#define HUE_REG_DEF		0
-#define HUE_REG_MAX		128
+#define ADV7180_HUE_MIN		-127
+#define ADV7180_HUE_DEF		0
+#define ADV7180_HUE_MAX		128
 
 #define ADV7180_ADI_CTRL_REG				0x0e
 #define ADV7180_ADI_CTRL_IRQ_SPACE			0x20
@@ -98,12 +97,12 @@
 #define ADV7180_ICONF1_ACTIVE_LOW	0x01
 #define ADV7180_ICONF1_PSYNC_ONLY	0x10
 #define ADV7180_ICONF1_ACTIVE_TO_CLR	0xC0
-
+/* Saturation */
 #define ADV7180_SD_SAT_CB_REG	0xe3	/*Unsigned */
 #define ADV7180_SD_SAT_CR_REG	0xe4	/*Unsigned */
-#define SAT_REG_MIN		0
-#define SAT_REG_DEF		128
-#define SAT_REG_MAX		255
+#define ADV7180_SAT_MIN		0
+#define ADV7180_SAT_DEF		128
+#define ADV7180_SAT_MAX		255
 
 #define ADV7180_IRQ1_LOCK	0x01
 #define ADV7180_IRQ1_UNLOCK	0x02
@@ -121,18 +120,18 @@
 #define ADV7180_NTSC_V_BIT_END_MANUAL_NVEND	0x4F
 
 struct adv7180_state {
+	struct v4l2_ctrl_handler ctrl_hdl;
 	struct v4l2_subdev	sd;
 	struct work_struct	work;
 	struct mutex		mutex; /* mutual excl. when accessing chip */
 	int			irq;
 	v4l2_std_id		curr_norm;
 	bool			autodetect;
-	s8			brightness;
-	s16			hue;
-	u8			contrast;
-	u8			saturation;
 	u8			input;
 };
+#define to_adv7180_sd(_ctrl) (&container_of(_ctrl->handler,		\
+					    struct adv7180_state,	\
+					    ctrl_hdl)->sd)
 
 static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
 {
@@ -237,7 +236,7 @@ static int adv7180_s_routing(struct v4l2_subdev *sd, u32 input,
 	if (ret)
 		return ret;
 
-	/*We cannot discriminate between LQFP and 40-pin LFCSP, so accept
+	/* We cannot discriminate between LQFP and 40-pin LFCSP, so accept
 	 * all inputs and let the card driver take care of validation
 	 */
 	if ((input & ADV7180_INPUT_CONTROL_INSEL_MASK) != input)
@@ -316,117 +315,39 @@ out:
 	return ret;
 }
 
-static int adv7180_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
-{
-	switch (qc->id) {
-	case V4L2_CID_BRIGHTNESS:
-		return v4l2_ctrl_query_fill(qc, BRI_REG_MIN, BRI_REG_MAX,
-					    1, BRI_REG_DEF);
-	case V4L2_CID_HUE:
-		return v4l2_ctrl_query_fill(qc, HUE_REG_MIN, HUE_REG_MAX,
-					    1, HUE_REG_DEF);
-	case V4L2_CID_CONTRAST:
-		return v4l2_ctrl_query_fill(qc, CON_REG_MIN, CON_REG_MAX,
-					    1, CON_REG_DEF);
-	case V4L2_CID_SATURATION:
-		return v4l2_ctrl_query_fill(qc, SAT_REG_MIN, SAT_REG_MAX,
-					    1, SAT_REG_DEF);
-	default:
-		break;
-	}
-
-	return -EINVAL;
-}
-
-static int adv7180_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
-{
-	struct adv7180_state *state = to_state(sd);
-	int ret = mutex_lock_interruptible(&state->mutex);
-	if (ret)
-		return ret;
-
-	switch (ctrl->id) {
-	case V4L2_CID_BRIGHTNESS:
-		ctrl->value = state->brightness;
-		break;
-	case V4L2_CID_HUE:
-		ctrl->value = state->hue;
-		break;
-	case V4L2_CID_CONTRAST:
-		ctrl->value = state->contrast;
-		break;
-	case V4L2_CID_SATURATION:
-		ctrl->value = state->saturation;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-
-	mutex_unlock(&state->mutex);
-	return ret;
-}
-
-static int adv7180_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+static int adv7180_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct v4l2_subdev *sd = to_adv7180_sd(ctrl);
 	struct adv7180_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	int ret = mutex_lock_interruptible(&state->mutex);
+	int val;
+
 	if (ret)
 		return ret;
-
+	val = ctrl->val;
 	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		if ((ctrl->value > BRI_REG_MAX)
-		    || (ctrl->value < BRI_REG_MIN)) {
-			ret = -ERANGE;
-			break;
-		}
-		state->brightness = ctrl->value;
-		ret = i2c_smbus_write_byte_data(client,
-						ADV7180_BRI_REG,
-						state->brightness);
+		ret = i2c_smbus_write_byte_data(client, ADV7180_BRI_REG, val);
 		break;
 	case V4L2_CID_HUE:
-		if ((ctrl->value > HUE_REG_MAX)
-		    || (ctrl->value < HUE_REG_MIN)) {
-			ret = -ERANGE;
-			break;
-		}
-		state->hue = ctrl->value;
 		/*Hue is inverted according to HSL chart */
-		ret = i2c_smbus_write_byte_data(client,
-						ADV7180_HUE_REG, -state->hue);
+		ret = i2c_smbus_write_byte_data(client, ADV7180_HUE_REG, -val);
 		break;
 	case V4L2_CID_CONTRAST:
-		if ((ctrl->value > CON_REG_MAX)
-		    || (ctrl->value < CON_REG_MIN)) {
-			ret = -ERANGE;
-			break;
-		}
-		state->contrast = ctrl->value;
-		ret = i2c_smbus_write_byte_data(client,
-						ADV7180_CON_REG,
-						state->contrast);
+		ret = i2c_smbus_write_byte_data(client, ADV7180_CON_REG, val);
 		break;
 	case V4L2_CID_SATURATION:
-		if ((ctrl->value > SAT_REG_MAX)
-		    || (ctrl->value < SAT_REG_MIN)) {
-			ret = -ERANGE;
-			break;
-		}
 		/*
 		 *This could be V4L2_CID_BLUE_BALANCE/V4L2_CID_RED_BALANCE
 		 *Let's not confuse the user, everybody understands saturation
 		 */
-		state->saturation = ctrl->value;
-		ret = i2c_smbus_write_byte_data(client,
-						ADV7180_SD_SAT_CB_REG,
-						state->saturation);
+		ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CB_REG,
+						val);
 		if (ret < 0)
 			break;
-		ret = i2c_smbus_write_byte_data(client,
-						ADV7180_SD_SAT_CR_REG,
-						state->saturation);
+		ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CR_REG,
+						val);
 		break;
 	default:
 		ret = -EINVAL;
@@ -436,6 +357,42 @@ static int adv7180_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 	return ret;
 }
 
+static const struct v4l2_ctrl_ops adv7180_ctrl_ops = {
+	.s_ctrl = adv7180_s_ctrl,
+};
+
+static int adv7180_init_controls(struct adv7180_state *state)
+{
+	v4l2_ctrl_handler_init(&state->ctrl_hdl, 4);
+
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, ADV7180_BRI_MIN,
+			  ADV7180_BRI_MAX, 1, ADV7180_BRI_DEF);
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_CONTRAST, ADV7180_CON_MIN,
+			  ADV7180_CON_MAX, 1, ADV7180_CON_DEF);
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_SATURATION, ADV7180_SAT_MIN,
+			  ADV7180_SAT_MAX, 1, ADV7180_SAT_DEF);
+	v4l2_ctrl_new_std(&state->ctrl_hdl, &adv7180_ctrl_ops,
+			  V4L2_CID_HUE, ADV7180_HUE_MIN,
+			  ADV7180_HUE_MAX, 1, ADV7180_HUE_DEF);
+	state->sd.ctrl_handler = &state->ctrl_hdl;
+	if (state->ctrl_hdl.error) {
+		int err = state->ctrl_hdl.error;
+
+		v4l2_ctrl_handler_free(&state->ctrl_hdl);
+		return err;
+	}
+	v4l2_ctrl_handler_setup(&state->ctrl_hdl);
+
+	return 0;
+}
+static void adv7180_exit_controls(struct adv7180_state *state)
+{
+	v4l2_ctrl_handler_free(&state->ctrl_hdl);
+}
+
 static const struct v4l2_subdev_video_ops adv7180_video_ops = {
 	.querystd = adv7180_querystd,
 	.g_input_status = adv7180_g_input_status,
@@ -445,9 +402,9 @@ static const struct v4l2_subdev_video_ops adv7180_video_ops = {
 static const struct v4l2_subdev_core_ops adv7180_core_ops = {
 	.g_chip_ident = adv7180_g_chip_ident,
 	.s_std = adv7180_s_std,
-	.queryctrl = adv7180_queryctrl,
-	.g_ctrl = adv7180_g_ctrl,
-	.s_ctrl = adv7180_s_ctrl,
+	.queryctrl = v4l2_subdev_queryctrl,
+	.g_ctrl = v4l2_subdev_g_ctrl,
+	.s_ctrl = v4l2_subdev_s_ctrl,
 };
 
 static const struct v4l2_subdev_ops adv7180_ops = {
@@ -539,7 +496,7 @@ static int init_device(struct i2c_client *client, struct adv7180_state *state)
 
 	/* register for interrupts */
 	if (state->irq > 0) {
-		ret = request_irq(state->irq, adv7180_irq, 0, DRIVER_NAME,
+		ret = request_irq(state->irq, adv7180_irq, 0, KBUILD_MODNAME,
 				  state);
 		if (ret)
 			return ret;
@@ -580,31 +537,6 @@ static int init_device(struct i2c_client *client, struct adv7180_state *state)
 			return ret;
 	}
 
-	/*Set default value for controls */
-	ret = i2c_smbus_write_byte_data(client, ADV7180_BRI_REG,
-					state->brightness);
-	if (ret < 0)
-		return ret;
-
-	ret = i2c_smbus_write_byte_data(client, ADV7180_HUE_REG, state->hue);
-	if (ret < 0)
-		return ret;
-
-	ret = i2c_smbus_write_byte_data(client, ADV7180_CON_REG,
-					state->contrast);
-	if (ret < 0)
-		return ret;
-
-	ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CB_REG,
-					state->saturation);
-	if (ret < 0)
-		return ret;
-
-	ret = i2c_smbus_write_byte_data(client, ADV7180_SD_SAT_CR_REG,
-					state->saturation);
-	if (ret < 0)
-		return ret;
-
 	return 0;
 }
 
@@ -632,25 +564,26 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 	INIT_WORK(&state->work, adv7180_work);
 	mutex_init(&state->mutex);
 	state->autodetect = true;
-	state->brightness = BRI_REG_DEF;
-	state->hue = HUE_REG_DEF;
-	state->contrast = CON_REG_DEF;
-	state->saturation = SAT_REG_DEF;
 	state->input = 0;
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
 
-	ret = init_device(client, state);
-	if (0 != ret)
+	ret = adv7180_init_controls(state);
+	if (ret)
 		goto err_unreg_subdev;
+	ret = init_device(client, state);
+	if (ret)
+		goto err_free_ctrl;
 	return 0;
 
+err_free_ctrl:
+	adv7180_exit_controls(state);
 err_unreg_subdev:
 	mutex_destroy(&state->mutex);
 	v4l2_device_unregister_subdev(sd);
 	kfree(state);
 err:
-	printk(KERN_ERR DRIVER_NAME ": Failed to probe: %d\n", ret);
+	printk(KERN_ERR KBUILD_MODNAME ": Failed to probe: %d\n", ret);
 	return ret;
 }
 
@@ -678,7 +611,7 @@ static __devexit int adv7180_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id adv7180_id[] = {
-	{DRIVER_NAME, 0},
+	{KBUILD_MODNAME, 0},
 	{},
 };
 
@@ -716,7 +649,7 @@ MODULE_DEVICE_TABLE(i2c, adv7180_id);
 static struct i2c_driver adv7180_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
-		   .name = DRIVER_NAME,
+		   .name = KBUILD_MODNAME,
 		   },
 	.probe = adv7180_probe,
 	.remove = __devexit_p(adv7180_remove),
diff --git a/drivers/media/video/adv7393.c b/drivers/media/video/adv7393.c
new file mode 100644
index 00000000000..3dc6098c726
--- /dev/null
+++ b/drivers/media/video/adv7393.c
@@ -0,0 +1,487 @@
+/*
+ * adv7393 - ADV7393 Video Encoder Driver
+ *
+ * The encoder hardware does not support SECAM.
+ *
+ * Copyright (C) 2010-2012 ADVANSEE - http://www.advansee.com/
+ * Benoît Thébaudeau <benoit.thebaudeau@advansee.com>
+ *
+ * Based on ADV7343 driver,
+ *
+ * Copyright (C) 2009 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/kernel.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/videodev2.h>
+#include <linux/uaccess.h>
+
+#include <media/adv7393.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-ctrls.h>
+
+#include "adv7393_regs.h"
+
+MODULE_DESCRIPTION("ADV7393 video encoder driver");
+MODULE_LICENSE("GPL");
+
+static bool debug;
+module_param(debug, bool, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+struct adv7393_state {
+	struct v4l2_subdev sd;
+	struct v4l2_ctrl_handler hdl;
+	u8 reg00;
+	u8 reg01;
+	u8 reg02;
+	u8 reg35;
+	u8 reg80;
+	u8 reg82;
+	u32 output;
+	v4l2_std_id std;
+};
+
+static inline struct adv7393_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7393_state, sd);
+}
+
+static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
+{
+	return &container_of(ctrl->handler, struct adv7393_state, hdl)->sd;
+}
+
+static inline int adv7393_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static const u8 adv7393_init_reg_val[] = {
+	ADV7393_SOFT_RESET, ADV7393_SOFT_RESET_DEFAULT,
+	ADV7393_POWER_MODE_REG, ADV7393_POWER_MODE_REG_DEFAULT,
+
+	ADV7393_HD_MODE_REG1, ADV7393_HD_MODE_REG1_DEFAULT,
+	ADV7393_HD_MODE_REG2, ADV7393_HD_MODE_REG2_DEFAULT,
+	ADV7393_HD_MODE_REG3, ADV7393_HD_MODE_REG3_DEFAULT,
+	ADV7393_HD_MODE_REG4, ADV7393_HD_MODE_REG4_DEFAULT,
+	ADV7393_HD_MODE_REG5, ADV7393_HD_MODE_REG5_DEFAULT,
+	ADV7393_HD_MODE_REG6, ADV7393_HD_MODE_REG6_DEFAULT,
+	ADV7393_HD_MODE_REG7, ADV7393_HD_MODE_REG7_DEFAULT,
+
+	ADV7393_SD_MODE_REG1, ADV7393_SD_MODE_REG1_DEFAULT,
+	ADV7393_SD_MODE_REG2, ADV7393_SD_MODE_REG2_DEFAULT,
+	ADV7393_SD_MODE_REG3, ADV7393_SD_MODE_REG3_DEFAULT,
+	ADV7393_SD_MODE_REG4, ADV7393_SD_MODE_REG4_DEFAULT,
+	ADV7393_SD_MODE_REG5, ADV7393_SD_MODE_REG5_DEFAULT,
+	ADV7393_SD_MODE_REG6, ADV7393_SD_MODE_REG6_DEFAULT,
+	ADV7393_SD_MODE_REG7, ADV7393_SD_MODE_REG7_DEFAULT,
+	ADV7393_SD_MODE_REG8, ADV7393_SD_MODE_REG8_DEFAULT,
+
+	ADV7393_SD_TIMING_REG0, ADV7393_SD_TIMING_REG0_DEFAULT,
+
+	ADV7393_SD_HUE_ADJUST, ADV7393_SD_HUE_ADJUST_DEFAULT,
+	ADV7393_SD_CGMS_WSS0, ADV7393_SD_CGMS_WSS0_DEFAULT,
+	ADV7393_SD_BRIGHTNESS_WSS, ADV7393_SD_BRIGHTNESS_WSS_DEFAULT,
+};
+
+/*
+ * 			    2^32
+ * FSC(reg) =  FSC (HZ) * --------
+ *			  27000000
+ */
+static const struct adv7393_std_info stdinfo[] = {
+	{
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_NTSC, 705268427, V4L2_STD_NTSC_443,
+	}, {
+		/* FSC(Hz) = 3,579,545.45 Hz */
+		SD_STD_NTSC, 569408542, V4L2_STD_NTSC,
+	}, {
+		/* FSC(Hz) = 3,575,611.00 Hz */
+		SD_STD_PAL_M, 568782678, V4L2_STD_PAL_M,
+	}, {
+		/* FSC(Hz) = 3,582,056.00 Hz */
+		SD_STD_PAL_N, 569807903, V4L2_STD_PAL_Nc,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_N, 705268427, V4L2_STD_PAL_N,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_M, 705268427, V4L2_STD_PAL_60,
+	}, {
+		/* FSC(Hz) = 4,433,618.75 Hz */
+		SD_STD_PAL_BDGHI, 705268427, V4L2_STD_PAL,
+	},
+};
+
+static int adv7393_setstd(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7393_state *state = to_state(sd);
+	const struct adv7393_std_info *std_info;
+	int num_std;
+	u8 reg;
+	u32 val;
+	int err = 0;
+	int i;
+
+	num_std = ARRAY_SIZE(stdinfo);
+
+	for (i = 0; i < num_std; i++) {
+		if (stdinfo[i].stdid & std)
+			break;
+	}
+
+	if (i == num_std) {
+		v4l2_dbg(1, debug, sd,
+				"Invalid std or std is not supported: %llx\n",
+						(unsigned long long)std);
+		return -EINVAL;
+	}
+
+	std_info = &stdinfo[i];
+
+	/* Set the standard */
+	val = state->reg80 & ~SD_STD_MASK;
+	val |= std_info->standard_val3;
+	err = adv7393_write(sd, ADV7393_SD_MODE_REG1, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg80 = val;
+
+	/* Configure the input mode register */
+	val = state->reg01 & ~INPUT_MODE_MASK;
+	val |= SD_INPUT_MODE;
+	err = adv7393_write(sd, ADV7393_MODE_SELECT_REG, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg01 = val;
+
+	/* Program the sub carrier frequency registers */
+	val = std_info->fsc_val;
+	for (reg = ADV7393_FSC_REG0; reg <= ADV7393_FSC_REG3; reg++) {
+		err = adv7393_write(sd, reg, val);
+		if (err < 0)
+			goto setstd_exit;
+		val >>= 8;
+	}
+
+	val = state->reg82;
+
+	/* Pedestal settings */
+	if (std & (V4L2_STD_NTSC | V4L2_STD_NTSC_443))
+		val |= SD_PEDESTAL_EN;
+	else
+		val &= SD_PEDESTAL_DI;
+
+	err = adv7393_write(sd, ADV7393_SD_MODE_REG2, val);
+	if (err < 0)
+		goto setstd_exit;
+
+	state->reg82 = val;
+
+setstd_exit:
+	if (err != 0)
+		v4l2_err(sd, "Error setting std, write failed\n");
+
+	return err;
+}
+
+static int adv7393_setoutput(struct v4l2_subdev *sd, u32 output_type)
+{
+	struct adv7393_state *state = to_state(sd);
+	u8 val;
+	int err = 0;
+
+	if (output_type > ADV7393_SVIDEO_ID) {
+		v4l2_dbg(1, debug, sd,
+			"Invalid output type or output type not supported:%d\n",
+								output_type);
+		return -EINVAL;
+	}
+
+	/* Enable Appropriate DAC */
+	val = state->reg00 & 0x03;
+
+	if (output_type == ADV7393_COMPOSITE_ID)
+		val |= ADV7393_COMPOSITE_POWER_VALUE;
+	else if (output_type == ADV7393_COMPONENT_ID)
+		val |= ADV7393_COMPONENT_POWER_VALUE;
+	else
+		val |= ADV7393_SVIDEO_POWER_VALUE;
+
+	err = adv7393_write(sd, ADV7393_POWER_MODE_REG, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg00 = val;
+
+	/* Enable YUV output */
+	val = state->reg02 | YUV_OUTPUT_SELECT;
+	err = adv7393_write(sd, ADV7393_MODE_REG0, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg02 = val;
+
+	/* configure SD DAC Output 1 bit */
+	val = state->reg82;
+	if (output_type == ADV7393_COMPONENT_ID)
+		val &= SD_DAC_OUT1_DI;
+	else
+		val |= SD_DAC_OUT1_EN;
+	err = adv7393_write(sd, ADV7393_SD_MODE_REG2, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg82 = val;
+
+	/* configure ED/HD Color DAC Swap bit to zero */
+	val = state->reg35 & HD_DAC_SWAP_DI;
+	err = adv7393_write(sd, ADV7393_HD_MODE_REG6, val);
+	if (err < 0)
+		goto setoutput_exit;
+
+	state->reg35 = val;
+
+setoutput_exit:
+	if (err != 0)
+		v4l2_err(sd, "Error setting output, write failed\n");
+
+	return err;
+}
+
+static int adv7393_log_status(struct v4l2_subdev *sd)
+{
+	struct adv7393_state *state = to_state(sd);
+
+	v4l2_info(sd, "Standard: %llx\n", (unsigned long long)state->std);
+	v4l2_info(sd, "Output: %s\n", (state->output == 0) ? "Composite" :
+			((state->output == 1) ? "Component" : "S-Video"));
+	return 0;
+}
+
+static int adv7393_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = to_sd(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return adv7393_write(sd, ADV7393_SD_BRIGHTNESS_WSS,
+					ctrl->val & SD_BRIGHTNESS_VALUE_MASK);
+
+	case V4L2_CID_HUE:
+		return adv7393_write(sd, ADV7393_SD_HUE_ADJUST,
+					ctrl->val - ADV7393_HUE_MIN);
+
+	case V4L2_CID_GAIN:
+		return adv7393_write(sd, ADV7393_DAC123_OUTPUT_LEVEL,
+					ctrl->val);
+	}
+	return -EINVAL;
+}
+
+static int adv7393_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_ADV7393, 0);
+}
+
+static const struct v4l2_ctrl_ops adv7393_ctrl_ops = {
+	.s_ctrl = adv7393_s_ctrl,
+};
+
+static const struct v4l2_subdev_core_ops adv7393_core_ops = {
+	.log_status = adv7393_log_status,
+	.g_chip_ident = adv7393_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 adv7393_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7393_state *state = to_state(sd);
+	int err = 0;
+
+	if (state->std == std)
+		return 0;
+
+	err = adv7393_setstd(sd, std);
+	if (!err)
+		state->std = std;
+
+	return err;
+}
+
+static int adv7393_s_routing(struct v4l2_subdev *sd,
+		u32 input, u32 output, u32 config)
+{
+	struct adv7393_state *state = to_state(sd);
+	int err = 0;
+
+	if (state->output == output)
+		return 0;
+
+	err = adv7393_setoutput(sd, output);
+	if (!err)
+		state->output = output;
+
+	return err;
+}
+
+static const struct v4l2_subdev_video_ops adv7393_video_ops = {
+	.s_std_output	= adv7393_s_std_output,
+	.s_routing	= adv7393_s_routing,
+};
+
+static const struct v4l2_subdev_ops adv7393_ops = {
+	.core	= &adv7393_core_ops,
+	.video	= &adv7393_video_ops,
+};
+
+static int adv7393_initialize(struct v4l2_subdev *sd)
+{
+	struct adv7393_state *state = to_state(sd);
+	int err = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(adv7393_init_reg_val); i += 2) {
+
+		err = adv7393_write(sd, adv7393_init_reg_val[i],
+					adv7393_init_reg_val[i+1]);
+		if (err) {
+			v4l2_err(sd, "Error initializing\n");
+			return err;
+		}
+	}
+
+	/* Configure for default video standard */
+	err = adv7393_setoutput(sd, state->output);
+	if (err < 0) {
+		v4l2_err(sd, "Error setting output during init\n");
+		return -EINVAL;
+	}
+
+	err = adv7393_setstd(sd, state->std);
+	if (err < 0) {
+		v4l2_err(sd, "Error setting std during init\n");
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+static int adv7393_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct adv7393_state *state;
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = kzalloc(sizeof(struct adv7393_state), GFP_KERNEL);
+	if (state == NULL)
+		return -ENOMEM;
+
+	state->reg00	= ADV7393_POWER_MODE_REG_DEFAULT;
+	state->reg01	= 0x00;
+	state->reg02	= 0x20;
+	state->reg35	= ADV7393_HD_MODE_REG6_DEFAULT;
+	state->reg80	= ADV7393_SD_MODE_REG1_DEFAULT;
+	state->reg82	= ADV7393_SD_MODE_REG2_DEFAULT;
+
+	state->output = ADV7393_COMPOSITE_ID;
+	state->std = V4L2_STD_NTSC;
+
+	v4l2_i2c_subdev_init(&state->sd, client, &adv7393_ops);
+
+	v4l2_ctrl_handler_init(&state->hdl, 3);
+	v4l2_ctrl_new_std(&state->hdl, &adv7393_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, ADV7393_BRIGHTNESS_MIN,
+					     ADV7393_BRIGHTNESS_MAX, 1,
+					     ADV7393_BRIGHTNESS_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7393_ctrl_ops,
+			V4L2_CID_HUE, ADV7393_HUE_MIN,
+				      ADV7393_HUE_MAX, 1,
+				      ADV7393_HUE_DEF);
+	v4l2_ctrl_new_std(&state->hdl, &adv7393_ctrl_ops,
+			V4L2_CID_GAIN, ADV7393_GAIN_MIN,
+				       ADV7393_GAIN_MAX, 1,
+				       ADV7393_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 = adv7393_initialize(&state->sd);
+	if (err) {
+		v4l2_ctrl_handler_free(&state->hdl);
+		kfree(state);
+	}
+	return err;
+}
+
+static int adv7393_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7393_state *state = to_state(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_ctrl_handler_free(&state->hdl);
+	kfree(state);
+
+	return 0;
+}
+
+static const struct i2c_device_id adv7393_id[] = {
+	{"adv7393", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, adv7393_id);
+
+static struct i2c_driver adv7393_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "adv7393",
+	},
+	.probe		= adv7393_probe,
+	.remove		= adv7393_remove,
+	.id_table	= adv7393_id,
+};
+module_i2c_driver(adv7393_driver);
diff --git a/drivers/media/video/adv7393_regs.h b/drivers/media/video/adv7393_regs.h
new file mode 100644
index 00000000000..78968330f0b
--- /dev/null
+++ b/drivers/media/video/adv7393_regs.h
@@ -0,0 +1,188 @@
+/*
+ * ADV7393 encoder related structure and register definitions
+ *
+ * Copyright (C) 2010-2012 ADVANSEE - http://www.advansee.com/
+ * Benoît Thébaudeau <benoit.thebaudeau@advansee.com>
+ *
+ * Based on ADV7343 driver,
+ *
+ * Copyright (C) 2009 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.
+ */
+
+#ifndef ADV7393_REGS_H
+#define ADV7393_REGS_H
+
+struct adv7393_std_info {
+	u32 standard_val3;
+	u32 fsc_val;
+	v4l2_std_id stdid;
+};
+
+/* Register offset macros */
+#define ADV7393_POWER_MODE_REG		(0x00)
+#define ADV7393_MODE_SELECT_REG		(0x01)
+#define ADV7393_MODE_REG0		(0x02)
+
+#define ADV7393_DAC123_OUTPUT_LEVEL	(0x0B)
+
+#define ADV7393_SOFT_RESET		(0x17)
+
+#define ADV7393_HD_MODE_REG1		(0x30)
+#define ADV7393_HD_MODE_REG2		(0x31)
+#define ADV7393_HD_MODE_REG3		(0x32)
+#define ADV7393_HD_MODE_REG4		(0x33)
+#define ADV7393_HD_MODE_REG5		(0x34)
+#define ADV7393_HD_MODE_REG6		(0x35)
+
+#define ADV7393_HD_MODE_REG7		(0x39)
+
+#define ADV7393_SD_MODE_REG1		(0x80)
+#define ADV7393_SD_MODE_REG2		(0x82)
+#define ADV7393_SD_MODE_REG3		(0x83)
+#define ADV7393_SD_MODE_REG4		(0x84)
+#define ADV7393_SD_MODE_REG5		(0x86)
+#define ADV7393_SD_MODE_REG6		(0x87)
+#define ADV7393_SD_MODE_REG7		(0x88)
+#define ADV7393_SD_MODE_REG8		(0x89)
+
+#define ADV7393_SD_TIMING_REG0		(0x8A)
+
+#define ADV7393_FSC_REG0		(0x8C)
+#define ADV7393_FSC_REG1		(0x8D)
+#define ADV7393_FSC_REG2		(0x8E)
+#define ADV7393_FSC_REG3		(0x8F)
+
+#define ADV7393_SD_CGMS_WSS0		(0x99)
+
+#define ADV7393_SD_HUE_ADJUST		(0xA0)
+#define ADV7393_SD_BRIGHTNESS_WSS	(0xA1)
+
+/* Default values for the registers */
+#define ADV7393_POWER_MODE_REG_DEFAULT		(0x10)
+#define ADV7393_HD_MODE_REG1_DEFAULT		(0x3C)	/* Changed Default
+							   720p EAV/SAV code*/
+#define ADV7393_HD_MODE_REG2_DEFAULT		(0x01)	/* Changed Pixel data
+							   valid */
+#define ADV7393_HD_MODE_REG3_DEFAULT		(0x00)	/* Color delay 0 clks */
+#define ADV7393_HD_MODE_REG4_DEFAULT		(0xEC)	/* Changed */
+#define ADV7393_HD_MODE_REG5_DEFAULT		(0x08)
+#define ADV7393_HD_MODE_REG6_DEFAULT		(0x00)
+#define ADV7393_HD_MODE_REG7_DEFAULT		(0x00)
+#define ADV7393_SOFT_RESET_DEFAULT		(0x02)
+#define ADV7393_COMPOSITE_POWER_VALUE		(0x10)
+#define ADV7393_COMPONENT_POWER_VALUE		(0x1C)
+#define ADV7393_SVIDEO_POWER_VALUE		(0x0C)
+#define ADV7393_SD_HUE_ADJUST_DEFAULT		(0x80)
+#define ADV7393_SD_BRIGHTNESS_WSS_DEFAULT	(0x00)
+
+#define ADV7393_SD_CGMS_WSS0_DEFAULT		(0x10)
+
+#define ADV7393_SD_MODE_REG1_DEFAULT		(0x10)
+#define ADV7393_SD_MODE_REG2_DEFAULT		(0xC9)
+#define ADV7393_SD_MODE_REG3_DEFAULT		(0x00)
+#define ADV7393_SD_MODE_REG4_DEFAULT		(0x00)
+#define ADV7393_SD_MODE_REG5_DEFAULT		(0x02)
+#define ADV7393_SD_MODE_REG6_DEFAULT		(0x8C)
+#define ADV7393_SD_MODE_REG7_DEFAULT		(0x14)
+#define ADV7393_SD_MODE_REG8_DEFAULT		(0x00)
+
+#define ADV7393_SD_TIMING_REG0_DEFAULT		(0x0C)
+
+/* Bit masks for Mode Select Register */
+#define INPUT_MODE_MASK			(0x70)
+#define SD_INPUT_MODE			(0x00)
+#define HD_720P_INPUT_MODE		(0x10)
+#define HD_1080I_INPUT_MODE		(0x10)
+
+/* Bit masks for Mode Register 0 */
+#define TEST_PATTERN_BLACK_BAR_EN	(0x04)
+#define YUV_OUTPUT_SELECT		(0x20)
+#define RGB_OUTPUT_SELECT		(0xDF)
+
+/* Bit masks for SD brightness/WSS */
+#define SD_BRIGHTNESS_VALUE_MASK	(0x7F)
+#define SD_BLANK_WSS_DATA_MASK		(0x80)
+
+/* Bit masks for soft reset register */
+#define SOFT_RESET			(0x02)
+
+/* Bit masks for HD Mode Register 1 */
+#define OUTPUT_STD_MASK		(0x03)
+#define OUTPUT_STD_SHIFT	(0)
+#define OUTPUT_STD_EIA0_2	(0x00)
+#define OUTPUT_STD_EIA0_1	(0x01)
+#define OUTPUT_STD_FULL		(0x02)
+#define EMBEDDED_SYNC		(0x04)
+#define EXTERNAL_SYNC		(0xFB)
+#define STD_MODE_MASK		(0x1F)
+#define STD_MODE_SHIFT		(3)
+#define STD_MODE_720P		(0x05)
+#define STD_MODE_720P_25	(0x08)
+#define STD_MODE_720P_30	(0x07)
+#define STD_MODE_720P_50	(0x06)
+#define STD_MODE_1080I		(0x0D)
+#define STD_MODE_1080I_25	(0x0E)
+#define STD_MODE_1080P_24	(0x11)
+#define STD_MODE_1080P_25	(0x10)
+#define STD_MODE_1080P_30	(0x0F)
+#define STD_MODE_525P		(0x00)
+#define STD_MODE_625P		(0x03)
+
+/* Bit masks for SD Mode Register 1 */
+#define SD_STD_MASK		(0x03)
+#define SD_STD_NTSC		(0x00)
+#define SD_STD_PAL_BDGHI	(0x01)
+#define SD_STD_PAL_M		(0x02)
+#define SD_STD_PAL_N		(0x03)
+#define SD_LUMA_FLTR_MASK	(0x07)
+#define SD_LUMA_FLTR_SHIFT	(2)
+#define SD_CHROMA_FLTR_MASK	(0x07)
+#define SD_CHROMA_FLTR_SHIFT	(5)
+
+/* Bit masks for SD Mode Register 2 */
+#define SD_PRPB_SSAF_EN		(0x01)
+#define SD_PRPB_SSAF_DI		(0xFE)
+#define SD_DAC_OUT1_EN		(0x02)
+#define SD_DAC_OUT1_DI		(0xFD)
+#define SD_PEDESTAL_EN		(0x08)
+#define SD_PEDESTAL_DI		(0xF7)
+#define SD_SQUARE_PIXEL_EN	(0x10)
+#define SD_SQUARE_PIXEL_DI	(0xEF)
+#define SD_PIXEL_DATA_VALID	(0x40)
+#define SD_ACTIVE_EDGE_EN	(0x80)
+#define SD_ACTIVE_EDGE_DI	(0x7F)
+
+/* Bit masks for HD Mode Register 6 */
+#define HD_PRPB_SYNC_EN		(0x04)
+#define HD_PRPB_SYNC_DI		(0xFB)
+#define HD_DAC_SWAP_EN		(0x08)
+#define HD_DAC_SWAP_DI		(0xF7)
+#define HD_GAMMA_CURVE_A	(0xEF)
+#define HD_GAMMA_CURVE_B	(0x10)
+#define HD_GAMMA_EN		(0x20)
+#define HD_GAMMA_DI		(0xDF)
+#define HD_ADPT_FLTR_MODEA	(0xBF)
+#define HD_ADPT_FLTR_MODEB	(0x40)
+#define HD_ADPT_FLTR_EN		(0x80)
+#define HD_ADPT_FLTR_DI		(0x7F)
+
+#define ADV7393_BRIGHTNESS_MAX	(63)
+#define ADV7393_BRIGHTNESS_MIN	(-64)
+#define ADV7393_BRIGHTNESS_DEF	(0)
+#define ADV7393_HUE_MAX		(127)
+#define ADV7393_HUE_MIN		(-128)
+#define ADV7393_HUE_DEF		(0)
+#define ADV7393_GAIN_MAX	(64)
+#define ADV7393_GAIN_MIN	(-64)
+#define ADV7393_GAIN_DEF	(0)
+
+#endif
diff --git a/drivers/media/video/bt8xx/bttv-cards.c b/drivers/media/video/bt8xx/bttv-cards.c
index 856ab962cd6..38952faaffd 100644
--- a/drivers/media/video/bt8xx/bttv-cards.c
+++ b/drivers/media/video/bt8xx/bttv-cards.c
@@ -345,7 +345,7 @@ static struct CARD {
 	{ 0x15401836, BTTV_BOARD_PV183,         "Provideo PV183-7" },
 	{ 0x15401837, BTTV_BOARD_PV183,         "Provideo PV183-8" },
 	{ 0x3116f200, BTTV_BOARD_TVT_TD3116,	"Tongwei Video Technology TD-3116" },
-
+	{ 0x02280279, BTTV_BOARD_APOSONIC_WDVR, "Aposonic W-DVR" },
 	{ 0, -1, NULL }
 };
 
@@ -676,6 +676,7 @@ struct tvcard bttv_tvcards[] = {
 		.tuner_type	= UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.has_remote     = 1,
+		.has_radio	= 1,  /* not every card has radio */
 	},
 	[BTTV_BOARD_VOBIS_BOOSTAR] = {
 		.name           = "Terratec TerraTV+ Version 1.0 (Bt848)/ Terra TValue Version 1.0/ Vobis TV-Boostar",
@@ -2817,6 +2818,14 @@ struct tvcard bttv_tvcards[] = {
 		.pll		= PLL_28,
 		.tuner_type     = TUNER_ABSENT,
 	},
+	[BTTV_BOARD_APOSONIC_WDVR] = {
+		.name           = "Aposonic W-DVR",
+		.video_inputs   = 4,
+		.svhs           = NO_SVHS,
+		.muxsel         = MUXSEL(2, 3, 1, 0),
+		.tuner_type     = TUNER_ABSENT,
+	},
+
 };
 
 static const unsigned int bttv_num_tvcards = ARRAY_SIZE(bttv_tvcards);
diff --git a/drivers/media/video/bt8xx/bttv-driver.c b/drivers/media/video/bt8xx/bttv-driver.c
index ff7a589d8e0..b58ff87db77 100644
--- a/drivers/media/video/bt8xx/bttv-driver.c
+++ b/drivers/media/video/bt8xx/bttv-driver.c
@@ -558,12 +558,6 @@ static const struct bttv_format formats[] = {
 		.depth    = 16,
 		.flags    = FORMAT_FLAGS_PACKED,
 	},{
-		.name     = "4:2:2, packed, YUYV",
-		.fourcc   = V4L2_PIX_FMT_YUYV,
-		.btformat = BT848_COLOR_FMT_YUY2,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	},{
 		.name     = "4:2:2, packed, UYVY",
 		.fourcc   = V4L2_PIX_FMT_UYVY,
 		.btformat = BT848_COLOR_FMT_YUY2,
diff --git a/drivers/media/video/bt8xx/bttv.h b/drivers/media/video/bt8xx/bttv.h
index acfe2f3b92d..79a11240a59 100644
--- a/drivers/media/video/bt8xx/bttv.h
+++ b/drivers/media/video/bt8xx/bttv.h
@@ -184,7 +184,7 @@
 #define BTTV_BOARD_GEOVISION_GV800S_SL	   0x9e
 #define BTTV_BOARD_PV183                   0x9f
 #define BTTV_BOARD_TVT_TD3116		   0xa0
-
+#define BTTV_BOARD_APOSONIC_WDVR           0xa1
 
 /* more card-specific defines */
 #define PT2254_L_CHANNEL 0x10
diff --git a/drivers/media/video/cpia2/cpia2_v4l.c b/drivers/media/video/cpia2/cpia2_v4l.c
index 55e92902a76..a62a7b73999 100644
--- a/drivers/media/video/cpia2/cpia2_v4l.c
+++ b/drivers/media/video/cpia2/cpia2_v4l.c
@@ -932,7 +932,7 @@ static int cpia2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 	buf->sequence = cam->buffers[buf->index].seq;
 	buf->m.offset = cam->buffers[buf->index].data - cam->frame_buffer;
 	buf->length = cam->frame_size;
-	buf->input = 0;
+	buf->reserved2 = 0;
 	buf->reserved = 0;
 	memset(&buf->timecode, 0, sizeof(buf->timecode));
 
diff --git a/drivers/media/video/cs8420.h b/drivers/media/video/cs8420.h
deleted file mode 100644
index 621c0c6678e..00000000000
--- a/drivers/media/video/cs8420.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/* cs8420.h - cs8420 initializations
-   Copyright (C) 1999 Nathan Laredo (laredo@gnu.org)
-
-   This program is free software; you can redistribute it and/or modify
-   it under the terms of the GNU General Public License as published by
-   the Free Software Foundation; either version 2 of the License, or
-   (at your option) any later version.
-
-   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
-
-   You 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 __CS8420_H__
-#define __CS8420_H__
-
-/* Initialization Sequence */
-
-static __u8 init8420[] = {
-	1, 0x01,	2, 0x02,	3, 0x00,	4, 0x46,
-	5, 0x24,	6, 0x84,	18, 0x18,	19, 0x13,
-};
-
-#define INIT8420LEN	(sizeof(init8420)/2)
-
-static __u8 mode8420pro[] = {	/* professional output mode */
-	32, 0xa1,	33, 0x00,	34, 0x00,	35, 0x00,
-	36, 0x00,	37, 0x00,	38, 0x00,	39, 0x00,
-	40, 0x00,	41, 0x00,	42, 0x00,	43, 0x00,
-	44, 0x00,	45, 0x00,	46, 0x00,	47, 0x00,
-	48, 0x00,	49, 0x00,	50, 0x00,	51, 0x00,
-	52, 0x00,	53, 0x00,	54, 0x00,	55, 0x00,
-};
-#define MODE8420LEN	(sizeof(mode8420pro)/2)
-
-static __u8 mode8420con[] = {	/* consumer output mode */
-	32, 0x20,	33, 0x00,	34, 0x00,	35, 0x48,
-	36, 0x00,	37, 0x00,	38, 0x00,	39, 0x00,
-	40, 0x00,	41, 0x00,	42, 0x00,	43, 0x00,
-	44, 0x00,	45, 0x00,	46, 0x00,	47, 0x00,
-	48, 0x00,	49, 0x00,	50, 0x00,	51, 0x00,
-	52, 0x00,	53, 0x00,	54, 0x00,	55, 0x00,
-};
-
-#endif
diff --git a/drivers/media/video/cx18/cx18-ioctl.c b/drivers/media/video/cx18/cx18-ioctl.c
index 35fde4e931f..e9912db3b49 100644
--- a/drivers/media/video/cx18/cx18-ioctl.c
+++ b/drivers/media/video/cx18/cx18-ioctl.c
@@ -1142,24 +1142,6 @@ static long cx18_default(struct file *file, void *fh, bool valid_prio,
 	return 0;
 }
 
-long cx18_v4l2_ioctl(struct file *filp, unsigned int cmd,
-		    unsigned long arg)
-{
-	struct video_device *vfd = video_devdata(filp);
-	struct cx18_open_id *id = file2id(filp);
-	struct cx18 *cx = id->cx;
-	long res;
-
-	mutex_lock(&cx->serialize_lock);
-
-	if (cx18_debug & CX18_DBGFLG_IOCTL)
-		vfd->debug = V4L2_DEBUG_IOCTL | V4L2_DEBUG_IOCTL_ARG;
-	res = video_ioctl2(filp, cmd, arg);
-	vfd->debug = 0;
-	mutex_unlock(&cx->serialize_lock);
-	return res;
-}
-
 static const struct v4l2_ioctl_ops cx18_ioctl_ops = {
 	.vidioc_querycap                = cx18_querycap,
 	.vidioc_s_audio                 = cx18_s_audio,
diff --git a/drivers/media/video/cx18/cx18-ioctl.h b/drivers/media/video/cx18/cx18-ioctl.h
index dcb2559ad52..2f9dd591ee0 100644
--- a/drivers/media/video/cx18/cx18-ioctl.h
+++ b/drivers/media/video/cx18/cx18-ioctl.h
@@ -29,5 +29,3 @@ void cx18_set_funcs(struct video_device *vdev);
 int cx18_s_std(struct file *file, void *fh, v4l2_std_id *std);
 int cx18_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
 int cx18_s_input(struct file *file, void *fh, unsigned int inp);
-long cx18_v4l2_ioctl(struct file *filp, unsigned int cmd,
-		    unsigned long arg);
diff --git a/drivers/media/video/cx18/cx18-streams.c b/drivers/media/video/cx18/cx18-streams.c
index 4185bcb80ca..9d598ab8861 100644
--- a/drivers/media/video/cx18/cx18-streams.c
+++ b/drivers/media/video/cx18/cx18-streams.c
@@ -40,8 +40,7 @@ static struct v4l2_file_operations cx18_v4l2_enc_fops = {
 	.owner = THIS_MODULE,
 	.read = cx18_v4l2_read,
 	.open = cx18_v4l2_open,
-	/* FIXME change to video_ioctl2 if serialization lock can be removed */
-	.unlocked_ioctl = cx18_v4l2_ioctl,
+	.unlocked_ioctl = video_ioctl2,
 	.release = cx18_v4l2_close,
 	.poll = cx18_v4l2_enc_poll,
 	.mmap = cx18_v4l2_mmap,
@@ -376,6 +375,7 @@ static int cx18_prep_dev(struct cx18 *cx, int type)
 	s->video_dev->fops = &cx18_v4l2_enc_fops;
 	s->video_dev->release = video_device_release;
 	s->video_dev->tvnorms = V4L2_STD_ALL;
+	s->video_dev->lock = &cx->serialize_lock;
 	set_bit(V4L2_FL_USE_FH_PRIO, &s->video_dev->flags);
 	cx18_set_funcs(s->video_dev);
 	return 0;
diff --git a/drivers/media/video/cx231xx/cx231xx-avcore.c b/drivers/media/video/cx231xx/cx231xx-avcore.c
index b085a3c6dc0..447148eff95 100644
--- a/drivers/media/video/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/video/cx231xx/cx231xx-avcore.c
@@ -89,7 +89,7 @@ void initGPIO(struct cx231xx *dev)
 	verve_read_byte(dev, 0x07, &val);
 	cx231xx_info(" verve_read_byte address0x07=0x%x\n", val);
 
-	cx231xx_capture_start(dev, 1, 2);
+	cx231xx_capture_start(dev, 1, Vbi);
 
 	cx231xx_mode_register(dev, EP_MODE_SET, 0x0500FE00);
 	cx231xx_mode_register(dev, GBULK_BIT_EN, 0xFFFDFFFF);
@@ -99,7 +99,7 @@ void uninitGPIO(struct cx231xx *dev)
 {
 	u8 value[4] = { 0, 0, 0, 0 };
 
-	cx231xx_capture_start(dev, 0, 2);
+	cx231xx_capture_start(dev, 0, Vbi);
 	verve_write_byte(dev, 0x07, 0x14);
 	cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
 			0x68, value, 4);
@@ -2516,29 +2516,29 @@ int cx231xx_initialize_stream_xfer(struct cx231xx *dev, u32 media_type)
 
 	if (dev->udev->speed == USB_SPEED_HIGH) {
 		switch (media_type) {
-		case 81: /* audio */
+		case Audio:
 			cx231xx_info("%s: Audio enter HANC\n", __func__);
 			status =
 			    cx231xx_mode_register(dev, TS_MODE_REG, 0x9300);
 			break;
 
-		case 2:	/* vbi */
+		case Vbi:
 			cx231xx_info("%s: set vanc registers\n", __func__);
 			status = cx231xx_mode_register(dev, TS_MODE_REG, 0x300);
 			break;
 
-		case 3:	/* sliced cc */
+		case Sliced_cc:
 			cx231xx_info("%s: set hanc registers\n", __func__);
 			status =
 			    cx231xx_mode_register(dev, TS_MODE_REG, 0x1300);
 			break;
 
-		case 0:	/* video */
+		case Raw_Video:
 			cx231xx_info("%s: set video registers\n", __func__);
 			status = cx231xx_mode_register(dev, TS_MODE_REG, 0x100);
 			break;
 
-		case 4:	/* ts1 */
+		case TS1_serial_mode:
 			cx231xx_info("%s: set ts1 registers", __func__);
 
 		if (dev->board.has_417) {
@@ -2569,7 +2569,7 @@ int cx231xx_initialize_stream_xfer(struct cx231xx *dev, u32 media_type)
 		}
 			break;
 
-		case 6:	/* ts1 parallel mode */
+		case TS1_parallel_mode:
 			cx231xx_info("%s: set ts1 parallel mode registers\n",
 				     __func__);
 			status = cx231xx_mode_register(dev, TS_MODE_REG, 0x100);
@@ -2592,52 +2592,28 @@ int cx231xx_capture_start(struct cx231xx *dev, int start, u8 media_type)
 	/* get EP for media type */
 	pcb_config = (struct pcb_config *)&dev->current_pcb_config;
 
-	if (pcb_config->config_num == 1) {
+	if (pcb_config->config_num) {
 		switch (media_type) {
-		case 0:	/* Video */
+		case Raw_Video:
 			ep_mask = ENABLE_EP4;	/* ep4  [00:1000] */
 			break;
-		case 1:	/* Audio */
+		case Audio:
 			ep_mask = ENABLE_EP3;	/* ep3  [00:0100] */
 			break;
-		case 2:	/* Vbi */
+		case Vbi:
 			ep_mask = ENABLE_EP5;	/* ep5 [01:0000] */
 			break;
-		case 3:	/* Sliced_cc */
+		case Sliced_cc:
 			ep_mask = ENABLE_EP6;	/* ep6 [10:0000] */
 			break;
-		case 4:	/* ts1 */
-		case 6:	/* ts1 parallel mode */
+		case TS1_serial_mode:
+		case TS1_parallel_mode:
 			ep_mask = ENABLE_EP1;	/* ep1 [00:0001] */
 			break;
-		case 5:	/* ts2 */
+		case TS2:
 			ep_mask = ENABLE_EP2;	/* ep2 [00:0010] */
 			break;
 		}
-
-	} else if (pcb_config->config_num > 1) {
-		switch (media_type) {
-		case 0:	/* Video */
-			ep_mask = ENABLE_EP4;	/* ep4  [00:1000] */
-			break;
-		case 1:	/* Audio */
-			ep_mask = ENABLE_EP3;	/* ep3  [00:0100] */
-			break;
-		case 2:	/* Vbi */
-			ep_mask = ENABLE_EP5;	/* ep5 [01:0000] */
-			break;
-		case 3:	/* Sliced_cc */
-			ep_mask = ENABLE_EP6;	/* ep6 [10:0000] */
-			break;
-		case 4:	/* ts1 */
-		case 6:	/* ts1 parallel mode */
-			ep_mask = ENABLE_EP1;	/* ep1 [00:0001] */
-			break;
-		case 5:	/* ts2 */
-			ep_mask = ENABLE_EP2;	/* ep2 [00:0010] */
-			break;
-		}
-
 	}
 
 	if (start) {
diff --git a/drivers/media/video/cx231xx/cx231xx-cards.c b/drivers/media/video/cx231xx/cx231xx-cards.c
index 8ed460d692e..02d4d36735d 100644
--- a/drivers/media/video/cx231xx/cx231xx-cards.c
+++ b/drivers/media/video/cx231xx/cx231xx-cards.c
@@ -1023,7 +1023,6 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 	int nr = 0, ifnum;
 	int i, isoc_pipe = 0;
 	char *speed;
-	char descr[255] = "";
 	struct usb_interface_assoc_descriptor *assoc_desc;
 
 	udev = usb_get_dev(interface_to_usbdev(interface));
@@ -1098,20 +1097,10 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 		speed = "unknown";
 	}
 
-	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 (*descr)
-		strlcat(descr, " ", sizeof(descr));
-
-	cx231xx_info("New device %s@ %s Mbps "
+	cx231xx_info("New device %s %s @ %s Mbps "
 	     "(%04x:%04x) with %d interfaces\n",
-	     descr,
+	     udev->manufacturer ? udev->manufacturer : "",
+	     udev->product ? udev->product : "",
 	     speed,
 	     le16_to_cpu(udev->descriptor.idVendor),
 	     le16_to_cpu(udev->descriptor.idProduct),
diff --git a/drivers/media/video/cx231xx/cx231xx-i2c.c b/drivers/media/video/cx231xx/cx231xx-i2c.c
index 925f3a04e53..781feed406f 100644
--- a/drivers/media/video/cx231xx/cx231xx-i2c.c
+++ b/drivers/media/video/cx231xx/cx231xx-i2c.c
@@ -499,16 +499,12 @@ int cx231xx_i2c_register(struct cx231xx_i2c *bus)
 
 	BUG_ON(!dev->cx231xx_send_usb_command);
 
-	memcpy(&bus->i2c_adap, &cx231xx_adap_template, sizeof(bus->i2c_adap));
-	memcpy(&bus->i2c_algo, &cx231xx_algo, sizeof(bus->i2c_algo));
-	memcpy(&bus->i2c_client, &cx231xx_client_template,
-	       sizeof(bus->i2c_client));
-
+	bus->i2c_adap = cx231xx_adap_template;
+	bus->i2c_client = cx231xx_client_template;
 	bus->i2c_adap.dev.parent = &dev->udev->dev;
 
 	strlcpy(bus->i2c_adap.name, bus->dev->name, sizeof(bus->i2c_adap.name));
 
-	bus->i2c_algo.data = bus;
 	bus->i2c_adap.algo_data = bus;
 	i2c_set_adapdata(&bus->i2c_adap, &dev->v4l2_dev);
 	i2c_add_adapter(&bus->i2c_adap);
diff --git a/drivers/media/video/cx231xx/cx231xx.h b/drivers/media/video/cx231xx/cx231xx.h
index e17447554a0..a89d020de94 100644
--- a/drivers/media/video/cx231xx/cx231xx.h
+++ b/drivers/media/video/cx231xx/cx231xx.h
@@ -26,7 +26,6 @@
 #include <linux/types.h>
 #include <linux/ioctl.h>
 #include <linux/i2c.h>
-#include <linux/i2c-algo-bit.h>
 #include <linux/workqueue.h>
 #include <linux/mutex.h>
 
@@ -481,7 +480,6 @@ struct cx231xx_i2c {
 
 	/* i2c i/o */
 	struct i2c_adapter i2c_adap;
-	struct i2c_algo_bit_data i2c_algo;
 	struct i2c_client i2c_client;
 	u32 i2c_rc;
 
diff --git a/drivers/media/video/cx23885/cx23885-i2c.c b/drivers/media/video/cx23885/cx23885-i2c.c
index be1e21d8295..4887314339c 100644
--- a/drivers/media/video/cx23885/cx23885-i2c.c
+++ b/drivers/media/video/cx23885/cx23885-i2c.c
@@ -316,19 +316,13 @@ int cx23885_i2c_register(struct cx23885_i2c *bus)
 
 	dprintk(1, "%s(bus = %d)\n", __func__, bus->nr);
 
-	memcpy(&bus->i2c_adap, &cx23885_i2c_adap_template,
-	       sizeof(bus->i2c_adap));
-	memcpy(&bus->i2c_algo, &cx23885_i2c_algo_template,
-	       sizeof(bus->i2c_algo));
-	memcpy(&bus->i2c_client, &cx23885_i2c_client_template,
-	       sizeof(bus->i2c_client));
-
+	bus->i2c_adap = cx23885_i2c_adap_template;
+	bus->i2c_client = cx23885_i2c_client_template;
 	bus->i2c_adap.dev.parent = &dev->pci->dev;
 
 	strlcpy(bus->i2c_adap.name, bus->dev->name,
 		sizeof(bus->i2c_adap.name));
 
-	bus->i2c_algo.data = bus;
 	bus->i2c_adap.algo_data = bus;
 	i2c_set_adapdata(&bus->i2c_adap, &dev->v4l2_dev);
 	i2c_add_adapter(&bus->i2c_adap);
diff --git a/drivers/media/video/cx23885/cx23885.h b/drivers/media/video/cx23885/cx23885.h
index 13c37ec07ae..5d560c747e0 100644
--- a/drivers/media/video/cx23885/cx23885.h
+++ b/drivers/media/video/cx23885/cx23885.h
@@ -21,7 +21,6 @@
 
 #include <linux/pci.h>
 #include <linux/i2c.h>
-#include <linux/i2c-algo-bit.h>
 #include <linux/kdev_t.h>
 #include <linux/slab.h>
 
@@ -247,7 +246,6 @@ struct cx23885_i2c {
 
 	/* i2c i/o */
 	struct i2c_adapter         i2c_adap;
-	struct i2c_algo_bit_data   i2c_algo;
 	struct i2c_client          i2c_client;
 	u32                        i2c_rc;
 
diff --git a/drivers/media/video/cx25821/cx25821-core.c b/drivers/media/video/cx25821/cx25821-core.c
index 83c1aa6b2e6..f11f6f07e91 100644
--- a/drivers/media/video/cx25821/cx25821-core.c
+++ b/drivers/media/video/cx25821/cx25821-core.c
@@ -904,9 +904,6 @@ static int cx25821_dev_setup(struct cx25821_dev *dev)
 	list_add_tail(&dev->devlist, &cx25821_devlist);
 	mutex_unlock(&cx25821_devlist_mutex);
 
-	strcpy(cx25821_boards[UNKNOWN_BOARD].name, "unknown");
-	strcpy(cx25821_boards[CX25821_BOARD].name, "cx25821");
-
 	if (dev->pci->device != 0x8210) {
 		pr_info("%s(): Exiting. Incorrect Hardware device = 0x%02x\n",
 			__func__, dev->pci->device);
diff --git a/drivers/media/video/cx25821/cx25821-i2c.c b/drivers/media/video/cx25821/cx25821-i2c.c
index 6311180f430..9844549764c 100644
--- a/drivers/media/video/cx25821/cx25821-i2c.c
+++ b/drivers/media/video/cx25821/cx25821-i2c.c
@@ -305,18 +305,12 @@ int cx25821_i2c_register(struct cx25821_i2c *bus)
 
 	dprintk(1, "%s(bus = %d)\n", __func__, bus->nr);
 
-	memcpy(&bus->i2c_adap, &cx25821_i2c_adap_template,
-	       sizeof(bus->i2c_adap));
-	memcpy(&bus->i2c_algo, &cx25821_i2c_algo_template,
-	       sizeof(bus->i2c_algo));
-	memcpy(&bus->i2c_client, &cx25821_i2c_client_template,
-	       sizeof(bus->i2c_client));
-
+	bus->i2c_adap = cx25821_i2c_adap_template;
+	bus->i2c_client = cx25821_i2c_client_template;
 	bus->i2c_adap.dev.parent = &dev->pci->dev;
 
 	strlcpy(bus->i2c_adap.name, bus->dev->name, sizeof(bus->i2c_adap.name));
 
-	bus->i2c_algo.data = bus;
 	bus->i2c_adap.algo_data = bus;
 	i2c_set_adapdata(&bus->i2c_adap, &dev->v4l2_dev);
 	i2c_add_adapter(&bus->i2c_adap);
diff --git a/drivers/media/video/cx25821/cx25821-medusa-video.c b/drivers/media/video/cx25821/cx25821-medusa-video.c
index 313fb20a0b4..6a92e5c70c2 100644
--- a/drivers/media/video/cx25821/cx25821-medusa-video.c
+++ b/drivers/media/video/cx25821/cx25821-medusa-video.c
@@ -499,7 +499,7 @@ static void medusa_set_decoderduration(struct cx25821_dev *dev, int decoder,
 	mutex_lock(&dev->lock);
 
 	/* no support */
-	if (decoder < VDEC_A && decoder > VDEC_H) {
+	if (decoder < VDEC_A || decoder > VDEC_H) {
 		mutex_unlock(&dev->lock);
 		return;
 	}
diff --git a/drivers/media/video/cx25821/cx25821.h b/drivers/media/video/cx25821/cx25821.h
index b9aa801b00a..8a9c0c86941 100644
--- a/drivers/media/video/cx25821/cx25821.h
+++ b/drivers/media/video/cx25821/cx25821.h
@@ -26,7 +26,6 @@
 
 #include <linux/pci.h>
 #include <linux/i2c.h>
-#include <linux/i2c-algo-bit.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/sched.h>
@@ -187,7 +186,7 @@ enum port {
 };
 
 struct cx25821_board {
-	char *name;
+	const char *name;
 	enum port porta;
 	enum port portb;
 	enum port portc;
@@ -213,7 +212,6 @@ struct cx25821_i2c {
 
 	/* i2c i/o */
 	struct i2c_adapter i2c_adap;
-	struct i2c_algo_bit_data i2c_algo;
 	struct i2c_client i2c_client;
 	u32 i2c_rc;
 
diff --git a/drivers/media/video/cx88/cx88-alsa.c b/drivers/media/video/cx88/cx88-alsa.c
index 04bf6627d36..dfac6e34859 100644
--- a/drivers/media/video/cx88/cx88-alsa.c
+++ b/drivers/media/video/cx88/cx88-alsa.c
@@ -585,13 +585,10 @@ static void snd_cx88_wm8775_volume_put(struct snd_kcontrol *kcontrol,
 {
 	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;
@@ -600,13 +597,8 @@ static void snd_cx88_wm8775_volume_put(struct snd_kcontrol *kcontrol,
 		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);
+	wm8775_s_ctrl(core, V4L2_CID_AUDIO_VOLUME, v);
+	wm8775_s_ctrl(core, V4L2_CID_AUDIO_BALANCE, b);
 }
 
 /* OK - TODO: test it */
@@ -687,14 +679,8 @@ static int snd_cx88_switch_put(struct snd_kcontrol *kcontrol,
 		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);
-		}
+		    ((1<<6) == bit))
+			wm8775_s_ctrl(core, V4L2_CID_AUDIO_MUTE, 0 != (vol & bit));
 		ret = 1;
 	}
 	spin_unlock_irq(&chip->reg_lock);
@@ -724,13 +710,10 @@ static int snd_cx88_alc_get(struct snd_kcontrol *kcontrol,
 {
 	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;
+	s32 val;
 
+	val = wm8775_g_ctrl(core, V4L2_CID_AUDIO_LOUDNESS);
+	value->value.integer.value[0] = val ? 1 : 0;
 	return 0;
 }
 
diff --git a/drivers/media/video/cx88/cx88-blackbird.c b/drivers/media/video/cx88/cx88-blackbird.c
index ed7b2aa1ed8..843ffd9e533 100644
--- a/drivers/media/video/cx88/cx88-blackbird.c
+++ b/drivers/media/video/cx88/cx88-blackbird.c
@@ -35,6 +35,7 @@
 #include <linux/firmware.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include <media/cx2341x.h>
 
 #include "cx88.h"
@@ -523,11 +524,10 @@ static void blackbird_codec_settings(struct cx8802_dev *dev)
 	blackbird_api_cmd(dev, CX2341X_ENC_SET_FRAME_SIZE, 2, 0,
 				dev->height, dev->width);
 
-	dev->params.width = dev->width;
-	dev->params.height = dev->height;
-	dev->params.is_50hz = (dev->core->tvnorm & V4L2_STD_625_50) != 0;
-
-	cx2341x_update(dev, blackbird_mbox_func, NULL, &dev->params);
+	dev->cxhdl.width = dev->width;
+	dev->cxhdl.height = dev->height;
+	cx2341x_handler_set_50hz(&dev->cxhdl, dev->core->tvnorm & V4L2_STD_625_50);
+	cx2341x_handler_setup(&dev->cxhdl);
 }
 
 static int blackbird_initialize_codec(struct cx8802_dev *dev)
@@ -618,6 +618,8 @@ static int blackbird_start_codec(struct file *file, void *priv)
 	/* initialize the video input */
 	blackbird_api_cmd(dev, CX2341X_ENC_INITIALIZE_INPUT, 0, 0);
 
+	cx2341x_handler_set_busy(&dev->cxhdl, 1);
+
 	/* start capturing to the host interface */
 	blackbird_api_cmd(dev, CX2341X_ENC_START_CAPTURE, 2, 0,
 			BLACKBIRD_MPEG_CAPTURE,
@@ -636,6 +638,8 @@ static int blackbird_stop_codec(struct cx8802_dev *dev)
 			BLACKBIRD_RAW_BITS_NONE
 		);
 
+	cx2341x_handler_set_busy(&dev->cxhdl, 0);
+
 	dev->mpeg_active = 0;
 	return 0;
 }
@@ -685,58 +689,15 @@ static struct videobuf_queue_ops blackbird_qops = {
 
 /* ------------------------------------------------------------------ */
 
-static const u32 *ctrl_classes[] = {
-	cx88_user_ctrls,
-	cx2341x_mpeg_ctrls,
-	NULL
-};
-
-static int blackbird_queryctrl(struct cx8802_dev *dev, struct v4l2_queryctrl *qctrl)
-{
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (qctrl->id == 0)
-		return -EINVAL;
-
-	/* Standard V4L2 controls */
-	if (cx8800_ctrl_query(dev->core, qctrl) == 0)
-		return 0;
-
-	/* MPEG V4L2 controls */
-	if (cx2341x_ctrl_query(&dev->params, qctrl))
-		qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
-	return 0;
-}
-
-/* ------------------------------------------------------------------ */
-/* IOCTL Handlers                                                     */
-
-static int vidioc_querymenu (struct file *file, void *priv,
-				struct v4l2_querymenu *qmenu)
-{
-	struct cx8802_dev *dev  = ((struct cx8802_fh *)priv)->dev;
-	struct v4l2_queryctrl qctrl;
-
-	qctrl.id = qmenu->id;
-	blackbird_queryctrl(dev, &qctrl);
-	return v4l2_ctrl_query_menu(qmenu, &qctrl,
-			cx2341x_ctrl_get_menu(&dev->params, qmenu->id));
-}
-
-static int vidioc_querycap (struct file *file, void  *priv,
+static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
 	struct cx8802_dev *dev  = ((struct cx8802_fh *)priv)->dev;
 	struct cx88_core  *core = dev->core;
 
 	strcpy(cap->driver, "cx88_blackbird");
-	strlcpy(cap->card, core->board.name, sizeof(cap->card));
-	sprintf(cap->bus_info,"PCI:%s",pci_name(dev->pci));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_READWRITE     |
-		V4L2_CAP_STREAMING;
-	if (UNSET != core->board.tuner_type)
-		cap->capabilities |= V4L2_CAP_TUNER;
+	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
+	cx88_querycap(file, core, cap);
 	return 0;
 }
 
@@ -748,6 +709,7 @@ static int vidioc_enum_fmt_vid_cap (struct file *file, void  *priv,
 
 	strlcpy(f->description, "MPEG", sizeof(f->description));
 	f->pixelformat = V4L2_PIX_FMT_MPEG;
+	f->flags = V4L2_FMT_FLAG_COMPRESSED;
 	return 0;
 }
 
@@ -759,12 +721,12 @@ static int vidioc_g_fmt_vid_cap (struct file *file, void *priv,
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    = dev->ts_packet_size * dev->ts_packet_count; /* 188 * 4 * 1024; */
-	f->fmt.pix.colorspace   = 0;
+	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 	f->fmt.pix.width        = dev->width;
 	f->fmt.pix.height       = dev->height;
 	f->fmt.pix.field        = fh->mpegq.field;
-	dprintk(0,"VIDIOC_G_FMT: w: %d, h: %d, f: %d\n",
+	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d, f: %d\n",
 		dev->width, dev->height, fh->mpegq.field );
 	return 0;
 }
@@ -777,9 +739,9 @@ static int vidioc_try_fmt_vid_cap (struct file *file, void *priv,
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    = dev->ts_packet_size * dev->ts_packet_count; /* 188 * 4 * 1024; */;
-	f->fmt.pix.colorspace   = 0;
-	dprintk(0,"VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
+	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
+	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
 		dev->width, dev->height, fh->mpegq.field );
 	return 0;
 }
@@ -793,15 +755,15 @@ static int vidioc_s_fmt_vid_cap (struct file *file, void *priv,
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    = dev->ts_packet_size * dev->ts_packet_count; /* 188 * 4 * 1024; */;
-	f->fmt.pix.colorspace   = 0;
+	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 	dev->width              = f->fmt.pix.width;
 	dev->height             = f->fmt.pix.height;
 	fh->mpegq.field         = f->fmt.pix.field;
 	cx88_set_scale(core, f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.field);
 	blackbird_api_cmd(dev, CX2341X_ENC_SET_FRAME_SIZE, 2, 0,
 				f->fmt.pix.height, f->fmt.pix.width);
-	dprintk(0,"VIDIOC_S_FMT: w: %d, h: %d, f: %d\n",
+	dprintk(1, "VIDIOC_S_FMT: w: %d, h: %d, f: %d\n",
 		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.field );
 	return 0;
 }
@@ -834,60 +796,21 @@ static int vidioc_dqbuf (struct file *file, void *priv, struct v4l2_buffer *p)
 static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
 {
 	struct cx8802_fh  *fh   = priv;
+	struct cx8802_dev *dev  = fh->dev;
+
+	if (!dev->mpeg_active)
+		blackbird_start_codec(file, fh);
 	return videobuf_streamon(&fh->mpegq);
 }
 
 static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 {
 	struct cx8802_fh  *fh   = priv;
-	return videobuf_streamoff(&fh->mpegq);
-}
-
-static int vidioc_g_ext_ctrls (struct file *file, void *priv,
-			       struct v4l2_ext_controls *f)
-{
-	struct cx8802_dev *dev  = ((struct cx8802_fh *)priv)->dev;
-
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	return cx2341x_ext_ctrls(&dev->params, 0, f, VIDIOC_G_EXT_CTRLS);
-}
-
-static int vidioc_s_ext_ctrls (struct file *file, void *priv,
-			       struct v4l2_ext_controls *f)
-{
-	struct cx8802_dev *dev  = ((struct cx8802_fh *)priv)->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
+	struct cx8802_dev *dev  = fh->dev;
 
 	if (dev->mpeg_active)
 		blackbird_stop_codec(dev);
-
-	p = dev->params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_S_EXT_CTRLS);
-	if (!err) {
-		err = cx2341x_update(dev, blackbird_mbox_func, &dev->params, &p);
-		dev->params = p;
-	}
-	return err;
-}
-
-static int vidioc_try_ext_ctrls (struct file *file, void *priv,
-			       struct v4l2_ext_controls *f)
-{
-	struct cx8802_dev *dev  = ((struct cx8802_fh *)priv)->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	p = dev->params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_TRY_EXT_CTRLS);
-
-	return err;
+	return videobuf_streamoff(&fh->mpegq);
 }
 
 static int vidioc_s_frequency (struct file *file, void *priv,
@@ -897,6 +820,10 @@ static int vidioc_s_frequency (struct file *file, void *priv,
 	struct cx8802_dev *dev  = fh->dev;
 	struct cx88_core  *core = dev->core;
 
+	if (unlikely(UNSET == core->board.tuner_type))
+		return -EINVAL;
+	if (unlikely(f->tuner != 0))
+		return -EINVAL;
 	if (dev->mpeg_active)
 		blackbird_stop_codec(dev);
 
@@ -914,29 +841,11 @@ static int vidioc_log_status (struct file *file, void *priv)
 	char name[32 + 2];
 
 	snprintf(name, sizeof(name), "%s/2", core->name);
-	printk("%s/2: ============  START LOG STATUS  ============\n",
-		core->name);
 	call_all(core, core, log_status);
-	cx2341x_log_status(&dev->params, name);
-	printk("%s/2: =============  END LOG STATUS  =============\n",
-		core->name);
+	v4l2_ctrl_handler_log_status(&dev->cxhdl.hdl, name);
 	return 0;
 }
 
-static int vidioc_queryctrl (struct file *file, void *priv,
-				struct v4l2_queryctrl *qctrl)
-{
-	struct cx8802_dev *dev  = ((struct cx8802_fh *)priv)->dev;
-
-	if (blackbird_queryctrl(dev, qctrl) == 0)
-		return 0;
-
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (unlikely(qctrl->id == 0))
-		return -EINVAL;
-	return cx8800_ctrl_query(dev->core, qctrl);
-}
-
 static int vidioc_enum_input (struct file *file, void *priv,
 				struct v4l2_input *i)
 {
@@ -944,22 +853,6 @@ static int vidioc_enum_input (struct file *file, void *priv,
 	return cx88_enum_input (core,i);
 }
 
-static int vidioc_g_ctrl (struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
-	return
-		cx88_get_control(core,ctl);
-}
-
-static int vidioc_s_ctrl (struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
-	return
-		cx88_set_control(core,ctl);
-}
-
 static int vidioc_g_frequency (struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
@@ -968,8 +861,9 @@ static int vidioc_g_frequency (struct file *file, void *priv,
 
 	if (unlikely(UNSET == core->board.tuner_type))
 		return -EINVAL;
+	if (unlikely(f->tuner != 0))
+		return -EINVAL;
 
-	f->type = V4L2_TUNER_ANALOG_TV;
 	f->frequency = core->freq;
 	call_all(core, tuner, g_frequency, f);
 
@@ -990,6 +884,8 @@ static int vidioc_s_input (struct file *file, void *priv, unsigned int i)
 
 	if (i >= 4)
 		return -EINVAL;
+	if (0 == INPUT(i).type)
+		return -EINVAL;
 
 	mutex_lock(&core->lock);
 	cx88_newstation(core);
@@ -1010,9 +906,9 @@ static int vidioc_g_tuner (struct file *file, void *priv,
 		return -EINVAL;
 
 	strcpy(t->name, "Television");
-	t->type       = V4L2_TUNER_ANALOG_TV;
 	t->capability = V4L2_TUNER_CAP_NORM;
 	t->rangehigh  = 0xffffffffUL;
+	call_all(core, tuner, g_tuner, t);
 
 	cx88_get_stereo(core ,t);
 	reg = cx_read(MO_DEVICE_STATUS);
@@ -1034,6 +930,14 @@ static int vidioc_s_tuner (struct file *file, void *priv,
 	return 0;
 }
 
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorm)
+{
+	struct cx88_core *core = ((struct cx8802_fh *)priv)->dev->core;
+
+	*tvnorm = core->tvnorm;
+	return 0;
+}
+
 static int vidioc_s_std (struct file *file, void *priv, v4l2_std_id *id)
 {
 	struct cx88_core  *core = ((struct cx8802_fh *)priv)->dev->core;
@@ -1087,6 +991,7 @@ static int mpeg_open(struct file *file)
 		mutex_unlock(&dev->core->lock);
 		return -ENOMEM;
 	}
+	v4l2_fh_init(&fh->fh, vdev);
 	file->private_data = fh;
 	fh->dev      = dev;
 
@@ -1103,6 +1008,7 @@ static int mpeg_open(struct file *file)
 
 	dev->core->mpeg_users++;
 	mutex_unlock(&dev->core->lock);
+	v4l2_fh_add(&fh->fh);
 	return 0;
 }
 
@@ -1123,6 +1029,8 @@ static int mpeg_release(struct file *file)
 
 	videobuf_mmap_free(&fh->mpegq);
 
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	file->private_data = NULL;
 	kfree(fh);
 
@@ -1155,13 +1063,14 @@ mpeg_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 static unsigned int
 mpeg_poll(struct file *file, struct poll_table_struct *wait)
 {
+	unsigned long req_events = poll_requested_events(wait);
 	struct cx8802_fh *fh = file->private_data;
 	struct cx8802_dev *dev = fh->dev;
 
-	if (!dev->mpeg_active)
+	if (!dev->mpeg_active && (req_events & (POLLIN | POLLRDNORM)))
 		blackbird_start_codec(file, fh);
 
-	return videobuf_poll_stream(file, &fh->mpegq, wait);
+	return v4l2_ctrl_poll(file, wait) | videobuf_poll_stream(file, &fh->mpegq, wait);
 }
 
 static int
@@ -1180,11 +1089,10 @@ static const struct v4l2_file_operations mpeg_fops =
 	.read	       = mpeg_read,
 	.poll          = mpeg_poll,
 	.mmap	       = mpeg_mmap,
-	.ioctl	       = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
-	.vidioc_querymenu     = vidioc_querymenu,
 	.vidioc_querycap      = vidioc_querycap,
 	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
@@ -1196,21 +1104,18 @@ static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
 	.vidioc_dqbuf         = vidioc_dqbuf,
 	.vidioc_streamon      = vidioc_streamon,
 	.vidioc_streamoff     = vidioc_streamoff,
-	.vidioc_g_ext_ctrls   = vidioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls   = vidioc_s_ext_ctrls,
-	.vidioc_try_ext_ctrls = vidioc_try_ext_ctrls,
 	.vidioc_s_frequency   = vidioc_s_frequency,
 	.vidioc_log_status    = vidioc_log_status,
-	.vidioc_queryctrl     = vidioc_queryctrl,
 	.vidioc_enum_input    = vidioc_enum_input,
-	.vidioc_g_ctrl        = vidioc_g_ctrl,
-	.vidioc_s_ctrl        = vidioc_s_ctrl,
 	.vidioc_g_frequency   = vidioc_g_frequency,
 	.vidioc_g_input       = vidioc_g_input,
 	.vidioc_s_input       = vidioc_s_input,
 	.vidioc_g_tuner       = vidioc_g_tuner,
 	.vidioc_s_tuner       = vidioc_s_tuner,
+	.vidioc_g_std         = vidioc_g_std,
 	.vidioc_s_std         = vidioc_s_std,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx8802_mpeg_template = {
@@ -1218,7 +1123,6 @@ static struct video_device cx8802_mpeg_template = {
 	.fops                 = &mpeg_fops,
 	.ioctl_ops 	      = &mpeg_ioctl_ops,
 	.tvnorms              = CX88_NORMS,
-	.current_norm         = V4L2_STD_NTSC_M,
 };
 
 /* ------------------------------------------------------------------ */
@@ -1286,6 +1190,7 @@ static int blackbird_register_video(struct cx8802_dev *dev)
 
 	dev->mpeg_dev = cx88_vdev_init(dev->core,dev->pci,
 				       &cx8802_mpeg_template,"mpeg");
+	dev->mpeg_dev->ctrl_handler = &dev->cxhdl.hdl;
 	video_set_drvdata(dev->mpeg_dev, dev);
 	err = video_register_device(dev->mpeg_dev,VFL_TYPE_GRABBER, -1);
 	if (err < 0) {
@@ -1318,17 +1223,20 @@ static int cx8802_blackbird_probe(struct cx8802_driver *drv)
 		goto fail_core;
 
 	dev->width = 720;
-	dev->height = 576;
-	cx2341x_fill_defaults(&dev->params);
-	dev->params.port = CX2341X_PORT_STREAMING;
-
-	cx8802_mpeg_template.current_norm = core->tvnorm;
-
 	if (core->tvnorm & V4L2_STD_525_60) {
 		dev->height = 480;
 	} else {
 		dev->height = 576;
 	}
+	dev->cxhdl.port = CX2341X_PORT_STREAMING;
+	dev->cxhdl.width = dev->width;
+	dev->cxhdl.height = dev->height;
+	dev->cxhdl.func = blackbird_mbox_func;
+	dev->cxhdl.priv = dev;
+	err = cx2341x_handler_init(&dev->cxhdl, 36);
+	if (err)
+		goto fail_core;
+	v4l2_ctrl_add_handler(&dev->cxhdl.hdl, &core->video_hdl);
 
 	/* blackbird stuff */
 	printk("%s/2: cx23416 based mpeg encoder (blackbird reference design)\n",
@@ -1336,12 +1244,14 @@ static int cx8802_blackbird_probe(struct cx8802_driver *drv)
 	host_setup(dev->core);
 
 	blackbird_initialize_codec(dev);
-	blackbird_register_video(dev);
 
 	/* initial device configuration: needed ? */
 //	init_controls(core);
 	cx88_set_tvnorm(core,core->tvnorm);
 	cx88_video_mux(core,0);
+	cx2341x_handler_set_50hz(&dev->cxhdl, dev->height == 576);
+	cx2341x_handler_setup(&dev->cxhdl);
+	blackbird_register_video(dev);
 
 	return 0;
 
@@ -1351,8 +1261,12 @@ static int cx8802_blackbird_probe(struct cx8802_driver *drv)
 
 static int cx8802_blackbird_remove(struct cx8802_driver *drv)
 {
+	struct cx88_core *core = drv->core;
+	struct cx8802_dev *dev = core->dvbdev;
+
 	/* blackbird */
 	blackbird_unregister_video(drv->core->dvbdev);
+	v4l2_ctrl_handler_free(&dev->cxhdl.hdl);
 
 	return 0;
 }
diff --git a/drivers/media/video/cx88/cx88-cards.c b/drivers/media/video/cx88/cx88-cards.c
index cbd5d119a2c..4e9d4f72296 100644
--- a/drivers/media/video/cx88/cx88-cards.c
+++ b/drivers/media/video/cx88/cx88-cards.c
@@ -3693,7 +3693,22 @@ struct cx88_core *cx88_core_create(struct pci_dev *pci, int nr)
 		return NULL;
 	}
 
+	if (v4l2_ctrl_handler_init(&core->video_hdl, 13)) {
+		v4l2_device_unregister(&core->v4l2_dev);
+		kfree(core);
+		return NULL;
+	}
+
+	if (v4l2_ctrl_handler_init(&core->audio_hdl, 13)) {
+		v4l2_ctrl_handler_free(&core->video_hdl);
+		v4l2_device_unregister(&core->v4l2_dev);
+		kfree(core);
+		return NULL;
+	}
+
 	if (0 != cx88_get_resources(core, pci)) {
+		v4l2_ctrl_handler_free(&core->video_hdl);
+		v4l2_ctrl_handler_free(&core->audio_hdl);
 		v4l2_device_unregister(&core->v4l2_dev);
 		kfree(core);
 		return NULL;
@@ -3706,6 +3721,11 @@ struct cx88_core *cx88_core_create(struct pci_dev *pci, int nr)
 	core->bmmio = (u8 __iomem *)core->lmmio;
 
 	if (core->lmmio == NULL) {
+		release_mem_region(pci_resource_start(pci, 0),
+			   pci_resource_len(pci, 0));
+		v4l2_ctrl_handler_free(&core->video_hdl);
+		v4l2_ctrl_handler_free(&core->audio_hdl);
+		v4l2_device_unregister(&core->v4l2_dev);
 		kfree(core);
 		return NULL;
 	}
diff --git a/drivers/media/video/cx88/cx88-core.c b/drivers/media/video/cx88/cx88-core.c
index fbfdd806793..e81c735f012 100644
--- a/drivers/media/video/cx88/cx88-core.c
+++ b/drivers/media/video/cx88/cx88-core.c
@@ -1012,6 +1012,9 @@ int cx88_set_tvnorm(struct cx88_core *core, v4l2_std_id norm)
 	// tell i2c chips
 	call_all(core, core, s_std, norm);
 
+	/* The chroma_agc control should be inaccessible if the video format is SECAM */
+	v4l2_ctrl_grab(core->chroma_agc, cxiformat == VideoFormatSECAM);
+
 	// done
 	return 0;
 }
@@ -1030,10 +1033,10 @@ struct video_device *cx88_vdev_init(struct cx88_core *core,
 		return NULL;
 	*vfd = *template_;
 	vfd->v4l2_dev = &core->v4l2_dev;
-	vfd->parent = &pci->dev;
 	vfd->release = video_device_release;
 	snprintf(vfd->name, sizeof(vfd->name), "%s %s (%s)",
 		 core->name, type, core->board.name);
+	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	return vfd;
 }
 
@@ -1086,6 +1089,8 @@ void cx88_core_put(struct cx88_core *core, struct pci_dev *pci)
 	iounmap(core->lmmio);
 	cx88_devcount--;
 	mutex_unlock(&devlist);
+	v4l2_ctrl_handler_free(&core->video_hdl);
+	v4l2_ctrl_handler_free(&core->audio_hdl);
 	v4l2_device_unregister(&core->v4l2_dev);
 	kfree(core);
 }
diff --git a/drivers/media/video/cx88/cx88-video.c b/drivers/media/video/cx88/cx88-video.c
index 921c56d115d..f6fcc7e763a 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/v4l2-event.h>
 #include <media/wm8775.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx2388x based TV cards");
@@ -155,219 +156,147 @@ static const struct cx8800_fmt* format_by_fourcc(unsigned int fourcc)
 
 /* ------------------------------------------------------------------- */
 
-static const struct v4l2_queryctrl no_ctl = {
-	.name  = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
+struct cx88_ctrl {
+	/* control information */
+	u32 id;
+	s32 minimum;
+	s32 maximum;
+	u32 step;
+	s32 default_value;
+
+	/* control register information */
+	u32 off;
+	u32 reg;
+	u32 sreg;
+	u32 mask;
+	u32 shift;
 };
 
-static const struct cx88_ctrl cx8800_ctls[] = {
+static const struct cx88_ctrl cx8800_vid_ctls[] = {
 	/* --- video --- */
 	{
-		.v = {
-			.id            = V4L2_CID_BRIGHTNESS,
-			.name          = "Brightness",
-			.minimum       = 0x00,
-			.maximum       = 0xff,
-			.step          = 1,
-			.default_value = 0x7f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 128,
-		.reg                   = MO_CONTR_BRIGHT,
-		.mask                  = 0x00ff,
-		.shift                 = 0,
+		.id            = V4L2_CID_BRIGHTNESS,
+		.minimum       = 0x00,
+		.maximum       = 0xff,
+		.step          = 1,
+		.default_value = 0x7f,
+		.off           = 128,
+		.reg           = MO_CONTR_BRIGHT,
+		.mask          = 0x00ff,
+		.shift         = 0,
 	},{
-		.v = {
-			.id            = V4L2_CID_CONTRAST,
-			.name          = "Contrast",
-			.minimum       = 0,
-			.maximum       = 0xff,
-			.step          = 1,
-			.default_value = 0x3f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 0,
-		.reg                   = MO_CONTR_BRIGHT,
-		.mask                  = 0xff00,
-		.shift                 = 8,
+		.id            = V4L2_CID_CONTRAST,
+		.minimum       = 0,
+		.maximum       = 0xff,
+		.step          = 1,
+		.default_value = 0x3f,
+		.off           = 0,
+		.reg           = MO_CONTR_BRIGHT,
+		.mask          = 0xff00,
+		.shift         = 8,
 	},{
-		.v = {
-			.id            = V4L2_CID_HUE,
-			.name          = "Hue",
-			.minimum       = 0,
-			.maximum       = 0xff,
-			.step          = 1,
-			.default_value = 0x7f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 128,
-		.reg                   = MO_HUE,
-		.mask                  = 0x00ff,
-		.shift                 = 0,
+		.id            = V4L2_CID_HUE,
+		.minimum       = 0,
+		.maximum       = 0xff,
+		.step          = 1,
+		.default_value = 0x7f,
+		.off           = 128,
+		.reg           = MO_HUE,
+		.mask          = 0x00ff,
+		.shift         = 0,
 	},{
 		/* strictly, this only describes only U saturation.
 		 * V saturation is handled specially through code.
 		 */
-		.v = {
-			.id            = V4L2_CID_SATURATION,
-			.name          = "Saturation",
-			.minimum       = 0,
-			.maximum       = 0xff,
-			.step          = 1,
-			.default_value = 0x7f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 0,
-		.reg                   = MO_UV_SATURATION,
-		.mask                  = 0x00ff,
-		.shift                 = 0,
+		.id            = V4L2_CID_SATURATION,
+		.minimum       = 0,
+		.maximum       = 0xff,
+		.step          = 1,
+		.default_value = 0x7f,
+		.off           = 0,
+		.reg           = MO_UV_SATURATION,
+		.mask          = 0x00ff,
+		.shift         = 0,
 	}, {
-		.v = {
-			.id            = V4L2_CID_SHARPNESS,
-			.name          = "Sharpness",
-			.minimum       = 0,
-			.maximum       = 4,
-			.step          = 1,
-			.default_value = 0x0,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 0,
+		.id            = V4L2_CID_SHARPNESS,
+		.minimum       = 0,
+		.maximum       = 4,
+		.step          = 1,
+		.default_value = 0x0,
+		.off           = 0,
 		/* NOTE: the value is converted and written to both even
 		   and odd registers in the code */
-		.reg                   = MO_FILTER_ODD,
-		.mask                  = 7 << 7,
-		.shift                 = 7,
+		.reg           = MO_FILTER_ODD,
+		.mask          = 7 << 7,
+		.shift         = 7,
 	}, {
-		.v = {
-			.id            = V4L2_CID_CHROMA_AGC,
-			.name          = "Chroma AGC",
-			.minimum       = 0,
-			.maximum       = 1,
-			.default_value = 0x1,
-			.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		},
-		.reg                   = MO_INPUT_FORMAT,
-		.mask                  = 1 << 10,
-		.shift                 = 10,
+		.id            = V4L2_CID_CHROMA_AGC,
+		.minimum       = 0,
+		.maximum       = 1,
+		.default_value = 0x1,
+		.reg           = MO_INPUT_FORMAT,
+		.mask          = 1 << 10,
+		.shift         = 10,
 	}, {
-		.v = {
-			.id            = V4L2_CID_COLOR_KILLER,
-			.name          = "Color killer",
-			.minimum       = 0,
-			.maximum       = 1,
-			.default_value = 0x1,
-			.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		},
-		.reg                   = MO_INPUT_FORMAT,
-		.mask                  = 1 << 9,
-		.shift                 = 9,
+		.id            = V4L2_CID_COLOR_KILLER,
+		.minimum       = 0,
+		.maximum       = 1,
+		.default_value = 0x1,
+		.reg           = MO_INPUT_FORMAT,
+		.mask          = 1 << 9,
+		.shift         = 9,
 	}, {
-		.v = {
-			.id            = V4L2_CID_BAND_STOP_FILTER,
-			.name          = "Notch filter",
-			.minimum       = 0,
-			.maximum       = 1,
-			.step          = 1,
-			.default_value = 0x0,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 0,
-		.reg                   = MO_HTOTAL,
-		.mask                  = 3 << 11,
-		.shift                 = 11,
-	}, {
-	/* --- audio --- */
-		.v = {
-			.id            = V4L2_CID_AUDIO_MUTE,
-			.name          = "Mute",
-			.minimum       = 0,
-			.maximum       = 1,
-			.default_value = 1,
-			.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		},
-		.reg                   = AUD_VOL_CTL,
-		.sreg                  = SHADOW_AUD_VOL_CTL,
-		.mask                  = (1 << 6),
-		.shift                 = 6,
+		.id            = V4L2_CID_BAND_STOP_FILTER,
+		.minimum       = 0,
+		.maximum       = 1,
+		.step          = 1,
+		.default_value = 0x0,
+		.off           = 0,
+		.reg           = MO_HTOTAL,
+		.mask          = 3 << 11,
+		.shift         = 11,
+	}
+};
+
+static const struct cx88_ctrl cx8800_aud_ctls[] = {
+	{
+		/* --- audio --- */
+		.id            = V4L2_CID_AUDIO_MUTE,
+		.minimum       = 0,
+		.maximum       = 1,
+		.default_value = 1,
+		.reg           = AUD_VOL_CTL,
+		.sreg          = SHADOW_AUD_VOL_CTL,
+		.mask          = (1 << 6),
+		.shift         = 6,
 	},{
-		.v = {
-			.id            = V4L2_CID_AUDIO_VOLUME,
-			.name          = "Volume",
-			.minimum       = 0,
-			.maximum       = 0x3f,
-			.step          = 1,
-			.default_value = 0x3f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.reg                   = AUD_VOL_CTL,
-		.sreg                  = SHADOW_AUD_VOL_CTL,
-		.mask                  = 0x3f,
-		.shift                 = 0,
+		.id            = V4L2_CID_AUDIO_VOLUME,
+		.minimum       = 0,
+		.maximum       = 0x3f,
+		.step          = 1,
+		.default_value = 0x3f,
+		.reg           = AUD_VOL_CTL,
+		.sreg          = SHADOW_AUD_VOL_CTL,
+		.mask          = 0x3f,
+		.shift         = 0,
 	},{
-		.v = {
-			.id            = V4L2_CID_AUDIO_BALANCE,
-			.name          = "Balance",
-			.minimum       = 0,
-			.maximum       = 0x7f,
-			.step          = 1,
-			.default_value = 0x40,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.reg                   = AUD_BAL_CTL,
-		.sreg                  = SHADOW_AUD_BAL_CTL,
-		.mask                  = 0x7f,
-		.shift                 = 0,
+		.id            = V4L2_CID_AUDIO_BALANCE,
+		.minimum       = 0,
+		.maximum       = 0x7f,
+		.step          = 1,
+		.default_value = 0x40,
+		.reg           = AUD_BAL_CTL,
+		.sreg          = SHADOW_AUD_BAL_CTL,
+		.mask          = 0x7f,
+		.shift         = 0,
 	}
 };
-enum { CX8800_CTLS = ARRAY_SIZE(cx8800_ctls) };
-
-/* Must be sorted from low to high control ID! */
-const u32 cx88_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_MUTE,
-	V4L2_CID_SHARPNESS,
-	V4L2_CID_CHROMA_AGC,
-	V4L2_CID_COLOR_KILLER,
-	V4L2_CID_BAND_STOP_FILTER,
-	0
-};
-EXPORT_SYMBOL(cx88_user_ctrls);
 
-static const u32 * const ctrl_classes[] = {
-	cx88_user_ctrls,
-	NULL
+enum {
+	CX8800_VID_CTLS = ARRAY_SIZE(cx8800_vid_ctls),
+	CX8800_AUD_CTLS = ARRAY_SIZE(cx8800_aud_ctls),
 };
 
-int cx8800_ctrl_query(struct cx88_core *core, struct v4l2_queryctrl *qctrl)
-{
-	int i;
-
-	if (qctrl->id < V4L2_CID_BASE ||
-	    qctrl->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	for (i = 0; i < CX8800_CTLS; i++)
-		if (cx8800_ctls[i].v.id == qctrl->id)
-			break;
-	if (i == CX8800_CTLS) {
-		*qctrl = no_ctl;
-		return 0;
-	}
-	*qctrl = cx8800_ctls[i].v;
-	/* Report chroma AGC as inactive when SECAM is selected */
-	if (cx8800_ctls[i].v.id == V4L2_CID_CHROMA_AGC &&
-	    core->tvnorm & V4L2_STD_SECAM)
-		qctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
-
-	return 0;
-}
-EXPORT_SYMBOL(cx8800_ctrl_query);
-
 /* ------------------------------------------------------------------- */
 /* resource management                                                 */
 
@@ -591,8 +520,9 @@ static int
 buffer_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
 {
 	struct cx8800_fh *fh = q->priv_data;
+	struct cx8800_dev  *dev = fh->dev;
 
-	*size = fh->fmt->depth*fh->width*fh->height >> 3;
+	*size = dev->fmt->depth * dev->width * dev->height >> 3;
 	if (0 == *count)
 		*count = 32;
 	if (*size * *count > vid_limit * 1024 * 1024)
@@ -611,21 +541,21 @@ buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	struct videobuf_dmabuf *dma=videobuf_to_dma(&buf->vb);
 	int rc, init_buffer = 0;
 
-	BUG_ON(NULL == fh->fmt);
-	if (fh->width  < 48 || fh->width  > norm_maxw(core->tvnorm) ||
-	    fh->height < 32 || fh->height > norm_maxh(core->tvnorm))
+	BUG_ON(NULL == dev->fmt);
+	if (dev->width  < 48 || dev->width  > norm_maxw(core->tvnorm) ||
+	    dev->height < 32 || dev->height > norm_maxh(core->tvnorm))
 		return -EINVAL;
-	buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
+	buf->vb.size = (dev->width * dev->height * dev->fmt->depth) >> 3;
 	if (0 != buf->vb.baddr  &&  buf->vb.bsize < buf->vb.size)
 		return -EINVAL;
 
-	if (buf->fmt       != fh->fmt    ||
-	    buf->vb.width  != fh->width  ||
-	    buf->vb.height != fh->height ||
+	if (buf->fmt       != dev->fmt    ||
+	    buf->vb.width  != dev->width  ||
+	    buf->vb.height != dev->height ||
 	    buf->vb.field  != field) {
-		buf->fmt       = fh->fmt;
-		buf->vb.width  = fh->width;
-		buf->vb.height = fh->height;
+		buf->fmt       = dev->fmt;
+		buf->vb.width  = dev->width;
+		buf->vb.height = dev->height;
 		buf->vb.field  = field;
 		init_buffer = 1;
 	}
@@ -675,7 +605,7 @@ buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
 	}
 	dprintk(2,"[%p/%d] buffer_prepare - %dx%d %dbpp \"%s\" - dma=0x%08lx\n",
 		buf, buf->vb.i,
-		fh->width, fh->height, fh->fmt->depth, fh->fmt->name,
+		dev->width, dev->height, dev->fmt->depth, dev->fmt->name,
 		(unsigned long)buf->risc.dma);
 
 	buf->vb.state = VIDEOBUF_PREPARED;
@@ -755,12 +685,15 @@ static const struct videobuf_queue_ops cx8800_video_qops = {
 
 /* ------------------------------------------------------------------ */
 
-static struct videobuf_queue* get_queue(struct cx8800_fh *fh)
+static struct videobuf_queue *get_queue(struct file *file)
 {
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	struct video_device *vdev = video_devdata(file);
+	struct cx8800_fh *fh = file->private_data;
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 		return &fh->vidq;
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 		return &fh->vbiq;
 	default:
 		BUG();
@@ -768,12 +701,14 @@ static struct videobuf_queue* get_queue(struct cx8800_fh *fh)
 	}
 }
 
-static int get_ressource(struct cx8800_fh *fh)
+static int get_resource(struct file *file)
 {
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	struct video_device *vdev = video_devdata(file);
+
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 		return RESOURCE_VIDEO;
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 		return RESOURCE_VBI;
 	default:
 		BUG();
@@ -810,13 +745,9 @@ static int video_open(struct file *file)
 	if (unlikely(!fh))
 		return -ENOMEM;
 
+	v4l2_fh_init(&fh->fh, vdev);
 	file->private_data = fh;
 	fh->dev      = dev;
-	fh->radio    = radio;
-	fh->type     = type;
-	fh->width    = 320;
-	fh->height   = 240;
-	fh->fmt      = format_by_fourcc(V4L2_PIX_FMT_BGR24);
 
 	mutex_lock(&core->lock);
 
@@ -833,7 +764,7 @@ static int video_open(struct file *file)
 			    sizeof(struct cx88_buffer),
 			    fh, NULL);
 
-	if (fh->radio) {
+	if (vdev->vfl_type == VFL_TYPE_RADIO) {
 		dprintk(1,"video_open: setting radio device\n");
 		cx_write(MO_GP3_IO, core->board.radio.gpio3);
 		cx_write(MO_GP0_IO, core->board.radio.gpio0);
@@ -859,6 +790,7 @@ static int video_open(struct file *file)
 
 	core->users++;
 	mutex_unlock(&core->lock);
+	v4l2_fh_add(&fh->fh);
 
 	return 0;
 }
@@ -866,15 +798,16 @@ static int video_open(struct file *file)
 static ssize_t
 video_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct cx8800_fh *fh = file->private_data;
 
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_GRABBER:
 		if (res_locked(fh->dev,RESOURCE_VIDEO))
 			return -EBUSY;
 		return videobuf_read_one(&fh->vidq, data, count, ppos,
 					 file->f_flags & O_NONBLOCK);
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case VFL_TYPE_VBI:
 		if (!res_get(fh->dev,fh,RESOURCE_VBI))
 			return -EBUSY;
 		return videobuf_read_stream(&fh->vbiq, data, count, ppos, 1,
@@ -888,16 +821,16 @@ video_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 static unsigned int
 video_poll(struct file *file, struct poll_table_struct *wait)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct cx8800_fh *fh = file->private_data;
 	struct cx88_buffer *buf;
-	unsigned int rc = POLLERR;
+	unsigned int rc = v4l2_ctrl_poll(file, wait);
 
-	if (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type) {
+	if (vdev->vfl_type == VFL_TYPE_VBI) {
 		if (!res_get(fh->dev,fh,RESOURCE_VBI))
-			return POLLERR;
-		return videobuf_poll_stream(file, &fh->vbiq, wait);
+			return rc | POLLERR;
+		return rc | videobuf_poll_stream(file, &fh->vbiq, wait);
 	}
-
 	mutex_lock(&fh->vidq.vb_lock);
 	if (res_check(fh,RESOURCE_VIDEO)) {
 		/* streaming capture */
@@ -913,9 +846,7 @@ video_poll(struct file *file, struct poll_table_struct *wait)
 	poll_wait(file, &buf->vb.done, wait);
 	if (buf->vb.state == VIDEOBUF_DONE ||
 	    buf->vb.state == VIDEOBUF_ERROR)
-		rc = POLLIN|POLLRDNORM;
-	else
-		rc = 0;
+		rc |= POLLIN|POLLRDNORM;
 done:
 	mutex_unlock(&fh->vidq.vb_lock);
 	return rc;
@@ -952,6 +883,8 @@ static int video_release(struct file *file)
 	videobuf_mmap_free(&fh->vbiq);
 
 	mutex_lock(&dev->core->lock);
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
 	file->private_data = NULL;
 	kfree(fh);
 
@@ -966,156 +899,104 @@ static int video_release(struct file *file)
 static int
 video_mmap(struct file *file, struct vm_area_struct * vma)
 {
-	struct cx8800_fh *fh = file->private_data;
-
-	return videobuf_mmap_mapper(get_queue(fh), vma);
+	return videobuf_mmap_mapper(get_queue(file), vma);
 }
 
 /* ------------------------------------------------------------------ */
 /* VIDEO CTRL IOCTLS                                                  */
 
-int cx88_get_control (struct cx88_core  *core, struct v4l2_control *ctl)
+static int cx8800_s_vid_ctrl(struct v4l2_ctrl *ctrl)
 {
-	const struct cx88_ctrl  *c    = NULL;
-	u32 value;
-	int i;
+	struct cx88_core *core =
+		container_of(ctrl->handler, struct cx88_core, video_hdl);
+	const struct cx88_ctrl *cc = ctrl->priv;
+	u32 value, mask;
 
-	for (i = 0; i < CX8800_CTLS; i++)
-		if (cx8800_ctls[i].v.id == ctl->id)
-			c = &cx8800_ctls[i];
-	if (unlikely(NULL == c))
-		return -EINVAL;
+	mask = cc->mask;
+	switch (ctrl->id) {
+	case V4L2_CID_SATURATION:
+		/* special v_sat handling */
 
-	value = c->sreg ? cx_sread(c->sreg) : cx_read(c->reg);
-	switch (ctl->id) {
-	case V4L2_CID_AUDIO_BALANCE:
-		ctl->value = ((value & 0x7f) < 0x40) ? ((value & 0x7f) + 0x40)
-					: (0x7f - (value & 0x7f));
-		break;
-	case V4L2_CID_AUDIO_VOLUME:
-		ctl->value = 0x3f - (value & 0x3f);
+		value = ((ctrl->val - cc->off) << cc->shift) & cc->mask;
+
+		if (core->tvnorm & V4L2_STD_SECAM) {
+			/* For SECAM, both U and V sat should be equal */
+			value = value << 8 | value;
+		} else {
+			/* Keeps U Saturation proportional to V Sat */
+			value = (value * 0x5a) / 0x7f << 8 | value;
+		}
+		mask = 0xffff;
 		break;
 	case V4L2_CID_SHARPNESS:
-		ctl->value = ((value & 0x0200) ? (((value & 0x0180) >> 7) + 1)
-						 : 0);
+		/* 0b000, 0b100, 0b101, 0b110, or 0b111 */
+		value = (ctrl->val < 1 ? 0 : ((ctrl->val + 3) << 7));
+		/* needs to be set for both fields */
+		cx_andor(MO_FILTER_EVEN, mask, value);
+		break;
+	case V4L2_CID_CHROMA_AGC:
+		value = ((ctrl->val - cc->off) << cc->shift) & cc->mask;
 		break;
 	default:
-		ctl->value = ((value + (c->off << c->shift)) & c->mask) >> c->shift;
+		value = ((ctrl->val - cc->off) << cc->shift) & cc->mask;
 		break;
 	}
-	dprintk(1,"get_control id=0x%X(%s) ctrl=0x%02x, reg=0x%02x val=0x%02x (mask 0x%02x)%s\n",
-				ctl->id, c->v.name, ctl->value, c->reg,
-				value,c->mask, c->sreg ? " [shadowed]" : "");
+	dprintk(1, "set_control id=0x%X(%s) ctrl=0x%02x, reg=0x%02x val=0x%02x (mask 0x%02x)%s\n",
+				ctrl->id, ctrl->name, ctrl->val, cc->reg, value,
+				mask, cc->sreg ? " [shadowed]" : "");
+	if (cc->sreg)
+		cx_sandor(cc->sreg, cc->reg, mask, value);
+	else
+		cx_andor(cc->reg, mask, value);
 	return 0;
 }
-EXPORT_SYMBOL(cx88_get_control);
 
-int cx88_set_control(struct cx88_core *core, struct v4l2_control *ctl)
+static int cx8800_s_aud_ctrl(struct v4l2_ctrl *ctrl)
 {
-	const struct cx88_ctrl *c = NULL;
+	struct cx88_core *core =
+		container_of(ctrl->handler, struct cx88_core, audio_hdl);
+	const struct cx88_ctrl *cc = ctrl->priv;
 	u32 value,mask;
-	int i;
-
-	for (i = 0; i < CX8800_CTLS; i++) {
-		if (cx8800_ctls[i].v.id == ctl->id) {
-			c = &cx8800_ctls[i];
-		}
-	}
-	if (unlikely(NULL == c))
-		return -EINVAL;
-
-	if (ctl->value < c->v.minimum)
-		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) {
+		switch (ctrl->id) {
 		case V4L2_CID_AUDIO_MUTE:
-			client_ctl.value = ctl->value;
+			wm8775_s_ctrl(core, ctrl->id, ctrl->val);
 			break;
 		case V4L2_CID_AUDIO_VOLUME:
-			client_ctl.value = (ctl->value) ?
-				(0x90 + ctl->value) << 8 : 0;
+			wm8775_s_ctrl(core, ctrl->id, (ctrl->val) ?
+						(0x90 + ctrl->val) << 8 : 0);
 			break;
 		case V4L2_CID_AUDIO_BALANCE:
-			client_ctl.value = ctl->value << 9;
+			wm8775_s_ctrl(core, ctrl->id, ctrl->val << 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) {
+	mask = cc->mask;
+	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_BALANCE:
-		value = (ctl->value < 0x40) ? (0x7f - ctl->value) : (ctl->value - 0x40);
+		value = (ctrl->val < 0x40) ? (0x7f - ctrl->val) : (ctrl->val - 0x40);
 		break;
 	case V4L2_CID_AUDIO_VOLUME:
-		value = 0x3f - (ctl->value & 0x3f);
-		break;
-	case V4L2_CID_SATURATION:
-		/* special v_sat handling */
-
-		value = ((ctl->value - c->off) << c->shift) & c->mask;
-
-		if (core->tvnorm & V4L2_STD_SECAM) {
-			/* For SECAM, both U and V sat should be equal */
-			value=value<<8|value;
-		} else {
-			/* Keeps U Saturation proportional to V Sat */
-			value=(value*0x5a)/0x7f<<8|value;
-		}
-		mask=0xffff;
-		break;
-	case V4L2_CID_SHARPNESS:
-		/* 0b000, 0b100, 0b101, 0b110, or 0b111 */
-		value = (ctl->value < 1 ? 0 : ((ctl->value + 3) << 7));
-		/* needs to be set for both fields */
-		cx_andor(MO_FILTER_EVEN, mask, value);
-		break;
-	case V4L2_CID_CHROMA_AGC:
-		/* Do not allow chroma AGC to be enabled for SECAM */
-		value = ((ctl->value - c->off) << c->shift) & c->mask;
-		if (core->tvnorm & V4L2_STD_SECAM && value)
-			return -EINVAL;
+		value = 0x3f - (ctrl->val & 0x3f);
 		break;
 	default:
-		value = ((ctl->value - c->off) << c->shift) & c->mask;
+		value = ((ctrl->val - cc->off) << cc->shift) & cc->mask;
 		break;
 	}
 	dprintk(1,"set_control id=0x%X(%s) ctrl=0x%02x, reg=0x%02x val=0x%02x (mask 0x%02x)%s\n",
-				ctl->id, c->v.name, ctl->value, c->reg, value,
-				mask, c->sreg ? " [shadowed]" : "");
-	if (c->sreg) {
-		cx_sandor(c->sreg, c->reg, mask, value);
-	} else {
-		cx_andor(c->reg, mask, value);
-	}
+				ctrl->id, ctrl->name, ctrl->val, cc->reg, value,
+				mask, cc->sreg ? " [shadowed]" : "");
+	if (cc->sreg)
+		cx_sandor(cc->sreg, cc->reg, mask, value);
+	else
+		cx_andor(cc->reg, mask, value);
 	return 0;
 }
-EXPORT_SYMBOL(cx88_set_control);
-
-static void init_controls(struct cx88_core *core)
-{
-	struct v4l2_control ctrl;
-	int i;
-
-	for (i = 0; i < CX8800_CTLS; i++) {
-		ctrl.id=cx8800_ctls[i].v.id;
-		ctrl.value=cx8800_ctls[i].v.default_value;
-
-		cx88_set_control(core, &ctrl);
-	}
-}
 
 /* ------------------------------------------------------------------ */
 /* VIDEO IOCTLS                                                       */
@@ -1124,15 +1005,17 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 					struct v4l2_format *f)
 {
 	struct cx8800_fh  *fh   = priv;
+	struct cx8800_dev *dev = fh->dev;
 
-	f->fmt.pix.width        = fh->width;
-	f->fmt.pix.height       = fh->height;
+	f->fmt.pix.width        = dev->width;
+	f->fmt.pix.height       = dev->height;
 	f->fmt.pix.field        = fh->vidq.field;
-	f->fmt.pix.pixelformat  = fh->fmt->fourcc;
+	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
 	f->fmt.pix.bytesperline =
-		(f->fmt.pix.width * fh->fmt->depth) >> 3;
+		(f->fmt.pix.width * dev->fmt->depth) >> 3;
 	f->fmt.pix.sizeimage =
 		f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 	return 0;
 }
 
@@ -1184,33 +1067,54 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 					struct v4l2_format *f)
 {
 	struct cx8800_fh  *fh   = priv;
+	struct cx8800_dev *dev = fh->dev;
 	int err = vidioc_try_fmt_vid_cap (file,priv,f);
 
 	if (0 != err)
 		return err;
-	fh->fmt        = format_by_fourcc(f->fmt.pix.pixelformat);
-	fh->width      = f->fmt.pix.width;
-	fh->height     = f->fmt.pix.height;
+	dev->fmt        = format_by_fourcc(f->fmt.pix.pixelformat);
+	dev->width      = f->fmt.pix.width;
+	dev->height     = f->fmt.pix.height;
 	fh->vidq.field = f->fmt.pix.field;
 	return 0;
 }
 
-static int vidioc_querycap (struct file *file, void  *priv,
+void cx88_querycap(struct file *file, struct cx88_core *core,
+		struct v4l2_capability *cap)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	strlcpy(cap->card, core->board.name, sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	if (UNSET != core->board.tuner_type)
+		cap->device_caps |= V4L2_CAP_TUNER;
+	switch (vdev->vfl_type) {
+	case VFL_TYPE_RADIO:
+		cap->device_caps = V4L2_CAP_RADIO | V4L2_CAP_TUNER;
+		break;
+	case VFL_TYPE_GRABBER:
+		cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+		break;
+	case VFL_TYPE_VBI:
+		cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+		break;
+	}
+	cap->capabilities = cap->device_caps | V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_VBI_CAPTURE | V4L2_CAP_DEVICE_CAPS;
+	if (core->board.radio.type == CX88_RADIO)
+		cap->capabilities |= V4L2_CAP_RADIO;
+}
+EXPORT_SYMBOL(cx88_querycap);
+
+static int vidioc_querycap(struct file *file, void  *priv,
 					struct v4l2_capability *cap)
 {
 	struct cx8800_dev *dev  = ((struct cx8800_fh *)priv)->dev;
 	struct cx88_core  *core = dev->core;
 
 	strcpy(cap->driver, "cx8800");
-	strlcpy(cap->card, core->board.name, sizeof(cap->card));
-	sprintf(cap->bus_info,"PCI:%s",pci_name(dev->pci));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_READWRITE     |
-		V4L2_CAP_STREAMING     |
-		V4L2_CAP_VBI_CAPTURE;
-	if (UNSET != core->board.tuner_type)
-		cap->capabilities |= V4L2_CAP_TUNER;
+	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
+	cx88_querycap(file, core, cap);
 	return 0;
 }
 
@@ -1228,69 +1132,67 @@ static int vidioc_enum_fmt_vid_cap (struct file *file, void  *priv,
 
 static int vidioc_reqbufs (struct file *file, void *priv, struct v4l2_requestbuffers *p)
 {
-	struct cx8800_fh  *fh   = priv;
-	return (videobuf_reqbufs(get_queue(fh), p));
+	return videobuf_reqbufs(get_queue(file), p);
 }
 
 static int vidioc_querybuf (struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx8800_fh  *fh   = priv;
-	return (videobuf_querybuf(get_queue(fh), p));
+	return videobuf_querybuf(get_queue(file), p);
 }
 
 static int vidioc_qbuf (struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx8800_fh  *fh   = priv;
-	return (videobuf_qbuf(get_queue(fh), p));
+	return videobuf_qbuf(get_queue(file), p);
 }
 
 static int vidioc_dqbuf (struct file *file, void *priv, struct v4l2_buffer *p)
 {
-	struct cx8800_fh  *fh   = priv;
-	return (videobuf_dqbuf(get_queue(fh), p,
-				file->f_flags & O_NONBLOCK));
+	return videobuf_dqbuf(get_queue(file), p,
+				file->f_flags & O_NONBLOCK);
 }
 
 static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct cx8800_fh  *fh   = priv;
 	struct cx8800_dev *dev  = fh->dev;
 
-	/* We should remember that this driver also supports teletext,  */
-	/* so we have to test if the v4l2_buf_type is VBI capture data. */
-	if (unlikely((fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
-		     (fh->type != V4L2_BUF_TYPE_VBI_CAPTURE)))
-		return -EINVAL;
-
-	if (unlikely(i != fh->type))
+	if ((vdev->vfl_type == VFL_TYPE_GRABBER && i != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+	    (vdev->vfl_type == VFL_TYPE_VBI && i != V4L2_BUF_TYPE_VBI_CAPTURE))
 		return -EINVAL;
 
-	if (unlikely(!res_get(dev,fh,get_ressource(fh))))
+	if (unlikely(!res_get(dev, fh, get_resource(file))))
 		return -EBUSY;
-	return videobuf_streamon(get_queue(fh));
+	return videobuf_streamon(get_queue(file));
 }
 
 static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 {
+	struct video_device *vdev = video_devdata(file);
 	struct cx8800_fh  *fh   = priv;
 	struct cx8800_dev *dev  = fh->dev;
 	int               err, res;
 
-	if ((fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
-	    (fh->type != V4L2_BUF_TYPE_VBI_CAPTURE))
+	if ((vdev->vfl_type == VFL_TYPE_GRABBER && i != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+	    (vdev->vfl_type == VFL_TYPE_VBI && i != V4L2_BUF_TYPE_VBI_CAPTURE))
 		return -EINVAL;
 
-	if (i != fh->type)
-		return -EINVAL;
-
-	res = get_ressource(fh);
-	err = videobuf_streamoff(get_queue(fh));
+	res = get_resource(file);
+	err = videobuf_streamoff(get_queue(file));
 	if (err < 0)
 		return err;
 	res_free(dev,fh,res);
 	return 0;
 }
 
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *tvnorm)
+{
+	struct cx88_core *core = ((struct cx8800_fh *)priv)->dev->core;
+
+	*tvnorm = core->tvnorm;
+	return 0;
+}
+
 static int vidioc_s_std (struct file *file, void *priv, v4l2_std_id *tvnorms)
 {
 	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
@@ -1327,8 +1229,8 @@ int cx88_enum_input (struct cx88_core  *core,struct v4l2_input *i)
 	if ((CX88_VMUX_TELEVISION == INPUT(n).type) ||
 	    (CX88_VMUX_CABLE      == INPUT(n).type)) {
 		i->type = V4L2_INPUT_TYPE_TUNER;
-		i->std = CX88_NORMS;
 	}
+	i->std = CX88_NORMS;
 	return 0;
 }
 EXPORT_SYMBOL(cx88_enum_input);
@@ -1354,6 +1256,8 @@ static int vidioc_s_input (struct file *file, void *priv, unsigned int i)
 
 	if (i >= 4)
 		return -EINVAL;
+	if (0 == INPUT(i).type)
+		return -EINVAL;
 
 	mutex_lock(&core->lock);
 	cx88_newstation(core);
@@ -1362,35 +1266,6 @@ static int vidioc_s_input (struct file *file, void *priv, unsigned int i)
 	return 0;
 }
 
-
-
-static int vidioc_queryctrl (struct file *file, void *priv,
-				struct v4l2_queryctrl *qctrl)
-{
-	struct cx88_core *core = ((struct cx8800_fh *)priv)->dev->core;
-
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (unlikely(qctrl->id == 0))
-		return -EINVAL;
-	return cx8800_ctrl_query(core, qctrl);
-}
-
-static int vidioc_g_ctrl (struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
-	return
-		cx88_get_control(core,ctl);
-}
-
-static int vidioc_s_ctrl (struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx88_core  *core = ((struct cx8800_fh *)priv)->dev->core;
-	return
-		cx88_set_control(core,ctl);
-}
-
 static int vidioc_g_tuner (struct file *file, void *priv,
 				struct v4l2_tuner *t)
 {
@@ -1403,9 +1278,9 @@ static int vidioc_g_tuner (struct file *file, void *priv,
 		return -EINVAL;
 
 	strcpy(t->name, "Television");
-	t->type       = V4L2_TUNER_ANALOG_TV;
 	t->capability = V4L2_TUNER_CAP_NORM;
 	t->rangehigh  = 0xffffffffUL;
+	call_all(core, tuner, g_tuner, t);
 
 	cx88_get_stereo(core ,t);
 	reg = cx_read(MO_DEVICE_STATUS);
@@ -1435,9 +1310,9 @@ static int vidioc_g_frequency (struct file *file, void *priv,
 
 	if (unlikely(UNSET == core->board.tuner_type))
 		return -EINVAL;
+	if (f->tuner)
+		return -EINVAL;
 
-	/* f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV; */
-	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
 	f->frequency = core->freq;
 
 	call_all(core, tuner, g_frequency, f);
@@ -1454,9 +1329,10 @@ int cx88_set_freq (struct cx88_core  *core,
 		return -EINVAL;
 
 	mutex_lock(&core->lock);
-	core->freq = f->frequency;
 	cx88_newstation(core);
 	call_all(core, tuner, s_frequency, f);
+	call_all(core, tuner, g_frequency, f);
+	core->freq = f->frequency;
 
 	/* When changing channels it is required to reset TVAUDIO */
 	msleep (10);
@@ -1474,13 +1350,17 @@ static int vidioc_s_frequency (struct file *file, void *priv,
 	struct cx8800_fh  *fh   = priv;
 	struct cx88_core  *core = fh->dev->core;
 
-	if (unlikely(0 == fh->radio && f->type != V4L2_TUNER_ANALOG_TV))
-		return -EINVAL;
-	if (unlikely(1 == fh->radio && f->type != V4L2_TUNER_RADIO))
-		return -EINVAL;
+	return cx88_set_freq(core, f);
+}
 
-	return
-		cx88_set_freq (core,f);
+static int vidioc_g_chip_ident(struct file *file, void *priv,
+				struct v4l2_dbg_chip_ident *chip)
+{
+	if (!v4l2_chip_match_host(&chip->match))
+		return -EINVAL;
+	chip->revision = 0;
+	chip->ident = V4L2_IDENT_UNKNOWN;
+	return 0;
 }
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -1513,19 +1393,6 @@ static int vidioc_s_register (struct file *file, void *fh,
 /* RADIO ESPECIFIC IOCTLS                                      */
 /* ----------------------------------------------------------- */
 
-static int radio_querycap (struct file *file, void  *priv,
-					struct v4l2_capability *cap)
-{
-	struct cx8800_dev *dev  = ((struct cx8800_fh *)priv)->dev;
-	struct cx88_core  *core = dev->core;
-
-	strcpy(cap->driver, "cx8800");
-	strlcpy(cap->card, core->board.name, sizeof(cap->card));
-	sprintf(cap->bus_info,"PCI:%s", pci_name(dev->pci));
-	cap->capabilities = V4L2_CAP_TUNER;
-	return 0;
-}
-
 static int radio_g_tuner (struct file *file, void *priv,
 				struct v4l2_tuner *t)
 {
@@ -1535,32 +1402,11 @@ static int radio_g_tuner (struct file *file, void *priv,
 		return -EINVAL;
 
 	strcpy(t->name, "Radio");
-	t->type = V4L2_TUNER_RADIO;
 
 	call_all(core, tuner, g_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_audio (struct file *file, void *priv, struct v4l2_audio *a)
-{
-	if (unlikely(a->index))
-		return -EINVAL;
-
-	strcpy(a->name,"Radio");
-	return 0;
-}
-
 /* FIXME: Should add a standard for radio */
 
 static int radio_s_tuner (struct file *file, void *priv,
@@ -1570,46 +1416,14 @@ static int radio_s_tuner (struct file *file, void *priv,
 
 	if (0 != t->index)
 		return -EINVAL;
+	if (t->audmode > V4L2_TUNER_MODE_STEREO)
+		t->audmode = V4L2_TUNER_MODE_STEREO;
 
 	call_all(core, tuner, s_tuner, t);
 
 	return 0;
 }
 
-static int radio_s_audio (struct file *file, void *fh,
-			  struct v4l2_audio *a)
-{
-	return 0;
-}
-
-static int radio_s_input (struct file *file, void *fh, unsigned int i)
-{
-	return 0;
-}
-
-static int radio_queryctrl (struct file *file, void *priv,
-			    struct v4l2_queryctrl *c)
-{
-	int i;
-
-	if (c->id <  V4L2_CID_BASE ||
-		c->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	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;
-		}
-		if (i == CX8800_CTLS)
-			return -EINVAL;
-		*c = cx8800_ctls[i].v;
-	} else
-		*c = no_ctl;
-	return 0;
-}
-
 /* ----------------------------------------------------------- */
 
 static void cx8800_vid_timeout(unsigned long data)
@@ -1752,63 +1566,89 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
-	.vidioc_g_fmt_vbi_cap     = cx8800_vbi_fmt,
-	.vidioc_try_fmt_vbi_cap   = cx8800_vbi_fmt,
-	.vidioc_s_fmt_vbi_cap     = cx8800_vbi_fmt,
 	.vidioc_reqbufs       = vidioc_reqbufs,
 	.vidioc_querybuf      = vidioc_querybuf,
 	.vidioc_qbuf          = vidioc_qbuf,
 	.vidioc_dqbuf         = vidioc_dqbuf,
+	.vidioc_g_std         = vidioc_g_std,
 	.vidioc_s_std         = vidioc_s_std,
 	.vidioc_enum_input    = vidioc_enum_input,
 	.vidioc_g_input       = vidioc_g_input,
 	.vidioc_s_input       = vidioc_s_input,
-	.vidioc_queryctrl     = vidioc_queryctrl,
-	.vidioc_g_ctrl        = vidioc_g_ctrl,
-	.vidioc_s_ctrl        = vidioc_s_ctrl,
 	.vidioc_streamon      = vidioc_streamon,
 	.vidioc_streamoff     = vidioc_streamoff,
 	.vidioc_g_tuner       = vidioc_g_tuner,
 	.vidioc_s_tuner       = vidioc_s_tuner,
 	.vidioc_g_frequency   = vidioc_g_frequency,
 	.vidioc_s_frequency   = vidioc_s_frequency,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident  = vidioc_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_register    = vidioc_g_register,
 	.vidioc_s_register    = vidioc_s_register,
 #endif
 };
 
-static struct video_device cx8800_vbi_template;
-
 static const struct video_device cx8800_video_template = {
 	.name                 = "cx8800-video",
 	.fops                 = &video_fops,
 	.ioctl_ops 	      = &video_ioctl_ops,
 	.tvnorms              = CX88_NORMS,
-	.current_norm         = V4L2_STD_NTSC_M,
+};
+
+static const struct v4l2_ioctl_ops vbi_ioctl_ops = {
+	.vidioc_querycap      = vidioc_querycap,
+	.vidioc_g_fmt_vbi_cap     = cx8800_vbi_fmt,
+	.vidioc_try_fmt_vbi_cap   = cx8800_vbi_fmt,
+	.vidioc_s_fmt_vbi_cap     = cx8800_vbi_fmt,
+	.vidioc_reqbufs       = vidioc_reqbufs,
+	.vidioc_querybuf      = vidioc_querybuf,
+	.vidioc_qbuf          = vidioc_qbuf,
+	.vidioc_dqbuf         = vidioc_dqbuf,
+	.vidioc_g_std         = vidioc_g_std,
+	.vidioc_s_std         = vidioc_s_std,
+	.vidioc_enum_input    = vidioc_enum_input,
+	.vidioc_g_input       = vidioc_g_input,
+	.vidioc_s_input       = vidioc_s_input,
+	.vidioc_streamon      = vidioc_streamon,
+	.vidioc_streamoff     = vidioc_streamoff,
+	.vidioc_g_tuner       = vidioc_g_tuner,
+	.vidioc_s_tuner       = vidioc_s_tuner,
+	.vidioc_g_frequency   = vidioc_g_frequency,
+	.vidioc_s_frequency   = vidioc_s_frequency,
+	.vidioc_g_chip_ident  = vidioc_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register    = vidioc_g_register,
+	.vidioc_s_register    = vidioc_s_register,
+#endif
+};
+
+static const struct video_device cx8800_vbi_template = {
+	.name                 = "cx8800-vbi",
+	.fops                 = &video_fops,
+	.ioctl_ops	      = &vbi_ioctl_ops,
+	.tvnorms              = CX88_NORMS,
 };
 
 static const struct v4l2_file_operations radio_fops =
 {
 	.owner         = THIS_MODULE,
 	.open          = video_open,
+	.poll          = v4l2_ctrl_poll,
 	.release       = video_release,
 	.unlocked_ioctl = video_ioctl2,
 };
 
 static const struct v4l2_ioctl_ops radio_ioctl_ops = {
-	.vidioc_querycap      = radio_querycap,
+	.vidioc_querycap      = vidioc_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_queryctrl     = radio_queryctrl,
-	.vidioc_g_ctrl        = vidioc_g_ctrl,
-	.vidioc_s_ctrl        = vidioc_s_ctrl,
 	.vidioc_g_frequency   = vidioc_g_frequency,
 	.vidioc_s_frequency   = vidioc_s_frequency,
+	.vidioc_subscribe_event      = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event    = v4l2_event_unsubscribe,
+	.vidioc_g_chip_ident  = vidioc_g_chip_ident,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_register    = vidioc_g_register,
 	.vidioc_s_register    = vidioc_s_register,
@@ -1821,6 +1661,14 @@ static const struct video_device cx8800_radio_template = {
 	.ioctl_ops 	      = &radio_ioctl_ops,
 };
 
+static const struct v4l2_ctrl_ops cx8800_ctrl_vid_ops = {
+	.s_ctrl = cx8800_s_vid_ctrl,
+};
+
+static const struct v4l2_ctrl_ops cx8800_ctrl_aud_ops = {
+	.s_ctrl = cx8800_s_aud_ctrl,
+};
+
 /* ----------------------------------------------------------- */
 
 static void cx8800_unregister_video(struct cx8800_dev *dev)
@@ -1853,8 +1701,8 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 {
 	struct cx8800_dev *dev;
 	struct cx88_core *core;
-
 	int err;
+	int i;
 
 	dev = kzalloc(sizeof(*dev),GFP_KERNEL);
 	if (NULL == dev)
@@ -1888,14 +1736,9 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 		goto fail_core;
 	}
 
-	/* Initialize VBI template */
-	memcpy( &cx8800_vbi_template, &cx8800_video_template,
-		sizeof(cx8800_vbi_template) );
-	strcpy(cx8800_vbi_template.name,"cx8800-vbi");
-
 	/* initialize driver struct */
 	spin_lock_init(&dev->slock);
-	core->tvnorm = cx8800_video_template.current_norm;
+	core->tvnorm = V4L2_STD_NTSC_M;
 
 	/* init video dma queues */
 	INIT_LIST_HEAD(&dev->vidq.active);
@@ -1925,6 +1768,35 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 	}
 	cx_set(MO_PCI_INTMSK, core->pci_irqmask);
 
+	for (i = 0; i < CX8800_AUD_CTLS; i++) {
+		const struct cx88_ctrl *cc = &cx8800_aud_ctls[i];
+		struct v4l2_ctrl *vc;
+
+		vc = v4l2_ctrl_new_std(&core->audio_hdl, &cx8800_ctrl_aud_ops,
+			cc->id, cc->minimum, cc->maximum, cc->step, cc->default_value);
+		if (vc == NULL) {
+			err = core->audio_hdl.error;
+			goto fail_core;
+		}
+		vc->priv = (void *)cc;
+	}
+
+	for (i = 0; i < CX8800_VID_CTLS; i++) {
+		const struct cx88_ctrl *cc = &cx8800_vid_ctls[i];
+		struct v4l2_ctrl *vc;
+
+		vc = v4l2_ctrl_new_std(&core->video_hdl, &cx8800_ctrl_vid_ops,
+			cc->id, cc->minimum, cc->maximum, cc->step, cc->default_value);
+		if (vc == NULL) {
+			err = core->video_hdl.error;
+			goto fail_core;
+		}
+		vc->priv = (void *)cc;
+		if (vc->id == V4L2_CID_CHROMA_AGC)
+			core->chroma_agc = vc;
+	}
+	v4l2_ctrl_add_handler(&core->video_hdl, &core->audio_hdl);
+
 	/* load and configure helper modules */
 
 	if (core->board.audio_chip == V4L2_IDENT_WM8775) {
@@ -1942,8 +1814,10 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 
 		sd = v4l2_i2c_new_subdev_board(&core->v4l2_dev, &core->i2c_adap,
 				&wm8775_info, NULL);
-		if (sd != NULL)
+		if (sd != NULL) {
+			core->sd_wm8775 = sd;
 			sd->grp_id = WM8775_GID;
+		}
 	}
 
 	if (core->board.audio_chip == V4L2_IDENT_TVAUDIO) {
@@ -1971,16 +1845,22 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 	/* Sets device info at pci_dev */
 	pci_set_drvdata(pci_dev, dev);
 
+	dev->width   = 320;
+	dev->height  = 240;
+	dev->fmt     = format_by_fourcc(V4L2_PIX_FMT_BGR24);
+
 	/* initial device configuration */
 	mutex_lock(&core->lock);
 	cx88_set_tvnorm(core, core->tvnorm);
-	init_controls(core);
+	v4l2_ctrl_handler_setup(&core->video_hdl);
+	v4l2_ctrl_handler_setup(&core->audio_hdl);
 	cx88_video_mux(core, 0);
 
 	/* register v4l devices */
 	dev->video_dev = cx88_vdev_init(core,dev->pci,
 					&cx8800_video_template,"video");
 	video_set_drvdata(dev->video_dev, dev);
+	dev->video_dev->ctrl_handler = &core->video_hdl;
 	err = video_register_device(dev->video_dev,VFL_TYPE_GRABBER,
 				    video_nr[core->nr]);
 	if (err < 0) {
@@ -2007,6 +1887,7 @@ static int __devinit cx8800_initdev(struct pci_dev *pci_dev,
 		dev->radio_dev = cx88_vdev_init(core,dev->pci,
 						&cx8800_radio_template,"radio");
 		video_set_drvdata(dev->radio_dev, dev);
+		dev->radio_dev->ctrl_handler = &core->audio_hdl;
 		err = video_register_device(dev->radio_dev,VFL_TYPE_RADIO,
 					    radio_nr[core->nr]);
 		if (err < 0) {
diff --git a/drivers/media/video/cx88/cx88.h b/drivers/media/video/cx88/cx88.h
index c9659def2a7..0cae0fd9e16 100644
--- a/drivers/media/video/cx88/cx88.h
+++ b/drivers/media/video/cx88/cx88.h
@@ -26,6 +26,7 @@
 #include <linux/kdev_t.h>
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
 #include <media/tuner.h>
 #include <media/tveeprom.h>
 #include <media/videobuf-dma-sg.h>
@@ -115,15 +116,6 @@ struct cx8800_fmt {
 	u32   cxformat;
 };
 
-struct cx88_ctrl {
-	struct v4l2_queryctrl  v;
-	u32                    off;
-	u32                    reg;
-	u32                    sreg;
-	u32                    mask;
-	u32                    shift;
-};
-
 /* ----------------------------------------------------------- */
 /* SRAM memory management data (see cx88-core.c)               */
 
@@ -359,6 +351,10 @@ struct cx88_core {
 
 	/* config info -- analog */
 	struct v4l2_device 	   v4l2_dev;
+	struct v4l2_ctrl_handler   video_hdl;
+	struct v4l2_ctrl	   *chroma_agc;
+	struct v4l2_ctrl_handler   audio_hdl;
+	struct v4l2_subdev	   *sd_wm8775;
 	struct i2c_client 	   *i2c_rtc;
 	unsigned int               boardnr;
 	struct cx88_board	   board;
@@ -409,8 +405,6 @@ static inline struct cx88_core *to_core(struct v4l2_device *v4l2_dev)
 	return container_of(v4l2_dev, struct cx88_core, v4l2_dev);
 }
 
-#define WM8775_GID	(1 << 0)
-
 #define call_hw(core, grpid, o, f, args...) \
 	do {							\
 		if (!core->i2c_rc) {				\
@@ -424,6 +418,36 @@ static inline struct cx88_core *to_core(struct v4l2_device *v4l2_dev)
 
 #define call_all(core, o, f, args...) call_hw(core, 0, o, f, ##args)
 
+#define WM8775_GID      (1 << 0)
+
+#define wm8775_s_ctrl(core, id, val) \
+	do {									\
+		struct v4l2_ctrl *ctrl_ =					\
+			v4l2_ctrl_find(core->sd_wm8775->ctrl_handler, id);	\
+		if (ctrl_ && !core->i2c_rc) {					\
+			if (core->gate_ctrl)					\
+				core->gate_ctrl(core, 1);			\
+			v4l2_ctrl_s_ctrl(ctrl_, val);				\
+			if (core->gate_ctrl)					\
+				core->gate_ctrl(core, 0);			\
+		}								\
+	} while (0)
+
+#define wm8775_g_ctrl(core, id) \
+	({									\
+		struct v4l2_ctrl *ctrl_ =					\
+			v4l2_ctrl_find(core->sd_wm8775->ctrl_handler, id);	\
+		s32 val = 0;							\
+		if (ctrl_ && !core->i2c_rc) {					\
+			if (core->gate_ctrl)					\
+				core->gate_ctrl(core, 1);			\
+			val = v4l2_ctrl_g_ctrl(ctrl_);				\
+			if (core->gate_ctrl)					\
+				core->gate_ctrl(core, 0);			\
+		}								\
+		val;								\
+	})
+
 struct cx8800_dev;
 struct cx8802_dev;
 
@@ -431,19 +455,11 @@ struct cx8802_dev;
 /* function 0: video stuff                                     */
 
 struct cx8800_fh {
+	struct v4l2_fh		   fh;
 	struct cx8800_dev          *dev;
-	enum v4l2_buf_type         type;
-	int                        radio;
 	unsigned int               resources;
 
-	/* video overlay */
-	struct v4l2_window         win;
-	struct v4l2_clip           *clips;
-	unsigned int               nclips;
-
 	/* video capture */
-	const struct cx8800_fmt    *fmt;
-	unsigned int               width,height;
 	struct videobuf_queue      vidq;
 
 	/* vbi capture */
@@ -468,6 +484,8 @@ struct cx8800_dev {
 	struct pci_dev             *pci;
 	unsigned char              pci_rev,pci_lat;
 
+	const struct cx8800_fmt    *fmt;
+	unsigned int               width, height;
 
 	/* capture queues */
 	struct cx88_dmaqueue       vidq;
@@ -488,6 +506,7 @@ struct cx8800_dev {
 /* function 2: mpeg stuff                                      */
 
 struct cx8802_fh {
+	struct v4l2_fh		   fh;
 	struct cx8802_dev          *dev;
 	struct videobuf_queue      mpegq;
 };
@@ -552,7 +571,7 @@ struct cx8802_dev {
 	unsigned char              mpeg_active; /* nonzero if mpeg encoder is active */
 
 	/* mpeg params */
-	struct cx2341x_mpeg_params params;
+	struct cx2341x_handler     cxhdl;
 #endif
 
 #if defined(CONFIG_VIDEO_CX88_DVB) || defined(CONFIG_VIDEO_CX88_DVB_MODULE)
@@ -722,11 +741,8 @@ void cx8802_cancel_buffers(struct cx8802_dev *dev);
 
 /* ----------------------------------------------------------- */
 /* cx88-video.c*/
-extern const u32 cx88_user_ctrls[];
-extern int cx8800_ctrl_query(struct cx88_core *core,
-			     struct v4l2_queryctrl *qctrl);
 int cx88_enum_input (struct cx88_core  *core,struct v4l2_input *i);
 int cx88_set_freq (struct cx88_core  *core,struct v4l2_frequency *f);
-int cx88_get_control(struct cx88_core *core, struct v4l2_control *ctl);
-int cx88_set_control(struct cx88_core *core, struct v4l2_control *ctl);
 int cx88_video_mux(struct cx88_core *core, unsigned int input);
+void cx88_querycap(struct file *file, struct cx88_core *core,
+		struct v4l2_capability *cap);
diff --git a/drivers/media/video/davinci/Kconfig b/drivers/media/video/davinci/Kconfig
index 9337b5605c9..52c5ca68cb3 100644
--- a/drivers/media/video/davinci/Kconfig
+++ b/drivers/media/video/davinci/Kconfig
@@ -1,30 +1,34 @@
-config DISPLAY_DAVINCI_DM646X_EVM
-	tristate "DM646x EVM Video Display"
-	depends on VIDEO_DEV && MACH_DAVINCI_DM6467_EVM
-	select VIDEOBUF_DMA_CONTIG
+config VIDEO_DAVINCI_VPIF_DISPLAY
+	tristate "DM646x/DA850/OMAPL138 EVM Video Display"
+	depends on VIDEO_DEV && (MACH_DAVINCI_DM6467_EVM || MACH_DAVINCI_DA850_EVM)
+	select VIDEOBUF2_DMA_CONTIG
 	select VIDEO_DAVINCI_VPIF
-	select VIDEO_ADV7343
-	select VIDEO_THS7303
+	select VIDEO_ADV7343 if VIDEO_HELPER_CHIPS_AUTO
+	select VIDEO_THS7303 if VIDEO_HELPER_CHIPS_AUTO
 	help
-	  Support for DM6467 based display device.
+	  Enables Davinci VPIF module used for display devices.
+	  This module is common for following DM6467/DA850/OMAPL138
+	  based display devices.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called vpif_display.
 
-config CAPTURE_DAVINCI_DM646X_EVM
-	tristate "DM646x EVM Video Capture"
-	depends on VIDEO_DEV && MACH_DAVINCI_DM6467_EVM
-	select VIDEOBUF_DMA_CONTIG
+config VIDEO_DAVINCI_VPIF_CAPTURE
+	tristate "DM646x/DA850/OMAPL138 EVM Video Capture"
+	depends on VIDEO_DEV && (MACH_DAVINCI_DM6467_EVM || MACH_DAVINCI_DA850_EVM)
+	select VIDEOBUF2_DMA_CONTIG
 	select VIDEO_DAVINCI_VPIF
 	help
-	  Support for DM6467 based capture device.
+	  Enables Davinci VPIF module used for captur devices.
+	  This module is common for following DM6467/DA850/OMAPL138
+	  based capture devices.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called vpif_capture.
 
 config VIDEO_DAVINCI_VPIF
 	tristate "DaVinci VPIF Driver"
-	depends on DISPLAY_DAVINCI_DM646X_EVM
+	depends on VIDEO_DAVINCI_VPIF_DISPLAY || VIDEO_DAVINCI_VPIF_CAPTURE
 	help
 	  Support for DaVinci VPIF Driver.
 
diff --git a/drivers/media/video/davinci/Makefile b/drivers/media/video/davinci/Makefile
index ae7dafb689a..74ed92d0925 100644
--- a/drivers/media/video/davinci/Makefile
+++ b/drivers/media/video/davinci/Makefile
@@ -5,10 +5,10 @@
 # VPIF
 obj-$(CONFIG_VIDEO_DAVINCI_VPIF) += vpif.o
 
-#DM646x EVM Display driver
-obj-$(CONFIG_DISPLAY_DAVINCI_DM646X_EVM) += vpif_display.o
-#DM646x EVM Capture driver
-obj-$(CONFIG_CAPTURE_DAVINCI_DM646X_EVM) += vpif_capture.o
+#VPIF Display driver
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_DISPLAY) += vpif_display.o
+#VPIF Capture driver
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_CAPTURE) += vpif_capture.o
 
 # Capture: DM6446 and DM355
 obj-$(CONFIG_VIDEO_VPSS_SYSTEM) += vpss.o
diff --git a/drivers/media/video/davinci/vpbe_display.c b/drivers/media/video/davinci/vpbe_display.c
index e106b72810a..6fe7034bea7 100644
--- a/drivers/media/video/davinci/vpbe_display.c
+++ b/drivers/media/video/davinci/vpbe_display.c
@@ -1083,7 +1083,7 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
 	}
 
 	/* Set the given standard in the encoder */
-	if (NULL != vpbe_dev->ops.s_dv_preset)
+	if (!vpbe_dev->ops.s_dv_preset)
 		return -EINVAL;
 
 	ret = vpbe_dev->ops.s_dv_preset(vpbe_dev, preset);
@@ -1517,6 +1517,8 @@ static int vpbe_display_g_register(struct file *file, void *priv,
 			struct v4l2_dbg_register *reg)
 {
 	struct v4l2_dbg_match *match = &reg->match;
+	struct vpbe_fh *fh = file->private_data;
+	struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
 
 	if (match->type >= 2) {
 		v4l2_subdev_call(vpbe_dev->venc,
diff --git a/drivers/media/video/davinci/vpif.c b/drivers/media/video/davinci/vpif.c
index af9680273ff..b3637aff8fe 100644
--- a/drivers/media/video/davinci/vpif.c
+++ b/drivers/media/video/davinci/vpif.c
@@ -1,5 +1,5 @@
 /*
- * vpif - DM646x Video Port Interface driver
+ * vpif - Video Port Interface driver
  * VPIF is a receiver and transmitter for video data. It has two channels(0, 1)
  * that receiveing video byte stream and two channels(2, 3) for video output.
  * The hardware supports SDTV, HDTV formats, raw data capture.
@@ -23,6 +23,8 @@
 #include <linux/spinlock.h>
 #include <linux/kernel.h>
 #include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 #include <mach/hardware.h>
 
 #include "vpif.h"
@@ -40,6 +42,7 @@ static struct resource	*res;
 spinlock_t vpif_lock;
 
 void __iomem *vpif_base;
+struct clk *vpif_clk;
 
 /**
  * ch_params: video standard configuration parameters for vpif
@@ -346,7 +349,7 @@ static void config_vpif_params(struct vpif_params *vpifparams,
 
 			value = regr(reg);
 			/* Set data width */
-			value &= ((~(unsigned int)(0x3)) <<
+			value &= ~(0x3u <<
 					VPIF_CH_DATA_WIDTH_BIT);
 			value |= ((vpifparams->params.data_sz) <<
 						     VPIF_CH_DATA_WIDTH_BIT);
@@ -434,10 +437,19 @@ static int __init vpif_probe(struct platform_device *pdev)
 		goto fail;
 	}
 
+	vpif_clk = clk_get(&pdev->dev, "vpif");
+	if (IS_ERR(vpif_clk)) {
+		status = PTR_ERR(vpif_clk);
+		goto clk_fail;
+	}
+	clk_enable(vpif_clk);
+
 	spin_lock_init(&vpif_lock);
 	dev_info(&pdev->dev, "vpif probe success\n");
 	return 0;
 
+clk_fail:
+	iounmap(vpif_base);
 fail:
 	release_mem_region(res->start, res_len);
 	return status;
@@ -445,15 +457,44 @@ fail:
 
 static int __devexit vpif_remove(struct platform_device *pdev)
 {
+	if (vpif_clk) {
+		clk_disable(vpif_clk);
+		clk_put(vpif_clk);
+	}
+
 	iounmap(vpif_base);
 	release_mem_region(res->start, res_len);
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int vpif_suspend(struct device *dev)
+{
+	clk_disable(vpif_clk);
+	return 0;
+}
+
+static int vpif_resume(struct device *dev)
+{
+	clk_enable(vpif_clk);
+	return 0;
+}
+
+static const struct dev_pm_ops vpif_pm = {
+	.suspend        = vpif_suspend,
+	.resume         = vpif_resume,
+};
+
+#define vpif_pm_ops (&vpif_pm)
+#else
+#define vpif_pm_ops NULL
+#endif
+
 static struct platform_driver vpif_driver = {
 	.driver = {
 		.name	= "vpif",
 		.owner = THIS_MODULE,
+		.pm	= vpif_pm_ops,
 	},
 	.remove = __devexit_p(vpif_remove),
 	.probe = vpif_probe,
diff --git a/drivers/media/video/davinci/vpif.h b/drivers/media/video/davinci/vpif.h
index 8bcac65f929..c2ce4d97c27 100644
--- a/drivers/media/video/davinci/vpif.h
+++ b/drivers/media/video/davinci/vpif.h
@@ -211,6 +211,12 @@ static inline void vpif_clr_bit(u32 reg, u32 bit)
 #define VPIF_CH3_INT_CTRL_SHIFT	(6)
 #define VPIF_CH_INT_CTRL_SHIFT	(6)
 
+#define VPIF_CH2_CLIP_ANC_EN	14
+#define VPIF_CH2_CLIP_ACTIVE_EN	13
+
+#define VPIF_CH3_CLIP_ANC_EN	14
+#define VPIF_CH3_CLIP_ACTIVE_EN	13
+
 /* enabled interrupt on both the fields on vpid_ch0_ctrl register */
 #define channel0_intr_assert()	(regw((regr(VPIF_CH0_CTRL)|\
 	(VPIF_INT_BOTH << VPIF_CH0_INT_CTRL_SHIFT)), VPIF_CH0_CTRL))
@@ -515,6 +521,30 @@ static inline void channel3_raw_enable(int enable, u8 index)
 		vpif_clr_bit(VPIF_CH3_CTRL, mask);
 }
 
+/* function to enable clipping (for both active and blanking regions) on ch 2 */
+static inline void channel2_clipping_enable(int enable)
+{
+	if (enable) {
+		vpif_set_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ANC_EN);
+		vpif_set_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ACTIVE_EN);
+	} else {
+		vpif_clr_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ANC_EN);
+		vpif_clr_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ACTIVE_EN);
+	}
+}
+
+/* function to enable clipping (for both active and blanking regions) on ch 2 */
+static inline void channel3_clipping_enable(int enable)
+{
+	if (enable) {
+		vpif_set_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ANC_EN);
+		vpif_set_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ACTIVE_EN);
+	} else {
+		vpif_clr_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ANC_EN);
+		vpif_clr_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ACTIVE_EN);
+	}
+}
+
 /* inline function to set buffer addresses in case of Y/C non mux mode */
 static inline void ch2_set_videobuf_addr_yc_nmux(unsigned long top_strt_luma,
 						 unsigned long btm_strt_luma,
@@ -569,6 +599,21 @@ static inline void ch3_set_vbi_addr(unsigned long top_strt_luma,
 	regw(btm_strt_luma, VPIF_CH3_BTM_STRT_ADD_VANC);
 }
 
+static inline int vpif_intr_status(int channel)
+{
+	int status = 0;
+	int mask;
+
+	if (channel < 0 || channel > 3)
+		return 0;
+
+	mask = 1 << channel;
+	status = regr(VPIF_STATUS) & mask;
+	regw(status, VPIF_STATUS_CLR);
+
+	return status;
+}
+
 #define VPIF_MAX_NAME	(30)
 
 /* This structure will store size parameters as per the mode selected by user */
diff --git a/drivers/media/video/davinci/vpif_capture.c b/drivers/media/video/davinci/vpif_capture.c
index 96046957bf2..266025e5d81 100644
--- a/drivers/media/video/davinci/vpif_capture.c
+++ b/drivers/media/video/davinci/vpif_capture.c
@@ -80,108 +80,45 @@ static struct vpif_config_params config_params = {
 /* global variables */
 static struct vpif_device vpif_obj = { {NULL} };
 static struct device *vpif_dev;
-
-/**
- * vpif_uservirt_to_phys : translate user/virtual address to phy address
- * @virtp: user/virtual address
- *
- * This inline function is used to convert user space virtual address to
- * physical address.
- */
-static inline u32 vpif_uservirt_to_phys(u32 virtp)
-{
-	unsigned long physp = 0;
-	struct mm_struct *mm = current->mm;
-	struct vm_area_struct *vma;
-
-	vma = find_vma(mm, virtp);
-
-	/* For kernel direct-mapped memory, take the easy way */
-	if (virtp >= PAGE_OFFSET)
-		physp = virt_to_phys((void *)virtp);
-	else if (vma && (vma->vm_flags & VM_IO) && (vma->vm_pgoff))
-		/**
-		 * this will catch, kernel-allocated, mmaped-to-usermode
-		 * addresses
-		 */
-		physp = (vma->vm_pgoff << PAGE_SHIFT) + (virtp - vma->vm_start);
-	else {
-		/* otherwise, use get_user_pages() for general userland pages */
-		int res, nr_pages = 1;
-			struct page *pages;
-
-		down_read(&current->mm->mmap_sem);
-
-		res = get_user_pages(current, current->mm,
-				     virtp, nr_pages, 1, 0, &pages, NULL);
-		up_read(&current->mm->mmap_sem);
-
-		if (res == nr_pages)
-			physp = __pa(page_address(&pages[0]) +
-				     (virtp & ~PAGE_MASK));
-		else {
-			vpif_err("get_user_pages failed\n");
-			return 0;
-		}
-	}
-	return physp;
-}
+static void vpif_calculate_offsets(struct channel_obj *ch);
+static void vpif_config_addr(struct channel_obj *ch, int muxmode);
 
 /**
  * buffer_prepare :  callback function for buffer prepare
- * @q : buffer queue ptr
- * @vb: ptr to video buffer
- * @field: field info
+ * @vb: ptr to vb2_buffer
  *
- * This is the callback function for buffer prepare when videobuf_qbuf()
+ * This is the callback function for buffer prepare when vb2_qbuf()
  * function is called. The buffer is prepared and user space virtual address
  * or user address is converted into  physical address
  */
-static int vpif_buffer_prepare(struct videobuf_queue *q,
-			       struct videobuf_buffer *vb,
-			       enum v4l2_field field)
+static int vpif_buffer_prepare(struct vb2_buffer *vb)
 {
 	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_queue *q = vb->vb2_queue;
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
 	unsigned long addr;
 
-
 	vpif_dbg(2, debug, "vpif_buffer_prepare\n");
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
-	/* If buffer is not initialized, initialize it */
-	if (VIDEOBUF_NEEDS_INIT == vb->state) {
-		vb->width = common->width;
-		vb->height = common->height;
-		vb->size = vb->width * vb->height;
-		vb->field = field;
-	}
-	vb->state = VIDEOBUF_PREPARED;
-	/**
-	 * if user pointer memory mechanism is used, get the physical
-	 * address of the buffer
-	 */
-	if (V4L2_MEMORY_USERPTR == common->memory) {
-		if (0 == vb->baddr) {
-			vpif_dbg(1, debug, "buffer address is 0\n");
-			return -EINVAL;
-
-		}
-		vb->boff = vpif_uservirt_to_phys(vb->baddr);
-		if (!IS_ALIGNED(vb->boff, 8))
+	if (vb->state != VB2_BUF_STATE_ACTIVE &&
+		vb->state != VB2_BUF_STATE_PREPARED) {
+		vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
+		if (vb2_plane_vaddr(vb, 0) &&
+		vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
 			goto exit;
-	}
+		addr = vb2_dma_contig_plane_dma_addr(vb, 0);
 
-	addr = vb->boff;
-	if (q->streaming) {
-		if (!IS_ALIGNED((addr + common->ytop_off), 8) ||
-		    !IS_ALIGNED((addr + common->ybtm_off), 8) ||
-		    !IS_ALIGNED((addr + common->ctop_off), 8) ||
-		    !IS_ALIGNED((addr + common->cbtm_off), 8))
-			goto exit;
+		if (q->streaming) {
+			if (!IS_ALIGNED((addr + common->ytop_off), 8) ||
+				!IS_ALIGNED((addr + common->ybtm_off), 8) ||
+				!IS_ALIGNED((addr + common->ctop_off), 8) ||
+				!IS_ALIGNED((addr + common->cbtm_off), 8))
+				goto exit;
+		}
 	}
 	return 0;
 exit:
@@ -190,49 +127,79 @@ exit:
 }
 
 /**
- * vpif_buffer_setup : Callback function for buffer setup.
- * @q: buffer queue ptr
- * @count: number of buffers
- * @size: size of the buffer
+ * vpif_buffer_queue_setup : Callback function for buffer setup.
+ * @vq: vb2_queue ptr
+ * @fmt: v4l2 format
+ * @nbuffers: ptr to number of buffers requested by application
+ * @nplanes:: contains number of distinct video planes needed to hold a frame
+ * @sizes[]: contains the size (in bytes) of each plane.
+ * @alloc_ctxs: ptr to allocation context
  *
  * This callback function is called when reqbuf() is called to adjust
  * the buffer count and buffer size
  */
-static int vpif_buffer_setup(struct videobuf_queue *q, unsigned int *count,
-			     unsigned int *size)
+static int vpif_buffer_queue_setup(struct vb2_queue *vq,
+				const struct v4l2_format *fmt,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
 {
 	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
+	unsigned long size;
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
 	vpif_dbg(2, debug, "vpif_buffer_setup\n");
 
 	/* If memory type is not mmap, return */
-	if (V4L2_MEMORY_MMAP != common->memory)
-		return 0;
+	if (V4L2_MEMORY_MMAP == common->memory) {
+		/* Calculate the size of the buffer */
+		size = config_params.channel_bufsize[ch->channel_id];
+		/*
+		 * Checking if the buffer size exceeds the available buffer
+		 * ycmux_mode = 0 means 1 channel mode HD and
+		 * ycmux_mode = 1 means 2 channels mode SD
+		 */
+		if (ch->vpifparams.std_info.ycmux_mode == 0) {
+			if (config_params.video_limit[ch->channel_id])
+				while (size * *nbuffers >
+					(config_params.video_limit[0]
+						+ config_params.video_limit[1]))
+					(*nbuffers)--;
+		} else {
+			if (config_params.video_limit[ch->channel_id])
+				while (size * *nbuffers >
+				config_params.video_limit[ch->channel_id])
+					(*nbuffers)--;
+		}
+
+	} else {
+		size = common->fmt.fmt.pix.sizeimage;
+	}
+
+	if (*nbuffers < config_params.min_numbuffers)
+		*nbuffers = config_params.min_numbuffers;
 
-	/* Calculate the size of the buffer */
-	*size = config_params.channel_bufsize[ch->channel_id];
+	*nplanes = 1;
+	sizes[0] = size;
+	alloc_ctxs[0] = common->alloc_ctx;
 
-	if (*count < config_params.min_numbuffers)
-		*count = config_params.min_numbuffers;
 	return 0;
 }
 
 /**
  * vpif_buffer_queue : Callback function to add buffer to DMA queue
- * @q: ptr to videobuf_queue
- * @vb: ptr to videobuf_buffer
+ * @vb: ptr to vb2_buffer
  */
-static void vpif_buffer_queue(struct videobuf_queue *q,
-			      struct videobuf_buffer *vb)
+static void vpif_buffer_queue(struct vb2_buffer *vb)
 {
 	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
 	struct channel_obj *ch = fh->channel;
+	struct vpif_cap_buffer *buf = container_of(vb,
+				struct vpif_cap_buffer, vb);
 	struct common_obj *common;
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
@@ -240,43 +207,189 @@ static void vpif_buffer_queue(struct videobuf_queue *q,
 	vpif_dbg(2, debug, "vpif_buffer_queue\n");
 
 	/* add the buffer to the DMA queue */
-	list_add_tail(&vb->queue, &common->dma_queue);
-	/* Change state of the buffer */
-	vb->state = VIDEOBUF_QUEUED;
+	list_add_tail(&buf->list, &common->dma_queue);
 }
 
 /**
- * vpif_buffer_release : Callback function to free buffer
- * @q: buffer queue ptr
- * @vb: ptr to video buffer
+ * vpif_buf_cleanup : Callback function to free buffer
+ * @vb: ptr to vb2_buffer
  *
- * This function is called from the videobuf layer to free memory
+ * This function is called from the videobuf2 layer to free memory
  * allocated to  the buffers
  */
-static void vpif_buffer_release(struct videobuf_queue *q,
-				struct videobuf_buffer *vb)
+static void vpif_buf_cleanup(struct vb2_buffer *vb)
 {
 	/* Get the file handle object and channel object */
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpif_cap_buffer *buf = container_of(vb,
+					struct vpif_cap_buffer, vb);
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
+	unsigned long flags;
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
-	videobuf_dma_contig_free(q, vb);
-	vb->state = VIDEOBUF_NEEDS_INIT;
+	spin_lock_irqsave(&common->irqlock, flags);
+	if (vb->state == VB2_BUF_STATE_ACTIVE)
+		list_del_init(&buf->list);
+	spin_unlock_irqrestore(&common->irqlock, flags);
+
 }
 
-static struct videobuf_queue_ops video_qops = {
-	.buf_setup = vpif_buffer_setup,
-	.buf_prepare = vpif_buffer_prepare,
-	.buf_queue = vpif_buffer_queue,
-	.buf_release = vpif_buffer_release,
-};
+static void vpif_wait_prepare(struct vb2_queue *vq)
+{
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+	mutex_unlock(&common->lock);
+}
+
+static void vpif_wait_finish(struct vb2_queue *vq)
+{
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+	mutex_lock(&common->lock);
+}
+
+static int vpif_buffer_init(struct vb2_buffer *vb)
+{
+	struct vpif_cap_buffer *buf = container_of(vb,
+					struct vpif_cap_buffer, vb);
+
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
+}
 
 static u8 channel_first_int[VPIF_NUMBER_OF_OBJECTS][2] =
 	{ {1, 1} };
 
+static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vpif_capture_config *vpif_config_data =
+					vpif_dev->platform_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_params *vpif = &ch->vpifparams;
+	unsigned long addr = 0;
+	int ret;
+
+		/* If buffer queue is empty, return error */
+	if (list_empty(&common->dma_queue)) {
+		vpif_dbg(1, debug, "buffer queue is empty\n");
+		return -EIO;
+	}
+
+	/* Get the next frame from the buffer queue */
+	common->cur_frm = common->next_frm = list_entry(common->dma_queue.next,
+				    struct vpif_cap_buffer, list);
+	/* Remove buffer from the buffer queue */
+	list_del(&common->cur_frm->list);
+	/* Mark state of the current frame to active */
+	common->cur_frm->vb.state = VB2_BUF_STATE_ACTIVE;
+	/* Initialize field_id and started member */
+	ch->field_id = 0;
+	common->started = 1;
+	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb, 0);
+
+	/* Calculate the offset for Y and C data in the buffer */
+	vpif_calculate_offsets(ch);
+
+	if ((vpif->std_info.frm_fmt &&
+	    ((common->fmt.fmt.pix.field != V4L2_FIELD_NONE) &&
+	     (common->fmt.fmt.pix.field != V4L2_FIELD_ANY))) ||
+	    (!vpif->std_info.frm_fmt &&
+	     (common->fmt.fmt.pix.field == V4L2_FIELD_NONE))) {
+		vpif_dbg(1, debug, "conflict in field format and std format\n");
+		return -EINVAL;
+	}
+
+	/* configure 1 or 2 channel mode */
+	ret = vpif_config_data->setup_input_channel_mode
+					(vpif->std_info.ycmux_mode);
+
+	if (ret < 0) {
+		vpif_dbg(1, debug, "can't set vpif channel mode\n");
+		return ret;
+	}
+
+	/* Call vpif_set_params function to set the parameters and addresses */
+	ret = vpif_set_video_params(vpif, ch->channel_id);
+
+	if (ret < 0) {
+		vpif_dbg(1, debug, "can't set video params\n");
+		return ret;
+	}
+
+	common->started = ret;
+	vpif_config_addr(ch, ret);
+
+	common->set_addr(addr + common->ytop_off,
+			 addr + common->ybtm_off,
+			 addr + common->ctop_off,
+			 addr + common->cbtm_off);
+
+	/**
+	 * Set interrupt for both the fields in VPIF Register enable channel in
+	 * VPIF register
+	 */
+	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id)) {
+		channel0_intr_assert();
+		channel0_intr_enable(1);
+		enable_channel0(1);
+	}
+	if ((VPIF_CHANNEL1_VIDEO == ch->channel_id) ||
+	    (common->started == 2)) {
+		channel1_intr_assert();
+		channel1_intr_enable(1);
+		enable_channel1(1);
+	}
+	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
+
+	return 0;
+}
+
+/* abort streaming and wait for last buffer */
+static int vpif_stop_streaming(struct vb2_queue *vq)
+{
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common;
+
+	if (!vb2_is_streaming(vq))
+		return 0;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* release all active buffers */
+	while (!list_empty(&common->dma_queue)) {
+		common->next_frm = list_entry(common->dma_queue.next,
+						struct vpif_cap_buffer, list);
+		list_del(&common->next_frm->list);
+		vb2_buffer_done(&common->next_frm->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	return 0;
+}
+
+static struct vb2_ops video_qops = {
+	.queue_setup		= vpif_buffer_queue_setup,
+	.wait_prepare		= vpif_wait_prepare,
+	.wait_finish		= vpif_wait_finish,
+	.buf_init		= vpif_buffer_init,
+	.buf_prepare		= vpif_buffer_prepare,
+	.start_streaming	= vpif_start_streaming,
+	.stop_streaming		= vpif_stop_streaming,
+	.buf_cleanup		= vpif_buf_cleanup,
+	.buf_queue		= vpif_buffer_queue,
+};
+
 /**
  * vpif_process_buffer_complete: process a completed buffer
  * @common: ptr to common channel object
@@ -287,9 +400,9 @@ static u8 channel_first_int[VPIF_NUMBER_OF_OBJECTS][2] =
  */
 static void vpif_process_buffer_complete(struct common_obj *common)
 {
-	do_gettimeofday(&common->cur_frm->ts);
-	common->cur_frm->state = VIDEOBUF_DONE;
-	wake_up_interruptible(&common->cur_frm->done);
+	do_gettimeofday(&common->cur_frm->vb.v4l2_buf.timestamp);
+	vb2_buffer_done(&common->cur_frm->vb,
+					    VB2_BUF_STATE_DONE);
 	/* Make curFrm pointing to nextFrm */
 	common->cur_frm = common->next_frm;
 }
@@ -307,14 +420,11 @@ static void vpif_schedule_next_buffer(struct common_obj *common)
 	unsigned long addr = 0;
 
 	common->next_frm = list_entry(common->dma_queue.next,
-				     struct videobuf_buffer, queue);
+				     struct vpif_cap_buffer, list);
 	/* Remove that buffer from the buffer queue */
-	list_del(&common->next_frm->queue);
-	common->next_frm->state = VIDEOBUF_ACTIVE;
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		addr = common->next_frm->boff;
-	else
-		addr = videobuf_to_dma_contig(common->next_frm);
+	list_del(&common->next_frm->list);
+	common->next_frm->vb.state = VB2_BUF_STATE_ACTIVE;
+	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb, 0);
 
 	/* Set top and bottom field addresses in VPIF registers */
 	common->set_addr(addr + common->ytop_off,
@@ -341,6 +451,9 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 	int fid = -1, i;
 
 	channel_id = *(int *)(dev_id);
+	if (!vpif_intr_status(channel_id))
+		return IRQ_NONE;
+
 	ch = dev->dev[channel_id];
 
 	field = ch->common[VPIF_VIDEO_INDEX].fmt.fmt.pix.field;
@@ -485,10 +598,7 @@ static void vpif_calculate_offsets(struct channel_obj *ch)
 	} else
 		vid_ch->buf_field = common->fmt.fmt.pix.field;
 
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		sizeimage = common->fmt.fmt.pix.sizeimage;
-	else
-		sizeimage = config_params.channel_bufsize[ch->channel_id];
+	sizeimage = common->fmt.fmt.pix.sizeimage;
 
 	hpitch = common->fmt.fmt.pix.bytesperline;
 	vpitch = sizeimage / (hpitch * 2);
@@ -640,10 +750,7 @@ static int vpif_check_format(struct channel_obj *ch,
 		hpitch = vpif_params->std_info.width;
 	}
 
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		sizeimage = pixfmt->sizeimage;
-	else
-		sizeimage = config_params.channel_bufsize[ch->channel_id];
+	sizeimage = pixfmt->sizeimage;
 
 	vpitch = sizeimage / (hpitch * 2);
 
@@ -703,7 +810,7 @@ static void vpif_config_addr(struct channel_obj *ch, int muxmode)
 }
 
 /**
- * vpfe_mmap : It is used to map kernel space buffers into user spaces
+ * vpif_mmap : It is used to map kernel space buffers into user spaces
  * @filep: file pointer
  * @vma: ptr to vm_area_struct
  */
@@ -716,7 +823,7 @@ static int vpif_mmap(struct file *filep, struct vm_area_struct *vma)
 
 	vpif_dbg(2, debug, "vpif_mmap\n");
 
-	return videobuf_mmap_mapper(&common->buffer_queue, vma);
+	return vb2_mmap(&common->buffer_queue, vma);
 }
 
 /**
@@ -733,7 +840,7 @@ static unsigned int vpif_poll(struct file *filep, poll_table * wait)
 	vpif_dbg(2, debug, "vpif_poll\n");
 
 	if (common->started)
-		return videobuf_poll_stream(filep, &common->buffer_queue, wait);
+		return vb2_poll(&common->buffer_queue, filep, wait);
 	return 0;
 }
 
@@ -812,7 +919,7 @@ static int vpif_open(struct file *filep)
  * vpif_release : function to clean up file close
  * @filep: file pointer
  *
- * This function deletes buffer queue, frees the buffers and the vpfe file
+ * This function deletes buffer queue, frees the buffers and the vpif file
  * handle
  */
 static int vpif_release(struct file *filep)
@@ -841,8 +948,8 @@ static int vpif_release(struct file *filep)
 		}
 		common->started = 0;
 		/* Free buffers allocated */
-		videobuf_queue_cancel(&common->buffer_queue);
-		videobuf_mmap_free(&common->buffer_queue);
+		vb2_queue_release(&common->buffer_queue);
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
 	}
 
 	/* Decrement channel usrs counter */
@@ -872,6 +979,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
 	u8 index = 0;
+	struct vb2_queue *q;
 
 	vpif_dbg(2, debug, "vpif_reqbufs\n");
 
@@ -887,7 +995,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 		}
 	}
 
-	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != reqbuf->type)
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != reqbuf->type || !vpif_dev)
 		return -EINVAL;
 
 	index = VPIF_VIDEO_INDEX;
@@ -897,14 +1005,21 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	if (0 != common->io_usrs)
 		return -EBUSY;
 
-	/* Initialize videobuf queue as per the buffer type */
-	videobuf_queue_dma_contig_init(&common->buffer_queue,
-					    &video_qops, NULL,
-					    &common->irqlock,
-					    reqbuf->type,
-					    common->fmt.fmt.pix.field,
-					    sizeof(struct videobuf_buffer), fh,
-					    &common->lock);
+	/* Initialize videobuf2 queue as per the buffer type */
+	common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
+	if (!common->alloc_ctx) {
+		vpif_err("Failed to get the context\n");
+		return -EINVAL;
+	}
+	q = &common->buffer_queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = fh;
+	q->ops = &video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct vpif_cap_buffer);
+
+	vb2_queue_init(q);
 
 	/* Set io allowed member of file handle to TRUE */
 	fh->io_allowed[index] = 1;
@@ -915,7 +1030,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	INIT_LIST_HEAD(&common->dma_queue);
 
 	/* Allocate buffers */
-	return videobuf_reqbufs(&common->buffer_queue, reqbuf);
+	return vb2_reqbufs(&common->buffer_queue, reqbuf);
 }
 
 /**
@@ -941,7 +1056,7 @@ static int vpif_querybuf(struct file *file, void *priv,
 		return -EINVAL;
 	}
 
-	return videobuf_querybuf(&common->buffer_queue, buf);
+	return vb2_querybuf(&common->buffer_queue, buf);
 }
 
 /**
@@ -957,10 +1072,6 @@ static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct v4l2_buffer tbuf = *buf;
-	struct videobuf_buffer *buf1;
-	unsigned long addr = 0;
-	unsigned long flags;
-	int ret = 0;
 
 	vpif_dbg(2, debug, "vpif_qbuf\n");
 
@@ -970,76 +1081,11 @@ static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 	}
 
 	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
-		vpif_err("fh io not allowed \n");
+		vpif_err("fh io not allowed\n");
 		return -EACCES;
 	}
 
-	if (!(list_empty(&common->dma_queue)) ||
-	    (common->cur_frm != common->next_frm) ||
-	    !common->started ||
-	    (common->started && (0 == ch->field_id)))
-		return videobuf_qbuf(&common->buffer_queue, buf);
-
-	/* bufferqueue is empty store buffer address in VPIF registers */
-	mutex_lock(&common->buffer_queue.vb_lock);
-	buf1 = common->buffer_queue.bufs[tbuf.index];
-
-	if ((buf1->state == VIDEOBUF_QUEUED) ||
-	    (buf1->state == VIDEOBUF_ACTIVE)) {
-		vpif_err("invalid state\n");
-		goto qbuf_exit;
-	}
-
-	switch (buf1->memory) {
-	case V4L2_MEMORY_MMAP:
-		if (buf1->baddr == 0)
-			goto qbuf_exit;
-		break;
-
-	case V4L2_MEMORY_USERPTR:
-		if (tbuf.length < buf1->bsize)
-			goto qbuf_exit;
-
-		if ((VIDEOBUF_NEEDS_INIT != buf1->state)
-			    && (buf1->baddr != tbuf.m.userptr)) {
-			vpif_buffer_release(&common->buffer_queue, buf1);
-			buf1->baddr = tbuf.m.userptr;
-		}
-		break;
-
-	default:
-		goto qbuf_exit;
-	}
-
-	local_irq_save(flags);
-	ret = vpif_buffer_prepare(&common->buffer_queue, buf1,
-					common->buffer_queue.field);
-	if (ret < 0) {
-		local_irq_restore(flags);
-		goto qbuf_exit;
-	}
-
-	buf1->state = VIDEOBUF_ACTIVE;
-
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		addr = buf1->boff;
-	else
-		addr = videobuf_to_dma_contig(buf1);
-
-	common->next_frm = buf1;
-	common->set_addr(addr + common->ytop_off,
-			 addr + common->ybtm_off,
-			 addr + common->ctop_off,
-			 addr + common->cbtm_off);
-
-	local_irq_restore(flags);
-	list_add_tail(&buf1->stream, &common->buffer_queue.stream);
-	mutex_unlock(&common->buffer_queue.vb_lock);
-	return 0;
-
-qbuf_exit:
-	mutex_unlock(&common->buffer_queue.vb_lock);
-	return -EINVAL;
+	return vb2_qbuf(&common->buffer_queue, buf);
 }
 
 /**
@@ -1056,8 +1102,8 @@ static int vpif_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 
 	vpif_dbg(2, debug, "vpif_dqbuf\n");
 
-	return videobuf_dqbuf(&common->buffer_queue, buf,
-					file->f_flags & O_NONBLOCK);
+	return vb2_dqbuf(&common->buffer_queue, buf,
+			 (file->f_flags & O_NONBLOCK));
 }
 
 /**
@@ -1070,13 +1116,11 @@ static int vpif_streamon(struct file *file, void *priv,
 				enum v4l2_buf_type buftype)
 {
 
-	struct vpif_capture_config *config = vpif_dev->platform_data;
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct channel_obj *oth_ch = vpif_obj.dev[!ch->channel_id];
 	struct vpif_params *vpif;
-	unsigned long addr = 0;
 	int ret = 0;
 
 	vpif_dbg(2, debug, "vpif_streamon\n");
@@ -1122,95 +1166,13 @@ static int vpif_streamon(struct file *file, void *priv,
 		return ret;
 	}
 
-	/* Call videobuf_streamon to start streaming in videobuf */
-	ret = videobuf_streamon(&common->buffer_queue);
+	/* Call vb2_streamon to start streaming in videobuf2 */
+	ret = vb2_streamon(&common->buffer_queue, buftype);
 	if (ret) {
-		vpif_dbg(1, debug, "videobuf_streamon\n");
+		vpif_dbg(1, debug, "vb2_streamon\n");
 		return ret;
 	}
 
-	/* If buffer queue is empty, return error */
-	if (list_empty(&common->dma_queue)) {
-		vpif_dbg(1, debug, "buffer queue is empty\n");
-		ret = -EIO;
-		goto exit;
-	}
-
-	/* Get the next frame from the buffer queue */
-	common->cur_frm = list_entry(common->dma_queue.next,
-				    struct videobuf_buffer, queue);
-	common->next_frm = common->cur_frm;
-
-	/* Remove buffer from the buffer queue */
-	list_del(&common->cur_frm->queue);
-	/* Mark state of the current frame to active */
-	common->cur_frm->state = VIDEOBUF_ACTIVE;
-	/* Initialize field_id and started member */
-	ch->field_id = 0;
-	common->started = 1;
-
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		addr = common->cur_frm->boff;
-	else
-		addr = videobuf_to_dma_contig(common->cur_frm);
-
-	/* Calculate the offset for Y and C data in the buffer */
-	vpif_calculate_offsets(ch);
-
-	if ((vpif->std_info.frm_fmt &&
-	    ((common->fmt.fmt.pix.field != V4L2_FIELD_NONE) &&
-	     (common->fmt.fmt.pix.field != V4L2_FIELD_ANY))) ||
-	    (!vpif->std_info.frm_fmt &&
-	     (common->fmt.fmt.pix.field == V4L2_FIELD_NONE))) {
-		vpif_dbg(1, debug, "conflict in field format and std format\n");
-		ret = -EINVAL;
-		goto exit;
-	}
-
-	/* configure 1 or 2 channel mode */
-	ret = config->setup_input_channel_mode(vpif->std_info.ycmux_mode);
-
-	if (ret < 0) {
-		vpif_dbg(1, debug, "can't set vpif channel mode\n");
-		goto exit;
-	}
-
-	/* Call vpif_set_params function to set the parameters and addresses */
-	ret = vpif_set_video_params(vpif, ch->channel_id);
-
-	if (ret < 0) {
-		vpif_dbg(1, debug, "can't set video params\n");
-		goto exit;
-	}
-
-	common->started = ret;
-	vpif_config_addr(ch, ret);
-
-	common->set_addr(addr + common->ytop_off,
-			 addr + common->ybtm_off,
-			 addr + common->ctop_off,
-			 addr + common->cbtm_off);
-
-	/**
-	 * Set interrupt for both the fields in VPIF Register enable channel in
-	 * VPIF register
-	 */
-	if ((VPIF_CHANNEL0_VIDEO == ch->channel_id)) {
-		channel0_intr_assert();
-		channel0_intr_enable(1);
-		enable_channel0(1);
-	}
-	if ((VPIF_CHANNEL1_VIDEO == ch->channel_id) ||
-	    (common->started == 2)) {
-		channel1_intr_assert();
-		channel1_intr_enable(1);
-		enable_channel1(1);
-	}
-	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
-	return ret;
-
-exit:
-	videobuf_streamoff(&common->buffer_queue);
 	return ret;
 }
 
@@ -1265,7 +1227,7 @@ static int vpif_streamoff(struct file *file, void *priv,
 	if (ret && (ret != -ENOIOCTLCMD))
 		vpif_dbg(1, debug, "stream off failed in subdev\n");
 
-	return videobuf_streamoff(&common->buffer_queue);
+	return vb2_streamoff(&common->buffer_queue, buftype);
 }
 
 /**
@@ -1679,7 +1641,7 @@ static int vpif_querycap(struct file *file, void  *priv,
 
 	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 	strlcpy(cap->driver, "vpif capture", sizeof(cap->driver));
-	strlcpy(cap->bus_info, "DM646x Platform", sizeof(cap->bus_info));
+	strlcpy(cap->bus_info, "VPIF Platform", sizeof(cap->bus_info));
 	strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
 	return 0;
@@ -2168,6 +2130,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 	struct video_device *vfd;
 	struct resource *res;
 	int subdev_count;
+	size_t size;
 
 	vpif_dev = &pdev->dev;
 
@@ -2186,8 +2149,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 	k = 0;
 	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
 		for (i = res->start; i <= res->end; i++) {
-			if (request_irq(i, vpif_channel_isr, IRQF_DISABLED,
-					"DM646x_Capture",
+			if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
+					"VPIF_Capture",
 				(void *)(&vpif_obj.dev[k]->channel_id))) {
 				err = -EBUSY;
 				i--;
@@ -2216,12 +2179,29 @@ static __init int vpif_probe(struct platform_device *pdev)
 		vfd->v4l2_dev = &vpif_obj.v4l2_dev;
 		vfd->release = video_device_release;
 		snprintf(vfd->name, sizeof(vfd->name),
-			 "DM646x_VPIFCapture_DRIVER_V%s",
+			 "VPIF_Capture_DRIVER_V%s",
 			 VPIF_CAPTURE_VERSION);
 		/* Set video_dev to the video device */
 		ch->video_dev = vfd;
 	}
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res) {
+		size = resource_size(res);
+		/* The resources are divided into two equal memory and when we
+		 * have HD output we can add them together
+		 */
+		for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
+			ch = vpif_obj.dev[j];
+			ch->channel_id = j;
+			/* only enabled if second resource exists */
+			config_params.video_limit[ch->channel_id] = 0;
+			if (size)
+				config_params.video_limit[ch->channel_id] =
+									size/2;
+		}
+	}
+
 	for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
 		ch = vpif_obj.dev[j];
 		ch->channel_id = j;
@@ -2275,8 +2255,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 			vpif_obj.sd[i]->grp_id = 1 << i;
 	}
 
-	v4l2_info(&vpif_obj.v4l2_dev,
-			"DM646x VPIF capture driver initialized\n");
+	v4l2_info(&vpif_obj.v4l2_dev, "VPIF capture driver initialized\n");
 	return 0;
 
 probe_subdev_out:
@@ -2333,26 +2312,70 @@ static int vpif_remove(struct platform_device *device)
 	return 0;
 }
 
+#ifdef CONFIG_PM
 /**
  * vpif_suspend: vpif device suspend
- *
- * TODO: Add suspend code here
  */
-static int
-vpif_suspend(struct device *dev)
+static int vpif_suspend(struct device *dev)
 {
-	return -1;
+
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+		mutex_lock(&common->lock);
+		if (ch->usrs && common->io_usrs) {
+			/* Disable channel */
+			if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
+				enable_channel0(0);
+				channel0_intr_enable(0);
+			}
+			if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
+			    common->started == 2) {
+				enable_channel1(0);
+				channel1_intr_enable(0);
+			}
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
 }
 
-/**
+/*
  * vpif_resume: vpif device suspend
- *
- * TODO: Add resume code here
  */
-static int
-vpif_resume(struct device *dev)
+static int vpif_resume(struct device *dev)
 {
-	return -1;
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+		mutex_lock(&common->lock);
+		if (ch->usrs && common->io_usrs) {
+			/* Disable channel */
+			if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
+				enable_channel0(1);
+				channel0_intr_enable(1);
+			}
+			if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
+			    common->started == 2) {
+				enable_channel1(1);
+				channel1_intr_enable(1);
+			}
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
 }
 
 static const struct dev_pm_ops vpif_dev_pm_ops = {
@@ -2360,11 +2383,16 @@ static const struct dev_pm_ops vpif_dev_pm_ops = {
 	.resume = vpif_resume,
 };
 
+#define vpif_pm_ops (&vpif_dev_pm_ops)
+#else
+#define vpif_pm_ops NULL
+#endif
+
 static __refdata struct platform_driver vpif_driver = {
 	.driver	= {
 		.name	= "vpif_capture",
 		.owner	= THIS_MODULE,
-		.pm = &vpif_dev_pm_ops,
+		.pm	= vpif_pm_ops,
 	},
 	.probe = vpif_probe,
 	.remove = vpif_remove,
diff --git a/drivers/media/video/davinci/vpif_capture.h b/drivers/media/video/davinci/vpif_capture.h
index a693d4ebda5..3511510f43e 100644
--- a/drivers/media/video/davinci/vpif_capture.h
+++ b/drivers/media/video/davinci/vpif_capture.h
@@ -26,7 +26,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/videobuf-core.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-dma-contig.h>
 #include <media/davinci/vpif_types.h>
 
 #include "vpif.h"
@@ -60,11 +60,16 @@ struct video_obj {
 	u32 input_idx;
 };
 
+struct vpif_cap_buffer {
+	struct vb2_buffer vb;
+	struct list_head list;
+};
+
 struct common_obj {
 	/* Pointer pointing to current v4l2_buffer */
-	struct videobuf_buffer *cur_frm;
+	struct vpif_cap_buffer *cur_frm;
 	/* Pointer pointing to current v4l2_buffer */
-	struct videobuf_buffer *next_frm;
+	struct vpif_cap_buffer *next_frm;
 	/*
 	 * This field keeps track of type of buffer exchange mechanism
 	 * user has selected
@@ -73,7 +78,9 @@ struct common_obj {
 	/* Used to store pixel format */
 	struct v4l2_format fmt;
 	/* Buffer queue used in video-buf */
-	struct videobuf_queue buffer_queue;
+	struct vb2_queue buffer_queue;
+	/* allocator-specific contexts for each plane */
+	struct vb2_alloc_ctx *alloc_ctx;
 	/* Queue of filled frames */
 	struct list_head dma_queue;
 	/* Used in video-buf */
@@ -151,6 +158,7 @@ struct vpif_config_params {
 	u32 min_bufsize[VPIF_CAPTURE_NUM_CHANNELS];
 	u32 channel_bufsize[VPIF_CAPTURE_NUM_CHANNELS];
 	u8 default_device[VPIF_CAPTURE_NUM_CHANNELS];
+	u32 video_limit[VPIF_CAPTURE_NUM_CHANNELS];
 	u8 max_device_type;
 };
 /* Struct which keeps track of the line numbers for the sliced vbi service */
diff --git a/drivers/media/video/davinci/vpif_display.c b/drivers/media/video/davinci/vpif_display.c
index e6488ee7db1..e129c98921a 100644
--- a/drivers/media/video/davinci/vpif_display.c
+++ b/drivers/media/video/davinci/vpif_display.c
@@ -46,7 +46,7 @@ MODULE_DESCRIPTION("TI DaVinci VPIF Display driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(VPIF_DISPLAY_VERSION);
 
-#define DM646X_V4L2_STD (V4L2_STD_525_60 | V4L2_STD_625_50)
+#define VPIF_V4L2_STD (V4L2_STD_525_60 | V4L2_STD_625_50)
 
 #define vpif_err(fmt, arg...)	v4l2_err(&vpif_obj.v4l2_dev, fmt, ## arg)
 #define vpif_dbg(level, debug, fmt, arg...)	\
@@ -82,89 +82,38 @@ static struct vpif_config_params config_params = {
 
 static struct vpif_device vpif_obj = { {NULL} };
 static struct device *vpif_dev;
+static void vpif_calculate_offsets(struct channel_obj *ch);
+static void vpif_config_addr(struct channel_obj *ch, int muxmode);
 
 /*
- * vpif_uservirt_to_phys: This function is used to convert user
- * space virtual address to physical address.
- */
-static u32 vpif_uservirt_to_phys(u32 virtp)
-{
-	struct mm_struct *mm = current->mm;
-	unsigned long physp = 0;
-	struct vm_area_struct *vma;
-
-	vma = find_vma(mm, virtp);
-
-	/* For kernel direct-mapped memory, take the easy way */
-	if (virtp >= PAGE_OFFSET) {
-		physp = virt_to_phys((void *)virtp);
-	} else if (vma && (vma->vm_flags & VM_IO) && (vma->vm_pgoff)) {
-		/* this will catch, kernel-allocated, mmaped-to-usermode addr */
-		physp = (vma->vm_pgoff << PAGE_SHIFT) + (virtp - vma->vm_start);
-	} else {
-		/* otherwise, use get_user_pages() for general userland pages */
-		int res, nr_pages = 1;
-		struct page *pages;
-		down_read(&current->mm->mmap_sem);
-
-		res = get_user_pages(current, current->mm,
-				     virtp, nr_pages, 1, 0, &pages, NULL);
-		up_read(&current->mm->mmap_sem);
-
-		if (res == nr_pages) {
-			physp = __pa(page_address(&pages[0]) +
-							(virtp & ~PAGE_MASK));
-		} else {
-			vpif_err("get_user_pages failed\n");
-			return 0;
-		}
-	}
-
-	return physp;
-}
-
-/*
- * buffer_prepare: This is the callback function called from videobuf_qbuf()
+ * buffer_prepare: This is the callback function called from vb2_qbuf()
  * function the buffer is prepared and user space virtual address is converted
  * into physical address
  */
-static int vpif_buffer_prepare(struct videobuf_queue *q,
-			       struct videobuf_buffer *vb,
-			       enum v4l2_field field)
+static int vpif_buffer_prepare(struct vb2_buffer *vb)
 {
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_queue *q = vb->vb2_queue;
 	struct common_obj *common;
 	unsigned long addr;
 
 	common = &fh->channel->common[VPIF_VIDEO_INDEX];
-	if (VIDEOBUF_NEEDS_INIT == vb->state) {
-		vb->width	= common->width;
-		vb->height	= common->height;
-		vb->size	= vb->width * vb->height;
-		vb->field	= field;
-	}
-	vb->state = VIDEOBUF_PREPARED;
-
-	/* if user pointer memory mechanism is used, get the physical
-	 * address of the buffer */
-	if (V4L2_MEMORY_USERPTR == common->memory) {
-		if (!vb->baddr) {
-			vpif_err("buffer_address is 0\n");
-			return -EINVAL;
-		}
-
-		vb->boff = vpif_uservirt_to_phys(vb->baddr);
-		if (!ISALIGNED(vb->boff))
+	if (vb->state != VB2_BUF_STATE_ACTIVE &&
+		vb->state != VB2_BUF_STATE_PREPARED) {
+		vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
+		if (vb2_plane_vaddr(vb, 0) &&
+		vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
 			goto buf_align_exit;
-	}
 
-	addr = vb->boff;
-	if (q->streaming && (V4L2_BUF_TYPE_SLICED_VBI_OUTPUT != q->type)) {
-		if (!ISALIGNED(addr + common->ytop_off) ||
-		    !ISALIGNED(addr + common->ybtm_off) ||
-		    !ISALIGNED(addr + common->ctop_off) ||
-		    !ISALIGNED(addr + common->cbtm_off))
-			goto buf_align_exit;
+		addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+		if (q->streaming &&
+			(V4L2_BUF_TYPE_SLICED_VBI_OUTPUT != q->type)) {
+			if (!ISALIGNED(addr + common->ytop_off) ||
+			!ISALIGNED(addr + common->ybtm_off) ||
+			!ISALIGNED(addr + common->ctop_off) ||
+			!ISALIGNED(addr + common->cbtm_off))
+				goto buf_align_exit;
+		}
 	}
 	return 0;
 
@@ -174,86 +123,255 @@ buf_align_exit:
 }
 
 /*
- * vpif_buffer_setup: This function allocates memory for the buffers
+ * vpif_buffer_queue_setup: This function allocates memory for the buffers
  */
-static int vpif_buffer_setup(struct videobuf_queue *q, unsigned int *count,
-				unsigned int *size)
+static int vpif_buffer_queue_setup(struct vb2_queue *vq,
+				const struct v4l2_format *fmt,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	unsigned long size;
+
+	if (V4L2_MEMORY_MMAP == common->memory) {
+		size = config_params.channel_bufsize[ch->channel_id];
+		/*
+		* Checking if the buffer size exceeds the available buffer
+		* ycmux_mode = 0 means 1 channel mode HD and
+		* ycmux_mode = 1 means 2 channels mode SD
+		*/
+		if (ch->vpifparams.std_info.ycmux_mode == 0) {
+			if (config_params.video_limit[ch->channel_id])
+				while (size * *nbuffers >
+					(config_params.video_limit[0]
+						+ config_params.video_limit[1]))
+					(*nbuffers)--;
+		} else {
+			if (config_params.video_limit[ch->channel_id])
+				while (size * *nbuffers >
+				config_params.video_limit[ch->channel_id])
+					(*nbuffers)--;
+		}
+	} else {
+		size = common->fmt.fmt.pix.sizeimage;
+	}
 
-	if (V4L2_MEMORY_MMAP != common->memory)
-		return 0;
-
-	*size = config_params.channel_bufsize[ch->channel_id];
-	if (*count < config_params.min_numbuffers)
-		*count = config_params.min_numbuffers;
+	if (*nbuffers < config_params.min_numbuffers)
+			*nbuffers = config_params.min_numbuffers;
 
+	*nplanes = 1;
+	sizes[0] = size;
+	alloc_ctxs[0] = common->alloc_ctx;
 	return 0;
 }
 
 /*
  * vpif_buffer_queue: This function adds the buffer to DMA queue
  */
-static void vpif_buffer_queue(struct videobuf_queue *q,
-			      struct videobuf_buffer *vb)
+static void vpif_buffer_queue(struct vb2_buffer *vb)
 {
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpif_disp_buffer *buf = container_of(vb,
+				struct vpif_disp_buffer, vb);
+	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
 
-	common = &fh->channel->common[VPIF_VIDEO_INDEX];
+	common = &ch->common[VPIF_VIDEO_INDEX];
 
 	/* add the buffer to the DMA queue */
-	list_add_tail(&vb->queue, &common->dma_queue);
-	vb->state = VIDEOBUF_QUEUED;
+	list_add_tail(&buf->list, &common->dma_queue);
 }
 
 /*
- * vpif_buffer_release: This function is called from the videobuf layer to
+ * vpif_buf_cleanup: This function is called from the videobuf2 layer to
  * free memory allocated to the buffers
  */
-static void vpif_buffer_release(struct videobuf_queue *q,
-				struct videobuf_buffer *vb)
+static void vpif_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct vpif_fh *fh = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpif_disp_buffer *buf = container_of(vb,
+					struct vpif_disp_buffer, vb);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common;
+	unsigned long flags;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	spin_lock_irqsave(&common->irqlock, flags);
+	if (vb->state == VB2_BUF_STATE_ACTIVE)
+		list_del_init(&buf->list);
+	spin_unlock_irqrestore(&common->irqlock, flags);
+}
+
+static void vpif_wait_prepare(struct vb2_queue *vq)
 {
-	struct vpif_fh *fh = q->priv_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
-	unsigned int buf_size = 0;
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
+	mutex_unlock(&common->lock);
+}
 
-	videobuf_dma_contig_free(q, vb);
-	vb->state = VIDEOBUF_NEEDS_INIT;
+static void vpif_wait_finish(struct vb2_queue *vq)
+{
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common;
 
-	if (V4L2_MEMORY_MMAP != common->memory)
-		return;
+	common = &ch->common[VPIF_VIDEO_INDEX];
+	mutex_lock(&common->lock);
+}
 
-	buf_size = config_params.channel_bufsize[ch->channel_id];
+static int vpif_buffer_init(struct vb2_buffer *vb)
+{
+	struct vpif_disp_buffer *buf = container_of(vb,
+					struct vpif_disp_buffer, vb);
+
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
 }
 
-static struct videobuf_queue_ops video_qops = {
-	.buf_setup	= vpif_buffer_setup,
-	.buf_prepare	= vpif_buffer_prepare,
-	.buf_queue	= vpif_buffer_queue,
-	.buf_release	= vpif_buffer_release,
-};
 static u8 channel_first_int[VPIF_NUMOBJECTS][2] = { {1, 1} };
 
+static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vpif_display_config *vpif_config_data =
+					vpif_dev->platform_data;
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_params *vpif = &ch->vpifparams;
+	unsigned long addr = 0;
+	int ret;
+
+	/* If buffer queue is empty, return error */
+	if (list_empty(&common->dma_queue)) {
+		vpif_err("buffer queue is empty\n");
+		return -EIO;
+	}
+
+	/* Get the next frame from the buffer queue */
+	common->next_frm = common->cur_frm =
+			    list_entry(common->dma_queue.next,
+				       struct vpif_disp_buffer, list);
+
+	list_del(&common->cur_frm->list);
+	/* Mark state of the current frame to active */
+	common->cur_frm->vb.state = VB2_BUF_STATE_ACTIVE;
+
+	/* Initialize field_id and started member */
+	ch->field_id = 0;
+	common->started = 1;
+	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb, 0);
+	/* Calculate the offset for Y and C data  in the buffer */
+	vpif_calculate_offsets(ch);
+
+	if ((ch->vpifparams.std_info.frm_fmt &&
+		((common->fmt.fmt.pix.field != V4L2_FIELD_NONE)
+		&& (common->fmt.fmt.pix.field != V4L2_FIELD_ANY)))
+		|| (!ch->vpifparams.std_info.frm_fmt
+		&& (common->fmt.fmt.pix.field == V4L2_FIELD_NONE))) {
+		vpif_err("conflict in field format and std format\n");
+		return -EINVAL;
+	}
+
+	/* clock settings */
+	ret =
+	    vpif_config_data->set_clock(ch->vpifparams.std_info.ycmux_mode,
+					ch->vpifparams.std_info.hd_sd);
+	if (ret < 0) {
+		vpif_err("can't set clock\n");
+		return ret;
+	}
+
+	/* set the parameters and addresses */
+	ret = vpif_set_video_params(vpif, ch->channel_id + 2);
+	if (ret < 0)
+		return ret;
+
+	common->started = ret;
+	vpif_config_addr(ch, ret);
+	common->set_addr((addr + common->ytop_off),
+			    (addr + common->ybtm_off),
+			    (addr + common->ctop_off),
+			    (addr + common->cbtm_off));
+
+	/* Set interrupt for both the fields in VPIF
+	    Register enable channel in VPIF register */
+	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
+		channel2_intr_assert();
+		channel2_intr_enable(1);
+		enable_channel2(1);
+		if (vpif_config_data->ch2_clip_en)
+			channel2_clipping_enable(1);
+	}
+
+	if ((VPIF_CHANNEL3_VIDEO == ch->channel_id)
+		|| (common->started == 2)) {
+		channel3_intr_assert();
+		channel3_intr_enable(1);
+		enable_channel3(1);
+		if (vpif_config_data->ch3_clip_en)
+			channel3_clipping_enable(1);
+	}
+	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
+
+	return 0;
+}
+
+/* abort streaming and wait for last buffer */
+static int vpif_stop_streaming(struct vb2_queue *vq)
+{
+	struct vpif_fh *fh = vb2_get_drv_priv(vq);
+	struct channel_obj *ch = fh->channel;
+	struct common_obj *common;
+
+	if (!vb2_is_streaming(vq))
+		return 0;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* release all active buffers */
+	while (!list_empty(&common->dma_queue)) {
+		common->next_frm = list_entry(common->dma_queue.next,
+						struct vpif_disp_buffer, list);
+		list_del(&common->next_frm->list);
+		vb2_buffer_done(&common->next_frm->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	return 0;
+}
+
+static struct vb2_ops video_qops = {
+	.queue_setup		= vpif_buffer_queue_setup,
+	.wait_prepare		= vpif_wait_prepare,
+	.wait_finish		= vpif_wait_finish,
+	.buf_init		= vpif_buffer_init,
+	.buf_prepare		= vpif_buffer_prepare,
+	.start_streaming	= vpif_start_streaming,
+	.stop_streaming		= vpif_stop_streaming,
+	.buf_cleanup		= vpif_buf_cleanup,
+	.buf_queue		= vpif_buffer_queue,
+};
+
 static void process_progressive_mode(struct common_obj *common)
 {
 	unsigned long addr = 0;
 
 	/* Get the next buffer from buffer queue */
 	common->next_frm = list_entry(common->dma_queue.next,
-				struct videobuf_buffer, queue);
+				struct vpif_disp_buffer, list);
 	/* Remove that buffer from the buffer queue */
-	list_del(&common->next_frm->queue);
+	list_del(&common->next_frm->list);
 	/* Mark status of the buffer as active */
-	common->next_frm->state = VIDEOBUF_ACTIVE;
+	common->next_frm->vb.state = VB2_BUF_STATE_ACTIVE;
 
 	/* Set top and bottom field addrs in VPIF registers */
-	addr = videobuf_to_dma_contig(common->next_frm);
+	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb, 0);
 	common->set_addr(addr + common->ytop_off,
 				 addr + common->ybtm_off,
 				 addr + common->ctop_off,
@@ -271,11 +389,10 @@ static void process_interlaced_mode(int fid, struct common_obj *common)
 		/* one frame is displayed If next frame is
 		 *  available, release cur_frm and move on */
 		/* Copy frame display time */
-		do_gettimeofday(&common->cur_frm->ts);
+		do_gettimeofday(&common->cur_frm->vb.v4l2_buf.timestamp);
 		/* Change status of the cur_frm */
-		common->cur_frm->state = VIDEOBUF_DONE;
-		/* unlock semaphore on cur_frm */
-		wake_up_interruptible(&common->cur_frm->done);
+		vb2_buffer_done(&common->cur_frm->vb,
+					    VB2_BUF_STATE_DONE);
 		/* Make cur_frm pointing to next_frm */
 		common->cur_frm = common->next_frm;
 
@@ -307,6 +424,9 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 	int channel_id = 0;
 
 	channel_id = *(int *)(dev_id);
+	if (!vpif_intr_status(channel_id + 2))
+		return IRQ_NONE;
+
 	ch = dev->dev[channel_id];
 	field = ch->common[VPIF_VIDEO_INDEX].fmt.fmt.pix.field;
 	for (i = 0; i < VPIF_NUMOBJECTS; i++) {
@@ -323,9 +443,10 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 			if (!channel_first_int[i][channel_id]) {
 				/* Mark status of the cur_frm to
 				 * done and unlock semaphore on it */
-				do_gettimeofday(&common->cur_frm->ts);
-				common->cur_frm->state = VIDEOBUF_DONE;
-				wake_up_interruptible(&common->cur_frm->done);
+				do_gettimeofday(&common->cur_frm->vb.
+						v4l2_buf.timestamp);
+				vb2_buffer_done(&common->cur_frm->vb,
+					    VB2_BUF_STATE_DONE);
 				/* Make cur_frm pointing to next_frm */
 				common->cur_frm = common->next_frm;
 			}
@@ -443,10 +564,7 @@ static void vpif_calculate_offsets(struct channel_obj *ch)
 		vid_ch->buf_field = common->fmt.fmt.pix.field;
 	}
 
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		sizeimage = common->fmt.fmt.pix.sizeimage;
-	else
-		sizeimage = config_params.channel_bufsize[ch->channel_id];
+	sizeimage = common->fmt.fmt.pix.sizeimage;
 
 	hpitch = common->fmt.fmt.pix.bytesperline;
 	vpitch = sizeimage / (hpitch * 2);
@@ -523,10 +641,7 @@ static int vpif_check_format(struct channel_obj *ch,
 	if (pixfmt->bytesperline <= 0)
 		goto invalid_pitch_exit;
 
-	if (V4L2_MEMORY_USERPTR == common->memory)
-		sizeimage = pixfmt->sizeimage;
-	else
-		sizeimage = config_params.channel_bufsize[ch->channel_id];
+	sizeimage = pixfmt->sizeimage;
 
 	if (vpif_update_resolution(ch))
 		return -EINVAL;
@@ -583,7 +698,7 @@ static int vpif_mmap(struct file *filep, struct vm_area_struct *vma)
 
 	vpif_dbg(2, debug, "vpif_mmap\n");
 
-	return videobuf_mmap_mapper(&common->buffer_queue, vma);
+	return vb2_mmap(&common->buffer_queue, vma);
 }
 
 /*
@@ -596,7 +711,7 @@ static unsigned int vpif_poll(struct file *filep, poll_table *wait)
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 
 	if (common->started)
-		return videobuf_poll_stream(filep, &common->buffer_queue, wait);
+		return vb2_poll(&common->buffer_queue, filep, wait);
 
 	return 0;
 }
@@ -665,9 +780,11 @@ static int vpif_release(struct file *filep)
 			channel3_intr_enable(0);
 		}
 		common->started = 0;
+
 		/* Free buffers allocated */
-		videobuf_queue_cancel(&common->buffer_queue);
-		videobuf_mmap_free(&common->buffer_queue);
+		vb2_queue_release(&common->buffer_queue);
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+
 		common->numbuffers =
 		    config_params.numbuffers[ch->channel_id];
 	}
@@ -806,6 +923,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common;
 	enum v4l2_field field;
+	struct vb2_queue *q;
 	u8 index = 0;
 
 	/* This file handle has not initialized the channel,
@@ -825,9 +943,8 @@ static int vpif_reqbufs(struct file *file, void *priv,
 
 	common = &ch->common[index];
 
-	if (common->fmt.type != reqbuf->type)
+	if (common->fmt.type != reqbuf->type || !vpif_dev)
 		return -EINVAL;
-
 	if (0 != common->io_usrs)
 		return -EBUSY;
 
@@ -839,14 +956,21 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	} else {
 		field = V4L2_VBI_INTERLACED;
 	}
+	/* Initialize videobuf2 queue as per the buffer type */
+	common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
+	if (!common->alloc_ctx) {
+		vpif_err("Failed to get the context\n");
+		return -EINVAL;
+	}
+	q = &common->buffer_queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = fh;
+	q->ops = &video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct vpif_disp_buffer);
 
-	/* Initialize videobuf queue as per the buffer type */
-	videobuf_queue_dma_contig_init(&common->buffer_queue,
-					    &video_qops, NULL,
-					    &common->irqlock,
-					    reqbuf->type, field,
-					    sizeof(struct videobuf_buffer), fh,
-					    &common->lock);
+	vb2_queue_init(q);
 
 	/* Set io allowed member of file handle to TRUE */
 	fh->io_allowed[index] = 1;
@@ -855,9 +979,8 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	/* Store type of memory requested in channel object */
 	common->memory = reqbuf->memory;
 	INIT_LIST_HEAD(&common->dma_queue);
-
 	/* Allocate buffers */
-	return videobuf_reqbufs(&common->buffer_queue, reqbuf);
+	return vb2_reqbufs(&common->buffer_queue, reqbuf);
 }
 
 static int vpif_querybuf(struct file *file, void *priv,
@@ -870,22 +993,25 @@ static int vpif_querybuf(struct file *file, void *priv,
 	if (common->fmt.type != tbuf->type)
 		return -EINVAL;
 
-	return videobuf_querybuf(&common->buffer_queue, tbuf);
+	return vb2_querybuf(&common->buffer_queue, tbuf);
 }
 
 static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 {
+	struct vpif_fh *fh = NULL;
+	struct channel_obj *ch = NULL;
+	struct common_obj *common = NULL;
 
-	struct vpif_fh *fh = priv;
-	struct channel_obj *ch = fh->channel;
-	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct v4l2_buffer tbuf = *buf;
-	struct videobuf_buffer *buf1;
-	unsigned long addr = 0;
-	unsigned long flags;
-	int ret = 0;
+	if (!buf || !priv)
+		return -EINVAL;
+
+	fh = priv;
+	ch = fh->channel;
+	if (!ch)
+		return -EINVAL;
 
-	if (common->fmt.type != tbuf.type)
+	common = &(ch->common[VPIF_VIDEO_INDEX]);
+	if (common->fmt.type != buf->type)
 		return -EINVAL;
 
 	if (!fh->io_allowed[VPIF_VIDEO_INDEX]) {
@@ -893,73 +1019,7 @@ static int vpif_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 		return -EACCES;
 	}
 
-	if (!(list_empty(&common->dma_queue)) ||
-	    (common->cur_frm != common->next_frm) ||
-	    !(common->started) ||
-	    (common->started && (0 == ch->field_id)))
-		return videobuf_qbuf(&common->buffer_queue, buf);
-
-	/* bufferqueue is empty store buffer address in VPIF registers */
-	mutex_lock(&common->buffer_queue.vb_lock);
-	buf1 = common->buffer_queue.bufs[tbuf.index];
-	if (buf1->memory != tbuf.memory) {
-		vpif_err("invalid buffer type\n");
-		goto qbuf_exit;
-	}
-
-	if ((buf1->state == VIDEOBUF_QUEUED) ||
-	    (buf1->state == VIDEOBUF_ACTIVE)) {
-		vpif_err("invalid state\n");
-		goto qbuf_exit;
-	}
-
-	switch (buf1->memory) {
-	case V4L2_MEMORY_MMAP:
-		if (buf1->baddr == 0)
-			goto qbuf_exit;
-		break;
-
-	case V4L2_MEMORY_USERPTR:
-		if (tbuf.length < buf1->bsize)
-			goto qbuf_exit;
-
-		if ((VIDEOBUF_NEEDS_INIT != buf1->state)
-			    && (buf1->baddr != tbuf.m.userptr)) {
-			vpif_buffer_release(&common->buffer_queue, buf1);
-			buf1->baddr = tbuf.m.userptr;
-		}
-		break;
-
-	default:
-		goto qbuf_exit;
-	}
-
-	local_irq_save(flags);
-	ret = vpif_buffer_prepare(&common->buffer_queue, buf1,
-					common->buffer_queue.field);
-	if (ret < 0) {
-		local_irq_restore(flags);
-		goto qbuf_exit;
-	}
-
-	buf1->state = VIDEOBUF_ACTIVE;
-	addr = buf1->boff;
-	common->next_frm = buf1;
-	if (tbuf.type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT) {
-		common->set_addr((addr + common->ytop_off),
-				 (addr + common->ybtm_off),
-				 (addr + common->ctop_off),
-				 (addr + common->cbtm_off));
-	}
-
-	local_irq_restore(flags);
-	list_add_tail(&buf1->stream, &common->buffer_queue.stream);
-	mutex_unlock(&common->buffer_queue.vb_lock);
-	return 0;
-
-qbuf_exit:
-	mutex_unlock(&common->buffer_queue.vb_lock);
-	return -EINVAL;
+	return vb2_qbuf(&common->buffer_queue, buf);
 }
 
 static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
@@ -969,7 +1029,7 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	int ret = 0;
 
-	if (!(*std_id & DM646X_V4L2_STD))
+	if (!(*std_id & VPIF_V4L2_STD))
 		return -EINVAL;
 
 	if (common->started) {
@@ -1026,7 +1086,7 @@ static int vpif_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 
-	return videobuf_dqbuf(&common->buffer_queue, p,
+	return vb2_dqbuf(&common->buffer_queue, p,
 					(file->f_flags & O_NONBLOCK));
 }
 
@@ -1037,10 +1097,6 @@ static int vpif_streamon(struct file *file, void *priv,
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
 	struct channel_obj *oth_ch = vpif_obj.dev[!ch->channel_id];
-	struct vpif_params *vpif = &ch->vpifparams;
-	struct vpif_display_config *vpif_config_data =
-					vpif_dev->platform_data;
-	unsigned long addr = 0;
 	int ret = 0;
 
 	if (buftype != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
@@ -1072,82 +1128,13 @@ static int vpif_streamon(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
-	/* Call videobuf_streamon to start streaming in videobuf */
-	ret = videobuf_streamon(&common->buffer_queue);
+	/* Call vb2_streamon to start streaming in videobuf2 */
+	ret = vb2_streamon(&common->buffer_queue, buftype);
 	if (ret < 0) {
-		vpif_err("videobuf_streamon\n");
+		vpif_err("vb2_streamon\n");
 		return ret;
 	}
 
-	/* If buffer queue is empty, return error */
-	if (list_empty(&common->dma_queue)) {
-		vpif_err("buffer queue is empty\n");
-		return -EIO;
-	}
-
-	/* Get the next frame from the buffer queue */
-	common->next_frm = common->cur_frm =
-			    list_entry(common->dma_queue.next,
-				       struct videobuf_buffer, queue);
-
-	list_del(&common->cur_frm->queue);
-	/* Mark state of the current frame to active */
-	common->cur_frm->state = VIDEOBUF_ACTIVE;
-
-	/* Initialize field_id and started member */
-	ch->field_id = 0;
-	common->started = 1;
-	if (buftype == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		addr = common->cur_frm->boff;
-		/* Calculate the offset for Y and C data  in the buffer */
-		vpif_calculate_offsets(ch);
-
-		if ((ch->vpifparams.std_info.frm_fmt &&
-			((common->fmt.fmt.pix.field != V4L2_FIELD_NONE)
-			&& (common->fmt.fmt.pix.field != V4L2_FIELD_ANY)))
-			|| (!ch->vpifparams.std_info.frm_fmt
-			&& (common->fmt.fmt.pix.field == V4L2_FIELD_NONE))) {
-			vpif_err("conflict in field format and std format\n");
-			return -EINVAL;
-		}
-
-		/* clock settings */
-		ret =
-		 vpif_config_data->set_clock(ch->vpifparams.std_info.ycmux_mode,
-						ch->vpifparams.std_info.hd_sd);
-		if (ret < 0) {
-			vpif_err("can't set clock\n");
-			return ret;
-		}
-
-		/* set the parameters and addresses */
-		ret = vpif_set_video_params(vpif, ch->channel_id + 2);
-		if (ret < 0)
-			return ret;
-
-		common->started = ret;
-		vpif_config_addr(ch, ret);
-		common->set_addr((addr + common->ytop_off),
-				 (addr + common->ybtm_off),
-				 (addr + common->ctop_off),
-				 (addr + common->cbtm_off));
-
-		/* Set interrupt for both the fields in VPIF
-		   Register enable channel in VPIF register */
-		if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-			channel2_intr_assert();
-			channel2_intr_enable(1);
-			enable_channel2(1);
-		}
-
-		if ((VPIF_CHANNEL3_VIDEO == ch->channel_id)
-			|| (common->started == 2)) {
-			channel3_intr_assert();
-			channel3_intr_enable(1);
-			enable_channel3(1);
-		}
-		channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
-	}
 	return ret;
 }
 
@@ -1157,6 +1144,8 @@ static int vpif_streamoff(struct file *file, void *priv,
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_display_config *vpif_config_data =
+					vpif_dev->platform_data;
 
 	if (buftype != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 		vpif_err("buffer type not supported\n");
@@ -1176,18 +1165,22 @@ static int vpif_streamoff(struct file *file, void *priv,
 	if (buftype == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 		/* disable channel */
 		if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
+			if (vpif_config_data->ch2_clip_en)
+				channel2_clipping_enable(0);
 			enable_channel2(0);
 			channel2_intr_enable(0);
 		}
 		if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
 					(2 == common->started)) {
+			if (vpif_config_data->ch3_clip_en)
+				channel3_clipping_enable(0);
 			enable_channel3(0);
 			channel3_intr_enable(0);
 		}
 	}
 
 	common->started = 0;
-	return videobuf_streamoff(&common->buffer_queue);
+	return vb2_streamoff(&common->buffer_queue, buftype);
 }
 
 static int vpif_cropcap(struct file *file, void *priv,
@@ -1220,7 +1213,7 @@ static int vpif_enum_output(struct file *file, void *fh,
 
 	strcpy(output->name, config->output[output->index]);
 	output->type = V4L2_OUTPUT_TYPE_ANALOG;
-	output->std = DM646X_V4L2_STD;
+	output->std = VPIF_V4L2_STD;
 
 	return 0;
 }
@@ -1605,7 +1598,7 @@ static struct video_device vpif_video_template = {
 	.name		= "vpif",
 	.fops		= &vpif_fops,
 	.ioctl_ops	= &vpif_ioctl_ops,
-	.tvnorms	= DM646X_V4L2_STD,
+	.tvnorms	= VPIF_V4L2_STD,
 	.current_norm	= V4L2_STD_625_50,
 
 };
@@ -1687,9 +1680,9 @@ static __init int vpif_probe(struct platform_device *pdev)
 	struct video_device *vfd;
 	struct resource *res;
 	int subdev_count;
+	size_t size;
 
 	vpif_dev = &pdev->dev;
-
 	err = initialize_vpif();
 
 	if (err) {
@@ -1706,8 +1699,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 	k = 0;
 	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
 		for (i = res->start; i <= res->end; i++) {
-			if (request_irq(i, vpif_channel_isr, IRQF_DISABLED,
-					"DM646x_Display",
+			if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
+					"VPIF_Display",
 				(void *)(&vpif_obj.dev[k]->channel_id))) {
 				err = -EBUSY;
 				goto vpif_int_err;
@@ -1737,13 +1730,31 @@ static __init int vpif_probe(struct platform_device *pdev)
 		vfd->v4l2_dev = &vpif_obj.v4l2_dev;
 		vfd->release = video_device_release;
 		snprintf(vfd->name, sizeof(vfd->name),
-			 "DM646x_VPIFDisplay_DRIVER_V%s",
+			 "VPIF_Display_DRIVER_V%s",
 			 VPIF_DISPLAY_VERSION);
 
 		/* Set video_dev to the video device */
 		ch->video_dev = vfd;
 	}
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res) {
+		size = resource_size(res);
+		/* The resources are divided into two equal memory and when
+		 * we have HD output we can add them together
+		 */
+		for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
+			ch = vpif_obj.dev[j];
+			ch->channel_id = j;
+
+			/* only enabled if second resource exists */
+			config_params.video_limit[ch->channel_id] = 0;
+			if (size)
+				config_params.video_limit[ch->channel_id] =
+									size/2;
+		}
+	}
+
 	for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
 		ch = vpif_obj.dev[j];
 		/* Initialize field of the channel objects */
@@ -1823,7 +1834,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 	}
 
 	v4l2_info(&vpif_obj.v4l2_dev,
-			"DM646x VPIF display driver initialized\n");
+			" VPIF display driver initialized\n");
 	return 0;
 
 probe_subdev_out:
@@ -1871,10 +1882,81 @@ static int vpif_remove(struct platform_device *device)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int vpif_suspend(struct device *dev)
+{
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+		mutex_lock(&common->lock);
+		if (atomic_read(&ch->usrs) && common->io_usrs) {
+			/* Disable channel */
+			if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
+				enable_channel2(0);
+				channel2_intr_enable(0);
+			}
+			if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
+					common->started == 2) {
+				enable_channel3(0);
+				channel3_intr_enable(0);
+			}
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
+}
+
+static int vpif_resume(struct device *dev)
+{
+
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+		mutex_lock(&common->lock);
+		if (atomic_read(&ch->usrs) && common->io_usrs) {
+			/* Enable channel */
+			if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
+				enable_channel2(1);
+				channel2_intr_enable(1);
+			}
+			if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
+					common->started == 2) {
+				enable_channel3(1);
+				channel3_intr_enable(1);
+			}
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops vpif_pm = {
+	.suspend        = vpif_suspend,
+	.resume         = vpif_resume,
+};
+
+#define vpif_pm_ops (&vpif_pm)
+#else
+#define vpif_pm_ops NULL
+#endif
+
 static __refdata struct platform_driver vpif_driver = {
 	.driver	= {
 			.name	= "vpif_display",
 			.owner	= THIS_MODULE,
+			.pm	= vpif_pm_ops,
 	},
 	.probe	= vpif_probe,
 	.remove	= vpif_remove,
diff --git a/drivers/media/video/davinci/vpif_display.h b/drivers/media/video/davinci/vpif_display.h
index 56879d1a068..8967ffb4405 100644
--- a/drivers/media/video/davinci/vpif_display.h
+++ b/drivers/media/video/davinci/vpif_display.h
@@ -1,5 +1,5 @@
 /*
- * DM646x display header file
+ * VPIF display header file
  *
  * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
  *
@@ -21,7 +21,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/videobuf-core.h>
-#include <media/videobuf-dma-contig.h>
+#include <media/videobuf2-dma-contig.h>
 #include <media/davinci/vpif_types.h>
 
 #include "vpif.h"
@@ -73,21 +73,29 @@ struct vbi_obj {
 						 * vbi data */
 };
 
+struct vpif_disp_buffer {
+	struct vb2_buffer vb;
+	struct list_head list;
+};
+
 struct common_obj {
 	/* Buffer specific parameters */
 	u8 *fbuffers[VIDEO_MAX_FRAME];		/* List of buffer pointers for
 						 * storing frames */
 	u32 numbuffers;				/* number of buffers */
-	struct videobuf_buffer *cur_frm;	/* Pointer pointing to current
-						 * videobuf_buffer */
-	struct videobuf_buffer *next_frm;	/* Pointer pointing to next
-						 * videobuf_buffer */
+	struct vpif_disp_buffer *cur_frm;	/* Pointer pointing to current
+						 * vb2_buffer */
+	struct vpif_disp_buffer *next_frm;	/* Pointer pointing to next
+						 * vb2_buffer */
 	enum v4l2_memory memory;		/* This field keeps track of
 						 * type of buffer exchange
 						 * method user has selected */
 	struct v4l2_format fmt;			/* Used to store the format */
-	struct videobuf_queue buffer_queue;	/* Buffer queue used in
+	struct vb2_queue buffer_queue;		/* Buffer queue used in
 						 * video-buf */
+	/* allocator-specific contexts for each plane */
+	struct vb2_alloc_ctx *alloc_ctx;
+
 	struct list_head dma_queue;		/* Queue of filled frames */
 	spinlock_t irqlock;			/* Used in video-buf */
 
@@ -158,6 +166,7 @@ struct vpif_config_params {
 	u32 min_bufsize[VPIF_DISPLAY_NUM_CHANNELS];
 	u32 channel_bufsize[VPIF_DISPLAY_NUM_CHANNELS];
 	u8 numbuffers[VPIF_DISPLAY_NUM_CHANNELS];
+	u32 video_limit[VPIF_DISPLAY_NUM_CHANNELS];
 	u8 min_numbuffers;
 };
 
diff --git a/drivers/media/video/em28xx/em28xx-audio.c b/drivers/media/video/em28xx/em28xx-audio.c
index d7e2a3dc552..07dc594e79f 100644
--- a/drivers/media/video/em28xx/em28xx-audio.c
+++ b/drivers/media/video/em28xx/em28xx-audio.c
@@ -42,6 +42,7 @@
 #include <sound/initval.h>
 #include <sound/control.h>
 #include <sound/tlv.h>
+#include <sound/ac97_codec.h>
 #include <media/v4l2-common.h>
 #include "em28xx.h"
 
@@ -679,19 +680,19 @@ static int em28xx_audio_init(struct em28xx *dev)
 	INIT_WORK(&dev->wq_trigger, audio_trigger);
 
 	if (dev->audio_mode.ac97 != EM28XX_NO_AC97) {
-		em28xx_cvol_new(card, dev, "Video", AC97_VIDEO_VOL);
-		em28xx_cvol_new(card, dev, "Line In", AC97_LINEIN_VOL);
-		em28xx_cvol_new(card, dev, "Phone", AC97_PHONE_VOL);
-		em28xx_cvol_new(card, dev, "Microphone", AC97_PHONE_VOL);
-		em28xx_cvol_new(card, dev, "CD", AC97_CD_VOL);
-		em28xx_cvol_new(card, dev, "AUX", AC97_AUX_VOL);
-		em28xx_cvol_new(card, dev, "PCM", AC97_PCM_OUT_VOL);
-
-		em28xx_cvol_new(card, dev, "Master", AC97_MASTER_VOL);
-		em28xx_cvol_new(card, dev, "Line", AC97_LINE_LEVEL_VOL);
-		em28xx_cvol_new(card, dev, "Mono", AC97_MASTER_MONO_VOL);
-		em28xx_cvol_new(card, dev, "LFE", AC97_LFE_MASTER_VOL);
-		em28xx_cvol_new(card, dev, "Surround", AC97_SURR_MASTER_VOL);
+		em28xx_cvol_new(card, dev, "Video", AC97_VIDEO);
+		em28xx_cvol_new(card, dev, "Line In", AC97_LINE);
+		em28xx_cvol_new(card, dev, "Phone", AC97_PHONE);
+		em28xx_cvol_new(card, dev, "Microphone", AC97_MIC);
+		em28xx_cvol_new(card, dev, "CD", AC97_CD);
+		em28xx_cvol_new(card, dev, "AUX", AC97_AUX);
+		em28xx_cvol_new(card, dev, "PCM", AC97_PCM);
+
+		em28xx_cvol_new(card, dev, "Master", AC97_MASTER);
+		em28xx_cvol_new(card, dev, "Line", AC97_HEADPHONE);
+		em28xx_cvol_new(card, dev, "Mono", AC97_MASTER_MONO);
+		em28xx_cvol_new(card, dev, "LFE", AC97_CENTER_LFE_MASTER);
+		em28xx_cvol_new(card, dev, "Surround", AC97_SURROUND_MASTER);
 	}
 
 	err = snd_card_register(card);
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index 862c6575c55..ca62b998138 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -975,12 +975,7 @@ struct em28xx_board em28xx_boards[] = {
 		.name         = "Terratec Cinergy HTC Stick",
 		.has_dvb      = 1,
 		.ir_codes     = RC_MAP_NEC_TERRATEC_CINERGY_XS,
-#if 0
-		.tuner_type   = TUNER_PHILIPS_TDA8290,
-		.tuner_addr   = 0x41,
-		.dvb_gpio     = terratec_h5_digital, /* FIXME: probably wrong */
-		.tuner_gpio   = terratec_h5_gpio,
-#endif
+		.tuner_type   = TUNER_ABSENT,
 		.i2c_speed    = EM2874_I2C_SECONDARY_BUS_SELECT |
 				EM28XX_I2C_CLK_WAIT_ENABLE |
 				EM28XX_I2C_FREQ_400_KHZ,
diff --git a/drivers/media/video/em28xx/em28xx-core.c b/drivers/media/video/em28xx/em28xx-core.c
index 5717bdee8f1..de2cb20ad2c 100644
--- a/drivers/media/video/em28xx/em28xx-core.c
+++ b/drivers/media/video/em28xx/em28xx-core.c
@@ -27,6 +27,7 @@
 #include <linux/slab.h>
 #include <linux/usb.h>
 #include <linux/vmalloc.h>
+#include <sound/ac97_codec.h>
 #include <media/v4l2-common.h>
 
 #include "em28xx.h"
@@ -326,13 +327,13 @@ struct em28xx_vol_itable {
 };
 
 static struct em28xx_vol_itable inputs[] = {
-	{ EM28XX_AMUX_VIDEO, 	AC97_VIDEO_VOL   },
-	{ EM28XX_AMUX_LINE_IN,	AC97_LINEIN_VOL  },
-	{ EM28XX_AMUX_PHONE,	AC97_PHONE_VOL   },
-	{ EM28XX_AMUX_MIC,	AC97_MIC_VOL     },
-	{ EM28XX_AMUX_CD,	AC97_CD_VOL      },
-	{ EM28XX_AMUX_AUX,	AC97_AUX_VOL     },
-	{ EM28XX_AMUX_PCM_OUT,	AC97_PCM_OUT_VOL },
+	{ EM28XX_AMUX_VIDEO,	AC97_VIDEO	},
+	{ EM28XX_AMUX_LINE_IN,	AC97_LINE	},
+	{ EM28XX_AMUX_PHONE,	AC97_PHONE	},
+	{ EM28XX_AMUX_MIC,	AC97_MIC	},
+	{ EM28XX_AMUX_CD,	AC97_CD		},
+	{ EM28XX_AMUX_AUX,	AC97_AUX	},
+	{ EM28XX_AMUX_PCM_OUT,	AC97_PCM	},
 };
 
 static int set_ac97_input(struct em28xx *dev)
@@ -415,11 +416,11 @@ struct em28xx_vol_otable {
 };
 
 static const struct em28xx_vol_otable outputs[] = {
-	{ EM28XX_AOUT_MASTER, AC97_MASTER_VOL      },
-	{ EM28XX_AOUT_LINE,   AC97_LINE_LEVEL_VOL  },
-	{ EM28XX_AOUT_MONO,   AC97_MASTER_MONO_VOL },
-	{ EM28XX_AOUT_LFE,    AC97_LFE_MASTER_VOL  },
-	{ EM28XX_AOUT_SURR,   AC97_SURR_MASTER_VOL },
+	{ EM28XX_AOUT_MASTER, AC97_MASTER		},
+	{ EM28XX_AOUT_LINE,   AC97_HEADPHONE		},
+	{ EM28XX_AOUT_MONO,   AC97_MASTER_MONO		},
+	{ EM28XX_AOUT_LFE,    AC97_CENTER_LFE_MASTER	},
+	{ EM28XX_AOUT_SURR,   AC97_SURROUND_MASTER	},
 };
 
 int em28xx_audio_analog_set(struct em28xx *dev)
@@ -459,9 +460,9 @@ int em28xx_audio_analog_set(struct em28xx *dev)
 	if (dev->audio_mode.ac97 != EM28XX_NO_AC97) {
 		int vol;
 
-		em28xx_write_ac97(dev, AC97_POWER_DOWN_CTRL, 0x4200);
-		em28xx_write_ac97(dev, AC97_EXT_AUD_CTRL, 0x0031);
-		em28xx_write_ac97(dev, AC97_PCM_IN_SRATE, 0xbb80);
+		em28xx_write_ac97(dev, AC97_POWERDOWN, 0x4200);
+		em28xx_write_ac97(dev, AC97_EXTENDED_STATUS, 0x0031);
+		em28xx_write_ac97(dev, AC97_PCM_LR_ADC_RATE, 0xbb80);
 
 		/* LSB: left channel - both channels with the same level */
 		vol = (0x1f - dev->volume) | ((0x1f - dev->volume) << 8);
@@ -487,7 +488,7 @@ int em28xx_audio_analog_set(struct em28xx *dev)
 			   channels */
 			sel |= (sel << 8);
 
-			em28xx_write_ac97(dev, AC97_RECORD_SELECT, sel);
+			em28xx_write_ac97(dev, AC97_REC_SEL, sel);
 		}
 	}
 
diff --git a/drivers/media/video/em28xx/em28xx-dvb.c b/drivers/media/video/em28xx/em28xx-dvb.c
index 16410ac2009..a16531fa937 100644
--- a/drivers/media/video/em28xx/em28xx-dvb.c
+++ b/drivers/media/video/em28xx/em28xx-dvb.c
@@ -310,31 +310,47 @@ static struct drxd_config em28xx_drxd = {
 	.disable_i2c_gate_ctrl = 1,
 };
 
-struct drxk_config terratec_h5_drxk = {
+static struct drxk_config terratec_h5_drxk = {
 	.adr = 0x29,
 	.single_master = 1,
 	.no_i2c_bridge = 1,
 	.microcode_name = "dvb-usb-terratec-h5-drxk.fw",
+	.qam_demod_parameter_count = 2,
 };
 
-struct drxk_config hauppauge_930c_drxk = {
+static struct drxk_config hauppauge_930c_drxk = {
 	.adr = 0x29,
 	.single_master = 1,
 	.no_i2c_bridge = 1,
 	.microcode_name = "dvb-usb-hauppauge-hvr930c-drxk.fw",
 	.chunk_size = 56,
+	.qam_demod_parameter_count = 2,
 };
 
-struct drxk_config maxmedia_ub425_tc_drxk = {
+struct drxk_config terratec_htc_stick_drxk = {
 	.adr = 0x29,
 	.single_master = 1,
 	.no_i2c_bridge = 1,
+	.microcode_name = "dvb-usb-terratec-htc-stick-drxk.fw",
+	.chunk_size = 54,
+	.qam_demod_parameter_count = 2,
+	/* Required for the antenna_gpio to disable LNA. */
+	.antenna_dvbt = true,
+	/* The windows driver uses the same. This will disable LNA. */
+	.antenna_gpio = 0x6,
 };
 
-struct drxk_config pctv_520e_drxk = {
+static struct drxk_config maxmedia_ub425_tc_drxk = {
+	.adr = 0x29,
+	.single_master = 1,
+	.no_i2c_bridge = 1,
+};
+
+static struct drxk_config pctv_520e_drxk = {
 	.adr = 0x29,
 	.single_master = 1,
 	.microcode_name = "dvb-demod-drxk-pctv.fw",
+	.qam_demod_parameter_count = 2,
 	.chunk_size = 58,
 	.antenna_dvbt = true, /* disable LNA */
 	.antenna_gpio = (1 << 2), /* disable LNA */
@@ -473,6 +489,57 @@ static void terratec_h5_init(struct em28xx *dev)
 	em28xx_gpio_set(dev, terratec_h5_end);
 };
 
+static void terratec_htc_stick_init(struct em28xx *dev)
+{
+	int i;
+
+	/*
+	 * GPIO configuration:
+	 * 0xff: unknown (does not affect DVB-T).
+	 * 0xf6: DRX-K (demodulator).
+	 * 0xe6: unknown (does not affect DVB-T).
+	 * 0xb6: unknown (does not affect DVB-T).
+	 */
+	struct em28xx_reg_seq terratec_htc_stick_init[] = {
+		{EM28XX_R08_GPIO,	0xff,	0xff,	10},
+		{EM2874_R80_GPIO,	0xf6,	0xff,	100},
+		{EM2874_R80_GPIO,	0xe6,	0xff,	50},
+		{EM2874_R80_GPIO,	0xf6,	0xff,	100},
+		{ -1,                   -1,     -1,     -1},
+	};
+	struct em28xx_reg_seq terratec_htc_stick_end[] = {
+		{EM2874_R80_GPIO,	0xb6,	0xff,	100},
+		{EM2874_R80_GPIO,	0xf6,	0xff,	50},
+		{ -1,                   -1,     -1,     -1},
+	};
+
+	/* Init the analog decoder? */
+	struct {
+		unsigned char r[4];
+		int len;
+	} regs[] = {
+		{{ 0x06, 0x02, 0x00, 0x31 }, 4},
+		{{ 0x01, 0x02 }, 2},
+		{{ 0x01, 0x02, 0x00, 0xc6 }, 4},
+		{{ 0x01, 0x00 }, 2},
+		{{ 0x01, 0x00, 0xff, 0xaf }, 4},
+	};
+
+	em28xx_gpio_set(dev, terratec_htc_stick_init);
+
+	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x40);
+	msleep(10);
+	em28xx_write_reg(dev, EM28XX_R06_I2C_CLK, 0x44);
+	msleep(10);
+
+	dev->i2c_client.addr = 0x82 >> 1;
+
+	for (i = 0; i < ARRAY_SIZE(regs); i++)
+		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
+
+	em28xx_gpio_set(dev, terratec_htc_stick_end);
+};
+
 static void pctv_520e_init(struct em28xx *dev)
 {
 	/*
@@ -944,7 +1011,6 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		break;
 	}
 	case EM2884_BOARD_TERRATEC_H5:
-	case EM2884_BOARD_CINERGY_HTC_STICK:
 		terratec_h5_init(dev);
 
 		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_h5_drxk, &dev->i2c_adap);
@@ -1021,6 +1087,25 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			}
 		}
 		break;
+	case EM2884_BOARD_CINERGY_HTC_STICK:
+		terratec_htc_stick_init(dev);
+
+		/* attach demodulator */
+		dvb->fe[0] = dvb_attach(drxk_attach, &terratec_htc_stick_drxk,
+					&dev->i2c_adap);
+		if (!dvb->fe[0]) {
+			result = -EINVAL;
+			goto out_free;
+		}
+
+		/* Attach the demodulator. */
+		if (!dvb_attach(tda18271_attach, dvb->fe[0], 0x60,
+				&dev->i2c_adap,
+				&em28xx_cxd2820r_tda18271_config)) {
+			result = -EINVAL;
+			goto out_free;
+		}
+		break;
 	default:
 		em28xx_errdev("/2: The frontend of your DVB/ATSC card"
 				" isn't supported yet\n");
diff --git a/drivers/media/video/em28xx/em28xx-i2c.c b/drivers/media/video/em28xx/em28xx-i2c.c
index 185db65b766..1683bd9d51e 100644
--- a/drivers/media/video/em28xx/em28xx-i2c.c
+++ b/drivers/media/video/em28xx/em28xx-i2c.c
@@ -475,6 +475,7 @@ static struct i2c_client em28xx_client_template = {
  */
 static char *i2c_devs[128] = {
 	[0x4a >> 1] = "saa7113h",
+	[0x52 >> 1] = "drxk",
 	[0x60 >> 1] = "remote IR sensor",
 	[0x8e >> 1] = "remote IR sensor",
 	[0x86 >> 1] = "tda9887",
diff --git a/drivers/media/video/em28xx/em28xx-input.c b/drivers/media/video/em28xx/em28xx-input.c
index 5e30c4f3f24..97d36b4f19d 100644
--- a/drivers/media/video/em28xx/em28xx-input.c
+++ b/drivers/media/video/em28xx/em28xx-input.c
@@ -345,7 +345,7 @@ static void em28xx_ir_stop(struct rc_dev *rc)
 	cancel_delayed_work_sync(&ir->work);
 }
 
-int em28xx_ir_change_protocol(struct rc_dev *rc_dev, u64 rc_type)
+static int em28xx_ir_change_protocol(struct rc_dev *rc_dev, u64 rc_type)
 {
 	int rc = 0;
 	struct em28xx_IR *ir = rc_dev->priv;
diff --git a/drivers/media/video/em28xx/em28xx-reg.h b/drivers/media/video/em28xx/em28xx-reg.h
index 2f626850572..6ff368297f6 100644
--- a/drivers/media/video/em28xx/em28xx-reg.h
+++ b/drivers/media/video/em28xx/em28xx-reg.h
@@ -211,58 +211,9 @@ enum em28xx_chip_id {
 };
 
 /*
- * Registers used by em202 and other AC97 chips
+ * Registers used by em202
  */
 
-/* Standard AC97 registers */
-#define AC97_RESET               0x00
-
-	/* Output volumes */
-#define AC97_MASTER_VOL          0x02
-#define AC97_LINE_LEVEL_VOL      0x04	/* Some devices use for headphones */
-#define AC97_MASTER_MONO_VOL     0x06
-
-	/* Input volumes */
-#define AC97_PC_BEEP_VOL         0x0a
-#define AC97_PHONE_VOL           0x0c
-#define AC97_MIC_VOL             0x0e
-#define AC97_LINEIN_VOL          0x10
-#define AC97_CD_VOL              0x12
-#define AC97_VIDEO_VOL           0x14
-#define AC97_AUX_VOL             0x16
-#define AC97_PCM_OUT_VOL         0x18
-
-	/* capture registers */
-#define AC97_RECORD_SELECT       0x1a
-#define AC97_RECORD_GAIN         0x1c
-
-	/* control registers */
-#define AC97_GENERAL_PURPOSE     0x20
-#define AC97_3D_CTRL             0x22
-#define AC97_AUD_INT_AND_PAG     0x24
-#define AC97_POWER_DOWN_CTRL     0x26
-#define AC97_EXT_AUD_ID          0x28
-#define AC97_EXT_AUD_CTRL        0x2a
-
-/* Supported rate varies for each AC97 device
-   if write an unsupported value, it will return the closest one
- */
-#define AC97_PCM_OUT_FRONT_SRATE 0x2c
-#define AC97_PCM_OUT_SURR_SRATE  0x2e
-#define AC97_PCM_OUT_LFE_SRATE   0x30
-#define AC97_PCM_IN_SRATE        0x32
-
-	/* For devices with more than 2 channels, extra output volumes */
-#define AC97_LFE_MASTER_VOL      0x36
-#define AC97_SURR_MASTER_VOL     0x38
-
-	/* Digital SPDIF output control */
-#define AC97_SPDIF_OUT_CTRL      0x3a
-
-	/* Vendor ID identifier */
-#define AC97_VENDOR_ID1          0x7c
-#define AC97_VENDOR_ID2          0x7e
-
 /* EMP202 vendor registers */
 #define EM202_EXT_MODEM_CTRL     0x3e
 #define EM202_GPIO_CONF          0x4c
diff --git a/drivers/media/video/gspca/benq.c b/drivers/media/video/gspca/benq.c
index 9769f17915c..352f32190e6 100644
--- a/drivers/media/video/gspca/benq.c
+++ b/drivers/media/video/gspca/benq.c
@@ -33,10 +33,6 @@ struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 };
 
-/* V4L2 controls supported by the driver */
-static const struct ctrl sd_ctrls[] = {
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -256,8 +252,6 @@ static void sd_isoc_irq(struct urb *urb)
 /* 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,
@@ -288,6 +282,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/conex.c b/drivers/media/video/gspca/conex.c
index f39fee0fd10..c9052f20435 100644
--- a/drivers/media/video/gspca/conex.c
+++ b/drivers/media/video/gspca/conex.c
@@ -31,74 +31,18 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA USB Conexant Camera Driver");
 MODULE_LICENSE("GPL");
 
+#define QUALITY 50
+
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-
-	unsigned char brightness;
-	unsigned char contrast;
-	unsigned char colors;
-	u8 quality;
-#define QUALITY_MIN 30
-#define QUALITY_MAX 60
-#define QUALITY_DEF 40
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *contrast;
+	struct v4l2_ctrl *sat;
 
 	u8 jpeg_hdr[JPEG_HDR_SZ];
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id	 = V4L2_CID_BRIGHTNESS,
-		.type	 = V4L2_CTRL_TYPE_INTEGER,
-		.name	 = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step	 = 1,
-#define BRIGHTNESS_DEF 0xd4
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0x0a,
-		.maximum = 0x1f,
-		.step    = 1,
-#define CONTRAST_DEF 0x0c
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 0,
-		.maximum = 7,
-		.step    = 1,
-#define COLOR_DEF 3
-		.default_value = COLOR_DEF,
-	    },
-	    .set = sd_setcolors,
-	    .get = sd_getcolors,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
@@ -817,17 +761,11 @@ static void cx11646_init1(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 
 	cam = &gspca_dev->cam;
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
-
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->colors = COLOR_DEF;
-	sd->quality = QUALITY_DEF;
 	return 0;
 }
 
@@ -849,7 +787,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x22);		/* JPEG 411 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	jpeg_set_qual(sd->jpeg_hdr, QUALITY);
 
 	cx11646_initsize(gspca_dev);
 	cx11646_fw(gspca_dev);
@@ -903,142 +841,99 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val, s32 sat)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	__u8 regE5cbx[] = { 0x88, 0x00, 0xd4, 0x01, 0x88, 0x01, 0x01, 0x01 };
 	__u8 reg51c[2];
-	__u8 bright;
-	__u8 colors;
 
-	bright = sd->brightness;
-	regE5cbx[2] = bright;
+	regE5cbx[2] = val;
 	reg_w(gspca_dev, 0x00e5, regE5cbx, 8);
 	reg_r(gspca_dev, 0x00e8, 8);
 	reg_w(gspca_dev, 0x00e5, regE5c, 4);
 	reg_r(gspca_dev, 0x00e8, 1);		/* 0x00 */
 
-	colors = sd->colors;
 	reg51c[0] = 0x77;
-	reg51c[1] = colors;
+	reg51c[1] = sat;
 	reg_w(gspca_dev, 0x0051, reg51c, 2);
 	reg_w(gspca_dev, 0x0010, reg10, 2);
 	reg_w_val(gspca_dev, 0x0070, reg70);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val, s32 sat)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	__u8 regE5acx[] = { 0x88, 0x0a, 0x0c, 0x01 };	/* seem MSB */
 /*	__u8 regE5bcx[] = { 0x88, 0x0b, 0x12, 0x01};	 * LSB */
 	__u8 reg51c[2];
 
-	regE5acx[2] = sd->contrast;
+	regE5acx[2] = val;
 	reg_w(gspca_dev, 0x00e5, regE5acx, 4);
 	reg_r(gspca_dev, 0x00e8, 1);		/* 0x00 */
 	reg51c[0] = 0x77;
-	reg51c[1] = sd->colors;
+	reg51c[1] = sat;
 	reg_w(gspca_dev, 0x0051, reg51c, 2);
 	reg_w(gspca_dev, 0x0010, reg10, 2);
 	reg_w_val(gspca_dev, 0x0070, reg70);
 }
 
-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)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
 
-	*val = sd->contrast;
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	sd->colors = val;
-	if (gspca_dev->streaming) {
-		setbrightness(gspca_dev);
-		setcontrast(gspca_dev);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val, sd->sat->cur.val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val, sd->sat->cur.val);
+		break;
+	case V4L2_CID_SATURATION:
+		setbrightness(gspca_dev, sd->brightness->cur.val, ctrl->val);
+		setcontrast(gspca_dev, sd->contrast->cur.val, ctrl->val);
+		break;
 	}
-	return 0;
+	return gspca_dev->usb_err;
 }
 
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
-}
-
-static int sd_set_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	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;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_get_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
-	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
-			| V4L2_JPEG_MARKER_DQT;
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 0xd4);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0x0a, 0x1f, 1, 0x0c);
+	sd->sat = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 7, 1, 3);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
-	.get_jcomp = sd_get_jcomp,
-	.set_jcomp = sd_set_jcomp,
 };
 
 /* -- module initialisation -- */
@@ -1064,6 +959,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/cpia1.c b/drivers/media/video/gspca/cpia1.c
index 8f33bbd091a..2499a881d9a 100644
--- a/drivers/media/video/gspca/cpia1.c
+++ b/drivers/media/video/gspca/cpia1.c
@@ -225,6 +225,15 @@ MODULE_LICENSE("GPL");
 #define FIRMWARE_VERSION(x, y) (sd->params.version.firmwareVersion == (x) && \
 				sd->params.version.firmwareRevision == (y))
 
+#define CPIA1_CID_COMP_TARGET (V4L2_CTRL_CLASS_USER + 0x1000)
+#define BRIGHTNESS_DEF 50
+#define CONTRAST_DEF 48
+#define SATURATION_DEF 50
+#define FREQ_DEF V4L2_CID_POWER_LINE_FREQUENCY_50HZ
+#define ILLUMINATORS_1_DEF 0
+#define ILLUMINATORS_2_DEF 0
+#define COMP_TARGET_DEF CPIA_COMPRESSION_TARGET_QUALITY
+
 /* Developer's Guide Table 5 p 3-34
  * indexed by [mains][sensorFps.baserate][sensorFps.divisor]*/
 static u8 flicker_jumps[2][2][4] =
@@ -360,135 +369,9 @@ struct sd {
 	atomic_t fps;
 	int exposure_count;
 	u8 exposure_status;
+	struct v4l2_ctrl *freq;
 	u8 mainsFreq;				/* 0 = 50hz, 1 = 60hz */
 	u8 first_frame;
-	u8 freq;
-};
-
-/* V4L2 controls supported by the driver */
-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_setsaturation(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getsaturation(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setcomptarget(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcomptarget(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setilluminator1(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getilluminator1(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setilluminator2(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getilluminator2(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-#define BRIGHTNESS_IDX 0
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 100,
-		.step = 1,
-#define BRIGHTNESS_DEF 50
-		.default_value = BRIGHTNESS_DEF,
-		.flags = 0,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define CONTRAST_IDX 1
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 96,
-		.step    = 8,
-#define CONTRAST_DEF 48
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-#define SATURATION_IDX 2
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 100,
-		.step    = 1,
-#define SATURATION_DEF 50
-		.default_value = SATURATION_DEF,
-	    },
-	    .set = sd_setsaturation,
-	    .get = sd_getsaturation,
-	},
-#define POWER_LINE_FREQUENCY_IDX 3
-	{
-		{
-			.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-			.type    = V4L2_CTRL_TYPE_MENU,
-			.name    = "Light frequency filter",
-			.minimum = 0,
-			.maximum = 2,	/* 0: 0, 1: 50Hz, 2:60Hz */
-			.step    = 1,
-#define FREQ_DEF 1
-			.default_value = FREQ_DEF,
-		},
-		.set = sd_setfreq,
-		.get = sd_getfreq,
-	},
-#define ILLUMINATORS_1_IDX 4
-	{
-		{
-			.id	 = V4L2_CID_ILLUMINATORS_1,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "Illuminator 1",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-#define ILLUMINATORS_1_DEF 0
-			.default_value = ILLUMINATORS_1_DEF,
-		},
-		.set = sd_setilluminator1,
-		.get = sd_getilluminator1,
-	},
-#define ILLUMINATORS_2_IDX 5
-	{
-		{
-			.id	 = V4L2_CID_ILLUMINATORS_2,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "Illuminator 2",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-#define ILLUMINATORS_2_DEF 0
-			.default_value = ILLUMINATORS_2_DEF,
-		},
-		.set = sd_setilluminator2,
-		.get = sd_getilluminator2,
-	},
-#define COMP_TARGET_IDX 6
-	{
-		{
-#define V4L2_CID_COMP_TARGET V4L2_CID_PRIVATE_BASE
-			.id	 = V4L2_CID_COMP_TARGET,
-			.type    = V4L2_CTRL_TYPE_MENU,
-			.name    = "Compression Target",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-#define COMP_TARGET_DEF CPIA_COMPRESSION_TARGET_QUALITY
-			.default_value = COMP_TARGET_DEF,
-		},
-		.set = sd_setcomptarget,
-		.get = sd_getcomptarget,
-	},
 };
 
 static const struct v4l2_pix_format mode[] = {
@@ -770,15 +653,6 @@ static void reset_camera_params(struct gspca_dev *gspca_dev)
 	params->apcor.gain2 = 0x16;
 	params->apcor.gain4 = 0x24;
 	params->apcor.gain8 = 0x34;
-	params->flickerControl.flickerMode = 0;
-	params->flickerControl.disabled = 1;
-
-	params->flickerControl.coarseJump =
-		flicker_jumps[sd->mainsFreq]
-			     [params->sensorFps.baserate]
-			     [params->sensorFps.divisor];
-	params->flickerControl.allowableOverExposure =
-		find_over_exposure(params->colourParams.brightness);
 	params->vlOffset.gain1 = 20;
 	params->vlOffset.gain2 = 24;
 	params->vlOffset.gain4 = 26;
@@ -798,6 +672,15 @@ static void reset_camera_params(struct gspca_dev *gspca_dev)
 	params->sensorFps.divisor = 1;
 	params->sensorFps.baserate = 1;
 
+	params->flickerControl.flickerMode = 0;
+	params->flickerControl.disabled = 1;
+	params->flickerControl.coarseJump =
+		flicker_jumps[sd->mainsFreq]
+			     [params->sensorFps.baserate]
+			     [params->sensorFps.divisor];
+	params->flickerControl.allowableOverExposure =
+		find_over_exposure(params->colourParams.brightness);
+
 	params->yuvThreshold.yThreshold = 6; /* From windows driver */
 	params->yuvThreshold.uvThreshold = 6; /* From windows driver */
 
@@ -1110,9 +993,6 @@ static int command_setlights(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	int ret, p1, p2;
 
-	if (!sd->params.qx3.qx3_detected)
-		return 0;
-
 	p1 = (sd->params.qx3.bottomlight == 0) << 1;
 	p2 = (sd->params.qx3.toplight == 0) << 3;
 
@@ -1551,8 +1431,10 @@ static void restart_flicker(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 
+	sd->mainsFreq = FREQ_DEF == V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
 	reset_camera_params(gspca_dev);
 
 	PDEBUG(D_PROBE, "cpia CPiA camera detected (vid/pid 0x%04X:0x%04X)",
@@ -1562,8 +1444,25 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam->cam_mode = mode;
 	cam->nmodes = ARRAY_SIZE(mode);
 
-	sd_setfreq(gspca_dev, FREQ_DEF);
+	goto_low_power(gspca_dev);
+	/* Check the firmware version. */
+	sd->params.version.firmwareVersion = 0;
+	get_version_information(gspca_dev);
+	if (sd->params.version.firmwareVersion != 1) {
+		PDEBUG(D_ERR, "only firmware version 1 is supported (got: %d)",
+		       sd->params.version.firmwareVersion);
+		return -ENODEV;
+	}
 
+	/* A bug in firmware 1-02 limits gainMode to 2 */
+	if (sd->params.version.firmwareRevision <= 2 &&
+	    sd->params.exposure.gainMode > 2) {
+		sd->params.exposure.gainMode = 2;
+	}
+
+	/* set QX3 detected flag */
+	sd->params.qx3.qx3_detected = (sd->params.pnpID.vendor == 0x0813 &&
+				       sd->params.pnpID.product == 0x0001);
 	return 0;
 }
 
@@ -1602,21 +1501,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* Check the firmware version. */
 	sd->params.version.firmwareVersion = 0;
 	get_version_information(gspca_dev);
-	if (sd->params.version.firmwareVersion != 1) {
-		PDEBUG(D_ERR, "only firmware version 1 is supported (got: %d)",
-		       sd->params.version.firmwareVersion);
-		return -ENODEV;
-	}
-
-	/* A bug in firmware 1-02 limits gainMode to 2 */
-	if (sd->params.version.firmwareRevision <= 2 &&
-	    sd->params.exposure.gainMode > 2) {
-		sd->params.exposure.gainMode = 2;
-	}
-
-	/* set QX3 detected flag */
-	sd->params.qx3.qx3_detected = (sd->params.pnpID.vendor == 0x0813 &&
-				       sd->params.pnpID.product == 0x0001);
 
 	/* The fatal error checking should be done after
 	 * the camera powers up (developer's guide p 3-38) */
@@ -1785,9 +1669,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	   or disable the illuminator controls, if this isn't a QX3 */
 	if (sd->params.qx3.qx3_detected)
 		command_setlights(gspca_dev);
-	else
-		gspca_dev->ctrl_dis |=
-			((1 << ILLUMINATORS_1_IDX) | (1 << ILLUMINATORS_2_IDX));
 
 	sd_stopN(gspca_dev);
 
@@ -1871,235 +1752,123 @@ static void sd_dq_callback(struct gspca_dev *gspca_dev)
 	do_command(gspca_dev, CPIA_COMMAND_ReadMCPorts, 0, 0, 0, 0);
 }
 
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int ret;
-
-	sd->params.colourParams.brightness = val;
-	sd->params.flickerControl.allowableOverExposure =
-		find_over_exposure(sd->params.colourParams.brightness);
-	if (gspca_dev->streaming) {
-		ret = command_setcolourparams(gspca_dev);
-		if (ret)
-			return ret;
-		return command_setflickerctrl(gspca_dev);
-	}
-	return 0;
-}
-
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->params.colourParams.brightness;
-	return 0;
-}
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
 
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	gspca_dev->usb_err = 0;
 
-	sd->params.colourParams.contrast = val;
-	if (gspca_dev->streaming)
-		return command_setcolourparams(gspca_dev);
-
-	return 0;
-}
-
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->params.colourParams.contrast;
-	return 0;
-}
-
-static int sd_setsaturation(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->params.colourParams.saturation = val;
-	if (gspca_dev->streaming)
-		return command_setcolourparams(gspca_dev);
-
-	return 0;
-}
-
-static int sd_getsaturation(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->params.colourParams.saturation;
-	return 0;
-}
-
-static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int on;
+	if (!gspca_dev->streaming && ctrl->id != V4L2_CID_POWER_LINE_FREQUENCY)
+		return 0;
 
-	switch (val) {
-	case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-		on = 0;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		sd->params.colourParams.brightness = ctrl->val;
+		sd->params.flickerControl.allowableOverExposure =
+			find_over_exposure(sd->params.colourParams.brightness);
+		gspca_dev->usb_err = command_setcolourparams(gspca_dev);
+		if (!gspca_dev->usb_err)
+			gspca_dev->usb_err = command_setflickerctrl(gspca_dev);
 		break;
-	case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-		on = 1;
-		sd->mainsFreq = 0;
+	case V4L2_CID_CONTRAST:
+		sd->params.colourParams.contrast = ctrl->val;
+		gspca_dev->usb_err = command_setcolourparams(gspca_dev);
 		break;
-	case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-		on = 1;
-		sd->mainsFreq = 1;
+	case V4L2_CID_SATURATION:
+		sd->params.colourParams.saturation = ctrl->val;
+		gspca_dev->usb_err = command_setcolourparams(gspca_dev);
 		break;
-	default:
-		return -EINVAL;
-	}
-
-	sd->freq = val;
-	sd->params.flickerControl.coarseJump =
-		flicker_jumps[sd->mainsFreq]
-			     [sd->params.sensorFps.baserate]
-			     [sd->params.sensorFps.divisor];
-
-	return set_flicker(gspca_dev, on, gspca_dev->streaming);
-}
-
-static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->freq;
-	return 0;
-}
-
-static int sd_setcomptarget(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->params.compressionTarget.frTargeting = val;
-	if (gspca_dev->streaming)
-		return command_setcompressiontarget(gspca_dev);
-
-	return 0;
-}
-
-static int sd_getcomptarget(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->params.compressionTarget.frTargeting;
-	return 0;
-}
-
-static int sd_setilluminator(struct gspca_dev *gspca_dev, __s32 val, int n)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int ret;
-
-	if (!sd->params.qx3.qx3_detected)
-		return -EINVAL;
-
-	switch (n) {
-	case 1:
-		sd->params.qx3.bottomlight = val ? 1 : 0;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		sd->mainsFreq = ctrl->val == V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
+		sd->params.flickerControl.coarseJump =
+			flicker_jumps[sd->mainsFreq]
+			[sd->params.sensorFps.baserate]
+			[sd->params.sensorFps.divisor];
+
+		gspca_dev->usb_err = set_flicker(gspca_dev,
+			ctrl->val != V4L2_CID_POWER_LINE_FREQUENCY_DISABLED,
+			gspca_dev->streaming);
 		break;
-	case 2:
-		sd->params.qx3.toplight = val ? 1 : 0;
+	case V4L2_CID_ILLUMINATORS_1:
+		sd->params.qx3.bottomlight = ctrl->val;
+		gspca_dev->usb_err = command_setlights(gspca_dev);
 		break;
-	default:
-		return -EINVAL;
-	}
-
-	ret = command_setlights(gspca_dev);
-	if (ret && ret != -EINVAL)
-		ret = -EBUSY;
-
-	return ret;
-}
-
-static int sd_setilluminator1(struct gspca_dev *gspca_dev, __s32 val)
-{
-	return sd_setilluminator(gspca_dev, val, 1);
-}
-
-static int sd_setilluminator2(struct gspca_dev *gspca_dev, __s32 val)
-{
-	return sd_setilluminator(gspca_dev, val, 2);
-}
-
-static int sd_getilluminator(struct gspca_dev *gspca_dev, __s32 *val, int n)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (!sd->params.qx3.qx3_detected)
-		return -EINVAL;
-
-	switch (n) {
-	case 1:
-		*val = sd->params.qx3.bottomlight;
+	case V4L2_CID_ILLUMINATORS_2:
+		sd->params.qx3.toplight = ctrl->val;
+		gspca_dev->usb_err = command_setlights(gspca_dev);
 		break;
-	case 2:
-		*val = sd->params.qx3.toplight;
+	case CPIA1_CID_COMP_TARGET:
+		sd->params.compressionTarget.frTargeting = ctrl->val;
+		gspca_dev->usb_err = command_setcompressiontarget(gspca_dev);
 		break;
-	default:
-		return -EINVAL;
 	}
-	return 0;
+	return gspca_dev->usb_err;
 }
 
-static int sd_getilluminator1(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	return sd_getilluminator(gspca_dev, val, 1);
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_getilluminator2(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	return sd_getilluminator(gspca_dev, val, 2);
-}
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	static const char * const comp_target_menu[] = {
+		"Quality",
+		"Framerate",
+		NULL
+	};
+	static const struct v4l2_ctrl_config comp_target = {
+		.ops = &sd_ctrl_ops,
+		.id = CPIA1_CID_COMP_TARGET,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.name = "Compression Target",
+		.qmenu = comp_target_menu,
+		.max = 1,
+		.def = COMP_TARGET_DEF,
+	};
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 7);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 100, 1, BRIGHTNESS_DEF);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 96, 8, CONTRAST_DEF);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 100, 1, SATURATION_DEF);
+	sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			FREQ_DEF);
+	if (sd->params.qx3.qx3_detected) {
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_ILLUMINATORS_1, 0, 1, 1,
+				ILLUMINATORS_1_DEF);
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_ILLUMINATORS_2, 0, 1, 1,
+				ILLUMINATORS_2_DEF);
+	}
+	v4l2_ctrl_new_custom(hdl, &comp_target, NULL);
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
-		break;
-	case V4L2_CID_COMP_TARGET:
-		switch (menu->index) {
-		case CPIA_COMPRESSION_TARGET_QUALITY:
-			strcpy((char *) menu->name, "Quality");
-			return 0;
-		case CPIA_COMPRESSION_TARGET_FRAMERATE:
-			strcpy((char *) menu->name, "Framerate");
-			return 0;
-		}
-		break;
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
 	}
-	return -EINVAL;
+	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.dq_callback = sd_dq_callback,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 	.other_input = 1,
 #endif
@@ -2129,6 +1898,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/etoms.c b/drivers/media/video/gspca/etoms.c
index 81a4adbd9f7..38f68e11c3a 100644
--- a/drivers/media/video/gspca/etoms.c
+++ b/drivers/media/video/gspca/etoms.c
@@ -32,9 +32,6 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	unsigned char brightness;
-	unsigned char contrast;
-	unsigned char colors;
 	unsigned char autogain;
 
 	char sensor;
@@ -44,76 +41,6 @@ struct sd {
 #define AG_CNT_START 13
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	 {
-	  .id = V4L2_CID_BRIGHTNESS,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Brightness",
-	  .minimum = 1,
-	  .maximum = 127,
-	  .step = 1,
-#define BRIGHTNESS_DEF 63
-	  .default_value = BRIGHTNESS_DEF,
-	  },
-	 .set = sd_setbrightness,
-	 .get = sd_getbrightness,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_CONTRAST,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Contrast",
-	  .minimum = 0,
-	  .maximum = 255,
-	  .step = 1,
-#define CONTRAST_DEF 127
-	  .default_value = CONTRAST_DEF,
-	  },
-	 .set = sd_setcontrast,
-	 .get = sd_getcontrast,
-	 },
-#define COLOR_IDX 2
-	{
-	 {
-	  .id = V4L2_CID_SATURATION,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Color",
-	  .minimum = 0,
-	  .maximum = 15,
-	  .step = 1,
-#define COLOR_DEF 7
-	  .default_value = COLOR_DEF,
-	  },
-	 .set = sd_setcolors,
-	 .get = sd_getcolors,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_AUTOGAIN,
-	  .type = V4L2_CTRL_TYPE_BOOLEAN,
-	  .name = "Auto Gain",
-	  .minimum = 0,
-	  .maximum = 1,
-	  .step = 1,
-#define AUTOGAIN_DEF 1
-	  .default_value = AUTOGAIN_DEF,
-	  },
-	 .set = sd_setautogain,
-	 .get = sd_getautogain,
-	 },
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -464,36 +391,31 @@ static void Et_init2(struct gspca_dev *gspca_dev)
 	reg_w_val(gspca_dev, 0x80, 0x20);	/* 0x20; */
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	int i;
-	__u8 brightness = sd->brightness;
 
 	for (i = 0; i < 4; i++)
-		reg_w_val(gspca_dev, ET_O_RED + i, brightness);
+		reg_w_val(gspca_dev, ET_O_RED + i, val);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	__u8 RGBG[] = { 0x80, 0x80, 0x80, 0x80, 0x00, 0x00 };
-	__u8 contrast = sd->contrast;
 
-	memset(RGBG, contrast, sizeof(RGBG) - 2);
+	memset(RGBG, val, sizeof(RGBG) - 2);
 	reg_w(gspca_dev, ET_G_RED, RGBG, 6);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	__u8 I2cc[] = { 0x05, 0x02, 0x02, 0x05, 0x0d };
 	__u8 i2cflags = 0x01;
 	/* __u8 green = 0; */
-	__u8 colors = sd->colors;
 
-	I2cc[3] = colors;	/* red */
-	I2cc[0] = 15 - colors;	/* blue */
+	I2cc[3] = val;	/* red */
+	I2cc[0] = 15 - val;	/* blue */
 	/* green = 15 - ((((7*I2cc[0]) >> 2 ) + I2cc[3]) >> 1); */
 	/* I2cc[1] = I2cc[2] = green; */
 	if (sd->sensor == SENSOR_PAS106) {
@@ -504,15 +426,16 @@ static void setcolors(struct gspca_dev *gspca_dev)
 		I2cc[3], I2cc[0], green); */
 }
 
-static void getcolors(struct gspca_dev *gspca_dev)
+static s32 getcolors(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->sensor == SENSOR_PAS106) {
 /*		i2c_r(gspca_dev, PAS106_REG9);		 * blue */
 		i2c_r(gspca_dev, PAS106_REG9 + 3);	/* red */
-		sd->colors = gspca_dev->usb_buf[0] & 0x0f;
+		return gspca_dev->usb_buf[0] & 0x0f;
 	}
+	return 0;
 }
 
 static void setautogain(struct gspca_dev *gspca_dev)
@@ -622,8 +545,7 @@ static void Et_init1(struct gspca_dev *gspca_dev)
 	i2c_w(gspca_dev, PAS106_REG7, I2c4, sizeof I2c4, 1);
 	/* now set by fifo the whole colors setting */
 	reg_w(gspca_dev, ET_G_RED, GainRGBG, 6);
-	getcolors(gspca_dev);
-	setcolors(gspca_dev);
+	setcolors(gspca_dev, getcolors(gspca_dev));
 }
 
 /* this function is called at probe time */
@@ -641,12 +563,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	} else {
 		cam->cam_mode = vga_mode;
 		cam->nmodes = ARRAY_SIZE(vga_mode);
-		gspca_dev->ctrl_dis = (1 << COLOR_IDX);
 	}
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->colors = COLOR_DEF;
-	sd->autogain = AUTOGAIN_DEF;
 	sd->ag_cnt = -1;
 	return 0;
 }
@@ -780,85 +697,68 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-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)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	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_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
-	return 0;
-}
-
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
-}
-
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	if (gspca_dev->streaming)
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		sd->autogain = ctrl->val;
 		setautogain(gspca_dev);
-	return 0;
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-	*val = sd->autogain;
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 1, 127, 1, 63);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 127);
+	if (sd->sensor == SENSOR_PAS106)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 15, 1, 7);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -892,6 +792,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/finepix.c b/drivers/media/video/gspca/finepix.c
index 6e26c93b465..c8f2201cc35 100644
--- a/drivers/media/video/gspca/finepix.c
+++ b/drivers/media/video/gspca/finepix.c
@@ -299,6 +299,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume  = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/gl860/gl860.c b/drivers/media/video/gspca/gl860/gl860.c
index c549574c1c7..ced3b71f14e 100644
--- a/drivers/media/video/gspca/gl860/gl860.c
+++ b/drivers/media/video/gspca/gl860/gl860.c
@@ -521,6 +521,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend    = gspca_suspend,
 	.resume     = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/gspca.c b/drivers/media/video/gspca/gspca.c
index 31721eadc59..d4e8343f5b1 100644
--- a/drivers/media/video/gspca/gspca.c
+++ b/drivers/media/video/gspca/gspca.c
@@ -930,6 +930,7 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 			goto out;
 		}
 		gspca_dev->streaming = 1;
+		v4l2_ctrl_handler_setup(gspca_dev->vdev.ctrl_handler);
 
 		/* some bulk transfers are started by the subdriver */
 		if (gspca_dev->cam.bulk && gspca_dev->cam.bulk_nurbs == 0)
@@ -1049,12 +1050,6 @@ static int vidioc_g_register(struct file *file, void *priv,
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -ENOTTY;
-
-	if (!gspca_dev->sd_desc->get_register)
-		return -ENOTTY;
-
 	gspca_dev->usb_err = 0;
 	return gspca_dev->sd_desc->get_register(gspca_dev, reg);
 }
@@ -1064,12 +1059,6 @@ static int vidioc_s_register(struct file *file, void *priv,
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -ENOTTY;
-
-	if (!gspca_dev->sd_desc->set_register)
-		return -ENOTTY;
-
 	gspca_dev->usb_err = 0;
 	return gspca_dev->sd_desc->set_register(gspca_dev, reg);
 }
@@ -1080,9 +1069,6 @@ static int vidioc_g_chip_ident(struct file *file, void *priv,
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	if (!gspca_dev->sd_desc->get_chip_ident)
-		return -ENOTTY;
-
 	gspca_dev->usb_err = 0;
 	return gspca_dev->sd_desc->get_chip_ident(gspca_dev, chip);
 }
@@ -1136,8 +1122,10 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	int mode;
 
 	mode = gspca_dev->curr_mode;
-	memcpy(&fmt->fmt.pix, &gspca_dev->cam.cam_mode[mode],
-		sizeof fmt->fmt.pix);
+	fmt->fmt.pix = gspca_dev->cam.cam_mode[mode];
+	/* some drivers use priv internally, zero it before giving it to
+	   userspace */
+	fmt->fmt.pix.priv = 0;
 	return 0;
 }
 
@@ -1168,8 +1156,10 @@ static int try_fmt_vid_cap(struct gspca_dev *gspca_dev,
 /*		else
 			;		 * no chance, return this mode */
 	}
-	memcpy(&fmt->fmt.pix, &gspca_dev->cam.cam_mode[mode],
-		sizeof fmt->fmt.pix);
+	fmt->fmt.pix = gspca_dev->cam.cam_mode[mode];
+	/* some drivers use priv internally, zero it before giving it to
+	   userspace */
+	fmt->fmt.pix.priv = 0;
 	return mode;			/* used when s_fmt */
 }
 
@@ -1284,9 +1274,6 @@ static void gspca_release(struct v4l2_device *v4l2_device)
 	struct gspca_dev *gspca_dev =
 		container_of(v4l2_device, struct gspca_dev, v4l2_dev);
 
-	PDEBUG(D_PROBE, "%s released",
-		video_device_node_name(&gspca_dev->vdev));
-
 	v4l2_ctrl_handler_free(gspca_dev->vdev.ctrl_handler);
 	v4l2_device_unregister(&gspca_dev->v4l2_dev);
 	kfree(gspca_dev->usb_buf);
@@ -1694,8 +1681,6 @@ static int vidioc_g_jpegcomp(struct file *file, void *priv,
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	if (!gspca_dev->sd_desc->get_jcomp)
-		return -ENOTTY;
 	gspca_dev->usb_err = 0;
 	return gspca_dev->sd_desc->get_jcomp(gspca_dev, jpegcomp);
 }
@@ -1705,8 +1690,6 @@ static int vidioc_s_jpegcomp(struct file *file, void *priv,
 {
 	struct gspca_dev *gspca_dev = video_drvdata(file);
 
-	if (!gspca_dev->sd_desc->set_jcomp)
-		return -ENOTTY;
 	gspca_dev->usb_err = 0;
 	return gspca_dev->sd_desc->set_jcomp(gspca_dev, jpegcomp);
 }
@@ -2290,6 +2273,20 @@ int gspca_dev_probe2(struct usb_interface *intf,
 	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_DQBUF);
 	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_QBUF);
 	v4l2_disable_ioctl_locking(&gspca_dev->vdev, VIDIOC_QUERYBUF);
+	if (!gspca_dev->sd_desc->get_chip_ident)
+		v4l2_disable_ioctl(&gspca_dev->vdev, VIDIOC_DBG_G_CHIP_IDENT);
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	if (!gspca_dev->sd_desc->get_chip_ident ||
+	    !gspca_dev->sd_desc->get_register)
+		v4l2_disable_ioctl(&gspca_dev->vdev, VIDIOC_DBG_G_REGISTER);
+	if (!gspca_dev->sd_desc->get_chip_ident ||
+	    !gspca_dev->sd_desc->set_register)
+		v4l2_disable_ioctl(&gspca_dev->vdev, VIDIOC_DBG_S_REGISTER);
+#endif
+	if (!gspca_dev->sd_desc->get_jcomp)
+		v4l2_disable_ioctl(&gspca_dev->vdev, VIDIOC_G_JPEGCOMP);
+	if (!gspca_dev->sd_desc->set_jcomp)
+		v4l2_disable_ioctl(&gspca_dev->vdev, VIDIOC_S_JPEGCOMP);
 
 	/* init video stuff */
 	ret = video_register_device(&gspca_dev->vdev,
@@ -2429,7 +2426,6 @@ int gspca_resume(struct usb_interface *intf)
 	 */
 	streaming = gspca_dev->streaming;
 	gspca_dev->streaming = 0;
-	v4l2_ctrl_handler_setup(gspca_dev->vdev.ctrl_handler);
 	if (streaming)
 		ret = gspca_init_transfer(gspca_dev);
 	mutex_unlock(&gspca_dev->usb_lock);
diff --git a/drivers/media/video/gspca/jeilinj.c b/drivers/media/video/gspca/jeilinj.c
index 5ab3f7e1276..26b99310d62 100644
--- a/drivers/media/video/gspca/jeilinj.c
+++ b/drivers/media/video/gspca/jeilinj.c
@@ -54,21 +54,13 @@ enum {
 #define CAMQUALITY_MIN 0	/* highest cam quality */
 #define CAMQUALITY_MAX 97	/* lowest cam quality  */
 
-enum e_ctrl {
-	LIGHTFREQ,
-	AUTOGAIN,
-	RED,
-	GREEN,
-	BLUE,
-	NCTRLS		/* number of controls */
-};
-
 /* Structure to hold all of our device specific stuff */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-	struct gspca_ctrl ctrls[NCTRLS];
 	int blocks_left;
 	const struct v4l2_pix_format *cap_mode;
+	struct v4l2_ctrl *freq;
+	struct v4l2_ctrl *jpegqual;
 	/* Driver stuff */
 	u8 type;
 	u8 quality;				 /* image quality */
@@ -139,23 +131,21 @@ static void jlj_read1(struct gspca_dev *gspca_dev, unsigned char response)
 	}
 }
 
-static void setfreq(struct gspca_dev *gspca_dev)
+static void setfreq(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 freq_commands[][2] = {
 		{0x71, 0x80},
 		{0x70, 0x07}
 	};
 
-	freq_commands[0][1] |= (sd->ctrls[LIGHTFREQ].val >> 1);
+	freq_commands[0][1] |= val >> 1;
 
 	jlj_write2(gspca_dev, freq_commands[0]);
 	jlj_write2(gspca_dev, freq_commands[1]);
 }
 
-static void setcamquality(struct gspca_dev *gspca_dev)
+static void setcamquality(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 quality_commands[][2] = {
 		{0x71, 0x1E},
 		{0x70, 0x06}
@@ -163,7 +153,7 @@ static void setcamquality(struct gspca_dev *gspca_dev)
 	u8 camquality;
 
 	/* adapt camera quality from jpeg quality */
-	camquality = ((QUALITY_MAX - sd->quality) * CAMQUALITY_MAX)
+	camquality = ((QUALITY_MAX - val) * CAMQUALITY_MAX)
 		/ (QUALITY_MAX - QUALITY_MIN);
 	quality_commands[0][1] += camquality;
 
@@ -171,130 +161,58 @@ static void setcamquality(struct gspca_dev *gspca_dev)
 	jlj_write2(gspca_dev, quality_commands[1]);
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 autogain_commands[][2] = {
 		{0x94, 0x02},
 		{0xcf, 0x00}
 	};
 
-	autogain_commands[1][1] = (sd->ctrls[AUTOGAIN].val << 4);
+	autogain_commands[1][1] = val << 4;
 
 	jlj_write2(gspca_dev, autogain_commands[0]);
 	jlj_write2(gspca_dev, autogain_commands[1]);
 }
 
-static void setred(struct gspca_dev *gspca_dev)
+static void setred(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 setred_commands[][2] = {
 		{0x94, 0x02},
 		{0xe6, 0x00}
 	};
 
-	setred_commands[1][1] = sd->ctrls[RED].val;
+	setred_commands[1][1] = val;
 
 	jlj_write2(gspca_dev, setred_commands[0]);
 	jlj_write2(gspca_dev, setred_commands[1]);
 }
 
-static void setgreen(struct gspca_dev *gspca_dev)
+static void setgreen(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 setgreen_commands[][2] = {
 		{0x94, 0x02},
 		{0xe7, 0x00}
 	};
 
-	setgreen_commands[1][1] = sd->ctrls[GREEN].val;
+	setgreen_commands[1][1] = val;
 
 	jlj_write2(gspca_dev, setgreen_commands[0]);
 	jlj_write2(gspca_dev, setgreen_commands[1]);
 }
 
-static void setblue(struct gspca_dev *gspca_dev)
+static void setblue(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 setblue_commands[][2] = {
 		{0x94, 0x02},
 		{0xe9, 0x00}
 	};
 
-	setblue_commands[1][1] = sd->ctrls[BLUE].val;
+	setblue_commands[1][1] = val;
 
 	jlj_write2(gspca_dev, setblue_commands[0]);
 	jlj_write2(gspca_dev, setblue_commands[1]);
 }
 
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[LIGHTFREQ] = {
-	    {
-		.id      = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = V4L2_CID_POWER_LINE_FREQUENCY_DISABLED, /* 1 */
-		.maximum = V4L2_CID_POWER_LINE_FREQUENCY_60HZ, /* 2 */
-		.step    = 1,
-		.default_value = V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
-	    },
-	    .set_control = setfreq
-	},
-[AUTOGAIN] = {
-	    {
-		.id = V4L2_CID_AUTOGAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Automatic Gain (and Exposure)",
-		.minimum = 0,
-		.maximum = 3,
-		.step = 1,
-#define AUTOGAIN_DEF 0
-		.default_value = AUTOGAIN_DEF,
-	   },
-	   .set_control = setautogain
-	},
-[RED] = {
-	    {
-		.id = V4L2_CID_RED_BALANCE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "red balance",
-		.minimum = 0,
-		.maximum = 3,
-		.step = 1,
-#define RED_BALANCE_DEF 2
-		.default_value = RED_BALANCE_DEF,
-	   },
-	   .set_control = setred
-	},
-
-[GREEN]	= {
-	    {
-		.id = V4L2_CID_GAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "green balance",
-		.minimum = 0,
-		.maximum = 3,
-		.step = 1,
-#define GREEN_BALANCE_DEF 2
-		.default_value = GREEN_BALANCE_DEF,
-	   },
-	   .set_control = setgreen
-	},
-[BLUE] = {
-	    {
-		.id = V4L2_CID_BLUE_BALANCE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "blue balance",
-		.minimum = 0,
-		.maximum = 3,
-		.step = 1,
-#define BLUE_BALANCE_DEF 2
-		.default_value = BLUE_BALANCE_DEF,
-	   },
-	   .set_control = setblue
-	},
-};
-
 static int jlj_start(struct gspca_dev *gspca_dev)
 {
 	int i;
@@ -344,9 +262,9 @@ static int jlj_start(struct gspca_dev *gspca_dev)
 		if (start_commands[i].ack_wanted)
 			jlj_read1(gspca_dev, response);
 	}
-	setcamquality(gspca_dev);
+	setcamquality(gspca_dev, v4l2_ctrl_g_ctrl(sd->jpegqual));
 	msleep(2);
-	setfreq(gspca_dev);
+	setfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->freq));
 	if (gspca_dev->usb_err < 0)
 		PDEBUG(D_ERR, "Start streaming command failed");
 	return gspca_dev->usb_err;
@@ -403,7 +321,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	struct sd *dev  = (struct sd *) gspca_dev;
 
 	dev->type = id->driver_info;
-	gspca_dev->cam.ctrls = dev->ctrls;
 	dev->quality = QUALITY_DEF;
 
 	cam->cam_mode = jlj_mode;
@@ -479,25 +396,81 @@ static const struct usb_device_id device_table[] = {
 
 MODULE_DEVICE_TABLE(usb, device_table);
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (menu->id) {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
 	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:	/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "disable");
-			return 0;
-		case 1:	/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:	/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
+		setfreq(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		setred(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		setgreen(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		setblue(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		jpeg_set_qual(sd->jpeg_hdr, ctrl->val);
+		setcamquality(gspca_dev, ctrl->val);
 		break;
 	}
-	return -EINVAL;
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	static const struct v4l2_ctrl_config custom_autogain = {
+		.ops = &sd_ctrl_ops,
+		.id = V4L2_CID_AUTOGAIN,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Automatic Gain (and Exposure)",
+		.max = 3,
+		.step = 1,
+		.def = 0,
+	};
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 6);
+	sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 1,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ);
+	v4l2_ctrl_new_custom(hdl, &custom_autogain, NULL);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 0, 3, 1, 2);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 3, 1, 2);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 0, 3, 1, 2);
+	sd->jpegqual = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	return 0;
 }
 
 static int sd_set_jcomp(struct gspca_dev *gspca_dev,
@@ -505,16 +478,7 @@ static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	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);
-		setcamquality(gspca_dev);
-	}
+	v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
 	return 0;
 }
 
@@ -524,7 +488,7 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
 			| V4L2_JPEG_MARKER_DQT;
 	return 0;
@@ -546,12 +510,10 @@ static const struct sd_desc sd_desc_sportscam_dv15 = {
 	.name   = MODULE_NAME,
 	.config = sd_config,
 	.init   = sd_init,
+	.init_controls = sd_init_controls,
 	.start  = sd_start,
 	.stopN  = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
-	.querymenu = sd_querymenu,
 	.get_jcomp = sd_get_jcomp,
 	.set_jcomp = sd_set_jcomp,
 };
@@ -579,6 +541,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume  = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/jl2005bcd.c b/drivers/media/video/gspca/jl2005bcd.c
index 9c591c7c6f5..cf9d9fca5b8 100644
--- a/drivers/media/video/gspca/jl2005bcd.c
+++ b/drivers/media/video/gspca/jl2005bcd.c
@@ -505,8 +505,6 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	/* .ctrls = none have been detected */
-	/* .nctrls = ARRAY_SIZE(sd_ctrls),  */
 	.config = sd_config,
 	.init = sd_init,
 	.start = sd_start,
@@ -536,6 +534,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/kinect.c b/drivers/media/video/gspca/kinect.c
index e8e8f2fe916..40ad6687ee5 100644
--- a/drivers/media/video/gspca/kinect.c
+++ b/drivers/media/video/gspca/kinect.c
@@ -63,12 +63,6 @@ struct sd {
 	uint8_t ibuf[0x200];        /* input buffer for control commands */
 };
 
-/* V4L2 controls supported by the driver */
-/* controls prototypes here */
-
-static const struct ctrl sd_ctrls[] = {
-};
-
 #define MODE_640x480   0x0001
 #define MODE_640x488   0x0002
 #define MODE_1280x1024 0x0004
@@ -373,15 +367,12 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev, u8 *__data, int len)
 /* 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,
 	.stopN     = sd_stopN,
 	.pkt_scan  = sd_pkt_scan,
 	/*
-	.querymenu = sd_querymenu,
 	.get_streamparm = sd_get_streamparm,
 	.set_streamparm = sd_set_streamparm,
 	*/
@@ -410,6 +401,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend    = gspca_suspend,
 	.resume     = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/konica.c b/drivers/media/video/gspca/konica.c
index f0c0d74dfe9..bbf91e07e38 100644
--- a/drivers/media/video/gspca/konica.c
+++ b/drivers/media/video/gspca/konica.c
@@ -50,107 +50,8 @@ struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 	struct urb *last_data_urb;
 	u8 snapshot_pressed;
-	u8 brightness;
-	u8 contrast;
-	u8 saturation;
-	u8 whitebal;
-	u8 sharpness;
 };
 
-/* V4L2 controls supported by the driver */
-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_setsaturation(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getsaturation(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setwhitebal(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getwhitebal(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 const struct ctrl sd_ctrls[] = {
-#define SD_BRIGHTNESS 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 9,
-		.step = 1,
-#define BRIGHTNESS_DEFAULT 4
-		.default_value = BRIGHTNESS_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define SD_CONTRAST 1
-	{
-	    {
-		.id = V4L2_CID_CONTRAST,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Contrast",
-		.minimum = 0,
-		.maximum = 9,
-		.step = 4,
-#define CONTRAST_DEFAULT 10
-		.default_value = CONTRAST_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-#define SD_SATURATION 2
-	{
-	    {
-		.id	= V4L2_CID_SATURATION,
-		.type	= V4L2_CTRL_TYPE_INTEGER,
-		.name	= "Saturation",
-		.minimum = 0,
-		.maximum = 9,
-		.step	= 1,
-#define SATURATION_DEFAULT 4
-		.default_value = SATURATION_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setsaturation,
-	    .get = sd_getsaturation,
-	},
-#define SD_WHITEBAL 3
-	{
-	    {
-		.id = V4L2_CID_WHITE_BALANCE_TEMPERATURE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "White Balance",
-		.minimum = 0,
-		.maximum = 33,
-		.step = 1,
-#define WHITEBAL_DEFAULT 25
-		.default_value = WHITEBAL_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setwhitebal,
-	    .get = sd_getwhitebal,
-	},
-#define SD_SHARPNESS 4
-	{
-	    {
-		.id = V4L2_CID_SHARPNESS,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Sharpness",
-		.minimum = 0,
-		.maximum = 9,
-		.step = 1,
-#define SHARPNESS_DEFAULT 4
-		.default_value = SHARPNESS_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setsharpness,
-	    .get = sd_getsharpness,
-	},
-};
 
 /* .priv is what goes to register 8 for this mode, known working values:
    0x00 -> 176x144, cropped
@@ -202,7 +103,8 @@ static void reg_w(struct gspca_dev *gspca_dev, u16 value, u16 index)
 			0,
 			1000);
 	if (ret < 0) {
-		pr_err("reg_w err %d\n", ret);
+		pr_err("reg_w err writing %02x to %02x: %d\n",
+		       value, index, ret);
 		gspca_dev->usb_err = ret;
 	}
 }
@@ -223,7 +125,7 @@ static void reg_r(struct gspca_dev *gspca_dev, u16 value, u16 index)
 			2,
 			1000);
 	if (ret < 0) {
-		pr_err("reg_w err %d\n", ret);
+		pr_err("reg_r err %d\n", ret);
 		gspca_dev->usb_err = ret;
 	}
 }
@@ -242,34 +144,33 @@ static void konica_stream_off(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->cam.cam_mode = vga_mode;
 	gspca_dev->cam.nmodes = ARRAY_SIZE(vga_mode);
 	gspca_dev->cam.no_urb_create = 1;
 
-	sd->brightness  = BRIGHTNESS_DEFAULT;
-	sd->contrast    = CONTRAST_DEFAULT;
-	sd->saturation  = SATURATION_DEFAULT;
-	sd->whitebal    = WHITEBAL_DEFAULT;
-	sd->sharpness   = SHARPNESS_DEFAULT;
-
 	return 0;
 }
 
 /* this function is called at probe and resume time */
 static int sd_init(struct gspca_dev *gspca_dev)
 {
-	/* HDG not sure if these 2 reads are needed */
-	reg_r(gspca_dev, 0, 0x10);
-	PDEBUG(D_PROBE, "Reg 0x10 reads: %02x %02x",
-	       gspca_dev->usb_buf[0], gspca_dev->usb_buf[1]);
-	reg_r(gspca_dev, 0, 0x10);
-	PDEBUG(D_PROBE, "Reg 0x10 reads: %02x %02x",
-	       gspca_dev->usb_buf[0], gspca_dev->usb_buf[1]);
+	int i;
+
+	/*
+	 * The konica needs a freaking large time to "boot" (approx 6.5 sec.),
+	 * and does not want to be bothered while doing so :|
+	 * Register 0x10 counts from 1 - 3, with 3 being "ready"
+	 */
+	msleep(6000);
+	for (i = 0; i < 20; i++) {
+		reg_r(gspca_dev, 0, 0x10);
+		if (gspca_dev->usb_buf[0] == 3)
+			break;
+		msleep(100);
+	}
 	reg_w(gspca_dev, 0, 0x0d);
 
-	return 0;
+	return gspca_dev->usb_err;
 }
 
 static int sd_start(struct gspca_dev *gspca_dev)
@@ -289,12 +190,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	packet_size = le16_to_cpu(alt->endpoint[0].desc.wMaxPacketSize);
 
-	reg_w(gspca_dev, sd->brightness, BRIGHTNESS_REG);
-	reg_w(gspca_dev, sd->whitebal, WHITEBAL_REG);
-	reg_w(gspca_dev, sd->contrast, CONTRAST_REG);
-	reg_w(gspca_dev, sd->saturation, SATURATION_REG);
-	reg_w(gspca_dev, sd->sharpness, SHARPNESS_REG);
-
 	n = gspca_dev->cam.cam_mode[gspca_dev->curr_mode].priv;
 	reg_w(gspca_dev, n, 0x08);
 
@@ -479,125 +374,82 @@ resubmit:
 		pr_err("usb_submit_urb(status_urb) ret %d\n", st);
 }
 
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->brightness = val;
-	if (gspca_dev->streaming) {
-		konica_stream_off(gspca_dev);
-		reg_w(gspca_dev, sd->brightness, BRIGHTNESS_REG);
-		konica_stream_on(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;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	sd->contrast = val;
-	if (gspca_dev->streaming) {
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
 		konica_stream_off(gspca_dev);
-		reg_w(gspca_dev, sd->contrast, CONTRAST_REG);
+		reg_w(gspca_dev, ctrl->val, BRIGHTNESS_REG);
 		konica_stream_on(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_setsaturation(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->saturation = val;
-	if (gspca_dev->streaming) {
+		break;
+	case V4L2_CID_CONTRAST:
 		konica_stream_off(gspca_dev);
-		reg_w(gspca_dev, sd->saturation, SATURATION_REG);
+		reg_w(gspca_dev, ctrl->val, CONTRAST_REG);
 		konica_stream_on(gspca_dev);
-	}
-	return 0;
-}
-
-static int sd_getsaturation(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->saturation;
-
-	return 0;
-}
-
-static int sd_setwhitebal(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->whitebal = val;
-	if (gspca_dev->streaming) {
+		break;
+	case V4L2_CID_SATURATION:
 		konica_stream_off(gspca_dev);
-		reg_w(gspca_dev, sd->whitebal, WHITEBAL_REG);
+		reg_w(gspca_dev, ctrl->val, SATURATION_REG);
 		konica_stream_on(gspca_dev);
-	}
-	return 0;
-}
-
-static int sd_getwhitebal(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->whitebal;
-
-	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) {
+		break;
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
 		konica_stream_off(gspca_dev);
-		reg_w(gspca_dev, sd->sharpness, SHARPNESS_REG);
+		reg_w(gspca_dev, ctrl->val, WHITEBAL_REG);
 		konica_stream_on(gspca_dev);
+		break;
+	case V4L2_CID_SHARPNESS:
+		konica_stream_off(gspca_dev);
+		reg_w(gspca_dev, ctrl->val, SHARPNESS_REG);
+		konica_stream_on(gspca_dev);
+		break;
 	}
-	return 0;
+	return gspca_dev->usb_err;
 }
 
-static int sd_getsharpness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->sharpness;
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 9, 1, 4);
+	/* Needs to be verified */
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 9, 1, 4);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 9, 1, 4);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+			0, 33, 1, 25);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 9, 1, 4);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
@@ -628,6 +480,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/m5602/m5602_core.c b/drivers/media/video/gspca/m5602/m5602_core.c
index 0c449367543..ed22638978c 100644
--- a/drivers/media/video/gspca/m5602/m5602_core.c
+++ b/drivers/media/video/gspca/m5602/m5602_core.c
@@ -400,6 +400,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 	.disconnect = m5602_disconnect
 };
diff --git a/drivers/media/video/gspca/mars.c b/drivers/media/video/gspca/mars.c
index ec7b21ee79f..ff2c5abf115 100644
--- a/drivers/media/video/gspca/mars.c
+++ b/drivers/media/video/gspca/mars.c
@@ -30,6 +30,8 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA/Mars USB Camera Driver");
 MODULE_LICENSE("GPL");
 
+#define QUALITY 50
+
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
@@ -42,13 +44,6 @@ struct sd {
 		struct v4l2_ctrl *illum_top;
 		struct v4l2_ctrl *illum_bottom;
 	};
-	struct v4l2_ctrl *jpegqual;
-
-	u8 quality;
-#define QUALITY_MIN 40
-#define QUALITY_MAX 70
-#define QUALITY_DEF 50
-
 	u8 jpeg_hdr[JPEG_HDR_SZ];
 };
 
@@ -194,9 +189,6 @@ static int mars_s_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_SHARPNESS:
 		setsharpness(gspca_dev, ctrl->val);
 		break;
-	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
-		jpeg_set_qual(sd->jpeg_hdr, ctrl->val);
-		break;
 	default:
 		return -EINVAL;
 	}
@@ -214,7 +206,7 @@ static int sd_init_controls(struct gspca_dev *gspca_dev)
 	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 
 	gspca_dev->vdev.ctrl_handler = hdl;
-	v4l2_ctrl_handler_init(hdl, 7);
+	v4l2_ctrl_handler_init(hdl, 6);
 	sd->brightness = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
 			V4L2_CID_BRIGHTNESS, 0, 30, 1, 15);
 	sd->saturation = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
@@ -229,9 +221,6 @@ static int sd_init_controls(struct gspca_dev *gspca_dev)
 	sd->illum_bottom = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
 			V4L2_CID_ILLUMINATORS_2, 0, 1, 1, 0);
 	sd->illum_bottom->flags |= V4L2_CTRL_FLAG_UPDATE;
-	sd->jpegqual = v4l2_ctrl_new_std(hdl, &mars_ctrl_ops,
-			V4L2_CID_JPEG_COMPRESSION_QUALITY,
-			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
 	if (hdl->error) {
 		pr_err("Could not initialize controls\n");
 		return hdl->error;
@@ -244,13 +233,11 @@ static int sd_init_controls(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 
 	cam = &gspca_dev->cam;
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
-	sd->quality = QUALITY_DEF;
 	return 0;
 }
 
@@ -269,7 +256,7 @@ 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, v4l2_ctrl_g_ctrl(sd->jpegqual));
+	jpeg_set_qual(sd->jpeg_hdr, QUALITY);
 
 	data = gspca_dev->usb_buf;
 
@@ -411,31 +398,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_set_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	int ret;
-
-	ret = v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
-	if (ret)
-		return ret;
-	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
-	return 0;
-}
-
-static int sd_get_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
-	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
-			| V4L2_JPEG_MARKER_DQT;
-	return 0;
-}
-
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
@@ -445,8 +407,6 @@ static const struct sd_desc sd_desc = {
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.get_jcomp = sd_get_jcomp,
-	.set_jcomp = sd_set_jcomp,
 };
 
 /* -- module initialisation -- */
diff --git a/drivers/media/video/gspca/mr97310a.c b/drivers/media/video/gspca/mr97310a.c
index d73e5bd3dbf..8f4714df599 100644
--- a/drivers/media/video/gspca/mr97310a.c
+++ b/drivers/media/video/gspca/mr97310a.c
@@ -67,6 +67,7 @@
 #define MR97310A_CS_GAIN_MAX		0x7ff
 #define MR97310A_CS_GAIN_DEFAULT	0x110
 
+#define MR97310A_CID_CLOCKDIV (V4L2_CTRL_CLASS_USER + 0x1000)
 #define MR97310A_MIN_CLOCKDIV_MIN	3
 #define MR97310A_MIN_CLOCKDIV_MAX	8
 #define MR97310A_MIN_CLOCKDIV_DEFAULT	3
@@ -84,17 +85,15 @@ MODULE_PARM_DESC(force_sensor_type, "Force sensor type (-1 (auto), 0 or 1)");
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;  /* !! must be the first item */
+	struct { /* exposure/min_clockdiv control cluster */
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *min_clockdiv;
+	};
 	u8 sof_read;
 	u8 cam_type;	/* 0 is CIF and 1 is VGA */
 	u8 sensor_type;	/* We use 0 and 1 here, too. */
 	u8 do_lcd_stop;
 	u8 adj_colors;
-
-	int brightness;
-	u16 exposure;
-	u32 gain;
-	u8 contrast;
-	u8 min_clockdiv;
 };
 
 struct sensor_w_data {
@@ -105,132 +104,6 @@ struct sensor_w_data {
 };
 
 static void sd_stopN(struct gspca_dev *gspca_dev);
-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_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val);
-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_setmin_clockdiv(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getmin_clockdiv(struct gspca_dev *gspca_dev, __s32 *val);
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static void setgain(struct gspca_dev *gspca_dev);
-static void setcontrast(struct gspca_dev *gspca_dev);
-
-/* V4L2 controls supported by the driver */
-static const struct ctrl sd_ctrls[] = {
-/* Separate brightness control description for Argus QuickClix as it has
- * different limits from the other mr97310a cameras, and separate gain
- * control for Sakar CyberPix camera. */
-	{
-#define NORM_BRIGHTNESS_IDX 0
-		{
-			.id = V4L2_CID_BRIGHTNESS,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Brightness",
-			.minimum = -254,
-			.maximum = 255,
-			.step = 1,
-			.default_value = MR97310A_BRIGHTNESS_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setbrightness,
-		.get = sd_getbrightness,
-	},
-	{
-#define ARGUS_QC_BRIGHTNESS_IDX 1
-		{
-			.id = V4L2_CID_BRIGHTNESS,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Brightness",
-			.minimum = 0,
-			.maximum = 15,
-			.step = 1,
-			.default_value = MR97310A_BRIGHTNESS_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setbrightness,
-		.get = sd_getbrightness,
-	},
-	{
-#define EXPOSURE_IDX 2
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Exposure",
-			.minimum = MR97310A_EXPOSURE_MIN,
-			.maximum = MR97310A_EXPOSURE_MAX,
-			.step = 1,
-			.default_value = MR97310A_EXPOSURE_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setexposure,
-		.get = sd_getexposure,
-	},
-	{
-#define GAIN_IDX 3
-		{
-			.id = V4L2_CID_GAIN,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Gain",
-			.minimum = MR97310A_GAIN_MIN,
-			.maximum = MR97310A_GAIN_MAX,
-			.step = 1,
-			.default_value = MR97310A_GAIN_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setgain,
-		.get = sd_getgain,
-	},
-	{
-#define SAKAR_CS_GAIN_IDX 4
-		{
-			.id = V4L2_CID_GAIN,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Gain",
-			.minimum = MR97310A_CS_GAIN_MIN,
-			.maximum = MR97310A_CS_GAIN_MAX,
-			.step = 1,
-			.default_value = MR97310A_CS_GAIN_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setgain,
-		.get = sd_getgain,
-	},
-	{
-#define CONTRAST_IDX 5
-		{
-			.id = V4L2_CID_CONTRAST,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Contrast",
-			.minimum = MR97310A_CONTRAST_MIN,
-			.maximum = MR97310A_CONTRAST_MAX,
-			.step = 1,
-			.default_value = MR97310A_CONTRAST_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setcontrast,
-		.get = sd_getcontrast,
-	},
-	{
-#define MIN_CLOCKDIV_IDX 6
-		{
-			.id = V4L2_CID_PRIVATE_BASE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Minimum Clock Divider",
-			.minimum = MR97310A_MIN_CLOCKDIV_MIN,
-			.maximum = MR97310A_MIN_CLOCKDIV_MAX,
-			.step = 1,
-			.default_value = MR97310A_MIN_CLOCKDIV_DEFAULT,
-			.flags = 0,
-		},
-		.set = sd_setmin_clockdiv,
-		.get = sd_getmin_clockdiv,
-	},
-};
 
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_MR97310A, V4L2_FIELD_NONE,
@@ -481,7 +354,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
-	int gain_default = MR97310A_GAIN_DEFAULT;
 	int err_code;
 
 	cam = &gspca_dev->cam;
@@ -615,52 +487,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		       sd->sensor_type);
 	}
 
-	/* Setup controls depending on camera type */
-	if (sd->cam_type == CAM_TYPE_CIF) {
-		/* No brightness for sensor_type 0 */
-		if (sd->sensor_type == 0)
-			gspca_dev->ctrl_dis = (1 << NORM_BRIGHTNESS_IDX) |
-					      (1 << ARGUS_QC_BRIGHTNESS_IDX) |
-					      (1 << CONTRAST_IDX) |
-					      (1 << SAKAR_CS_GAIN_IDX);
-		else
-			gspca_dev->ctrl_dis = (1 << ARGUS_QC_BRIGHTNESS_IDX) |
-					      (1 << CONTRAST_IDX) |
-					      (1 << SAKAR_CS_GAIN_IDX) |
-					      (1 << MIN_CLOCKDIV_IDX);
-	} else {
-		/* All controls need to be disabled if VGA sensor_type is 0 */
-		if (sd->sensor_type == 0)
-			gspca_dev->ctrl_dis = (1 << NORM_BRIGHTNESS_IDX) |
-					      (1 << ARGUS_QC_BRIGHTNESS_IDX) |
-					      (1 << EXPOSURE_IDX) |
-					      (1 << GAIN_IDX) |
-					      (1 << CONTRAST_IDX) |
-					      (1 << SAKAR_CS_GAIN_IDX) |
-					      (1 << MIN_CLOCKDIV_IDX);
-		else if (sd->sensor_type == 2) {
-			gspca_dev->ctrl_dis = (1 << NORM_BRIGHTNESS_IDX) |
-					      (1 << ARGUS_QC_BRIGHTNESS_IDX) |
-					      (1 << GAIN_IDX) |
-					      (1 << MIN_CLOCKDIV_IDX);
-			gain_default = MR97310A_CS_GAIN_DEFAULT;
-		} else if (sd->do_lcd_stop)
-			/* Argus QuickClix has different brightness limits */
-			gspca_dev->ctrl_dis = (1 << NORM_BRIGHTNESS_IDX) |
-					      (1 << CONTRAST_IDX) |
-					      (1 << SAKAR_CS_GAIN_IDX);
-		else
-			gspca_dev->ctrl_dis = (1 << ARGUS_QC_BRIGHTNESS_IDX) |
-					      (1 << CONTRAST_IDX) |
-					      (1 << SAKAR_CS_GAIN_IDX);
-	}
-
-	sd->brightness = MR97310A_BRIGHTNESS_DEFAULT;
-	sd->exposure = MR97310A_EXPOSURE_DEFAULT;
-	sd->gain = gain_default;
-	sd->contrast = MR97310A_CONTRAST_DEFAULT;
-	sd->min_clockdiv = MR97310A_MIN_CLOCKDIV_DEFAULT;
-
 	return 0;
 }
 
@@ -952,11 +778,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	if (err_code < 0)
 		return err_code;
 
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setexposure(gspca_dev);
-	setgain(gspca_dev);
-
 	return isoc_enable(gspca_dev);
 }
 
@@ -971,37 +792,25 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 		lcd_stop(gspca_dev);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
 	u8 sign_reg = 7;  /* This reg and the next one used on CIF cams. */
 	u8 value_reg = 8; /* VGA cams seem to use regs 0x0b and 0x0c */
 	static const u8 quick_clix_table[] =
 	/*	  0  1  2   3  4  5  6  7  8  9  10  11  12  13  14  15 */
 		{ 0, 4, 8, 12, 1, 2, 3, 5, 6, 9,  7, 10, 13, 11, 14, 15};
-	/*
-	 * This control is disabled for CIF type 1 and VGA type 0 cameras.
-	 * It does not quite act linearly for the Argus QuickClix camera,
-	 * but it does control brightness. The values are 0 - 15 only, and
-	 * the table above makes them act consecutively.
-	 */
-	if ((gspca_dev->ctrl_dis & (1 << NORM_BRIGHTNESS_IDX)) &&
-	    (gspca_dev->ctrl_dis & (1 << ARGUS_QC_BRIGHTNESS_IDX)))
-		return;
-
 	if (sd->cam_type == CAM_TYPE_VGA) {
 		sign_reg += 4;
 		value_reg += 4;
 	}
 
 	/* Note register 7 is also seen as 0x8x or 0xCx in some dumps */
-	if (sd->brightness > 0) {
+	if (val > 0) {
 		sensor_write1(gspca_dev, sign_reg, 0x00);
-		val = sd->brightness;
 	} else {
 		sensor_write1(gspca_dev, sign_reg, 0x01);
-		val = (257 - sd->brightness);
+		val = 257 - val;
 	}
 	/* Use lookup table for funky Argus QuickClix brightness */
 	if (sd->do_lcd_stop)
@@ -1010,23 +819,20 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	sensor_write1(gspca_dev, value_reg, val);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 expo, s32 min_clockdiv)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int exposure = MR97310A_EXPOSURE_DEFAULT;
 	u8 buf[2];
 
-	if (gspca_dev->ctrl_dis & (1 << EXPOSURE_IDX))
-		return;
-
 	if (sd->cam_type == CAM_TYPE_CIF && sd->sensor_type == 1) {
 		/* This cam does not like exposure settings < 300,
 		   so scale 0 - 4095 to 300 - 4095 */
-		exposure = (sd->exposure * 9267) / 10000 + 300;
+		exposure = (expo * 9267) / 10000 + 300;
 		sensor_write1(gspca_dev, 3, exposure >> 4);
 		sensor_write1(gspca_dev, 4, exposure & 0x0f);
 	} else if (sd->sensor_type == 2) {
-		exposure = sd->exposure;
+		exposure = expo;
 		exposure >>= 3;
 		sensor_write1(gspca_dev, 3, exposure >> 8);
 		sensor_write1(gspca_dev, 4, exposure & 0xff);
@@ -1038,11 +844,11 @@ static void setexposure(struct gspca_dev *gspca_dev)
 
 		   Note our 0 - 4095 exposure is mapped to 0 - 511
 		   milliseconds exposure time */
-		u8 clockdiv = (60 * sd->exposure + 7999) / 8000;
+		u8 clockdiv = (60 * expo + 7999) / 8000;
 
 		/* Limit framerate to not exceed usb bandwidth */
-		if (clockdiv < sd->min_clockdiv && gspca_dev->width >= 320)
-			clockdiv = sd->min_clockdiv;
+		if (clockdiv < min_clockdiv && gspca_dev->width >= 320)
+			clockdiv = min_clockdiv;
 		else if (clockdiv < 2)
 			clockdiv = 2;
 
@@ -1051,7 +857,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 
 		/* Frame exposure time in ms = 1000 * clockdiv / 60 ->
 		exposure = (sd->exposure / 8) * 511 / (1000 * clockdiv / 60) */
-		exposure = (60 * 511 * sd->exposure) / (8000 * clockdiv);
+		exposure = (60 * 511 * expo) / (8000 * clockdiv);
 		if (exposure > 511)
 			exposure = 511;
 
@@ -1065,125 +871,148 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 gainreg;
 
-	if ((gspca_dev->ctrl_dis & (1 << GAIN_IDX)) &&
-	    (gspca_dev->ctrl_dis & (1 << SAKAR_CS_GAIN_IDX)))
-		return;
-
 	if (sd->cam_type == CAM_TYPE_CIF && sd->sensor_type == 1)
-		sensor_write1(gspca_dev, 0x0e, sd->gain);
+		sensor_write1(gspca_dev, 0x0e, val);
 	else if (sd->cam_type == CAM_TYPE_VGA && sd->sensor_type == 2)
 		for (gainreg = 0x0a; gainreg < 0x11; gainreg += 2) {
-			sensor_write1(gspca_dev, gainreg, sd->gain >> 8);
-			sensor_write1(gspca_dev, gainreg + 1, sd->gain & 0xff);
+			sensor_write1(gspca_dev, gainreg, val >> 8);
+			sensor_write1(gspca_dev, gainreg + 1, val & 0xff);
 		}
 	else
-		sensor_write1(gspca_dev, 0x10, sd->gain);
-}
-
-static void setcontrast(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (gspca_dev->ctrl_dis & (1 << CONTRAST_IDX))
-		return;
-
-	sensor_write1(gspca_dev, 0x1c, sd->contrast);
-}
-
-
-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_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;
+		sensor_write1(gspca_dev, 0x10, val);
 }
 
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
-	return 0;
+	sensor_write1(gspca_dev, 0x1c, val);
 }
 
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
 
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
-	return 0;
-}
+	if (!gspca_dev->streaming)
+		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;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, sd->exposure->val,
+			    sd->min_clockdiv ? sd->min_clockdiv->val : 0);
+		break;
+	case V4L2_CID_GAIN:
+		setgain(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-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_setmin_clockdiv(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	static const struct v4l2_ctrl_config clockdiv = {
+		.ops = &sd_ctrl_ops,
+		.id = MR97310A_CID_CLOCKDIV,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Minimum Clock Divider",
+		.min = MR97310A_MIN_CLOCKDIV_MIN,
+		.max = MR97310A_MIN_CLOCKDIV_MAX,
+		.step = 1,
+		.def = MR97310A_MIN_CLOCKDIV_DEFAULT,
+	};
+	bool has_brightness = false;
+	bool has_argus_brightness = false;
+	bool has_contrast = false;
+	bool has_gain = false;
+	bool has_cs_gain = false;
+	bool has_exposure = false;
+	bool has_clockdiv = false;
 
-	sd->min_clockdiv = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return 0;
-}
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
 
-static int sd_getmin_clockdiv(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	/* Setup controls depending on camera type */
+	if (sd->cam_type == CAM_TYPE_CIF) {
+		/* No brightness for sensor_type 0 */
+		if (sd->sensor_type == 0)
+			has_exposure = has_gain = has_clockdiv = true;
+		else
+			has_exposure = has_gain = has_brightness = true;
+	} else {
+		/* All controls need to be disabled if VGA sensor_type is 0 */
+		if (sd->sensor_type == 0)
+			; /* no controls! */
+		else if (sd->sensor_type == 2)
+			has_exposure = has_cs_gain = has_contrast = true;
+		else if (sd->do_lcd_stop)
+			has_exposure = has_gain = has_argus_brightness =
+				has_clockdiv = true;
+		else
+			has_exposure = has_gain = has_brightness =
+				has_clockdiv = true;
+	}
 
-	*val = sd->min_clockdiv;
+	/* Separate brightness control description for Argus QuickClix as it has
+	 * different limits from the other mr97310a cameras, and separate gain
+	 * control for Sakar CyberPix camera. */
+	/*
+	 * This control is disabled for CIF type 1 and VGA type 0 cameras.
+	 * It does not quite act linearly for the Argus QuickClix camera,
+	 * but it does control brightness. The values are 0 - 15 only, and
+	 * the table above makes them act consecutively.
+	 */
+	if (has_brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -254, 255, 1,
+			MR97310A_BRIGHTNESS_DEFAULT);
+	else if (has_argus_brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 15, 1,
+			MR97310A_BRIGHTNESS_DEFAULT);
+	if (has_contrast)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, MR97310A_CONTRAST_MIN,
+			MR97310A_CONTRAST_MAX, 1, MR97310A_CONTRAST_DEFAULT);
+	if (has_gain)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, MR97310A_GAIN_MIN, MR97310A_GAIN_MAX,
+			1, MR97310A_GAIN_DEFAULT);
+	else if (has_cs_gain)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops, V4L2_CID_GAIN,
+			MR97310A_CS_GAIN_MIN, MR97310A_CS_GAIN_MAX,
+			1, MR97310A_CS_GAIN_DEFAULT);
+	if (has_exposure)
+		sd->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, MR97310A_EXPOSURE_MIN,
+			MR97310A_EXPOSURE_MAX, 1, MR97310A_EXPOSURE_DEFAULT);
+	if (has_clockdiv)
+		sd->min_clockdiv = v4l2_ctrl_new_custom(hdl, &clockdiv, NULL);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	if (has_exposure && has_clockdiv)
+		v4l2_ctrl_cluster(2, &sd->exposure);
 	return 0;
 }
 
@@ -1221,10 +1050,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -1256,6 +1084,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/nw80x.c b/drivers/media/video/gspca/nw80x.c
index 42e021931e6..44c9964b1b3 100644
--- a/drivers/media/video/gspca/nw80x.c
+++ b/drivers/media/video/gspca/nw80x.c
@@ -32,22 +32,10 @@ MODULE_LICENSE("GPL");
 
 static int webcam;
 
-/* controls */
-enum e_ctrl {
-	GAIN,
-	EXPOSURE,
-	AUTOGAIN,
-	NCTRLS		/* number of controls */
-};
-
-#define AUTOGAIN_DEF 1
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
-
 	u32 ae_res;
 	s8 ag_cnt;
 #define AG_CNT_START 13
@@ -1667,17 +1655,13 @@ static int swap_bits(int v)
 	return r;
 }
 
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, u8 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val, v[2];
+	u8 v[2];
 
-	val = sd->ctrls[GAIN].val;
 	switch (sd->webcam) {
 	case P35u:
-		/* Note the control goes from 0-255 not 0-127, but anything
-		   above 127 just means amplifying noise */
-		val >>= 1;			/* 0 - 255 -> 0 - 127 */
 		reg_w(gspca_dev, 0x1026, &val, 1);
 		break;
 	case Kr651us:
@@ -1690,13 +1674,11 @@ static void setgain(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	s16 val;
 	u8 v[2];
 
-	val = sd->ctrls[EXPOSURE].val;
 	switch (sd->webcam) {
 	case P35u:
 		v[0] = ((9 - val) << 3) | 0x01;
@@ -1713,14 +1695,12 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int w, h;
 
-	if (gspca_dev->ctrl_dis & (1 << AUTOGAIN))
-		return;
-	if (!sd->ctrls[AUTOGAIN].val) {
+	if (!val) {
 		sd->ag_cnt = -1;
 		return;
 	}
@@ -1763,7 +1743,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	if ((unsigned) webcam >= NWEBCAMS)
 		webcam = 0;
 	sd->webcam = webcam;
-	gspca_dev->cam.ctrls = sd->ctrls;
 	gspca_dev->cam.needs_full_bandwidth = 1;
 	sd->ag_cnt = -1;
 
@@ -1834,33 +1813,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 			break;
 		}
 	}
-	switch (sd->webcam) {
-	case P35u:
-/*		sd->ctrls[EXPOSURE].max = 9;
- *		sd->ctrls[EXPOSURE].def = 9; */
-		/* coarse expo auto gain function gain minimum, to avoid
-		 * a large settings jump the first auto adjustment */
-		sd->ctrls[GAIN].def = 255 / 5 * 2;
-		break;
-	case Cvideopro:
-	case DvcV6:
-	case Kritter:
-		gspca_dev->ctrl_dis = (1 << GAIN) | (1 << AUTOGAIN);
-		/* fall thru */
-	case Kr651us:
-		sd->ctrls[EXPOSURE].max = 315;
-		sd->ctrls[EXPOSURE].def = 150;
-		break;
-	default:
-		gspca_dev->ctrl_dis = (1 << GAIN) | (1 << EXPOSURE)
-					 | (1 << AUTOGAIN);
-		break;
-	}
 
-#if AUTOGAIN_DEF
-	if (!(gspca_dev->ctrl_dis & (1 << AUTOGAIN)))
-		gspca_dev->ctrl_inac = (1 << GAIN) | (1 << EXPOSURE);
-#endif
 	return gspca_dev->usb_err;
 }
 
@@ -1925,9 +1878,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	setgain(gspca_dev);
-	setexposure(gspca_dev);
-	setautogain(gspca_dev);
 	sd->exp_too_high_cnt = 0;
 	sd->exp_too_low_cnt = 0;
 	return gspca_dev->usb_err;
@@ -1987,24 +1937,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->ctrls[AUTOGAIN].val = val;
-	if (val)
-		gspca_dev->ctrl_inac = (1 << GAIN) | (1 << EXPOSURE);
-	else
-		gspca_dev->ctrl_inac = 0;
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-#define WANT_REGULAR_AUTOGAIN
-#define WANT_COARSE_EXPO_AUTOGAIN
-#include "autogain_functions.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -2024,62 +1956,100 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 
 	switch (sd->webcam) {
 	case P35u:
-		coarse_grained_expo_autogain(gspca_dev, luma, 100, 5);
+		gspca_coarse_grained_expo_autogain(gspca_dev, luma, 100, 5);
 		break;
 	default:
-		auto_gain_n_exposure(gspca_dev, luma, 100, 5, 230, 0);
+		gspca_expo_autogain(gspca_dev, luma, 100, 5, 230, 0);
 		break;
 	}
 }
 
-/* V4L2 controls supported by the driver */
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-		.maximum = 253,
-		.step    = 1,
-		.default_value = 128
-	    },
-	    .set_control = setgain
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-		.maximum = 9,
-		.step    = 1,
-		.default_value = 9
-	    },
-	    .set_control = setexposure
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = AUTOGAIN_DEF,
-		.flags   = V4L2_CTRL_FLAG_UPDATE
-	    },
-	    .set = sd_setautogain
-	},
+
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	/* autogain/gain/exposure control cluster */
+	case V4L2_CID_AUTOGAIN:
+		if (ctrl->is_new)
+			setautogain(gspca_dev, ctrl->val);
+		if (!ctrl->val) {
+			if (gspca_dev->gain->is_new)
+				setgain(gspca_dev, gspca_dev->gain->val);
+			if (gspca_dev->exposure->is_new)
+				setexposure(gspca_dev,
+					    gspca_dev->exposure->val);
+		}
+		break;
+	/* Some webcams only have exposure, so handle that separately from the
+	   autogain/gain/exposure cluster in the previous case. */
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, gspca_dev->exposure->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
 };
 
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	switch (sd->webcam) {
+	case P35u:
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		/* For P35u choose coarse expo auto gain function gain minimum,
+		 * to avoid a large settings jump the first auto adjustment */
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 127, 1, 127 / 5 * 2);
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 9, 1, 9);
+		break;
+	case Kr651us:
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 253, 1, 128);
+		/* fall through */
+	case Cvideopro:
+	case DvcV6:
+	case Kritter:
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 315, 1, 150);
+		break;
+	default:
+		break;
+	}
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	if (gspca_dev->autogain)
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	return 0;
+}
+
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -2117,6 +2087,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/ov519.c b/drivers/media/video/gspca/ov519.c
index 183457c5cfd..bfc7cefa59f 100644
--- a/drivers/media/video/gspca/ov519.c
+++ b/drivers/media/video/gspca/ov519.c
@@ -60,25 +60,20 @@ static int frame_rate;
  * are getting "Failed to read sensor ID..." */
 static int i2c_detect_tries = 10;
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	EXPOSURE,
-	COLORS,
-	HFLIP,
-	VFLIP,
-	AUTOBRIGHT,
-	AUTOGAIN,
-	FREQ,
-	NCTRL		/* number of controls */
-};
-
 /* ov519 device descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRL];
+	struct v4l2_ctrl *jpegqual;
+	struct v4l2_ctrl *freq;
+	struct { /* h/vflip control cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
+	struct { /* autobrightness/brightness control cluster */
+		struct v4l2_ctrl *autobright;
+		struct v4l2_ctrl *brightness;
+	};
 
 	u8 packet_nr;
 
@@ -101,7 +96,6 @@ struct sd {
 	/* Determined by sensor type */
 	u8 sif;
 
-	u8 quality;
 #define QUALITY_MIN 50
 #define QUALITY_MAX 70
 #define QUALITY_DEF 50
@@ -145,209 +139,112 @@ enum sensors {
    really should move the sensor drivers to v4l2 sub drivers. */
 #include "w996Xcf.c"
 
-/* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcontrast(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void sethvflip(struct gspca_dev *gspca_dev);
-static void setautobright(struct gspca_dev *gspca_dev);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static void setfreq(struct gspca_dev *gspca_dev);
-static void setfreq_i(struct sd *sd);
-
-static const struct ctrl sd_ctrls[] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setbrightness,
+/* table of the disabled controls */
+struct ctrl_valid {
+	int has_brightness:1;
+	int has_contrast:1;
+	int has_exposure:1;
+	int has_autogain:1;
+	int has_sat:1;
+	int has_hvflip:1;
+	int has_autobright:1;
+	int has_freq:1;
+};
+
+static const struct ctrl_valid valid_controls[] = {
+	[SEN_OV2610] = {
+		.has_exposure = 1,
+		.has_autogain = 1,
 	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setcontrast,
+	[SEN_OV2610AE] = {
+		.has_exposure = 1,
+		.has_autogain = 1,
 	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setexposure,
+	[SEN_OV3610] = {
+		/* No controls */
 	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setcolors,
+	[SEN_OV6620] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
 	},
-/* The flip controls work for sensors ov7660 and ov7670 only */
-[HFLIP] = {
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip,
+	[SEN_OV6630] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
 	},
-[VFLIP] = {
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip,
+	[SEN_OV66308AF] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
 	},
-[AUTOBRIGHT] = {
-	    {
-		.id      = V4L2_CID_AUTOBRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Brightness",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setautobright,
+	[SEN_OV7610] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
 	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 1,
-		.flags	 = V4L2_CTRL_FLAG_UPDATE
-	    },
-	    .set = sd_setautogain,
+	[SEN_OV7620] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
 	},
-[FREQ] = {
-	    {
-		.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = 0,
-		.maximum = 2,	/* 0: no flicker, 1: 50Hz, 2:60Hz, 3: auto */
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = setfreq,
+	[SEN_OV7620AE] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
+	},
+	[SEN_OV7640] = {
+		.has_brightness = 1,
+		.has_sat = 1,
+		.has_freq = 1,
+	},
+	[SEN_OV7648] = {
+		.has_brightness = 1,
+		.has_sat = 1,
+		.has_freq = 1,
+	},
+	[SEN_OV7660] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_hvflip = 1,
+		.has_freq = 1,
+	},
+	[SEN_OV7670] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_hvflip = 1,
+		.has_freq = 1,
+	},
+	[SEN_OV76BE] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+		.has_freq = 1,
+	},
+	[SEN_OV8610] = {
+		.has_brightness = 1,
+		.has_contrast = 1,
+		.has_sat = 1,
+		.has_autobright = 1,
+	},
+	[SEN_OV9600] = {
+		.has_exposure = 1,
+		.has_autogain = 1,
 	},
-};
-
-/* table of the disabled controls */
-static const unsigned ctrl_dis[] = {
-[SEN_OV2610] =		((1 << NCTRL) - 1)	/* no control */
-			^ ((1 << EXPOSURE)	/* but exposure */
-			 | (1 << AUTOGAIN)),	/* and autogain */
-
-[SEN_OV2610AE] =	((1 << NCTRL) - 1)	/* no control */
-			^ ((1 << EXPOSURE)	/* but exposure */
-			 | (1 << AUTOGAIN)),	/* and autogain */
-
-[SEN_OV3610] =		(1 << NCTRL) - 1,	/* no control */
-
-[SEN_OV6620] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV6630] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV66308AF] =	(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7610] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7620] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7620AE] =	(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7640] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << AUTOBRIGHT) |
-			(1 << CONTRAST) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7648] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << AUTOBRIGHT) |
-			(1 << CONTRAST) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7660] =		(1 << AUTOBRIGHT) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV7670] =		(1 << COLORS) |
-			(1 << AUTOBRIGHT) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV76BE] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN),
-
-[SEN_OV8610] =		(1 << HFLIP) |
-			(1 << VFLIP) |
-			(1 << EXPOSURE) |
-			(1 << AUTOGAIN) |
-			(1 << FREQ),
-[SEN_OV9600] =		((1 << NCTRL) - 1)	/* no control */
-			^ ((1 << EXPOSURE)	/* but exposure */
-			 | (1 << AUTOGAIN)),	/* and autogain */
-
 };
 
 static const struct v4l2_pix_format ov519_vga_mode[] = {
@@ -3306,11 +3203,11 @@ static void ov519_set_fr(struct sd *sd)
 	ov518_i2c_w(sd, OV7670_R11_CLKRC, clock);
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	i2c_w_mask(sd, 0x13, sd->ctrls[AUTOGAIN].val ? 0x05 : 0x00, 0x05);
+	i2c_w_mask(sd, 0x13, val ? 0x05 : 0x00, 0x05);
 }
 
 /* this function is called at probe time */
@@ -3351,8 +3248,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		break;
 	}
 
-	gspca_dev->cam.ctrls = sd->ctrls;
-	sd->quality = QUALITY_DEF;
 	sd->frame_rate = 15;
 
 	return 0;
@@ -3467,8 +3362,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	gspca_dev->ctrl_dis = ctrl_dis[sd->sensor];
-
 	/* initialize the sensor */
 	switch (sd->sensor) {
 	case SEN_OV2610:
@@ -3494,8 +3387,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		break;
 	case SEN_OV6630:
 	case SEN_OV66308AF:
-		sd->ctrls[CONTRAST].def = 200;
-				 /* The default is too low for the ov6630 */
 		write_i2c_regvals(sd, norm_6x30, ARRAY_SIZE(norm_6x30));
 		break;
 	default:
@@ -3522,26 +3413,12 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		sd->gspca_dev.curr_mode = 1;	/* 640x480 */
 		ov519_set_mode(sd);
 		ov519_set_fr(sd);
-		sd->ctrls[COLORS].max = 4;	/* 0..4 */
-		sd->ctrls[COLORS].val =
-			sd->ctrls[COLORS].def = 2;
-		setcolors(gspca_dev);
-		sd->ctrls[CONTRAST].max = 6;	/* 0..6 */
-		sd->ctrls[CONTRAST].val =
-			sd->ctrls[CONTRAST].def = 3;
-		setcontrast(gspca_dev);
-		sd->ctrls[BRIGHTNESS].max = 6;	/* 0..6 */
-		sd->ctrls[BRIGHTNESS].val =
-			sd->ctrls[BRIGHTNESS].def = 3;
-		setbrightness(gspca_dev);
 		sd_reset_snapshot(gspca_dev);
 		ov51x_restart(sd);
 		ov51x_stop(sd);			/* not in win traces */
 		ov51x_led_control(sd, 0);
 		break;
 	case SEN_OV7670:
-		sd->ctrls[FREQ].max = 3;	/* auto */
-		sd->ctrls[FREQ].def = 3;
 		write_i2c_regvals(sd, norm_7670, ARRAY_SIZE(norm_7670));
 		break;
 	case SEN_OV8610:
@@ -4177,15 +4054,14 @@ static void mode_init_ov_sensor_regs(struct sd *sd)
 }
 
 /* this function works for bridge ov519 and sensors ov7660 and ov7670 only */
-static void sethvflip(struct gspca_dev *gspca_dev)
+static void sethvflip(struct gspca_dev *gspca_dev, s32 hflip, s32 vflip)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->gspca_dev.streaming)
 		reg_w(sd, OV519_R51_RESET1, 0x0f);	/* block stream */
 	i2c_w_mask(sd, OV7670_R1E_MVFP,
-		OV7670_MVFP_MIRROR * sd->ctrls[HFLIP].val
-			| OV7670_MVFP_VFLIP * sd->ctrls[VFLIP].val,
+		OV7670_MVFP_MIRROR * hflip | OV7670_MVFP_VFLIP * vflip,
 		OV7670_MVFP_MIRROR | OV7670_MVFP_VFLIP);
 	if (sd->gspca_dev.streaming)
 		reg_w(sd, OV519_R51_RESET1, 0x00);	/* restart stream */
@@ -4333,23 +4209,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	set_ov_sensor_window(sd);
 
-	if (!(sd->gspca_dev.ctrl_dis & (1 << CONTRAST)))
-		setcontrast(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & (1 << BRIGHTNESS)))
-		setbrightness(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & (1 << EXPOSURE)))
-		setexposure(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & (1 << COLORS)))
-		setcolors(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & ((1 << HFLIP) | (1 << VFLIP))))
-		sethvflip(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & (1 << AUTOBRIGHT)))
-		setautobright(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & (1 << AUTOGAIN)))
-		setautogain(gspca_dev);
-	if (!(sd->gspca_dev.ctrl_dis & (1 << FREQ)))
-		setfreq_i(sd);
-
 	/* Force clear snapshot state in case the snapshot button was
 	   pressed while we weren't streaming */
 	sd->snapshot_needs_reset = 1;
@@ -4605,10 +4464,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 
 /* -- management routines -- */
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
 	static const struct ov_i2c_regvals brit_7660[][7] = {
 		{{0x0f, 0x6a}, {0x24, 0x40}, {0x25, 0x2b}, {0x26, 0x90},
 			{0x27, 0xe0}, {0x28, 0xe0}, {0x2c, 0xe0}},
@@ -4626,7 +4484,6 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 			{0x27, 0x60}, {0x28, 0x60}, {0x2c, 0x60}}
 	};
 
-	val = sd->ctrls[BRIGHTNESS].val;
 	switch (sd->sensor) {
 	case SEN_OV8610:
 	case SEN_OV7610:
@@ -4640,9 +4497,7 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 		break;
 	case SEN_OV7620:
 	case SEN_OV7620AE:
-		/* 7620 doesn't like manual changes when in auto mode */
-		if (!sd->ctrls[AUTOBRIGHT].val)
-			i2c_w(sd, OV7610_REG_BRT, val);
+		i2c_w(sd, OV7610_REG_BRT, val);
 		break;
 	case SEN_OV7660:
 		write_i2c_regvals(sd, brit_7660[val],
@@ -4656,10 +4511,9 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
 	static const struct ov_i2c_regvals contrast_7660[][31] = {
 		{{0x6c, 0xf0}, {0x6d, 0xf0}, {0x6e, 0xf8}, {0x6f, 0xa0},
 		 {0x70, 0x58}, {0x71, 0x38}, {0x72, 0x30}, {0x73, 0x30},
@@ -4719,7 +4573,6 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 		 {0x88, 0xf1}, {0x89, 0xf9}, {0x8a, 0xfd}},
 	};
 
-	val = sd->ctrls[CONTRAST].val;
 	switch (sd->sensor) {
 	case SEN_OV7610:
 	case SEN_OV6620:
@@ -4760,18 +4613,16 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (!sd->ctrls[AUTOGAIN].val)
-		i2c_w(sd, 0x10, sd->ctrls[EXPOSURE].val);
+	i2c_w(sd, 0x10, val);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
 	static const struct ov_i2c_regvals colors_7660[][6] = {
 		{{0x4f, 0x28}, {0x50, 0x2a}, {0x51, 0x02}, {0x52, 0x0a},
 		 {0x53, 0x19}, {0x54, 0x23}},
@@ -4785,7 +4636,6 @@ static void setcolors(struct gspca_dev *gspca_dev)
 		 {0x53, 0x66}, {0x54, 0x8e}},
 	};
 
-	val = sd->ctrls[COLORS].val;
 	switch (sd->sensor) {
 	case SEN_OV8610:
 	case SEN_OV7610:
@@ -4819,34 +4669,18 @@ static void setcolors(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setautobright(struct gspca_dev *gspca_dev)
+static void setautobright(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	i2c_w_mask(sd, 0x2d, sd->ctrls[AUTOBRIGHT].val ? 0x10 : 0x00, 0x10);
+	i2c_w_mask(sd, 0x2d, val ? 0x10 : 0x00, 0x10);
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->ctrls[AUTOGAIN].val = val;
-	if (val) {
-		gspca_dev->ctrl_inac |= (1 << EXPOSURE);
-	} else {
-		gspca_dev->ctrl_inac &= ~(1 << EXPOSURE);
-		sd->ctrls[EXPOSURE].val = i2c_r(sd, 0x10);
-	}
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static void setfreq_i(struct sd *sd)
+static void setfreq_i(struct sd *sd, s32 val)
 {
 	if (sd->sensor == SEN_OV7660
 	 || sd->sensor == SEN_OV7670) {
-		switch (sd->ctrls[FREQ].val) {
+		switch (val) {
 		case 0: /* Banding filter disabled */
 			i2c_w_mask(sd, OV7670_R13_COM8, 0, OV7670_COM8_BFILT);
 			break;
@@ -4868,7 +4702,7 @@ static void setfreq_i(struct sd *sd)
 			break;
 		}
 	} else {
-		switch (sd->ctrls[FREQ].val) {
+		switch (val) {
 		case 0: /* Banding filter disabled */
 			i2c_w_mask(sd, 0x2d, 0x00, 0x04);
 			i2c_w_mask(sd, 0x2a, 0x00, 0x80);
@@ -4900,56 +4734,28 @@ static void setfreq_i(struct sd *sd)
 		}
 	}
 }
-static void setfreq(struct gspca_dev *gspca_dev)
+
+static void setfreq(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	setfreq_i(sd);
+	setfreq_i(sd, val);
 
 	/* Ugly but necessary */
 	if (sd->bridge == BRIDGE_W9968CF)
 		w9968cf_set_crop_window(sd);
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		case 3:
-			if (sd->sensor != SEN_OV7670)
-				return -EINVAL;
-
-			strcpy((char *) menu->name, "Automatic");
-			return 0;
-		}
-		break;
-	}
-	return -EINVAL;
-}
-
 static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 			struct v4l2_jpegcompression *jcomp)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->bridge != BRIDGE_W9968CF)
-		return -EINVAL;
+		return -ENOTTY;
 
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT | V4L2_JPEG_MARKER_DQT |
 			      V4L2_JPEG_MARKER_DRI;
 	return 0;
@@ -4961,38 +4767,161 @@ static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->bridge != BRIDGE_W9968CF)
-		return -EINVAL;
+		return -ENOTTY;
+
+	v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
+	return 0;
+}
 
-	if (gspca_dev->streaming)
-		return -EBUSY;
+static int sd_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
 
-	if (jcomp->quality < QUALITY_MIN)
-		sd->quality = QUALITY_MIN;
-	else if (jcomp->quality > QUALITY_MAX)
-		sd->quality = QUALITY_MAX;
-	else
-		sd->quality = jcomp->quality;
+	gspca_dev->usb_err = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		gspca_dev->exposure->val = i2c_r(sd, 0x10);
+		break;
+	}
+	return 0;
+}
+
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setfreq(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOBRIGHTNESS:
+		if (ctrl->is_new)
+			setautobright(gspca_dev, ctrl->val);
+		if (!ctrl->val && sd->brightness->is_new)
+			setbrightness(gspca_dev, sd->brightness->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		sethvflip(gspca_dev, ctrl->val, sd->vflip->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (ctrl->is_new)
+			setautogain(gspca_dev, ctrl->val);
+		if (!ctrl->val && gspca_dev->exposure->is_new)
+			setexposure(gspca_dev, gspca_dev->exposure->val);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		return -EBUSY; /* Should never happen, as we grab the ctrl */
+	}
+	return gspca_dev->usb_err;
+}
 
-	/* Return resulting jcomp params to app */
-	sd_get_jcomp(gspca_dev, jcomp);
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.g_volatile_ctrl = sd_g_volatile_ctrl,
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 10);
+	if (valid_controls[sd->sensor].has_brightness)
+		sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0,
+			sd->sensor == SEN_OV7660 ? 6 : 255, 1,
+			sd->sensor == SEN_OV7660 ? 3 : 127);
+	if (valid_controls[sd->sensor].has_contrast) {
+		if (sd->sensor == SEN_OV7660)
+			v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 6, 1, 3);
+		else
+			v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_CONTRAST, 0, 255, 1,
+				(sd->sensor == SEN_OV6630 ||
+				 sd->sensor == SEN_OV66308AF) ? 200 : 127);
+	}
+	if (valid_controls[sd->sensor].has_sat)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0,
+			sd->sensor == SEN_OV7660 ? 4 : 255, 1,
+			sd->sensor == SEN_OV7660 ? 2 : 127);
+	if (valid_controls[sd->sensor].has_exposure)
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 255, 1, 127);
+	if (valid_controls[sd->sensor].has_hvflip) {
+		sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+		sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	}
+	if (valid_controls[sd->sensor].has_autobright)
+		sd->autobright = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOBRIGHTNESS, 0, 1, 1, 1);
+	if (valid_controls[sd->sensor].has_autogain)
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	if (valid_controls[sd->sensor].has_freq) {
+		if (sd->sensor == SEN_OV7670)
+			sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+				V4L2_CID_POWER_LINE_FREQUENCY,
+				V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+				V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+		else
+			sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+				V4L2_CID_POWER_LINE_FREQUENCY,
+				V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0, 0);
+	}
+	if (sd->bridge == BRIDGE_W9968CF)
+		sd->jpegqual = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			QUALITY_MIN, QUALITY_MAX, 1, QUALITY_DEF);
 
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	if (gspca_dev->autogain)
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, true);
+	if (sd->autobright)
+		v4l2_ctrl_auto_cluster(2, &sd->autobright, 0, false);
+	if (sd->hflip)
+		v4l2_ctrl_cluster(2, &sd->hflip);
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
 	.dq_callback = sd_reset_snapshot,
-	.querymenu = sd_querymenu,
 	.get_jcomp = sd_get_jcomp,
 	.set_jcomp = sd_set_jcomp,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
@@ -5052,6 +4981,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/ov534.c b/drivers/media/video/gspca/ov534.c
index 80c81dd6d68..bb09d7884b8 100644
--- a/drivers/media/video/gspca/ov534.c
+++ b/drivers/media/video/gspca/ov534.c
@@ -35,6 +35,7 @@
 #include "gspca.h"
 
 #include <linux/fixp-arith.h>
+#include <media/v4l2-ctrls.h>
 
 #define OV534_REG_ADDRESS	0xf1	/* sensor address */
 #define OV534_REG_SUBADDR	0xf2
@@ -53,29 +54,28 @@ MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");
 MODULE_DESCRIPTION("GSPCA/OV534 USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	HUE,
-	SATURATION,
-	BRIGHTNESS,
-	CONTRAST,
-	GAIN,
-	EXPOSURE,
-	AGC,
-	AWB,
-	AEC,
-	SHARPNESS,
-	HFLIP,
-	VFLIP,
-	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];
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *hue;
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *contrast;
+	struct { /* gain control cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+	};
+	struct v4l2_ctrl *autowhitebalance;
+	struct { /* exposure control cluster */
+		struct v4l2_ctrl *autoexposure;
+		struct v4l2_ctrl *exposure;
+	};
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *plfreq;
 
 	__u32 last_pts;
 	u16 last_fid;
@@ -89,181 +89,9 @@ enum sensors {
 	NSENSORS
 };
 
-/* V4L2 controls supported by the driver */
-static void sethue(struct gspca_dev *gspca_dev);
-static void setsaturation(struct gspca_dev *gspca_dev);
-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 void setagc(struct gspca_dev *gspca_dev);
-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 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[] = {
-[HUE] = {
-		{
-			.id      = V4L2_CID_HUE,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Hue",
-			.minimum = -90,
-			.maximum = 90,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = sethue
-	},
-[SATURATION] = {
-		{
-			.id      = V4L2_CID_SATURATION,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Saturation",
-			.minimum = 0,
-			.maximum = 255,
-			.step    = 1,
-			.default_value = 64,
-		},
-		.set_control = setsaturation
-	},
-[BRIGHTNESS] = {
-		{
-			.id      = V4L2_CID_BRIGHTNESS,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Brightness",
-			.minimum = 0,
-			.maximum = 255,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = setbrightness
-	},
-[CONTRAST] = {
-		{
-			.id      = V4L2_CID_CONTRAST,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Contrast",
-			.minimum = 0,
-			.maximum = 255,
-			.step    = 1,
-			.default_value = 32,
-		},
-		.set_control = setcontrast
-	},
-[GAIN] = {
-		{
-			.id      = V4L2_CID_GAIN,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Main Gain",
-			.minimum = 0,
-			.maximum = 63,
-			.step    = 1,
-			.default_value = 20,
-		},
-		.set_control = setgain
-	},
-[EXPOSURE] = {
-		{
-			.id      = V4L2_CID_EXPOSURE,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Exposure",
-			.minimum = 0,
-			.maximum = 255,
-			.step    = 1,
-			.default_value = 120,
-		},
-		.set_control = setexposure
-	},
-[AGC] = {
-		{
-			.id      = V4L2_CID_AUTOGAIN,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "Auto Gain",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-			.default_value = 1,
-		},
-		.set_control = setagc
-	},
-[AWB] = {
-		{
-			.id      = V4L2_CID_AUTO_WHITE_BALANCE,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "Auto White Balance",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-			.default_value = 1,
-		},
-		.set_control = setawb
-	},
-[AEC] = {
-		{
-			.id      = V4L2_CID_EXPOSURE_AUTO,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "Auto Exposure",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-			.default_value = 1,
-		},
-		.set_control = setaec
-	},
-[SHARPNESS] = {
-		{
-			.id      = V4L2_CID_SHARPNESS,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Sharpness",
-			.minimum = 0,
-			.maximum = 63,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = setsharpness
-	},
-[HFLIP] = {
-		{
-			.id      = V4L2_CID_HFLIP,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "HFlip",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = sethvflip
-	},
-[VFLIP] = {
-		{
-			.id      = V4L2_CID_VFLIP,
-			.type    = V4L2_CTRL_TYPE_BOOLEAN,
-			.name    = "VFlip",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = sethvflip
-	},
-[LIGHTFREQ] = {
-		{
-			.id      = V4L2_CID_POWER_LINE_FREQUENCY,
-			.type    = V4L2_CTRL_TYPE_MENU,
-			.name    = "Light Frequency Filter",
-			.minimum = 0,
-			.maximum = 1,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = setlightfreq
-	},
-};
 
 static const struct v4l2_pix_format ov772x_mode[] = {
 	{320, 240, V4L2_PIX_FMT_YUYV, V4L2_FIELD_NONE,
@@ -972,12 +800,10 @@ static void set_frame_rate(struct gspca_dev *gspca_dev)
 	PDEBUG(D_PROBE, "frame_rate: %d", r->fps);
 }
 
-static void sethue(struct gspca_dev *gspca_dev)
+static void sethue(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
 
-	val = sd->ctrls[HUE].val;
 	if (sd->sensor == SENSOR_OV767x) {
 		/* TBD */
 	} else {
@@ -1014,12 +840,10 @@ static void sethue(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setsaturation(struct gspca_dev *gspca_dev)
+static void setsaturation(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
 
-	val = sd->ctrls[SATURATION].val;
 	if (sd->sensor == SENSOR_OV767x) {
 		int i;
 		static u8 color_tb[][6] = {
@@ -1040,12 +864,10 @@ static void setsaturation(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int val;
 
-	val = sd->ctrls[BRIGHTNESS].val;
 	if (sd->sensor == SENSOR_OV767x) {
 		if (val < 0)
 			val = 0x80 - val;
@@ -1055,27 +877,18 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
 
-	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)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
-
-	if (sd->ctrls[AGC].val)
-		return;
-
-	val = sd->ctrls[GAIN].val;
 	switch (val & 0x30) {
 	case 0x00:
 		val &= 0x0f;
@@ -1097,15 +910,15 @@ static void setgain(struct gspca_dev *gspca_dev)
 	sccb_reg_write(gspca_dev, 0x00, val);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static s32 getgain(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
+	return sccb_reg_read(gspca_dev, 0x00);
+}
 
-	if (sd->ctrls[AEC].val)
-		return;
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
 
-	val = sd->ctrls[EXPOSURE].val;
 	if (sd->sensor == SENSOR_OV767x) {
 
 		/* set only aec[9:2] */
@@ -1123,11 +936,23 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setagc(struct gspca_dev *gspca_dev)
+static s32 getexposure(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->ctrls[AGC].val) {
+	if (sd->sensor == SENSOR_OV767x) {
+		/* get only aec[9:2] */
+		return sccb_reg_read(gspca_dev, 0x10);	/* aech */
+	} else {
+		u8 hi = sccb_reg_read(gspca_dev, 0x08);
+		u8 lo = sccb_reg_read(gspca_dev, 0x10);
+		return (hi << 8 | lo) >> 1;
+	}
+}
+
+static void setagc(struct gspca_dev *gspca_dev, s32 val)
+{
+	if (val) {
 		sccb_reg_write(gspca_dev, 0x13,
 				sccb_reg_read(gspca_dev, 0x13) | 0x04);
 		sccb_reg_write(gspca_dev, 0x64,
@@ -1137,16 +962,14 @@ static void setagc(struct gspca_dev *gspca_dev)
 				sccb_reg_read(gspca_dev, 0x13) & ~0x04);
 		sccb_reg_write(gspca_dev, 0x64,
 				sccb_reg_read(gspca_dev, 0x64) & ~0x03);
-
-		setgain(gspca_dev);
 	}
 }
 
-static void setawb(struct gspca_dev *gspca_dev)
+static void setawb(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->ctrls[AWB].val) {
+	if (val) {
 		sccb_reg_write(gspca_dev, 0x13,
 				sccb_reg_read(gspca_dev, 0x13) | 0x02);
 		if (sd->sensor == SENSOR_OV772x)
@@ -1161,7 +984,7 @@ static void setawb(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setaec(struct gspca_dev *gspca_dev)
+static void setaec(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 data;
@@ -1169,31 +992,25 @@ static void setaec(struct gspca_dev *gspca_dev)
 	data = sd->sensor == SENSOR_OV767x ?
 			0x05 :		/* agc + aec */
 			0x01;		/* agc */
-	if (sd->ctrls[AEC].val)
+	switch (val) {
+	case V4L2_EXPOSURE_AUTO:
 		sccb_reg_write(gspca_dev, 0x13,
 				sccb_reg_read(gspca_dev, 0x13) | data);
-	else {
+		break;
+	case V4L2_EXPOSURE_MANUAL:
 		sccb_reg_write(gspca_dev, 0x13,
 				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);
+		break;
 	}
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
-
-	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 sethvflip(struct gspca_dev *gspca_dev)
+static void sethvflip(struct gspca_dev *gspca_dev, s32 hflip, s32 vflip)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
@@ -1201,28 +1018,27 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 	if (sd->sensor == SENSOR_OV767x) {
 		val = sccb_reg_read(gspca_dev, 0x1e);	/* mvfp */
 		val &= ~0x30;
-		if (sd->ctrls[HFLIP].val)
+		if (hflip)
 			val |= 0x20;
-		if (sd->ctrls[VFLIP].val)
+		if (vflip)
 			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)
+		if (hflip == 0)
 			val |= 0x40;
-		if (sd->ctrls[VFLIP].val == 0)
+		if (vflip == 0)
 			val |= 0x80;
 		sccb_reg_write(gspca_dev, 0x0c, val);
 	}
 }
 
-static void setlightfreq(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
 
-	val = sd->ctrls[LIGHTFREQ].val ? 0x9e : 0x00;
+	val = val ? 0x9e : 0x00;
 	if (sd->sensor == SENSOR_OV767x) {
 		sccb_reg_write(gspca_dev, 0x2a, 0x00);
 		if (val)
@@ -1241,8 +1057,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	cam = &gspca_dev->cam;
 
-	cam->ctrls = sd->ctrls;
-
 	cam->cam_mode = ov772x_mode;
 	cam->nmodes = ARRAY_SIZE(ov772x_mode);
 
@@ -1251,6 +1065,195 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
+static int ov534_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct sd *sd = container_of(ctrl->handler, struct sd, ctrl_handler);
+	struct gspca_dev *gspca_dev = &sd->gspca_dev;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		gspca_dev->usb_err = 0;
+		if (ctrl->val && sd->gain && gspca_dev->streaming)
+			sd->gain->val = getgain(gspca_dev);
+		return gspca_dev->usb_err;
+
+	case V4L2_CID_EXPOSURE_AUTO:
+		gspca_dev->usb_err = 0;
+		if (ctrl->val == V4L2_EXPOSURE_AUTO && sd->exposure &&
+		    gspca_dev->streaming)
+			sd->exposure->val = getexposure(gspca_dev);
+		return gspca_dev->usb_err;
+	}
+	return -EINVAL;
+}
+
+static int ov534_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct sd *sd = container_of(ctrl->handler, struct sd, ctrl_handler);
+	struct gspca_dev *gspca_dev = &sd->gspca_dev;
+
+	gspca_dev->usb_err = 0;
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HUE:
+		sethue(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setsaturation(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+	/* case V4L2_CID_GAIN: */
+		setagc(gspca_dev, ctrl->val);
+		if (!gspca_dev->usb_err && !ctrl->val && sd->gain)
+			setgain(gspca_dev, sd->gain->val);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		setawb(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+	/* case V4L2_CID_EXPOSURE: */
+		setaec(gspca_dev, ctrl->val);
+		if (!gspca_dev->usb_err && ctrl->val == V4L2_EXPOSURE_MANUAL &&
+		    sd->exposure)
+			setexposure(gspca_dev, sd->exposure->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		sethvflip(gspca_dev, ctrl->val, sd->vflip->val);
+		break;
+	case V4L2_CID_VFLIP:
+		sethvflip(gspca_dev, sd->hflip->val, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setlightfreq(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops ov534_ctrl_ops = {
+	.g_volatile_ctrl = ov534_g_volatile_ctrl,
+	.s_ctrl = ov534_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &sd->ctrl_handler;
+	/* parameters with different values between the supported sensors */
+	int saturation_min;
+	int saturation_max;
+	int saturation_def;
+	int brightness_min;
+	int brightness_max;
+	int brightness_def;
+	int contrast_max;
+	int contrast_def;
+	int exposure_min;
+	int exposure_max;
+	int exposure_def;
+	int hflip_def;
+
+	if (sd->sensor == SENSOR_OV767x) {
+		saturation_min = 0,
+		saturation_max = 6,
+		saturation_def = 3,
+		brightness_min = -127;
+		brightness_max = 127;
+		brightness_def = 0;
+		contrast_max = 0x80;
+		contrast_def = 0x40;
+		exposure_min = 0x08;
+		exposure_max = 0x60;
+		exposure_def = 0x13;
+		hflip_def = 1;
+	} else {
+		saturation_min = 0,
+		saturation_max = 255,
+		saturation_def = 64,
+		brightness_min = 0;
+		brightness_max = 255;
+		brightness_def = 0;
+		contrast_max = 255;
+		contrast_def = 32;
+		exposure_min = 0;
+		exposure_max = 255;
+		exposure_def = 120;
+		hflip_def = 0;
+	}
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+
+	v4l2_ctrl_handler_init(hdl, 13);
+
+	if (sd->sensor == SENSOR_OV772x)
+		sd->hue = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+				V4L2_CID_HUE, -90, 90, 1, 0);
+
+	sd->saturation = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_SATURATION, saturation_min, saturation_max, 1,
+			saturation_def);
+	sd->brightness = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, brightness_min, brightness_max, 1,
+			brightness_def);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, contrast_max, 1, contrast_def);
+
+	if (sd->sensor == SENSOR_OV772x) {
+		sd->autogain = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+				V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		sd->gain = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+				V4L2_CID_GAIN, 0, 63, 1, 20);
+	}
+
+	sd->autoexposure = v4l2_ctrl_new_std_menu(hdl, &ov534_ctrl_ops,
+			V4L2_CID_EXPOSURE_AUTO,
+			V4L2_EXPOSURE_MANUAL, 0,
+			V4L2_EXPOSURE_AUTO);
+	sd->exposure = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_EXPOSURE, exposure_min, exposure_max, 1,
+			exposure_def);
+
+	sd->autowhitebalance = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+
+	if (sd->sensor == SENSOR_OV772x)
+		sd->sharpness = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+				V4L2_CID_SHARPNESS, 0, 63, 1, 0);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, hflip_def);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &ov534_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+	sd->plfreq = v4l2_ctrl_new_std_menu(hdl, &ov534_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_50HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_DISABLED);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	if (sd->sensor == SENSOR_OV772x)
+		v4l2_ctrl_auto_cluster(2, &sd->autogain, 0, true);
+
+	v4l2_ctrl_auto_cluster(2, &sd->autoexposure, V4L2_EXPOSURE_MANUAL,
+			       true);
+
+	return 0;
+}
+
 /* this function is called at probe and resume time */
 static int sd_init(struct gspca_dev *gspca_dev)
 {
@@ -1286,24 +1289,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 
 	if ((sensor_id & 0xfff0) == 0x7670) {
 		sd->sensor = SENSOR_OV767x;
-		gspca_dev->ctrl_dis = (1 << HUE) |
-					(1 << GAIN) |
-					(1 << AGC) |
-					(1 << SHARPNESS);	/* auto */
-		sd->ctrls[SATURATION].min = 0,
-		sd->ctrls[SATURATION].max = 6,
-		sd->ctrls[SATURATION].def = 3,
-		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 {
@@ -1366,22 +1351,23 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	set_frame_rate(gspca_dev);
 
-	if (!(gspca_dev->ctrl_dis & (1 << HUE)))
-		sethue(gspca_dev);
-	setsaturation(gspca_dev);
-	if (!(gspca_dev->ctrl_dis & (1 << AGC)))
-		setagc(gspca_dev);
-	setawb(gspca_dev);
-	setaec(gspca_dev);
-	if (!(gspca_dev->ctrl_dis & (1 << GAIN)))
-		setgain(gspca_dev);
-	setexposure(gspca_dev);
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	if (!(gspca_dev->ctrl_dis & (1 << SHARPNESS)))
-		setsharpness(gspca_dev);
-	sethvflip(gspca_dev);
-	setlightfreq(gspca_dev);
+	if (sd->hue)
+		sethue(gspca_dev, v4l2_ctrl_g_ctrl(sd->hue));
+	setsaturation(gspca_dev, v4l2_ctrl_g_ctrl(sd->saturation));
+	if (sd->autogain)
+		setagc(gspca_dev, v4l2_ctrl_g_ctrl(sd->autogain));
+	setawb(gspca_dev, v4l2_ctrl_g_ctrl(sd->autowhitebalance));
+	setaec(gspca_dev, v4l2_ctrl_g_ctrl(sd->autoexposure));
+	if (sd->gain)
+		setgain(gspca_dev, v4l2_ctrl_g_ctrl(sd->gain));
+	setexposure(gspca_dev, v4l2_ctrl_g_ctrl(sd->exposure));
+	setbrightness(gspca_dev, v4l2_ctrl_g_ctrl(sd->brightness));
+	setcontrast(gspca_dev, v4l2_ctrl_g_ctrl(sd->contrast));
+	if (sd->sharpness)
+		setsharpness(gspca_dev, v4l2_ctrl_g_ctrl(sd->sharpness));
+	sethvflip(gspca_dev, v4l2_ctrl_g_ctrl(sd->hflip),
+		  v4l2_ctrl_g_ctrl(sd->vflip));
+	setlightfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->plfreq));
 
 	ov534_set_led(gspca_dev, 1);
 	ov534_reg_write(gspca_dev, 0xe0, 0x00);
@@ -1483,25 +1469,6 @@ scan_next:
 	} while (remaining_len > 0);
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-		struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:         /* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "Disabled");
-			return 0;
-		case 1:         /* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		}
-		break;
-	}
-
-	return -EINVAL;
-}
-
 /* get stream parameters (framerate) */
 static void sd_get_streamparm(struct gspca_dev *gspca_dev,
 			     struct v4l2_streamparm *parm)
@@ -1536,14 +1503,12 @@ static void sd_set_streamparm(struct gspca_dev *gspca_dev,
 /* 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,
+	.init_controls = sd_init_controls,
 	.start    = sd_start,
 	.stopN    = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 	.get_streamparm = sd_get_streamparm,
 	.set_streamparm = sd_set_streamparm,
 };
@@ -1572,6 +1537,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend    = gspca_suspend,
 	.resume     = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/ov534_9.c b/drivers/media/video/gspca/ov534_9.c
index 1fd41f0d2e9..c4cd028fe0b 100644
--- a/drivers/media/video/gspca/ov534_9.c
+++ b/drivers/media/video/gspca/ov534_9.c
@@ -47,22 +47,9 @@ MODULE_AUTHOR("Jean-Francois Moine <moinejf@free.fr>");
 MODULE_DESCRIPTION("GSPCA/OV534_9 USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	AUTOGAIN,
-	EXPOSURE,
-	SHARPNESS,
-	SATUR,
-	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;
 	u8 last_fid;
 
@@ -75,103 +62,6 @@ enum sensors {
 	NSENSORS
 };
 
-/* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcontrast(struct gspca_dev *gspca_dev);
-static void setautogain(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static void setsharpness(struct gspca_dev *gspca_dev);
-static void setsatur(struct gspca_dev *gspca_dev);
-static void setlightfreq(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	{
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 15,
-		.step    = 1,
-		.default_value = 7
-	},
-	.set_control = setbrightness
-    },
-[CONTRAST] = {
-	{
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 15,
-		.step    = 1,
-		.default_value = 3
-	},
-	.set_control = setcontrast
-    },
-[AUTOGAIN] = {
-	{
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Autogain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-	},
-	.set_control = setautogain
-    },
-[EXPOSURE] = {
-	{
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 0
-	},
-	.set_control = setexposure
-    },
-[SHARPNESS] = {
-	{
-		.id      = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = -1,		/* -1 = auto */
-		.maximum = 4,
-		.step    = 1,
-		.default_value = -1
-	},
-	.set_control = setsharpness
-    },
-[SATUR] = {
-	{
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 4,
-		.step    = 1,
-		.default_value = 2
-	},
-	.set_control = setsatur
-    },
-[LIGHTFREQ] = {
-	{
-		.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = 0,
-		.maximum = 2,	/* 0: 0, 1: 50Hz, 2:60Hz */
-		.step    = 1,
-		.default_value = 0
-	},
-	.set_control = setlightfreq
-    },
-};
-
 static const struct v4l2_pix_format ov965x_mode[] = {
 #define QVGA_MODE 0
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
@@ -1104,16 +994,14 @@ static void set_led(struct gspca_dev *gspca_dev, int status)
 	}
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
 	s8 sval;
 
-	if (gspca_dev->ctrl_dis & (1 << BRIGHTNESS))
-		return;
 	if (sd->sensor == SENSOR_OV562x) {
-		sval = sd->ctrls[BRIGHTNESS].val;
+		sval = brightness;
 		val = 0x76;
 		val += sval;
 		sccb_write(gspca_dev, 0x24, val);
@@ -1128,7 +1016,7 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 			val = 0xe6;
 		sccb_write(gspca_dev, 0x26, val);
 	} else {
-		val = sd->ctrls[BRIGHTNESS].val;
+		val = brightness;
 		if (val < 8)
 			val = 15 - val;		/* f .. 8 */
 		else
@@ -1138,43 +1026,32 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (gspca_dev->ctrl_dis & (1 << CONTRAST))
-		return;
 	sccb_write(gspca_dev, 0x56,	/* cnst1 - contrast 1 ctrl coeff */
-			sd->ctrls[CONTRAST].val << 4);
+			val << 4);
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 autogain)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
 
-	if (gspca_dev->ctrl_dis & (1 << AUTOGAIN))
-		return;
 /*fixme: should adjust agc/awb/aec by different controls */
 	val = sccb_read(gspca_dev, 0x13);		/* com8 */
 	sccb_write(gspca_dev, 0xff, 0x00);
-	if (sd->ctrls[AUTOGAIN].val)
+	if (autogain)
 		val |= 0x05;		/* agc & aec */
 	else
 		val &= 0xfa;
 	sccb_write(gspca_dev, 0x13, val);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 exposure)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
 	static const u8 expo[4] = {0x00, 0x25, 0x38, 0x5e};
+	u8 val;
 
-	if (gspca_dev->ctrl_dis & (1 << EXPOSURE))
-		return;
-	sccb_write(gspca_dev, 0x10,			/* aec[9:2] */
-			expo[sd->ctrls[EXPOSURE].val]);
+	sccb_write(gspca_dev, 0x10, expo[exposure]);	/* aec[9:2] */
 
 	val = sccb_read(gspca_dev, 0x13);		/* com8 */
 	sccb_write(gspca_dev, 0xff, 0x00);
@@ -1185,14 +1062,8 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	sccb_write(gspca_dev, 0xa1, val & 0xe0);	/* aec[15:10] = 0 */
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	s8 val;
-
-	if (gspca_dev->ctrl_dis & (1 << SHARPNESS))
-		return;
-	val = sd->ctrls[SHARPNESS].val;
 	if (val < 0) {				/* auto */
 		val = sccb_read(gspca_dev, 0x42);	/* com17 */
 		sccb_write(gspca_dev, 0xff, 0x00);
@@ -1209,9 +1080,8 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 	sccb_write(gspca_dev, 0x42, val & 0xbf);
 }
 
-static void setsatur(struct gspca_dev *gspca_dev)
+static void setsatur(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val1, val2, val3;
 	static const u8 matrix[5][2] = {
 		{0x14, 0x38},
@@ -1221,10 +1091,8 @@ static void setsatur(struct gspca_dev *gspca_dev)
 		{0x48, 0x90}
 	};
 
-	if (gspca_dev->ctrl_dis & (1 << SATUR))
-		return;
-	val1 = matrix[sd->ctrls[SATUR].val][0];
-	val2 = matrix[sd->ctrls[SATUR].val][1];
+	val1 = matrix[val][0];
+	val2 = matrix[val][1];
 	val3 = val1 + val2;
 	sccb_write(gspca_dev, 0x4f, val3);	/* matrix coeff */
 	sccb_write(gspca_dev, 0x50, val3);
@@ -1239,16 +1107,13 @@ static void setsatur(struct gspca_dev *gspca_dev)
 	sccb_write(gspca_dev, 0x41, val1);
 }
 
-static void setlightfreq(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev, s32 freq)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 val;
 
-	if (gspca_dev->ctrl_dis & (1 << LIGHTFREQ))
-		return;
 	val = sccb_read(gspca_dev, 0x13);		/* com8 */
 	sccb_write(gspca_dev, 0xff, 0x00);
-	if (sd->ctrls[LIGHTFREQ].val == 0) {
+	if (freq == 0) {
 		sccb_write(gspca_dev, 0x13, val & 0xdf);
 		return;
 	}
@@ -1256,7 +1121,7 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 
 	val = sccb_read(gspca_dev, 0x42);		/* com17 */
 	sccb_write(gspca_dev, 0xff, 0x00);
-	if (sd->ctrls[LIGHTFREQ].val == 1)
+	if (freq == 1)
 		val |= 0x01;
 	else
 		val &= 0xfe;
@@ -1267,13 +1132,6 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 		     const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	gspca_dev->cam.ctrls = sd->ctrls;
-
-#if AUTOGAIN_DEF != 0
-	gspca_dev->ctrl_inac |= (1 << EXPOSURE);
-#endif
 	return 0;
 }
 
@@ -1330,9 +1188,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		gspca_dev->cam.cam_mode = ov971x_mode;
 		gspca_dev->cam.nmodes = ARRAY_SIZE(ov971x_mode);
 
-		/* no control yet */
-		gspca_dev->ctrl_dis = (1 << NCTRLS) - 1;
-
 		gspca_dev->cam.bulk = 1;
 		gspca_dev->cam.bulk_size = 16384;
 		gspca_dev->cam.bulk_nurbs = 2;
@@ -1358,16 +1213,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 			reg_w(gspca_dev, 0x56, 0x17);
 	} else if ((sensor_id & 0xfff0) == 0x5620) {
 		sd->sensor = SENSOR_OV562x;
-		gspca_dev->ctrl_dis = (1 << CONTRAST) |
-					(1 << AUTOGAIN) |
-					(1 << EXPOSURE) |
-					(1 << SHARPNESS) |
-					(1 << SATUR) |
-					(1 << LIGHTFREQ);
-
-		sd->ctrls[BRIGHTNESS].min = -90;
-		sd->ctrls[BRIGHTNESS].max = 90;
-		sd->ctrls[BRIGHTNESS].def = 0;
 		gspca_dev->cam.cam_mode = ov562x_mode;
 		gspca_dev->cam.nmodes = ARRAY_SIZE(ov562x_mode);
 
@@ -1390,10 +1235,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	if (sd->sensor == SENSOR_OV971x)
 		return gspca_dev->usb_err;
-	else if (sd->sensor == SENSOR_OV562x) {
-		setbrightness(gspca_dev);
+	if (sd->sensor == SENSOR_OV562x)
 		return gspca_dev->usb_err;
-	}
+
 	switch (gspca_dev->curr_mode) {
 	case QVGA_MODE:			/* 320x240 */
 		sccb_w_array(gspca_dev, ov965x_start_1_vga,
@@ -1437,13 +1281,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 				ARRAY_SIZE(ov965x_start_2_sxga));
 		break;
 	}
-	setlightfreq(gspca_dev);
-	setautogain(gspca_dev);
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setexposure(gspca_dev);
-	setsharpness(gspca_dev);
-	setsatur(gspca_dev);
 
 	reg_w(gspca_dev, 0xe0, 0x00);
 	reg_w(gspca_dev, 0xe0, 0x00);
@@ -1541,38 +1378,94 @@ scan_next:
 	} while (remaining_len > 0);
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	switch (menu->id) {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setsatur(gspca_dev, ctrl->val);
+		break;
 	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case 0:		/* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case 1:		/* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case 2:		/* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
+		setlightfreq(gspca_dev, ctrl->val);
 		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (ctrl->is_new)
+			setautogain(gspca_dev, ctrl->val);
+		if (!ctrl->val && gspca_dev->exposure->is_new)
+			setexposure(gspca_dev, gspca_dev->exposure->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	if (sd->sensor == SENSOR_OV971x)
+		return 0;
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 7);
+	if (sd->sensor == SENSOR_OV562x) {
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -90, 90, 1, 0);
+	} else {
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 15, 1, 7);
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 15, 1, 3);
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 4, 1, 2);
+		/* -1 = auto */
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, -1, 4, 1, -1);
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 3, 1, 0);
+		v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0, 0);
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
 	}
-	return -EINVAL;
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	return 0;
 }
 
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name     = MODULE_NAME,
-	.ctrls    = sd_ctrls,
-	.nctrls   = NCTRLS,
 	.config   = sd_config,
 	.init     = sd_init,
+	.init_controls = sd_init_controls,
 	.start    = sd_start,
 	.stopN    = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 };
 
 /* -- module initialisation -- */
@@ -1600,6 +1493,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend    = gspca_suspend,
 	.resume     = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/pac207.c b/drivers/media/video/gspca/pac207.c
index fa661c6d6d5..d236d1791f7 100644
--- a/drivers/media/video/gspca/pac207.c
+++ b/drivers/media/video/gspca/pac207.c
@@ -462,6 +462,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/pac7302.c b/drivers/media/video/gspca/pac7302.c
index a0369a58c4b..4877f7ab3d5 100644
--- a/drivers/media/video/gspca/pac7302.c
+++ b/drivers/media/video/gspca/pac7302.c
@@ -84,31 +84,31 @@
 /* Include pac common sof detection functions */
 #include "pac_common.h"
 
+#define PAC7302_GAIN_DEFAULT      15
+#define PAC7302_GAIN_KNEE         42
+#define PAC7302_EXPOSURE_DEFAULT  66 /* 33 ms / 30 fps */
+#define PAC7302_EXPOSURE_KNEE    133 /* 66 ms / 15 fps */
+
 MODULE_AUTHOR("Jean-Francois Moine <http://moinejf.free.fr>, "
 		"Thomas Kaiser thomas@kaiser-linux.li");
 MODULE_DESCRIPTION("Pixart PAC7302");
 MODULE_LICENSE("GPL");
 
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	COLORS,
-	WHITE_BALANCE,
-	RED_BALANCE,
-	BLUE_BALANCE,
-	GAIN,
-	AUTOGAIN,
-	EXPOSURE,
-	VFLIP,
-	HFLIP,
-	NCTRLS		/* number of controls */
-};
-
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
-
+	struct { /* brightness / contrast cluster */
+		struct v4l2_ctrl *brightness;
+		struct v4l2_ctrl *contrast;
+	};
+	struct v4l2_ctrl *saturation;
+	struct v4l2_ctrl *white_balance;
+	struct v4l2_ctrl *red_balance;
+	struct v4l2_ctrl *blue_balance;
+	struct { /* flip cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
 	u8 flags;
 #define FL_HFLIP 0x01		/* mirrored by default */
 #define FL_VFLIP 0x02		/* vertical flipped by default */
@@ -119,160 +119,6 @@ struct sd {
 	atomic_t avg_lum;
 };
 
-/* V4L2 controls supported by the driver */
-static void setbrightcont(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void setwhitebalance(struct gspca_dev *gspca_dev);
-static void setredbalance(struct gspca_dev *gspca_dev);
-static void setbluebalance(struct gspca_dev *gspca_dev);
-static void setgain(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static void setautogain(struct gspca_dev *gspca_dev);
-static void sethvflip(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-#define BRIGHTNESS_MAX 0x20
-		.maximum = BRIGHTNESS_MAX,
-		.step    = 1,
-		.default_value = 0x10,
-	    },
-	    .set_control = setbrightcont
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-#define CONTRAST_MAX 255
-		.maximum = CONTRAST_MAX,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setbrightcont
-	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-#define COLOR_MAX 255
-		.maximum = COLOR_MAX,
-		.step    = 1,
-		.default_value = 127
-	    },
-	    .set_control = setcolors
-	},
-[WHITE_BALANCE] = {
-	    {
-		.id      = V4L2_CID_WHITE_BALANCE_TEMPERATURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "White Balance",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 4,
-	    },
-	    .set_control = setwhitebalance
-	},
-[RED_BALANCE] = {
-	    {
-		.id      = V4L2_CID_RED_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Red",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setredbalance
-	},
-[BLUE_BALANCE] = {
-	    {
-		.id      = V4L2_CID_BLUE_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Blue",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setbluebalance
-	},
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-		.maximum = 62,
-		.step    = 1,
-#define GAIN_DEF 15
-#define GAIN_KNEE 46
-		.default_value = GAIN_DEF,
-	    },
-	    .set_control = setgain
-	},
-[EXPOSURE] = {
-	    {
-		.id      = V4L2_CID_EXPOSURE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Exposure",
-		.minimum = 0,
-		.maximum = 1023,
-		.step    = 1,
-#define EXPOSURE_DEF  66  /*  33 ms / 30 fps */
-#define EXPOSURE_KNEE 133 /*  66 ms / 15 fps */
-		.default_value = EXPOSURE_DEF,
-	    },
-	    .set_control = setexposure
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-	    },
-	    .set_control = setautogain,
-	},
-[HFLIP] = {
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip,
-	},
-[VFLIP] = {
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set_control = sethvflip
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{640, 480, V4L2_PIX_FMT_PJPG, V4L2_FIELD_NONE,
 		.bytesperline = 640,
@@ -516,8 +362,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam->cam_mode = vga_mode;	/* only 640x480 */
 	cam->nmodes = ARRAY_SIZE(vga_mode);
 
-	gspca_dev->cam.ctrls = sd->ctrls;
-
 	sd->flags = id->driver_info;
 	return 0;
 }
@@ -536,9 +380,9 @@ static void setbrightcont(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0xff, 0x00);		/* page 0 */
 	for (i = 0; i < 10; i++) {
 		v = max[i];
-		v += (sd->ctrls[BRIGHTNESS].val - BRIGHTNESS_MAX)
-			* 150 / BRIGHTNESS_MAX;		/* 200 ? */
-		v -= delta[i] * sd->ctrls[CONTRAST].val / CONTRAST_MAX;
+		v += (sd->brightness->val - sd->brightness->maximum)
+			* 150 / sd->brightness->maximum; /* 200 ? */
+		v -= delta[i] * sd->contrast->val / sd->contrast->maximum;
 		if (v < 0)
 			v = 0;
 		else if (v > 0xff)
@@ -561,7 +405,8 @@ static void setcolors(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x11, 0x01);
 	reg_w(gspca_dev, 0xff, 0x00);			/* page 0 */
 	for (i = 0; i < 9; i++) {
-		v = a[i] * sd->ctrls[COLORS].val / COLOR_MAX + b[i];
+		v = a[i] * sd->saturation->val / sd->saturation->maximum;
+		v += b[i];
 		reg_w(gspca_dev, 0x0f + 2 * i, (v >> 8) & 0x07);
 		reg_w(gspca_dev, 0x0f + 2 * i + 1, v);
 	}
@@ -573,7 +418,7 @@ static void setwhitebalance(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	reg_w(gspca_dev, 0xff, 0x00);		/* page 0 */
-	reg_w(gspca_dev, 0xc6, sd->ctrls[WHITE_BALANCE].val);
+	reg_w(gspca_dev, 0xc6, sd->white_balance->val);
 
 	reg_w(gspca_dev, 0xdc, 0x01);
 }
@@ -583,7 +428,7 @@ static void setredbalance(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	reg_w(gspca_dev, 0xff, 0x00);		/* page 0 */
-	reg_w(gspca_dev, 0xc5, sd->ctrls[RED_BALANCE].val);
+	reg_w(gspca_dev, 0xc5, sd->red_balance->val);
 
 	reg_w(gspca_dev, 0xdc, 0x01);
 }
@@ -593,22 +438,21 @@ static void setbluebalance(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	reg_w(gspca_dev, 0xff, 0x00);			/* page 0 */
-	reg_w(gspca_dev, 0xc7, sd->ctrls[BLUE_BALANCE].val);
+	reg_w(gspca_dev, 0xc7, sd->blue_balance->val);
 
 	reg_w(gspca_dev, 0xdc, 0x01);
 }
 
 static void setgain(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 reg10, reg12;
 
-	if (sd->ctrls[GAIN].val < 32) {
-		reg10 = sd->ctrls[GAIN].val;
+	if (gspca_dev->gain->val < 32) {
+		reg10 = gspca_dev->gain->val;
 		reg12 = 0;
 	} else {
 		reg10 = 31;
-		reg12 = sd->ctrls[GAIN].val - 31;
+		reg12 = gspca_dev->gain->val - 31;
 	}
 
 	reg_w(gspca_dev, 0xff, 0x03);			/* page 3 */
@@ -621,7 +465,6 @@ static void setgain(struct gspca_dev *gspca_dev)
 
 static void setexposure(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 clockdiv;
 	u16 exposure;
 
@@ -630,7 +473,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	 * no fps according to the formula: 90 / reg. sd->exposure is the
 	 * desired exposure time in 0.5 ms.
 	 */
-	clockdiv = (90 * sd->ctrls[EXPOSURE].val + 1999) / 2000;
+	clockdiv = (90 * gspca_dev->exposure->val + 1999) / 2000;
 
 	/*
 	 * Note clockdiv = 3 also works, but when running at 30 fps, depending
@@ -655,7 +498,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	 * frame exposure time in ms = 1000 * clockdiv / 90    ->
 	 * exposure = (sd->exposure / 2) * 448 / (1000 * clockdiv / 90)
 	 */
-	exposure = (sd->ctrls[EXPOSURE].val * 45 * 448) / (1000 * clockdiv);
+	exposure = (gspca_dev->exposure->val * 45 * 448) / (1000 * clockdiv);
 	/* 0 = use full frametime, 448 = no exposure, reverse it */
 	exposure = 448 - exposure;
 
@@ -668,37 +511,15 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x11, 0x01);
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	/*
-	 * When switching to autogain set defaults to make sure
-	 * we are on a valid point of the autogain gain /
-	 * exposure knee graph, and give this change time to
-	 * take effect before doing autogain.
-	 */
-	if (sd->ctrls[AUTOGAIN].val) {
-		sd->ctrls[EXPOSURE].val = EXPOSURE_DEF;
-		sd->ctrls[GAIN].val = GAIN_DEF;
-		sd->autogain_ignore_frames =
-				PAC_AUTOGAIN_IGNORE_FRAMES;
-	} else {
-		sd->autogain_ignore_frames = -1;
-	}
-	setexposure(gspca_dev);
-	setgain(gspca_dev);
-}
-
 static void sethvflip(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 data, hflip, vflip;
 
-	hflip = sd->ctrls[HFLIP].val;
+	hflip = sd->hflip->val;
 	if (sd->flags & FL_HFLIP)
 		hflip = !hflip;
-	vflip = sd->ctrls[VFLIP].val;
+	vflip = sd->vflip->val;
 	if (sd->flags & FL_VFLIP)
 		vflip = !vflip;
 
@@ -717,6 +538,112 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return gspca_dev->usb_err;
 }
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_AUTOGAIN && ctrl->is_new && ctrl->val) {
+		/* when switching to autogain set defaults to make sure
+		   we are on a valid point of the autogain gain /
+		   exposure knee graph, and give this change time to
+		   take effect before doing autogain. */
+		gspca_dev->exposure->val    = PAC7302_EXPOSURE_DEFAULT;
+		gspca_dev->gain->val        = PAC7302_GAIN_DEFAULT;
+		sd->autogain_ignore_frames  = PAC_AUTOGAIN_IGNORE_FRAMES;
+	}
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightcont(gspca_dev);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev);
+		break;
+	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
+		setwhitebalance(gspca_dev);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		setredbalance(gspca_dev);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		setbluebalance(gspca_dev);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (gspca_dev->exposure->is_new || (ctrl->is_new && ctrl->val))
+			setexposure(gspca_dev);
+		if (gspca_dev->gain->is_new || (ctrl->is_new && ctrl->val))
+			setgain(gspca_dev);
+		break;
+	case V4L2_CID_HFLIP:
+		sethvflip(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+/* this function is called at probe time */
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 11);
+
+	sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_BRIGHTNESS, 0, 32, 1, 16);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_CONTRAST, 0, 255, 1, 127);
+
+	sd->saturation = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_SATURATION, 0, 255, 1, 127);
+	sd->white_balance = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_WHITE_BALANCE_TEMPERATURE,
+					0, 255, 1, 4);
+	sd->red_balance = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 3, 1, 1);
+	sd->blue_balance = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_RED_BALANCE, 0, 3, 1, 1);
+
+	gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_EXPOSURE, 0, 1023, 1,
+					PAC7302_EXPOSURE_DEFAULT);
+	gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 62, 1,
+					PAC7302_GAIN_DEFAULT);
+
+	sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	v4l2_ctrl_cluster(2, &sd->brightness);
+	v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	v4l2_ctrl_cluster(2, &sd->hflip);
+	return 0;
+}
+
+/* -- start the camera -- */
 static int sd_start(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -728,11 +655,13 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	setwhitebalance(gspca_dev);
 	setredbalance(gspca_dev);
 	setbluebalance(gspca_dev);
-	setautogain(gspca_dev);
+	setexposure(gspca_dev);
+	setgain(gspca_dev);
 	sethvflip(gspca_dev);
 
 	sd->sof_read = 0;
-	atomic_set(&sd->avg_lum, 270 + sd->ctrls[BRIGHTNESS].val);
+	sd->autogain_ignore_frames = 0;
+	atomic_set(&sd->avg_lum, 270 + sd->brightness->val);
 
 	/* start stream */
 	reg_w(gspca_dev, 0xff, 0x01);
@@ -758,9 +687,6 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x78, 0x40);
 }
 
-#define WANT_REGULAR_AUTOGAIN
-#include "autogain_functions.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -774,11 +700,13 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 	if (sd->autogain_ignore_frames > 0) {
 		sd->autogain_ignore_frames--;
 	} else {
-		desired_lum = 270 + sd->ctrls[BRIGHTNESS].val;
+		desired_lum = 270 + sd->brightness->val;
 
-		auto_gain_n_exposure(gspca_dev, avg_lum, desired_lum,
-				deadzone, GAIN_KNEE, EXPOSURE_KNEE);
-		sd->autogain_ignore_frames = PAC_AUTOGAIN_IGNORE_FRAMES;
+		if (gspca_expo_autogain(gspca_dev, avg_lum, desired_lum,
+					deadzone, PAC7302_GAIN_KNEE,
+					PAC7302_EXPOSURE_KNEE))
+			sd->autogain_ignore_frames =
+						PAC_AUTOGAIN_IGNORE_FRAMES;
 	}
 }
 
@@ -944,10 +872,9 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 /* sub-driver description for pac7302 */
 static const struct sd_desc sd_desc = {
 	.name = KBUILD_MODNAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
@@ -998,6 +925,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/pac7311.c b/drivers/media/video/gspca/pac7311.c
index 115da169f32..ba3558d3f01 100644
--- a/drivers/media/video/gspca/pac7311.c
+++ b/drivers/media/video/gspca/pac7311.c
@@ -694,6 +694,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/se401.c b/drivers/media/video/gspca/se401.c
index bb70092c222..a33cb78a839 100644
--- a/drivers/media/video/gspca/se401.c
+++ b/drivers/media/video/gspca/se401.c
@@ -45,15 +45,6 @@ MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
 MODULE_DESCRIPTION("Endpoints se401");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	GAIN,
-	EXPOSURE,
-	FREQ,
-	NCTRL	/* number of controls */
-};
-
 /* exposure change state machine states */
 enum {
 	EXPO_CHANGED,
@@ -64,7 +55,11 @@ enum {
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-	struct gspca_ctrl ctrls[NCTRL];
+	struct { /* exposure/freq control cluster */
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *freq;
+	};
+	bool has_brightness;
 	struct v4l2_pix_format fmts[MAX_MODES];
 	int pixels_read;
 	int packet_read;
@@ -77,60 +72,6 @@ struct sd {
 	int expo_change_state;
 };
 
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setgain(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[NCTRL] = {
-[BRIGHTNESS] = {
-		{
-			.id      = V4L2_CID_BRIGHTNESS,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Brightness",
-			.minimum = 0,
-			.maximum = 255,
-			.step    = 1,
-			.default_value = 15,
-		},
-		.set_control = setbrightness
-	},
-[GAIN] = {
-		{
-			.id      = V4L2_CID_GAIN,
-			.type    = V4L2_CTRL_TYPE_INTEGER,
-			.name    = "Gain",
-			.minimum = 0,
-			.maximum = 50, /* Really 63 but > 50 is not pretty */
-			.step    = 1,
-			.default_value = 25,
-		},
-		.set_control = setgain
-	},
-[EXPOSURE] = {
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Exposure",
-			.minimum = 0,
-			.maximum = 32767,
-			.step = 1,
-			.default_value = 15000,
-		},
-		.set_control = setexposure
-	},
-[FREQ] = {
-		{
-			.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-			.type    = V4L2_CTRL_TYPE_MENU,
-			.name    = "Light frequency filter",
-			.minimum = 0,
-			.maximum = 2,
-			.step    = 1,
-			.default_value = 0,
-		},
-		.set_control = setexposure
-	},
-};
 
 static void se401_write_req(struct gspca_dev *gspca_dev, u16 req, u16 value,
 			    int silent)
@@ -224,22 +165,15 @@ static int se401_get_feature(struct gspca_dev *gspca_dev, u16 selector)
 	return gspca_dev->usb_buf[0] | (gspca_dev->usb_buf[1] << 8);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (gspca_dev->ctrl_dis & (1 << BRIGHTNESS))
-		return;
-
 	/* HDG: this does not seem to do anything on my cam */
-	se401_write_req(gspca_dev, SE401_REQ_SET_BRT,
-			sd->ctrls[BRIGHTNESS].val, 0);
+	se401_write_req(gspca_dev, SE401_REQ_SET_BRT, val, 0);
 }
 
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u16 gain = 63 - sd->ctrls[GAIN].val;
+	u16 gain = 63 - val;
 
 	/* red color gain */
 	se401_set_feature(gspca_dev, HV7131_REG_ARCG, gain);
@@ -249,10 +183,10 @@ static void setgain(struct gspca_dev *gspca_dev)
 	se401_set_feature(gspca_dev, HV7131_REG_ABCG, gain);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val, s32 freq)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int integration = sd->ctrls[EXPOSURE].val << 6;
+	int integration = val << 6;
 	u8 expose_h, expose_m, expose_l;
 
 	/* Do this before the set_feature calls, for proper timing wrt
@@ -262,9 +196,9 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	   through so be it */
 	sd->expo_change_state = EXPO_CHANGED;
 
-	if (sd->ctrls[FREQ].val == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
+	if (freq == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
 		integration = integration - integration % 106667;
-	if (sd->ctrls[FREQ].val == V4L2_CID_POWER_LINE_FREQUENCY_60HZ)
+	if (freq == V4L2_CID_POWER_LINE_FREQUENCY_60HZ)
 		integration = integration - integration % 88889;
 
 	expose_h = (integration >> 16);
@@ -375,15 +309,12 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam->bulk = 1;
 	cam->bulk_size = BULK_SIZE;
 	cam->bulk_nurbs = 4;
-	cam->ctrls = sd->ctrls;
 	sd->resetlevel = 0x2d; /* Set initial resetlevel */
 
 	/* See if the camera supports brightness */
 	se401_read_req(gspca_dev, SE401_REQ_GET_BRT, 1);
-	if (gspca_dev->usb_err) {
-		gspca_dev->ctrl_dis = (1 << BRIGHTNESS);
-		gspca_dev->usb_err = 0;
-	}
+	sd->has_brightness = !!gspca_dev->usb_err;
+	gspca_dev->usb_err = 0;
 
 	return 0;
 }
@@ -442,9 +373,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	}
 	se401_set_feature(gspca_dev, SE401_OPERATINGMODE, mode);
 
-	setbrightness(gspca_dev);
-	setgain(gspca_dev);
-	setexposure(gspca_dev);
 	se401_set_feature(gspca_dev, HV7131_REG_ARLV, sd->resetlevel);
 
 	sd->packet_read = 0;
@@ -666,27 +594,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev, u8 *data, int len)
 		sd_pkt_scan_janggu(gspca_dev, data, len);
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		switch (menu->index) {
-		case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
-			strcpy((char *) menu->name, "NoFliker");
-			return 0;
-		case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
-			strcpy((char *) menu->name, "50 Hz");
-			return 0;
-		case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
-			strcpy((char *) menu->name, "60 Hz");
-			return 0;
-		}
-		break;
-	}
-	return -EINVAL;
-}
-
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 static int sd_int_pkt_scan(struct gspca_dev *gspca_dev, u8 *data, int len)
 {
@@ -714,19 +621,73 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev, u8 *data, int len)
 }
 #endif
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		setgain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, ctrl->val, sd->freq->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	if (sd->has_brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 15);
+	/* max is really 63 but > 50 is not pretty */
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 50, 1, 25);
+	sd->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 32767, 1, 15000);
+	sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(2, &sd->exposure);
+	return 0;
+}
+
 /* 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,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.dq_callback = sd_dq_callback,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 	.int_pkt_scan = sd_int_pkt_scan,
 #endif
@@ -769,6 +730,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 	.pre_reset = sd_pre_reset,
 	.post_reset = sd_post_reset,
diff --git a/drivers/media/video/gspca/sn9c2028.c b/drivers/media/video/gspca/sn9c2028.c
index 478533cb115..03fa3fd940b 100644
--- a/drivers/media/video/gspca/sn9c2028.c
+++ b/drivers/media/video/gspca/sn9c2028.c
@@ -40,10 +40,6 @@ struct init_command {
 	unsigned char to_read; /* length to read. 0 means no reply requested */
 };
 
-/* V4L2 controls supported by the driver */
-static const struct ctrl sd_ctrls[] = {
-};
-
 /* How to change the resolution of any of the VGA cams is unknown */
 static const struct v4l2_pix_format vga_mode[] = {
 	{640, 480, V4L2_PIX_FMT_SN9C2028, V4L2_FIELD_NONE,
@@ -695,8 +691,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* 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,
@@ -734,6 +728,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/sonixb.c b/drivers/media/video/gspca/sonixb.c
index e2bdf8f632f..fd1f8d2d3b0 100644
--- a/drivers/media/video/gspca/sonixb.c
+++ b/drivers/media/video/gspca/sonixb.c
@@ -56,26 +56,16 @@ MODULE_AUTHOR("Jean-François Moine <http://moinejf.free.fr>");
 MODULE_DESCRIPTION("GSPCA/SN9C102 USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	GAIN,
-	EXPOSURE,
-	AUTOGAIN,
-	FREQ,
-	NCTRLS		/* number of controls */
-};
-
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct v4l2_ctrl *brightness;
+	struct v4l2_ctrl *plfreq;
 
 	atomic_t avg_lum;
 	int prev_avg_lum;
-	int exp_too_low_cnt;
-	int exp_too_high_cnt;
+	int exposure_knee;
 	int header_read;
 	u8 header[12]; /* Header without sof marker */
 
@@ -107,24 +97,16 @@ struct sensor_data {
 	sensor_init_t *sensor_init;
 	int sensor_init_size;
 	int flags;
-	unsigned ctrl_dis;
 	__u8 sensor_addr;
 };
 
 /* sensor_data flags */
-#define F_GAIN 0x01		/* has gain */
-#define F_SIF  0x02		/* sif or vga */
-#define F_COARSE_EXPO 0x04	/* exposure control is coarse */
+#define F_SIF		0x01	/* sif or vga */
 
 /* priv field of struct v4l2_pix_format flags (do not use low nibble!) */
 #define MODE_RAW 0x10		/* raw bayer mode */
 #define MODE_REDUCED_SIF 0x20	/* vga mode (320x240 / 160x120) on sif cam */
 
-/* ctrl_dis helper macros */
-#define NO_EXPO ((1 << EXPOSURE) | (1 << AUTOGAIN))
-#define NO_FREQ (1 << FREQ)
-#define NO_BRIGHTNESS (1 << BRIGHTNESS)
-
 #define COMP 0xc7		/* 0x87 //0x07 */
 #define COMP1 0xc9		/* 0x89 //0x09 */
 
@@ -133,12 +115,12 @@ struct sensor_data {
 
 #define SYS_CLK 0x04
 
-#define SENS(bridge, sensor, _flags, _ctrl_dis, _sensor_addr) \
+#define SENS(bridge, sensor, _flags, _sensor_addr) \
 { \
 	.bridge_init = bridge, \
 	.sensor_init = sensor, \
 	.sensor_init_size = sizeof(sensor), \
-	.flags = _flags, .ctrl_dis = _ctrl_dis, .sensor_addr = _sensor_addr \
+	.flags = _flags, .sensor_addr = _sensor_addr \
 }
 
 /* We calculate the autogain at the end of the transfer of a frame, at this
@@ -147,87 +129,6 @@ struct sensor_data {
    the new settings to come into effect before doing any other adjustments. */
 #define AUTOGAIN_IGNORE_FRAMES 1
 
-/* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setgain(struct gspca_dev *gspca_dev);
-static void setexposure(struct gspca_dev *gspca_dev);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static void setfreq(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setbrightness
-	},
-[GAIN] = {
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define GAIN_KNEE 230
-		.default_value = 127,
-	    },
-	    .set_control = setgain
-	},
-[EXPOSURE] = {
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Exposure",
-			.minimum = 0,
-			.maximum = 1023,
-			.step = 1,
-			.default_value = 66,
-				/*  33 ms / 30 fps (except on PASXXX) */
-#define EXPOSURE_KNEE 200	/* 100 ms / 10 fps (except on PASXXX) */
-			.flags = 0,
-		},
-		.set_control = setexposure
-	},
-/* for coarse exposure */
-#define COARSE_EXPOSURE_MIN 2
-#define COARSE_EXPOSURE_MAX 15
-#define COARSE_EXPOSURE_DEF  2 /* 30 fps */
-[AUTOGAIN] = {
-		{
-			.id = V4L2_CID_AUTOGAIN,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "Automatic Gain (and Exposure)",
-			.minimum = 0,
-			.maximum = 1,
-			.step = 1,
-#define AUTOGAIN_DEF 1
-			.default_value = AUTOGAIN_DEF,
-			.flags = V4L2_CTRL_FLAG_UPDATE
-		},
-		.set = sd_setautogain,
-	},
-[FREQ] = {
-		{
-			.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-			.type    = V4L2_CTRL_TYPE_MENU,
-			.name    = "Light frequency filter",
-			.minimum = 0,
-			.maximum = 2,	/* 0: 0, 1: 50Hz, 2:60Hz */
-			.step    = 1,
-#define FREQ_DEF 0
-			.default_value = FREQ_DEF,
-		},
-		.set_control = setfreq
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -532,25 +433,27 @@ static const __u8 tas5130_sensor_init[][8] = {
 };
 
 static const struct sensor_data sensor_data[] = {
-SENS(initHv7131d, hv7131d_sensor_init, F_GAIN, NO_BRIGHTNESS|NO_FREQ, 0),
-SENS(initHv7131r, hv7131r_sensor_init, 0, NO_BRIGHTNESS|NO_EXPO|NO_FREQ, 0),
-SENS(initOv6650, ov6650_sensor_init, F_GAIN|F_SIF, 0, 0x60),
-SENS(initOv7630, ov7630_sensor_init, F_GAIN, 0, 0x21),
-SENS(initPas106, pas106_sensor_init, F_GAIN|F_SIF, NO_FREQ, 0),
-SENS(initPas202, pas202_sensor_init, F_GAIN, NO_FREQ, 0),
-SENS(initTas5110c, tas5110c_sensor_init, F_GAIN|F_SIF|F_COARSE_EXPO,
-	NO_BRIGHTNESS|NO_FREQ, 0),
-SENS(initTas5110d, tas5110d_sensor_init, F_GAIN|F_SIF|F_COARSE_EXPO,
-	NO_BRIGHTNESS|NO_FREQ, 0),
-SENS(initTas5130, tas5130_sensor_init, F_GAIN,
-	NO_BRIGHTNESS|NO_EXPO|NO_FREQ, 0),
+	SENS(initHv7131d, hv7131d_sensor_init, 0, 0),
+	SENS(initHv7131r, hv7131r_sensor_init, 0, 0),
+	SENS(initOv6650, ov6650_sensor_init, F_SIF, 0x60),
+	SENS(initOv7630, ov7630_sensor_init, 0, 0x21),
+	SENS(initPas106, pas106_sensor_init, F_SIF, 0),
+	SENS(initPas202, pas202_sensor_init, 0, 0),
+	SENS(initTas5110c, tas5110c_sensor_init, F_SIF, 0),
+	SENS(initTas5110d, tas5110d_sensor_init, F_SIF, 0),
+	SENS(initTas5130, tas5130_sensor_init, 0, 0),
 };
 
 /* get one byte in gspca_dev->usb_buf */
 static void reg_r(struct gspca_dev *gspca_dev,
 		  __u16 value)
 {
-	usb_control_msg(gspca_dev->dev,
+	int res;
+
+	if (gspca_dev->usb_err < 0)
+		return;
+
+	res = usb_control_msg(gspca_dev->dev,
 			usb_rcvctrlpipe(gspca_dev->dev, 0),
 			0,			/* request */
 			USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
@@ -558,6 +461,12 @@ static void reg_r(struct gspca_dev *gspca_dev,
 			0,			/* index */
 			gspca_dev->usb_buf, 1,
 			500);
+
+	if (res < 0) {
+		dev_err(gspca_dev->v4l2_dev.dev,
+			"Error reading register %02x: %d\n", value, res);
+		gspca_dev->usb_err = res;
+	}
 }
 
 static void reg_w(struct gspca_dev *gspca_dev,
@@ -565,14 +474,13 @@ static void reg_w(struct gspca_dev *gspca_dev,
 		  const __u8 *buffer,
 		  int len)
 {
-#ifdef GSPCA_DEBUG
-	if (len > USB_BUF_SZ) {
-		PDEBUG(D_ERR|D_PACK, "reg_w: buffer overflow");
+	int res;
+
+	if (gspca_dev->usb_err < 0)
 		return;
-	}
-#endif
+
 	memcpy(gspca_dev->usb_buf, buffer, len);
-	usb_control_msg(gspca_dev->dev,
+	res = usb_control_msg(gspca_dev->dev,
 			usb_sndctrlpipe(gspca_dev->dev, 0),
 			0x08,			/* request */
 			USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
@@ -580,30 +488,48 @@ static void reg_w(struct gspca_dev *gspca_dev,
 			0,			/* index */
 			gspca_dev->usb_buf, len,
 			500);
+
+	if (res < 0) {
+		dev_err(gspca_dev->v4l2_dev.dev,
+			"Error writing register %02x: %d\n", value, res);
+		gspca_dev->usb_err = res;
+	}
 }
 
-static int i2c_w(struct gspca_dev *gspca_dev, const __u8 *buffer)
+static void i2c_w(struct gspca_dev *gspca_dev, const __u8 *buffer)
 {
 	int retry = 60;
 
+	if (gspca_dev->usb_err < 0)
+		return;
+
 	/* is i2c ready */
 	reg_w(gspca_dev, 0x08, buffer, 8);
 	while (retry--) {
+		if (gspca_dev->usb_err < 0)
+			return;
 		msleep(10);
 		reg_r(gspca_dev, 0x08);
 		if (gspca_dev->usb_buf[0] & 0x04) {
-			if (gspca_dev->usb_buf[0] & 0x08)
-				return -1;
-			return 0;
+			if (gspca_dev->usb_buf[0] & 0x08) {
+				dev_err(gspca_dev->v4l2_dev.dev,
+					"i2c write error\n");
+				gspca_dev->usb_err = -EIO;
+			}
+			return;
 		}
 	}
-	return -1;
+
+	dev_err(gspca_dev->v4l2_dev.dev, "i2c write timeout\n");
+	gspca_dev->usb_err = -EIO;
 }
 
 static void i2c_w_vector(struct gspca_dev *gspca_dev,
 			const __u8 buffer[][8], int len)
 {
 	for (;;) {
+		if (gspca_dev->usb_err < 0)
+			return;
 		reg_w(gspca_dev, 0x08, *buffer, 8);
 		len -= 8;
 		if (len <= 0)
@@ -624,11 +550,10 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 
 		/* change reg 0x06 */
 		i2cOV[1] = sensor_data[sd->sensor].sensor_addr;
-		i2cOV[3] = sd->ctrls[BRIGHTNESS].val;
-		if (i2c_w(gspca_dev, i2cOV) < 0)
-			goto err;
+		i2cOV[3] = sd->brightness->val;
+		i2c_w(gspca_dev, i2cOV);
 		break;
-	    }
+	}
 	case SENSOR_PAS106:
 	case SENSOR_PAS202: {
 		__u8 i2cpbright[] =
@@ -642,54 +567,49 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 			i2cpdoit[2] = 0x13;
 		}
 
-		if (sd->ctrls[BRIGHTNESS].val < 127) {
+		if (sd->brightness->val < 127) {
 			/* change reg 0x0b, signreg */
 			i2cpbright[3] = 0x01;
 			/* set reg 0x0c, offset */
-			i2cpbright[4] = 127 - sd->ctrls[BRIGHTNESS].val;
+			i2cpbright[4] = 127 - sd->brightness->val;
 		} else
-			i2cpbright[4] = sd->ctrls[BRIGHTNESS].val - 127;
+			i2cpbright[4] = sd->brightness->val - 127;
 
-		if (i2c_w(gspca_dev, i2cpbright) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpdoit) < 0)
-			goto err;
+		i2c_w(gspca_dev, i2cpbright);
+		i2c_w(gspca_dev, i2cpdoit);
+		break;
+	}
+	default:
 		break;
-	    }
 	}
-	return;
-err:
-	PDEBUG(D_ERR, "i2c error brightness");
 }
 
-static void setsensorgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 gain = sd->ctrls[GAIN].val;
+	u8 gain = gspca_dev->gain->val;
 
 	switch (sd->sensor) {
 	case SENSOR_HV7131D: {
 		__u8 i2c[] =
 			{0xc0, 0x11, 0x31, 0x00, 0x00, 0x00, 0x00, 0x17};
 
-		i2c[3] = 0x3f - (gain / 4);
-		i2c[4] = 0x3f - (gain / 4);
-		i2c[5] = 0x3f - (gain / 4);
+		i2c[3] = 0x3f - gain;
+		i2c[4] = 0x3f - gain;
+		i2c[5] = 0x3f - gain;
 
-		if (i2c_w(gspca_dev, i2c) < 0)
-			goto err;
+		i2c_w(gspca_dev, i2c);
 		break;
-	    }
+	}
 	case SENSOR_TAS5110C:
 	case SENSOR_TAS5130CXX: {
 		__u8 i2c[] =
 			{0x30, 0x11, 0x02, 0x20, 0x70, 0x00, 0x00, 0x10};
 
 		i2c[4] = 255 - gain;
-		if (i2c_w(gspca_dev, i2c) < 0)
-			goto err;
+		i2c_w(gspca_dev, i2c);
 		break;
-	    }
+	}
 	case SENSOR_TAS5110D: {
 		__u8 i2c[] = {
 			0xb0, 0x61, 0x02, 0x00, 0x10, 0x00, 0x00, 0x17 };
@@ -703,23 +623,25 @@ static void setsensorgain(struct gspca_dev *gspca_dev)
 		i2c[3] |= (gain & 0x04) << 3;
 		i2c[3] |= (gain & 0x02) << 5;
 		i2c[3] |= (gain & 0x01) << 7;
-		if (i2c_w(gspca_dev, i2c) < 0)
-			goto err;
+		i2c_w(gspca_dev, i2c);
 		break;
-	    }
-
+	}
 	case SENSOR_OV6650:
-		gain >>= 1;
-		/* fall thru */
 	case SENSOR_OV7630: {
 		__u8 i2c[] = {0xa0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10};
 
+		/*
+		 * The ov7630's gain is weird, at 32 the gain drops to the
+		 * same level as at 16, so skip 32-47 (of the 0-63 scale).
+		 */
+		if (sd->sensor == SENSOR_OV7630 && gain >= 32)
+			gain += 16;
+
 		i2c[1] = sensor_data[sd->sensor].sensor_addr;
-		i2c[3] = gain >> 2;
-		if (i2c_w(gspca_dev, i2c) < 0)
-			goto err;
+		i2c[3] = gain;
+		i2c_w(gspca_dev, i2c);
 		break;
-	    }
+	}
 	case SENSOR_PAS106:
 	case SENSOR_PAS202: {
 		__u8 i2cpgain[] =
@@ -737,49 +659,27 @@ static void setsensorgain(struct gspca_dev *gspca_dev)
 			i2cpdoit[2] = 0x13;
 		}
 
-		i2cpgain[3] = gain >> 3;
-		i2cpcolorgain[3] = gain >> 4;
-		i2cpcolorgain[4] = gain >> 4;
-		i2cpcolorgain[5] = gain >> 4;
-		i2cpcolorgain[6] = gain >> 4;
-
-		if (i2c_w(gspca_dev, i2cpgain) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpcolorgain) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpdoit) < 0)
-			goto err;
-		break;
-	    }
-	}
-	return;
-err:
-	PDEBUG(D_ERR, "i2c error gain");
-}
-
-static void setgain(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	__u8 gain;
-	__u8 buf[3] = { 0, 0, 0 };
+		i2cpgain[3] = gain;
+		i2cpcolorgain[3] = gain >> 1;
+		i2cpcolorgain[4] = gain >> 1;
+		i2cpcolorgain[5] = gain >> 1;
+		i2cpcolorgain[6] = gain >> 1;
 
-	if (sensor_data[sd->sensor].flags & F_GAIN) {
-		/* Use the sensor gain to do the actual gain */
-		setsensorgain(gspca_dev);
-		return;
+		i2c_w(gspca_dev, i2cpgain);
+		i2c_w(gspca_dev, i2cpcolorgain);
+		i2c_w(gspca_dev, i2cpdoit);
+		break;
 	}
-
-	if (sd->bridge == BRIDGE_103) {
-		gain = sd->ctrls[GAIN].val >> 1;
-		buf[0] = gain; /* Red */
-		buf[1] = gain; /* Green */
-		buf[2] = gain; /* Blue */
-		reg_w(gspca_dev, 0x05, buf, 3);
-	} else {
-		gain = sd->ctrls[GAIN].val >> 4;
-		buf[0] = gain << 4 | gain; /* Red and blue */
-		buf[1] = gain; /* Green */
-		reg_w(gspca_dev, 0x10, buf, 2);
+	default:
+		if (sd->bridge == BRIDGE_103) {
+			u8 buf[3] = { gain, gain, gain }; /* R, G, B */
+			reg_w(gspca_dev, 0x05, buf, 3);
+		} else {
+			u8 buf[2];
+			buf[0] = gain << 4 | gain; /* Red and blue */
+			buf[1] = gain; /* Green */
+			reg_w(gspca_dev, 0x10, buf, 2);
+		}
 	}
 }
 
@@ -792,31 +692,24 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		/* Note the datasheet wrongly says line mode exposure uses reg
 		   0x26 and 0x27, testing has shown 0x25 + 0x26 */
 		__u8 i2c[] = {0xc0, 0x11, 0x25, 0x00, 0x00, 0x00, 0x00, 0x17};
-		/* The HV7131D's exposure goes from 0 - 65535, we scale our
-		   exposure of 0-1023 to 0-6138. There are 2 reasons for this:
-		   1) This puts our exposure knee of 200 at approx the point
-		      where the framerate starts dropping
-		   2) At 6138 the framerate has already dropped to 2 fps,
-		      going any lower makes little sense */
-		u16 reg = sd->ctrls[EXPOSURE].val * 6;
+		u16 reg = gspca_dev->exposure->val;
 
 		i2c[3] = reg >> 8;
 		i2c[4] = reg & 0xff;
-		if (i2c_w(gspca_dev, i2c) != 0)
-			goto err;
+		i2c_w(gspca_dev, i2c);
 		break;
-	    }
+	}
 	case SENSOR_TAS5110C:
 	case SENSOR_TAS5110D: {
 		/* register 19's high nibble contains the sn9c10x clock divider
 		   The high nibble configures the no fps according to the
 		   formula: 60 / high_nibble. With a maximum of 30 fps */
-		u8 reg = sd->ctrls[EXPOSURE].val;
+		u8 reg = gspca_dev->exposure->val;
 
 		reg = (reg << 4) | 0x0b;
 		reg_w(gspca_dev, 0x19, &reg, 1);
 		break;
-	    }
+	}
 	case SENSOR_OV6650:
 	case SENSOR_OV7630: {
 		/* The ov6650 / ov7630 have 2 registers which both influence
@@ -848,7 +741,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		} else
 			reg10_max = 0x41;
 
-		reg11 = (15 * sd->ctrls[EXPOSURE].val + 999) / 1000;
+		reg11 = (15 * gspca_dev->exposure->val + 999) / 1000;
 		if (reg11 < 1)
 			reg11 = 1;
 		else if (reg11 > 16)
@@ -861,16 +754,16 @@ static void setexposure(struct gspca_dev *gspca_dev)
 			reg11 = 4;
 
 		/* frame exposure time in ms = 1000 * reg11 / 30    ->
-		reg10 = (sd->ctrls[EXPOSURE].val / 2) * reg10_max
+		reg10 = (gspca_dev->exposure->val / 2) * reg10_max
 				/ (1000 * reg11 / 30) */
-		reg10 = (sd->ctrls[EXPOSURE].val * 15 * reg10_max)
+		reg10 = (gspca_dev->exposure->val * 15 * reg10_max)
 				/ (1000 * reg11);
 
 		/* Don't allow this to get below 10 when using autogain, the
 		   steps become very large (relatively) when below 10 causing
 		   the image to oscilate from much too dark, to much too bright
 		   and back again. */
-		if (sd->ctrls[AUTOGAIN].val && reg10 < 10)
+		if (gspca_dev->autogain->val && reg10 < 10)
 			reg10 = 10;
 		else if (reg10 > reg10_max)
 			reg10 = reg10_max;
@@ -884,12 +777,11 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		if (sd->reg11 == reg11)
 			i2c[0] = 0xa0;
 
-		if (i2c_w(gspca_dev, i2c) == 0)
+		i2c_w(gspca_dev, i2c);
+		if (gspca_dev->usb_err == 0)
 			sd->reg11 = reg11;
-		else
-			goto err;
 		break;
-	    }
+	}
 	case SENSOR_PAS202: {
 		__u8 i2cpframerate[] =
 			{0xb0, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, 0x16};
@@ -909,28 +801,25 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		   frame exposure times (like we are doing with the ov chips),
 		   as that sometimes leads to jumps in the exposure control,
 		   which are bad for auto exposure. */
-		if (sd->ctrls[EXPOSURE].val < 200) {
-			i2cpexpo[3] = 255 - (sd->ctrls[EXPOSURE].val * 255)
+		if (gspca_dev->exposure->val < 200) {
+			i2cpexpo[3] = 255 - (gspca_dev->exposure->val * 255)
 						/ 200;
 			framerate_ctrl = 500;
 		} else {
 			/* The PAS202's exposure control goes from 0 - 4095,
 			   but anything below 500 causes vsync issues, so scale
 			   our 200-1023 to 500-4095 */
-			framerate_ctrl = (sd->ctrls[EXPOSURE].val - 200)
+			framerate_ctrl = (gspca_dev->exposure->val - 200)
 							* 1000 / 229 +  500;
 		}
 
 		i2cpframerate[3] = framerate_ctrl >> 6;
 		i2cpframerate[4] = framerate_ctrl & 0x3f;
-		if (i2c_w(gspca_dev, i2cpframerate) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpexpo) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpdoit) < 0)
-			goto err;
+		i2c_w(gspca_dev, i2cpframerate);
+		i2c_w(gspca_dev, i2cpexpo);
+		i2c_w(gspca_dev, i2cpdoit);
 		break;
-	    }
+	}
 	case SENSOR_PAS106: {
 		__u8 i2cpframerate[] =
 			{0xb1, 0x40, 0x03, 0x00, 0x00, 0x00, 0x00, 0x14};
@@ -942,46 +831,40 @@ static void setexposure(struct gspca_dev *gspca_dev)
 
 		/* For values below 150 use partial frame exposure, above
 		   that use framerate ctrl */
-		if (sd->ctrls[EXPOSURE].val < 150) {
-			i2cpexpo[3] = 150 - sd->ctrls[EXPOSURE].val;
+		if (gspca_dev->exposure->val < 150) {
+			i2cpexpo[3] = 150 - gspca_dev->exposure->val;
 			framerate_ctrl = 300;
 		} else {
 			/* The PAS106's exposure control goes from 0 - 4095,
 			   but anything below 300 causes vsync issues, so scale
 			   our 150-1023 to 300-4095 */
-			framerate_ctrl = (sd->ctrls[EXPOSURE].val - 150)
+			framerate_ctrl = (gspca_dev->exposure->val - 150)
 						* 1000 / 230 + 300;
 		}
 
 		i2cpframerate[3] = framerate_ctrl >> 4;
 		i2cpframerate[4] = framerate_ctrl & 0x0f;
-		if (i2c_w(gspca_dev, i2cpframerate) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpexpo) < 0)
-			goto err;
-		if (i2c_w(gspca_dev, i2cpdoit) < 0)
-			goto err;
+		i2c_w(gspca_dev, i2cpframerate);
+		i2c_w(gspca_dev, i2cpexpo);
+		i2c_w(gspca_dev, i2cpdoit);
+		break;
+	}
+	default:
 		break;
-	    }
 	}
-	return;
-err:
-	PDEBUG(D_ERR, "i2c error exposure");
 }
 
 static void setfreq(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	switch (sd->sensor) {
-	case SENSOR_OV6650:
-	case SENSOR_OV7630: {
+	if (sd->sensor == SENSOR_OV6650 || sd->sensor == SENSOR_OV7630) {
 		/* Framerate adjust register for artificial light 50 hz flicker
 		   compensation, for the ov6650 this is identical to ov6630
 		   0x2b register, see ov6630 datasheet.
 		   0x4f / 0x8a -> (30 fps -> 25 fps), 0x00 -> no adjustment */
 		__u8 i2c[] = {0xa0, 0x00, 0x2b, 0x00, 0x00, 0x00, 0x00, 0x10};
-		switch (sd->ctrls[FREQ].val) {
+		switch (sd->plfreq->val) {
 		default:
 /*		case 0:			 * no filter*/
 /*		case 2:			 * 60 hz */
@@ -993,25 +876,17 @@ static void setfreq(struct gspca_dev *gspca_dev)
 			break;
 		}
 		i2c[1] = sensor_data[sd->sensor].sensor_addr;
-		if (i2c_w(gspca_dev, i2c) < 0)
-			PDEBUG(D_ERR, "i2c error setfreq");
-		break;
-	    }
+		i2c_w(gspca_dev, i2c);
 	}
 }
 
-#define WANT_REGULAR_AUTOGAIN
-#define WANT_COARSE_EXPO_AUTOGAIN
-#include "autogain_functions.h"
-
 static void do_autogain(struct gspca_dev *gspca_dev)
 {
-	int deadzone, desired_avg_lum, result;
 	struct sd *sd = (struct sd *) gspca_dev;
-	int avg_lum = atomic_read(&sd->avg_lum);
+	int deadzone, desired_avg_lum, avg_lum;
 
-	if ((gspca_dev->ctrl_dis & (1 << AUTOGAIN)) ||
-	    avg_lum == -1 || !sd->ctrls[AUTOGAIN].val)
+	avg_lum = atomic_read(&sd->avg_lum);
+	if (avg_lum == -1)
 		return;
 
 	if (sd->autogain_ignore_frames > 0) {
@@ -1030,22 +905,18 @@ static void do_autogain(struct gspca_dev *gspca_dev)
 		desired_avg_lum = 13000;
 	}
 
-	if (sensor_data[sd->sensor].flags & F_COARSE_EXPO)
-		result = coarse_grained_expo_autogain(gspca_dev, avg_lum,
-				sd->ctrls[BRIGHTNESS].val
-						* desired_avg_lum / 127,
-				deadzone);
-	else
-		result = auto_gain_n_exposure(gspca_dev, avg_lum,
-				sd->ctrls[BRIGHTNESS].val
-						* desired_avg_lum / 127,
-				deadzone, GAIN_KNEE, EXPOSURE_KNEE);
-
-	if (result) {
-		PDEBUG(D_FRAM, "autogain: gain changed: gain: %d expo: %d",
-			(int) sd->ctrls[GAIN].val,
-			(int) sd->ctrls[EXPOSURE].val);
-		sd->autogain_ignore_frames = AUTOGAIN_IGNORE_FRAMES;
+	if (sd->brightness)
+		desired_avg_lum = sd->brightness->val * desired_avg_lum / 127;
+
+	if (gspca_dev->exposure->maximum < 500) {
+		if (gspca_coarse_grained_expo_autogain(gspca_dev, avg_lum,
+				desired_avg_lum, deadzone))
+			sd->autogain_ignore_frames = AUTOGAIN_IGNORE_FRAMES;
+	} else {
+		int gain_knee = gspca_dev->gain->maximum * 9 / 10;
+		if (gspca_expo_autogain(gspca_dev, avg_lum, desired_avg_lum,
+				deadzone, gain_knee, sd->exposure_knee))
+			sd->autogain_ignore_frames = AUTOGAIN_IGNORE_FRAMES;
 	}
 }
 
@@ -1064,14 +935,7 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	sd->sensor = id->driver_info >> 8;
 	sd->bridge = id->driver_info & 0xff;
 
-	gspca_dev->ctrl_dis = sensor_data[sd->sensor].ctrl_dis;
-#if AUTOGAIN_DEF
-	if (!(gspca_dev->ctrl_dis & (1 << AUTOGAIN)))
-		gspca_dev->ctrl_inac = (1 << GAIN) | (1 << EXPOSURE);
-#endif
-
 	cam = &gspca_dev->cam;
-	cam->ctrls = sd->ctrls;
 	if (!(sensor_data[sd->sensor].flags & F_SIF)) {
 		cam->cam_mode = vga_mode;
 		cam->nmodes = ARRAY_SIZE(vga_mode);
@@ -1087,18 +951,143 @@ static int sd_config(struct gspca_dev *gspca_dev,
 /* 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;
 	const __u8 stop = 0x09; /* Disable stream turn of LED */
 
-	if (sensor_data[sd->sensor].flags & F_COARSE_EXPO) {
-		sd->ctrls[EXPOSURE].min = COARSE_EXPOSURE_MIN;
-		sd->ctrls[EXPOSURE].max = COARSE_EXPOSURE_MAX;
-		sd->ctrls[EXPOSURE].def = COARSE_EXPOSURE_DEF;
-		if (sd->ctrls[EXPOSURE].val > COARSE_EXPOSURE_MAX)
-			sd->ctrls[EXPOSURE].val = COARSE_EXPOSURE_DEF;
+	reg_w(gspca_dev, 0x01, &stop, 1);
+
+	return gspca_dev->usb_err;
+}
+
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (ctrl->id == V4L2_CID_AUTOGAIN && ctrl->is_new && ctrl->val) {
+		/* when switching to autogain set defaults to make sure
+		   we are on a valid point of the autogain gain /
+		   exposure knee graph, and give this change time to
+		   take effect before doing autogain. */
+		gspca_dev->gain->val = gspca_dev->gain->default_value;
+		gspca_dev->exposure->val = gspca_dev->exposure->default_value;
+		sd->autogain_ignore_frames = AUTOGAIN_IGNORE_FRAMES;
 	}
 
-	reg_w(gspca_dev, 0x01, &stop, 1);
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (gspca_dev->exposure->is_new || (ctrl->is_new && ctrl->val))
+			setexposure(gspca_dev);
+		if (gspca_dev->gain->is_new || (ctrl->is_new && ctrl->val))
+			setgain(gspca_dev);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setfreq(gspca_dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+/* this function is called at probe time */
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+
+	if (sd->sensor == SENSOR_OV6650 || sd->sensor == SENSOR_OV7630 ||
+	    sd->sensor == SENSOR_PAS106 || sd->sensor == SENSOR_PAS202)
+		sd->brightness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+
+	/* Gain range is sensor dependent */
+	switch (sd->sensor) {
+	case SENSOR_OV6650:
+	case SENSOR_PAS106:
+	case SENSOR_PAS202:
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 31, 1, 15);
+		break;
+	case SENSOR_OV7630:
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 47, 1, 31);
+		break;
+	case SENSOR_HV7131D:
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 63, 1, 31);
+		break;
+	case SENSOR_TAS5110C:
+	case SENSOR_TAS5110D:
+	case SENSOR_TAS5130CXX:
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_GAIN, 0, 255, 1, 127);
+		break;
+	default:
+		if (sd->bridge == BRIDGE_103) {
+			gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+						V4L2_CID_GAIN, 0, 127, 1, 63);
+		} else {
+			gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+						V4L2_CID_GAIN, 0, 15, 1, 7);
+		}
+	}
+
+	/* Exposure range is sensor dependent, and not all have exposure */
+	switch (sd->sensor) {
+	case SENSOR_HV7131D:
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_EXPOSURE, 0, 8191, 1, 482);
+		sd->exposure_knee = 964;
+		break;
+	case SENSOR_OV6650:
+	case SENSOR_OV7630:
+	case SENSOR_PAS106:
+	case SENSOR_PAS202:
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_EXPOSURE, 0, 1023, 1, 66);
+		sd->exposure_knee = 200;
+		break;
+	case SENSOR_TAS5110C:
+	case SENSOR_TAS5110D:
+		gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+					V4L2_CID_EXPOSURE, 2, 15, 1, 2);
+		break;
+	}
+
+	if (gspca_dev->exposure) {
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+						V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	}
+
+	if (sd->sensor == SENSOR_OV6650 || sd->sensor == SENSOR_OV7630)
+		sd->plfreq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_DISABLED);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+
+	if (gspca_dev->autogain)
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
 
 	return 0;
 }
@@ -1242,10 +1231,10 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	sd->frames_to_drop = 0;
 	sd->autogain_ignore_frames = 0;
-	sd->exp_too_high_cnt = 0;
-	sd->exp_too_low_cnt = 0;
+	gspca_dev->exp_too_high_cnt = 0;
+	gspca_dev->exp_too_low_cnt = 0;
 	atomic_set(&sd->avg_lum, -1);
-	return 0;
+	return gspca_dev->usb_err;
 }
 
 static void sd_stopN(struct gspca_dev *gspca_dev)
@@ -1387,37 +1376,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->ctrls[AUTOGAIN].val = val;
-	sd->exp_too_high_cnt = 0;
-	sd->exp_too_low_cnt = 0;
-
-	/* when switching to autogain set defaults to make sure
-	   we are on a valid point of the autogain gain /
-	   exposure knee graph, and give this change time to
-	   take effect before doing autogain. */
-	if (sd->ctrls[AUTOGAIN].val
-	 && !(sensor_data[sd->sensor].flags & F_COARSE_EXPO)) {
-		sd->ctrls[EXPOSURE].val = sd->ctrls[EXPOSURE].def;
-		sd->ctrls[GAIN].val = sd->ctrls[GAIN].def;
-		if (gspca_dev->streaming) {
-			sd->autogain_ignore_frames = AUTOGAIN_IGNORE_FRAMES;
-			setexposure(gspca_dev);
-			setgain(gspca_dev);
-		}
-	}
-
-	if (sd->ctrls[AUTOGAIN].val)
-		gspca_dev->ctrl_inac = (1 << GAIN) | (1 << EXPOSURE);
-	else
-		gspca_dev->ctrl_inac = 0;
-
-	return 0;
-}
-
 static int sd_querymenu(struct gspca_dev *gspca_dev,
 			struct v4l2_querymenu *menu)
 {
@@ -1461,10 +1419,9 @@ static int sd_int_pkt_scan(struct gspca_dev *gspca_dev,
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -1529,6 +1486,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/sonixj.c b/drivers/media/video/gspca/sonixj.c
index f38faa9b37c..150b2df40f7 100644
--- a/drivers/media/video/gspca/sonixj.c
+++ b/drivers/media/video/gspca/sonixj.c
@@ -3199,6 +3199,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca1528.c b/drivers/media/video/gspca/spca1528.c
index 070b9c33b51..14d635277d7 100644
--- a/drivers/media/video/gspca/spca1528.c
+++ b/drivers/media/video/gspca/spca1528.c
@@ -33,102 +33,11 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	u8 brightness;
-	u8 contrast;
-	u8 hue;
-	u8 color;
-	u8 sharpness;
-
 	u8 pkt_seq;
 
 	u8 jpeg_hdr[JPEG_HDR_SZ];
 };
 
-/* V4L2 controls supported by the driver */
-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_sethue(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_gethue(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setcolor(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolor(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 const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define BRIGHTNESS_DEF 128
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 8,
-		.step    = 1,
-#define CONTRAST_DEF 1
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-	{
-	    {
-		.id      = V4L2_CID_HUE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Hue",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define HUE_DEF 0
-		.default_value = HUE_DEF,
-	    },
-	    .set = sd_sethue,
-	    .get = sd_gethue,
-	},
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 8,
-		.step    = 1,
-#define COLOR_DEF 1
-		.default_value = COLOR_DEF,
-	    },
-	    .set = sd_setcolor,
-	    .get = sd_getcolor,
-	},
-	{
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define SHARPNESS_DEF 0
-		.default_value = SHARPNESS_DEF,
-	    },
-	    .set = sd_setsharpness,
-	    .get = sd_getsharpness,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 /*		(does not work correctly)
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
@@ -259,58 +168,40 @@ static void wait_status_1(struct gspca_dev *gspca_dev)
 	gspca_dev->usb_err = -ETIME;
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_wb(gspca_dev, 0xc0, 0x0000, 0x00c0, sd->brightness);
+	reg_wb(gspca_dev, 0xc0, 0x0000, 0x00c0, val);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_wb(gspca_dev, 0xc1, 0x0000, 0x00c1, sd->contrast);
+	reg_wb(gspca_dev, 0xc1, 0x0000, 0x00c1, val);
 }
 
-static void sethue(struct gspca_dev *gspca_dev)
+static void sethue(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_wb(gspca_dev, 0xc2, 0x0000, 0x0000, sd->hue);
+	reg_wb(gspca_dev, 0xc2, 0x0000, 0x0000, val);
 }
 
-static void setcolor(struct gspca_dev *gspca_dev)
+static void setcolor(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_wb(gspca_dev, 0xc3, 0x0000, 0x00c3, sd->color);
+	reg_wb(gspca_dev, 0xc3, 0x0000, 0x00c3, val);
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_wb(gspca_dev, 0xc4, 0x0000, 0x00c4, sd->sharpness);
+	reg_wb(gspca_dev, 0xc4, 0x0000, 0x00c4, val);
 }
 
 /* this function is called at probe time */
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->cam.cam_mode = vga_mode;
 	gspca_dev->cam.nmodes = ARRAY_SIZE(vga_mode);
 	gspca_dev->cam.npkt = 128; /* number of packets per ISOC message */
 			/*fixme: 256 in ms-win traces*/
 
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->hue = HUE_DEF;
-	sd->color = COLOR_DEF;
-	sd->sharpness = SHARPNESS_DEF;
-
 	return 0;
 }
 
@@ -370,14 +261,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* the JPEG quality shall be 85% */
 	jpeg_set_qual(sd->jpeg_hdr, 85);
 
-	/* set the controls */
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	sethue(gspca_dev);
-	setcolor(gspca_dev);
-	setsharpness(gspca_dev);
-
-	msleep(5);
 	reg_r(gspca_dev, 0x00, 0x2520, 1);
 	msleep(8);
 
@@ -457,103 +340,70 @@ err:
 	gspca_dev->last_packet_type = DISCARD_PACKET;
 }
 
-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 gspca_dev->usb_err;
-}
-
-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)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->contrast = val;
-	if (gspca_dev->streaming)
-		setcontrast(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-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_sethue(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->hue = val;
-	if (gspca_dev->streaming)
-		sethue(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_gethue(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->hue;
-	return 0;
-}
-
-static int sd_setcolor(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->color = val;
-	if (gspca_dev->streaming)
-		setcolor(gspca_dev);
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		sethue(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolor(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	}
 	return gspca_dev->usb_err;
 }
 
-static int sd_getcolor(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->color;
-	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 gspca_dev->usb_err;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_getsharpness(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->sharpness;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 8, 1, 1);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 0);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 8, 1, 1);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 255, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
@@ -587,6 +437,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca500.c b/drivers/media/video/gspca/spca500.c
index 103984708c7..25cb68d0556 100644
--- a/drivers/media/video/gspca/spca500.c
+++ b/drivers/media/video/gspca/spca500.c
@@ -30,18 +30,12 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA/SPCA500 USB Camera Driver");
 MODULE_LICENSE("GPL");
 
+#define QUALITY 85
+
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	unsigned char brightness;
-	unsigned char contrast;
-	unsigned char colors;
-	u8 quality;
-#define QUALITY_MIN 70
-#define QUALITY_MAX 95
-#define QUALITY_DEF 85
-
 	char subtype;
 #define AgfaCl20 0
 #define AiptekPocketDV 1
@@ -62,59 +56,6 @@ struct sd {
 	u8 jpeg_hdr[JPEG_HDR_SZ];
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define BRIGHTNESS_DEF 127
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 63,
-		.step    = 1,
-#define CONTRAST_DEF 31
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 0,
-		.maximum = 63,
-		.step    = 1,
-#define COLOR_DEF 31
-		.default_value = COLOR_DEF,
-	    },
-	    .set = sd_setcolors,
-	    .get = sd_getcolors,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -641,10 +582,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->cam_mode = sif_mode;
 		cam->nmodes = ARRAY_SIZE(sif_mode);
 	}
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->colors = COLOR_DEF;
-	sd->quality = QUALITY_DEF;
 	return 0;
 }
 
@@ -673,7 +610,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x22);		/* JPEG 411 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	jpeg_set_qual(sd->jpeg_hdr, QUALITY);
 
 	if (sd->subtype == LogitechClickSmart310) {
 		xmult = 0x16;
@@ -934,122 +871,79 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	reg_w(gspca_dev, 0x00, 0x8167,
-			(__u8) (sd->brightness - 128));
+			(__u8) (val - 128));
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_w(gspca_dev, 0x00, 0x8168, sd->contrast);
+	reg_w(gspca_dev, 0x00, 0x8168, val);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_w(gspca_dev, 0x00, 0x8169, sd->colors);
+	reg_w(gspca_dev, 0x00, 0x8169, val);
 }
 
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->brightness = val;
-	if (gspca_dev->streaming)
-		setbrightness(gspca_dev);
-	return 0;
-}
+	gspca_dev->usb_err = 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;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	*val = sd->contrast;
-	return 0;
-}
-
-static int sd_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
-	return 0;
-}
-
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_set_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	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;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_get_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
-	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
-			| V4L2_JPEG_MARKER_DQT;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 63, 1, 31);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 63, 1, 31);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.get_jcomp = sd_get_jcomp,
-	.set_jcomp = sd_set_jcomp,
 };
 
 /* -- module initialisation -- */
@@ -1089,6 +983,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca501.c b/drivers/media/video/gspca/spca501.c
index 9c16821addd..3b7f777785b 100644
--- a/drivers/media/video/gspca/spca501.c
+++ b/drivers/media/video/gspca/spca501.c
@@ -49,91 +49,6 @@ struct sd {
 #define ViewQuestM318B 6
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setblue_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getblue_balance(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setred_balance(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getred_balance(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-#define MY_BRIGHTNESS 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 127,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define MY_CONTRAST 1
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 64725,
-		.step    = 1,
-		.default_value = 64725,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-#define MY_COLOR 2
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 0,
-		.maximum = 63,
-		.step    = 1,
-		.default_value = 20,
-	    },
-	    .set = sd_setcolors,
-	    .get = sd_getcolors,
-	},
-#define MY_BLUE_BALANCE 3
-	{
-	    {
-		.id      = V4L2_CID_BLUE_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Blue Balance",
-		.minimum = 0,
-		.maximum = 127,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set = sd_setblue_balance,
-	    .get = sd_getblue_balance,
-	},
-#define MY_RED_BALANCE 4
-	{
-	    {
-		.id      = V4L2_CID_RED_BALANCE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Red Balance",
-		.minimum = 0,
-		.maximum = 127,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set = sd_setred_balance,
-	    .get = sd_getred_balance,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA501, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -1878,42 +1793,32 @@ static int write_vector(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x12, sd->brightness);
+	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x12, val);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	reg_write(gspca_dev->dev, 0x00, 0x00,
-				  (sd->contrast >> 8) & 0xff);
+				  (val >> 8) & 0xff);
 	reg_write(gspca_dev->dev, 0x00, 0x01,
-				  sd->contrast & 0xff);
+				  val & 0xff);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x0c, sd->colors);
+	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x0c, val);
 }
 
-static void setblue_balance(struct gspca_dev *gspca_dev)
+static void setblue_balance(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x11, sd->blue_balance);
+	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x11, val);
 }
 
-static void setred_balance(struct gspca_dev *gspca_dev)
+static void setred_balance(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x13, sd->red_balance);
+	reg_write(gspca_dev->dev, SPCA501_REG_CCDSP, 0x13, val);
 }
 
 /* this function is called at probe time */
@@ -1927,9 +1832,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
 	sd->subtype = id->driver_info;
-	sd->brightness = sd_ctrls[MY_BRIGHTNESS].qctrl.default_value;
-	sd->contrast = sd_ctrls[MY_CONTRAST].qctrl.default_value;
-	sd->colors = sd_ctrls[MY_COLOR].qctrl.default_value;
 
 	return 0;
 }
@@ -2008,13 +1910,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	}
 	reg_write(dev, SPCA501_REG_CTLRL, 0x01, 0x02);
 
-	/* HDG atleast the Intel CreateAndShare needs to have one of its
-	 * brightness / contrast / color set otherwise it assumes what seems
-	 * max contrast. Note that strange enough setting any of these is
-	 * enough to fix the max contrast problem, to be sure we set all 3 */
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setcolors(gspca_dev);
 	return 0;
 }
 
@@ -2053,103 +1948,70 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	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;
+	gspca_dev->usb_err = 0;
 
-	*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_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
-	return 0;
-}
-
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
-}
-
-static int sd_setblue_balance(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->blue_balance = val;
-	if (gspca_dev->streaming)
-		setblue_balance(gspca_dev);
-	return 0;
-}
-
-static int sd_getblue_balance(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->blue_balance;
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		setblue_balance(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		setred_balance(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_setred_balance(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->red_balance = val;
-	if (gspca_dev->streaming)
-		setred_balance(gspca_dev);
-	return 0;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_getred_balance(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->red_balance;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 64725, 1, 64725);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 63, 1, 20);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 0, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 0, 127, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
@@ -2185,6 +2047,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca505.c b/drivers/media/video/gspca/spca505.c
index 1320f35e39f..bc7d67c3cb0 100644
--- a/drivers/media/video/gspca/spca505.c
+++ b/drivers/media/video/gspca/spca505.c
@@ -33,34 +33,11 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	u8 brightness;
-
 	u8 subtype;
 #define IntelPCCameraPro 0
 #define Nxultra 1
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define BRIGHTNESS_DEF 127
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA505, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -633,7 +610,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->nmodes = ARRAY_SIZE(vga_mode);
 	else			/* no 640x480 for IntelPCCameraPro */
 		cam->nmodes = ARRAY_SIZE(vga_mode) - 1;
-	sd->brightness = BRIGHTNESS_DEF;
 
 	return 0;
 }
@@ -651,11 +627,8 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return 0;
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 brightness = sd->brightness;
-
 	reg_write(gspca_dev->dev, 0x05, 0x00, (255 - brightness) >> 6);
 	reg_write(gspca_dev->dev, 0x05, 0x01, (255 - brightness) << 2);
 }
@@ -706,13 +679,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	reg_write(dev, SPCA50X_REG_COMPRESS, 0x06, mode_tb[mode][1]);
 	reg_write(dev, SPCA50X_REG_COMPRESS, 0x07, mode_tb[mode][2]);
 
-	ret = reg_write(dev, SPCA50X_REG_USB,
+	return reg_write(dev, SPCA50X_REG_USB,
 			 SPCA50X_USB_CTRL,
 			 SPCA50X_CUSB_ENABLE);
-
-	setbrightness(gspca_dev);
-
-	return ret;
 }
 
 static void sd_stopN(struct gspca_dev *gspca_dev)
@@ -756,30 +725,49 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->brightness = val;
-	if (gspca_dev->streaming)
-		setbrightness(gspca_dev);
-	return 0;
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 
-	*val = sd->brightness;
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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_controls = sd_init_controls,
 	.init = sd_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
@@ -812,6 +800,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca506.c b/drivers/media/video/gspca/spca506.c
index 54eed87672d..969bb5a4cd9 100644
--- a/drivers/media/video/gspca/spca506.c
+++ b/drivers/media/video/gspca/spca506.c
@@ -33,83 +33,10 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	unsigned char brightness;
-	unsigned char contrast;
-	unsigned char colors;
-	unsigned char hue;
 	char norme;
 	char channel;
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_sethue(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_gethue(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-#define SD_BRIGHTNESS 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x80,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define SD_CONTRAST 1
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x47,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-#define SD_COLOR 2
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0x40,
-	    },
-	    .set = sd_setcolors,
-	    .get = sd_getcolors,
-	},
-#define SD_HUE 3
-	{
-	    {
-		.id      = V4L2_CID_HUE,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Hue",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-		.default_value = 0,
-	    },
-	    .set = sd_sethue,
-	    .get = sd_gethue,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA505, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -281,16 +208,11 @@ static void spca506_Setsize(struct gspca_dev *gspca_dev, __u16 code,
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 
 	cam = &gspca_dev->cam;
 	cam->cam_mode = vga_mode;
 	cam->nmodes = ARRAY_SIZE(vga_mode);
-	sd->brightness = sd_ctrls[SD_BRIGHTNESS].qctrl.default_value;
-	sd->contrast = sd_ctrls[SD_CONTRAST].qctrl.default_value;
-	sd->colors = sd_ctrls[SD_COLOR].qctrl.default_value;
-	sd->hue = sd_ctrls[SD_HUE].qctrl.default_value;
 	return 0;
 }
 
@@ -564,121 +486,93 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	spca506_Initi2c(gspca_dev);
-	spca506_WriteI2c(gspca_dev, sd->brightness, SAA7113_bright);
+	spca506_WriteI2c(gspca_dev, val, SAA7113_bright);
 	spca506_WriteI2c(gspca_dev, 0x01, 0x09);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	spca506_Initi2c(gspca_dev);
-	spca506_WriteI2c(gspca_dev, sd->contrast, SAA7113_contrast);
+	spca506_WriteI2c(gspca_dev, val, SAA7113_contrast);
 	spca506_WriteI2c(gspca_dev, 0x01, 0x09);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	spca506_Initi2c(gspca_dev);
-	spca506_WriteI2c(gspca_dev, sd->colors, SAA7113_saturation);
+	spca506_WriteI2c(gspca_dev, val, SAA7113_saturation);
 	spca506_WriteI2c(gspca_dev, 0x01, 0x09);
 }
 
-static void sethue(struct gspca_dev *gspca_dev)
+static void sethue(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	spca506_Initi2c(gspca_dev);
-	spca506_WriteI2c(gspca_dev, sd->hue, SAA7113_hue);
+	spca506_WriteI2c(gspca_dev, val, SAA7113_hue);
 	spca506_WriteI2c(gspca_dev, 0x01, 0x09);
 }
 
-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)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->contrast = val;
-	if (gspca_dev->streaming)
-		setcontrast(gspca_dev);
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	*val = sd->contrast;
-	return 0;
-}
-
-static int sd_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
-	return 0;
-}
-
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		sethue(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_sethue(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->hue = val;
-	if (gspca_dev->streaming)
-		sethue(gspca_dev);
-	return 0;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_gethue(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->hue;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 0x47);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 0x40);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, 0, 255, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -711,6 +605,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca508.c b/drivers/media/video/gspca/spca508.c
index df4e1699646..1286b4170b8 100644
--- a/drivers/media/video/gspca/spca508.c
+++ b/drivers/media/video/gspca/spca508.c
@@ -32,8 +32,6 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
 
-	u8 brightness;
-
 	u8 subtype;
 #define CreativeVista 0
 #define HamaUSBSightcam 1
@@ -43,27 +41,6 @@ struct sd {
 #define ViewQuestVQ110 5
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define BRIGHTNESS_DEF 128
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-};
-
 static const struct v4l2_pix_format sif_mode[] = {
 	{160, 120, V4L2_PIX_FMT_SPCA508, V4L2_FIELD_NONE,
 		.bytesperline = 160,
@@ -1411,7 +1388,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	cam->nmodes = ARRAY_SIZE(sif_mode);
 
 	sd->subtype = id->driver_info;
-	sd->brightness = BRIGHTNESS_DEF;
 
 	init_data = init_data_tb[sd->subtype];
 	return write_vector(gspca_dev, init_data);
@@ -1471,11 +1447,8 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u8 brightness = sd->brightness;
-
 	/* MX seem contrast */
 	reg_write(gspca_dev->dev, 0x8651, brightness);
 	reg_write(gspca_dev->dev, 0x8652, brightness);
@@ -1483,31 +1456,50 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	reg_write(gspca_dev->dev, 0x8654, brightness);
 }
 
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->brightness = val;
-	if (gspca_dev->streaming)
-		setbrightness(gspca_dev);
-	return 0;
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_getbrightness(struct gspca_dev *gspca_dev, __s32 *val)
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 
-	*val = sd->brightness;
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -1541,6 +1533,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/spca561.c b/drivers/media/video/gspca/spca561.c
index 4a5f209ce71..cfe71dd6747 100644
--- a/drivers/media/video/gspca/spca561.c
+++ b/drivers/media/video/gspca/spca561.c
@@ -31,39 +31,17 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA/SPCA561 USB Camera Driver");
 MODULE_LICENSE("GPL");
 
+#define EXPOSURE_MAX (2047 + 325)
+
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	__u16 exposure;			/* rev12a only */
-#define EXPOSURE_MIN 1
-#define EXPOSURE_DEF 700		/* == 10 fps */
-#define EXPOSURE_MAX (2047 + 325)	/* see setexposure */
-
-	__u8 contrast;			/* rev72a only */
-#define CONTRAST_MIN 0x00
-#define CONTRAST_DEF 0x20
-#define CONTRAST_MAX 0x3f
-
-	__u8 brightness;		/* rev72a only */
-#define BRIGHTNESS_MIN 0
-#define BRIGHTNESS_DEF 0x20
-#define BRIGHTNESS_MAX 0x3f
-
-	__u8 white;
-#define HUE_MIN 1
-#define HUE_DEF 0x40
-#define HUE_MAX 0x7f
-
-	__u8 autogain;
-#define AUTOGAIN_MIN 0
-#define AUTOGAIN_DEF 1
-#define AUTOGAIN_MAX 1
-
-	__u8 gain;			/* rev12a only */
-#define GAIN_MIN 0
-#define GAIN_DEF 63
-#define GAIN_MAX 255
+	struct { /* hue/contrast control cluster */
+		struct v4l2_ctrl *contrast;
+		struct v4l2_ctrl *hue;
+	};
+	struct v4l2_ctrl *autogain;
 
 #define EXPO12A_DEF 3
 	__u8 expo12a;		/* expo/gain? for rev 12a */
@@ -461,12 +439,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->cam_mode = sif_072a_mode;
 		cam->nmodes = ARRAY_SIZE(sif_072a_mode);
 	}
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->white = HUE_DEF;
-	sd->exposure = EXPOSURE_DEF;
-	sd->autogain = AUTOGAIN_DEF;
-	sd->gain = GAIN_DEF;
 	sd->expo12a = EXPO12A_DEF;
 	return 0;
 }
@@ -491,66 +463,49 @@ static int sd_init_72a(struct gspca_dev *gspca_dev)
 	return 0;
 }
 
-/* rev 72a only */
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	struct usb_device *dev = gspca_dev->dev;
-	__u8 value;
+	__u16 reg;
 
-	value = sd->brightness;
+	if (sd->chip_revision == Rev012A)
+		reg = 0x8610;
+	else
+		reg = 0x8611;
 
-	/* offsets for white balance */
-	reg_w_val(dev, 0x8611, value);		/* R */
-	reg_w_val(dev, 0x8612, value);		/* Gr */
-	reg_w_val(dev, 0x8613, value);		/* B */
-	reg_w_val(dev, 0x8614, value);		/* Gb */
+	reg_w_val(dev, reg + 0, val);		/* R */
+	reg_w_val(dev, reg + 1, val);		/* Gr */
+	reg_w_val(dev, reg + 2, val);		/* B */
+	reg_w_val(dev, reg + 3, val);		/* Gb */
 }
 
-static void setwhite(struct gspca_dev *gspca_dev)
+static void setwhite(struct gspca_dev *gspca_dev, s32 white, s32 contrast)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	__u16 white;
+	struct usb_device *dev = gspca_dev->dev;
 	__u8 blue, red;
 	__u16 reg;
 
 	/* try to emulate MS-win as possible */
-	white = sd->white;
 	red = 0x20 + white * 3 / 8;
 	blue = 0x90 - white * 5 / 8;
 	if (sd->chip_revision == Rev012A) {
 		reg = 0x8614;
 	} else {
 		reg = 0x8651;
-		red += sd->contrast - 0x20;
-		blue += sd->contrast - 0x20;
+		red += contrast - 0x20;
+		blue += contrast - 0x20;
+		reg_w_val(dev, 0x8652, contrast + 0x20); /* Gr */
+		reg_w_val(dev, 0x8654, contrast + 0x20); /* Gb */
 	}
-	reg_w_val(gspca_dev->dev, reg, red);
-	reg_w_val(gspca_dev->dev, reg + 2, blue);
-}
-
-static void setcontrast(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct usb_device *dev = gspca_dev->dev;
-	__u8 value;
-
-	if (sd->chip_revision != Rev072A)
-		return;
-	value = sd->contrast + 0x20;
-
-	/* gains for white balance */
-	setwhite(gspca_dev);
-/*	reg_w_val(dev, 0x8651, value);		 * R - done by setwhite */
-	reg_w_val(dev, 0x8652, value);		/* Gr */
-/*	reg_w_val(dev, 0x8653, value);		 * B - done by setwhite */
-	reg_w_val(dev, 0x8654, value);		/* Gb */
+	reg_w_val(dev, reg, red);
+	reg_w_val(dev, reg + 2, blue);
 }
 
 /* rev 12a only */
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	int i, expo = 0;
 
 	/* Register 0x8309 controls exposure for the spca561,
@@ -572,8 +527,8 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	int table[] =  { 0, 450, 550, 625, EXPOSURE_MAX };
 
 	for (i = 0; i < ARRAY_SIZE(table) - 1; i++) {
-		if (sd->exposure <= table[i + 1]) {
-			expo  = sd->exposure - table[i];
+		if (val <= table[i + 1]) {
+			expo  = val - table[i];
 			if (i)
 				expo += 300;
 			expo |= i << 11;
@@ -587,29 +542,27 @@ static void setexposure(struct gspca_dev *gspca_dev)
 }
 
 /* rev 12a only */
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	/* gain reg low 6 bits  0-63 gain, bit 6 and 7, both double the
 	   sensitivity when set, so 31 + one of them set == 63, and 15
 	   with both of them set == 63 */
-	if (sd->gain < 64)
-		gspca_dev->usb_buf[0] = sd->gain;
-	else if (sd->gain < 128)
-		gspca_dev->usb_buf[0] = (sd->gain / 2) | 0x40;
+	if (val < 64)
+		gspca_dev->usb_buf[0] = val;
+	else if (val < 128)
+		gspca_dev->usb_buf[0] = (val / 2) | 0x40;
 	else
-		gspca_dev->usb_buf[0] = (sd->gain / 4) | 0xc0;
+		gspca_dev->usb_buf[0] = (val / 4) | 0xc0;
 
 	gspca_dev->usb_buf[1] = 0;
 	reg_w_buf(gspca_dev, 0x8335, 2);
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->autogain)
+	if (val)
 		sd->ag_cnt = AG_CNT_START;
 	else
 		sd->ag_cnt = -1;
@@ -644,9 +597,6 @@ static int sd_start_12a(struct gspca_dev *gspca_dev)
 	memcpy(gspca_dev->usb_buf, Reg8391, 8);
 	reg_w_buf(gspca_dev, 0x8391, 8);
 	reg_w_buf(gspca_dev, 0x8390, 8);
-	setwhite(gspca_dev);
-	setgain(gspca_dev);
-	setexposure(gspca_dev);
 
 	/* Led ON (bit 3 -> 0 */
 	reg_w_val(gspca_dev->dev, 0x8114, 0x00);
@@ -654,6 +604,7 @@ static int sd_start_12a(struct gspca_dev *gspca_dev)
 }
 static int sd_start_72a(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	struct usb_device *dev = gspca_dev->dev;
 	int Clck;
 	int mode;
@@ -683,9 +634,10 @@ static int sd_start_72a(struct gspca_dev *gspca_dev)
 	reg_w_val(dev, 0x8702, 0x81);
 	reg_w_val(dev, 0x8500, mode);	/* mode */
 	write_sensor_72a(gspca_dev, rev72a_init_sensor2);
-	setcontrast(gspca_dev);
+	setwhite(gspca_dev, v4l2_ctrl_g_ctrl(sd->hue),
+			v4l2_ctrl_g_ctrl(sd->contrast));
 /*	setbrightness(gspca_dev);	 * fixme: bad values */
-	setautogain(gspca_dev);
+	setautogain(gspca_dev, v4l2_ctrl_g_ctrl(sd->autogain));
 	reg_w_val(dev, 0x8112, 0x10 | 0x20);
 	return 0;
 }
@@ -819,221 +771,96 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-/* rev 72a only */
-static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	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;
-}
-
-/* rev 72a only */
-static int sd_setcontrast(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	gspca_dev->usb_err = 0;
 
-	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_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	sd->autogain = val;
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
-	return 0;
-}
-
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
-static int sd_setwhite(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->white = val;
-	if (gspca_dev->streaming)
-		setwhite(gspca_dev);
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		/* hue/contrast control cluster for 72a */
+		setwhite(gspca_dev, sd->hue->val, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		/* just plain hue control for 12a */
+		setwhite(gspca_dev, ctrl->val, 0);
+		break;
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		setgain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_getwhite(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->white;
-	return 0;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-/* rev12a only */
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_init_controls_12a(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return 0;
-}
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, 1, 0x7f, 1, 0x40);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 1, EXPOSURE_MAX, 1, 700);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 63);
 
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->exposure;
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
-/* rev12a only */
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_init_controls_72a(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		setgain(gspca_dev);
-	return 0;
-}
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	sd->contrast = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 0x3f, 1, 0x20);
+	sd->hue = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, 1, 0x7f, 1, 0x40);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 0x3f, 1, 0x20);
+	sd->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
 
-	*val = sd->gain;
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(2, &sd->contrast);
 	return 0;
 }
 
-/* control tables */
-static const struct ctrl sd_ctrls_12a[] = {
-	{
-	    {
-		.id = V4L2_CID_HUE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Hue",
-		.minimum = HUE_MIN,
-		.maximum = HUE_MAX,
-		.step = 1,
-		.default_value = HUE_DEF,
-	    },
-	    .set = sd_setwhite,
-	    .get = sd_getwhite,
-	},
-	{
-	    {
-		.id = V4L2_CID_EXPOSURE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Exposure",
-		.minimum = EXPOSURE_MIN,
-		.maximum = EXPOSURE_MAX,
-		.step = 1,
-		.default_value = EXPOSURE_DEF,
-	    },
-	    .set = sd_setexposure,
-	    .get = sd_getexposure,
-	},
-	{
-	    {
-		.id = V4L2_CID_GAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Gain",
-		.minimum = GAIN_MIN,
-		.maximum = GAIN_MAX,
-		.step = 1,
-		.default_value = GAIN_DEF,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-};
-
-static const struct ctrl sd_ctrls_72a[] = {
-	{
-	    {
-		.id = V4L2_CID_HUE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Hue",
-		.minimum = HUE_MIN,
-		.maximum = HUE_MAX,
-		.step = 1,
-		.default_value = HUE_DEF,
-	    },
-	    .set = sd_setwhite,
-	    .get = sd_getwhite,
-	},
-	{
-	   {
-		.id = V4L2_CID_BRIGHTNESS,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Brightness",
-		.minimum = BRIGHTNESS_MIN,
-		.maximum = BRIGHTNESS_MAX,
-		.step = 1,
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-	{
-	    {
-		.id = V4L2_CID_CONTRAST,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Contrast",
-		.minimum = CONTRAST_MIN,
-		.maximum = CONTRAST_MAX,
-		.step = 1,
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-	{
-	    {
-		.id = V4L2_CID_AUTOGAIN,
-		.type = V4L2_CTRL_TYPE_BOOLEAN,
-		.name = "Auto Gain",
-		.minimum = AUTOGAIN_MIN,
-		.maximum = AUTOGAIN_MAX,
-		.step = 1,
-		.default_value = AUTOGAIN_DEF,
-	    },
-	    .set = sd_setautogain,
-	    .get = sd_getautogain,
-	},
-};
-
 /* sub-driver description */
 static const struct sd_desc sd_desc_12a = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls_12a,
-	.nctrls = ARRAY_SIZE(sd_ctrls_12a),
+	.init_controls = sd_init_controls_12a,
 	.config = sd_config,
 	.init = sd_init_12a,
 	.start = sd_start_12a,
@@ -1045,8 +872,7 @@ static const struct sd_desc sd_desc_12a = {
 };
 static const struct sd_desc sd_desc_72a = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls_72a,
-	.nctrls = ARRAY_SIZE(sd_ctrls_72a),
+	.init_controls = sd_init_controls_72a,
 	.config = sd_config,
 	.init = sd_init_72a,
 	.start = sd_start_72a,
@@ -1103,6 +929,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/sq905.c b/drivers/media/video/gspca/sq905.c
index 04f54654a02..a8ac97931ad 100644
--- a/drivers/media/video/gspca/sq905.c
+++ b/drivers/media/video/gspca/sq905.c
@@ -433,6 +433,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume  = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/sq905c.c b/drivers/media/video/gspca/sq905c.c
index f34ddb0570c..2c2f3d2f357 100644
--- a/drivers/media/video/gspca/sq905c.c
+++ b/drivers/media/video/gspca/sq905c.c
@@ -340,6 +340,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume  = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/sq930x.c b/drivers/media/video/gspca/sq930x.c
index 1a8ba9b3550..3e1e486af88 100644
--- a/drivers/media/video/gspca/sq930x.c
+++ b/drivers/media/video/gspca/sq930x.c
@@ -36,8 +36,10 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	u16 expo;
-	u8 gain;
+	struct { /* exposure/gain control cluster */
+		struct v4l2_ctrl *exposure;
+		struct v4l2_ctrl *gain;
+	};
 
 	u8 do_ctrl;
 	u8 gpio[2];
@@ -55,42 +57,6 @@ enum sensors {
 	SENSOR_OV9630,
 };
 
-static int sd_setexpo(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexpo(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id = V4L2_CID_EXPOSURE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Exposure",
-		.minimum = 0x0001,
-		.maximum = 0x0fff,
-		.step = 1,
-#define EXPO_DEF 0x0356
-		.default_value = EXPO_DEF,
-	    },
-	    .set = sd_setexpo,
-	    .get = sd_getexpo,
-	},
-	{
-	    {
-		.id = V4L2_CID_GAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Gain",
-		.minimum = 0x01,
-		.maximum = 0xff,
-		.step = 1,
-#define GAIN_DEF 0x8d
-		.default_value = GAIN_DEF,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-};
-
 static struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_SRGGB8, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -791,7 +757,7 @@ static void lz24bp_ppl(struct sd *sd, u16 ppl)
 	ucbus_write(&sd->gspca_dev, cmds, ARRAY_SIZE(cmds), 2);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 expo, s32 gain)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int i, integclks, intstartclk, frameclks, min_frclk;
@@ -799,7 +765,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 	u16 cmd;
 	u8 buf[15];
 
-	integclks = sd->expo;
+	integclks = expo;
 	i = 0;
 	cmd = SQ930_CTRL_SET_EXPOSURE;
 
@@ -818,7 +784,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		buf[i++] = intstartclk;
 		buf[i++] = frameclks >> 8;
 		buf[i++] = frameclks;
-		buf[i++] = sd->gain;
+		buf[i++] = gain;
 		break;
 	default:				/* cmos */
 /*	case SENSOR_MI0360: */
@@ -834,7 +800,7 @@ static void setexposure(struct gspca_dev *gspca_dev)
 		buf[i++] = 0x35;	/* reg = global gain */
 		buf[i++] = 0x00;	/* val H */
 		buf[i++] = sensor->i2c_dum;
-		buf[i++] = 0x80 + sd->gain / 2; /* val L */
+		buf[i++] = 0x80 + gain / 2; /* val L */
 		buf[i++] = 0x00;
 		buf[i++] = 0x00;
 		buf[i++] = 0x00;
@@ -860,9 +826,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
 	cam->bulk = 1;
 
-	sd->gain = GAIN_DEF;
-	sd->expo = EXPO_DEF;
-
 	return 0;
 }
 
@@ -1089,7 +1052,8 @@ static void sd_dq_callback(struct gspca_dev *gspca_dev)
 		return;
 	sd->do_ctrl = 0;
 
-	setexposure(gspca_dev);
+	setexposure(gspca_dev, v4l2_ctrl_g_ctrl(sd->exposure),
+			v4l2_ctrl_g_ctrl(sd->gain));
 
 	gspca_dev->cam.bulk_nurbs = 1;
 	ret = usb_submit_urb(gspca_dev->urb[0], GFP_ATOMIC);
@@ -1113,48 +1077,55 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, LAST_PACKET, NULL, 0);
 }
 
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	sd->gain = val;
-	if (gspca_dev->streaming)
-		sd->do_ctrl = 1;
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	*val = sd->gain;
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, ctrl->val, sd->gain->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
-static int sd_setexpo(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
 
-	sd->expo = val;
-	if (gspca_dev->streaming)
-		sd->do_ctrl = 1;
-	return 0;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_getexpo(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	*val = sd->expo;
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 2);
+	sd->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 1, 0xfff, 1, 0x356);
+	sd->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 1, 255, 1, 0x8d);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	v4l2_ctrl_cluster(2, &sd->exposure);
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start  = sd_start,
 	.stopN  = sd_stopN,
@@ -1194,6 +1165,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend    = gspca_suspend,
 	.resume     = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/stk014.c b/drivers/media/video/gspca/stk014.c
index 4ae7cc8f463..8c0982607f2 100644
--- a/drivers/media/video/gspca/stk014.c
+++ b/drivers/media/video/gspca/stk014.c
@@ -29,86 +29,14 @@ MODULE_AUTHOR("Jean-Francois Moine <http://moinejf.free.fr>");
 MODULE_DESCRIPTION("Syntek DV4000 (STK014) USB Camera Driver");
 MODULE_LICENSE("GPL");
 
-/* controls */
-enum e_ctrl {
-	BRIGHTNESS,
-	CONTRAST,
-	COLORS,
-	LIGHTFREQ,
-	NCTRLS		/* number of controls */
-};
+#define QUALITY 50
 
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-
-	struct gspca_ctrl ctrls[NCTRLS];
-
-	u8 quality;
-#define QUALITY_MIN 70
-#define QUALITY_MAX 95
-#define QUALITY_DEF 88
-
 	u8 jpeg_hdr[JPEG_HDR_SZ];
 };
 
-/* V4L2 controls supported by the driver */
-static void setbrightness(struct gspca_dev *gspca_dev);
-static void setcontrast(struct gspca_dev *gspca_dev);
-static void setcolors(struct gspca_dev *gspca_dev);
-static void setlightfreq(struct gspca_dev *gspca_dev);
-
-static const struct ctrl sd_ctrls[NCTRLS] = {
-[BRIGHTNESS] = {
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setbrightness
-	},
-[CONTRAST] = {
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setcontrast
-	},
-[COLORS] = {
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-		.default_value = 127,
-	    },
-	    .set_control = setcolors
-	},
-[LIGHTFREQ] = {
-	    {
-		.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = 1,
-		.maximum = 2,	/* 0: 0, 1: 50Hz, 2:60Hz */
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setlightfreq
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -255,41 +183,36 @@ static void set_par(struct gspca_dev *gspca_dev,
 	snd_val(gspca_dev, 0x003f08, parval);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	int parval;
 
 	parval = 0x06000000		/* whiteness */
-		+ (sd->ctrls[BRIGHTNESS].val << 16);
+		+ (val << 16);
 	set_par(gspca_dev, parval);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	int parval;
 
 	parval = 0x07000000		/* contrast */
-		+ (sd->ctrls[CONTRAST].val << 16);
+		+ (val << 16);
 	set_par(gspca_dev, parval);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	int parval;
 
 	parval = 0x08000000		/* saturation */
-		+ (sd->ctrls[COLORS].val << 16);
+		+ (val << 16);
 	set_par(gspca_dev, parval);
 }
 
-static void setlightfreq(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	set_par(gspca_dev, sd->ctrls[LIGHTFREQ].val == 1
+	set_par(gspca_dev, val == 1
 			? 0x33640000		/* 50 Hz */
 			: 0x33780000);		/* 60 Hz */
 }
@@ -298,12 +221,8 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 			const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	gspca_dev->cam.cam_mode = vga_mode;
 	gspca_dev->cam.nmodes = ARRAY_SIZE(vga_mode);
-	gspca_dev->cam.ctrls = sd->ctrls;
-	sd->quality = QUALITY_DEF;
 	return 0;
 }
 
@@ -333,7 +252,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x22);		/* JPEG 411 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	jpeg_set_qual(sd->jpeg_hdr, QUALITY);
 
 	/* work on alternate 1 */
 	usb_set_interface(gspca_dev->dev, gspca_dev->iface, 1);
@@ -365,14 +284,10 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x0640, 0);
 	reg_w(gspca_dev, 0x0650, 0);
 	reg_w(gspca_dev, 0x0660, 0);
-	setbrightness(gspca_dev);		/* whiteness */
-	setcontrast(gspca_dev);			/* contrast */
-	setcolors(gspca_dev);			/* saturation */
 	set_par(gspca_dev, 0x09800000);		/* Red ? */
 	set_par(gspca_dev, 0x0a800000);		/* Green ? */
 	set_par(gspca_dev, 0x0b800000);		/* Blue ? */
 	set_par(gspca_dev, 0x0d030000);		/* Gamma ? */
-	setlightfreq(gspca_dev);
 
 	/* start the video flow */
 	set_par(gspca_dev, 0x01000000);
@@ -435,62 +350,70 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	static const char *freq_nm[3] = {"NoFliker", "50 Hz", "60 Hz"};
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		if ((unsigned) menu->index >= ARRAY_SIZE(freq_nm))
-			break;
-		strcpy((char *) menu->name, freq_nm[menu->index]);
-		return 0;
-	}
-	return -EINVAL;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_set_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 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);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setlightfreq(gspca_dev, ctrl->val);
+		break;
+	}
 	return gspca_dev->usb_err;
 }
 
-static int sd_get_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
-	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
-			| V4L2_JPEG_MARKER_DQT;
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 127);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 127);
+	v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 1,
+			V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = NCTRLS,
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
-	.get_jcomp = sd_get_jcomp,
-	.set_jcomp = sd_set_jcomp,
 };
 
 /* -- module initialisation -- */
@@ -516,6 +439,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/stv0680.c b/drivers/media/video/gspca/stv0680.c
index 461ed645f30..67605272aaa 100644
--- a/drivers/media/video/gspca/stv0680.c
+++ b/drivers/media/video/gspca/stv0680.c
@@ -46,10 +46,6 @@ struct sd {
 	u8 current_mode;
 };
 
-/* V4L2 controls supported by the driver */
-static const struct ctrl sd_ctrls[] = {
-};
-
 static int stv_sndctrl(struct gspca_dev *gspca_dev, int set, u8 req, u16 val,
 		       int size)
 {
@@ -318,8 +314,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 /* 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,
@@ -352,6 +346,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/sunplus.c b/drivers/media/video/gspca/sunplus.c
index c80f0c0c75b..9ccfcb1c647 100644
--- a/drivers/media/video/gspca/sunplus.c
+++ b/drivers/media/video/gspca/sunplus.c
@@ -30,18 +30,13 @@ MODULE_AUTHOR("Michel Xhaard <mxhaard@users.sourceforge.net>");
 MODULE_DESCRIPTION("GSPCA/SPCA5xx USB Camera Driver");
 MODULE_LICENSE("GPL");
 
+#define QUALITY 85
+
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	s8 brightness;
-	u8 contrast;
-	u8 colors;
-	u8 autogain;
-	u8 quality;
-#define QUALITY_MIN 70
-#define QUALITY_MAX 95
-#define QUALITY_DEF 85
+	bool autogain;
 
 	u8 bridge;
 #define BRIDGE_SPCA504 0
@@ -59,75 +54,6 @@ struct sd {
 	u8 jpeg_hdr[JPEG_HDR_SZ];
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = -128,
-		.maximum = 127,
-		.step    = 1,
-#define BRIGHTNESS_DEF 0
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-#define CONTRAST_DEF 0x20
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Color",
-		.minimum = 0,
-		.maximum = 0xff,
-		.step    = 1,
-#define COLOR_DEF 0x1a
-		.default_value = COLOR_DEF,
-	    },
-	    .set = sd_setcolors,
-	    .get = sd_getcolors,
-	},
-	{
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define AUTOGAIN_DEF 1
-		.default_value = AUTOGAIN_DEF,
-	    },
-	    .set = sd_setautogain,
-	    .get = sd_getautogain,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode[] = {
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
@@ -597,31 +523,31 @@ static void spca504B_setQtable(struct gspca_dev *gspca_dev)
 	spca504B_PollingDataReady(gspca_dev);
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u16 reg;
 
 	reg = sd->bridge == BRIDGE_SPCA536 ? 0x20f0 : 0x21a7;
-	reg_w_riv(gspca_dev, 0x00, reg, sd->brightness);
+	reg_w_riv(gspca_dev, 0x00, reg, val);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u16 reg;
 
 	reg = sd->bridge == BRIDGE_SPCA536 ? 0x20f1 : 0x21a8;
-	reg_w_riv(gspca_dev, 0x00, reg, sd->contrast);
+	reg_w_riv(gspca_dev, 0x00, reg, val);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u16 reg;
 
 	reg = sd->bridge == BRIDGE_SPCA536 ? 0x20f6 : 0x21ae;
-	reg_w_riv(gspca_dev, 0x00, reg, sd->colors);
+	reg_w_riv(gspca_dev, 0x00, reg, val);
 }
 
 static void init_ctl_reg(struct gspca_dev *gspca_dev)
@@ -629,10 +555,6 @@ static void init_ctl_reg(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	int pollreg = 1;
 
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setcolors(gspca_dev);
-
 	switch (sd->bridge) {
 	case BRIDGE_SPCA504:
 	case BRIDGE_SPCA504C:
@@ -704,11 +626,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 		cam->nmodes = ARRAY_SIZE(vga_mode2);
 		break;
 	}
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->colors = COLOR_DEF;
-	sd->autogain = AUTOGAIN_DEF;
-	sd->quality = QUALITY_DEF;
 	return 0;
 }
 
@@ -807,7 +724,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	/* create the JPEG header */
 	jpeg_define(sd->jpeg_hdr, gspca_dev->height, gspca_dev->width,
 			0x22);		/* JPEG 411 */
-	jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+	jpeg_set_qual(sd->jpeg_hdr, QUALITY);
 
 	if (sd->bridge == BRIDGE_SPCA504B)
 		spca504B_setQtable(gspca_dev);
@@ -1012,116 +929,69 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-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 gspca_dev->usb_err;
-}
-
-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)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
 
-	sd->contrast = val;
-	if (gspca_dev->streaming)
-		setcontrast(gspca_dev);
-	return gspca_dev->usb_err;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_getcontrast(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	*val = sd->contrast;
-	return 0;
-}
-
-static int sd_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		sd->autogain = ctrl->val;
+		break;
+	}
 	return gspca_dev->usb_err;
 }
 
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
-}
-
-static int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	return 0;
-}
-
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
-static int sd_set_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	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 gspca_dev->usb_err;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_get_jcomp(struct gspca_dev *gspca_dev,
-			struct v4l2_jpegcompression *jcomp)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = sd->quality;
-	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
-			| V4L2_JPEG_MARKER_DQT;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 0x20);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 0x1a);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.get_jcomp = sd_get_jcomp,
-	.set_jcomp = sd_set_jcomp,
 };
 
 /* -- module initialisation -- */
@@ -1208,6 +1078,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/t613.c b/drivers/media/video/gspca/t613.c
index 9b9f85a8e60..8bc6c3ceec2 100644
--- a/drivers/media/video/gspca/t613.c
+++ b/drivers/media/video/gspca/t613.c
@@ -34,28 +34,19 @@
 #include <linux/slab.h>
 #include "gspca.h"
 
-#define V4L2_CID_EFFECTS (V4L2_CID_PRIVATE_BASE + 0)
-
 MODULE_AUTHOR("Leandro Costantino <le_costantino@pixartargentina.com.ar>");
 MODULE_DESCRIPTION("GSPCA/T613 (JPEG Compliance) USB Camera Driver");
 MODULE_LICENSE("GPL");
 
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-
-	u8 brightness;
-	u8 contrast;
-	u8 colors;
-	u8 autogain;
-	u8 gamma;
-	u8 sharpness;
-	u8 freq;
-	u8 red_gain;
-	u8 blue_gain;
-	u8 green_gain;
-	u8 awb; /* set default r/g/b and activate */
-	u8 mirror;
-	u8 effect;
+	struct v4l2_ctrl *freq;
+	struct { /* awb / color gains control cluster */
+		struct v4l2_ctrl *awb;
+		struct v4l2_ctrl *gain;
+		struct v4l2_ctrl *red_balance;
+		struct v4l2_ctrl *blue_balance;
+	};
 
 	u8 sensor;
 	u8 button_pressed;
@@ -67,245 +58,31 @@ enum sensors {
 	SENSOR_LT168G,		/* must verify if this is the actual model */
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setlowlight(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getlowlight(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgamma(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgamma(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_setfreq(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getfreq(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_setblue_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getblue_gain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setred_gain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getred_gain(struct gspca_dev *gspca_dev, __s32 *val);
-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_setmirror(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getmirror(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_seteffect(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_geteffect(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	 {
-	  .id = V4L2_CID_BRIGHTNESS,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Brightness",
-	  .minimum = 0,
-	  .maximum = 14,
-	  .step = 1,
-#define BRIGHTNESS_DEF 8
-	  .default_value = BRIGHTNESS_DEF,
-	  },
-	 .set = sd_setbrightness,
-	 .get = sd_getbrightness,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_CONTRAST,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Contrast",
-	  .minimum = 0,
-	  .maximum = 0x0d,
-	  .step = 1,
-#define CONTRAST_DEF 0x07
-	  .default_value = CONTRAST_DEF,
-	  },
-	 .set = sd_setcontrast,
-	 .get = sd_getcontrast,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_SATURATION,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Color",
-	  .minimum = 0,
-	  .maximum = 0x0f,
-	  .step = 1,
-#define COLORS_DEF 0x05
-	  .default_value = COLORS_DEF,
-	  },
-	 .set = sd_setcolors,
-	 .get = sd_getcolors,
-	 },
-#define GAMMA_MAX 16
-#define GAMMA_DEF 10
-	{
-	 {
-	  .id = V4L2_CID_GAMMA,	/* (gamma on win) */
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Gamma",
-	  .minimum = 0,
-	  .maximum = GAMMA_MAX - 1,
-	  .step = 1,
-	  .default_value = GAMMA_DEF,
-	  },
-	 .set = sd_setgamma,
-	 .get = sd_getgamma,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_BACKLIGHT_COMPENSATION, /* Activa lowlight,
-				 * some apps dont bring up the
-				 * backligth_compensation control) */
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Low Light",
-	  .minimum = 0,
-	  .maximum = 1,
-	  .step = 1,
-#define AUTOGAIN_DEF 0x01
-	  .default_value = AUTOGAIN_DEF,
-	  },
-	 .set = sd_setlowlight,
-	 .get = sd_getlowlight,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_HFLIP,
-	  .type = V4L2_CTRL_TYPE_BOOLEAN,
-	  .name = "Mirror Image",
-	  .minimum = 0,
-	  .maximum = 1,
-	  .step = 1,
-#define MIRROR_DEF 0
-	  .default_value = MIRROR_DEF,
-	  },
-	 .set = sd_setmirror,
-	 .get = sd_getmirror
-	},
-	{
-	 {
-	  .id = V4L2_CID_POWER_LINE_FREQUENCY,
-	  .type = V4L2_CTRL_TYPE_MENU,
-	  .name = "Light Frequency Filter",
-	  .minimum = 1,		/* 1 -> 0x50, 2->0x60 */
-	  .maximum = 2,
-	  .step = 1,
-#define FREQ_DEF 1
-	  .default_value = FREQ_DEF,
-	  },
-	 .set = sd_setfreq,
-	 .get = sd_getfreq},
-
-	{
-	 {
-	  .id =  V4L2_CID_AUTO_WHITE_BALANCE,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Auto White Balance",
-	  .minimum = 0,
-	  .maximum = 1,
-	  .step = 1,
-#define AWB_DEF 0
-	  .default_value = AWB_DEF,
-	  },
-	 .set = sd_setawb,
-	 .get = sd_getawb
-	},
-	{
-	 {
-	  .id = V4L2_CID_SHARPNESS,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Sharpness",
-	  .minimum = 0,
-	  .maximum = 15,
-	  .step = 1,
-#define SHARPNESS_DEF 0x06
-	  .default_value = SHARPNESS_DEF,
-	  },
-	 .set = sd_setsharpness,
-	 .get = sd_getsharpness,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_EFFECTS,
-	  .type = V4L2_CTRL_TYPE_MENU,
-	  .name = "Webcam Effects",
-	  .minimum = 0,
-	  .maximum = 4,
-	  .step = 1,
-#define EFFECTS_DEF 0
-	  .default_value = EFFECTS_DEF,
-	  },
-	 .set = sd_seteffect,
-	 .get = sd_geteffect
-	},
-	{
-	 {
-	    .id      = V4L2_CID_BLUE_BALANCE,
-	    .type    = V4L2_CTRL_TYPE_INTEGER,
-	    .name    = "Blue Balance",
-	    .minimum = 0x10,
-	    .maximum = 0x40,
-	    .step    = 1,
-#define BLUE_GAIN_DEF 0x20
-	    .default_value = BLUE_GAIN_DEF,
-	 },
-	.set = sd_setblue_gain,
-	.get = sd_getblue_gain,
-	},
-	{
-	 {
-	    .id      = V4L2_CID_RED_BALANCE,
-	    .type    = V4L2_CTRL_TYPE_INTEGER,
-	    .name    = "Red Balance",
-	    .minimum = 0x10,
-	    .maximum = 0x40,
-	    .step    = 1,
-#define RED_GAIN_DEF 0x20
-	    .default_value = RED_GAIN_DEF,
-	 },
-	.set = sd_setred_gain,
-	.get = sd_getred_gain,
-	},
-	{
-	 {
-	    .id      = V4L2_CID_GAIN,
-	    .type    = V4L2_CTRL_TYPE_INTEGER,
-	    .name    = "Gain",
-	    .minimum = 0x10,
-	    .maximum = 0x40,
-	    .step    = 1,
-#define GAIN_DEF  0x20
-	    .default_value = GAIN_DEF,
-	 },
-	.set = sd_setgain,
-	.get = sd_getgain,
-	},
-};
-
 static const struct v4l2_pix_format vga_mode_t16[] = {
 	{160, 120, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 160,
 		.sizeimage = 160 * 120 * 4 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = 4},
+#if 0 /* HDG: broken with my test cam, so lets disable it */
 	{176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 176,
 		.sizeimage = 176 * 144 * 3 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = 3},
+#endif
 	{320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 320,
 		.sizeimage = 320 * 240 * 3 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = 2},
+#if 0 /* HDG: broken with my test cam, so lets disable it */
 	{352, 288, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 352,
 		.sizeimage = 352 * 288 * 3 / 8 + 590,
 		.colorspace = V4L2_COLORSPACE_JPEG,
 		.priv = 1},
+#endif
 	{640, 480, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
 		.bytesperline = 640,
 		.sizeimage = 640 * 480 * 3 / 8 + 590,
@@ -454,17 +231,6 @@ static const struct additional_sensor_data sensor_data[] = {
 };
 
 #define MAX_EFFECTS 7
-/* easily done by soft, this table could be removed,
- * i keep it here just in case */
-static char *effects_control[MAX_EFFECTS] = {
-	"Normal",
-	"Emboss",		/* disabled */
-	"Monochrome",
-	"Sepia",
-	"Sketch",
-	"Sun Effect",		/* disabled */
-	"Negative",
-};
 static const u8 effects_table[MAX_EFFECTS][6] = {
 	{0xa8, 0xe8, 0xc6, 0xd2, 0xc0, 0x00},	/* Normal */
 	{0xa8, 0xc8, 0xc6, 0x52, 0xc0, 0x04},	/* Repujar */
@@ -475,7 +241,8 @@ static const u8 effects_table[MAX_EFFECTS][6] = {
 	{0xa8, 0xc8, 0xc6, 0xd2, 0xc0, 0x40},	/* Negative */
 };
 
-static const u8 gamma_table[GAMMA_MAX][17] = {
+#define GAMMA_MAX (15)
+static const u8 gamma_table[GAMMA_MAX+1][17] = {
 /* gamma table from cam1690.ini */
 	{0x00, 0x00, 0x01, 0x04, 0x08, 0x0e, 0x16, 0x21,	/* 0 */
 	 0x2e, 0x3d, 0x50, 0x65, 0x7d, 0x99, 0xb8, 0xdb,
@@ -683,38 +450,18 @@ static void om6802_sensor_init(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 		     const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	struct cam *cam;
-
-	cam = &gspca_dev->cam;
+	struct cam *cam  = &gspca_dev->cam;
 
 	cam->cam_mode = vga_mode_t16;
 	cam->nmodes = ARRAY_SIZE(vga_mode_t16);
 
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->colors = COLORS_DEF;
-	sd->gamma = GAMMA_DEF;
-	sd->autogain = AUTOGAIN_DEF;
-	sd->mirror = MIRROR_DEF;
-	sd->freq = FREQ_DEF;
-	sd->awb = AWB_DEF;
-	sd->sharpness = SHARPNESS_DEF;
-	sd->effect = EFFECTS_DEF;
-	sd->red_gain = RED_GAIN_DEF;
-	sd->blue_gain = BLUE_GAIN_DEF;
-	sd->green_gain = GAIN_DEF * 3 - RED_GAIN_DEF - BLUE_GAIN_DEF;
-
 	return 0;
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 brightness)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	unsigned int brightness;
 	u8 set6[4] = { 0x8f, 0x24, 0xc3, 0x00 };
 
-	brightness = sd->brightness;
 	if (brightness < 7) {
 		set6[1] = 0x26;
 		set6[3] = 0x70 - brightness * 0x10;
@@ -725,10 +472,8 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	reg_w_buf(gspca_dev, set6, sizeof set6);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, s32 contrast)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	unsigned int contrast = sd->contrast;
 	u16 reg_to_write;
 
 	if (contrast < 7)
@@ -739,89 +484,62 @@ static void setcontrast(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, reg_to_write);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u16 reg_to_write;
 
-	reg_to_write = 0x80bb + sd->colors * 0x100;	/* was 0xc0 */
+	reg_to_write = 0x80bb + val * 0x100;	/* was 0xc0 */
 	reg_w(gspca_dev, reg_to_write);
 }
 
-static void setgamma(struct gspca_dev *gspca_dev)
+static void setgamma(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
 	PDEBUG(D_CONF, "Gamma: %d", sd->gamma);
 	reg_w_ixbuf(gspca_dev, 0x90,
-		gamma_table[sd->gamma], sizeof gamma_table[0]);
+		gamma_table[val], sizeof gamma_table[0]);
 }
 
-static void setRGB(struct gspca_dev *gspca_dev)
+static void setawb_n_RGB(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 all_gain_reg[6] =
-		{0x87, 0x00, 0x88, 0x00, 0x89, 0x00};
+	u8 all_gain_reg[8] = {
+		0x87, 0x00, 0x88, 0x00, 0x89, 0x00, 0x80, 0x00 };
+	s32 red_gain, blue_gain, green_gain;
+
+	green_gain = sd->gain->val;
+
+	red_gain = green_gain + sd->red_balance->val;
+	if (red_gain > 0x40)
+		red_gain = 0x40;
+	else if (red_gain < 0x10)
+		red_gain = 0x10;
+
+	blue_gain = green_gain + sd->blue_balance->val;
+	if (blue_gain > 0x40)
+		blue_gain = 0x40;
+	else if (blue_gain < 0x10)
+		blue_gain = 0x10;
+
+	all_gain_reg[1] = red_gain;
+	all_gain_reg[3] = blue_gain;
+	all_gain_reg[5] = green_gain;
+	all_gain_reg[7] = sensor_data[sd->sensor].reg80;
+	if (!sd->awb->val)
+		all_gain_reg[7] &= ~0x04; /* AWB off */
 
-	all_gain_reg[1] = sd->red_gain;
-	all_gain_reg[3] = sd->blue_gain;
-	all_gain_reg[5] = sd->green_gain;
 	reg_w_buf(gspca_dev, all_gain_reg, sizeof all_gain_reg);
 }
 
-/* Generic fnc for r/b balance, exposure and awb */
-static void setawb(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	u16 reg80;
-
-	reg80 = (sensor_data[sd->sensor].reg80 << 8) | 0x80;
-
-	/* on awb leave defaults values */
-	if (!sd->awb) {
-		/* shoud we wait here.. */
-		/* update and reset RGB gains with webcam values */
-		sd->red_gain = reg_r(gspca_dev, 0x0087);
-		sd->blue_gain = reg_r(gspca_dev, 0x0088);
-		sd->green_gain = reg_r(gspca_dev, 0x0089);
-		reg80 &= ~0x0400;		/* AWB off */
-	}
-	reg_w(gspca_dev, reg80);
-	reg_w(gspca_dev, reg80);
-}
-
-static void init_gains(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	u16 reg80;
-	u8 all_gain_reg[8] =
-		{0x87, 0x00, 0x88, 0x00, 0x89, 0x00, 0x80, 0x00};
-
-	all_gain_reg[1] = sd->red_gain;
-	all_gain_reg[3] = sd->blue_gain;
-	all_gain_reg[5] = sd->green_gain;
-	reg80 = sensor_data[sd->sensor].reg80;
-	if (!sd->awb)
-		reg80 &= ~0x04;
-	all_gain_reg[7] = reg80;
-	reg_w_buf(gspca_dev, all_gain_reg, sizeof all_gain_reg);
-
-	reg_w(gspca_dev, (sd->red_gain  << 8) + 0x87);
-	reg_w(gspca_dev, (sd->blue_gain << 8) + 0x88);
-	reg_w(gspca_dev, (sd->green_gain  << 8) + 0x89);
-}
-
-static void setsharpness(struct gspca_dev *gspca_dev)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
 	u16 reg_to_write;
 
-	reg_to_write = 0x0aa6 + 0x1000 * sd->sharpness;
+	reg_to_write = 0x0aa6 + 0x1000 * val;
 
 	reg_w(gspca_dev, reg_to_write);
 }
 
-static void setfreq(struct gspca_dev *gspca_dev)
+static void setfreq(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 reg66;
@@ -829,7 +547,7 @@ static void setfreq(struct gspca_dev *gspca_dev)
 
 	switch (sd->sensor) {
 	case SENSOR_LT168G:
-		if (sd->freq != 0)
+		if (val != 0)
 			freq[3] = 0xa8;
 		reg66 = 0x41;
 		break;
@@ -840,7 +558,7 @@ static void setfreq(struct gspca_dev *gspca_dev)
 		reg66 = 0x40;
 		break;
 	}
-	switch (sd->freq) {
+	switch (val) {
 	case 0:				/* no flicker */
 		freq[3] = 0xf0;
 		break;
@@ -941,14 +659,9 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, (sensor->reg80 << 8) + 0x80);
 	reg_w(gspca_dev, (sensor->reg80 << 8) + 0x80);
 	reg_w(gspca_dev, (sensor->reg8e << 8) + 0x8e);
-
-	setbrightness(gspca_dev);
-	setcontrast(gspca_dev);
-	setgamma(gspca_dev);
-	setcolors(gspca_dev);
-	setsharpness(gspca_dev);
-	init_gains(gspca_dev);
-	setfreq(gspca_dev);
+	reg_w(gspca_dev, (0x20 << 8) + 0x87);
+	reg_w(gspca_dev, (0x20 << 8) + 0x88);
+	reg_w(gspca_dev, (0x20 << 8) + 0x89);
 
 	reg_w_buf(gspca_dev, sensor->data5, sizeof sensor->data5);
 	reg_w_buf(gspca_dev, sensor->nset8, sizeof sensor->nset8);
@@ -968,31 +681,44 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return 0;
 }
 
-static void setmirror(struct gspca_dev *gspca_dev)
+static void setmirror(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 hflipcmd[8] =
 		{0x62, 0x07, 0x63, 0x03, 0x64, 0x00, 0x60, 0x09};
 
-	if (sd->mirror)
+	if (val)
 		hflipcmd[3] = 0x01;
 
 	reg_w_buf(gspca_dev, hflipcmd, sizeof hflipcmd);
 }
 
-static void seteffect(struct gspca_dev *gspca_dev)
+static void seteffect(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	int idx = 0;
 
-	reg_w_buf(gspca_dev, effects_table[sd->effect],
-				sizeof effects_table[0]);
-	if (sd->effect == 1 || sd->effect == 5) {
-		PDEBUG(D_CONF,
-		       "This effect have been disabled for webcam \"safety\"");
-		return;
+	switch (val) {
+	case V4L2_COLORFX_NONE:
+		break;
+	case V4L2_COLORFX_BW:
+		idx = 2;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		idx = 3;
+		break;
+	case V4L2_COLORFX_SKETCH:
+		idx = 4;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		idx = 6;
+		break;
+	default:
+		break;
 	}
 
-	if (sd->effect == 1 || sd->effect == 4)
+	reg_w_buf(gspca_dev, effects_table[idx],
+				sizeof effects_table[0]);
+
+	if (val == V4L2_COLORFX_SKETCH)
 		reg_w(gspca_dev, 0x4aa6);
 	else
 		reg_w(gspca_dev, 0xfaa6);
@@ -1070,7 +796,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 	sensor = &sensor_data[sd->sensor];
-	setfreq(gspca_dev);
+	setfreq(gspca_dev, v4l2_ctrl_g_ctrl(sd->freq));
 	reg_r(gspca_dev, 0x0012);
 	reg_w_buf(gspca_dev, t2, sizeof t2);
 	reg_w_ixbuf(gspca_dev, 0xb3, t3, sizeof t3);
@@ -1142,296 +868,157 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, pkt_type, data, len);
 }
 
-static int sd_setblue_gain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->blue_gain = val;
-	if (gspca_dev->streaming)
-		reg_w(gspca_dev, (val << 8) + 0x88);
-	return 0;
-}
-
-static int sd_getblue_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->blue_gain;
-	return 0;
-}
-
-static int sd_setred_gain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->red_gain = val;
-	if (gspca_dev->streaming)
-		reg_w(gspca_dev, (val << 8) + 0x87);
-
-	return 0;
-}
-
-static int sd_getred_gain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->red_gain;
-	return 0;
-}
-
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-	u16 psg, nsg;
-
-	psg = sd->red_gain + sd->blue_gain + sd->green_gain;
-	nsg = val * 3;
-	sd->red_gain = sd->red_gain * nsg / psg;
-	if (sd->red_gain > 0x40)
-		sd->red_gain = 0x40;
-	else if (sd->red_gain < 0x10)
-		sd->red_gain = 0x10;
-	sd->blue_gain = sd->blue_gain * nsg / psg;
-	if (sd->blue_gain > 0x40)
-		sd->blue_gain = 0x40;
-	else if (sd->blue_gain < 0x10)
-		sd->blue_gain = 0x10;
-	sd->green_gain = sd->green_gain * nsg / psg;
-	if (sd->green_gain > 0x40)
-		sd->green_gain = 0x40;
-	else if (sd->green_gain < 0x10)
-		sd->green_gain = 0x10;
-
-	if (gspca_dev->streaming)
-		setRGB(gspca_dev);
-	return 0;
-}
-
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = (sd->red_gain + sd->blue_gain + sd->green_gain) / 3;
-	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 *val;
-}
-
-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 *val;
-}
-
-static int sd_setmirror(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->mirror = val;
-	if (gspca_dev->streaming)
-		setmirror(gspca_dev);
-	return 0;
-}
-
-static int sd_getmirror(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->mirror;
-	return *val;
-}
-
-static int sd_seteffect(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->effect = val;
-	if (gspca_dev->streaming)
-		seteffect(gspca_dev);
-	return 0;
-}
-
-static int sd_geteffect(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->effect;
-	return *val;
-}
-
-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 *val;
-}
-
-static int sd_setcolors(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
-	return 0;
-}
-
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	return 0;
-}
-
-static int sd_setgamma(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->gamma = val;
-	if (gspca_dev->streaming)
-		setgamma(gspca_dev);
-	return 0;
-}
-
-static int sd_getgamma(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gamma;
-	return 0;
-}
-
-static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->freq = val;
-	if (gspca_dev->streaming)
-		setfreq(gspca_dev);
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+	s32 red_gain, blue_gain, green_gain;
+
+	gspca_dev->usb_err = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		red_gain = reg_r(gspca_dev, 0x0087);
+		if (red_gain > 0x40)
+			red_gain = 0x40;
+		else if (red_gain < 0x10)
+			red_gain = 0x10;
+
+		blue_gain = reg_r(gspca_dev, 0x0088);
+		if (blue_gain > 0x40)
+			blue_gain = 0x40;
+		else if (blue_gain < 0x10)
+			blue_gain = 0x10;
+
+		green_gain = reg_r(gspca_dev, 0x0089);
+		if (green_gain > 0x40)
+			green_gain = 0x40;
+		else if (green_gain < 0x10)
+			green_gain = 0x10;
+
+		sd->gain->val = green_gain;
+		sd->red_balance->val = red_gain - green_gain;
+		sd->blue_balance->val = blue_gain - green_gain;
+		break;
+	}
 	return 0;
 }
 
-static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	*val = sd->freq;
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_setsharpness(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	sd->sharpness = val;
-	if (gspca_dev->streaming)
-		setsharpness(gspca_dev);
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAMMA:
+		setgamma(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		setmirror(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setfreq(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BACKLIGHT_COMPENSATION:
+		reg_w(gspca_dev, ctrl->val ? 0xf48e : 0xb48e);
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		setawb_n_RGB(gspca_dev);
+		break;
+	case V4L2_CID_COLORFX:
+		seteffect(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-static int sd_getsharpness(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->sharpness;
-	return 0;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.g_volatile_ctrl = sd_g_volatile_ctrl,
+	.s_ctrl = sd_s_ctrl,
+};
 
-/* Low Light set  here......*/
-static int sd_setlowlight(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 12);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 14, 1, 8);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 0x0d, 1, 7);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 0xf, 1, 5);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAMMA, 0, GAMMA_MAX, 1, 10);
+	/* Activate lowlight, some apps dont bring up the
+	   backlight_compensation control) */
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BACKLIGHT_COMPENSATION, 0, 1, 1, 1);
+	if (sd->sensor == SENSOR_TAS5130A)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+				V4L2_CID_HFLIP, 0, 1, 1, 0);
+	sd->awb = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
+	sd->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0x10, 0x40, 1, 0x20);
+	sd->blue_balance = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, -0x30, 0x30, 1, 0);
+	sd->red_balance = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, -0x30, 0x30, 1, 0);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 15, 1, 6);
+	v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_COLORFX, V4L2_COLORFX_SKETCH,
+			~((1 << V4L2_COLORFX_NONE) |
+			  (1 << V4L2_COLORFX_BW) |
+			  (1 << V4L2_COLORFX_SEPIA) |
+			  (1 << V4L2_COLORFX_SKETCH) |
+			  (1 << V4L2_COLORFX_NEGATIVE)),
+			V4L2_COLORFX_NONE);
+	sd->freq = v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 1,
+			V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 
-	sd->autogain = val;
-	if (val != 0)
-		reg_w(gspca_dev, 0xf48e);
-	else
-		reg_w(gspca_dev, 0xb48e);
-	return 0;
-}
+	v4l2_ctrl_auto_cluster(4, &sd->awb, 0, true);
 
-static int sd_getlowlight(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
 	return 0;
 }
 
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	static const char *freq_nm[3] = {"NoFliker", "50 Hz", "60 Hz"};
-
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		if ((unsigned) menu->index >= ARRAY_SIZE(freq_nm))
-			break;
-		strcpy((char *) menu->name, freq_nm[menu->index]);
-		return 0;
-	case V4L2_CID_EFFECTS:
-		if ((unsigned) menu->index < ARRAY_SIZE(effects_control)) {
-			strlcpy((char *) menu->name,
-				effects_control[menu->index],
-				sizeof menu->name);
-			return 0;
-		}
-		break;
-	}
-	return -EINVAL;
-}
-
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 	.other_input = 1,
 #endif
@@ -1460,6 +1047,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/topro.c b/drivers/media/video/gspca/topro.c
index c6326d177a3..a6055246cb9 100644
--- a/drivers/media/video/gspca/topro.c
+++ b/drivers/media/video/gspca/topro.c
@@ -120,24 +120,13 @@ static const u8 jpeg_head[] = {
 #define JPEG_HDR_SZ 521
 };
 
-enum e_ctrl {
-	EXPOSURE,
-	QUALITY,
-	SHARPNESS,
-	RGAIN,
-	GAIN,
-	BGAIN,
-	GAMMA,
-	AUTOGAIN,
-	NCTRLS		/* number of controls */
-};
-
-#define AUTOGAIN_DEF 1
-
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-
-	struct gspca_ctrl ctrls[NCTRLS];
+	struct v4l2_ctrl *jpegqual;
+	struct v4l2_ctrl *sharpness;
+	struct v4l2_ctrl *gamma;
+	struct v4l2_ctrl *blue;
+	struct v4l2_ctrl *red;
 
 	u8 framerate;
 	u8 quality;		/* webcam current JPEG quality (0..16) */
@@ -1415,32 +1404,33 @@ static void soi763a_6810_init(struct gspca_dev *gspca_dev)
 }
 
 /* set the gain and exposure */
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 expo, s32 gain,
+							s32 blue, s32 red)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	if (sd->sensor == SENSOR_CX0342) {
-		int expo;
-
-		expo = (sd->ctrls[EXPOSURE].val << 2) - 1;
+		expo = (expo << 2) - 1;
 		i2c_w(gspca_dev, CX0342_EXPO_LINE_L, expo);
 		i2c_w(gspca_dev, CX0342_EXPO_LINE_H, expo >> 8);
 		if (sd->bridge == BRIDGE_TP6800)
 			i2c_w(gspca_dev, CX0342_RAW_GBGAIN_H,
-						sd->ctrls[GAIN].val >> 8);
-		i2c_w(gspca_dev, CX0342_RAW_GBGAIN_L, sd->ctrls[GAIN].val);
+						gain >> 8);
+		i2c_w(gspca_dev, CX0342_RAW_GBGAIN_L, gain);
 		if (sd->bridge == BRIDGE_TP6800)
 			i2c_w(gspca_dev, CX0342_RAW_GRGAIN_H,
-						sd->ctrls[GAIN].val >> 8);
-		i2c_w(gspca_dev, CX0342_RAW_GRGAIN_L, sd->ctrls[GAIN].val);
-		if (sd->bridge == BRIDGE_TP6800)
-			i2c_w(gspca_dev, CX0342_RAW_BGAIN_H,
-						sd->ctrls[BGAIN].val >> 8);
-		i2c_w(gspca_dev, CX0342_RAW_BGAIN_L, sd->ctrls[BGAIN].val);
-		if (sd->bridge == BRIDGE_TP6800)
-			i2c_w(gspca_dev, CX0342_RAW_RGAIN_H,
-						sd->ctrls[RGAIN].val >> 8);
-		i2c_w(gspca_dev, CX0342_RAW_RGAIN_L, sd->ctrls[RGAIN].val);
+					gain >> 8);
+		i2c_w(gspca_dev, CX0342_RAW_GRGAIN_L, gain);
+		if (sd->sensor == SENSOR_CX0342) {
+			if (sd->bridge == BRIDGE_TP6800)
+				i2c_w(gspca_dev, CX0342_RAW_BGAIN_H,
+						blue >> 8);
+			i2c_w(gspca_dev, CX0342_RAW_BGAIN_L, blue);
+			if (sd->bridge == BRIDGE_TP6800)
+				i2c_w(gspca_dev, CX0342_RAW_RGAIN_H,
+						red >> 8);
+			i2c_w(gspca_dev, CX0342_RAW_RGAIN_L, red);
+		}
 		i2c_w(gspca_dev, CX0342_SYS_CTRL_0,
 				sd->bridge == BRIDGE_TP6800 ? 0x80 : 0x81);
 		return;
@@ -1448,10 +1438,10 @@ static void setexposure(struct gspca_dev *gspca_dev)
 
 	/* soi763a */
 	i2c_w(gspca_dev, 0x10,		/* AEC_H (exposure time) */
-			 sd->ctrls[EXPOSURE].val);
+			 expo);
 /*	i2c_w(gspca_dev, 0x76, 0x02);	 * AEC_L ([1:0] */
 	i2c_w(gspca_dev, 0x00,		/* gain */
-			 sd->ctrls[GAIN].val);
+			 gain);
 }
 
 /* set the JPEG quantization tables */
@@ -1472,12 +1462,10 @@ static void set_dqt(struct gspca_dev *gspca_dev, u8 q)
 }
 
 /* set the JPEG compression quality factor */
-static void setquality(struct gspca_dev *gspca_dev)
+static void setquality(struct gspca_dev *gspca_dev, s32 q)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u16 q;
 
-	q = sd->ctrls[QUALITY].val;
 	if (q != 16)
 		q = 15 - q;
 
@@ -1508,10 +1496,9 @@ static const u8 color_gain[NSENSORS][18] = {
 	 0xd5, 0x00, 0x46, 0x03, 0xdc, 0x03},	/* V R/G/B */
 };
 
-static void setgamma(struct gspca_dev *gspca_dev)
+static void setgamma(struct gspca_dev *gspca_dev, s32 gamma)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	int gamma;
 #define NGAMMA 6
 	static const u8 gamma_tb[NGAMMA][3][1024] = {
 	    {				/* gamma 0 - from tp6800 + soi763a */
@@ -3836,7 +3823,6 @@ static void setgamma(struct gspca_dev *gspca_dev)
 	if (sd->bridge == BRIDGE_TP6810)
 		reg_w(gspca_dev, 0x02, 0x28);
 /*	msleep(50); */
-	gamma = sd->ctrls[GAMMA].val;
 	bulk_w(gspca_dev, 0x00, gamma_tb[gamma][0], 1024);
 	bulk_w(gspca_dev, 0x01, gamma_tb[gamma][1], 1024);
 	bulk_w(gspca_dev, 0x02, gamma_tb[gamma][2], 1024);
@@ -3864,43 +3850,35 @@ static void setgamma(struct gspca_dev *gspca_dev)
 /*	msleep(50); */
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 val;
 
 	if (sd->bridge == BRIDGE_TP6800) {
-		val = sd->ctrls[SHARPNESS].val
-				| 0x08;		/* grid compensation enable */
+		val |= 0x08;		/* grid compensation enable */
 		if (gspca_dev->width == 640)
 			reg_w(gspca_dev, TP6800_R78_FORMAT, 0x00); /* vga */
 		else
 			val |= 0x04;		/* scaling down enable */
 		reg_w(gspca_dev, TP6800_R5D_DEMOSAIC_CFG, val);
 	} else {
-		val = (sd->ctrls[SHARPNESS].val << 5) | 0x08;
+		val = (val << 5) | 0x08;
 		reg_w(gspca_dev, 0x59, val);
 	}
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (gspca_dev->ctrl_dis & (1 << AUTOGAIN))
-		return;
-	if (sd->ctrls[AUTOGAIN].val) {
-		sd->ag_cnt = AG_CNT_START;
-		gspca_dev->ctrl_inac |= (1 << EXPOSURE) | (1 << GAIN);
-	} else {
-		sd->ag_cnt = -1;
-		gspca_dev->ctrl_inac &= ~((1 << EXPOSURE) | (1 << GAIN));
-	}
+	sd->ag_cnt = val ? AG_CNT_START : -1;
 }
 
 /* set the resolution for sensor cx0342 */
 static void set_resolution(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
+
 	reg_w(gspca_dev, TP6800_R21_ENDP_1_CTL, 0x00);
 	if (gspca_dev->width == 320) {
 		reg_w(gspca_dev, TP6800_R3F_FRAME_RATE, 0x06);
@@ -3926,8 +3904,9 @@ static void set_resolution(struct gspca_dev *gspca_dev)
 	i2c_w(gspca_dev, CX0342_SYS_CTRL_0, 0x01);
 	bulk_w(gspca_dev, 0x03, color_gain[SENSOR_CX0342],
 				ARRAY_SIZE(color_gain[0]));
-	setgamma(gspca_dev);
-	setquality(gspca_dev);
+	setgamma(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma));
+	if (sd->sensor == SENSOR_SOI763A)
+		setquality(gspca_dev, v4l2_ctrl_g_ctrl(sd->jpegqual));
 }
 
 /* convert the frame rate to a tp68x0 value */
@@ -3963,7 +3942,7 @@ static int get_fr_idx(struct gspca_dev *gspca_dev)
 	return i;
 }
 
-static void setframerate(struct gspca_dev *gspca_dev)
+static void setframerate(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 fr_idx;
@@ -3974,7 +3953,7 @@ static void setframerate(struct gspca_dev *gspca_dev)
 		reg_r(gspca_dev, 0x7b);
 		reg_w(gspca_dev, 0x7b,
 			sd->sensor == SENSOR_CX0342 ? 0x10 : 0x90);
-		if (sd->ctrls[EXPOSURE].val >= 128)
+		if (val >= 128)
 			fr_idx = 0xf0;		/* lower frame rate */
 	}
 
@@ -3984,43 +3963,43 @@ static void setframerate(struct gspca_dev *gspca_dev)
 		i2c_w(gspca_dev, CX0342_AUTO_ADC_CALIB, 0x01);
 }
 
-static void setrgain(struct gspca_dev *gspca_dev)
+static void setrgain(struct gspca_dev *gspca_dev, s32 rgain)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int rgain;
-
-	rgain = sd->ctrls[RGAIN].val;
 	i2c_w(gspca_dev, CX0342_RAW_RGAIN_H, rgain >> 8);
 	i2c_w(gspca_dev, CX0342_RAW_RGAIN_L, rgain);
 	i2c_w(gspca_dev, CX0342_SYS_CTRL_0, 0x80);
 }
 
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_setgain(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
+	s32 val = gspca_dev->gain->val;
 
 	if (sd->sensor == SENSOR_CX0342) {
-		sd->ctrls[BGAIN].val = sd->ctrls[BGAIN].val
-					* val / sd->ctrls[GAIN].val;
-		if (sd->ctrls[BGAIN].val > 4095)
-			sd->ctrls[BGAIN].val = 4095;
-		sd->ctrls[RGAIN].val = sd->ctrls[RGAIN].val
-					* val / sd->ctrls[GAIN].val;
-		if (sd->ctrls[RGAIN].val > 4095)
-			sd->ctrls[RGAIN].val = 4095;
+		s32 old = gspca_dev->gain->cur.val ?
+					gspca_dev->gain->cur.val : 1;
+
+		sd->blue->val = sd->blue->val * val / old;
+		if (sd->blue->val > 4095)
+			sd->blue->val = 4095;
+		sd->red->val = sd->red->val * val / old;
+		if (sd->red->val > 4095)
+			sd->red->val = 4095;
+	}
+	if (gspca_dev->streaming) {
+		if (sd->sensor == SENSOR_CX0342)
+			setexposure(gspca_dev, gspca_dev->exposure->val,
+					gspca_dev->gain->val,
+					sd->blue->val, sd->red->val);
+		else
+			setexposure(gspca_dev, gspca_dev->exposure->val,
+					gspca_dev->gain->val, 0, 0);
 	}
-	sd->ctrls[GAIN].val = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
 	return gspca_dev->usb_err;
 }
 
-static void setbgain(struct gspca_dev *gspca_dev)
+static void setbgain(struct gspca_dev *gspca_dev, s32 bgain)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-	int bgain;
-
-	bgain = sd->ctrls[BGAIN].val;
 	i2c_w(gspca_dev, CX0342_RAW_BGAIN_H, bgain >> 8);
 	i2c_w(gspca_dev, CX0342_RAW_BGAIN_L, bgain);
 	i2c_w(gspca_dev, CX0342_SYS_CTRL_0, 0x80);
@@ -4040,7 +4019,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 			framerates : framerates_6810;
 
 	sd->framerate = 30;		/* default: 30 fps */
-	gspca_dev->cam.ctrls = sd->ctrls;
 	return 0;
 }
 
@@ -4108,32 +4086,16 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	}
 	if (sd->sensor == SENSOR_SOI763A) {
 		pr_info("Sensor soi763a\n");
-		sd->ctrls[GAMMA].def = sd->bridge == BRIDGE_TP6800 ? 0 : 1;
-		sd->ctrls[GAIN].max = 15;
-		sd->ctrls[GAIN].def = 3;
-		gspca_dev->ctrl_dis = (1 << RGAIN) | (1 << BGAIN);
 		if (sd->bridge == BRIDGE_TP6810) {
 			soi763a_6810_init(gspca_dev);
-#if AUTOGAIN_DEF
-			gspca_dev->ctrl_inac |= (1 << EXPOSURE) | (1 << GAIN);
-#endif
-		} else {
-			gspca_dev->ctrl_dis |= (1 << AUTOGAIN);
 		}
 	} else {
 		pr_info("Sensor cx0342\n");
 		if (sd->bridge == BRIDGE_TP6810) {
 			cx0342_6810_init(gspca_dev);
-#if AUTOGAIN_DEF
-			gspca_dev->ctrl_inac |= (1 << EXPOSURE) | (1 << GAIN);
-#endif
-		} else {
-			gspca_dev->ctrl_dis |= (1 << AUTOGAIN);
 		}
 	}
 
-	if (sd->bridge == BRIDGE_TP6810)
-		sd->ctrls[QUALITY].def = 0;	/* auto quality */
 	set_dqt(gspca_dev, 0);
 	return 0;
 }
@@ -4207,8 +4169,9 @@ static void set_led(struct gspca_dev *gspca_dev, int on)
 
 static void cx0342_6800_start(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	static const struct cmd reg_init[] = {
-/*fixme: is this usefull?*/
+		/* fixme: is this useful? */
 		{TP6800_R17_GPIO_IO, 0x9f},
 		{TP6800_R16_GPIO_PD, 0x40},
 		{TP6800_R10_SIF_TYPE, 0x00},	/* i2c 8 bits */
@@ -4279,13 +4242,21 @@ static void cx0342_6800_start(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, TP6800_R54_DARK_CFG, 0x00);
 	i2c_w(gspca_dev, CX0342_EXPO_LINE_H, 0x00);
 	i2c_w(gspca_dev, CX0342_SYS_CTRL_0, 0x01);
-	setexposure(gspca_dev);
+	if (sd->sensor == SENSOR_CX0342)
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain),
+			v4l2_ctrl_g_ctrl(sd->blue),
+			v4l2_ctrl_g_ctrl(sd->red));
+	else
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain), 0, 0);
 	set_led(gspca_dev, 1);
 	set_resolution(gspca_dev);
 }
 
 static void cx0342_6810_start(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	static const struct cmd sensor_init_2[] = {
 		{CX0342_EXPO_LINE_L, 0x6f},
 		{CX0342_EXPO_LINE_H, 0x02},
@@ -4366,10 +4337,10 @@ static void cx0342_6810_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x07, 0x85);
 		reg_w(gspca_dev, TP6800_R78_FORMAT, 0x01);	/* qvga */
 	}
-	setgamma(gspca_dev);
+	setgamma(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma));
 	reg_w_buf(gspca_dev, tp6810_bridge_start,
 			ARRAY_SIZE(tp6810_bridge_start));
-	setsharpness(gspca_dev);
+	setsharpness(gspca_dev, v4l2_ctrl_g_ctrl(sd->sharpness));
 	bulk_w(gspca_dev, 0x03, color_gain[SENSOR_CX0342],
 				ARRAY_SIZE(color_gain[0]));
 	reg_w(gspca_dev, TP6800_R3F_FRAME_RATE, 0x87);
@@ -4380,11 +4351,12 @@ static void cx0342_6810_start(struct gspca_dev *gspca_dev)
 	i2c_w_buf(gspca_dev, sensor_init_5, ARRAY_SIZE(sensor_init_5));
 
 	set_led(gspca_dev, 1);
-/*	setquality(gspca_dev); */
+/*	setquality(gspca_dev, v4l2_ctrl_g_ctrl(sd->jpegqual)); */
 }
 
 static void soi763a_6800_start(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	static const struct cmd reg_init[] = {
 		{TP6800_R79_QUALITY, 0x04},
 		{TP6800_R79_QUALITY, 0x01},
@@ -4484,19 +4456,28 @@ static void soi763a_6800_start(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, TP6800_R5C_EDGE_THRLD, 0x10);
 	reg_w(gspca_dev, TP6800_R54_DARK_CFG, 0x00);
 
-	setsharpness(gspca_dev);
+	setsharpness(gspca_dev, v4l2_ctrl_g_ctrl(sd->sharpness));
 
 	bulk_w(gspca_dev, 0x03, color_gain[SENSOR_SOI763A],
 				ARRAY_SIZE(color_gain[0]));
 
 	set_led(gspca_dev, 1);
-	setexposure(gspca_dev);
-	setquality(gspca_dev);
-	setgamma(gspca_dev);
+	if (sd->sensor == SENSOR_CX0342)
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain),
+			v4l2_ctrl_g_ctrl(sd->blue),
+			v4l2_ctrl_g_ctrl(sd->red));
+	else
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain), 0, 0);
+	if (sd->sensor == SENSOR_SOI763A)
+		setquality(gspca_dev, v4l2_ctrl_g_ctrl(sd->jpegqual));
+	setgamma(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma));
 }
 
 static void soi763a_6810_start(struct gspca_dev *gspca_dev)
 {
+	struct sd *sd = (struct sd *) gspca_dev;
 	static const struct cmd bridge_init_2[] = {
 		{TP6800_R7A_BLK_THRLD, 0x00},
 		{TP6800_R79_QUALITY, 0x04},
@@ -4520,7 +4501,14 @@ static void soi763a_6810_start(struct gspca_dev *gspca_dev)
 	reg_w(gspca_dev, 0x22, gspca_dev->alt);
 	bulk_w(gspca_dev, 0x03, color_null, sizeof color_null);
 	reg_w(gspca_dev, 0x59, 0x40);
-	setexposure(gspca_dev);
+	if (sd->sensor == SENSOR_CX0342)
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain),
+			v4l2_ctrl_g_ctrl(sd->blue),
+			v4l2_ctrl_g_ctrl(sd->red));
+	else
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain), 0, 0);
 	reg_w_buf(gspca_dev, bridge_init_2, ARRAY_SIZE(bridge_init_2));
 	reg_w_buf(gspca_dev, tp6810_ov_init_common,
 			ARRAY_SIZE(tp6810_ov_init_common));
@@ -4534,7 +4522,7 @@ static void soi763a_6810_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x07, 0x85);
 		reg_w(gspca_dev, TP6800_R78_FORMAT, 0x01);	/* qvga */
 	}
-	setgamma(gspca_dev);
+	setgamma(gspca_dev, v4l2_ctrl_g_ctrl(sd->gamma));
 	reg_w_buf(gspca_dev, tp6810_bridge_start,
 			ARRAY_SIZE(tp6810_bridge_start));
 
@@ -4545,12 +4533,19 @@ static void soi763a_6810_start(struct gspca_dev *gspca_dev)
 
 	reg_w(gspca_dev, 0x00, 0x00);
 
-	setsharpness(gspca_dev);
+	setsharpness(gspca_dev, v4l2_ctrl_g_ctrl(sd->sharpness));
 	bulk_w(gspca_dev, 0x03, color_gain[SENSOR_SOI763A],
 				ARRAY_SIZE(color_gain[0]));
 	set_led(gspca_dev, 1);
 	reg_w(gspca_dev, TP6800_R3F_FRAME_RATE, 0xf0);
-	setexposure(gspca_dev);
+	if (sd->sensor == SENSOR_CX0342)
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain),
+			v4l2_ctrl_g_ctrl(sd->blue),
+			v4l2_ctrl_g_ctrl(sd->red));
+	else
+		setexposure(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+			v4l2_ctrl_g_ctrl(gspca_dev->gain), 0, 0);
 	reg_w_buf(gspca_dev, bridge_init_6, ARRAY_SIZE(bridge_init_6));
 }
 
@@ -4576,12 +4571,25 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		reg_w(gspca_dev, 0x80, 0x03);
 		reg_w(gspca_dev, 0x82, gspca_dev->curr_mode ? 0x0a : 0x0e);
 
-		setexposure(gspca_dev);
-		setquality(gspca_dev);
-		setautogain(gspca_dev);
+		if (sd->sensor == SENSOR_CX0342)
+			setexposure(gspca_dev,
+				v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+				v4l2_ctrl_g_ctrl(gspca_dev->gain),
+				v4l2_ctrl_g_ctrl(sd->blue),
+				v4l2_ctrl_g_ctrl(sd->red));
+		else
+			setexposure(gspca_dev,
+				v4l2_ctrl_g_ctrl(gspca_dev->exposure),
+				v4l2_ctrl_g_ctrl(gspca_dev->gain), 0, 0);
+		if (sd->sensor == SENSOR_SOI763A)
+			setquality(gspca_dev,
+				   v4l2_ctrl_g_ctrl(sd->jpegqual));
+		if (sd->bridge == BRIDGE_TP6810)
+			setautogain(gspca_dev,
+				    v4l2_ctrl_g_ctrl(gspca_dev->autogain));
 	}
 
-	setframerate(gspca_dev);
+	setframerate(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure));
 
 	return gspca_dev->usb_err;
 }
@@ -4672,12 +4680,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	}
 }
 
-/* -- do autogain -- */
-/* gain setting is done in setexposure() for tp6810 */
-static void setgain(struct gspca_dev *gspca_dev) {}
-#define WANT_REGULAR_AUTOGAIN
-#include "autogain_functions.h"
-
 static void sd_dq_callback(struct gspca_dev *gspca_dev)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
@@ -4739,17 +4741,19 @@ static void sd_dq_callback(struct gspca_dev *gspca_dev)
 			luma /= 4;
 		reg_w(gspca_dev, 0x7d, 0x00);
 
-		expo = sd->ctrls[EXPOSURE].val;
-		ret = auto_gain_n_exposure(gspca_dev, luma,
+		expo = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+		ret = gspca_expo_autogain(gspca_dev, luma,
 				60,	/* desired luma */
 				6,	/* dead zone */
 				2,	/* gain knee */
 				70);	/* expo knee */
 		sd->ag_cnt = AG_CNT_START;
 		if (sd->bridge == BRIDGE_TP6810) {
-			if ((expo >= 128 && sd->ctrls[EXPOSURE].val < 128)
-			 || (expo < 128 && sd->ctrls[EXPOSURE].val >= 128))
-				setframerate(gspca_dev);
+			int new_expo = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+
+			if ((expo >= 128 && new_expo < 128)
+			 || (expo < 128 && new_expo >= 128))
+				setframerate(gspca_dev, new_expo);
 		}
 		break;
 	}
@@ -4789,7 +4793,7 @@ static void sd_set_streamparm(struct gspca_dev *gspca_dev,
 
 	sd->framerate = tpf->denominator / tpf->numerator;
 	if (gspca_dev->streaming)
-		setframerate(gspca_dev);
+		setframerate(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure));
 
 	/* Return the actual framerate */
 	i = get_fr_idx(gspca_dev);
@@ -4806,12 +4810,10 @@ static int sd_set_jcomp(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	if (sd->sensor == SENSOR_SOI763A)
-		jpeg_set_qual(sd->jpeg_hdr, jcomp->quality);
-/*	else
-		fixme: TODO
-*/
-	return gspca_dev->usb_err;
+	if (sd->sensor != SENSOR_SOI763A)
+		return -ENOTTY;
+	v4l2_ctrl_s_ctrl(sd->jpegqual, jcomp->quality);
+	return 0;
 }
 
 static int sd_get_jcomp(struct gspca_dev *gspca_dev,
@@ -4819,118 +4821,109 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
+	if (sd->sensor != SENSOR_SOI763A)
+		return -ENOTTY;
 	memset(jcomp, 0, sizeof *jcomp);
-	jcomp->quality = jpeg_q[sd->quality];
+	jcomp->quality = v4l2_ctrl_g_ctrl(sd->jpegqual);
 	jcomp->jpeg_markers = V4L2_JPEG_MARKER_DHT
 			| V4L2_JPEG_MARKER_DQT;
 	return 0;
 }
 
-static struct ctrl sd_ctrls[NCTRLS] = {
-[EXPOSURE] = {
-	    {
-		.id = V4L2_CID_EXPOSURE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Exposure",
-		.minimum = 0x01,
-		.maximum = 0xdc,
-		.step = 1,
-		.default_value = 0x4e,
-	    },
-	    .set_control = setexposure
-	},
-[QUALITY] = {
-	    {
-		.id = V4L2_CID_PRIVATE_BASE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Compression quality",
-		.minimum = 0,
-		.maximum = 15,
-		.step = 1,
-		.default_value = 13,
-	    },
-	    .set_control = setquality
-	},
-[RGAIN] = {
-	    {
-		.id = V4L2_CID_RED_BALANCE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Red balance",
-		.minimum = 0,
-		.maximum = 4095,
-		.step = 1,
-		.default_value = 256,
-	    },
-	    .set_control = setrgain
-	},
-[GAIN] = {
-	    {
-		.id = V4L2_CID_GAIN,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Gain",
-		.minimum = 0,
-		.maximum = 4095,
-		.step = 1,
-		.default_value = 256,
-	    },
-	    .set = sd_setgain
-	},
-[BGAIN] = {
-	    {
-		.id = V4L2_CID_BLUE_BALANCE,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Blue balance",
-		.minimum = 0,
-		.maximum = 4095,
-		.step = 1,
-		.default_value = 256,
-	    },
-	    .set_control = setbgain
-	},
-[SHARPNESS] = {
-	    {
-		.id	 = V4L2_CID_SHARPNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Sharpness",
-		.minimum = 0,
-		.maximum = 3,
-		.step    = 1,
-		.default_value = 2,
-	    },
-	    .set_control = setsharpness
-	},
-[GAMMA] = {
-	    {
-		.id      = V4L2_CID_GAMMA,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gamma",
-		.minimum = 0,
-		.maximum = NGAMMA - 1,
-		.step    = 1,
-		.default_value = 1,
-	    },
-	    .set_control = setgamma
-	},
-[AUTOGAIN] = {
-	    {
-		.id      = V4L2_CID_AUTOGAIN,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Auto Gain",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-		.default_value = AUTOGAIN_DEF
-	    },
-	    .set_control = setautogain
-	},
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAMMA:
+		setgamma(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		setbgain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		setrgain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		sd_setgain(gspca_dev);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		if (ctrl->val)
+			break;
+		sd_setgain(gspca_dev);
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		jpeg_set_qual(sd->jpeg_hdr, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
 };
 
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 1, 0xdc, 1, 0x4e);
+	if (sd->sensor == SENSOR_CX0342) {
+		sd->red = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_RED_BALANCE, 0, 4095, 1, 256);
+		sd->blue = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BLUE_BALANCE, 0, 4095, 1, 256);
+	}
+	if (sd->sensor == SENSOR_SOI763A)
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 15, 1, 3);
+	else
+		gspca_dev->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 4095, 1, 256);
+	sd->sharpness = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 3, 1, 2);
+	sd->gamma = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAMMA, 0, NGAMMA - 1, 1,
+			(sd->sensor == SENSOR_SOI763A &&
+			 sd->bridge == BRIDGE_TP6800) ? 0 : 1);
+	if (sd->bridge == BRIDGE_TP6810)
+		gspca_dev->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	if (sd->sensor == SENSOR_SOI763A)
+		sd->jpegqual = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_JPEG_COMPRESSION_QUALITY,
+			0, 15, 1, (sd->bridge == BRIDGE_TP6810) ? 0 : 13);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	if (gspca_dev->autogain)
+		v4l2_ctrl_auto_cluster(3, &gspca_dev->autogain, 0, false);
+	else
+		v4l2_ctrl_cluster(2, &gspca_dev->exposure);
+	return 0;
+}
+
 static const struct sd_desc sd_desc = {
 	.name = KBUILD_MODNAME,
-	.ctrls = sd_ctrls,
-	.nctrls = NCTRLS,
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.isoc_init = sd_isoc_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
diff --git a/drivers/media/video/gspca/tv8532.c b/drivers/media/video/gspca/tv8532.c
index c8922c5ffbf..8591324a53e 100644
--- a/drivers/media/video/gspca/tv8532.c
+++ b/drivers/media/video/gspca/tv8532.c
@@ -30,49 +30,9 @@ MODULE_LICENSE("GPL");
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
 
-	__u16 exposure;
-	__u16 gain;
-
 	__u8 packet;
 };
 
-/* V4L2 controls supported by the driver */
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setgain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-	{
-	 {
-	  .id = V4L2_CID_EXPOSURE,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Exposure",
-	  .minimum = 1,
-	  .maximum = 0x18f,
-	  .step = 1,
-#define EXPOSURE_DEF 0x18f
-	  .default_value = EXPOSURE_DEF,
-	  },
-	 .set = sd_setexposure,
-	 .get = sd_getexposure,
-	 },
-	{
-	 {
-	  .id = V4L2_CID_GAIN,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Gain",
-	  .minimum = 0,
-	  .maximum = 0x7ff,
-	  .step = 1,
-#define GAIN_DEF 0x100
-	  .default_value = GAIN_DEF,
-	  },
-	 .set = sd_setgain,
-	 .get = sd_getgain,
-	 },
-};
-
 static const struct v4l2_pix_format sif_mode[] = {
 	{176, 144, V4L2_PIX_FMT_SBGGR8, V4L2_FIELD_NONE,
 		.bytesperline = 176,
@@ -202,15 +162,12 @@ static void tv_8532WriteEEprom(struct gspca_dev *gspca_dev)
 static int sd_config(struct gspca_dev *gspca_dev,
 		     const struct usb_device_id *id)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	struct cam *cam;
 
 	cam = &gspca_dev->cam;
 	cam->cam_mode = sif_mode;
 	cam->nmodes = ARRAY_SIZE(sif_mode);
 
-	sd->exposure = EXPOSURE_DEF;
-	sd->gain = GAIN_DEF;
 	return 0;
 }
 
@@ -241,23 +198,19 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return 0;
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_w2(gspca_dev, R1C_AD_EXPOSE_TIMEL, sd->exposure);
+	reg_w2(gspca_dev, R1C_AD_EXPOSE_TIMEL, val);
 	reg_w1(gspca_dev, R00_PART_CONTROL, LATENT_CHANGE | EXPO_CHANGE);
 						/* 0x84 */
 }
 
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	reg_w2(gspca_dev, R20_GAIN_G1L, sd->gain);
-	reg_w2(gspca_dev, R22_GAIN_RL, sd->gain);
-	reg_w2(gspca_dev, R24_GAIN_BL, sd->gain);
-	reg_w2(gspca_dev, R26_GAIN_G2L, sd->gain);
+	reg_w2(gspca_dev, R20_GAIN_G1L, val);
+	reg_w2(gspca_dev, R22_GAIN_RL, val);
+	reg_w2(gspca_dev, R24_GAIN_BL, val);
+	reg_w2(gspca_dev, R26_GAIN_G2L, val);
 }
 
 /* -- start the camera -- */
@@ -289,9 +242,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 
 	tv_8532_setReg(gspca_dev);
 
-	setexposure(gspca_dev);
-	setgain(gspca_dev);
-
 	/************************************************/
 	reg_w1(gspca_dev, R31_UPD, 0x01);	/* update registers */
 	msleep(200);
@@ -339,49 +289,55 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 			data + gspca_dev->width + 5, gspca_dev->width);
 }
 
-static int sd_setexposure(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
 
-	sd->exposure = val;
-	if (gspca_dev->streaming)
-		setexposure(gspca_dev);
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_getexposure(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	if (!gspca_dev->streaming)
+		return 0;
 
-	*val = sd->exposure;
-	return 0;
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		setgain(gspca_dev, ctrl->val);
+		break;
+	}
+	return gspca_dev->usb_err;
 }
 
-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 const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_getgain(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->gain;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 2);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 0x18f, 1, 0x18f);
+	v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 0x7ff, 1, 0x100);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
 	return 0;
 }
 
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.pkt_scan = sd_pkt_scan,
@@ -415,6 +371,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/vc032x.c b/drivers/media/video/gspca/vc032x.c
index 208f6b2d512..f21fd1677c3 100644
--- a/drivers/media/video/gspca/vc032x.c
+++ b/drivers/media/video/gspca/vc032x.c
@@ -33,18 +33,10 @@ MODULE_LICENSE("GPL");
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;	/* !! must be the first item */
-
-	u8 brightness;
-	u8 contrast;
-	u8 colors;
-	u8 hflip;
-	u8 vflip;
-	u8 lightfreq;
-	s8 sharpness;
-	u16 exposure;
-	u8 gain;
-	u8 autogain;
-	u8 backlight;
+	struct { /* hvflip cluster */
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
 
 	u8 image_offset;
 
@@ -73,252 +65,6 @@ enum sensors {
 	NSENSORS
 };
 
-/* V4L2 controls supported by the driver */
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getcolors(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_setfreq(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getfreq(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_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 int sd_setautogain(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val);
-static int sd_setbacklight(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getbacklight(struct gspca_dev *gspca_dev, __s32 *val);
-
-static const struct ctrl sd_ctrls[] = {
-#define BRIGHTNESS_IDX 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define BRIGHTNESS_DEF 128
-		.default_value = BRIGHTNESS_DEF,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define CONTRAST_IDX 1
-	{
-	    {
-		.id      = V4L2_CID_CONTRAST,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Contrast",
-		.minimum = 0,
-		.maximum = 255,
-		.step    = 1,
-#define CONTRAST_DEF 127
-		.default_value = CONTRAST_DEF,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-#define COLORS_IDX 2
-	{
-	    {
-		.id      = V4L2_CID_SATURATION,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Saturation",
-		.minimum = 1,
-		.maximum = 127,
-		.step    = 1,
-#define COLOR_DEF 63
-		.default_value = COLOR_DEF,
-	    },
-	    .set = sd_setcolors,
-	    .get = sd_getcolors,
-	},
-/* next 2 controls work with some sensors only */
-#define HFLIP_IDX 3
-	{
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define HFLIP_DEF 0
-		.default_value = HFLIP_DEF,
-	    },
-	    .set = sd_sethflip,
-	    .get = sd_gethflip,
-	},
-#define VFLIP_IDX 4
-	{
-	    {
-		.id      = V4L2_CID_VFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Vflip",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define VFLIP_DEF 0
-		.default_value = VFLIP_DEF,
-	    },
-	    .set = sd_setvflip,
-	    .get = sd_getvflip,
-	},
-#define LIGHTFREQ_IDX 5
-	{
-	    {
-		.id	 = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type    = V4L2_CTRL_TYPE_MENU,
-		.name    = "Light frequency filter",
-		.minimum = 0,
-		.maximum = 2,	/* 0: No, 1: 50Hz, 2:60Hz */
-		.step    = 1,
-#define FREQ_DEF 1
-		.default_value = FREQ_DEF,
-	    },
-	    .set = sd_setfreq,
-	    .get = sd_getfreq,
-	},
-#define SHARPNESS_IDX 6
-	{
-	 {
-	  .id = V4L2_CID_SHARPNESS,
-	  .type = V4L2_CTRL_TYPE_INTEGER,
-	  .name = "Sharpness",
-	  .minimum = -1,
-	  .maximum = 2,
-	  .step = 1,
-#define SHARPNESS_DEF -1
-	  .default_value = SHARPNESS_DEF,
-	  },
-	 .set = sd_setsharpness,
-	 .get = sd_getsharpness,
-	 },
-#define GAIN_IDX 7
-	{
-	    {
-		.id      = V4L2_CID_GAIN,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Gain",
-		.minimum = 0,
-		.maximum = 78,
-		.step    = 1,
-#define GAIN_DEF 0
-		.default_value = GAIN_DEF,
-	    },
-	    .set = sd_setgain,
-	    .get = sd_getgain,
-	},
-#define EXPOSURE_IDX 8
-	{
-		{
-			.id = V4L2_CID_EXPOSURE,
-			.type = V4L2_CTRL_TYPE_INTEGER,
-			.name = "Exposure",
-#define EXPOSURE_DEF 450
-			.minimum = 0,
-			.maximum = 4095,
-			.step = 1,
-			.default_value = EXPOSURE_DEF,
-		},
-		.set = sd_setexposure,
-		.get = sd_getexposure,
-	},
-#define AUTOGAIN_IDX 9
-	{
-		{
-			.id = V4L2_CID_AUTOGAIN,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "Automatic Gain and Exposure",
-			.minimum = 0,
-			.maximum = 1,
-			.step = 1,
-#define AUTOGAIN_DEF 1
-			.default_value = AUTOGAIN_DEF,
-		},
-		.set = sd_setautogain,
-		.get = sd_getautogain,
-	},
-#define BACKLIGHT_IDX 10
-	{
-		{
-			.id = V4L2_CID_BACKLIGHT_COMPENSATION,
-			.type = V4L2_CTRL_TYPE_BOOLEAN,
-			.name = "Backlight Compensation",
-			.minimum = 0,
-			.maximum = 15,
-			.step = 1,
-#define BACKLIGHT_DEF 15
-			.default_value = BACKLIGHT_DEF,
-		},
-		.set = sd_setbacklight,
-		.get = sd_getbacklight,
-	},
-};
-
-/* table of the disabled controls */
-static u32 ctrl_dis[NSENSORS] = {
-    [SENSOR_HV7131R] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << HFLIP_IDX) | (1 << VFLIP_IDX) | (1 << LIGHTFREQ_IDX)
-		| (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_MI0360] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << HFLIP_IDX) | (1 << VFLIP_IDX) | (1 << LIGHTFREQ_IDX)
-		| (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_MI1310_SOC] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << LIGHTFREQ_IDX) | (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_MI1320] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << LIGHTFREQ_IDX) | (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_MI1320_SOC] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << LIGHTFREQ_IDX) | (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_OV7660] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << LIGHTFREQ_IDX) | (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_OV7670] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_PO1200] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << LIGHTFREQ_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_PO3130NC] =
-	(1 << BRIGHTNESS_IDX) | (1 << CONTRAST_IDX) | (1 << COLORS_IDX)
-		| (1 << HFLIP_IDX) | (1 << VFLIP_IDX) | (1 << LIGHTFREQ_IDX)
-		| (1 << SHARPNESS_IDX)
-		| (1 << GAIN_IDX) | (1 << EXPOSURE_IDX)
-		| (1 << AUTOGAIN_IDX) | (1 << BACKLIGHT_IDX),
-    [SENSOR_POxxxx] =
-	(1 << HFLIP_IDX) | (1 << VFLIP_IDX) | (1 << LIGHTFREQ_IDX),
-};
 
 static const struct v4l2_pix_format vc0321_mode[] = {
 	{320, 240, V4L2_PIX_FMT_YVYU, V4L2_FIELD_NONE,
@@ -3405,18 +3151,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	    (id->idProduct == 0x0892 || id->idProduct == 0x0896))
 		sd->sensor = SENSOR_POxxxx;	/* no probe */
 
-	sd->brightness = BRIGHTNESS_DEF;
-	sd->contrast = CONTRAST_DEF;
-	sd->colors = COLOR_DEF;
-	sd->hflip = HFLIP_DEF;
-	sd->vflip = VFLIP_DEF;
-	sd->lightfreq = FREQ_DEF;
-	sd->sharpness = SHARPNESS_DEF;
-	sd->gain = GAIN_DEF;
-	sd->exposure = EXPOSURE_DEF;
-	sd->autogain = AUTOGAIN_DEF;
-	sd->backlight = BACKLIGHT_DEF;
-
 	return 0;
 }
 
@@ -3512,7 +3246,6 @@ static int sd_init(struct gspca_dev *gspca_dev)
 		}
 	}
 	cam->npkt = npkt[sd->sensor];
-	gspca_dev->ctrl_dis = ctrl_dis[sd->sensor];
 
 	if (sd->sensor == SENSOR_OV7670)
 		sd->flags |= FL_HFLIP | FL_VFLIP;
@@ -3534,14 +3267,11 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return gspca_dev->usb_err;
 }
 
-static void setbrightness(struct gspca_dev *gspca_dev)
+static void setbrightness(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 data;
 
-	if (gspca_dev->ctrl_dis & (1 << BRIGHTNESS_IDX))
-		return;
-	data = sd->brightness;
+	data = val;
 	if (data >= 0x80)
 		data &= 0x7f;
 	else
@@ -3549,36 +3279,27 @@ static void setbrightness(struct gspca_dev *gspca_dev)
 	i2c_write(gspca_dev, 0x98, &data, 1);
 }
 
-static void setcontrast(struct gspca_dev *gspca_dev)
+static void setcontrast(struct gspca_dev *gspca_dev, u8 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (gspca_dev->ctrl_dis & (1 << CONTRAST_IDX))
-		return;
-	i2c_write(gspca_dev, 0x99, &sd->contrast, 1);
+	i2c_write(gspca_dev, 0x99, &val, 1);
 }
 
-static void setcolors(struct gspca_dev *gspca_dev)
+static void setcolors(struct gspca_dev *gspca_dev, u8 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 data;
 
-	if (gspca_dev->ctrl_dis & (1 << COLORS_IDX))
-		return;
-	data = sd->colors - (sd->colors >> 3) - 1;
+	data = val - (val >> 3) - 1;
 	i2c_write(gspca_dev, 0x94, &data, 1);
-	i2c_write(gspca_dev, 0x95, &sd->colors, 1);
+	i2c_write(gspca_dev, 0x95, &val, 1);
 }
 
-static void sethvflip(struct gspca_dev *gspca_dev)
+static void sethvflip(struct gspca_dev *gspca_dev, bool hflip, bool vflip)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
-	u8 data[2], hflip, vflip;
+	u8 data[2];
 
-	hflip = sd->hflip;
 	if (sd->flags & FL_HFLIP)
 		hflip = !hflip;
-	vflip = sd->vflip;
 	if (sd->flags & FL_VFLIP)
 		vflip = !vflip;
 	switch (sd->sensor) {
@@ -3610,7 +3331,7 @@ static void sethvflip(struct gspca_dev *gspca_dev)
 	}
 }
 
-static void setlightfreq(struct gspca_dev *gspca_dev)
+static void setlightfreq(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	static const u8 (*ov7660_freq_tb[3])[4] =
@@ -3618,10 +3339,10 @@ static void setlightfreq(struct gspca_dev *gspca_dev)
 
 	if (sd->sensor != SENSOR_OV7660)
 		return;
-	usb_exchange(gspca_dev, ov7660_freq_tb[sd->lightfreq]);
+	usb_exchange(gspca_dev, ov7660_freq_tb[val]);
 }
 
-static void setsharpness(struct gspca_dev *gspca_dev)
+static void setsharpness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	u8 data;
@@ -3630,51 +3351,41 @@ static void setsharpness(struct gspca_dev *gspca_dev)
 	case SENSOR_PO1200:
 		data = 0;
 		i2c_write(gspca_dev, 0x03, &data, 1);
-		if (sd->sharpness < 0)
+		if (val < 0)
 			data = 0x6a;
 		else
-			data = 0xb5 + sd->sharpness * 3;
+			data = 0xb5 + val * 3;
 		i2c_write(gspca_dev, 0x61, &data, 1);
 		break;
 	case SENSOR_POxxxx:
-		if (sd->sharpness < 0)
+		if (val < 0)
 			data = 0x7e;	/* def = max */
 		else
-			data = 0x60 + sd->sharpness * 0x0f;
+			data = 0x60 + val * 0x0f;
 		i2c_write(gspca_dev, 0x59, &data, 1);
 		break;
 	}
 }
-static void setgain(struct gspca_dev *gspca_dev)
+static void setgain(struct gspca_dev *gspca_dev, u8 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	if (gspca_dev->ctrl_dis & (1 << GAIN_IDX))
-		return;
-	i2c_write(gspca_dev, 0x15, &sd->gain, 1);
+	i2c_write(gspca_dev, 0x15, &val, 1);
 }
 
-static void setexposure(struct gspca_dev *gspca_dev)
+static void setexposure(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u8 data;
 
-	if (gspca_dev->ctrl_dis & (1 << EXPOSURE_IDX))
-		return;
-	data = sd->exposure >> 8;
+	data = val >> 8;
 	i2c_write(gspca_dev, 0x1a, &data, 1);
-	data = sd->exposure;
+	data = val;
 	i2c_write(gspca_dev, 0x1b, &data, 1);
 }
 
-static void setautogain(struct gspca_dev *gspca_dev)
+static void setautogain(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	static const u8 data[2] = {0x28, 0x3c};
 
-	if (gspca_dev->ctrl_dis & (1 << AUTOGAIN_IDX))
-		return;
-	i2c_write(gspca_dev, 0xd1, &data[sd->autogain], 1);
+	i2c_write(gspca_dev, 0xd1, &data[val], 1);
 }
 
 static void setgamma(struct gspca_dev *gspca_dev)
@@ -3683,30 +3394,29 @@ static void setgamma(struct gspca_dev *gspca_dev)
 	usb_exchange(gspca_dev, poxxxx_gamma);
 }
 
-static void setbacklight(struct gspca_dev *gspca_dev)
+static void setbacklight(struct gspca_dev *gspca_dev, s32 val)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
 	u16 v;
 	u8 data;
 
-	data = (sd->backlight << 4) | 0x0f;
+	data = (val << 4) | 0x0f;
 	i2c_write(gspca_dev, 0xaa, &data, 1);
-	v = 613 + 12 * sd->backlight;
+	v = 613 + 12 * val;
 	data = v >> 8;
 	i2c_write(gspca_dev, 0xc4, &data, 1);
 	data = v;
 	i2c_write(gspca_dev, 0xc5, &data, 1);
-	v = 1093 - 12 * sd->backlight;
+	v = 1093 - 12 * val;
 	data = v >> 8;
 	i2c_write(gspca_dev, 0xc6, &data, 1);
 	data = v;
 	i2c_write(gspca_dev, 0xc7, &data, 1);
-	v = 342 + 9 * sd->backlight;
+	v = 342 + 9 * val;
 	data = v >> 8;
 	i2c_write(gspca_dev, 0xc8, &data, 1);
 	data = v;
 	i2c_write(gspca_dev, 0xc9, &data, 1);
-	v = 702 - 9 * sd->backlight;
+	v = 702 - 9 * val;
 	data = v >> 8;
 	i2c_write(gspca_dev, 0xca, &data, 1);
 	data = v;
@@ -3833,14 +3543,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 /*	case SENSOR_POxxxx: */
 		usb_exchange(gspca_dev, poxxxx_init_common);
 		setgamma(gspca_dev);
-		setbacklight(gspca_dev);
-		setbrightness(gspca_dev);
-		setcontrast(gspca_dev);
-		setcolors(gspca_dev);
-		setsharpness(gspca_dev);
-		setautogain(gspca_dev);
-		setexposure(gspca_dev);
-		setgain(gspca_dev);
 		usb_exchange(gspca_dev, poxxxx_init_start_3);
 		if (mode)
 			init = poxxxx_initQVGA;
@@ -3873,8 +3575,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 			break;
 		}
 		msleep(100);
-		sethvflip(gspca_dev);
-		setlightfreq(gspca_dev);
 	}
 	switch (sd->sensor) {
 	case SENSOR_OV7670:
@@ -3960,233 +3660,152 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-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 gspca_dev->usb_err;
-}
-
-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 gspca_dev->usb_err;
-}
-
-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_setcolors(struct gspca_dev *gspca_dev, __s32 val)
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->colors = val;
-	if (gspca_dev->streaming)
-		setcolors(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getcolors(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->colors;
-	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)
-		sethvflip(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-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)
-		sethvflip(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
 
-	*val = sd->vflip;
-	return 0;
-}
+	gspca_dev->usb_err = 0;
 
-static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->lightfreq = val;
-	if (gspca_dev->streaming)
-		setlightfreq(gspca_dev);
-	return gspca_dev->usb_err;
-}
-
-static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->lightfreq;
-	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 gspca_dev->usb_err;
-}
-
-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_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 gspca_dev->usb_err;
-}
-
-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 gspca_dev->usb_err;
-}
-
-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_setautogain(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->autogain = val;
-	if (gspca_dev->streaming)
-		setautogain(gspca_dev);
+	if (!gspca_dev->streaming && ctrl->id != V4L2_CID_POWER_LINE_FREQUENCY)
+		return 0;
 
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		setbrightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		setcontrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		setcolors(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		sethvflip(gspca_dev, sd->hflip->val, sd->vflip->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		setsharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		setautogain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_GAIN:
+		setgain(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE:
+		setexposure(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BACKLIGHT_COMPENSATION:
+		setbacklight(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		setlightfreq(gspca_dev, ctrl->val);
+		break;
+	}
 	return gspca_dev->usb_err;
 }
 
-static int sd_getautogain(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->autogain;
-	return 0;
-}
-
-static int sd_setbacklight(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->backlight = val;
-	if (gspca_dev->streaming)
-		setbacklight(gspca_dev);
-
-	return gspca_dev->usb_err;
-}
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
 
-static int sd_getbacklight(struct gspca_dev *gspca_dev, __s32 *val)
+static int sd_init_controls(struct gspca_dev *gspca_dev)
 {
-	struct sd *sd = (struct sd *) gspca_dev;
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	bool has_brightness = false;
+	bool has_contrast = false;
+	bool has_sat = false;
+	bool has_hvflip = false;
+	bool has_freq = false;
+	bool has_backlight = false;
+	bool has_exposure = false;
+	bool has_autogain = false;
+	bool has_gain = false;
+	bool has_sharpness = false;
 
-	*val = sd->backlight;
-	return 0;
-}
-
-static int sd_querymenu(struct gspca_dev *gspca_dev,
-			struct v4l2_querymenu *menu)
-{
-	static const char *freq_nm[3] = {"NoFliker", "50 Hz", "60 Hz"};
+	switch (sd->sensor) {
+	case SENSOR_HV7131R:
+	case SENSOR_MI0360:
+	case SENSOR_PO3130NC:
+		break;
+	case SENSOR_MI1310_SOC:
+	case SENSOR_MI1320:
+	case SENSOR_MI1320_SOC:
+	case SENSOR_OV7660:
+		has_hvflip = true;
+		break;
+	case SENSOR_OV7670:
+		has_hvflip = has_freq = true;
+		break;
+	case SENSOR_PO1200:
+		has_hvflip = has_sharpness = true;
+		break;
+	case SENSOR_POxxxx:
+		has_brightness = has_contrast = has_sat = has_backlight =
+			has_exposure = has_autogain = has_gain =
+			has_sharpness = true;
+		break;
+	}
 
-	switch (menu->id) {
-	case V4L2_CID_POWER_LINE_FREQUENCY:
-		if (menu->index >= ARRAY_SIZE(freq_nm))
-			break;
-		strcpy((char *) menu->name, freq_nm[menu->index]);
-		return 0;
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 8);
+	if (has_brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+	if (has_contrast)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 127);
+	if (has_sat)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SATURATION, 1, 127, 1, 63);
+	if (has_hvflip) {
+		sd->hflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+		sd->vflip = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
 	}
-	return -EINVAL;
+	if (has_sharpness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, -1, 2, 1, -1);
+	if (has_freq)
+		v4l2_ctrl_new_std_menu(hdl, &sd_ctrl_ops,
+			V4L2_CID_POWER_LINE_FREQUENCY,
+			V4L2_CID_POWER_LINE_FREQUENCY_60HZ, 0,
+			V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
+	if (has_autogain)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+	if (has_gain)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 78, 1, 0);
+	if (has_exposure)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_EXPOSURE, 0, 4095, 1, 450);
+	if (has_backlight)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BACKLIGHT_COMPENSATION, 0, 15, 1, 15);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	if (sd->hflip)
+		v4l2_ctrl_cluster(2, &sd->hflip);
+	return 0;
 }
 
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
+	.init_controls = sd_init_controls,
 	.config = sd_config,
 	.init = sd_init,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
 	.pkt_scan = sd_pkt_scan,
-	.querymenu = sd_querymenu,
 };
 
 /* -- module initialisation -- */
@@ -4227,6 +3846,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/vicam.c b/drivers/media/video/gspca/vicam.c
index 15a30f7a4b2..b1a64b91266 100644
--- a/drivers/media/video/gspca/vicam.c
+++ b/drivers/media/video/gspca/vicam.c
@@ -44,17 +44,10 @@ MODULE_DESCRIPTION("GSPCA ViCam USB Camera Driver");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(VICAM_FIRMWARE);
 
-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
@@ -86,31 +79,6 @@ static struct v4l2_pix_format vicam_mode[] = {
 		.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)
 {
@@ -146,12 +114,13 @@ static int vicam_set_camera_power(struct gspca_dev *gspca_dev, int state)
  */
 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;
+	s32 expo = v4l2_ctrl_g_ctrl(gspca_dev->exposure);
+	s32 gain = v4l2_ctrl_g_ctrl(gspca_dev->gain);
 
 	memset(req_data, 0, 16);
-	req_data[0] = sd->ctrls[GAIN].val;
+	req_data[0] = gain;
 	if (gspca_dev->width == 256)
 		req_data[1] |= 0x01; /* low nibble x-scale */
 	if (gspca_dev->height <= 122) {
@@ -167,9 +136,9 @@ static int vicam_read_frame(struct gspca_dev *gspca_dev, u8 *data, int size)
 	else /* Up to 244 lines with req_data[3] == 0x08 */
 		req_data[3] = 0x08; /* vend? */
 
-	if (sd->ctrls[EXPOSURE].val < 256) {
+	if (expo < 256) {
 		/* Frame rate maxed out, use partial frame expo time */
-		req_data[4] = 255 - sd->ctrls[EXPOSURE].val;
+		req_data[4] = 255 - expo;
 		req_data[5] = 0x00;
 		req_data[6] = 0x00;
 		req_data[7] = 0x01;
@@ -177,8 +146,8 @@ static int vicam_read_frame(struct gspca_dev *gspca_dev, u8 *data, int size)
 		/* 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[6] = expo & 0xFF;
+		req_data[7] = expo >> 8;
 	}
 	req_data[8] = ((244 - unscaled_height) / 2) & ~0x01; /* vstart */
 	/* bytes 9-15 do not seem to affect exposure or image quality */
@@ -260,7 +229,6 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	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);
 
@@ -335,6 +303,24 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 		vicam_set_camera_power(gspca_dev, 0);
 }
 
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 2);
+	gspca_dev->exposure = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_EXPOSURE, 0, 2047, 1, 256);
+	gspca_dev->gain = v4l2_ctrl_new_std(hdl, NULL,
+			V4L2_CID_GAIN, 0, 255, 1, 200);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	return 0;
+}
+
 /* Table of supported USB devices */
 static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x04c1, 0x009d)},
@@ -347,10 +333,9 @@ 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,
+	.init_controls = sd_init_controls,
 	.start  = sd_start,
 	.stop0  = sd_stop0,
 };
@@ -373,6 +358,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume  = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/gspca/w996Xcf.c b/drivers/media/video/gspca/w996Xcf.c
index 27d2cef0692..9e3a909e0a0 100644
--- a/drivers/media/video/gspca/w996Xcf.c
+++ b/drivers/media/video/gspca/w996Xcf.c
@@ -404,9 +404,14 @@ static void w9968cf_set_crop_window(struct sd *sd)
 	}
 
 	if (sd->sensor == SEN_OV7620) {
-		/* Sigh, this is dependend on the clock / framerate changes
-		   made by the frequency control, sick. */
-		if (sd->ctrls[FREQ].val == 1) {
+		/*
+		 * Sigh, this is dependend on the clock / framerate changes
+		 * made by the frequency control, sick.
+		 *
+		 * Note we cannot use v4l2_ctrl_g_ctrl here, as we get called
+		 * from ov519.c:setfreq() with the ctrl lock held!
+		 */
+		if (sd->freq->val == 1) {
 			start_cropx = 277;
 			start_cropy = 37;
 		} else {
@@ -474,8 +479,9 @@ static void w9968cf_mode_init_regs(struct sd *sd)
 		/* We may get called multiple times (usb isoc bw negotiat.) */
 		jpeg_define(sd->jpeg_hdr, sd->gspca_dev.height,
 			    sd->gspca_dev.width, 0x22); /* JPEG 420 */
-		jpeg_set_qual(sd->jpeg_hdr, sd->quality);
+		jpeg_set_qual(sd->jpeg_hdr, v4l2_ctrl_g_ctrl(sd->jpegqual));
 		w9968cf_upload_quantizationtables(sd);
+		v4l2_ctrl_grab(sd->jpegqual, true);
 	}
 
 	/* Video Capture Control Register */
@@ -514,6 +520,7 @@ static void w9968cf_mode_init_regs(struct sd *sd)
 
 static void w9968cf_stop0(struct sd *sd)
 {
+	v4l2_ctrl_grab(sd->jpegqual, false);
 	reg_w(sd, 0x39, 0x0000); /* disable JPEG encoder */
 	reg_w(sd, 0x16, 0x0000); /* stop video capture */
 }
diff --git a/drivers/media/video/gspca/xirlink_cit.c b/drivers/media/video/gspca/xirlink_cit.c
index ecada178bce..13b8d395d21 100644
--- a/drivers/media/video/gspca/xirlink_cit.c
+++ b/drivers/media/video/gspca/xirlink_cit.c
@@ -53,6 +53,7 @@ MODULE_PARM_DESC(rca_input,
 /* specific webcam descriptor */
 struct sd {
 	struct gspca_dev gspca_dev;		/* !! must be the first item */
+	struct v4l2_ctrl *lighting;
 	u8 model;
 #define CIT_MODEL0 0 /* bcd version 0.01 cams ie the xvp-500 */
 #define CIT_MODEL1 1 /* The model 1 - 4 nomenclature comes from the old */
@@ -65,127 +66,10 @@ struct sd {
 	u8 stop_on_control_change;
 	u8 sof_read;
 	u8 sof_len;
-	u8 contrast;
-	u8 brightness;
-	u8 hue;
-	u8 sharpness;
-	u8 lighting;
-	u8 hflip;
 };
 
-/* V4L2 controls supported by the driver */
-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_sethue(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_gethue(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_setlighting(struct gspca_dev *gspca_dev, __s32 val);
-static int sd_getlighting(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 void sd_stop0(struct gspca_dev *gspca_dev);
 
-static const struct ctrl sd_ctrls[] = {
-#define SD_BRIGHTNESS 0
-	{
-	    {
-		.id      = V4L2_CID_BRIGHTNESS,
-		.type    = V4L2_CTRL_TYPE_INTEGER,
-		.name    = "Brightness",
-		.minimum = 0,
-		.maximum = 63,
-		.step = 1,
-#define BRIGHTNESS_DEFAULT 32
-		.default_value = BRIGHTNESS_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setbrightness,
-	    .get = sd_getbrightness,
-	},
-#define SD_CONTRAST 1
-	{
-	    {
-		.id = V4L2_CID_CONTRAST,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "contrast",
-		.minimum = 0,
-		.maximum = 20,
-		.step = 1,
-#define CONTRAST_DEFAULT 10
-		.default_value = CONTRAST_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setcontrast,
-	    .get = sd_getcontrast,
-	},
-#define SD_HUE 2
-	{
-	    {
-		.id	= V4L2_CID_HUE,
-		.type	= V4L2_CTRL_TYPE_INTEGER,
-		.name	= "Hue",
-		.minimum = 0,
-		.maximum = 127,
-		.step	= 1,
-#define HUE_DEFAULT 63
-		.default_value = HUE_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_sethue,
-	    .get = sd_gethue,
-	},
-#define SD_SHARPNESS 3
-	{
-	    {
-		.id = V4L2_CID_SHARPNESS,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Sharpness",
-		.minimum = 0,
-		.maximum = 6,
-		.step = 1,
-#define SHARPNESS_DEFAULT 3
-		.default_value = SHARPNESS_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setsharpness,
-	    .get = sd_getsharpness,
-	},
-#define SD_LIGHTING 4
-	{
-	    {
-		.id = V4L2_CID_BACKLIGHT_COMPENSATION,
-		.type = V4L2_CTRL_TYPE_INTEGER,
-		.name = "Lighting",
-		.minimum = 0,
-		.maximum = 2,
-		.step = 1,
-#define LIGHTING_DEFAULT 1
-		.default_value = LIGHTING_DEFAULT,
-		.flags = 0,
-	    },
-	    .set = sd_setlighting,
-	    .get = sd_getlighting,
-	},
-#define SD_HFLIP 5
-	{
-	    {
-		.id      = V4L2_CID_HFLIP,
-		.type    = V4L2_CTRL_TYPE_BOOLEAN,
-		.name    = "Mirror",
-		.minimum = 0,
-		.maximum = 1,
-		.step    = 1,
-#define HFLIP_DEFAULT 0
-		.default_value = HFLIP_DEFAULT,
-	    },
-	    .set = sd_sethflip,
-	    .get = sd_gethflip,
-	},
-};
-
 static const struct v4l2_pix_format cif_yuv_mode[] = {
 	{176, 144, V4L2_PIX_FMT_CIT_YYVYUY, V4L2_FIELD_NONE,
 		.bytesperline = 176,
@@ -995,56 +879,36 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	case CIT_MODEL0:
 		cam->cam_mode = model0_mode;
 		cam->nmodes = ARRAY_SIZE(model0_mode);
-		gspca_dev->ctrl_dis = ~((1 << SD_CONTRAST) | (1 << SD_HFLIP));
 		sd->sof_len = 4;
 		break;
 	case CIT_MODEL1:
 		cam->cam_mode = cif_yuv_mode;
 		cam->nmodes = ARRAY_SIZE(cif_yuv_mode);
-		gspca_dev->ctrl_dis = (1 << SD_HUE) | (1 << SD_HFLIP);
 		sd->sof_len = 4;
 		break;
 	case CIT_MODEL2:
 		cam->cam_mode = model2_mode + 1; /* no 160x120 */
 		cam->nmodes = 3;
-		gspca_dev->ctrl_dis = (1 << SD_CONTRAST) |
-				      (1 << SD_SHARPNESS) |
-				      (1 << SD_HFLIP);
 		break;
 	case CIT_MODEL3:
 		cam->cam_mode = vga_yuv_mode;
 		cam->nmodes = ARRAY_SIZE(vga_yuv_mode);
-		gspca_dev->ctrl_dis = (1 << SD_HUE) |
-				      (1 << SD_LIGHTING) |
-				      (1 << SD_HFLIP);
 		sd->stop_on_control_change = 1;
 		sd->sof_len = 4;
 		break;
 	case CIT_MODEL4:
 		cam->cam_mode = model2_mode;
 		cam->nmodes = ARRAY_SIZE(model2_mode);
-		gspca_dev->ctrl_dis = (1 << SD_CONTRAST) |
-				      (1 << SD_SHARPNESS) |
-				      (1 << SD_LIGHTING) |
-				      (1 << SD_HFLIP);
 		break;
 	case CIT_IBM_NETCAM_PRO:
 		cam->cam_mode = vga_yuv_mode;
 		cam->nmodes = 2; /* no 640 x 480 */
 		cam->input_flags = V4L2_IN_ST_VFLIP;
-		gspca_dev->ctrl_dis = ~(1 << SD_CONTRAST);
 		sd->stop_on_control_change = 1;
 		sd->sof_len = 4;
 		break;
 	}
 
-	sd->brightness = BRIGHTNESS_DEFAULT;
-	sd->contrast = CONTRAST_DEFAULT;
-	sd->hue = HUE_DEFAULT;
-	sd->sharpness = SHARPNESS_DEFAULT;
-	sd->lighting = LIGHTING_DEFAULT;
-	sd->hflip = HFLIP_DEFAULT;
-
 	return 0;
 }
 
@@ -1287,7 +1151,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	return 0;
 }
 
-static int cit_set_brightness(struct gspca_dev *gspca_dev)
+static int cit_set_brightness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int i;
@@ -1299,19 +1163,19 @@ static int cit_set_brightness(struct gspca_dev *gspca_dev)
 		break;
 	case CIT_MODEL1:
 		/* Model 1: Brightness range 0 - 63 */
-		cit_Packet_Format1(gspca_dev, 0x0031, sd->brightness);
-		cit_Packet_Format1(gspca_dev, 0x0032, sd->brightness);
-		cit_Packet_Format1(gspca_dev, 0x0033, sd->brightness);
+		cit_Packet_Format1(gspca_dev, 0x0031, val);
+		cit_Packet_Format1(gspca_dev, 0x0032, val);
+		cit_Packet_Format1(gspca_dev, 0x0033, val);
 		break;
 	case CIT_MODEL2:
 		/* Model 2: Brightness range 0x60 - 0xee */
 		/* Scale 0 - 63 to 0x60 - 0xee */
-		i = 0x60 + sd->brightness * 2254 / 1000;
+		i = 0x60 + val * 2254 / 1000;
 		cit_model2_Packet1(gspca_dev, 0x001a, i);
 		break;
 	case CIT_MODEL3:
 		/* Model 3: Brightness range 'i' in [0x0C..0x3F] */
-		i = sd->brightness;
+		i = val;
 		if (i < 0x0c)
 			i = 0x0c;
 		cit_model3_Packet1(gspca_dev, 0x0036, i);
@@ -1319,7 +1183,7 @@ static int cit_set_brightness(struct gspca_dev *gspca_dev)
 	case CIT_MODEL4:
 		/* Model 4: Brightness range 'i' in [0x04..0xb4] */
 		/* Scale 0 - 63 to 0x04 - 0xb4 */
-		i = 0x04 + sd->brightness * 2794 / 1000;
+		i = 0x04 + val * 2794 / 1000;
 		cit_model4_BrightnessPacket(gspca_dev, i);
 		break;
 	}
@@ -1327,7 +1191,7 @@ static int cit_set_brightness(struct gspca_dev *gspca_dev)
 	return 0;
 }
 
-static int cit_set_contrast(struct gspca_dev *gspca_dev)
+static int cit_set_contrast(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
@@ -1335,16 +1199,16 @@ static int cit_set_contrast(struct gspca_dev *gspca_dev)
 	case CIT_MODEL0: {
 		int i;
 		/* gain 0-15, 0-20 -> 0-15 */
-		i = sd->contrast * 1000 / 1333;
+		i = val * 1000 / 1333;
 		cit_write_reg(gspca_dev, i, 0x0422);
 		/* gain 0-31, may not be lower then 0x0422, 0-20 -> 0-31 */
-		i = sd->contrast * 2000 / 1333;
+		i = val * 2000 / 1333;
 		cit_write_reg(gspca_dev, i, 0x0423);
 		/* gain 0-127, may not be lower then 0x0423, 0-20 -> 0-63  */
-		i = sd->contrast * 4000 / 1333;
+		i = val * 4000 / 1333;
 		cit_write_reg(gspca_dev, i, 0x0424);
 		/* gain 0-127, may not be lower then 0x0424, , 0-20 -> 0-127 */
-		i = sd->contrast * 8000 / 1333;
+		i = val * 8000 / 1333;
 		cit_write_reg(gspca_dev, i, 0x0425);
 		break;
 	}
@@ -1355,7 +1219,7 @@ static int cit_set_contrast(struct gspca_dev *gspca_dev)
 	case CIT_MODEL1:
 	{
 		/* Scale 0 - 20 to 15 - 0 */
-		int i, new_contrast = (20 - sd->contrast) * 1000 / 1333;
+		int i, new_contrast = (20 - val) * 1000 / 1333;
 		for (i = 0; i < cit_model1_ntries; i++) {
 			cit_Packet_Format1(gspca_dev, 0x0014, new_contrast);
 			cit_send_FF_04_02(gspca_dev);
@@ -1377,20 +1241,20 @@ static int cit_set_contrast(struct gspca_dev *gspca_dev)
 			{ 0x01, 0x0e, 0x16 },
 			{ 0x01, 0x10, 0x16 }	/* Maximum */
 		};
-		int i = sd->contrast / 3;
+		int i = val / 3;
 		cit_model3_Packet1(gspca_dev, 0x0067, cv[i].cv1);
 		cit_model3_Packet1(gspca_dev, 0x005b, cv[i].cv2);
 		cit_model3_Packet1(gspca_dev, 0x005c, cv[i].cv3);
 		break;
 	}
 	case CIT_IBM_NETCAM_PRO:
-		cit_model3_Packet1(gspca_dev, 0x005b, sd->contrast + 1);
+		cit_model3_Packet1(gspca_dev, 0x005b, val + 1);
 		break;
 	}
 	return 0;
 }
 
-static int cit_set_hue(struct gspca_dev *gspca_dev)
+static int cit_set_hue(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
@@ -1401,7 +1265,7 @@ static int cit_set_hue(struct gspca_dev *gspca_dev)
 		/* No hue control for these models */
 		break;
 	case CIT_MODEL2:
-		cit_model2_Packet1(gspca_dev, 0x0024, sd->hue);
+		cit_model2_Packet1(gspca_dev, 0x0024, val);
 		/* cit_model2_Packet1(gspca_dev, 0x0020, sat); */
 		break;
 	case CIT_MODEL3: {
@@ -1409,7 +1273,7 @@ static int cit_set_hue(struct gspca_dev *gspca_dev)
 		/* TESTME according to the ibmcam driver this does not work */
 		if (0) {
 			/* Scale 0 - 127 to 0x05 - 0x37 */
-			int i = 0x05 + sd->hue * 1000 / 2540;
+			int i = 0x05 + val * 1000 / 2540;
 			cit_model3_Packet1(gspca_dev, 0x007e, i);
 		}
 		break;
@@ -1435,14 +1299,14 @@ static int cit_set_hue(struct gspca_dev *gspca_dev)
 		cit_write_reg(gspca_dev,    160, 0x012e);  /* Red gain */
 		cit_write_reg(gspca_dev,    160, 0x0130);  /* Blue gain */
 		cit_write_reg(gspca_dev, 0x8a28, 0x0124);
-		cit_write_reg(gspca_dev, sd->hue, 0x012d); /* Hue */
+		cit_write_reg(gspca_dev, val, 0x012d); /* Hue */
 		cit_write_reg(gspca_dev, 0xf545, 0x0124);
 		break;
 	}
 	return 0;
 }
 
-static int cit_set_sharpness(struct gspca_dev *gspca_dev)
+static int cit_set_sharpness(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
@@ -1459,7 +1323,7 @@ static int cit_set_sharpness(struct gspca_dev *gspca_dev)
 			0x11, 0x13, 0x16, 0x18, 0x1a, 0x8, 0x0a };
 
 		for (i = 0; i < cit_model1_ntries; i++)
-			cit_PacketFormat2(gspca_dev, 0x0013, sa[sd->sharpness]);
+			cit_PacketFormat2(gspca_dev, 0x0013, sa[val]);
 		break;
 	}
 	case CIT_MODEL3:
@@ -1482,10 +1346,10 @@ static int cit_set_sharpness(struct gspca_dev *gspca_dev)
 			{ 0x03, 0x06, 0x05, 0x14 },
 			{ 0x03, 0x07, 0x05, 0x14 }	/* Sharpest */
 		};
-		cit_model3_Packet1(gspca_dev, 0x0060, sv[sd->sharpness].sv1);
-		cit_model3_Packet1(gspca_dev, 0x0061, sv[sd->sharpness].sv2);
-		cit_model3_Packet1(gspca_dev, 0x0062, sv[sd->sharpness].sv3);
-		cit_model3_Packet1(gspca_dev, 0x0063, sv[sd->sharpness].sv4);
+		cit_model3_Packet1(gspca_dev, 0x0060, sv[val].sv1);
+		cit_model3_Packet1(gspca_dev, 0x0061, sv[val].sv2);
+		cit_model3_Packet1(gspca_dev, 0x0062, sv[val].sv3);
+		cit_model3_Packet1(gspca_dev, 0x0063, sv[val].sv4);
 		break;
 	}
 	}
@@ -1510,7 +1374,7 @@ static int cit_set_sharpness(struct gspca_dev *gspca_dev)
  * 1/5/00   Created.
  * 2/20/00  Added support for Model 2 cameras.
  */
-static void cit_set_lighting(struct gspca_dev *gspca_dev)
+static void cit_set_lighting(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
@@ -1524,19 +1388,19 @@ static void cit_set_lighting(struct gspca_dev *gspca_dev)
 	case CIT_MODEL1: {
 		int i;
 		for (i = 0; i < cit_model1_ntries; i++)
-			cit_Packet_Format1(gspca_dev, 0x0027, sd->lighting);
+			cit_Packet_Format1(gspca_dev, 0x0027, val);
 		break;
 	}
 	}
 }
 
-static void cit_set_hflip(struct gspca_dev *gspca_dev)
+static void cit_set_hflip(struct gspca_dev *gspca_dev, s32 val)
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 
 	switch (sd->model) {
 	case CIT_MODEL0:
-		if (sd->hflip)
+		if (val)
 			cit_write_reg(gspca_dev, 0x0020, 0x0115);
 		else
 			cit_write_reg(gspca_dev, 0x0040, 0x0115);
@@ -1831,7 +1695,8 @@ static int cit_start_model1(struct gspca_dev *gspca_dev)
 			cit_PacketFormat2(gspca_dev, 0x13, 0x1a);
 
 		/* Default lighting conditions */
-		cit_Packet_Format1(gspca_dev, 0x0027, sd->lighting);
+		cit_Packet_Format1(gspca_dev, 0x0027,
+				   v4l2_ctrl_g_ctrl(sd->lighting));
 	}
 
 	/* Assorted init */
@@ -2049,9 +1914,10 @@ static int cit_start_model2(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	/* FIXME this cannot be changed while streaming, so we
-	   should report a grabbed flag for this control. */
-	cit_model2_Packet1(gspca_dev, 0x0028, sd->lighting);
+	cit_model2_Packet1(gspca_dev, 0x0028, v4l2_ctrl_g_ctrl(sd->lighting));
+	/* model2 cannot change the backlight compensation while streaming */
+	v4l2_ctrl_grab(sd->lighting, true);
+
 	/* color balance rg2 */
 	cit_model2_Packet1(gspca_dev, 0x001e, 0x002f);
 	/* saturation */
@@ -2755,13 +2621,6 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		break;
 	}
 
-	cit_set_brightness(gspca_dev);
-	cit_set_contrast(gspca_dev);
-	cit_set_hue(gspca_dev);
-	cit_set_sharpness(gspca_dev);
-	cit_set_lighting(gspca_dev);
-	cit_set_hflip(gspca_dev);
-
 	/* Program max isoc packet size */
 	cit_write_reg(gspca_dev, packet_size >> 8, 0x0106);
 	cit_write_reg(gspca_dev, packet_size & 0xff, 0x0107);
@@ -2857,6 +2716,8 @@ static void sd_stop0(struct gspca_dev *gspca_dev)
 		cit_write_reg(gspca_dev, 0x81, 0x0100);	/* LED Off */
 		break;
 	case CIT_MODEL2:
+		v4l2_ctrl_grab(sd->lighting, false);
+		/* Fall through! */
 	case CIT_MODEL4:
 		cit_model2_Packet1(gspca_dev, 0x0030, 0x0004);
 
@@ -3055,152 +2916,6 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 	gspca_frame_add(gspca_dev, INTER_PACKET, data, len);
 }
 
-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) {
-		if (sd->stop_on_control_change)
-			sd_stopN(gspca_dev);
-		cit_set_brightness(gspca_dev);
-		if (sd->stop_on_control_change)
-			cit_restart_stream(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) {
-		if (sd->stop_on_control_change)
-			sd_stopN(gspca_dev);
-		cit_set_contrast(gspca_dev);
-		if (sd->stop_on_control_change)
-			cit_restart_stream(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_sethue(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->hue = val;
-	if (gspca_dev->streaming) {
-		if (sd->stop_on_control_change)
-			sd_stopN(gspca_dev);
-		cit_set_hue(gspca_dev);
-		if (sd->stop_on_control_change)
-			cit_restart_stream(gspca_dev);
-	}
-	return 0;
-}
-
-static int sd_gethue(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->hue;
-
-	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) {
-		if (sd->stop_on_control_change)
-			sd_stopN(gspca_dev);
-		cit_set_sharpness(gspca_dev);
-		if (sd->stop_on_control_change)
-			cit_restart_stream(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_setlighting(struct gspca_dev *gspca_dev, __s32 val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	sd->lighting = val;
-	if (gspca_dev->streaming) {
-		if (sd->stop_on_control_change)
-			sd_stopN(gspca_dev);
-		cit_set_lighting(gspca_dev);
-		if (sd->stop_on_control_change)
-			cit_restart_stream(gspca_dev);
-	}
-	return 0;
-}
-
-static int sd_getlighting(struct gspca_dev *gspca_dev, __s32 *val)
-{
-	struct sd *sd = (struct sd *) gspca_dev;
-
-	*val = sd->lighting;
-
-	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) {
-		if (sd->stop_on_control_change)
-			sd_stopN(gspca_dev);
-		cit_set_hflip(gspca_dev);
-		if (sd->stop_on_control_change)
-			cit_restart_stream(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;
-}
-
 #if defined(CONFIG_INPUT) || defined(CONFIG_INPUT_MODULE)
 static void cit_check_button(struct gspca_dev *gspca_dev)
 {
@@ -3234,13 +2949,117 @@ static void cit_check_button(struct gspca_dev *gspca_dev)
 }
 #endif
 
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	if (sd->stop_on_control_change)
+		sd_stopN(gspca_dev);
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		cit_set_brightness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		cit_set_contrast(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		cit_set_hue(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		cit_set_hflip(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_SHARPNESS:
+		cit_set_sharpness(gspca_dev, ctrl->val);
+		break;
+	case V4L2_CID_BACKLIGHT_COMPENSATION:
+		cit_set_lighting(gspca_dev, ctrl->val);
+		break;
+	}
+	if (sd->stop_on_control_change)
+		cit_restart_stream(gspca_dev);
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *)gspca_dev;
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	bool has_brightness;
+	bool has_contrast;
+	bool has_hue;
+	bool has_sharpness;
+	bool has_lighting;
+	bool has_hflip;
+
+	has_brightness = has_contrast = has_hue =
+		has_sharpness = has_hflip = has_lighting = false;
+	switch (sd->model) {
+	case CIT_MODEL0:
+		has_contrast = has_hflip = true;
+		break;
+	case CIT_MODEL1:
+		has_brightness = has_contrast =
+			has_sharpness = has_lighting = true;
+		break;
+	case CIT_MODEL2:
+		has_brightness = has_hue = has_lighting = true;
+		break;
+	case CIT_MODEL3:
+		has_brightness = has_contrast = has_sharpness = true;
+		break;
+	case CIT_MODEL4:
+		has_brightness = has_hue = true;
+		break;
+	case CIT_IBM_NETCAM_PRO:
+		has_brightness = has_hue =
+			has_sharpness = has_hflip = has_lighting = true;
+		break;
+	}
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 5);
+	if (has_brightness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 63, 1, 32);
+	if (has_contrast)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 20, 1, 10);
+	if (has_hue)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HUE, 0, 127, 1, 63);
+	if (has_sharpness)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_SHARPNESS, 0, 6, 1, 3);
+	if (has_lighting)
+		sd->lighting = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_BACKLIGHT_COMPENSATION, 0, 2, 1, 1);
+	if (has_hflip)
+		v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	if (hdl->error) {
+		pr_err("Could not initialize controls\n");
+		return hdl->error;
+	}
+	return 0;
+}
+
 /* 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,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
 	.stop0 = sd_stop0,
@@ -3253,10 +3072,9 @@ static const struct sd_desc sd_desc = {
 
 static const struct sd_desc sd_desc_isoc_nego = {
 	.name = MODULE_NAME,
-	.ctrls = sd_ctrls,
-	.nctrls = ARRAY_SIZE(sd_ctrls),
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.isoc_init = sd_isoc_init,
 	.isoc_nego = sd_isoc_nego,
@@ -3320,6 +3138,7 @@ static struct usb_driver sd_driver = {
 #ifdef CONFIG_PM
 	.suspend = gspca_suspend,
 	.resume = gspca_resume,
+	.reset_resume = gspca_resume,
 #endif
 };
 
diff --git a/drivers/media/video/ibmmpeg2.h b/drivers/media/video/ibmmpeg2.h
deleted file mode 100644
index 68e10387c49..00000000000
--- a/drivers/media/video/ibmmpeg2.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/* ibmmpeg2.h - IBM MPEGCD21 definitions */
-
-#ifndef __IBM_MPEG2__
-#define __IBM_MPEG2__
-
-/* Define all MPEG Decoder registers */
-/* Chip Control and Status */
-#define IBM_MP2_CHIP_CONTROL	0x200*2
-#define IBM_MP2_CHIP_MODE	0x201*2
-/* Timer Control and Status */
-#define IBM_MP2_SYNC_STC2	0x202*2
-#define IBM_MP2_SYNC_STC1	0x203*2
-#define IBM_MP2_SYNC_STC0	0x204*2
-#define IBM_MP2_SYNC_PTS2	0x205*2
-#define IBM_MP2_SYNC_PTS1	0x206*2
-#define IBM_MP2_SYNC_PTS0	0x207*2
-/* Video FIFO Control */
-#define IBM_MP2_FIFO		0x208*2
-#define IBM_MP2_FIFOW		0x100*2
-#define IBM_MP2_FIFO_STAT	0x209*2
-#define IBM_MP2_RB_THRESHOLD	0x22b*2
-/* Command buffer */
-#define IBM_MP2_COMMAND		0x20a*2
-#define IBM_MP2_CMD_DATA	0x20b*2
-#define IBM_MP2_CMD_STAT	0x20c*2
-#define IBM_MP2_CMD_ADDR	0x20d*2
-/* Internal Processor Control and Status */
-#define IBM_MP2_PROC_IADDR	0x20e*2
-#define IBM_MP2_PROC_IDATA	0x20f*2
-#define IBM_MP2_WR_PROT		0x235*2
-/* DRAM Access */
-#define IBM_MP2_DRAM_ADDR	0x210*2
-#define IBM_MP2_DRAM_DATA	0x212*2
-#define IBM_MP2_DRAM_CMD_STAT	0x213*2
-#define IBM_MP2_BLOCK_SIZE	0x23b*2
-#define IBM_MP2_SRC_ADDR	0x23c*2
-/* Onscreen Display */
-#define IBM_MP2_OSD_ADDR	0x214*2
-#define IBM_MP2_OSD_DATA	0x215*2
-#define IBM_MP2_OSD_MODE	0x217*2
-#define IBM_MP2_OSD_LINK_ADDR	0x229*2
-#define IBM_MP2_OSD_SIZE	0x22a*2
-/* Interrupt Control */
-#define IBM_MP2_HOST_INT	0x218*2
-#define IBM_MP2_MASK0		0x219*2
-#define IBM_MP2_HOST_INT1	0x23e*2
-#define IBM_MP2_MASK1		0x23f*2
-/* Audio Control */
-#define IBM_MP2_AUD_IADDR	0x21a*2
-#define IBM_MP2_AUD_IDATA	0x21b*2
-#define IBM_MP2_AUD_FIFO	0x21c*2
-#define IBM_MP2_AUD_FIFOW	0x101*2
-#define IBM_MP2_AUD_CTL		0x21d*2
-#define IBM_MP2_BEEP_CTL	0x21e*2
-#define IBM_MP2_FRNT_ATTEN	0x22d*2
-/* Display Control */
-#define IBM_MP2_DISP_MODE	0x220*2
-#define IBM_MP2_DISP_DLY	0x221*2
-#define IBM_MP2_VBI_CTL		0x222*2
-#define IBM_MP2_DISP_LBOR	0x223*2
-#define IBM_MP2_DISP_TBOR	0x224*2
-/* Polarity Control */
-#define IBM_MP2_INFC_CTL	0x22c*2
-
-/* control commands */
-#define IBM_MP2_PLAY		0
-#define IBM_MP2_PAUSE		1
-#define IBM_MP2_SINGLE_FRAME	2
-#define IBM_MP2_FAST_FORWARD	3
-#define IBM_MP2_SLOW_MOTION	4
-#define IBM_MP2_IMED_NORM_PLAY	5
-#define IBM_MP2_RESET_WINDOW	6
-#define IBM_MP2_FREEZE_FRAME	7
-#define IBM_MP2_RESET_VID_RATE	8
-#define IBM_MP2_CONFIG_DECODER	9
-#define IBM_MP2_CHANNEL_SWITCH	10
-#define IBM_MP2_RESET_AUD_RATE	11
-#define IBM_MP2_PRE_OP_CHN_SW	12
-#define IBM_MP2_SET_STILL_MODE	14
-
-/* Define Xilinx FPGA Internal Registers */
-
-/* general control register 0 */
-#define XILINX_CTL0		0x600
-/* genlock delay resister 1 */
-#define XILINX_GLDELAY		0x602
-/* send 16 bits to CS3310 port */
-#define XILINX_CS3310		0x604
-/* send 16 bits to CS3310 and complete */
-#define XILINX_CS3310_CMPLT	0x60c
-/* pulse width modulator control */
-#define XILINX_PWM		0x606
-
-#endif
diff --git a/drivers/media/video/ivtv/ivtv-ioctl.c b/drivers/media/video/ivtv/ivtv-ioctl.c
index f7d57b3f284..32a591062d0 100644
--- a/drivers/media/video/ivtv/ivtv-ioctl.c
+++ b/drivers/media/video/ivtv/ivtv-ioctl.c
@@ -1830,18 +1830,6 @@ static long ivtv_default(struct file *file, void *fh, bool valid_prio,
 	return 0;
 }
 
-long ivtv_v4l2_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
-{
-	struct video_device *vfd = video_devdata(filp);
-	long ret;
-
-	if (ivtv_debug & IVTV_DBGFLG_IOCTL)
-		vfd->debug = V4L2_DEBUG_IOCTL | V4L2_DEBUG_IOCTL_ARG;
-	ret = video_ioctl2(filp, cmd, arg);
-	vfd->debug = 0;
-	return ret;
-}
-
 static const struct v4l2_ioctl_ops ivtv_ioctl_ops = {
 	.vidioc_querycap    		    = ivtv_querycap,
 	.vidioc_s_audio     		    = ivtv_s_audio,
diff --git a/drivers/media/video/ivtv/ivtv-ioctl.h b/drivers/media/video/ivtv/ivtv-ioctl.h
index 89185caeafa..7c553d16579 100644
--- a/drivers/media/video/ivtv/ivtv-ioctl.h
+++ b/drivers/media/video/ivtv/ivtv-ioctl.h
@@ -31,6 +31,5 @@ void ivtv_s_std_enc(struct ivtv *itv, v4l2_std_id *std);
 void ivtv_s_std_dec(struct ivtv *itv, v4l2_std_id *std);
 int ivtv_s_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
 int ivtv_s_input(struct file *file, void *fh, unsigned int inp);
-long ivtv_v4l2_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
 
 #endif
diff --git a/drivers/media/video/ivtv/ivtv-streams.c b/drivers/media/video/ivtv/ivtv-streams.c
index 6738592aa35..87990c5f091 100644
--- a/drivers/media/video/ivtv/ivtv-streams.c
+++ b/drivers/media/video/ivtv/ivtv-streams.c
@@ -50,7 +50,7 @@ static const struct v4l2_file_operations ivtv_v4l2_enc_fops = {
 	.read = ivtv_v4l2_read,
 	.write = ivtv_v4l2_write,
 	.open = ivtv_v4l2_open,
-	.unlocked_ioctl = ivtv_v4l2_ioctl,
+	.unlocked_ioctl = video_ioctl2,
 	.release = ivtv_v4l2_close,
 	.poll = ivtv_v4l2_enc_poll,
 };
@@ -60,7 +60,7 @@ static const struct v4l2_file_operations ivtv_v4l2_dec_fops = {
 	.read = ivtv_v4l2_read,
 	.write = ivtv_v4l2_write,
 	.open = ivtv_v4l2_open,
-	.unlocked_ioctl = ivtv_v4l2_ioctl,
+	.unlocked_ioctl = video_ioctl2,
 	.release = ivtv_v4l2_close,
 	.poll = ivtv_v4l2_dec_poll,
 };
diff --git a/drivers/media/video/m5mols/Kconfig b/drivers/media/video/m5mols/Kconfig
index 302dc3d7019..dc8c2505907 100644
--- a/drivers/media/video/m5mols/Kconfig
+++ b/drivers/media/video/m5mols/Kconfig
@@ -1,5 +1,6 @@
 config VIDEO_M5MOLS
 	tristate "Fujitsu M-5MOLS 8MP sensor support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on MEDIA_CAMERA_SUPPORT
 	---help---
 	  This driver supports Fujitsu M-5MOLS camera sensor with ISP
diff --git a/drivers/media/video/m5mols/m5mols_controls.c b/drivers/media/video/m5mols/m5mols_controls.c
index 392a028730e..fdbc205a296 100644
--- a/drivers/media/video/m5mols/m5mols_controls.c
+++ b/drivers/media/video/m5mols/m5mols_controls.c
@@ -527,8 +527,8 @@ static const struct v4l2_ctrl_ops m5mols_ctrl_ops = {
 
 /* Supported manual ISO values */
 static const s64 iso_qmenu[] = {
-	/* AE_ISO: 0x01...0x07 */
-	50, 100, 200, 400, 800, 1600, 3200
+	/* AE_ISO: 0x01...0x07 (ISO: 50...3200) */
+	50000, 100000, 200000, 400000, 800000, 1600000, 3200000
 };
 
 /* Supported Exposure Bias values, -2.0EV...+2.0EV */
diff --git a/drivers/media/video/mem2mem_testdev.c b/drivers/media/video/mem2mem_testdev.c
index 3945556f573..7efe9ad7acc 100644
--- a/drivers/media/video/mem2mem_testdev.c
+++ b/drivers/media/video/mem2mem_testdev.c
@@ -27,6 +27,8 @@
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/videobuf2-vmalloc.h>
 
 #define MEM2MEM_TEST_MODULE_NAME "mem2mem-testdev"
@@ -60,6 +62,10 @@ MODULE_VERSION("0.1.1");
 #define MEM2MEM_COLOR_STEP	(0xff >> 4)
 #define MEM2MEM_NUM_TILES	8
 
+/* Flags that indicate processing mode */
+#define MEM2MEM_HFLIP	(1 << 0)
+#define MEM2MEM_VFLIP	(1 << 1)
+
 #define dprintk(dev, fmt, arg...) \
 	v4l2_dbg(1, 1, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
 
@@ -97,6 +103,8 @@ static struct m2mtest_fmt formats[] = {
 	},
 };
 
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
 /* Per-queue, driver-specific private data */
 struct m2mtest_q_data {
 	unsigned int		width;
@@ -110,32 +118,8 @@ enum {
 	V4L2_M2M_DST = 1,
 };
 
-#define V4L2_CID_TRANS_TIME_MSEC	V4L2_CID_PRIVATE_BASE
-#define V4L2_CID_TRANS_NUM_BUFS		(V4L2_CID_PRIVATE_BASE + 1)
-
-static struct v4l2_queryctrl m2mtest_ctrls[] = {
-	{
-		.id		= V4L2_CID_TRANS_TIME_MSEC,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.name		= "Transaction time (msec)",
-		.minimum	= 1,
-		.maximum	= 10000,
-		.step		= 100,
-		.default_value	= 1000,
-		.flags		= 0,
-	}, {
-		.id		= V4L2_CID_TRANS_NUM_BUFS,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.name		= "Buffers per transaction",
-		.minimum	= 1,
-		.maximum	= MEM2MEM_DEF_NUM_BUFS,
-		.step		= 1,
-		.default_value	= 1,
-		.flags		= 0,
-	},
-};
-
-#define NUM_FORMATS ARRAY_SIZE(formats)
+#define V4L2_CID_TRANS_TIME_MSEC	(V4L2_CID_USER_BASE + 0x1000)
+#define V4L2_CID_TRANS_NUM_BUFS		(V4L2_CID_USER_BASE + 0x1001)
 
 static struct m2mtest_fmt *find_format(struct v4l2_format *f)
 {
@@ -168,8 +152,11 @@ struct m2mtest_dev {
 };
 
 struct m2mtest_ctx {
+	struct v4l2_fh		fh;
 	struct m2mtest_dev	*dev;
 
+	struct v4l2_ctrl_handler hdl;
+
 	/* Processed buffers in this transaction */
 	u8			num_processed;
 
@@ -181,12 +168,22 @@ struct m2mtest_ctx {
 	/* Abort requested by m2m */
 	int			aborting;
 
+	/* Processing mode */
+	int			mode;
+
+	enum v4l2_colorspace	colorspace;
+
 	struct v4l2_m2m_ctx	*m2m_ctx;
 
 	/* Source and destination queue data */
 	struct m2mtest_q_data   q_data[2];
 };
 
+static inline struct m2mtest_ctx *file2ctx(struct file *file)
+{
+	return container_of(file->private_data, struct m2mtest_ctx, fh);
+}
+
 static struct m2mtest_q_data *get_q_data(struct m2mtest_ctx *ctx,
 					 enum v4l2_buf_type type)
 {
@@ -202,18 +199,6 @@ static struct m2mtest_q_data *get_q_data(struct m2mtest_ctx *ctx,
 }
 
 
-static struct v4l2_queryctrl *get_ctrl(int id)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(m2mtest_ctrls); ++i) {
-		if (id == m2mtest_ctrls[i].id)
-			return &m2mtest_ctrls[i];
-	}
-
-	return NULL;
-}
-
 static int device_process(struct m2mtest_ctx *ctx,
 			  struct vb2_buffer *in_vb,
 			  struct vb2_buffer *out_vb)
@@ -249,19 +234,84 @@ static int device_process(struct m2mtest_ctx *ctx,
 	bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
 	w = 0;
 
-	for (y = 0; y < height; ++y) {
-		for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
-			if (w & 0x1) {
-				for (x = 0; x < tile_w; ++x)
-					*p_out++ = *p_in++ + MEM2MEM_COLOR_STEP;
-			} else {
-				for (x = 0; x < tile_w; ++x)
-					*p_out++ = *p_in++ - MEM2MEM_COLOR_STEP;
+	switch (ctx->mode) {
+	case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
+		p_out += bytesperline * height - bytes_left;
+		for (y = 0; y < height; ++y) {
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x1) {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
 			}
-			++w;
+			p_in += bytes_left;
+			p_out -= bytes_left;
+		}
+		break;
+
+	case MEM2MEM_HFLIP:
+		for (y = 0; y < height; ++y) {
+			p_out += MEM2MEM_NUM_TILES * tile_w;
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x01) {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out += bytesperline;
+		}
+		break;
+
+	case MEM2MEM_VFLIP:
+		p_out += bytesperline * (height - 1);
+		for (y = 0; y < height; ++y) {
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x1) {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out += bytes_left - 2 * bytesperline;
+		}
+		break;
+
+	default:
+		for (y = 0; y < height; ++y) {
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x1) {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out += bytes_left;
 		}
-		p_in += bytes_left;
-		p_out += bytes_left;
 	}
 
 	return 0;
@@ -380,10 +430,9 @@ static int vidioc_querycap(struct file *file, void *priv,
 {
 	strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
 	strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
-	cap->bus_info[0] = 0;
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
-			  | V4L2_CAP_STREAMING;
-
+	strlcpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->bus_info));
+	cap->capabilities = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -446,6 +495,7 @@ static int vidioc_g_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 	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;
+	f->fmt.pix.colorspace	= ctx->colorspace;
 
 	return 0;
 }
@@ -453,13 +503,13 @@ static int vidioc_g_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	return vidioc_g_fmt(priv, f);
+	return vidioc_g_fmt(file2ctx(file), f);
 }
 
 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	return vidioc_g_fmt(priv, f);
+	return vidioc_g_fmt(file2ctx(file), f);
 }
 
 static int vidioc_try_fmt(struct v4l2_format *f, struct m2mtest_fmt *fmt)
@@ -498,7 +548,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
 	struct m2mtest_fmt *fmt;
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	fmt = find_format(f);
 	if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
@@ -507,6 +557,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 			 f->fmt.pix.pixelformat);
 		return -EINVAL;
 	}
+	f->fmt.pix.colorspace = ctx->colorspace;
 
 	return vidioc_try_fmt(f, fmt);
 }
@@ -515,7 +566,7 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
 	struct m2mtest_fmt *fmt;
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	fmt = find_format(f);
 	if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
@@ -524,6 +575,8 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 			 f->fmt.pix.pixelformat);
 		return -EINVAL;
 	}
+	if (!f->fmt.pix.colorspace)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
 
 	return vidioc_try_fmt(f, fmt);
 }
@@ -568,25 +621,29 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	if (ret)
 		return ret;
 
-	return vidioc_s_fmt(priv, f);
+	return vidioc_s_fmt(file2ctx(file), f);
 }
 
 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
+	struct m2mtest_ctx *ctx = file2ctx(file);
 	int ret;
 
 	ret = vidioc_try_fmt_vid_out(file, priv, f);
 	if (ret)
 		return ret;
 
-	return vidioc_s_fmt(priv, f);
+	ret = vidioc_s_fmt(file2ctx(file), f);
+	if (!ret)
+		ctx->colorspace = f->fmt.pix.colorspace;
+	return ret;
 }
 
 static int vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *reqbufs)
 {
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
 }
@@ -594,21 +651,21 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 static int vidioc_querybuf(struct file *file, void *priv,
 			   struct v4l2_buffer *buf)
 {
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
 }
 
 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 {
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 }
 
 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 {
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 }
@@ -616,7 +673,7 @@ static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 static int vidioc_streamon(struct file *file, void *priv,
 			   enum v4l2_buf_type type)
 {
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 }
@@ -624,79 +681,37 @@ static int vidioc_streamon(struct file *file, void *priv,
 static int vidioc_streamoff(struct file *file, void *priv,
 			    enum v4l2_buf_type type)
 {
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-			    struct v4l2_queryctrl *qc)
+static int m2mtest_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct v4l2_queryctrl *c;
-
-	c = get_ctrl(qc->id);
-	if (!c)
-		return -EINVAL;
-
-	*qc = *c;
-	return 0;
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct m2mtest_ctx *ctx = priv;
+	struct m2mtest_ctx *ctx =
+		container_of(ctrl->handler, struct m2mtest_ctx, hdl);
 
 	switch (ctrl->id) {
-	case V4L2_CID_TRANS_TIME_MSEC:
-		ctrl->value = ctx->transtime;
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			ctx->mode |= MEM2MEM_HFLIP;
+		else
+			ctx->mode &= ~MEM2MEM_HFLIP;
 		break;
 
-	case V4L2_CID_TRANS_NUM_BUFS:
-		ctrl->value = ctx->translen;
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			ctx->mode |= MEM2MEM_VFLIP;
+		else
+			ctx->mode &= ~MEM2MEM_VFLIP;
 		break;
 
-	default:
-		v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int check_ctrl_val(struct m2mtest_ctx *ctx, struct v4l2_control *ctrl)
-{
-	struct v4l2_queryctrl *c;
-
-	c = get_ctrl(ctrl->id);
-	if (!c)
-		return -EINVAL;
-
-	if (ctrl->value < c->minimum || ctrl->value > c->maximum) {
-		v4l2_err(&ctx->dev->v4l2_dev, "Value out of range\n");
-		return -ERANGE;
-	}
-
-	return 0;
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-			 struct v4l2_control *ctrl)
-{
-	struct m2mtest_ctx *ctx = priv;
-	int ret = 0;
-
-	ret = check_ctrl_val(ctx, ctrl);
-	if (ret != 0)
-		return ret;
-
-	switch (ctrl->id) {
 	case V4L2_CID_TRANS_TIME_MSEC:
-		ctx->transtime = ctrl->value;
+		ctx->transtime = ctrl->val;
 		break;
 
 	case V4L2_CID_TRANS_NUM_BUFS:
-		ctx->translen = ctrl->value;
+		ctx->translen = ctrl->val;
 		break;
 
 	default:
@@ -707,6 +722,10 @@ static int vidioc_s_ctrl(struct file *file, void *priv,
 	return 0;
 }
 
+static const struct v4l2_ctrl_ops m2mtest_ctrl_ops = {
+	.s_ctrl = m2mtest_s_ctrl,
+};
+
 
 static const struct v4l2_ioctl_ops m2mtest_ioctl_ops = {
 	.vidioc_querycap	= vidioc_querycap,
@@ -729,10 +748,8 @@ static const struct v4l2_ioctl_ops m2mtest_ioctl_ops = {
 
 	.vidioc_streamon	= vidioc_streamon,
 	.vidioc_streamoff	= vidioc_streamoff,
-
-	.vidioc_queryctrl	= vidioc_queryctrl,
-	.vidioc_g_ctrl		= vidioc_g_ctrl,
-	.vidioc_s_ctrl		= vidioc_s_ctrl,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 
@@ -844,6 +861,28 @@ static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *ds
 	return vb2_queue_init(dst_vq);
 }
 
+static const struct v4l2_ctrl_config m2mtest_ctrl_trans_time_msec = {
+	.ops = &m2mtest_ctrl_ops,
+	.id = V4L2_CID_TRANS_TIME_MSEC,
+	.name = "Transaction Time (msec)",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.def = 1001,
+	.min = 1,
+	.max = 10001,
+	.step = 100,
+};
+
+static const struct v4l2_ctrl_config m2mtest_ctrl_trans_num_bufs = {
+	.ops = &m2mtest_ctrl_ops,
+	.id = V4L2_CID_TRANS_NUM_BUFS,
+	.name = "Buffers Per Transaction",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.def = 1,
+	.min = 1,
+	.max = MEM2MEM_DEF_NUM_BUFS,
+	.step = 1,
+};
+
 /*
  * File operations
  */
@@ -851,29 +890,51 @@ static int m2mtest_open(struct file *file)
 {
 	struct m2mtest_dev *dev = video_drvdata(file);
 	struct m2mtest_ctx *ctx = NULL;
+	struct v4l2_ctrl_handler *hdl;
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
 	if (!ctx)
 		return -ENOMEM;
 
-	file->private_data = ctx;
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
 	ctx->dev = dev;
-	ctx->translen = MEM2MEM_DEF_TRANSLEN;
-	ctx->transtime = MEM2MEM_DEF_TRANSTIME;
-	ctx->num_processed = 0;
+	hdl = &ctx->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &m2mtest_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &m2mtest_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_custom(hdl, &m2mtest_ctrl_trans_time_msec, NULL);
+	v4l2_ctrl_new_custom(hdl, &m2mtest_ctrl_trans_num_bufs, NULL);
+	if (hdl->error) {
+		int err = hdl->error;
+
+		v4l2_ctrl_handler_free(hdl);
+		return err;
+	}
+	ctx->fh.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(hdl);
 
 	ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
-	ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
+	ctx->q_data[V4L2_M2M_SRC].width = 640;
+	ctx->q_data[V4L2_M2M_SRC].height = 480;
+	ctx->q_data[V4L2_M2M_SRC].sizeimage =
+		ctx->q_data[V4L2_M2M_SRC].width *
+		ctx->q_data[V4L2_M2M_SRC].height *
+		(ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
+	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
 
 	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);
 
+		v4l2_ctrl_handler_free(hdl);
 		kfree(ctx);
 		return ret;
 	}
 
+	v4l2_fh_add(&ctx->fh);
 	atomic_inc(&dev->num_inst);
 
 	dprintk(dev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
@@ -884,10 +945,13 @@ static int m2mtest_open(struct file *file)
 static int m2mtest_release(struct file *file)
 {
 	struct m2mtest_dev *dev = video_drvdata(file);
-	struct m2mtest_ctx *ctx = file->private_data;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	dprintk(dev, "Releasing instance %p\n", ctx);
 
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	v4l2_ctrl_handler_free(&ctx->hdl);
 	v4l2_m2m_ctx_release(ctx->m2m_ctx);
 	kfree(ctx);
 
@@ -899,14 +963,14 @@ static int m2mtest_release(struct file *file)
 static unsigned int m2mtest_poll(struct file *file,
 				 struct poll_table_struct *wait)
 {
-	struct m2mtest_ctx *ctx = file->private_data;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
 }
 
 static int m2mtest_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	struct m2mtest_ctx *ctx = file->private_data;
+	struct m2mtest_ctx *ctx = file2ctx(file);
 
 	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
 }
diff --git a/drivers/media/video/mt9m001.c b/drivers/media/video/mt9m001.c
index 7e648183f15..00583f5fd26 100644
--- a/drivers/media/video/mt9m001.c
+++ b/drivers/media/video/mt9m001.c
@@ -22,7 +22,7 @@
 
 /*
  * mt9m001 i2c address 0x5d
- * The platform has to define ctruct i2c_board_info objects and link to them
+ * The platform has to define struct i2c_board_info objects and link to them
  * from struct soc_camera_link
  */
 
diff --git a/drivers/media/video/mt9m032.c b/drivers/media/video/mt9m032.c
index 3c1e626139b..445359c9611 100644
--- a/drivers/media/video/mt9m032.c
+++ b/drivers/media/video/mt9m032.c
@@ -688,11 +688,17 @@ static const struct v4l2_subdev_ops mt9m032_ops = {
 static int mt9m032_probe(struct i2c_client *client,
 			 const struct i2c_device_id *devid)
 {
+	struct mt9m032_platform_data *pdata = client->dev.platform_data;
 	struct i2c_adapter *adapter = client->adapter;
 	struct mt9m032 *sensor;
 	int chip_version;
 	int ret;
 
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 		dev_warn(&client->dev,
 			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
@@ -708,7 +714,7 @@ static int mt9m032_probe(struct i2c_client *client,
 
 	mutex_init(&sensor->lock);
 
-	sensor->pdata = client->dev.platform_data;
+	sensor->pdata = pdata;
 
 	v4l2_i2c_subdev_init(&sensor->subdev, client, &mt9m032_ops);
 	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
@@ -738,7 +744,7 @@ static int mt9m032_probe(struct i2c_client *client,
 	sensor->format.field = V4L2_FIELD_NONE;
 	sensor->format.colorspace = V4L2_COLORSPACE_SRGB;
 
-	v4l2_ctrl_handler_init(&sensor->ctrls, 4);
+	v4l2_ctrl_handler_init(&sensor->ctrls, 5);
 
 	v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
 			  V4L2_CID_GAIN, 0, 127, 1, 64);
@@ -754,6 +760,9 @@ static int mt9m032_probe(struct i2c_client *client,
 			  V4L2_CID_EXPOSURE, MT9M032_SHUTTER_WIDTH_MIN,
 			  MT9M032_SHUTTER_WIDTH_MAX, 1,
 			  MT9M032_SHUTTER_WIDTH_DEF);
+	v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pdata->pix_clock,
+			  pdata->pix_clock, 1, pdata->pix_clock);
 
 	if (sensor->ctrls.error) {
 		ret = sensor->ctrls.error;
diff --git a/drivers/media/video/mt9m111.c b/drivers/media/video/mt9m111.c
index b0c52996432..863d722dda0 100644
--- a/drivers/media/video/mt9m111.c
+++ b/drivers/media/video/mt9m111.c
@@ -214,7 +214,6 @@ struct mt9m111 {
 	int power_count;
 	const struct mt9m111_datafmt *fmt;
 	int lastpage;	/* PageMap cache value */
-	unsigned char datawidth;
 };
 
 /* Find a data format by a pixel code */
diff --git a/drivers/media/video/mt9p031.c b/drivers/media/video/mt9p031.c
index 8f061d9ac44..3be537ef22d 100644
--- a/drivers/media/video/mt9p031.c
+++ b/drivers/media/video/mt9p031.c
@@ -950,7 +950,7 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->model = did->driver_data;
 	mt9p031->reset = -1;
 
-	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 4);
+	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 5);
 
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 			  V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
@@ -963,6 +963,9 @@ static int mt9p031_probe(struct i2c_client *client,
 			  V4L2_CID_HFLIP, 0, 1, 1, 0);
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 			  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pdata->target_freq,
+			  pdata->target_freq, 1, pdata->target_freq);
 
 	for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
 		v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
diff --git a/drivers/media/video/mt9t001.c b/drivers/media/video/mt9t001.c
index 49ca3cbfc6f..6d343adf891 100644
--- a/drivers/media/video/mt9t001.c
+++ b/drivers/media/video/mt9t001.c
@@ -691,7 +691,7 @@ static int mt9t001_video_probe(struct i2c_client *client)
 		return ret;
 
 	/* Configure the pixel clock polarity */
-	if (pdata && pdata->clk_pol) {
+	if (pdata->clk_pol) {
 		ret  = mt9t001_write(client, MT9T001_PIXEL_CLOCK,
 				     MT9T001_PIXEL_CLOCK_INVERT);
 		if (ret < 0)
@@ -715,10 +715,16 @@ static int mt9t001_video_probe(struct i2c_client *client)
 static int mt9t001_probe(struct i2c_client *client,
 			 const struct i2c_device_id *did)
 {
+	struct mt9t001_platform_data *pdata = client->dev.platform_data;
 	struct mt9t001 *mt9t001;
 	unsigned int i;
 	int ret;
 
+	if (pdata == NULL) {
+		dev_err(&client->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
 	if (!i2c_check_functionality(client->adapter,
 				     I2C_FUNC_SMBUS_WORD_DATA)) {
 		dev_warn(&client->adapter->dev,
@@ -735,7 +741,7 @@ static int mt9t001_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	v4l2_ctrl_handler_init(&mt9t001->ctrls, ARRAY_SIZE(mt9t001_ctrls) +
-						ARRAY_SIZE(mt9t001_gains) + 2);
+						ARRAY_SIZE(mt9t001_gains) + 3);
 
 	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 			  V4L2_CID_EXPOSURE, MT9T001_SHUTTER_WIDTH_MIN,
@@ -743,6 +749,9 @@ static int mt9t001_probe(struct i2c_client *client,
 			  MT9T001_SHUTTER_WIDTH_DEF);
 	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 			  V4L2_CID_BLACK_LEVEL, 1, 1, 1, 1);
+	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pdata->ext_clk, pdata->ext_clk,
+			  1, pdata->ext_clk);
 
 	for (i = 0; i < ARRAY_SIZE(mt9t001_ctrls); ++i)
 		v4l2_ctrl_new_custom(&mt9t001->ctrls, &mt9t001_ctrls[i], NULL);
diff --git a/drivers/media/video/mt9v022.c b/drivers/media/video/mt9v022.c
index bf63417adb8..72479247522 100644
--- a/drivers/media/video/mt9v022.c
+++ b/drivers/media/video/mt9v022.c
@@ -23,7 +23,7 @@
 
 /*
  * mt9v022 i2c address 0x48, 0x4c, 0x58, 0x5c
- * The platform has to define ctruct i2c_board_info objects and link to them
+ * The platform has to define struct i2c_board_info objects and link to them
  * from struct soc_camera_link
  */
 
diff --git a/drivers/media/video/mx1_camera.c b/drivers/media/video/mx1_camera.c
index 4296a835029..d2e6f82ecfa 100644
--- a/drivers/media/video/mx1_camera.c
+++ b/drivers/media/video/mx1_camera.c
@@ -43,6 +43,7 @@
 #include <asm/fiq.h>
 #include <mach/dma-mx1-mx2.h>
 #include <mach/hardware.h>
+#include <mach/irqs.h>
 #include <mach/mx1_camera.h>
 
 /*
diff --git a/drivers/media/video/mx2_emmaprp.c b/drivers/media/video/mx2_emmaprp.c
index 0bd5815de36..5f8a6f5b98f 100644
--- a/drivers/media/video/mx2_emmaprp.c
+++ b/drivers/media/video/mx2_emmaprp.c
@@ -396,9 +396,13 @@ static int vidioc_querycap(struct file *file, void *priv,
 {
 	strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
 	strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
-			  | V4L2_CAP_STREAMING;
-
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
+			    V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
 	return 0;
 }
 
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index 7e32331b60f..f1220d3d497 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -2014,7 +2014,7 @@ static int ccdc_get_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 		return -EINVAL;
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = INT_MAX;
@@ -2024,7 +2024,7 @@ static int ccdc_get_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 		ccdc_try_crop(ccdc, format, &sel->r);
 		break;
 
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		sel->r = *__ccdc_get_crop(ccdc, fh, sel->which);
 		break;
 
@@ -2052,7 +2052,7 @@ static int ccdc_set_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
 
-	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	if (sel->target != V4L2_SEL_TGT_CROP ||
 	    sel->pad != CCDC_PAD_SOURCE_OF)
 		return -EINVAL;
 
@@ -2064,7 +2064,7 @@ static int ccdc_set_selection(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
 	 * rectangle.
 	 */
-	if (sel->flags & V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG) {
+	if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
 		sel->r = *__ccdc_get_crop(ccdc, fh, sel->which);
 		return 0;
 	}
diff --git a/drivers/media/video/omap3isp/isppreview.c b/drivers/media/video/omap3isp/isppreview.c
index dd91da26f1b..53f5a703e31 100644
--- a/drivers/media/video/omap3isp/isppreview.c
+++ b/drivers/media/video/omap3isp/isppreview.c
@@ -1949,7 +1949,7 @@ static int preview_get_selection(struct v4l2_subdev *sd,
 		return -EINVAL;
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = INT_MAX;
@@ -1960,7 +1960,7 @@ static int preview_get_selection(struct v4l2_subdev *sd,
 		preview_try_crop(prev, format, &sel->r);
 		break;
 
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		sel->r = *__preview_get_crop(prev, fh, sel->which);
 		break;
 
@@ -1988,7 +1988,7 @@ static int preview_set_selection(struct v4l2_subdev *sd,
 	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
 
-	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	if (sel->target != V4L2_SEL_TGT_CROP ||
 	    sel->pad != PREV_PAD_SINK)
 		return -EINVAL;
 
@@ -2000,7 +2000,7 @@ static int preview_set_selection(struct v4l2_subdev *sd,
 	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
 	 * rectangle.
 	 */
-	if (sel->flags & V4L2_SUBDEV_SEL_FLAG_KEEP_CONFIG) {
+	if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
 		sel->r = *__preview_get_crop(prev, fh, sel->which);
 		return 0;
 	}
diff --git a/drivers/media/video/omap3isp/ispresizer.c b/drivers/media/video/omap3isp/ispresizer.c
index 14041c9c864..ae17d917f77 100644
--- a/drivers/media/video/omap3isp/ispresizer.c
+++ b/drivers/media/video/omap3isp/ispresizer.c
@@ -1249,7 +1249,7 @@ static int resizer_get_selection(struct v4l2_subdev *sd,
 					     sel->which);
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = INT_MAX;
@@ -1259,7 +1259,7 @@ static int resizer_get_selection(struct v4l2_subdev *sd,
 		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
 		break;
 
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		sel->r = *__resizer_get_crop(res, fh, sel->which);
 		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
 		break;
@@ -1293,7 +1293,7 @@ static int resizer_set_selection(struct v4l2_subdev *sd,
 	struct v4l2_mbus_framefmt *format_sink, *format_source;
 	struct resizer_ratio ratio;
 
-	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
+	if (sel->target != V4L2_SEL_TGT_CROP ||
 	    sel->pad != RESZ_PAD_SINK)
 		return -EINVAL;
 
diff --git a/drivers/media/video/ov2640.c b/drivers/media/video/ov2640.c
index 3c2c5d3bcc6..7c44d1fe3c8 100644
--- a/drivers/media/video/ov2640.c
+++ b/drivers/media/video/ov2640.c
@@ -837,10 +837,8 @@ static int ov2640_g_fmt(struct v4l2_subdev *sd,
 
 	if (!priv->win) {
 		u32 width = W_SVGA, height = H_SVGA;
-		int ret = ov2640_set_params(client, &width, &height,
-					    V4L2_MBUS_FMT_UYVY8_2X8);
-		if (ret < 0)
-			return ret;
+		priv->win = ov2640_select_win(&width, &height);
+		priv->cfmt_code = V4L2_MBUS_FMT_UYVY8_2X8;
 	}
 
 	mf->width	= priv->win->width;
diff --git a/drivers/media/video/ov772x.c b/drivers/media/video/ov772x.c
index 74e77d327ed..6d79b89b860 100644
--- a/drivers/media/video/ov772x.c
+++ b/drivers/media/video/ov772x.c
@@ -880,15 +880,11 @@ static int ov772x_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 static int ov772x_g_fmt(struct v4l2_subdev *sd,
 			struct v4l2_mbus_framefmt *mf)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov772x_priv *priv = container_of(sd, struct ov772x_priv, subdev);
 
 	if (!priv->win || !priv->cfmt) {
-		u32 width = VGA_WIDTH, height = VGA_HEIGHT;
-		int ret = ov772x_set_params(client, &width, &height,
-					    V4L2_MBUS_FMT_YUYV8_2X8);
-		if (ret < 0)
-			return ret;
+		priv->cfmt = &ov772x_cfmts[0];
+		priv->win = ov772x_select_win(VGA_WIDTH, VGA_HEIGHT);
 	}
 
 	mf->width	= priv->win->width;
diff --git a/drivers/media/video/ov9640.c b/drivers/media/video/ov9640.c
index 23412debb36..9ed4ba4236c 100644
--- a/drivers/media/video/ov9640.c
+++ b/drivers/media/video/ov9640.c
@@ -605,6 +605,7 @@ static int ov9640_video_probe(struct i2c_client *client)
 		devname		= "ov9640";
 		priv->model	= V4L2_IDENT_OV9640;
 		priv->revision	= 2;
+		break;
 	case OV9640_V3:
 		devname		= "ov9640";
 		priv->model	= V4L2_IDENT_OV9640;
diff --git a/drivers/media/video/pms.c b/drivers/media/video/pms.c
index b4c679b3fb0..77f9c92186f 100644
--- a/drivers/media/video/pms.c
+++ b/drivers/media/video/pms.c
@@ -30,7 +30,6 @@
 #include <linux/ioport.h>
 #include <linux/init.h>
 #include <linux/mutex.h>
-#include <linux/slab.h>
 #include <linux/uaccess.h>
 #include <linux/isa.h>
 #include <asm/io.h>
diff --git a/drivers/media/video/pvrusb2/Kconfig b/drivers/media/video/pvrusb2/Kconfig
index f9b6001e1dd..25e412ecad2 100644
--- a/drivers/media/video/pvrusb2/Kconfig
+++ b/drivers/media/video/pvrusb2/Kconfig
@@ -1,7 +1,6 @@
 config VIDEO_PVRUSB2
 	tristate "Hauppauge WinTV-PVR USB2 support"
 	depends on VIDEO_V4L2 && I2C
-	depends on VIDEO_MEDIA	# Avoids pvrusb = Y / DVB = M
 	select VIDEO_TUNER
 	select VIDEO_TVEEPROM
 	select VIDEO_CX2341X
diff --git a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
index 7bddfaeeafc..f344aed32a9 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
@@ -226,13 +226,11 @@ static int pvr2_enum_input(struct file *file, void *priv, struct v4l2_input *vi)
 	struct v4l2_input tmp;
 	unsigned int cnt;
 	int val;
-	int ret;
 
 	cptr = pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_INPUT);
 
 	memset(&tmp, 0, sizeof(tmp));
 	tmp.index = vi->index;
-	ret = 0;
 	if (vi->index >= fh->input_cnt)
 		return -EINVAL;
 	val = fh->input_map[vi->index];
@@ -556,9 +554,7 @@ static int pvr2_queryctrl(struct file *file, void *priv,
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 	struct pvr2_ctrl *cptr;
 	int val;
-	int ret;
 
-	ret = 0;
 	if (vc->id & V4L2_CTRL_FLAG_NEXT_CTRL) {
 		cptr = pvr2_hdw_get_ctrl_nextv4l(
 				hdw, (vc->id & ~V4L2_CTRL_FLAG_NEXT_CTRL));
@@ -705,11 +701,9 @@ static int pvr2_try_ext_ctrls(struct file *file, void *priv,
 	struct v4l2_ext_control *ctrl;
 	struct pvr2_ctrl *pctl;
 	unsigned int idx;
-	int ret;
 
 	/* For the moment just validate that the requested control
 	   actually exists. */
-	ret = 0;
 	for (idx = 0; idx < ctls->count; idx++) {
 		ctrl = ctls->controls + idx;
 		pctl = pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id);
@@ -770,12 +764,10 @@ static int pvr2_s_crop(struct file *file, void *priv, struct v4l2_crop *crop)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
-	struct v4l2_cropcap cap;
 	int ret;
 
 	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	ret = pvr2_ctrl_set_value(
 			pvr2_hdw_get_ctrl_by_id(hdw, PVR2_CID_CROPL),
 			crop->c.left);
@@ -965,7 +957,7 @@ static long pvr2_v4l2_ioctl(struct file *file,
 	long ret = -EINVAL;
 
 	if (pvrusb2_debug & PVR2_TRACE_V4LIOCTL)
-		v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw), cmd);
+		v4l_printk_ioctl(pvr2_hdw_get_driver_name(hdw), cmd);
 
 	if (!pvr2_hdw_dev_ok(hdw)) {
 		pvr2_trace(PVR2_TRACE_ERROR_LEGS,
@@ -998,7 +990,7 @@ static long pvr2_v4l2_ioctl(struct file *file,
 				pvr2_trace(PVR2_TRACE_V4LIOCTL,
 					   "pvr2_v4l2_do_ioctl failure, ret=%ld"
 					   " command was:", ret);
-				v4l_print_ioctl(pvr2_hdw_get_driver_name(hdw),
+				v4l_printk_ioctl(pvr2_hdw_get_driver_name(hdw),
 						cmd);
 			}
 		}
diff --git a/drivers/media/video/pwc/pwc-if.c b/drivers/media/video/pwc/pwc-if.c
index ec4e2ef54e6..de7c7ba99ef 100644
--- a/drivers/media/video/pwc/pwc-if.c
+++ b/drivers/media/video/pwc/pwc-if.c
@@ -136,19 +136,13 @@ static int leds[2] = { 100, 0 };
 
 /***/
 
-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 int  pwc_video_mmap(struct file *file, struct vm_area_struct *vma);
-
 static const struct v4l2_file_operations pwc_fops = {
 	.owner =	THIS_MODULE,
 	.open =		v4l2_fh_open,
-	.release =     	pwc_video_close,
-	.read =		pwc_video_read,
-	.poll =		pwc_video_poll,
-	.mmap =		pwc_video_mmap,
+	.release =	vb2_fop_release,
+	.read =		vb2_fop_read,
+	.poll =		vb2_fop_poll,
+	.mmap =		vb2_fop_mmap,
 	.unlocked_ioctl = video_ioctl2,
 };
 static struct video_device pwc_template = {
@@ -562,17 +556,6 @@ static const char *pwc_sensor_type_to_string(unsigned int sensor_type)
 /***************************************************************************/
 /* Video4Linux functions */
 
-int pwc_test_n_set_capt_file(struct pwc_device *pdev, struct file *file)
-{
-	if (pdev->capt_file != NULL &&
-	    pdev->capt_file != file)
-		return -EBUSY;
-
-	pdev->capt_file = file;
-
-	return 0;
-}
-
 static void pwc_video_release(struct v4l2_device *v)
 {
 	struct pwc_device *pdev = container_of(v, struct pwc_device, v4l2_dev);
@@ -583,113 +566,6 @@ static void pwc_video_release(struct v4l2_device *v)
 	kfree(pdev);
 }
 
-static int pwc_video_close(struct file *file)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-
-	/*
-	 * If we're still streaming vb2_queue_release will call stream_stop
-	 * so we must take both the v4l2_lock and the vb_queue_lock.
-	 */
-	if (mutex_lock_interruptible(&pdev->v4l2_lock))
-		return -ERESTARTSYS;
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock)) {
-		mutex_unlock(&pdev->v4l2_lock);
-		return -ERESTARTSYS;
-	}
-
-	if (pdev->capt_file == file) {
-		vb2_queue_release(&pdev->vb_queue);
-		pdev->capt_file = NULL;
-	}
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	mutex_unlock(&pdev->v4l2_lock);
-
-	return v4l2_fh_release(file);
-}
-
-static ssize_t pwc_video_read(struct file *file, char __user *buf,
-			      size_t count, loff_t *ppos)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int lock_v4l2 = 0;
-	ssize_t ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret)
-		goto out;
-
-	/* stream_start will get called so we must take the v4l2_lock */
-	if (pdev->vb_queue.fileio == NULL)
-		lock_v4l2 = 1;
-
-	/* Use try_lock, since we're taking the locks in the *wrong* order! */
-	if (lock_v4l2 && !mutex_trylock(&pdev->v4l2_lock)) {
-		ret = -ERESTARTSYS;
-		goto out;
-	}
-	ret = vb2_read(&pdev->vb_queue, buf, count, ppos,
-		       file->f_flags & O_NONBLOCK);
-	if (lock_v4l2)
-		mutex_unlock(&pdev->v4l2_lock);
-out:
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static unsigned int pwc_video_poll(struct file *file, poll_table *wait)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	struct vb2_queue *q = &pdev->vb_queue;
-	unsigned long req_events = poll_requested_events(wait);
-	unsigned int ret = POLL_ERR;
-	int lock_v4l2 = 0;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return POLL_ERR;
-
-	/* Will this start fileio and thus call start_stream? */
-	if ((req_events & (POLLIN | POLLRDNORM)) &&
-	    q->num_buffers == 0 && !q->streaming && q->fileio == NULL) {
-		if (pwc_test_n_set_capt_file(pdev, file))
-			goto out;
-		lock_v4l2 = 1;
-	}
-
-	/* Use try_lock, since we're taking the locks in the *wrong* order! */
-	if (lock_v4l2 && !mutex_trylock(&pdev->v4l2_lock))
-		goto out;
-	ret = vb2_poll(&pdev->vb_queue, file, wait);
-	if (lock_v4l2)
-		mutex_unlock(&pdev->v4l2_lock);
-
-out:
-	if (!pdev->udev)
-		ret |= POLLHUP;
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static int pwc_video_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_mmap(&pdev->vb_queue, vma);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
 /***************************************************************************/
 /* Videobuf2 operations */
 
@@ -782,6 +658,8 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	if (!pdev->udev)
 		return -ENODEV;
 
+	if (mutex_lock_interruptible(&pdev->v4l2_lock))
+		return -ERESTARTSYS;
 	/* Turn on camera and set LEDS on */
 	pwc_camera_power(pdev, 1);
 	pwc_set_leds(pdev, leds[0], leds[1]);
@@ -794,6 +672,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		/* And cleanup any queued bufs!! */
 		pwc_cleanup_queued_bufs(pdev);
 	}
+	mutex_unlock(&pdev->v4l2_lock);
 
 	return r;
 }
@@ -802,6 +681,8 @@ static int stop_streaming(struct vb2_queue *vq)
 {
 	struct pwc_device *pdev = vb2_get_drv_priv(vq);
 
+	if (mutex_lock_interruptible(&pdev->v4l2_lock))
+		return -ERESTARTSYS;
 	if (pdev->udev) {
 		pwc_set_leds(pdev, 0, 0);
 		pwc_camera_power(pdev, 0);
@@ -809,22 +690,11 @@ static int stop_streaming(struct vb2_queue *vq)
 	}
 
 	pwc_cleanup_queued_bufs(pdev);
+	mutex_unlock(&pdev->v4l2_lock);
 
 	return 0;
 }
 
-static void wait_prepare(struct vb2_queue *vq)
-{
-	struct pwc_device *pdev = vb2_get_drv_priv(vq);
-	mutex_unlock(&pdev->vb_queue_lock);
-}
-
-static void wait_finish(struct vb2_queue *vq)
-{
-	struct pwc_device *pdev = vb2_get_drv_priv(vq);
-	mutex_lock(&pdev->vb_queue_lock);
-}
-
 static struct vb2_ops pwc_vb_queue_ops = {
 	.queue_setup		= queue_setup,
 	.buf_init		= buffer_init,
@@ -834,8 +704,8 @@ static struct vb2_ops pwc_vb_queue_ops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= wait_prepare,
-	.wait_finish		= wait_finish,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /***************************************************************************/
@@ -1136,6 +1006,8 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	/* Init video_device structure */
 	memcpy(&pdev->vdev, &pwc_template, sizeof(pwc_template));
 	strcpy(pdev->vdev.name, name);
+	pdev->vdev.queue = &pdev->vb_queue;
+	pdev->vdev.queue->lock = &pdev->vb_queue_lock;
 	set_bit(V4L2_FL_USE_FH_PRIO, &pdev->vdev.flags);
 	video_set_drvdata(&pdev->vdev, pdev);
 
@@ -1190,15 +1062,6 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->vdev.v4l2_dev = &pdev->v4l2_dev;
 	pdev->vdev.lock = &pdev->v4l2_lock;
 
-	/*
-	 * Don't take v4l2_lock for these ioctls. This improves latency if
-	 * v4l2_lock is taken for a long time, e.g. when changing a control
-	 * value, and a new frame is ready to be dequeued.
-	 */
-	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_DQBUF);
-	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_QBUF);
-	v4l2_disable_ioctl_locking(&pdev->vdev, VIDIOC_QUERYBUF);
-
 	rc = video_register_device(&pdev->vdev, VFL_TYPE_GRABBER, -1);
 	if (rc < 0) {
 		PWC_ERROR("Failed to register as video device (%d).\n", rc);
@@ -1253,20 +1116,18 @@ static void usb_pwc_disconnect(struct usb_interface *intf)
 	struct v4l2_device *v = usb_get_intfdata(intf);
 	struct pwc_device *pdev = container_of(v, struct pwc_device, v4l2_dev);
 
-	mutex_lock(&pdev->v4l2_lock);
-
 	mutex_lock(&pdev->vb_queue_lock);
+	mutex_lock(&pdev->v4l2_lock);
 	/* No need to keep the urbs around after disconnection */
 	if (pdev->vb_queue.streaming)
 		pwc_isoc_cleanup(pdev);
 	pdev->udev = NULL;
 	pwc_cleanup_queued_bufs(pdev);
-	mutex_unlock(&pdev->vb_queue_lock);
 
 	v4l2_device_disconnect(&pdev->v4l2_dev);
 	video_unregister_device(&pdev->vdev);
-
 	mutex_unlock(&pdev->v4l2_lock);
+	mutex_unlock(pdev->vb_queue.lock);
 
 #ifdef CONFIG_USB_PWC_INPUT_EVDEV
 	if (pdev->button_dev)
diff --git a/drivers/media/video/pwc/pwc-v4l.c b/drivers/media/video/pwc/pwc-v4l.c
index c691e29cc36..545e9bbdeed 100644
--- a/drivers/media/video/pwc/pwc-v4l.c
+++ b/drivers/media/video/pwc/pwc-v4l.c
@@ -405,6 +405,7 @@ static void pwc_vidioc_fill_fmt(struct v4l2_format *f,
 	f->fmt.pix.pixelformat  = pixfmt;
 	f->fmt.pix.bytesperline = f->fmt.pix.width;
 	f->fmt.pix.sizeimage	= f->fmt.pix.height * f->fmt.pix.width * 3 / 2;
+	f->fmt.pix.colorspace	= V4L2_COLORSPACE_SRGB;
 	PWC_DEBUG_IOCTL("pwc_vidioc_fill_fmt() "
 			"width=%d, height=%d, bytesperline=%d, sizeimage=%d, pixelformat=%c%c%c%c\n",
 			f->fmt.pix.width,
@@ -468,17 +469,8 @@ static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 	if (ret < 0)
 		return ret;
 
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret)
-		goto leave;
-
-	if (pdev->vb_queue.streaming) {
-		ret = -EBUSY;
-		goto leave;
-	}
+	if (vb2_is_busy(&pdev->vb_queue))
+		return -EBUSY;
 
 	pixelformat = f->fmt.pix.pixelformat;
 
@@ -496,8 +488,6 @@ static int pwc_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f)
 	PWC_DEBUG_IOCTL("pwc_set_video_mode(), return=%d\n", ret);
 
 	pwc_vidioc_fill_fmt(f, pdev->width, pdev->height, pdev->pixfmt);
-leave:
-	mutex_unlock(&pdev->vb_queue_lock);
 	return ret;
 }
 
@@ -508,10 +498,9 @@ static int pwc_querycap(struct file *file, void *fh, struct v4l2_capability *cap
 	strcpy(cap->driver, PWC_NAME);
 	strlcpy(cap->card, pdev->vdev.name, sizeof(cap->card));
 	usb_make_path(pdev->udev, cap->bus_info, sizeof(cap->bus_info));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE	|
-		V4L2_CAP_STREAMING	|
-		V4L2_CAP_READWRITE;
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+					V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -520,7 +509,8 @@ static int pwc_enum_input(struct file *file, void *fh, struct v4l2_input *i)
 	if (i->index)	/* Only one INPUT is supported */
 		return -EINVAL;
 
-	strcpy(i->name, "usb");
+	strlcpy(i->name, "Camera", sizeof(i->name));
+	i->type = V4L2_INPUT_TYPE_CAMERA;
 	return 0;
 }
 
@@ -933,104 +923,6 @@ static int pwc_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *
 	return pwc_vidioc_try_fmt(pdev, f);
 }
 
-static int pwc_reqbufs(struct file *file, void *fh,
-		       struct v4l2_requestbuffers *rb)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_reqbufs(&pdev->vb_queue, rb);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static int pwc_querybuf(struct file *file, void *fh, struct v4l2_buffer *buf)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_querybuf(&pdev->vb_queue, buf);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static int pwc_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_qbuf(&pdev->vb_queue, buf);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static int pwc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_dqbuf(&pdev->vb_queue, buf,
-				file->f_flags & O_NONBLOCK);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static int pwc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_streamon(&pdev->vb_queue, i);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
-static int pwc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
-{
-	struct pwc_device *pdev = video_drvdata(file);
-	int ret;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
-
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret == 0)
-		ret = vb2_streamoff(&pdev->vb_queue, i);
-
-	mutex_unlock(&pdev->vb_queue_lock);
-	return ret;
-}
-
 static int pwc_enum_framesizes(struct file *file, void *fh,
 					 struct v4l2_frmsizeenum *fsize)
 {
@@ -1112,32 +1004,27 @@ static int pwc_s_parm(struct file *file, void *fh,
 	int compression = 0;
 	int ret, fps;
 
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    parm->parm.capture.timeperframe.numerator == 0)
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
 
-	fps = parm->parm.capture.timeperframe.denominator /
-	      parm->parm.capture.timeperframe.numerator;
-
-	if (mutex_lock_interruptible(&pdev->vb_queue_lock))
-		return -ERESTARTSYS;
+	/* If timeperframe == 0, then reset the framerate to the nominal value.
+	   We pick a high framerate here, and let pwc_set_video_mode() figure
+	   out the best match. */
+	if (parm->parm.capture.timeperframe.numerator == 0 ||
+	    parm->parm.capture.timeperframe.denominator == 0)
+		fps = 30;
+	else
+		fps = parm->parm.capture.timeperframe.denominator /
+		      parm->parm.capture.timeperframe.numerator;
 
-	ret = pwc_test_n_set_capt_file(pdev, file);
-	if (ret)
-		goto leave;
-
-	if (pdev->vb_queue.streaming) {
-		ret = -EBUSY;
-		goto leave;
-	}
+	if (vb2_is_busy(&pdev->vb_queue))
+		return -EBUSY;
 
 	ret = pwc_set_video_mode(pdev, pdev->width, pdev->height, pdev->pixfmt,
 				 fps, &compression, 0);
 
 	pwc_g_parm(file, fh, parm);
 
-leave:
-	mutex_unlock(&pdev->vb_queue_lock);
 	return ret;
 }
 
@@ -1150,12 +1037,12 @@ const struct v4l2_ioctl_ops pwc_ioctl_ops = {
 	.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_reqbufs			    = pwc_reqbufs,
-	.vidioc_querybuf		    = pwc_querybuf,
-	.vidioc_qbuf			    = pwc_qbuf,
-	.vidioc_dqbuf			    = pwc_dqbuf,
-	.vidioc_streamon		    = pwc_streamon,
-	.vidioc_streamoff		    = pwc_streamoff,
+	.vidioc_reqbufs			    = vb2_ioctl_reqbufs,
+	.vidioc_querybuf		    = vb2_ioctl_querybuf,
+	.vidioc_qbuf			    = vb2_ioctl_qbuf,
+	.vidioc_dqbuf			    = vb2_ioctl_dqbuf,
+	.vidioc_streamon		    = vb2_ioctl_streamon,
+	.vidioc_streamoff		    = vb2_ioctl_streamoff,
 	.vidioc_log_status		    = v4l2_ctrl_log_status,
 	.vidioc_enum_framesizes		    = pwc_enum_framesizes,
 	.vidioc_enum_frameintervals	    = pwc_enum_frameintervals,
diff --git a/drivers/media/video/pwc/pwc.h b/drivers/media/video/pwc/pwc.h
index d6b5b216b9d..7a6a0d39c2c 100644
--- a/drivers/media/video/pwc/pwc.h
+++ b/drivers/media/video/pwc/pwc.h
@@ -239,7 +239,6 @@ struct pwc_device
 	int features;		/* feature bits */
 
 	/*** Video data ***/
-	struct file *capt_file;	/* file doing video capture */
 	int vendpoint;		/* video isoc endpoint */
 	int vcinterface;	/* video control interface */
 	int valternate;		/* alternate interface needed */
@@ -355,8 +354,6 @@ struct pwc_device
 extern int pwc_trace;
 #endif
 
-int pwc_test_n_set_capt_file(struct pwc_device *pdev, struct file *file);
-
 /** Functions in pwc-misc.c */
 /* sizes in pixels */
 extern const int pwc_image_sizes[PSZ_MAX][2];
diff --git a/drivers/media/video/s2255drv.c b/drivers/media/video/s2255drv.c
index 01c2179f052..95007dda0c9 100644
--- a/drivers/media/video/s2255drv.c
+++ b/drivers/media/video/s2255drv.c
@@ -2686,3 +2686,4 @@ MODULE_DESCRIPTION("Sensoray 2255 Video for Linux driver");
 MODULE_AUTHOR("Dean Anderson (Sensoray Company Inc.)");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(S2255_VERSION);
+MODULE_FIRMWARE(FIRMWARE_FILE_NAME);
diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index 725812aa0c3..8e413dd3c0b 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -480,48 +480,59 @@ static int fimc_capture_set_default_format(struct fimc_dev *fimc);
 static int fimc_capture_open(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
-	int ret;
+	int ret = -EBUSY;
 
 	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
 
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
+
 	if (fimc_m2m_active(fimc))
-		return -EBUSY;
+		goto unlock;
 
 	set_bit(ST_CAPT_BUSY, &fimc->state);
 	ret = pm_runtime_get_sync(&fimc->pdev->dev);
 	if (ret < 0)
-		return ret;
+		goto unlock;
 
 	ret = v4l2_fh_open(file);
-	if (ret)
-		return ret;
-
-	if (++fimc->vid_cap.refcnt != 1)
-		return 0;
+	if (ret) {
+		pm_runtime_put(&fimc->pdev->dev);
+		goto unlock;
+	}
 
-	ret = fimc_pipeline_initialize(&fimc->pipeline,
+	if (++fimc->vid_cap.refcnt == 1) {
+		ret = fimc_pipeline_initialize(&fimc->pipeline,
 				       &fimc->vid_cap.vfd->entity, true);
-	if (ret < 0) {
-		clear_bit(ST_CAPT_BUSY, &fimc->state);
-		pm_runtime_put_sync(&fimc->pdev->dev);
-		fimc->vid_cap.refcnt--;
-		v4l2_fh_release(file);
-		return ret;
-	}
-	ret = fimc_capture_ctrls_create(fimc);
 
-	if (!ret && !fimc->vid_cap.user_subdev_api)
-		ret = fimc_capture_set_default_format(fimc);
+		if (!ret && !fimc->vid_cap.user_subdev_api)
+			ret = fimc_capture_set_default_format(fimc);
 
+		if (!ret)
+			ret = fimc_capture_ctrls_create(fimc);
+
+		if (ret < 0) {
+			clear_bit(ST_CAPT_BUSY, &fimc->state);
+			pm_runtime_put_sync(&fimc->pdev->dev);
+			fimc->vid_cap.refcnt--;
+			v4l2_fh_release(file);
+		}
+	}
+unlock:
+	mutex_unlock(&fimc->lock);
 	return ret;
 }
 
 static int fimc_capture_close(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
+	int ret;
 
 	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
 
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
+
 	if (--fimc->vid_cap.refcnt == 0) {
 		clear_bit(ST_CAPT_BUSY, &fimc->state);
 		fimc_stop_capture(fimc, false);
@@ -535,22 +546,40 @@ static int fimc_capture_close(struct file *file)
 		vb2_queue_release(&fimc->vid_cap.vbq);
 		fimc_ctrls_delete(fimc->vid_cap.ctx);
 	}
-	return v4l2_fh_release(file);
+
+	ret = v4l2_fh_release(file);
+
+	mutex_unlock(&fimc->lock);
+	return ret;
 }
 
 static unsigned int fimc_capture_poll(struct file *file,
 				      struct poll_table_struct *wait)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
+	int ret;
 
-	return vb2_poll(&fimc->vid_cap.vbq, file, wait);
+	if (mutex_lock_interruptible(&fimc->lock))
+		return POLL_ERR;
+
+	ret = vb2_poll(&fimc->vid_cap.vbq, file, wait);
+	mutex_unlock(&fimc->lock);
+
+	return ret;
 }
 
 static int fimc_capture_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
+	int ret;
 
-	return vb2_mmap(&fimc->vid_cap.vbq, vma);
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
+
+	ret = vb2_mmap(&fimc->vid_cap.vbq, vma);
+	mutex_unlock(&fimc->lock);
+
+	return ret;
 }
 
 static const struct v4l2_file_operations fimc_capture_fops = {
@@ -658,7 +687,7 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
 		r->left   = r->top = 0;
 		return;
 	}
-	if (target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+	if (target == V4L2_SEL_TGT_COMPOSE) {
 		if (ctx->rotation != 90 && ctx->rotation != 270)
 			align_h = 1;
 		max_sc_h = min(SCALER_MAX_HRATIO, 1 << (ffs(sink->width) - 3));
@@ -685,7 +714,7 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
 		      rotate ? sink->f_height : sink->f_width);
 	max_h = min_t(u32, FIMC_CAMIF_MAX_HEIGHT, sink->f_height);
 
-	if (target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+	if (target == V4L2_SEL_TGT_COMPOSE) {
 		min_w = min_t(u32, max_w, sink->f_width / max_sc_h);
 		min_h = min_t(u32, max_h, sink->f_height / max_sc_v);
 		if (rotate) {
@@ -1146,9 +1175,9 @@ static int fimc_cap_g_selection(struct file *file, void *fh,
 		s->r.height = f->o_height;
 		return 0;
 
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	case V4L2_SEL_TGT_COMPOSE:
 		f = &ctx->d_frame;
-	case V4L2_SEL_TGT_CROP_ACTIVE:
+	case V4L2_SEL_TGT_CROP:
 		s->r.left = f->offs_h;
 		s->r.top = f->offs_v;
 		s->r.width = f->width;
@@ -1160,7 +1189,7 @@ static int fimc_cap_g_selection(struct file *file, void *fh,
 }
 
 /* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
-int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
+static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
 {
 	if (a->left < b->left || a->top < b->top)
 		return 0;
@@ -1184,9 +1213,9 @@ static int fimc_cap_s_selection(struct file *file, void *fh,
 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 		return -EINVAL;
 
-	if (s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE)
+	if (s->target == V4L2_SEL_TGT_COMPOSE)
 		f = &ctx->d_frame;
-	else if (s->target == V4L2_SEL_TGT_CROP_ACTIVE)
+	else if (s->target == V4L2_SEL_TGT_CROP)
 		f = &ctx->s_frame;
 	else
 		return -EINVAL;
@@ -1428,9 +1457,9 @@ static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
 	mutex_lock(&fimc->lock);
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 		f = &ctx->d_frame;
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		r->width = f->o_width;
 		r->height = f->o_height;
 		r->left = 0;
@@ -1438,10 +1467,10 @@ static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
 		mutex_unlock(&fimc->lock);
 		return 0;
 
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		try_sel = v4l2_subdev_get_try_crop(fh, sel->pad);
 		break;
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SEL_TGT_COMPOSE:
 		try_sel = v4l2_subdev_get_try_compose(fh, sel->pad);
 		f = &ctx->d_frame;
 		break;
@@ -1482,12 +1511,12 @@ static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
 		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
-	fimc_capture_try_selection(ctx, r, V4L2_SEL_TGT_CROP_ACTIVE);
+	fimc_capture_try_selection(ctx, r, V4L2_SEL_TGT_CROP);
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 		f = &ctx->d_frame;
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		r->width = f->o_width;
 		r->height = f->o_height;
 		r->left = 0;
@@ -1495,10 +1524,10 @@ static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
 		mutex_unlock(&fimc->lock);
 		return 0;
 
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		try_sel = v4l2_subdev_get_try_crop(fh, sel->pad);
 		break;
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SEL_TGT_COMPOSE:
 		try_sel = v4l2_subdev_get_try_compose(fh, sel->pad);
 		f = &ctx->d_frame;
 		break;
@@ -1514,7 +1543,7 @@ static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
 		set_frame_crop(f, r->left, r->top, r->width, r->height);
 		set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
 		spin_unlock_irqrestore(&fimc->slock, flags);
-		if (sel->target == V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL)
+		if (sel->target == V4L2_SEL_TGT_COMPOSE)
 			ctx->state |= FIMC_COMPOSE;
 	}
 
@@ -1589,10 +1618,7 @@ static int fimc_register_capture_device(struct fimc_dev *fimc,
 	vfd->minor	= -1;
 	vfd->release	= video_device_release;
 	vfd->lock	= &fimc->lock;
-	/* Locking in file operations other than ioctl should be done
-	   by the driver, not the V4L2 core.
-	   This driver needs auditing so that this flag can be removed. */
-	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
+
 	video_set_drvdata(vfd, fimc);
 
 	vid_cap = &fimc->vid_cap;
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index a4646ca1d56..1a445404e73 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -463,7 +463,7 @@ void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
 	    f->fmt->color, f->dma_offset.y_h, f->dma_offset.y_v);
 }
 
-int fimc_set_color_effect(struct fimc_ctx *ctx, enum v4l2_colorfx colorfx)
+static int fimc_set_color_effect(struct fimc_ctx *ctx, enum v4l2_colorfx colorfx)
 {
 	struct fimc_effect *effect = &ctx->effect;
 
diff --git a/drivers/media/video/s5p-fimc/fimc-core.h b/drivers/media/video/s5p-fimc/fimc-core.h
index 95b27ae5cf2..808ccc62184 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.h
+++ b/drivers/media/video/s5p-fimc/fimc-core.h
@@ -27,9 +27,6 @@
 #include <media/v4l2-mediabus.h>
 #include <media/s5p_fimc.h>
 
-#define err(fmt, args...) \
-	printk(KERN_ERR "%s:%d: " fmt "\n", __func__, __LINE__, ##args)
-
 #define dbg(fmt, args...) \
 	pr_debug("%s:%d: " fmt "\n", __func__, __LINE__, ##args)
 
diff --git a/drivers/media/video/s5p-fimc/fimc-lite-reg.c b/drivers/media/video/s5p-fimc/fimc-lite-reg.c
index 419adfb7cdf..f996e94873f 100644
--- a/drivers/media/video/s5p-fimc/fimc-lite-reg.c
+++ b/drivers/media/video/s5p-fimc/fimc-lite-reg.c
@@ -215,7 +215,7 @@ void flite_hw_set_camera_bus(struct fimc_lite *dev,
 	flite_hw_set_camera_port(dev, s_info->mux_id);
 }
 
-void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
+static void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
 {
 	static const u32 pixcode[4][2] = {
 		{ V4L2_MBUS_FMT_YUYV8_2X8, FLITE_REG_CIODMAFMT_YCBYCR },
diff --git a/drivers/media/video/s5p-fimc/fimc-lite.c b/drivers/media/video/s5p-fimc/fimc-lite.c
index 74ff310db30..c5b57e805b6 100644
--- a/drivers/media/video/s5p-fimc/fimc-lite.c
+++ b/drivers/media/video/s5p-fimc/fimc-lite.c
@@ -902,7 +902,7 @@ static int fimc_lite_g_selection(struct file *file, void *fh,
 		sel->r.height = f->f_height;
 		return 0;
 
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	case V4L2_SEL_TGT_COMPOSE:
 		sel->r = f->rect;
 		return 0;
 	}
@@ -919,7 +919,7 @@ static int fimc_lite_s_selection(struct file *file, void *fh,
 	unsigned long flags;
 
 	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
-	    sel->target != V4L2_SEL_TGT_COMPOSE_ACTIVE)
+	    sel->target != V4L2_SEL_TGT_COMPOSE)
 		return -EINVAL;
 
 	fimc_lite_try_compose(fimc, &rect);
@@ -1117,9 +1117,9 @@ static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
 	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
 	struct flite_frame *f = &fimc->inp_frame;
 
-	if ((sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL &&
-	     sel->target != V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS) ||
-	    sel->pad != FLITE_SD_PAD_SINK)
+	if ((sel->target != V4L2_SEL_TGT_CROP &&
+	     sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
+	     sel->pad != FLITE_SD_PAD_SINK)
 		return -EINVAL;
 
 	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
@@ -1128,7 +1128,7 @@ static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
 	}
 
 	mutex_lock(&fimc->lock);
-	if (sel->target == V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL) {
+	if (sel->target == V4L2_SEL_TGT_CROP) {
 		sel->r = f->rect;
 	} else {
 		sel->r.left = 0;
@@ -1153,8 +1153,7 @@ static int fimc_lite_subdev_set_selection(struct v4l2_subdev *sd,
 	struct flite_frame *f = &fimc->inp_frame;
 	int ret = 0;
 
-	if (sel->target != V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL ||
-	    sel->pad != FLITE_SD_PAD_SINK)
+	if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != FLITE_SD_PAD_SINK)
 		return -EINVAL;
 
 	mutex_lock(&fimc->lock);
diff --git a/drivers/media/video/s5p-fimc/fimc-m2m.c b/drivers/media/video/s5p-fimc/fimc-m2m.c
index 4c58e057096..c587011d80e 100644
--- a/drivers/media/video/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/video/s5p-fimc/fimc-m2m.c
@@ -259,7 +259,12 @@ static int fimc_m2m_querycap(struct file *file, void *fh,
 	strncpy(cap->driver, fimc->pdev->name, sizeof(cap->driver) - 1);
 	strncpy(cap->card, fimc->pdev->name, sizeof(cap->card) - 1);
 	cap->bus_info[0] = 0;
-	cap->capabilities = V4L2_CAP_STREAMING |
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
 		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
 	return 0;
@@ -642,21 +647,25 @@ static int fimc_m2m_open(struct file *file)
 {
 	struct fimc_dev *fimc = video_drvdata(file);
 	struct fimc_ctx *ctx;
-	int ret;
+	int ret = -EBUSY;
 
 	dbg("pid: %d, state: 0x%lx, refcnt: %d",
 	    task_pid_nr(current), fimc->state, fimc->vid_cap.refcnt);
 
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
 	/*
 	 * Return if the corresponding video capture node
 	 * is already opened.
 	 */
 	if (fimc->vid_cap.refcnt > 0)
-		return -EBUSY;
+		goto unlock;
 
 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
-	if (!ctx)
-		return -ENOMEM;
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
 	v4l2_fh_init(&ctx->fh, fimc->m2m.vfd);
 	ctx->fimc_dev = fimc;
 
@@ -687,6 +696,8 @@ static int fimc_m2m_open(struct file *file)
 
 	if (fimc->m2m.refcnt++ == 0)
 		set_bit(ST_M2M_RUN, &fimc->state);
+
+	mutex_unlock(&fimc->lock);
 	return 0;
 
 error_c:
@@ -695,6 +706,8 @@ error_fh:
 	v4l2_fh_del(&ctx->fh);
 	v4l2_fh_exit(&ctx->fh);
 	kfree(ctx);
+unlock:
+	mutex_unlock(&fimc->lock);
 	return ret;
 }
 
@@ -706,6 +719,9 @@ static int fimc_m2m_release(struct file *file)
 	dbg("pid: %d, state: 0x%lx, refcnt= %d",
 		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
 
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
+
 	v4l2_m2m_ctx_release(ctx->m2m_ctx);
 	fimc_ctrls_delete(ctx);
 	v4l2_fh_del(&ctx->fh);
@@ -714,6 +730,8 @@ static int fimc_m2m_release(struct file *file)
 	if (--fimc->m2m.refcnt <= 0)
 		clear_bit(ST_M2M_RUN, &fimc->state);
 	kfree(ctx);
+
+	mutex_unlock(&fimc->lock);
 	return 0;
 }
 
@@ -721,16 +739,32 @@ static unsigned int fimc_m2m_poll(struct file *file,
 				  struct poll_table_struct *wait)
 {
 	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+	mutex_unlock(&fimc->lock);
 
-	return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+	return ret;
 }
 
 
 static int fimc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
 
-	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+	ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+	mutex_unlock(&fimc->lock);
+
+	return ret;
 }
 
 static const struct v4l2_file_operations fimc_m2m_fops = {
@@ -772,10 +806,6 @@ int fimc_register_m2m_device(struct fimc_dev *fimc,
 	vfd->minor = -1;
 	vfd->release = video_device_release;
 	vfd->lock = &fimc->lock;
-	/* Locking in file operations other than ioctl should be done
-	   by the driver, not the V4L2 core.
-	   This driver needs auditing so that this flag can be removed. */
-	set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
 
 	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
 	video_set_drvdata(vfd, fimc);
diff --git a/drivers/media/video/s5p-fimc/fimc-mdevice.c b/drivers/media/video/s5p-fimc/fimc-mdevice.c
index 52cef486542..e65bb283fd8 100644
--- a/drivers/media/video/s5p-fimc/fimc-mdevice.c
+++ b/drivers/media/video/s5p-fimc/fimc-mdevice.c
@@ -180,7 +180,7 @@ EXPORT_SYMBOL_GPL(fimc_pipeline_initialize);
  * sensor clock.
  * Called with the graph mutex held.
  */
-int __fimc_pipeline_shutdown(struct fimc_pipeline *p)
+static int __fimc_pipeline_shutdown(struct fimc_pipeline *p)
 {
 	int ret = 0;
 
@@ -1010,7 +1010,7 @@ static struct platform_driver fimc_md_driver = {
 	}
 };
 
-int __init fimc_md_init(void)
+static int __init fimc_md_init(void)
 {
 	int ret;
 
@@ -1021,7 +1021,8 @@ int __init fimc_md_init(void)
 
 	return platform_driver_register(&fimc_md_driver);
 }
-void __exit fimc_md_exit(void)
+
+static void __exit fimc_md_exit(void)
 {
 	platform_driver_unregister(&fimc_md_driver);
 	fimc_unregister_driver();
diff --git a/drivers/media/video/s5p-fimc/fimc-reg.c b/drivers/media/video/s5p-fimc/fimc-reg.c
index 1fc4ce8446f..0e3eb9ce4f9 100644
--- a/drivers/media/video/s5p-fimc/fimc-reg.c
+++ b/drivers/media/video/s5p-fimc/fimc-reg.c
@@ -667,7 +667,8 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 		FIMC_REG_CIGCTRL_SELCAM_MIPI | FIMC_REG_CIGCTRL_CAMIF_SELWB |
 		FIMC_REG_CIGCTRL_SELCAM_MIPI_A | FIMC_REG_CIGCTRL_CAM_JPEG);
 
-	if (cam->bus_type == FIMC_MIPI_CSI2) {
+	switch (cam->bus_type) {
+	case FIMC_MIPI_CSI2:
 		cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI;
 
 		if (cam->mux_id == 0)
@@ -683,23 +684,24 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
 			break;
 		default:
-			v4l2_err(fimc->vid_cap.vfd,
-				 "Not supported camera pixel format: %d",
+			v4l2_err(vid_cap->vfd,
+				 "Not supported camera pixel format: %#x\n",
 				 vid_cap->mf.code);
 			return -EINVAL;
 		}
 		tmp |= (csis_data_alignment == 32) << 8;
 
 		writel(tmp, fimc->regs + FIMC_REG_CSIIMGFMT);
-
-	} else if (cam->bus_type == FIMC_ITU_601 ||
-		   cam->bus_type == FIMC_ITU_656) {
+		break;
+	case FIMC_ITU_601...FIMC_ITU_656:
 		if (cam->mux_id == 0) /* ITU-A, ITU-B: 0, 1 */
 			cfg |= FIMC_REG_CIGCTRL_SELCAM_ITU_A;
-	} else if (cam->bus_type == FIMC_LCD_WB) {
+		break;
+	case FIMC_LCD_WB:
 		cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
-	} else {
-		err("invalid camera bus type selected\n");
+		break;
+	default:
+		v4l2_err(vid_cap->vfd, "Invalid camera bus type selected\n");
 		return -EINVAL;
 	}
 	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
diff --git a/drivers/media/video/s5p-g2d/g2d.c b/drivers/media/video/s5p-g2d/g2d.c
index 7c98ee7377e..7c220043520 100644
--- a/drivers/media/video/s5p-g2d/g2d.c
+++ b/drivers/media/video/s5p-g2d/g2d.c
@@ -290,8 +290,13 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strncpy(cap->card, G2D_NAME, sizeof(cap->card) - 1);
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
-							| V4L2_CAP_STREAMING;
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
+			    V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-jpeg/jpeg-core.c b/drivers/media/video/s5p-jpeg/jpeg-core.c
index 28b5225d94f..813b801238d 100644
--- a/drivers/media/video/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/video/s5p-jpeg/jpeg-core.c
@@ -489,9 +489,13 @@ static int s5p_jpeg_querycap(struct file *file, void *priv,
 			sizeof(cap->card));
 	}
 	cap->bus_info[0] = 0;
-	cap->capabilities = V4L2_CAP_STREAMING |
-			    V4L2_CAP_VIDEO_CAPTURE |
-			    V4L2_CAP_VIDEO_OUTPUT;
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M |
+			    V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT;
 	return 0;
 }
 
@@ -824,10 +828,10 @@ static int s5p_jpeg_g_selection(struct file *file, void *priv,
 
 	/* For JPEG blob active == default == bounds */
 	switch (s->target) {
-	case V4L2_SEL_TGT_CROP_ACTIVE:
+	case V4L2_SEL_TGT_CROP:
 	case V4L2_SEL_TGT_CROP_BOUNDS:
 	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	case V4L2_SEL_TGT_COMPOSE:
 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 		s->r.width = ctx->out_q.w;
 		s->r.height = ctx->out_q.h;
@@ -1503,29 +1507,7 @@ static struct platform_driver s5p_jpeg_driver = {
 	},
 };
 
-static int __init
-s5p_jpeg_register(void)
-{
-	int ret;
-
-	pr_info("S5P JPEG V4L2 Driver, (c) 2011 Samsung Electronics\n");
-
-	ret = platform_driver_register(&s5p_jpeg_driver);
-
-	if (ret)
-		pr_err("%s: failed to register jpeg driver\n", __func__);
-
-	return ret;
-}
-
-static void __exit
-s5p_jpeg_unregister(void)
-{
-	platform_driver_unregister(&s5p_jpeg_driver);
-}
-
-module_init(s5p_jpeg_register);
-module_exit(s5p_jpeg_unregister);
+module_platform_driver(s5p_jpeg_driver);
 
 MODULE_AUTHOR("Andrzej Pietrasiewicz <andrzej.p@samsung.com>");
 MODULE_DESCRIPTION("Samsung JPEG codec driver");
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_dec.c b/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
index feea867f318..c5d567f87d7 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_dec.c
@@ -220,8 +220,14 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strncpy(cap->card, dev->plat_dev->name, sizeof(cap->card) - 1);
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
-			V4L2_CAP_VIDEO_OUTPUT_MPLANE | V4L2_CAP_STREAMING;
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->capabilities = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING |
+			    V4L2_CAP_VIDEO_CAPTURE_MPLANE |
+			    V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
index 158b78989b8..aa1c244cf66 100644
--- a/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/video/s5p-mfc/s5p_mfc_enc.c
@@ -779,9 +779,14 @@ static int vidioc_querycap(struct file *file, void *priv,
 	strncpy(cap->card, dev->plat_dev->name, sizeof(cap->card) - 1);
 	cap->bus_info[0] = 0;
 	cap->version = KERNEL_VERSION(1, 0, 0);
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE_MPLANE
-			  | V4L2_CAP_VIDEO_OUTPUT_MPLANE
-			  | V4L2_CAP_STREAMING;
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->capabilities = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING |
+			    V4L2_CAP_VIDEO_CAPTURE_MPLANE |
+			    V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 	return 0;
 }
 
diff --git a/drivers/media/video/s5p-tv/mixer_video.c b/drivers/media/video/s5p-tv/mixer_video.c
index 33fde2a763e..6c74b05d1f9 100644
--- a/drivers/media/video/s5p-tv/mixer_video.c
+++ b/drivers/media/video/s5p-tv/mixer_video.c
@@ -367,7 +367,7 @@ static int mxr_g_selection(struct file *file, void *fh,
 		return -EINVAL;
 
 	switch (s->target) {
-	case V4L2_SEL_TGT_CROP_ACTIVE:
+	case V4L2_SEL_TGT_CROP:
 		s->r.left = geo->src.x_offset;
 		s->r.top = geo->src.y_offset;
 		s->r.width = geo->src.width;
@@ -380,7 +380,7 @@ static int mxr_g_selection(struct file *file, void *fh,
 		s->r.width = geo->src.full_width;
 		s->r.height = geo->src.full_height;
 		break;
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	case V4L2_SEL_TGT_COMPOSE:
 	case V4L2_SEL_TGT_COMPOSE_PADDED:
 		s->r.left = geo->dst.x_offset;
 		s->r.top = geo->dst.y_offset;
@@ -449,11 +449,11 @@ static int mxr_s_selection(struct file *file, void *fh,
 		res.height = geo->dst.full_height;
 		break;
 
-	case V4L2_SEL_TGT_CROP_ACTIVE:
+	case V4L2_SEL_TGT_CROP:
 		target = &geo->src;
 		stage = MXR_GEOMETRY_CROP;
 		break;
-	case V4L2_SEL_TGT_COMPOSE_ACTIVE:
+	case V4L2_SEL_TGT_COMPOSE:
 	case V4L2_SEL_TGT_COMPOSE_PADDED:
 		target = &geo->dst;
 		stage = MXR_GEOMETRY_COMPOSE;
diff --git a/drivers/media/video/s5p-tv/sii9234_drv.c b/drivers/media/video/s5p-tv/sii9234_drv.c
index 0f31eccd7b8..6d348f90237 100644
--- a/drivers/media/video/s5p-tv/sii9234_drv.c
+++ b/drivers/media/video/s5p-tv/sii9234_drv.c
@@ -419,14 +419,4 @@ static struct i2c_driver sii9234_driver = {
 	.id_table = sii9234_id,
 };
 
-static int __init sii9234_init(void)
-{
-	return i2c_add_driver(&sii9234_driver);
-}
-module_init(sii9234_init);
-
-static void __exit sii9234_exit(void)
-{
-	i2c_del_driver(&sii9234_driver);
-}
-module_exit(sii9234_exit);
+module_i2c_driver(sii9234_driver);
diff --git a/drivers/media/video/saa7121.h b/drivers/media/video/saa7121.h
deleted file mode 100644
index 66967ae3749..00000000000
--- a/drivers/media/video/saa7121.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/* saa7121.h - saa7121 initializations
-   Copyright (C) 1999 Nathan Laredo (laredo@gnu.org)
-
-   This program is free software; you can redistribute it and/or modify
-   it under the terms of the GNU General Public License as published by
-   the Free Software Foundation; either version 2 of the License, or
-   (at your option) any later version.
-
-   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
-
-   You 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 __SAA7121_H__
-#define __SAA7121_H__
-
-#define NTSC_BURST_START	0x19	/* 28 */
-#define NTSC_BURST_END		0x1d	/* 29 */
-#define NTSC_CHROMA_PHASE	0x67	/* 5a */
-#define NTSC_GAINU		0x76	/* 5b */
-#define NTSC_GAINV		0xa5	/* 5c */
-#define NTSC_BLACK_LEVEL	0x2a	/* 5d */
-#define NTSC_BLANKING_LEVEL	0x2e	/* 5e */
-#define NTSC_VBI_BLANKING	0x2e	/* 5f */
-#define NTSC_DAC_CONTROL	0x11	/* 61 */
-#define NTSC_BURST_AMP		0x3f	/* 62 */
-#define NTSC_SUBC3		0x1f	/* 63 */
-#define NTSC_SUBC2		0x7c	/* 64 */
-#define NTSC_SUBC1		0xf0	/* 65 */
-#define NTSC_SUBC0		0x21	/* 66 */
-#define NTSC_HTRIG		0x72	/* 6c */
-#define NTSC_VTRIG		0x00	/* 6c */
-#define NTSC_MULTI		0x30	/* 6e */
-#define NTSC_CCTTX		0x11	/* 6f */
-#define NTSC_FIRST_ACTIVE	0x12	/* 7a */
-#define NTSC_LAST_ACTIVE	0x02	/* 7b */
-#define NTSC_MSB_VERTICAL	0x40	/* 7c */
-
-#define PAL_BURST_START		0x21	/* 28 */
-#define PAL_BURST_END		0x1d	/* 29 */
-#define PAL_CHROMA_PHASE	0x3f	/* 5a */
-#define PAL_GAINU		0x7d	/* 5b */
-#define PAL_GAINV		0xaf	/* 5c */
-#define PAL_BLACK_LEVEL		0x23	/* 5d */
-#define PAL_BLANKING_LEVEL	0x35	/* 5e */
-#define PAL_VBI_BLANKING	0x35	/* 5f */
-#define PAL_DAC_CONTROL		0x02	/* 61 */
-#define PAL_BURST_AMP		0x2f	/* 62 */
-#define PAL_SUBC3		0xcb	/* 63 */
-#define PAL_SUBC2		0x8a	/* 64 */
-#define PAL_SUBC1		0x09	/* 65 */
-#define PAL_SUBC0		0x2a	/* 66 */
-#define PAL_HTRIG		0x86	/* 6c */
-#define PAL_VTRIG		0x04	/* 6d */
-#define PAL_MULTI		0x20	/* 6e */
-#define PAL_CCTTX		0x15	/* 6f */
-#define PAL_FIRST_ACTIVE	0x16	/* 7a */
-#define PAL_LAST_ACTIVE		0x36	/* 7b */
-#define PAL_MSB_VERTICAL	0x40	/* 7c */
-
-/* Initialization Sequence */
-
-static __u8 init7121ntsc[] = {
-	0x26, 0x0,	0x27, 0x0,
-	0x28, NTSC_BURST_START,		0x29, NTSC_BURST_END,
-	0x2a, 0x0,	0x2b, 0x0,	0x2c, 0x0,	0x2d, 0x0,
-	0x2e, 0x0,	0x2f, 0x0,	0x30, 0x0,	0x31, 0x0,
-	0x32, 0x0,	0x33, 0x0,	0x34, 0x0,	0x35, 0x0,
-	0x36, 0x0,	0x37, 0x0,	0x38, 0x0,	0x39, 0x0,
-	0x3a, 0x03,	0x3b, 0x0,	0x3c, 0x0,	0x3d, 0x0,
-	0x3e, 0x0,	0x3f, 0x0,	0x40, 0x0,	0x41, 0x0,
-	0x42, 0x0,	0x43, 0x0,	0x44, 0x0,	0x45, 0x0,
-	0x46, 0x0,	0x47, 0x0,	0x48, 0x0,	0x49, 0x0,
-	0x4a, 0x0,	0x4b, 0x0,	0x4c, 0x0,	0x4d, 0x0,
-	0x4e, 0x0,	0x4f, 0x0,	0x50, 0x0,	0x51, 0x0,
-	0x52, 0x0,	0x53, 0x0,	0x54, 0x0,	0x55, 0x0,
-	0x56, 0x0,	0x57, 0x0,	0x58, 0x0,	0x59, 0x0,
-	0x5a, NTSC_CHROMA_PHASE,	0x5b, NTSC_GAINU,
-	0x5c, NTSC_GAINV,		0x5d, NTSC_BLACK_LEVEL,
-	0x5e, NTSC_BLANKING_LEVEL,	0x5f, NTSC_VBI_BLANKING,
-	0x60, 0x0,			0x61, NTSC_DAC_CONTROL,
-	0x62, NTSC_BURST_AMP,		0x63, NTSC_SUBC3,
-	0x64, NTSC_SUBC2,		0x65, NTSC_SUBC1,
-	0x66, NTSC_SUBC0,		0x67, 0x80,	0x68, 0x80,
-	0x69, 0x80,	0x6a, 0x80,	0x6b, 0x29,
-	0x6c, NTSC_HTRIG,		0x6d, NTSC_VTRIG,
-	0x6e, NTSC_MULTI,		0x6f, NTSC_CCTTX,
-	0x70, 0xc9,	0x71, 0x68,	0x72, 0x60,	0x73, 0x0,
-	0x74, 0x0,	0x75, 0x0,	0x76, 0x0,	0x77, 0x0,
-	0x78, 0x0,	0x79, 0x0,	0x7a, NTSC_FIRST_ACTIVE,
-	0x7b, NTSC_LAST_ACTIVE,		0x7c, NTSC_MSB_VERTICAL,
-	0x7d, 0x0,	0x7e, 0x0,	0x7f, 0x0
-};
-#define INIT7121LEN	(sizeof(init7121ntsc)/2)
-
-static __u8 init7121pal[] = {
-	0x26, 0x0,	0x27, 0x0,
-	0x28, PAL_BURST_START,		0x29, PAL_BURST_END,
-	0x2a, 0x0,	0x2b, 0x0,	0x2c, 0x0,	0x2d, 0x0,
-	0x2e, 0x0,	0x2f, 0x0,	0x30, 0x0,	0x31, 0x0,
-	0x32, 0x0,	0x33, 0x0,	0x34, 0x0,	0x35, 0x0,
-	0x36, 0x0,	0x37, 0x0,	0x38, 0x0,	0x39, 0x0,
-	0x3a, 0x03,	0x3b, 0x0,	0x3c, 0x0,	0x3d, 0x0,
-	0x3e, 0x0,	0x3f, 0x0,	0x40, 0x0,	0x41, 0x0,
-	0x42, 0x0,	0x43, 0x0,	0x44, 0x0,	0x45, 0x0,
-	0x46, 0x0,	0x47, 0x0,	0x48, 0x0,	0x49, 0x0,
-	0x4a, 0x0,	0x4b, 0x0,	0x4c, 0x0,	0x4d, 0x0,
-	0x4e, 0x0,	0x4f, 0x0,	0x50, 0x0,	0x51, 0x0,
-	0x52, 0x0,	0x53, 0x0,	0x54, 0x0,	0x55, 0x0,
-	0x56, 0x0,	0x57, 0x0,	0x58, 0x0,	0x59, 0x0,
-	0x5a, PAL_CHROMA_PHASE,		0x5b, PAL_GAINU,
-	0x5c, PAL_GAINV,		0x5d, PAL_BLACK_LEVEL,
-	0x5e, PAL_BLANKING_LEVEL,	0x5f, PAL_VBI_BLANKING,
-	0x60, 0x0,			0x61, PAL_DAC_CONTROL,
-	0x62, PAL_BURST_AMP,		0x63, PAL_SUBC3,
-	0x64, PAL_SUBC2,		0x65, PAL_SUBC1,
-	0x66, PAL_SUBC0,		0x67, 0x80,	0x68, 0x80,
-	0x69, 0x80,	0x6a, 0x80,	0x6b, 0x29,
-	0x6c, PAL_HTRIG,		0x6d, PAL_VTRIG,
-	0x6e, PAL_MULTI,		0x6f, PAL_CCTTX,
-	0x70, 0xc9,	0x71, 0x68,	0x72, 0x60,	0x73, 0x0,
-	0x74, 0x0,	0x75, 0x0,	0x76, 0x0,	0x77, 0x0,
-	0x78, 0x0,	0x79, 0x0,	0x7a, PAL_FIRST_ACTIVE,
-	0x7b, PAL_LAST_ACTIVE,		0x7c, PAL_MSB_VERTICAL,
-	0x7d, 0x0,	0x7e, 0x0,	0x7f, 0x0
-};
-#endif
diff --git a/drivers/media/video/saa7134/saa7134-dvb.c b/drivers/media/video/saa7134/saa7134-dvb.c
index 5dfd826d734..cc7f3d6ee96 100644
--- a/drivers/media/video/saa7134/saa7134-dvb.c
+++ b/drivers/media/video/saa7134/saa7134-dvb.c
@@ -1282,7 +1282,7 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_FLYDVBT_DUO_CARDBUS:
 		if (configure_tda827x_fe(dev, &tda827x_lifeview_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_PHILIPS_EUROPA:
 	case SAA7134_BOARD_VIDEOMATE_DVBT_300:
@@ -1322,7 +1322,7 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_KWORLD_DVBT_210:
 		if (configure_tda827x_fe(dev, &kworld_dvb_t_210_config,
 					 &tda827x_cfg_2) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_HAUPPAUGE_HVR1120:
 		fe0->dvb.frontend = dvb_attach(tda10048_attach,
@@ -1340,17 +1340,17 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_PHILIPS_TIGER:
 		if (configure_tda827x_fe(dev, &philips_tiger_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_PINNACLE_PCTV_310i:
 		if (configure_tda827x_fe(dev, &pinnacle_pctv_310i_config,
 					 &tda827x_cfg_1) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_HAUPPAUGE_HVR1110:
 		if (configure_tda827x_fe(dev, &hauppauge_hvr_1110_config,
 					 &tda827x_cfg_1) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_HAUPPAUGE_HVR1150:
 		fe0->dvb.frontend = dvb_attach(lgdt3305_attach,
@@ -1368,30 +1368,30 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_ASUSTeK_P7131_DUAL:
 		if (configure_tda827x_fe(dev, &asus_p7131_dual_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_FLYDVBT_LR301:
 		if (configure_tda827x_fe(dev, &tda827x_lifeview_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_FLYDVB_TRIO:
 		if (!use_frontend) {	/* terrestrial */
 			if (configure_tda827x_fe(dev, &lifeview_trio_config,
 						 &tda827x_cfg_0) < 0)
-				goto dettach_frontend;
+				goto detach_frontend;
 		} else {  		/* satellite */
 			fe0->dvb.frontend = dvb_attach(tda10086_attach, &flydvbs, &dev->i2c_adap);
 			if (fe0->dvb.frontend) {
 				if (dvb_attach(tda826x_attach, fe0->dvb.frontend, 0x63,
 									&dev->i2c_adap, 0) == NULL) {
 					wprintk("%s: Lifeview Trio, No tda826x found!\n", __func__);
-					goto dettach_frontend;
+					goto detach_frontend;
 				}
 				if (dvb_attach(isl6421_attach, fe0->dvb.frontend, &dev->i2c_adap,
 										0x08, 0, 0) == NULL) {
 					wprintk("%s: Lifeview Trio, No ISL6421 found!\n", __func__);
-					goto dettach_frontend;
+					goto detach_frontend;
 				}
 			}
 		}
@@ -1407,7 +1407,7 @@ static int dvb_init(struct saa7134_dev *dev)
 								&ads_duo_cfg) == NULL) {
 				wprintk("no tda827x tuner found at addr: %02x\n",
 					ads_tech_duo_config.tuner_address);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 		} else
 			wprintk("failed to attach tda10046\n");
@@ -1415,13 +1415,13 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_TEVION_DVBT_220RF:
 		if (configure_tda827x_fe(dev, &tevion_dvbt220rf_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_MEDION_MD8800_QUADRO:
 		if (!use_frontend) {     /* terrestrial */
 			if (configure_tda827x_fe(dev, &md8800_dvbt_config,
 						 &tda827x_cfg_0) < 0)
-				goto dettach_frontend;
+				goto detach_frontend;
 		} else {        /* satellite */
 			fe0->dvb.frontend = dvb_attach(tda10086_attach,
 							&flydvbs, &dev->i2c_adap);
@@ -1435,7 +1435,7 @@ static int dvb_init(struct saa7134_dev *dev)
 						0x60, &dev->i2c_adap, 0) == NULL) {
 					wprintk("%s: Medion Quadro, no tda826x "
 						"found !\n", __func__);
-					goto dettach_frontend;
+					goto detach_frontend;
 				}
 				if (dev_id != 0x08) {
 					/* we need to open the i2c gate (we know it exists) */
@@ -1444,7 +1444,7 @@ static int dvb_init(struct saa7134_dev *dev)
 							&dev->i2c_adap, 0x08, 0, 0) == NULL) {
 						wprintk("%s: Medion Quadro, no ISL6405 "
 							"found !\n", __func__);
-						goto dettach_frontend;
+						goto detach_frontend;
 					}
 					if (dev_id == 0x07) {
 						/* fire up the 2nd section of the LNB supply since
@@ -1503,12 +1503,12 @@ static int dvb_init(struct saa7134_dev *dev)
 			if (dvb_attach(tda826x_attach, fe0->dvb.frontend, 0x60,
 				       &dev->i2c_adap, 0) == NULL) {
 				wprintk("%s: No tda826x found!\n", __func__);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 			if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
 				       &dev->i2c_adap, 0x08, 0, 0) == NULL) {
 				wprintk("%s: No ISL6421 found!\n", __func__);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 		}
 		break;
@@ -1537,37 +1537,37 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_CINERGY_HT_PCMCIA:
 		if (configure_tda827x_fe(dev, &cinergy_ht_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_CINERGY_HT_PCI:
 		if (configure_tda827x_fe(dev, &cinergy_ht_pci_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_PHILIPS_TIGER_S:
 		if (configure_tda827x_fe(dev, &philips_tiger_s_config,
 					 &tda827x_cfg_2) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_ASUS_P7131_4871:
 		if (configure_tda827x_fe(dev, &asus_p7131_4871_config,
 					 &tda827x_cfg_2) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_ASUSTeK_P7131_HYBRID_LNA:
 		if (configure_tda827x_fe(dev, &asus_p7131_hybrid_lna_config,
 					 &tda827x_cfg_2) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_AVERMEDIA_SUPER_007:
 		if (configure_tda827x_fe(dev, &avermedia_super_007_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_TWINHAN_DTV_DVB_3056:
 		if (configure_tda827x_fe(dev, &twinhan_dtv_dvb_3056_config,
 					 &tda827x_cfg_2_sw42) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_PHILIPS_SNAKE:
 		fe0->dvb.frontend = dvb_attach(tda10086_attach, &flydvbs,
@@ -1576,24 +1576,24 @@ static int dvb_init(struct saa7134_dev *dev)
 			if (dvb_attach(tda826x_attach, fe0->dvb.frontend, 0x60,
 					&dev->i2c_adap, 0) == NULL) {
 				wprintk("%s: No tda826x found!\n", __func__);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 			if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
 					&dev->i2c_adap, 0, 0) == NULL) {
 				wprintk("%s: No lnbp21 found!\n", __func__);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 		}
 		break;
 	case SAA7134_BOARD_CREATIX_CTX953:
 		if (configure_tda827x_fe(dev, &md8800_dvbt_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_MSI_TVANYWHERE_AD11:
 		if (configure_tda827x_fe(dev, &philips_tiger_s_config,
 					 &tda827x_cfg_2) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_AVERMEDIA_CARDBUS_506:
 		dprintk("AverMedia E506R dvb setup\n");
@@ -1614,7 +1614,7 @@ static int dvb_init(struct saa7134_dev *dev)
 				  &dev->i2c_adap, DVB_PLL_PHILIPS_SD1878_TDA8261) == NULL) {
 				wprintk("%s: MD7134 DVB-S, no SD1878 "
 					"found !\n", __func__);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 			/* we need to open the i2c gate (we know it exists) */
 			fe = fe0->dvb.frontend;
@@ -1623,7 +1623,7 @@ static int dvb_init(struct saa7134_dev *dev)
 					&dev->i2c_adap, 0x08, 0, 0) == NULL) {
 				wprintk("%s: MD7134 DVB-S, no ISL6405 "
 					"found !\n", __func__);
-				goto dettach_frontend;
+				goto detach_frontend;
 			}
 			fe->ops.i2c_gate_ctrl(fe, 0);
 			dev->original_set_voltage = fe->ops.set_voltage;
@@ -1645,7 +1645,7 @@ static int dvb_init(struct saa7134_dev *dev)
 		if (!use_frontend) {     /* terrestrial */
 			if (configure_tda827x_fe(dev, &asus_tiger_3in1_config,
 							&tda827x_cfg_2) < 0)
-				goto dettach_frontend;
+				goto detach_frontend;
 		} else {  		/* satellite */
 			fe0->dvb.frontend = dvb_attach(tda10086_attach,
 						&flydvbs, &dev->i2c_adap);
@@ -1655,13 +1655,13 @@ static int dvb_init(struct saa7134_dev *dev)
 						&dev->i2c_adap, 0) == NULL) {
 					wprintk("%s: Asus Tiger 3in1, no "
 						"tda826x found!\n", __func__);
-					goto dettach_frontend;
+					goto detach_frontend;
 				}
 				if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
 						&dev->i2c_adap, 0, 0) == NULL) {
 					wprintk("%s: Asus Tiger 3in1, no lnbp21"
 						" found!\n", __func__);
-				       goto dettach_frontend;
+				       goto detach_frontend;
 			       }
 		       }
 	       }
@@ -1670,7 +1670,7 @@ static int dvb_init(struct saa7134_dev *dev)
 		if (!use_frontend) {     /* terrestrial */
 			if (configure_tda827x_fe(dev, &asus_ps3_100_config,
 						 &tda827x_cfg_2) < 0)
-				goto dettach_frontend;
+				goto detach_frontend;
 	       } else {                /* satellite */
 			fe0->dvb.frontend = dvb_attach(tda10086_attach,
 						       &flydvbs, &dev->i2c_adap);
@@ -1680,13 +1680,13 @@ static int dvb_init(struct saa7134_dev *dev)
 					       &dev->i2c_adap, 0) == NULL) {
 					wprintk("%s: Asus My Cinema PS3-100, no "
 						"tda826x found!\n", __func__);
-					goto dettach_frontend;
+					goto detach_frontend;
 				}
 				if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
 					       &dev->i2c_adap, 0, 0) == NULL) {
 					wprintk("%s: Asus My Cinema PS3-100, no lnbp21"
 						" found!\n", __func__);
-					goto dettach_frontend;
+					goto detach_frontend;
 				}
 			}
 		}
@@ -1694,7 +1694,7 @@ static int dvb_init(struct saa7134_dev *dev)
 	case SAA7134_BOARD_ASUSTeK_TIGER:
 		if (configure_tda827x_fe(dev, &philips_tiger_config,
 					 &tda827x_cfg_0) < 0)
-			goto dettach_frontend;
+			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_BEHOLD_H6:
 		fe0->dvb.frontend = dvb_attach(zl10353_attach,
@@ -1830,19 +1830,19 @@ static int dvb_init(struct saa7134_dev *dev)
 		};
 
 		if (!fe0->dvb.frontend)
-			goto dettach_frontend;
+			goto detach_frontend;
 
 		fe = dvb_attach(xc2028_attach, fe0->dvb.frontend, &cfg);
 		if (!fe) {
 			printk(KERN_ERR "%s/2: xc3028 attach failed\n",
 			       dev->name);
-			goto dettach_frontend;
+			goto detach_frontend;
 		}
 	}
 
 	if (NULL == fe0->dvb.frontend) {
 		printk(KERN_ERR "%s/dvb: frontend initialization failed\n", dev->name);
-		goto dettach_frontend;
+		goto detach_frontend;
 	}
 	/* define general-purpose callback pointer */
 	fe0->dvb.frontend->callback = saa7134_tuner_callback;
@@ -1864,7 +1864,7 @@ static int dvb_init(struct saa7134_dev *dev)
 	}
 	return ret;
 
-dettach_frontend:
+detach_frontend:
 	videobuf_dvb_dealloc_frontends(&dev->frontends);
 	return -EINVAL;
 }
diff --git a/drivers/media/video/saa7146.h b/drivers/media/video/saa7146.h
deleted file mode 100644
index 9fadb331a40..00000000000
--- a/drivers/media/video/saa7146.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
-    saa7146.h - definitions philips saa7146 based cards
-    Copyright (C) 1999 Nathan Laredo (laredo@gnu.org)
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You 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 __SAA7146__
-#define __SAA7146__
-
-#define SAA7146_VERSION_CODE 0x000101
-
-#include <linux/types.h>
-#include <linux/wait.h>
-
-#ifndef O_NONCAP
-#define O_NONCAP	O_TRUNC
-#endif
-
-#define MAX_GBUFFERS	2
-#define FBUF_SIZE	0x190000
-
-#ifdef __KERNEL__
-
-struct saa7146_window
-{
-	int x, y;
-	ushort width, height;
-	ushort bpp, bpl;
-	ushort swidth, sheight;
-	short cropx, cropy;
-	ushort cropwidth, cropheight;
-	unsigned long vidadr;
-	int color_fmt;
-	ushort depth;
-};
-
-/*  Per-open data for handling multiple opens on one device */
-struct device_open
-{
-	int	     isopen;
-	int	     noncapturing;
-	struct saa7146  *dev;
-};
-#define MAX_OPENS 3
-
-struct saa7146
-{
-	struct video_device video_dev;
-	struct video_picture picture;
-	struct video_audio audio_dev;
-	struct video_info vidinfo;
-	int user;
-	int cap;
-	int capuser;
-	int irqstate;		/* irq routine is state driven */
-	int writemode;
-	int playmode;
-	unsigned int nr;
-	unsigned long irq;          /* IRQ used by SAA7146 card */
-	unsigned short id;
-	unsigned char revision;
-	unsigned char boardcfg[64];	/* 64 bytes of config from eeprom */
-	unsigned long saa7146_adr;   /* bus address of IO mem from PCI BIOS */
-	struct saa7146_window win;
-	unsigned char __iomem *saa7146_mem; /* pointer to mapped IO memory */
-	struct device_open open_data[MAX_OPENS];
-#define MAX_MARKS 16
-	/* for a/v sync */
-	int endmark[MAX_MARKS], endmarkhead, endmarktail;
-	u32 *dmaRPS1, *pageRPS1, *dmaRPS2, *pageRPS2, *dmavid1, *dmavid2,
-		*dmavid3, *dmaa1in, *dmaa1out, *dmaa2in, *dmaa2out,
-		*pagedebi, *pagevid1, *pagevid2, *pagevid3, *pagea1in,
-		*pagea1out, *pagea2in, *pagea2out;
-	wait_queue_head_t i2cq, debiq, audq, vidq;
-	u8  *vidbuf, *audbuf, *osdbuf, *dmadebi;
-	int audhead, vidhead, osdhead, audtail, vidtail, osdtail;
-	spinlock_t lock;	/* the device lock */
-};
-#endif
-
-#ifdef _ALPHA_SAA7146
-#define saawrite(dat,adr)    writel((dat), saa->saa7146_adr+(adr))
-#define saaread(adr)         readl(saa->saa7146_adr+(adr))
-#else
-#define saawrite(dat,adr)    writel((dat), saa->saa7146_mem+(adr))
-#define saaread(adr)         readl(saa->saa7146_mem+(adr))
-#endif
-
-#define saaand(dat,adr)      saawrite((dat) & saaread(adr), adr)
-#define saaor(dat,adr)       saawrite((dat) | saaread(adr), adr)
-#define saaaor(dat,mask,adr) saawrite((dat) | ((mask) & saaread(adr)), adr)
-
-/* bitmask of attached hardware found */
-#define SAA7146_UNKNOWN		0x00000000
-#define SAA7146_SAA7111		0x00000001
-#define SAA7146_SAA7121		0x00000002
-#define SAA7146_IBMMPEG		0x00000004
-
-#endif
diff --git a/drivers/media/video/saa7146reg.h b/drivers/media/video/saa7146reg.h
deleted file mode 100644
index 80ec2c146b4..00000000000
--- a/drivers/media/video/saa7146reg.h
+++ /dev/null
@@ -1,283 +0,0 @@
-/*
-    saa7146.h - definitions philips saa7146 based cards
-    Copyright (C) 1999 Nathan Laredo (laredo@gnu.org)
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You 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 __SAA7146_REG__
-#define __SAA7146_REG__
-#define SAA7146_BASE_ODD1	0x00
-#define SAA7146_BASE_EVEN1	0x04
-#define SAA7146_PROT_ADDR1	0x08
-#define SAA7146_PITCH1		0x0c
-#define SAA7146_PAGE1		0x10
-#define SAA7146_NUM_LINE_BYTE1	0x14
-#define SAA7146_BASE_ODD2	0x18
-#define SAA7146_BASE_EVEN2	0x1c
-#define SAA7146_PROT_ADDR2	0x20
-#define SAA7146_PITCH2		0x24
-#define SAA7146_PAGE2		0x28
-#define SAA7146_NUM_LINE_BYTE2	0x2c
-#define SAA7146_BASE_ODD3	0x30
-#define SAA7146_BASE_EVEN3	0x34
-#define SAA7146_PROT_ADDR3	0x38
-#define SAA7146_PITCH3		0x3c
-#define SAA7146_PAGE3		0x40
-#define SAA7146_NUM_LINE_BYTE3	0x44
-#define SAA7146_PCI_BT_V1	0x48
-#define SAA7146_PCI_BT_V2	0x49
-#define SAA7146_PCI_BT_V3	0x4a
-#define SAA7146_PCI_BT_DEBI	0x4b
-#define SAA7146_PCI_BT_A	0x4c
-#define SAA7146_DD1_INIT	0x50
-#define SAA7146_DD1_STREAM_B	0x54
-#define SAA7146_DD1_STREAM_A	0x56
-#define SAA7146_BRS_CTRL	0x58
-#define SAA7146_HPS_CTRL	0x5c
-#define SAA7146_HPS_V_SCALE	0x60
-#define SAA7146_HPS_V_GAIN	0x64
-#define SAA7146_HPS_H_PRESCALE	0x68
-#define SAA7146_HPS_H_SCALE	0x6c
-#define SAA7146_BCS_CTRL	0x70
-#define SAA7146_CHROMA_KEY_RANGE	0x74
-#define SAA7146_CLIP_FORMAT_CTRL	0x78
-#define SAA7146_DEBI_CONFIG	0x7c
-#define SAA7146_DEBI_COMMAND	0x80
-#define SAA7146_DEBI_PAGE	0x84
-#define SAA7146_DEBI_AD		0x88
-#define SAA7146_I2C_TRANSFER	0x8c
-#define SAA7146_I2C_STATUS	0x90
-#define SAA7146_BASE_A1_IN	0x94
-#define SAA7146_PROT_A1_IN	0x98
-#define SAA7146_PAGE_A1_IN	0x9C
-#define SAA7146_BASE_A1_OUT	0xa0
-#define SAA7146_PROT_A1_OUT	0xa4
-#define SAA7146_PAGE_A1_OUT	0xa8
-#define SAA7146_BASE_A2_IN	0xac
-#define SAA7146_PROT_A2_IN	0xb0
-#define SAA7146_PAGE_A2_IN	0xb4
-#define SAA7146_BASE_A2_OUT	0xb8
-#define SAA7146_PROT_A2_OUT	0xbc
-#define SAA7146_PAGE_A2_OUT	0xc0
-#define SAA7146_RPS_PAGE0	0xc4
-#define SAA7146_RPS_PAGE1	0xc8
-#define SAA7146_RPS_THRESH0	0xcc
-#define SAA7146_RPS_THRESH1	0xd0
-#define SAA7146_RPS_TOV0	0xd4
-#define SAA7146_RPS_TOV1	0xd8
-#define SAA7146_IER		0xdc
-#define SAA7146_GPIO_CTRL	0xe0
-#define SAA7146_EC1SSR		0xe4
-#define SAA7146_EC2SSR		0xe8
-#define SAA7146_ECT1R		0xec
-#define SAA7146_ECT2R		0xf0
-#define SAA7146_ACON1		0xf4
-#define SAA7146_ACON2		0xf8
-#define SAA7146_MC1		0xfc
-#define SAA7146_MC2		0x100
-#define SAA7146_RPS_ADDR0	0x104
-#define SAA7146_RPS_ADDR1	0x108
-#define SAA7146_ISR		0x10c
-#define SAA7146_PSR		0x110
-#define SAA7146_SSR		0x114
-#define SAA7146_EC1R		0x118
-#define SAA7146_EC2R		0x11c
-#define SAA7146_VDP1		0x120
-#define SAA7146_VDP2		0x124
-#define SAA7146_VDP3		0x128
-#define SAA7146_ADP1		0x12c
-#define SAA7146_ADP2		0x130
-#define SAA7146_ADP3		0x134
-#define SAA7146_ADP4		0x138
-#define SAA7146_DDP		0x13c
-#define SAA7146_LEVEL_REP	0x140
-#define SAA7146_FB_BUFFER1	0x144
-#define SAA7146_FB_BUFFER2	0x148
-#define SAA7146_A_TIME_SLOT1	0x180
-#define SAA7146_A_TIME_SLOT2	0x1C0
-
-/* bitfield defines */
-#define MASK_31			0x80000000
-#define MASK_30			0x40000000
-#define MASK_29			0x20000000
-#define MASK_28			0x10000000
-#define MASK_27			0x08000000
-#define MASK_26			0x04000000
-#define MASK_25			0x02000000
-#define MASK_24			0x01000000
-#define MASK_23			0x00800000
-#define MASK_22			0x00400000
-#define MASK_21			0x00200000
-#define MASK_20			0x00100000
-#define MASK_19			0x00080000
-#define MASK_18			0x00040000
-#define MASK_17			0x00020000
-#define MASK_16			0x00010000
-#define MASK_15			0x00008000
-#define MASK_14			0x00004000
-#define MASK_13			0x00002000
-#define MASK_12			0x00001000
-#define MASK_11			0x00000800
-#define MASK_10			0x00000400
-#define MASK_09			0x00000200
-#define MASK_08			0x00000100
-#define MASK_07			0x00000080
-#define MASK_06			0x00000040
-#define MASK_05			0x00000020
-#define MASK_04			0x00000010
-#define MASK_03			0x00000008
-#define MASK_02			0x00000004
-#define MASK_01			0x00000002
-#define MASK_00			0x00000001
-#define MASK_B0			0x000000ff
-#define MASK_B1			0x0000ff00
-#define MASK_B2			0x00ff0000
-#define MASK_B3			0xff000000
-#define MASK_W0			0x0000ffff
-#define MASK_W1			0xffff0000
-#define MASK_PA			0xfffffffc
-#define MASK_PR			0xfffffffe
-#define MASK_ER			0xffffffff
-#define MASK_NONE		0x00000000
-
-#define SAA7146_PAGE_MAP_EN	MASK_11
-/* main control register 1 */
-#define SAA7146_MC1_MRST_N	MASK_15
-#define SAA7146_MC1_ERPS1	MASK_13
-#define SAA7146_MC1_ERPS0	MASK_12
-#define SAA7146_MC1_EDP		MASK_11
-#define SAA7146_MC1_EVP		MASK_10
-#define SAA7146_MC1_EAP		MASK_09
-#define SAA7146_MC1_EI2C	MASK_08
-#define SAA7146_MC1_TR_E_DEBI	MASK_07
-#define SAA7146_MC1_TR_E_1	MASK_06
-#define SAA7146_MC1_TR_E_2	MASK_05
-#define SAA7146_MC1_TR_E_3	MASK_04
-#define SAA7146_MC1_TR_E_A2_OUT	MASK_03
-#define SAA7146_MC1_TR_E_A2_IN	MASK_02
-#define SAA7146_MC1_TR_E_A1_OUT	MASK_01
-#define SAA7146_MC1_TR_E_A1_IN	MASK_00
-/* main control register 2 */
-#define SAA7146_MC2_RPS_SIG4	MASK_15
-#define SAA7146_MC2_RPS_SIG3	MASK_14
-#define SAA7146_MC2_RPS_SIG2	MASK_13
-#define SAA7146_MC2_RPS_SIG1	MASK_12
-#define SAA7146_MC2_RPS_SIG0	MASK_11
-#define SAA7146_MC2_UPLD_D1_B	MASK_10
-#define SAA7146_MC2_UPLD_D1_A	MASK_09
-#define SAA7146_MC2_UPLD_BRS	MASK_08
-#define SAA7146_MC2_UPLD_HPS_H	MASK_06
-#define SAA7146_MC2_UPLD_HPS_V	MASK_05
-#define SAA7146_MC2_UPLD_DMA3	MASK_04
-#define SAA7146_MC2_UPLD_DMA2	MASK_03
-#define SAA7146_MC2_UPLD_DMA1	MASK_02
-#define SAA7146_MC2_UPLD_DEBI	MASK_01
-#define SAA7146_MC2_UPLD_I2C	MASK_00
-/* Primary Status Register and Interrupt Enable/Status Registers */
-#define SAA7146_PSR_PPEF	MASK_31
-#define SAA7146_PSR_PABO	MASK_30
-#define SAA7146_PSR_PPED	MASK_29
-#define SAA7146_PSR_RPS_I1	MASK_28
-#define SAA7146_PSR_RPS_I0	MASK_27
-#define SAA7146_PSR_RPS_LATE1	MASK_26
-#define SAA7146_PSR_RPS_LATE0	MASK_25
-#define SAA7146_PSR_RPS_E1	MASK_24
-#define SAA7146_PSR_RPS_E0	MASK_23
-#define SAA7146_PSR_RPS_TO1	MASK_22
-#define SAA7146_PSR_RPS_TO0	MASK_21
-#define SAA7146_PSR_UPLD	MASK_20
-#define SAA7146_PSR_DEBI_S	MASK_19
-#define SAA7146_PSR_DEBI_E	MASK_18
-#define SAA7146_PSR_I2C_S	MASK_17
-#define SAA7146_PSR_I2C_E	MASK_16
-#define SAA7146_PSR_A2_IN	MASK_15
-#define SAA7146_PSR_A2_OUT	MASK_14
-#define SAA7146_PSR_A1_IN	MASK_13
-#define SAA7146_PSR_A1_OUT	MASK_12
-#define SAA7146_PSR_AFOU	MASK_11
-#define SAA7146_PSR_V_PE	MASK_10
-#define SAA7146_PSR_VFOU	MASK_09
-#define SAA7146_PSR_FIDA	MASK_08
-#define SAA7146_PSR_FIDB	MASK_07
-#define SAA7146_PSR_PIN3	MASK_06
-#define SAA7146_PSR_PIN2	MASK_05
-#define SAA7146_PSR_PIN1	MASK_04
-#define SAA7146_PSR_PIN0	MASK_03
-#define SAA7146_PSR_ECS		MASK_02
-#define SAA7146_PSR_EC3S	MASK_01
-#define SAA7146_PSR_EC0S	MASK_00
-/* Secondary Status Register */
-#define SAA7146_SSR_PRQ		MASK_31
-#define SAA7146_SSR_PMA		MASK_30
-#define SAA7146_SSR_RPS_RE1	MASK_29
-#define SAA7146_SSR_RPS_PE1	MASK_28
-#define SAA7146_SSR_RPS_A1	MASK_27
-#define SAA7146_SSR_RPS_RE0	MASK_26
-#define SAA7146_SSR_RPS_PE0	MASK_25
-#define SAA7146_SSR_RPS_A0	MASK_24
-#define SAA7146_SSR_DEBI_TO	MASK_23
-#define SAA7146_SSR_DEBI_EF	MASK_22
-#define SAA7146_SSR_I2C_EA	MASK_21
-#define SAA7146_SSR_I2C_EW	MASK_20
-#define SAA7146_SSR_I2C_ER	MASK_19
-#define SAA7146_SSR_I2C_EL	MASK_18
-#define SAA7146_SSR_I2C_EF	MASK_17
-#define SAA7146_SSR_V3P		MASK_16
-#define SAA7146_SSR_V2P		MASK_15
-#define SAA7146_SSR_V1P		MASK_14
-#define SAA7146_SSR_VF3		MASK_13
-#define SAA7146_SSR_VF2		MASK_12
-#define SAA7146_SSR_VF1		MASK_11
-#define SAA7146_SSR_AF2_IN	MASK_10
-#define SAA7146_SSR_AF2_OUT	MASK_09
-#define SAA7146_SSR_AF1_IN	MASK_08
-#define SAA7146_SSR_AF1_OUT	MASK_07
-#define SAA7146_SSR_VGT		MASK_05
-#define SAA7146_SSR_LNQG	MASK_04
-#define SAA7146_SSR_EC5S	MASK_03
-#define SAA7146_SSR_EC4S	MASK_02
-#define SAA7146_SSR_EC2S	MASK_01
-#define SAA7146_SSR_EC1S	MASK_00
-/* I2C status register */
-#define SAA7146_I2C_ABORT	MASK_07
-#define SAA7146_I2C_SPERR	MASK_06
-#define SAA7146_I2C_APERR	MASK_05
-#define SAA7146_I2C_DTERR	MASK_04
-#define SAA7146_I2C_DRERR	MASK_03
-#define SAA7146_I2C_AL		MASK_02
-#define SAA7146_I2C_ERR		MASK_01
-#define SAA7146_I2C_BUSY	MASK_00
-/* output formats */
-#define SAA7146_YUV422	0
-#define SAA7146_RGB16	0
-#define SAA7146_YUV444	1
-#define SAA7146_RGB24	1
-#define SAA7146_ARGB32	2
-#define SAA7146_YUV411	3
-#define SAA7146_ARGB15  3
-#define SAA7146_YUV2	4
-#define SAA7146_RGAB15	4
-#define SAA7146_Y8	6
-#define SAA7146_YUV8	7
-#define SAA7146_RGB8	7
-#define SAA7146_YUV444p	8
-#define SAA7146_YUV422p	9
-#define SAA7146_YUV420p	10
-#define SAA7146_YUV1620	11
-#define SAA7146_Y1	13
-#define SAA7146_Y2	14
-#define SAA7146_YUV1	15
-#endif
diff --git a/drivers/media/video/saa7164/saa7164-api.c b/drivers/media/video/saa7164/saa7164-api.c
index 8a98ab68239..c8799fdaae6 100644
--- a/drivers/media/video/saa7164/saa7164-api.c
+++ b/drivers/media/video/saa7164/saa7164-api.c
@@ -1367,7 +1367,6 @@ int saa7164_api_i2c_read(struct saa7164_i2c *bus, u8 addr, u32 reglen, u8 *reg,
 	struct saa7164_dev *dev = bus->dev;
 	u16 len = 0;
 	int unitid;
-	u32 regval;
 	u8 buf[256];
 	int ret;
 
@@ -1376,19 +1375,6 @@ int saa7164_api_i2c_read(struct saa7164_i2c *bus, u8 addr, u32 reglen, u8 *reg,
 	if (reglen > 4)
 		return -EIO;
 
-	if (reglen == 1)
-		regval = *(reg);
-	else
-	if (reglen == 2)
-		regval = ((*(reg) << 8) || *(reg+1));
-	else
-	if (reglen == 3)
-		regval = ((*(reg) << 16) | (*(reg+1) << 8) | *(reg+2));
-	else
-	if (reglen == 4)
-		regval = ((*(reg) << 24) | (*(reg+1) << 16) |
-			(*(reg+2) << 8) | *(reg+3));
-
 	/* Prepare the send buffer */
 	/* Bytes 00-03 source register length
 	 *       04-07 source bytes to read
diff --git a/drivers/media/video/saa7164/saa7164-i2c.c b/drivers/media/video/saa7164/saa7164-i2c.c
index 26148f76cba..4f7e3b42263 100644
--- a/drivers/media/video/saa7164/saa7164-i2c.c
+++ b/drivers/media/video/saa7164/saa7164-i2c.c
@@ -69,15 +69,6 @@ err:
 	return retval;
 }
 
-void saa7164_call_i2c_clients(struct saa7164_i2c *bus, unsigned int cmd,
-	void *arg)
-{
-	if (bus->i2c_rc != 0)
-		return;
-
-	i2c_clients_command(&bus->i2c_adap, cmd, arg);
-}
-
 static u32 saa7164_functionality(struct i2c_adapter *adap)
 {
 	return I2C_FUNC_I2C;
@@ -106,21 +97,14 @@ int saa7164_i2c_register(struct saa7164_i2c *bus)
 
 	dprintk(DBGLVL_I2C, "%s(bus = %d)\n", __func__, bus->nr);
 
-	memcpy(&bus->i2c_adap, &saa7164_i2c_adap_template,
-	       sizeof(bus->i2c_adap));
-
-	memcpy(&bus->i2c_algo, &saa7164_i2c_algo_template,
-	       sizeof(bus->i2c_algo));
-
-	memcpy(&bus->i2c_client, &saa7164_i2c_client_template,
-	       sizeof(bus->i2c_client));
+	bus->i2c_adap = saa7164_i2c_adap_template;
+	bus->i2c_client = saa7164_i2c_client_template;
 
 	bus->i2c_adap.dev.parent = &dev->pci->dev;
 
 	strlcpy(bus->i2c_adap.name, bus->dev->name,
 		sizeof(bus->i2c_adap.name));
 
-	bus->i2c_algo.data = bus;
 	bus->i2c_adap.algo_data = bus;
 	i2c_set_adapdata(&bus->i2c_adap, bus);
 	i2c_add_adapter(&bus->i2c_adap);
diff --git a/drivers/media/video/saa7164/saa7164.h b/drivers/media/video/saa7164/saa7164.h
index 8d120e3baf7..35219b9b0fb 100644
--- a/drivers/media/video/saa7164/saa7164.h
+++ b/drivers/media/video/saa7164/saa7164.h
@@ -46,7 +46,6 @@
 
 #include <linux/pci.h>
 #include <linux/i2c.h>
-#include <linux/i2c-algo-bit.h>
 #include <linux/kdev_t.h>
 #include <linux/mutex.h>
 #include <linux/crc32.h>
@@ -251,7 +250,6 @@ struct saa7164_i2c {
 
 	/* I2C I/O */
 	struct i2c_adapter		i2c_adap;
-	struct i2c_algo_bit_data	i2c_algo;
 	struct i2c_client		i2c_client;
 	u32				i2c_rc;
 };
diff --git a/drivers/media/video/smiapp/Kconfig b/drivers/media/video/smiapp/Kconfig
index fb99ff18be0..3149cda1d0d 100644
--- a/drivers/media/video/smiapp/Kconfig
+++ b/drivers/media/video/smiapp/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_SMIAPP
 	tristate "SMIA++/SMIA sensor support"
 	depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAVE_CLK
+	depends on MEDIA_CAMERA_SUPPORT
 	select VIDEO_SMIAPP_PLL
 	---help---
 	  This is a generic driver for SMIA++/SMIA camera modules.
diff --git a/drivers/media/video/smiapp/smiapp-core.c b/drivers/media/video/smiapp/smiapp-core.c
index 9cf5bda35fb..bfd47c10613 100644
--- a/drivers/media/video/smiapp/smiapp-core.c
+++ b/drivers/media/video/smiapp/smiapp-core.c
@@ -33,15 +33,14 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/regulator/consumer.h>
-#include <linux/slab.h>
 #include <linux/v4l2-mediabus.h>
 #include <media/v4l2-device.h>
 
 #include "smiapp.h"
 
-#define SMIAPP_ALIGN_DIM(dim, flags)		\
-	((flags) & V4L2_SUBDEV_SEL_FLAG_SIZE_GE	\
-	 ? ALIGN((dim), 2)			\
+#define SMIAPP_ALIGN_DIM(dim, flags)	\
+	((flags) & V4L2_SEL_FLAG_GE	\
+	 ? ALIGN((dim), 2)		\
 	 : (dim) & ~1)
 
 /*
@@ -1631,7 +1630,7 @@ static void smiapp_propagate(struct v4l2_subdev *subdev,
 	smiapp_get_crop_compose(subdev, fh, crops, &comp, which);
 
 	switch (target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		comp->width = crops[SMIAPP_PAD_SINK]->width;
 		comp->height = crops[SMIAPP_PAD_SINK]->height;
 		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
@@ -1647,7 +1646,7 @@ static void smiapp_propagate(struct v4l2_subdev *subdev,
 			}
 		}
 		/* Fall through */
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SEL_TGT_COMPOSE:
 		*crops[SMIAPP_PAD_SRC] = *comp;
 		break;
 	default:
@@ -1723,7 +1722,7 @@ static int smiapp_set_format(struct v4l2_subdev *subdev,
 	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 		ssd->sink_fmt = *crops[ssd->sink_pad];
 	smiapp_propagate(subdev, fh, fmt->which,
-			 V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL);
+			 V4L2_SEL_TGT_CROP);
 
 	mutex_unlock(&sensor->mutex);
 
@@ -1748,14 +1747,14 @@ static int scaling_goodness(struct v4l2_subdev *subdev, int w, int ask_w,
 	h &= ~1;
 	ask_h &= ~1;
 
-	if (flags & V4L2_SUBDEV_SEL_FLAG_SIZE_GE) {
+	if (flags & V4L2_SEL_FLAG_GE) {
 		if (w < ask_w)
 			val -= SCALING_GOODNESS;
 		if (h < ask_h)
 			val -= SCALING_GOODNESS;
 	}
 
-	if (flags & V4L2_SUBDEV_SEL_FLAG_SIZE_LE) {
+	if (flags & V4L2_SEL_FLAG_LE) {
 		if (w > ask_w)
 			val -= SCALING_GOODNESS;
 		if (h > ask_h)
@@ -1958,7 +1957,7 @@ static int smiapp_set_compose(struct v4l2_subdev *subdev,
 
 	*comp = sel->r;
 	smiapp_propagate(subdev, fh, sel->which,
-			 V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL);
+			 V4L2_SEL_TGT_COMPOSE);
 
 	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 		return smiapp_update_mode(sensor);
@@ -1974,8 +1973,8 @@ static int __smiapp_sel_supported(struct v4l2_subdev *subdev,
 
 	/* We only implement crop in three places. */
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		if (ssd == sensor->pixel_array
 		    && sel->pad == SMIAPP_PA_PAD_SRC)
 			return 0;
@@ -1988,8 +1987,8 @@ static int __smiapp_sel_supported(struct v4l2_subdev *subdev,
 		    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP)
 			return 0;
 		return -EINVAL;
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 		if (sel->pad == ssd->source_pad)
 			return -EINVAL;
 		if (ssd == sensor->binner)
@@ -2051,7 +2050,7 @@ static int smiapp_set_crop(struct v4l2_subdev *subdev,
 
 	if (ssd != sensor->pixel_array && sel->pad == SMIAPP_PAD_SINK)
 		smiapp_propagate(subdev, fh, sel->which,
-				 V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL);
+				 V4L2_SEL_TGT_CROP);
 
 	return 0;
 }
@@ -2085,7 +2084,7 @@ static int __smiapp_get_selection(struct v4l2_subdev *subdev,
 	}
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
 		if (ssd == sensor->pixel_array) {
 			sel->r.width =
 				sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
@@ -2097,11 +2096,11 @@ static int __smiapp_get_selection(struct v4l2_subdev *subdev,
 			sel->r = *comp;
 		}
 		break;
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 		sel->r = *crops[sel->pad];
 		break;
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SEL_TGT_COMPOSE:
 		sel->r = *comp;
 		break;
 	}
@@ -2148,10 +2147,10 @@ static int smiapp_set_selection(struct v4l2_subdev *subdev,
 			      sel->r.height);
 
 	switch (sel->target) {
-	case V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL:
+	case V4L2_SEL_TGT_CROP:
 		ret = smiapp_set_crop(subdev, fh, sel);
 		break;
-	case V4L2_SUBDEV_SEL_TGT_COMPOSE_ACTUAL:
+	case V4L2_SEL_TGT_COMPOSE:
 		ret = smiapp_set_compose(subdev, fh, sel);
 		break;
 	default:
diff --git a/drivers/media/video/sn9c102/sn9c102.h b/drivers/media/video/sn9c102/sn9c102.h
index 22ea211ab54..2bc153e869b 100644
--- a/drivers/media/video/sn9c102/sn9c102.h
+++ b/drivers/media/video/sn9c102/sn9c102.h
@@ -182,7 +182,7 @@ do {                                                                          \
 #	define V4LDBG(level, name, cmd)                                       \
 do {                                                                          \
 	if (debug >= (level))                                                 \
-		v4l_print_ioctl(name, cmd);                                   \
+		v4l_printk_ioctl(name, cmd);                                  \
 } while (0)
 #	define KDBG(level, fmt, args...)                                      \
 do {                                                                          \
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index 0421bf9453b..b03ffecb743 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -62,7 +62,7 @@ static int soc_camera_power_on(struct soc_camera_device *icd,
 	}
 
 	if (icl->power) {
-		ret = icl->power(icd->pdev, 1);
+		ret = icl->power(icd->control, 1);
 		if (ret < 0) {
 			dev_err(icd->pdev,
 				"Platform failed to power-on the camera.\n");
@@ -78,7 +78,7 @@ static int soc_camera_power_on(struct soc_camera_device *icd,
 
 esdpwr:
 	if (icl->power)
-		icl->power(icd->pdev, 0);
+		icl->power(icd->control, 0);
 elinkpwr:
 	regulator_bulk_disable(icl->num_regulators,
 			       icl->regulators);
@@ -95,7 +95,7 @@ static int soc_camera_power_off(struct soc_camera_device *icd,
 		return ret;
 
 	if (icl->power) {
-		ret = icl->power(icd->pdev, 0);
+		ret = icl->power(icd->control, 0);
 		if (ret < 0) {
 			dev_err(icd->pdev,
 				"Platform failed to power-off the camera.\n");
@@ -1518,6 +1518,7 @@ static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver __refdata soc_camera_pdrv = {
+	.probe = soc_camera_pdrv_probe,
 	.remove  = __devexit_p(soc_camera_pdrv_remove),
 	.driver  = {
 		.name	= "soc-camera-pdrv",
@@ -1527,7 +1528,7 @@ static struct platform_driver __refdata soc_camera_pdrv = {
 
 static int __init soc_camera_init(void)
 {
-	return platform_driver_probe(&soc_camera_pdrv, soc_camera_pdrv_probe);
+	return platform_driver_register(&soc_camera_pdrv);
 }
 
 static void __exit soc_camera_exit(void)
diff --git a/drivers/media/video/tlg2300/pd-main.c b/drivers/media/video/tlg2300/pd-main.c
index c096b3f7420..7b1f6ebd0e2 100644
--- a/drivers/media/video/tlg2300/pd-main.c
+++ b/drivers/media/video/tlg2300/pd-main.c
@@ -53,7 +53,8 @@ int debug_mode;
 module_param(debug_mode, int, 0644);
 MODULE_PARM_DESC(debug_mode, "0 = disable, 1 = enable, 2 = verbose");
 
-static const char *firmware_name = "tlg2300_firmware.bin";
+#define TLG2300_FIRMWARE "tlg2300_firmware.bin"
+static const char *firmware_name = TLG2300_FIRMWARE;
 static struct usb_driver poseidon_driver;
 static LIST_HEAD(pd_device_list);
 
@@ -532,3 +533,4 @@ MODULE_AUTHOR("Telegent Systems");
 MODULE_DESCRIPTION("For tlg2300-based USB device ");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.0.2");
+MODULE_FIRMWARE(TLG2300_FIRMWARE);
diff --git a/drivers/media/video/tuner-core.c b/drivers/media/video/tuner-core.c
index 1ad5ab6ce5c..b5a819af2b8 100644
--- a/drivers/media/video/tuner-core.c
+++ b/drivers/media/video/tuner-core.c
@@ -228,6 +228,16 @@ static int fe_has_signal(struct dvb_frontend *fe)
 	return strength;
 }
 
+static int fe_get_afc(struct dvb_frontend *fe)
+{
+	s32 afc = 0;
+
+	if (fe->ops.tuner_ops.get_afc)
+		fe->ops.tuner_ops.get_afc(fe, &afc);
+
+	return 0;
+}
+
 static int fe_set_config(struct dvb_frontend *fe, void *priv_cfg)
 {
 	struct dvb_tuner_ops *fe_tuner_ops = &fe->ops.tuner_ops;
@@ -247,6 +257,7 @@ static struct analog_demod_ops tuner_analog_ops = {
 	.set_params     = fe_set_params,
 	.standby        = fe_standby,
 	.has_signal     = fe_has_signal,
+	.get_afc        = fe_get_afc,
 	.set_config     = fe_set_config,
 	.tuner_status   = tuner_status
 };
@@ -1178,6 +1189,8 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 		return 0;
 	if (vt->type == t->mode && analog_ops->get_afc)
 		vt->afc = analog_ops->get_afc(&t->fe);
+	if (analog_ops->has_signal)
+		vt->signal = analog_ops->has_signal(&t->fe);
 	if (vt->type != V4L2_TUNER_RADIO) {
 		vt->capability |= V4L2_TUNER_CAP_NORM;
 		vt->rangelow = tv_range[0] * 16;
@@ -1197,8 +1210,6 @@ static int tuner_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 				V4L2_TUNER_SUB_STEREO :
 				V4L2_TUNER_SUB_MONO;
 		}
-		if (analog_ops->has_signal)
-			vt->signal = analog_ops->has_signal(&t->fe);
 		vt->audmode = t->audmode;
 	}
 	vt->capability |= V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
diff --git a/drivers/media/video/tvaudio.c b/drivers/media/video/tvaudio.c
index c5b1a7365e4..321b3153df8 100644
--- a/drivers/media/video/tvaudio.c
+++ b/drivers/media/video/tvaudio.c
@@ -59,8 +59,8 @@ struct CHIPSTATE;
 typedef int  (*getvalue)(int);
 typedef int  (*checkit)(struct CHIPSTATE*);
 typedef int  (*initialize)(struct CHIPSTATE*);
-typedef int  (*getmode)(struct CHIPSTATE*);
-typedef void (*setmode)(struct CHIPSTATE*, int mode);
+typedef int  (*getrxsubchans)(struct CHIPSTATE *);
+typedef void (*setaudmode)(struct CHIPSTATE*, int mode);
 
 /* i2c command */
 typedef struct AUDIOCMD {
@@ -96,8 +96,8 @@ struct CHIPDESC {
 	getvalue volfunc,treblefunc,bassfunc;
 
 	/* get/set mode */
-	getmode  getmode;
-	setmode  setmode;
+	getrxsubchans	getrxsubchans;
+	setaudmode	setaudmode;
 
 	/* input switch register + values for v4l inputs */
 	int  inputreg;
@@ -118,7 +118,7 @@ struct CHIPSTATE {
 	audiocmd   shadow;
 
 	/* current settings */
-	__u16 left,right,treble,bass,muted,mode;
+	__u16 left, right, treble, bass, muted;
 	int prevmode;
 	int radio;
 	int input;
@@ -126,7 +126,6 @@ struct CHIPSTATE {
 	/* thread */
 	struct task_struct   *thread;
 	struct timer_list    wt;
-	int                  watch_stereo;
 	int 		     audmode;
 };
 
@@ -288,7 +287,7 @@ static int chip_thread(void *data)
 	struct CHIPSTATE *chip = data;
 	struct CHIPDESC  *desc = chip->desc;
 	struct v4l2_subdev *sd = &chip->sd;
-	int mode;
+	int mode, selected;
 
 	v4l2_dbg(1, debug, sd, "thread started\n");
 	set_freezable();
@@ -302,12 +301,12 @@ static int chip_thread(void *data)
 			break;
 		v4l2_dbg(1, debug, sd, "thread wakeup\n");
 
-		/* don't do anything for radio or if mode != auto */
-		if (chip->radio || chip->mode != 0)
+		/* don't do anything for radio */
+		if (chip->radio)
 			continue;
 
 		/* have a look what's going on */
-		mode = desc->getmode(chip);
+		mode = desc->getrxsubchans(chip);
 		if (mode == chip->prevmode)
 			continue;
 
@@ -316,16 +315,32 @@ static int chip_thread(void *data)
 
 		chip->prevmode = mode;
 
-		if (mode & V4L2_TUNER_MODE_STEREO)
-			desc->setmode(chip, V4L2_TUNER_MODE_STEREO);
-		if (mode & V4L2_TUNER_MODE_LANG1_LANG2)
-			desc->setmode(chip, V4L2_TUNER_MODE_STEREO);
-		else if (mode & V4L2_TUNER_MODE_LANG1)
-			desc->setmode(chip, V4L2_TUNER_MODE_LANG1);
-		else if (mode & V4L2_TUNER_MODE_LANG2)
-			desc->setmode(chip, V4L2_TUNER_MODE_LANG2);
-		else
-			desc->setmode(chip, V4L2_TUNER_MODE_MONO);
+		selected = V4L2_TUNER_MODE_MONO;
+		switch (chip->audmode) {
+		case V4L2_TUNER_MODE_MONO:
+			if (mode & V4L2_TUNER_SUB_LANG1)
+				selected = V4L2_TUNER_MODE_LANG1;
+			break;
+		case V4L2_TUNER_MODE_STEREO:
+		case V4L2_TUNER_MODE_LANG1:
+			if (mode & V4L2_TUNER_SUB_LANG1)
+				selected = V4L2_TUNER_MODE_LANG1;
+			else if (mode & V4L2_TUNER_SUB_STEREO)
+				selected = V4L2_TUNER_MODE_STEREO;
+			break;
+		case V4L2_TUNER_MODE_LANG2:
+			if (mode & V4L2_TUNER_SUB_LANG2)
+				selected = V4L2_TUNER_MODE_LANG2;
+			else if (mode & V4L2_TUNER_SUB_STEREO)
+				selected = V4L2_TUNER_MODE_STEREO;
+			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			if (mode & V4L2_TUNER_SUB_LANG2)
+				selected = V4L2_TUNER_MODE_LANG1_LANG2;
+			else if (mode & V4L2_TUNER_SUB_STEREO)
+				selected = V4L2_TUNER_MODE_STEREO;
+		}
+		desc->setaudmode(chip, selected);
 
 		/* schedule next check */
 		mod_timer(&chip->wt, jiffies+msecs_to_jiffies(2000));
@@ -358,24 +373,25 @@ static int chip_thread(void *data)
 #define TDA9840_TEST_INT1SN 0x1 /* Integration time 0.5s when set */
 #define TDA9840_TEST_INTFU 0x02 /* Disables integrator function */
 
-static int tda9840_getmode(struct CHIPSTATE *chip)
+static int tda9840_getrxsubchans(struct CHIPSTATE *chip)
 {
 	struct v4l2_subdev *sd = &chip->sd;
 	int val, mode;
 
 	val = chip_read(chip);
-	mode = V4L2_TUNER_MODE_MONO;
+	mode = V4L2_TUNER_SUB_MONO;
 	if (val & TDA9840_DS_DUAL)
-		mode |= V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+		mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
 	if (val & TDA9840_ST_STEREO)
-		mode |= V4L2_TUNER_MODE_STEREO;
+		mode = V4L2_TUNER_SUB_STEREO;
 
-	v4l2_dbg(1, debug, sd, "tda9840_getmode(): raw chip read: %d, return: %d\n",
+	v4l2_dbg(1, debug, sd,
+		"tda9840_getrxsubchans(): raw chip read: %d, return: %d\n",
 		val, mode);
 	return mode;
 }
 
-static void tda9840_setmode(struct CHIPSTATE *chip, int mode)
+static void tda9840_setaudmode(struct CHIPSTATE *chip, int mode)
 {
 	int update = 1;
 	int t = chip->shadow.bytes[TDA9840_SW + 1] & ~0x7e;
@@ -393,6 +409,9 @@ static void tda9840_setmode(struct CHIPSTATE *chip, int mode)
 	case V4L2_TUNER_MODE_LANG2:
 		t |= TDA9840_DUALB;
 		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		t |= TDA9840_DUALAB;
+		break;
 	default:
 		update = 0;
 	}
@@ -477,6 +496,7 @@ static int tda9840_checkit(struct CHIPSTATE *chip)
 /* 0x06 - C6 - Control 2 in TDA9855, Control 3 in TDA9850 */
 /* Common to TDA9855 and TDA9850: */
 #define TDA985x_SAP	3<<6 /* Selects SAP output, mute if not received */
+#define TDA985x_MONOSAP	2<<6 /* Selects Mono on left, SAP on right */
 #define TDA985x_STEREO	1<<6 /* Selects Stereo ouput, mono if not received */
 #define TDA985x_MONO	0    /* Forces Mono output */
 #define TDA985x_LMU	1<<3 /* Mute (LOR/LOL for 9855, OUTL/OUTR for 9850) */
@@ -513,18 +533,22 @@ static int tda9855_volume(int val) { return val/0x2e8+0x27; }
 static int tda9855_bass(int val)   { return val/0xccc+0x06; }
 static int tda9855_treble(int val) { return (val/0x1c71+0x3)<<1; }
 
-static int  tda985x_getmode(struct CHIPSTATE *chip)
+static int  tda985x_getrxsubchans(struct CHIPSTATE *chip)
 {
-	int mode;
+	int mode, val;
 
-	mode = ((TDA985x_STP | TDA985x_SAPP) &
-		chip_read(chip)) >> 4;
 	/* Add mono mode regardless of SAP and stereo */
 	/* Allows forced mono */
-	return mode | V4L2_TUNER_MODE_MONO;
+	mode = V4L2_TUNER_SUB_MONO;
+	val = chip_read(chip);
+	if (val & TDA985x_STP)
+		mode = V4L2_TUNER_SUB_STEREO;
+	if (val & TDA985x_SAPP)
+		mode |= V4L2_TUNER_SUB_SAP;
+	return mode;
 }
 
-static void tda985x_setmode(struct CHIPSTATE *chip, int mode)
+static void tda985x_setaudmode(struct CHIPSTATE *chip, int mode)
 {
 	int update = 1;
 	int c6 = chip->shadow.bytes[TDA985x_C6+1] & 0x3f;
@@ -534,11 +558,15 @@ static void tda985x_setmode(struct CHIPSTATE *chip, int mode)
 		c6 |= TDA985x_MONO;
 		break;
 	case V4L2_TUNER_MODE_STEREO:
+	case V4L2_TUNER_MODE_LANG1:
 		c6 |= TDA985x_STEREO;
 		break;
-	case V4L2_TUNER_MODE_LANG1:
+	case V4L2_TUNER_MODE_SAP:
 		c6 |= TDA985x_SAP;
 		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		c6 |= TDA985x_MONOSAP;
+		break;
 	default:
 		update = 0;
 	}
@@ -583,9 +611,10 @@ static void tda985x_setmode(struct CHIPSTATE *chip, int mode)
 #define TDA9873_TR_MASK     (7 << 2)
 #define TDA9873_TR_MONO     4
 #define TDA9873_TR_STEREO   1 << 4
-#define TDA9873_TR_REVERSE  (1 << 3) & (1 << 2)
+#define TDA9873_TR_REVERSE  ((1 << 3) | (1 << 2))
 #define TDA9873_TR_DUALA    1 << 2
 #define TDA9873_TR_DUALB    1 << 3
+#define TDA9873_TR_DUALAB   0
 
 /* output level controls
  * B5:  output level switch (0 = reduced gain, 1 = normal gain)
@@ -653,46 +682,51 @@ static void tda985x_setmode(struct CHIPSTATE *chip, int mode)
 #define TDA9873_MOUT_DUALA  0
 #define TDA9873_MOUT_DUALB  1 << 3
 #define TDA9873_MOUT_ST     1 << 4
-#define TDA9873_MOUT_EXTM   (1 << 4 ) & (1 << 3)
+#define TDA9873_MOUT_EXTM   ((1 << 4) | (1 << 3))
 #define TDA9873_MOUT_EXTL   1 << 5
-#define TDA9873_MOUT_EXTR   (1 << 5 ) & (1 << 3)
-#define TDA9873_MOUT_EXTLR  (1 << 5 ) & (1 << 4)
-#define TDA9873_MOUT_MUTE   (1 << 5 ) & (1 << 4) & (1 << 3)
+#define TDA9873_MOUT_EXTR   ((1 << 5) | (1 << 3))
+#define TDA9873_MOUT_EXTLR  ((1 << 5) | (1 << 4))
+#define TDA9873_MOUT_MUTE   ((1 << 5) | (1 << 4) | (1 << 3))
 
 /* Status bits: (chip read) */
 #define TDA9873_PONR        0 /* Power-on reset detected if = 1 */
 #define TDA9873_STEREO      2 /* Stereo sound is identified     */
 #define TDA9873_DUAL        4 /* Dual sound is identified       */
 
-static int tda9873_getmode(struct CHIPSTATE *chip)
+static int tda9873_getrxsubchans(struct CHIPSTATE *chip)
 {
 	struct v4l2_subdev *sd = &chip->sd;
 	int val,mode;
 
 	val = chip_read(chip);
-	mode = V4L2_TUNER_MODE_MONO;
+	mode = V4L2_TUNER_SUB_MONO;
 	if (val & TDA9873_STEREO)
-		mode |= V4L2_TUNER_MODE_STEREO;
+		mode = V4L2_TUNER_SUB_STEREO;
 	if (val & TDA9873_DUAL)
-		mode |= V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
-	v4l2_dbg(1, debug, sd, "tda9873_getmode(): raw chip read: %d, return: %d\n",
+		mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+	v4l2_dbg(1, debug, sd,
+		"tda9873_getrxsubchans(): raw chip read: %d, return: %d\n",
 		val, mode);
 	return mode;
 }
 
-static void tda9873_setmode(struct CHIPSTATE *chip, int mode)
+static void tda9873_setaudmode(struct CHIPSTATE *chip, int mode)
 {
 	struct v4l2_subdev *sd = &chip->sd;
 	int sw_data  = chip->shadow.bytes[TDA9873_SW+1] & ~ TDA9873_TR_MASK;
 	/*	int adj_data = chip->shadow.bytes[TDA9873_AD+1] ; */
 
 	if ((sw_data & TDA9873_INP_MASK) != TDA9873_INTERNAL) {
-		v4l2_dbg(1, debug, sd, "tda9873_setmode(): external input\n");
+		v4l2_dbg(1, debug, sd,
+			 "tda9873_setaudmode(): external input\n");
 		return;
 	}
 
-	v4l2_dbg(1, debug, sd, "tda9873_setmode(): chip->shadow.bytes[%d] = %d\n", TDA9873_SW+1, chip->shadow.bytes[TDA9873_SW+1]);
-	v4l2_dbg(1, debug, sd, "tda9873_setmode(): sw_data  = %d\n", sw_data);
+	v4l2_dbg(1, debug, sd,
+		 "tda9873_setaudmode(): chip->shadow.bytes[%d] = %d\n",
+		 TDA9873_SW+1, chip->shadow.bytes[TDA9873_SW+1]);
+	v4l2_dbg(1, debug, sd, "tda9873_setaudmode(): sw_data  = %d\n",
+		 sw_data);
 
 	switch (mode) {
 	case V4L2_TUNER_MODE_MONO:
@@ -707,13 +741,16 @@ static void tda9873_setmode(struct CHIPSTATE *chip, int mode)
 	case V4L2_TUNER_MODE_LANG2:
 		sw_data |= TDA9873_TR_DUALB;
 		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		sw_data |= TDA9873_TR_DUALAB;
+		break;
 	default:
-		chip->mode = 0;
 		return;
 	}
 
 	chip_write(chip, TDA9873_SW, sw_data);
-	v4l2_dbg(1, debug, sd, "tda9873_setmode(): req. mode %d; chip_write: %d\n",
+	v4l2_dbg(1, debug, sd,
+		"tda9873_setaudmode(): req. mode %d; chip_write: %d\n",
 		mode, sw_data);
 }
 
@@ -859,13 +896,13 @@ static int tda9874a_setup(struct CHIPSTATE *chip)
 	return 1;
 }
 
-static int tda9874a_getmode(struct CHIPSTATE *chip)
+static int tda9874a_getrxsubchans(struct CHIPSTATE *chip)
 {
 	struct v4l2_subdev *sd = &chip->sd;
 	int dsr,nsr,mode;
 	int necr; /* just for debugging */
 
-	mode = V4L2_TUNER_MODE_MONO;
+	mode = V4L2_TUNER_SUB_MONO;
 
 	if(-1 == (dsr = chip_read2(chip,TDA9874A_DSR)))
 		return mode;
@@ -888,22 +925,23 @@ static int tda9874a_getmode(struct CHIPSTATE *chip)
 		 * external 4052 multiplexer in audio_hook().
 		 */
 		if(nsr & 0x02) /* NSR.S/MB=1 */
-			mode |= V4L2_TUNER_MODE_STEREO;
+			mode = V4L2_TUNER_SUB_STEREO;
 		if(nsr & 0x01) /* NSR.D/SB=1 */
-			mode |= V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+			mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
 	} else {
 		if(dsr & 0x02) /* DSR.IDSTE=1 */
-			mode |= V4L2_TUNER_MODE_STEREO;
+			mode = V4L2_TUNER_SUB_STEREO;
 		if(dsr & 0x04) /* DSR.IDDUA=1 */
-			mode |= V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+			mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
 	}
 
-	v4l2_dbg(1, debug, sd, "tda9874a_getmode(): DSR=0x%X, NSR=0x%X, NECR=0x%X, return: %d.\n",
+	v4l2_dbg(1, debug, sd,
+		 "tda9874a_getrxsubchans(): DSR=0x%X, NSR=0x%X, NECR=0x%X, return: %d.\n",
 		 dsr, nsr, necr, mode);
 	return mode;
 }
 
-static void tda9874a_setmode(struct CHIPSTATE *chip, int mode)
+static void tda9874a_setaudmode(struct CHIPSTATE *chip, int mode)
 {
 	struct v4l2_subdev *sd = &chip->sd;
 
@@ -939,14 +977,18 @@ static void tda9874a_setmode(struct CHIPSTATE *chip, int mode)
 			aosr = 0xa0; /* auto-select, dual B/B */
 			mdacosr = (tda9874a_mode) ? 0x83:0x81;
 			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			aosr = 0x00; /* always route L to L and R to R */
+			mdacosr = (tda9874a_mode) ? 0x82:0x80;
+			break;
 		default:
-			chip->mode = 0;
 			return;
 		}
 		chip_write(chip, TDA9874A_AOSR, aosr);
 		chip_write(chip, TDA9874A_MDACOSR, mdacosr);
 
-		v4l2_dbg(1, debug, sd, "tda9874a_setmode(): req. mode %d; AOSR=0x%X, MDACOSR=0x%X.\n",
+		v4l2_dbg(1, debug, sd,
+			"tda9874a_setaudmode(): req. mode %d; AOSR=0x%X, MDACOSR=0x%X.\n",
 			mode, aosr, mdacosr);
 
 	} else { /* dic == 0x07 */
@@ -974,14 +1016,18 @@ static void tda9874a_setmode(struct CHIPSTATE *chip, int mode)
 			fmmr = 0x02; /* dual */
 			aosr = 0x20; /* dual B/B */
 			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			fmmr = 0x02; /* dual */
+			aosr = 0x00; /* dual A/B */
+			break;
 		default:
-			chip->mode = 0;
 			return;
 		}
 		chip_write(chip, TDA9874A_FMMR, fmmr);
 		chip_write(chip, TDA9874A_AOSR, aosr);
 
-		v4l2_dbg(1, debug, sd, "tda9874a_setmode(): req. mode %d; FMMR=0x%X, AOSR=0x%X.\n",
+		v4l2_dbg(1, debug, sd,
+			"tda9874a_setaudmode(): req. mode %d; FMMR=0x%X, AOSR=0x%X.\n",
 			mode, fmmr, aosr);
 	}
 }
@@ -1226,25 +1272,33 @@ static int tea6320_initialize(struct CHIPSTATE * chip)
 static int tda8425_shift10(int val) { return (val >> 10) | 0xc0; }
 static int tda8425_shift12(int val) { return (val >> 12) | 0xf0; }
 
-static void tda8425_setmode(struct CHIPSTATE *chip, int mode)
+static void tda8425_setaudmode(struct CHIPSTATE *chip, int mode)
 {
 	int s1 = chip->shadow.bytes[TDA8425_S1+1] & 0xe1;
 
-	if (mode & V4L2_TUNER_MODE_LANG1) {
+	switch (mode) {
+	case V4L2_TUNER_MODE_LANG1:
 		s1 |= TDA8425_S1_ML_SOUND_A;
 		s1 |= TDA8425_S1_STEREO_PSEUDO;
-
-	} else if (mode & V4L2_TUNER_MODE_LANG2) {
+		break;
+	case V4L2_TUNER_MODE_LANG2:
 		s1 |= TDA8425_S1_ML_SOUND_B;
 		s1 |= TDA8425_S1_STEREO_PSEUDO;
-
-	} else {
+		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
 		s1 |= TDA8425_S1_ML_STEREO;
-
-		if (mode & V4L2_TUNER_MODE_MONO)
-			s1 |= TDA8425_S1_STEREO_MONO;
-		if (mode & V4L2_TUNER_MODE_STEREO)
-			s1 |= TDA8425_S1_STEREO_SPATIAL;
+		s1 |= TDA8425_S1_STEREO_LINEAR;
+		break;
+	case V4L2_TUNER_MODE_MONO:
+		s1 |= TDA8425_S1_ML_STEREO;
+		s1 |= TDA8425_S1_STEREO_MONO;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		s1 |= TDA8425_S1_ML_STEREO;
+		s1 |= TDA8425_S1_STEREO_SPATIAL;
+		break;
+	default:
+		return;
 	}
 	chip_write(chip,TDA8425_S1,s1);
 }
@@ -1297,18 +1351,20 @@ static void tda8425_setmode(struct CHIPSTATE *chip, int mode)
  * stereo  L  L
  * BIL     H  L
  */
-static int ta8874z_getmode(struct CHIPSTATE *chip)
+static int ta8874z_getrxsubchans(struct CHIPSTATE *chip)
 {
 	int val, mode;
 
 	val = chip_read(chip);
-	mode = V4L2_TUNER_MODE_MONO;
+	mode = V4L2_TUNER_SUB_MONO;
 	if (val & TA8874Z_B1){
-		mode |= V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+		mode |= V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
 	}else if (!(val & TA8874Z_B0)){
-		mode |= V4L2_TUNER_MODE_STEREO;
+		mode = V4L2_TUNER_SUB_STEREO;
 	}
-	/* v4l_dbg(1, debug, chip->c, "ta8874z_getmode(): raw chip read: 0x%02x, return: 0x%02x\n", val, mode); */
+	/* v4l2_dbg(1, debug, &chip->sd,
+		 "ta8874z_getrxsubchans(): raw chip read: 0x%02x, return: 0x%02x\n",
+		 val, mode); */
 	return mode;
 }
 
@@ -1316,14 +1372,15 @@ static audiocmd ta8874z_stereo = { 2, {0, TA8874Z_SEPARATION_DEFAULT}};
 static audiocmd ta8874z_mono = {2, { TA8874Z_MONO_SET, TA8874Z_SEPARATION_DEFAULT}};
 static audiocmd ta8874z_main = {2, { 0, TA8874Z_SEPARATION_DEFAULT}};
 static audiocmd ta8874z_sub = {2, { TA8874Z_MODE_SUB, TA8874Z_SEPARATION_DEFAULT}};
+static audiocmd ta8874z_both = {2, { TA8874Z_MODE_MAIN | TA8874Z_MODE_SUB, TA8874Z_SEPARATION_DEFAULT}};
 
-static void ta8874z_setmode(struct CHIPSTATE *chip, int mode)
+static void ta8874z_setaudmode(struct CHIPSTATE *chip, int mode)
 {
 	struct v4l2_subdev *sd = &chip->sd;
 	int update = 1;
 	audiocmd *t = NULL;
 
-	v4l2_dbg(1, debug, sd, "ta8874z_setmode(): mode: 0x%02x\n", mode);
+	v4l2_dbg(1, debug, sd, "ta8874z_setaudmode(): mode: 0x%02x\n", mode);
 
 	switch(mode){
 	case V4L2_TUNER_MODE_MONO:
@@ -1338,6 +1395,9 @@ static void ta8874z_setmode(struct CHIPSTATE *chip, int mode)
 	case V4L2_TUNER_MODE_LANG2:
 		t = &ta8874z_sub;
 		break;
+	case V4L2_TUNER_MODE_LANG1_LANG2:
+		t = &ta8874z_both;
+		break;
 	default:
 		update = 0;
 	}
@@ -1394,8 +1454,8 @@ static struct CHIPDESC chiplist[] = {
 
 		/* callbacks */
 		.checkit    = tda9840_checkit,
-		.getmode    = tda9840_getmode,
-		.setmode    = tda9840_setmode,
+		.getrxsubchans = tda9840_getrxsubchans,
+		.setaudmode = tda9840_setaudmode,
 
 		.init       = { 2, { TDA9840_TEST, TDA9840_TEST_INT1SN
 				/* ,TDA9840_SW, TDA9840_MONO */} }
@@ -1410,8 +1470,8 @@ static struct CHIPDESC chiplist[] = {
 
 		/* callbacks */
 		.checkit    = tda9873_checkit,
-		.getmode    = tda9873_getmode,
-		.setmode    = tda9873_setmode,
+		.getrxsubchans = tda9873_getrxsubchans,
+		.setaudmode = tda9873_setaudmode,
 
 		.init       = { 4, { TDA9873_SW, 0xa4, 0x06, 0x03 } },
 		.inputreg   = TDA9873_SW,
@@ -1430,8 +1490,8 @@ static struct CHIPDESC chiplist[] = {
 		/* callbacks */
 		.initialize = tda9874a_initialize,
 		.checkit    = tda9874a_checkit,
-		.getmode    = tda9874a_getmode,
-		.setmode    = tda9874a_setmode,
+		.getrxsubchans = tda9874a_getrxsubchans,
+		.setaudmode = tda9874a_setaudmode,
 	},
 	{
 		.name       = "tda9875",
@@ -1460,8 +1520,8 @@ static struct CHIPDESC chiplist[] = {
 		.addr_hi    = I2C_ADDR_TDA985x_H >> 1,
 		.registers  = 11,
 
-		.getmode    = tda985x_getmode,
-		.setmode    = tda985x_setmode,
+		.getrxsubchans = tda985x_getrxsubchans,
+		.setaudmode = tda985x_setaudmode,
 
 		.init       = { 8, { TDA9850_C4, 0x08, 0x08, TDA985x_STEREO, 0x07, 0x10, 0x10, 0x03 } }
 	},
@@ -1482,8 +1542,8 @@ static struct CHIPDESC chiplist[] = {
 		.volfunc    = tda9855_volume,
 		.bassfunc   = tda9855_bass,
 		.treblefunc = tda9855_treble,
-		.getmode    = tda985x_getmode,
-		.setmode    = tda985x_setmode,
+		.getrxsubchans = tda985x_getrxsubchans,
+		.setaudmode = tda985x_setaudmode,
 
 		.init       = { 12, { 0, 0x6f, 0x6f, 0x0e, 0x07<<1, 0x8<<2,
 				    TDA9855_MUTE | TDA9855_AVL | TDA9855_LOUD | TDA9855_INT,
@@ -1564,7 +1624,7 @@ static struct CHIPDESC chiplist[] = {
 		.volfunc    = tda8425_shift10,
 		.bassfunc   = tda8425_shift12,
 		.treblefunc = tda8425_shift12,
-		.setmode    = tda8425_setmode,
+		.setaudmode = tda8425_setaudmode,
 
 		.inputreg   = TDA8425_S1,
 		.inputmap   = { TDA8425_S1_CH1, TDA8425_S1_CH1, TDA8425_S1_CH1 },
@@ -1593,11 +1653,10 @@ static struct CHIPDESC chiplist[] = {
 		.addr_lo    = I2C_ADDR_TDA9840 >> 1,
 		.addr_hi    = I2C_ADDR_TDA9840 >> 1,
 		.registers  = 2,
-		.flags      = CHIP_NEED_CHECKMODE,
 
 		/* callbacks */
-		.getmode    = ta8874z_getmode,
-		.setmode    = ta8874z_setmode,
+		.getrxsubchans = ta8874z_getrxsubchans,
+		.setaudmode = ta8874z_setaudmode,
 
 		.init       = {2, { TA8874Z_MONO_SET, TA8874Z_SEPARATION_DEFAULT}},
 	},
@@ -1736,7 +1795,6 @@ static int tvaudio_s_radio(struct v4l2_subdev *sd)
 	struct CHIPSTATE *chip = to_state(sd);
 
 	chip->radio = 1;
-	chip->watch_stereo = 0;
 	/* del_timer(&chip->wt); */
 	return 0;
 }
@@ -1793,9 +1851,8 @@ static int tvaudio_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 {
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
-	int mode = 0;
 
-	if (!desc->setmode)
+	if (!desc->setaudmode)
 		return 0;
 	if (chip->radio)
 		return 0;
@@ -1805,22 +1862,18 @@ static int tvaudio_s_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 	case V4L2_TUNER_MODE_STEREO:
 	case V4L2_TUNER_MODE_LANG1:
 	case V4L2_TUNER_MODE_LANG2:
-		mode = vt->audmode;
-		break;
 	case V4L2_TUNER_MODE_LANG1_LANG2:
-		mode = V4L2_TUNER_MODE_STEREO;
 		break;
 	default:
 		return -EINVAL;
 	}
 	chip->audmode = vt->audmode;
 
-	if (mode) {
-		chip->watch_stereo = 0;
-		/* del_timer(&chip->wt); */
-		chip->mode = mode;
-		desc->setmode(chip, mode);
-	}
+	if (chip->thread)
+		wake_up_process(chip->thread);
+	else
+		desc->setaudmode(chip, vt->audmode);
+
 	return 0;
 }
 
@@ -1828,30 +1881,17 @@ static int tvaudio_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *vt)
 {
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
-	int mode = V4L2_TUNER_MODE_MONO;
 
-	if (!desc->getmode)
+	if (!desc->getrxsubchans)
 		return 0;
 	if (chip->radio)
 		return 0;
 
 	vt->audmode = chip->audmode;
-	vt->rxsubchans = 0;
+	vt->rxsubchans = desc->getrxsubchans(chip);
 	vt->capability = V4L2_TUNER_CAP_STEREO |
 		V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
 
-	mode = desc->getmode(chip);
-
-	if (mode & V4L2_TUNER_MODE_MONO)
-		vt->rxsubchans |= V4L2_TUNER_SUB_MONO;
-	if (mode & V4L2_TUNER_MODE_STEREO)
-		vt->rxsubchans |= V4L2_TUNER_SUB_STEREO;
-	/* Note: for SAP it should be mono/lang2 or stereo/lang2.
-	   When this module is converted fully to v4l2, then this
-	   should change for those chips that can detect SAP. */
-	if (mode & V4L2_TUNER_MODE_LANG1)
-		vt->rxsubchans = V4L2_TUNER_SUB_LANG1 |
-			V4L2_TUNER_SUB_LANG2;
 	return 0;
 }
 
@@ -1868,9 +1908,7 @@ static int tvaudio_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *fr
 	struct CHIPSTATE *chip = to_state(sd);
 	struct CHIPDESC *desc = chip->desc;
 
-	chip->mode = 0; /* automatic */
-
-	/* For chips that provide getmode and setmode, and doesn't
+	/* For chips that provide getrxsubchans and setaudmode, and doesn't
 	   automatically follows the stereo carrier, a kthread is
 	   created to set the audio standard. In this case, when then
 	   the video channel is changed, tvaudio starts on MONO mode.
@@ -1879,9 +1917,8 @@ static int tvaudio_s_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *fr
 	   audio carrier.
 	 */
 	if (chip->thread) {
-		desc->setmode(chip, V4L2_TUNER_MODE_MONO);
-		if (chip->prevmode != V4L2_TUNER_MODE_MONO)
-			chip->prevmode = -1; /* reset previous mode */
+		desc->setaudmode(chip, V4L2_TUNER_MODE_MONO);
+		chip->prevmode = -1; /* reset previous mode */
 		mod_timer(&chip->wt, jiffies+msecs_to_jiffies(2000));
 	}
 	return 0;
@@ -2023,7 +2060,7 @@ static int tvaudio_probe(struct i2c_client *client, const struct i2c_device_id *
 	chip->thread = NULL;
 	init_timer(&chip->wt);
 	if (desc->flags & CHIP_NEED_CHECKMODE) {
-		if (!desc->getmode || !desc->setmode) {
+		if (!desc->getrxsubchans || !desc->setaudmode) {
 			/* This shouldn't be happen. Warn user, but keep working
 			   without kthread
 			 */
diff --git a/drivers/media/video/tvp5150.c b/drivers/media/video/tvp5150.c
index b7867427e5c..a751b6c146f 100644
--- a/drivers/media/video/tvp5150.c
+++ b/drivers/media/video/tvp5150.c
@@ -61,13 +61,20 @@ static int tvp5150_read(struct v4l2_subdev *sd, unsigned char addr)
 	int rc;
 
 	buffer[0] = addr;
-	if (1 != (rc = i2c_master_send(c, buffer, 1)))
-		v4l2_dbg(0, debug, sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
+
+	rc = i2c_master_send(c, buffer, 1);
+	if (rc < 0) {
+		v4l2_err(sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
+		return rc;
+	}
 
 	msleep(10);
 
-	if (1 != (rc = i2c_master_recv(c, buffer, 1)))
-		v4l2_dbg(0, debug, sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
+	rc = i2c_master_recv(c, buffer, 1);
+	if (rc < 0) {
+		v4l2_err(sd, "i2c i/o error: rc == %d (should be 1)\n", rc);
+		return rc;
+	}
 
 	v4l2_dbg(2, debug, sd, "tvp5150: read 0x%02x = 0x%02x\n", addr, buffer[0]);
 
@@ -250,7 +257,7 @@ static inline void tvp5150_selmux(struct v4l2_subdev *sd)
 	int opmode = 0;
 	struct tvp5150 *decoder = to_tvp5150(sd);
 	int input = 0;
-	unsigned char val;
+	int val;
 
 	if ((decoder->output & TVP5150_BLACK_SCREEN) || !decoder->enable)
 		input = 8;
@@ -279,6 +286,11 @@ static inline void tvp5150_selmux(struct v4l2_subdev *sd)
 	 * For Composite and TV, it should be the reverse
 	 */
 	val = tvp5150_read(sd, TVP5150_MISC_CTL);
+	if (val < 0) {
+		v4l2_err(sd, "%s: failed with error = %d\n", __func__, val);
+		return;
+	}
+
 	if (decoder->input == TVP5150_SVIDEO)
 		val = (val & ~0x40) | 0x10;
 	else
@@ -676,6 +688,7 @@ static int tvp5150_get_vbi(struct v4l2_subdev *sd,
 	v4l2_std_id std = decoder->norm;
 	u8 reg;
 	int pos, type = 0;
+	int i, ret = 0;
 
 	if (std == V4L2_STD_ALL) {
 		v4l2_err(sd, "VBI can't be configured without knowing number of lines\n");
@@ -690,13 +703,17 @@ static int tvp5150_get_vbi(struct v4l2_subdev *sd,
 
 	reg = ((line - 6) << 1) + TVP5150_LINE_MODE_INI;
 
-	pos = tvp5150_read(sd, reg) & 0x0f;
-	if (pos < 0x0f)
-		type = regs[pos].type.vbi_type;
-
-	pos = tvp5150_read(sd, reg + 1) & 0x0f;
-	if (pos < 0x0f)
-		type |= regs[pos].type.vbi_type;
+	for (i = 0; i <= 1; i++) {
+		ret = tvp5150_read(sd, reg + i);
+		if (ret < 0) {
+			v4l2_err(sd, "%s: failed with error = %d\n",
+				 __func__, ret);
+			return 0;
+		}
+		pos = ret & 0x0f;
+		if (pos < 0x0f)
+			type |= regs[pos].type.vbi_type;
+	}
 
 	return type;
 }
@@ -1031,13 +1048,21 @@ static int tvp5150_g_chip_ident(struct v4l2_subdev *sd,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int tvp5150_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 {
+	int res;
+
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
 	if (!v4l2_chip_match_i2c_client(client, &reg->match))
 		return -EINVAL;
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
-	reg->val = tvp5150_read(sd, reg->reg & 0xff);
+	res = tvp5150_read(sd, reg->reg & 0xff);
+	if (res < 0) {
+		v4l2_err(sd, "%s: failed with error = %d\n", __func__, res);
+		return res;
+	}
+
+	reg->val = res;
 	reg->size = 1;
 	return 0;
 }
@@ -1126,7 +1151,8 @@ static int tvp5150_probe(struct i2c_client *c,
 {
 	struct tvp5150 *core;
 	struct v4l2_subdev *sd;
-	u8 msb_id, lsb_id, msb_rom, lsb_rom;
+	int tvp5150_id[4];
+	int i, res;
 
 	/* Check if the adapter supports the needed features */
 	if (!i2c_check_functionality(c->adapter,
@@ -1139,26 +1165,37 @@ static int tvp5150_probe(struct i2c_client *c,
 	}
 	sd = &core->sd;
 	v4l2_i2c_subdev_init(sd, c, &tvp5150_ops);
+
+	/* 
+	 * Read consequent registers - TVP5150_MSB_DEV_ID, TVP5150_LSB_DEV_ID,
+	 * TVP5150_ROM_MAJOR_VER, TVP5150_ROM_MINOR_VER 
+	 */
+	for (i = 0; i < 4; i++) {
+		res = tvp5150_read(sd, TVP5150_MSB_DEV_ID + i);
+		if (res < 0)
+			goto free_core;
+		tvp5150_id[i] = res;
+	}
+
 	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);
+	if (tvp5150_id[2] == 4 && tvp5150_id[3] == 0) { /* Is TVP5150AM1 */
+		v4l2_info(sd, "tvp%02x%02xam1 detected.\n",
+			  tvp5150_id[0], tvp5150_id[1]);
 
 		/* 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);
+		/* Is TVP5150A */
+		if (tvp5150_id[2] == 3 || tvp5150_id[3] == 0x21) {
+			v4l2_info(sd, "tvp%02x%02xa detected.\n",
+				  tvp5150_id[2], tvp5150_id[3]);
 		} 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);
+				  tvp5150_id[2], tvp5150_id[3]);
+			v4l2_info(sd, "*** Rom ver is %d.%d\n",
+				  tvp5150_id[2], tvp5150_id[3]);
 		}
 	}
 
@@ -1177,11 +1214,9 @@ static int tvp5150_probe(struct i2c_client *c,
 			V4L2_CID_HUE, -128, 127, 1, 0);
 	sd->ctrl_handler = &core->hdl;
 	if (core->hdl.error) {
-		int err = core->hdl.error;
-
+		res = core->hdl.error;
 		v4l2_ctrl_handler_free(&core->hdl);
-		kfree(core);
-		return err;
+		goto free_core;
 	}
 	v4l2_ctrl_handler_setup(&core->hdl);
 
@@ -1197,6 +1232,10 @@ static int tvp5150_probe(struct i2c_client *c,
 	if (debug > 1)
 		tvp5150_log_status(sd);
 	return 0;
+
+free_core:
+	kfree(core);
+	return res;
 }
 
 static int tvp5150_remove(struct i2c_client *c)
diff --git a/drivers/media/video/tw9910.c b/drivers/media/video/tw9910.c
index 8768efb8508..9f53eacb66e 100644
--- a/drivers/media/video/tw9910.c
+++ b/drivers/media/video/tw9910.c
@@ -699,11 +699,9 @@ static int tw9910_g_fmt(struct v4l2_subdev *sd,
 	struct tw9910_priv *priv = to_tw9910(client);
 
 	if (!priv->scale) {
-		int ret;
-		u32 width = 640, height = 480;
-		ret = tw9910_set_frame(sd, &width, &height);
-		if (ret < 0)
-			return ret;
+		priv->scale = tw9910_select_norm(priv->norm, 640, 480);
+		if (!priv->scale)
+			return -EINVAL;
 	}
 
 	mf->width	= priv->scale->width;
diff --git a/drivers/media/video/uvc/Kconfig b/drivers/media/video/uvc/Kconfig
index 6c197da531b..541c9f1e4c6 100644
--- a/drivers/media/video/uvc/Kconfig
+++ b/drivers/media/video/uvc/Kconfig
@@ -10,6 +10,7 @@ config USB_VIDEO_CLASS
 config USB_VIDEO_CLASS_INPUT_EVDEV
 	bool "UVC input events device support"
 	default y
+	depends on USB_VIDEO_CLASS
 	depends on USB_VIDEO_CLASS=INPUT || INPUT=y
 	---help---
 	  This option makes USB Video Class devices register an input device
diff --git a/drivers/media/video/uvc/uvc_ctrl.c b/drivers/media/video/uvc/uvc_ctrl.c
index af26bbe6f76..f7061a5ef1d 100644
--- a/drivers/media/video/uvc/uvc_ctrl.c
+++ b/drivers/media/video/uvc/uvc_ctrl.c
@@ -2083,7 +2083,7 @@ int uvc_ctrl_init_device(struct uvc_device *dev)
 	/* Walk the entities list and instantiate controls */
 	list_for_each_entry(entity, &dev->entities, list) {
 		struct uvc_control *ctrl;
-		unsigned int bControlSize = 0, ncontrols = 0;
+		unsigned int bControlSize = 0, ncontrols;
 		__u8 *bmControls = NULL;
 
 		if (UVC_ENTITY_TYPE(entity) == UVC_VC_EXTENSION_UNIT) {
@@ -2101,8 +2101,7 @@ int uvc_ctrl_init_device(struct uvc_device *dev)
 		uvc_ctrl_prune_entity(dev, entity);
 
 		/* Count supported controls and allocate the controls array */
-		for (i = 0; i < bControlSize; ++i)
-			ncontrols += hweight8(bmControls[i]);
+		ncontrols = memweight(bmControls, bControlSize);
 		if (ncontrols == 0)
 			continue;
 
diff --git a/drivers/media/video/uvc/uvc_v4l2.c b/drivers/media/video/uvc/uvc_v4l2.c
index 759bef8897e..f00db3060e0 100644
--- a/drivers/media/video/uvc/uvc_v4l2.c
+++ b/drivers/media/video/uvc/uvc_v4l2.c
@@ -1051,7 +1051,7 @@ static long uvc_v4l2_ioctl(struct file *file,
 {
 	if (uvc_trace_param & UVC_TRACE_IOCTL) {
 		uvc_printk(KERN_DEBUG, "uvc_v4l2_ioctl(");
-		v4l_printk_ioctl(cmd);
+		v4l_printk_ioctl(NULL, cmd);
 		printk(")\n");
 	}
 
diff --git a/drivers/media/video/uvc/uvc_video.c b/drivers/media/video/uvc/uvc_video.c
index b76b0ac0958..7ac4347ca09 100644
--- a/drivers/media/video/uvc/uvc_video.c
+++ b/drivers/media/video/uvc/uvc_video.c
@@ -1188,7 +1188,11 @@ static void uvc_video_decode_bulk(struct urb *urb, struct uvc_streaming *stream,
 	u8 *mem;
 	int len, ret;
 
-	if (urb->actual_length == 0)
+	/*
+	 * Ignore ZLPs if they're not part of a frame, otherwise process them
+	 * to trigger the end of payload detection.
+	 */
+	if (urb->actual_length == 0 && stream->bulk.header_size == 0)
 		return;
 
 	mem = urb->transfer_buffer;
@@ -1594,7 +1598,7 @@ static int uvc_init_video(struct uvc_streaming *stream, gfp_t gfp_flags)
 			psize = le16_to_cpu(ep->desc.wMaxPacketSize);
 			psize = (psize & 0x07ff) * (1 + ((psize >> 11) & 3));
 			if (psize >= bandwidth && psize <= best_psize) {
-				altsetting = i;
+				altsetting = alts->desc.bAlternateSetting;
 				best_psize = psize;
 				best_ep = ep;
 			}
diff --git a/drivers/media/video/v4l2-compat-ioctl32.c b/drivers/media/video/v4l2-compat-ioctl32.c
index 5327ad3a639..9ebd5c540d1 100644
--- a/drivers/media/video/v4l2-compat-ioctl32.c
+++ b/drivers/media/video/v4l2-compat-ioctl32.c
@@ -327,7 +327,7 @@ struct v4l2_buffer32 {
 		compat_caddr_t  planes;
 	} m;
 	__u32			length;
-	__u32			input;
+	__u32			reserved2;
 	__u32			reserved;
 };
 
@@ -387,8 +387,7 @@ static int get_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		get_user(kp->index, &up->index) ||
 		get_user(kp->type, &up->type) ||
 		get_user(kp->flags, &up->flags) ||
-		get_user(kp->memory, &up->memory) ||
-		get_user(kp->input, &up->input))
+		get_user(kp->memory, &up->memory))
 			return -EFAULT;
 
 	if (V4L2_TYPE_IS_OUTPUT(kp->type))
@@ -472,8 +471,7 @@ static int put_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		put_user(kp->index, &up->index) ||
 		put_user(kp->type, &up->type) ||
 		put_user(kp->flags, &up->flags) ||
-		put_user(kp->memory, &up->memory) ||
-		put_user(kp->input, &up->input))
+		put_user(kp->memory, &up->memory))
 			return -EFAULT;
 
 	if (put_user(kp->bytesused, &up->bytesused) ||
@@ -482,6 +480,7 @@ static int put_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		put_user(kp->timestamp.tv_usec, &up->timestamp.tv_usec) ||
 		copy_to_user(&up->timecode, &kp->timecode, sizeof(struct v4l2_timecode)) ||
 		put_user(kp->sequence, &up->sequence) ||
+		put_user(kp->reserved2, &up->reserved2) ||
 		put_user(kp->reserved, &up->reserved))
 			return -EFAULT;
 
@@ -1026,6 +1025,7 @@ long v4l2_compat_ioctl32(struct file *file, unsigned int cmd, unsigned long arg)
 	case VIDIOC_ENUM_DV_TIMINGS:
 	case VIDIOC_QUERY_DV_TIMINGS:
 	case VIDIOC_DV_TIMINGS_CAP:
+	case VIDIOC_ENUM_FREQ_BANDS:
 		ret = do_video_ioctl(file, cmd, arg);
 		break;
 
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index 9abd9abd450..b6a2ee71e5c 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -755,6 +755,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_HUE_AUTO:
 	case V4L2_CID_CHROMA_AGC:
 	case V4L2_CID_COLOR_KILLER:
+	case V4L2_CID_AUTOBRIGHTNESS:
 	case V4L2_CID_MPEG_AUDIO_MUTE:
 	case V4L2_CID_MPEG_VIDEO_MUTE:
 	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
@@ -2120,7 +2121,7 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 
 	/* First zero the helper field in the master control references */
 	for (i = 0; i < cs->count; i++)
-		helpers[i].mref->helper = 0;
+		helpers[i].mref->helper = NULL;
 	for (i = 0, h = helpers; i < cs->count; i++, h++) {
 		struct v4l2_ctrl_ref *mref = h->mref;
 
diff --git a/drivers/media/video/v4l2-dev.c b/drivers/media/video/v4l2-dev.c
index 83dbb2ddff1..07aeafca9ea 100644
--- a/drivers/media/video/v4l2-dev.c
+++ b/drivers/media/video/v4l2-dev.c
@@ -46,6 +46,29 @@ static ssize_t show_index(struct device *cd,
 	return sprintf(buf, "%i\n", vdev->index);
 }
 
+static ssize_t show_debug(struct device *cd,
+			 struct device_attribute *attr, char *buf)
+{
+	struct video_device *vdev = to_video_device(cd);
+
+	return sprintf(buf, "%i\n", vdev->debug);
+}
+
+static ssize_t set_debug(struct device *cd, struct device_attribute *attr,
+		   const char *buf, size_t len)
+{
+	struct video_device *vdev = to_video_device(cd);
+	int res = 0;
+	u16 value;
+
+	res = kstrtou16(buf, 0, &value);
+	if (res)
+		return res;
+
+	vdev->debug = value;
+	return len;
+}
+
 static ssize_t show_name(struct device *cd,
 			 struct device_attribute *attr, char *buf)
 {
@@ -56,6 +79,7 @@ static ssize_t show_name(struct device *cd,
 
 static struct device_attribute video_device_attrs[] = {
 	__ATTR(name, S_IRUGO, show_name, NULL),
+	__ATTR(debug, 0644, show_debug, set_debug),
 	__ATTR(index, S_IRUGO, show_index, NULL),
 	__ATTR_NULL
 };
@@ -281,6 +305,9 @@ static ssize_t v4l2_read(struct file *filp, char __user *buf,
 		ret = vdev->fops->read(filp, buf, sz, off);
 	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: read: %zd (%d)\n",
+			video_device_node_name(vdev), sz, ret);
 	return ret;
 }
 
@@ -299,6 +326,9 @@ static ssize_t v4l2_write(struct file *filp, const char __user *buf,
 		ret = vdev->fops->write(filp, buf, sz, off);
 	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: write: %zd (%d)\n",
+			video_device_node_name(vdev), sz, ret);
 	return ret;
 }
 
@@ -315,6 +345,9 @@ static unsigned int v4l2_poll(struct file *filp, struct poll_table_struct *poll)
 		ret = vdev->fops->poll(filp, poll);
 	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: poll: %08x\n",
+			video_device_node_name(vdev), ret);
 	return ret;
 }
 
@@ -324,20 +357,14 @@ static long v4l2_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	int ret = -ENODEV;
 
 	if (vdev->fops->unlocked_ioctl) {
-		bool locked = false;
+		struct mutex *lock = v4l2_ioctl_get_lock(vdev, cmd);
 
-		if (vdev->lock) {
-			/* always lock unless the cmd is marked as "don't use lock" */
-			locked = !v4l2_is_known_ioctl(cmd) ||
-				 !test_bit(_IOC_NR(cmd), vdev->disable_locking);
-
-			if (locked && mutex_lock_interruptible(vdev->lock))
-				return -ERESTARTSYS;
-		}
+		if (lock && mutex_lock_interruptible(lock))
+			return -ERESTARTSYS;
 		if (video_is_registered(vdev))
 			ret = vdev->fops->unlocked_ioctl(filp, cmd, arg);
-		if (locked)
-			mutex_unlock(vdev->lock);
+		if (lock)
+			mutex_unlock(lock);
 	} else if (vdev->fops->ioctl) {
 		/* This code path is a replacement for the BKL. It is a major
 		 * hack but it will have to do for those drivers that are not
@@ -385,12 +412,17 @@ static unsigned long v4l2_get_unmapped_area(struct file *filp,
 		unsigned long flags)
 {
 	struct video_device *vdev = video_devdata(filp);
+	int ret;
 
 	if (!vdev->fops->get_unmapped_area)
 		return -ENOSYS;
 	if (!video_is_registered(vdev))
 		return -ENODEV;
-	return vdev->fops->get_unmapped_area(filp, addr, len, pgoff, flags);
+	ret = vdev->fops->get_unmapped_area(filp, addr, len, pgoff, flags);
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: get_unmapped_area (%d)\n",
+			video_device_node_name(vdev), ret);
+	return ret;
 }
 #endif
 
@@ -408,6 +440,9 @@ static int v4l2_mmap(struct file *filp, struct vm_area_struct *vm)
 		ret = vdev->fops->mmap(filp, vm);
 	if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 		mutex_unlock(vdev->lock);
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: mmap (%d)\n",
+			video_device_node_name(vdev), ret);
 	return ret;
 }
 
@@ -443,6 +478,9 @@ static int v4l2_open(struct inode *inode, struct file *filp)
 	}
 
 err:
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: open (%d)\n",
+			video_device_node_name(vdev), ret);
 	/* decrease the refcount in case of an error */
 	if (ret)
 		video_put(vdev);
@@ -462,6 +500,9 @@ static int v4l2_release(struct inode *inode, struct file *filp)
 		if (test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags))
 			mutex_unlock(vdev->lock);
 	}
+	if (vdev->debug)
+		printk(KERN_DEBUG "%s: release\n",
+			video_device_node_name(vdev));
 	/* decrease the refcount unconditionally since the release()
 	   return value is ignored. */
 	video_put(vdev);
@@ -656,7 +697,8 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	SET_VALID_IOCTL(ops, VIDIOC_TRY_ENCODER_CMD, vidioc_try_encoder_cmd);
 	SET_VALID_IOCTL(ops, VIDIOC_DECODER_CMD, vidioc_decoder_cmd);
 	SET_VALID_IOCTL(ops, VIDIOC_TRY_DECODER_CMD, vidioc_try_decoder_cmd);
-	if (ops->vidioc_g_parm || vdev->vfl_type == VFL_TYPE_GRABBER)
+	if (ops->vidioc_g_parm || (vdev->vfl_type == VFL_TYPE_GRABBER &&
+					(ops->vidioc_g_std || vdev->tvnorms)))
 		set_bit(_IOC_NR(VIDIOC_G_PARM), valid_ioctls);
 	SET_VALID_IOCTL(ops, VIDIOC_S_PARM, vidioc_s_parm);
 	SET_VALID_IOCTL(ops, VIDIOC_G_TUNER, vidioc_g_tuner);
@@ -681,12 +723,15 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	SET_VALID_IOCTL(ops, VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings);
 	SET_VALID_IOCTL(ops, VIDIOC_ENUM_DV_TIMINGS, vidioc_enum_dv_timings);
 	SET_VALID_IOCTL(ops, VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings);
+	SET_VALID_IOCTL(ops, VIDIOC_DV_TIMINGS_CAP, vidioc_dv_timings_cap);
 	/* yes, really vidioc_subscribe_event */
 	SET_VALID_IOCTL(ops, VIDIOC_DQEVENT, vidioc_subscribe_event);
 	SET_VALID_IOCTL(ops, VIDIOC_SUBSCRIBE_EVENT, vidioc_subscribe_event);
 	SET_VALID_IOCTL(ops, VIDIOC_UNSUBSCRIBE_EVENT, vidioc_unsubscribe_event);
 	SET_VALID_IOCTL(ops, VIDIOC_CREATE_BUFS, vidioc_create_bufs);
 	SET_VALID_IOCTL(ops, VIDIOC_PREPARE_BUF, vidioc_prepare_buf);
+	if (ops->vidioc_enum_freq_bands || ops->vidioc_g_tuner || ops->vidioc_g_modulator)
+		set_bit(_IOC_NR(VIDIOC_ENUM_FREQ_BANDS), valid_ioctls);
 	bitmap_andnot(vdev->valid_ioctls, valid_ioctls, vdev->valid_ioctls,
 			BASE_VIDIOC_PRIVATE);
 }
diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index d7fa8962d8b..c3b7b5f59b3 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -27,27 +27,7 @@
 #include <media/v4l2-event.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
-
-#define dbgarg(cmd, fmt, arg...) \
-		do {							\
-		    if (vfd->debug & V4L2_DEBUG_IOCTL_ARG) {		\
-			printk(KERN_DEBUG "%s: ",  vfd->name);		\
-			v4l_printk_ioctl(cmd);				\
-			printk(" " fmt,  ## arg);			\
-		    }							\
-		} while (0)
-
-#define dbgarg2(fmt, arg...) \
-		do {							\
-		    if (vfd->debug & V4L2_DEBUG_IOCTL_ARG)		\
-			printk(KERN_DEBUG "%s: " fmt, vfd->name, ## arg);\
-		} while (0)
-
-#define dbgarg3(fmt, arg...) \
-		do {							\
-		    if (vfd->debug & V4L2_DEBUG_IOCTL_ARG)		\
-			printk(KERN_CONT "%s: " fmt, vfd->name, ## arg);\
-		} while (0)
+#include <media/videobuf2-core.h>
 
 /* Zero out the end of the struct pointed to by p.  Everything after, but
  * not including, the specified field is cleared. */
@@ -183,207 +163,507 @@ static const char *v4l2_memory_names[] = {
 /* ------------------------------------------------------------------ */
 /* debug help functions                                               */
 
-struct v4l2_ioctl_info {
-	unsigned int ioctl;
-	u16 flags;
-	const char * const name;
-};
+static void v4l_print_querycap(const void *arg, bool write_only)
+{
+	const struct v4l2_capability *p = arg;
 
-/* This control needs a priority check */
-#define INFO_FL_PRIO	(1 << 0)
-/* This control can be valid if the filehandle passes a control handler. */
-#define INFO_FL_CTRL	(1 << 1)
+	pr_cont("driver=%s, card=%s, bus=%s, version=0x%08x, "
+		"capabilities=0x%08x, device_caps=0x%08x\n",
+		p->driver, p->card, p->bus_info,
+		p->version, p->capabilities, p->device_caps);
+}
 
-#define IOCTL_INFO(_ioctl, _flags) [_IOC_NR(_ioctl)] = {	\
-	.ioctl = _ioctl,					\
-	.flags = _flags,					\
-	.name = #_ioctl,					\
+static void v4l_print_enuminput(const void *arg, bool write_only)
+{
+	const struct v4l2_input *p = arg;
+
+	pr_cont("index=%u, name=%s, type=%u, audioset=0x%x, tuner=%u, "
+		"std=0x%08Lx, status=0x%x, capabilities=0x%x\n",
+		p->index, p->name, p->type, p->audioset, p->tuner,
+		(unsigned long long)p->std, p->status, p->capabilities);
 }
 
-static struct v4l2_ioctl_info v4l2_ioctls[] = {
-	IOCTL_INFO(VIDIOC_QUERYCAP, 0),
-	IOCTL_INFO(VIDIOC_ENUM_FMT, 0),
-	IOCTL_INFO(VIDIOC_G_FMT, 0),
-	IOCTL_INFO(VIDIOC_S_FMT, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_REQBUFS, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_QUERYBUF, 0),
-	IOCTL_INFO(VIDIOC_G_FBUF, 0),
-	IOCTL_INFO(VIDIOC_S_FBUF, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_OVERLAY, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_QBUF, 0),
-	IOCTL_INFO(VIDIOC_DQBUF, 0),
-	IOCTL_INFO(VIDIOC_STREAMON, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_STREAMOFF, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_PARM, 0),
-	IOCTL_INFO(VIDIOC_S_PARM, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_STD, 0),
-	IOCTL_INFO(VIDIOC_S_STD, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_ENUMSTD, 0),
-	IOCTL_INFO(VIDIOC_ENUMINPUT, 0),
-	IOCTL_INFO(VIDIOC_G_CTRL, INFO_FL_CTRL),
-	IOCTL_INFO(VIDIOC_S_CTRL, INFO_FL_PRIO | INFO_FL_CTRL),
-	IOCTL_INFO(VIDIOC_G_TUNER, 0),
-	IOCTL_INFO(VIDIOC_S_TUNER, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_AUDIO, 0),
-	IOCTL_INFO(VIDIOC_S_AUDIO, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_QUERYCTRL, INFO_FL_CTRL),
-	IOCTL_INFO(VIDIOC_QUERYMENU, INFO_FL_CTRL),
-	IOCTL_INFO(VIDIOC_G_INPUT, 0),
-	IOCTL_INFO(VIDIOC_S_INPUT, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_OUTPUT, 0),
-	IOCTL_INFO(VIDIOC_S_OUTPUT, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_ENUMOUTPUT, 0),
-	IOCTL_INFO(VIDIOC_G_AUDOUT, 0),
-	IOCTL_INFO(VIDIOC_S_AUDOUT, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_MODULATOR, 0),
-	IOCTL_INFO(VIDIOC_S_MODULATOR, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_FREQUENCY, 0),
-	IOCTL_INFO(VIDIOC_S_FREQUENCY, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_CROPCAP, 0),
-	IOCTL_INFO(VIDIOC_G_CROP, 0),
-	IOCTL_INFO(VIDIOC_S_CROP, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_SELECTION, 0),
-	IOCTL_INFO(VIDIOC_S_SELECTION, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_JPEGCOMP, 0),
-	IOCTL_INFO(VIDIOC_S_JPEGCOMP, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_QUERYSTD, 0),
-	IOCTL_INFO(VIDIOC_TRY_FMT, 0),
-	IOCTL_INFO(VIDIOC_ENUMAUDIO, 0),
-	IOCTL_INFO(VIDIOC_ENUMAUDOUT, 0),
-	IOCTL_INFO(VIDIOC_G_PRIORITY, 0),
-	IOCTL_INFO(VIDIOC_S_PRIORITY, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_SLICED_VBI_CAP, 0),
-	IOCTL_INFO(VIDIOC_LOG_STATUS, 0),
-	IOCTL_INFO(VIDIOC_G_EXT_CTRLS, INFO_FL_CTRL),
-	IOCTL_INFO(VIDIOC_S_EXT_CTRLS, INFO_FL_PRIO | INFO_FL_CTRL),
-	IOCTL_INFO(VIDIOC_TRY_EXT_CTRLS, 0),
-	IOCTL_INFO(VIDIOC_ENUM_FRAMESIZES, 0),
-	IOCTL_INFO(VIDIOC_ENUM_FRAMEINTERVALS, 0),
-	IOCTL_INFO(VIDIOC_G_ENC_INDEX, 0),
-	IOCTL_INFO(VIDIOC_ENCODER_CMD, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_TRY_ENCODER_CMD, 0),
-	IOCTL_INFO(VIDIOC_DECODER_CMD, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_TRY_DECODER_CMD, 0),
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-	IOCTL_INFO(VIDIOC_DBG_S_REGISTER, 0),
-	IOCTL_INFO(VIDIOC_DBG_G_REGISTER, 0),
-#endif
-	IOCTL_INFO(VIDIOC_DBG_G_CHIP_IDENT, 0),
-	IOCTL_INFO(VIDIOC_S_HW_FREQ_SEEK, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_ENUM_DV_PRESETS, 0),
-	IOCTL_INFO(VIDIOC_S_DV_PRESET, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_DV_PRESET, 0),
-	IOCTL_INFO(VIDIOC_QUERY_DV_PRESET, 0),
-	IOCTL_INFO(VIDIOC_S_DV_TIMINGS, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_G_DV_TIMINGS, 0),
-	IOCTL_INFO(VIDIOC_DQEVENT, 0),
-	IOCTL_INFO(VIDIOC_SUBSCRIBE_EVENT, 0),
-	IOCTL_INFO(VIDIOC_UNSUBSCRIBE_EVENT, 0),
-	IOCTL_INFO(VIDIOC_CREATE_BUFS, INFO_FL_PRIO),
-	IOCTL_INFO(VIDIOC_PREPARE_BUF, 0),
-	IOCTL_INFO(VIDIOC_ENUM_DV_TIMINGS, 0),
-	IOCTL_INFO(VIDIOC_QUERY_DV_TIMINGS, 0),
-	IOCTL_INFO(VIDIOC_DV_TIMINGS_CAP, 0),
-};
-#define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
+static void v4l_print_enumoutput(const void *arg, bool write_only)
+{
+	const struct v4l2_output *p = arg;
 
-bool v4l2_is_known_ioctl(unsigned int cmd)
+	pr_cont("index=%u, name=%s, type=%u, audioset=0x%x, "
+		"modulator=%u, std=0x%08Lx, capabilities=0x%x\n",
+		p->index, p->name, p->type, p->audioset, p->modulator,
+		(unsigned long long)p->std, p->capabilities);
+}
+
+static void v4l_print_audio(const void *arg, bool write_only)
 {
-	if (_IOC_NR(cmd) >= V4L2_IOCTLS)
-		return false;
-	return v4l2_ioctls[_IOC_NR(cmd)].ioctl == cmd;
+	const struct v4l2_audio *p = arg;
+
+	if (write_only)
+		pr_cont("index=%u, mode=0x%x\n", p->index, p->mode);
+	else
+		pr_cont("index=%u, name=%s, capability=0x%x, mode=0x%x\n",
+			p->index, p->name, p->capability, p->mode);
 }
 
-/* Common ioctl debug function. This function can be used by
-   external ioctl messages as well as internal V4L ioctl */
-void v4l_printk_ioctl(unsigned int cmd)
+static void v4l_print_audioout(const void *arg, bool write_only)
 {
-	char *dir, *type;
+	const struct v4l2_audioout *p = arg;
 
-	switch (_IOC_TYPE(cmd)) {
-	case 'd':
-		type = "v4l2_int";
+	if (write_only)
+		pr_cont("index=%u\n", p->index);
+	else
+		pr_cont("index=%u, name=%s, capability=0x%x, mode=0x%x\n",
+			p->index, p->name, p->capability, p->mode);
+}
+
+static void v4l_print_fmtdesc(const void *arg, bool write_only)
+{
+	const struct v4l2_fmtdesc *p = arg;
+
+	pr_cont("index=%u, type=%s, flags=0x%x, pixelformat=%c%c%c%c, description='%s'\n",
+		p->index, prt_names(p->type, v4l2_type_names),
+		p->flags, (p->pixelformat & 0xff),
+		(p->pixelformat >>  8) & 0xff,
+		(p->pixelformat >> 16) & 0xff,
+		(p->pixelformat >> 24) & 0xff,
+		p->description);
+}
+
+static void v4l_print_format(const void *arg, bool write_only)
+{
+	const struct v4l2_format *p = arg;
+	const struct v4l2_pix_format *pix;
+	const struct v4l2_pix_format_mplane *mp;
+	const struct v4l2_vbi_format *vbi;
+	const struct v4l2_sliced_vbi_format *sliced;
+	const struct v4l2_window *win;
+	const struct v4l2_clip *clip;
+	unsigned i;
+
+	pr_cont("type=%s", prt_names(p->type, v4l2_type_names));
+	switch (p->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		pix = &p->fmt.pix;
+		pr_cont(", width=%u, height=%u, "
+			"pixelformat=%c%c%c%c, field=%s, "
+			"bytesperline=%u sizeimage=%u, colorspace=%d\n",
+			pix->width, pix->height,
+			(pix->pixelformat & 0xff),
+			(pix->pixelformat >>  8) & 0xff,
+			(pix->pixelformat >> 16) & 0xff,
+			(pix->pixelformat >> 24) & 0xff,
+			prt_names(pix->field, v4l2_field_names),
+			pix->bytesperline, pix->sizeimage,
+			pix->colorspace);
 		break;
-	case 'V':
-		if (_IOC_NR(cmd) >= V4L2_IOCTLS) {
-			type = "v4l2";
-			break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		mp = &p->fmt.pix_mp;
+		pr_cont(", width=%u, height=%u, "
+			"format=%c%c%c%c, field=%s, "
+			"colorspace=%d, num_planes=%u\n",
+			mp->width, mp->height,
+			(mp->pixelformat & 0xff),
+			(mp->pixelformat >>  8) & 0xff,
+			(mp->pixelformat >> 16) & 0xff,
+			(mp->pixelformat >> 24) & 0xff,
+			prt_names(mp->field, v4l2_field_names),
+			mp->colorspace, mp->num_planes);
+		for (i = 0; i < mp->num_planes; i++)
+			printk(KERN_DEBUG "plane %u: bytesperline=%u sizeimage=%u\n", i,
+					mp->plane_fmt[i].bytesperline,
+					mp->plane_fmt[i].sizeimage);
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
+		win = &p->fmt.win;
+		pr_cont(", wxh=%dx%d, x,y=%d,%d, field=%s, "
+			"chromakey=0x%08x, bitmap=%p, "
+			"global_alpha=0x%02x\n",
+			win->w.width, win->w.height,
+			win->w.left, win->w.top,
+			prt_names(win->field, v4l2_field_names),
+			win->chromakey, win->bitmap, win->global_alpha);
+		clip = win->clips;
+		for (i = 0; i < win->clipcount; i++) {
+			printk(KERN_DEBUG "clip %u: wxh=%dx%d, x,y=%d,%d\n",
+					i, clip->c.width, clip->c.height,
+					clip->c.left, clip->c.top);
+			clip = clip->next;
 		}
-		printk("%s", v4l2_ioctls[_IOC_NR(cmd)].name);
-		return;
-	default:
-		type = "unknown";
+		break;
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+		vbi = &p->fmt.vbi;
+		pr_cont(", sampling_rate=%u, offset=%u, samples_per_line=%u, "
+			"sample_format=%c%c%c%c, start=%u,%u, count=%u,%u\n",
+			vbi->sampling_rate, vbi->offset,
+			vbi->samples_per_line,
+			(vbi->sample_format & 0xff),
+			(vbi->sample_format >>  8) & 0xff,
+			(vbi->sample_format >> 16) & 0xff,
+			(vbi->sample_format >> 24) & 0xff,
+			vbi->start[0], vbi->start[1],
+			vbi->count[0], vbi->count[1]);
+		break;
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		sliced = &p->fmt.sliced;
+		pr_cont(", service_set=0x%08x, io_size=%d\n",
+				sliced->service_set, sliced->io_size);
+		for (i = 0; i < 24; i++)
+			printk(KERN_DEBUG "line[%02u]=0x%04x, 0x%04x\n", i,
+				sliced->service_lines[0][i],
+				sliced->service_lines[1][i]);
+		break;
+	case V4L2_BUF_TYPE_PRIVATE:
+		pr_cont("\n");
+		break;
 	}
+}
 
-	switch (_IOC_DIR(cmd)) {
-	case _IOC_NONE:              dir = "--"; break;
-	case _IOC_READ:              dir = "r-"; break;
-	case _IOC_WRITE:             dir = "-w"; break;
-	case _IOC_READ | _IOC_WRITE: dir = "rw"; break;
-	default:                     dir = "*ERR*"; break;
-	}
-	printk("%s ioctl '%c', dir=%s, #%d (0x%08x)",
-		type, _IOC_TYPE(cmd), dir, _IOC_NR(cmd), cmd);
+static void v4l_print_framebuffer(const void *arg, bool write_only)
+{
+	const struct v4l2_framebuffer *p = arg;
+
+	pr_cont("capability=0x%x, flags=0x%x, base=0x%p, width=%u, "
+		"height=%u, pixelformat=%c%c%c%c, "
+		"bytesperline=%u sizeimage=%u, colorspace=%d\n",
+			p->capability, p->flags, p->base,
+			p->fmt.width, p->fmt.height,
+			(p->fmt.pixelformat & 0xff),
+			(p->fmt.pixelformat >>  8) & 0xff,
+			(p->fmt.pixelformat >> 16) & 0xff,
+			(p->fmt.pixelformat >> 24) & 0xff,
+			p->fmt.bytesperline, p->fmt.sizeimage,
+			p->fmt.colorspace);
+}
+
+static void v4l_print_buftype(const void *arg, bool write_only)
+{
+	pr_cont("type=%s\n", prt_names(*(u32 *)arg, v4l2_type_names));
+}
+
+static void v4l_print_modulator(const void *arg, bool write_only)
+{
+	const struct v4l2_modulator *p = arg;
+
+	if (write_only)
+		pr_cont("index=%u, txsubchans=0x%x", p->index, p->txsubchans);
+	else
+		pr_cont("index=%u, name=%s, capability=0x%x, "
+			"rangelow=%u, rangehigh=%u, txsubchans=0x%x\n",
+			p->index, p->name, p->capability,
+			p->rangelow, p->rangehigh, p->txsubchans);
+}
+
+static void v4l_print_tuner(const void *arg, bool write_only)
+{
+	const struct v4l2_tuner *p = arg;
+
+	if (write_only)
+		pr_cont("index=%u, audmode=%u\n", p->index, p->audmode);
+	else
+		pr_cont("index=%u, name=%s, type=%u, capability=0x%x, "
+			"rangelow=%u, rangehigh=%u, signal=%u, afc=%d, "
+			"rxsubchans=0x%x, audmode=%u\n",
+			p->index, p->name, p->type,
+			p->capability, p->rangelow,
+			p->rangehigh, p->signal, p->afc,
+			p->rxsubchans, p->audmode);
 }
-EXPORT_SYMBOL(v4l_printk_ioctl);
 
-static void dbgbuf(unsigned int cmd, struct video_device *vfd,
-					struct v4l2_buffer *p)
+static void v4l_print_frequency(const void *arg, bool write_only)
 {
-	struct v4l2_timecode *tc = &p->timecode;
-	struct v4l2_plane *plane;
+	const struct v4l2_frequency *p = arg;
+
+	pr_cont("tuner=%u, type=%u, frequency=%u\n",
+				p->tuner, p->type, p->frequency);
+}
+
+static void v4l_print_standard(const void *arg, bool write_only)
+{
+	const struct v4l2_standard *p = arg;
+
+	pr_cont("index=%u, id=0x%Lx, name=%s, fps=%u/%u, "
+		"framelines=%u\n", p->index,
+		(unsigned long long)p->id, p->name,
+		p->frameperiod.numerator,
+		p->frameperiod.denominator,
+		p->framelines);
+}
+
+static void v4l_print_std(const void *arg, bool write_only)
+{
+	pr_cont("std=0x%08Lx\n", *(const long long unsigned *)arg);
+}
+
+static void v4l_print_hw_freq_seek(const void *arg, bool write_only)
+{
+	const struct v4l2_hw_freq_seek *p = arg;
+
+	pr_cont("tuner=%u, type=%u, seek_upward=%u, wrap_around=%u, spacing=%u\n",
+		p->tuner, p->type, p->seek_upward, p->wrap_around, p->spacing);
+}
+
+static void v4l_print_requestbuffers(const void *arg, bool write_only)
+{
+	const struct v4l2_requestbuffers *p = arg;
+
+	pr_cont("count=%d, type=%s, memory=%s\n",
+		p->count,
+		prt_names(p->type, v4l2_type_names),
+		prt_names(p->memory, v4l2_memory_names));
+}
+
+static void v4l_print_buffer(const void *arg, bool write_only)
+{
+	const struct v4l2_buffer *p = arg;
+	const struct v4l2_timecode *tc = &p->timecode;
+	const struct v4l2_plane *plane;
 	int i;
 
-	dbgarg(cmd, "%02ld:%02d:%02d.%08ld index=%d, type=%s, "
-		"flags=0x%08d, field=%0d, sequence=%d, memory=%s\n",
+	pr_cont("%02ld:%02d:%02d.%08ld index=%d, type=%s, "
+		"flags=0x%08x, field=%s, sequence=%d, memory=%s",
 			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->flags, p->field, p->sequence,
-			prt_names(p->memory, v4l2_memory_names));
+			p->flags, prt_names(p->field, v4l2_field_names),
+			p->sequence, prt_names(p->memory, v4l2_memory_names));
 
 	if (V4L2_TYPE_IS_MULTIPLANAR(p->type) && p->m.planes) {
+		pr_cont("\n");
 		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",
+			printk(KERN_DEBUG
+				"plane %d: bytesused=%d, data_offset=0x%08x "
+				"offset/userptr=0x%lx, 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",
+		pr_cont("bytesused=%d, offset/userptr=0x%lx, 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",
+	printk(KERN_DEBUG "timecode=%02d:%02d:%02d type=%d, "
+		"flags=0x%08x, frames=%d, userbits=0x%08x\n",
 			tc->hours, tc->minutes, tc->seconds,
 			tc->type, tc->flags, tc->frames, *(__u32 *)tc->userbits);
 }
 
-static inline void dbgrect(struct video_device *vfd, char *s,
-							struct v4l2_rect *r)
+static void v4l_print_create_buffers(const void *arg, bool write_only)
 {
-	dbgarg2("%sRect start at %dx%d, size=%dx%d\n", s, r->left, r->top,
-						r->width, r->height);
-};
+	const struct v4l2_create_buffers *p = arg;
+
+	pr_cont("index=%d, count=%d, memory=%s, ",
+			p->index, p->count,
+			prt_names(p->memory, v4l2_memory_names));
+	v4l_print_format(&p->format, write_only);
+}
 
-static void dbgtimings(struct video_device *vfd,
-			const struct v4l2_dv_timings *p)
+static void v4l_print_streamparm(const void *arg, bool write_only)
 {
+	const struct v4l2_streamparm *p = arg;
+
+	pr_cont("type=%s", prt_names(p->type, v4l2_type_names));
+
+	if (p->type == V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    p->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		const struct v4l2_captureparm *c = &p->parm.capture;
+
+		pr_cont(", capability=0x%x, capturemode=0x%x, timeperframe=%d/%d, "
+			"extendedmode=%d, readbuffers=%d\n",
+			c->capability, c->capturemode,
+			c->timeperframe.numerator, c->timeperframe.denominator,
+			c->extendedmode, c->readbuffers);
+	} else if (p->type == V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+		   p->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		const struct v4l2_outputparm *c = &p->parm.output;
+
+		pr_cont(", capability=0x%x, outputmode=0x%x, timeperframe=%d/%d, "
+			"extendedmode=%d, writebuffers=%d\n",
+			c->capability, c->outputmode,
+			c->timeperframe.numerator, c->timeperframe.denominator,
+			c->extendedmode, c->writebuffers);
+	}
+}
+
+static void v4l_print_queryctrl(const void *arg, bool write_only)
+{
+	const struct v4l2_queryctrl *p = arg;
+
+	pr_cont("id=0x%x, type=%d, name=%s, min/max=%d/%d, "
+		"step=%d, default=%d, flags=0x%08x\n",
+			p->id, p->type, p->name,
+			p->minimum, p->maximum,
+			p->step, p->default_value, p->flags);
+}
+
+static void v4l_print_querymenu(const void *arg, bool write_only)
+{
+	const struct v4l2_querymenu *p = arg;
+
+	pr_cont("id=0x%x, index=%d\n", p->id, p->index);
+}
+
+static void v4l_print_control(const void *arg, bool write_only)
+{
+	const struct v4l2_control *p = arg;
+
+	pr_cont("id=0x%x, value=%d\n", p->id, p->value);
+}
+
+static void v4l_print_ext_controls(const void *arg, bool write_only)
+{
+	const struct v4l2_ext_controls *p = arg;
+	int i;
+
+	pr_cont("class=0x%x, count=%d, error_idx=%d",
+			p->ctrl_class, p->count, p->error_idx);
+	for (i = 0; i < p->count; i++) {
+		if (p->controls[i].size)
+			pr_cont(", id/val=0x%x/0x%x",
+				p->controls[i].id, p->controls[i].value);
+		else
+			pr_cont(", id/size=0x%x/%u",
+				p->controls[i].id, p->controls[i].size);
+	}
+	pr_cont("\n");
+}
+
+static void v4l_print_cropcap(const void *arg, bool write_only)
+{
+	const struct v4l2_cropcap *p = arg;
+
+	pr_cont("type=%s, bounds wxh=%dx%d, x,y=%d,%d, "
+		"defrect wxh=%dx%d, x,y=%d,%d\n, "
+		"pixelaspect %d/%d\n",
+		prt_names(p->type, v4l2_type_names),
+		p->bounds.width, p->bounds.height,
+		p->bounds.left, p->bounds.top,
+		p->defrect.width, p->defrect.height,
+		p->defrect.left, p->defrect.top,
+		p->pixelaspect.numerator, p->pixelaspect.denominator);
+}
+
+static void v4l_print_crop(const void *arg, bool write_only)
+{
+	const struct v4l2_crop *p = arg;
+
+	pr_cont("type=%s, wxh=%dx%d, x,y=%d,%d\n",
+		prt_names(p->type, v4l2_type_names),
+		p->c.width, p->c.height,
+		p->c.left, p->c.top);
+}
+
+static void v4l_print_selection(const void *arg, bool write_only)
+{
+	const struct v4l2_selection *p = arg;
+
+	pr_cont("type=%s, target=%d, flags=0x%x, wxh=%dx%d, x,y=%d,%d\n",
+		prt_names(p->type, v4l2_type_names),
+		p->target, p->flags,
+		p->r.width, p->r.height, p->r.left, p->r.top);
+}
+
+static void v4l_print_jpegcompression(const void *arg, bool write_only)
+{
+	const struct v4l2_jpegcompression *p = arg;
+
+	pr_cont("quality=%d, APPn=%d, APP_len=%d, "
+		"COM_len=%d, jpeg_markers=0x%x\n",
+		p->quality, p->APPn, p->APP_len,
+		p->COM_len, p->jpeg_markers);
+}
+
+static void v4l_print_enc_idx(const void *arg, bool write_only)
+{
+	const struct v4l2_enc_idx *p = arg;
+
+	pr_cont("entries=%d, entries_cap=%d\n",
+			p->entries, p->entries_cap);
+}
+
+static void v4l_print_encoder_cmd(const void *arg, bool write_only)
+{
+	const struct v4l2_encoder_cmd *p = arg;
+
+	pr_cont("cmd=%d, flags=0x%x\n",
+			p->cmd, p->flags);
+}
+
+static void v4l_print_decoder_cmd(const void *arg, bool write_only)
+{
+	const struct v4l2_decoder_cmd *p = arg;
+
+	pr_cont("cmd=%d, flags=0x%x\n", p->cmd, p->flags);
+
+	if (p->cmd == V4L2_DEC_CMD_START)
+		pr_info("speed=%d, format=%u\n",
+				p->start.speed, p->start.format);
+	else if (p->cmd == V4L2_DEC_CMD_STOP)
+		pr_info("pts=%llu\n", p->stop.pts);
+}
+
+static void v4l_print_dbg_chip_ident(const void *arg, bool write_only)
+{
+	const struct v4l2_dbg_chip_ident *p = arg;
+
+	pr_cont("type=%u, ", p->match.type);
+	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
+		pr_cont("name=%s, ", p->match.name);
+	else
+		pr_cont("addr=%u, ", p->match.addr);
+	pr_cont("chip_ident=%u, revision=0x%x\n",
+			p->ident, p->revision);
+}
+
+static void v4l_print_dbg_register(const void *arg, bool write_only)
+{
+	const struct v4l2_dbg_register *p = arg;
+
+	pr_cont("type=%u, ", p->match.type);
+	if (p->match.type == V4L2_CHIP_MATCH_I2C_DRIVER)
+		pr_cont("name=%s, ", p->match.name);
+	else
+		pr_cont("addr=%u, ", p->match.addr);
+	pr_cont("reg=0x%llx, val=0x%llx\n",
+			p->reg, p->val);
+}
+
+static void v4l_print_dv_enum_presets(const void *arg, bool write_only)
+{
+	const struct v4l2_dv_enum_preset *p = arg;
+
+	pr_cont("index=%u, preset=%u, name=%s, width=%u, height=%u\n",
+			p->index, p->preset, p->name, p->width, p->height);
+}
+
+static void v4l_print_dv_preset(const void *arg, bool write_only)
+{
+	const struct v4l2_dv_preset *p = arg;
+
+	pr_cont("preset=%u\n", p->preset);
+}
+
+static void v4l_print_dv_timings(const void *arg, bool write_only)
+{
+	const struct v4l2_dv_timings *p = arg;
+
 	switch (p->type) {
 	case V4L2_DV_BT_656_1120:
-		dbgarg2("bt-656/1120:interlaced=%d,"
-				" pixelclock=%lld,"
-				" width=%d, height=%d, polarities=%x,"
-				" hfrontporch=%d, hsync=%d,"
-				" hbackporch=%d, vfrontporch=%d,"
-				" vsync=%d, vbackporch=%d,"
-				" il_vfrontporch=%d, il_vsync=%d,"
-				" il_vbackporch=%d, standards=%x, flags=%x\n",
+		pr_cont("type=bt-656/1120, interlaced=%u, "
+			"pixelclock=%llu, "
+			"width=%u, height=%u, polarities=0x%x, "
+			"hfrontporch=%u, hsync=%u, "
+			"hbackporch=%u, vfrontporch=%u, "
+			"vsync=%u, vbackporch=%u, "
+			"il_vfrontporch=%u, il_vsync=%u, "
+			"il_vbackporch=%u, standards=0x%x, flags=0x%x\n",
 				p->bt.interlaced, p->bt.pixelclock,
 				p->bt.width, p->bt.height,
 				p->bt.polarities, p->bt.hfrontporch,
@@ -394,67 +674,184 @@ static void dbgtimings(struct video_device *vfd,
 				p->bt.standards, p->bt.flags);
 		break;
 	default:
-		dbgarg2("Unknown type %d!\n", p->type);
+		pr_cont("type=%d\n", p->type);
 		break;
 	}
 }
 
-static inline void v4l_print_pix_fmt(struct video_device *vfd,
-						struct v4l2_pix_format *fmt)
+static void v4l_print_enum_dv_timings(const void *arg, bool write_only)
 {
-	dbgarg2("width=%d, height=%d, format=%c%c%c%c, field=%s, "
-		"bytesperline=%d sizeimage=%d, colorspace=%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->bytesperline, fmt->sizeimage, fmt->colorspace);
-};
+	const struct v4l2_enum_dv_timings *p = arg;
+
+	pr_cont("index=%u, ", p->index);
+	v4l_print_dv_timings(&p->timings, write_only);
+}
 
-static inline void v4l_print_pix_fmt_mplane(struct video_device *vfd,
-					    struct v4l2_pix_format_mplane *fmt)
+static void v4l_print_dv_timings_cap(const void *arg, bool write_only)
 {
-	int i;
+	const struct v4l2_dv_timings_cap *p = arg;
 
-	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);
+	switch (p->type) {
+	case V4L2_DV_BT_656_1120:
+		pr_cont("type=bt-656/1120, width=%u-%u, height=%u-%u, "
+			"pixelclock=%llu-%llu, standards=0x%x, capabilities=0x%x\n",
+			p->bt.min_width, p->bt.max_width,
+			p->bt.min_height, p->bt.max_height,
+			p->bt.min_pixelclock, p->bt.max_pixelclock,
+			p->bt.standards, p->bt.capabilities);
+		break;
+	default:
+		pr_cont("type=%u\n", p->type);
+		break;
+	}
+}
 
-	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 void v4l_print_frmsizeenum(const void *arg, bool write_only)
+{
+	const struct v4l2_frmsizeenum *p = arg;
+
+	pr_cont("index=%u, pixelformat=%c%c%c%c, type=%u",
+			p->index,
+			(p->pixel_format & 0xff),
+			(p->pixel_format >>  8) & 0xff,
+			(p->pixel_format >> 16) & 0xff,
+			(p->pixel_format >> 24) & 0xff,
+			p->type);
+	switch (p->type) {
+	case V4L2_FRMSIZE_TYPE_DISCRETE:
+		pr_cont(" wxh=%ux%u\n",
+			p->discrete.width, p->discrete.height);
+		break;
+	case V4L2_FRMSIZE_TYPE_STEPWISE:
+		pr_cont(" min=%ux%u, max=%ux%u, step=%ux%u\n",
+				p->stepwise.min_width,  p->stepwise.min_height,
+				p->stepwise.step_width, p->stepwise.step_height,
+				p->stepwise.max_width,  p->stepwise.max_height);
+		break;
+	case V4L2_FRMSIZE_TYPE_CONTINUOUS:
+		/* fall through */
+	default:
+		pr_cont("\n");
+		break;
+	}
 }
 
-static inline void v4l_print_ext_ctrls(unsigned int cmd,
-	struct video_device *vfd, struct v4l2_ext_controls *c, int show_vals)
+static void v4l_print_frmivalenum(const void *arg, bool write_only)
 {
-	__u32 i;
+	const struct v4l2_frmivalenum *p = arg;
 
-	if (!(vfd->debug & V4L2_DEBUG_IOCTL_ARG))
-		return;
-	dbgarg(cmd, "");
-	printk(KERN_CONT "class=0x%x", c->ctrl_class);
-	for (i = 0; i < c->count; i++) {
-		if (show_vals && !c->controls[i].size)
-			printk(KERN_CONT " id/val=0x%x/0x%x",
-				c->controls[i].id, c->controls[i].value);
+	pr_cont("index=%u, pixelformat=%c%c%c%c, wxh=%ux%u, type=%u",
+			p->index,
+			(p->pixel_format & 0xff),
+			(p->pixel_format >>  8) & 0xff,
+			(p->pixel_format >> 16) & 0xff,
+			(p->pixel_format >> 24) & 0xff,
+			p->width, p->height, p->type);
+	switch (p->type) {
+	case V4L2_FRMIVAL_TYPE_DISCRETE:
+		pr_cont(" fps=%d/%d\n",
+				p->discrete.numerator,
+				p->discrete.denominator);
+		break;
+	case V4L2_FRMIVAL_TYPE_STEPWISE:
+		pr_cont(" min=%d/%d, max=%d/%d, step=%d/%d\n",
+				p->stepwise.min.numerator,
+				p->stepwise.min.denominator,
+				p->stepwise.max.numerator,
+				p->stepwise.max.denominator,
+				p->stepwise.step.numerator,
+				p->stepwise.step.denominator);
+		break;
+	case V4L2_FRMIVAL_TYPE_CONTINUOUS:
+		/* fall through */
+	default:
+		pr_cont("\n");
+		break;
+	}
+}
+
+static void v4l_print_event(const void *arg, bool write_only)
+{
+	const struct v4l2_event *p = arg;
+	const struct v4l2_event_ctrl *c;
+
+	pr_cont("type=0x%x, pending=%u, sequence=%u, id=%u, "
+		"timestamp=%lu.%9.9lu\n",
+			p->type, p->pending, p->sequence, p->id,
+			p->timestamp.tv_sec, p->timestamp.tv_nsec);
+	switch (p->type) {
+	case V4L2_EVENT_VSYNC:
+		printk(KERN_DEBUG "field=%s\n",
+			prt_names(p->u.vsync.field, v4l2_field_names));
+		break;
+	case V4L2_EVENT_CTRL:
+		c = &p->u.ctrl;
+		printk(KERN_DEBUG "changes=0x%x, type=%u, ",
+			c->changes, c->type);
+		if (c->type == V4L2_CTRL_TYPE_INTEGER64)
+			pr_cont("value64=%lld, ", c->value64);
 		else
-			printk(KERN_CONT " id=0x%x,size=%u",
-				c->controls[i].id, c->controls[i].size);
+			pr_cont("value=%d, ", c->value);
+		pr_cont("flags=0x%x, minimum=%d, maximum=%d, step=%d,"
+				" default_value=%d\n",
+			c->flags, c->minimum, c->maximum,
+			c->step, c->default_value);
+		break;
+	case V4L2_EVENT_FRAME_SYNC:
+		pr_cont("frame_sequence=%u\n",
+			p->u.frame_sync.frame_sequence);
+		break;
 	}
-	printk(KERN_CONT "\n");
-};
+}
+
+static void v4l_print_event_subscription(const void *arg, bool write_only)
+{
+	const struct v4l2_event_subscription *p = arg;
 
-static inline int check_ext_ctrls(struct v4l2_ext_controls *c, int allow_priv)
+	pr_cont("type=0x%x, id=0x%x, flags=0x%x\n",
+			p->type, p->id, p->flags);
+}
+
+static void v4l_print_sliced_vbi_cap(const void *arg, bool write_only)
+{
+	const struct v4l2_sliced_vbi_cap *p = arg;
+	int i;
+
+	pr_cont("type=%s, service_set=0x%08x\n",
+			prt_names(p->type, v4l2_type_names), p->service_set);
+	for (i = 0; i < 24; i++)
+		printk(KERN_DEBUG "line[%02u]=0x%04x, 0x%04x\n", i,
+				p->service_lines[0][i],
+				p->service_lines[1][i]);
+}
+
+static void v4l_print_freq_band(const void *arg, bool write_only)
+{
+	const struct v4l2_frequency_band *p = arg;
+
+	pr_cont("tuner=%u, type=%u, index=%u, capability=0x%x, "
+			"rangelow=%u, rangehigh=%u, modulation=0x%x\n",
+			p->tuner, p->type, p->index,
+			p->capability, p->rangelow,
+			p->rangehigh, p->modulation);
+}
+
+static void v4l_print_u32(const void *arg, bool write_only)
+{
+	pr_cont("value=%u\n", *(const u32 *)arg);
+}
+
+static void v4l_print_newline(const void *arg, bool write_only)
+{
+	pr_cont("\n");
+}
+
+static void v4l_print_default(const void *arg, bool write_only)
+{
+	pr_cont("driver-specific ioctl\n");
+}
+
+static int check_ext_ctrls(struct v4l2_ext_controls *c, int allow_priv)
 {
 	__u32 i;
 
@@ -536,1615 +933,1234 @@ static int check_fmt(const struct v4l2_ioctl_ops *ops, enum v4l2_buf_type type)
 	return -EINVAL;
 }
 
-static long __video_do_ioctl(struct file *file,
-		unsigned int cmd, void *arg)
+static int v4l_querycap(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, 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_fh *vfh = NULL;
-	int use_fh_prio = 0;
-	long ret = -ENOTTY;
-
-	if (ops == NULL) {
-		printk(KERN_WARNING "videodev: \"%s\" has no ioctl_ops.\n",
-				vfd->name);
-		return ret;
-	}
+	struct v4l2_capability *cap = (struct v4l2_capability *)arg;
 
-	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
-		vfh = file->private_data;
-		use_fh_prio = test_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
-	}
+	cap->version = LINUX_VERSION_CODE;
+	return ops->vidioc_querycap(file, fh, cap);
+}
 
-	if (v4l2_is_known_ioctl(cmd)) {
-		struct v4l2_ioctl_info *info = &v4l2_ioctls[_IOC_NR(cmd)];
+static int v4l_s_input(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return ops->vidioc_s_input(file, fh, *(unsigned int *)arg);
+}
 
-	        if (!test_bit(_IOC_NR(cmd), vfd->valid_ioctls) &&
-		    !((info->flags & INFO_FL_CTRL) && vfh && vfh->ctrl_handler))
-			return -ENOTTY;
+static int v4l_s_output(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return ops->vidioc_s_output(file, fh, *(unsigned int *)arg);
+}
 
-		if (use_fh_prio && (info->flags & INFO_FL_PRIO)) {
-			ret = v4l2_prio_check(vfd->prio, vfh->prio);
-			if (ret)
-				return ret;
-		}
-	}
+static int v4l_g_priority(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd;
+	u32 *p = arg;
 
-	if ((vfd->debug & V4L2_DEBUG_IOCTL) &&
-				!(vfd->debug & V4L2_DEBUG_IOCTL_ARG)) {
-		v4l_print_ioctl(vfd->name, cmd);
-		printk(KERN_CONT "\n");
-	}
+	if (ops->vidioc_g_priority)
+		return ops->vidioc_g_priority(file, fh, arg);
+	vfd = video_devdata(file);
+	*p = v4l2_prio_max(&vfd->v4l2_dev->prio);
+	return 0;
+}
 
-	switch (cmd) {
+static int v4l_s_priority(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd;
+	struct v4l2_fh *vfh;
+	u32 *p = arg;
+
+	if (ops->vidioc_s_priority)
+		return ops->vidioc_s_priority(file, fh, *p);
+	vfd = video_devdata(file);
+	vfh = file->private_data;
+	return v4l2_prio_change(&vfd->v4l2_dev->prio, &vfh->prio, *p);
+}
 
-	/* --- capabilities ------------------------------------------ */
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability *cap = (struct v4l2_capability *)arg;
-
-		cap->version = LINUX_VERSION_CODE;
-		ret = ops->vidioc_querycap(file, fh, cap);
-		if (!ret)
-			dbgarg(cmd, "driver=%s, card=%s, bus=%s, "
-					"version=0x%08x, "
-					"capabilities=0x%08x, "
-					"device_caps=0x%08x\n",
-					cap->driver, cap->card, cap->bus_info,
-					cap->version,
-					cap->capabilities,
-					cap->device_caps);
-		break;
-	}
+static int v4l_enuminput(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_input *p = arg;
 
-	/* --- priority ------------------------------------------ */
-	case VIDIOC_G_PRIORITY:
-	{
-		enum v4l2_priority *p = arg;
+	/*
+	 * We set the flags for CAP_PRESETS, CAP_CUSTOM_TIMINGS &
+	 * CAP_STD here based on ioctl handler provided by the
+	 * driver. If the driver doesn't support these
+	 * for a specific input, it must override these flags.
+	 */
+	if (ops->vidioc_s_std)
+		p->capabilities |= V4L2_IN_CAP_STD;
+	if (ops->vidioc_s_dv_preset)
+		p->capabilities |= V4L2_IN_CAP_PRESETS;
+	if (ops->vidioc_s_dv_timings)
+		p->capabilities |= V4L2_IN_CAP_CUSTOM_TIMINGS;
+
+	return ops->vidioc_enum_input(file, fh, p);
+}
 
-		if (ops->vidioc_g_priority) {
-			ret = ops->vidioc_g_priority(file, fh, p);
-		} else if (use_fh_prio) {
-			*p = v4l2_prio_max(&vfd->v4l2_dev->prio);
-			ret = 0;
-		}
-		if (!ret)
-			dbgarg(cmd, "priority is %d\n", *p);
-		break;
-	}
-	case VIDIOC_S_PRIORITY:
-	{
-		enum v4l2_priority *p = arg;
+static int v4l_enumoutput(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_output *p = arg;
 
-		dbgarg(cmd, "setting priority to %d\n", *p);
-		if (ops->vidioc_s_priority)
-			ret = ops->vidioc_s_priority(file, fh, *p);
-		else
-			ret = v4l2_prio_change(&vfd->v4l2_dev->prio,
-							&vfh->prio, *p);
-		break;
-	}
+	/*
+	 * We set the flags for CAP_PRESETS, CAP_CUSTOM_TIMINGS &
+	 * CAP_STD here based on ioctl handler provided by the
+	 * driver. If the driver doesn't support these
+	 * for a specific output, it must override these flags.
+	 */
+	if (ops->vidioc_s_std)
+		p->capabilities |= V4L2_OUT_CAP_STD;
+	if (ops->vidioc_s_dv_preset)
+		p->capabilities |= V4L2_OUT_CAP_PRESETS;
+	if (ops->vidioc_s_dv_timings)
+		p->capabilities |= V4L2_OUT_CAP_CUSTOM_TIMINGS;
+
+	return ops->vidioc_enum_output(file, fh, p);
+}
 
-	/* --- capture ioctls ---------------------------------------- */
-	case VIDIOC_ENUM_FMT:
-	{
-		struct v4l2_fmtdesc *f = arg;
+static int v4l_enum_fmt(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_fmtdesc *p = arg;
 
-		ret = -EINVAL;
-		switch (f->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-			if (likely(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 (likely(ops->vidioc_enum_fmt_vid_cap_mplane))
-				ret = ops->vidioc_enum_fmt_vid_cap_mplane(file,
-									fh, f);
+	switch (p->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (unlikely(!ops->vidioc_enum_fmt_vid_cap))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
-			if (likely(ops->vidioc_enum_fmt_vid_overlay))
-				ret = ops->vidioc_enum_fmt_vid_overlay(file,
-					fh, f);
+		return ops->vidioc_enum_fmt_vid_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		if (unlikely(!ops->vidioc_enum_fmt_vid_cap_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
-			if (likely(ops->vidioc_enum_fmt_vid_out))
-				ret = ops->vidioc_enum_fmt_vid_out(file, fh, f);
+		return ops->vidioc_enum_fmt_vid_cap_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		if (unlikely(!ops->vidioc_enum_fmt_vid_overlay))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
-			if (likely(ops->vidioc_enum_fmt_vid_out_mplane))
-				ret = ops->vidioc_enum_fmt_vid_out_mplane(file,
-									fh, f);
+		return ops->vidioc_enum_fmt_vid_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		if (unlikely(!ops->vidioc_enum_fmt_vid_out))
 			break;
-		case V4L2_BUF_TYPE_PRIVATE:
-			if (likely(ops->vidioc_enum_fmt_type_private))
-				ret = ops->vidioc_enum_fmt_type_private(file,
-								fh, f);
+		return ops->vidioc_enum_fmt_vid_out(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (unlikely(!ops->vidioc_enum_fmt_vid_out_mplane))
 			break;
-		default:
+		return ops->vidioc_enum_fmt_vid_out_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_PRIVATE:
+		if (unlikely(!ops->vidioc_enum_fmt_type_private))
 			break;
-		}
-		if (likely(!ret))
-			dbgarg(cmd, "index=%d, type=%d, flags=%d, "
-				"pixelformat=%c%c%c%c, description='%s'\n",
-				f->index, f->type, f->flags,
-				(f->pixelformat & 0xff),
-				(f->pixelformat >>  8) & 0xff,
-				(f->pixelformat >> 16) & 0xff,
-				(f->pixelformat >> 24) & 0xff,
-				f->description);
-		break;
+		return ops->vidioc_enum_fmt_type_private(file, fh, arg);
 	}
-	case VIDIOC_G_FMT:
-	{
-		struct v4l2_format *f = (struct v4l2_format *)arg;
-
-		/* FIXME: Should be one dump per type */
-		dbgarg(cmd, "type=%s\n", prt_names(f->type, v4l2_type_names));
-
-		ret = -EINVAL;
-		switch (f->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-			if (ops->vidioc_g_fmt_vid_cap)
-				ret = ops->vidioc_g_fmt_vid_cap(file, fh, f);
-			if (!ret)
-				v4l_print_pix_fmt(vfd, &f->fmt.pix);
+	return -EINVAL;
+}
+
+static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_format *p = arg;
+
+	switch (p->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (unlikely(!ops->vidioc_g_fmt_vid_cap))
 			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);
-			if (!ret)
-				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+		return ops->vidioc_g_fmt_vid_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		if (unlikely(!ops->vidioc_g_fmt_vid_cap_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
-			if (likely(ops->vidioc_g_fmt_vid_overlay))
-				ret = ops->vidioc_g_fmt_vid_overlay(file,
-								    fh, f);
+		return ops->vidioc_g_fmt_vid_cap_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		if (unlikely(!ops->vidioc_g_fmt_vid_overlay))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
-			if (ops->vidioc_g_fmt_vid_out)
-				ret = ops->vidioc_g_fmt_vid_out(file, fh, f);
-			if (!ret)
-				v4l_print_pix_fmt(vfd, &f->fmt.pix);
+		return ops->vidioc_g_fmt_vid_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		if (unlikely(!ops->vidioc_g_fmt_vid_out))
 			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);
-			if (!ret)
-				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+		return ops->vidioc_g_fmt_vid_out(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (unlikely(!ops->vidioc_g_fmt_vid_out_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
-			if (likely(ops->vidioc_g_fmt_vid_out_overlay))
-				ret = ops->vidioc_g_fmt_vid_out_overlay(file,
-				       fh, f);
+		return ops->vidioc_g_fmt_vid_out_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
+		if (unlikely(!ops->vidioc_g_fmt_vid_out_overlay))
 			break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			if (likely(ops->vidioc_g_fmt_vbi_cap))
-				ret = ops->vidioc_g_fmt_vbi_cap(file, fh, f);
+		return ops->vidioc_g_fmt_vid_out_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+		if (unlikely(!ops->vidioc_g_fmt_vbi_cap))
 			break;
-		case V4L2_BUF_TYPE_VBI_OUTPUT:
-			if (likely(ops->vidioc_g_fmt_vbi_out))
-				ret = ops->vidioc_g_fmt_vbi_out(file, fh, f);
+		return ops->vidioc_g_fmt_vbi_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+		if (unlikely(!ops->vidioc_g_fmt_vbi_out))
 			break;
-		case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
-			if (likely(ops->vidioc_g_fmt_sliced_vbi_cap))
-				ret = ops->vidioc_g_fmt_sliced_vbi_cap(file,
-									fh, f);
+		return ops->vidioc_g_fmt_vbi_out(file, fh, arg);
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+		if (unlikely(!ops->vidioc_g_fmt_sliced_vbi_cap))
 			break;
-		case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
-			if (likely(ops->vidioc_g_fmt_sliced_vbi_out))
-				ret = ops->vidioc_g_fmt_sliced_vbi_out(file,
-									fh, f);
+		return ops->vidioc_g_fmt_sliced_vbi_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		if (unlikely(!ops->vidioc_g_fmt_sliced_vbi_out))
 			break;
-		case V4L2_BUF_TYPE_PRIVATE:
-			if (likely(ops->vidioc_g_fmt_type_private))
-				ret = ops->vidioc_g_fmt_type_private(file,
-								fh, f);
+		return ops->vidioc_g_fmt_sliced_vbi_out(file, fh, arg);
+	case V4L2_BUF_TYPE_PRIVATE:
+		if (unlikely(!ops->vidioc_g_fmt_type_private))
 			break;
-		}
-		break;
+		return ops->vidioc_g_fmt_type_private(file, fh, arg);
 	}
-	case VIDIOC_S_FMT:
-	{
-		struct v4l2_format *f = (struct v4l2_format *)arg;
-
-		ret = -EINVAL;
+	return -EINVAL;
+}
 
-		/* FIXME: Should be one dump per type */
-		dbgarg(cmd, "type=%s\n", prt_names(f->type, v4l2_type_names));
+static int v4l_s_fmt(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_format *p = arg;
 
-		switch (f->type) {
-		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)
-				ret = ops->vidioc_s_fmt_vid_cap(file, fh, f);
+	switch (p->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (unlikely(!ops->vidioc_s_fmt_vid_cap))
 			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);
+		CLEAR_AFTER_FIELD(p, fmt.pix);
+		return ops->vidioc_s_fmt_vid_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		if (unlikely(!ops->vidioc_s_fmt_vid_cap_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
-			CLEAR_AFTER_FIELD(f, fmt.win);
-			if (ops->vidioc_s_fmt_vid_overlay)
-				ret = ops->vidioc_s_fmt_vid_overlay(file,
-								    fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		return ops->vidioc_s_fmt_vid_cap_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		if (unlikely(!ops->vidioc_s_fmt_vid_overlay))
 			break;
-		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)
-				ret = ops->vidioc_s_fmt_vid_out(file, fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.win);
+		return ops->vidioc_s_fmt_vid_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		if (unlikely(!ops->vidioc_s_fmt_vid_out))
 			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);
+		CLEAR_AFTER_FIELD(p, fmt.pix);
+		return ops->vidioc_s_fmt_vid_out(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (unlikely(!ops->vidioc_s_fmt_vid_out_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
-			CLEAR_AFTER_FIELD(f, fmt.win);
-			if (ops->vidioc_s_fmt_vid_out_overlay)
-				ret = ops->vidioc_s_fmt_vid_out_overlay(file,
-					fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		return ops->vidioc_s_fmt_vid_out_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
+		if (unlikely(!ops->vidioc_s_fmt_vid_out_overlay))
 			break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			CLEAR_AFTER_FIELD(f, fmt.vbi);
-			if (likely(ops->vidioc_s_fmt_vbi_cap))
-				ret = ops->vidioc_s_fmt_vbi_cap(file, fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.win);
+		return ops->vidioc_s_fmt_vid_out_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+		if (unlikely(!ops->vidioc_s_fmt_vbi_cap))
 			break;
-		case V4L2_BUF_TYPE_VBI_OUTPUT:
-			CLEAR_AFTER_FIELD(f, fmt.vbi);
-			if (likely(ops->vidioc_s_fmt_vbi_out))
-				ret = ops->vidioc_s_fmt_vbi_out(file, fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.vbi);
+		return ops->vidioc_s_fmt_vbi_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+		if (unlikely(!ops->vidioc_s_fmt_vbi_out))
 			break;
-		case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
-			CLEAR_AFTER_FIELD(f, fmt.sliced);
-			if (likely(ops->vidioc_s_fmt_sliced_vbi_cap))
-				ret = ops->vidioc_s_fmt_sliced_vbi_cap(file,
-									fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.vbi);
+		return ops->vidioc_s_fmt_vbi_out(file, fh, arg);
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+		if (unlikely(!ops->vidioc_s_fmt_sliced_vbi_cap))
 			break;
-		case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
-			CLEAR_AFTER_FIELD(f, fmt.sliced);
-			if (likely(ops->vidioc_s_fmt_sliced_vbi_out))
-				ret = ops->vidioc_s_fmt_sliced_vbi_out(file,
-									fh, f);
-
+		CLEAR_AFTER_FIELD(p, fmt.sliced);
+		return ops->vidioc_s_fmt_sliced_vbi_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		if (unlikely(!ops->vidioc_s_fmt_sliced_vbi_out))
 			break;
-		case V4L2_BUF_TYPE_PRIVATE:
-			/* CLEAR_AFTER_FIELD(f, fmt.raw_data); <- does nothing */
-			if (likely(ops->vidioc_s_fmt_type_private))
-				ret = ops->vidioc_s_fmt_type_private(file,
-								fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.sliced);
+		return ops->vidioc_s_fmt_sliced_vbi_out(file, fh, arg);
+	case V4L2_BUF_TYPE_PRIVATE:
+		if (unlikely(!ops->vidioc_s_fmt_type_private))
 			break;
-		}
-		break;
+		return ops->vidioc_s_fmt_type_private(file, fh, arg);
 	}
-	case VIDIOC_TRY_FMT:
-	{
-		struct v4l2_format *f = (struct v4l2_format *)arg;
-
-		/* FIXME: Should be one dump per type */
-		dbgarg(cmd, "type=%s\n", prt_names(f->type,
-						v4l2_type_names));
-		ret = -EINVAL;
-		switch (f->type) {
-		case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-			CLEAR_AFTER_FIELD(f, fmt.pix);
-			if (ops->vidioc_try_fmt_vid_cap)
-				ret = ops->vidioc_try_fmt_vid_cap(file, fh, f);
-			if (!ret)
-				v4l_print_pix_fmt(vfd, &f->fmt.pix);
+	return -EINVAL;
+}
+
+static int v4l_try_fmt(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_format *p = arg;
+
+	switch (p->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (unlikely(!ops->vidioc_try_fmt_vid_cap))
 			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);
-			if (!ret)
-				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+		CLEAR_AFTER_FIELD(p, fmt.pix);
+		return ops->vidioc_try_fmt_vid_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		if (unlikely(!ops->vidioc_try_fmt_vid_cap_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OVERLAY:
-			CLEAR_AFTER_FIELD(f, fmt.win);
-			if (likely(ops->vidioc_try_fmt_vid_overlay))
-				ret = ops->vidioc_try_fmt_vid_overlay(file,
-					fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		return ops->vidioc_try_fmt_vid_cap_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		if (unlikely(!ops->vidioc_try_fmt_vid_overlay))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT:
-			CLEAR_AFTER_FIELD(f, fmt.pix);
-			if (ops->vidioc_try_fmt_vid_out)
-				ret = ops->vidioc_try_fmt_vid_out(file, fh, f);
-			if (!ret)
-				v4l_print_pix_fmt(vfd, &f->fmt.pix);
+		CLEAR_AFTER_FIELD(p, fmt.win);
+		return ops->vidioc_try_fmt_vid_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		if (unlikely(!ops->vidioc_try_fmt_vid_out))
 			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);
-			if (!ret)
-				v4l_print_pix_fmt_mplane(vfd, &f->fmt.pix_mp);
+		CLEAR_AFTER_FIELD(p, fmt.pix);
+		return ops->vidioc_try_fmt_vid_out(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (unlikely(!ops->vidioc_try_fmt_vid_out_mplane))
 			break;
-		case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
-			CLEAR_AFTER_FIELD(f, fmt.win);
-			if (likely(ops->vidioc_try_fmt_vid_out_overlay))
-				ret = ops->vidioc_try_fmt_vid_out_overlay(file,
-				       fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.pix_mp);
+		return ops->vidioc_try_fmt_vid_out_mplane(file, fh, arg);
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
+		if (unlikely(!ops->vidioc_try_fmt_vid_out_overlay))
 			break;
-		case V4L2_BUF_TYPE_VBI_CAPTURE:
-			CLEAR_AFTER_FIELD(f, fmt.vbi);
-			if (likely(ops->vidioc_try_fmt_vbi_cap))
-				ret = ops->vidioc_try_fmt_vbi_cap(file, fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.win);
+		return ops->vidioc_try_fmt_vid_out_overlay(file, fh, arg);
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+		if (unlikely(!ops->vidioc_try_fmt_vbi_cap))
 			break;
-		case V4L2_BUF_TYPE_VBI_OUTPUT:
-			CLEAR_AFTER_FIELD(f, fmt.vbi);
-			if (likely(ops->vidioc_try_fmt_vbi_out))
-				ret = ops->vidioc_try_fmt_vbi_out(file, fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.vbi);
+		return ops->vidioc_try_fmt_vbi_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+		if (unlikely(!ops->vidioc_try_fmt_vbi_out))
 			break;
-		case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
-			CLEAR_AFTER_FIELD(f, fmt.sliced);
-			if (likely(ops->vidioc_try_fmt_sliced_vbi_cap))
-				ret = ops->vidioc_try_fmt_sliced_vbi_cap(file,
-								fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.vbi);
+		return ops->vidioc_try_fmt_vbi_out(file, fh, arg);
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+		if (unlikely(!ops->vidioc_try_fmt_sliced_vbi_cap))
 			break;
-		case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
-			CLEAR_AFTER_FIELD(f, fmt.sliced);
-			if (likely(ops->vidioc_try_fmt_sliced_vbi_out))
-				ret = ops->vidioc_try_fmt_sliced_vbi_out(file,
-								fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.sliced);
+		return ops->vidioc_try_fmt_sliced_vbi_cap(file, fh, arg);
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		if (unlikely(!ops->vidioc_try_fmt_sliced_vbi_out))
 			break;
-		case V4L2_BUF_TYPE_PRIVATE:
-			/* CLEAR_AFTER_FIELD(f, fmt.raw_data); <- does nothing */
-			if (likely(ops->vidioc_try_fmt_type_private))
-				ret = ops->vidioc_try_fmt_type_private(file,
-								fh, f);
+		CLEAR_AFTER_FIELD(p, fmt.sliced);
+		return ops->vidioc_try_fmt_sliced_vbi_out(file, fh, arg);
+	case V4L2_BUF_TYPE_PRIVATE:
+		if (unlikely(!ops->vidioc_try_fmt_type_private))
 			break;
-		}
-		break;
+		return ops->vidioc_try_fmt_type_private(file, fh, arg);
 	}
-	/* FIXME: Those buf reqs could be handled here,
-	   with some changes on videobuf to allow its header to be included at
-	   videodev2.h or being merged at videodev2.
-	 */
-	case VIDIOC_REQBUFS:
-	{
-		struct v4l2_requestbuffers *p = arg;
+	return -EINVAL;
+}
 
-		ret = check_fmt(ops, p->type);
-		if (ret)
-			break;
+static int v4l_streamon(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return ops->vidioc_streamon(file, fh, *(unsigned int *)arg);
+}
 
-		if (p->type < V4L2_BUF_TYPE_PRIVATE)
-			CLEAR_AFTER_FIELD(p, memory);
+static int v4l_streamoff(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return ops->vidioc_streamoff(file, fh, *(unsigned int *)arg);
+}
 
-		ret = ops->vidioc_reqbufs(file, fh, p);
-		dbgarg(cmd, "count=%d, type=%s, memory=%s\n",
-				p->count,
-				prt_names(p->type, v4l2_type_names),
-				prt_names(p->memory, v4l2_memory_names));
-		break;
-	}
-	case VIDIOC_QUERYBUF:
-	{
-		struct v4l2_buffer *p = arg;
+static int v4l_g_tuner(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_tuner *p = arg;
+	int err;
 
-		ret = check_fmt(ops, p->type);
-		if (ret)
-			break;
+	p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
+			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
+	err = ops->vidioc_g_tuner(file, fh, p);
+	if (!err)
+		p->capability |= V4L2_TUNER_CAP_FREQ_BANDS;
+	return err;
+}
 
-		ret = ops->vidioc_querybuf(file, fh, p);
-		if (!ret)
-			dbgbuf(cmd, vfd, p);
-		break;
-	}
-	case VIDIOC_QBUF:
-	{
-		struct v4l2_buffer *p = arg;
+static int v4l_s_tuner(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_tuner *p = arg;
 
-		ret = check_fmt(ops, p->type);
-		if (ret)
-			break;
+	p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
+			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
+	return ops->vidioc_s_tuner(file, fh, p);
+}
 
-		ret = ops->vidioc_qbuf(file, fh, p);
-		if (!ret)
-			dbgbuf(cmd, vfd, p);
-		break;
-	}
-	case VIDIOC_DQBUF:
-	{
-		struct v4l2_buffer *p = arg;
+static int v4l_g_modulator(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_modulator *p = arg;
+	int err;
 
-		ret = check_fmt(ops, p->type);
-		if (ret)
-			break;
+	err = ops->vidioc_g_modulator(file, fh, p);
+	if (!err)
+		p->capability |= V4L2_TUNER_CAP_FREQ_BANDS;
+	return err;
+}
 
-		ret = ops->vidioc_dqbuf(file, fh, p);
-		if (!ret)
-			dbgbuf(cmd, vfd, p);
-		break;
-	}
-	case VIDIOC_OVERLAY:
-	{
-		int *i = arg;
+static int v4l_g_frequency(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_frequency *p = arg;
 
-		dbgarg(cmd, "value=%d\n", *i);
-		ret = ops->vidioc_overlay(file, fh, *i);
-		break;
-	}
-	case VIDIOC_G_FBUF:
-	{
-		struct v4l2_framebuffer *p = arg;
-
-		ret = ops->vidioc_g_fbuf(file, fh, arg);
-		if (!ret) {
-			dbgarg(cmd, "capability=0x%x, flags=%d, base=0x%08lx\n",
-					p->capability, p->flags,
-					(unsigned long)p->base);
-			v4l_print_pix_fmt(vfd, &p->fmt);
-		}
-		break;
-	}
-	case VIDIOC_S_FBUF:
-	{
-		struct v4l2_framebuffer *p = arg;
-
-		dbgarg(cmd, "capability=0x%x, flags=%d, base=0x%08lx\n",
-			p->capability, p->flags, (unsigned long)p->base);
-		v4l_print_pix_fmt(vfd, &p->fmt);
-		ret = ops->vidioc_s_fbuf(file, fh, arg);
-		break;
-	}
-	case VIDIOC_STREAMON:
-	{
-		enum v4l2_buf_type i = *(int *)arg;
+	p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
+			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
+	return ops->vidioc_g_frequency(file, fh, p);
+}
 
-		dbgarg(cmd, "type=%s\n", prt_names(i, v4l2_type_names));
-		ret = ops->vidioc_streamon(file, fh, i);
-		break;
-	}
-	case VIDIOC_STREAMOFF:
-	{
-		enum v4l2_buf_type i = *(int *)arg;
+static int v4l_s_frequency(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_frequency *p = arg;
+	enum v4l2_tuner_type type;
 
-		dbgarg(cmd, "type=%s\n", prt_names(i, v4l2_type_names));
-		ret = ops->vidioc_streamoff(file, fh, i);
-		break;
-	}
-	/* ---------- tv norms ---------- */
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *p = arg;
-		v4l2_std_id id = vfd->tvnorms, curr_id = 0;
-		unsigned int index = p->index, i, j = 0;
-		const char *descr = "";
-
-		if (id == 0)
-			break;
-		ret = -EINVAL;
-
-		/* Return norm array in a canonical way */
-		for (i = 0; i <= index && id; i++) {
-			/* last std value in the standards array is 0, so this
-			   while always ends there since (id & 0) == 0. */
-			while ((id & standards[j].std) != standards[j].std)
-				j++;
-			curr_id = standards[j].std;
-			descr = standards[j].descr;
+	type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
+			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
+	if (p->type != type)
+		return -EINVAL;
+	return ops->vidioc_s_frequency(file, fh, p);
+}
+
+static int v4l_enumstd(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_standard *p = arg;
+	v4l2_std_id id = vfd->tvnorms, curr_id = 0;
+	unsigned int index = p->index, i, j = 0;
+	const char *descr = "";
+
+	/* Return norm array in a canonical way */
+	for (i = 0; i <= index && id; i++) {
+		/* last std value in the standards array is 0, so this
+		   while always ends there since (id & 0) == 0. */
+		while ((id & standards[j].std) != standards[j].std)
 			j++;
-			if (curr_id == 0)
-				break;
-			if (curr_id != V4L2_STD_PAL &&
-			    curr_id != V4L2_STD_SECAM &&
-			    curr_id != V4L2_STD_NTSC)
-				id &= ~curr_id;
-		}
-		if (i <= index)
+		curr_id = standards[j].std;
+		descr = standards[j].descr;
+		j++;
+		if (curr_id == 0)
 			break;
+		if (curr_id != V4L2_STD_PAL &&
+				curr_id != V4L2_STD_SECAM &&
+				curr_id != V4L2_STD_NTSC)
+			id &= ~curr_id;
+	}
+	if (i <= index)
+		return -EINVAL;
 
-		v4l2_video_std_construct(p, curr_id, descr);
+	v4l2_video_std_construct(p, curr_id, descr);
+	return 0;
+}
 
-		dbgarg(cmd, "index=%d, id=0x%Lx, name=%s, fps=%d/%d, "
-				"framelines=%d\n", p->index,
-				(unsigned long long)p->id, p->name,
-				p->frameperiod.numerator,
-				p->frameperiod.denominator,
-				p->framelines);
+static int v4l_g_std(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	v4l2_std_id *id = arg;
 
-		ret = 0;
-		break;
+	/* Calls the specific handler */
+	if (ops->vidioc_g_std)
+		return ops->vidioc_g_std(file, fh, arg);
+	if (vfd->current_norm) {
+		*id = vfd->current_norm;
+		return 0;
 	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *id = arg;
-
-		/* Calls the specific handler */
-		if (ops->vidioc_g_std)
-			ret = ops->vidioc_g_std(file, fh, id);
-		else if (vfd->current_norm) {
-			ret = 0;
-			*id = vfd->current_norm;
-		}
+	return -ENOTTY;
+}
 
-		if (likely(!ret))
-			dbgarg(cmd, "std=0x%08Lx\n", (long long unsigned)*id);
-		break;
-	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *id = arg, norm;
+static int v4l_s_std(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	v4l2_std_id *id = arg, norm;
+	int ret;
 
-		dbgarg(cmd, "std=%08Lx\n", (long long unsigned)*id);
+	norm = (*id) & vfd->tvnorms;
+	if (vfd->tvnorms && !norm)	/* Check if std is supported */
+		return -EINVAL;
 
-		ret = -EINVAL;
-		norm = (*id) & vfd->tvnorms;
-		if (vfd->tvnorms && !norm)	/* Check if std is supported */
-			break;
+	/* Calls the specific handler */
+	ret = ops->vidioc_s_std(file, fh, &norm);
 
-		/* Calls the specific handler */
-		ret = ops->vidioc_s_std(file, fh, &norm);
+	/* Updates standard information */
+	if (ret >= 0)
+		vfd->current_norm = norm;
+	return ret;
+}
 
-		/* Updates standard information */
-		if (ret >= 0)
-			vfd->current_norm = norm;
-		break;
-	}
-	case VIDIOC_QUERYSTD:
-	{
-		v4l2_std_id *p = arg;
+static int v4l_querystd(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	v4l2_std_id *p = arg;
+
+	/*
+	 * If nothing detected, it should return all supported
+	 * standard.
+	 * Drivers just need to mask the std argument, in order
+	 * to remove the standards that don't apply from the mask.
+	 * This means that tuners, audio and video decoders can join
+	 * their efforts to improve the standards detection.
+	 */
+	*p = vfd->tvnorms;
+	return ops->vidioc_querystd(file, fh, arg);
+}
 
-		/*
-		 * If nothing detected, it should return all supported
-		 * Drivers just need to mask the std argument, in order
-		 * to remove the standards that don't apply from the mask.
-		 * This means that tuners, audio and video decoders can join
-		 * their efforts to improve the standards detection
-		 */
-		*p = vfd->tvnorms;
-		ret = ops->vidioc_querystd(file, fh, arg);
-		if (!ret)
-			dbgarg(cmd, "detected std=%08Lx\n",
-						(unsigned long long)*p);
-		break;
-	}
-	/* ------ input switching ---------- */
-	/* FIXME: Inputs can be handled inside videodev2 */
-	case VIDIOC_ENUMINPUT:
-	{
-		struct v4l2_input *p = arg;
+static int v4l_s_hw_freq_seek(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_hw_freq_seek *p = arg;
+	enum v4l2_tuner_type type;
 
-		/*
-		 * We set the flags for CAP_PRESETS, CAP_CUSTOM_TIMINGS &
-		 * CAP_STD here based on ioctl handler provided by the
-		 * driver. If the driver doesn't support these
-		 * for a specific input, it must override these flags.
-		 */
-		if (ops->vidioc_s_std)
-			p->capabilities |= V4L2_IN_CAP_STD;
-		if (ops->vidioc_s_dv_preset)
-			p->capabilities |= V4L2_IN_CAP_PRESETS;
-		if (ops->vidioc_s_dv_timings)
-			p->capabilities |= V4L2_IN_CAP_CUSTOM_TIMINGS;
-
-		ret = ops->vidioc_enum_input(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "index=%d, name=%s, type=%d, "
-				"audioset=%d, "
-				"tuner=%d, std=%08Lx, status=%d\n",
-				p->index, p->name, p->type, p->audioset,
-				p->tuner,
-				(unsigned long long)p->std,
-				p->status);
-		break;
-	}
-	case VIDIOC_G_INPUT:
-	{
-		unsigned int *i = arg;
+	type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
+		V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
+	if (p->type != type)
+		return -EINVAL;
+	return ops->vidioc_s_hw_freq_seek(file, fh, p);
+}
 
-		ret = ops->vidioc_g_input(file, fh, i);
-		if (!ret)
-			dbgarg(cmd, "value=%d\n", *i);
-		break;
-	}
-	case VIDIOC_S_INPUT:
-	{
-		unsigned int *i = arg;
+static int v4l_reqbufs(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_requestbuffers *p = arg;
+	int ret = check_fmt(ops, p->type);
 
-		dbgarg(cmd, "value=%d\n", *i);
-		ret = ops->vidioc_s_input(file, fh, *i);
-		break;
-	}
+	if (ret)
+		return ret;
 
-	/* ------ output switching ---------- */
-	case VIDIOC_ENUMOUTPUT:
-	{
-		struct v4l2_output *p = arg;
+	if (p->type < V4L2_BUF_TYPE_PRIVATE)
+		CLEAR_AFTER_FIELD(p, memory);
 
-		/*
-		 * We set the flags for CAP_PRESETS, CAP_CUSTOM_TIMINGS &
-		 * CAP_STD here based on ioctl handler provided by the
-		 * driver. If the driver doesn't support these
-		 * for a specific output, it must override these flags.
-		 */
-		if (ops->vidioc_s_std)
-			p->capabilities |= V4L2_OUT_CAP_STD;
-		if (ops->vidioc_s_dv_preset)
-			p->capabilities |= V4L2_OUT_CAP_PRESETS;
-		if (ops->vidioc_s_dv_timings)
-			p->capabilities |= V4L2_OUT_CAP_CUSTOM_TIMINGS;
-
-		ret = ops->vidioc_enum_output(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "index=%d, name=%s, type=%d, "
-				"audioset=0x%x, "
-				"modulator=%d, std=0x%08Lx\n",
-				p->index, p->name, p->type, p->audioset,
-				p->modulator, (unsigned long long)p->std);
-		break;
-	}
-	case VIDIOC_G_OUTPUT:
-	{
-		unsigned int *i = arg;
+	return ops->vidioc_reqbufs(file, fh, p);
+}
 
-		ret = ops->vidioc_g_output(file, fh, i);
-		if (!ret)
-			dbgarg(cmd, "value=%d\n", *i);
-		break;
-	}
-	case VIDIOC_S_OUTPUT:
-	{
-		unsigned int *i = arg;
+static int v4l_querybuf(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_buffer *p = arg;
+	int ret = check_fmt(ops, p->type);
 
-		dbgarg(cmd, "value=%d\n", *i);
-		ret = ops->vidioc_s_output(file, fh, *i);
-		break;
-	}
+	return ret ? ret : ops->vidioc_querybuf(file, fh, p);
+}
 
-	/* --- controls ---------------------------------------------- */
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *p = arg;
-
-		if (vfh && vfh->ctrl_handler)
-			ret = v4l2_queryctrl(vfh->ctrl_handler, p);
-		else if (vfd->ctrl_handler)
-			ret = v4l2_queryctrl(vfd->ctrl_handler, p);
-		else if (ops->vidioc_queryctrl)
-			ret = ops->vidioc_queryctrl(file, fh, p);
-		else
-			break;
-		if (!ret)
-			dbgarg(cmd, "id=0x%x, type=%d, name=%s, min/max=%d/%d, "
-					"step=%d, default=%d, flags=0x%08x\n",
-					p->id, p->type, p->name,
-					p->minimum, p->maximum,
-					p->step, p->default_value, p->flags);
-		else
-			dbgarg(cmd, "id=0x%x\n", p->id);
-		break;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *p = arg;
-
-		if (vfh && vfh->ctrl_handler)
-			ret = v4l2_g_ctrl(vfh->ctrl_handler, p);
-		else if (vfd->ctrl_handler)
-			ret = v4l2_g_ctrl(vfd->ctrl_handler, p);
-		else if (ops->vidioc_g_ctrl)
-			ret = ops->vidioc_g_ctrl(file, fh, p);
-		else if (ops->vidioc_g_ext_ctrls) {
-			struct v4l2_ext_controls ctrls;
-			struct v4l2_ext_control ctrl;
-
-			ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(p->id);
-			ctrls.count = 1;
-			ctrls.controls = &ctrl;
-			ctrl.id = p->id;
-			ctrl.value = p->value;
-			if (check_ext_ctrls(&ctrls, 1)) {
-				ret = ops->vidioc_g_ext_ctrls(file, fh, &ctrls);
-				if (ret == 0)
-					p->value = ctrl.value;
-			}
-		} else
-			break;
-		if (!ret)
-			dbgarg(cmd, "id=0x%x, value=%d\n", p->id, p->value);
-		else
-			dbgarg(cmd, "id=0x%x\n", p->id);
-		break;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *p = arg;
-		struct v4l2_ext_controls ctrls;
-		struct v4l2_ext_control ctrl;
-
-		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
-			!ops->vidioc_s_ctrl && !ops->vidioc_s_ext_ctrls)
-			break;
+static int v4l_qbuf(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_buffer *p = arg;
+	int ret = check_fmt(ops, p->type);
 
-		dbgarg(cmd, "id=0x%x, value=%d\n", p->id, p->value);
+	return ret ? ret : ops->vidioc_qbuf(file, fh, p);
+}
 
-		if (vfh && vfh->ctrl_handler) {
-			ret = v4l2_s_ctrl(vfh, vfh->ctrl_handler, p);
-			break;
-		}
-		if (vfd->ctrl_handler) {
-			ret = v4l2_s_ctrl(NULL, vfd->ctrl_handler, p);
-			break;
-		}
-		if (ops->vidioc_s_ctrl) {
-			ret = ops->vidioc_s_ctrl(file, fh, p);
-			break;
-		}
-		if (!ops->vidioc_s_ext_ctrls)
-			break;
+static int v4l_dqbuf(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_buffer *p = arg;
+	int ret = check_fmt(ops, p->type);
 
-		ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(p->id);
-		ctrls.count = 1;
-		ctrls.controls = &ctrl;
-		ctrl.id = p->id;
-		ctrl.value = p->value;
-		if (check_ext_ctrls(&ctrls, 1))
-			ret = ops->vidioc_s_ext_ctrls(file, fh, &ctrls);
-		else
-			ret = -EINVAL;
-		break;
-	}
-	case VIDIOC_G_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *p = arg;
-
-		p->error_idx = p->count;
-		if (vfh && vfh->ctrl_handler)
-			ret = v4l2_g_ext_ctrls(vfh->ctrl_handler, p);
-		else if (vfd->ctrl_handler)
-			ret = v4l2_g_ext_ctrls(vfd->ctrl_handler, p);
-		else if (ops->vidioc_g_ext_ctrls)
-			ret = check_ext_ctrls(p, 0) ?
-				ops->vidioc_g_ext_ctrls(file, fh, p) :
-				-EINVAL;
-		else
-			break;
-		v4l_print_ext_ctrls(cmd, vfd, p, !ret);
-		break;
-	}
-	case VIDIOC_S_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *p = arg;
+	return ret ? ret : ops->vidioc_dqbuf(file, fh, p);
+}
 
-		p->error_idx = p->count;
-		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
-				!ops->vidioc_s_ext_ctrls)
-			break;
-		v4l_print_ext_ctrls(cmd, vfd, p, 1);
-		if (vfh && vfh->ctrl_handler)
-			ret = v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler, p);
-		else if (vfd->ctrl_handler)
-			ret = v4l2_s_ext_ctrls(NULL, vfd->ctrl_handler, p);
-		else if (check_ext_ctrls(p, 0))
-			ret = ops->vidioc_s_ext_ctrls(file, fh, p);
-		else
-			ret = -EINVAL;
-		break;
-	}
-	case VIDIOC_TRY_EXT_CTRLS:
-	{
-		struct v4l2_ext_controls *p = arg;
+static int v4l_create_bufs(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_create_buffers *create = arg;
+	int ret = check_fmt(ops, create->format.type);
 
-		p->error_idx = p->count;
-		if (!(vfh && vfh->ctrl_handler) && !vfd->ctrl_handler &&
-				!ops->vidioc_try_ext_ctrls)
-			break;
-		v4l_print_ext_ctrls(cmd, vfd, p, 1);
-		if (vfh && vfh->ctrl_handler)
-			ret = v4l2_try_ext_ctrls(vfh->ctrl_handler, p);
-		else if (vfd->ctrl_handler)
-			ret = v4l2_try_ext_ctrls(vfd->ctrl_handler, p);
-		else if (check_ext_ctrls(p, 0))
-			ret = ops->vidioc_try_ext_ctrls(file, fh, p);
-		else
-			ret = -EINVAL;
-		break;
-	}
-	case VIDIOC_QUERYMENU:
-	{
-		struct v4l2_querymenu *p = arg;
-
-		if (vfh && vfh->ctrl_handler)
-			ret = v4l2_querymenu(vfh->ctrl_handler, p);
-		else if (vfd->ctrl_handler)
-			ret = v4l2_querymenu(vfd->ctrl_handler, p);
-		else if (ops->vidioc_querymenu)
-			ret = ops->vidioc_querymenu(file, fh, p);
-		else
-			break;
-		if (!ret)
-			dbgarg(cmd, "id=0x%x, index=%d, name=%s\n",
-				p->id, p->index, p->name);
-		else
-			dbgarg(cmd, "id=0x%x, index=%d\n",
-				p->id, p->index);
-		break;
-	}
-	/* --- audio ---------------------------------------------- */
-	case VIDIOC_ENUMAUDIO:
-	{
-		struct v4l2_audio *p = arg;
-
-		ret = ops->vidioc_enumaudio(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
-					"mode=0x%x\n", p->index, p->name,
-					p->capability, p->mode);
-		else
-			dbgarg(cmd, "index=%d\n", p->index);
-		break;
-	}
-	case VIDIOC_G_AUDIO:
-	{
-		struct v4l2_audio *p = arg;
-
-		ret = ops->vidioc_g_audio(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
-					"mode=0x%x\n", p->index,
-					p->name, p->capability, p->mode);
-		else
-			dbgarg(cmd, "index=%d\n", p->index);
-		break;
-	}
-	case VIDIOC_S_AUDIO:
-	{
-		struct v4l2_audio *p = arg;
-
-		dbgarg(cmd, "index=%d, name=%s, capability=0x%x, "
-					"mode=0x%x\n", p->index, p->name,
-					p->capability, p->mode);
-		ret = ops->vidioc_s_audio(file, fh, p);
-		break;
-	}
-	case VIDIOC_ENUMAUDOUT:
-	{
-		struct v4l2_audioout *p = arg;
-
-		dbgarg(cmd, "Enum for index=%d\n", p->index);
-		ret = ops->vidioc_enumaudout(file, fh, p);
-		if (!ret)
-			dbgarg2("index=%d, name=%s, capability=%d, "
-					"mode=%d\n", p->index, p->name,
-					p->capability, p->mode);
-		break;
-	}
-	case VIDIOC_G_AUDOUT:
-	{
-		struct v4l2_audioout *p = arg;
-
-		ret = ops->vidioc_g_audout(file, fh, p);
-		if (!ret)
-			dbgarg2("index=%d, name=%s, capability=%d, "
-					"mode=%d\n", p->index, p->name,
-					p->capability, p->mode);
-		break;
-	}
-	case VIDIOC_S_AUDOUT:
-	{
-		struct v4l2_audioout *p = arg;
+	return ret ? ret : ops->vidioc_create_bufs(file, fh, create);
+}
 
-		dbgarg(cmd, "index=%d, name=%s, capability=%d, "
-					"mode=%d\n", p->index, p->name,
-					p->capability, p->mode);
+static int v4l_prepare_buf(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_buffer *b = arg;
+	int ret = check_fmt(ops, b->type);
 
-		ret = ops->vidioc_s_audout(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_MODULATOR:
-	{
-		struct v4l2_modulator *p = arg;
-
-		ret = ops->vidioc_g_modulator(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "index=%d, name=%s, "
-					"capability=%d, rangelow=%d,"
-					" rangehigh=%d, txsubchans=%d\n",
-					p->index, p->name, p->capability,
-					p->rangelow, p->rangehigh,
-					p->txsubchans);
-		break;
-	}
-	case VIDIOC_S_MODULATOR:
-	{
-		struct v4l2_modulator *p = arg;
-
-		dbgarg(cmd, "index=%d, name=%s, capability=%d, "
-				"rangelow=%d, rangehigh=%d, txsubchans=%d\n",
-				p->index, p->name, p->capability, p->rangelow,
-				p->rangehigh, p->txsubchans);
-			ret = ops->vidioc_s_modulator(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_CROP:
-	{
-		struct v4l2_crop *p = arg;
+	return ret ? ret : ops->vidioc_prepare_buf(file, fh, b);
+}
 
-		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
+static int v4l_g_parm(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_streamparm *p = arg;
+	v4l2_std_id std;
+	int ret = check_fmt(ops, p->type);
 
-		if (ops->vidioc_g_crop) {
-			ret = ops->vidioc_g_crop(file, fh, p);
-		} else {
-			/* simulate capture crop using selection api */
-			struct v4l2_selection s = {
-				.type = p->type,
-			};
-
-			/* crop means compose for output devices */
-			if (V4L2_TYPE_IS_OUTPUT(p->type))
-				s.target = V4L2_SEL_TGT_COMPOSE_ACTIVE;
-			else
-				s.target = V4L2_SEL_TGT_CROP_ACTIVE;
-
-			ret = ops->vidioc_g_selection(file, fh, &s);
-
-			/* copying results to old structure on success */
-			if (!ret)
-				p->c = s.r;
-		}
+	if (ret)
+		return ret;
+	if (ops->vidioc_g_parm)
+		return ops->vidioc_g_parm(file, fh, p);
+	std = vfd->current_norm;
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+	p->parm.capture.readbuffers = 2;
+	if (ops->vidioc_g_std)
+		ret = ops->vidioc_g_std(file, fh, &std);
+	if (ret == 0)
+		v4l2_video_std_frame_period(std,
+			    &p->parm.capture.timeperframe);
+	return ret;
+}
 
-		if (!ret)
-			dbgrect(vfd, "", &p->c);
-		break;
-	}
-	case VIDIOC_S_CROP:
-	{
-		struct v4l2_crop *p = arg;
+static int v4l_s_parm(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_streamparm *p = arg;
+	int ret = check_fmt(ops, p->type);
 
-		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
-		dbgrect(vfd, "", &p->c);
+	return ret ? ret : ops->vidioc_s_parm(file, fh, p);
+}
 
-		if (ops->vidioc_s_crop) {
-			ret = ops->vidioc_s_crop(file, fh, p);
-		} else {
-			/* simulate capture crop using selection api */
-			struct v4l2_selection s = {
-				.type = p->type,
-				.r = p->c,
-			};
-
-			/* crop means compose for output devices */
-			if (V4L2_TYPE_IS_OUTPUT(p->type))
-				s.target = V4L2_SEL_TGT_COMPOSE_ACTIVE;
-			else
-				s.target = V4L2_SEL_TGT_CROP_ACTIVE;
-
-			ret = ops->vidioc_s_selection(file, fh, &s);
-		}
-		break;
-	}
-	case VIDIOC_G_SELECTION:
-	{
-		struct v4l2_selection *p = arg;
+static int v4l_queryctrl(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_queryctrl *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_queryctrl(vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_queryctrl(vfd->ctrl_handler, p);
+	if (ops->vidioc_queryctrl)
+		return ops->vidioc_queryctrl(file, fh, p);
+	return -ENOTTY;
+}
 
-		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
+static int v4l_querymenu(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_querymenu *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_querymenu(vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_querymenu(vfd->ctrl_handler, p);
+	if (ops->vidioc_querymenu)
+		return ops->vidioc_querymenu(file, fh, p);
+	return -ENOTTY;
+}
 
-		ret = ops->vidioc_g_selection(file, fh, p);
-		if (!ret)
-			dbgrect(vfd, "", &p->r);
-		break;
+static int v4l_g_ctrl(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_control *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+	struct v4l2_ext_controls ctrls;
+	struct v4l2_ext_control ctrl;
+
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_g_ctrl(vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_g_ctrl(vfd->ctrl_handler, p);
+	if (ops->vidioc_g_ctrl)
+		return ops->vidioc_g_ctrl(file, fh, p);
+	if (ops->vidioc_g_ext_ctrls == NULL)
+		return -ENOTTY;
+
+	ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(p->id);
+	ctrls.count = 1;
+	ctrls.controls = &ctrl;
+	ctrl.id = p->id;
+	ctrl.value = p->value;
+	if (check_ext_ctrls(&ctrls, 1)) {
+		int ret = ops->vidioc_g_ext_ctrls(file, fh, &ctrls);
+
+		if (ret == 0)
+			p->value = ctrl.value;
+		return ret;
 	}
-	case VIDIOC_S_SELECTION:
-	{
-		struct v4l2_selection *p = arg;
+	return -EINVAL;
+}
 
+static int v4l_s_ctrl(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_control *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+	struct v4l2_ext_controls ctrls;
+	struct v4l2_ext_control ctrl;
+
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_s_ctrl(vfh, vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_s_ctrl(NULL, vfd->ctrl_handler, p);
+	if (ops->vidioc_s_ctrl)
+		return ops->vidioc_s_ctrl(file, fh, p);
+	if (ops->vidioc_s_ext_ctrls == NULL)
+		return -ENOTTY;
+
+	ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(p->id);
+	ctrls.count = 1;
+	ctrls.controls = &ctrl;
+	ctrl.id = p->id;
+	ctrl.value = p->value;
+	if (check_ext_ctrls(&ctrls, 1))
+		return ops->vidioc_s_ext_ctrls(file, fh, &ctrls);
+	return -EINVAL;
+}
 
-		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
-		dbgrect(vfd, "", &p->r);
+static int v4l_g_ext_ctrls(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_ext_controls *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+
+	p->error_idx = p->count;
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_g_ext_ctrls(vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_g_ext_ctrls(vfd->ctrl_handler, p);
+	if (ops->vidioc_g_ext_ctrls == NULL)
+		return -ENOTTY;
+	return check_ext_ctrls(p, 0) ? ops->vidioc_g_ext_ctrls(file, fh, p) :
+					-EINVAL;
+}
 
-		ret = ops->vidioc_s_selection(file, fh, p);
-		break;
-	}
-	case VIDIOC_CROPCAP:
-	{
-		struct v4l2_cropcap *p = arg;
-
-		/*FIXME: Should also show v4l2_fract pixelaspect */
-		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
-		if (ops->vidioc_cropcap) {
-			ret = ops->vidioc_cropcap(file, fh, p);
-		} else {
-			struct v4l2_selection s = { .type = p->type };
+static int v4l_s_ext_ctrls(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_ext_controls *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+
+	p->error_idx = p->count;
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_s_ext_ctrls(vfh, vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_s_ext_ctrls(NULL, vfd->ctrl_handler, p);
+	if (ops->vidioc_s_ext_ctrls == NULL)
+		return -ENOTTY;
+	return check_ext_ctrls(p, 0) ? ops->vidioc_s_ext_ctrls(file, fh, p) :
+					-EINVAL;
+}
 
-			/* obtaining bounds */
-			if (V4L2_TYPE_IS_OUTPUT(p->type))
-				s.target = V4L2_SEL_TGT_COMPOSE_BOUNDS;
-			else
-				s.target = V4L2_SEL_TGT_CROP_BOUNDS;
+static int v4l_try_ext_ctrls(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_ext_controls *p = arg;
+	struct v4l2_fh *vfh =
+		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+
+	p->error_idx = p->count;
+	if (vfh && vfh->ctrl_handler)
+		return v4l2_try_ext_ctrls(vfh->ctrl_handler, p);
+	if (vfd->ctrl_handler)
+		return v4l2_try_ext_ctrls(vfd->ctrl_handler, p);
+	if (ops->vidioc_try_ext_ctrls == NULL)
+		return -ENOTTY;
+	return check_ext_ctrls(p, 0) ? ops->vidioc_try_ext_ctrls(file, fh, p) :
+					-EINVAL;
+}
 
-			ret = ops->vidioc_g_selection(file, fh, &s);
-			if (ret)
-				break;
-			p->bounds = s.r;
+static int v4l_g_crop(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_crop *p = arg;
+	struct v4l2_selection s = {
+		.type = p->type,
+	};
+	int ret;
+
+	if (ops->vidioc_g_crop)
+		return ops->vidioc_g_crop(file, fh, p);
+	/* simulate capture crop using selection api */
+
+	/* crop means compose for output devices */
+	if (V4L2_TYPE_IS_OUTPUT(p->type))
+		s.target = V4L2_SEL_TGT_COMPOSE_ACTIVE;
+	else
+		s.target = V4L2_SEL_TGT_CROP_ACTIVE;
+
+	ret = ops->vidioc_g_selection(file, fh, &s);
+
+	/* copying results to old structure on success */
+	if (!ret)
+		p->c = s.r;
+	return ret;
+}
 
-			/* obtaining defrect */
-			if (V4L2_TYPE_IS_OUTPUT(p->type))
-				s.target = V4L2_SEL_TGT_COMPOSE_DEFAULT;
-			else
-				s.target = V4L2_SEL_TGT_CROP_DEFAULT;
+static int v4l_s_crop(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_crop *p = arg;
+	struct v4l2_selection s = {
+		.type = p->type,
+		.r = p->c,
+	};
+
+	if (ops->vidioc_s_crop)
+		return ops->vidioc_s_crop(file, fh, p);
+	/* simulate capture crop using selection api */
+
+	/* crop means compose for output devices */
+	if (V4L2_TYPE_IS_OUTPUT(p->type))
+		s.target = V4L2_SEL_TGT_COMPOSE_ACTIVE;
+	else
+		s.target = V4L2_SEL_TGT_CROP_ACTIVE;
+
+	return ops->vidioc_s_selection(file, fh, &s);
+}
 
-			ret = ops->vidioc_g_selection(file, fh, &s);
-			if (ret)
-				break;
-			p->defrect = s.r;
+static int v4l_cropcap(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_cropcap *p = arg;
+	struct v4l2_selection s = { .type = p->type };
+	int ret;
 
-			/* setting trivial pixelaspect */
-			p->pixelaspect.numerator = 1;
-			p->pixelaspect.denominator = 1;
-		}
+	if (ops->vidioc_cropcap)
+		return ops->vidioc_cropcap(file, fh, p);
 
-		if (!ret) {
-			dbgrect(vfd, "bounds ", &p->bounds);
-			dbgrect(vfd, "defrect ", &p->defrect);
-		}
-		break;
-	}
-	case VIDIOC_G_JPEGCOMP:
-	{
-		struct v4l2_jpegcompression *p = arg;
-
-		ret = ops->vidioc_g_jpegcomp(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "quality=%d, APPn=%d, "
-					"APP_len=%d, COM_len=%d, "
-					"jpeg_markers=%d\n",
-					p->quality, p->APPn, p->APP_len,
-					p->COM_len, p->jpeg_markers);
-		break;
-	}
-	case VIDIOC_S_JPEGCOMP:
-	{
-		struct v4l2_jpegcompression *p = arg;
-
-		dbgarg(cmd, "quality=%d, APPn=%d, APP_len=%d, "
-					"COM_len=%d, jpeg_markers=%d\n",
-					p->quality, p->APPn, p->APP_len,
-					p->COM_len, p->jpeg_markers);
-		ret = ops->vidioc_s_jpegcomp(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_ENC_INDEX:
-	{
-		struct v4l2_enc_idx *p = arg;
-
-		ret = ops->vidioc_g_enc_index(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "entries=%d, entries_cap=%d\n",
-					p->entries, p->entries_cap);
-		break;
-	}
-	case VIDIOC_ENCODER_CMD:
-	{
-		struct v4l2_encoder_cmd *p = arg;
+	/* obtaining bounds */
+	if (V4L2_TYPE_IS_OUTPUT(p->type))
+		s.target = V4L2_SEL_TGT_COMPOSE_BOUNDS;
+	else
+		s.target = V4L2_SEL_TGT_CROP_BOUNDS;
 
-		ret = ops->vidioc_encoder_cmd(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
-		break;
-	}
-	case VIDIOC_TRY_ENCODER_CMD:
-	{
-		struct v4l2_encoder_cmd *p = arg;
+	ret = ops->vidioc_g_selection(file, fh, &s);
+	if (ret)
+		return ret;
+	p->bounds = s.r;
 
-		ret = ops->vidioc_try_encoder_cmd(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
-		break;
-	}
-	case VIDIOC_DECODER_CMD:
-	{
-		struct v4l2_decoder_cmd *p = arg;
+	/* obtaining defrect */
+	if (V4L2_TYPE_IS_OUTPUT(p->type))
+		s.target = V4L2_SEL_TGT_COMPOSE_DEFAULT;
+	else
+		s.target = V4L2_SEL_TGT_CROP_DEFAULT;
 
-		ret = ops->vidioc_decoder_cmd(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
-		break;
-	}
-	case VIDIOC_TRY_DECODER_CMD:
-	{
-		struct v4l2_decoder_cmd *p = arg;
+	ret = ops->vidioc_g_selection(file, fh, &s);
+	if (ret)
+		return ret;
+	p->defrect = s.r;
 
-		ret = ops->vidioc_try_decoder_cmd(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "cmd=%d, flags=%x\n", p->cmd, p->flags);
-		break;
-	}
-	case VIDIOC_G_PARM:
-	{
-		struct v4l2_streamparm *p = arg;
+	/* setting trivial pixelaspect */
+	p->pixelaspect.numerator = 1;
+	p->pixelaspect.denominator = 1;
+	return 0;
+}
 
-		if (ops->vidioc_g_parm) {
-			ret = check_fmt(ops, p->type);
-			if (ret)
-				break;
-			ret = ops->vidioc_g_parm(file, fh, p);
-		} else {
-			v4l2_std_id std = vfd->current_norm;
+static int v4l_log_status(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	int ret;
+
+	if (vfd->v4l2_dev)
+		pr_info("%s: =================  START STATUS  =================\n",
+			vfd->v4l2_dev->name);
+	ret = ops->vidioc_log_status(file, fh);
+	if (vfd->v4l2_dev)
+		pr_info("%s: ==================  END STATUS  ==================\n",
+			vfd->v4l2_dev->name);
+	return ret;
+}
 
-			ret = -EINVAL;
-			if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-				break;
+static int v4l_dbg_g_register(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	struct v4l2_dbg_register *p = arg;
 
-			ret = 0;
-			p->parm.capture.readbuffers = 2;
-			if (ops->vidioc_g_std)
-				ret = ops->vidioc_g_std(file, fh, &std);
-			if (ret == 0)
-				v4l2_video_std_frame_period(std,
-						    &p->parm.capture.timeperframe);
-		}
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	return ops->vidioc_g_register(file, fh, p);
+#else
+	return -ENOTTY;
+#endif
+}
 
-		dbgarg(cmd, "type=%d\n", p->type);
-		break;
-	}
-	case VIDIOC_S_PARM:
-	{
-		struct v4l2_streamparm *p = arg;
+static int v4l_dbg_s_register(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	struct v4l2_dbg_register *p = arg;
 
-		ret = check_fmt(ops, p->type);
-		if (ret)
-			break;
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+	return ops->vidioc_s_register(file, fh, p);
+#else
+	return -ENOTTY;
+#endif
+}
 
-		dbgarg(cmd, "type=%d\n", p->type);
-		ret = ops->vidioc_s_parm(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *p = arg;
+static int v4l_dbg_g_chip_ident(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_dbg_chip_ident *p = arg;
 
-		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
-			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-		ret = ops->vidioc_g_tuner(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "index=%d, name=%s, type=%d, "
-					"capability=0x%x, rangelow=%d, "
-					"rangehigh=%d, signal=%d, afc=%d, "
-					"rxsubchans=0x%x, audmode=%d\n",
-					p->index, p->name, p->type,
-					p->capability, p->rangelow,
-					p->rangehigh, p->signal, p->afc,
-					p->rxsubchans, p->audmode);
-		break;
-	}
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *p = arg;
+	p->ident = V4L2_IDENT_NONE;
+	p->revision = 0;
+	return ops->vidioc_g_chip_ident(file, fh, p);
+}
 
-		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
-			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-		dbgarg(cmd, "index=%d, name=%s, type=%d, "
-				"capability=0x%x, rangelow=%d, "
-				"rangehigh=%d, signal=%d, afc=%d, "
-				"rxsubchans=0x%x, audmode=%d\n",
-				p->index, p->name, p->type,
-				p->capability, p->rangelow,
-				p->rangehigh, p->signal, p->afc,
-				p->rxsubchans, p->audmode);
-		ret = ops->vidioc_s_tuner(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *p = arg;
+static int v4l_dqevent(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return v4l2_event_dequeue(fh, arg, file->f_flags & O_NONBLOCK);
+}
 
-		p->type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
-			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-		ret = ops->vidioc_g_frequency(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "tuner=%d, type=%d, frequency=%d\n",
-					p->tuner, p->type, p->frequency);
-		break;
-	}
-	case VIDIOC_S_FREQUENCY:
-	{
-		struct v4l2_frequency *p = arg;
-		enum v4l2_tuner_type type;
+static int v4l_subscribe_event(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return ops->vidioc_subscribe_event(fh, arg);
+}
 
-		type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
-			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-		dbgarg(cmd, "tuner=%d, type=%d, frequency=%d\n",
-				p->tuner, p->type, p->frequency);
-		if (p->type != type)
-			ret = -EINVAL;
-		else
-			ret = ops->vidioc_s_frequency(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_SLICED_VBI_CAP:
-	{
-		struct v4l2_sliced_vbi_cap *p = arg;
+static int v4l_unsubscribe_event(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	return ops->vidioc_unsubscribe_event(fh, arg);
+}
 
-		/* Clear up to type, everything after type is zerod already */
-		memset(p, 0, offsetof(struct v4l2_sliced_vbi_cap, type));
+static int v4l_g_sliced_vbi_cap(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct v4l2_sliced_vbi_cap *p = arg;
 
-		dbgarg(cmd, "type=%s\n", prt_names(p->type, v4l2_type_names));
-		ret = ops->vidioc_g_sliced_vbi_cap(file, fh, p);
-		if (!ret)
-			dbgarg2("service_set=%d\n", p->service_set);
-		break;
-	}
-	case VIDIOC_LOG_STATUS:
-	{
-		if (vfd->v4l2_dev)
-			pr_info("%s: =================  START STATUS  =================\n",
-				vfd->v4l2_dev->name);
-		ret = ops->vidioc_log_status(file, fh);
-		if (vfd->v4l2_dev)
-			pr_info("%s: ==================  END STATUS  ==================\n",
-				vfd->v4l2_dev->name);
-		break;
-	}
-	case VIDIOC_DBG_G_REGISTER:
-	{
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-		struct v4l2_dbg_register *p = arg;
+	/* Clear up to type, everything after type is zeroed already */
+	memset(p, 0, offsetof(struct v4l2_sliced_vbi_cap, type));
 
-		if (!capable(CAP_SYS_ADMIN))
-			ret = -EPERM;
-		else
-			ret = ops->vidioc_g_register(file, fh, p);
-#endif
-		break;
-	}
-	case VIDIOC_DBG_S_REGISTER:
-	{
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-		struct v4l2_dbg_register *p = arg;
+	return ops->vidioc_g_sliced_vbi_cap(file, fh, p);
+}
 
-		if (!capable(CAP_SYS_ADMIN))
-			ret = -EPERM;
-		else
-			ret = ops->vidioc_s_register(file, fh, p);
-#endif
-		break;
-	}
-	case VIDIOC_DBG_G_CHIP_IDENT:
-	{
-		struct v4l2_dbg_chip_ident *p = arg;
-
-		p->ident = V4L2_IDENT_NONE;
-		p->revision = 0;
-		ret = ops->vidioc_g_chip_ident(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "chip_ident=%u, revision=0x%x\n", p->ident, p->revision);
-		break;
-	}
-	case VIDIOC_S_HW_FREQ_SEEK:
-	{
-		struct v4l2_hw_freq_seek *p = arg;
-		enum v4l2_tuner_type type;
+static int v4l_enum_freq_bands(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *arg)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct v4l2_frequency_band *p = arg;
+	enum v4l2_tuner_type type;
+	int err;
 
-		type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
+	type = (vfd->vfl_type == VFL_TYPE_RADIO) ?
 			V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
-		dbgarg(cmd,
-			"tuner=%u, type=%u, seek_upward=%u, wrap_around=%u, spacing=%u\n",
-			p->tuner, p->type, p->seek_upward, p->wrap_around, p->spacing);
-		if (p->type != type)
-			ret = -EINVAL;
-		else
-			ret = ops->vidioc_s_hw_freq_seek(file, fh, p);
-		break;
-	}
-	case VIDIOC_ENUM_FRAMESIZES:
-	{
-		struct v4l2_frmsizeenum *p = arg;
 
-		ret = ops->vidioc_enum_framesizes(file, fh, p);
-		dbgarg(cmd,
-			"index=%d, pixelformat=%c%c%c%c, type=%d ",
-			p->index,
-			(p->pixel_format & 0xff),
-			(p->pixel_format >>  8) & 0xff,
-			(p->pixel_format >> 16) & 0xff,
-			(p->pixel_format >> 24) & 0xff,
-			p->type);
-		switch (p->type) {
-		case V4L2_FRMSIZE_TYPE_DISCRETE:
-			dbgarg3("width = %d, height=%d\n",
-				p->discrete.width, p->discrete.height);
-			break;
-		case V4L2_FRMSIZE_TYPE_STEPWISE:
-			dbgarg3("min %dx%d, max %dx%d, step %dx%d\n",
-				p->stepwise.min_width,  p->stepwise.min_height,
-				p->stepwise.step_width, p->stepwise.step_height,
-				p->stepwise.max_width,  p->stepwise.max_height);
-			break;
-		case V4L2_FRMSIZE_TYPE_CONTINUOUS:
-			dbgarg3("continuous\n");
-			break;
-		default:
-			dbgarg3("- Unknown type!\n");
-		}
-
-		break;
-	}
-	case VIDIOC_ENUM_FRAMEINTERVALS:
-	{
-		struct v4l2_frmivalenum *p = arg;
-
-		ret = ops->vidioc_enum_frameintervals(file, fh, p);
-		dbgarg(cmd,
-			"index=%d, pixelformat=%d, width=%d, height=%d, type=%d ",
-			p->index, p->pixel_format,
-			p->width, p->height, p->type);
-		switch (p->type) {
-		case V4L2_FRMIVAL_TYPE_DISCRETE:
-			dbgarg2("fps=%d/%d\n",
-				p->discrete.numerator,
-				p->discrete.denominator);
-			break;
-		case V4L2_FRMIVAL_TYPE_STEPWISE:
-			dbgarg2("min=%d/%d, max=%d/%d, step=%d/%d\n",
-				p->stepwise.min.numerator,
-				p->stepwise.min.denominator,
-				p->stepwise.max.numerator,
-				p->stepwise.max.denominator,
-				p->stepwise.step.numerator,
-				p->stepwise.step.denominator);
-			break;
-		case V4L2_FRMIVAL_TYPE_CONTINUOUS:
-			dbgarg2("continuous\n");
-			break;
-		default:
-			dbgarg2("- Unknown type!\n");
-		}
-		break;
+	if (type != p->type)
+		return -EINVAL;
+	if (ops->vidioc_enum_freq_bands)
+		return ops->vidioc_enum_freq_bands(file, fh, p);
+	if (ops->vidioc_g_tuner) {
+		struct v4l2_tuner t = {
+			.index = p->tuner,
+			.type = type,
+		};
+
+		err = ops->vidioc_g_tuner(file, fh, &t);
+		if (err)
+			return err;
+		p->capability = t.capability | V4L2_TUNER_CAP_FREQ_BANDS;
+		p->rangelow = t.rangelow;
+		p->rangehigh = t.rangehigh;
+		p->modulation = (type == V4L2_TUNER_RADIO) ?
+			V4L2_BAND_MODULATION_FM : V4L2_BAND_MODULATION_VSB;
+		return 0;
 	}
-	case VIDIOC_ENUM_DV_PRESETS:
-	{
-		struct v4l2_dv_enum_preset *p = arg;
-
-		ret = ops->vidioc_enum_dv_presets(file, fh, p);
-		if (!ret)
-			dbgarg(cmd,
-				"index=%d, preset=%d, name=%s, width=%d,"
-				" height=%d ",
-				p->index, p->preset, p->name, p->width,
-				p->height);
-		break;
+	if (ops->vidioc_g_modulator) {
+		struct v4l2_modulator m = {
+			.index = p->tuner,
+		};
+
+		if (type != V4L2_TUNER_RADIO)
+			return -EINVAL;
+		err = ops->vidioc_g_modulator(file, fh, &m);
+		if (err)
+			return err;
+		p->capability = m.capability | V4L2_TUNER_CAP_FREQ_BANDS;
+		p->rangelow = m.rangelow;
+		p->rangehigh = m.rangehigh;
+		p->modulation = (type == V4L2_TUNER_RADIO) ?
+			V4L2_BAND_MODULATION_FM : V4L2_BAND_MODULATION_VSB;
+		return 0;
 	}
-	case VIDIOC_S_DV_PRESET:
-	{
-		struct v4l2_dv_preset *p = arg;
+	return -ENOTTY;
+}
 
-		dbgarg(cmd, "preset=%d\n", p->preset);
-		ret = ops->vidioc_s_dv_preset(file, fh, p);
-		break;
-	}
-	case VIDIOC_G_DV_PRESET:
-	{
-		struct v4l2_dv_preset *p = arg;
+struct v4l2_ioctl_info {
+	unsigned int ioctl;
+	u32 flags;
+	const char * const name;
+	union {
+		u32 offset;
+		int (*func)(const struct v4l2_ioctl_ops *ops,
+				struct file *file, void *fh, void *p);
+	} u;
+	void (*debug)(const void *arg, bool write_only);
+};
 
-		ret = ops->vidioc_g_dv_preset(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "preset=%d\n", p->preset);
-		break;
+/* This control needs a priority check */
+#define INFO_FL_PRIO	(1 << 0)
+/* This control can be valid if the filehandle passes a control handler. */
+#define INFO_FL_CTRL	(1 << 1)
+/* This is a standard ioctl, no need for special code */
+#define INFO_FL_STD	(1 << 2)
+/* This is ioctl has its own function */
+#define INFO_FL_FUNC	(1 << 3)
+/* Queuing ioctl */
+#define INFO_FL_QUEUE	(1 << 4)
+/* Zero struct from after the field to the end */
+#define INFO_FL_CLEAR(v4l2_struct, field)			\
+	((offsetof(struct v4l2_struct, field) +			\
+	  sizeof(((struct v4l2_struct *)0)->field)) << 16)
+#define INFO_FL_CLEAR_MASK (_IOC_SIZEMASK << 16)
+
+#define IOCTL_INFO_STD(_ioctl, _vidioc, _debug, _flags)			\
+	[_IOC_NR(_ioctl)] = {						\
+		.ioctl = _ioctl,					\
+		.flags = _flags | INFO_FL_STD,				\
+		.name = #_ioctl,					\
+		.u.offset = offsetof(struct v4l2_ioctl_ops, _vidioc),	\
+		.debug = _debug,					\
+	}
+
+#define IOCTL_INFO_FNC(_ioctl, _func, _debug, _flags)			\
+	[_IOC_NR(_ioctl)] = {						\
+		.ioctl = _ioctl,					\
+		.flags = _flags | INFO_FL_FUNC,				\
+		.name = #_ioctl,					\
+		.u.func = _func,					\
+		.debug = _debug,					\
 	}
-	case VIDIOC_QUERY_DV_PRESET:
-	{
-		struct v4l2_dv_preset *p = arg;
 
-		ret = ops->vidioc_query_dv_preset(file, fh, p);
-		if (!ret)
-			dbgarg(cmd, "preset=%d\n", p->preset);
-		break;
-	}
-	case VIDIOC_S_DV_TIMINGS:
-	{
-		struct v4l2_dv_timings *p = arg;
-
-		dbgtimings(vfd, p);
-		switch (p->type) {
-		case V4L2_DV_BT_656_1120:
-			ret = ops->vidioc_s_dv_timings(file, fh, p);
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-		break;
-	}
-	case VIDIOC_G_DV_TIMINGS:
-	{
-		struct v4l2_dv_timings *p = arg;
+static struct v4l2_ioctl_info v4l2_ioctls[] = {
+	IOCTL_INFO_FNC(VIDIOC_QUERYCAP, v4l_querycap, v4l_print_querycap, 0),
+	IOCTL_INFO_FNC(VIDIOC_ENUM_FMT, v4l_enum_fmt, v4l_print_fmtdesc, INFO_FL_CLEAR(v4l2_fmtdesc, type)),
+	IOCTL_INFO_FNC(VIDIOC_G_FMT, v4l_g_fmt, v4l_print_format, INFO_FL_CLEAR(v4l2_format, type)),
+	IOCTL_INFO_FNC(VIDIOC_S_FMT, v4l_s_fmt, v4l_print_format, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_REQBUFS, v4l_reqbufs, v4l_print_requestbuffers, INFO_FL_PRIO | INFO_FL_QUEUE),
+	IOCTL_INFO_FNC(VIDIOC_QUERYBUF, v4l_querybuf, v4l_print_buffer, INFO_FL_QUEUE | INFO_FL_CLEAR(v4l2_buffer, length)),
+	IOCTL_INFO_STD(VIDIOC_G_FBUF, vidioc_g_fbuf, v4l_print_framebuffer, 0),
+	IOCTL_INFO_STD(VIDIOC_S_FBUF, vidioc_s_fbuf, v4l_print_framebuffer, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_OVERLAY, vidioc_overlay, v4l_print_u32, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_QBUF, v4l_qbuf, v4l_print_buffer, INFO_FL_QUEUE),
+	IOCTL_INFO_FNC(VIDIOC_DQBUF, v4l_dqbuf, v4l_print_buffer, INFO_FL_QUEUE),
+	IOCTL_INFO_FNC(VIDIOC_STREAMON, v4l_streamon, v4l_print_buftype, INFO_FL_PRIO | INFO_FL_QUEUE),
+	IOCTL_INFO_FNC(VIDIOC_STREAMOFF, v4l_streamoff, v4l_print_buftype, INFO_FL_PRIO | INFO_FL_QUEUE),
+	IOCTL_INFO_FNC(VIDIOC_G_PARM, v4l_g_parm, v4l_print_streamparm, INFO_FL_CLEAR(v4l2_streamparm, type)),
+	IOCTL_INFO_FNC(VIDIOC_S_PARM, v4l_s_parm, v4l_print_streamparm, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_G_STD, v4l_g_std, v4l_print_std, 0),
+	IOCTL_INFO_FNC(VIDIOC_S_STD, v4l_s_std, v4l_print_std, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_ENUMSTD, v4l_enumstd, v4l_print_standard, INFO_FL_CLEAR(v4l2_standard, index)),
+	IOCTL_INFO_FNC(VIDIOC_ENUMINPUT, v4l_enuminput, v4l_print_enuminput, INFO_FL_CLEAR(v4l2_input, index)),
+	IOCTL_INFO_FNC(VIDIOC_G_CTRL, v4l_g_ctrl, v4l_print_control, INFO_FL_CTRL | INFO_FL_CLEAR(v4l2_control, id)),
+	IOCTL_INFO_FNC(VIDIOC_S_CTRL, v4l_s_ctrl, v4l_print_control, INFO_FL_PRIO | INFO_FL_CTRL),
+	IOCTL_INFO_FNC(VIDIOC_G_TUNER, v4l_g_tuner, v4l_print_tuner, INFO_FL_CLEAR(v4l2_tuner, index)),
+	IOCTL_INFO_FNC(VIDIOC_S_TUNER, v4l_s_tuner, v4l_print_tuner, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_AUDIO, vidioc_g_audio, v4l_print_audio, 0),
+	IOCTL_INFO_STD(VIDIOC_S_AUDIO, vidioc_s_audio, v4l_print_audio, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_QUERYCTRL, v4l_queryctrl, v4l_print_queryctrl, INFO_FL_CTRL | INFO_FL_CLEAR(v4l2_queryctrl, id)),
+	IOCTL_INFO_FNC(VIDIOC_QUERYMENU, v4l_querymenu, v4l_print_querymenu, INFO_FL_CTRL | INFO_FL_CLEAR(v4l2_querymenu, index)),
+	IOCTL_INFO_STD(VIDIOC_G_INPUT, vidioc_g_input, v4l_print_u32, 0),
+	IOCTL_INFO_FNC(VIDIOC_S_INPUT, v4l_s_input, v4l_print_u32, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_OUTPUT, vidioc_g_output, v4l_print_u32, 0),
+	IOCTL_INFO_FNC(VIDIOC_S_OUTPUT, v4l_s_output, v4l_print_u32, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_ENUMOUTPUT, v4l_enumoutput, v4l_print_enumoutput, INFO_FL_CLEAR(v4l2_output, index)),
+	IOCTL_INFO_STD(VIDIOC_G_AUDOUT, vidioc_g_audout, v4l_print_audioout, 0),
+	IOCTL_INFO_STD(VIDIOC_S_AUDOUT, vidioc_s_audout, v4l_print_audioout, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_G_MODULATOR, v4l_g_modulator, v4l_print_modulator, INFO_FL_CLEAR(v4l2_modulator, index)),
+	IOCTL_INFO_STD(VIDIOC_S_MODULATOR, vidioc_s_modulator, v4l_print_modulator, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_G_FREQUENCY, v4l_g_frequency, v4l_print_frequency, INFO_FL_CLEAR(v4l2_frequency, tuner)),
+	IOCTL_INFO_FNC(VIDIOC_S_FREQUENCY, v4l_s_frequency, v4l_print_frequency, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_CROPCAP, v4l_cropcap, v4l_print_cropcap, INFO_FL_CLEAR(v4l2_cropcap, type)),
+	IOCTL_INFO_FNC(VIDIOC_G_CROP, v4l_g_crop, v4l_print_crop, INFO_FL_CLEAR(v4l2_crop, type)),
+	IOCTL_INFO_FNC(VIDIOC_S_CROP, v4l_s_crop, v4l_print_crop, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_SELECTION, vidioc_g_selection, v4l_print_selection, 0),
+	IOCTL_INFO_STD(VIDIOC_S_SELECTION, vidioc_s_selection, v4l_print_selection, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_JPEGCOMP, vidioc_g_jpegcomp, v4l_print_jpegcompression, 0),
+	IOCTL_INFO_STD(VIDIOC_S_JPEGCOMP, vidioc_s_jpegcomp, v4l_print_jpegcompression, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_QUERYSTD, v4l_querystd, v4l_print_std, 0),
+	IOCTL_INFO_FNC(VIDIOC_TRY_FMT, v4l_try_fmt, v4l_print_format, 0),
+	IOCTL_INFO_STD(VIDIOC_ENUMAUDIO, vidioc_enumaudio, v4l_print_audio, INFO_FL_CLEAR(v4l2_audio, index)),
+	IOCTL_INFO_STD(VIDIOC_ENUMAUDOUT, vidioc_enumaudout, v4l_print_audioout, INFO_FL_CLEAR(v4l2_audioout, index)),
+	IOCTL_INFO_FNC(VIDIOC_G_PRIORITY, v4l_g_priority, v4l_print_u32, 0),
+	IOCTL_INFO_FNC(VIDIOC_S_PRIORITY, v4l_s_priority, v4l_print_u32, INFO_FL_PRIO),
+	IOCTL_INFO_FNC(VIDIOC_G_SLICED_VBI_CAP, v4l_g_sliced_vbi_cap, v4l_print_sliced_vbi_cap, INFO_FL_CLEAR(v4l2_sliced_vbi_cap, type)),
+	IOCTL_INFO_FNC(VIDIOC_LOG_STATUS, v4l_log_status, v4l_print_newline, 0),
+	IOCTL_INFO_FNC(VIDIOC_G_EXT_CTRLS, v4l_g_ext_ctrls, v4l_print_ext_controls, INFO_FL_CTRL),
+	IOCTL_INFO_FNC(VIDIOC_S_EXT_CTRLS, v4l_s_ext_ctrls, v4l_print_ext_controls, INFO_FL_PRIO | INFO_FL_CTRL),
+	IOCTL_INFO_FNC(VIDIOC_TRY_EXT_CTRLS, v4l_try_ext_ctrls, v4l_print_ext_controls, INFO_FL_CTRL),
+	IOCTL_INFO_STD(VIDIOC_ENUM_FRAMESIZES, vidioc_enum_framesizes, v4l_print_frmsizeenum, INFO_FL_CLEAR(v4l2_frmsizeenum, pixel_format)),
+	IOCTL_INFO_STD(VIDIOC_ENUM_FRAMEINTERVALS, vidioc_enum_frameintervals, v4l_print_frmivalenum, INFO_FL_CLEAR(v4l2_frmivalenum, height)),
+	IOCTL_INFO_STD(VIDIOC_G_ENC_INDEX, vidioc_g_enc_index, v4l_print_enc_idx, 0),
+	IOCTL_INFO_STD(VIDIOC_ENCODER_CMD, vidioc_encoder_cmd, v4l_print_encoder_cmd, INFO_FL_PRIO | INFO_FL_CLEAR(v4l2_encoder_cmd, flags)),
+	IOCTL_INFO_STD(VIDIOC_TRY_ENCODER_CMD, vidioc_try_encoder_cmd, v4l_print_encoder_cmd, INFO_FL_CLEAR(v4l2_encoder_cmd, flags)),
+	IOCTL_INFO_STD(VIDIOC_DECODER_CMD, vidioc_decoder_cmd, v4l_print_decoder_cmd, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_TRY_DECODER_CMD, vidioc_try_decoder_cmd, v4l_print_decoder_cmd, 0),
+	IOCTL_INFO_FNC(VIDIOC_DBG_S_REGISTER, v4l_dbg_s_register, v4l_print_dbg_register, 0),
+	IOCTL_INFO_FNC(VIDIOC_DBG_G_REGISTER, v4l_dbg_g_register, v4l_print_dbg_register, 0),
+	IOCTL_INFO_FNC(VIDIOC_DBG_G_CHIP_IDENT, v4l_dbg_g_chip_ident, v4l_print_dbg_chip_ident, 0),
+	IOCTL_INFO_FNC(VIDIOC_S_HW_FREQ_SEEK, v4l_s_hw_freq_seek, v4l_print_hw_freq_seek, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_ENUM_DV_PRESETS, vidioc_enum_dv_presets, v4l_print_dv_enum_presets, 0),
+	IOCTL_INFO_STD(VIDIOC_S_DV_PRESET, vidioc_s_dv_preset, v4l_print_dv_preset, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_DV_PRESET, vidioc_g_dv_preset, v4l_print_dv_preset, 0),
+	IOCTL_INFO_STD(VIDIOC_QUERY_DV_PRESET, vidioc_query_dv_preset, v4l_print_dv_preset, 0),
+	IOCTL_INFO_STD(VIDIOC_S_DV_TIMINGS, vidioc_s_dv_timings, v4l_print_dv_timings, INFO_FL_PRIO),
+	IOCTL_INFO_STD(VIDIOC_G_DV_TIMINGS, vidioc_g_dv_timings, v4l_print_dv_timings, 0),
+	IOCTL_INFO_FNC(VIDIOC_DQEVENT, v4l_dqevent, v4l_print_event, 0),
+	IOCTL_INFO_FNC(VIDIOC_SUBSCRIBE_EVENT, v4l_subscribe_event, v4l_print_event_subscription, 0),
+	IOCTL_INFO_FNC(VIDIOC_UNSUBSCRIBE_EVENT, v4l_unsubscribe_event, v4l_print_event_subscription, 0),
+	IOCTL_INFO_FNC(VIDIOC_CREATE_BUFS, v4l_create_bufs, v4l_print_create_buffers, INFO_FL_PRIO | INFO_FL_QUEUE),
+	IOCTL_INFO_FNC(VIDIOC_PREPARE_BUF, v4l_prepare_buf, v4l_print_buffer, INFO_FL_QUEUE),
+	IOCTL_INFO_STD(VIDIOC_ENUM_DV_TIMINGS, vidioc_enum_dv_timings, v4l_print_enum_dv_timings, 0),
+	IOCTL_INFO_STD(VIDIOC_QUERY_DV_TIMINGS, vidioc_query_dv_timings, v4l_print_dv_timings, 0),
+	IOCTL_INFO_STD(VIDIOC_DV_TIMINGS_CAP, vidioc_dv_timings_cap, v4l_print_dv_timings_cap, INFO_FL_CLEAR(v4l2_dv_timings_cap, type)),
+	IOCTL_INFO_FNC(VIDIOC_ENUM_FREQ_BANDS, v4l_enum_freq_bands, v4l_print_freq_band, 0),
+};
+#define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
-		ret = ops->vidioc_g_dv_timings(file, fh, p);
-		if (!ret)
-			dbgtimings(vfd, p);
-		break;
-	}
-	case VIDIOC_ENUM_DV_TIMINGS:
-	{
-		struct v4l2_enum_dv_timings *p = arg;
+bool v4l2_is_known_ioctl(unsigned int cmd)
+{
+	if (_IOC_NR(cmd) >= V4L2_IOCTLS)
+		return false;
+	return v4l2_ioctls[_IOC_NR(cmd)].ioctl == cmd;
+}
 
-		if (!ops->vidioc_enum_dv_timings)
-			break;
+struct mutex *v4l2_ioctl_get_lock(struct video_device *vdev, unsigned cmd)
+{
+	if (_IOC_NR(cmd) >= V4L2_IOCTLS)
+		return vdev->lock;
+	if (test_bit(_IOC_NR(cmd), vdev->disable_locking))
+		return NULL;
+	if (vdev->queue && vdev->queue->lock &&
+			(v4l2_ioctls[_IOC_NR(cmd)].flags & INFO_FL_QUEUE))
+		return vdev->queue->lock;
+	return vdev->lock;
+}
 
-		ret = ops->vidioc_enum_dv_timings(file, fh, p);
-		if (!ret) {
-			dbgarg(cmd, "index=%d: ", p->index);
-			dbgtimings(vfd, &p->timings);
-		}
-		break;
-	}
-	case VIDIOC_QUERY_DV_TIMINGS:
-	{
-		struct v4l2_dv_timings *p = arg;
+/* Common ioctl debug function. This function can be used by
+   external ioctl messages as well as internal V4L ioctl */
+void v4l_printk_ioctl(const char *prefix, unsigned int cmd)
+{
+	const char *dir, *type;
 
-		if (!ops->vidioc_query_dv_timings)
-			break;
+	if (prefix)
+		printk(KERN_DEBUG "%s: ", prefix);
 
-		ret = ops->vidioc_query_dv_timings(file, fh, p);
-		if (!ret)
-			dbgtimings(vfd, p);
+	switch (_IOC_TYPE(cmd)) {
+	case 'd':
+		type = "v4l2_int";
 		break;
-	}
-	case VIDIOC_DV_TIMINGS_CAP:
-	{
-		struct v4l2_dv_timings_cap *p = arg;
-
-		if (!ops->vidioc_dv_timings_cap)
-			break;
-
-		ret = ops->vidioc_dv_timings_cap(file, fh, p);
-		if (ret)
-			break;
-		switch (p->type) {
-		case V4L2_DV_BT_656_1120:
-			dbgarg(cmd,
-			       "type=%d, width=%u-%u, height=%u-%u, "
-			       "pixelclock=%llu-%llu, standards=%x, capabilities=%x ",
-			       p->type,
-			       p->bt.min_width, p->bt.max_width,
-			       p->bt.min_height, p->bt.max_height,
-			       p->bt.min_pixelclock, p->bt.max_pixelclock,
-			       p->bt.standards, p->bt.capabilities);
-			break;
-		default:
-			dbgarg(cmd, "unknown type ");
+	case 'V':
+		if (_IOC_NR(cmd) >= V4L2_IOCTLS) {
+			type = "v4l2";
 			break;
 		}
+		pr_cont("%s", v4l2_ioctls[_IOC_NR(cmd)].name);
+		return;
+	default:
+		type = "unknown";
 		break;
 	}
-	case VIDIOC_DQEVENT:
-	{
-		struct v4l2_event *ev = arg;
 
-		ret = v4l2_event_dequeue(fh, ev, file->f_flags & O_NONBLOCK);
-		if (ret < 0) {
-			dbgarg(cmd, "no pending events?");
-			break;
-		}
-		dbgarg(cmd,
-		       "pending=%d, type=0x%8.8x, sequence=%d, "
-		       "timestamp=%lu.%9.9lu ",
-		       ev->pending, ev->type, ev->sequence,
-		       ev->timestamp.tv_sec, ev->timestamp.tv_nsec);
-		break;
+	switch (_IOC_DIR(cmd)) {
+	case _IOC_NONE:              dir = "--"; break;
+	case _IOC_READ:              dir = "r-"; break;
+	case _IOC_WRITE:             dir = "-w"; break;
+	case _IOC_READ | _IOC_WRITE: dir = "rw"; break;
+	default:                     dir = "*ERR*"; break;
 	}
-	case VIDIOC_SUBSCRIBE_EVENT:
-	{
-		struct v4l2_event_subscription *sub = arg;
+	pr_cont("%s ioctl '%c', dir=%s, #%d (0x%08x)",
+		type, _IOC_TYPE(cmd), dir, _IOC_NR(cmd), cmd);
+}
+EXPORT_SYMBOL(v4l_printk_ioctl);
 
-		ret = ops->vidioc_subscribe_event(fh, sub);
-		if (ret < 0) {
-			dbgarg(cmd, "failed, ret=%ld", ret);
-			break;
-		}
-		dbgarg(cmd, "type=0x%8.8x", sub->type);
-		break;
-	}
-	case VIDIOC_UNSUBSCRIBE_EVENT:
-	{
-		struct v4l2_event_subscription *sub = arg;
+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;
+	bool write_only = false;
+	struct v4l2_ioctl_info default_info;
+	const struct v4l2_ioctl_info *info;
+	void *fh = file->private_data;
+	struct v4l2_fh *vfh = NULL;
+	int use_fh_prio = 0;
+	int debug = vfd->debug;
+	long ret = -ENOTTY;
 
-		ret = ops->vidioc_unsubscribe_event(fh, sub);
-		if (ret < 0) {
-			dbgarg(cmd, "failed, ret=%ld", ret);
-			break;
-		}
-		dbgarg(cmd, "type=0x%8.8x", sub->type);
-		break;
+	if (ops == NULL) {
+		pr_warn("%s: has no ioctl_ops.\n",
+				video_device_node_name(vfd));
+		return ret;
 	}
-	case VIDIOC_CREATE_BUFS:
-	{
-		struct v4l2_create_buffers *create = arg;
 
-		ret = check_fmt(ops, create->format.type);
-		if (ret)
-			break;
-
-		ret = ops->vidioc_create_bufs(file, fh, create);
-
-		dbgarg(cmd, "count=%d @ %d\n", create->count, create->index);
-		break;
+	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
+		vfh = file->private_data;
+		use_fh_prio = test_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 	}
-	case VIDIOC_PREPARE_BUF:
-	{
-		struct v4l2_buffer *b = arg;
-
-		ret = check_fmt(ops, b->type);
-		if (ret)
-			break;
 
-		ret = ops->vidioc_prepare_buf(file, fh, b);
+	if (v4l2_is_known_ioctl(cmd)) {
+		info = &v4l2_ioctls[_IOC_NR(cmd)];
 
-		dbgarg(cmd, "index=%d", b->index);
-		break;
-	}
-	default:
-		if (!ops->vidioc_default)
-			break;
-		ret = ops->vidioc_default(file, fh, use_fh_prio ?
-				v4l2_prio_check(vfd->prio, vfh->prio) >= 0 : 0,
-				cmd, arg);
-		break;
-	} /* switch */
+	        if (!test_bit(_IOC_NR(cmd), vfd->valid_ioctls) &&
+		    !((info->flags & INFO_FL_CTRL) && vfh && vfh->ctrl_handler))
+			goto done;
 
-	if (vfd->debug & V4L2_DEBUG_IOCTL_ARG) {
-		if (ret < 0) {
-			v4l_print_ioctl(vfd->name, cmd);
-			printk(KERN_CONT " error %ld\n", ret);
+		if (use_fh_prio && (info->flags & INFO_FL_PRIO)) {
+			ret = v4l2_prio_check(vfd->prio, vfh->prio);
+			if (ret)
+				goto done;
+		}
+	} else {
+		default_info.ioctl = cmd;
+		default_info.flags = 0;
+		default_info.debug = v4l_print_default;
+		info = &default_info;
+	}
+
+	write_only = _IOC_DIR(cmd) == _IOC_WRITE;
+	if (write_only && debug > V4L2_DEBUG_IOCTL) {
+		v4l_printk_ioctl(video_device_node_name(vfd), cmd);
+		pr_cont(": ");
+		info->debug(arg, write_only);
+	}
+	if (info->flags & INFO_FL_STD) {
+		typedef int (*vidioc_op)(struct file *file, void *fh, void *p);
+		const void *p = vfd->ioctl_ops;
+		const vidioc_op *vidioc = p + info->u.offset;
+
+		ret = (*vidioc)(file, fh, arg);
+	} else if (info->flags & INFO_FL_FUNC) {
+		ret = info->u.func(ops, file, fh, arg);
+	} else if (!ops->vidioc_default) {
+		ret = -ENOTTY;
+	} else {
+		ret = ops->vidioc_default(file, fh,
+			use_fh_prio ? v4l2_prio_check(vfd->prio, vfh->prio) >= 0 : 0,
+			cmd, arg);
+	}
+
+done:
+	if (debug) {
+		if (write_only && debug > V4L2_DEBUG_IOCTL) {
+			if (ret < 0)
+				printk(KERN_DEBUG "%s: error %ld\n",
+					video_device_node_name(vfd), ret);
+			return ret;
+		}
+		v4l_printk_ioctl(video_device_node_name(vfd), cmd);
+		if (ret < 0)
+			pr_cont(": error %ld\n", ret);
+		else if (debug == V4L2_DEBUG_IOCTL)
+			pr_cont("\n");
+		else if (_IOC_DIR(cmd) == _IOC_NONE)
+			info->debug(arg, write_only);
+		else {
+			pr_cont(": ");
+			info->debug(arg, write_only);
 		}
 	}
 
 	return ret;
 }
 
-/* In some cases, only a few fields are used as input, i.e. when the app sets
- * "index" and then the driver fills in the rest of the structure for the thing
- * with that index.  We only need to copy up the first non-input field.  */
-static unsigned long cmd_input_size(unsigned int cmd)
-{
-	/* Size of structure up to and including 'field' */
-#define CMDINSIZE(cmd, type, field) 				\
-	case VIDIOC_##cmd: 					\
-		return offsetof(struct v4l2_##type, field) + 	\
-			sizeof(((struct v4l2_##type *)0)->field);
-
-	switch (cmd) {
-		CMDINSIZE(ENUM_FMT,		fmtdesc,	type);
-		CMDINSIZE(G_FMT,		format,		type);
-		CMDINSIZE(QUERYBUF,		buffer,		length);
-		CMDINSIZE(G_PARM,		streamparm,	type);
-		CMDINSIZE(ENUMSTD,		standard,	index);
-		CMDINSIZE(ENUMINPUT,		input,		index);
-		CMDINSIZE(G_CTRL,		control,	id);
-		CMDINSIZE(G_TUNER,		tuner,		index);
-		CMDINSIZE(QUERYCTRL,		queryctrl,	id);
-		CMDINSIZE(QUERYMENU,		querymenu,	index);
-		CMDINSIZE(ENUMOUTPUT,		output,		index);
-		CMDINSIZE(G_MODULATOR,		modulator,	index);
-		CMDINSIZE(G_FREQUENCY,		frequency,	tuner);
-		CMDINSIZE(CROPCAP,		cropcap,	type);
-		CMDINSIZE(G_CROP,		crop,		type);
-		CMDINSIZE(ENUMAUDIO,		audio, 		index);
-		CMDINSIZE(ENUMAUDOUT,		audioout, 	index);
-		CMDINSIZE(ENCODER_CMD,		encoder_cmd,	flags);
-		CMDINSIZE(TRY_ENCODER_CMD,	encoder_cmd,	flags);
-		CMDINSIZE(G_SLICED_VBI_CAP,	sliced_vbi_cap,	type);
-		CMDINSIZE(ENUM_FRAMESIZES,	frmsizeenum,	pixel_format);
-		CMDINSIZE(ENUM_FRAMEINTERVALS,	frmivalenum,	height);
-	default:
-		return _IOC_SIZE(cmd);
-	}
-}
-
 static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 			    void * __user *user_ptr, void ***kernel_ptr)
 {
@@ -2219,7 +2235,20 @@ video_usercopy(struct file *file, unsigned int cmd, unsigned long arg,
 
 		err = -EFAULT;
 		if (_IOC_DIR(cmd) & _IOC_WRITE) {
-			unsigned long n = cmd_input_size(cmd);
+			unsigned int n = _IOC_SIZE(cmd);
+
+			/*
+			 * In some cases, only a few fields are used as input,
+			 * i.e. when the app sets "index" and then the driver
+			 * fills in the rest of the structure for the thing
+			 * with that index.  We only need to copy up the first
+			 * non-input field.
+			 */
+			if (v4l2_is_known_ioctl(cmd)) {
+				u32 flags = v4l2_ioctls[_IOC_NR(cmd)].flags;
+				if (flags & INFO_FL_CLEAR_MASK)
+					n = (flags & INFO_FL_CLEAR_MASK) >> 16;
+			}
 
 			if (copy_from_user(parg, (void __user *)arg, n))
 				goto out;
diff --git a/drivers/media/video/v4l2-mem2mem.c b/drivers/media/video/v4l2-mem2mem.c
index 975d0fa938c..97b48318aee 100644
--- a/drivers/media/video/v4l2-mem2mem.c
+++ b/drivers/media/video/v4l2-mem2mem.c
@@ -19,6 +19,9 @@
 
 #include <media/videobuf2-core.h>
 #include <media/v4l2-mem2mem.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 
 MODULE_DESCRIPTION("Mem to mem device framework for videobuf");
 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
@@ -407,11 +410,24 @@ 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 video_device *vfd = video_devdata(file);
+	unsigned long req_events = poll_requested_events(wait);
 	struct vb2_queue *src_q, *dst_q;
 	struct vb2_buffer *src_vb = NULL, *dst_vb = NULL;
 	unsigned int rc = 0;
 	unsigned long flags;
 
+	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
+		struct v4l2_fh *fh = file->private_data;
+
+		if (v4l2_event_pending(fh))
+			rc = POLLPRI;
+		else if (req_events & POLLPRI)
+			poll_wait(file, &fh->wait, wait);
+		if (!(req_events & (POLLOUT | POLLWRNORM | POLLIN | POLLRDNORM)))
+			return rc;
+	}
+
 	src_q = v4l2_m2m_get_src_vq(m2m_ctx);
 	dst_q = v4l2_m2m_get_dst_vq(m2m_ctx);
 
@@ -422,7 +438,7 @@ unsigned int v4l2_m2m_poll(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 	 */
 	if ((!src_q->streaming || list_empty(&src_q->queued_list))
 		&& (!dst_q->streaming || list_empty(&dst_q->queued_list))) {
-		rc = POLLERR;
+		rc |= POLLERR;
 		goto end;
 	}
 
diff --git a/drivers/media/video/v4l2-subdev.c b/drivers/media/video/v4l2-subdev.c
index db6e859b93d..9182f81deb5 100644
--- a/drivers/media/video/v4l2-subdev.c
+++ b/drivers/media/video/v4l2-subdev.c
@@ -245,7 +245,7 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		memset(&sel, 0, sizeof(sel));
 		sel.which = crop->which;
 		sel.pad = crop->pad;
-		sel.target = V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL;
+		sel.target = V4L2_SEL_TGT_CROP;
 
 		rval = v4l2_subdev_call(
 			sd, pad, get_selection, subdev_fh, &sel);
@@ -274,7 +274,7 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg)
 		memset(&sel, 0, sizeof(sel));
 		sel.which = crop->which;
 		sel.pad = crop->pad;
-		sel.target = V4L2_SUBDEV_SEL_TGT_CROP_ACTUAL;
+		sel.target = V4L2_SEL_TGT_CROP;
 		sel.r = crop->rect;
 
 		rval = v4l2_subdev_call(
diff --git a/drivers/media/video/via-camera.c b/drivers/media/video/via-camera.c
index 308e150a39b..eb404c2ce27 100644
--- a/drivers/media/video/via-camera.c
+++ b/drivers/media/video/via-camera.c
@@ -963,7 +963,7 @@ static int viacam_do_try_fmt(struct via_camera *cam,
 
 	upix->pixelformat = f->pixelformat;
 	viacam_fmt_pre(upix, spix);
-	v4l2_fill_mbus_format(&mbus_fmt, upix, f->mbus_code);
+	v4l2_fill_mbus_format(&mbus_fmt, spix, f->mbus_code);
 	ret = sensor_call(cam, video, try_mbus_fmt, &mbus_fmt);
 	v4l2_fill_pix_format(spix, &mbus_fmt);
 	viacam_fmt_post(upix, spix);
diff --git a/drivers/media/video/videobuf-core.c b/drivers/media/video/videobuf-core.c
index ffdf59cfe40..bf7a326b1cd 100644
--- a/drivers/media/video/videobuf-core.c
+++ b/drivers/media/video/videobuf-core.c
@@ -359,11 +359,6 @@ static void videobuf_status(struct videobuf_queue *q, struct v4l2_buffer *b,
 		break;
 	}
 
-	if (vb->input != UNSET) {
-		b->flags |= V4L2_BUF_FLAG_INPUT;
-		b->input  = vb->input;
-	}
-
 	b->field     = vb->field;
 	b->timestamp = vb->ts;
 	b->bytesused = vb->size;
@@ -402,7 +397,6 @@ int __videobuf_mmap_setup(struct videobuf_queue *q,
 			break;
 
 		q->bufs[i]->i      = i;
-		q->bufs[i]->input  = UNSET;
 		q->bufs[i]->memory = memory;
 		q->bufs[i]->bsize  = bsize;
 		switch (memory) {
@@ -566,16 +560,6 @@ int videobuf_qbuf(struct videobuf_queue *q, struct v4l2_buffer *b)
 		goto done;
 	}
 
-	if (b->flags & V4L2_BUF_FLAG_INPUT) {
-		if (b->input >= q->inputs) {
-			dprintk(1, "qbuf: wrong input.\n");
-			goto done;
-		}
-		buf->input = b->input;
-	} else {
-		buf->input = UNSET;
-	}
-
 	switch (b->memory) {
 	case V4L2_MEMORY_MMAP:
 		if (0 == buf->baddr) {
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index b6b5cc1a43c..3a43ba0959b 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -40,7 +40,7 @@ struct videobuf_dma_contig_memory {
 
 static int __videobuf_dc_alloc(struct device *dev,
 			       struct videobuf_dma_contig_memory *mem,
-			       unsigned long size, unsigned long flags)
+			       unsigned long size, gfp_t flags)
 {
 	mem->size = size;
 	if (mem->cached) {
@@ -56,7 +56,7 @@ static int __videobuf_dc_alloc(struct device *dev,
 				dev_err(dev, "dma_map_single failed\n");
 
 				free_pages_exact(mem->vaddr, mem->size);
-				mem->vaddr = 0;
+				mem->vaddr = NULL;
 				return err;
 			}
 		}
@@ -359,32 +359,43 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	size = vma->vm_end - vma->vm_start;
 	size = (size < mem->size) ? size : mem->size;
 
-	if (!mem->cached)
+	if (!mem->cached) {
 		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-
-	pos = (unsigned long)mem->vaddr;
-
-	while (size > 0) {
-		page = virt_to_page((void *)pos);
-		if (NULL == page) {
-			dev_err(q->dev, "mmap: virt_to_page failed\n");
-			__videobuf_dc_free(q->dev, mem);
-			goto error;
-		}
-		retval = vm_insert_page(vma, start, page);
+		retval = remap_pfn_range(vma, vma->vm_start,
+			 mem->dma_handle >> PAGE_SHIFT,
+				 size, vma->vm_page_prot);
 		if (retval) {
-			dev_err(q->dev, "mmap: insert failed with error %d\n",
-				retval);
-			__videobuf_dc_free(q->dev, mem);
+			dev_err(q->dev, "mmap: remap failed with error %d. ",
+								retval);
+			dma_free_coherent(q->dev, mem->size,
+					mem->vaddr, mem->dma_handle);
 			goto error;
 		}
-		start += PAGE_SIZE;
-		pos += PAGE_SIZE;
+	} else {
+		pos = (unsigned long)mem->vaddr;
 
-		if (size > PAGE_SIZE)
-			size -= PAGE_SIZE;
-		else
-			size = 0;
+		while (size > 0) {
+			page = virt_to_page((void *)pos);
+			if (NULL == page) {
+				dev_err(q->dev, "mmap: virt_to_page failed\n");
+				__videobuf_dc_free(q->dev, mem);
+				goto error;
+			}
+			retval = vm_insert_page(vma, start, page);
+			if (retval) {
+				dev_err(q->dev, "mmap: insert failed with error %d\n",
+					retval);
+				__videobuf_dc_free(q->dev, mem);
+				goto error;
+			}
+			start += PAGE_SIZE;
+			pos += PAGE_SIZE;
+
+			if (size > PAGE_SIZE)
+				size -= PAGE_SIZE;
+			else
+				size = 0;
+		}
 	}
 
 	vma->vm_ops = &videobuf_vm_ops;
diff --git a/drivers/media/video/videobuf2-core.c b/drivers/media/video/videobuf2-core.c
index 9d4e9edbd2e..268c7dd4f82 100644
--- a/drivers/media/video/videobuf2-core.c
+++ b/drivers/media/video/videobuf2-core.c
@@ -336,9 +336,9 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 	struct vb2_queue *q = vb->vb2_queue;
 	int ret;
 
-	/* Copy back data such as timestamp, flags, input, etc. */
+	/* Copy back data such as timestamp, flags, etc. */
 	memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
-	b->input = vb->v4l2_buf.input;
+	b->reserved2 = vb->v4l2_buf.reserved2;
 	b->reserved = vb->v4l2_buf.reserved;
 
 	if (V4L2_TYPE_IS_MULTIPLANAR(q->type)) {
@@ -454,7 +454,50 @@ static int __verify_mmap_ops(struct vb2_queue *q)
 }
 
 /**
- * vb2_reqbufs() - Initiate streaming
+ * __verify_memory_type() - Check whether the memory type and buffer type
+ * passed to a buffer operation are compatible with the queue.
+ */
+static int __verify_memory_type(struct vb2_queue *q,
+		enum v4l2_memory memory, enum v4l2_buf_type type)
+{
+	if (memory != V4L2_MEMORY_MMAP && memory != V4L2_MEMORY_USERPTR) {
+		dprintk(1, "reqbufs: unsupported memory type\n");
+		return -EINVAL;
+	}
+
+	if (type != q->type) {
+		dprintk(1, "reqbufs: requested type is incorrect\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Make sure all the required memory ops for given memory type
+	 * are available.
+	 */
+	if (memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
+		dprintk(1, "reqbufs: MMAP for current setup unsupported\n");
+		return -EINVAL;
+	}
+
+	if (memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
+		dprintk(1, "reqbufs: USERPTR for current setup unsupported\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Place the busy tests at the end: -EBUSY can be ignored when
+	 * create_bufs is called with count == 0, but count == 0 should still
+	 * do the memory and type validation.
+	 */
+	if (q->fileio) {
+		dprintk(1, "reqbufs: file io in progress\n");
+		return -EBUSY;
+	}
+	return 0;
+}
+
+/**
+ * __reqbufs() - Initiate streaming
  * @q:		videobuf2 queue
  * @req:	struct passed from userspace to vidioc_reqbufs handler in driver
  *
@@ -476,46 +519,16 @@ static int __verify_mmap_ops(struct vb2_queue *q)
  * 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)
+static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 {
 	unsigned int num_buffers, allocated_buffers, num_planes = 0;
-	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;
-	}
+	int ret;
 
 	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 || q->memory != req->memory) {
 		/*
 		 * We already have buffers allocated, so first check if they
@@ -595,10 +608,23 @@ int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 
 	return 0;
 }
+
+/**
+ * vb2_reqbufs() - Wrapper for __reqbufs() that also verifies the memory and
+ * type values.
+ * @q:		videobuf2 queue
+ * @req:	struct passed from userspace to vidioc_reqbufs handler in driver
+ */
+int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
+{
+	int ret = __verify_memory_type(q, req->memory, req->type);
+
+	return ret ? ret : __reqbufs(q, req);
+}
 EXPORT_SYMBOL_GPL(vb2_reqbufs);
 
 /**
- * vb2_create_bufs() - Allocate buffers and any required auxiliary structs
+ * __create_bufs() - Allocate buffers and any required auxiliary structs
  * @q:		videobuf2 queue
  * @create:	creation parameters, passed from userspace to vidioc_create_bufs
  *		handler in driver
@@ -612,40 +638,10 @@ EXPORT_SYMBOL_GPL(vb2_reqbufs);
  * The return values from this function are intended to be directly returned
  * from vidioc_create_bufs handler in driver.
  */
-int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
+static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 {
 	unsigned int num_planes = 0, num_buffers, allocated_buffers;
-	int ret = 0;
-
-	if (q->fileio) {
-		dprintk(1, "%s(): file io in progress\n", __func__);
-		return -EBUSY;
-	}
-
-	if (create->memory != V4L2_MEMORY_MMAP
-			&& create->memory != V4L2_MEMORY_USERPTR) {
-		dprintk(1, "%s(): unsupported memory type\n", __func__);
-		return -EINVAL;
-	}
-
-	if (create->format.type != q->type) {
-		dprintk(1, "%s(): requested type is incorrect\n", __func__);
-		return -EINVAL;
-	}
-
-	/*
-	 * Make sure all the required memory ops for given memory type
-	 * are available.
-	 */
-	if (create->memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
-		dprintk(1, "%s(): MMAP for current setup unsupported\n", __func__);
-		return -EINVAL;
-	}
-
-	if (create->memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
-		dprintk(1, "%s(): USERPTR for current setup unsupported\n", __func__);
-		return -EINVAL;
-	}
+	int ret;
 
 	if (q->num_buffers == VIDEO_MAX_FRAME) {
 		dprintk(1, "%s(): maximum number of buffers already allocated\n",
@@ -653,8 +649,6 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 		return -ENOBUFS;
 	}
 
-	create->index = q->num_buffers;
-
 	if (!q->num_buffers) {
 		memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 		memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
@@ -675,9 +669,9 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 	/* Finally, allocate buffers and video memory */
 	ret = __vb2_queue_alloc(q, create->memory, num_buffers,
 				num_planes);
-	if (ret < 0) {
-		dprintk(1, "Memory allocation failed with error: %d\n", ret);
-		return ret;
+	if (ret == 0) {
+		dprintk(1, "Memory allocation failed\n");
+		return -ENOMEM;
 	}
 
 	allocated_buffers = ret;
@@ -708,7 +702,7 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 
 	if (ret < 0) {
 		__vb2_queue_free(q, allocated_buffers);
-		return ret;
+		return -ENOMEM;
 	}
 
 	/*
@@ -719,6 +713,23 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 
 	return 0;
 }
+
+/**
+ * vb2_create_bufs() - Wrapper for __create_bufs() that also verifies the
+ * memory and type values.
+ * @q:		videobuf2 queue
+ * @create:	creation parameters, passed from userspace to vidioc_create_bufs
+ *		handler in driver
+ */
+int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
+{
+	int ret = __verify_memory_type(q, create->memory, create->format.type);
+
+	create->index = q->num_buffers;
+	if (create->count == 0)
+		return ret != -EBUSY ? ret : 0;
+	return ret ? ret : __create_bufs(q, create);
+}
 EXPORT_SYMBOL_GPL(vb2_create_bufs);
 
 /**
@@ -860,7 +871,6 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
 
 	vb->v4l2_buf.field = b->field;
 	vb->v4l2_buf.timestamp = b->timestamp;
-	vb->v4l2_buf.input = b->input;
 	vb->v4l2_buf.flags = b->flags & ~V4L2_BUFFER_STATE_FLAGS;
 
 	return 0;
@@ -2115,6 +2125,263 @@ size_t vb2_write(struct vb2_queue *q, char __user *data, size_t count,
 }
 EXPORT_SYMBOL_GPL(vb2_write);
 
+
+/*
+ * The following functions are not part of the vb2 core API, but are helper
+ * functions that plug into struct v4l2_ioctl_ops, struct v4l2_file_operations
+ * and struct vb2_ops.
+ * They contain boilerplate code that most if not all drivers have to do
+ * and so they simplify the driver code.
+ */
+
+/* The queue is busy if there is a owner and you are not that owner. */
+static inline bool vb2_queue_is_busy(struct video_device *vdev, struct file *file)
+{
+	return vdev->queue->owner && vdev->queue->owner != file->private_data;
+}
+
+/* vb2 ioctl helpers */
+
+int vb2_ioctl_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	int res = __verify_memory_type(vdev->queue, p->memory, p->type);
+
+	if (res)
+		return res;
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	res = __reqbufs(vdev->queue, p);
+	/* If count == 0, then the owner has released all buffers and he
+	   is no longer owner of the queue. Otherwise we have a new owner. */
+	if (res == 0)
+		vdev->queue->owner = p->count ? file->private_data : NULL;
+	return res;
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_reqbufs);
+
+int vb2_ioctl_create_bufs(struct file *file, void *priv,
+			  struct v4l2_create_buffers *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	int res = __verify_memory_type(vdev->queue, p->memory, p->format.type);
+
+	p->index = vdev->queue->num_buffers;
+	/* If count == 0, then just check if memory and type are valid.
+	   Any -EBUSY result from __verify_memory_type can be mapped to 0. */
+	if (p->count == 0)
+		return res != -EBUSY ? res : 0;
+	if (res)
+		return res;
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	res = __create_bufs(vdev->queue, p);
+	if (res == 0)
+		vdev->queue->owner = file->private_data;
+	return res;
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_create_bufs);
+
+int vb2_ioctl_prepare_buf(struct file *file, void *priv,
+			  struct v4l2_buffer *p)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	return vb2_prepare_buf(vdev->queue, p);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_prepare_buf);
+
+int vb2_ioctl_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	/* No need to call vb2_queue_is_busy(), anyone can query buffers. */
+	return vb2_querybuf(vdev->queue, p);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_querybuf);
+
+int vb2_ioctl_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	return vb2_qbuf(vdev->queue, p);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_qbuf);
+
+int vb2_ioctl_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	return vb2_dqbuf(vdev->queue, p, file->f_flags & O_NONBLOCK);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_dqbuf);
+
+int vb2_ioctl_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	return vb2_streamon(vdev->queue, i);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_streamon);
+
+int vb2_ioctl_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vb2_queue_is_busy(vdev, file))
+		return -EBUSY;
+	return vb2_streamoff(vdev->queue, i);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_streamoff);
+
+/* v4l2_file_operations helpers */
+
+int vb2_fop_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	return vb2_mmap(vdev->queue, vma);
+}
+EXPORT_SYMBOL_GPL(vb2_fop_mmap);
+
+int vb2_fop_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (file->private_data == vdev->queue->owner) {
+		vb2_queue_release(vdev->queue);
+		vdev->queue->owner = NULL;
+	}
+	return v4l2_fh_release(file);
+}
+EXPORT_SYMBOL_GPL(vb2_fop_release);
+
+ssize_t vb2_fop_write(struct file *file, char __user *buf,
+		size_t count, loff_t *ppos)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct mutex *lock = vdev->queue->lock ? vdev->queue->lock : vdev->lock;
+	bool must_lock = !test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) && lock;
+	int err = -EBUSY;
+
+	if (must_lock && mutex_lock_interruptible(lock))
+		return -ERESTARTSYS;
+	if (vb2_queue_is_busy(vdev, file))
+		goto exit;
+	err = vb2_write(vdev->queue, buf, count, ppos,
+		       file->f_flags & O_NONBLOCK);
+	if (err >= 0)
+		vdev->queue->owner = file->private_data;
+exit:
+	if (must_lock)
+		mutex_unlock(lock);
+	return err;
+}
+EXPORT_SYMBOL_GPL(vb2_fop_write);
+
+ssize_t vb2_fop_read(struct file *file, char __user *buf,
+		size_t count, loff_t *ppos)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct mutex *lock = vdev->queue->lock ? vdev->queue->lock : vdev->lock;
+	bool must_lock = !test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags) && vdev->lock;
+	int err = -EBUSY;
+
+	if (must_lock && mutex_lock_interruptible(lock))
+		return -ERESTARTSYS;
+	if (vb2_queue_is_busy(vdev, file))
+		goto exit;
+	err = vb2_read(vdev->queue, buf, count, ppos,
+		       file->f_flags & O_NONBLOCK);
+	if (err >= 0)
+		vdev->queue->owner = file->private_data;
+exit:
+	if (must_lock)
+		mutex_unlock(lock);
+	return err;
+}
+EXPORT_SYMBOL_GPL(vb2_fop_read);
+
+unsigned int vb2_fop_poll(struct file *file, poll_table *wait)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct vb2_queue *q = vdev->queue;
+	struct mutex *lock = q->lock ? q->lock : vdev->lock;
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned res;
+	void *fileio;
+	/* Yuck. We really need to get rid of this flag asap. If it is
+	   set, then the core took the serialization lock before calling
+	   poll(). This is being phased out, but for now we have to handle
+	   this case. */
+	bool locked = test_bit(V4L2_FL_LOCK_ALL_FOPS, &vdev->flags);
+	bool must_lock = false;
+
+	/* Try to be smart: only lock if polling might start fileio,
+	   otherwise locking will only introduce unwanted delays. */
+	if (q->num_buffers == 0 && q->fileio == NULL) {
+		if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ) &&
+				(req_events & (POLLIN | POLLRDNORM)))
+			must_lock = true;
+		else if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE) &&
+				(req_events & (POLLOUT | POLLWRNORM)))
+			must_lock = true;
+	}
+
+	/* If locking is needed, but this helper doesn't know how, then you
+	   shouldn't be using this helper but you should write your own. */
+	WARN_ON(must_lock && !locked && !lock);
+
+	if (must_lock && !locked && lock && mutex_lock_interruptible(lock))
+		return POLLERR;
+
+	fileio = q->fileio;
+
+	res = vb2_poll(vdev->queue, file, wait);
+
+	/* If fileio was started, then we have a new queue owner. */
+	if (must_lock && !fileio && q->fileio)
+		q->owner = file->private_data;
+	if (must_lock && !locked && lock)
+		mutex_unlock(lock);
+	return res;
+}
+EXPORT_SYMBOL_GPL(vb2_fop_poll);
+
+#ifndef CONFIG_MMU
+unsigned long vb2_fop_get_unmapped_area(struct file *file, unsigned long addr,
+		unsigned long len, unsigned long pgoff, unsigned long flags)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	return vb2_get_unmapped_area(vdev->queue, addr, len, pgoff, flags);
+}
+EXPORT_SYMBOL_GPL(vb2_fop_get_unmapped_area);
+#endif
+
+/* vb2_ops helpers. Only use if vq->lock is non-NULL. */
+
+void vb2_ops_wait_prepare(struct vb2_queue *vq)
+{
+	mutex_unlock(vq->lock);
+}
+EXPORT_SYMBOL_GPL(vb2_ops_wait_prepare);
+
+void vb2_ops_wait_finish(struct vb2_queue *vq)
+{
+	mutex_lock(vq->lock);
+}
+EXPORT_SYMBOL_GPL(vb2_ops_wait_finish);
+
 MODULE_DESCRIPTION("Driver helper framework for Video for Linux 2");
 MODULE_AUTHOR("Pawel Osciak <pawel@osciak.com>, Marek Szyprowski");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index 08c10240e70..a05494b71b2 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -188,6 +188,7 @@ struct vivi_dev {
 	struct list_head           vivi_devlist;
 	struct v4l2_device 	   v4l2_dev;
 	struct v4l2_ctrl_handler   ctrl_handler;
+	struct video_device	   vdev;
 
 	/* controls */
 	struct v4l2_ctrl	   *brightness;
@@ -213,9 +214,6 @@ struct vivi_dev {
 	spinlock_t                 slock;
 	struct mutex		   mutex;
 
-	/* various device info */
-	struct video_device        *vfd;
-
 	struct vivi_dmaqueue       vidq;
 
 	/* Several counters */
@@ -232,7 +230,6 @@ struct vivi_dev {
 	struct vivi_fmt            *fmt;
 	unsigned int               width, height;
 	struct vb2_queue	   vb_vidq;
-	enum v4l2_field		   field;
 	unsigned int		   field_count;
 
 	u8			   bars[9][3];
@@ -625,7 +622,7 @@ static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
 
 	dev->mv_count += 2;
 
-	buf->vb.v4l2_buf.field = dev->field;
+	buf->vb.v4l2_buf.field = V4L2_FIELD_INTERLACED;
 	dev->field_count++;
 	buf->vb.v4l2_buf.sequence = dev->field_count >> 1;
 	do_gettimeofday(&ts);
@@ -769,7 +766,13 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct vivi_dev *dev = vb2_get_drv_priv(vq);
 	unsigned long size;
 
-	size = dev->width * dev->height * dev->pixelsize;
+	if (fmt)
+		size = fmt->fmt.pix.sizeimage;
+	else
+		size = dev->width * dev->height * dev->pixelsize;
+
+	if (size == 0)
+		return -EINVAL;
 
 	if (0 == *nbuffers)
 		*nbuffers = 32;
@@ -792,27 +795,6 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	return 0;
 }
 
-static int buffer_init(struct vb2_buffer *vb)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-
-	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.
-	 */
-
-	return 0;
-}
-
 static int buffer_prepare(struct vb2_buffer *vb)
 {
 	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
@@ -850,20 +832,6 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	return 0;
 }
 
-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__);
-
-}
-
 static void buffer_queue(struct vb2_buffer *vb)
 {
 	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
@@ -909,10 +877,7 @@ static void vivi_unlock(struct vb2_queue *vq)
 
 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,
@@ -959,7 +924,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->field;
+	f->fmt.pix.field        = V4L2_FIELD_INTERLACED;
 	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
 	f->fmt.pix.bytesperline =
 		(f->fmt.pix.width * dev->fmt->depth) >> 3;
@@ -978,25 +943,16 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct vivi_dev *dev = video_drvdata(file);
 	struct vivi_fmt *fmt;
-	enum v4l2_field field;
 
 	fmt = get_format(f);
 	if (!fmt) {
-		dprintk(dev, 1, "Fourcc format (0x%08x) invalid.\n",
+		dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
 			f->fmt.pix.pixelformat);
-		return -EINVAL;
-	}
-
-	field = f->fmt.pix.field;
-
-	if (field == V4L2_FIELD_ANY) {
-		field = V4L2_FIELD_INTERLACED;
-	} else if (V4L2_FIELD_INTERLACED != field) {
-		dprintk(dev, 1, "Field type invalid.\n");
-		return -EINVAL;
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+		fmt = get_format(f);
 	}
 
-	f->fmt.pix.field = field;
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
 	v4l_bound_align_image(&f->fmt.pix.width, 48, MAX_WIDTH, 2,
 			      &f->fmt.pix.height, 32, MAX_HEIGHT, 0, 0);
 	f->fmt.pix.bytesperline =
@@ -1021,7 +977,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
-	if (vb2_is_streaming(q)) {
+	if (vb2_is_busy(q)) {
 		dprintk(dev, 1, "%s device busy\n", __func__);
 		return -EBUSY;
 	}
@@ -1030,53 +986,10 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	dev->pixelsize = dev->fmt->depth / 8;
 	dev->width = f->fmt.pix.width;
 	dev->height = f->fmt.pix.height;
-	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 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 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 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 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);
-	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);
-	return vb2_streamoff(&dev->vb_vidq, i);
-}
-
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *i)
-{
-	return 0;
-}
-
 /* only one input in this sample driver */
 static int vidioc_enum_input(struct file *file, void *priv,
 				struct v4l2_input *inp)
@@ -1085,7 +998,6 @@ static int vidioc_enum_input(struct file *file, void *priv,
 		return -EINVAL;
 
 	inp->type = V4L2_INPUT_TYPE_CAMERA;
-	inp->std = V4L2_STD_525_60;
 	sprintf(inp->name, "Camera %u", inp->index);
 	return 0;
 }
@@ -1145,58 +1057,6 @@ static int vivi_s_ctrl(struct v4l2_ctrl *ctrl)
 	File operations for the device
    ------------------------------------------------------------------*/
 
-static ssize_t
-vivi_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	int err;
-
-	dprintk(dev, 1, "read called\n");
-	mutex_lock(&dev->mutex);
-	err = vb2_read(&dev->vb_vidq, data, count, ppos,
-		       file->f_flags & O_NONBLOCK);
-	mutex_unlock(&dev->mutex);
-	return err;
-}
-
-static unsigned int
-vivi_poll(struct file *file, struct poll_table_struct *wait)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	struct vb2_queue *q = &dev->vb_vidq;
-
-	dprintk(dev, 1, "%s\n", __func__);
-	return vb2_poll(q, file, wait);
-}
-
-static int vivi_close(struct file *file)
-{
-	struct video_device  *vdev = video_devdata(file);
-	struct vivi_dev *dev = video_drvdata(file);
-
-	dprintk(dev, 1, "close called (dev=%s), file %p\n",
-		video_device_node_name(vdev), file);
-
-	if (v4l2_fh_is_singular_file(file))
-		vb2_queue_release(&dev->vb_vidq);
-	return v4l2_fh_release(file);
-}
-
-static int vivi_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	int ret;
-
-	dprintk(dev, 1, "mmap called, vma=0x%08lx\n", (unsigned long)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,
-		ret);
-	return ret;
-}
-
 static const struct v4l2_ctrl_ops vivi_ctrl_ops = {
 	.g_volatile_ctrl = vivi_g_volatile_ctrl,
 	.s_ctrl = vivi_s_ctrl,
@@ -1301,11 +1161,11 @@ static const struct v4l2_ctrl_config vivi_ctrl_int_menu = {
 static const struct v4l2_file_operations vivi_fops = {
 	.owner		= THIS_MODULE,
 	.open           = v4l2_fh_open,
-	.release        = vivi_close,
-	.read           = vivi_read,
-	.poll		= vivi_poll,
+	.release        = vb2_fop_release,
+	.read           = vb2_fop_read,
+	.poll		= vb2_fop_poll,
 	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
-	.mmap           = vivi_mmap,
+	.mmap           = vb2_fop_mmap,
 };
 
 static const struct v4l2_ioctl_ops vivi_ioctl_ops = {
@@ -1314,16 +1174,17 @@ static const struct v4l2_ioctl_ops vivi_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
-	.vidioc_reqbufs       = vidioc_reqbufs,
-	.vidioc_querybuf      = vidioc_querybuf,
-	.vidioc_qbuf          = vidioc_qbuf,
-	.vidioc_dqbuf         = vidioc_dqbuf,
-	.vidioc_s_std         = vidioc_s_std,
+	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs   = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf      = vb2_ioctl_querybuf,
+	.vidioc_qbuf          = vb2_ioctl_qbuf,
+	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
 	.vidioc_enum_input    = vidioc_enum_input,
 	.vidioc_g_input       = vidioc_g_input,
 	.vidioc_s_input       = vidioc_s_input,
-	.vidioc_streamon      = vidioc_streamon,
-	.vidioc_streamoff     = vidioc_streamoff,
+	.vidioc_streamon      = vb2_ioctl_streamon,
+	.vidioc_streamoff     = vb2_ioctl_streamoff,
 	.vidioc_log_status    = v4l2_ctrl_log_status,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
@@ -1333,10 +1194,7 @@ static struct video_device vivi_template = {
 	.name		= "vivi",
 	.fops           = &vivi_fops,
 	.ioctl_ops 	= &vivi_ioctl_ops,
-	.release	= video_device_release,
-
-	.tvnorms              = V4L2_STD_525_60,
-	.current_norm         = V4L2_STD_NTSC_M,
+	.release	= video_device_release_empty,
 };
 
 /* -----------------------------------------------------------------
@@ -1354,8 +1212,8 @@ static int vivi_release(void)
 		dev = list_entry(list, struct vivi_dev, vivi_devlist);
 
 		v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
-			video_device_node_name(dev->vfd));
-		video_unregister_device(dev->vfd);
+			video_device_node_name(&dev->vdev));
+		video_unregister_device(&dev->vdev);
 		v4l2_device_unregister(&dev->v4l2_dev);
 		v4l2_ctrl_handler_free(&dev->ctrl_handler);
 		kfree(dev);
@@ -1440,14 +1298,11 @@ static int __init vivi_create_instance(int inst)
 	INIT_LIST_HEAD(&dev->vidq.active);
 	init_waitqueue_head(&dev->vidq.wq);
 
-	ret = -ENOMEM;
-	vfd = video_device_alloc();
-	if (!vfd)
-		goto unreg_dev;
-
+	vfd = &dev->vdev;
 	*vfd = vivi_template;
 	vfd->debug = debug;
 	vfd->v4l2_dev = &dev->v4l2_dev;
+	vfd->queue = q;
 	set_bit(V4L2_FL_USE_FH_PRIO, &vfd->flags);
 
 	/*
@@ -1455,26 +1310,19 @@ static int __init vivi_create_instance(int inst)
 	 * all fops and v4l2 ioctls.
 	 */
 	vfd->lock = &dev->mutex;
+	video_set_drvdata(vfd, dev);
 
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr);
 	if (ret < 0)
-		goto rel_vdev;
-
-	video_set_drvdata(vfd, dev);
+		goto unreg_dev;
 
 	/* Now that everything is fine, let's add it to device list */
 	list_add_tail(&dev->vivi_devlist, &vivi_devlist);
 
-	if (video_nr != -1)
-		video_nr++;
-
-	dev->vfd = vfd;
 	v4l2_info(&dev->v4l2_dev, "V4L2 device registered as %s\n",
 		  video_device_node_name(vfd));
 	return 0;
 
-rel_vdev:
-	video_device_release(vfd);
 unreg_dev:
 	v4l2_ctrl_handler_free(hdl);
 	v4l2_device_unregister(&dev->v4l2_dev);
diff --git a/drivers/media/video/zoran/zoran.h b/drivers/media/video/zoran/zoran.h
index d7166afc255..ca2754a3cd6 100644
--- a/drivers/media/video/zoran/zoran.h
+++ b/drivers/media/video/zoran/zoran.h
@@ -172,8 +172,10 @@ struct zoran_jpg_settings {
 	struct v4l2_jpegcompression jpg_comp;	/* JPEG-specific capture settings */
 };
 
+struct zoran_fh;
+
 struct zoran_mapping {
-	struct file *file;
+	struct zoran_fh *fh;
 	int count;
 };
 
diff --git a/drivers/media/video/zoran/zoran_driver.c b/drivers/media/video/zoran/zoran_driver.c
index c5731093181..c6ccdeb6d8d 100644
--- a/drivers/media/video/zoran/zoran_driver.c
+++ b/drivers/media/video/zoran/zoran_driver.c
@@ -2811,7 +2811,7 @@ static void
 zoran_vm_close (struct vm_area_struct *vma)
 {
 	struct zoran_mapping *map = vma->vm_private_data;
-	struct zoran_fh *fh = map->file->private_data;
+	struct zoran_fh *fh = map->fh;
 	struct zoran *zr = fh->zr;
 	int i;
 
@@ -2938,7 +2938,7 @@ zoran_mmap (struct file           *file,
 		res = -ENOMEM;
 		goto mmap_unlock_and_return;
 	}
-	map->file = file;
+	map->fh = fh;
 	map->count = 1;
 
 	vma->vm_ops = &zoran_vm_ops;
diff --git a/drivers/media/video/zoran/zr36016.c b/drivers/media/video/zoran/zr36016.c
index 21c088ea904..b87ddba8608 100644
--- a/drivers/media/video/zoran/zr36016.c
+++ b/drivers/media/video/zoran/zr36016.c
@@ -40,10 +40,10 @@
 /* v4l  API */
 
 /* headerfile of this module */
-#include"zr36016.h"
+#include "zr36016.h"
 
 /* codec io API */
-#include"videocodec.h"
+#include "videocodec.h"
 
 /* it doesn't make sense to have more than 20 or so,
   just to prevent some unwanted loops */
diff --git a/drivers/media/video/zr364xx.c b/drivers/media/video/zr364xx.c
index e44cb330bbc..9afab35878b 100644
--- a/drivers/media/video/zr364xx.c
+++ b/drivers/media/video/zr364xx.c
@@ -37,6 +37,10 @@
 #include <linux/highmem.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
 #include <media/videobuf-vmalloc.h>
 
 
@@ -120,11 +124,6 @@ static struct usb_device_id device_table[] = {
 
 MODULE_DEVICE_TABLE(usb, device_table);
 
-struct zr364xx_mode {
-	u32 color;	/* output video color format */
-	u32 brightness;	/* brightness */
-};
-
 /* frame structure */
 struct zr364xx_framei {
 	unsigned long ulState;	/* ulState:ZR364XX_READ_IDLE,
@@ -173,7 +172,10 @@ static const struct zr364xx_fmt formats[] = {
 struct zr364xx_camera {
 	struct usb_device *udev;	/* save off the usb device pointer */
 	struct usb_interface *interface;/* the interface for this device */
-	struct video_device *vdev;	/* v4l video device */
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct video_device vdev;	/* v4l video device */
+	struct v4l2_fh *owner;		/* owns the streaming */
 	int nb;
 	struct zr364xx_bufferi		buffer;
 	int skip;
@@ -181,12 +183,9 @@ struct zr364xx_camera {
 	int height;
 	int method;
 	struct mutex lock;
-	struct mutex open_lock;
-	int users;
 
 	spinlock_t		slock;
 	struct zr364xx_dmaqueue	vidq;
-	int			resources;
 	int			last_frame;
 	int			cur_frame;
 	unsigned long		frame_count;
@@ -197,8 +196,7 @@ struct zr364xx_camera {
 
 	const struct zr364xx_fmt *fmt;
 	struct videobuf_queue	vb_vidq;
-	enum v4l2_buf_type	type;
-	struct zr364xx_mode	mode;
+	bool was_streaming;
 };
 
 /* buffer for one video frame */
@@ -230,11 +228,6 @@ static int send_control_msg(struct usb_device *udev, u8 request, u16 value,
 				 transfer_buffer, size, CTRL_TIMEOUT);
 
 	kfree(transfer_buffer);
-
-	if (status < 0)
-		dev_err(&udev->dev,
-			"Failed sending control message, error %d.\n", status);
-
 	return status;
 }
 
@@ -468,6 +461,7 @@ static ssize_t zr364xx_read(struct file *file, char __user *buf, size_t count,
 			    loff_t * ppos)
 {
 	struct zr364xx_camera *cam = video_drvdata(file);
+	int err = 0;
 
 	_DBG("%s\n", __func__);
 
@@ -477,17 +471,21 @@ static ssize_t zr364xx_read(struct file *file, char __user *buf, size_t count,
 	if (!count)
 		return -EINVAL;
 
-	if (cam->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    zr364xx_vidioc_streamon(file, cam, cam->type) == 0) {
-		DBG("%s: reading %d bytes at pos %d.\n", __func__, (int) count,
-		    (int) *ppos);
+	if (mutex_lock_interruptible(&cam->lock))
+		return -ERESTARTSYS;
+
+	err = zr364xx_vidioc_streamon(file, file->private_data,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	if (err == 0) {
+		DBG("%s: reading %d bytes at pos %d.\n", __func__,
+				(int) count, (int) *ppos);
 
 		/* NoMan Sux ! */
-		return videobuf_read_one(&cam->vb_vidq, buf, count, ppos,
+		err = videobuf_read_one(&cam->vb_vidq, buf, count, ppos,
 					file->f_flags & O_NONBLOCK);
 	}
-
-	return 0;
+	mutex_unlock(&cam->lock);
+	return err;
 }
 
 /* video buffer vmalloc implementation based partly on VIVI driver which is
@@ -702,35 +700,6 @@ static int zr364xx_read_video_callback(struct zr364xx_camera *cam,
 	return 0;
 }
 
-static int res_get(struct zr364xx_camera *cam)
-{
-	/* is it free? */
-	mutex_lock(&cam->lock);
-	if (cam->resources) {
-		/* no, someone else uses it */
-		mutex_unlock(&cam->lock);
-		return 0;
-	}
-	/* it's free, grab it */
-	cam->resources = 1;
-	_DBG("res: get\n");
-	mutex_unlock(&cam->lock);
-	return 1;
-}
-
-static inline int res_check(struct zr364xx_camera *cam)
-{
-	return cam->resources;
-}
-
-static void res_free(struct zr364xx_camera *cam)
-{
-	mutex_lock(&cam->lock);
-	cam->resources = 0;
-	mutex_unlock(&cam->lock);
-	_DBG("res: put\n");
-}
-
 static int zr364xx_vidioc_querycap(struct file *file, void *priv,
 				   struct v4l2_capability *cap)
 {
@@ -740,9 +709,10 @@ static int zr364xx_vidioc_querycap(struct file *file, void *priv,
 	strlcpy(cap->card, cam->udev->product, sizeof(cap->card));
 	strlcpy(cap->bus_info, dev_name(&cam->udev->dev),
 		sizeof(cap->bus_info));
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
 			    V4L2_CAP_READWRITE |
 			    V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
@@ -772,50 +742,18 @@ static int zr364xx_vidioc_s_input(struct file *file, void *priv,
 	return 0;
 }
 
-static int zr364xx_vidioc_queryctrl(struct file *file, void *priv,
-				    struct v4l2_queryctrl *c)
+static int zr364xx_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct zr364xx_camera *cam;
-
-	if (file == NULL)
-		return -ENODEV;
-	cam = video_drvdata(file);
-
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->type = V4L2_CTRL_TYPE_INTEGER;
-		strcpy(c->name, "Brightness");
-		c->minimum = 0;
-		c->maximum = 127;
-		c->step = 1;
-		c->default_value = cam->mode.brightness;
-		c->flags = 0;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int zr364xx_vidioc_s_ctrl(struct file *file, void *priv,
-				 struct v4l2_control *c)
-{
-	struct zr364xx_camera *cam;
+	struct zr364xx_camera *cam =
+		container_of(ctrl->handler, struct zr364xx_camera, ctrl_handler);
 	int temp;
 
-	if (file == NULL)
-		return -ENODEV;
-	cam = video_drvdata(file);
-
-	switch (c->id) {
+	switch (ctrl->id) {
 	case V4L2_CID_BRIGHTNESS:
-		cam->mode.brightness = c->value;
 		/* hardware brightness */
-		mutex_lock(&cam->lock);
 		send_control_msg(cam->udev, 1, 0x2001, 0, NULL, 0);
-		temp = (0x60 << 8) + 127 - cam->mode.brightness;
+		temp = (0x60 << 8) + 127 - ctrl->val;
 		send_control_msg(cam->udev, 1, temp, 0, NULL, 0);
-		mutex_unlock(&cam->lock);
 		break;
 	default:
 		return -EINVAL;
@@ -824,25 +762,6 @@ static int zr364xx_vidioc_s_ctrl(struct file *file, void *priv,
 	return 0;
 }
 
-static int zr364xx_vidioc_g_ctrl(struct file *file, void *priv,
-				 struct v4l2_control *c)
-{
-	struct zr364xx_camera *cam;
-
-	if (file == NULL)
-		return -ENODEV;
-	cam = video_drvdata(file);
-
-	switch (c->id) {
-	case V4L2_CID_BRIGHTNESS:
-		c->value = cam->mode.brightness;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
 static int zr364xx_vidioc_enum_fmt_vid_cap(struct file *file,
 				       void *priv, struct v4l2_fmtdesc *f)
 {
@@ -888,7 +807,7 @@ static int zr364xx_vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.field = V4L2_FIELD_NONE;
 	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
-	f->fmt.pix.colorspace = 0;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
 	f->fmt.pix.priv = 0;
 	DBG("%s: V4L2_PIX_FMT_%s (%d) ok!\n", __func__,
 	    decode_fourcc(f->fmt.pix.pixelformat, pixelformat_name),
@@ -911,7 +830,7 @@ static int zr364xx_vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.height = cam->height;
 	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
-	f->fmt.pix.colorspace = 0;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
 	f->fmt.pix.priv = 0;
 	return 0;
 }
@@ -936,7 +855,7 @@ static int zr364xx_vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 		goto out;
 	}
 
-	if (res_check(cam)) {
+	if (cam->owner) {
 		DBG("%s can't change format after started\n", __func__);
 		ret = -EBUSY;
 		goto out;
@@ -944,14 +863,13 @@ static int zr364xx_vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 
 	cam->width = f->fmt.pix.width;
 	cam->height = f->fmt.pix.height;
-	dev_info(&cam->udev->dev, "%s: %dx%d mode selected\n", __func__,
+	DBG("%s: %dx%d mode selected\n", __func__,
 		 cam->width, cam->height);
 	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
-	f->fmt.pix.colorspace = 0;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
 	f->fmt.pix.priv = 0;
 	cam->vb_vidq.field = f->fmt.pix.field;
-	cam->mode.color = V4L2_PIX_FMT_JPEG;
 
 	if (f->fmt.pix.width == 160 && f->fmt.pix.height == 120)
 		mode = 1;
@@ -1015,10 +933,11 @@ out:
 static int zr364xx_vidioc_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *p)
 {
-	int rc;
 	struct zr364xx_camera *cam = video_drvdata(file);
-	rc = videobuf_reqbufs(&cam->vb_vidq, p);
-	return rc;
+
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+	return videobuf_reqbufs(&cam->vb_vidq, p);
 }
 
 static int zr364xx_vidioc_querybuf(struct file *file,
@@ -1038,6 +957,8 @@ static int zr364xx_vidioc_qbuf(struct file *file,
 	int rc;
 	struct zr364xx_camera *cam = video_drvdata(file);
 	_DBG("%s\n", __func__);
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
 	rc = videobuf_qbuf(&cam->vb_vidq, p);
 	return rc;
 }
@@ -1049,6 +970,8 @@ static int zr364xx_vidioc_dqbuf(struct file *file,
 	int rc;
 	struct zr364xx_camera *cam = video_drvdata(file);
 	_DBG("%s\n", __func__);
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
 	rc = videobuf_dqbuf(&cam->vb_vidq, p, file->f_flags & O_NONBLOCK);
 	return rc;
 }
@@ -1197,29 +1120,23 @@ static inline int zr364xx_stop_acquire(struct zr364xx_camera *cam)
 	return 0;
 }
 
-static int zr364xx_vidioc_streamon(struct file *file, void *priv,
-				   enum v4l2_buf_type type)
+static int zr364xx_prepare(struct zr364xx_camera *cam)
 {
-	struct zr364xx_camera *cam = video_drvdata(file);
-	int j;
 	int res;
+	int i, j;
 
-	DBG("%s\n", __func__);
-
-	if (cam->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		dev_err(&cam->udev->dev, "invalid fh type0\n");
-		return -EINVAL;
-	}
-	if (cam->type != type) {
-		dev_err(&cam->udev->dev, "invalid fh type1\n");
-		return -EINVAL;
-	}
-
-	if (!res_get(cam)) {
-		dev_err(&cam->udev->dev, "stream busy\n");
-		return -EBUSY;
+	for (i = 0; init[cam->method][i].size != -1; i++) {
+		res = send_control_msg(cam->udev, 1, init[cam->method][i].value,
+				     0, init[cam->method][i].bytes,
+				     init[cam->method][i].size);
+		if (res < 0) {
+			dev_err(&cam->udev->dev,
+				"error during open sequence: %d\n", i);
+			return res;
+		}
 	}
 
+	cam->skip = 2;
 	cam->last_frame = -1;
 	cam->cur_frame = 0;
 	cam->frame_count = 0;
@@ -1227,11 +1144,31 @@ static int zr364xx_vidioc_streamon(struct file *file, void *priv,
 		cam->buffer.frame[j].ulState = ZR364XX_READ_IDLE;
 		cam->buffer.frame[j].cur_size = 0;
 	}
+	v4l2_ctrl_handler_setup(&cam->ctrl_handler);
+	return 0;
+}
+
+static int zr364xx_vidioc_streamon(struct file *file, void *priv,
+				   enum v4l2_buf_type type)
+{
+	struct zr364xx_camera *cam = video_drvdata(file);
+	int res;
+
+	DBG("%s\n", __func__);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
+
+	res = zr364xx_prepare(cam);
+	if (res)
+		return res;
 	res = videobuf_streamon(&cam->vb_vidq);
 	if (res == 0) {
 		zr364xx_start_acquire(cam);
-	} else {
-		res_free(cam);
+		cam->owner = file->private_data;
 	}
 	return res;
 }
@@ -1239,67 +1176,32 @@ static int zr364xx_vidioc_streamon(struct file *file, void *priv,
 static int zr364xx_vidioc_streamoff(struct file *file, void *priv,
 				    enum v4l2_buf_type type)
 {
-	int res;
 	struct zr364xx_camera *cam = video_drvdata(file);
 
 	DBG("%s\n", __func__);
-	if (cam->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		dev_err(&cam->udev->dev, "invalid fh type0\n");
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	}
-	if (cam->type != type) {
-		dev_err(&cam->udev->dev, "invalid fh type1\n");
-		return -EINVAL;
-	}
+	if (cam->owner && cam->owner != priv)
+		return -EBUSY;
 	zr364xx_stop_acquire(cam);
-	res = videobuf_streamoff(&cam->vb_vidq);
-	if (res < 0)
-		return res;
-	res_free(cam);
-	return 0;
+	return videobuf_streamoff(&cam->vb_vidq);
 }
 
 
 /* open the camera */
 static int zr364xx_open(struct file *file)
 {
-	struct video_device *vdev = video_devdata(file);
 	struct zr364xx_camera *cam = video_drvdata(file);
-	struct usb_device *udev = cam->udev;
-	int i, err;
+	int err;
 
 	DBG("%s\n", __func__);
 
-	mutex_lock(&cam->open_lock);
+	if (mutex_lock_interruptible(&cam->lock))
+		return -ERESTARTSYS;
 
-	if (cam->users) {
-		err = -EBUSY;
+	err = v4l2_fh_open(file);
+	if (err)
 		goto out;
-	}
-
-	for (i = 0; init[cam->method][i].size != -1; i++) {
-		err =
-		    send_control_msg(udev, 1, init[cam->method][i].value,
-				     0, init[cam->method][i].bytes,
-				     init[cam->method][i].size);
-		if (err < 0) {
-			dev_err(&cam->udev->dev,
-				"error during open sequence: %d\n", i);
-			goto out;
-		}
-	}
-
-	cam->skip = 2;
-	cam->users++;
-	file->private_data = vdev;
-	cam->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	cam->fmt = formats;
-
-	videobuf_queue_vmalloc_init(&cam->vb_vidq, &zr364xx_video_qops,
-				    NULL, &cam->slock,
-				    cam->type,
-				    V4L2_FIELD_NONE,
-				    sizeof(struct zr364xx_buffer), cam, NULL);
 
 	/* Added some delay here, since opening/closing the camera quickly,
 	 * like Ekiga does during its startup, can crash the webcam
@@ -1308,29 +1210,20 @@ static int zr364xx_open(struct file *file)
 	err = 0;
 
 out:
-	mutex_unlock(&cam->open_lock);
+	mutex_unlock(&cam->lock);
 	DBG("%s: %d\n", __func__, err);
 	return err;
 }
 
-static void zr364xx_destroy(struct zr364xx_camera *cam)
+static void zr364xx_release(struct v4l2_device *v4l2_dev)
 {
+	struct zr364xx_camera *cam =
+		container_of(v4l2_dev, struct zr364xx_camera, v4l2_dev);
 	unsigned long i;
 
-	if (!cam) {
-		printk(KERN_ERR KBUILD_MODNAME ", %s: no device\n", __func__);
-		return;
-	}
-	mutex_lock(&cam->open_lock);
-	if (cam->vdev)
-		video_unregister_device(cam->vdev);
-	cam->vdev = NULL;
-
-	/* stops the read pipe if it is running */
-	if (cam->b_acquire)
-		zr364xx_stop_acquire(cam);
+	v4l2_device_unregister(&cam->v4l2_dev);
 
-	zr364xx_stop_readpipe(cam);
+	videobuf_mmap_free(&cam->vb_vidq);
 
 	/* release sys buffers */
 	for (i = 0; i < FRAMES; i++) {
@@ -1341,62 +1234,45 @@ static void zr364xx_destroy(struct zr364xx_camera *cam)
 		cam->buffer.frame[i].lpvbits = NULL;
 	}
 
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
 	/* release transfer buffer */
 	kfree(cam->pipe->transfer_buffer);
-	cam->pipe->transfer_buffer = NULL;
-	mutex_unlock(&cam->open_lock);
 	kfree(cam);
-	cam = NULL;
 }
 
 /* release the camera */
-static int zr364xx_release(struct file *file)
+static int zr364xx_close(struct file *file)
 {
 	struct zr364xx_camera *cam;
 	struct usb_device *udev;
-	int i, err;
+	int i;
 
 	DBG("%s\n", __func__);
 	cam = video_drvdata(file);
 
-	if (!cam)
-		return -ENODEV;
-
-	mutex_lock(&cam->open_lock);
+	mutex_lock(&cam->lock);
 	udev = cam->udev;
 
-	/* turn off stream */
-	if (res_check(cam)) {
+	if (file->private_data == cam->owner) {
+		/* turn off stream */
 		if (cam->b_acquire)
 			zr364xx_stop_acquire(cam);
 		videobuf_streamoff(&cam->vb_vidq);
-		res_free(cam);
-	}
-
-	cam->users--;
-	file->private_data = NULL;
 
-	for (i = 0; i < 2; i++) {
-		err =
-		    send_control_msg(udev, 1, init[cam->method][i].value,
-				     0, init[cam->method][i].bytes,
-				     init[cam->method][i].size);
-		if (err < 0) {
-			dev_err(&udev->dev, "error during release sequence\n");
-			goto out;
+		for (i = 0; i < 2; i++) {
+			send_control_msg(udev, 1, init[cam->method][i].value,
+					0, init[cam->method][i].bytes,
+					init[cam->method][i].size);
 		}
+		cam->owner = NULL;
 	}
 
 	/* Added some delay here, since opening/closing the camera quickly,
 	 * like Ekiga does during its startup, can crash the webcam
 	 */
 	mdelay(100);
-	err = 0;
-
-out:
-	mutex_unlock(&cam->open_lock);
-
-	return err;
+	mutex_unlock(&cam->lock);
+	return v4l2_fh_release(file);
 }
 
 
@@ -1424,21 +1300,24 @@ static unsigned int zr364xx_poll(struct file *file,
 {
 	struct zr364xx_camera *cam = video_drvdata(file);
 	struct videobuf_queue *q = &cam->vb_vidq;
-	_DBG("%s\n", __func__);
+	unsigned res = v4l2_ctrl_poll(file, wait);
 
-	if (cam->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return POLLERR;
+	_DBG("%s\n", __func__);
 
-	return videobuf_poll_stream(file, q, wait);
+	return res | videobuf_poll_stream(file, q, wait);
 }
 
+static const struct v4l2_ctrl_ops zr364xx_ctrl_ops = {
+	.s_ctrl = zr364xx_s_ctrl,
+};
+
 static const struct v4l2_file_operations zr364xx_fops = {
 	.owner = THIS_MODULE,
 	.open = zr364xx_open,
-	.release = zr364xx_release,
+	.release = zr364xx_close,
 	.read = zr364xx_read,
 	.mmap = zr364xx_mmap,
-	.ioctl = video_ioctl2,
+	.unlocked_ioctl = video_ioctl2,
 	.poll = zr364xx_poll,
 };
 
@@ -1453,20 +1332,20 @@ static const struct v4l2_ioctl_ops zr364xx_ioctl_ops = {
 	.vidioc_s_input		= zr364xx_vidioc_s_input,
 	.vidioc_streamon	= zr364xx_vidioc_streamon,
 	.vidioc_streamoff	= zr364xx_vidioc_streamoff,
-	.vidioc_queryctrl	= zr364xx_vidioc_queryctrl,
-	.vidioc_g_ctrl		= zr364xx_vidioc_g_ctrl,
-	.vidioc_s_ctrl		= zr364xx_vidioc_s_ctrl,
 	.vidioc_reqbufs         = zr364xx_vidioc_reqbufs,
 	.vidioc_querybuf        = zr364xx_vidioc_querybuf,
 	.vidioc_qbuf            = zr364xx_vidioc_qbuf,
 	.vidioc_dqbuf           = zr364xx_vidioc_dqbuf,
+	.vidioc_log_status      = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device zr364xx_template = {
 	.name = DRIVER_DESC,
 	.fops = &zr364xx_fops,
 	.ioctl_ops = &zr364xx_ioctl_ops,
-	.release = video_device_release,
+	.release = video_device_release_empty,
 };
 
 
@@ -1540,6 +1419,7 @@ static int zr364xx_probe(struct usb_interface *intf,
 	struct zr364xx_camera *cam = NULL;
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
+	struct v4l2_ctrl_handler *hdl;
 	int err;
 	int i;
 
@@ -1555,21 +1435,34 @@ static int zr364xx_probe(struct usb_interface *intf,
 		dev_err(&udev->dev, "cam: out of memory !\n");
 		return -ENOMEM;
 	}
-	/* save the init method used by this camera */
-	cam->method = id->driver_info;
 
-	cam->vdev = video_device_alloc();
-	if (cam->vdev == NULL) {
-		dev_err(&udev->dev, "cam->vdev: out of memory !\n");
+	cam->v4l2_dev.release = zr364xx_release;
+	err = v4l2_device_register(&intf->dev, &cam->v4l2_dev);
+	if (err < 0) {
+		dev_err(&udev->dev, "couldn't register v4l2_device\n");
 		kfree(cam);
-		cam = NULL;
-		return -ENOMEM;
+		return err;
 	}
-	memcpy(cam->vdev, &zr364xx_template, sizeof(zr364xx_template));
-	cam->vdev->parent = &intf->dev;
-	video_set_drvdata(cam->vdev, cam);
+	hdl = &cam->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 1);
+	v4l2_ctrl_new_std(hdl, &zr364xx_ctrl_ops,
+			  V4L2_CID_BRIGHTNESS, 0, 127, 1, 64);
+	if (hdl->error) {
+		err = hdl->error;
+		dev_err(&udev->dev, "couldn't register control\n");
+		goto fail;
+	}
+	/* save the init method used by this camera */
+	cam->method = id->driver_info;
+	mutex_init(&cam->lock);
+	cam->vdev = zr364xx_template;
+	cam->vdev.lock = &cam->lock;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	cam->vdev.ctrl_handler = &cam->ctrl_handler;
+	set_bit(V4L2_FL_USE_FH_PRIO, &cam->vdev.flags);
+	video_set_drvdata(&cam->vdev, cam);
 	if (debug)
-		cam->vdev->debug = V4L2_DEBUG_IOCTL | V4L2_DEBUG_IOCTL_ARG;
+		cam->vdev.debug = V4L2_DEBUG_IOCTL | V4L2_DEBUG_IOCTL_ARG;
 
 	cam->udev = udev;
 
@@ -1615,11 +1508,7 @@ static int zr364xx_probe(struct usb_interface *intf,
 	header2[439] = cam->width / 256;
 	header2[440] = cam->width % 256;
 
-	cam->users = 0;
 	cam->nb = 0;
-	cam->mode.brightness = 64;
-	mutex_init(&cam->lock);
-	mutex_init(&cam->open_lock);
 
 	DBG("dev: %p, udev %p interface %p\n", cam, cam->udev, intf);
 
@@ -1635,52 +1524,100 @@ static int zr364xx_probe(struct usb_interface *intf,
 	}
 
 	if (!cam->read_endpoint) {
+		err = -ENOMEM;
 		dev_err(&intf->dev, "Could not find bulk-in endpoint\n");
-		video_device_release(cam->vdev);
-		kfree(cam);
-		cam = NULL;
-		return -ENOMEM;
+		goto fail;
 	}
 
 	/* v4l */
 	INIT_LIST_HEAD(&cam->vidq.active);
 	cam->vidq.cam = cam;
-	err = video_register_device(cam->vdev, VFL_TYPE_GRABBER, -1);
-	if (err) {
-		dev_err(&udev->dev, "video_register_device failed\n");
-		video_device_release(cam->vdev);
-		kfree(cam);
-		cam = NULL;
-		return err;
-	}
 
 	usb_set_intfdata(intf, cam);
 
 	/* load zr364xx board specific */
 	err = zr364xx_board_init(cam);
-	if (err) {
-		spin_lock_init(&cam->slock);
-		return err;
-	}
+	if (!err)
+		err = v4l2_ctrl_handler_setup(hdl);
+	if (err)
+		goto fail;
 
 	spin_lock_init(&cam->slock);
 
+	cam->fmt = formats;
+
+	videobuf_queue_vmalloc_init(&cam->vb_vidq, &zr364xx_video_qops,
+				    NULL, &cam->slock,
+				    V4L2_BUF_TYPE_VIDEO_CAPTURE,
+				    V4L2_FIELD_NONE,
+				    sizeof(struct zr364xx_buffer), cam, &cam->lock);
+
+	err = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
+	if (err) {
+		dev_err(&udev->dev, "video_register_device failed\n");
+		goto fail;
+	}
+
 	dev_info(&udev->dev, DRIVER_DESC " controlling device %s\n",
-		 video_device_node_name(cam->vdev));
+		 video_device_node_name(&cam->vdev));
 	return 0;
+
+fail:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	kfree(cam);
+	return err;
 }
 
 
 static void zr364xx_disconnect(struct usb_interface *intf)
 {
 	struct zr364xx_camera *cam = usb_get_intfdata(intf);
-	videobuf_mmap_free(&cam->vb_vidq);
+
+	mutex_lock(&cam->lock);
 	usb_set_intfdata(intf, NULL);
 	dev_info(&intf->dev, DRIVER_DESC " webcam unplugged\n");
-	zr364xx_destroy(cam);
+	video_unregister_device(&cam->vdev);
+	v4l2_device_disconnect(&cam->v4l2_dev);
+
+	/* stops the read pipe if it is running */
+	if (cam->b_acquire)
+		zr364xx_stop_acquire(cam);
+
+	zr364xx_stop_readpipe(cam);
+	mutex_unlock(&cam->lock);
+	v4l2_device_put(&cam->v4l2_dev);
 }
 
 
+#ifdef CONFIG_PM
+static int zr364xx_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct zr364xx_camera *cam = usb_get_intfdata(intf);
+
+	cam->was_streaming = cam->b_acquire;
+	if (!cam->was_streaming)
+		return 0;
+	zr364xx_stop_acquire(cam);
+	zr364xx_stop_readpipe(cam);
+	return 0;
+}
+
+static int zr364xx_resume(struct usb_interface *intf)
+{
+	struct zr364xx_camera *cam = usb_get_intfdata(intf);
+	int res;
+
+	if (!cam->was_streaming)
+		return 0;
+
+	zr364xx_start_readpipe(cam);
+	res = zr364xx_prepare(cam);
+	if (!res)
+		zr364xx_start_acquire(cam);
+	return res;
+}
+#endif
 
 /**********************/
 /* Module integration */
@@ -1690,6 +1627,11 @@ static struct usb_driver zr364xx_driver = {
 	.name = "zr364xx",
 	.probe = zr364xx_probe,
 	.disconnect = zr364xx_disconnect,
+#ifdef CONFIG_PM
+	.suspend = zr364xx_suspend,
+	.resume = zr364xx_resume,
+	.reset_resume = zr364xx_resume,
+#endif
 	.id_table = device_table
 };
 
diff --git a/drivers/message/i2o/i2o_config.c b/drivers/message/i2o/i2o_config.c
index 098de2b3578..9a49c243a6a 100644
--- a/drivers/message/i2o/i2o_config.c
+++ b/drivers/message/i2o/i2o_config.c
@@ -188,6 +188,13 @@ static int i2o_cfg_parms(unsigned long arg, unsigned int type)
 	if (!dev)
 		return -ENXIO;
 
+	/*
+	 * Stop users being able to try and allocate arbitary amounts
+	 * of DMA space. 64K is way more than sufficient for this.
+	 */
+	if (kcmd.oplen > 65536)
+		return -EMSGSIZE;
+
 	ops = memdup_user(kcmd.opbuf, kcmd.oplen);
 	if (IS_ERR(ops))
 		return PTR_ERR(ops);
diff --git a/drivers/message/i2o/i2o_proc.c b/drivers/message/i2o/i2o_proc.c
index 506c36f6e1d..8001aa6bfb4 100644
--- a/drivers/message/i2o/i2o_proc.c
+++ b/drivers/message/i2o/i2o_proc.c
@@ -255,9 +255,8 @@ static char *scsi_devices[] = {
 	"Array Controller Device"
 };
 
-static char *chtostr(u8 * chars, int n)
+static char *chtostr(char *tmp, u8 *chars, int n)
 {
-	char tmp[256];
 	tmp[0] = 0;
 	return strncat(tmp, (char *)chars, n);
 }
@@ -791,6 +790,7 @@ static int i2o_seq_show_ddm_table(struct seq_file *seq, void *v)
 	} *result;
 
 	i2o_exec_execute_ddm_table ddm_table;
+	char tmp[28 + 1];
 
 	result = kmalloc(sizeof(*result), GFP_KERNEL);
 	if (!result)
@@ -826,7 +826,7 @@ static int i2o_seq_show_ddm_table(struct seq_file *seq, void *v)
 		seq_printf(seq, "%-#7x", ddm_table.i2o_vendor_id);
 		seq_printf(seq, "%-#8x", ddm_table.module_id);
 		seq_printf(seq, "%-29s",
-			   chtostr(ddm_table.module_name_version, 28));
+			   chtostr(tmp, ddm_table.module_name_version, 28));
 		seq_printf(seq, "%9d  ", ddm_table.data_size);
 		seq_printf(seq, "%8d", ddm_table.code_size);
 
@@ -893,6 +893,7 @@ static int i2o_seq_show_drivers_stored(struct seq_file *seq, void *v)
 
 	i2o_driver_result_table *result;
 	i2o_driver_store_table *dst;
+	char tmp[28 + 1];
 
 	result = kmalloc(sizeof(i2o_driver_result_table), GFP_KERNEL);
 	if (result == NULL)
@@ -927,8 +928,9 @@ static int i2o_seq_show_drivers_stored(struct seq_file *seq, void *v)
 
 		seq_printf(seq, "%-#7x", dst->i2o_vendor_id);
 		seq_printf(seq, "%-#8x", dst->module_id);
-		seq_printf(seq, "%-29s", chtostr(dst->module_name_version, 28));
-		seq_printf(seq, "%-9s", chtostr(dst->date, 8));
+		seq_printf(seq, "%-29s",
+			   chtostr(tmp, dst->module_name_version, 28));
+		seq_printf(seq, "%-9s", chtostr(tmp, dst->date, 8));
 		seq_printf(seq, "%8d ", dst->module_size);
 		seq_printf(seq, "%8d ", dst->mpb_size);
 		seq_printf(seq, "0x%04x", dst->module_flags);
@@ -1248,6 +1250,7 @@ static int i2o_seq_show_dev_identity(struct seq_file *seq, void *v)
 	// == (allow) 512d bytes (max)
 	static u16 *work16 = (u16 *) work32;
 	int token;
+	char tmp[16 + 1];
 
 	token = i2o_parm_field_get(d, 0xF100, -1, &work32, sizeof(work32));
 
@@ -1260,13 +1263,13 @@ static int i2o_seq_show_dev_identity(struct seq_file *seq, void *v)
 	seq_printf(seq, "Owner TID     : %0#5x\n", work16[2]);
 	seq_printf(seq, "Parent TID    : %0#5x\n", work16[3]);
 	seq_printf(seq, "Vendor info   : %s\n",
-		   chtostr((u8 *) (work32 + 2), 16));
+		   chtostr(tmp, (u8 *) (work32 + 2), 16));
 	seq_printf(seq, "Product info  : %s\n",
-		   chtostr((u8 *) (work32 + 6), 16));
+		   chtostr(tmp, (u8 *) (work32 + 6), 16));
 	seq_printf(seq, "Description   : %s\n",
-		   chtostr((u8 *) (work32 + 10), 16));
+		   chtostr(tmp, (u8 *) (work32 + 10), 16));
 	seq_printf(seq, "Product rev.  : %s\n",
-		   chtostr((u8 *) (work32 + 14), 8));
+		   chtostr(tmp, (u8 *) (work32 + 14), 8));
 
 	seq_printf(seq, "Serial number : ");
 	print_serial_number(seq, (u8 *) (work32 + 16),
@@ -1303,6 +1306,8 @@ static int i2o_seq_show_ddm_identity(struct seq_file *seq, void *v)
 		u8 pad[256];	// allow up to 256 byte (max) serial number
 	} result;
 
+	char tmp[24 + 1];
+
 	token = i2o_parm_field_get(d, 0xF101, -1, &result, sizeof(result));
 
 	if (token < 0) {
@@ -1312,9 +1317,9 @@ static int i2o_seq_show_ddm_identity(struct seq_file *seq, void *v)
 
 	seq_printf(seq, "Registering DDM TID : 0x%03x\n", result.ddm_tid);
 	seq_printf(seq, "Module name         : %s\n",
-		   chtostr(result.module_name, 24));
+		   chtostr(tmp, result.module_name, 24));
 	seq_printf(seq, "Module revision     : %s\n",
-		   chtostr(result.module_rev, 8));
+		   chtostr(tmp, result.module_rev, 8));
 
 	seq_printf(seq, "Serial number       : ");
 	print_serial_number(seq, result.serial_number, sizeof(result) - 36);
@@ -1338,6 +1343,8 @@ static int i2o_seq_show_uinfo(struct seq_file *seq, void *v)
 		u8 instance_number[4];
 	} result;
 
+	char tmp[64 + 1];
+
 	token = i2o_parm_field_get(d, 0xF102, -1, &result, sizeof(result));
 
 	if (token < 0) {
@@ -1346,13 +1353,13 @@ static int i2o_seq_show_uinfo(struct seq_file *seq, void *v)
 	}
 
 	seq_printf(seq, "Device name     : %s\n",
-		   chtostr(result.device_name, 64));
+		   chtostr(tmp, result.device_name, 64));
 	seq_printf(seq, "Service name    : %s\n",
-		   chtostr(result.service_name, 64));
+		   chtostr(tmp, result.service_name, 64));
 	seq_printf(seq, "Physical name   : %s\n",
-		   chtostr(result.physical_location, 64));
+		   chtostr(tmp, result.physical_location, 64));
 	seq_printf(seq, "Instance number : %s\n",
-		   chtostr(result.instance_number, 4));
+		   chtostr(tmp, result.instance_number, 4));
 
 	return 0;
 }
diff --git a/drivers/mfd/88pm800.c b/drivers/mfd/88pm800.c
new file mode 100644
index 00000000000..b67a3018b13
--- /dev/null
+++ b/drivers/mfd/88pm800.c
@@ -0,0 +1,596 @@
+/*
+ * Base driver for Marvell 88PM800
+ *
+ * Copyright (C) 2012 Marvell International Ltd.
+ * Haojian Zhuang <haojian.zhuang@marvell.com>
+ * Joseph(Yossi) Hanin <yhanin@marvell.com>
+ * Qiao Zhou <zhouqiao@marvell.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License. See the file "COPYING" in the main directory of this
+ * archive for more details.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/88pm80x.h>
+#include <linux/slab.h>
+
+#define PM800_CHIP_ID			(0x00)
+
+/* Interrupt Registers */
+#define PM800_INT_STATUS1		(0x05)
+#define PM800_ONKEY_INT_STS1		(1 << 0)
+#define PM800_EXTON_INT_STS1		(1 << 1)
+#define PM800_CHG_INT_STS1			(1 << 2)
+#define PM800_BAT_INT_STS1			(1 << 3)
+#define PM800_RTC_INT_STS1			(1 << 4)
+#define PM800_CLASSD_OC_INT_STS1	(1 << 5)
+
+#define PM800_INT_STATUS2		(0x06)
+#define PM800_VBAT_INT_STS2		(1 << 0)
+#define PM800_VSYS_INT_STS2		(1 << 1)
+#define PM800_VCHG_INT_STS2		(1 << 2)
+#define PM800_TINT_INT_STS2		(1 << 3)
+#define PM800_GPADC0_INT_STS2	(1 << 4)
+#define PM800_TBAT_INT_STS2		(1 << 5)
+#define PM800_GPADC2_INT_STS2	(1 << 6)
+#define PM800_GPADC3_INT_STS2	(1 << 7)
+
+#define PM800_INT_STATUS3		(0x07)
+
+#define PM800_INT_STATUS4		(0x08)
+#define PM800_GPIO0_INT_STS4		(1 << 0)
+#define PM800_GPIO1_INT_STS4		(1 << 1)
+#define PM800_GPIO2_INT_STS4		(1 << 2)
+#define PM800_GPIO3_INT_STS4		(1 << 3)
+#define PM800_GPIO4_INT_STS4		(1 << 4)
+
+#define PM800_INT_ENA_1		(0x09)
+#define PM800_ONKEY_INT_ENA1		(1 << 0)
+#define PM800_EXTON_INT_ENA1		(1 << 1)
+#define PM800_CHG_INT_ENA1			(1 << 2)
+#define PM800_BAT_INT_ENA1			(1 << 3)
+#define PM800_RTC_INT_ENA1			(1 << 4)
+#define PM800_CLASSD_OC_INT_ENA1	(1 << 5)
+
+#define PM800_INT_ENA_2		(0x0A)
+#define PM800_VBAT_INT_ENA2		(1 << 0)
+#define PM800_VSYS_INT_ENA2		(1 << 1)
+#define PM800_VCHG_INT_ENA2		(1 << 2)
+#define PM800_TINT_INT_ENA2		(1 << 3)
+
+#define PM800_INT_ENA_3		(0x0B)
+#define PM800_GPADC0_INT_ENA3		(1 << 0)
+#define PM800_GPADC1_INT_ENA3		(1 << 1)
+#define PM800_GPADC2_INT_ENA3		(1 << 2)
+#define PM800_GPADC3_INT_ENA3		(1 << 3)
+#define PM800_GPADC4_INT_ENA3		(1 << 4)
+
+#define PM800_INT_ENA_4		(0x0C)
+#define PM800_GPIO0_INT_ENA4		(1 << 0)
+#define PM800_GPIO1_INT_ENA4		(1 << 1)
+#define PM800_GPIO2_INT_ENA4		(1 << 2)
+#define PM800_GPIO3_INT_ENA4		(1 << 3)
+#define PM800_GPIO4_INT_ENA4		(1 << 4)
+
+/* number of INT_ENA & INT_STATUS regs */
+#define PM800_INT_REG_NUM			(4)
+
+/* Interrupt Number in 88PM800 */
+enum {
+	PM800_IRQ_ONKEY,	/*EN1b0 *//*0 */
+	PM800_IRQ_EXTON,	/*EN1b1 */
+	PM800_IRQ_CHG,		/*EN1b2 */
+	PM800_IRQ_BAT,		/*EN1b3 */
+	PM800_IRQ_RTC,		/*EN1b4 */
+	PM800_IRQ_CLASSD,	/*EN1b5 *//*5 */
+	PM800_IRQ_VBAT,		/*EN2b0 */
+	PM800_IRQ_VSYS,		/*EN2b1 */
+	PM800_IRQ_VCHG,		/*EN2b2 */
+	PM800_IRQ_TINT,		/*EN2b3 */
+	PM800_IRQ_GPADC0,	/*EN3b0 *//*10 */
+	PM800_IRQ_GPADC1,	/*EN3b1 */
+	PM800_IRQ_GPADC2,	/*EN3b2 */
+	PM800_IRQ_GPADC3,	/*EN3b3 */
+	PM800_IRQ_GPADC4,	/*EN3b4 */
+	PM800_IRQ_GPIO0,	/*EN4b0 *//*15 */
+	PM800_IRQ_GPIO1,	/*EN4b1 */
+	PM800_IRQ_GPIO2,	/*EN4b2 */
+	PM800_IRQ_GPIO3,	/*EN4b3 */
+	PM800_IRQ_GPIO4,	/*EN4b4 *//*19 */
+	PM800_MAX_IRQ,
+};
+
+enum {
+	/* Procida */
+	PM800_CHIP_A0  = 0x60,
+	PM800_CHIP_A1  = 0x61,
+	PM800_CHIP_B0  = 0x62,
+	PM800_CHIP_C0  = 0x63,
+	PM800_CHIP_END = PM800_CHIP_C0,
+
+	/* Make sure to update this to the last stepping */
+	PM8XXX_CHIP_END = PM800_CHIP_END
+};
+
+static const struct i2c_device_id pm80x_id_table[] = {
+	{"88PM800", CHIP_PM800},
+	{} /* NULL terminated */
+};
+MODULE_DEVICE_TABLE(i2c, pm80x_id_table);
+
+static struct resource rtc_resources[] = {
+	{
+	 .name = "88pm80x-rtc",
+	 .start = PM800_IRQ_RTC,
+	 .end = PM800_IRQ_RTC,
+	 .flags = IORESOURCE_IRQ,
+	 },
+};
+
+static struct mfd_cell rtc_devs[] = {
+	{
+	 .name = "88pm80x-rtc",
+	 .num_resources = ARRAY_SIZE(rtc_resources),
+	 .resources = &rtc_resources[0],
+	 .id = -1,
+	 },
+};
+
+static struct resource onkey_resources[] = {
+	{
+	 .name = "88pm80x-onkey",
+	 .start = PM800_IRQ_ONKEY,
+	 .end = PM800_IRQ_ONKEY,
+	 .flags = IORESOURCE_IRQ,
+	 },
+};
+
+static struct mfd_cell onkey_devs[] = {
+	{
+	 .name = "88pm80x-onkey",
+	 .num_resources = 1,
+	 .resources = &onkey_resources[0],
+	 .id = -1,
+	 },
+};
+
+static const struct regmap_irq pm800_irqs[] = {
+	/* INT0 */
+	[PM800_IRQ_ONKEY] = {
+		.mask = PM800_ONKEY_INT_ENA1,
+	},
+	[PM800_IRQ_EXTON] = {
+		.mask = PM800_EXTON_INT_ENA1,
+	},
+	[PM800_IRQ_CHG] = {
+		.mask = PM800_CHG_INT_ENA1,
+	},
+	[PM800_IRQ_BAT] = {
+		.mask = PM800_BAT_INT_ENA1,
+	},
+	[PM800_IRQ_RTC] = {
+		.mask = PM800_RTC_INT_ENA1,
+	},
+	[PM800_IRQ_CLASSD] = {
+		.mask = PM800_CLASSD_OC_INT_ENA1,
+	},
+	/* INT1 */
+	[PM800_IRQ_VBAT] = {
+		.reg_offset = 1,
+		.mask = PM800_VBAT_INT_ENA2,
+	},
+	[PM800_IRQ_VSYS] = {
+		.reg_offset = 1,
+		.mask = PM800_VSYS_INT_ENA2,
+	},
+	[PM800_IRQ_VCHG] = {
+		.reg_offset = 1,
+		.mask = PM800_VCHG_INT_ENA2,
+	},
+	[PM800_IRQ_TINT] = {
+		.reg_offset = 1,
+		.mask = PM800_TINT_INT_ENA2,
+	},
+	/* INT2 */
+	[PM800_IRQ_GPADC0] = {
+		.reg_offset = 2,
+		.mask = PM800_GPADC0_INT_ENA3,
+	},
+	[PM800_IRQ_GPADC1] = {
+		.reg_offset = 2,
+		.mask = PM800_GPADC1_INT_ENA3,
+	},
+	[PM800_IRQ_GPADC2] = {
+		.reg_offset = 2,
+		.mask = PM800_GPADC2_INT_ENA3,
+	},
+	[PM800_IRQ_GPADC3] = {
+		.reg_offset = 2,
+		.mask = PM800_GPADC3_INT_ENA3,
+	},
+	[PM800_IRQ_GPADC4] = {
+		.reg_offset = 2,
+		.mask = PM800_GPADC4_INT_ENA3,
+	},
+	/* INT3 */
+	[PM800_IRQ_GPIO0] = {
+		.reg_offset = 3,
+		.mask = PM800_GPIO0_INT_ENA4,
+	},
+	[PM800_IRQ_GPIO1] = {
+		.reg_offset = 3,
+		.mask = PM800_GPIO1_INT_ENA4,
+	},
+	[PM800_IRQ_GPIO2] = {
+		.reg_offset = 3,
+		.mask = PM800_GPIO2_INT_ENA4,
+	},
+	[PM800_IRQ_GPIO3] = {
+		.reg_offset = 3,
+		.mask = PM800_GPIO3_INT_ENA4,
+	},
+	[PM800_IRQ_GPIO4] = {
+		.reg_offset = 3,
+		.mask = PM800_GPIO4_INT_ENA4,
+	},
+};
+
+static int __devinit device_gpadc_init(struct pm80x_chip *chip,
+				       struct pm80x_platform_data *pdata)
+{
+	struct pm80x_subchip *subchip = chip->subchip;
+	struct regmap *map = subchip->regmap_gpadc;
+	int data = 0, mask = 0, ret = 0;
+
+	if (!map) {
+		dev_warn(chip->dev,
+			 "Warning: gpadc regmap is not available!\n");
+		return -EINVAL;
+	}
+	/*
+	 * initialize GPADC without activating it turn on GPADC
+	 * measurments
+	 */
+	ret = regmap_update_bits(map,
+				 PM800_GPADC_MISC_CONFIG2,
+				 PM800_GPADC_MISC_GPFSM_EN,
+				 PM800_GPADC_MISC_GPFSM_EN);
+	if (ret < 0)
+		goto out;
+	/*
+	 * This function configures the ADC as requires for
+	 * CP implementation.CP does not "own" the ADC configuration
+	 * registers and relies on AP.
+	 * Reason: enable automatic ADC measurements needed
+	 * for CP to get VBAT and RF temperature readings.
+	 */
+	ret = regmap_update_bits(map, PM800_GPADC_MEAS_EN1,
+				 PM800_MEAS_EN1_VBAT, PM800_MEAS_EN1_VBAT);
+	if (ret < 0)
+		goto out;
+	ret = regmap_update_bits(map, PM800_GPADC_MEAS_EN2,
+				 (PM800_MEAS_EN2_RFTMP | PM800_MEAS_GP0_EN),
+				 (PM800_MEAS_EN2_RFTMP | PM800_MEAS_GP0_EN));
+	if (ret < 0)
+		goto out;
+
+	/*
+	 * the defult of PM800 is GPADC operates at 100Ks/s rate
+	 * and Number of GPADC slots with active current bias prior
+	 * to GPADC sampling = 1 slot for all GPADCs set for
+	 * Temprature mesurmants
+	 */
+	mask = (PM800_GPADC_GP_BIAS_EN0 | PM800_GPADC_GP_BIAS_EN1 |
+		PM800_GPADC_GP_BIAS_EN2 | PM800_GPADC_GP_BIAS_EN3);
+
+	if (pdata && (pdata->batt_det == 0))
+		data = (PM800_GPADC_GP_BIAS_EN0 | PM800_GPADC_GP_BIAS_EN1 |
+			PM800_GPADC_GP_BIAS_EN2 | PM800_GPADC_GP_BIAS_EN3);
+	else
+		data = (PM800_GPADC_GP_BIAS_EN0 | PM800_GPADC_GP_BIAS_EN2 |
+			PM800_GPADC_GP_BIAS_EN3);
+
+	ret = regmap_update_bits(map, PM800_GP_BIAS_ENA1, mask, data);
+	if (ret < 0)
+		goto out;
+
+	dev_info(chip->dev, "pm800 device_gpadc_init: Done\n");
+	return 0;
+
+out:
+	dev_info(chip->dev, "pm800 device_gpadc_init: Failed!\n");
+	return ret;
+}
+
+static int __devinit device_irq_init_800(struct pm80x_chip *chip)
+{
+	struct regmap *map = chip->regmap;
+	unsigned long flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT;
+	int data, mask, ret = -EINVAL;
+
+	if (!map || !chip->irq) {
+		dev_err(chip->dev, "incorrect parameters\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * irq_mode defines the way of clearing interrupt. it's read-clear by
+	 * default.
+	 */
+	mask =
+	    PM800_WAKEUP2_INV_INT | PM800_WAKEUP2_INT_CLEAR |
+	    PM800_WAKEUP2_INT_MASK;
+
+	data = PM800_WAKEUP2_INT_CLEAR;
+	ret = regmap_update_bits(map, PM800_WAKEUP2, mask, data);
+
+	if (ret < 0)
+		goto out;
+
+	ret =
+	    regmap_add_irq_chip(chip->regmap, chip->irq, flags, -1,
+				chip->regmap_irq_chip, &chip->irq_data);
+
+out:
+	return ret;
+}
+
+static void device_irq_exit_800(struct pm80x_chip *chip)
+{
+	regmap_del_irq_chip(chip->irq, chip->irq_data);
+}
+
+static struct regmap_irq_chip pm800_irq_chip = {
+	.name = "88pm800",
+	.irqs = pm800_irqs,
+	.num_irqs = ARRAY_SIZE(pm800_irqs),
+
+	.num_regs = 4,
+	.status_base = PM800_INT_STATUS1,
+	.mask_base = PM800_INT_ENA_1,
+	.ack_base = PM800_INT_STATUS1,
+};
+
+static int pm800_pages_init(struct pm80x_chip *chip)
+{
+	struct pm80x_subchip *subchip;
+	struct i2c_client *client = chip->client;
+
+	subchip = chip->subchip;
+	/* PM800 block power: i2c addr 0x31 */
+	if (subchip->power_page_addr) {
+		subchip->power_page =
+		    i2c_new_dummy(client->adapter, subchip->power_page_addr);
+		subchip->regmap_power =
+		    devm_regmap_init_i2c(subchip->power_page,
+					 &pm80x_regmap_config);
+		i2c_set_clientdata(subchip->power_page, chip);
+	} else
+		dev_info(chip->dev,
+			 "PM800 block power 0x31: No power_page_addr\n");
+
+	/* PM800 block GPADC: i2c addr 0x32 */
+	if (subchip->gpadc_page_addr) {
+		subchip->gpadc_page = i2c_new_dummy(client->adapter,
+						    subchip->gpadc_page_addr);
+		subchip->regmap_gpadc =
+		    devm_regmap_init_i2c(subchip->gpadc_page,
+					 &pm80x_regmap_config);
+		i2c_set_clientdata(subchip->gpadc_page, chip);
+	} else
+		dev_info(chip->dev,
+			 "PM800 block GPADC 0x32: No gpadc_page_addr\n");
+
+	return 0;
+}
+
+static void pm800_pages_exit(struct pm80x_chip *chip)
+{
+	struct pm80x_subchip *subchip;
+
+	regmap_exit(chip->regmap);
+	i2c_unregister_device(chip->client);
+
+	subchip = chip->subchip;
+	if (subchip->power_page) {
+		regmap_exit(subchip->regmap_power);
+		i2c_unregister_device(subchip->power_page);
+	}
+	if (subchip->gpadc_page) {
+		regmap_exit(subchip->regmap_gpadc);
+		i2c_unregister_device(subchip->gpadc_page);
+	}
+}
+
+static int __devinit device_800_init(struct pm80x_chip *chip,
+				     struct pm80x_platform_data *pdata)
+{
+	int ret, pmic_id;
+	unsigned int val;
+
+	ret = regmap_read(chip->regmap, PM800_CHIP_ID, &val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read CHIP ID: %d\n", ret);
+		goto out;
+	}
+
+	pmic_id = val & PM80X_VERSION_MASK;
+
+	if ((pmic_id >= PM800_CHIP_A0) && (pmic_id <= PM800_CHIP_END)) {
+		chip->version = val;
+		dev_info(chip->dev,
+			 "88PM80x:Marvell 88PM800 (ID:0x%x) detected\n", val);
+	} else {
+		dev_err(chip->dev,
+			"Failed to detect Marvell 88PM800:ChipID[0x%x]\n", val);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * alarm wake up bit will be clear in device_irq_init(),
+	 * read before that
+	 */
+	ret = regmap_read(chip->regmap, PM800_RTC_CONTROL, &val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read RTC register: %d\n", ret);
+		goto out;
+	}
+	if (val & PM800_ALARM_WAKEUP) {
+		if (pdata && pdata->rtc)
+			pdata->rtc->rtc_wakeup = 1;
+	}
+
+	ret = device_gpadc_init(chip, pdata);
+	if (ret < 0) {
+		dev_err(chip->dev, "[%s]Failed to init gpadc\n", __func__);
+		goto out;
+	}
+
+	chip->regmap_irq_chip = &pm800_irq_chip;
+
+	ret = device_irq_init_800(chip);
+	if (ret < 0) {
+		dev_err(chip->dev, "[%s]Failed to init pm800 irq\n", __func__);
+		goto out;
+	}
+
+	ret =
+	    mfd_add_devices(chip->dev, 0, &onkey_devs[0],
+			    ARRAY_SIZE(onkey_devs), &onkey_resources[0], 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to add onkey subdev\n");
+		goto out_dev;
+	} else
+		dev_info(chip->dev, "[%s]:Added mfd onkey_devs\n", __func__);
+
+	if (pdata && pdata->rtc) {
+		rtc_devs[0].platform_data = pdata->rtc;
+		rtc_devs[0].pdata_size = sizeof(struct pm80x_rtc_pdata);
+		ret = mfd_add_devices(chip->dev, 0, &rtc_devs[0],
+				      ARRAY_SIZE(rtc_devs), NULL, 0);
+		if (ret < 0) {
+			dev_err(chip->dev, "Failed to add rtc subdev\n");
+			goto out_dev;
+		} else
+			dev_info(chip->dev,
+				 "[%s]:Added mfd rtc_devs\n", __func__);
+	}
+
+	return 0;
+out_dev:
+	mfd_remove_devices(chip->dev);
+	device_irq_exit_800(chip);
+out:
+	return ret;
+}
+
+static int __devinit pm800_probe(struct i2c_client *client,
+				 const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct pm80x_chip *chip;
+	struct pm80x_platform_data *pdata = client->dev.platform_data;
+	struct pm80x_subchip *subchip;
+
+	ret = pm80x_init(client, id);
+	if (ret) {
+		dev_err(&client->dev, "pm800_init fail\n");
+		goto out_init;
+	}
+
+	chip = i2c_get_clientdata(client);
+
+	/* init subchip for PM800 */
+	subchip =
+	    devm_kzalloc(&client->dev, sizeof(struct pm80x_subchip),
+			 GFP_KERNEL);
+	if (!subchip) {
+		ret = -ENOMEM;
+		goto err_subchip_alloc;
+	}
+
+	subchip->power_page_addr = pdata->power_page_addr;
+	subchip->gpadc_page_addr = pdata->gpadc_page_addr;
+	chip->subchip = subchip;
+
+	ret = device_800_init(chip, pdata);
+	if (ret) {
+		dev_err(chip->dev, "%s id 0x%x failed!\n", __func__, chip->id);
+		goto err_800_init;
+	}
+
+	ret = pm800_pages_init(chip);
+	if (ret) {
+		dev_err(&client->dev, "pm800_pages_init failed!\n");
+		goto err_page_init;
+	}
+
+	if (pdata->plat_config)
+		pdata->plat_config(chip, pdata);
+
+err_page_init:
+	mfd_remove_devices(chip->dev);
+	device_irq_exit_800(chip);
+err_800_init:
+	devm_kfree(&client->dev, subchip);
+err_subchip_alloc:
+	pm80x_deinit(client);
+out_init:
+	return ret;
+}
+
+static int __devexit pm800_remove(struct i2c_client *client)
+{
+	struct pm80x_chip *chip = i2c_get_clientdata(client);
+
+	mfd_remove_devices(chip->dev);
+	device_irq_exit_800(chip);
+
+	pm800_pages_exit(chip);
+	devm_kfree(&client->dev, chip->subchip);
+
+	pm80x_deinit(client);
+
+	return 0;
+}
+
+static struct i2c_driver pm800_driver = {
+	.driver = {
+		.name = "88PM80X",
+		.owner = THIS_MODULE,
+		.pm = &pm80x_pm_ops,
+		},
+	.probe = pm800_probe,
+	.remove = __devexit_p(pm800_remove),
+	.id_table = pm80x_id_table,
+};
+
+static int __init pm800_i2c_init(void)
+{
+	return i2c_add_driver(&pm800_driver);
+}
+subsys_initcall(pm800_i2c_init);
+
+static void __exit pm800_i2c_exit(void)
+{
+	i2c_del_driver(&pm800_driver);
+}
+module_exit(pm800_i2c_exit);
+
+MODULE_DESCRIPTION("PMIC Driver for Marvell 88PM800");
+MODULE_AUTHOR("Qiao Zhou <zhouqiao@marvell.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/88pm805.c b/drivers/mfd/88pm805.c
new file mode 100644
index 00000000000..6146583589f
--- /dev/null
+++ b/drivers/mfd/88pm805.c
@@ -0,0 +1,301 @@
+/*
+ * Base driver for Marvell 88PM805
+ *
+ * Copyright (C) 2012 Marvell International Ltd.
+ * Haojian Zhuang <haojian.zhuang@marvell.com>
+ * Joseph(Yossi) Hanin <yhanin@marvell.com>
+ * Qiao Zhou <zhouqiao@marvell.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License. See the file "COPYING" in the main directory of this
+ * archive for more details.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/88pm80x.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+
+#define PM805_CHIP_ID			(0x00)
+
+static const struct i2c_device_id pm80x_id_table[] = {
+	{"88PM805", CHIP_PM805},
+	{} /* NULL terminated */
+};
+MODULE_DEVICE_TABLE(i2c, pm80x_id_table);
+
+/* Interrupt Number in 88PM805 */
+enum {
+	PM805_IRQ_LDO_OFF,	/*0 */
+	PM805_IRQ_SRC_DPLL_LOCK,	/*1 */
+	PM805_IRQ_CLIP_FAULT,
+	PM805_IRQ_MIC_CONFLICT,
+	PM805_IRQ_HP2_SHRT,
+	PM805_IRQ_HP1_SHRT,	/*5 */
+	PM805_IRQ_FINE_PLL_FAULT,
+	PM805_IRQ_RAW_PLL_FAULT,
+	PM805_IRQ_VOLP_BTN_DET,
+	PM805_IRQ_VOLM_BTN_DET,
+	PM805_IRQ_SHRT_BTN_DET,	/*10 */
+	PM805_IRQ_MIC_DET,	/*11 */
+
+	PM805_MAX_IRQ,
+};
+
+static struct resource codec_resources[] = {
+	{
+	 /* Headset microphone insertion or removal */
+	 .name = "micin",
+	 .start = PM805_IRQ_MIC_DET,
+	 .end = PM805_IRQ_MIC_DET,
+	 .flags = IORESOURCE_IRQ,
+	 },
+	{
+	 /* Audio short HP1 */
+	 .name = "audio-short1",
+	 .start = PM805_IRQ_HP1_SHRT,
+	 .end = PM805_IRQ_HP1_SHRT,
+	 .flags = IORESOURCE_IRQ,
+	 },
+	{
+	 /* Audio short HP2 */
+	 .name = "audio-short2",
+	 .start = PM805_IRQ_HP2_SHRT,
+	 .end = PM805_IRQ_HP2_SHRT,
+	 .flags = IORESOURCE_IRQ,
+	 },
+};
+
+static struct mfd_cell codec_devs[] = {
+	{
+	 .name = "88pm80x-codec",
+	 .num_resources = ARRAY_SIZE(codec_resources),
+	 .resources = &codec_resources[0],
+	 .id = -1,
+	 },
+};
+
+static struct regmap_irq pm805_irqs[] = {
+	/* INT0 */
+	[PM805_IRQ_LDO_OFF] = {
+		.mask = PM805_INT1_HP1_SHRT,
+	},
+	[PM805_IRQ_SRC_DPLL_LOCK] = {
+		.mask = PM805_INT1_HP2_SHRT,
+	},
+	[PM805_IRQ_CLIP_FAULT] = {
+		.mask = PM805_INT1_MIC_CONFLICT,
+	},
+	[PM805_IRQ_MIC_CONFLICT] = {
+		.mask = PM805_INT1_CLIP_FAULT,
+	},
+	[PM805_IRQ_HP2_SHRT] = {
+		.mask = PM805_INT1_LDO_OFF,
+	},
+	[PM805_IRQ_HP1_SHRT] = {
+		.mask = PM805_INT1_SRC_DPLL_LOCK,
+	},
+	/* INT1 */
+	[PM805_IRQ_FINE_PLL_FAULT] = {
+		.reg_offset = 1,
+		.mask = PM805_INT2_MIC_DET,
+	},
+	[PM805_IRQ_RAW_PLL_FAULT] = {
+		.reg_offset = 1,
+		.mask = PM805_INT2_SHRT_BTN_DET,
+	},
+	[PM805_IRQ_VOLP_BTN_DET] = {
+		.reg_offset = 1,
+		.mask = PM805_INT2_VOLM_BTN_DET,
+	},
+	[PM805_IRQ_VOLM_BTN_DET] = {
+		.reg_offset = 1,
+		.mask = PM805_INT2_VOLP_BTN_DET,
+	},
+	[PM805_IRQ_SHRT_BTN_DET] = {
+		.reg_offset = 1,
+		.mask = PM805_INT2_RAW_PLL_FAULT,
+	},
+	[PM805_IRQ_MIC_DET] = {
+		.reg_offset = 1,
+		.mask = PM805_INT2_FINE_PLL_FAULT,
+	},
+};
+
+static int __devinit device_irq_init_805(struct pm80x_chip *chip)
+{
+	struct regmap *map = chip->regmap;
+	unsigned long flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT;
+	int data, mask, ret = -EINVAL;
+
+	if (!map || !chip->irq) {
+		dev_err(chip->dev, "incorrect parameters\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * irq_mode defines the way of clearing interrupt. it's read-clear by
+	 * default.
+	 */
+	mask =
+	    PM805_STATUS0_INT_CLEAR | PM805_STATUS0_INV_INT |
+	    PM800_STATUS0_INT_MASK;
+
+	data = PM805_STATUS0_INT_CLEAR;
+	ret = regmap_update_bits(map, PM805_INT_STATUS0, mask, data);
+	/*
+	 * PM805_INT_STATUS is under 32K clock domain, so need to
+	 * add proper delay before the next I2C register access.
+	 */
+	msleep(1);
+
+	if (ret < 0)
+		goto out;
+
+	ret =
+	    regmap_add_irq_chip(chip->regmap, chip->irq, flags, -1,
+				chip->regmap_irq_chip, &chip->irq_data);
+
+out:
+	return ret;
+}
+
+static void device_irq_exit_805(struct pm80x_chip *chip)
+{
+	regmap_del_irq_chip(chip->irq, chip->irq_data);
+}
+
+static struct regmap_irq_chip pm805_irq_chip = {
+	.name = "88pm805",
+	.irqs = pm805_irqs,
+	.num_irqs = ARRAY_SIZE(pm805_irqs),
+
+	.num_regs = 2,
+	.status_base = PM805_INT_STATUS1,
+	.mask_base = PM805_INT_MASK1,
+	.ack_base = PM805_INT_STATUS1,
+};
+
+static int __devinit device_805_init(struct pm80x_chip *chip)
+{
+	int ret = 0;
+	unsigned int val;
+	struct regmap *map = chip->regmap;
+
+	if (!map) {
+		dev_err(chip->dev, "regmap is invalid\n");
+		return -EINVAL;
+	}
+
+	ret = regmap_read(map, PM805_CHIP_ID, &val);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read CHIP ID: %d\n", ret);
+		goto out_irq_init;
+	}
+	chip->version = val;
+
+	chip->regmap_irq_chip = &pm805_irq_chip;
+
+	ret = device_irq_init_805(chip);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to init pm805 irq!\n");
+		goto out_irq_init;
+	}
+
+	ret = mfd_add_devices(chip->dev, 0, &codec_devs[0],
+			      ARRAY_SIZE(codec_devs), &codec_resources[0], 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to add codec subdev\n");
+		goto out_codec;
+	} else
+		dev_info(chip->dev, "[%s]:Added mfd codec_devs\n", __func__);
+
+	return 0;
+
+out_codec:
+	device_irq_exit_805(chip);
+out_irq_init:
+	return ret;
+}
+
+static int __devinit pm805_probe(struct i2c_client *client,
+				 const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct pm80x_chip *chip;
+	struct pm80x_platform_data *pdata = client->dev.platform_data;
+
+	ret = pm80x_init(client, id);
+	if (ret) {
+		dev_err(&client->dev, "pm805_init fail!\n");
+		goto out_init;
+	}
+
+	chip = i2c_get_clientdata(client);
+
+	ret = device_805_init(chip);
+	if (ret) {
+		dev_err(chip->dev, "%s id 0x%x failed!\n", __func__, chip->id);
+		goto err_805_init;
+	}
+
+	if (pdata->plat_config)
+		pdata->plat_config(chip, pdata);
+
+err_805_init:
+	pm80x_deinit(client);
+out_init:
+	return ret;
+}
+
+static int __devexit pm805_remove(struct i2c_client *client)
+{
+	struct pm80x_chip *chip = i2c_get_clientdata(client);
+
+	mfd_remove_devices(chip->dev);
+	device_irq_exit_805(chip);
+
+	pm80x_deinit(client);
+
+	return 0;
+}
+
+static struct i2c_driver pm805_driver = {
+	.driver = {
+		.name = "88PM80X",
+		.owner = THIS_MODULE,
+		.pm = &pm80x_pm_ops,
+		},
+	.probe = pm805_probe,
+	.remove = __devexit_p(pm805_remove),
+	.id_table = pm80x_id_table,
+};
+
+static int __init pm805_i2c_init(void)
+{
+	return i2c_add_driver(&pm805_driver);
+}
+subsys_initcall(pm805_i2c_init);
+
+static void __exit pm805_i2c_exit(void)
+{
+	i2c_del_driver(&pm805_driver);
+}
+module_exit(pm805_i2c_exit);
+
+MODULE_DESCRIPTION("PMIC Driver for Marvell 88PM805");
+MODULE_AUTHOR("Qiao Zhou <zhouqiao@marvell.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/88pm80x.c b/drivers/mfd/88pm80x.c
new file mode 100644
index 00000000000..cd0bf527d76
--- /dev/null
+++ b/drivers/mfd/88pm80x.c
@@ -0,0 +1,145 @@
+/*
+ * I2C driver for Marvell 88PM80x
+ *
+ * Copyright (C) 2012 Marvell International Ltd.
+ * Haojian Zhuang <haojian.zhuang@marvell.com>
+ * Joseph(Yossi) Hanin <yhanin@marvell.com>
+ * Qiao Zhou <zhouqiao@marvell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mfd/88pm80x.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/err.h>
+
+/*
+ * workaround: some registers needed by pm805 are defined in pm800, so
+ * need to use this global variable to maintain the relation between
+ * pm800 and pm805. would remove it after HW chip fixes the issue.
+ */
+static struct pm80x_chip *g_pm80x_chip;
+
+const struct regmap_config pm80x_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+EXPORT_SYMBOL_GPL(pm80x_regmap_config);
+
+int __devinit pm80x_init(struct i2c_client *client,
+				 const struct i2c_device_id *id)
+{
+	struct pm80x_chip *chip;
+	struct regmap *map;
+	int ret = 0;
+
+	chip =
+	    devm_kzalloc(&client->dev, sizeof(struct pm80x_chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	map = devm_regmap_init_i2c(client, &pm80x_regmap_config);
+	if (IS_ERR(map)) {
+		ret = PTR_ERR(map);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			ret);
+		goto err_regmap_init;
+	}
+
+	chip->id = id->driver_data;
+	if (chip->id < CHIP_PM800 || chip->id > CHIP_PM805) {
+		ret = -EINVAL;
+		goto err_chip_id;
+	}
+
+	chip->client = client;
+	chip->regmap = map;
+
+	chip->irq = client->irq;
+
+	chip->dev = &client->dev;
+	dev_set_drvdata(chip->dev, chip);
+	i2c_set_clientdata(chip->client, chip);
+
+	device_init_wakeup(&client->dev, 1);
+
+	/*
+	 * workaround: set g_pm80x_chip to the first probed chip. if the
+	 * second chip is probed, just point to the companion to each
+	 * other so that pm805 can access those specific register. would
+	 * remove it after HW chip fixes the issue.
+	 */
+	if (!g_pm80x_chip)
+		g_pm80x_chip = chip;
+	else {
+		chip->companion = g_pm80x_chip->client;
+		g_pm80x_chip->companion = chip->client;
+	}
+
+	return 0;
+
+err_chip_id:
+	regmap_exit(map);
+err_regmap_init:
+	devm_kfree(&client->dev, chip);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(pm80x_init);
+
+int pm80x_deinit(struct i2c_client *client)
+{
+	struct pm80x_chip *chip = i2c_get_clientdata(client);
+
+	/*
+	 * workaround: clear the dependency between pm800 and pm805.
+	 * would remove it after HW chip fixes the issue.
+	 */
+	if (g_pm80x_chip->companion)
+		g_pm80x_chip->companion = NULL;
+	else
+		g_pm80x_chip = NULL;
+
+	regmap_exit(chip->regmap);
+	devm_kfree(&client->dev, chip);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(pm80x_deinit);
+
+#ifdef CONFIG_PM_SLEEP
+static int pm80x_suspend(struct device *dev)
+{
+	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct pm80x_chip *chip = i2c_get_clientdata(client);
+
+	if (chip && chip->wu_flag)
+		if (device_may_wakeup(chip->dev))
+			enable_irq_wake(chip->irq);
+
+	return 0;
+}
+
+static int pm80x_resume(struct device *dev)
+{
+	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct pm80x_chip *chip = i2c_get_clientdata(client);
+
+	if (chip && chip->wu_flag)
+		if (device_may_wakeup(chip->dev))
+			disable_irq_wake(chip->irq);
+
+	return 0;
+}
+#endif
+
+SIMPLE_DEV_PM_OPS(pm80x_pm_ops, pm80x_suspend, pm80x_resume);
+EXPORT_SYMBOL_GPL(pm80x_pm_ops);
+
+MODULE_DESCRIPTION("I2C Driver for Marvell 88PM80x");
+MODULE_AUTHOR("Qiao Zhou <zhouqiao@marvell.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index 87bd5ba38d5..d09918cf1b1 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -90,6 +90,10 @@ static struct resource charger_resources[] __devinitdata = {
 	{PM8607_IRQ_VCHG, PM8607_IRQ_VCHG, "vchg voltage",    IORESOURCE_IRQ,},
 };
 
+static struct resource preg_resources[] __devinitdata = {
+	{PM8606_ID_PREG,  PM8606_ID_PREG,  "preg",   IORESOURCE_IO,},
+};
+
 static struct resource rtc_resources[] __devinitdata = {
 	{PM8607_IRQ_RTC, PM8607_IRQ_RTC, "rtc", IORESOURCE_IRQ,},
 };
@@ -142,9 +146,19 @@ static struct mfd_cell codec_devs[] = {
 	{"88pm860x-codec", -1,},
 };
 
+static struct regulator_consumer_supply preg_supply[] = {
+	REGULATOR_SUPPLY("preg", "charger-manager"),
+};
+
+static struct regulator_init_data preg_init_data = {
+	.num_consumer_supplies	= ARRAY_SIZE(preg_supply),
+	.consumer_supplies	= &preg_supply[0],
+};
+
 static struct mfd_cell power_devs[] = {
 	{"88pm860x-battery", -1,},
 	{"88pm860x-charger", -1,},
+	{"88pm860x-preg",    -1,},
 };
 
 static struct mfd_cell rtc_devs[] = {
@@ -768,6 +782,15 @@ static void __devinit device_power_init(struct pm860x_chip *chip,
 			      &charger_resources[0], chip->irq_base);
 	if (ret < 0)
 		dev_err(chip->dev, "Failed to add charger subdev\n");
+
+	power_devs[2].platform_data = &preg_init_data;
+	power_devs[2].pdata_size = sizeof(struct regulator_init_data);
+	power_devs[2].num_resources = ARRAY_SIZE(preg_resources);
+	power_devs[2].resources = &preg_resources[0],
+	ret = mfd_add_devices(chip->dev, 0, &power_devs[2], 1,
+			      &preg_resources[0], chip->irq_base);
+	if (ret < 0)
+		dev_err(chip->dev, "Failed to add preg subdev\n");
 }
 
 static void __devinit device_onkey_init(struct pm860x_chip *chip,
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 92144ed1ad4..d1facef28a6 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -7,6 +7,7 @@ menu "Multifunction device drivers"
 
 config MFD_CORE
 	tristate
+	select IRQ_DOMAIN
 	default n
 
 config MFD_88PM860X
@@ -20,6 +21,30 @@ config MFD_88PM860X
 	  select individual components like voltage regulators, RTC and
 	  battery-charger under the corresponding menus.
 
+config MFD_88PM800
+	tristate "Support Marvell 88PM800"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	select MFD_CORE
+	help
+	  This supports for Marvell 88PM800 Power Management IC.
+	  This includes the I2C driver and the core APIs _only_, you have to
+	  select individual components like voltage regulators, RTC and
+	  battery-charger under the corresponding menus.
+
+config MFD_88PM805
+	tristate "Support Marvell 88PM805"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	select MFD_CORE
+	help
+	  This supports for Marvell 88PM805 Power Management IC. This includes
+	  the I2C driver and the core APIs _only_, you have to select individual
+	  components like codec device, headset/Mic device under the
+	  corresponding menus.
+
 config MFD_SM501
 	tristate "Support for Silicon Motion SM501"
 	 ---help---
@@ -173,8 +198,9 @@ config MFD_TPS65217
 
 config MFD_TPS6586X
 	bool "TPS6586x Power Management chips"
-	depends on I2C=y && GPIOLIB && GENERIC_HARDIRQS
+	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
+	select REGMAP_I2C
 	depends on REGULATOR
 	help
 	  If you say yes here you get support for the TPS6586X series of
@@ -276,6 +302,7 @@ config TWL6030_PWM
 	tristate "TWL6030 PWM (Pulse Width Modulator) Support"
 	depends on TWL4030_CORE
 	select HAVE_PWM
+	depends on !PWM
 	default n
 	help
 	  Say yes here if you want support for TWL6030 PWM.
@@ -423,6 +450,19 @@ config PMIC_ADP5520
 	  individual components like LCD backlight, LEDs, GPIOs and Kepad
 	  under the corresponding menus.
 
+config MFD_MAX77686
+	bool "Maxim Semiconductor MAX77686 PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	select IRQ_DOMAIN
+	help
+	  Say yes here to support for Maxim Semiconductor MAX77686.
+	  This is a Power Management IC with RTC on chip.
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
 config MFD_MAX77693
 	bool "Maxim Semiconductor MAX77693 PMIC Support"
 	depends on I2C=y && GENERIC_HARDIRQS
@@ -450,6 +490,7 @@ config MFD_MAX8997
 	bool "Maxim Semiconductor MAX8997/8966 PMIC Support"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
+	select IRQ_DOMAIN
 	help
 	  Say yes here to support for Maxim Semiconductor MAX8997/8966.
 	  This is a Power Management IC with RTC, Flash, Fuel Gauge, Haptic,
@@ -469,17 +510,56 @@ config MFD_MAX8998
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
 
-config MFD_S5M_CORE
-	bool "SAMSUNG S5M Series Support"
+config MFD_SEC_CORE
+	bool "SAMSUNG Electronics PMIC Series Support"
 	depends on I2C=y && GENERIC_HARDIRQS
 	select MFD_CORE
 	select REGMAP_I2C
+	select REGMAP_IRQ
 	help
-	 Support for the Samsung Electronics S5M MFD series.
+	 Support for the Samsung Electronics MFD series.
 	 This driver provides common support for accessing the device,
 	 additional drivers must be enabled in order to use the functionality
 	 of the device
 
+config MFD_ARIZONA
+	select REGMAP
+	select REGMAP_IRQ
+	select MFD_CORE
+	bool
+
+config MFD_ARIZONA_I2C
+	tristate "Support Wolfson Microelectronics Arizona platform with I2C"
+	select MFD_ARIZONA
+	select MFD_CORE
+	select REGMAP_I2C
+	depends on I2C
+	help
+	  Support for the Wolfson Microelectronics Arizona platform audio SoC
+	  core functionality controlled via I2C.
+
+config MFD_ARIZONA_SPI
+	tristate "Support Wolfson Microelectronics Arizona platform with SPI"
+	select MFD_ARIZONA
+	select MFD_CORE
+	select REGMAP_SPI
+	depends on SPI_MASTER
+	help
+	  Support for the Wolfson Microelectronics Arizona platform audio SoC
+	  core functionality controlled via I2C.
+
+config MFD_WM5102
+	bool "Support Wolfson Microelectronics WM5102"
+	depends on MFD_ARIZONA
+	help
+	  Support for Wolfson Microelectronics WM5102 low power audio SoC
+
+config MFD_WM5110
+	bool "Support Wolfson Microelectronics WM5110"
+	depends on MFD_ARIZONA
+	help
+	  Support for Wolfson Microelectronics WM5110 low power audio SoC
+
 config MFD_WM8400
 	bool "Support Wolfson Microelectronics WM8400"
 	select MFD_CORE
@@ -697,6 +777,7 @@ config AB8500_CORE
 	bool "ST-Ericsson AB8500 Mixed Signal Power Management chip"
 	depends on GENERIC_HARDIRQS && ABX500_CORE && MFD_DB8500_PRCMU
 	select MFD_CORE
+	select IRQ_DOMAIN
 	help
 	  Select this option to enable access to AB8500 power management
 	  chip. This connects to U8500 either on the SSP/SPI bus (deprecated
@@ -704,16 +785,6 @@ config AB8500_CORE
 	  the irq_chip parts for handling the Mixed Signal chip events.
 	  This chip embeds various other multimedia funtionalities as well.
 
-config AB8500_I2C_CORE
-	bool "AB8500 register access via PRCMU I2C"
-	depends on AB8500_CORE && MFD_DB8500_PRCMU
-	default y
-	help
-	  This enables register access to the AB8500 chip via PRCMU I2C.
-	  The AB8500 chip can be accessed via SPI or I2C. On DB8500 hardware
-	  the I2C bus is connected to the Power Reset
-	  and Mangagement Unit, PRCMU.
-
 config AB8500_DEBUG
        bool "Enable debug info via debugfs"
        depends on AB8500_CORE && DEBUG_FS
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 75f6ed68a4b..79dd22d1dc3 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -4,6 +4,8 @@
 
 88pm860x-objs			:= 88pm860x-core.o 88pm860x-i2c.o
 obj-$(CONFIG_MFD_88PM860X)	+= 88pm860x.o
+obj-$(CONFIG_MFD_88PM800)	+= 88pm800.o 88pm80x.o
+obj-$(CONFIG_MFD_88PM805)	+= 88pm805.o 88pm80x.o
 obj-$(CONFIG_MFD_SM501)		+= sm501.o
 obj-$(CONFIG_MFD_ASIC3)		+= asic3.o tmio_core.o
 
@@ -24,6 +26,16 @@ obj-$(CONFIG_MFD_T7L66XB)	+= t7l66xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6387XB)	+= tc6387xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6393XB)	+= tc6393xb.o tmio_core.o
 
+obj-$(CONFIG_MFD_ARIZONA)	+= arizona-core.o
+obj-$(CONFIG_MFD_ARIZONA)	+= arizona-irq.o
+obj-$(CONFIG_MFD_ARIZONA_I2C)	+= arizona-i2c.o
+obj-$(CONFIG_MFD_ARIZONA_SPI)	+= arizona-spi.o
+ifneq ($(CONFIG_MFD_WM5102),n)
+obj-$(CONFIG_MFD_ARIZONA)	+= wm5102-tables.o
+endif
+ifneq ($(CONFIG_MFD_WM5110),n)
+obj-$(CONFIG_MFD_ARIZONA)	+= wm5110-tables.o
+endif
 obj-$(CONFIG_MFD_WM8400)	+= wm8400-core.o
 wm831x-objs			:= wm831x-core.o wm831x-irq.o wm831x-otp.o
 wm831x-objs			+= wm831x-auxadc.o
@@ -78,6 +90,7 @@ obj-$(CONFIG_PMIC_DA9052)	+= da9052-core.o
 obj-$(CONFIG_MFD_DA9052_SPI)	+= da9052-spi.o
 obj-$(CONFIG_MFD_DA9052_I2C)	+= da9052-i2c.o
 
+obj-$(CONFIG_MFD_MAX77686)	+= max77686.o max77686-irq.o
 obj-$(CONFIG_MFD_MAX77693)	+= max77693.o max77693-irq.o
 max8925-objs			:= max8925-core.o max8925-i2c.o
 obj-$(CONFIG_MFD_MAX8925)	+= max8925.o
@@ -116,6 +129,6 @@ obj-$(CONFIG_MFD_AAT2870_CORE)	+= aat2870-core.o
 obj-$(CONFIG_MFD_INTEL_MSIC)	+= intel_msic.o
 obj-$(CONFIG_MFD_PALMAS)	+= palmas.o
 obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.o
-obj-$(CONFIG_MFD_S5M_CORE)	+= s5m-core.o s5m-irq.o
+obj-$(CONFIG_MFD_SEC_CORE)	+= sec-core.o sec-irq.o
 obj-$(CONFIG_MFD_ANATOP)	+= anatop-mfd.o
 obj-$(CONFIG_MFD_LM3533)	+= lm3533-core.o lm3533-ctrlbank.o
diff --git a/drivers/mfd/ab3100-core.c b/drivers/mfd/ab3100-core.c
index 1efad20fb17..78fca2902c8 100644
--- a/drivers/mfd/ab3100-core.c
+++ b/drivers/mfd/ab3100-core.c
@@ -409,8 +409,6 @@ static irqreturn_t ab3100_irq_handler(int irq, void *data)
 	u32 fatevent;
 	int err;
 
-	add_interrupt_randomness(irq);
-
 	err = ab3100_get_register_page_interruptible(ab3100, AB3100_EVENTA1,
 				       event_regs, 3);
 	if (err)
@@ -867,7 +865,7 @@ static int __devinit ab3100_probe(struct i2c_client *client,
 	int err;
 	int i;
 
-	ab3100 = kzalloc(sizeof(struct ab3100), GFP_KERNEL);
+	ab3100 = devm_kzalloc(&client->dev, sizeof(struct ab3100), GFP_KERNEL);
 	if (!ab3100) {
 		dev_err(&client->dev, "could not allocate AB3100 device\n");
 		return -ENOMEM;
@@ -921,7 +919,7 @@ static int __devinit ab3100_probe(struct i2c_client *client,
 
 	/* Attach a second dummy i2c_client to the test register address */
 	ab3100->testreg_client = i2c_new_dummy(client->adapter,
-						     client->addr + 1);
+					       client->addr + 1);
 	if (!ab3100->testreg_client) {
 		err = -ENOMEM;
 		goto exit_no_testreg_client;
@@ -931,11 +929,9 @@ static int __devinit ab3100_probe(struct i2c_client *client,
 	if (err)
 		goto exit_no_setup;
 
-	err = request_threaded_irq(client->irq, NULL, ab3100_irq_handler,
-				IRQF_ONESHOT, "ab3100-core", ab3100);
-	/* This real unpredictable IRQ is of course sampled for entropy */
-	rand_initialize_irq(client->irq);
-
+	err = devm_request_threaded_irq(&client->dev,
+					client->irq, NULL, ab3100_irq_handler,
+					IRQF_ONESHOT, "ab3100-core", ab3100);
 	if (err)
 		goto exit_no_irq;
 
@@ -962,7 +958,6 @@ static int __devinit ab3100_probe(struct i2c_client *client,
 	i2c_unregister_device(ab3100->testreg_client);
  exit_no_testreg_client:
  exit_no_detect:
-	kfree(ab3100);
 	return err;
 }
 
@@ -972,16 +967,8 @@ static int __devexit ab3100_remove(struct i2c_client *client)
 
 	/* Unregister subdevices */
 	mfd_remove_devices(&client->dev);
-
 	ab3100_remove_debugfs();
 	i2c_unregister_device(ab3100->testreg_client);
-
-	/*
-	 * At this point, all subscribers should have unregistered
-	 * their notifiers so deactivate IRQ
-	 */
-	free_irq(client->irq, ab3100);
-	kfree(ab3100);
 	return 0;
 }
 
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index dac0e299860..626b4ecaf64 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -11,6 +11,7 @@
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
@@ -140,7 +141,7 @@ static const char ab8500_version_str[][7] = {
 	[AB8500_VERSION_AB8540] = "AB8540",
 };
 
-static int ab8500_i2c_write(struct ab8500 *ab8500, u16 addr, u8 data)
+static int ab8500_prcmu_write(struct ab8500 *ab8500, u16 addr, u8 data)
 {
 	int ret;
 
@@ -150,7 +151,7 @@ static int ab8500_i2c_write(struct ab8500 *ab8500, u16 addr, u8 data)
 	return ret;
 }
 
-static int ab8500_i2c_write_masked(struct ab8500 *ab8500, u16 addr, u8 mask,
+static int ab8500_prcmu_write_masked(struct ab8500 *ab8500, u16 addr, u8 mask,
 	u8 data)
 {
 	int ret;
@@ -162,7 +163,7 @@ static int ab8500_i2c_write_masked(struct ab8500 *ab8500, u16 addr, u8 mask,
 	return ret;
 }
 
-static int ab8500_i2c_read(struct ab8500 *ab8500, u16 addr)
+static int ab8500_prcmu_read(struct ab8500 *ab8500, u16 addr)
 {
 	int ret;
 	u8 data;
@@ -361,7 +362,7 @@ static void ab8500_irq_sync_unlock(struct irq_data *data)
 static void ab8500_irq_mask(struct irq_data *data)
 {
 	struct ab8500 *ab8500 = irq_data_get_irq_chip_data(data);
-	int offset = data->irq - ab8500->irq_base;
+	int offset = data->hwirq;
 	int index = offset / 8;
 	int mask = 1 << (offset % 8);
 
@@ -371,7 +372,7 @@ static void ab8500_irq_mask(struct irq_data *data)
 static void ab8500_irq_unmask(struct irq_data *data)
 {
 	struct ab8500 *ab8500 = irq_data_get_irq_chip_data(data);
-	int offset = data->irq - ab8500->irq_base;
+	int offset = data->hwirq;
 	int index = offset / 8;
 	int mask = 1 << (offset % 8);
 
@@ -510,38 +511,51 @@ static irqreturn_t ab8500_irq(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-static int ab8500_irq_init(struct ab8500 *ab8500)
+/**
+ * ab8500_irq_get_virq(): Map an interrupt on a chip to a virtual IRQ
+ *
+ * @ab8500: ab8500_irq controller to operate on.
+ * @irq: index of the interrupt requested in the chip IRQs
+ *
+ * Useful for drivers to request their own IRQs.
+ */
+int ab8500_irq_get_virq(struct ab8500 *ab8500, int irq)
 {
-	int base = ab8500->irq_base;
-	int irq;
-	int num_irqs;
+	if (!ab8500)
+		return -EINVAL;
 
-	if (is_ab9540(ab8500))
-		num_irqs = AB9540_NR_IRQS;
-	else if (is_ab8505(ab8500))
-		num_irqs = AB8505_NR_IRQS;
-	else
-		num_irqs = AB8500_NR_IRQS;
+	return irq_create_mapping(ab8500->domain, irq);
+}
+EXPORT_SYMBOL_GPL(ab8500_irq_get_virq);
+
+static int ab8500_irq_map(struct irq_domain *d, unsigned int virq,
+				irq_hw_number_t hwirq)
+{
+	struct ab8500 *ab8500 = d->host_data;
 
-	for (irq = base; irq < base + num_irqs; irq++) {
-		irq_set_chip_data(irq, ab8500);
-		irq_set_chip_and_handler(irq, &ab8500_irq_chip,
-					 handle_simple_irq);
-		irq_set_nested_thread(irq, 1);
+	if (!ab8500)
+		return -EINVAL;
+
+	irq_set_chip_data(virq, ab8500);
+	irq_set_chip_and_handler(virq, &ab8500_irq_chip,
+				handle_simple_irq);
+	irq_set_nested_thread(virq, 1);
 #ifdef CONFIG_ARM
-		set_irq_flags(irq, IRQF_VALID);
+	set_irq_flags(virq, IRQF_VALID);
 #else
-		irq_set_noprobe(irq);
+	irq_set_noprobe(virq);
 #endif
-	}
 
 	return 0;
 }
 
-static void ab8500_irq_remove(struct ab8500 *ab8500)
+static struct irq_domain_ops ab8500_irq_ops = {
+        .map    = ab8500_irq_map,
+        .xlate  = irq_domain_xlate_twocell,
+};
+
+static int ab8500_irq_init(struct ab8500 *ab8500, struct device_node *np)
 {
-	int base = ab8500->irq_base;
-	int irq;
 	int num_irqs;
 
 	if (is_ab9540(ab8500))
@@ -551,13 +565,22 @@ static void ab8500_irq_remove(struct ab8500 *ab8500)
 	else
 		num_irqs = AB8500_NR_IRQS;
 
-	for (irq = base; irq < base + num_irqs; irq++) {
-#ifdef CONFIG_ARM
-		set_irq_flags(irq, 0);
-#endif
-		irq_set_chip_and_handler(irq, NULL, NULL);
-		irq_set_chip_data(irq, NULL);
+	if (ab8500->irq_base) {
+		ab8500->domain = irq_domain_add_legacy(
+			NULL, num_irqs, ab8500->irq_base,
+			0, &ab8500_irq_ops, ab8500);
+	}
+	else {
+		ab8500->domain = irq_domain_add_linear(
+			np, num_irqs, &ab8500_irq_ops, ab8500);
+	}
+
+	if (!ab8500->domain) {
+		dev_err(ab8500->dev, "Failed to create irqdomain\n");
+		return -ENOSYS;
 	}
+
+	return 0;
 }
 
 int ab8500_suspend(struct ab8500 *ab8500)
@@ -947,54 +970,69 @@ static struct mfd_cell __devinitdata abx500_common_devs[] = {
 #ifdef CONFIG_DEBUG_FS
 	{
 		.name = "ab8500-debug",
+		.of_compatible = "stericsson,ab8500-debug",
 		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
 		.resources = ab8500_debug_resources,
 	},
 #endif
 	{
 		.name = "ab8500-sysctrl",
+		.of_compatible = "stericsson,ab8500-sysctrl",
 	},
 	{
 		.name = "ab8500-regulator",
+		.of_compatible = "stericsson,ab8500-regulator",
 	},
 	{
 		.name = "ab8500-gpadc",
+		.of_compatible = "stericsson,ab8500-gpadc",
 		.num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
 		.resources = ab8500_gpadc_resources,
 	},
 	{
 		.name = "ab8500-rtc",
+		.of_compatible = "stericsson,ab8500-rtc",
 		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
 		.resources = ab8500_rtc_resources,
 	},
 	{
 		.name = "ab8500-acc-det",
+		.of_compatible = "stericsson,ab8500-acc-det",
 		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
 		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
 		.name = "ab8500-poweron-key",
+		.of_compatible = "stericsson,ab8500-poweron-key",
 		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
 		.resources = ab8500_poweronkey_db_resources,
 	},
 	{
 		.name = "ab8500-pwm",
+		.of_compatible = "stericsson,ab8500-pwm",
 		.id = 1,
 	},
 	{
 		.name = "ab8500-pwm",
+		.of_compatible = "stericsson,ab8500-pwm",
 		.id = 2,
 	},
 	{
 		.name = "ab8500-pwm",
+		.of_compatible = "stericsson,ab8500-pwm",
 		.id = 3,
 	},
-	{ .name = "ab8500-leds", },
+	{
+		.name = "ab8500-leds",
+		.of_compatible = "stericsson,ab8500-leds",
+	},
 	{
 		.name = "ab8500-denc",
+		.of_compatible = "stericsson,ab8500-denc",
 	},
 	{
 		.name = "ab8500-temp",
+		.of_compatible = "stericsson,ab8500-temp",
 		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
 		.resources = ab8500_temp_resources,
 	},
@@ -1026,11 +1064,13 @@ static struct mfd_cell __devinitdata ab8500_bm_devs[] = {
 static struct mfd_cell __devinitdata ab8500_devs[] = {
 	{
 		.name = "ab8500-gpio",
+		.of_compatible = "stericsson,ab8500-gpio",
 		.num_resources = ARRAY_SIZE(ab8500_gpio_resources),
 		.resources = ab8500_gpio_resources,
 	},
 	{
 		.name = "ab8500-usb",
+		.of_compatible = "stericsson,ab8500-usb",
 		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
 		.resources = ab8500_usb_resources,
 	},
@@ -1207,16 +1247,17 @@ static struct attribute_group ab9540_attr_group = {
 	.attrs	= ab9540_sysfs_entries,
 };
 
-static const struct of_device_id ab8500_match[] = {
-	{
-		.compatible = "stericsson,ab8500",
-		.data = (void *)AB8500_VERSION_AB8500,
-	},
-	{},
-};
-
 static int __devinit ab8500_probe(struct platform_device *pdev)
 {
+	static char *switch_off_status[] = {
+		"Swoff bit programming",
+		"Thermal protection activation",
+		"Vbat lower then BattOk falling threshold",
+		"Watchdog expired",
+		"Non presence of 32kHz clock",
+		"Battery level lower than power on reset threshold",
+		"Power on key 1 pressed longer than 10 seconds",
+		"DB8500 thermal shutdown"};
 	struct ab8500_platform_data *plat = dev_get_platdata(&pdev->dev);
 	const struct platform_device_id *platid = platform_get_device_id(pdev);
 	enum ab8500_version version = AB8500_VERSION_UNDEFINED;
@@ -1233,14 +1274,6 @@ static int __devinit ab8500_probe(struct platform_device *pdev)
 
 	if (plat)
 		ab8500->irq_base = plat->irq_base;
-	else if (np)
-		ret = of_property_read_u32(np, "stericsson,irq-base", &ab8500->irq_base);
-
-	if (!ab8500->irq_base) {
-		dev_info(&pdev->dev, "couldn't find irq-base\n");
-		ret = -EINVAL;
-		goto out_free_ab8500;
-	}
 
 	ab8500->dev = &pdev->dev;
 
@@ -1252,9 +1285,9 @@ static int __devinit ab8500_probe(struct platform_device *pdev)
 
 	ab8500->irq = resource->start;
 
-	ab8500->read = ab8500_i2c_read;
-	ab8500->write = ab8500_i2c_write;
-	ab8500->write_masked = ab8500_i2c_write_masked;
+	ab8500->read = ab8500_prcmu_read;
+	ab8500->write = ab8500_prcmu_write;
+	ab8500->write_masked = ab8500_prcmu_write_masked;
 
 	mutex_init(&ab8500->lock);
 	mutex_init(&ab8500->irq_lock);
@@ -1264,9 +1297,6 @@ static int __devinit ab8500_probe(struct platform_device *pdev)
 
 	if (platid)
 		version = platid->driver_data;
-	else if (np)
-		version = (unsigned int)
-			of_match_device(ab8500_match, &pdev->dev)->data;
 
 	if (version != AB8500_VERSION_UNDEFINED)
 		ab8500->version = version;
@@ -1323,7 +1353,20 @@ static int __devinit ab8500_probe(struct platform_device *pdev)
 		AB8500_SWITCH_OFF_STATUS, &value);
 	if (ret < 0)
 		return ret;
-	dev_info(ab8500->dev, "switch off status: %#x", value);
+	dev_info(ab8500->dev, "switch off cause(s) (%#x): ", value);
+
+	if (value) {
+		for (i = 0; i < ARRAY_SIZE(switch_off_status); i++) {
+			if (value & 1)
+				printk(KERN_CONT " \"%s\"",
+				       switch_off_status[i]);
+			value = value >> 1;
+
+		}
+		printk(KERN_CONT "\n");
+	} else {
+		printk(KERN_CONT " None\n");
+	}
 
 	if (plat && plat->init)
 		plat->init(ab8500);
@@ -1352,53 +1395,50 @@ static int __devinit ab8500_probe(struct platform_device *pdev)
 	for (i = 0; i < ab8500->mask_size; i++)
 		ab8500->mask[i] = ab8500->oldmask[i] = 0xff;
 
-	if (ab8500->irq_base) {
-		ret = ab8500_irq_init(ab8500);
-		if (ret)
-			goto out_freeoldmask;
+	ret = ab8500_irq_init(ab8500, np);
+	if (ret)
+		goto out_freeoldmask;
 
-		/*  Activate this feature only in ab9540 */
-		/*  till tests are done on ab8500 1p2 or later*/
-		if (is_ab9540(ab8500))
-			ret = request_threaded_irq(ab8500->irq, NULL,
+	/*  Activate this feature only in ab9540 */
+	/*  till tests are done on ab8500 1p2 or later*/
+	if (is_ab9540(ab8500)) {
+		ret = request_threaded_irq(ab8500->irq, NULL,
 					ab8500_hierarchical_irq,
 					IRQF_ONESHOT | IRQF_NO_SUSPEND,
 					"ab8500", ab8500);
-		else
-			ret = request_threaded_irq(ab8500->irq, NULL,
+	}
+	else {
+		ret = request_threaded_irq(ab8500->irq, NULL,
 					ab8500_irq,
 					IRQF_ONESHOT | IRQF_NO_SUSPEND,
 					"ab8500", ab8500);
 		if (ret)
-			goto out_removeirq;
+			goto out_freeoldmask;
 	}
 
-	if (!np) {
-		ret = mfd_add_devices(ab8500->dev, 0, abx500_common_devs,
-				ARRAY_SIZE(abx500_common_devs), NULL,
-				ab8500->irq_base);
+	ret = mfd_add_devices(ab8500->dev, 0, abx500_common_devs,
+			ARRAY_SIZE(abx500_common_devs), NULL,
+			ab8500->irq_base);
+	if (ret)
+		goto out_freeirq;
 
-		if (ret)
-			goto out_freeirq;
-
-		if (is_ab9540(ab8500))
-			ret = mfd_add_devices(ab8500->dev, 0, ab9540_devs,
-					ARRAY_SIZE(ab9540_devs), NULL,
-					ab8500->irq_base);
-		else
-			ret = mfd_add_devices(ab8500->dev, 0, ab8500_devs,
-					ARRAY_SIZE(ab8500_devs), NULL,
-					ab8500->irq_base);
-		if (ret)
-			goto out_freeirq;
+	if (is_ab9540(ab8500))
+		ret = mfd_add_devices(ab8500->dev, 0, ab9540_devs,
+				ARRAY_SIZE(ab9540_devs), NULL,
+				ab8500->irq_base);
+	else
+		ret = mfd_add_devices(ab8500->dev, 0, ab8500_devs,
+				ARRAY_SIZE(ab8500_devs), NULL,
+				ab8500->irq_base);
+	if (ret)
+		goto out_freeirq;
 
-		if (is_ab9540(ab8500) || is_ab8505(ab8500))
-			ret = mfd_add_devices(ab8500->dev, 0, ab9540_ab8505_devs,
-					ARRAY_SIZE(ab9540_ab8505_devs), NULL,
-					ab8500->irq_base);
-		if (ret)
-			goto out_freeirq;
-	}
+	if (is_ab9540(ab8500) || is_ab8505(ab8500))
+		ret = mfd_add_devices(ab8500->dev, 0, ab9540_ab8505_devs,
+				ARRAY_SIZE(ab9540_ab8505_devs), NULL,
+				ab8500->irq_base);
+	if (ret)
+		goto out_freeirq;
 
 	if (!no_bm) {
 		/* Add battery management devices */
@@ -1417,15 +1457,11 @@ static int __devinit ab8500_probe(struct platform_device *pdev)
 					&ab8500_attr_group);
 	if (ret)
 		dev_err(ab8500->dev, "error creating sysfs entries\n");
-	else
-		return ret;
+
+	return ret;
 
 out_freeirq:
-	if (ab8500->irq_base)
-		free_irq(ab8500->irq, ab8500);
-out_removeirq:
-	if (ab8500->irq_base)
-		ab8500_irq_remove(ab8500);
+	free_irq(ab8500->irq, ab8500);
 out_freeoldmask:
 	kfree(ab8500->oldmask);
 out_freemask:
@@ -1444,11 +1480,10 @@ static int __devexit ab8500_remove(struct platform_device *pdev)
 		sysfs_remove_group(&ab8500->dev->kobj, &ab9540_attr_group);
 	else
 		sysfs_remove_group(&ab8500->dev->kobj, &ab8500_attr_group);
+
 	mfd_remove_devices(ab8500->dev);
-	if (ab8500->irq_base) {
-		free_irq(ab8500->irq, ab8500);
-		ab8500_irq_remove(ab8500);
-	}
+	free_irq(ab8500->irq, ab8500);
+
 	kfree(ab8500->oldmask);
 	kfree(ab8500->mask);
 	kfree(ab8500);
@@ -1468,7 +1503,6 @@ static struct platform_driver ab8500_core_driver = {
 	.driver = {
 		.name = "ab8500-core",
 		.owner = THIS_MODULE,
-		.of_match_table = ab8500_match,
 	},
 	.probe	= ab8500_probe,
 	.remove	= __devexit_p(ab8500_remove),
@@ -1484,7 +1518,7 @@ static void __exit ab8500_core_exit(void)
 {
 	platform_driver_unregister(&ab8500_core_driver);
 }
-arch_initcall(ab8500_core_init);
+core_initcall(ab8500_core_init);
 module_exit(ab8500_core_exit);
 
 MODULE_AUTHOR("Mattias Wallin, Srinidhi Kasagar, Rabin Vincent");
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 50c4c89ab22..c4cb806978a 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -31,12 +31,12 @@ struct ab8500_reg_range {
 };
 
 /**
- * struct ab8500_i2c_ranges
+ * struct ab8500_prcmu_ranges
  * @num_ranges: the number of ranges in the list
  * @bankid: bank identifier
  * @range: the list of register ranges
  */
-struct ab8500_i2c_ranges {
+struct ab8500_prcmu_ranges {
 	u8 num_ranges;
 	u8 bankid;
 	const struct ab8500_reg_range *range;
@@ -47,7 +47,7 @@ struct ab8500_i2c_ranges {
 
 #define AB8500_REV_REG 0x80
 
-static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
+static struct ab8500_prcmu_ranges debug_ranges[AB8500_NUM_BANKS] = {
 	[0x0] = {
 		.num_ranges = 0,
 		.range = 0,
@@ -608,16 +608,10 @@ static int __devexit ab8500_debug_remove(struct platform_device *plf)
 	return 0;
 }
 
-static const struct of_device_id ab8500_debug_match[] = {
-        { .compatible = "stericsson,ab8500-debug", },
-        {}
-};
-
 static struct platform_driver ab8500_debug_driver = {
 	.driver = {
 		.name = "ab8500-debug",
 		.owner = THIS_MODULE,
-		.of_match_table = ab8500_debug_match,
 	},
 	.probe  = ab8500_debug_probe,
 	.remove = __devexit_p(ab8500_debug_remove)
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index b86fd8e1ec3..866f95960b4 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -599,7 +599,8 @@ static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
 	/* Register interrupt  - SwAdcComplete */
 	ret = request_threaded_irq(gpadc->irq, NULL,
 		ab8500_bm_gpswadcconvend_handler,
-		IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc", gpadc);
+		IRQF_ONESHOT | IRQF_NO_SUSPEND | IRQF_SHARED,
+				"ab8500-gpadc", gpadc);
 	if (ret < 0) {
 		dev_err(gpadc->dev, "Failed to register interrupt, irq: %d\n",
 			gpadc->irq);
@@ -648,18 +649,12 @@ static int __devexit ab8500_gpadc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id ab8500_gpadc_match[] = {
-	{ .compatible = "stericsson,ab8500-gpadc", },
-	{}
-};
-
 static struct platform_driver ab8500_gpadc_driver = {
 	.probe = ab8500_gpadc_probe,
 	.remove = __devexit_p(ab8500_gpadc_remove),
 	.driver = {
 		.name = "ab8500-gpadc",
 		.owner = THIS_MODULE,
-		.of_match_table = ab8500_gpadc_match,
 	},
 };
 
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index 5a3e51ccf25..c28d4eb1eff 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -61,16 +61,10 @@ static int __devexit ab8500_sysctrl_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id ab8500_sysctrl_match[] = {
-	{ .compatible = "stericsson,ab8500-sysctrl", },
-	{}
-};
-
 static struct platform_driver ab8500_sysctrl_driver = {
 	.driver = {
 		.name = "ab8500-sysctrl",
 		.owner = THIS_MODULE,
-		.of_match_table = ab8500_sysctrl_match,
 	},
 	.probe = ab8500_sysctrl_probe,
 	.remove = __devexit_p(ab8500_sysctrl_remove),
diff --git a/drivers/mfd/adp5520.c b/drivers/mfd/adp5520.c
index 8d816cce832..ea8b9475731 100644
--- a/drivers/mfd/adp5520.c
+++ b/drivers/mfd/adp5520.c
@@ -320,7 +320,7 @@ static int __devexit adp5520_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int adp5520_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
diff --git a/drivers/mfd/anatop-mfd.c b/drivers/mfd/anatop-mfd.c
index 6da06341f6c..5576e07576d 100644
--- a/drivers/mfd/anatop-mfd.c
+++ b/drivers/mfd/anatop-mfd.c
@@ -83,7 +83,7 @@ static int __devinit of_anatop_probe(struct platform_device *pdev)
 	drvdata->ioreg = ioreg;
 	spin_lock_init(&drvdata->reglock);
 	platform_set_drvdata(pdev, drvdata);
-	of_platform_populate(np, of_anatop_match, NULL, dev);
+	of_platform_populate(np, NULL, NULL, dev);
 
 	return 0;
 }
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
new file mode 100644
index 00000000000..c7983e86254
--- /dev/null
+++ b/drivers/mfd/arizona-core.c
@@ -0,0 +1,566 @@
+/*
+ * Arizona core driver
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/core.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/registers.h>
+
+#include "arizona.h"
+
+static const char *wm5102_core_supplies[] = {
+	"AVDD",
+	"DBVDD1",
+};
+
+int arizona_clk32k_enable(struct arizona *arizona)
+{
+	int ret = 0;
+
+	mutex_lock(&arizona->clk_lock);
+
+	arizona->clk32k_ref++;
+
+	if (arizona->clk32k_ref == 1)
+		ret = regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
+					 ARIZONA_CLK_32K_ENA,
+					 ARIZONA_CLK_32K_ENA);
+
+	if (ret != 0)
+		arizona->clk32k_ref--;
+
+	mutex_unlock(&arizona->clk_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(arizona_clk32k_enable);
+
+int arizona_clk32k_disable(struct arizona *arizona)
+{
+	int ret = 0;
+
+	mutex_lock(&arizona->clk_lock);
+
+	BUG_ON(arizona->clk32k_ref <= 0);
+
+	arizona->clk32k_ref--;
+
+	if (arizona->clk32k_ref == 0)
+		regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
+				   ARIZONA_CLK_32K_ENA, 0);
+
+	mutex_unlock(&arizona->clk_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(arizona_clk32k_disable);
+
+static irqreturn_t arizona_clkgen_err(int irq, void *data)
+{
+	struct arizona *arizona = data;
+
+	dev_err(arizona->dev, "CLKGEN error\n");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arizona_underclocked(int irq, void *data)
+{
+	struct arizona *arizona = data;
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(arizona->regmap, ARIZONA_INTERRUPT_RAW_STATUS_8,
+			  &val);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to read underclock status: %d\n",
+			ret);
+		return IRQ_NONE;
+	}
+
+	if (val & ARIZONA_AIF3_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "AIF3 underclocked\n");
+	if (val & ARIZONA_AIF3_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "AIF3 underclocked\n");
+	if (val & ARIZONA_AIF2_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "AIF1 underclocked\n");
+	if (val & ARIZONA_ISRC2_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "ISRC2 underclocked\n");
+	if (val & ARIZONA_ISRC1_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "ISRC1 underclocked\n");
+	if (val & ARIZONA_FX_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "FX underclocked\n");
+	if (val & ARIZONA_ASRC_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "ASRC underclocked\n");
+	if (val & ARIZONA_DAC_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "DAC underclocked\n");
+	if (val & ARIZONA_ADC_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "ADC underclocked\n");
+	if (val & ARIZONA_MIXER_UNDERCLOCKED_STS)
+		dev_err(arizona->dev, "Mixer underclocked\n");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arizona_overclocked(int irq, void *data)
+{
+	struct arizona *arizona = data;
+	unsigned int val[2];
+	int ret;
+	
+	ret = regmap_bulk_read(arizona->regmap, ARIZONA_INTERRUPT_RAW_STATUS_6,
+			       &val[0], 2);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to read overclock status: %d\n",
+			ret);
+		return IRQ_NONE;
+	}
+
+	if (val[0] & ARIZONA_PWM_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "PWM overclocked\n");
+	if (val[0] & ARIZONA_FX_CORE_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "FX core overclocked\n");
+	if (val[0] & ARIZONA_DAC_SYS_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "DAC SYS overclocked\n");
+	if (val[0] & ARIZONA_DAC_WARP_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "DAC WARP overclocked\n");
+	if (val[0] & ARIZONA_ADC_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ADC overclocked\n");
+	if (val[0] & ARIZONA_MIXER_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "Mixer overclocked\n");
+	if (val[0] & ARIZONA_AIF3_SYNC_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "AIF3 overclocked\n");
+	if (val[0] & ARIZONA_AIF2_SYNC_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "AIF2 overclocked\n");
+	if (val[0] & ARIZONA_AIF1_SYNC_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "AIF1 overclocked\n");
+	if (val[0] & ARIZONA_PAD_CTRL_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "Pad control overclocked\n");
+
+	if (val[1] & ARIZONA_SLIMBUS_SUBSYS_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "Slimbus subsystem overclocked\n");
+	if (val[1] & ARIZONA_SLIMBUS_ASYNC_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "Slimbus async overclocked\n");
+	if (val[1] & ARIZONA_SLIMBUS_SYNC_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "Slimbus sync overclocked\n");
+	if (val[1] & ARIZONA_ASRC_ASYNC_SYS_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ASRC async system overclocked\n");
+	if (val[1] & ARIZONA_ASRC_ASYNC_WARP_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ASRC async WARP overclocked\n");
+	if (val[1] & ARIZONA_ASRC_SYNC_SYS_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ASRC sync system overclocked\n");
+	if (val[1] & ARIZONA_ASRC_SYNC_WARP_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ASRC sync WARP overclocked\n");
+	if (val[1] & ARIZONA_ADSP2_1_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "DSP1 overclocked\n");
+	if (val[1] & ARIZONA_ISRC2_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ISRC2 overclocked\n");
+	if (val[1] & ARIZONA_ISRC1_OVERCLOCKED_STS)
+		dev_err(arizona->dev, "ISRC1 overclocked\n");
+
+	return IRQ_HANDLED;
+}
+
+static int arizona_wait_for_boot(struct arizona *arizona)
+{
+	unsigned int reg;
+	int ret, i;
+
+	/*
+	 * We can't use an interrupt as we need to runtime resume to do so,
+	 * we won't race with the interrupt handler as it'll be blocked on
+	 * runtime resume.
+	 */
+	for (i = 0; i < 5; i++) {
+		msleep(1);
+
+		ret = regmap_read(arizona->regmap,
+				  ARIZONA_INTERRUPT_RAW_STATUS_5, &reg);
+		if (ret != 0) {
+			dev_err(arizona->dev, "Failed to read boot state: %d\n",
+				ret);
+			continue;
+		}
+
+		if (reg & ARIZONA_BOOT_DONE_STS)
+			break;
+	}
+
+	if (reg & ARIZONA_BOOT_DONE_STS) {
+		regmap_write(arizona->regmap, ARIZONA_INTERRUPT_STATUS_5,
+			     ARIZONA_BOOT_DONE_STS);
+	} else {
+		dev_err(arizona->dev, "Device boot timed out: %x\n", reg);
+		return -ETIMEDOUT;
+	}
+
+	pm_runtime_mark_last_busy(arizona->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int arizona_runtime_resume(struct device *dev)
+{
+	struct arizona *arizona = dev_get_drvdata(dev);
+	int ret;
+
+	dev_dbg(arizona->dev, "Leaving AoD mode\n");
+
+	ret = regulator_enable(arizona->dcvdd);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to enable DCVDD: %d\n", ret);
+		return ret;
+	}
+
+	regcache_cache_only(arizona->regmap, false);
+
+	ret = arizona_wait_for_boot(arizona);
+	if (ret != 0) {
+		regulator_disable(arizona->dcvdd);
+		return ret;
+	}
+
+	regcache_sync(arizona->regmap);
+
+	return 0;
+}
+
+static int arizona_runtime_suspend(struct device *dev)
+{
+	struct arizona *arizona = dev_get_drvdata(dev);
+
+	dev_dbg(arizona->dev, "Entering AoD mode\n");
+
+	regulator_disable(arizona->dcvdd);
+	regcache_cache_only(arizona->regmap, true);
+	regcache_mark_dirty(arizona->regmap);
+
+	return 0;
+}
+#endif
+
+const struct dev_pm_ops arizona_pm_ops = {
+	SET_RUNTIME_PM_OPS(arizona_runtime_suspend,
+			   arizona_runtime_resume,
+			   NULL)
+};
+EXPORT_SYMBOL_GPL(arizona_pm_ops);
+
+static struct mfd_cell early_devs[] = {
+	{ .name = "arizona-ldo1" },
+};
+
+static struct mfd_cell wm5102_devs[] = {
+	{ .name = "arizona-extcon" },
+	{ .name = "arizona-gpio" },
+	{ .name = "arizona-micsupp" },
+	{ .name = "arizona-pwm" },
+	{ .name = "wm5102-codec" },
+};
+
+static struct mfd_cell wm5110_devs[] = {
+	{ .name = "arizona-extcon" },
+	{ .name = "arizona-gpio" },
+	{ .name = "arizona-micsupp" },
+	{ .name = "arizona-pwm" },
+	{ .name = "wm5110-codec" },
+};
+
+int __devinit arizona_dev_init(struct arizona *arizona)
+{
+	struct device *dev = arizona->dev;
+	const char *type_name;
+	unsigned int reg, val;
+	int ret, i;
+
+	dev_set_drvdata(arizona->dev, arizona);
+	mutex_init(&arizona->clk_lock);
+
+	if (dev_get_platdata(arizona->dev))
+		memcpy(&arizona->pdata, dev_get_platdata(arizona->dev),
+		       sizeof(arizona->pdata));
+
+	regcache_cache_only(arizona->regmap, true);
+
+	switch (arizona->type) {
+	case WM5102:
+	case WM5110:
+		for (i = 0; i < ARRAY_SIZE(wm5102_core_supplies); i++)
+			arizona->core_supplies[i].supply
+				= wm5102_core_supplies[i];
+		arizona->num_core_supplies = ARRAY_SIZE(wm5102_core_supplies);
+		break;
+	default:
+		dev_err(arizona->dev, "Unknown device type %d\n",
+			arizona->type);
+		return -EINVAL;
+	}
+
+	ret = mfd_add_devices(arizona->dev, -1, early_devs,
+			      ARRAY_SIZE(early_devs), NULL, 0);
+	if (ret != 0) {
+		dev_err(dev, "Failed to add early children: %d\n", ret);
+		return ret;
+	}
+
+	ret = devm_regulator_bulk_get(dev, arizona->num_core_supplies,
+				      arizona->core_supplies);
+	if (ret != 0) {
+		dev_err(dev, "Failed to request core supplies: %d\n",
+			ret);
+		goto err_early;
+	}
+
+	arizona->dcvdd = devm_regulator_get(arizona->dev, "DCVDD");
+	if (IS_ERR(arizona->dcvdd)) {
+		ret = PTR_ERR(arizona->dcvdd);
+		dev_err(dev, "Failed to request DCVDD: %d\n", ret);
+		goto err_early;
+	}
+
+	ret = regulator_bulk_enable(arizona->num_core_supplies,
+				    arizona->core_supplies);
+	if (ret != 0) {
+		dev_err(dev, "Failed to enable core supplies: %d\n",
+			ret);
+		goto err_early;
+	}
+
+	ret = regulator_enable(arizona->dcvdd);
+	if (ret != 0) {
+		dev_err(dev, "Failed to enable DCVDD: %d\n", ret);
+		goto err_enable;
+	}
+
+	if (arizona->pdata.reset) {
+		/* Start out with /RESET low to put the chip into reset */
+		ret = gpio_request_one(arizona->pdata.reset,
+				       GPIOF_DIR_OUT | GPIOF_INIT_LOW,
+				       "arizona /RESET");
+		if (ret != 0) {
+			dev_err(dev, "Failed to request /RESET: %d\n", ret);
+			goto err_dcvdd;
+		}
+
+		gpio_set_value_cansleep(arizona->pdata.reset, 1);
+	}
+
+	regcache_cache_only(arizona->regmap, false);
+
+	ret = regmap_read(arizona->regmap, ARIZONA_SOFTWARE_RESET, &reg);
+	if (ret != 0) {
+		dev_err(dev, "Failed to read ID register: %d\n", ret);
+		goto err_reset;
+	}
+
+	ret = regmap_read(arizona->regmap, ARIZONA_DEVICE_REVISION,
+			  &arizona->rev);
+	if (ret != 0) {
+		dev_err(dev, "Failed to read revision register: %d\n", ret);
+		goto err_reset;
+	}
+	arizona->rev &= ARIZONA_DEVICE_REVISION_MASK;
+
+	switch (reg) {
+#ifdef CONFIG_MFD_WM5102
+	case 0x5102:
+		type_name = "WM5102";
+		if (arizona->type != WM5102) {
+			dev_err(arizona->dev, "WM5102 registered as %d\n",
+				arizona->type);
+			arizona->type = WM5102;
+		}
+		ret = wm5102_patch(arizona);
+		break;
+#endif
+#ifdef CONFIG_MFD_WM5110
+	case 0x5110:
+		type_name = "WM5110";
+		if (arizona->type != WM5110) {
+			dev_err(arizona->dev, "WM5110 registered as %d\n",
+				arizona->type);
+			arizona->type = WM5110;
+		}
+		ret = wm5110_patch(arizona);
+		break;
+#endif
+	default:
+		dev_err(arizona->dev, "Unknown device ID %x\n", reg);
+		goto err_reset;
+	}
+
+	dev_info(dev, "%s revision %c\n", type_name, arizona->rev + 'A');
+
+	if (ret != 0)
+		dev_err(arizona->dev, "Failed to apply patch: %d\n", ret);
+
+	/* If we have a /RESET GPIO we'll already be reset */
+	if (!arizona->pdata.reset) {
+		ret = regmap_write(arizona->regmap, ARIZONA_SOFTWARE_RESET, 0);
+		if (ret != 0) {
+			dev_err(dev, "Failed to reset device: %d\n", ret);
+			goto err_reset;
+		}
+	}
+
+	ret = arizona_wait_for_boot(arizona);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Device failed initial boot: %d\n", ret);
+		goto err_reset;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(arizona->pdata.gpio_defaults); i++) {
+		if (!arizona->pdata.gpio_defaults[i])
+			continue;
+
+		regmap_write(arizona->regmap, ARIZONA_GPIO1_CTRL + i,
+			     arizona->pdata.gpio_defaults[i]);
+	}
+
+	pm_runtime_set_autosuspend_delay(arizona->dev, 100);
+	pm_runtime_use_autosuspend(arizona->dev);
+	pm_runtime_enable(arizona->dev);
+
+	/* Chip default */
+	if (!arizona->pdata.clk32k_src)
+		arizona->pdata.clk32k_src = ARIZONA_32KZ_MCLK2;
+
+	switch (arizona->pdata.clk32k_src) {
+	case ARIZONA_32KZ_MCLK1:
+	case ARIZONA_32KZ_MCLK2:
+		regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
+				   ARIZONA_CLK_32K_SRC_MASK,
+				   arizona->pdata.clk32k_src - 1);
+		break;
+	case ARIZONA_32KZ_NONE:
+		regmap_update_bits(arizona->regmap, ARIZONA_CLOCK_32K_1,
+				   ARIZONA_CLK_32K_SRC_MASK, 2);
+		break;
+	default:
+		dev_err(arizona->dev, "Invalid 32kHz clock source: %d\n",
+			arizona->pdata.clk32k_src);
+		ret = -EINVAL;
+		goto err_reset;
+	}
+
+	for (i = 0; i < ARIZONA_MAX_INPUT; i++) {
+		/* Default for both is 0 so noop with defaults */
+		val = arizona->pdata.dmic_ref[i]
+			<< ARIZONA_IN1_DMIC_SUP_SHIFT;
+		val |= arizona->pdata.inmode[i] << ARIZONA_IN1_MODE_SHIFT;
+
+		regmap_update_bits(arizona->regmap,
+				   ARIZONA_IN1L_CONTROL + (i * 8),
+				   ARIZONA_IN1_DMIC_SUP_MASK |
+				   ARIZONA_IN1_MODE_MASK, val);
+	}
+
+	for (i = 0; i < ARIZONA_MAX_OUTPUT; i++) {
+		/* Default is 0 so noop with defaults */
+		if (arizona->pdata.out_mono[i])
+			val = ARIZONA_OUT1_MONO;
+		else
+			val = 0;
+
+		regmap_update_bits(arizona->regmap,
+				   ARIZONA_OUTPUT_PATH_CONFIG_1L + (i * 8),
+				   ARIZONA_OUT1_MONO, val);
+	}
+
+	for (i = 0; i < ARIZONA_MAX_PDM_SPK; i++) {
+		if (arizona->pdata.spk_mute[i])
+			regmap_update_bits(arizona->regmap,
+					   ARIZONA_PDM_SPK1_CTRL_1 + (i * 2),
+					   ARIZONA_SPK1_MUTE_ENDIAN_MASK |
+					   ARIZONA_SPK1_MUTE_SEQ1_MASK,
+					   arizona->pdata.spk_mute[i]);
+
+		if (arizona->pdata.spk_fmt[i])
+			regmap_update_bits(arizona->regmap,
+					   ARIZONA_PDM_SPK1_CTRL_2 + (i * 2),
+					   ARIZONA_SPK1_FMT_MASK,
+					   arizona->pdata.spk_fmt[i]);
+	}
+
+	/* Set up for interrupts */
+	ret = arizona_irq_init(arizona);
+	if (ret != 0)
+		goto err_reset;
+
+	arizona_request_irq(arizona, ARIZONA_IRQ_CLKGEN_ERR, "CLKGEN error",
+			    arizona_clkgen_err, arizona);
+	arizona_request_irq(arizona, ARIZONA_IRQ_OVERCLOCKED, "Overclocked",
+			    arizona_overclocked, arizona);
+	arizona_request_irq(arizona, ARIZONA_IRQ_UNDERCLOCKED, "Underclocked",
+			    arizona_underclocked, arizona);
+
+	switch (arizona->type) {
+	case WM5102:
+		ret = mfd_add_devices(arizona->dev, -1, wm5102_devs,
+				      ARRAY_SIZE(wm5102_devs), NULL, 0);
+		break;
+	case WM5110:
+		ret = mfd_add_devices(arizona->dev, -1, wm5110_devs,
+				      ARRAY_SIZE(wm5102_devs), NULL, 0);
+		break;
+	}
+
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to add subdevices: %d\n", ret);
+		goto err_irq;
+	}
+
+#ifdef CONFIG_PM_RUNTIME
+	regulator_disable(arizona->dcvdd);
+#endif
+
+	return 0;
+
+err_irq:
+	arizona_irq_exit(arizona);
+err_reset:
+	if (arizona->pdata.reset) {
+		gpio_set_value_cansleep(arizona->pdata.reset, 1);
+		gpio_free(arizona->pdata.reset);
+	}
+err_dcvdd:
+	regulator_disable(arizona->dcvdd);
+err_enable:
+	regulator_bulk_disable(arizona->num_core_supplies,
+			       arizona->core_supplies);
+err_early:
+	mfd_remove_devices(dev);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(arizona_dev_init);
+
+int __devexit arizona_dev_exit(struct arizona *arizona)
+{
+	mfd_remove_devices(arizona->dev);
+	arizona_free_irq(arizona, ARIZONA_IRQ_UNDERCLOCKED, arizona);
+	arizona_free_irq(arizona, ARIZONA_IRQ_OVERCLOCKED, arizona);
+	arizona_free_irq(arizona, ARIZONA_IRQ_CLKGEN_ERR, arizona);
+	pm_runtime_disable(arizona->dev);
+	arizona_irq_exit(arizona);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(arizona_dev_exit);
diff --git a/drivers/mfd/arizona-i2c.c b/drivers/mfd/arizona-i2c.c
new file mode 100644
index 00000000000..570c4b43808
--- /dev/null
+++ b/drivers/mfd/arizona-i2c.c
@@ -0,0 +1,97 @@
+/*
+ * Arizona-i2c.c  --  Arizona I2C bus interface
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/arizona/core.h>
+
+#include "arizona.h"
+
+static __devinit int arizona_i2c_probe(struct i2c_client *i2c,
+					  const struct i2c_device_id *id)
+{
+	struct arizona *arizona;
+	const struct regmap_config *regmap_config;
+	int ret;
+
+	switch (id->driver_data) {
+#ifdef CONFIG_MFD_WM5102
+	case WM5102:
+		regmap_config = &wm5102_i2c_regmap;
+		break;
+#endif
+#ifdef CONFIG_MFD_WM5110
+	case WM5110:
+		regmap_config = &wm5110_i2c_regmap;
+		break;
+#endif
+	default:
+		dev_err(&i2c->dev, "Unknown device type %ld\n",
+			id->driver_data);
+		return -EINVAL;
+	}
+
+	arizona = devm_kzalloc(&i2c->dev, sizeof(*arizona), GFP_KERNEL);
+	if (arizona == NULL)
+		return -ENOMEM;
+
+	arizona->regmap = devm_regmap_init_i2c(i2c, regmap_config);
+	if (IS_ERR(arizona->regmap)) {
+		ret = PTR_ERR(arizona->regmap);
+		dev_err(&i2c->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	arizona->type = id->driver_data;
+	arizona->dev = &i2c->dev;
+	arizona->irq = i2c->irq;
+
+	return arizona_dev_init(arizona);
+}
+
+static int __devexit arizona_i2c_remove(struct i2c_client *i2c)
+{
+	struct arizona *arizona = dev_get_drvdata(&i2c->dev);
+	arizona_dev_exit(arizona);
+	return 0;
+}
+
+static const struct i2c_device_id arizona_i2c_id[] = {
+	{ "wm5102", WM5102 },
+	{ "wm5110", WM5110 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, arizona_i2c_id);
+
+static struct i2c_driver arizona_i2c_driver = {
+	.driver = {
+		.name	= "arizona",
+		.owner	= THIS_MODULE,
+		.pm	= &arizona_pm_ops,
+	},
+	.probe		= arizona_i2c_probe,
+	.remove		= __devexit_p(arizona_i2c_remove),
+	.id_table	= arizona_i2c_id,
+};
+
+module_i2c_driver(arizona_i2c_driver);
+
+MODULE_DESCRIPTION("Arizona I2C bus interface");
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
new file mode 100644
index 00000000000..98ac345f468
--- /dev/null
+++ b/drivers/mfd/arizona-irq.c
@@ -0,0 +1,275 @@
+/*
+ * Arizona interrupt support
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/registers.h>
+
+#include "arizona.h"
+
+static int arizona_map_irq(struct arizona *arizona, int irq)
+{
+	int ret;
+
+	ret = regmap_irq_get_virq(arizona->aod_irq_chip, irq);
+	if (ret < 0)
+		ret = regmap_irq_get_virq(arizona->irq_chip, irq);
+
+	return ret;
+}
+
+int arizona_request_irq(struct arizona *arizona, int irq, char *name,
+			   irq_handler_t handler, void *data)
+{
+	irq = arizona_map_irq(arizona, irq);
+	if (irq < 0)
+		return irq;
+
+	return request_threaded_irq(irq, NULL, handler, IRQF_ONESHOT,
+				    name, data);
+}
+EXPORT_SYMBOL_GPL(arizona_request_irq);
+
+void arizona_free_irq(struct arizona *arizona, int irq, void *data)
+{
+	irq = arizona_map_irq(arizona, irq);
+	if (irq < 0)
+		return;
+
+	free_irq(irq, data);
+}
+EXPORT_SYMBOL_GPL(arizona_free_irq);
+
+int arizona_set_irq_wake(struct arizona *arizona, int irq, int on)
+{
+	irq = arizona_map_irq(arizona, irq);
+	if (irq < 0)
+		return irq;
+
+	return irq_set_irq_wake(irq, on);
+}
+EXPORT_SYMBOL_GPL(arizona_set_irq_wake);
+
+static irqreturn_t arizona_boot_done(int irq, void *data)
+{
+	struct arizona *arizona = data;
+
+	dev_dbg(arizona->dev, "Boot done\n");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arizona_ctrlif_err(int irq, void *data)
+{
+	struct arizona *arizona = data;
+
+	/*
+	 * For pretty much all potential sources a register cache sync
+	 * won't help, we've just got a software bug somewhere.
+	 */
+	dev_err(arizona->dev, "Control interface error\n");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arizona_irq_thread(int irq, void *data)
+{
+	struct arizona *arizona = data;
+	int i, ret;
+
+	ret = pm_runtime_get_sync(arizona->dev);
+	if (ret < 0) {
+		dev_err(arizona->dev, "Failed to resume device: %d\n", ret);
+		return IRQ_NONE;
+	}
+
+	/* Check both domains */
+	for (i = 0; i < 2; i++)
+		handle_nested_irq(irq_find_mapping(arizona->virq, i));
+
+	pm_runtime_mark_last_busy(arizona->dev);
+	pm_runtime_put_autosuspend(arizona->dev);
+
+	return IRQ_HANDLED;
+}
+
+static void arizona_irq_enable(struct irq_data *data)
+{
+}
+
+static void arizona_irq_disable(struct irq_data *data)
+{
+}
+
+static struct irq_chip arizona_irq_chip = {
+	.name			= "arizona",
+	.irq_disable		= arizona_irq_disable,
+	.irq_enable		= arizona_irq_enable,
+};
+
+static int arizona_irq_map(struct irq_domain *h, unsigned int virq,
+			      irq_hw_number_t hw)
+{
+	struct regmap_irq_chip_data *data = h->host_data;
+
+	irq_set_chip_data(virq, data);
+	irq_set_chip_and_handler(virq, &arizona_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(virq, 1);
+
+	/* ARM needs us to explicitly flag the IRQ as valid
+	 * and will set them noprobe when we do so. */
+#ifdef CONFIG_ARM
+	set_irq_flags(virq, IRQF_VALID);
+#else
+	irq_set_noprobe(virq);
+#endif
+
+	return 0;
+}
+
+static struct irq_domain_ops arizona_domain_ops = {
+	.map	= arizona_irq_map,
+	.xlate	= irq_domain_xlate_twocell,
+};
+
+int arizona_irq_init(struct arizona *arizona)
+{
+	int flags = IRQF_ONESHOT;
+	int ret, i;
+	const struct regmap_irq_chip *aod, *irq;
+
+	switch (arizona->type) {
+#ifdef CONFIG_MFD_WM5102
+	case WM5102:
+		aod = &wm5102_aod;
+		irq = &wm5102_irq;
+		break;
+#endif
+#ifdef CONFIG_MFD_WM5110
+	case WM5110:
+		aod = &wm5110_aod;
+		irq = &wm5110_irq;
+		break;
+#endif
+	default:
+		BUG_ON("Unknown Arizona class device" == NULL);
+		return -EINVAL;
+	}
+
+	if (arizona->pdata.irq_active_high) {
+		ret = regmap_update_bits(arizona->regmap, ARIZONA_IRQ_CTRL_1,
+					 ARIZONA_IRQ_POL, 0);
+		if (ret != 0) {
+			dev_err(arizona->dev, "Couldn't set IRQ polarity: %d\n",
+				ret);
+			goto err;
+		}
+
+		flags |= IRQF_TRIGGER_HIGH;
+	} else {
+		flags |= IRQF_TRIGGER_LOW;
+	}
+
+	/* Allocate a virtual IRQ domain to distribute to the regmap domains */
+	arizona->virq = irq_domain_add_linear(NULL, 2, &arizona_domain_ops,
+					      arizona);
+	if (!arizona->virq) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = regmap_add_irq_chip(arizona->regmap,
+				  irq_create_mapping(arizona->virq, 0),
+				  IRQF_ONESHOT, -1, aod,
+				  &arizona->aod_irq_chip);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to add AOD IRQs: %d\n", ret);
+		goto err_domain;
+	}
+
+	ret = regmap_add_irq_chip(arizona->regmap,
+				  irq_create_mapping(arizona->virq, 1),
+				  IRQF_ONESHOT, -1, irq,
+				  &arizona->irq_chip);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to add AOD IRQs: %d\n", ret);
+		goto err_aod;
+	}
+
+	/* Make sure the boot done IRQ is unmasked for resumes */
+	i = arizona_map_irq(arizona, ARIZONA_IRQ_BOOT_DONE);
+	ret = request_threaded_irq(i, NULL, arizona_boot_done, IRQF_ONESHOT,
+				   "Boot done", arizona);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to request boot done %d: %d\n",
+			arizona->irq, ret);
+		goto err_boot_done;
+	}
+
+	/* Handle control interface errors in the core */
+	i = arizona_map_irq(arizona, ARIZONA_IRQ_CTRLIF_ERR);
+	ret = request_threaded_irq(i, NULL, arizona_ctrlif_err, IRQF_ONESHOT,
+				   "Control interface error", arizona);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to request boot done %d: %d\n",
+			arizona->irq, ret);
+		goto err_ctrlif;
+	}
+
+	ret = request_threaded_irq(arizona->irq, NULL, arizona_irq_thread,
+				   flags, "arizona", arizona);
+
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to request IRQ %d: %d\n",
+			arizona->irq, ret);
+		goto err_main_irq;
+	}
+
+	return 0;
+
+err_main_irq:
+	free_irq(arizona_map_irq(arizona, ARIZONA_IRQ_CTRLIF_ERR), arizona);
+err_ctrlif:
+	free_irq(arizona_map_irq(arizona, ARIZONA_IRQ_BOOT_DONE), arizona);
+err_boot_done:
+	regmap_del_irq_chip(irq_create_mapping(arizona->virq, 1),
+			    arizona->irq_chip);
+err_aod:
+	regmap_del_irq_chip(irq_create_mapping(arizona->virq, 0),
+			    arizona->aod_irq_chip);
+err_domain:
+err:
+	return ret;
+}
+
+int arizona_irq_exit(struct arizona *arizona)
+{
+	free_irq(arizona_map_irq(arizona, ARIZONA_IRQ_CTRLIF_ERR), arizona);
+	free_irq(arizona_map_irq(arizona, ARIZONA_IRQ_BOOT_DONE), arizona);
+	regmap_del_irq_chip(irq_create_mapping(arizona->virq, 1),
+			    arizona->irq_chip);
+	regmap_del_irq_chip(irq_create_mapping(arizona->virq, 0),
+			    arizona->aod_irq_chip);
+	free_irq(arizona->irq, arizona);
+
+	return 0;
+}
diff --git a/drivers/mfd/arizona-spi.c b/drivers/mfd/arizona-spi.c
new file mode 100644
index 00000000000..df2e5a8bee2
--- /dev/null
+++ b/drivers/mfd/arizona-spi.c
@@ -0,0 +1,97 @@
+/*
+ * arizona-spi.c  --  Arizona SPI bus interface
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+
+#include <linux/mfd/arizona/core.h>
+
+#include "arizona.h"
+
+static int __devinit arizona_spi_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	struct arizona *arizona;
+	const struct regmap_config *regmap_config;
+	int ret;
+
+	switch (id->driver_data) {
+#ifdef CONFIG_MFD_WM5102
+	case WM5102:
+		regmap_config = &wm5102_spi_regmap;
+		break;
+#endif
+#ifdef CONFIG_MFD_WM5110
+	case WM5110:
+		regmap_config = &wm5110_spi_regmap;
+		break;
+#endif
+	default:
+		dev_err(&spi->dev, "Unknown device type %ld\n",
+			id->driver_data);
+		return -EINVAL;
+	}
+
+	arizona = devm_kzalloc(&spi->dev, sizeof(*arizona), GFP_KERNEL);
+	if (arizona == NULL)
+		return -ENOMEM;
+
+	arizona->regmap = devm_regmap_init_spi(spi, regmap_config);
+	if (IS_ERR(arizona->regmap)) {
+		ret = PTR_ERR(arizona->regmap);
+		dev_err(&spi->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	arizona->type = id->driver_data;
+	arizona->dev = &spi->dev;
+	arizona->irq = spi->irq;
+
+	return arizona_dev_init(arizona);
+}
+
+static int __devexit arizona_spi_remove(struct spi_device *spi)
+{
+	struct arizona *arizona = dev_get_drvdata(&spi->dev);
+	arizona_dev_exit(arizona);
+	return 0;
+}
+
+static const struct spi_device_id arizona_spi_ids[] = {
+	{ "wm5102", WM5102 },
+	{ "wm5110", WM5110 },
+	{ },
+};
+MODULE_DEVICE_TABLE(spi, arizona_spi_ids);
+
+static struct spi_driver arizona_spi_driver = {
+	.driver = {
+		.name	= "arizona",
+		.owner	= THIS_MODULE,
+		.pm	= &arizona_pm_ops,
+	},
+	.probe		= arizona_spi_probe,
+	.remove		= __devexit_p(arizona_spi_remove),
+	.id_table	= arizona_spi_ids,
+};
+
+module_spi_driver(arizona_spi_driver);
+
+MODULE_DESCRIPTION("Arizona SPI bus interface");
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/arizona.h b/drivers/mfd/arizona.h
new file mode 100644
index 00000000000..9798ae5da67
--- /dev/null
+++ b/drivers/mfd/arizona.h
@@ -0,0 +1,40 @@
+/*
+ * wm5102.h  --  WM5102 MFD internals
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _WM5102_H
+#define _WM5102_H
+
+#include <linux/regmap.h>
+#include <linux/pm.h>
+
+struct wm_arizona;
+
+extern const struct regmap_config wm5102_i2c_regmap;
+extern const struct regmap_config wm5102_spi_regmap;
+
+extern const struct regmap_config wm5110_i2c_regmap;
+extern const struct regmap_config wm5110_spi_regmap;
+
+extern const struct dev_pm_ops arizona_pm_ops;
+
+extern const struct regmap_irq_chip wm5102_aod;
+extern const struct regmap_irq_chip wm5102_irq;
+
+extern const struct regmap_irq_chip wm5110_aod;
+extern const struct regmap_irq_chip wm5110_irq;
+
+int arizona_dev_init(struct arizona *arizona);
+int arizona_dev_exit(struct arizona *arizona);
+int arizona_irq_init(struct arizona *arizona);
+int arizona_irq_exit(struct arizona *arizona);
+
+#endif
diff --git a/drivers/mfd/da9052-core.c b/drivers/mfd/da9052-core.c
index 1f1313c9057..2544910e1fd 100644
--- a/drivers/mfd/da9052-core.c
+++ b/drivers/mfd/da9052-core.c
@@ -772,7 +772,6 @@ EXPORT_SYMBOL_GPL(da9052_regmap_config);
 int __devinit da9052_device_init(struct da9052 *da9052, u8 chip_id)
 {
 	struct da9052_pdata *pdata = da9052->dev->platform_data;
-	struct irq_desc *desc;
 	int ret;
 
 	mutex_init(&da9052->auxadc_lock);
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 50e83dc5dc4..7040a008113 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -28,6 +28,7 @@
 #include <linux/uaccess.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/dbx500-prcmu.h>
+#include <linux/mfd/abx500/ab8500.h>
 #include <linux/regulator/db8500-prcmu.h>
 #include <linux/regulator/machine.h>
 #include <asm/hardware/gic.h>
@@ -2269,10 +2270,10 @@ int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 /**
  * prcmu_ac_wake_req - should be called whenever ARM wants to wakeup Modem
  */
-void prcmu_ac_wake_req(void)
+int prcmu_ac_wake_req(void)
 {
 	u32 val;
-	u32 status;
+	int ret = 0;
 
 	mutex_lock(&mb0_transfer.ac_wake_lock);
 
@@ -2282,39 +2283,32 @@ void prcmu_ac_wake_req(void)
 
 	atomic_set(&ac_wake_req_state, 1);
 
-retry:
-	writel((val | PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ), PRCM_HOSTACCESS_REQ);
+	/*
+	 * Force Modem Wake-up before hostaccess_req ping-pong.
+	 * It prevents Modem to enter in Sleep while acking the hostaccess
+	 * request. The 31us delay has been calculated by HWI.
+	 */
+	val |= PRCM_HOSTACCESS_REQ_WAKE_REQ;
+	writel(val, PRCM_HOSTACCESS_REQ);
+
+	udelay(31);
+
+	val |= PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ;
+	writel(val, PRCM_HOSTACCESS_REQ);
 
 	if (!wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
 			msecs_to_jiffies(5000))) {
+#if defined(CONFIG_DBX500_PRCMU_DEBUG)
+		db8500_prcmu_debug_dump(__func__, true, true);
+#endif
 		pr_crit("prcmu: %s timed out (5 s) waiting for a reply.\n",
 			__func__);
-		goto unlock_and_return;
-	}
-
-	/*
-	 * The modem can generate an AC_WAKE_ACK, and then still go to sleep.
-	 * As a workaround, we wait, and then check that the modem is indeed
-	 * awake (in terms of the value of the PRCM_MOD_AWAKE_STATUS
-	 * register, which may not be the whole truth).
-	 */
-	udelay(400);
-	status = (readl(PRCM_MOD_AWAKE_STATUS) & BITS(0, 2));
-	if (status != (PRCM_MOD_AWAKE_STATUS_PRCM_MOD_AAPD_AWAKE |
-			PRCM_MOD_AWAKE_STATUS_PRCM_MOD_COREPD_AWAKE)) {
-		pr_err("prcmu: %s received ack, but modem not awake (0x%X).\n",
-			__func__, status);
-		udelay(1200);
-		writel(val, PRCM_HOSTACCESS_REQ);
-		if (wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
-				msecs_to_jiffies(5000)))
-			goto retry;
-		pr_crit("prcmu: %s timed out (5 s) waiting for AC_SLEEP_ACK.\n",
-			__func__);
+		ret = -EFAULT;
 	}
 
 unlock_and_return:
 	mutex_unlock(&mb0_transfer.ac_wake_lock);
+	return ret;
 }
 
 /**
@@ -2945,14 +2939,31 @@ static struct regulator_init_data db8500_regulators[DB8500_NUM_REGULATORS] = {
 	},
 };
 
+static struct resource ab8500_resources[] = {
+	[0] = {
+		.start	= IRQ_DB8500_AB8500,
+		.end	= IRQ_DB8500_AB8500,
+		.flags	= IORESOURCE_IRQ
+	}
+};
+
 static struct mfd_cell db8500_prcmu_devs[] = {
 	{
 		.name = "db8500-prcmu-regulators",
+		.of_compatible = "stericsson,db8500-prcmu-regulator",
 		.platform_data = &db8500_regulators,
 		.pdata_size = sizeof(db8500_regulators),
 	},
 	{
 		.name = "cpufreq-u8500",
+		.of_compatible = "stericsson,cpufreq-u8500",
+	},
+	{
+		.name = "ab8500-core",
+		.of_compatible = "stericsson,ab8500",
+		.num_resources = ARRAY_SIZE(ab8500_resources),
+		.resources = ab8500_resources,
+		.id = AB8500_VERSION_AB8500,
 	},
 };
 
@@ -2962,8 +2973,9 @@ static struct mfd_cell db8500_prcmu_devs[] = {
  */
 static int __devinit db8500_prcmu_probe(struct platform_device *pdev)
 {
+	struct ab8500_platform_data *ab8500_platdata = pdev->dev.platform_data;
 	struct device_node *np = pdev->dev.of_node;
-	int irq = 0, err = 0;
+	int irq = 0, err = 0, i;
 
 	if (ux500_is_svp())
 		return -ENODEV;
@@ -2987,16 +2999,21 @@ static int __devinit db8500_prcmu_probe(struct platform_device *pdev)
 		goto no_irq_return;
 	}
 
+	for (i = 0; i < ARRAY_SIZE(db8500_prcmu_devs); i++) {
+		if (!strcmp(db8500_prcmu_devs[i].name, "ab8500-core")) {
+			db8500_prcmu_devs[i].platform_data = ab8500_platdata;
+			db8500_prcmu_devs[i].pdata_size = sizeof(struct ab8500_platform_data);
+		}
+	}
+
 	if (cpu_is_u8500v20_or_later())
 		prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
 
-	if (!np) {
-		err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
-				ARRAY_SIZE(db8500_prcmu_devs), NULL, 0);
-		if (err) {
-			pr_err("prcmu: Failed to add subdevices\n");
-			return err;
-		}
+	err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
+			ARRAY_SIZE(db8500_prcmu_devs), NULL, 0);
+	if (err) {
+		pr_err("prcmu: Failed to add subdevices\n");
+		return err;
 	}
 
 	pr_info("DB8500 PRCMU initialized\n");
@@ -3004,11 +3021,16 @@ static int __devinit db8500_prcmu_probe(struct platform_device *pdev)
 no_irq_return:
 	return err;
 }
+static const struct of_device_id db8500_prcmu_match[] = {
+	{ .compatible = "stericsson,db8500-prcmu"},
+	{ },
+};
 
 static struct platform_driver db8500_prcmu_driver = {
 	.driver = {
 		.name = "db8500-prcmu",
 		.owner = THIS_MODULE,
+		.of_match_table = db8500_prcmu_match,
 	},
 	.probe = db8500_prcmu_probe,
 };
@@ -3018,7 +3040,7 @@ static int __init db8500_prcmu_init(void)
 	return platform_driver_register(&db8500_prcmu_driver);
 }
 
-arch_initcall(db8500_prcmu_init);
+core_initcall(db8500_prcmu_init);
 
 MODULE_AUTHOR("Mattias Nilsson <mattias.i.nilsson@stericsson.com>");
 MODULE_DESCRIPTION("DB8500 PRCM Unit driver");
diff --git a/drivers/mfd/dbx500-prcmu-regs.h b/drivers/mfd/dbx500-prcmu-regs.h
index 3a0bf91d778..23108a6e316 100644
--- a/drivers/mfd/dbx500-prcmu-regs.h
+++ b/drivers/mfd/dbx500-prcmu-regs.h
@@ -106,6 +106,7 @@
 
 #define PRCM_HOSTACCESS_REQ	(_PRCMU_BASE + 0x334)
 #define PRCM_HOSTACCESS_REQ_HOSTACCESS_REQ 0x1
+#define PRCM_HOSTACCESS_REQ_WAKE_REQ	BIT(16)
 #define ARM_WAKEUP_MODEM	0x1
 
 #define PRCM_ARM_IT1_CLR	(_PRCMU_BASE + 0x48C)
diff --git a/drivers/mfd/max77686-irq.c b/drivers/mfd/max77686-irq.c
new file mode 100644
index 00000000000..cdc3280e2ec
--- /dev/null
+++ b/drivers/mfd/max77686-irq.c
@@ -0,0 +1,319 @@
+/*
+ * max77686-irq.c - Interrupt controller support for MAX77686
+ *
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ * Chiwoong Byun <woong.byun@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * This driver is based on max8997-irq.c
+ */
+
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/mfd/max77686.h>
+#include <linux/mfd/max77686-private.h>
+#include <linux/irqdomain.h>
+#include <linux/regmap.h>
+
+enum {
+	MAX77686_DEBUG_IRQ_INFO = 1 << 0,
+	MAX77686_DEBUG_IRQ_MASK = 1 << 1,
+	MAX77686_DEBUG_IRQ_INT = 1 << 2,
+};
+
+static int debug_mask = 0;
+module_param(debug_mask, int, 0);
+MODULE_PARM_DESC(debug_mask, "Set debug_mask : 0x0=off 0x1=IRQ_INFO  0x2=IRQ_MASK 0x4=IRQ_INI)");
+
+static const u8 max77686_mask_reg[] = {
+	[PMIC_INT1] = MAX77686_REG_INT1MSK,
+	[PMIC_INT2] = MAX77686_REG_INT2MSK,
+	[RTC_INT] = MAX77686_RTC_INTM,
+};
+
+static struct regmap *max77686_get_regmap(struct max77686_dev *max77686,
+				enum max77686_irq_source src)
+{
+	switch (src) {
+	case PMIC_INT1 ... PMIC_INT2:
+		return max77686->regmap;
+	case RTC_INT:
+		return max77686->rtc_regmap;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+}
+
+struct max77686_irq_data {
+	int mask;
+	enum max77686_irq_source group;
+};
+
+#define DECLARE_IRQ(idx, _group, _mask)		\
+	[(idx)] = { .group = (_group), .mask = (_mask) }
+static const struct max77686_irq_data max77686_irqs[] = {
+	DECLARE_IRQ(MAX77686_PMICIRQ_PWRONF,	PMIC_INT1, 1 << 0),
+	DECLARE_IRQ(MAX77686_PMICIRQ_PWRONR,	PMIC_INT1, 1 << 1),
+	DECLARE_IRQ(MAX77686_PMICIRQ_JIGONBF,	PMIC_INT1, 1 << 2),
+	DECLARE_IRQ(MAX77686_PMICIRQ_JIGONBR,	PMIC_INT1, 1 << 3),
+	DECLARE_IRQ(MAX77686_PMICIRQ_ACOKBF,	PMIC_INT1, 1 << 4),
+	DECLARE_IRQ(MAX77686_PMICIRQ_ACOKBR,	PMIC_INT1, 1 << 5),
+	DECLARE_IRQ(MAX77686_PMICIRQ_ONKEY1S,	PMIC_INT1, 1 << 6),
+	DECLARE_IRQ(MAX77686_PMICIRQ_MRSTB,		PMIC_INT1, 1 << 7),
+	DECLARE_IRQ(MAX77686_PMICIRQ_140C,		PMIC_INT2, 1 << 0),
+	DECLARE_IRQ(MAX77686_PMICIRQ_120C,		PMIC_INT2, 1 << 1),
+	DECLARE_IRQ(MAX77686_RTCIRQ_RTC60S,		RTC_INT, 1 << 0),
+	DECLARE_IRQ(MAX77686_RTCIRQ_RTCA1,		RTC_INT, 1 << 1),
+	DECLARE_IRQ(MAX77686_RTCIRQ_RTCA2,		RTC_INT, 1 << 2),
+	DECLARE_IRQ(MAX77686_RTCIRQ_SMPL,		RTC_INT, 1 << 3),
+	DECLARE_IRQ(MAX77686_RTCIRQ_RTC1S,		RTC_INT, 1 << 4),
+	DECLARE_IRQ(MAX77686_RTCIRQ_WTSR,		RTC_INT, 1 << 5),
+};
+
+static void max77686_irq_lock(struct irq_data *data)
+{
+	struct max77686_dev *max77686 = irq_get_chip_data(data->irq);
+
+	if (debug_mask & MAX77686_DEBUG_IRQ_MASK)
+		pr_info("%s\n", __func__);
+
+	mutex_lock(&max77686->irqlock);
+}
+
+static void max77686_irq_sync_unlock(struct irq_data *data)
+{
+	struct max77686_dev *max77686 = irq_get_chip_data(data->irq);
+	int i;
+
+	for (i = 0; i < MAX77686_IRQ_GROUP_NR; i++) {
+		u8 mask_reg = max77686_mask_reg[i];
+		struct regmap *map = max77686_get_regmap(max77686, i);
+
+		if (debug_mask & MAX77686_DEBUG_IRQ_MASK)
+			pr_debug("%s: mask_reg[%d]=0x%x, cur=0x%x\n",
+			__func__, i, mask_reg, max77686->irq_masks_cur[i]);
+
+		if (mask_reg == MAX77686_REG_INVALID ||
+				IS_ERR_OR_NULL(map))
+			continue;
+
+		max77686->irq_masks_cache[i] = max77686->irq_masks_cur[i];
+
+		regmap_write(map, max77686_mask_reg[i],
+				max77686->irq_masks_cur[i]);
+	}
+
+	mutex_unlock(&max77686->irqlock);
+}
+
+static const inline struct max77686_irq_data *to_max77686_irq(int irq)
+{
+	struct irq_data *data = irq_get_irq_data(irq);
+	return &max77686_irqs[data->hwirq];
+}
+
+static void max77686_irq_mask(struct irq_data *data)
+{
+	struct max77686_dev *max77686 = irq_get_chip_data(data->irq);
+	const struct max77686_irq_data *irq_data = to_max77686_irq(data->irq);
+
+	max77686->irq_masks_cur[irq_data->group] |= irq_data->mask;
+
+	if (debug_mask & MAX77686_DEBUG_IRQ_MASK)
+		pr_info("%s: group=%d, cur=0x%x\n",
+			__func__, irq_data->group,
+			max77686->irq_masks_cur[irq_data->group]);
+}
+
+static void max77686_irq_unmask(struct irq_data *data)
+{
+	struct max77686_dev *max77686 = irq_get_chip_data(data->irq);
+	const struct max77686_irq_data *irq_data = to_max77686_irq(data->irq);
+
+	max77686->irq_masks_cur[irq_data->group] &= ~irq_data->mask;
+
+	if (debug_mask & MAX77686_DEBUG_IRQ_MASK)
+		pr_info("%s: group=%d, cur=0x%x\n",
+			__func__, irq_data->group,
+			max77686->irq_masks_cur[irq_data->group]);
+}
+
+static struct irq_chip max77686_irq_chip = {
+	.name			= "max77686",
+	.irq_bus_lock		= max77686_irq_lock,
+	.irq_bus_sync_unlock	= max77686_irq_sync_unlock,
+	.irq_mask		= max77686_irq_mask,
+	.irq_unmask		= max77686_irq_unmask,
+};
+
+static irqreturn_t max77686_irq_thread(int irq, void *data)
+{
+	struct max77686_dev *max77686 = data;
+	unsigned int irq_reg[MAX77686_IRQ_GROUP_NR] = {};
+	unsigned int irq_src;
+	int ret;
+	int i, cur_irq;
+
+	ret = regmap_read(max77686->regmap,  MAX77686_REG_INTSRC, &irq_src);
+	if (ret < 0) {
+		dev_err(max77686->dev, "Failed to read interrupt source: %d\n",
+				ret);
+		return IRQ_NONE;
+	}
+
+	if (debug_mask & MAX77686_DEBUG_IRQ_INT)
+		pr_info("%s: irq_src=0x%x\n", __func__, irq_src);
+
+	if (irq_src == MAX77686_IRQSRC_PMIC) {
+		ret = regmap_bulk_read(max77686->regmap,
+					 MAX77686_REG_INT1, irq_reg, 2);
+		if (ret < 0) {
+			dev_err(max77686->dev, "Failed to read interrupt source: %d\n",
+					ret);
+			return IRQ_NONE;
+		}
+
+		if (debug_mask & MAX77686_DEBUG_IRQ_INT)
+			pr_info("%s: int1=0x%x, int2=0x%x\n", __func__,
+				 irq_reg[PMIC_INT1], irq_reg[PMIC_INT2]);
+	}
+
+	if (irq_src & MAX77686_IRQSRC_RTC) {
+		ret = regmap_read(max77686->rtc_regmap,
+					MAX77686_RTC_INT, &irq_reg[RTC_INT]);
+		if (ret < 0) {
+			dev_err(max77686->dev, "Failed to read interrupt source: %d\n",
+					ret);
+			return IRQ_NONE;
+		}
+
+		if (debug_mask & MAX77686_DEBUG_IRQ_INT)
+			pr_info("%s: rtc int=0x%x\n", __func__,
+							 irq_reg[RTC_INT]);
+
+	}
+
+	for (i = 0; i < MAX77686_IRQ_GROUP_NR; i++)
+		irq_reg[i] &= ~max77686->irq_masks_cur[i];
+
+	for (i = 0; i < MAX77686_IRQ_NR; i++) {
+		if (irq_reg[max77686_irqs[i].group] & max77686_irqs[i].mask) {
+			cur_irq = irq_find_mapping(max77686->irq_domain, i);
+			if (cur_irq)
+				handle_nested_irq(cur_irq);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int max77686_irq_domain_map(struct irq_domain *d, unsigned int irq,
+					irq_hw_number_t hw)
+{
+	struct max77686_dev *max77686 = d->host_data;
+
+	irq_set_chip_data(irq, max77686);
+	irq_set_chip_and_handler(irq, &max77686_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+	set_irq_flags(irq, IRQF_VALID);
+#else
+	irq_set_noprobe(irq);
+#endif
+	return 0;
+}
+
+static struct irq_domain_ops max77686_irq_domain_ops = {
+	.map = max77686_irq_domain_map,
+};
+
+int max77686_irq_init(struct max77686_dev *max77686)
+{
+	struct irq_domain *domain;
+	int i;
+	int ret;
+	int val;
+	struct regmap *map;
+
+	mutex_init(&max77686->irqlock);
+
+	if (max77686->irq_gpio && !max77686->irq) {
+		max77686->irq = gpio_to_irq(max77686->irq_gpio);
+
+		if (debug_mask & MAX77686_DEBUG_IRQ_INT) {
+			ret = gpio_request(max77686->irq_gpio, "pmic_irq");
+			if (ret < 0) {
+				dev_err(max77686->dev,
+					"Failed to request gpio %d with ret:"
+					"%d\n",	max77686->irq_gpio, ret);
+				return IRQ_NONE;
+			}
+
+			gpio_direction_input(max77686->irq_gpio);
+			val = gpio_get_value(max77686->irq_gpio);
+			gpio_free(max77686->irq_gpio);
+			pr_info("%s: gpio_irq=%x\n", __func__, val);
+		}
+	}
+
+	if (!max77686->irq) {
+		dev_err(max77686->dev, "irq is not specified\n");
+		return -ENODEV;
+	}
+
+	/* Mask individual interrupt sources */
+	for (i = 0; i < MAX77686_IRQ_GROUP_NR; i++) {
+		max77686->irq_masks_cur[i] = 0xff;
+		max77686->irq_masks_cache[i] = 0xff;
+		map = max77686_get_regmap(max77686, i);
+
+		if (IS_ERR_OR_NULL(map))
+			continue;
+		if (max77686_mask_reg[i] == MAX77686_REG_INVALID)
+			continue;
+
+		regmap_write(map, max77686_mask_reg[i], 0xff);
+	}
+	domain = irq_domain_add_linear(NULL, MAX77686_IRQ_NR,
+					&max77686_irq_domain_ops, max77686);
+	if (!domain) {
+		dev_err(max77686->dev, "could not create irq domain\n");
+		return -ENODEV;
+	}
+	max77686->irq_domain = domain;
+
+	ret = request_threaded_irq(max77686->irq, NULL, max77686_irq_thread,
+				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				   "max77686-irq", max77686);
+
+	if (ret)
+		dev_err(max77686->dev, "Failed to request IRQ %d: %d\n",
+			max77686->irq, ret);
+
+
+	if (debug_mask & MAX77686_DEBUG_IRQ_INFO)
+		pr_info("%s-\n", __func__);
+
+	return 0;
+}
+
+void max77686_irq_exit(struct max77686_dev *max77686)
+{
+	if (max77686->irq)
+		free_irq(max77686->irq, max77686);
+}
diff --git a/drivers/mfd/max77686.c b/drivers/mfd/max77686.c
new file mode 100644
index 00000000000..c03e12b5192
--- /dev/null
+++ b/drivers/mfd/max77686.c
@@ -0,0 +1,187 @@
+/*
+ * max77686.c - mfd core driver for the Maxim 77686
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * Chiwoong Byun <woong.byun@smasung.com>
+ * Jonghwa Lee <jonghwa3.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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 is based on max8997.c
+ */
+
+#include <linux/export.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+#include <linux/module.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max77686.h>
+#include <linux/mfd/max77686-private.h>
+#include <linux/err.h>
+
+#define I2C_ADDR_RTC	(0x0C >> 1)
+
+static struct mfd_cell max77686_devs[] = {
+	{ .name = "max77686-pmic", },
+	{ .name = "max77686-rtc", },
+};
+
+static struct regmap_config max77686_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+#ifdef CONFIG_OF
+static struct of_device_id __devinitdata max77686_pmic_dt_match[] = {
+	{.compatible = "maxim,max77686",        .data = 0},
+	{},
+};
+
+static struct max77686_platform_data *max77686_i2c_parse_dt_pdata(struct device
+								  *dev)
+{
+	struct max77686_platform_data *pd;
+
+	pd = devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd) {
+		dev_err(dev, "could not allocate memory for pdata\n");
+		return NULL;
+	}
+
+	dev->platform_data = pd;
+	return pd;
+}
+#else
+static struct max77686_platform_data *max77686_i2c_parse_dt_pdata(struct device
+								  *dev)
+{
+	return 0;
+}
+#endif
+
+static int max77686_i2c_probe(struct i2c_client *i2c,
+			      const struct i2c_device_id *id)
+{
+	struct max77686_dev *max77686 = NULL;
+	struct max77686_platform_data *pdata = i2c->dev.platform_data;
+	unsigned int data;
+	int ret = 0;
+
+	if (i2c->dev.of_node)
+		pdata = max77686_i2c_parse_dt_pdata(&i2c->dev);
+
+	if (!pdata) {
+		ret = -EIO;
+		dev_err(&i2c->dev, "No platform data found.\n");
+		goto err;
+	}
+
+	max77686 = kzalloc(sizeof(struct max77686_dev), GFP_KERNEL);
+	if (max77686 == NULL)
+		return -ENOMEM;
+
+	max77686->regmap = regmap_init_i2c(i2c, &max77686_regmap_config);
+	if (IS_ERR(max77686->regmap)) {
+		ret = PTR_ERR(max77686->regmap);
+		dev_err(max77686->dev, "Failed to allocate register map: %d\n",
+				ret);
+		kfree(max77686);
+		return ret;
+	}
+
+	i2c_set_clientdata(i2c, max77686);
+	max77686->dev = &i2c->dev;
+	max77686->i2c = i2c;
+	max77686->type = id->driver_data;
+
+	max77686->wakeup = pdata->wakeup;
+	max77686->irq_gpio = pdata->irq_gpio;
+	max77686->irq = i2c->irq;
+
+	if (regmap_read(max77686->regmap,
+			 MAX77686_REG_DEVICE_ID, &data) < 0) {
+		dev_err(max77686->dev,
+			"device not found on this channel (this is not an error)\n");
+		ret = -ENODEV;
+		goto err;
+	} else
+		dev_info(max77686->dev, "device found\n");
+
+	max77686->rtc = i2c_new_dummy(i2c->adapter, I2C_ADDR_RTC);
+	i2c_set_clientdata(max77686->rtc, max77686);
+
+	max77686_irq_init(max77686);
+
+	ret = mfd_add_devices(max77686->dev, -1, max77686_devs,
+			      ARRAY_SIZE(max77686_devs), NULL, 0);
+
+	if (ret < 0)
+		goto err_mfd;
+
+	return ret;
+
+err_mfd:
+	mfd_remove_devices(max77686->dev);
+	i2c_unregister_device(max77686->rtc);
+err:
+	kfree(max77686);
+	return ret;
+}
+
+static int max77686_i2c_remove(struct i2c_client *i2c)
+{
+	struct max77686_dev *max77686 = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(max77686->dev);
+	i2c_unregister_device(max77686->rtc);
+	kfree(max77686);
+
+	return 0;
+}
+
+static const struct i2c_device_id max77686_i2c_id[] = {
+	{ "max77686", TYPE_MAX77686 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, max77686_i2c_id);
+
+static struct i2c_driver max77686_i2c_driver = {
+	.driver = {
+		   .name = "max77686",
+		   .owner = THIS_MODULE,
+		   .of_match_table = of_match_ptr(max77686_pmic_dt_match),
+	},
+	.probe = max77686_i2c_probe,
+	.remove = max77686_i2c_remove,
+	.id_table = max77686_i2c_id,
+};
+
+static int __init max77686_i2c_init(void)
+{
+	return i2c_add_driver(&max77686_i2c_driver);
+}
+/* init early so consumer devices can complete system boot */
+subsys_initcall(max77686_i2c_init);
+
+static void __exit max77686_i2c_exit(void)
+{
+	i2c_del_driver(&max77686_i2c_driver);
+}
+module_exit(max77686_i2c_exit);
+
+MODULE_DESCRIPTION("MAXIM 77686 multi-function core driver");
+MODULE_AUTHOR("Chiwoong Byun <woong.byun@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/max77693.c b/drivers/mfd/max77693.c
index e9e4278722f..a1811cb50ec 100644
--- a/drivers/mfd/max77693.c
+++ b/drivers/mfd/max77693.c
@@ -138,8 +138,6 @@ static int max77693_i2c_probe(struct i2c_client *i2c,
 
 	max77693->wakeup = pdata->wakeup;
 
-	mutex_init(&max77693->iolock);
-
 	if (max77693_read_reg(max77693->regmap,
 				MAX77693_PMIC_REG_PMIC_ID2, &reg_data) < 0) {
 		dev_err(max77693->dev, "device not found on this channel\n");
@@ -156,7 +154,7 @@ static int max77693_i2c_probe(struct i2c_client *i2c,
 
 	ret = max77693_irq_init(max77693);
 	if (ret < 0)
-		goto err_mfd;
+		goto err_irq;
 
 	pm_runtime_set_active(max77693->dev);
 
@@ -170,11 +168,11 @@ static int max77693_i2c_probe(struct i2c_client *i2c,
 	return ret;
 
 err_mfd:
+	max77693_irq_exit(max77693);
+err_irq:
 	i2c_unregister_device(max77693->muic);
 	i2c_unregister_device(max77693->haptic);
 err_regmap:
-	kfree(max77693);
-
 	return ret;
 }
 
@@ -183,6 +181,7 @@ static int max77693_i2c_remove(struct i2c_client *i2c)
 	struct max77693_dev *max77693 = i2c_get_clientdata(i2c);
 
 	mfd_remove_devices(max77693->dev);
+	max77693_irq_exit(max77693);
 	i2c_unregister_device(max77693->muic);
 	i2c_unregister_device(max77693->haptic);
 
@@ -215,7 +214,7 @@ static int max77693_resume(struct device *dev)
 	return max77693_irq_resume(max77693);
 }
 
-const struct dev_pm_ops max77693_pm = {
+static const struct dev_pm_ops max77693_pm = {
 	.suspend = max77693_suspend,
 	.resume = max77693_resume,
 };
diff --git a/drivers/mfd/max8925-core.c b/drivers/mfd/max8925-core.c
index ca881efedf7..825a7f06d9b 100644
--- a/drivers/mfd/max8925-core.c
+++ b/drivers/mfd/max8925-core.c
@@ -75,9 +75,9 @@ static struct mfd_cell power_devs[] = {
 static struct resource rtc_resources[] = {
 	{
 		.name	= "max8925-rtc",
-		.start	= MAX8925_RTC_IRQ,
-		.end	= MAX8925_RTC_IRQ_MASK,
-		.flags	= IORESOURCE_IO,
+		.start	= MAX8925_IRQ_RTC_ALARM0,
+		.end	= MAX8925_IRQ_RTC_ALARM0,
+		.flags	= IORESOURCE_IRQ,
 	},
 };
 
@@ -598,7 +598,7 @@ int __devinit max8925_device_init(struct max8925_chip *chip,
 
 	ret = mfd_add_devices(chip->dev, 0, &rtc_devs[0],
 			      ARRAY_SIZE(rtc_devs),
-			      &rtc_resources[0], 0);
+			      &rtc_resources[0], chip->irq_base);
 	if (ret < 0) {
 		dev_err(chip->dev, "Failed to add rtc subdev\n");
 		goto out;
diff --git a/drivers/mfd/max8997-irq.c b/drivers/mfd/max8997-irq.c
index 09274cf7c33..43fa61413e9 100644
--- a/drivers/mfd/max8997-irq.c
+++ b/drivers/mfd/max8997-irq.c
@@ -142,7 +142,8 @@ static void max8997_irq_sync_unlock(struct irq_data *data)
 static const inline struct max8997_irq_data *
 irq_to_max8997_irq(struct max8997_dev *max8997, int irq)
 {
-	return &max8997_irqs[irq - max8997->irq_base];
+	struct irq_data *data = irq_get_irq_data(irq);
+	return &max8997_irqs[data->hwirq];
 }
 
 static void max8997_irq_mask(struct irq_data *data)
@@ -182,7 +183,7 @@ static irqreturn_t max8997_irq_thread(int irq, void *data)
 	u8 irq_reg[MAX8997_IRQ_GROUP_NR] = {};
 	u8 irq_src;
 	int ret;
-	int i;
+	int i, cur_irq;
 
 	ret = max8997_read_reg(max8997->i2c, MAX8997_REG_INTSRC, &irq_src);
 	if (ret < 0) {
@@ -269,8 +270,11 @@ static irqreturn_t max8997_irq_thread(int irq, void *data)
 
 	/* Report */
 	for (i = 0; i < MAX8997_IRQ_NR; i++) {
-		if (irq_reg[max8997_irqs[i].group] & max8997_irqs[i].mask)
-			handle_nested_irq(max8997->irq_base + i);
+		if (irq_reg[max8997_irqs[i].group] & max8997_irqs[i].mask) {
+			cur_irq = irq_find_mapping(max8997->irq_domain, i);
+			if (cur_irq)
+				handle_nested_irq(cur_irq);
+		}
 	}
 
 	return IRQ_HANDLED;
@@ -278,26 +282,40 @@ static irqreturn_t max8997_irq_thread(int irq, void *data)
 
 int max8997_irq_resume(struct max8997_dev *max8997)
 {
-	if (max8997->irq && max8997->irq_base)
-		max8997_irq_thread(max8997->irq_base, max8997);
+	if (max8997->irq && max8997->irq_domain)
+		max8997_irq_thread(0, max8997);
+	return 0;
+}
+
+static int max8997_irq_domain_map(struct irq_domain *d, unsigned int irq,
+					irq_hw_number_t hw)
+{
+	struct max8997_dev *max8997 = d->host_data;
+
+	irq_set_chip_data(irq, max8997);
+	irq_set_chip_and_handler(irq, &max8997_irq_chip, handle_edge_irq);
+	irq_set_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+	set_irq_flags(irq, IRQF_VALID);
+#else
+	irq_set_noprobe(irq);
+#endif
 	return 0;
 }
 
+static struct irq_domain_ops max8997_irq_domain_ops = {
+	.map = max8997_irq_domain_map,
+};
+
 int max8997_irq_init(struct max8997_dev *max8997)
 {
+	struct irq_domain *domain;
 	int i;
-	int cur_irq;
 	int ret;
 	u8 val;
 
 	if (!max8997->irq) {
 		dev_warn(max8997->dev, "No interrupt specified.\n");
-		max8997->irq_base = 0;
-		return 0;
-	}
-
-	if (!max8997->irq_base) {
-		dev_err(max8997->dev, "No interrupt base specified.\n");
 		return 0;
 	}
 
@@ -327,19 +345,13 @@ int max8997_irq_init(struct max8997_dev *max8997)
 					true : false;
 	}
 
-	/* Register with genirq */
-	for (i = 0; i < MAX8997_IRQ_NR; i++) {
-		cur_irq = i + max8997->irq_base;
-		irq_set_chip_data(cur_irq, max8997);
-		irq_set_chip_and_handler(cur_irq, &max8997_irq_chip,
-				handle_edge_irq);
-		irq_set_nested_thread(cur_irq, 1);
-#ifdef CONFIG_ARM
-		set_irq_flags(cur_irq, IRQF_VALID);
-#else
-		irq_set_noprobe(cur_irq);
-#endif
+	domain = irq_domain_add_linear(NULL, MAX8997_IRQ_NR,
+					&max8997_irq_domain_ops, max8997);
+	if (!domain) {
+		dev_err(max8997->dev, "could not create irq domain\n");
+		return -ENODEV;
 	}
+	max8997->irq_domain = domain;
 
 	ret = request_threaded_irq(max8997->irq, NULL, max8997_irq_thread,
 			IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
diff --git a/drivers/mfd/max8997.c b/drivers/mfd/max8997.c
index cb83a7ab53e..10b629c245b 100644
--- a/drivers/mfd/max8997.c
+++ b/drivers/mfd/max8997.c
@@ -143,7 +143,6 @@ static int max8997_i2c_probe(struct i2c_client *i2c,
 	if (!pdata)
 		goto err;
 
-	max8997->irq_base = pdata->irq_base;
 	max8997->ono = pdata->ono;
 
 	mutex_init(&max8997->iolock);
@@ -206,7 +205,7 @@ static const struct i2c_device_id max8997_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, max8998_i2c_id);
 
-u8 max8997_dumpaddr_pmic[] = {
+static u8 max8997_dumpaddr_pmic[] = {
 	MAX8997_REG_INT1MSK,
 	MAX8997_REG_INT2MSK,
 	MAX8997_REG_INT3MSK,
@@ -331,7 +330,7 @@ u8 max8997_dumpaddr_pmic[] = {
 	MAX8997_REG_DVSOKTIMER5,
 };
 
-u8 max8997_dumpaddr_muic[] = {
+static u8 max8997_dumpaddr_muic[] = {
 	MAX8997_MUIC_REG_INTMASK1,
 	MAX8997_MUIC_REG_INTMASK2,
 	MAX8997_MUIC_REG_INTMASK3,
@@ -341,7 +340,7 @@ u8 max8997_dumpaddr_muic[] = {
 	MAX8997_MUIC_REG_CONTROL3,
 };
 
-u8 max8997_dumpaddr_haptic[] = {
+static u8 max8997_dumpaddr_haptic[] = {
 	MAX8997_HAPTIC_REG_CONF1,
 	MAX8997_HAPTIC_REG_CONF2,
 	MAX8997_HAPTIC_REG_DRVCONF,
@@ -423,7 +422,7 @@ static int max8997_resume(struct device *dev)
 	return max8997_irq_resume(max8997);
 }
 
-const struct dev_pm_ops max8997_pm = {
+static const struct dev_pm_ops max8997_pm = {
 	.suspend = max8997_suspend,
 	.resume = max8997_resume,
 	.freeze = max8997_freeze,
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index f0ea3b8b3e4..b801dc72f04 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -723,10 +723,6 @@ void mc13xxx_common_cleanup(struct mc13xxx *mc13xxx)
 	free_irq(mc13xxx->irq, mc13xxx);
 
 	mfd_remove_devices(mc13xxx->dev);
-
-	regmap_exit(mc13xxx->regmap);
-
-	kfree(mc13xxx);
 }
 EXPORT_SYMBOL_GPL(mc13xxx_common_cleanup);
 
diff --git a/drivers/mfd/mc13xxx-i2c.c b/drivers/mfd/mc13xxx-i2c.c
index d22501dad6a..9d18dde3cd2 100644
--- a/drivers/mfd/mc13xxx-i2c.c
+++ b/drivers/mfd/mc13xxx-i2c.c
@@ -53,17 +53,11 @@ static struct regmap_config mc13xxx_regmap_i2c_config = {
 static int mc13xxx_i2c_probe(struct i2c_client *client,
 		const struct i2c_device_id *id)
 {
-	const struct of_device_id *of_id;
-	struct i2c_driver *idrv = to_i2c_driver(client->dev.driver);
 	struct mc13xxx *mc13xxx;
 	struct mc13xxx_platform_data *pdata = dev_get_platdata(&client->dev);
 	int ret;
 
-	of_id = of_match_device(mc13xxx_dt_ids, &client->dev);
-	if (of_id)
-		idrv->id_table = (const struct i2c_device_id*) of_id->data;
-
-	mc13xxx = kzalloc(sizeof(*mc13xxx), GFP_KERNEL);
+	mc13xxx = devm_kzalloc(&client->dev, sizeof(*mc13xxx), GFP_KERNEL);
 	if (!mc13xxx)
 		return -ENOMEM;
 
@@ -72,13 +66,13 @@ static int mc13xxx_i2c_probe(struct i2c_client *client,
 	mc13xxx->dev = &client->dev;
 	mutex_init(&mc13xxx->lock);
 
-	mc13xxx->regmap = regmap_init_i2c(client, &mc13xxx_regmap_i2c_config);
+	mc13xxx->regmap = devm_regmap_init_i2c(client,
+					       &mc13xxx_regmap_i2c_config);
 	if (IS_ERR(mc13xxx->regmap)) {
 		ret = PTR_ERR(mc13xxx->regmap);
 		dev_err(mc13xxx->dev, "Failed to initialize register map: %d\n",
 				ret);
 		dev_set_drvdata(&client->dev, NULL);
-		kfree(mc13xxx);
 		return ret;
 	}
 
diff --git a/drivers/mfd/mc13xxx-spi.c b/drivers/mfd/mc13xxx-spi.c
index 03df422feb7..0bdb43a0aff 100644
--- a/drivers/mfd/mc13xxx-spi.c
+++ b/drivers/mfd/mc13xxx-spi.c
@@ -119,17 +119,11 @@ static struct regmap_bus regmap_mc13xxx_bus = {
 
 static int mc13xxx_spi_probe(struct spi_device *spi)
 {
-	const struct of_device_id *of_id;
-	struct spi_driver *sdrv = to_spi_driver(spi->dev.driver);
 	struct mc13xxx *mc13xxx;
 	struct mc13xxx_platform_data *pdata = dev_get_platdata(&spi->dev);
 	int ret;
 
-	of_id = of_match_device(mc13xxx_dt_ids, &spi->dev);
-	if (of_id)
-		sdrv->id_table = &mc13xxx_device_id[(enum mc13xxx_id) of_id->data];
-
-	mc13xxx = kzalloc(sizeof(*mc13xxx), GFP_KERNEL);
+	mc13xxx = devm_kzalloc(&spi->dev, sizeof(*mc13xxx), GFP_KERNEL);
 	if (!mc13xxx)
 		return -ENOMEM;
 
@@ -139,15 +133,14 @@ static int mc13xxx_spi_probe(struct spi_device *spi)
 	mc13xxx->dev = &spi->dev;
 	mutex_init(&mc13xxx->lock);
 
-	mc13xxx->regmap = regmap_init(&spi->dev, &regmap_mc13xxx_bus, &spi->dev,
-					&mc13xxx_regmap_spi_config);
-
+	mc13xxx->regmap = devm_regmap_init(&spi->dev, &regmap_mc13xxx_bus,
+					   &spi->dev,
+					   &mc13xxx_regmap_spi_config);
 	if (IS_ERR(mc13xxx->regmap)) {
 		ret = PTR_ERR(mc13xxx->regmap);
 		dev_err(mc13xxx->dev, "Failed to initialize register map: %d\n",
 				ret);
 		dev_set_drvdata(&spi->dev, NULL);
-		kfree(mc13xxx);
 		return ret;
 	}
 
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index ffc3d48676a..0c3a01cde2f 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -18,6 +18,8 @@
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/irqdomain.h>
+#include <linux/of.h>
 
 int mfd_cell_enable(struct platform_device *pdev)
 {
@@ -76,6 +78,8 @@ static int mfd_add_device(struct device *parent, int id,
 {
 	struct resource *res;
 	struct platform_device *pdev;
+	struct device_node *np = NULL;
+	struct irq_domain *domain = NULL;
 	int ret = -ENOMEM;
 	int r;
 
@@ -89,6 +93,16 @@ static int mfd_add_device(struct device *parent, int id,
 
 	pdev->dev.parent = parent;
 
+	if (parent->of_node && cell->of_compatible) {
+		for_each_child_of_node(parent->of_node, np) {
+			if (of_device_is_compatible(np, cell->of_compatible)) {
+				pdev->dev.of_node = np;
+				domain = irq_find_host(parent->of_node);
+				break;
+			}
+		}
+	}
+
 	if (cell->pdata_size) {
 		ret = platform_device_add_data(pdev,
 					cell->platform_data, cell->pdata_size);
@@ -112,10 +126,18 @@ static int mfd_add_device(struct device *parent, int id,
 			res[r].end = mem_base->start +
 				cell->resources[r].end;
 		} else if (cell->resources[r].flags & IORESOURCE_IRQ) {
-			res[r].start = irq_base +
-				cell->resources[r].start;
-			res[r].end   = irq_base +
-				cell->resources[r].end;
+			if (domain) {
+				/* Unable to create mappings for IRQ ranges. */
+				WARN_ON(cell->resources[r].start !=
+					cell->resources[r].end);
+				res[r].start = res[r].end = irq_create_mapping(
+					domain, cell->resources[r].start);
+			} else {
+				res[r].start = irq_base +
+					cell->resources[r].start;
+				res[r].end   = irq_base +
+					cell->resources[r].end;
+			}
 		} else {
 			res[r].parent = cell->resources[r].parent;
 			res[r].start = cell->resources[r].start;
diff --git a/drivers/mfd/pcf50633-core.c b/drivers/mfd/pcf50633-core.c
index 29c122bf28e..45ce1fb5a54 100644
--- a/drivers/mfd/pcf50633-core.c
+++ b/drivers/mfd/pcf50633-core.c
@@ -253,8 +253,13 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 		}
 
 		pdev->dev.parent = pcf->dev;
-		platform_device_add_data(pdev, &pdata->reg_init_data[i],
-					sizeof(pdata->reg_init_data[i]));
+		if (platform_device_add_data(pdev, &pdata->reg_init_data[i],
+					sizeof(pdata->reg_init_data[i])) < 0) {
+			platform_device_put(pdev);
+			dev_err(pcf->dev, "Out of memory for regulator parameters %d\n",
+									i);
+			continue;
+		}
 		pcf->regulator_pdev[i] = pdev;
 
 		platform_device_add(pdev);
diff --git a/drivers/mfd/s5m-core.c b/drivers/mfd/s5m-core.c
deleted file mode 100644
index dd170307e60..00000000000
--- a/drivers/mfd/s5m-core.c
+++ /dev/null
@@ -1,206 +0,0 @@
-/*
- * s5m87xx.c
- *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd
- *              http://www.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/moduleparam.h>
-#include <linux/init.h>
-#include <linux/err.h>
-#include <linux/slab.h>
-#include <linux/i2c.h>
-#include <linux/interrupt.h>
-#include <linux/pm_runtime.h>
-#include <linux/mutex.h>
-#include <linux/mfd/core.h>
-#include <linux/mfd/s5m87xx/s5m-core.h>
-#include <linux/mfd/s5m87xx/s5m-pmic.h>
-#include <linux/mfd/s5m87xx/s5m-rtc.h>
-#include <linux/regmap.h>
-
-static struct mfd_cell s5m8751_devs[] = {
-	{
-		.name = "s5m8751-pmic",
-	}, {
-		.name = "s5m-charger",
-	}, {
-		.name = "s5m8751-codec",
-	},
-};
-
-static struct mfd_cell s5m8763_devs[] = {
-	{
-		.name = "s5m8763-pmic",
-	}, {
-		.name = "s5m-rtc",
-	}, {
-		.name = "s5m-charger",
-	},
-};
-
-static struct mfd_cell s5m8767_devs[] = {
-	{
-		.name = "s5m8767-pmic",
-	}, {
-		.name = "s5m-rtc",
-	},
-};
-
-int s5m_reg_read(struct s5m87xx_dev *s5m87xx, u8 reg, void *dest)
-{
-	return regmap_read(s5m87xx->regmap, reg, dest);
-}
-EXPORT_SYMBOL_GPL(s5m_reg_read);
-
-int s5m_bulk_read(struct s5m87xx_dev *s5m87xx, u8 reg, int count, u8 *buf)
-{
-	return regmap_bulk_read(s5m87xx->regmap, reg, buf, count);
-}
-EXPORT_SYMBOL_GPL(s5m_bulk_read);
-
-int s5m_reg_write(struct s5m87xx_dev *s5m87xx, u8 reg, u8 value)
-{
-	return regmap_write(s5m87xx->regmap, reg, value);
-}
-EXPORT_SYMBOL_GPL(s5m_reg_write);
-
-int s5m_bulk_write(struct s5m87xx_dev *s5m87xx, u8 reg, int count, u8 *buf)
-{
-	return regmap_raw_write(s5m87xx->regmap, reg, buf, count);
-}
-EXPORT_SYMBOL_GPL(s5m_bulk_write);
-
-int s5m_reg_update(struct s5m87xx_dev *s5m87xx, u8 reg, u8 val, u8 mask)
-{
-	return regmap_update_bits(s5m87xx->regmap, reg, mask, val);
-}
-EXPORT_SYMBOL_GPL(s5m_reg_update);
-
-static struct regmap_config s5m_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
-};
-
-static int s5m87xx_i2c_probe(struct i2c_client *i2c,
-			    const struct i2c_device_id *id)
-{
-	struct s5m_platform_data *pdata = i2c->dev.platform_data;
-	struct s5m87xx_dev *s5m87xx;
-	int ret;
-
-	s5m87xx = devm_kzalloc(&i2c->dev, sizeof(struct s5m87xx_dev),
-				GFP_KERNEL);
-	if (s5m87xx == NULL)
-		return -ENOMEM;
-
-	i2c_set_clientdata(i2c, s5m87xx);
-	s5m87xx->dev = &i2c->dev;
-	s5m87xx->i2c = i2c;
-	s5m87xx->irq = i2c->irq;
-	s5m87xx->type = id->driver_data;
-
-	if (pdata) {
-		s5m87xx->device_type = pdata->device_type;
-		s5m87xx->ono = pdata->ono;
-		s5m87xx->irq_base = pdata->irq_base;
-		s5m87xx->wakeup = pdata->wakeup;
-	}
-
-	s5m87xx->regmap = devm_regmap_init_i2c(i2c, &s5m_regmap_config);
-	if (IS_ERR(s5m87xx->regmap)) {
-		ret = PTR_ERR(s5m87xx->regmap);
-		dev_err(&i2c->dev, "Failed to allocate register map: %d\n",
-			ret);
-		return ret;
-	}
-
-	s5m87xx->rtc = i2c_new_dummy(i2c->adapter, RTC_I2C_ADDR);
-	i2c_set_clientdata(s5m87xx->rtc, s5m87xx);
-
-	if (pdata && pdata->cfg_pmic_irq)
-		pdata->cfg_pmic_irq();
-
-	s5m_irq_init(s5m87xx);
-
-	pm_runtime_set_active(s5m87xx->dev);
-
-	switch (s5m87xx->device_type) {
-	case S5M8751X:
-		ret = mfd_add_devices(s5m87xx->dev, -1, s5m8751_devs,
-					ARRAY_SIZE(s5m8751_devs), NULL, 0);
-		break;
-	case S5M8763X:
-		ret = mfd_add_devices(s5m87xx->dev, -1, s5m8763_devs,
-					ARRAY_SIZE(s5m8763_devs), NULL, 0);
-		break;
-	case S5M8767X:
-		ret = mfd_add_devices(s5m87xx->dev, -1, s5m8767_devs,
-					ARRAY_SIZE(s5m8767_devs), NULL, 0);
-		break;
-	default:
-		/* If this happens the probe function is problem */
-		BUG();
-	}
-
-	if (ret < 0)
-		goto err;
-
-	return ret;
-
-err:
-	mfd_remove_devices(s5m87xx->dev);
-	s5m_irq_exit(s5m87xx);
-	i2c_unregister_device(s5m87xx->rtc);
-	return ret;
-}
-
-static int s5m87xx_i2c_remove(struct i2c_client *i2c)
-{
-	struct s5m87xx_dev *s5m87xx = i2c_get_clientdata(i2c);
-
-	mfd_remove_devices(s5m87xx->dev);
-	s5m_irq_exit(s5m87xx);
-	i2c_unregister_device(s5m87xx->rtc);
-	return 0;
-}
-
-static const struct i2c_device_id s5m87xx_i2c_id[] = {
-	{ "s5m87xx", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, s5m87xx_i2c_id);
-
-static struct i2c_driver s5m87xx_i2c_driver = {
-	.driver = {
-		   .name = "s5m87xx",
-		   .owner = THIS_MODULE,
-	},
-	.probe = s5m87xx_i2c_probe,
-	.remove = s5m87xx_i2c_remove,
-	.id_table = s5m87xx_i2c_id,
-};
-
-static int __init s5m87xx_i2c_init(void)
-{
-	return i2c_add_driver(&s5m87xx_i2c_driver);
-}
-
-subsys_initcall(s5m87xx_i2c_init);
-
-static void __exit s5m87xx_i2c_exit(void)
-{
-	i2c_del_driver(&s5m87xx_i2c_driver);
-}
-module_exit(s5m87xx_i2c_exit);
-
-MODULE_AUTHOR("Sangbeom Kim <sbkim73@samsung.com>");
-MODULE_DESCRIPTION("Core support for the S5M MFD");
-MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/s5m-irq.c b/drivers/mfd/s5m-irq.c
deleted file mode 100644
index 0236676085c..00000000000
--- a/drivers/mfd/s5m-irq.c
+++ /dev/null
@@ -1,495 +0,0 @@
-/*
- * s5m-irq.c
- *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd
- *              http://www.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/device.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/mfd/s5m87xx/s5m-core.h>
-
-struct s5m_irq_data {
-	int reg;
-	int mask;
-};
-
-static struct s5m_irq_data s5m8767_irqs[] = {
-	[S5M8767_IRQ_PWRR] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_PWRR_MASK,
-	},
-	[S5M8767_IRQ_PWRF] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_PWRF_MASK,
-	},
-	[S5M8767_IRQ_PWR1S] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_PWR1S_MASK,
-	},
-	[S5M8767_IRQ_JIGR] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_JIGR_MASK,
-	},
-	[S5M8767_IRQ_JIGF] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_JIGF_MASK,
-	},
-	[S5M8767_IRQ_LOWBAT2] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_LOWBAT2_MASK,
-	},
-	[S5M8767_IRQ_LOWBAT1] = {
-		.reg = 1,
-		.mask = S5M8767_IRQ_LOWBAT1_MASK,
-	},
-	[S5M8767_IRQ_MRB] = {
-		.reg = 2,
-		.mask = S5M8767_IRQ_MRB_MASK,
-	},
-	[S5M8767_IRQ_DVSOK2] = {
-		.reg = 2,
-		.mask = S5M8767_IRQ_DVSOK2_MASK,
-	},
-	[S5M8767_IRQ_DVSOK3] = {
-		.reg = 2,
-		.mask = S5M8767_IRQ_DVSOK3_MASK,
-	},
-	[S5M8767_IRQ_DVSOK4] = {
-		.reg = 2,
-		.mask = S5M8767_IRQ_DVSOK4_MASK,
-	},
-	[S5M8767_IRQ_RTC60S] = {
-		.reg = 3,
-		.mask = S5M8767_IRQ_RTC60S_MASK,
-	},
-	[S5M8767_IRQ_RTCA1] = {
-		.reg = 3,
-		.mask = S5M8767_IRQ_RTCA1_MASK,
-	},
-	[S5M8767_IRQ_RTCA2] = {
-		.reg = 3,
-		.mask = S5M8767_IRQ_RTCA2_MASK,
-	},
-	[S5M8767_IRQ_SMPL] = {
-		.reg = 3,
-		.mask = S5M8767_IRQ_SMPL_MASK,
-	},
-	[S5M8767_IRQ_RTC1S] = {
-		.reg = 3,
-		.mask = S5M8767_IRQ_RTC1S_MASK,
-	},
-	[S5M8767_IRQ_WTSR] = {
-		.reg = 3,
-		.mask = S5M8767_IRQ_WTSR_MASK,
-	},
-};
-
-static struct s5m_irq_data s5m8763_irqs[] = {
-	[S5M8763_IRQ_DCINF] = {
-		.reg = 1,
-		.mask = S5M8763_IRQ_DCINF_MASK,
-	},
-	[S5M8763_IRQ_DCINR] = {
-		.reg = 1,
-		.mask = S5M8763_IRQ_DCINR_MASK,
-	},
-	[S5M8763_IRQ_JIGF] = {
-		.reg = 1,
-		.mask = S5M8763_IRQ_JIGF_MASK,
-	},
-	[S5M8763_IRQ_JIGR] = {
-		.reg = 1,
-		.mask = S5M8763_IRQ_JIGR_MASK,
-	},
-	[S5M8763_IRQ_PWRONF] = {
-		.reg = 1,
-		.mask = S5M8763_IRQ_PWRONF_MASK,
-	},
-	[S5M8763_IRQ_PWRONR] = {
-		.reg = 1,
-		.mask = S5M8763_IRQ_PWRONR_MASK,
-	},
-	[S5M8763_IRQ_WTSREVNT] = {
-		.reg = 2,
-		.mask = S5M8763_IRQ_WTSREVNT_MASK,
-	},
-	[S5M8763_IRQ_SMPLEVNT] = {
-		.reg = 2,
-		.mask = S5M8763_IRQ_SMPLEVNT_MASK,
-	},
-	[S5M8763_IRQ_ALARM1] = {
-		.reg = 2,
-		.mask = S5M8763_IRQ_ALARM1_MASK,
-	},
-	[S5M8763_IRQ_ALARM0] = {
-		.reg = 2,
-		.mask = S5M8763_IRQ_ALARM0_MASK,
-	},
-	[S5M8763_IRQ_ONKEY1S] = {
-		.reg = 3,
-		.mask = S5M8763_IRQ_ONKEY1S_MASK,
-	},
-	[S5M8763_IRQ_TOPOFFR] = {
-		.reg = 3,
-		.mask = S5M8763_IRQ_TOPOFFR_MASK,
-	},
-	[S5M8763_IRQ_DCINOVPR] = {
-		.reg = 3,
-		.mask = S5M8763_IRQ_DCINOVPR_MASK,
-	},
-	[S5M8763_IRQ_CHGRSTF] = {
-		.reg = 3,
-		.mask = S5M8763_IRQ_CHGRSTF_MASK,
-	},
-	[S5M8763_IRQ_DONER] = {
-		.reg = 3,
-		.mask = S5M8763_IRQ_DONER_MASK,
-	},
-	[S5M8763_IRQ_CHGFAULT] = {
-		.reg = 3,
-		.mask = S5M8763_IRQ_CHGFAULT_MASK,
-	},
-	[S5M8763_IRQ_LOBAT1] = {
-		.reg = 4,
-		.mask = S5M8763_IRQ_LOBAT1_MASK,
-	},
-	[S5M8763_IRQ_LOBAT2] = {
-		.reg = 4,
-		.mask = S5M8763_IRQ_LOBAT2_MASK,
-	},
-};
-
-static inline struct s5m_irq_data *
-irq_to_s5m8767_irq(struct s5m87xx_dev *s5m87xx, int irq)
-{
-	return &s5m8767_irqs[irq - s5m87xx->irq_base];
-}
-
-static void s5m8767_irq_lock(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-
-	mutex_lock(&s5m87xx->irqlock);
-}
-
-static void s5m8767_irq_sync_unlock(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(s5m87xx->irq_masks_cur); i++) {
-		if (s5m87xx->irq_masks_cur[i] != s5m87xx->irq_masks_cache[i]) {
-			s5m87xx->irq_masks_cache[i] = s5m87xx->irq_masks_cur[i];
-			s5m_reg_write(s5m87xx, S5M8767_REG_INT1M + i,
-					s5m87xx->irq_masks_cur[i]);
-		}
-	}
-
-	mutex_unlock(&s5m87xx->irqlock);
-}
-
-static void s5m8767_irq_unmask(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-	struct s5m_irq_data *irq_data = irq_to_s5m8767_irq(s5m87xx,
-							       data->irq);
-
-	s5m87xx->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
-}
-
-static void s5m8767_irq_mask(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-	struct s5m_irq_data *irq_data = irq_to_s5m8767_irq(s5m87xx,
-							       data->irq);
-
-	s5m87xx->irq_masks_cur[irq_data->reg - 1] |= irq_data->mask;
-}
-
-static struct irq_chip s5m8767_irq_chip = {
-	.name = "s5m8767",
-	.irq_bus_lock = s5m8767_irq_lock,
-	.irq_bus_sync_unlock = s5m8767_irq_sync_unlock,
-	.irq_mask = s5m8767_irq_mask,
-	.irq_unmask = s5m8767_irq_unmask,
-};
-
-static inline struct s5m_irq_data *
-irq_to_s5m8763_irq(struct s5m87xx_dev *s5m87xx, int irq)
-{
-	return &s5m8763_irqs[irq - s5m87xx->irq_base];
-}
-
-static void s5m8763_irq_lock(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-
-	mutex_lock(&s5m87xx->irqlock);
-}
-
-static void s5m8763_irq_sync_unlock(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(s5m87xx->irq_masks_cur); i++) {
-		if (s5m87xx->irq_masks_cur[i] != s5m87xx->irq_masks_cache[i]) {
-			s5m87xx->irq_masks_cache[i] = s5m87xx->irq_masks_cur[i];
-			s5m_reg_write(s5m87xx, S5M8763_REG_IRQM1 + i,
-					s5m87xx->irq_masks_cur[i]);
-		}
-	}
-
-	mutex_unlock(&s5m87xx->irqlock);
-}
-
-static void s5m8763_irq_unmask(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-	struct s5m_irq_data *irq_data = irq_to_s5m8763_irq(s5m87xx,
-							       data->irq);
-
-	s5m87xx->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
-}
-
-static void s5m8763_irq_mask(struct irq_data *data)
-{
-	struct s5m87xx_dev *s5m87xx = irq_data_get_irq_chip_data(data);
-	struct s5m_irq_data *irq_data = irq_to_s5m8763_irq(s5m87xx,
-							       data->irq);
-
-	s5m87xx->irq_masks_cur[irq_data->reg - 1] |= irq_data->mask;
-}
-
-static struct irq_chip s5m8763_irq_chip = {
-	.name = "s5m8763",
-	.irq_bus_lock = s5m8763_irq_lock,
-	.irq_bus_sync_unlock = s5m8763_irq_sync_unlock,
-	.irq_mask = s5m8763_irq_mask,
-	.irq_unmask = s5m8763_irq_unmask,
-};
-
-
-static irqreturn_t s5m8767_irq_thread(int irq, void *data)
-{
-	struct s5m87xx_dev *s5m87xx = data;
-	u8 irq_reg[NUM_IRQ_REGS-1];
-	int ret;
-	int i;
-
-
-	ret = s5m_bulk_read(s5m87xx, S5M8767_REG_INT1,
-				NUM_IRQ_REGS - 1, irq_reg);
-	if (ret < 0) {
-		dev_err(s5m87xx->dev, "Failed to read interrupt register: %d\n",
-				ret);
-		return IRQ_NONE;
-	}
-
-	for (i = 0; i < NUM_IRQ_REGS - 1; i++)
-		irq_reg[i] &= ~s5m87xx->irq_masks_cur[i];
-
-	for (i = 0; i < S5M8767_IRQ_NR; i++) {
-		if (irq_reg[s5m8767_irqs[i].reg - 1] & s5m8767_irqs[i].mask)
-			handle_nested_irq(s5m87xx->irq_base + i);
-	}
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t s5m8763_irq_thread(int irq, void *data)
-{
-	struct s5m87xx_dev *s5m87xx = data;
-	u8 irq_reg[NUM_IRQ_REGS];
-	int ret;
-	int i;
-
-	ret = s5m_bulk_read(s5m87xx, S5M8763_REG_IRQ1,
-				NUM_IRQ_REGS, irq_reg);
-	if (ret < 0) {
-		dev_err(s5m87xx->dev, "Failed to read interrupt register: %d\n",
-				ret);
-		return IRQ_NONE;
-	}
-
-	for (i = 0; i < NUM_IRQ_REGS; i++)
-		irq_reg[i] &= ~s5m87xx->irq_masks_cur[i];
-
-	for (i = 0; i < S5M8763_IRQ_NR; i++) {
-		if (irq_reg[s5m8763_irqs[i].reg - 1] & s5m8763_irqs[i].mask)
-			handle_nested_irq(s5m87xx->irq_base + i);
-	}
-
-	return IRQ_HANDLED;
-}
-
-int s5m_irq_resume(struct s5m87xx_dev *s5m87xx)
-{
-	if (s5m87xx->irq && s5m87xx->irq_base){
-		switch (s5m87xx->device_type) {
-		case S5M8763X:
-			s5m8763_irq_thread(s5m87xx->irq_base, s5m87xx);
-			break;
-		case S5M8767X:
-			s5m8767_irq_thread(s5m87xx->irq_base, s5m87xx);
-			break;
-		default:
-			dev_err(s5m87xx->dev,
-				"Unknown device type %d\n",
-				s5m87xx->device_type);
-			return -EINVAL;
-
-		}
-	}
-	return 0;
-}
-
-int s5m_irq_init(struct s5m87xx_dev *s5m87xx)
-{
-	int i;
-	int cur_irq;
-	int ret = 0;
-	int type = s5m87xx->device_type;
-
-	if (!s5m87xx->irq) {
-		dev_warn(s5m87xx->dev,
-			 "No interrupt specified, no interrupts\n");
-		s5m87xx->irq_base = 0;
-		return 0;
-	}
-
-	if (!s5m87xx->irq_base) {
-		dev_err(s5m87xx->dev,
-			"No interrupt base specified, no interrupts\n");
-		return 0;
-	}
-
-	mutex_init(&s5m87xx->irqlock);
-
-	switch (type) {
-	case S5M8763X:
-		for (i = 0; i < NUM_IRQ_REGS; i++) {
-			s5m87xx->irq_masks_cur[i] = 0xff;
-			s5m87xx->irq_masks_cache[i] = 0xff;
-			s5m_reg_write(s5m87xx, S5M8763_REG_IRQM1 + i,
-						0xff);
-		}
-
-		s5m_reg_write(s5m87xx, S5M8763_REG_STATUSM1, 0xff);
-		s5m_reg_write(s5m87xx, S5M8763_REG_STATUSM2, 0xff);
-
-		for (i = 0; i < S5M8763_IRQ_NR; i++) {
-			cur_irq = i + s5m87xx->irq_base;
-			irq_set_chip_data(cur_irq, s5m87xx);
-			irq_set_chip_and_handler(cur_irq, &s5m8763_irq_chip,
-						 handle_edge_irq);
-			irq_set_nested_thread(cur_irq, 1);
-#ifdef CONFIG_ARM
-			set_irq_flags(cur_irq, IRQF_VALID);
-#else
-			irq_set_noprobe(cur_irq);
-#endif
-		}
-
-		ret = request_threaded_irq(s5m87xx->irq, NULL,
-					s5m8763_irq_thread,
-					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-					"s5m87xx-irq", s5m87xx);
-		if (ret) {
-			dev_err(s5m87xx->dev, "Failed to request IRQ %d: %d\n",
-				s5m87xx->irq, ret);
-			return ret;
-		}
-		break;
-	case S5M8767X:
-		for (i = 0; i < NUM_IRQ_REGS - 1; i++) {
-			s5m87xx->irq_masks_cur[i] = 0xff;
-			s5m87xx->irq_masks_cache[i] = 0xff;
-			s5m_reg_write(s5m87xx, S5M8767_REG_INT1M + i,
-						0xff);
-		}
-		for (i = 0; i < S5M8767_IRQ_NR; i++) {
-			cur_irq = i + s5m87xx->irq_base;
-			irq_set_chip_data(cur_irq, s5m87xx);
-			if (ret) {
-				dev_err(s5m87xx->dev,
-					"Failed to irq_set_chip_data %d: %d\n",
-					s5m87xx->irq, ret);
-				return ret;
-			}
-
-			irq_set_chip_and_handler(cur_irq, &s5m8767_irq_chip,
-						 handle_edge_irq);
-			irq_set_nested_thread(cur_irq, 1);
-#ifdef CONFIG_ARM
-			set_irq_flags(cur_irq, IRQF_VALID);
-#else
-			irq_set_noprobe(cur_irq);
-#endif
-		}
-
-		ret = request_threaded_irq(s5m87xx->irq, NULL,
-					   s5m8767_irq_thread,
-					   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-					   "s5m87xx-irq", s5m87xx);
-		if (ret) {
-			dev_err(s5m87xx->dev, "Failed to request IRQ %d: %d\n",
-				s5m87xx->irq, ret);
-			return ret;
-		}
-		break;
-	default:
-		dev_err(s5m87xx->dev,
-			"Unknown device type %d\n", s5m87xx->device_type);
-		return -EINVAL;
-	}
-
-	if (!s5m87xx->ono)
-		return 0;
-
-	switch (type) {
-	case S5M8763X:
-		ret = request_threaded_irq(s5m87xx->ono, NULL,
-						s5m8763_irq_thread,
-						IRQF_TRIGGER_FALLING |
-						IRQF_TRIGGER_RISING |
-						IRQF_ONESHOT, "s5m87xx-ono",
-						s5m87xx);
-		break;
-	case S5M8767X:
-		ret = request_threaded_irq(s5m87xx->ono, NULL,
-					s5m8767_irq_thread,
-					IRQF_TRIGGER_FALLING |
-					IRQF_TRIGGER_RISING |
-					IRQF_ONESHOT, "s5m87xx-ono", s5m87xx);
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
-
-	if (ret) {
-		dev_err(s5m87xx->dev, "Failed to request IRQ %d: %d\n",
-			s5m87xx->ono, ret);
-		return ret;
-	}
-
-	return 0;
-}
-
-void s5m_irq_exit(struct s5m87xx_dev *s5m87xx)
-{
-	if (s5m87xx->ono)
-		free_irq(s5m87xx->ono, s5m87xx);
-
-	if (s5m87xx->irq)
-		free_irq(s5m87xx->irq, s5m87xx);
-}
diff --git a/drivers/mfd/sec-core.c b/drivers/mfd/sec-core.c
new file mode 100644
index 00000000000..2988efde11e
--- /dev/null
+++ b/drivers/mfd/sec-core.c
@@ -0,0 +1,216 @@
+/*
+ * sec-core.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd
+ *              http://www.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/moduleparam.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/mutex.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/samsung/core.h>
+#include <linux/mfd/samsung/irq.h>
+#include <linux/mfd/samsung/rtc.h>
+#include <linux/regmap.h>
+
+static struct mfd_cell s5m8751_devs[] = {
+	{
+		.name = "s5m8751-pmic",
+	}, {
+		.name = "s5m-charger",
+	}, {
+		.name = "s5m8751-codec",
+	},
+};
+
+static struct mfd_cell s5m8763_devs[] = {
+	{
+		.name = "s5m8763-pmic",
+	}, {
+		.name = "s5m-rtc",
+	}, {
+		.name = "s5m-charger",
+	},
+};
+
+static struct mfd_cell s5m8767_devs[] = {
+	{
+		.name = "s5m8767-pmic",
+	}, {
+		.name = "s5m-rtc",
+	},
+};
+
+static struct mfd_cell s2mps11_devs[] = {
+	{
+		.name = "s2mps11-pmic",
+	},
+};
+
+int sec_reg_read(struct sec_pmic_dev *sec_pmic, u8 reg, void *dest)
+{
+	return regmap_read(sec_pmic->regmap, reg, dest);
+}
+EXPORT_SYMBOL_GPL(sec_reg_read);
+
+int sec_bulk_read(struct sec_pmic_dev *sec_pmic, u8 reg, int count, u8 *buf)
+{
+	return regmap_bulk_read(sec_pmic->regmap, reg, buf, count);
+}
+EXPORT_SYMBOL_GPL(sec_bulk_read);
+
+int sec_reg_write(struct sec_pmic_dev *sec_pmic, u8 reg, u8 value)
+{
+	return regmap_write(sec_pmic->regmap, reg, value);
+}
+EXPORT_SYMBOL_GPL(sec_reg_write);
+
+int sec_bulk_write(struct sec_pmic_dev *sec_pmic, u8 reg, int count, u8 *buf)
+{
+	return regmap_raw_write(sec_pmic->regmap, reg, buf, count);
+}
+EXPORT_SYMBOL_GPL(sec_bulk_write);
+
+int sec_reg_update(struct sec_pmic_dev *sec_pmic, u8 reg, u8 val, u8 mask)
+{
+	return regmap_update_bits(sec_pmic->regmap, reg, mask, val);
+}
+EXPORT_SYMBOL_GPL(sec_reg_update);
+
+static struct regmap_config sec_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+static int sec_pmic_probe(struct i2c_client *i2c,
+			    const struct i2c_device_id *id)
+{
+	struct sec_platform_data *pdata = i2c->dev.platform_data;
+	struct sec_pmic_dev *sec_pmic;
+	int ret;
+
+	sec_pmic = devm_kzalloc(&i2c->dev, sizeof(struct sec_pmic_dev),
+				GFP_KERNEL);
+	if (sec_pmic == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, sec_pmic);
+	sec_pmic->dev = &i2c->dev;
+	sec_pmic->i2c = i2c;
+	sec_pmic->irq = i2c->irq;
+	sec_pmic->type = id->driver_data;
+
+	if (pdata) {
+		sec_pmic->device_type = pdata->device_type;
+		sec_pmic->ono = pdata->ono;
+		sec_pmic->irq_base = pdata->irq_base;
+		sec_pmic->wakeup = pdata->wakeup;
+	}
+
+	sec_pmic->regmap = devm_regmap_init_i2c(i2c, &sec_regmap_config);
+	if (IS_ERR(sec_pmic->regmap)) {
+		ret = PTR_ERR(sec_pmic->regmap);
+		dev_err(&i2c->dev, "Failed to allocate register map: %d\n",
+			ret);
+		return ret;
+	}
+
+	sec_pmic->rtc = i2c_new_dummy(i2c->adapter, RTC_I2C_ADDR);
+	i2c_set_clientdata(sec_pmic->rtc, sec_pmic);
+
+	if (pdata && pdata->cfg_pmic_irq)
+		pdata->cfg_pmic_irq();
+
+	sec_irq_init(sec_pmic);
+
+	pm_runtime_set_active(sec_pmic->dev);
+
+	switch (sec_pmic->device_type) {
+	case S5M8751X:
+		ret = mfd_add_devices(sec_pmic->dev, -1, s5m8751_devs,
+					ARRAY_SIZE(s5m8751_devs), NULL, 0);
+		break;
+	case S5M8763X:
+		ret = mfd_add_devices(sec_pmic->dev, -1, s5m8763_devs,
+					ARRAY_SIZE(s5m8763_devs), NULL, 0);
+		break;
+	case S5M8767X:
+		ret = mfd_add_devices(sec_pmic->dev, -1, s5m8767_devs,
+					ARRAY_SIZE(s5m8767_devs), NULL, 0);
+		break;
+	case S2MPS11X:
+		ret = mfd_add_devices(sec_pmic->dev, -1, s2mps11_devs,
+					ARRAY_SIZE(s2mps11_devs), NULL, 0);
+		break;
+	default:
+		/* If this happens the probe function is problem */
+		BUG();
+	}
+
+	if (ret < 0)
+		goto err;
+
+	return ret;
+
+err:
+	mfd_remove_devices(sec_pmic->dev);
+	sec_irq_exit(sec_pmic);
+	i2c_unregister_device(sec_pmic->rtc);
+	return ret;
+}
+
+static int sec_pmic_remove(struct i2c_client *i2c)
+{
+	struct sec_pmic_dev *sec_pmic = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(sec_pmic->dev);
+	sec_irq_exit(sec_pmic);
+	i2c_unregister_device(sec_pmic->rtc);
+	return 0;
+}
+
+static const struct i2c_device_id sec_pmic_id[] = {
+	{ "sec_pmic", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sec_pmic_id);
+
+static struct i2c_driver sec_pmic_driver = {
+	.driver = {
+		   .name = "sec_pmic",
+		   .owner = THIS_MODULE,
+	},
+	.probe = sec_pmic_probe,
+	.remove = sec_pmic_remove,
+	.id_table = sec_pmic_id,
+};
+
+static int __init sec_pmic_init(void)
+{
+	return i2c_add_driver(&sec_pmic_driver);
+}
+
+subsys_initcall(sec_pmic_init);
+
+static void __exit sec_pmic_exit(void)
+{
+	i2c_del_driver(&sec_pmic_driver);
+}
+module_exit(sec_pmic_exit);
+
+MODULE_AUTHOR("Sangbeom Kim <sbkim73@samsung.com>");
+MODULE_DESCRIPTION("Core support for the S5M MFD");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/sec-irq.c b/drivers/mfd/sec-irq.c
new file mode 100644
index 00000000000..c901fa50fea
--- /dev/null
+++ b/drivers/mfd/sec-irq.c
@@ -0,0 +1,317 @@
+/*
+ * sec-irq.c
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd
+ *              http://www.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/device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/regmap.h>
+
+#include <linux/mfd/samsung/core.h>
+#include <linux/mfd/samsung/irq.h>
+#include <linux/mfd/samsung/s2mps11.h>
+#include <linux/mfd/samsung/s5m8763.h>
+#include <linux/mfd/samsung/s5m8767.h>
+
+static struct regmap_irq s2mps11_irqs[] = {
+	[S2MPS11_IRQ_PWRONF] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_PWRONF_MASK,
+	},
+	[S2MPS11_IRQ_PWRONR] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_PWRONR_MASK,
+	},
+	[S2MPS11_IRQ_JIGONBF] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_JIGONBF_MASK,
+	},
+	[S2MPS11_IRQ_JIGONBR] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_JIGONBR_MASK,
+	},
+	[S2MPS11_IRQ_ACOKBF] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_ACOKBF_MASK,
+	},
+	[S2MPS11_IRQ_ACOKBR] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_ACOKBR_MASK,
+	},
+	[S2MPS11_IRQ_PWRON1S] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_PWRON1S_MASK,
+	},
+	[S2MPS11_IRQ_MRB] = {
+		.reg_offset = 1,
+		.mask = S2MPS11_IRQ_MRB_MASK,
+	},
+	[S2MPS11_IRQ_RTC60S] = {
+		.reg_offset = 2,
+		.mask = S2MPS11_IRQ_RTC60S_MASK,
+	},
+	[S2MPS11_IRQ_RTCA1] = {
+		.reg_offset = 2,
+		.mask = S2MPS11_IRQ_RTCA1_MASK,
+	},
+	[S2MPS11_IRQ_RTCA2] = {
+		.reg_offset = 2,
+		.mask = S2MPS11_IRQ_RTCA2_MASK,
+	},
+	[S2MPS11_IRQ_SMPL] = {
+		.reg_offset = 2,
+		.mask = S2MPS11_IRQ_SMPL_MASK,
+	},
+	[S2MPS11_IRQ_RTC1S] = {
+		.reg_offset = 2,
+		.mask = S2MPS11_IRQ_RTC1S_MASK,
+	},
+	[S2MPS11_IRQ_WTSR] = {
+		.reg_offset = 2,
+		.mask = S2MPS11_IRQ_WTSR_MASK,
+	},
+	[S2MPS11_IRQ_INT120C] = {
+		.reg_offset = 3,
+		.mask = S2MPS11_IRQ_INT120C_MASK,
+	},
+	[S2MPS11_IRQ_INT140C] = {
+		.reg_offset = 3,
+		.mask = S2MPS11_IRQ_INT140C_MASK,
+	},
+};
+
+
+static struct regmap_irq s5m8767_irqs[] = {
+	[S5M8767_IRQ_PWRR] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_PWRR_MASK,
+	},
+	[S5M8767_IRQ_PWRF] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_PWRF_MASK,
+	},
+	[S5M8767_IRQ_PWR1S] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_PWR1S_MASK,
+	},
+	[S5M8767_IRQ_JIGR] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_JIGR_MASK,
+	},
+	[S5M8767_IRQ_JIGF] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_JIGF_MASK,
+	},
+	[S5M8767_IRQ_LOWBAT2] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_LOWBAT2_MASK,
+	},
+	[S5M8767_IRQ_LOWBAT1] = {
+		.reg_offset = 1,
+		.mask = S5M8767_IRQ_LOWBAT1_MASK,
+	},
+	[S5M8767_IRQ_MRB] = {
+		.reg_offset = 2,
+		.mask = S5M8767_IRQ_MRB_MASK,
+	},
+	[S5M8767_IRQ_DVSOK2] = {
+		.reg_offset = 2,
+		.mask = S5M8767_IRQ_DVSOK2_MASK,
+	},
+	[S5M8767_IRQ_DVSOK3] = {
+		.reg_offset = 2,
+		.mask = S5M8767_IRQ_DVSOK3_MASK,
+	},
+	[S5M8767_IRQ_DVSOK4] = {
+		.reg_offset = 2,
+		.mask = S5M8767_IRQ_DVSOK4_MASK,
+	},
+	[S5M8767_IRQ_RTC60S] = {
+		.reg_offset = 3,
+		.mask = S5M8767_IRQ_RTC60S_MASK,
+	},
+	[S5M8767_IRQ_RTCA1] = {
+		.reg_offset = 3,
+		.mask = S5M8767_IRQ_RTCA1_MASK,
+	},
+	[S5M8767_IRQ_RTCA2] = {
+		.reg_offset = 3,
+		.mask = S5M8767_IRQ_RTCA2_MASK,
+	},
+	[S5M8767_IRQ_SMPL] = {
+		.reg_offset = 3,
+		.mask = S5M8767_IRQ_SMPL_MASK,
+	},
+	[S5M8767_IRQ_RTC1S] = {
+		.reg_offset = 3,
+		.mask = S5M8767_IRQ_RTC1S_MASK,
+	},
+	[S5M8767_IRQ_WTSR] = {
+		.reg_offset = 3,
+		.mask = S5M8767_IRQ_WTSR_MASK,
+	},
+};
+
+static struct regmap_irq s5m8763_irqs[] = {
+	[S5M8763_IRQ_DCINF] = {
+		.reg_offset = 1,
+		.mask = S5M8763_IRQ_DCINF_MASK,
+	},
+	[S5M8763_IRQ_DCINR] = {
+		.reg_offset = 1,
+		.mask = S5M8763_IRQ_DCINR_MASK,
+	},
+	[S5M8763_IRQ_JIGF] = {
+		.reg_offset = 1,
+		.mask = S5M8763_IRQ_JIGF_MASK,
+	},
+	[S5M8763_IRQ_JIGR] = {
+		.reg_offset = 1,
+		.mask = S5M8763_IRQ_JIGR_MASK,
+	},
+	[S5M8763_IRQ_PWRONF] = {
+		.reg_offset = 1,
+		.mask = S5M8763_IRQ_PWRONF_MASK,
+	},
+	[S5M8763_IRQ_PWRONR] = {
+		.reg_offset = 1,
+		.mask = S5M8763_IRQ_PWRONR_MASK,
+	},
+	[S5M8763_IRQ_WTSREVNT] = {
+		.reg_offset = 2,
+		.mask = S5M8763_IRQ_WTSREVNT_MASK,
+	},
+	[S5M8763_IRQ_SMPLEVNT] = {
+		.reg_offset = 2,
+		.mask = S5M8763_IRQ_SMPLEVNT_MASK,
+	},
+	[S5M8763_IRQ_ALARM1] = {
+		.reg_offset = 2,
+		.mask = S5M8763_IRQ_ALARM1_MASK,
+	},
+	[S5M8763_IRQ_ALARM0] = {
+		.reg_offset = 2,
+		.mask = S5M8763_IRQ_ALARM0_MASK,
+	},
+	[S5M8763_IRQ_ONKEY1S] = {
+		.reg_offset = 3,
+		.mask = S5M8763_IRQ_ONKEY1S_MASK,
+	},
+	[S5M8763_IRQ_TOPOFFR] = {
+		.reg_offset = 3,
+		.mask = S5M8763_IRQ_TOPOFFR_MASK,
+	},
+	[S5M8763_IRQ_DCINOVPR] = {
+		.reg_offset = 3,
+		.mask = S5M8763_IRQ_DCINOVPR_MASK,
+	},
+	[S5M8763_IRQ_CHGRSTF] = {
+		.reg_offset = 3,
+		.mask = S5M8763_IRQ_CHGRSTF_MASK,
+	},
+	[S5M8763_IRQ_DONER] = {
+		.reg_offset = 3,
+		.mask = S5M8763_IRQ_DONER_MASK,
+	},
+	[S5M8763_IRQ_CHGFAULT] = {
+		.reg_offset = 3,
+		.mask = S5M8763_IRQ_CHGFAULT_MASK,
+	},
+	[S5M8763_IRQ_LOBAT1] = {
+		.reg_offset = 4,
+		.mask = S5M8763_IRQ_LOBAT1_MASK,
+	},
+	[S5M8763_IRQ_LOBAT2] = {
+		.reg_offset = 4,
+		.mask = S5M8763_IRQ_LOBAT2_MASK,
+	},
+};
+
+static struct regmap_irq_chip s2mps11_irq_chip = {
+	.name = "s2mps11",
+	.irqs = s2mps11_irqs,
+	.num_irqs = ARRAY_SIZE(s2mps11_irqs),
+	.num_regs = 3,
+	.status_base = S2MPS11_REG_INT1,
+	.mask_base = S2MPS11_REG_INT1M,
+	.ack_base = S2MPS11_REG_INT1,
+};
+
+static struct regmap_irq_chip s5m8767_irq_chip = {
+	.name = "s5m8767",
+	.irqs = s5m8767_irqs,
+	.num_irqs = ARRAY_SIZE(s5m8767_irqs),
+	.num_regs = 3,
+	.status_base = S5M8767_REG_INT1,
+	.mask_base = S5M8767_REG_INT1M,
+	.ack_base = S5M8767_REG_INT1,
+};
+
+static struct regmap_irq_chip s5m8763_irq_chip = {
+	.name = "s5m8763",
+	.irqs = s5m8763_irqs,
+	.num_irqs = ARRAY_SIZE(s5m8763_irqs),
+	.num_regs = 4,
+	.status_base = S5M8763_REG_IRQ1,
+	.mask_base = S5M8763_REG_IRQM1,
+	.ack_base = S5M8763_REG_IRQ1,
+};
+
+int sec_irq_init(struct sec_pmic_dev *sec_pmic)
+{
+	int ret = 0;
+	int type = sec_pmic->device_type;
+
+	if (!sec_pmic->irq) {
+		dev_warn(sec_pmic->dev,
+			 "No interrupt specified, no interrupts\n");
+		sec_pmic->irq_base = 0;
+		return 0;
+	}
+
+	switch (type) {
+	case S5M8763X:
+		ret = regmap_add_irq_chip(sec_pmic->regmap, sec_pmic->irq,
+				  IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				  sec_pmic->irq_base, &s5m8763_irq_chip,
+				  &sec_pmic->irq_data);
+		break;
+	case S5M8767X:
+		ret = regmap_add_irq_chip(sec_pmic->regmap, sec_pmic->irq,
+				  IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				  sec_pmic->irq_base, &s5m8767_irq_chip,
+				  &sec_pmic->irq_data);
+		break;
+	case S2MPS11X:
+		ret = regmap_add_irq_chip(sec_pmic->regmap, sec_pmic->irq,
+				  IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				  sec_pmic->irq_base, &s2mps11_irq_chip,
+				  &sec_pmic->irq_data);
+		break;
+	default:
+		dev_err(sec_pmic->dev, "Unknown device type %d\n",
+			sec_pmic->device_type);
+		return -EINVAL;
+	}
+
+	if (ret != 0) {
+		dev_err(sec_pmic->dev, "Failed to register IRQ chip: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+void sec_irq_exit(struct sec_pmic_dev *sec_pmic)
+{
+	regmap_del_irq_chip(sec_pmic->irq, sec_pmic->irq_data);
+}
diff --git a/drivers/mfd/tc3589x.c b/drivers/mfd/tc3589x.c
index de979742c6f..048bf0532a0 100644
--- a/drivers/mfd/tc3589x.c
+++ b/drivers/mfd/tc3589x.c
@@ -357,7 +357,7 @@ static int __devexit tc3589x_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int tc3589x_suspend(struct device *dev)
 {
 	struct tc3589x *tc3589x = dev_get_drvdata(dev);
@@ -385,11 +385,10 @@ static int tc3589x_resume(struct device *dev)
 
 	return ret;
 }
-
-static const SIMPLE_DEV_PM_OPS(tc3589x_dev_pm_ops, tc3589x_suspend,
-						tc3589x_resume);
 #endif
 
+static SIMPLE_DEV_PM_OPS(tc3589x_dev_pm_ops, tc3589x_suspend, tc3589x_resume);
+
 static const struct i2c_device_id tc3589x_id[] = {
 	{ "tc3589x", 24 },
 	{ }
@@ -399,9 +398,7 @@ MODULE_DEVICE_TABLE(i2c, tc3589x_id);
 static struct i2c_driver tc3589x_driver = {
 	.driver.name	= "tc3589x",
 	.driver.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
 	.driver.pm	= &tc3589x_dev_pm_ops,
-#endif
 	.probe		= tc3589x_probe,
 	.remove		= __devexit_p(tc3589x_remove),
 	.id_table	= tc3589x_id,
diff --git a/drivers/mfd/timberdale.c b/drivers/mfd/timberdale.c
index 0ba26fb12cf..a447f4ec11f 100644
--- a/drivers/mfd/timberdale.c
+++ b/drivers/mfd/timberdale.c
@@ -83,7 +83,7 @@ timberdale_xiic_platform_data = {
 
 static __devinitdata struct ocores_i2c_platform_data
 timberdale_ocores_platform_data = {
-	.regstep = 4,
+	.reg_shift = 2,
 	.clock_khz = 62500,
 	.devices = timberdale_i2c_board_info,
 	.num_devices = ARRAY_SIZE(timberdale_i2c_board_info)
diff --git a/drivers/mfd/tps65010.c b/drivers/mfd/tps65010.c
index 93d5fdf020c..da2691f22e1 100644
--- a/drivers/mfd/tps65010.c
+++ b/drivers/mfd/tps65010.c
@@ -563,8 +563,7 @@ static int tps65010_probe(struct i2c_client *client,
 	 */
 	if (client->irq > 0) {
 		status = request_irq(client->irq, tps65010_irq,
-			IRQF_SAMPLE_RANDOM | IRQF_TRIGGER_FALLING,
-			DRIVER_NAME, tps);
+				     IRQF_TRIGGER_FALLING, DRIVER_NAME, tps);
 		if (status < 0) {
 			dev_dbg(&client->dev, "can't get IRQ %d, err %d\n",
 					client->irq, status);
diff --git a/drivers/mfd/tps65090.c b/drivers/mfd/tps65090.c
index 396b9d1b6bd..80e24f4b47b 100644
--- a/drivers/mfd/tps65090.c
+++ b/drivers/mfd/tps65090.c
@@ -71,10 +71,10 @@ static const struct tps65090_irq_data tps65090_irqs[] = {
 
 static struct mfd_cell tps65090s[] = {
 	{
-		.name = "tps65910-pmic",
+		.name = "tps65090-pmic",
 	},
 	{
-		.name = "tps65910-regulator",
+		.name = "tps65090-regulator",
 	},
 };
 
diff --git a/drivers/mfd/tps65217.c b/drivers/mfd/tps65217.c
index db194e433c0..61c097a98f5 100644
--- a/drivers/mfd/tps65217.c
+++ b/drivers/mfd/tps65217.c
@@ -24,6 +24,7 @@
 #include <linux/slab.h>
 #include <linux/regmap.h>
 #include <linux/err.h>
+#include <linux/regulator/of_regulator.h>
 
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps65217.h>
@@ -132,6 +133,61 @@ int tps65217_clear_bits(struct tps65217 *tps, unsigned int reg,
 }
 EXPORT_SYMBOL_GPL(tps65217_clear_bits);
 
+#ifdef CONFIG_OF
+static struct of_regulator_match reg_matches[] = {
+	{ .name = "dcdc1", .driver_data = (void *)TPS65217_DCDC_1 },
+	{ .name = "dcdc2", .driver_data = (void *)TPS65217_DCDC_2 },
+	{ .name = "dcdc3", .driver_data = (void *)TPS65217_DCDC_3 },
+	{ .name = "ldo1", .driver_data = (void *)TPS65217_LDO_1 },
+	{ .name = "ldo2", .driver_data = (void *)TPS65217_LDO_2 },
+	{ .name = "ldo3", .driver_data = (void *)TPS65217_LDO_3 },
+	{ .name = "ldo4", .driver_data = (void *)TPS65217_LDO_4 },
+};
+
+static struct tps65217_board *tps65217_parse_dt(struct i2c_client *client)
+{
+	struct device_node *node = client->dev.of_node;
+	struct tps65217_board *pdata;
+	struct device_node *regs;
+	int count = ARRAY_SIZE(reg_matches);
+	int ret, i;
+
+	regs = of_find_node_by_name(node, "regulators");
+	if (!regs)
+		return NULL;
+
+	ret = of_regulator_match(&client->dev, regs, reg_matches, count);
+	of_node_put(regs);
+	if ((ret < 0) || (ret > count))
+		return NULL;
+
+	count = ret;
+	pdata = devm_kzalloc(&client->dev, count * sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	for (i = 0; i < count; i++) {
+		if (!reg_matches[i].init_data || !reg_matches[i].of_node)
+			continue;
+
+		pdata->tps65217_init_data[i] = reg_matches[i].init_data;
+		pdata->of_node[i] = reg_matches[i].of_node;
+	}
+
+	return pdata;
+}
+
+static struct of_device_id tps65217_of_match[] = {
+	{ .compatible = "ti,tps65217", },
+	{ },
+};
+#else
+static struct tps65217_board *tps65217_parse_dt(struct i2c_client *client)
+{
+	return NULL;
+}
+#endif
+
 static struct regmap_config tps65217_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
@@ -141,10 +197,14 @@ static int __devinit tps65217_probe(struct i2c_client *client,
 				const struct i2c_device_id *ids)
 {
 	struct tps65217 *tps;
+	struct regulator_init_data *reg_data;
 	struct tps65217_board *pdata = client->dev.platform_data;
 	int i, ret;
 	unsigned int version;
 
+	if (!pdata && client->dev.of_node)
+		pdata = tps65217_parse_dt(client);
+
 	tps = devm_kzalloc(&client->dev, sizeof(*tps), GFP_KERNEL);
 	if (!tps)
 		return -ENOMEM;
@@ -182,8 +242,9 @@ static int __devinit tps65217_probe(struct i2c_client *client,
 		}
 
 		pdev->dev.parent = tps->dev;
-		platform_device_add_data(pdev, &pdata->tps65217_init_data[i],
-					sizeof(pdata->tps65217_init_data[i]));
+		pdev->dev.of_node = pdata->of_node[i];
+		reg_data = pdata->tps65217_init_data[i];
+		platform_device_add_data(pdev, reg_data, sizeof(*reg_data));
 		tps->regulator_pdev[i] = pdev;
 
 		platform_device_add(pdev);
@@ -212,6 +273,8 @@ MODULE_DEVICE_TABLE(i2c, tps65217_id_table);
 static struct i2c_driver tps65217_driver = {
 	.driver		= {
 		.name	= "tps65217",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(tps65217_of_match),
 	},
 	.id_table	= tps65217_id_table,
 	.probe		= tps65217_probe,
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index c84b5506d5f..353c3481212 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -21,17 +21,14 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
-#include <linux/gpio.h>
+#include <linux/err.h>
 #include <linux/i2c.h>
+#include <linux/regmap.h>
 #include <linux/regulator/of_regulator.h>
 
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps6586x.h>
 
-/* GPIO control registers */
-#define TPS6586X_GPIOSET1	0x5d
-#define TPS6586X_GPIOSET2	0x5e
-
 /* interrupt control registers */
 #define TPS6586X_INT_ACK1	0xb5
 #define TPS6586X_INT_ACK2	0xb6
@@ -48,6 +45,9 @@
 /* device id */
 #define TPS6586X_VERSIONCRC	0xcd
 
+/* Maximum register */
+#define TPS6586X_MAX_REGISTER	(TPS6586X_VERSIONCRC + 1)
+
 struct tps6586x_irq_data {
 	u8	mask_reg;
 	u8	mask_mask;
@@ -89,226 +89,96 @@ static const struct tps6586x_irq_data tps6586x_irqs[] = {
 	[TPS6586X_INT_RTC_ALM2] = TPS6586X_IRQ(TPS6586X_INT_MASK4, 1 << 1),
 };
 
+static struct mfd_cell tps6586x_cell[] = {
+	{
+		.name = "tps6586x-gpio",
+	},
+	{
+		.name = "tps6586x-rtc",
+	},
+	{
+		.name = "tps6586x-onkey",
+	},
+};
+
 struct tps6586x {
-	struct mutex		lock;
 	struct device		*dev;
 	struct i2c_client	*client;
+	struct regmap		*regmap;
 
-	struct gpio_chip	gpio;
 	struct irq_chip		irq_chip;
 	struct mutex		irq_lock;
 	int			irq_base;
 	u32			irq_en;
-	u8			mask_cache[5];
 	u8			mask_reg[5];
 };
 
-static inline int __tps6586x_read(struct i2c_client *client,
-				  int reg, uint8_t *val)
-{
-	int ret;
-
-	ret = i2c_smbus_read_byte_data(client, reg);
-	if (ret < 0) {
-		dev_err(&client->dev, "failed reading at 0x%02x\n", reg);
-		return ret;
-	}
-
-	*val = (uint8_t)ret;
-
-	return 0;
-}
-
-static inline int __tps6586x_reads(struct i2c_client *client, int reg,
-				   int len, uint8_t *val)
-{
-	int ret;
-
-	ret = i2c_smbus_read_i2c_block_data(client, reg, len, val);
-	if (ret < 0) {
-		dev_err(&client->dev, "failed reading from 0x%02x\n", reg);
-		return ret;
-	}
-
-	return 0;
-}
-
-static inline int __tps6586x_write(struct i2c_client *client,
-				 int reg, uint8_t val)
+static inline struct tps6586x *dev_to_tps6586x(struct device *dev)
 {
-	int ret;
-
-	ret = i2c_smbus_write_byte_data(client, reg, val);
-	if (ret < 0) {
-		dev_err(&client->dev, "failed writing 0x%02x to 0x%02x\n",
-				val, reg);
-		return ret;
-	}
-
-	return 0;
-}
-
-static inline int __tps6586x_writes(struct i2c_client *client, int reg,
-				  int len, uint8_t *val)
-{
-	int ret, i;
-
-	for (i = 0; i < len; i++) {
-		ret = __tps6586x_write(client, reg + i, *(val + i));
-		if (ret < 0)
-			return ret;
-	}
-
-	return 0;
+	return i2c_get_clientdata(to_i2c_client(dev));
 }
 
 int tps6586x_write(struct device *dev, int reg, uint8_t val)
 {
-	return __tps6586x_write(to_i2c_client(dev), reg, val);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
+
+	return regmap_write(tps6586x->regmap, reg, val);
 }
 EXPORT_SYMBOL_GPL(tps6586x_write);
 
 int tps6586x_writes(struct device *dev, int reg, int len, uint8_t *val)
 {
-	return __tps6586x_writes(to_i2c_client(dev), reg, len, val);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
+
+	return regmap_bulk_write(tps6586x->regmap, reg, val, len);
 }
 EXPORT_SYMBOL_GPL(tps6586x_writes);
 
 int tps6586x_read(struct device *dev, int reg, uint8_t *val)
 {
-	return __tps6586x_read(to_i2c_client(dev), reg, val);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
+	unsigned int rval;
+	int ret;
+
+	ret = regmap_read(tps6586x->regmap, reg, &rval);
+	if (!ret)
+		*val = rval;
+	return ret;
 }
 EXPORT_SYMBOL_GPL(tps6586x_read);
 
 int tps6586x_reads(struct device *dev, int reg, int len, uint8_t *val)
 {
-	return __tps6586x_reads(to_i2c_client(dev), reg, len, val);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
+
+	return regmap_bulk_read(tps6586x->regmap, reg, val, len);
 }
 EXPORT_SYMBOL_GPL(tps6586x_reads);
 
 int tps6586x_set_bits(struct device *dev, int reg, uint8_t bit_mask)
 {
-	struct tps6586x *tps6586x = dev_get_drvdata(dev);
-	uint8_t reg_val;
-	int ret = 0;
-
-	mutex_lock(&tps6586x->lock);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
 
-	ret = __tps6586x_read(to_i2c_client(dev), reg, &reg_val);
-	if (ret)
-		goto out;
-
-	if ((reg_val & bit_mask) != bit_mask) {
-		reg_val |= bit_mask;
-		ret = __tps6586x_write(to_i2c_client(dev), reg, reg_val);
-	}
-out:
-	mutex_unlock(&tps6586x->lock);
-	return ret;
+	return regmap_update_bits(tps6586x->regmap, reg, bit_mask, bit_mask);
 }
 EXPORT_SYMBOL_GPL(tps6586x_set_bits);
 
 int tps6586x_clr_bits(struct device *dev, int reg, uint8_t bit_mask)
 {
-	struct tps6586x *tps6586x = dev_get_drvdata(dev);
-	uint8_t reg_val;
-	int ret = 0;
-
-	mutex_lock(&tps6586x->lock);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
 
-	ret = __tps6586x_read(to_i2c_client(dev), reg, &reg_val);
-	if (ret)
-		goto out;
-
-	if (reg_val & bit_mask) {
-		reg_val &= ~bit_mask;
-		ret = __tps6586x_write(to_i2c_client(dev), reg, reg_val);
-	}
-out:
-	mutex_unlock(&tps6586x->lock);
-	return ret;
+	return regmap_update_bits(tps6586x->regmap, reg, bit_mask, 0);
 }
 EXPORT_SYMBOL_GPL(tps6586x_clr_bits);
 
 int tps6586x_update(struct device *dev, int reg, uint8_t val, uint8_t mask)
 {
-	struct tps6586x *tps6586x = dev_get_drvdata(dev);
-	uint8_t reg_val;
-	int ret = 0;
-
-	mutex_lock(&tps6586x->lock);
+	struct tps6586x *tps6586x = dev_to_tps6586x(dev);
 
-	ret = __tps6586x_read(tps6586x->client, reg, &reg_val);
-	if (ret)
-		goto out;
-
-	if ((reg_val & mask) != val) {
-		reg_val = (reg_val & ~mask) | val;
-		ret = __tps6586x_write(tps6586x->client, reg, reg_val);
-	}
-out:
-	mutex_unlock(&tps6586x->lock);
-	return ret;
+	return regmap_update_bits(tps6586x->regmap, reg, mask, val);
 }
 EXPORT_SYMBOL_GPL(tps6586x_update);
 
-static int tps6586x_gpio_get(struct gpio_chip *gc, unsigned offset)
-{
-	struct tps6586x *tps6586x = container_of(gc, struct tps6586x, gpio);
-	uint8_t val;
-	int ret;
-
-	ret = __tps6586x_read(tps6586x->client, TPS6586X_GPIOSET2, &val);
-	if (ret)
-		return ret;
-
-	return !!(val & (1 << offset));
-}
-
-
-static void tps6586x_gpio_set(struct gpio_chip *chip, unsigned offset,
-			      int value)
-{
-	struct tps6586x *tps6586x = container_of(chip, struct tps6586x, gpio);
-
-	tps6586x_update(tps6586x->dev, TPS6586X_GPIOSET2,
-			value << offset, 1 << offset);
-}
-
-static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
-				int value)
-{
-	struct tps6586x *tps6586x = container_of(gc, struct tps6586x, gpio);
-	uint8_t val, mask;
-
-	tps6586x_gpio_set(gc, offset, value);
-
-	val = 0x1 << (offset * 2);
-	mask = 0x3 << (offset * 2);
-
-	return tps6586x_update(tps6586x->dev, TPS6586X_GPIOSET1, val, mask);
-}
-
-static int tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
-{
-	if (!gpio_base)
-		return 0;
-
-	tps6586x->gpio.owner		= THIS_MODULE;
-	tps6586x->gpio.label		= tps6586x->client->name;
-	tps6586x->gpio.dev		= tps6586x->dev;
-	tps6586x->gpio.base		= gpio_base;
-	tps6586x->gpio.ngpio		= 4;
-	tps6586x->gpio.can_sleep	= 1;
-
-	/* FIXME: add handling of GPIOs as dedicated inputs */
-	tps6586x->gpio.direction_output	= tps6586x_gpio_output;
-	tps6586x->gpio.set		= tps6586x_gpio_set;
-	tps6586x->gpio.get		= tps6586x_gpio_get;
-
-	return gpiochip_add(&tps6586x->gpio);
-}
-
 static int __remove_subdev(struct device *dev, void *unused)
 {
 	platform_device_unregister(to_platform_device(dev));
@@ -354,12 +224,11 @@ static void tps6586x_irq_sync_unlock(struct irq_data *data)
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(tps6586x->mask_reg); i++) {
-		if (tps6586x->mask_reg[i] != tps6586x->mask_cache[i]) {
-			if (!WARN_ON(tps6586x_write(tps6586x->dev,
-						    TPS6586X_INT_MASK1 + i,
-						    tps6586x->mask_reg[i])))
-				tps6586x->mask_cache[i] = tps6586x->mask_reg[i];
-		}
+		int ret;
+		ret = tps6586x_write(tps6586x->dev,
+					    TPS6586X_INT_MASK1 + i,
+					    tps6586x->mask_reg[i]);
+		WARN_ON(ret);
 	}
 
 	mutex_unlock(&tps6586x->irq_lock);
@@ -406,7 +275,6 @@ static int __devinit tps6586x_irq_init(struct tps6586x *tps6586x, int irq,
 
 	mutex_init(&tps6586x->irq_lock);
 	for (i = 0; i < 5; i++) {
-		tps6586x->mask_cache[i] = 0xff;
 		tps6586x->mask_reg[i] = 0xff;
 		tps6586x_write(tps6586x->dev, TPS6586X_INT_MASK1 + i, 0xff);
 	}
@@ -556,6 +424,23 @@ static struct tps6586x_platform_data *tps6586x_parse_dt(struct i2c_client *clien
 }
 #endif
 
+static bool is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	/* Cache all interrupt mask register */
+	if ((reg >= TPS6586X_INT_MASK1) && (reg <= TPS6586X_INT_MASK5))
+		return false;
+
+	return true;
+}
+
+static const struct regmap_config tps6586x_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = TPS6586X_MAX_REGISTER - 1,
+	.volatile_reg = is_volatile_reg,
+	.cache_type = REGCACHE_RBTREE,
+};
+
 static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
@@ -579,29 +464,39 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 
 	dev_info(&client->dev, "VERSIONCRC is %02x\n", ret);
 
-	tps6586x = kzalloc(sizeof(struct tps6586x), GFP_KERNEL);
-	if (tps6586x == NULL)
+	tps6586x = devm_kzalloc(&client->dev, sizeof(*tps6586x), GFP_KERNEL);
+	if (tps6586x == NULL) {
+		dev_err(&client->dev, "memory for tps6586x alloc failed\n");
 		return -ENOMEM;
+	}
 
 	tps6586x->client = client;
 	tps6586x->dev = &client->dev;
 	i2c_set_clientdata(client, tps6586x);
 
-	mutex_init(&tps6586x->lock);
+	tps6586x->regmap = devm_regmap_init_i2c(client,
+					&tps6586x_regmap_config);
+	if (IS_ERR(tps6586x->regmap)) {
+		ret = PTR_ERR(tps6586x->regmap);
+		dev_err(&client->dev, "regmap init failed: %d\n", ret);
+		return ret;
+	}
+
 
 	if (client->irq) {
 		ret = tps6586x_irq_init(tps6586x, client->irq,
 					pdata->irq_base);
 		if (ret) {
 			dev_err(&client->dev, "IRQ init failed: %d\n", ret);
-			goto err_irq_init;
+			return ret;
 		}
 	}
 
-	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 = mfd_add_devices(tps6586x->dev, -1,
+			tps6586x_cell, ARRAY_SIZE(tps6586x_cell), NULL, 0);
+	if (ret < 0) {
+		dev_err(&client->dev, "mfd_add_devices failed: %d\n", ret);
+		goto err_mfd_add;
 	}
 
 	ret = tps6586x_add_subdevs(tps6586x, pdata);
@@ -613,38 +508,21 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 	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:
+	mfd_remove_devices(tps6586x->dev);
+err_mfd_add:
 	if (client->irq)
 		free_irq(client->irq, tps6586x);
-err_irq_init:
-	kfree(tps6586x);
 	return ret;
 }
 
 static int __devexit tps6586x_i2c_remove(struct i2c_client *client)
 {
 	struct tps6586x *tps6586x = i2c_get_clientdata(client);
-	struct tps6586x_platform_data *pdata = client->dev.platform_data;
-	int ret;
 
+	tps6586x_remove_subdevs(tps6586x);
+	mfd_remove_devices(tps6586x->dev);
 	if (client->irq)
 		free_irq(client->irq, tps6586x);
-
-	if (pdata->gpio_base) {
-		ret = gpiochip_remove(&tps6586x->gpio);
-		if (ret)
-			dev_err(&client->dev, "Can't remove gpio chip: %d\n",
-				ret);
-	}
-
-	tps6586x_remove_subdevs(tps6586x);
-	kfree(tps6586x);
 	return 0;
 }
 
diff --git a/drivers/mfd/tps65910.c b/drivers/mfd/tps65910.c
index be9e07b7732..1c563792c77 100644
--- a/drivers/mfd/tps65910.c
+++ b/drivers/mfd/tps65910.c
@@ -68,6 +68,24 @@ static const struct regmap_config tps65910_regmap_config = {
 	.cache_type = REGCACHE_RBTREE,
 };
 
+static int __devinit tps65910_ck32k_init(struct tps65910 *tps65910,
+					struct tps65910_board *pmic_pdata)
+{
+	int ret;
+
+	if (!pmic_pdata->en_ck32k_xtal)
+		return 0;
+
+	ret = tps65910_reg_clear_bits(tps65910, TPS65910_DEVCTRL,
+						DEVCTRL_CK32K_CTRL_MASK);
+	if (ret < 0) {
+		dev_err(tps65910->dev, "clear ck32k_ctrl failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 static int __devinit tps65910_sleepinit(struct tps65910 *tps65910,
 		struct tps65910_board *pmic_pdata)
 {
@@ -175,6 +193,9 @@ static struct tps65910_board *tps65910_parse_dt(struct i2c_client *client,
 	else if (*chip_id == TPS65911)
 		dev_warn(&client->dev, "VMBCH2-Threshold not specified");
 
+	prop = of_property_read_bool(np, "ti,en-ck32k-xtal");
+	board_info->en_ck32k_xtal = prop;
+
 	board_info->irq = client->irq;
 	board_info->irq_base = -1;
 
@@ -243,7 +264,7 @@ static __devinit int tps65910_i2c_probe(struct i2c_client *i2c,
 	init_data->irq_base = pmic_plat_data->irq_base;
 
 	tps65910_irq_init(tps65910, init_data->irq, init_data);
-
+	tps65910_ck32k_init(tps65910, pmic_plat_data);
 	tps65910_sleepinit(tps65910, pmic_plat_data);
 
 	return ret;
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index 6fc90befa79..1c32afed28a 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -568,7 +568,6 @@ add_numbered_child(unsigned chip, const char *name, int num,
 		goto err;
 	}
 
-	device_init_wakeup(&pdev->dev, can_wakeup);
 	pdev->dev.parent = &twl->client->dev;
 
 	if (pdata) {
@@ -593,6 +592,8 @@ add_numbered_child(unsigned chip, const char *name, int num,
 	}
 
 	status = platform_device_add(pdev);
+	if (status == 0)
+		device_init_wakeup(&pdev->dev, can_wakeup);
 
 err:
 	if (status < 0) {
@@ -716,8 +717,9 @@ add_children(struct twl4030_platform_data *pdata, unsigned irq_base,
 		static struct regulator_consumer_supply usb1v8 = {
 			.supply =	"usb1v8",
 		};
-		static struct regulator_consumer_supply usb3v1 = {
-			.supply =	"usb3v1",
+		static struct regulator_consumer_supply usb3v1[] = {
+			{ .supply =	"usb3v1" },
+			{ .supply =	"bci3v1" },
 		};
 
 	/* First add the regulators so that they can be used by transceiver */
@@ -745,7 +747,7 @@ add_children(struct twl4030_platform_data *pdata, unsigned irq_base,
 				return PTR_ERR(child);
 
 			child = add_regulator_linked(TWL4030_REG_VUSB3V1,
-						      &usb_fixed, &usb3v1, 1,
+						      &usb_fixed, usb3v1, 2,
 						      features);
 			if (IS_ERR(child))
 				return PTR_ERR(child);
@@ -766,7 +768,7 @@ add_children(struct twl4030_platform_data *pdata, unsigned irq_base,
 		if (twl_has_regulator() && child) {
 			usb1v5.dev_name = dev_name(child);
 			usb1v8.dev_name = dev_name(child);
-			usb3v1.dev_name = dev_name(child);
+			usb3v1[0].dev_name = dev_name(child);
 		}
 	}
 	if (twl_has_usb() && pdata->usb && twl_class_is_6030()) {
diff --git a/drivers/mfd/twl6040-core.c b/drivers/mfd/twl6040-core.c
index 4ded9e7aa24..b0fad0ffca5 100644
--- a/drivers/mfd/twl6040-core.c
+++ b/drivers/mfd/twl6040-core.c
@@ -64,19 +64,15 @@ int twl6040_reg_read(struct twl6040 *twl6040, unsigned int reg)
 	int ret;
 	unsigned int val;
 
-	mutex_lock(&twl6040->io_mutex);
 	/* Vibra control registers from cache */
 	if (unlikely(reg == TWL6040_REG_VIBCTLL ||
 		     reg == TWL6040_REG_VIBCTLR)) {
 		val = twl6040->vibra_ctrl_cache[VIBRACTRL_MEMBER(reg)];
 	} else {
 		ret = regmap_read(twl6040->regmap, reg, &val);
-		if (ret < 0) {
-			mutex_unlock(&twl6040->io_mutex);
+		if (ret < 0)
 			return ret;
-		}
 	}
-	mutex_unlock(&twl6040->io_mutex);
 
 	return val;
 }
@@ -86,12 +82,10 @@ int twl6040_reg_write(struct twl6040 *twl6040, unsigned int reg, u8 val)
 {
 	int ret;
 
-	mutex_lock(&twl6040->io_mutex);
 	ret = regmap_write(twl6040->regmap, reg, val);
 	/* Cache the vibra control registers */
 	if (reg == TWL6040_REG_VIBCTLL || reg == TWL6040_REG_VIBCTLR)
 		twl6040->vibra_ctrl_cache[VIBRACTRL_MEMBER(reg)] = val;
-	mutex_unlock(&twl6040->io_mutex);
 
 	return ret;
 }
@@ -99,23 +93,13 @@ EXPORT_SYMBOL(twl6040_reg_write);
 
 int twl6040_set_bits(struct twl6040 *twl6040, unsigned int reg, u8 mask)
 {
-	int ret;
-
-	mutex_lock(&twl6040->io_mutex);
-	ret = regmap_update_bits(twl6040->regmap, reg, mask, mask);
-	mutex_unlock(&twl6040->io_mutex);
-	return ret;
+	return regmap_update_bits(twl6040->regmap, reg, mask, mask);
 }
 EXPORT_SYMBOL(twl6040_set_bits);
 
 int twl6040_clear_bits(struct twl6040 *twl6040, unsigned int reg, u8 mask)
 {
-	int ret;
-
-	mutex_lock(&twl6040->io_mutex);
-	ret = regmap_update_bits(twl6040->regmap, reg, mask, 0);
-	mutex_unlock(&twl6040->io_mutex);
-	return ret;
+	return regmap_update_bits(twl6040->regmap, reg, mask, 0);
 }
 EXPORT_SYMBOL(twl6040_clear_bits);
 
@@ -573,7 +557,6 @@ static int __devinit twl6040_probe(struct i2c_client *client,
 	twl6040->irq = client->irq;
 
 	mutex_init(&twl6040->mutex);
-	mutex_init(&twl6040->io_mutex);
 	init_completion(&twl6040->ready);
 
 	twl6040->rev = twl6040_reg_read(twl6040, TWL6040_REG_ASICREV);
@@ -696,6 +679,7 @@ static int __devexit twl6040_remove(struct i2c_client *client)
 
 static const struct i2c_device_id twl6040_i2c_id[] = {
 	{ "twl6040", 0, },
+	{ "twl6041", 0, },
 	{ },
 };
 MODULE_DEVICE_TABLE(i2c, twl6040_i2c_id);
diff --git a/drivers/mfd/wm5102-tables.c b/drivers/mfd/wm5102-tables.c
new file mode 100644
index 00000000000..01b9255ed63
--- /dev/null
+++ b/drivers/mfd/wm5102-tables.c
@@ -0,0 +1,2399 @@
+/*
+ * wm5102-tables.c  --  WM5102 data tables
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/registers.h>
+
+#include "arizona.h"
+
+#define WM5102_NUM_AOD_ISR 2
+#define WM5102_NUM_ISR 5
+
+static const struct reg_default wm5102_reva_patch[] = {
+	{ 0x80, 0x0003 },
+	{ 0x221, 0x0090 },
+	{ 0x211, 0x0014 },
+	{ 0x212, 0x0000 },
+	{ 0x214, 0x000C },
+	{ 0x171, 0x0002 },
+	{ 0x171, 0x0000 },
+	{ 0x461, 0x8000 },
+	{ 0x463, 0x50F0 },
+	{ 0x465, 0x4820 },
+	{ 0x467, 0x4040 },
+	{ 0x469, 0x3940 },
+	{ 0x46B, 0x3310 },
+	{ 0x46D, 0x2D80 },
+	{ 0x46F, 0x2890 },
+	{ 0x471, 0x1990 },
+	{ 0x473, 0x1450 },
+	{ 0x475, 0x1020 },
+	{ 0x477, 0x0CD0 },
+	{ 0x479, 0x0A30 },
+	{ 0x47B, 0x0810 },
+	{ 0x47D, 0x0510 },
+	{ 0x500, 0x000D },
+	{ 0x507, 0x1820 },
+	{ 0x508, 0x1820 },
+	{ 0x540, 0x000D },
+	{ 0x547, 0x1820 },
+	{ 0x548, 0x1820 },
+	{ 0x580, 0x000D },
+	{ 0x587, 0x1820 },
+	{ 0x588, 0x1820 },
+	{ 0x101, 0x8140 },
+	{ 0x3000, 0x2225 },
+	{ 0x3001, 0x3a03 },
+	{ 0x3002, 0x0225 },
+	{ 0x3003, 0x0801 },
+	{ 0x3004, 0x6249 },
+	{ 0x3005, 0x0c04 },
+	{ 0x3006, 0x0225 },
+	{ 0x3007, 0x5901 },
+	{ 0x3008, 0xe249 },
+	{ 0x3009, 0x030d },
+	{ 0x300a, 0x0249 },
+	{ 0x300b, 0x2c01 },
+	{ 0x300c, 0xe249 },
+	{ 0x300d, 0x4342 },
+	{ 0x300e, 0xe249 },
+	{ 0x300f, 0x73c0 },
+	{ 0x3010, 0x4249 },
+	{ 0x3011, 0x0c00 },
+	{ 0x3012, 0x0225 },
+	{ 0x3013, 0x1f01 },
+	{ 0x3014, 0x0225 },
+	{ 0x3015, 0x1e01 },
+	{ 0x3016, 0x0225 },
+	{ 0x3017, 0xfa00 },
+	{ 0x3018, 0x0000 },
+	{ 0x3019, 0xf000 },
+	{ 0x301a, 0x0000 },
+	{ 0x301b, 0xf000 },
+	{ 0x301c, 0x0000 },
+	{ 0x301d, 0xf000 },
+	{ 0x301e, 0x0000 },
+	{ 0x301f, 0xf000 },
+	{ 0x3020, 0x0000 },
+	{ 0x3021, 0xf000 },
+	{ 0x3022, 0x0000 },
+	{ 0x3023, 0xf000 },
+	{ 0x3024, 0x0000 },
+	{ 0x3025, 0xf000 },
+	{ 0x3026, 0x0000 },
+	{ 0x3027, 0xf000 },
+	{ 0x3028, 0x0000 },
+	{ 0x3029, 0xf000 },
+	{ 0x302a, 0x0000 },
+	{ 0x302b, 0xf000 },
+	{ 0x302c, 0x0000 },
+	{ 0x302d, 0xf000 },
+	{ 0x302e, 0x0000 },
+	{ 0x302f, 0xf000 },
+	{ 0x3030, 0x0225 },
+	{ 0x3031, 0x1a01 },
+	{ 0x3032, 0x0225 },
+	{ 0x3033, 0x1e00 },
+	{ 0x3034, 0x0225 },
+	{ 0x3035, 0x1f00 },
+	{ 0x3036, 0x6225 },
+	{ 0x3037, 0xf800 },
+	{ 0x3038, 0x0000 },
+	{ 0x3039, 0xf000 },
+	{ 0x303a, 0x0000 },
+	{ 0x303b, 0xf000 },
+	{ 0x303c, 0x0000 },
+	{ 0x303d, 0xf000 },
+	{ 0x303e, 0x0000 },
+	{ 0x303f, 0xf000 },
+	{ 0x3040, 0x2226 },
+	{ 0x3041, 0x3a03 },
+	{ 0x3042, 0x0226 },
+	{ 0x3043, 0x0801 },
+	{ 0x3044, 0x6249 },
+	{ 0x3045, 0x0c06 },
+	{ 0x3046, 0x0226 },
+	{ 0x3047, 0x5901 },
+	{ 0x3048, 0xe249 },
+	{ 0x3049, 0x030d },
+	{ 0x304a, 0x0249 },
+	{ 0x304b, 0x2c01 },
+	{ 0x304c, 0xe249 },
+	{ 0x304d, 0x4342 },
+	{ 0x304e, 0xe249 },
+	{ 0x304f, 0x73c0 },
+	{ 0x3050, 0x4249 },
+	{ 0x3051, 0x0c00 },
+	{ 0x3052, 0x0226 },
+	{ 0x3053, 0x1f01 },
+	{ 0x3054, 0x0226 },
+	{ 0x3055, 0x1e01 },
+	{ 0x3056, 0x0226 },
+	{ 0x3057, 0xfa00 },
+	{ 0x3058, 0x0000 },
+	{ 0x3059, 0xf000 },
+	{ 0x305a, 0x0000 },
+	{ 0x305b, 0xf000 },
+	{ 0x305c, 0x0000 },
+	{ 0x305d, 0xf000 },
+	{ 0x305e, 0x0000 },
+	{ 0x305f, 0xf000 },
+	{ 0x3060, 0x0000 },
+	{ 0x3061, 0xf000 },
+	{ 0x3062, 0x0000 },
+	{ 0x3063, 0xf000 },
+	{ 0x3064, 0x0000 },
+	{ 0x3065, 0xf000 },
+	{ 0x3066, 0x0000 },
+	{ 0x3067, 0xf000 },
+	{ 0x3068, 0x0000 },
+	{ 0x3069, 0xf000 },
+	{ 0x306a, 0x0000 },
+	{ 0x306b, 0xf000 },
+	{ 0x306c, 0x0000 },
+	{ 0x306d, 0xf000 },
+	{ 0x306e, 0x0000 },
+	{ 0x306f, 0xf000 },
+	{ 0x3070, 0x0226 },
+	{ 0x3071, 0x1a01 },
+	{ 0x3072, 0x0226 },
+	{ 0x3073, 0x1e00 },
+	{ 0x3074, 0x0226 },
+	{ 0x3075, 0x1f00 },
+	{ 0x3076, 0x6226 },
+	{ 0x3077, 0xf800 },
+	{ 0x3078, 0x0000 },
+	{ 0x3079, 0xf000 },
+	{ 0x307a, 0x0000 },
+	{ 0x307b, 0xf000 },
+	{ 0x307c, 0x0000 },
+	{ 0x307d, 0xf000 },
+	{ 0x307e, 0x0000 },
+	{ 0x307f, 0xf000 },
+	{ 0x3080, 0x2227 },
+	{ 0x3081, 0x3a03 },
+	{ 0x3082, 0x0227 },
+	{ 0x3083, 0x0801 },
+	{ 0x3084, 0x6255 },
+	{ 0x3085, 0x0c04 },
+	{ 0x3086, 0x0227 },
+	{ 0x3087, 0x5901 },
+	{ 0x3088, 0xe255 },
+	{ 0x3089, 0x030d },
+	{ 0x308a, 0x0255 },
+	{ 0x308b, 0x2c01 },
+	{ 0x308c, 0xe255 },
+	{ 0x308d, 0x4342 },
+	{ 0x308e, 0xe255 },
+	{ 0x308f, 0x73c0 },
+	{ 0x3090, 0x4255 },
+	{ 0x3091, 0x0c00 },
+	{ 0x3092, 0x0227 },
+	{ 0x3093, 0x1f01 },
+	{ 0x3094, 0x0227 },
+	{ 0x3095, 0x1e01 },
+	{ 0x3096, 0x0227 },
+	{ 0x3097, 0xfa00 },
+	{ 0x3098, 0x0000 },
+	{ 0x3099, 0xf000 },
+	{ 0x309a, 0x0000 },
+	{ 0x309b, 0xf000 },
+	{ 0x309c, 0x0000 },
+	{ 0x309d, 0xf000 },
+	{ 0x309e, 0x0000 },
+	{ 0x309f, 0xf000 },
+	{ 0x30a0, 0x0000 },
+	{ 0x30a1, 0xf000 },
+	{ 0x30a2, 0x0000 },
+	{ 0x30a3, 0xf000 },
+	{ 0x30a4, 0x0000 },
+	{ 0x30a5, 0xf000 },
+	{ 0x30a6, 0x0000 },
+	{ 0x30a7, 0xf000 },
+	{ 0x30a8, 0x0000 },
+	{ 0x30a9, 0xf000 },
+	{ 0x30aa, 0x0000 },
+	{ 0x30ab, 0xf000 },
+	{ 0x30ac, 0x0000 },
+	{ 0x30ad, 0xf000 },
+	{ 0x30ae, 0x0000 },
+	{ 0x30af, 0xf000 },
+	{ 0x30b0, 0x0227 },
+	{ 0x30b1, 0x1a01 },
+	{ 0x30b2, 0x0227 },
+	{ 0x30b3, 0x1e00 },
+	{ 0x30b4, 0x0227 },
+	{ 0x30b5, 0x1f00 },
+	{ 0x30b6, 0x6227 },
+	{ 0x30b7, 0xf800 },
+	{ 0x30b8, 0x0000 },
+	{ 0x30b9, 0xf000 },
+	{ 0x30ba, 0x0000 },
+	{ 0x30bb, 0xf000 },
+	{ 0x30bc, 0x0000 },
+	{ 0x30bd, 0xf000 },
+	{ 0x30be, 0x0000 },
+	{ 0x30bf, 0xf000 },
+	{ 0x30c0, 0x2228 },
+	{ 0x30c1, 0x3a03 },
+	{ 0x30c2, 0x0228 },
+	{ 0x30c3, 0x0801 },
+	{ 0x30c4, 0x6255 },
+	{ 0x30c5, 0x0c06 },
+	{ 0x30c6, 0x0228 },
+	{ 0x30c7, 0x5901 },
+	{ 0x30c8, 0xe255 },
+	{ 0x30c9, 0x030d },
+	{ 0x30ca, 0x0255 },
+	{ 0x30cb, 0x2c01 },
+	{ 0x30cc, 0xe255 },
+	{ 0x30cd, 0x4342 },
+	{ 0x30ce, 0xe255 },
+	{ 0x30cf, 0x73c0 },
+	{ 0x30d0, 0x4255 },
+	{ 0x30d1, 0x0c00 },
+	{ 0x30d2, 0x0228 },
+	{ 0x30d3, 0x1f01 },
+	{ 0x30d4, 0x0228 },
+	{ 0x30d5, 0x1e01 },
+	{ 0x30d6, 0x0228 },
+	{ 0x30d7, 0xfa00 },
+	{ 0x30d8, 0x0000 },
+	{ 0x30d9, 0xf000 },
+	{ 0x30da, 0x0000 },
+	{ 0x30db, 0xf000 },
+	{ 0x30dc, 0x0000 },
+	{ 0x30dd, 0xf000 },
+	{ 0x30de, 0x0000 },
+	{ 0x30df, 0xf000 },
+	{ 0x30e0, 0x0000 },
+	{ 0x30e1, 0xf000 },
+	{ 0x30e2, 0x0000 },
+	{ 0x30e3, 0xf000 },
+	{ 0x30e4, 0x0000 },
+	{ 0x30e5, 0xf000 },
+	{ 0x30e6, 0x0000 },
+	{ 0x30e7, 0xf000 },
+	{ 0x30e8, 0x0000 },
+	{ 0x30e9, 0xf000 },
+	{ 0x30ea, 0x0000 },
+	{ 0x30eb, 0xf000 },
+	{ 0x30ec, 0x0000 },
+	{ 0x30ed, 0xf000 },
+	{ 0x30ee, 0x0000 },
+	{ 0x30ef, 0xf000 },
+	{ 0x30f0, 0x0228 },
+	{ 0x30f1, 0x1a01 },
+	{ 0x30f2, 0x0228 },
+	{ 0x30f3, 0x1e00 },
+	{ 0x30f4, 0x0228 },
+	{ 0x30f5, 0x1f00 },
+	{ 0x30f6, 0x6228 },
+	{ 0x30f7, 0xf800 },
+	{ 0x30f8, 0x0000 },
+	{ 0x30f9, 0xf000 },
+	{ 0x30fa, 0x0000 },
+	{ 0x30fb, 0xf000 },
+	{ 0x30fc, 0x0000 },
+	{ 0x30fd, 0xf000 },
+	{ 0x30fe, 0x0000 },
+	{ 0x30ff, 0xf000 },
+	{ 0x3100, 0x222b },
+	{ 0x3101, 0x3a03 },
+	{ 0x3102, 0x222b },
+	{ 0x3103, 0x5803 },
+	{ 0x3104, 0xe26f },
+	{ 0x3105, 0x030d },
+	{ 0x3106, 0x626f },
+	{ 0x3107, 0x2c01 },
+	{ 0x3108, 0xe26f },
+	{ 0x3109, 0x4342 },
+	{ 0x310a, 0xe26f },
+	{ 0x310b, 0x73c0 },
+	{ 0x310c, 0x026f },
+	{ 0x310d, 0x0c00 },
+	{ 0x310e, 0x022b },
+	{ 0x310f, 0x1f01 },
+	{ 0x3110, 0x022b },
+	{ 0x3111, 0x1e01 },
+	{ 0x3112, 0x022b },
+	{ 0x3113, 0xfa00 },
+	{ 0x3114, 0x0000 },
+	{ 0x3115, 0xf000 },
+	{ 0x3116, 0x0000 },
+	{ 0x3117, 0xf000 },
+	{ 0x3118, 0x0000 },
+	{ 0x3119, 0xf000 },
+	{ 0x311a, 0x0000 },
+	{ 0x311b, 0xf000 },
+	{ 0x311c, 0x0000 },
+	{ 0x311d, 0xf000 },
+	{ 0x311e, 0x0000 },
+	{ 0x311f, 0xf000 },
+	{ 0x3120, 0x022b },
+	{ 0x3121, 0x0a01 },
+	{ 0x3122, 0x022b },
+	{ 0x3123, 0x1e00 },
+	{ 0x3124, 0x022b },
+	{ 0x3125, 0x1f00 },
+	{ 0x3126, 0x622b },
+	{ 0x3127, 0xf800 },
+	{ 0x3128, 0x0000 },
+	{ 0x3129, 0xf000 },
+	{ 0x312a, 0x0000 },
+	{ 0x312b, 0xf000 },
+	{ 0x312c, 0x0000 },
+	{ 0x312d, 0xf000 },
+	{ 0x312e, 0x0000 },
+	{ 0x312f, 0xf000 },
+	{ 0x3130, 0x0000 },
+	{ 0x3131, 0xf000 },
+	{ 0x3132, 0x0000 },
+	{ 0x3133, 0xf000 },
+	{ 0x3134, 0x0000 },
+	{ 0x3135, 0xf000 },
+	{ 0x3136, 0x0000 },
+	{ 0x3137, 0xf000 },
+	{ 0x3138, 0x0000 },
+	{ 0x3139, 0xf000 },
+	{ 0x313a, 0x0000 },
+	{ 0x313b, 0xf000 },
+	{ 0x313c, 0x0000 },
+	{ 0x313d, 0xf000 },
+	{ 0x313e, 0x0000 },
+	{ 0x313f, 0xf000 },
+	{ 0x3140, 0x0000 },
+	{ 0x3141, 0xf000 },
+	{ 0x3142, 0x0000 },
+	{ 0x3143, 0xf000 },
+	{ 0x3144, 0x0000 },
+	{ 0x3145, 0xf000 },
+	{ 0x3146, 0x0000 },
+	{ 0x3147, 0xf000 },
+	{ 0x3148, 0x0000 },
+	{ 0x3149, 0xf000 },
+	{ 0x314a, 0x0000 },
+	{ 0x314b, 0xf000 },
+	{ 0x314c, 0x0000 },
+	{ 0x314d, 0xf000 },
+	{ 0x314e, 0x0000 },
+	{ 0x314f, 0xf000 },
+	{ 0x3150, 0x0000 },
+	{ 0x3151, 0xf000 },
+	{ 0x3152, 0x0000 },
+	{ 0x3153, 0xf000 },
+	{ 0x3154, 0x0000 },
+	{ 0x3155, 0xf000 },
+	{ 0x3156, 0x0000 },
+	{ 0x3157, 0xf000 },
+	{ 0x3158, 0x0000 },
+	{ 0x3159, 0xf000 },
+	{ 0x315a, 0x0000 },
+	{ 0x315b, 0xf000 },
+	{ 0x315c, 0x0000 },
+	{ 0x315d, 0xf000 },
+	{ 0x315e, 0x0000 },
+	{ 0x315f, 0xf000 },
+	{ 0x3160, 0x0000 },
+	{ 0x3161, 0xf000 },
+	{ 0x3162, 0x0000 },
+	{ 0x3163, 0xf000 },
+	{ 0x3164, 0x0000 },
+	{ 0x3165, 0xf000 },
+	{ 0x3166, 0x0000 },
+	{ 0x3167, 0xf000 },
+	{ 0x3168, 0x0000 },
+	{ 0x3169, 0xf000 },
+	{ 0x316a, 0x0000 },
+	{ 0x316b, 0xf000 },
+	{ 0x316c, 0x0000 },
+	{ 0x316d, 0xf000 },
+	{ 0x316e, 0x0000 },
+	{ 0x316f, 0xf000 },
+	{ 0x3170, 0x0000 },
+	{ 0x3171, 0xf000 },
+	{ 0x3172, 0x0000 },
+	{ 0x3173, 0xf000 },
+	{ 0x3174, 0x0000 },
+	{ 0x3175, 0xf000 },
+	{ 0x3176, 0x0000 },
+	{ 0x3177, 0xf000 },
+	{ 0x3178, 0x0000 },
+	{ 0x3179, 0xf000 },
+	{ 0x317a, 0x0000 },
+	{ 0x317b, 0xf000 },
+	{ 0x317c, 0x0000 },
+	{ 0x317d, 0xf000 },
+	{ 0x317e, 0x0000 },
+	{ 0x317f, 0xf000 },
+	{ 0x3180, 0x2001 },
+	{ 0x3181, 0xf101 },
+	{ 0x3182, 0x0000 },
+	{ 0x3183, 0xf000 },
+	{ 0x3184, 0x0000 },
+	{ 0x3185, 0xf000 },
+	{ 0x3186, 0x0000 },
+	{ 0x3187, 0xf000 },
+	{ 0x3188, 0x0000 },
+	{ 0x3189, 0xf000 },
+	{ 0x318a, 0x0000 },
+	{ 0x318b, 0xf000 },
+	{ 0x318c, 0x0000 },
+	{ 0x318d, 0xf000 },
+	{ 0x318e, 0x0000 },
+	{ 0x318f, 0xf000 },
+	{ 0x3190, 0x0000 },
+	{ 0x3191, 0xf000 },
+	{ 0x3192, 0x0000 },
+	{ 0x3193, 0xf000 },
+	{ 0x3194, 0x0000 },
+	{ 0x3195, 0xf000 },
+	{ 0x3196, 0x0000 },
+	{ 0x3197, 0xf000 },
+	{ 0x3198, 0x0000 },
+	{ 0x3199, 0xf000 },
+	{ 0x319a, 0x0000 },
+	{ 0x319b, 0xf000 },
+	{ 0x319c, 0x0000 },
+	{ 0x319d, 0xf000 },
+	{ 0x319e, 0x0000 },
+	{ 0x319f, 0xf000 },
+	{ 0x31a0, 0x0000 },
+	{ 0x31a1, 0xf000 },
+	{ 0x31a2, 0x0000 },
+	{ 0x31a3, 0xf000 },
+	{ 0x31a4, 0x0000 },
+	{ 0x31a5, 0xf000 },
+	{ 0x31a6, 0x0000 },
+	{ 0x31a7, 0xf000 },
+	{ 0x31a8, 0x0000 },
+	{ 0x31a9, 0xf000 },
+	{ 0x31aa, 0x0000 },
+	{ 0x31ab, 0xf000 },
+	{ 0x31ac, 0x0000 },
+	{ 0x31ad, 0xf000 },
+	{ 0x31ae, 0x0000 },
+	{ 0x31af, 0xf000 },
+	{ 0x31b0, 0x0000 },
+	{ 0x31b1, 0xf000 },
+	{ 0x31b2, 0x0000 },
+	{ 0x31b3, 0xf000 },
+	{ 0x31b4, 0x0000 },
+	{ 0x31b5, 0xf000 },
+	{ 0x31b6, 0x0000 },
+	{ 0x31b7, 0xf000 },
+	{ 0x31b8, 0x0000 },
+	{ 0x31b9, 0xf000 },
+	{ 0x31ba, 0x0000 },
+	{ 0x31bb, 0xf000 },
+	{ 0x31bc, 0x0000 },
+	{ 0x31bd, 0xf000 },
+	{ 0x31be, 0x0000 },
+	{ 0x31bf, 0xf000 },
+	{ 0x31c0, 0x0000 },
+	{ 0x31c1, 0xf000 },
+	{ 0x31c2, 0x0000 },
+	{ 0x31c3, 0xf000 },
+	{ 0x31c4, 0x0000 },
+	{ 0x31c5, 0xf000 },
+	{ 0x31c6, 0x0000 },
+	{ 0x31c7, 0xf000 },
+	{ 0x31c8, 0x0000 },
+	{ 0x31c9, 0xf000 },
+	{ 0x31ca, 0x0000 },
+	{ 0x31cb, 0xf000 },
+	{ 0x31cc, 0x0000 },
+	{ 0x31cd, 0xf000 },
+	{ 0x31ce, 0x0000 },
+	{ 0x31cf, 0xf000 },
+	{ 0x31d0, 0x0000 },
+	{ 0x31d1, 0xf000 },
+	{ 0x31d2, 0x0000 },
+	{ 0x31d3, 0xf000 },
+	{ 0x31d4, 0x0000 },
+	{ 0x31d5, 0xf000 },
+	{ 0x31d6, 0x0000 },
+	{ 0x31d7, 0xf000 },
+	{ 0x31d8, 0x0000 },
+	{ 0x31d9, 0xf000 },
+	{ 0x31da, 0x0000 },
+	{ 0x31db, 0xf000 },
+	{ 0x31dc, 0x0000 },
+	{ 0x31dd, 0xf000 },
+	{ 0x31de, 0x0000 },
+	{ 0x31df, 0xf000 },
+	{ 0x31e0, 0x0000 },
+	{ 0x31e1, 0xf000 },
+	{ 0x31e2, 0x0000 },
+	{ 0x31e3, 0xf000 },
+	{ 0x31e4, 0x0000 },
+	{ 0x31e5, 0xf000 },
+	{ 0x31e6, 0x0000 },
+	{ 0x31e7, 0xf000 },
+	{ 0x31e8, 0x0000 },
+	{ 0x31e9, 0xf000 },
+	{ 0x31ea, 0x0000 },
+	{ 0x31eb, 0xf000 },
+	{ 0x31ec, 0x0000 },
+	{ 0x31ed, 0xf000 },
+	{ 0x31ee, 0x0000 },
+	{ 0x31ef, 0xf000 },
+	{ 0x31f0, 0x0000 },
+	{ 0x31f1, 0xf000 },
+	{ 0x31f2, 0x0000 },
+	{ 0x31f3, 0xf000 },
+	{ 0x31f4, 0x0000 },
+	{ 0x31f5, 0xf000 },
+	{ 0x31f6, 0x0000 },
+	{ 0x31f7, 0xf000 },
+	{ 0x31f8, 0x0000 },
+	{ 0x31f9, 0xf000 },
+	{ 0x31fa, 0x0000 },
+	{ 0x31fb, 0xf000 },
+	{ 0x31fc, 0x0000 },
+	{ 0x31fd, 0xf000 },
+	{ 0x31fe, 0x0000 },
+	{ 0x31ff, 0xf000 },
+	{ 0x024d, 0xff50 },
+	{ 0x0252, 0xff50 },
+	{ 0x0259, 0x0112 },
+	{ 0x025e, 0x0112 },
+	{ 0x101, 0x0304 },
+	{ 0x80, 0x0000 },
+};
+
+/* We use a function so we can use ARRAY_SIZE() */
+int wm5102_patch(struct arizona *arizona)
+{
+	switch (arizona->rev) {
+	case 0:
+		return regmap_register_patch(arizona->regmap,
+					     wm5102_reva_patch,
+					     ARRAY_SIZE(wm5102_reva_patch));
+	default:
+		return 0;
+	}
+}
+
+static const struct regmap_irq wm5102_aod_irqs[ARIZONA_NUM_IRQ] = {
+	[ARIZONA_IRQ_GP5_FALL] = { .mask = ARIZONA_GP5_FALL_EINT1 },
+	[ARIZONA_IRQ_GP5_RISE] = { .mask = ARIZONA_GP5_RISE_EINT1 },
+	[ARIZONA_IRQ_JD_FALL] = { .mask = ARIZONA_JD1_FALL_EINT1 },
+	[ARIZONA_IRQ_JD_RISE] = { .mask = ARIZONA_JD1_RISE_EINT1 },
+};
+
+const struct regmap_irq_chip wm5102_aod = {
+	.name = "wm5102 AOD",
+	.status_base = ARIZONA_AOD_IRQ1,
+	.mask_base = ARIZONA_AOD_IRQ_MASK_IRQ1,
+	.ack_base = ARIZONA_AOD_IRQ1,
+	.wake_base = ARIZONA_WAKE_CONTROL,
+	.num_regs = 1,
+	.irqs = wm5102_aod_irqs,
+	.num_irqs = ARRAY_SIZE(wm5102_aod_irqs),
+};
+
+static const struct regmap_irq wm5102_irqs[ARIZONA_NUM_IRQ] = {
+	[ARIZONA_IRQ_GP4] = { .reg_offset = 0, .mask = ARIZONA_GP4_EINT1 },
+	[ARIZONA_IRQ_GP3] = { .reg_offset = 0, .mask = ARIZONA_GP3_EINT1 },
+	[ARIZONA_IRQ_GP2] = { .reg_offset = 0, .mask = ARIZONA_GP2_EINT1 },
+	[ARIZONA_IRQ_GP1] = { .reg_offset = 0, .mask = ARIZONA_GP1_EINT1 },
+
+	[ARIZONA_IRQ_DSP1_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP1_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ2] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ2_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ1] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ1_EINT1
+	},
+
+	[ARIZONA_IRQ_SPK_SHUTDOWN_WARN] = {
+		.reg_offset = 2, .mask = ARIZONA_SPK_SHUTDOWN_WARN_EINT1
+	},
+	[ARIZONA_IRQ_SPK_SHUTDOWN] = {
+		.reg_offset = 2, .mask = ARIZONA_SPK_SHUTDOWN_EINT1
+	},
+	[ARIZONA_IRQ_HPDET] = {
+		.reg_offset = 2, .mask = ARIZONA_HPDET_EINT1
+	},
+	[ARIZONA_IRQ_MICDET] = {
+		.reg_offset = 2, .mask = ARIZONA_MICDET_EINT1
+	},
+	[ARIZONA_IRQ_WSEQ_DONE] = {
+		.reg_offset = 2, .mask = ARIZONA_WSEQ_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DRC2_SIG_DET] = {
+		.reg_offset = 2, .mask = ARIZONA_DRC2_SIG_DET_EINT1
+	},
+	[ARIZONA_IRQ_DRC1_SIG_DET] = {
+		.reg_offset = 2, .mask = ARIZONA_DRC1_SIG_DET_EINT1
+	},
+	[ARIZONA_IRQ_ASRC2_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_ASRC2_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_ASRC1_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_ASRC1_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_UNDERCLOCKED] = {
+		.reg_offset = 2, .mask = ARIZONA_UNDERCLOCKED_EINT1
+	},
+	[ARIZONA_IRQ_OVERCLOCKED] = {
+		.reg_offset = 2, .mask = ARIZONA_OVERCLOCKED_EINT1
+	},
+	[ARIZONA_IRQ_FLL2_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_FLL2_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_FLL1_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_FLL1_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_CLKGEN_ERR] = {
+		.reg_offset = 2, .mask = ARIZONA_CLKGEN_ERR_EINT1
+	},
+	[ARIZONA_IRQ_CLKGEN_ERR_ASYNC] = {
+		.reg_offset = 2, .mask = ARIZONA_CLKGEN_ERR_ASYNC_EINT1
+	},
+
+	[ARIZONA_IRQ_ASRC_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_ASRC_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_AIF3_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_AIF3_ERR_EINT1
+	},
+	[ARIZONA_IRQ_AIF2_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_AIF2_ERR_EINT1
+	},
+	[ARIZONA_IRQ_AIF1_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_AIF1_ERR_EINT1
+	},
+	[ARIZONA_IRQ_CTRLIF_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_CTRLIF_ERR_EINT1
+	},
+	[ARIZONA_IRQ_MIXER_DROPPED_SAMPLES] = {
+		.reg_offset = 3, .mask = ARIZONA_MIXER_DROPPED_SAMPLE_EINT1
+	},
+	[ARIZONA_IRQ_ASYNC_CLK_ENA_LOW] = {
+		.reg_offset = 3, .mask = ARIZONA_ASYNC_CLK_ENA_LOW_EINT1
+	},
+	[ARIZONA_IRQ_SYSCLK_ENA_LOW] = {
+		.reg_offset = 3, .mask = ARIZONA_SYSCLK_ENA_LOW_EINT1
+	},
+	[ARIZONA_IRQ_ISRC1_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_ISRC1_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_ISRC2_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_ISRC2_CFG_ERR_EINT1
+	},
+
+	[ARIZONA_IRQ_BOOT_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_BOOT_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DCS_DAC_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_DCS_DAC_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DCS_HP_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_DCS_HP_DONE_EINT1
+	},
+	[ARIZONA_IRQ_FLL2_CLOCK_OK] = {
+		.reg_offset = 4, .mask = ARIZONA_FLL2_CLOCK_OK_EINT1
+	},
+	[ARIZONA_IRQ_FLL1_CLOCK_OK] = {
+		.reg_offset = 4, .mask = ARIZONA_FLL1_CLOCK_OK_EINT1
+	},
+};
+
+const struct regmap_irq_chip wm5102_irq = {
+	.name = "wm5102 IRQ",
+	.status_base = ARIZONA_INTERRUPT_STATUS_1,
+	.mask_base = ARIZONA_INTERRUPT_STATUS_1_MASK,
+	.ack_base = ARIZONA_INTERRUPT_STATUS_1,
+	.num_regs = 5,
+	.irqs = wm5102_irqs,
+	.num_irqs = ARRAY_SIZE(wm5102_irqs),
+};
+
+static const struct reg_default wm5102_reg_default[] = {
+	{ 0x00000008, 0x0019 },   /* R8     - Ctrl IF SPI CFG 1 */ 
+	{ 0x00000009, 0x0001 },   /* R9     - Ctrl IF I2C1 CFG 1 */ 
+	{ 0x0000000D, 0x0000 },   /* R13    - Ctrl IF Status 1 */ 
+	{ 0x00000016, 0x0000 },   /* R22    - Write Sequencer Ctrl 0 */ 
+	{ 0x00000017, 0x0000 },   /* R23    - Write Sequencer Ctrl 1 */ 
+	{ 0x00000018, 0x0000 },   /* R24    - Write Sequencer Ctrl 2 */ 
+	{ 0x0000001A, 0x0000 },   /* R26    - Write Sequencer PROM */ 
+	{ 0x00000020, 0x0000 },   /* R32    - Tone Generator 1 */ 
+	{ 0x00000021, 0x1000 },   /* R33    - Tone Generator 2 */ 
+	{ 0x00000022, 0x0000 },   /* R34    - Tone Generator 3 */ 
+	{ 0x00000023, 0x1000 },   /* R35    - Tone Generator 4 */ 
+	{ 0x00000024, 0x0000 },   /* R36    - Tone Generator 5 */ 
+	{ 0x00000030, 0x0000 },   /* R48    - PWM Drive 1 */ 
+	{ 0x00000031, 0x0100 },   /* R49    - PWM Drive 2 */ 
+	{ 0x00000032, 0x0100 },   /* R50    - PWM Drive 3 */ 
+	{ 0x00000040, 0x0000 },   /* R64    - Wake control */ 
+	{ 0x00000041, 0x0000 },   /* R65    - Sequence control */ 
+	{ 0x00000061, 0x01FF },   /* R97    - Sample Rate Sequence Select 1 */ 
+	{ 0x00000062, 0x01FF },   /* R98    - Sample Rate Sequence Select 2 */ 
+	{ 0x00000063, 0x01FF },   /* R99    - Sample Rate Sequence Select 3 */ 
+	{ 0x00000064, 0x01FF },   /* R100   - Sample Rate Sequence Select 4 */ 
+	{ 0x00000068, 0x01FF },   /* R104   - Always On Triggers Sequence Select 1 */ 
+	{ 0x00000069, 0x01FF },   /* R105   - Always On Triggers Sequence Select 2 */ 
+	{ 0x0000006A, 0x01FF },   /* R106   - Always On Triggers Sequence Select 3 */ 
+	{ 0x0000006B, 0x01FF },   /* R107   - Always On Triggers Sequence Select 4 */ 
+	{ 0x0000006C, 0x01FF },   /* R108   - Always On Triggers Sequence Select 5 */ 
+	{ 0x0000006D, 0x01FF },   /* R109   - Always On Triggers Sequence Select 6 */ 
+	{ 0x00000070, 0x0000 },   /* R112   - Comfort Noise Generator */ 
+	{ 0x00000090, 0x0000 },   /* R144   - Haptics Control 1 */ 
+	{ 0x00000091, 0x7FFF },   /* R145   - Haptics Control 2 */ 
+	{ 0x00000092, 0x0000 },   /* R146   - Haptics phase 1 intensity */ 
+	{ 0x00000093, 0x0000 },   /* R147   - Haptics phase 1 duration */ 
+	{ 0x00000094, 0x0000 },   /* R148   - Haptics phase 2 intensity */ 
+	{ 0x00000095, 0x0000 },   /* R149   - Haptics phase 2 duration */ 
+	{ 0x00000096, 0x0000 },   /* R150   - Haptics phase 3 intensity */ 
+	{ 0x00000097, 0x0000 },   /* R151   - Haptics phase 3 duration */ 
+	{ 0x00000100, 0x0001 },   /* R256   - Clock 32k 1 */ 
+	{ 0x00000101, 0x0304 },   /* R257   - System Clock 1 */ 
+	{ 0x00000102, 0x0011 },   /* R258   - Sample rate 1 */ 
+	{ 0x00000103, 0x0011 },   /* R259   - Sample rate 2 */ 
+	{ 0x00000104, 0x0011 },   /* R260   - Sample rate 3 */ 
+	{ 0x00000112, 0x0305 },   /* R274   - Async clock 1 */ 
+	{ 0x00000113, 0x0011 },   /* R275   - Async sample rate 1 */ 
+	{ 0x00000149, 0x0000 },   /* R329   - Output system clock */ 
+	{ 0x0000014A, 0x0000 },   /* R330   - Output async clock */ 
+	{ 0x00000152, 0x0000 },   /* R338   - Rate Estimator 1 */ 
+	{ 0x00000153, 0x0000 },   /* R339   - Rate Estimator 2 */ 
+	{ 0x00000154, 0x0000 },   /* R340   - Rate Estimator 3 */ 
+	{ 0x00000155, 0x0000 },   /* R341   - Rate Estimator 4 */ 
+	{ 0x00000156, 0x0000 },   /* R342   - Rate Estimator 5 */ 
+	{ 0x00000171, 0x0000 },   /* R369   - FLL1 Control 1 */ 
+	{ 0x00000172, 0x0008 },   /* R370   - FLL1 Control 2 */ 
+	{ 0x00000173, 0x0018 },   /* R371   - FLL1 Control 3 */ 
+	{ 0x00000174, 0x007D },   /* R372   - FLL1 Control 4 */ 
+	{ 0x00000175, 0x0004 },   /* R373   - FLL1 Control 5 */ 
+	{ 0x00000176, 0x0000 },   /* R374   - FLL1 Control 6 */ 
+	{ 0x00000177, 0x0181 },   /* R375   - FLL1 Loop Filter Test 1 */ 
+	{ 0x00000181, 0x0000 },   /* R385   - FLL1 Synchroniser 1 */ 
+	{ 0x00000182, 0x0000 },   /* R386   - FLL1 Synchroniser 2 */ 
+	{ 0x00000183, 0x0000 },   /* R387   - FLL1 Synchroniser 3 */ 
+	{ 0x00000184, 0x0000 },   /* R388   - FLL1 Synchroniser 4 */ 
+	{ 0x00000185, 0x0000 },   /* R389   - FLL1 Synchroniser 5 */ 
+	{ 0x00000186, 0x0000 },   /* R390   - FLL1 Synchroniser 6 */ 
+	{ 0x00000189, 0x0000 },   /* R393   - FLL1 Spread Spectrum */ 
+	{ 0x0000018A, 0x0004 },   /* R394   - FLL1 GPIO Clock */ 
+	{ 0x00000191, 0x0000 },   /* R401   - FLL2 Control 1 */ 
+	{ 0x00000192, 0x0008 },   /* R402   - FLL2 Control 2 */ 
+	{ 0x00000193, 0x0018 },   /* R403   - FLL2 Control 3 */ 
+	{ 0x00000194, 0x007D },   /* R404   - FLL2 Control 4 */ 
+	{ 0x00000195, 0x0004 },   /* R405   - FLL2 Control 5 */ 
+	{ 0x00000196, 0x0000 },   /* R406   - FLL2 Control 6 */ 
+	{ 0x00000197, 0x0000 },   /* R407   - FLL2 Loop Filter Test 1 */ 
+	{ 0x000001A1, 0x0000 },   /* R417   - FLL2 Synchroniser 1 */ 
+	{ 0x000001A2, 0x0000 },   /* R418   - FLL2 Synchroniser 2 */ 
+	{ 0x000001A3, 0x0000 },   /* R419   - FLL2 Synchroniser 3 */ 
+	{ 0x000001A4, 0x0000 },   /* R420   - FLL2 Synchroniser 4 */ 
+	{ 0x000001A5, 0x0000 },   /* R421   - FLL2 Synchroniser 5 */ 
+	{ 0x000001A6, 0x0000 },   /* R422   - FLL2 Synchroniser 6 */ 
+	{ 0x000001A9, 0x0000 },   /* R425   - FLL2 Spread Spectrum */ 
+	{ 0x000001AA, 0x0004 },   /* R426   - FLL2 GPIO Clock */ 
+	{ 0x00000200, 0x0006 },   /* R512   - Mic Charge Pump 1 */ 
+	{ 0x00000210, 0x00D4 },   /* R528   - LDO1 Control 1 */ 
+	{ 0x00000213, 0x0344 },   /* R531   - LDO2 Control 1 */ 
+	{ 0x00000218, 0x01A6 },   /* R536   - Mic Bias Ctrl 1 */ 
+	{ 0x00000219, 0x01A6 },   /* R537   - Mic Bias Ctrl 2 */ 
+	{ 0x0000021A, 0x01A6 },   /* R538   - Mic Bias Ctrl 3 */ 
+	{ 0x00000293, 0x0000 },   /* R659   - Accessory Detect Mode 1 */ 
+	{ 0x0000029B, 0x0020 },   /* R667   - Headphone Detect 1 */ 
+	{ 0x000002A3, 0x1102 },   /* R675   - Mic Detect 1 */ 
+	{ 0x000002A4, 0x009F },   /* R676   - Mic Detect 2 */ 
+	{ 0x000002A5, 0x0000 },   /* R677   - Mic Detect 3 */ 
+	{ 0x000002C3, 0x0000 },   /* R707   - Mic noise mix control 1 */ 
+	{ 0x000002CB, 0x0000 },   /* R715   - Isolation control */ 
+	{ 0x000002D3, 0x0000 },   /* R723   - Jack detect analogue */ 
+	{ 0x00000300, 0x0000 },   /* R768   - Input Enables */ 
+	{ 0x00000308, 0x0000 },   /* R776   - Input Rate */ 
+	{ 0x00000309, 0x0022 },   /* R777   - Input Volume Ramp */ 
+	{ 0x00000310, 0x2080 },   /* R784   - IN1L Control */ 
+	{ 0x00000311, 0x0180 },   /* R785   - ADC Digital Volume 1L */ 
+	{ 0x00000312, 0x0000 },   /* R786   - DMIC1L Control */ 
+	{ 0x00000314, 0x0080 },   /* R788   - IN1R Control */ 
+	{ 0x00000315, 0x0180 },   /* R789   - ADC Digital Volume 1R */ 
+	{ 0x00000316, 0x0000 },   /* R790   - DMIC1R Control */ 
+	{ 0x00000318, 0x2080 },   /* R792   - IN2L Control */ 
+	{ 0x00000319, 0x0180 },   /* R793   - ADC Digital Volume 2L */ 
+	{ 0x0000031A, 0x0000 },   /* R794   - DMIC2L Control */ 
+	{ 0x0000031C, 0x0080 },   /* R796   - IN2R Control */ 
+	{ 0x0000031D, 0x0180 },   /* R797   - ADC Digital Volume 2R */ 
+	{ 0x0000031E, 0x0000 },   /* R798   - DMIC2R Control */ 
+	{ 0x00000320, 0x2080 },   /* R800   - IN3L Control */ 
+	{ 0x00000321, 0x0180 },   /* R801   - ADC Digital Volume 3L */ 
+	{ 0x00000322, 0x0000 },   /* R802   - DMIC3L Control */ 
+	{ 0x00000324, 0x0080 },   /* R804   - IN3R Control */ 
+	{ 0x00000325, 0x0180 },   /* R805   - ADC Digital Volume 3R */ 
+	{ 0x00000326, 0x0000 },   /* R806   - DMIC3R Control */ 
+	{ 0x00000400, 0x0000 },   /* R1024  - Output Enables 1 */ 
+	{ 0x00000408, 0x0000 },   /* R1032  - Output Rate 1 */ 
+	{ 0x00000409, 0x0022 },   /* R1033  - Output Volume Ramp */ 
+	{ 0x00000410, 0x0080 },   /* R1040  - Output Path Config 1L */ 
+	{ 0x00000411, 0x0180 },   /* R1041  - DAC Digital Volume 1L */ 
+	{ 0x00000412, 0x0080 },   /* R1042  - DAC Volume Limit 1L */ 
+	{ 0x00000413, 0x0001 },   /* R1043  - Noise Gate Select 1L */ 
+	{ 0x00000414, 0x0080 },   /* R1044  - Output Path Config 1R */ 
+	{ 0x00000415, 0x0180 },   /* R1045  - DAC Digital Volume 1R */ 
+	{ 0x00000416, 0x0080 },   /* R1046  - DAC Volume Limit 1R */ 
+	{ 0x00000417, 0x0002 },   /* R1047  - Noise Gate Select 1R */ 
+	{ 0x00000418, 0x0080 },   /* R1048  - Output Path Config 2L */ 
+	{ 0x00000419, 0x0180 },   /* R1049  - DAC Digital Volume 2L */ 
+	{ 0x0000041A, 0x0080 },   /* R1050  - DAC Volume Limit 2L */ 
+	{ 0x0000041B, 0x0004 },   /* R1051  - Noise Gate Select 2L */ 
+	{ 0x0000041C, 0x0080 },   /* R1052  - Output Path Config 2R */ 
+	{ 0x0000041D, 0x0180 },   /* R1053  - DAC Digital Volume 2R */ 
+	{ 0x0000041E, 0x0080 },   /* R1054  - DAC Volume Limit 2R */ 
+	{ 0x0000041F, 0x0008 },   /* R1055  - Noise Gate Select 2R */ 
+	{ 0x00000420, 0x0080 },   /* R1056  - Output Path Config 3L */ 
+	{ 0x00000421, 0x0180 },   /* R1057  - DAC Digital Volume 3L */ 
+	{ 0x00000422, 0x0080 },   /* R1058  - DAC Volume Limit 3L */ 
+	{ 0x00000423, 0x0010 },   /* R1059  - Noise Gate Select 3L */ 
+	{ 0x00000424, 0x0080 },   /* R1060  - Output Path Config 3R */ 
+	{ 0x00000425, 0x0180 },   /* R1061  - DAC Digital Volume 3R */ 
+	{ 0x00000426, 0x0080 },   /* R1062  - DAC Volume Limit 3R */ 
+	{ 0x00000428, 0x0000 },   /* R1064  - Output Path Config 4L */ 
+	{ 0x00000429, 0x0180 },   /* R1065  - DAC Digital Volume 4L */ 
+	{ 0x0000042A, 0x0080 },   /* R1066  - Out Volume 4L */ 
+	{ 0x0000042B, 0x0040 },   /* R1067  - Noise Gate Select 4L */ 
+	{ 0x0000042C, 0x0000 },   /* R1068  - Output Path Config 4R */ 
+	{ 0x0000042D, 0x0180 },   /* R1069  - DAC Digital Volume 4R */ 
+	{ 0x0000042E, 0x0080 },   /* R1070  - Out Volume 4R */ 
+	{ 0x0000042F, 0x0080 },   /* R1071  - Noise Gate Select 4R */ 
+	{ 0x00000430, 0x0000 },   /* R1072  - Output Path Config 5L */ 
+	{ 0x00000431, 0x0180 },   /* R1073  - DAC Digital Volume 5L */ 
+	{ 0x00000432, 0x0080 },   /* R1074  - DAC Volume Limit 5L */ 
+	{ 0x00000433, 0x0100 },   /* R1075  - Noise Gate Select 5L */ 
+	{ 0x00000434, 0x0000 },   /* R1076  - Output Path Config 5R */ 
+	{ 0x00000435, 0x0180 },   /* R1077  - DAC Digital Volume 5R */ 
+	{ 0x00000436, 0x0080 },   /* R1078  - DAC Volume Limit 5R */ 
+	{ 0x00000437, 0x0200 },   /* R1079  - Noise Gate Select 5R */ 
+	{ 0x00000450, 0x0000 },   /* R1104  - DAC AEC Control 1 */ 
+	{ 0x00000458, 0x0001 },   /* R1112  - Noise Gate Control */ 
+	{ 0x00000490, 0x0069 },   /* R1168  - PDM SPK1 CTRL 1 */ 
+	{ 0x00000491, 0x0000 },   /* R1169  - PDM SPK1 CTRL 2 */ 
+	{ 0x000004DC, 0x0000 },   /* R1244  - DAC comp 1 */ 
+	{ 0x000004DD, 0x0000 },   /* R1245  - DAC comp 2 */ 
+	{ 0x000004DE, 0x0000 },   /* R1246  - DAC comp 3 */ 
+	{ 0x000004DF, 0x0000 },   /* R1247  - DAC comp 4 */ 
+	{ 0x00000500, 0x000C },   /* R1280  - AIF1 BCLK Ctrl */ 
+	{ 0x00000501, 0x0008 },   /* R1281  - AIF1 Tx Pin Ctrl */ 
+	{ 0x00000502, 0x0000 },   /* R1282  - AIF1 Rx Pin Ctrl */ 
+	{ 0x00000503, 0x0000 },   /* R1283  - AIF1 Rate Ctrl */ 
+	{ 0x00000504, 0x0000 },   /* R1284  - AIF1 Format */ 
+	{ 0x00000505, 0x0040 },   /* R1285  - AIF1 Tx BCLK Rate */ 
+	{ 0x00000506, 0x0040 },   /* R1286  - AIF1 Rx BCLK Rate */ 
+	{ 0x00000507, 0x1818 },   /* R1287  - AIF1 Frame Ctrl 1 */ 
+	{ 0x00000508, 0x1818 },   /* R1288  - AIF1 Frame Ctrl 2 */ 
+	{ 0x00000509, 0x0000 },   /* R1289  - AIF1 Frame Ctrl 3 */ 
+	{ 0x0000050A, 0x0001 },   /* R1290  - AIF1 Frame Ctrl 4 */ 
+	{ 0x0000050B, 0x0002 },   /* R1291  - AIF1 Frame Ctrl 5 */ 
+	{ 0x0000050C, 0x0003 },   /* R1292  - AIF1 Frame Ctrl 6 */ 
+	{ 0x0000050D, 0x0004 },   /* R1293  - AIF1 Frame Ctrl 7 */ 
+	{ 0x0000050E, 0x0005 },   /* R1294  - AIF1 Frame Ctrl 8 */ 
+	{ 0x0000050F, 0x0006 },   /* R1295  - AIF1 Frame Ctrl 9 */ 
+	{ 0x00000510, 0x0007 },   /* R1296  - AIF1 Frame Ctrl 10 */ 
+	{ 0x00000511, 0x0000 },   /* R1297  - AIF1 Frame Ctrl 11 */ 
+	{ 0x00000512, 0x0001 },   /* R1298  - AIF1 Frame Ctrl 12 */ 
+	{ 0x00000513, 0x0002 },   /* R1299  - AIF1 Frame Ctrl 13 */ 
+	{ 0x00000514, 0x0003 },   /* R1300  - AIF1 Frame Ctrl 14 */ 
+	{ 0x00000515, 0x0004 },   /* R1301  - AIF1 Frame Ctrl 15 */ 
+	{ 0x00000516, 0x0005 },   /* R1302  - AIF1 Frame Ctrl 16 */ 
+	{ 0x00000517, 0x0006 },   /* R1303  - AIF1 Frame Ctrl 17 */ 
+	{ 0x00000518, 0x0007 },   /* R1304  - AIF1 Frame Ctrl 18 */ 
+	{ 0x00000519, 0x0000 },   /* R1305  - AIF1 Tx Enables */ 
+	{ 0x0000051A, 0x0000 },   /* R1306  - AIF1 Rx Enables */ 
+	{ 0x0000051B, 0x0000 },   /* R1307  - AIF1 Force Write */ 
+	{ 0x00000540, 0x000C },   /* R1344  - AIF2 BCLK Ctrl */ 
+	{ 0x00000541, 0x0008 },   /* R1345  - AIF2 Tx Pin Ctrl */ 
+	{ 0x00000542, 0x0000 },   /* R1346  - AIF2 Rx Pin Ctrl */ 
+	{ 0x00000543, 0x0000 },   /* R1347  - AIF2 Rate Ctrl */ 
+	{ 0x00000544, 0x0000 },   /* R1348  - AIF2 Format */ 
+	{ 0x00000545, 0x0040 },   /* R1349  - AIF2 Tx BCLK Rate */ 
+	{ 0x00000546, 0x0040 },   /* R1350  - AIF2 Rx BCLK Rate */ 
+	{ 0x00000547, 0x1818 },   /* R1351  - AIF2 Frame Ctrl 1 */ 
+	{ 0x00000548, 0x1818 },   /* R1352  - AIF2 Frame Ctrl 2 */ 
+	{ 0x00000549, 0x0000 },   /* R1353  - AIF2 Frame Ctrl 3 */ 
+	{ 0x0000054A, 0x0001 },   /* R1354  - AIF2 Frame Ctrl 4 */ 
+	{ 0x00000551, 0x0000 },   /* R1361  - AIF2 Frame Ctrl 11 */ 
+	{ 0x00000552, 0x0001 },   /* R1362  - AIF2 Frame Ctrl 12 */ 
+	{ 0x00000559, 0x0000 },   /* R1369  - AIF2 Tx Enables */ 
+	{ 0x0000055A, 0x0000 },   /* R1370  - AIF2 Rx Enables */ 
+	{ 0x0000055B, 0x0000 },   /* R1371  - AIF2 Force Write */ 
+	{ 0x00000580, 0x000C },   /* R1408  - AIF3 BCLK Ctrl */ 
+	{ 0x00000581, 0x0008 },   /* R1409  - AIF3 Tx Pin Ctrl */ 
+	{ 0x00000582, 0x0000 },   /* R1410  - AIF3 Rx Pin Ctrl */ 
+	{ 0x00000583, 0x0000 },   /* R1411  - AIF3 Rate Ctrl */ 
+	{ 0x00000584, 0x0000 },   /* R1412  - AIF3 Format */ 
+	{ 0x00000585, 0x0040 },   /* R1413  - AIF3 Tx BCLK Rate */ 
+	{ 0x00000586, 0x0040 },   /* R1414  - AIF3 Rx BCLK Rate */ 
+	{ 0x00000587, 0x1818 },   /* R1415  - AIF3 Frame Ctrl 1 */ 
+	{ 0x00000588, 0x1818 },   /* R1416  - AIF3 Frame Ctrl 2 */ 
+	{ 0x00000589, 0x0000 },   /* R1417  - AIF3 Frame Ctrl 3 */ 
+	{ 0x0000058A, 0x0001 },   /* R1418  - AIF3 Frame Ctrl 4 */ 
+	{ 0x00000591, 0x0000 },   /* R1425  - AIF3 Frame Ctrl 11 */ 
+	{ 0x00000592, 0x0001 },   /* R1426  - AIF3 Frame Ctrl 12 */ 
+	{ 0x00000599, 0x0000 },   /* R1433  - AIF3 Tx Enables */ 
+	{ 0x0000059A, 0x0000 },   /* R1434  - AIF3 Rx Enables */ 
+	{ 0x0000059B, 0x0000 },   /* R1435  - AIF3 Force Write */ 
+	{ 0x000005E3, 0x0004 },   /* R1507  - SLIMbus Framer Ref Gear */ 
+	{ 0x000005E5, 0x0000 },   /* R1509  - SLIMbus Rates 1 */ 
+	{ 0x000005E6, 0x0000 },   /* R1510  - SLIMbus Rates 2 */ 
+	{ 0x000005E7, 0x0000 },   /* R1511  - SLIMbus Rates 3 */ 
+	{ 0x000005E8, 0x0000 },   /* R1512  - SLIMbus Rates 4 */ 
+	{ 0x000005E9, 0x0000 },   /* R1513  - SLIMbus Rates 5 */ 
+	{ 0x000005EA, 0x0000 },   /* R1514  - SLIMbus Rates 6 */ 
+	{ 0x000005EB, 0x0000 },   /* R1515  - SLIMbus Rates 7 */ 
+	{ 0x000005EC, 0x0000 },   /* R1516  - SLIMbus Rates 8 */ 
+	{ 0x000005F5, 0x0000 },   /* R1525  - SLIMbus RX Channel Enable */ 
+	{ 0x000005F6, 0x0000 },   /* R1526  - SLIMbus TX Channel Enable */ 
+	{ 0x00000640, 0x0000 },   /* R1600  - PWM1MIX Input 1 Source */ 
+	{ 0x00000641, 0x0080 },   /* R1601  - PWM1MIX Input 1 Volume */ 
+	{ 0x00000642, 0x0000 },   /* R1602  - PWM1MIX Input 2 Source */ 
+	{ 0x00000643, 0x0080 },   /* R1603  - PWM1MIX Input 2 Volume */ 
+	{ 0x00000644, 0x0000 },   /* R1604  - PWM1MIX Input 3 Source */ 
+	{ 0x00000645, 0x0080 },   /* R1605  - PWM1MIX Input 3 Volume */ 
+	{ 0x00000646, 0x0000 },   /* R1606  - PWM1MIX Input 4 Source */ 
+	{ 0x00000647, 0x0080 },   /* R1607  - PWM1MIX Input 4 Volume */ 
+	{ 0x00000648, 0x0000 },   /* R1608  - PWM2MIX Input 1 Source */ 
+	{ 0x00000649, 0x0080 },   /* R1609  - PWM2MIX Input 1 Volume */ 
+	{ 0x0000064A, 0x0000 },   /* R1610  - PWM2MIX Input 2 Source */ 
+	{ 0x0000064B, 0x0080 },   /* R1611  - PWM2MIX Input 2 Volume */ 
+	{ 0x0000064C, 0x0000 },   /* R1612  - PWM2MIX Input 3 Source */ 
+	{ 0x0000064D, 0x0080 },   /* R1613  - PWM2MIX Input 3 Volume */ 
+	{ 0x0000064E, 0x0000 },   /* R1614  - PWM2MIX Input 4 Source */ 
+	{ 0x0000064F, 0x0080 },   /* R1615  - PWM2MIX Input 4 Volume */ 
+	{ 0x00000660, 0x0000 },   /* R1632  - MICMIX Input 1 Source */ 
+	{ 0x00000661, 0x0080 },   /* R1633  - MICMIX Input 1 Volume */ 
+	{ 0x00000662, 0x0000 },   /* R1634  - MICMIX Input 2 Source */ 
+	{ 0x00000663, 0x0080 },   /* R1635  - MICMIX Input 2 Volume */ 
+	{ 0x00000664, 0x0000 },   /* R1636  - MICMIX Input 3 Source */ 
+	{ 0x00000665, 0x0080 },   /* R1637  - MICMIX Input 3 Volume */ 
+	{ 0x00000666, 0x0000 },   /* R1638  - MICMIX Input 4 Source */ 
+	{ 0x00000667, 0x0080 },   /* R1639  - MICMIX Input 4 Volume */ 
+	{ 0x00000668, 0x0000 },   /* R1640  - NOISEMIX Input 1 Source */ 
+	{ 0x00000669, 0x0080 },   /* R1641  - NOISEMIX Input 1 Volume */ 
+	{ 0x0000066A, 0x0000 },   /* R1642  - NOISEMIX Input 2 Source */ 
+	{ 0x0000066B, 0x0080 },   /* R1643  - NOISEMIX Input 2 Volume */ 
+	{ 0x0000066C, 0x0000 },   /* R1644  - NOISEMIX Input 3 Source */ 
+	{ 0x0000066D, 0x0080 },   /* R1645  - NOISEMIX Input 3 Volume */ 
+	{ 0x0000066E, 0x0000 },   /* R1646  - NOISEMIX Input 4 Source */ 
+	{ 0x0000066F, 0x0080 },   /* R1647  - NOISEMIX Input 4 Volume */ 
+	{ 0x00000680, 0x0000 },   /* R1664  - OUT1LMIX Input 1 Source */ 
+	{ 0x00000681, 0x0080 },   /* R1665  - OUT1LMIX Input 1 Volume */ 
+	{ 0x00000682, 0x0000 },   /* R1666  - OUT1LMIX Input 2 Source */ 
+	{ 0x00000683, 0x0080 },   /* R1667  - OUT1LMIX Input 2 Volume */ 
+	{ 0x00000684, 0x0000 },   /* R1668  - OUT1LMIX Input 3 Source */ 
+	{ 0x00000685, 0x0080 },   /* R1669  - OUT1LMIX Input 3 Volume */ 
+	{ 0x00000686, 0x0000 },   /* R1670  - OUT1LMIX Input 4 Source */ 
+	{ 0x00000687, 0x0080 },   /* R1671  - OUT1LMIX Input 4 Volume */ 
+	{ 0x00000688, 0x0000 },   /* R1672  - OUT1RMIX Input 1 Source */ 
+	{ 0x00000689, 0x0080 },   /* R1673  - OUT1RMIX Input 1 Volume */ 
+	{ 0x0000068A, 0x0000 },   /* R1674  - OUT1RMIX Input 2 Source */ 
+	{ 0x0000068B, 0x0080 },   /* R1675  - OUT1RMIX Input 2 Volume */ 
+	{ 0x0000068C, 0x0000 },   /* R1676  - OUT1RMIX Input 3 Source */ 
+	{ 0x0000068D, 0x0080 },   /* R1677  - OUT1RMIX Input 3 Volume */ 
+	{ 0x0000068E, 0x0000 },   /* R1678  - OUT1RMIX Input 4 Source */ 
+	{ 0x0000068F, 0x0080 },   /* R1679  - OUT1RMIX Input 4 Volume */ 
+	{ 0x00000690, 0x0000 },   /* R1680  - OUT2LMIX Input 1 Source */ 
+	{ 0x00000691, 0x0080 },   /* R1681  - OUT2LMIX Input 1 Volume */ 
+	{ 0x00000692, 0x0000 },   /* R1682  - OUT2LMIX Input 2 Source */ 
+	{ 0x00000693, 0x0080 },   /* R1683  - OUT2LMIX Input 2 Volume */ 
+	{ 0x00000694, 0x0000 },   /* R1684  - OUT2LMIX Input 3 Source */ 
+	{ 0x00000695, 0x0080 },   /* R1685  - OUT2LMIX Input 3 Volume */ 
+	{ 0x00000696, 0x0000 },   /* R1686  - OUT2LMIX Input 4 Source */ 
+	{ 0x00000697, 0x0080 },   /* R1687  - OUT2LMIX Input 4 Volume */ 
+	{ 0x00000698, 0x0000 },   /* R1688  - OUT2RMIX Input 1 Source */ 
+	{ 0x00000699, 0x0080 },   /* R1689  - OUT2RMIX Input 1 Volume */ 
+	{ 0x0000069A, 0x0000 },   /* R1690  - OUT2RMIX Input 2 Source */ 
+	{ 0x0000069B, 0x0080 },   /* R1691  - OUT2RMIX Input 2 Volume */ 
+	{ 0x0000069C, 0x0000 },   /* R1692  - OUT2RMIX Input 3 Source */ 
+	{ 0x0000069D, 0x0080 },   /* R1693  - OUT2RMIX Input 3 Volume */ 
+	{ 0x0000069E, 0x0000 },   /* R1694  - OUT2RMIX Input 4 Source */ 
+	{ 0x0000069F, 0x0080 },   /* R1695  - OUT2RMIX Input 4 Volume */ 
+	{ 0x000006A0, 0x0000 },   /* R1696  - OUT3LMIX Input 1 Source */ 
+	{ 0x000006A1, 0x0080 },   /* R1697  - OUT3LMIX Input 1 Volume */ 
+	{ 0x000006A2, 0x0000 },   /* R1698  - OUT3LMIX Input 2 Source */ 
+	{ 0x000006A3, 0x0080 },   /* R1699  - OUT3LMIX Input 2 Volume */ 
+	{ 0x000006A4, 0x0000 },   /* R1700  - OUT3LMIX Input 3 Source */ 
+	{ 0x000006A5, 0x0080 },   /* R1701  - OUT3LMIX Input 3 Volume */ 
+	{ 0x000006A6, 0x0000 },   /* R1702  - OUT3LMIX Input 4 Source */ 
+	{ 0x000006A7, 0x0080 },   /* R1703  - OUT3LMIX Input 4 Volume */ 
+	{ 0x000006B0, 0x0000 },   /* R1712  - OUT4LMIX Input 1 Source */ 
+	{ 0x000006B1, 0x0080 },   /* R1713  - OUT4LMIX Input 1 Volume */ 
+	{ 0x000006B2, 0x0000 },   /* R1714  - OUT4LMIX Input 2 Source */ 
+	{ 0x000006B3, 0x0080 },   /* R1715  - OUT4LMIX Input 2 Volume */ 
+	{ 0x000006B4, 0x0000 },   /* R1716  - OUT4LMIX Input 3 Source */ 
+	{ 0x000006B5, 0x0080 },   /* R1717  - OUT4LMIX Input 3 Volume */ 
+	{ 0x000006B6, 0x0000 },   /* R1718  - OUT4LMIX Input 4 Source */ 
+	{ 0x000006B7, 0x0080 },   /* R1719  - OUT4LMIX Input 4 Volume */ 
+	{ 0x000006B8, 0x0000 },   /* R1720  - OUT4RMIX Input 1 Source */ 
+	{ 0x000006B9, 0x0080 },   /* R1721  - OUT4RMIX Input 1 Volume */ 
+	{ 0x000006BA, 0x0000 },   /* R1722  - OUT4RMIX Input 2 Source */ 
+	{ 0x000006BB, 0x0080 },   /* R1723  - OUT4RMIX Input 2 Volume */ 
+	{ 0x000006BC, 0x0000 },   /* R1724  - OUT4RMIX Input 3 Source */ 
+	{ 0x000006BD, 0x0080 },   /* R1725  - OUT4RMIX Input 3 Volume */ 
+	{ 0x000006BE, 0x0000 },   /* R1726  - OUT4RMIX Input 4 Source */ 
+	{ 0x000006BF, 0x0080 },   /* R1727  - OUT4RMIX Input 4 Volume */ 
+	{ 0x000006C0, 0x0000 },   /* R1728  - OUT5LMIX Input 1 Source */ 
+	{ 0x000006C1, 0x0080 },   /* R1729  - OUT5LMIX Input 1 Volume */ 
+	{ 0x000006C2, 0x0000 },   /* R1730  - OUT5LMIX Input 2 Source */ 
+	{ 0x000006C3, 0x0080 },   /* R1731  - OUT5LMIX Input 2 Volume */ 
+	{ 0x000006C4, 0x0000 },   /* R1732  - OUT5LMIX Input 3 Source */ 
+	{ 0x000006C5, 0x0080 },   /* R1733  - OUT5LMIX Input 3 Volume */ 
+	{ 0x000006C6, 0x0000 },   /* R1734  - OUT5LMIX Input 4 Source */ 
+	{ 0x000006C7, 0x0080 },   /* R1735  - OUT5LMIX Input 4 Volume */ 
+	{ 0x000006C8, 0x0000 },   /* R1736  - OUT5RMIX Input 1 Source */ 
+	{ 0x000006C9, 0x0080 },   /* R1737  - OUT5RMIX Input 1 Volume */ 
+	{ 0x000006CA, 0x0000 },   /* R1738  - OUT5RMIX Input 2 Source */ 
+	{ 0x000006CB, 0x0080 },   /* R1739  - OUT5RMIX Input 2 Volume */ 
+	{ 0x000006CC, 0x0000 },   /* R1740  - OUT5RMIX Input 3 Source */ 
+	{ 0x000006CD, 0x0080 },   /* R1741  - OUT5RMIX Input 3 Volume */ 
+	{ 0x000006CE, 0x0000 },   /* R1742  - OUT5RMIX Input 4 Source */ 
+	{ 0x000006CF, 0x0080 },   /* R1743  - OUT5RMIX Input 4 Volume */ 
+	{ 0x00000700, 0x0000 },   /* R1792  - AIF1TX1MIX Input 1 Source */ 
+	{ 0x00000701, 0x0080 },   /* R1793  - AIF1TX1MIX Input 1 Volume */ 
+	{ 0x00000702, 0x0000 },   /* R1794  - AIF1TX1MIX Input 2 Source */ 
+	{ 0x00000703, 0x0080 },   /* R1795  - AIF1TX1MIX Input 2 Volume */ 
+	{ 0x00000704, 0x0000 },   /* R1796  - AIF1TX1MIX Input 3 Source */ 
+	{ 0x00000705, 0x0080 },   /* R1797  - AIF1TX1MIX Input 3 Volume */ 
+	{ 0x00000706, 0x0000 },   /* R1798  - AIF1TX1MIX Input 4 Source */ 
+	{ 0x00000707, 0x0080 },   /* R1799  - AIF1TX1MIX Input 4 Volume */ 
+	{ 0x00000708, 0x0000 },   /* R1800  - AIF1TX2MIX Input 1 Source */ 
+	{ 0x00000709, 0x0080 },   /* R1801  - AIF1TX2MIX Input 1 Volume */ 
+	{ 0x0000070A, 0x0000 },   /* R1802  - AIF1TX2MIX Input 2 Source */ 
+	{ 0x0000070B, 0x0080 },   /* R1803  - AIF1TX2MIX Input 2 Volume */ 
+	{ 0x0000070C, 0x0000 },   /* R1804  - AIF1TX2MIX Input 3 Source */ 
+	{ 0x0000070D, 0x0080 },   /* R1805  - AIF1TX2MIX Input 3 Volume */ 
+	{ 0x0000070E, 0x0000 },   /* R1806  - AIF1TX2MIX Input 4 Source */ 
+	{ 0x0000070F, 0x0080 },   /* R1807  - AIF1TX2MIX Input 4 Volume */ 
+	{ 0x00000710, 0x0000 },   /* R1808  - AIF1TX3MIX Input 1 Source */ 
+	{ 0x00000711, 0x0080 },   /* R1809  - AIF1TX3MIX Input 1 Volume */ 
+	{ 0x00000712, 0x0000 },   /* R1810  - AIF1TX3MIX Input 2 Source */ 
+	{ 0x00000713, 0x0080 },   /* R1811  - AIF1TX3MIX Input 2 Volume */ 
+	{ 0x00000714, 0x0000 },   /* R1812  - AIF1TX3MIX Input 3 Source */ 
+	{ 0x00000715, 0x0080 },   /* R1813  - AIF1TX3MIX Input 3 Volume */ 
+	{ 0x00000716, 0x0000 },   /* R1814  - AIF1TX3MIX Input 4 Source */ 
+	{ 0x00000717, 0x0080 },   /* R1815  - AIF1TX3MIX Input 4 Volume */ 
+	{ 0x00000718, 0x0000 },   /* R1816  - AIF1TX4MIX Input 1 Source */ 
+	{ 0x00000719, 0x0080 },   /* R1817  - AIF1TX4MIX Input 1 Volume */ 
+	{ 0x0000071A, 0x0000 },   /* R1818  - AIF1TX4MIX Input 2 Source */ 
+	{ 0x0000071B, 0x0080 },   /* R1819  - AIF1TX4MIX Input 2 Volume */ 
+	{ 0x0000071C, 0x0000 },   /* R1820  - AIF1TX4MIX Input 3 Source */ 
+	{ 0x0000071D, 0x0080 },   /* R1821  - AIF1TX4MIX Input 3 Volume */ 
+	{ 0x0000071E, 0x0000 },   /* R1822  - AIF1TX4MIX Input 4 Source */ 
+	{ 0x0000071F, 0x0080 },   /* R1823  - AIF1TX4MIX Input 4 Volume */ 
+	{ 0x00000720, 0x0000 },   /* R1824  - AIF1TX5MIX Input 1 Source */ 
+	{ 0x00000721, 0x0080 },   /* R1825  - AIF1TX5MIX Input 1 Volume */ 
+	{ 0x00000722, 0x0000 },   /* R1826  - AIF1TX5MIX Input 2 Source */ 
+	{ 0x00000723, 0x0080 },   /* R1827  - AIF1TX5MIX Input 2 Volume */ 
+	{ 0x00000724, 0x0000 },   /* R1828  - AIF1TX5MIX Input 3 Source */ 
+	{ 0x00000725, 0x0080 },   /* R1829  - AIF1TX5MIX Input 3 Volume */ 
+	{ 0x00000726, 0x0000 },   /* R1830  - AIF1TX5MIX Input 4 Source */ 
+	{ 0x00000727, 0x0080 },   /* R1831  - AIF1TX5MIX Input 4 Volume */ 
+	{ 0x00000728, 0x0000 },   /* R1832  - AIF1TX6MIX Input 1 Source */ 
+	{ 0x00000729, 0x0080 },   /* R1833  - AIF1TX6MIX Input 1 Volume */ 
+	{ 0x0000072A, 0x0000 },   /* R1834  - AIF1TX6MIX Input 2 Source */ 
+	{ 0x0000072B, 0x0080 },   /* R1835  - AIF1TX6MIX Input 2 Volume */ 
+	{ 0x0000072C, 0x0000 },   /* R1836  - AIF1TX6MIX Input 3 Source */ 
+	{ 0x0000072D, 0x0080 },   /* R1837  - AIF1TX6MIX Input 3 Volume */ 
+	{ 0x0000072E, 0x0000 },   /* R1838  - AIF1TX6MIX Input 4 Source */ 
+	{ 0x0000072F, 0x0080 },   /* R1839  - AIF1TX6MIX Input 4 Volume */ 
+	{ 0x00000730, 0x0000 },   /* R1840  - AIF1TX7MIX Input 1 Source */ 
+	{ 0x00000731, 0x0080 },   /* R1841  - AIF1TX7MIX Input 1 Volume */ 
+	{ 0x00000732, 0x0000 },   /* R1842  - AIF1TX7MIX Input 2 Source */ 
+	{ 0x00000733, 0x0080 },   /* R1843  - AIF1TX7MIX Input 2 Volume */ 
+	{ 0x00000734, 0x0000 },   /* R1844  - AIF1TX7MIX Input 3 Source */ 
+	{ 0x00000735, 0x0080 },   /* R1845  - AIF1TX7MIX Input 3 Volume */ 
+	{ 0x00000736, 0x0000 },   /* R1846  - AIF1TX7MIX Input 4 Source */ 
+	{ 0x00000737, 0x0080 },   /* R1847  - AIF1TX7MIX Input 4 Volume */ 
+	{ 0x00000738, 0x0000 },   /* R1848  - AIF1TX8MIX Input 1 Source */ 
+	{ 0x00000739, 0x0080 },   /* R1849  - AIF1TX8MIX Input 1 Volume */ 
+	{ 0x0000073A, 0x0000 },   /* R1850  - AIF1TX8MIX Input 2 Source */ 
+	{ 0x0000073B, 0x0080 },   /* R1851  - AIF1TX8MIX Input 2 Volume */ 
+	{ 0x0000073C, 0x0000 },   /* R1852  - AIF1TX8MIX Input 3 Source */ 
+	{ 0x0000073D, 0x0080 },   /* R1853  - AIF1TX8MIX Input 3 Volume */ 
+	{ 0x0000073E, 0x0000 },   /* R1854  - AIF1TX8MIX Input 4 Source */ 
+	{ 0x0000073F, 0x0080 },   /* R1855  - AIF1TX8MIX Input 4 Volume */ 
+	{ 0x00000740, 0x0000 },   /* R1856  - AIF2TX1MIX Input 1 Source */ 
+	{ 0x00000741, 0x0080 },   /* R1857  - AIF2TX1MIX Input 1 Volume */ 
+	{ 0x00000742, 0x0000 },   /* R1858  - AIF2TX1MIX Input 2 Source */ 
+	{ 0x00000743, 0x0080 },   /* R1859  - AIF2TX1MIX Input 2 Volume */ 
+	{ 0x00000744, 0x0000 },   /* R1860  - AIF2TX1MIX Input 3 Source */ 
+	{ 0x00000745, 0x0080 },   /* R1861  - AIF2TX1MIX Input 3 Volume */ 
+	{ 0x00000746, 0x0000 },   /* R1862  - AIF2TX1MIX Input 4 Source */ 
+	{ 0x00000747, 0x0080 },   /* R1863  - AIF2TX1MIX Input 4 Volume */ 
+	{ 0x00000748, 0x0000 },   /* R1864  - AIF2TX2MIX Input 1 Source */ 
+	{ 0x00000749, 0x0080 },   /* R1865  - AIF2TX2MIX Input 1 Volume */ 
+	{ 0x0000074A, 0x0000 },   /* R1866  - AIF2TX2MIX Input 2 Source */ 
+	{ 0x0000074B, 0x0080 },   /* R1867  - AIF2TX2MIX Input 2 Volume */ 
+	{ 0x0000074C, 0x0000 },   /* R1868  - AIF2TX2MIX Input 3 Source */ 
+	{ 0x0000074D, 0x0080 },   /* R1869  - AIF2TX2MIX Input 3 Volume */ 
+	{ 0x0000074E, 0x0000 },   /* R1870  - AIF2TX2MIX Input 4 Source */ 
+	{ 0x0000074F, 0x0080 },   /* R1871  - AIF2TX2MIX Input 4 Volume */ 
+	{ 0x00000780, 0x0000 },   /* R1920  - AIF3TX1MIX Input 1 Source */ 
+	{ 0x00000781, 0x0080 },   /* R1921  - AIF3TX1MIX Input 1 Volume */ 
+	{ 0x00000782, 0x0000 },   /* R1922  - AIF3TX1MIX Input 2 Source */ 
+	{ 0x00000783, 0x0080 },   /* R1923  - AIF3TX1MIX Input 2 Volume */ 
+	{ 0x00000784, 0x0000 },   /* R1924  - AIF3TX1MIX Input 3 Source */ 
+	{ 0x00000785, 0x0080 },   /* R1925  - AIF3TX1MIX Input 3 Volume */ 
+	{ 0x00000786, 0x0000 },   /* R1926  - AIF3TX1MIX Input 4 Source */ 
+	{ 0x00000787, 0x0080 },   /* R1927  - AIF3TX1MIX Input 4 Volume */ 
+	{ 0x00000788, 0x0000 },   /* R1928  - AIF3TX2MIX Input 1 Source */ 
+	{ 0x00000789, 0x0080 },   /* R1929  - AIF3TX2MIX Input 1 Volume */ 
+	{ 0x0000078A, 0x0000 },   /* R1930  - AIF3TX2MIX Input 2 Source */ 
+	{ 0x0000078B, 0x0080 },   /* R1931  - AIF3TX2MIX Input 2 Volume */ 
+	{ 0x0000078C, 0x0000 },   /* R1932  - AIF3TX2MIX Input 3 Source */ 
+	{ 0x0000078D, 0x0080 },   /* R1933  - AIF3TX2MIX Input 3 Volume */ 
+	{ 0x0000078E, 0x0000 },   /* R1934  - AIF3TX2MIX Input 4 Source */ 
+	{ 0x0000078F, 0x0080 },   /* R1935  - AIF3TX2MIX Input 4 Volume */ 
+	{ 0x000007C0, 0x0000 },   /* R1984  - SLIMTX1MIX Input 1 Source */ 
+	{ 0x000007C1, 0x0080 },   /* R1985  - SLIMTX1MIX Input 1 Volume */ 
+	{ 0x000007C2, 0x0000 },   /* R1986  - SLIMTX1MIX Input 2 Source */ 
+	{ 0x000007C3, 0x0080 },   /* R1987  - SLIMTX1MIX Input 2 Volume */ 
+	{ 0x000007C4, 0x0000 },   /* R1988  - SLIMTX1MIX Input 3 Source */ 
+	{ 0x000007C5, 0x0080 },   /* R1989  - SLIMTX1MIX Input 3 Volume */ 
+	{ 0x000007C6, 0x0000 },   /* R1990  - SLIMTX1MIX Input 4 Source */ 
+	{ 0x000007C7, 0x0080 },   /* R1991  - SLIMTX1MIX Input 4 Volume */ 
+	{ 0x000007C8, 0x0000 },   /* R1992  - SLIMTX2MIX Input 1 Source */ 
+	{ 0x000007C9, 0x0080 },   /* R1993  - SLIMTX2MIX Input 1 Volume */ 
+	{ 0x000007CA, 0x0000 },   /* R1994  - SLIMTX2MIX Input 2 Source */ 
+	{ 0x000007CB, 0x0080 },   /* R1995  - SLIMTX2MIX Input 2 Volume */ 
+	{ 0x000007CC, 0x0000 },   /* R1996  - SLIMTX2MIX Input 3 Source */ 
+	{ 0x000007CD, 0x0080 },   /* R1997  - SLIMTX2MIX Input 3 Volume */ 
+	{ 0x000007CE, 0x0000 },   /* R1998  - SLIMTX2MIX Input 4 Source */ 
+	{ 0x000007CF, 0x0080 },   /* R1999  - SLIMTX2MIX Input 4 Volume */ 
+	{ 0x000007D0, 0x0000 },   /* R2000  - SLIMTX3MIX Input 1 Source */ 
+	{ 0x000007D1, 0x0080 },   /* R2001  - SLIMTX3MIX Input 1 Volume */ 
+	{ 0x000007D2, 0x0000 },   /* R2002  - SLIMTX3MIX Input 2 Source */ 
+	{ 0x000007D3, 0x0080 },   /* R2003  - SLIMTX3MIX Input 2 Volume */ 
+	{ 0x000007D4, 0x0000 },   /* R2004  - SLIMTX3MIX Input 3 Source */ 
+	{ 0x000007D5, 0x0080 },   /* R2005  - SLIMTX3MIX Input 3 Volume */ 
+	{ 0x000007D6, 0x0000 },   /* R2006  - SLIMTX3MIX Input 4 Source */ 
+	{ 0x000007D7, 0x0080 },   /* R2007  - SLIMTX3MIX Input 4 Volume */ 
+	{ 0x000007D8, 0x0000 },   /* R2008  - SLIMTX4MIX Input 1 Source */ 
+	{ 0x000007D9, 0x0080 },   /* R2009  - SLIMTX4MIX Input 1 Volume */ 
+	{ 0x000007DA, 0x0000 },   /* R2010  - SLIMTX4MIX Input 2 Source */ 
+	{ 0x000007DB, 0x0080 },   /* R2011  - SLIMTX4MIX Input 2 Volume */ 
+	{ 0x000007DC, 0x0000 },   /* R2012  - SLIMTX4MIX Input 3 Source */ 
+	{ 0x000007DD, 0x0080 },   /* R2013  - SLIMTX4MIX Input 3 Volume */ 
+	{ 0x000007DE, 0x0000 },   /* R2014  - SLIMTX4MIX Input 4 Source */ 
+	{ 0x000007DF, 0x0080 },   /* R2015  - SLIMTX4MIX Input 4 Volume */ 
+	{ 0x000007E0, 0x0000 },   /* R2016  - SLIMTX5MIX Input 1 Source */ 
+	{ 0x000007E1, 0x0080 },   /* R2017  - SLIMTX5MIX Input 1 Volume */ 
+	{ 0x000007E2, 0x0000 },   /* R2018  - SLIMTX5MIX Input 2 Source */ 
+	{ 0x000007E3, 0x0080 },   /* R2019  - SLIMTX5MIX Input 2 Volume */ 
+	{ 0x000007E4, 0x0000 },   /* R2020  - SLIMTX5MIX Input 3 Source */ 
+	{ 0x000007E5, 0x0080 },   /* R2021  - SLIMTX5MIX Input 3 Volume */ 
+	{ 0x000007E6, 0x0000 },   /* R2022  - SLIMTX5MIX Input 4 Source */ 
+	{ 0x000007E7, 0x0080 },   /* R2023  - SLIMTX5MIX Input 4 Volume */ 
+	{ 0x000007E8, 0x0000 },   /* R2024  - SLIMTX6MIX Input 1 Source */ 
+	{ 0x000007E9, 0x0080 },   /* R2025  - SLIMTX6MIX Input 1 Volume */ 
+	{ 0x000007EA, 0x0000 },   /* R2026  - SLIMTX6MIX Input 2 Source */ 
+	{ 0x000007EB, 0x0080 },   /* R2027  - SLIMTX6MIX Input 2 Volume */ 
+	{ 0x000007EC, 0x0000 },   /* R2028  - SLIMTX6MIX Input 3 Source */ 
+	{ 0x000007ED, 0x0080 },   /* R2029  - SLIMTX6MIX Input 3 Volume */ 
+	{ 0x000007EE, 0x0000 },   /* R2030  - SLIMTX6MIX Input 4 Source */ 
+	{ 0x000007EF, 0x0080 },   /* R2031  - SLIMTX6MIX Input 4 Volume */ 
+	{ 0x000007F0, 0x0000 },   /* R2032  - SLIMTX7MIX Input 1 Source */ 
+	{ 0x000007F1, 0x0080 },   /* R2033  - SLIMTX7MIX Input 1 Volume */ 
+	{ 0x000007F2, 0x0000 },   /* R2034  - SLIMTX7MIX Input 2 Source */ 
+	{ 0x000007F3, 0x0080 },   /* R2035  - SLIMTX7MIX Input 2 Volume */ 
+	{ 0x000007F4, 0x0000 },   /* R2036  - SLIMTX7MIX Input 3 Source */ 
+	{ 0x000007F5, 0x0080 },   /* R2037  - SLIMTX7MIX Input 3 Volume */ 
+	{ 0x000007F6, 0x0000 },   /* R2038  - SLIMTX7MIX Input 4 Source */ 
+	{ 0x000007F7, 0x0080 },   /* R2039  - SLIMTX7MIX Input 4 Volume */ 
+	{ 0x000007F8, 0x0000 },   /* R2040  - SLIMTX8MIX Input 1 Source */ 
+	{ 0x000007F9, 0x0080 },   /* R2041  - SLIMTX8MIX Input 1 Volume */ 
+	{ 0x000007FA, 0x0000 },   /* R2042  - SLIMTX8MIX Input 2 Source */ 
+	{ 0x000007FB, 0x0080 },   /* R2043  - SLIMTX8MIX Input 2 Volume */ 
+	{ 0x000007FC, 0x0000 },   /* R2044  - SLIMTX8MIX Input 3 Source */ 
+	{ 0x000007FD, 0x0080 },   /* R2045  - SLIMTX8MIX Input 3 Volume */ 
+	{ 0x000007FE, 0x0000 },   /* R2046  - SLIMTX8MIX Input 4 Source */ 
+	{ 0x000007FF, 0x0080 },   /* R2047  - SLIMTX8MIX Input 4 Volume */ 
+	{ 0x00000880, 0x0000 },   /* R2176  - EQ1MIX Input 1 Source */ 
+	{ 0x00000881, 0x0080 },   /* R2177  - EQ1MIX Input 1 Volume */ 
+	{ 0x00000882, 0x0000 },   /* R2178  - EQ1MIX Input 2 Source */ 
+	{ 0x00000883, 0x0080 },   /* R2179  - EQ1MIX Input 2 Volume */ 
+	{ 0x00000884, 0x0000 },   /* R2180  - EQ1MIX Input 3 Source */ 
+	{ 0x00000885, 0x0080 },   /* R2181  - EQ1MIX Input 3 Volume */ 
+	{ 0x00000886, 0x0000 },   /* R2182  - EQ1MIX Input 4 Source */ 
+	{ 0x00000887, 0x0080 },   /* R2183  - EQ1MIX Input 4 Volume */ 
+	{ 0x00000888, 0x0000 },   /* R2184  - EQ2MIX Input 1 Source */ 
+	{ 0x00000889, 0x0080 },   /* R2185  - EQ2MIX Input 1 Volume */ 
+	{ 0x0000088A, 0x0000 },   /* R2186  - EQ2MIX Input 2 Source */ 
+	{ 0x0000088B, 0x0080 },   /* R2187  - EQ2MIX Input 2 Volume */ 
+	{ 0x0000088C, 0x0000 },   /* R2188  - EQ2MIX Input 3 Source */ 
+	{ 0x0000088D, 0x0080 },   /* R2189  - EQ2MIX Input 3 Volume */ 
+	{ 0x0000088E, 0x0000 },   /* R2190  - EQ2MIX Input 4 Source */ 
+	{ 0x0000088F, 0x0080 },   /* R2191  - EQ2MIX Input 4 Volume */ 
+	{ 0x00000890, 0x0000 },   /* R2192  - EQ3MIX Input 1 Source */ 
+	{ 0x00000891, 0x0080 },   /* R2193  - EQ3MIX Input 1 Volume */ 
+	{ 0x00000892, 0x0000 },   /* R2194  - EQ3MIX Input 2 Source */ 
+	{ 0x00000893, 0x0080 },   /* R2195  - EQ3MIX Input 2 Volume */ 
+	{ 0x00000894, 0x0000 },   /* R2196  - EQ3MIX Input 3 Source */ 
+	{ 0x00000895, 0x0080 },   /* R2197  - EQ3MIX Input 3 Volume */ 
+	{ 0x00000896, 0x0000 },   /* R2198  - EQ3MIX Input 4 Source */ 
+	{ 0x00000897, 0x0080 },   /* R2199  - EQ3MIX Input 4 Volume */ 
+	{ 0x00000898, 0x0000 },   /* R2200  - EQ4MIX Input 1 Source */ 
+	{ 0x00000899, 0x0080 },   /* R2201  - EQ4MIX Input 1 Volume */ 
+	{ 0x0000089A, 0x0000 },   /* R2202  - EQ4MIX Input 2 Source */ 
+	{ 0x0000089B, 0x0080 },   /* R2203  - EQ4MIX Input 2 Volume */ 
+	{ 0x0000089C, 0x0000 },   /* R2204  - EQ4MIX Input 3 Source */ 
+	{ 0x0000089D, 0x0080 },   /* R2205  - EQ4MIX Input 3 Volume */ 
+	{ 0x0000089E, 0x0000 },   /* R2206  - EQ4MIX Input 4 Source */ 
+	{ 0x0000089F, 0x0080 },   /* R2207  - EQ4MIX Input 4 Volume */ 
+	{ 0x000008C0, 0x0000 },   /* R2240  - DRC1LMIX Input 1 Source */ 
+	{ 0x000008C1, 0x0080 },   /* R2241  - DRC1LMIX Input 1 Volume */ 
+	{ 0x000008C2, 0x0000 },   /* R2242  - DRC1LMIX Input 2 Source */ 
+	{ 0x000008C3, 0x0080 },   /* R2243  - DRC1LMIX Input 2 Volume */ 
+	{ 0x000008C4, 0x0000 },   /* R2244  - DRC1LMIX Input 3 Source */ 
+	{ 0x000008C5, 0x0080 },   /* R2245  - DRC1LMIX Input 3 Volume */ 
+	{ 0x000008C6, 0x0000 },   /* R2246  - DRC1LMIX Input 4 Source */ 
+	{ 0x000008C7, 0x0080 },   /* R2247  - DRC1LMIX Input 4 Volume */ 
+	{ 0x000008C8, 0x0000 },   /* R2248  - DRC1RMIX Input 1 Source */ 
+	{ 0x000008C9, 0x0080 },   /* R2249  - DRC1RMIX Input 1 Volume */ 
+	{ 0x000008CA, 0x0000 },   /* R2250  - DRC1RMIX Input 2 Source */ 
+	{ 0x000008CB, 0x0080 },   /* R2251  - DRC1RMIX Input 2 Volume */ 
+	{ 0x000008CC, 0x0000 },   /* R2252  - DRC1RMIX Input 3 Source */ 
+	{ 0x000008CD, 0x0080 },   /* R2253  - DRC1RMIX Input 3 Volume */ 
+	{ 0x000008CE, 0x0000 },   /* R2254  - DRC1RMIX Input 4 Source */ 
+	{ 0x000008CF, 0x0080 },   /* R2255  - DRC1RMIX Input 4 Volume */ 
+	{ 0x000008D0, 0x0000 },   /* R2256  - DRC2LMIX Input 1 Source */ 
+	{ 0x000008D1, 0x0080 },   /* R2257  - DRC2LMIX Input 1 Volume */ 
+	{ 0x000008D2, 0x0000 },   /* R2258  - DRC2LMIX Input 2 Source */ 
+	{ 0x000008D3, 0x0080 },   /* R2259  - DRC2LMIX Input 2 Volume */ 
+	{ 0x000008D4, 0x0000 },   /* R2260  - DRC2LMIX Input 3 Source */ 
+	{ 0x000008D5, 0x0080 },   /* R2261  - DRC2LMIX Input 3 Volume */ 
+	{ 0x000008D6, 0x0000 },   /* R2262  - DRC2LMIX Input 4 Source */ 
+	{ 0x000008D7, 0x0080 },   /* R2263  - DRC2LMIX Input 4 Volume */ 
+	{ 0x000008D8, 0x0000 },   /* R2264  - DRC2RMIX Input 1 Source */ 
+	{ 0x000008D9, 0x0080 },   /* R2265  - DRC2RMIX Input 1 Volume */ 
+	{ 0x000008DA, 0x0000 },   /* R2266  - DRC2RMIX Input 2 Source */ 
+	{ 0x000008DB, 0x0080 },   /* R2267  - DRC2RMIX Input 2 Volume */ 
+	{ 0x000008DC, 0x0000 },   /* R2268  - DRC2RMIX Input 3 Source */ 
+	{ 0x000008DD, 0x0080 },   /* R2269  - DRC2RMIX Input 3 Volume */ 
+	{ 0x000008DE, 0x0000 },   /* R2270  - DRC2RMIX Input 4 Source */ 
+	{ 0x000008DF, 0x0080 },   /* R2271  - DRC2RMIX Input 4 Volume */ 
+	{ 0x00000900, 0x0000 },   /* R2304  - HPLP1MIX Input 1 Source */ 
+	{ 0x00000901, 0x0080 },   /* R2305  - HPLP1MIX Input 1 Volume */ 
+	{ 0x00000902, 0x0000 },   /* R2306  - HPLP1MIX Input 2 Source */ 
+	{ 0x00000903, 0x0080 },   /* R2307  - HPLP1MIX Input 2 Volume */ 
+	{ 0x00000904, 0x0000 },   /* R2308  - HPLP1MIX Input 3 Source */ 
+	{ 0x00000905, 0x0080 },   /* R2309  - HPLP1MIX Input 3 Volume */ 
+	{ 0x00000906, 0x0000 },   /* R2310  - HPLP1MIX Input 4 Source */ 
+	{ 0x00000907, 0x0080 },   /* R2311  - HPLP1MIX Input 4 Volume */ 
+	{ 0x00000908, 0x0000 },   /* R2312  - HPLP2MIX Input 1 Source */ 
+	{ 0x00000909, 0x0080 },   /* R2313  - HPLP2MIX Input 1 Volume */ 
+	{ 0x0000090A, 0x0000 },   /* R2314  - HPLP2MIX Input 2 Source */ 
+	{ 0x0000090B, 0x0080 },   /* R2315  - HPLP2MIX Input 2 Volume */ 
+	{ 0x0000090C, 0x0000 },   /* R2316  - HPLP2MIX Input 3 Source */ 
+	{ 0x0000090D, 0x0080 },   /* R2317  - HPLP2MIX Input 3 Volume */ 
+	{ 0x0000090E, 0x0000 },   /* R2318  - HPLP2MIX Input 4 Source */ 
+	{ 0x0000090F, 0x0080 },   /* R2319  - HPLP2MIX Input 4 Volume */ 
+	{ 0x00000910, 0x0000 },   /* R2320  - HPLP3MIX Input 1 Source */ 
+	{ 0x00000911, 0x0080 },   /* R2321  - HPLP3MIX Input 1 Volume */ 
+	{ 0x00000912, 0x0000 },   /* R2322  - HPLP3MIX Input 2 Source */ 
+	{ 0x00000913, 0x0080 },   /* R2323  - HPLP3MIX Input 2 Volume */ 
+	{ 0x00000914, 0x0000 },   /* R2324  - HPLP3MIX Input 3 Source */ 
+	{ 0x00000915, 0x0080 },   /* R2325  - HPLP3MIX Input 3 Volume */ 
+	{ 0x00000916, 0x0000 },   /* R2326  - HPLP3MIX Input 4 Source */ 
+	{ 0x00000917, 0x0080 },   /* R2327  - HPLP3MIX Input 4 Volume */ 
+	{ 0x00000918, 0x0000 },   /* R2328  - HPLP4MIX Input 1 Source */ 
+	{ 0x00000919, 0x0080 },   /* R2329  - HPLP4MIX Input 1 Volume */ 
+	{ 0x0000091A, 0x0000 },   /* R2330  - HPLP4MIX Input 2 Source */ 
+	{ 0x0000091B, 0x0080 },   /* R2331  - HPLP4MIX Input 2 Volume */ 
+	{ 0x0000091C, 0x0000 },   /* R2332  - HPLP4MIX Input 3 Source */ 
+	{ 0x0000091D, 0x0080 },   /* R2333  - HPLP4MIX Input 3 Volume */ 
+	{ 0x0000091E, 0x0000 },   /* R2334  - HPLP4MIX Input 4 Source */ 
+	{ 0x0000091F, 0x0080 },   /* R2335  - HPLP4MIX Input 4 Volume */ 
+	{ 0x00000940, 0x0000 },   /* R2368  - DSP1LMIX Input 1 Source */ 
+	{ 0x00000941, 0x0080 },   /* R2369  - DSP1LMIX Input 1 Volume */ 
+	{ 0x00000942, 0x0000 },   /* R2370  - DSP1LMIX Input 2 Source */ 
+	{ 0x00000943, 0x0080 },   /* R2371  - DSP1LMIX Input 2 Volume */ 
+	{ 0x00000944, 0x0000 },   /* R2372  - DSP1LMIX Input 3 Source */ 
+	{ 0x00000945, 0x0080 },   /* R2373  - DSP1LMIX Input 3 Volume */ 
+	{ 0x00000946, 0x0000 },   /* R2374  - DSP1LMIX Input 4 Source */ 
+	{ 0x00000947, 0x0080 },   /* R2375  - DSP1LMIX Input 4 Volume */ 
+	{ 0x00000948, 0x0000 },   /* R2376  - DSP1RMIX Input 1 Source */ 
+	{ 0x00000949, 0x0080 },   /* R2377  - DSP1RMIX Input 1 Volume */ 
+	{ 0x0000094A, 0x0000 },   /* R2378  - DSP1RMIX Input 2 Source */ 
+	{ 0x0000094B, 0x0080 },   /* R2379  - DSP1RMIX Input 2 Volume */ 
+	{ 0x0000094C, 0x0000 },   /* R2380  - DSP1RMIX Input 3 Source */ 
+	{ 0x0000094D, 0x0080 },   /* R2381  - DSP1RMIX Input 3 Volume */ 
+	{ 0x0000094E, 0x0000 },   /* R2382  - DSP1RMIX Input 4 Source */ 
+	{ 0x0000094F, 0x0080 },   /* R2383  - DSP1RMIX Input 4 Volume */ 
+	{ 0x00000950, 0x0000 },   /* R2384  - DSP1AUX1MIX Input 1 Source */ 
+	{ 0x00000958, 0x0000 },   /* R2392  - DSP1AUX2MIX Input 1 Source */ 
+	{ 0x00000960, 0x0000 },   /* R2400  - DSP1AUX3MIX Input 1 Source */ 
+	{ 0x00000968, 0x0000 },   /* R2408  - DSP1AUX4MIX Input 1 Source */ 
+	{ 0x00000970, 0x0000 },   /* R2416  - DSP1AUX5MIX Input 1 Source */ 
+	{ 0x00000978, 0x0000 },   /* R2424  - DSP1AUX6MIX Input 1 Source */ 
+	{ 0x00000A80, 0x0000 },   /* R2688  - ASRC1LMIX Input 1 Source */ 
+	{ 0x00000A88, 0x0000 },   /* R2696  - ASRC1RMIX Input 1 Source */ 
+	{ 0x00000A90, 0x0000 },   /* R2704  - ASRC2LMIX Input 1 Source */ 
+	{ 0x00000A98, 0x0000 },   /* R2712  - ASRC2RMIX Input 1 Source */ 
+	{ 0x00000B00, 0x0000 },   /* R2816  - ISRC1DEC1MIX Input 1 Source */ 
+	{ 0x00000B08, 0x0000 },   /* R2824  - ISRC1DEC2MIX Input 1 Source */ 
+	{ 0x00000B20, 0x0000 },   /* R2848  - ISRC1INT1MIX Input 1 Source */ 
+	{ 0x00000B28, 0x0000 },   /* R2856  - ISRC1INT2MIX Input 1 Source */ 
+	{ 0x00000B40, 0x0000 },   /* R2880  - ISRC2DEC1MIX Input 1 Source */ 
+	{ 0x00000B48, 0x0000 },   /* R2888  - ISRC2DEC2MIX Input 1 Source */ 
+	{ 0x00000B60, 0x0000 },   /* R2912  - ISRC2INT1MIX Input 1 Source */ 
+	{ 0x00000B68, 0x0000 },   /* R2920  - ISRC2INT2MIX Input 1 Source */ 
+	{ 0x00000C00, 0xA101 },   /* R3072  - GPIO1 CTRL */ 
+	{ 0x00000C01, 0xA101 },   /* R3073  - GPIO2 CTRL */ 
+	{ 0x00000C02, 0xA101 },   /* R3074  - GPIO3 CTRL */ 
+	{ 0x00000C03, 0xA101 },   /* R3075  - GPIO4 CTRL */ 
+	{ 0x00000C04, 0xA101 },   /* R3076  - GPIO5 CTRL */ 
+	{ 0x00000C0F, 0x0400 },   /* R3087  - IRQ CTRL 1 */ 
+	{ 0x00000C10, 0x1000 },   /* R3088  - GPIO Debounce Config */ 
+	{ 0x00000C20, 0x8002 },   /* R3104  - Misc Pad Ctrl 1 */ 
+	{ 0x00000C21, 0x8001 },   /* R3105  - Misc Pad Ctrl 2 */ 
+	{ 0x00000C22, 0x0000 },   /* R3106  - Misc Pad Ctrl 3 */ 
+	{ 0x00000C23, 0x0000 },   /* R3107  - Misc Pad Ctrl 4 */ 
+	{ 0x00000C24, 0x0000 },   /* R3108  - Misc Pad Ctrl 5 */ 
+	{ 0x00000C25, 0x0000 },   /* R3109  - Misc Pad Ctrl 6 */ 
+	{ 0x00000D08, 0xFFFF },   /* R3336  - Interrupt Status 1 Mask */ 
+	{ 0x00000D09, 0xFFFF },   /* R3337  - Interrupt Status 2 Mask */ 
+	{ 0x00000D0A, 0xFFFF },   /* R3338  - Interrupt Status 3 Mask */ 
+	{ 0x00000D0B, 0xFFFF },   /* R3339  - Interrupt Status 4 Mask */ 
+	{ 0x00000D0C, 0xFEFF },   /* R3340  - Interrupt Status 5 Mask */ 
+	{ 0x00000D0F, 0x0000 },   /* R3343  - Interrupt Control */ 
+	{ 0x00000D18, 0xFFFF },   /* R3352  - IRQ2 Status 1 Mask */ 
+	{ 0x00000D19, 0xFFFF },   /* R3353  - IRQ2 Status 2 Mask */ 
+	{ 0x00000D1A, 0xFFFF },   /* R3354  - IRQ2 Status 3 Mask */ 
+	{ 0x00000D1B, 0xFFFF },   /* R3355  - IRQ2 Status 4 Mask */ 
+	{ 0x00000D1C, 0xFFFF },   /* R3356  - IRQ2 Status 5 Mask */ 
+	{ 0x00000D1F, 0x0000 },   /* R3359  - IRQ2 Control */ 
+	{ 0x00000D41, 0x0000 },   /* R3393  - ADSP2 IRQ0 */ 
+	{ 0x00000D53, 0xFFFF },   /* R3411  - AOD IRQ Mask IRQ1 */ 
+	{ 0x00000D54, 0xFFFF },   /* R3412  - AOD IRQ Mask IRQ2 */ 
+	{ 0x00000D56, 0x0000 },   /* R3414  - Jack detect debounce */ 
+	{ 0x00000E00, 0x0000 },   /* R3584  - FX_Ctrl1 */ 
+	{ 0x00000E01, 0x0000 },   /* R3585  - FX_Ctrl2 */ 
+	{ 0x00000E10, 0x6318 },   /* R3600  - EQ1_1 */ 
+	{ 0x00000E11, 0x6300 },   /* R3601  - EQ1_2 */ 
+	{ 0x00000E12, 0x0FC8 },   /* R3602  - EQ1_3 */ 
+	{ 0x00000E13, 0x03FE },   /* R3603  - EQ1_4 */ 
+	{ 0x00000E14, 0x00E0 },   /* R3604  - EQ1_5 */ 
+	{ 0x00000E15, 0x1EC4 },   /* R3605  - EQ1_6 */ 
+	{ 0x00000E16, 0xF136 },   /* R3606  - EQ1_7 */ 
+	{ 0x00000E17, 0x0409 },   /* R3607  - EQ1_8 */ 
+	{ 0x00000E18, 0x04CC },   /* R3608  - EQ1_9 */ 
+	{ 0x00000E19, 0x1C9B },   /* R3609  - EQ1_10 */ 
+	{ 0x00000E1A, 0xF337 },   /* R3610  - EQ1_11 */ 
+	{ 0x00000E1B, 0x040B },   /* R3611  - EQ1_12 */ 
+	{ 0x00000E1C, 0x0CBB },   /* R3612  - EQ1_13 */ 
+	{ 0x00000E1D, 0x16F8 },   /* R3613  - EQ1_14 */ 
+	{ 0x00000E1E, 0xF7D9 },   /* R3614  - EQ1_15 */ 
+	{ 0x00000E1F, 0x040A },   /* R3615  - EQ1_16 */ 
+	{ 0x00000E20, 0x1F14 },   /* R3616  - EQ1_17 */ 
+	{ 0x00000E21, 0x058C },   /* R3617  - EQ1_18 */ 
+	{ 0x00000E22, 0x0563 },   /* R3618  - EQ1_19 */ 
+	{ 0x00000E23, 0x4000 },   /* R3619  - EQ1_20 */ 
+	{ 0x00000E24, 0x0B75 },   /* R3620  - EQ1_21 */ 
+	{ 0x00000E26, 0x6318 },   /* R3622  - EQ2_1 */ 
+	{ 0x00000E27, 0x6300 },   /* R3623  - EQ2_2 */ 
+	{ 0x00000E28, 0x0FC8 },   /* R3624  - EQ2_3 */ 
+	{ 0x00000E29, 0x03FE },   /* R3625  - EQ2_4 */ 
+	{ 0x00000E2A, 0x00E0 },   /* R3626  - EQ2_5 */ 
+	{ 0x00000E2B, 0x1EC4 },   /* R3627  - EQ2_6 */ 
+	{ 0x00000E2C, 0xF136 },   /* R3628  - EQ2_7 */ 
+	{ 0x00000E2D, 0x0409 },   /* R3629  - EQ2_8 */ 
+	{ 0x00000E2E, 0x04CC },   /* R3630  - EQ2_9 */ 
+	{ 0x00000E2F, 0x1C9B },   /* R3631  - EQ2_10 */ 
+	{ 0x00000E30, 0xF337 },   /* R3632  - EQ2_11 */ 
+	{ 0x00000E31, 0x040B },   /* R3633  - EQ2_12 */ 
+	{ 0x00000E32, 0x0CBB },   /* R3634  - EQ2_13 */ 
+	{ 0x00000E33, 0x16F8 },   /* R3635  - EQ2_14 */ 
+	{ 0x00000E34, 0xF7D9 },   /* R3636  - EQ2_15 */ 
+	{ 0x00000E35, 0x040A },   /* R3637  - EQ2_16 */ 
+	{ 0x00000E36, 0x1F14 },   /* R3638  - EQ2_17 */ 
+	{ 0x00000E37, 0x058C },   /* R3639  - EQ2_18 */ 
+	{ 0x00000E38, 0x0563 },   /* R3640  - EQ2_19 */ 
+	{ 0x00000E39, 0x4000 },   /* R3641  - EQ2_20 */ 
+	{ 0x00000E3A, 0x0B75 },   /* R3642  - EQ2_21 */ 
+	{ 0x00000E3C, 0x6318 },   /* R3644  - EQ3_1 */ 
+	{ 0x00000E3D, 0x6300 },   /* R3645  - EQ3_2 */ 
+	{ 0x00000E3E, 0x0FC8 },   /* R3646  - EQ3_3 */ 
+	{ 0x00000E3F, 0x03FE },   /* R3647  - EQ3_4 */ 
+	{ 0x00000E40, 0x00E0 },   /* R3648  - EQ3_5 */ 
+	{ 0x00000E41, 0x1EC4 },   /* R3649  - EQ3_6 */ 
+	{ 0x00000E42, 0xF136 },   /* R3650  - EQ3_7 */ 
+	{ 0x00000E43, 0x0409 },   /* R3651  - EQ3_8 */ 
+	{ 0x00000E44, 0x04CC },   /* R3652  - EQ3_9 */ 
+	{ 0x00000E45, 0x1C9B },   /* R3653  - EQ3_10 */ 
+	{ 0x00000E46, 0xF337 },   /* R3654  - EQ3_11 */ 
+	{ 0x00000E47, 0x040B },   /* R3655  - EQ3_12 */ 
+	{ 0x00000E48, 0x0CBB },   /* R3656  - EQ3_13 */ 
+	{ 0x00000E49, 0x16F8 },   /* R3657  - EQ3_14 */ 
+	{ 0x00000E4A, 0xF7D9 },   /* R3658  - EQ3_15 */ 
+	{ 0x00000E4B, 0x040A },   /* R3659  - EQ3_16 */ 
+	{ 0x00000E4C, 0x1F14 },   /* R3660  - EQ3_17 */ 
+	{ 0x00000E4D, 0x058C },   /* R3661  - EQ3_18 */ 
+	{ 0x00000E4E, 0x0563 },   /* R3662  - EQ3_19 */ 
+	{ 0x00000E4F, 0x4000 },   /* R3663  - EQ3_20 */ 
+	{ 0x00000E50, 0x0B75 },   /* R3664  - EQ3_21 */ 
+	{ 0x00000E52, 0x6318 },   /* R3666  - EQ4_1 */ 
+	{ 0x00000E53, 0x6300 },   /* R3667  - EQ4_2 */ 
+	{ 0x00000E54, 0x0FC8 },   /* R3668  - EQ4_3 */ 
+	{ 0x00000E55, 0x03FE },   /* R3669  - EQ4_4 */ 
+	{ 0x00000E56, 0x00E0 },   /* R3670  - EQ4_5 */ 
+	{ 0x00000E57, 0x1EC4 },   /* R3671  - EQ4_6 */ 
+	{ 0x00000E58, 0xF136 },   /* R3672  - EQ4_7 */ 
+	{ 0x00000E59, 0x0409 },   /* R3673  - EQ4_8 */ 
+	{ 0x00000E5A, 0x04CC },   /* R3674  - EQ4_9 */ 
+	{ 0x00000E5B, 0x1C9B },   /* R3675  - EQ4_10 */ 
+	{ 0x00000E5C, 0xF337 },   /* R3676  - EQ4_11 */ 
+	{ 0x00000E5D, 0x040B },   /* R3677  - EQ4_12 */ 
+	{ 0x00000E5E, 0x0CBB },   /* R3678  - EQ4_13 */ 
+	{ 0x00000E5F, 0x16F8 },   /* R3679  - EQ4_14 */ 
+	{ 0x00000E60, 0xF7D9 },   /* R3680  - EQ4_15 */ 
+	{ 0x00000E61, 0x040A },   /* R3681  - EQ4_16 */ 
+	{ 0x00000E62, 0x1F14 },   /* R3682  - EQ4_17 */ 
+	{ 0x00000E63, 0x058C },   /* R3683  - EQ4_18 */ 
+	{ 0x00000E64, 0x0563 },   /* R3684  - EQ4_19 */ 
+	{ 0x00000E65, 0x4000 },   /* R3685  - EQ4_20 */ 
+	{ 0x00000E66, 0x0B75 },   /* R3686  - EQ4_21 */ 
+	{ 0x00000E80, 0x0018 },   /* R3712  - DRC1 ctrl1 */ 
+	{ 0x00000E81, 0x0933 },   /* R3713  - DRC1 ctrl2 */ 
+	{ 0x00000E82, 0x0018 },   /* R3714  - DRC1 ctrl3 */ 
+	{ 0x00000E83, 0x0000 },   /* R3715  - DRC1 ctrl4 */ 
+	{ 0x00000E84, 0x0000 },   /* R3716  - DRC1 ctrl5 */ 
+	{ 0x00000E89, 0x0018 },   /* R3721  - DRC2 ctrl1 */ 
+	{ 0x00000E8A, 0x0933 },   /* R3722  - DRC2 ctrl2 */ 
+	{ 0x00000E8B, 0x0018 },   /* R3723  - DRC2 ctrl3 */ 
+	{ 0x00000E8C, 0x0000 },   /* R3724  - DRC2 ctrl4 */ 
+	{ 0x00000E8D, 0x0000 },   /* R3725  - DRC2 ctrl5 */ 
+	{ 0x00000EC0, 0x0000 },   /* R3776  - HPLPF1_1 */ 
+	{ 0x00000EC1, 0x0000 },   /* R3777  - HPLPF1_2 */ 
+	{ 0x00000EC4, 0x0000 },   /* R3780  - HPLPF2_1 */ 
+	{ 0x00000EC5, 0x0000 },   /* R3781  - HPLPF2_2 */ 
+	{ 0x00000EC8, 0x0000 },   /* R3784  - HPLPF3_1 */ 
+	{ 0x00000EC9, 0x0000 },   /* R3785  - HPLPF3_2 */ 
+	{ 0x00000ECC, 0x0000 },   /* R3788  - HPLPF4_1 */ 
+	{ 0x00000ECD, 0x0000 },   /* R3789  - HPLPF4_2 */ 
+	{ 0x00000EE0, 0x0000 },   /* R3808  - ASRC_ENABLE */ 
+	{ 0x00000EE2, 0x0000 },   /* R3810  - ASRC_RATE1 */ 
+	{ 0x00000EE3, 0x4000 },   /* R3811  - ASRC_RATE2 */ 
+	{ 0x00000EF0, 0x0000 },   /* R3824  - ISRC 1 CTRL 1 */ 
+	{ 0x00000EF1, 0x0000 },   /* R3825  - ISRC 1 CTRL 2 */ 
+	{ 0x00000EF2, 0x0000 },   /* R3826  - ISRC 1 CTRL 3 */ 
+	{ 0x00000EF3, 0x0000 },   /* R3827  - ISRC 2 CTRL 1 */ 
+	{ 0x00000EF4, 0x0000 },   /* R3828  - ISRC 2 CTRL 2 */ 
+	{ 0x00000EF5, 0x0000 },   /* R3829  - ISRC 2 CTRL 3 */ 
+	{ 0x00000EF6, 0x0000 },   /* R3830  - ISRC 3 CTRL 1 */ 
+	{ 0x00000EF7, 0x0000 },   /* R3831  - ISRC 3 CTRL 2 */ 
+	{ 0x00000EF8, 0x0000 },   /* R3832  - ISRC 3 CTRL 3 */ 
+	{ 0x00001100, 0x0010 },   /* R4352  - DSP1 Control 1 */ 
+	{ 0x00001101, 0x0000 },   /* R4353  - DSP1 Clocking 1 */ 
+};
+
+static bool wm5102_readable_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ARIZONA_SOFTWARE_RESET:
+	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_CTRL_IF_SPI_CFG_1:
+	case ARIZONA_CTRL_IF_I2C1_CFG_1:
+	case ARIZONA_CTRL_IF_STATUS_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
+	case ARIZONA_WRITE_SEQUENCER_PROM:
+	case ARIZONA_TONE_GENERATOR_1:
+	case ARIZONA_TONE_GENERATOR_2:
+	case ARIZONA_TONE_GENERATOR_3:
+	case ARIZONA_TONE_GENERATOR_4:
+	case ARIZONA_TONE_GENERATOR_5:
+	case ARIZONA_PWM_DRIVE_1:
+	case ARIZONA_PWM_DRIVE_2:
+	case ARIZONA_PWM_DRIVE_3:
+	case ARIZONA_WAKE_CONTROL:
+	case ARIZONA_SEQUENCE_CONTROL:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_1:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_2:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_3:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_4:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_1:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_2:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_3:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_4:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_5:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_6:
+	case ARIZONA_COMFORT_NOISE_GENERATOR:
+	case ARIZONA_HAPTICS_CONTROL_1:
+	case ARIZONA_HAPTICS_CONTROL_2:
+	case ARIZONA_HAPTICS_PHASE_1_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_1_DURATION:
+	case ARIZONA_HAPTICS_PHASE_2_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_2_DURATION:
+	case ARIZONA_HAPTICS_PHASE_3_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_3_DURATION:
+	case ARIZONA_HAPTICS_STATUS:
+	case ARIZONA_CLOCK_32K_1:
+	case ARIZONA_SYSTEM_CLOCK_1:
+	case ARIZONA_SAMPLE_RATE_1:
+	case ARIZONA_SAMPLE_RATE_2:
+	case ARIZONA_SAMPLE_RATE_3:
+	case ARIZONA_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_SAMPLE_RATE_3_STATUS:
+	case ARIZONA_ASYNC_CLOCK_1:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_OUTPUT_SYSTEM_CLOCK:
+	case ARIZONA_OUTPUT_ASYNC_CLOCK:
+	case ARIZONA_RATE_ESTIMATOR_1:
+	case ARIZONA_RATE_ESTIMATOR_2:
+	case ARIZONA_RATE_ESTIMATOR_3:
+	case ARIZONA_RATE_ESTIMATOR_4:
+	case ARIZONA_RATE_ESTIMATOR_5:
+	case ARIZONA_FLL1_CONTROL_1:
+	case ARIZONA_FLL1_CONTROL_2:
+	case ARIZONA_FLL1_CONTROL_3:
+	case ARIZONA_FLL1_CONTROL_4:
+	case ARIZONA_FLL1_CONTROL_5:
+	case ARIZONA_FLL1_CONTROL_6:
+	case ARIZONA_FLL1_LOOP_FILTER_TEST_1:
+	case ARIZONA_FLL1_SYNCHRONISER_1:
+	case ARIZONA_FLL1_SYNCHRONISER_2:
+	case ARIZONA_FLL1_SYNCHRONISER_3:
+	case ARIZONA_FLL1_SYNCHRONISER_4:
+	case ARIZONA_FLL1_SYNCHRONISER_5:
+	case ARIZONA_FLL1_SYNCHRONISER_6:
+	case ARIZONA_FLL1_SPREAD_SPECTRUM:
+	case ARIZONA_FLL1_GPIO_CLOCK:
+	case ARIZONA_FLL2_CONTROL_1:
+	case ARIZONA_FLL2_CONTROL_2:
+	case ARIZONA_FLL2_CONTROL_3:
+	case ARIZONA_FLL2_CONTROL_4:
+	case ARIZONA_FLL2_CONTROL_5:
+	case ARIZONA_FLL2_CONTROL_6:
+	case ARIZONA_FLL2_LOOP_FILTER_TEST_1:
+	case ARIZONA_FLL2_SYNCHRONISER_1:
+	case ARIZONA_FLL2_SYNCHRONISER_2:
+	case ARIZONA_FLL2_SYNCHRONISER_3:
+	case ARIZONA_FLL2_SYNCHRONISER_4:
+	case ARIZONA_FLL2_SYNCHRONISER_5:
+	case ARIZONA_FLL2_SYNCHRONISER_6:
+	case ARIZONA_FLL2_SPREAD_SPECTRUM:
+	case ARIZONA_FLL2_GPIO_CLOCK:
+	case ARIZONA_MIC_CHARGE_PUMP_1:
+	case ARIZONA_LDO1_CONTROL_1:
+	case ARIZONA_LDO2_CONTROL_1:
+	case ARIZONA_MIC_BIAS_CTRL_1:
+	case ARIZONA_MIC_BIAS_CTRL_2:
+	case ARIZONA_MIC_BIAS_CTRL_3:
+	case ARIZONA_ACCESSORY_DETECT_MODE_1:
+	case ARIZONA_HEADPHONE_DETECT_1:
+	case ARIZONA_HEADPHONE_DETECT_2:
+	case ARIZONA_MIC_DETECT_1:
+	case ARIZONA_MIC_DETECT_2:
+	case ARIZONA_MIC_DETECT_3:
+	case ARIZONA_MIC_NOISE_MIX_CONTROL_1:
+	case ARIZONA_ISOLATION_CONTROL:
+	case ARIZONA_JACK_DETECT_ANALOGUE:
+	case ARIZONA_INPUT_ENABLES:
+	case ARIZONA_INPUT_RATE:
+	case ARIZONA_INPUT_VOLUME_RAMP:
+	case ARIZONA_IN1L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_1L:
+	case ARIZONA_DMIC1L_CONTROL:
+	case ARIZONA_IN1R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_1R:
+	case ARIZONA_DMIC1R_CONTROL:
+	case ARIZONA_IN2L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_2L:
+	case ARIZONA_DMIC2L_CONTROL:
+	case ARIZONA_IN2R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_2R:
+	case ARIZONA_DMIC2R_CONTROL:
+	case ARIZONA_IN3L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_3L:
+	case ARIZONA_DMIC3L_CONTROL:
+	case ARIZONA_IN3R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_3R:
+	case ARIZONA_DMIC3R_CONTROL:
+	case ARIZONA_OUTPUT_ENABLES_1:
+	case ARIZONA_OUTPUT_STATUS_1:
+	case ARIZONA_OUTPUT_RATE_1:
+	case ARIZONA_OUTPUT_VOLUME_RAMP:
+	case ARIZONA_OUTPUT_PATH_CONFIG_1L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_1L:
+	case ARIZONA_DAC_VOLUME_LIMIT_1L:
+	case ARIZONA_NOISE_GATE_SELECT_1L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_1R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_1R:
+	case ARIZONA_DAC_VOLUME_LIMIT_1R:
+	case ARIZONA_NOISE_GATE_SELECT_1R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_2L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_2L:
+	case ARIZONA_DAC_VOLUME_LIMIT_2L:
+	case ARIZONA_NOISE_GATE_SELECT_2L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_2R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_2R:
+	case ARIZONA_DAC_VOLUME_LIMIT_2R:
+	case ARIZONA_NOISE_GATE_SELECT_2R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_3L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_3L:
+	case ARIZONA_DAC_VOLUME_LIMIT_3L:
+	case ARIZONA_NOISE_GATE_SELECT_3L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_3R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_3R:
+	case ARIZONA_DAC_VOLUME_LIMIT_3R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_4L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_4L:
+	case ARIZONA_OUT_VOLUME_4L:
+	case ARIZONA_NOISE_GATE_SELECT_4L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_4R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_4R:
+	case ARIZONA_OUT_VOLUME_4R:
+	case ARIZONA_NOISE_GATE_SELECT_4R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_5L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_5L:
+	case ARIZONA_DAC_VOLUME_LIMIT_5L:
+	case ARIZONA_NOISE_GATE_SELECT_5L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_5R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_5R:
+	case ARIZONA_DAC_VOLUME_LIMIT_5R:
+	case ARIZONA_NOISE_GATE_SELECT_5R:
+	case ARIZONA_DAC_AEC_CONTROL_1:
+	case ARIZONA_NOISE_GATE_CONTROL:
+	case ARIZONA_PDM_SPK1_CTRL_1:
+	case ARIZONA_PDM_SPK1_CTRL_2:
+	case ARIZONA_DAC_COMP_1:
+	case ARIZONA_DAC_COMP_2:
+	case ARIZONA_DAC_COMP_3:
+	case ARIZONA_DAC_COMP_4:
+	case ARIZONA_AIF1_BCLK_CTRL:
+	case ARIZONA_AIF1_TX_PIN_CTRL:
+	case ARIZONA_AIF1_RX_PIN_CTRL:
+	case ARIZONA_AIF1_RATE_CTRL:
+	case ARIZONA_AIF1_FORMAT:
+	case ARIZONA_AIF1_TX_BCLK_RATE:
+	case ARIZONA_AIF1_RX_BCLK_RATE:
+	case ARIZONA_AIF1_FRAME_CTRL_1:
+	case ARIZONA_AIF1_FRAME_CTRL_2:
+	case ARIZONA_AIF1_FRAME_CTRL_3:
+	case ARIZONA_AIF1_FRAME_CTRL_4:
+	case ARIZONA_AIF1_FRAME_CTRL_5:
+	case ARIZONA_AIF1_FRAME_CTRL_6:
+	case ARIZONA_AIF1_FRAME_CTRL_7:
+	case ARIZONA_AIF1_FRAME_CTRL_8:
+	case ARIZONA_AIF1_FRAME_CTRL_9:
+	case ARIZONA_AIF1_FRAME_CTRL_10:
+	case ARIZONA_AIF1_FRAME_CTRL_11:
+	case ARIZONA_AIF1_FRAME_CTRL_12:
+	case ARIZONA_AIF1_FRAME_CTRL_13:
+	case ARIZONA_AIF1_FRAME_CTRL_14:
+	case ARIZONA_AIF1_FRAME_CTRL_15:
+	case ARIZONA_AIF1_FRAME_CTRL_16:
+	case ARIZONA_AIF1_FRAME_CTRL_17:
+	case ARIZONA_AIF1_FRAME_CTRL_18:
+	case ARIZONA_AIF1_TX_ENABLES:
+	case ARIZONA_AIF1_RX_ENABLES:
+	case ARIZONA_AIF1_FORCE_WRITE:
+	case ARIZONA_AIF2_BCLK_CTRL:
+	case ARIZONA_AIF2_TX_PIN_CTRL:
+	case ARIZONA_AIF2_RX_PIN_CTRL:
+	case ARIZONA_AIF2_RATE_CTRL:
+	case ARIZONA_AIF2_FORMAT:
+	case ARIZONA_AIF2_TX_BCLK_RATE:
+	case ARIZONA_AIF2_RX_BCLK_RATE:
+	case ARIZONA_AIF2_FRAME_CTRL_1:
+	case ARIZONA_AIF2_FRAME_CTRL_2:
+	case ARIZONA_AIF2_FRAME_CTRL_3:
+	case ARIZONA_AIF2_FRAME_CTRL_4:
+	case ARIZONA_AIF2_FRAME_CTRL_11:
+	case ARIZONA_AIF2_FRAME_CTRL_12:
+	case ARIZONA_AIF2_TX_ENABLES:
+	case ARIZONA_AIF2_RX_ENABLES:
+	case ARIZONA_AIF2_FORCE_WRITE:
+	case ARIZONA_AIF3_BCLK_CTRL:
+	case ARIZONA_AIF3_TX_PIN_CTRL:
+	case ARIZONA_AIF3_RX_PIN_CTRL:
+	case ARIZONA_AIF3_RATE_CTRL:
+	case ARIZONA_AIF3_FORMAT:
+	case ARIZONA_AIF3_TX_BCLK_RATE:
+	case ARIZONA_AIF3_RX_BCLK_RATE:
+	case ARIZONA_AIF3_FRAME_CTRL_1:
+	case ARIZONA_AIF3_FRAME_CTRL_2:
+	case ARIZONA_AIF3_FRAME_CTRL_3:
+	case ARIZONA_AIF3_FRAME_CTRL_4:
+	case ARIZONA_AIF3_FRAME_CTRL_11:
+	case ARIZONA_AIF3_FRAME_CTRL_12:
+	case ARIZONA_AIF3_TX_ENABLES:
+	case ARIZONA_AIF3_RX_ENABLES:
+	case ARIZONA_AIF3_FORCE_WRITE:
+	case ARIZONA_SLIMBUS_FRAMER_REF_GEAR:
+	case ARIZONA_SLIMBUS_RATES_1:
+	case ARIZONA_SLIMBUS_RATES_2:
+	case ARIZONA_SLIMBUS_RATES_3:
+	case ARIZONA_SLIMBUS_RATES_4:
+	case ARIZONA_SLIMBUS_RATES_5:
+	case ARIZONA_SLIMBUS_RATES_6:
+	case ARIZONA_SLIMBUS_RATES_7:
+	case ARIZONA_SLIMBUS_RATES_8:
+	case ARIZONA_SLIMBUS_RX_CHANNEL_ENABLE:
+	case ARIZONA_SLIMBUS_TX_CHANNEL_ENABLE:
+	case ARIZONA_SLIMBUS_RX_PORT_STATUS:
+	case ARIZONA_SLIMBUS_TX_PORT_STATUS:
+	case ARIZONA_PWM1MIX_INPUT_1_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_1_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_2_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_2_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_3_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_3_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_4_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_4_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_1_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_1_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_2_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_2_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_3_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_3_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_4_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_4_VOLUME:
+	case ARIZONA_MICMIX_INPUT_1_SOURCE:
+	case ARIZONA_MICMIX_INPUT_1_VOLUME:
+	case ARIZONA_MICMIX_INPUT_2_SOURCE:
+	case ARIZONA_MICMIX_INPUT_2_VOLUME:
+	case ARIZONA_MICMIX_INPUT_3_SOURCE:
+	case ARIZONA_MICMIX_INPUT_3_VOLUME:
+	case ARIZONA_MICMIX_INPUT_4_SOURCE:
+	case ARIZONA_MICMIX_INPUT_4_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_1_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_1_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_2_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_2_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_3_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_3_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_4_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP1AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_GPIO1_CTRL:
+	case ARIZONA_GPIO2_CTRL:
+	case ARIZONA_GPIO3_CTRL:
+	case ARIZONA_GPIO4_CTRL:
+	case ARIZONA_GPIO5_CTRL:
+	case ARIZONA_IRQ_CTRL_1:
+	case ARIZONA_GPIO_DEBOUNCE_CONFIG:
+	case ARIZONA_MISC_PAD_CTRL_1:
+	case ARIZONA_MISC_PAD_CTRL_2:
+	case ARIZONA_MISC_PAD_CTRL_3:
+	case ARIZONA_MISC_PAD_CTRL_4:
+	case ARIZONA_MISC_PAD_CTRL_5:
+	case ARIZONA_MISC_PAD_CTRL_6:
+	case ARIZONA_INTERRUPT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_2:
+	case ARIZONA_INTERRUPT_STATUS_3:
+	case ARIZONA_INTERRUPT_STATUS_4:
+	case ARIZONA_INTERRUPT_STATUS_5:
+	case ARIZONA_INTERRUPT_STATUS_1_MASK:
+	case ARIZONA_INTERRUPT_STATUS_2_MASK:
+	case ARIZONA_INTERRUPT_STATUS_3_MASK:
+	case ARIZONA_INTERRUPT_STATUS_4_MASK:
+	case ARIZONA_INTERRUPT_STATUS_5_MASK:
+	case ARIZONA_INTERRUPT_CONTROL:
+	case ARIZONA_IRQ2_STATUS_1:
+	case ARIZONA_IRQ2_STATUS_2:
+	case ARIZONA_IRQ2_STATUS_3:
+	case ARIZONA_IRQ2_STATUS_4:
+	case ARIZONA_IRQ2_STATUS_5:
+	case ARIZONA_IRQ2_STATUS_1_MASK:
+	case ARIZONA_IRQ2_STATUS_2_MASK:
+	case ARIZONA_IRQ2_STATUS_3_MASK:
+	case ARIZONA_IRQ2_STATUS_4_MASK:
+	case ARIZONA_IRQ2_STATUS_5_MASK:
+	case ARIZONA_IRQ2_CONTROL:
+	case ARIZONA_INTERRUPT_RAW_STATUS_2:
+	case ARIZONA_INTERRUPT_RAW_STATUS_3:
+	case ARIZONA_INTERRUPT_RAW_STATUS_4:
+	case ARIZONA_INTERRUPT_RAW_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_7:
+	case ARIZONA_INTERRUPT_RAW_STATUS_8:
+	case ARIZONA_IRQ_PIN_STATUS:
+	case ARIZONA_ADSP2_IRQ0:
+	case ARIZONA_AOD_WKUP_AND_TRIG:
+	case ARIZONA_AOD_IRQ1:
+	case ARIZONA_AOD_IRQ2:
+	case ARIZONA_AOD_IRQ_MASK_IRQ1:
+	case ARIZONA_AOD_IRQ_MASK_IRQ2:
+	case ARIZONA_AOD_IRQ_RAW_STATUS:
+	case ARIZONA_JACK_DETECT_DEBOUNCE:
+	case ARIZONA_FX_CTRL1:
+	case ARIZONA_FX_CTRL2:
+	case ARIZONA_EQ1_1:
+	case ARIZONA_EQ1_2:
+	case ARIZONA_EQ1_3:
+	case ARIZONA_EQ1_4:
+	case ARIZONA_EQ1_5:
+	case ARIZONA_EQ1_6:
+	case ARIZONA_EQ1_7:
+	case ARIZONA_EQ1_8:
+	case ARIZONA_EQ1_9:
+	case ARIZONA_EQ1_10:
+	case ARIZONA_EQ1_11:
+	case ARIZONA_EQ1_12:
+	case ARIZONA_EQ1_13:
+	case ARIZONA_EQ1_14:
+	case ARIZONA_EQ1_15:
+	case ARIZONA_EQ1_16:
+	case ARIZONA_EQ1_17:
+	case ARIZONA_EQ1_18:
+	case ARIZONA_EQ1_19:
+	case ARIZONA_EQ1_20:
+	case ARIZONA_EQ1_21:
+	case ARIZONA_EQ2_1:
+	case ARIZONA_EQ2_2:
+	case ARIZONA_EQ2_3:
+	case ARIZONA_EQ2_4:
+	case ARIZONA_EQ2_5:
+	case ARIZONA_EQ2_6:
+	case ARIZONA_EQ2_7:
+	case ARIZONA_EQ2_8:
+	case ARIZONA_EQ2_9:
+	case ARIZONA_EQ2_10:
+	case ARIZONA_EQ2_11:
+	case ARIZONA_EQ2_12:
+	case ARIZONA_EQ2_13:
+	case ARIZONA_EQ2_14:
+	case ARIZONA_EQ2_15:
+	case ARIZONA_EQ2_16:
+	case ARIZONA_EQ2_17:
+	case ARIZONA_EQ2_18:
+	case ARIZONA_EQ2_19:
+	case ARIZONA_EQ2_20:
+	case ARIZONA_EQ2_21:
+	case ARIZONA_EQ3_1:
+	case ARIZONA_EQ3_2:
+	case ARIZONA_EQ3_3:
+	case ARIZONA_EQ3_4:
+	case ARIZONA_EQ3_5:
+	case ARIZONA_EQ3_6:
+	case ARIZONA_EQ3_7:
+	case ARIZONA_EQ3_8:
+	case ARIZONA_EQ3_9:
+	case ARIZONA_EQ3_10:
+	case ARIZONA_EQ3_11:
+	case ARIZONA_EQ3_12:
+	case ARIZONA_EQ3_13:
+	case ARIZONA_EQ3_14:
+	case ARIZONA_EQ3_15:
+	case ARIZONA_EQ3_16:
+	case ARIZONA_EQ3_17:
+	case ARIZONA_EQ3_18:
+	case ARIZONA_EQ3_19:
+	case ARIZONA_EQ3_20:
+	case ARIZONA_EQ3_21:
+	case ARIZONA_EQ4_1:
+	case ARIZONA_EQ4_2:
+	case ARIZONA_EQ4_3:
+	case ARIZONA_EQ4_4:
+	case ARIZONA_EQ4_5:
+	case ARIZONA_EQ4_6:
+	case ARIZONA_EQ4_7:
+	case ARIZONA_EQ4_8:
+	case ARIZONA_EQ4_9:
+	case ARIZONA_EQ4_10:
+	case ARIZONA_EQ4_11:
+	case ARIZONA_EQ4_12:
+	case ARIZONA_EQ4_13:
+	case ARIZONA_EQ4_14:
+	case ARIZONA_EQ4_15:
+	case ARIZONA_EQ4_16:
+	case ARIZONA_EQ4_17:
+	case ARIZONA_EQ4_18:
+	case ARIZONA_EQ4_19:
+	case ARIZONA_EQ4_20:
+	case ARIZONA_EQ4_21:
+	case ARIZONA_DRC1_CTRL1:
+	case ARIZONA_DRC1_CTRL2:
+	case ARIZONA_DRC1_CTRL3:
+	case ARIZONA_DRC1_CTRL4:
+	case ARIZONA_DRC1_CTRL5:
+	case ARIZONA_DRC2_CTRL1:
+	case ARIZONA_DRC2_CTRL2:
+	case ARIZONA_DRC2_CTRL3:
+	case ARIZONA_DRC2_CTRL4:
+	case ARIZONA_DRC2_CTRL5:
+	case ARIZONA_HPLPF1_1:
+	case ARIZONA_HPLPF1_2:
+	case ARIZONA_HPLPF2_1:
+	case ARIZONA_HPLPF2_2:
+	case ARIZONA_HPLPF3_1:
+	case ARIZONA_HPLPF3_2:
+	case ARIZONA_HPLPF4_1:
+	case ARIZONA_HPLPF4_2:
+	case ARIZONA_ASRC_ENABLE:
+	case ARIZONA_ASRC_RATE1:
+	case ARIZONA_ASRC_RATE2:
+	case ARIZONA_ISRC_1_CTRL_1:
+	case ARIZONA_ISRC_1_CTRL_2:
+	case ARIZONA_ISRC_1_CTRL_3:
+	case ARIZONA_ISRC_2_CTRL_1:
+	case ARIZONA_ISRC_2_CTRL_2:
+	case ARIZONA_ISRC_2_CTRL_3:
+	case ARIZONA_ISRC_3_CTRL_1:
+	case ARIZONA_ISRC_3_CTRL_2:
+	case ARIZONA_ISRC_3_CTRL_3:
+	case ARIZONA_DSP1_CONTROL_1:
+	case ARIZONA_DSP1_CLOCKING_1:
+	case ARIZONA_DSP1_STATUS_1:
+	case ARIZONA_DSP1_STATUS_2:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool wm5102_volatile_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ARIZONA_SOFTWARE_RESET:
+	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_OUTPUT_STATUS_1:
+	case ARIZONA_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_SAMPLE_RATE_3_STATUS:
+	case ARIZONA_HAPTICS_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_FX_CTRL2:
+	case ARIZONA_INTERRUPT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_2:
+	case ARIZONA_INTERRUPT_STATUS_3:
+	case ARIZONA_INTERRUPT_STATUS_4:
+	case ARIZONA_INTERRUPT_STATUS_5:
+	case ARIZONA_IRQ2_STATUS_1:
+	case ARIZONA_IRQ2_STATUS_2:
+	case ARIZONA_IRQ2_STATUS_3:
+	case ARIZONA_IRQ2_STATUS_4:
+	case ARIZONA_IRQ2_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_2:
+	case ARIZONA_INTERRUPT_RAW_STATUS_3:
+	case ARIZONA_INTERRUPT_RAW_STATUS_4:
+	case ARIZONA_INTERRUPT_RAW_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_7:
+	case ARIZONA_INTERRUPT_RAW_STATUS_8:
+	case ARIZONA_IRQ_PIN_STATUS:
+	case ARIZONA_AOD_WKUP_AND_TRIG:
+	case ARIZONA_AOD_IRQ1:
+	case ARIZONA_AOD_IRQ2:
+	case ARIZONA_AOD_IRQ_RAW_STATUS:
+	case ARIZONA_DSP1_STATUS_1:
+	case ARIZONA_DSP1_STATUS_2:
+	case ARIZONA_HEADPHONE_DETECT_2:
+	case ARIZONA_MIC_DETECT_3:
+		return true;
+	default:
+		return false;
+	}
+}
+
+const struct regmap_config wm5102_spi_regmap = {
+	.reg_bits = 32,
+	.pad_bits = 16,
+	.val_bits = 16,
+
+	.max_register = ARIZONA_DSP1_STATUS_2,
+	.readable_reg = wm5102_readable_register,
+	.volatile_reg = wm5102_volatile_register,
+
+	.cache_type = REGCACHE_RBTREE,
+	.reg_defaults = wm5102_reg_default,
+	.num_reg_defaults = ARRAY_SIZE(wm5102_reg_default),
+};
+EXPORT_SYMBOL_GPL(wm5102_spi_regmap);
+
+const struct regmap_config wm5102_i2c_regmap = {
+	.reg_bits = 32,
+	.val_bits = 16,
+
+	.max_register = ARIZONA_DSP1_STATUS_2,
+	.readable_reg = wm5102_readable_register,
+	.volatile_reg = wm5102_volatile_register,
+
+	.cache_type = REGCACHE_RBTREE,
+	.reg_defaults = wm5102_reg_default,
+	.num_reg_defaults = ARRAY_SIZE(wm5102_reg_default),
+};
+EXPORT_SYMBOL_GPL(wm5102_i2c_regmap);
diff --git a/drivers/mfd/wm5110-tables.c b/drivers/mfd/wm5110-tables.c
new file mode 100644
index 00000000000..bd8782c8896
--- /dev/null
+++ b/drivers/mfd/wm5110-tables.c
@@ -0,0 +1,2281 @@
+/*
+ * wm5110-tables.c  --  WM5110 data tables
+ *
+ * Copyright 2012 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/registers.h>
+
+#include "arizona.h"
+
+#define WM5110_NUM_AOD_ISR 2
+#define WM5110_NUM_ISR 5
+
+static const struct reg_default wm5110_reva_patch[] = {
+	{ 0x80, 0x3 },
+	{ 0x44, 0x20 },
+	{ 0x45, 0x40 },
+	{ 0x46, 0x60 },
+	{ 0x47, 0x80 },
+	{ 0x48, 0xa0 },
+	{ 0x51, 0x13 },
+	{ 0x52, 0x33 },
+	{ 0x53, 0x53 },
+	{ 0x54, 0x73 },
+	{ 0x55, 0x75 },
+	{ 0x56, 0xb3 },
+	{ 0x2ef, 0x124 },
+	{ 0x2ef, 0x124 },
+	{ 0x2f0, 0x124 },
+	{ 0x2f0, 0x124 },
+	{ 0x2f1, 0x124 },
+	{ 0x2f1, 0x124 },
+	{ 0x2f2, 0x124 },
+	{ 0x2f2, 0x124 },
+	{ 0x2f3, 0x124 },
+	{ 0x2f3, 0x124 },
+	{ 0x2f4, 0x124 },
+	{ 0x2f4, 0x124 },
+	{ 0x2eb, 0x60 },
+	{ 0x2ec, 0x60 },
+	{ 0x2ed, 0x60 },
+	{ 0xc30, 0x3e3e },
+	{ 0xc30, 0x3e3e },
+	{ 0xc31, 0x3e },
+	{ 0xc32, 0x3e3e },
+	{ 0xc32, 0x3e3e },
+	{ 0xc33, 0x3e3e },
+	{ 0xc33, 0x3e3e },
+	{ 0xc34, 0x3e3e },
+	{ 0xc34, 0x3e3e },
+	{ 0xc35, 0x3e3e },
+	{ 0xc35, 0x3e3e },
+	{ 0xc36, 0x3e3e },
+	{ 0xc36, 0x3e3e },
+	{ 0xc37, 0x3e3e },
+	{ 0xc37, 0x3e3e },
+	{ 0xc38, 0x3e3e },
+	{ 0xc38, 0x3e3e },
+	{ 0xc30, 0x3e3e },
+	{ 0xc30, 0x3e3e },
+	{ 0xc39, 0x3e3e },
+	{ 0xc39, 0x3e3e },
+	{ 0xc3a, 0x3e3e },
+	{ 0xc3a, 0x3e3e },
+	{ 0xc3b, 0x3e3e },
+	{ 0xc3b, 0x3e3e },
+	{ 0xc3c, 0x3e },
+	{ 0x201, 0x18a5 },
+	{ 0x201, 0x18a5 },
+	{ 0x201, 0x18a5 },
+	{ 0x202, 0x4100 },
+	{ 0x460, 0xc00 },
+	{ 0x461, 0x8000 },
+	{ 0x462, 0xc01 },
+	{ 0x463, 0x50f0 },
+	{ 0x464, 0xc01 },
+	{ 0x465, 0x4820 },
+	{ 0x466, 0xc01 },
+	{ 0x466, 0xc01 },
+	{ 0x467, 0x4040 },
+	{ 0x468, 0xc01 },
+	{ 0x468, 0xc01 },
+	{ 0x469, 0x3940 },
+	{ 0x46a, 0xc01 },
+	{ 0x46a, 0xc01 },
+	{ 0x46a, 0xc01 },
+	{ 0x46b, 0x3310 },
+	{ 0x46c, 0x801 },
+	{ 0x46c, 0x801 },
+	{ 0x46d, 0x2d80 },
+	{ 0x46e, 0x801 },
+	{ 0x46e, 0x801 },
+	{ 0x46f, 0x2890 },
+	{ 0x470, 0x801 },
+	{ 0x470, 0x801 },
+	{ 0x471, 0x1990 },
+	{ 0x472, 0x801 },
+	{ 0x472, 0x801 },
+	{ 0x473, 0x1450 },
+	{ 0x474, 0x801 },
+	{ 0x474, 0x801 },
+	{ 0x474, 0x801 },
+	{ 0x475, 0x1020 },
+	{ 0x476, 0x801 },
+	{ 0x476, 0x801 },
+	{ 0x476, 0x801 },
+	{ 0x477, 0xcd0 },
+	{ 0x478, 0x806 },
+	{ 0x478, 0x806 },
+	{ 0x479, 0xa30 },
+	{ 0x47a, 0x806 },
+	{ 0x47a, 0x806 },
+	{ 0x47b, 0x810 },
+	{ 0x47c, 0x80e },
+	{ 0x47c, 0x80e },
+	{ 0x47d, 0x510 },
+	{ 0x47e, 0x81f },
+	{ 0x47e, 0x81f },
+	{ 0x2DB, 0x0A00 },
+	{ 0x2DD, 0x0023 },
+	{ 0x2DF, 0x0102 },
+	{ 0x80, 0x0 },
+	{ 0xC20, 0x0002 },
+	{ 0x209, 0x002A },
+};
+
+/* We use a function so we can use ARRAY_SIZE() */
+int wm5110_patch(struct arizona *arizona)
+{
+	switch (arizona->rev) {
+	case 0:
+	case 1:
+		return regmap_register_patch(arizona->regmap,
+					     wm5110_reva_patch,
+					     ARRAY_SIZE(wm5110_reva_patch));
+	default:
+		return 0;
+	}
+}
+EXPORT_SYMBOL_GPL(wm5110_patch);
+
+static const struct regmap_irq wm5110_aod_irqs[ARIZONA_NUM_IRQ] = {
+	[ARIZONA_IRQ_GP5_FALL] = { .mask = ARIZONA_GP5_FALL_EINT1 },
+	[ARIZONA_IRQ_GP5_RISE] = { .mask = ARIZONA_GP5_RISE_EINT1 },
+	[ARIZONA_IRQ_JD_FALL] = { .mask = ARIZONA_JD1_FALL_EINT1 },
+	[ARIZONA_IRQ_JD_RISE] = { .mask = ARIZONA_JD1_RISE_EINT1 },
+};
+
+const struct regmap_irq_chip wm5110_aod = {
+	.name = "wm5110 AOD",
+	.status_base = ARIZONA_AOD_IRQ1,
+	.mask_base = ARIZONA_AOD_IRQ_MASK_IRQ1,
+	.ack_base = ARIZONA_AOD_IRQ1,
+	.wake_base = ARIZONA_WAKE_CONTROL,
+	.num_regs = 1,
+	.irqs = wm5110_aod_irqs,
+	.num_irqs = ARRAY_SIZE(wm5110_aod_irqs),
+};
+EXPORT_SYMBOL_GPL(wm5110_aod);
+
+static const struct regmap_irq wm5110_irqs[ARIZONA_NUM_IRQ] = {
+	[ARIZONA_IRQ_GP4] = { .reg_offset = 0, .mask = ARIZONA_GP4_EINT1 },
+	[ARIZONA_IRQ_GP3] = { .reg_offset = 0, .mask = ARIZONA_GP3_EINT1 },
+	[ARIZONA_IRQ_GP2] = { .reg_offset = 0, .mask = ARIZONA_GP2_EINT1 },
+	[ARIZONA_IRQ_GP1] = { .reg_offset = 0, .mask = ARIZONA_GP1_EINT1 },
+
+	[ARIZONA_IRQ_DSP4_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP4_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP3_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP3_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP2_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP2_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP1_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP1_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ8] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ8_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ7] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ7_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ6] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ6_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ5] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ5_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ4] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ4_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ3] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ3_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ2] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ2_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ1] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ1_EINT1
+	},
+
+	[ARIZONA_IRQ_SPK_SHUTDOWN_WARN] = {
+		.reg_offset = 2, .mask = ARIZONA_SPK_SHUTDOWN_WARN_EINT1
+	},
+	[ARIZONA_IRQ_SPK_SHUTDOWN] = {
+		.reg_offset = 2, .mask = ARIZONA_SPK_SHUTDOWN_EINT1
+	},
+	[ARIZONA_IRQ_HPDET] = {
+		.reg_offset = 2, .mask = ARIZONA_HPDET_EINT1
+	},
+	[ARIZONA_IRQ_MICDET] = {
+		.reg_offset = 2, .mask = ARIZONA_MICDET_EINT1
+	},
+	[ARIZONA_IRQ_WSEQ_DONE] = {
+		.reg_offset = 2, .mask = ARIZONA_WSEQ_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DRC2_SIG_DET] = {
+		.reg_offset = 2, .mask = ARIZONA_DRC2_SIG_DET_EINT1
+	},
+	[ARIZONA_IRQ_DRC1_SIG_DET] = {
+		.reg_offset = 2, .mask = ARIZONA_DRC1_SIG_DET_EINT1
+	},
+	[ARIZONA_IRQ_ASRC2_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_ASRC2_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_ASRC1_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_ASRC1_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_UNDERCLOCKED] = {
+		.reg_offset = 2, .mask = ARIZONA_UNDERCLOCKED_EINT1
+	},
+	[ARIZONA_IRQ_OVERCLOCKED] = {
+		.reg_offset = 2, .mask = ARIZONA_OVERCLOCKED_EINT1
+	},
+	[ARIZONA_IRQ_FLL2_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_FLL2_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_FLL1_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_FLL1_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_CLKGEN_ERR] = {
+		.reg_offset = 2, .mask = ARIZONA_CLKGEN_ERR_EINT1
+	},
+	[ARIZONA_IRQ_CLKGEN_ERR_ASYNC] = {
+		.reg_offset = 2, .mask = ARIZONA_CLKGEN_ERR_ASYNC_EINT1
+	},
+
+	[ARIZONA_IRQ_ASRC_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_ASRC_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_AIF3_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_AIF3_ERR_EINT1
+	},
+	[ARIZONA_IRQ_AIF2_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_AIF2_ERR_EINT1
+	},
+	[ARIZONA_IRQ_AIF1_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_AIF1_ERR_EINT1
+	},
+	[ARIZONA_IRQ_CTRLIF_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_CTRLIF_ERR_EINT1
+	},
+	[ARIZONA_IRQ_MIXER_DROPPED_SAMPLES] = {
+		.reg_offset = 3, .mask = ARIZONA_MIXER_DROPPED_SAMPLE_EINT1
+	},
+	[ARIZONA_IRQ_ASYNC_CLK_ENA_LOW] = {
+		.reg_offset = 3, .mask = ARIZONA_ASYNC_CLK_ENA_LOW_EINT1
+	},
+	[ARIZONA_IRQ_SYSCLK_ENA_LOW] = {
+		.reg_offset = 3, .mask = ARIZONA_SYSCLK_ENA_LOW_EINT1
+	},
+	[ARIZONA_IRQ_ISRC1_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_ISRC1_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_ISRC2_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_ISRC2_CFG_ERR_EINT1
+	},
+
+	[ARIZONA_IRQ_BOOT_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_BOOT_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DCS_DAC_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_DCS_DAC_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DCS_HP_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_DCS_HP_DONE_EINT1
+	},
+	[ARIZONA_IRQ_FLL2_CLOCK_OK] = {
+		.reg_offset = 4, .mask = ARIZONA_FLL2_CLOCK_OK_EINT1
+	},
+	[ARIZONA_IRQ_FLL1_CLOCK_OK] = {
+		.reg_offset = 4, .mask = ARIZONA_FLL1_CLOCK_OK_EINT1
+	},
+};
+
+const struct regmap_irq_chip wm5110_irq = {
+	.name = "wm5110 IRQ",
+	.status_base = ARIZONA_INTERRUPT_STATUS_1,
+	.mask_base = ARIZONA_INTERRUPT_STATUS_1_MASK,
+	.ack_base = ARIZONA_INTERRUPT_STATUS_1,
+	.num_regs = 5,
+	.irqs = wm5110_irqs,
+	.num_irqs = ARRAY_SIZE(wm5110_irqs),
+};
+EXPORT_SYMBOL_GPL(wm5110_irq);
+
+static const struct reg_default wm5110_reg_default[] = {
+	{ 0x00000008, 0x0019 },    /* R8     - Ctrl IF SPI CFG 1 */
+	{ 0x00000009, 0x0001 },    /* R9     - Ctrl IF I2C1 CFG 1 */
+	{ 0x0000000A, 0x0001 },    /* R10    - Ctrl IF I2C2 CFG 1 */
+	{ 0x0000000B, 0x0036 },    /* R11    - Ctrl IF I2C1 CFG 2 */
+	{ 0x0000000C, 0x0036 },    /* R12    - Ctrl IF I2C2 CFG 2 */
+	{ 0x00000016, 0x0000 },    /* R22    - Write Sequencer Ctrl 0 */
+	{ 0x00000017, 0x0000 },    /* R23    - Write Sequencer Ctrl 1 */
+	{ 0x00000018, 0x0000 },    /* R24    - Write Sequencer Ctrl 2 */
+	{ 0x00000020, 0x0000 },    /* R32    - Tone Generator 1 */
+	{ 0x00000021, 0x1000 },    /* R33    - Tone Generator 2 */
+	{ 0x00000022, 0x0000 },    /* R34    - Tone Generator 3 */
+	{ 0x00000023, 0x1000 },    /* R35    - Tone Generator 4 */
+	{ 0x00000024, 0x0000 },    /* R36    - Tone Generator 5 */
+	{ 0x00000030, 0x0000 },    /* R48    - PWM Drive 1 */
+	{ 0x00000031, 0x0100 },    /* R49    - PWM Drive 2 */
+	{ 0x00000032, 0x0100 },    /* R50    - PWM Drive 3 */
+	{ 0x00000040, 0x0000 },    /* R64    - Wake control */
+	{ 0x00000041, 0x0000 },    /* R65    - Sequence control */
+	{ 0x00000061, 0x01FF },    /* R97    - Sample Rate Sequence Select 1 */
+	{ 0x00000062, 0x01FF },    /* R98    - Sample Rate Sequence Select 2 */
+	{ 0x00000063, 0x01FF },    /* R99    - Sample Rate Sequence Select 3 */
+	{ 0x00000064, 0x01FF },    /* R100   - Sample Rate Sequence Select 4 */
+	{ 0x00000068, 0x01FF },    /* R104   - Always On Triggers Sequence Select 1 */
+	{ 0x00000069, 0x01FF },    /* R105   - Always On Triggers Sequence Select 2 */
+	{ 0x0000006A, 0x01FF },    /* R106   - Always On Triggers Sequence Select 3 */
+	{ 0x0000006B, 0x01FF },    /* R107   - Always On Triggers Sequence Select 4 */
+	{ 0x00000070, 0x0000 },    /* R112   - Comfort Noise Generator */
+	{ 0x00000090, 0x0000 },    /* R144   - Haptics Control 1 */
+	{ 0x00000091, 0x7FFF },    /* R145   - Haptics Control 2 */
+	{ 0x00000092, 0x0000 },    /* R146   - Haptics phase 1 intensity */
+	{ 0x00000093, 0x0000 },    /* R147   - Haptics phase 1 duration */
+	{ 0x00000094, 0x0000 },    /* R148   - Haptics phase 2 intensity */
+	{ 0x00000095, 0x0000 },    /* R149   - Haptics phase 2 duration */
+	{ 0x00000096, 0x0000 },    /* R150   - Haptics phase 3 intensity */
+	{ 0x00000097, 0x0000 },    /* R151   - Haptics phase 3 duration */
+	{ 0x00000100, 0x0001 },    /* R256   - Clock 32k 1 */
+	{ 0x00000101, 0x0504 },    /* R257   - System Clock 1 */
+	{ 0x00000102, 0x0011 },    /* R258   - Sample rate 1 */
+	{ 0x00000103, 0x0011 },    /* R259   - Sample rate 2 */
+	{ 0x00000104, 0x0011 },    /* R260   - Sample rate 3 */
+	{ 0x00000112, 0x0305 },    /* R274   - Async clock 1 */
+	{ 0x00000113, 0x0011 },    /* R275   - Async sample rate 1 */
+	{ 0x00000149, 0x0000 },    /* R329   - Output system clock */
+	{ 0x0000014A, 0x0000 },    /* R330   - Output async clock */
+	{ 0x00000152, 0x0000 },    /* R338   - Rate Estimator 1 */
+	{ 0x00000153, 0x0000 },    /* R339   - Rate Estimator 2 */
+	{ 0x00000154, 0x0000 },    /* R340   - Rate Estimator 3 */
+	{ 0x00000155, 0x0000 },    /* R341   - Rate Estimator 4 */
+	{ 0x00000156, 0x0000 },    /* R342   - Rate Estimator 5 */
+	{ 0x00000171, 0x0000 },    /* R369   - FLL1 Control 1 */
+	{ 0x00000172, 0x0008 },    /* R370   - FLL1 Control 2 */
+	{ 0x00000173, 0x0018 },    /* R371   - FLL1 Control 3 */
+	{ 0x00000174, 0x007D },    /* R372   - FLL1 Control 4 */
+	{ 0x00000175, 0x0006 },    /* R373   - FLL1 Control 5 */
+	{ 0x00000176, 0x0000 },    /* R374   - FLL1 Control 6 */
+	{ 0x00000177, 0x0281 },    /* R375   - FLL1 Loop Filter Test 1 */
+	{ 0x00000178, 0x0000 },    /* R376   - FLL1 NCO Test 0 */
+	{ 0x00000181, 0x0000 },    /* R385   - FLL1 Synchroniser 1 */
+	{ 0x00000182, 0x0000 },    /* R386   - FLL1 Synchroniser 2 */
+	{ 0x00000183, 0x0000 },    /* R387   - FLL1 Synchroniser 3 */
+	{ 0x00000184, 0x0000 },    /* R388   - FLL1 Synchroniser 4 */
+	{ 0x00000185, 0x0000 },    /* R389   - FLL1 Synchroniser 5 */
+	{ 0x00000186, 0x0000 },    /* R390   - FLL1 Synchroniser 6 */
+	{ 0x00000189, 0x0000 },    /* R393   - FLL1 Spread Spectrum */
+	{ 0x0000018A, 0x0004 },    /* R394   - FLL1 GPIO Clock */
+	{ 0x00000191, 0x0000 },    /* R401   - FLL2 Control 1 */
+	{ 0x00000192, 0x0008 },    /* R402   - FLL2 Control 2 */
+	{ 0x00000193, 0x0018 },    /* R403   - FLL2 Control 3 */
+	{ 0x00000194, 0x007D },    /* R404   - FLL2 Control 4 */
+	{ 0x00000195, 0x000C },    /* R405   - FLL2 Control 5 */
+	{ 0x00000196, 0x0000 },    /* R406   - FLL2 Control 6 */
+	{ 0x00000197, 0x0000 },    /* R407   - FLL2 Loop Filter Test 1 */
+	{ 0x00000198, 0x0000 },    /* R408   - FLL2 NCO Test 0 */
+	{ 0x000001A1, 0x0000 },    /* R417   - FLL2 Synchroniser 1 */
+	{ 0x000001A2, 0x0000 },    /* R418   - FLL2 Synchroniser 2 */
+	{ 0x000001A3, 0x0000 },    /* R419   - FLL2 Synchroniser 3 */
+	{ 0x000001A4, 0x0000 },    /* R420   - FLL2 Synchroniser 4 */
+	{ 0x000001A5, 0x0000 },    /* R421   - FLL2 Synchroniser 5 */
+	{ 0x000001A6, 0x0000 },    /* R422   - FLL2 Synchroniser 6 */
+	{ 0x000001A9, 0x0000 },    /* R425   - FLL2 Spread Spectrum */
+	{ 0x000001AA, 0x0004 },    /* R426   - FLL2 GPIO Clock */
+	{ 0x00000200, 0x0006 },    /* R512   - Mic Charge Pump 1 */
+	{ 0x00000210, 0x0184 },    /* R528   - LDO1 Control 1 */
+	{ 0x00000213, 0x0344 },    /* R531   - LDO2 Control 1 */
+	{ 0x00000218, 0x01A6 },    /* R536   - Mic Bias Ctrl 1 */
+	{ 0x00000219, 0x01A6 },    /* R537   - Mic Bias Ctrl 2 */
+	{ 0x0000021A, 0x01A6 },    /* R538   - Mic Bias Ctrl 3 */
+	{ 0x00000293, 0x0000 },    /* R659   - Accessory Detect Mode 1 */
+	{ 0x0000029B, 0x0020 },    /* R667   - Headphone Detect 1 */
+	{ 0x0000029C, 0x0000 },    /* R668   - Headphone Detect 2 */
+	{ 0x000002A3, 0x1102 },    /* R675   - Mic Detect 1 */
+	{ 0x000002A4, 0x009F },    /* R676   - Mic Detect 2 */
+	{ 0x000002C3, 0x0000 },    /* R707   - Mic noise mix control 1 */
+	{ 0x000002D3, 0x0000 },    /* R723   - Jack detect analogue */
+	{ 0x00000300, 0x0000 },    /* R768   - Input Enables */
+	{ 0x00000308, 0x0000 },    /* R776   - Input Rate */
+	{ 0x00000309, 0x0022 },    /* R777   - Input Volume Ramp */
+	{ 0x00000310, 0x2080 },    /* R784   - IN1L Control */
+	{ 0x00000311, 0x0180 },    /* R785   - ADC Digital Volume 1L */
+	{ 0x00000312, 0x0000 },    /* R786   - DMIC1L Control */
+	{ 0x00000314, 0x0080 },    /* R788   - IN1R Control */
+	{ 0x00000315, 0x0180 },    /* R789   - ADC Digital Volume 1R */
+	{ 0x00000316, 0x0000 },    /* R790   - DMIC1R Control */
+	{ 0x00000318, 0x2080 },    /* R792   - IN2L Control */
+	{ 0x00000319, 0x0180 },    /* R793   - ADC Digital Volume 2L */
+	{ 0x0000031A, 0x0000 },    /* R794   - DMIC2L Control */
+	{ 0x0000031C, 0x0080 },    /* R796   - IN2R Control */
+	{ 0x0000031D, 0x0180 },    /* R797   - ADC Digital Volume 2R */
+	{ 0x0000031E, 0x0000 },    /* R798   - DMIC2R Control */
+	{ 0x00000320, 0x2080 },    /* R800   - IN3L Control */
+	{ 0x00000321, 0x0180 },    /* R801   - ADC Digital Volume 3L */
+	{ 0x00000322, 0x0000 },    /* R802   - DMIC3L Control */
+	{ 0x00000324, 0x0080 },    /* R804   - IN3R Control */
+	{ 0x00000325, 0x0180 },    /* R805   - ADC Digital Volume 3R */
+	{ 0x00000326, 0x0000 },    /* R806   - DMIC3R Control */
+	{ 0x00000328, 0x2000 },    /* R808   - IN4L Control */
+	{ 0x00000329, 0x0180 },    /* R809   - ADC Digital Volume 4L */
+	{ 0x0000032A, 0x0000 },    /* R810   - DMIC4L Control */
+	{ 0x0000032D, 0x0180 },    /* R813   - ADC Digital Volume 4R */
+	{ 0x0000032E, 0x0000 },    /* R814   - DMIC4R Control */
+	{ 0x00000400, 0x0000 },    /* R1024  - Output Enables 1 */
+	{ 0x00000408, 0x0000 },    /* R1032  - Output Rate 1 */
+	{ 0x00000409, 0x0022 },    /* R1033  - Output Volume Ramp */
+	{ 0x00000410, 0x0080 },    /* R1040  - Output Path Config 1L */
+	{ 0x00000411, 0x0180 },    /* R1041  - DAC Digital Volume 1L */
+	{ 0x00000412, 0x0080 },    /* R1042  - DAC Volume Limit 1L */
+	{ 0x00000413, 0x0001 },    /* R1043  - Noise Gate Select 1L */
+	{ 0x00000414, 0x0080 },    /* R1044  - Output Path Config 1R */
+	{ 0x00000415, 0x0180 },    /* R1045  - DAC Digital Volume 1R */
+	{ 0x00000416, 0x0080 },    /* R1046  - DAC Volume Limit 1R */
+	{ 0x00000417, 0x0002 },    /* R1047  - Noise Gate Select 1R */
+	{ 0x00000418, 0x0080 },    /* R1048  - Output Path Config 2L */
+	{ 0x00000419, 0x0180 },    /* R1049  - DAC Digital Volume 2L */
+	{ 0x0000041A, 0x0080 },    /* R1050  - DAC Volume Limit 2L */
+	{ 0x0000041B, 0x0004 },    /* R1051  - Noise Gate Select 2L */
+	{ 0x0000041C, 0x0080 },    /* R1052  - Output Path Config 2R */
+	{ 0x0000041D, 0x0180 },    /* R1053  - DAC Digital Volume 2R */
+	{ 0x0000041E, 0x0080 },    /* R1054  - DAC Volume Limit 2R */
+	{ 0x0000041F, 0x0008 },    /* R1055  - Noise Gate Select 2R */
+	{ 0x00000420, 0x0080 },    /* R1056  - Output Path Config 3L */
+	{ 0x00000421, 0x0180 },    /* R1057  - DAC Digital Volume 3L */
+	{ 0x00000422, 0x0080 },    /* R1058  - DAC Volume Limit 3L */
+	{ 0x00000423, 0x0010 },    /* R1059  - Noise Gate Select 3L */
+	{ 0x00000424, 0x0080 },    /* R1060  - Output Path Config 3R */
+	{ 0x00000425, 0x0180 },    /* R1061  - DAC Digital Volume 3R */
+	{ 0x00000426, 0x0080 },    /* R1062  - DAC Volume Limit 3R */
+	{ 0x00000427, 0x0020 },    /* R1063  - Noise Gate Select 3R */
+	{ 0x00000428, 0x0000 },    /* R1064  - Output Path Config 4L */
+	{ 0x00000429, 0x0180 },    /* R1065  - DAC Digital Volume 4L */
+	{ 0x0000042A, 0x0080 },    /* R1066  - Out Volume 4L */
+	{ 0x0000042B, 0x0040 },    /* R1067  - Noise Gate Select 4L */
+	{ 0x0000042C, 0x0000 },    /* R1068  - Output Path Config 4R */
+	{ 0x0000042D, 0x0180 },    /* R1069  - DAC Digital Volume 4R */
+	{ 0x0000042E, 0x0080 },    /* R1070  - Out Volume 4R */
+	{ 0x0000042F, 0x0080 },    /* R1071  - Noise Gate Select 4R */
+	{ 0x00000430, 0x0000 },    /* R1072  - Output Path Config 5L */
+	{ 0x00000431, 0x0180 },    /* R1073  - DAC Digital Volume 5L */
+	{ 0x00000432, 0x0080 },    /* R1074  - DAC Volume Limit 5L */
+	{ 0x00000433, 0x0100 },    /* R1075  - Noise Gate Select 5L */
+	{ 0x00000434, 0x0000 },    /* R1076  - Output Path Config 5R */
+	{ 0x00000435, 0x0180 },    /* R1077  - DAC Digital Volume 5R */
+	{ 0x00000436, 0x0080 },    /* R1078  - DAC Volume Limit 5R */
+	{ 0x00000437, 0x0200 },    /* R1079  - Noise Gate Select 5R */
+	{ 0x00000438, 0x0000 },    /* R1080  - Output Path Config 6L */
+	{ 0x00000439, 0x0180 },    /* R1081  - DAC Digital Volume 6L */
+	{ 0x0000043A, 0x0080 },    /* R1082  - DAC Volume Limit 6L */
+	{ 0x0000043B, 0x0400 },    /* R1083  - Noise Gate Select 6L */
+	{ 0x0000043C, 0x0000 },    /* R1084  - Output Path Config 6R */
+	{ 0x0000043D, 0x0180 },    /* R1085  - DAC Digital Volume 6R */
+	{ 0x0000043E, 0x0080 },    /* R1086  - DAC Volume Limit 6R */
+	{ 0x0000043F, 0x0800 },    /* R1087  - Noise Gate Select 6R */
+	{ 0x00000450, 0x0000 },    /* R1104  - DAC AEC Control 1 */
+	{ 0x00000458, 0x0001 },    /* R1112  - Noise Gate Control */
+	{ 0x00000480, 0x0040 },    /* R1152  - Class W ANC Threshold 1 */
+	{ 0x00000481, 0x0040 },    /* R1153  - Class W ANC Threshold 2 */
+	{ 0x00000490, 0x0069 },    /* R1168  - PDM SPK1 CTRL 1 */
+	{ 0x00000491, 0x0000 },    /* R1169  - PDM SPK1 CTRL 2 */
+	{ 0x00000492, 0x0069 },    /* R1170  - PDM SPK2 CTRL 1 */
+	{ 0x00000493, 0x0000 },    /* R1171  - PDM SPK2 CTRL 2 */
+	{ 0x00000500, 0x000C },    /* R1280  - AIF1 BCLK Ctrl */
+	{ 0x00000501, 0x0008 },    /* R1281  - AIF1 Tx Pin Ctrl */
+	{ 0x00000502, 0x0000 },    /* R1282  - AIF1 Rx Pin Ctrl */
+	{ 0x00000503, 0x0000 },    /* R1283  - AIF1 Rate Ctrl */
+	{ 0x00000504, 0x0000 },    /* R1284  - AIF1 Format */
+	{ 0x00000505, 0x0040 },    /* R1285  - AIF1 Tx BCLK Rate */
+	{ 0x00000506, 0x0040 },    /* R1286  - AIF1 Rx BCLK Rate */
+	{ 0x00000507, 0x1818 },    /* R1287  - AIF1 Frame Ctrl 1 */
+	{ 0x00000508, 0x1818 },    /* R1288  - AIF1 Frame Ctrl 2 */
+	{ 0x00000509, 0x0000 },    /* R1289  - AIF1 Frame Ctrl 3 */
+	{ 0x0000050A, 0x0001 },    /* R1290  - AIF1 Frame Ctrl 4 */
+	{ 0x0000050B, 0x0002 },    /* R1291  - AIF1 Frame Ctrl 5 */
+	{ 0x0000050C, 0x0003 },    /* R1292  - AIF1 Frame Ctrl 6 */
+	{ 0x0000050D, 0x0004 },    /* R1293  - AIF1 Frame Ctrl 7 */
+	{ 0x0000050E, 0x0005 },    /* R1294  - AIF1 Frame Ctrl 8 */
+	{ 0x0000050F, 0x0006 },    /* R1295  - AIF1 Frame Ctrl 9 */
+	{ 0x00000510, 0x0007 },    /* R1296  - AIF1 Frame Ctrl 10 */
+	{ 0x00000511, 0x0000 },    /* R1297  - AIF1 Frame Ctrl 11 */
+	{ 0x00000512, 0x0001 },    /* R1298  - AIF1 Frame Ctrl 12 */
+	{ 0x00000513, 0x0002 },    /* R1299  - AIF1 Frame Ctrl 13 */
+	{ 0x00000514, 0x0003 },    /* R1300  - AIF1 Frame Ctrl 14 */
+	{ 0x00000515, 0x0004 },    /* R1301  - AIF1 Frame Ctrl 15 */
+	{ 0x00000516, 0x0005 },    /* R1302  - AIF1 Frame Ctrl 16 */
+	{ 0x00000517, 0x0006 },    /* R1303  - AIF1 Frame Ctrl 17 */
+	{ 0x00000518, 0x0007 },    /* R1304  - AIF1 Frame Ctrl 18 */
+	{ 0x00000519, 0x0000 },    /* R1305  - AIF1 Tx Enables */
+	{ 0x0000051A, 0x0000 },    /* R1306  - AIF1 Rx Enables */
+	{ 0x00000540, 0x000C },    /* R1344  - AIF2 BCLK Ctrl */
+	{ 0x00000541, 0x0008 },    /* R1345  - AIF2 Tx Pin Ctrl */
+	{ 0x00000542, 0x0000 },    /* R1346  - AIF2 Rx Pin Ctrl */
+	{ 0x00000543, 0x0000 },    /* R1347  - AIF2 Rate Ctrl */
+	{ 0x00000544, 0x0000 },    /* R1348  - AIF2 Format */
+	{ 0x00000545, 0x0040 },    /* R1349  - AIF2 Tx BCLK Rate */
+	{ 0x00000546, 0x0040 },    /* R1350  - AIF2 Rx BCLK Rate */
+	{ 0x00000547, 0x1818 },    /* R1351  - AIF2 Frame Ctrl 1 */
+	{ 0x00000548, 0x1818 },    /* R1352  - AIF2 Frame Ctrl 2 */
+	{ 0x00000549, 0x0000 },    /* R1353  - AIF2 Frame Ctrl 3 */
+	{ 0x0000054A, 0x0001 },    /* R1354  - AIF2 Frame Ctrl 4 */
+	{ 0x00000551, 0x0000 },    /* R1361  - AIF2 Frame Ctrl 11 */
+	{ 0x00000552, 0x0001 },    /* R1362  - AIF2 Frame Ctrl 12 */
+	{ 0x00000559, 0x0000 },    /* R1369  - AIF2 Tx Enables */
+	{ 0x0000055A, 0x0000 },    /* R1370  - AIF2 Rx Enables */
+	{ 0x00000580, 0x000C },    /* R1408  - AIF3 BCLK Ctrl */
+	{ 0x00000581, 0x0008 },    /* R1409  - AIF3 Tx Pin Ctrl */
+	{ 0x00000582, 0x0000 },    /* R1410  - AIF3 Rx Pin Ctrl */
+	{ 0x00000583, 0x0000 },    /* R1411  - AIF3 Rate Ctrl */
+	{ 0x00000584, 0x0000 },    /* R1412  - AIF3 Format */
+	{ 0x00000585, 0x0040 },    /* R1413  - AIF3 Tx BCLK Rate */
+	{ 0x00000586, 0x0040 },    /* R1414  - AIF3 Rx BCLK Rate */
+	{ 0x00000587, 0x1818 },    /* R1415  - AIF3 Frame Ctrl 1 */
+	{ 0x00000588, 0x1818 },    /* R1416  - AIF3 Frame Ctrl 2 */
+	{ 0x00000589, 0x0000 },    /* R1417  - AIF3 Frame Ctrl 3 */
+	{ 0x0000058A, 0x0001 },    /* R1418  - AIF3 Frame Ctrl 4 */
+	{ 0x00000591, 0x0000 },    /* R1425  - AIF3 Frame Ctrl 11 */
+	{ 0x00000592, 0x0001 },    /* R1426  - AIF3 Frame Ctrl 12 */
+	{ 0x00000599, 0x0000 },    /* R1433  - AIF3 Tx Enables */
+	{ 0x0000059A, 0x0000 },    /* R1434  - AIF3 Rx Enables */
+	{ 0x000005E3, 0x0004 },    /* R1507  - SLIMbus Framer Ref Gear */
+	{ 0x000005E5, 0x0000 },    /* R1509  - SLIMbus Rates 1 */
+	{ 0x000005E6, 0x0000 },    /* R1510  - SLIMbus Rates 2 */
+	{ 0x000005E7, 0x0000 },    /* R1511  - SLIMbus Rates 3 */
+	{ 0x000005E8, 0x0000 },    /* R1512  - SLIMbus Rates 4 */
+	{ 0x000005E9, 0x0000 },    /* R1513  - SLIMbus Rates 5 */
+	{ 0x000005EA, 0x0000 },    /* R1514  - SLIMbus Rates 6 */
+	{ 0x000005EB, 0x0000 },    /* R1515  - SLIMbus Rates 7 */
+	{ 0x000005EC, 0x0000 },    /* R1516  - SLIMbus Rates 8 */
+	{ 0x000005F5, 0x0000 },    /* R1525  - SLIMbus RX Channel Enable */
+	{ 0x000005F6, 0x0000 },    /* R1526  - SLIMbus TX Channel Enable */
+	{ 0x00000640, 0x0000 },    /* R1600  - PWM1MIX Input 1 Source */
+	{ 0x00000641, 0x0080 },    /* R1601  - PWM1MIX Input 1 Volume */
+	{ 0x00000642, 0x0000 },    /* R1602  - PWM1MIX Input 2 Source */
+	{ 0x00000643, 0x0080 },    /* R1603  - PWM1MIX Input 2 Volume */
+	{ 0x00000644, 0x0000 },    /* R1604  - PWM1MIX Input 3 Source */
+	{ 0x00000645, 0x0080 },    /* R1605  - PWM1MIX Input 3 Volume */
+	{ 0x00000646, 0x0000 },    /* R1606  - PWM1MIX Input 4 Source */
+	{ 0x00000647, 0x0080 },    /* R1607  - PWM1MIX Input 4 Volume */
+	{ 0x00000648, 0x0000 },    /* R1608  - PWM2MIX Input 1 Source */
+	{ 0x00000649, 0x0080 },    /* R1609  - PWM2MIX Input 1 Volume */
+	{ 0x0000064A, 0x0000 },    /* R1610  - PWM2MIX Input 2 Source */
+	{ 0x0000064B, 0x0080 },    /* R1611  - PWM2MIX Input 2 Volume */
+	{ 0x0000064C, 0x0000 },    /* R1612  - PWM2MIX Input 3 Source */
+	{ 0x0000064D, 0x0080 },    /* R1613  - PWM2MIX Input 3 Volume */
+	{ 0x0000064E, 0x0000 },    /* R1614  - PWM2MIX Input 4 Source */
+	{ 0x0000064F, 0x0080 },    /* R1615  - PWM2MIX Input 4 Volume */
+	{ 0x00000660, 0x0000 },    /* R1632  - MICMIX Input 1 Source */
+	{ 0x00000661, 0x0080 },    /* R1633  - MICMIX Input 1 Volume */
+	{ 0x00000662, 0x0000 },    /* R1634  - MICMIX Input 2 Source */
+	{ 0x00000663, 0x0080 },    /* R1635  - MICMIX Input 2 Volume */
+	{ 0x00000664, 0x0000 },    /* R1636  - MICMIX Input 3 Source */
+	{ 0x00000665, 0x0080 },    /* R1637  - MICMIX Input 3 Volume */
+	{ 0x00000666, 0x0000 },    /* R1638  - MICMIX Input 4 Source */
+	{ 0x00000667, 0x0080 },    /* R1639  - MICMIX Input 4 Volume */
+	{ 0x00000668, 0x0000 },    /* R1640  - NOISEMIX Input 1 Source */
+	{ 0x00000669, 0x0080 },    /* R1641  - NOISEMIX Input 1 Volume */
+	{ 0x0000066A, 0x0000 },    /* R1642  - NOISEMIX Input 2 Source */
+	{ 0x0000066B, 0x0080 },    /* R1643  - NOISEMIX Input 2 Volume */
+	{ 0x0000066C, 0x0000 },    /* R1644  - NOISEMIX Input 3 Source */
+	{ 0x0000066D, 0x0080 },    /* R1645  - NOISEMIX Input 3 Volume */
+	{ 0x0000066E, 0x0000 },    /* R1646  - NOISEMIX Input 4 Source */
+	{ 0x0000066F, 0x0080 },    /* R1647  - NOISEMIX Input 4 Volume */
+	{ 0x00000680, 0x0000 },    /* R1664  - OUT1LMIX Input 1 Source */
+	{ 0x00000681, 0x0080 },    /* R1665  - OUT1LMIX Input 1 Volume */
+	{ 0x00000682, 0x0000 },    /* R1666  - OUT1LMIX Input 2 Source */
+	{ 0x00000683, 0x0080 },    /* R1667  - OUT1LMIX Input 2 Volume */
+	{ 0x00000684, 0x0000 },    /* R1668  - OUT1LMIX Input 3 Source */
+	{ 0x00000685, 0x0080 },    /* R1669  - OUT1LMIX Input 3 Volume */
+	{ 0x00000686, 0x0000 },    /* R1670  - OUT1LMIX Input 4 Source */
+	{ 0x00000687, 0x0080 },    /* R1671  - OUT1LMIX Input 4 Volume */
+	{ 0x00000688, 0x0000 },    /* R1672  - OUT1RMIX Input 1 Source */
+	{ 0x00000689, 0x0080 },    /* R1673  - OUT1RMIX Input 1 Volume */
+	{ 0x0000068A, 0x0000 },    /* R1674  - OUT1RMIX Input 2 Source */
+	{ 0x0000068B, 0x0080 },    /* R1675  - OUT1RMIX Input 2 Volume */
+	{ 0x0000068C, 0x0000 },    /* R1676  - OUT1RMIX Input 3 Source */
+	{ 0x0000068D, 0x0080 },    /* R1677  - OUT1RMIX Input 3 Volume */
+	{ 0x0000068E, 0x0000 },    /* R1678  - OUT1RMIX Input 4 Source */
+	{ 0x0000068F, 0x0080 },    /* R1679  - OUT1RMIX Input 4 Volume */
+	{ 0x00000690, 0x0000 },    /* R1680  - OUT2LMIX Input 1 Source */
+	{ 0x00000691, 0x0080 },    /* R1681  - OUT2LMIX Input 1 Volume */
+	{ 0x00000692, 0x0000 },    /* R1682  - OUT2LMIX Input 2 Source */
+	{ 0x00000693, 0x0080 },    /* R1683  - OUT2LMIX Input 2 Volume */
+	{ 0x00000694, 0x0000 },    /* R1684  - OUT2LMIX Input 3 Source */
+	{ 0x00000695, 0x0080 },    /* R1685  - OUT2LMIX Input 3 Volume */
+	{ 0x00000696, 0x0000 },    /* R1686  - OUT2LMIX Input 4 Source */
+	{ 0x00000697, 0x0080 },    /* R1687  - OUT2LMIX Input 4 Volume */
+	{ 0x00000698, 0x0000 },    /* R1688  - OUT2RMIX Input 1 Source */
+	{ 0x00000699, 0x0080 },    /* R1689  - OUT2RMIX Input 1 Volume */
+	{ 0x0000069A, 0x0000 },    /* R1690  - OUT2RMIX Input 2 Source */
+	{ 0x0000069B, 0x0080 },    /* R1691  - OUT2RMIX Input 2 Volume */
+	{ 0x0000069C, 0x0000 },    /* R1692  - OUT2RMIX Input 3 Source */
+	{ 0x0000069D, 0x0080 },    /* R1693  - OUT2RMIX Input 3 Volume */
+	{ 0x0000069E, 0x0000 },    /* R1694  - OUT2RMIX Input 4 Source */
+	{ 0x0000069F, 0x0080 },    /* R1695  - OUT2RMIX Input 4 Volume */
+	{ 0x000006A0, 0x0000 },    /* R1696  - OUT3LMIX Input 1 Source */
+	{ 0x000006A1, 0x0080 },    /* R1697  - OUT3LMIX Input 1 Volume */
+	{ 0x000006A2, 0x0000 },    /* R1698  - OUT3LMIX Input 2 Source */
+	{ 0x000006A3, 0x0080 },    /* R1699  - OUT3LMIX Input 2 Volume */
+	{ 0x000006A4, 0x0000 },    /* R1700  - OUT3LMIX Input 3 Source */
+	{ 0x000006A5, 0x0080 },    /* R1701  - OUT3LMIX Input 3 Volume */
+	{ 0x000006A6, 0x0000 },    /* R1702  - OUT3LMIX Input 4 Source */
+	{ 0x000006A7, 0x0080 },    /* R1703  - OUT3LMIX Input 4 Volume */
+	{ 0x000006A8, 0x0000 },    /* R1704  - OUT3RMIX Input 1 Source */
+	{ 0x000006A9, 0x0080 },    /* R1705  - OUT3RMIX Input 1 Volume */
+	{ 0x000006AA, 0x0000 },    /* R1706  - OUT3RMIX Input 2 Source */
+	{ 0x000006AB, 0x0080 },    /* R1707  - OUT3RMIX Input 2 Volume */
+	{ 0x000006AC, 0x0000 },    /* R1708  - OUT3RMIX Input 3 Source */
+	{ 0x000006AD, 0x0080 },    /* R1709  - OUT3RMIX Input 3 Volume */
+	{ 0x000006AE, 0x0000 },    /* R1710  - OUT3RMIX Input 4 Source */
+	{ 0x000006AF, 0x0080 },    /* R1711  - OUT3RMIX Input 4 Volume */
+	{ 0x000006B0, 0x0000 },    /* R1712  - OUT4LMIX Input 1 Source */
+	{ 0x000006B1, 0x0080 },    /* R1713  - OUT4LMIX Input 1 Volume */
+	{ 0x000006B2, 0x0000 },    /* R1714  - OUT4LMIX Input 2 Source */
+	{ 0x000006B3, 0x0080 },    /* R1715  - OUT4LMIX Input 2 Volume */
+	{ 0x000006B4, 0x0000 },    /* R1716  - OUT4LMIX Input 3 Source */
+	{ 0x000006B5, 0x0080 },    /* R1717  - OUT4LMIX Input 3 Volume */
+	{ 0x000006B6, 0x0000 },    /* R1718  - OUT4LMIX Input 4 Source */
+	{ 0x000006B7, 0x0080 },    /* R1719  - OUT4LMIX Input 4 Volume */
+	{ 0x000006B8, 0x0000 },    /* R1720  - OUT4RMIX Input 1 Source */
+	{ 0x000006B9, 0x0080 },    /* R1721  - OUT4RMIX Input 1 Volume */
+	{ 0x000006BA, 0x0000 },    /* R1722  - OUT4RMIX Input 2 Source */
+	{ 0x000006BB, 0x0080 },    /* R1723  - OUT4RMIX Input 2 Volume */
+	{ 0x000006BC, 0x0000 },    /* R1724  - OUT4RMIX Input 3 Source */
+	{ 0x000006BD, 0x0080 },    /* R1725  - OUT4RMIX Input 3 Volume */
+	{ 0x000006BE, 0x0000 },    /* R1726  - OUT4RMIX Input 4 Source */
+	{ 0x000006BF, 0x0080 },    /* R1727  - OUT4RMIX Input 4 Volume */
+	{ 0x000006C0, 0x0000 },    /* R1728  - OUT5LMIX Input 1 Source */
+	{ 0x000006C1, 0x0080 },    /* R1729  - OUT5LMIX Input 1 Volume */
+	{ 0x000006C2, 0x0000 },    /* R1730  - OUT5LMIX Input 2 Source */
+	{ 0x000006C3, 0x0080 },    /* R1731  - OUT5LMIX Input 2 Volume */
+	{ 0x000006C4, 0x0000 },    /* R1732  - OUT5LMIX Input 3 Source */
+	{ 0x000006C5, 0x0080 },    /* R1733  - OUT5LMIX Input 3 Volume */
+	{ 0x000006C6, 0x0000 },    /* R1734  - OUT5LMIX Input 4 Source */
+	{ 0x000006C7, 0x0080 },    /* R1735  - OUT5LMIX Input 4 Volume */
+	{ 0x000006C8, 0x0000 },    /* R1736  - OUT5RMIX Input 1 Source */
+	{ 0x000006C9, 0x0080 },    /* R1737  - OUT5RMIX Input 1 Volume */
+	{ 0x000006CA, 0x0000 },    /* R1738  - OUT5RMIX Input 2 Source */
+	{ 0x000006CB, 0x0080 },    /* R1739  - OUT5RMIX Input 2 Volume */
+	{ 0x000006CC, 0x0000 },    /* R1740  - OUT5RMIX Input 3 Source */
+	{ 0x000006CD, 0x0080 },    /* R1741  - OUT5RMIX Input 3 Volume */
+	{ 0x000006CE, 0x0000 },    /* R1742  - OUT5RMIX Input 4 Source */
+	{ 0x000006CF, 0x0080 },    /* R1743  - OUT5RMIX Input 4 Volume */
+	{ 0x000006D0, 0x0000 },    /* R1744  - OUT6LMIX Input 1 Source */
+	{ 0x000006D1, 0x0080 },    /* R1745  - OUT6LMIX Input 1 Volume */
+	{ 0x000006D2, 0x0000 },    /* R1746  - OUT6LMIX Input 2 Source */
+	{ 0x000006D3, 0x0080 },    /* R1747  - OUT6LMIX Input 2 Volume */
+	{ 0x000006D4, 0x0000 },    /* R1748  - OUT6LMIX Input 3 Source */
+	{ 0x000006D5, 0x0080 },    /* R1749  - OUT6LMIX Input 3 Volume */
+	{ 0x000006D6, 0x0000 },    /* R1750  - OUT6LMIX Input 4 Source */
+	{ 0x000006D7, 0x0080 },    /* R1751  - OUT6LMIX Input 4 Volume */
+	{ 0x000006D8, 0x0000 },    /* R1752  - OUT6RMIX Input 1 Source */
+	{ 0x000006D9, 0x0080 },    /* R1753  - OUT6RMIX Input 1 Volume */
+	{ 0x000006DA, 0x0000 },    /* R1754  - OUT6RMIX Input 2 Source */
+	{ 0x000006DB, 0x0080 },    /* R1755  - OUT6RMIX Input 2 Volume */
+	{ 0x000006DC, 0x0000 },    /* R1756  - OUT6RMIX Input 3 Source */
+	{ 0x000006DD, 0x0080 },    /* R1757  - OUT6RMIX Input 3 Volume */
+	{ 0x000006DE, 0x0000 },    /* R1758  - OUT6RMIX Input 4 Source */
+	{ 0x000006DF, 0x0080 },    /* R1759  - OUT6RMIX Input 4 Volume */
+	{ 0x00000700, 0x0000 },    /* R1792  - AIF1TX1MIX Input 1 Source */
+	{ 0x00000701, 0x0080 },    /* R1793  - AIF1TX1MIX Input 1 Volume */
+	{ 0x00000702, 0x0000 },    /* R1794  - AIF1TX1MIX Input 2 Source */
+	{ 0x00000703, 0x0080 },    /* R1795  - AIF1TX1MIX Input 2 Volume */
+	{ 0x00000704, 0x0000 },    /* R1796  - AIF1TX1MIX Input 3 Source */
+	{ 0x00000705, 0x0080 },    /* R1797  - AIF1TX1MIX Input 3 Volume */
+	{ 0x00000706, 0x0000 },    /* R1798  - AIF1TX1MIX Input 4 Source */
+	{ 0x00000707, 0x0080 },    /* R1799  - AIF1TX1MIX Input 4 Volume */
+	{ 0x00000708, 0x0000 },    /* R1800  - AIF1TX2MIX Input 1 Source */
+	{ 0x00000709, 0x0080 },    /* R1801  - AIF1TX2MIX Input 1 Volume */
+	{ 0x0000070A, 0x0000 },    /* R1802  - AIF1TX2MIX Input 2 Source */
+	{ 0x0000070B, 0x0080 },    /* R1803  - AIF1TX2MIX Input 2 Volume */
+	{ 0x0000070C, 0x0000 },    /* R1804  - AIF1TX2MIX Input 3 Source */
+	{ 0x0000070D, 0x0080 },    /* R1805  - AIF1TX2MIX Input 3 Volume */
+	{ 0x0000070E, 0x0000 },    /* R1806  - AIF1TX2MIX Input 4 Source */
+	{ 0x0000070F, 0x0080 },    /* R1807  - AIF1TX2MIX Input 4 Volume */
+	{ 0x00000710, 0x0000 },    /* R1808  - AIF1TX3MIX Input 1 Source */
+	{ 0x00000711, 0x0080 },    /* R1809  - AIF1TX3MIX Input 1 Volume */
+	{ 0x00000712, 0x0000 },    /* R1810  - AIF1TX3MIX Input 2 Source */
+	{ 0x00000713, 0x0080 },    /* R1811  - AIF1TX3MIX Input 2 Volume */
+	{ 0x00000714, 0x0000 },    /* R1812  - AIF1TX3MIX Input 3 Source */
+	{ 0x00000715, 0x0080 },    /* R1813  - AIF1TX3MIX Input 3 Volume */
+	{ 0x00000716, 0x0000 },    /* R1814  - AIF1TX3MIX Input 4 Source */
+	{ 0x00000717, 0x0080 },    /* R1815  - AIF1TX3MIX Input 4 Volume */
+	{ 0x00000718, 0x0000 },    /* R1816  - AIF1TX4MIX Input 1 Source */
+	{ 0x00000719, 0x0080 },    /* R1817  - AIF1TX4MIX Input 1 Volume */
+	{ 0x0000071A, 0x0000 },    /* R1818  - AIF1TX4MIX Input 2 Source */
+	{ 0x0000071B, 0x0080 },    /* R1819  - AIF1TX4MIX Input 2 Volume */
+	{ 0x0000071C, 0x0000 },    /* R1820  - AIF1TX4MIX Input 3 Source */
+	{ 0x0000071D, 0x0080 },    /* R1821  - AIF1TX4MIX Input 3 Volume */
+	{ 0x0000071E, 0x0000 },    /* R1822  - AIF1TX4MIX Input 4 Source */
+	{ 0x0000071F, 0x0080 },    /* R1823  - AIF1TX4MIX Input 4 Volume */
+	{ 0x00000720, 0x0000 },    /* R1824  - AIF1TX5MIX Input 1 Source */
+	{ 0x00000721, 0x0080 },    /* R1825  - AIF1TX5MIX Input 1 Volume */
+	{ 0x00000722, 0x0000 },    /* R1826  - AIF1TX5MIX Input 2 Source */
+	{ 0x00000723, 0x0080 },    /* R1827  - AIF1TX5MIX Input 2 Volume */
+	{ 0x00000724, 0x0000 },    /* R1828  - AIF1TX5MIX Input 3 Source */
+	{ 0x00000725, 0x0080 },    /* R1829  - AIF1TX5MIX Input 3 Volume */
+	{ 0x00000726, 0x0000 },    /* R1830  - AIF1TX5MIX Input 4 Source */
+	{ 0x00000727, 0x0080 },    /* R1831  - AIF1TX5MIX Input 4 Volume */
+	{ 0x00000728, 0x0000 },    /* R1832  - AIF1TX6MIX Input 1 Source */
+	{ 0x00000729, 0x0080 },    /* R1833  - AIF1TX6MIX Input 1 Volume */
+	{ 0x0000072A, 0x0000 },    /* R1834  - AIF1TX6MIX Input 2 Source */
+	{ 0x0000072B, 0x0080 },    /* R1835  - AIF1TX6MIX Input 2 Volume */
+	{ 0x0000072C, 0x0000 },    /* R1836  - AIF1TX6MIX Input 3 Source */
+	{ 0x0000072D, 0x0080 },    /* R1837  - AIF1TX6MIX Input 3 Volume */
+	{ 0x0000072E, 0x0000 },    /* R1838  - AIF1TX6MIX Input 4 Source */
+	{ 0x0000072F, 0x0080 },    /* R1839  - AIF1TX6MIX Input 4 Volume */
+	{ 0x00000730, 0x0000 },    /* R1840  - AIF1TX7MIX Input 1 Source */
+	{ 0x00000731, 0x0080 },    /* R1841  - AIF1TX7MIX Input 1 Volume */
+	{ 0x00000732, 0x0000 },    /* R1842  - AIF1TX7MIX Input 2 Source */
+	{ 0x00000733, 0x0080 },    /* R1843  - AIF1TX7MIX Input 2 Volume */
+	{ 0x00000734, 0x0000 },    /* R1844  - AIF1TX7MIX Input 3 Source */
+	{ 0x00000735, 0x0080 },    /* R1845  - AIF1TX7MIX Input 3 Volume */
+	{ 0x00000736, 0x0000 },    /* R1846  - AIF1TX7MIX Input 4 Source */
+	{ 0x00000737, 0x0080 },    /* R1847  - AIF1TX7MIX Input 4 Volume */
+	{ 0x00000738, 0x0000 },    /* R1848  - AIF1TX8MIX Input 1 Source */
+	{ 0x00000739, 0x0080 },    /* R1849  - AIF1TX8MIX Input 1 Volume */
+	{ 0x0000073A, 0x0000 },    /* R1850  - AIF1TX8MIX Input 2 Source */
+	{ 0x0000073B, 0x0080 },    /* R1851  - AIF1TX8MIX Input 2 Volume */
+	{ 0x0000073C, 0x0000 },    /* R1852  - AIF1TX8MIX Input 3 Source */
+	{ 0x0000073D, 0x0080 },    /* R1853  - AIF1TX8MIX Input 3 Volume */
+	{ 0x0000073E, 0x0000 },    /* R1854  - AIF1TX8MIX Input 4 Source */
+	{ 0x0000073F, 0x0080 },    /* R1855  - AIF1TX8MIX Input 4 Volume */
+	{ 0x00000740, 0x0000 },    /* R1856  - AIF2TX1MIX Input 1 Source */
+	{ 0x00000741, 0x0080 },    /* R1857  - AIF2TX1MIX Input 1 Volume */
+	{ 0x00000742, 0x0000 },    /* R1858  - AIF2TX1MIX Input 2 Source */
+	{ 0x00000743, 0x0080 },    /* R1859  - AIF2TX1MIX Input 2 Volume */
+	{ 0x00000744, 0x0000 },    /* R1860  - AIF2TX1MIX Input 3 Source */
+	{ 0x00000745, 0x0080 },    /* R1861  - AIF2TX1MIX Input 3 Volume */
+	{ 0x00000746, 0x0000 },    /* R1862  - AIF2TX1MIX Input 4 Source */
+	{ 0x00000747, 0x0080 },    /* R1863  - AIF2TX1MIX Input 4 Volume */
+	{ 0x00000748, 0x0000 },    /* R1864  - AIF2TX2MIX Input 1 Source */
+	{ 0x00000749, 0x0080 },    /* R1865  - AIF2TX2MIX Input 1 Volume */
+	{ 0x0000074A, 0x0000 },    /* R1866  - AIF2TX2MIX Input 2 Source */
+	{ 0x0000074B, 0x0080 },    /* R1867  - AIF2TX2MIX Input 2 Volume */
+	{ 0x0000074C, 0x0000 },    /* R1868  - AIF2TX2MIX Input 3 Source */
+	{ 0x0000074D, 0x0080 },    /* R1869  - AIF2TX2MIX Input 3 Volume */
+	{ 0x0000074E, 0x0000 },    /* R1870  - AIF2TX2MIX Input 4 Source */
+	{ 0x0000074F, 0x0080 },    /* R1871  - AIF2TX2MIX Input 4 Volume */
+	{ 0x00000780, 0x0000 },    /* R1920  - AIF3TX1MIX Input 1 Source */
+	{ 0x00000781, 0x0080 },    /* R1921  - AIF3TX1MIX Input 1 Volume */
+	{ 0x00000782, 0x0000 },    /* R1922  - AIF3TX1MIX Input 2 Source */
+	{ 0x00000783, 0x0080 },    /* R1923  - AIF3TX1MIX Input 2 Volume */
+	{ 0x00000784, 0x0000 },    /* R1924  - AIF3TX1MIX Input 3 Source */
+	{ 0x00000785, 0x0080 },    /* R1925  - AIF3TX1MIX Input 3 Volume */
+	{ 0x00000786, 0x0000 },    /* R1926  - AIF3TX1MIX Input 4 Source */
+	{ 0x00000787, 0x0080 },    /* R1927  - AIF3TX1MIX Input 4 Volume */
+	{ 0x00000788, 0x0000 },    /* R1928  - AIF3TX2MIX Input 1 Source */
+	{ 0x00000789, 0x0080 },    /* R1929  - AIF3TX2MIX Input 1 Volume */
+	{ 0x0000078A, 0x0000 },    /* R1930  - AIF3TX2MIX Input 2 Source */
+	{ 0x0000078B, 0x0080 },    /* R1931  - AIF3TX2MIX Input 2 Volume */
+	{ 0x0000078C, 0x0000 },    /* R1932  - AIF3TX2MIX Input 3 Source */
+	{ 0x0000078D, 0x0080 },    /* R1933  - AIF3TX2MIX Input 3 Volume */
+	{ 0x0000078E, 0x0000 },    /* R1934  - AIF3TX2MIX Input 4 Source */
+	{ 0x0000078F, 0x0080 },    /* R1935  - AIF3TX2MIX Input 4 Volume */
+	{ 0x000007C0, 0x0000 },    /* R1984  - SLIMTX1MIX Input 1 Source */
+	{ 0x000007C1, 0x0080 },    /* R1985  - SLIMTX1MIX Input 1 Volume */
+	{ 0x000007C2, 0x0000 },    /* R1986  - SLIMTX1MIX Input 2 Source */
+	{ 0x000007C3, 0x0080 },    /* R1987  - SLIMTX1MIX Input 2 Volume */
+	{ 0x000007C4, 0x0000 },    /* R1988  - SLIMTX1MIX Input 3 Source */
+	{ 0x000007C5, 0x0080 },    /* R1989  - SLIMTX1MIX Input 3 Volume */
+	{ 0x000007C6, 0x0000 },    /* R1990  - SLIMTX1MIX Input 4 Source */
+	{ 0x000007C7, 0x0080 },    /* R1991  - SLIMTX1MIX Input 4 Volume */
+	{ 0x000007C8, 0x0000 },    /* R1992  - SLIMTX2MIX Input 1 Source */
+	{ 0x000007C9, 0x0080 },    /* R1993  - SLIMTX2MIX Input 1 Volume */
+	{ 0x000007CA, 0x0000 },    /* R1994  - SLIMTX2MIX Input 2 Source */
+	{ 0x000007CB, 0x0080 },    /* R1995  - SLIMTX2MIX Input 2 Volume */
+	{ 0x000007CC, 0x0000 },    /* R1996  - SLIMTX2MIX Input 3 Source */
+	{ 0x000007CD, 0x0080 },    /* R1997  - SLIMTX2MIX Input 3 Volume */
+	{ 0x000007CE, 0x0000 },    /* R1998  - SLIMTX2MIX Input 4 Source */
+	{ 0x000007CF, 0x0080 },    /* R1999  - SLIMTX2MIX Input 4 Volume */
+	{ 0x000007D0, 0x0000 },    /* R2000  - SLIMTX3MIX Input 1 Source */
+	{ 0x000007D1, 0x0080 },    /* R2001  - SLIMTX3MIX Input 1 Volume */
+	{ 0x000007D2, 0x0000 },    /* R2002  - SLIMTX3MIX Input 2 Source */
+	{ 0x000007D3, 0x0080 },    /* R2003  - SLIMTX3MIX Input 2 Volume */
+	{ 0x000007D4, 0x0000 },    /* R2004  - SLIMTX3MIX Input 3 Source */
+	{ 0x000007D5, 0x0080 },    /* R2005  - SLIMTX3MIX Input 3 Volume */
+	{ 0x000007D6, 0x0000 },    /* R2006  - SLIMTX3MIX Input 4 Source */
+	{ 0x000007D7, 0x0080 },    /* R2007  - SLIMTX3MIX Input 4 Volume */
+	{ 0x000007D8, 0x0000 },    /* R2008  - SLIMTX4MIX Input 1 Source */
+	{ 0x000007D9, 0x0080 },    /* R2009  - SLIMTX4MIX Input 1 Volume */
+	{ 0x000007DA, 0x0000 },    /* R2010  - SLIMTX4MIX Input 2 Source */
+	{ 0x000007DB, 0x0080 },    /* R2011  - SLIMTX4MIX Input 2 Volume */
+	{ 0x000007DC, 0x0000 },    /* R2012  - SLIMTX4MIX Input 3 Source */
+	{ 0x000007DD, 0x0080 },    /* R2013  - SLIMTX4MIX Input 3 Volume */
+	{ 0x000007DE, 0x0000 },    /* R2014  - SLIMTX4MIX Input 4 Source */
+	{ 0x000007DF, 0x0080 },    /* R2015  - SLIMTX4MIX Input 4 Volume */
+	{ 0x000007E0, 0x0000 },    /* R2016  - SLIMTX5MIX Input 1 Source */
+	{ 0x000007E1, 0x0080 },    /* R2017  - SLIMTX5MIX Input 1 Volume */
+	{ 0x000007E2, 0x0000 },    /* R2018  - SLIMTX5MIX Input 2 Source */
+	{ 0x000007E3, 0x0080 },    /* R2019  - SLIMTX5MIX Input 2 Volume */
+	{ 0x000007E4, 0x0000 },    /* R2020  - SLIMTX5MIX Input 3 Source */
+	{ 0x000007E5, 0x0080 },    /* R2021  - SLIMTX5MIX Input 3 Volume */
+	{ 0x000007E6, 0x0000 },    /* R2022  - SLIMTX5MIX Input 4 Source */
+	{ 0x000007E7, 0x0080 },    /* R2023  - SLIMTX5MIX Input 4 Volume */
+	{ 0x000007E8, 0x0000 },    /* R2024  - SLIMTX6MIX Input 1 Source */
+	{ 0x000007E9, 0x0080 },    /* R2025  - SLIMTX6MIX Input 1 Volume */
+	{ 0x000007EA, 0x0000 },    /* R2026  - SLIMTX6MIX Input 2 Source */
+	{ 0x000007EB, 0x0080 },    /* R2027  - SLIMTX6MIX Input 2 Volume */
+	{ 0x000007EC, 0x0000 },    /* R2028  - SLIMTX6MIX Input 3 Source */
+	{ 0x000007ED, 0x0080 },    /* R2029  - SLIMTX6MIX Input 3 Volume */
+	{ 0x000007EE, 0x0000 },    /* R2030  - SLIMTX6MIX Input 4 Source */
+	{ 0x000007EF, 0x0080 },    /* R2031  - SLIMTX6MIX Input 4 Volume */
+	{ 0x000007F0, 0x0000 },    /* R2032  - SLIMTX7MIX Input 1 Source */
+	{ 0x000007F1, 0x0080 },    /* R2033  - SLIMTX7MIX Input 1 Volume */
+	{ 0x000007F2, 0x0000 },    /* R2034  - SLIMTX7MIX Input 2 Source */
+	{ 0x000007F3, 0x0080 },    /* R2035  - SLIMTX7MIX Input 2 Volume */
+	{ 0x000007F4, 0x0000 },    /* R2036  - SLIMTX7MIX Input 3 Source */
+	{ 0x000007F5, 0x0080 },    /* R2037  - SLIMTX7MIX Input 3 Volume */
+	{ 0x000007F6, 0x0000 },    /* R2038  - SLIMTX7MIX Input 4 Source */
+	{ 0x000007F7, 0x0080 },    /* R2039  - SLIMTX7MIX Input 4 Volume */
+	{ 0x000007F8, 0x0000 },    /* R2040  - SLIMTX8MIX Input 1 Source */
+	{ 0x000007F9, 0x0080 },    /* R2041  - SLIMTX8MIX Input 1 Volume */
+	{ 0x000007FA, 0x0000 },    /* R2042  - SLIMTX8MIX Input 2 Source */
+	{ 0x000007FB, 0x0080 },    /* R2043  - SLIMTX8MIX Input 2 Volume */
+	{ 0x000007FC, 0x0000 },    /* R2044  - SLIMTX8MIX Input 3 Source */
+	{ 0x000007FD, 0x0080 },    /* R2045  - SLIMTX8MIX Input 3 Volume */
+	{ 0x000007FE, 0x0000 },    /* R2046  - SLIMTX8MIX Input 4 Source */
+	{ 0x000007FF, 0x0080 },    /* R2047  - SLIMTX8MIX Input 4 Volume */
+	{ 0x00000880, 0x0000 },    /* R2176  - EQ1MIX Input 1 Source */
+	{ 0x00000881, 0x0080 },    /* R2177  - EQ1MIX Input 1 Volume */
+	{ 0x00000882, 0x0000 },    /* R2178  - EQ1MIX Input 2 Source */
+	{ 0x00000883, 0x0080 },    /* R2179  - EQ1MIX Input 2 Volume */
+	{ 0x00000884, 0x0000 },    /* R2180  - EQ1MIX Input 3 Source */
+	{ 0x00000885, 0x0080 },    /* R2181  - EQ1MIX Input 3 Volume */
+	{ 0x00000886, 0x0000 },    /* R2182  - EQ1MIX Input 4 Source */
+	{ 0x00000887, 0x0080 },    /* R2183  - EQ1MIX Input 4 Volume */
+	{ 0x00000888, 0x0000 },    /* R2184  - EQ2MIX Input 1 Source */
+	{ 0x00000889, 0x0080 },    /* R2185  - EQ2MIX Input 1 Volume */
+	{ 0x0000088A, 0x0000 },    /* R2186  - EQ2MIX Input 2 Source */
+	{ 0x0000088B, 0x0080 },    /* R2187  - EQ2MIX Input 2 Volume */
+	{ 0x0000088C, 0x0000 },    /* R2188  - EQ2MIX Input 3 Source */
+	{ 0x0000088D, 0x0080 },    /* R2189  - EQ2MIX Input 3 Volume */
+	{ 0x0000088E, 0x0000 },    /* R2190  - EQ2MIX Input 4 Source */
+	{ 0x0000088F, 0x0080 },    /* R2191  - EQ2MIX Input 4 Volume */
+	{ 0x00000890, 0x0000 },    /* R2192  - EQ3MIX Input 1 Source */
+	{ 0x00000891, 0x0080 },    /* R2193  - EQ3MIX Input 1 Volume */
+	{ 0x00000892, 0x0000 },    /* R2194  - EQ3MIX Input 2 Source */
+	{ 0x00000893, 0x0080 },    /* R2195  - EQ3MIX Input 2 Volume */
+	{ 0x00000894, 0x0000 },    /* R2196  - EQ3MIX Input 3 Source */
+	{ 0x00000895, 0x0080 },    /* R2197  - EQ3MIX Input 3 Volume */
+	{ 0x00000896, 0x0000 },    /* R2198  - EQ3MIX Input 4 Source */
+	{ 0x00000897, 0x0080 },    /* R2199  - EQ3MIX Input 4 Volume */
+	{ 0x00000898, 0x0000 },    /* R2200  - EQ4MIX Input 1 Source */
+	{ 0x00000899, 0x0080 },    /* R2201  - EQ4MIX Input 1 Volume */
+	{ 0x0000089A, 0x0000 },    /* R2202  - EQ4MIX Input 2 Source */
+	{ 0x0000089B, 0x0080 },    /* R2203  - EQ4MIX Input 2 Volume */
+	{ 0x0000089C, 0x0000 },    /* R2204  - EQ4MIX Input 3 Source */
+	{ 0x0000089D, 0x0080 },    /* R2205  - EQ4MIX Input 3 Volume */
+	{ 0x0000089E, 0x0000 },    /* R2206  - EQ4MIX Input 4 Source */
+	{ 0x0000089F, 0x0080 },    /* R2207  - EQ4MIX Input 4 Volume */
+	{ 0x000008C0, 0x0000 },    /* R2240  - DRC1LMIX Input 1 Source */
+	{ 0x000008C1, 0x0080 },    /* R2241  - DRC1LMIX Input 1 Volume */
+	{ 0x000008C2, 0x0000 },    /* R2242  - DRC1LMIX Input 2 Source */
+	{ 0x000008C3, 0x0080 },    /* R2243  - DRC1LMIX Input 2 Volume */
+	{ 0x000008C4, 0x0000 },    /* R2244  - DRC1LMIX Input 3 Source */
+	{ 0x000008C5, 0x0080 },    /* R2245  - DRC1LMIX Input 3 Volume */
+	{ 0x000008C6, 0x0000 },    /* R2246  - DRC1LMIX Input 4 Source */
+	{ 0x000008C7, 0x0080 },    /* R2247  - DRC1LMIX Input 4 Volume */
+	{ 0x000008C8, 0x0000 },    /* R2248  - DRC1RMIX Input 1 Source */
+	{ 0x000008C9, 0x0080 },    /* R2249  - DRC1RMIX Input 1 Volume */
+	{ 0x000008CA, 0x0000 },    /* R2250  - DRC1RMIX Input 2 Source */
+	{ 0x000008CB, 0x0080 },    /* R2251  - DRC1RMIX Input 2 Volume */
+	{ 0x000008CC, 0x0000 },    /* R2252  - DRC1RMIX Input 3 Source */
+	{ 0x000008CD, 0x0080 },    /* R2253  - DRC1RMIX Input 3 Volume */
+	{ 0x000008CE, 0x0000 },    /* R2254  - DRC1RMIX Input 4 Source */
+	{ 0x000008CF, 0x0080 },    /* R2255  - DRC1RMIX Input 4 Volume */
+	{ 0x000008D0, 0x0000 },    /* R2256  - DRC2LMIX Input 1 Source */
+	{ 0x000008D1, 0x0080 },    /* R2257  - DRC2LMIX Input 1 Volume */
+	{ 0x000008D2, 0x0000 },    /* R2258  - DRC2LMIX Input 2 Source */
+	{ 0x000008D3, 0x0080 },    /* R2259  - DRC2LMIX Input 2 Volume */
+	{ 0x000008D4, 0x0000 },    /* R2260  - DRC2LMIX Input 3 Source */
+	{ 0x000008D5, 0x0080 },    /* R2261  - DRC2LMIX Input 3 Volume */
+	{ 0x000008D6, 0x0000 },    /* R2262  - DRC2LMIX Input 4 Source */
+	{ 0x000008D7, 0x0080 },    /* R2263  - DRC2LMIX Input 4 Volume */
+	{ 0x000008D8, 0x0000 },    /* R2264  - DRC2RMIX Input 1 Source */
+	{ 0x000008D9, 0x0080 },    /* R2265  - DRC2RMIX Input 1 Volume */
+	{ 0x000008DA, 0x0000 },    /* R2266  - DRC2RMIX Input 2 Source */
+	{ 0x000008DB, 0x0080 },    /* R2267  - DRC2RMIX Input 2 Volume */
+	{ 0x000008DC, 0x0000 },    /* R2268  - DRC2RMIX Input 3 Source */
+	{ 0x000008DD, 0x0080 },    /* R2269  - DRC2RMIX Input 3 Volume */
+	{ 0x000008DE, 0x0000 },    /* R2270  - DRC2RMIX Input 4 Source */
+	{ 0x000008DF, 0x0080 },    /* R2271  - DRC2RMIX Input 4 Volume */
+	{ 0x00000900, 0x0000 },    /* R2304  - HPLP1MIX Input 1 Source */
+	{ 0x00000901, 0x0080 },    /* R2305  - HPLP1MIX Input 1 Volume */
+	{ 0x00000902, 0x0000 },    /* R2306  - HPLP1MIX Input 2 Source */
+	{ 0x00000903, 0x0080 },    /* R2307  - HPLP1MIX Input 2 Volume */
+	{ 0x00000904, 0x0000 },    /* R2308  - HPLP1MIX Input 3 Source */
+	{ 0x00000905, 0x0080 },    /* R2309  - HPLP1MIX Input 3 Volume */
+	{ 0x00000906, 0x0000 },    /* R2310  - HPLP1MIX Input 4 Source */
+	{ 0x00000907, 0x0080 },    /* R2311  - HPLP1MIX Input 4 Volume */
+	{ 0x00000908, 0x0000 },    /* R2312  - HPLP2MIX Input 1 Source */
+	{ 0x00000909, 0x0080 },    /* R2313  - HPLP2MIX Input 1 Volume */
+	{ 0x0000090A, 0x0000 },    /* R2314  - HPLP2MIX Input 2 Source */
+	{ 0x0000090B, 0x0080 },    /* R2315  - HPLP2MIX Input 2 Volume */
+	{ 0x0000090C, 0x0000 },    /* R2316  - HPLP2MIX Input 3 Source */
+	{ 0x0000090D, 0x0080 },    /* R2317  - HPLP2MIX Input 3 Volume */
+	{ 0x0000090E, 0x0000 },    /* R2318  - HPLP2MIX Input 4 Source */
+	{ 0x0000090F, 0x0080 },    /* R2319  - HPLP2MIX Input 4 Volume */
+	{ 0x00000910, 0x0000 },    /* R2320  - HPLP3MIX Input 1 Source */
+	{ 0x00000911, 0x0080 },    /* R2321  - HPLP3MIX Input 1 Volume */
+	{ 0x00000912, 0x0000 },    /* R2322  - HPLP3MIX Input 2 Source */
+	{ 0x00000913, 0x0080 },    /* R2323  - HPLP3MIX Input 2 Volume */
+	{ 0x00000914, 0x0000 },    /* R2324  - HPLP3MIX Input 3 Source */
+	{ 0x00000915, 0x0080 },    /* R2325  - HPLP3MIX Input 3 Volume */
+	{ 0x00000916, 0x0000 },    /* R2326  - HPLP3MIX Input 4 Source */
+	{ 0x00000917, 0x0080 },    /* R2327  - HPLP3MIX Input 4 Volume */
+	{ 0x00000918, 0x0000 },    /* R2328  - HPLP4MIX Input 1 Source */
+	{ 0x00000919, 0x0080 },    /* R2329  - HPLP4MIX Input 1 Volume */
+	{ 0x0000091A, 0x0000 },    /* R2330  - HPLP4MIX Input 2 Source */
+	{ 0x0000091B, 0x0080 },    /* R2331  - HPLP4MIX Input 2 Volume */
+	{ 0x0000091C, 0x0000 },    /* R2332  - HPLP4MIX Input 3 Source */
+	{ 0x0000091D, 0x0080 },    /* R2333  - HPLP4MIX Input 3 Volume */
+	{ 0x0000091E, 0x0000 },    /* R2334  - HPLP4MIX Input 4 Source */
+	{ 0x0000091F, 0x0080 },    /* R2335  - HPLP4MIX Input 4 Volume */
+	{ 0x00000940, 0x0000 },    /* R2368  - DSP1LMIX Input 1 Source */
+	{ 0x00000941, 0x0080 },    /* R2369  - DSP1LMIX Input 1 Volume */
+	{ 0x00000942, 0x0000 },    /* R2370  - DSP1LMIX Input 2 Source */
+	{ 0x00000943, 0x0080 },    /* R2371  - DSP1LMIX Input 2 Volume */
+	{ 0x00000944, 0x0000 },    /* R2372  - DSP1LMIX Input 3 Source */
+	{ 0x00000945, 0x0080 },    /* R2373  - DSP1LMIX Input 3 Volume */
+	{ 0x00000946, 0x0000 },    /* R2374  - DSP1LMIX Input 4 Source */
+	{ 0x00000947, 0x0080 },    /* R2375  - DSP1LMIX Input 4 Volume */
+	{ 0x00000948, 0x0000 },    /* R2376  - DSP1RMIX Input 1 Source */
+	{ 0x00000949, 0x0080 },    /* R2377  - DSP1RMIX Input 1 Volume */
+	{ 0x0000094A, 0x0000 },    /* R2378  - DSP1RMIX Input 2 Source */
+	{ 0x0000094B, 0x0080 },    /* R2379  - DSP1RMIX Input 2 Volume */
+	{ 0x0000094C, 0x0000 },    /* R2380  - DSP1RMIX Input 3 Source */
+	{ 0x0000094D, 0x0080 },    /* R2381  - DSP1RMIX Input 3 Volume */
+	{ 0x0000094E, 0x0000 },    /* R2382  - DSP1RMIX Input 4 Source */
+	{ 0x0000094F, 0x0080 },    /* R2383  - DSP1RMIX Input 4 Volume */
+	{ 0x00000950, 0x0000 },    /* R2384  - DSP1AUX1MIX Input 1 Source */
+	{ 0x00000958, 0x0000 },    /* R2392  - DSP1AUX2MIX Input 1 Source */
+	{ 0x00000960, 0x0000 },    /* R2400  - DSP1AUX3MIX Input 1 Source */
+	{ 0x00000968, 0x0000 },    /* R2408  - DSP1AUX4MIX Input 1 Source */
+	{ 0x00000970, 0x0000 },    /* R2416  - DSP1AUX5MIX Input 1 Source */
+	{ 0x00000978, 0x0000 },    /* R2424  - DSP1AUX6MIX Input 1 Source */
+	{ 0x00000980, 0x0000 },    /* R2432  - DSP2LMIX Input 1 Source */
+	{ 0x00000981, 0x0080 },    /* R2433  - DSP2LMIX Input 1 Volume */
+	{ 0x00000982, 0x0000 },    /* R2434  - DSP2LMIX Input 2 Source */
+	{ 0x00000983, 0x0080 },    /* R2435  - DSP2LMIX Input 2 Volume */
+	{ 0x00000984, 0x0000 },    /* R2436  - DSP2LMIX Input 3 Source */
+	{ 0x00000985, 0x0080 },    /* R2437  - DSP2LMIX Input 3 Volume */
+	{ 0x00000986, 0x0000 },    /* R2438  - DSP2LMIX Input 4 Source */
+	{ 0x00000987, 0x0080 },    /* R2439  - DSP2LMIX Input 4 Volume */
+	{ 0x00000988, 0x0000 },    /* R2440  - DSP2RMIX Input 1 Source */
+	{ 0x00000989, 0x0080 },    /* R2441  - DSP2RMIX Input 1 Volume */
+	{ 0x0000098A, 0x0000 },    /* R2442  - DSP2RMIX Input 2 Source */
+	{ 0x0000098B, 0x0080 },    /* R2443  - DSP2RMIX Input 2 Volume */
+	{ 0x0000098C, 0x0000 },    /* R2444  - DSP2RMIX Input 3 Source */
+	{ 0x0000098D, 0x0080 },    /* R2445  - DSP2RMIX Input 3 Volume */
+	{ 0x0000098E, 0x0000 },    /* R2446  - DSP2RMIX Input 4 Source */
+	{ 0x0000098F, 0x0080 },    /* R2447  - DSP2RMIX Input 4 Volume */
+	{ 0x00000990, 0x0000 },    /* R2448  - DSP2AUX1MIX Input 1 Source */
+	{ 0x00000998, 0x0000 },    /* R2456  - DSP2AUX2MIX Input 1 Source */
+	{ 0x000009A0, 0x0000 },    /* R2464  - DSP2AUX3MIX Input 1 Source */
+	{ 0x000009A8, 0x0000 },    /* R2472  - DSP2AUX4MIX Input 1 Source */
+	{ 0x000009B0, 0x0000 },    /* R2480  - DSP2AUX5MIX Input 1 Source */
+	{ 0x000009B8, 0x0000 },    /* R2488  - DSP2AUX6MIX Input 1 Source */
+	{ 0x000009C0, 0x0000 },    /* R2496  - DSP3LMIX Input 1 Source */
+	{ 0x000009C1, 0x0080 },    /* R2497  - DSP3LMIX Input 1 Volume */
+	{ 0x000009C2, 0x0000 },    /* R2498  - DSP3LMIX Input 2 Source */
+	{ 0x000009C3, 0x0080 },    /* R2499  - DSP3LMIX Input 2 Volume */
+	{ 0x000009C4, 0x0000 },    /* R2500  - DSP3LMIX Input 3 Source */
+	{ 0x000009C5, 0x0080 },    /* R2501  - DSP3LMIX Input 3 Volume */
+	{ 0x000009C6, 0x0000 },    /* R2502  - DSP3LMIX Input 4 Source */
+	{ 0x000009C7, 0x0080 },    /* R2503  - DSP3LMIX Input 4 Volume */
+	{ 0x000009C8, 0x0000 },    /* R2504  - DSP3RMIX Input 1 Source */
+	{ 0x000009C9, 0x0080 },    /* R2505  - DSP3RMIX Input 1 Volume */
+	{ 0x000009CA, 0x0000 },    /* R2506  - DSP3RMIX Input 2 Source */
+	{ 0x000009CB, 0x0080 },    /* R2507  - DSP3RMIX Input 2 Volume */
+	{ 0x000009CC, 0x0000 },    /* R2508  - DSP3RMIX Input 3 Source */
+	{ 0x000009CD, 0x0080 },    /* R2509  - DSP3RMIX Input 3 Volume */
+	{ 0x000009CE, 0x0000 },    /* R2510  - DSP3RMIX Input 4 Source */
+	{ 0x000009CF, 0x0080 },    /* R2511  - DSP3RMIX Input 4 Volume */
+	{ 0x000009D0, 0x0000 },    /* R2512  - DSP3AUX1MIX Input 1 Source */
+	{ 0x000009D8, 0x0000 },    /* R2520  - DSP3AUX2MIX Input 1 Source */
+	{ 0x000009E0, 0x0000 },    /* R2528  - DSP3AUX3MIX Input 1 Source */
+	{ 0x000009E8, 0x0000 },    /* R2536  - DSP3AUX4MIX Input 1 Source */
+	{ 0x000009F0, 0x0000 },    /* R2544  - DSP3AUX5MIX Input 1 Source */
+	{ 0x000009F8, 0x0000 },    /* R2552  - DSP3AUX6MIX Input 1 Source */
+	{ 0x00000A00, 0x0000 },    /* R2560  - DSP4LMIX Input 1 Source */
+	{ 0x00000A01, 0x0080 },    /* R2561  - DSP4LMIX Input 1 Volume */
+	{ 0x00000A02, 0x0000 },    /* R2562  - DSP4LMIX Input 2 Source */
+	{ 0x00000A03, 0x0080 },    /* R2563  - DSP4LMIX Input 2 Volume */
+	{ 0x00000A04, 0x0000 },    /* R2564  - DSP4LMIX Input 3 Source */
+	{ 0x00000A05, 0x0080 },    /* R2565  - DSP4LMIX Input 3 Volume */
+	{ 0x00000A06, 0x0000 },    /* R2566  - DSP4LMIX Input 4 Source */
+	{ 0x00000A07, 0x0080 },    /* R2567  - DSP4LMIX Input 4 Volume */
+	{ 0x00000A08, 0x0000 },    /* R2568  - DSP4RMIX Input 1 Source */
+	{ 0x00000A09, 0x0080 },    /* R2569  - DSP4RMIX Input 1 Volume */
+	{ 0x00000A0A, 0x0000 },    /* R2570  - DSP4RMIX Input 2 Source */
+	{ 0x00000A0B, 0x0080 },    /* R2571  - DSP4RMIX Input 2 Volume */
+	{ 0x00000A0C, 0x0000 },    /* R2572  - DSP4RMIX Input 3 Source */
+	{ 0x00000A0D, 0x0080 },    /* R2573  - DSP4RMIX Input 3 Volume */
+	{ 0x00000A0E, 0x0000 },    /* R2574  - DSP4RMIX Input 4 Source */
+	{ 0x00000A0F, 0x0080 },    /* R2575  - DSP4RMIX Input 4 Volume */
+	{ 0x00000A10, 0x0000 },    /* R2576  - DSP4AUX1MIX Input 1 Source */
+	{ 0x00000A18, 0x0000 },    /* R2584  - DSP4AUX2MIX Input 1 Source */
+	{ 0x00000A20, 0x0000 },    /* R2592  - DSP4AUX3MIX Input 1 Source */
+	{ 0x00000A28, 0x0000 },    /* R2600  - DSP4AUX4MIX Input 1 Source */
+	{ 0x00000A30, 0x0000 },    /* R2608  - DSP4AUX5MIX Input 1 Source */
+	{ 0x00000A38, 0x0000 },    /* R2616  - DSP4AUX6MIX Input 1 Source */
+	{ 0x00000A80, 0x0000 },    /* R2688  - ASRC1LMIX Input 1 Source */
+	{ 0x00000A88, 0x0000 },    /* R2696  - ASRC1RMIX Input 1 Source */
+	{ 0x00000A90, 0x0000 },    /* R2704  - ASRC2LMIX Input 1 Source */
+	{ 0x00000A98, 0x0000 },    /* R2712  - ASRC2RMIX Input 1 Source */
+	{ 0x00000B00, 0x0000 },    /* R2816  - ISRC1DEC1MIX Input 1 Source */
+	{ 0x00000B08, 0x0000 },    /* R2824  - ISRC1DEC2MIX Input 1 Source */
+	{ 0x00000B10, 0x0000 },    /* R2832  - ISRC1DEC3MIX Input 1 Source */
+	{ 0x00000B18, 0x0000 },    /* R2840  - ISRC1DEC4MIX Input 1 Source */
+	{ 0x00000B20, 0x0000 },    /* R2848  - ISRC1INT1MIX Input 1 Source */
+	{ 0x00000B28, 0x0000 },    /* R2856  - ISRC1INT2MIX Input 1 Source */
+	{ 0x00000B30, 0x0000 },    /* R2864  - ISRC1INT3MIX Input 1 Source */
+	{ 0x00000B38, 0x0000 },    /* R2872  - ISRC1INT4MIX Input 1 Source */
+	{ 0x00000B40, 0x0000 },    /* R2880  - ISRC2DEC1MIX Input 1 Source */
+	{ 0x00000B48, 0x0000 },    /* R2888  - ISRC2DEC2MIX Input 1 Source */
+	{ 0x00000B50, 0x0000 },    /* R2896  - ISRC2DEC3MIX Input 1 Source */
+	{ 0x00000B58, 0x0000 },    /* R2904  - ISRC2DEC4MIX Input 1 Source */
+	{ 0x00000B60, 0x0000 },    /* R2912  - ISRC2INT1MIX Input 1 Source */
+	{ 0x00000B68, 0x0000 },    /* R2920  - ISRC2INT2MIX Input 1 Source */
+	{ 0x00000B70, 0x0000 },    /* R2928  - ISRC2INT3MIX Input 1 Source */
+	{ 0x00000B78, 0x0000 },    /* R2936  - ISRC2INT4MIX Input 1 Source */
+	{ 0x00000B80, 0x0000 },    /* R2944  - ISRC3DEC1MIX Input 1 Source */
+	{ 0x00000B88, 0x0000 },    /* R2952  - ISRC3DEC2MIX Input 1 Source */
+	{ 0x00000B90, 0x0000 },    /* R2960  - ISRC3DEC3MIX Input 1 Source */
+	{ 0x00000B98, 0x0000 },    /* R2968  - ISRC3DEC4MIX Input 1 Source */
+	{ 0x00000BA0, 0x0000 },    /* R2976  - ISRC3INT1MIX Input 1 Source */
+	{ 0x00000BA8, 0x0000 },    /* R2984  - ISRC3INT2MIX Input 1 Source */
+	{ 0x00000BB0, 0x0000 },    /* R2992  - ISRC3INT3MIX Input 1 Source */
+	{ 0x00000BB8, 0x0000 },    /* R3000  - ISRC3INT4MIX Input 1 Source */
+	{ 0x00000C00, 0xA101 },    /* R3072  - GPIO1 CTRL */
+	{ 0x00000C01, 0xA101 },    /* R3073  - GPIO2 CTRL */
+	{ 0x00000C02, 0xA101 },    /* R3074  - GPIO3 CTRL */
+	{ 0x00000C03, 0xA101 },    /* R3075  - GPIO4 CTRL */
+	{ 0x00000C04, 0xA101 },    /* R3076  - GPIO5 CTRL */
+	{ 0x00000C0F, 0x0400 },    /* R3087  - IRQ CTRL 1 */
+	{ 0x00000C10, 0x1000 },    /* R3088  - GPIO Debounce Config */
+	{ 0x00000C20, 0x8002 },    /* R3104  - Misc Pad Ctrl 1 */
+	{ 0x00000C21, 0x8001 },    /* R3105  - Misc Pad Ctrl 2 */
+	{ 0x00000C22, 0x0000 },    /* R3106  - Misc Pad Ctrl 3 */
+	{ 0x00000C23, 0x0000 },    /* R3107  - Misc Pad Ctrl 4 */
+	{ 0x00000C24, 0x0000 },    /* R3108  - Misc Pad Ctrl 5 */
+	{ 0x00000C25, 0x0000 },    /* R3109  - Misc Pad Ctrl 6 */
+	{ 0x00000C30, 0x8282 },    /* R3120  - Misc Pad Ctrl 7 */
+	{ 0x00000C31, 0x0082 },    /* R3121  - Misc Pad Ctrl 8 */
+	{ 0x00000C32, 0x8282 },    /* R3122  - Misc Pad Ctrl 9 */
+	{ 0x00000C33, 0x8282 },    /* R3123  - Misc Pad Ctrl 10 */
+	{ 0x00000C34, 0x8282 },    /* R3124  - Misc Pad Ctrl 11 */
+	{ 0x00000C35, 0x8282 },    /* R3125  - Misc Pad Ctrl 12 */
+	{ 0x00000C36, 0x8282 },    /* R3126  - Misc Pad Ctrl 13 */
+	{ 0x00000C37, 0x8282 },    /* R3127  - Misc Pad Ctrl 14 */
+	{ 0x00000C38, 0x8282 },    /* R3128  - Misc Pad Ctrl 15 */
+	{ 0x00000C39, 0x8282 },    /* R3129  - Misc Pad Ctrl 16 */
+	{ 0x00000C3A, 0x8282 },    /* R3130  - Misc Pad Ctrl 17 */
+	{ 0x00000C3B, 0x8282 },    /* R3131  - Misc Pad Ctrl 18 */
+	{ 0x00000D08, 0xFFFF },    /* R3336  - Interrupt Status 1 Mask */
+	{ 0x00000D09, 0xFFFF },    /* R3337  - Interrupt Status 2 Mask */
+	{ 0x00000D0A, 0xFFFF },    /* R3338  - Interrupt Status 3 Mask */
+	{ 0x00000D0B, 0xFFFF },    /* R3339  - Interrupt Status 4 Mask */
+	{ 0x00000D0C, 0xFEFF },    /* R3340  - Interrupt Status 5 Mask */
+	{ 0x00000D0F, 0x0000 },    /* R3343  - Interrupt Control */
+	{ 0x00000D18, 0xFFFF },    /* R3352  - IRQ2 Status 1 Mask */
+	{ 0x00000D19, 0xFFFF },    /* R3353  - IRQ2 Status 2 Mask */
+	{ 0x00000D1A, 0xFFFF },    /* R3354  - IRQ2 Status 3 Mask */
+	{ 0x00000D1B, 0xFFFF },    /* R3355  - IRQ2 Status 4 Mask */
+	{ 0x00000D1C, 0xFFFF },    /* R3356  - IRQ2 Status 5 Mask */
+	{ 0x00000D1F, 0x0000 },    /* R3359  - IRQ2 Control */
+	{ 0x00000D50, 0x0000 },    /* R3408  - AOD wkup and trig */
+	{ 0x00000D53, 0xFFFF },    /* R3411  - AOD IRQ Mask IRQ1 */
+	{ 0x00000D54, 0xFFFF },    /* R3412  - AOD IRQ Mask IRQ2 */
+	{ 0x00000D56, 0x0000 },    /* R3414  - Jack detect debounce */
+	{ 0x00000E00, 0x0000 },    /* R3584  - FX_Ctrl1 */
+	{ 0x00000E01, 0x0000 },    /* R3585  - FX_Ctrl2 */
+	{ 0x00000E10, 0x6318 },    /* R3600  - EQ1_1 */
+	{ 0x00000E11, 0x6300 },    /* R3601  - EQ1_2 */
+	{ 0x00000E12, 0x0FC8 },    /* R3602  - EQ1_3 */
+	{ 0x00000E13, 0x03FE },    /* R3603  - EQ1_4 */
+	{ 0x00000E14, 0x00E0 },    /* R3604  - EQ1_5 */
+	{ 0x00000E15, 0x1EC4 },    /* R3605  - EQ1_6 */
+	{ 0x00000E16, 0xF136 },    /* R3606  - EQ1_7 */
+	{ 0x00000E17, 0x0409 },    /* R3607  - EQ1_8 */
+	{ 0x00000E18, 0x04CC },    /* R3608  - EQ1_9 */
+	{ 0x00000E19, 0x1C9B },    /* R3609  - EQ1_10 */
+	{ 0x00000E1A, 0xF337 },    /* R3610  - EQ1_11 */
+	{ 0x00000E1B, 0x040B },    /* R3611  - EQ1_12 */
+	{ 0x00000E1C, 0x0CBB },    /* R3612  - EQ1_13 */
+	{ 0x00000E1D, 0x16F8 },    /* R3613  - EQ1_14 */
+	{ 0x00000E1E, 0xF7D9 },    /* R3614  - EQ1_15 */
+	{ 0x00000E1F, 0x040A },    /* R3615  - EQ1_16 */
+	{ 0x00000E20, 0x1F14 },    /* R3616  - EQ1_17 */
+	{ 0x00000E21, 0x058C },    /* R3617  - EQ1_18 */
+	{ 0x00000E22, 0x0563 },    /* R3618  - EQ1_19 */
+	{ 0x00000E23, 0x4000 },    /* R3619  - EQ1_20 */
+	{ 0x00000E24, 0x0B75 },    /* R3620  - EQ1_21 */
+	{ 0x00000E26, 0x6318 },    /* R3622  - EQ2_1 */
+	{ 0x00000E27, 0x6300 },    /* R3623  - EQ2_2 */
+	{ 0x00000E28, 0x0FC8 },    /* R3624  - EQ2_3 */
+	{ 0x00000E29, 0x03FE },    /* R3625  - EQ2_4 */
+	{ 0x00000E2A, 0x00E0 },    /* R3626  - EQ2_5 */
+	{ 0x00000E2B, 0x1EC4 },    /* R3627  - EQ2_6 */
+	{ 0x00000E2C, 0xF136 },    /* R3628  - EQ2_7 */
+	{ 0x00000E2D, 0x0409 },    /* R3629  - EQ2_8 */
+	{ 0x00000E2E, 0x04CC },    /* R3630  - EQ2_9 */
+	{ 0x00000E2F, 0x1C9B },    /* R3631  - EQ2_10 */
+	{ 0x00000E30, 0xF337 },    /* R3632  - EQ2_11 */
+	{ 0x00000E31, 0x040B },    /* R3633  - EQ2_12 */
+	{ 0x00000E32, 0x0CBB },    /* R3634  - EQ2_13 */
+	{ 0x00000E33, 0x16F8 },    /* R3635  - EQ2_14 */
+	{ 0x00000E34, 0xF7D9 },    /* R3636  - EQ2_15 */
+	{ 0x00000E35, 0x040A },    /* R3637  - EQ2_16 */
+	{ 0x00000E36, 0x1F14 },    /* R3638  - EQ2_17 */
+	{ 0x00000E37, 0x058C },    /* R3639  - EQ2_18 */
+	{ 0x00000E38, 0x0563 },    /* R3640  - EQ2_19 */
+	{ 0x00000E39, 0x4000 },    /* R3641  - EQ2_20 */
+	{ 0x00000E3A, 0x0B75 },    /* R3642  - EQ2_21 */
+	{ 0x00000E3C, 0x6318 },    /* R3644  - EQ3_1 */
+	{ 0x00000E3D, 0x6300 },    /* R3645  - EQ3_2 */
+	{ 0x00000E3E, 0x0FC8 },    /* R3646  - EQ3_3 */
+	{ 0x00000E3F, 0x03FE },    /* R3647  - EQ3_4 */
+	{ 0x00000E40, 0x00E0 },    /* R3648  - EQ3_5 */
+	{ 0x00000E41, 0x1EC4 },    /* R3649  - EQ3_6 */
+	{ 0x00000E42, 0xF136 },    /* R3650  - EQ3_7 */
+	{ 0x00000E43, 0x0409 },    /* R3651  - EQ3_8 */
+	{ 0x00000E44, 0x04CC },    /* R3652  - EQ3_9 */
+	{ 0x00000E45, 0x1C9B },    /* R3653  - EQ3_10 */
+	{ 0x00000E46, 0xF337 },    /* R3654  - EQ3_11 */
+	{ 0x00000E47, 0x040B },    /* R3655  - EQ3_12 */
+	{ 0x00000E48, 0x0CBB },    /* R3656  - EQ3_13 */
+	{ 0x00000E49, 0x16F8 },    /* R3657  - EQ3_14 */
+	{ 0x00000E4A, 0xF7D9 },    /* R3658  - EQ3_15 */
+	{ 0x00000E4B, 0x040A },    /* R3659  - EQ3_16 */
+	{ 0x00000E4C, 0x1F14 },    /* R3660  - EQ3_17 */
+	{ 0x00000E4D, 0x058C },    /* R3661  - EQ3_18 */
+	{ 0x00000E4E, 0x0563 },    /* R3662  - EQ3_19 */
+	{ 0x00000E4F, 0x4000 },    /* R3663  - EQ3_20 */
+	{ 0x00000E50, 0x0B75 },    /* R3664  - EQ3_21 */
+	{ 0x00000E52, 0x6318 },    /* R3666  - EQ4_1 */
+	{ 0x00000E53, 0x6300 },    /* R3667  - EQ4_2 */
+	{ 0x00000E54, 0x0FC8 },    /* R3668  - EQ4_3 */
+	{ 0x00000E55, 0x03FE },    /* R3669  - EQ4_4 */
+	{ 0x00000E56, 0x00E0 },    /* R3670  - EQ4_5 */
+	{ 0x00000E57, 0x1EC4 },    /* R3671  - EQ4_6 */
+	{ 0x00000E58, 0xF136 },    /* R3672  - EQ4_7 */
+	{ 0x00000E59, 0x0409 },    /* R3673  - EQ4_8 */
+	{ 0x00000E5A, 0x04CC },    /* R3674  - EQ4_9 */
+	{ 0x00000E5B, 0x1C9B },    /* R3675  - EQ4_10 */
+	{ 0x00000E5C, 0xF337 },    /* R3676  - EQ4_11 */
+	{ 0x00000E5D, 0x040B },    /* R3677  - EQ4_12 */
+	{ 0x00000E5E, 0x0CBB },    /* R3678  - EQ4_13 */
+	{ 0x00000E5F, 0x16F8 },    /* R3679  - EQ4_14 */
+	{ 0x00000E60, 0xF7D9 },    /* R3680  - EQ4_15 */
+	{ 0x00000E61, 0x040A },    /* R3681  - EQ4_16 */
+	{ 0x00000E62, 0x1F14 },    /* R3682  - EQ4_17 */
+	{ 0x00000E63, 0x058C },    /* R3683  - EQ4_18 */
+	{ 0x00000E64, 0x0563 },    /* R3684  - EQ4_19 */
+	{ 0x00000E65, 0x4000 },    /* R3685  - EQ4_20 */
+	{ 0x00000E66, 0x0B75 },    /* R3686  - EQ4_21 */
+	{ 0x00000E80, 0x0018 },    /* R3712  - DRC1 ctrl1 */
+	{ 0x00000E81, 0x0933 },    /* R3713  - DRC1 ctrl2 */
+	{ 0x00000E82, 0x0018 },    /* R3714  - DRC1 ctrl3 */
+	{ 0x00000E83, 0x0000 },    /* R3715  - DRC1 ctrl4 */
+	{ 0x00000E84, 0x0000 },    /* R3716  - DRC1 ctrl5 */
+	{ 0x00000E89, 0x0018 },    /* R3721  - DRC2 ctrl1 */
+	{ 0x00000E8A, 0x0933 },    /* R3722  - DRC2 ctrl2 */
+	{ 0x00000E8B, 0x0018 },    /* R3723  - DRC2 ctrl3 */
+	{ 0x00000E8C, 0x0000 },    /* R3724  - DRC2 ctrl4 */
+	{ 0x00000E8D, 0x0000 },    /* R3725  - DRC2 ctrl5 */
+	{ 0x00000EC0, 0x0000 },    /* R3776  - HPLPF1_1 */
+	{ 0x00000EC1, 0x0000 },    /* R3777  - HPLPF1_2 */
+	{ 0x00000EC4, 0x0000 },    /* R3780  - HPLPF2_1 */
+	{ 0x00000EC5, 0x0000 },    /* R3781  - HPLPF2_2 */
+	{ 0x00000EC8, 0x0000 },    /* R3784  - HPLPF3_1 */
+	{ 0x00000EC9, 0x0000 },    /* R3785  - HPLPF3_2 */
+	{ 0x00000ECC, 0x0000 },    /* R3788  - HPLPF4_1 */
+	{ 0x00000ECD, 0x0000 },    /* R3789  - HPLPF4_2 */
+	{ 0x00000EE0, 0x0000 },    /* R3808  - ASRC_ENABLE */
+	{ 0x00000EE2, 0x0000 },    /* R3810  - ASRC_RATE1 */
+	{ 0x00000EF0, 0x0000 },    /* R3824  - ISRC 1 CTRL 1 */
+	{ 0x00000EF1, 0x0000 },    /* R3825  - ISRC 1 CTRL 2 */
+	{ 0x00000EF2, 0x0000 },    /* R3826  - ISRC 1 CTRL 3 */
+	{ 0x00000EF3, 0x0000 },    /* R3827  - ISRC 2 CTRL 1 */
+	{ 0x00000EF4, 0x0000 },    /* R3828  - ISRC 2 CTRL 2 */
+	{ 0x00000EF5, 0x0000 },    /* R3829  - ISRC 2 CTRL 3 */
+	{ 0x00000EF6, 0x0000 },    /* R3830  - ISRC 3 CTRL 1 */
+	{ 0x00000EF7, 0x0000 },    /* R3831  - ISRC 3 CTRL 2 */
+	{ 0x00000EF8, 0x0000 },    /* R3832  - ISRC 3 CTRL 3 */
+	{ 0x00000F00, 0x0000 },    /* R3840  - Clock Control */
+	{ 0x00000F01, 0x0000 },    /* R3841  - ANC_SRC */
+	{ 0x00001100, 0x0010 },    /* R4352  - DSP1 Control 1 */
+	{ 0x00001101, 0x0000 },    /* R4353  - DSP1 Clocking 1 */
+	{ 0x00001200, 0x0010 },    /* R4608  - DSP2 Control 1 */
+	{ 0x00001201, 0x0000 },    /* R4609  - DSP2 Clocking 1 */
+	{ 0x00001300, 0x0010 },    /* R4864  - DSP3 Control 1 */
+	{ 0x00001301, 0x0000 },    /* R4865  - DSP3 Clocking 1 */
+	{ 0x00001400, 0x0010 },    /* R5120  - DSP4 Control 1 */
+	{ 0x00001401, 0x0000 },    /* R5121  - DSP4 Clocking 1 */
+	{ 0x00001404, 0x0000 },    /* R5124  - DSP4 Status 1 */
+};
+
+static bool wm5110_readable_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ARIZONA_SOFTWARE_RESET:
+	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_CTRL_IF_SPI_CFG_1:
+	case ARIZONA_CTRL_IF_I2C1_CFG_1:
+	case ARIZONA_CTRL_IF_I2C2_CFG_1:
+	case ARIZONA_CTRL_IF_I2C1_CFG_2:
+	case ARIZONA_CTRL_IF_I2C2_CFG_2:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
+	case ARIZONA_TONE_GENERATOR_1:
+	case ARIZONA_TONE_GENERATOR_2:
+	case ARIZONA_TONE_GENERATOR_3:
+	case ARIZONA_TONE_GENERATOR_4:
+	case ARIZONA_TONE_GENERATOR_5:
+	case ARIZONA_PWM_DRIVE_1:
+	case ARIZONA_PWM_DRIVE_2:
+	case ARIZONA_PWM_DRIVE_3:
+	case ARIZONA_WAKE_CONTROL:
+	case ARIZONA_SEQUENCE_CONTROL:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_1:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_2:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_3:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_4:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_1:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_2:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_3:
+	case ARIZONA_ALWAYS_ON_TRIGGERS_SEQUENCE_SELECT_4:
+	case ARIZONA_COMFORT_NOISE_GENERATOR:
+	case ARIZONA_HAPTICS_CONTROL_1:
+	case ARIZONA_HAPTICS_CONTROL_2:
+	case ARIZONA_HAPTICS_PHASE_1_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_1_DURATION:
+	case ARIZONA_HAPTICS_PHASE_2_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_2_DURATION:
+	case ARIZONA_HAPTICS_PHASE_3_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_3_DURATION:
+	case ARIZONA_HAPTICS_STATUS:
+	case ARIZONA_CLOCK_32K_1:
+	case ARIZONA_SYSTEM_CLOCK_1:
+	case ARIZONA_SAMPLE_RATE_1:
+	case ARIZONA_SAMPLE_RATE_2:
+	case ARIZONA_SAMPLE_RATE_3:
+	case ARIZONA_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_SAMPLE_RATE_3_STATUS:
+	case ARIZONA_ASYNC_CLOCK_1:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_OUTPUT_SYSTEM_CLOCK:
+	case ARIZONA_OUTPUT_ASYNC_CLOCK:
+	case ARIZONA_RATE_ESTIMATOR_1:
+	case ARIZONA_RATE_ESTIMATOR_2:
+	case ARIZONA_RATE_ESTIMATOR_3:
+	case ARIZONA_RATE_ESTIMATOR_4:
+	case ARIZONA_RATE_ESTIMATOR_5:
+	case ARIZONA_FLL1_CONTROL_1:
+	case ARIZONA_FLL1_CONTROL_2:
+	case ARIZONA_FLL1_CONTROL_3:
+	case ARIZONA_FLL1_CONTROL_4:
+	case ARIZONA_FLL1_CONTROL_5:
+	case ARIZONA_FLL1_CONTROL_6:
+	case ARIZONA_FLL1_LOOP_FILTER_TEST_1:
+	case ARIZONA_FLL1_NCO_TEST_0:
+	case ARIZONA_FLL1_SYNCHRONISER_1:
+	case ARIZONA_FLL1_SYNCHRONISER_2:
+	case ARIZONA_FLL1_SYNCHRONISER_3:
+	case ARIZONA_FLL1_SYNCHRONISER_4:
+	case ARIZONA_FLL1_SYNCHRONISER_5:
+	case ARIZONA_FLL1_SYNCHRONISER_6:
+	case ARIZONA_FLL1_SPREAD_SPECTRUM:
+	case ARIZONA_FLL1_GPIO_CLOCK:
+	case ARIZONA_FLL2_CONTROL_1:
+	case ARIZONA_FLL2_CONTROL_2:
+	case ARIZONA_FLL2_CONTROL_3:
+	case ARIZONA_FLL2_CONTROL_4:
+	case ARIZONA_FLL2_CONTROL_5:
+	case ARIZONA_FLL2_CONTROL_6:
+	case ARIZONA_FLL2_LOOP_FILTER_TEST_1:
+	case ARIZONA_FLL2_NCO_TEST_0:
+	case ARIZONA_FLL2_SYNCHRONISER_1:
+	case ARIZONA_FLL2_SYNCHRONISER_2:
+	case ARIZONA_FLL2_SYNCHRONISER_3:
+	case ARIZONA_FLL2_SYNCHRONISER_4:
+	case ARIZONA_FLL2_SYNCHRONISER_5:
+	case ARIZONA_FLL2_SYNCHRONISER_6:
+	case ARIZONA_FLL2_SPREAD_SPECTRUM:
+	case ARIZONA_FLL2_GPIO_CLOCK:
+	case ARIZONA_MIC_CHARGE_PUMP_1:
+	case ARIZONA_LDO1_CONTROL_1:
+	case ARIZONA_LDO2_CONTROL_1:
+	case ARIZONA_MIC_BIAS_CTRL_1:
+	case ARIZONA_MIC_BIAS_CTRL_2:
+	case ARIZONA_MIC_BIAS_CTRL_3:
+	case ARIZONA_ACCESSORY_DETECT_MODE_1:
+	case ARIZONA_HEADPHONE_DETECT_1:
+	case ARIZONA_HEADPHONE_DETECT_2:
+	case ARIZONA_MIC_DETECT_1:
+	case ARIZONA_MIC_DETECT_2:
+	case ARIZONA_MIC_DETECT_3:
+	case ARIZONA_MIC_NOISE_MIX_CONTROL_1:
+	case ARIZONA_JACK_DETECT_ANALOGUE:
+	case ARIZONA_INPUT_ENABLES:
+	case ARIZONA_INPUT_ENABLES_STATUS:
+	case ARIZONA_INPUT_RATE:
+	case ARIZONA_INPUT_VOLUME_RAMP:
+	case ARIZONA_IN1L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_1L:
+	case ARIZONA_DMIC1L_CONTROL:
+	case ARIZONA_IN1R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_1R:
+	case ARIZONA_DMIC1R_CONTROL:
+	case ARIZONA_IN2L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_2L:
+	case ARIZONA_DMIC2L_CONTROL:
+	case ARIZONA_IN2R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_2R:
+	case ARIZONA_DMIC2R_CONTROL:
+	case ARIZONA_IN3L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_3L:
+	case ARIZONA_DMIC3L_CONTROL:
+	case ARIZONA_IN3R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_3R:
+	case ARIZONA_DMIC3R_CONTROL:
+	case ARIZONA_IN4L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_4L:
+	case ARIZONA_DMIC4L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_4R:
+	case ARIZONA_DMIC4R_CONTROL:
+	case ARIZONA_OUTPUT_ENABLES_1:
+	case ARIZONA_OUTPUT_STATUS_1:
+	case ARIZONA_RAW_OUTPUT_STATUS_1:
+	case ARIZONA_OUTPUT_RATE_1:
+	case ARIZONA_OUTPUT_VOLUME_RAMP:
+	case ARIZONA_OUTPUT_PATH_CONFIG_1L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_1L:
+	case ARIZONA_DAC_VOLUME_LIMIT_1L:
+	case ARIZONA_NOISE_GATE_SELECT_1L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_1R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_1R:
+	case ARIZONA_DAC_VOLUME_LIMIT_1R:
+	case ARIZONA_NOISE_GATE_SELECT_1R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_2L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_2L:
+	case ARIZONA_DAC_VOLUME_LIMIT_2L:
+	case ARIZONA_NOISE_GATE_SELECT_2L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_2R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_2R:
+	case ARIZONA_DAC_VOLUME_LIMIT_2R:
+	case ARIZONA_NOISE_GATE_SELECT_2R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_3L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_3L:
+	case ARIZONA_DAC_VOLUME_LIMIT_3L:
+	case ARIZONA_NOISE_GATE_SELECT_3L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_3R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_3R:
+	case ARIZONA_DAC_VOLUME_LIMIT_3R:
+	case ARIZONA_NOISE_GATE_SELECT_3R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_4L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_4L:
+	case ARIZONA_OUT_VOLUME_4L:
+	case ARIZONA_NOISE_GATE_SELECT_4L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_4R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_4R:
+	case ARIZONA_OUT_VOLUME_4R:
+	case ARIZONA_NOISE_GATE_SELECT_4R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_5L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_5L:
+	case ARIZONA_DAC_VOLUME_LIMIT_5L:
+	case ARIZONA_NOISE_GATE_SELECT_5L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_5R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_5R:
+	case ARIZONA_DAC_VOLUME_LIMIT_5R:
+	case ARIZONA_NOISE_GATE_SELECT_5R:
+	case ARIZONA_OUTPUT_PATH_CONFIG_6L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_6L:
+	case ARIZONA_DAC_VOLUME_LIMIT_6L:
+	case ARIZONA_NOISE_GATE_SELECT_6L:
+	case ARIZONA_OUTPUT_PATH_CONFIG_6R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_6R:
+	case ARIZONA_DAC_VOLUME_LIMIT_6R:
+	case ARIZONA_NOISE_GATE_SELECT_6R:
+	case ARIZONA_DAC_AEC_CONTROL_1:
+	case ARIZONA_NOISE_GATE_CONTROL:
+	case ARIZONA_PDM_SPK1_CTRL_1:
+	case ARIZONA_PDM_SPK1_CTRL_2:
+	case ARIZONA_PDM_SPK2_CTRL_1:
+	case ARIZONA_PDM_SPK2_CTRL_2:
+	case ARIZONA_AIF1_BCLK_CTRL:
+	case ARIZONA_AIF1_TX_PIN_CTRL:
+	case ARIZONA_AIF1_RX_PIN_CTRL:
+	case ARIZONA_AIF1_RATE_CTRL:
+	case ARIZONA_AIF1_FORMAT:
+	case ARIZONA_AIF1_TX_BCLK_RATE:
+	case ARIZONA_AIF1_RX_BCLK_RATE:
+	case ARIZONA_AIF1_FRAME_CTRL_1:
+	case ARIZONA_AIF1_FRAME_CTRL_2:
+	case ARIZONA_AIF1_FRAME_CTRL_3:
+	case ARIZONA_AIF1_FRAME_CTRL_4:
+	case ARIZONA_AIF1_FRAME_CTRL_5:
+	case ARIZONA_AIF1_FRAME_CTRL_6:
+	case ARIZONA_AIF1_FRAME_CTRL_7:
+	case ARIZONA_AIF1_FRAME_CTRL_8:
+	case ARIZONA_AIF1_FRAME_CTRL_9:
+	case ARIZONA_AIF1_FRAME_CTRL_10:
+	case ARIZONA_AIF1_FRAME_CTRL_11:
+	case ARIZONA_AIF1_FRAME_CTRL_12:
+	case ARIZONA_AIF1_FRAME_CTRL_13:
+	case ARIZONA_AIF1_FRAME_CTRL_14:
+	case ARIZONA_AIF1_FRAME_CTRL_15:
+	case ARIZONA_AIF1_FRAME_CTRL_16:
+	case ARIZONA_AIF1_FRAME_CTRL_17:
+	case ARIZONA_AIF1_FRAME_CTRL_18:
+	case ARIZONA_AIF1_TX_ENABLES:
+	case ARIZONA_AIF1_RX_ENABLES:
+	case ARIZONA_AIF2_BCLK_CTRL:
+	case ARIZONA_AIF2_TX_PIN_CTRL:
+	case ARIZONA_AIF2_RX_PIN_CTRL:
+	case ARIZONA_AIF2_RATE_CTRL:
+	case ARIZONA_AIF2_FORMAT:
+	case ARIZONA_AIF2_TX_BCLK_RATE:
+	case ARIZONA_AIF2_RX_BCLK_RATE:
+	case ARIZONA_AIF2_FRAME_CTRL_1:
+	case ARIZONA_AIF2_FRAME_CTRL_2:
+	case ARIZONA_AIF2_FRAME_CTRL_3:
+	case ARIZONA_AIF2_FRAME_CTRL_4:
+	case ARIZONA_AIF2_FRAME_CTRL_11:
+	case ARIZONA_AIF2_FRAME_CTRL_12:
+	case ARIZONA_AIF2_TX_ENABLES:
+	case ARIZONA_AIF2_RX_ENABLES:
+	case ARIZONA_AIF3_BCLK_CTRL:
+	case ARIZONA_AIF3_TX_PIN_CTRL:
+	case ARIZONA_AIF3_RX_PIN_CTRL:
+	case ARIZONA_AIF3_RATE_CTRL:
+	case ARIZONA_AIF3_FORMAT:
+	case ARIZONA_AIF3_TX_BCLK_RATE:
+	case ARIZONA_AIF3_RX_BCLK_RATE:
+	case ARIZONA_AIF3_FRAME_CTRL_1:
+	case ARIZONA_AIF3_FRAME_CTRL_2:
+	case ARIZONA_AIF3_FRAME_CTRL_3:
+	case ARIZONA_AIF3_FRAME_CTRL_4:
+	case ARIZONA_AIF3_FRAME_CTRL_11:
+	case ARIZONA_AIF3_FRAME_CTRL_12:
+	case ARIZONA_AIF3_TX_ENABLES:
+	case ARIZONA_AIF3_RX_ENABLES:
+	case ARIZONA_SLIMBUS_FRAMER_REF_GEAR:
+	case ARIZONA_SLIMBUS_RATES_1:
+	case ARIZONA_SLIMBUS_RATES_2:
+	case ARIZONA_SLIMBUS_RATES_3:
+	case ARIZONA_SLIMBUS_RATES_4:
+	case ARIZONA_SLIMBUS_RATES_5:
+	case ARIZONA_SLIMBUS_RATES_6:
+	case ARIZONA_SLIMBUS_RATES_7:
+	case ARIZONA_SLIMBUS_RATES_8:
+	case ARIZONA_SLIMBUS_RX_CHANNEL_ENABLE:
+	case ARIZONA_SLIMBUS_TX_CHANNEL_ENABLE:
+	case ARIZONA_SLIMBUS_RX_PORT_STATUS:
+	case ARIZONA_SLIMBUS_TX_PORT_STATUS:
+	case ARIZONA_PWM1MIX_INPUT_1_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_1_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_2_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_2_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_3_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_3_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_4_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_4_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_1_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_1_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_2_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_2_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_3_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_3_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_4_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_4_VOLUME:
+	case ARIZONA_MICMIX_INPUT_1_SOURCE:
+	case ARIZONA_MICMIX_INPUT_1_VOLUME:
+	case ARIZONA_MICMIX_INPUT_2_SOURCE:
+	case ARIZONA_MICMIX_INPUT_2_VOLUME:
+	case ARIZONA_MICMIX_INPUT_3_SOURCE:
+	case ARIZONA_MICMIX_INPUT_3_VOLUME:
+	case ARIZONA_MICMIX_INPUT_4_SOURCE:
+	case ARIZONA_MICMIX_INPUT_4_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_1_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_1_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_2_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_2_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_3_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_3_VOLUME:
+	case ARIZONA_NOISEMIX_INPUT_4_SOURCE:
+	case ARIZONA_NOISEMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT3LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT3LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT3RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT3RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT3RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT3RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT3RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT3RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT3RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT3RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT4RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT4RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT5LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT5LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT5RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT5RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT6LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT6LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT6LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT6LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT6LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT6LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT6LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT6LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT6RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT6RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT6RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT6RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT6RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT6RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT6RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT6RMIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX3MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX3MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX4MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX4MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX5MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX5MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX6MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX6MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX7MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX7MIX_INPUT_4_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_1_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_1_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_2_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_2_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_3_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_3_VOLUME:
+	case ARIZONA_SLIMTX8MIX_INPUT_4_SOURCE:
+	case ARIZONA_SLIMTX8MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ3MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ3MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ4MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ4MIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP1AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP1AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP2AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP3LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP3LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP3LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP3AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP4LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP4LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP4LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP4LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP4LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP4LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP4RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP4RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP4RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP4RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP4RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP4RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP4RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP4AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP4AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT4MIX_INPUT_1_SOURCE:
+	case ARIZONA_GPIO1_CTRL:
+	case ARIZONA_GPIO2_CTRL:
+	case ARIZONA_GPIO3_CTRL:
+	case ARIZONA_GPIO4_CTRL:
+	case ARIZONA_GPIO5_CTRL:
+	case ARIZONA_IRQ_CTRL_1:
+	case ARIZONA_GPIO_DEBOUNCE_CONFIG:
+	case ARIZONA_MISC_PAD_CTRL_1:
+	case ARIZONA_MISC_PAD_CTRL_2:
+	case ARIZONA_MISC_PAD_CTRL_3:
+	case ARIZONA_MISC_PAD_CTRL_4:
+	case ARIZONA_MISC_PAD_CTRL_5:
+	case ARIZONA_MISC_PAD_CTRL_6:
+	case ARIZONA_MISC_PAD_CTRL_7:
+	case ARIZONA_MISC_PAD_CTRL_8:
+	case ARIZONA_MISC_PAD_CTRL_9:
+	case ARIZONA_MISC_PAD_CTRL_10:
+	case ARIZONA_MISC_PAD_CTRL_11:
+	case ARIZONA_MISC_PAD_CTRL_12:
+	case ARIZONA_MISC_PAD_CTRL_13:
+	case ARIZONA_MISC_PAD_CTRL_14:
+	case ARIZONA_MISC_PAD_CTRL_15:
+	case ARIZONA_MISC_PAD_CTRL_16:
+	case ARIZONA_MISC_PAD_CTRL_17:
+	case ARIZONA_MISC_PAD_CTRL_18:
+	case ARIZONA_INTERRUPT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_2:
+	case ARIZONA_INTERRUPT_STATUS_3:
+	case ARIZONA_INTERRUPT_STATUS_4:
+	case ARIZONA_INTERRUPT_STATUS_5:
+	case ARIZONA_INTERRUPT_STATUS_1_MASK:
+	case ARIZONA_INTERRUPT_STATUS_2_MASK:
+	case ARIZONA_INTERRUPT_STATUS_3_MASK:
+	case ARIZONA_INTERRUPT_STATUS_4_MASK:
+	case ARIZONA_INTERRUPT_STATUS_5_MASK:
+	case ARIZONA_INTERRUPT_CONTROL:
+	case ARIZONA_IRQ2_STATUS_1:
+	case ARIZONA_IRQ2_STATUS_2:
+	case ARIZONA_IRQ2_STATUS_3:
+	case ARIZONA_IRQ2_STATUS_4:
+	case ARIZONA_IRQ2_STATUS_5:
+	case ARIZONA_IRQ2_STATUS_1_MASK:
+	case ARIZONA_IRQ2_STATUS_2_MASK:
+	case ARIZONA_IRQ2_STATUS_3_MASK:
+	case ARIZONA_IRQ2_STATUS_4_MASK:
+	case ARIZONA_IRQ2_STATUS_5_MASK:
+	case ARIZONA_IRQ2_CONTROL:
+	case ARIZONA_INTERRUPT_RAW_STATUS_2:
+	case ARIZONA_INTERRUPT_RAW_STATUS_3:
+	case ARIZONA_INTERRUPT_RAW_STATUS_4:
+	case ARIZONA_INTERRUPT_RAW_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_7:
+	case ARIZONA_INTERRUPT_RAW_STATUS_8:
+	case ARIZONA_IRQ_PIN_STATUS:
+	case ARIZONA_AOD_WKUP_AND_TRIG:
+	case ARIZONA_AOD_IRQ1:
+	case ARIZONA_AOD_IRQ2:
+	case ARIZONA_AOD_IRQ_MASK_IRQ1:
+	case ARIZONA_AOD_IRQ_MASK_IRQ2:
+	case ARIZONA_AOD_IRQ_RAW_STATUS:
+	case ARIZONA_JACK_DETECT_DEBOUNCE:
+	case ARIZONA_FX_CTRL1:
+	case ARIZONA_FX_CTRL2:
+	case ARIZONA_EQ1_1:
+	case ARIZONA_EQ1_2:
+	case ARIZONA_EQ1_3:
+	case ARIZONA_EQ1_4:
+	case ARIZONA_EQ1_5:
+	case ARIZONA_EQ1_6:
+	case ARIZONA_EQ1_7:
+	case ARIZONA_EQ1_8:
+	case ARIZONA_EQ1_9:
+	case ARIZONA_EQ1_10:
+	case ARIZONA_EQ1_11:
+	case ARIZONA_EQ1_12:
+	case ARIZONA_EQ1_13:
+	case ARIZONA_EQ1_14:
+	case ARIZONA_EQ1_15:
+	case ARIZONA_EQ1_16:
+	case ARIZONA_EQ1_17:
+	case ARIZONA_EQ1_18:
+	case ARIZONA_EQ1_19:
+	case ARIZONA_EQ1_20:
+	case ARIZONA_EQ1_21:
+	case ARIZONA_EQ2_1:
+	case ARIZONA_EQ2_2:
+	case ARIZONA_EQ2_3:
+	case ARIZONA_EQ2_4:
+	case ARIZONA_EQ2_5:
+	case ARIZONA_EQ2_6:
+	case ARIZONA_EQ2_7:
+	case ARIZONA_EQ2_8:
+	case ARIZONA_EQ2_9:
+	case ARIZONA_EQ2_10:
+	case ARIZONA_EQ2_11:
+	case ARIZONA_EQ2_12:
+	case ARIZONA_EQ2_13:
+	case ARIZONA_EQ2_14:
+	case ARIZONA_EQ2_15:
+	case ARIZONA_EQ2_16:
+	case ARIZONA_EQ2_17:
+	case ARIZONA_EQ2_18:
+	case ARIZONA_EQ2_19:
+	case ARIZONA_EQ2_20:
+	case ARIZONA_EQ2_21:
+	case ARIZONA_EQ3_1:
+	case ARIZONA_EQ3_2:
+	case ARIZONA_EQ3_3:
+	case ARIZONA_EQ3_4:
+	case ARIZONA_EQ3_5:
+	case ARIZONA_EQ3_6:
+	case ARIZONA_EQ3_7:
+	case ARIZONA_EQ3_8:
+	case ARIZONA_EQ3_9:
+	case ARIZONA_EQ3_10:
+	case ARIZONA_EQ3_11:
+	case ARIZONA_EQ3_12:
+	case ARIZONA_EQ3_13:
+	case ARIZONA_EQ3_14:
+	case ARIZONA_EQ3_15:
+	case ARIZONA_EQ3_16:
+	case ARIZONA_EQ3_17:
+	case ARIZONA_EQ3_18:
+	case ARIZONA_EQ3_19:
+	case ARIZONA_EQ3_20:
+	case ARIZONA_EQ3_21:
+	case ARIZONA_EQ4_1:
+	case ARIZONA_EQ4_2:
+	case ARIZONA_EQ4_3:
+	case ARIZONA_EQ4_4:
+	case ARIZONA_EQ4_5:
+	case ARIZONA_EQ4_6:
+	case ARIZONA_EQ4_7:
+	case ARIZONA_EQ4_8:
+	case ARIZONA_EQ4_9:
+	case ARIZONA_EQ4_10:
+	case ARIZONA_EQ4_11:
+	case ARIZONA_EQ4_12:
+	case ARIZONA_EQ4_13:
+	case ARIZONA_EQ4_14:
+	case ARIZONA_EQ4_15:
+	case ARIZONA_EQ4_16:
+	case ARIZONA_EQ4_17:
+	case ARIZONA_EQ4_18:
+	case ARIZONA_EQ4_19:
+	case ARIZONA_EQ4_20:
+	case ARIZONA_EQ4_21:
+	case ARIZONA_DRC1_CTRL1:
+	case ARIZONA_DRC1_CTRL2:
+	case ARIZONA_DRC1_CTRL3:
+	case ARIZONA_DRC1_CTRL4:
+	case ARIZONA_DRC1_CTRL5:
+	case ARIZONA_DRC2_CTRL1:
+	case ARIZONA_DRC2_CTRL2:
+	case ARIZONA_DRC2_CTRL3:
+	case ARIZONA_DRC2_CTRL4:
+	case ARIZONA_DRC2_CTRL5:
+	case ARIZONA_HPLPF1_1:
+	case ARIZONA_HPLPF1_2:
+	case ARIZONA_HPLPF2_1:
+	case ARIZONA_HPLPF2_2:
+	case ARIZONA_HPLPF3_1:
+	case ARIZONA_HPLPF3_2:
+	case ARIZONA_HPLPF4_1:
+	case ARIZONA_HPLPF4_2:
+	case ARIZONA_ASRC_ENABLE:
+	case ARIZONA_ASRC_STATUS:
+	case ARIZONA_ASRC_RATE1:
+	case ARIZONA_ISRC_1_CTRL_1:
+	case ARIZONA_ISRC_1_CTRL_2:
+	case ARIZONA_ISRC_1_CTRL_3:
+	case ARIZONA_ISRC_2_CTRL_1:
+	case ARIZONA_ISRC_2_CTRL_2:
+	case ARIZONA_ISRC_2_CTRL_3:
+	case ARIZONA_ISRC_3_CTRL_1:
+	case ARIZONA_ISRC_3_CTRL_2:
+	case ARIZONA_ISRC_3_CTRL_3:
+	case ARIZONA_CLOCK_CONTROL:
+	case ARIZONA_ANC_SRC:
+	case ARIZONA_DSP_STATUS:
+	case ARIZONA_DSP1_CONTROL_1:
+	case ARIZONA_DSP1_CLOCKING_1:
+	case ARIZONA_DSP1_STATUS_1:
+	case ARIZONA_DSP1_STATUS_2:
+	case ARIZONA_DSP2_CONTROL_1:
+	case ARIZONA_DSP2_CLOCKING_1:
+	case ARIZONA_DSP2_STATUS_1:
+	case ARIZONA_DSP2_STATUS_2:
+	case ARIZONA_DSP3_CONTROL_1:
+	case ARIZONA_DSP3_CLOCKING_1:
+	case ARIZONA_DSP3_STATUS_1:
+	case ARIZONA_DSP3_STATUS_2:
+	case ARIZONA_DSP4_CONTROL_1:
+	case ARIZONA_DSP4_CLOCKING_1:
+	case ARIZONA_DSP4_STATUS_1:
+	case ARIZONA_DSP4_STATUS_2:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool wm5110_volatile_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ARIZONA_SOFTWARE_RESET:
+	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_HAPTICS_STATUS:
+	case ARIZONA_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_SAMPLE_RATE_3_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_MIC_DETECT_3:
+	case ARIZONA_HEADPHONE_DETECT_2:
+	case ARIZONA_INPUT_ENABLES_STATUS:
+	case ARIZONA_OUTPUT_STATUS_1:
+	case ARIZONA_RAW_OUTPUT_STATUS_1:
+	case ARIZONA_SLIMBUS_RX_PORT_STATUS:
+	case ARIZONA_SLIMBUS_TX_PORT_STATUS:
+	case ARIZONA_INTERRUPT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_2:
+	case ARIZONA_INTERRUPT_STATUS_3:
+	case ARIZONA_INTERRUPT_STATUS_4:
+	case ARIZONA_INTERRUPT_STATUS_5:
+	case ARIZONA_IRQ2_STATUS_1:
+	case ARIZONA_IRQ2_STATUS_2:
+	case ARIZONA_IRQ2_STATUS_3:
+	case ARIZONA_IRQ2_STATUS_4:
+	case ARIZONA_IRQ2_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_2:
+	case ARIZONA_INTERRUPT_RAW_STATUS_3:
+	case ARIZONA_INTERRUPT_RAW_STATUS_4:
+	case ARIZONA_INTERRUPT_RAW_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_7:
+	case ARIZONA_INTERRUPT_RAW_STATUS_8:
+	case ARIZONA_IRQ_PIN_STATUS:
+	case ARIZONA_AOD_IRQ1:
+	case ARIZONA_AOD_IRQ2:
+	case ARIZONA_ASRC_STATUS:
+	case ARIZONA_DSP_STATUS:
+	case ARIZONA_DSP1_CONTROL_1:
+	case ARIZONA_DSP1_CLOCKING_1:
+	case ARIZONA_DSP1_STATUS_1:
+	case ARIZONA_DSP1_STATUS_2:
+	case ARIZONA_DSP2_STATUS_1:
+	case ARIZONA_DSP2_STATUS_2:
+	case ARIZONA_DSP3_STATUS_1:
+	case ARIZONA_DSP3_STATUS_2:
+	case ARIZONA_DSP4_STATUS_1:
+	case ARIZONA_DSP4_STATUS_2:
+		return true;
+	default:
+		return false;
+	}
+}
+
+const struct regmap_config wm5110_spi_regmap = {
+	.reg_bits = 32,
+	.pad_bits = 16,
+	.val_bits = 16,
+
+	.max_register = ARIZONA_DSP1_STATUS_2,
+	.readable_reg = wm5110_readable_register,
+	.volatile_reg = wm5110_volatile_register,
+
+	.cache_type = REGCACHE_RBTREE,
+	.reg_defaults = wm5110_reg_default,
+	.num_reg_defaults = ARRAY_SIZE(wm5110_reg_default),
+};
+EXPORT_SYMBOL_GPL(wm5110_spi_regmap);
+
+const struct regmap_config wm5110_i2c_regmap = {
+	.reg_bits = 32,
+	.val_bits = 16,
+
+	.max_register = ARIZONA_DSP1_STATUS_2,
+	.readable_reg = wm5110_readable_register,
+	.volatile_reg = wm5110_volatile_register,
+
+	.cache_type = REGCACHE_RBTREE,
+	.reg_defaults = wm5110_reg_default,
+	.num_reg_defaults = ARRAY_SIZE(wm5110_reg_default),
+};
+EXPORT_SYMBOL_GPL(wm5110_i2c_regmap);
diff --git a/drivers/mfd/wm831x-otp.c b/drivers/mfd/wm831x-otp.c
index f742745ff35..b90f3e06b6c 100644
--- a/drivers/mfd/wm831x-otp.c
+++ b/drivers/mfd/wm831x-otp.c
@@ -18,6 +18,7 @@
 #include <linux/bcd.h>
 #include <linux/delay.h>
 #include <linux/mfd/core.h>
+#include <linux/random.h>
 
 #include <linux/mfd/wm831x/core.h>
 #include <linux/mfd/wm831x/otp.h>
@@ -66,6 +67,7 @@ static DEVICE_ATTR(unique_id, 0444, wm831x_unique_id_show, NULL);
 
 int wm831x_otp_init(struct wm831x *wm831x)
 {
+	char uuid[WM831X_UNIQUE_ID_LEN];
 	int ret;
 
 	ret = device_create_file(wm831x->dev, &dev_attr_unique_id);
@@ -73,6 +75,12 @@ int wm831x_otp_init(struct wm831x *wm831x)
 		dev_err(wm831x->dev, "Unique ID attribute not created: %d\n",
 			ret);
 
+	ret = wm831x_unique_id_read(wm831x, uuid);
+	if (ret == 0)
+		add_device_randomness(uuid, sizeof(uuid));
+	else
+		dev_err(wm831x->dev, "Failed to read UUID: %d\n", ret);
+
 	return ret;
 }
 
diff --git a/drivers/mfd/wm8350-core.c b/drivers/mfd/wm8350-core.c
index 8a9b11ca076..7c1ae24605d 100644
--- a/drivers/mfd/wm8350-core.c
+++ b/drivers/mfd/wm8350-core.c
@@ -32,9 +32,6 @@
 #include <linux/mfd/wm8350/supply.h>
 #include <linux/mfd/wm8350/wdt.h>
 
-#define WM8350_UNLOCK_KEY		0x0013
-#define WM8350_LOCK_KEY			0x0000
-
 #define WM8350_CLOCK_CONTROL_1		0x28
 #define WM8350_AIF_TEST			0x74
 
@@ -63,181 +60,32 @@
 /*
  * WM8350 Device IO
  */
-static DEFINE_MUTEX(io_mutex);
 static DEFINE_MUTEX(reg_lock_mutex);
 
-/* Perform a physical read from the device.
- */
-static int wm8350_phys_read(struct wm8350 *wm8350, u8 reg, int num_regs,
-			    u16 *dest)
-{
-	int i, ret;
-	int bytes = num_regs * 2;
-
-	dev_dbg(wm8350->dev, "volatile read\n");
-	ret = regmap_raw_read(wm8350->regmap, reg, dest, bytes);
-
-	for (i = reg; i < reg + num_regs; i++) {
-		/* Cache is CPU endian */
-		dest[i - reg] = be16_to_cpu(dest[i - reg]);
-
-		/* Mask out non-readable bits */
-		dest[i - reg] &= wm8350_reg_io_map[i].readable;
-	}
-
-	dump(num_regs, dest);
-
-	return ret;
-}
-
-static int wm8350_read(struct wm8350 *wm8350, u8 reg, int num_regs, u16 *dest)
-{
-	int i;
-	int end = reg + num_regs;
-	int ret = 0;
-	int bytes = num_regs * 2;
-
-	if ((reg + num_regs - 1) > WM8350_MAX_REGISTER) {
-		dev_err(wm8350->dev, "invalid reg %x\n",
-			reg + num_regs - 1);
-		return -EINVAL;
-	}
-
-	dev_dbg(wm8350->dev,
-		"%s R%d(0x%2.2x) %d regs\n", __func__, reg, reg, num_regs);
-
-#if WM8350_BUS_DEBUG
-	/* we can _safely_ read any register, but warn if read not supported */
-	for (i = reg; i < end; i++) {
-		if (!wm8350_reg_io_map[i].readable)
-			dev_warn(wm8350->dev,
-				"reg R%d is not readable\n", i);
-	}
-#endif
-
-	/* if any volatile registers are required, then read back all */
-	for (i = reg; i < end; i++)
-		if (wm8350_reg_io_map[i].vol)
-			return wm8350_phys_read(wm8350, reg, num_regs, dest);
-
-	/* no volatiles, then cache is good */
-	dev_dbg(wm8350->dev, "cache read\n");
-	memcpy(dest, &wm8350->reg_cache[reg], bytes);
-	dump(num_regs, dest);
-	return ret;
-}
-
-static inline int is_reg_locked(struct wm8350 *wm8350, u8 reg)
-{
-	if (reg == WM8350_SECURITY ||
-	    wm8350->reg_cache[WM8350_SECURITY] == WM8350_UNLOCK_KEY)
-		return 0;
-
-	if ((reg >= WM8350_GPIO_FUNCTION_SELECT_1 &&
-	     reg <= WM8350_GPIO_FUNCTION_SELECT_4) ||
-	    (reg >= WM8350_BATTERY_CHARGER_CONTROL_1 &&
-	     reg <= WM8350_BATTERY_CHARGER_CONTROL_3))
-		return 1;
-	return 0;
-}
-
-static int wm8350_write(struct wm8350 *wm8350, u8 reg, int num_regs, u16 *src)
-{
-	int i;
-	int end = reg + num_regs;
-	int bytes = num_regs * 2;
-
-	if ((reg + num_regs - 1) > WM8350_MAX_REGISTER) {
-		dev_err(wm8350->dev, "invalid reg %x\n",
-			reg + num_regs - 1);
-		return -EINVAL;
-	}
-
-	/* it's generally not a good idea to write to RO or locked registers */
-	for (i = reg; i < end; i++) {
-		if (!wm8350_reg_io_map[i].writable) {
-			dev_err(wm8350->dev,
-				"attempted write to read only reg R%d\n", i);
-			return -EINVAL;
-		}
-
-		if (is_reg_locked(wm8350, i)) {
-			dev_err(wm8350->dev,
-			       "attempted write to locked reg R%d\n", i);
-			return -EINVAL;
-		}
-
-		src[i - reg] &= wm8350_reg_io_map[i].writable;
-
-		wm8350->reg_cache[i] =
-			(wm8350->reg_cache[i] & ~wm8350_reg_io_map[i].writable)
-			| src[i - reg];
-
-		src[i - reg] = cpu_to_be16(src[i - reg]);
-	}
-
-	/* Actually write it out */
-	return regmap_raw_write(wm8350->regmap, reg, src, bytes);
-}
-
 /*
  * Safe read, modify, write methods
  */
 int wm8350_clear_bits(struct wm8350 *wm8350, u16 reg, u16 mask)
 {
-	u16 data;
-	int err;
-
-	mutex_lock(&io_mutex);
-	err = wm8350_read(wm8350, reg, 1, &data);
-	if (err) {
-		dev_err(wm8350->dev, "read from reg R%d failed\n", reg);
-		goto out;
-	}
-
-	data &= ~mask;
-	err = wm8350_write(wm8350, reg, 1, &data);
-	if (err)
-		dev_err(wm8350->dev, "write to reg R%d failed\n", reg);
-out:
-	mutex_unlock(&io_mutex);
-	return err;
+	return regmap_update_bits(wm8350->regmap, reg, mask, 0);
 }
 EXPORT_SYMBOL_GPL(wm8350_clear_bits);
 
 int wm8350_set_bits(struct wm8350 *wm8350, u16 reg, u16 mask)
 {
-	u16 data;
-	int err;
-
-	mutex_lock(&io_mutex);
-	err = wm8350_read(wm8350, reg, 1, &data);
-	if (err) {
-		dev_err(wm8350->dev, "read from reg R%d failed\n", reg);
-		goto out;
-	}
-
-	data |= mask;
-	err = wm8350_write(wm8350, reg, 1, &data);
-	if (err)
-		dev_err(wm8350->dev, "write to reg R%d failed\n", reg);
-out:
-	mutex_unlock(&io_mutex);
-	return err;
+	return regmap_update_bits(wm8350->regmap, reg, mask, mask);
 }
 EXPORT_SYMBOL_GPL(wm8350_set_bits);
 
 u16 wm8350_reg_read(struct wm8350 *wm8350, int reg)
 {
-	u16 data;
+	unsigned int data;
 	int err;
 
-	mutex_lock(&io_mutex);
-	err = wm8350_read(wm8350, reg, 1, &data);
+	err = regmap_read(wm8350->regmap, reg, &data);
 	if (err)
 		dev_err(wm8350->dev, "read from reg R%d failed\n", reg);
 
-	mutex_unlock(&io_mutex);
 	return data;
 }
 EXPORT_SYMBOL_GPL(wm8350_reg_read);
@@ -245,13 +93,11 @@ EXPORT_SYMBOL_GPL(wm8350_reg_read);
 int wm8350_reg_write(struct wm8350 *wm8350, int reg, u16 val)
 {
 	int ret;
-	u16 data = val;
 
-	mutex_lock(&io_mutex);
-	ret = wm8350_write(wm8350, reg, 1, &data);
+	ret = regmap_write(wm8350->regmap, reg, val);
+
 	if (ret)
 		dev_err(wm8350->dev, "write to reg R%d failed\n", reg);
-	mutex_unlock(&io_mutex);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(wm8350_reg_write);
@@ -261,12 +107,11 @@ int wm8350_block_read(struct wm8350 *wm8350, int start_reg, int regs,
 {
 	int err = 0;
 
-	mutex_lock(&io_mutex);
-	err = wm8350_read(wm8350, start_reg, regs, dest);
+	err = regmap_bulk_read(wm8350->regmap, start_reg, dest, regs);
 	if (err)
 		dev_err(wm8350->dev, "block read starting from R%d failed\n",
 			start_reg);
-	mutex_unlock(&io_mutex);
+
 	return err;
 }
 EXPORT_SYMBOL_GPL(wm8350_block_read);
@@ -276,12 +121,11 @@ int wm8350_block_write(struct wm8350 *wm8350, int start_reg, int regs,
 {
 	int ret = 0;
 
-	mutex_lock(&io_mutex);
-	ret = wm8350_write(wm8350, start_reg, regs, src);
+	ret = regmap_bulk_write(wm8350->regmap, start_reg, src, regs);
 	if (ret)
 		dev_err(wm8350->dev, "block write starting at R%d failed\n",
 			start_reg);
-	mutex_unlock(&io_mutex);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(wm8350_block_write);
@@ -295,15 +139,20 @@ EXPORT_SYMBOL_GPL(wm8350_block_write);
  */
 int wm8350_reg_lock(struct wm8350 *wm8350)
 {
-	u16 key = WM8350_LOCK_KEY;
 	int ret;
 
+	mutex_lock(&reg_lock_mutex);
+
 	ldbg(__func__);
-	mutex_lock(&io_mutex);
-	ret = wm8350_write(wm8350, WM8350_SECURITY, 1, &key);
+
+	ret = wm8350_reg_write(wm8350, WM8350_SECURITY, WM8350_LOCK_KEY);
 	if (ret)
 		dev_err(wm8350->dev, "lock failed\n");
-	mutex_unlock(&io_mutex);
+
+	wm8350->unlocked = false;
+
+	mutex_unlock(&reg_lock_mutex);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(wm8350_reg_lock);
@@ -319,15 +168,20 @@ EXPORT_SYMBOL_GPL(wm8350_reg_lock);
  */
 int wm8350_reg_unlock(struct wm8350 *wm8350)
 {
-	u16 key = WM8350_UNLOCK_KEY;
 	int ret;
 
+	mutex_lock(&reg_lock_mutex);
+
 	ldbg(__func__);
-	mutex_lock(&io_mutex);
-	ret = wm8350_write(wm8350, WM8350_SECURITY, 1, &key);
+
+	ret = wm8350_reg_write(wm8350, WM8350_SECURITY, WM8350_UNLOCK_KEY);
 	if (ret)
 		dev_err(wm8350->dev, "unlock failed\n");
-	mutex_unlock(&io_mutex);
+
+	wm8350->unlocked = true;
+
+	mutex_unlock(&reg_lock_mutex);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(wm8350_reg_unlock);
@@ -395,146 +249,6 @@ static irqreturn_t wm8350_auxadc_irq(int irq, void *irq_data)
 }
 
 /*
- * Cache is always host endian.
- */
-static int wm8350_create_cache(struct wm8350 *wm8350, int type, int mode)
-{
-	int i, ret = 0;
-	u16 value;
-	const u16 *reg_map;
-
-	switch (type) {
-	case 0:
-		switch (mode) {
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_0
-		case 0:
-			reg_map = wm8350_mode0_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_1
-		case 1:
-			reg_map = wm8350_mode1_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_2
-		case 2:
-			reg_map = wm8350_mode2_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_3
-		case 3:
-			reg_map = wm8350_mode3_defaults;
-			break;
-#endif
-		default:
-			dev_err(wm8350->dev,
-				"WM8350 configuration mode %d not supported\n",
-				mode);
-			return -EINVAL;
-		}
-		break;
-
-	case 1:
-		switch (mode) {
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_0
-		case 0:
-			reg_map = wm8351_mode0_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_1
-		case 1:
-			reg_map = wm8351_mode1_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_2
-		case 2:
-			reg_map = wm8351_mode2_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_3
-		case 3:
-			reg_map = wm8351_mode3_defaults;
-			break;
-#endif
-		default:
-			dev_err(wm8350->dev,
-				"WM8351 configuration mode %d not supported\n",
-				mode);
-			return -EINVAL;
-		}
-		break;
-
-	case 2:
-		switch (mode) {
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_0
-		case 0:
-			reg_map = wm8352_mode0_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_1
-		case 1:
-			reg_map = wm8352_mode1_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_2
-		case 2:
-			reg_map = wm8352_mode2_defaults;
-			break;
-#endif
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_3
-		case 3:
-			reg_map = wm8352_mode3_defaults;
-			break;
-#endif
-		default:
-			dev_err(wm8350->dev,
-				"WM8352 configuration mode %d not supported\n",
-				mode);
-			return -EINVAL;
-		}
-		break;
-
-	default:
-		dev_err(wm8350->dev,
-			"WM835x configuration mode %d not supported\n",
-			mode);
-		return -EINVAL;
-	}
-
-	wm8350->reg_cache =
-		kmalloc(sizeof(u16) * (WM8350_MAX_REGISTER + 1), GFP_KERNEL);
-	if (wm8350->reg_cache == NULL)
-		return -ENOMEM;
-
-	/* Read the initial cache state back from the device - this is
-	 * a PMIC so the device many not be in a virgin state and we
-	 * can't rely on the silicon values.
-	 */
-	ret = regmap_raw_read(wm8350->regmap, 0, wm8350->reg_cache,
-			      sizeof(u16) * (WM8350_MAX_REGISTER + 1));
-	if (ret < 0) {
-		dev_err(wm8350->dev,
-			"failed to read initial cache values\n");
-		goto out;
-	}
-
-	/* Mask out uncacheable/unreadable bits and the audio. */
-	for (i = 0; i < WM8350_MAX_REGISTER; i++) {
-		if (wm8350_reg_io_map[i].readable &&
-		    (i < WM8350_CLOCK_CONTROL_1 || i > WM8350_AIF_TEST)) {
-			value = be16_to_cpu(wm8350->reg_cache[i]);
-			value &= wm8350_reg_io_map[i].readable;
-			wm8350->reg_cache[i] = value;
-		} else
-			wm8350->reg_cache[i] = reg_map[i];
-	}
-
-out:
-	kfree(wm8350->reg_cache);
-	return ret;
-}
-
-/*
  * Register a client device.  This is non-fatal since there is no need to
  * fail the entire device init due to a single platform device failing.
  */
@@ -681,18 +395,12 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 		goto err;
 	}
 
-	ret = wm8350_create_cache(wm8350, mask_rev, mode);
-	if (ret < 0) {
-		dev_err(wm8350->dev, "Failed to create register cache\n");
-		return ret;
-	}
-
 	mutex_init(&wm8350->auxadc_mutex);
 	init_completion(&wm8350->auxadc_done);
 
 	ret = wm8350_irq_init(wm8350, irq, pdata);
 	if (ret < 0)
-		goto err_free;
+		goto err;
 
 	if (wm8350->irq_base) {
 		ret = request_threaded_irq(wm8350->irq_base +
@@ -730,8 +438,6 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 
 err_irq:
 	wm8350_irq_exit(wm8350);
-err_free:
-	kfree(wm8350->reg_cache);
 err:
 	return ret;
 }
@@ -758,8 +464,6 @@ void wm8350_device_exit(struct wm8350 *wm8350)
 		free_irq(wm8350->irq_base + WM8350_IRQ_AUXADC_DATARDY, wm8350);
 
 	wm8350_irq_exit(wm8350);
-
-	kfree(wm8350->reg_cache);
 }
 EXPORT_SYMBOL_GPL(wm8350_device_exit);
 
diff --git a/drivers/mfd/wm8350-i2c.c b/drivers/mfd/wm8350-i2c.c
index a68aceb4e48..2e57101c8d3 100644
--- a/drivers/mfd/wm8350-i2c.c
+++ b/drivers/mfd/wm8350-i2c.c
@@ -23,11 +23,6 @@
 #include <linux/regmap.h>
 #include <linux/slab.h>
 
-static const struct regmap_config wm8350_regmap = {
-	.reg_bits = 8,
-	.val_bits = 16,
-};
-
 static int wm8350_i2c_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
 {
diff --git a/drivers/mfd/wm8350-irq.c b/drivers/mfd/wm8350-irq.c
index 9fd01bf63c5..624ff90501c 100644
--- a/drivers/mfd/wm8350-irq.c
+++ b/drivers/mfd/wm8350-irq.c
@@ -432,11 +432,9 @@ static void wm8350_irq_sync_unlock(struct irq_data *data)
 	for (i = 0; i < ARRAY_SIZE(wm8350->irq_masks); i++) {
 		/* If there's been a change in the mask write it back
 		 * to the hardware. */
-		if (wm8350->irq_masks[i] !=
-		    wm8350->reg_cache[WM8350_INT_STATUS_1_MASK + i])
-			WARN_ON(wm8350_reg_write(wm8350,
-					 WM8350_INT_STATUS_1_MASK + i,
-						 wm8350->irq_masks[i]));
+		WARN_ON(regmap_update_bits(wm8350->regmap,
+					   WM8350_INT_STATUS_1_MASK + i,
+					   0xffff, wm8350->irq_masks[i]));
 	}
 
 	mutex_unlock(&wm8350->irq_lock);
diff --git a/drivers/mfd/wm8350-regmap.c b/drivers/mfd/wm8350-regmap.c
index e965139e5cd..9efc64750fb 100644
--- a/drivers/mfd/wm8350-regmap.c
+++ b/drivers/mfd/wm8350-regmap.c
@@ -14,3170 +14,18 @@
 
 #include <linux/mfd/wm8350/core.h>
 
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_0
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8350_mode0_defaults[] = {
-	0x17FF,     /* R0   - Reset/ID */
-	0x1000,     /* R1   - ID */
-	0x0000,     /* R2 */
-	0x1002,     /* R3   - System Control 1 */
-	0x0004,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27  - Power Up Interrupt Status */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35  - Power Up Interrupt Status Mask */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3B00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - LOUT1 Volume */
-	0x00E4,     /* R105 - ROUT1 Volume */
-	0x00E4,     /* R106 - LOUT2 Volume */
-	0x02E4,     /* R107 - ROUT2 Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 - AIF Test */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x03FC,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0FFC,     /* R134 - GPIO Configuration (i/o) */
-	0x0FFC,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0013,     /* R140 - GPIO Function Select 1 */
-	0x0000,     /* R141 - GPIO Function Select 2 */
-	0x0000,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x002D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0000,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0000,     /* R186 - DCDC3 Control */
-	0x0000,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0000,     /* R189 - DCDC4 Control */
-	0x0000,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0000,     /* R195 - DCDC6 Control */
-	0x0000,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001C,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x001B,     /* R203 - LDO2 Control */
-	0x0000,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001B,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001B,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 */
-	0x4000,     /* R220 - RAM BIST 1 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 */
-	0x0000,     /* R227 */
-	0x0000,     /* R228 */
-	0x0000,     /* R229 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 */
-	0x0000,     /* R234 */
-	0x0000,     /* R235 */
-	0x0000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0000,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0000,     /* R243 */
-	0x0000,     /* R244 */
-	0x0000,     /* R245 */
-	0x0000,     /* R246 */
-	0x0000,     /* R247 */
-	0x0000,     /* R248 */
-	0x0000,     /* R249 */
-	0x0000,     /* R250 */
-	0x0000,     /* R251 */
-	0x0000,     /* R252 */
-	0x0000,     /* R253 */
-	0x0000,     /* R254 */
-	0x0000,     /* R255 */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_1
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8350_mode1_defaults[] = {
-	0x17FF,     /* R0   - Reset/ID */
-	0x1000,     /* R1   - ID */
-	0x0000,     /* R2 */
-	0x1002,     /* R3   - System Control 1 */
-	0x0014,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27  - Power Up Interrupt Status */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35  - Power Up Interrupt Status Mask */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3B00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - LOUT1 Volume */
-	0x00E4,     /* R105 - ROUT1 Volume */
-	0x00E4,     /* R106 - LOUT2 Volume */
-	0x02E4,     /* R107 - ROUT2 Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 - AIF Test */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x03FC,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x00FB,     /* R134 - GPIO Configuration (i/o) */
-	0x04FE,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0312,     /* R140 - GPIO Function Select 1 */
-	0x1003,     /* R141 - GPIO Function Select 2 */
-	0x1331,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x002D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x0062,     /* R180 - DCDC1 Control */
-	0x0400,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0026,     /* R186 - DCDC3 Control */
-	0x0400,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0062,     /* R189 - DCDC4 Control */
-	0x0400,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 - DCDC6 Control */
-	0x0800,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x0006,     /* R200 - LDO1 Control */
-	0x0400,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0006,     /* R203 - LDO2 Control */
-	0x0400,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001B,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001B,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 */
-	0x4000,     /* R220 - RAM BIST 1 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 */
-	0x0000,     /* R227 */
-	0x0000,     /* R228 */
-	0x0000,     /* R229 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 */
-	0x0000,     /* R234 */
-	0x0000,     /* R235 */
-	0x0000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0000,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0000,     /* R243 */
-	0x0000,     /* R244 */
-	0x0000,     /* R245 */
-	0x0000,     /* R246 */
-	0x0000,     /* R247 */
-	0x0000,     /* R248 */
-	0x0000,     /* R249 */
-	0x0000,     /* R250 */
-	0x0000,     /* R251 */
-	0x0000,     /* R252 */
-	0x0000,     /* R253 */
-	0x0000,     /* R254 */
-	0x0000,     /* R255 */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_2
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8350_mode2_defaults[] = {
-	0x17FF,     /* R0   - Reset/ID */
-	0x1000,     /* R1   - ID */
-	0x0000,     /* R2 */
-	0x1002,     /* R3   - System Control 1 */
-	0x0014,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27  - Power Up Interrupt Status */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35  - Power Up Interrupt Status Mask */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3B00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - LOUT1 Volume */
-	0x00E4,     /* R105 - ROUT1 Volume */
-	0x00E4,     /* R106 - LOUT2 Volume */
-	0x02E4,     /* R107 - ROUT2 Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 - AIF Test */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x03FC,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x08FB,     /* R134 - GPIO Configuration (i/o) */
-	0x0CFE,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0312,     /* R140 - GPIO Function Select 1 */
-	0x0003,     /* R141 - GPIO Function Select 2 */
-	0x2331,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x002D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0400,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x002E,     /* R186 - DCDC3 Control */
-	0x0800,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x000E,     /* R189 - DCDC4 Control */
-	0x0800,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 - DCDC6 Control */
-	0x0C00,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001A,     /* R200 - LDO1 Control */
-	0x0800,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0010,     /* R203 - LDO2 Control */
-	0x0800,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x000A,     /* R206 - LDO3 Control */
-	0x0C00,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001A,     /* R209 - LDO4 Control */
-	0x0800,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 */
-	0x4000,     /* R220 - RAM BIST 1 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 */
-	0x0000,     /* R227 */
-	0x0000,     /* R228 */
-	0x0000,     /* R229 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 */
-	0x0000,     /* R234 */
-	0x0000,     /* R235 */
-	0x0000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0000,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0000,     /* R243 */
-	0x0000,     /* R244 */
-	0x0000,     /* R245 */
-	0x0000,     /* R246 */
-	0x0000,     /* R247 */
-	0x0000,     /* R248 */
-	0x0000,     /* R249 */
-	0x0000,     /* R250 */
-	0x0000,     /* R251 */
-	0x0000,     /* R252 */
-	0x0000,     /* R253 */
-	0x0000,     /* R254 */
-	0x0000,     /* R255 */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8350_CONFIG_MODE_3
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8350_mode3_defaults[] = {
-	0x17FF,     /* R0   - Reset/ID */
-	0x1000,     /* R1   - ID */
-	0x0000,     /* R2 */
-	0x1000,     /* R3   - System Control 1 */
-	0x0004,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27  - Power Up Interrupt Status */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35  - Power Up Interrupt Status Mask */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3B00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - LOUT1 Volume */
-	0x00E4,     /* R105 - ROUT1 Volume */
-	0x00E4,     /* R106 - LOUT2 Volume */
-	0x02E4,     /* R107 - ROUT2 Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 - AIF Test */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x03FC,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0A7B,     /* R134 - GPIO Configuration (i/o) */
-	0x06FE,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x1312,     /* R140 - GPIO Function Select 1 */
-	0x1030,     /* R141 - GPIO Function Select 2 */
-	0x2231,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x002D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0400,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x000E,     /* R186 - DCDC3 Control */
-	0x0400,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0026,     /* R189 - DCDC4 Control */
-	0x0400,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 - DCDC6 Control */
-	0x0400,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001C,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x001C,     /* R203 - LDO2 Control */
-	0x0400,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001C,     /* R206 - LDO3 Control */
-	0x0400,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001F,     /* R209 - LDO4 Control */
-	0x0400,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 */
-	0x4000,     /* R220 - RAM BIST 1 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 */
-	0x0000,     /* R227 */
-	0x0000,     /* R228 */
-	0x0000,     /* R229 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 */
-	0x0000,     /* R234 */
-	0x0000,     /* R235 */
-	0x0000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0000,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0000,     /* R243 */
-	0x0000,     /* R244 */
-	0x0000,     /* R245 */
-	0x0000,     /* R246 */
-	0x0000,     /* R247 */
-	0x0000,     /* R248 */
-	0x0000,     /* R249 */
-	0x0000,     /* R250 */
-	0x0000,     /* R251 */
-	0x0000,     /* R252 */
-	0x0000,     /* R253 */
-	0x0000,     /* R254 */
-	0x0000,     /* R255 */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_0
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8351_mode0_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0001,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0004,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x0000,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0FFC,     /* R134 - GPIO Configuration (i/o) */
-	0x0FFC,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0013,     /* R140 - GPIO Function Select 1 */
-	0x0000,     /* R141 - GPIO Function Select 2 */
-	0x0000,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 */
-	0x0000,     /* R175 */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0000,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0000,     /* R186 - DCDC3 Control */
-	0x0000,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0000,     /* R189 - DCDC4 Control */
-	0x0000,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 */
-	0x0000,     /* R193 */
-	0x0000,     /* R194 */
-	0x0000,     /* R195 */
-	0x0000,     /* R196 */
-	0x0006,     /* R197 */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001C,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x001B,     /* R203 - LDO2 Control */
-	0x0000,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001B,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001B,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 - FLL Test 1 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x1000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_1
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8351_mode1_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0001,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0204,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x0000,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0CFB,     /* R134 - GPIO Configuration (i/o) */
-	0x0C1F,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0300,     /* R140 - GPIO Function Select 1 */
-	0x1110,     /* R141 - GPIO Function Select 2 */
-	0x0013,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 */
-	0x0000,     /* R175 */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0C00,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0026,     /* R186 - DCDC3 Control */
-	0x0400,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0062,     /* R189 - DCDC4 Control */
-	0x0800,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 */
-	0x0000,     /* R193 */
-	0x0000,     /* R194 */
-	0x000A,     /* R195 */
-	0x1000,     /* R196 */
-	0x0006,     /* R197 */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x0006,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0010,     /* R203 - LDO2 Control */
-	0x0C00,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001F,     /* R206 - LDO3 Control */
-	0x0800,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x000A,     /* R209 - LDO4 Control */
-	0x0800,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 - FLL Test 1 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x1000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x1000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_2
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8351_mode2_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0001,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0214,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x0110,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x09FA,     /* R134 - GPIO Configuration (i/o) */
-	0x0DF6,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x1310,     /* R140 - GPIO Function Select 1 */
-	0x0003,     /* R141 - GPIO Function Select 2 */
-	0x2000,     /* R142 - GPIO Function Select 3 */
-	0x0000,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 */
-	0x0000,     /* R175 */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x001A,     /* R180 - DCDC1 Control */
-	0x0800,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0056,     /* R186 - DCDC3 Control */
-	0x0400,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0026,     /* R189 - DCDC4 Control */
-	0x0C00,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 */
-	0x0000,     /* R193 */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 */
-	0x0C00,     /* R196 */
-	0x0006,     /* R197 */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001C,     /* R200 - LDO1 Control */
-	0x0400,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0010,     /* R203 - LDO2 Control */
-	0x0C00,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x0015,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001A,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 - FLL Test 1 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x1000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8351_CONFIG_MODE_3
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8351_mode3_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0001,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0204,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0010,     /* R129 - GPIO Pin pull up Control */
-	0x0000,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0BFB,     /* R134 - GPIO Configuration (i/o) */
-	0x0FFD,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0310,     /* R140 - GPIO Function Select 1 */
-	0x0001,     /* R141 - GPIO Function Select 2 */
-	0x2300,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 */
-	0x0000,     /* R175 */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0400,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0026,     /* R186 - DCDC3 Control */
-	0x0800,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0062,     /* R189 - DCDC4 Control */
-	0x1400,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 */
-	0x0000,     /* R193 */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 */
-	0x0400,     /* R196 */
-	0x0006,     /* R197 */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x0006,     /* R200 - LDO1 Control */
-	0x0C00,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0016,     /* R203 - LDO2 Control */
-	0x0000,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x0019,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001A,     /* R209 - LDO4 Control */
-	0x1000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 - FLL Test 1 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x1000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_0
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8352_mode0_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0002,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0004,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x0000,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0FFC,     /* R134 - GPIO Configuration (i/o) */
-	0x0FFC,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0013,     /* R140 - GPIO Function Select 1 */
-	0x0000,     /* R141 - GPIO Function Select 2 */
-	0x0000,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0000,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0000,     /* R186 - DCDC3 Control */
-	0x0000,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0000,     /* R189 - DCDC4 Control */
-	0x0000,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0000,     /* R195 - DCDC6 Control */
-	0x0000,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001C,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x001B,     /* R203 - LDO2 Control */
-	0x0000,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001B,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001B,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x5000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-	0x5100,     /* R252 */
-	0x1000,     /* R253 - DCDC6 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_1
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8352_mode1_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0002,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0204,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x0000,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0BFB,     /* R134 - GPIO Configuration (i/o) */
-	0x0FFF,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0300,     /* R140 - GPIO Function Select 1 */
-	0x0000,     /* R141 - GPIO Function Select 2 */
-	0x2300,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x0062,     /* R180 - DCDC1 Control */
-	0x0400,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0006,     /* R186 - DCDC3 Control */
-	0x0800,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x0006,     /* R189 - DCDC4 Control */
-	0x0C00,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 - DCDC6 Control */
-	0x1000,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x0002,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x001A,     /* R203 - LDO2 Control */
-	0x0000,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001F,     /* R206 - LDO3 Control */
-	0x0000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001F,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x5000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-	0x5100,     /* R252 */
-	0x1000,     /* R253 - DCDC6 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_2
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8352_mode2_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0002,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0204,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0000,     /* R129 - GPIO Pin pull up Control */
-	0x0110,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x09DA,     /* R134 - GPIO Configuration (i/o) */
-	0x0DD6,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x1310,     /* R140 - GPIO Function Select 1 */
-	0x0033,     /* R141 - GPIO Function Select 2 */
-	0x2000,     /* R142 - GPIO Function Select 3 */
-	0x0000,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x000E,     /* R180 - DCDC1 Control */
-	0x0800,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0056,     /* R186 - DCDC3 Control */
-	0x1800,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x000E,     /* R189 - DCDC4 Control */
-	0x1000,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0026,     /* R195 - DCDC6 Control */
-	0x0C00,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001C,     /* R200 - LDO1 Control */
-	0x0000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0006,     /* R203 - LDO2 Control */
-	0x0400,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x001C,     /* R206 - LDO3 Control */
-	0x1400,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x001A,     /* R209 - LDO4 Control */
-	0x0000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x5000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-	0x5100,     /* R252 */
-	0x1000,     /* R253 - DCDC6 Test Controls */
-};
-#endif
-
-#ifdef CONFIG_MFD_WM8352_CONFIG_MODE_3
-
-#undef WM8350_HAVE_CONFIG_MODE
-#define WM8350_HAVE_CONFIG_MODE
-
-const u16 wm8352_mode3_defaults[] = {
-	0x6143,     /* R0   - Reset/ID */
-	0x0000,     /* R1   - ID */
-	0x0002,     /* R2   - Revision */
-	0x1C02,     /* R3   - System Control 1 */
-	0x0204,     /* R4   - System Control 2 */
-	0x0000,     /* R5   - System Hibernate */
-	0x8A00,     /* R6   - Interface Control */
-	0x0000,     /* R7 */
-	0x8000,     /* R8   - Power mgmt (1) */
-	0x0000,     /* R9   - Power mgmt (2) */
-	0x0000,     /* R10  - Power mgmt (3) */
-	0x2000,     /* R11  - Power mgmt (4) */
-	0x0E00,     /* R12  - Power mgmt (5) */
-	0x0000,     /* R13  - Power mgmt (6) */
-	0x0000,     /* R14  - Power mgmt (7) */
-	0x0000,     /* R15 */
-	0x0000,     /* R16  - RTC Seconds/Minutes */
-	0x0100,     /* R17  - RTC Hours/Day */
-	0x0101,     /* R18  - RTC Date/Month */
-	0x1400,     /* R19  - RTC Year */
-	0x0000,     /* R20  - Alarm Seconds/Minutes */
-	0x0000,     /* R21  - Alarm Hours/Day */
-	0x0000,     /* R22  - Alarm Date/Month */
-	0x0320,     /* R23  - RTC Time Control */
-	0x0000,     /* R24  - System Interrupts */
-	0x0000,     /* R25  - Interrupt Status 1 */
-	0x0000,     /* R26  - Interrupt Status 2 */
-	0x0000,     /* R27 */
-	0x0000,     /* R28  - Under Voltage Interrupt status */
-	0x0000,     /* R29  - Over Current Interrupt status */
-	0x0000,     /* R30  - GPIO Interrupt Status */
-	0x0000,     /* R31  - Comparator Interrupt Status */
-	0x3FFF,     /* R32  - System Interrupts Mask */
-	0x0000,     /* R33  - Interrupt Status 1 Mask */
-	0x0000,     /* R34  - Interrupt Status 2 Mask */
-	0x0000,     /* R35 */
-	0x0000,     /* R36  - Under Voltage Interrupt status Mask */
-	0x0000,     /* R37  - Over Current Interrupt status Mask */
-	0x0000,     /* R38  - GPIO Interrupt Status Mask */
-	0x0000,     /* R39  - Comparator Interrupt Status Mask */
-	0x0040,     /* R40  - Clock Control 1 */
-	0x0000,     /* R41  - Clock Control 2 */
-	0x3A00,     /* R42  - FLL Control 1 */
-	0x7086,     /* R43  - FLL Control 2 */
-	0xC226,     /* R44  - FLL Control 3 */
-	0x0000,     /* R45  - FLL Control 4 */
-	0x0000,     /* R46 */
-	0x0000,     /* R47 */
-	0x0000,     /* R48  - DAC Control */
-	0x0000,     /* R49 */
-	0x00C0,     /* R50  - DAC Digital Volume L */
-	0x00C0,     /* R51  - DAC Digital Volume R */
-	0x0000,     /* R52 */
-	0x0040,     /* R53  - DAC LR Rate */
-	0x0000,     /* R54  - DAC Clock Control */
-	0x0000,     /* R55 */
-	0x0000,     /* R56 */
-	0x0000,     /* R57 */
-	0x4000,     /* R58  - DAC Mute */
-	0x0000,     /* R59  - DAC Mute Volume */
-	0x0000,     /* R60  - DAC Side */
-	0x0000,     /* R61 */
-	0x0000,     /* R62 */
-	0x0000,     /* R63 */
-	0x8000,     /* R64  - ADC Control */
-	0x0000,     /* R65 */
-	0x00C0,     /* R66  - ADC Digital Volume L */
-	0x00C0,     /* R67  - ADC Digital Volume R */
-	0x0000,     /* R68  - ADC Divider */
-	0x0000,     /* R69 */
-	0x0040,     /* R70  - ADC LR Rate */
-	0x0000,     /* R71 */
-	0x0303,     /* R72  - Input Control */
-	0x0000,     /* R73  - IN3 Input Control */
-	0x0000,     /* R74  - Mic Bias Control */
-	0x0000,     /* R75 */
-	0x0000,     /* R76  - Output Control */
-	0x0000,     /* R77  - Jack Detect */
-	0x0000,     /* R78  - Anti Pop Control */
-	0x0000,     /* R79 */
-	0x0040,     /* R80  - Left Input Volume */
-	0x0040,     /* R81  - Right Input Volume */
-	0x0000,     /* R82 */
-	0x0000,     /* R83 */
-	0x0000,     /* R84 */
-	0x0000,     /* R85 */
-	0x0000,     /* R86 */
-	0x0000,     /* R87 */
-	0x0800,     /* R88  - Left Mixer Control */
-	0x1000,     /* R89  - Right Mixer Control */
-	0x0000,     /* R90 */
-	0x0000,     /* R91 */
-	0x0000,     /* R92  - OUT3 Mixer Control */
-	0x0000,     /* R93  - OUT4 Mixer Control */
-	0x0000,     /* R94 */
-	0x0000,     /* R95 */
-	0x0000,     /* R96  - Output Left Mixer Volume */
-	0x0000,     /* R97  - Output Right Mixer Volume */
-	0x0000,     /* R98  - Input Mixer Volume L */
-	0x0000,     /* R99  - Input Mixer Volume R */
-	0x0000,     /* R100 - Input Mixer Volume */
-	0x0000,     /* R101 */
-	0x0000,     /* R102 */
-	0x0000,     /* R103 */
-	0x00E4,     /* R104 - OUT1L Volume */
-	0x00E4,     /* R105 - OUT1R Volume */
-	0x00E4,     /* R106 - OUT2L Volume */
-	0x02E4,     /* R107 - OUT2R Volume */
-	0x0000,     /* R108 */
-	0x0000,     /* R109 */
-	0x0000,     /* R110 */
-	0x0000,     /* R111 - BEEP Volume */
-	0x0A00,     /* R112 - AI Formating */
-	0x0000,     /* R113 - ADC DAC COMP */
-	0x0020,     /* R114 - AI ADC Control */
-	0x0020,     /* R115 - AI DAC Control */
-	0x0000,     /* R116 */
-	0x0000,     /* R117 */
-	0x0000,     /* R118 */
-	0x0000,     /* R119 */
-	0x0000,     /* R120 */
-	0x0000,     /* R121 */
-	0x0000,     /* R122 */
-	0x0000,     /* R123 */
-	0x0000,     /* R124 */
-	0x0000,     /* R125 */
-	0x0000,     /* R126 */
-	0x0000,     /* R127 */
-	0x1FFF,     /* R128 - GPIO Debounce */
-	0x0010,     /* R129 - GPIO Pin pull up Control */
-	0x0000,     /* R130 - GPIO Pull down Control */
-	0x0000,     /* R131 - GPIO Interrupt Mode */
-	0x0000,     /* R132 */
-	0x0000,     /* R133 - GPIO Control */
-	0x0BFB,     /* R134 - GPIO Configuration (i/o) */
-	0x0FFD,     /* R135 - GPIO Pin Polarity / Type */
-	0x0000,     /* R136 */
-	0x0000,     /* R137 */
-	0x0000,     /* R138 */
-	0x0000,     /* R139 */
-	0x0310,     /* R140 - GPIO Function Select 1 */
-	0x0001,     /* R141 - GPIO Function Select 2 */
-	0x2300,     /* R142 - GPIO Function Select 3 */
-	0x0003,     /* R143 - GPIO Function Select 4 */
-	0x0000,     /* R144 - Digitiser Control (1) */
-	0x0002,     /* R145 - Digitiser Control (2) */
-	0x0000,     /* R146 */
-	0x0000,     /* R147 */
-	0x0000,     /* R148 */
-	0x0000,     /* R149 */
-	0x0000,     /* R150 */
-	0x0000,     /* R151 */
-	0x7000,     /* R152 - AUX1 Readback */
-	0x7000,     /* R153 - AUX2 Readback */
-	0x7000,     /* R154 - AUX3 Readback */
-	0x7000,     /* R155 - AUX4 Readback */
-	0x0000,     /* R156 - USB Voltage Readback */
-	0x0000,     /* R157 - LINE Voltage Readback */
-	0x0000,     /* R158 - BATT Voltage Readback */
-	0x0000,     /* R159 - Chip Temp Readback */
-	0x0000,     /* R160 */
-	0x0000,     /* R161 */
-	0x0000,     /* R162 */
-	0x0000,     /* R163 - Generic Comparator Control */
-	0x0000,     /* R164 - Generic comparator 1 */
-	0x0000,     /* R165 - Generic comparator 2 */
-	0x0000,     /* R166 - Generic comparator 3 */
-	0x0000,     /* R167 - Generic comparator 4 */
-	0xA00F,     /* R168 - Battery Charger Control 1 */
-	0x0B06,     /* R169 - Battery Charger Control 2 */
-	0x0000,     /* R170 - Battery Charger Control 3 */
-	0x0000,     /* R171 */
-	0x0000,     /* R172 - Current Sink Driver A */
-	0x0000,     /* R173 - CSA Flash control */
-	0x0000,     /* R174 - Current Sink Driver B */
-	0x0000,     /* R175 - CSB Flash control */
-	0x0000,     /* R176 - DCDC/LDO requested */
-	0x032D,     /* R177 - DCDC Active options */
-	0x0000,     /* R178 - DCDC Sleep options */
-	0x0025,     /* R179 - Power-check comparator */
-	0x0006,     /* R180 - DCDC1 Control */
-	0x0400,     /* R181 - DCDC1 Timeouts */
-	0x1006,     /* R182 - DCDC1 Low Power */
-	0x0018,     /* R183 - DCDC2 Control */
-	0x0000,     /* R184 - DCDC2 Timeouts */
-	0x0000,     /* R185 */
-	0x0050,     /* R186 - DCDC3 Control */
-	0x0C00,     /* R187 - DCDC3 Timeouts */
-	0x0006,     /* R188 - DCDC3 Low Power */
-	0x000E,     /* R189 - DCDC4 Control */
-	0x0400,     /* R190 - DCDC4 Timeouts */
-	0x0006,     /* R191 - DCDC4 Low Power */
-	0x0008,     /* R192 - DCDC5 Control */
-	0x0000,     /* R193 - DCDC5 Timeouts */
-	0x0000,     /* R194 */
-	0x0029,     /* R195 - DCDC6 Control */
-	0x0800,     /* R196 - DCDC6 Timeouts */
-	0x0006,     /* R197 - DCDC6 Low Power */
-	0x0000,     /* R198 */
-	0x0003,     /* R199 - Limit Switch Control */
-	0x001D,     /* R200 - LDO1 Control */
-	0x1000,     /* R201 - LDO1 Timeouts */
-	0x001C,     /* R202 - LDO1 Low Power */
-	0x0017,     /* R203 - LDO2 Control */
-	0x1000,     /* R204 - LDO2 Timeouts */
-	0x001C,     /* R205 - LDO2 Low Power */
-	0x0006,     /* R206 - LDO3 Control */
-	0x1000,     /* R207 - LDO3 Timeouts */
-	0x001C,     /* R208 - LDO3 Low Power */
-	0x0010,     /* R209 - LDO4 Control */
-	0x1000,     /* R210 - LDO4 Timeouts */
-	0x001C,     /* R211 - LDO4 Low Power */
-	0x0000,     /* R212 */
-	0x0000,     /* R213 */
-	0x0000,     /* R214 */
-	0x0000,     /* R215 - VCC_FAULT Masks */
-	0x001F,     /* R216 - Main Bandgap Control */
-	0x0000,     /* R217 - OSC Control */
-	0x9000,     /* R218 - RTC Tick Control */
-	0x0000,     /* R219 - Security1 */
-	0x4000,     /* R220 */
-	0x0000,     /* R221 */
-	0x0000,     /* R222 */
-	0x0000,     /* R223 */
-	0x0000,     /* R224 - Signal overrides */
-	0x0000,     /* R225 - DCDC/LDO status */
-	0x0000,     /* R226 - Charger Overides/status */
-	0x0000,     /* R227 - misc overrides */
-	0x0000,     /* R228 - Supply overrides/status 1 */
-	0x0000,     /* R229 - Supply overrides/status 2 */
-	0xE000,     /* R230 - GPIO Pin Status */
-	0x0000,     /* R231 - comparotor overrides */
-	0x0000,     /* R232 */
-	0x0000,     /* R233 - State Machine status */
-	0x1200,     /* R234 */
-	0x0000,     /* R235 */
-	0x8000,     /* R236 */
-	0x0000,     /* R237 */
-	0x0000,     /* R238 */
-	0x0000,     /* R239 */
-	0x0003,     /* R240 */
-	0x0000,     /* R241 */
-	0x0000,     /* R242 */
-	0x0004,     /* R243 */
-	0x0300,     /* R244 */
-	0x0000,     /* R245 */
-	0x0200,     /* R246 */
-	0x0000,     /* R247 */
-	0x1000,     /* R248 - DCDC1 Test Controls */
-	0x5000,     /* R249 */
-	0x1000,     /* R250 - DCDC3 Test Controls */
-	0x1000,     /* R251 - DCDC4 Test Controls */
-	0x5100,     /* R252 */
-	0x1000,     /* R253 - DCDC6 Test Controls */
-};
-#endif
-
 /*
  * Access masks.
  */
 
-const struct wm8350_reg_access wm8350_reg_io_map[] = {
+static const struct wm8350_reg_access {
+	u16 readable;		/* Mask of readable bits */
+	u16 writable;		/* Mask of writable bits */
+	u16 vol;		/* Mask of volatile bits */
+} wm8350_reg_io_map[] = {
 	/*  read    write volatile */
-	{ 0xFFFF, 0xFFFF, 0xFFFF }, /* R0   - Reset/ID */
-	{ 0x7CFF, 0x0C00, 0x7FFF }, /* R1   - ID */
+	{ 0xFFFF, 0xFFFF, 0x0000 }, /* R0   - Reset/ID */
+	{ 0x7CFF, 0x0C00, 0x0000 }, /* R1   - ID */
 	{ 0x007F, 0x0000, 0x0000 }, /* R2   - ROM Mask ID */
 	{ 0xBE3B, 0xBE3B, 0x8000 }, /* R3   - System Control 1 */
 	{ 0xFEF7, 0xFEF7, 0xF800 }, /* R4   - System Control 2 */
@@ -3433,3 +281,59 @@ const struct wm8350_reg_access wm8350_reg_io_map[] = {
 	{ 0x0000, 0x0000, 0x0000 }, /* R254 */
 	{ 0x0000, 0x0000, 0x0000 }, /* R255 */
 };
+
+static bool wm8350_readable(struct device *dev, unsigned int reg)
+{
+	return wm8350_reg_io_map[reg].readable;
+}
+
+static bool wm8350_writeable(struct device *dev, unsigned int reg)
+{
+	struct wm8350 *wm8350 = dev_get_drvdata(dev);
+
+	if (!wm8350->unlocked) {
+		if ((reg >= WM8350_GPIO_FUNCTION_SELECT_1 &&
+		     reg <= WM8350_GPIO_FUNCTION_SELECT_4) ||
+		    (reg >= WM8350_BATTERY_CHARGER_CONTROL_1 &&
+		     reg <= WM8350_BATTERY_CHARGER_CONTROL_3))
+			return false;
+	}
+
+	return wm8350_reg_io_map[reg].writable;
+}
+
+static bool wm8350_volatile(struct device *dev, unsigned int reg)
+{
+	return wm8350_reg_io_map[reg].vol;
+}
+
+static bool wm8350_precious(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case WM8350_SYSTEM_INTERRUPTS:
+	case WM8350_INT_STATUS_1:
+	case WM8350_INT_STATUS_2:
+	case WM8350_POWER_UP_INT_STATUS:
+	case WM8350_UNDER_VOLTAGE_INT_STATUS:
+	case WM8350_OVER_CURRENT_INT_STATUS:
+	case WM8350_GPIO_INT_STATUS:
+	case WM8350_COMPARATOR_INT_STATUS:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+const struct regmap_config wm8350_regmap = {
+	.reg_bits = 8,
+	.val_bits = 16,
+
+	.cache_type = REGCACHE_RBTREE,
+
+	.max_register = WM8350_MAX_REGISTER,
+	.readable_reg = wm8350_readable,
+	.writeable_reg = wm8350_writeable,
+	.volatile_reg = wm8350_volatile,
+	.precious_reg = wm8350_precious,
+};
diff --git a/drivers/mfd/wm8994-core.c b/drivers/mfd/wm8994-core.c
index 1e321d34977..eec74aa55fd 100644
--- a/drivers/mfd/wm8994-core.c
+++ b/drivers/mfd/wm8994-core.c
@@ -283,9 +283,24 @@ static int wm8994_suspend(struct device *dev)
 	wm8994_reg_write(wm8994, WM8994_SOFTWARE_RESET,
 			 wm8994_reg_read(wm8994, WM8994_SOFTWARE_RESET));
 
-	regcache_cache_only(wm8994->regmap, true);
 	regcache_mark_dirty(wm8994->regmap);
 
+	/* Restore GPIO registers to prevent problems with mismatched
+	 * pin configurations.
+	 */
+	ret = regcache_sync_region(wm8994->regmap, WM8994_GPIO_1,
+				   WM8994_GPIO_11);
+	if (ret != 0)
+		dev_err(dev, "Failed to restore GPIO registers: %d\n", ret);
+
+	/* In case one of the GPIOs is used as a wake input. */
+	ret = regcache_sync_region(wm8994->regmap,
+				   WM8994_INTERRUPT_STATUS_1_MASK,
+				   WM8994_INTERRUPT_STATUS_1_MASK);
+	if (ret != 0)
+		dev_err(dev, "Failed to restore interrupt mask: %d\n", ret);
+
+	regcache_cache_only(wm8994->regmap, true);
 	wm8994->suspended = true;
 
 	ret = regulator_bulk_disable(wm8994->num_supplies,
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index f1837f66975..0aac4aff17a 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -21,6 +21,7 @@
 #include <linux/regmap.h>
 
 #include <linux/mfd/wm8994/core.h>
+#include <linux/mfd/wm8994/pdata.h>
 #include <linux/mfd/wm8994/registers.h>
 
 #include <linux/delay.h>
@@ -139,6 +140,8 @@ static struct regmap_irq_chip wm8994_irq_chip = {
 int wm8994_irq_init(struct wm8994 *wm8994)
 {
 	int ret;
+	unsigned long irqflags;
+	struct wm8994_pdata *pdata = wm8994->dev->platform_data;
 
 	if (!wm8994->irq) {
 		dev_warn(wm8994->dev,
@@ -147,8 +150,13 @@ int wm8994_irq_init(struct wm8994 *wm8994)
 		return 0;
 	}
 
+	/* select user or default irq flags */
+	irqflags = IRQF_TRIGGER_HIGH | IRQF_ONESHOT;
+	if (pdata->irq_flags)
+		irqflags = pdata->irq_flags;
+
 	ret = regmap_add_irq_chip(wm8994->regmap, wm8994->irq,
-				  IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				  irqflags,
 				  wm8994->irq_base, &wm8994_irq_chip,
 				  &wm8994->irq_data);
 	if (ret != 0) {
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 2661f6e366f..98a442da892 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -64,6 +64,7 @@ config AB8500_PWM
 	bool "AB8500 PWM support"
 	depends on AB8500_CORE && ARCH_U8500
 	select HAVE_PWM
+	depends on !PWM
 	help
 	  This driver exports functions to enable/disble/config/free Pulse
 	  Width Modulation in the Analog Baseband Chip AB8500.
@@ -511,7 +512,6 @@ config USB_SWITCH_FSA9480
 source "drivers/misc/c2port/Kconfig"
 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"
 source "drivers/misc/carma/Kconfig"
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 456972faaeb..b88df7a350b 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -36,7 +36,6 @@ obj-$(CONFIG_EP93XX_PWM)	+= ep93xx_pwm.o
 obj-$(CONFIG_DS1682)		+= ds1682.o
 obj-$(CONFIG_TI_DAC7512)	+= ti_dac7512.o
 obj-$(CONFIG_C2PORT)		+= c2port/
-obj-$(CONFIG_IWMC3200TOP)      += iwmc3200top/
 obj-$(CONFIG_HMC6352)		+= hmc6352.o
 obj-y				+= eeprom/
 obj-y				+= cb710/
diff --git a/drivers/misc/ab8500-pwm.c b/drivers/misc/ab8500-pwm.c
index 042a8fe4efa..d7a9aa14e5d 100644
--- a/drivers/misc/ab8500-pwm.c
+++ b/drivers/misc/ab8500-pwm.c
@@ -142,16 +142,10 @@ static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id ab8500_pwm_match[] = {
-	{ .compatible = "stericsson,ab8500-pwm", },
-	{}
-};
-
 static struct platform_driver ab8500_pwm_driver = {
 	.driver = {
 		.name = "ab8500-pwm",
 		.owner = THIS_MODULE,
-		.of_match_table = ab8500_pwm_match,
 	},
 	.probe = ab8500_pwm_probe,
 	.remove = __devexit_p(ab8500_pwm_remove),
diff --git a/drivers/misc/cb710/core.c b/drivers/misc/cb710/core.c
index 85cc7710193..9d5eed75466 100644
--- a/drivers/misc/cb710/core.c
+++ b/drivers/misc/cb710/core.c
@@ -180,7 +180,7 @@ static int cb710_suspend(struct pci_dev *pdev, pm_message_t state)
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 	if (state.event & PM_EVENT_SLEEP)
-		pci_set_power_state(pdev, PCI_D3cold);
+		pci_set_power_state(pdev, PCI_D3hot);
 	return 0;
 }
 
diff --git a/drivers/misc/eeprom/at25.c b/drivers/misc/eeprom/at25.c
index 0842c2994ee..25003d6ceb5 100644
--- a/drivers/misc/eeprom/at25.c
+++ b/drivers/misc/eeprom/at25.c
@@ -19,7 +19,7 @@
 
 #include <linux/spi/spi.h>
 #include <linux/spi/eeprom.h>
-
+#include <linux/of.h>
 
 /*
  * NOTE: this is an *EEPROM* driver.  The vagaries of product naming
@@ -305,25 +305,54 @@ static ssize_t at25_mem_write(struct memory_accessor *mem, const char *buf,
 static int at25_probe(struct spi_device *spi)
 {
 	struct at25_data	*at25 = NULL;
-	const struct spi_eeprom *chip;
+	struct spi_eeprom	chip;
+	struct device_node	*np = spi->dev.of_node;
 	int			err;
 	int			sr;
 	int			addrlen;
 
 	/* Chip description */
-	chip = spi->dev.platform_data;
-	if (!chip) {
-		dev_dbg(&spi->dev, "no chip description\n");
-		err = -ENODEV;
-		goto fail;
-	}
+	if (!spi->dev.platform_data) {
+		if (np) {
+			u32 val;
+
+			memset(&chip, 0, sizeof(chip));
+			strncpy(chip.name, np->name, 10);
+
+			err = of_property_read_u32(np, "at25,byte-len", &val);
+			if (err) {
+				dev_dbg(&spi->dev, "invalid chip dt description\n");
+				goto fail;
+			}
+			chip.byte_len = val;
+
+			err = of_property_read_u32(np, "at25,addr-mode", &val);
+			if (err) {
+				dev_dbg(&spi->dev, "invalid chip dt description\n");
+				goto fail;
+			}
+			chip.flags = (u16)val;
+
+			err = of_property_read_u32(np, "at25,page-size", &val);
+			if (err) {
+				dev_dbg(&spi->dev, "invalid chip dt description\n");
+				goto fail;
+			}
+			chip.page_size = (u16)val;
+		} else {
+			dev_dbg(&spi->dev, "no chip description\n");
+			err = -ENODEV;
+			goto fail;
+		}
+	} else
+		chip = *(struct spi_eeprom *)spi->dev.platform_data;
 
 	/* For now we only support 8/16/24 bit addressing */
-	if (chip->flags & EE_ADDR1)
+	if (chip.flags & EE_ADDR1)
 		addrlen = 1;
-	else if (chip->flags & EE_ADDR2)
+	else if (chip.flags & EE_ADDR2)
 		addrlen = 2;
-	else if (chip->flags & EE_ADDR3)
+	else if (chip.flags & EE_ADDR3)
 		addrlen = 3;
 	else {
 		dev_dbg(&spi->dev, "unsupported address type\n");
@@ -348,7 +377,7 @@ static int at25_probe(struct spi_device *spi)
 	}
 
 	mutex_init(&at25->lock);
-	at25->chip = *chip;
+	at25->chip = chip;
 	at25->spi = spi_dev_get(spi);
 	dev_set_drvdata(&spi->dev, at25);
 	at25->addrlen = addrlen;
@@ -369,7 +398,7 @@ static int at25_probe(struct spi_device *spi)
 	at25->mem.read = at25_mem_read;
 
 	at25->bin.size = at25->chip.byte_len;
-	if (!(chip->flags & EE_READONLY)) {
+	if (!(chip.flags & EE_READONLY)) {
 		at25->bin.write = at25_bin_write;
 		at25->bin.attr.mode |= S_IWUSR;
 		at25->mem.write = at25_mem_write;
@@ -379,8 +408,8 @@ static int at25_probe(struct spi_device *spi)
 	if (err)
 		goto fail;
 
-	if (chip->setup)
-		chip->setup(&at25->mem, chip->context);
+	if (chip.setup)
+		chip.setup(&at25->mem, chip.context);
 
 	dev_info(&spi->dev, "%Zd %s %s eeprom%s, pagesize %u\n",
 		(at25->bin.size < 1024)
@@ -388,7 +417,7 @@ static int at25_probe(struct spi_device *spi)
 			: (at25->bin.size / 1024),
 		(at25->bin.size < 1024) ? "Byte" : "KByte",
 		at25->chip.name,
-		(chip->flags & EE_READONLY) ? " (readonly)" : "",
+		(chip.flags & EE_READONLY) ? " (readonly)" : "",
 		at25->chip.page_size);
 	return 0;
 fail:
diff --git a/drivers/misc/hpilo.c b/drivers/misc/hpilo.c
index fffc227181b..6df0da4085e 100644
--- a/drivers/misc/hpilo.c
+++ b/drivers/misc/hpilo.c
@@ -30,6 +30,7 @@
 
 static struct class *ilo_class;
 static unsigned int ilo_major;
+static unsigned int max_ccb = MIN_CCB;
 static char ilo_hwdev[MAX_ILO_DEV];
 
 static inline int get_entry_id(int entry)
@@ -424,7 +425,7 @@ static void ilo_set_reset(struct ilo_hwinfo *hw)
 	 * Mapped memory is zeroed on ilo reset, so set a per ccb flag
 	 * to indicate that this ccb needs to be closed and reopened.
 	 */
-	for (slot = 0; slot < MAX_CCB; slot++) {
+	for (slot = 0; slot < max_ccb; slot++) {
 		if (!hw->ccb_alloc[slot])
 			continue;
 		set_channel_reset(&hw->ccb_alloc[slot]->driver_ccb);
@@ -535,7 +536,7 @@ static int ilo_close(struct inode *ip, struct file *fp)
 	struct ilo_hwinfo *hw;
 	unsigned long flags;
 
-	slot = iminor(ip) % MAX_CCB;
+	slot = iminor(ip) % max_ccb;
 	hw = container_of(ip->i_cdev, struct ilo_hwinfo, cdev);
 
 	spin_lock(&hw->open_lock);
@@ -566,7 +567,7 @@ static int ilo_open(struct inode *ip, struct file *fp)
 	struct ilo_hwinfo *hw;
 	unsigned long flags;
 
-	slot = iminor(ip) % MAX_CCB;
+	slot = iminor(ip) % max_ccb;
 	hw = container_of(ip->i_cdev, struct ilo_hwinfo, cdev);
 
 	/* new ccb allocation */
@@ -663,7 +664,7 @@ static irqreturn_t ilo_isr(int irq, void *data)
 		ilo_set_reset(hw);
 	}
 
-	for (i = 0; i < MAX_CCB; i++) {
+	for (i = 0; i < max_ccb; i++) {
 		if (!hw->ccb_alloc[i])
 			continue;
 		if (pending & (1 << i))
@@ -697,14 +698,14 @@ static int __devinit ilo_map_device(struct pci_dev *pdev, struct ilo_hwinfo *hw)
 	}
 
 	/* map the adapter shared memory region */
-	hw->ram_vaddr = pci_iomap(pdev, 2, MAX_CCB * ILOHW_CCB_SZ);
+	hw->ram_vaddr = pci_iomap(pdev, 2, max_ccb * ILOHW_CCB_SZ);
 	if (hw->ram_vaddr == NULL) {
 		dev_err(&pdev->dev, "Error mapping shared mem\n");
 		goto mmio_free;
 	}
 
 	/* map the doorbell aperture */
-	hw->db_vaddr = pci_iomap(pdev, 3, MAX_CCB * ONE_DB_SIZE);
+	hw->db_vaddr = pci_iomap(pdev, 3, max_ccb * ONE_DB_SIZE);
 	if (hw->db_vaddr == NULL) {
 		dev_err(&pdev->dev, "Error mapping doorbell\n");
 		goto ram_free;
@@ -727,7 +728,7 @@ static void ilo_remove(struct pci_dev *pdev)
 	clear_device(ilo_hw);
 
 	minor = MINOR(ilo_hw->cdev.dev);
-	for (i = minor; i < minor + MAX_CCB; i++)
+	for (i = minor; i < minor + max_ccb; i++)
 		device_destroy(ilo_class, MKDEV(ilo_major, i));
 
 	cdev_del(&ilo_hw->cdev);
@@ -737,7 +738,7 @@ static void ilo_remove(struct pci_dev *pdev)
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
 	kfree(ilo_hw);
-	ilo_hwdev[(minor / MAX_CCB)] = 0;
+	ilo_hwdev[(minor / max_ccb)] = 0;
 }
 
 static int __devinit ilo_probe(struct pci_dev *pdev,
@@ -746,6 +747,11 @@ static int __devinit ilo_probe(struct pci_dev *pdev,
 	int devnum, minor, start, error;
 	struct ilo_hwinfo *ilo_hw;
 
+	if (max_ccb > MAX_CCB)
+		max_ccb = MAX_CCB;
+	else if (max_ccb < MIN_CCB)
+		max_ccb = MIN_CCB;
+
 	/* find a free range for device files */
 	for (devnum = 0; devnum < MAX_ILO_DEV; devnum++) {
 		if (ilo_hwdev[devnum] == 0) {
@@ -795,14 +801,14 @@ static int __devinit ilo_probe(struct pci_dev *pdev,
 
 	cdev_init(&ilo_hw->cdev, &ilo_fops);
 	ilo_hw->cdev.owner = THIS_MODULE;
-	start = devnum * MAX_CCB;
-	error = cdev_add(&ilo_hw->cdev, MKDEV(ilo_major, start), MAX_CCB);
+	start = devnum * max_ccb;
+	error = cdev_add(&ilo_hw->cdev, MKDEV(ilo_major, start), max_ccb);
 	if (error) {
 		dev_err(&pdev->dev, "Could not add cdev\n");
 		goto remove_isr;
 	}
 
-	for (minor = 0 ; minor < MAX_CCB; minor++) {
+	for (minor = 0 ; minor < max_ccb; minor++) {
 		struct device *dev;
 		dev = device_create(ilo_class, &pdev->dev,
 				    MKDEV(ilo_major, minor), NULL,
@@ -879,11 +885,14 @@ static void __exit ilo_exit(void)
 	class_destroy(ilo_class);
 }
 
-MODULE_VERSION("1.2");
+MODULE_VERSION("1.3");
 MODULE_ALIAS(ILO_NAME);
 MODULE_DESCRIPTION(ILO_NAME);
 MODULE_AUTHOR("David Altobelli <david.altobelli@hp.com>");
 MODULE_LICENSE("GPL v2");
 
+module_param(max_ccb, uint, 0444);
+MODULE_PARM_DESC(max_ccb, "Maximum number of HP iLO channels to attach (8)");
+
 module_init(ilo_init);
 module_exit(ilo_exit);
diff --git a/drivers/misc/hpilo.h b/drivers/misc/hpilo.h
index 54e43adbdea..b97672e0cf9 100644
--- a/drivers/misc/hpilo.h
+++ b/drivers/misc/hpilo.h
@@ -14,7 +14,9 @@
 #define ILO_NAME "hpilo"
 
 /* max number of open channel control blocks per device, hw limited to 32 */
-#define MAX_CCB		8
+#define MAX_CCB	       24
+/* min number of open channel control blocks per device, hw limited to 32 */
+#define MIN_CCB		8
 /* max number of supported devices */
 #define MAX_ILO_DEV	1
 /* max number of files */
diff --git a/drivers/misc/iwmc3200top/Kconfig b/drivers/misc/iwmc3200top/Kconfig
deleted file mode 100644
index 9e4b88fb57f..00000000000
--- a/drivers/misc/iwmc3200top/Kconfig
+++ /dev/null
@@ -1,20 +0,0 @@
-config IWMC3200TOP
-        tristate "Intel Wireless MultiCom Top Driver"
-        depends on MMC && EXPERIMENTAL
-        select FW_LOADER
-	---help---
-	  Intel Wireless MultiCom 3200 Top driver is responsible for
-	  for firmware load and enabled coms enumeration
-
-config IWMC3200TOP_DEBUG
-	bool "Enable full debug output of iwmc3200top Driver"
-	depends on IWMC3200TOP
-	---help---
-	  Enable full debug output of iwmc3200top Driver
-
-config IWMC3200TOP_DEBUGFS
-	bool "Enable Debugfs debugging interface for iwmc3200top"
-	depends on IWMC3200TOP
-	---help---
-	  Enable creation of debugfs files for iwmc3200top
-
diff --git a/drivers/misc/iwmc3200top/Makefile b/drivers/misc/iwmc3200top/Makefile
deleted file mode 100644
index fbf53fb4634..00000000000
--- a/drivers/misc/iwmc3200top/Makefile
+++ /dev/null
@@ -1,29 +0,0 @@
-# iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
-# drivers/misc/iwmc3200top/Makefile
-#
-# Copyright (C) 2009 Intel Corporation. All rights reserved.
-#
-# This program is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License version
-# 2 as published by the Free Software Foundation.
-#
-# This program is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-# GNU General Public License for more details.
-#
-# You 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.
-#
-#
-# Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
-#  -
-#
-#
-
-obj-$(CONFIG_IWMC3200TOP)	+= iwmc3200top.o
-iwmc3200top-objs	:= main.o fw-download.o
-iwmc3200top-$(CONFIG_IWMC3200TOP_DEBUG) += log.o
-iwmc3200top-$(CONFIG_IWMC3200TOP_DEBUGFS) += debugfs.o
diff --git a/drivers/misc/iwmc3200top/debugfs.c b/drivers/misc/iwmc3200top/debugfs.c
deleted file mode 100644
index 62fbaec4820..00000000000
--- a/drivers/misc/iwmc3200top/debugfs.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/debufs.c
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/ctype.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/mmc/sdio.h>
-#include <linux/debugfs.h>
-
-#include "iwmc3200top.h"
-#include "fw-msg.h"
-#include "log.h"
-#include "debugfs.h"
-
-
-
-/*      Constants definition        */
-#define HEXADECIMAL_RADIX	16
-
-/*      Functions definition        */
-
-
-#define DEBUGFS_ADD(name, parent) do {					\
-	dbgfs->dbgfs_##parent##_files.file_##name =			\
-	debugfs_create_file(#name, 0644, dbgfs->dir_##parent, priv,	\
-				&iwmct_dbgfs_##name##_ops);		\
-} while (0)
-
-#define DEBUGFS_RM(name)  do {		\
-	debugfs_remove(name);		\
-	name = NULL;			\
-} while (0)
-
-#define DEBUGFS_READ_FUNC(name)						\
-ssize_t iwmct_dbgfs_##name##_read(struct file *file,			\
-				  char __user *user_buf,		\
-				  size_t count, loff_t *ppos);
-
-#define DEBUGFS_WRITE_FUNC(name)					\
-ssize_t iwmct_dbgfs_##name##_write(struct file *file,			\
-				   const char __user *user_buf,		\
-				   size_t count, loff_t *ppos);
-
-#define DEBUGFS_READ_FILE_OPS(name)					\
-	DEBUGFS_READ_FUNC(name)						\
-	static const struct file_operations iwmct_dbgfs_##name##_ops = {  \
-		.read = iwmct_dbgfs_##name##_read,			\
-		.open = iwmct_dbgfs_open_file_generic,			\
-		.llseek = generic_file_llseek,				\
-	};
-
-#define DEBUGFS_WRITE_FILE_OPS(name)					\
-	DEBUGFS_WRITE_FUNC(name)					\
-	static const struct file_operations iwmct_dbgfs_##name##_ops = {  \
-		.write = iwmct_dbgfs_##name##_write,			\
-		.open = iwmct_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 iwmct_dbgfs_##name##_ops = {\
-		.write = iwmct_dbgfs_##name##_write,			\
-		.read = iwmct_dbgfs_##name##_read,			\
-		.open = iwmct_dbgfs_open_file_generic,			\
-		.llseek = generic_file_llseek,				\
-	};
-
-
-/*      Debugfs file ops definitions        */
-
-/*
- * Create the debugfs files and directories
- *
- */
-void iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name)
-{
-	struct iwmct_debugfs *dbgfs;
-
-	dbgfs = kzalloc(sizeof(struct iwmct_debugfs), GFP_KERNEL);
-	if (!dbgfs) {
-		LOG_ERROR(priv, DEBUGFS, "failed to allocate %zd bytes\n",
-					sizeof(struct iwmct_debugfs));
-		return;
-	}
-
-	priv->dbgfs = dbgfs;
-	dbgfs->name = name;
-	dbgfs->dir_drv = debugfs_create_dir(name, NULL);
-	if (!dbgfs->dir_drv) {
-		LOG_ERROR(priv, DEBUGFS, "failed to create debugfs dir\n");
-		return;
-	}
-
-	return;
-}
-
-/**
- * Remove the debugfs files and directories
- *
- */
-void iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs)
-{
-	if (!dbgfs)
-		return;
-
-	DEBUGFS_RM(dbgfs->dir_drv);
-	kfree(dbgfs);
-	dbgfs = NULL;
-}
-
diff --git a/drivers/misc/iwmc3200top/debugfs.h b/drivers/misc/iwmc3200top/debugfs.h
deleted file mode 100644
index 71d45759b40..00000000000
--- a/drivers/misc/iwmc3200top/debugfs.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/debufs.h
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#ifndef __DEBUGFS_H__
-#define __DEBUGFS_H__
-
-
-#ifdef CONFIG_IWMC3200TOP_DEBUGFS
-
-struct iwmct_debugfs {
-	const char *name;
-	struct dentry *dir_drv;
-	struct dir_drv_files {
-	} dbgfs_drv_files;
-};
-
-void iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name);
-void iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs);
-
-#else /* CONFIG_IWMC3200TOP_DEBUGFS */
-
-struct iwmct_debugfs;
-
-static inline void
-iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name)
-{}
-
-static inline void
-iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs)
-{}
-
-#endif /* CONFIG_IWMC3200TOP_DEBUGFS */
-
-#endif /* __DEBUGFS_H__ */
-
diff --git a/drivers/misc/iwmc3200top/fw-download.c b/drivers/misc/iwmc3200top/fw-download.c
deleted file mode 100644
index e27afde6e99..00000000000
--- a/drivers/misc/iwmc3200top/fw-download.c
+++ /dev/null
@@ -1,358 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/fw-download.c
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#include <linux/firmware.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/slab.h>
-#include <asm/unaligned.h>
-
-#include "iwmc3200top.h"
-#include "log.h"
-#include "fw-msg.h"
-
-#define CHECKSUM_BYTES_NUM sizeof(u32)
-
-/**
-  init parser struct with file
- */
-static int iwmct_fw_parser_init(struct iwmct_priv *priv, const u8 *file,
-			      size_t file_size, size_t block_size)
-{
-	struct iwmct_parser *parser = &priv->parser;
-	struct iwmct_fw_hdr *fw_hdr = &parser->versions;
-
-	LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
-
-	LOG_INFO(priv, FW_DOWNLOAD, "file_size=%zd\n", file_size);
-
-	parser->file = file;
-	parser->file_size = file_size;
-	parser->cur_pos = 0;
-	parser->entry_point = 0;
-	parser->buf = kzalloc(block_size, GFP_KERNEL);
-	if (!parser->buf) {
-		LOG_ERROR(priv, FW_DOWNLOAD, "kzalloc error\n");
-		return -ENOMEM;
-	}
-	parser->buf_size = block_size;
-
-	/* extract fw versions */
-	memcpy(fw_hdr, parser->file, sizeof(struct iwmct_fw_hdr));
-	LOG_INFO(priv, FW_DOWNLOAD, "fw versions are:\n"
-		"top %u.%u.%u gps %u.%u.%u bt %u.%u.%u tic %s\n",
-		fw_hdr->top_major, fw_hdr->top_minor, fw_hdr->top_revision,
-		fw_hdr->gps_major, fw_hdr->gps_minor, fw_hdr->gps_revision,
-		fw_hdr->bt_major, fw_hdr->bt_minor, fw_hdr->bt_revision,
-		fw_hdr->tic_name);
-
-	parser->cur_pos += sizeof(struct iwmct_fw_hdr);
-
-	LOG_TRACE(priv, FW_DOWNLOAD, "<--\n");
-	return 0;
-}
-
-static bool iwmct_checksum(struct iwmct_priv *priv)
-{
-	struct iwmct_parser *parser = &priv->parser;
-	__le32 *file = (__le32 *)parser->file;
-	int i, pad, steps;
-	u32 accum = 0;
-	u32 checksum;
-	u32 mask = 0xffffffff;
-
-	pad = (parser->file_size - CHECKSUM_BYTES_NUM) % 4;
-	steps =  (parser->file_size - CHECKSUM_BYTES_NUM) / 4;
-
-	LOG_INFO(priv, FW_DOWNLOAD, "pad=%d steps=%d\n", pad, steps);
-
-	for (i = 0; i < steps; i++)
-		accum += le32_to_cpu(file[i]);
-
-	if (pad) {
-		mask <<= 8 * (4 - pad);
-		accum += le32_to_cpu(file[steps]) & mask;
-	}
-
-	checksum = get_unaligned_le32((__le32 *)(parser->file +
-			parser->file_size - CHECKSUM_BYTES_NUM));
-
-	LOG_INFO(priv, FW_DOWNLOAD,
-		"compare checksum accum=0x%x to checksum=0x%x\n",
-		accum, checksum);
-
-	return checksum == accum;
-}
-
-static int iwmct_parse_next_section(struct iwmct_priv *priv, const u8 **p_sec,
-				  size_t *sec_size, __le32 *sec_addr)
-{
-	struct iwmct_parser *parser = &priv->parser;
-	struct iwmct_dbg *dbg = &priv->dbg;
-	struct iwmct_fw_sec_hdr *sec_hdr;
-
-	LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
-
-	while (parser->cur_pos + sizeof(struct iwmct_fw_sec_hdr)
-		<= parser->file_size) {
-
-		sec_hdr = (struct iwmct_fw_sec_hdr *)
-				(parser->file + parser->cur_pos);
-		parser->cur_pos += sizeof(struct iwmct_fw_sec_hdr);
-
-		LOG_INFO(priv, FW_DOWNLOAD,
-			"sec hdr: type=%s addr=0x%x size=%d\n",
-			sec_hdr->type, sec_hdr->target_addr,
-			sec_hdr->data_size);
-
-		if (strcmp(sec_hdr->type, "ENT") == 0)
-			parser->entry_point = le32_to_cpu(sec_hdr->target_addr);
-		else if (strcmp(sec_hdr->type, "LBL") == 0)
-			strcpy(dbg->label_fw, parser->file + parser->cur_pos);
-		else if (((strcmp(sec_hdr->type, "TOP") == 0) &&
-			  (priv->barker & BARKER_DNLOAD_TOP_MSK)) ||
-			 ((strcmp(sec_hdr->type, "GPS") == 0) &&
-			  (priv->barker & BARKER_DNLOAD_GPS_MSK)) ||
-			 ((strcmp(sec_hdr->type, "BTH") == 0) &&
-			  (priv->barker & BARKER_DNLOAD_BT_MSK))) {
-			*sec_addr = sec_hdr->target_addr;
-			*sec_size = le32_to_cpu(sec_hdr->data_size);
-			*p_sec = parser->file + parser->cur_pos;
-			parser->cur_pos += le32_to_cpu(sec_hdr->data_size);
-			return 1;
-		} else if (strcmp(sec_hdr->type, "LOG") != 0)
-			LOG_WARNING(priv, FW_DOWNLOAD,
-				    "skipping section type %s\n",
-				    sec_hdr->type);
-
-		parser->cur_pos += le32_to_cpu(sec_hdr->data_size);
-		LOG_INFO(priv, FW_DOWNLOAD,
-			"finished with section cur_pos=%zd\n", parser->cur_pos);
-	}
-
-	LOG_TRACE(priv, INIT, "<--\n");
-	return 0;
-}
-
-static int iwmct_download_section(struct iwmct_priv *priv, const u8 *p_sec,
-				size_t sec_size, __le32 addr)
-{
-	struct iwmct_parser *parser = &priv->parser;
-	struct iwmct_fw_load_hdr *hdr = (struct iwmct_fw_load_hdr *)parser->buf;
-	const u8 *cur_block = p_sec;
-	size_t sent = 0;
-	int cnt = 0;
-	int ret = 0;
-	u32 cmd = 0;
-
-	LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
-	LOG_INFO(priv, FW_DOWNLOAD, "Download address 0x%x size 0x%zx\n",
-				addr, sec_size);
-
-	while (sent < sec_size) {
-		int i;
-		u32 chksm = 0;
-		u32 reset = atomic_read(&priv->reset);
-		/* actual FW data */
-		u32 data_size = min(parser->buf_size - sizeof(*hdr),
-				    sec_size - sent);
-		/* Pad to block size */
-		u32 trans_size = (data_size + sizeof(*hdr) +
-				  IWMC_SDIO_BLK_SIZE - 1) &
-				  ~(IWMC_SDIO_BLK_SIZE - 1);
-		++cnt;
-
-		/* in case of reset, interrupt FW DOWNLAOD */
-		if (reset) {
-			LOG_INFO(priv, FW_DOWNLOAD,
-				 "Reset detected. Abort FW download!!!");
-			ret = -ECANCELED;
-			goto exit;
-		}
-
-		memset(parser->buf, 0, parser->buf_size);
-		cmd |= IWMC_OPCODE_WRITE << CMD_HDR_OPCODE_POS;
-		cmd |= IWMC_CMD_SIGNATURE << CMD_HDR_SIGNATURE_POS;
-		cmd |= (priv->dbg.direct ? 1 : 0) << CMD_HDR_DIRECT_ACCESS_POS;
-		cmd |= (priv->dbg.checksum ? 1 : 0) << CMD_HDR_USE_CHECKSUM_POS;
-		hdr->data_size = cpu_to_le32(data_size);
-		hdr->target_addr = addr;
-
-		/* checksum is allowed for sizes divisible by 4 */
-		if (data_size & 0x3)
-			cmd &= ~CMD_HDR_USE_CHECKSUM_MSK;
-
-		memcpy(hdr->data, cur_block, data_size);
-
-
-		if (cmd & CMD_HDR_USE_CHECKSUM_MSK) {
-
-			chksm = data_size + le32_to_cpu(addr) + cmd;
-			for (i = 0; i < data_size >> 2; i++)
-				chksm += ((u32 *)cur_block)[i];
-
-			hdr->block_chksm = cpu_to_le32(chksm);
-			LOG_INFO(priv, FW_DOWNLOAD, "Checksum = 0x%X\n",
-				 hdr->block_chksm);
-		}
-
-		LOG_INFO(priv, FW_DOWNLOAD, "trans#%d, len=%d, sent=%zd, "
-				"sec_size=%zd, startAddress 0x%X\n",
-				cnt, trans_size, sent, sec_size, addr);
-
-		if (priv->dbg.dump)
-			LOG_HEXDUMP(FW_DOWNLOAD, parser->buf, trans_size);
-
-
-		hdr->cmd = cpu_to_le32(cmd);
-		/* send it down */
-		/* TODO: add more proper sending and error checking */
-		ret = iwmct_tx(priv, parser->buf, trans_size);
-		if (ret != 0) {
-			LOG_INFO(priv, FW_DOWNLOAD,
-				"iwmct_tx returned %d\n", ret);
-			goto exit;
-		}
-
-		addr = cpu_to_le32(le32_to_cpu(addr) + data_size);
-		sent += data_size;
-		cur_block = p_sec + sent;
-
-		if (priv->dbg.blocks && (cnt + 1) >= priv->dbg.blocks) {
-			LOG_INFO(priv, FW_DOWNLOAD,
-				"Block number limit is reached [%d]\n",
-				priv->dbg.blocks);
-			break;
-		}
-	}
-
-	if (sent < sec_size)
-		ret = -EINVAL;
-exit:
-	LOG_TRACE(priv, FW_DOWNLOAD, "<--\n");
-	return ret;
-}
-
-static int iwmct_kick_fw(struct iwmct_priv *priv, bool jump)
-{
-	struct iwmct_parser *parser = &priv->parser;
-	struct iwmct_fw_load_hdr *hdr = (struct iwmct_fw_load_hdr *)parser->buf;
-	int ret;
-	u32 cmd;
-
-	LOG_TRACE(priv, FW_DOWNLOAD, "-->\n");
-
-	memset(parser->buf, 0, parser->buf_size);
-	cmd = IWMC_CMD_SIGNATURE << CMD_HDR_SIGNATURE_POS;
-	if (jump) {
-		cmd |= IWMC_OPCODE_JUMP << CMD_HDR_OPCODE_POS;
-		hdr->target_addr = cpu_to_le32(parser->entry_point);
-		LOG_INFO(priv, FW_DOWNLOAD, "jump address 0x%x\n",
-				parser->entry_point);
-	} else {
-		cmd |= IWMC_OPCODE_LAST_COMMAND << CMD_HDR_OPCODE_POS;
-		LOG_INFO(priv, FW_DOWNLOAD, "last command\n");
-	}
-
-	hdr->cmd = cpu_to_le32(cmd);
-
-	LOG_HEXDUMP(FW_DOWNLOAD, parser->buf, sizeof(*hdr));
-	/* send it down */
-	/* TODO: add more proper sending and error checking */
-	ret = iwmct_tx(priv, parser->buf, IWMC_SDIO_BLK_SIZE);
-	if (ret)
-		LOG_INFO(priv, FW_DOWNLOAD, "iwmct_tx returned %d", ret);
-
-	LOG_TRACE(priv, FW_DOWNLOAD, "<--\n");
-	return 0;
-}
-
-int iwmct_fw_load(struct iwmct_priv *priv)
-{
-	const u8 *fw_name = FW_NAME(FW_API_VER);
-	const struct firmware *raw;
-	const u8 *pdata;
-	size_t len;
-	__le32 addr;
-	int ret;
-
-
-	LOG_INFO(priv, FW_DOWNLOAD, "barker download request 0x%x is:\n",
-			priv->barker);
-	LOG_INFO(priv, FW_DOWNLOAD, "*******  Top FW %s requested ********\n",
-			(priv->barker & BARKER_DNLOAD_TOP_MSK) ? "was" : "not");
-	LOG_INFO(priv, FW_DOWNLOAD, "*******  GPS FW %s requested ********\n",
-			(priv->barker & BARKER_DNLOAD_GPS_MSK) ? "was" : "not");
-	LOG_INFO(priv, FW_DOWNLOAD, "*******  BT FW %s requested ********\n",
-			(priv->barker & BARKER_DNLOAD_BT_MSK) ? "was" : "not");
-
-
-	/* get the firmware */
-	ret = request_firmware(&raw, fw_name, &priv->func->dev);
-	if (ret < 0) {
-		LOG_ERROR(priv, FW_DOWNLOAD, "%s request_firmware failed %d\n",
-			  fw_name, ret);
-		goto exit;
-	}
-
-	if (raw->size < sizeof(struct iwmct_fw_sec_hdr)) {
-		LOG_ERROR(priv, FW_DOWNLOAD, "%s smaller then (%zd) (%zd)\n",
-			  fw_name, sizeof(struct iwmct_fw_sec_hdr), raw->size);
-		goto exit;
-	}
-
-	LOG_INFO(priv, FW_DOWNLOAD, "Read firmware '%s'\n", fw_name);
-
-	/* clear parser struct */
-	ret = iwmct_fw_parser_init(priv, raw->data, raw->size, priv->trans_len);
-	if (ret < 0) {
-		LOG_ERROR(priv, FW_DOWNLOAD,
-			  "iwmct_parser_init failed: Reason %d\n", ret);
-		goto exit;
-	}
-
-	if (!iwmct_checksum(priv)) {
-		LOG_ERROR(priv, FW_DOWNLOAD, "checksum error\n");
-		ret = -EINVAL;
-		goto exit;
-	}
-
-	/* download firmware to device */
-	while (iwmct_parse_next_section(priv, &pdata, &len, &addr)) {
-		ret = iwmct_download_section(priv, pdata, len, addr);
-		if (ret) {
-			LOG_ERROR(priv, FW_DOWNLOAD,
-				  "%s download section failed\n", fw_name);
-			goto exit;
-		}
-	}
-
-	ret = iwmct_kick_fw(priv, !!(priv->barker & BARKER_DNLOAD_JUMP_MSK));
-
-exit:
-	kfree(priv->parser.buf);
-	release_firmware(raw);
-	return ret;
-}
diff --git a/drivers/misc/iwmc3200top/fw-msg.h b/drivers/misc/iwmc3200top/fw-msg.h
deleted file mode 100644
index 9e26b75bd48..00000000000
--- a/drivers/misc/iwmc3200top/fw-msg.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/fw-msg.h
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#ifndef __FWMSG_H__
-#define __FWMSG_H__
-
-#define COMM_TYPE_D2H           	0xFF
-#define COMM_TYPE_H2D           	0xEE
-
-#define COMM_CATEGORY_OPERATIONAL      	0x00
-#define COMM_CATEGORY_DEBUG            	0x01
-#define COMM_CATEGORY_TESTABILITY      	0x02
-#define COMM_CATEGORY_DIAGNOSTICS      	0x03
-
-#define OP_DBG_ZSTR_MSG			cpu_to_le16(0x1A)
-
-#define FW_LOG_SRC_MAX			32
-#define FW_LOG_SRC_ALL			255
-
-#define FW_STRING_TABLE_ADDR		cpu_to_le32(0x0C000000)
-
-#define CMD_DBG_LOG_LEVEL		cpu_to_le16(0x0001)
-#define CMD_TST_DEV_RESET		cpu_to_le16(0x0060)
-#define CMD_TST_FUNC_RESET		cpu_to_le16(0x0062)
-#define CMD_TST_IFACE_RESET		cpu_to_le16(0x0064)
-#define CMD_TST_CPU_UTILIZATION		cpu_to_le16(0x0065)
-#define CMD_TST_TOP_DEEP_SLEEP		cpu_to_le16(0x0080)
-#define CMD_TST_WAKEUP			cpu_to_le16(0x0081)
-#define CMD_TST_FUNC_WAKEUP		cpu_to_le16(0x0082)
-#define CMD_TST_FUNC_DEEP_SLEEP_REQUEST	cpu_to_le16(0x0083)
-#define CMD_TST_GET_MEM_DUMP		cpu_to_le16(0x0096)
-
-#define OP_OPR_ALIVE			cpu_to_le16(0x0010)
-#define OP_OPR_CMD_ACK			cpu_to_le16(0x001F)
-#define OP_OPR_CMD_NACK			cpu_to_le16(0x0020)
-#define OP_TST_MEM_DUMP			cpu_to_le16(0x0043)
-
-#define CMD_FLAG_PADDING_256		0x80
-
-#define FW_HCMD_BLOCK_SIZE      	256
-
-struct msg_hdr {
-	u8 type;
-	u8 category;
-	__le16 opcode;
-	u8 seqnum;
-	u8 flags;
-	__le16 length;
-} __attribute__((__packed__));
-
-struct log_hdr {
-	__le32 timestamp;
-	u8 severity;
-	u8 logsource;
-	__le16 reserved;
-} __attribute__((__packed__));
-
-struct mdump_hdr {
-	u8 dmpid;
-	u8 frag;
-	__le16 size;
-	__le32 addr;
-} __attribute__((__packed__));
-
-struct top_msg {
-	struct msg_hdr hdr;
-	union {
-		/* D2H messages */
-		struct {
-			struct log_hdr log_hdr;
-			u8 data[1];
-		} __attribute__((__packed__)) log;
-
-		struct {
-			struct log_hdr log_hdr;
-			struct mdump_hdr md_hdr;
-			u8 data[1];
-		} __attribute__((__packed__)) mdump;
-
-		/* H2D messages */
-		struct {
-			u8 logsource;
-			u8 sevmask;
-		} __attribute__((__packed__)) logdefs[FW_LOG_SRC_MAX];
-		struct mdump_hdr mdump_req;
-	} u;
-} __attribute__((__packed__));
-
-
-#endif /* __FWMSG_H__ */
diff --git a/drivers/misc/iwmc3200top/iwmc3200top.h b/drivers/misc/iwmc3200top/iwmc3200top.h
deleted file mode 100644
index 620973ed8bf..00000000000
--- a/drivers/misc/iwmc3200top/iwmc3200top.h
+++ /dev/null
@@ -1,205 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/iwmc3200top.h
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#ifndef __IWMC3200TOP_H__
-#define __IWMC3200TOP_H__
-
-#include <linux/workqueue.h>
-
-#define DRV_NAME "iwmc3200top"
-#define FW_API_VER 1
-#define _FW_NAME(api) DRV_NAME "." #api ".fw"
-#define FW_NAME(api) _FW_NAME(api)
-
-#define IWMC_SDIO_BLK_SIZE			256
-#define IWMC_DEFAULT_TR_BLK			64
-#define IWMC_SDIO_DATA_ADDR			0x0
-#define IWMC_SDIO_INTR_ENABLE_ADDR		0x14
-#define IWMC_SDIO_INTR_STATUS_ADDR		0x13
-#define IWMC_SDIO_INTR_CLEAR_ADDR		0x13
-#define IWMC_SDIO_INTR_GET_SIZE_ADDR		0x2C
-
-#define COMM_HUB_HEADER_LENGTH 16
-#define LOGGER_HEADER_LENGTH   10
-
-
-#define BARKER_DNLOAD_BT_POS		0
-#define BARKER_DNLOAD_BT_MSK		BIT(BARKER_DNLOAD_BT_POS)
-#define BARKER_DNLOAD_GPS_POS		1
-#define BARKER_DNLOAD_GPS_MSK		BIT(BARKER_DNLOAD_GPS_POS)
-#define BARKER_DNLOAD_TOP_POS		2
-#define BARKER_DNLOAD_TOP_MSK		BIT(BARKER_DNLOAD_TOP_POS)
-#define BARKER_DNLOAD_RESERVED1_POS	3
-#define BARKER_DNLOAD_RESERVED1_MSK	BIT(BARKER_DNLOAD_RESERVED1_POS)
-#define BARKER_DNLOAD_JUMP_POS		4
-#define BARKER_DNLOAD_JUMP_MSK		BIT(BARKER_DNLOAD_JUMP_POS)
-#define BARKER_DNLOAD_SYNC_POS		5
-#define BARKER_DNLOAD_SYNC_MSK		BIT(BARKER_DNLOAD_SYNC_POS)
-#define BARKER_DNLOAD_RESERVED2_POS	6
-#define BARKER_DNLOAD_RESERVED2_MSK	(0x3 << BARKER_DNLOAD_RESERVED2_POS)
-#define BARKER_DNLOAD_BARKER_POS	8
-#define BARKER_DNLOAD_BARKER_MSK	(0xffffff << BARKER_DNLOAD_BARKER_POS)
-
-#define IWMC_BARKER_REBOOT 	(0xdeadbe << BARKER_DNLOAD_BARKER_POS)
-/* whole field barker */
-#define IWMC_BARKER_ACK 	0xfeedbabe
-
-#define IWMC_CMD_SIGNATURE 	0xcbbc
-
-#define CMD_HDR_OPCODE_POS		0
-#define CMD_HDR_OPCODE_MSK_MSK		(0xf << CMD_HDR_OPCODE_MSK_POS)
-#define CMD_HDR_RESPONSE_CODE_POS	4
-#define CMD_HDR_RESPONSE_CODE_MSK	(0xf << CMD_HDR_RESPONSE_CODE_POS)
-#define CMD_HDR_USE_CHECKSUM_POS	8
-#define CMD_HDR_USE_CHECKSUM_MSK	BIT(CMD_HDR_USE_CHECKSUM_POS)
-#define CMD_HDR_RESPONSE_REQUIRED_POS	9
-#define CMD_HDR_RESPONSE_REQUIRED_MSK	BIT(CMD_HDR_RESPONSE_REQUIRED_POS)
-#define CMD_HDR_DIRECT_ACCESS_POS	10
-#define CMD_HDR_DIRECT_ACCESS_MSK	BIT(CMD_HDR_DIRECT_ACCESS_POS)
-#define CMD_HDR_RESERVED_POS		11
-#define CMD_HDR_RESERVED_MSK		BIT(0x1f << CMD_HDR_RESERVED_POS)
-#define CMD_HDR_SIGNATURE_POS		16
-#define CMD_HDR_SIGNATURE_MSK		BIT(0xffff << CMD_HDR_SIGNATURE_POS)
-
-enum {
-	IWMC_OPCODE_PING = 0,
-	IWMC_OPCODE_READ = 1,
-	IWMC_OPCODE_WRITE = 2,
-	IWMC_OPCODE_JUMP = 3,
-	IWMC_OPCODE_REBOOT = 4,
-	IWMC_OPCODE_PERSISTENT_WRITE = 5,
-	IWMC_OPCODE_PERSISTENT_READ = 6,
-	IWMC_OPCODE_READ_MODIFY_WRITE = 7,
-	IWMC_OPCODE_LAST_COMMAND = 15
-};
-
-struct iwmct_fw_load_hdr {
-	__le32 cmd;
-	__le32 target_addr;
-	__le32 data_size;
-	__le32 block_chksm;
-	u8 data[0];
-};
-
-/**
- * struct iwmct_fw_hdr
- * holds all sw components versions
- */
-struct iwmct_fw_hdr {
-	u8 top_major;
-	u8 top_minor;
-	u8 top_revision;
-	u8 gps_major;
-	u8 gps_minor;
-	u8 gps_revision;
-	u8 bt_major;
-	u8 bt_minor;
-	u8 bt_revision;
-	u8 tic_name[31];
-};
-
-/**
- * struct iwmct_fw_sec_hdr
- * @type: function type
- * @data_size: section's data size
- * @target_addr: download address
- */
-struct iwmct_fw_sec_hdr {
-	u8 type[4];
-	__le32 data_size;
-	__le32 target_addr;
-};
-
-/**
- * struct iwmct_parser
- * @file: fw image
- * @file_size: fw size
- * @cur_pos: position in file
- * @buf: temp buf for download
- * @buf_size: size of buf
- * @entry_point: address to jump in fw kick-off
- */
-struct iwmct_parser {
-	const u8 *file;
-	size_t file_size;
-	size_t cur_pos;
-	u8 *buf;
-	size_t buf_size;
-	u32 entry_point;
-	struct iwmct_fw_hdr versions;
-};
-
-
-struct iwmct_work_struct {
-	struct list_head list;
-	ssize_t iosize;
-};
-
-struct iwmct_dbg {
-	int blocks;
-	bool dump;
-	bool jump;
-	bool direct;
-	bool checksum;
-	bool fw_download;
-	int block_size;
-	int download_trans_blks;
-
-	char label_fw[256];
-};
-
-struct iwmct_debugfs;
-
-struct iwmct_priv {
-	struct sdio_func *func;
-	struct iwmct_debugfs *dbgfs;
-	struct iwmct_parser parser;
-	atomic_t reset;
-	atomic_t dev_sync;
-	u32 trans_len;
-	u32 barker;
-	struct iwmct_dbg dbg;
-
-	/* drivers work items */
-	struct work_struct bus_rescan_worker;
-	struct work_struct isr_worker;
-
-	/* drivers wait queue */
-	wait_queue_head_t wait_q;
-
-	/* rx request list */
-	struct list_head read_req_list;
-};
-
-extern int iwmct_tx(struct iwmct_priv *priv, void *src, int count);
-extern int iwmct_fw_load(struct iwmct_priv *priv);
-
-extern void iwmct_dbg_init_params(struct iwmct_priv *drv);
-extern void iwmct_dbg_init_drv_attrs(struct device_driver *drv);
-extern void iwmct_dbg_remove_drv_attrs(struct device_driver *drv);
-extern int iwmct_send_hcmd(struct iwmct_priv *priv, u8 *cmd, u16 len);
-
-#endif  /*  __IWMC3200TOP_H__  */
diff --git a/drivers/misc/iwmc3200top/log.c b/drivers/misc/iwmc3200top/log.c
deleted file mode 100644
index a36a55a49ca..00000000000
--- a/drivers/misc/iwmc3200top/log.c
+++ /dev/null
@@ -1,348 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/log.c
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/slab.h>
-#include <linux/ctype.h>
-#include "fw-msg.h"
-#include "iwmc3200top.h"
-#include "log.h"
-
-/* Maximal hexadecimal string size of the FW memdump message */
-#define LOG_MSG_SIZE_MAX		12400
-
-/* iwmct_logdefs is a global used by log macros */
-u8 iwmct_logdefs[LOG_SRC_MAX];
-static u8 iwmct_fw_logdefs[FW_LOG_SRC_MAX];
-
-
-static int _log_set_log_filter(u8 *logdefs, int size, u8 src, u8 logmask)
-{
-	int i;
-
-	if (src < size)
-		logdefs[src] = logmask;
-	else if (src == LOG_SRC_ALL)
-		for (i = 0; i < size; i++)
-			logdefs[i] = logmask;
-	else
-		return -1;
-
-	return 0;
-}
-
-
-int iwmct_log_set_filter(u8 src, u8 logmask)
-{
-	return _log_set_log_filter(iwmct_logdefs, LOG_SRC_MAX, src, logmask);
-}
-
-
-int iwmct_log_set_fw_filter(u8 src, u8 logmask)
-{
-	return _log_set_log_filter(iwmct_fw_logdefs,
-				   FW_LOG_SRC_MAX, src, logmask);
-}
-
-
-static int log_msg_format_hex(char *str, int slen, u8 *ibuf,
-			      int ilen, char *pref)
-{
-	int pos = 0;
-	int i;
-	int len;
-
-	for (pos = 0, i = 0; pos < slen - 2 && pref[i] != '\0'; i++, pos++)
-		str[pos] = pref[i];
-
-	for (i = 0; pos < slen - 2 && i < ilen; pos += len, i++)
-		len = snprintf(&str[pos], slen - pos - 1, " %2.2X", ibuf[i]);
-
-	if (i < ilen)
-		return -1;
-
-	return 0;
-}
-
-/*	NOTE: This function is not thread safe.
-	Currently it's called only from sdio rx worker - no race there
-*/
-void iwmct_log_top_message(struct iwmct_priv *priv, u8 *buf, int len)
-{
-	struct top_msg *msg;
-	static char logbuf[LOG_MSG_SIZE_MAX];
-
-	msg = (struct top_msg *)buf;
-
-	if (len < sizeof(msg->hdr) + sizeof(msg->u.log.log_hdr)) {
-		LOG_ERROR(priv, FW_MSG, "Log message from TOP "
-			  "is too short %d (expected %zd)\n",
-			  len, sizeof(msg->hdr) + sizeof(msg->u.log.log_hdr));
-		return;
-	}
-
-	if (!(iwmct_fw_logdefs[msg->u.log.log_hdr.logsource] &
-		BIT(msg->u.log.log_hdr.severity)) ||
-	    !(iwmct_logdefs[LOG_SRC_FW_MSG] & BIT(msg->u.log.log_hdr.severity)))
-		return;
-
-	switch (msg->hdr.category) {
-	case COMM_CATEGORY_TESTABILITY:
-		if (!(iwmct_logdefs[LOG_SRC_TST] &
-		      BIT(msg->u.log.log_hdr.severity)))
-			return;
-		if (log_msg_format_hex(logbuf, LOG_MSG_SIZE_MAX, buf,
-				       le16_to_cpu(msg->hdr.length) +
-				       sizeof(msg->hdr), "<TST>"))
-			LOG_WARNING(priv, TST,
-				  "TOP TST message is too long, truncating...");
-		LOG_WARNING(priv, TST, "%s\n", logbuf);
-		break;
-	case COMM_CATEGORY_DEBUG:
-		if (msg->hdr.opcode == OP_DBG_ZSTR_MSG)
-			LOG_INFO(priv, FW_MSG, "%s %s", "<DBG>",
-				       ((u8 *)msg) + sizeof(msg->hdr)
-					+ sizeof(msg->u.log.log_hdr));
-		else {
-			if (log_msg_format_hex(logbuf, LOG_MSG_SIZE_MAX, buf,
-					le16_to_cpu(msg->hdr.length)
-						+ sizeof(msg->hdr),
-					"<DBG>"))
-				LOG_WARNING(priv, FW_MSG,
-					"TOP DBG message is too long,"
-					"truncating...");
-			LOG_WARNING(priv, FW_MSG, "%s\n", logbuf);
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static int _log_get_filter_str(u8 *logdefs, int logdefsz, char *buf, int size)
-{
-	int i, pos, len;
-	for (i = 0, pos = 0; (pos < size-1) && (i < logdefsz); i++) {
-		len = snprintf(&buf[pos], size - pos - 1, "0x%02X%02X,",
-				i, logdefs[i]);
-		pos += len;
-	}
-	buf[pos-1] = '\n';
-	buf[pos] = '\0';
-
-	if (i < logdefsz)
-		return -1;
-	return 0;
-}
-
-int log_get_filter_str(char *buf, int size)
-{
-	return _log_get_filter_str(iwmct_logdefs, LOG_SRC_MAX, buf, size);
-}
-
-int log_get_fw_filter_str(char *buf, int size)
-{
-	return _log_get_filter_str(iwmct_fw_logdefs, FW_LOG_SRC_MAX, buf, size);
-}
-
-#define HEXADECIMAL_RADIX	16
-#define LOG_SRC_FORMAT		7 /* log level is in format of "0xXXXX," */
-
-ssize_t show_iwmct_log_level(struct device *d,
-				struct device_attribute *attr, char *buf)
-{
-	struct iwmct_priv *priv = dev_get_drvdata(d);
-	char *str_buf;
-	int buf_size;
-	ssize_t ret;
-
-	buf_size = (LOG_SRC_FORMAT * LOG_SRC_MAX) + 1;
-	str_buf = kzalloc(buf_size, GFP_KERNEL);
-	if (!str_buf) {
-		LOG_ERROR(priv, DEBUGFS,
-			"failed to allocate %d bytes\n", buf_size);
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	if (log_get_filter_str(str_buf, buf_size) < 0) {
-		ret = -EINVAL;
-		goto exit;
-	}
-
-	ret = sprintf(buf, "%s", str_buf);
-
-exit:
-	kfree(str_buf);
-	return ret;
-}
-
-ssize_t store_iwmct_log_level(struct device *d,
-			struct device_attribute *attr,
-			const char *buf, size_t count)
-{
-	struct iwmct_priv *priv = dev_get_drvdata(d);
-	char *token, *str_buf = NULL;
-	long val;
-	ssize_t ret = count;
-	u8 src, mask;
-
-	if (!count)
-		goto exit;
-
-	str_buf = kzalloc(count, GFP_KERNEL);
-	if (!str_buf) {
-		LOG_ERROR(priv, DEBUGFS,
-			"failed to allocate %zd bytes\n", count);
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	memcpy(str_buf, buf, count);
-
-	while ((token = strsep(&str_buf, ",")) != NULL) {
-		while (isspace(*token))
-			++token;
-		if (strict_strtol(token, HEXADECIMAL_RADIX, &val)) {
-			LOG_ERROR(priv, DEBUGFS,
-				  "failed to convert string to long %s\n",
-				  token);
-			ret = -EINVAL;
-			goto exit;
-		}
-
-		mask  = val & 0xFF;
-		src = (val & 0XFF00) >> 8;
-		iwmct_log_set_filter(src, mask);
-	}
-
-exit:
-	kfree(str_buf);
-	return ret;
-}
-
-ssize_t show_iwmct_log_level_fw(struct device *d,
-			struct device_attribute *attr, char *buf)
-{
-	struct iwmct_priv *priv = dev_get_drvdata(d);
-	char *str_buf;
-	int buf_size;
-	ssize_t ret;
-
-	buf_size = (LOG_SRC_FORMAT * FW_LOG_SRC_MAX) + 2;
-
-	str_buf = kzalloc(buf_size, GFP_KERNEL);
-	if (!str_buf) {
-		LOG_ERROR(priv, DEBUGFS,
-			"failed to allocate %d bytes\n", buf_size);
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	if (log_get_fw_filter_str(str_buf, buf_size) < 0) {
-		ret = -EINVAL;
-		goto exit;
-	}
-
-	ret = sprintf(buf, "%s", str_buf);
-
-exit:
-	kfree(str_buf);
-	return ret;
-}
-
-ssize_t store_iwmct_log_level_fw(struct device *d,
-			struct device_attribute *attr,
-			const char *buf, size_t count)
-{
-	struct iwmct_priv *priv = dev_get_drvdata(d);
-	struct top_msg cmd;
-	char *token, *str_buf = NULL;
-	ssize_t ret = count;
-	u16 cmdlen = 0;
-	int i;
-	long val;
-	u8 src, mask;
-
-	if (!count)
-		goto exit;
-
-	str_buf = kzalloc(count, GFP_KERNEL);
-	if (!str_buf) {
-		LOG_ERROR(priv, DEBUGFS,
-			"failed to allocate %zd bytes\n", count);
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	memcpy(str_buf, buf, count);
-
-	cmd.hdr.type = COMM_TYPE_H2D;
-	cmd.hdr.category = COMM_CATEGORY_DEBUG;
-	cmd.hdr.opcode = CMD_DBG_LOG_LEVEL;
-
-	for (i = 0; ((token = strsep(&str_buf, ",")) != NULL) &&
-		     (i < FW_LOG_SRC_MAX); i++) {
-
-		while (isspace(*token))
-			++token;
-
-		if (strict_strtol(token, HEXADECIMAL_RADIX, &val)) {
-			LOG_ERROR(priv, DEBUGFS,
-				  "failed to convert string to long %s\n",
-				  token);
-			ret = -EINVAL;
-			goto exit;
-		}
-
-		mask  = val & 0xFF; /* LSB */
-		src = (val & 0XFF00) >> 8; /* 2nd least significant byte. */
-		iwmct_log_set_fw_filter(src, mask);
-
-		cmd.u.logdefs[i].logsource = src;
-		cmd.u.logdefs[i].sevmask = mask;
-	}
-
-	cmd.hdr.length = cpu_to_le16(i * sizeof(cmd.u.logdefs[0]));
-	cmdlen = (i * sizeof(cmd.u.logdefs[0]) + sizeof(cmd.hdr));
-
-	ret = iwmct_send_hcmd(priv, (u8 *)&cmd, cmdlen);
-	if (ret) {
-		LOG_ERROR(priv, DEBUGFS,
-			  "Failed to send %d bytes of fwcmd, ret=%zd\n",
-			  cmdlen, ret);
-		goto exit;
-	} else
-		LOG_INFO(priv, DEBUGFS, "fwcmd sent (%d bytes)\n", cmdlen);
-
-	ret = count;
-
-exit:
-	kfree(str_buf);
-	return ret;
-}
-
diff --git a/drivers/misc/iwmc3200top/log.h b/drivers/misc/iwmc3200top/log.h
deleted file mode 100644
index 4434bb16cea..00000000000
--- a/drivers/misc/iwmc3200top/log.h
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/log.h
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#ifndef __LOG_H__
-#define __LOG_H__
-
-
-/* log severity:
- * The log levels here match FW log levels
- * so values need to stay as is */
-#define LOG_SEV_CRITICAL		0
-#define LOG_SEV_ERROR			1
-#define LOG_SEV_WARNING			2
-#define LOG_SEV_INFO			3
-#define LOG_SEV_INFOEX			4
-
-/* Log levels not defined for FW */
-#define LOG_SEV_TRACE			5
-#define LOG_SEV_DUMP			6
-
-#define LOG_SEV_FW_FILTER_ALL		\
-	(BIT(LOG_SEV_CRITICAL)	|	\
-	 BIT(LOG_SEV_ERROR)	|	\
-	 BIT(LOG_SEV_WARNING)	| 	\
-	 BIT(LOG_SEV_INFO)	|	\
-	 BIT(LOG_SEV_INFOEX))
-
-#define LOG_SEV_FILTER_ALL		\
-	(BIT(LOG_SEV_CRITICAL)	|	\
-	 BIT(LOG_SEV_ERROR)	|	\
-	 BIT(LOG_SEV_WARNING)	| 	\
-	 BIT(LOG_SEV_INFO)	|	\
-	 BIT(LOG_SEV_INFOEX)	|	\
-	 BIT(LOG_SEV_TRACE)	|	\
-	 BIT(LOG_SEV_DUMP))
-
-/* log source */
-#define LOG_SRC_INIT			0
-#define LOG_SRC_DEBUGFS			1
-#define LOG_SRC_FW_DOWNLOAD		2
-#define LOG_SRC_FW_MSG			3
-#define LOG_SRC_TST			4
-#define LOG_SRC_IRQ			5
-
-#define	LOG_SRC_MAX			6
-#define	LOG_SRC_ALL			0xFF
-
-/**
- * Default intitialization runtime log level
- */
-#ifndef LOG_SEV_FILTER_RUNTIME
-#define LOG_SEV_FILTER_RUNTIME			\
-	(BIT(LOG_SEV_CRITICAL)	|		\
-	 BIT(LOG_SEV_ERROR)	|		\
-	 BIT(LOG_SEV_WARNING))
-#endif
-
-#ifndef FW_LOG_SEV_FILTER_RUNTIME
-#define FW_LOG_SEV_FILTER_RUNTIME	LOG_SEV_FILTER_ALL
-#endif
-
-#ifdef CONFIG_IWMC3200TOP_DEBUG
-/**
- * Log macros
- */
-
-#define priv2dev(priv) (&(priv->func)->dev)
-
-#define LOG_CRITICAL(priv, src, fmt, args...)				\
-do {									\
-	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_CRITICAL))	\
-		dev_crit(priv2dev(priv), "%s %d: " fmt,			\
-			__func__, __LINE__, ##args);			\
-} while (0)
-
-#define LOG_ERROR(priv, src, fmt, args...)				\
-do {									\
-	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_ERROR))	\
-		dev_err(priv2dev(priv), "%s %d: " fmt,			\
-			__func__, __LINE__, ##args);			\
-} while (0)
-
-#define LOG_WARNING(priv, src, fmt, args...)				\
-do {									\
-	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_WARNING))	\
-		dev_warn(priv2dev(priv), "%s %d: " fmt,			\
-			 __func__, __LINE__, ##args);			\
-} while (0)
-
-#define LOG_INFO(priv, src, fmt, args...)				\
-do {									\
-	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_INFO))		\
-		dev_info(priv2dev(priv), "%s %d: " fmt,			\
-			 __func__, __LINE__, ##args);			\
-} while (0)
-
-#define LOG_TRACE(priv, src, fmt, args...)				\
-do {									\
-	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_TRACE))	\
-		dev_dbg(priv2dev(priv), "%s %d: " fmt,			\
-			 __func__, __LINE__, ##args);			\
-} while (0)
-
-#define LOG_HEXDUMP(src, ptr, len)					\
-do {									\
-	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_DUMP))	\
-		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,	\
-				16, 1, ptr, len, false);		\
-} while (0)
-
-void iwmct_log_top_message(struct iwmct_priv *priv, u8 *buf, int len);
-
-extern u8 iwmct_logdefs[];
-
-int iwmct_log_set_filter(u8 src, u8 logmask);
-int iwmct_log_set_fw_filter(u8 src, u8 logmask);
-
-ssize_t show_iwmct_log_level(struct device *d,
-			struct device_attribute *attr, char *buf);
-ssize_t store_iwmct_log_level(struct device *d,
-			struct device_attribute *attr,
-			const char *buf, size_t count);
-ssize_t show_iwmct_log_level_fw(struct device *d,
-			struct device_attribute *attr, char *buf);
-ssize_t store_iwmct_log_level_fw(struct device *d,
-			struct device_attribute *attr,
-			const char *buf, size_t count);
-
-#else
-
-#define LOG_CRITICAL(priv, src, fmt, args...)
-#define LOG_ERROR(priv, src, fmt, args...)
-#define LOG_WARNING(priv, src, fmt, args...)
-#define LOG_INFO(priv, src, fmt, args...)
-#define LOG_TRACE(priv, src, fmt, args...)
-#define LOG_HEXDUMP(src, ptr, len)
-
-static inline void iwmct_log_top_message(struct iwmct_priv *priv,
-					 u8 *buf, int len) {}
-static inline int iwmct_log_set_filter(u8 src, u8 logmask) { return 0; }
-static inline int iwmct_log_set_fw_filter(u8 src, u8 logmask) { return 0; }
-
-#endif /* CONFIG_IWMC3200TOP_DEBUG */
-
-int log_get_filter_str(char *buf, int size);
-int log_get_fw_filter_str(char *buf, int size);
-
-#endif /* __LOG_H__ */
diff --git a/drivers/misc/iwmc3200top/main.c b/drivers/misc/iwmc3200top/main.c
deleted file mode 100644
index 701eb600b12..00000000000
--- a/drivers/misc/iwmc3200top/main.c
+++ /dev/null
@@ -1,662 +0,0 @@
-/*
- * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
- * drivers/misc/iwmc3200top/main.c
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
- *  -
- *
- */
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/debugfs.h>
-#include <linux/mmc/sdio_ids.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/mmc/sdio.h>
-
-#include "iwmc3200top.h"
-#include "log.h"
-#include "fw-msg.h"
-#include "debugfs.h"
-
-
-#define DRIVER_DESCRIPTION "Intel(R) IWMC 3200 Top Driver"
-#define DRIVER_COPYRIGHT "Copyright (c) 2008 Intel Corporation."
-
-#define DRIVER_VERSION  "0.1.62"
-
-MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
-MODULE_VERSION(DRIVER_VERSION);
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR(DRIVER_COPYRIGHT);
-MODULE_FIRMWARE(FW_NAME(FW_API_VER));
-
-
-static inline int __iwmct_tx(struct iwmct_priv *priv, void *src, int count)
-{
-	return sdio_memcpy_toio(priv->func, IWMC_SDIO_DATA_ADDR, src, count);
-
-}
-int iwmct_tx(struct iwmct_priv *priv, void *src, int count)
-{
-	int ret;
-	sdio_claim_host(priv->func);
-	ret =  __iwmct_tx(priv, src, count);
-	sdio_release_host(priv->func);
-	return ret;
-}
-/*
- * This workers main task is to wait for OP_OPR_ALIVE
- * from TOP FW until ALIVE_MSG_TIMOUT timeout is elapsed.
- * When OP_OPR_ALIVE received it will issue
- * a call to "bus_rescan_devices".
- */
-static void iwmct_rescan_worker(struct work_struct *ws)
-{
-	struct iwmct_priv *priv;
-	int ret;
-
-	priv = container_of(ws, struct iwmct_priv, bus_rescan_worker);
-
-	LOG_INFO(priv, FW_MSG, "Calling bus_rescan\n");
-
-	ret = bus_rescan_devices(priv->func->dev.bus);
-	if (ret < 0)
-		LOG_INFO(priv, INIT, "bus_rescan_devices FAILED!!!\n");
-}
-
-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");
-		schedule_work(&priv->bus_rescan_worker);
-		break;
-	default:
-		LOG_INFO(priv, FW_MSG, "Received msg opcode 0x%X\n",
-			msg->hdr.opcode);
-		break;
-	}
-}
-
-
-static void handle_top_message(struct iwmct_priv *priv, u8 *buf, int len)
-{
-	struct top_msg *msg;
-
-	msg = (struct top_msg *)buf;
-
-	if (msg->hdr.type != COMM_TYPE_D2H) {
-		LOG_ERROR(priv, FW_MSG,
-			"Message from TOP with invalid message type 0x%X\n",
-			msg->hdr.type);
-		return;
-	}
-
-	if (len < sizeof(msg->hdr)) {
-		LOG_ERROR(priv, FW_MSG,
-			"Message from TOP is too short for message header "
-			"received %d bytes, expected at least %zd bytes\n",
-			len, sizeof(msg->hdr));
-		return;
-	}
-
-	if (len < le16_to_cpu(msg->hdr.length) + sizeof(msg->hdr)) {
-		LOG_ERROR(priv, FW_MSG,
-			"Message length (%d bytes) is shorter than "
-			"in header (%d bytes)\n",
-			len, le16_to_cpu(msg->hdr.length));
-		return;
-	}
-
-	switch (msg->hdr.category) {
-	case COMM_CATEGORY_OPERATIONAL:
-		op_top_message(priv, (struct top_msg *)buf);
-		break;
-
-	case COMM_CATEGORY_DEBUG:
-	case COMM_CATEGORY_TESTABILITY:
-	case COMM_CATEGORY_DIAGNOSTICS:
-		iwmct_log_top_message(priv, buf, len);
-		break;
-
-	default:
-		LOG_ERROR(priv, FW_MSG,
-			"Message from TOP with unknown category 0x%X\n",
-			msg->hdr.category);
-		break;
-	}
-}
-
-int iwmct_send_hcmd(struct iwmct_priv *priv, u8 *cmd, u16 len)
-{
-	int ret;
-	u8 *buf;
-
-	LOG_TRACE(priv, FW_MSG, "Sending hcmd:\n");
-
-	/* add padding to 256 for IWMC */
-	((struct top_msg *)cmd)->hdr.flags |= CMD_FLAG_PADDING_256;
-
-	LOG_HEXDUMP(FW_MSG, cmd, len);
-
-	if (len > FW_HCMD_BLOCK_SIZE) {
-		LOG_ERROR(priv, FW_MSG, "size %d exceeded hcmd max size %d\n",
-			  len, FW_HCMD_BLOCK_SIZE);
-		return -1;
-	}
-
-	buf = kzalloc(FW_HCMD_BLOCK_SIZE, GFP_KERNEL);
-	if (!buf) {
-		LOG_ERROR(priv, FW_MSG, "kzalloc error, buf size %d\n",
-			  FW_HCMD_BLOCK_SIZE);
-		return -1;
-	}
-
-	memcpy(buf, cmd, len);
-	ret = iwmct_tx(priv, buf, FW_HCMD_BLOCK_SIZE);
-
-	kfree(buf);
-	return ret;
-}
-
-
-static void iwmct_irq_read_worker(struct work_struct *ws)
-{
-	struct iwmct_priv *priv;
-	struct iwmct_work_struct *read_req;
-	__le32 *buf = NULL;
-	int ret;
-	int iosize;
-	u32 barker;
-	bool is_barker;
-
-	priv = container_of(ws, struct iwmct_priv, isr_worker);
-
-	LOG_TRACE(priv, IRQ, "enter iwmct_irq_read_worker %p\n", ws);
-
-	/* --------------------- Handshake with device -------------------- */
-	sdio_claim_host(priv->func);
-
-	/* all list manipulations have to be protected by
-	 * sdio_claim_host/sdio_release_host */
-	if (list_empty(&priv->read_req_list)) {
-		LOG_ERROR(priv, IRQ, "read_req_list empty in read worker\n");
-		goto exit_release;
-	}
-
-	read_req = list_entry(priv->read_req_list.next,
-			      struct iwmct_work_struct, list);
-
-	list_del(&read_req->list);
-	iosize = read_req->iosize;
-	kfree(read_req);
-
-	buf = kzalloc(iosize, GFP_KERNEL);
-	if (!buf) {
-		LOG_ERROR(priv, IRQ, "kzalloc error, buf size %d\n", iosize);
-		goto exit_release;
-	}
-
-	LOG_INFO(priv, IRQ, "iosize=%d, buf=%p, func=%d\n",
-				iosize, buf, priv->func->num);
-
-	/* read from device */
-	ret = sdio_memcpy_fromio(priv->func, buf, IWMC_SDIO_DATA_ADDR, iosize);
-	if (ret) {
-		LOG_ERROR(priv, IRQ, "error %d reading buffer\n", ret);
-		goto exit_release;
-	}
-
-	LOG_HEXDUMP(IRQ, (u8 *)buf, iosize);
-
-	barker = le32_to_cpu(buf[0]);
-
-	/* Verify whether it's a barker and if not - treat as regular Rx */
-	if (barker == IWMC_BARKER_ACK ||
-	    (barker & BARKER_DNLOAD_BARKER_MSK) == IWMC_BARKER_REBOOT) {
-
-		/* Valid Barker is equal on first 4 dwords */
-		is_barker = (buf[1] == buf[0]) &&
-			    (buf[2] == buf[0]) &&
-			    (buf[3] == buf[0]);
-
-		if (!is_barker) {
-			LOG_WARNING(priv, IRQ,
-				"Potentially inconsistent barker "
-				"%08X_%08X_%08X_%08X\n",
-				le32_to_cpu(buf[0]), le32_to_cpu(buf[1]),
-				le32_to_cpu(buf[2]), le32_to_cpu(buf[3]));
-		}
-	} else {
-		is_barker = false;
-	}
-
-	/* Handle Top CommHub message */
-	if (!is_barker) {
-		sdio_release_host(priv->func);
-		handle_top_message(priv, (u8 *)buf, iosize);
-		goto exit;
-	} else if (barker == IWMC_BARKER_ACK) { /* Handle barkers */
-		if (atomic_read(&priv->dev_sync) == 0) {
-			LOG_ERROR(priv, IRQ,
-				  "ACK barker arrived out-of-sync\n");
-			goto exit_release;
-		}
-
-		/* Continuing to FW download (after Sync is completed)*/
-		atomic_set(&priv->dev_sync, 0);
-		LOG_INFO(priv, IRQ, "ACK barker arrived "
-				"- starting FW download\n");
-	} else { /* REBOOT barker */
-		LOG_INFO(priv, IRQ, "Received reboot barker: %x\n", barker);
-		priv->barker = barker;
-
-		if (barker & BARKER_DNLOAD_SYNC_MSK) {
-			/* Send the same barker back */
-			ret = __iwmct_tx(priv, buf, iosize);
-			if (ret) {
-				LOG_ERROR(priv, IRQ,
-					 "error %d echoing barker\n", ret);
-				goto exit_release;
-			}
-			LOG_INFO(priv, IRQ, "Echoing barker to device\n");
-			atomic_set(&priv->dev_sync, 1);
-			goto exit_release;
-		}
-
-		/* Continuing to FW download (without Sync) */
-		LOG_INFO(priv, IRQ, "No sync requested "
-				    "- starting FW download\n");
-	}
-
-	sdio_release_host(priv->func);
-
-	if (priv->dbg.fw_download)
-		iwmct_fw_load(priv);
-	else
-		LOG_ERROR(priv, IRQ, "FW download not allowed\n");
-
-	goto exit;
-
-exit_release:
-	sdio_release_host(priv->func);
-exit:
-	kfree(buf);
-	LOG_TRACE(priv, IRQ, "exit iwmct_irq_read_worker\n");
-}
-
-static void iwmct_irq(struct sdio_func *func)
-{
-	struct iwmct_priv *priv;
-	int val, ret;
-	int iosize;
-	int addr = IWMC_SDIO_INTR_GET_SIZE_ADDR;
-	struct iwmct_work_struct *read_req;
-
-	priv = sdio_get_drvdata(func);
-
-	LOG_TRACE(priv, IRQ, "enter iwmct_irq\n");
-
-	/* read the function's status register */
-	val = sdio_readb(func, IWMC_SDIO_INTR_STATUS_ADDR, &ret);
-
-	LOG_TRACE(priv, IRQ, "iir value = %d, ret=%d\n", val, ret);
-
-	if (!val) {
-		LOG_ERROR(priv, IRQ, "iir = 0, exiting ISR\n");
-		goto exit_clear_intr;
-	}
-
-
-	/*
-	 * read 2 bytes of the transaction size
-	 * IMPORTANT: sdio transaction size has to be read before clearing
-	 * sdio interrupt!!!
-	 */
-	val = sdio_readb(priv->func, addr++, &ret);
-	iosize = val;
-	val = sdio_readb(priv->func, addr++, &ret);
-	iosize += val << 8;
-
-	LOG_INFO(priv, IRQ, "READ size %d\n", iosize);
-
-	if (iosize == 0) {
-		LOG_ERROR(priv, IRQ, "READ size %d, exiting ISR\n", iosize);
-		goto exit_clear_intr;
-	}
-
-	/* allocate a work structure to pass iosize to the worker */
-	read_req = kzalloc(sizeof(struct iwmct_work_struct), GFP_KERNEL);
-	if (!read_req) {
-		LOG_ERROR(priv, IRQ, "failed to allocate read_req, exit ISR\n");
-		goto exit_clear_intr;
-	}
-
-	INIT_LIST_HEAD(&read_req->list);
-	read_req->iosize = iosize;
-
-	list_add_tail(&priv->read_req_list, &read_req->list);
-
-	/* clear the function's interrupt request bit (write 1 to clear) */
-	sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
-
-	schedule_work(&priv->isr_worker);
-
-	LOG_TRACE(priv, IRQ, "exit iwmct_irq\n");
-
-	return;
-
-exit_clear_intr:
-	/* clear the function's interrupt request bit (write 1 to clear) */
-	sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
-}
-
-
-static int blocks;
-module_param(blocks, int, 0604);
-MODULE_PARM_DESC(blocks, "max_blocks_to_send");
-
-static bool dump;
-module_param(dump, bool, 0604);
-MODULE_PARM_DESC(dump, "dump_hex_content");
-
-static bool jump = 1;
-module_param(jump, bool, 0604);
-
-static bool direct = 1;
-module_param(direct, bool, 0604);
-
-static bool checksum = 1;
-module_param(checksum, bool, 0604);
-
-static bool fw_download = 1;
-module_param(fw_download, bool, 0604);
-
-static int block_size = IWMC_SDIO_BLK_SIZE;
-module_param(block_size, int, 0404);
-
-static int download_trans_blks = IWMC_DEFAULT_TR_BLK;
-module_param(download_trans_blks, int, 0604);
-
-static bool rubbish_barker;
-module_param(rubbish_barker, bool, 0604);
-
-#ifdef CONFIG_IWMC3200TOP_DEBUG
-static int log_level[LOG_SRC_MAX];
-static unsigned int log_level_argc;
-module_param_array(log_level, int, &log_level_argc, 0604);
-MODULE_PARM_DESC(log_level, "log_level");
-
-static int log_level_fw[FW_LOG_SRC_MAX];
-static unsigned int log_level_fw_argc;
-module_param_array(log_level_fw, int, &log_level_fw_argc, 0604);
-MODULE_PARM_DESC(log_level_fw, "log_level_fw");
-#endif
-
-void iwmct_dbg_init_params(struct iwmct_priv *priv)
-{
-#ifdef CONFIG_IWMC3200TOP_DEBUG
-	int i;
-
-	for (i = 0; i < log_level_argc; i++) {
-		dev_notice(&priv->func->dev, "log_level[%d]=0x%X\n",
-						i, log_level[i]);
-		iwmct_log_set_filter((log_level[i] >> 8) & 0xFF,
-			       log_level[i] & 0xFF);
-	}
-	for (i = 0; i < log_level_fw_argc; i++) {
-		dev_notice(&priv->func->dev, "log_level_fw[%d]=0x%X\n",
-						i, log_level_fw[i]);
-		iwmct_log_set_fw_filter((log_level_fw[i] >> 8) & 0xFF,
-				  log_level_fw[i] & 0xFF);
-	}
-#endif
-
-	priv->dbg.blocks = blocks;
-	LOG_INFO(priv, INIT, "blocks=%d\n", blocks);
-	priv->dbg.dump = (bool)dump;
-	LOG_INFO(priv, INIT, "dump=%d\n", dump);
-	priv->dbg.jump = (bool)jump;
-	LOG_INFO(priv, INIT, "jump=%d\n", jump);
-	priv->dbg.direct = (bool)direct;
-	LOG_INFO(priv, INIT, "direct=%d\n", direct);
-	priv->dbg.checksum = (bool)checksum;
-	LOG_INFO(priv, INIT, "checksum=%d\n", checksum);
-	priv->dbg.fw_download = (bool)fw_download;
-	LOG_INFO(priv, INIT, "fw_download=%d\n", fw_download);
-	priv->dbg.block_size = block_size;
-	LOG_INFO(priv, INIT, "block_size=%d\n", block_size);
-	priv->dbg.download_trans_blks = download_trans_blks;
-	LOG_INFO(priv, INIT, "download_trans_blks=%d\n", download_trans_blks);
-}
-
-/*****************************************************************************
- *
- * sysfs attributes
- *
- *****************************************************************************/
-static ssize_t show_iwmct_fw_version(struct device *d,
-				  struct device_attribute *attr, char *buf)
-{
-	struct iwmct_priv *priv = dev_get_drvdata(d);
-	return sprintf(buf, "%s\n", priv->dbg.label_fw);
-}
-static DEVICE_ATTR(cc_label_fw, S_IRUGO, show_iwmct_fw_version, NULL);
-
-#ifdef CONFIG_IWMC3200TOP_DEBUG
-static DEVICE_ATTR(log_level, S_IWUSR | S_IRUGO,
-		   show_iwmct_log_level, store_iwmct_log_level);
-static DEVICE_ATTR(log_level_fw, S_IWUSR | S_IRUGO,
-		   show_iwmct_log_level_fw, store_iwmct_log_level_fw);
-#endif
-
-static struct attribute *iwmct_sysfs_entries[] = {
-	&dev_attr_cc_label_fw.attr,
-#ifdef CONFIG_IWMC3200TOP_DEBUG
-	&dev_attr_log_level.attr,
-	&dev_attr_log_level_fw.attr,
-#endif
-	NULL
-};
-
-static struct attribute_group iwmct_attribute_group = {
-	.name = NULL,		/* put in device directory */
-	.attrs = iwmct_sysfs_entries,
-};
-
-
-static int iwmct_probe(struct sdio_func *func,
-			   const struct sdio_device_id *id)
-{
-	struct iwmct_priv *priv;
-	int ret;
-	int val = 1;
-	int addr = IWMC_SDIO_INTR_ENABLE_ADDR;
-
-	dev_dbg(&func->dev, "enter iwmct_probe\n");
-
-	dev_dbg(&func->dev, "IRQ polling period id %u msecs, HZ is %d\n",
-		jiffies_to_msecs(2147483647), HZ);
-
-	priv = kzalloc(sizeof(struct iwmct_priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&func->dev, "kzalloc error\n");
-		return -ENOMEM;
-	}
-	priv->func = func;
-	sdio_set_drvdata(func, priv);
-
-	INIT_WORK(&priv->bus_rescan_worker, iwmct_rescan_worker);
-	INIT_WORK(&priv->isr_worker, iwmct_irq_read_worker);
-
-	init_waitqueue_head(&priv->wait_q);
-
-	sdio_claim_host(func);
-	/* FIXME: Remove after it is fixed in the Boot ROM upgrade */
-	func->enable_timeout = 10;
-
-	/* In our HW, setting the block size also wakes up the boot rom. */
-	ret = sdio_set_block_size(func, priv->dbg.block_size);
-	if (ret) {
-		LOG_ERROR(priv, INIT,
-			"sdio_set_block_size() failure: %d\n", ret);
-		goto error_sdio_enable;
-	}
-
-	ret = sdio_enable_func(func);
-	if (ret) {
-		LOG_ERROR(priv, INIT, "sdio_enable_func() failure: %d\n", ret);
-		goto error_sdio_enable;
-	}
-
-	/* init reset and dev_sync states */
-	atomic_set(&priv->reset, 0);
-	atomic_set(&priv->dev_sync, 0);
-
-	/* init read req queue */
-	INIT_LIST_HEAD(&priv->read_req_list);
-
-	/* process configurable parameters */
-	iwmct_dbg_init_params(priv);
-	ret = sysfs_create_group(&func->dev.kobj, &iwmct_attribute_group);
-	if (ret) {
-		LOG_ERROR(priv, INIT, "Failed to register attributes and "
-			 "initialize module_params\n");
-		goto error_dev_attrs;
-	}
-
-	iwmct_dbgfs_register(priv, DRV_NAME);
-
-	if (!priv->dbg.direct && priv->dbg.download_trans_blks > 8) {
-		LOG_INFO(priv, INIT,
-			 "Reducing transaction to 8 blocks = 2K (from %d)\n",
-			 priv->dbg.download_trans_blks);
-		priv->dbg.download_trans_blks = 8;
-	}
-	priv->trans_len = priv->dbg.download_trans_blks * priv->dbg.block_size;
-	LOG_INFO(priv, INIT, "Transaction length = %d\n", priv->trans_len);
-
-	ret = sdio_claim_irq(func, iwmct_irq);
-	if (ret) {
-		LOG_ERROR(priv, INIT, "sdio_claim_irq() failure: %d\n", ret);
-		goto error_claim_irq;
-	}
-
-
-	/* Enable function's interrupt */
-	sdio_writeb(priv->func, val, addr, &ret);
-	if (ret) {
-		LOG_ERROR(priv, INIT, "Failure writing to "
-			  "Interrupt Enable Register (%d): %d\n", addr, ret);
-		goto error_enable_int;
-	}
-
-	sdio_release_host(func);
-
-	LOG_INFO(priv, INIT, "exit iwmct_probe\n");
-
-	return ret;
-
-error_enable_int:
-	sdio_release_irq(func);
-error_claim_irq:
-	sdio_disable_func(func);
-error_dev_attrs:
-	iwmct_dbgfs_unregister(priv->dbgfs);
-	sysfs_remove_group(&func->dev.kobj, &iwmct_attribute_group);
-error_sdio_enable:
-	sdio_release_host(func);
-	return ret;
-}
-
-static void iwmct_remove(struct sdio_func *func)
-{
-	struct iwmct_work_struct *read_req;
-	struct iwmct_priv *priv = sdio_get_drvdata(func);
-
-	LOG_INFO(priv, INIT, "enter\n");
-
-	sdio_claim_host(func);
-	sdio_release_irq(func);
-	sdio_release_host(func);
-
-	/* 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);
-	sysfs_remove_group(&func->dev.kobj, &iwmct_attribute_group);
-	iwmct_dbgfs_unregister(priv->dbgfs);
-	sdio_release_host(func);
-
-	/* free read requests */
-	while (!list_empty(&priv->read_req_list)) {
-		read_req = list_entry(priv->read_req_list.next,
-			struct iwmct_work_struct, list);
-
-		list_del(&read_req->list);
-		kfree(read_req);
-	}
-
-	kfree(priv);
-}
-
-
-static const struct sdio_device_id iwmct_ids[] = {
-	/* Intel Wireless MultiCom 3200 Top Driver */
-	{ SDIO_DEVICE(SDIO_VENDOR_ID_INTEL, 0x1404)},
-	{ },	/* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(sdio, iwmct_ids);
-
-static struct sdio_driver iwmct_driver = {
-	.probe		= iwmct_probe,
-	.remove		= iwmct_remove,
-	.name		= DRV_NAME,
-	.id_table	= iwmct_ids,
-};
-
-static int __init iwmct_init(void)
-{
-	int rc;
-
-	/* Default log filter settings */
-	iwmct_log_set_filter(LOG_SRC_ALL, LOG_SEV_FILTER_RUNTIME);
-	iwmct_log_set_filter(LOG_SRC_FW_MSG, LOG_SEV_FW_FILTER_ALL);
-	iwmct_log_set_fw_filter(LOG_SRC_ALL, FW_LOG_SEV_FILTER_RUNTIME);
-
-	rc = sdio_register_driver(&iwmct_driver);
-
-	return rc;
-}
-
-static void __exit iwmct_exit(void)
-{
-	sdio_unregister_driver(&iwmct_driver);
-}
-
-module_init(iwmct_init);
-module_exit(iwmct_exit);
-
diff --git a/drivers/misc/lkdtm.c b/drivers/misc/lkdtm.c
index 28adefe70f9..08aad69c8da 100644
--- a/drivers/misc/lkdtm.c
+++ b/drivers/misc/lkdtm.c
@@ -477,6 +477,8 @@ static ssize_t lkdtm_debugfs_read(struct file *f, char __user *user_buf,
 	int i, n, out;
 
 	buf = (char *)__get_free_page(GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
 
 	n = snprintf(buf, PAGE_SIZE, "Available crash types:\n");
 	for (i = 0; i < ARRAY_SIZE(cp_type); i++)
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index a7d0bb0880e..e77f86e69fb 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -162,6 +162,9 @@ int mei_hw_init(struct mei_device *dev)
 	if ((dev->host_hw_state & H_IS) == H_IS)
 		mei_reg_write(dev, H_CSR, dev->host_hw_state);
 
+	/* Doesn't change in runtime */
+	dev->hbuf_depth = (dev->host_hw_state & H_CBD) >> 24;
+
 	dev->recvd_msg = false;
 	dev_dbg(&dev->pdev->dev, "reset in start the mei device.\n");
 
@@ -303,7 +306,6 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 				dev->iamthif_cl.host_client_id);
 
 		mei_reset_iamthif_params(dev);
-		dev->wd_due_counter = 0;
 		dev->extra_write_index = 0;
 	}
 
diff --git a/drivers/misc/mei/interface.c b/drivers/misc/mei/interface.c
index 428d21e3641..509c3957ff4 100644
--- a/drivers/misc/mei/interface.c
+++ b/drivers/misc/mei/interface.c
@@ -58,16 +58,18 @@ void mei_disable_interrupts(struct mei_device *dev)
 }
 
 /**
- * _host_get_filled_slots - gets number of device filled buffer slots
+ * mei_hbuf_filled_slots - gets number of device filled buffer slots
  *
  * @device: the device structure
  *
  * returns number of filled slots
  */
-static unsigned char _host_get_filled_slots(const struct mei_device *dev)
+static unsigned char mei_hbuf_filled_slots(struct mei_device *dev)
 {
 	char read_ptr, write_ptr;
 
+	dev->host_hw_state = mei_hcsr_read(dev);
+
 	read_ptr = (char) ((dev->host_hw_state & H_CBRP) >> 8);
 	write_ptr = (char) ((dev->host_hw_state & H_CBWP) >> 16);
 
@@ -75,43 +77,33 @@ static unsigned char _host_get_filled_slots(const struct mei_device *dev)
 }
 
 /**
- * mei_host_buffer_is_empty - checks if host buffer is empty.
+ * mei_hbuf_is_empty - checks if host buffer is empty.
  *
  * @dev: the device structure
  *
- * returns 1 if empty, 0 - otherwise.
+ * returns true if empty, false - otherwise.
  */
-int mei_host_buffer_is_empty(struct mei_device *dev)
+bool mei_hbuf_is_empty(struct mei_device *dev)
 {
-	unsigned char filled_slots;
-
-	dev->host_hw_state = mei_hcsr_read(dev);
-	filled_slots = _host_get_filled_slots(dev);
-
-	if (filled_slots == 0)
-		return 1;
-
-	return 0;
+	return mei_hbuf_filled_slots(dev) == 0;
 }
 
 /**
- * mei_count_empty_write_slots - counts write empty slots.
+ * mei_hbuf_empty_slots - counts write empty slots.
  *
  * @dev: the device structure
  *
  * returns -1(ESLOTS_OVERFLOW) if overflow, otherwise empty slots count
  */
-int mei_count_empty_write_slots(struct mei_device *dev)
+int mei_hbuf_empty_slots(struct mei_device *dev)
 {
-	unsigned char buffer_depth, filled_slots, empty_slots;
+	unsigned char filled_slots, empty_slots;
 
-	dev->host_hw_state = mei_hcsr_read(dev);
-	buffer_depth = (unsigned char) ((dev->host_hw_state & H_CBD) >> 24);
-	filled_slots = _host_get_filled_slots(dev);
-	empty_slots = buffer_depth - filled_slots;
+	filled_slots = mei_hbuf_filled_slots(dev);
+	empty_slots = dev->hbuf_depth - filled_slots;
 
 	/* check for overflow */
-	if (filled_slots > buffer_depth)
+	if (filled_slots > dev->hbuf_depth)
 		return -EOVERFLOW;
 
 	return empty_slots;
@@ -127,52 +119,39 @@ int mei_count_empty_write_slots(struct mei_device *dev)
  *
  * This function returns -EIO if write has failed
  */
-int mei_write_message(struct mei_device *dev,
-		      struct mei_msg_hdr *header,
-		      unsigned char *write_buffer,
-		      unsigned long write_length)
+int mei_write_message(struct mei_device *dev, struct mei_msg_hdr *header,
+		      unsigned char *buf, unsigned long length)
 {
-	u32 temp_msg = 0;
-	unsigned long bytes_written = 0;
-	unsigned char buffer_depth, filled_slots, empty_slots;
-	unsigned long dw_to_write;
-
-	dev->host_hw_state = mei_hcsr_read(dev);
+	unsigned long rem, dw_cnt;
+	u32 *reg_buf = (u32 *)buf;
+	int i;
+	int empty_slots;
 
-	dev_dbg(&dev->pdev->dev,
-			"host_hw_state = 0x%08x.\n",
-			dev->host_hw_state);
 
 	dev_dbg(&dev->pdev->dev,
 			"mei_write_message header=%08x.\n",
 			*((u32 *) header));
 
-	buffer_depth = (unsigned char) ((dev->host_hw_state & H_CBD) >> 24);
-	filled_slots = _host_get_filled_slots(dev);
-	empty_slots = buffer_depth - filled_slots;
-	dev_dbg(&dev->pdev->dev,
-			"filled = %hu, empty = %hu.\n",
-			filled_slots, empty_slots);
-
-	dw_to_write = ((write_length + 3) / 4);
+	empty_slots = mei_hbuf_empty_slots(dev);
+	dev_dbg(&dev->pdev->dev, "empty slots = %hu.\n", empty_slots);
 
-	if (dw_to_write > empty_slots)
+	dw_cnt = mei_data2slots(length);
+	if (empty_slots < 0 || dw_cnt > empty_slots)
 		return -EIO;
 
 	mei_reg_write(dev, H_CB_WW, *((u32 *) header));
 
-	while (write_length >= 4) {
-		mei_reg_write(dev, H_CB_WW,
-				*(u32 *) (write_buffer + bytes_written));
-		bytes_written += 4;
-		write_length -= 4;
-	}
+	for (i = 0; i < length / 4; i++)
+		mei_reg_write(dev, H_CB_WW, reg_buf[i]);
 
-	if (write_length > 0) {
-		memcpy(&temp_msg, &write_buffer[bytes_written], write_length);
-		mei_reg_write(dev, H_CB_WW, temp_msg);
+	rem = length & 0x3;
+	if (rem > 0) {
+		u32 reg = 0;
+		memcpy(&reg, &buf[length - rem], rem);
+		mei_reg_write(dev, H_CB_WW, reg);
 	}
 
+	dev->host_hw_state = mei_hcsr_read(dev);
 	dev->host_hw_state |= H_IG;
 	mei_hcsr_set(dev);
 	dev->me_hw_state = mei_mecsr_read(dev);
diff --git a/drivers/misc/mei/interface.h b/drivers/misc/mei/interface.h
index ddff5d16616..fb5c7db4723 100644
--- a/drivers/misc/mei/interface.h
+++ b/drivers/misc/mei/interface.h
@@ -41,14 +41,28 @@ int mei_write_message(struct mei_device *dev,
 			     unsigned char *write_buffer,
 			     unsigned long write_length);
 
-int mei_host_buffer_is_empty(struct mei_device *dev);
+bool mei_hbuf_is_empty(struct mei_device *dev);
+
+int mei_hbuf_empty_slots(struct mei_device *dev);
+
+static inline size_t mei_hbuf_max_data(const struct mei_device *dev)
+{
+	return dev->hbuf_depth * sizeof(u32) - sizeof(struct mei_msg_hdr);
+}
+
+/* get slots (dwords) from a message length + header (bytes) */
+static inline unsigned char mei_data2slots(size_t length)
+{
+	return DIV_ROUND_UP(sizeof(struct mei_msg_hdr) + length, 4);
+}
 
 int mei_count_full_read_slots(struct mei_device *dev);
 
-int mei_count_empty_write_slots(struct mei_device *dev);
 
 int mei_flow_ctrl_creds(struct mei_device *dev, struct mei_cl *cl);
 
+
+
 int mei_wd_send(struct mei_device *dev);
 int mei_wd_stop(struct mei_device *dev, bool preserve);
 int mei_wd_host_init(struct mei_device *dev);
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 23f5463d4ca..c6ffbbe5a6c 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -267,8 +267,7 @@ static int _mei_irq_thread_iamthif_read(struct mei_device *dev, s32 *slots)
 			+ sizeof(struct hbm_flow_control))) {
 		return -EMSGSIZE;
 	}
-	*slots -= (sizeof(struct mei_msg_hdr) +
-				sizeof(struct hbm_flow_control) + 3) / 4;
+	*slots -= mei_data2slots(sizeof(struct hbm_flow_control));
 	if (mei_send_flow_control(dev, &dev->iamthif_cl)) {
 		dev_dbg(&dev->pdev->dev, "iamthif flow control failed\n");
 		return -EIO;
@@ -280,7 +279,7 @@ static int _mei_irq_thread_iamthif_read(struct mei_device *dev, s32 *slots)
 	dev->iamthif_msg_buf_index = 0;
 	dev->iamthif_msg_buf_size = 0;
 	dev->iamthif_stall_timer = IAMTHIF_STALL_TIMER;
-	dev->mei_host_buffer_is_empty = mei_host_buffer_is_empty(dev);
+	dev->mei_host_buffer_is_empty = mei_hbuf_is_empty(dev);
 	return 0;
 }
 
@@ -300,28 +299,25 @@ static int _mei_irq_thread_close(struct mei_device *dev, s32 *slots,
 				struct mei_cl *cl,
 				struct mei_io_list *cmpl_list)
 {
-	if ((*slots * sizeof(u32)) >= (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_client_disconnect_request))) {
-		*slots -= (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_client_disconnect_request) + 3) / 4;
+	if ((*slots * sizeof(u32)) < (sizeof(struct mei_msg_hdr) +
+			sizeof(struct hbm_client_disconnect_request)))
+		return -EBADMSG;
 
-		if (mei_disconnect(dev, cl)) {
-			cl->status = 0;
-			cb_pos->information = 0;
-			list_move_tail(&cb_pos->cb_list,
-					&cmpl_list->mei_cb.cb_list);
-			return -EMSGSIZE;
-		} else {
-			cl->state = MEI_FILE_DISCONNECTING;
-			cl->status = 0;
-			cb_pos->information = 0;
-			list_move_tail(&cb_pos->cb_list,
-					&dev->ctrl_rd_list.mei_cb.cb_list);
-			cl->timer_count = MEI_CONNECT_TIMEOUT;
-		}
+	*slots -= mei_data2slots(sizeof(struct hbm_client_disconnect_request));
+
+	if (mei_disconnect(dev, cl)) {
+		cl->status = 0;
+		cb_pos->information = 0;
+		list_move_tail(&cb_pos->cb_list,
+				&cmpl_list->mei_cb.cb_list);
+		return -EMSGSIZE;
 	} else {
-		/* return the cancel routine */
-		return -EBADMSG;
+		cl->state = MEI_FILE_DISCONNECTING;
+		cl->status = 0;
+		cb_pos->information = 0;
+		list_move_tail(&cb_pos->cb_list,
+				&dev->ctrl_rd_list.mei_cb.cb_list);
+		cl->timer_count = MEI_CONNECT_TIMEOUT;
 	}
 
 	return 0;
@@ -575,10 +571,9 @@ static void mei_client_disconnect_request(struct mei_device *dev,
 					disconnect_req->me_addr);
 			cl_pos->state = MEI_FILE_DISCONNECTED;
 			cl_pos->timer_count = 0;
-			if (cl_pos == &dev->wd_cl) {
-				dev->wd_due_counter = 0;
+			if (cl_pos == &dev->wd_cl)
 				dev->wd_pending = false;
-			} else if (cl_pos == &dev->iamthif_cl)
+			else if (cl_pos == &dev->iamthif_cl)
 				dev->iamthif_timer = 0;
 
 			/* prepare disconnect response */
@@ -842,8 +837,8 @@ static int _mei_irq_thread_read(struct mei_device *dev,	s32 *slots,
 		return -EBADMSG;
 	}
 
-	*slots -= (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_flow_control) + 3) / 4;
+	*slots -= mei_data2slots(sizeof(struct hbm_flow_control));
+
 	if (mei_send_flow_control(dev, cl)) {
 		cl->status = -ENODEV;
 		cb_pos->information = 0;
@@ -872,27 +867,25 @@ static int _mei_irq_thread_ioctl(struct mei_device *dev, s32 *slots,
 			struct mei_cl *cl,
 			struct mei_io_list *cmpl_list)
 {
-	if ((*slots * sizeof(u32)) >= (sizeof(struct mei_msg_hdr) +
+	if ((*slots * sizeof(u32)) < (sizeof(struct mei_msg_hdr) +
 			sizeof(struct hbm_client_connect_request))) {
-		cl->state = MEI_FILE_CONNECTING;
-		*slots -= (sizeof(struct mei_msg_hdr) +
-			sizeof(struct hbm_client_connect_request) + 3) / 4;
-		if (mei_connect(dev, cl)) {
-			cl->status = -ENODEV;
-			cb_pos->information = 0;
-			list_del(&cb_pos->cb_list);
-			return -ENODEV;
-		} else {
-			list_move_tail(&cb_pos->cb_list,
-				&dev->ctrl_rd_list.mei_cb.cb_list);
-			cl->timer_count = MEI_CONNECT_TIMEOUT;
-		}
-	} else {
 		/* return the cancel routine */
 		list_del(&cb_pos->cb_list);
 		return -EBADMSG;
 	}
 
+	cl->state = MEI_FILE_CONNECTING;
+	 *slots -= mei_data2slots(sizeof(struct hbm_client_connect_request));
+	if (mei_connect(dev, cl)) {
+		cl->status = -ENODEV;
+		cb_pos->information = 0;
+		list_del(&cb_pos->cb_list);
+		return -ENODEV;
+	} else {
+		list_move_tail(&cb_pos->cb_list,
+			&dev->ctrl_rd_list.mei_cb.cb_list);
+		cl->timer_count = MEI_CONNECT_TIMEOUT;
+	}
 	return 0;
 }
 
@@ -932,8 +925,7 @@ static int _mei_irq_thread_cmpl(struct mei_device *dev,	s32 *slots,
 				cb_pos->information);
 		dev_dbg(&dev->pdev->dev, "mei_hdr->length  =%d\n",
 				mei_hdr->length);
-		*slots -= (sizeof(struct mei_msg_hdr) +
-				mei_hdr->length + 3) / 4;
+		*slots -= mei_data2slots(mei_hdr->length);
 		if (mei_write_message(dev, mei_hdr,
 				(unsigned char *)
 				(cb_pos->request_buffer.data +
@@ -951,7 +943,7 @@ static int _mei_irq_thread_cmpl(struct mei_device *dev,	s32 *slots,
 			list_move_tail(&cb_pos->cb_list,
 				&dev->write_waiting_list.mei_cb.cb_list);
 		}
-	} else if (*slots == ((dev->host_hw_state & H_CBD) >> 24)) {
+	} else if (*slots == dev->hbuf_depth) {
 		/* buffer is still empty */
 		mei_hdr = (struct mei_msg_hdr *) &dev->wr_msg_buf[0];
 		mei_hdr->host_addr = cl->host_client_id;
@@ -960,9 +952,7 @@ static int _mei_irq_thread_cmpl(struct mei_device *dev,	s32 *slots,
 			(*slots * sizeof(u32)) - sizeof(struct mei_msg_hdr);
 		mei_hdr->msg_complete = 0;
 		mei_hdr->reserved = 0;
-
-		(*slots) -= (sizeof(struct mei_msg_hdr) +
-				mei_hdr->length + 3) / 4;
+		*slots -= mei_data2slots(mei_hdr->length);
 		if (mei_write_message(dev, mei_hdr,
 					(unsigned char *)
 					(cb_pos->request_buffer.data +
@@ -1021,8 +1011,7 @@ static int _mei_irq_thread_cmpl_iamthif(struct mei_device *dev, s32 *slots,
 		mei_hdr->msg_complete = 1;
 		mei_hdr->reserved = 0;
 
-		*slots -= (sizeof(struct mei_msg_hdr) +
-				mei_hdr->length + 3) / 4;
+		*slots -= mei_data2slots(mei_hdr->length);
 
 		if (mei_write_message(dev, mei_hdr,
 					(dev->iamthif_msg_buf +
@@ -1046,8 +1035,8 @@ static int _mei_irq_thread_cmpl_iamthif(struct mei_device *dev, s32 *slots,
 				&dev->write_waiting_list.mei_cb.cb_list);
 
 		}
-	} else if (*slots == ((dev->host_hw_state & H_CBD) >> 24)) {
-			/* buffer is still empty */
+	} else if (*slots == dev->hbuf_depth) {
+		/* buffer is still empty */
 		mei_hdr = (struct mei_msg_hdr *) &dev->wr_msg_buf[0];
 		mei_hdr->host_addr = cl->host_client_id;
 		mei_hdr->me_addr = cl->me_client_id;
@@ -1056,8 +1045,7 @@ static int _mei_irq_thread_cmpl_iamthif(struct mei_device *dev, s32 *slots,
 		mei_hdr->msg_complete = 0;
 		mei_hdr->reserved = 0;
 
-		*slots -= (sizeof(struct mei_msg_hdr) +
-				mei_hdr->length + 3) / 4;
+		*slots -= mei_data2slots(mei_hdr->length);
 
 		if (mei_write_message(dev, mei_hdr,
 					(dev->iamthif_msg_buf +
@@ -1199,17 +1187,19 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 	struct mei_io_list *list;
 	int ret;
 
-	if (!mei_host_buffer_is_empty(dev)) {
+	if (!mei_hbuf_is_empty(dev)) {
 		dev_dbg(&dev->pdev->dev, "host buffer is not empty.\n");
 		return 0;
 	}
-	*slots = mei_count_empty_write_slots(dev);
+	*slots = mei_hbuf_empty_slots(dev);
+	if (*slots <= 0)
+		return -EMSGSIZE;
+
 	/* complete all waiting for write CB */
 	dev_dbg(&dev->pdev->dev, "complete all waiting for write cb.\n");
 
 	list = &dev->write_waiting_list;
-	list_for_each_entry_safe(pos, next,
-			&list->mei_cb.cb_list, cb_list) {
+	list_for_each_entry_safe(pos, next, &list->mei_cb.cb_list, cb_list) {
 		cl = (struct mei_cl *)pos->file_private;
 		if (cl == NULL)
 			continue;
@@ -1219,17 +1209,15 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 		if (MEI_WRITING == cl->writing_state &&
 		   (pos->major_file_operations == MEI_WRITE) &&
 		   (cl != &dev->iamthif_cl)) {
-			dev_dbg(&dev->pdev->dev,
-				"MEI WRITE COMPLETE\n");
+			dev_dbg(&dev->pdev->dev, "MEI WRITE COMPLETE\n");
 			cl->writing_state = MEI_WRITE_COMPLETE;
 			list_add_tail(&pos->cb_list,
-				&cmpl_list->mei_cb.cb_list);
+				      &cmpl_list->mei_cb.cb_list);
 		}
 		if (cl == &dev->iamthif_cl) {
 			dev_dbg(&dev->pdev->dev, "check iamthif flow control.\n");
 			if (dev->iamthif_flow_control_pending) {
-				ret = _mei_irq_thread_iamthif_read(
-						dev, slots);
+				ret = _mei_irq_thread_iamthif_read(dev, slots);
 				if (ret)
 					return ret;
 			}
@@ -1254,25 +1242,18 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 	}
 	if (dev->mei_state == MEI_ENABLED) {
 		if (dev->wd_pending &&
-			mei_flow_ctrl_creds(dev, &dev->wd_cl) > 0) {
+		    mei_flow_ctrl_creds(dev, &dev->wd_cl) > 0) {
 			if (mei_wd_send(dev))
 				dev_dbg(&dev->pdev->dev, "wd send failed.\n");
-			else
-				if (mei_flow_ctrl_reduce(dev, &dev->wd_cl))
-					return -ENODEV;
+			else if (mei_flow_ctrl_reduce(dev, &dev->wd_cl))
+				return -ENODEV;
 
 			dev->wd_pending = false;
 
-			if (dev->wd_timeout) {
-				*slots -= (sizeof(struct mei_msg_hdr) +
-					 MEI_START_WD_DATA_SIZE + 3) / 4;
-				dev->wd_due_counter = 2;
-			} else {
-				*slots -= (sizeof(struct mei_msg_hdr) +
-					 MEI_WD_PARAMS_SIZE + 3) / 4;
-				dev->wd_due_counter = 0;
-			}
-
+			if (dev->wd_timeout)
+				*slots -= mei_data2slots(MEI_START_WD_DATA_SIZE);
+			else
+				*slots -= mei_data2slots(MEI_START_WD_DATA_SIZE);
 		}
 	}
 	if (dev->stop)
@@ -1320,42 +1301,34 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 	/* complete  write list CB */
 	dev_dbg(&dev->pdev->dev, "complete write list cb.\n");
 	list_for_each_entry_safe(pos, next,
-			&dev->write_list.mei_cb.cb_list, cb_list) {
+				&dev->write_list.mei_cb.cb_list, cb_list) {
 		cl = (struct mei_cl *)pos->file_private;
 		if (cl == NULL)
 			continue;
 
 		if (cl != &dev->iamthif_cl) {
-			if (!mei_flow_ctrl_creds(dev, cl)) {
+			if (mei_flow_ctrl_creds(dev, cl) <= 0) {
 				dev_dbg(&dev->pdev->dev,
-					"No flow control"
-				    " credentials for client"
-				    " %d, not sending.\n",
-				    cl->host_client_id);
+					"No flow control credentials for client %d, not sending.\n",
+					cl->host_client_id);
 				continue;
 			}
-			ret = _mei_irq_thread_cmpl(dev, slots,
-					    pos,
-					    cl, cmpl_list);
+			ret = _mei_irq_thread_cmpl(dev, slots, pos,
+						cl, cmpl_list);
 			if (ret)
 				return ret;
 
 		} else if (cl == &dev->iamthif_cl) {
 			/* IAMTHIF IOCTL */
 			dev_dbg(&dev->pdev->dev, "complete amthi write cb.\n");
-			if (!mei_flow_ctrl_creds(dev, cl)) {
+			if (mei_flow_ctrl_creds(dev, cl) <= 0) {
 				dev_dbg(&dev->pdev->dev,
-					"No flow control"
-				    " credentials for amthi"
-				    " client %d.\n",
-				    cl->host_client_id);
+					"No flow control credentials for amthi client %d.\n",
+					cl->host_client_id);
 				continue;
 			}
-			ret = _mei_irq_thread_cmpl_iamthif(dev,
-						slots,
-						pos,
-						cl,
-						cmpl_list);
+			ret = _mei_irq_thread_cmpl_iamthif(dev, slots, pos,
+						cl, cmpl_list);
 			if (ret)
 				return ret;
 
@@ -1555,7 +1528,7 @@ irqreturn_t mei_interrupt_thread_handler(int irq, void *dev_id)
 end:
 	dev_dbg(&dev->pdev->dev, "end of bottom half function.\n");
 	dev->host_hw_state = mei_hcsr_read(dev);
-	dev->mei_host_buffer_is_empty = mei_host_buffer_is_empty(dev);
+	dev->mei_host_buffer_is_empty = mei_hbuf_is_empty(dev);
 
 	bus_message_received = false;
 	if (dev->recvd_msg && waitqueue_active(&dev->wait_recvd_msg)) {
diff --git a/drivers/misc/mei/iorw.c b/drivers/misc/mei/iorw.c
index f9cced69b65..50f52e21f58 100644
--- a/drivers/misc/mei/iorw.c
+++ b/drivers/misc/mei/iorw.c
@@ -481,12 +481,8 @@ int amthi_write(struct mei_device *dev, struct mei_cl_cb *cb)
 	if (ret && dev->mei_host_buffer_is_empty) {
 		ret = 0;
 		dev->mei_host_buffer_is_empty = false;
-		if (cb->request_buffer.size >
-			(((dev->host_hw_state & H_CBD) >> 24) * sizeof(u32))
-				-sizeof(struct mei_msg_hdr)) {
-			mei_hdr.length =
-			    (((dev->host_hw_state & H_CBD) >> 24) *
-			    sizeof(u32)) - sizeof(struct mei_msg_hdr);
+		if (cb->request_buffer.size > mei_hbuf_max_data(dev)) {
+			mei_hdr.length = mei_hbuf_max_data(dev);
 			mei_hdr.msg_complete = 0;
 		} else {
 			mei_hdr.length = cb->request_buffer.size;
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 783fcd7365b..09233020886 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -714,13 +714,8 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 	if (rets && dev->mei_host_buffer_is_empty) {
 		rets = 0;
 		dev->mei_host_buffer_is_empty = false;
-		if (length > ((((dev->host_hw_state & H_CBD) >> 24) *
-			sizeof(u32)) - sizeof(struct mei_msg_hdr))) {
-
-			mei_hdr.length =
-				(((dev->host_hw_state & H_CBD) >> 24) *
-				sizeof(u32)) -
-				sizeof(struct mei_msg_hdr);
+		if (length >  mei_hbuf_max_data(dev)) {
+			mei_hdr.length = mei_hbuf_max_data(dev);
 			mei_hdr.msg_complete = 0;
 		} else {
 			mei_hdr.length = length;
@@ -1187,44 +1182,7 @@ static struct pci_driver mei_driver = {
 	.driver.pm = MEI_PM_OPS,
 };
 
-/**
- * mei_init_module - Driver Registration Routine
- *
- * mei_init_module is the first routine called when the driver is
- * loaded. All it does is to register with the PCI subsystem.
- *
- * returns 0 on success, <0 on failure.
- */
-static int __init mei_init_module(void)
-{
-	int ret;
-
-	pr_debug("loading.\n");
-	/* init pci module */
-	ret = pci_register_driver(&mei_driver);
-	if (ret < 0)
-		pr_err("error registering driver.\n");
-
-	return ret;
-}
-
-module_init(mei_init_module);
-
-/**
- * mei_exit_module - Driver Exit Cleanup Routine
- *
- * mei_exit_module is called just before the driver is removed
- * from memory.
- */
-static void __exit mei_exit_module(void)
-{
-	pci_unregister_driver(&mei_driver);
-
-	pr_debug("unloaded successfully.\n");
-}
-
-module_exit(mei_exit_module);
-
+module_pci_driver(mei_driver);
 
 MODULE_AUTHOR("Intel Corporation");
 MODULE_DESCRIPTION("Intel(R) Management Engine Interface");
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 63d7ee97c5f..d61c4ddfc80 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -167,7 +167,10 @@ struct mei_io_list {
 	struct mei_cl_cb mei_cb;
 };
 
-/* MEI private device struct */
+/**
+ * struct mei_deive -  MEI private device struct
+ * @hbuf_depth - depth of host(write) buffer
+ */
 struct mei_device {
 	struct pci_dev *pdev;	/* pointer to pci device struct */
 	/*
@@ -205,6 +208,7 @@ struct mei_device {
 	 */
 	u32 host_hw_state;
 	u32 me_hw_state;
+	u8  hbuf_depth;
 	/*
 	 * waiting queue for receive message from FW
 	 */
@@ -237,15 +241,14 @@ struct mei_device {
 	bool mei_host_buffer_is_empty;
 
 	struct mei_cl wd_cl;
+	bool wd_interface_reg;
 	bool wd_pending;
 	bool wd_stopped;
 	bool wd_bypass;	/* if false, don't refresh watchdog ME client */
 	u16 wd_timeout;	/* seconds ((wd_data[1] << 8) + wd_data[0]) */
-	u16 wd_due_counter;
 	unsigned char wd_data[MEI_START_WD_DATA_SIZE];
 
 
-
 	struct file *iamthif_file_object;
 	struct mei_cl iamthif_cl;
 	struct mei_cl_cb *iamthif_current_cb;
@@ -259,8 +262,6 @@ struct mei_device {
 	bool iamthif_flow_control_pending;
 	bool iamthif_ioctl;
 	bool iamthif_canceled;
-
-	bool wd_interface_reg;
 };
 
 
@@ -361,7 +362,8 @@ int mei_find_me_client_index(const struct mei_device *dev, uuid_le cuuid);
  *
  * returns register value (u32)
  */
-static inline u32 mei_reg_read(struct mei_device *dev, unsigned long offset)
+static inline u32 mei_reg_read(const struct mei_device *dev,
+			       unsigned long offset)
 {
 	return ioread32(dev->mem_addr + offset);
 }
@@ -373,8 +375,8 @@ static inline u32 mei_reg_read(struct mei_device *dev, unsigned long offset)
  * @offset: offset from which to write the data
  * @value: register value to write (u32)
  */
-static inline void mei_reg_write(struct mei_device *dev,
-				unsigned long offset, u32 value)
+static inline void mei_reg_write(const struct mei_device *dev,
+				 unsigned long offset, u32 value)
 {
 	iowrite32(value, dev->mem_addr + offset);
 }
@@ -386,7 +388,7 @@ static inline void mei_reg_write(struct mei_device *dev,
  *
  * returns the byte read.
  */
-static inline u32 mei_hcsr_read(struct mei_device *dev)
+static inline u32 mei_hcsr_read(const struct mei_device *dev)
 {
 	return mei_reg_read(dev, H_CSR);
 }
@@ -398,7 +400,7 @@ static inline u32 mei_hcsr_read(struct mei_device *dev)
  *
  * returns ME_CSR_HA register value (u32)
  */
-static inline u32 mei_mecsr_read(struct mei_device *dev)
+static inline u32 mei_mecsr_read(const struct mei_device *dev)
 {
 	return mei_reg_read(dev, ME_CSR_HA);
 }
@@ -410,7 +412,7 @@ static inline u32 mei_mecsr_read(struct mei_device *dev)
  *
  * returns ME_CB_RW register value (u32)
  */
-static inline u32 mei_mecbrw_read(struct mei_device *dev)
+static inline u32 mei_mecbrw_read(const struct mei_device *dev)
 {
 	return mei_reg_read(dev, ME_CB_RW);
 }
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index e2ec0505eb5..5133fd77b91 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -53,11 +53,12 @@ static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
 }
 
 /**
- * host_init_wd - mei initialization wd.
+ * mei_wd_host_init - connect to the watchdog client
  *
  * @dev: the device structure
  * returns -ENENT if wd client cannot be found
  *         -EIO if write has failed
+ *         0 on success
  */
 int mei_wd_host_init(struct mei_device *dev)
 {
@@ -137,7 +138,6 @@ int mei_wd_stop(struct mei_device *dev, bool preserve)
 		return 0;
 
 	dev->wd_timeout = 0;
-	dev->wd_due_counter = 0;
 	memcpy(dev->wd_data, mei_stop_wd_params, MEI_WD_PARAMS_SIZE);
 	dev->stop = true;
 
@@ -357,8 +357,6 @@ void  mei_watchdog_register(struct mei_device *dev)
 {
 	dev_dbg(&dev->pdev->dev, "dev->wd_timeout =%d.\n", dev->wd_timeout);
 
-	dev->wd_due_counter = !!dev->wd_timeout;
-
 	if (watchdog_register_device(&amt_wd_dev)) {
 		dev_err(&dev->pdev->dev,
 			"wd: unable to register watchdog device.\n");
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index 2b62232c2c6..acfaeeb9e01 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -349,6 +349,11 @@ void st_int_recv(void *disc_data,
 			st_gdata->rx_skb = alloc_skb(
 					st_gdata->list[type]->max_frame_size,
 					GFP_ATOMIC);
+			if (st_gdata->rx_skb == NULL) {
+				pr_err("out of memory: dropping\n");
+				goto done;
+			}
+
 			skb_reserve(st_gdata->rx_skb,
 					st_gdata->list[type]->reserve);
 			/* next 2 required for BT only */
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 276d21ce6bc..f1c84decb19 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -850,9 +850,7 @@ out:
 		goto retry;
 	if (!err)
 		mmc_blk_reset_success(md, type);
-	spin_lock_irq(&md->lock);
-	__blk_end_request(req, err, blk_rq_bytes(req));
-	spin_unlock_irq(&md->lock);
+	blk_end_request(req, err, blk_rq_bytes(req));
 
 	return err ? 0 : 1;
 }
@@ -934,9 +932,7 @@ out_retry:
 	if (!err)
 		mmc_blk_reset_success(md, type);
 out:
-	spin_lock_irq(&md->lock);
-	__blk_end_request(req, err, blk_rq_bytes(req));
-	spin_unlock_irq(&md->lock);
+	blk_end_request(req, err, blk_rq_bytes(req));
 
 	return err ? 0 : 1;
 }
@@ -951,9 +947,7 @@ static int mmc_blk_issue_flush(struct mmc_queue *mq, struct request *req)
 	if (ret)
 		ret = -EIO;
 
-	spin_lock_irq(&md->lock);
-	__blk_end_request_all(req, ret);
-	spin_unlock_irq(&md->lock);
+	blk_end_request_all(req, ret);
 
 	return ret ? 0 : 1;
 }
@@ -1252,14 +1246,10 @@ static int mmc_blk_cmd_err(struct mmc_blk_data *md, struct mmc_card *card,
 
 		blocks = mmc_sd_num_wr_blocks(card);
 		if (blocks != (u32)-1) {
-			spin_lock_irq(&md->lock);
-			ret = __blk_end_request(req, 0, blocks << 9);
-			spin_unlock_irq(&md->lock);
+			ret = blk_end_request(req, 0, blocks << 9);
 		}
 	} else {
-		spin_lock_irq(&md->lock);
-		ret = __blk_end_request(req, 0, brq->data.bytes_xfered);
-		spin_unlock_irq(&md->lock);
+		ret = blk_end_request(req, 0, brq->data.bytes_xfered);
 	}
 	return ret;
 }
@@ -1311,10 +1301,8 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 			 * A block was successfully transferred.
 			 */
 			mmc_blk_reset_success(md, type);
-			spin_lock_irq(&md->lock);
-			ret = __blk_end_request(req, 0,
+			ret = blk_end_request(req, 0,
 						brq->data.bytes_xfered);
-			spin_unlock_irq(&md->lock);
 			/*
 			 * If the blk_end_request function returns non-zero even
 			 * though all data has been transferred and no errors
@@ -1364,10 +1352,8 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 			 * time, so we only reach here after trying to
 			 * read a single sector.
 			 */
-			spin_lock_irq(&md->lock);
-			ret = __blk_end_request(req, -EIO,
+			ret = blk_end_request(req, -EIO,
 						brq->data.blksz);
-			spin_unlock_irq(&md->lock);
 			if (!ret)
 				goto start_new_req;
 			break;
@@ -1388,12 +1374,10 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 	return 1;
 
  cmd_abort:
-	spin_lock_irq(&md->lock);
 	if (mmc_card_removed(card))
 		req->cmd_flags |= REQ_QUIET;
 	while (ret)
-		ret = __blk_end_request(req, -EIO, blk_rq_cur_bytes(req));
-	spin_unlock_irq(&md->lock);
+		ret = blk_end_request(req, -EIO, blk_rq_cur_bytes(req));
 
  start_new_req:
 	if (rqc) {
@@ -1417,9 +1401,7 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 	ret = mmc_blk_part_switch(card, md);
 	if (ret) {
 		if (req) {
-			spin_lock_irq(&md->lock);
-			__blk_end_request_all(req, -EIO);
-			spin_unlock_irq(&md->lock);
+			blk_end_request_all(req, -EIO);
 		}
 		ret = 0;
 		goto out;
diff --git a/drivers/mmc/core/Makefile b/drivers/mmc/core/Makefile
index dca4428380f..38ed210ce2f 100644
--- a/drivers/mmc/core/Makefile
+++ b/drivers/mmc/core/Makefile
@@ -7,6 +7,6 @@ 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 \
-				   quirks.o cd-gpio.o
+				   quirks.o slot-gpio.o
 
 mmc_core-$(CONFIG_DEBUG_FS)	+= debugfs.o
diff --git a/drivers/mmc/core/cd-gpio.c b/drivers/mmc/core/cd-gpio.c
deleted file mode 100644
index 8f5dc08d659..00000000000
--- a/drivers/mmc/core/cd-gpio.c
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Generic GPIO card-detect helper
- *
- * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/err.h>
-#include <linux/gpio.h>
-#include <linux/interrupt.h>
-#include <linux/jiffies.h>
-#include <linux/mmc/cd-gpio.h>
-#include <linux/mmc/host.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-
-struct mmc_cd_gpio {
-	unsigned int gpio;
-	char label[0];
-};
-
-static irqreturn_t mmc_cd_gpio_irqt(int irq, void *dev_id)
-{
-	/* Schedule a card detection after a debounce timeout */
-	mmc_detect_change(dev_id, msecs_to_jiffies(100));
-	return IRQ_HANDLED;
-}
-
-int mmc_cd_gpio_request(struct mmc_host *host, unsigned int gpio)
-{
-	size_t len = strlen(dev_name(host->parent)) + 4;
-	struct mmc_cd_gpio *cd;
-	int irq = gpio_to_irq(gpio);
-	int ret;
-
-	if (irq < 0)
-		return irq;
-
-	cd = kmalloc(sizeof(*cd) + len, GFP_KERNEL);
-	if (!cd)
-		return -ENOMEM;
-
-	snprintf(cd->label, len, "%s cd", dev_name(host->parent));
-
-	ret = gpio_request_one(gpio, GPIOF_DIR_IN, cd->label);
-	if (ret < 0)
-		goto egpioreq;
-
-	ret = request_threaded_irq(irq, NULL, mmc_cd_gpio_irqt,
-				   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING |
-				   IRQF_ONESHOT, cd->label, host);
-	if (ret < 0)
-		goto eirqreq;
-
-	cd->gpio = gpio;
-	host->hotplug.irq = irq;
-	host->hotplug.handler_priv = cd;
-
-	return 0;
-
-eirqreq:
-	gpio_free(gpio);
-egpioreq:
-	kfree(cd);
-	return ret;
-}
-EXPORT_SYMBOL(mmc_cd_gpio_request);
-
-void mmc_cd_gpio_free(struct mmc_host *host)
-{
-	struct mmc_cd_gpio *cd = host->hotplug.handler_priv;
-
-	if (!cd)
-		return;
-
-	free_irq(host->hotplug.irq, host);
-	gpio_free(cd->gpio);
-	kfree(cd);
-}
-EXPORT_SYMBOL(mmc_cd_gpio_free);
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 0b6141d29db..8ac5246e2ab 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -404,6 +404,7 @@ int mmc_interrupt_hpi(struct mmc_card *card)
 {
 	int err;
 	u32 status;
+	unsigned long prg_wait;
 
 	BUG_ON(!card);
 
@@ -419,30 +420,38 @@ int mmc_interrupt_hpi(struct mmc_card *card)
 		goto out;
 	}
 
-	/*
-	 * If the card status is in PRG-state, we can send the HPI command.
-	 */
-	if (R1_CURRENT_STATE(status) == R1_STATE_PRG) {
-		do {
-			/*
-			 * We don't know when the HPI command will finish
-			 * processing, so we need to resend HPI until out
-			 * of prg-state, and keep checking the card status
-			 * with SEND_STATUS.  If a timeout error occurs when
-			 * sending the HPI command, we are already out of
-			 * prg-state.
-			 */
-			err = mmc_send_hpi_cmd(card, &status);
-			if (err)
-				pr_debug("%s: abort HPI (%d error)\n",
-					 mmc_hostname(card->host), err);
+	switch (R1_CURRENT_STATE(status)) {
+	case R1_STATE_IDLE:
+	case R1_STATE_READY:
+	case R1_STATE_STBY:
+		/*
+		 * In idle states, HPI is not needed and the caller
+		 * can issue the next intended command immediately
+		 */
+		goto out;
+	case R1_STATE_PRG:
+		break;
+	default:
+		/* In all other states, it's illegal to issue HPI */
+		pr_debug("%s: HPI cannot be sent. Card state=%d\n",
+			mmc_hostname(card->host), R1_CURRENT_STATE(status));
+		err = -EINVAL;
+		goto out;
+	}
 
-			err = mmc_send_status(card, &status);
-			if (err)
-				break;
-		} while (R1_CURRENT_STATE(status) == R1_STATE_PRG);
-	} else
-		pr_debug("%s: Left prg-state\n", mmc_hostname(card->host));
+	err = mmc_send_hpi_cmd(card, &status);
+	if (err)
+		goto out;
+
+	prg_wait = jiffies + msecs_to_jiffies(card->ext_csd.out_of_int_time);
+	do {
+		err = mmc_send_status(card, &status);
+
+		if (!err && R1_CURRENT_STATE(status) == R1_STATE_TRAN)
+			break;
+		if (time_after(jiffies, prg_wait))
+			err = -ETIMEDOUT;
+	} while (!err);
 
 out:
 	mmc_release_host(card->host);
@@ -941,7 +950,7 @@ int mmc_regulator_get_ocrmask(struct regulator *supply)
 
 	return result;
 }
-EXPORT_SYMBOL(mmc_regulator_get_ocrmask);
+EXPORT_SYMBOL_GPL(mmc_regulator_get_ocrmask);
 
 /**
  * mmc_regulator_set_ocr - set regulator to match host->ios voltage
@@ -1011,7 +1020,30 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
 			"could not set regulator OCR (%d)\n", result);
 	return result;
 }
-EXPORT_SYMBOL(mmc_regulator_set_ocr);
+EXPORT_SYMBOL_GPL(mmc_regulator_set_ocr);
+
+int mmc_regulator_get_supply(struct mmc_host *mmc)
+{
+	struct device *dev = mmc_dev(mmc);
+	struct regulator *supply;
+	int ret;
+
+	supply = devm_regulator_get(dev, "vmmc");
+	mmc->supply.vmmc = supply;
+	mmc->supply.vqmmc = devm_regulator_get(dev, "vqmmc");
+
+	if (IS_ERR(supply))
+		return PTR_ERR(supply);
+
+	ret = mmc_regulator_get_ocrmask(supply);
+	if (ret > 0)
+		mmc->ocr_avail = ret;
+	else
+		dev_warn(mmc_dev(mmc), "Failed getting OCR mask: %d\n", ret);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mmc_regulator_get_supply);
 
 #endif /* CONFIG_REGULATOR */
 
@@ -1180,6 +1212,9 @@ static void mmc_power_up(struct mmc_host *host)
 	host->ios.timing = MMC_TIMING_LEGACY;
 	mmc_set_ios(host);
 
+	/* Set signal voltage to 3.3V */
+	mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330, false);
+
 	/*
 	 * This delay should be sufficient to allow the power supply
 	 * to reach the minimum voltage.
@@ -1931,9 +1966,6 @@ static int mmc_rescan_try_freq(struct mmc_host *host, unsigned freq)
 	 */
 	mmc_hw_reset_for_init(host);
 
-	/* Initialization should be done at 3.3 V I/O voltage. */
-	mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330, 0);
-
 	/*
 	 * sdio_reset sends CMD52 to reset card.  Since we do not know
 	 * if the card is being re-initialized, just send it.  CMD52
@@ -2075,6 +2107,7 @@ void mmc_rescan(struct work_struct *work)
 void mmc_start_host(struct mmc_host *host)
 {
 	host->f_init = max(freqs[0], host->f_min);
+	host->rescan_disable = 0;
 	mmc_power_up(host);
 	mmc_detect_change(host, 0);
 }
@@ -2088,6 +2121,7 @@ void mmc_stop_host(struct mmc_host *host)
 	spin_unlock_irqrestore(&host->lock, flags);
 #endif
 
+	host->rescan_disable = 1;
 	cancel_delayed_work_sync(&host->detect);
 	mmc_flush_scheduled_work();
 
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 91c84c7a182..597f189b442 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -32,6 +32,7 @@
 static void mmc_host_classdev_release(struct device *dev)
 {
 	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+	mutex_destroy(&host->slot.lock);
 	kfree(host);
 }
 
@@ -312,6 +313,8 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 	if (!host)
 		return NULL;
 
+	/* scanning will be enabled when we're ready */
+	host->rescan_disable = 1;
 	spin_lock(&mmc_host_lock);
 	err = idr_get_new(&mmc_host_idr, host, &host->index);
 	spin_unlock(&mmc_host_lock);
@@ -327,6 +330,9 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 
 	mmc_host_clk_init(host);
 
+	mutex_init(&host->slot.lock);
+	host->slot.cd_irq = -EINVAL;
+
 	spin_lock_init(&host->lock);
 	init_waitqueue_head(&host->wq);
 	INIT_DELAYED_WORK(&host->detect, mmc_rescan);
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 4f4489aa6ba..396b25891bb 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -818,9 +818,6 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	if (!mmc_host_is_spi(host))
 		mmc_set_bus_mode(host, MMC_BUSMODE_OPENDRAIN);
 
-	/* Initialization should be done at 3.3 V I/O voltage. */
-	mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330, 0);
-
 	/*
 	 * Since we're changing the OCR value, we seem to
 	 * need to tell some cards to go back to the idle
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 69370f494e0..0ed2cc5f35b 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -569,7 +569,6 @@ int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status)
 
 	cmd.opcode = opcode;
 	cmd.arg = card->rca << 16 | 1;
-	cmd.cmd_timeout_ms = card->ext_csd.out_of_int_time;
 
 	err = mmc_wait_for_cmd(card->host, &cmd, 0);
 	if (err) {
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index b2b43f624b9..74972c241df 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -244,7 +244,7 @@ static int mmc_read_ssr(struct mmc_card *card)
 	 * bitfield positions accordingly.
 	 */
 	au = UNSTUFF_BITS(ssr, 428 - 384, 4);
-	if (au > 0 || au <= 9) {
+	if (au > 0 && au <= 9) {
 		card->ssr.au = 1 << (au + 4);
 		es = UNSTUFF_BITS(ssr, 408 - 384, 16);
 		et = UNSTUFF_BITS(ssr, 402 - 384, 6);
@@ -290,8 +290,12 @@ static int mmc_read_switch(struct mmc_card *card)
 		return -ENOMEM;
 	}
 
-	/* Find out the supported Bus Speed Modes. */
-	err = mmc_sd_switch(card, 0, 0, 1, status);
+	/*
+	 * Find out the card's support bits with a mode 0 operation.
+	 * The argument does not matter, as the support bits do not
+	 * change with the arguments.
+	 */
+	err = mmc_sd_switch(card, 0, 0, 0, status);
 	if (err) {
 		/*
 		 * If the host or the card can't do the switch,
@@ -312,46 +316,8 @@ static int mmc_read_switch(struct mmc_card *card)
 
 	if (card->scr.sda_spec3) {
 		card->sw_caps.sd3_bus_mode = status[13];
-
-		/* Find out Driver Strengths supported by the card */
-		err = mmc_sd_switch(card, 0, 2, 1, status);
-		if (err) {
-			/*
-			 * If the host or the card can't do the switch,
-			 * fail more gracefully.
-			 */
-			if (err != -EINVAL && err != -ENOSYS && err != -EFAULT)
-				goto out;
-
-			pr_warning("%s: problem reading "
-				"Driver Strength.\n",
-				mmc_hostname(card->host));
-			err = 0;
-
-			goto out;
-		}
-
+		/* Driver Strengths supported by the card */
 		card->sw_caps.sd3_drv_type = status[9];
-
-		/* Find out Current Limits supported by the card */
-		err = mmc_sd_switch(card, 0, 3, 1, status);
-		if (err) {
-			/*
-			 * If the host or the card can't do the switch,
-			 * fail more gracefully.
-			 */
-			if (err != -EINVAL && err != -ENOSYS && err != -EFAULT)
-				goto out;
-
-			pr_warning("%s: problem reading "
-				"Current Limit.\n",
-				mmc_hostname(card->host));
-			err = 0;
-
-			goto out;
-		}
-
-		card->sw_caps.sd3_curr_limit = status[7];
 	}
 
 out:
@@ -551,60 +517,80 @@ static int sd_set_bus_speed_mode(struct mmc_card *card, u8 *status)
 	return 0;
 }
 
+/* Get host's max current setting at its current voltage */
+static u32 sd_get_host_max_current(struct mmc_host *host)
+{
+	u32 voltage, max_current;
+
+	voltage = 1 << host->ios.vdd;
+	switch (voltage) {
+	case MMC_VDD_165_195:
+		max_current = host->max_current_180;
+		break;
+	case MMC_VDD_29_30:
+	case MMC_VDD_30_31:
+		max_current = host->max_current_300;
+		break;
+	case MMC_VDD_32_33:
+	case MMC_VDD_33_34:
+		max_current = host->max_current_330;
+		break;
+	default:
+		max_current = 0;
+	}
+
+	return max_current;
+}
+
 static int sd_set_current_limit(struct mmc_card *card, u8 *status)
 {
-	int current_limit = 0;
+	int current_limit = SD_SET_CURRENT_NO_CHANGE;
 	int err;
+	u32 max_current;
 
 	/*
 	 * Current limit switch is only defined for SDR50, SDR104, and DDR50
-	 * bus speed modes. For other bus speed modes, we set the default
-	 * current limit of 200mA.
+	 * bus speed modes. For other bus speed modes, we do not change the
+	 * current limit.
 	 */
-	if ((card->sd_bus_speed == UHS_SDR50_BUS_SPEED) ||
-	    (card->sd_bus_speed == UHS_SDR104_BUS_SPEED) ||
-	    (card->sd_bus_speed == UHS_DDR50_BUS_SPEED)) {
-		if (card->host->caps & MMC_CAP_MAX_CURRENT_800) {
-			if (card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_800)
-				current_limit = SD_SET_CURRENT_LIMIT_800;
-			else if (card->sw_caps.sd3_curr_limit &
-					SD_MAX_CURRENT_600)
-				current_limit = SD_SET_CURRENT_LIMIT_600;
-			else if (card->sw_caps.sd3_curr_limit &
-					SD_MAX_CURRENT_400)
-				current_limit = SD_SET_CURRENT_LIMIT_400;
-			else if (card->sw_caps.sd3_curr_limit &
-					SD_MAX_CURRENT_200)
-				current_limit = SD_SET_CURRENT_LIMIT_200;
-		} else if (card->host->caps & MMC_CAP_MAX_CURRENT_600) {
-			if (card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_600)
-				current_limit = SD_SET_CURRENT_LIMIT_600;
-			else if (card->sw_caps.sd3_curr_limit &
-					SD_MAX_CURRENT_400)
-				current_limit = SD_SET_CURRENT_LIMIT_400;
-			else if (card->sw_caps.sd3_curr_limit &
-					SD_MAX_CURRENT_200)
-				current_limit = SD_SET_CURRENT_LIMIT_200;
-		} else if (card->host->caps & MMC_CAP_MAX_CURRENT_400) {
-			if (card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_400)
-				current_limit = SD_SET_CURRENT_LIMIT_400;
-			else if (card->sw_caps.sd3_curr_limit &
-					SD_MAX_CURRENT_200)
-				current_limit = SD_SET_CURRENT_LIMIT_200;
-		} else if (card->host->caps & MMC_CAP_MAX_CURRENT_200) {
-			if (card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_200)
-				current_limit = SD_SET_CURRENT_LIMIT_200;
-		}
-	} else
+	if ((card->sd_bus_speed != UHS_SDR50_BUS_SPEED) &&
+	    (card->sd_bus_speed != UHS_SDR104_BUS_SPEED) &&
+	    (card->sd_bus_speed != UHS_DDR50_BUS_SPEED))
+		return 0;
+
+	/*
+	 * Host has different current capabilities when operating at
+	 * different voltages, so find out its max current first.
+	 */
+	max_current = sd_get_host_max_current(card->host);
+
+	/*
+	 * We only check host's capability here, if we set a limit that is
+	 * higher than the card's maximum current, the card will be using its
+	 * maximum current, e.g. if the card's maximum current is 300ma, and
+	 * when we set current limit to 200ma, the card will draw 200ma, and
+	 * when we set current limit to 400/600/800ma, the card will draw its
+	 * maximum 300ma from the host.
+	 */
+	if (max_current >= 800)
+		current_limit = SD_SET_CURRENT_LIMIT_800;
+	else if (max_current >= 600)
+		current_limit = SD_SET_CURRENT_LIMIT_600;
+	else if (max_current >= 400)
+		current_limit = SD_SET_CURRENT_LIMIT_400;
+	else if (max_current >= 200)
 		current_limit = SD_SET_CURRENT_LIMIT_200;
 
-	err = mmc_sd_switch(card, 1, 3, current_limit, status);
-	if (err)
-		return err;
+	if (current_limit != SD_SET_CURRENT_NO_CHANGE) {
+		err = mmc_sd_switch(card, 1, 3, current_limit, status);
+		if (err)
+			return err;
 
-	if (((status[15] >> 4) & 0x0F) != current_limit)
-		pr_warning("%s: Problem setting current limit!\n",
-			mmc_hostname(card->host));
+		if (((status[15] >> 4) & 0x0F) != current_limit)
+			pr_warning("%s: Problem setting current limit!\n",
+				mmc_hostname(card->host));
+
+	}
 
 	return 0;
 }
@@ -726,6 +712,7 @@ struct device_type sd_type = {
 int mmc_sd_get_cid(struct mmc_host *host, u32 ocr, u32 *cid, u32 *rocr)
 {
 	int err;
+	u32 max_current;
 
 	/*
 	 * Since we're changing the OCR value, we seem to
@@ -753,9 +740,12 @@ int mmc_sd_get_cid(struct mmc_host *host, u32 ocr, u32 *cid, u32 *rocr)
 	    MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_SDR104 | MMC_CAP_UHS_DDR50))
 		ocr |= SD_OCR_S18R;
 
-	/* If the host can supply more than 150mA, XPC should be set to 1. */
-	if (host->caps & (MMC_CAP_SET_XPC_330 | MMC_CAP_SET_XPC_300 |
-	    MMC_CAP_SET_XPC_180))
+	/*
+	 * If the host can supply more than 150mA at current voltage,
+	 * XPC should be set to 1.
+	 */
+	max_current = sd_get_host_max_current(host);
+	if (max_current > 150)
 		ocr |= SD_OCR_XPC;
 
 try_again:
@@ -911,9 +901,6 @@ static int mmc_sd_init_card(struct mmc_host *host, u32 ocr,
 	BUG_ON(!host);
 	WARN_ON(!host->claimed);
 
-	/* The initialization should be done at 3.3 V I/O voltage. */
-	mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330, 0);
-
 	err = mmc_sd_get_cid(host, ocr, cid, &rocr);
 	if (err)
 		return err;
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 41c5fd8848f..d4619e2ec03 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -591,9 +591,6 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 	 * Inform the card of the voltage
 	 */
 	if (!powered_resume) {
-		/* The initialization should be done at 3.3 V I/O voltage. */
-		mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330, 0);
-
 		err = mmc_send_io_op_cond(host, host->ocr, &ocr);
 		if (err)
 			goto err;
@@ -1006,10 +1003,6 @@ static int mmc_sdio_power_restore(struct mmc_host *host)
 	 * restore the correct voltage setting of the card.
 	 */
 
-	/* The initialization should be done at 3.3 V I/O voltage. */
-	if (!mmc_card_keep_power(host))
-		mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330, 0);
-
 	sdio_reset(host);
 	mmc_go_idle(host);
 	mmc_send_if_cond(host, host->ocr_avail);
diff --git a/drivers/mmc/core/sdio_cis.c b/drivers/mmc/core/sdio_cis.c
index f1c7ed8f4d8..8e94e555b78 100644
--- a/drivers/mmc/core/sdio_cis.c
+++ b/drivers/mmc/core/sdio_cis.c
@@ -313,7 +313,7 @@ static int sdio_read_cis(struct mmc_card *card, struct sdio_func *func)
 
 			if (ret == -ENOENT) {
 				/* warn about unknown tuples */
-				pr_warning("%s: queuing unknown"
+				pr_warn_ratelimited("%s: queuing unknown"
 				       " CIS tuple 0x%02x (%u bytes)\n",
 				       mmc_hostname(card->host),
 				       tpl_code, tpl_link);
diff --git a/drivers/mmc/core/slot-gpio.c b/drivers/mmc/core/slot-gpio.c
new file mode 100644
index 00000000000..058242916ce
--- /dev/null
+++ b/drivers/mmc/core/slot-gpio.c
@@ -0,0 +1,188 @@
+/*
+ * Generic GPIO card-detect helper
+ *
+ * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/slot-gpio.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+struct mmc_gpio {
+	int ro_gpio;
+	int cd_gpio;
+	char *ro_label;
+	char cd_label[0];
+};
+
+static irqreturn_t mmc_gpio_cd_irqt(int irq, void *dev_id)
+{
+	/* Schedule a card detection after a debounce timeout */
+	mmc_detect_change(dev_id, msecs_to_jiffies(100));
+	return IRQ_HANDLED;
+}
+
+static int mmc_gpio_alloc(struct mmc_host *host)
+{
+	size_t len = strlen(dev_name(host->parent)) + 4;
+	struct mmc_gpio *ctx;
+
+	mutex_lock(&host->slot.lock);
+
+	ctx = host->slot.handler_priv;
+	if (!ctx) {
+		/*
+		 * devm_kzalloc() can be called after device_initialize(), even
+		 * before device_add(), i.e., between mmc_alloc_host() and
+		 * mmc_add_host()
+		 */
+		ctx = devm_kzalloc(&host->class_dev, sizeof(*ctx) + 2 * len,
+				   GFP_KERNEL);
+		if (ctx) {
+			ctx->ro_label = ctx->cd_label + len;
+			snprintf(ctx->cd_label, len, "%s cd", dev_name(host->parent));
+			snprintf(ctx->ro_label, len, "%s ro", dev_name(host->parent));
+			ctx->cd_gpio = -EINVAL;
+			ctx->ro_gpio = -EINVAL;
+			host->slot.handler_priv = ctx;
+		}
+	}
+
+	mutex_unlock(&host->slot.lock);
+
+	return ctx ? 0 : -ENOMEM;
+}
+
+int mmc_gpio_get_ro(struct mmc_host *host)
+{
+	struct mmc_gpio *ctx = host->slot.handler_priv;
+
+	if (!ctx || !gpio_is_valid(ctx->ro_gpio))
+		return -ENOSYS;
+
+	return !gpio_get_value_cansleep(ctx->ro_gpio) ^
+		!!(host->caps2 & MMC_CAP2_RO_ACTIVE_HIGH);
+}
+EXPORT_SYMBOL(mmc_gpio_get_ro);
+
+int mmc_gpio_get_cd(struct mmc_host *host)
+{
+	struct mmc_gpio *ctx = host->slot.handler_priv;
+
+	if (!ctx || !gpio_is_valid(ctx->cd_gpio))
+		return -ENOSYS;
+
+	return !gpio_get_value_cansleep(ctx->cd_gpio) ^
+		!!(host->caps2 & MMC_CAP2_CD_ACTIVE_HIGH);
+}
+EXPORT_SYMBOL(mmc_gpio_get_cd);
+
+int mmc_gpio_request_ro(struct mmc_host *host, unsigned int gpio)
+{
+	struct mmc_gpio *ctx;
+	int ret;
+
+	if (!gpio_is_valid(gpio))
+		return -EINVAL;
+
+	ret = mmc_gpio_alloc(host);
+	if (ret < 0)
+		return ret;
+
+	ctx = host->slot.handler_priv;
+
+	return gpio_request_one(gpio, GPIOF_DIR_IN, ctx->ro_label);
+}
+EXPORT_SYMBOL(mmc_gpio_request_ro);
+
+int mmc_gpio_request_cd(struct mmc_host *host, unsigned int gpio)
+{
+	struct mmc_gpio *ctx;
+	int irq = gpio_to_irq(gpio);
+	int ret;
+
+	ret = mmc_gpio_alloc(host);
+	if (ret < 0)
+		return ret;
+
+	ctx = host->slot.handler_priv;
+
+	ret = gpio_request_one(gpio, GPIOF_DIR_IN, ctx->cd_label);
+	if (ret < 0)
+		/*
+		 * don't bother freeing memory. It might still get used by other
+		 * slot functions, in any case it will be freed, when the device
+		 * is destroyed.
+		 */
+		return ret;
+
+	/*
+	 * Even if gpio_to_irq() returns a valid IRQ number, the platform might
+	 * still prefer to poll, e.g., because that IRQ number is already used
+	 * by another unit and cannot be shared.
+	 */
+	if (irq >= 0 && host->caps & MMC_CAP_NEEDS_POLL)
+		irq = -EINVAL;
+
+	if (irq >= 0) {
+		ret = request_threaded_irq(irq, NULL, mmc_gpio_cd_irqt,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+			ctx->cd_label, host);
+		if (ret < 0)
+			irq = ret;
+	}
+
+	host->slot.cd_irq = irq;
+
+	if (irq < 0)
+		host->caps |= MMC_CAP_NEEDS_POLL;
+
+	ctx->cd_gpio = gpio;
+
+	return 0;
+}
+EXPORT_SYMBOL(mmc_gpio_request_cd);
+
+void mmc_gpio_free_ro(struct mmc_host *host)
+{
+	struct mmc_gpio *ctx = host->slot.handler_priv;
+	int gpio;
+
+	if (!ctx || !gpio_is_valid(ctx->ro_gpio))
+		return;
+
+	gpio = ctx->ro_gpio;
+	ctx->ro_gpio = -EINVAL;
+
+	gpio_free(gpio);
+}
+EXPORT_SYMBOL(mmc_gpio_free_ro);
+
+void mmc_gpio_free_cd(struct mmc_host *host)
+{
+	struct mmc_gpio *ctx = host->slot.handler_priv;
+	int gpio;
+
+	if (!ctx || !gpio_is_valid(ctx->cd_gpio))
+		return;
+
+	if (host->slot.cd_irq >= 0) {
+		free_irq(host->slot.cd_irq, host);
+		host->slot.cd_irq = -EINVAL;
+	}
+
+	gpio = ctx->cd_gpio;
+	ctx->cd_gpio = -EINVAL;
+
+	gpio_free(gpio);
+}
+EXPORT_SYMBOL(mmc_gpio_free_cd);
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index f2c115e0643..322412cec4e 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -391,11 +391,17 @@ static int atmci_regs_show(struct seq_file *s, void *v)
 	clk_disable(host->mck);
 	spin_unlock_bh(&host->lock);
 
-	seq_printf(s, "MR:\t0x%08x%s%s CLKDIV=%u\n",
+	seq_printf(s, "MR:\t0x%08x%s%s ",
 			buf[ATMCI_MR / 4],
 			buf[ATMCI_MR / 4] & ATMCI_MR_RDPROOF ? " RDPROOF" : "",
-			buf[ATMCI_MR / 4] & ATMCI_MR_WRPROOF ? " WRPROOF" : "",
-			buf[ATMCI_MR / 4] & 0xff);
+			buf[ATMCI_MR / 4] & ATMCI_MR_WRPROOF ? " WRPROOF" : "");
+	if (host->caps.has_odd_clk_div)
+		seq_printf(s, "{CLKDIV,CLKODD}=%u\n",
+				((buf[ATMCI_MR / 4] & 0xff) << 1)
+				| ((buf[ATMCI_MR / 4] >> 16) & 1));
+	else
+		seq_printf(s, "CLKDIV=%u\n",
+				(buf[ATMCI_MR / 4] & 0xff));
 	seq_printf(s, "DTOR:\t0x%08x\n", buf[ATMCI_DTOR / 4]);
 	seq_printf(s, "SDCR:\t0x%08x\n", buf[ATMCI_SDCR / 4]);
 	seq_printf(s, "ARGR:\t0x%08x\n", buf[ATMCI_ARGR / 4]);
@@ -1685,7 +1691,6 @@ static void atmci_tasklet_func(unsigned long priv)
 
 			dev_dbg(&host->pdev->dev, "FSM: cmd ready\n");
 			host->cmd = NULL;
-			host->data = NULL;
 			data->bytes_xfered = data->blocks * data->blksz;
 			data->error = 0;
 			atmci_command_complete(host, mrq->stop);
@@ -1699,6 +1704,7 @@ static void atmci_tasklet_func(unsigned long priv)
 				atmci_writel(host, ATMCI_IER, ATMCI_NOTBUSY);
 				state = STATE_WAITING_NOTBUSY;
 			}
+			host->data = NULL;
 			break;
 
 		case STATE_END_REQUEST:
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 1ca5e72ceb6..72dc3cde646 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -405,11 +405,23 @@ static void dw_mci_idmac_start_dma(struct dw_mci *host, unsigned int sg_len)
 static int dw_mci_idmac_init(struct dw_mci *host)
 {
 	struct idmac_desc *p;
-	int i;
+	int i, dma_support;
 
 	/* Number of descriptors in the ring buffer */
 	host->ring_size = PAGE_SIZE / sizeof(struct idmac_desc);
 
+	/* Check if Hardware Configuration Register has support for DMA */
+	dma_support = (mci_readl(host, HCON) >> 16) & 0x3;
+
+	if (!dma_support || dma_support > 2) {
+		dev_err(&host->dev,
+			"Host Controller does not support IDMA Tx.\n");
+		host->dma_ops = NULL;
+		return -ENODEV;
+	}
+
+	dev_info(&host->dev, "Using internal DMA controller.\n");
+
 	/* Forward link the descriptor list */
 	for (i = 0, p = host->sg_cpu; i < host->ring_size - 1; i++, p++)
 		p->des3 = host->sg_dma + (sizeof(struct idmac_desc) * (i + 1));
@@ -1876,7 +1888,6 @@ static void dw_mci_init_dma(struct dw_mci *host)
 	/* Determine which DMA interface to use */
 #ifdef CONFIG_MMC_DW_IDMAC
 	host->dma_ops = &dw_mci_idmac_ops;
-	dev_info(&host->dev, "Using internal DMA controller.\n");
 #endif
 
 	if (!host->dma_ops)
@@ -2175,7 +2186,7 @@ int dw_mci_resume(struct dw_mci *host)
 		return ret;
 	}
 
-	if (host->dma_ops->init)
+	if (host->use_dma && host->dma_ops->init)
 		host->dma_ops->init(host);
 
 	/* Restore the old value at FIFOTH register */
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index 3b9136c1a47..a61cb5fca22 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -839,6 +839,10 @@ out:
 	if (r)
 		release_resource(r);
 	if (mmc)
+		if (!IS_ERR_OR_NULL(host->clk)) {
+			clk_disable_unprepare(host->clk);
+			clk_put(host->clk);
+		}
 		mmc_free_host(mmc);
 
 	return ret;
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index 277161d279b..a51f9309ffb 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -164,16 +164,23 @@ struct mxs_mmc_host {
 	spinlock_t			lock;
 	int				sdio_irq_en;
 	int				wp_gpio;
+	bool				wp_inverted;
 };
 
 static int mxs_mmc_get_ro(struct mmc_host *mmc)
 {
 	struct mxs_mmc_host *host = mmc_priv(mmc);
+	int ret;
 
 	if (!gpio_is_valid(host->wp_gpio))
 		return -EINVAL;
 
-	return gpio_get_value(host->wp_gpio);
+	ret = gpio_get_value(host->wp_gpio);
+
+	if (host->wp_inverted)
+		ret = !ret;
+
+	return ret;
 }
 
 static int mxs_mmc_get_cd(struct mmc_host *mmc)
@@ -707,6 +714,8 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 	struct pinctrl *pinctrl;
 	int ret = 0, irq_err, irq_dma;
 	dma_cap_mask_t mask;
+	struct regulator *reg_vmmc;
+	enum of_gpio_flags flags;
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dmares = platform_get_resource(pdev, IORESOURCE_DMA, 0);
@@ -747,6 +756,16 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 	host->mmc = mmc;
 	host->sdio_irq_en = 0;
 
+	reg_vmmc = devm_regulator_get(&pdev->dev, "vmmc");
+	if (!IS_ERR(reg_vmmc)) {
+		ret = regulator_enable(reg_vmmc);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Failed to enable vmmc regulator: %d\n", ret);
+			goto out_mmc_free;
+		}
+	}
+
 	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
 	if (IS_ERR(pinctrl)) {
 		ret = PTR_ERR(pinctrl);
@@ -785,7 +804,10 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 			mmc->caps |= MMC_CAP_4_BIT_DATA;
 		else if (bus_width == 8)
 			mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA;
-		host->wp_gpio = of_get_named_gpio(np, "wp-gpios", 0);
+		host->wp_gpio = of_get_named_gpio_flags(np, "wp-gpios", 0,
+							&flags);
+		if (flags & OF_GPIO_ACTIVE_LOW)
+			host->wp_inverted = 1;
 	} else {
 		if (pdata->flags & SLOTF_8_BIT_CAPABLE)
 			mmc->caps |= MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA;
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index 3e8dcf8d2e0..50e08f03aa6 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -17,10 +17,12 @@
 #include <linux/ioport.h>
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
+#include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
 #include <linux/delay.h>
 #include <linux/spinlock.h>
 #include <linux/timer.h>
+#include <linux/omap-dma.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
 #include <linux/clk.h>
@@ -128,6 +130,10 @@ struct mmc_omap_host {
 	unsigned char		id; /* 16xx chips have 2 MMC blocks */
 	struct clk *		iclk;
 	struct clk *		fclk;
+	struct dma_chan		*dma_rx;
+	u32			dma_rx_burst;
+	struct dma_chan		*dma_tx;
+	u32			dma_tx_burst;
 	struct resource		*mem_res;
 	void __iomem		*virt_base;
 	unsigned int		phys_base;
@@ -153,12 +159,8 @@ struct mmc_omap_host {
 
 	unsigned		use_dma:1;
 	unsigned		brs_received:1, dma_done:1;
-	unsigned		dma_is_read:1;
 	unsigned		dma_in_use:1;
-	int			dma_ch;
 	spinlock_t		dma_lock;
-	struct timer_list	dma_timer;
-	unsigned		dma_len;
 
 	struct mmc_omap_slot    *slots[OMAP_MMC_MAX_SLOTS];
 	struct mmc_omap_slot    *current_slot;
@@ -406,18 +408,25 @@ mmc_omap_release_dma(struct mmc_omap_host *host, struct mmc_data *data,
 		     int abort)
 {
 	enum dma_data_direction dma_data_dir;
+	struct device *dev = mmc_dev(host->mmc);
+	struct dma_chan *c;
 
-	BUG_ON(host->dma_ch < 0);
-	if (data->error)
-		omap_stop_dma(host->dma_ch);
-	/* Release DMA channel lazily */
-	mod_timer(&host->dma_timer, jiffies + HZ);
-	if (data->flags & MMC_DATA_WRITE)
+	if (data->flags & MMC_DATA_WRITE) {
 		dma_data_dir = DMA_TO_DEVICE;
-	else
+		c = host->dma_tx;
+	} else {
 		dma_data_dir = DMA_FROM_DEVICE;
-	dma_unmap_sg(mmc_dev(host->mmc), data->sg, host->sg_len,
-		     dma_data_dir);
+		c = host->dma_rx;
+	}
+	if (c) {
+		if (data->error) {
+			dmaengine_terminate_all(c);
+			/* Claim nothing transferred on error... */
+			data->bytes_xfered = 0;
+		}
+		dev = c->device->dev;
+	}
+	dma_unmap_sg(dev, data->sg, host->sg_len, dma_data_dir);
 }
 
 static void mmc_omap_send_stop_work(struct work_struct *work)
@@ -525,16 +534,6 @@ mmc_omap_end_of_data(struct mmc_omap_host *host, struct mmc_data *data)
 }
 
 static void
-mmc_omap_dma_timer(unsigned long data)
-{
-	struct mmc_omap_host *host = (struct mmc_omap_host *) data;
-
-	BUG_ON(host->dma_ch < 0);
-	omap_free_dma(host->dma_ch);
-	host->dma_ch = -1;
-}
-
-static void
 mmc_omap_dma_done(struct mmc_omap_host *host, struct mmc_data *data)
 {
 	unsigned long flags;
@@ -891,159 +890,15 @@ static void mmc_omap_cover_handler(unsigned long param)
 		  jiffies + msecs_to_jiffies(OMAP_MMC_COVER_POLL_DELAY));
 }
 
-/* Prepare to transfer the next segment of a scatterlist */
-static void
-mmc_omap_prepare_dma(struct mmc_omap_host *host, struct mmc_data *data)
+static void mmc_omap_dma_callback(void *priv)
 {
-	int dma_ch = host->dma_ch;
-	unsigned long data_addr;
-	u16 buf, frame;
-	u32 count;
-	struct scatterlist *sg = &data->sg[host->sg_idx];
-	int src_port = 0;
-	int dst_port = 0;
-	int sync_dev = 0;
-
-	data_addr = host->phys_base + OMAP_MMC_REG(host, DATA);
-	frame = data->blksz;
-	count = sg_dma_len(sg);
-
-	if ((data->blocks == 1) && (count > data->blksz))
-		count = frame;
-
-	host->dma_len = count;
-
-	/* FIFO is 16x2 bytes on 15xx, and 32x2 bytes on 16xx and 24xx.
-	 * Use 16 or 32 word frames when the blocksize is at least that large.
-	 * Blocksize is usually 512 bytes; but not for some SD reads.
-	 */
-	if (cpu_is_omap15xx() && frame > 32)
-		frame = 32;
-	else if (frame > 64)
-		frame = 64;
-	count /= frame;
-	frame >>= 1;
-
-	if (!(data->flags & MMC_DATA_WRITE)) {
-		buf = 0x800f | ((frame - 1) << 8);
-
-		if (cpu_class_is_omap1()) {
-			src_port = OMAP_DMA_PORT_TIPB;
-			dst_port = OMAP_DMA_PORT_EMIFF;
-		}
-		if (cpu_is_omap24xx())
-			sync_dev = OMAP24XX_DMA_MMC1_RX;
-
-		omap_set_dma_src_params(dma_ch, src_port,
-					OMAP_DMA_AMODE_CONSTANT,
-					data_addr, 0, 0);
-		omap_set_dma_dest_params(dma_ch, dst_port,
-					 OMAP_DMA_AMODE_POST_INC,
-					 sg_dma_address(sg), 0, 0);
-		omap_set_dma_dest_data_pack(dma_ch, 1);
-		omap_set_dma_dest_burst_mode(dma_ch, OMAP_DMA_DATA_BURST_4);
-	} else {
-		buf = 0x0f80 | ((frame - 1) << 0);
-
-		if (cpu_class_is_omap1()) {
-			src_port = OMAP_DMA_PORT_EMIFF;
-			dst_port = OMAP_DMA_PORT_TIPB;
-		}
-		if (cpu_is_omap24xx())
-			sync_dev = OMAP24XX_DMA_MMC1_TX;
-
-		omap_set_dma_dest_params(dma_ch, dst_port,
-					 OMAP_DMA_AMODE_CONSTANT,
-					 data_addr, 0, 0);
-		omap_set_dma_src_params(dma_ch, src_port,
-					OMAP_DMA_AMODE_POST_INC,
-					sg_dma_address(sg), 0, 0);
-		omap_set_dma_src_data_pack(dma_ch, 1);
-		omap_set_dma_src_burst_mode(dma_ch, OMAP_DMA_DATA_BURST_4);
-	}
+	struct mmc_omap_host *host = priv;
+	struct mmc_data *data = host->data;
 
-	/* Max limit for DMA frame count is 0xffff */
-	BUG_ON(count > 0xffff);
+	/* If we got to the end of DMA, assume everything went well */
+	data->bytes_xfered += data->blocks * data->blksz;
 
-	OMAP_MMC_WRITE(host, BUF, buf);
-	omap_set_dma_transfer_params(dma_ch, OMAP_DMA_DATA_TYPE_S16,
-				     frame, count, OMAP_DMA_SYNC_FRAME,
-				     sync_dev, 0);
-}
-
-/* A scatterlist segment completed */
-static void mmc_omap_dma_cb(int lch, u16 ch_status, void *data)
-{
-	struct mmc_omap_host *host = (struct mmc_omap_host *) data;
-	struct mmc_data *mmcdat = host->data;
-
-	if (unlikely(host->dma_ch < 0)) {
-		dev_err(mmc_dev(host->mmc),
-			"DMA callback while DMA not enabled\n");
-		return;
-	}
-	/* FIXME: We really should do something to _handle_ the errors */
-	if (ch_status & OMAP1_DMA_TOUT_IRQ) {
-		dev_err(mmc_dev(host->mmc),"DMA timeout\n");
-		return;
-	}
-	if (ch_status & OMAP_DMA_DROP_IRQ) {
-		dev_err(mmc_dev(host->mmc), "DMA sync error\n");
-		return;
-	}
-	if (!(ch_status & OMAP_DMA_BLOCK_IRQ)) {
-		return;
-	}
-	mmcdat->bytes_xfered += host->dma_len;
-	host->sg_idx++;
-	if (host->sg_idx < host->sg_len) {
-		mmc_omap_prepare_dma(host, host->data);
-		omap_start_dma(host->dma_ch);
-	} else
-		mmc_omap_dma_done(host, host->data);
-}
-
-static int mmc_omap_get_dma_channel(struct mmc_omap_host *host, struct mmc_data *data)
-{
-	const char *dma_dev_name;
-	int sync_dev, dma_ch, is_read, r;
-
-	is_read = !(data->flags & MMC_DATA_WRITE);
-	del_timer_sync(&host->dma_timer);
-	if (host->dma_ch >= 0) {
-		if (is_read == host->dma_is_read)
-			return 0;
-		omap_free_dma(host->dma_ch);
-		host->dma_ch = -1;
-	}
-
-	if (is_read) {
-		if (host->id == 0) {
-			sync_dev = OMAP_DMA_MMC_RX;
-			dma_dev_name = "MMC1 read";
-		} else {
-			sync_dev = OMAP_DMA_MMC2_RX;
-			dma_dev_name = "MMC2 read";
-		}
-	} else {
-		if (host->id == 0) {
-			sync_dev = OMAP_DMA_MMC_TX;
-			dma_dev_name = "MMC1 write";
-		} else {
-			sync_dev = OMAP_DMA_MMC2_TX;
-			dma_dev_name = "MMC2 write";
-		}
-	}
-	r = omap_request_dma(sync_dev, dma_dev_name, mmc_omap_dma_cb,
-			     host, &dma_ch);
-	if (r != 0) {
-		dev_dbg(mmc_dev(host->mmc), "omap_request_dma() failed with %d\n", r);
-		return r;
-	}
-	host->dma_ch = dma_ch;
-	host->dma_is_read = is_read;
-
-	return 0;
+	mmc_omap_dma_done(host, data);
 }
 
 static inline void set_cmd_timeout(struct mmc_omap_host *host, struct mmc_request *req)
@@ -1118,33 +973,85 @@ mmc_omap_prepare_data(struct mmc_omap_host *host, struct mmc_request *req)
 
 	host->sg_idx = 0;
 	if (use_dma) {
-		if (mmc_omap_get_dma_channel(host, data) == 0) {
-			enum dma_data_direction dma_data_dir;
-
-			if (data->flags & MMC_DATA_WRITE)
-				dma_data_dir = DMA_TO_DEVICE;
-			else
-				dma_data_dir = DMA_FROM_DEVICE;
-
-			host->sg_len = dma_map_sg(mmc_dev(host->mmc), data->sg,
-						sg_len, dma_data_dir);
-			host->total_bytes_left = 0;
-			mmc_omap_prepare_dma(host, req->data);
-			host->brs_received = 0;
-			host->dma_done = 0;
-			host->dma_in_use = 1;
-		} else
-			use_dma = 0;
+		enum dma_data_direction dma_data_dir;
+		struct dma_async_tx_descriptor *tx;
+		struct dma_chan *c;
+		u32 burst, *bp;
+		u16 buf;
+
+		/*
+		 * FIFO is 16x2 bytes on 15xx, and 32x2 bytes on 16xx
+		 * and 24xx. Use 16 or 32 word frames when the
+		 * blocksize is at least that large. Blocksize is
+		 * usually 512 bytes; but not for some SD reads.
+		 */
+		burst = cpu_is_omap15xx() ? 32 : 64;
+		if (burst > data->blksz)
+			burst = data->blksz;
+
+		burst >>= 1;
+
+		if (data->flags & MMC_DATA_WRITE) {
+			c = host->dma_tx;
+			bp = &host->dma_tx_burst;
+			buf = 0x0f80 | (burst - 1) << 0;
+			dma_data_dir = DMA_TO_DEVICE;
+		} else {
+			c = host->dma_rx;
+			bp = &host->dma_rx_burst;
+			buf = 0x800f | (burst - 1) << 8;
+			dma_data_dir = DMA_FROM_DEVICE;
+		}
+
+		if (!c)
+			goto use_pio;
+
+		/* Only reconfigure if we have a different burst size */
+		if (*bp != burst) {
+			struct dma_slave_config cfg;
+
+			cfg.src_addr = host->phys_base + OMAP_MMC_REG(host, DATA);
+			cfg.dst_addr = host->phys_base + OMAP_MMC_REG(host, DATA);
+			cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+			cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+			cfg.src_maxburst = burst;
+			cfg.dst_maxburst = burst;
+
+			if (dmaengine_slave_config(c, &cfg))
+				goto use_pio;
+
+			*bp = burst;
+		}
+
+		host->sg_len = dma_map_sg(c->device->dev, data->sg, sg_len,
+					  dma_data_dir);
+		if (host->sg_len == 0)
+			goto use_pio;
+
+		tx = dmaengine_prep_slave_sg(c, data->sg, host->sg_len,
+			data->flags & MMC_DATA_WRITE ? DMA_MEM_TO_DEV : DMA_DEV_TO_MEM,
+			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!tx)
+			goto use_pio;
+
+		OMAP_MMC_WRITE(host, BUF, buf);
+
+		tx->callback = mmc_omap_dma_callback;
+		tx->callback_param = host;
+		dmaengine_submit(tx);
+		host->brs_received = 0;
+		host->dma_done = 0;
+		host->dma_in_use = 1;
+		return;
 	}
+ use_pio:
 
 	/* Revert to PIO? */
-	if (!use_dma) {
-		OMAP_MMC_WRITE(host, BUF, 0x1f1f);
-		host->total_bytes_left = data->blocks * block_size;
-		host->sg_len = sg_len;
-		mmc_omap_sg_to_buf(host);
-		host->dma_in_use = 0;
-	}
+	OMAP_MMC_WRITE(host, BUF, 0x1f1f);
+	host->total_bytes_left = data->blocks * block_size;
+	host->sg_len = sg_len;
+	mmc_omap_sg_to_buf(host);
+	host->dma_in_use = 0;
 }
 
 static void mmc_omap_start_request(struct mmc_omap_host *host,
@@ -1157,8 +1064,12 @@ static void mmc_omap_start_request(struct mmc_omap_host *host,
 	/* only touch fifo AFTER the controller readies it */
 	mmc_omap_prepare_data(host, req);
 	mmc_omap_start_command(host, req->cmd);
-	if (host->dma_in_use)
-		omap_start_dma(host->dma_ch);
+	if (host->dma_in_use) {
+		struct dma_chan *c = host->data->flags & MMC_DATA_WRITE ?
+				host->dma_tx : host->dma_rx;
+
+		dma_async_issue_pending(c);
+	}
 }
 
 static void mmc_omap_request(struct mmc_host *mmc, struct mmc_request *req)
@@ -1400,6 +1311,8 @@ static int __devinit mmc_omap_probe(struct platform_device *pdev)
 	struct omap_mmc_platform_data *pdata = pdev->dev.platform_data;
 	struct mmc_omap_host *host = NULL;
 	struct resource *res;
+	dma_cap_mask_t mask;
+	unsigned sig;
 	int i, ret = 0;
 	int irq;
 
@@ -1439,7 +1352,6 @@ static int __devinit mmc_omap_probe(struct platform_device *pdev)
 	setup_timer(&host->clk_timer, mmc_omap_clk_timer, (unsigned long) host);
 
 	spin_lock_init(&host->dma_lock);
-	setup_timer(&host->dma_timer, mmc_omap_dma_timer, (unsigned long) host);
 	spin_lock_init(&host->slot_lock);
 	init_waitqueue_head(&host->slot_wq);
 
@@ -1450,11 +1362,7 @@ static int __devinit mmc_omap_probe(struct platform_device *pdev)
 	host->id = pdev->id;
 	host->mem_res = res;
 	host->irq = irq;
-
 	host->use_dma = 1;
-	host->dev->dma_mask = &pdata->dma_mask;
-	host->dma_ch = -1;
-
 	host->irq = irq;
 	host->phys_base = host->mem_res->start;
 	host->virt_base = ioremap(res->start, resource_size(res));
@@ -1474,9 +1382,48 @@ static int __devinit mmc_omap_probe(struct platform_device *pdev)
 		goto err_free_iclk;
 	}
 
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	host->dma_tx_burst = -1;
+	host->dma_rx_burst = -1;
+
+	if (cpu_is_omap24xx())
+		sig = host->id == 0 ? OMAP24XX_DMA_MMC1_TX : OMAP24XX_DMA_MMC2_TX;
+	else
+		sig = host->id == 0 ? OMAP_DMA_MMC_TX : OMAP_DMA_MMC2_TX;
+	host->dma_tx = dma_request_channel(mask, omap_dma_filter_fn, &sig);
+#if 0
+	if (!host->dma_tx) {
+		dev_err(host->dev, "unable to obtain TX DMA engine channel %u\n",
+			sig);
+		goto err_dma;
+	}
+#else
+	if (!host->dma_tx)
+		dev_warn(host->dev, "unable to obtain TX DMA engine channel %u\n",
+			sig);
+#endif
+	if (cpu_is_omap24xx())
+		sig = host->id == 0 ? OMAP24XX_DMA_MMC1_RX : OMAP24XX_DMA_MMC2_RX;
+	else
+		sig = host->id == 0 ? OMAP_DMA_MMC_RX : OMAP_DMA_MMC2_RX;
+	host->dma_rx = dma_request_channel(mask, omap_dma_filter_fn, &sig);
+#if 0
+	if (!host->dma_rx) {
+		dev_err(host->dev, "unable to obtain RX DMA engine channel %u\n",
+			sig);
+		goto err_dma;
+	}
+#else
+	if (!host->dma_rx)
+		dev_warn(host->dev, "unable to obtain RX DMA engine channel %u\n",
+			sig);
+#endif
+
 	ret = request_irq(host->irq, mmc_omap_irq, 0, DRIVER_NAME, host);
 	if (ret)
-		goto err_free_fclk;
+		goto err_free_dma;
 
 	if (pdata->init != NULL) {
 		ret = pdata->init(&pdev->dev);
@@ -1510,7 +1457,11 @@ err_plat_cleanup:
 		pdata->cleanup(&pdev->dev);
 err_free_irq:
 	free_irq(host->irq, host);
-err_free_fclk:
+err_free_dma:
+	if (host->dma_tx)
+		dma_release_channel(host->dma_tx);
+	if (host->dma_rx)
+		dma_release_channel(host->dma_rx);
 	clk_put(host->fclk);
 err_free_iclk:
 	clk_disable(host->iclk);
@@ -1545,6 +1496,11 @@ static int __devexit mmc_omap_remove(struct platform_device *pdev)
 	clk_disable(host->iclk);
 	clk_put(host->iclk);
 
+	if (host->dma_tx)
+		dma_release_channel(host->dma_tx);
+	if (host->dma_rx)
+		dma_release_channel(host->dma_rx);
+
 	iounmap(host->virt_base);
 	release_mem_region(pdev->resource[0].start,
 			   pdev->resource[0].end - pdev->resource[0].start + 1);
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 389a3eedfc2..3a09f93cc3b 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -19,6 +19,7 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/debugfs.h>
+#include <linux/dmaengine.h>
 #include <linux/seq_file.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
@@ -29,6 +30,7 @@
 #include <linux/of.h>
 #include <linux/of_gpio.h>
 #include <linux/of_device.h>
+#include <linux/omap-dma.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/core.h>
 #include <linux/mmc/mmc.h>
@@ -37,7 +39,6 @@
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
-#include <plat/dma.h>
 #include <mach/hardware.h>
 #include <plat/board.h>
 #include <plat/mmc.h>
@@ -166,7 +167,8 @@ struct omap_hsmmc_host {
 	int			suspended;
 	int			irq;
 	int			use_dma, dma_ch;
-	int			dma_line_tx, dma_line_rx;
+	struct dma_chan		*tx_chan;
+	struct dma_chan		*rx_chan;
 	int			slot_id;
 	int			response_busy;
 	int			context_loss;
@@ -797,6 +799,12 @@ omap_hsmmc_get_dma_dir(struct omap_hsmmc_host *host, struct mmc_data *data)
 		return DMA_FROM_DEVICE;
 }
 
+static struct dma_chan *omap_hsmmc_get_dma_chan(struct omap_hsmmc_host *host,
+	struct mmc_data *data)
+{
+	return data->flags & MMC_DATA_WRITE ? host->tx_chan : host->rx_chan;
+}
+
 static void omap_hsmmc_request_done(struct omap_hsmmc_host *host, struct mmc_request *mrq)
 {
 	int dma_ch;
@@ -889,10 +897,13 @@ static void omap_hsmmc_dma_cleanup(struct omap_hsmmc_host *host, int errno)
 	spin_unlock_irqrestore(&host->irq_lock, flags);
 
 	if (host->use_dma && dma_ch != -1) {
-		dma_unmap_sg(mmc_dev(host->mmc), host->data->sg,
-			host->data->sg_len,
+		struct dma_chan *chan = omap_hsmmc_get_dma_chan(host, host->data);
+
+		dmaengine_terminate_all(chan);
+		dma_unmap_sg(chan->device->dev,
+			host->data->sg, host->data->sg_len,
 			omap_hsmmc_get_dma_dir(host, host->data));
-		omap_free_dma(dma_ch);
+
 		host->data->host_cookie = 0;
 	}
 	host->data = NULL;
@@ -1089,7 +1100,7 @@ static int omap_hsmmc_switch_opcond(struct omap_hsmmc_host *host, int vdd)
 	/* Disable the clocks */
 	pm_runtime_put_sync(host->dev);
 	if (host->dbclk)
-		clk_disable(host->dbclk);
+		clk_disable_unprepare(host->dbclk);
 
 	/* Turn the power off */
 	ret = mmc_slot(host).set_power(host->dev, host->slot_id, 0, 0);
@@ -1100,7 +1111,7 @@ static int omap_hsmmc_switch_opcond(struct omap_hsmmc_host *host, int vdd)
 					       vdd);
 	pm_runtime_get_sync(host->dev);
 	if (host->dbclk)
-		clk_enable(host->dbclk);
+		clk_prepare_enable(host->dbclk);
 
 	if (ret != 0)
 		goto err;
@@ -1190,90 +1201,29 @@ static irqreturn_t omap_hsmmc_detect(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int omap_hsmmc_get_dma_sync_dev(struct omap_hsmmc_host *host,
-				     struct mmc_data *data)
-{
-	int sync_dev;
-
-	if (data->flags & MMC_DATA_WRITE)
-		sync_dev = host->dma_line_tx;
-	else
-		sync_dev = host->dma_line_rx;
-	return sync_dev;
-}
-
-static void omap_hsmmc_config_dma_params(struct omap_hsmmc_host *host,
-				       struct mmc_data *data,
-				       struct scatterlist *sgl)
+static void omap_hsmmc_dma_callback(void *param)
 {
-	int blksz, nblk, dma_ch;
-
-	dma_ch = host->dma_ch;
-	if (data->flags & MMC_DATA_WRITE) {
-		omap_set_dma_dest_params(dma_ch, 0, OMAP_DMA_AMODE_CONSTANT,
-			(host->mapbase + OMAP_HSMMC_DATA), 0, 0);
-		omap_set_dma_src_params(dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
-			sg_dma_address(sgl), 0, 0);
-	} else {
-		omap_set_dma_src_params(dma_ch, 0, OMAP_DMA_AMODE_CONSTANT,
-			(host->mapbase + OMAP_HSMMC_DATA), 0, 0);
-		omap_set_dma_dest_params(dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
-			sg_dma_address(sgl), 0, 0);
-	}
-
-	blksz = host->data->blksz;
-	nblk = sg_dma_len(sgl) / blksz;
-
-	omap_set_dma_transfer_params(dma_ch, OMAP_DMA_DATA_TYPE_S32,
-			blksz / 4, nblk, OMAP_DMA_SYNC_FRAME,
-			omap_hsmmc_get_dma_sync_dev(host, data),
-			!(data->flags & MMC_DATA_WRITE));
-
-	omap_start_dma(dma_ch);
-}
-
-/*
- * DMA call back function
- */
-static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
-{
-	struct omap_hsmmc_host *host = cb_data;
+	struct omap_hsmmc_host *host = param;
+	struct dma_chan *chan;
 	struct mmc_data *data;
-	int dma_ch, req_in_progress;
-	unsigned long flags;
+	int req_in_progress;
 
-	if (!(ch_status & OMAP_DMA_BLOCK_IRQ)) {
-		dev_warn(mmc_dev(host->mmc), "unexpected dma status %x\n",
-			ch_status);
-		return;
-	}
-
-	spin_lock_irqsave(&host->irq_lock, flags);
+	spin_lock_irq(&host->irq_lock);
 	if (host->dma_ch < 0) {
-		spin_unlock_irqrestore(&host->irq_lock, flags);
+		spin_unlock_irq(&host->irq_lock);
 		return;
 	}
 
 	data = host->mrq->data;
-	host->dma_sg_idx++;
-	if (host->dma_sg_idx < host->dma_len) {
-		/* Fire up the next transfer. */
-		omap_hsmmc_config_dma_params(host, data,
-					   data->sg + host->dma_sg_idx);
-		spin_unlock_irqrestore(&host->irq_lock, flags);
-		return;
-	}
-
+	chan = omap_hsmmc_get_dma_chan(host, data);
 	if (!data->host_cookie)
-		dma_unmap_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+		dma_unmap_sg(chan->device->dev,
+			     data->sg, data->sg_len,
 			     omap_hsmmc_get_dma_dir(host, data));
 
 	req_in_progress = host->req_in_progress;
-	dma_ch = host->dma_ch;
 	host->dma_ch = -1;
-	spin_unlock_irqrestore(&host->irq_lock, flags);
-
-	omap_free_dma(dma_ch);
+	spin_unlock_irq(&host->irq_lock);
 
 	/* If DMA has finished after TC, complete the request */
 	if (!req_in_progress) {
@@ -1286,7 +1236,8 @@ static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
 
 static int omap_hsmmc_pre_dma_transfer(struct omap_hsmmc_host *host,
 				       struct mmc_data *data,
-				       struct omap_hsmmc_next *next)
+				       struct omap_hsmmc_next *next,
+				       struct dma_chan *chan)
 {
 	int dma_len;
 
@@ -1301,8 +1252,7 @@ static int omap_hsmmc_pre_dma_transfer(struct omap_hsmmc_host *host,
 	/* Check if next job is already prepared */
 	if (next ||
 	    (!next && data->host_cookie != host->next_data.cookie)) {
-		dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg,
-				     data->sg_len,
+		dma_len = dma_map_sg(chan->device->dev, data->sg, data->sg_len,
 				     omap_hsmmc_get_dma_dir(host, data));
 
 	} else {
@@ -1329,8 +1279,11 @@ static int omap_hsmmc_pre_dma_transfer(struct omap_hsmmc_host *host,
 static int omap_hsmmc_start_dma_transfer(struct omap_hsmmc_host *host,
 					struct mmc_request *req)
 {
-	int dma_ch = 0, ret = 0, i;
+	struct dma_slave_config cfg;
+	struct dma_async_tx_descriptor *tx;
+	int ret = 0, i;
 	struct mmc_data *data = req->data;
+	struct dma_chan *chan;
 
 	/* Sanity check: all the SG entries must be aligned by block size. */
 	for (i = 0; i < data->sg_len; i++) {
@@ -1348,22 +1301,41 @@ static int omap_hsmmc_start_dma_transfer(struct omap_hsmmc_host *host,
 
 	BUG_ON(host->dma_ch != -1);
 
-	ret = omap_request_dma(omap_hsmmc_get_dma_sync_dev(host, data),
-			       "MMC/SD", omap_hsmmc_dma_cb, host, &dma_ch);
-	if (ret != 0) {
-		dev_err(mmc_dev(host->mmc),
-			"%s: omap_request_dma() failed with %d\n",
-			mmc_hostname(host->mmc), ret);
+	chan = omap_hsmmc_get_dma_chan(host, data);
+
+	cfg.src_addr = host->mapbase + OMAP_HSMMC_DATA;
+	cfg.dst_addr = host->mapbase + OMAP_HSMMC_DATA;
+	cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	cfg.src_maxburst = data->blksz / 4;
+	cfg.dst_maxburst = data->blksz / 4;
+
+	ret = dmaengine_slave_config(chan, &cfg);
+	if (ret)
 		return ret;
-	}
-	ret = omap_hsmmc_pre_dma_transfer(host, data, NULL);
+
+	ret = omap_hsmmc_pre_dma_transfer(host, data, NULL, chan);
 	if (ret)
 		return ret;
 
-	host->dma_ch = dma_ch;
-	host->dma_sg_idx = 0;
+	tx = dmaengine_prep_slave_sg(chan, data->sg, data->sg_len,
+		data->flags & MMC_DATA_WRITE ? DMA_MEM_TO_DEV : DMA_DEV_TO_MEM,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!tx) {
+		dev_err(mmc_dev(host->mmc), "prep_slave_sg() failed\n");
+		/* FIXME: cleanup */
+		return -1;
+	}
+
+	tx->callback = omap_hsmmc_dma_callback;
+	tx->callback_param = host;
+
+	/* Does not fail */
+	dmaengine_submit(tx);
 
-	omap_hsmmc_config_dma_params(host, data, data->sg);
+	host->dma_ch = 1;
+
+	dma_async_issue_pending(chan);
 
 	return 0;
 }
@@ -1445,11 +1417,11 @@ static void omap_hsmmc_post_req(struct mmc_host *mmc, struct mmc_request *mrq,
 	struct omap_hsmmc_host *host = mmc_priv(mmc);
 	struct mmc_data *data = mrq->data;
 
-	if (host->use_dma) {
-		if (data->host_cookie)
-			dma_unmap_sg(mmc_dev(host->mmc), data->sg,
-				     data->sg_len,
-				     omap_hsmmc_get_dma_dir(host, data));
+	if (host->use_dma && data->host_cookie) {
+		struct dma_chan *c = omap_hsmmc_get_dma_chan(host, data);
+
+		dma_unmap_sg(c->device->dev, data->sg, data->sg_len,
+			     omap_hsmmc_get_dma_dir(host, data));
 		data->host_cookie = 0;
 	}
 }
@@ -1464,10 +1436,13 @@ static void omap_hsmmc_pre_req(struct mmc_host *mmc, struct mmc_request *mrq,
 		return ;
 	}
 
-	if (host->use_dma)
+	if (host->use_dma) {
+		struct dma_chan *c = omap_hsmmc_get_dma_chan(host, mrq->data);
+
 		if (omap_hsmmc_pre_dma_transfer(host, mrq->data,
-						&host->next_data))
+						&host->next_data, c))
 			mrq->data->host_cookie = 0;
+	}
 }
 
 /*
@@ -1800,6 +1775,8 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret, irq;
 	const struct of_device_id *match;
+	dma_cap_mask_t mask;
+	unsigned tx_req, rx_req;
 
 	match = of_match_device(of_match_ptr(omap_mmc_of_match), &pdev->dev);
 	if (match) {
@@ -1844,7 +1821,6 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 	host->pdata	= pdata;
 	host->dev	= &pdev->dev;
 	host->use_dma	= 1;
-	host->dev->dma_mask = &pdata->dma_mask;
 	host->dma_ch	= -1;
 	host->irq	= irq;
 	host->slot_id	= 0;
@@ -1899,7 +1875,7 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 	if (IS_ERR(host->dbclk)) {
 		dev_warn(mmc_dev(host->mmc), "Failed to get debounce clk\n");
 		host->dbclk = NULL;
-	} else if (clk_enable(host->dbclk) != 0) {
+	} else if (clk_prepare_enable(host->dbclk) != 0) {
 		dev_warn(mmc_dev(host->mmc), "Failed to enable debounce clk\n");
 		clk_put(host->dbclk);
 		host->dbclk = NULL;
@@ -1931,16 +1907,35 @@ static int __devinit omap_hsmmc_probe(struct platform_device *pdev)
 	res = platform_get_resource_byname(pdev, IORESOURCE_DMA, "tx");
 	if (!res) {
 		dev_err(mmc_dev(host->mmc), "cannot get DMA TX channel\n");
+		ret = -ENXIO;
 		goto err_irq;
 	}
-	host->dma_line_tx = res->start;
+	tx_req = res->start;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_DMA, "rx");
 	if (!res) {
 		dev_err(mmc_dev(host->mmc), "cannot get DMA RX channel\n");
+		ret = -ENXIO;
+		goto err_irq;
+	}
+	rx_req = res->start;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	host->rx_chan = dma_request_channel(mask, omap_dma_filter_fn, &rx_req);
+	if (!host->rx_chan) {
+		dev_err(mmc_dev(host->mmc), "unable to obtain RX DMA engine channel %u\n", rx_req);
+		ret = -ENXIO;
+		goto err_irq;
+	}
+
+	host->tx_chan = dma_request_channel(mask, omap_dma_filter_fn, &tx_req);
+	if (!host->tx_chan) {
+		dev_err(mmc_dev(host->mmc), "unable to obtain TX DMA engine channel %u\n", tx_req);
+		ret = -ENXIO;
 		goto err_irq;
 	}
-	host->dma_line_rx = res->start;
 
 	/* Request IRQ for MMC operations */
 	ret = request_irq(host->irq, omap_hsmmc_irq, 0,
@@ -2019,11 +2014,15 @@ err_reg:
 err_irq_cd_init:
 	free_irq(host->irq, host);
 err_irq:
+	if (host->tx_chan)
+		dma_release_channel(host->tx_chan);
+	if (host->rx_chan)
+		dma_release_channel(host->rx_chan);
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
 	clk_put(host->fclk);
 	if (host->dbclk) {
-		clk_disable(host->dbclk);
+		clk_disable_unprepare(host->dbclk);
 		clk_put(host->dbclk);
 	}
 err1:
@@ -2054,11 +2053,16 @@ static int __devexit omap_hsmmc_remove(struct platform_device *pdev)
 	if (mmc_slot(host).card_detect_irq)
 		free_irq(mmc_slot(host).card_detect_irq, host);
 
+	if (host->tx_chan)
+		dma_release_channel(host->tx_chan);
+	if (host->rx_chan)
+		dma_release_channel(host->rx_chan);
+
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
 	clk_put(host->fclk);
 	if (host->dbclk) {
-		clk_disable(host->dbclk);
+		clk_disable_unprepare(host->dbclk);
 		clk_put(host->dbclk);
 	}
 
@@ -2116,7 +2120,7 @@ static int omap_hsmmc_suspend(struct device *dev)
 	}
 
 	if (host->dbclk)
-		clk_disable(host->dbclk);
+		clk_disable_unprepare(host->dbclk);
 err:
 	pm_runtime_put_sync(host->dev);
 	return ret;
@@ -2137,7 +2141,7 @@ static int omap_hsmmc_resume(struct device *dev)
 	pm_runtime_get_sync(host->dev);
 
 	if (host->dbclk)
-		clk_enable(host->dbclk);
+		clk_prepare_enable(host->dbclk);
 
 	if (!(host->mmc->pm_flags & MMC_PM_KEEP_POWER))
 		omap_hsmmc_conf_bus_power(host);
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index c3622a69f43..bd5a5cce122 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -26,7 +26,6 @@
 #include <mach/dma.h>
 
 #include <mach/regs-sdi.h>
-#include <mach/regs-gpio.h>
 
 #include <plat/mci.h>
 
@@ -1237,12 +1236,9 @@ static void s3cmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	switch (ios->power_mode) {
 	case MMC_POWER_ON:
 	case MMC_POWER_UP:
-		s3c2410_gpio_cfgpin(S3C2410_GPE(5), S3C2410_GPE5_SDCLK);
-		s3c2410_gpio_cfgpin(S3C2410_GPE(6), S3C2410_GPE6_SDCMD);
-		s3c2410_gpio_cfgpin(S3C2410_GPE(7), S3C2410_GPE7_SDDAT0);
-		s3c2410_gpio_cfgpin(S3C2410_GPE(8), S3C2410_GPE8_SDDAT1);
-		s3c2410_gpio_cfgpin(S3C2410_GPE(9), S3C2410_GPE9_SDDAT2);
-		s3c2410_gpio_cfgpin(S3C2410_GPE(10), S3C2410_GPE10_SDDAT3);
+		/* Configure GPE5...GPE10 pins in SD mode */
+		s3c_gpio_cfgall_range(S3C2410_GPE(5), 6, S3C_GPIO_SFN(2),
+				      S3C_GPIO_PULL_NONE);
 
 		if (host->pdata->set_power)
 			host->pdata->set_power(ios->power_mode, ios->vdd);
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index 177f697b583..a6e53a1ebb0 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -20,11 +20,17 @@
  */
 
 #include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/module.h>
 #include <linux/mmc/host.h>
 
 #include "sdhci-pltfm.h"
 
+struct sdhci_dove_priv {
+	struct clk *clk;
+};
+
 static u16 sdhci_dove_readw(struct sdhci_host *host, int reg)
 {
 	u16 ret;
@@ -66,16 +72,57 @@ static struct sdhci_pltfm_data sdhci_dove_pdata = {
 	.quirks	= SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER |
 		  SDHCI_QUIRK_NO_BUSY_IRQ |
 		  SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
-		  SDHCI_QUIRK_FORCE_DMA,
+		  SDHCI_QUIRK_FORCE_DMA |
+		  SDHCI_QUIRK_NO_HISPD_BIT,
 };
 
 static int __devinit sdhci_dove_probe(struct platform_device *pdev)
 {
-	return sdhci_pltfm_register(pdev, &sdhci_dove_pdata);
+	struct sdhci_host *host;
+	struct sdhci_pltfm_host *pltfm_host;
+	struct sdhci_dove_priv *priv;
+	int ret;
+
+	ret = sdhci_pltfm_register(pdev, &sdhci_dove_pdata);
+	if (ret)
+		goto sdhci_dove_register_fail;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct sdhci_dove_priv),
+			    GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "unable to allocate private data");
+		ret = -ENOMEM;
+		goto sdhci_dove_allocate_fail;
+	}
+
+	host = platform_get_drvdata(pdev);
+	pltfm_host = sdhci_priv(host);
+	pltfm_host->priv = priv;
+
+	priv->clk = clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(priv->clk))
+		clk_prepare_enable(priv->clk);
+	return 0;
+
+sdhci_dove_allocate_fail:
+	sdhci_pltfm_unregister(pdev);
+sdhci_dove_register_fail:
+	return ret;
 }
 
 static int __devexit sdhci_dove_remove(struct platform_device *pdev)
 {
+	struct sdhci_host *host = platform_get_drvdata(pdev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_dove_priv *priv = pltfm_host->priv;
+
+	if (priv->clk) {
+		if (!IS_ERR(priv->clk)) {
+			clk_disable_unprepare(priv->clk);
+			clk_put(priv->clk);
+		}
+		devm_kfree(&pdev->dev, priv->clk);
+	}
 	return sdhci_pltfm_unregister(pdev);
 }
 
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index ebbe984e5d0..e23f8134591 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -299,6 +299,8 @@ static void esdhc_writew_le(struct sdhci_host *host, u16 val, int reg)
 
 static void esdhc_writeb_le(struct sdhci_host *host, u8 val, int reg)
 {
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct pltfm_imx_data *imx_data = pltfm_host->priv;
 	u32 new_val;
 
 	switch (reg) {
@@ -315,8 +317,11 @@ static void esdhc_writeb_le(struct sdhci_host *host, u8 val, int reg)
 				SDHCI_CTRL_D3CD);
 		/* ensure the endianess */
 		new_val |= ESDHC_HOST_CONTROL_LE;
-		/* DMA mode bits are shifted */
-		new_val |= (val & SDHCI_CTRL_DMA_MASK) << 5;
+		/* bits 8&9 are reserved on mx25 */
+		if (!is_imx25_esdhc(imx_data)) {
+			/* DMA mode bits are shifted */
+			new_val |= (val & SDHCI_CTRL_DMA_MASK) << 5;
+		}
 
 		esdhc_clrset_le(host, 0xffff, new_val, reg);
 		return;
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 69ef0beae10..504da715a41 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -157,6 +157,7 @@ static const struct sdhci_pci_fixes sdhci_ene_714 = {
 static const struct sdhci_pci_fixes sdhci_cafe = {
 	.quirks		= SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER |
 			  SDHCI_QUIRK_NO_BUSY_IRQ |
+			  SDHCI_QUIRK_BROKEN_CARD_DETECTION |
 			  SDHCI_QUIRK_BROKEN_TIMEOUT_VAL,
 };
 
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index dbb75bfbcff..b6ee8857e22 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -28,6 +28,9 @@
 #include <linux/mmc/host.h>
 #include <linux/platform_data/pxa_sdhci.h>
 #include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
 #include "sdhci.h"
 #include "sdhci-pltfm.h"
 
@@ -121,6 +124,48 @@ static struct sdhci_ops pxav2_sdhci_ops = {
 	.platform_8bit_width = pxav2_mmc_set_width,
 };
 
+#ifdef CONFIG_OF
+static const struct of_device_id sdhci_pxav2_of_match[] = {
+	{
+		.compatible = "mrvl,pxav2-mmc",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, sdhci_pxav2_of_match);
+
+static struct sdhci_pxa_platdata *pxav2_get_mmc_pdata(struct device *dev)
+{
+	struct sdhci_pxa_platdata *pdata;
+	struct device_node *np = dev->of_node;
+	u32 bus_width;
+	u32 clk_delay_cycles;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	if (of_find_property(np, "non-removable", NULL))
+		pdata->flags |= PXA_FLAG_CARD_PERMANENT;
+
+	of_property_read_u32(np, "bus-width", &bus_width);
+	if (bus_width == 8)
+		pdata->flags |= PXA_FLAG_SD_8_BIT_CAPABLE_SLOT;
+
+	of_property_read_u32(np, "mrvl,clk-delay-cycles", &clk_delay_cycles);
+	if (clk_delay_cycles > 0) {
+		pdata->clk_delay_sel = 1;
+		pdata->clk_delay_cycles = clk_delay_cycles;
+	}
+
+	return pdata;
+}
+#else
+static inline struct sdhci_pxa_platdata *pxav2_get_mmc_pdata(struct device *dev)
+{
+	return NULL;
+}
+#endif
+
 static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
 {
 	struct sdhci_pltfm_host *pltfm_host;
@@ -128,6 +173,8 @@ static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct sdhci_host *host = NULL;
 	struct sdhci_pxa *pxa = NULL;
+	const struct of_device_id *match;
+
 	int ret;
 	struct clk *clk;
 
@@ -156,6 +203,10 @@ static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
 		| SDHCI_QUIRK_BROKEN_TIMEOUT_VAL
 		| SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN;
 
+	match = of_match_device(of_match_ptr(sdhci_pxav2_of_match), &pdev->dev);
+	if (match) {
+		pdata = pxav2_get_mmc_pdata(dev);
+	}
 	if (pdata) {
 		if (pdata->flags & PXA_FLAG_CARD_PERMANENT) {
 			/* on-chip device */
@@ -218,6 +269,9 @@ static struct platform_driver sdhci_pxav2_driver = {
 	.driver		= {
 		.name	= "sdhci-pxav2",
 		.owner	= THIS_MODULE,
+#ifdef CONFIG_OF
+		.of_match_table = sdhci_pxav2_of_match,
+#endif
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
 	.probe		= sdhci_pxav2_probe,
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index f2969568355..07fe3834fe0 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -28,6 +28,9 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
 #include "sdhci.h"
 #include "sdhci-pltfm.h"
 
@@ -164,6 +167,46 @@ static struct sdhci_ops pxav3_sdhci_ops = {
 	.platform_send_init_74_clocks = pxav3_gen_init_74_clocks,
 };
 
+#ifdef CONFIG_OF
+static const struct of_device_id sdhci_pxav3_of_match[] = {
+	{
+		.compatible = "mrvl,pxav3-mmc",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, sdhci_pxav3_of_match);
+
+static struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
+{
+	struct sdhci_pxa_platdata *pdata;
+	struct device_node *np = dev->of_node;
+	u32 bus_width;
+	u32 clk_delay_cycles;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	if (of_find_property(np, "non-removable", NULL))
+		pdata->flags |= PXA_FLAG_CARD_PERMANENT;
+
+	of_property_read_u32(np, "bus-width", &bus_width);
+	if (bus_width == 8)
+		pdata->flags |= PXA_FLAG_SD_8_BIT_CAPABLE_SLOT;
+
+	of_property_read_u32(np, "mrvl,clk-delay-cycles", &clk_delay_cycles);
+	if (clk_delay_cycles > 0)
+		pdata->clk_delay_cycles = clk_delay_cycles;
+
+	return pdata;
+}
+#else
+static inline struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
+{
+	return NULL;
+}
+#endif
+
 static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
 {
 	struct sdhci_pltfm_host *pltfm_host;
@@ -171,6 +214,8 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct sdhci_host *host = NULL;
 	struct sdhci_pxa *pxa = NULL;
+	const struct of_device_id *match;
+
 	int ret;
 	struct clk *clk;
 
@@ -202,6 +247,10 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
 	/* enable 1/8V DDR capable */
 	host->mmc->caps |= MMC_CAP_1_8V_DDR;
 
+	match = of_match_device(of_match_ptr(sdhci_pxav3_of_match), &pdev->dev);
+	if (match)
+		pdata = pxav3_get_mmc_pdata(dev);
+
 	if (pdata) {
 		if (pdata->flags & PXA_FLAG_CARD_PERMANENT) {
 			/* on-chip device */
@@ -263,6 +312,9 @@ static int __devexit sdhci_pxav3_remove(struct platform_device *pdev)
 static struct platform_driver sdhci_pxav3_driver = {
 	.driver		= {
 		.name	= "sdhci-pxav3",
+#ifdef CONFIG_OF
+		.of_match_table = sdhci_pxav3_of_match,
+#endif
 		.owner	= THIS_MODULE,
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index b38d8a78f6a..0810ccc23d7 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -223,6 +223,7 @@ static struct tegra_sdhci_platform_data * __devinit sdhci_tegra_dt_parse_pdata(
 {
 	struct tegra_sdhci_platform_data *plat;
 	struct device_node *np = pdev->dev.of_node;
+	u32 bus_width;
 
 	if (!np)
 		return NULL;
@@ -236,7 +237,9 @@ static struct tegra_sdhci_platform_data * __devinit sdhci_tegra_dt_parse_pdata(
 	plat->cd_gpio = of_get_named_gpio(np, "cd-gpios", 0);
 	plat->wp_gpio = of_get_named_gpio(np, "wp-gpios", 0);
 	plat->power_gpio = of_get_named_gpio(np, "power-gpios", 0);
-	if (of_find_property(np, "support-8bit", NULL))
+
+	if (of_property_read_u32(np, "bus-width", &bus_width) == 0 &&
+	    bus_width == 8)
 		plat->is_8bit = 1;
 
 	return plat;
@@ -334,7 +337,7 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 		rc = PTR_ERR(clk);
 		goto err_clk_get;
 	}
-	clk_enable(clk);
+	clk_prepare_enable(clk);
 	pltfm_host->clk = clk;
 
 	host->mmc->pm_caps = plat->pm_flags;
@@ -349,7 +352,7 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_host:
-	clk_disable(pltfm_host->clk);
+	clk_disable_unprepare(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 err_clk_get:
 	if (gpio_is_valid(plat->wp_gpio))
@@ -390,7 +393,7 @@ static int __devexit sdhci_tegra_remove(struct platform_device *pdev)
 	if (gpio_is_valid(plat->power_gpio))
 		gpio_free(plat->power_gpio);
 
-	clk_disable(pltfm_host->clk);
+	clk_disable_unprepare(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 
 	sdhci_pltfm_free(pdev);
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index f4b8b4db3a9..9a11dc39921 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -27,6 +27,7 @@
 
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/card.h>
 
 #include "sdhci.h"
 
@@ -244,6 +245,19 @@ static void sdhci_init(struct sdhci_host *host, int soft)
 static void sdhci_reinit(struct sdhci_host *host)
 {
 	sdhci_init(host, 0);
+	/*
+	 * Retuning stuffs are affected by different cards inserted and only
+	 * applicable to UHS-I cards. So reset these fields to their initial
+	 * value when card is removed.
+	 */
+	if (host->flags & SDHCI_USING_RETUNING_TIMER) {
+		host->flags &= ~SDHCI_USING_RETUNING_TIMER;
+
+		del_timer_sync(&host->tuning_timer);
+		host->flags &= ~SDHCI_NEEDS_RETUNING;
+		host->mmc->max_blk_count =
+			(host->quirks & SDHCI_QUIRK_NO_MULTIBLOCK) ? 1 : 65535;
+	}
 	sdhci_enable_card_detection(host);
 }
 
@@ -1245,6 +1259,7 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	struct sdhci_host *host;
 	bool present;
 	unsigned long flags;
+	u32 tuning_opcode;
 
 	host = mmc_priv(mmc);
 
@@ -1292,8 +1307,12 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 		 */
 		if ((host->flags & SDHCI_NEEDS_RETUNING) &&
 		    !(present_state & (SDHCI_DOING_WRITE | SDHCI_DOING_READ))) {
+			/* eMMC uses cmd21 while sd and sdio use cmd19 */
+			tuning_opcode = mmc->card->type == MMC_TYPE_MMC ?
+				MMC_SEND_TUNING_BLOCK_HS200 :
+				MMC_SEND_TUNING_BLOCK;
 			spin_unlock_irqrestore(&host->lock, flags);
-			sdhci_execute_tuning(mmc, mrq->cmd->opcode);
+			sdhci_execute_tuning(mmc, tuning_opcode);
 			spin_lock_irqsave(&host->lock, flags);
 
 			/* Restore original mmc_request structure */
@@ -1663,11 +1682,15 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 		pwr = sdhci_readb(host, SDHCI_POWER_CONTROL);
 		pwr &= ~SDHCI_POWER_ON;
 		sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+		if (host->vmmc)
+			regulator_disable(host->vmmc);
 
 		/* Wait for 1ms as per the spec */
 		usleep_range(1000, 1500);
 		pwr |= SDHCI_POWER_ON;
 		sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+		if (host->vmmc)
+			regulator_enable(host->vmmc);
 
 		pr_info(DRIVER_NAME ": Switching to 1.8V signalling "
 			"voltage failed, retrying with S18R set to 0\n");
@@ -1855,6 +1878,7 @@ out:
 	 */
 	if (!(host->flags & SDHCI_NEEDS_RETUNING) && host->tuning_count &&
 	    (host->tuning_mode == SDHCI_TUNING_MODE_1)) {
+		host->flags |= SDHCI_USING_RETUNING_TIMER;
 		mod_timer(&host->tuning_timer, jiffies +
 			host->tuning_count * HZ);
 		/* Tuning mode 1 limits the maximum data length to 4MB */
@@ -1872,10 +1896,10 @@ out:
 	 * try tuning again at a later time, when the re-tuning timer expires.
 	 * So for these controllers, we return 0. Since there might be other
 	 * controllers who do not have this capability, we return error for
-	 * them.
+	 * them. SDHCI_USING_RETUNING_TIMER means the host is currently using
+	 * a retuning timer to do the retuning for the card.
 	 */
-	if (err && host->tuning_count &&
-	    host->tuning_mode == SDHCI_TUNING_MODE_1)
+	if (err && (host->flags & SDHCI_USING_RETUNING_TIMER))
 		err = 0;
 
 	sdhci_clear_set_irqs(host, SDHCI_INT_DATA_AVAIL, ier);
@@ -2382,7 +2406,6 @@ out:
 int sdhci_suspend_host(struct sdhci_host *host)
 {
 	int ret;
-	bool has_tuning_timer;
 
 	if (host->ops->platform_suspend)
 		host->ops->platform_suspend(host);
@@ -2390,16 +2413,14 @@ int sdhci_suspend_host(struct sdhci_host *host)
 	sdhci_disable_card_detection(host);
 
 	/* Disable tuning since we are suspending */
-	has_tuning_timer = host->version >= SDHCI_SPEC_300 &&
-		host->tuning_count && host->tuning_mode == SDHCI_TUNING_MODE_1;
-	if (has_tuning_timer) {
+	if (host->flags & SDHCI_USING_RETUNING_TIMER) {
 		del_timer_sync(&host->tuning_timer);
 		host->flags &= ~SDHCI_NEEDS_RETUNING;
 	}
 
 	ret = mmc_suspend_host(host->mmc);
 	if (ret) {
-		if (has_tuning_timer) {
+		if (host->flags & SDHCI_USING_RETUNING_TIMER) {
 			host->flags |= SDHCI_NEEDS_RETUNING;
 			mod_timer(&host->tuning_timer, jiffies +
 					host->tuning_count * HZ);
@@ -2450,8 +2471,7 @@ int sdhci_resume_host(struct sdhci_host *host)
 		host->ops->platform_resume(host);
 
 	/* Set the re-tuning expiration flag */
-	if ((host->version >= SDHCI_SPEC_300) && host->tuning_count &&
-	    (host->tuning_mode == SDHCI_TUNING_MODE_1))
+	if (host->flags & SDHCI_USING_RETUNING_TIMER)
 		host->flags |= SDHCI_NEEDS_RETUNING;
 
 	return ret;
@@ -2490,8 +2510,7 @@ int sdhci_runtime_suspend_host(struct sdhci_host *host)
 	int ret = 0;
 
 	/* Disable tuning since we are suspending */
-	if (host->version >= SDHCI_SPEC_300 &&
-	    host->tuning_mode == SDHCI_TUNING_MODE_1) {
+	if (host->flags & SDHCI_USING_RETUNING_TIMER) {
 		del_timer_sync(&host->tuning_timer);
 		host->flags &= ~SDHCI_NEEDS_RETUNING;
 	}
@@ -2532,8 +2551,7 @@ int sdhci_runtime_resume_host(struct sdhci_host *host)
 		sdhci_do_enable_preset_value(host, true);
 
 	/* Set the re-tuning expiration flag */
-	if ((host->version >= SDHCI_SPEC_300) && host->tuning_count &&
-	    (host->tuning_mode == SDHCI_TUNING_MODE_1))
+	if (host->flags & SDHCI_USING_RETUNING_TIMER)
 		host->flags |= SDHCI_NEEDS_RETUNING;
 
 	spin_lock_irqsave(&host->lock, flags);
@@ -2584,7 +2602,7 @@ EXPORT_SYMBOL_GPL(sdhci_alloc_host);
 int sdhci_add_host(struct sdhci_host *host)
 {
 	struct mmc_host *mmc;
-	u32 caps[2];
+	u32 caps[2] = {0, 0};
 	u32 max_current_caps;
 	unsigned int ocr_avail;
 	int ret;
@@ -2614,8 +2632,10 @@ int sdhci_add_host(struct sdhci_host *host)
 	caps[0] = (host->quirks & SDHCI_QUIRK_MISSING_CAPS) ? host->caps :
 		sdhci_readl(host, SDHCI_CAPABILITIES);
 
-	caps[1] = (host->version >= SDHCI_SPEC_300) ?
-		sdhci_readl(host, SDHCI_CAPABILITIES_1) : 0;
+	if (host->version >= SDHCI_SPEC_300)
+		caps[1] = (host->quirks & SDHCI_QUIRK_MISSING_CAPS) ?
+			host->caps1 :
+			sdhci_readl(host, SDHCI_CAPABILITIES_1);
 
 	if (host->quirks & SDHCI_QUIRK_FORCE_DMA)
 		host->flags |= SDHCI_USE_SDMA;
@@ -2779,7 +2799,7 @@ int sdhci_add_host(struct sdhci_host *host)
 		mmc->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
 
 	if ((host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION) &&
-	    mmc_card_is_removable(mmc))
+	    !(host->mmc->caps & MMC_CAP_NONREMOVABLE))
 		mmc->caps |= MMC_CAP_NEEDS_POLL;
 
 	/* Any UHS-I mode in caps implies SDR12 and SDR25 support. */
@@ -2837,6 +2857,30 @@ int sdhci_add_host(struct sdhci_host *host)
 			     SDHCI_RETUNING_MODE_SHIFT;
 
 	ocr_avail = 0;
+
+	host->vmmc = regulator_get(mmc_dev(mmc), "vmmc");
+	if (IS_ERR(host->vmmc)) {
+		pr_info("%s: no vmmc regulator found\n", mmc_hostname(mmc));
+		host->vmmc = NULL;
+	}
+
+#ifdef CONFIG_REGULATOR
+	if (host->vmmc) {
+		ret = regulator_is_supported_voltage(host->vmmc, 3300000,
+			3300000);
+		if ((ret <= 0) || (!(caps[0] & SDHCI_CAN_VDD_330)))
+			caps[0] &= ~SDHCI_CAN_VDD_330;
+		ret = regulator_is_supported_voltage(host->vmmc, 3000000,
+			3000000);
+		if ((ret <= 0) || (!(caps[0] & SDHCI_CAN_VDD_300)))
+			caps[0] &= ~SDHCI_CAN_VDD_300;
+		ret = regulator_is_supported_voltage(host->vmmc, 1800000,
+			1800000);
+		if ((ret <= 0) || (!(caps[0] & SDHCI_CAN_VDD_180)))
+			caps[0] &= ~SDHCI_CAN_VDD_180;
+	}
+#endif /* CONFIG_REGULATOR */
+
 	/*
 	 * According to SD Host Controller spec v3.00, if the Host System
 	 * can afford more than 150mA, Host Driver should set XPC to 1. Also
@@ -2845,55 +2889,45 @@ int sdhci_add_host(struct sdhci_host *host)
 	 * value.
 	 */
 	max_current_caps = sdhci_readl(host, SDHCI_MAX_CURRENT);
+	if (!max_current_caps && host->vmmc) {
+		u32 curr = regulator_get_current_limit(host->vmmc);
+		if (curr > 0) {
+
+			/* convert to SDHCI_MAX_CURRENT format */
+			curr = curr/1000;  /* convert to mA */
+			curr = curr/SDHCI_MAX_CURRENT_MULTIPLIER;
+
+			curr = min_t(u32, curr, SDHCI_MAX_CURRENT_LIMIT);
+			max_current_caps =
+				(curr << SDHCI_MAX_CURRENT_330_SHIFT) |
+				(curr << SDHCI_MAX_CURRENT_300_SHIFT) |
+				(curr << SDHCI_MAX_CURRENT_180_SHIFT);
+		}
+	}
 
 	if (caps[0] & SDHCI_CAN_VDD_330) {
-		int max_current_330;
-
 		ocr_avail |= MMC_VDD_32_33 | MMC_VDD_33_34;
 
-		max_current_330 = ((max_current_caps &
+		mmc->max_current_330 = ((max_current_caps &
 				   SDHCI_MAX_CURRENT_330_MASK) >>
 				   SDHCI_MAX_CURRENT_330_SHIFT) *
 				   SDHCI_MAX_CURRENT_MULTIPLIER;
-
-		if (max_current_330 > 150)
-			mmc->caps |= MMC_CAP_SET_XPC_330;
 	}
 	if (caps[0] & SDHCI_CAN_VDD_300) {
-		int max_current_300;
-
 		ocr_avail |= MMC_VDD_29_30 | MMC_VDD_30_31;
 
-		max_current_300 = ((max_current_caps &
+		mmc->max_current_300 = ((max_current_caps &
 				   SDHCI_MAX_CURRENT_300_MASK) >>
 				   SDHCI_MAX_CURRENT_300_SHIFT) *
 				   SDHCI_MAX_CURRENT_MULTIPLIER;
-
-		if (max_current_300 > 150)
-			mmc->caps |= MMC_CAP_SET_XPC_300;
 	}
 	if (caps[0] & SDHCI_CAN_VDD_180) {
-		int max_current_180;
-
 		ocr_avail |= MMC_VDD_165_195;
 
-		max_current_180 = ((max_current_caps &
+		mmc->max_current_180 = ((max_current_caps &
 				   SDHCI_MAX_CURRENT_180_MASK) >>
 				   SDHCI_MAX_CURRENT_180_SHIFT) *
 				   SDHCI_MAX_CURRENT_MULTIPLIER;
-
-		if (max_current_180 > 150)
-			mmc->caps |= MMC_CAP_SET_XPC_180;
-
-		/* Maximum current capabilities of the host at 1.8V */
-		if (max_current_180 >= 800)
-			mmc->caps |= MMC_CAP_MAX_CURRENT_800;
-		else if (max_current_180 >= 600)
-			mmc->caps |= MMC_CAP_MAX_CURRENT_600;
-		else if (max_current_180 >= 400)
-			mmc->caps |= MMC_CAP_MAX_CURRENT_400;
-		else
-			mmc->caps |= MMC_CAP_MAX_CURRENT_200;
 	}
 
 	mmc->ocr_avail = ocr_avail;
@@ -2992,13 +3026,10 @@ int sdhci_add_host(struct sdhci_host *host)
 
 	ret = request_irq(host->irq, sdhci_irq, IRQF_SHARED,
 		mmc_hostname(mmc), host);
-	if (ret)
+	if (ret) {
+		pr_err("%s: Failed to request IRQ %d: %d\n",
+		       mmc_hostname(mmc), host->irq, ret);
 		goto untasklet;
-
-	host->vmmc = regulator_get(mmc_dev(mmc), "vmmc");
-	if (IS_ERR(host->vmmc)) {
-		pr_info("%s: no vmmc regulator found\n", mmc_hostname(mmc));
-		host->vmmc = NULL;
 	}
 
 	sdhci_init(host, 0);
@@ -3016,8 +3047,11 @@ int sdhci_add_host(struct sdhci_host *host)
 	host->led.brightness_set = sdhci_led_control;
 
 	ret = led_classdev_register(mmc_dev(mmc), &host->led);
-	if (ret)
+	if (ret) {
+		pr_err("%s: Failed to register LED device: %d\n",
+		       mmc_hostname(mmc), ret);
 		goto reset;
+	}
 #endif
 
 	mmiowb();
@@ -3081,8 +3115,6 @@ void sdhci_remove_host(struct sdhci_host *host, int dead)
 	free_irq(host->irq, host);
 
 	del_timer_sync(&host->timer);
-	if (host->version >= SDHCI_SPEC_300)
-		del_timer_sync(&host->tuning_timer);
 
 	tasklet_kill(&host->card_tasklet);
 	tasklet_kill(&host->finish_tasklet);
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index f761f23d2a2..97653ea8942 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -205,6 +205,7 @@
 #define SDHCI_CAPABILITIES_1	0x44
 
 #define SDHCI_MAX_CURRENT		0x48
+#define  SDHCI_MAX_CURRENT_LIMIT	0xFF
 #define  SDHCI_MAX_CURRENT_330_MASK	0x0000FF
 #define  SDHCI_MAX_CURRENT_330_SHIFT	0
 #define  SDHCI_MAX_CURRENT_300_MASK	0x00FF00
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 724b35e85a2..5d8142773fa 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -54,6 +54,8 @@
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/sdio.h>
 #include <linux/mmc/sh_mmcif.h>
+#include <linux/mmc/slot-gpio.h>
+#include <linux/mod_devicetable.h>
 #include <linux/pagemap.h>
 #include <linux/platform_device.h>
 #include <linux/pm_qos.h>
@@ -211,8 +213,6 @@ struct sh_mmcif_host {
 	struct mmc_host *mmc;
 	struct mmc_request *mrq;
 	struct platform_device *pd;
-	struct sh_dmae_slave dma_slave_tx;
-	struct sh_dmae_slave dma_slave_rx;
 	struct clk *hclk;
 	unsigned int clk;
 	int bus_width;
@@ -371,56 +371,69 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 		desc, cookie);
 }
 
-static bool sh_mmcif_filter(struct dma_chan *chan, void *arg)
-{
-	dev_dbg(chan->device->dev, "%s: slave data %p\n", __func__, arg);
-	chan->private = arg;
-	return true;
-}
-
 static void sh_mmcif_request_dma(struct sh_mmcif_host *host,
 				 struct sh_mmcif_plat_data *pdata)
 {
-	struct sh_dmae_slave *tx, *rx;
+	struct resource *res = platform_get_resource(host->pd, IORESOURCE_MEM, 0);
+	struct dma_slave_config cfg;
+	dma_cap_mask_t mask;
+	int ret;
+
 	host->dma_active = false;
 
+	if (!pdata)
+		return;
+
+	if (pdata->slave_id_tx <= 0 || pdata->slave_id_rx <= 0)
+		return;
+
 	/* We can only either use DMA for both Tx and Rx or not use it at all */
-	if (pdata->dma) {
-		dev_warn(&host->pd->dev,
-			 "Update your platform to use embedded DMA slave IDs\n");
-		tx = &pdata->dma->chan_priv_tx;
-		rx = &pdata->dma->chan_priv_rx;
-	} else {
-		tx = &host->dma_slave_tx;
-		tx->slave_id = pdata->slave_id_tx;
-		rx = &host->dma_slave_rx;
-		rx->slave_id = pdata->slave_id_rx;
-	}
-	if (tx->slave_id > 0 && rx->slave_id > 0) {
-		dma_cap_mask_t mask;
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
 
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
+	host->chan_tx = dma_request_channel(mask, shdma_chan_filter,
+					    (void *)pdata->slave_id_tx);
+	dev_dbg(&host->pd->dev, "%s: TX: got channel %p\n", __func__,
+		host->chan_tx);
 
-		host->chan_tx = dma_request_channel(mask, sh_mmcif_filter, tx);
-		dev_dbg(&host->pd->dev, "%s: TX: got channel %p\n", __func__,
-			host->chan_tx);
+	if (!host->chan_tx)
+		return;
 
-		if (!host->chan_tx)
-			return;
+	cfg.slave_id = pdata->slave_id_tx;
+	cfg.direction = DMA_MEM_TO_DEV;
+	cfg.dst_addr = res->start + MMCIF_CE_DATA;
+	cfg.src_addr = 0;
+	ret = dmaengine_slave_config(host->chan_tx, &cfg);
+	if (ret < 0)
+		goto ecfgtx;
 
-		host->chan_rx = dma_request_channel(mask, sh_mmcif_filter, rx);
-		dev_dbg(&host->pd->dev, "%s: RX: got channel %p\n", __func__,
-			host->chan_rx);
+	host->chan_rx = dma_request_channel(mask, shdma_chan_filter,
+					    (void *)pdata->slave_id_rx);
+	dev_dbg(&host->pd->dev, "%s: RX: got channel %p\n", __func__,
+		host->chan_rx);
 
-		if (!host->chan_rx) {
-			dma_release_channel(host->chan_tx);
-			host->chan_tx = NULL;
-			return;
-		}
+	if (!host->chan_rx)
+		goto erqrx;
 
-		init_completion(&host->dma_complete);
-	}
+	cfg.slave_id = pdata->slave_id_rx;
+	cfg.direction = DMA_DEV_TO_MEM;
+	cfg.dst_addr = 0;
+	cfg.src_addr = res->start + MMCIF_CE_DATA;
+	ret = dmaengine_slave_config(host->chan_rx, &cfg);
+	if (ret < 0)
+		goto ecfgrx;
+
+	init_completion(&host->dma_complete);
+
+	return;
+
+ecfgrx:
+	dma_release_channel(host->chan_rx);
+	host->chan_rx = NULL;
+erqrx:
+ecfgtx:
+	dma_release_channel(host->chan_tx);
+	host->chan_tx = NULL;
 }
 
 static void sh_mmcif_release_dma(struct sh_mmcif_host *host)
@@ -444,13 +457,14 @@ static void sh_mmcif_release_dma(struct sh_mmcif_host *host)
 static void sh_mmcif_clock_control(struct sh_mmcif_host *host, unsigned int clk)
 {
 	struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
+	bool sup_pclk = p ? p->sup_pclk : false;
 
 	sh_mmcif_bitclr(host, MMCIF_CE_CLK_CTRL, CLK_ENABLE);
 	sh_mmcif_bitclr(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR);
 
 	if (!clk)
 		return;
-	if (p->sup_pclk && clk == host->clk)
+	if (sup_pclk && clk == host->clk)
 		sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_SUP_PCLK);
 	else
 		sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR &
@@ -892,21 +906,15 @@ static void sh_mmcif_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	switch (mrq->cmd->opcode) {
 	/* MMCIF does not support SD/SDIO command */
-	case SD_IO_SEND_OP_COND:
+	case MMC_SLEEP_AWAKE: /* = SD_IO_SEND_OP_COND (5) */
+	case MMC_SEND_EXT_CSD: /* = SD_SEND_IF_COND (8) */
+		if ((mrq->cmd->flags & MMC_CMD_MASK) != MMC_CMD_BCR)
+			break;
 	case MMC_APP_CMD:
 		host->state = STATE_IDLE;
 		mrq->cmd->error = -ETIMEDOUT;
 		mmc_request_done(mmc, mrq);
 		return;
-	case MMC_SEND_EXT_CSD: /* = SD_SEND_IF_COND (8) */
-		if (!mrq->data) {
-			/* send_if_cond cmd (not support) */
-			host->state = STATE_IDLE;
-			mrq->cmd->error = -ETIMEDOUT;
-			mmc_request_done(mmc, mrq);
-			return;
-		}
-		break;
 	default:
 		break;
 	}
@@ -916,10 +924,35 @@ static void sh_mmcif_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	sh_mmcif_start_cmd(host, mrq);
 }
 
+static int sh_mmcif_clk_update(struct sh_mmcif_host *host)
+{
+	int ret = clk_enable(host->hclk);
+
+	if (!ret) {
+		host->clk = clk_get_rate(host->hclk);
+		host->mmc->f_max = host->clk / 2;
+		host->mmc->f_min = host->clk / 512;
+	}
+
+	return ret;
+}
+
+static void sh_mmcif_set_power(struct sh_mmcif_host *host, struct mmc_ios *ios)
+{
+	struct sh_mmcif_plat_data *pd = host->pd->dev.platform_data;
+	struct mmc_host *mmc = host->mmc;
+
+	if (pd && pd->set_pwr)
+		pd->set_pwr(host->pd, ios->power_mode != MMC_POWER_OFF);
+	if (!IS_ERR(mmc->supply.vmmc))
+		/* Errors ignored... */
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc,
+				      ios->power_mode ? ios->vdd : 0);
+}
+
 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;
 	unsigned long flags;
 
 	spin_lock_irqsave(&host->lock, flags);
@@ -937,6 +970,7 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 			sh_mmcif_request_dma(host, host->pd->dev.platform_data);
 			host->card_present = true;
 		}
+		sh_mmcif_set_power(host, ios);
 	} else if (ios->power_mode == MMC_POWER_OFF || !ios->clock) {
 		/* clock stop */
 		sh_mmcif_clock_control(host, 0);
@@ -948,9 +982,10 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		}
 		if (host->power) {
 			pm_runtime_put(&host->pd->dev);
+			clk_disable(host->hclk);
 			host->power = false;
-			if (p->down_pwr && ios->power_mode == MMC_POWER_OFF)
-				p->down_pwr(host->pd);
+			if (ios->power_mode == MMC_POWER_OFF)
+				sh_mmcif_set_power(host, ios);
 		}
 		host->state = STATE_IDLE;
 		return;
@@ -958,8 +993,7 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	if (ios->clock) {
 		if (!host->power) {
-			if (p->set_pwr)
-				p->set_pwr(host->pd, ios->power_mode);
+			sh_mmcif_clk_update(host);
 			pm_runtime_get_sync(&host->pd->dev);
 			host->power = true;
 			sh_mmcif_sync_reset(host);
@@ -975,8 +1009,12 @@ static int sh_mmcif_get_cd(struct mmc_host *mmc)
 {
 	struct sh_mmcif_host *host = mmc_priv(mmc);
 	struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
+	int ret = mmc_gpio_get_cd(mmc);
+
+	if (ret >= 0)
+		return ret;
 
-	if (!p->get_cd)
+	if (!p || !p->get_cd)
 		return -ENOSYS;
 	else
 		return p->get_cd(host->pd);
@@ -1242,12 +1280,28 @@ static void mmcif_timeout_work(struct work_struct *work)
 	mmc_request_done(host->mmc, mrq);
 }
 
+static void sh_mmcif_init_ocr(struct sh_mmcif_host *host)
+{
+	struct sh_mmcif_plat_data *pd = host->pd->dev.platform_data;
+	struct mmc_host *mmc = host->mmc;
+
+	mmc_regulator_get_supply(mmc);
+
+	if (!pd)
+		return;
+
+	if (!mmc->ocr_avail)
+		mmc->ocr_avail = pd->ocr;
+	else if (pd->ocr)
+		dev_warn(mmc_dev(mmc), "Platform OCR mask is ignored\n");
+}
+
 static int __devinit sh_mmcif_probe(struct platform_device *pdev)
 {
 	int ret = 0, irq[2];
 	struct mmc_host *mmc;
 	struct sh_mmcif_host *host;
-	struct sh_mmcif_plat_data *pd;
+	struct sh_mmcif_plat_data *pd = pdev->dev.platform_data;
 	struct resource *res;
 	void __iomem *reg;
 	char clk_name[8];
@@ -1268,42 +1322,26 @@ static int __devinit sh_mmcif_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "ioremap error.\n");
 		return -ENOMEM;
 	}
-	pd = pdev->dev.platform_data;
-	if (!pd) {
-		dev_err(&pdev->dev, "sh_mmcif plat data error.\n");
-		ret = -ENXIO;
-		goto clean_up;
-	}
+
 	mmc = mmc_alloc_host(sizeof(struct sh_mmcif_host), &pdev->dev);
 	if (!mmc) {
 		ret = -ENOMEM;
-		goto clean_up;
+		goto ealloch;
 	}
 	host		= mmc_priv(mmc);
 	host->mmc	= mmc;
 	host->addr	= reg;
 	host->timeout	= 1000;
 
-	snprintf(clk_name, sizeof(clk_name), "mmc%d", pdev->id);
-	host->hclk = clk_get(&pdev->dev, clk_name);
-	if (IS_ERR(host->hclk)) {
-		dev_err(&pdev->dev, "cannot get clock \"%s\"\n", clk_name);
-		ret = PTR_ERR(host->hclk);
-		goto clean_up1;
-	}
-	clk_enable(host->hclk);
-	host->clk = clk_get_rate(host->hclk);
 	host->pd = pdev;
 
 	spin_lock_init(&host->lock);
 
 	mmc->ops = &sh_mmcif_ops;
-	mmc->f_max = host->clk / 2;
-	mmc->f_min = host->clk / 512;
-	if (pd->ocr)
-		mmc->ocr_avail = pd->ocr;
+	sh_mmcif_init_ocr(host);
+
 	mmc->caps = MMC_CAP_MMC_HIGHSPEED;
-	if (pd->caps)
+	if (pd && pd->caps)
 		mmc->caps |= pd->caps;
 	mmc->max_segs = 32;
 	mmc->max_blk_size = 512;
@@ -1311,34 +1349,52 @@ static int __devinit sh_mmcif_probe(struct platform_device *pdev)
 	mmc->max_blk_count = mmc->max_req_size / mmc->max_blk_size;
 	mmc->max_seg_size = mmc->max_req_size;
 
-	sh_mmcif_sync_reset(host);
 	platform_set_drvdata(pdev, host);
 
 	pm_runtime_enable(&pdev->dev);
 	host->power = false;
 
+	snprintf(clk_name, sizeof(clk_name), "mmc%d", pdev->id);
+	host->hclk = clk_get(&pdev->dev, clk_name);
+	if (IS_ERR(host->hclk)) {
+		ret = PTR_ERR(host->hclk);
+		dev_err(&pdev->dev, "cannot get clock \"%s\": %d\n", clk_name, ret);
+		goto eclkget;
+	}
+	ret = sh_mmcif_clk_update(host);
+	if (ret < 0)
+		goto eclkupdate;
+
 	ret = pm_runtime_resume(&pdev->dev);
 	if (ret < 0)
-		goto clean_up2;
+		goto eresume;
 
 	INIT_DELAYED_WORK(&host->timeout_work, mmcif_timeout_work);
 
+	sh_mmcif_sync_reset(host);
 	sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, MASK_ALL);
 
 	ret = request_threaded_irq(irq[0], sh_mmcif_intr, sh_mmcif_irqt, 0, "sh_mmc:error", host);
 	if (ret) {
 		dev_err(&pdev->dev, "request_irq error (sh_mmc:error)\n");
-		goto clean_up3;
+		goto ereqirq0;
 	}
 	ret = request_threaded_irq(irq[1], sh_mmcif_intr, sh_mmcif_irqt, 0, "sh_mmc:int", host);
 	if (ret) {
 		dev_err(&pdev->dev, "request_irq error (sh_mmc:int)\n");
-		goto clean_up4;
+		goto ereqirq1;
+	}
+
+	if (pd && pd->use_cd_gpio) {
+		ret = mmc_gpio_request_cd(mmc, pd->cd_gpio);
+		if (ret < 0)
+			goto erqcd;
 	}
 
+	clk_disable(host->hclk);
 	ret = mmc_add_host(mmc);
 	if (ret < 0)
-		goto clean_up5;
+		goto emmcaddh;
 
 	dev_pm_qos_expose_latency_limit(&pdev->dev, 100);
 
@@ -1347,33 +1403,42 @@ static int __devinit sh_mmcif_probe(struct platform_device *pdev)
 		sh_mmcif_readl(host->addr, MMCIF_CE_VERSION) & 0x0000ffff);
 	return ret;
 
-clean_up5:
+emmcaddh:
+	if (pd && pd->use_cd_gpio)
+		mmc_gpio_free_cd(mmc);
+erqcd:
 	free_irq(irq[1], host);
-clean_up4:
+ereqirq1:
 	free_irq(irq[0], host);
-clean_up3:
+ereqirq0:
 	pm_runtime_suspend(&pdev->dev);
-clean_up2:
-	pm_runtime_disable(&pdev->dev);
+eresume:
 	clk_disable(host->hclk);
-clean_up1:
+eclkupdate:
+	clk_put(host->hclk);
+eclkget:
+	pm_runtime_disable(&pdev->dev);
 	mmc_free_host(mmc);
-clean_up:
-	if (reg)
-		iounmap(reg);
+ealloch:
+	iounmap(reg);
 	return ret;
 }
 
 static int __devexit sh_mmcif_remove(struct platform_device *pdev)
 {
 	struct sh_mmcif_host *host = platform_get_drvdata(pdev);
+	struct sh_mmcif_plat_data *pd = pdev->dev.platform_data;
 	int irq[2];
 
 	host->dying = true;
+	clk_enable(host->hclk);
 	pm_runtime_get_sync(&pdev->dev);
 
 	dev_pm_qos_hide_latency_limit(&pdev->dev);
 
+	if (pd && pd->use_cd_gpio)
+		mmc_gpio_free_cd(host->mmc);
+
 	mmc_remove_host(host->mmc);
 	sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, MASK_ALL);
 
@@ -1395,9 +1460,9 @@ static int __devexit sh_mmcif_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	clk_disable(host->hclk);
 	mmc_free_host(host->mmc);
 	pm_runtime_put_sync(&pdev->dev);
+	clk_disable(host->hclk);
 	pm_runtime_disable(&pdev->dev);
 
 	return 0;
@@ -1406,24 +1471,18 @@ static int __devexit sh_mmcif_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int sh_mmcif_suspend(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct sh_mmcif_host *host = platform_get_drvdata(pdev);
+	struct sh_mmcif_host *host = dev_get_drvdata(dev);
 	int ret = mmc_suspend_host(host->mmc);
 
-	if (!ret) {
+	if (!ret)
 		sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, MASK_ALL);
-		clk_disable(host->hclk);
-	}
 
 	return ret;
 }
 
 static int sh_mmcif_resume(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct sh_mmcif_host *host = platform_get_drvdata(pdev);
-
-	clk_enable(host->hclk);
+	struct sh_mmcif_host *host = dev_get_drvdata(dev);
 
 	return mmc_resume_host(host->mmc);
 }
@@ -1432,6 +1491,12 @@ static int sh_mmcif_resume(struct device *dev)
 #define sh_mmcif_resume		NULL
 #endif	/* CONFIG_PM */
 
+static const struct of_device_id mmcif_of_match[] = {
+	{ .compatible = "renesas,sh-mmcif" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mmcif_of_match);
+
 static const struct dev_pm_ops sh_mmcif_dev_pm_ops = {
 	.suspend = sh_mmcif_suspend,
 	.resume = sh_mmcif_resume,
@@ -1443,6 +1508,8 @@ static struct platform_driver sh_mmcif_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.pm	= &sh_mmcif_dev_pm_ops,
+		.owner	= THIS_MODULE,
+		.of_match_table = mmcif_of_match,
 	},
 };
 
diff --git a/drivers/mmc/host/sh_mobile_sdhi.c b/drivers/mmc/host/sh_mobile_sdhi.c
index 934b68e9efc..0bdc146178d 100644
--- a/drivers/mmc/host/sh_mobile_sdhi.c
+++ b/drivers/mmc/host/sh_mobile_sdhi.c
@@ -21,6 +21,7 @@
 #include <linux/kernel.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
+#include <linux/mod_devicetable.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/mmc/host.h>
@@ -39,22 +40,39 @@ struct sh_mobile_sdhi {
 	struct tmio_mmc_dma dma_priv;
 };
 
+static int sh_mobile_sdhi_clk_enable(struct platform_device *pdev, unsigned int *f)
+{
+	struct mmc_host *mmc = dev_get_drvdata(&pdev->dev);
+	struct tmio_mmc_host *host = mmc_priv(mmc);
+	struct sh_mobile_sdhi *priv = container_of(host->pdata, struct sh_mobile_sdhi, mmc_data);
+	int ret = clk_enable(priv->clk);
+	if (ret < 0)
+		return ret;
+
+	*f = clk_get_rate(priv->clk);
+	return 0;
+}
+
+static void sh_mobile_sdhi_clk_disable(struct platform_device *pdev)
+{
+	struct mmc_host *mmc = dev_get_drvdata(&pdev->dev);
+	struct tmio_mmc_host *host = mmc_priv(mmc);
+	struct sh_mobile_sdhi *priv = container_of(host->pdata, struct sh_mobile_sdhi, mmc_data);
+	clk_disable(priv->clk);
+}
+
 static void sh_mobile_sdhi_set_pwr(struct platform_device *pdev, int state)
 {
 	struct sh_mobile_sdhi_info *p = pdev->dev.platform_data;
 
-	if (p && p->set_pwr)
-		p->set_pwr(pdev, state);
+	p->set_pwr(pdev, state);
 }
 
 static int sh_mobile_sdhi_get_cd(struct platform_device *pdev)
 {
 	struct sh_mobile_sdhi_info *p = pdev->dev.platform_data;
 
-	if (p && p->get_cd)
-		return p->get_cd(pdev);
-	else
-		return -ENOSYS;
+	return p->get_cd(pdev);
 }
 
 static int sh_mobile_sdhi_wait_idle(struct tmio_mmc_host *host)
@@ -116,12 +134,14 @@ static int __devinit sh_mobile_sdhi_probe(struct platform_device *pdev)
 	}
 
 	mmc_data = &priv->mmc_data;
-	p->pdata = mmc_data;
 
-	if (p->init) {
-		ret = p->init(pdev, &sdhi_ops);
-		if (ret)
-			goto einit;
+	if (p) {
+		p->pdata = mmc_data;
+		if (p->init) {
+			ret = p->init(pdev, &sdhi_ops);
+			if (ret)
+				goto einit;
+		}
 	}
 
 	snprintf(clk_name, sizeof(clk_name), "sdhi%d", pdev->id);
@@ -132,9 +152,8 @@ static int __devinit sh_mobile_sdhi_probe(struct platform_device *pdev)
 		goto eclkget;
 	}
 
-	mmc_data->hclk = clk_get_rate(priv->clk);
-	mmc_data->set_pwr = sh_mobile_sdhi_set_pwr;
-	mmc_data->get_cd = sh_mobile_sdhi_get_cd;
+	mmc_data->clk_enable = sh_mobile_sdhi_clk_enable;
+	mmc_data->clk_disable = sh_mobile_sdhi_clk_disable;
 	mmc_data->capabilities = MMC_CAP_MMC_HIGHSPEED;
 	if (p) {
 		mmc_data->flags = p->tmio_flags;
@@ -142,13 +161,18 @@ static int __devinit sh_mobile_sdhi_probe(struct platform_device *pdev)
 			mmc_data->write16_hook = sh_mobile_sdhi_write16_hook;
 		mmc_data->ocr_mask = p->tmio_ocr_mask;
 		mmc_data->capabilities |= p->tmio_caps;
+		mmc_data->capabilities2 |= p->tmio_caps2;
 		mmc_data->cd_gpio = p->cd_gpio;
+		if (p->set_pwr)
+			mmc_data->set_pwr = sh_mobile_sdhi_set_pwr;
+		if (p->get_cd)
+			mmc_data->get_cd = sh_mobile_sdhi_get_cd;
 
 		if (p->dma_slave_tx > 0 && p->dma_slave_rx > 0) {
-			priv->param_tx.slave_id = p->dma_slave_tx;
-			priv->param_rx.slave_id = p->dma_slave_rx;
-			priv->dma_priv.chan_priv_tx = &priv->param_tx;
-			priv->dma_priv.chan_priv_rx = &priv->param_rx;
+			priv->param_tx.shdma_slave.slave_id = p->dma_slave_tx;
+			priv->param_rx.shdma_slave.slave_id = p->dma_slave_rx;
+			priv->dma_priv.chan_priv_tx = &priv->param_tx.shdma_slave;
+			priv->dma_priv.chan_priv_rx = &priv->param_rx.shdma_slave;
 			priv->dma_priv.alignment_shift = 1; /* 2-byte alignment */
 			mmc_data->dma = &priv->dma_priv;
 		}
@@ -248,7 +272,7 @@ eirq_card_detect:
 eprobe:
 	clk_put(priv->clk);
 eclkget:
-	if (p->cleanup)
+	if (p && p->cleanup)
 		p->cleanup(pdev);
 einit:
 	kfree(priv);
@@ -263,7 +287,8 @@ static int sh_mobile_sdhi_remove(struct platform_device *pdev)
 	struct sh_mobile_sdhi_info *p = pdev->dev.platform_data;
 	int i = 0, irq;
 
-	p->pdata = NULL;
+	if (p)
+		p->pdata = NULL;
 
 	tmio_mmc_host_remove(host);
 
@@ -276,7 +301,7 @@ static int sh_mobile_sdhi_remove(struct platform_device *pdev)
 
 	clk_put(priv->clk);
 
-	if (p->cleanup)
+	if (p && p->cleanup)
 		p->cleanup(pdev);
 
 	kfree(priv);
@@ -291,11 +316,18 @@ static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
 	.runtime_resume = tmio_mmc_host_runtime_resume,
 };
 
+static const struct of_device_id sh_mobile_sdhi_of_match[] = {
+	{ .compatible = "renesas,shmobile-sdhi" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sh_mobile_sdhi_of_match);
+
 static struct platform_driver sh_mobile_sdhi_driver = {
 	.driver		= {
 		.name	= "sh_mobile_sdhi",
 		.owner	= THIS_MODULE,
 		.pm	= &tmio_mmc_dev_pm_ops,
+		.of_match_table = sh_mobile_sdhi_of_match,
 	},
 	.probe		= sh_mobile_sdhi_probe,
 	.remove		= __devexit_p(sh_mobile_sdhi_remove),
diff --git a/drivers/mmc/host/tmio_mmc_pio.c b/drivers/mmc/host/tmio_mmc_pio.c
index 9a7996ade58..0d8a9bbe30b 100644
--- a/drivers/mmc/host/tmio_mmc_pio.c
+++ b/drivers/mmc/host/tmio_mmc_pio.c
@@ -34,8 +34,9 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/mfd/tmio.h>
-#include <linux/mmc/cd-gpio.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/slot-gpio.h>
 #include <linux/mmc/tmio.h>
 #include <linux/module.h>
 #include <linux/pagemap.h>
@@ -305,8 +306,8 @@ static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command
 	int c = cmd->opcode;
 	u32 irq_mask = TMIO_MASK_CMD;
 
-	/* Command 12 is handled by hardware */
-	if (cmd->opcode == 12 && !cmd->arg) {
+	/* CMD12 is handled by hardware */
+	if (cmd->opcode == MMC_STOP_TRANSMISSION && !cmd->arg) {
 		sd_ctrl_write16(host, CTL_STOP_INTERNAL_ACTION, 0x001);
 		return 0;
 	}
@@ -449,7 +450,7 @@ void tmio_mmc_do_data_irq(struct tmio_mmc_host *host)
 	}
 
 	if (stop) {
-		if (stop->opcode == 12 && !stop->arg)
+		if (stop->opcode == MMC_STOP_TRANSMISSION && !stop->arg)
 			sd_ctrl_write16(host, CTL_STOP_INTERNAL_ACTION, 0x000);
 		else
 			BUG();
@@ -751,6 +752,34 @@ fail:
 	mmc_request_done(mmc, mrq);
 }
 
+static int tmio_mmc_clk_update(struct mmc_host *mmc)
+{
+	struct tmio_mmc_host *host = mmc_priv(mmc);
+	struct tmio_mmc_data *pdata = host->pdata;
+	int ret;
+
+	if (!pdata->clk_enable)
+		return -ENOTSUPP;
+
+	ret = pdata->clk_enable(host->pdev, &mmc->f_max);
+	if (!ret)
+		mmc->f_min = mmc->f_max / 512;
+
+	return ret;
+}
+
+static void tmio_mmc_set_power(struct tmio_mmc_host *host, struct mmc_ios *ios)
+{
+	struct mmc_host *mmc = host->mmc;
+
+	if (host->set_pwr)
+		host->set_pwr(host->pdev, ios->power_mode != MMC_POWER_OFF);
+	if (!IS_ERR(mmc->supply.vmmc))
+		/* Errors ignored... */
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc,
+				      ios->power_mode ? ios->vdd : 0);
+}
+
 /* Set MMC clock / power.
  * Note: This controller uses a simple divider scheme therefore it cannot
  * run a MMC card at full speed (20MHz). The max clock is 24MHz on SD, but as
@@ -797,32 +826,37 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	 */
 	if (ios->power_mode == MMC_POWER_ON && ios->clock) {
 		if (!host->power) {
+			tmio_mmc_clk_update(mmc);
 			pm_runtime_get_sync(dev);
 			host->power = true;
 		}
 		tmio_mmc_set_clock(host, ios->clock);
 		/* power up SD bus */
-		if (host->set_pwr)
-			host->set_pwr(host->pdev, 1);
+		tmio_mmc_set_power(host, ios);
 		/* start bus clock */
 		tmio_mmc_clk_start(host);
 	} else if (ios->power_mode != MMC_POWER_UP) {
-		if (host->set_pwr && ios->power_mode == MMC_POWER_OFF)
-			host->set_pwr(host->pdev, 0);
+		if (ios->power_mode == MMC_POWER_OFF)
+			tmio_mmc_set_power(host, ios);
 		if (host->power) {
+			struct tmio_mmc_data *pdata = host->pdata;
+			tmio_mmc_clk_stop(host);
 			host->power = false;
 			pm_runtime_put(dev);
+			if (pdata->clk_disable)
+				pdata->clk_disable(host->pdev);
 		}
-		tmio_mmc_clk_stop(host);
 	}
 
-	switch (ios->bus_width) {
-	case MMC_BUS_WIDTH_1:
-		sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x80e0);
-	break;
-	case MMC_BUS_WIDTH_4:
-		sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x00e0);
-	break;
+	if (host->power) {
+		switch (ios->bus_width) {
+		case MMC_BUS_WIDTH_1:
+			sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x80e0);
+		break;
+		case MMC_BUS_WIDTH_4:
+			sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x00e0);
+		break;
+		}
 	}
 
 	/* Let things settle. delay taken from winCE driver */
@@ -841,6 +875,9 @@ static int tmio_mmc_get_ro(struct mmc_host *mmc)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 	struct tmio_mmc_data *pdata = host->pdata;
+	int ret = mmc_gpio_get_ro(mmc);
+	if (ret >= 0)
+		return ret;
 
 	return !((pdata->flags & TMIO_MMC_WRPROTECT_DISABLE) ||
 		 (sd_ctrl_read32(host, CTL_STATUS) & TMIO_STAT_WRPROTECT));
@@ -850,6 +887,9 @@ static int tmio_mmc_get_cd(struct mmc_host *mmc)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 	struct tmio_mmc_data *pdata = host->pdata;
+	int ret = mmc_gpio_get_cd(mmc);
+	if (ret >= 0)
+		return ret;
 
 	if (!pdata->get_cd)
 		return -ENOSYS;
@@ -865,6 +905,19 @@ static const struct mmc_host_ops tmio_mmc_ops = {
 	.enable_sdio_irq = tmio_mmc_enable_sdio_irq,
 };
 
+static void tmio_mmc_init_ocr(struct tmio_mmc_host *host)
+{
+	struct tmio_mmc_data *pdata = host->pdata;
+	struct mmc_host *mmc = host->mmc;
+
+	mmc_regulator_get_supply(mmc);
+
+	if (!mmc->ocr_avail)
+		mmc->ocr_avail = pdata->ocr_mask ? : MMC_VDD_32_33 | MMC_VDD_33_34;
+	else if (pdata->ocr_mask)
+		dev_warn(mmc_dev(mmc), "Platform OCR mask is ignored\n");
+}
+
 int __devinit tmio_mmc_host_probe(struct tmio_mmc_host **host,
 				  struct platform_device *pdev,
 				  struct tmio_mmc_data *pdata)
@@ -904,18 +957,14 @@ int __devinit tmio_mmc_host_probe(struct tmio_mmc_host **host,
 
 	mmc->ops = &tmio_mmc_ops;
 	mmc->caps = MMC_CAP_4_BIT_DATA | pdata->capabilities;
-	mmc->f_max = pdata->hclk;
-	mmc->f_min = mmc->f_max / 512;
+	mmc->caps2 = pdata->capabilities2;
 	mmc->max_segs = 32;
 	mmc->max_blk_size = 512;
 	mmc->max_blk_count = (PAGE_CACHE_SIZE / mmc->max_blk_size) *
 		mmc->max_segs;
 	mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
 	mmc->max_seg_size = mmc->max_req_size;
-	if (pdata->ocr_mask)
-		mmc->ocr_avail = pdata->ocr_mask;
-	else
-		mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
+	tmio_mmc_init_ocr(_host);
 
 	_host->native_hotplug = !(pdata->flags & TMIO_MMC_USE_GPIO_CD ||
 				  mmc->caps & MMC_CAP_NEEDS_POLL ||
@@ -927,6 +976,11 @@ int __devinit tmio_mmc_host_probe(struct tmio_mmc_host **host,
 	if (ret < 0)
 		goto pm_disable;
 
+	if (tmio_mmc_clk_update(mmc) < 0) {
+		mmc->f_max = pdata->hclk;
+		mmc->f_min = mmc->f_max / 512;
+	}
+
 	/*
 	 * There are 4 different scenarios for the card detection:
 	 *  1) an external gpio irq handles the cd (best for power savings)
@@ -937,7 +991,6 @@ int __devinit tmio_mmc_host_probe(struct tmio_mmc_host **host,
 	 *  While we increment the runtime PM counter for all scenarios when
 	 *  the mmc core activates us by calling an appropriate set_ios(), we
 	 *  must additionally ensure that in case 2) the tmio mmc hardware stays
-	 *  additionally ensure that in case 2) the tmio mmc hardware stays
 	 *  powered on during runtime for the card detection to work.
 	 */
 	if (_host->native_hotplug)
@@ -948,6 +1001,17 @@ int __devinit tmio_mmc_host_probe(struct tmio_mmc_host **host,
 
 	_host->sdcard_irq_mask = sd_ctrl_read32(_host, CTL_IRQ_MASK);
 	tmio_mmc_disable_mmc_irqs(_host, TMIO_MASK_ALL);
+
+	/* Unmask the IRQs we want to know about */
+	if (!_host->chan_rx)
+		irq_mask |= TMIO_MASK_READOP;
+	if (!_host->chan_tx)
+		irq_mask |= TMIO_MASK_WRITEOP;
+	if (!_host->native_hotplug)
+		irq_mask &= ~(TMIO_STAT_CARD_REMOVE | TMIO_STAT_CARD_INSERT);
+
+	_host->sdcard_irq_mask &= ~irq_mask;
+
 	if (pdata->flags & TMIO_MMC_SDIO_IRQ)
 		tmio_mmc_enable_sdio_irq(mmc, 0);
 
@@ -961,22 +1025,18 @@ int __devinit tmio_mmc_host_probe(struct tmio_mmc_host **host,
 	/* See if we also get DMA */
 	tmio_mmc_request_dma(_host, pdata);
 
-	mmc_add_host(mmc);
+	ret = mmc_add_host(mmc);
+	if (pdata->clk_disable)
+		pdata->clk_disable(pdev);
+	if (ret < 0) {
+		tmio_mmc_host_remove(_host);
+		return ret;
+	}
 
 	dev_pm_qos_expose_latency_limit(&pdev->dev, 100);
 
-	/* Unmask the IRQs we want to know about */
-	if (!_host->chan_rx)
-		irq_mask |= TMIO_MASK_READOP;
-	if (!_host->chan_tx)
-		irq_mask |= TMIO_MASK_WRITEOP;
-	if (!_host->native_hotplug)
-		irq_mask &= ~(TMIO_STAT_CARD_REMOVE | TMIO_STAT_CARD_INSERT);
-
-	tmio_mmc_enable_mmc_irqs(_host, irq_mask);
-
 	if (pdata->flags & TMIO_MMC_USE_GPIO_CD) {
-		ret = mmc_cd_gpio_request(mmc, pdata->cd_gpio);
+		ret = mmc_gpio_request_cd(mmc, pdata->cd_gpio);
 		if (ret < 0) {
 			tmio_mmc_host_remove(_host);
 			return ret;
@@ -1008,7 +1068,7 @@ void tmio_mmc_host_remove(struct tmio_mmc_host *host)
 		 * This means we can miss a card-eject, but this is anyway
 		 * possible, because of delayed processing of hotplug events.
 		 */
-		mmc_cd_gpio_free(mmc);
+		mmc_gpio_free_cd(mmc);
 
 	if (!host->native_hotplug)
 		pm_runtime_get_sync(&pdev->dev);
diff --git a/drivers/mtd/mtdsuper.c b/drivers/mtd/mtdsuper.c
index a90bfe79916..334da5f583c 100644
--- a/drivers/mtd/mtdsuper.c
+++ b/drivers/mtd/mtdsuper.c
@@ -63,7 +63,7 @@ static struct dentry *mount_mtd_aux(struct file_system_type *fs_type, int flags,
 	struct super_block *sb;
 	int ret;
 
-	sb = sget(fs_type, get_sb_mtd_compare, get_sb_mtd_set, mtd);
+	sb = sget(fs_type, get_sb_mtd_compare, get_sb_mtd_set, flags, mtd);
 	if (IS_ERR(sb))
 		goto out_error;
 
@@ -74,8 +74,6 @@ static struct dentry *mount_mtd_aux(struct file_system_type *fs_type, int flags,
 	pr_debug("MTDSB: New superblock for device %d (\"%s\")\n",
 	      mtd->index, mtd->name);
 
-	sb->s_flags = flags;
-
 	ret = fill_super(sb, data, flags & MS_SILENT ? 1 : 0);
 	if (ret < 0) {
 		deactivate_locked_super(sb);
diff --git a/drivers/mtd/nand/jz4740_nand.c b/drivers/mtd/nand/jz4740_nand.c
index a6fa884ae49..100b6775e17 100644
--- a/drivers/mtd/nand/jz4740_nand.c
+++ b/drivers/mtd/nand/jz4740_nand.c
@@ -52,9 +52,10 @@
 
 #define JZ_NAND_CTRL_ENABLE_CHIP(x) BIT((x) << 1)
 #define JZ_NAND_CTRL_ASSERT_CHIP(x) BIT(((x) << 1) + 1)
+#define JZ_NAND_CTRL_ASSERT_CHIP_MASK 0xaa
 
-#define JZ_NAND_MEM_ADDR_OFFSET 0x10000
 #define JZ_NAND_MEM_CMD_OFFSET 0x08000
+#define JZ_NAND_MEM_ADDR_OFFSET 0x10000
 
 struct jz_nand {
 	struct mtd_info mtd;
@@ -62,8 +63,11 @@ struct jz_nand {
 	void __iomem *base;
 	struct resource *mem;
 
-	void __iomem *bank_base;
-	struct resource *bank_mem;
+	unsigned char banks[JZ_NAND_NUM_BANKS];
+	void __iomem *bank_base[JZ_NAND_NUM_BANKS];
+	struct resource *bank_mem[JZ_NAND_NUM_BANKS];
+
+	int selected_bank;
 
 	struct jz_nand_platform_data *pdata;
 	bool is_reading;
@@ -74,26 +78,50 @@ static inline struct jz_nand *mtd_to_jz_nand(struct mtd_info *mtd)
 	return container_of(mtd, struct jz_nand, mtd);
 }
 
+static void jz_nand_select_chip(struct mtd_info *mtd, int chipnr)
+{
+	struct jz_nand *nand = mtd_to_jz_nand(mtd);
+	struct nand_chip *chip = mtd->priv;
+	uint32_t ctrl;
+	int banknr;
+
+	ctrl = readl(nand->base + JZ_REG_NAND_CTRL);
+	ctrl &= ~JZ_NAND_CTRL_ASSERT_CHIP_MASK;
+
+	if (chipnr == -1) {
+		banknr = -1;
+	} else {
+		banknr = nand->banks[chipnr] - 1;
+		chip->IO_ADDR_R = nand->bank_base[banknr];
+		chip->IO_ADDR_W = nand->bank_base[banknr];
+	}
+	writel(ctrl, nand->base + JZ_REG_NAND_CTRL);
+
+	nand->selected_bank = banknr;
+}
+
 static void jz_nand_cmd_ctrl(struct mtd_info *mtd, int dat, unsigned int ctrl)
 {
 	struct jz_nand *nand = mtd_to_jz_nand(mtd);
 	struct nand_chip *chip = mtd->priv;
 	uint32_t reg;
+	void __iomem *bank_base = nand->bank_base[nand->selected_bank];
+
+	BUG_ON(nand->selected_bank < 0);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		BUG_ON((ctrl & NAND_ALE) && (ctrl & NAND_CLE));
 		if (ctrl & NAND_ALE)
-			chip->IO_ADDR_W = nand->bank_base + JZ_NAND_MEM_ADDR_OFFSET;
+			bank_base += JZ_NAND_MEM_ADDR_OFFSET;
 		else if (ctrl & NAND_CLE)
-			chip->IO_ADDR_W = nand->bank_base + JZ_NAND_MEM_CMD_OFFSET;
-		else
-			chip->IO_ADDR_W = nand->bank_base;
+			bank_base += JZ_NAND_MEM_CMD_OFFSET;
+		chip->IO_ADDR_W = bank_base;
 
 		reg = readl(nand->base + JZ_REG_NAND_CTRL);
 		if (ctrl & NAND_NCE)
-			reg |= JZ_NAND_CTRL_ASSERT_CHIP(0);
+			reg |= JZ_NAND_CTRL_ASSERT_CHIP(nand->selected_bank);
 		else
-			reg &= ~JZ_NAND_CTRL_ASSERT_CHIP(0);
+			reg &= ~JZ_NAND_CTRL_ASSERT_CHIP(nand->selected_bank);
 		writel(reg, nand->base + JZ_REG_NAND_CTRL);
 	}
 	if (dat != NAND_CMD_NONE)
@@ -252,7 +280,7 @@ static int jz_nand_correct_ecc_rs(struct mtd_info *mtd, uint8_t *dat,
 }
 
 static int jz_nand_ioremap_resource(struct platform_device *pdev,
-	const char *name, struct resource **res, void __iomem **base)
+	const char *name, struct resource **res, void *__iomem *base)
 {
 	int ret;
 
@@ -288,6 +316,90 @@ err:
 	return ret;
 }
 
+static inline void jz_nand_iounmap_resource(struct resource *res, void __iomem *base)
+{
+	iounmap(base);
+	release_mem_region(res->start, resource_size(res));
+}
+
+static int __devinit jz_nand_detect_bank(struct platform_device *pdev, struct jz_nand *nand, unsigned char bank, size_t chipnr, uint8_t *nand_maf_id, uint8_t *nand_dev_id) {
+	int ret;
+	int gpio;
+	char gpio_name[9];
+	char res_name[6];
+	uint32_t ctrl;
+	struct mtd_info *mtd = &nand->mtd;
+	struct nand_chip *chip = &nand->chip;
+
+	/* Request GPIO port. */
+	gpio = JZ_GPIO_MEM_CS0 + bank - 1;
+	sprintf(gpio_name, "NAND CS%d", bank);
+	ret = gpio_request(gpio, gpio_name);
+	if (ret) {
+		dev_warn(&pdev->dev,
+			"Failed to request %s gpio %d: %d\n",
+			gpio_name, gpio, ret);
+		goto notfound_gpio;
+	}
+
+	/* Request I/O resource. */
+	sprintf(res_name, "bank%d", bank);
+	ret = jz_nand_ioremap_resource(pdev, res_name,
+					&nand->bank_mem[bank - 1],
+					&nand->bank_base[bank - 1]);
+	if (ret)
+		goto notfound_resource;
+
+	/* Enable chip in bank. */
+	jz_gpio_set_function(gpio, JZ_GPIO_FUNC_MEM_CS0);
+	ctrl = readl(nand->base + JZ_REG_NAND_CTRL);
+	ctrl |= JZ_NAND_CTRL_ENABLE_CHIP(bank - 1);
+	writel(ctrl, nand->base + JZ_REG_NAND_CTRL);
+
+	if (chipnr == 0) {
+		/* Detect first chip. */
+		ret = nand_scan_ident(mtd, 1, NULL);
+		if (ret)
+			goto notfound_id;
+
+		/* Retrieve the IDs from the first chip. */
+		chip->select_chip(mtd, 0);
+		chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+		chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
+		*nand_maf_id = chip->read_byte(mtd);
+		*nand_dev_id = chip->read_byte(mtd);
+	} else {
+		/* Detect additional chip. */
+		chip->select_chip(mtd, chipnr);
+		chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+		chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
+		if (*nand_maf_id != chip->read_byte(mtd)
+		 || *nand_dev_id != chip->read_byte(mtd)) {
+			ret = -ENODEV;
+			goto notfound_id;
+		}
+
+		/* Update size of the MTD. */
+		chip->numchips++;
+		mtd->size += chip->chipsize;
+	}
+
+	dev_info(&pdev->dev, "Found chip %i on bank %i\n", chipnr, bank);
+	return 0;
+
+notfound_id:
+	dev_info(&pdev->dev, "No chip found on bank %i\n", bank);
+	ctrl &= ~(JZ_NAND_CTRL_ENABLE_CHIP(bank - 1));
+	writel(ctrl, nand->base + JZ_REG_NAND_CTRL);
+	jz_gpio_set_function(gpio, JZ_GPIO_FUNC_NONE);
+	jz_nand_iounmap_resource(nand->bank_mem[bank - 1],
+				 nand->bank_base[bank - 1]);
+notfound_resource:
+	gpio_free(gpio);
+notfound_gpio:
+	return ret;
+}
+
 static int __devinit jz_nand_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -295,6 +407,8 @@ static int __devinit jz_nand_probe(struct platform_device *pdev)
 	struct nand_chip *chip;
 	struct mtd_info *mtd;
 	struct jz_nand_platform_data *pdata = pdev->dev.platform_data;
+	size_t chipnr, bank_idx;
+	uint8_t nand_maf_id = 0, nand_dev_id = 0;
 
 	nand = kzalloc(sizeof(*nand), GFP_KERNEL);
 	if (!nand) {
@@ -305,10 +419,6 @@ static int __devinit jz_nand_probe(struct platform_device *pdev)
 	ret = jz_nand_ioremap_resource(pdev, "mmio", &nand->mem, &nand->base);
 	if (ret)
 		goto err_free;
-	ret = jz_nand_ioremap_resource(pdev, "bank", &nand->bank_mem,
-			&nand->bank_base);
-	if (ret)
-		goto err_iounmap_mmio;
 
 	if (pdata && gpio_is_valid(pdata->busy_gpio)) {
 		ret = gpio_request(pdata->busy_gpio, "NAND busy pin");
@@ -316,7 +426,7 @@ static int __devinit jz_nand_probe(struct platform_device *pdev)
 			dev_err(&pdev->dev,
 				"Failed to request busy gpio %d: %d\n",
 				pdata->busy_gpio, ret);
-			goto err_iounmap_mem;
+			goto err_iounmap_mmio;
 		}
 	}
 
@@ -339,22 +449,51 @@ static int __devinit jz_nand_probe(struct platform_device *pdev)
 
 	chip->chip_delay = 50;
 	chip->cmd_ctrl = jz_nand_cmd_ctrl;
+	chip->select_chip = jz_nand_select_chip;
 
 	if (pdata && gpio_is_valid(pdata->busy_gpio))
 		chip->dev_ready = jz_nand_dev_ready;
 
-	chip->IO_ADDR_R = nand->bank_base;
-	chip->IO_ADDR_W = nand->bank_base;
-
 	nand->pdata = pdata;
 	platform_set_drvdata(pdev, nand);
 
-	writel(JZ_NAND_CTRL_ENABLE_CHIP(0), nand->base + JZ_REG_NAND_CTRL);
-
-	ret = nand_scan_ident(mtd, 1, NULL);
-	if (ret) {
-		dev_err(&pdev->dev,  "Failed to scan nand\n");
-		goto err_gpio_free;
+	/* We are going to autodetect NAND chips in the banks specified in the
+	 * platform data. Although nand_scan_ident() can detect multiple chips,
+	 * it requires those chips to be numbered consecuitively, which is not
+	 * always the case for external memory banks. And a fixed chip-to-bank
+	 * mapping is not practical either, since for example Dingoo units
+	 * produced at different times have NAND chips in different banks.
+	 */
+	chipnr = 0;
+	for (bank_idx = 0; bank_idx < JZ_NAND_NUM_BANKS; bank_idx++) {
+		unsigned char bank;
+
+		/* If there is no platform data, look for NAND in bank 1,
+		 * which is the most likely bank since it is the only one
+		 * that can be booted from.
+		 */
+		bank = pdata ? pdata->banks[bank_idx] : bank_idx ^ 1;
+		if (bank == 0)
+			break;
+		if (bank > JZ_NAND_NUM_BANKS) {
+			dev_warn(&pdev->dev,
+				"Skipping non-existing bank: %d\n", bank);
+			continue;
+		}
+		/* The detection routine will directly or indirectly call
+		 * jz_nand_select_chip(), so nand->banks has to contain the
+		 * bank we're checking.
+		 */
+		nand->banks[chipnr] = bank;
+		if (jz_nand_detect_bank(pdev, nand, bank, chipnr,
+					&nand_maf_id, &nand_dev_id) == 0)
+			chipnr++;
+		else
+			nand->banks[chipnr] = 0;
+	}
+	if (chipnr == 0) {
+		dev_err(&pdev->dev, "No NAND chips found\n");
+		goto err_gpio_busy;
 	}
 
 	if (pdata && pdata->ident_callback) {
@@ -364,8 +503,8 @@ static int __devinit jz_nand_probe(struct platform_device *pdev)
 
 	ret = nand_scan_tail(mtd);
 	if (ret) {
-		dev_err(&pdev->dev,  "Failed to scan nand\n");
-		goto err_gpio_free;
+		dev_err(&pdev->dev,  "Failed to scan NAND\n");
+		goto err_unclaim_banks;
 	}
 
 	ret = mtd_device_parse_register(mtd, NULL, NULL,
@@ -382,14 +521,21 @@ static int __devinit jz_nand_probe(struct platform_device *pdev)
 	return 0;
 
 err_nand_release:
-	nand_release(&nand->mtd);
-err_gpio_free:
+	nand_release(mtd);
+err_unclaim_banks:
+	while (chipnr--) {
+		unsigned char bank = nand->banks[chipnr];
+		gpio_free(JZ_GPIO_MEM_CS0 + bank - 1);
+		jz_nand_iounmap_resource(nand->bank_mem[bank - 1],
+					 nand->bank_base[bank - 1]);
+	}
+	writel(0, nand->base + JZ_REG_NAND_CTRL);
+err_gpio_busy:
+	if (pdata && gpio_is_valid(pdata->busy_gpio))
+		gpio_free(pdata->busy_gpio);
 	platform_set_drvdata(pdev, NULL);
-	gpio_free(pdata->busy_gpio);
-err_iounmap_mem:
-	iounmap(nand->bank_base);
 err_iounmap_mmio:
-	iounmap(nand->base);
+	jz_nand_iounmap_resource(nand->mem, nand->base);
 err_free:
 	kfree(nand);
 	return ret;
@@ -398,16 +544,26 @@ err_free:
 static int __devexit jz_nand_remove(struct platform_device *pdev)
 {
 	struct jz_nand *nand = platform_get_drvdata(pdev);
+	struct jz_nand_platform_data *pdata = pdev->dev.platform_data;
+	size_t i;
 
 	nand_release(&nand->mtd);
 
 	/* Deassert and disable all chips */
 	writel(0, nand->base + JZ_REG_NAND_CTRL);
 
-	iounmap(nand->bank_base);
-	release_mem_region(nand->bank_mem->start, resource_size(nand->bank_mem));
-	iounmap(nand->base);
-	release_mem_region(nand->mem->start, resource_size(nand->mem));
+	for (i = 0; i < JZ_NAND_NUM_BANKS; ++i) {
+		unsigned char bank = nand->banks[i];
+		if (bank != 0) {
+			jz_nand_iounmap_resource(nand->bank_mem[bank - 1],
+						 nand->bank_base[bank - 1]);
+			gpio_free(JZ_GPIO_MEM_CS0 + bank - 1);
+		}
+	}
+	if (pdata && gpio_is_valid(pdata->busy_gpio))
+		gpio_free(pdata->busy_gpio);
+
+	jz_nand_iounmap_resource(nand->mem, nand->base);
 
 	platform_set_drvdata(pdev, NULL);
 	kfree(nand);
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index d7f681d0c9b..e9309b3659e 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/platform_device.h>
+#include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
 #include <linux/delay.h>
 #include <linux/module.h>
@@ -18,6 +19,7 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
+#include <linux/omap-dma.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 
@@ -123,7 +125,7 @@ struct omap_nand_info {
 	int				gpmc_cs;
 	unsigned long			phys_base;
 	struct completion		comp;
-	int				dma_ch;
+	struct dma_chan			*dma;
 	int				gpmc_irq;
 	enum {
 		OMAP_NAND_IO_READ = 0,	/* read */
@@ -336,12 +338,10 @@ static void omap_write_buf_pref(struct mtd_info *mtd,
 }
 
 /*
- * omap_nand_dma_cb: callback on the completion of dma transfer
- * @lch: logical channel
- * @ch_satuts: channel status
+ * omap_nand_dma_callback: callback on the completion of dma transfer
  * @data: pointer to completion data structure
  */
-static void omap_nand_dma_cb(int lch, u16 ch_status, void *data)
+static void omap_nand_dma_callback(void *data)
 {
 	complete((struct completion *) data);
 }
@@ -358,17 +358,13 @@ 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);
+	struct dma_async_tx_descriptor *tx;
 	enum dma_data_direction dir = is_write ? DMA_TO_DEVICE :
 							DMA_FROM_DEVICE;
-	dma_addr_t dma_addr;
-	int ret;
+	struct scatterlist sg;
 	unsigned long tim, limit;
-
-	/* 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;
+	unsigned n;
+	int ret;
 
 	if (addr >= high_memory) {
 		struct page *p1;
@@ -382,40 +378,33 @@ static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
 		addr = page_address(p1) + ((size_t)addr & ~PAGE_MASK);
 	}
 
-	dma_addr = dma_map_single(&info->pdev->dev, addr, len, dir);
-	if (dma_mapping_error(&info->pdev->dev, dma_addr)) {
+	sg_init_one(&sg, addr, len);
+	n = dma_map_sg(info->dma->device->dev, &sg, 1, dir);
+	if (n == 0) {
 		dev_err(&info->pdev->dev,
 			"Couldn't DMA map a %d byte buffer\n", len);
 		goto out_copy;
 	}
 
-	if (is_write) {
-	    omap_set_dma_dest_params(info->dma_ch, 0, OMAP_DMA_AMODE_CONSTANT,
-						info->phys_base, 0, 0);
-	    omap_set_dma_src_params(info->dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
-							dma_addr, 0, 0);
-	    omap_set_dma_transfer_params(info->dma_ch, OMAP_DMA_DATA_TYPE_S32,
-					0x10, buf_len, OMAP_DMA_SYNC_FRAME,
-					OMAP24XX_DMA_GPMC, OMAP_DMA_DST_SYNC);
-	} else {
-	    omap_set_dma_src_params(info->dma_ch, 0, OMAP_DMA_AMODE_CONSTANT,
-						info->phys_base, 0, 0);
-	    omap_set_dma_dest_params(info->dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
-							dma_addr, 0, 0);
-	    omap_set_dma_transfer_params(info->dma_ch, OMAP_DMA_DATA_TYPE_S32,
-					0x10, buf_len, OMAP_DMA_SYNC_FRAME,
-					OMAP24XX_DMA_GPMC, OMAP_DMA_SRC_SYNC);
-	}
-	/*  configure and start prefetch transfer */
+	tx = dmaengine_prep_slave_sg(info->dma, &sg, n,
+		is_write ? DMA_MEM_TO_DEV : DMA_DEV_TO_MEM,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!tx)
+		goto out_copy_unmap;
+
+	tx->callback = omap_nand_dma_callback;
+	tx->callback_param = &info->comp;
+	dmaengine_submit(tx);
+
+	/* configure and start prefetch transfer */
 	ret = gpmc_prefetch_enable(info->gpmc_cs,
-			PREFETCH_FIFOTHRESHOLD_MAX, 0x1, len, is_write);
+		PREFETCH_FIFOTHRESHOLD_MAX, 0x1, len, is_write);
 	if (ret)
 		/* PFPW engine is busy, use cpu copy method */
 		goto out_copy_unmap;
 
 	init_completion(&info->comp);
-
-	omap_start_dma(info->dma_ch);
+	dma_async_issue_pending(info->dma);
 
 	/* setup and start DMA using dma_addr */
 	wait_for_completion(&info->comp);
@@ -427,11 +416,11 @@ static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
 	/* disable and stop the PFPW engine */
 	gpmc_prefetch_reset(info->gpmc_cs);
 
-	dma_unmap_single(&info->pdev->dev, dma_addr, len, dir);
+	dma_unmap_sg(info->dma->device->dev, &sg, 1, dir);
 	return 0;
 
 out_copy_unmap:
-	dma_unmap_single(&info->pdev->dev, dma_addr, len, dir);
+	dma_unmap_sg(info->dma->device->dev, &sg, 1, dir);
 out_copy:
 	if (info->nand.options & NAND_BUSWIDTH_16)
 		is_write == 0 ? omap_read_buf16(mtd, (u_char *) addr, len)
@@ -1164,6 +1153,8 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 	struct omap_nand_platform_data	*pdata;
 	int				err;
 	int				i, offset;
+	dma_cap_mask_t mask;
+	unsigned sig;
 
 	pdata = pdev->dev.platform_data;
 	if (pdata == NULL) {
@@ -1244,18 +1235,31 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 		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");
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		sig = OMAP24XX_DMA_GPMC;
+		info->dma = dma_request_channel(mask, omap_dma_filter_fn, &sig);
+		if (!info->dma) {
+			dev_err(&pdev->dev, "DMA engine request failed\n");
+			err = -ENXIO;
 			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);
-
+			struct dma_slave_config cfg;
+			int rc;
+
+			memset(&cfg, 0, sizeof(cfg));
+			cfg.src_addr = info->phys_base;
+			cfg.dst_addr = info->phys_base;
+			cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+			cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+			cfg.src_maxburst = 16;
+			cfg.dst_maxburst = 16;
+			rc = dmaengine_slave_config(info->dma, &cfg);
+			if (rc) {
+				dev_err(&pdev->dev, "DMA engine slave config failed: %d\n",
+					rc);
+				goto out_release_mem_region;
+			}
 			info->nand.read_buf   = omap_read_buf_dma_pref;
 			info->nand.write_buf  = omap_write_buf_dma_pref;
 		}
@@ -1358,6 +1362,8 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 	return 0;
 
 out_release_mem_region:
+	if (info->dma)
+		dma_release_channel(info->dma);
 	release_mem_region(info->phys_base, NAND_IO_SIZE);
 out_free_info:
 	kfree(info);
@@ -1373,8 +1379,8 @@ static int omap_nand_remove(struct platform_device *pdev)
 	omap3_free_bch(&info->mtd);
 
 	platform_set_drvdata(pdev, NULL);
-	if (info->dma_ch != -1)
-		omap_free_dma(info->dma_ch);
+	if (info->dma)
+		dma_release_channel(info->dma);
 
 	if (info->gpmc_irq)
 		free_irq(info->gpmc_irq, info);
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index 513dc88a05c..fc5a868c436 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -183,6 +183,10 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 	return 0;
 
 no_dev:
+	if (!IS_ERR(clk)) {
+		clk_disable_unprepare(clk);
+		clk_put(clk);
+	}
 	platform_set_drvdata(pdev, NULL);
 	iounmap(io_base);
 no_res:
@@ -214,7 +218,7 @@ static int __devexit orion_nand_remove(struct platform_device *pdev)
 
 #ifdef CONFIG_OF
 static struct of_device_id orion_nand_of_match_table[] = {
-	{ .compatible = "mrvl,orion-nand", },
+	{ .compatible = "marvell,orion-nand", },
 	{},
 };
 #endif
diff --git a/drivers/mtd/ubi/Kconfig b/drivers/mtd/ubi/Kconfig
index 738ee8dc16c..ea4b95b5451 100644
--- a/drivers/mtd/ubi/Kconfig
+++ b/drivers/mtd/ubi/Kconfig
@@ -29,7 +29,7 @@ config MTD_UBI_WL_THRESHOLD
 
 config MTD_UBI_BEB_RESERVE
 	int "Percentage of reserved eraseblocks for bad eraseblocks handling"
-	default 1
+	default 2
 	range 0 25
 	help
 	  If the MTD device admits of bad eraseblocks (e.g. NAND flash), UBI
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index acec85deb6a..fb556787818 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -1026,7 +1026,7 @@ static long ctrl_cdev_ioctl(struct file *file, unsigned int cmd,
 	{
 		int ubi_num;
 
-		dbg_gen("dettach MTD device");
+		dbg_gen("detach MTD device");
 		err = get_user(ubi_num, (__user int32_t *)argp);
 		if (err) {
 			err = -EFAULT;
diff --git a/drivers/mtd/ubi/misc.c b/drivers/mtd/ubi/misc.c
index f6a7d7ac4b9..8bbfb444b89 100644
--- a/drivers/mtd/ubi/misc.c
+++ b/drivers/mtd/ubi/misc.c
@@ -92,7 +92,30 @@ int ubi_check_volume(struct ubi_device *ubi, int vol_id)
 }
 
 /**
- * ubi_calculate_rsvd_pool - calculate how many PEBs must be reserved for bad
+ * ubi_update_reserved - update bad eraseblock handling accounting data.
+ * @ubi: UBI device description object
+ *
+ * This function calculates the gap between current number of PEBs reserved for
+ * bad eraseblock handling and the required level of PEBs that must be
+ * reserved, and if necessary, reserves more PEBs to fill that gap, according
+ * to availability. Should be called with ubi->volumes_lock held.
+ */
+void ubi_update_reserved(struct ubi_device *ubi)
+{
+	int need = ubi->beb_rsvd_level - ubi->beb_rsvd_pebs;
+
+	if (need <= 0 || ubi->avail_pebs == 0)
+		return;
+
+	need = min_t(int, need, ubi->avail_pebs);
+	ubi->avail_pebs -= need;
+	ubi->rsvd_pebs += need;
+	ubi->beb_rsvd_pebs += need;
+	ubi_msg("reserved more %d PEBs for bad PEB handling", need);
+}
+
+/**
+ * ubi_calculate_reserved - calculate how many PEBs must be reserved for bad
  * eraseblock handling.
  * @ubi: UBI device description object
  */
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index a1a81c9ea8c..84f66e3fa05 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -647,6 +647,7 @@ int ubi_more_leb_change_data(struct ubi_device *ubi, struct ubi_volume *vol,
 int ubi_calc_data_len(const struct ubi_device *ubi, const void *buf,
 		      int length);
 int ubi_check_volume(struct ubi_device *ubi, int vol_id);
+void ubi_update_reserved(struct ubi_device *ubi);
 void ubi_calculate_reserved(struct ubi_device *ubi);
 int ubi_check_pattern(const void *buf, uint8_t patt, int size);
 
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 0669cff8ac3..9169e58c262 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -443,15 +443,7 @@ int ubi_remove_volume(struct ubi_volume_desc *desc, int no_vtbl)
 	spin_lock(&ubi->volumes_lock);
 	ubi->rsvd_pebs -= reserved_pebs;
 	ubi->avail_pebs += reserved_pebs;
-	i = ubi->beb_rsvd_level - ubi->beb_rsvd_pebs;
-	if (i > 0) {
-		i = ubi->avail_pebs >= i ? i : ubi->avail_pebs;
-		ubi->avail_pebs -= i;
-		ubi->rsvd_pebs += i;
-		ubi->beb_rsvd_pebs += i;
-		if (i > 0)
-			ubi_msg("reserve more %d PEBs", i);
-	}
+	ubi_update_reserved(ubi);
 	ubi->vol_count -= 1;
 	spin_unlock(&ubi->volumes_lock);
 
@@ -558,15 +550,7 @@ int ubi_resize_volume(struct ubi_volume_desc *desc, int reserved_pebs)
 		spin_lock(&ubi->volumes_lock);
 		ubi->rsvd_pebs += pebs;
 		ubi->avail_pebs -= pebs;
-		pebs = ubi->beb_rsvd_level - ubi->beb_rsvd_pebs;
-		if (pebs > 0) {
-			pebs = ubi->avail_pebs >= pebs ? pebs : ubi->avail_pebs;
-			ubi->avail_pebs -= pebs;
-			ubi->rsvd_pebs += pebs;
-			ubi->beb_rsvd_pebs += pebs;
-			if (pebs > 0)
-				ubi_msg("reserve more %d PEBs", pebs);
-		}
+		ubi_update_reserved(ubi);
 		for (i = 0; i < reserved_pebs; i++)
 			new_mapping[i] = vol->eba_tbl[i];
 		kfree(vol->eba_tbl);
diff --git a/drivers/net/appletalk/cops.c b/drivers/net/appletalk/cops.c
index dd5e04813b7..545c09ed907 100644
--- a/drivers/net/appletalk/cops.c
+++ b/drivers/net/appletalk/cops.c
@@ -936,7 +936,7 @@ static int cops_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
         struct cops_local *lp = netdev_priv(dev);
         struct sockaddr_at *sa = (struct sockaddr_at *)&ifr->ifr_addr;
-        struct atalk_addr *aa = (struct atalk_addr *)&lp->node_addr;
+        struct atalk_addr *aa = &lp->node_addr;
 
         switch(cmd)
         {
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index 3463b469e65..a030e635f00 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -2454,24 +2454,27 @@ int bond_3ad_xmit_xor(struct sk_buff *skb, struct net_device *dev)
 out:
 	if (res) {
 		/* no suitable interface, frame not sent */
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 	}
 
 	return NETDEV_TX_OK;
 }
 
-int bond_3ad_lacpdu_recv(struct sk_buff *skb, struct bonding *bond,
-			  struct slave *slave)
+int bond_3ad_lacpdu_recv(const struct sk_buff *skb, struct bonding *bond,
+			 struct slave *slave)
 {
 	int ret = RX_HANDLER_ANOTHER;
+	struct lacpdu *lacpdu, _lacpdu;
+
 	if (skb->protocol != PKT_TYPE_LACPDU)
 		return ret;
 
-	if (!pskb_may_pull(skb, sizeof(struct lacpdu)))
+	lacpdu = skb_header_pointer(skb, 0, sizeof(_lacpdu), &_lacpdu);
+	if (!lacpdu)
 		return ret;
 
 	read_lock(&bond->lock);
-	ret = bond_3ad_rx_indication((struct lacpdu *) skb->data, slave, skb->len);
+	ret = bond_3ad_rx_indication(lacpdu, slave, skb->len);
 	read_unlock(&bond->lock);
 	return ret;
 }
diff --git a/drivers/net/bonding/bond_3ad.h b/drivers/net/bonding/bond_3ad.h
index 5ee7e3c45db..0cfaa4afdec 100644
--- a/drivers/net/bonding/bond_3ad.h
+++ b/drivers/net/bonding/bond_3ad.h
@@ -274,8 +274,8 @@ void bond_3ad_adapter_duplex_changed(struct slave *slave);
 void bond_3ad_handle_link_change(struct slave *slave, char link);
 int  bond_3ad_get_active_agg_info(struct bonding *bond, struct ad_info *ad_info);
 int bond_3ad_xmit_xor(struct sk_buff *skb, struct net_device *dev);
-int bond_3ad_lacpdu_recv(struct sk_buff *skb, struct bonding *bond,
-			  struct slave *slave);
+int bond_3ad_lacpdu_recv(const struct sk_buff *skb, struct bonding *bond,
+			 struct slave *slave);
 int bond_3ad_set_carrier(struct bonding *bond);
 void bond_3ad_update_lacp_rate(struct bonding *bond);
 #endif //__BOND_3AD_H__
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 0f59c1564e5..e15cc11edbb 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -342,27 +342,17 @@ static void rlb_update_entry_from_arp(struct bonding *bond, struct arp_pkt *arp)
 	_unlock_rx_hashtbl_bh(bond);
 }
 
-static int rlb_arp_recv(struct sk_buff *skb, struct bonding *bond,
-			 struct slave *slave)
+static int rlb_arp_recv(const struct sk_buff *skb, struct bonding *bond,
+			struct slave *slave)
 {
-	struct arp_pkt *arp;
+	struct arp_pkt *arp, _arp;
 
 	if (skb->protocol != cpu_to_be16(ETH_P_ARP))
 		goto out;
 
-	arp = (struct arp_pkt *) skb->data;
-	if (!arp) {
-		pr_debug("Packet has no ARP data\n");
+	arp = skb_header_pointer(skb, 0, sizeof(_arp), &_arp);
+	if (!arp)
 		goto out;
-	}
-
-	if (!pskb_may_pull(skb, arp_hdr_len(bond->dev)))
-		goto out;
-
-	if (skb->len < sizeof(struct arp_pkt)) {
-		pr_debug("Packet is too small to be an ARP\n");
-		goto out;
-	}
 
 	if (arp->op_code == htons(ARPOP_REPLY)) {
 		/* update rx hash table for this ARP */
@@ -1356,12 +1346,12 @@ int bond_alb_xmit(struct sk_buff *skb, struct net_device *bond_dev)
 		}
 	}
 
+	read_unlock(&bond->curr_slave_lock);
+
 	if (res) {
 		/* no suitable interface, frame not sent */
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 	}
-	read_unlock(&bond->curr_slave_lock);
-
 	return NETDEV_TX_OK;
 }
 
diff --git a/drivers/net/bonding/bond_debugfs.c b/drivers/net/bonding/bond_debugfs.c
index 3680aa251de..2cf084eb9d5 100644
--- a/drivers/net/bonding/bond_debugfs.c
+++ b/drivers/net/bonding/bond_debugfs.c
@@ -6,7 +6,7 @@
 #include "bonding.h"
 #include "bond_alb.h"
 
-#ifdef CONFIG_DEBUG_FS
+#if defined(CONFIG_DEBUG_FS) && !defined(CONFIG_NET_NS)
 
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index b9c2ae62166..6fae5f3ec7f 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -395,8 +395,8 @@ int bond_dev_queue_xmit(struct bonding *bond, struct sk_buff *skb,
 	skb->dev = slave_dev;
 
 	BUILD_BUG_ON(sizeof(skb->queue_mapping) !=
-		     sizeof(qdisc_skb_cb(skb)->bond_queue_mapping));
-	skb->queue_mapping = qdisc_skb_cb(skb)->bond_queue_mapping;
+		     sizeof(qdisc_skb_cb(skb)->slave_dev_queue_mapping));
+	skb->queue_mapping = qdisc_skb_cb(skb)->slave_dev_queue_mapping;
 
 	if (unlikely(netpoll_tx_running(slave_dev)))
 		bond_netpoll_send_skb(bond_get_slave_by_dev(bond, slave_dev), skb);
@@ -1240,9 +1240,7 @@ static inline int slave_enable_netpoll(struct slave *slave)
 	if (!np)
 		goto out;
 
-	np->dev = slave->dev;
-	strlcpy(np->dev_name, slave->dev->name, IFNAMSIZ);
-	err = __netpoll_setup(np);
+	err = __netpoll_setup(np, slave->dev);
 	if (err) {
 		kfree(np);
 		goto out;
@@ -1384,6 +1382,7 @@ static void bond_compute_features(struct bonding *bond)
 	netdev_features_t vlan_features = BOND_VLAN_FEATURES;
 	unsigned short max_hard_header_len = ETH_HLEN;
 	int i;
+	unsigned int flags, dst_release_flag = IFF_XMIT_DST_RELEASE;
 
 	read_lock(&bond->lock);
 
@@ -1394,6 +1393,7 @@ static void bond_compute_features(struct bonding *bond)
 		vlan_features = netdev_increment_features(vlan_features,
 			slave->dev->vlan_features, BOND_VLAN_FEATURES);
 
+		dst_release_flag &= slave->dev->priv_flags;
 		if (slave->dev->hard_header_len > max_hard_header_len)
 			max_hard_header_len = slave->dev->hard_header_len;
 	}
@@ -1402,6 +1402,9 @@ done:
 	bond_dev->vlan_features = vlan_features;
 	bond_dev->hard_header_len = max_hard_header_len;
 
+	flags = bond_dev->priv_flags & ~IFF_XMIT_DST_RELEASE;
+	bond_dev->priv_flags = flags | dst_release_flag;
+
 	read_unlock(&bond->lock);
 
 	netdev_change_features(bond_dev);
@@ -1445,8 +1448,8 @@ static rx_handler_result_t bond_handle_frame(struct sk_buff **pskb)
 	struct sk_buff *skb = *pskb;
 	struct slave *slave;
 	struct bonding *bond;
-	int (*recv_probe)(struct sk_buff *, struct bonding *,
-				struct slave *);
+	int (*recv_probe)(const struct sk_buff *, struct bonding *,
+			  struct slave *);
 	int ret = RX_HANDLER_ANOTHER;
 
 	skb = skb_share_check(skb, GFP_ATOMIC);
@@ -1463,15 +1466,10 @@ static rx_handler_result_t bond_handle_frame(struct sk_buff **pskb)
 
 	recv_probe = ACCESS_ONCE(bond->recv_probe);
 	if (recv_probe) {
-		struct sk_buff *nskb = skb_clone(skb, GFP_ATOMIC);
-
-		if (likely(nskb)) {
-			ret = recv_probe(nskb, bond, slave);
-			dev_kfree_skb(nskb);
-			if (ret == RX_HANDLER_CONSUMED) {
-				consume_skb(skb);
-				return ret;
-			}
+		ret = recv_probe(skb, bond, slave);
+		if (ret == RX_HANDLER_CONSUMED) {
+			consume_skb(skb);
+			return ret;
 		}
 	}
 
@@ -2738,25 +2736,31 @@ static void bond_validate_arp(struct bonding *bond, struct slave *slave, __be32
 	}
 }
 
-static int bond_arp_rcv(struct sk_buff *skb, struct bonding *bond,
-			 struct slave *slave)
+static int bond_arp_rcv(const struct sk_buff *skb, struct bonding *bond,
+			struct slave *slave)
 {
-	struct arphdr *arp;
+	struct arphdr *arp = (struct arphdr *)skb->data;
 	unsigned char *arp_ptr;
 	__be32 sip, tip;
+	int alen;
 
 	if (skb->protocol != __cpu_to_be16(ETH_P_ARP))
 		return RX_HANDLER_ANOTHER;
 
 	read_lock(&bond->lock);
+	alen = arp_hdr_len(bond->dev);
 
 	pr_debug("bond_arp_rcv: bond %s skb->dev %s\n",
 		 bond->dev->name, skb->dev->name);
 
-	if (!pskb_may_pull(skb, arp_hdr_len(bond->dev)))
-		goto out_unlock;
+	if (alen > skb_headlen(skb)) {
+		arp = kmalloc(alen, GFP_ATOMIC);
+		if (!arp)
+			goto out_unlock;
+		if (skb_copy_bits(skb, 0, arp, alen) < 0)
+			goto out_unlock;
+	}
 
-	arp = arp_hdr(skb);
 	if (arp->ar_hln != bond->dev->addr_len ||
 	    skb->pkt_type == PACKET_OTHERHOST ||
 	    skb->pkt_type == PACKET_LOOPBACK ||
@@ -2791,6 +2795,8 @@ static int bond_arp_rcv(struct sk_buff *skb, struct bonding *bond,
 
 out_unlock:
 	read_unlock(&bond->lock);
+	if (arp != (struct arphdr *)skb->data)
+		kfree(arp);
 	return RX_HANDLER_ANOTHER;
 }
 
@@ -3227,6 +3233,12 @@ static int bond_master_netdev_event(unsigned long event,
 	switch (event) {
 	case NETDEV_CHANGENAME:
 		return bond_event_changename(event_bond);
+	case NETDEV_UNREGISTER:
+		bond_remove_proc_entry(event_bond);
+		break;
+	case NETDEV_REGISTER:
+		bond_create_proc_entry(event_bond);
+		break;
 	default:
 		break;
 	}
@@ -3987,7 +3999,7 @@ static int bond_xmit_roundrobin(struct sk_buff *skb, struct net_device *bond_dev
 out:
 	if (res) {
 		/* no suitable interface, frame not sent */
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 	}
 
 	return NETDEV_TX_OK;
@@ -4009,11 +4021,11 @@ static int bond_xmit_activebackup(struct sk_buff *skb, struct net_device *bond_d
 		res = bond_dev_queue_xmit(bond, skb,
 			bond->curr_active_slave->dev);
 
+	read_unlock(&bond->curr_slave_lock);
+
 	if (res)
 		/* no suitable interface, frame not sent */
-		dev_kfree_skb(skb);
-
-	read_unlock(&bond->curr_slave_lock);
+		kfree_skb(skb);
 
 	return NETDEV_TX_OK;
 }
@@ -4052,7 +4064,7 @@ static int bond_xmit_xor(struct sk_buff *skb, struct net_device *bond_dev)
 
 	if (res) {
 		/* no suitable interface, frame not sent */
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 	}
 
 	return NETDEV_TX_OK;
@@ -4090,7 +4102,7 @@ static int bond_xmit_broadcast(struct sk_buff *skb, struct net_device *bond_dev)
 
 				res = bond_dev_queue_xmit(bond, skb2, tx_dev);
 				if (res) {
-					dev_kfree_skb(skb2);
+					kfree_skb(skb2);
 					continue;
 				}
 			}
@@ -4104,7 +4116,7 @@ static int bond_xmit_broadcast(struct sk_buff *skb, struct net_device *bond_dev)
 out:
 	if (res)
 		/* no suitable interface, frame not sent */
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 
 	/* frame sent to all suitable interfaces */
 	return NETDEV_TX_OK;
@@ -4172,7 +4184,7 @@ static u16 bond_select_queue(struct net_device *dev, struct sk_buff *skb)
 	/*
 	 * Save the original txq to restore before passing to the driver
 	 */
-	qdisc_skb_cb(skb)->bond_queue_mapping = skb->queue_mapping;
+	qdisc_skb_cb(skb)->slave_dev_queue_mapping = skb->queue_mapping;
 
 	if (unlikely(txq >= dev->real_num_tx_queues)) {
 		do {
@@ -4210,7 +4222,7 @@ static netdev_tx_t __bond_start_xmit(struct sk_buff *skb, struct net_device *dev
 		pr_err("%s: Error: Unknown bonding mode %d\n",
 		       dev->name, bond->params.mode);
 		WARN_ON_ONCE(1);
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 		return NETDEV_TX_OK;
 	}
 }
@@ -4232,7 +4244,7 @@ static netdev_tx_t bond_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (bond->slave_cnt)
 		ret = __bond_start_xmit(skb, dev);
 	else
-		dev_kfree_skb(skb);
+		kfree_skb(skb);
 
 	read_unlock(&bond->lock);
 
@@ -4411,8 +4423,6 @@ static void bond_uninit(struct net_device *bond_dev)
 
 	bond_work_cancel_all(bond);
 
-	bond_remove_proc_entry(bond);
-
 	bond_debug_unregister(bond);
 
 	__hw_addr_flush(&bond->mc_list);
@@ -4814,7 +4824,6 @@ static int bond_init(struct net_device *bond_dev)
 
 	bond_set_lockdep_class(bond_dev);
 
-	bond_create_proc_entry(bond);
 	list_add_tail(&bond->bond_list, &bn->dev_list);
 
 	bond_prepare_sysfs_group(bond);
@@ -4836,17 +4845,19 @@ static int bond_validate(struct nlattr *tb[], struct nlattr *data[])
 	return 0;
 }
 
-static int bond_get_tx_queues(struct net *net, struct nlattr *tb[])
+static unsigned int bond_get_num_tx_queues(void)
 {
 	return tx_queues;
 }
 
 static struct rtnl_link_ops bond_link_ops __read_mostly = {
-	.kind		= "bond",
-	.priv_size	= sizeof(struct bonding),
-	.setup		= bond_setup,
-	.validate	= bond_validate,
-	.get_tx_queues	= bond_get_tx_queues,
+	.kind			= "bond",
+	.priv_size		= sizeof(struct bonding),
+	.setup			= bond_setup,
+	.validate		= bond_validate,
+	.get_num_tx_queues	= bond_get_num_tx_queues,
+	.get_num_rx_queues	= bond_get_num_tx_queues, /* Use the same number
+							     as for TX queues */
 };
 
 /* Create a new bond based on the specified name and bonding parameters.
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index 485bedb8278..dc15d248443 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -1495,7 +1495,7 @@ static ssize_t bonding_store_queue_id(struct device *d,
 	/* Check buffer length, valid ifname and queue id */
 	if (strlen(buffer) > IFNAMSIZ ||
 	    !dev_valid_name(buffer) ||
-	    qid > bond->params.tx_queues)
+	    qid > bond->dev->real_num_tx_queues)
 		goto err_no_cmd;
 
 	/* Get the pointer to that interface if it exists */
diff --git a/drivers/net/bonding/bonding.h b/drivers/net/bonding/bonding.h
index 4581aa5ccab..f8af2fcd3d1 100644
--- a/drivers/net/bonding/bonding.h
+++ b/drivers/net/bonding/bonding.h
@@ -218,8 +218,8 @@ struct bonding {
 	struct   slave *primary_slave;
 	bool     force_primary;
 	s32      slave_cnt; /* never change this value outside the attach/detach wrappers */
-	int     (*recv_probe)(struct sk_buff *, struct bonding *,
-			       struct slave *);
+	int     (*recv_probe)(const struct sk_buff *, struct bonding *,
+			      struct slave *);
 	rwlock_t lock;
 	rwlock_t curr_slave_lock;
 	u8	 send_peer_notif;
diff --git a/drivers/net/caif/caif_hsi.c b/drivers/net/caif/caif_hsi.c
index 4a27adb7ae6..0def8b3106f 100644
--- a/drivers/net/caif/caif_hsi.c
+++ b/drivers/net/caif/caif_hsi.c
@@ -11,7 +11,6 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/device.h>
-#include <linux/platform_device.h>
 #include <linux/netdevice.h>
 #include <linux/string.h>
 #include <linux/list.h>
@@ -20,7 +19,7 @@
 #include <linux/sched.h>
 #include <linux/if_arp.h>
 #include <linux/timer.h>
-#include <linux/rtnetlink.h>
+#include <net/rtnetlink.h>
 #include <linux/pkt_sched.h>
 #include <net/caif/caif_layer.h>
 #include <net/caif/caif_hsi.h>
@@ -33,59 +32,46 @@ MODULE_DESCRIPTION("CAIF HSI driver");
 #define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\
 				(((pow)-((x)&((pow)-1)))))
 
-static int inactivity_timeout = 1000;
-module_param(inactivity_timeout, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(inactivity_timeout, "Inactivity timeout on HSI, ms.");
+static const struct cfhsi_config  hsi_default_config = {
 
-static int aggregation_timeout = 1;
-module_param(aggregation_timeout, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(aggregation_timeout, "Aggregation timeout on HSI, ms.");
+	/* Inactivity timeout on HSI, ms */
+	.inactivity_timeout = HZ,
 
-/*
- * HSI padding options.
- * Warning: must be a base of 2 (& operation used) and can not be zero !
- */
-static int hsi_head_align = 4;
-module_param(hsi_head_align, int, S_IRUGO);
-MODULE_PARM_DESC(hsi_head_align, "HSI head alignment.");
+	/* Aggregation timeout (ms) of zero means no aggregation is done*/
+	.aggregation_timeout = 1,
 
-static int hsi_tail_align = 4;
-module_param(hsi_tail_align, int, S_IRUGO);
-MODULE_PARM_DESC(hsi_tail_align, "HSI tail alignment.");
-
-/*
- * HSI link layer flowcontrol thresholds.
- * Warning: A high threshold value migth increase throughput but it will at
- * the same time prevent channel prioritization and increase the risk of
- * flooding the modem. The high threshold should be above the low.
- */
-static int hsi_high_threshold = 100;
-module_param(hsi_high_threshold, int, S_IRUGO);
-MODULE_PARM_DESC(hsi_high_threshold, "HSI high threshold (FLOW OFF).");
+	/*
+	 * HSI link layer flow-control thresholds.
+	 * Threshold values for the HSI packet queue. Flow-control will be
+	 * asserted when the number of packets exceeds q_high_mark. It will
+	 * not be de-asserted before the number of packets drops below
+	 * q_low_mark.
+	 * Warning: A high threshold value might increase throughput but it
+	 * will at the same time prevent channel prioritization and increase
+	 * the risk of flooding the modem. The high threshold should be above
+	 * the low.
+	 */
+	.q_high_mark = 100,
+	.q_low_mark = 50,
 
-static int hsi_low_threshold = 50;
-module_param(hsi_low_threshold, int, S_IRUGO);
-MODULE_PARM_DESC(hsi_low_threshold, "HSI high threshold (FLOW ON).");
+	/*
+	 * HSI padding options.
+	 * Warning: must be a base of 2 (& operation used) and can not be zero !
+	 */
+	.head_align = 4,
+	.tail_align = 4,
+};
 
 #define ON 1
 #define OFF 0
 
-/*
- * Threshold values for the HSI packet queue. Flowcontrol will be asserted
- * when the number of packets exceeds HIGH_WATER_MARK. It will not be
- * de-asserted before the number of packets drops below LOW_WATER_MARK.
- */
-#define LOW_WATER_MARK   hsi_low_threshold
-#define HIGH_WATER_MARK  hsi_high_threshold
-
 static LIST_HEAD(cfhsi_list);
-static spinlock_t cfhsi_list_lock;
 
 static void cfhsi_inactivity_tout(unsigned long arg)
 {
 	struct cfhsi *cfhsi = (struct cfhsi *)arg;
 
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	/* Schedule power down work queue. */
@@ -101,8 +87,8 @@ static void cfhsi_update_aggregation_stats(struct cfhsi *cfhsi,
 	int hpad, tpad, len;
 
 	info = (struct caif_payload_info *)&skb->cb;
-	hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
-	tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
+	hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align);
+	tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align);
 	len = skb->len + hpad + tpad;
 
 	if (direction > 0)
@@ -115,7 +101,7 @@ static bool cfhsi_can_send_aggregate(struct cfhsi *cfhsi)
 {
 	int i;
 
-	if (cfhsi->aggregation_timeout < 0)
+	if (cfhsi->cfg.aggregation_timeout == 0)
 		return true;
 
 	for (i = 0; i < CFHSI_PRIO_BEBK; ++i) {
@@ -171,7 +157,7 @@ static void cfhsi_abort_tx(struct cfhsi *cfhsi)
 	cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 	if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 		mod_timer(&cfhsi->inactivity_timer,
-			jiffies + cfhsi->inactivity_timeout);
+			jiffies + cfhsi->cfg.inactivity_timeout);
 	spin_unlock_bh(&cfhsi->lock);
 }
 
@@ -181,14 +167,14 @@ static int cfhsi_flush_fifo(struct cfhsi *cfhsi)
 	size_t fifo_occupancy;
 	int ret;
 
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	do {
-		ret = cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
+		ret = cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 				&fifo_occupancy);
 		if (ret) {
-			dev_warn(&cfhsi->ndev->dev,
+			netdev_warn(cfhsi->ndev,
 				"%s: can't get FIFO occupancy: %d.\n",
 				__func__, ret);
 			break;
@@ -198,11 +184,11 @@ static int cfhsi_flush_fifo(struct cfhsi *cfhsi)
 
 		fifo_occupancy = min(sizeof(buffer), fifo_occupancy);
 		set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
-		ret = cfhsi->dev->cfhsi_rx(buffer, fifo_occupancy,
-				cfhsi->dev);
+		ret = cfhsi->ops->cfhsi_rx(buffer, fifo_occupancy,
+				cfhsi->ops);
 		if (ret) {
 			clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
-			dev_warn(&cfhsi->ndev->dev,
+			netdev_warn(cfhsi->ndev,
 				"%s: can't read data: %d.\n",
 				__func__, ret);
 			break;
@@ -213,13 +199,13 @@ static int cfhsi_flush_fifo(struct cfhsi *cfhsi)
 			 !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret);
 
 		if (ret < 0) {
-			dev_warn(&cfhsi->ndev->dev,
+			netdev_warn(cfhsi->ndev,
 				"%s: can't wait for flush complete: %d.\n",
 				__func__, ret);
 			break;
 		} else if (!ret) {
 			ret = -ETIMEDOUT;
-			dev_warn(&cfhsi->ndev->dev,
+			netdev_warn(cfhsi->ndev,
 				"%s: timeout waiting for flush complete.\n",
 				__func__);
 			break;
@@ -246,14 +232,14 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	/* Check if we can embed a CAIF frame. */
 	if (skb->len < CFHSI_MAX_EMB_FRM_SZ) {
 		struct caif_payload_info *info;
-		int hpad = 0;
-		int tpad = 0;
+		int hpad;
+		int tpad;
 
 		/* Calculate needed head alignment and tail alignment. */
 		info = (struct caif_payload_info *)&skb->cb;
 
-		hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
-		tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
+		hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align);
+		tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align);
 
 		/* Check if frame still fits with added alignment. */
 		if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) {
@@ -282,8 +268,8 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
 	while (nfrms < CFHSI_MAX_PKTS) {
 		struct caif_payload_info *info;
-		int hpad = 0;
-		int tpad = 0;
+		int hpad;
+		int tpad;
 
 		if (!skb)
 			skb = cfhsi_dequeue(cfhsi);
@@ -294,8 +280,8 @@ static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		/* Calculate needed head alignment and tail alignment. */
 		info = (struct caif_payload_info *)&skb->cb;
 
-		hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
-		tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
+		hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align);
+		tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align);
 
 		/* Fill in CAIF frame length in descriptor. */
 		desc->cffrm_len[nfrms] = hpad + skb->len + tpad;
@@ -348,7 +334,7 @@ static void cfhsi_start_tx(struct cfhsi *cfhsi)
 	struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
 	int len, res;
 
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
+	netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 		return;
@@ -366,22 +352,22 @@ static void cfhsi_start_tx(struct cfhsi *cfhsi)
 			cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
 			/* Start inactivity timer. */
 			mod_timer(&cfhsi->inactivity_timer,
-				jiffies + cfhsi->inactivity_timeout);
+				jiffies + cfhsi->cfg.inactivity_timeout);
 			spin_unlock_bh(&cfhsi->lock);
 			break;
 		}
 
 		/* Set up new transfer. */
-		res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
+		res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
 		if (WARN_ON(res < 0))
-			dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
+			netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
 				__func__, res);
 	} while (res < 0);
 }
 
 static void cfhsi_tx_done(struct cfhsi *cfhsi)
 {
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
+	netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 		return;
@@ -392,7 +378,7 @@ static void cfhsi_tx_done(struct cfhsi *cfhsi)
 	 */
 	spin_lock_bh(&cfhsi->lock);
 	if (cfhsi->flow_off_sent &&
-			cfhsi_tx_queue_len(cfhsi) <= cfhsi->q_low_mark &&
+			cfhsi_tx_queue_len(cfhsi) <= cfhsi->cfg.q_low_mark &&
 			cfhsi->cfdev.flowctrl) {
 
 		cfhsi->flow_off_sent = 0;
@@ -404,19 +390,19 @@ static void cfhsi_tx_done(struct cfhsi *cfhsi)
 		cfhsi_start_tx(cfhsi);
 	} else {
 		mod_timer(&cfhsi->aggregation_timer,
-			jiffies + cfhsi->aggregation_timeout);
+			jiffies + cfhsi->cfg.aggregation_timeout);
 		spin_unlock_bh(&cfhsi->lock);
 	}
 
 	return;
 }
 
-static void cfhsi_tx_done_cb(struct cfhsi_drv *drv)
+static void cfhsi_tx_done_cb(struct cfhsi_cb_ops *cb_ops)
 {
 	struct cfhsi *cfhsi;
 
-	cfhsi = container_of(drv, struct cfhsi, drv);
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
@@ -433,7 +419,7 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 
 	if ((desc->header & ~CFHSI_PIGGY_DESC) ||
 			(desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
-		dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
+		netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n",
 			__func__);
 		return -EPROTO;
 	}
@@ -455,7 +441,7 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 
 		/* Sanity check length of CAIF frame. */
 		if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
-			dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n",
+			netdev_err(cfhsi->ndev, "%s: Invalid length.\n",
 				__func__);
 			return -EPROTO;
 		}
@@ -463,7 +449,7 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		/* Allocate SKB (OK even in IRQ context). */
 		skb = alloc_skb(len + 1, GFP_ATOMIC);
 		if (!skb) {
-			dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n",
+			netdev_err(cfhsi->ndev, "%s: Out of memory !\n",
 				__func__);
 			return -ENOMEM;
 		}
@@ -477,8 +463,8 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		skb->dev = cfhsi->ndev;
 
 		/*
-		 * We are called from a arch specific platform device.
-		 * Unfortunately we don't know what context we're
+		 * We are in a callback handler and
+		 * unfortunately we don't know what context we're
 		 * running in.
 		 */
 		if (in_interrupt())
@@ -504,7 +490,7 @@ static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		xfer_sz += CFHSI_DESC_SZ;
 
 	if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) {
-		dev_err(&cfhsi->ndev->dev,
+		netdev_err(cfhsi->ndev,
 				"%s: Invalid payload len: %d, ignored.\n",
 			__func__, xfer_sz);
 		return -EPROTO;
@@ -551,7 +537,7 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 	/* Sanity check header and offset. */
 	if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) ||
 			(desc->offset > CFHSI_MAX_EMB_FRM_SZ))) {
-		dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
+		netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n",
 			__func__);
 		return -EPROTO;
 	}
@@ -573,7 +559,7 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		struct sk_buff *skb;
 		u8 *dst = NULL;
 		u8 *pcffrm = NULL;
-		int len = 0;
+		int len;
 
 		/* CAIF frame starts after head padding. */
 		pcffrm = pfrm + *pfrm + 1;
@@ -585,7 +571,7 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 
 		/* Sanity check length of CAIF frames. */
 		if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
-			dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n",
+			netdev_err(cfhsi->ndev, "%s: Invalid length.\n",
 				__func__);
 			return -EPROTO;
 		}
@@ -593,7 +579,7 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		/* Allocate SKB (OK even in IRQ context). */
 		skb = alloc_skb(len + 1, GFP_ATOMIC);
 		if (!skb) {
-			dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n",
+			netdev_err(cfhsi->ndev, "%s: Out of memory !\n",
 				__func__);
 			cfhsi->rx_state.nfrms = nfrms;
 			return -ENOMEM;
@@ -608,7 +594,7 @@ static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
 		skb->dev = cfhsi->ndev;
 
 		/*
-		 * We're called from a platform device,
+		 * We're called in callback from HSI
 		 * and don't know the context we're running in.
 		 */
 		if (in_interrupt())
@@ -639,7 +625,7 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 
 	desc = (struct cfhsi_desc *)cfhsi->rx_buf;
 
-	dev_dbg(&cfhsi->ndev->dev, "%s\n", __func__);
+	netdev_dbg(cfhsi->ndev, "%s\n", __func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 		return;
@@ -647,7 +633,7 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 	/* Update inactivity timer if pending. */
 	spin_lock_bh(&cfhsi->lock);
 	mod_timer_pending(&cfhsi->inactivity_timer,
-			jiffies + cfhsi->inactivity_timeout);
+			jiffies + cfhsi->cfg.inactivity_timeout);
 	spin_unlock_bh(&cfhsi->lock);
 
 	if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
@@ -680,12 +666,11 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 			if (desc_pld_len < 0)
 				goto out_of_sync;
 
-			if (desc_pld_len > 0)
+			if (desc_pld_len > 0) {
 				rx_len = desc_pld_len;
-
-			if (desc_pld_len > 0 &&
-					(piggy_desc->header & CFHSI_PIGGY_DESC))
-				rx_len += CFHSI_DESC_SZ;
+				if (piggy_desc->header & CFHSI_PIGGY_DESC)
+					rx_len += CFHSI_DESC_SZ;
+			}
 
 			/*
 			 * Copy needed information from the piggy-backed
@@ -693,8 +678,6 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 			 */
 			memcpy(rx_buf, (u8 *)piggy_desc,
 					CFHSI_DESC_SHORT_SZ);
-			if (desc_pld_len == -EPROTO)
-				goto out_of_sync;
 		}
 	}
 
@@ -710,13 +693,13 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 	/* Initiate next read */
 	if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) {
 		/* Set up new transfer. */
-		dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n",
+		netdev_dbg(cfhsi->ndev, "%s: Start RX.\n",
 				__func__);
 
-		res = cfhsi->dev->cfhsi_rx(rx_ptr, rx_len,
-				cfhsi->dev);
+		res = cfhsi->ops->cfhsi_rx(rx_ptr, rx_len,
+				cfhsi->ops);
 		if (WARN_ON(res < 0)) {
-			dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n",
+			netdev_err(cfhsi->ndev, "%s: RX error %d.\n",
 				__func__, res);
 			cfhsi->ndev->stats.rx_errors++;
 			cfhsi->ndev->stats.rx_dropped++;
@@ -753,7 +736,7 @@ static void cfhsi_rx_done(struct cfhsi *cfhsi)
 	return;
 
 out_of_sync:
-	dev_err(&cfhsi->ndev->dev, "%s: Out of sync.\n", __func__);
+	netdev_err(cfhsi->ndev, "%s: Out of sync.\n", __func__);
 	print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE,
 			cfhsi->rx_buf, CFHSI_DESC_SZ);
 	schedule_work(&cfhsi->out_of_sync_work);
@@ -763,18 +746,18 @@ static void cfhsi_rx_slowpath(unsigned long arg)
 {
 	struct cfhsi *cfhsi = (struct cfhsi *)arg;
 
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	cfhsi_rx_done(cfhsi);
 }
 
-static void cfhsi_rx_done_cb(struct cfhsi_drv *drv)
+static void cfhsi_rx_done_cb(struct cfhsi_cb_ops *cb_ops)
 {
 	struct cfhsi *cfhsi;
 
-	cfhsi = container_of(drv, struct cfhsi, drv);
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
@@ -807,9 +790,9 @@ static void cfhsi_wake_up(struct work_struct *work)
 	}
 
 	/* Activate wake line. */
-	cfhsi->dev->cfhsi_wake_up(cfhsi->dev);
+	cfhsi->ops->cfhsi_wake_up(cfhsi->ops);
 
-	dev_dbg(&cfhsi->ndev->dev, "%s: Start waiting.\n",
+	netdev_dbg(cfhsi->ndev, "%s: Start waiting.\n",
 		__func__);
 
 	/* Wait for acknowledge. */
@@ -819,33 +802,33 @@ static void cfhsi_wake_up(struct work_struct *work)
 							&cfhsi->bits), ret);
 	if (unlikely(ret < 0)) {
 		/* Interrupted by signal. */
-		dev_err(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
+		netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n",
 			__func__, ret);
 
 		clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
-		cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
+		cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 		return;
 	} else if (!ret) {
 		bool ca_wake = false;
 		size_t fifo_occupancy = 0;
 
 		/* Wakeup timeout */
-		dev_dbg(&cfhsi->ndev->dev, "%s: Timeout.\n",
+		netdev_dbg(cfhsi->ndev, "%s: Timeout.\n",
 			__func__);
 
 		/* Check FIFO to check if modem has sent something. */
-		WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
+		WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 					&fifo_occupancy));
 
-		dev_dbg(&cfhsi->ndev->dev, "%s: Bytes in FIFO: %u.\n",
+		netdev_dbg(cfhsi->ndev, "%s: Bytes in FIFO: %u.\n",
 				__func__, (unsigned) fifo_occupancy);
 
 		/* Check if we misssed the interrupt. */
-		WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev,
+		WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops,
 							&ca_wake));
 
 		if (ca_wake) {
-			dev_err(&cfhsi->ndev->dev, "%s: CA Wake missed !.\n",
+			netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n",
 				__func__);
 
 			/* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */
@@ -856,11 +839,11 @@ static void cfhsi_wake_up(struct work_struct *work)
 		}
 
 		clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
-		cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
+		cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 		return;
 	}
 wake_ack:
-	dev_dbg(&cfhsi->ndev->dev, "%s: Woken.\n",
+	netdev_dbg(cfhsi->ndev, "%s: Woken.\n",
 		__func__);
 
 	/* Clear power up bit. */
@@ -868,11 +851,11 @@ wake_ack:
 	clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
 
 	/* Resume read operation. */
-	dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n", __func__);
-	res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->dev);
+	netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", __func__);
+	res = cfhsi->ops->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->ops);
 
 	if (WARN_ON(res < 0))
-		dev_err(&cfhsi->ndev->dev, "%s: RX err %d.\n", __func__, res);
+		netdev_err(cfhsi->ndev, "%s: RX err %d.\n", __func__, res);
 
 	/* Clear power up acknowledment. */
 	clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
@@ -881,16 +864,16 @@ wake_ack:
 
 	/* Resume transmit if queues are not empty. */
 	if (!cfhsi_tx_queue_len(cfhsi)) {
-		dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n",
+		netdev_dbg(cfhsi->ndev, "%s: Peer wake, start timer.\n",
 			__func__);
 		/* Start inactivity timer. */
 		mod_timer(&cfhsi->inactivity_timer,
-				jiffies + cfhsi->inactivity_timeout);
+				jiffies + cfhsi->cfg.inactivity_timeout);
 		spin_unlock_bh(&cfhsi->lock);
 		return;
 	}
 
-	dev_dbg(&cfhsi->ndev->dev, "%s: Host wake.\n",
+	netdev_dbg(cfhsi->ndev, "%s: Host wake.\n",
 		__func__);
 
 	spin_unlock_bh(&cfhsi->lock);
@@ -900,14 +883,14 @@ wake_ack:
 
 	if (likely(len > 0)) {
 		/* Set up new transfer. */
-		res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
+		res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
 		if (WARN_ON(res < 0)) {
-			dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
+			netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
 				__func__, res);
 			cfhsi_abort_tx(cfhsi);
 		}
 	} else {
-		dev_err(&cfhsi->ndev->dev,
+		netdev_err(cfhsi->ndev,
 				"%s: Failed to create HSI frame: %d.\n",
 				__func__, len);
 	}
@@ -921,13 +904,13 @@ static void cfhsi_wake_down(struct work_struct *work)
 	int retry = CFHSI_WAKE_TOUT;
 
 	cfhsi = container_of(work, struct cfhsi, wake_down_work);
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
+	netdev_dbg(cfhsi->ndev, "%s.\n", __func__);
 
 	if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
 		return;
 
 	/* Deactivate wake line. */
-	cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
+	cfhsi->ops->cfhsi_wake_down(cfhsi->ops);
 
 	/* Wait for acknowledge. */
 	ret = CFHSI_WAKE_TOUT;
@@ -936,26 +919,26 @@ static void cfhsi_wake_down(struct work_struct *work)
 							&cfhsi->bits), ret);
 	if (ret < 0) {
 		/* Interrupted by signal. */
-		dev_err(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
+		netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n",
 			__func__, ret);
 		return;
 	} else if (!ret) {
 		bool ca_wake = true;
 
 		/* Timeout */
-		dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n", __func__);
+		netdev_err(cfhsi->ndev, "%s: Timeout.\n", __func__);
 
 		/* Check if we misssed the interrupt. */
-		WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev,
+		WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops,
 							&ca_wake));
 		if (!ca_wake)
-			dev_err(&cfhsi->ndev->dev, "%s: CA Wake missed !.\n",
+			netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n",
 				__func__);
 	}
 
 	/* Check FIFO occupancy. */
 	while (retry) {
-		WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
+		WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops,
 							&fifo_occupancy));
 
 		if (!fifo_occupancy)
@@ -967,14 +950,13 @@ static void cfhsi_wake_down(struct work_struct *work)
 	}
 
 	if (!retry)
-		dev_err(&cfhsi->ndev->dev, "%s: FIFO Timeout.\n", __func__);
+		netdev_err(cfhsi->ndev, "%s: FIFO Timeout.\n", __func__);
 
 	/* Clear AWAKE condition. */
 	clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 
 	/* Cancel pending RX requests. */
-	cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
-
+	cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops);
 }
 
 static void cfhsi_out_of_sync(struct work_struct *work)
@@ -988,12 +970,12 @@ static void cfhsi_out_of_sync(struct work_struct *work)
 	rtnl_unlock();
 }
 
-static void cfhsi_wake_up_cb(struct cfhsi_drv *drv)
+static void cfhsi_wake_up_cb(struct cfhsi_cb_ops *cb_ops)
 {
 	struct cfhsi *cfhsi = NULL;
 
-	cfhsi = container_of(drv, struct cfhsi, drv);
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
@@ -1007,12 +989,12 @@ static void cfhsi_wake_up_cb(struct cfhsi_drv *drv)
 		queue_work(cfhsi->wq, &cfhsi->wake_up_work);
 }
 
-static void cfhsi_wake_down_cb(struct cfhsi_drv *drv)
+static void cfhsi_wake_down_cb(struct cfhsi_cb_ops *cb_ops)
 {
 	struct cfhsi *cfhsi = NULL;
 
-	cfhsi = container_of(drv, struct cfhsi, drv);
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	cfhsi = container_of(cb_ops, struct cfhsi, cb_ops);
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	/* Initiating low power is only permitted by the host (us). */
@@ -1024,7 +1006,7 @@ static void cfhsi_aggregation_tout(unsigned long arg)
 {
 	struct cfhsi *cfhsi = (struct cfhsi *)arg;
 
-	dev_dbg(&cfhsi->ndev->dev, "%s.\n",
+	netdev_dbg(cfhsi->ndev, "%s.\n",
 		__func__);
 
 	cfhsi_start_tx(cfhsi);
@@ -1077,7 +1059,7 @@ static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Send flow off if number of packets is above high water mark. */
 	if (!cfhsi->flow_off_sent &&
-		cfhsi_tx_queue_len(cfhsi) > cfhsi->q_high_mark &&
+		cfhsi_tx_queue_len(cfhsi) > cfhsi->cfg.q_high_mark &&
 		cfhsi->cfdev.flowctrl) {
 		cfhsi->flow_off_sent = 1;
 		cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
@@ -1114,9 +1096,9 @@ static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 		WARN_ON(!len);
 
 		/* Set up new transfer. */
-		res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
+		res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops);
 		if (WARN_ON(res < 0)) {
-			dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
+			netdev_err(cfhsi->ndev, "%s: TX error %d.\n",
 				__func__, res);
 			cfhsi_abort_tx(cfhsi);
 		}
@@ -1129,19 +1111,19 @@ static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
 	return 0;
 }
 
-static const struct net_device_ops cfhsi_ops;
+static const struct net_device_ops cfhsi_netdevops;
 
 static void cfhsi_setup(struct net_device *dev)
 {
 	int i;
 	struct cfhsi *cfhsi = netdev_priv(dev);
 	dev->features = 0;
-	dev->netdev_ops = &cfhsi_ops;
 	dev->type = ARPHRD_CAIF;
 	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
 	dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ;
 	dev->tx_queue_len = 0;
 	dev->destructor = free_netdev;
+	dev->netdev_ops = &cfhsi_netdevops;
 	for (i = 0; i < CFHSI_PRIO_LAST; ++i)
 		skb_queue_head_init(&cfhsi->qhead[i]);
 	cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
@@ -1149,43 +1131,7 @@ static void cfhsi_setup(struct net_device *dev)
 	cfhsi->cfdev.use_stx = false;
 	cfhsi->cfdev.use_fcs = false;
 	cfhsi->ndev = dev;
-}
-
-int cfhsi_probe(struct platform_device *pdev)
-{
-	struct cfhsi *cfhsi = NULL;
-	struct net_device *ndev;
-
-	int res;
-
-	ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup);
-	if (!ndev)
-		return -ENODEV;
-
-	cfhsi = netdev_priv(ndev);
-	cfhsi->ndev = ndev;
-	cfhsi->pdev = pdev;
-
-	/* Assign the HSI device. */
-	cfhsi->dev = pdev->dev.platform_data;
-
-	/* Assign the driver to this HSI device. */
-	cfhsi->dev->drv = &cfhsi->drv;
-
-	/* Register network device. */
-	res = register_netdev(ndev);
-	if (res) {
-		dev_err(&ndev->dev, "%s: Registration error: %d.\n",
-			__func__, res);
-		free_netdev(ndev);
-		return -ENODEV;
-	}
-	/* Add CAIF HSI device to list. */
-	spin_lock(&cfhsi_list_lock);
-	list_add_tail(&cfhsi->list, &cfhsi_list);
-	spin_unlock(&cfhsi_list_lock);
-
-	return res;
+	cfhsi->cfg = hsi_default_config;
 }
 
 static int cfhsi_open(struct net_device *ndev)
@@ -1201,9 +1147,6 @@ static int cfhsi_open(struct net_device *ndev)
 
 	/* Set flow info */
 	cfhsi->flow_off_sent = 0;
-	cfhsi->q_low_mark = LOW_WATER_MARK;
-	cfhsi->q_high_mark = HIGH_WATER_MARK;
-
 
 	/*
 	 * Allocate a TX buffer with the size of a HSI packet descriptors
@@ -1231,20 +1174,8 @@ static int cfhsi_open(struct net_device *ndev)
 		goto err_alloc_rx_flip;
 	}
 
-	/* Pre-calculate inactivity timeout. */
-	if (inactivity_timeout != -1) {
-		cfhsi->inactivity_timeout =
-				inactivity_timeout * HZ / 1000;
-		if (!cfhsi->inactivity_timeout)
-			cfhsi->inactivity_timeout = 1;
-		else if (cfhsi->inactivity_timeout > NEXT_TIMER_MAX_DELTA)
-			cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA;
-	} else {
-		cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA;
-	}
-
 	/* Initialize aggregation timeout */
-	cfhsi->aggregation_timeout = aggregation_timeout;
+	cfhsi->cfg.aggregation_timeout = hsi_default_config.aggregation_timeout;
 
 	/* Initialize recieve vaiables. */
 	cfhsi->rx_ptr = cfhsi->rx_buf;
@@ -1254,10 +1185,10 @@ static int cfhsi_open(struct net_device *ndev)
 	spin_lock_init(&cfhsi->lock);
 
 	/* Set up the driver. */
-	cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb;
-	cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb;
-	cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb;
-	cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb;
+	cfhsi->cb_ops.tx_done_cb = cfhsi_tx_done_cb;
+	cfhsi->cb_ops.rx_done_cb = cfhsi_rx_done_cb;
+	cfhsi->cb_ops.wake_up_cb = cfhsi_wake_up_cb;
+	cfhsi->cb_ops.wake_down_cb = cfhsi_wake_down_cb;
 
 	/* Initialize the work queues. */
 	INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up);
@@ -1271,9 +1202,9 @@ static int cfhsi_open(struct net_device *ndev)
 	clear_bit(CFHSI_AWAKE, &cfhsi->bits);
 
 	/* Create work thread. */
-	cfhsi->wq = create_singlethread_workqueue(cfhsi->pdev->name);
+	cfhsi->wq = create_singlethread_workqueue(cfhsi->ndev->name);
 	if (!cfhsi->wq) {
-		dev_err(&cfhsi->ndev->dev, "%s: Failed to create work queue.\n",
+		netdev_err(cfhsi->ndev, "%s: Failed to create work queue.\n",
 			__func__);
 		res = -ENODEV;
 		goto err_create_wq;
@@ -1298,9 +1229,9 @@ static int cfhsi_open(struct net_device *ndev)
 	cfhsi->aggregation_timer.function = cfhsi_aggregation_tout;
 
 	/* Activate HSI interface. */
-	res = cfhsi->dev->cfhsi_up(cfhsi->dev);
+	res = cfhsi->ops->cfhsi_up(cfhsi->ops);
 	if (res) {
-		dev_err(&cfhsi->ndev->dev,
+		netdev_err(cfhsi->ndev,
 			"%s: can't activate HSI interface: %d.\n",
 			__func__, res);
 		goto err_activate;
@@ -1309,14 +1240,14 @@ static int cfhsi_open(struct net_device *ndev)
 	/* Flush FIFO */
 	res = cfhsi_flush_fifo(cfhsi);
 	if (res) {
-		dev_err(&cfhsi->ndev->dev, "%s: Can't flush FIFO: %d.\n",
+		netdev_err(cfhsi->ndev, "%s: Can't flush FIFO: %d.\n",
 			__func__, res);
 		goto err_net_reg;
 	}
 	return res;
 
  err_net_reg:
-	cfhsi->dev->cfhsi_down(cfhsi->dev);
+	cfhsi->ops->cfhsi_down(cfhsi->ops);
  err_activate:
 	destroy_workqueue(cfhsi->wq);
  err_create_wq:
@@ -1346,7 +1277,7 @@ static int cfhsi_close(struct net_device *ndev)
 	del_timer_sync(&cfhsi->aggregation_timer);
 
 	/* Cancel pending RX request (if any) */
-	cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
+	cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops);
 
 	/* Destroy workqueue */
 	destroy_workqueue(cfhsi->wq);
@@ -1359,7 +1290,7 @@ static int cfhsi_close(struct net_device *ndev)
 	cfhsi_abort_tx(cfhsi);
 
 	/* Deactivate interface */
-	cfhsi->dev->cfhsi_down(cfhsi->dev);
+	cfhsi->ops->cfhsi_down(cfhsi->ops);
 
 	/* Free buffers. */
 	kfree(tx_buf);
@@ -1368,85 +1299,184 @@ static int cfhsi_close(struct net_device *ndev)
 	return 0;
 }
 
-static const struct net_device_ops cfhsi_ops = {
+static void cfhsi_uninit(struct net_device *dev)
+{
+	struct cfhsi *cfhsi = netdev_priv(dev);
+	ASSERT_RTNL();
+	symbol_put(cfhsi_get_device);
+	list_del(&cfhsi->list);
+}
+
+static const struct net_device_ops cfhsi_netdevops = {
+	.ndo_uninit = cfhsi_uninit,
 	.ndo_open = cfhsi_open,
 	.ndo_stop = cfhsi_close,
 	.ndo_start_xmit = cfhsi_xmit
 };
 
-int cfhsi_remove(struct platform_device *pdev)
+static void cfhsi_netlink_parms(struct nlattr *data[], struct cfhsi *cfhsi)
 {
-	struct list_head *list_node;
-	struct list_head *n;
-	struct cfhsi *cfhsi = NULL;
-	struct cfhsi_dev *dev;
+	int i;
 
-	dev = (struct cfhsi_dev *)pdev->dev.platform_data;
-	spin_lock(&cfhsi_list_lock);
-	list_for_each_safe(list_node, n, &cfhsi_list) {
-		cfhsi = list_entry(list_node, struct cfhsi, list);
-		/* Find the corresponding device. */
-		if (cfhsi->dev == dev) {
-			/* Remove from list. */
-			list_del(list_node);
-			spin_unlock(&cfhsi_list_lock);
-			return 0;
-		}
+	if (!data) {
+		pr_debug("no params data found\n");
+		return;
 	}
-	spin_unlock(&cfhsi_list_lock);
-	return -ENODEV;
+
+	i = __IFLA_CAIF_HSI_INACTIVITY_TOUT;
+	/*
+	 * Inactivity timeout in millisecs. Lowest possible value is 1,
+	 * and highest possible is NEXT_TIMER_MAX_DELTA.
+	 */
+	if (data[i]) {
+		u32 inactivity_timeout = nla_get_u32(data[i]);
+		/* Pre-calculate inactivity timeout. */
+		cfhsi->cfg.inactivity_timeout =	inactivity_timeout * HZ / 1000;
+		if (cfhsi->cfg.inactivity_timeout == 0)
+			cfhsi->cfg.inactivity_timeout = 1;
+		else if (cfhsi->cfg.inactivity_timeout > NEXT_TIMER_MAX_DELTA)
+			cfhsi->cfg.inactivity_timeout = NEXT_TIMER_MAX_DELTA;
+	}
+
+	i = __IFLA_CAIF_HSI_AGGREGATION_TOUT;
+	if (data[i])
+		cfhsi->cfg.aggregation_timeout = nla_get_u32(data[i]);
+
+	i = __IFLA_CAIF_HSI_HEAD_ALIGN;
+	if (data[i])
+		cfhsi->cfg.head_align = nla_get_u32(data[i]);
+
+	i = __IFLA_CAIF_HSI_TAIL_ALIGN;
+	if (data[i])
+		cfhsi->cfg.tail_align = nla_get_u32(data[i]);
+
+	i = __IFLA_CAIF_HSI_QHIGH_WATERMARK;
+	if (data[i])
+		cfhsi->cfg.q_high_mark = nla_get_u32(data[i]);
+
+	i = __IFLA_CAIF_HSI_QLOW_WATERMARK;
+	if (data[i])
+		cfhsi->cfg.q_low_mark = nla_get_u32(data[i]);
+}
+
+static int caif_hsi_changelink(struct net_device *dev, struct nlattr *tb[],
+				struct nlattr *data[])
+{
+	cfhsi_netlink_parms(data, netdev_priv(dev));
+	netdev_state_change(dev);
+	return 0;
 }
 
-struct platform_driver cfhsi_plat_drv = {
-	.probe = cfhsi_probe,
-	.remove = cfhsi_remove,
-	.driver = {
-		   .name = "cfhsi",
-		   .owner = THIS_MODULE,
-		   },
+static const struct nla_policy caif_hsi_policy[__IFLA_CAIF_HSI_MAX + 1] = {
+	[__IFLA_CAIF_HSI_INACTIVITY_TOUT] = { .type = NLA_U32, .len = 4 },
+	[__IFLA_CAIF_HSI_AGGREGATION_TOUT] = { .type = NLA_U32, .len = 4 },
+	[__IFLA_CAIF_HSI_HEAD_ALIGN] = { .type = NLA_U32, .len = 4 },
+	[__IFLA_CAIF_HSI_TAIL_ALIGN] = { .type = NLA_U32, .len = 4 },
+	[__IFLA_CAIF_HSI_QHIGH_WATERMARK] = { .type = NLA_U32, .len = 4 },
+	[__IFLA_CAIF_HSI_QLOW_WATERMARK] = { .type = NLA_U32, .len = 4 },
 };
 
-static void __exit cfhsi_exit_module(void)
+static size_t caif_hsi_get_size(const struct net_device *dev)
+{
+	int i;
+	size_t s = 0;
+	for (i = __IFLA_CAIF_HSI_UNSPEC + 1; i < __IFLA_CAIF_HSI_MAX; i++)
+		s += nla_total_size(caif_hsi_policy[i].len);
+	return s;
+}
+
+static int caif_hsi_fill_info(struct sk_buff *skb, const struct net_device *dev)
+{
+	struct cfhsi *cfhsi = netdev_priv(dev);
+
+	if (nla_put_u32(skb, __IFLA_CAIF_HSI_INACTIVITY_TOUT,
+			cfhsi->cfg.inactivity_timeout) ||
+	    nla_put_u32(skb, __IFLA_CAIF_HSI_AGGREGATION_TOUT,
+			cfhsi->cfg.aggregation_timeout) ||
+	    nla_put_u32(skb, __IFLA_CAIF_HSI_HEAD_ALIGN,
+			cfhsi->cfg.head_align) ||
+	    nla_put_u32(skb, __IFLA_CAIF_HSI_TAIL_ALIGN,
+			cfhsi->cfg.tail_align) ||
+	    nla_put_u32(skb, __IFLA_CAIF_HSI_QHIGH_WATERMARK,
+			cfhsi->cfg.q_high_mark) ||
+	    nla_put_u32(skb, __IFLA_CAIF_HSI_QLOW_WATERMARK,
+			cfhsi->cfg.q_low_mark))
+		return -EMSGSIZE;
+
+	return 0;
+}
+
+static int caif_hsi_newlink(struct net *src_net, struct net_device *dev,
+			  struct nlattr *tb[], struct nlattr *data[])
 {
-	struct list_head *list_node;
-	struct list_head *n;
 	struct cfhsi *cfhsi = NULL;
+	struct cfhsi_ops *(*get_ops)(void);
 
-	spin_lock(&cfhsi_list_lock);
-	list_for_each_safe(list_node, n, &cfhsi_list) {
-		cfhsi = list_entry(list_node, struct cfhsi, list);
+	ASSERT_RTNL();
 
-		/* Remove from list. */
-		list_del(list_node);
-		spin_unlock(&cfhsi_list_lock);
+	cfhsi = netdev_priv(dev);
+	cfhsi_netlink_parms(data, cfhsi);
+	dev_net_set(cfhsi->ndev, src_net);
+
+	get_ops = symbol_get(cfhsi_get_ops);
+	if (!get_ops) {
+		pr_err("%s: failed to get the cfhsi_ops\n", __func__);
+		return -ENODEV;
+	}
 
-		unregister_netdevice(cfhsi->ndev);
+	/* Assign the HSI device. */
+	cfhsi->ops = (*get_ops)();
+	if (!cfhsi->ops) {
+		pr_err("%s: failed to get the cfhsi_ops\n", __func__);
+		goto err;
+	}
 
-		spin_lock(&cfhsi_list_lock);
+	/* Assign the driver to this HSI device. */
+	cfhsi->ops->cb_ops = &cfhsi->cb_ops;
+	if (register_netdevice(dev)) {
+		pr_warn("%s: caif_hsi device registration failed\n", __func__);
+		goto err;
 	}
-	spin_unlock(&cfhsi_list_lock);
+	/* Add CAIF HSI device to list. */
+	list_add_tail(&cfhsi->list, &cfhsi_list);
 
-	/* Unregister platform driver. */
-	platform_driver_unregister(&cfhsi_plat_drv);
+	return 0;
+err:
+	symbol_put(cfhsi_get_ops);
+	return -ENODEV;
 }
 
-static int __init cfhsi_init_module(void)
+static struct rtnl_link_ops caif_hsi_link_ops __read_mostly = {
+	.kind		= "cfhsi",
+	.priv_size	= sizeof(struct cfhsi),
+	.setup		= cfhsi_setup,
+	.maxtype	= __IFLA_CAIF_HSI_MAX,
+	.policy	= caif_hsi_policy,
+	.newlink	= caif_hsi_newlink,
+	.changelink	= caif_hsi_changelink,
+	.get_size	= caif_hsi_get_size,
+	.fill_info	= caif_hsi_fill_info,
+};
+
+static void __exit cfhsi_exit_module(void)
 {
-	int result;
+	struct list_head *list_node;
+	struct list_head *n;
+	struct cfhsi *cfhsi;
 
-	/* Initialize spin lock. */
-	spin_lock_init(&cfhsi_list_lock);
+	rtnl_link_unregister(&caif_hsi_link_ops);
 
-	/* Register platform driver. */
-	result = platform_driver_register(&cfhsi_plat_drv);
-	if (result) {
-		printk(KERN_ERR "Could not register platform HSI driver: %d.\n",
-			result);
-		goto err_dev_register;
+	rtnl_lock();
+	list_for_each_safe(list_node, n, &cfhsi_list) {
+		cfhsi = list_entry(list_node, struct cfhsi, list);
+		unregister_netdev(cfhsi->ndev);
 	}
+	rtnl_unlock();
+}
 
- err_dev_register:
-	return result;
+static int __init cfhsi_init_module(void)
+{
+	return rtnl_link_register(&caif_hsi_link_ops);
 }
 
 module_init(cfhsi_init_module);
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index 8a3054b8481..5de74e76202 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -325,6 +325,9 @@ static int ldisc_open(struct tty_struct *tty)
 
 	sprintf(name, "cf%s", tty->name);
 	dev = alloc_netdev(sizeof(*ser), name, caifdev_setup);
+	if (!dev)
+		return -ENOMEM;
+
 	ser = netdev_priv(dev);
 	ser->tty = tty_kref_get(tty);
 	ser->dev = dev;
diff --git a/drivers/net/can/at91_can.c b/drivers/net/can/at91_can.c
index 6ea905c2cf6..fcff73a73b1 100644
--- a/drivers/net/can/at91_can.c
+++ b/drivers/net/can/at91_can.c
@@ -170,7 +170,7 @@ static const struct at91_devtype_data at91_devtype_data[] __devinitconst = {
 	},
 };
 
-static struct can_bittiming_const at91_bittiming_const = {
+static const struct can_bittiming_const at91_bittiming_const = {
 	.name		= KBUILD_MODNAME,
 	.tseg1_min	= 4,
 	.tseg1_max	= 16,
diff --git a/drivers/net/can/bfin_can.c b/drivers/net/can/bfin_can.c
index 3f88473423e..f2d6d258a28 100644
--- a/drivers/net/can/bfin_can.c
+++ b/drivers/net/can/bfin_can.c
@@ -44,7 +44,7 @@ struct bfin_can_priv {
 /*
  * bfin can timing parameters
  */
-static struct can_bittiming_const bfin_can_bittiming_const = {
+static const struct can_bittiming_const bfin_can_bittiming_const = {
 	.name = DRV_NAME,
 	.tseg1_min = 1,
 	.tseg1_max = 16,
@@ -597,7 +597,7 @@ static int __devinit bfin_can_probe(struct platform_device *pdev)
 	dev_info(&pdev->dev,
 		"%s device registered"
 		"(&reg_base=%p, rx_irq=%d, tx_irq=%d, err_irq=%d, sclk=%d)\n",
-		DRV_NAME, (void *)priv->membase, priv->rx_irq,
+		DRV_NAME, priv->membase, priv->rx_irq,
 		priv->tx_irq, priv->err_irq, priv->can.clock.freq);
 	return 0;
 
diff --git a/drivers/net/can/c_can/Kconfig b/drivers/net/can/c_can/Kconfig
index ffb9773d102..3b83bafcd94 100644
--- a/drivers/net/can/c_can/Kconfig
+++ b/drivers/net/can/c_can/Kconfig
@@ -1,15 +1,23 @@
 menuconfig CAN_C_CAN
-	tristate "Bosch C_CAN devices"
+	tristate "Bosch C_CAN/D_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"
+	tristate "Generic Platform Bus based C_CAN/D_CAN driver"
 	---help---
-	  This driver adds support for the C_CAN chips connected to
-	  the "platform bus" (Linux abstraction for directly to the
+	  This driver adds support for the C_CAN/D_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.
+	  boards from ST Microelectronics (http://www.st.com) like the
+	  SPEAr1310 and SPEAr320 evaluation boards & TI (www.ti.com)
+	  boards like am335x, dm814x, dm813x and dm811x.
+
+config CAN_C_CAN_PCI
+	tristate "Generic PCI Bus based C_CAN/D_CAN driver"
+	depends on PCI
+	---help---
+	  This driver adds support for the C_CAN/D_CAN chips connected
+	  to the PCI bus.
 endif
diff --git a/drivers/net/can/c_can/Makefile b/drivers/net/can/c_can/Makefile
index 9273f6d5c4b..ad1cc842170 100644
--- a/drivers/net/can/c_can/Makefile
+++ b/drivers/net/can/c_can/Makefile
@@ -4,5 +4,6 @@
 
 obj-$(CONFIG_CAN_C_CAN) += c_can.o
 obj-$(CONFIG_CAN_C_CAN_PLATFORM) += c_can_platform.o
+obj-$(CONFIG_CAN_C_CAN_PCI) += c_can_pci.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
index 86cd532c78f..4c538e38865 100644
--- a/drivers/net/can/c_can/c_can.c
+++ b/drivers/net/can/c_can/c_can.c
@@ -41,6 +41,10 @@
 
 #include "c_can.h"
 
+/* Number of interface registers */
+#define IF_ENUM_REG_LEN		11
+#define C_CAN_IFACE(reg, iface)	(C_CAN_IF1_##reg + (iface) * IF_ENUM_REG_LEN)
+
 /* control register */
 #define CONTROL_TEST		BIT(7)
 #define CONTROL_CCE		BIT(6)
@@ -185,7 +189,7 @@ enum c_can_bus_error_types {
 	C_CAN_ERROR_PASSIVE,
 };
 
-static struct can_bittiming_const c_can_bittiming_const = {
+static const struct can_bittiming_const c_can_bittiming_const = {
 	.name = KBUILD_MODNAME,
 	.tseg1_min = 2,		/* Time segment 1 = prop_seg + phase_seg1 */
 	.tseg1_max = 16,
@@ -209,10 +213,10 @@ static inline int get_tx_echo_msg_obj(const struct c_can_priv *priv)
 			C_CAN_MSG_OBJ_TX_FIRST;
 }
 
-static u32 c_can_read_reg32(struct c_can_priv *priv, void *reg)
+static u32 c_can_read_reg32(struct c_can_priv *priv, enum reg index)
 {
-	u32 val = priv->read_reg(priv, reg);
-	val |= ((u32) priv->read_reg(priv, reg + 2)) << 16;
+	u32 val = priv->read_reg(priv, index);
+	val |= ((u32) priv->read_reg(priv, index + 1)) << 16;
 	return val;
 }
 
@@ -220,14 +224,14 @@ 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);
+						C_CAN_CTRL_REG);
 
 	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);
+	priv->write_reg(priv, C_CAN_CTRL_REG, cntrl_save);
 }
 
 static inline int c_can_msg_obj_is_busy(struct c_can_priv *priv, int iface)
@@ -235,7 +239,7 @@ 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) &
+				C_CAN_IFACE(COMREQ_REG, iface)) &
 				IF_COMR_BUSY) {
 		count--;
 		udelay(1);
@@ -258,9 +262,9 @@ static inline void c_can_object_get(struct net_device *dev,
 	 * register and message RAM must be complete in 6 CAN-CLK
 	 * period.
 	 */
-	priv->write_reg(priv, &priv->regs->ifregs[iface].com_mask,
+	priv->write_reg(priv, C_CAN_IFACE(COMMSK_REG, iface),
 			IFX_WRITE_LOW_16BIT(mask));
-	priv->write_reg(priv, &priv->regs->ifregs[iface].com_req,
+	priv->write_reg(priv, C_CAN_IFACE(COMREQ_REG, iface),
 			IFX_WRITE_LOW_16BIT(objno));
 
 	if (c_can_msg_obj_is_busy(priv, iface))
@@ -278,9 +282,9 @@ static inline void c_can_object_put(struct net_device *dev,
 	 * register and message RAM must be complete in 6 CAN-CLK
 	 * period.
 	 */
-	priv->write_reg(priv, &priv->regs->ifregs[iface].com_mask,
+	priv->write_reg(priv, C_CAN_IFACE(COMMSK_REG, iface),
 			(IF_COMM_WR | IFX_WRITE_LOW_16BIT(mask)));
-	priv->write_reg(priv, &priv->regs->ifregs[iface].com_req,
+	priv->write_reg(priv, C_CAN_IFACE(COMREQ_REG, iface),
 			IFX_WRITE_LOW_16BIT(objno));
 
 	if (c_can_msg_obj_is_busy(priv, iface))
@@ -306,18 +310,18 @@ static void c_can_write_msg_object(struct net_device *dev,
 
 	flags |= IF_ARB_MSGVAL;
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].arb1,
+	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface),
 				IFX_WRITE_LOW_16BIT(id));
-	priv->write_reg(priv, &priv->regs->ifregs[iface].arb2, flags |
+	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), 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],
+		priv->write_reg(priv, C_CAN_IFACE(DATA1_REG, iface) + 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,
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
 			IF_MCONT_TXIE | IF_MCONT_TXRQST | IF_MCONT_EOB |
 			frame->can_dlc);
 	c_can_object_put(dev, iface, objno, IF_COMM_ALL);
@@ -329,7 +333,7 @@ static inline void c_can_mark_rx_msg_obj(struct net_device *dev,
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
 			ctrl_mask & ~(IF_MCONT_MSGLST | IF_MCONT_INTPND));
 	c_can_object_put(dev, iface, obj, IF_COMM_CONTROL);
 
@@ -343,7 +347,7 @@ static inline void c_can_activate_all_lower_rx_msg_obj(struct net_device *dev,
 	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,
+		priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
 				ctrl_mask & ~(IF_MCONT_MSGLST |
 					IF_MCONT_INTPND | IF_MCONT_NEWDAT));
 		c_can_object_put(dev, iface, i, IF_COMM_CONTROL);
@@ -356,7 +360,7 @@ static inline void c_can_activate_rx_msg_obj(struct net_device *dev,
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
 			ctrl_mask & ~(IF_MCONT_MSGLST |
 				IF_MCONT_INTPND | IF_MCONT_NEWDAT));
 	c_can_object_put(dev, iface, obj, IF_COMM_CONTROL);
@@ -374,7 +378,7 @@ static void c_can_handle_lost_msg_obj(struct net_device *dev,
 
 	c_can_object_get(dev, iface, objno, IF_COMM_ALL & ~IF_COMM_TXRQST);
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl,
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
 			IF_MCONT_CLR_MSGLST);
 
 	c_can_object_put(dev, 0, objno, IF_COMM_CONTROL);
@@ -410,8 +414,8 @@ static int c_can_read_msg_object(struct net_device *dev, int iface, int ctrl)
 
 	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 =	priv->read_reg(priv, C_CAN_IFACE(ARB2_REG, iface));
+	val = priv->read_reg(priv, C_CAN_IFACE(ARB1_REG, iface)) |
 		(flags << 16);
 
 	if (flags & IF_ARB_MSGXTD)
@@ -424,7 +428,7 @@ static int c_can_read_msg_object(struct net_device *dev, int iface, int ctrl)
 	else {
 		for (i = 0; i < frame->can_dlc; i += 2) {
 			data = priv->read_reg(priv,
-				&priv->regs->ifregs[iface].data[i / 2]);
+				C_CAN_IFACE(DATA1_REG, iface) + i / 2);
 			frame->data[i] = data;
 			frame->data[i + 1] = data >> 8;
 		}
@@ -444,40 +448,40 @@ static void c_can_setup_receive_object(struct net_device *dev, int iface,
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].mask1,
+	priv->write_reg(priv, C_CAN_IFACE(MASK1_REG, iface),
 			IFX_WRITE_LOW_16BIT(mask));
-	priv->write_reg(priv, &priv->regs->ifregs[iface].mask2,
+	priv->write_reg(priv, C_CAN_IFACE(MASK2_REG, iface),
 			IFX_WRITE_HIGH_16BIT(mask));
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].arb1,
+	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface),
 			IFX_WRITE_LOW_16BIT(id));
-	priv->write_reg(priv, &priv->regs->ifregs[iface].arb2,
+	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface),
 			(IF_ARB_MSGVAL | IFX_WRITE_HIGH_16BIT(id)));
 
-	priv->write_reg(priv, &priv->regs->ifregs[iface].msg_cntrl, mcont);
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), 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));
+			c_can_read_reg32(priv, C_CAN_MSGVAL1_REG));
 }
 
 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);
+	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface), 0);
+	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), 0);
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), 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));
+			c_can_read_reg32(priv, C_CAN_MSGVAL1_REG));
 }
 
 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);
+	int val = c_can_read_reg32(priv, C_CAN_TXRQST1_REG);
 
 	/*
 	 * as transmission request register's bit n-1 corresponds to
@@ -540,12 +544,12 @@ static int c_can_set_bittiming(struct net_device *dev)
 	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 = priv->read_reg(priv, C_CAN_CTRL_REG);
+	priv->write_reg(priv, C_CAN_CTRL_REG,
 			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);
+	priv->write_reg(priv, C_CAN_BTR_REG, reg_btr);
+	priv->write_reg(priv, C_CAN_BRPEXT_REG, reg_brpe);
+	priv->write_reg(priv, C_CAN_CTRL_REG, ctrl_save);
 
 	return 0;
 }
@@ -587,36 +591,36 @@ static void c_can_chip_config(struct net_device *dev)
 	struct c_can_priv *priv = netdev_priv(dev);
 
 	/* enable automatic retransmission */
-	priv->write_reg(priv, &priv->regs->control,
+	priv->write_reg(priv, C_CAN_CTRL_REG,
 			CONTROL_ENABLE_AR);
 
 	if ((priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) &&
 	    (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK)) {
 		/* loopback + silent mode : useful for hot self-test */
-		priv->write_reg(priv, &priv->regs->control, CONTROL_EIE |
+		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_EIE |
 				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
-		priv->write_reg(priv, &priv->regs->test,
+		priv->write_reg(priv, C_CAN_TEST_REG,
 				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 |
+		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_EIE |
 				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
-		priv->write_reg(priv, &priv->regs->test, TEST_LBACK);
+		priv->write_reg(priv, C_CAN_TEST_REG, TEST_LBACK);
 	} else if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 		/* silent mode : bus-monitoring mode */
-		priv->write_reg(priv, &priv->regs->control, CONTROL_EIE |
+		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_EIE |
 				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
-		priv->write_reg(priv, &priv->regs->test, TEST_SILENT);
+		priv->write_reg(priv, C_CAN_TEST_REG, TEST_SILENT);
 	} else
 		/* normal mode*/
-		priv->write_reg(priv, &priv->regs->control,
+		priv->write_reg(priv, C_CAN_CTRL_REG,
 				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);
+	priv->write_reg(priv, C_CAN_STS_REG, LEC_UNUSED);
 
 	/* set bittiming params */
 	c_can_set_bittiming(dev);
@@ -669,7 +673,7 @@ static int c_can_get_berr_counter(const struct net_device *dev,
 	unsigned int reg_err_counter;
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	reg_err_counter = priv->read_reg(priv, &priv->regs->err_cnt);
+	reg_err_counter = priv->read_reg(priv, C_CAN_ERR_CNT_REG);
 	bec->rxerr = (reg_err_counter & ERR_CNT_REC_MASK) >>
 				ERR_CNT_REC_SHIFT;
 	bec->txerr = reg_err_counter & ERR_CNT_TEC_MASK;
@@ -697,12 +701,12 @@ static void c_can_do_tx(struct net_device *dev)
 
 	for (/* nix */; (priv->tx_next - priv->tx_echo) > 0; priv->tx_echo++) {
 		msg_obj_no = get_tx_echo_msg_obj(priv);
-		val = c_can_read_reg32(priv, &priv->regs->txrqst1);
+		val = c_can_read_reg32(priv, C_CAN_TXRQST1_REG);
 		if (!(val & (1 << (msg_obj_no - 1)))) {
 			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)
+					C_CAN_IFACE(MSGCTRL_REG, 0))
 					& IF_MCONT_DLC_MASK;
 			stats->tx_packets++;
 			c_can_inval_msg_object(dev, 0, msg_obj_no);
@@ -744,11 +748,11 @@ 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);
+	u32 val = c_can_read_reg32(priv, C_CAN_INTPND1_REG);
 
 	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),
+			val = c_can_read_reg32(priv, C_CAN_INTPND1_REG),
 			msg_obj++) {
 		/*
 		 * as interrupt pending register's bit n-1 corresponds to
@@ -758,7 +762,7 @@ static int c_can_do_rx_poll(struct net_device *dev, int quota)
 			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);
+					C_CAN_IFACE(MSGCTRL_REG, 0));
 
 			if (msg_ctrl_save & IF_MCONT_EOB)
 				return num_rx_pkts;
@@ -819,7 +823,7 @@ static int c_can_handle_state_change(struct net_device *dev,
 		return 0;
 
 	c_can_get_berr_counter(dev, &bec);
-	reg_err_counter = priv->read_reg(priv, &priv->regs->err_cnt);
+	reg_err_counter = priv->read_reg(priv, C_CAN_ERR_CNT_REG);
 	rx_err_passive = (reg_err_counter & ERR_CNT_RP_MASK) >>
 				ERR_CNT_RP_SHIFT;
 
@@ -935,7 +939,7 @@ static int c_can_handle_bus_err(struct net_device *dev,
 	}
 
 	/* set a `lec` value so that we can check for updates later */
-	priv->write_reg(priv, &priv->regs->status, LEC_UNUSED);
+	priv->write_reg(priv, C_CAN_STS_REG, LEC_UNUSED);
 
 	netif_receive_skb(skb);
 	stats->rx_packets++;
@@ -959,15 +963,15 @@ static int c_can_poll(struct napi_struct *napi, int quota)
 	/* status events have the highest priority */
 	if (irqstatus == STATUS_INTERRUPT) {
 		priv->current_status = priv->read_reg(priv,
-					&priv->regs->status);
+					C_CAN_STS_REG);
 
 		/* handle Tx/Rx events */
 		if (priv->current_status & STATUS_TXOK)
-			priv->write_reg(priv, &priv->regs->status,
+			priv->write_reg(priv, C_CAN_STS_REG,
 					priv->current_status & ~STATUS_TXOK);
 
 		if (priv->current_status & STATUS_RXOK)
-			priv->write_reg(priv, &priv->regs->status,
+			priv->write_reg(priv, C_CAN_STS_REG,
 					priv->current_status & ~STATUS_RXOK);
 
 		/* handle state changes */
@@ -1033,7 +1037,7 @@ static irqreturn_t c_can_isr(int irq, void *dev_id)
 	struct net_device *dev = (struct net_device *)dev_id;
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	priv->irqstatus = priv->read_reg(priv, &priv->regs->interrupt);
+	priv->irqstatus = priv->read_reg(priv, C_CAN_INT_REG);
 	if (!priv->irqstatus)
 		return IRQ_NONE;
 
diff --git a/drivers/net/can/c_can/c_can.h b/drivers/net/can/c_can/c_can.h
index 5f32d34af50..01a7049ab99 100644
--- a/drivers/net/can/c_can/c_can.h
+++ b/drivers/net/can/c_can/c_can.h
@@ -22,43 +22,129 @@
 #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];
+enum reg {
+	C_CAN_CTRL_REG = 0,
+	C_CAN_STS_REG,
+	C_CAN_ERR_CNT_REG,
+	C_CAN_BTR_REG,
+	C_CAN_INT_REG,
+	C_CAN_TEST_REG,
+	C_CAN_BRPEXT_REG,
+	C_CAN_IF1_COMREQ_REG,
+	C_CAN_IF1_COMMSK_REG,
+	C_CAN_IF1_MASK1_REG,
+	C_CAN_IF1_MASK2_REG,
+	C_CAN_IF1_ARB1_REG,
+	C_CAN_IF1_ARB2_REG,
+	C_CAN_IF1_MSGCTRL_REG,
+	C_CAN_IF1_DATA1_REG,
+	C_CAN_IF1_DATA2_REG,
+	C_CAN_IF1_DATA3_REG,
+	C_CAN_IF1_DATA4_REG,
+	C_CAN_IF2_COMREQ_REG,
+	C_CAN_IF2_COMMSK_REG,
+	C_CAN_IF2_MASK1_REG,
+	C_CAN_IF2_MASK2_REG,
+	C_CAN_IF2_ARB1_REG,
+	C_CAN_IF2_ARB2_REG,
+	C_CAN_IF2_MSGCTRL_REG,
+	C_CAN_IF2_DATA1_REG,
+	C_CAN_IF2_DATA2_REG,
+	C_CAN_IF2_DATA3_REG,
+	C_CAN_IF2_DATA4_REG,
+	C_CAN_TXRQST1_REG,
+	C_CAN_TXRQST2_REG,
+	C_CAN_NEWDAT1_REG,
+	C_CAN_NEWDAT2_REG,
+	C_CAN_INTPND1_REG,
+	C_CAN_INTPND2_REG,
+	C_CAN_MSGVAL1_REG,
+	C_CAN_MSGVAL2_REG,
 };
 
-/* 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];
+static const u16 reg_map_c_can[] = {
+	[C_CAN_CTRL_REG]	= 0x00,
+	[C_CAN_STS_REG]		= 0x02,
+	[C_CAN_ERR_CNT_REG]	= 0x04,
+	[C_CAN_BTR_REG]		= 0x06,
+	[C_CAN_INT_REG]		= 0x08,
+	[C_CAN_TEST_REG]	= 0x0A,
+	[C_CAN_BRPEXT_REG]	= 0x0C,
+	[C_CAN_IF1_COMREQ_REG]	= 0x10,
+	[C_CAN_IF1_COMMSK_REG]	= 0x12,
+	[C_CAN_IF1_MASK1_REG]	= 0x14,
+	[C_CAN_IF1_MASK2_REG]	= 0x16,
+	[C_CAN_IF1_ARB1_REG]	= 0x18,
+	[C_CAN_IF1_ARB2_REG]	= 0x1A,
+	[C_CAN_IF1_MSGCTRL_REG]	= 0x1C,
+	[C_CAN_IF1_DATA1_REG]	= 0x1E,
+	[C_CAN_IF1_DATA2_REG]	= 0x20,
+	[C_CAN_IF1_DATA3_REG]	= 0x22,
+	[C_CAN_IF1_DATA4_REG]	= 0x24,
+	[C_CAN_IF2_COMREQ_REG]	= 0x40,
+	[C_CAN_IF2_COMMSK_REG]	= 0x42,
+	[C_CAN_IF2_MASK1_REG]	= 0x44,
+	[C_CAN_IF2_MASK2_REG]	= 0x46,
+	[C_CAN_IF2_ARB1_REG]	= 0x48,
+	[C_CAN_IF2_ARB2_REG]	= 0x4A,
+	[C_CAN_IF2_MSGCTRL_REG]	= 0x4C,
+	[C_CAN_IF2_DATA1_REG]	= 0x4E,
+	[C_CAN_IF2_DATA2_REG]	= 0x50,
+	[C_CAN_IF2_DATA3_REG]	= 0x52,
+	[C_CAN_IF2_DATA4_REG]	= 0x54,
+	[C_CAN_TXRQST1_REG]	= 0x80,
+	[C_CAN_TXRQST2_REG]	= 0x82,
+	[C_CAN_NEWDAT1_REG]	= 0x90,
+	[C_CAN_NEWDAT2_REG]	= 0x92,
+	[C_CAN_INTPND1_REG]	= 0xA0,
+	[C_CAN_INTPND2_REG]	= 0xA2,
+	[C_CAN_MSGVAL1_REG]	= 0xB0,
+	[C_CAN_MSGVAL2_REG]	= 0xB2,
+};
+
+static const u16 reg_map_d_can[] = {
+	[C_CAN_CTRL_REG]	= 0x00,
+	[C_CAN_STS_REG]		= 0x04,
+	[C_CAN_ERR_CNT_REG]	= 0x08,
+	[C_CAN_BTR_REG]		= 0x0C,
+	[C_CAN_BRPEXT_REG]	= 0x0E,
+	[C_CAN_INT_REG]		= 0x10,
+	[C_CAN_TEST_REG]	= 0x14,
+	[C_CAN_TXRQST1_REG]	= 0x88,
+	[C_CAN_TXRQST2_REG]	= 0x8A,
+	[C_CAN_NEWDAT1_REG]	= 0x9C,
+	[C_CAN_NEWDAT2_REG]	= 0x9E,
+	[C_CAN_INTPND1_REG]	= 0xB0,
+	[C_CAN_INTPND2_REG]	= 0xB2,
+	[C_CAN_MSGVAL1_REG]	= 0xC4,
+	[C_CAN_MSGVAL2_REG]	= 0xC6,
+	[C_CAN_IF1_COMREQ_REG]	= 0x100,
+	[C_CAN_IF1_COMMSK_REG]	= 0x102,
+	[C_CAN_IF1_MASK1_REG]	= 0x104,
+	[C_CAN_IF1_MASK2_REG]	= 0x106,
+	[C_CAN_IF1_ARB1_REG]	= 0x108,
+	[C_CAN_IF1_ARB2_REG]	= 0x10A,
+	[C_CAN_IF1_MSGCTRL_REG]	= 0x10C,
+	[C_CAN_IF1_DATA1_REG]	= 0x110,
+	[C_CAN_IF1_DATA2_REG]	= 0x112,
+	[C_CAN_IF1_DATA3_REG]	= 0x114,
+	[C_CAN_IF1_DATA4_REG]	= 0x116,
+	[C_CAN_IF2_COMREQ_REG]	= 0x120,
+	[C_CAN_IF2_COMMSK_REG]	= 0x122,
+	[C_CAN_IF2_MASK1_REG]	= 0x124,
+	[C_CAN_IF2_MASK2_REG]	= 0x126,
+	[C_CAN_IF2_ARB1_REG]	= 0x128,
+	[C_CAN_IF2_ARB2_REG]	= 0x12A,
+	[C_CAN_IF2_MSGCTRL_REG]	= 0x12C,
+	[C_CAN_IF2_DATA1_REG]	= 0x130,
+	[C_CAN_IF2_DATA2_REG]	= 0x132,
+	[C_CAN_IF2_DATA3_REG]	= 0x134,
+	[C_CAN_IF2_DATA4_REG]	= 0x136,
+};
+
+enum c_can_dev_id {
+	C_CAN_DEVTYPE,
+	D_CAN_DEVTYPE,
 };
 
 /* c_can private data structure */
@@ -69,9 +155,10 @@ struct c_can_priv {
 	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;
+	u16 (*read_reg) (struct c_can_priv *priv, enum reg index);
+	void (*write_reg) (struct c_can_priv *priv, enum reg index, u16 val);
+	void __iomem *base;
+	const u16 *regs;
 	unsigned long irq_flags; /* for request_irq() */
 	unsigned int tx_next;
 	unsigned int tx_echo;
diff --git a/drivers/net/can/c_can/c_can_pci.c b/drivers/net/can/c_can/c_can_pci.c
new file mode 100644
index 00000000000..1011146ea51
--- /dev/null
+++ b/drivers/net/can/c_can/c_can_pci.c
@@ -0,0 +1,221 @@
+/*
+ * PCI bus driver for Bosch C_CAN/D_CAN controller
+ *
+ * Copyright (C) 2012 Federico Vaga <federico.vaga@gmail.com>
+ *
+ * Borrowed from c_can_platform.c
+ *
+ * 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/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+
+#include <linux/can/dev.h>
+
+#include "c_can.h"
+
+enum c_can_pci_reg_align {
+	C_CAN_REG_ALIGN_16,
+	C_CAN_REG_ALIGN_32,
+};
+
+struct c_can_pci_data {
+	/* Specify if is C_CAN or D_CAN */
+	enum c_can_dev_id type;
+	/* Set the register alignment in the memory */
+	enum c_can_pci_reg_align reg_align;
+	/* Set the frequency */
+	unsigned int freq;
+};
+
+/*
+ * 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_pci_read_reg_aligned_to_16bit(struct c_can_priv *priv,
+						enum reg index)
+{
+	return readw(priv->base + priv->regs[index]);
+}
+
+static void c_can_pci_write_reg_aligned_to_16bit(struct c_can_priv *priv,
+						enum reg index, u16 val)
+{
+	writew(val, priv->base + priv->regs[index]);
+}
+
+static u16 c_can_pci_read_reg_aligned_to_32bit(struct c_can_priv *priv,
+						enum reg index)
+{
+	return readw(priv->base + 2 * priv->regs[index]);
+}
+
+static void c_can_pci_write_reg_aligned_to_32bit(struct c_can_priv *priv,
+						enum reg index, u16 val)
+{
+	writew(val, priv->base + 2 * priv->regs[index]);
+}
+
+static int __devinit c_can_pci_probe(struct pci_dev *pdev,
+				     const struct pci_device_id *ent)
+{
+	struct c_can_pci_data *c_can_pci_data = (void *)ent->driver_data;
+	struct c_can_priv *priv;
+	struct net_device *dev;
+	void __iomem *addr;
+	int ret;
+
+	ret = pci_enable_device(pdev);
+	if (ret) {
+		dev_err(&pdev->dev, "pci_enable_device FAILED\n");
+		goto out;
+	}
+
+	ret = pci_request_regions(pdev, KBUILD_MODNAME);
+	if (ret) {
+		dev_err(&pdev->dev, "pci_request_regions FAILED\n");
+		goto out_disable_device;
+	}
+
+	pci_set_master(pdev);
+	pci_enable_msi(pdev);
+
+	addr = pci_iomap(pdev, 0, pci_resource_len(pdev, 0));
+	if (!addr) {
+		dev_err(&pdev->dev,
+			"device has no PCI memory resources, "
+			"failing adapter\n");
+		ret = -ENOMEM;
+		goto out_release_regions;
+	}
+
+	/* allocate the c_can device */
+	dev = alloc_c_can_dev();
+	if (!dev) {
+		ret = -ENOMEM;
+		goto out_iounmap;
+	}
+
+	priv = netdev_priv(dev);
+	pci_set_drvdata(pdev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	dev->irq = pdev->irq;
+	priv->base = addr;
+
+	if (!c_can_pci_data->freq) {
+		dev_err(&pdev->dev, "no clock frequency defined\n");
+		ret = -ENODEV;
+		goto out_free_c_can;
+	} else {
+		priv->can.clock.freq = c_can_pci_data->freq;
+	}
+
+	/* Configure CAN type */
+	switch (c_can_pci_data->type) {
+	case C_CAN_DEVTYPE:
+		priv->regs = reg_map_c_can;
+		break;
+	case D_CAN_DEVTYPE:
+		priv->regs = reg_map_d_can;
+		priv->can.ctrlmode_supported |= CAN_CTRLMODE_3_SAMPLES;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out_free_c_can;
+	}
+
+	/* Configure access to registers */
+	switch (c_can_pci_data->reg_align) {
+	case C_CAN_REG_ALIGN_32:
+		priv->read_reg = c_can_pci_read_reg_aligned_to_32bit;
+		priv->write_reg = c_can_pci_write_reg_aligned_to_32bit;
+		break;
+	case C_CAN_REG_ALIGN_16:
+		priv->read_reg = c_can_pci_read_reg_aligned_to_16bit;
+		priv->write_reg = c_can_pci_write_reg_aligned_to_16bit;
+		break;
+	default:
+		ret = -EINVAL;
+		goto out_free_c_can;
+	}
+
+	ret = register_c_can_dev(dev);
+	if (ret) {
+		dev_err(&pdev->dev, "registering %s failed (err=%d)\n",
+			KBUILD_MODNAME, ret);
+		goto out_free_c_can;
+	}
+
+	dev_dbg(&pdev->dev, "%s device registered (regs=%p, irq=%d)\n",
+		 KBUILD_MODNAME, priv->regs, dev->irq);
+
+	return 0;
+
+out_free_c_can:
+	pci_set_drvdata(pdev, NULL);
+	free_c_can_dev(dev);
+out_iounmap:
+	pci_iounmap(pdev, addr);
+out_release_regions:
+	pci_disable_msi(pdev);
+	pci_clear_master(pdev);
+	pci_release_regions(pdev);
+out_disable_device:
+	pci_disable_device(pdev);
+out:
+	return ret;
+}
+
+static void __devexit c_can_pci_remove(struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+	struct c_can_priv *priv = netdev_priv(dev);
+
+	unregister_c_can_dev(dev);
+
+	pci_set_drvdata(pdev, NULL);
+	free_c_can_dev(dev);
+
+	pci_iounmap(pdev, priv->base);
+	pci_disable_msi(pdev);
+	pci_clear_master(pdev);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+static struct c_can_pci_data c_can_sta2x11= {
+	.type = C_CAN_DEVTYPE,
+	.reg_align = C_CAN_REG_ALIGN_32,
+	.freq = 52000000, /* 52 Mhz */
+};
+
+#define C_CAN_ID(_vend, _dev, _driverdata) {		\
+	PCI_DEVICE(_vend, _dev),			\
+	.driver_data = (unsigned long)&_driverdata,	\
+}
+static DEFINE_PCI_DEVICE_TABLE(c_can_pci_tbl) = {
+	C_CAN_ID(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_CAN,
+		 c_can_sta2x11),
+	{},
+};
+static struct pci_driver c_can_pci_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = c_can_pci_tbl,
+	.probe = c_can_pci_probe,
+	.remove = __devexit_p(c_can_pci_remove),
+};
+
+module_pci_driver(c_can_pci_driver);
+
+MODULE_AUTHOR("Federico Vaga <federico.vaga@gmail.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("PCI CAN bus driver for Bosch C_CAN/D_CAN controller");
+MODULE_DEVICE_TABLE(pci, c_can_pci_tbl);
diff --git a/drivers/net/can/c_can/c_can_platform.c b/drivers/net/can/c_can/c_can_platform.c
index 5e1a5ff6476..6ff7ad006c3 100644
--- a/drivers/net/can/c_can/c_can_platform.c
+++ b/drivers/net/can/c_can/c_can_platform.c
@@ -42,27 +42,27 @@
  * 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)
+						enum reg index)
 {
-	return readw(reg);
+	return readw(priv->base + priv->regs[index]);
 }
 
 static void c_can_plat_write_reg_aligned_to_16bit(struct c_can_priv *priv,
-						void *reg, u16 val)
+						enum reg index, u16 val)
 {
-	writew(val, reg);
+	writew(val, priv->base + priv->regs[index]);
 }
 
 static u16 c_can_plat_read_reg_aligned_to_32bit(struct c_can_priv *priv,
-						void *reg)
+						enum reg index)
 {
-	return readw(reg + (long)reg - (long)priv->regs);
+	return readw(priv->base + 2 * priv->regs[index]);
 }
 
 static void c_can_plat_write_reg_aligned_to_32bit(struct c_can_priv *priv,
-						void *reg, u16 val)
+						enum reg index, u16 val)
 {
-	writew(val, reg + (long)reg - (long)priv->regs);
+	writew(val, priv->base + 2 * priv->regs[index]);
 }
 
 static int __devinit c_can_plat_probe(struct platform_device *pdev)
@@ -71,9 +71,9 @@ static int __devinit c_can_plat_probe(struct platform_device *pdev)
 	void __iomem *addr;
 	struct net_device *dev;
 	struct c_can_priv *priv;
+	const struct platform_device_id *id;
 	struct resource *mem;
 	int irq;
-#ifdef CONFIG_HAVE_CLK
 	struct clk *clk;
 
 	/* get the appropriate clk */
@@ -83,7 +83,6 @@ static int __devinit c_can_plat_probe(struct platform_device *pdev)
 		ret = -ENODEV;
 		goto exit;
 	}
-#endif
 
 	/* get the platform data */
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -115,26 +114,38 @@ static int __devinit c_can_plat_probe(struct platform_device *pdev)
 	}
 
 	priv = netdev_priv(dev);
-
-	dev->irq = irq;
-	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;
+	id = platform_get_device_id(pdev);
+	switch (id->driver_data) {
+	case C_CAN_DEVTYPE:
+		priv->regs = reg_map_c_can;
+		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;
+		}
 		break;
-	case IORESOURCE_MEM_16BIT:
-	default:
+	case D_CAN_DEVTYPE:
+		priv->regs = reg_map_d_can;
+		priv->can.ctrlmode_supported |= CAN_CTRLMODE_3_SAMPLES;
 		priv->read_reg = c_can_plat_read_reg_aligned_to_16bit;
 		priv->write_reg = c_can_plat_write_reg_aligned_to_16bit;
 		break;
+	default:
+		ret = -EINVAL;
+		goto exit_free_device;
 	}
 
+	dev->irq = irq;
+	priv->base = addr;
+	priv->can.clock.freq = clk_get_rate(clk);
+	priv->priv = clk;
+
 	platform_set_drvdata(pdev, dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
@@ -146,7 +157,7 @@ static int __devinit c_can_plat_probe(struct platform_device *pdev)
 	}
 
 	dev_info(&pdev->dev, "%s device registered (regs=%p, irq=%d)\n",
-		 KBUILD_MODNAME, priv->regs, dev->irq);
+		 KBUILD_MODNAME, priv->base, dev->irq);
 	return 0;
 
 exit_free_device:
@@ -157,10 +168,8 @@ exit_iounmap:
 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;
@@ -176,18 +185,30 @@ static int __devexit c_can_plat_remove(struct platform_device *pdev)
 	platform_set_drvdata(pdev, NULL);
 
 	free_c_can_dev(dev);
-	iounmap(priv->regs);
+	iounmap(priv->base);
 
 	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 const struct platform_device_id c_can_id_table[] = {
+	{
+		.name = KBUILD_MODNAME,
+		.driver_data = C_CAN_DEVTYPE,
+	}, {
+		.name = "c_can",
+		.driver_data = C_CAN_DEVTYPE,
+	}, {
+		.name = "d_can",
+		.driver_data = D_CAN_DEVTYPE,
+	}, {
+	}
+};
+
 static struct platform_driver c_can_plat_driver = {
 	.driver = {
 		.name = KBUILD_MODNAME,
@@ -195,6 +216,7 @@ static struct platform_driver c_can_plat_driver = {
 	},
 	.probe = c_can_plat_probe,
 	.remove = __devexit_p(c_can_plat_remove),
+	.id_table = c_can_id_table,
 };
 
 module_platform_driver(c_can_plat_driver);
diff --git a/drivers/net/can/cc770/cc770.c b/drivers/net/can/cc770/cc770.c
index d42a6a7396f..0f12abf6591 100644
--- a/drivers/net/can/cc770/cc770.c
+++ b/drivers/net/can/cc770/cc770.c
@@ -90,7 +90,7 @@ static unsigned char cc770_obj_flags[CC770_OBJ_MAX] = {
 	[CC770_OBJ_TX] = 0,
 };
 
-static struct can_bittiming_const cc770_bittiming_const = {
+static const struct can_bittiming_const cc770_bittiming_const = {
 	.name = KBUILD_MODNAME,
 	.tseg1_min = 1,
 	.tseg1_max = 16,
@@ -695,7 +695,7 @@ static void cc770_tx_interrupt(struct net_device *dev, unsigned int o)
 	netif_wake_queue(dev);
 }
 
-irqreturn_t cc770_interrupt(int irq, void *dev_id)
+static irqreturn_t cc770_interrupt(int irq, void *dev_id)
 {
 	struct net_device *dev = (struct net_device *)dev_id;
 	struct cc770_priv *priv = netdev_priv(dev);
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index f03d7a481a8..963e2ccd10d 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -33,6 +33,39 @@ MODULE_DESCRIPTION(MOD_DESC);
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Wolfgang Grandegger <wg@grandegger.com>");
 
+/* CAN DLC to real data length conversion helpers */
+
+static const u8 dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7,
+			     8, 12, 16, 20, 24, 32, 48, 64};
+
+/* get data length from can_dlc with sanitized can_dlc */
+u8 can_dlc2len(u8 can_dlc)
+{
+	return dlc2len[can_dlc & 0x0F];
+}
+EXPORT_SYMBOL_GPL(can_dlc2len);
+
+static const u8 len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8,		/* 0 - 8 */
+			     9, 9, 9, 9,			/* 9 - 12 */
+			     10, 10, 10, 10,			/* 13 - 16 */
+			     11, 11, 11, 11,			/* 17 - 20 */
+			     12, 12, 12, 12,			/* 21 - 24 */
+			     13, 13, 13, 13, 13, 13, 13, 13,	/* 25 - 32 */
+			     14, 14, 14, 14, 14, 14, 14, 14,	/* 33 - 40 */
+			     14, 14, 14, 14, 14, 14, 14, 14,	/* 41 - 48 */
+			     15, 15, 15, 15, 15, 15, 15, 15,	/* 49 - 56 */
+			     15, 15, 15, 15, 15, 15, 15, 15};	/* 57 - 64 */
+
+/* map the sanitized data length to an appropriate data length code */
+u8 can_len2dlc(u8 len)
+{
+	if (unlikely(len > 64))
+		return 0xF;
+
+	return len2dlc[len];
+}
+EXPORT_SYMBOL_GPL(can_len2dlc);
+
 #ifdef CONFIG_CAN_CALC_BITTIMING
 #define CAN_CALC_MAX_ERROR 50 /* in one-tenth of a percent */
 
@@ -368,7 +401,7 @@ EXPORT_SYMBOL_GPL(can_free_echo_skb);
 /*
  * CAN device restart for bus-off recovery
  */
-void can_restart(unsigned long data)
+static void can_restart(unsigned long data)
 {
 	struct net_device *dev = (struct net_device *)data;
 	struct can_priv *priv = netdev_priv(dev);
@@ -454,7 +487,7 @@ EXPORT_SYMBOL_GPL(can_bus_off);
 static void can_setup(struct net_device *dev)
 {
 	dev->type = ARPHRD_CAN;
-	dev->mtu = sizeof(struct can_frame);
+	dev->mtu = CAN_MTU;
 	dev->hard_header_len = 0;
 	dev->addr_len = 0;
 	dev->tx_queue_len = 10;
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index 81d47410237..c5f143165f8 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -34,6 +34,7 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/pinctrl/consumer.h>
 
@@ -165,10 +166,21 @@ struct flexcan_regs {
 	u32 imask1;		/* 0x28 */
 	u32 iflag2;		/* 0x2c */
 	u32 iflag1;		/* 0x30 */
-	u32 _reserved2[19];
+	u32 crl2;		/* 0x34 */
+	u32 esr2;		/* 0x38 */
+	u32 imeur;		/* 0x3c */
+	u32 lrfr;		/* 0x40 */
+	u32 crcr;		/* 0x44 */
+	u32 rxfgmask;		/* 0x48 */
+	u32 rxfir;		/* 0x4c */
+	u32 _reserved3[12];
 	struct flexcan_mb cantxfg[64];
 };
 
+struct flexcan_devtype_data {
+	u32 hw_ver;	/* hardware controller version */
+};
+
 struct flexcan_priv {
 	struct can_priv can;
 	struct net_device *dev;
@@ -178,11 +190,21 @@ struct flexcan_priv {
 	u32 reg_esr;
 	u32 reg_ctrl_default;
 
-	struct clk *clk;
+	struct clk *clk_ipg;
+	struct clk *clk_per;
 	struct flexcan_platform_data *pdata;
+	const struct flexcan_devtype_data *devtype_data;
+};
+
+static struct flexcan_devtype_data fsl_p1010_devtype_data = {
+	.hw_ver = 3,
+};
+
+static struct flexcan_devtype_data fsl_imx6q_devtype_data = {
+	.hw_ver = 10,
 };
 
-static struct can_bittiming_const flexcan_bittiming_const = {
+static const struct can_bittiming_const flexcan_bittiming_const = {
 	.name = DRV_NAME,
 	.tseg1_min = 4,
 	.tseg1_max = 16,
@@ -750,6 +772,9 @@ static int flexcan_chip_start(struct net_device *dev)
 	flexcan_write(0x0, &regs->rx14mask);
 	flexcan_write(0x0, &regs->rx15mask);
 
+	if (priv->devtype_data->hw_ver >= 10)
+		flexcan_write(0x0, &regs->rxfgmask);
+
 	flexcan_transceiver_switch(priv, 1);
 
 	/* synchronize with the can bus */
@@ -804,7 +829,8 @@ static int flexcan_open(struct net_device *dev)
 	struct flexcan_priv *priv = netdev_priv(dev);
 	int err;
 
-	clk_prepare_enable(priv->clk);
+	clk_prepare_enable(priv->clk_ipg);
+	clk_prepare_enable(priv->clk_per);
 
 	err = open_candev(dev);
 	if (err)
@@ -826,7 +852,8 @@ static int flexcan_open(struct net_device *dev)
  out_close:
 	close_candev(dev);
  out:
-	clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(priv->clk_per);
+	clk_disable_unprepare(priv->clk_ipg);
 
 	return err;
 }
@@ -840,7 +867,8 @@ static int flexcan_close(struct net_device *dev)
 	flexcan_chip_stop(dev);
 
 	free_irq(dev->irq, dev);
-	clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(priv->clk_per);
+	clk_disable_unprepare(priv->clk_ipg);
 
 	close_candev(dev);
 
@@ -879,7 +907,8 @@ static int __devinit register_flexcandev(struct net_device *dev)
 	struct flexcan_regs __iomem *regs = priv->base;
 	u32 reg, err;
 
-	clk_prepare_enable(priv->clk);
+	clk_prepare_enable(priv->clk_ipg);
+	clk_prepare_enable(priv->clk_per);
 
 	/* select "bus clock", chip must be disabled */
 	flexcan_chip_disable(priv);
@@ -912,7 +941,8 @@ static int __devinit register_flexcandev(struct net_device *dev)
  out:
 	/* disable core and turn off clocks */
 	flexcan_chip_disable(priv);
-	clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(priv->clk_per);
+	clk_disable_unprepare(priv->clk_ipg);
 
 	return err;
 }
@@ -922,12 +952,25 @@ static void __devexit unregister_flexcandev(struct net_device *dev)
 	unregister_candev(dev);
 }
 
+static const struct of_device_id flexcan_of_match[] = {
+	{ .compatible = "fsl,p1010-flexcan", .data = &fsl_p1010_devtype_data, },
+	{ .compatible = "fsl,imx6q-flexcan", .data = &fsl_imx6q_devtype_data, },
+	{ /* sentinel */ },
+};
+
+static const struct platform_device_id flexcan_id_table[] = {
+	{ .name = "flexcan", .driver_data = (kernel_ulong_t)&fsl_p1010_devtype_data, },
+	{ /* sentinel */ },
+};
+
 static int __devinit flexcan_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id;
+	const struct flexcan_devtype_data *devtype_data;
 	struct net_device *dev;
 	struct flexcan_priv *priv;
 	struct resource *mem;
-	struct clk *clk = NULL;
+	struct clk *clk_ipg = NULL, *clk_per = NULL;
 	struct pinctrl *pinctrl;
 	void __iomem *base;
 	resource_size_t mem_size;
@@ -938,23 +981,25 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 	if (IS_ERR(pinctrl))
 		return PTR_ERR(pinctrl);
 
-	if (pdev->dev.of_node) {
-		const __be32 *clock_freq_p;
-
-		clock_freq_p = of_get_property(pdev->dev.of_node,
-						"clock-frequency", NULL);
-		if (clock_freq_p)
-			clock_freq = be32_to_cpup(clock_freq_p);
-	}
+	if (pdev->dev.of_node)
+		of_property_read_u32(pdev->dev.of_node,
+						"clock-frequency", &clock_freq);
 
 	if (!clock_freq) {
-		clk = clk_get(&pdev->dev, NULL);
-		if (IS_ERR(clk)) {
-			dev_err(&pdev->dev, "no clock defined\n");
-			err = PTR_ERR(clk);
+		clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+		if (IS_ERR(clk_ipg)) {
+			dev_err(&pdev->dev, "no ipg clock defined\n");
+			err = PTR_ERR(clk_ipg);
+			goto failed_clock;
+		}
+		clock_freq = clk_get_rate(clk_ipg);
+
+		clk_per = devm_clk_get(&pdev->dev, "per");
+		if (IS_ERR(clk_per)) {
+			dev_err(&pdev->dev, "no per clock defined\n");
+			err = PTR_ERR(clk_per);
 			goto failed_clock;
 		}
-		clock_freq = clk_get_rate(clk);
 	}
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -982,6 +1027,17 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 		goto failed_alloc;
 	}
 
+	of_id = of_match_device(flexcan_of_match, &pdev->dev);
+	if (of_id) {
+		devtype_data = of_id->data;
+	} else if (pdev->id_entry->driver_data) {
+		devtype_data = (struct flexcan_devtype_data *)
+			pdev->id_entry->driver_data;
+	} else {
+		err = -ENODEV;
+		goto failed_devtype;
+	}
+
 	dev->netdev_ops = &flexcan_netdev_ops;
 	dev->irq = irq;
 	dev->flags |= IFF_ECHO;
@@ -996,8 +1052,10 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 		CAN_CTRLMODE_BERR_REPORTING;
 	priv->base = base;
 	priv->dev = dev;
-	priv->clk = clk;
+	priv->clk_ipg = clk_ipg;
+	priv->clk_per = clk_per;
 	priv->pdata = pdev->dev.platform_data;
+	priv->devtype_data = devtype_data;
 
 	netif_napi_add(dev, &priv->napi, flexcan_poll, FLEXCAN_NAPI_WEIGHT);
 
@@ -1016,14 +1074,13 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 	return 0;
 
  failed_register:
+ failed_devtype:
 	free_candev(dev);
  failed_alloc:
 	iounmap(base);
  failed_map:
 	release_mem_region(mem->start, mem_size);
  failed_get:
-	if (clk)
-		clk_put(clk);
  failed_clock:
 	return err;
 }
@@ -1041,20 +1098,46 @@ static int __devexit flexcan_remove(struct platform_device *pdev)
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(mem->start, resource_size(mem));
 
-	if (priv->clk)
-		clk_put(priv->clk);
-
 	free_candev(dev);
 
 	return 0;
 }
 
-static struct of_device_id flexcan_of_match[] = {
-	{
-		.compatible = "fsl,p1010-flexcan",
-	},
-	{},
-};
+#ifdef CONFIG_PM
+static int flexcan_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct flexcan_priv *priv = netdev_priv(dev);
+
+	flexcan_chip_disable(priv);
+
+	if (netif_running(dev)) {
+		netif_stop_queue(dev);
+		netif_device_detach(dev);
+	}
+	priv->can.state = CAN_STATE_SLEEPING;
+
+	return 0;
+}
+
+static int flexcan_resume(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct flexcan_priv *priv = netdev_priv(dev);
+
+	priv->can.state = CAN_STATE_ERROR_ACTIVE;
+	if (netif_running(dev)) {
+		netif_device_attach(dev);
+		netif_start_queue(dev);
+	}
+	flexcan_chip_enable(priv);
+
+	return 0;
+}
+#else
+#define flexcan_suspend NULL
+#define flexcan_resume NULL
+#endif
 
 static struct platform_driver flexcan_driver = {
 	.driver = {
@@ -1064,6 +1147,9 @@ static struct platform_driver flexcan_driver = {
 	},
 	.probe = flexcan_probe,
 	.remove = __devexit_p(flexcan_remove),
+	.suspend = flexcan_suspend,
+	.resume = flexcan_resume,
+	.id_table = flexcan_id_table,
 };
 
 module_platform_driver(flexcan_driver);
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
index 08c893cb789..98ee4381991 100644
--- a/drivers/net/can/janz-ican3.c
+++ b/drivers/net/can/janz-ican3.c
@@ -116,6 +116,7 @@
 #define ICAN3_BUSERR_QUOTA_MAX	255
 
 /* Janz ICAN3 CAN Frame Conversion */
+#define ICAN3_SNGL	0x02
 #define ICAN3_ECHO	0x10
 #define ICAN3_EFF_RTR	0x40
 #define ICAN3_SFF_RTR	0x10
@@ -220,6 +221,9 @@ struct ican3_dev {
 	/* old and new style host interface */
 	unsigned int iftype;
 
+	/* queue for echo packets */
+	struct sk_buff_head echoq;
+
 	/*
 	 * Any function which changes the current DPM page must hold this
 	 * lock while it is performing data accesses. This ensures that the
@@ -235,7 +239,6 @@ struct ican3_dev {
 
 	/* fast host interface */
 	unsigned int fastrx_start;
-	unsigned int fastrx_int;
 	unsigned int fastrx_num;
 	unsigned int fasttx_start;
 	unsigned int fasttx_num;
@@ -454,7 +457,6 @@ static void __devinit ican3_init_fast_host_interface(struct ican3_dev *mod)
 	/* save the start recv page */
 	mod->fastrx_start = mod->free_page;
 	mod->fastrx_num = 0;
-	mod->fastrx_int = 0;
 
 	/* build a single fast tohost queue descriptor */
 	memset(&desc, 0, sizeof(desc));
@@ -813,10 +815,10 @@ static void ican3_to_can_frame(struct ican3_dev *mod,
 
 		cf->can_id |= desc->data[0] << 3;
 		cf->can_id |= (desc->data[1] & 0xe0) >> 5;
-		cf->can_dlc = desc->data[1] & ICAN3_CAN_DLC_MASK;
-		memcpy(cf->data, &desc->data[2], sizeof(cf->data));
+		cf->can_dlc = get_can_dlc(desc->data[1] & ICAN3_CAN_DLC_MASK);
+		memcpy(cf->data, &desc->data[2], cf->can_dlc);
 	} else {
-		cf->can_dlc = desc->data[0] & ICAN3_CAN_DLC_MASK;
+		cf->can_dlc = get_can_dlc(desc->data[0] & ICAN3_CAN_DLC_MASK);
 		if (desc->data[0] & ICAN3_EFF_RTR)
 			cf->can_id |= CAN_RTR_FLAG;
 
@@ -831,7 +833,7 @@ static void ican3_to_can_frame(struct ican3_dev *mod,
 			cf->can_id |= desc->data[3] >> 5;  /* 2-0   */
 		}
 
-		memcpy(cf->data, &desc->data[6], sizeof(cf->data));
+		memcpy(cf->data, &desc->data[6], cf->can_dlc);
 	}
 }
 
@@ -847,6 +849,10 @@ static void can_frame_to_ican3(struct ican3_dev *mod,
 	desc->data[0] |= cf->can_dlc;
 	desc->data[1] |= ICAN3_ECHO;
 
+	/* support single transmission (no retries) mode */
+	if (mod->can.ctrlmode & CAN_CTRLMODE_ONE_SHOT)
+		desc->data[1] |= ICAN3_SNGL;
+
 	if (cf->can_id & CAN_RTR_FLAG)
 		desc->data[0] |= ICAN3_EFF_RTR;
 
@@ -863,7 +869,7 @@ static void can_frame_to_ican3(struct ican3_dev *mod,
 	}
 
 	/* copy the data bits into the descriptor */
-	memcpy(&desc->data[6], cf->data, sizeof(cf->data));
+	memcpy(&desc->data[6], cf->data, cf->can_dlc);
 }
 
 /*
@@ -909,8 +915,8 @@ static void ican3_handle_msglost(struct ican3_dev *mod, struct ican3_msg *msg)
 	if (skb) {
 		cf->can_id |= CAN_ERR_CRTL;
 		cf->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+		stats->rx_over_errors++;
 		stats->rx_errors++;
-		stats->rx_bytes += cf->can_dlc;
 		netif_rx(skb);
 	}
 }
@@ -927,7 +933,7 @@ static int ican3_handle_cevtind(struct ican3_dev *mod, struct ican3_msg *msg)
 	struct net_device *dev = mod->ndev;
 	struct net_device_stats *stats = &dev->stats;
 	enum can_state state = mod->can.state;
-	u8 status, isrc, rxerr, txerr;
+	u8 isrc, ecc, status, rxerr, txerr;
 	struct can_frame *cf;
 	struct sk_buff *skb;
 
@@ -943,15 +949,53 @@ static int ican3_handle_cevtind(struct ican3_dev *mod, struct ican3_msg *msg)
 		return -EINVAL;
 	}
 
-	skb = alloc_can_err_skb(dev, &cf);
-	if (skb == NULL)
-		return -ENOMEM;
-
 	isrc = msg->data[0];
+	ecc = msg->data[2];
 	status = msg->data[3];
 	rxerr = msg->data[4];
 	txerr = msg->data[5];
 
+	/*
+	 * This hardware lacks any support other than bus error messages to
+	 * determine if packet transmission has failed.
+	 *
+	 * When TX errors happen, one echo skb needs to be dropped from the
+	 * front of the queue.
+	 *
+	 * A small bit of code is duplicated here and below, to avoid error
+	 * skb allocation when it will just be freed immediately.
+	 */
+	if (isrc == CEVTIND_BEI) {
+		int ret;
+		dev_dbg(mod->dev, "bus error interrupt\n");
+
+		/* TX error */
+		if (!(ecc & ECC_DIR)) {
+			kfree_skb(skb_dequeue(&mod->echoq));
+			stats->tx_errors++;
+		} else {
+			stats->rx_errors++;
+		}
+
+		/*
+		 * The controller automatically disables bus-error interrupts
+		 * and therefore we must re-enable them.
+		 */
+		ret = ican3_set_buserror(mod, 1);
+		if (ret) {
+			dev_err(mod->dev, "unable to re-enable bus-error\n");
+			return ret;
+		}
+
+		/* bus error reporting is off, return immediately */
+		if (!(mod->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING))
+			return 0;
+	}
+
+	skb = alloc_can_err_skb(dev, &cf);
+	if (skb == NULL)
+		return -ENOMEM;
+
 	/* data overrun interrupt */
 	if (isrc == CEVTIND_DOI || isrc == CEVTIND_LOST) {
 		dev_dbg(mod->dev, "data overrun interrupt\n");
@@ -980,11 +1024,7 @@ static int ican3_handle_cevtind(struct ican3_dev *mod, struct ican3_msg *msg)
 
 	/* bus error interrupt */
 	if (isrc == CEVTIND_BEI) {
-		u8 ecc = msg->data[2];
-
-		dev_dbg(mod->dev, "bus error interrupt\n");
 		mod->can.can_stats.bus_error++;
-		stats->rx_errors++;
 		cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
 
 		switch (ecc & ECC_MASK) {
@@ -1003,7 +1043,7 @@ static int ican3_handle_cevtind(struct ican3_dev *mod, struct ican3_msg *msg)
 			break;
 		}
 
-		if ((ecc & ECC_DIR) == 0)
+		if (!(ecc & ECC_DIR))
 			cf->data[2] |= CAN_ERR_PROT_TX;
 
 		cf->data[6] = txerr;
@@ -1030,8 +1070,6 @@ static int ican3_handle_cevtind(struct ican3_dev *mod, struct ican3_msg *msg)
 	}
 
 	mod->can.state = state;
-	stats->rx_errors++;
-	stats->rx_bytes += cf->can_dlc;
 	netif_rx(skb);
 	return 0;
 }
@@ -1091,6 +1129,88 @@ static void ican3_handle_message(struct ican3_dev *mod, struct ican3_msg *msg)
 }
 
 /*
+ * The ican3 needs to store all echo skbs, and therefore cannot
+ * use the generic infrastructure for this.
+ */
+static void ican3_put_echo_skb(struct ican3_dev *mod, struct sk_buff *skb)
+{
+	struct sock *srcsk = skb->sk;
+
+	if (atomic_read(&skb->users) != 1) {
+		struct sk_buff *old_skb = skb;
+
+		skb = skb_clone(old_skb, GFP_ATOMIC);
+		kfree_skb(old_skb);
+		if (!skb)
+			return;
+	} else {
+		skb_orphan(skb);
+	}
+
+	skb->sk = srcsk;
+
+	/* save this skb for tx interrupt echo handling */
+	skb_queue_tail(&mod->echoq, skb);
+}
+
+static unsigned int ican3_get_echo_skb(struct ican3_dev *mod)
+{
+	struct sk_buff *skb = skb_dequeue(&mod->echoq);
+	struct can_frame *cf;
+	u8 dlc;
+
+	/* this should never trigger unless there is a driver bug */
+	if (!skb) {
+		netdev_err(mod->ndev, "BUG: echo skb not occupied\n");
+		return 0;
+	}
+
+	cf = (struct can_frame *)skb->data;
+	dlc = cf->can_dlc;
+
+	/* check flag whether this packet has to be looped back */
+	if (skb->pkt_type != PACKET_LOOPBACK) {
+		kfree_skb(skb);
+		return dlc;
+	}
+
+	skb->protocol = htons(ETH_P_CAN);
+	skb->pkt_type = PACKET_BROADCAST;
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb->dev = mod->ndev;
+	netif_receive_skb(skb);
+	return dlc;
+}
+
+/*
+ * Compare an skb with an existing echo skb
+ *
+ * This function will be used on devices which have a hardware loopback.
+ * On these devices, this function can be used to compare a received skb
+ * with the saved echo skbs so that the hardware echo skb can be dropped.
+ *
+ * Returns true if the skb's are identical, false otherwise.
+ */
+static bool ican3_echo_skb_matches(struct ican3_dev *mod, struct sk_buff *skb)
+{
+	struct can_frame *cf = (struct can_frame *)skb->data;
+	struct sk_buff *echo_skb = skb_peek(&mod->echoq);
+	struct can_frame *echo_cf;
+
+	if (!echo_skb)
+		return false;
+
+	echo_cf = (struct can_frame *)echo_skb->data;
+	if (cf->can_id != echo_cf->can_id)
+		return false;
+
+	if (cf->can_dlc != echo_cf->can_dlc)
+		return false;
+
+	return memcmp(cf->data, echo_cf->data, cf->can_dlc) == 0;
+}
+
+/*
  * Check that there is room in the TX ring to transmit another skb
  *
  * LOCKING: must hold mod->lock
@@ -1100,6 +1220,10 @@ static bool ican3_txok(struct ican3_dev *mod)
 	struct ican3_fast_desc __iomem *desc;
 	u8 control;
 
+	/* check that we have echo queue space */
+	if (skb_queue_len(&mod->echoq) >= ICAN3_TX_BUFFERS)
+		return false;
+
 	/* copy the control bits of the descriptor */
 	ican3_set_page(mod, mod->fasttx_start + (mod->fasttx_num / 16));
 	desc = mod->dpm + ((mod->fasttx_num % 16) * sizeof(*desc));
@@ -1150,10 +1274,27 @@ static int ican3_recv_skb(struct ican3_dev *mod)
 	/* convert the ICAN3 frame into Linux CAN format */
 	ican3_to_can_frame(mod, &desc, cf);
 
-	/* receive the skb, update statistics */
-	netif_receive_skb(skb);
+	/*
+	 * If this is an ECHO frame received from the hardware loopback
+	 * feature, use the skb saved in the ECHO stack instead. This allows
+	 * the Linux CAN core to support CAN_RAW_RECV_OWN_MSGS correctly.
+	 *
+	 * Since this is a confirmation of a successfully transmitted packet
+	 * sent from this host, update the transmit statistics.
+	 *
+	 * Also, the netdevice queue needs to be allowed to send packets again.
+	 */
+	if (ican3_echo_skb_matches(mod, skb)) {
+		stats->tx_packets++;
+		stats->tx_bytes += ican3_get_echo_skb(mod);
+		kfree_skb(skb);
+		goto err_noalloc;
+	}
+
+	/* update statistics, receive the skb */
 	stats->rx_packets++;
 	stats->rx_bytes += cf->can_dlc;
+	netif_receive_skb(skb);
 
 err_noalloc:
 	/* toggle the valid bit and return the descriptor to the ring */
@@ -1176,13 +1317,13 @@ err_noalloc:
 static int ican3_napi(struct napi_struct *napi, int budget)
 {
 	struct ican3_dev *mod = container_of(napi, struct ican3_dev, napi);
-	struct ican3_msg msg;
 	unsigned long flags;
 	int received = 0;
 	int ret;
 
 	/* process all communication messages */
 	while (true) {
+		struct ican3_msg msg;
 		ret = ican3_recv_msg(mod, &msg);
 		if (ret)
 			break;
@@ -1325,7 +1466,7 @@ static int __devinit ican3_startup_module(struct ican3_dev *mod)
 	}
 
 	/* default to "bus errors enabled" */
-	ret = ican3_set_buserror(mod, ICAN3_BUSERR_QUOTA_MAX);
+	ret = ican3_set_buserror(mod, 1);
 	if (ret) {
 		dev_err(mod->dev, "unable to set bus-error\n");
 		return ret;
@@ -1354,7 +1495,6 @@ static int __devinit ican3_startup_module(struct ican3_dev *mod)
 static int ican3_open(struct net_device *ndev)
 {
 	struct ican3_dev *mod = netdev_priv(ndev);
-	u8 quota;
 	int ret;
 
 	/* open the CAN layer */
@@ -1364,19 +1504,6 @@ static int ican3_open(struct net_device *ndev)
 		return ret;
 	}
 
-	/* set the bus error generation state appropriately */
-	if (mod->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING)
-		quota = ICAN3_BUSERR_QUOTA_MAX;
-	else
-		quota = 0;
-
-	ret = ican3_set_buserror(mod, quota);
-	if (ret) {
-		dev_err(mod->dev, "unable to set bus-error\n");
-		close_candev(ndev);
-		return ret;
-	}
-
 	/* bring the bus online */
 	ret = ican3_set_bus_state(mod, true);
 	if (ret) {
@@ -1408,6 +1535,9 @@ static int ican3_stop(struct net_device *ndev)
 		return ret;
 	}
 
+	/* drop all outstanding echo skbs */
+	skb_queue_purge(&mod->echoq);
+
 	/* close the CAN layer */
 	close_candev(ndev);
 	return 0;
@@ -1416,18 +1546,19 @@ static int ican3_stop(struct net_device *ndev)
 static int ican3_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct ican3_dev *mod = netdev_priv(ndev);
-	struct net_device_stats *stats = &ndev->stats;
 	struct can_frame *cf = (struct can_frame *)skb->data;
 	struct ican3_fast_desc desc;
 	void __iomem *desc_addr;
 	unsigned long flags;
 
+	if (can_dropped_invalid_skb(ndev, skb))
+		return NETDEV_TX_OK;
+
 	spin_lock_irqsave(&mod->lock, flags);
 
 	/* check that we can actually transmit */
 	if (!ican3_txok(mod)) {
-		dev_err(mod->dev, "no free descriptors, stopping queue\n");
-		netif_stop_queue(ndev);
+		dev_err(mod->dev, "BUG: no free descriptors\n");
 		spin_unlock_irqrestore(&mod->lock, flags);
 		return NETDEV_TX_BUSY;
 	}
@@ -1442,6 +1573,14 @@ static int ican3_xmit(struct sk_buff *skb, struct net_device *ndev)
 	can_frame_to_ican3(mod, cf, &desc);
 
 	/*
+	 * This hardware doesn't have TX-done notifications, so we'll try and
+	 * emulate it the best we can using ECHO skbs. Add the skb to the ECHO
+	 * stack. Upon packet reception, check if the ECHO skb and received
+	 * skb match, and use that to wake the queue.
+	 */
+	ican3_put_echo_skb(mod, skb);
+
+	/*
 	 * the programming manual says that you must set the IVALID bit, then
 	 * interrupt, then set the valid bit. Quite weird, but it seems to be
 	 * required for this to work
@@ -1459,19 +1598,7 @@ static int ican3_xmit(struct sk_buff *skb, struct net_device *ndev)
 	mod->fasttx_num = (desc.control & DESC_WRAP) ? 0
 						     : (mod->fasttx_num + 1);
 
-	/* update statistics */
-	stats->tx_packets++;
-	stats->tx_bytes += cf->can_dlc;
-	kfree_skb(skb);
-
-	/*
-	 * This hardware doesn't have TX-done notifications, so we'll try and
-	 * emulate it the best we can using ECHO skbs. Get the next TX
-	 * descriptor, and see if we have room to send. If not, stop the queue.
-	 * It will be woken when the ECHO skb for the current packet is recv'd.
-	 */
-
-	/* copy the control bits of the descriptor */
+	/* if there is no free descriptor space, stop the transmit queue */
 	if (!ican3_txok(mod))
 		netif_stop_queue(ndev);
 
@@ -1490,7 +1617,7 @@ static const struct net_device_ops ican3_netdev_ops = {
  */
 
 /* This structure was stolen from drivers/net/can/sja1000/sja1000.c */
-static struct can_bittiming_const ican3_bittiming_const = {
+static const struct can_bittiming_const ican3_bittiming_const = {
 	.name = DRV_NAME,
 	.tseg1_min = 1,
 	.tseg1_max = 16,
@@ -1667,6 +1794,7 @@ static int __devinit ican3_probe(struct platform_device *pdev)
 	mod->dev = &pdev->dev;
 	mod->num = pdata->modno;
 	netif_napi_add(ndev, &mod->napi, ican3_napi, ICAN3_RX_BUFFERS);
+	skb_queue_head_init(&mod->echoq);
 	spin_lock_init(&mod->lock);
 	init_completion(&mod->termination_comp);
 	init_completion(&mod->buserror_comp);
@@ -1687,7 +1815,8 @@ static int __devinit ican3_probe(struct platform_device *pdev)
 	mod->can.do_set_mode = ican3_set_mode;
 	mod->can.do_get_berr_counter = ican3_get_berr_counter;
 	mod->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES
-				    | CAN_CTRLMODE_BERR_REPORTING;
+				    | CAN_CTRLMODE_BERR_REPORTING
+				    | CAN_CTRLMODE_ONE_SHOT;
 
 	/* find our IRQ number */
 	mod->irq = platform_get_irq(pdev, 0);
diff --git a/drivers/net/can/mcp251x.c b/drivers/net/can/mcp251x.c
index 346785c56a2..a580db29e50 100644
--- a/drivers/net/can/mcp251x.c
+++ b/drivers/net/can/mcp251x.c
@@ -214,7 +214,7 @@ static int mcp251x_enable_dma; /* Enable SPI DMA. Default: 0 (Off) */
 module_param(mcp251x_enable_dma, int, S_IRUGO);
 MODULE_PARM_DESC(mcp251x_enable_dma, "Enable SPI DMA. Default: 0 (Off)");
 
-static struct can_bittiming_const mcp251x_bittiming_const = {
+static const struct can_bittiming_const mcp251x_bittiming_const = {
 	.name = DEVICE_NAME,
 	.tseg1_min = 3,
 	.tseg1_max = 16,
@@ -1020,8 +1020,7 @@ static int __devinit mcp251x_can_probe(struct spi_device *spi)
 						      GFP_DMA);
 
 		if (priv->spi_tx_buf) {
-			priv->spi_rx_buf = (u8 *)(priv->spi_tx_buf +
-						  (PAGE_SIZE / 2));
+			priv->spi_rx_buf = (priv->spi_tx_buf + (PAGE_SIZE / 2));
 			priv->spi_rx_dma = (dma_addr_t)(priv->spi_tx_dma +
 							(PAGE_SIZE / 2));
 		} else {
diff --git a/drivers/net/can/mscan/mpc5xxx_can.c b/drivers/net/can/mscan/mpc5xxx_can.c
index 5caa572d71e..06adf881ea2 100644
--- a/drivers/net/can/mscan/mpc5xxx_can.c
+++ b/drivers/net/can/mscan/mpc5xxx_can.c
@@ -251,7 +251,7 @@ static struct of_device_id mpc5xxx_can_table[];
 static int __devinit mpc5xxx_can_probe(struct platform_device *ofdev)
 {
 	const struct of_device_id *match;
-	struct mpc5xxx_can_data *data;
+	const struct mpc5xxx_can_data *data;
 	struct device_node *np = ofdev->dev.of_node;
 	struct net_device *dev;
 	struct mscan_priv *priv;
diff --git a/drivers/net/can/mscan/mscan.c b/drivers/net/can/mscan/mscan.c
index 41a2a2dda7e..2b104d5f422 100644
--- a/drivers/net/can/mscan/mscan.c
+++ b/drivers/net/can/mscan/mscan.c
@@ -34,7 +34,7 @@
 
 #include "mscan.h"
 
-static struct can_bittiming_const mscan_bittiming_const = {
+static const struct can_bittiming_const mscan_bittiming_const = {
 	.name = "mscan",
 	.tseg1_min = 4,
 	.tseg1_max = 16,
diff --git a/drivers/net/can/pch_can.c b/drivers/net/can/pch_can.c
index 1226297e767..48b3d62b34c 100644
--- a/drivers/net/can/pch_can.c
+++ b/drivers/net/can/pch_can.c
@@ -184,7 +184,7 @@ struct pch_can_priv {
 	int use_msi;
 };
 
-static struct can_bittiming_const pch_can_bittiming_const = {
+static const struct can_bittiming_const pch_can_bittiming_const = {
 	.name = KBUILD_MODNAME,
 	.tseg1_min = 2,
 	.tseg1_max = 16,
diff --git a/drivers/net/can/sja1000/sja1000.c b/drivers/net/can/sja1000/sja1000.c
index 5e10472371e..4c4f33d482d 100644
--- a/drivers/net/can/sja1000/sja1000.c
+++ b/drivers/net/can/sja1000/sja1000.c
@@ -69,7 +69,7 @@ MODULE_AUTHOR("Oliver Hartkopp <oliver.hartkopp@volkswagen.de>");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_DESCRIPTION(DRV_NAME "CAN netdevice driver");
 
-static struct can_bittiming_const sja1000_bittiming_const = {
+static const struct can_bittiming_const sja1000_bittiming_const = {
 	.name = DRV_NAME,
 	.tseg1_min = 1,
 	.tseg1_max = 16,
diff --git a/drivers/net/can/softing/softing_main.c b/drivers/net/can/softing/softing_main.c
index a7c77c744ee..f2a221e7b96 100644
--- a/drivers/net/can/softing/softing_main.c
+++ b/drivers/net/can/softing/softing_main.c
@@ -826,12 +826,12 @@ static __devinit int softing_pdev_probe(struct platform_device *pdev)
 		goto sysfs_failed;
 	}
 
-	ret = -ENOMEM;
 	for (j = 0; j < ARRAY_SIZE(card->net); ++j) {
 		card->net[j] = netdev =
 			softing_netdev_create(card, card->id.chip[j]);
 		if (!netdev) {
 			dev_alert(&pdev->dev, "failed to make can[%i]", j);
+			ret = -ENOMEM;
 			goto netdev_failed;
 		}
 		priv = netdev_priv(card->net[j]);
diff --git a/drivers/net/can/ti_hecc.c b/drivers/net/can/ti_hecc.c
index 4accd7ec695..527dbcf9533 100644
--- a/drivers/net/can/ti_hecc.c
+++ b/drivers/net/can/ti_hecc.c
@@ -196,7 +196,7 @@ MODULE_VERSION(HECC_MODULE_VERSION);
 #define HECC_CANGIM_SIL		BIT(2)	/* system interrupts to int line 1 */
 
 /* CAN Bittiming constants as per HECC specs */
-static struct can_bittiming_const ti_hecc_bittiming_const = {
+static const struct can_bittiming_const ti_hecc_bittiming_const = {
 	.name = DRV_NAME,
 	.tseg1_min = 1,
 	.tseg1_max = 16,
diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c
index 7ae65fc8003..086fa321677 100644
--- a/drivers/net/can/usb/ems_usb.c
+++ b/drivers/net/can/usb/ems_usb.c
@@ -889,7 +889,7 @@ static const struct net_device_ops ems_usb_netdev_ops = {
 	.ndo_start_xmit = ems_usb_start_xmit,
 };
 
-static struct can_bittiming_const ems_usb_bittiming_const = {
+static const struct can_bittiming_const ems_usb_bittiming_const = {
 	.name = "ems_usb",
 	.tseg1_min = 1,
 	.tseg1_max = 16,
diff --git a/drivers/net/can/usb/esd_usb2.c b/drivers/net/can/usb/esd_usb2.c
index 09b1da5bc51..bd36e551717 100644
--- a/drivers/net/can/usb/esd_usb2.c
+++ b/drivers/net/can/usb/esd_usb2.c
@@ -871,7 +871,7 @@ static const struct net_device_ops esd_usb2_netdev_ops = {
 	.ndo_start_xmit = esd_usb2_start_xmit,
 };
 
-static struct can_bittiming_const esd_usb2_bittiming_const = {
+static const struct can_bittiming_const esd_usb2_bittiming_const = {
 	.name = "esd_usb2",
 	.tseg1_min = ESD_USB2_TSEG1_MIN,
 	.tseg1_max = ESD_USB2_TSEG1_MAX,
diff --git a/drivers/net/can/usb/peak_usb/pcan_usb_core.h b/drivers/net/can/usb/peak_usb/pcan_usb_core.h
index a948c5a8940..4c775b620be 100644
--- a/drivers/net/can/usb/peak_usb/pcan_usb_core.h
+++ b/drivers/net/can/usb/peak_usb/pcan_usb_core.h
@@ -45,7 +45,7 @@ struct peak_usb_adapter {
 	char *name;
 	u32 device_id;
 	struct can_clock clock;
-	struct can_bittiming_const bittiming_const;
+	const struct can_bittiming_const bittiming_const;
 	unsigned int ctrl_count;
 
 	int (*intf_probe)(struct usb_interface *intf);
diff --git a/drivers/net/can/vcan.c b/drivers/net/can/vcan.c
index ea2d9428593..4f93c0be005 100644
--- a/drivers/net/can/vcan.c
+++ b/drivers/net/can/vcan.c
@@ -70,13 +70,12 @@ MODULE_PARM_DESC(echo, "Echo sent frames (for testing). Default: 0 (Off)");
 
 static void vcan_rx(struct sk_buff *skb, struct net_device *dev)
 {
-	struct can_frame *cf = (struct can_frame *)skb->data;
+	struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
 	struct net_device_stats *stats = &dev->stats;
 
 	stats->rx_packets++;
-	stats->rx_bytes += cf->can_dlc;
+	stats->rx_bytes += cfd->len;
 
-	skb->protocol  = htons(ETH_P_CAN);
 	skb->pkt_type  = PACKET_BROADCAST;
 	skb->dev       = dev;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -86,7 +85,7 @@ static void vcan_rx(struct sk_buff *skb, struct net_device *dev)
 
 static netdev_tx_t vcan_tx(struct sk_buff *skb, struct net_device *dev)
 {
-	struct can_frame *cf = (struct can_frame *)skb->data;
+	struct canfd_frame *cfd = (struct canfd_frame *)skb->data;
 	struct net_device_stats *stats = &dev->stats;
 	int loop;
 
@@ -94,7 +93,7 @@ static netdev_tx_t vcan_tx(struct sk_buff *skb, struct net_device *dev)
 		return NETDEV_TX_OK;
 
 	stats->tx_packets++;
-	stats->tx_bytes += cf->can_dlc;
+	stats->tx_bytes += cfd->len;
 
 	/* set flag whether this packet has to be looped back */
 	loop = skb->pkt_type == PACKET_LOOPBACK;
@@ -108,7 +107,7 @@ static netdev_tx_t vcan_tx(struct sk_buff *skb, struct net_device *dev)
 			 * CAN core already did the echo for us
 			 */
 			stats->rx_packets++;
-			stats->rx_bytes += cf->can_dlc;
+			stats->rx_bytes += cfd->len;
 		}
 		kfree_skb(skb);
 		return NETDEV_TX_OK;
@@ -133,14 +132,28 @@ static netdev_tx_t vcan_tx(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
+static int vcan_change_mtu(struct net_device *dev, int new_mtu)
+{
+	/* Do not allow changing the MTU while running */
+	if (dev->flags & IFF_UP)
+		return -EBUSY;
+
+	if (new_mtu != CAN_MTU && new_mtu != CANFD_MTU)
+		return -EINVAL;
+
+	dev->mtu = new_mtu;
+	return 0;
+}
+
 static const struct net_device_ops vcan_netdev_ops = {
 	.ndo_start_xmit = vcan_tx,
+	.ndo_change_mtu = vcan_change_mtu,
 };
 
 static void vcan_setup(struct net_device *dev)
 {
 	dev->type		= ARPHRD_CAN;
-	dev->mtu		= sizeof(struct can_frame);
+	dev->mtu		= CAN_MTU;
 	dev->hard_header_len	= 0;
 	dev->addr_len		= 0;
 	dev->tx_queue_len	= 0;
diff --git a/drivers/net/cris/eth_v10.c b/drivers/net/cris/eth_v10.c
index 9c755db6b16..f0c8bd54ce2 100644
--- a/drivers/net/cris/eth_v10.c
+++ b/drivers/net/cris/eth_v10.c
@@ -1008,7 +1008,7 @@ e100_send_mdio_bit(unsigned char bit)
 }
 
 static unsigned char
-e100_receive_mdio_bit()
+e100_receive_mdio_bit(void)
 {
 	unsigned char bit;
 	*R_NETWORK_MGM_CTRL = 0;
diff --git a/drivers/net/dummy.c b/drivers/net/dummy.c
index bab0158f1cc..c260af5411d 100644
--- a/drivers/net/dummy.c
+++ b/drivers/net/dummy.c
@@ -40,18 +40,6 @@
 
 static int numdummies = 1;
 
-static int dummy_set_address(struct net_device *dev, void *p)
-{
-	struct sockaddr *sa = p;
-
-	if (!is_valid_ether_addr(sa->sa_data))
-		return -EADDRNOTAVAIL;
-
-	dev->addr_assign_type &= ~NET_ADDR_RANDOM;
-	memcpy(dev->dev_addr, sa->sa_data, ETH_ALEN);
-	return 0;
-}
-
 /* fake multicast ability */
 static void set_multicast_list(struct net_device *dev)
 {
@@ -75,10 +63,10 @@ static struct rtnl_link_stats64 *dummy_get_stats64(struct net_device *dev,
 
 		dstats = per_cpu_ptr(dev->dstats, i);
 		do {
-			start = u64_stats_fetch_begin(&dstats->syncp);
+			start = u64_stats_fetch_begin_bh(&dstats->syncp);
 			tbytes = dstats->tx_bytes;
 			tpackets = dstats->tx_packets;
-		} while (u64_stats_fetch_retry(&dstats->syncp, start));
+		} while (u64_stats_fetch_retry_bh(&dstats->syncp, start));
 		stats->tx_bytes += tbytes;
 		stats->tx_packets += tpackets;
 	}
@@ -118,7 +106,7 @@ static const struct net_device_ops dummy_netdev_ops = {
 	.ndo_start_xmit		= dummy_xmit,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_set_rx_mode	= set_multicast_list,
-	.ndo_set_mac_address	= dummy_set_address,
+	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_get_stats64	= dummy_get_stats64,
 };
 
@@ -134,6 +122,7 @@ static void dummy_setup(struct net_device *dev)
 	dev->tx_queue_len = 0;
 	dev->flags |= IFF_NOARP;
 	dev->flags &= ~IFF_MULTICAST;
+	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
 	dev->features	|= NETIF_F_SG | NETIF_F_FRAGLIST | NETIF_F_TSO;
 	dev->features	|= NETIF_F_HW_CSUM | NETIF_F_HIGHDMA | NETIF_F_LLTX;
 	eth_hw_addr_random(dev);
diff --git a/drivers/net/ethernet/3com/3c501.c b/drivers/net/ethernet/3com/3c501.c
index bf73e1a0229..2038eaabaea 100644
--- a/drivers/net/ethernet/3com/3c501.c
+++ b/drivers/net/ethernet/3com/3c501.c
@@ -143,7 +143,7 @@ static int irq = 5;
 static int mem_start;
 
 /**
- * el1_probe:		-	probe for a 3c501
+ * el1_probe		-	probe for a 3c501
  * @dev: The device structure passed in to probe.
  *
  * This can be called from two places. The network layer will probe using
diff --git a/drivers/net/ethernet/8390/Kconfig b/drivers/net/ethernet/8390/Kconfig
index 2e538676924..e1219e037c0 100644
--- a/drivers/net/ethernet/8390/Kconfig
+++ b/drivers/net/ethernet/8390/Kconfig
@@ -162,6 +162,20 @@ config MAC8390
 	  and read the Ethernet-HOWTO, available from
 	  <http://www.tldp.org/docs.html#howto>.
 
+config MCF8390
+	tristate "ColdFire NS8390 based Ethernet support"
+	depends on COLDFIRE
+	select CRC32
+	---help---
+	  This driver is for Ethernet devices using an NS8390-compatible
+	  chipset on many common ColdFire CPU based boards. Many of the older
+	  Freescale dev boards use this, and some other common boards like
+	  some SnapGear routers do as well.
+
+	  If you have one of these boards and want to use the network interface
+	  on them then choose Y. To compile this driver as a module, choose M
+	  here, the module will be called mcf8390.
+
 config NE2000
 	tristate "NE2000/NE1000 support"
 	depends on (ISA || (Q40 && m) || M32R || MACH_TX49XX)
diff --git a/drivers/net/ethernet/8390/Makefile b/drivers/net/ethernet/8390/Makefile
index d13790b7fd2..f43038babf8 100644
--- a/drivers/net/ethernet/8390/Makefile
+++ b/drivers/net/ethernet/8390/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_HPLAN_PLUS) += hp-plus.o 8390p.o
 obj-$(CONFIG_HPLAN) += hp.o 8390p.o
 obj-$(CONFIG_HYDRA) += hydra.o 8390.o
 obj-$(CONFIG_LNE390) += lne390.o 8390.o
+obj-$(CONFIG_MCF8390) += mcf8390.o 8390.o
 obj-$(CONFIG_NE2000) += ne.o 8390p.o
 obj-$(CONFIG_NE2_MCA) += ne2.o 8390p.o
 obj-$(CONFIG_NE2K_PCI) += ne2k-pci.o 8390.o
diff --git a/drivers/net/ethernet/8390/apne.c b/drivers/net/ethernet/8390/apne.c
index 923959275a8..912ed7a5f33 100644
--- a/drivers/net/ethernet/8390/apne.c
+++ b/drivers/net/ethernet/8390/apne.c
@@ -454,7 +454,7 @@ apne_block_input(struct net_device *dev, int count, struct sk_buff *skb, int rin
 	buf[count-1] = inb(NE_BASE + NE_DATAPORT);
       }
     } else {
-      ptrc = (char*)buf;
+      ptrc = buf;
       for (cnt = 0; cnt < count; cnt++)
         *ptrc++ = inb(NE_BASE + NE_DATAPORT);
     }
diff --git a/drivers/net/ethernet/8390/mcf8390.c b/drivers/net/ethernet/8390/mcf8390.c
new file mode 100644
index 00000000000..230efd6fa5d
--- /dev/null
+++ b/drivers/net/ethernet/8390/mcf8390.c
@@ -0,0 +1,480 @@
+/*
+ *  Support for ColdFire CPU based boards using a NS8390 Ethernet device.
+ *
+ *  Derived from the many other 8390 drivers.
+ *
+ *  (C) Copyright 2012,  Greg Ungerer <gerg@uclinux.org>
+ *
+ *  This file is subject to the terms and conditions of the GNU General Public
+ *  License.  See the file COPYING in the main directory of the Linux
+ *  distribution for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/jiffies.h>
+#include <linux/io.h>
+#include <asm/mcf8390.h>
+
+static const char version[] =
+	"mcf8390.c: (15-06-2012) Greg Ungerer <gerg@uclinux.org>";
+
+#define NE_CMD		0x00
+#define NE_DATAPORT	0x10	/* NatSemi-defined port window offset */
+#define NE_RESET	0x1f	/* Issue a read to reset ,a write to clear */
+#define NE_EN0_ISR	0x07
+#define NE_EN0_DCFG	0x0e
+#define NE_EN0_RSARLO	0x08
+#define NE_EN0_RSARHI	0x09
+#define NE_EN0_RCNTLO	0x0a
+#define NE_EN0_RXCR	0x0c
+#define NE_EN0_TXCR	0x0d
+#define NE_EN0_RCNTHI	0x0b
+#define NE_EN0_IMR	0x0f
+
+#define NESM_START_PG	0x40	/* First page of TX buffer */
+#define NESM_STOP_PG	0x80	/* Last page +1 of RX ring */
+
+#ifdef NE2000_ODDOFFSET
+/*
+ * A lot of the ColdFire boards use a separate address region for odd offset
+ * register addresses. The following functions convert and map as required.
+ * Note that the data port accesses are treated a little differently, and
+ * always accessed via the insX/outsX functions.
+ */
+static inline u32 NE_PTR(u32 addr)
+{
+	if (addr & 1)
+		return addr - 1 + NE2000_ODDOFFSET;
+	return addr;
+}
+
+static inline u32 NE_DATA_PTR(u32 addr)
+{
+	return addr;
+}
+
+void ei_outb(u32 val, u32 addr)
+{
+	NE2000_BYTE *rp;
+
+	rp = (NE2000_BYTE *) NE_PTR(addr);
+	*rp = RSWAP(val);
+}
+
+#define	ei_inb	ei_inb
+u8 ei_inb(u32 addr)
+{
+	NE2000_BYTE *rp, val;
+
+	rp = (NE2000_BYTE *) NE_PTR(addr);
+	val = *rp;
+	return (u8) (RSWAP(val) & 0xff);
+}
+
+void ei_insb(u32 addr, void *vbuf, int len)
+{
+	NE2000_BYTE *rp, val;
+	u8 *buf;
+
+	buf = (u8 *) vbuf;
+	rp = (NE2000_BYTE *) NE_DATA_PTR(addr);
+	for (; (len > 0); len--) {
+		val = *rp;
+		*buf++ = RSWAP(val);
+	}
+}
+
+void ei_insw(u32 addr, void *vbuf, int len)
+{
+	volatile u16 *rp;
+	u16 w, *buf;
+
+	buf = (u16 *) vbuf;
+	rp = (volatile u16 *) NE_DATA_PTR(addr);
+	for (; (len > 0); len--) {
+		w = *rp;
+		*buf++ = BSWAP(w);
+	}
+}
+
+void ei_outsb(u32 addr, const void *vbuf, int len)
+{
+	NE2000_BYTE *rp, val;
+	u8 *buf;
+
+	buf = (u8 *) vbuf;
+	rp = (NE2000_BYTE *) NE_DATA_PTR(addr);
+	for (; (len > 0); len--) {
+		val = *buf++;
+		*rp = RSWAP(val);
+	}
+}
+
+void ei_outsw(u32 addr, const void *vbuf, int len)
+{
+	volatile u16 *rp;
+	u16 w, *buf;
+
+	buf = (u16 *) vbuf;
+	rp = (volatile u16 *) NE_DATA_PTR(addr);
+	for (; (len > 0); len--) {
+		w = *buf++;
+		*rp = BSWAP(w);
+	}
+}
+
+#else /* !NE2000_ODDOFFSET */
+
+#define	ei_inb		inb
+#define	ei_outb		outb
+#define	ei_insb		insb
+#define	ei_insw		insw
+#define	ei_outsb	outsb
+#define	ei_outsw	outsw
+
+#endif /* !NE2000_ODDOFFSET */
+
+#define	ei_inb_p	ei_inb
+#define	ei_outb_p	ei_outb
+
+#include "lib8390.c"
+
+/*
+ * 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 mcf8390_reset_8390(struct net_device *dev)
+{
+	unsigned long reset_start_time = jiffies;
+	u32 addr = dev->base_addr;
+
+	if (ei_debug > 1)
+		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;
+
+	/* This check _should_not_ be necessary, omit eventually. */
+	while ((ei_inb(addr + NE_EN0_ISR) & ENISR_RESET) == 0) {
+		if (time_after(jiffies, reset_start_time + 2 * HZ / 100)) {
+			netdev_warn(dev, "%s: did not complete\n", __func__);
+			break;
+		}
+	}
+
+	ei_outb(ENISR_RESET, addr + NE_EN0_ISR);
+}
+
+/*
+ * This *shouldn't* happen.
+ * If it does, it's the last thing you'll see
+ */
+static void mcf8390_dmaing_err(const char *func, struct net_device *dev,
+			       struct ei_device *ei_local)
+{
+	netdev_err(dev, "%s: DMAing conflict [DMAstat:%d][irqlock:%d]\n",
+		func, ei_local->dmaing, ei_local->irqlock);
+}
+
+/*
+ * Grab the 8390 specific header. Similar to the block_input routine, but
+ * we don't need to be concerned with ring wrap as the header will be at
+ * the start of a page, so we optimize accordingly.
+ */
+static void mcf8390_get_8390_hdr(struct net_device *dev,
+				 struct e8390_pkt_hdr *hdr, int ring_page)
+{
+	struct ei_device *ei_local = netdev_priv(dev);
+	u32 addr = dev->base_addr;
+
+	if (ei_local->dmaing) {
+		mcf8390_dmaing_err(__func__, dev, ei_local);
+		return;
+	}
+
+	ei_local->dmaing |= 0x01;
+	ei_outb(E8390_NODMA + E8390_PAGE0 + E8390_START, addr + NE_CMD);
+	ei_outb(ENISR_RDC, addr + NE_EN0_ISR);
+	ei_outb(sizeof(struct e8390_pkt_hdr), addr + NE_EN0_RCNTLO);
+	ei_outb(0, addr + NE_EN0_RCNTHI);
+	ei_outb(0, addr + NE_EN0_RSARLO);		/* On page boundary */
+	ei_outb(ring_page, addr + NE_EN0_RSARHI);
+	ei_outb(E8390_RREAD + E8390_START, addr + NE_CMD);
+
+	ei_insw(addr + NE_DATAPORT, hdr, sizeof(struct e8390_pkt_hdr) >> 1);
+
+	outb(ENISR_RDC, addr + NE_EN0_ISR);	/* Ack intr */
+	ei_local->dmaing &= ~0x01;
+
+	hdr->count = cpu_to_le16(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 z_writeb.
+ */
+static void mcf8390_block_input(struct net_device *dev, int count,
+				struct sk_buff *skb, int ring_offset)
+{
+	struct ei_device *ei_local = netdev_priv(dev);
+	u32 addr = dev->base_addr;
+	char *buf = skb->data;
+
+	if (ei_local->dmaing) {
+		mcf8390_dmaing_err(__func__, dev, ei_local);
+		return;
+	}
+
+	ei_local->dmaing |= 0x01;
+	ei_outb(E8390_NODMA + E8390_PAGE0 + E8390_START, addr + NE_CMD);
+	ei_outb(ENISR_RDC, addr + NE_EN0_ISR);
+	ei_outb(count & 0xff, addr + NE_EN0_RCNTLO);
+	ei_outb(count >> 8, addr + NE_EN0_RCNTHI);
+	ei_outb(ring_offset & 0xff, addr + NE_EN0_RSARLO);
+	ei_outb(ring_offset >> 8, addr + NE_EN0_RSARHI);
+	ei_outb(E8390_RREAD + E8390_START, addr + NE_CMD);
+
+	ei_insw(addr + NE_DATAPORT, buf, count >> 1);
+	if (count & 1)
+		buf[count - 1] = ei_inb(addr + NE_DATAPORT);
+
+	ei_outb(ENISR_RDC, addr + NE_EN0_ISR);	/* Ack intr */
+	ei_local->dmaing &= ~0x01;
+}
+
+static void mcf8390_block_output(struct net_device *dev, int count,
+				 const unsigned char *buf,
+				 const int start_page)
+{
+	struct ei_device *ei_local = netdev_priv(dev);
+	u32 addr = dev->base_addr;
+	unsigned long dma_start;
+
+	/* Make sure we transfer all bytes if 16bit IO writes */
+	if (count & 0x1)
+		count++;
+
+	if (ei_local->dmaing) {
+		mcf8390_dmaing_err(__func__, dev, ei_local);
+		return;
+	}
+
+	ei_local->dmaing |= 0x01;
+	/* We should already be in page 0, but to be safe... */
+	ei_outb(E8390_PAGE0 + E8390_START + E8390_NODMA, addr + NE_CMD);
+
+	ei_outb(ENISR_RDC, addr + NE_EN0_ISR);
+
+	/* Now the normal output. */
+	ei_outb(count & 0xff, addr + NE_EN0_RCNTLO);
+	ei_outb(count >> 8, addr + NE_EN0_RCNTHI);
+	ei_outb(0x00, addr + NE_EN0_RSARLO);
+	ei_outb(start_page, addr + NE_EN0_RSARHI);
+	ei_outb(E8390_RWRITE + E8390_START, addr + NE_CMD);
+
+	ei_outsw(addr + NE_DATAPORT, buf, count >> 1);
+
+	dma_start = jiffies;
+	while ((ei_inb(addr + NE_EN0_ISR) & ENISR_RDC) == 0) {
+		if (time_after(jiffies, dma_start + 2 * HZ / 100)) { /* 20ms */
+			netdev_err(dev, "timeout waiting for Tx RDC\n");
+			mcf8390_reset_8390(dev);
+			__NS8390_init(dev, 1);
+			break;
+		}
+	}
+
+	ei_outb(ENISR_RDC, addr + NE_EN0_ISR);	/* Ack intr */
+	ei_local->dmaing &= ~0x01;
+}
+
+static const struct net_device_ops mcf8390_netdev_ops = {
+	.ndo_open		= __ei_open,
+	.ndo_stop		= __ei_close,
+	.ndo_start_xmit		= __ei_start_xmit,
+	.ndo_tx_timeout		= __ei_tx_timeout,
+	.ndo_get_stats		= __ei_get_stats,
+	.ndo_set_rx_mode	= __ei_set_multicast_list,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_change_mtu		= eth_change_mtu,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= __ei_poll,
+#endif
+};
+
+static int mcf8390_init(struct net_device *dev)
+{
+	static u32 offsets[] = {
+		0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+		0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+	};
+	struct ei_device *ei_local = netdev_priv(dev);
+	unsigned char SA_prom[32];
+	u32 addr = dev->base_addr;
+	int start_page, stop_page;
+	int i, ret;
+
+	mcf8390_reset_8390(dev);
+
+	/*
+	 * Read the 16 bytes of station address PROM.
+	 * We must first initialize registers,
+	 * similar to NS8390_init(eifdev, 0).
+	 * We can't reliably read the SAPROM address without this.
+	 * (I learned the hard way!).
+	 */
+	{
+		static const struct {
+			u32 value;
+			u32 offset;
+		} program_seq[] = {
+			{E8390_NODMA + E8390_PAGE0 + E8390_STOP, NE_CMD},
+						/* Select page 0 */
+			{0x48,	NE_EN0_DCFG},	/* 0x48: Set byte-wide access */
+			{0x00,	NE_EN0_RCNTLO},	/* Clear the count regs */
+			{0x00,	NE_EN0_RCNTHI},
+			{0x00,	NE_EN0_IMR},	/* Mask completion irq */
+			{0xFF,	NE_EN0_ISR},
+			{E8390_RXOFF, NE_EN0_RXCR}, /* 0x20 Set to monitor */
+			{E8390_TXOFF, NE_EN0_TXCR}, /* 0x02 and loopback mode */
+			{32,	NE_EN0_RCNTLO},
+			{0x00,	NE_EN0_RCNTHI},
+			{0x00,	NE_EN0_RSARLO},	/* DMA starting at 0x0000 */
+			{0x00,	NE_EN0_RSARHI},
+			{E8390_RREAD + E8390_START, NE_CMD},
+		};
+		for (i = 0; i < ARRAY_SIZE(program_seq); i++) {
+			ei_outb(program_seq[i].value,
+				 addr + program_seq[i].offset);
+		}
+	}
+
+	for (i = 0; i < 16; i++) {
+		SA_prom[i] = ei_inb(addr + NE_DATAPORT);
+		ei_inb(addr + NE_DATAPORT);
+	}
+
+	/* We must set the 8390 for word mode. */
+	ei_outb(0x49, addr + NE_EN0_DCFG);
+	start_page = NESM_START_PG;
+	stop_page = NESM_STOP_PG;
+
+	/* Install the Interrupt handler */
+	ret = request_irq(dev->irq, __ei_interrupt, 0, dev->name, dev);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ETH_ALEN; i++)
+		dev->dev_addr[i] = SA_prom[i];
+
+	netdev_dbg(dev, "Found ethernet address: %pM\n", dev->dev_addr);
+
+	ei_local->name = "mcf8390";
+	ei_local->tx_start_page = start_page;
+	ei_local->stop_page = stop_page;
+	ei_local->word16 = 1;
+	ei_local->rx_start_page = start_page + TX_PAGES;
+	ei_local->reset_8390 = mcf8390_reset_8390;
+	ei_local->block_input = mcf8390_block_input;
+	ei_local->block_output = mcf8390_block_output;
+	ei_local->get_8390_hdr = mcf8390_get_8390_hdr;
+	ei_local->reg_offset = offsets;
+
+	dev->netdev_ops = &mcf8390_netdev_ops;
+	__NS8390_init(dev, 0);
+	ret = register_netdev(dev);
+	if (ret) {
+		free_irq(dev->irq, dev);
+		return ret;
+	}
+
+	netdev_info(dev, "addr=0x%08x irq=%d, Ethernet Address %pM\n",
+		addr, dev->irq, dev->dev_addr);
+	return 0;
+}
+
+static int mcf8390_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct ei_device *ei_local;
+	struct resource *mem, *irq;
+	resource_size_t msize;
+	int ret;
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (irq == NULL) {
+		dev_err(&pdev->dev, "no IRQ specified?\n");
+		return -ENXIO;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (mem == NULL) {
+		dev_err(&pdev->dev, "no memory address specified?\n");
+		return -ENXIO;
+	}
+	msize = resource_size(mem);
+	if (!request_mem_region(mem->start, msize, pdev->name))
+		return -EBUSY;
+
+	dev = ____alloc_ei_netdev(0);
+	if (dev == NULL) {
+		release_mem_region(mem->start, msize);
+		return -ENOMEM;
+	}
+
+	SET_NETDEV_DEV(dev, &pdev->dev);
+	platform_set_drvdata(pdev, dev);
+	ei_local = netdev_priv(dev);
+
+	dev->irq = irq->start;
+	dev->base_addr = mem->start;
+
+	ret = mcf8390_init(dev);
+	if (ret) {
+		release_mem_region(mem->start, msize);
+		free_netdev(dev);
+		return ret;
+	}
+	return 0;
+}
+
+static int mcf8390_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+	struct resource *mem;
+
+	unregister_netdev(dev);
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (mem)
+		release_mem_region(mem->start, resource_size(mem));
+	free_netdev(dev);
+	return 0;
+}
+
+static struct platform_driver mcf8390_drv = {
+	.driver = {
+		.name	= "mcf8390",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= mcf8390_probe,
+	.remove		= mcf8390_remove,
+};
+
+module_platform_driver(mcf8390_drv);
+
+MODULE_DESCRIPTION("MCF8390 ColdFire NS8390 driver");
+MODULE_AUTHOR("Greg Ungerer <gerg@uclinux.org>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:mcf8390");
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index 34850117808..9c77c736f17 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -1014,7 +1014,7 @@ static int greth_set_mac_add(struct net_device *dev, void *p)
 	struct greth_regs *regs;
 
 	greth = netdev_priv(dev);
-	regs = (struct greth_regs *) greth->regs;
+	regs = greth->regs;
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
@@ -1036,7 +1036,7 @@ static void greth_set_hash_filter(struct net_device *dev)
 {
 	struct netdev_hw_addr *ha;
 	struct greth_private *greth = netdev_priv(dev);
-	struct greth_regs *regs = (struct greth_regs *) greth->regs;
+	struct greth_regs *regs = greth->regs;
 	u32 mc_filter[2];
 	unsigned int bitnr;
 
@@ -1055,7 +1055,7 @@ static void greth_set_multicast_list(struct net_device *dev)
 {
 	int cfg;
 	struct greth_private *greth = netdev_priv(dev);
-	struct greth_regs *regs = (struct greth_regs *) greth->regs;
+	struct greth_regs *regs = greth->regs;
 
 	cfg = GRETH_REGLOAD(regs->control);
 	if (dev->flags & IFF_PROMISC)
@@ -1414,7 +1414,7 @@ static int __devinit greth_of_probe(struct platform_device *ofdev)
 		goto error1;
 	}
 
-	regs = (struct greth_regs *) greth->regs;
+	regs = greth->regs;
 	greth->irq = ofdev->archdata.irqs[0];
 
 	dev_set_drvdata(greth->dev, dev);
diff --git a/drivers/net/ethernet/amd/declance.c b/drivers/net/ethernet/amd/declance.c
index 75299f500ee..7203b522f23 100644
--- a/drivers/net/ethernet/amd/declance.c
+++ b/drivers/net/ethernet/amd/declance.c
@@ -623,7 +623,7 @@ static int lance_rx(struct net_device *dev)
 			skb_put(skb, len);	/* make room */
 
 			cp_from_buf(lp->type, skb->data,
-				    (char *)lp->rx_buf_ptr_cpu[entry], len);
+				    lp->rx_buf_ptr_cpu[entry], len);
 
 			skb->protocol = eth_type_trans(skb, dev);
 			netif_rx(skb);
@@ -919,7 +919,7 @@ static int lance_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	*lib_ptr(ib, btx_ring[entry].length, lp->type) = (-len);
 	*lib_ptr(ib, btx_ring[entry].misc, lp->type) = 0;
 
-	cp_to_buf(lp->type, (char *)lp->tx_buf_ptr_cpu[entry], skb->data, len);
+	cp_to_buf(lp->type, lp->tx_buf_ptr_cpu[entry], skb->data, len);
 
 	/* Now, give the packet to the lance */
 	*lib_ptr(ib, btx_ring[entry].tmd1, lp->type) =
diff --git a/drivers/net/ethernet/amd/lance.c b/drivers/net/ethernet/amd/lance.c
index a6e2e840884..5c728436b85 100644
--- a/drivers/net/ethernet/amd/lance.c
+++ b/drivers/net/ethernet/amd/lance.c
@@ -873,10 +873,9 @@ lance_init_ring(struct net_device *dev, gfp_t gfp)
 
 		skb = alloc_skb(PKT_BUF_SZ, GFP_DMA | gfp);
 		lp->rx_skbuff[i] = skb;
-		if (skb) {
-			skb->dev = dev;
+		if (skb)
 			rx_buff = skb->data;
-		} else
+		else
 			rx_buff = kmalloc(PKT_BUF_SZ, GFP_DMA | gfp);
 		if (rx_buff == NULL)
 			lp->rx_ring[i].base = 0;
diff --git a/drivers/net/ethernet/apple/macmace.c b/drivers/net/ethernet/apple/macmace.c
index ab7ff8645ab..a92ddee7f66 100644
--- a/drivers/net/ethernet/apple/macmace.c
+++ b/drivers/net/ethernet/apple/macmace.c
@@ -228,7 +228,7 @@ static int __devinit mace_probe(struct platform_device *pdev)
 	 * bits are reversed.
 	 */
 
-	addr = (void *)MACE_PROM;
+	addr = MACE_PROM;
 
 	for (j = 0; j < 6; ++j) {
 		u8 v = bitrev8(addr[j<<4]);
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
index ff9c73859d4..21e261ffbe1 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.c
@@ -199,7 +199,7 @@ int atl1c_read_mac_addr(struct atl1c_hw *hw)
 
 	err = atl1c_get_permanent_address(hw);
 	if (err)
-		random_ether_addr(hw->perm_mac_addr);
+		eth_random_addr(hw->perm_mac_addr);
 
 	memcpy(hw->mac_addr, hw->perm_mac_addr, sizeof(hw->perm_mac_addr));
 	return err;
@@ -602,7 +602,7 @@ int atl1c_phy_reset(struct atl1c_hw *hw)
 
 int atl1c_phy_init(struct atl1c_hw *hw)
 {
-	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
+	struct atl1c_adapter *adapter = hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
 	int ret_val;
 	u16 mii_bmcr_data = BMCR_RESET;
@@ -696,7 +696,7 @@ int atl1c_get_speed_and_duplex(struct atl1c_hw *hw, u16 *speed, u16 *duplex)
 /* select one link mode to get lower power consumption */
 int atl1c_phy_to_ps_link(struct atl1c_hw *hw)
 {
-	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
+	struct atl1c_adapter *adapter = hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
 	int ret = 0;
 	u16 autoneg_advertised = ADVERTISED_10baseT_Half;
@@ -768,7 +768,7 @@ int atl1c_restart_autoneg(struct atl1c_hw *hw)
 
 int atl1c_power_saving(struct atl1c_hw *hw, u32 wufc)
 {
-	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
+	struct atl1c_adapter *adapter = hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
 	u32 master_ctrl, mac_ctrl, phy_ctrl;
 	u32 wol_ctrl, speed;
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h
index 17d935bdde0..21d8c4dbdbe 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_hw.h
@@ -74,6 +74,8 @@ void atl1c_post_phy_linkchg(struct atl1c_hw *hw, u16 link_speed);
 #define PCI_DEVICE_ID_ATHEROS_L1D_2_0	0x1083 /* AR8151 v2.0 Gigabit 1000 */
 #define L2CB_V10			0xc0
 #define L2CB_V11			0xc1
+#define L2CB_V20			0xc0
+#define L2CB_V21			0xc1
 
 /* register definition */
 #define REG_DEVICE_CAP              	0x5C
@@ -87,6 +89,9 @@ void atl1c_post_phy_linkchg(struct atl1c_hw *hw, u16 link_speed);
 #define LINK_CTRL_L1_EN			0x02
 #define LINK_CTRL_EXT_SYNC		0x80
 
+#define REG_PCIE_IND_ACC_ADDR		0x80
+#define REG_PCIE_IND_ACC_DATA		0x84
+
 #define REG_DEV_SERIALNUM_CTRL		0x200
 #define REG_DEV_MAC_SEL_MASK		0x0 /* 0:EUI; 1:MAC */
 #define REG_DEV_MAC_SEL_SHIFT		0
diff --git a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
index 9cc15701101..1bf5bbfe778 100644
--- a/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
+++ b/drivers/net/ethernet/atheros/atl1c/atl1c_main.c
@@ -166,7 +166,7 @@ static void atl1c_reset_pcie(struct atl1c_hw *hw, u32 flag)
 	msleep(5);
 }
 
-/*
+/**
  * atl1c_irq_enable - Enable default interrupt generation settings
  * @adapter: board private structure
  */
@@ -179,7 +179,7 @@ static inline void atl1c_irq_enable(struct atl1c_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1c_irq_disable - Mask off interrupt generation on the NIC
  * @adapter: board private structure
  */
@@ -192,7 +192,7 @@ static inline void atl1c_irq_disable(struct atl1c_adapter *adapter)
 	synchronize_irq(adapter->pdev->irq);
 }
 
-/*
+/**
  * atl1c_irq_reset - reset interrupt confiure on the NIC
  * @adapter: board private structure
  */
@@ -220,7 +220,7 @@ static u32 atl1c_wait_until_idle(struct atl1c_hw *hw, u32 modu_ctrl)
 	return data;
 }
 
-/*
+/**
  * atl1c_phy_config - Timer Call-back
  * @data: pointer to netdev cast into an unsigned long
  */
@@ -261,7 +261,6 @@ static void atl1c_check_link_status(struct atl1c_adapter *adapter)
 	if ((phy_data & BMSR_LSTATUS) == 0) {
 		/* link down */
 		netif_carrier_off(netdev);
-		netif_stop_queue(netdev);
 		hw->hibernate = true;
 		if (atl1c_reset_mac(hw) != 0)
 			if (netif_msg_hw(adapter))
@@ -361,7 +360,7 @@ static void atl1c_del_timer(struct atl1c_adapter *adapter)
 }
 
 
-/*
+/**
  * atl1c_tx_timeout - Respond to a Tx Hang
  * @netdev: network interface device structure
  */
@@ -374,7 +373,7 @@ static void atl1c_tx_timeout(struct net_device *netdev)
 	schedule_work(&adapter->common_task);
 }
 
-/*
+/**
  * atl1c_set_multi - Multicast and Promiscuous mode set
  * @netdev: network interface device structure
  *
@@ -453,7 +452,7 @@ static void atl1c_restore_vlan(struct atl1c_adapter *adapter)
 	atl1c_vlan_mode(adapter->netdev, adapter->netdev->features);
 }
 
-/*
+/**
  * atl1c_set_mac - Change the Ethernet Address of the NIC
  * @netdev: network interface device structure
  * @p: pointer to an address structure
@@ -518,7 +517,7 @@ static int atl1c_set_features(struct net_device *netdev,
 	return 0;
 }
 
-/*
+/**
  * atl1c_change_mtu - Change the Maximum Transfer Unit
  * @netdev: network interface device structure
  * @new_mtu: new value for maximum frame size
@@ -577,12 +576,6 @@ static void atl1c_mdio_write(struct net_device *netdev, int phy_id,
 	atl1c_write_phy_reg(&adapter->hw, reg_num, val);
 }
 
-/*
- * atl1c_mii_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl1c_mii_ioctl(struct net_device *netdev,
 			   struct ifreq *ifr, int cmd)
 {
@@ -633,12 +626,6 @@ out:
 	return retval;
 }
 
-/*
- * atl1c_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl1c_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 {
 	switch (cmd) {
@@ -651,7 +638,7 @@ static int atl1c_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 	}
 }
 
-/*
+/**
  * atl1c_alloc_queues - Allocate memory for all rings
  * @adapter: board private structure to initialize
  *
@@ -740,6 +727,8 @@ static const struct atl1c_platform_patch plats[] __devinitdata = {
 
 static void __devinit atl1c_patch_assign(struct atl1c_hw *hw)
 {
+	struct pci_dev	*pdev = hw->adapter->pdev;
+	u32 misc_ctrl;
 	int i = 0;
 
 	hw->msi_lnkpatch = false;
@@ -754,8 +743,20 @@ static void __devinit atl1c_patch_assign(struct atl1c_hw *hw)
 		}
 		i++;
 	}
+
+	if (hw->device_id == PCI_DEVICE_ID_ATHEROS_L2C_B2 &&
+	    hw->revision_id == L2CB_V21) {
+		/* config acess mode */
+		pci_write_config_dword(pdev, REG_PCIE_IND_ACC_ADDR,
+				       REG_PCIE_DEV_MISC_CTRL);
+		pci_read_config_dword(pdev, REG_PCIE_IND_ACC_DATA, &misc_ctrl);
+		misc_ctrl &= ~0x100;
+		pci_write_config_dword(pdev, REG_PCIE_IND_ACC_ADDR,
+				       REG_PCIE_DEV_MISC_CTRL);
+		pci_write_config_dword(pdev, REG_PCIE_IND_ACC_DATA, misc_ctrl);
+	}
 }
-/*
+/**
  * atl1c_sw_init - Initialize general software structures (struct atl1c_adapter)
  * @adapter: board private structure to initialize
  *
@@ -781,7 +782,7 @@ static int __devinit atl1c_sw_init(struct atl1c_adapter *adapter)
 	hw->device_id = pdev->device;
 	hw->subsystem_vendor_id = pdev->subsystem_vendor;
 	hw->subsystem_id = pdev->subsystem_device;
-	AT_READ_REG(hw, PCI_CLASS_REVISION, &revision);
+	pci_read_config_dword(pdev, PCI_CLASS_REVISION, &revision);
 	hw->revision_id = revision & 0xFF;
 	/* before link up, we assume hibernate is true */
 	hw->hibernate = true;
@@ -853,7 +854,7 @@ static inline void atl1c_clean_buffer(struct pci_dev *pdev,
 	buffer_info->skb = NULL;
 	ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_FREE);
 }
-/*
+/**
  * atl1c_clean_tx_ring - Free Tx-skb
  * @adapter: board private structure
  */
@@ -878,7 +879,7 @@ static void atl1c_clean_tx_ring(struct atl1c_adapter *adapter,
 	tpd_ring->next_to_use = 0;
 }
 
-/*
+/**
  * atl1c_clean_rx_ring - Free rx-reservation skbs
  * @adapter: board private structure
  */
@@ -931,7 +932,7 @@ static void atl1c_init_ring_ptrs(struct atl1c_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1c_free_ring_resources - Free Tx / RX descriptor Resources
  * @adapter: board private structure
  *
@@ -954,7 +955,7 @@ static void atl1c_free_ring_resources(struct atl1c_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1c_setup_mem_resources - allocate Tx / RX descriptor resources
  * @adapter: board private structure
  *
@@ -989,12 +990,12 @@ static int atl1c_setup_ring_resources(struct atl1c_adapter *adapter)
 	}
 	for (i = 0; i < AT_MAX_TRANSMIT_QUEUE; i++) {
 		tpd_ring[i].buffer_info =
-			(struct atl1c_buffer *) (tpd_ring->buffer_info + count);
+			(tpd_ring->buffer_info + count);
 		count += tpd_ring[i].count;
 	}
 
 	rfd_ring->buffer_info =
-		(struct atl1c_buffer *) (tpd_ring->buffer_info + count);
+		(tpd_ring->buffer_info + count);
 	count += rfd_ring->count;
 	rx_desc_count += rfd_ring->count;
 
@@ -1227,7 +1228,7 @@ static void atl1c_start_mac(struct atl1c_adapter *adapter)
  */
 static int atl1c_reset_mac(struct atl1c_hw *hw)
 {
-	struct atl1c_adapter *adapter = (struct atl1c_adapter *)hw->adapter;
+	struct atl1c_adapter *adapter = hw->adapter;
 	struct pci_dev *pdev = adapter->pdev;
 	u32 ctrl_data = 0;
 
@@ -1363,7 +1364,7 @@ static void atl1c_set_aspm(struct atl1c_hw *hw, u16 link_speed)
 	return;
 }
 
-/*
+/**
  * atl1c_configure - Configure Transmit&Receive Unit after Reset
  * @adapter: board private structure
  *
@@ -1477,7 +1478,7 @@ static void atl1c_update_hw_stats(struct atl1c_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1c_get_stats - Get System Network Statistics
  * @netdev: network interface device structure
  *
@@ -1531,8 +1532,7 @@ static inline void atl1c_clear_phy_int(struct atl1c_adapter *adapter)
 static bool atl1c_clean_tx_irq(struct atl1c_adapter *adapter,
 				enum atl1c_trans_queue type)
 {
-	struct atl1c_tpd_ring *tpd_ring = (struct atl1c_tpd_ring *)
-				&adapter->tpd_ring[type];
+	struct atl1c_tpd_ring *tpd_ring = &adapter->tpd_ring[type];
 	struct atl1c_buffer *buffer_info;
 	struct pci_dev *pdev = adapter->pdev;
 	u16 next_to_clean = atomic_read(&tpd_ring->next_to_clean);
@@ -1559,11 +1559,10 @@ static bool atl1c_clean_tx_irq(struct atl1c_adapter *adapter,
 	return true;
 }
 
-/*
+/**
  * atl1c_intr - Interrupt Handler
  * @irq: interrupt number
  * @data: pointer to a network interface device structure
- * @pt_regs: CPU registers structure
  */
 static irqreturn_t atl1c_intr(int irq, void *data)
 {
@@ -1814,9 +1813,8 @@ rrs_checked:
 		atl1c_alloc_rx_buffer(adapter);
 }
 
-/*
+/**
  * atl1c_clean - NAPI Rx polling callback
- * @adapter: board private structure
  */
 static int atl1c_clean(struct napi_struct *napi, int budget)
 {
@@ -2271,7 +2269,7 @@ static void atl1c_down(struct atl1c_adapter *adapter)
 	atl1c_reset_dma_ring(adapter);
 }
 
-/*
+/**
  * atl1c_open - Called when a network interface is made active
  * @netdev: network interface device structure
  *
@@ -2310,7 +2308,7 @@ err_up:
 	return err;
 }
 
-/*
+/**
  * atl1c_close - Disables a network interface
  * @netdev: network interface device structure
  *
@@ -2433,7 +2431,7 @@ static int atl1c_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 	return 0;
 }
 
-/*
+/**
  * atl1c_probe - Device Initialization Routine
  * @pdev: PCI device information struct
  * @ent: entry in atl1c_pci_tbl
@@ -2580,7 +2578,7 @@ err_dma:
 	return err;
 }
 
-/*
+/**
  * atl1c_remove - Device Removal Routine
  * @pdev: PCI device information struct
  *
@@ -2606,7 +2604,7 @@ static void __devexit atl1c_remove(struct pci_dev *pdev)
 	free_netdev(netdev);
 }
 
-/*
+/**
  * atl1c_io_error_detected - called when PCI error is detected
  * @pdev: Pointer to PCI device
  * @state: The current pci connection state
@@ -2634,7 +2632,7 @@ static pci_ers_result_t atl1c_io_error_detected(struct pci_dev *pdev,
 	return PCI_ERS_RESULT_NEED_RESET;
 }
 
-/*
+/**
  * atl1c_io_slot_reset - called after the pci bus has been reset.
  * @pdev: Pointer to PCI device
  *
@@ -2662,7 +2660,7 @@ static pci_ers_result_t atl1c_io_slot_reset(struct pci_dev *pdev)
 	return PCI_ERS_RESULT_RECOVERED;
 }
 
-/*
+/**
  * atl1c_io_resume - called when traffic can start flowing again.
  * @pdev: Pointer to PCI device
  *
@@ -2705,7 +2703,7 @@ static struct pci_driver atl1c_driver = {
 	.driver.pm = &atl1c_pm_ops,
 };
 
-/*
+/**
  * atl1c_init_module - Driver Registration Routine
  *
  * atl1c_init_module is the first routine called when the driver is
@@ -2716,7 +2714,7 @@ static int __init atl1c_init_module(void)
 	return pci_register_driver(&atl1c_driver);
 }
 
-/*
+/**
  * atl1c_exit_module - Driver Exit Cleanup Routine
  *
  * atl1c_exit_module is called just before the driver is removed
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_ethtool.c b/drivers/net/ethernet/atheros/atl1e/atl1e_ethtool.c
index 6e61f9f9ebb..82b23861bf5 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_ethtool.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_ethtool.c
@@ -268,7 +268,7 @@ static int atl1e_set_eeprom(struct net_device *netdev,
 	if (eeprom_buff == NULL)
 		return -ENOMEM;
 
-	ptr = (u32 *)eeprom_buff;
+	ptr = eeprom_buff;
 
 	if (eeprom->offset & 3) {
 		/* need read/modify/write of first changed EEPROM word */
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
index 1220e511ced..a98acc8a956 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_main.c
@@ -89,7 +89,7 @@ static const u16 atl1e_pay_load_size[] = {
 	128, 256, 512, 1024, 2048, 4096,
 };
 
-/*
+/**
  * atl1e_irq_enable - Enable default interrupt generation settings
  * @adapter: board private structure
  */
@@ -102,7 +102,7 @@ static inline void atl1e_irq_enable(struct atl1e_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1e_irq_disable - Mask off interrupt generation on the NIC
  * @adapter: board private structure
  */
@@ -114,7 +114,7 @@ static inline void atl1e_irq_disable(struct atl1e_adapter *adapter)
 	synchronize_irq(adapter->pdev->irq);
 }
 
-/*
+/**
  * atl1e_irq_reset - reset interrupt confiure on the NIC
  * @adapter: board private structure
  */
@@ -126,7 +126,7 @@ static inline void atl1e_irq_reset(struct atl1e_adapter *adapter)
 	AT_WRITE_FLUSH(&adapter->hw);
 }
 
-/*
+/**
  * atl1e_phy_config - Timer Call-back
  * @data: pointer to netdev cast into an unsigned long
  */
@@ -210,7 +210,7 @@ static int atl1e_check_link(struct atl1e_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl1e_link_chg_task - deal with link change event Out of interrupt context
  * @netdev: network interface device structure
  */
@@ -259,7 +259,7 @@ static void atl1e_cancel_work(struct atl1e_adapter *adapter)
 	cancel_work_sync(&adapter->link_chg_task);
 }
 
-/*
+/**
  * atl1e_tx_timeout - Respond to a Tx Hang
  * @netdev: network interface device structure
  */
@@ -271,7 +271,7 @@ static void atl1e_tx_timeout(struct net_device *netdev)
 	schedule_work(&adapter->reset_task);
 }
 
-/*
+/**
  * atl1e_set_multi - Multicast and Promiscuous mode set
  * @netdev: network interface device structure
  *
@@ -345,7 +345,7 @@ static void atl1e_restore_vlan(struct atl1e_adapter *adapter)
 	atl1e_vlan_mode(adapter->netdev, adapter->netdev->features);
 }
 
-/*
+/**
  * atl1e_set_mac - Change the Ethernet Address of the NIC
  * @netdev: network interface device structure
  * @p: pointer to an address structure
@@ -397,7 +397,7 @@ static int atl1e_set_features(struct net_device *netdev,
 	return 0;
 }
 
-/*
+/**
  * atl1e_change_mtu - Change the Maximum Transfer Unit
  * @netdev: network interface device structure
  * @new_mtu: new value for maximum frame size
@@ -449,12 +449,6 @@ static void atl1e_mdio_write(struct net_device *netdev, int phy_id,
 	atl1e_write_phy_reg(&adapter->hw, reg_num & MDIO_REG_ADDR_MASK, val);
 }
 
-/*
- * atl1e_mii_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl1e_mii_ioctl(struct net_device *netdev,
 			   struct ifreq *ifr, int cmd)
 {
@@ -505,12 +499,6 @@ out:
 
 }
 
-/*
- * atl1e_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl1e_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 {
 	switch (cmd) {
@@ -541,7 +529,7 @@ static void atl1e_setup_pcicmd(struct pci_dev *pdev)
 	msleep(1);
 }
 
-/*
+/**
  * atl1e_alloc_queues - Allocate memory for all rings
  * @adapter: board private structure to initialize
  *
@@ -551,7 +539,7 @@ static int __devinit atl1e_alloc_queues(struct atl1e_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl1e_sw_init - Initialize general software structures (struct atl1e_adapter)
  * @adapter: board private structure to initialize
  *
@@ -635,14 +623,13 @@ static int __devinit atl1e_sw_init(struct atl1e_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl1e_clean_tx_ring - Free Tx-skb
  * @adapter: board private structure
  */
 static void atl1e_clean_tx_ring(struct atl1e_adapter *adapter)
 {
-	struct atl1e_tx_ring *tx_ring = (struct atl1e_tx_ring *)
-				&adapter->tx_ring;
+	struct atl1e_tx_ring *tx_ring = &adapter->tx_ring;
 	struct atl1e_tx_buffer *tx_buffer = NULL;
 	struct pci_dev *pdev = adapter->pdev;
 	u16 index, ring_count;
@@ -679,14 +666,14 @@ static void atl1e_clean_tx_ring(struct atl1e_adapter *adapter)
 				ring_count);
 }
 
-/*
+/**
  * atl1e_clean_rx_ring - Free rx-reservation skbs
  * @adapter: board private structure
  */
 static void atl1e_clean_rx_ring(struct atl1e_adapter *adapter)
 {
 	struct atl1e_rx_ring *rx_ring =
-		(struct atl1e_rx_ring *)&adapter->rx_ring;
+		&adapter->rx_ring;
 	struct atl1e_rx_page_desc *rx_page_desc = rx_ring->rx_page_desc;
 	u16 i, j;
 
@@ -762,7 +749,7 @@ static void atl1e_init_ring_ptrs(struct atl1e_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1e_free_ring_resources - Free Tx / RX descriptor Resources
  * @adapter: board private structure
  *
@@ -787,7 +774,7 @@ static void atl1e_free_ring_resources(struct atl1e_adapter *adapter)
 	}
 }
 
-/*
+/**
  * atl1e_setup_mem_resources - allocate Tx / RX descriptor resources
  * @adapter: board private structure
  *
@@ -884,14 +871,12 @@ failed:
 	return err;
 }
 
-static inline void atl1e_configure_des_ring(const struct atl1e_adapter *adapter)
+static inline void atl1e_configure_des_ring(struct atl1e_adapter *adapter)
 {
 
-	struct atl1e_hw *hw = (struct atl1e_hw *)&adapter->hw;
-	struct atl1e_rx_ring *rx_ring =
-			(struct atl1e_rx_ring *)&adapter->rx_ring;
-	struct atl1e_tx_ring *tx_ring =
-			(struct atl1e_tx_ring *)&adapter->tx_ring;
+	struct atl1e_hw *hw = &adapter->hw;
+	struct atl1e_rx_ring *rx_ring = &adapter->rx_ring;
+	struct atl1e_tx_ring *tx_ring = &adapter->tx_ring;
 	struct atl1e_rx_page_desc *rx_page_desc = NULL;
 	int i, j;
 
@@ -932,7 +917,7 @@ static inline void atl1e_configure_des_ring(const struct atl1e_adapter *adapter)
 
 static inline void atl1e_configure_tx(struct atl1e_adapter *adapter)
 {
-	struct atl1e_hw *hw = (struct atl1e_hw *)&adapter->hw;
+	struct atl1e_hw *hw = &adapter->hw;
 	u32 dev_ctrl_data = 0;
 	u32 max_pay_load = 0;
 	u32 jumbo_thresh = 0;
@@ -975,7 +960,7 @@ static inline void atl1e_configure_tx(struct atl1e_adapter *adapter)
 
 static inline void atl1e_configure_rx(struct atl1e_adapter *adapter)
 {
-	struct atl1e_hw *hw = (struct atl1e_hw *)&adapter->hw;
+	struct atl1e_hw *hw = &adapter->hw;
 	u32 rxf_len  = 0;
 	u32 rxf_low  = 0;
 	u32 rxf_high = 0;
@@ -1078,7 +1063,7 @@ static void atl1e_setup_mac_ctrl(struct atl1e_adapter *adapter)
 	AT_WRITE_REG(hw, REG_MAC_CTRL, value);
 }
 
-/*
+/**
  * atl1e_configure - Configure Transmit&Receive Unit after Reset
  * @adapter: board private structure
  *
@@ -1148,7 +1133,7 @@ static int atl1e_configure(struct atl1e_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl1e_get_stats - Get System Network Statistics
  * @netdev: network interface device structure
  *
@@ -1224,8 +1209,7 @@ static inline void atl1e_clear_phy_int(struct atl1e_adapter *adapter)
 
 static bool atl1e_clean_tx_irq(struct atl1e_adapter *adapter)
 {
-	struct atl1e_tx_ring *tx_ring = (struct atl1e_tx_ring *)
-					&adapter->tx_ring;
+	struct atl1e_tx_ring *tx_ring = &adapter->tx_ring;
 	struct atl1e_tx_buffer *tx_buffer = NULL;
 	u16 hw_next_to_clean = AT_READ_REGW(&adapter->hw, REG_TPD_CONS_IDX);
 	u16 next_to_clean = atomic_read(&tx_ring->next_to_clean);
@@ -1261,11 +1245,10 @@ static bool atl1e_clean_tx_irq(struct atl1e_adapter *adapter)
 	return true;
 }
 
-/*
+/**
  * atl1e_intr - Interrupt Handler
  * @irq: interrupt number
  * @data: pointer to a network interface device structure
- * @pt_regs: CPU registers structure
  */
 static irqreturn_t atl1e_intr(int irq, void *data)
 {
@@ -1384,15 +1367,14 @@ static struct atl1e_rx_page *atl1e_get_rx_page(struct atl1e_adapter *adapter,
 		(struct atl1e_rx_page_desc *) adapter->rx_ring.rx_page_desc;
 	u8 rx_using = rx_page_desc[que].rx_using;
 
-	return (struct atl1e_rx_page *)&(rx_page_desc[que].rx_page[rx_using]);
+	return &(rx_page_desc[que].rx_page[rx_using]);
 }
 
 static void atl1e_clean_rx_irq(struct atl1e_adapter *adapter, u8 que,
 		   int *work_done, int work_to_do)
 {
 	struct net_device *netdev  = adapter->netdev;
-	struct atl1e_rx_ring *rx_ring = (struct atl1e_rx_ring *)
-					 &adapter->rx_ring;
+	struct atl1e_rx_ring *rx_ring = &adapter->rx_ring;
 	struct atl1e_rx_page_desc *rx_page_desc =
 		(struct atl1e_rx_page_desc *) rx_ring->rx_page_desc;
 	struct sk_buff *skb = NULL;
@@ -1494,9 +1476,8 @@ fatal_err:
 		schedule_work(&adapter->reset_task);
 }
 
-/*
+/**
  * atl1e_clean - NAPI Rx polling callback
- * @adapter: board private structure
  */
 static int atl1e_clean(struct napi_struct *napi, int budget)
 {
@@ -1576,7 +1557,7 @@ static struct atl1e_tpd_desc *atl1e_get_tpd(struct atl1e_adapter *adapter)
 		tx_ring->next_to_use = 0;
 
 	memset(&tx_ring->desc[next_to_use], 0, sizeof(struct atl1e_tpd_desc));
-	return (struct atl1e_tpd_desc *)&tx_ring->desc[next_to_use];
+	return &tx_ring->desc[next_to_use];
 }
 
 static struct atl1e_tx_buffer *
@@ -1961,7 +1942,7 @@ void atl1e_down(struct atl1e_adapter *adapter)
 	atl1e_clean_rx_ring(adapter);
 }
 
-/*
+/**
  * atl1e_open - Called when a network interface is made active
  * @netdev: network interface device structure
  *
@@ -2007,7 +1988,7 @@ err_req_irq:
 	return err;
 }
 
-/*
+/**
  * atl1e_close - Disables a network interface
  * @netdev: network interface device structure
  *
@@ -2061,8 +2042,8 @@ static int atl1e_suspend(struct pci_dev *pdev, pm_message_t state)
 
 	if (wufc) {
 		/* get link status */
-		atl1e_read_phy_reg(hw, MII_BMSR, (u16 *)&mii_bmsr_data);
-		atl1e_read_phy_reg(hw, MII_BMSR, (u16 *)&mii_bmsr_data);
+		atl1e_read_phy_reg(hw, MII_BMSR, &mii_bmsr_data);
+		atl1e_read_phy_reg(hw, MII_BMSR, &mii_bmsr_data);
 
 		mii_advertise_data = ADVERTISE_10HALF;
 
@@ -2086,7 +2067,7 @@ static int atl1e_suspend(struct pci_dev *pdev, pm_message_t state)
 				for (i = 0; i < AT_SUSPEND_LINK_TIMEOUT; i++) {
 					msleep(100);
 					atl1e_read_phy_reg(hw, MII_BMSR,
-							(u16 *)&mii_bmsr_data);
+							&mii_bmsr_data);
 					if (mii_bmsr_data & BMSR_LSTATUS)
 						break;
 				}
@@ -2243,7 +2224,7 @@ static int atl1e_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 	return 0;
 }
 
-/*
+/**
  * atl1e_probe - Device Initialization Routine
  * @pdev: PCI device information struct
  * @ent: entry in atl1e_pci_tbl
@@ -2397,7 +2378,7 @@ err_dma:
 	return err;
 }
 
-/*
+/**
  * atl1e_remove - Device Removal Routine
  * @pdev: PCI device information struct
  *
@@ -2429,7 +2410,7 @@ static void __devexit atl1e_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 }
 
-/*
+/**
  * atl1e_io_error_detected - called when PCI error is detected
  * @pdev: Pointer to PCI device
  * @state: The current pci connection state
@@ -2457,7 +2438,7 @@ atl1e_io_error_detected(struct pci_dev *pdev, pci_channel_state_t state)
 	return PCI_ERS_RESULT_NEED_RESET;
 }
 
-/*
+/**
  * atl1e_io_slot_reset - called after the pci bus has been reset.
  * @pdev: Pointer to PCI device
  *
@@ -2484,7 +2465,7 @@ static pci_ers_result_t atl1e_io_slot_reset(struct pci_dev *pdev)
 	return PCI_ERS_RESULT_RECOVERED;
 }
 
-/*
+/**
  * atl1e_io_resume - called when traffic can start flowing again.
  * @pdev: Pointer to PCI device
  *
@@ -2528,7 +2509,7 @@ static struct pci_driver atl1e_driver = {
 	.err_handler = &atl1e_err_handler
 };
 
-/*
+/**
  * atl1e_init_module - Driver Registration Routine
  *
  * atl1e_init_module is the first routine called when the driver is
@@ -2539,7 +2520,7 @@ static int __init atl1e_init_module(void)
 	return pci_register_driver(&atl1e_driver);
 }
 
-/*
+/**
  * atl1e_exit_module - Driver Exit Cleanup Routine
  *
  * atl1e_exit_module is called just before the driver is removed
diff --git a/drivers/net/ethernet/atheros/atl1e/atl1e_param.c b/drivers/net/ethernet/atheros/atl1e/atl1e_param.c
index 0ce60b6e7ef..b5086f1e637 100644
--- a/drivers/net/ethernet/atheros/atl1e/atl1e_param.c
+++ b/drivers/net/ethernet/atheros/atl1e/atl1e_param.c
@@ -168,7 +168,7 @@ static int __devinit atl1e_validate_option(int *value, struct atl1e_option *opt,
 	return -1;
 }
 
-/*
+/**
  * atl1e_check_options - Range Checking for Command Line Parameters
  * @adapter: board private structure
  *
diff --git a/drivers/net/ethernet/atheros/atlx/atl1.c b/drivers/net/ethernet/atheros/atlx/atl1.c
index 5d10884e508..7bae2ad7a7c 100644
--- a/drivers/net/ethernet/atheros/atlx/atl1.c
+++ b/drivers/net/ethernet/atheros/atlx/atl1.c
@@ -195,7 +195,7 @@ static int __devinit atl1_validate_option(int *value, struct atl1_option *opt,
 	return -1;
 }
 
-/*
+/**
  * atl1_check_options - Range Checking for Command Line Parameters
  * @adapter: board private structure
  *
@@ -538,7 +538,7 @@ static s32 atl1_read_mac_addr(struct atl1_hw *hw)
 	u16 i;
 
 	if (atl1_get_permanent_address(hw)) {
-		random_ether_addr(hw->perm_mac_addr);
+		eth_random_addr(hw->perm_mac_addr);
 		ret = 1;
 	}
 
@@ -937,7 +937,7 @@ static void atl1_set_mac_addr(struct atl1_hw *hw)
 	iowrite32(value, (hw->hw_addr + REG_MAC_STA_ADDR) + (1 << 2));
 }
 
-/*
+/**
  * atl1_sw_init - Initialize general software structures (struct atl1_adapter)
  * @adapter: board private structure to initialize
  *
@@ -1014,12 +1014,6 @@ static void mdio_write(struct net_device *netdev, int phy_id, int reg_num,
 	atl1_write_phy_reg(&adapter->hw, reg_num, val);
 }
 
-/*
- * atl1_mii_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl1_mii_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 {
 	struct atl1_adapter *adapter = netdev_priv(netdev);
@@ -1036,7 +1030,7 @@ static int atl1_mii_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 	return retval;
 }
 
-/*
+/**
  * atl1_setup_mem_resources - allocate Tx / RX descriptor resources
  * @adapter: board private structure
  *
@@ -1061,7 +1055,7 @@ static s32 atl1_setup_ring_resources(struct atl1_adapter *adapter)
 		goto err_nomem;
 	}
 	rfd_ring->buffer_info =
-		(struct atl1_buffer *)(tpd_ring->buffer_info + tpd_ring->count);
+		(tpd_ring->buffer_info + tpd_ring->count);
 
 	/*
 	 * real ring DMA buffer
@@ -1147,7 +1141,7 @@ static void atl1_init_ring_ptrs(struct atl1_adapter *adapter)
 	atomic_set(&rrd_ring->next_to_clean, 0);
 }
 
-/*
+/**
  * atl1_clean_rx_ring - Free RFD Buffers
  * @adapter: board private structure
  */
@@ -1187,7 +1181,7 @@ static void atl1_clean_rx_ring(struct atl1_adapter *adapter)
 	atomic_set(&rrd_ring->next_to_clean, 0);
 }
 
-/*
+/**
  * atl1_clean_tx_ring - Free Tx Buffers
  * @adapter: board private structure
  */
@@ -1227,7 +1221,7 @@ static void atl1_clean_tx_ring(struct atl1_adapter *adapter)
 	atomic_set(&tpd_ring->next_to_clean, 0);
 }
 
-/*
+/**
  * atl1_free_ring_resources - Free Tx / RX descriptor Resources
  * @adapter: board private structure
  *
@@ -1470,7 +1464,7 @@ static void set_flow_ctrl_new(struct atl1_hw *hw)
 	iowrite32(value, hw->hw_addr + REG_RXQ_RRD_PAUSE_THRESH);
 }
 
-/*
+/**
  * atl1_configure - Configure Transmit&Receive Unit after Reset
  * @adapter: board private structure
  *
@@ -1844,7 +1838,7 @@ static void atl1_rx_checksum(struct atl1_adapter *adapter,
 	}
 }
 
-/*
+/**
  * atl1_alloc_rx_buffers - Replace used receive buffers
  * @adapter: address of board private structure
  */
@@ -2489,11 +2483,10 @@ static inline int atl1_sched_rings_clean(struct atl1_adapter* adapter)
 	return 1;
 }
 
-/*
+/**
  * atl1_intr - Interrupt Handler
  * @irq: interrupt number
  * @data: pointer to a network interface device structure
- * @pt_regs: CPU registers structure
  */
 static irqreturn_t atl1_intr(int irq, void *data)
 {
@@ -2574,7 +2567,7 @@ static irqreturn_t atl1_intr(int irq, void *data)
 }
 
 
-/*
+/**
  * atl1_phy_config - Timer Call-back
  * @data: pointer to netdev cast into an unsigned long
  */
@@ -2693,7 +2686,7 @@ static void atl1_reset_dev_task(struct work_struct *work)
 	netif_device_attach(netdev);
 }
 
-/*
+/**
  * atl1_change_mtu - Change the Maximum Transfer Unit
  * @netdev: network interface device structure
  * @new_mtu: new value for maximum frame size
@@ -2727,7 +2720,7 @@ static int atl1_change_mtu(struct net_device *netdev, int new_mtu)
 	return 0;
 }
 
-/*
+/**
  * atl1_open - Called when a network interface is made active
  * @netdev: network interface device structure
  *
@@ -2762,7 +2755,7 @@ err_up:
 	return err;
 }
 
-/*
+/**
  * atl1_close - Disables a network interface
  * @netdev: network interface device structure
  *
@@ -2930,7 +2923,7 @@ static const struct net_device_ops atl1_netdev_ops = {
 #endif
 };
 
-/*
+/**
  * atl1_probe - Device Initialization Routine
  * @pdev: PCI device information struct
  * @ent: entry in atl1_pci_tbl
@@ -3111,7 +3104,7 @@ err_request_regions:
 	return err;
 }
 
-/*
+/**
  * atl1_remove - Device Removal Routine
  * @pdev: PCI device information struct
  *
@@ -3158,7 +3151,7 @@ static struct pci_driver atl1_driver = {
 	.driver.pm = ATL1_PM_OPS,
 };
 
-/*
+/**
  * atl1_exit_module - Driver Exit Cleanup Routine
  *
  * atl1_exit_module is called just before the driver is removed
@@ -3169,7 +3162,7 @@ static void __exit atl1_exit_module(void)
 	pci_unregister_driver(&atl1_driver);
 }
 
-/*
+/**
  * atl1_init_module - Driver Registration Routine
  *
  * atl1_init_module is the first routine called when the driver is
diff --git a/drivers/net/ethernet/atheros/atlx/atl2.c b/drivers/net/ethernet/atheros/atlx/atl2.c
index 6762dc406b2..57d64b80fd7 100644
--- a/drivers/net/ethernet/atheros/atlx/atl2.c
+++ b/drivers/net/ethernet/atheros/atlx/atl2.c
@@ -75,7 +75,7 @@ static void atl2_set_ethtool_ops(struct net_device *netdev);
 
 static void atl2_check_options(struct atl2_adapter *adapter);
 
-/*
+/**
  * atl2_sw_init - Initialize general software structures (struct atl2_adapter)
  * @adapter: board private structure to initialize
  *
@@ -123,7 +123,7 @@ static int __devinit atl2_sw_init(struct atl2_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl2_set_multi - Multicast and Promiscuous mode set
  * @netdev: network interface device structure
  *
@@ -177,7 +177,7 @@ static void init_ring_ptrs(struct atl2_adapter *adapter)
 	adapter->txs_next_clear = 0;
 }
 
-/*
+/**
  * atl2_configure - Configure Transmit&Receive Unit after Reset
  * @adapter: board private structure
  *
@@ -283,7 +283,7 @@ static int atl2_configure(struct atl2_adapter *adapter)
 	return value;
 }
 
-/*
+/**
  * atl2_setup_ring_resources - allocate Tx / RX descriptor resources
  * @adapter: board private structure
  *
@@ -340,7 +340,7 @@ static s32 atl2_setup_ring_resources(struct atl2_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl2_irq_enable - Enable default interrupt generation settings
  * @adapter: board private structure
  */
@@ -350,7 +350,7 @@ static inline void atl2_irq_enable(struct atl2_adapter *adapter)
 	ATL2_WRITE_FLUSH(&adapter->hw);
 }
 
-/*
+/**
  * atl2_irq_disable - Mask off interrupt generation on the NIC
  * @adapter: board private structure
  */
@@ -599,11 +599,10 @@ static inline void atl2_clear_phy_int(struct atl2_adapter *adapter)
 	spin_unlock(&adapter->stats_lock);
 }
 
-/*
+/**
  * atl2_intr - Interrupt Handler
  * @irq: interrupt number
  * @data: pointer to a network interface device structure
- * @pt_regs: CPU registers structure
  */
 static irqreturn_t atl2_intr(int irq, void *data)
 {
@@ -679,7 +678,7 @@ static int atl2_request_irq(struct atl2_adapter *adapter)
 		netdev);
 }
 
-/*
+/**
  * atl2_free_ring_resources - Free Tx / RX descriptor Resources
  * @adapter: board private structure
  *
@@ -692,7 +691,7 @@ static void atl2_free_ring_resources(struct atl2_adapter *adapter)
 		adapter->ring_dma);
 }
 
-/*
+/**
  * atl2_open - Called when a network interface is made active
  * @netdev: network interface device structure
  *
@@ -798,7 +797,7 @@ static void atl2_free_irq(struct atl2_adapter *adapter)
 #endif
 }
 
-/*
+/**
  * atl2_close - Disables a network interface
  * @netdev: network interface device structure
  *
@@ -918,7 +917,7 @@ static netdev_tx_t atl2_xmit_frame(struct sk_buff *skb,
 	return NETDEV_TX_OK;
 }
 
-/*
+/**
  * atl2_change_mtu - Change the Maximum Transfer Unit
  * @netdev: network interface device structure
  * @new_mtu: new value for maximum frame size
@@ -943,7 +942,7 @@ static int atl2_change_mtu(struct net_device *netdev, int new_mtu)
 	return 0;
 }
 
-/*
+/**
  * atl2_set_mac - Change the Ethernet Address of the NIC
  * @netdev: network interface device structure
  * @p: pointer to an address structure
@@ -969,12 +968,6 @@ static int atl2_set_mac(struct net_device *netdev, void *p)
 	return 0;
 }
 
-/*
- * atl2_mii_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl2_mii_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 {
 	struct atl2_adapter *adapter = netdev_priv(netdev);
@@ -1011,12 +1004,6 @@ static int atl2_mii_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 	return 0;
 }
 
-/*
- * atl2_ioctl -
- * @netdev:
- * @ifreq:
- * @cmd:
- */
 static int atl2_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 {
 	switch (cmd) {
@@ -1033,7 +1020,7 @@ static int atl2_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 	}
 }
 
-/*
+/**
  * atl2_tx_timeout - Respond to a Tx Hang
  * @netdev: network interface device structure
  */
@@ -1045,7 +1032,7 @@ static void atl2_tx_timeout(struct net_device *netdev)
 	schedule_work(&adapter->reset_task);
 }
 
-/*
+/**
  * atl2_watchdog - Timer Call-back
  * @data: pointer to netdev cast into an unsigned long
  */
@@ -1070,7 +1057,7 @@ static void atl2_watchdog(unsigned long data)
 	}
 }
 
-/*
+/**
  * atl2_phy_config - Timer Call-back
  * @data: pointer to netdev cast into an unsigned long
  */
@@ -1274,9 +1261,8 @@ static int atl2_check_link(struct atl2_adapter *adapter)
 	return 0;
 }
 
-/*
+/**
  * atl2_link_chg_task - deal with link change event Out of interrupt context
- * @netdev: network interface device structure
  */
 static void atl2_link_chg_task(struct work_struct *work)
 {
@@ -1341,7 +1327,7 @@ static const struct net_device_ops atl2_netdev_ops = {
 #endif
 };
 
-/*
+/**
  * atl2_probe - Device Initialization Routine
  * @pdev: PCI device information struct
  * @ent: entry in atl2_pci_tbl
@@ -1501,7 +1487,7 @@ err_dma:
 	return err;
 }
 
-/*
+/**
  * atl2_remove - Device Removal Routine
  * @pdev: PCI device information struct
  *
@@ -1728,7 +1714,7 @@ static struct pci_driver atl2_driver = {
 	.shutdown = atl2_shutdown,
 };
 
-/*
+/**
  * atl2_init_module - Driver Registration Routine
  *
  * atl2_init_module is the first routine called when the driver is
@@ -1743,7 +1729,7 @@ static int __init atl2_init_module(void)
 }
 module_init(atl2_init_module);
 
-/*
+/**
  * atl2_exit_module - Driver Exit Cleanup Routine
  *
  * atl2_exit_module is called just before the driver is removed
@@ -2360,7 +2346,7 @@ static s32 atl2_read_mac_addr(struct atl2_hw *hw)
 {
 	if (get_permanent_address(hw)) {
 		/* for test */
-		/* FIXME: shouldn't we use random_ether_addr() here? */
+		/* FIXME: shouldn't we use eth_random_addr() here? */
 		hw->perm_mac_addr[0] = 0x00;
 		hw->perm_mac_addr[1] = 0x13;
 		hw->perm_mac_addr[2] = 0x74;
@@ -2997,7 +2983,7 @@ static int __devinit atl2_validate_option(int *value, struct atl2_option *opt)
 	return -1;
 }
 
-/*
+/**
  * atl2_check_options - Range Checking for Command Line Parameters
  * @adapter: board private structure
  *
diff --git a/drivers/net/ethernet/atheros/atlx/atlx.c b/drivers/net/ethernet/atheros/atlx/atlx.c
index b4f3aa49a7f..77ffbc4a507 100644
--- a/drivers/net/ethernet/atheros/atlx/atlx.c
+++ b/drivers/net/ethernet/atheros/atlx/atlx.c
@@ -64,7 +64,7 @@ static int atlx_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 	}
 }
 
-/*
+/**
  * atlx_set_mac - Change the Ethernet Address of the NIC
  * @netdev: network interface device structure
  * @p: pointer to an address structure
@@ -115,7 +115,7 @@ static void atlx_check_for_link(struct atlx_adapter *adapter)
 	schedule_work(&adapter->link_chg_task);
 }
 
-/*
+/**
  * atlx_set_multi - Multicast and Promiscuous mode set
  * @netdev: network interface device structure
  *
@@ -162,7 +162,7 @@ static inline void atlx_imr_set(struct atlx_adapter *adapter,
 	ioread32(adapter->hw.hw_addr + REG_IMR);
 }
 
-/*
+/**
  * atlx_irq_enable - Enable default interrupt generation settings
  * @adapter: board private structure
  */
@@ -172,7 +172,7 @@ static void atlx_irq_enable(struct atlx_adapter *adapter)
 	adapter->int_enabled = true;
 }
 
-/*
+/**
  * atlx_irq_disable - Mask off interrupt generation on the NIC
  * @adapter: board private structure
  */
@@ -193,7 +193,7 @@ static void atlx_clear_phy_int(struct atlx_adapter *adapter)
 	spin_unlock_irqrestore(&adapter->lock, flags);
 }
 
-/*
+/**
  * atlx_tx_timeout - Respond to a Tx Hang
  * @netdev: network interface device structure
  */
diff --git a/drivers/net/ethernet/broadcom/b44.c b/drivers/net/ethernet/broadcom/b44.c
index 46b8b7d8163..9786c0e9890 100644
--- a/drivers/net/ethernet/broadcom/b44.c
+++ b/drivers/net/ethernet/broadcom/b44.c
@@ -483,9 +483,11 @@ out:
 static void b44_stats_update(struct b44 *bp)
 {
 	unsigned long reg;
-	u32 *val;
+	u64 *val;
 
 	val = &bp->hw_stats.tx_good_octets;
+	u64_stats_update_begin(&bp->hw_stats.syncp);
+
 	for (reg = B44_TX_GOOD_O; reg <= B44_TX_PAUSE; reg += 4UL) {
 		*val++ += br32(bp, reg);
 	}
@@ -496,6 +498,8 @@ static void b44_stats_update(struct b44 *bp)
 	for (reg = B44_RX_GOOD_O; reg <= B44_RX_NPAUSE; reg += 4UL) {
 		*val++ += br32(bp, reg);
 	}
+
+	u64_stats_update_end(&bp->hw_stats.syncp);
 }
 
 static void b44_link_report(struct b44 *bp)
@@ -656,7 +660,7 @@ static int b44_alloc_rx_skb(struct b44 *bp, int src_idx, u32 dest_idx_unmasked)
 			dma_unmap_single(bp->sdev->dma_dev, mapping,
 					     RX_PKT_BUF_SZ, DMA_FROM_DEVICE);
 		dev_kfree_skb_any(skb);
-		skb = __netdev_alloc_skb(bp->dev, RX_PKT_BUF_SZ, GFP_ATOMIC|GFP_DMA);
+		skb = alloc_skb(RX_PKT_BUF_SZ, GFP_ATOMIC | GFP_DMA);
 		if (skb == NULL)
 			return -ENOMEM;
 		mapping = dma_map_single(bp->sdev->dma_dev, skb->data,
@@ -967,7 +971,7 @@ static netdev_tx_t b44_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			dma_unmap_single(bp->sdev->dma_dev, mapping, len,
 					     DMA_TO_DEVICE);
 
-		bounce_skb = __netdev_alloc_skb(dev, len, GFP_ATOMIC | GFP_DMA);
+		bounce_skb = alloc_skb(len, GFP_ATOMIC | GFP_DMA);
 		if (!bounce_skb)
 			goto err_out;
 
@@ -1635,44 +1639,49 @@ static int b44_close(struct net_device *dev)
 	return 0;
 }
 
-static struct net_device_stats *b44_get_stats(struct net_device *dev)
+static struct rtnl_link_stats64 *b44_get_stats64(struct net_device *dev,
+					struct rtnl_link_stats64 *nstat)
 {
 	struct b44 *bp = netdev_priv(dev);
-	struct net_device_stats *nstat = &dev->stats;
 	struct b44_hw_stats *hwstat = &bp->hw_stats;
-
-	/* Convert HW stats into netdevice stats. */
-	nstat->rx_packets = hwstat->rx_pkts;
-	nstat->tx_packets = hwstat->tx_pkts;
-	nstat->rx_bytes   = hwstat->rx_octets;
-	nstat->tx_bytes   = hwstat->tx_octets;
-	nstat->tx_errors  = (hwstat->tx_jabber_pkts +
-			     hwstat->tx_oversize_pkts +
-			     hwstat->tx_underruns +
-			     hwstat->tx_excessive_cols +
-			     hwstat->tx_late_cols);
-	nstat->multicast  = hwstat->tx_multicast_pkts;
-	nstat->collisions = hwstat->tx_total_cols;
-
-	nstat->rx_length_errors = (hwstat->rx_oversize_pkts +
-				   hwstat->rx_undersize);
-	nstat->rx_over_errors   = hwstat->rx_missed_pkts;
-	nstat->rx_frame_errors  = hwstat->rx_align_errs;
-	nstat->rx_crc_errors    = hwstat->rx_crc_errs;
-	nstat->rx_errors        = (hwstat->rx_jabber_pkts +
-				   hwstat->rx_oversize_pkts +
-				   hwstat->rx_missed_pkts +
-				   hwstat->rx_crc_align_errs +
-				   hwstat->rx_undersize +
-				   hwstat->rx_crc_errs +
-				   hwstat->rx_align_errs +
-				   hwstat->rx_symbol_errs);
-
-	nstat->tx_aborted_errors = hwstat->tx_underruns;
+	unsigned int start;
+
+	do {
+		start = u64_stats_fetch_begin_bh(&hwstat->syncp);
+
+		/* Convert HW stats into rtnl_link_stats64 stats. */
+		nstat->rx_packets = hwstat->rx_pkts;
+		nstat->tx_packets = hwstat->tx_pkts;
+		nstat->rx_bytes   = hwstat->rx_octets;
+		nstat->tx_bytes   = hwstat->tx_octets;
+		nstat->tx_errors  = (hwstat->tx_jabber_pkts +
+				     hwstat->tx_oversize_pkts +
+				     hwstat->tx_underruns +
+				     hwstat->tx_excessive_cols +
+				     hwstat->tx_late_cols);
+		nstat->multicast  = hwstat->tx_multicast_pkts;
+		nstat->collisions = hwstat->tx_total_cols;
+
+		nstat->rx_length_errors = (hwstat->rx_oversize_pkts +
+					   hwstat->rx_undersize);
+		nstat->rx_over_errors   = hwstat->rx_missed_pkts;
+		nstat->rx_frame_errors  = hwstat->rx_align_errs;
+		nstat->rx_crc_errors    = hwstat->rx_crc_errs;
+		nstat->rx_errors        = (hwstat->rx_jabber_pkts +
+					   hwstat->rx_oversize_pkts +
+					   hwstat->rx_missed_pkts +
+					   hwstat->rx_crc_align_errs +
+					   hwstat->rx_undersize +
+					   hwstat->rx_crc_errs +
+					   hwstat->rx_align_errs +
+					   hwstat->rx_symbol_errs);
+
+		nstat->tx_aborted_errors = hwstat->tx_underruns;
 #if 0
-	/* Carrier lost counter seems to be broken for some devices */
-	nstat->tx_carrier_errors = hwstat->tx_carrier_lost;
+		/* Carrier lost counter seems to be broken for some devices */
+		nstat->tx_carrier_errors = hwstat->tx_carrier_lost;
 #endif
+	} while (u64_stats_fetch_retry_bh(&hwstat->syncp, start));
 
 	return nstat;
 }
@@ -1993,17 +2002,24 @@ static void b44_get_ethtool_stats(struct net_device *dev,
 				  struct ethtool_stats *stats, u64 *data)
 {
 	struct b44 *bp = netdev_priv(dev);
-	u32 *val = &bp->hw_stats.tx_good_octets;
+	struct b44_hw_stats *hwstat = &bp->hw_stats;
+	u64 *data_src, *data_dst;
+	unsigned int start;
 	u32 i;
 
 	spin_lock_irq(&bp->lock);
-
 	b44_stats_update(bp);
+	spin_unlock_irq(&bp->lock);
 
-	for (i = 0; i < ARRAY_SIZE(b44_gstrings); i++)
-		*data++ = *val++;
+	do {
+		data_src = &hwstat->tx_good_octets;
+		data_dst = data;
+		start = u64_stats_fetch_begin_bh(&hwstat->syncp);
 
-	spin_unlock_irq(&bp->lock);
+		for (i = 0; i < ARRAY_SIZE(b44_gstrings); i++)
+			*data_dst++ = *data_src++;
+
+	} while (u64_stats_fetch_retry_bh(&hwstat->syncp, start));
 }
 
 static void b44_get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
@@ -2113,7 +2129,7 @@ static const struct net_device_ops b44_netdev_ops = {
 	.ndo_open		= b44_open,
 	.ndo_stop		= b44_close,
 	.ndo_start_xmit		= b44_start_xmit,
-	.ndo_get_stats		= b44_get_stats,
+	.ndo_get_stats64	= b44_get_stats64,
 	.ndo_set_rx_mode	= b44_set_rx_mode,
 	.ndo_set_mac_address	= b44_set_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
diff --git a/drivers/net/ethernet/broadcom/b44.h b/drivers/net/ethernet/broadcom/b44.h
index e1905a49279..8993d72f042 100644
--- a/drivers/net/ethernet/broadcom/b44.h
+++ b/drivers/net/ethernet/broadcom/b44.h
@@ -338,9 +338,10 @@ struct ring_info {
  * the layout
  */
 struct b44_hw_stats {
-#define _B44(x)	u32 x;
+#define _B44(x)	u64 x;
 B44_STAT_REG_DECLARE
 #undef _B44
+	struct u64_stats_sync	syncp;
 };
 
 struct ssb_device;
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index ac7b7448853..79cebd8525c 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -14,6 +14,7 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 
+#include <linux/stringify.h>
 #include <linux/kernel.h>
 #include <linux/timer.h>
 #include <linux/errno.h>
@@ -57,8 +58,8 @@
 #include "bnx2_fw.h"
 
 #define DRV_MODULE_NAME		"bnx2"
-#define DRV_MODULE_VERSION	"2.2.1"
-#define DRV_MODULE_RELDATE	"Dec 18, 2011"
+#define DRV_MODULE_VERSION	"2.2.3"
+#define DRV_MODULE_RELDATE	"June 27, 2012"
 #define FW_MIPS_FILE_06		"bnx2/bnx2-mips-06-6.2.3.fw"
 #define FW_RV2P_FILE_06		"bnx2/bnx2-rv2p-06-6.0.15.fw"
 #define FW_MIPS_FILE_09		"bnx2/bnx2-mips-09-6.2.1b.fw"
@@ -872,8 +873,7 @@ bnx2_alloc_mem(struct bnx2 *bp)
 
 			bnapi = &bp->bnx2_napi[i];
 
-			sblk = (void *) (status_blk +
-					 BNX2_SBLK_MSIX_ALIGN_SIZE * i);
+			sblk = (status_blk + BNX2_SBLK_MSIX_ALIGN_SIZE * i);
 			bnapi->status_blk.msix = sblk;
 			bnapi->hw_tx_cons_ptr =
 				&sblk->status_tx_quick_consumer_index;
@@ -1972,22 +1972,26 @@ bnx2_remote_phy_event(struct bnx2 *bp)
 		switch (speed) {
 			case BNX2_LINK_STATUS_10HALF:
 				bp->duplex = DUPLEX_HALF;
+				/* fall through */
 			case BNX2_LINK_STATUS_10FULL:
 				bp->line_speed = SPEED_10;
 				break;
 			case BNX2_LINK_STATUS_100HALF:
 				bp->duplex = DUPLEX_HALF;
+				/* fall through */
 			case BNX2_LINK_STATUS_100BASE_T4:
 			case BNX2_LINK_STATUS_100FULL:
 				bp->line_speed = SPEED_100;
 				break;
 			case BNX2_LINK_STATUS_1000HALF:
 				bp->duplex = DUPLEX_HALF;
+				/* fall through */
 			case BNX2_LINK_STATUS_1000FULL:
 				bp->line_speed = SPEED_1000;
 				break;
 			case BNX2_LINK_STATUS_2500HALF:
 				bp->duplex = DUPLEX_HALF;
+				/* fall through */
 			case BNX2_LINK_STATUS_2500FULL:
 				bp->line_speed = SPEED_2500;
 				break;
@@ -2473,6 +2477,7 @@ bnx2_dump_mcp_state(struct bnx2 *bp)
 		   bnx2_shmem_rd(bp, BNX2_BC_STATE_RESET_TYPE));
 	pr_cont(" condition[%08x]\n",
 		bnx2_shmem_rd(bp, BNX2_BC_STATE_CONDITION));
+	DP_SHMEM_LINE(bp, BNX2_BC_RESET_TYPE);
 	DP_SHMEM_LINE(bp, 0x3cc);
 	DP_SHMEM_LINE(bp, 0x3dc);
 	DP_SHMEM_LINE(bp, 0x3ec);
@@ -5372,7 +5377,7 @@ bnx2_free_tx_skbs(struct bnx2 *bp)
 			int k, last;
 
 			if (skb == NULL) {
-				j++;
+				j = NEXT_TX_BD(j);
 				continue;
 			}
 
@@ -5384,8 +5389,8 @@ bnx2_free_tx_skbs(struct bnx2 *bp)
 			tx_buf->skb = NULL;
 
 			last = tx_buf->nr_frags;
-			j++;
-			for (k = 0; k < last; k++, j++) {
+			j = NEXT_TX_BD(j);
+			for (k = 0; k < last; k++, j = NEXT_TX_BD(j)) {
 				tx_buf = &txr->tx_buf_ring[TX_RING_IDX(j)];
 				dma_unmap_page(&bp->pdev->dev,
 					dma_unmap_addr(tx_buf, mapping),
@@ -6245,7 +6250,7 @@ bnx2_enable_msix(struct bnx2 *bp, int msix_vecs)
 static int
 bnx2_setup_int_mode(struct bnx2 *bp, int dis_msi)
 {
-	int cpus = num_online_cpus();
+	int cpus = netif_get_num_default_rss_queues();
 	int msix_vecs;
 
 	if (!bp->num_req_rx_rings)
@@ -6383,6 +6388,7 @@ bnx2_reset_task(struct work_struct *work)
 {
 	struct bnx2 *bp = container_of(work, struct bnx2, reset_task);
 	int rc;
+	u16 pcicmd;
 
 	rtnl_lock();
 	if (!netif_running(bp->dev)) {
@@ -6392,6 +6398,12 @@ bnx2_reset_task(struct work_struct *work)
 
 	bnx2_netif_stop(bp, true);
 
+	pci_read_config_word(bp->pdev, PCI_COMMAND, &pcicmd);
+	if (!(pcicmd & PCI_COMMAND_MEMORY)) {
+		/* in case PCI block has reset */
+		pci_restore_state(bp->pdev);
+		pci_save_state(bp->pdev);
+	}
 	rc = bnx2_init_nic(bp, 1);
 	if (rc) {
 		netdev_err(bp->dev, "failed to reset NIC, closing\n");
@@ -6406,6 +6418,75 @@ bnx2_reset_task(struct work_struct *work)
 	rtnl_unlock();
 }
 
+#define BNX2_FTQ_ENTRY(ftq) { __stringify(ftq##FTQ_CTL), BNX2_##ftq##FTQ_CTL }
+
+static void
+bnx2_dump_ftq(struct bnx2 *bp)
+{
+	int i;
+	u32 reg, bdidx, cid, valid;
+	struct net_device *dev = bp->dev;
+	static const struct ftq_reg {
+		char *name;
+		u32 off;
+	} ftq_arr[] = {
+		BNX2_FTQ_ENTRY(RV2P_P),
+		BNX2_FTQ_ENTRY(RV2P_T),
+		BNX2_FTQ_ENTRY(RV2P_M),
+		BNX2_FTQ_ENTRY(TBDR_),
+		BNX2_FTQ_ENTRY(TDMA_),
+		BNX2_FTQ_ENTRY(TXP_),
+		BNX2_FTQ_ENTRY(TXP_),
+		BNX2_FTQ_ENTRY(TPAT_),
+		BNX2_FTQ_ENTRY(RXP_C),
+		BNX2_FTQ_ENTRY(RXP_),
+		BNX2_FTQ_ENTRY(COM_COMXQ_),
+		BNX2_FTQ_ENTRY(COM_COMTQ_),
+		BNX2_FTQ_ENTRY(COM_COMQ_),
+		BNX2_FTQ_ENTRY(CP_CPQ_),
+	};
+
+	netdev_err(dev, "<--- start FTQ dump --->\n");
+	for (i = 0; i < ARRAY_SIZE(ftq_arr); i++)
+		netdev_err(dev, "%s %08x\n", ftq_arr[i].name,
+			   bnx2_reg_rd_ind(bp, ftq_arr[i].off));
+
+	netdev_err(dev, "CPU states:\n");
+	for (reg = BNX2_TXP_CPU_MODE; reg <= BNX2_CP_CPU_MODE; reg += 0x40000)
+		netdev_err(dev, "%06x mode %x state %x evt_mask %x pc %x pc %x instr %x\n",
+			   reg, bnx2_reg_rd_ind(bp, reg),
+			   bnx2_reg_rd_ind(bp, reg + 4),
+			   bnx2_reg_rd_ind(bp, reg + 8),
+			   bnx2_reg_rd_ind(bp, reg + 0x1c),
+			   bnx2_reg_rd_ind(bp, reg + 0x1c),
+			   bnx2_reg_rd_ind(bp, reg + 0x20));
+
+	netdev_err(dev, "<--- end FTQ dump --->\n");
+	netdev_err(dev, "<--- start TBDC dump --->\n");
+	netdev_err(dev, "TBDC free cnt: %ld\n",
+		   REG_RD(bp, BNX2_TBDC_STATUS) & BNX2_TBDC_STATUS_FREE_CNT);
+	netdev_err(dev, "LINE     CID  BIDX   CMD  VALIDS\n");
+	for (i = 0; i < 0x20; i++) {
+		int j = 0;
+
+		REG_WR(bp, BNX2_TBDC_BD_ADDR, i);
+		REG_WR(bp, BNX2_TBDC_CAM_OPCODE,
+		       BNX2_TBDC_CAM_OPCODE_OPCODE_CAM_READ);
+		REG_WR(bp, BNX2_TBDC_COMMAND, BNX2_TBDC_COMMAND_CMD_REG_ARB);
+		while ((REG_RD(bp, BNX2_TBDC_COMMAND) &
+			BNX2_TBDC_COMMAND_CMD_REG_ARB) && j < 100)
+			j++;
+
+		cid = REG_RD(bp, BNX2_TBDC_CID);
+		bdidx = REG_RD(bp, BNX2_TBDC_BIDX);
+		valid = REG_RD(bp, BNX2_TBDC_CAM_OPCODE);
+		netdev_err(dev, "%02x    %06x  %04lx   %02x    [%x]\n",
+			   i, cid, bdidx & BNX2_TBDC_BDIDX_BDIDX,
+			   bdidx >> 24, (valid >> 8) & 0x0ff);
+	}
+	netdev_err(dev, "<--- end TBDC dump --->\n");
+}
+
 static void
 bnx2_dump_state(struct bnx2 *bp)
 {
@@ -6435,6 +6516,7 @@ bnx2_tx_timeout(struct net_device *dev)
 {
 	struct bnx2 *bp = netdev_priv(dev);
 
+	bnx2_dump_ftq(bp);
 	bnx2_dump_state(bp);
 	bnx2_dump_mcp_state(bp);
 
@@ -6628,6 +6710,7 @@ bnx2_close(struct net_device *dev)
 
 	bnx2_disable_int_sync(bp);
 	bnx2_napi_disable(bp);
+	netif_tx_disable(dev);
 	del_timer_sync(&bp->timer);
 	bnx2_shutdown_chip(bp);
 	bnx2_free_irq(bp);
@@ -7832,7 +7915,7 @@ bnx2_get_5709_media(struct bnx2 *bp)
 	else
 		strap = (val & BNX2_MISC_DUAL_MEDIA_CTRL_PHY_CTRL_STRAP) >> 8;
 
-	if (PCI_FUNC(bp->pdev->devfn) == 0) {
+	if (bp->func == 0) {
 		switch (strap) {
 		case 0x4:
 		case 0x5:
@@ -8131,9 +8214,12 @@ bnx2_init_board(struct pci_dev *pdev, struct net_device *dev)
 
 	reg = bnx2_reg_rd_ind(bp, BNX2_SHM_HDR_SIGNATURE);
 
+	if (bnx2_reg_rd_ind(bp, BNX2_MCP_TOE_ID) & BNX2_MCP_TOE_ID_FUNCTION_ID)
+		bp->func = 1;
+
 	if ((reg & BNX2_SHM_HDR_SIGNATURE_SIG_MASK) ==
 	    BNX2_SHM_HDR_SIGNATURE_SIG) {
-		u32 off = PCI_FUNC(pdev->devfn) << 2;
+		u32 off = bp->func << 2;
 
 		bp->shmem_base = bnx2_reg_rd_ind(bp, BNX2_SHM_HDR_ADDR_0 + off);
 	} else
diff --git a/drivers/net/ethernet/broadcom/bnx2.h b/drivers/net/ethernet/broadcom/bnx2.h
index dc06bda73be..af6451dec29 100644
--- a/drivers/net/ethernet/broadcom/bnx2.h
+++ b/drivers/net/ethernet/broadcom/bnx2.h
@@ -4642,6 +4642,47 @@ struct l2_fhdr {
 #define BNX2_TBDR_FTQ_CTL_CUR_DEPTH			 (0x3ffL<<22)
 
 
+/*
+ *  tbdc definition
+ *  offset: 0x5400
+ */
+#define BNX2_TBDC_COMMAND                               0x5400
+#define BNX2_TBDC_COMMAND_CMD_ENABLED                    (1UL<<0)
+#define BNX2_TBDC_COMMAND_CMD_FLUSH                      (1UL<<1)
+#define BNX2_TBDC_COMMAND_CMD_SOFT_RST                   (1UL<<2)
+#define BNX2_TBDC_COMMAND_CMD_REG_ARB                    (1UL<<3)
+#define BNX2_TBDC_COMMAND_WRCHK_RANGE_ERROR              (1UL<<4)
+#define BNX2_TBDC_COMMAND_WRCHK_ALL_ONES_ERROR           (1UL<<5)
+#define BNX2_TBDC_COMMAND_WRCHK_ALL_ZEROS_ERROR          (1UL<<6)
+#define BNX2_TBDC_COMMAND_WRCHK_ANY_ONES_ERROR           (1UL<<7)
+#define BNX2_TBDC_COMMAND_WRCHK_ANY_ZEROS_ERROR          (1UL<<8)
+
+#define BNX2_TBDC_STATUS				0x5404
+#define BNX2_TBDC_STATUS_FREE_CNT                        (0x3fUL<<0)
+
+#define BNX2_TBDC_BD_ADDR                               0x5424
+
+#define BNX2_TBDC_BIDX                                  0x542c
+#define BNX2_TBDC_BDIDX_BDIDX                            (0xffffUL<<0)
+#define BNX2_TBDC_BDIDX_CMD                              (0xffUL<<24)
+
+#define BNX2_TBDC_CID                                   0x5430
+
+#define BNX2_TBDC_CAM_OPCODE                            0x5434
+#define BNX2_TBDC_CAM_OPCODE_OPCODE                      (0x7UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_SEARCH               (0UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_CACHE_WRITE          (1UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_INVALIDATE           (2UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_CAM_WRITE            (4UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_CAM_READ             (5UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_RAM_WRITE            (6UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_OPCODE_RAM_READ             (7UL<<0)
+#define BNX2_TBDC_CAM_OPCODE_SMASK_BDIDX                 (1UL<<4)
+#define BNX2_TBDC_CAM_OPCODE_SMASK_CID                   (1UL<<5)
+#define BNX2_TBDC_CAM_OPCODE_SMASK_CMD                   (1UL<<6)
+#define BNX2_TBDC_CAM_OPCODE_WMT_FAILED                  (1UL<<7)
+#define BNX2_TBDC_CAM_OPCODE_CAM_VALIDS                  (0xffUL<<8)
+
 
 /*
  *  tdma_reg definition
@@ -6930,6 +6971,8 @@ struct bnx2 {
 	struct bnx2_irq		irq_tbl[BNX2_MAX_MSIX_VEC];
 	int			irq_nvecs;
 
+	u8			func;
+
 	u8			num_tx_rings;
 	u8			num_rx_rings;
 
@@ -7314,6 +7357,8 @@ struct bnx2_rv2p_fw_file {
 #define BNX2_BC_STATE_RESET_TYPE_VALUE(msg) (BNX2_BC_STATE_RESET_TYPE_SIG | \
 					     (msg))
 
+#define BNX2_BC_RESET_TYPE			0x000001c0
+
 #define BNX2_BC_STATE				0x000001c4
 #define BNX2_BC_STATE_ERR_MASK			 0x0000ff00
 #define BNX2_BC_STATE_SIGN			 0x42530000
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index 7de82418497..77bcd4cb4ff 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -23,8 +23,8 @@
  * (you will need to reboot afterwards) */
 /* #define BNX2X_STOP_ON_ERROR */
 
-#define DRV_MODULE_VERSION      "1.72.50-0"
-#define DRV_MODULE_RELDATE      "2012/04/23"
+#define DRV_MODULE_VERSION      "1.72.51-0"
+#define DRV_MODULE_RELDATE      "2012/06/18"
 #define BNX2X_BC_VER            0x040200
 
 #if defined(CONFIG_DCB)
@@ -51,6 +51,7 @@
 
 #include "bnx2x_reg.h"
 #include "bnx2x_fw_defs.h"
+#include "bnx2x_mfw_req.h"
 #include "bnx2x_hsi.h"
 #include "bnx2x_link.h"
 #include "bnx2x_sp.h"
@@ -248,13 +249,12 @@ enum {
 	BNX2X_MAX_CNIC_ETH_CL_ID_IDX,
 };
 
-#define BNX2X_CNIC_START_ETH_CID	48
-enum {
+#define BNX2X_CNIC_START_ETH_CID(bp)	(BNX2X_NUM_NON_CNIC_QUEUES(bp) *\
+					 (bp)->max_cos)
 	/* iSCSI L2 */
-	BNX2X_ISCSI_ETH_CID = BNX2X_CNIC_START_ETH_CID,
+#define	BNX2X_ISCSI_ETH_CID(bp)		(BNX2X_CNIC_START_ETH_CID(bp))
 	/* FCoE L2 */
-	BNX2X_FCOE_ETH_CID,
-};
+#define	BNX2X_FCOE_ETH_CID(bp)		(BNX2X_CNIC_START_ETH_CID(bp) + 1)
 
 /** Additional rings budgeting */
 #ifdef BCM_CNIC
@@ -276,29 +276,30 @@ enum {
 #define FIRST_TX_ONLY_COS_INDEX		1
 #define FIRST_TX_COS_INDEX		0
 
-/* defines for decodeing the fastpath index and the cos index out of the
- * transmission queue index
- */
-#define MAX_TXQS_PER_COS	FP_SB_MAX_E1x
-
-#define TXQ_TO_FP(txq_index)	((txq_index) % MAX_TXQS_PER_COS)
-#define TXQ_TO_COS(txq_index)	((txq_index) / MAX_TXQS_PER_COS)
-
 /* rules for calculating the cids of tx-only connections */
-#define CID_TO_FP(cid)		((cid) % MAX_TXQS_PER_COS)
-#define CID_COS_TO_TX_ONLY_CID(cid, cos)	(cid + cos * MAX_TXQS_PER_COS)
+#define CID_TO_FP(cid, bp)		((cid) % BNX2X_NUM_NON_CNIC_QUEUES(bp))
+#define CID_COS_TO_TX_ONLY_CID(cid, cos, bp) \
+				(cid + cos * BNX2X_NUM_NON_CNIC_QUEUES(bp))
 
 /* fp index inside class of service range */
-#define FP_COS_TO_TXQ(fp, cos)    ((fp)->index + cos * MAX_TXQS_PER_COS)
-
-/*
- * 0..15 eth cos0
- * 16..31 eth cos1 if applicable
- * 32..47 eth cos2 If applicable
- * fcoe queue follows eth queues (16, 32, 48 depending on cos)
+#define FP_COS_TO_TXQ(fp, cos, bp) \
+			((fp)->index + cos * BNX2X_NUM_NON_CNIC_QUEUES(bp))
+
+/* Indexes for transmission queues array:
+ * txdata for RSS i CoS j is at location i + (j * num of RSS)
+ * txdata for FCoE (if exist) is at location max cos * num of RSS
+ * txdata for FWD (if exist) is one location after FCoE
+ * txdata for OOO (if exist) is one location after FWD
  */
-#define MAX_ETH_TXQ_IDX(bp)	(MAX_TXQS_PER_COS * (bp)->max_cos)
-#define FCOE_TXQ_IDX(bp)	(MAX_ETH_TXQ_IDX(bp))
+enum {
+	FCOE_TXQ_IDX_OFFSET,
+	FWD_TXQ_IDX_OFFSET,
+	OOO_TXQ_IDX_OFFSET,
+};
+#define MAX_ETH_TXQ_IDX(bp)	(BNX2X_NUM_NON_CNIC_QUEUES(bp) * (bp)->max_cos)
+#ifdef BCM_CNIC
+#define FCOE_TXQ_IDX(bp)	(MAX_ETH_TXQ_IDX(bp) + FCOE_TXQ_IDX_OFFSET)
+#endif
 
 /* fast path */
 /*
@@ -453,6 +454,7 @@ struct bnx2x_agg_info {
 	u16			vlan_tag;
 	u16			len_on_bd;
 	u32			rxhash;
+	bool			l4_rxhash;
 	u16			gro_size;
 	u16			full_page;
 };
@@ -481,6 +483,8 @@ struct bnx2x_fp_txdata {
 	__le16			*tx_cons_sb;
 
 	int			txq_index;
+	struct bnx2x_fastpath	*parent_fp;
+	int			tx_ring_size;
 };
 
 enum bnx2x_tpa_mode_t {
@@ -507,7 +511,7 @@ struct bnx2x_fastpath {
 	enum bnx2x_tpa_mode_t	mode;
 
 	u8			max_cos; /* actual number of active tx coses */
-	struct bnx2x_fp_txdata	txdata[BNX2X_MULTI_TX_COS];
+	struct bnx2x_fp_txdata	*txdata_ptr[BNX2X_MULTI_TX_COS];
 
 	struct sw_rx_bd		*rx_buf_ring;	/* BDs mappings ring */
 	struct sw_rx_page	*rx_page_ring;	/* SGE pages mappings ring */
@@ -547,51 +551,45 @@ struct bnx2x_fastpath {
 				rx_calls;
 
 	/* TPA related */
-	struct bnx2x_agg_info	tpa_info[ETH_MAX_AGGREGATION_QUEUES_E1H_E2];
+	struct bnx2x_agg_info	*tpa_info;
 	u8			disable_tpa;
 #ifdef BNX2X_STOP_ON_ERROR
 	u64			tpa_queue_used;
 #endif
-
-	struct tstorm_per_queue_stats old_tclient;
-	struct ustorm_per_queue_stats old_uclient;
-	struct xstorm_per_queue_stats old_xclient;
-	struct bnx2x_eth_q_stats eth_q_stats;
-	struct bnx2x_eth_q_stats_old eth_q_stats_old;
-
 	/* The size is calculated using the following:
 	     sizeof name field from netdev structure +
 	     4 ('-Xx-' string) +
 	     4 (for the digits and to make it DWORD aligned) */
 #define FP_NAME_SIZE		(sizeof(((struct net_device *)0)->name) + 8)
 	char			name[FP_NAME_SIZE];
-
-	/* MACs object */
-	struct bnx2x_vlan_mac_obj mac_obj;
-
-	/* Queue State object */
-	struct bnx2x_queue_sp_obj q_obj;
-
 };
 
-#define bnx2x_fp(bp, nr, var)		(bp->fp[nr].var)
+#define bnx2x_fp(bp, nr, var)	((bp)->fp[(nr)].var)
+#define bnx2x_sp_obj(bp, fp)	((bp)->sp_objs[(fp)->index])
+#define bnx2x_fp_stats(bp, fp)	(&((bp)->fp_stats[(fp)->index]))
+#define bnx2x_fp_qstats(bp, fp)	(&((bp)->fp_stats[(fp)->index].eth_q_stats))
 
 /* Use 2500 as a mini-jumbo MTU for FCoE */
 #define BNX2X_FCOE_MINI_JUMBO_MTU	2500
 
-/* FCoE L2 `fastpath' entry is right after the eth entries */
-#define FCOE_IDX			BNX2X_NUM_ETH_QUEUES(bp)
-#define bnx2x_fcoe_fp(bp)		(&bp->fp[FCOE_IDX])
-#define bnx2x_fcoe(bp, var)		(bnx2x_fcoe_fp(bp)->var)
-#define bnx2x_fcoe_tx(bp, var)		(bnx2x_fcoe_fp(bp)-> \
-						txdata[FIRST_TX_COS_INDEX].var)
+#define	FCOE_IDX_OFFSET		0
+
+#define FCOE_IDX(bp)		(BNX2X_NUM_NON_CNIC_QUEUES(bp) + \
+				 FCOE_IDX_OFFSET)
+#define bnx2x_fcoe_fp(bp)	(&bp->fp[FCOE_IDX(bp)])
+#define bnx2x_fcoe(bp, var)	(bnx2x_fcoe_fp(bp)->var)
+#define bnx2x_fcoe_inner_sp_obj(bp)	(&bp->sp_objs[FCOE_IDX(bp)])
+#define bnx2x_fcoe_sp_obj(bp, var)	(bnx2x_fcoe_inner_sp_obj(bp)->var)
+#define bnx2x_fcoe_tx(bp, var)	(bnx2x_fcoe_fp(bp)-> \
+						txdata_ptr[FIRST_TX_COS_INDEX] \
+						->var)
 
 
 #define IS_ETH_FP(fp)			(fp->index < \
 					 BNX2X_NUM_ETH_QUEUES(fp->bp))
 #ifdef BCM_CNIC
-#define IS_FCOE_FP(fp)			(fp->index == FCOE_IDX)
-#define IS_FCOE_IDX(idx)		((idx) == FCOE_IDX)
+#define IS_FCOE_FP(fp)			(fp->index == FCOE_IDX(fp->bp))
+#define IS_FCOE_IDX(idx)		((idx) == FCOE_IDX(bp))
 #else
 #define IS_FCOE_FP(fp)		false
 #define IS_FCOE_IDX(idx)	false
@@ -616,6 +614,22 @@ struct bnx2x_fastpath {
 #define TX_BD(x)		((x) & MAX_TX_BD)
 #define TX_BD_POFF(x)		((x) & MAX_TX_DESC_CNT)
 
+/* number of NEXT_PAGE descriptors may be required during placement */
+#define NEXT_CNT_PER_TX_PKT(bds)	\
+				(((bds) + MAX_TX_DESC_CNT - 1) / \
+				 MAX_TX_DESC_CNT * NEXT_PAGE_TX_DESC_CNT)
+/* max BDs per tx packet w/o next_pages:
+ * START_BD		- describes packed
+ * START_BD(splitted)	- includes unpaged data segment for GSO
+ * PARSING_BD		- for TSO and CSUM data
+ * Frag BDs		- decribes pages for frags
+ */
+#define BDS_PER_TX_PKT		3
+#define MAX_BDS_PER_TX_PKT	(MAX_SKB_FRAGS + BDS_PER_TX_PKT)
+/* max BDs per tx packet including next pages */
+#define MAX_DESC_PER_TX_PKT	(MAX_BDS_PER_TX_PKT + \
+				 NEXT_CNT_PER_TX_PKT(MAX_BDS_PER_TX_PKT))
+
 /* The RX BD ring is special, each bd is 8 bytes but the last one is 16 */
 #define NUM_RX_RINGS		8
 #define RX_DESC_CNT		(BCM_PAGE_SIZE / sizeof(struct eth_rx_bd))
@@ -805,8 +819,11 @@ struct bnx2x_common {
 #define CHIP_NUM_57810_MF		0x16ae
 #define CHIP_NUM_57811			0x163d
 #define CHIP_NUM_57811_MF		0x163e
-#define CHIP_NUM_57840			0x168d
-#define CHIP_NUM_57840_MF		0x16ab
+#define CHIP_NUM_57840_OBSOLETE	0x168d
+#define CHIP_NUM_57840_MF_OBSOLETE	0x16ab
+#define CHIP_NUM_57840_4_10		0x16a1
+#define CHIP_NUM_57840_2_20		0x16a2
+#define CHIP_NUM_57840_MF		0x16a4
 #define CHIP_IS_E1(bp)			(CHIP_NUM(bp) == CHIP_NUM_57710)
 #define CHIP_IS_57711(bp)		(CHIP_NUM(bp) == CHIP_NUM_57711)
 #define CHIP_IS_57711E(bp)		(CHIP_NUM(bp) == CHIP_NUM_57711E)
@@ -818,8 +835,12 @@ struct bnx2x_common {
 #define CHIP_IS_57810_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57810_MF)
 #define CHIP_IS_57811(bp)		(CHIP_NUM(bp) == CHIP_NUM_57811)
 #define CHIP_IS_57811_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57811_MF)
-#define CHIP_IS_57840(bp)		(CHIP_NUM(bp) == CHIP_NUM_57840)
-#define CHIP_IS_57840_MF(bp)		(CHIP_NUM(bp) == CHIP_NUM_57840_MF)
+#define CHIP_IS_57840(bp)		\
+		((CHIP_NUM(bp) == CHIP_NUM_57840_4_10) || \
+		 (CHIP_NUM(bp) == CHIP_NUM_57840_2_20) || \
+		 (CHIP_NUM(bp) == CHIP_NUM_57840_OBSOLETE))
+#define CHIP_IS_57840_MF(bp)	((CHIP_NUM(bp) == CHIP_NUM_57840_MF) || \
+				 (CHIP_NUM(bp) == CHIP_NUM_57840_MF_OBSOLETE))
 #define CHIP_IS_E1H(bp)			(CHIP_IS_57711(bp) || \
 					 CHIP_IS_57711E(bp))
 #define CHIP_IS_E2(bp)			(CHIP_IS_57712(bp) || \
@@ -978,8 +999,8 @@ union cdu_context {
 };
 
 /* CDU host DB constants */
-#define CDU_ILT_PAGE_SZ_HW	3
-#define CDU_ILT_PAGE_SZ		(8192 << CDU_ILT_PAGE_SZ_HW) /* 64K */
+#define CDU_ILT_PAGE_SZ_HW	2
+#define CDU_ILT_PAGE_SZ		(8192 << CDU_ILT_PAGE_SZ_HW) /* 32K */
 #define ILT_PAGE_CIDS		(CDU_ILT_PAGE_SZ / sizeof(union cdu_context))
 
 #ifdef BCM_CNIC
@@ -1182,11 +1203,31 @@ struct bnx2x_prev_path_list {
 	struct list_head list;
 };
 
+struct bnx2x_sp_objs {
+	/* MACs object */
+	struct bnx2x_vlan_mac_obj mac_obj;
+
+	/* Queue State object */
+	struct bnx2x_queue_sp_obj q_obj;
+};
+
+struct bnx2x_fp_stats {
+	struct tstorm_per_queue_stats old_tclient;
+	struct ustorm_per_queue_stats old_uclient;
+	struct xstorm_per_queue_stats old_xclient;
+	struct bnx2x_eth_q_stats eth_q_stats;
+	struct bnx2x_eth_q_stats_old eth_q_stats_old;
+};
+
 struct bnx2x {
 	/* Fields used in the tx and intr/napi performance paths
 	 * are grouped together in the beginning of the structure
 	 */
 	struct bnx2x_fastpath	*fp;
+	struct bnx2x_sp_objs	*sp_objs;
+	struct bnx2x_fp_stats	*fp_stats;
+	struct bnx2x_fp_txdata	*bnx2x_txq;
+	int			bnx2x_txq_size;
 	void __iomem		*regview;
 	void __iomem		*doorbells;
 	u16			db_size;
@@ -1301,7 +1342,9 @@ struct bnx2x {
 #define NO_ISCSI_FLAG			(1 << 14)
 #define NO_FCOE_FLAG			(1 << 15)
 #define BC_SUPPORTS_PFC_STATS		(1 << 17)
+#define BC_SUPPORTS_FCOE_FEATURES	(1 << 19)
 #define USING_SINGLE_MSIX_FLAG		(1 << 20)
+#define BC_SUPPORTS_DCBX_MSG_NON_PMF	(1 << 21)
 
 #define NO_ISCSI(bp)		((bp)->flags & NO_ISCSI_FLAG)
 #define NO_ISCSI_OOO(bp)	((bp)->flags & NO_ISCSI_OOO_FLAG)
@@ -1377,6 +1420,7 @@ struct bnx2x {
 #define BNX2X_MAX_COS			3
 #define BNX2X_MAX_TX_COS		2
 	int			num_queues;
+	int			num_napi_queues;
 	int			disable_tpa;
 
 	u32			rx_mode;
@@ -1389,6 +1433,7 @@ struct bnx2x {
 	u8			igu_dsb_id;
 	u8			igu_base_sb;
 	u8			igu_sb_cnt;
+
 	dma_addr_t		def_status_blk_mapping;
 
 	struct bnx2x_slowpath	*slowpath;
@@ -1420,7 +1465,11 @@ struct bnx2x {
 	dma_addr_t			fw_stats_data_mapping;
 	int				fw_stats_data_sz;
 
-	struct hw_context	context;
+	/* For max 196 cids (64*3 + non-eth), 32KB ILT page size and 1KB
+	 * context size we need 8 ILT entries.
+	 */
+#define ILT_MAX_L2_LINES	8
+	struct hw_context	context[ILT_MAX_L2_LINES];
 
 	struct bnx2x_ilt	*ilt;
 #define BP_ILT(bp)		((bp)->ilt)
@@ -1433,13 +1482,14 @@ struct bnx2x {
 
 /*
  * Maximum CID count that might be required by the bnx2x:
- * Max Tss * Max_Tx_Multi_Cos + CNIC L2 Clients (FCoE and iSCSI related)
+ * Max RSS * Max_Tx_Multi_Cos + FCoE + iSCSI
  */
-#define BNX2X_L2_CID_COUNT(bp)	(MAX_TXQS_PER_COS * BNX2X_MULTI_TX_COS +\
-					NON_ETH_CONTEXT_USE + CNIC_PRESENT)
+#define BNX2X_L2_CID_COUNT(bp)	(BNX2X_NUM_ETH_QUEUES(bp) * BNX2X_MULTI_TX_COS \
+				+ NON_ETH_CONTEXT_USE + CNIC_PRESENT)
+#define BNX2X_L2_MAX_CID(bp)	(BNX2X_MAX_RSS_COUNT(bp) * BNX2X_MULTI_TX_COS \
+				+ NON_ETH_CONTEXT_USE + CNIC_PRESENT)
 #define L2_ILT_LINES(bp)	(DIV_ROUND_UP(BNX2X_L2_CID_COUNT(bp),\
 					ILT_PAGE_CIDS))
-#define BNX2X_DB_SIZE(bp)	(BNX2X_L2_CID_COUNT(bp) * (1 << BNX2X_DB_SHIFT))
 
 	int			qm_cid_count;
 
@@ -1598,6 +1648,8 @@ struct bnx2x {
 extern int num_queues;
 #define BNX2X_NUM_QUEUES(bp)	(bp->num_queues)
 #define BNX2X_NUM_ETH_QUEUES(bp) (BNX2X_NUM_QUEUES(bp) - NON_ETH_CONTEXT_USE)
+#define BNX2X_NUM_NON_CNIC_QUEUES(bp)	(BNX2X_NUM_QUEUES(bp) - \
+					 NON_ETH_CONTEXT_USE)
 #define BNX2X_NUM_RX_QUEUES(bp)	BNX2X_NUM_QUEUES(bp)
 
 #define is_multi(bp)		(BNX2X_NUM_QUEUES(bp) > 1)
@@ -1656,6 +1708,9 @@ struct bnx2x_func_init_params {
 			continue;		\
 		else
 
+#define for_each_napi_rx_queue(bp, var) \
+	for ((var) = 0; (var) < bp->num_napi_queues; (var)++)
+
 /* Skip OOO FP */
 #define for_each_tx_queue(bp, var) \
 	for ((var) = 0; (var) < BNX2X_NUM_QUEUES(bp); (var)++) \
@@ -1709,15 +1764,6 @@ int bnx2x_set_mac_one(struct bnx2x *bp, u8 *mac,
 		      struct bnx2x_vlan_mac_obj *obj, bool set,
 		      int mac_type, unsigned long *ramrod_flags);
 /**
- * Deletes all MACs configured for the specific MAC object.
- *
- * @param bp Function driver instance
- * @param mac_obj MAC object to cleanup
- *
- * @return zero if all MACs were cleaned
- */
-
-/**
  * bnx2x_del_all_macs - delete all MACs configured for the specific MAC object
  *
  * @bp:			driver handle
@@ -1817,6 +1863,7 @@ static inline u32 reg_poll(struct bnx2x *bp, u32 reg, u32 expected, int ms,
 #define LOAD_NORMAL			0
 #define LOAD_OPEN			1
 #define LOAD_DIAG			2
+#define LOAD_LOOPBACK_EXT		3
 #define UNLOAD_NORMAL			0
 #define UNLOAD_CLOSE			1
 #define UNLOAD_RECOVERY			2
@@ -1899,13 +1946,17 @@ static inline u32 reg_poll(struct bnx2x *bp, u32 reg, u32 expected, int ms,
 #define PCICFG_LINK_SPEED		0xf0000
 #define PCICFG_LINK_SPEED_SHIFT		16
 
-
-#define BNX2X_NUM_TESTS			7
+#define BNX2X_NUM_TESTS_SF		7
+#define BNX2X_NUM_TESTS_MF		3
+#define BNX2X_NUM_TESTS(bp)		(IS_MF(bp) ? BNX2X_NUM_TESTS_MF : \
+						     BNX2X_NUM_TESTS_SF)
 
 #define BNX2X_PHY_LOOPBACK		0
 #define BNX2X_MAC_LOOPBACK		1
+#define BNX2X_EXT_LOOPBACK		2
 #define BNX2X_PHY_LOOPBACK_FAILED	1
 #define BNX2X_MAC_LOOPBACK_FAILED	2
+#define BNX2X_EXT_LOOPBACK_FAILED	3
 #define BNX2X_LOOPBACK_FAILED		(BNX2X_MAC_LOOPBACK_FAILED | \
 					 BNX2X_PHY_LOOPBACK_FAILED)
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 8098eea9704..e879e19eb0d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -40,12 +40,19 @@
  * Makes sure the contents of the bp->fp[to].napi is kept
  * intact. This is done by first copying the napi struct from
  * the target to the source, and then mem copying the entire
- * source onto the target
+ * source onto the target. Update txdata pointers and related
+ * content.
  */
 static inline void bnx2x_move_fp(struct bnx2x *bp, int from, int to)
 {
 	struct bnx2x_fastpath *from_fp = &bp->fp[from];
 	struct bnx2x_fastpath *to_fp = &bp->fp[to];
+	struct bnx2x_sp_objs *from_sp_objs = &bp->sp_objs[from];
+	struct bnx2x_sp_objs *to_sp_objs = &bp->sp_objs[to];
+	struct bnx2x_fp_stats *from_fp_stats = &bp->fp_stats[from];
+	struct bnx2x_fp_stats *to_fp_stats = &bp->fp_stats[to];
+	int old_max_eth_txqs, new_max_eth_txqs;
+	int old_txdata_index = 0, new_txdata_index = 0;
 
 	/* Copy the NAPI object as it has been already initialized */
 	from_fp->napi = to_fp->napi;
@@ -53,6 +60,30 @@ static inline void bnx2x_move_fp(struct bnx2x *bp, int from, int to)
 	/* Move bnx2x_fastpath contents */
 	memcpy(to_fp, from_fp, sizeof(*to_fp));
 	to_fp->index = to;
+
+	/* move sp_objs contents as well, as their indices match fp ones */
+	memcpy(to_sp_objs, from_sp_objs, sizeof(*to_sp_objs));
+
+	/* move fp_stats contents as well, as their indices match fp ones */
+	memcpy(to_fp_stats, from_fp_stats, sizeof(*to_fp_stats));
+
+	/* Update txdata pointers in fp and move txdata content accordingly:
+	 * Each fp consumes 'max_cos' txdata structures, so the index should be
+	 * decremented by max_cos x delta.
+	 */
+
+	old_max_eth_txqs = BNX2X_NUM_ETH_QUEUES(bp) * (bp)->max_cos;
+	new_max_eth_txqs = (BNX2X_NUM_ETH_QUEUES(bp) - from + to) *
+				(bp)->max_cos;
+	if (from == FCOE_IDX(bp)) {
+		old_txdata_index = old_max_eth_txqs + FCOE_TXQ_IDX_OFFSET;
+		new_txdata_index = new_max_eth_txqs + FCOE_TXQ_IDX_OFFSET;
+	}
+
+	memcpy(&bp->bnx2x_txq[old_txdata_index],
+	       &bp->bnx2x_txq[new_txdata_index],
+	       sizeof(struct bnx2x_fp_txdata));
+	to_fp->txdata_ptr[0] = &bp->bnx2x_txq[new_txdata_index];
 }
 
 int load_count[2][3] = { {0} }; /* per-path: 0-common, 1-port0, 2-port1 */
@@ -190,7 +221,7 @@ int bnx2x_tx_int(struct bnx2x *bp, struct bnx2x_fp_txdata *txdata)
 
 		if ((netif_tx_queue_stopped(txq)) &&
 		    (bp->state == BNX2X_STATE_OPEN) &&
-		    (bnx2x_tx_avail(bp, txdata) >= MAX_SKB_FRAGS + 4))
+		    (bnx2x_tx_avail(bp, txdata) >= MAX_DESC_PER_TX_PKT))
 			netif_tx_wake_queue(txq);
 
 		__netif_tx_unlock(txq);
@@ -264,12 +295,20 @@ static inline void bnx2x_update_sge_prod(struct bnx2x_fastpath *fp,
  * CQE (calculated by HW).
  */
 static u32 bnx2x_get_rxhash(const struct bnx2x *bp,
-			    const struct eth_fast_path_rx_cqe *cqe)
+			    const struct eth_fast_path_rx_cqe *cqe,
+			    bool *l4_rxhash)
 {
 	/* Set Toeplitz hash from CQE */
 	if ((bp->dev->features & NETIF_F_RXHASH) &&
-	    (cqe->status_flags & ETH_FAST_PATH_RX_CQE_RSS_HASH_FLG))
+	    (cqe->status_flags & ETH_FAST_PATH_RX_CQE_RSS_HASH_FLG)) {
+		enum eth_rss_hash_type htype;
+
+		htype = cqe->status_flags & ETH_FAST_PATH_RX_CQE_RSS_HASH_TYPE;
+		*l4_rxhash = (htype == TCP_IPV4_HASH_TYPE) ||
+			     (htype == TCP_IPV6_HASH_TYPE);
 		return le32_to_cpu(cqe->rss_hash_result);
+	}
+	*l4_rxhash = false;
 	return 0;
 }
 
@@ -323,7 +362,7 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
 	tpa_info->tpa_state = BNX2X_TPA_START;
 	tpa_info->len_on_bd = le16_to_cpu(cqe->len_on_bd);
 	tpa_info->placement_offset = cqe->placement_offset;
-	tpa_info->rxhash = bnx2x_get_rxhash(bp, cqe);
+	tpa_info->rxhash = bnx2x_get_rxhash(bp, cqe, &tpa_info->l4_rxhash);
 	if (fp->mode == TPA_MODE_GRO) {
 		u16 gro_size = le16_to_cpu(cqe->pkt_len_or_gro_seg_len);
 		tpa_info->full_page =
@@ -479,7 +518,7 @@ static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 		   where we are and drop the whole packet */
 		err = bnx2x_alloc_rx_sge(bp, fp, sge_idx);
 		if (unlikely(err)) {
-			fp->eth_q_stats.rx_skb_alloc_failed++;
+			bnx2x_fp_qstats(bp, fp)->rx_skb_alloc_failed++;
 			return err;
 		}
 
@@ -558,6 +597,7 @@ static void bnx2x_tpa_stop(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 		skb_reserve(skb, pad + NET_SKB_PAD);
 		skb_put(skb, len);
 		skb->rxhash = tpa_info->rxhash;
+		skb->l4_rxhash = tpa_info->l4_rxhash;
 
 		skb->protocol = eth_type_trans(skb, bp->dev);
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -584,7 +624,7 @@ drop:
 	/* drop the packet and keep the buffer in the bin */
 	DP(NETIF_MSG_RX_STATUS,
 	   "Failed to allocate or map a new skb - dropping packet!\n");
-	fp->eth_q_stats.rx_skb_alloc_failed++;
+	bnx2x_fp_stats(bp, fp)->eth_q_stats.rx_skb_alloc_failed++;
 }
 
 static int bnx2x_alloc_rx_data(struct bnx2x *bp,
@@ -617,8 +657,10 @@ static int bnx2x_alloc_rx_data(struct bnx2x *bp,
 	return 0;
 }
 
-static void bnx2x_csum_validate(struct sk_buff *skb, union eth_rx_cqe *cqe,
-				struct bnx2x_fastpath *fp)
+static
+void bnx2x_csum_validate(struct sk_buff *skb, union eth_rx_cqe *cqe,
+				 struct bnx2x_fastpath *fp,
+				 struct bnx2x_eth_q_stats *qstats)
 {
 	/* Do nothing if no IP/L4 csum validation was done */
 
@@ -632,7 +674,7 @@ static void bnx2x_csum_validate(struct sk_buff *skb, union eth_rx_cqe *cqe,
 	if (cqe->fast_path_cqe.type_error_flags &
 	    (ETH_FAST_PATH_RX_CQE_IP_BAD_XSUM_FLG |
 	     ETH_FAST_PATH_RX_CQE_L4_BAD_XSUM_FLG))
-		fp->eth_q_stats.hw_csum_err++;
+		qstats->hw_csum_err++;
 	else
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
 }
@@ -679,6 +721,7 @@ int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 		enum eth_rx_cqe_type cqe_fp_type;
 		u16 len, pad, queue;
 		u8 *data;
+		bool l4_rxhash;
 
 #ifdef BNX2X_STOP_ON_ERROR
 		if (unlikely(bp->panic))
@@ -776,7 +819,7 @@ int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 			DP(NETIF_MSG_RX_ERR | NETIF_MSG_RX_STATUS,
 			   "ERROR  flags %x  rx packet %u\n",
 			   cqe_fp_flags, sw_comp_cons);
-			fp->eth_q_stats.rx_err_discard_pkt++;
+			bnx2x_fp_qstats(bp, fp)->rx_err_discard_pkt++;
 			goto reuse_rx;
 		}
 
@@ -789,7 +832,7 @@ int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 			if (skb == NULL) {
 				DP(NETIF_MSG_RX_ERR | NETIF_MSG_RX_STATUS,
 				   "ERROR  packet dropped because of alloc failure\n");
-				fp->eth_q_stats.rx_skb_alloc_failed++;
+				bnx2x_fp_qstats(bp, fp)->rx_skb_alloc_failed++;
 				goto reuse_rx;
 			}
 			memcpy(skb->data, data + pad, len);
@@ -803,14 +846,15 @@ int bnx2x_rx_int(struct bnx2x_fastpath *fp, int budget)
 				skb = build_skb(data, 0);
 				if (unlikely(!skb)) {
 					kfree(data);
-					fp->eth_q_stats.rx_skb_alloc_failed++;
+					bnx2x_fp_qstats(bp, fp)->
+							rx_skb_alloc_failed++;
 					goto next_rx;
 				}
 				skb_reserve(skb, pad);
 			} else {
 				DP(NETIF_MSG_RX_ERR | NETIF_MSG_RX_STATUS,
 				   "ERROR  packet dropped because of alloc failure\n");
-				fp->eth_q_stats.rx_skb_alloc_failed++;
+				bnx2x_fp_qstats(bp, fp)->rx_skb_alloc_failed++;
 reuse_rx:
 				bnx2x_reuse_rx_data(fp, bd_cons, bd_prod);
 				goto next_rx;
@@ -821,13 +865,14 @@ reuse_rx:
 		skb->protocol = eth_type_trans(skb, bp->dev);
 
 		/* Set Toeplitz hash for a none-LRO skb */
-		skb->rxhash = bnx2x_get_rxhash(bp, cqe_fp);
+		skb->rxhash = bnx2x_get_rxhash(bp, cqe_fp, &l4_rxhash);
+		skb->l4_rxhash = l4_rxhash;
 
 		skb_checksum_none_assert(skb);
 
 		if (bp->dev->features & NETIF_F_RXCSUM)
-			bnx2x_csum_validate(skb, cqe, fp);
-
+			bnx2x_csum_validate(skb, cqe, fp,
+					    bnx2x_fp_qstats(bp, fp));
 
 		skb_record_rx_queue(skb, fp->rx_queue);
 
@@ -888,7 +933,7 @@ static irqreturn_t bnx2x_msix_fp_int(int irq, void *fp_cookie)
 	prefetch(fp->rx_cons_sb);
 
 	for_each_cos_in_tx_queue(fp, cos)
-		prefetch(fp->txdata[cos].tx_cons_sb);
+		prefetch(fp->txdata_ptr[cos]->tx_cons_sb);
 
 	prefetch(&fp->sb_running_index[SM_RX_ID]);
 	napi_schedule(&bnx2x_fp(bp, fp->index, napi));
@@ -1205,7 +1250,7 @@ static void bnx2x_free_tx_skbs(struct bnx2x *bp)
 	for_each_tx_queue(bp, i) {
 		struct bnx2x_fastpath *fp = &bp->fp[i];
 		for_each_cos_in_tx_queue(fp, cos) {
-			struct bnx2x_fp_txdata *txdata = &fp->txdata[cos];
+			struct bnx2x_fp_txdata *txdata = fp->txdata_ptr[cos];
 			unsigned pkts_compl = 0, bytes_compl = 0;
 
 			u16 sw_prod = txdata->tx_pkt_prod;
@@ -1217,7 +1262,8 @@ static void bnx2x_free_tx_skbs(struct bnx2x *bp)
 				sw_cons++;
 			}
 			netdev_tx_reset_queue(
-			    netdev_get_tx_queue(bp->dev, txdata->txq_index));
+				netdev_get_tx_queue(bp->dev,
+						    txdata->txq_index));
 		}
 	}
 }
@@ -1325,7 +1371,7 @@ void bnx2x_free_irq(struct bnx2x *bp)
 		free_irq(bp->dev->irq, bp->dev);
 }
 
-int __devinit bnx2x_enable_msix(struct bnx2x *bp)
+int bnx2x_enable_msix(struct bnx2x *bp)
 {
 	int msix_vec = 0, i, rc, req_cnt;
 
@@ -1579,6 +1625,8 @@ void bnx2x_set_num_queues(struct bnx2x *bp)
 #endif
 	/* Add special queues */
 	bp->num_queues += NON_ETH_CONTEXT_USE;
+
+	BNX2X_DEV_INFO("set number of queues to %d\n", bp->num_queues);
 }
 
 /**
@@ -1607,8 +1655,8 @@ static int bnx2x_set_real_num_queues(struct bnx2x *bp)
 {
 	int rc, tx, rx;
 
-	tx = MAX_TXQS_PER_COS * bp->max_cos;
-	rx = BNX2X_NUM_ETH_QUEUES(bp);
+	tx = BNX2X_NUM_ETH_QUEUES(bp) * bp->max_cos;
+	rx = BNX2X_NUM_QUEUES(bp) - NON_ETH_CONTEXT_USE;
 
 /* account for fcoe queue */
 #ifdef BCM_CNIC
@@ -1666,14 +1714,13 @@ static void bnx2x_set_rx_buf_size(struct bnx2x *bp)
 static int bnx2x_init_rss_pf(struct bnx2x *bp)
 {
 	int i;
-	u8 ind_table[T_ETH_INDIRECTION_TABLE_SIZE] = {0};
 	u8 num_eth_queues = BNX2X_NUM_ETH_QUEUES(bp);
 
 	/* Prepare the initial contents fo the indirection table if RSS is
 	 * enabled
 	 */
-	for (i = 0; i < sizeof(ind_table); i++)
-		ind_table[i] =
+	for (i = 0; i < sizeof(bp->rss_conf_obj.ind_table); i++)
+		bp->rss_conf_obj.ind_table[i] =
 			bp->fp->cl_id +
 			ethtool_rxfh_indir_default(i, num_eth_queues);
 
@@ -1685,12 +1732,11 @@ static int bnx2x_init_rss_pf(struct bnx2x *bp)
 	 * For 57712 and newer on the other hand it's a per-function
 	 * configuration.
 	 */
-	return bnx2x_config_rss_eth(bp, ind_table,
-				    bp->port.pmf || !CHIP_IS_E1x(bp));
+	return bnx2x_config_rss_eth(bp, bp->port.pmf || !CHIP_IS_E1x(bp));
 }
 
 int bnx2x_config_rss_pf(struct bnx2x *bp, struct bnx2x_rss_config_obj *rss_obj,
-			u8 *ind_table, bool config_hash)
+			bool config_hash)
 {
 	struct bnx2x_config_rss_params params = {NULL};
 	int i;
@@ -1713,11 +1759,15 @@ int bnx2x_config_rss_pf(struct bnx2x *bp, struct bnx2x_rss_config_obj *rss_obj,
 	__set_bit(BNX2X_RSS_IPV4_TCP, &params.rss_flags);
 	__set_bit(BNX2X_RSS_IPV6, &params.rss_flags);
 	__set_bit(BNX2X_RSS_IPV6_TCP, &params.rss_flags);
+	if (rss_obj->udp_rss_v4)
+		__set_bit(BNX2X_RSS_IPV4_UDP, &params.rss_flags);
+	if (rss_obj->udp_rss_v6)
+		__set_bit(BNX2X_RSS_IPV6_UDP, &params.rss_flags);
 
 	/* Hash bits */
 	params.rss_result_mask = MULTI_MASK;
 
-	memcpy(params.ind_table, ind_table, sizeof(params.ind_table));
+	memcpy(params.ind_table, rss_obj->ind_table, sizeof(params.ind_table));
 
 	if (config_hash) {
 		/* RSS keys */
@@ -1754,7 +1804,7 @@ static void bnx2x_squeeze_objects(struct bnx2x *bp)
 	int rc;
 	unsigned long ramrod_flags = 0, vlan_mac_flags = 0;
 	struct bnx2x_mcast_ramrod_params rparam = {NULL};
-	struct bnx2x_vlan_mac_obj *mac_obj = &bp->fp->mac_obj;
+	struct bnx2x_vlan_mac_obj *mac_obj = &bp->sp_objs->mac_obj;
 
 	/***************** Cleanup MACs' object first *************************/
 
@@ -1765,7 +1815,7 @@ static void bnx2x_squeeze_objects(struct bnx2x *bp)
 
 	/* Clean ETH primary MAC */
 	__set_bit(BNX2X_ETH_MAC, &vlan_mac_flags);
-	rc = mac_obj->delete_all(bp, &bp->fp->mac_obj, &vlan_mac_flags,
+	rc = mac_obj->delete_all(bp, &bp->sp_objs->mac_obj, &vlan_mac_flags,
 				 &ramrod_flags);
 	if (rc != 0)
 		BNX2X_ERR("Failed to clean ETH MACs: %d\n", rc);
@@ -1851,11 +1901,16 @@ bool bnx2x_test_firmware_version(struct bnx2x *bp, bool is_err)
 static void bnx2x_bz_fp(struct bnx2x *bp, int index)
 {
 	struct bnx2x_fastpath *fp = &bp->fp[index];
+	struct bnx2x_fp_stats *fp_stats = &bp->fp_stats[index];
+
+	int cos;
 	struct napi_struct orig_napi = fp->napi;
+	struct bnx2x_agg_info *orig_tpa_info = fp->tpa_info;
 	/* bzero bnx2x_fastpath contents */
-	if (bp->stats_init)
+	if (bp->stats_init) {
+		memset(fp->tpa_info, 0, sizeof(*fp->tpa_info));
 		memset(fp, 0, sizeof(*fp));
-	else {
+	} else {
 		/* Keep Queue statistics */
 		struct bnx2x_eth_q_stats *tmp_eth_q_stats;
 		struct bnx2x_eth_q_stats_old *tmp_eth_q_stats_old;
@@ -1863,26 +1918,27 @@ static void bnx2x_bz_fp(struct bnx2x *bp, int index)
 		tmp_eth_q_stats = kzalloc(sizeof(struct bnx2x_eth_q_stats),
 					  GFP_KERNEL);
 		if (tmp_eth_q_stats)
-			memcpy(tmp_eth_q_stats, &fp->eth_q_stats,
+			memcpy(tmp_eth_q_stats, &fp_stats->eth_q_stats,
 			       sizeof(struct bnx2x_eth_q_stats));
 
 		tmp_eth_q_stats_old =
 			kzalloc(sizeof(struct bnx2x_eth_q_stats_old),
 				GFP_KERNEL);
 		if (tmp_eth_q_stats_old)
-			memcpy(tmp_eth_q_stats_old, &fp->eth_q_stats_old,
+			memcpy(tmp_eth_q_stats_old, &fp_stats->eth_q_stats_old,
 			       sizeof(struct bnx2x_eth_q_stats_old));
 
+		memset(fp->tpa_info, 0, sizeof(*fp->tpa_info));
 		memset(fp, 0, sizeof(*fp));
 
 		if (tmp_eth_q_stats) {
-			memcpy(&fp->eth_q_stats, tmp_eth_q_stats,
-				   sizeof(struct bnx2x_eth_q_stats));
+			memcpy(&fp_stats->eth_q_stats, tmp_eth_q_stats,
+			       sizeof(struct bnx2x_eth_q_stats));
 			kfree(tmp_eth_q_stats);
 		}
 
 		if (tmp_eth_q_stats_old) {
-			memcpy(&fp->eth_q_stats_old, tmp_eth_q_stats_old,
+			memcpy(&fp_stats->eth_q_stats_old, tmp_eth_q_stats_old,
 			       sizeof(struct bnx2x_eth_q_stats_old));
 			kfree(tmp_eth_q_stats_old);
 		}
@@ -1891,7 +1947,7 @@ static void bnx2x_bz_fp(struct bnx2x *bp, int index)
 
 	/* Restore the NAPI object as it has been already initialized */
 	fp->napi = orig_napi;
-
+	fp->tpa_info = orig_tpa_info;
 	fp->bp = bp;
 	fp->index = index;
 	if (IS_ETH_FP(fp))
@@ -1900,6 +1956,16 @@ static void bnx2x_bz_fp(struct bnx2x *bp, int index)
 		/* Special queues support only one CoS */
 		fp->max_cos = 1;
 
+	/* Init txdata pointers */
+#ifdef BCM_CNIC
+	if (IS_FCOE_FP(fp))
+		fp->txdata_ptr[0] = &bp->bnx2x_txq[FCOE_TXQ_IDX(bp)];
+#endif
+	if (IS_ETH_FP(fp))
+		for_each_cos_in_tx_queue(fp, cos)
+			fp->txdata_ptr[cos] = &bp->bnx2x_txq[cos *
+				BNX2X_NUM_ETH_QUEUES(bp) + index];
+
 	/*
 	 * set the tpa flag for each queue. The tpa flag determines the queue
 	 * minimal size so it must be set prior to queue memory allocation
@@ -1949,11 +2015,13 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 	/*
 	 * Zero fastpath structures preserving invariants like napi, which are
 	 * allocated only once, fp index, max_cos, bp pointer.
-	 * Also set fp->disable_tpa.
+	 * Also set fp->disable_tpa and txdata_ptr.
 	 */
 	DP(NETIF_MSG_IFUP, "num queues: %d", bp->num_queues);
 	for_each_queue(bp, i)
 		bnx2x_bz_fp(bp, i);
+	memset(bp->bnx2x_txq, 0, bp->bnx2x_txq_size *
+	       sizeof(struct bnx2x_fp_txdata));
 
 
 	/* Set the receive queues buffer size */
@@ -2176,6 +2244,7 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 		break;
 
 	case LOAD_DIAG:
+	case LOAD_LOOPBACK_EXT:
 		bp->state = BNX2X_STATE_DIAG;
 		break;
 
@@ -2195,6 +2264,7 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 	/* re-read iscsi info */
 	bnx2x_get_iscsi_info(bp);
 	bnx2x_setup_cnic_irq_info(bp);
+	bnx2x_setup_cnic_info(bp);
 	if (bp->state == BNX2X_STATE_OPEN)
 		bnx2x_cnic_notify(bp, CNIC_CTL_START_CMD);
 #endif
@@ -2215,7 +2285,10 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 		return -EBUSY;
 	}
 
-	bnx2x_dcbx_init(bp);
+	/* If PMF - send ADMIN DCBX msg to MFW to initiate DCBX FSM */
+	if (bp->port.pmf && (bp->state != BNX2X_STATE_DIAG))
+		bnx2x_dcbx_init(bp, false);
+
 	return 0;
 
 #ifndef BNX2X_STOP_ON_ERROR
@@ -2298,6 +2371,7 @@ int bnx2x_nic_unload(struct bnx2x *bp, int unload_mode)
 
 	/* Stop Tx */
 	bnx2x_tx_disable(bp);
+	netdev_reset_tc(bp->dev);
 
 #ifdef BCM_CNIC
 	bnx2x_cnic_notify(bp, CNIC_CTL_STOP_CMD);
@@ -2456,8 +2530,8 @@ int bnx2x_poll(struct napi_struct *napi, int budget)
 #endif
 
 		for_each_cos_in_tx_queue(fp, cos)
-			if (bnx2x_tx_queue_has_work(&fp->txdata[cos]))
-				bnx2x_tx_int(bp, &fp->txdata[cos]);
+			if (bnx2x_tx_queue_has_work(fp->txdata_ptr[cos]))
+				bnx2x_tx_int(bp, fp->txdata_ptr[cos]);
 
 
 		if (bnx2x_has_rx_work(fp)) {
@@ -2834,7 +2908,6 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct bnx2x *bp = netdev_priv(dev);
 
-	struct bnx2x_fastpath *fp;
 	struct netdev_queue *txq;
 	struct bnx2x_fp_txdata *txdata;
 	struct sw_tx_bd *tx_buf;
@@ -2844,7 +2917,7 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct eth_tx_parse_bd_e2 *pbd_e2 = NULL;
 	u32 pbd_e2_parsing_data = 0;
 	u16 pkt_prod, bd_prod;
-	int nbd, txq_index, fp_index, txdata_index;
+	int nbd, txq_index;
 	dma_addr_t mapping;
 	u32 xmit_type = bnx2x_xmit_type(bp, skb);
 	int i;
@@ -2863,39 +2936,22 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	BUG_ON(txq_index >= MAX_ETH_TXQ_IDX(bp) + FCOE_PRESENT);
 
-	/* decode the fastpath index and the cos index from the txq */
-	fp_index = TXQ_TO_FP(txq_index);
-	txdata_index = TXQ_TO_COS(txq_index);
-
-#ifdef BCM_CNIC
-	/*
-	 * Override the above for the FCoE queue:
-	 *   - FCoE fp entry is right after the ETH entries.
-	 *   - FCoE L2 queue uses bp->txdata[0] only.
-	 */
-	if (unlikely(!NO_FCOE(bp) && (txq_index ==
-				      bnx2x_fcoe_tx(bp, txq_index)))) {
-		fp_index = FCOE_IDX;
-		txdata_index = 0;
-	}
-#endif
+	txdata = &bp->bnx2x_txq[txq_index];
 
 	/* enable this debug print to view the transmission queue being used
 	DP(NETIF_MSG_TX_QUEUED, "indices: txq %d, fp %d, txdata %d\n",
 	   txq_index, fp_index, txdata_index); */
 
-	/* locate the fastpath and the txdata */
-	fp = &bp->fp[fp_index];
-	txdata = &fp->txdata[txdata_index];
-
 	/* enable this debug print to view the tranmission details
 	DP(NETIF_MSG_TX_QUEUED,
 	   "transmitting packet cid %d fp index %d txdata_index %d tx_data ptr %p fp pointer %p\n",
 	   txdata->cid, fp_index, txdata_index, txdata, fp); */
 
 	if (unlikely(bnx2x_tx_avail(bp, txdata) <
-		     (skb_shinfo(skb)->nr_frags + 3))) {
-		fp->eth_q_stats.driver_xoff++;
+			skb_shinfo(skb)->nr_frags +
+			BDS_PER_TX_PKT +
+			NEXT_CNT_PER_TX_PKT(MAX_BDS_PER_TX_PKT))) {
+		bnx2x_fp_qstats(bp, txdata->parent_fp)->driver_xoff++;
 		netif_tx_stop_queue(txq);
 		BNX2X_ERR("BUG! Tx ring full when queue awake!\n");
 		return NETDEV_TX_BUSY;
@@ -3169,7 +3225,7 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	txdata->tx_bd_prod += nbd;
 
-	if (unlikely(bnx2x_tx_avail(bp, txdata) < MAX_SKB_FRAGS + 4)) {
+	if (unlikely(bnx2x_tx_avail(bp, txdata) < MAX_DESC_PER_TX_PKT)) {
 		netif_tx_stop_queue(txq);
 
 		/* paired memory barrier is in bnx2x_tx_int(), we have to keep
@@ -3177,8 +3233,8 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		 * fp->bd_tx_cons */
 		smp_mb();
 
-		fp->eth_q_stats.driver_xoff++;
-		if (bnx2x_tx_avail(bp, txdata) >= MAX_SKB_FRAGS + 4)
+		bnx2x_fp_qstats(bp, txdata->parent_fp)->driver_xoff++;
+		if (bnx2x_tx_avail(bp, txdata) >= MAX_DESC_PER_TX_PKT)
 			netif_tx_wake_queue(txq);
 	}
 	txdata->tx_pkt++;
@@ -3243,7 +3299,7 @@ int bnx2x_setup_tc(struct net_device *dev, u8 num_tc)
 	/* configure traffic class to transmission queue mapping */
 	for (cos = 0; cos < bp->max_cos; cos++) {
 		count = BNX2X_NUM_ETH_QUEUES(bp);
-		offset = cos * MAX_TXQS_PER_COS;
+		offset = cos * BNX2X_NUM_NON_CNIC_QUEUES(bp);
 		netdev_set_tc_queue(dev, cos, count, offset);
 		DP(BNX2X_MSG_SP | NETIF_MSG_IFUP,
 		   "mapping tc %d to offset %d count %d\n",
@@ -3342,7 +3398,7 @@ static void bnx2x_free_fp_mem_at(struct bnx2x *bp, int fp_index)
 	if (!skip_tx_queue(bp, fp_index)) {
 		/* fastpath tx rings: tx_buf tx_desc */
 		for_each_cos_in_tx_queue(fp, cos) {
-			struct bnx2x_fp_txdata *txdata = &fp->txdata[cos];
+			struct bnx2x_fp_txdata *txdata = fp->txdata_ptr[cos];
 
 			DP(NETIF_MSG_IFDOWN,
 			   "freeing tx memory of fp %d cos %d cid %d\n",
@@ -3414,7 +3470,7 @@ static int bnx2x_alloc_rx_bds(struct bnx2x_fastpath *fp,
 			       cqe_ring_prod);
 	fp->rx_pkt = fp->rx_calls = 0;
 
-	fp->eth_q_stats.rx_skb_alloc_failed += failure_cnt;
+	bnx2x_fp_stats(bp, fp)->eth_q_stats.rx_skb_alloc_failed += failure_cnt;
 
 	return i - failure_cnt;
 }
@@ -3499,7 +3555,7 @@ static int bnx2x_alloc_fp_mem_at(struct bnx2x *bp, int index)
 	if (!skip_tx_queue(bp, index)) {
 		/* fastpath tx rings: tx_buf tx_desc */
 		for_each_cos_in_tx_queue(fp, cos) {
-			struct bnx2x_fp_txdata *txdata = &fp->txdata[cos];
+			struct bnx2x_fp_txdata *txdata = fp->txdata_ptr[cos];
 
 			DP(NETIF_MSG_IFUP,
 			   "allocating tx memory of fp %d cos %d\n",
@@ -3582,7 +3638,7 @@ int bnx2x_alloc_fp_mem(struct bnx2x *bp)
 #ifdef BCM_CNIC
 	if (!NO_FCOE(bp))
 		/* FCoE */
-		if (bnx2x_alloc_fp_mem_at(bp, FCOE_IDX))
+		if (bnx2x_alloc_fp_mem_at(bp, FCOE_IDX(bp)))
 			/* we will fail load process instead of mark
 			 * NO_FCOE_FLAG
 			 */
@@ -3607,7 +3663,7 @@ int bnx2x_alloc_fp_mem(struct bnx2x *bp)
 		 */
 
 		/* move FCoE fp even NO_FCOE_FLAG is on */
-		bnx2x_move_fp(bp, FCOE_IDX, FCOE_IDX - delta);
+		bnx2x_move_fp(bp, FCOE_IDX(bp), FCOE_IDX(bp) - delta);
 #endif
 		bp->num_queues -= delta;
 		BNX2X_ERR("Adjusted num of queues from %d to %d\n",
@@ -3619,7 +3675,11 @@ int bnx2x_alloc_fp_mem(struct bnx2x *bp)
 
 void bnx2x_free_mem_bp(struct bnx2x *bp)
 {
+	kfree(bp->fp->tpa_info);
 	kfree(bp->fp);
+	kfree(bp->sp_objs);
+	kfree(bp->fp_stats);
+	kfree(bp->bnx2x_txq);
 	kfree(bp->msix_table);
 	kfree(bp->ilt);
 }
@@ -3630,6 +3690,8 @@ int __devinit bnx2x_alloc_mem_bp(struct bnx2x *bp)
 	struct msix_entry *tbl;
 	struct bnx2x_ilt *ilt;
 	int msix_table_size = 0;
+	int fp_array_size;
+	int i;
 
 	/*
 	 * The biggest MSI-X table we might need is as a maximum number of fast
@@ -3638,12 +3700,44 @@ int __devinit bnx2x_alloc_mem_bp(struct bnx2x *bp)
 	msix_table_size = bp->igu_sb_cnt + 1;
 
 	/* fp array: RSS plus CNIC related L2 queues */
-	fp = kcalloc(BNX2X_MAX_RSS_COUNT(bp) + NON_ETH_CONTEXT_USE,
-		     sizeof(*fp), GFP_KERNEL);
+	fp_array_size = BNX2X_MAX_RSS_COUNT(bp) + NON_ETH_CONTEXT_USE;
+	BNX2X_DEV_INFO("fp_array_size %d", fp_array_size);
+
+	fp = kcalloc(fp_array_size, sizeof(*fp), GFP_KERNEL);
 	if (!fp)
 		goto alloc_err;
+	for (i = 0; i < fp_array_size; i++) {
+		fp[i].tpa_info =
+			kcalloc(ETH_MAX_AGGREGATION_QUEUES_E1H_E2,
+				sizeof(struct bnx2x_agg_info), GFP_KERNEL);
+		if (!(fp[i].tpa_info))
+			goto alloc_err;
+	}
+
 	bp->fp = fp;
 
+	/* allocate sp objs */
+	bp->sp_objs = kcalloc(fp_array_size, sizeof(struct bnx2x_sp_objs),
+			      GFP_KERNEL);
+	if (!bp->sp_objs)
+		goto alloc_err;
+
+	/* allocate fp_stats */
+	bp->fp_stats = kcalloc(fp_array_size, sizeof(struct bnx2x_fp_stats),
+			       GFP_KERNEL);
+	if (!bp->fp_stats)
+		goto alloc_err;
+
+	/* Allocate memory for the transmission queues array */
+	bp->bnx2x_txq_size = BNX2X_MAX_RSS_COUNT(bp) * BNX2X_MULTI_TX_COS;
+#ifdef BCM_CNIC
+	bp->bnx2x_txq_size++;
+#endif
+	bp->bnx2x_txq = kcalloc(bp->bnx2x_txq_size,
+				sizeof(struct bnx2x_fp_txdata), GFP_KERNEL);
+	if (!bp->bnx2x_txq)
+		goto alloc_err;
+
 	/* msix table */
 	tbl = kcalloc(msix_table_size, sizeof(*tbl), GFP_KERNEL);
 	if (!tbl)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index 7cd99b75347..dfa757e7429 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -29,6 +29,7 @@
 extern int load_count[2][3]; /* per-path: 0-common, 1-port0, 2-port1 */
 
 extern int num_queues;
+extern int int_mode;
 
 /************************ Macros ********************************/
 #define BNX2X_PCI_FREE(x, y, size) \
@@ -89,12 +90,12 @@ void bnx2x_send_unload_done(struct bnx2x *bp);
  * bnx2x_config_rss_pf - configure RSS parameters in a PF.
  *
  * @bp:			driver handle
- * @rss_obj		RSS object to use
+ * @rss_obj:		RSS object to use
  * @ind_table:		indirection table to configure
  * @config_hash:	re-configure RSS hash keys configuration
  */
 int bnx2x_config_rss_pf(struct bnx2x *bp, struct bnx2x_rss_config_obj *rss_obj,
-			u8 *ind_table, bool config_hash);
+			bool config_hash);
 
 /**
  * bnx2x__init_func_obj - init function object
@@ -244,6 +245,14 @@ int bnx2x_cnic_notify(struct bnx2x *bp, int cmd);
  * @bp:		driver handle
  */
 void bnx2x_setup_cnic_irq_info(struct bnx2x *bp);
+
+/**
+ * bnx2x_setup_cnic_info - provides cnic with updated info
+ *
+ * @bp:		driver handle
+ */
+void bnx2x_setup_cnic_info(struct bnx2x *bp);
+
 #endif
 
 /**
@@ -409,7 +418,7 @@ void bnx2x_ilt_set_info(struct bnx2x *bp);
  *
  * @bp:		driver handle
  */
-void bnx2x_dcbx_init(struct bnx2x *bp);
+void bnx2x_dcbx_init(struct bnx2x *bp, bool update_shmem);
 
 /**
  * bnx2x_set_power_state - set power state to the requested value.
@@ -487,7 +496,7 @@ void bnx2x_netif_start(struct bnx2x *bp);
  * fills msix_table, requests vectors, updates num_queues
  * according to number of available vectors.
  */
-int __devinit bnx2x_enable_msix(struct bnx2x *bp);
+int bnx2x_enable_msix(struct bnx2x *bp);
 
 /**
  * bnx2x_enable_msi - request msi mode from OS, updated internals accordingly
@@ -728,7 +737,7 @@ static inline bool bnx2x_has_tx_work(struct bnx2x_fastpath *fp)
 {
 	u8 cos;
 	for_each_cos_in_tx_queue(fp, cos)
-		if (bnx2x_tx_queue_has_work(&fp->txdata[cos]))
+		if (bnx2x_tx_queue_has_work(fp->txdata_ptr[cos]))
 			return true;
 	return false;
 }
@@ -780,8 +789,10 @@ static inline void bnx2x_add_all_napi(struct bnx2x *bp)
 {
 	int i;
 
+	bp->num_napi_queues = bp->num_queues;
+
 	/* Add NAPI objects */
-	for_each_rx_queue(bp, i)
+	for_each_napi_rx_queue(bp, i)
 		netif_napi_add(bp->dev, &bnx2x_fp(bp, i, napi),
 			       bnx2x_poll, BNX2X_NAPI_WEIGHT);
 }
@@ -790,10 +801,12 @@ static inline void bnx2x_del_all_napi(struct bnx2x *bp)
 {
 	int i;
 
-	for_each_rx_queue(bp, i)
+	for_each_napi_rx_queue(bp, i)
 		netif_napi_del(&bnx2x_fp(bp, i, napi));
 }
 
+void bnx2x_set_int_mode(struct bnx2x *bp);
+
 static inline void bnx2x_disable_msi(struct bnx2x *bp)
 {
 	if (bp->flags & USING_MSIX_FLAG) {
@@ -809,7 +822,8 @@ static inline int bnx2x_calc_num_queues(struct bnx2x *bp)
 {
 	return  num_queues ?
 		 min_t(int, num_queues, BNX2X_MAX_QUEUES(bp)) :
-		 min_t(int, num_online_cpus(), BNX2X_MAX_QUEUES(bp));
+		 min_t(int, netif_get_num_default_rss_queues(),
+		       BNX2X_MAX_QUEUES(bp));
 }
 
 static inline void bnx2x_clear_sge_mask_next_elems(struct bnx2x_fastpath *fp)
@@ -865,11 +879,9 @@ static inline int func_by_vn(struct bnx2x *bp, int vn)
 	return 2 * vn + BP_PORT(bp);
 }
 
-static inline int bnx2x_config_rss_eth(struct bnx2x *bp, u8 *ind_table,
-				       bool config_hash)
+static inline int bnx2x_config_rss_eth(struct bnx2x *bp, bool config_hash)
 {
-	return bnx2x_config_rss_pf(bp, &bp->rss_conf_obj, ind_table,
-				   config_hash);
+	return bnx2x_config_rss_pf(bp, &bp->rss_conf_obj, config_hash);
 }
 
 /**
@@ -975,8 +987,8 @@ static inline void bnx2x_init_vlan_mac_fp_objs(struct bnx2x_fastpath *fp,
 	struct bnx2x *bp = fp->bp;
 
 	/* Configure classification DBs */
-	bnx2x_init_mac_obj(bp, &fp->mac_obj, fp->cl_id, fp->cid,
-			   BP_FUNC(bp), bnx2x_sp(bp, mac_rdata),
+	bnx2x_init_mac_obj(bp, &bnx2x_sp_obj(bp, fp).mac_obj, fp->cl_id,
+			   fp->cid, BP_FUNC(bp), bnx2x_sp(bp, mac_rdata),
 			   bnx2x_sp_mapping(bp, mac_rdata),
 			   BNX2X_FILTER_MAC_PENDING,
 			   &bp->sp_state, obj_type,
@@ -1068,12 +1080,14 @@ static inline u32 bnx2x_rx_ustorm_prods_offset(struct bnx2x_fastpath *fp)
 }
 
 static inline void bnx2x_init_txdata(struct bnx2x *bp,
-	struct bnx2x_fp_txdata *txdata, u32 cid, int txq_index,
-	__le16 *tx_cons_sb)
+				     struct bnx2x_fp_txdata *txdata, u32 cid,
+				     int txq_index, __le16 *tx_cons_sb,
+				     struct bnx2x_fastpath *fp)
 {
 	txdata->cid = cid;
 	txdata->txq_index = txq_index;
 	txdata->tx_cons_sb = tx_cons_sb;
+	txdata->parent_fp = fp;
 
 	DP(NETIF_MSG_IFUP, "created tx data cid %d, txq %d\n",
 	   txdata->cid, txdata->txq_index);
@@ -1107,18 +1121,13 @@ static inline void bnx2x_init_fcoe_fp(struct bnx2x *bp)
 	bnx2x_fcoe(bp, rx_queue) = BNX2X_NUM_ETH_QUEUES(bp);
 	bnx2x_fcoe(bp, cl_id) = bnx2x_cnic_eth_cl_id(bp,
 						     BNX2X_FCOE_ETH_CL_ID_IDX);
-	/** Current BNX2X_FCOE_ETH_CID deffinition implies not more than
-	 *  16 ETH clients per function when CNIC is enabled!
-	 *
-	 *  Fix it ASAP!!!
-	 */
-	bnx2x_fcoe(bp, cid) = BNX2X_FCOE_ETH_CID;
+	bnx2x_fcoe(bp, cid) = BNX2X_FCOE_ETH_CID(bp);
 	bnx2x_fcoe(bp, fw_sb_id) = DEF_SB_ID;
 	bnx2x_fcoe(bp, igu_sb_id) = bp->igu_dsb_id;
 	bnx2x_fcoe(bp, rx_cons_sb) = BNX2X_FCOE_L2_RX_INDEX;
-
-	bnx2x_init_txdata(bp, &bnx2x_fcoe(bp, txdata[0]),
-			  fp->cid, FCOE_TXQ_IDX(bp), BNX2X_FCOE_L2_TX_INDEX);
+	bnx2x_init_txdata(bp, bnx2x_fcoe(bp, txdata_ptr[0]),
+			  fp->cid, FCOE_TXQ_IDX(bp), BNX2X_FCOE_L2_TX_INDEX,
+			  fp);
 
 	DP(NETIF_MSG_IFUP, "created fcoe tx data (fp index %d)\n", fp->index);
 
@@ -1135,8 +1144,8 @@ static inline void bnx2x_init_fcoe_fp(struct bnx2x *bp)
 	/* No multi-CoS for FCoE L2 client */
 	BUG_ON(fp->max_cos != 1);
 
-	bnx2x_init_queue_obj(bp, &fp->q_obj, fp->cl_id, &fp->cid, 1,
-			     BP_FUNC(bp), bnx2x_sp(bp, q_rdata),
+	bnx2x_init_queue_obj(bp, &bnx2x_sp_obj(bp, fp).q_obj, fp->cl_id,
+			     &fp->cid, 1, BP_FUNC(bp), bnx2x_sp(bp, q_rdata),
 			     bnx2x_sp_mapping(bp, q_rdata), q_type);
 
 	DP(NETIF_MSG_IFUP,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
index 4f9244bd753..8a73374e52a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
@@ -972,23 +972,26 @@ void bnx2x_dcbx_init_params(struct bnx2x *bp)
 	bp->dcbx_config_params.admin_default_priority = 0;
 }
 
-void bnx2x_dcbx_init(struct bnx2x *bp)
+void bnx2x_dcbx_init(struct bnx2x *bp, bool update_shmem)
 {
 	u32 dcbx_lldp_params_offset = SHMEM_LLDP_DCBX_PARAMS_NONE;
 
+	/* only PMF can send ADMIN msg to MFW in old MFW versions */
+	if ((!bp->port.pmf) && (!(bp->flags & BC_SUPPORTS_DCBX_MSG_NON_PMF)))
+		return;
+
 	if (bp->dcbx_enabled <= 0)
 		return;
 
 	/* validate:
 	 * chip of good for dcbx version,
 	 * dcb is wanted
-	 * the function is pmf
 	 * shmem2 contains DCBX support fields
 	 */
 	DP(BNX2X_MSG_DCB, "dcb_state %d bp->port.pmf %d\n",
 	   bp->dcb_state, bp->port.pmf);
 
-	if (bp->dcb_state == BNX2X_DCB_STATE_ON && bp->port.pmf &&
+	if (bp->dcb_state == BNX2X_DCB_STATE_ON &&
 	    SHMEM2_HAS(bp, dcbx_lldp_params_offset)) {
 		dcbx_lldp_params_offset =
 			SHMEM2_RD(bp, dcbx_lldp_params_offset);
@@ -999,12 +1002,23 @@ void bnx2x_dcbx_init(struct bnx2x *bp)
 		bnx2x_update_drv_flags(bp, 1 << DRV_FLAGS_DCB_CONFIGURED, 0);
 
 		if (SHMEM_LLDP_DCBX_PARAMS_NONE != dcbx_lldp_params_offset) {
-			bnx2x_dcbx_admin_mib_updated_params(bp,
-				dcbx_lldp_params_offset);
+			/* need HW lock to avoid scenario of two drivers
+			 * writing in parallel to shmem
+			 */
+			bnx2x_acquire_hw_lock(bp,
+					      HW_LOCK_RESOURCE_DCBX_ADMIN_MIB);
+			if (update_shmem)
+				bnx2x_dcbx_admin_mib_updated_params(bp,
+					dcbx_lldp_params_offset);
 
 			/* Let HW start negotiation */
 			bnx2x_fw_command(bp,
 					 DRV_MSG_CODE_DCBX_ADMIN_PMF_MSG, 0);
+			/* release HW lock only after MFW acks that it finished
+			 * reading values from shmem
+			 */
+			bnx2x_release_hw_lock(bp,
+					      HW_LOCK_RESOURCE_DCBX_ADMIN_MIB);
 		}
 	}
 }
@@ -2063,10 +2077,8 @@ static u8 bnx2x_dcbnl_set_all(struct net_device *netdev)
 			   "Handling parity error recovery. Try again later\n");
 		return 1;
 	}
-	if (netif_running(bp->dev)) {
-		bnx2x_nic_unload(bp, UNLOAD_NORMAL);
-		rc = bnx2x_nic_load(bp, LOAD_NORMAL);
-	}
+	if (netif_running(bp->dev))
+		bnx2x_dcbx_init(bp, true);
 	DP(BNX2X_MSG_DCB, "set_dcbx_params done (%d)\n", rc);
 	if (rc)
 		return 1;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index ddc18ee5c5a..fc4e0e3885b 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -177,6 +177,8 @@ static const struct {
 			4, STATS_FLAGS_FUNC, "recoverable_errors" },
 	{ STATS_OFFSET32(unrecoverable_error),
 			4, STATS_FLAGS_FUNC, "unrecoverable_errors" },
+	{ STATS_OFFSET32(eee_tx_lpi),
+			4, STATS_FLAGS_PORT, "Tx LPI entry count"}
 };
 
 #define BNX2X_NUM_STATS		ARRAY_SIZE(bnx2x_stats_arr)
@@ -185,7 +187,8 @@ static int bnx2x_get_port_type(struct bnx2x *bp)
 	int port_type;
 	u32 phy_idx = bnx2x_get_cur_phy_idx(bp);
 	switch (bp->link_params.phy[phy_idx].media_type) {
-	case ETH_PHY_SFP_FIBER:
+	case ETH_PHY_SFPP_10G_FIBER:
+	case ETH_PHY_SFP_1G_FIBER:
 	case ETH_PHY_XFP_FIBER:
 	case ETH_PHY_KR:
 	case ETH_PHY_CX4:
@@ -218,6 +221,11 @@ static int bnx2x_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		(bp->port.supported[cfg_idx ^ 1] &
 		 (SUPPORTED_TP | SUPPORTED_FIBRE));
 	cmd->advertising = bp->port.advertising[cfg_idx];
+	if (bp->link_params.phy[bnx2x_get_cur_phy_idx(bp)].media_type ==
+	    ETH_PHY_SFP_1G_FIBER) {
+		cmd->supported &= ~(SUPPORTED_10000baseT_Full);
+		cmd->advertising &= ~(ADVERTISED_10000baseT_Full);
+	}
 
 	if ((bp->state == BNX2X_STATE_OPEN) && (bp->link_vars.link_up)) {
 		if (!(bp->flags & MF_FUNC_DIS)) {
@@ -293,7 +301,7 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
 	struct bnx2x *bp = netdev_priv(dev);
 	u32 advertising, cfg_idx, old_multi_phy_config, new_multi_phy_config;
-	u32 speed;
+	u32 speed, phy_idx;
 
 	if (IS_MF_SD(bp))
 		return 0;
@@ -548,9 +556,11 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 				   "10G half not supported\n");
 				return -EINVAL;
 			}
-
+			phy_idx = bnx2x_get_cur_phy_idx(bp);
 			if (!(bp->port.supported[cfg_idx]
-			      & SUPPORTED_10000baseT_Full)) {
+			      & SUPPORTED_10000baseT_Full) ||
+			    (bp->link_params.phy[phy_idx].media_type ==
+			     ETH_PHY_SFP_1G_FIBER)) {
 				DP(BNX2X_MSG_ETHTOOL,
 				   "10G full not supported\n");
 				return -EINVAL;
@@ -824,7 +834,7 @@ static void bnx2x_get_drvinfo(struct net_device *dev,
 		 ((phy_fw_ver[0] != '\0') ? " phy " : ""), phy_fw_ver);
 	strlcpy(info->bus_info, pci_name(bp->pdev), sizeof(info->bus_info));
 	info->n_stats = BNX2X_NUM_STATS;
-	info->testinfo_len = BNX2X_NUM_TESTS;
+	info->testinfo_len = BNX2X_NUM_TESTS(bp);
 	info->eedump_len = bp->common.flash_size;
 	info->regdump_len = bnx2x_get_regs_len(dev);
 }
@@ -1150,6 +1160,65 @@ static int bnx2x_get_eeprom(struct net_device *dev,
 	return rc;
 }
 
+static int bnx2x_get_module_eeprom(struct net_device *dev,
+				   struct ethtool_eeprom *ee,
+				   u8 *data)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	int rc = 0, phy_idx;
+	u8 *user_data = data;
+	int remaining_len = ee->len, xfer_size;
+	unsigned int page_off = ee->offset;
+
+	if (!netif_running(dev)) {
+		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
+		   "cannot access eeprom when the interface is down\n");
+		return -EAGAIN;
+	}
+
+	phy_idx = bnx2x_get_cur_phy_idx(bp);
+	bnx2x_acquire_phy_lock(bp);
+	while (!rc && remaining_len > 0) {
+		xfer_size = (remaining_len > SFP_EEPROM_PAGE_SIZE) ?
+			SFP_EEPROM_PAGE_SIZE : remaining_len;
+		rc = bnx2x_read_sfp_module_eeprom(&bp->link_params.phy[phy_idx],
+						  &bp->link_params,
+						  page_off,
+						  xfer_size,
+						  user_data);
+		remaining_len -= xfer_size;
+		user_data += xfer_size;
+		page_off += xfer_size;
+	}
+
+	bnx2x_release_phy_lock(bp);
+	return rc;
+}
+
+static int bnx2x_get_module_info(struct net_device *dev,
+				 struct ethtool_modinfo *modinfo)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	int phy_idx;
+	if (!netif_running(dev)) {
+		DP(BNX2X_MSG_ETHTOOL  | BNX2X_MSG_NVM,
+		   "cannot access eeprom when the interface is down\n");
+		return -EAGAIN;
+	}
+
+	phy_idx = bnx2x_get_cur_phy_idx(bp);
+	switch (bp->link_params.phy[phy_idx].media_type) {
+	case ETH_PHY_SFPP_10G_FIBER:
+	case ETH_PHY_SFP_1G_FIBER:
+	case ETH_PHY_DA_TWINAX:
+		modinfo->type = ETH_MODULE_SFF_8079;
+		modinfo->eeprom_len = ETH_MODULE_SFF_8079_LEN;
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static int bnx2x_nvram_write_dword(struct bnx2x *bp, u32 offset, u32 val,
 				   u32 cmd_flags)
 {
@@ -1531,18 +1600,146 @@ static int bnx2x_set_pauseparam(struct net_device *dev,
 	return 0;
 }
 
-static const struct {
-	char string[ETH_GSTRING_LEN];
-} bnx2x_tests_str_arr[BNX2X_NUM_TESTS] = {
-	{ "register_test (offline)" },
-	{ "memory_test (offline)" },
-	{ "loopback_test (offline)" },
-	{ "nvram_test (online)" },
-	{ "interrupt_test (online)" },
-	{ "link_test (online)" },
-	{ "idle check (online)" }
+static char *bnx2x_tests_str_arr[BNX2X_NUM_TESTS_SF] = {
+	"register_test (offline)    ",
+	"memory_test (offline)      ",
+	"int_loopback_test (offline)",
+	"ext_loopback_test (offline)",
+	"nvram_test (online)        ",
+	"interrupt_test (online)    ",
+	"link_test (online)         "
 };
 
+static u32 bnx2x_eee_to_adv(u32 eee_adv)
+{
+	u32 modes = 0;
+
+	if (eee_adv & SHMEM_EEE_100M_ADV)
+		modes |= ADVERTISED_100baseT_Full;
+	if (eee_adv & SHMEM_EEE_1G_ADV)
+		modes |= ADVERTISED_1000baseT_Full;
+	if (eee_adv & SHMEM_EEE_10G_ADV)
+		modes |= ADVERTISED_10000baseT_Full;
+
+	return modes;
+}
+
+static u32 bnx2x_adv_to_eee(u32 modes, u32 shift)
+{
+	u32 eee_adv = 0;
+	if (modes & ADVERTISED_100baseT_Full)
+		eee_adv |= SHMEM_EEE_100M_ADV;
+	if (modes & ADVERTISED_1000baseT_Full)
+		eee_adv |= SHMEM_EEE_1G_ADV;
+	if (modes & ADVERTISED_10000baseT_Full)
+		eee_adv |= SHMEM_EEE_10G_ADV;
+
+	return eee_adv << shift;
+}
+
+static int bnx2x_get_eee(struct net_device *dev, struct ethtool_eee *edata)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	u32 eee_cfg;
+
+	if (!SHMEM2_HAS(bp, eee_status[BP_PORT(bp)])) {
+		DP(BNX2X_MSG_ETHTOOL, "BC Version does not support EEE\n");
+		return -EOPNOTSUPP;
+	}
+
+	eee_cfg = SHMEM2_RD(bp, eee_status[BP_PORT(bp)]);
+
+	edata->supported =
+		bnx2x_eee_to_adv((eee_cfg & SHMEM_EEE_SUPPORTED_MASK) >>
+				 SHMEM_EEE_SUPPORTED_SHIFT);
+
+	edata->advertised =
+		bnx2x_eee_to_adv((eee_cfg & SHMEM_EEE_ADV_STATUS_MASK) >>
+				 SHMEM_EEE_ADV_STATUS_SHIFT);
+	edata->lp_advertised =
+		bnx2x_eee_to_adv((eee_cfg & SHMEM_EEE_LP_ADV_STATUS_MASK) >>
+				 SHMEM_EEE_LP_ADV_STATUS_SHIFT);
+
+	/* SHMEM value is in 16u units --> Convert to 1u units. */
+	edata->tx_lpi_timer = (eee_cfg & SHMEM_EEE_TIMER_MASK) << 4;
+
+	edata->eee_enabled    = (eee_cfg & SHMEM_EEE_REQUESTED_BIT)	? 1 : 0;
+	edata->eee_active     = (eee_cfg & SHMEM_EEE_ACTIVE_BIT)	? 1 : 0;
+	edata->tx_lpi_enabled = (eee_cfg & SHMEM_EEE_LPI_REQUESTED_BIT) ? 1 : 0;
+
+	return 0;
+}
+
+static int bnx2x_set_eee(struct net_device *dev, struct ethtool_eee *edata)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+	u32 eee_cfg;
+	u32 advertised;
+
+	if (IS_MF(bp))
+		return 0;
+
+	if (!SHMEM2_HAS(bp, eee_status[BP_PORT(bp)])) {
+		DP(BNX2X_MSG_ETHTOOL, "BC Version does not support EEE\n");
+		return -EOPNOTSUPP;
+	}
+
+	eee_cfg = SHMEM2_RD(bp, eee_status[BP_PORT(bp)]);
+
+	if (!(eee_cfg & SHMEM_EEE_SUPPORTED_MASK)) {
+		DP(BNX2X_MSG_ETHTOOL, "Board does not support EEE!\n");
+		return -EOPNOTSUPP;
+	}
+
+	advertised = bnx2x_adv_to_eee(edata->advertised,
+				      SHMEM_EEE_ADV_STATUS_SHIFT);
+	if ((advertised != (eee_cfg & SHMEM_EEE_ADV_STATUS_MASK))) {
+		DP(BNX2X_MSG_ETHTOOL,
+		   "Direct manipulation of EEE advertisment is not supported\n");
+		return -EINVAL;
+	}
+
+	if (edata->tx_lpi_timer > EEE_MODE_TIMER_MASK) {
+		DP(BNX2X_MSG_ETHTOOL,
+		   "Maximal Tx Lpi timer supported is %x(u)\n",
+		   EEE_MODE_TIMER_MASK);
+		return -EINVAL;
+	}
+	if (edata->tx_lpi_enabled &&
+	    (edata->tx_lpi_timer < EEE_MODE_NVRAM_AGGRESSIVE_TIME)) {
+		DP(BNX2X_MSG_ETHTOOL,
+		   "Minimal Tx Lpi timer supported is %d(u)\n",
+		   EEE_MODE_NVRAM_AGGRESSIVE_TIME);
+		return -EINVAL;
+	}
+
+	/* All is well; Apply changes*/
+	if (edata->eee_enabled)
+		bp->link_params.eee_mode |= EEE_MODE_ADV_LPI;
+	else
+		bp->link_params.eee_mode &= ~EEE_MODE_ADV_LPI;
+
+	if (edata->tx_lpi_enabled)
+		bp->link_params.eee_mode |= EEE_MODE_ENABLE_LPI;
+	else
+		bp->link_params.eee_mode &= ~EEE_MODE_ENABLE_LPI;
+
+	bp->link_params.eee_mode &= ~EEE_MODE_TIMER_MASK;
+	bp->link_params.eee_mode |= (edata->tx_lpi_timer &
+				    EEE_MODE_TIMER_MASK) |
+				    EEE_MODE_OVERRIDE_NVRAM |
+				    EEE_MODE_OUTPUT_TIME;
+
+	/* Restart link to propogate changes */
+	if (netif_running(dev)) {
+		bnx2x_stats_handle(bp, STATS_EVENT_STOP);
+		bnx2x_link_set(bp);
+	}
+
+	return 0;
+}
+
+
 enum {
 	BNX2X_CHIP_E1_OFST = 0,
 	BNX2X_CHIP_E1H_OFST,
@@ -1811,6 +2008,14 @@ static void bnx2x_wait_for_link(struct bnx2x *bp, u8 link_up, u8 is_serdes)
 
 		if (cnt <= 0 && bnx2x_link_test(bp, is_serdes))
 			DP(BNX2X_MSG_ETHTOOL, "Timeout waiting for link up\n");
+
+		cnt = 1400;
+		while (!bp->link_vars.link_up && cnt--)
+			msleep(20);
+
+		if (cnt <= 0 && !bp->link_vars.link_up)
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Timeout waiting for link init\n");
 	}
 }
 
@@ -1821,7 +2026,7 @@ static int bnx2x_run_loopback(struct bnx2x *bp, int loopback_mode)
 	unsigned char *packet;
 	struct bnx2x_fastpath *fp_rx = &bp->fp[0];
 	struct bnx2x_fastpath *fp_tx = &bp->fp[0];
-	struct bnx2x_fp_txdata *txdata = &fp_tx->txdata[0];
+	struct bnx2x_fp_txdata *txdata = fp_tx->txdata_ptr[0];
 	u16 tx_start_idx, tx_idx;
 	u16 rx_start_idx, rx_idx;
 	u16 pkt_prod, bd_prod;
@@ -1836,13 +2041,16 @@ static int bnx2x_run_loopback(struct bnx2x *bp, int loopback_mode)
 	u16 len;
 	int rc = -ENODEV;
 	u8 *data;
-	struct netdev_queue *txq = netdev_get_tx_queue(bp->dev, txdata->txq_index);
+	struct netdev_queue *txq = netdev_get_tx_queue(bp->dev,
+						       txdata->txq_index);
 
 	/* check the loopback mode */
 	switch (loopback_mode) {
 	case BNX2X_PHY_LOOPBACK:
-		if (bp->link_params.loopback_mode != LOOPBACK_XGXS)
+		if (bp->link_params.loopback_mode != LOOPBACK_XGXS) {
+			DP(BNX2X_MSG_ETHTOOL, "PHY loopback not supported\n");
 			return -EINVAL;
+		}
 		break;
 	case BNX2X_MAC_LOOPBACK:
 		if (CHIP_IS_E3(bp)) {
@@ -1859,6 +2067,13 @@ static int bnx2x_run_loopback(struct bnx2x *bp, int loopback_mode)
 
 		bnx2x_phy_init(&bp->link_params, &bp->link_vars);
 		break;
+	case BNX2X_EXT_LOOPBACK:
+		if (bp->link_params.loopback_mode != LOOPBACK_EXT) {
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Can't configure external loopback\n");
+			return -EINVAL;
+		}
+		break;
 	default:
 		DP(BNX2X_MSG_ETHTOOL, "Command parameters not supported\n");
 		return -EINVAL;
@@ -2030,6 +2245,38 @@ static int bnx2x_test_loopback(struct bnx2x *bp)
 	return rc;
 }
 
+static int bnx2x_test_ext_loopback(struct bnx2x *bp)
+{
+	int rc;
+	u8 is_serdes =
+		(bp->link_vars.link_status & LINK_STATUS_SERDES_LINK) > 0;
+
+	if (BP_NOMCP(bp))
+		return -ENODEV;
+
+	if (!netif_running(bp->dev))
+		return BNX2X_EXT_LOOPBACK_FAILED;
+
+	bnx2x_nic_unload(bp, UNLOAD_NORMAL);
+	rc = bnx2x_nic_load(bp, LOAD_LOOPBACK_EXT);
+	if (rc) {
+		DP(BNX2X_MSG_ETHTOOL,
+		   "Can't perform self-test, nic_load (for external lb) failed\n");
+		return -ENODEV;
+	}
+	bnx2x_wait_for_link(bp, 1, is_serdes);
+
+	bnx2x_netif_stop(bp, 1);
+
+	rc = bnx2x_run_loopback(bp, BNX2X_EXT_LOOPBACK);
+	if (rc)
+		DP(BNX2X_MSG_ETHTOOL, "EXT loopback failed  (res %d)\n", rc);
+
+	bnx2x_netif_start(bp);
+
+	return rc;
+}
+
 #define CRC32_RESIDUAL			0xdebb20e3
 
 static int bnx2x_test_nvram(struct bnx2x *bp)
@@ -2112,7 +2359,7 @@ static int bnx2x_test_intr(struct bnx2x *bp)
 		return -ENODEV;
 	}
 
-	params.q_obj = &bp->fp->q_obj;
+	params.q_obj = &bp->sp_objs->q_obj;
 	params.cmd = BNX2X_Q_CMD_EMPTY;
 
 	__set_bit(RAMROD_COMP_WAIT, &params.ramrod_flags);
@@ -2125,24 +2372,31 @@ static void bnx2x_self_test(struct net_device *dev,
 {
 	struct bnx2x *bp = netdev_priv(dev);
 	u8 is_serdes;
+	int rc;
+
 	if (bp->recovery_state != BNX2X_RECOVERY_DONE) {
 		netdev_err(bp->dev,
 			   "Handling parity error recovery. Try again later\n");
 		etest->flags |= ETH_TEST_FL_FAILED;
 		return;
 	}
+	DP(BNX2X_MSG_ETHTOOL,
+	   "Self-test command parameters: offline = %d, external_lb = %d\n",
+	   (etest->flags & ETH_TEST_FL_OFFLINE),
+	   (etest->flags & ETH_TEST_FL_EXTERNAL_LB)>>2);
 
-	memset(buf, 0, sizeof(u64) * BNX2X_NUM_TESTS);
+	memset(buf, 0, sizeof(u64) * BNX2X_NUM_TESTS(bp));
 
-	if (!netif_running(dev))
+	if (!netif_running(dev)) {
+		DP(BNX2X_MSG_ETHTOOL,
+		   "Can't perform self-test when interface is down\n");
 		return;
+	}
 
-	/* offline tests are not supported in MF mode */
-	if (IS_MF(bp))
-		etest->flags &= ~ETH_TEST_FL_OFFLINE;
 	is_serdes = (bp->link_vars.link_status & LINK_STATUS_SERDES_LINK) > 0;
 
-	if (etest->flags & ETH_TEST_FL_OFFLINE) {
+	/* offline tests are not supported in MF mode */
+	if ((etest->flags & ETH_TEST_FL_OFFLINE) && !IS_MF(bp)) {
 		int port = BP_PORT(bp);
 		u32 val;
 		u8 link_up;
@@ -2155,7 +2409,14 @@ static void bnx2x_self_test(struct net_device *dev,
 		link_up = bp->link_vars.link_up;
 
 		bnx2x_nic_unload(bp, UNLOAD_NORMAL);
-		bnx2x_nic_load(bp, LOAD_DIAG);
+		rc = bnx2x_nic_load(bp, LOAD_DIAG);
+		if (rc) {
+			etest->flags |= ETH_TEST_FL_FAILED;
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Can't perform self-test, nic_load (for offline) failed\n");
+			return;
+		}
+
 		/* wait until link state is restored */
 		bnx2x_wait_for_link(bp, 1, is_serdes);
 
@@ -2168,30 +2429,51 @@ static void bnx2x_self_test(struct net_device *dev,
 			etest->flags |= ETH_TEST_FL_FAILED;
 		}
 
-		buf[2] = bnx2x_test_loopback(bp);
+		buf[2] = bnx2x_test_loopback(bp); /* internal LB */
 		if (buf[2] != 0)
 			etest->flags |= ETH_TEST_FL_FAILED;
 
+		if (etest->flags & ETH_TEST_FL_EXTERNAL_LB) {
+			buf[3] = bnx2x_test_ext_loopback(bp); /* external LB */
+			if (buf[3] != 0)
+				etest->flags |= ETH_TEST_FL_FAILED;
+			etest->flags |= ETH_TEST_FL_EXTERNAL_LB_DONE;
+		}
+
 		bnx2x_nic_unload(bp, UNLOAD_NORMAL);
 
 		/* restore input for TX port IF */
 		REG_WR(bp, NIG_REG_EGRESS_UMP0_IN_EN + port*4, val);
-
-		bnx2x_nic_load(bp, LOAD_NORMAL);
+		rc = bnx2x_nic_load(bp, LOAD_NORMAL);
+		if (rc) {
+			etest->flags |= ETH_TEST_FL_FAILED;
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Can't perform self-test, nic_load (for online) failed\n");
+			return;
+		}
 		/* wait until link state is restored */
 		bnx2x_wait_for_link(bp, link_up, is_serdes);
 	}
 	if (bnx2x_test_nvram(bp) != 0) {
-		buf[3] = 1;
+		if (!IS_MF(bp))
+			buf[4] = 1;
+		else
+			buf[0] = 1;
 		etest->flags |= ETH_TEST_FL_FAILED;
 	}
 	if (bnx2x_test_intr(bp) != 0) {
-		buf[4] = 1;
+		if (!IS_MF(bp))
+			buf[5] = 1;
+		else
+			buf[1] = 1;
 		etest->flags |= ETH_TEST_FL_FAILED;
 	}
 
 	if (bnx2x_link_test(bp, is_serdes) != 0) {
-		buf[5] = 1;
+		if (!IS_MF(bp))
+			buf[6] = 1;
+		else
+			buf[2] = 1;
 		etest->flags |= ETH_TEST_FL_FAILED;
 	}
 
@@ -2236,7 +2518,7 @@ static int bnx2x_get_sset_count(struct net_device *dev, int stringset)
 		return num_stats;
 
 	case ETH_SS_TEST:
-		return BNX2X_NUM_TESTS;
+		return BNX2X_NUM_TESTS(bp);
 
 	default:
 		return -EINVAL;
@@ -2246,7 +2528,7 @@ static int bnx2x_get_sset_count(struct net_device *dev, int stringset)
 static void bnx2x_get_strings(struct net_device *dev, u32 stringset, u8 *buf)
 {
 	struct bnx2x *bp = netdev_priv(dev);
-	int i, j, k;
+	int i, j, k, offset, start;
 	char queue_name[MAX_QUEUE_NAME_LEN+1];
 
 	switch (stringset) {
@@ -2277,7 +2559,17 @@ static void bnx2x_get_strings(struct net_device *dev, u32 stringset, u8 *buf)
 		break;
 
 	case ETH_SS_TEST:
-		memcpy(buf, bnx2x_tests_str_arr, sizeof(bnx2x_tests_str_arr));
+		/* First 4 tests cannot be done in MF mode */
+		if (!IS_MF(bp))
+			start = 0;
+		else
+			start = 4;
+		for (i = 0, j = start; j < (start + BNX2X_NUM_TESTS(bp));
+		     i++, j++) {
+			offset = sprintf(buf+32*i, "%s",
+					 bnx2x_tests_str_arr[j]);
+			*(buf+offset) = '\0';
+		}
 		break;
 	}
 }
@@ -2291,7 +2583,7 @@ static void bnx2x_get_ethtool_stats(struct net_device *dev,
 
 	if (is_multi(bp)) {
 		for_each_eth_queue(bp, i) {
-			hw_stats = (u32 *)&bp->fp[i].eth_q_stats;
+			hw_stats = (u32 *)&bp->fp_stats[i].eth_q_stats;
 			for (j = 0; j < BNX2X_NUM_Q_STATS; j++) {
 				if (bnx2x_q_stats_arr[j].size == 0) {
 					/* skip this counter */
@@ -2375,6 +2667,41 @@ static int bnx2x_set_phys_id(struct net_device *dev,
 	return 0;
 }
 
+static int bnx2x_get_rss_flags(struct bnx2x *bp, struct ethtool_rxnfc *info)
+{
+
+	switch (info->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+		info->data = RXH_IP_SRC | RXH_IP_DST |
+			     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		break;
+	case UDP_V4_FLOW:
+		if (bp->rss_conf_obj.udp_rss_v4)
+			info->data = RXH_IP_SRC | RXH_IP_DST |
+				     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		else
+			info->data = RXH_IP_SRC | RXH_IP_DST;
+		break;
+	case UDP_V6_FLOW:
+		if (bp->rss_conf_obj.udp_rss_v6)
+			info->data = RXH_IP_SRC | RXH_IP_DST |
+				     RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		else
+			info->data = RXH_IP_SRC | RXH_IP_DST;
+		break;
+	case IPV4_FLOW:
+	case IPV6_FLOW:
+		info->data = RXH_IP_SRC | RXH_IP_DST;
+		break;
+	default:
+		info->data = 0;
+		break;
+	}
+
+	return 0;
+}
+
 static int bnx2x_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
 			   u32 *rules __always_unused)
 {
@@ -2384,7 +2711,102 @@ static int bnx2x_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
 	case ETHTOOL_GRXRINGS:
 		info->data = BNX2X_NUM_ETH_QUEUES(bp);
 		return 0;
+	case ETHTOOL_GRXFH:
+		return bnx2x_get_rss_flags(bp, info);
+	default:
+		DP(BNX2X_MSG_ETHTOOL, "Command parameters not supported\n");
+		return -EOPNOTSUPP;
+	}
+}
+
+static int bnx2x_set_rss_flags(struct bnx2x *bp, struct ethtool_rxnfc *info)
+{
+	int udp_rss_requested;
+
+	DP(BNX2X_MSG_ETHTOOL,
+	   "Set rss flags command parameters: flow type = %d, data = %llu\n",
+	   info->flow_type, info->data);
+
+	switch (info->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+		/* For TCP only 4-tupple hash is supported */
+		if (info->data ^ (RXH_IP_SRC | RXH_IP_DST |
+				  RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Command parameters not supported\n");
+			return -EINVAL;
+		} else {
+			return 0;
+		}
 
+	case UDP_V4_FLOW:
+	case UDP_V6_FLOW:
+		/* For UDP either 2-tupple hash or 4-tupple hash is supported */
+		if (info->data == (RXH_IP_SRC | RXH_IP_DST |
+				 RXH_L4_B_0_1 | RXH_L4_B_2_3))
+			udp_rss_requested = 1;
+		else if (info->data == (RXH_IP_SRC | RXH_IP_DST))
+			udp_rss_requested = 0;
+		else
+			return -EINVAL;
+		if ((info->flow_type == UDP_V4_FLOW) &&
+		    (bp->rss_conf_obj.udp_rss_v4 != udp_rss_requested)) {
+			bp->rss_conf_obj.udp_rss_v4 = udp_rss_requested;
+			DP(BNX2X_MSG_ETHTOOL,
+			   "rss re-configured, UDP 4-tupple %s\n",
+			   udp_rss_requested ? "enabled" : "disabled");
+			return bnx2x_config_rss_pf(bp, &bp->rss_conf_obj, 0);
+		} else if ((info->flow_type == UDP_V6_FLOW) &&
+			   (bp->rss_conf_obj.udp_rss_v6 != udp_rss_requested)) {
+			bp->rss_conf_obj.udp_rss_v6 = udp_rss_requested;
+			return bnx2x_config_rss_pf(bp, &bp->rss_conf_obj, 0);
+			DP(BNX2X_MSG_ETHTOOL,
+			   "rss re-configured, UDP 4-tupple %s\n",
+			   udp_rss_requested ? "enabled" : "disabled");
+		} else {
+			return 0;
+		}
+	case IPV4_FLOW:
+	case IPV6_FLOW:
+		/* For IP only 2-tupple hash is supported */
+		if (info->data ^ (RXH_IP_SRC | RXH_IP_DST)) {
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Command parameters not supported\n");
+			return -EINVAL;
+		} else {
+			return 0;
+		}
+	case SCTP_V4_FLOW:
+	case AH_ESP_V4_FLOW:
+	case AH_V4_FLOW:
+	case ESP_V4_FLOW:
+	case SCTP_V6_FLOW:
+	case AH_ESP_V6_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V6_FLOW:
+	case IP_USER_FLOW:
+	case ETHER_FLOW:
+		/* RSS is not supported for these protocols */
+		if (info->data) {
+			DP(BNX2X_MSG_ETHTOOL,
+			   "Command parameters not supported\n");
+			return -EINVAL;
+		} else {
+			return 0;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bnx2x_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+
+	switch (info->cmd) {
+	case ETHTOOL_SRXFH:
+		return bnx2x_set_rss_flags(bp, info);
 	default:
 		DP(BNX2X_MSG_ETHTOOL, "Command parameters not supported\n");
 		return -EOPNOTSUPP;
@@ -2424,7 +2846,6 @@ static int bnx2x_set_rxfh_indir(struct net_device *dev, const u32 *indir)
 {
 	struct bnx2x *bp = netdev_priv(dev);
 	size_t i;
-	u8 ind_table[T_ETH_INDIRECTION_TABLE_SIZE] = {0};
 
 	for (i = 0; i < T_ETH_INDIRECTION_TABLE_SIZE; i++) {
 		/*
@@ -2436,10 +2857,88 @@ static int bnx2x_set_rxfh_indir(struct net_device *dev, const u32 *indir)
 		 * align the received table to the Client ID of the leading RSS
 		 * queue
 		 */
-		ind_table[i] = indir[i] + bp->fp->cl_id;
+		bp->rss_conf_obj.ind_table[i] = indir[i] + bp->fp->cl_id;
 	}
 
-	return bnx2x_config_rss_eth(bp, ind_table, false);
+	return bnx2x_config_rss_eth(bp, false);
+}
+
+/**
+ * bnx2x_get_channels - gets the number of RSS queues.
+ *
+ * @dev:		net device
+ * @channels:		returns the number of max / current queues
+ */
+static void bnx2x_get_channels(struct net_device *dev,
+			       struct ethtool_channels *channels)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+
+	channels->max_combined = BNX2X_MAX_RSS_COUNT(bp);
+	channels->combined_count = BNX2X_NUM_ETH_QUEUES(bp);
+}
+
+/**
+ * bnx2x_change_num_queues - change the number of RSS queues.
+ *
+ * @bp:			bnx2x private structure
+ *
+ * Re-configure interrupt mode to get the new number of MSI-X
+ * vectors and re-add NAPI objects.
+ */
+static void bnx2x_change_num_queues(struct bnx2x *bp, int num_rss)
+{
+	bnx2x_del_all_napi(bp);
+	bnx2x_disable_msi(bp);
+	BNX2X_NUM_QUEUES(bp) = num_rss + NON_ETH_CONTEXT_USE;
+	bnx2x_set_int_mode(bp);
+	bnx2x_add_all_napi(bp);
+}
+
+/**
+ * bnx2x_set_channels - sets the number of RSS queues.
+ *
+ * @dev:		net device
+ * @channels:		includes the number of queues requested
+ */
+static int bnx2x_set_channels(struct net_device *dev,
+			      struct ethtool_channels *channels)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+
+
+	DP(BNX2X_MSG_ETHTOOL,
+	   "set-channels command parameters: rx = %d, tx = %d, other = %d, combined = %d\n",
+	   channels->rx_count, channels->tx_count, channels->other_count,
+	   channels->combined_count);
+
+	/* We don't support separate rx / tx channels.
+	 * We don't allow setting 'other' channels.
+	 */
+	if (channels->rx_count || channels->tx_count || channels->other_count
+	    || (channels->combined_count == 0) ||
+	    (channels->combined_count > BNX2X_MAX_RSS_COUNT(bp))) {
+		DP(BNX2X_MSG_ETHTOOL, "command parameters not supported\n");
+		return -EINVAL;
+	}
+
+	/* Check if there was a change in the active parameters */
+	if (channels->combined_count == BNX2X_NUM_ETH_QUEUES(bp)) {
+		DP(BNX2X_MSG_ETHTOOL, "No change in active parameters\n");
+		return 0;
+	}
+
+	/* Set the requested number of queues in bp context.
+	 * Note that the actual number of queues created during load may be
+	 * less than requested if memory is low.
+	 */
+	if (unlikely(!netif_running(dev))) {
+		bnx2x_change_num_queues(bp, channels->combined_count);
+		return 0;
+	}
+	bnx2x_nic_unload(bp, UNLOAD_NORMAL);
+	bnx2x_change_num_queues(bp, channels->combined_count);
+	return bnx2x_nic_load(bp, LOAD_NORMAL);
 }
 
 static const struct ethtool_ops bnx2x_ethtool_ops = {
@@ -2469,9 +2968,17 @@ static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.set_phys_id		= bnx2x_set_phys_id,
 	.get_ethtool_stats	= bnx2x_get_ethtool_stats,
 	.get_rxnfc		= bnx2x_get_rxnfc,
+	.set_rxnfc		= bnx2x_set_rxnfc,
 	.get_rxfh_indir_size	= bnx2x_get_rxfh_indir_size,
 	.get_rxfh_indir		= bnx2x_get_rxfh_indir,
 	.set_rxfh_indir		= bnx2x_set_rxfh_indir,
+	.get_channels		= bnx2x_get_channels,
+	.set_channels		= bnx2x_set_channels,
+	.get_module_info	= bnx2x_get_module_info,
+	.get_module_eeprom	= bnx2x_get_module_eeprom,
+	.get_eee		= bnx2x_get_eee,
+	.set_eee		= bnx2x_set_eee,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 void bnx2x_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
index 426f77aa721..bbc66ced9c2 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
@@ -321,9 +321,7 @@
 #define DISABLE_STATISTIC_COUNTER_ID_VALUE 0
 
 
-/**
- * This file defines HSI constants common to all microcode flows
- */
+/* This file defines HSI constants common to all microcode flows */
 
 #define PROTOCOL_STATE_BIT_OFFSET 6
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
index a440a8ba85f..76b6e65790f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
@@ -10,6 +10,7 @@
 #define BNX2X_HSI_H
 
 #include "bnx2x_fw_defs.h"
+#include "bnx2x_mfw_req.h"
 
 #define FW_ENCODE_32BIT_PATTERN         0x1e1e1e1e
 
@@ -33,12 +34,6 @@ struct license_key {
 	u32 reserved_b[4];
 };
 
-
-#define PORT_0			0
-#define PORT_1			1
-#define PORT_MAX		2
-#define NVM_PATH_MAX		2
-
 /****************************************************************************
  * Shared HW configuration                                                  *
  ****************************************************************************/
@@ -1067,8 +1062,18 @@ struct port_feat_cfg {		    /* port 0: 0x454  port 1: 0x4c8 */
 	   uses the same defines as link_config */
 	u32 mfw_wol_link_cfg2;				    /* 0x480 */
 
-	u32 Reserved2[17];				    /* 0x484 */
 
+	/*  EEE power saving mode */
+	u32 eee_power_mode;                                 /* 0x484 */
+	#define PORT_FEAT_CFG_EEE_POWER_MODE_MASK                     0x000000FF
+	#define PORT_FEAT_CFG_EEE_POWER_MODE_SHIFT                    0
+	#define PORT_FEAT_CFG_EEE_POWER_MODE_DISABLED                 0x00000000
+	#define PORT_FEAT_CFG_EEE_POWER_MODE_BALANCED                 0x00000001
+	#define PORT_FEAT_CFG_EEE_POWER_MODE_AGGRESSIVE               0x00000002
+	#define PORT_FEAT_CFG_EEE_POWER_MODE_LOW_LATENCY              0x00000003
+
+
+	u32 Reserved2[16];                                  /* 0x488 */
 };
 
 
@@ -1140,6 +1145,7 @@ struct drv_port_mb {
 	u32 link_status;
 	/* Driver should update this field on any link change event */
 
+	#define LINK_STATUS_NONE				(0<<0)
 	#define LINK_STATUS_LINK_FLAG_MASK			0x00000001
 	#define LINK_STATUS_LINK_UP				0x00000001
 	#define LINK_STATUS_SPEED_AND_DUPLEX_MASK		0x0000001E
@@ -1197,6 +1203,7 @@ struct drv_port_mb {
 	#define LINK_STATUS_PFC_ENABLED				0x20000000
 
 	#define LINK_STATUS_PHYSICAL_LINK_FLAG			0x40000000
+	#define LINK_STATUS_SFP_TX_FAULT			0x80000000
 
 	u32 port_stx;
 
@@ -1240,9 +1247,11 @@ struct drv_func_mb {
 	#define REQ_BC_VER_4_VRFY_AFEX_SUPPORTED        0x00070002
 	#define REQ_BC_VER_4_SFP_TX_DISABLE_SUPPORTED   0x00070014
 	#define REQ_BC_VER_4_PFC_STATS_SUPPORTED        0x00070201
+	#define REQ_BC_VER_4_FCOE_FEATURES              0x00070209
 
 	#define DRV_MSG_CODE_DCBX_ADMIN_PMF_MSG         0xb0000000
 	#define DRV_MSG_CODE_DCBX_PMF_DRV_OK            0xb2000000
+	#define REQ_BC_VER_4_DCBX_ADMIN_MSG_NON_PMF     0x00070401
 
 	#define DRV_MSG_CODE_VF_DISABLED_DONE           0xc0000000
 
@@ -1255,6 +1264,8 @@ struct drv_func_mb {
 	#define DRV_MSG_CODE_DRV_INFO_ACK               0xd8000000
 	#define DRV_MSG_CODE_DRV_INFO_NACK              0xd9000000
 
+	#define DRV_MSG_CODE_EEE_RESULTS_ACK            0xda000000
+
 	#define DRV_MSG_CODE_SET_MF_BW                  0xe0000000
 	#define REQ_BC_VER_4_SET_MF_BW                  0x00060202
 	#define DRV_MSG_CODE_SET_MF_BW_ACK              0xe1000000
@@ -1320,6 +1331,8 @@ struct drv_func_mb {
 	#define FW_MSG_CODE_DRV_INFO_ACK                0xd8100000
 	#define FW_MSG_CODE_DRV_INFO_NACK               0xd9100000
 
+	#define FW_MSG_CODE_EEE_RESULS_ACK              0xda100000
+
 	#define FW_MSG_CODE_SET_MF_BW_SENT              0xe0000000
 	#define FW_MSG_CODE_SET_MF_BW_DONE              0xe1000000
 
@@ -1383,6 +1396,8 @@ struct drv_func_mb {
 
 	#define DRV_STATUS_DRV_INFO_REQ                 0x04000000
 
+	#define DRV_STATUS_EEE_NEGOTIATION_RESULTS      0x08000000
+
 	u32 virt_mac_upper;
 	#define VIRT_MAC_SIGN_MASK                      0xffff0000
 	#define VIRT_MAC_SIGNATURE                      0x564d0000
@@ -1613,6 +1628,11 @@ struct fw_flr_mb {
 	struct fw_flr_ack ack;
 };
 
+struct eee_remote_vals {
+	u32         tx_tw;
+	u32         rx_tw;
+};
+
 /**** SUPPORT FOR SHMEM ARRRAYS ***
  * The SHMEM HSI is aligned on 32 bit boundaries which makes it difficult to
  * define arrays with storage types smaller then unsigned dwords.
@@ -2053,6 +2073,41 @@ struct shmem2_region {
 #define DRV_INFO_CONTROL_OP_CODE_MASK      0x0000ff00
 #define DRV_INFO_CONTROL_OP_CODE_SHIFT     8
 	u32 ibft_host_addr; /* initialized by option ROM */
+	struct eee_remote_vals eee_remote_vals[PORT_MAX];
+	u32 reserved[E2_FUNC_MAX];
+
+
+	/* the status of EEE auto-negotiation
+	 * bits 15:0 the configured tx-lpi entry timer value. Depends on bit 31.
+	 * bits 19:16 the supported modes for EEE.
+	 * bits 23:20 the speeds advertised for EEE.
+	 * bits 27:24 the speeds the Link partner advertised for EEE.
+	 * The supported/adv. modes in bits 27:19 originate from the
+	 * SHMEM_EEE_XXX_ADV definitions (where XXX is replaced by speed).
+	 * bit 28 when 1'b1 EEE was requested.
+	 * bit 29 when 1'b1 tx lpi was requested.
+	 * bit 30 when 1'b1 EEE was negotiated. Tx lpi will be asserted iff
+	 * 30:29 are 2'b11.
+	 * bit 31 when 1'b0 bits 15:0 contain a PORT_FEAT_CFG_EEE_ define as
+	 * value. When 1'b1 those bits contains a value times 16 microseconds.
+	 */
+	u32 eee_status[PORT_MAX];
+	#define SHMEM_EEE_TIMER_MASK		   0x0000ffff
+	#define SHMEM_EEE_SUPPORTED_MASK	   0x000f0000
+	#define SHMEM_EEE_SUPPORTED_SHIFT	   16
+	#define SHMEM_EEE_ADV_STATUS_MASK	   0x00f00000
+		#define SHMEM_EEE_100M_ADV	   (1<<0)
+		#define SHMEM_EEE_1G_ADV	   (1<<1)
+		#define SHMEM_EEE_10G_ADV	   (1<<2)
+	#define SHMEM_EEE_ADV_STATUS_SHIFT	   20
+	#define	SHMEM_EEE_LP_ADV_STATUS_MASK	   0x0f000000
+	#define SHMEM_EEE_LP_ADV_STATUS_SHIFT	   24
+	#define SHMEM_EEE_REQUESTED_BIT		   0x10000000
+	#define SHMEM_EEE_LPI_REQUESTED_BIT	   0x20000000
+	#define SHMEM_EEE_ACTIVE_BIT		   0x40000000
+	#define SHMEM_EEE_TIME_OUTPUT_BIT	   0x80000000
+
+	u32 sizeof_port_stats;
 };
 
 
@@ -2599,6 +2654,9 @@ struct host_port_stats {
 	u32            pfc_frames_tx_lo;
 	u32            pfc_frames_rx_hi;
 	u32            pfc_frames_rx_lo;
+
+	u32            eee_lpi_count_hi;
+	u32            eee_lpi_count_lo;
 };
 
 
@@ -2638,118 +2696,6 @@ struct host_func_stats {
 /* VIC definitions */
 #define VICSTATST_UIF_INDEX 2
 
-/* current drv_info version */
-#define DRV_INFO_CUR_VER 1
-
-/* drv_info op codes supported */
-enum drv_info_opcode {
-	ETH_STATS_OPCODE,
-	FCOE_STATS_OPCODE,
-	ISCSI_STATS_OPCODE
-};
-
-#define ETH_STAT_INFO_VERSION_LEN	12
-/*  Per PCI Function Ethernet Statistics required from the driver */
-struct eth_stats_info {
-	/* Function's Driver Version. padded to 12 */
-	u8 version[ETH_STAT_INFO_VERSION_LEN];
-	/* Locally Admin Addr. BigEndian EIU48. Actual size is 6 bytes */
-	u8 mac_local[8];
-	u8 mac_add1[8];		/* Additional Programmed MAC Addr 1. */
-	u8 mac_add2[8];		/* Additional Programmed MAC Addr 2. */
-	u32 mtu_size;		/* MTU Size. Note   : Negotiated MTU */
-	u32 feature_flags;	/* Feature_Flags. */
-#define FEATURE_ETH_CHKSUM_OFFLOAD_MASK		0x01
-#define FEATURE_ETH_LSO_MASK			0x02
-#define FEATURE_ETH_BOOTMODE_MASK		0x1C
-#define FEATURE_ETH_BOOTMODE_SHIFT		2
-#define FEATURE_ETH_BOOTMODE_NONE		(0x0 << 2)
-#define FEATURE_ETH_BOOTMODE_PXE		(0x1 << 2)
-#define FEATURE_ETH_BOOTMODE_ISCSI		(0x2 << 2)
-#define FEATURE_ETH_BOOTMODE_FCOE		(0x3 << 2)
-#define FEATURE_ETH_TOE_MASK			0x20
-	u32 lso_max_size;	/* LSO MaxOffloadSize. */
-	u32 lso_min_seg_cnt;	/* LSO MinSegmentCount. */
-	/* Num Offloaded Connections TCP_IPv4. */
-	u32 ipv4_ofld_cnt;
-	/* Num Offloaded Connections TCP_IPv6. */
-	u32 ipv6_ofld_cnt;
-	u32 promiscuous_mode;	/* Promiscuous Mode. non-zero true */
-	u32 txq_size;		/* TX Descriptors Queue Size */
-	u32 rxq_size;		/* RX Descriptors Queue Size */
-	/* TX Descriptor Queue Avg Depth. % Avg Queue Depth since last poll */
-	u32 txq_avg_depth;
-	/* RX Descriptors Queue Avg Depth. % Avg Queue Depth since last poll */
-	u32 rxq_avg_depth;
-	/* IOV_Offload. 0=none; 1=MultiQueue, 2=VEB 3= VEPA*/
-	u32 iov_offload;
-	/* Number of NetQueue/VMQ Config'd. */
-	u32 netq_cnt;
-	u32 vf_cnt;		/* Num VF assigned to this PF. */
-};
-
-/*  Per PCI Function FCOE Statistics required from the driver */
-struct fcoe_stats_info {
-	u8 version[12];		/* Function's Driver Version. */
-	u8 mac_local[8];	/* Locally Admin Addr. */
-	u8 mac_add1[8];		/* Additional Programmed MAC Addr 1. */
-	u8 mac_add2[8];		/* Additional Programmed MAC Addr 2. */
-	/* QoS Priority (per 802.1p). 0-7255 */
-	u32 qos_priority;
-	u32 txq_size;		/* FCoE TX Descriptors Queue Size. */
-	u32 rxq_size;		/* FCoE RX Descriptors Queue Size. */
-	/* FCoE TX Descriptor Queue Avg Depth. */
-	u32 txq_avg_depth;
-	/* FCoE RX Descriptors Queue Avg Depth. */
-	u32 rxq_avg_depth;
-	u32 rx_frames_lo;	/* FCoE RX Frames received. */
-	u32 rx_frames_hi;	/* FCoE RX Frames received. */
-	u32 rx_bytes_lo;	/* FCoE RX Bytes received. */
-	u32 rx_bytes_hi;	/* FCoE RX Bytes received. */
-	u32 tx_frames_lo;	/* FCoE TX Frames sent. */
-	u32 tx_frames_hi;	/* FCoE TX Frames sent. */
-	u32 tx_bytes_lo;	/* FCoE TX Bytes sent. */
-	u32 tx_bytes_hi;	/* FCoE TX Bytes sent. */
-};
-
-/* Per PCI  Function iSCSI Statistics required from the driver*/
-struct iscsi_stats_info {
-	u8 version[12];		/* Function's Driver Version. */
-	u8 mac_local[8];	/* Locally Admin iSCSI MAC Addr. */
-	u8 mac_add1[8];		/* Additional Programmed MAC Addr 1. */
-	/* QoS Priority (per 802.1p). 0-7255 */
-	u32 qos_priority;
-	u8 initiator_name[64];	/* iSCSI Boot Initiator Node name. */
-	u8 ww_port_name[64];	/* iSCSI World wide port name */
-	u8 boot_target_name[64];/* iSCSI Boot Target Name. */
-	u8 boot_target_ip[16];	/* iSCSI Boot Target IP. */
-	u32 boot_target_portal;	/* iSCSI Boot Target Portal. */
-	u8 boot_init_ip[16];	/* iSCSI Boot Initiator IP Address. */
-	u32 max_frame_size;	/* Max Frame Size. bytes */
-	u32 txq_size;		/* PDU TX Descriptors Queue Size. */
-	u32 rxq_size;		/* PDU RX Descriptors Queue Size. */
-	u32 txq_avg_depth;	/* PDU TX Descriptor Queue Avg Depth. */
-	u32 rxq_avg_depth;	/* PDU RX Descriptors Queue Avg Depth. */
-	u32 rx_pdus_lo;		/* iSCSI PDUs received. */
-	u32 rx_pdus_hi;		/* iSCSI PDUs received. */
-	u32 rx_bytes_lo;	/* iSCSI RX Bytes received. */
-	u32 rx_bytes_hi;	/* iSCSI RX Bytes received. */
-	u32 tx_pdus_lo;		/* iSCSI PDUs sent. */
-	u32 tx_pdus_hi;		/* iSCSI PDUs sent. */
-	u32 tx_bytes_lo;	/* iSCSI PDU TX Bytes sent. */
-	u32 tx_bytes_hi;	/* iSCSI PDU TX Bytes sent. */
-	u32 pcp_prior_map_tbl;	/* C-PCP to S-PCP Priority MapTable.
-				 * 9 nibbles, the position of each nibble
-				 * represents the C-PCP value, the value
-				 * of the nibble = S-PCP value.
-				 */
-};
-
-union drv_info_to_mcp {
-	struct eth_stats_info	ether_stat;
-	struct fcoe_stats_info	fcoe_stat;
-	struct iscsi_stats_info	iscsi_stat;
-};
 
 /* stats collected for afex.
  * NOTE: structure is exactly as expected to be received by the switch.
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 6e7d5c0843b..f4beb46c470 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -285,7 +285,6 @@
 #define ETS_E3B0_PBF_MIN_W_VAL				(10000)
 
 #define MAX_PACKET_SIZE					(9700)
-#define WC_UC_TIMEOUT					100
 #define MAX_KR_LINK_RETRY				4
 
 /**********************************************************/
@@ -1306,6 +1305,94 @@ int bnx2x_ets_strict(const struct link_params *params, const u8 strict_cos)
 
 	return 0;
 }
+
+/******************************************************************/
+/*			EEE section				   */
+/******************************************************************/
+static u8 bnx2x_eee_has_cap(struct link_params *params)
+{
+	struct bnx2x *bp = params->bp;
+
+	if (REG_RD(bp, params->shmem2_base) <=
+		   offsetof(struct shmem2_region, eee_status[params->port]))
+		return 0;
+
+	return 1;
+}
+
+static int bnx2x_eee_nvram_to_time(u32 nvram_mode, u32 *idle_timer)
+{
+	switch (nvram_mode) {
+	case PORT_FEAT_CFG_EEE_POWER_MODE_BALANCED:
+		*idle_timer = EEE_MODE_NVRAM_BALANCED_TIME;
+		break;
+	case PORT_FEAT_CFG_EEE_POWER_MODE_AGGRESSIVE:
+		*idle_timer = EEE_MODE_NVRAM_AGGRESSIVE_TIME;
+		break;
+	case PORT_FEAT_CFG_EEE_POWER_MODE_LOW_LATENCY:
+		*idle_timer = EEE_MODE_NVRAM_LATENCY_TIME;
+		break;
+	default:
+		*idle_timer = 0;
+		break;
+	}
+
+	return 0;
+}
+
+static int bnx2x_eee_time_to_nvram(u32 idle_timer, u32 *nvram_mode)
+{
+	switch (idle_timer) {
+	case EEE_MODE_NVRAM_BALANCED_TIME:
+		*nvram_mode = PORT_FEAT_CFG_EEE_POWER_MODE_BALANCED;
+		break;
+	case EEE_MODE_NVRAM_AGGRESSIVE_TIME:
+		*nvram_mode = PORT_FEAT_CFG_EEE_POWER_MODE_AGGRESSIVE;
+		break;
+	case EEE_MODE_NVRAM_LATENCY_TIME:
+		*nvram_mode = PORT_FEAT_CFG_EEE_POWER_MODE_LOW_LATENCY;
+		break;
+	default:
+		*nvram_mode = PORT_FEAT_CFG_EEE_POWER_MODE_DISABLED;
+		break;
+	}
+
+	return 0;
+}
+
+static u32 bnx2x_eee_calc_timer(struct link_params *params)
+{
+	u32 eee_mode, eee_idle;
+	struct bnx2x *bp = params->bp;
+
+	if (params->eee_mode & EEE_MODE_OVERRIDE_NVRAM) {
+		if (params->eee_mode & EEE_MODE_OUTPUT_TIME) {
+			/* time value in eee_mode --> used directly*/
+			eee_idle = params->eee_mode & EEE_MODE_TIMER_MASK;
+		} else {
+			/* hsi value in eee_mode --> time */
+			if (bnx2x_eee_nvram_to_time(params->eee_mode &
+						    EEE_MODE_NVRAM_MASK,
+						    &eee_idle))
+				return 0;
+		}
+	} else {
+		/* hsi values in nvram --> time*/
+		eee_mode = ((REG_RD(bp, params->shmem_base +
+				    offsetof(struct shmem_region, dev_info.
+				    port_feature_config[params->port].
+				    eee_power_mode)) &
+			     PORT_FEAT_CFG_EEE_POWER_MODE_MASK) >>
+			    PORT_FEAT_CFG_EEE_POWER_MODE_SHIFT);
+
+		if (bnx2x_eee_nvram_to_time(eee_mode, &eee_idle))
+			return 0;
+	}
+
+	return eee_idle;
+}
+
+
 /******************************************************************/
 /*			PFC section				  */
 /******************************************************************/
@@ -1540,7 +1627,7 @@ static void bnx2x_umac_enable(struct link_params *params,
 	/* Reset UMAC */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
 	       (MISC_REGISTERS_RESET_REG_2_UMAC0 << params->port));
-	usleep_range(1000, 1000);
+	usleep_range(1000, 2000);
 
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
 	       (MISC_REGISTERS_RESET_REG_2_UMAC0 << params->port));
@@ -1631,7 +1718,7 @@ static void bnx2x_xmac_init(struct link_params *params, u32 max_speed)
 	 * ports of the path
 	 */
 
-	if ((CHIP_NUM(bp) == CHIP_NUM_57840) &&
+	if ((CHIP_NUM(bp) == CHIP_NUM_57840_4_10) &&
 	    (REG_RD(bp, MISC_REG_RESET_REG_2) &
 	     MISC_REGISTERS_RESET_REG_2_XMAC)) {
 		DP(NETIF_MSG_LINK,
@@ -1642,7 +1729,7 @@ static void bnx2x_xmac_init(struct link_params *params, u32 max_speed)
 	/* Hard reset */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
 	       MISC_REGISTERS_RESET_REG_2_XMAC);
-	usleep_range(1000, 1000);
+	usleep_range(1000, 2000);
 
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
 	       MISC_REGISTERS_RESET_REG_2_XMAC);
@@ -1672,7 +1759,7 @@ static void bnx2x_xmac_init(struct link_params *params, u32 max_speed)
 	/* Soft reset */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
 	       MISC_REGISTERS_RESET_REG_2_XMAC_SOFT);
-	usleep_range(1000, 1000);
+	usleep_range(1000, 2000);
 
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
 	       MISC_REGISTERS_RESET_REG_2_XMAC_SOFT);
@@ -1730,6 +1817,14 @@ static int bnx2x_xmac_enable(struct link_params *params,
 	/* update PFC */
 	bnx2x_update_pfc_xmac(params, vars, 0);
 
+	if (vars->eee_status & SHMEM_EEE_ADV_STATUS_MASK) {
+		DP(NETIF_MSG_LINK, "Setting XMAC for EEE\n");
+		REG_WR(bp, xmac_base + XMAC_REG_EEE_TIMERS_HI, 0x1380008);
+		REG_WR(bp, xmac_base + XMAC_REG_EEE_CTRL, 0x1);
+	} else {
+		REG_WR(bp, xmac_base + XMAC_REG_EEE_CTRL, 0x0);
+	}
+
 	/* Enable TX and RX */
 	val = XMAC_CTRL_REG_TX_EN | XMAC_CTRL_REG_RX_EN;
 
@@ -1785,11 +1880,6 @@ static int bnx2x_emac_enable(struct link_params *params,
 	bnx2x_bits_en(bp, emac_base + EMAC_REG_EMAC_TX_MODE,
 		      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));
-	} else { /* ASIC */
 		/* pause enable/disable */
 		bnx2x_bits_dis(bp, emac_base + EMAC_REG_EMAC_RX_MODE,
 			       EMAC_RX_MODE_FLOW_EN);
@@ -1812,7 +1902,6 @@ static int bnx2x_emac_enable(struct link_params *params,
 		} else
 			bnx2x_bits_en(bp, emac_base + EMAC_REG_EMAC_TX_MODE,
 				      EMAC_TX_MODE_FLOW_EN);
-	}
 
 	/* KEEP_VLAN_TAG, promiscuous */
 	val = REG_RD(bp, emac_base + EMAC_REG_EMAC_RX_MODE);
@@ -1851,23 +1940,23 @@ static int bnx2x_emac_enable(struct link_params *params,
 		val &= ~0x810;
 	EMAC_WR(bp, EMAC_REG_EMAC_MODE, val);
 
-	/* enable emac */
+	/* Enable emac */
 	REG_WR(bp, NIG_REG_NIG_EMAC0_EN + port*4, 1);
 
-	/* enable emac for jumbo packets */
+	/* Enable emac for jumbo packets */
 	EMAC_WR(bp, EMAC_REG_EMAC_RX_MTU_SIZE,
 		(EMAC_RX_MTU_SIZE_JUMBO_ENA |
 		 (ETH_MAX_JUMBO_PACKET_SIZE + ETH_OVREHEAD)));
 
-	/* strip CRC */
+	/* Strip CRC */
 	REG_WR(bp, NIG_REG_NIG_INGRESS_EMAC0_NO_CRC + port*4, 0x1);
 
-	/* disable the NIG in/out to the bmac */
+	/* Disable the NIG in/out to the bmac */
 	REG_WR(bp, NIG_REG_BMAC0_IN_EN + port*4, 0x0);
 	REG_WR(bp, NIG_REG_BMAC0_PAUSE_OUT_EN + port*4, 0x0);
 	REG_WR(bp, NIG_REG_BMAC0_OUT_EN + port*4, 0x0);
 
-	/* enable the NIG in/out to the emac */
+	/* Enable the NIG in/out to the emac */
 	REG_WR(bp, NIG_REG_EMAC0_IN_EN + port*4, 0x1);
 	val = 0;
 	if ((params->feature_config_flags &
@@ -1902,7 +1991,7 @@ static void bnx2x_update_pfc_bmac1(struct link_params *params,
 	wb_data[1] = 0;
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_RX_CONTROL, wb_data, 2);
 
-	/* tx control */
+	/* TX control */
 	val = 0xc0;
 	if (!(params->feature_config_flags &
 	      FEATURE_CONFIG_PFC_ENABLED) &&
@@ -1962,7 +2051,7 @@ static void bnx2x_update_pfc_bmac2(struct link_params *params,
 		wb_data[0] &= ~(1<<2);
 	} else {
 		DP(NETIF_MSG_LINK, "PFC is disabled\n");
-		/* disable PFC RX & TX & STATS and set 8 COS */
+		/* Disable PFC RX & TX & STATS and set 8 COS */
 		wb_data[0] = 0x8;
 		wb_data[1] = 0;
 	}
@@ -2056,7 +2145,7 @@ static int bnx2x_pfc_brb_get_config_params(
 			PFC_E2_BRB_MAC_FULL_XOFF_THR_PAUSE;
 		config_val->pauseable_th.full_xon =
 			PFC_E2_BRB_MAC_FULL_XON_THR_PAUSE;
-		/* non pause able*/
+		/* Non pause able*/
 		config_val->non_pauseable_th.pause_xoff =
 			PFC_E2_BRB_MAC_PAUSE_XOFF_THR_NON_PAUSE;
 		config_val->non_pauseable_th.pause_xon =
@@ -2084,7 +2173,7 @@ static int bnx2x_pfc_brb_get_config_params(
 			PFC_E3A0_BRB_MAC_FULL_XOFF_THR_PAUSE;
 		config_val->pauseable_th.full_xon =
 			PFC_E3A0_BRB_MAC_FULL_XON_THR_PAUSE;
-		/* non pause able*/
+		/* Non pause able*/
 		config_val->non_pauseable_th.pause_xoff =
 			PFC_E3A0_BRB_MAC_PAUSE_XOFF_THR_NON_PAUSE;
 		config_val->non_pauseable_th.pause_xon =
@@ -2114,7 +2203,7 @@ static int bnx2x_pfc_brb_get_config_params(
 				PFC_E3B0_4P_BRB_MAC_FULL_XOFF_THR_PAUSE;
 			config_val->pauseable_th.full_xon =
 				PFC_E3B0_4P_BRB_MAC_FULL_XON_THR_PAUSE;
-			/* non pause able*/
+			/* Non pause able*/
 			config_val->non_pauseable_th.pause_xoff =
 			PFC_E3B0_4P_BRB_MAC_PAUSE_XOFF_THR_NON_PAUSE;
 			config_val->non_pauseable_th.pause_xon =
@@ -2132,7 +2221,7 @@ static int bnx2x_pfc_brb_get_config_params(
 				PFC_E3B0_2P_BRB_MAC_FULL_XOFF_THR_PAUSE;
 			config_val->pauseable_th.full_xon =
 				PFC_E3B0_2P_BRB_MAC_FULL_XON_THR_PAUSE;
-			/* non pause able*/
+			/* Non pause able*/
 			config_val->non_pauseable_th.pause_xoff =
 				PFC_E3B0_2P_BRB_MAC_PAUSE_XOFF_THR_NON_PAUSE;
 			config_val->non_pauseable_th.pause_xon =
@@ -2189,7 +2278,7 @@ static void bnx2x_pfc_brb_get_e3b0_config_params(
 
 			if (pfc_params->cos0_pauseable !=
 				pfc_params->cos1_pauseable) {
-				/* nonpauseable= Lossy + pauseable = Lossless*/
+				/* Nonpauseable= Lossy + pauseable = Lossless*/
 				e3b0_val->lb_guarantied =
 					PFC_E3B0_2P_MIX_PAUSE_LB_GUART;
 				e3b0_val->mac_0_class_t_guarantied =
@@ -2388,9 +2477,9 @@ static int bnx2x_update_pfc_brb(struct link_params *params,
 *  This function is needed because NIG ARB_CREDIT_WEIGHT_X are
 *  not continues and ARB_CREDIT_WEIGHT_0 + offset is suitable.
 ******************************************************************************/
-int bnx2x_pfc_nig_rx_priority_mask(struct bnx2x *bp,
-					      u8 cos_entry,
-					      u32 priority_mask, u8 port)
+static int bnx2x_pfc_nig_rx_priority_mask(struct bnx2x *bp,
+					   u8 cos_entry,
+					   u32 priority_mask, u8 port)
 {
 	u32 nig_reg_rx_priority_mask_add = 0;
 
@@ -2440,6 +2529,16 @@ static void bnx2x_update_mng(struct link_params *params, u32 link_status)
 			port_mb[params->port].link_status), link_status);
 }
 
+static void bnx2x_update_mng_eee(struct link_params *params, u32 eee_status)
+{
+	struct bnx2x *bp = params->bp;
+
+	if (bnx2x_eee_has_cap(params))
+		REG_WR(bp, params->shmem2_base +
+		       offsetof(struct shmem2_region,
+				eee_status[params->port]), eee_status);
+}
+
 static void bnx2x_update_pfc_nig(struct link_params *params,
 		struct link_vars *vars,
 		struct bnx2x_nig_brb_pfc_port_params *nig_params)
@@ -2507,7 +2606,7 @@ static void bnx2x_update_pfc_nig(struct link_params *params,
 	REG_WR(bp, port ? NIG_REG_LLFC_EGRESS_SRC_ENABLE_1 :
 	       NIG_REG_LLFC_EGRESS_SRC_ENABLE_0, 0x7);
 
-	/* output enable for RX_XCM # IF */
+	/* Output enable for RX_XCM # IF */
 	REG_WR(bp, port ? NIG_REG_XCM1_OUT_EN :
 	       NIG_REG_XCM0_OUT_EN, xcm_out_en);
 
@@ -2556,10 +2655,10 @@ int bnx2x_update_pfc(struct link_params *params,
 
 	bnx2x_update_mng(params, vars->link_status);
 
-	/* update NIG params */
+	/* Update NIG params */
 	bnx2x_update_pfc_nig(params, vars, pfc_params);
 
-	/* update BRB params */
+	/* Update BRB params */
 	bnx2x_status = bnx2x_update_pfc_brb(params, vars, pfc_params);
 	if (bnx2x_status)
 		return bnx2x_status;
@@ -2614,7 +2713,7 @@ static int bnx2x_bmac1_enable(struct link_params *params,
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_BMAC_XGXS_CONTROL,
 		    wb_data, 2);
 
-	/* tx MAC SA */
+	/* TX MAC SA */
 	wb_data[0] = ((params->mac_addr[2] << 24) |
 		       (params->mac_addr[3] << 16) |
 		       (params->mac_addr[4] << 8) |
@@ -2623,7 +2722,7 @@ static int bnx2x_bmac1_enable(struct link_params *params,
 			params->mac_addr[1]);
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_TX_SOURCE_ADDR, wb_data, 2);
 
-	/* mac control */
+	/* MAC control */
 	val = 0x3;
 	if (is_lb) {
 		val |= 0x4;
@@ -2633,24 +2732,24 @@ static int bnx2x_bmac1_enable(struct link_params *params,
 	wb_data[1] = 0;
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_BMAC_CONTROL, wb_data, 2);
 
-	/* set rx mtu */
+	/* 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);
 
 	bnx2x_update_pfc_bmac1(params, vars);
 
-	/* set tx mtu */
+	/* 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);
 
-	/* set cnt max size */
+	/* 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);
 
-	/* configure safc */
+	/* Configure SAFC */
 	wb_data[0] = 0x1000200;
 	wb_data[1] = 0;
 	REG_WR_DMAE(bp, bmac_addr + BIGMAC_REGISTER_RX_LLFC_MSG_FLDS,
@@ -2684,7 +2783,7 @@ static int bnx2x_bmac2_enable(struct link_params *params,
 
 	udelay(30);
 
-	/* tx MAC SA */
+	/* TX MAC SA */
 	wb_data[0] = ((params->mac_addr[2] << 24) |
 		       (params->mac_addr[3] << 16) |
 		       (params->mac_addr[4] << 8) |
@@ -2703,18 +2802,18 @@ static int bnx2x_bmac2_enable(struct link_params *params,
 		    wb_data, 2);
 	udelay(30);
 
-	/* set rx mtu */
+	/* 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);
 	udelay(30);
 
-	/* set tx mtu */
+	/* 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);
 	udelay(30);
-	/* set cnt max size */
+	/* 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);
@@ -2732,15 +2831,15 @@ static int bnx2x_bmac_enable(struct link_params *params,
 	u8 port = params->port;
 	struct bnx2x *bp = params->bp;
 	u32 val;
-	/* reset and unreset the BigMac */
+	/* Reset and unreset the BigMac */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
 	       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
-	msleep(1);
+	usleep_range(1000, 2000);
 
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_SET,
 	       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
 
-	/* enable access for bmac registers */
+	/* Enable access for bmac registers */
 	REG_WR(bp, NIG_REG_BMAC0_REGS_OUT_EN + port*4, 0x1);
 
 	/* Enable BMAC according to BMAC type*/
@@ -2798,7 +2897,7 @@ static void bnx2x_bmac_rx_disable(struct bnx2x *bp, u8 port)
 					BIGMAC_REGISTER_BMAC_CONTROL,
 					wb_data, 2);
 		}
-		msleep(1);
+		usleep_range(1000, 2000);
 	}
 }
 
@@ -2810,17 +2909,16 @@ static int bnx2x_pbf_update(struct link_params *params, u32 flow_ctrl,
 	u32 init_crd, crd;
 	u32 count = 1000;
 
-	/* disable port */
+	/* Disable port */
 	REG_WR(bp, PBF_REG_DISABLE_NEW_TASK_PROC_P0 + port*4, 0x1);
 
-	/* wait for init credit */
+	/* Wait for init credit */
 	init_crd = REG_RD(bp, PBF_REG_P0_INIT_CRD + port*4);
 	crd = REG_RD(bp, PBF_REG_P0_CREDIT + port*8);
 	DP(NETIF_MSG_LINK, "init_crd 0x%x  crd 0x%x\n", init_crd, crd);
 
 	while ((init_crd != crd) && count) {
-		msleep(5);
-
+		usleep_range(5000, 10000);
 		crd = REG_RD(bp, PBF_REG_P0_CREDIT + port*8);
 		count--;
 	}
@@ -2837,18 +2935,18 @@ static int bnx2x_pbf_update(struct link_params *params, u32 flow_ctrl,
 	    line_speed == SPEED_1000 ||
 	    line_speed == SPEED_2500) {
 		REG_WR(bp, PBF_REG_P0_PAUSE_ENABLE + port*4, 1);
-		/* update threshold */
+		/* Update threshold */
 		REG_WR(bp, PBF_REG_P0_ARB_THRSH + port*4, 0);
-		/* update init credit */
+		/* Update init credit */
 		init_crd = 778;		/* (800-18-4) */
 
 	} else {
 		u32 thresh = (ETH_MAX_JUMBO_PACKET_SIZE +
 			      ETH_OVREHEAD)/16;
 		REG_WR(bp, PBF_REG_P0_PAUSE_ENABLE + port*4, 0);
-		/* update threshold */
+		/* Update threshold */
 		REG_WR(bp, PBF_REG_P0_ARB_THRSH + port*4, thresh);
-		/* update init credit */
+		/* Update init credit */
 		switch (line_speed) {
 		case SPEED_10000:
 			init_crd = thresh + 553 - 22;
@@ -2863,12 +2961,12 @@ static int bnx2x_pbf_update(struct link_params *params, u32 flow_ctrl,
 	DP(NETIF_MSG_LINK, "PBF updated to speed %d credit %d\n",
 		 line_speed, init_crd);
 
-	/* probe the credit changes */
+	/* Probe the credit changes */
 	REG_WR(bp, PBF_REG_INIT_P0 + port*4, 0x1);
-	msleep(5);
+	usleep_range(5000, 10000);
 	REG_WR(bp, PBF_REG_INIT_P0 + port*4, 0x0);
 
-	/* enable port */
+	/* Enable port */
 	REG_WR(bp, PBF_REG_DISABLE_NEW_TASK_PROC_P0 + port*4, 0x0);
 	return 0;
 }
@@ -2935,7 +3033,7 @@ static int bnx2x_cl22_write(struct bnx2x *bp,
 	REG_WR(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_MODE,
 	       mode & ~EMAC_MDIO_MODE_CLAUSE_45);
 
-	/* address */
+	/* Address */
 	tmp = ((phy->addr << 21) | (reg << 16) | val |
 	       EMAC_MDIO_COMM_COMMAND_WRITE_22 |
 	       EMAC_MDIO_COMM_START_BUSY);
@@ -2971,7 +3069,7 @@ static int bnx2x_cl22_read(struct bnx2x *bp,
 	REG_WR(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_MODE,
 	       mode & ~EMAC_MDIO_MODE_CLAUSE_45);
 
-	/* address */
+	/* Address */
 	val = ((phy->addr << 21) | (reg << 16) |
 	       EMAC_MDIO_COMM_COMMAND_READ_22 |
 	       EMAC_MDIO_COMM_START_BUSY);
@@ -3009,7 +3107,7 @@ static int bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
 	if (phy->flags & FLAGS_MDC_MDIO_WA_B0)
 		bnx2x_bits_en(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_STATUS,
 			      EMAC_MDIO_STATUS_10MB);
-	/* address */
+	/* Address */
 	val = ((phy->addr << 21) | (devad << 16) | reg |
 	       EMAC_MDIO_COMM_COMMAND_ADDRESS |
 	       EMAC_MDIO_COMM_START_BUSY);
@@ -3030,7 +3128,7 @@ static int bnx2x_cl45_read(struct bnx2x *bp, struct bnx2x_phy *phy,
 		*ret_val = 0;
 		rc = -EFAULT;
 	} else {
-		/* data */
+		/* Data */
 		val = ((phy->addr << 21) | (devad << 16) |
 		       EMAC_MDIO_COMM_COMMAND_READ_45 |
 		       EMAC_MDIO_COMM_START_BUSY);
@@ -3078,7 +3176,7 @@ static int bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 		bnx2x_bits_en(bp, phy->mdio_ctrl + EMAC_REG_EMAC_MDIO_STATUS,
 			      EMAC_MDIO_STATUS_10MB);
 
-	/* address */
+	/* Address */
 	tmp = ((phy->addr << 21) | (devad << 16) | reg |
 	       EMAC_MDIO_COMM_COMMAND_ADDRESS |
 	       EMAC_MDIO_COMM_START_BUSY);
@@ -3098,7 +3196,7 @@ static int bnx2x_cl45_write(struct bnx2x *bp, struct bnx2x_phy *phy,
 		netdev_err(bp->dev,  "MDC/MDIO access timeout\n");
 		rc = -EFAULT;
 	} else {
-		/* data */
+		/* Data */
 		tmp = ((phy->addr << 21) | (devad << 16) | val |
 		       EMAC_MDIO_COMM_COMMAND_WRITE_45 |
 		       EMAC_MDIO_COMM_START_BUSY);
@@ -3188,23 +3286,23 @@ static int bnx2x_bsc_read(struct link_params *params,
 
 	xfer_cnt = 16 - lc_addr;
 
-	/* enable the engine */
+	/* Enable the engine */
 	val = REG_RD(bp, MCP_REG_MCPR_IMC_COMMAND);
 	val |= MCPR_IMC_COMMAND_ENABLE;
 	REG_WR(bp, MCP_REG_MCPR_IMC_COMMAND, val);
 
-	/* program slave device ID */
+	/* Program slave device ID */
 	val = (sl_devid << 16) | sl_addr;
 	REG_WR(bp, MCP_REG_MCPR_IMC_SLAVE_CONTROL, val);
 
-	/* start xfer with 0 byte to update the address pointer ???*/
+	/* Start xfer with 0 byte to update the address pointer ???*/
 	val = (MCPR_IMC_COMMAND_ENABLE) |
 	      (MCPR_IMC_COMMAND_WRITE_OP <<
 		MCPR_IMC_COMMAND_OPERATION_BITSHIFT) |
 		(lc_addr << MCPR_IMC_COMMAND_TRANSFER_ADDRESS_BITSHIFT) | (0);
 	REG_WR(bp, MCP_REG_MCPR_IMC_COMMAND, val);
 
-	/* poll for completion */
+	/* Poll for completion */
 	i = 0;
 	val = REG_RD(bp, MCP_REG_MCPR_IMC_COMMAND);
 	while (((val >> MCPR_IMC_COMMAND_IMC_STATUS_BITSHIFT) & 0x3) != 1) {
@@ -3220,7 +3318,7 @@ static int bnx2x_bsc_read(struct link_params *params,
 	if (rc == -EFAULT)
 		return rc;
 
-	/* start xfer with read op */
+	/* Start xfer with read op */
 	val = (MCPR_IMC_COMMAND_ENABLE) |
 		(MCPR_IMC_COMMAND_READ_OP <<
 		MCPR_IMC_COMMAND_OPERATION_BITSHIFT) |
@@ -3228,7 +3326,7 @@ static int bnx2x_bsc_read(struct link_params *params,
 		  (xfer_cnt);
 	REG_WR(bp, MCP_REG_MCPR_IMC_COMMAND, val);
 
-	/* poll for completion */
+	/* Poll for completion */
 	i = 0;
 	val = REG_RD(bp, MCP_REG_MCPR_IMC_COMMAND);
 	while (((val >> MCPR_IMC_COMMAND_IMC_STATUS_BITSHIFT) & 0x3) != 1) {
@@ -3331,7 +3429,7 @@ static u8 bnx2x_get_warpcore_lane(struct bnx2x_phy *phy,
 			port = port ^ 1;
 
 		lane = (port<<1) + path;
-	} else { /* two port mode - no port swap */
+	} else { /* Two port mode - no port swap */
 
 		/* Figure out path swap value */
 		path_swap_ovr =
@@ -3409,7 +3507,7 @@ static void bnx2x_serdes_deassert(struct bnx2x *bp, u8 port)
 
 	val = SERDES_RESET_BITS << (port*16);
 
-	/* reset and unreset the SerDes/XGXS */
+	/* Reset and unreset the SerDes/XGXS */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_CLEAR, val);
 	udelay(500);
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_SET, val);
@@ -3430,7 +3528,7 @@ static void bnx2x_xgxs_deassert(struct link_params *params)
 
 	val = XGXS_RESET_BITS << (port*16);
 
-	/* reset and unreset the SerDes/XGXS */
+	/* Reset and unreset the SerDes/XGXS */
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_CLEAR, val);
 	udelay(500);
 	REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_SET, val);
@@ -3522,7 +3620,7 @@ static void bnx2x_ext_phy_set_pause(struct link_params *params,
 {
 	u16 val;
 	struct bnx2x *bp = params->bp;
-	/* read modify write pause advertizing */
+	/* Read modify write pause advertizing */
 	bnx2x_cl45_read(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_ADV_PAUSE, &val);
 
 	val &= ~MDIO_AN_REG_ADV_PAUSE_BOTH;
@@ -3657,44 +3755,35 @@ static u8 bnx2x_ext_phy_resolve_fc(struct bnx2x_phy *phy,
 static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 					struct link_params *params,
 					struct link_vars *vars) {
-	u16 val16 = 0, lane, bam37 = 0;
-	struct bnx2x *bp = params->bp;
+	u16 val16 = 0, lane, i;
+	struct bnx2x *bp = params->bp;
+	static struct bnx2x_reg_set reg_set[] = {
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x7},
+		{MDIO_AN_DEVAD, MDIO_WC_REG_PAR_DET_10G_CTRL, 0},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL, 0},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL0, 0xff},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL1, 0x5555},
+		{MDIO_PMA_DEVAD, MDIO_WC_REG_IEEE0BLK_AUTONEGNP, 0x0},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_RX66_CONTROL, 0x7415},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_MISC2, 0x6190},
+		/* Disable Autoneg: re-enable it after adv is done. */
+		{MDIO_AN_DEVAD, MDIO_WC_REG_IEEE0BLK_MIICNTL, 0}
+	};
 	DP(NETIF_MSG_LINK, "Enable Auto Negotiation for KR\n");
 	/* Set to default registers that may be overriden by 10G force */
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x7);
-	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-			 MDIO_WC_REG_PAR_DET_10G_CTRL, 0);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL, 0);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_XGXSBLK1_LANECTRL0, 0xff);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_XGXSBLK1_LANECTRL1, 0x5555);
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD,
-			 MDIO_WC_REG_IEEE0BLK_AUTONEGNP, 0x0);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_RX66_CONTROL, 0x7415);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_MISC2, 0x6190);
-	/* Disable Autoneg: re-enable it after adv is done. */
-	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-			 MDIO_WC_REG_IEEE0BLK_MIICNTL, 0);
+	for (i = 0; i < sizeof(reg_set)/sizeof(struct bnx2x_reg_set); i++)
+		bnx2x_cl45_write(bp, phy, reg_set[i].devad, reg_set[i].reg,
+				 reg_set[i].val);
 
 	/* Check adding advertisement for 1G KX */
 	if (((vars->line_speed == SPEED_AUTO_NEG) &&
 	     (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_1G)) ||
 	    (vars->line_speed == SPEED_1000)) {
-		u16 sd_digital;
+		u32 addr = MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2;
 		val16 |= (1<<5);
 
 		/* Enable CL37 1G Parallel Detect */
-		bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, &sd_digital);
-		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2,
-				 (sd_digital | 0x1));
-
+		bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD, addr, 0x1);
 		DP(NETIF_MSG_LINK, "Advertize 1G\n");
 	}
 	if (((vars->line_speed == SPEED_AUTO_NEG) &&
@@ -3704,7 +3793,7 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 		val16 |= (1<<7);
 		/* Enable 10G Parallel Detect */
 		bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-				MDIO_WC_REG_PAR_DET_10G_CTRL, 1);
+				 MDIO_WC_REG_PAR_DET_10G_CTRL, 1);
 
 		DP(NETIF_MSG_LINK, "Advertize 10G\n");
 	}
@@ -3738,10 +3827,9 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 		   offsetof(struct shmem_region, dev_info.
 			    port_hw_config[params->port].default_cfg)) &
 	    PORT_HW_CFG_ENABLE_BAM_ON_KR_ENABLED) {
-		bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_DIGITAL6_MP5_NEXTPAGECTRL, &bam37);
-		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_DIGITAL6_MP5_NEXTPAGECTRL, bam37 | 1);
+		bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+					 MDIO_WC_REG_DIGITAL6_MP5_NEXTPAGECTRL,
+					 1);
 		DP(NETIF_MSG_LINK, "Enable CL37 BAM on KR\n");
 	}
 
@@ -3755,11 +3843,8 @@ static void bnx2x_warpcore_enable_AN_KR(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK, "Enable AN KR work-around\n");
 		vars->rx_tx_asic_rst = MAX_KR_LINK_RETRY;
 	}
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_DIGITAL5_MISC7, &val16);
-
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_DIGITAL5_MISC7, val16 | 0x100);
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_DIGITAL5_MISC7, 0x100);
 
 	/* Over 1G - AN local device user page 1 */
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
@@ -3776,50 +3861,35 @@ static void bnx2x_warpcore_set_10G_KR(struct bnx2x_phy *phy,
 				      struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
-	u16 val;
-
-	/* Disable Autoneg */
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x7);
-
-	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-			 MDIO_WC_REG_PAR_DET_10G_CTRL, 0);
-
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL, 0x3f00);
-
-	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-			 MDIO_WC_REG_AN_IEEE1BLK_AN_ADVERTISEMENT1, 0);
-
-	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
-			 MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x0);
-
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_DIGITAL3_UP1, 0x1);
-
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_DIGITAL5_MISC7, 0xa);
-
-	/* Disable CL36 PCS Tx */
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_XGXSBLK1_LANECTRL0, 0x0);
-
-	/* Double Wide Single Data Rate @ pll rate */
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_XGXSBLK1_LANECTRL1, 0xFFFF);
-
-	/* Leave cl72 training enable, needed for KR */
-	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD,
+	u16 i;
+	static struct bnx2x_reg_set reg_set[] = {
+		/* Disable Autoneg */
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x7},
+		{MDIO_AN_DEVAD, MDIO_WC_REG_PAR_DET_10G_CTRL, 0},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL,
+			0x3f00},
+		{MDIO_AN_DEVAD, MDIO_WC_REG_AN_IEEE1BLK_AN_ADVERTISEMENT1, 0},
+		{MDIO_AN_DEVAD, MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x0},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_DIGITAL3_UP1, 0x1},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_DIGITAL5_MISC7, 0xa},
+		/* Disable CL36 PCS Tx */
+		{MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL0, 0x0},
+		/* Double Wide Single Data Rate @ pll rate */
+		{MDIO_WC_DEVAD, MDIO_WC_REG_XGXSBLK1_LANECTRL1, 0xFFFF},
+		/* Leave cl72 training enable, needed for KR */
+		{MDIO_PMA_DEVAD,
 		MDIO_WC_REG_PMD_IEEE9BLK_TENGBASE_KR_PMD_CONTROL_REGISTER_150,
-		0x2);
+		0x2}
+	};
+
+	for (i = 0; i < sizeof(reg_set)/sizeof(struct bnx2x_reg_set); i++)
+		bnx2x_cl45_write(bp, phy, reg_set[i].devad, reg_set[i].reg,
+				 reg_set[i].val);
 
 	/* Leave CL72 enabled */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL,
-			 &val);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL,
-			 val | 0x3800);
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_CL72_USERB0_CL72_MISC1_CONTROL,
+				 0x3800);
 
 	/* Set speed via PMA/PMD register */
 	bnx2x_cl45_write(bp, phy, MDIO_PMA_DEVAD,
@@ -3840,7 +3910,7 @@ static void bnx2x_warpcore_set_10G_KR(struct bnx2x_phy *phy,
 	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
 				 MDIO_WC_REG_RX66_CONTROL, 0xF9);
 
-	/* set and clear loopback to cause a reset to 64/66 decoder */
+	/* Set and clear loopback to cause a reset to 64/66 decoder */
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x4000);
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
@@ -3855,16 +3925,12 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 misc1_val, tap_val, tx_driver_val, lane, val;
 	/* Hold rxSeqStart */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_DSC2B0_DSC_MISC_CTRL0, &val);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_DSC2B0_DSC_MISC_CTRL0, (val | 0x8000));
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_DSC2B0_DSC_MISC_CTRL0, 0x8000);
 
 	/* Hold tx_fifo_reset */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X3, &val);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X3, (val | 0x1));
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X3, 0x1);
 
 	/* Disable CL73 AN */
 	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_CTRL, 0);
@@ -3876,10 +3942,8 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 			 MDIO_WC_REG_FX100_CTRL1, (val & 0xFFFA));
 
 	/* Disable 100FX Idle detect */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_FX100_CTRL3, &val);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_FX100_CTRL3, (val | 0x0080));
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_FX100_CTRL3, 0x0080);
 
 	/* Set Block address to Remote PHY & Clear forced_speed[5] */
 	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
@@ -3940,16 +4004,20 @@ static void bnx2x_warpcore_set_10G_XFI(struct bnx2x_phy *phy,
 			 tx_driver_val);
 
 	/* Enable fiber mode, enable and invert sig_det */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X1, &val);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X1, val | 0xd);
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X1, 0xd);
 
 	/* Set Block address to Remote PHY & Set forced_speed[5], 40bit mode */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_DIGITAL4_MISC3, &val);
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_DIGITAL4_MISC3, 0x8080);
+
+	/* Enable LPI pass through */
+	DP(NETIF_MSG_LINK, "Configure WC for LPI pass through\n");
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_DIGITAL4_MISC3, val | 0x8080);
+			 MDIO_WC_REG_EEE_COMBO_CONTROL0,
+			 0x7c);
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_DIGITAL4_MISC5, 0xc000);
 
 	/* 10G XFI Full Duplex */
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
@@ -4139,40 +4207,35 @@ static void bnx2x_warpcore_clear_regs(struct bnx2x_phy *phy,
 				      u16 lane)
 {
 	struct bnx2x *bp = params->bp;
-	u16 val16;
-
+	u16 i;
+	static struct bnx2x_reg_set wc_regs[] = {
+		{MDIO_AN_DEVAD, MDIO_AN_REG_CTRL, 0},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_FX100_CTRL1, 0x014a},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_FX100_CTRL3, 0x0800},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_DIGITAL4_MISC3, 0x8008},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X1,
+			0x0195},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2,
+			0x0007},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X3,
+			0x0002},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_SERDESDIGITAL_MISC1, 0x6000},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_TX_FIR_TAP, 0x0000},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x2040},
+		{MDIO_WC_DEVAD, MDIO_WC_REG_COMBO_IEEE0_MIICTRL, 0x0140}
+	};
 	/* Set XFI clock comp as default. */
-	bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-			MDIO_WC_REG_RX66_CONTROL, &val16);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_RX66_CONTROL, val16 | (3<<13));
+	bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+				 MDIO_WC_REG_RX66_CONTROL, (3<<13));
+
+	for (i = 0; i < sizeof(wc_regs)/sizeof(struct bnx2x_reg_set); i++)
+		bnx2x_cl45_write(bp, phy, wc_regs[i].devad, wc_regs[i].reg,
+				 wc_regs[i].val);
 
-	bnx2x_warpcore_reset_lane(bp, phy, 1);
-	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_CTRL, 0);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_FX100_CTRL1, 0x014a);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_FX100_CTRL3, 0x0800);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_DIGITAL4_MISC3, 0x8008);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X1, 0x0195);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X2, 0x0007);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_CONTROL1000X3, 0x0002);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_SERDESDIGITAL_MISC1, 0x6000);
 	lane = bnx2x_get_warpcore_lane(phy, params);
 	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_TX_FIR_TAP, 0x0000);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
 			 MDIO_WC_REG_TX0_TX_DRIVER + 0x10*lane, 0x0990);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x2040);
-	bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-			 MDIO_WC_REG_COMBO_IEEE0_MIICTRL, 0x0140);
-	bnx2x_warpcore_reset_lane(bp, phy, 0);
+
 }
 
 static int bnx2x_get_mod_abs_int_cfg(struct bnx2x *bp,
@@ -4260,7 +4323,7 @@ static void bnx2x_warpcore_config_runtime(struct bnx2x_phy *phy,
 	if (!vars->turn_to_run_wc_rt)
 		return;
 
-	/* return if there is no link partner */
+	/* Return if there is no link partner */
 	if (!(bnx2x_warpcore_get_sigdet(phy, params))) {
 		DP(NETIF_MSG_LINK, "bnx2x_warpcore_get_sigdet false\n");
 		return;
@@ -4294,7 +4357,7 @@ static void bnx2x_warpcore_config_runtime(struct bnx2x_phy *phy,
 				bnx2x_warpcore_reset_lane(bp, phy, 1);
 				bnx2x_warpcore_reset_lane(bp, phy, 0);
 
-				/* restart Autoneg */
+				/* Restart Autoneg */
 				bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
 					MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x1200);
 
@@ -4311,6 +4374,23 @@ static void bnx2x_warpcore_config_runtime(struct bnx2x_phy *phy,
 	} /*params->rx_tx_asic_rst*/
 
 }
+static void bnx2x_warpcore_config_sfi(struct bnx2x_phy *phy,
+				      struct link_params *params)
+{
+	u16 lane = bnx2x_get_warpcore_lane(phy, params);
+	struct bnx2x *bp = params->bp;
+	bnx2x_warpcore_clear_regs(phy, params, lane);
+	if ((params->req_line_speed[LINK_CONFIG_IDX(INT_PHY)] ==
+	     SPEED_10000) &&
+	    (phy->media_type != ETH_PHY_SFP_1G_FIBER)) {
+		DP(NETIF_MSG_LINK, "Setting 10G SFI\n");
+		bnx2x_warpcore_set_10G_XFI(phy, params, 0);
+	} else {
+		DP(NETIF_MSG_LINK, "Setting 1G Fiber\n");
+		bnx2x_warpcore_set_sgmii_speed(phy, params, 1, 0);
+	}
+}
+
 static void bnx2x_warpcore_config_init(struct bnx2x_phy *phy,
 				       struct link_params *params,
 				       struct link_vars *vars)
@@ -4371,19 +4451,11 @@ static void bnx2x_warpcore_config_init(struct bnx2x_phy *phy,
 			break;
 
 		case PORT_HW_CFG_NET_SERDES_IF_SFI:
-
-			bnx2x_warpcore_clear_regs(phy, params, lane);
-			if (vars->line_speed == SPEED_10000) {
-				DP(NETIF_MSG_LINK, "Setting 10G SFI\n");
-				bnx2x_warpcore_set_10G_XFI(phy, params, 0);
-			} else if (vars->line_speed == SPEED_1000) {
-				DP(NETIF_MSG_LINK, "Setting 1G Fiber\n");
-				bnx2x_warpcore_set_sgmii_speed(
-						phy, params, 1, 0);
-			}
 			/* Issue Module detection */
 			if (bnx2x_is_sfp_module_plugged(phy, params))
 				bnx2x_sfp_module_detection(phy, params);
+
+			bnx2x_warpcore_config_sfi(phy, params);
 			break;
 
 		case PORT_HW_CFG_NET_SERDES_IF_DXGXS:
@@ -4500,12 +4572,9 @@ static void bnx2x_set_warpcore_loopback(struct bnx2x_phy *phy,
 		CL22_WR_OVER_CL45(bp, phy, MDIO_REG_BANK_AER_BLOCK,
 				  MDIO_AER_BLOCK_AER_REG, 0);
 		/* Enable 1G MDIO (1-copy) */
-		bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_XGXSBLK0_XGXSCONTROL,
-				&val16);
-		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_XGXSBLK0_XGXSCONTROL,
-				val16 | 0x10);
+		bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+					 MDIO_WC_REG_XGXSBLK0_XGXSCONTROL,
+					 0x10);
 		/* Set 1G loopback based on lane (1-copy) */
 		lane = bnx2x_get_warpcore_lane(phy, params);
 		bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
@@ -4518,22 +4587,19 @@ static void bnx2x_set_warpcore_loopback(struct bnx2x_phy *phy,
 		bnx2x_set_aer_mmd(params, phy);
 	} else {
 		/* 10G & 20G */
-		bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_COMBO_IEEE0_MIICTRL, &val16);
-		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_COMBO_IEEE0_MIICTRL, val16 |
-				 0x4000);
+		bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+					 MDIO_WC_REG_COMBO_IEEE0_MIICTRL,
+					 0x4000);
 
-		bnx2x_cl45_read(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_IEEE0BLK_MIICNTL, &val16);
-		bnx2x_cl45_write(bp, phy, MDIO_WC_DEVAD,
-				MDIO_WC_REG_IEEE0BLK_MIICNTL, val16 | 0x1);
+		bnx2x_cl45_read_or_write(bp, phy, MDIO_WC_DEVAD,
+					 MDIO_WC_REG_IEEE0BLK_MIICNTL, 0x1);
 	}
 }
 
 
-void bnx2x_sync_link(struct link_params *params,
-			   struct link_vars *vars)
+
+static void bnx2x_sync_link(struct link_params *params,
+			     struct link_vars *vars)
 {
 	struct bnx2x *bp = params->bp;
 	u8 link_10g_plus;
@@ -4606,7 +4672,7 @@ void bnx2x_sync_link(struct link_params *params,
 		    USES_WARPCORE(bp) &&
 		    (vars->line_speed == SPEED_1000))
 			vars->phy_flags |= PHY_SGMII_FLAG;
-		/* anything 10 and over uses the bmac */
+		/* Anything 10 and over uses the bmac */
 		link_10g_plus = (vars->line_speed >= SPEED_10000);
 
 		if (link_10g_plus) {
@@ -4620,7 +4686,7 @@ void bnx2x_sync_link(struct link_params *params,
 			else
 				vars->mac_type = MAC_TYPE_EMAC;
 		}
-	} else { /* link down */
+	} else { /* Link down */
 		DP(NETIF_MSG_LINK, "phy link down\n");
 
 		vars->phy_link_up = 0;
@@ -4629,10 +4695,12 @@ void bnx2x_sync_link(struct link_params *params,
 		vars->duplex = DUPLEX_FULL;
 		vars->flow_ctrl = BNX2X_FLOW_CTRL_NONE;
 
-		/* indicate no mac active */
+		/* Indicate no mac active */
 		vars->mac_type = MAC_TYPE_NONE;
 		if (vars->link_status & LINK_STATUS_PHYSICAL_LINK_FLAG)
 			vars->phy_flags |= PHY_HALF_OPEN_CONN_FLAG;
+		if (vars->link_status & LINK_STATUS_SFP_TX_FAULT)
+			vars->phy_flags |= PHY_SFP_TX_FAULT_FLAG;
 	}
 }
 
@@ -4698,7 +4766,7 @@ static void bnx2x_set_master_ln(struct link_params *params,
 		     PORT_HW_CFG_LANE_SWAP_CFG_MASTER_MASK) >>
 		    PORT_HW_CFG_LANE_SWAP_CFG_MASTER_SHIFT);
 
-	/* set the master_ln for AN */
+	/* Set the master_ln for AN */
 	CL22_RD_OVER_CL45(bp, phy,
 			  MDIO_REG_BANK_XGXS_BLOCK2,
 			  MDIO_XGXS_BLOCK2_TEST_MODE_LANE,
@@ -4721,7 +4789,7 @@ static int bnx2x_reset_unicore(struct link_params *params,
 			  MDIO_REG_BANK_COMBO_IEEE0,
 			  MDIO_COMBO_IEEE0_MII_CONTROL, &mii_control);
 
-	/* reset the unicore */
+	/* Reset the unicore */
 	CL22_WR_OVER_CL45(bp, phy,
 			  MDIO_REG_BANK_COMBO_IEEE0,
 			  MDIO_COMBO_IEEE0_MII_CONTROL,
@@ -4730,11 +4798,11 @@ static int bnx2x_reset_unicore(struct link_params *params,
 	if (set_serdes)
 		bnx2x_set_serdes_access(bp, params->port);
 
-	/* wait for the reset to self clear */
+	/* Wait for the reset to self clear */
 	for (i = 0; i < MDIO_ACCESS_TIMEOUT; i++) {
 		udelay(5);
 
-		/* the reset erased the previous bank value */
+		/* The reset erased the previous bank value */
 		CL22_RD_OVER_CL45(bp, phy,
 				  MDIO_REG_BANK_COMBO_IEEE0,
 				  MDIO_COMBO_IEEE0_MII_CONTROL,
@@ -4952,7 +5020,7 @@ static void bnx2x_set_autoneg(struct bnx2x_phy *phy,
 			  MDIO_CL73_IEEEB0_CL73_AN_CONTROL, reg_val);
 }
 
-/* program SerDes, forced speed */
+/* Program SerDes, forced speed */
 static void bnx2x_program_serdes(struct bnx2x_phy *phy,
 				 struct link_params *params,
 				 struct link_vars *vars)
@@ -4960,7 +5028,7 @@ static void bnx2x_program_serdes(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 reg_val;
 
-	/* program duplex, disable autoneg and sgmii*/
+	/* Program duplex, disable autoneg and sgmii*/
 	CL22_RD_OVER_CL45(bp, phy,
 			  MDIO_REG_BANK_COMBO_IEEE0,
 			  MDIO_COMBO_IEEE0_MII_CONTROL, &reg_val);
@@ -4979,7 +5047,7 @@ static void bnx2x_program_serdes(struct bnx2x_phy *phy,
 	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 */
+	/* Clearing the speed value before setting the right speed */
 	DP(NETIF_MSG_LINK, "MDIO_REG_BANK_SERDES_DIGITAL = 0x%x\n", reg_val);
 
 	reg_val &= ~(MDIO_SERDES_DIGITAL_MISC1_FORCE_SPEED_MASK |
@@ -5008,7 +5076,7 @@ static void bnx2x_set_brcm_cl37_advertisement(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 val = 0;
 
-	/* set extended capabilities */
+	/* Set extended capabilities */
 	if (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_2_5G)
 		val |= MDIO_OVER_1G_UP1_2_5G;
 	if (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)
@@ -5028,7 +5096,7 @@ static void bnx2x_set_ieee_aneg_advertisement(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u16 val;
-	/* for AN, we are always publishing full duplex */
+	/* For AN, we are always publishing full duplex */
 
 	CL22_WR_OVER_CL45(bp, phy,
 			  MDIO_REG_BANK_COMBO_IEEE0,
@@ -5090,14 +5158,14 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 control1;
 
-	/* in SGMII mode, the unicore is always slave */
+	/* In SGMII mode, the unicore is always slave */
 
 	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) */
+	/* 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);
@@ -5106,9 +5174,9 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 			  MDIO_SERDES_DIGITAL_A_1000X_CONTROL1,
 			  control1);
 
-	/* if forced speed */
+	/* If forced speed */
 	if (!(vars->line_speed == SPEED_AUTO_NEG)) {
-		/* set speed, disable autoneg */
+		/* Set speed, disable autoneg */
 		u16 mii_control;
 
 		CL22_RD_OVER_CL45(bp, phy,
@@ -5129,16 +5197,16 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 				MDIO_COMBO_IEEO_MII_CONTROL_MAN_SGMII_SP_1000;
 			break;
 		case SPEED_10:
-			/* there is nothing to set for 10M */
+			/* There is nothing to set for 10M */
 			break;
 		default:
-			/* invalid speed for SGMII */
+			/* Invalid speed for SGMII */
 			DP(NETIF_MSG_LINK, "Invalid line_speed 0x%x\n",
 				  vars->line_speed);
 			break;
 		}
 
-		/* setting the full duplex */
+		/* Setting the full duplex */
 		if (phy->req_duplex == DUPLEX_FULL)
 			mii_control |=
 				MDIO_COMBO_IEEO_MII_CONTROL_FULL_DUPLEX;
@@ -5148,7 +5216,7 @@ static void bnx2x_initialize_sgmii_process(struct bnx2x_phy *phy,
 				  mii_control);
 
 	} else { /* AN mode */
-		/* enable and restart AN */
+		/* Enable and restart AN */
 		bnx2x_restart_autoneg(phy, params, 0);
 	}
 }
@@ -5244,7 +5312,7 @@ static void bnx2x_flow_ctrl_resolve(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	vars->flow_ctrl = BNX2X_FLOW_CTRL_NONE;
 
-	/* resolve from gp_status in case of AN complete and not sgmii */
+	/* Resolve from gp_status in case of AN complete and not sgmii */
 	if (phy->req_flow_ctrl != BNX2X_FLOW_CTRL_AUTO) {
 		/* Update the advertised flow-controled of LD/LP in AN */
 		if (phy->req_line_speed == SPEED_AUTO_NEG)
@@ -5468,7 +5536,7 @@ static int bnx2x_link_settings_status(struct bnx2x_phy *phy,
 				bnx2x_xgxs_an_resolve(phy, params, vars,
 						      gp_status);
 		}
-	} else { /* link_down */
+	} else { /* Link_down */
 		if ((phy->req_line_speed == SPEED_AUTO_NEG) &&
 		    SINGLE_MEDIA_DIRECT(params)) {
 			/* Check signal is detected */
@@ -5617,12 +5685,12 @@ static void bnx2x_set_gmii_tx_driver(struct link_params *params)
 	u16 tx_driver;
 	u16 bank;
 
-	/* read precomp */
+	/* Read precomp */
 	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] */
+	/* Bits [10:7] at lp_up2, positioned at [15:12] */
 	lp_up2 = (((lp_up2 & MDIO_OVER_1G_LP_UP2_PREEMPHASIS_MASK) >>
 		   MDIO_OVER_1G_LP_UP2_PREEMPHASIS_SHIFT) <<
 		  MDIO_TX0_TX_DRIVER_PREEMPHASIS_SHIFT);
@@ -5636,7 +5704,7 @@ static void bnx2x_set_gmii_tx_driver(struct link_params *params)
 				  bank,
 				  MDIO_TX0_TX_DRIVER, &tx_driver);
 
-		/* replace tx_driver bits [15:12] */
+		/* 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;
@@ -5732,16 +5800,16 @@ static void bnx2x_xgxs_config_init(struct bnx2x_phy *phy,
 		     FEATURE_CONFIG_OVERRIDE_PREEMPHASIS_ENABLED))
 			bnx2x_set_preemphasis(phy, params);
 
-		/* forced speed requested? */
+		/* Forced speed requested? */
 		if (vars->line_speed != SPEED_AUTO_NEG ||
 		    (SINGLE_MEDIA_DIRECT(params) &&
 		     params->loopback_mode == LOOPBACK_EXT)) {
 			DP(NETIF_MSG_LINK, "not SGMII, no AN\n");
 
-			/* disable autoneg */
+			/* Disable autoneg */
 			bnx2x_set_autoneg(phy, params, vars, 0);
 
-			/* program speed and duplex */
+			/* Program speed and duplex */
 			bnx2x_program_serdes(phy, params, vars);
 
 		} else { /* AN_mode */
@@ -5750,14 +5818,14 @@ static void bnx2x_xgxs_config_init(struct bnx2x_phy *phy,
 			/* AN enabled */
 			bnx2x_set_brcm_cl37_advertisement(phy, params);
 
-			/* program duplex & pause advertisement (for aneg) */
+			/* Program duplex & pause advertisement (for aneg) */
 			bnx2x_set_ieee_aneg_advertisement(phy, params,
 							  vars->ieee_fc);
 
-			/* enable autoneg */
+			/* Enable autoneg */
 			bnx2x_set_autoneg(phy, params, vars, enable_cl73);
 
-			/* enable and restart AN */
+			/* Enable and restart AN */
 			bnx2x_restart_autoneg(phy, params, enable_cl73);
 		}
 
@@ -5793,12 +5861,12 @@ static int bnx2x_prepare_xgxs(struct bnx2x_phy *phy,
 		bnx2x_set_master_ln(params, phy);
 
 	rc = bnx2x_reset_unicore(params, phy, 0);
-	/* reset the SerDes and wait for reset bit return low */
-	if (rc != 0)
+	/* Reset the SerDes and wait for reset bit return low */
+	if (rc)
 		return rc;
 
 	bnx2x_set_aer_mmd(params, phy);
-	/* setting the masterLn_def again after the reset */
+	/* Setting the masterLn_def again after the reset */
 	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_DIRECT) {
 		bnx2x_set_master_ln(params, phy);
 		bnx2x_set_swap_lanes(params, phy);
@@ -5823,7 +5891,7 @@ static u16 bnx2x_wait_reset_complete(struct bnx2x *bp,
 				MDIO_PMA_REG_CTRL, &ctrl);
 		if (!(ctrl & (1<<15)))
 			break;
-		msleep(1);
+		usleep_range(1000, 2000);
 	}
 
 	if (cnt == 1000)
@@ -6054,7 +6122,7 @@ static void bnx2x_set_xgxs_loopback(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK, "XGXS 10G loopback enable\n");
 
 		if (!CHIP_IS_E3(bp)) {
-			/* change the uni_phy_addr in the nig */
+			/* Change the uni_phy_addr in the nig */
 			md_devad = REG_RD(bp, (NIG_REG_XGXS0_CTRL_MD_DEVAD +
 					       port*0x18));
 
@@ -6074,11 +6142,11 @@ static void bnx2x_set_xgxs_loopback(struct bnx2x_phy *phy,
 				  (MDIO_CL73_IEEEB0_CL73_AN_CONTROL & 0xf)),
 				 0x6041);
 		msleep(200);
-		/* set aer mmd back */
+		/* Set aer mmd back */
 		bnx2x_set_aer_mmd(params, phy);
 
 		if (!CHIP_IS_E3(bp)) {
-			/* and md_devad */
+			/* And md_devad */
 			REG_WR(bp, NIG_REG_XGXS0_CTRL_MD_DEVAD + port*0x18,
 			       md_devad);
 		}
@@ -6275,7 +6343,7 @@ int bnx2x_test_link(struct link_params *params, struct link_vars *vars,
 			  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 */
+	/* 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;
 	}
@@ -6296,7 +6364,9 @@ int bnx2x_test_link(struct link_params *params, struct link_vars *vars,
 		for (phy_index = EXT_PHY1; phy_index < params->num_phys;
 		      phy_index++) {
 			serdes_phy_type = ((params->phy[phy_index].media_type ==
-					    ETH_PHY_SFP_FIBER) ||
+					    ETH_PHY_SFPP_10G_FIBER) ||
+					   (params->phy[phy_index].media_type ==
+					    ETH_PHY_SFP_1G_FIBER) ||
 					   (params->phy[phy_index].media_type ==
 					    ETH_PHY_XFP_FIBER) ||
 					   (params->phy[phy_index].media_type ==
@@ -6397,7 +6467,7 @@ static int bnx2x_link_initialize(struct link_params *params,
 static void bnx2x_int_link_reset(struct bnx2x_phy *phy,
 				 struct link_params *params)
 {
-	/* reset the SerDes/XGXS */
+	/* Reset the SerDes/XGXS */
 	REG_WR(params->bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_3_CLEAR,
 	       (0x1ff << (params->port*16)));
 }
@@ -6430,10 +6500,10 @@ static int bnx2x_update_link_down(struct link_params *params,
 	DP(NETIF_MSG_LINK, "Port %x: Link is down\n", port);
 	bnx2x_set_led(params, vars, LED_MODE_OFF, 0);
 	vars->phy_flags &= ~PHY_PHYSICAL_LINK_FLAG;
-	/* indicate no mac active */
+	/* Indicate no mac active */
 	vars->mac_type = MAC_TYPE_NONE;
 
-	/* update shared memory */
+	/* Update shared memory */
 	vars->link_status &= ~(LINK_STATUS_SPEED_AND_DUPLEX_MASK |
 			       LINK_STATUS_LINK_UP |
 			       LINK_STATUS_PHYSICAL_LINK_FLAG |
@@ -6446,15 +6516,15 @@ static int bnx2x_update_link_down(struct link_params *params,
 	vars->line_speed = 0;
 	bnx2x_update_mng(params, vars->link_status);
 
-	/* activate nig drain */
+	/* Activate nig drain */
 	REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + port*4, 1);
 
-	/* disable emac */
+	/* Disable emac */
 	if (!CHIP_IS_E3(bp))
 		REG_WR(bp, NIG_REG_NIG_EMAC0_EN + port*4, 0);
 
-	msleep(10);
-	/* reset BigMac/Xmac */
+	usleep_range(10000, 20000);
+	/* Reset BigMac/Xmac */
 	if (CHIP_IS_E1x(bp) ||
 	    CHIP_IS_E2(bp)) {
 		bnx2x_bmac_rx_disable(bp, params->port);
@@ -6463,6 +6533,16 @@ static int bnx2x_update_link_down(struct link_params *params,
 	       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
 	}
 	if (CHIP_IS_E3(bp)) {
+		/* Prevent LPI Generation by chip */
+		REG_WR(bp, MISC_REG_CPMU_LP_FW_ENABLE_P0 + (params->port << 2),
+		       0);
+		REG_WR(bp, MISC_REG_CPMU_LP_DR_ENABLE, 0);
+		REG_WR(bp, MISC_REG_CPMU_LP_MASK_ENT_P0 + (params->port << 2),
+		       0);
+		vars->eee_status &= ~(SHMEM_EEE_LP_ADV_STATUS_MASK |
+				      SHMEM_EEE_ACTIVE_BIT);
+
+		bnx2x_update_mng_eee(params, vars->eee_status);
 		bnx2x_xmac_disable(params);
 		bnx2x_umac_disable(params);
 	}
@@ -6502,6 +6582,16 @@ static int bnx2x_update_link_up(struct link_params *params,
 			bnx2x_umac_enable(params, vars, 0);
 		bnx2x_set_led(params, vars,
 			      LED_MODE_OPER, vars->line_speed);
+
+		if ((vars->eee_status & SHMEM_EEE_ACTIVE_BIT) &&
+		    (vars->eee_status & SHMEM_EEE_LPI_REQUESTED_BIT)) {
+			DP(NETIF_MSG_LINK, "Enabling LPI assertion\n");
+			REG_WR(bp, MISC_REG_CPMU_LP_FW_ENABLE_P0 +
+			       (params->port << 2), 1);
+			REG_WR(bp, MISC_REG_CPMU_LP_DR_ENABLE, 1);
+			REG_WR(bp, MISC_REG_CPMU_LP_MASK_ENT_P0 +
+			       (params->port << 2), 0xfc20);
+		}
 	}
 	if ((CHIP_IS_E1x(bp) ||
 	     CHIP_IS_E2(bp))) {
@@ -6534,12 +6624,12 @@ static int bnx2x_update_link_up(struct link_params *params,
 		rc |= bnx2x_pbf_update(params, vars->flow_ctrl,
 				       vars->line_speed);
 
-	/* disable drain */
+	/* Disable drain */
 	REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + port*4, 0);
 
-	/* update shared memory */
+	/* Update shared memory */
 	bnx2x_update_mng(params, vars->link_status);
-
+	bnx2x_update_mng_eee(params, vars->eee_status);
 	/* Check remote fault */
 	for (phy_idx = INT_PHY; phy_idx < MAX_PHYS; phy_idx++) {
 		if (params->phy[phy_idx].flags & FLAGS_TX_ERROR_CHECK) {
@@ -6583,6 +6673,8 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 		phy_vars[phy_index].phy_link_up = 0;
 		phy_vars[phy_index].link_up = 0;
 		phy_vars[phy_index].fault_detected = 0;
+		/* different consideration, since vars holds inner state */
+		phy_vars[phy_index].eee_status = vars->eee_status;
 	}
 
 	if (USES_WARPCORE(bp))
@@ -6603,7 +6695,7 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 	  REG_RD(bp, NIG_REG_XGXS0_STATUS_LINK10G + port*0x68),
 	  REG_RD(bp, NIG_REG_XGXS0_STATUS_LINK_STATUS + port*0x68));
 
-	/* disable emac */
+	/* Disable emac */
 	if (!CHIP_IS_E3(bp))
 		REG_WR(bp, NIG_REG_NIG_EMAC0_EN + port*4, 0);
 
@@ -6712,6 +6804,9 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 			vars->link_status |= LINK_STATUS_SERDES_LINK;
 		else
 			vars->link_status &= ~LINK_STATUS_SERDES_LINK;
+
+		vars->eee_status = phy_vars[active_external_phy].eee_status;
+
 		DP(NETIF_MSG_LINK, "Active external phy selected: %x\n",
 			   active_external_phy);
 	}
@@ -6745,11 +6840,11 @@ int bnx2x_link_update(struct link_params *params, struct link_vars *vars)
 		} else if (prev_line_speed != vars->line_speed) {
 			REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4,
 			       0);
-			msleep(1);
+			 usleep_range(1000, 2000);
 		}
 	}
 
-	/* anything 10 and over uses the bmac */
+	/* Anything 10 and over uses the bmac */
 	link_10g_plus = (vars->line_speed >= SPEED_10000);
 
 	bnx2x_link_int_ack(params, vars, link_10g_plus);
@@ -6815,7 +6910,7 @@ 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);
-	msleep(1);
+	 usleep_range(1000, 2000);
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
 		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, port);
 }
@@ -6912,7 +7007,7 @@ static int bnx2x_8073_8727_external_rom_boot(struct bnx2x *bp,
 			 MDIO_PMA_REG_GEN_CTRL,
 			 0x0001);
 
-	/* ucode reboot and rst */
+	/* Ucode reboot and rst */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
 			 MDIO_PMA_REG_GEN_CTRL,
@@ -6956,7 +7051,7 @@ static int bnx2x_8073_8727_external_rom_boot(struct bnx2x *bp,
 				MDIO_PMA_DEVAD,
 				MDIO_PMA_REG_M8051_MSGOUT_REG, &fw_msgout);
 
-		msleep(1);
+		 usleep_range(1000, 2000);
 	} while (fw_ver1 == 0 || fw_ver1 == 0x4321 ||
 			((fw_msgout & 0xff) != 0x03 && (phy->type ==
 			PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8073)));
@@ -7050,11 +7145,11 @@ static int bnx2x_8073_xaui_wa(struct bnx2x *bp, struct bnx2x_phy *phy)
 					  "XAUI workaround has completed\n");
 					return 0;
 				 }
-				 msleep(3);
+				 usleep_range(3000, 6000);
 			}
 			break;
 		}
-		msleep(3);
+		usleep_range(3000, 6000);
 	}
 	DP(NETIF_MSG_LINK, "Warning: XAUI work-around timeout !!!\n");
 	return -EINVAL;
@@ -7128,7 +7223,7 @@ static int bnx2x_8073_config_init(struct bnx2x_phy *phy,
 	bnx2x_set_gpio(bp, MISC_REGISTERS_GPIO_1,
 		       MISC_REGISTERS_GPIO_OUTPUT_HIGH, gpio_port);
 
-	/* enable LASI */
+	/* Enable LASI */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_LASI_RXCTRL, (1<<2));
 	bnx2x_cl45_write(bp, phy,
@@ -7276,7 +7371,7 @@ static u8 bnx2x_8073_read_status(struct bnx2x_phy *phy,
 
 	DP(NETIF_MSG_LINK, "8703 LASI status 0x%x\n", val1);
 
-	/* clear the interrupt LASI status register */
+	/* Clear the interrupt LASI status register */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PCS_DEVAD, MDIO_PCS_REG_STATUS, &val2);
 	bnx2x_cl45_read(bp, phy,
@@ -7601,7 +7696,7 @@ static int bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 val = 0;
 	u16 i;
-	if (byte_cnt > 16) {
+	if (byte_cnt > SFP_EEPROM_PAGE_SIZE) {
 		DP(NETIF_MSG_LINK,
 		   "Reading from eeprom is limited to 0xf\n");
 		return -EINVAL;
@@ -7655,7 +7750,7 @@ static int bnx2x_8726_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 		if ((val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK) ==
 		    MDIO_PMA_REG_SFP_TWO_WIRE_STATUS_IDLE)
 			return 0;
-		msleep(1);
+		 usleep_range(1000, 2000);
 	}
 	return -EINVAL;
 }
@@ -7692,7 +7787,8 @@ static int bnx2x_warpcore_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	u32 data_array[4];
 	u16 addr32;
 	struct bnx2x *bp = params->bp;
-	if (byte_cnt > 16) {
+
+	if (byte_cnt > SFP_EEPROM_PAGE_SIZE) {
 		DP(NETIF_MSG_LINK,
 		   "Reading from eeprom is limited to 16 bytes\n");
 		return -EINVAL;
@@ -7728,7 +7824,7 @@ static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 val, i;
 
-	if (byte_cnt > 16) {
+	if (byte_cnt > SFP_EEPROM_PAGE_SIZE) {
 		DP(NETIF_MSG_LINK,
 		   "Reading from eeprom is limited to 0xf\n");
 		return -EINVAL;
@@ -7765,7 +7861,7 @@ static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 	/* Wait appropriate time for two-wire command to finish before
 	 * polling the status register
 	 */
-	msleep(1);
+	 usleep_range(1000, 2000);
 
 	/* Wait up to 500us for command complete status */
 	for (i = 0; i < 100; i++) {
@@ -7801,7 +7897,7 @@ static int bnx2x_8727_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 		if ((val & MDIO_PMA_REG_SFP_TWO_WIRE_CTRL_STATUS_MASK) ==
 		    MDIO_PMA_REG_SFP_TWO_WIRE_STATUS_IDLE)
 			return 0;
-		msleep(1);
+		 usleep_range(1000, 2000);
 	}
 
 	return -EINVAL;
@@ -7811,7 +7907,7 @@ int bnx2x_read_sfp_module_eeprom(struct bnx2x_phy *phy,
 				 struct link_params *params, u16 addr,
 				 u8 byte_cnt, u8 *o_buf)
 {
-	int rc = -EINVAL;
+	int rc = -EOPNOTSUPP;
 	switch (phy->type) {
 	case PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM8726:
 		rc = bnx2x_8726_read_sfp_module_eeprom(phy, params, addr,
@@ -7836,7 +7932,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u32 sync_offset = 0, phy_idx, media_types;
-	u8 val, check_limiting_mode = 0;
+	u8 val[2], check_limiting_mode = 0;
 	*edc_mode = EDC_MODE_LIMITING;
 
 	phy->media_type = ETH_PHY_UNSPECIFIED;
@@ -7844,13 +7940,13 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
 					 SFP_EEPROM_CON_TYPE_ADDR,
-					 1,
-					 &val) != 0) {
+					 2,
+					 (u8 *)val) != 0) {
 		DP(NETIF_MSG_LINK, "Failed to read from SFP+ module EEPROM\n");
 		return -EINVAL;
 	}
 
-	switch (val) {
+	switch (val[0]) {
 	case SFP_EEPROM_CON_TYPE_VAL_COPPER:
 	{
 		u8 copper_module_type;
@@ -7888,13 +7984,29 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		break;
 	}
 	case SFP_EEPROM_CON_TYPE_VAL_LC:
-		phy->media_type = ETH_PHY_SFP_FIBER;
-		DP(NETIF_MSG_LINK, "Optic module detected\n");
 		check_limiting_mode = 1;
+		if ((val[1] & (SFP_EEPROM_COMP_CODE_SR_MASK |
+			       SFP_EEPROM_COMP_CODE_LR_MASK |
+			       SFP_EEPROM_COMP_CODE_LRM_MASK)) == 0) {
+			DP(NETIF_MSG_LINK, "1G Optic module detected\n");
+			phy->media_type = ETH_PHY_SFP_1G_FIBER;
+			phy->req_line_speed = SPEED_1000;
+		} else {
+			int idx, cfg_idx = 0;
+			DP(NETIF_MSG_LINK, "10G Optic module detected\n");
+			for (idx = INT_PHY; idx < MAX_PHYS; idx++) {
+				if (params->phy[idx].type == phy->type) {
+					cfg_idx = LINK_CONFIG_IDX(idx);
+					break;
+				}
+			}
+			phy->media_type = ETH_PHY_SFPP_10G_FIBER;
+			phy->req_line_speed = params->req_line_speed[cfg_idx];
+		}
 		break;
 	default:
 		DP(NETIF_MSG_LINK, "Unable to determine module type 0x%x !!!\n",
-			 val);
+			 val[0]);
 		return -EINVAL;
 	}
 	sync_offset = params->shmem_base +
@@ -7980,7 +8092,7 @@ static int bnx2x_verify_sfp_module(struct bnx2x_phy *phy,
 		return 0;
 	}
 
-	/* format the warning message */
+	/* Format the warning message */
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
 					 SFP_EEPROM_VENDOR_NAME_ADDR,
@@ -8026,7 +8138,7 @@ static int bnx2x_wait_for_sfp_module_initialized(struct bnx2x_phy *phy,
 			   timeout * 5);
 			return 0;
 		}
-		msleep(5);
+		usleep_range(5000, 10000);
 	}
 	return -EINVAL;
 }
@@ -8338,7 +8450,7 @@ int bnx2x_sfp_module_detection(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK, "Failed to get valid module type\n");
 		return -EINVAL;
 	} else if (bnx2x_verify_sfp_module(phy, params) != 0) {
-		/* check SFP+ module compatibility */
+		/* Check SFP+ module compatibility */
 		DP(NETIF_MSG_LINK, "Module verification failed!!\n");
 		rc = -EINVAL;
 		/* Turn on fault module-detected led */
@@ -8401,14 +8513,34 @@ void bnx2x_handle_module_detect_int(struct link_params *params)
 
 	/* Call the handling function in case module is detected */
 	if (gpio_val == 0) {
+		bnx2x_set_mdio_clk(bp, params->chip_id, params->port);
+		bnx2x_set_aer_mmd(params, phy);
+
 		bnx2x_power_sfp_module(params, phy, 1);
 		bnx2x_set_gpio_int(bp, gpio_num,
 				   MISC_REGISTERS_GPIO_INT_OUTPUT_CLR,
 				   gpio_port);
-		if (bnx2x_wait_for_sfp_module_initialized(phy, params) == 0)
+		if (bnx2x_wait_for_sfp_module_initialized(phy, params) == 0) {
 			bnx2x_sfp_module_detection(phy, params);
-		else
+			if (CHIP_IS_E3(bp)) {
+				u16 rx_tx_in_reset;
+				/* In case WC is out of reset, reconfigure the
+				 * link speed while taking into account 1G
+				 * module limitation.
+				 */
+				bnx2x_cl45_read(bp, phy,
+						MDIO_WC_DEVAD,
+						MDIO_WC_REG_DIGITAL5_MISC6,
+						&rx_tx_in_reset);
+				if (!rx_tx_in_reset) {
+					bnx2x_warpcore_reset_lane(bp, phy, 1);
+					bnx2x_warpcore_config_sfi(phy, params);
+					bnx2x_warpcore_reset_lane(bp, phy, 0);
+				}
+			}
+		} else {
 			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.
@@ -8469,7 +8601,7 @@ static u8 bnx2x_8706_8726_read_status(struct bnx2x_phy *phy,
 	bnx2x_sfp_mask_fault(bp, phy, MDIO_PMA_LASI_TXSTAT,
 			     MDIO_PMA_LASI_TXCTRL);
 
-	/* clear LASI indication*/
+	/* Clear LASI indication*/
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD, MDIO_PMA_LASI_STAT, &val1);
 	bnx2x_cl45_read(bp, phy,
@@ -8537,7 +8669,7 @@ static u8 bnx2x_8706_config_init(struct bnx2x_phy *phy,
 				MDIO_PMA_DEVAD, MDIO_PMA_REG_ROM_VER1, &val);
 		if (val)
 			break;
-		msleep(10);
+		usleep_range(10000, 20000);
 	}
 	DP(NETIF_MSG_LINK, "XGXS 8706 is initialized after %d ms\n", cnt);
 	if ((params->feature_config_flags &
@@ -8666,7 +8798,7 @@ static void bnx2x_8726_external_rom_boot(struct bnx2x_phy *phy,
 			 MDIO_PMA_REG_GEN_CTRL,
 			 MDIO_PMA_REG_GEN_CTRL_ROM_RESET_INTERNAL_MP);
 
-	/* wait for 150ms for microcode load */
+	/* Wait for 150ms for microcode load */
 	msleep(150);
 
 	/* Disable serial boot control, tristates pins SS_N, SCK, MOSI, MISO */
@@ -8860,6 +8992,63 @@ static void bnx2x_8727_hw_reset(struct bnx2x_phy *phy,
 		       MISC_REGISTERS_GPIO_OUTPUT_LOW, port);
 }
 
+static void bnx2x_8727_config_speed(struct bnx2x_phy *phy,
+				    struct link_params *params)
+{
+	struct bnx2x *bp = params->bp;
+	u16 tmp1, val;
+	/* Set option 1G speed */
+	if ((phy->req_line_speed == SPEED_1000) ||
+	    (phy->media_type == ETH_PHY_SFP_1G_FIBER)) {
+		DP(NETIF_MSG_LINK, "Setting 1G force\n");
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 0x40);
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_PMA_DEVAD, MDIO_PMA_REG_10G_CTRL2, 0xD);
+		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
+		 */
+		if (DUAL_MEDIA(params)) {
+			bnx2x_cl45_read(bp, phy,
+					MDIO_PMA_DEVAD,
+					MDIO_PMA_REG_8727_PCS_GP, &val);
+			val |= (3<<10);
+			bnx2x_cl45_write(bp, phy,
+					 MDIO_PMA_DEVAD,
+					 MDIO_PMA_REG_8727_PCS_GP, val);
+		}
+	} else if ((phy->req_line_speed == SPEED_AUTO_NEG) &&
+		   ((phy->speed_cap_mask &
+		     PORT_HW_CFG_SPEED_CAPABILITY_D0_1G)) &&
+		   ((phy->speed_cap_mask &
+		      PORT_HW_CFG_SPEED_CAPABILITY_D0_10G) !=
+		   PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)) {
+
+		DP(NETIF_MSG_LINK, "Setting 1G clause37\n");
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_AN_DEVAD, MDIO_AN_REG_8727_MISC_CTRL, 0);
+		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
+		 */
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_AN_DEVAD, MDIO_AN_REG_8727_MISC_CTRL,
+				 0x0020);
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x0100);
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 0x2040);
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_PMA_DEVAD, MDIO_PMA_REG_10G_CTRL2,
+				 0x0008);
+	}
+}
+
 static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 				  struct link_params *params,
 				  struct link_vars *vars)
@@ -8877,7 +9066,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 	lasi_ctrl_val = 0x0006;
 
 	DP(NETIF_MSG_LINK, "Initializing BCM8727\n");
-	/* enable LASI */
+	/* Enable LASI */
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD, MDIO_PMA_LASI_RXCTRL,
 			 rx_alarm_ctrl_val);
@@ -8929,56 +9118,7 @@ static int bnx2x_8727_config_init(struct bnx2x_phy *phy,
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD, MDIO_PMA_LASI_RXSTAT, &tmp1);
 
-	/* Set option 1G speed */
-	if (phy->req_line_speed == SPEED_1000) {
-		DP(NETIF_MSG_LINK, "Setting 1G force\n");
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 0x40);
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_PMA_DEVAD, MDIO_PMA_REG_10G_CTRL2, 0xD);
-		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
-		 */
-		if (DUAL_MEDIA(params)) {
-			bnx2x_cl45_read(bp, phy,
-					MDIO_PMA_DEVAD,
-					MDIO_PMA_REG_8727_PCS_GP, &val);
-			val |= (3<<10);
-			bnx2x_cl45_write(bp, phy,
-					 MDIO_PMA_DEVAD,
-					 MDIO_PMA_REG_8727_PCS_GP, val);
-		}
-	} else if ((phy->req_line_speed == SPEED_AUTO_NEG) &&
-		   ((phy->speed_cap_mask &
-		     PORT_HW_CFG_SPEED_CAPABILITY_D0_1G)) &&
-		   ((phy->speed_cap_mask &
-		      PORT_HW_CFG_SPEED_CAPABILITY_D0_10G) !=
-		   PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)) {
-
-		DP(NETIF_MSG_LINK, "Setting 1G clause37\n");
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_AN_DEVAD, MDIO_AN_REG_8727_MISC_CTRL, 0);
-		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
-		 */
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_AN_DEVAD, MDIO_AN_REG_8727_MISC_CTRL,
-				 0x0020);
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_AN_DEVAD, MDIO_AN_REG_CL37_AN, 0x0100);
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_PMA_DEVAD, MDIO_PMA_REG_CTRL, 0x2040);
-		bnx2x_cl45_write(bp, phy,
-				 MDIO_PMA_DEVAD, MDIO_PMA_REG_10G_CTRL2,
-				 0x0008);
-	}
-
+	bnx2x_8727_config_speed(phy, params);
 	/* Set 2-wire transfer rate of SFP+ module EEPROM
 	 * to 100Khz since some DACs(direct attached cables) do
 	 * not work at 400Khz.
@@ -9105,6 +9245,9 @@ static void bnx2x_8727_handle_mod_abs(struct bnx2x_phy *phy,
 			bnx2x_sfp_module_detection(phy, params);
 		else
 			DP(NETIF_MSG_LINK, "SFP+ module is not initialized\n");
+
+		/* Reconfigure link speed based on module type limitations */
+		bnx2x_8727_config_speed(phy, params);
 	}
 
 	DP(NETIF_MSG_LINK, "8727 RX_ALARM_STATUS 0x%x\n",
@@ -9585,9 +9728,9 @@ static int bnx2x_8481_config_init(struct bnx2x_phy *phy,
 static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 				   struct link_params *params,
 		   u16 fw_cmd,
-		   u16 cmd_args[])
+		   u16 cmd_args[], int argc)
 {
-	u32 idx;
+	int idx;
 	u16 val;
 	struct bnx2x *bp = params->bp;
 	/* Write CMD_OPEN_OVERRIDE to STATUS reg */
@@ -9599,7 +9742,7 @@ static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 				MDIO_84833_CMD_HDLR_STATUS, &val);
 		if (val == PHY84833_STATUS_CMD_OPEN_FOR_CMDS)
 			break;
-		msleep(1);
+		 usleep_range(1000, 2000);
 	}
 	if (idx >= PHY84833_CMDHDLR_WAIT) {
 		DP(NETIF_MSG_LINK, "FW cmd: FW not ready.\n");
@@ -9607,7 +9750,7 @@ static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 	}
 
 	/* Prepare argument(s) and issue command */
-	for (idx = 0; idx < PHY84833_CMDHDLR_MAX_ARGS; idx++) {
+	for (idx = 0; idx < argc; idx++) {
 		bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
 				MDIO_84833_CMD_HDLR_DATA1 + idx,
 				cmd_args[idx]);
@@ -9620,7 +9763,7 @@ static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 		if ((val == PHY84833_STATUS_CMD_COMPLETE_PASS) ||
 			(val == PHY84833_STATUS_CMD_COMPLETE_ERROR))
 			break;
-		msleep(1);
+		 usleep_range(1000, 2000);
 	}
 	if ((idx >= PHY84833_CMDHDLR_WAIT) ||
 		(val == PHY84833_STATUS_CMD_COMPLETE_ERROR)) {
@@ -9628,7 +9771,7 @@ static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 		return -EINVAL;
 	}
 	/* Gather returning data */
-	for (idx = 0; idx < PHY84833_CMDHDLR_MAX_ARGS; idx++) {
+	for (idx = 0; idx < argc; idx++) {
 		bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
 				MDIO_84833_CMD_HDLR_DATA1 + idx,
 				&cmd_args[idx]);
@@ -9662,7 +9805,7 @@ static int bnx2x_84833_pair_swap_cfg(struct bnx2x_phy *phy,
 	data[1] = (u16)pair_swap;
 
 	status = bnx2x_84833_cmd_hdlr(phy, params,
-		PHY84833_CMD_SET_PAIR_SWAP, data);
+		PHY84833_CMD_SET_PAIR_SWAP, data, PHY84833_CMDHDLR_MAX_ARGS);
 	if (status == 0)
 		DP(NETIF_MSG_LINK, "Pairswap OK, val=0x%x\n", data[1]);
 
@@ -9740,6 +9883,95 @@ static int bnx2x_84833_hw_reset_phy(struct bnx2x_phy *phy,
 	return 0;
 }
 
+static int bnx2x_8483x_eee_timers(struct link_params *params,
+				   struct link_vars *vars)
+{
+	u32 eee_idle = 0, eee_mode;
+	struct bnx2x *bp = params->bp;
+
+	eee_idle = bnx2x_eee_calc_timer(params);
+
+	if (eee_idle) {
+		REG_WR(bp, MISC_REG_CPMU_LP_IDLE_THR_P0 + (params->port << 2),
+		       eee_idle);
+	} else if ((params->eee_mode & EEE_MODE_ENABLE_LPI) &&
+		   (params->eee_mode & EEE_MODE_OVERRIDE_NVRAM) &&
+		   (params->eee_mode & EEE_MODE_OUTPUT_TIME)) {
+		DP(NETIF_MSG_LINK, "Error: Tx LPI is enabled with timer 0\n");
+		return -EINVAL;
+	}
+
+	vars->eee_status &= ~(SHMEM_EEE_TIMER_MASK | SHMEM_EEE_TIME_OUTPUT_BIT);
+	if (params->eee_mode & EEE_MODE_OUTPUT_TIME) {
+		/* eee_idle in 1u --> eee_status in 16u */
+		eee_idle >>= 4;
+		vars->eee_status |= (eee_idle & SHMEM_EEE_TIMER_MASK) |
+				    SHMEM_EEE_TIME_OUTPUT_BIT;
+	} else {
+		if (bnx2x_eee_time_to_nvram(eee_idle, &eee_mode))
+			return -EINVAL;
+		vars->eee_status |= eee_mode;
+	}
+
+	return 0;
+}
+
+static int bnx2x_8483x_disable_eee(struct bnx2x_phy *phy,
+				   struct link_params *params,
+				   struct link_vars *vars)
+{
+	int rc;
+	struct bnx2x *bp = params->bp;
+	u16 cmd_args = 0;
+
+	DP(NETIF_MSG_LINK, "Don't Advertise 10GBase-T EEE\n");
+
+	/* Make Certain LPI is disabled */
+	REG_WR(bp, MISC_REG_CPMU_LP_FW_ENABLE_P0 + (params->port << 2), 0);
+	REG_WR(bp, MISC_REG_CPMU_LP_DR_ENABLE, 0);
+
+	/* Prevent Phy from working in EEE and advertising it */
+	rc = bnx2x_84833_cmd_hdlr(phy, params,
+		PHY84833_CMD_SET_EEE_MODE, &cmd_args, 1);
+	if (rc) {
+		DP(NETIF_MSG_LINK, "EEE disable failed.\n");
+		return rc;
+	}
+
+	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_EEE_ADV, 0);
+	vars->eee_status &= ~SHMEM_EEE_ADV_STATUS_MASK;
+
+	return 0;
+}
+
+static int bnx2x_8483x_enable_eee(struct bnx2x_phy *phy,
+				   struct link_params *params,
+				   struct link_vars *vars)
+{
+	int rc;
+	struct bnx2x *bp = params->bp;
+	u16 cmd_args = 1;
+
+	DP(NETIF_MSG_LINK, "Advertise 10GBase-T EEE\n");
+
+	rc = bnx2x_84833_cmd_hdlr(phy, params,
+		PHY84833_CMD_SET_EEE_MODE, &cmd_args, 1);
+	if (rc) {
+		DP(NETIF_MSG_LINK, "EEE enable failed.\n");
+		return rc;
+	}
+
+	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_EEE_ADV, 0x8);
+
+	/* Mask events preventing LPI generation */
+	REG_WR(bp, MISC_REG_CPMU_LP_MASK_EXT_P0 + (params->port << 2), 0xfc20);
+
+	vars->eee_status &= ~SHMEM_EEE_ADV_STATUS_MASK;
+	vars->eee_status |= (SHMEM_EEE_10G_ADV << SHMEM_EEE_ADV_STATUS_SHIFT);
+
+	return 0;
+}
+
 #define PHY84833_CONSTANT_LATENCY 1193
 static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 				   struct link_params *params,
@@ -9752,7 +9984,7 @@ static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 	u16 cmd_args[PHY84833_CMDHDLR_MAX_ARGS];
 	int rc = 0;
 
-	msleep(1);
+	 usleep_range(1000, 2000);
 
 	if (!(CHIP_IS_E1x(bp)))
 		port = BP_PATH(bp);
@@ -9839,8 +10071,9 @@ static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 		cmd_args[2] = PHY84833_CONSTANT_LATENCY + 1;
 		cmd_args[3] = PHY84833_CONSTANT_LATENCY;
 		rc = bnx2x_84833_cmd_hdlr(phy, params,
-			PHY84833_CMD_SET_EEE_MODE, cmd_args);
-		if (rc != 0)
+			PHY84833_CMD_SET_EEE_MODE, cmd_args,
+			PHY84833_CMDHDLR_MAX_ARGS);
+		if (rc)
 			DP(NETIF_MSG_LINK, "Cfg AutogrEEEn failed.\n");
 	}
 	if (initialize)
@@ -9864,6 +10097,48 @@ static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 				 MDIO_CTL_REG_84823_USER_CTRL_REG, val);
 	}
 
+	bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
+			MDIO_84833_TOP_CFG_FW_REV, &val);
+
+	/* Configure EEE support */
+	if ((val >= MDIO_84833_TOP_CFG_FW_EEE) && bnx2x_eee_has_cap(params)) {
+		phy->flags |= FLAGS_EEE_10GBT;
+		vars->eee_status |= SHMEM_EEE_10G_ADV <<
+				    SHMEM_EEE_SUPPORTED_SHIFT;
+		/* Propogate params' bits --> vars (for migration exposure) */
+		if (params->eee_mode & EEE_MODE_ENABLE_LPI)
+			vars->eee_status |= SHMEM_EEE_LPI_REQUESTED_BIT;
+		else
+			vars->eee_status &= ~SHMEM_EEE_LPI_REQUESTED_BIT;
+
+		if (params->eee_mode & EEE_MODE_ADV_LPI)
+			vars->eee_status |= SHMEM_EEE_REQUESTED_BIT;
+		else
+			vars->eee_status &= ~SHMEM_EEE_REQUESTED_BIT;
+
+		rc = bnx2x_8483x_eee_timers(params, vars);
+		if (rc) {
+			DP(NETIF_MSG_LINK, "Failed to configure EEE timers\n");
+			bnx2x_8483x_disable_eee(phy, params, vars);
+			return rc;
+		}
+
+		if ((params->req_duplex[actual_phy_selection] == DUPLEX_FULL) &&
+		    (params->eee_mode & EEE_MODE_ADV_LPI) &&
+		    (bnx2x_eee_calc_timer(params) ||
+		     !(params->eee_mode & EEE_MODE_ENABLE_LPI)))
+			rc = bnx2x_8483x_enable_eee(phy, params, vars);
+		else
+			rc = bnx2x_8483x_disable_eee(phy, params, vars);
+		if (rc) {
+			DP(NETIF_MSG_LINK, "Failed to set EEE advertisment\n");
+			return rc;
+		}
+	} else {
+		phy->flags &= ~FLAGS_EEE_10GBT;
+		vars->eee_status &= ~SHMEM_EEE_SUPPORTED_MASK;
+	}
+
 	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) {
 		/* Bring PHY out of super isolate mode as the final step. */
 		bnx2x_cl45_read(bp, phy,
@@ -9918,17 +10193,19 @@ static u8 bnx2x_848xx_read_status(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK, "Legacy speed status = 0x%x\n",
 		   legacy_status);
 		link_up = ((legacy_status & (1<<11)) == (1<<11));
-		if (link_up) {
-			legacy_speed = (legacy_status & (3<<9));
-			if (legacy_speed == (0<<9))
-				vars->line_speed = SPEED_10;
-			else if (legacy_speed == (1<<9))
-				vars->line_speed = SPEED_100;
-			else if (legacy_speed == (2<<9))
-				vars->line_speed = SPEED_1000;
-			else /* Should not happen */
-				vars->line_speed = 0;
+		legacy_speed = (legacy_status & (3<<9));
+		if (legacy_speed == (0<<9))
+			vars->line_speed = SPEED_10;
+		else if (legacy_speed == (1<<9))
+			vars->line_speed = SPEED_100;
+		else if (legacy_speed == (2<<9))
+			vars->line_speed = SPEED_1000;
+		else { /* Should not happen: Treat as link down */
+			vars->line_speed = 0;
+			link_up = 0;
+		}
 
+		if (link_up) {
 			if (legacy_status & (1<<8))
 				vars->duplex = DUPLEX_FULL;
 			else
@@ -9956,7 +10233,7 @@ static u8 bnx2x_848xx_read_status(struct bnx2x_phy *phy,
 		}
 	}
 	if (link_up) {
-		DP(NETIF_MSG_LINK, "BCM84823: link speed is %d\n",
+		DP(NETIF_MSG_LINK, "BCM848x3: link speed is %d\n",
 			   vars->line_speed);
 		bnx2x_ext_phy_resolve_fc(phy, params, vars);
 
@@ -9995,6 +10272,31 @@ static u8 bnx2x_848xx_read_status(struct bnx2x_phy *phy,
 		if (val & (1<<11))
 			vars->link_status |=
 				LINK_STATUS_LINK_PARTNER_10GXFD_CAPABLE;
+
+		/* Determine if EEE was negotiated */
+		if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) {
+			u32 eee_shmem = 0;
+
+			bnx2x_cl45_read(bp, phy, MDIO_AN_DEVAD,
+					MDIO_AN_REG_EEE_ADV, &val1);
+			bnx2x_cl45_read(bp, phy, MDIO_AN_DEVAD,
+					MDIO_AN_REG_LP_EEE_ADV, &val2);
+			if ((val1 & val2) & 0x8) {
+				DP(NETIF_MSG_LINK, "EEE negotiated\n");
+				vars->eee_status |= SHMEM_EEE_ACTIVE_BIT;
+			}
+
+			if (val2 & 0x12)
+				eee_shmem |= SHMEM_EEE_100M_ADV;
+			if (val2 & 0x4)
+				eee_shmem |= SHMEM_EEE_1G_ADV;
+			if (val2 & 0x68)
+				eee_shmem |= SHMEM_EEE_10G_ADV;
+
+			vars->eee_status &= ~SHMEM_EEE_LP_ADV_STATUS_MASK;
+			vars->eee_status |= (eee_shmem <<
+					     SHMEM_EEE_LP_ADV_STATUS_SHIFT);
+		}
 	}
 
 	return link_up;
@@ -10273,7 +10575,7 @@ static int bnx2x_54618se_config_init(struct bnx2x_phy *phy,
 	u32 cfg_pin;
 
 	DP(NETIF_MSG_LINK, "54618SE cfg init\n");
-	usleep_range(1000, 1000);
+	usleep_range(1000, 2000);
 
 	/* This works with E3 only, no need to check the chip
 	 * before determining the port.
@@ -10342,7 +10644,7 @@ static int bnx2x_54618se_config_init(struct bnx2x_phy *phy,
 			MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH)
 		fc_val |= MDIO_AN_REG_ADV_PAUSE_PAUSE;
 
-	/* read all advertisement */
+	/* Read all advertisement */
 	bnx2x_cl22_read(bp, phy,
 			0x09,
 			&an_1000_val);
@@ -10379,7 +10681,7 @@ static int bnx2x_54618se_config_init(struct bnx2x_phy *phy,
 			0x09,
 			&an_1000_val);
 
-	/* set 100 speed advertisement */
+	/* Set 100 speed advertisement */
 	if (((phy->req_line_speed == SPEED_AUTO_NEG) &&
 			(phy->speed_cap_mask &
 			(PORT_HW_CFG_SPEED_CAPABILITY_D0_100M_FULL |
@@ -10393,7 +10695,7 @@ static int bnx2x_54618se_config_init(struct bnx2x_phy *phy,
 		DP(NETIF_MSG_LINK, "Advertising 100M\n");
 	}
 
-	/* set 10 speed advertisement */
+	/* Set 10 speed advertisement */
 	if (((phy->req_line_speed == SPEED_AUTO_NEG) &&
 			(phy->speed_cap_mask &
 			(PORT_HW_CFG_SPEED_CAPABILITY_D0_10M_FULL |
@@ -10532,7 +10834,7 @@ static u8 bnx2x_54618se_read_status(struct bnx2x_phy *phy,
 
 	/* Get speed operation status */
 	bnx2x_cl22_read(bp, phy,
-			0x19,
+			MDIO_REG_GPHY_AUX_STATUS,
 			&legacy_status);
 	DP(NETIF_MSG_LINK, "54618SE read_status: 0x%x\n", legacy_status);
 
@@ -10759,7 +11061,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,
@@ -10771,7 +11073,7 @@ static u8 bnx2x_7101_read_status(struct bnx2x_phy *phy,
 		bnx2x_ext_phy_10G_an_resolve(bp, phy, vars);
 		bnx2x_ext_phy_resolve_fc(phy, params, vars);
 
-		/* read LP advertised speeds */
+		/* Read LP advertised speeds */
 		if (val2 & (1<<11))
 			vars->link_status |=
 				LINK_STATUS_LINK_PARTNER_10GXFD_CAPABLE;
@@ -11090,7 +11392,7 @@ static struct bnx2x_phy phy_8706 = {
 			   SUPPORTED_FIBRE |
 			   SUPPORTED_Pause |
 			   SUPPORTED_Asym_Pause),
-	.media_type	= ETH_PHY_SFP_FIBER,
+	.media_type	= ETH_PHY_SFPP_10G_FIBER,
 	.ver_addr	= 0,
 	.req_flow_ctrl	= 0,
 	.req_line_speed	= 0,
@@ -11249,7 +11551,8 @@ static struct bnx2x_phy phy_84833 = {
 	.def_md_devad	= 0,
 	.flags		= (FLAGS_FAN_FAILURE_DET_REQ |
 			   FLAGS_REARM_LATCH_SIGNAL |
-			   FLAGS_TX_ERROR_CHECK),
+			   FLAGS_TX_ERROR_CHECK |
+			   FLAGS_EEE_10GBT),
 	.rx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.tx_preemphasis	= {0xffff, 0xffff, 0xffff, 0xffff},
 	.mdio_ctrl	= 0,
@@ -11428,7 +11731,7 @@ static int bnx2x_populate_int_phy(struct bnx2x *bp, u32 shmem_base, u8 port,
 					   SUPPORTED_FIBRE |
 					   SUPPORTED_Pause |
 					   SUPPORTED_Asym_Pause);
-			phy->media_type = ETH_PHY_SFP_FIBER;
+			phy->media_type = ETH_PHY_SFPP_10G_FIBER;
 			break;
 		case PORT_HW_CFG_NET_SERDES_IF_KR:
 			phy->media_type = ETH_PHY_KR;
@@ -11968,7 +12271,7 @@ int bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 	vars->mac_type = MAC_TYPE_NONE;
 	vars->phy_flags = 0;
 
-	/* disable attentions */
+	/* Disable attentions */
 	bnx2x_bits_dis(bp, NIG_REG_MASK_INTERRUPT_PORT0 + params->port*4,
 		       (NIG_MASK_XGXS0_LINK_STATUS |
 			NIG_MASK_XGXS0_LINK10G |
@@ -12017,6 +12320,8 @@ int bnx2x_phy_init(struct link_params *params, struct link_vars *vars)
 		break;
 	}
 	bnx2x_update_mng(params, vars->link_status);
+
+	bnx2x_update_mng_eee(params, vars->eee_status);
 	return 0;
 }
 
@@ -12026,19 +12331,22 @@ int bnx2x_link_reset(struct link_params *params, struct link_vars *vars,
 	struct bnx2x *bp = params->bp;
 	u8 phy_index, port = params->port, clear_latch_ind = 0;
 	DP(NETIF_MSG_LINK, "Resetting the link of port %d\n", port);
-	/* disable attentions */
+	/* Disable attentions */
 	vars->link_status = 0;
 	bnx2x_update_mng(params, vars->link_status);
+	vars->eee_status &= ~(SHMEM_EEE_LP_ADV_STATUS_MASK |
+			      SHMEM_EEE_ACTIVE_BIT);
+	bnx2x_update_mng_eee(params, vars->eee_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));
 
-	/* activate nig drain */
+	/* Activate nig drain */
 	REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + port*4, 1);
 
-	/* disable nig egress interface */
+	/* Disable nig egress interface */
 	if (!CHIP_IS_E3(bp)) {
 		REG_WR(bp, NIG_REG_BMAC0_OUT_EN + port*4, 0);
 		REG_WR(bp, NIG_REG_EGRESS_EMAC0_OUT_EN + port*4, 0);
@@ -12051,15 +12359,15 @@ int bnx2x_link_reset(struct link_params *params, struct link_vars *vars,
 		bnx2x_xmac_disable(params);
 		bnx2x_umac_disable(params);
 	}
-	/* disable emac */
+	/* Disable emac */
 	if (!CHIP_IS_E3(bp))
 		REG_WR(bp, NIG_REG_NIG_EMAC0_EN + port*4, 0);
 
-	msleep(10);
+	usleep_range(10000, 20000);
 	/* The PHY reset is controlled by GPIO 1
 	 * Hold it as vars low
 	 */
-	 /* clear link led */
+	 /* Clear link led */
 	bnx2x_set_mdio_clk(bp, params->chip_id, port);
 	bnx2x_set_led(params, vars, LED_MODE_OFF, 0);
 
@@ -12089,9 +12397,9 @@ int bnx2x_link_reset(struct link_params *params, struct link_vars *vars,
 		params->phy[INT_PHY].link_reset(
 			&params->phy[INT_PHY], params);
 
-	/* disable nig ingress interface */
+	/* Disable nig ingress interface */
 	if (!CHIP_IS_E3(bp)) {
-		/* reset BigMac */
+		/* Reset BigMac */
 		REG_WR(bp, GRCBASE_MISC + MISC_REGISTERS_RESET_REG_2_CLEAR,
 		       (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << port));
 		REG_WR(bp, NIG_REG_BMAC0_IN_EN + port*4, 0);
@@ -12148,7 +12456,7 @@ static int bnx2x_8073_common_init_phy(struct bnx2x *bp,
 			DP(NETIF_MSG_LINK, "populate_phy failed\n");
 			return -EINVAL;
 		}
-		/* disable attentions */
+		/* Disable attentions */
 		bnx2x_bits_dis(bp, NIG_REG_MASK_INTERRUPT_PORT0 +
 			       port_of_path*4,
 			       (NIG_MASK_XGXS0_LINK_STATUS |
@@ -12222,7 +12530,7 @@ static int bnx2x_8073_common_init_phy(struct bnx2x *bp,
 		bnx2x_cl45_write(bp, phy_blk[port],
 				MDIO_PMA_DEVAD,
 				MDIO_PMA_REG_TX_POWER_DOWN, (val & (~(1<<10))));
-		msleep(15);
+		usleep_range(15000, 30000);
 
 		/* Read modify write the SPI-ROM version select register */
 		bnx2x_cl45_read(bp, phy_blk[port],
@@ -12254,7 +12562,7 @@ static int bnx2x_8726_common_init_phy(struct bnx2x *bp,
 	REG_WR(bp, MISC_REG_GPIO_EVENT_EN, val);
 
 	bnx2x_ext_phy_hw_reset(bp, 0);
-	msleep(5);
+	usleep_range(5000, 10000);
 	for (port = 0; port < PORT_MAX; port++) {
 		u32 shmem_base, shmem2_base;
 
@@ -12361,11 +12669,11 @@ static int bnx2x_8727_common_init_phy(struct bnx2x *bp,
 	/* Initiate PHY reset*/
 	bnx2x_set_gpio(bp, reset_gpio, MISC_REGISTERS_GPIO_OUTPUT_LOW,
 		       port);
-	msleep(1);
+	 usleep_range(1000, 2000);
 	bnx2x_set_gpio(bp, reset_gpio, MISC_REGISTERS_GPIO_OUTPUT_HIGH,
 		       port);
 
-	msleep(5);
+	usleep_range(5000, 10000);
 
 	/* PART1 - Reset both phys */
 	for (port = PORT_MAX - 1; port >= PORT_0; port--) {
@@ -12459,7 +12767,7 @@ static int bnx2x_84833_pre_init_phy(struct bnx2x *bp,
 				MDIO_PMA_REG_CTRL, &val);
 		if (!(val & (1<<15)))
 			break;
-		msleep(1);
+		 usleep_range(1000, 2000);
 	}
 	if (cnt >= 1500) {
 		DP(NETIF_MSG_LINK, "84833 reset timeout\n");
@@ -12549,7 +12857,7 @@ static int bnx2x_ext_phy_common_init(struct bnx2x *bp, u32 shmem_base_path[],
 		break;
 	}
 
-	if (rc != 0)
+	if (rc)
 		netdev_err(bp->dev,  "Warning: PHY was not initialized,"
 				      " Port %d\n",
 			 0);
@@ -12630,30 +12938,41 @@ static void bnx2x_check_over_curr(struct link_params *params,
 		vars->phy_flags &= ~PHY_OVER_CURRENT_FLAG;
 }
 
-static void bnx2x_analyze_link_error(struct link_params *params,
-				     struct link_vars *vars, u32 lss_status,
-				     u8 notify)
+/* Returns 0 if no change occured since last check; 1 otherwise. */
+static u8 bnx2x_analyze_link_error(struct link_params *params,
+				    struct link_vars *vars, u32 status,
+				    u32 phy_flag, u32 link_flag, u8 notify)
 {
 	struct bnx2x *bp = params->bp;
 	/* Compare new value with previous value */
 	u8 led_mode;
-	u32 half_open_conn = (vars->phy_flags & PHY_HALF_OPEN_CONN_FLAG) > 0;
+	u32 old_status = (vars->phy_flags & phy_flag) ? 1 : 0;
 
-	if ((lss_status ^ half_open_conn) == 0)
-		return;
+	if ((status ^ old_status) == 0)
+		return 0;
 
 	/* If values differ */
-	DP(NETIF_MSG_LINK, "Link changed:%x %x->%x\n", vars->link_up,
-		       half_open_conn, lss_status);
+	switch (phy_flag) {
+	case PHY_HALF_OPEN_CONN_FLAG:
+		DP(NETIF_MSG_LINK, "Analyze Remote Fault\n");
+		break;
+	case PHY_SFP_TX_FAULT_FLAG:
+		DP(NETIF_MSG_LINK, "Analyze TX Fault\n");
+		break;
+	default:
+		DP(NETIF_MSG_LINK, "Analyze UNKOWN\n");
+	}
+	DP(NETIF_MSG_LINK, "Link changed:[%x %x]->%x\n", vars->link_up,
+	   old_status, status);
 
 	/* a. Update shmem->link_status accordingly
 	 * b. Update link_vars->link_up
 	 */
-	if (lss_status) {
-		DP(NETIF_MSG_LINK, "Remote Fault detected !!!\n");
+	if (status) {
 		vars->link_status &= ~LINK_STATUS_LINK_UP;
+		vars->link_status |= link_flag;
 		vars->link_up = 0;
-		vars->phy_flags |= PHY_HALF_OPEN_CONN_FLAG;
+		vars->phy_flags |= phy_flag;
 
 		/* activate nig drain */
 		REG_WR(bp, NIG_REG_EGRESS_DRAIN0_MODE + params->port*4, 1);
@@ -12662,10 +12981,10 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 		 */
 		led_mode = LED_MODE_OFF;
 	} else {
-		DP(NETIF_MSG_LINK, "Remote Fault cleared\n");
 		vars->link_status |= LINK_STATUS_LINK_UP;
+		vars->link_status &= ~link_flag;
 		vars->link_up = 1;
-		vars->phy_flags &= ~PHY_HALF_OPEN_CONN_FLAG;
+		vars->phy_flags &= ~phy_flag;
 		led_mode = LED_MODE_OPER;
 
 		/* Clear nig drain */
@@ -12682,6 +13001,8 @@ static void bnx2x_analyze_link_error(struct link_params *params,
 	vars->periodic_flags |= PERIODIC_FLAGS_LINK_EVENT;
 	if (notify)
 		bnx2x_notify_link_changed(bp);
+
+	return 1;
 }
 
 /******************************************************************************
@@ -12723,7 +13044,9 @@ int bnx2x_check_half_open_conn(struct link_params *params,
 		if (REG_RD(bp, mac_base + XMAC_REG_RX_LSS_STATUS))
 			lss_status = 1;
 
-		bnx2x_analyze_link_error(params, vars, lss_status, notify);
+		bnx2x_analyze_link_error(params, vars, lss_status,
+					 PHY_HALF_OPEN_CONN_FLAG,
+					 LINK_STATUS_NONE, notify);
 	} else if (REG_RD(bp, MISC_REG_RESET_REG_2) &
 		   (MISC_REGISTERS_RESET_REG_2_RST_BMAC0 << params->port)) {
 		/* Check E1X / E2 BMAC */
@@ -12740,11 +13063,55 @@ int bnx2x_check_half_open_conn(struct link_params *params,
 		REG_RD_DMAE(bp, mac_base + lss_status_reg, wb_data, 2);
 		lss_status = (wb_data[0] > 0);
 
-		bnx2x_analyze_link_error(params, vars, lss_status, notify);
+		bnx2x_analyze_link_error(params, vars, lss_status,
+					 PHY_HALF_OPEN_CONN_FLAG,
+					 LINK_STATUS_NONE, notify);
 	}
 	return 0;
 }
+static void bnx2x_sfp_tx_fault_detection(struct bnx2x_phy *phy,
+					 struct link_params *params,
+					 struct link_vars *vars)
+{
+	struct bnx2x *bp = params->bp;
+	u32 cfg_pin, value = 0;
+	u8 led_change, port = params->port;
 
+	/* Get The SFP+ TX_Fault controlling pin ([eg]pio) */
+	cfg_pin = (REG_RD(bp, params->shmem_base + offsetof(struct shmem_region,
+			  dev_info.port_hw_config[port].e3_cmn_pin_cfg)) &
+		   PORT_HW_CFG_E3_TX_FAULT_MASK) >>
+		  PORT_HW_CFG_E3_TX_FAULT_SHIFT;
+
+	if (bnx2x_get_cfg_pin(bp, cfg_pin, &value)) {
+		DP(NETIF_MSG_LINK, "Failed to read pin 0x%02x\n", cfg_pin);
+		return;
+	}
+
+	led_change = bnx2x_analyze_link_error(params, vars, value,
+					      PHY_SFP_TX_FAULT_FLAG,
+					      LINK_STATUS_SFP_TX_FAULT, 1);
+
+	if (led_change) {
+		/* Change TX_Fault led, set link status for further syncs */
+		u8 led_mode;
+
+		if (vars->phy_flags & PHY_SFP_TX_FAULT_FLAG) {
+			led_mode = MISC_REGISTERS_GPIO_HIGH;
+			vars->link_status |= LINK_STATUS_SFP_TX_FAULT;
+		} else {
+			led_mode = MISC_REGISTERS_GPIO_LOW;
+			vars->link_status &= ~LINK_STATUS_SFP_TX_FAULT;
+		}
+
+		/* If module is unapproved, led should be on regardless */
+		if (!(phy->flags & FLAGS_SFP_NOT_APPROVED)) {
+			DP(NETIF_MSG_LINK, "Change TX_Fault LED: ->%x\n",
+			   led_mode);
+			bnx2x_set_e3_module_fault_led(params, led_mode);
+		}
+	}
+}
 void bnx2x_period_func(struct link_params *params, struct link_vars *vars)
 {
 	u16 phy_idx;
@@ -12763,7 +13130,26 @@ void bnx2x_period_func(struct link_params *params, struct link_vars *vars)
 		struct bnx2x_phy *phy = &params->phy[INT_PHY];
 		bnx2x_set_aer_mmd(params, phy);
 		bnx2x_check_over_curr(params, vars);
-		bnx2x_warpcore_config_runtime(phy, params, vars);
+		if (vars->rx_tx_asic_rst)
+			bnx2x_warpcore_config_runtime(phy, params, vars);
+
+		if ((REG_RD(bp, params->shmem_base +
+			    offsetof(struct shmem_region, dev_info.
+				port_hw_config[params->port].default_cfg))
+		    & PORT_HW_CFG_NET_SERDES_IF_MASK) ==
+		    PORT_HW_CFG_NET_SERDES_IF_SFI) {
+			if (bnx2x_is_sfp_module_plugged(phy, params)) {
+				bnx2x_sfp_tx_fault_detection(phy, params, vars);
+			} else if (vars->link_status &
+				LINK_STATUS_SFP_TX_FAULT) {
+				/* Clean trail, interrupt corrects the leds */
+				vars->link_status &= ~LINK_STATUS_SFP_TX_FAULT;
+				vars->phy_flags &= ~PHY_SFP_TX_FAULT_FLAG;
+				/* Update link status in the shared memory */
+				bnx2x_update_mng(params, vars->link_status);
+			}
+		}
+
 	}
 
 }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
index ea4371f4335..51cac813005 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
@@ -41,6 +41,7 @@
 #define SPEED_AUTO_NEG		0
 #define SPEED_20000		20000
 
+#define SFP_EEPROM_PAGE_SIZE			16
 #define SFP_EEPROM_VENDOR_NAME_ADDR		0x14
 #define SFP_EEPROM_VENDOR_NAME_SIZE		16
 #define SFP_EEPROM_VENDOR_OUI_ADDR		0x25
@@ -125,6 +126,11 @@ typedef void (*set_link_led_t)(struct bnx2x_phy *phy,
 			       struct link_params *params, u8 mode);
 typedef void (*phy_specific_func_t)(struct bnx2x_phy *phy,
 				    struct link_params *params, u32 action);
+struct bnx2x_reg_set {
+	u8  devad;
+	u16 reg;
+	u16 val;
+};
 
 struct bnx2x_phy {
 	u32 type;
@@ -149,6 +155,7 @@ struct bnx2x_phy {
 #define FLAGS_DUMMY_READ		(1<<9)
 #define FLAGS_MDC_MDIO_WA_B0		(1<<10)
 #define FLAGS_TX_ERROR_CHECK		(1<<12)
+#define FLAGS_EEE_10GBT			(1<<13)
 
 	/* preemphasis values for the rx side */
 	u16 rx_preemphasis[4];
@@ -162,14 +169,15 @@ struct bnx2x_phy {
 	u32 supported;
 
 	u32 media_type;
-#define	ETH_PHY_UNSPECIFIED 0x0
-#define	ETH_PHY_SFP_FIBER   0x1
-#define	ETH_PHY_XFP_FIBER   0x2
-#define	ETH_PHY_DA_TWINAX   0x3
-#define	ETH_PHY_BASE_T      0x4
-#define	ETH_PHY_KR          0xf0
-#define	ETH_PHY_CX4         0xf1
-#define	ETH_PHY_NOT_PRESENT 0xff
+#define	ETH_PHY_UNSPECIFIED	0x0
+#define	ETH_PHY_SFPP_10G_FIBER	0x1
+#define	ETH_PHY_XFP_FIBER		0x2
+#define	ETH_PHY_DA_TWINAX		0x3
+#define	ETH_PHY_BASE_T		0x4
+#define	ETH_PHY_SFP_1G_FIBER	0x5
+#define	ETH_PHY_KR		0xf0
+#define	ETH_PHY_CX4		0xf1
+#define	ETH_PHY_NOT_PRESENT	0xff
 
 	/* The address in which version is located*/
 	u32 ver_addr;
@@ -265,6 +273,30 @@ struct link_params {
 	u8 num_phys;
 
 	u8 rsrv;
+
+	/* Used to configure the EEE Tx LPI timer, has several modes of
+	 * operation, according to bits 29:28 -
+	 * 2'b00: Timer will be configured by nvram, output will be the value
+	 *        from nvram.
+	 * 2'b01: Timer will be configured by nvram, output will be in
+	 *        microseconds.
+	 * 2'b10: bits 1:0 contain an nvram value which will be used instead
+	 *        of the one located in the nvram. Output will be that value.
+	 * 2'b11: bits 19:0 contain the idle timer in microseconds; output
+	 *        will be in microseconds.
+	 * Bits 31:30 should be 2'b11 in order for EEE to be enabled.
+	 */
+	u32 eee_mode;
+#define EEE_MODE_NVRAM_BALANCED_TIME		(0xa00)
+#define EEE_MODE_NVRAM_AGGRESSIVE_TIME		(0x100)
+#define EEE_MODE_NVRAM_LATENCY_TIME		(0x6000)
+#define EEE_MODE_NVRAM_MASK		(0x3)
+#define EEE_MODE_TIMER_MASK		(0xfffff)
+#define EEE_MODE_OUTPUT_TIME		(1<<28)
+#define EEE_MODE_OVERRIDE_NVRAM		(1<<29)
+#define EEE_MODE_ENABLE_LPI		(1<<30)
+#define EEE_MODE_ADV_LPI			(1<<31)
+
 	u16 hw_led_mode; /* part of the hw_config read from the shmem */
 	u32 multi_phy_config;
 
@@ -282,6 +314,7 @@ struct link_vars {
 #define PHY_PHYSICAL_LINK_FLAG		(1<<2)
 #define PHY_HALF_OPEN_CONN_FLAG		(1<<3)
 #define PHY_OVER_CURRENT_FLAG		(1<<4)
+#define PHY_SFP_TX_FAULT_FLAG		(1<<5)
 
 	u8 mac_type;
 #define MAC_TYPE_NONE		0
@@ -301,6 +334,7 @@ struct link_vars {
 
 	/* The same definitions as the shmem parameter */
 	u32 link_status;
+	u32 eee_status;
 	u8 fault_detected;
 	u8 rsrv1;
 	u16 periodic_flags;
@@ -459,8 +493,7 @@ struct bnx2x_ets_params {
 	struct bnx2x_ets_cos_params cos[DCBX_MAX_NUM_COS];
 };
 
-/**
- * Used to update the PFC attributes in EMAC, BMAC, NIG and BRB
+/* Used to update the PFC attributes in EMAC, BMAC, NIG and BRB
  * when link is already up
  */
 int bnx2x_update_pfc(struct link_params *params,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index f755a665dab..dd451c3dd83 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -74,6 +74,8 @@
 #define FW_FILE_NAME_E1H	"bnx2x/bnx2x-e1h-" FW_FILE_VERSION ".fw"
 #define FW_FILE_NAME_E2		"bnx2x/bnx2x-e2-" FW_FILE_VERSION ".fw"
 
+#define MAC_LEADING_ZERO_CNT (ALIGN(ETH_ALEN, sizeof(u32)) - ETH_ALEN)
+
 /* Time in jiffies before concluding the transmitter is hung */
 #define TX_TIMEOUT		(5*HZ)
 
@@ -104,7 +106,7 @@ MODULE_PARM_DESC(disable_tpa, " Disable the TPA (LRO) feature");
 
 #define INT_MODE_INTx			1
 #define INT_MODE_MSI			2
-static int int_mode;
+int int_mode;
 module_param(int_mode, int, 0);
 MODULE_PARM_DESC(int_mode, " Force interrupt mode other than MSI-X "
 				"(1 INT#x; 2 MSI)");
@@ -135,7 +137,10 @@ enum bnx2x_board_type {
 	BCM57800_MF,
 	BCM57810,
 	BCM57810_MF,
-	BCM57840,
+	BCM57840_O,
+	BCM57840_4_10,
+	BCM57840_2_20,
+	BCM57840_MFO,
 	BCM57840_MF,
 	BCM57811,
 	BCM57811_MF
@@ -155,6 +160,9 @@ static struct {
 	{ "Broadcom NetXtreme II BCM57810 10 Gigabit Ethernet" },
 	{ "Broadcom NetXtreme II BCM57810 10 Gigabit Ethernet Multi Function" },
 	{ "Broadcom NetXtreme II BCM57840 10/20 Gigabit Ethernet" },
+	{ "Broadcom NetXtreme II BCM57840 10 Gigabit Ethernet" },
+	{ "Broadcom NetXtreme II BCM57840 20 Gigabit Ethernet" },
+	{ "Broadcom NetXtreme II BCM57840 10/20 Gigabit Ethernet Multi Function"},
 	{ "Broadcom NetXtreme II BCM57840 10/20 Gigabit Ethernet Multi Function"},
 	{ "Broadcom NetXtreme II BCM57811 10 Gigabit Ethernet"},
 	{ "Broadcom NetXtreme II BCM57811 10 Gigabit Ethernet Multi Function"},
@@ -187,8 +195,17 @@ static struct {
 #ifndef PCI_DEVICE_ID_NX2_57810_MF
 #define PCI_DEVICE_ID_NX2_57810_MF	CHIP_NUM_57810_MF
 #endif
-#ifndef PCI_DEVICE_ID_NX2_57840
-#define PCI_DEVICE_ID_NX2_57840		CHIP_NUM_57840
+#ifndef PCI_DEVICE_ID_NX2_57840_O
+#define PCI_DEVICE_ID_NX2_57840_O	CHIP_NUM_57840_OBSOLETE
+#endif
+#ifndef PCI_DEVICE_ID_NX2_57840_4_10
+#define PCI_DEVICE_ID_NX2_57840_4_10	CHIP_NUM_57840_4_10
+#endif
+#ifndef PCI_DEVICE_ID_NX2_57840_2_20
+#define PCI_DEVICE_ID_NX2_57840_2_20	CHIP_NUM_57840_2_20
+#endif
+#ifndef PCI_DEVICE_ID_NX2_57840_MFO
+#define PCI_DEVICE_ID_NX2_57840_MFO	CHIP_NUM_57840_MF_OBSOLETE
 #endif
 #ifndef PCI_DEVICE_ID_NX2_57840_MF
 #define PCI_DEVICE_ID_NX2_57840_MF	CHIP_NUM_57840_MF
@@ -209,7 +226,10 @@ static DEFINE_PCI_DEVICE_TABLE(bnx2x_pci_tbl) = {
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57800_MF), BCM57800_MF },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57810), BCM57810 },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57810_MF), BCM57810_MF },
-	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840), BCM57840 },
+	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840_O), BCM57840_O },
+	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840_4_10), BCM57840_4_10 },
+	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840_2_20), BCM57840_2_20 },
+	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840_MFO), BCM57840_MFO },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57840_MF), BCM57840_MF },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57811), BCM57811 },
 	{ PCI_VDEVICE(BROADCOM, PCI_DEVICE_ID_NX2_57811_MF), BCM57811_MF },
@@ -758,7 +778,7 @@ void bnx2x_panic_dump(struct bnx2x *bp)
 		/* Tx */
 		for_each_cos_in_tx_queue(fp, cos)
 		{
-			txdata = fp->txdata[cos];
+			txdata = *fp->txdata_ptr[cos];
 			BNX2X_ERR("fp%d: tx_pkt_prod(0x%x)  tx_pkt_cons(0x%x)  tx_bd_prod(0x%x)  tx_bd_cons(0x%x)  *tx_cons_sb(0x%x)\n",
 				  i, txdata.tx_pkt_prod,
 				  txdata.tx_pkt_cons, txdata.tx_bd_prod,
@@ -876,7 +896,7 @@ void bnx2x_panic_dump(struct bnx2x *bp)
 	for_each_tx_queue(bp, i) {
 		struct bnx2x_fastpath *fp = &bp->fp[i];
 		for_each_cos_in_tx_queue(fp, cos) {
-			struct bnx2x_fp_txdata *txdata = &fp->txdata[cos];
+			struct bnx2x_fp_txdata *txdata = fp->txdata_ptr[cos];
 
 			start = TX_BD(le16_to_cpu(*txdata->tx_cons_sb) - 10);
 			end = TX_BD(le16_to_cpu(*txdata->tx_cons_sb) + 245);
@@ -1583,7 +1603,7 @@ void bnx2x_sp_event(struct bnx2x_fastpath *fp, union eth_rx_cqe *rr_cqe)
 	int cid = SW_CID(rr_cqe->ramrod_cqe.conn_and_cmd_data);
 	int command = CQE_CMD(rr_cqe->ramrod_cqe.conn_and_cmd_data);
 	enum bnx2x_queue_cmd drv_cmd = BNX2X_Q_CMD_MAX;
-	struct bnx2x_queue_sp_obj *q_obj = &fp->q_obj;
+	struct bnx2x_queue_sp_obj *q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
 
 	DP(BNX2X_MSG_SP,
 	   "fp %d  cid %d  got ramrod #%d  state is %x  type is %d\n",
@@ -1710,7 +1730,7 @@ irqreturn_t bnx2x_interrupt(int irq, void *dev_instance)
 			/* Handle Rx or Tx according to SB id */
 			prefetch(fp->rx_cons_sb);
 			for_each_cos_in_tx_queue(fp, cos)
-				prefetch(fp->txdata[cos].tx_cons_sb);
+				prefetch(fp->txdata_ptr[cos]->tx_cons_sb);
 			prefetch(&fp->sb_running_index[SM_RX_ID]);
 			napi_schedule(&bnx2x_fp(bp, fp->index, napi));
 			status &= ~mask;
@@ -2124,6 +2144,11 @@ u8 bnx2x_initial_phy_init(struct bnx2x *bp, int load_mode)
 			}
 		}
 
+		if (load_mode == LOAD_LOOPBACK_EXT) {
+			struct link_params *lp = &bp->link_params;
+			lp->loopback_mode = LOOPBACK_EXT;
+		}
+
 		rc = bnx2x_phy_init(&bp->link_params, &bp->link_vars);
 
 		bnx2x_release_phy_lock(bp);
@@ -2916,7 +2941,7 @@ static void bnx2x_pf_tx_q_prep(struct bnx2x *bp,
 	struct bnx2x_fastpath *fp, struct bnx2x_txq_setup_params *txq_init,
 	u8 cos)
 {
-	txq_init->dscr_map = fp->txdata[cos].tx_desc_mapping;
+	txq_init->dscr_map = fp->txdata_ptr[cos]->tx_desc_mapping;
 	txq_init->sb_cq_index = HC_INDEX_ETH_FIRST_TX_CQ_CONS + cos;
 	txq_init->traffic_type = LLFC_TRAFFIC_TYPE_NW;
 	txq_init->fw_sb_id = fp->fw_sb_id;
@@ -3030,9 +3055,9 @@ static void bnx2x_drv_info_ether_stat(struct bnx2x *bp)
 	memcpy(ether_stat->version, DRV_MODULE_VERSION,
 	       ETH_STAT_INFO_VERSION_LEN - 1);
 
-	bp->fp[0].mac_obj.get_n_elements(bp, &bp->fp[0].mac_obj,
-					 DRV_INFO_ETH_STAT_NUM_MACS_REQUIRED,
-					 ether_stat->mac_local);
+	bp->sp_objs[0].mac_obj.get_n_elements(bp, &bp->sp_objs[0].mac_obj,
+					DRV_INFO_ETH_STAT_NUM_MACS_REQUIRED,
+					ether_stat->mac_local);
 
 	ether_stat->mtu_size = bp->dev->mtu;
 
@@ -3055,7 +3080,8 @@ static void bnx2x_drv_info_fcoe_stat(struct bnx2x *bp)
 	struct fcoe_stats_info *fcoe_stat =
 		&bp->slowpath->drv_info_to_mcp.fcoe_stat;
 
-	memcpy(fcoe_stat->mac_local, bp->fip_mac, ETH_ALEN);
+	memcpy(fcoe_stat->mac_local + MAC_LEADING_ZERO_CNT,
+	       bp->fip_mac, ETH_ALEN);
 
 	fcoe_stat->qos_priority =
 		app->traffic_type_priority[LLFC_TRAFFIC_TYPE_FCOE];
@@ -3063,11 +3089,11 @@ static void bnx2x_drv_info_fcoe_stat(struct bnx2x *bp)
 	/* insert FCoE stats from ramrod response */
 	if (!NO_FCOE(bp)) {
 		struct tstorm_per_queue_stats *fcoe_q_tstorm_stats =
-			&bp->fw_stats_data->queue_stats[FCOE_IDX].
+			&bp->fw_stats_data->queue_stats[FCOE_IDX(bp)].
 			tstorm_queue_statistics;
 
 		struct xstorm_per_queue_stats *fcoe_q_xstorm_stats =
-			&bp->fw_stats_data->queue_stats[FCOE_IDX].
+			&bp->fw_stats_data->queue_stats[FCOE_IDX(bp)].
 			xstorm_queue_statistics;
 
 		struct fcoe_statistics_params *fw_fcoe_stat =
@@ -3146,7 +3172,8 @@ static void bnx2x_drv_info_iscsi_stat(struct bnx2x *bp)
 	struct iscsi_stats_info *iscsi_stat =
 		&bp->slowpath->drv_info_to_mcp.iscsi_stat;
 
-	memcpy(iscsi_stat->mac_local, bp->cnic_eth_dev.iscsi_mac, ETH_ALEN);
+	memcpy(iscsi_stat->mac_local + MAC_LEADING_ZERO_CNT,
+	       bp->cnic_eth_dev.iscsi_mac, ETH_ALEN);
 
 	iscsi_stat->qos_priority =
 		app->traffic_type_priority[LLFC_TRAFFIC_TYPE_ISCSI];
@@ -3176,6 +3203,12 @@ static void bnx2x_set_mf_bw(struct bnx2x *bp)
 	bnx2x_fw_command(bp, DRV_MSG_CODE_SET_MF_BW_ACK, 0);
 }
 
+static void bnx2x_handle_eee_event(struct bnx2x *bp)
+{
+	DP(BNX2X_MSG_MCP, "EEE - LLDP event\n");
+	bnx2x_fw_command(bp, DRV_MSG_CODE_EEE_RESULTS_ACK, 0);
+}
+
 static void bnx2x_handle_drv_info_req(struct bnx2x *bp)
 {
 	enum drv_info_opcode op_code;
@@ -3742,6 +3775,8 @@ static void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
 			if (val & DRV_STATUS_AFEX_EVENT_MASK)
 				bnx2x_handle_afex_cmd(bp,
 					val & DRV_STATUS_AFEX_EVENT_MASK);
+			if (val & DRV_STATUS_EEE_NEGOTIATION_RESULTS)
+				bnx2x_handle_eee_event(bp);
 			if (bp->link_vars.periodic_flags &
 			    PERIODIC_FLAGS_LINK_EVENT) {
 				/*  sync with link */
@@ -4615,11 +4650,11 @@ static void bnx2x_handle_classification_eqe(struct bnx2x *bp,
 	case BNX2X_FILTER_MAC_PENDING:
 		DP(BNX2X_MSG_SP, "Got SETUP_MAC completions\n");
 #ifdef BCM_CNIC
-		if (cid == BNX2X_ISCSI_ETH_CID)
+		if (cid == BNX2X_ISCSI_ETH_CID(bp))
 			vlan_mac_obj = &bp->iscsi_l2_mac_obj;
 		else
 #endif
-			vlan_mac_obj = &bp->fp[cid].mac_obj;
+			vlan_mac_obj = &bp->sp_objs[cid].mac_obj;
 
 		break;
 	case BNX2X_FILTER_MCAST_PENDING:
@@ -4717,7 +4752,7 @@ static void bnx2x_after_function_update(struct bnx2x *bp)
 	for_each_eth_queue(bp, q) {
 		/* Set the appropriate Queue object */
 		fp = &bp->fp[q];
-		queue_params.q_obj = &fp->q_obj;
+		queue_params.q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
 
 		/* send the ramrod */
 		rc = bnx2x_queue_state_change(bp, &queue_params);
@@ -4728,8 +4763,8 @@ static void bnx2x_after_function_update(struct bnx2x *bp)
 
 #ifdef BCM_CNIC
 	if (!NO_FCOE(bp)) {
-		fp = &bp->fp[FCOE_IDX];
-		queue_params.q_obj = &fp->q_obj;
+		fp = &bp->fp[FCOE_IDX(bp)];
+		queue_params.q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
 
 		/* clear pending completion bit */
 		__clear_bit(RAMROD_COMP_WAIT, &queue_params.ramrod_flags);
@@ -4761,11 +4796,11 @@ static struct bnx2x_queue_sp_obj *bnx2x_cid_to_q_obj(
 {
 	DP(BNX2X_MSG_SP, "retrieving fp from cid %d\n", cid);
 #ifdef BCM_CNIC
-	if (cid == BNX2X_FCOE_ETH_CID)
-		return &bnx2x_fcoe(bp, q_obj);
+	if (cid == BNX2X_FCOE_ETH_CID(bp))
+		return &bnx2x_fcoe_sp_obj(bp, q_obj);
 	else
 #endif
-		return &bnx2x_fp(bp, CID_TO_FP(cid), q_obj);
+		return &bp->sp_objs[CID_TO_FP(cid, bp)].q_obj;
 }
 
 static void bnx2x_eq_int(struct bnx2x *bp)
@@ -5647,15 +5682,15 @@ static void bnx2x_init_eth_fp(struct bnx2x *bp, int fp_idx)
 
 	/* init tx data */
 	for_each_cos_in_tx_queue(fp, cos) {
-		bnx2x_init_txdata(bp, &fp->txdata[cos],
-				  CID_COS_TO_TX_ONLY_CID(fp->cid, cos),
-				  FP_COS_TO_TXQ(fp, cos),
-				  BNX2X_TX_SB_INDEX_BASE + cos);
-		cids[cos] = fp->txdata[cos].cid;
+		bnx2x_init_txdata(bp, fp->txdata_ptr[cos],
+				  CID_COS_TO_TX_ONLY_CID(fp->cid, cos, bp),
+				  FP_COS_TO_TXQ(fp, cos, bp),
+				  BNX2X_TX_SB_INDEX_BASE + cos, fp);
+		cids[cos] = fp->txdata_ptr[cos]->cid;
 	}
 
-	bnx2x_init_queue_obj(bp, &fp->q_obj, fp->cl_id, cids, fp->max_cos,
-			     BP_FUNC(bp), bnx2x_sp(bp, q_rdata),
+	bnx2x_init_queue_obj(bp, &bnx2x_sp_obj(bp, fp).q_obj, fp->cl_id, cids,
+			     fp->max_cos, BP_FUNC(bp), bnx2x_sp(bp, q_rdata),
 			     bnx2x_sp_mapping(bp, q_rdata), q_type);
 
 	/**
@@ -5706,7 +5741,7 @@ static void bnx2x_init_tx_rings(struct bnx2x *bp)
 
 	for_each_tx_queue(bp, i)
 		for_each_cos_in_tx_queue(&bp->fp[i], cos)
-			bnx2x_init_tx_ring_one(&bp->fp[i].txdata[cos]);
+			bnx2x_init_tx_ring_one(bp->fp[i].txdata_ptr[cos]);
 }
 
 void bnx2x_nic_init(struct bnx2x *bp, u32 load_code)
@@ -7055,12 +7090,10 @@ static int bnx2x_init_hw_func(struct bnx2x *bp)
 	cdu_ilt_start = ilt->clients[ILT_CLIENT_CDU].start;
 
 	for (i = 0; i < L2_ILT_LINES(bp); i++) {
-		ilt->lines[cdu_ilt_start + i].page =
-			bp->context.vcxt + (ILT_PAGE_CIDS * i);
+		ilt->lines[cdu_ilt_start + i].page = bp->context[i].vcxt;
 		ilt->lines[cdu_ilt_start + i].page_mapping =
-			bp->context.cxt_mapping + (CDU_ILT_PAGE_SZ * i);
-		/* cdu ilt pages are allocated manually so there's no need to
-		set the size */
+			bp->context[i].cxt_mapping;
+		ilt->lines[cdu_ilt_start + i].size = bp->context[i].size;
 	}
 	bnx2x_ilt_init_op(bp, INITOP_SET);
 
@@ -7327,6 +7360,8 @@ static int bnx2x_init_hw_func(struct bnx2x *bp)
 
 void bnx2x_free_mem(struct bnx2x *bp)
 {
+	int i;
+
 	/* fastpath */
 	bnx2x_free_fp_mem(bp);
 	/* end of fastpath */
@@ -7340,9 +7375,9 @@ void bnx2x_free_mem(struct bnx2x *bp)
 	BNX2X_PCI_FREE(bp->slowpath, bp->slowpath_mapping,
 		       sizeof(struct bnx2x_slowpath));
 
-	BNX2X_PCI_FREE(bp->context.vcxt, bp->context.cxt_mapping,
-		       bp->context.size);
-
+	for (i = 0; i < L2_ILT_LINES(bp); i++)
+		BNX2X_PCI_FREE(bp->context[i].vcxt, bp->context[i].cxt_mapping,
+			       bp->context[i].size);
 	bnx2x_ilt_mem_op(bp, ILT_MEMOP_FREE);
 
 	BNX2X_FREE(bp->ilt->lines);
@@ -7428,6 +7463,8 @@ alloc_mem_err:
 
 int bnx2x_alloc_mem(struct bnx2x *bp)
 {
+	int i, allocated, context_size;
+
 #ifdef BCM_CNIC
 	if (!CHIP_IS_E1x(bp))
 		/* size = the status block + ramrod buffers */
@@ -7457,11 +7494,29 @@ int bnx2x_alloc_mem(struct bnx2x *bp)
 	if (bnx2x_alloc_fw_stats_mem(bp))
 		goto alloc_mem_err;
 
-	bp->context.size = sizeof(union cdu_context) * BNX2X_L2_CID_COUNT(bp);
-
-	BNX2X_PCI_ALLOC(bp->context.vcxt, &bp->context.cxt_mapping,
-			bp->context.size);
+	/* Allocate memory for CDU context:
+	 * This memory is allocated separately and not in the generic ILT
+	 * functions because CDU differs in few aspects:
+	 * 1. There are multiple entities allocating memory for context -
+	 * 'regular' driver, CNIC and SRIOV driver. Each separately controls
+	 * its own ILT lines.
+	 * 2. Since CDU page-size is not a single 4KB page (which is the case
+	 * for the other ILT clients), to be efficient we want to support
+	 * allocation of sub-page-size in the last entry.
+	 * 3. Context pointers are used by the driver to pass to FW / update
+	 * the context (for the other ILT clients the pointers are used just to
+	 * free the memory during unload).
+	 */
+	context_size = sizeof(union cdu_context) * BNX2X_L2_CID_COUNT(bp);
 
+	for (i = 0, allocated = 0; allocated < context_size; i++) {
+		bp->context[i].size = min(CDU_ILT_PAGE_SZ,
+					  (context_size - allocated));
+		BNX2X_PCI_ALLOC(bp->context[i].vcxt,
+				&bp->context[i].cxt_mapping,
+				bp->context[i].size);
+		allocated += bp->context[i].size;
+	}
 	BNX2X_ALLOC(bp->ilt->lines, sizeof(struct ilt_line) * ILT_MAX_LINES);
 
 	if (bnx2x_ilt_mem_op(bp, ILT_MEMOP_ALLOC))
@@ -7563,8 +7618,8 @@ int bnx2x_set_eth_mac(struct bnx2x *bp, bool set)
 
 	__set_bit(RAMROD_COMP_WAIT, &ramrod_flags);
 	/* Eth MAC is set on RSS leading client (fp[0]) */
-	return bnx2x_set_mac_one(bp, bp->dev->dev_addr, &bp->fp->mac_obj, set,
-				 BNX2X_ETH_MAC, &ramrod_flags);
+	return bnx2x_set_mac_one(bp, bp->dev->dev_addr, &bp->sp_objs->mac_obj,
+				 set, BNX2X_ETH_MAC, &ramrod_flags);
 }
 
 int bnx2x_setup_leading(struct bnx2x *bp)
@@ -7579,7 +7634,7 @@ int bnx2x_setup_leading(struct bnx2x *bp)
  *
  * In case of MSI-X it will also try to enable MSI-X.
  */
-static void __devinit bnx2x_set_int_mode(struct bnx2x *bp)
+void bnx2x_set_int_mode(struct bnx2x *bp)
 {
 	switch (int_mode) {
 	case INT_MODE_MSI:
@@ -7590,11 +7645,6 @@ static void __devinit bnx2x_set_int_mode(struct bnx2x *bp)
 		BNX2X_DEV_INFO("set number of queues to 1\n");
 		break;
 	default:
-		/* Set number of queues for MSI-X mode */
-		bnx2x_set_num_queues(bp);
-
-		BNX2X_DEV_INFO("set number of queues to %d\n", bp->num_queues);
-
 		/* if we can't use MSI-X we only need one fp,
 		 * so try to enable MSI-X with the requested number of fp's
 		 * and fallback to MSI or legacy INTx with one fp
@@ -7735,6 +7785,8 @@ static void bnx2x_pf_q_prep_init(struct bnx2x *bp,
 {
 
 	u8 cos;
+	int cxt_index, cxt_offset;
+
 	/* FCoE Queue uses Default SB, thus has no HC capabilities */
 	if (!IS_FCOE_FP(fp)) {
 		__set_bit(BNX2X_Q_FLG_HC, &init_params->rx.flags);
@@ -7771,9 +7823,13 @@ static void bnx2x_pf_q_prep_init(struct bnx2x *bp,
 	    fp->index, init_params->max_cos);
 
 	/* set the context pointers queue object */
-	for (cos = FIRST_TX_COS_INDEX; cos < init_params->max_cos; cos++)
+	for (cos = FIRST_TX_COS_INDEX; cos < init_params->max_cos; cos++) {
+		cxt_index = fp->txdata_ptr[cos]->cid / ILT_PAGE_CIDS;
+		cxt_offset = fp->txdata_ptr[cos]->cid - (cxt_index *
+				ILT_PAGE_CIDS);
 		init_params->cxts[cos] =
-			&bp->context.vcxt[fp->txdata[cos].cid].eth;
+			&bp->context[cxt_index].vcxt[cxt_offset].eth;
+	}
 }
 
 int bnx2x_setup_tx_only(struct bnx2x *bp, struct bnx2x_fastpath *fp,
@@ -7838,7 +7894,7 @@ int bnx2x_setup_queue(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 		bnx2x_ack_sb(bp, fp->igu_sb_id, USTORM_ID, 0,
 			     IGU_INT_ENABLE, 0);
 
-	q_params.q_obj = &fp->q_obj;
+	q_params.q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
 	/* We want to wait for completion in this context */
 	__set_bit(RAMROD_COMP_WAIT, &q_params.ramrod_flags);
 
@@ -7911,7 +7967,7 @@ static int bnx2x_stop_queue(struct bnx2x *bp, int index)
 
 	DP(NETIF_MSG_IFDOWN, "stopping queue %d cid %d\n", index, fp->cid);
 
-	q_params.q_obj = &fp->q_obj;
+	q_params.q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
 	/* We want to wait for completion in this context */
 	__set_bit(RAMROD_COMP_WAIT, &q_params.ramrod_flags);
 
@@ -7922,7 +7978,7 @@ static int bnx2x_stop_queue(struct bnx2x *bp, int index)
 	     tx_index++){
 
 		/* ascertain this is a normal queue*/
-		txdata = &fp->txdata[tx_index];
+		txdata = fp->txdata_ptr[tx_index];
 
 		DP(NETIF_MSG_IFDOWN, "stopping tx-only queue %d\n",
 							txdata->txq_index);
@@ -8289,7 +8345,7 @@ void bnx2x_chip_cleanup(struct bnx2x *bp, int unload_mode)
 		struct bnx2x_fastpath *fp = &bp->fp[i];
 
 		for_each_cos_in_tx_queue(fp, cos)
-			rc = bnx2x_clean_tx_queue(bp, &fp->txdata[cos]);
+			rc = bnx2x_clean_tx_queue(bp, fp->txdata_ptr[cos]);
 #ifdef BNX2X_STOP_ON_ERROR
 		if (rc)
 			return;
@@ -8300,12 +8356,13 @@ void bnx2x_chip_cleanup(struct bnx2x *bp, int unload_mode)
 	usleep_range(1000, 1000);
 
 	/* Clean all ETH MACs */
-	rc = bnx2x_del_all_macs(bp, &bp->fp[0].mac_obj, BNX2X_ETH_MAC, false);
+	rc = bnx2x_del_all_macs(bp, &bp->sp_objs[0].mac_obj, BNX2X_ETH_MAC,
+				false);
 	if (rc < 0)
 		BNX2X_ERR("Failed to delete all ETH macs: %d\n", rc);
 
 	/* Clean up UC list  */
-	rc = bnx2x_del_all_macs(bp, &bp->fp[0].mac_obj, BNX2X_UC_LIST_MAC,
+	rc = bnx2x_del_all_macs(bp, &bp->sp_objs[0].mac_obj, BNX2X_UC_LIST_MAC,
 				true);
 	if (rc < 0)
 		BNX2X_ERR("Failed to schedule DEL commands for UC MACs list: %d\n",
@@ -9303,8 +9360,7 @@ static int __devinit bnx2x_prev_mark_path(struct bnx2x *bp)
 	struct bnx2x_prev_path_list *tmp_list;
 	int rc;
 
-	tmp_list = (struct bnx2x_prev_path_list *)
-		    kmalloc(sizeof(struct bnx2x_prev_path_list), GFP_KERNEL);
+	tmp_list = kmalloc(sizeof(struct bnx2x_prev_path_list), GFP_KERNEL);
 	if (!tmp_list) {
 		BNX2X_ERR("Failed to allocate 'bnx2x_prev_path_list'\n");
 		return -ENOMEM;
@@ -9697,6 +9753,11 @@ static void __devinit bnx2x_get_common_hwinfo(struct bnx2x *bp)
 	bp->flags |= (val >= REQ_BC_VER_4_PFC_STATS_SUPPORTED) ?
 			BC_SUPPORTS_PFC_STATS : 0;
 
+	bp->flags |= (val >= REQ_BC_VER_4_FCOE_FEATURES) ?
+			BC_SUPPORTS_FCOE_FEATURES : 0;
+
+	bp->flags |= (val >= REQ_BC_VER_4_DCBX_ADMIN_MSG_NON_PMF) ?
+			BC_SUPPORTS_DCBX_MSG_NON_PMF : 0;
 	boot_mode = SHMEM_RD(bp,
 			dev_info.port_feature_config[BP_PORT(bp)].mba_config) &
 			PORT_FEATURE_MBA_BOOT_AGENT_TYPE_MASK;
@@ -10082,7 +10143,7 @@ static void __devinit bnx2x_get_port_hwinfo(struct bnx2x *bp)
 {
 	int port = BP_PORT(bp);
 	u32 config;
-	u32 ext_phy_type, ext_phy_config;
+	u32 ext_phy_type, ext_phy_config, eee_mode;
 
 	bp->link_params.bp = bp;
 	bp->link_params.port = port;
@@ -10149,6 +10210,19 @@ static void __devinit bnx2x_get_port_hwinfo(struct bnx2x *bp)
 		bp->port.need_hw_lock = bnx2x_hw_lock_required(bp,
 							bp->common.shmem_base,
 							bp->common.shmem2_base);
+
+	/* Configure link feature according to nvram value */
+	eee_mode = (((SHMEM_RD(bp, dev_info.
+		      port_feature_config[port].eee_power_mode)) &
+		     PORT_FEAT_CFG_EEE_POWER_MODE_MASK) >>
+		    PORT_FEAT_CFG_EEE_POWER_MODE_SHIFT);
+	if (eee_mode != PORT_FEAT_CFG_EEE_POWER_MODE_DISABLED) {
+		bp->link_params.eee_mode = EEE_MODE_ADV_LPI |
+					   EEE_MODE_ENABLE_LPI |
+					   EEE_MODE_OUTPUT_TIME;
+	} else {
+		bp->link_params.eee_mode = 0;
+	}
 }
 
 void bnx2x_get_iscsi_info(struct bnx2x *bp)
@@ -10997,7 +11071,7 @@ static int bnx2x_set_uc_list(struct bnx2x *bp)
 	int rc;
 	struct net_device *dev = bp->dev;
 	struct netdev_hw_addr *ha;
-	struct bnx2x_vlan_mac_obj *mac_obj = &bp->fp->mac_obj;
+	struct bnx2x_vlan_mac_obj *mac_obj = &bp->sp_objs->mac_obj;
 	unsigned long ramrod_flags = 0;
 
 	/* First schedule a cleanup up of old configuration */
@@ -11503,8 +11577,7 @@ static void bnx2x_prep_ops(const u8 *_source, u8 *_target, u32 n)
 	}
 }
 
-/**
- * IRO array is stored in the following format:
+/* IRO array is stored in the following format:
  * {base(24bit), m1(16bit), m2(16bit), m3(16bit), size(16bit) }
  */
 static void bnx2x_prep_iro(const u8 *_source, u8 *_target, u32 n)
@@ -11672,7 +11745,7 @@ void bnx2x__init_func_obj(struct bnx2x *bp)
 /* must be called after sriov-enable */
 static int bnx2x_set_qm_cid_count(struct bnx2x *bp)
 {
-	int cid_count = BNX2X_L2_CID_COUNT(bp);
+	int cid_count = BNX2X_L2_MAX_CID(bp);
 
 #ifdef BCM_CNIC
 	cid_count += CNIC_CID_MAX;
@@ -11717,7 +11790,7 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 	struct bnx2x *bp;
 	int pcie_width, pcie_speed;
 	int rc, max_non_def_sbs;
-	int rx_count, tx_count, rss_count;
+	int rx_count, tx_count, rss_count, doorbell_size;
 	/*
 	 * An estimated maximum supported CoS number according to the chip
 	 * version.
@@ -11745,7 +11818,10 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 	case BCM57800_MF:
 	case BCM57810:
 	case BCM57810_MF:
-	case BCM57840:
+	case BCM57840_O:
+	case BCM57840_4_10:
+	case BCM57840_2_20:
+	case BCM57840_MFO:
 	case BCM57840_MF:
 	case BCM57811:
 	case BCM57811_MF:
@@ -11760,13 +11836,6 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 
 	max_non_def_sbs = bnx2x_get_num_non_def_sbs(pdev);
 
-	/* !!! FIXME !!!
-	 * Do not allow the maximum SB count to grow above 16
-	 * since Special CIDs starts from 16*BNX2X_MULTI_TX_COS=48.
-	 * We will use the FP_SB_MAX_E1x macro for this matter.
-	 */
-	max_non_def_sbs = min_t(int, FP_SB_MAX_E1x, max_non_def_sbs);
-
 	WARN_ON(!max_non_def_sbs);
 
 	/* Maximum number of RSS queues: one IGU SB goes to CNIC */
@@ -11777,9 +11846,9 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 
 	/*
 	 * Maximum number of netdev Tx queues:
-	 *      Maximum TSS queues * Maximum supported number of CoS  + FCoE L2
+	 * Maximum TSS queues * Maximum supported number of CoS  + FCoE L2
 	 */
-	tx_count = MAX_TXQS_PER_COS * max_cos_est + FCOE_PRESENT;
+	tx_count = rss_count * max_cos_est + FCOE_PRESENT;
 
 	/* dev zeroed in init_etherdev */
 	dev = alloc_etherdev_mqs(sizeof(*bp), tx_count, rx_count);
@@ -11788,9 +11857,6 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 
 	bp = netdev_priv(dev);
 
-	BNX2X_DEV_INFO("Allocated netdev with %d tx and %d rx queues\n",
-			  tx_count, rx_count);
-
 	bp->igu_sb_cnt = max_non_def_sbs;
 	bp->msg_enable = debug;
 	pci_set_drvdata(pdev, dev);
@@ -11803,6 +11869,9 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 
 	BNX2X_DEV_INFO("max_non_def_sbs %d\n", max_non_def_sbs);
 
+	BNX2X_DEV_INFO("Allocated netdev with %d tx and %d rx queues\n",
+			  tx_count, rx_count);
+
 	rc = bnx2x_init_bp(bp);
 	if (rc)
 		goto init_one_exit;
@@ -11811,9 +11880,15 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 	 * Map doorbels here as we need the real value of bp->max_cos which
 	 * is initialized in bnx2x_init_bp().
 	 */
+	doorbell_size = BNX2X_L2_MAX_CID(bp) * (1 << BNX2X_DB_SHIFT);
+	if (doorbell_size > pci_resource_len(pdev, 2)) {
+		dev_err(&bp->pdev->dev,
+			"Cannot map doorbells, bar size too small, aborting\n");
+		rc = -ENOMEM;
+		goto init_one_exit;
+	}
 	bp->doorbells = ioremap_nocache(pci_resource_start(pdev, 2),
-					min_t(u64, BNX2X_DB_SIZE(bp),
-					      pci_resource_len(pdev, 2)));
+					doorbell_size);
 	if (!bp->doorbells) {
 		dev_err(&bp->pdev->dev,
 			"Cannot map doorbell space, aborting\n");
@@ -11831,8 +11906,12 @@ static int __devinit bnx2x_init_one(struct pci_dev *pdev,
 
 #endif
 
+
+	/* Set bp->num_queues for MSI-X mode*/
+	bnx2x_set_num_queues(bp);
+
 	/* Configure interrupt mode: try to enable MSI-X/MSI if
-	 * needed, set bp->num_queues appropriately.
+	 * needed.
 	 */
 	bnx2x_set_int_mode(bp);
 
@@ -12176,6 +12255,7 @@ static int bnx2x_set_iscsi_eth_mac_addr(struct bnx2x *bp)
 static void bnx2x_cnic_sp_post(struct bnx2x *bp, int count)
 {
 	struct eth_spe *spe;
+	int cxt_index, cxt_offset;
 
 #ifdef BNX2X_STOP_ON_ERROR
 	if (unlikely(bp->panic))
@@ -12198,10 +12278,16 @@ static void bnx2x_cnic_sp_post(struct bnx2x *bp, int count)
 		 *  ramrod
 		 */
 		if (type == ETH_CONNECTION_TYPE) {
-			if (cmd == RAMROD_CMD_ID_ETH_CLIENT_SETUP)
-				bnx2x_set_ctx_validation(bp, &bp->context.
-					vcxt[BNX2X_ISCSI_ETH_CID].eth,
-					BNX2X_ISCSI_ETH_CID);
+			if (cmd == RAMROD_CMD_ID_ETH_CLIENT_SETUP) {
+				cxt_index = BNX2X_ISCSI_ETH_CID(bp) /
+					ILT_PAGE_CIDS;
+				cxt_offset = BNX2X_ISCSI_ETH_CID(bp) -
+					(cxt_index * ILT_PAGE_CIDS);
+				bnx2x_set_ctx_validation(bp,
+					&bp->context[cxt_index].
+							 vcxt[cxt_offset].eth,
+					BNX2X_ISCSI_ETH_CID(bp));
+			}
 		}
 
 		/*
@@ -12488,21 +12574,45 @@ static int bnx2x_drv_ctl(struct net_device *dev, struct drv_ctl_info *ctl)
 		break;
 	}
 	case DRV_CTL_ULP_REGISTER_CMD: {
-		int ulp_type = ctl->data.ulp_type;
+		int ulp_type = ctl->data.register_data.ulp_type;
 
 		if (CHIP_IS_E3(bp)) {
 			int idx = BP_FW_MB_IDX(bp);
-			u32 cap;
+			u32 cap = SHMEM2_RD(bp, drv_capabilities_flag[idx]);
+			int path = BP_PATH(bp);
+			int port = BP_PORT(bp);
+			int i;
+			u32 scratch_offset;
+			u32 *host_addr;
 
-			cap = SHMEM2_RD(bp, drv_capabilities_flag[idx]);
+			/* first write capability to shmem2 */
 			if (ulp_type == CNIC_ULP_ISCSI)
 				cap |= DRV_FLAGS_CAPABILITIES_LOADED_ISCSI;
 			else if (ulp_type == CNIC_ULP_FCOE)
 				cap |= DRV_FLAGS_CAPABILITIES_LOADED_FCOE;
 			SHMEM2_WR(bp, drv_capabilities_flag[idx], cap);
+
+			if ((ulp_type != CNIC_ULP_FCOE) ||
+			    (!SHMEM2_HAS(bp, ncsi_oem_data_addr)) ||
+			    (!(bp->flags &  BC_SUPPORTS_FCOE_FEATURES)))
+				break;
+
+			/* if reached here - should write fcoe capabilities */
+			scratch_offset = SHMEM2_RD(bp, ncsi_oem_data_addr);
+			if (!scratch_offset)
+				break;
+			scratch_offset += offsetof(struct glob_ncsi_oem_data,
+						   fcoe_features[path][port]);
+			host_addr = (u32 *) &(ctl->data.register_data.
+					      fcoe_features);
+			for (i = 0; i < sizeof(struct fcoe_capabilities);
+			     i += 4)
+				REG_WR(bp, scratch_offset + i,
+				       *(host_addr + i/4));
 		}
 		break;
 	}
+
 	case DRV_CTL_ULP_UNREGISTER_CMD: {
 		int ulp_type = ctl->data.ulp_type;
 
@@ -12554,6 +12664,21 @@ void bnx2x_setup_cnic_irq_info(struct bnx2x *bp)
 	cp->num_irq = 2;
 }
 
+void bnx2x_setup_cnic_info(struct bnx2x *bp)
+{
+	struct cnic_eth_dev *cp = &bp->cnic_eth_dev;
+
+
+	cp->ctx_tbl_offset = FUNC_ILT_BASE(BP_FUNC(bp)) +
+			     bnx2x_cid_ilt_lines(bp);
+	cp->starting_cid = bnx2x_cid_ilt_lines(bp) * ILT_PAGE_CIDS;
+	cp->fcoe_init_cid = BNX2X_FCOE_ETH_CID(bp);
+	cp->iscsi_l2_cid = BNX2X_ISCSI_ETH_CID(bp);
+
+	if (NO_ISCSI_OOO(bp))
+		cp->drv_state |= CNIC_DRV_STATE_NO_ISCSI_OOO;
+}
+
 static int bnx2x_register_cnic(struct net_device *dev, struct cnic_ops *ops,
 			       void *data)
 {
@@ -12632,10 +12757,10 @@ struct cnic_eth_dev *bnx2x_cnic_probe(struct net_device *dev)
 	cp->drv_ctl = bnx2x_drv_ctl;
 	cp->drv_register_cnic = bnx2x_register_cnic;
 	cp->drv_unregister_cnic = bnx2x_unregister_cnic;
-	cp->fcoe_init_cid = BNX2X_FCOE_ETH_CID;
+	cp->fcoe_init_cid = BNX2X_FCOE_ETH_CID(bp);
 	cp->iscsi_l2_client_id =
 		bnx2x_cnic_eth_cl_id(bp, BNX2X_ISCSI_ETH_CL_ID_IDX);
-	cp->iscsi_l2_cid = BNX2X_ISCSI_ETH_CID;
+	cp->iscsi_l2_cid = BNX2X_ISCSI_ETH_CID(bp);
 
 	if (NO_ISCSI_OOO(bp))
 		cp->drv_state |= CNIC_DRV_STATE_NO_ISCSI_OOO;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_mfw_req.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_mfw_req.h
new file mode 100644
index 00000000000..ddd5106ad2f
--- /dev/null
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_mfw_req.h
@@ -0,0 +1,168 @@
+/* bnx2x_mfw_req.h: Broadcom Everest network driver.
+ *
+ * Copyright (c) 2012 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.
+ */
+
+#ifndef BNX2X_MFW_REQ_H
+#define BNX2X_MFW_REQ_H
+
+#define PORT_0			0
+#define PORT_1			1
+#define PORT_MAX		2
+#define NVM_PATH_MAX		2
+
+/* FCoE capabilities required from the driver */
+struct fcoe_capabilities {
+	u32 capability1;
+	/* Maximum number of I/Os per connection */
+	#define FCOE_IOS_PER_CONNECTION_MASK    0x0000ffff
+	#define FCOE_IOS_PER_CONNECTION_SHIFT   0
+	/* Maximum number of Logins per port */
+	#define FCOE_LOGINS_PER_PORT_MASK       0xffff0000
+	#define FCOE_LOGINS_PER_PORT_SHIFT   16
+
+	u32 capability2;
+	/* Maximum number of exchanges */
+	#define FCOE_NUMBER_OF_EXCHANGES_MASK   0x0000ffff
+	#define FCOE_NUMBER_OF_EXCHANGES_SHIFT  0
+	/* Maximum NPIV WWN per port */
+	#define FCOE_NPIV_WWN_PER_PORT_MASK     0xffff0000
+	#define FCOE_NPIV_WWN_PER_PORT_SHIFT    16
+
+	u32 capability3;
+	/* Maximum number of targets supported */
+	#define FCOE_TARGETS_SUPPORTED_MASK     0x0000ffff
+	#define FCOE_TARGETS_SUPPORTED_SHIFT    0
+	/* Maximum number of outstanding commands across all connections */
+	#define FCOE_OUTSTANDING_COMMANDS_MASK  0xffff0000
+	#define FCOE_OUTSTANDING_COMMANDS_SHIFT 16
+
+	u32 capability4;
+	#define FCOE_CAPABILITY4_STATEFUL			0x00000001
+	#define FCOE_CAPABILITY4_STATELESS			0x00000002
+	#define FCOE_CAPABILITY4_CAPABILITIES_REPORTED_VALID	0x00000004
+};
+
+struct glob_ncsi_oem_data {
+	u32 driver_version;
+	u32 unused[3];
+	struct fcoe_capabilities fcoe_features[NVM_PATH_MAX][PORT_MAX];
+};
+
+/* current drv_info version */
+#define DRV_INFO_CUR_VER 2
+
+/* drv_info op codes supported */
+enum drv_info_opcode {
+	ETH_STATS_OPCODE,
+	FCOE_STATS_OPCODE,
+	ISCSI_STATS_OPCODE
+};
+
+#define ETH_STAT_INFO_VERSION_LEN	12
+/*  Per PCI Function Ethernet Statistics required from the driver */
+struct eth_stats_info {
+	/* Function's Driver Version. padded to 12 */
+	u8 version[ETH_STAT_INFO_VERSION_LEN];
+	/* Locally Admin Addr. BigEndian EIU48. Actual size is 6 bytes */
+	u8 mac_local[8];
+	u8 mac_add1[8];		/* Additional Programmed MAC Addr 1. */
+	u8 mac_add2[8];		/* Additional Programmed MAC Addr 2. */
+	u32 mtu_size;		/* MTU Size. Note   : Negotiated MTU */
+	u32 feature_flags;	/* Feature_Flags. */
+#define FEATURE_ETH_CHKSUM_OFFLOAD_MASK		0x01
+#define FEATURE_ETH_LSO_MASK			0x02
+#define FEATURE_ETH_BOOTMODE_MASK		0x1C
+#define FEATURE_ETH_BOOTMODE_SHIFT		2
+#define FEATURE_ETH_BOOTMODE_NONE		(0x0 << 2)
+#define FEATURE_ETH_BOOTMODE_PXE		(0x1 << 2)
+#define FEATURE_ETH_BOOTMODE_ISCSI		(0x2 << 2)
+#define FEATURE_ETH_BOOTMODE_FCOE		(0x3 << 2)
+#define FEATURE_ETH_TOE_MASK			0x20
+	u32 lso_max_size;	/* LSO MaxOffloadSize. */
+	u32 lso_min_seg_cnt;	/* LSO MinSegmentCount. */
+	/* Num Offloaded Connections TCP_IPv4. */
+	u32 ipv4_ofld_cnt;
+	/* Num Offloaded Connections TCP_IPv6. */
+	u32 ipv6_ofld_cnt;
+	u32 promiscuous_mode;	/* Promiscuous Mode. non-zero true */
+	u32 txq_size;		/* TX Descriptors Queue Size */
+	u32 rxq_size;		/* RX Descriptors Queue Size */
+	/* TX Descriptor Queue Avg Depth. % Avg Queue Depth since last poll */
+	u32 txq_avg_depth;
+	/* RX Descriptors Queue Avg Depth. % Avg Queue Depth since last poll */
+	u32 rxq_avg_depth;
+	/* IOV_Offload. 0=none; 1=MultiQueue, 2=VEB 3= VEPA*/
+	u32 iov_offload;
+	/* Number of NetQueue/VMQ Config'd. */
+	u32 netq_cnt;
+	u32 vf_cnt;		/* Num VF assigned to this PF. */
+};
+
+/*  Per PCI Function FCOE Statistics required from the driver */
+struct fcoe_stats_info {
+	u8 version[12];		/* Function's Driver Version. */
+	u8 mac_local[8];	/* Locally Admin Addr. */
+	u8 mac_add1[8];		/* Additional Programmed MAC Addr 1. */
+	u8 mac_add2[8];		/* Additional Programmed MAC Addr 2. */
+	/* QoS Priority (per 802.1p). 0-7255 */
+	u32 qos_priority;
+	u32 txq_size;		/* FCoE TX Descriptors Queue Size. */
+	u32 rxq_size;		/* FCoE RX Descriptors Queue Size. */
+	/* FCoE TX Descriptor Queue Avg Depth. */
+	u32 txq_avg_depth;
+	/* FCoE RX Descriptors Queue Avg Depth. */
+	u32 rxq_avg_depth;
+	u32 rx_frames_lo;	/* FCoE RX Frames received. */
+	u32 rx_frames_hi;	/* FCoE RX Frames received. */
+	u32 rx_bytes_lo;	/* FCoE RX Bytes received. */
+	u32 rx_bytes_hi;	/* FCoE RX Bytes received. */
+	u32 tx_frames_lo;	/* FCoE TX Frames sent. */
+	u32 tx_frames_hi;	/* FCoE TX Frames sent. */
+	u32 tx_bytes_lo;	/* FCoE TX Bytes sent. */
+	u32 tx_bytes_hi;	/* FCoE TX Bytes sent. */
+};
+
+/* Per PCI  Function iSCSI Statistics required from the driver*/
+struct iscsi_stats_info {
+	u8 version[12];		/* Function's Driver Version. */
+	u8 mac_local[8];	/* Locally Admin iSCSI MAC Addr. */
+	u8 mac_add1[8];		/* Additional Programmed MAC Addr 1. */
+	/* QoS Priority (per 802.1p). 0-7255 */
+	u32 qos_priority;
+	u8 initiator_name[64];	/* iSCSI Boot Initiator Node name. */
+	u8 ww_port_name[64];	/* iSCSI World wide port name */
+	u8 boot_target_name[64];/* iSCSI Boot Target Name. */
+	u8 boot_target_ip[16];	/* iSCSI Boot Target IP. */
+	u32 boot_target_portal;	/* iSCSI Boot Target Portal. */
+	u8 boot_init_ip[16];	/* iSCSI Boot Initiator IP Address. */
+	u32 max_frame_size;	/* Max Frame Size. bytes */
+	u32 txq_size;		/* PDU TX Descriptors Queue Size. */
+	u32 rxq_size;		/* PDU RX Descriptors Queue Size. */
+	u32 txq_avg_depth;	/* PDU TX Descriptor Queue Avg Depth. */
+	u32 rxq_avg_depth;	/* PDU RX Descriptors Queue Avg Depth. */
+	u32 rx_pdus_lo;		/* iSCSI PDUs received. */
+	u32 rx_pdus_hi;		/* iSCSI PDUs received. */
+	u32 rx_bytes_lo;	/* iSCSI RX Bytes received. */
+	u32 rx_bytes_hi;	/* iSCSI RX Bytes received. */
+	u32 tx_pdus_lo;		/* iSCSI PDUs sent. */
+	u32 tx_pdus_hi;		/* iSCSI PDUs sent. */
+	u32 tx_bytes_lo;	/* iSCSI PDU TX Bytes sent. */
+	u32 tx_bytes_hi;	/* iSCSI PDU TX Bytes sent. */
+	u32 pcp_prior_map_tbl;	/* C-PCP to S-PCP Priority MapTable.
+				 * 9 nibbles, the position of each nibble
+				 * represents the C-PCP value, the value
+				 * of the nibble = S-PCP value.
+				 */
+};
+
+union drv_info_to_mcp {
+	struct eth_stats_info	ether_stat;
+	struct fcoe_stats_info	fcoe_stat;
+	struct iscsi_stats_info	iscsi_stat;
+};
+#endif /* BNX2X_MFW_REQ_H */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index bbd387492a8..28a0bcfe61f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -1488,6 +1488,125 @@
  * 2:1 - otp_misc_do[51:50]; 0 - otp_misc_do[1]. */
 #define MISC_REG_CHIP_TYPE					 0xac60
 #define MISC_REG_CHIP_TYPE_57811_MASK				 (1<<1)
+#define MISC_REG_CPMU_LP_DR_ENABLE				 0xa858
+/* [RW 1] FW EEE LPI Enable. When 1 indicates that EEE LPI mode is enabled
+ * by FW. When 0 indicates that the EEE LPI mode is disabled by FW. Clk
+ * 25MHz. Reset on hard reset. */
+#define MISC_REG_CPMU_LP_FW_ENABLE_P0				 0xa84c
+/* [RW 32] EEE LPI Idle Threshold. The threshold value for the idle EEE LPI
+ * counter. Timer tick is 1 us. Clock 25MHz. Reset on hard reset. */
+#define MISC_REG_CPMU_LP_IDLE_THR_P0				 0xa8a0
+/* [RW 18] LPI entry events mask. [0] - Vmain SM Mask. When 1 indicates that
+ * the Vmain SM end state is disabled. When 0 indicates that the Vmain SM
+ * end state is enabled. [1] - FW Queues Empty Mask. When 1 indicates that
+ * the FW command that all Queues are empty is disabled. When 0 indicates
+ * that the FW command that all Queues are empty is enabled. [2] - FW Early
+ * Exit Mask / Reserved (Entry mask). When 1 indicates that the FW Early
+ * Exit command is disabled. When 0 indicates that the FW Early Exit command
+ * is enabled. This bit applicable only in the EXIT Events Mask registers.
+ * [3] - PBF Request Mask. When 1 indicates that the PBF Request indication
+ * is disabled. When 0 indicates that the PBF Request indication is enabled.
+ * [4] - Tx Request Mask. When =1 indicates that the Tx other Than PBF
+ * Request indication is disabled. When 0 indicates that the Tx Other Than
+ * PBF Request indication is enabled. [5] - Rx EEE LPI Status Mask. When 1
+ * indicates that the RX EEE LPI Status indication is disabled. When 0
+ * indicates that the RX EEE LPI Status indication is enabled. In the EXIT
+ * Events Masks registers; this bit masks the falling edge detect of the LPI
+ * Status (Rx LPI is on - off). [6] - Tx Pause Mask. When 1 indicates that
+ * the Tx Pause indication is disabled. When 0 indicates that the Tx Pause
+ * indication is enabled. [7] - BRB1 Empty Mask. When 1 indicates that the
+ * BRB1 EMPTY indication is disabled. When 0 indicates that the BRB1 EMPTY
+ * indication is enabled. [8] - QM Idle Mask. When 1 indicates that the QM
+ * IDLE indication is disabled. When 0 indicates that the QM IDLE indication
+ * is enabled. (One bit for both VOQ0 and VOQ1). [9] - QM LB Idle Mask. When
+ * 1 indicates that the QM IDLE indication for LOOPBACK is disabled. When 0
+ * indicates that the QM IDLE indication for LOOPBACK is enabled. [10] - L1
+ * Status Mask. When 1 indicates that the L1 Status indication from the PCIE
+ * CORE is disabled. When 0 indicates that the RX EEE LPI Status indication
+ * from the PCIE CORE is enabled. In the EXIT Events Masks registers; this
+ * bit masks the falling edge detect of the L1 status (L1 is on - off). [11]
+ * - P0 E0 EEE EEE LPI REQ Mask. When =1 indicates that the P0 E0 EEE EEE
+ * LPI REQ indication is disabled. When =0 indicates that the P0 E0 EEE LPI
+ * REQ indication is enabled. [12] - P1 E0 EEE LPI REQ Mask. When =1
+ * indicates that the P0 EEE LPI REQ indication is disabled. When =0
+ * indicates that the P0 EEE LPI REQ indication is enabled. [13] - P0 E1 EEE
+ * LPI REQ Mask. When =1 indicates that the P0 EEE LPI REQ indication is
+ * disabled. When =0 indicates that the P0 EEE LPI REQ indication is
+ * enabled. [14] - P1 E1 EEE LPI REQ Mask. When =1 indicates that the P0 EEE
+ * LPI REQ indication is disabled. When =0 indicates that the P0 EEE LPI REQ
+ * indication is enabled. [15] - L1 REQ Mask. When =1 indicates that the L1
+ * REQ indication is disabled. When =0 indicates that the L1 indication is
+ * enabled. [16] - Rx EEE LPI Status Edge Detect Mask. When =1 indicates
+ * that the RX EEE LPI Status Falling Edge Detect indication is disabled (Rx
+ * EEE LPI is on - off). When =0 indicates that the RX EEE LPI Status
+ * Falling Edge Detec indication is enabled (Rx EEE LPI is on - off). This
+ * bit is applicable only in the EXIT Events Masks registers. [17] - L1
+ * Status Edge Detect Mask. When =1 indicates that the L1 Status Falling
+ * Edge Detect indication from the PCIE CORE is disabled (L1 is on - off).
+ * When =0 indicates that the L1 Status Falling Edge Detect indication from
+ * the PCIE CORE is enabled (L1 is on - off). This bit is applicable only in
+ * the EXIT Events Masks registers. Clock 25MHz. Reset on hard reset. */
+#define MISC_REG_CPMU_LP_MASK_ENT_P0				 0xa880
+/* [RW 18] EEE LPI exit events mask. [0] - Vmain SM Mask. When 1 indicates
+ * that the Vmain SM end state is disabled. When 0 indicates that the Vmain
+ * SM end state is enabled. [1] - FW Queues Empty Mask. When 1 indicates
+ * that the FW command that all Queues are empty is disabled. When 0
+ * indicates that the FW command that all Queues are empty is enabled. [2] -
+ * FW Early Exit Mask / Reserved (Entry mask). When 1 indicates that the FW
+ * Early Exit command is disabled. When 0 indicates that the FW Early Exit
+ * command is enabled. This bit applicable only in the EXIT Events Mask
+ * registers. [3] - PBF Request Mask. When 1 indicates that the PBF Request
+ * indication is disabled. When 0 indicates that the PBF Request indication
+ * is enabled. [4] - Tx Request Mask. When =1 indicates that the Tx other
+ * Than PBF Request indication is disabled. When 0 indicates that the Tx
+ * Other Than PBF Request indication is enabled. [5] - Rx EEE LPI Status
+ * Mask. When 1 indicates that the RX EEE LPI Status indication is disabled.
+ * When 0 indicates that the RX LPI Status indication is enabled. In the
+ * EXIT Events Masks registers; this bit masks the falling edge detect of
+ * the EEE LPI Status (Rx EEE LPI is on - off). [6] - Tx Pause Mask. When 1
+ * indicates that the Tx Pause indication is disabled. When 0 indicates that
+ * the Tx Pause indication is enabled. [7] - BRB1 Empty Mask. When 1
+ * indicates that the BRB1 EMPTY indication is disabled. When 0 indicates
+ * that the BRB1 EMPTY indication is enabled. [8] - QM Idle Mask. When 1
+ * indicates that the QM IDLE indication is disabled. When 0 indicates that
+ * the QM IDLE indication is enabled. (One bit for both VOQ0 and VOQ1). [9]
+ * - QM LB Idle Mask. When 1 indicates that the QM IDLE indication for
+ * LOOPBACK is disabled. When 0 indicates that the QM IDLE indication for
+ * LOOPBACK is enabled. [10] - L1 Status Mask. When 1 indicates that the L1
+ * Status indication from the PCIE CORE is disabled. When 0 indicates that
+ * the RX EEE LPI Status indication from the PCIE CORE is enabled. In the
+ * EXIT Events Masks registers; this bit masks the falling edge detect of
+ * the L1 status (L1 is on - off). [11] - P0 E0 EEE EEE LPI REQ Mask. When
+ * =1 indicates that the P0 E0 EEE EEE LPI REQ indication is disabled. When
+ * =0 indicates that the P0 E0 EEE LPI REQ indication is enabled. [12] - P1
+ * E0 EEE LPI REQ Mask. When =1 indicates that the P0 EEE LPI REQ indication
+ * is disabled. When =0 indicates that the P0 EEE LPI REQ indication is
+ * enabled. [13] - P0 E1 EEE LPI REQ Mask. When =1 indicates that the P0 EEE
+ * LPI REQ indication is disabled. When =0 indicates that the P0 EEE LPI REQ
+ * indication is enabled. [14] - P1 E1 EEE LPI REQ Mask. When =1 indicates
+ * that the P0 EEE LPI REQ indication is disabled. When =0 indicates that
+ * the P0 EEE LPI REQ indication is enabled. [15] - L1 REQ Mask. When =1
+ * indicates that the L1 REQ indication is disabled. When =0 indicates that
+ * the L1 indication is enabled. [16] - Rx EEE LPI Status Edge Detect Mask.
+ * When =1 indicates that the RX EEE LPI Status Falling Edge Detect
+ * indication is disabled (Rx EEE LPI is on - off). When =0 indicates that
+ * the RX EEE LPI Status Falling Edge Detec indication is enabled (Rx EEE
+ * LPI is on - off). This bit is applicable only in the EXIT Events Masks
+ * registers. [17] - L1 Status Edge Detect Mask. When =1 indicates that the
+ * L1 Status Falling Edge Detect indication from the PCIE CORE is disabled
+ * (L1 is on - off). When =0 indicates that the L1 Status Falling Edge
+ * Detect indication from the PCIE CORE is enabled (L1 is on - off). This
+ * bit is applicable only in the EXIT Events Masks registers.Clock 25MHz.
+ * Reset on hard reset. */
+#define MISC_REG_CPMU_LP_MASK_EXT_P0				 0xa888
+/* [RW 16] EEE LPI Entry Events Counter. A statistic counter with the number
+ * of counts that the SM entered the EEE LPI state. Clock 25MHz. Read only
+ * register. Reset on hard reset. */
+#define MISC_REG_CPMU_LP_SM_ENT_CNT_P0				 0xa8b8
+/* [RW 16] EEE LPI Entry Events Counter. A statistic counter with the number
+ * of counts that the SM entered the EEE LPI state. Clock 25MHz. Read only
+ * register. Reset on hard reset. */
+#define MISC_REG_CPMU_LP_SM_ENT_CNT_P1				 0xa8bc
 /* [RW 32] The following driver registers(1...16) represent 16 drivers and
    32 clients. Each client can be controlled by one driver only. One in each
    bit represent that this driver control the appropriate client (Ex: bit 5
@@ -5372,6 +5491,8 @@
 /* [RW 32] Lower 48 bits of ctrl_sa register. Used as the SA in PAUSE/PFC
  * packets transmitted by the MAC */
 #define XMAC_REG_CTRL_SA_LO					 0x28
+#define XMAC_REG_EEE_CTRL					 0xd8
+#define XMAC_REG_EEE_TIMERS_HI					 0xe4
 #define XMAC_REG_PAUSE_CTRL					 0x68
 #define XMAC_REG_PFC_CTRL					 0x70
 #define XMAC_REG_PFC_CTRL_HI					 0x74
@@ -5796,6 +5917,7 @@
 #define MISC_REGISTERS_SPIO_OUTPUT_LOW				 0
 #define MISC_REGISTERS_SPIO_SET_POS				 8
 #define HW_LOCK_MAX_RESOURCE_VALUE				 31
+#define HW_LOCK_RESOURCE_DCBX_ADMIN_MIB				 13
 #define HW_LOCK_RESOURCE_DRV_FLAGS				 10
 #define HW_LOCK_RESOURCE_GPIO					 1
 #define HW_LOCK_RESOURCE_MDIO					 0
@@ -6813,6 +6935,8 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_AN_REG_LP_AUTO_NEG		0x0013
 #define MDIO_AN_REG_LP_AUTO_NEG2	0x0014
 #define MDIO_AN_REG_MASTER_STATUS	0x0021
+#define MDIO_AN_REG_EEE_ADV		0x003c
+#define MDIO_AN_REG_LP_EEE_ADV		0x003d
 /*bcm*/
 #define MDIO_AN_REG_LINK_STATUS 	0x8304
 #define MDIO_AN_REG_CL37_CL73		0x8370
@@ -6866,6 +6990,8 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_PMA_REG_84823_LED3_STRETCH_EN			0x0080
 
 /* BCM84833 only */
+#define MDIO_84833_TOP_CFG_FW_REV			0x400f
+#define MDIO_84833_TOP_CFG_FW_EEE		0x10b1
 #define MDIO_84833_TOP_CFG_XGPHY_STRAP1			0x401a
 #define MDIO_84833_SUPER_ISOLATE		0x8000
 /* These are mailbox register set used by 84833. */
@@ -6993,11 +7119,13 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_WC_REG_DIGITAL3_UP1			0x8329
 #define MDIO_WC_REG_DIGITAL3_LP_UP1			 0x832c
 #define MDIO_WC_REG_DIGITAL4_MISC3			0x833c
+#define MDIO_WC_REG_DIGITAL4_MISC5			0x833e
 #define MDIO_WC_REG_DIGITAL5_MISC6			0x8345
 #define MDIO_WC_REG_DIGITAL5_MISC7			0x8349
 #define MDIO_WC_REG_DIGITAL5_ACTUAL_SPEED		0x834e
 #define MDIO_WC_REG_DIGITAL6_MP5_NEXTPAGECTRL		0x8350
 #define MDIO_WC_REG_CL49_USERB0_CTRL			0x8368
+#define MDIO_WC_REG_EEE_COMBO_CONTROL0			0x8390
 #define MDIO_WC_REG_TX66_CONTROL			0x83b0
 #define MDIO_WC_REG_RX66_CONTROL			0x83c0
 #define MDIO_WC_REG_RX66_SCW0				0x83c2
@@ -7036,6 +7164,7 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_REG_GPHY_EEE_1G		(0x1 << 2)
 #define MDIO_REG_GPHY_EEE_100		(0x1 << 1)
 #define MDIO_REG_GPHY_EEE_RESOLVED		0x803e
+#define MDIO_REG_GPHY_AUX_STATUS			0x19
 #define MDIO_REG_INTR_STATUS				0x1a
 #define MDIO_REG_INTR_MASK				0x1b
 #define MDIO_REG_INTR_MASK_LINK_STATUS			(0x1 << 1)
@@ -7150,8 +7279,7 @@ Theotherbitsarereservedandshouldbezero*/
 #define CDU_REGION_NUMBER_UCM_AG 4
 
 
-/**
- * String-to-compress [31:8] = CID (all 24 bits)
+/* String-to-compress [31:8] = CID (all 24 bits)
  * String-to-compress [7:4] = Region
  * String-to-compress [3:0] = Type
  */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index 6c14b4a4e82..734fd87cd99 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -4107,6 +4107,10 @@ static int bnx2x_setup_rss(struct bnx2x *bp,
 		data->capabilities |=
 			ETH_RSS_UPDATE_RAMROD_DATA_IPV4_TCP_CAPABILITY;
 
+	if (test_bit(BNX2X_RSS_IPV4_UDP, &p->rss_flags))
+		data->capabilities |=
+			ETH_RSS_UPDATE_RAMROD_DATA_IPV4_UDP_CAPABILITY;
+
 	if (test_bit(BNX2X_RSS_IPV6, &p->rss_flags))
 		data->capabilities |=
 			ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY;
@@ -4115,6 +4119,10 @@ static int bnx2x_setup_rss(struct bnx2x *bp,
 		data->capabilities |=
 			ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY;
 
+	if (test_bit(BNX2X_RSS_IPV6_UDP, &p->rss_flags))
+		data->capabilities |=
+			ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY;
+
 	/* Hashing mask */
 	data->rss_result_mask = p->rss_result_mask;
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
index efd80bdd0df..f83e033da6d 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
@@ -167,9 +167,8 @@ typedef int (*exe_q_remove)(struct bnx2x *bp,
 			    union bnx2x_qable_obj *o,
 			    struct bnx2x_exeq_elem *elem);
 
-/**
- * @return positive is entry was optimized, 0 - if not, negative
- *         in case of an error.
+/* Return positive if entry was optimized, 0 - if not, negative
+ * in case of an error.
  */
 typedef int (*exe_q_optimize)(struct bnx2x *bp,
 			      union bnx2x_qable_obj *o,
@@ -694,8 +693,10 @@ enum {
 
 	BNX2X_RSS_IPV4,
 	BNX2X_RSS_IPV4_TCP,
+	BNX2X_RSS_IPV4_UDP,
 	BNX2X_RSS_IPV6,
 	BNX2X_RSS_IPV6_TCP,
+	BNX2X_RSS_IPV6_UDP,
 };
 
 struct bnx2x_config_rss_params {
@@ -729,6 +730,10 @@ struct bnx2x_rss_config_obj {
 	/* Last configured indirection table */
 	u8			ind_table[T_ETH_INDIRECTION_TABLE_SIZE];
 
+	/* flags for enabling 4-tupple hash on UDP */
+	u8			udp_rss_v4;
+	u8			udp_rss_v6;
+
 	int (*config_rss)(struct bnx2x *bp,
 			  struct bnx2x_config_rss_params *p);
 };
@@ -1280,12 +1285,11 @@ void bnx2x_init_rx_mode_obj(struct bnx2x *bp,
 			    struct bnx2x_rx_mode_obj *o);
 
 /**
- * Send and RX_MODE ramrod according to the provided parameters.
+ * bnx2x_config_rx_mode - Send and RX_MODE ramrod according to the provided parameters.
  *
- * @param bp
- * @param p Command parameters
+ * @p: Command parameters
  *
- * @return 0 - if operation was successfull and there is no pending completions,
+ * Return: 0 - if operation was successfull and there is no pending completions,
  *         positive number - if there are pending completions,
  *         negative - if there were errors
  */
@@ -1302,7 +1306,11 @@ void bnx2x_init_mcast_obj(struct bnx2x *bp,
 			  bnx2x_obj_type type);
 
 /**
- * Configure multicast MACs list. May configure a new list
+ * bnx2x_config_mcast - Configure multicast MACs list.
+ *
+ * @cmd: command to execute: BNX2X_MCAST_CMD_X
+ *
+ * May configure a new list
  * provided in p->mcast_list (BNX2X_MCAST_CMD_ADD), clean up
  * (BNX2X_MCAST_CMD_DEL) or restore (BNX2X_MCAST_CMD_RESTORE) a current
  * configuration, continue to execute the pending commands
@@ -1313,11 +1321,7 @@ void bnx2x_init_mcast_obj(struct bnx2x *bp,
  * the current command will be enqueued to the tail of the
  * pending commands list.
  *
- * @param bp
- * @param p
- * @param command to execute: BNX2X_MCAST_CMD_X
- *
- * @return 0 is operation was sucessfull and there are no pending completions,
+ * Return: 0 is operation was sucessfull and there are no pending completions,
  *         negative if there were errors, positive if there are pending
  *         completions.
  */
@@ -1342,21 +1346,17 @@ void bnx2x_init_rss_config_obj(struct bnx2x *bp,
 			       bnx2x_obj_type type);
 
 /**
- * Updates RSS configuration according to provided parameters.
- *
- * @param bp
- * @param p
+ * bnx2x_config_rss - Updates RSS configuration according to provided parameters
  *
- * @return 0 in case of success
+ * Return: 0 in case of success
  */
 int bnx2x_config_rss(struct bnx2x *bp,
 		     struct bnx2x_config_rss_params *p);
 
 /**
- * Return the current ind_table configuration.
+ * bnx2x_get_rss_ind_table - Return the current ind_table configuration.
  *
- * @param bp
- * @param ind_table buffer to fill with the current indirection
+ * @ind_table: buffer to fill with the current indirection
  *                  table content. Should be at least
  *                  T_ETH_INDIRECTION_TABLE_SIZE bytes long.
  */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index 1e2785cd11d..332db64dd5b 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
@@ -785,6 +785,12 @@ static int bnx2x_hw_stats_update(struct bnx2x *bp)
 
 	pstats->host_port_stats_counter++;
 
+	if (CHIP_IS_E3(bp)) {
+		u32 lpi_reg = BP_PORT(bp) ? MISC_REG_CPMU_LP_SM_ENT_CNT_P1
+					  : MISC_REG_CPMU_LP_SM_ENT_CNT_P0;
+		estats->eee_tx_lpi += REG_RD(bp, lpi_reg);
+	}
+
 	if (!BP_NOMCP(bp)) {
 		u32 nig_timer_max =
 			SHMEM_RD(bp, port_mb[BP_PORT(bp)].stat_nig_timer);
@@ -855,17 +861,22 @@ static int bnx2x_storm_stats_update(struct bnx2x *bp)
 		struct tstorm_per_queue_stats *tclient =
 			&bp->fw_stats_data->queue_stats[i].
 			tstorm_queue_statistics;
-		struct tstorm_per_queue_stats *old_tclient = &fp->old_tclient;
+		struct tstorm_per_queue_stats *old_tclient =
+			&bnx2x_fp_stats(bp, fp)->old_tclient;
 		struct ustorm_per_queue_stats *uclient =
 			&bp->fw_stats_data->queue_stats[i].
 			ustorm_queue_statistics;
-		struct ustorm_per_queue_stats *old_uclient = &fp->old_uclient;
+		struct ustorm_per_queue_stats *old_uclient =
+			&bnx2x_fp_stats(bp, fp)->old_uclient;
 		struct xstorm_per_queue_stats *xclient =
 			&bp->fw_stats_data->queue_stats[i].
 			xstorm_queue_statistics;
-		struct xstorm_per_queue_stats *old_xclient = &fp->old_xclient;
-		struct bnx2x_eth_q_stats *qstats = &fp->eth_q_stats;
-		struct bnx2x_eth_q_stats_old *qstats_old = &fp->eth_q_stats_old;
+		struct xstorm_per_queue_stats *old_xclient =
+			&bnx2x_fp_stats(bp, fp)->old_xclient;
+		struct bnx2x_eth_q_stats *qstats =
+			&bnx2x_fp_stats(bp, fp)->eth_q_stats;
+		struct bnx2x_eth_q_stats_old *qstats_old =
+			&bnx2x_fp_stats(bp, fp)->eth_q_stats_old;
 
 		u32 diff;
 
@@ -1048,8 +1059,11 @@ static void bnx2x_net_stats_update(struct bnx2x *bp)
 	nstats->tx_bytes = bnx2x_hilo(&estats->total_bytes_transmitted_hi);
 
 	tmp = estats->mac_discard;
-	for_each_rx_queue(bp, i)
-		tmp += le32_to_cpu(bp->fp[i].old_tclient.checksum_discard);
+	for_each_rx_queue(bp, i) {
+		struct tstorm_per_queue_stats *old_tclient =
+			&bp->fp_stats[i].old_tclient;
+		tmp += le32_to_cpu(old_tclient->checksum_discard);
+	}
 	nstats->rx_dropped = tmp + bp->net_stats_old.rx_dropped;
 
 	nstats->tx_dropped = 0;
@@ -1099,9 +1113,9 @@ static void bnx2x_drv_stats_update(struct bnx2x *bp)
 	int i;
 
 	for_each_queue(bp, i) {
-		struct bnx2x_eth_q_stats *qstats = &bp->fp[i].eth_q_stats;
+		struct bnx2x_eth_q_stats *qstats = &bp->fp_stats[i].eth_q_stats;
 		struct bnx2x_eth_q_stats_old *qstats_old =
-						&bp->fp[i].eth_q_stats_old;
+			&bp->fp_stats[i].eth_q_stats_old;
 
 		UPDATE_ESTAT_QSTAT(driver_xoff);
 		UPDATE_ESTAT_QSTAT(rx_err_discard_pkt);
@@ -1309,12 +1323,9 @@ static void bnx2x_port_stats_base_init(struct bnx2x *bp)
 	bnx2x_stats_comp(bp);
 }
 
-/**
- * This function will prepare the statistics ramrod data the way
+/* This function will prepare the statistics ramrod data the way
  * we will only have to increment the statistics counter and
  * send the ramrod each time we have to.
- *
- * @param bp
  */
 static void bnx2x_prep_fw_stats_req(struct bnx2x *bp)
 {
@@ -1428,7 +1439,7 @@ static void bnx2x_prep_fw_stats_req(struct bnx2x *bp)
 					query[first_queue_query_index + i];
 
 		cur_query_entry->kind = STATS_TYPE_QUEUE;
-		cur_query_entry->index = bnx2x_stats_id(&bp->fp[FCOE_IDX]);
+		cur_query_entry->index = bnx2x_stats_id(&bp->fp[FCOE_IDX(bp)]);
 		cur_query_entry->funcID = cpu_to_le16(BP_FUNC(bp));
 		cur_query_entry->address.hi =
 			cpu_to_le32(U64_HI(cur_data_offset));
@@ -1479,15 +1490,19 @@ void bnx2x_stats_init(struct bnx2x *bp)
 
 	/* function stats */
 	for_each_queue(bp, i) {
-		struct bnx2x_fastpath *fp = &bp->fp[i];
-
-		memset(&fp->old_tclient, 0, sizeof(fp->old_tclient));
-		memset(&fp->old_uclient, 0, sizeof(fp->old_uclient));
-		memset(&fp->old_xclient, 0, sizeof(fp->old_xclient));
+		struct bnx2x_fp_stats *fp_stats = &bp->fp_stats[i];
+
+		memset(&fp_stats->old_tclient, 0,
+		       sizeof(fp_stats->old_tclient));
+		memset(&fp_stats->old_uclient, 0,
+		       sizeof(fp_stats->old_uclient));
+		memset(&fp_stats->old_xclient, 0,
+		       sizeof(fp_stats->old_xclient));
 		if (bp->stats_init) {
-			memset(&fp->eth_q_stats, 0, sizeof(fp->eth_q_stats));
-			memset(&fp->eth_q_stats_old, 0,
-			       sizeof(fp->eth_q_stats_old));
+			memset(&fp_stats->eth_q_stats, 0,
+			       sizeof(fp_stats->eth_q_stats));
+			memset(&fp_stats->eth_q_stats_old, 0,
+			       sizeof(fp_stats->eth_q_stats_old));
 		}
 	}
 
@@ -1529,8 +1544,10 @@ void bnx2x_save_statistics(struct bnx2x *bp)
 	/* save queue statistics */
 	for_each_eth_queue(bp, i) {
 		struct bnx2x_fastpath *fp = &bp->fp[i];
-		struct bnx2x_eth_q_stats *qstats = &fp->eth_q_stats;
-		struct bnx2x_eth_q_stats_old *qstats_old = &fp->eth_q_stats_old;
+		struct bnx2x_eth_q_stats *qstats =
+			&bnx2x_fp_stats(bp, fp)->eth_q_stats;
+		struct bnx2x_eth_q_stats_old *qstats_old =
+			&bnx2x_fp_stats(bp, fp)->eth_q_stats_old;
 
 		UPDATE_QSTAT_OLD(total_unicast_bytes_received_hi);
 		UPDATE_QSTAT_OLD(total_unicast_bytes_received_lo);
@@ -1569,7 +1586,7 @@ void bnx2x_afex_collect_stats(struct bnx2x *bp, void *void_afex_stats,
 	struct afex_stats *afex_stats = (struct afex_stats *)void_afex_stats;
 	struct bnx2x_eth_stats *estats = &bp->eth_stats;
 	struct per_queue_stats *fcoe_q_stats =
-		&bp->fw_stats_data->queue_stats[FCOE_IDX];
+		&bp->fw_stats_data->queue_stats[FCOE_IDX(bp)];
 
 	struct tstorm_per_queue_stats *fcoe_q_tstorm_stats =
 		&fcoe_q_stats->tstorm_queue_statistics;
@@ -1586,8 +1603,7 @@ void bnx2x_afex_collect_stats(struct bnx2x *bp, void *void_afex_stats,
 	memset(afex_stats, 0, sizeof(struct afex_stats));
 
 	for_each_eth_queue(bp, i) {
-		struct bnx2x_fastpath *fp = &bp->fp[i];
-		struct bnx2x_eth_q_stats *qstats = &fp->eth_q_stats;
+		struct bnx2x_eth_q_stats *qstats = &bp->fp_stats[i].eth_q_stats;
 
 		ADD_64(afex_stats->rx_unicast_bytes_hi,
 		       qstats->total_unicast_bytes_received_hi,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
index 93e689fdfed..24b8e505b60 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.h
@@ -203,6 +203,8 @@ struct bnx2x_eth_stats {
 	/* Recovery */
 	u32 recoverable_error;
 	u32 unrecoverable_error;
+	/* src: Clear-on-Read register; Will not survive PMF Migration */
+	u32 eee_tx_lpi;
 };
 
 
diff --git a/drivers/net/ethernet/broadcom/cnic.c b/drivers/net/ethernet/broadcom/cnic.c
index c95e7b5e2b8..3b4fc61f24c 100644
--- a/drivers/net/ethernet/broadcom/cnic.c
+++ b/drivers/net/ethernet/broadcom/cnic.c
@@ -256,11 +256,16 @@ static void cnic_ulp_ctl(struct cnic_dev *dev, int ulp_type, bool reg)
 	struct cnic_local *cp = dev->cnic_priv;
 	struct cnic_eth_dev *ethdev = cp->ethdev;
 	struct drv_ctl_info info;
+	struct fcoe_capabilities *fcoe_cap =
+		&info.data.register_data.fcoe_features;
 
-	if (reg)
+	if (reg) {
 		info.cmd = DRV_CTL_ULP_REGISTER_CMD;
-	else
+		if (ulp_type == CNIC_ULP_FCOE && dev->fcoe_cap)
+			memcpy(fcoe_cap, dev->fcoe_cap, sizeof(*fcoe_cap));
+	} else {
 		info.cmd = DRV_CTL_ULP_UNREGISTER_CMD;
+	}
 
 	info.data.ulp_type = ulp_type;
 	ethdev->drv_ctl(dev->netdev, &info);
@@ -286,6 +291,9 @@ static int cnic_get_l5_cid(struct cnic_local *cp, u32 cid, u32 *l5_cid)
 {
 	u32 i;
 
+	if (!cp->ctx_tbl)
+		return -EINVAL;
+
 	for (i = 0; i < cp->max_cid_space; i++) {
 		if (cp->ctx_tbl[i].cid == cid) {
 			*l5_cid = i;
@@ -534,7 +542,8 @@ int cnic_unregister_driver(int ulp_type)
 	}
 
 	if (atomic_read(&ulp_ops->ref_count) != 0)
-		netdev_warn(dev->netdev, "Failed waiting for ref count to go to zero\n");
+		pr_warn("%s: Failed waiting for ref count to go to zero\n",
+			__func__);
 	return 0;
 
 out_unlock:
@@ -611,6 +620,8 @@ static int cnic_unregister_device(struct cnic_dev *dev, int ulp_type)
 
 	if (ulp_type == CNIC_ULP_ISCSI)
 		cnic_send_nlmsg(cp, ISCSI_KEVENT_IF_DOWN, NULL);
+	else if (ulp_type == CNIC_ULP_FCOE)
+		dev->fcoe_cap = NULL;
 
 	synchronize_rcu();
 
@@ -1053,12 +1064,13 @@ static int cnic_init_uio(struct cnic_dev *dev)
 
 	uinfo = &udev->cnic_uinfo;
 
-	uinfo->mem[0].addr = dev->netdev->base_addr;
+	uinfo->mem[0].addr = pci_resource_start(dev->pcidev, 0);
 	uinfo->mem[0].internal_addr = dev->regview;
-	uinfo->mem[0].size = dev->netdev->mem_end - dev->netdev->mem_start;
 	uinfo->mem[0].memtype = UIO_MEM_PHYS;
 
 	if (test_bit(CNIC_F_BNX2_CLASS, &dev->flags)) {
+		uinfo->mem[0].size = MB_GET_CID_ADDR(TX_TSS_CID +
+						     TX_MAX_TSS_RINGS + 1);
 		uinfo->mem[1].addr = (unsigned long) cp->status_blk.gen &
 					PAGE_MASK;
 		if (cp->ethdev->drv_state & CNIC_DRV_STATE_USING_MSIX)
@@ -1068,6 +1080,8 @@ static int cnic_init_uio(struct cnic_dev *dev)
 
 		uinfo->name = "bnx2_cnic";
 	} else if (test_bit(CNIC_F_BNX2X_CLASS, &dev->flags)) {
+		uinfo->mem[0].size = pci_resource_len(dev->pcidev, 0);
+
 		uinfo->mem[1].addr = (unsigned long) cp->bnx2x_def_status_blk &
 			PAGE_MASK;
 		uinfo->mem[1].size = sizeof(*cp->bnx2x_def_status_blk);
@@ -2585,7 +2599,7 @@ static void cnic_bnx2x_kwqe_err(struct cnic_dev *dev, struct kwqe *kwqe)
 		return;
 	}
 
-	cqes[0] = (struct kcqe *) &kcqe;
+	cqes[0] = &kcqe;
 	cnic_reply_bnx2x_kcqes(dev, ulp_type, cqes, 1);
 }
 
@@ -3213,6 +3227,9 @@ static int cnic_ctl(void *data, struct cnic_ctl_info *info)
 		u32 l5_cid;
 		struct cnic_local *cp = dev->cnic_priv;
 
+		if (!test_bit(CNIC_F_CNIC_UP, &dev->flags))
+			break;
+
 		if (cnic_get_l5_cid(cp, cid, &l5_cid) == 0) {
 			struct cnic_context *ctx = &cp->ctx_tbl[l5_cid];
 
@@ -3943,6 +3960,15 @@ static void cnic_cm_process_kcqe(struct cnic_dev *dev, struct kcqe *kcqe)
 		cnic_cm_upcall(cp, csk, opcode);
 		break;
 
+	case L5CM_RAMROD_CMD_ID_CLOSE:
+		if (l4kcqe->status != 0) {
+			netdev_warn(dev->netdev, "RAMROD CLOSE compl with "
+				    "status 0x%x\n", l4kcqe->status);
+			opcode = L4_KCQE_OPCODE_VALUE_CLOSE_COMP;
+			/* Fall through */
+		} else {
+			break;
+		}
 	case L4_KCQE_OPCODE_VALUE_RESET_RECEIVED:
 	case L4_KCQE_OPCODE_VALUE_CLOSE_COMP:
 	case L4_KCQE_OPCODE_VALUE_RESET_COMP:
@@ -4246,8 +4272,6 @@ static int cnic_cm_shutdown(struct cnic_dev *dev)
 	struct cnic_local *cp = dev->cnic_priv;
 	int i;
 
-	cp->stop_cm(dev);
-
 	if (!cp->csk_tbl)
 		return 0;
 
@@ -4665,9 +4689,9 @@ static int cnic_start_bnx2_hw(struct cnic_dev *dev)
 
 	cp->kcq1.sw_prod_idx = 0;
 	cp->kcq1.hw_prod_idx_ptr =
-		(u16 *) &sblk->status_completion_producer_index;
+		&sblk->status_completion_producer_index;
 
-	cp->kcq1.status_idx_ptr = (u16 *) &sblk->status_idx;
+	cp->kcq1.status_idx_ptr = &sblk->status_idx;
 
 	/* Initialize the kernel complete queue context. */
 	val = KRNLQ_TYPE_TYPE_KRNLQ | KRNLQ_SIZE_TYPE_SIZE |
@@ -4693,9 +4717,9 @@ static int cnic_start_bnx2_hw(struct cnic_dev *dev)
 		u32 sb = BNX2_L2CTX_L5_STATUSB_NUM(sb_id);
 
 		cp->kcq1.hw_prod_idx_ptr =
-			(u16 *) &msblk->status_completion_producer_index;
-		cp->kcq1.status_idx_ptr = (u16 *) &msblk->status_idx;
-		cp->kwq_con_idx_ptr = (u16 *) &msblk->status_cmd_consumer_index;
+			&msblk->status_completion_producer_index;
+		cp->kcq1.status_idx_ptr = &msblk->status_idx;
+		cp->kwq_con_idx_ptr = &msblk->status_cmd_consumer_index;
 		cp->int_num = sb_id << BNX2_PCICFG_INT_ACK_CMD_INT_NUM_SHIFT;
 		cnic_ctx_wr(dev, kwq_cid_addr, L5_KRNLQ_HOST_QIDX, sb);
 		cnic_ctx_wr(dev, kcq_cid_addr, L5_KRNLQ_HOST_QIDX, sb);
@@ -4977,8 +5001,14 @@ static int cnic_start_bnx2x_hw(struct cnic_dev *dev)
 	cp->port_mode = CHIP_PORT_MODE_NONE;
 
 	if (BNX2X_CHIP_IS_E2_PLUS(cp->chip_id)) {
-		u32 val = CNIC_RD(dev, MISC_REG_PORT4MODE_EN_OVWR);
+		u32 val;
 
+		pci_read_config_dword(dev->pcidev, PCICFG_ME_REGISTER, &val);
+		cp->func = (u8) ((val & ME_REG_ABS_PF_NUM) >>
+				 ME_REG_ABS_PF_NUM_SHIFT);
+		func = CNIC_FUNC(cp);
+
+		val = CNIC_RD(dev, MISC_REG_PORT4MODE_EN_OVWR);
 		if (!(val & 1))
 			val = CNIC_RD(dev, MISC_REG_PORT4MODE_EN);
 		else
@@ -5283,6 +5313,7 @@ static void cnic_stop_hw(struct cnic_dev *dev)
 			i++;
 		}
 		cnic_shutdown_rings(dev);
+		cp->stop_cm(dev);
 		clear_bit(CNIC_F_CNIC_UP, &dev->flags);
 		RCU_INIT_POINTER(cp->ulp_ops[CNIC_ULP_L4], NULL);
 		synchronize_rcu();
@@ -5512,9 +5543,7 @@ static void cnic_rcv_netevent(struct cnic_local *cp, unsigned long event,
 	rcu_read_unlock();
 }
 
-/**
- * netdev event handler
- */
+/* netdev event handler */
 static int cnic_netdev_event(struct notifier_block *this, unsigned long event,
 							 void *ptr)
 {
diff --git a/drivers/net/ethernet/broadcom/cnic_if.h b/drivers/net/ethernet/broadcom/cnic_if.h
index 289274e546b..5cb88881bba 100644
--- a/drivers/net/ethernet/broadcom/cnic_if.h
+++ b/drivers/net/ethernet/broadcom/cnic_if.h
@@ -12,8 +12,10 @@
 #ifndef CNIC_IF_H
 #define CNIC_IF_H
 
-#define CNIC_MODULE_VERSION	"2.5.10"
-#define CNIC_MODULE_RELDATE	"March 21, 2012"
+#include "bnx2x/bnx2x_mfw_req.h"
+
+#define CNIC_MODULE_VERSION	"2.5.12"
+#define CNIC_MODULE_RELDATE	"June 29, 2012"
 
 #define CNIC_ULP_RDMA		0
 #define CNIC_ULP_ISCSI		1
@@ -131,6 +133,11 @@ struct drv_ctl_l2_ring {
 	u32		cid;
 };
 
+struct drv_ctl_register_data {
+	int ulp_type;
+	struct fcoe_capabilities fcoe_features;
+};
+
 struct drv_ctl_info {
 	int	cmd;
 	union {
@@ -138,6 +145,7 @@ struct drv_ctl_info {
 		struct drv_ctl_io io;
 		struct drv_ctl_l2_ring ring;
 		int ulp_type;
+		struct drv_ctl_register_data register_data;
 		char bytes[MAX_DRV_CTL_DATA];
 	} data;
 };
@@ -305,6 +313,7 @@ struct cnic_dev {
 	int		max_rdma_conn;
 
 	union drv_info_to_mcp	*stats_addr;
+	struct fcoe_capabilities	*fcoe_cap;
 
 	void		*cnic_priv;
 };
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index e47ff8be1d7..bf906c51d82 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -44,6 +44,10 @@
 #include <linux/prefetch.h>
 #include <linux/dma-mapping.h>
 #include <linux/firmware.h>
+#if IS_ENABLED(CONFIG_HWMON)
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#endif
 
 #include <net/checksum.h>
 #include <net/ip.h>
@@ -88,7 +92,7 @@ static inline void _tg3_flag_clear(enum TG3_FLAGS flag, unsigned long *bits)
 
 #define DRV_MODULE_NAME		"tg3"
 #define TG3_MAJ_NUM			3
-#define TG3_MIN_NUM			123
+#define TG3_MIN_NUM			124
 #define DRV_MODULE_VERSION	\
 	__stringify(TG3_MAJ_NUM) "." __stringify(TG3_MIN_NUM)
 #define DRV_MODULE_RELDATE	"March 21, 2012"
@@ -298,6 +302,7 @@ static DEFINE_PCI_DEVICE_TABLE(tg3_pci_tbl) = {
 	{PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, TG3PCI_DEVICE_TIGON3_57795)},
 	{PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, TG3PCI_DEVICE_TIGON3_5719)},
 	{PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, TG3PCI_DEVICE_TIGON3_5720)},
+	{PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, TG3PCI_DEVICE_TIGON3_57762)},
 	{PCI_DEVICE(PCI_VENDOR_ID_SYSKONNECT, PCI_DEVICE_ID_SYSKONNECT_9DXX)},
 	{PCI_DEVICE(PCI_VENDOR_ID_SYSKONNECT, PCI_DEVICE_ID_SYSKONNECT_9MXX)},
 	{PCI_DEVICE(PCI_VENDOR_ID_ALTIMA, PCI_DEVICE_ID_ALTIMA_AC1000)},
@@ -667,6 +672,12 @@ static int tg3_ape_lock(struct tg3 *tp, int locknum)
 		else
 			bit = 1 << tp->pci_fn;
 		break;
+	case TG3_APE_LOCK_PHY0:
+	case TG3_APE_LOCK_PHY1:
+	case TG3_APE_LOCK_PHY2:
+	case TG3_APE_LOCK_PHY3:
+		bit = APE_LOCK_REQ_DRIVER;
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -718,6 +729,12 @@ static void tg3_ape_unlock(struct tg3 *tp, int locknum)
 		else
 			bit = 1 << tp->pci_fn;
 		break;
+	case TG3_APE_LOCK_PHY0:
+	case TG3_APE_LOCK_PHY1:
+	case TG3_APE_LOCK_PHY2:
+	case TG3_APE_LOCK_PHY3:
+		bit = APE_LOCK_GRANT_DRIVER;
+		break;
 	default:
 		return;
 	}
@@ -730,44 +747,131 @@ static void tg3_ape_unlock(struct tg3 *tp, int locknum)
 	tg3_ape_write32(tp, gnt + 4 * locknum, bit);
 }
 
-static void tg3_ape_send_event(struct tg3 *tp, u32 event)
+static int tg3_ape_event_lock(struct tg3 *tp, u32 timeout_us)
 {
-	int i;
 	u32 apedata;
 
-	/* NCSI does not support APE events */
-	if (tg3_flag(tp, APE_HAS_NCSI))
-		return;
+	while (timeout_us) {
+		if (tg3_ape_lock(tp, TG3_APE_LOCK_MEM))
+			return -EBUSY;
+
+		apedata = tg3_ape_read32(tp, TG3_APE_EVENT_STATUS);
+		if (!(apedata & APE_EVENT_STATUS_EVENT_PENDING))
+			break;
+
+		tg3_ape_unlock(tp, TG3_APE_LOCK_MEM);
+
+		udelay(10);
+		timeout_us -= (timeout_us > 10) ? 10 : timeout_us;
+	}
+
+	return timeout_us ? 0 : -EBUSY;
+}
+
+static int tg3_ape_wait_for_event(struct tg3 *tp, u32 timeout_us)
+{
+	u32 i, apedata;
+
+	for (i = 0; i < timeout_us / 10; i++) {
+		apedata = tg3_ape_read32(tp, TG3_APE_EVENT_STATUS);
+
+		if (!(apedata & APE_EVENT_STATUS_EVENT_PENDING))
+			break;
+
+		udelay(10);
+	}
+
+	return i == timeout_us / 10;
+}
+
+int tg3_ape_scratchpad_read(struct tg3 *tp, u32 *data, u32 base_off, u32 len)
+{
+	int err;
+	u32 i, bufoff, msgoff, maxlen, apedata;
+
+	if (!tg3_flag(tp, APE_HAS_NCSI))
+		return 0;
 
 	apedata = tg3_ape_read32(tp, TG3_APE_SEG_SIG);
 	if (apedata != APE_SEG_SIG_MAGIC)
-		return;
+		return -ENODEV;
 
 	apedata = tg3_ape_read32(tp, TG3_APE_FW_STATUS);
 	if (!(apedata & APE_FW_STATUS_READY))
-		return;
+		return -EAGAIN;
 
-	/* Wait for up to 1 millisecond for APE to service previous event. */
-	for (i = 0; i < 10; i++) {
-		if (tg3_ape_lock(tp, TG3_APE_LOCK_MEM))
-			return;
+	bufoff = tg3_ape_read32(tp, TG3_APE_SEG_MSG_BUF_OFF) +
+		 TG3_APE_SHMEM_BASE;
+	msgoff = bufoff + 2 * sizeof(u32);
+	maxlen = tg3_ape_read32(tp, TG3_APE_SEG_MSG_BUF_LEN);
 
-		apedata = tg3_ape_read32(tp, TG3_APE_EVENT_STATUS);
+	while (len) {
+		u32 length;
 
-		if (!(apedata & APE_EVENT_STATUS_EVENT_PENDING))
-			tg3_ape_write32(tp, TG3_APE_EVENT_STATUS,
-					event | APE_EVENT_STATUS_EVENT_PENDING);
+		/* Cap xfer sizes to scratchpad limits. */
+		length = (len > maxlen) ? maxlen : len;
+		len -= length;
+
+		apedata = tg3_ape_read32(tp, TG3_APE_FW_STATUS);
+		if (!(apedata & APE_FW_STATUS_READY))
+			return -EAGAIN;
+
+		/* Wait for up to 1 msec for APE to service previous event. */
+		err = tg3_ape_event_lock(tp, 1000);
+		if (err)
+			return err;
+
+		apedata = APE_EVENT_STATUS_DRIVER_EVNT |
+			  APE_EVENT_STATUS_SCRTCHPD_READ |
+			  APE_EVENT_STATUS_EVENT_PENDING;
+		tg3_ape_write32(tp, TG3_APE_EVENT_STATUS, apedata);
+
+		tg3_ape_write32(tp, bufoff, base_off);
+		tg3_ape_write32(tp, bufoff + sizeof(u32), length);
 
 		tg3_ape_unlock(tp, TG3_APE_LOCK_MEM);
+		tg3_ape_write32(tp, TG3_APE_EVENT, APE_EVENT_1);
 
-		if (!(apedata & APE_EVENT_STATUS_EVENT_PENDING))
-			break;
+		base_off += length;
 
-		udelay(100);
+		if (tg3_ape_wait_for_event(tp, 30000))
+			return -EAGAIN;
+
+		for (i = 0; length; i += 4, length -= 4) {
+			u32 val = tg3_ape_read32(tp, msgoff + i);
+			memcpy(data, &val, sizeof(u32));
+			data++;
+		}
 	}
 
-	if (!(apedata & APE_EVENT_STATUS_EVENT_PENDING))
-		tg3_ape_write32(tp, TG3_APE_EVENT, APE_EVENT_1);
+	return 0;
+}
+
+static int tg3_ape_send_event(struct tg3 *tp, u32 event)
+{
+	int err;
+	u32 apedata;
+
+	apedata = tg3_ape_read32(tp, TG3_APE_SEG_SIG);
+	if (apedata != APE_SEG_SIG_MAGIC)
+		return -EAGAIN;
+
+	apedata = tg3_ape_read32(tp, TG3_APE_FW_STATUS);
+	if (!(apedata & APE_FW_STATUS_READY))
+		return -EAGAIN;
+
+	/* Wait for up to 1 millisecond for APE to service previous event. */
+	err = tg3_ape_event_lock(tp, 1000);
+	if (err)
+		return err;
+
+	tg3_ape_write32(tp, TG3_APE_EVENT_STATUS,
+			event | APE_EVENT_STATUS_EVENT_PENDING);
+
+	tg3_ape_unlock(tp, TG3_APE_LOCK_MEM);
+	tg3_ape_write32(tp, TG3_APE_EVENT, APE_EVENT_1);
+
+	return 0;
 }
 
 static void tg3_ape_driver_state_change(struct tg3 *tp, int kind)
@@ -960,6 +1064,8 @@ static int tg3_readphy(struct tg3 *tp, int reg, u32 *val)
 		udelay(80);
 	}
 
+	tg3_ape_lock(tp, tp->phy_ape_lock);
+
 	*val = 0x0;
 
 	frame_val  = ((tp->phy_addr << MI_COM_PHY_ADDR_SHIFT) &
@@ -994,6 +1100,8 @@ static int tg3_readphy(struct tg3 *tp, int reg, u32 *val)
 		udelay(80);
 	}
 
+	tg3_ape_unlock(tp, tp->phy_ape_lock);
+
 	return ret;
 }
 
@@ -1013,6 +1121,8 @@ static int tg3_writephy(struct tg3 *tp, int reg, u32 val)
 		udelay(80);
 	}
 
+	tg3_ape_lock(tp, tp->phy_ape_lock);
+
 	frame_val  = ((tp->phy_addr << MI_COM_PHY_ADDR_SHIFT) &
 		      MI_COM_PHY_ADDR_MASK);
 	frame_val |= ((reg << MI_COM_REG_ADDR_SHIFT) &
@@ -1043,6 +1153,8 @@ static int tg3_writephy(struct tg3 *tp, int reg, u32 val)
 		udelay(80);
 	}
 
+	tg3_ape_unlock(tp, tp->phy_ape_lock);
+
 	return ret;
 }
 
@@ -8974,8 +9086,7 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 	    GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_57780 ||
 	    tg3_flag(tp, 57765_PLUS)) {
 		val = tr32(TG3_RDMA_RSRVCTRL_REG);
-		if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5719 ||
-		    GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5720) {
+		if (tp->pci_chip_rev_id == CHIPREV_ID_5719_A0) {
 			val &= ~(TG3_RDMA_RSRVCTRL_TXMRGN_MASK |
 				 TG3_RDMA_RSRVCTRL_FIFO_LWM_MASK |
 				 TG3_RDMA_RSRVCTRL_FIFO_HWM_MASK);
@@ -9165,6 +9276,19 @@ static int tg3_reset_hw(struct tg3 *tp, int reset_phy)
 	tw32_f(RDMAC_MODE, rdmac_mode);
 	udelay(40);
 
+	if (GET_ASIC_REV(tp->pci_chip_rev_id) == ASIC_REV_5719) {
+		for (i = 0; i < TG3_NUM_RDMA_CHANNELS; i++) {
+			if (tr32(TG3_RDMA_LENGTH + (i << 2)) > TG3_MAX_MTU(tp))
+				break;
+		}
+		if (i < TG3_NUM_RDMA_CHANNELS) {
+			val = tr32(TG3_LSO_RD_DMA_CRPTEN_CTRL);
+			val |= TG3_LSO_RD_DMA_TX_LENGTH_WA;
+			tw32(TG3_LSO_RD_DMA_CRPTEN_CTRL, val);
+			tg3_flag_set(tp, 5719_RDMA_BUG);
+		}
+	}
+
 	tw32(RCVDCC_MODE, RCVDCC_MODE_ENABLE | RCVDCC_MODE_ATTN_ENABLE);
 	if (!tg3_flag(tp, 5705_PLUS))
 		tw32(MBFREE_MODE, MBFREE_MODE_ENABLE);
@@ -9393,6 +9517,110 @@ static int tg3_init_hw(struct tg3 *tp, int reset_phy)
 	return tg3_reset_hw(tp, reset_phy);
 }
 
+#if IS_ENABLED(CONFIG_HWMON)
+static void tg3_sd_scan_scratchpad(struct tg3 *tp, struct tg3_ocir *ocir)
+{
+	int i;
+
+	for (i = 0; i < TG3_SD_NUM_RECS; i++, ocir++) {
+		u32 off = i * TG3_OCIR_LEN, len = TG3_OCIR_LEN;
+
+		tg3_ape_scratchpad_read(tp, (u32 *) ocir, off, len);
+		off += len;
+
+		if (ocir->signature != TG3_OCIR_SIG_MAGIC ||
+		    !(ocir->version_flags & TG3_OCIR_FLAG_ACTIVE))
+			memset(ocir, 0, TG3_OCIR_LEN);
+	}
+}
+
+/* sysfs attributes for hwmon */
+static ssize_t tg3_show_temp(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct tg3 *tp = netdev_priv(netdev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	u32 temperature;
+
+	spin_lock_bh(&tp->lock);
+	tg3_ape_scratchpad_read(tp, &temperature, attr->index,
+				sizeof(temperature));
+	spin_unlock_bh(&tp->lock);
+	return sprintf(buf, "%u\n", temperature);
+}
+
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, tg3_show_temp, NULL,
+			  TG3_TEMP_SENSOR_OFFSET);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, tg3_show_temp, NULL,
+			  TG3_TEMP_CAUTION_OFFSET);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, tg3_show_temp, NULL,
+			  TG3_TEMP_MAX_OFFSET);
+
+static struct attribute *tg3_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group tg3_group = {
+	.attrs = tg3_attributes,
+};
+
+#endif
+
+static void tg3_hwmon_close(struct tg3 *tp)
+{
+#if IS_ENABLED(CONFIG_HWMON)
+	if (tp->hwmon_dev) {
+		hwmon_device_unregister(tp->hwmon_dev);
+		tp->hwmon_dev = NULL;
+		sysfs_remove_group(&tp->pdev->dev.kobj, &tg3_group);
+	}
+#endif
+}
+
+static void tg3_hwmon_open(struct tg3 *tp)
+{
+#if IS_ENABLED(CONFIG_HWMON)
+	int i, err;
+	u32 size = 0;
+	struct pci_dev *pdev = tp->pdev;
+	struct tg3_ocir ocirs[TG3_SD_NUM_RECS];
+
+	tg3_sd_scan_scratchpad(tp, ocirs);
+
+	for (i = 0; i < TG3_SD_NUM_RECS; i++) {
+		if (!ocirs[i].src_data_length)
+			continue;
+
+		size += ocirs[i].src_hdr_length;
+		size += ocirs[i].src_data_length;
+	}
+
+	if (!size)
+		return;
+
+	/* Register hwmon sysfs hooks */
+	err = sysfs_create_group(&pdev->dev.kobj, &tg3_group);
+	if (err) {
+		dev_err(&pdev->dev, "Cannot create sysfs group, aborting\n");
+		return;
+	}
+
+	tp->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(tp->hwmon_dev)) {
+		tp->hwmon_dev = NULL;
+		dev_err(&pdev->dev, "Cannot register hwmon device, aborting\n");
+		sysfs_remove_group(&pdev->dev.kobj, &tg3_group);
+	}
+#endif
+}
+
+
 #define TG3_STAT_ADD32(PSTAT, REG) \
 do {	u32 __val = tr32(REG); \
 	(PSTAT)->low += __val; \
@@ -9420,6 +9648,16 @@ static void tg3_periodic_fetch_stats(struct tg3 *tp)
 	TG3_STAT_ADD32(&sp->tx_ucast_packets, MAC_TX_STATS_UCAST);
 	TG3_STAT_ADD32(&sp->tx_mcast_packets, MAC_TX_STATS_MCAST);
 	TG3_STAT_ADD32(&sp->tx_bcast_packets, MAC_TX_STATS_BCAST);
+	if (unlikely(tg3_flag(tp, 5719_RDMA_BUG) &&
+		     (sp->tx_ucast_packets.low + sp->tx_mcast_packets.low +
+		      sp->tx_bcast_packets.low) > TG3_NUM_RDMA_CHANNELS)) {
+		u32 val;
+
+		val = tr32(TG3_LSO_RD_DMA_CRPTEN_CTRL);
+		val &= ~TG3_LSO_RD_DMA_TX_LENGTH_WA;
+		tw32(TG3_LSO_RD_DMA_CRPTEN_CTRL, val);
+		tg3_flag_clear(tp, 5719_RDMA_BUG);
+	}
 
 	TG3_STAT_ADD32(&sp->rx_octets, MAC_RX_STATS_OCTETS);
 	TG3_STAT_ADD32(&sp->rx_fragments, MAC_RX_STATS_FRAGMENTS);
@@ -9908,7 +10146,7 @@ static bool tg3_enable_msix(struct tg3 *tp)
 	int i, rc;
 	struct msix_entry msix_ent[tp->irq_max];
 
-	tp->irq_cnt = num_online_cpus();
+	tp->irq_cnt = netif_get_num_default_rss_queues();
 	if (tp->irq_cnt > 1) {
 		/* We want as many rx rings enabled as there are cpus.
 		 * In multiqueue MSI-X mode, the first MSI-X vector
@@ -10101,6 +10339,8 @@ static int tg3_open(struct net_device *dev)
 
 	tg3_phy_start(tp);
 
+	tg3_hwmon_open(tp);
+
 	tg3_full_lock(tp, 0);
 
 	tg3_timer_start(tp);
@@ -10150,6 +10390,8 @@ static int tg3_close(struct net_device *dev)
 
 	tg3_timer_stop(tp);
 
+	tg3_hwmon_close(tp);
+
 	tg3_phy_stop(tp);
 
 	tg3_full_lock(tp, 1);
@@ -12282,10 +12524,12 @@ static struct rtnl_link_stats64 *tg3_get_stats64(struct net_device *dev,
 {
 	struct tg3 *tp = netdev_priv(dev);
 
-	if (!tp->hw_stats)
+	spin_lock_bh(&tp->lock);
+	if (!tp->hw_stats) {
+		spin_unlock_bh(&tp->lock);
 		return &tp->net_stats_prev;
+	}
 
-	spin_lock_bh(&tp->lock);
 	tg3_get_nstats(tp, stats);
 	spin_unlock_bh(&tp->lock);
 
@@ -13448,6 +13692,23 @@ static int __devinit tg3_phy_probe(struct tg3 *tp)
 	tg3_flag_set(tp, PAUSE_AUTONEG);
 	tp->link_config.flowctrl = FLOW_CTRL_TX | FLOW_CTRL_RX;
 
+	if (tg3_flag(tp, ENABLE_APE)) {
+		switch (tp->pci_fn) {
+		case 0:
+			tp->phy_ape_lock = TG3_APE_LOCK_PHY0;
+			break;
+		case 1:
+			tp->phy_ape_lock = TG3_APE_LOCK_PHY1;
+			break;
+		case 2:
+			tp->phy_ape_lock = TG3_APE_LOCK_PHY2;
+			break;
+		case 3:
+			tp->phy_ape_lock = TG3_APE_LOCK_PHY3;
+			break;
+		}
+	}
+
 	if (tg3_flag(tp, USE_PHYLIB))
 		return tg3_phy_init(tp);
 
@@ -13857,14 +14118,9 @@ static void __devinit tg3_read_mgmtfw_ver(struct tg3 *tp)
 	}
 }
 
-static void __devinit tg3_read_dash_ver(struct tg3 *tp)
+static void __devinit tg3_probe_ncsi(struct tg3 *tp)
 {
-	int vlen;
 	u32 apedata;
-	char *fwtype;
-
-	if (!tg3_flag(tp, ENABLE_APE) || !tg3_flag(tp, ENABLE_ASF))
-		return;
 
 	apedata = tg3_ape_read32(tp, TG3_APE_SEG_SIG);
 	if (apedata != APE_SEG_SIG_MAGIC)
@@ -13874,14 +14130,22 @@ static void __devinit tg3_read_dash_ver(struct tg3 *tp)
 	if (!(apedata & APE_FW_STATUS_READY))
 		return;
 
+	if (tg3_ape_read32(tp, TG3_APE_FW_FEATURES) & TG3_APE_FW_FEATURE_NCSI)
+		tg3_flag_set(tp, APE_HAS_NCSI);
+}
+
+static void __devinit tg3_read_dash_ver(struct tg3 *tp)
+{
+	int vlen;
+	u32 apedata;
+	char *fwtype;
+
 	apedata = tg3_ape_read32(tp, TG3_APE_FW_VERSION);
 
-	if (tg3_ape_read32(tp, TG3_APE_FW_FEATURES) & TG3_APE_FW_FEATURE_NCSI) {
-		tg3_flag_set(tp, APE_HAS_NCSI);
+	if (tg3_flag(tp, APE_HAS_NCSI))
 		fwtype = "NCSI";
-	} else {
+	else
 		fwtype = "DASH";
-	}
 
 	vlen = strlen(tp->fw_ver);
 
@@ -13915,20 +14179,17 @@ static void __devinit tg3_read_fw_ver(struct tg3 *tp)
 		tg3_read_sb_ver(tp, val);
 	else if ((val & TG3_EEPROM_MAGIC_HW_MSK) == TG3_EEPROM_MAGIC_HW)
 		tg3_read_hwsb_ver(tp);
-	else
-		return;
 
-	if (vpd_vers)
-		goto done;
-
-	if (tg3_flag(tp, ENABLE_APE)) {
-		if (tg3_flag(tp, ENABLE_ASF))
-			tg3_read_dash_ver(tp);
-	} else if (tg3_flag(tp, ENABLE_ASF)) {
-		tg3_read_mgmtfw_ver(tp);
+	if (tg3_flag(tp, ENABLE_ASF)) {
+		if (tg3_flag(tp, ENABLE_APE)) {
+			tg3_probe_ncsi(tp);
+			if (!vpd_vers)
+				tg3_read_dash_ver(tp);
+		} else if (!vpd_vers) {
+			tg3_read_mgmtfw_ver(tp);
+		}
 	}
 
-done:
 	tp->fw_ver[TG3_VER_SIZE - 1] = 0;
 }
 
@@ -14168,7 +14429,7 @@ static int __devinit tg3_get_invariants(struct tg3 *tp)
 			if (bridge->subordinate &&
 			    (bridge->subordinate->number <=
 			     tp->pdev->bus->number) &&
-			    (bridge->subordinate->subordinate >=
+			    (bridge->subordinate->busn_res.end >=
 			     tp->pdev->bus->number)) {
 				tg3_flag_set(tp, 5701_DMA_BUG);
 				pci_dev_put(bridge);
@@ -14196,7 +14457,7 @@ static int __devinit tg3_get_invariants(struct tg3 *tp)
 			if (bridge && bridge->subordinate &&
 			    (bridge->subordinate->number <=
 			     tp->pdev->bus->number) &&
-			    (bridge->subordinate->subordinate >=
+			    (bridge->subordinate->busn_res.end >=
 			     tp->pdev->bus->number)) {
 				tg3_flag_set(tp, 40BIT_DMA_BUG);
 				pci_dev_put(bridge);
diff --git a/drivers/net/ethernet/broadcom/tg3.h b/drivers/net/ethernet/broadcom/tg3.h
index 93865f899a4..6d52cb28682 100644
--- a/drivers/net/ethernet/broadcom/tg3.h
+++ b/drivers/net/ethernet/broadcom/tg3.h
@@ -1376,7 +1376,11 @@
 #define TG3_LSO_RD_DMA_CRPTEN_CTRL	0x00004910
 #define TG3_LSO_RD_DMA_CRPTEN_CTRL_BLEN_BD_4K	 0x00030000
 #define TG3_LSO_RD_DMA_CRPTEN_CTRL_BLEN_LSO_4K	 0x000c0000
-/* 0x4914 --> 0x4c00 unused */
+#define TG3_LSO_RD_DMA_TX_LENGTH_WA	 0x02000000
+/* 0x4914 --> 0x4be0 unused */
+
+#define TG3_NUM_RDMA_CHANNELS		4
+#define TG3_RDMA_LENGTH			0x00004be0
 
 /* Write DMA control registers */
 #define WDMAC_MODE			0x00004c00
@@ -2311,10 +2315,11 @@
 #define  APE_LOCK_REQ_DRIVER		 0x00001000
 #define TG3_APE_LOCK_GRANT		0x004c
 #define  APE_LOCK_GRANT_DRIVER		 0x00001000
-#define TG3_APE_SEG_SIG			0x4000
-#define  APE_SEG_SIG_MAGIC		 0x41504521
 
 /* APE shared memory.  Accessible through BAR1 */
+#define TG3_APE_SHMEM_BASE		0x4000
+#define TG3_APE_SEG_SIG			0x4000
+#define  APE_SEG_SIG_MAGIC		 0x41504521
 #define TG3_APE_FW_STATUS		0x400c
 #define  APE_FW_STATUS_READY		 0x00000100
 #define TG3_APE_FW_FEATURES		0x4010
@@ -2327,6 +2332,8 @@
 #define  APE_FW_VERSION_REVMSK		 0x0000ff00
 #define  APE_FW_VERSION_REVSFT		 8
 #define  APE_FW_VERSION_BLDMSK		 0x000000ff
+#define TG3_APE_SEG_MSG_BUF_OFF		0x401c
+#define TG3_APE_SEG_MSG_BUF_LEN		0x4020
 #define TG3_APE_HOST_SEG_SIG		0x4200
 #define  APE_HOST_SEG_SIG_MAGIC		 0x484f5354
 #define TG3_APE_HOST_SEG_LEN		0x4204
@@ -2353,6 +2360,8 @@
 
 #define  APE_EVENT_STATUS_DRIVER_EVNT	 0x00000010
 #define  APE_EVENT_STATUS_STATE_CHNGE	 0x00000500
+#define  APE_EVENT_STATUS_SCRTCHPD_READ	 0x00001600
+#define  APE_EVENT_STATUS_SCRTCHPD_WRITE 0x00001700
 #define  APE_EVENT_STATUS_STATE_START	 0x00010000
 #define  APE_EVENT_STATUS_STATE_UNLOAD	 0x00020000
 #define  APE_EVENT_STATUS_STATE_WOL	 0x00030000
@@ -2671,6 +2680,40 @@ struct tg3_hw_stats {
 	u8				__reserved4[0xb00-0x9c8];
 };
 
+#define TG3_SD_NUM_RECS			3
+#define TG3_OCIR_LEN			(sizeof(struct tg3_ocir))
+#define TG3_OCIR_SIG_MAGIC		0x5253434f
+#define TG3_OCIR_FLAG_ACTIVE		0x00000001
+
+#define TG3_TEMP_CAUTION_OFFSET		0xc8
+#define TG3_TEMP_MAX_OFFSET		0xcc
+#define TG3_TEMP_SENSOR_OFFSET		0xd4
+
+
+struct tg3_ocir {
+	u32				signature;
+	u16				version_flags;
+	u16				refresh_int;
+	u32				refresh_tmr;
+	u32				update_tmr;
+	u32				dst_base_addr;
+	u16				src_hdr_offset;
+	u16				src_hdr_length;
+	u16				src_data_offset;
+	u16				src_data_length;
+	u16				dst_hdr_offset;
+	u16				dst_data_offset;
+	u16				dst_reg_upd_offset;
+	u16				dst_sem_offset;
+	u32				reserved1[2];
+	u32				port0_flags;
+	u32				port1_flags;
+	u32				port2_flags;
+	u32				port3_flags;
+	u32				reserved2[1];
+};
+
+
 /* 'mapping' is superfluous as the chip does not write into
  * the tx/rx post rings so we could just fetch it from there.
  * But the cache behavior is better how we are doing it now.
@@ -2920,6 +2963,7 @@ enum TG3_FLAGS {
 	TG3_FLAG_L1PLLPD_EN,
 	TG3_FLAG_APE_HAS_NCSI,
 	TG3_FLAG_4K_FIFO_LIMIT,
+	TG3_FLAG_5719_RDMA_BUG,
 	TG3_FLAG_RESET_TASK_PENDING,
 	TG3_FLAG_5705_PLUS,
 	TG3_FLAG_IS_5788,
@@ -3068,6 +3112,7 @@ struct tg3 {
 	int				old_link;
 
 	u8				phy_addr;
+	u8				phy_ape_lock;
 
 	/* PHY info */
 	u32				phy_id;
@@ -3206,6 +3251,10 @@ struct tg3 {
 	const char			*fw_needed;
 	const struct firmware		*fw;
 	u32				fw_len; /* includes BSS */
+
+#if IS_ENABLED(CONFIG_HWMON)
+	struct device			*hwmon_dev;
+#endif
 };
 
 #endif /* !(_T3_H) */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cee.c b/drivers/net/ethernet/brocade/bna/bfa_cee.c
index 689e5e19cc0..550d2521ba7 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cee.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_cee.c
@@ -52,13 +52,7 @@ bfa_cee_format_lldp_cfg(struct bfa_cee_lldp_cfg *lldp_cfg)
 }
 
 /**
- * bfa_cee_attr_meminfo()
- *
- * @brief Returns the size of the DMA memory needed by CEE attributes
- *
- * @param[in] void
- *
- * @return Size of DMA region
+ * bfa_cee_attr_meminfo - Returns the size of the DMA memory needed by CEE attributes
  */
 static u32
 bfa_cee_attr_meminfo(void)
@@ -66,13 +60,7 @@ bfa_cee_attr_meminfo(void)
 	return roundup(sizeof(struct bfa_cee_attr), BFA_DMA_ALIGN_SZ);
 }
 /**
- * bfa_cee_stats_meminfo()
- *
- * @brief Returns the size of the DMA memory needed by CEE stats
- *
- * @param[in] void
- *
- * @return Size of DMA region
+ * bfa_cee_stats_meminfo - Returns the size of the DMA memory needed by CEE stats
  */
 static u32
 bfa_cee_stats_meminfo(void)
@@ -81,14 +69,10 @@ bfa_cee_stats_meminfo(void)
 }
 
 /**
- * bfa_cee_get_attr_isr()
- *
- * @brief CEE ISR for get-attributes responses from f/w
- *
- * @param[in] cee - Pointer to the CEE module
- *            status - Return status from the f/w
+ * bfa_cee_get_attr_isr - CEE ISR for get-attributes responses from f/w
  *
- * @return void
+ * @cee: Pointer to the CEE module
+ * @status: Return status from the f/w
  */
 static void
 bfa_cee_get_attr_isr(struct bfa_cee *cee, enum bfa_status status)
@@ -105,14 +89,10 @@ bfa_cee_get_attr_isr(struct bfa_cee *cee, enum bfa_status status)
 }
 
 /**
- * bfa_cee_get_attr_isr()
- *
- * @brief CEE ISR for get-stats responses from f/w
+ * bfa_cee_get_attr_isr - CEE ISR for get-stats responses from f/w
  *
- * @param[in] cee - Pointer to the CEE module
- *            status - Return status from the f/w
- *
- * @return void
+ * @cee: Pointer to the CEE module
+ * @status: Return status from the f/w
  */
 static void
 bfa_cee_get_stats_isr(struct bfa_cee *cee, enum bfa_status status)
@@ -147,13 +127,7 @@ bfa_cee_reset_stats_isr(struct bfa_cee *cee, enum bfa_status status)
 		cee->cbfn.reset_stats_cbfn(cee->cbfn.reset_stats_cbarg, status);
 }
 /**
- * bfa_nw_cee_meminfo()
- *
- * @brief Returns the size of the DMA memory needed by CEE module
- *
- * @param[in] void
- *
- * @return Size of DMA region
+ * bfa_nw_cee_meminfo - Returns the size of the DMA memory needed by CEE module
  */
 u32
 bfa_nw_cee_meminfo(void)
@@ -162,15 +136,11 @@ bfa_nw_cee_meminfo(void)
 }
 
 /**
- * bfa_nw_cee_mem_claim()
- *
- * @brief Initialized CEE DMA Memory
- *
- * @param[in] cee CEE module pointer
- *	      dma_kva Kernel Virtual Address of CEE DMA Memory
- *	      dma_pa  Physical Address of CEE DMA Memory
+ * bfa_nw_cee_mem_claim - Initialized CEE DMA Memory
  *
- * @return void
+ * @cee: CEE module pointer
+ * @dma_kva: Kernel Virtual Address of CEE DMA Memory
+ * @dma_pa:  Physical Address of CEE DMA Memory
  */
 void
 bfa_nw_cee_mem_claim(struct bfa_cee *cee, u8 *dma_kva, u64 dma_pa)
@@ -185,13 +155,11 @@ bfa_nw_cee_mem_claim(struct bfa_cee *cee, u8 *dma_kva, u64 dma_pa)
 }
 
 /**
- * bfa_cee_get_attr()
- *
- * @brief	Send the request to the f/w to fetch CEE attributes.
+ * bfa_cee_get_attr - Send the request to the f/w to fetch CEE attributes.
  *
- * @param[in]	Pointer to the CEE module data structure.
+ * @cee: Pointer to the CEE module data structure.
  *
- * @return	Status
+ * Return: status
  */
 enum bfa_status
 bfa_nw_cee_get_attr(struct bfa_cee *cee, struct bfa_cee_attr *attr,
@@ -220,13 +188,7 @@ bfa_nw_cee_get_attr(struct bfa_cee *cee, struct bfa_cee_attr *attr,
 }
 
 /**
- * bfa_cee_isrs()
- *
- * @brief Handles Mail-box interrupts for CEE module.
- *
- * @param[in] Pointer to the CEE module data structure.
- *
- * @return void
+ * bfa_cee_isrs - Handles Mail-box interrupts for CEE module.
  */
 
 static void
@@ -253,14 +215,9 @@ bfa_cee_isr(void *cbarg, struct bfi_mbmsg *m)
 }
 
 /**
- * bfa_cee_notify()
- *
- * @brief CEE module heart-beat failure handler.
- * @brief CEE module IOC event handler.
- *
- * @param[in] IOC event type
+ * bfa_cee_notify - CEE module heart-beat failure handler.
  *
- * @return void
+ * @event: IOC event type
  */
 
 static void
@@ -307,17 +264,13 @@ bfa_cee_notify(void *arg, enum bfa_ioc_event event)
 }
 
 /**
- * bfa_nw_cee_attach()
- *
- * @brief CEE module-attach API
+ * bfa_nw_cee_attach - CEE module-attach API
  *
- * @param[in] cee - Pointer to the CEE module data structure
- *            ioc - Pointer to the ioc module data structure
- *            dev - Pointer to the device driver module data structure
- *                  The device driver specific mbox ISR functions have
- *                  this pointer as one of the parameters.
- *
- * @return void
+ * @cee: Pointer to the CEE module data structure
+ * @ioc: Pointer to the ioc module data structure
+ * @dev: Pointer to the device driver module data structure.
+ *       The device driver specific mbox ISR functions have
+ *       this pointer as one of the parameters.
  */
 void
 bfa_nw_cee_attach(struct bfa_cee *cee, struct bfa_ioc *ioc,
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cs.h b/drivers/net/ethernet/brocade/bna/bfa_cs.h
index 3da1a946ccd..ad004a4c389 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cs.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_cs.h
@@ -16,23 +16,18 @@
  * www.brocade.com
  */
 
-/**
- * @file bfa_cs.h BFA common services
- */
+/* BFA common services */
 
 #ifndef __BFA_CS_H__
 #define __BFA_CS_H__
 
 #include "cna.h"
 
-/**
- * @ BFA state machine interfaces
- */
+/* BFA state machine interfaces */
 
 typedef void (*bfa_sm_t)(void *sm, int event);
 
-/**
- * oc - object class eg. bfa_ioc
+/* oc - object class eg. bfa_ioc
  * st - state, eg. reset
  * otype - object type, eg. struct bfa_ioc
  * etype - object type, eg. enum ioc_event
@@ -45,9 +40,7 @@ typedef void (*bfa_sm_t)(void *sm, int event);
 #define bfa_sm_get_state(_sm)		((_sm)->sm)
 #define bfa_sm_cmp_state(_sm, _state)	((_sm)->sm == (bfa_sm_t)(_state))
 
-/**
- * For converting from state machine function to state encoding.
- */
+/* For converting from state machine function to state encoding. */
 struct bfa_sm_table {
 	bfa_sm_t	sm;	/*!< state machine function	*/
 	int		state;	/*!< state machine encoding	*/
@@ -55,13 +48,10 @@ struct bfa_sm_table {
 };
 #define BFA_SM(_sm)		((bfa_sm_t)(_sm))
 
-/**
- * State machine with entry actions.
- */
+/* State machine with entry actions. */
 typedef void (*bfa_fsm_t)(void *fsm, int event);
 
-/**
- * oc - object class eg. bfa_ioc
+/* oc - object class eg. bfa_ioc
  * st - state, eg. reset
  * otype - object type, eg. struct bfa_ioc
  * etype - object type, eg. enum ioc_event
@@ -90,9 +80,7 @@ bfa_sm_to_state(const struct bfa_sm_table *smt, bfa_sm_t sm)
 	return smt[i].state;
 }
 
-/**
- * @ Generic wait counter.
- */
+/* Generic wait counter. */
 
 typedef void (*bfa_wc_resume_t) (void *cbarg);
 
@@ -116,9 +104,7 @@ bfa_wc_down(struct bfa_wc *wc)
 		wc->wc_resume(wc->wc_cbarg);
 }
 
-/**
- * Initialize a waiting counter.
- */
+/* Initialize a waiting counter. */
 static inline void
 bfa_wc_init(struct bfa_wc *wc, bfa_wc_resume_t wc_resume, void *wc_cbarg)
 {
@@ -128,9 +114,7 @@ bfa_wc_init(struct bfa_wc *wc, bfa_wc_resume_t wc_resume, void *wc_cbarg)
 	bfa_wc_up(wc);
 }
 
-/**
- * Wait for counter to reach zero
- */
+/* Wait for counter to reach zero */
 static inline void
 bfa_wc_wait(struct bfa_wc *wc)
 {
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs.h b/drivers/net/ethernet/brocade/bna/bfa_defs.h
index 48f87733739..e423f82da49 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs.h
@@ -26,13 +26,9 @@
 #define BFA_STRING_32	32
 #define BFA_VERSION_LEN 64
 
-/**
- * ---------------------- adapter definitions ------------
- */
+/* ---------------------- adapter definitions ------------ */
 
-/**
- * BFA adapter level attributes.
- */
+/* BFA adapter level attributes. */
 enum {
 	BFA_ADAPTER_SERIAL_NUM_LEN = STRSZ(BFA_MFG_SERIALNUM_SIZE),
 					/*
@@ -74,18 +70,14 @@ struct bfa_adapter_attr {
 	u8		trunk_capable;
 };
 
-/**
- * ---------------------- IOC definitions ------------
- */
+/* ---------------------- IOC definitions ------------ */
 
 enum {
 	BFA_IOC_DRIVER_LEN	= 16,
 	BFA_IOC_CHIP_REV_LEN	= 8,
 };
 
-/**
- * Driver and firmware versions.
- */
+/* Driver and firmware versions. */
 struct bfa_ioc_driver_attr {
 	char		driver[BFA_IOC_DRIVER_LEN];	/*!< driver name */
 	char		driver_ver[BFA_VERSION_LEN];	/*!< driver version */
@@ -95,9 +87,7 @@ struct bfa_ioc_driver_attr {
 	char		ob_ver[BFA_VERSION_LEN];	/*!< openboot version */
 };
 
-/**
- * IOC PCI device attributes
- */
+/* IOC PCI device attributes */
 struct bfa_ioc_pci_attr {
 	u16	vendor_id;	/*!< PCI vendor ID */
 	u16	device_id;	/*!< PCI device ID */
@@ -108,9 +98,7 @@ struct bfa_ioc_pci_attr {
 	char		chip_rev[BFA_IOC_CHIP_REV_LEN];	 /*!< chip revision */
 };
 
-/**
- * IOC states
- */
+/* IOC states */
 enum bfa_ioc_state {
 	BFA_IOC_UNINIT		= 1,	/*!< IOC is in uninit state */
 	BFA_IOC_RESET		= 2,	/*!< IOC is in reset state */
@@ -127,9 +115,7 @@ enum bfa_ioc_state {
 	BFA_IOC_HWFAIL		= 13,	/*!< PCI mapping doesn't exist */
 };
 
-/**
- * IOC firmware stats
- */
+/* IOC firmware stats */
 struct bfa_fw_ioc_stats {
 	u32	enable_reqs;
 	u32	disable_reqs;
@@ -139,9 +125,7 @@ struct bfa_fw_ioc_stats {
 	u32	unknown_reqs;
 };
 
-/**
- * IOC driver stats
- */
+/* IOC driver stats */
 struct bfa_ioc_drv_stats {
 	u32	ioc_isrs;
 	u32	ioc_enables;
@@ -157,9 +141,7 @@ struct bfa_ioc_drv_stats {
 	u32	rsvd;
 };
 
-/**
- * IOC statistics
- */
+/* IOC statistics */
 struct bfa_ioc_stats {
 	struct bfa_ioc_drv_stats drv_stats; /*!< driver IOC stats */
 	struct bfa_fw_ioc_stats fw_stats;  /*!< firmware IOC stats */
@@ -171,9 +153,7 @@ enum bfa_ioc_type {
 	BFA_IOC_TYPE_LL		= 3,
 };
 
-/**
- * IOC attributes returned in queries
- */
+/* IOC attributes returned in queries */
 struct bfa_ioc_attr {
 	enum bfa_ioc_type ioc_type;
 	enum bfa_ioc_state		state;		/*!< IOC state      */
@@ -187,22 +167,16 @@ struct bfa_ioc_attr {
 	u8				rsvd[4];	/*!< 64bit align */
 };
 
-/**
- * Adapter capability mask definition
- */
+/* Adapter capability mask definition */
 enum {
 	BFA_CM_HBA	=	0x01,
 	BFA_CM_CNA	=	0x02,
 	BFA_CM_NIC	=	0x04,
 };
 
-/**
- * ---------------------- mfg definitions ------------
- */
+/* ---------------------- mfg definitions ------------ */
 
-/**
- * Checksum size
- */
+/* Checksum size */
 #define BFA_MFG_CHKSUM_SIZE			16
 
 #define BFA_MFG_PARTNUM_SIZE			14
@@ -213,8 +187,7 @@ enum {
 
 #pragma pack(1)
 
-/**
- * @brief BFA adapter manufacturing block definition.
+/* BFA adapter manufacturing block definition.
  *
  * All numerical fields are in big-endian format.
  */
@@ -256,9 +229,7 @@ struct bfa_mfg_block {
 
 #pragma pack()
 
-/**
- * ---------------------- pci definitions ------------
- */
+/* ---------------------- pci definitions ------------ */
 
 /*
  * PCI device ID information
@@ -275,9 +246,7 @@ enum {
 #define bfa_asic_id_ctc(device)			\
 	(bfa_asic_id_ct(device) || bfa_asic_id_ct2(device))
 
-/**
- * PCI sub-system device and vendor ID information
- */
+/* PCI sub-system device and vendor ID information */
 enum {
 	BFA_PCI_FCOE_SSDEVICE_ID	= 0x14,
 	BFA_PCI_CT2_SSID_FCoE		= 0x22,
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h b/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
index 8ab33ee2c2b..b39c5f23974 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
@@ -20,10 +20,7 @@
 
 #include "bfa_defs.h"
 
-/**
- * @brief
- * FC physical port statistics.
- */
+/* FC physical port statistics. */
 struct bfa_port_fc_stats {
 	u64	secs_reset;	/*!< Seconds since stats is reset */
 	u64	tx_frames;	/*!< Tx frames			*/
@@ -59,10 +56,7 @@ struct bfa_port_fc_stats {
 	u64	bbsc_link_resets; /*!< Credit Recovery-Link Resets   */
 };
 
-/**
- * @brief
- * Eth Physical Port statistics.
- */
+/* Eth Physical Port statistics. */
 struct bfa_port_eth_stats {
 	u64	secs_reset;	/*!< Seconds since stats is reset */
 	u64	frame_64;	/*!< Frames 64 bytes		*/
@@ -108,10 +102,7 @@ struct bfa_port_eth_stats {
 	u64	tx_iscsi_zero_pause; /*!< Tx iSCSI zero pause	*/
 };
 
-/**
- * @brief
- *		Port statistics.
- */
+/* Port statistics. */
 union bfa_port_stats_u {
 	struct bfa_port_fc_stats fc;
 	struct bfa_port_eth_stats eth;
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h b/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
index 6681fe87c1e..7fb396fe679 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
@@ -20,33 +20,23 @@
 
 #include "bfa_defs.h"
 
-/**
- * Manufacturing block version
- */
+/* Manufacturing block version */
 #define BFA_MFG_VERSION				3
 #define BFA_MFG_VERSION_UNINIT			0xFF
 
-/**
- * Manufacturing block encrypted version
- */
+/* Manufacturing block encrypted version */
 #define BFA_MFG_ENC_VER				2
 
-/**
- * Manufacturing block version 1 length
- */
+/* Manufacturing block version 1 length */
 #define BFA_MFG_VER1_LEN			128
 
-/**
- * Manufacturing block header length
- */
+/* Manufacturing block header length */
 #define BFA_MFG_HDR_LEN				4
 
 #define BFA_MFG_SERIALNUM_SIZE			11
 #define STRSZ(_n)				(((_n) + 4) & ~3)
 
-/**
- * Manufacturing card type
- */
+/* Manufacturing card type */
 enum {
 	BFA_MFG_TYPE_CB_MAX  = 825,      /*!< Crossbow card type max	*/
 	BFA_MFG_TYPE_FC8P2   = 825,      /*!< 8G 2port FC card		*/
@@ -70,9 +60,7 @@ enum {
 
 #pragma pack(1)
 
-/**
- * Check if Mezz card
- */
+/* Check if Mezz card */
 #define bfa_mfg_is_mezz(type) (( \
 	(type) == BFA_MFG_TYPE_JAYHAWK || \
 	(type) == BFA_MFG_TYPE_WANCHESE || \
@@ -127,9 +115,7 @@ do {								\
 	}							\
 } while (0)
 
-/**
- * VPD data length
- */
+/* VPD data length */
 #define BFA_MFG_VPD_LEN			512
 #define BFA_MFG_VPD_LEN_INVALID		0
 
@@ -137,9 +123,7 @@ do {								\
 #define BFA_MFG_VPD_PCI_VER_MASK	0x07	/*!< version mask 3 bits */
 #define BFA_MFG_VPD_PCI_VDR_MASK	0xf8	/*!< vendor mask 5 bits */
 
-/**
- * VPD vendor tag
- */
+/* VPD vendor tag */
 enum {
 	BFA_MFG_VPD_UNKNOWN	= 0,     /*!< vendor unknown		*/
 	BFA_MFG_VPD_IBM		= 1,     /*!< vendor IBM		*/
@@ -151,8 +135,7 @@ enum {
 	BFA_MFG_VPD_PCI_BRCD	= 0xf8,  /*!< PCI VPD Brocade		*/
 };
 
-/**
- * @brief BFA adapter flash vpd data definition.
+/* BFA adapter flash vpd data definition.
  *
  * All numerical fields are in big-endian format.
  */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_status.h b/drivers/net/ethernet/brocade/bna/bfa_defs_status.h
index 7c5fe6c2e80..ea9af9ae754 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_status.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_status.h
@@ -18,8 +18,7 @@
 #ifndef __BFA_DEFS_STATUS_H__
 #define __BFA_DEFS_STATUS_H__
 
-/**
- * API status return values
+/* API status return values
  *
  * NOTE: The error msgs are auto generated from the comments. Only singe line
  * comments are supported
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.c b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
index 0b640fafbda..959c58ef972 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
@@ -20,13 +20,9 @@
 #include "bfi_reg.h"
 #include "bfa_defs.h"
 
-/**
- * IOC local definitions
- */
+/* IOC local definitions */
 
-/**
- * Asic specific macros : see bfa_hw_cb.c and bfa_hw_ct.c for details.
- */
+/* Asic specific macros : see bfa_hw_cb.c and bfa_hw_ct.c for details. */
 
 #define bfa_ioc_firmware_lock(__ioc)			\
 			((__ioc)->ioc_hwif->ioc_firmware_lock(__ioc))
@@ -96,9 +92,7 @@ static void bfa_ioc_get_adapter_manufacturer(struct bfa_ioc *ioc,
 static void bfa_ioc_get_adapter_model(struct bfa_ioc *ioc, char *model);
 static u64 bfa_ioc_get_pwwn(struct bfa_ioc *ioc);
 
-/**
- * IOC state machine definitions/declarations
- */
+/* IOC state machine definitions/declarations */
 enum ioc_event {
 	IOC_E_RESET		= 1,	/*!< IOC reset request		*/
 	IOC_E_ENABLE		= 2,	/*!< IOC enable request		*/
@@ -148,9 +142,7 @@ static void bfa_iocpf_initfail(struct bfa_ioc *ioc);
 static void bfa_iocpf_getattrfail(struct bfa_ioc *ioc);
 static void bfa_iocpf_stop(struct bfa_ioc *ioc);
 
-/**
- * IOCPF state machine events
- */
+/* IOCPF state machine events */
 enum iocpf_event {
 	IOCPF_E_ENABLE		= 1,	/*!< IOCPF enable request	*/
 	IOCPF_E_DISABLE		= 2,	/*!< IOCPF disable request	*/
@@ -166,9 +158,7 @@ enum iocpf_event {
 	IOCPF_E_SEM_ERROR	= 12,   /*!< h/w sem mapping error	*/
 };
 
-/**
- * IOCPF states
- */
+/* IOCPF states */
 enum bfa_iocpf_state {
 	BFA_IOCPF_RESET		= 1,	/*!< IOC is in reset state */
 	BFA_IOCPF_SEMWAIT	= 2,	/*!< Waiting for IOC h/w semaphore */
@@ -215,21 +205,15 @@ static struct bfa_sm_table iocpf_sm_table[] = {
 	{BFA_SM(bfa_iocpf_sm_disabled), BFA_IOCPF_DISABLED},
 };
 
-/**
- * IOC State Machine
- */
+/* IOC State Machine */
 
-/**
- * Beginning state. IOC uninit state.
- */
+/* Beginning state. IOC uninit state. */
 static void
 bfa_ioc_sm_uninit_entry(struct bfa_ioc *ioc)
 {
 }
 
-/**
- * IOC is in uninit state.
- */
+/* IOC is in uninit state. */
 static void
 bfa_ioc_sm_uninit(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -243,18 +227,14 @@ bfa_ioc_sm_uninit(struct bfa_ioc *ioc, enum ioc_event event)
 	}
 }
 
-/**
- * Reset entry actions -- initialize state machine
- */
+/* Reset entry actions -- initialize state machine */
 static void
 bfa_ioc_sm_reset_entry(struct bfa_ioc *ioc)
 {
 	bfa_fsm_set_state(&ioc->iocpf, bfa_iocpf_sm_reset);
 }
 
-/**
- * IOC is in reset state.
- */
+/* IOC is in reset state. */
 static void
 bfa_ioc_sm_reset(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -282,8 +262,7 @@ bfa_ioc_sm_enabling_entry(struct bfa_ioc *ioc)
 	bfa_iocpf_enable(ioc);
 }
 
-/**
- * Host IOC function is being enabled, awaiting response from firmware.
+/* Host IOC function is being enabled, awaiting response from firmware.
  * Semaphore is acquired.
  */
 static void
@@ -325,9 +304,7 @@ bfa_ioc_sm_enabling(struct bfa_ioc *ioc, enum ioc_event event)
 	}
 }
 
-/**
- * Semaphore should be acquired for version check.
- */
+/* Semaphore should be acquired for version check. */
 static void
 bfa_ioc_sm_getattr_entry(struct bfa_ioc *ioc)
 {
@@ -336,9 +313,7 @@ bfa_ioc_sm_getattr_entry(struct bfa_ioc *ioc)
 	bfa_ioc_send_getattr(ioc);
 }
 
-/**
- * IOC configuration in progress. Timer is active.
- */
+/* IOC configuration in progress. Timer is active. */
 static void
 bfa_ioc_sm_getattr(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -419,9 +394,7 @@ bfa_ioc_sm_disabling_entry(struct bfa_ioc *ioc)
 	bfa_iocpf_disable(ioc);
 }
 
-/**
- * IOC is being disabled
- */
+/* IOC is being disabled */
 static void
 bfa_ioc_sm_disabling(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -449,9 +422,7 @@ bfa_ioc_sm_disabling(struct bfa_ioc *ioc, enum ioc_event event)
 	}
 }
 
-/**
- * IOC disable completion entry.
- */
+/* IOC disable completion entry. */
 static void
 bfa_ioc_sm_disabled_entry(struct bfa_ioc *ioc)
 {
@@ -485,9 +456,7 @@ bfa_ioc_sm_fail_retry_entry(struct bfa_ioc *ioc)
 {
 }
 
-/**
- * Hardware initialization retry.
- */
+/* Hardware initialization retry. */
 static void
 bfa_ioc_sm_fail_retry(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -534,9 +503,7 @@ bfa_ioc_sm_fail_entry(struct bfa_ioc *ioc)
 {
 }
 
-/**
- * IOC failure.
- */
+/* IOC failure. */
 static void
 bfa_ioc_sm_fail(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -568,9 +535,7 @@ bfa_ioc_sm_hwfail_entry(struct bfa_ioc *ioc)
 {
 }
 
-/**
- * IOC failure.
- */
+/* IOC failure. */
 static void
 bfa_ioc_sm_hwfail(struct bfa_ioc *ioc, enum ioc_event event)
 {
@@ -593,13 +558,9 @@ bfa_ioc_sm_hwfail(struct bfa_ioc *ioc, enum ioc_event event)
 	}
 }
 
-/**
- * IOCPF State Machine
- */
+/* IOCPF State Machine */
 
-/**
- * Reset entry actions -- initialize state machine
- */
+/* Reset entry actions -- initialize state machine */
 static void
 bfa_iocpf_sm_reset_entry(struct bfa_iocpf *iocpf)
 {
@@ -607,9 +568,7 @@ bfa_iocpf_sm_reset_entry(struct bfa_iocpf *iocpf)
 	iocpf->auto_recover = bfa_nw_auto_recover;
 }
 
-/**
- * Beginning state. IOC is in reset state.
- */
+/* Beginning state. IOC is in reset state. */
 static void
 bfa_iocpf_sm_reset(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -626,9 +585,7 @@ bfa_iocpf_sm_reset(struct bfa_iocpf *iocpf, enum iocpf_event event)
 	}
 }
 
-/**
- * Semaphore should be acquired for version check.
- */
+/* Semaphore should be acquired for version check. */
 static void
 bfa_iocpf_sm_fwcheck_entry(struct bfa_iocpf *iocpf)
 {
@@ -636,9 +593,7 @@ bfa_iocpf_sm_fwcheck_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_hw_sem_get(iocpf->ioc);
 }
 
-/**
- * Awaiting h/w semaphore to continue with version check.
- */
+/* Awaiting h/w semaphore to continue with version check. */
 static void
 bfa_iocpf_sm_fwcheck(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -683,9 +638,7 @@ bfa_iocpf_sm_fwcheck(struct bfa_iocpf *iocpf, enum iocpf_event event)
 	}
 }
 
-/**
- * Notify enable completion callback
- */
+/* Notify enable completion callback */
 static void
 bfa_iocpf_sm_mismatch_entry(struct bfa_iocpf *iocpf)
 {
@@ -698,9 +651,7 @@ bfa_iocpf_sm_mismatch_entry(struct bfa_iocpf *iocpf)
 		msecs_to_jiffies(BFA_IOC_TOV));
 }
 
-/**
- * Awaiting firmware version match.
- */
+/* Awaiting firmware version match. */
 static void
 bfa_iocpf_sm_mismatch(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -727,18 +678,14 @@ bfa_iocpf_sm_mismatch(struct bfa_iocpf *iocpf, enum iocpf_event event)
 	}
 }
 
-/**
- * Request for semaphore.
- */
+/* Request for semaphore. */
 static void
 bfa_iocpf_sm_semwait_entry(struct bfa_iocpf *iocpf)
 {
 	bfa_ioc_hw_sem_get(iocpf->ioc);
 }
 
-/**
- * Awaiting semaphore for h/w initialzation.
- */
+/* Awaiting semaphore for h/w initialzation. */
 static void
 bfa_iocpf_sm_semwait(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -778,8 +725,7 @@ bfa_iocpf_sm_hwinit_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_reset(iocpf->ioc, false);
 }
 
-/**
- * Hardware is being initialized. Interrupts are enabled.
+/* Hardware is being initialized. Interrupts are enabled.
  * Holding hardware semaphore lock.
  */
 static void
@@ -822,8 +768,7 @@ bfa_iocpf_sm_enabling_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_send_enable(iocpf->ioc);
 }
 
-/**
- * Host IOC function is being enabled, awaiting response from firmware.
+/* Host IOC function is being enabled, awaiting response from firmware.
  * Semaphore is acquired.
  */
 static void
@@ -896,9 +841,7 @@ bfa_iocpf_sm_disabling_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_send_disable(iocpf->ioc);
 }
 
-/**
- * IOC is being disabled
- */
+/* IOC is being disabled */
 static void
 bfa_iocpf_sm_disabling(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -935,9 +878,7 @@ bfa_iocpf_sm_disabling_sync_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_hw_sem_get(iocpf->ioc);
 }
 
-/**
- * IOC hb ack request is being removed.
- */
+/* IOC hb ack request is being removed. */
 static void
 bfa_iocpf_sm_disabling_sync(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -963,9 +904,7 @@ bfa_iocpf_sm_disabling_sync(struct bfa_iocpf *iocpf, enum iocpf_event event)
 	}
 }
 
-/**
- * IOC disable completion entry.
- */
+/* IOC disable completion entry. */
 static void
 bfa_iocpf_sm_disabled_entry(struct bfa_iocpf *iocpf)
 {
@@ -1000,9 +939,7 @@ bfa_iocpf_sm_initfail_sync_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_hw_sem_get(iocpf->ioc);
 }
 
-/**
- * Hardware initialization failed.
- */
+/* Hardware initialization failed. */
 static void
 bfa_iocpf_sm_initfail_sync(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -1046,9 +983,7 @@ bfa_iocpf_sm_initfail_entry(struct bfa_iocpf *iocpf)
 {
 }
 
-/**
- * Hardware initialization failed.
- */
+/* Hardware initialization failed. */
 static void
 bfa_iocpf_sm_initfail(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -1084,9 +1019,7 @@ bfa_iocpf_sm_fail_sync_entry(struct bfa_iocpf *iocpf)
 	bfa_ioc_hw_sem_get(iocpf->ioc);
 }
 
-/**
- * IOC is in failed state.
- */
+/* IOC is in failed state. */
 static void
 bfa_iocpf_sm_fail_sync(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -1134,10 +1067,7 @@ bfa_iocpf_sm_fail_entry(struct bfa_iocpf *iocpf)
 {
 }
 
-/**
- * @brief
- * IOC is in failed state.
- */
+/* IOC is in failed state. */
 static void
 bfa_iocpf_sm_fail(struct bfa_iocpf *iocpf, enum iocpf_event event)
 {
@@ -1151,13 +1081,9 @@ bfa_iocpf_sm_fail(struct bfa_iocpf *iocpf, enum iocpf_event event)
 	}
 }
 
-/**
- * BFA IOC private functions
- */
+/* BFA IOC private functions */
 
-/**
- * Notify common modules registered for notification.
- */
+/* Notify common modules registered for notification. */
 static void
 bfa_ioc_event_notify(struct bfa_ioc *ioc, enum bfa_ioc_event event)
 {
@@ -1298,10 +1224,7 @@ bfa_ioc_hw_sem_get_cancel(struct bfa_ioc *ioc)
 	del_timer(&ioc->sem_timer);
 }
 
-/**
- * @brief
- * Initialize LPU local memory (aka secondary memory / SRAM)
- */
+/* Initialize LPU local memory (aka secondary memory / SRAM) */
 static void
 bfa_ioc_lmem_init(struct bfa_ioc *ioc)
 {
@@ -1366,9 +1289,7 @@ bfa_ioc_lpu_stop(struct bfa_ioc *ioc)
 	writel(pss_ctl, ioc->ioc_regs.pss_ctl_reg);
 }
 
-/**
- * Get driver and firmware versions.
- */
+/* Get driver and firmware versions. */
 void
 bfa_nw_ioc_fwver_get(struct bfa_ioc *ioc, struct bfi_ioc_image_hdr *fwhdr)
 {
@@ -1388,9 +1309,7 @@ bfa_nw_ioc_fwver_get(struct bfa_ioc *ioc, struct bfi_ioc_image_hdr *fwhdr)
 	}
 }
 
-/**
- * Returns TRUE if same.
- */
+/* Returns TRUE if same. */
 bool
 bfa_nw_ioc_fwver_cmp(struct bfa_ioc *ioc, struct bfi_ioc_image_hdr *fwhdr)
 {
@@ -1408,8 +1327,7 @@ bfa_nw_ioc_fwver_cmp(struct bfa_ioc *ioc, struct bfi_ioc_image_hdr *fwhdr)
 	return true;
 }
 
-/**
- * Return true if current running version is valid. Firmware signature and
+/* Return true if current running version is valid. Firmware signature and
  * execution context (driver/bios) must match.
  */
 static bool
@@ -1430,9 +1348,7 @@ bfa_ioc_fwver_valid(struct bfa_ioc *ioc, u32 boot_env)
 	return bfa_nw_ioc_fwver_cmp(ioc, &fwhdr);
 }
 
-/**
- * Conditionally flush any pending message from firmware at start.
- */
+/* Conditionally flush any pending message from firmware at start. */
 static void
 bfa_ioc_msgflush(struct bfa_ioc *ioc)
 {
@@ -1443,9 +1359,6 @@ bfa_ioc_msgflush(struct bfa_ioc *ioc)
 		writel(1, ioc->ioc_regs.lpu_mbox_cmd);
 }
 
-/**
- * @img ioc_init_logic.jpg
- */
 static void
 bfa_ioc_hwinit(struct bfa_ioc *ioc, bool force)
 {
@@ -1603,10 +1516,7 @@ bfa_ioc_hb_stop(struct bfa_ioc *ioc)
 	del_timer(&ioc->hb_timer);
 }
 
-/**
- * @brief
- *	Initiate a full firmware download.
- */
+/* Initiate a full firmware download. */
 static void
 bfa_ioc_download_fw(struct bfa_ioc *ioc, u32 boot_type,
 		    u32 boot_env)
@@ -1672,9 +1582,7 @@ bfa_ioc_reset(struct bfa_ioc *ioc, bool force)
 	bfa_ioc_hwinit(ioc, force);
 }
 
-/**
- * BFA ioc enable reply by firmware
- */
+/* BFA ioc enable reply by firmware */
 static void
 bfa_ioc_enable_reply(struct bfa_ioc *ioc, enum bfa_mode port_mode,
 			u8 cap_bm)
@@ -1686,10 +1594,7 @@ bfa_ioc_enable_reply(struct bfa_ioc *ioc, enum bfa_mode port_mode,
 	bfa_fsm_send_event(iocpf, IOCPF_E_FWRSP_ENABLE);
 }
 
-/**
- * @brief
- * Update BFA configuration from firmware configuration.
- */
+/* Update BFA configuration from firmware configuration. */
 static void
 bfa_ioc_getattr_reply(struct bfa_ioc *ioc)
 {
@@ -1702,9 +1607,7 @@ bfa_ioc_getattr_reply(struct bfa_ioc *ioc)
 	bfa_fsm_send_event(ioc, IOC_E_FWRSP_GETATTR);
 }
 
-/**
- * Attach time initialization of mbox logic.
- */
+/* Attach time initialization of mbox logic. */
 static void
 bfa_ioc_mbox_attach(struct bfa_ioc *ioc)
 {
@@ -1718,9 +1621,7 @@ bfa_ioc_mbox_attach(struct bfa_ioc *ioc)
 	}
 }
 
-/**
- * Mbox poll timer -- restarts any pending mailbox requests.
- */
+/* Mbox poll timer -- restarts any pending mailbox requests. */
 static void
 bfa_ioc_mbox_poll(struct bfa_ioc *ioc)
 {
@@ -1760,9 +1661,7 @@ bfa_ioc_mbox_poll(struct bfa_ioc *ioc)
 	}
 }
 
-/**
- * Cleanup any pending requests.
- */
+/* Cleanup any pending requests. */
 static void
 bfa_ioc_mbox_flush(struct bfa_ioc *ioc)
 {
@@ -1774,12 +1673,12 @@ bfa_ioc_mbox_flush(struct bfa_ioc *ioc)
 }
 
 /**
- * Read data from SMEM to host through PCI memmap
+ * bfa_nw_ioc_smem_read - Read data from SMEM to host through PCI memmap
  *
- * @param[in]  ioc     memory for IOC
- * @param[in]  tbuf    app memory to store data from smem
- * @param[in]  soff    smem offset
- * @param[in]  sz      size of smem in bytes
+ * @ioc:     memory for IOC
+ * @tbuf:    app memory to store data from smem
+ * @soff:    smem offset
+ * @sz:      size of smem in bytes
  */
 static int
 bfa_nw_ioc_smem_read(struct bfa_ioc *ioc, void *tbuf, u32 soff, u32 sz)
@@ -1826,9 +1725,7 @@ bfa_nw_ioc_smem_read(struct bfa_ioc *ioc, void *tbuf, u32 soff, u32 sz)
 	return 0;
 }
 
-/**
- * Retrieve saved firmware trace from a prior IOC failure.
- */
+/* Retrieve saved firmware trace from a prior IOC failure. */
 int
 bfa_nw_ioc_debug_fwtrc(struct bfa_ioc *ioc, void *trcdata, int *trclen)
 {
@@ -1844,9 +1741,7 @@ bfa_nw_ioc_debug_fwtrc(struct bfa_ioc *ioc, void *trcdata, int *trclen)
 	return status;
 }
 
-/**
- * Save firmware trace if configured.
- */
+/* Save firmware trace if configured. */
 static void
 bfa_nw_ioc_debug_save_ftrc(struct bfa_ioc *ioc)
 {
@@ -1861,9 +1756,7 @@ bfa_nw_ioc_debug_save_ftrc(struct bfa_ioc *ioc)
 	}
 }
 
-/**
- * Retrieve saved firmware trace from a prior IOC failure.
- */
+/* Retrieve saved firmware trace from a prior IOC failure. */
 int
 bfa_nw_ioc_debug_fwsave(struct bfa_ioc *ioc, void *trcdata, int *trclen)
 {
@@ -1892,9 +1785,7 @@ bfa_ioc_fail_notify(struct bfa_ioc *ioc)
 	bfa_nw_ioc_debug_save_ftrc(ioc);
 }
 
-/**
- * IOCPF to IOC interface
- */
+/* IOCPF to IOC interface */
 static void
 bfa_ioc_pf_enabled(struct bfa_ioc *ioc)
 {
@@ -1928,9 +1819,7 @@ bfa_ioc_pf_fwmismatch(struct bfa_ioc *ioc)
 	ioc->cbfn->enable_cbfn(ioc->bfa, BFA_STATUS_IOC_FAILURE);
 }
 
-/**
- * IOC public
- */
+/* IOC public */
 static enum bfa_status
 bfa_ioc_pll_init(struct bfa_ioc *ioc)
 {
@@ -1954,8 +1843,7 @@ bfa_ioc_pll_init(struct bfa_ioc *ioc)
 	return BFA_STATUS_OK;
 }
 
-/**
- * Interface used by diag module to do firmware boot with memory test
+/* Interface used by diag module to do firmware boot with memory test
  * as the entry vector.
  */
 static void
@@ -1983,9 +1871,7 @@ bfa_ioc_boot(struct bfa_ioc *ioc, enum bfi_fwboot_type boot_type,
 	bfa_ioc_lpu_start(ioc);
 }
 
-/**
- * Enable/disable IOC failure auto recovery.
- */
+/* Enable/disable IOC failure auto recovery. */
 void
 bfa_nw_ioc_auto_recover(bool auto_recover)
 {
@@ -2056,10 +1942,10 @@ bfa_ioc_isr(struct bfa_ioc *ioc, struct bfi_mbmsg *m)
 }
 
 /**
- * IOC attach time initialization and setup.
+ * bfa_nw_ioc_attach - IOC attach time initialization and setup.
  *
- * @param[in]	ioc	memory for IOC
- * @param[in]	bfa	driver instance structure
+ * @ioc:	memory for IOC
+ * @bfa:	driver instance structure
  */
 void
 bfa_nw_ioc_attach(struct bfa_ioc *ioc, void *bfa, struct bfa_ioc_cbfn *cbfn)
@@ -2078,9 +1964,7 @@ bfa_nw_ioc_attach(struct bfa_ioc *ioc, void *bfa, struct bfa_ioc_cbfn *cbfn)
 	bfa_fsm_send_event(ioc, IOC_E_RESET);
 }
 
-/**
- * Driver detach time IOC cleanup.
- */
+/* Driver detach time IOC cleanup. */
 void
 bfa_nw_ioc_detach(struct bfa_ioc *ioc)
 {
@@ -2091,9 +1975,9 @@ bfa_nw_ioc_detach(struct bfa_ioc *ioc)
 }
 
 /**
- * Setup IOC PCI properties.
+ * bfa_nw_ioc_pci_init - Setup IOC PCI properties.
  *
- * @param[in]	pcidev	PCI device information for this IOC
+ * @pcidev:	PCI device information for this IOC
  */
 void
 bfa_nw_ioc_pci_init(struct bfa_ioc *ioc, struct bfa_pcidev *pcidev,
@@ -2160,10 +2044,10 @@ bfa_nw_ioc_pci_init(struct bfa_ioc *ioc, struct bfa_pcidev *pcidev,
 }
 
 /**
- * Initialize IOC dma memory
+ * bfa_nw_ioc_mem_claim - Initialize IOC dma memory
  *
- * @param[in]	dm_kva	kernel virtual address of IOC dma memory
- * @param[in]	dm_pa	physical address of IOC dma memory
+ * @dm_kva:	kernel virtual address of IOC dma memory
+ * @dm_pa:	physical address of IOC dma memory
  */
 void
 bfa_nw_ioc_mem_claim(struct bfa_ioc *ioc,  u8 *dm_kva, u64 dm_pa)
@@ -2176,9 +2060,7 @@ bfa_nw_ioc_mem_claim(struct bfa_ioc *ioc,  u8 *dm_kva, u64 dm_pa)
 	ioc->attr = (struct bfi_ioc_attr *) dm_kva;
 }
 
-/**
- * Return size of dma memory required.
- */
+/* Return size of dma memory required. */
 u32
 bfa_nw_ioc_meminfo(void)
 {
@@ -2201,9 +2083,7 @@ bfa_nw_ioc_disable(struct bfa_ioc *ioc)
 	bfa_fsm_send_event(ioc, IOC_E_DISABLE);
 }
 
-/**
- * Initialize memory for saving firmware trace.
- */
+/* Initialize memory for saving firmware trace. */
 void
 bfa_nw_ioc_debug_memclaim(struct bfa_ioc *ioc, void *dbg_fwsave)
 {
@@ -2217,9 +2097,7 @@ bfa_ioc_smem_pgnum(struct bfa_ioc *ioc, u32 fmaddr)
 	return PSS_SMEM_PGNUM(ioc->ioc_regs.smem_pg0, fmaddr);
 }
 
-/**
- * Register mailbox message handler function, to be called by common modules
- */
+/* Register mailbox message handler function, to be called by common modules */
 void
 bfa_nw_ioc_mbox_regisr(struct bfa_ioc *ioc, enum bfi_mclass mc,
 		    bfa_ioc_mbox_mcfunc_t cbfn, void *cbarg)
@@ -2231,11 +2109,12 @@ bfa_nw_ioc_mbox_regisr(struct bfa_ioc *ioc, enum bfi_mclass mc,
 }
 
 /**
- * Queue a mailbox command request to firmware. Waits if mailbox is busy.
- * Responsibility of caller to serialize
+ * bfa_nw_ioc_mbox_queue - Queue a mailbox command request to firmware.
  *
- * @param[in]	ioc	IOC instance
- * @param[i]	cmd	Mailbox command
+ * @ioc:	IOC instance
+ * @cmd:	Mailbox command
+ *
+ * Waits if mailbox is busy. Responsibility of caller to serialize
  */
 bool
 bfa_nw_ioc_mbox_queue(struct bfa_ioc *ioc, struct bfa_mbox_cmd *cmd,
@@ -2272,9 +2151,7 @@ bfa_nw_ioc_mbox_queue(struct bfa_ioc *ioc, struct bfa_mbox_cmd *cmd,
 	return false;
 }
 
-/**
- * Handle mailbox interrupts
- */
+/* Handle mailbox interrupts */
 void
 bfa_nw_ioc_mbox_isr(struct bfa_ioc *ioc)
 {
@@ -2314,9 +2191,7 @@ bfa_nw_ioc_error_isr(struct bfa_ioc *ioc)
 	bfa_fsm_send_event(ioc, IOC_E_HWERROR);
 }
 
-/**
- * return true if IOC is disabled
- */
+/* return true if IOC is disabled */
 bool
 bfa_nw_ioc_is_disabled(struct bfa_ioc *ioc)
 {
@@ -2324,17 +2199,14 @@ bfa_nw_ioc_is_disabled(struct bfa_ioc *ioc)
 		bfa_fsm_cmp_state(ioc, bfa_ioc_sm_disabled);
 }
 
-/**
- * return true if IOC is operational
- */
+/* return true if IOC is operational */
 bool
 bfa_nw_ioc_is_operational(struct bfa_ioc *ioc)
 {
 	return bfa_fsm_cmp_state(ioc, bfa_ioc_sm_op);
 }
 
-/**
- * Add to IOC heartbeat failure notification queue. To be used by common
+/* Add to IOC heartbeat failure notification queue. To be used by common
  * modules such as cee, port, diag.
  */
 void
@@ -2518,9 +2390,7 @@ bfa_nw_ioc_get_attr(struct bfa_ioc *ioc, struct bfa_ioc_attr *ioc_attr)
 	bfa_ioc_get_pci_chip_rev(ioc, ioc_attr->pci_attr.chip_rev);
 }
 
-/**
- * WWN public
- */
+/* WWN public */
 static u64
 bfa_ioc_get_pwwn(struct bfa_ioc *ioc)
 {
@@ -2533,9 +2403,7 @@ bfa_nw_ioc_get_mac(struct bfa_ioc *ioc)
 	return ioc->attr->mac;
 }
 
-/**
- * Firmware failure detected. Start recovery actions.
- */
+/* Firmware failure detected. Start recovery actions. */
 static void
 bfa_ioc_recover(struct bfa_ioc *ioc)
 {
@@ -2545,10 +2413,7 @@ bfa_ioc_recover(struct bfa_ioc *ioc)
 	bfa_fsm_send_event(ioc, IOC_E_HBFAIL);
 }
 
-/**
- * @dg hal_iocpf_pvt BFA IOC PF private functions
- * @{
- */
+/* BFA IOC PF private functions */
 
 static void
 bfa_iocpf_enable(struct bfa_ioc *ioc)
@@ -2669,8 +2534,6 @@ bfa_flash_notify(void *cbarg, enum bfa_ioc_event event)
 
 /*
  * Send flash write request.
- *
- * @param[in] cbarg - callback argument
  */
 static void
 bfa_flash_write_send(struct bfa_flash *flash)
@@ -2699,10 +2562,10 @@ bfa_flash_write_send(struct bfa_flash *flash)
 	flash->offset += len;
 }
 
-/*
- * Send flash read request.
+/**
+ * bfa_flash_read_send - Send flash read request.
  *
- * @param[in] cbarg - callback argument
+ * @cbarg: callback argument
  */
 static void
 bfa_flash_read_send(void *cbarg)
@@ -2724,11 +2587,11 @@ bfa_flash_read_send(void *cbarg)
 	bfa_nw_ioc_mbox_queue(flash->ioc, &flash->mb, NULL, NULL);
 }
 
-/*
- * Process flash response messages upon receiving interrupts.
+/**
+ * bfa_flash_intr - Process flash response messages upon receiving interrupts.
  *
- * @param[in] flasharg - flash structure
- * @param[in] msg - message structure
+ * @flasharg: flash structure
+ * @msg: message structure
  */
 static void
 bfa_flash_intr(void *flasharg, struct bfi_mbmsg *msg)
@@ -2821,12 +2684,12 @@ bfa_nw_flash_meminfo(void)
 	return roundup(BFA_FLASH_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
 }
 
-/*
- * Flash attach API.
+/**
+ * bfa_nw_flash_attach - Flash attach API.
  *
- * @param[in] flash - flash structure
- * @param[in] ioc  - ioc structure
- * @param[in] dev  - device structure
+ * @flash: flash structure
+ * @ioc: ioc structure
+ * @dev: device structure
  */
 void
 bfa_nw_flash_attach(struct bfa_flash *flash, struct bfa_ioc *ioc, void *dev)
@@ -2842,12 +2705,12 @@ bfa_nw_flash_attach(struct bfa_flash *flash, struct bfa_ioc *ioc, void *dev)
 	list_add_tail(&flash->ioc_notify.qe, &flash->ioc->notify_q);
 }
 
-/*
- * Claim memory for flash
+/**
+ * bfa_nw_flash_memclaim - Claim memory for flash
  *
- * @param[in] flash - flash structure
- * @param[in] dm_kva - pointer to virtual memory address
- * @param[in] dm_pa - physical memory address
+ * @flash: flash structure
+ * @dm_kva: pointer to virtual memory address
+ * @dm_pa: physical memory address
  */
 void
 bfa_nw_flash_memclaim(struct bfa_flash *flash, u8 *dm_kva, u64 dm_pa)
@@ -2859,13 +2722,13 @@ bfa_nw_flash_memclaim(struct bfa_flash *flash, u8 *dm_kva, u64 dm_pa)
 	dm_pa += roundup(BFA_FLASH_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
 }
 
-/*
- * Get flash attribute.
+/**
+ * bfa_nw_flash_get_attr - Get flash attribute.
  *
- * @param[in] flash - flash structure
- * @param[in] attr - flash attribute structure
- * @param[in] cbfn - callback function
- * @param[in] cbarg - callback argument
+ * @flash: flash structure
+ * @attr: flash attribute structure
+ * @cbfn: callback function
+ * @cbarg: callback argument
  *
  * Return status.
  */
@@ -2895,17 +2758,17 @@ bfa_nw_flash_get_attr(struct bfa_flash *flash, struct bfa_flash_attr *attr,
 	return BFA_STATUS_OK;
 }
 
-/*
- * Update flash partition.
+/**
+ * bfa_nw_flash_update_part - Update flash partition.
  *
- * @param[in] flash - flash structure
- * @param[in] type - flash partition type
- * @param[in] instance - flash partition instance
- * @param[in] buf - update data buffer
- * @param[in] len - data buffer length
- * @param[in] offset - offset relative to the partition starting address
- * @param[in] cbfn - callback function
- * @param[in] cbarg - callback argument
+ * @flash: flash structure
+ * @type: flash partition type
+ * @instance: flash partition instance
+ * @buf: update data buffer
+ * @len: data buffer length
+ * @offset: offset relative to the partition starting address
+ * @cbfn: callback function
+ * @cbarg: callback argument
  *
  * Return status.
  */
@@ -2944,17 +2807,17 @@ bfa_nw_flash_update_part(struct bfa_flash *flash, u32 type, u8 instance,
 	return BFA_STATUS_OK;
 }
 
-/*
- * Read flash partition.
+/**
+ * bfa_nw_flash_read_part - Read flash partition.
  *
- * @param[in] flash - flash structure
- * @param[in] type - flash partition type
- * @param[in] instance - flash partition instance
- * @param[in] buf - read data buffer
- * @param[in] len - data buffer length
- * @param[in] offset - offset relative to the partition starting address
- * @param[in] cbfn - callback function
- * @param[in] cbarg - callback argument
+ * @flash: flash structure
+ * @type: flash partition type
+ * @instance: flash partition instance
+ * @buf: read data buffer
+ * @len: data buffer length
+ * @offset: offset relative to the partition starting address
+ * @cbfn: callback function
+ * @cbarg: callback argument
  *
  * Return status.
  */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.h b/drivers/net/ethernet/brocade/bna/bfa_ioc.h
index 3b4460fdc14..63a85e555df 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.h
@@ -30,9 +30,7 @@
 #define BNA_DBG_FWTRC_LEN      (BFI_IOC_TRC_ENTS * BFI_IOC_TRC_ENT_SZ + \
 				BFI_IOC_TRC_HDR_SZ)
 
-/**
- * PCI device information required by IOC
- */
+/* PCI device information required by IOC */
 struct bfa_pcidev {
 	int	pci_slot;
 	u8	pci_func;
@@ -41,8 +39,7 @@ struct bfa_pcidev {
 	void	__iomem *pci_bar_kva;
 };
 
-/**
- * Structure used to remember the DMA-able memory block's KVA and Physical
+/* Structure used to remember the DMA-able memory block's KVA and Physical
  * Address
  */
 struct bfa_dma {
@@ -52,15 +49,11 @@ struct bfa_dma {
 
 #define BFA_DMA_ALIGN_SZ	256
 
-/**
- * smem size for Crossbow and Catapult
- */
+/* smem size for Crossbow and Catapult */
 #define BFI_SMEM_CB_SIZE	0x200000U	/* ! 2MB for crossbow	*/
 #define BFI_SMEM_CT_SIZE	0x280000U	/* ! 2.5MB for catapult	*/
 
-/**
- * @brief BFA dma address assignment macro. (big endian format)
- */
+/* BFA dma address assignment macro. (big endian format) */
 #define bfa_dma_be_addr_set(dma_addr, pa)	\
 		__bfa_dma_be_addr_set(&dma_addr, (u64)pa)
 static inline void
@@ -108,9 +101,7 @@ struct bfa_ioc_regs {
 	u32	smem_pg0;
 };
 
-/**
- * IOC Mailbox structures
- */
+/* IOC Mailbox structures */
 typedef void (*bfa_mbox_cmd_cbfn_t)(void *cbarg);
 struct bfa_mbox_cmd {
 	struct list_head	qe;
@@ -119,9 +110,7 @@ struct bfa_mbox_cmd {
 	u32     msg[BFI_IOC_MSGSZ];
 };
 
-/**
- * IOC mailbox module
- */
+/* IOC mailbox module */
 typedef void (*bfa_ioc_mbox_mcfunc_t)(void *cbarg, struct bfi_mbmsg *m);
 struct bfa_ioc_mbox_mod {
 	struct list_head	cmd_q;		/*!< pending mbox queue	*/
@@ -132,9 +121,7 @@ struct bfa_ioc_mbox_mod {
 	} mbhdlr[BFI_MC_MAX];
 };
 
-/**
- * IOC callback function interfaces
- */
+/* IOC callback function interfaces */
 typedef void (*bfa_ioc_enable_cbfn_t)(void *bfa, enum bfa_status status);
 typedef void (*bfa_ioc_disable_cbfn_t)(void *bfa);
 typedef void (*bfa_ioc_hbfail_cbfn_t)(void *bfa);
@@ -146,9 +133,7 @@ struct bfa_ioc_cbfn {
 	bfa_ioc_reset_cbfn_t	reset_cbfn;
 };
 
-/**
- * IOC event notification mechanism.
- */
+/* IOC event notification mechanism. */
 enum bfa_ioc_event {
 	BFA_IOC_E_ENABLED	= 1,
 	BFA_IOC_E_DISABLED	= 2,
@@ -163,9 +148,7 @@ struct bfa_ioc_notify {
 	void			*cbarg;
 };
 
-/**
- * Initialize a IOC event notification structure
- */
+/* Initialize a IOC event notification structure */
 #define bfa_ioc_notify_init(__notify, __cbfn, __cbarg) do {	\
 	(__notify)->cbfn = (__cbfn);				\
 	(__notify)->cbarg = (__cbarg);				\
@@ -261,9 +244,7 @@ struct bfa_ioc_hwif {
 #define BFA_IOC_FLASH_OFFSET_IN_CHUNK(off)	(off % BFI_FLASH_CHUNK_SZ_WORDS)
 #define BFA_IOC_FLASH_CHUNK_ADDR(chunkno)  (chunkno * BFI_FLASH_CHUNK_SZ_WORDS)
 
-/**
- * IOC mailbox interface
- */
+/* IOC mailbox interface */
 bool bfa_nw_ioc_mbox_queue(struct bfa_ioc *ioc,
 			struct bfa_mbox_cmd *cmd,
 			bfa_mbox_cmd_cbfn_t cbfn, void *cbarg);
@@ -271,9 +252,7 @@ void bfa_nw_ioc_mbox_isr(struct bfa_ioc *ioc);
 void bfa_nw_ioc_mbox_regisr(struct bfa_ioc *ioc, enum bfi_mclass mc,
 		bfa_ioc_mbox_mcfunc_t cbfn, void *cbarg);
 
-/**
- * IOC interfaces
- */
+/* IOC interfaces */
 
 #define bfa_ioc_pll_init_asic(__ioc) \
 	((__ioc)->ioc_hwif->ioc_pll_init((__ioc)->pcidev.pci_bar_kva, \
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
index b6b036a143a..5df0b0c68c5 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
@@ -87,9 +87,7 @@ static const struct bfa_ioc_hwif nw_hwif_ct2 = {
 	.ioc_sync_complete   = bfa_ioc_ct_sync_complete,
 };
 
-/**
- * Called from bfa_ioc_attach() to map asic specific calls.
- */
+/* Called from bfa_ioc_attach() to map asic specific calls. */
 void
 bfa_nw_ioc_set_ct_hwif(struct bfa_ioc *ioc)
 {
@@ -102,9 +100,7 @@ bfa_nw_ioc_set_ct2_hwif(struct bfa_ioc *ioc)
 	ioc->ioc_hwif = &nw_hwif_ct2;
 }
 
-/**
- * Return true if firmware of current driver matches the running firmware.
- */
+/* Return true if firmware of current driver matches the running firmware. */
 static bool
 bfa_ioc_ct_firmware_lock(struct bfa_ioc *ioc)
 {
@@ -182,9 +178,7 @@ bfa_ioc_ct_firmware_unlock(struct bfa_ioc *ioc)
 	bfa_nw_ioc_sem_release(ioc->ioc_regs.ioc_usage_sem_reg);
 }
 
-/**
- * Notify other functions on HB failure.
- */
+/* Notify other functions on HB failure. */
 static void
 bfa_ioc_ct_notify_fail(struct bfa_ioc *ioc)
 {
@@ -195,9 +189,7 @@ bfa_ioc_ct_notify_fail(struct bfa_ioc *ioc)
 	readl(ioc->ioc_regs.alt_ll_halt);
 }
 
-/**
- * Host to LPU mailbox message addresses
- */
+/* Host to LPU mailbox message addresses */
 static const struct {
 	u32	hfn_mbox;
 	u32	lpu_mbox;
@@ -209,9 +201,7 @@ static const struct {
 	{ HOSTFN3_LPU_MBOX0_8, LPU_HOSTFN3_MBOX0_8, HOST_PAGE_NUM_FN3 }
 };
 
-/**
- * Host <-> LPU mailbox command/status registers - port 0
- */
+/* Host <-> LPU mailbox command/status registers - port 0 */
 static const struct {
 	u32	hfn;
 	u32	lpu;
@@ -222,9 +212,7 @@ static const struct {
 	{ HOSTFN3_LPU0_CMD_STAT, LPU0_HOSTFN3_CMD_STAT }
 };
 
-/**
- * Host <-> LPU mailbox command/status registers - port 1
- */
+/* Host <-> LPU mailbox command/status registers - port 1 */
 static const struct {
 	u32	hfn;
 	u32	lpu;
@@ -368,9 +356,7 @@ bfa_ioc_ct2_reg_init(struct bfa_ioc *ioc)
 	ioc->ioc_regs.err_set = rb + ERR_SET_REG;
 }
 
-/**
- * Initialize IOC to port mapping.
- */
+/* Initialize IOC to port mapping. */
 
 #define FNC_PERS_FN_SHIFT(__fn)	((__fn) * 8)
 static void
@@ -398,9 +384,7 @@ bfa_ioc_ct2_map_port(struct bfa_ioc *ioc)
 	ioc->port_id = ((r32 & __FC_LL_PORT_MAP__MK) >> __FC_LL_PORT_MAP__SH);
 }
 
-/**
- * Set interrupt mode for a function: INTX or MSIX
- */
+/* Set interrupt mode for a function: INTX or MSIX */
 static void
 bfa_ioc_ct_isr_mode_set(struct bfa_ioc *ioc, bool msix)
 {
@@ -443,9 +427,7 @@ bfa_ioc_ct2_lpu_read_stat(struct bfa_ioc *ioc)
 	return false;
 }
 
-/**
- * MSI-X resource allocation for 1860 with no asic block
- */
+/* MSI-X resource allocation for 1860 with no asic block */
 #define HOSTFN_MSIX_DEFAULT		64
 #define HOSTFN_MSIX_VT_INDEX_MBOX_ERR	0x30138
 #define HOSTFN_MSIX_VT_OFST_NUMVT	0x3013c
@@ -473,9 +455,7 @@ bfa_nw_ioc_ct2_poweron(struct bfa_ioc *ioc)
 			rb + HOSTFN_MSIX_VT_INDEX_MBOX_ERR);
 }
 
-/**
- * Cleanup hw semaphore and usecnt registers
- */
+/* Cleanup hw semaphore and usecnt registers */
 static void
 bfa_ioc_ct_ownership_reset(struct bfa_ioc *ioc)
 {
@@ -492,9 +472,7 @@ bfa_ioc_ct_ownership_reset(struct bfa_ioc *ioc)
 	bfa_nw_ioc_hw_sem_release(ioc);
 }
 
-/**
- * Synchronized IOC failure processing routines
- */
+/* Synchronized IOC failure processing routines */
 static bool
 bfa_ioc_ct_sync_start(struct bfa_ioc *ioc)
 {
@@ -518,9 +496,7 @@ bfa_ioc_ct_sync_start(struct bfa_ioc *ioc)
 
 	return bfa_ioc_ct_sync_complete(ioc);
 }
-/**
- * Synchronized IOC failure processing routines
- */
+/* Synchronized IOC failure processing routines */
 static void
 bfa_ioc_ct_sync_join(struct bfa_ioc *ioc)
 {
diff --git a/drivers/net/ethernet/brocade/bna/bfa_msgq.c b/drivers/net/ethernet/brocade/bna/bfa_msgq.c
index dd36427f475..55067d0d25c 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_msgq.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_msgq.c
@@ -16,9 +16,7 @@
  * www.brocade.com
  */
 
-/**
- * @file bfa_msgq.c MSGQ module source file.
- */
+/* MSGQ module source file. */
 
 #include "bfi.h"
 #include "bfa_msgq.h"
diff --git a/drivers/net/ethernet/brocade/bna/bfi.h b/drivers/net/ethernet/brocade/bna/bfi.h
index 0d9df695397..1f24c23dc78 100644
--- a/drivers/net/ethernet/brocade/bna/bfi.h
+++ b/drivers/net/ethernet/brocade/bna/bfi.h
@@ -22,15 +22,11 @@
 
 #pragma pack(1)
 
-/**
- * BFI FW image type
- */
+/* BFI FW image type */
 #define	BFI_FLASH_CHUNK_SZ			256	/*!< Flash chunk size */
 #define	BFI_FLASH_CHUNK_SZ_WORDS	(BFI_FLASH_CHUNK_SZ/sizeof(u32))
 
-/**
- * Msg header common to all msgs
- */
+/* Msg header common to all msgs */
 struct bfi_mhdr {
 	u8		msg_class;	/*!< @ref enum bfi_mclass	    */
 	u8		msg_id;		/*!< msg opcode with in the class   */
@@ -65,17 +61,14 @@ struct bfi_mhdr {
 #define BFI_I2H_OPCODE_BASE	128
 #define BFA_I2HM(_x)			((_x) + BFI_I2H_OPCODE_BASE)
 
-/**
- ****************************************************************************
+/****************************************************************************
  *
  * Scatter Gather Element and Page definition
  *
  ****************************************************************************
  */
 
-/**
- * DMA addresses
- */
+/* DMA addresses */
 union bfi_addr_u {
 	struct {
 		u32	addr_lo;
@@ -83,9 +76,7 @@ union bfi_addr_u {
 	} a32;
 };
 
-/**
- * Generic DMA addr-len pair.
- */
+/* Generic DMA addr-len pair. */
 struct bfi_alen {
 	union bfi_addr_u	al_addr;	/* DMA addr of buffer	*/
 	u32			al_len;		/* length of buffer */
@@ -98,26 +89,20 @@ struct bfi_alen {
 #define BFI_LMSG_PL_WSZ	\
 			((BFI_LMSG_SZ - sizeof(struct bfi_mhdr)) / 4)
 
-/**
- * Mailbox message structure
- */
+/* Mailbox message structure */
 #define BFI_MBMSG_SZ		7
 struct bfi_mbmsg {
 	struct bfi_mhdr mh;
 	u32		pl[BFI_MBMSG_SZ];
 };
 
-/**
- * Supported PCI function class codes (personality)
- */
+/* Supported PCI function class codes (personality) */
 enum bfi_pcifn_class {
 	BFI_PCIFN_CLASS_FC	= 0x0c04,
 	BFI_PCIFN_CLASS_ETH	= 0x0200,
 };
 
-/**
- * Message Classes
- */
+/* Message Classes */
 enum bfi_mclass {
 	BFI_MC_IOC		= 1,	/*!< IO Controller (IOC)	    */
 	BFI_MC_DIAG		= 2,	/*!< Diagnostic Msgs		    */
@@ -159,15 +144,12 @@ enum bfi_mclass {
 
 #define BFI_FWBOOT_ENV_OS		0
 
-/**
- *----------------------------------------------------------------------
+/*----------------------------------------------------------------------
  *				IOC
  *----------------------------------------------------------------------
  */
 
-/**
- * Different asic generations
- */
+/* Different asic generations */
 enum bfi_asic_gen {
 	BFI_ASIC_GEN_CB		= 1,
 	BFI_ASIC_GEN_CT		= 2,
@@ -196,9 +178,7 @@ enum bfi_ioc_i2h_msgs {
 	BFI_IOC_I2H_HBEAT		= BFA_I2HM(4),
 };
 
-/**
- * BFI_IOC_H2I_GETATTR_REQ message
- */
+/* BFI_IOC_H2I_GETATTR_REQ message */
 struct bfi_ioc_getattr_req {
 	struct bfi_mhdr mh;
 	union bfi_addr_u	attr_addr;
@@ -231,30 +211,22 @@ struct bfi_ioc_attr {
 	u32	card_type;	/*!< card type			*/
 };
 
-/**
- * BFI_IOC_I2H_GETATTR_REPLY message
- */
+/* BFI_IOC_I2H_GETATTR_REPLY message */
 struct bfi_ioc_getattr_reply {
 	struct bfi_mhdr mh;	/*!< Common msg header		*/
 	u8			status;	/*!< cfg reply status		*/
 	u8			rsvd[3];
 };
 
-/**
- * Firmware memory page offsets
- */
+/* Firmware memory page offsets */
 #define BFI_IOC_SMEM_PG0_CB	(0x40)
 #define BFI_IOC_SMEM_PG0_CT	(0x180)
 
-/**
- * Firmware statistic offset
- */
+/* Firmware statistic offset */
 #define BFI_IOC_FWSTATS_OFF	(0x6B40)
 #define BFI_IOC_FWSTATS_SZ	(4096)
 
-/**
- * Firmware trace offset
- */
+/* Firmware trace offset */
 #define BFI_IOC_TRC_OFF		(0x4b00)
 #define BFI_IOC_TRC_ENTS	256
 #define BFI_IOC_TRC_ENT_SZ	16
@@ -299,9 +271,7 @@ struct bfi_ioc_hbeat {
 	u32	   hb_count;	/*!< current heart beat count	*/
 };
 
-/**
- * IOC hardware/firmware state
- */
+/* IOC hardware/firmware state */
 enum bfi_ioc_state {
 	BFI_IOC_UNINIT		= 0,	/*!< not initialized		     */
 	BFI_IOC_INITING		= 1,	/*!< h/w is being initialized	     */
@@ -345,9 +315,7 @@ enum {
 	((__adap_type) & (BFI_ADAPTER_TTV | BFI_ADAPTER_PROTO |	\
 			BFI_ADAPTER_UNSUPP))
 
-/**
- * BFI_IOC_H2I_ENABLE_REQ & BFI_IOC_H2I_DISABLE_REQ messages
- */
+/* BFI_IOC_H2I_ENABLE_REQ & BFI_IOC_H2I_DISABLE_REQ messages */
 struct bfi_ioc_ctrl_req {
 	struct bfi_mhdr mh;
 	u16			clscode;
@@ -355,9 +323,7 @@ struct bfi_ioc_ctrl_req {
 	u32		tv_sec;
 };
 
-/**
- * BFI_IOC_I2H_ENABLE_REPLY & BFI_IOC_I2H_DISABLE_REPLY messages
- */
+/* BFI_IOC_I2H_ENABLE_REPLY & BFI_IOC_I2H_DISABLE_REPLY messages */
 struct bfi_ioc_ctrl_reply {
 	struct bfi_mhdr mh;			/*!< Common msg header     */
 	u8			status;		/*!< enable/disable status */
@@ -367,9 +333,7 @@ struct bfi_ioc_ctrl_reply {
 };
 
 #define BFI_IOC_MSGSZ   8
-/**
- * H2I Messages
- */
+/* H2I Messages */
 union bfi_ioc_h2i_msg_u {
 	struct bfi_mhdr mh;
 	struct bfi_ioc_ctrl_req enable_req;
@@ -378,17 +342,14 @@ union bfi_ioc_h2i_msg_u {
 	u32			mboxmsg[BFI_IOC_MSGSZ];
 };
 
-/**
- * I2H Messages
- */
+/* I2H Messages */
 union bfi_ioc_i2h_msg_u {
 	struct bfi_mhdr mh;
 	struct bfi_ioc_ctrl_reply fw_event;
 	u32			mboxmsg[BFI_IOC_MSGSZ];
 };
 
-/**
- *----------------------------------------------------------------------
+/*----------------------------------------------------------------------
  *				MSGQ
  *----------------------------------------------------------------------
  */
diff --git a/drivers/net/ethernet/brocade/bna/bfi_cna.h b/drivers/net/ethernet/brocade/bna/bfi_cna.h
index 4eecabea397..6704a439297 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_cna.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_cna.h
@@ -37,18 +37,14 @@ enum bfi_port_i2h {
 	BFI_PORT_I2H_CLEAR_STATS_RSP	= BFA_I2HM(4),
 };
 
-/**
- * Generic REQ type
- */
+/* Generic REQ type */
 struct bfi_port_generic_req {
 	struct bfi_mhdr mh;		/*!< msg header			    */
 	u32	msgtag;		/*!< msgtag for reply		    */
 	u32	rsvd;
 };
 
-/**
- * Generic RSP type
- */
+/* Generic RSP type */
 struct bfi_port_generic_rsp {
 	struct bfi_mhdr mh;		/*!< common msg header		    */
 	u8		status;		/*!< port enable status		    */
@@ -56,44 +52,12 @@ struct bfi_port_generic_rsp {
 	u32	msgtag;		/*!< msgtag for reply		    */
 };
 
-/**
- * @todo
- * BFI_PORT_H2I_ENABLE_REQ
- */
-
-/**
- * @todo
- * BFI_PORT_I2H_ENABLE_RSP
- */
-
-/**
- * BFI_PORT_H2I_DISABLE_REQ
- */
-
-/**
- * BFI_PORT_I2H_DISABLE_RSP
- */
-
-/**
- * BFI_PORT_H2I_GET_STATS_REQ
- */
+/* BFI_PORT_H2I_GET_STATS_REQ */
 struct bfi_port_get_stats_req {
 	struct bfi_mhdr mh;		/*!< common msg header		    */
 	union bfi_addr_u   dma_addr;
 };
 
-/**
- * BFI_PORT_I2H_GET_STATS_RSP
- */
-
-/**
- * BFI_PORT_H2I_CLEAR_STATS_REQ
- */
-
-/**
- * BFI_PORT_I2H_CLEAR_STATS_RSP
- */
-
 union bfi_port_h2i_msg_u {
 	struct bfi_mhdr mh;
 	struct bfi_port_generic_req enable_req;
diff --git a/drivers/net/ethernet/brocade/bna/bfi_enet.h b/drivers/net/ethernet/brocade/bna/bfi_enet.h
index a90f1cf46b4..eef6e1f8aec 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_enet.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_enet.h
@@ -16,12 +16,9 @@
  * www.brocade.com
  */
 
-/**
- * @file bfi_enet.h BNA Hardware and Firmware Interface
- */
+/* BNA Hardware and Firmware Interface */
 
-/**
- * Skipping statistics collection to avoid clutter.
+/* Skipping statistics collection to avoid clutter.
  * Command is no longer needed:
  *	MTU
  *	TxQ Stop
@@ -64,9 +61,7 @@ union bfi_addr_be_u {
 	} a32;
 };
 
-/**
- *	T X   Q U E U E   D E F I N E S
- */
+/*	T X   Q U E U E   D E F I N E S      */
 /* TxQ Vector (a.k.a. Tx-Buffer Descriptor) */
 /* TxQ Entry Opcodes */
 #define BFI_ENET_TXQ_WI_SEND		(0x402)	/* Single Frame Transmission */
@@ -106,10 +101,7 @@ struct bfi_enet_txq_wi_vector {			/* Tx Buffer Descriptor */
 	union bfi_addr_be_u	addr;
 };
 
-/**
- *  TxQ Entry Structure
- *
- */
+/*  TxQ Entry Structure  */
 struct bfi_enet_txq_entry {
 	union {
 		struct bfi_enet_txq_wi_base	base;
@@ -124,16 +116,12 @@ struct bfi_enet_txq_entry {
 #define BFI_ENET_TXQ_WI_L4_HDR_N_OFFSET(_hdr_size, _offset) \
 		(((_hdr_size) << 10) | ((_offset) & 0x3FF))
 
-/**
- *   R X   Q U E U E   D E F I N E S
- */
+/*   R X   Q U E U E   D E F I N E S   */
 struct bfi_enet_rxq_entry {
 	union bfi_addr_be_u  rx_buffer;
 };
 
-/**
- *   R X   C O M P L E T I O N   Q U E U E   D E F I N E S
- */
+/*   R X   C O M P L E T I O N   Q U E U E   D E F I N E S   */
 /* CQ Entry Flags */
 #define	BFI_ENET_CQ_EF_MAC_ERROR	(1 <<  0)
 #define	BFI_ENET_CQ_EF_FCS_ERROR	(1 <<  1)
@@ -174,9 +162,7 @@ struct bfi_enet_cq_entry {
 	u8	rxq_id;
 };
 
-/**
- *   E N E T   C O N T R O L   P A T H   C O M M A N D S
- */
+/*   E N E T   C O N T R O L   P A T H   C O M M A N D S   */
 struct bfi_enet_q {
 	union bfi_addr_u	pg_tbl;
 	union bfi_addr_u	first_entry;
@@ -222,9 +208,7 @@ struct bfi_enet_ib {
 	u16		rsvd;
 };
 
-/**
- * ENET command messages
- */
+/* ENET command messages */
 enum bfi_enet_h2i_msgs {
 	/* Rx Commands */
 	BFI_ENET_H2I_RX_CFG_SET_REQ = 1,
@@ -350,9 +334,7 @@ enum bfi_enet_i2h_msgs {
 	BFI_ENET_I2H_BW_UPDATE_AEN = BFA_I2HM(BFI_ENET_H2I_MAX + 4),
 };
 
-/**
- *  The following error codes can be returned by the enet commands
- */
+/* The following error codes can be returned by the enet commands */
 enum bfi_enet_err {
 	BFI_ENET_CMD_OK		= 0,
 	BFI_ENET_CMD_FAIL	= 1,
@@ -364,8 +346,7 @@ enum bfi_enet_err {
 	BFI_ENET_CMD_PORT_DISABLED = 7,	/* !< port in disabled state */
 };
 
-/**
- * Generic Request
+/* Generic Request
  *
  * bfi_enet_req is used by:
  *	BFI_ENET_H2I_RX_CFG_CLR_REQ
@@ -375,8 +356,7 @@ struct bfi_enet_req {
 	struct bfi_msgq_mhdr mh;
 };
 
-/**
- * Enable/Disable Request
+/* Enable/Disable Request
  *
  * bfi_enet_enable_req is used by:
  *	BFI_ENET_H2I_RSS_ENABLE_REQ	(enet_id must be zero)
@@ -391,9 +371,7 @@ struct bfi_enet_enable_req {
 	u8		rsvd[3];
 };
 
-/**
- * Generic Response
- */
+/* Generic Response */
 struct bfi_enet_rsp {
 	struct bfi_msgq_mhdr mh;
 	u8		error;		/*!< if error see cmd_offset */
@@ -401,20 +379,16 @@ struct bfi_enet_rsp {
 	u16		cmd_offset;	/*!< offset to invalid parameter */
 };
 
-/**
- * GLOBAL CONFIGURATION
- */
+/* GLOBAL CONFIGURATION */
 
-/**
- * bfi_enet_attr_req is used by:
+/* bfi_enet_attr_req is used by:
  *	BFI_ENET_H2I_GET_ATTR_REQ
  */
 struct bfi_enet_attr_req {
 	struct bfi_msgq_mhdr	mh;
 };
 
-/**
- * bfi_enet_attr_rsp is used by:
+/* bfi_enet_attr_rsp is used by:
  *	BFI_ENET_I2H_GET_ATTR_RSP
  */
 struct bfi_enet_attr_rsp {
@@ -427,8 +401,7 @@ struct bfi_enet_attr_rsp {
 	u32		rit_size;
 };
 
-/**
- * Tx Configuration
+/* Tx Configuration
  *
  * bfi_enet_tx_cfg is used by:
  *	BFI_ENET_H2I_TX_CFG_SET_REQ
@@ -477,8 +450,7 @@ struct bfi_enet_tx_cfg_rsp {
 	} q_handles[BFI_ENET_TXQ_PRIO_MAX];
 };
 
-/**
- * Rx Configuration
+/* Rx Configuration
  *
  * bfi_enet_rx_cfg is used by:
  *	BFI_ENET_H2I_RX_CFG_SET_REQ
@@ -553,8 +525,7 @@ struct bfi_enet_rx_cfg_rsp {
 	} q_handles[BFI_ENET_RX_QSET_MAX];
 };
 
-/**
- * RIT
+/* RIT
  *
  * bfi_enet_rit_req is used by:
  *	BFI_ENET_H2I_RIT_CFG_REQ
@@ -566,8 +537,7 @@ struct bfi_enet_rit_req {
 	u8	table[BFI_ENET_RSS_RIT_MAX];
 };
 
-/**
- * RSS
+/* RSS
  *
  * bfi_enet_rss_cfg_req is used by:
  *	BFI_ENET_H2I_RSS_CFG_REQ
@@ -591,8 +561,7 @@ struct bfi_enet_rss_cfg_req {
 	struct bfi_enet_rss_cfg	cfg;
 };
 
-/**
- * MAC Unicast
+/* MAC Unicast
  *
  * bfi_enet_rx_vlan_req is used by:
  *	BFI_ENET_H2I_MAC_UCAST_SET_REQ
@@ -606,17 +575,14 @@ struct bfi_enet_ucast_req {
 	u8			rsvd[2];
 };
 
-/**
- * MAC Unicast + VLAN
- */
+/* MAC Unicast + VLAN */
 struct bfi_enet_mac_n_vlan_req {
 	struct bfi_msgq_mhdr	mh;
 	u16			vlan_id;
 	mac_t			mac_addr;
 };
 
-/**
- * MAC Multicast
+/* MAC Multicast
  *
  * bfi_enet_mac_mfilter_add_req is used by:
  *	BFI_ENET_H2I_MAC_MCAST_ADD_REQ
@@ -627,8 +593,7 @@ struct bfi_enet_mcast_add_req {
 	u8			rsvd[2];
 };
 
-/**
- * bfi_enet_mac_mfilter_add_rsp is used by:
+/* bfi_enet_mac_mfilter_add_rsp is used by:
  *	BFI_ENET_I2H_MAC_MCAST_ADD_RSP
  */
 struct bfi_enet_mcast_add_rsp {
@@ -640,8 +605,7 @@ struct bfi_enet_mcast_add_rsp {
 	u8			rsvd1[2];
 };
 
-/**
- * bfi_enet_mac_mfilter_del_req is used by:
+/* bfi_enet_mac_mfilter_del_req is used by:
  *	BFI_ENET_H2I_MAC_MCAST_DEL_REQ
  */
 struct bfi_enet_mcast_del_req {
@@ -650,8 +614,7 @@ struct bfi_enet_mcast_del_req {
 	u8			rsvd[2];
 };
 
-/**
- * VLAN
+/* VLAN
  *
  * bfi_enet_rx_vlan_req is used by:
  *	BFI_ENET_H2I_RX_VLAN_SET_REQ
@@ -663,8 +626,7 @@ struct bfi_enet_rx_vlan_req {
 	u32			bit_mask[BFI_ENET_VLAN_WORDS_MAX];
 };
 
-/**
- * PAUSE
+/* PAUSE
  *
  * bfi_enet_set_pause_req is used by:
  *	BFI_ENET_H2I_SET_PAUSE_REQ
@@ -676,8 +638,7 @@ struct bfi_enet_set_pause_req {
 	u8			rx_pause;	/* 1 = enable;  0 = disable */
 };
 
-/**
- * DIAGNOSTICS
+/* DIAGNOSTICS
  *
  * bfi_enet_diag_lb_req is used by:
  *      BFI_ENET_H2I_DIAG_LOOPBACK
@@ -689,16 +650,13 @@ struct bfi_enet_diag_lb_req {
 	u8			enable;		/* 1 = enable;  0 = disable */
 };
 
-/**
- * enum for Loopback opmodes
- */
+/* enum for Loopback opmodes */
 enum {
 	BFI_ENET_DIAG_LB_OPMODE_EXT = 0,
 	BFI_ENET_DIAG_LB_OPMODE_CBL = 1,
 };
 
-/**
- * STATISTICS
+/* STATISTICS
  *
  * bfi_enet_stats_req is used by:
  *    BFI_ENET_H2I_STATS_GET_REQ
@@ -713,9 +671,7 @@ struct bfi_enet_stats_req {
 	union bfi_addr_u	host_buffer;
 };
 
-/**
- * defines for "stats_mask" above.
- */
+/* defines for "stats_mask" above. */
 #define BFI_ENET_STATS_MAC    (1 << 0)    /* !< MAC Statistics */
 #define BFI_ENET_STATS_BPC    (1 << 1)    /* !< Pause Stats from BPC */
 #define BFI_ENET_STATS_RAD    (1 << 2)    /* !< Rx Admission Statistics */
@@ -881,8 +837,7 @@ struct bfi_enet_stats_mac {
 	u64 tx_fragments;
 };
 
-/**
- * Complete statistics, DMAed from fw to host followed by
+/* Complete statistics, DMAed from fw to host followed by
  * BFI_ENET_I2H_STATS_GET_RSP
  */
 struct bfi_enet_stats {
diff --git a/drivers/net/ethernet/brocade/bna/bfi_reg.h b/drivers/net/ethernet/brocade/bna/bfi_reg.h
index 0e094fe46df..c49fa312ddb 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_reg.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_reg.h
@@ -221,9 +221,7 @@ enum {
 #define __PMM_1T_RESET_P		0x00000001
 #define PMM_1T_RESET_REG_P1		0x00023c1c
 
-/**
- * Brocade 1860 Adapter specific defines
- */
+/* Brocade 1860 Adapter specific defines */
 #define CT2_PCI_CPQ_BASE		0x00030000
 #define CT2_PCI_APP_BASE		0x00030100
 #define CT2_PCI_ETH_BASE		0x00030400
diff --git a/drivers/net/ethernet/brocade/bna/bna.h b/drivers/net/ethernet/brocade/bna/bna.h
index 4d7a5de08e1..ede532b4e9d 100644
--- a/drivers/net/ethernet/brocade/bna/bna.h
+++ b/drivers/net/ethernet/brocade/bna/bna.h
@@ -25,11 +25,7 @@
 
 extern const u32 bna_napi_dim_vector[][BNA_BIAS_T_MAX];
 
-/**
- *
- *  Macros and constants
- *
- */
+/*  Macros and constants  */
 
 #define BNA_IOC_TIMER_FREQ		200
 
@@ -356,11 +352,7 @@ do {									\
 	}								\
 } while (0)
 
-/**
- *
- *  Inline functions
- *
- */
+/*  Inline functions  */
 
 static inline struct bna_mac *bna_mac_find(struct list_head *q, u8 *addr)
 {
@@ -377,15 +369,9 @@ static inline struct bna_mac *bna_mac_find(struct list_head *q, u8 *addr)
 
 #define bna_attr(_bna) (&(_bna)->ioceth.attr)
 
-/**
- *
- * Function prototypes
- *
- */
+/* Function prototypes */
 
-/**
- * BNA
- */
+/* BNA */
 
 /* FW response handlers */
 void bna_bfi_stats_clr_rsp(struct bna *bna, struct bfi_msgq_mhdr *msghdr);
@@ -413,24 +399,19 @@ struct bna_mcam_handle *bna_mcam_mod_handle_get(struct bna_mcam_mod *mod);
 void bna_mcam_mod_handle_put(struct bna_mcam_mod *mcam_mod,
 			  struct bna_mcam_handle *handle);
 
-/**
- * MBOX
- */
+/* MBOX */
 
 /* API for BNAD */
 void bna_mbox_handler(struct bna *bna, u32 intr_status);
 
-/**
- * ETHPORT
- */
+/* ETHPORT */
 
 /* Callbacks for RX */
 void bna_ethport_cb_rx_started(struct bna_ethport *ethport);
 void bna_ethport_cb_rx_stopped(struct bna_ethport *ethport);
 
-/**
- * TX MODULE AND TX
- */
+/* TX MODULE AND TX */
+
 /* FW response handelrs */
 void bna_bfi_tx_enet_start_rsp(struct bna_tx *tx,
 			       struct bfi_msgq_mhdr *msghdr);
@@ -462,9 +443,7 @@ void bna_tx_disable(struct bna_tx *tx, enum bna_cleanup_type type,
 void bna_tx_cleanup_complete(struct bna_tx *tx);
 void bna_tx_coalescing_timeo_set(struct bna_tx *tx, int coalescing_timeo);
 
-/**
- * RX MODULE, RX, RXF
- */
+/* RX MODULE, RX, RXF */
 
 /* FW response handlers */
 void bna_bfi_rx_enet_start_rsp(struct bna_rx *rx,
@@ -522,9 +501,7 @@ bna_rx_mode_set(struct bna_rx *rx, enum bna_rxmode rxmode,
 void bna_rx_vlan_add(struct bna_rx *rx, int vlan_id);
 void bna_rx_vlan_del(struct bna_rx *rx, int vlan_id);
 void bna_rx_vlanfilter_enable(struct bna_rx *rx);
-/**
- * ENET
- */
+/* ENET */
 
 /* API for RX */
 int bna_enet_mtu_get(struct bna_enet *enet);
@@ -544,18 +521,14 @@ void bna_enet_mtu_set(struct bna_enet *enet, int mtu,
 		      void (*cbfn)(struct bnad *));
 void bna_enet_perm_mac_get(struct bna_enet *enet, mac_t *mac);
 
-/**
- * IOCETH
- */
+/* IOCETH */
 
 /* APIs for BNAD */
 void bna_ioceth_enable(struct bna_ioceth *ioceth);
 void bna_ioceth_disable(struct bna_ioceth *ioceth,
 			enum bna_cleanup_type type);
 
-/**
- * BNAD
- */
+/* BNAD */
 
 /* Callbacks for ENET */
 void bnad_cb_ethport_link_status(struct bnad *bnad,
diff --git a/drivers/net/ethernet/brocade/bna/bna_enet.c b/drivers/net/ethernet/brocade/bna/bna_enet.c
index 9ccc586e376..db14f69d63b 100644
--- a/drivers/net/ethernet/brocade/bna/bna_enet.c
+++ b/drivers/net/ethernet/brocade/bna/bna_enet.c
@@ -378,9 +378,8 @@ bna_msgq_rsp_handler(void *arg, struct bfi_msgq_mhdr *msghdr)
 	}
 }
 
-/**
- * ETHPORT
- */
+/* ETHPORT */
+
 #define call_ethport_stop_cbfn(_ethport)				\
 do {									\
 	if ((_ethport)->stop_cbfn) {					\
@@ -804,9 +803,8 @@ bna_ethport_cb_rx_stopped(struct bna_ethport *ethport)
 	}
 }
 
-/**
- * ENET
- */
+/* ENET */
+
 #define bna_enet_chld_start(enet)					\
 do {									\
 	enum bna_tx_type tx_type =					\
@@ -1328,9 +1326,8 @@ bna_enet_perm_mac_get(struct bna_enet *enet, mac_t *mac)
 	*mac = bfa_nw_ioc_get_mac(&enet->bna->ioceth.ioc);
 }
 
-/**
- * IOCETH
- */
+/* IOCETH */
+
 #define enable_mbox_intr(_ioceth)					\
 do {									\
 	u32 intr_status;						\
diff --git a/drivers/net/ethernet/brocade/bna/bna_hw_defs.h b/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
index 4c6aab2a953..b8c4e21fbf4 100644
--- a/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
+++ b/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
@@ -16,20 +16,15 @@
  * www.brocade.com
  */
 
-/**
- * File for interrupt macros and functions
- */
+/* File for interrupt macros and functions */
 
 #ifndef __BNA_HW_DEFS_H__
 #define __BNA_HW_DEFS_H__
 
 #include "bfi_reg.h"
 
-/**
- *
- * SW imposed limits
- *
- */
+/* SW imposed limits */
+
 #define BFI_ENET_DEF_TXQ		1
 #define BFI_ENET_DEF_RXP		1
 #define BFI_ENET_DEF_UCAM		1
@@ -141,11 +136,8 @@
 }
 
 #define bna_port_id_get(_bna) ((_bna)->ioceth.ioc.port_id)
-/**
- *
- *  Interrupt related bits, flags and macros
- *
- */
+
+/*  Interrupt related bits, flags and macros  */
 
 #define IB_STATUS_BITS		0x0000ffff
 
@@ -280,11 +272,7 @@ do {									\
 	(writel(BNA_DOORBELL_Q_PRD_IDX((_rcb)->producer_index), \
 		(_rcb)->q_dbell));
 
-/**
- *
- * TxQ, RxQ, CQ related bits, offsets, macros
- *
- */
+/* TxQ, RxQ, CQ related bits, offsets, macros */
 
 /* TxQ Entry Opcodes */
 #define BNA_TXQ_WI_SEND			(0x402)	/* Single Frame Transmission */
@@ -334,11 +322,7 @@ do {									\
 
 #define	BNA_CQ_EF_LOCAL		(1 << 20)
 
-/**
- *
- * Data structures
- *
- */
+/* Data structures */
 
 struct bna_reg_offset {
 	u32 fn_int_status;
@@ -371,8 +355,7 @@ struct bna_txq_wi_vector {
 	struct bna_dma_addr host_addr; /* Tx-Buf DMA addr */
 };
 
-/**
- *  TxQ Entry Structure
+/*  TxQ Entry Structure
  *
  *  BEWARE:  Load values into this structure with correct endianess.
  */
diff --git a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
index 276fcb589f4..71144b396e0 100644
--- a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
+++ b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
@@ -18,9 +18,7 @@
 #include "bna.h"
 #include "bfi.h"
 
-/**
- * IB
- */
+/* IB */
 static void
 bna_ib_coalescing_timeo_set(struct bna_ib *ib, u8 coalescing_timeo)
 {
@@ -29,9 +27,7 @@ bna_ib_coalescing_timeo_set(struct bna_ib *ib, u8 coalescing_timeo)
 				(u32)ib->coalescing_timeo, 0);
 }
 
-/**
- * RXF
- */
+/* RXF */
 
 #define bna_rxf_vlan_cfg_soft_reset(rxf)				\
 do {									\
@@ -1312,9 +1308,7 @@ bna_rxf_vlan_strip_cfg_apply(struct bna_rxf *rxf)
 	return 0;
 }
 
-/**
- * RX
- */
+/* RX */
 
 #define	BNA_GET_RXQS(qcfg)	(((qcfg)->rxp_type == BNA_RXP_SINGLE) ?	\
 	(qcfg)->num_paths : ((qcfg)->num_paths * 2))
@@ -2791,9 +2785,8 @@ const u32 bna_napi_dim_vector[BNA_LOAD_T_MAX][BNA_BIAS_T_MAX] = {
 	{1, 2},
 };
 
-/**
- * TX
- */
+/* TX */
+
 #define call_tx_stop_cbfn(tx)						\
 do {									\
 	if ((tx)->stop_cbfn) {						\
diff --git a/drivers/net/ethernet/brocade/bna/bna_types.h b/drivers/net/ethernet/brocade/bna/bna_types.h
index e8d3ab7ea6c..d3eb8bddfb2 100644
--- a/drivers/net/ethernet/brocade/bna/bna_types.h
+++ b/drivers/net/ethernet/brocade/bna/bna_types.h
@@ -23,11 +23,7 @@
 #include "bfa_cee.h"
 #include "bfa_msgq.h"
 
-/**
- *
- * Forward declarations
- *
- */
+/* Forward declarations */
 
 struct bna_mcam_handle;
 struct bna_txq;
@@ -40,11 +36,7 @@ struct bna_enet;
 struct bna;
 struct bnad;
 
-/**
- *
- * Enums, primitive data types
- *
- */
+/* Enums, primitive data types */
 
 enum bna_status {
 	BNA_STATUS_T_DISABLED	= 0,
@@ -331,11 +323,7 @@ struct bna_attr {
 	int			max_rit_size;
 };
 
-/**
- *
- * IOCEth
- *
- */
+/* IOCEth */
 
 struct bna_ioceth {
 	bfa_fsm_t		fsm;
@@ -351,11 +339,7 @@ struct bna_ioceth {
 	struct bna *bna;
 };
 
-/**
- *
- * Enet
- *
- */
+/* Enet */
 
 /* Pause configuration */
 struct bna_pause_config {
@@ -390,11 +374,7 @@ struct bna_enet {
 	struct bna *bna;
 };
 
-/**
- *
- * Ethport
- *
- */
+/* Ethport */
 
 struct bna_ethport {
 	bfa_fsm_t		fsm;
@@ -419,11 +399,7 @@ struct bna_ethport {
 	struct bna *bna;
 };
 
-/**
- *
- * Interrupt Block
- *
- */
+/* Interrupt Block */
 
 /* Doorbell structure */
 struct bna_ib_dbell {
@@ -447,11 +423,7 @@ struct bna_ib {
 	int			interpkt_timeo;
 };
 
-/**
- *
- * Tx object
- *
- */
+/* Tx object */
 
 /* Tx datapath control structure */
 #define BNA_Q_NAME_SIZE		16
@@ -585,11 +557,7 @@ struct bna_tx_mod {
 	struct bna *bna;
 };
 
-/**
- *
- * Rx object
- *
- */
+/* Rx object */
 
 /* Rx datapath control structure */
 struct bna_rcb {
@@ -898,11 +866,7 @@ struct bna_rx_mod {
 	u32		rid_mask;
 };
 
-/**
- *
- * CAM
- *
- */
+/* CAM */
 
 struct bna_ucam_mod {
 	struct bna_mac *ucmac;		/* BFI_MAX_UCMAC entries */
@@ -927,11 +891,7 @@ struct bna_mcam_mod {
 	struct bna *bna;
 };
 
-/**
- *
- * Statistics
- *
- */
+/* Statistics */
 
 struct bna_stats {
 	struct bna_dma_addr	hw_stats_dma;
@@ -949,11 +909,7 @@ struct bna_stats_mod {
 	struct bfi_enet_stats_req stats_clr;
 };
 
-/**
- *
- * BNA
- *
- */
+/* BNA */
 
 struct bna {
 	struct bna_ident ident;
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 67cd2ed0306..b441f33258e 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -1302,8 +1302,7 @@ bnad_txrx_irq_alloc(struct bnad *bnad, enum bnad_intr_source src,
 	return 0;
 }
 
-/**
- * NOTE: Should be called for MSIX only
+/* NOTE: Should be called for MSIX only
  * Unregisters Tx MSIX vector(s) from the kernel
  */
 static void
@@ -1322,8 +1321,7 @@ bnad_tx_msix_unregister(struct bnad *bnad, struct bnad_tx_info *tx_info,
 	}
 }
 
-/**
- * NOTE: Should be called for MSIX only
+/* NOTE: Should be called for MSIX only
  * Registers Tx MSIX vector(s) and ISR(s), cookie with the kernel
  */
 static int
@@ -1354,8 +1352,7 @@ err_return:
 	return -1;
 }
 
-/**
- * NOTE: Should be called for MSIX only
+/* NOTE: Should be called for MSIX only
  * Unregisters Rx MSIX vector(s) from the kernel
  */
 static void
@@ -1375,8 +1372,7 @@ bnad_rx_msix_unregister(struct bnad *bnad, struct bnad_rx_info *rx_info,
 	}
 }
 
-/**
- * NOTE: Should be called for MSIX only
+/* NOTE: Should be called for MSIX only
  * Registers Tx MSIX vector(s) and ISR(s), cookie with the kernel
  */
 static int
diff --git a/drivers/net/ethernet/brocade/bna/bnad.h b/drivers/net/ethernet/brocade/bna/bnad.h
index 72742be1127..d7833922475 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.h
+++ b/drivers/net/ethernet/brocade/bna/bnad.h
@@ -389,9 +389,7 @@ extern void bnad_netdev_hwstats_fill(struct bnad *bnad,
 void	bnad_debugfs_init(struct bnad *bnad);
 void	bnad_debugfs_uninit(struct bnad *bnad);
 
-/**
- * MACROS
- */
+/* MACROS */
 /* To set & get the stats counters */
 #define BNAD_UPDATE_CTR(_bnad, _ctr)				\
 				(((_bnad)->stats.drv_stats._ctr)++)
diff --git a/drivers/net/ethernet/brocade/bna/cna_fwimg.c b/drivers/net/ethernet/brocade/bna/cna_fwimg.c
index cfc22a64157..6a68e8d9330 100644
--- a/drivers/net/ethernet/brocade/bna/cna_fwimg.c
+++ b/drivers/net/ethernet/brocade/bna/cna_fwimg.c
@@ -67,10 +67,10 @@ bfa_cb_image_get_chunk(enum bfi_asic_gen asic_gen, u32 off)
 {
 	switch (asic_gen) {
 	case BFI_ASIC_GEN_CT:
-		return (u32 *)(bfi_image_ct_cna + off);
+		return (bfi_image_ct_cna + off);
 		break;
 	case BFI_ASIC_GEN_CT2:
-		return (u32 *)(bfi_image_ct2_cna + off);
+		return (bfi_image_ct2_cna + off);
 		break;
 	default:
 		return NULL;
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index 1466bc4e3dd..033064b7b57 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -179,13 +179,16 @@ static void macb_handle_link_change(struct net_device *dev)
 	spin_unlock_irqrestore(&bp->lock, flags);
 
 	if (status_change) {
-		if (phydev->link)
+		if (phydev->link) {
+			netif_carrier_on(dev);
 			netdev_info(dev, "link up (%d/%s)\n",
 				    phydev->speed,
 				    phydev->duplex == DUPLEX_FULL ?
 				    "Full" : "Half");
-		else
+		} else {
+			netif_carrier_off(dev);
 			netdev_info(dev, "link down\n");
+		}
 	}
 }
 
@@ -1033,6 +1036,9 @@ static int macb_open(struct net_device *dev)
 
 	netdev_dbg(bp->dev, "open\n");
 
+	/* carrier starts down */
+	netif_carrier_off(dev);
+
 	/* if the phy is not yet register, retry later*/
 	if (!bp->phy_dev)
 		return -EAGAIN;
@@ -1406,6 +1412,8 @@ static int __init macb_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dev);
 
+	netif_carrier_off(dev);
+
 	netdev_info(dev, "Cadence %s at 0x%08lx irq %d (%pM)\n",
 		    macb_is_gem(bp) ? "GEM" : "MACB", dev->base_addr,
 		    dev->irq, dev->dev_addr);
@@ -1469,6 +1477,7 @@ static int macb_suspend(struct platform_device *pdev, pm_message_t state)
 	struct net_device *netdev = platform_get_drvdata(pdev);
 	struct macb *bp = netdev_priv(netdev);
 
+	netif_carrier_off(netdev);
 	netif_device_detach(netdev);
 
 	clk_disable(bp->hclk);
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index 11f667f6131..2b4b4f529ab 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -264,7 +264,7 @@
 #define XGMAC_OMR_FEF		0x00000080	/* Forward Error Frames */
 #define XGMAC_OMR_DT		0x00000040	/* Drop TCP/IP csum Errors */
 #define XGMAC_OMR_RSF		0x00000020	/* RX FIFO Store and Forward */
-#define XGMAC_OMR_RTC		0x00000010	/* RX Threshhold Ctrl */
+#define XGMAC_OMR_RTC_256	0x00000018	/* RX Threshhold Ctrl */
 #define XGMAC_OMR_RTC_MASK	0x00000018	/* RX Threshhold Ctrl MASK */
 
 /* XGMAC HW Features Register */
@@ -671,26 +671,23 @@ static void xgmac_rx_refill(struct xgmac_priv *priv)
 
 		p = priv->dma_rx + entry;
 
-		if (priv->rx_skbuff[entry] != NULL)
-			continue;
-
-		skb = __skb_dequeue(&priv->rx_recycle);
-		if (skb == NULL)
-			skb = netdev_alloc_skb(priv->dev, priv->dma_buf_sz);
-		if (unlikely(skb == NULL))
-			break;
-
-		priv->rx_skbuff[entry] = skb;
-		paddr = dma_map_single(priv->device, skb->data,
-					 priv->dma_buf_sz, DMA_FROM_DEVICE);
-		desc_set_buf_addr(p, paddr, priv->dma_buf_sz);
+		if (priv->rx_skbuff[entry] == NULL) {
+			skb = __skb_dequeue(&priv->rx_recycle);
+			if (skb == NULL)
+				skb = netdev_alloc_skb(priv->dev, priv->dma_buf_sz);
+			if (unlikely(skb == NULL))
+				break;
+
+			priv->rx_skbuff[entry] = skb;
+			paddr = dma_map_single(priv->device, skb->data,
+					       priv->dma_buf_sz, DMA_FROM_DEVICE);
+			desc_set_buf_addr(p, paddr, priv->dma_buf_sz);
+		}
 
 		netdev_dbg(priv->dev, "rx ring: head %d, tail %d\n",
 			priv->rx_head, priv->rx_tail);
 
 		priv->rx_head = dma_ring_incr(priv->rx_head, DMA_RX_RING_SZ);
-		/* Ensure descriptor is in memory before handing to h/w */
-		wmb();
 		desc_set_rx_owner(p);
 	}
 }
@@ -933,6 +930,7 @@ static void xgmac_tx_err(struct xgmac_priv *priv)
 	desc_init_tx_desc(priv->dma_tx, DMA_TX_RING_SZ);
 	priv->tx_tail = 0;
 	priv->tx_head = 0;
+	writel(priv->dma_tx_phy, priv->base + XGMAC_DMA_TX_BASE_ADDR);
 	writel(reg | DMA_CONTROL_ST, priv->base + XGMAC_DMA_CONTROL);
 
 	writel(DMA_STATUS_TU | DMA_STATUS_TPS | DMA_STATUS_NIS | DMA_STATUS_AIS,
@@ -972,7 +970,7 @@ static int xgmac_hw_init(struct net_device *dev)
 	writel(DMA_INTR_DEFAULT_MASK, ioaddr + XGMAC_DMA_INTR_ENA);
 
 	/* XGMAC requires AXI bus init. This is a 'magic number' for now */
-	writel(0x000100E, ioaddr + XGMAC_DMA_AXI_BUS);
+	writel(0x0077000E, ioaddr + XGMAC_DMA_AXI_BUS);
 
 	ctrl |= XGMAC_CONTROL_DDIC | XGMAC_CONTROL_JE | XGMAC_CONTROL_ACS |
 		XGMAC_CONTROL_CAR;
@@ -984,7 +982,8 @@ static int xgmac_hw_init(struct net_device *dev)
 	writel(value, ioaddr + XGMAC_DMA_CONTROL);
 
 	/* Set the HW DMA mode and the COE */
-	writel(XGMAC_OMR_TSF | XGMAC_OMR_RSF | XGMAC_OMR_RFD | XGMAC_OMR_RFA,
+	writel(XGMAC_OMR_TSF | XGMAC_OMR_RFD | XGMAC_OMR_RFA |
+		XGMAC_OMR_RTC_256,
 		ioaddr + XGMAC_OMR);
 
 	/* Reset the MMC counters */
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index abb6ce7c1b7..6505070abcf 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -3050,7 +3050,7 @@ static struct pci_error_handlers t3_err_handler = {
 static void set_nqsets(struct adapter *adap)
 {
 	int i, j = 0;
-	int num_cpus = num_online_cpus();
+	int num_cpus = netif_get_num_default_rss_queues();
 	int hwports = adap->params.nports;
 	int nqsets = adap->msix_nvectors - 1;
 
@@ -3173,6 +3173,9 @@ static void __devinit cxgb3_init_iscsi_mac(struct net_device *dev)
 	pi->iscsic.mac_addr[3] |= 0x80;
 }
 
+#define TSO_FLAGS (NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_TSO_ECN)
+#define VLAN_FEAT (NETIF_F_SG | NETIF_F_IP_CSUM | TSO_FLAGS | \
+			NETIF_F_IPV6_CSUM | NETIF_F_HIGHDMA)
 static int __devinit init_one(struct pci_dev *pdev,
 			      const struct pci_device_id *ent)
 {
@@ -3293,6 +3296,7 @@ static int __devinit init_one(struct pci_dev *pdev,
 		netdev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM |
 			NETIF_F_TSO | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_RX;
 		netdev->features |= netdev->hw_features | NETIF_F_HW_VLAN_TX;
+		netdev->vlan_features |= netdev->features & VLAN_FEAT;
 		if (pci_using_dac)
 			netdev->features |= NETIF_F_HIGHDMA;
 
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
index 65e4b280619..2dbbcbb450d 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
@@ -62,7 +62,9 @@ static const unsigned int MAX_ATIDS = 64 * 1024;
 static const unsigned int ATID_BASE = 0x10000;
 
 static void cxgb_neigh_update(struct neighbour *neigh);
-static void cxgb_redirect(struct dst_entry *old, struct dst_entry *new);
+static void cxgb_redirect(struct dst_entry *old, struct neighbour *old_neigh,
+			  struct dst_entry *new, struct neighbour *new_neigh,
+			  const void *daddr);
 
 static inline int offload_activated(struct t3cdev *tdev)
 {
@@ -575,7 +577,7 @@ static void t3_process_tid_release_list(struct work_struct *work)
 		if (!skb) {
 			spin_lock_bh(&td->tid_release_lock);
 			p->ctx = (void *)td->tid_release_list;
-			td->tid_release_list = (struct t3c_tid_entry *)p;
+			td->tid_release_list = p;
 			break;
 		}
 		mk_tid_release(skb, p - td->tid_maps.tid_tab);
@@ -968,8 +970,10 @@ static int nb_callback(struct notifier_block *self, unsigned long event,
 	}
 	case (NETEVENT_REDIRECT):{
 		struct netevent_redirect *nr = ctx;
-		cxgb_redirect(nr->old, nr->new);
-		cxgb_neigh_update(dst_get_neighbour_noref(nr->new));
+		cxgb_redirect(nr->old, nr->old_neigh,
+			      nr->new, nr->new_neigh,
+			      nr->daddr);
+		cxgb_neigh_update(nr->new_neigh);
 		break;
 	}
 	default:
@@ -1107,10 +1111,11 @@ static void set_l2t_ix(struct t3cdev *tdev, u32 tid, struct l2t_entry *e)
 	tdev->send(tdev, skb);
 }
 
-static void cxgb_redirect(struct dst_entry *old, struct dst_entry *new)
+static void cxgb_redirect(struct dst_entry *old, struct neighbour *old_neigh,
+			  struct dst_entry *new, struct neighbour *new_neigh,
+			  const void *daddr)
 {
 	struct net_device *olddev, *newdev;
-	struct neighbour *n;
 	struct tid_info *ti;
 	struct t3cdev *tdev;
 	u32 tid;
@@ -1118,15 +1123,8 @@ static void cxgb_redirect(struct dst_entry *old, struct dst_entry *new)
 	struct l2t_entry *e;
 	struct t3c_tid_entry *te;
 
-	n = dst_get_neighbour_noref(old);
-	if (!n)
-		return;
-	olddev = n->dev;
-
-	n = dst_get_neighbour_noref(new);
-	if (!n)
-		return;
-	newdev = n->dev;
+	olddev = old_neigh->dev;
+	newdev = new_neigh->dev;
 
 	if (!is_offloading(olddev))
 		return;
@@ -1144,7 +1142,7 @@ static void cxgb_redirect(struct dst_entry *old, struct dst_entry *new)
 	}
 
 	/* Add new L2T entry */
-	e = t3_l2t_get(tdev, new, newdev);
+	e = t3_l2t_get(tdev, new, newdev, daddr);
 	if (!e) {
 		printk(KERN_ERR "%s: couldn't allocate new l2t entry!\n",
 		       __func__);
diff --git a/drivers/net/ethernet/chelsio/cxgb3/l2t.c b/drivers/net/ethernet/chelsio/cxgb3/l2t.c
index 3fa3c8833ed..8d53438638b 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/l2t.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/l2t.c
@@ -299,7 +299,7 @@ static inline void reuse_entry(struct l2t_entry *e, struct neighbour *neigh)
 }
 
 struct l2t_entry *t3_l2t_get(struct t3cdev *cdev, struct dst_entry *dst,
-			     struct net_device *dev)
+			     struct net_device *dev, const void *daddr)
 {
 	struct l2t_entry *e = NULL;
 	struct neighbour *neigh;
@@ -311,7 +311,7 @@ struct l2t_entry *t3_l2t_get(struct t3cdev *cdev, struct dst_entry *dst,
 	int smt_idx;
 
 	rcu_read_lock();
-	neigh = dst_get_neighbour_noref(dst);
+	neigh = dst_neigh_lookup(dst, daddr);
 	if (!neigh)
 		goto done_rcu;
 
@@ -360,6 +360,8 @@ struct l2t_entry *t3_l2t_get(struct t3cdev *cdev, struct dst_entry *dst,
 done_unlock:
 	write_unlock_bh(&d->lock);
 done_rcu:
+	if (neigh)
+		neigh_release(neigh);
 	rcu_read_unlock();
 	return e;
 }
diff --git a/drivers/net/ethernet/chelsio/cxgb3/l2t.h b/drivers/net/ethernet/chelsio/cxgb3/l2t.h
index c4e86436975..8cffcdfd567 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/l2t.h
+++ b/drivers/net/ethernet/chelsio/cxgb3/l2t.h
@@ -110,7 +110,7 @@ static inline void set_arp_failure_handler(struct sk_buff *skb,
 void t3_l2e_free(struct l2t_data *d, struct l2t_entry *e);
 void t3_l2t_update(struct t3cdev *dev, struct neighbour *neigh);
 struct l2t_entry *t3_l2t_get(struct t3cdev *cdev, struct dst_entry *dst,
-			     struct net_device *dev);
+			     struct net_device *dev, const void *daddr);
 int t3_l2t_send_slow(struct t3cdev *dev, struct sk_buff *skb,
 		     struct l2t_entry *e);
 void t3_l2t_send_event(struct t3cdev *dev, struct l2t_entry *e);
diff --git a/drivers/net/ethernet/chelsio/cxgb3/sge.c b/drivers/net/ethernet/chelsio/cxgb3/sge.c
index cfb60e1f51d..dd901c5061b 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/sge.c
@@ -2877,7 +2877,7 @@ static void sge_timer_tx(unsigned long data)
 	mod_timer(&qs->tx_reclaim_timer, jiffies + next_period);
 }
 
-/*
+/**
  *	sge_timer_rx - perform periodic maintenance of an SGE qset
  *	@data: the SGE queue set to maintain
  *
diff --git a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
index 44ac2f40b64..bff8a3cdd3d 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
@@ -1076,7 +1076,7 @@ static int t3_flash_erase_sectors(struct adapter *adapter, int start, int end)
 	return 0;
 }
 
-/*
+/**
  *	t3_load_fw - download firmware
  *	@adapter: the adapter
  *	@fw_data: the firmware image to write
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index e1f96fbb48c..5ed49af23d6 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -3493,8 +3493,8 @@ static void __devinit cfg_queues(struct adapter *adap)
 	 */
 	if (n10g)
 		q10g = (MAX_ETH_QSETS - (adap->params.nports - n10g)) / n10g;
-	if (q10g > num_online_cpus())
-		q10g = num_online_cpus();
+	if (q10g > netif_get_num_default_rss_queues())
+		q10g = netif_get_num_default_rss_queues();
 
 	for_each_port(adap, i) {
 		struct port_info *pi = adap2pinfo(adap, i);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index e111d974afd..d49933ed551 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -528,7 +528,7 @@ static unsigned int refill_fl(struct adapter *adap, struct sge_fl *q, int n,
 #endif
 
 	while (n--) {
-		pg = alloc_page(gfp);
+		pg = __skb_alloc_page(gfp, NULL);
 		if (unlikely(!pg)) {
 			q->alloc_failed++;
 			break;
@@ -753,7 +753,7 @@ static void write_sgl(const struct sk_buff *skb, struct sge_txq *q,
 		end = (void *)q->desc + part1;
 	}
 	if ((uintptr_t)end & 8)           /* 0-pad to multiple of 16 */
-		*(u64 *)end = 0;
+		*end = 0;
 }
 
 /**
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 32e1dd566a1..fa947dfa4c3 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -2010,7 +2010,7 @@ int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
-/*
+/**
  *     t4_mem_win_read_len - read memory through PCIE memory window
  *     @adap: the adapter
  *     @addr: address of first byte requested aligned on 32b.
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index 25e3308fc9d..9dad56101e2 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -418,7 +418,7 @@ static int fwevtq_handler(struct sge_rspq *rspq, const __be64 *rsp,
 		 * restart a TX Ethernet Queue which was stopped for lack of
 		 * free TX Queue Descriptors ...
 		 */
-		const struct cpl_sge_egr_update *p = (void *)cpl;
+		const struct cpl_sge_egr_update *p = cpl;
 		unsigned int qid = EGR_QID(be32_to_cpu(p->opcode_qid));
 		struct sge *s = &adapter->sge;
 		struct sge_txq *tq;
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index 0bd585bba39..8877fbfefb6 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -653,7 +653,7 @@ static unsigned int refill_fl(struct adapter *adapter, struct sge_fl *fl,
 
 alloc_small_pages:
 	while (n--) {
-		page = alloc_page(gfp | __GFP_NOWARN | __GFP_COLD);
+		page = __skb_alloc_page(gfp | __GFP_NOWARN, NULL);
 		if (unlikely(!page)) {
 			fl->alloc_failed++;
 			break;
@@ -934,7 +934,7 @@ static void write_sgl(const struct sk_buff *skb, struct sge_txq *tq,
 		end = (void *)tq->desc + part1;
 	}
 	if ((uintptr_t)end & 8)           /* 0-pad to multiple of 16 */
-		*(u64 *)end = 0;
+		*end = 0;
 }
 
 /**
@@ -1323,8 +1323,7 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 		 */
 		if (unlikely((void *)sgl == (void *)tq->stat)) {
 			sgl = (void *)tq->desc;
-			end = (void *)((void *)tq->desc +
-				       ((void *)end - (void *)tq->stat));
+			end = ((void *)tq->desc + ((void *)end - (void *)tq->stat));
 		}
 
 		write_sgl(skb, tq, sgl, end, 0, addr);
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 8132c785cea..ad1468b3ab9 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -1300,8 +1300,6 @@ static void enic_rq_indicate_buf(struct vnic_rq *rq,
 			skb->ip_summed = CHECKSUM_COMPLETE;
 		}
 
-		skb->dev = netdev;
-
 		if (vlan_stripped)
 			__vlan_hwaccel_put_tag(skb, vlan_tci);
 
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.c b/drivers/net/ethernet/dec/tulip/de4x5.c
index d3cd489d11a..f879e922484 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.c
+++ b/drivers/net/ethernet/dec/tulip/de4x5.c
@@ -3973,7 +3973,7 @@ DevicePresent(struct net_device *dev, u_long aprom_addr)
 	    tmp = srom_rd(aprom_addr, i);
 	    *p++ = cpu_to_le16(tmp);
 	}
-	de4x5_dbg_srom((struct de4x5_srom *)&lp->srom);
+	de4x5_dbg_srom(&lp->srom);
     }
 }
 
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index c5c4c0e83bd..d266c86a53f 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -34,7 +34,7 @@
 #include "be_hw.h"
 #include "be_roce.h"
 
-#define DRV_VER			"4.2.220u"
+#define DRV_VER			"4.4.31.0u"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"ServerEngines BladeEngine2 10Gbps NIC"
 #define BE3_NAME		"ServerEngines BladeEngine3 10Gbps NIC"
@@ -389,6 +389,7 @@ struct be_adapter {
 	struct delayed_work work;
 	u16 work_counter;
 
+	struct delayed_work func_recovery_work;
 	u32 flags;
 	/* Ethtool knobs and info */
 	char fw_ver[FW_VER_LEN];
@@ -396,9 +397,10 @@ struct be_adapter {
 	u32 *pmac_id;		/* MAC addr handle used by BE card */
 	u32 beacon_state;	/* for set_phys_id */
 
-	bool eeh_err;
-	bool ue_detected;
+	bool eeh_error;
 	bool fw_timeout;
+	bool hw_error;
+
 	u32 port_num;
 	bool promiscuous;
 	u32 function_mode;
@@ -435,6 +437,7 @@ struct be_adapter {
 	u32 max_pmac_cnt;	/* Max secondary UC MACs programmable */
 	u32 uc_macs;		/* Count of secondary UC MAC programmed */
 	u32 msg_enable;
+	int be_get_temp_freq;
 };
 
 #define be_physfn(adapter)		(!adapter->virtfn)
@@ -454,6 +457,9 @@ struct be_adapter {
 #define lancer_chip(adapter)	((adapter->pdev->device == OC_DEVICE_ID3) || \
 				 (adapter->pdev->device == OC_DEVICE_ID4))
 
+#define skyhawk_chip(adapter)	(adapter->pdev->device == OC_DEVICE_ID5)
+
+
 #define be_roce_supported(adapter) ((adapter->if_type == SLI_INTF_TYPE_3 || \
 				adapter->sli_family == SKYHAWK_SLI_FAMILY) && \
 				(adapter->function_mode & RDMA_ENABLED))
@@ -573,6 +579,11 @@ static inline u8 is_udp_pkt(struct sk_buff *skb)
 	return val;
 }
 
+static inline bool is_ipv4_pkt(struct sk_buff *skb)
+{
+	return skb->protocol == htons(ETH_P_IP) && ip_hdr(skb)->version == 4;
+}
+
 static inline void be_vf_eth_addr_generate(struct be_adapter *adapter, u8 *mac)
 {
 	u32 addr;
@@ -593,7 +604,19 @@ static inline bool be_multi_rxq(const struct be_adapter *adapter)
 
 static inline bool be_error(struct be_adapter *adapter)
 {
-	return adapter->eeh_err || adapter->ue_detected || adapter->fw_timeout;
+	return adapter->eeh_error || adapter->hw_error || adapter->fw_timeout;
+}
+
+static inline bool be_crit_error(struct be_adapter *adapter)
+{
+	return adapter->eeh_error || adapter->hw_error;
+}
+
+static inline void  be_clear_all_error(struct be_adapter *adapter)
+{
+	adapter->eeh_error = false;
+	adapter->hw_error = false;
+	adapter->fw_timeout = false;
 }
 
 static inline bool be_is_wol_excluded(struct be_adapter *adapter)
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 921c2082af4..7fac97b4bb5 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -19,9 +19,6 @@
 #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 = 64;
-
 static inline void *embedded_payload(struct be_mcc_wrb *wrb)
 {
 	return wrb->payload.embedded_payload;
@@ -115,7 +112,7 @@ static int be_mcc_compl_process(struct be_adapter *adapter,
 		}
 	} else {
 		if (opcode == OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES)
-			be_get_temp_freq = 0;
+			adapter->be_get_temp_freq = 0;
 
 		if (compl_status == MCC_STATUS_NOT_SUPPORTED ||
 			compl_status == MCC_STATUS_ILLEGAL_REQUEST)
@@ -144,6 +141,11 @@ static void be_async_link_state_process(struct be_adapter *adapter,
 	/* When link status changes, link speed must be re-queried from FW */
 	adapter->phy.link_speed = -1;
 
+	/* Ignore physical link event */
+	if (lancer_chip(adapter) &&
+	    !(evt->port_link_status & LOGICAL_LINK_STATUS_MASK))
+		return;
+
 	/* For the initial link status do not rely on the ASYNC event as
 	 * it may not be received in some cases.
 	 */
@@ -352,7 +354,7 @@ static int be_mbox_db_ready_wait(struct be_adapter *adapter, void __iomem *db)
 		if (msecs > 4000) {
 			dev_err(&adapter->pdev->dev, "FW not responding\n");
 			adapter->fw_timeout = true;
-			be_detect_dump_ue(adapter);
+			be_detect_error(adapter);
 			return -1;
 		}
 
@@ -429,12 +431,65 @@ static int be_POST_stage_get(struct be_adapter *adapter, u16 *stage)
 		return 0;
 }
 
-int be_cmd_POST(struct be_adapter *adapter)
+int lancer_wait_ready(struct be_adapter *adapter)
+{
+#define SLIPORT_READY_TIMEOUT 30
+	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(1000);
+	}
+
+	if (i == SLIPORT_READY_TIMEOUT)
+		status = -1;
+
+	return status;
+}
+
+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;
+}
+
+int be_fw_wait_ready(struct be_adapter *adapter)
 {
 	u16 stage;
 	int status, timeout = 0;
 	struct device *dev = &adapter->pdev->dev;
 
+	if (lancer_chip(adapter)) {
+		status = lancer_wait_ready(adapter);
+		return status;
+	}
+
 	do {
 		status = be_POST_stage_get(adapter, &stage);
 		if (status) {
@@ -565,6 +620,9 @@ int be_cmd_fw_init(struct be_adapter *adapter)
 	u8 *wrb;
 	int status;
 
+	if (lancer_chip(adapter))
+		return 0;
+
 	if (mutex_lock_interruptible(&adapter->mbox_lock))
 		return -1;
 
@@ -592,6 +650,9 @@ int be_cmd_fw_clean(struct be_adapter *adapter)
 	u8 *wrb;
 	int status;
 
+	if (lancer_chip(adapter))
+		return 0;
+
 	if (mutex_lock_interruptible(&adapter->mbox_lock))
 		return -1;
 
@@ -610,6 +671,7 @@ int be_cmd_fw_clean(struct be_adapter *adapter)
 	mutex_unlock(&adapter->mbox_lock);
 	return status;
 }
+
 int be_cmd_eq_create(struct be_adapter *adapter,
 		struct be_queue_info *eq, int eq_delay)
 {
@@ -1132,7 +1194,7 @@ err:
  * Uses MCCQ
  */
 int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags, u32 en_flags,
-		u8 *mac, u32 *if_handle, u32 *pmac_id, u32 domain)
+		     u32 *if_handle, u32 domain)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_if_create *req;
@@ -1152,17 +1214,13 @@ int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags, u32 en_flags,
 	req->hdr.domain = domain;
 	req->capability_flags = cpu_to_le32(cap_flags);
 	req->enable_flags = cpu_to_le32(en_flags);
-	if (mac)
-		memcpy(req->mac_addr, mac, ETH_ALEN);
-	else
-		req->pmac_invalid = true;
+
+	req->pmac_invalid = true;
 
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_if_create *resp = embedded_payload(wrb);
 		*if_handle = le32_to_cpu(resp->interface_id);
-		if (mac)
-			*pmac_id = le32_to_cpu(resp->pmac_id);
 	}
 
 err:
@@ -1210,9 +1268,6 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 	struct be_cmd_req_hdr *hdr;
 	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);
@@ -1581,7 +1636,8 @@ int be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 		/* Reset mcast promisc mode if already set by setting mask
 		 * and not setting flags field
 		 */
-		req->if_flags_mask |=
+		if (!lancer_chip(adapter) || be_physfn(adapter))
+			req->if_flags_mask |=
 				cpu_to_le32(BE_IF_FLAGS_MCAST_PROMISCUOUS);
 
 		req->mcast_num = cpu_to_le32(netdev_mc_count(adapter->netdev));
@@ -1692,6 +1748,20 @@ int be_cmd_reset_function(struct be_adapter *adapter)
 	struct be_cmd_req_hdr *req;
 	int status;
 
+	if (lancer_chip(adapter)) {
+		status = lancer_wait_ready(adapter);
+		if (!status) {
+			iowrite32(SLI_PORT_CONTROL_IP_MASK,
+				  adapter->db + SLIPORT_CONTROL_OFFSET);
+			status = lancer_test_and_set_rdy_state(adapter);
+		}
+		if (status) {
+			dev_err(&adapter->pdev->dev,
+				"Adapter in non recoverable error\n");
+		}
+		return status;
+	}
+
 	if (mutex_lock_interruptible(&adapter->mbox_lock))
 		return -1;
 
@@ -1728,6 +1798,13 @@ int be_cmd_rss_config(struct be_adapter *adapter, u8 *rsstable, u16 table_size)
 	req->if_id = cpu_to_le32(adapter->if_handle);
 	req->enable_rss = cpu_to_le16(RSS_ENABLE_TCP_IPV4 | RSS_ENABLE_IPV4 |
 				      RSS_ENABLE_TCP_IPV6 | RSS_ENABLE_IPV6);
+
+	if (lancer_chip(adapter) || skyhawk_chip(adapter)) {
+		req->hdr.version = 1;
+		req->enable_rss |= cpu_to_le16(RSS_ENABLE_UDP_IPV4 |
+					       RSS_ENABLE_UDP_IPV6);
+	}
+
 	req->cpu_table_size_log2 = cpu_to_le16(fls(table_size) - 1);
 	memcpy(req->cpu_table, rsstable, table_size);
 	memcpy(req->hash, myhash, sizeof(myhash));
@@ -1805,8 +1882,9 @@ err:
 }
 
 int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
-			u32 data_size, u32 data_offset, const char *obj_name,
-			u32 *data_written, u8 *addn_status)
+			    u32 data_size, u32 data_offset,
+			    const char *obj_name, u32 *data_written,
+			    u8 *change_status, u8 *addn_status)
 {
 	struct be_mcc_wrb *wrb;
 	struct lancer_cmd_req_write_object *req;
@@ -1862,10 +1940,12 @@ int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 		status = adapter->flash_status;
 
 	resp = embedded_payload(wrb);
-	if (!status)
+	if (!status) {
 		*data_written = le32_to_cpu(resp->actual_write_len);
-	else
+		*change_status = resp->change_status;
+	} else {
 		*addn_status = resp->additional_status;
+	}
 
 	return status;
 
@@ -2330,8 +2410,8 @@ err:
 }
 
 /* Uses synchronous MCCQ */
-int be_cmd_get_mac_from_list(struct be_adapter *adapter, u32 domain,
-			bool *pmac_id_active, u32 *pmac_id, u8 *mac)
+int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
+			     bool *pmac_id_active, u32 *pmac_id, u8 domain)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_mac_list *req;
@@ -2376,8 +2456,9 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u32 domain,
 						get_mac_list_cmd.va;
 		mac_count = resp->true_mac_count + resp->pseudo_mac_count;
 		/* Mac list returned could contain one or more active mac_ids
-		 * or one or more pseudo permanant mac addresses. If an active
-		 * mac_id is present, return first active mac_id found
+		 * or one or more true or pseudo permanant mac addresses.
+		 * If an active mac_id is present, return first active mac_id
+		 * found.
 		 */
 		for (i = 0; i < mac_count; i++) {
 			struct get_list_macaddr *mac_entry;
@@ -2396,7 +2477,7 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u32 domain,
 				goto out;
 			}
 		}
-		/* If no active mac_id found, return first pseudo mac addr */
+		/* If no active mac_id found, return first mac addr */
 		*pmac_id_active = false;
 		memcpy(mac, resp->macaddr_list[0].mac_addr_id.macaddr,
 								ETH_ALEN);
@@ -2648,6 +2729,44 @@ err:
 	return status;
 }
 
+int be_cmd_query_port_name(struct be_adapter *adapter, u8 *port_name)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_get_port_name *req;
+	int status;
+
+	if (!lancer_chip(adapter)) {
+		*port_name = adapter->hba_port_num + '0';
+		return 0;
+	}
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+
+	req = embedded_payload(wrb);
+
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_GET_PORT_NAME, sizeof(*req), wrb,
+			       NULL);
+	req->hdr.version = 1;
+
+	status = be_mcc_notify_wait(adapter);
+	if (!status) {
+		struct be_cmd_resp_get_port_name *resp = embedded_payload(wrb);
+		*port_name = resp->port_name[adapter->hba_port_num];
+	} else {
+		*port_name = adapter->hba_port_num + '0';
+	}
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
 int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 			int wrb_payload_size, u16 *cmd_status, u16 *ext_status)
 {
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index b3f3fc3d132..250f19b5f7b 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -93,6 +93,7 @@ enum {
 	LINK_UP		= 0x1
 };
 #define LINK_STATUS_MASK			0x1
+#define LOGICAL_LINK_STATUS_MASK		0x2
 
 /* When the event code of an async trailer is link-state, the mcc_compl
  * must be interpreted as follows
@@ -186,6 +187,7 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_ENABLE_DISABLE_BEACON		69
 #define OPCODE_COMMON_GET_BEACON_STATE			70
 #define OPCODE_COMMON_READ_TRANSRECV_DATA		73
+#define OPCODE_COMMON_GET_PORT_NAME			77
 #define OPCODE_COMMON_GET_PHY_DETAILS			102
 #define OPCODE_COMMON_SET_DRIVER_FUNCTION_CAP		103
 #define OPCODE_COMMON_GET_CNTL_ADDITIONAL_ATTRIBUTES	121
@@ -1081,13 +1083,25 @@ struct be_cmd_resp_query_fw_cfg {
 	u32 function_caps;
 };
 
-/******************** RSS Config *******************/
-/* RSS types */
+/******************** RSS Config ****************************************/
+/* RSS type		Input parameters used to compute RX hash
+ * RSS_ENABLE_IPV4	SRC IPv4, DST IPv4
+ * RSS_ENABLE_TCP_IPV4	SRC IPv4, DST IPv4, TCP SRC PORT, TCP DST PORT
+ * RSS_ENABLE_IPV6	SRC IPv6, DST IPv6
+ * RSS_ENABLE_TCP_IPV6	SRC IPv6, DST IPv6, TCP SRC PORT, TCP DST PORT
+ * RSS_ENABLE_UDP_IPV4	SRC IPv4, DST IPv4, UDP SRC PORT, UDP DST PORT
+ * RSS_ENABLE_UDP_IPV6	SRC IPv6, DST IPv6, UDP SRC PORT, UDP DST PORT
+ *
+ * When multiple RSS types are enabled, HW picks the best hash policy
+ * based on the type of the received packet.
+ */
 #define RSS_ENABLE_NONE				0x0
 #define RSS_ENABLE_IPV4				0x1
 #define RSS_ENABLE_TCP_IPV4			0x2
 #define RSS_ENABLE_IPV6				0x4
 #define RSS_ENABLE_TCP_IPV6			0x8
+#define RSS_ENABLE_UDP_IPV4			0x10
+#define RSS_ENABLE_UDP_IPV6			0x20
 
 struct be_cmd_req_rss_config {
 	struct be_cmd_req_hdr hdr;
@@ -1163,6 +1177,8 @@ struct lancer_cmd_req_write_object {
 	u32 addr_high;
 };
 
+#define LANCER_NO_RESET_NEEDED		0x00
+#define LANCER_FW_RESET_NEEDED		0x02
 struct lancer_cmd_resp_write_object {
 	u8 opcode;
 	u8 subsystem;
@@ -1173,6 +1189,8 @@ struct lancer_cmd_resp_write_object {
 	u32 resp_len;
 	u32 actual_resp_len;
 	u32 actual_write_len;
+	u8 change_status;
+	u8 rsvd3[3];
 };
 
 /************************ Lancer Read FW info **************/
@@ -1502,6 +1520,17 @@ struct be_cmd_resp_get_hsw_config {
 	u32 rsvd;
 };
 
+/******************* get port names ***************/
+struct be_cmd_req_get_port_name {
+	struct be_cmd_req_hdr hdr;
+	u32 rsvd0;
+};
+
+struct be_cmd_resp_get_port_name {
+	struct be_cmd_req_hdr hdr;
+	u8 port_name[4];
+};
+
 /*************** HW Stats Get v1 **********************************/
 #define BE_TXP_SW_SZ			48
 struct be_port_rxf_stats_v1 {
@@ -1656,7 +1685,7 @@ struct be_cmd_req_set_ext_fat_caps {
 };
 
 extern int be_pci_fnum_get(struct be_adapter *adapter);
-extern int be_cmd_POST(struct be_adapter *adapter);
+extern int be_fw_wait_ready(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, u32 pmac_id);
 extern int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
@@ -1664,8 +1693,7 @@ extern int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 extern int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id,
 			int pmac_id, u32 domain);
 extern int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags,
-			u32 en_flags, u8 *mac, u32 *if_handle, u32 *pmac_id,
-			u32 domain);
+			    u32 en_flags, u32 *if_handle, u32 domain);
 extern int be_cmd_if_destroy(struct be_adapter *adapter, int if_handle,
 			u32 domain);
 extern int be_cmd_eq_create(struct be_adapter *adapter,
@@ -1719,10 +1747,11 @@ extern int be_cmd_write_flashrom(struct be_adapter *adapter,
 			struct be_dma_mem *cmd, u32 flash_oper,
 			u32 flash_opcode, u32 buf_size);
 extern int lancer_cmd_write_object(struct be_adapter *adapter,
-				struct be_dma_mem *cmd,
-				u32 data_size, u32 data_offset,
-				const char *obj_name,
-				u32 *data_written, u8 *addn_status);
+				   struct be_dma_mem *cmd,
+				   u32 data_size, u32 data_offset,
+				   const char *obj_name,
+				   u32 *data_written, u8 *change_status,
+				   u8 *addn_status);
 int lancer_cmd_read_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 		u32 data_size, u32 data_offset, const char *obj_name,
 		u32 *data_read, u32 *eof, u8 *addn_status);
@@ -1745,14 +1774,15 @@ extern int be_cmd_set_loopback(struct be_adapter *adapter, u8 port_num,
 				u8 loopback_type, u8 enable);
 extern int be_cmd_get_phy_info(struct be_adapter *adapter);
 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 void be_detect_error(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);
 extern int be_cmd_req_native_mode(struct be_adapter *adapter);
 extern int be_cmd_get_reg_len(struct be_adapter *adapter, u32 *log_size);
 extern void be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf);
-extern int be_cmd_get_mac_from_list(struct be_adapter *adapter, u32 domain,
-				bool *pmac_id_active, u32 *pmac_id, u8 *mac);
+extern int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
+				    bool *pmac_id_active, u32 *pmac_id,
+				    u8 domain);
 extern int be_cmd_set_mac_list(struct be_adapter *adapter, u8 *mac_array,
 						u8 mac_count, u32 domain);
 extern int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid,
@@ -1765,4 +1795,7 @@ extern int be_cmd_get_ext_fat_capabilites(struct be_adapter *adapter,
 extern int be_cmd_set_ext_fat_capabilites(struct be_adapter *adapter,
 					  struct be_dma_mem *cmd,
 					  struct be_fat_conf_params *cfgs);
+extern int lancer_wait_ready(struct be_adapter *adapter);
+extern int lancer_test_and_set_rdy_state(struct be_adapter *adapter);
+extern int be_cmd_query_port_name(struct be_adapter *adapter, u8 *port_name);
 
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index 63e51d47690..c0e700653f9 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -648,7 +648,7 @@ be_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
 	struct be_adapter *adapter = netdev_priv(netdev);
 	int status;
 
-	if (ecmd->autoneg != 0)
+	if (ecmd->autoneg != adapter->phy.fc_autoneg)
 		return -EINVAL;
 	adapter->tx_fc = ecmd->tx_pause;
 	adapter->rx_fc = ecmd->rx_pause;
@@ -910,8 +910,9 @@ static void be_set_fw_log_level(struct be_adapter *adapter, u32 level)
 	if (!status) {
 		cfgs = (struct be_fat_conf_params *)(extfat_cmd.va +
 					sizeof(struct be_cmd_resp_hdr));
-		for (i = 0; i < cfgs->num_modules; i++) {
-			for (j = 0; j < cfgs->module[i].num_modes; j++) {
+		for (i = 0; i < le32_to_cpu(cfgs->num_modules); i++) {
+			u32 num_modes = le32_to_cpu(cfgs->module[i].num_modes);
+			for (j = 0; j < num_modes; j++) {
 				if (cfgs->module[i].trace_lvl[j].mode ==
 								MODE_UART)
 					cfgs->module[i].trace_lvl[j].dbg_lvl =
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index d9fb0c501fa..b755f7061dc 100644
--- a/drivers/net/ethernet/emulex/benet/be_hw.h
+++ b/drivers/net/ethernet/emulex/benet/be_hw.h
@@ -45,20 +45,19 @@
 #define POST_STAGE_ARMFW_RDY		0xc000	/* FW is done with POST */
 
 
-/* Lancer SLIPORT_CONTROL SLIPORT_STATUS registers */
+/* Lancer SLIPORT registers */
 #define SLIPORT_STATUS_OFFSET		0x404
 #define SLIPORT_CONTROL_OFFSET		0x408
 #define SLIPORT_ERROR1_OFFSET		0x40C
 #define SLIPORT_ERROR2_OFFSET		0x410
+#define PHYSDEV_CONTROL_OFFSET		0x414
 
 #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
-
-#define PCICFG_CUST_SCRATCHPAD_CSR	0x1EC
+#define PHYSDEV_CONTROL_FW_RESET_MASK	0x00000002
+#define PHYSDEV_CONTROL_INP_MASK	0x40000000
 
 /********* Memory BAR register ************/
 #define PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET 	0xfc
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 501dfa9c88e..c60de89b666 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -155,7 +155,7 @@ static void be_intr_set(struct be_adapter *adapter, bool enable)
 {
 	u32 reg, enabled;
 
-	if (adapter->eeh_err)
+	if (adapter->eeh_error)
 		return;
 
 	pci_read_config_dword(adapter->pdev, PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET,
@@ -201,7 +201,7 @@ static void be_eq_notify(struct be_adapter *adapter, u16 qid,
 	val |= ((qid & DB_EQ_RING_ID_EXT_MASK) <<
 			DB_EQ_RING_ID_EXT_MASK_SHIFT);
 
-	if (adapter->eeh_err)
+	if (adapter->eeh_error)
 		return;
 
 	if (arm)
@@ -220,7 +220,7 @@ void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm, u16 num_popped)
 	val |= ((qid & DB_CQ_RING_ID_EXT_MASK) <<
 			DB_CQ_RING_ID_EXT_MASK_SHIFT);
 
-	if (adapter->eeh_err)
+	if (adapter->eeh_error)
 		return;
 
 	if (arm)
@@ -558,6 +558,7 @@ static inline void wrb_fill(struct be_eth_wrb *wrb, u64 addr, int len)
 	wrb->frag_pa_hi = upper_32_bits(addr);
 	wrb->frag_pa_lo = addr & 0xFFFFFFFF;
 	wrb->frag_len = len & ETH_WRB_FRAG_LEN_MASK;
+	wrb->rsvd0 = 0;
 }
 
 static inline u16 be_get_tx_vlan_tag(struct be_adapter *adapter,
@@ -576,6 +577,11 @@ static inline u16 be_get_tx_vlan_tag(struct be_adapter *adapter,
 	return vlan_tag;
 }
 
+static int be_vlan_tag_chk(struct be_adapter *adapter, struct sk_buff *skb)
+{
+	return vlan_tx_tag_present(skb) || adapter->pvid;
+}
+
 static void wrb_fill_hdr(struct be_adapter *adapter, struct be_eth_hdr_wrb *hdr,
 		struct sk_buff *skb, u32 wrb_cnt, u32 len)
 {
@@ -703,33 +709,56 @@ dma_err:
 	return 0;
 }
 
+static struct sk_buff *be_insert_vlan_in_pkt(struct be_adapter *adapter,
+					     struct sk_buff *skb)
+{
+	u16 vlan_tag = 0;
+
+	skb = skb_share_check(skb, GFP_ATOMIC);
+	if (unlikely(!skb))
+		return skb;
+
+	if (vlan_tx_tag_present(skb)) {
+		vlan_tag = be_get_tx_vlan_tag(adapter, skb);
+		__vlan_put_tag(skb, vlan_tag);
+		skb->vlan_tci = 0;
+	}
+
+	return skb;
+}
+
 static netdev_tx_t be_xmit(struct sk_buff *skb,
 			struct net_device *netdev)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 	struct be_tx_obj *txo = &adapter->tx_obj[skb_get_queue_mapping(skb)];
 	struct be_queue_info *txq = &txo->q;
+	struct iphdr *ip = NULL;
 	u32 wrb_cnt = 0, copied = 0;
-	u32 start = txq->head;
+	u32 start = txq->head, eth_hdr_len;
 	bool dummy_wrb, stopped = false;
 
-	/* For vlan tagged pkts, BE
-	 * 1) calculates checksum even when CSO is not requested
-	 * 2) calculates checksum wrongly for padded pkt less than
-	 * 60 bytes long.
-	 * As a workaround disable TX vlan offloading in such cases.
+	eth_hdr_len = ntohs(skb->protocol) == ETH_P_8021Q ?
+		VLAN_ETH_HLEN : ETH_HLEN;
+
+	/* HW has a bug which considers padding bytes as legal
+	 * and modifies the IPv4 hdr's 'tot_len' field
 	 */
-	if (unlikely(vlan_tx_tag_present(skb) &&
-		     (skb->ip_summed != CHECKSUM_PARTIAL || skb->len <= 60))) {
-		skb = skb_share_check(skb, GFP_ATOMIC);
-		if (unlikely(!skb))
-			goto tx_drop;
+	if (skb->len <= 60 && be_vlan_tag_chk(adapter, skb) &&
+			is_ipv4_pkt(skb)) {
+		ip = (struct iphdr *)ip_hdr(skb);
+		pskb_trim(skb, eth_hdr_len + ntohs(ip->tot_len));
+	}
 
-		skb = __vlan_put_tag(skb, be_get_tx_vlan_tag(adapter, skb));
+	/* HW has a bug wherein it will calculate CSUM for VLAN
+	 * pkts even though it is disabled.
+	 * Manually insert VLAN in pkt.
+	 */
+	if (skb->ip_summed != CHECKSUM_PARTIAL &&
+			be_vlan_tag_chk(adapter, skb)) {
+		skb = be_insert_vlan_in_pkt(adapter, skb);
 		if (unlikely(!skb))
 			goto tx_drop;
-
-		skb->vlan_tci = 0;
 	}
 
 	wrb_cnt = wrb_cnt_for_skb(adapter, skb, &dummy_wrb);
@@ -786,19 +815,12 @@ static int be_change_mtu(struct net_device *netdev, int new_mtu)
  * A max of 64 (BE_NUM_VLANS_SUPPORTED) vlans can be configured in BE.
  * If the user configures more, place BE in vlan promiscuous mode.
  */
-static int be_vid_config(struct be_adapter *adapter, bool vf, u32 vf_num)
+static int be_vid_config(struct be_adapter *adapter)
 {
-	struct be_vf_cfg *vf_cfg = &adapter->vf_cfg[vf_num];
-	u16 vtag[BE_NUM_VLANS_SUPPORTED];
-	u16 ntags = 0, i;
+	u16 vids[BE_NUM_VLANS_SUPPORTED];
+	u16 num = 0, i;
 	int status = 0;
 
-	if (vf) {
-		vtag[0] = cpu_to_le16(vf_cfg->vlan_tag);
-		status = be_cmd_vlan_config(adapter, vf_cfg->if_handle, vtag,
-					    1, 1, 0);
-	}
-
 	/* No need to further configure vids if in promiscuous mode */
 	if (adapter->promiscuous)
 		return 0;
@@ -809,10 +831,10 @@ static int be_vid_config(struct be_adapter *adapter, bool vf, u32 vf_num)
 	/* Construct VLAN Table to give to HW */
 	for (i = 0; i < VLAN_N_VID; i++)
 		if (adapter->vlan_tag[i])
-			vtag[ntags++] = cpu_to_le16(i);
+			vids[num++] = cpu_to_le16(i);
 
 	status = be_cmd_vlan_config(adapter, adapter->if_handle,
-				    vtag, ntags, 1, 0);
+				    vids, num, 1, 0);
 
 	/* Set to VLAN promisc mode as setting VLAN filter failed */
 	if (status) {
@@ -841,7 +863,7 @@ static int be_vlan_add_vid(struct net_device *netdev, u16 vid)
 
 	adapter->vlan_tag[vid] = 1;
 	if (adapter->vlans_added <= (adapter->max_vlans + 1))
-		status = be_vid_config(adapter, false, 0);
+		status = be_vid_config(adapter);
 
 	if (!status)
 		adapter->vlans_added++;
@@ -863,7 +885,7 @@ static int be_vlan_rem_vid(struct net_device *netdev, u16 vid)
 
 	adapter->vlan_tag[vid] = 0;
 	if (adapter->vlans_added <= adapter->max_vlans)
-		status = be_vid_config(adapter, false, 0);
+		status = be_vid_config(adapter);
 
 	if (!status)
 		adapter->vlans_added--;
@@ -890,7 +912,7 @@ static void be_set_rx_mode(struct net_device *netdev)
 		be_cmd_rx_filter(adapter, IFF_PROMISC, OFF);
 
 		if (adapter->vlans_added)
-			be_vid_config(adapter, false, 0);
+			be_vid_config(adapter);
 	}
 
 	/* Enable multicast promisc if num configured exceeds what we support */
@@ -1057,13 +1079,16 @@ static int be_find_vfs(struct be_adapter *adapter, int vf_state)
 	u16 offset, stride;
 
 	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_SRIOV);
+	if (!pos)
+		return 0;
 	pci_read_config_word(pdev, pos + PCI_SRIOV_VF_OFFSET, &offset);
 	pci_read_config_word(pdev, pos + PCI_SRIOV_VF_STRIDE, &stride);
 
 	dev = pci_get_device(pdev->vendor, PCI_ANY_ID, NULL);
 	while (dev) {
 		vf_fn = (pdev->devfn + offset + stride * vfs) & 0xFFFF;
-		if (dev->is_virtfn && dev->devfn == vf_fn) {
+		if (dev->is_virtfn && dev->devfn == vf_fn &&
+			dev->bus->number == pdev->bus->number) {
 			vfs++;
 			if (dev->dev_flags & PCI_DEV_FLAGS_ASSIGNED)
 				assigned_vfs++;
@@ -1203,16 +1228,16 @@ static void skb_fill_rx_data(struct be_rx_obj *rxo, struct sk_buff *skb,
 	/* Copy data in the first descriptor of this completion */
 	curr_frag_len = min(rxcp->pkt_size, rx_frag_size);
 
-	/* Copy the header portion into skb_data */
-	hdr_len = min(BE_HDR_LEN, curr_frag_len);
-	memcpy(skb->data, start, hdr_len);
 	skb->len = curr_frag_len;
 	if (curr_frag_len <= BE_HDR_LEN) { /* tiny packet */
+		memcpy(skb->data, start, curr_frag_len);
 		/* Complete packet has now been moved to data */
 		put_page(page_info->page);
 		skb->data_len = 0;
 		skb->tail += curr_frag_len;
 	} else {
+		hdr_len = ETH_HLEN;
+		memcpy(skb->data, start, hdr_len);
 		skb_shinfo(skb)->nr_frags = 1;
 		skb_frag_set_page(skb, 0, page_info->page);
 		skb_shinfo(skb)->frags[0].page_offset =
@@ -1372,7 +1397,7 @@ static void be_parse_rx_compl_v1(struct be_eth_rx_compl *compl,
 	rxcp->pkt_type =
 		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, cast_enc, compl);
 	rxcp->rss_hash =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, rsshash, rxcp);
+		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, rsshash, compl);
 	if (rxcp->vlanf) {
 		rxcp->vtm = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, vtm,
 					  compl);
@@ -1404,7 +1429,7 @@ static void be_parse_rx_compl_v0(struct be_eth_rx_compl *compl,
 	rxcp->pkt_type =
 		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, cast_enc, compl);
 	rxcp->rss_hash =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, rsshash, rxcp);
+		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, rsshash, compl);
 	if (rxcp->vlanf) {
 		rxcp->vtm = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, vtm,
 					  compl);
@@ -1709,9 +1734,10 @@ static void be_evt_queues_destroy(struct be_adapter *adapter)
 	int i;
 
 	for_all_evt_queues(adapter, eqo, i) {
-		be_eq_clean(eqo);
-		if (eqo->q.created)
+		if (eqo->q.created) {
+			be_eq_clean(eqo);
 			be_cmd_q_destroy(adapter, &eqo->q, QTYPE_EQ);
+		}
 		be_queue_free(adapter, &eqo->q);
 	}
 }
@@ -1898,6 +1924,12 @@ static int be_rx_cqs_create(struct be_adapter *adapter)
 	 */
 	adapter->num_rx_qs = (num_irqs(adapter) > 1) ?
 				num_irqs(adapter) + 1 : 1;
+	if (adapter->num_rx_qs != MAX_RX_QS) {
+		rtnl_lock();
+		netif_set_real_num_rx_queues(adapter->netdev,
+					     adapter->num_rx_qs);
+		rtnl_unlock();
+	}
 
 	adapter->big_page_size = (1 << get_order(rx_frag_size)) * PAGE_SIZE;
 	for_all_rx_queues(adapter, rxo, i) {
@@ -2067,13 +2099,13 @@ int be_poll(struct napi_struct *napi, int budget)
 	return max_work;
 }
 
-void be_detect_dump_ue(struct be_adapter *adapter)
+void be_detect_error(struct be_adapter *adapter)
 {
 	u32 ue_lo = 0, ue_hi = 0, ue_lo_mask = 0, ue_hi_mask = 0;
 	u32 sliport_status = 0, sliport_err1 = 0, sliport_err2 = 0;
 	u32 i;
 
-	if (adapter->eeh_err || adapter->ue_detected)
+	if (be_crit_error(adapter))
 		return;
 
 	if (lancer_chip(adapter)) {
@@ -2094,16 +2126,24 @@ void be_detect_dump_ue(struct be_adapter *adapter)
 		pci_read_config_dword(adapter->pdev,
 				PCICFG_UE_STATUS_HI_MASK, &ue_hi_mask);
 
-		ue_lo = (ue_lo & (~ue_lo_mask));
-		ue_hi = (ue_hi & (~ue_hi_mask));
+		ue_lo = (ue_lo & ~ue_lo_mask);
+		ue_hi = (ue_hi & ~ue_hi_mask);
 	}
 
 	if (ue_lo || ue_hi ||
 		sliport_status & SLIPORT_STATUS_ERR_MASK) {
-		adapter->ue_detected = true;
-		adapter->eeh_err = true;
+		adapter->hw_error = true;
+		dev_err(&adapter->pdev->dev,
+			"Error detected in the card\n");
+	}
+
+	if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
+		dev_err(&adapter->pdev->dev,
+			"ERR: sliport status 0x%x\n", sliport_status);
+		dev_err(&adapter->pdev->dev,
+			"ERR: sliport error1 0x%x\n", sliport_err1);
 		dev_err(&adapter->pdev->dev,
-			"Unrecoverable error in the card\n");
+			"ERR: sliport error2 0x%x\n", sliport_err2);
 	}
 
 	if (ue_lo) {
@@ -2113,6 +2153,7 @@ void be_detect_dump_ue(struct be_adapter *adapter)
 				"UE: %s bit set\n", ue_status_low_desc[i]);
 		}
 	}
+
 	if (ue_hi) {
 		for (i = 0; ue_hi; ue_hi >>= 1, i++) {
 			if (ue_hi & 1)
@@ -2121,14 +2162,6 @@ void be_detect_dump_ue(struct be_adapter *adapter)
 		}
 	}
 
-	if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
-		dev_err(&adapter->pdev->dev,
-			"sliport status 0x%x\n", sliport_status);
-		dev_err(&adapter->pdev->dev,
-			"sliport error1 0x%x\n", sliport_err1);
-		dev_err(&adapter->pdev->dev,
-			"sliport error2 0x%x\n", sliport_err2);
-	}
 }
 
 static void be_msix_disable(struct be_adapter *adapter)
@@ -2141,12 +2174,14 @@ static void be_msix_disable(struct be_adapter *adapter)
 
 static uint be_num_rss_want(struct be_adapter *adapter)
 {
+	u32 num = 0;
 	if ((adapter->function_caps & BE_FUNCTION_CAPS_RSS) &&
 	     !sriov_want(adapter) && be_physfn(adapter) &&
-	     !be_is_mc(adapter))
-		return (adapter->be3_native) ? BE3_MAX_RSS_QS : BE2_MAX_RSS_QS;
-	else
-		return 0;
+	     !be_is_mc(adapter)) {
+		num = (adapter->be3_native) ? BE3_MAX_RSS_QS : BE2_MAX_RSS_QS;
+		num = min_t(u32, num, (u32)netif_get_num_default_rss_queues());
+	}
+	return num;
 }
 
 static void be_msix_enable(struct be_adapter *adapter)
@@ -2540,11 +2575,7 @@ static int be_clear(struct be_adapter *adapter)
 	be_tx_queues_destroy(adapter);
 	be_evt_queues_destroy(adapter);
 
-	/* tell fw we're done with firing cmds */
-	be_cmd_fw_clean(adapter);
-
 	be_msix_disable(adapter);
-	pci_write_config_dword(adapter->pdev, PCICFG_CUST_SCRATCHPAD_CSR, 0);
 	return 0;
 }
 
@@ -2602,8 +2633,8 @@ static int be_vf_setup(struct be_adapter *adapter)
 	cap_flags = en_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST |
 				BE_IF_FLAGS_MULTICAST;
 	for_all_vfs(adapter, vf_cfg, vf) {
-		status = be_cmd_if_create(adapter, cap_flags, en_flags, NULL,
-					  &vf_cfg->if_handle, NULL, vf + 1);
+		status = be_cmd_if_create(adapter, cap_flags, en_flags,
+					  &vf_cfg->if_handle, vf + 1);
 		if (status)
 			goto err;
 	}
@@ -2643,29 +2674,43 @@ static void be_setup_init(struct be_adapter *adapter)
 	adapter->phy.forced_port_speed = -1;
 }
 
-static int be_add_mac_from_list(struct be_adapter *adapter, u8 *mac)
+static int be_get_mac_addr(struct be_adapter *adapter, u8 *mac, u32 if_handle,
+			   bool *active_mac, u32 *pmac_id)
 {
-	u32 pmac_id;
-	int status;
-	bool pmac_id_active;
+	int status = 0;
 
-	status = be_cmd_get_mac_from_list(adapter, 0, &pmac_id_active,
-							&pmac_id, mac);
-	if (status != 0)
-		goto do_none;
+	if (!is_zero_ether_addr(adapter->netdev->perm_addr)) {
+		memcpy(mac, adapter->netdev->dev_addr, ETH_ALEN);
+		if (!lancer_chip(adapter) && !be_physfn(adapter))
+			*active_mac = true;
+		else
+			*active_mac = false;
 
-	if (pmac_id_active) {
-		status = be_cmd_mac_addr_query(adapter, mac,
-				MAC_ADDRESS_TYPE_NETWORK,
-				false, adapter->if_handle, pmac_id);
+		return status;
+	}
 
-		if (!status)
-			adapter->pmac_id[0] = pmac_id;
+	if (lancer_chip(adapter)) {
+		status = be_cmd_get_mac_from_list(adapter, mac,
+						  active_mac, pmac_id, 0);
+		if (*active_mac) {
+			status = be_cmd_mac_addr_query(adapter, mac,
+						       MAC_ADDRESS_TYPE_NETWORK,
+						       false, if_handle,
+						       *pmac_id);
+		}
+	} else if (be_physfn(adapter)) {
+		/* For BE3, for PF get permanent MAC */
+		status = be_cmd_mac_addr_query(adapter, mac,
+					       MAC_ADDRESS_TYPE_NETWORK, true,
+					       0, 0);
+		*active_mac = false;
 	} else {
-		status = be_cmd_pmac_add(adapter, mac,
-				adapter->if_handle, &adapter->pmac_id[0], 0);
+		/* For BE3, for VF get soft MAC assigned by PF*/
+		status = be_cmd_mac_addr_query(adapter, mac,
+					       MAC_ADDRESS_TYPE_NETWORK, false,
+					       if_handle, 0);
+		*active_mac = true;
 	}
-do_none:
 	return status;
 }
 
@@ -2686,12 +2731,12 @@ static int be_get_config(struct be_adapter *adapter)
 
 static int be_setup(struct be_adapter *adapter)
 {
-	struct net_device *netdev = adapter->netdev;
 	struct device *dev = &adapter->pdev->dev;
 	u32 cap_flags, en_flags;
 	u32 tx_fc, rx_fc;
 	int status;
 	u8 mac[ETH_ALEN];
+	bool active_mac;
 
 	be_setup_init(adapter);
 
@@ -2717,14 +2762,6 @@ static int be_setup(struct be_adapter *adapter)
 	if (status)
 		goto err;
 
-	memset(mac, 0, ETH_ALEN);
-	status = be_cmd_mac_addr_query(adapter, mac, MAC_ADDRESS_TYPE_NETWORK,
-			true /*permanent */, 0, 0);
-	if (status)
-		return status;
-	memcpy(adapter->netdev->dev_addr, mac, ETH_ALEN);
-	memcpy(adapter->netdev->perm_addr, mac, ETH_ALEN);
-
 	en_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST |
 			BE_IF_FLAGS_MULTICAST | BE_IF_FLAGS_PASS_L3L4_ERRORS;
 	cap_flags = en_flags | BE_IF_FLAGS_MCAST_PROMISCUOUS |
@@ -2734,27 +2771,36 @@ static int be_setup(struct be_adapter *adapter)
 		cap_flags |= BE_IF_FLAGS_RSS;
 		en_flags |= BE_IF_FLAGS_RSS;
 	}
+
+	if (lancer_chip(adapter) && !be_physfn(adapter)) {
+		en_flags = BE_IF_FLAGS_UNTAGGED |
+			    BE_IF_FLAGS_BROADCAST |
+			    BE_IF_FLAGS_MULTICAST;
+		cap_flags = en_flags;
+	}
+
 	status = be_cmd_if_create(adapter, cap_flags, en_flags,
-			netdev->dev_addr, &adapter->if_handle,
-			&adapter->pmac_id[0], 0);
+				  &adapter->if_handle, 0);
 	if (status != 0)
 		goto err;
 
-	 /* The VF's permanent mac queried from card is incorrect.
-	  * For BEx: Query the mac configued by the PF using if_handle
-	  * For Lancer: Get and use mac_list to obtain mac address.
-	  */
-	if (!be_physfn(adapter)) {
-		if (lancer_chip(adapter))
-			status = be_add_mac_from_list(adapter, mac);
-		else
-			status = be_cmd_mac_addr_query(adapter, mac,
-					MAC_ADDRESS_TYPE_NETWORK, false,
-					adapter->if_handle, 0);
-		if (!status) {
-			memcpy(adapter->netdev->dev_addr, mac, ETH_ALEN);
-			memcpy(adapter->netdev->perm_addr, mac, ETH_ALEN);
-		}
+	memset(mac, 0, ETH_ALEN);
+	active_mac = false;
+	status = be_get_mac_addr(adapter, mac, adapter->if_handle,
+				 &active_mac, &adapter->pmac_id[0]);
+	if (status != 0)
+		goto err;
+
+	if (!active_mac) {
+		status = be_cmd_pmac_add(adapter, mac, adapter->if_handle,
+					 &adapter->pmac_id[0], 0);
+		if (status != 0)
+			goto err;
+	}
+
+	if (is_zero_ether_addr(adapter->netdev->dev_addr)) {
+		memcpy(adapter->netdev->dev_addr, mac, ETH_ALEN);
+		memcpy(adapter->netdev->perm_addr, mac, ETH_ALEN);
 	}
 
 	status = be_tx_qs_create(adapter);
@@ -2763,7 +2809,8 @@ static int be_setup(struct be_adapter *adapter)
 
 	be_cmd_get_fw_ver(adapter, adapter->fw_ver, NULL);
 
-	be_vid_config(adapter, false, 0);
+	if (adapter->vlans_added)
+		be_vid_config(adapter);
 
 	be_set_rx_mode(adapter->netdev);
 
@@ -2773,8 +2820,6 @@ static int be_setup(struct be_adapter *adapter)
 		be_cmd_set_flow_control(adapter, adapter->tx_fc,
 					adapter->rx_fc);
 
-	pcie_set_readrq(adapter->pdev, 4096);
-
 	if (be_physfn(adapter) && num_vfs) {
 		if (adapter->dev_num_vfs)
 			be_vf_setup(adapter);
@@ -2788,8 +2833,6 @@ static int be_setup(struct be_adapter *adapter)
 
 	schedule_delayed_work(&adapter->work, msecs_to_jiffies(1000));
 	adapter->flags |= BE_FLAGS_WORKER_SCHEDULED;
-
-	pci_write_config_dword(adapter->pdev, PCICFG_CUST_SCRATCHPAD_CSR, 1);
 	return 0;
 err:
 	be_clear(adapter);
@@ -3033,6 +3076,40 @@ static int get_ufigen_type(struct flash_file_hdr_g2 *fhdr)
 		return 0;
 }
 
+static int lancer_wait_idle(struct be_adapter *adapter)
+{
+#define SLIPORT_IDLE_TIMEOUT 30
+	u32 reg_val;
+	int status = 0, i;
+
+	for (i = 0; i < SLIPORT_IDLE_TIMEOUT; i++) {
+		reg_val = ioread32(adapter->db + PHYSDEV_CONTROL_OFFSET);
+		if ((reg_val & PHYSDEV_CONTROL_INP_MASK) == 0)
+			break;
+
+		ssleep(1);
+	}
+
+	if (i == SLIPORT_IDLE_TIMEOUT)
+		status = -1;
+
+	return status;
+}
+
+static int lancer_fw_reset(struct be_adapter *adapter)
+{
+	int status = 0;
+
+	status = lancer_wait_idle(adapter);
+	if (status)
+		return status;
+
+	iowrite32(PHYSDEV_CONTROL_FW_RESET_MASK, adapter->db +
+		  PHYSDEV_CONTROL_OFFSET);
+
+	return status;
+}
+
 static int lancer_fw_download(struct be_adapter *adapter,
 				const struct firmware *fw)
 {
@@ -3047,6 +3124,7 @@ static int lancer_fw_download(struct be_adapter *adapter,
 	u32 offset = 0;
 	int status = 0;
 	u8 add_status = 0;
+	u8 change_status;
 
 	if (!IS_ALIGNED(fw->size, sizeof(u32))) {
 		dev_err(&adapter->pdev->dev,
@@ -3079,9 +3157,10 @@ static int lancer_fw_download(struct be_adapter *adapter,
 		memcpy(dest_image_ptr, data_ptr, chunk_size);
 
 		status = lancer_cmd_write_object(adapter, &flash_cmd,
-				chunk_size, offset, LANCER_FW_DOWNLOAD_LOCATION,
-				&data_written, &add_status);
-
+						 chunk_size, offset,
+						 LANCER_FW_DOWNLOAD_LOCATION,
+						 &data_written, &change_status,
+						 &add_status);
 		if (status)
 			break;
 
@@ -3093,8 +3172,10 @@ static int lancer_fw_download(struct be_adapter *adapter,
 	if (!status) {
 		/* Commit the FW written */
 		status = lancer_cmd_write_object(adapter, &flash_cmd,
-					0, offset, LANCER_FW_DOWNLOAD_LOCATION,
-					&data_written, &add_status);
+						 0, offset,
+						 LANCER_FW_DOWNLOAD_LOCATION,
+						 &data_written, &change_status,
+						 &add_status);
 	}
 
 	dma_free_coherent(&adapter->pdev->dev, flash_cmd.size, flash_cmd.va,
@@ -3107,6 +3188,20 @@ static int lancer_fw_download(struct be_adapter *adapter,
 		goto lancer_fw_exit;
 	}
 
+	if (change_status == LANCER_FW_RESET_NEEDED) {
+		status = lancer_fw_reset(adapter);
+		if (status) {
+			dev_err(&adapter->pdev->dev,
+				"Adapter busy for FW reset.\n"
+				"New FW will not be active.\n");
+			goto lancer_fw_exit;
+		}
+	} else if (change_status != LANCER_NO_RESET_NEEDED) {
+			dev_err(&adapter->pdev->dev,
+				"System reboot required for new FW"
+				" to be active\n");
+	}
+
 	dev_info(&adapter->pdev->dev, "Firmware flashed successfully\n");
 lancer_fw_exit:
 	return status;
@@ -3435,10 +3530,15 @@ static void __devexit be_remove(struct pci_dev *pdev)
 
 	be_roce_dev_remove(adapter);
 
+	cancel_delayed_work_sync(&adapter->func_recovery_work);
+
 	unregister_netdev(adapter->netdev);
 
 	be_clear(adapter);
 
+	/* tell fw we're done with firing cmds */
+	be_cmd_fw_clean(adapter);
+
 	be_stats_cleanup(adapter);
 
 	be_ctrl_cleanup(adapter);
@@ -3479,7 +3579,7 @@ u32 be_get_fw_log_level(struct be_adapter *adapter)
 	if (!status) {
 		cfgs = (struct be_fat_conf_params *)(extfat_cmd.va +
 						sizeof(struct be_cmd_resp_hdr));
-		for (j = 0; j < cfgs->module[0].num_modes; j++) {
+		for (j = 0; j < le32_to_cpu(cfgs->module[0].num_modes); j++) {
 			if (cfgs->module[0].trace_lvl[j].mode == MODE_UART)
 				level = cfgs->module[0].trace_lvl[j].dbg_lvl;
 		}
@@ -3530,6 +3630,9 @@ static int be_get_initial_config(struct be_adapter *adapter)
 	if (be_is_wol_supported(adapter))
 		adapter->wol = true;
 
+	/* Must be a power of 2 or else MODULO will BUG_ON */
+	adapter->be_get_temp_freq = 64;
+
 	level = be_get_fw_log_level(adapter);
 	adapter->msg_enable = level <= FW_LOG_LEVEL_DEFAULT ? NETIF_MSG_HW : 0;
 
@@ -3585,101 +3688,68 @@ static int be_dev_type_check(struct be_adapter *adapter)
 	return 0;
 }
 
-static int lancer_wait_ready(struct be_adapter *adapter)
+static int lancer_recover_func(struct be_adapter *adapter)
 {
-#define SLIPORT_READY_TIMEOUT 30
-	u32 sliport_status;
-	int status = 0, i;
+	int status;
 
-	for (i = 0; i < SLIPORT_READY_TIMEOUT; i++) {
-		sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
-		if (sliport_status & SLIPORT_STATUS_RDY_MASK)
-			break;
+	status = lancer_test_and_set_rdy_state(adapter);
+	if (status)
+		goto err;
 
-		msleep(1000);
-	}
+	if (netif_running(adapter->netdev))
+		be_close(adapter->netdev);
 
-	if (i == SLIPORT_READY_TIMEOUT)
-		status = -1;
+	be_clear(adapter);
 
-	return status;
-}
+	adapter->hw_error = false;
+	adapter->fw_timeout = false;
 
-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;
-		}
+	status = be_setup(adapter);
+	if (status)
+		goto err;
+
+	if (netif_running(adapter->netdev)) {
+		status = be_open(adapter->netdev);
+		if (status)
+			goto err;
 	}
+
+	dev_err(&adapter->pdev->dev,
+		"Adapter SLIPORT recovery succeeded\n");
+	return 0;
+err:
+	dev_err(&adapter->pdev->dev,
+		"Adapter SLIPORT recovery failed\n");
+
 	return status;
 }
 
-static void lancer_test_and_recover_fn_err(struct be_adapter *adapter)
+static void be_func_recovery_task(struct work_struct *work)
 {
+	struct be_adapter *adapter =
+		container_of(work, struct be_adapter,  func_recovery_work.work);
 	int status;
-	u32 sliport_status;
-
-	if (adapter->eeh_err || adapter->ue_detected)
-		return;
 
-	sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
+	be_detect_error(adapter);
 
-	if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
-		dev_err(&adapter->pdev->dev,
-				"Adapter in error state."
-				"Trying to recover.\n");
+	if (adapter->hw_error && lancer_chip(adapter)) {
 
-		status = lancer_test_and_set_rdy_state(adapter);
-		if (status)
-			goto err;
+		if (adapter->eeh_error)
+			goto out;
 
+		rtnl_lock();
 		netif_device_detach(adapter->netdev);
+		rtnl_unlock();
 
-		if (netif_running(adapter->netdev))
-			be_close(adapter->netdev);
-
-		be_clear(adapter);
-
-		adapter->fw_timeout = false;
-
-		status = be_setup(adapter);
-		if (status)
-			goto err;
-
-		if (netif_running(adapter->netdev)) {
-			status = be_open(adapter->netdev);
-			if (status)
-				goto err;
-		}
-
-		netif_device_attach(adapter->netdev);
+		status = lancer_recover_func(adapter);
 
-		dev_err(&adapter->pdev->dev,
-				"Adapter error recovery succeeded\n");
+		if (!status)
+			netif_device_attach(adapter->netdev);
 	}
-	return;
-err:
-	dev_err(&adapter->pdev->dev,
-			"Adapter error recovery failed\n");
+
+out:
+	schedule_delayed_work(&adapter->func_recovery_work,
+			      msecs_to_jiffies(1000));
 }
 
 static void be_worker(struct work_struct *work)
@@ -3690,11 +3760,6 @@ static void be_worker(struct work_struct *work)
 	struct be_eq_obj *eqo;
 	int i;
 
-	if (lancer_chip(adapter))
-		lancer_test_and_recover_fn_err(adapter);
-
-	be_detect_dump_ue(adapter);
-
 	/* when interrupts are not yet enabled, just reap any pending
 	* mcc completions */
 	if (!netif_running(adapter->netdev)) {
@@ -3710,6 +3775,9 @@ static void be_worker(struct work_struct *work)
 			be_cmd_get_stats(adapter, &adapter->stats_cmd);
 	}
 
+	if (MODULO(adapter->work_counter, adapter->be_get_temp_freq) == 0)
+		be_cmd_get_die_temperature(adapter);
+
 	for_all_rx_queues(adapter, rxo, i) {
 		if (rxo->rx_post_starved) {
 			rxo->rx_post_starved = false;
@@ -3727,10 +3795,7 @@ reschedule:
 
 static bool be_reset_required(struct be_adapter *adapter)
 {
-	u32 reg;
-
-	pci_read_config_dword(adapter->pdev, PCICFG_CUST_SCRATCHPAD_CSR, &reg);
-	return reg;
+	return be_find_vfs(adapter, ENABLED) > 0 ? false : true;
 }
 
 static int __devinit be_probe(struct pci_dev *pdev,
@@ -3739,6 +3804,7 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	int status = 0;
 	struct be_adapter *adapter;
 	struct net_device *netdev;
+	char port_name;
 
 	status = pci_enable_device(pdev);
 	if (status)
@@ -3749,7 +3815,7 @@ static int __devinit be_probe(struct pci_dev *pdev,
 		goto disable_dev;
 	pci_set_master(pdev);
 
-	netdev = alloc_etherdev_mq(sizeof(struct be_adapter), MAX_TX_QS);
+	netdev = alloc_etherdev_mqs(sizeof(*adapter), MAX_TX_QS, MAX_RX_QS);
 	if (netdev == NULL) {
 		status = -ENOMEM;
 		goto rel_reg;
@@ -3780,22 +3846,9 @@ static int __devinit be_probe(struct pci_dev *pdev,
 	if (status)
 		goto free_netdev;
 
-	if (lancer_chip(adapter)) {
-		status = lancer_wait_ready(adapter);
-		if (!status) {
-			iowrite32(SLI_PORT_CONTROL_IP_MASK,
-					adapter->db + SLIPORT_CONTROL_OFFSET);
-			status = lancer_test_and_set_rdy_state(adapter);
-		}
-		if (status) {
-			dev_err(&pdev->dev, "Adapter in non recoverable error\n");
-			goto ctrl_clean;
-		}
-	}
-
 	/* sync up with fw's ready state */
 	if (be_physfn(adapter)) {
-		status = be_cmd_POST(adapter);
+		status = be_fw_wait_ready(adapter);
 		if (status)
 			goto ctrl_clean;
 	}
@@ -3826,6 +3879,7 @@ static int __devinit be_probe(struct pci_dev *pdev,
 		goto stats_clean;
 
 	INIT_DELAYED_WORK(&adapter->work, be_worker);
+	INIT_DELAYED_WORK(&adapter->func_recovery_work, be_func_recovery_task);
 	adapter->rx_fc = adapter->tx_fc = true;
 
 	status = be_setup(adapter);
@@ -3839,8 +3893,13 @@ static int __devinit be_probe(struct pci_dev *pdev,
 
 	be_roce_dev_add(adapter);
 
-	dev_info(&pdev->dev, "%s: %s port %d\n", netdev->name, nic_name(pdev),
-		adapter->port_num);
+	schedule_delayed_work(&adapter->func_recovery_work,
+			      msecs_to_jiffies(1000));
+
+	be_cmd_query_port_name(adapter, &port_name);
+
+	dev_info(&pdev->dev, "%s: %s port %c\n", netdev->name, nic_name(pdev),
+		 port_name);
 
 	return 0;
 
@@ -3872,6 +3931,8 @@ static int be_suspend(struct pci_dev *pdev, pm_message_t state)
 	if (adapter->wol)
 		be_setup_wol(adapter, true);
 
+	cancel_delayed_work_sync(&adapter->func_recovery_work);
+
 	netif_device_detach(netdev);
 	if (netif_running(netdev)) {
 		rtnl_lock();
@@ -3912,6 +3973,9 @@ static int be_resume(struct pci_dev *pdev)
 		be_open(netdev);
 		rtnl_unlock();
 	}
+
+	schedule_delayed_work(&adapter->func_recovery_work,
+			      msecs_to_jiffies(1000));
 	netif_device_attach(netdev);
 
 	if (adapter->wol)
@@ -3931,6 +3995,7 @@ static void be_shutdown(struct pci_dev *pdev)
 		return;
 
 	cancel_delayed_work_sync(&adapter->work);
+	cancel_delayed_work_sync(&adapter->func_recovery_work);
 
 	netif_device_detach(adapter->netdev);
 
@@ -3950,9 +4015,13 @@ static pci_ers_result_t be_eeh_err_detected(struct pci_dev *pdev,
 
 	dev_err(&adapter->pdev->dev, "EEH error detected\n");
 
-	adapter->eeh_err = true;
+	adapter->eeh_error = true;
 
+	cancel_delayed_work_sync(&adapter->func_recovery_work);
+
+	rtnl_lock();
 	netif_device_detach(netdev);
+	rtnl_unlock();
 
 	if (netif_running(netdev)) {
 		rtnl_lock();
@@ -3980,9 +4049,7 @@ static pci_ers_result_t be_eeh_reset(struct pci_dev *pdev)
 	int status;
 
 	dev_info(&adapter->pdev->dev, "EEH reset\n");
-	adapter->eeh_err = false;
-	adapter->ue_detected = false;
-	adapter->fw_timeout = false;
+	be_clear_all_error(adapter);
 
 	status = pci_enable_device(pdev);
 	if (status)
@@ -3993,7 +4060,7 @@ static pci_ers_result_t be_eeh_reset(struct pci_dev *pdev)
 	pci_restore_state(pdev);
 
 	/* Check if card is ok and fw is ready */
-	status = be_cmd_POST(adapter);
+	status = be_fw_wait_ready(adapter);
 	if (status)
 		return PCI_ERS_RESULT_DISCONNECT;
 
@@ -4015,6 +4082,10 @@ static void be_eeh_resume(struct pci_dev *pdev)
 	if (status)
 		goto err;
 
+	status = be_cmd_reset_function(adapter);
+	if (status)
+		goto err;
+
 	status = be_setup(adapter);
 	if (status)
 		goto err;
@@ -4024,6 +4095,9 @@ static void be_eeh_resume(struct pci_dev *pdev)
 		if (status)
 			goto err;
 	}
+
+	schedule_delayed_work(&adapter->func_recovery_work,
+			      msecs_to_jiffies(1000));
 	netif_device_attach(netdev);
 	return;
 err:
diff --git a/drivers/net/ethernet/ethoc.c b/drivers/net/ethernet/ethoc.c
index a3816781054..94b7bfcdb24 100644
--- a/drivers/net/ethernet/ethoc.c
+++ b/drivers/net/ethernet/ethoc.c
@@ -902,7 +902,7 @@ static const struct net_device_ops ethoc_netdev_ops = {
 };
 
 /**
- * ethoc_probe() - initialize OpenCores ethernet MAC
+ * ethoc_probe - initialize OpenCores ethernet MAC
  * pdev:	platform device
  */
 static int __devinit ethoc_probe(struct platform_device *pdev)
@@ -1057,7 +1057,7 @@ static int __devinit ethoc_probe(struct platform_device *pdev)
 	/* Check the MAC again for validity, if it still isn't choose and
 	 * program a random one. */
 	if (!is_valid_ether_addr(netdev->dev_addr)) {
-		random_ether_addr(netdev->dev_addr);
+		eth_random_addr(netdev->dev_addr);
 		random_mac = true;
 	}
 
@@ -1140,7 +1140,7 @@ out:
 }
 
 /**
- * ethoc_remove() - shutdown OpenCores ethernet MAC
+ * ethoc_remove - shutdown OpenCores ethernet MAC
  * @pdev:	platform device
  */
 static int __devexit ethoc_remove(struct platform_device *pdev)
diff --git a/drivers/net/ethernet/faraday/ftgmac100.c b/drivers/net/ethernet/faraday/ftgmac100.c
index 16b07048274..74d749e29aa 100644
--- a/drivers/net/ethernet/faraday/ftgmac100.c
+++ b/drivers/net/ethernet/faraday/ftgmac100.c
@@ -479,9 +479,14 @@ static bool ftgmac100_rx_packet(struct ftgmac100 *priv, int *processed)
 		rxdes = ftgmac100_current_rxdes(priv);
 	} while (!done);
 
-	if (skb->len <= 64)
+	/* Small frames are copied into linear part of skb to free one page */
+	if (skb->len <= 128) {
 		skb->truesize -= PAGE_SIZE;
-	__pskb_pull_tail(skb, min(skb->len, 64U));
+		__pskb_pull_tail(skb, skb->len);
+	} else {
+		/* We pull the minimum amount into linear part */
+		__pskb_pull_tail(skb, ETH_HLEN);
+	}
 	skb->protocol = eth_type_trans(skb, netdev);
 
 	netdev->stats.rx_packets++;
diff --git a/drivers/net/ethernet/faraday/ftmac100.c b/drivers/net/ethernet/faraday/ftmac100.c
index 829b1092fd7..b901a01e3fa 100644
--- a/drivers/net/ethernet/faraday/ftmac100.c
+++ b/drivers/net/ethernet/faraday/ftmac100.c
@@ -441,11 +441,14 @@ static bool ftmac100_rx_packet(struct ftmac100 *priv, int *processed)
 	skb->len += length;
 	skb->data_len += length;
 
-	/* page might be freed in __pskb_pull_tail() */
-	if (length > 64)
+	if (length > 128) {
 		skb->truesize += PAGE_SIZE;
-	__pskb_pull_tail(skb, min(length, 64));
-
+		/* We pull the minimum amount into linear part */
+		__pskb_pull_tail(skb, ETH_HLEN);
+	} else {
+		/* Small frames are copied into linear part to free one page */
+		__pskb_pull_tail(skb, length);
+	}
 	ftmac100_alloc_rx_page(priv, rxdes, GFP_ATOMIC);
 
 	ftmac100_rx_pointer_advance(priv);
diff --git a/drivers/net/ethernet/freescale/fec.c b/drivers/net/ethernet/freescale/fec.c
index ff7f4c5115a..fffd20528b5 100644
--- a/drivers/net/ethernet/freescale/fec.c
+++ b/drivers/net/ethernet/freescale/fec.c
@@ -49,6 +49,7 @@
 #include <linux/of_gpio.h>
 #include <linux/of_net.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/regulator/consumer.h>
 
 #include <asm/cacheflush.h>
 
@@ -1388,8 +1389,8 @@ fec_set_mac_address(struct net_device *ndev, void *p)
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-/*
- * fec_poll_controller: FEC Poll controller function
+/**
+ * fec_poll_controller - FEC Poll controller function
  * @dev: The FEC network adapter
  *
  * Polled functionality used by netconsole and others in non interrupt mode
@@ -1506,18 +1507,25 @@ static int __devinit fec_get_phy_mode_dt(struct platform_device *pdev)
 static void __devinit fec_reset_phy(struct platform_device *pdev)
 {
 	int err, phy_reset;
+	int msec = 1;
 	struct device_node *np = pdev->dev.of_node;
 
 	if (!np)
 		return;
 
+	of_property_read_u32(np, "phy-reset-duration", &msec);
+	/* A sane reset duration should not be longer than 1s */
+	if (msec > 1000)
+		msec = 1;
+
 	phy_reset = of_get_named_gpio(np, "phy-reset-gpios", 0);
-	err = gpio_request_one(phy_reset, GPIOF_OUT_INIT_LOW, "phy-reset");
+	err = devm_gpio_request_one(&pdev->dev, phy_reset,
+				    GPIOF_OUT_INIT_LOW, "phy-reset");
 	if (err) {
 		pr_debug("FEC: failed to get gpio phy-reset: %d\n", err);
 		return;
 	}
-	msleep(1);
+	msleep(msec);
 	gpio_set_value(phy_reset, 1);
 }
 #else /* CONFIG_OF */
@@ -1546,6 +1554,7 @@ fec_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id;
 	static int dev_id;
 	struct pinctrl *pinctrl;
+	struct regulator *reg_phy;
 
 	of_id = of_match_device(fec_dt_ids, &pdev->dev);
 	if (of_id)
@@ -1593,8 +1602,6 @@ fec_probe(struct platform_device *pdev)
 		fep->phy_interface = ret;
 	}
 
-	fec_reset_phy(pdev);
-
 	for (i = 0; i < FEC_IRQ_NUM; i++) {
 		irq = platform_get_irq(pdev, i);
 		if (irq < 0) {
@@ -1634,6 +1641,18 @@ fec_probe(struct platform_device *pdev)
 	clk_prepare_enable(fep->clk_ahb);
 	clk_prepare_enable(fep->clk_ipg);
 
+	reg_phy = devm_regulator_get(&pdev->dev, "phy");
+	if (!IS_ERR(reg_phy)) {
+		ret = regulator_enable(reg_phy);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Failed to enable phy regulator: %d\n", ret);
+			goto failed_regulator;
+		}
+	}
+
+	fec_reset_phy(pdev);
+
 	ret = fec_enet_init(ndev);
 	if (ret)
 		goto failed_init;
@@ -1655,6 +1674,7 @@ failed_register:
 	fec_enet_mii_remove(fep);
 failed_mii_init:
 failed_init:
+failed_regulator:
 	clk_disable_unprepare(fep->clk_ahb);
 	clk_disable_unprepare(fep->clk_ipg);
 failed_pin:
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index f7f0bf5d037..9527b28d70d 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -47,6 +47,9 @@
 #include "gianfar.h"
 #include "fsl_pq_mdio.h"
 
+/* Number of microseconds to wait for an MII register to respond */
+#define MII_TIMEOUT	1000
+
 struct fsl_pq_mdio_priv {
 	void __iomem *map;
 	struct fsl_pq_mdio __iomem *regs;
@@ -64,6 +67,8 @@ struct fsl_pq_mdio_priv {
 int fsl_pq_local_mdio_write(struct fsl_pq_mdio __iomem *regs, int mii_id,
 		int regnum, u16 value)
 {
+	u32 status;
+
 	/* Set the PHY address and the register address we want to write */
 	out_be32(&regs->miimadd, (mii_id << 8) | regnum);
 
@@ -71,10 +76,10 @@ int fsl_pq_local_mdio_write(struct fsl_pq_mdio __iomem *regs, int mii_id,
 	out_be32(&regs->miimcon, value);
 
 	/* Wait for the transaction to finish */
-	while (in_be32(&regs->miimind) & MIIMIND_BUSY)
-		cpu_relax();
+	status = spin_event_timeout(!(in_be32(&regs->miimind) &	MIIMIND_BUSY),
+				    MII_TIMEOUT, 0);
 
-	return 0;
+	return status ? 0 : -ETIMEDOUT;
 }
 
 /*
@@ -91,6 +96,7 @@ int fsl_pq_local_mdio_read(struct fsl_pq_mdio __iomem *regs,
 		int mii_id, int regnum)
 {
 	u16 value;
+	u32 status;
 
 	/* Set the PHY address and the register address we want to read */
 	out_be32(&regs->miimadd, (mii_id << 8) | regnum);
@@ -99,9 +105,12 @@ int fsl_pq_local_mdio_read(struct fsl_pq_mdio __iomem *regs,
 	out_be32(&regs->miimcom, 0);
 	out_be32(&regs->miimcom, MII_READ_COMMAND);
 
-	/* Wait for the transaction to finish */
-	while (in_be32(&regs->miimind) & (MIIMIND_NOTVALID | MIIMIND_BUSY))
-		cpu_relax();
+	/* Wait for the transaction to finish, normally less than 100us */
+	status = spin_event_timeout(!(in_be32(&regs->miimind) &
+				    (MIIMIND_NOTVALID | MIIMIND_BUSY)),
+				    MII_TIMEOUT, 0);
+	if (!status)
+		return -ETIMEDOUT;
 
 	/* Grab the value of the register from miimstat */
 	value = in_be32(&regs->miimstat);
@@ -144,7 +153,7 @@ int fsl_pq_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 static int fsl_pq_mdio_reset(struct mii_bus *bus)
 {
 	struct fsl_pq_mdio __iomem *regs = fsl_pq_mdio_get_regs(bus);
-	int timeout = PHY_INIT_TIMEOUT;
+	u32 status;
 
 	mutex_lock(&bus->mdio_lock);
 
@@ -155,12 +164,12 @@ static int fsl_pq_mdio_reset(struct mii_bus *bus)
 	out_be32(&regs->miimcfg, MIIMCFG_INIT_VALUE);
 
 	/* Wait until the bus is free */
-	while ((in_be32(&regs->miimind) & MIIMIND_BUSY) && timeout--)
-		cpu_relax();
+	status = spin_event_timeout(!(in_be32(&regs->miimind) &	MIIMIND_BUSY),
+				    MII_TIMEOUT, 0);
 
 	mutex_unlock(&bus->mdio_lock);
 
-	if (timeout < 0) {
+	if (!status) {
 		printk(KERN_ERR "%s: The MII Bus is stuck!\n",
 				bus->name);
 		return -EBUSY;
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index f2db8fca46a..4605f724668 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1,5 +1,4 @@
-/*
- * drivers/net/ethernet/freescale/gianfar.c
+/* drivers/net/ethernet/freescale/gianfar.c
  *
  * Gianfar Ethernet Driver
  * This driver is designed for the non-CPM ethernet controllers
@@ -114,7 +113,7 @@ static void gfar_timeout(struct net_device *dev);
 static int gfar_close(struct net_device *dev);
 struct sk_buff *gfar_new_skb(struct net_device *dev);
 static void gfar_new_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
-		struct sk_buff *skb);
+			   struct sk_buff *skb);
 static int gfar_set_mac_address(struct net_device *dev);
 static int gfar_change_mtu(struct net_device *dev, int new_mtu);
 static irqreturn_t gfar_error(int irq, void *dev_id);
@@ -266,8 +265,8 @@ static int gfar_alloc_skb_resources(struct net_device *ndev)
 		tx_queue->tx_bd_dma_base = addr;
 		tx_queue->dev = ndev;
 		/* enet DMA only understands physical addresses */
-		addr    += sizeof(struct txbd8) *tx_queue->tx_ring_size;
-		vaddr   += sizeof(struct txbd8) *tx_queue->tx_ring_size;
+		addr  += sizeof(struct txbd8) * tx_queue->tx_ring_size;
+		vaddr += sizeof(struct txbd8) * tx_queue->tx_ring_size;
 	}
 
 	/* Start the rx descriptor ring where the tx ring leaves off */
@@ -276,15 +275,16 @@ static int gfar_alloc_skb_resources(struct net_device *ndev)
 		rx_queue->rx_bd_base = vaddr;
 		rx_queue->rx_bd_dma_base = addr;
 		rx_queue->dev = ndev;
-		addr    += sizeof (struct rxbd8) * rx_queue->rx_ring_size;
-		vaddr   += sizeof (struct rxbd8) * rx_queue->rx_ring_size;
+		addr  += sizeof(struct rxbd8) * rx_queue->rx_ring_size;
+		vaddr += sizeof(struct rxbd8) * rx_queue->rx_ring_size;
 	}
 
 	/* Setup the skbuff rings */
 	for (i = 0; i < priv->num_tx_queues; i++) {
 		tx_queue = priv->tx_queue[i];
 		tx_queue->tx_skbuff = kmalloc(sizeof(*tx_queue->tx_skbuff) *
-				  tx_queue->tx_ring_size, GFP_KERNEL);
+					      tx_queue->tx_ring_size,
+					      GFP_KERNEL);
 		if (!tx_queue->tx_skbuff) {
 			netif_err(priv, ifup, ndev,
 				  "Could not allocate tx_skbuff\n");
@@ -298,7 +298,8 @@ static int gfar_alloc_skb_resources(struct net_device *ndev)
 	for (i = 0; i < priv->num_rx_queues; i++) {
 		rx_queue = priv->rx_queue[i];
 		rx_queue->rx_skbuff = kmalloc(sizeof(*rx_queue->rx_skbuff) *
-				  rx_queue->rx_ring_size, GFP_KERNEL);
+					      rx_queue->rx_ring_size,
+					      GFP_KERNEL);
 
 		if (!rx_queue->rx_skbuff) {
 			netif_err(priv, ifup, ndev,
@@ -327,15 +328,15 @@ static void gfar_init_tx_rx_base(struct gfar_private *priv)
 	int i;
 
 	baddr = &regs->tbase0;
-	for(i = 0; i < priv->num_tx_queues; i++) {
+	for (i = 0; i < priv->num_tx_queues; i++) {
 		gfar_write(baddr, priv->tx_queue[i]->tx_bd_dma_base);
-		baddr	+= 2;
+		baddr += 2;
 	}
 
 	baddr = &regs->rbase0;
-	for(i = 0; i < priv->num_rx_queues; i++) {
+	for (i = 0; i < priv->num_rx_queues; i++) {
 		gfar_write(baddr, priv->rx_queue[i]->rx_bd_dma_base);
-		baddr   += 2;
+		baddr += 2;
 	}
 }
 
@@ -405,7 +406,8 @@ static void gfar_init_mac(struct net_device *ndev)
 	gfar_write(&regs->attreli, attrs);
 
 	/* Start with defaults, and add stashing or locking
-	 * depending on the approprate variables */
+	 * depending on the approprate variables
+	 */
 	attrs = ATTR_INIT_SETTINGS;
 
 	if (priv->bd_stash_en)
@@ -426,16 +428,16 @@ static struct net_device_stats *gfar_get_stats(struct net_device *dev)
 	struct gfar_private *priv = netdev_priv(dev);
 	unsigned long rx_packets = 0, rx_bytes = 0, rx_dropped = 0;
 	unsigned long tx_packets = 0, tx_bytes = 0;
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_rx_queues; i++) {
 		rx_packets += priv->rx_queue[i]->stats.rx_packets;
-		rx_bytes += priv->rx_queue[i]->stats.rx_bytes;
+		rx_bytes   += priv->rx_queue[i]->stats.rx_bytes;
 		rx_dropped += priv->rx_queue[i]->stats.rx_dropped;
 	}
 
 	dev->stats.rx_packets = rx_packets;
-	dev->stats.rx_bytes = rx_bytes;
+	dev->stats.rx_bytes   = rx_bytes;
 	dev->stats.rx_dropped = rx_dropped;
 
 	for (i = 0; i < priv->num_tx_queues; i++) {
@@ -443,7 +445,7 @@ static struct net_device_stats *gfar_get_stats(struct net_device *dev)
 		tx_packets += priv->tx_queue[i]->stats.tx_packets;
 	}
 
-	dev->stats.tx_bytes = tx_bytes;
+	dev->stats.tx_bytes   = tx_bytes;
 	dev->stats.tx_packets = tx_packets;
 
 	return &dev->stats;
@@ -468,7 +470,7 @@ static const struct net_device_ops gfar_netdev_ops = {
 
 void lock_rx_qs(struct gfar_private *priv)
 {
-	int i = 0x0;
+	int i;
 
 	for (i = 0; i < priv->num_rx_queues; i++)
 		spin_lock(&priv->rx_queue[i]->rxlock);
@@ -476,7 +478,7 @@ void lock_rx_qs(struct gfar_private *priv)
 
 void lock_tx_qs(struct gfar_private *priv)
 {
-	int i = 0x0;
+	int i;
 
 	for (i = 0; i < priv->num_tx_queues; i++)
 		spin_lock(&priv->tx_queue[i]->txlock);
@@ -484,7 +486,7 @@ void lock_tx_qs(struct gfar_private *priv)
 
 void unlock_rx_qs(struct gfar_private *priv)
 {
-	int i = 0x0;
+	int i;
 
 	for (i = 0; i < priv->num_rx_queues; i++)
 		spin_unlock(&priv->rx_queue[i]->rxlock);
@@ -492,7 +494,7 @@ void unlock_rx_qs(struct gfar_private *priv)
 
 void unlock_tx_qs(struct gfar_private *priv)
 {
-	int i = 0x0;
+	int i;
 
 	for (i = 0; i < priv->num_tx_queues; i++)
 		spin_unlock(&priv->tx_queue[i]->txlock);
@@ -508,13 +510,13 @@ static bool gfar_is_vlan_on(struct gfar_private *priv)
 static inline int gfar_uses_fcb(struct gfar_private *priv)
 {
 	return gfar_is_vlan_on(priv) ||
-		(priv->ndev->features & NETIF_F_RXCSUM) ||
-		(priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER);
+	       (priv->ndev->features & NETIF_F_RXCSUM) ||
+	       (priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER);
 }
 
 static void free_tx_pointers(struct gfar_private *priv)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_tx_queues; i++)
 		kfree(priv->tx_queue[i]);
@@ -522,7 +524,7 @@ static void free_tx_pointers(struct gfar_private *priv)
 
 static void free_rx_pointers(struct gfar_private *priv)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_rx_queues; i++)
 		kfree(priv->rx_queue[i]);
@@ -530,7 +532,7 @@ static void free_rx_pointers(struct gfar_private *priv)
 
 static void unmap_group_regs(struct gfar_private *priv)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < MAXGROUPS; i++)
 		if (priv->gfargrp[i].regs)
@@ -539,7 +541,7 @@ static void unmap_group_regs(struct gfar_private *priv)
 
 static void disable_napi(struct gfar_private *priv)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_grps; i++)
 		napi_disable(&priv->gfargrp[i].napi);
@@ -547,14 +549,14 @@ static void disable_napi(struct gfar_private *priv)
 
 static void enable_napi(struct gfar_private *priv)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_grps; i++)
 		napi_enable(&priv->gfargrp[i].napi);
 }
 
 static int gfar_parse_group(struct device_node *np,
-		struct gfar_private *priv, const char *model)
+			    struct gfar_private *priv, const char *model)
 {
 	u32 *queue_mask;
 
@@ -580,15 +582,13 @@ static int gfar_parse_group(struct device_node *np,
 	priv->gfargrp[priv->num_grps].grp_id = priv->num_grps;
 	priv->gfargrp[priv->num_grps].priv = priv;
 	spin_lock_init(&priv->gfargrp[priv->num_grps].grplock);
-	if(priv->mode == MQ_MG_MODE) {
-		queue_mask = (u32 *)of_get_property(np,
-					"fsl,rx-bit-map", NULL);
-		priv->gfargrp[priv->num_grps].rx_bit_map =
-			queue_mask ?  *queue_mask :(DEFAULT_MAPPING >> priv->num_grps);
-		queue_mask = (u32 *)of_get_property(np,
-					"fsl,tx-bit-map", NULL);
-		priv->gfargrp[priv->num_grps].tx_bit_map =
-			queue_mask ? *queue_mask : (DEFAULT_MAPPING >> priv->num_grps);
+	if (priv->mode == MQ_MG_MODE) {
+		queue_mask = (u32 *)of_get_property(np, "fsl,rx-bit-map", NULL);
+		priv->gfargrp[priv->num_grps].rx_bit_map = queue_mask ?
+			*queue_mask : (DEFAULT_MAPPING >> priv->num_grps);
+		queue_mask = (u32 *)of_get_property(np, "fsl,tx-bit-map", NULL);
+		priv->gfargrp[priv->num_grps].tx_bit_map = queue_mask ?
+			*queue_mask : (DEFAULT_MAPPING >> priv->num_grps);
 	} else {
 		priv->gfargrp[priv->num_grps].rx_bit_map = 0xFF;
 		priv->gfargrp[priv->num_grps].tx_bit_map = 0xFF;
@@ -652,7 +652,7 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 	priv->num_rx_queues = num_rx_qs;
 	priv->num_grps = 0x0;
 
-	/* Init Rx queue filer rule set linked list*/
+	/* Init Rx queue filer rule set linked list */
 	INIT_LIST_HEAD(&priv->rx_list.list);
 	priv->rx_list.count = 0;
 	mutex_init(&priv->rx_queue_access);
@@ -673,7 +673,7 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 	} else {
 		priv->mode = SQ_SG_MODE;
 		err = gfar_parse_group(np, priv, model);
-		if(err)
+		if (err)
 			goto err_grp_init;
 	}
 
@@ -730,27 +730,27 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 		priv->device_flags |= FSL_GIANFAR_DEV_HAS_BUF_STASHING;
 
 	mac_addr = of_get_mac_address(np);
+
 	if (mac_addr)
 		memcpy(dev->dev_addr, mac_addr, ETH_ALEN);
 
 	if (model && !strcasecmp(model, "TSEC"))
-		priv->device_flags =
-			FSL_GIANFAR_DEV_HAS_GIGABIT |
-			FSL_GIANFAR_DEV_HAS_COALESCE |
-			FSL_GIANFAR_DEV_HAS_RMON |
-			FSL_GIANFAR_DEV_HAS_MULTI_INTR;
+		priv->device_flags = FSL_GIANFAR_DEV_HAS_GIGABIT |
+				     FSL_GIANFAR_DEV_HAS_COALESCE |
+				     FSL_GIANFAR_DEV_HAS_RMON |
+				     FSL_GIANFAR_DEV_HAS_MULTI_INTR;
+
 	if (model && !strcasecmp(model, "eTSEC"))
-		priv->device_flags =
-			FSL_GIANFAR_DEV_HAS_GIGABIT |
-			FSL_GIANFAR_DEV_HAS_COALESCE |
-			FSL_GIANFAR_DEV_HAS_RMON |
-			FSL_GIANFAR_DEV_HAS_MULTI_INTR |
-			FSL_GIANFAR_DEV_HAS_PADDING |
-			FSL_GIANFAR_DEV_HAS_CSUM |
-			FSL_GIANFAR_DEV_HAS_VLAN |
-			FSL_GIANFAR_DEV_HAS_MAGIC_PACKET |
-			FSL_GIANFAR_DEV_HAS_EXTENDED_HASH |
-			FSL_GIANFAR_DEV_HAS_TIMER;
+		priv->device_flags = FSL_GIANFAR_DEV_HAS_GIGABIT |
+				     FSL_GIANFAR_DEV_HAS_COALESCE |
+				     FSL_GIANFAR_DEV_HAS_RMON |
+				     FSL_GIANFAR_DEV_HAS_MULTI_INTR |
+				     FSL_GIANFAR_DEV_HAS_PADDING |
+				     FSL_GIANFAR_DEV_HAS_CSUM |
+				     FSL_GIANFAR_DEV_HAS_VLAN |
+				     FSL_GIANFAR_DEV_HAS_MAGIC_PACKET |
+				     FSL_GIANFAR_DEV_HAS_EXTENDED_HASH |
+				     FSL_GIANFAR_DEV_HAS_TIMER;
 
 	ctype = of_get_property(np, "phy-connection-type", NULL);
 
@@ -781,7 +781,7 @@ err_grp_init:
 }
 
 static int gfar_hwtstamp_ioctl(struct net_device *netdev,
-			struct ifreq *ifr, int cmd)
+			       struct ifreq *ifr, int cmd)
 {
 	struct hwtstamp_config config;
 	struct gfar_private *priv = netdev_priv(netdev);
@@ -851,6 +851,7 @@ static unsigned int reverse_bitmap(unsigned int bit_map, unsigned int max_qs)
 {
 	unsigned int new_bit_map = 0x0;
 	int mask = 0x1 << (max_qs - 1), i;
+
 	for (i = 0; i < max_qs; i++) {
 		if (bit_map & mask)
 			new_bit_map = new_bit_map + (1 << i);
@@ -936,22 +937,22 @@ static void gfar_detect_errata(struct gfar_private *priv)
 
 	/* MPC8313 Rev 2.0 and higher; All MPC837x */
 	if ((pvr == 0x80850010 && mod == 0x80b0 && rev >= 0x0020) ||
-			(pvr == 0x80861010 && (mod & 0xfff9) == 0x80c0))
+	    (pvr == 0x80861010 && (mod & 0xfff9) == 0x80c0))
 		priv->errata |= GFAR_ERRATA_74;
 
 	/* MPC8313 and MPC837x all rev */
 	if ((pvr == 0x80850010 && mod == 0x80b0) ||
-			(pvr == 0x80861010 && (mod & 0xfff9) == 0x80c0))
+	    (pvr == 0x80861010 && (mod & 0xfff9) == 0x80c0))
 		priv->errata |= GFAR_ERRATA_76;
 
 	/* MPC8313 and MPC837x all rev */
 	if ((pvr == 0x80850010 && mod == 0x80b0) ||
-			(pvr == 0x80861010 && (mod & 0xfff9) == 0x80c0))
+	    (pvr == 0x80861010 && (mod & 0xfff9) == 0x80c0))
 		priv->errata |= GFAR_ERRATA_A002;
 
 	/* MPC8313 Rev < 2.0, MPC8548 rev 2.0 */
 	if ((pvr == 0x80850010 && mod == 0x80b0 && rev < 0x0020) ||
-			(pvr == 0x80210020 && mod == 0x8030 && rev == 0x0020))
+	    (pvr == 0x80210020 && mod == 0x8030 && rev == 0x0020))
 		priv->errata |= GFAR_ERRATA_12;
 
 	if (priv->errata)
@@ -960,7 +961,8 @@ 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 */
+ * and anything else we need before we start
+ */
 static int gfar_probe(struct platform_device *ofdev)
 {
 	u32 tempval;
@@ -991,8 +993,9 @@ static int gfar_probe(struct platform_device *ofdev)
 
 	gfar_detect_errata(priv);
 
-	/* Stop the DMA engine now, in case it was running before */
-	/* (The firmware could have used it, and left it running). */
+	/* Stop the DMA engine now, in case it was running before
+	 * (The firmware could have used it, and left it running).
+	 */
 	gfar_halt(dev);
 
 	/* Reset MAC layer */
@@ -1026,13 +1029,14 @@ static int gfar_probe(struct platform_device *ofdev)
 
 	/* Register for napi ...We are registering NAPI for each grp */
 	for (i = 0; i < priv->num_grps; i++)
-		netif_napi_add(dev, &priv->gfargrp[i].napi, gfar_poll, GFAR_DEV_WEIGHT);
+		netif_napi_add(dev, &priv->gfargrp[i].napi, gfar_poll,
+			       GFAR_DEV_WEIGHT);
 
 	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_CSUM) {
 		dev->hw_features = NETIF_F_IP_CSUM | NETIF_F_SG |
-			NETIF_F_RXCSUM;
+				   NETIF_F_RXCSUM;
 		dev->features |= NETIF_F_IP_CSUM | NETIF_F_SG |
-			NETIF_F_RXCSUM | NETIF_F_HIGHDMA;
+				 NETIF_F_RXCSUM | NETIF_F_HIGHDMA;
 	}
 
 	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_VLAN) {
@@ -1081,7 +1085,7 @@ static int gfar_probe(struct platform_device *ofdev)
 		priv->padding = 0;
 
 	if (dev->features & NETIF_F_IP_CSUM ||
-			priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)
+	    priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)
 		dev->needed_headroom = GMAC_FCB_LEN;
 
 	/* Program the isrg regs only if number of grps > 1 */
@@ -1098,28 +1102,32 @@ static int gfar_probe(struct platform_device *ofdev)
 
 	/* Need to reverse the bit maps as  bit_map's MSB is q0
 	 * but, for_each_set_bit parses from right to left, which
-	 * basically reverses the queue numbers */
+	 * basically reverses the queue numbers
+	 */
 	for (i = 0; i< priv->num_grps; i++) {
-		priv->gfargrp[i].tx_bit_map = reverse_bitmap(
-				priv->gfargrp[i].tx_bit_map, MAX_TX_QS);
-		priv->gfargrp[i].rx_bit_map = reverse_bitmap(
-				priv->gfargrp[i].rx_bit_map, MAX_RX_QS);
+		priv->gfargrp[i].tx_bit_map =
+			reverse_bitmap(priv->gfargrp[i].tx_bit_map, MAX_TX_QS);
+		priv->gfargrp[i].rx_bit_map =
+			reverse_bitmap(priv->gfargrp[i].rx_bit_map, MAX_RX_QS);
 	}
 
 	/* Calculate RSTAT, TSTAT, RQUEUE and TQUEUE values,
-	 * also assign queues to groups */
+	 * also assign queues to groups
+	 */
 	for (grp_idx = 0; grp_idx < priv->num_grps; grp_idx++) {
 		priv->gfargrp[grp_idx].num_rx_queues = 0x0;
+
 		for_each_set_bit(i, &priv->gfargrp[grp_idx].rx_bit_map,
-				priv->num_rx_queues) {
+				 priv->num_rx_queues) {
 			priv->gfargrp[grp_idx].num_rx_queues++;
 			priv->rx_queue[i]->grp = &priv->gfargrp[grp_idx];
 			rstat = rstat | (RSTAT_CLEAR_RHALT >> i);
 			rqueue = rqueue | ((RQUEUE_EN0 | RQUEUE_EX0) >> i);
 		}
 		priv->gfargrp[grp_idx].num_tx_queues = 0x0;
+
 		for_each_set_bit(i, &priv->gfargrp[grp_idx].tx_bit_map,
-				priv->num_tx_queues) {
+				 priv->num_tx_queues) {
 			priv->gfargrp[grp_idx].num_tx_queues++;
 			priv->tx_queue[i]->grp = &priv->gfargrp[grp_idx];
 			tstat = tstat | (TSTAT_CLEAR_THALT >> i);
@@ -1149,7 +1157,7 @@ static int gfar_probe(struct platform_device *ofdev)
 		priv->rx_queue[i]->rxic = DEFAULT_RXIC;
 	}
 
-	/* always enable rx filer*/
+	/* always enable rx filer */
 	priv->rx_filer_enable = 1;
 	/* Enable most messages by default */
 	priv->msg_enable = (NETIF_MSG_IFUP << 1 ) - 1;
@@ -1165,7 +1173,8 @@ static int gfar_probe(struct platform_device *ofdev)
 	}
 
 	device_init_wakeup(&dev->dev,
-		priv->device_flags & FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
+			   priv->device_flags &
+			   FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
 
 	/* fill out IRQ number and name fields */
 	for (i = 0; i < priv->num_grps; i++) {
@@ -1189,13 +1198,14 @@ static int gfar_probe(struct platform_device *ofdev)
 	/* Print out the device info */
 	netdev_info(dev, "mac: %pM\n", dev->dev_addr);
 
-	/* Even more device info helps when determining which kernel */
-	/* provided which set of benchmarks. */
+	/* Even more device info helps when determining which kernel
+	 * provided which set of benchmarks.
+	 */
 	netdev_info(dev, "Running with NAPI enabled\n");
 	for (i = 0; i < priv->num_rx_queues; i++)
 		netdev_info(dev, "RX BD ring size for Q[%d]: %d\n",
 			    i, priv->rx_queue[i]->rx_ring_size);
-	for(i = 0; i < priv->num_tx_queues; i++)
+	for (i = 0; i < priv->num_tx_queues; i++)
 		netdev_info(dev, "TX BD ring size for Q[%d]: %d\n",
 			    i, priv->tx_queue[i]->tx_ring_size);
 
@@ -1242,7 +1252,8 @@ static int gfar_suspend(struct device *dev)
 	u32 tempval;
 
 	int magic_packet = priv->wol_en &&
-		(priv->device_flags & FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
+			   (priv->device_flags &
+			    FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
 
 	netif_device_detach(ndev);
 
@@ -1294,7 +1305,8 @@ static int gfar_resume(struct device *dev)
 	unsigned long flags;
 	u32 tempval;
 	int magic_packet = priv->wol_en &&
-		(priv->device_flags & FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
+			   (priv->device_flags &
+			    FSL_GIANFAR_DEV_HAS_MAGIC_PACKET);
 
 	if (!netif_running(ndev)) {
 		netif_device_attach(ndev);
@@ -1393,13 +1405,13 @@ static phy_interface_t gfar_get_interface(struct net_device *dev)
 	}
 
 	if (ecntrl & ECNTRL_REDUCED_MODE) {
-		if (ecntrl & ECNTRL_REDUCED_MII_MODE)
+		if (ecntrl & ECNTRL_REDUCED_MII_MODE) {
 			return PHY_INTERFACE_MODE_RMII;
+		}
 		else {
 			phy_interface_t interface = priv->interface;
 
-			/*
-			 * This isn't autodetected right now, so it must
+			/* This isn't autodetected right now, so it must
 			 * be set by the device tree or platform code.
 			 */
 			if (interface == PHY_INTERFACE_MODE_RGMII_ID)
@@ -1453,8 +1465,7 @@ static int init_phy(struct net_device *dev)
 	return 0;
 }
 
-/*
- * Initialize TBI PHY interface for communicating with the
+/* Initialize TBI PHY interface for communicating with the
  * SERDES lynx PHY on the chip.  We communicate with this PHY
  * through the MDIO bus on each controller, treating it as a
  * "normal" PHY at the address found in the TBIPA register.  We assume
@@ -1479,8 +1490,7 @@ static void gfar_configure_serdes(struct net_device *dev)
 		return;
 	}
 
-	/*
-	 * If the link is already up, we must already be ok, and don't need to
+	/* If the link is already up, we must already be ok, and don't need to
 	 * configure and reset the TBI<->SerDes link.  Maybe U-Boot configured
 	 * everything for us?  Resetting it takes the link down and requires
 	 * several seconds for it to come back.
@@ -1492,18 +1502,19 @@ static void gfar_configure_serdes(struct net_device *dev)
 	phy_write(tbiphy, MII_TBICON, TBICON_CLK_SELECT);
 
 	phy_write(tbiphy, MII_ADVERTISE,
-			ADVERTISE_1000XFULL | ADVERTISE_1000XPAUSE |
-			ADVERTISE_1000XPSE_ASYM);
+		  ADVERTISE_1000XFULL | ADVERTISE_1000XPAUSE |
+		  ADVERTISE_1000XPSE_ASYM);
 
-	phy_write(tbiphy, MII_BMCR, BMCR_ANENABLE |
-			BMCR_ANRESTART | BMCR_FULLDPLX | BMCR_SPEED1000);
+	phy_write(tbiphy, MII_BMCR,
+		  BMCR_ANENABLE | BMCR_ANRESTART | BMCR_FULLDPLX |
+		  BMCR_SPEED1000);
 }
 
 static void init_registers(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct gfar __iomem *regs = NULL;
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_grps; i++) {
 		regs = priv->gfargrp[i].regs;
@@ -1554,15 +1565,13 @@ static int __gfar_is_rx_idle(struct gfar_private *priv)
 {
 	u32 res;
 
-	/*
-	 * Normaly TSEC should not hang on GRS commands, so we should
+	/* Normaly TSEC should not hang on GRS commands, so we should
 	 * actually wait for IEVENT_GRSC flag.
 	 */
 	if (likely(!gfar_has_errata(priv, GFAR_ERRATA_A002)))
 		return 0;
 
-	/*
-	 * Read the eTSEC register at offset 0xD1C. If bits 7-14 are
+	/* Read the eTSEC register at offset 0xD1C. If bits 7-14 are
 	 * the same as bits 23-30, the eTSEC Rx is assumed to be idle
 	 * and the Rx can be safely reset.
 	 */
@@ -1580,7 +1589,7 @@ static void gfar_halt_nodisable(struct net_device *dev)
 	struct gfar_private *priv = netdev_priv(dev);
 	struct gfar __iomem *regs = NULL;
 	u32 tempval;
-	int i = 0;
+	int i;
 
 	for (i = 0; i < priv->num_grps; i++) {
 		regs = priv->gfargrp[i].regs;
@@ -1594,8 +1603,8 @@ static void gfar_halt_nodisable(struct net_device *dev)
 	regs = priv->gfargrp[0].regs;
 	/* Stop the DMA, and wait for it to stop */
 	tempval = gfar_read(&regs->dmactrl);
-	if ((tempval & (DMACTRL_GRS | DMACTRL_GTS))
-	    != (DMACTRL_GRS | DMACTRL_GTS)) {
+	if ((tempval & (DMACTRL_GRS | DMACTRL_GTS)) !=
+	    (DMACTRL_GRS | DMACTRL_GTS)) {
 		int ret;
 
 		tempval |= (DMACTRL_GRS | DMACTRL_GTS);
@@ -1660,7 +1669,7 @@ void stop_gfar(struct net_device *dev)
 	} else {
 		for (i = 0; i < priv->num_grps; i++)
 			free_irq(priv->gfargrp[i].interruptTransmit,
-					&priv->gfargrp[i]);
+				 &priv->gfargrp[i]);
 	}
 
 	free_skb_resources(priv);
@@ -1679,13 +1688,13 @@ static void free_skb_tx_queue(struct gfar_priv_tx_q *tx_queue)
 			continue;
 
 		dma_unmap_single(&priv->ofdev->dev, txbdp->bufPtr,
-				txbdp->length, DMA_TO_DEVICE);
+				 txbdp->length, DMA_TO_DEVICE);
 		txbdp->lstatus = 0;
 		for (j = 0; j < skb_shinfo(tx_queue->tx_skbuff[i])->nr_frags;
-				j++) {
+		     j++) {
 			txbdp++;
 			dma_unmap_page(&priv->ofdev->dev, txbdp->bufPtr,
-					txbdp->length, DMA_TO_DEVICE);
+				       txbdp->length, DMA_TO_DEVICE);
 		}
 		txbdp++;
 		dev_kfree_skb_any(tx_queue->tx_skbuff[i]);
@@ -1705,8 +1714,8 @@ static void free_skb_rx_queue(struct gfar_priv_rx_q *rx_queue)
 	for (i = 0; i < rx_queue->rx_ring_size; i++) {
 		if (rx_queue->rx_skbuff[i]) {
 			dma_unmap_single(&priv->ofdev->dev,
-					rxbdp->bufPtr, priv->rx_buffer_size,
-					DMA_FROM_DEVICE);
+					 rxbdp->bufPtr, priv->rx_buffer_size,
+					 DMA_FROM_DEVICE);
 			dev_kfree_skb_any(rx_queue->rx_skbuff[i]);
 			rx_queue->rx_skbuff[i] = NULL;
 		}
@@ -1718,7 +1727,8 @@ static void free_skb_rx_queue(struct gfar_priv_rx_q *rx_queue)
 }
 
 /* If there are any tx skbs or rx skbs still around, free them.
- * Then free tx_skbuff and rx_skbuff */
+ * Then free tx_skbuff and rx_skbuff
+ */
 static void free_skb_resources(struct gfar_private *priv)
 {
 	struct gfar_priv_tx_q *tx_queue = NULL;
@@ -1728,24 +1738,25 @@ static void free_skb_resources(struct gfar_private *priv)
 	/* Go through all the buffer descriptors and free their data buffers */
 	for (i = 0; i < priv->num_tx_queues; i++) {
 		struct netdev_queue *txq;
+
 		tx_queue = priv->tx_queue[i];
 		txq = netdev_get_tx_queue(tx_queue->dev, tx_queue->qindex);
-		if(tx_queue->tx_skbuff)
+		if (tx_queue->tx_skbuff)
 			free_skb_tx_queue(tx_queue);
 		netdev_tx_reset_queue(txq);
 	}
 
 	for (i = 0; i < priv->num_rx_queues; i++) {
 		rx_queue = priv->rx_queue[i];
-		if(rx_queue->rx_skbuff)
+		if (rx_queue->rx_skbuff)
 			free_skb_rx_queue(rx_queue);
 	}
 
 	dma_free_coherent(&priv->ofdev->dev,
-			sizeof(struct txbd8) * priv->total_tx_ring_size +
-			sizeof(struct rxbd8) * priv->total_rx_ring_size,
-			priv->tx_queue[0]->tx_bd_base,
-			priv->tx_queue[0]->tx_bd_dma_base);
+			  sizeof(struct txbd8) * priv->total_tx_ring_size +
+			  sizeof(struct rxbd8) * priv->total_rx_ring_size,
+			  priv->tx_queue[0]->tx_bd_base,
+			  priv->tx_queue[0]->tx_bd_dma_base);
 	skb_queue_purge(&priv->rx_recycle);
 }
 
@@ -1784,7 +1795,7 @@ void gfar_start(struct net_device *dev)
 }
 
 void gfar_configure_coalescing(struct gfar_private *priv,
-	unsigned long tx_mask, unsigned long rx_mask)
+			       unsigned long tx_mask, unsigned long rx_mask)
 {
 	struct gfar __iomem *regs = priv->gfargrp[0].regs;
 	u32 __iomem *baddr;
@@ -1794,11 +1805,11 @@ void gfar_configure_coalescing(struct gfar_private *priv,
 	 * multiple queues, there's only single reg to program
 	 */
 	gfar_write(&regs->txic, 0);
-	if(likely(priv->tx_queue[0]->txcoalescing))
+	if (likely(priv->tx_queue[0]->txcoalescing))
 		gfar_write(&regs->txic, priv->tx_queue[0]->txic);
 
 	gfar_write(&regs->rxic, 0);
-	if(unlikely(priv->rx_queue[0]->rxcoalescing))
+	if (unlikely(priv->rx_queue[0]->rxcoalescing))
 		gfar_write(&regs->rxic, priv->rx_queue[0]->rxic);
 
 	if (priv->mode == MQ_MG_MODE) {
@@ -1825,12 +1836,14 @@ static int register_grp_irqs(struct gfar_priv_grp *grp)
 	int err;
 
 	/* If the device has multiple interrupts, register for
-	 * them.  Otherwise, only register for the one */
+	 * them.  Otherwise, only register for the one
+	 */
 	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_MULTI_INTR) {
 		/* Install our interrupt handlers for Error,
-		 * Transmit, and Receive */
-		if ((err = request_irq(grp->interruptError, gfar_error, 0,
-				grp->int_name_er,grp)) < 0) {
+		 * Transmit, and Receive
+		 */
+		if ((err = request_irq(grp->interruptError, gfar_error,
+				       0, grp->int_name_er, grp)) < 0) {
 			netif_err(priv, intr, dev, "Can't get IRQ %d\n",
 				  grp->interruptError);
 
@@ -1838,21 +1851,21 @@ static int register_grp_irqs(struct gfar_priv_grp *grp)
 		}
 
 		if ((err = request_irq(grp->interruptTransmit, gfar_transmit,
-				0, grp->int_name_tx, grp)) < 0) {
+				       0, grp->int_name_tx, grp)) < 0) {
 			netif_err(priv, intr, dev, "Can't get IRQ %d\n",
 				  grp->interruptTransmit);
 			goto tx_irq_fail;
 		}
 
-		if ((err = request_irq(grp->interruptReceive, gfar_receive, 0,
-				grp->int_name_rx, grp)) < 0) {
+		if ((err = request_irq(grp->interruptReceive, gfar_receive,
+				       0, grp->int_name_rx, grp)) < 0) {
 			netif_err(priv, intr, dev, "Can't get IRQ %d\n",
 				  grp->interruptReceive);
 			goto rx_irq_fail;
 		}
 	} else {
-		if ((err = request_irq(grp->interruptTransmit, gfar_interrupt, 0,
-				grp->int_name_tx, grp)) < 0) {
+		if ((err = request_irq(grp->interruptTransmit, gfar_interrupt,
+				       0, grp->int_name_tx, grp)) < 0) {
 			netif_err(priv, intr, dev, "Can't get IRQ %d\n",
 				  grp->interruptTransmit);
 			goto err_irq_fail;
@@ -1912,8 +1925,9 @@ irq_fail:
 	return err;
 }
 
-/* Called when something needs to use the ethernet device */
-/* Returns 0 for success. */
+/* Called when something needs to use the ethernet device
+ * Returns 0 for success.
+ */
 static int gfar_enet_open(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
@@ -1958,18 +1972,17 @@ static inline struct txfcb *gfar_add_fcb(struct sk_buff *skb)
 }
 
 static inline void gfar_tx_checksum(struct sk_buff *skb, struct txfcb *fcb,
-		int fcb_length)
+				    int fcb_length)
 {
-	u8 flags = 0;
-
 	/* If we're here, it's a IP packet with a TCP or UDP
 	 * payload.  We set it to checksum, using a pseudo-header
 	 * we provide
 	 */
-	flags = TXFCB_DEFAULT;
+	u8 flags = TXFCB_DEFAULT;
 
-	/* Tell the controller what the protocol is */
-	/* And provide the already calculated phcs */
+	/* Tell the controller what the protocol is
+	 * And provide the already calculated phcs
+	 */
 	if (ip_hdr(skb)->protocol == IPPROTO_UDP) {
 		flags |= TXFCB_UDP;
 		fcb->phcs = udp_hdr(skb)->check;
@@ -1979,7 +1992,8 @@ static inline void gfar_tx_checksum(struct sk_buff *skb, struct txfcb *fcb,
 	/* l3os is the distance between the start of the
 	 * frame (skb->data) and the start of the IP hdr.
 	 * l4os is the distance between the start of the
-	 * l3 hdr and the l4 hdr */
+	 * l3 hdr and the l4 hdr
+	 */
 	fcb->l3os = (u16)(skb_network_offset(skb) - fcb_length);
 	fcb->l4os = skb_network_header_len(skb);
 
@@ -1993,7 +2007,7 @@ void inline gfar_tx_vlan(struct sk_buff *skb, struct txfcb *fcb)
 }
 
 static inline struct txbd8 *skip_txbd(struct txbd8 *bdp, int stride,
-			       struct txbd8 *base, int ring_size)
+				      struct txbd8 *base, int ring_size)
 {
 	struct txbd8 *new_bd = bdp + stride;
 
@@ -2001,13 +2015,14 @@ static inline struct txbd8 *skip_txbd(struct txbd8 *bdp, int stride,
 }
 
 static inline struct txbd8 *next_txbd(struct txbd8 *bdp, struct txbd8 *base,
-		int ring_size)
+				      int ring_size)
 {
 	return skip_txbd(bdp, 1, base, ring_size);
 }
 
-/* This is called by the kernel when a frame is ready for transmission. */
-/* It is pointed to by the dev->hard_start_xmit function pointer */
+/* This is called by the kernel when a frame is ready for transmission.
+ * It is pointed to by the dev->hard_start_xmit function pointer
+ */
 static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
@@ -2022,13 +2037,12 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	unsigned long flags;
 	unsigned int nr_frags, nr_txbds, length, fcb_length = GMAC_FCB_LEN;
 
-	/*
-	 * TOE=1 frames larger than 2500 bytes may see excess delays
+	/* TOE=1 frames larger than 2500 bytes may see excess delays
 	 * before start of transmission.
 	 */
 	if (unlikely(gfar_has_errata(priv, GFAR_ERRATA_76) &&
-			skb->ip_summed == CHECKSUM_PARTIAL &&
-			skb->len > 2500)) {
+		     skb->ip_summed == CHECKSUM_PARTIAL &&
+		     skb->len > 2500)) {
 		int ret;
 
 		ret = skb_checksum_help(skb);
@@ -2044,16 +2058,16 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* check if time stamp should be generated */
 	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
-			priv->hwts_tx_en)) {
+		     priv->hwts_tx_en)) {
 		do_tstamp = 1;
 		fcb_length = GMAC_FCB_LEN + GMAC_TXPAL_LEN;
 	}
 
 	/* make space for additional header when fcb is needed */
 	if (((skb->ip_summed == CHECKSUM_PARTIAL) ||
-			vlan_tx_tag_present(skb) ||
-			unlikely(do_tstamp)) &&
-			(skb_headroom(skb) < fcb_length)) {
+	     vlan_tx_tag_present(skb) ||
+	     unlikely(do_tstamp)) &&
+	    (skb_headroom(skb) < fcb_length)) {
 		struct sk_buff *skb_new;
 
 		skb_new = skb_realloc_headroom(skb, fcb_length);
@@ -2063,10 +2077,9 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			return NETDEV_TX_OK;
 		}
 
-		/* Steal sock reference for processing TX time stamps */
-		swap(skb_new->sk, skb->sk);
-		swap(skb_new->destructor, skb->destructor);
-		kfree_skb(skb);
+		if (skb->sk)
+			skb_set_owner_w(skb_new, skb->sk);
+		consume_skb(skb);
 		skb = skb_new;
 	}
 
@@ -2097,12 +2110,12 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* Time stamp insertion requires one additional TxBD */
 	if (unlikely(do_tstamp))
 		txbdp_tstamp = txbdp = next_txbd(txbdp, base,
-				tx_queue->tx_ring_size);
+						 tx_queue->tx_ring_size);
 
 	if (nr_frags == 0) {
 		if (unlikely(do_tstamp))
 			txbdp_tstamp->lstatus |= BD_LFLAG(TXBD_LAST |
-					TXBD_INTERRUPT);
+							  TXBD_INTERRUPT);
 		else
 			lstatus |= BD_LFLAG(TXBD_LAST | TXBD_INTERRUPT);
 	} else {
@@ -2114,7 +2127,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			length = skb_shinfo(skb)->frags[i].size;
 
 			lstatus = txbdp->lstatus | length |
-				BD_LFLAG(TXBD_READY);
+				  BD_LFLAG(TXBD_READY);
 
 			/* Handle the last BD specially */
 			if (i == nr_frags - 1)
@@ -2144,8 +2157,8 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (CHECKSUM_PARTIAL == skb->ip_summed) {
 		fcb = gfar_add_fcb(skb);
 		/* as specified by errata */
-		if (unlikely(gfar_has_errata(priv, GFAR_ERRATA_12)
-			     && ((unsigned long)fcb % 0x20) > 0x18)) {
+		if (unlikely(gfar_has_errata(priv, GFAR_ERRATA_12) &&
+			     ((unsigned long)fcb % 0x20) > 0x18)) {
 			__skb_pull(skb, GMAC_FCB_LEN);
 			skb_checksum_help(skb);
 		} else {
@@ -2173,10 +2186,9 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	txbdp_start->bufPtr = dma_map_single(&priv->ofdev->dev, skb->data,
-			skb_headlen(skb), DMA_TO_DEVICE);
+					     skb_headlen(skb), DMA_TO_DEVICE);
 
-	/*
-	 * If time stamping is requested one additional TxBD must be set up. The
+	/* If time stamping is requested one additional TxBD must be set up. The
 	 * first TxBD points to the FCB and must have a data length of
 	 * GMAC_FCB_LEN. The second TxBD points to the actual frame data with
 	 * the full frame length.
@@ -2184,7 +2196,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(do_tstamp)) {
 		txbdp_tstamp->bufPtr = txbdp_start->bufPtr + fcb_length;
 		txbdp_tstamp->lstatus |= BD_LFLAG(TXBD_READY) |
-				(skb_headlen(skb) - fcb_length);
+					 (skb_headlen(skb) - fcb_length);
 		lstatus |= BD_LFLAG(TXBD_CRC | TXBD_READY) | GMAC_FCB_LEN;
 	} else {
 		lstatus |= BD_LFLAG(TXBD_CRC | TXBD_READY) | skb_headlen(skb);
@@ -2192,8 +2204,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	netdev_tx_sent_queue(txq, skb->len);
 
-	/*
-	 * We can work in parallel with gfar_clean_tx_ring(), except
+	/* We can work in parallel with gfar_clean_tx_ring(), except
 	 * when modifying num_txbdfree. Note that we didn't grab the lock
 	 * when we were reading the num_txbdfree and checking for available
 	 * space, that's because outside of this function it can only grow,
@@ -2206,8 +2217,7 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 */
 	spin_lock_irqsave(&tx_queue->txlock, flags);
 
-	/*
-	 * The powerpc-specific eieio() is used, as wmb() has too strong
+	/* The powerpc-specific eieio() is used, as wmb() has too strong
 	 * semantics (it requires synchronization between cacheable and
 	 * uncacheable mappings, which eieio doesn't provide and which we
 	 * don't need), thus requiring a more expensive sync instruction.  At
@@ -2223,9 +2233,10 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx_queue->tx_skbuff[tx_queue->skb_curtx] = skb;
 
 	/* Update the current skb pointer to the next entry we will use
-	 * (wrapping if necessary) */
+	 * (wrapping if necessary)
+	 */
 	tx_queue->skb_curtx = (tx_queue->skb_curtx + 1) &
-		TX_RING_MOD_MASK(tx_queue->tx_ring_size);
+			      TX_RING_MOD_MASK(tx_queue->tx_ring_size);
 
 	tx_queue->cur_tx = next_txbd(txbdp, base, tx_queue->tx_ring_size);
 
@@ -2233,7 +2244,8 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx_queue->num_txbdfree -= (nr_txbds);
 
 	/* If the next BD still needs to be cleaned up, then the bds
-	   are full.  We need to tell the kernel to stop sending us stuff. */
+	 * are full.  We need to tell the kernel to stop sending us stuff.
+	 */
 	if (!tx_queue->num_txbdfree) {
 		netif_tx_stop_queue(txq);
 
@@ -2358,12 +2370,12 @@ static int gfar_change_mtu(struct net_device *dev, int new_mtu)
 
 	frame_size += priv->padding;
 
-	tempsize =
-	    (frame_size & ~(INCREMENTAL_BUFFER_SIZE - 1)) +
-	    INCREMENTAL_BUFFER_SIZE;
+	tempsize = (frame_size & ~(INCREMENTAL_BUFFER_SIZE - 1)) +
+		   INCREMENTAL_BUFFER_SIZE;
 
 	/* Only stop and start the controller if it isn't already
-	 * stopped, and we changed something */
+	 * stopped, and we changed something
+	 */
 	if ((oldsize != tempsize) && (dev->flags & IFF_UP))
 		stop_gfar(dev);
 
@@ -2376,11 +2388,12 @@ static int gfar_change_mtu(struct net_device *dev, int new_mtu)
 
 	/* If the mtu is larger than the max size for standard
 	 * ethernet frames (ie, a jumbo frame), then set maccfg2
-	 * to allow huge frames, and to check the length */
+	 * to allow huge frames, and to check the length
+	 */
 	tempval = gfar_read(&regs->maccfg2);
 
 	if (priv->rx_buffer_size > DEFAULT_RX_BUFFER_SIZE ||
-			gfar_has_errata(priv, GFAR_ERRATA_74))
+	    gfar_has_errata(priv, GFAR_ERRATA_74))
 		tempval |= (MACCFG2_HUGEFRAME | MACCFG2_LENGTHCHECK);
 	else
 		tempval &= ~(MACCFG2_HUGEFRAME | MACCFG2_LENGTHCHECK);
@@ -2401,7 +2414,7 @@ static int gfar_change_mtu(struct net_device *dev, int new_mtu)
 static void gfar_reset_task(struct work_struct *work)
 {
 	struct gfar_private *priv = container_of(work, struct gfar_private,
-			reset_task);
+						 reset_task);
 	struct net_device *dev = priv->ndev;
 
 	if (dev->flags & IFF_UP) {
@@ -2428,7 +2441,7 @@ static void gfar_align_skb(struct sk_buff *skb)
 	 * as many bytes as needed to align the data properly
 	 */
 	skb_reserve(skb, RXBUF_ALIGNMENT -
-		(((unsigned long) skb->data) & (RXBUF_ALIGNMENT - 1)));
+		    (((unsigned long) skb->data) & (RXBUF_ALIGNMENT - 1)));
 }
 
 /* Interrupt Handler for Transmit complete */
@@ -2462,8 +2475,7 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
 		frags = skb_shinfo(skb)->nr_frags;
 
-		/*
-		 * When time stamping, one additional TxBD must be freed.
+		/* When time stamping, one additional TxBD must be freed.
 		 * Also, we need to dma_unmap_single() the TxPAL.
 		 */
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
@@ -2477,7 +2489,7 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
 		/* Only clean completed frames */
 		if ((lstatus & BD_LFLAG(TXBD_READY)) &&
-				(lstatus & BD_LENGTH_MASK))
+		    (lstatus & BD_LENGTH_MASK))
 			break;
 
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) {
@@ -2487,11 +2499,12 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 			buflen = bdp->length;
 
 		dma_unmap_single(&priv->ofdev->dev, bdp->bufPtr,
-				buflen, DMA_TO_DEVICE);
+				 buflen, DMA_TO_DEVICE);
 
 		if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)) {
 			struct skb_shared_hwtstamps shhwtstamps;
 			u64 *ns = (u64*) (((u32)skb->data + 0x10) & ~0x7);
+
 			memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 			shhwtstamps.hwtstamp = ns_to_ktime(*ns);
 			skb_pull(skb, GMAC_FCB_LEN + GMAC_TXPAL_LEN);
@@ -2504,23 +2517,20 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 		bdp = next_txbd(bdp, base, tx_ring_size);
 
 		for (i = 0; i < frags; i++) {
-			dma_unmap_page(&priv->ofdev->dev,
-					bdp->bufPtr,
-					bdp->length,
-					DMA_TO_DEVICE);
+			dma_unmap_page(&priv->ofdev->dev, bdp->bufPtr,
+				       bdp->length, DMA_TO_DEVICE);
 			bdp->lstatus &= BD_LFLAG(TXBD_WRAP);
 			bdp = next_txbd(bdp, base, tx_ring_size);
 		}
 
 		bytes_sent += skb->len;
 
-		/*
-		 * If there's room in the queue (limit it to rx_buffer_size)
+		/* If there's room in the queue (limit it to rx_buffer_size)
 		 * we add this skb back into the pool, if it's the right size
 		 */
 		if (skb_queue_len(&priv->rx_recycle) < rx_queue->rx_ring_size &&
-				skb_recycle_check(skb, priv->rx_buffer_size +
-					RXBUF_ALIGNMENT)) {
+		    skb_recycle_check(skb, priv->rx_buffer_size +
+				      RXBUF_ALIGNMENT)) {
 			gfar_align_skb(skb);
 			skb_queue_head(&priv->rx_recycle, skb);
 		} else
@@ -2529,7 +2539,7 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 		tx_queue->tx_skbuff[skb_dirtytx] = NULL;
 
 		skb_dirtytx = (skb_dirtytx + 1) &
-			TX_RING_MOD_MASK(tx_ring_size);
+			      TX_RING_MOD_MASK(tx_ring_size);
 
 		howmany++;
 		spin_lock_irqsave(&tx_queue->txlock, flags);
@@ -2559,8 +2569,7 @@ static void gfar_schedule_cleanup(struct gfar_priv_grp *gfargrp)
 		gfar_write(&gfargrp->regs->imask, IMASK_RTX_DISABLED);
 		__napi_schedule(&gfargrp->napi);
 	} else {
-		/*
-		 * Clear IEVENT, so interrupts aren't called again
+		/* Clear IEVENT, so interrupts aren't called again
 		 * because of the packets that have already arrived.
 		 */
 		gfar_write(&gfargrp->regs->ievent, IEVENT_RTX_MASK);
@@ -2577,7 +2586,7 @@ static irqreturn_t gfar_transmit(int irq, void *grp_id)
 }
 
 static void gfar_new_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
-		struct sk_buff *skb)
+			   struct sk_buff *skb)
 {
 	struct net_device *dev = rx_queue->dev;
 	struct gfar_private *priv = netdev_priv(dev);
@@ -2588,7 +2597,7 @@ static void gfar_new_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
 	gfar_init_rxbdp(rx_queue, bdp, buf);
 }
 
-static struct sk_buff * gfar_alloc_skb(struct net_device *dev)
+static struct sk_buff *gfar_alloc_skb(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct sk_buff *skb = NULL;
@@ -2602,7 +2611,7 @@ static struct sk_buff * gfar_alloc_skb(struct net_device *dev)
 	return skb;
 }
 
-struct sk_buff * gfar_new_skb(struct net_device *dev)
+struct sk_buff *gfar_new_skb(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct sk_buff *skb = NULL;
@@ -2620,8 +2629,7 @@ static inline void count_errors(unsigned short status, struct net_device *dev)
 	struct net_device_stats *stats = &dev->stats;
 	struct gfar_extra_stats *estats = &priv->extra_stats;
 
-	/* If the packet was truncated, none of the other errors
-	 * matter */
+	/* If the packet was truncated, none of the other errors matter */
 	if (status & RXBD_TRUNCATED) {
 		stats->rx_length_errors++;
 
@@ -2662,7 +2670,8 @@ static inline void gfar_rx_checksum(struct sk_buff *skb, struct rxfcb *fcb)
 {
 	/* If valid headers were found, and valid sums
 	 * were verified, then we tell the kernel that no
-	 * checksumming is necessary.  Otherwise, it is */
+	 * checksumming is necessary.  Otherwise, it is [FIXME]
+	 */
 	if ((fcb->flags & RXFCB_CSUM_MASK) == (RXFCB_CIP | RXFCB_CTU))
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
 	else
@@ -2670,8 +2679,7 @@ static inline void gfar_rx_checksum(struct sk_buff *skb, struct rxfcb *fcb)
 }
 
 
-/* gfar_process_frame() -- handle one incoming packet if skb
- * isn't NULL.  */
+/* gfar_process_frame() -- handle one incoming packet if skb isn't NULL. */
 static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 			      int amount_pull, struct napi_struct *napi)
 {
@@ -2683,8 +2691,9 @@ static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	/* fcb is at the beginning if exists */
 	fcb = (struct rxfcb *)skb->data;
 
-	/* Remove the FCB from the skb */
-	/* Remove the padded bytes, if there are any */
+	/* Remove the FCB from the skb
+	 * Remove the padded bytes, if there are any
+	 */
 	if (amount_pull) {
 		skb_record_rx_queue(skb, fcb->rq);
 		skb_pull(skb, amount_pull);
@@ -2694,6 +2703,7 @@ static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	if (priv->hwts_rx_en) {
 		struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
 		u64 *ns = (u64 *) skb->data;
+
 		memset(shhwtstamps, 0, sizeof(*shhwtstamps));
 		shhwtstamps->hwtstamp = ns_to_ktime(*ns);
 	}
@@ -2707,8 +2717,7 @@ static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 	/* Tell the skb what kind of packet this is */
 	skb->protocol = eth_type_trans(skb, dev);
 
-	/*
-	 * There's need to check for NETIF_F_HW_VLAN_RX here.
+	/* There's need to check for NETIF_F_HW_VLAN_RX here.
 	 * Even if vlan rx accel is disabled, on some chips
 	 * RXFCB_VLN is pseudo randomly set.
 	 */
@@ -2726,8 +2735,8 @@ static int gfar_process_frame(struct net_device *dev, struct sk_buff *skb,
 }
 
 /* gfar_clean_rx_ring() -- Processes each frame in the rx ring
- *   until the budget/quota has been reached. Returns the number
- *   of frames handled
+ * until the budget/quota has been reached. Returns the number
+ * of frames handled
  */
 int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 {
@@ -2747,6 +2756,7 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 
 	while (!((bdp->status & RXBD_EMPTY) || (--rx_work_limit < 0))) {
 		struct sk_buff *newskb;
+
 		rmb();
 
 		/* Add another skb for the future */
@@ -2755,15 +2765,15 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 		skb = rx_queue->rx_skbuff[rx_queue->skb_currx];
 
 		dma_unmap_single(&priv->ofdev->dev, bdp->bufPtr,
-				priv->rx_buffer_size, DMA_FROM_DEVICE);
+				 priv->rx_buffer_size, DMA_FROM_DEVICE);
 
 		if (unlikely(!(bdp->status & RXBD_ERR) &&
-				bdp->length > priv->rx_buffer_size))
+			     bdp->length > priv->rx_buffer_size))
 			bdp->status = RXBD_LARGE;
 
 		/* We drop the frame if we failed to allocate a new buffer */
 		if (unlikely(!newskb || !(bdp->status & RXBD_LAST) ||
-				 bdp->status & RXBD_ERR)) {
+			     bdp->status & RXBD_ERR)) {
 			count_errors(bdp->status, dev);
 
 			if (unlikely(!newskb))
@@ -2782,7 +2792,7 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 				rx_queue->stats.rx_bytes += pkt_len;
 				skb_record_rx_queue(skb, rx_queue->qindex);
 				gfar_process_frame(dev, skb, amount_pull,
-						&rx_queue->grp->napi);
+						   &rx_queue->grp->napi);
 
 			} else {
 				netif_warn(priv, rx_err, dev, "Missing skb!\n");
@@ -2801,9 +2811,8 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 		bdp = next_bd(bdp, base, rx_queue->rx_ring_size);
 
 		/* update to point at the next skb */
-		rx_queue->skb_currx =
-		    (rx_queue->skb_currx + 1) &
-		    RX_RING_MOD_MASK(rx_queue->rx_ring_size);
+		rx_queue->skb_currx = (rx_queue->skb_currx + 1) &
+				      RX_RING_MOD_MASK(rx_queue->rx_ring_size);
 	}
 
 	/* Update the current rxbd pointer to be the next one */
@@ -2814,8 +2823,8 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 
 static int gfar_poll(struct napi_struct *napi, int budget)
 {
-	struct gfar_priv_grp *gfargrp = container_of(napi,
-			struct gfar_priv_grp, napi);
+	struct gfar_priv_grp *gfargrp =
+		container_of(napi, struct gfar_priv_grp, napi);
 	struct gfar_private *priv = gfargrp->priv;
 	struct gfar __iomem *regs = gfargrp->regs;
 	struct gfar_priv_tx_q *tx_queue = NULL;
@@ -2829,11 +2838,11 @@ static int gfar_poll(struct napi_struct *napi, int budget)
 	budget_per_queue = budget/num_queues;
 
 	/* Clear IEVENT, so interrupts aren't called again
-	 * because of the packets that have already arrived */
+	 * because of the packets that have already arrived
+	 */
 	gfar_write(&regs->ievent, IEVENT_RTX_MASK);
 
 	while (num_queues && left_over_budget) {
-
 		budget_per_queue = left_over_budget/num_queues;
 		left_over_budget = 0;
 
@@ -2844,12 +2853,13 @@ static int gfar_poll(struct napi_struct *napi, int budget)
 			tx_queue = priv->tx_queue[rx_queue->qindex];
 
 			tx_cleaned += gfar_clean_tx_ring(tx_queue);
-			rx_cleaned_per_queue = gfar_clean_rx_ring(rx_queue,
-							budget_per_queue);
+			rx_cleaned_per_queue =
+				gfar_clean_rx_ring(rx_queue, budget_per_queue);
 			rx_cleaned += rx_cleaned_per_queue;
-			if(rx_cleaned_per_queue < budget_per_queue) {
+			if (rx_cleaned_per_queue < budget_per_queue) {
 				left_over_budget = left_over_budget +
-					(budget_per_queue - rx_cleaned_per_queue);
+					(budget_per_queue -
+					 rx_cleaned_per_queue);
 				set_bit(i, &serviced_queues);
 				num_queues--;
 			}
@@ -2867,25 +2877,25 @@ static int gfar_poll(struct napi_struct *napi, int budget)
 
 		gfar_write(&regs->imask, IMASK_DEFAULT);
 
-		/* If we are coalescing interrupts, update the timer */
-		/* Otherwise, clear it */
-		gfar_configure_coalescing(priv,
-				gfargrp->rx_bit_map, gfargrp->tx_bit_map);
+		/* If we are coalescing interrupts, update the timer
+		 * Otherwise, clear it
+		 */
+		gfar_configure_coalescing(priv, gfargrp->rx_bit_map,
+					  gfargrp->tx_bit_map);
 	}
 
 	return rx_cleaned;
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-/*
- * Polling 'interrupt' - used by things like netconsole to send skbs
+/* Polling 'interrupt' - used by things like netconsole to send skbs
  * without having to re-enable interrupts. It's not called while
  * the interrupt routine is executing.
  */
 static void gfar_netpoll(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
-	int i = 0;
+	int i;
 
 	/* If the device has multiple interrupts, run tx/rx */
 	if (priv->device_flags & FSL_GIANFAR_DEV_HAS_MULTI_INTR) {
@@ -2894,7 +2904,7 @@ static void gfar_netpoll(struct net_device *dev)
 			disable_irq(priv->gfargrp[i].interruptReceive);
 			disable_irq(priv->gfargrp[i].interruptError);
 			gfar_interrupt(priv->gfargrp[i].interruptTransmit,
-						&priv->gfargrp[i]);
+				       &priv->gfargrp[i]);
 			enable_irq(priv->gfargrp[i].interruptError);
 			enable_irq(priv->gfargrp[i].interruptReceive);
 			enable_irq(priv->gfargrp[i].interruptTransmit);
@@ -2903,7 +2913,7 @@ static void gfar_netpoll(struct net_device *dev)
 		for (i = 0; i < priv->num_grps; i++) {
 			disable_irq(priv->gfargrp[i].interruptTransmit);
 			gfar_interrupt(priv->gfargrp[i].interruptTransmit,
-						&priv->gfargrp[i]);
+				       &priv->gfargrp[i]);
 			enable_irq(priv->gfargrp[i].interruptTransmit);
 		}
 	}
@@ -2955,7 +2965,8 @@ static void adjust_link(struct net_device *dev)
 		u32 ecntrl = gfar_read(&regs->ecntrl);
 
 		/* Now we make sure that we can be in full duplex mode.
-		 * If not, we operate in half-duplex mode. */
+		 * If not, we operate in half-duplex mode.
+		 */
 		if (phydev->duplex != priv->oldduplex) {
 			new_state = 1;
 			if (!(phydev->duplex))
@@ -2981,7 +2992,8 @@ static void adjust_link(struct net_device *dev)
 				    ((tempval & ~(MACCFG2_IF)) | MACCFG2_MII);
 
 				/* Reduced mode distinguishes
-				 * between 10 and 100 */
+				 * between 10 and 100
+				 */
 				if (phydev->speed == SPEED_100)
 					ecntrl |= ECNTRL_R100;
 				else
@@ -3020,7 +3032,8 @@ static void adjust_link(struct net_device *dev)
 /* Update the hash table based on the current list of multicast
  * addresses we subscribe to.  Also, change the promiscuity of
  * the device based on the flags (this function is called
- * whenever dev->flags is changed */
+ * whenever dev->flags is changed
+ */
 static void gfar_set_multi(struct net_device *dev)
 {
 	struct netdev_hw_addr *ha;
@@ -3082,7 +3095,8 @@ static void gfar_set_multi(struct net_device *dev)
 
 		/* If we have extended hash tables, we need to
 		 * clear the exact match registers to prepare for
-		 * setting them */
+		 * setting them
+		 */
 		if (priv->extended_hash) {
 			em_num = GFAR_EM_NUM + 1;
 			gfar_clear_exact_match(dev);
@@ -3108,13 +3122,14 @@ static void gfar_set_multi(struct net_device *dev)
 
 
 /* Clears each of the exact match registers to zero, so they
- * don't interfere with normal reception */
+ * don't interfere with normal reception
+ */
 static void gfar_clear_exact_match(struct net_device *dev)
 {
 	int idx;
 	static const u8 zero_arr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
 
-	for(idx = 1;idx < GFAR_EM_NUM + 1;idx++)
+	for (idx = 1; idx < GFAR_EM_NUM + 1; idx++)
 		gfar_set_mac_for_addr(dev, idx, zero_arr);
 }
 
@@ -3130,7 +3145,8 @@ static void gfar_clear_exact_match(struct net_device *dev)
  * hash index which gaddr register to use, and the 5 other bits
  * indicate which bit (assuming an IBM numbering scheme, which
  * for PowerPC (tm) is usually the case) in the register holds
- * the entry. */
+ * the entry.
+ */
 static void gfar_set_hash_for_addr(struct net_device *dev, u8 *addr)
 {
 	u32 tempval;
@@ -3162,8 +3178,9 @@ static void gfar_set_mac_for_addr(struct net_device *dev, int num,
 
 	macptr += num*2;
 
-	/* Now copy it into the mac registers backwards, cuz */
-	/* little endian is silly */
+	/* Now copy it into the mac registers backwards, cuz
+	 * little endian is silly
+	 */
 	for (idx = 0; idx < ETH_ALEN; idx++)
 		tmpbuf[ETH_ALEN - 1 - idx] = addr[idx];
 
@@ -3195,7 +3212,8 @@ static irqreturn_t gfar_error(int irq, void *grp_id)
 
 	/* Hmm... */
 	if (netif_msg_rx_err(priv) || netif_msg_tx_err(priv))
-		netdev_dbg(dev, "error interrupt (ievent=0x%08x imask=0x%08x)\n",
+		netdev_dbg(dev,
+			   "error interrupt (ievent=0x%08x imask=0x%08x)\n",
 			   events, gfar_read(&regs->imask));
 
 	/* Update the error counters */
diff --git a/drivers/net/ethernet/freescale/gianfar_ethtool.c b/drivers/net/ethernet/freescale/gianfar_ethtool.c
index 8a025570d97..8971921cc1c 100644
--- a/drivers/net/ethernet/freescale/gianfar_ethtool.c
+++ b/drivers/net/ethernet/freescale/gianfar_ethtool.c
@@ -46,18 +46,24 @@
 #include "gianfar.h"
 
 extern void gfar_start(struct net_device *dev);
-extern int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit);
+extern int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue,
+			      int rx_work_limit);
 
 #define GFAR_MAX_COAL_USECS 0xffff
 #define GFAR_MAX_COAL_FRAMES 0xff
 static void gfar_fill_stats(struct net_device *dev, struct ethtool_stats *dummy,
-		     u64 * buf);
+			    u64 *buf);
 static void gfar_gstrings(struct net_device *dev, u32 stringset, u8 * buf);
-static int gfar_gcoalesce(struct net_device *dev, struct ethtool_coalesce *cvals);
-static int gfar_scoalesce(struct net_device *dev, struct ethtool_coalesce *cvals);
-static void gfar_gringparam(struct net_device *dev, struct ethtool_ringparam *rvals);
-static int gfar_sringparam(struct net_device *dev, struct ethtool_ringparam *rvals);
-static void gfar_gdrvinfo(struct net_device *dev, struct ethtool_drvinfo *drvinfo);
+static int gfar_gcoalesce(struct net_device *dev,
+			  struct ethtool_coalesce *cvals);
+static int gfar_scoalesce(struct net_device *dev,
+			  struct ethtool_coalesce *cvals);
+static void gfar_gringparam(struct net_device *dev,
+			    struct ethtool_ringparam *rvals);
+static int gfar_sringparam(struct net_device *dev,
+			   struct ethtool_ringparam *rvals);
+static void gfar_gdrvinfo(struct net_device *dev,
+			  struct ethtool_drvinfo *drvinfo);
 
 static const char stat_gstrings[][ETH_GSTRING_LEN] = {
 	"rx-dropped-by-kernel",
@@ -130,14 +136,15 @@ static void gfar_gstrings(struct net_device *dev, u32 stringset, u8 * buf)
 		memcpy(buf, stat_gstrings, GFAR_STATS_LEN * ETH_GSTRING_LEN);
 	else
 		memcpy(buf, stat_gstrings,
-				GFAR_EXTRA_STATS_LEN * ETH_GSTRING_LEN);
+		       GFAR_EXTRA_STATS_LEN * ETH_GSTRING_LEN);
 }
 
 /* Fill in an array of 64-bit statistics from various sources.
  * This array will be appended to the end of the ethtool_stats
  * structure, and returned to user space
  */
-static void gfar_fill_stats(struct net_device *dev, struct ethtool_stats *dummy, u64 * buf)
+static void gfar_fill_stats(struct net_device *dev, struct ethtool_stats *dummy,
+			    u64 *buf)
 {
 	int i;
 	struct gfar_private *priv = netdev_priv(dev);
@@ -174,8 +181,8 @@ static int gfar_sset_count(struct net_device *dev, int sset)
 }
 
 /* Fills in the drvinfo structure with some basic info */
-static void gfar_gdrvinfo(struct net_device *dev, struct
-	      ethtool_drvinfo *drvinfo)
+static void gfar_gdrvinfo(struct net_device *dev,
+			  struct ethtool_drvinfo *drvinfo)
 {
 	strncpy(drvinfo->driver, DRV_NAME, GFAR_INFOSTR_LEN);
 	strncpy(drvinfo->version, gfar_driver_version, GFAR_INFOSTR_LEN);
@@ -226,7 +233,8 @@ static int gfar_reglen(struct net_device *dev)
 }
 
 /* Return a dump of the GFAR register space */
-static void gfar_get_regs(struct net_device *dev, struct ethtool_regs *regs, void *regbuf)
+static void gfar_get_regs(struct net_device *dev, struct ethtool_regs *regs,
+			  void *regbuf)
 {
 	int i;
 	struct gfar_private *priv = netdev_priv(dev);
@@ -239,7 +247,8 @@ static void gfar_get_regs(struct net_device *dev, struct ethtool_regs *regs, voi
 
 /* Convert microseconds to ethernet clock ticks, which changes
  * depending on what speed the controller is running at */
-static unsigned int gfar_usecs2ticks(struct gfar_private *priv, unsigned int usecs)
+static unsigned int gfar_usecs2ticks(struct gfar_private *priv,
+				     unsigned int usecs)
 {
 	unsigned int count;
 
@@ -263,7 +272,8 @@ static unsigned int gfar_usecs2ticks(struct gfar_private *priv, unsigned int use
 }
 
 /* Convert ethernet clock ticks to microseconds */
-static unsigned int gfar_ticks2usecs(struct gfar_private *priv, unsigned int ticks)
+static unsigned int gfar_ticks2usecs(struct gfar_private *priv,
+				     unsigned int ticks)
 {
 	unsigned int count;
 
@@ -288,7 +298,8 @@ static unsigned int gfar_ticks2usecs(struct gfar_private *priv, unsigned int tic
 
 /* Get the coalescing parameters, and put them in the cvals
  * structure.  */
-static int gfar_gcoalesce(struct net_device *dev, struct ethtool_coalesce *cvals)
+static int gfar_gcoalesce(struct net_device *dev,
+			  struct ethtool_coalesce *cvals)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct gfar_priv_rx_q *rx_queue = NULL;
@@ -353,7 +364,8 @@ static int gfar_gcoalesce(struct net_device *dev, struct ethtool_coalesce *cvals
  * Both cvals->*_usecs and cvals->*_frames have to be > 0
  * in order for coalescing to be active
  */
-static int gfar_scoalesce(struct net_device *dev, struct ethtool_coalesce *cvals)
+static int gfar_scoalesce(struct net_device *dev,
+			  struct ethtool_coalesce *cvals)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	int i = 0;
@@ -364,7 +376,8 @@ static int gfar_scoalesce(struct net_device *dev, struct ethtool_coalesce *cvals
 	/* Set up rx coalescing */
 	/* As of now, we will enable/disable coalescing for all
 	 * queues together in case of eTSEC2, this will be modified
-	 * along with the ethtool interface */
+	 * along with the ethtool interface
+	 */
 	if ((cvals->rx_coalesce_usecs == 0) ||
 	    (cvals->rx_max_coalesced_frames == 0)) {
 		for (i = 0; i < priv->num_rx_queues; i++)
@@ -433,7 +446,8 @@ static int gfar_scoalesce(struct net_device *dev, struct ethtool_coalesce *cvals
 /* Fills in rvals with the current ring parameters.  Currently,
  * rx, rx_mini, and rx_jumbo rings are the same size, as mini and
  * jumbo are ignored by the driver */
-static void gfar_gringparam(struct net_device *dev, struct ethtool_ringparam *rvals)
+static void gfar_gringparam(struct net_device *dev,
+			    struct ethtool_ringparam *rvals)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct gfar_priv_tx_q *tx_queue = NULL;
@@ -459,8 +473,10 @@ static void gfar_gringparam(struct net_device *dev, struct ethtool_ringparam *rv
 /* Change the current ring parameters, stopping the controller if
  * necessary so that we don't mess things up while we're in
  * motion.  We wait for the ring to be clean before reallocating
- * the rings. */
-static int gfar_sringparam(struct net_device *dev, struct ethtool_ringparam *rvals)
+ * the rings.
+ */
+static int gfar_sringparam(struct net_device *dev,
+			   struct ethtool_ringparam *rvals)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	int err = 0, i = 0;
@@ -486,7 +502,8 @@ static int gfar_sringparam(struct net_device *dev, struct ethtool_ringparam *rva
 		unsigned long flags;
 
 		/* Halt TX and RX, and process the frames which
-		 * have already been received */
+		 * have already been received
+		 */
 		local_irq_save(flags);
 		lock_tx_qs(priv);
 		lock_rx_qs(priv);
@@ -499,7 +516,7 @@ static int gfar_sringparam(struct net_device *dev, struct ethtool_ringparam *rva
 
 		for (i = 0; i < priv->num_rx_queues; i++)
 			gfar_clean_rx_ring(priv->rx_queue[i],
-					priv->rx_queue[i]->rx_ring_size);
+					   priv->rx_queue[i]->rx_ring_size);
 
 		/* Now we take down the rings to rebuild them */
 		stop_gfar(dev);
@@ -509,7 +526,8 @@ static int gfar_sringparam(struct net_device *dev, struct ethtool_ringparam *rva
 	for (i = 0; i < priv->num_rx_queues; i++) {
 		priv->rx_queue[i]->rx_ring_size = rvals->rx_pending;
 		priv->tx_queue[i]->tx_ring_size = rvals->tx_pending;
-		priv->tx_queue[i]->num_txbdfree = priv->tx_queue[i]->tx_ring_size;
+		priv->tx_queue[i]->num_txbdfree =
+			priv->tx_queue[i]->tx_ring_size;
 	}
 
 	/* Rebuild the rings with the new size */
@@ -535,7 +553,8 @@ int gfar_set_features(struct net_device *dev, netdev_features_t features)
 
 	if (dev->flags & IFF_UP) {
 		/* Halt TX and RX, and process the frames which
-		 * have already been received */
+		 * have already been received
+		 */
 		local_irq_save(flags);
 		lock_tx_qs(priv);
 		lock_rx_qs(priv);
@@ -548,7 +567,7 @@ int gfar_set_features(struct net_device *dev, netdev_features_t features)
 
 		for (i = 0; i < priv->num_rx_queues; i++)
 			gfar_clean_rx_ring(priv->rx_queue[i],
-					priv->rx_queue[i]->rx_ring_size);
+					   priv->rx_queue[i]->rx_ring_size);
 
 		/* Now we take down the rings to rebuild them */
 		stop_gfar(dev);
@@ -564,12 +583,14 @@ int gfar_set_features(struct net_device *dev, netdev_features_t features)
 static uint32_t gfar_get_msglevel(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
+
 	return priv->msg_enable;
 }
 
 static void gfar_set_msglevel(struct net_device *dev, uint32_t data)
 {
 	struct gfar_private *priv = netdev_priv(dev);
+
 	priv->msg_enable = data;
 }
 
@@ -614,14 +635,14 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & RXH_L2DA) {
 		fcr = RQFCR_PID_DAH |RQFCR_CMP_NOMATCH |
-			RQFCR_HASH | RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_HASH | RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
 		priv->cur_filer_idx = priv->cur_filer_idx - 1;
 
 		fcr = RQFCR_PID_DAL | RQFCR_AND | RQFCR_CMP_NOMATCH |
-				RQFCR_HASH | RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_HASH | RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
@@ -630,7 +651,7 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & RXH_VLAN) {
 		fcr = RQFCR_PID_VID | RQFCR_CMP_NOMATCH | RQFCR_HASH |
-				RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_AND | RQFCR_HASHTBL_0;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
@@ -639,7 +660,7 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & RXH_IP_SRC) {
 		fcr = RQFCR_PID_SIA | RQFCR_CMP_NOMATCH | RQFCR_HASH |
-			RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
@@ -648,7 +669,7 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & (RXH_IP_DST)) {
 		fcr = RQFCR_PID_DIA | RQFCR_CMP_NOMATCH | RQFCR_HASH |
-			RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
@@ -657,7 +678,7 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & RXH_L3_PROTO) {
 		fcr = RQFCR_PID_L4P | RQFCR_CMP_NOMATCH | RQFCR_HASH |
-			RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
@@ -666,7 +687,7 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & RXH_L4_B_0_1) {
 		fcr = RQFCR_PID_SPT | RQFCR_CMP_NOMATCH | RQFCR_HASH |
-			RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
@@ -675,7 +696,7 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 
 	if (ethflow & RXH_L4_B_2_3) {
 		fcr = RQFCR_PID_DPT | RQFCR_CMP_NOMATCH | RQFCR_HASH |
-			RQFCR_AND | RQFCR_HASHTBL_0;
+		      RQFCR_AND | RQFCR_HASHTBL_0;
 		priv->ftp_rqfpr[priv->cur_filer_idx] = fpr;
 		priv->ftp_rqfcr[priv->cur_filer_idx] = fcr;
 		gfar_write_filer(priv, priv->cur_filer_idx, fcr, fpr);
@@ -683,7 +704,8 @@ static void ethflow_to_filer_rules (struct gfar_private *priv, u64 ethflow)
 	}
 }
 
-static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow, u64 class)
+static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow,
+				       u64 class)
 {
 	unsigned int last_rule_idx = priv->cur_filer_idx;
 	unsigned int cmp_rqfpr;
@@ -694,9 +716,9 @@ static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow, u
 	int ret = 1;
 
 	local_rqfpr = kmalloc(sizeof(unsigned int) * (MAX_FILER_IDX + 1),
-		GFP_KERNEL);
+			      GFP_KERNEL);
 	local_rqfcr = kmalloc(sizeof(unsigned int) * (MAX_FILER_IDX + 1),
-		GFP_KERNEL);
+			      GFP_KERNEL);
 	if (!local_rqfpr || !local_rqfcr) {
 		pr_err("Out of memory\n");
 		ret = 0;
@@ -726,9 +748,9 @@ static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow, u
 		local_rqfpr[j] = priv->ftp_rqfpr[i];
 		local_rqfcr[j] = priv->ftp_rqfcr[i];
 		j--;
-		if ((priv->ftp_rqfcr[i] == (RQFCR_PID_PARSE |
-			RQFCR_CLE |RQFCR_AND)) &&
-			(priv->ftp_rqfpr[i] == cmp_rqfpr))
+		if ((priv->ftp_rqfcr[i] ==
+		     (RQFCR_PID_PARSE | RQFCR_CLE | RQFCR_AND)) &&
+		    (priv->ftp_rqfpr[i] == cmp_rqfpr))
 			break;
 	}
 
@@ -743,12 +765,12 @@ static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow, u
 	 */
 	for (l = i+1; l < MAX_FILER_IDX; l++) {
 		if ((priv->ftp_rqfcr[l] & RQFCR_CLE) &&
-			!(priv->ftp_rqfcr[l] & RQFCR_AND)) {
+		    !(priv->ftp_rqfcr[l] & RQFCR_AND)) {
 			priv->ftp_rqfcr[l] = RQFCR_CLE | RQFCR_CMP_EXACT |
-				RQFCR_HASHTBL_0 | RQFCR_PID_MASK;
+					     RQFCR_HASHTBL_0 | RQFCR_PID_MASK;
 			priv->ftp_rqfpr[l] = FPR_FILER_MASK;
 			gfar_write_filer(priv, l, priv->ftp_rqfcr[l],
-				priv->ftp_rqfpr[l]);
+					 priv->ftp_rqfpr[l]);
 			break;
 		}
 
@@ -773,7 +795,7 @@ static int gfar_ethflow_to_filer_table(struct gfar_private *priv, u64 ethflow, u
 		priv->ftp_rqfpr[priv->cur_filer_idx] = local_rqfpr[k];
 		priv->ftp_rqfcr[priv->cur_filer_idx] = local_rqfcr[k];
 		gfar_write_filer(priv, priv->cur_filer_idx,
-				local_rqfcr[k], local_rqfpr[k]);
+				 local_rqfcr[k], local_rqfpr[k]);
 		if (!priv->cur_filer_idx)
 			break;
 		priv->cur_filer_idx = priv->cur_filer_idx - 1;
@@ -785,7 +807,8 @@ err:
 	return ret;
 }
 
-static int gfar_set_hash_opts(struct gfar_private *priv, struct ethtool_rxnfc *cmd)
+static int gfar_set_hash_opts(struct gfar_private *priv,
+			      struct ethtool_rxnfc *cmd)
 {
 	/* write the filer rules here */
 	if (!gfar_ethflow_to_filer_table(priv, cmd->data, cmd->flow_type))
@@ -810,10 +833,10 @@ static int gfar_check_filer_hardware(struct gfar_private *priv)
 		i &= RCTRL_PRSDEP_MASK | RCTRL_PRSFM;
 		if (i == (RCTRL_PRSDEP_MASK | RCTRL_PRSFM)) {
 			netdev_info(priv->ndev,
-					"Receive Queue Filtering enabled\n");
+				    "Receive Queue Filtering enabled\n");
 		} else {
 			netdev_warn(priv->ndev,
-					"Receive Queue Filtering disabled\n");
+				    "Receive Queue Filtering disabled\n");
 			return -EOPNOTSUPP;
 		}
 	}
@@ -823,16 +846,17 @@ static int gfar_check_filer_hardware(struct gfar_private *priv)
 		i &= RCTRL_PRSDEP_MASK;
 		if (i == RCTRL_PRSDEP_MASK) {
 			netdev_info(priv->ndev,
-					"Receive Queue Filtering enabled\n");
+				    "Receive Queue Filtering enabled\n");
 		} else {
 			netdev_warn(priv->ndev,
-					"Receive Queue Filtering disabled\n");
+				    "Receive Queue Filtering disabled\n");
 			return -EOPNOTSUPP;
 		}
 	}
 
 	/* Sets the properties for arbitrary filer rule
-	 * to the first 4 Layer 4 Bytes */
+	 * to the first 4 Layer 4 Bytes
+	 */
 	regs->rbifx = 0xC0C1C2C3;
 	return 0;
 }
@@ -870,14 +894,14 @@ static void gfar_set_mask(u32 mask, struct filer_table *tab)
 static void gfar_set_parse_bits(u32 value, u32 mask, struct filer_table *tab)
 {
 	gfar_set_mask(mask, tab);
-	tab->fe[tab->index].ctrl = RQFCR_CMP_EXACT | RQFCR_PID_PARSE
-			| RQFCR_AND;
+	tab->fe[tab->index].ctrl = RQFCR_CMP_EXACT | RQFCR_PID_PARSE |
+				   RQFCR_AND;
 	tab->fe[tab->index].prop = value;
 	tab->index++;
 }
 
 static void gfar_set_general_attribute(u32 value, u32 mask, u32 flag,
-		struct filer_table *tab)
+				       struct filer_table *tab)
 {
 	gfar_set_mask(mask, tab);
 	tab->fe[tab->index].ctrl = RQFCR_CMP_EXACT | RQFCR_AND | flag;
@@ -885,8 +909,7 @@ static void gfar_set_general_attribute(u32 value, u32 mask, u32 flag,
 	tab->index++;
 }
 
-/*
- * For setting a tuple of value and mask of type flag
+/* For setting a tuple of value and mask of type flag
  * Example:
  * IP-Src = 10.0.0.0/255.0.0.0
  * value: 0x0A000000 mask: FF000000 flag: RQFPR_IPV4
@@ -901,7 +924,7 @@ static void gfar_set_general_attribute(u32 value, u32 mask, u32 flag,
  * Further the all masks are one-padded for better hardware efficiency.
  */
 static void gfar_set_attribute(u32 value, u32 mask, u32 flag,
-		struct filer_table *tab)
+			       struct filer_table *tab)
 {
 	switch (flag) {
 		/* 3bit */
@@ -959,7 +982,8 @@ static void gfar_set_attribute(u32 value, u32 mask, u32 flag,
 
 /* Translates value and mask for UDP, TCP or SCTP */
 static void gfar_set_basic_ip(struct ethtool_tcpip4_spec *value,
-		struct ethtool_tcpip4_spec *mask, struct filer_table *tab)
+			      struct ethtool_tcpip4_spec *mask,
+			      struct filer_table *tab)
 {
 	gfar_set_attribute(value->ip4src, mask->ip4src, RQFCR_PID_SIA, tab);
 	gfar_set_attribute(value->ip4dst, mask->ip4dst, RQFCR_PID_DIA, tab);
@@ -970,97 +994,92 @@ static void gfar_set_basic_ip(struct ethtool_tcpip4_spec *value,
 
 /* Translates value and mask for RAW-IP4 */
 static void gfar_set_user_ip(struct ethtool_usrip4_spec *value,
-		struct ethtool_usrip4_spec *mask, struct filer_table *tab)
+			     struct ethtool_usrip4_spec *mask,
+			     struct filer_table *tab)
 {
 	gfar_set_attribute(value->ip4src, mask->ip4src, RQFCR_PID_SIA, tab);
 	gfar_set_attribute(value->ip4dst, mask->ip4dst, RQFCR_PID_DIA, tab);
 	gfar_set_attribute(value->tos, mask->tos, RQFCR_PID_TOS, tab);
 	gfar_set_attribute(value->proto, mask->proto, RQFCR_PID_L4P, tab);
 	gfar_set_attribute(value->l4_4_bytes, mask->l4_4_bytes, RQFCR_PID_ARB,
-			tab);
+			   tab);
 
 }
 
 /* Translates value and mask for ETHER spec */
 static void gfar_set_ether(struct ethhdr *value, struct ethhdr *mask,
-		struct filer_table *tab)
+			   struct filer_table *tab)
 {
 	u32 upper_temp_mask = 0;
 	u32 lower_temp_mask = 0;
+
 	/* Source address */
 	if (!is_broadcast_ether_addr(mask->h_source)) {
-
 		if (is_zero_ether_addr(mask->h_source)) {
 			upper_temp_mask = 0xFFFFFFFF;
 			lower_temp_mask = 0xFFFFFFFF;
 		} else {
-			upper_temp_mask = mask->h_source[0] << 16
-					| mask->h_source[1] << 8
-					| mask->h_source[2];
-			lower_temp_mask = mask->h_source[3] << 16
-					| mask->h_source[4] << 8
-					| mask->h_source[5];
+			upper_temp_mask = mask->h_source[0] << 16 |
+					  mask->h_source[1] << 8  |
+					  mask->h_source[2];
+			lower_temp_mask = mask->h_source[3] << 16 |
+					  mask->h_source[4] << 8  |
+					  mask->h_source[5];
 		}
 		/* Upper 24bit */
-		gfar_set_attribute(
-				value->h_source[0] << 16 | value->h_source[1]
-						<< 8 | value->h_source[2],
-				upper_temp_mask, RQFCR_PID_SAH, tab);
+		gfar_set_attribute(value->h_source[0] << 16 |
+				   value->h_source[1] << 8  |
+				   value->h_source[2],
+				   upper_temp_mask, RQFCR_PID_SAH, tab);
 		/* And the same for the lower part */
-		gfar_set_attribute(
-				value->h_source[3] << 16 | value->h_source[4]
-						<< 8 | value->h_source[5],
-				lower_temp_mask, RQFCR_PID_SAL, tab);
+		gfar_set_attribute(value->h_source[3] << 16 |
+				   value->h_source[4] << 8  |
+				   value->h_source[5],
+				   lower_temp_mask, RQFCR_PID_SAL, tab);
 	}
 	/* Destination address */
 	if (!is_broadcast_ether_addr(mask->h_dest)) {
-
 		/* Special for destination is limited broadcast */
-		if ((is_broadcast_ether_addr(value->h_dest)
-				&& is_zero_ether_addr(mask->h_dest))) {
+		if ((is_broadcast_ether_addr(value->h_dest) &&
+		    is_zero_ether_addr(mask->h_dest))) {
 			gfar_set_parse_bits(RQFPR_EBC, RQFPR_EBC, tab);
 		} else {
-
 			if (is_zero_ether_addr(mask->h_dest)) {
 				upper_temp_mask = 0xFFFFFFFF;
 				lower_temp_mask = 0xFFFFFFFF;
 			} else {
-				upper_temp_mask = mask->h_dest[0] << 16
-						| mask->h_dest[1] << 8
-						| mask->h_dest[2];
-				lower_temp_mask = mask->h_dest[3] << 16
-						| mask->h_dest[4] << 8
-						| mask->h_dest[5];
+				upper_temp_mask = mask->h_dest[0] << 16 |
+						  mask->h_dest[1] << 8  |
+						  mask->h_dest[2];
+				lower_temp_mask = mask->h_dest[3] << 16 |
+						  mask->h_dest[4] << 8  |
+						  mask->h_dest[5];
 			}
 
 			/* Upper 24bit */
-			gfar_set_attribute(
-					value->h_dest[0] << 16
-							| value->h_dest[1] << 8
-							| value->h_dest[2],
-					upper_temp_mask, RQFCR_PID_DAH, tab);
+			gfar_set_attribute(value->h_dest[0] << 16 |
+					   value->h_dest[1] << 8  |
+					   value->h_dest[2],
+					   upper_temp_mask, RQFCR_PID_DAH, tab);
 			/* And the same for the lower part */
-			gfar_set_attribute(
-					value->h_dest[3] << 16
-							| value->h_dest[4] << 8
-							| value->h_dest[5],
-					lower_temp_mask, RQFCR_PID_DAL, tab);
+			gfar_set_attribute(value->h_dest[3] << 16 |
+					   value->h_dest[4] << 8  |
+					   value->h_dest[5],
+					   lower_temp_mask, RQFCR_PID_DAL, tab);
 		}
 	}
 
 	gfar_set_attribute(value->h_proto, mask->h_proto, RQFCR_PID_ETY, tab);
-
 }
 
 /* Convert a rule to binary filter format of gianfar */
 static int gfar_convert_to_filer(struct ethtool_rx_flow_spec *rule,
-		struct filer_table *tab)
+				 struct filer_table *tab)
 {
 	u32 vlan = 0, vlan_mask = 0;
 	u32 id = 0, id_mask = 0;
 	u32 cfi = 0, cfi_mask = 0;
 	u32 prio = 0, prio_mask = 0;
-
 	u32 old_index = tab->index;
 
 	/* Check if vlan is wanted */
@@ -1076,13 +1095,16 @@ static int gfar_convert_to_filer(struct ethtool_rx_flow_spec *rule,
 		id_mask = rule->m_ext.vlan_tci & VLAN_VID_MASK;
 		cfi = rule->h_ext.vlan_tci & VLAN_CFI_MASK;
 		cfi_mask = rule->m_ext.vlan_tci & VLAN_CFI_MASK;
-		prio = (rule->h_ext.vlan_tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
-		prio_mask = (rule->m_ext.vlan_tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
+		prio = (rule->h_ext.vlan_tci & VLAN_PRIO_MASK) >>
+		       VLAN_PRIO_SHIFT;
+		prio_mask = (rule->m_ext.vlan_tci & VLAN_PRIO_MASK) >>
+			    VLAN_PRIO_SHIFT;
 
 		if (cfi == VLAN_TAG_PRESENT && cfi_mask == VLAN_TAG_PRESENT) {
 			vlan |= RQFPR_CFI;
 			vlan_mask |= RQFPR_CFI;
-		} else if (cfi != VLAN_TAG_PRESENT && cfi_mask == VLAN_TAG_PRESENT) {
+		} else if (cfi != VLAN_TAG_PRESENT &&
+			   cfi_mask == VLAN_TAG_PRESENT) {
 			vlan_mask |= RQFPR_CFI;
 		}
 	}
@@ -1090,34 +1112,36 @@ static int gfar_convert_to_filer(struct ethtool_rx_flow_spec *rule,
 	switch (rule->flow_type & ~FLOW_EXT) {
 	case TCP_V4_FLOW:
 		gfar_set_parse_bits(RQFPR_IPV4 | RQFPR_TCP | vlan,
-				RQFPR_IPV4 | RQFPR_TCP | vlan_mask, tab);
+				    RQFPR_IPV4 | RQFPR_TCP | vlan_mask, tab);
 		gfar_set_basic_ip(&rule->h_u.tcp_ip4_spec,
-				&rule->m_u.tcp_ip4_spec, tab);
+				  &rule->m_u.tcp_ip4_spec, tab);
 		break;
 	case UDP_V4_FLOW:
 		gfar_set_parse_bits(RQFPR_IPV4 | RQFPR_UDP | vlan,
-				RQFPR_IPV4 | RQFPR_UDP | vlan_mask, tab);
+				    RQFPR_IPV4 | RQFPR_UDP | vlan_mask, tab);
 		gfar_set_basic_ip(&rule->h_u.udp_ip4_spec,
-				&rule->m_u.udp_ip4_spec, tab);
+				  &rule->m_u.udp_ip4_spec, tab);
 		break;
 	case SCTP_V4_FLOW:
 		gfar_set_parse_bits(RQFPR_IPV4 | vlan, RQFPR_IPV4 | vlan_mask,
-				tab);
+				    tab);
 		gfar_set_attribute(132, 0, RQFCR_PID_L4P, tab);
-		gfar_set_basic_ip((struct ethtool_tcpip4_spec *) &rule->h_u,
-				(struct ethtool_tcpip4_spec *) &rule->m_u, tab);
+		gfar_set_basic_ip((struct ethtool_tcpip4_spec *)&rule->h_u,
+				  (struct ethtool_tcpip4_spec *)&rule->m_u,
+				  tab);
 		break;
 	case IP_USER_FLOW:
 		gfar_set_parse_bits(RQFPR_IPV4 | vlan, RQFPR_IPV4 | vlan_mask,
-				tab);
+				    tab);
 		gfar_set_user_ip((struct ethtool_usrip4_spec *) &rule->h_u,
-				(struct ethtool_usrip4_spec *) &rule->m_u, tab);
+				 (struct ethtool_usrip4_spec *) &rule->m_u,
+				 tab);
 		break;
 	case ETHER_FLOW:
 		if (vlan)
 			gfar_set_parse_bits(vlan, vlan_mask, tab);
 		gfar_set_ether((struct ethhdr *) &rule->h_u,
-				(struct ethhdr *) &rule->m_u, tab);
+			       (struct ethhdr *) &rule->m_u, tab);
 		break;
 	default:
 		return -1;
@@ -1152,7 +1176,9 @@ static int gfar_convert_to_filer(struct ethtool_rx_flow_spec *rule,
 		tab->fe[tab->index - 1].ctrl |= RQFCR_CLE;
 	}
 
-	/* In rare cases the cache can be full while there is free space in hw */
+	/* In rare cases the cache can be full while there is
+	 * free space in hw
+	 */
 	if (tab->index > MAX_FILER_CACHE_IDX - 1)
 		return -EBUSY;
 
@@ -1161,7 +1187,7 @@ static int gfar_convert_to_filer(struct ethtool_rx_flow_spec *rule,
 
 /* Copy size filer entries */
 static void gfar_copy_filer_entries(struct gfar_filer_entry dst[0],
-		struct gfar_filer_entry src[0], s32 size)
+				    struct gfar_filer_entry src[0], s32 size)
 {
 	while (size > 0) {
 		size--;
@@ -1171,10 +1197,12 @@ static void gfar_copy_filer_entries(struct gfar_filer_entry dst[0],
 }
 
 /* Delete the contents of the filer-table between start and end
- * and collapse them */
+ * and collapse them
+ */
 static int gfar_trim_filer_entries(u32 begin, u32 end, struct filer_table *tab)
 {
 	int length;
+
 	if (end > MAX_FILER_CACHE_IDX || end < begin)
 		return -EINVAL;
 
@@ -1200,14 +1228,14 @@ static int gfar_trim_filer_entries(u32 begin, u32 end, struct filer_table *tab)
 
 /* Make space on the wanted location */
 static int gfar_expand_filer_entries(u32 begin, u32 length,
-		struct filer_table *tab)
+				     struct filer_table *tab)
 {
-	if (length == 0 || length + tab->index > MAX_FILER_CACHE_IDX || begin
-			> MAX_FILER_CACHE_IDX)
+	if (length == 0 || length + tab->index > MAX_FILER_CACHE_IDX ||
+	    begin > MAX_FILER_CACHE_IDX)
 		return -EINVAL;
 
 	gfar_copy_filer_entries(&(tab->fe[begin + length]), &(tab->fe[begin]),
-			tab->index - length + 1);
+				tab->index - length + 1);
 
 	tab->index += length;
 	return 0;
@@ -1215,9 +1243,10 @@ static int gfar_expand_filer_entries(u32 begin, u32 length,
 
 static int gfar_get_next_cluster_start(int start, struct filer_table *tab)
 {
-	for (; (start < tab->index) && (start < MAX_FILER_CACHE_IDX - 1); start++) {
-		if ((tab->fe[start].ctrl & (RQFCR_AND | RQFCR_CLE))
-				== (RQFCR_AND | RQFCR_CLE))
+	for (; (start < tab->index) && (start < MAX_FILER_CACHE_IDX - 1);
+	     start++) {
+		if ((tab->fe[start].ctrl & (RQFCR_AND | RQFCR_CLE)) ==
+		    (RQFCR_AND | RQFCR_CLE))
 			return start;
 	}
 	return -1;
@@ -1225,16 +1254,16 @@ static int gfar_get_next_cluster_start(int start, struct filer_table *tab)
 
 static int gfar_get_next_cluster_end(int start, struct filer_table *tab)
 {
-	for (; (start < tab->index) && (start < MAX_FILER_CACHE_IDX - 1); start++) {
-		if ((tab->fe[start].ctrl & (RQFCR_AND | RQFCR_CLE))
-				== (RQFCR_CLE))
+	for (; (start < tab->index) && (start < MAX_FILER_CACHE_IDX - 1);
+	     start++) {
+		if ((tab->fe[start].ctrl & (RQFCR_AND | RQFCR_CLE)) ==
+		    (RQFCR_CLE))
 			return start;
 	}
 	return -1;
 }
 
-/*
- * Uses hardwares clustering option to reduce
+/* Uses hardwares clustering option to reduce
  * the number of filer table entries
  */
 static void gfar_cluster_filer(struct filer_table *tab)
@@ -1244,8 +1273,7 @@ static void gfar_cluster_filer(struct filer_table *tab)
 	while ((i = gfar_get_next_cluster_start(++i, tab)) != -1) {
 		j = i;
 		while ((j = gfar_get_next_cluster_start(++j, tab)) != -1) {
-			/*
-			 * The cluster entries self and the previous one
+			/* The cluster entries self and the previous one
 			 * (a mask) must be identical!
 			 */
 			if (tab->fe[i].ctrl != tab->fe[j].ctrl)
@@ -1260,21 +1288,21 @@ static void gfar_cluster_filer(struct filer_table *tab)
 			jend = gfar_get_next_cluster_end(j, tab);
 			if (jend == -1 || iend == -1)
 				break;
-			/*
-			 * First we make some free space, where our cluster
+
+			/* First we make some free space, where our cluster
 			 * element should be. Then we copy it there and finally
 			 * delete in from its old location.
 			 */
-
-			if (gfar_expand_filer_entries(iend, (jend - j), tab)
-					== -EINVAL)
+			if (gfar_expand_filer_entries(iend, (jend - j), tab) ==
+			    -EINVAL)
 				break;
 
 			gfar_copy_filer_entries(&(tab->fe[iend + 1]),
-					&(tab->fe[jend + 1]), jend - j);
+						&(tab->fe[jend + 1]), jend - j);
 
 			if (gfar_trim_filer_entries(jend - 1,
-					jend + (jend - j), tab) == -EINVAL)
+						    jend + (jend - j),
+						    tab) == -EINVAL)
 				return;
 
 			/* Mask out cluster bit */
@@ -1285,8 +1313,9 @@ static void gfar_cluster_filer(struct filer_table *tab)
 
 /* Swaps the masked bits of a1<>a2 and b1<>b2 */
 static void gfar_swap_bits(struct gfar_filer_entry *a1,
-		struct gfar_filer_entry *a2, struct gfar_filer_entry *b1,
-		struct gfar_filer_entry *b2, u32 mask)
+			   struct gfar_filer_entry *a2,
+			   struct gfar_filer_entry *b1,
+			   struct gfar_filer_entry *b2, u32 mask)
 {
 	u32 temp[4];
 	temp[0] = a1->ctrl & mask;
@@ -1305,13 +1334,12 @@ static void gfar_swap_bits(struct gfar_filer_entry *a1,
 	b2->ctrl |= temp[2];
 }
 
-/*
- * Generate a list consisting of masks values with their start and
+/* Generate a list consisting of masks values with their start and
  * end of validity and block as indicator for parts belonging
  * together (glued by ANDs) in mask_table
  */
 static u32 gfar_generate_mask_table(struct gfar_mask_entry *mask_table,
-		struct filer_table *tab)
+				    struct filer_table *tab)
 {
 	u32 i, and_index = 0, block_index = 1;
 
@@ -1327,13 +1355,13 @@ static u32 gfar_generate_mask_table(struct gfar_mask_entry *mask_table,
 			and_index++;
 		}
 		/* cluster starts and ends will be separated because they should
-		 * hold their position */
+		 * hold their position
+		 */
 		if (tab->fe[i].ctrl & RQFCR_CLE)
 			block_index++;
 		/* A not set AND indicates the end of a depended block */
 		if (!(tab->fe[i].ctrl & RQFCR_AND))
 			block_index++;
-
 	}
 
 	mask_table[and_index - 1].end = i - 1;
@@ -1341,14 +1369,13 @@ static u32 gfar_generate_mask_table(struct gfar_mask_entry *mask_table,
 	return and_index;
 }
 
-/*
- * Sorts the entries of mask_table by the values of the masks.
+/* Sorts the entries of mask_table by the values of the masks.
  * Important: The 0xFF80 flags of the first and last entry of a
  * block must hold their position (which queue, CLusterEnable, ReJEct,
  * AND)
  */
 static void gfar_sort_mask_table(struct gfar_mask_entry *mask_table,
-		struct filer_table *temp_table, u32 and_index)
+				 struct filer_table *temp_table, u32 and_index)
 {
 	/* Pointer to compare function (_asc or _desc) */
 	int (*gfar_comp)(const void *, const void *);
@@ -1359,16 +1386,16 @@ static void gfar_sort_mask_table(struct gfar_mask_entry *mask_table,
 	gfar_comp = &gfar_comp_desc;
 
 	for (i = 0; i < and_index; i++) {
-
 		if (prev != mask_table[i].block) {
 			old_first = mask_table[start].start + 1;
 			old_last = mask_table[i - 1].end;
 			sort(mask_table + start, size,
-					sizeof(struct gfar_mask_entry),
-					gfar_comp, &gfar_swap);
+			     sizeof(struct gfar_mask_entry),
+			     gfar_comp, &gfar_swap);
 
 			/* Toggle order for every block. This makes the
-			 * thing more efficient! */
+			 * thing more efficient!
+			 */
 			if (gfar_comp == gfar_comp_desc)
 				gfar_comp = &gfar_comp_asc;
 			else
@@ -1378,12 +1405,11 @@ static void gfar_sort_mask_table(struct gfar_mask_entry *mask_table,
 			new_last = mask_table[i - 1].end;
 
 			gfar_swap_bits(&temp_table->fe[new_first],
-					&temp_table->fe[old_first],
-					&temp_table->fe[new_last],
-					&temp_table->fe[old_last],
-					RQFCR_QUEUE | RQFCR_CLE |
-						RQFCR_RJE | RQFCR_AND
-					);
+				       &temp_table->fe[old_first],
+				       &temp_table->fe[new_last],
+				       &temp_table->fe[old_last],
+				       RQFCR_QUEUE | RQFCR_CLE |
+				       RQFCR_RJE | RQFCR_AND);
 
 			start = i;
 			size = 0;
@@ -1391,11 +1417,9 @@ static void gfar_sort_mask_table(struct gfar_mask_entry *mask_table,
 		size++;
 		prev = mask_table[i].block;
 	}
-
 }
 
-/*
- * Reduces the number of masks needed in the filer table to save entries
+/* Reduces the number of masks needed in the filer table to save entries
  * This is done by sorting the masks of a depended block. A depended block is
  * identified by gluing ANDs or CLE. The sorting order toggles after every
  * block. Of course entries in scope of a mask must change their location with
@@ -1410,13 +1434,14 @@ static int gfar_optimize_filer_masks(struct filer_table *tab)
 	s32 ret = 0;
 
 	/* We need a copy of the filer table because
-	 * we want to change its order */
+	 * we want to change its order
+	 */
 	temp_table = kmemdup(tab, sizeof(*temp_table), GFP_KERNEL);
 	if (temp_table == NULL)
 		return -ENOMEM;
 
 	mask_table = kcalloc(MAX_FILER_CACHE_IDX / 2 + 1,
-			sizeof(struct gfar_mask_entry), GFP_KERNEL);
+			     sizeof(struct gfar_mask_entry), GFP_KERNEL);
 
 	if (mask_table == NULL) {
 		ret = -ENOMEM;
@@ -1428,7 +1453,8 @@ static int gfar_optimize_filer_masks(struct filer_table *tab)
 	gfar_sort_mask_table(mask_table, temp_table, and_index);
 
 	/* Now we can copy the data from our duplicated filer table to
-	 * the real one in the order the mask table says */
+	 * the real one in the order the mask table says
+	 */
 	for (i = 0; i < and_index; i++) {
 		size = mask_table[i].end - mask_table[i].start + 1;
 		gfar_copy_filer_entries(&(tab->fe[j]),
@@ -1437,7 +1463,8 @@ static int gfar_optimize_filer_masks(struct filer_table *tab)
 	}
 
 	/* And finally we just have to check for duplicated masks and drop the
-	 * second ones */
+	 * second ones
+	 */
 	for (i = 0; i < tab->index && i < MAX_FILER_CACHE_IDX; i++) {
 		if (tab->fe[i].ctrl == 0x80) {
 			previous_mask = i++;
@@ -1448,7 +1475,8 @@ static int gfar_optimize_filer_masks(struct filer_table *tab)
 		if (tab->fe[i].ctrl == 0x80) {
 			if (tab->fe[i].prop == tab->fe[previous_mask].prop) {
 				/* Two identical ones found!
-				 * So drop the second one! */
+				 * So drop the second one!
+				 */
 				gfar_trim_filer_entries(i, i, tab);
 			} else
 				/* Not identical! */
@@ -1463,7 +1491,7 @@ end:	kfree(temp_table);
 
 /* Write the bit-pattern from software's buffer to hardware registers */
 static int gfar_write_filer_table(struct gfar_private *priv,
-		struct filer_table *tab)
+				  struct filer_table *tab)
 {
 	u32 i = 0;
 	if (tab->index > MAX_FILER_IDX - 1)
@@ -1473,13 +1501,15 @@ static int gfar_write_filer_table(struct gfar_private *priv,
 	lock_rx_qs(priv);
 
 	/* Fill regular entries */
-	for (; i < MAX_FILER_IDX - 1 && (tab->fe[i].ctrl | tab->fe[i].ctrl); i++)
+	for (; i < MAX_FILER_IDX - 1 && (tab->fe[i].ctrl | tab->fe[i].ctrl);
+	     i++)
 		gfar_write_filer(priv, i, tab->fe[i].ctrl, tab->fe[i].prop);
 	/* Fill the rest with fall-troughs */
 	for (; i < MAX_FILER_IDX - 1; i++)
 		gfar_write_filer(priv, i, 0x60, 0xFFFFFFFF);
 	/* Last entry must be default accept
-	 * because that's what people expect */
+	 * because that's what people expect
+	 */
 	gfar_write_filer(priv, i, 0x20, 0x0);
 
 	unlock_rx_qs(priv);
@@ -1488,21 +1518,21 @@ static int gfar_write_filer_table(struct gfar_private *priv,
 }
 
 static int gfar_check_capability(struct ethtool_rx_flow_spec *flow,
-		struct gfar_private *priv)
+				 struct gfar_private *priv)
 {
 
 	if (flow->flow_type & FLOW_EXT)	{
 		if (~flow->m_ext.data[0] || ~flow->m_ext.data[1])
 			netdev_warn(priv->ndev,
-					"User-specific data not supported!\n");
+				    "User-specific data not supported!\n");
 		if (~flow->m_ext.vlan_etype)
 			netdev_warn(priv->ndev,
-					"VLAN-etype not supported!\n");
+				    "VLAN-etype not supported!\n");
 	}
 	if (flow->flow_type == IP_USER_FLOW)
 		if (flow->h_u.usr_ip4_spec.ip_ver != ETH_RX_NFC_IP4)
 			netdev_warn(priv->ndev,
-					"IP-Version differing from IPv4 not supported!\n");
+				    "IP-Version differing from IPv4 not supported!\n");
 
 	return 0;
 }
@@ -1520,15 +1550,18 @@ static int gfar_process_filer_changes(struct gfar_private *priv)
 		return -ENOMEM;
 
 	/* Now convert the existing filer data from flow_spec into
-	 * filer tables binary format */
+	 * filer tables binary format
+	 */
 	list_for_each_entry(j, &priv->rx_list.list, list) {
 		ret = gfar_convert_to_filer(&j->fs, tab);
 		if (ret == -EBUSY) {
-			netdev_err(priv->ndev, "Rule not added: No free space!\n");
+			netdev_err(priv->ndev,
+				   "Rule not added: No free space!\n");
 			goto end;
 		}
 		if (ret == -1) {
-			netdev_err(priv->ndev, "Rule not added: Unsupported Flow-type!\n");
+			netdev_err(priv->ndev,
+				   "Rule not added: Unsupported Flow-type!\n");
 			goto end;
 		}
 	}
@@ -1540,9 +1573,9 @@ static int gfar_process_filer_changes(struct gfar_private *priv)
 	gfar_optimize_filer_masks(tab);
 
 	pr_debug("\n\tSummary:\n"
-		"\tData on hardware: %d\n"
-		"\tCompression rate: %d%%\n",
-		tab->index, 100 - (100 * tab->index) / i);
+		 "\tData on hardware: %d\n"
+		 "\tCompression rate: %d%%\n",
+		 tab->index, 100 - (100 * tab->index) / i);
 
 	/* Write everything to hardware */
 	ret = gfar_write_filer_table(priv, tab);
@@ -1551,7 +1584,8 @@ static int gfar_process_filer_changes(struct gfar_private *priv)
 		goto end;
 	}
 
-end:	kfree(tab);
+end:
+	kfree(tab);
 	return ret;
 }
 
@@ -1569,7 +1603,7 @@ static void gfar_invert_masks(struct ethtool_rx_flow_spec *flow)
 }
 
 static int gfar_add_cls(struct gfar_private *priv,
-		struct ethtool_rx_flow_spec *flow)
+			struct ethtool_rx_flow_spec *flow)
 {
 	struct ethtool_flow_spec_container *temp, *comp;
 	int ret = 0;
@@ -1591,7 +1625,6 @@ static int gfar_add_cls(struct gfar_private *priv,
 		list_add(&temp->list, &priv->rx_list.list);
 		goto process;
 	} else {
-
 		list_for_each_entry(comp, &priv->rx_list.list, list) {
 			if (comp->fs.location > flow->location) {
 				list_add_tail(&temp->list, &comp->list);
@@ -1599,8 +1632,8 @@ static int gfar_add_cls(struct gfar_private *priv,
 			}
 			if (comp->fs.location == flow->location) {
 				netdev_err(priv->ndev,
-						"Rule not added: ID %d not free!\n",
-					flow->location);
+					   "Rule not added: ID %d not free!\n",
+					   flow->location);
 				ret = -EBUSY;
 				goto clean_mem;
 			}
@@ -1642,7 +1675,6 @@ static int gfar_del_cls(struct gfar_private *priv, u32 loc)
 	}
 
 	return ret;
-
 }
 
 static int gfar_get_cls(struct gfar_private *priv, struct ethtool_rxnfc *cmd)
@@ -1663,7 +1695,7 @@ static int gfar_get_cls(struct gfar_private *priv, struct ethtool_rxnfc *cmd)
 }
 
 static int gfar_get_cls_all(struct gfar_private *priv,
-		struct ethtool_rxnfc *cmd, u32 *rule_locs)
+			    struct ethtool_rxnfc *cmd, u32 *rule_locs)
 {
 	struct ethtool_flow_spec_container *comp;
 	u32 i = 0;
@@ -1714,7 +1746,7 @@ static int gfar_set_nfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
 }
 
 static int gfar_get_nfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
-		u32 *rule_locs)
+			u32 *rule_locs)
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	int ret = 0;
@@ -1748,23 +1780,19 @@ static int gfar_get_ts_info(struct net_device *dev,
 	struct gfar_private *priv = netdev_priv(dev);
 
 	if (!(priv->device_flags & FSL_GIANFAR_DEV_HAS_TIMER)) {
-		info->so_timestamping =
-			SOF_TIMESTAMPING_RX_SOFTWARE |
-			SOF_TIMESTAMPING_SOFTWARE;
+		info->so_timestamping = SOF_TIMESTAMPING_RX_SOFTWARE |
+					SOF_TIMESTAMPING_SOFTWARE;
 		info->phc_index = -1;
 		return 0;
 	}
-	info->so_timestamping =
-		SOF_TIMESTAMPING_TX_HARDWARE |
-		SOF_TIMESTAMPING_RX_HARDWARE |
-		SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
+				SOF_TIMESTAMPING_RX_HARDWARE |
+				SOF_TIMESTAMPING_RAW_HARDWARE;
 	info->phc_index = gfar_phc_index;
-	info->tx_types =
-		(1 << HWTSTAMP_TX_OFF) |
-		(1 << HWTSTAMP_TX_ON);
-	info->rx_filters =
-		(1 << HWTSTAMP_FILTER_NONE) |
-		(1 << HWTSTAMP_FILTER_ALL);
+	info->tx_types = (1 << HWTSTAMP_TX_OFF) |
+			 (1 << HWTSTAMP_TX_ON);
+	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+			   (1 << HWTSTAMP_FILTER_ALL);
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 9ac14f80485..21c6574c5f1 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -185,7 +185,7 @@ static void mem_disp(u8 *addr, int size)
 	for (; (u32) i < (u32) addr + size4Aling; i += 4)
 		printk("%08x ", *((u32 *) (i)));
 	for (; (u32) i < (u32) addr + size; i++)
-		printk("%02x", *((u8 *) (i)));
+		printk("%02x", *((i)));
 	if (notAlign == 1)
 		printk("\r\n");
 }
diff --git a/drivers/net/ethernet/hp/hp100.c b/drivers/net/ethernet/hp/hp100.c
index d496673f090..3f4391bede8 100644
--- a/drivers/net/ethernet/hp/hp100.c
+++ b/drivers/net/ethernet/hp/hp100.c
@@ -1217,7 +1217,7 @@ static int hp100_init_rxpdl(struct net_device *dev,
 
 	ringptr->pdl = pdlptr + 1;
 	ringptr->pdl_paddr = virt_to_whatever(dev, pdlptr + 1);
-	ringptr->skb = (void *) NULL;
+	ringptr->skb = NULL;
 
 	/*
 	 * Write address and length of first PDL Fragment (which is used for
@@ -1243,7 +1243,7 @@ static int hp100_init_txpdl(struct net_device *dev,
 
 	ringptr->pdl = pdlptr;	/* +1; */
 	ringptr->pdl_paddr = virt_to_whatever(dev, pdlptr);	/* +1 */
-	ringptr->skb = (void *) NULL;
+	ringptr->skb = NULL;
 
 	return roundup(MAX_TX_FRAG * 2 + 2, 4);
 }
@@ -1628,7 +1628,7 @@ static void hp100_clean_txring(struct net_device *dev)
 		/* Conversion to new PCI API : NOP */
 		pci_unmap_single(lp->pci_dev, (dma_addr_t) lp->txrhead->pdl[1], lp->txrhead->pdl[2], PCI_DMA_TODEVICE);
 		dev_kfree_skb_any(lp->txrhead->skb);
-		lp->txrhead->skb = (void *) NULL;
+		lp->txrhead->skb = NULL;
 		lp->txrhead = lp->txrhead->next;
 		lp->txrcommit--;
 	}
diff --git a/drivers/net/ethernet/i825xx/lp486e.c b/drivers/net/ethernet/i825xx/lp486e.c
index 6c2952c8ea1..3735bfa5360 100644
--- a/drivers/net/ethernet/i825xx/lp486e.c
+++ b/drivers/net/ethernet/i825xx/lp486e.c
@@ -629,10 +629,10 @@ init_i596(struct net_device *dev) {
 
 	memcpy ((void *)lp->eth_addr, dev->dev_addr, 6);
 	lp->set_add.command = CmdIASetup;
-	i596_add_cmd(dev, (struct i596_cmd *)&lp->set_add);
+	i596_add_cmd(dev, &lp->set_add);
 
 	lp->tdr.command = CmdTDR;
-	i596_add_cmd(dev, (struct i596_cmd *)&lp->tdr);
+	i596_add_cmd(dev, &lp->tdr);
 
 	if (lp->scb.command && i596_timeout(dev, "i82596 init", 200))
 		return 1;
@@ -737,7 +737,7 @@ i596_cleanup_cmd(struct net_device *dev) {
 
 	lp = netdev_priv(dev);
 	while (lp->cmd_head) {
-		cmd = (struct i596_cmd *)lp->cmd_head;
+		cmd = lp->cmd_head;
 
 		lp->cmd_head = pa_to_va(lp->cmd_head->pa_next);
 		lp->cmd_backlog--;
@@ -1281,7 +1281,7 @@ static void set_multicast_list(struct net_device *dev) {
 			lp->i596_config[8] |= 0x01;
 		}
 
-		i596_add_cmd(dev, (struct i596_cmd *) &lp->set_conf);
+		i596_add_cmd(dev, &lp->set_conf);
 	}
 }
 
diff --git a/drivers/net/ethernet/i825xx/sun3_82586.c b/drivers/net/ethernet/i825xx/sun3_82586.c
index cae17f4bc93..353f57f675d 100644
--- a/drivers/net/ethernet/i825xx/sun3_82586.c
+++ b/drivers/net/ethernet/i825xx/sun3_82586.c
@@ -571,7 +571,7 @@ static int init586(struct net_device *dev)
 	}
 #endif
 
-	ptr = alloc_rfa(dev,(void *)ptr); /* init receive-frame-area */
+	ptr = alloc_rfa(dev,ptr); /* init receive-frame-area */
 
 	/*
 	 * alloc xmit-buffs / init xmit_cmds
@@ -584,7 +584,7 @@ static int init586(struct net_device *dev)
 		ptr = (char *) ptr + XMIT_BUFF_SIZE;
 		p->xmit_buffs[i] = (struct tbd_struct *)ptr; /* TBD */
 		ptr = (char *) ptr + sizeof(struct tbd_struct);
-		if((void *)ptr > (void *)dev->mem_end)
+		if(ptr > (void *)dev->mem_end)
 		{
 			printk("%s: not enough shared-mem for your configuration!\n",dev->name);
 			return 1;
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_qmr.c b/drivers/net/ethernet/ibm/ehea/ehea_qmr.c
index 4fb47f14dbf..cb66f574dc9 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_qmr.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_qmr.c
@@ -376,9 +376,7 @@ int ehea_destroy_eq(struct ehea_eq *eq)
 	return 0;
 }
 
-/**
- * allocates memory for a queue and registers pages in phyp
- */
+/* allocates memory for a queue and registers pages in phyp */
 static int ehea_qp_alloc_register(struct ehea_qp *qp, struct hw_queue *hw_queue,
 			   int nr_pages, int wqe_size, int act_nr_sges,
 			   struct ehea_adapter *adapter, int h_call_q_selector)
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index ada720b42ff..535f94fac4a 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -1249,20 +1249,35 @@ static const struct firmware *e100_request_firmware(struct nic *nic)
 	const struct firmware *fw = nic->fw;
 	u8 timer, bundle, min_size;
 	int err = 0;
+	bool required = false;
 
 	/* do not load u-code for ICH devices */
 	if (nic->flags & ich)
 		return NULL;
 
-	/* Search for ucode match against h/w revision */
-	if (nic->mac == mac_82559_D101M)
+	/* Search for ucode match against h/w revision
+	 *
+	 * Based on comments in the source code for the FreeBSD fxp
+	 * driver, the FIRMWARE_D102E ucode includes both CPUSaver and
+	 *
+	 *    "fixes for bugs in the B-step hardware (specifically, bugs
+	 *     with Inline Receive)."
+	 *
+	 * So we must fail if it cannot be loaded.
+	 *
+	 * The other microcode files are only required for the optional
+	 * CPUSaver feature.  Nice to have, but no reason to fail.
+	 */
+	if (nic->mac == mac_82559_D101M) {
 		fw_name = FIRMWARE_D101M;
-	else if (nic->mac == mac_82559_D101S)
+	} else if (nic->mac == mac_82559_D101S) {
 		fw_name = FIRMWARE_D101S;
-	else if (nic->mac == mac_82551_F || nic->mac == mac_82551_10)
+	} else if (nic->mac == mac_82551_F || nic->mac == mac_82551_10) {
 		fw_name = FIRMWARE_D102E;
-	else /* No ucode on other devices */
+		required = true;
+	} else { /* No ucode on other devices */
 		return NULL;
+	}
 
 	/* If the firmware has not previously been loaded, request a pointer
 	 * to it. If it was previously loaded, we are reinitializing the
@@ -1273,10 +1288,17 @@ static const struct firmware *e100_request_firmware(struct nic *nic)
 		err = request_firmware(&fw, fw_name, &nic->pdev->dev);
 
 	if (err) {
-		netif_err(nic, probe, nic->netdev,
-			  "Failed to load firmware \"%s\": %d\n",
-			  fw_name, err);
-		return ERR_PTR(err);
+		if (required) {
+			netif_err(nic, probe, nic->netdev,
+				  "Failed to load firmware \"%s\": %d\n",
+				  fw_name, err);
+			return ERR_PTR(err);
+		} else {
+			netif_info(nic, probe, nic->netdev,
+				   "CPUSaver disabled. Needs \"%s\": %d\n",
+				   fw_name, err);
+			return NULL;
+		}
 	}
 
 	/* Firmware should be precisely UCODE_SIZE (words) plus three bytes
diff --git a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
index 3103f0b6bf5..736a7d987db 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
@@ -1851,6 +1851,7 @@ static const struct ethtool_ops e1000_ethtool_ops = {
 	.get_sset_count         = e1000_get_sset_count,
 	.get_coalesce           = e1000_get_coalesce,
 	.set_coalesce           = e1000_set_coalesce,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 void e1000_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/intel/e1000/e1000_hw.c b/drivers/net/ethernet/intel/e1000/e1000_hw.c
index c526279e492..3d683952876 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_hw.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_hw.c
@@ -399,7 +399,7 @@ void e1000_set_media_type(struct e1000_hw *hw)
 }
 
 /**
- * e1000_reset_hw: reset the hardware completely
+ * e1000_reset_hw - reset the hardware completely
  * @hw: Struct containing variables accessed by shared code
  *
  * Reset the transmit and receive units; mask and clear all interrupts.
@@ -546,7 +546,7 @@ s32 e1000_reset_hw(struct e1000_hw *hw)
 }
 
 /**
- * e1000_init_hw: Performs basic configuration of the adapter.
+ * e1000_init_hw - Performs basic configuration of the adapter.
  * @hw: Struct containing variables accessed by shared code
  *
  * Assumes that the controller has previously been reset and is in a
@@ -2591,7 +2591,7 @@ s32 e1000_check_for_link(struct e1000_hw *hw)
  * @hw: Struct containing variables accessed by shared code
  * @speed: Speed of the connection
  * @duplex: Duplex setting of the connection
-
+ *
  * Detects the current speed and duplex settings of the hardware.
  */
 s32 e1000_get_speed_and_duplex(struct e1000_hw *hw, u16 *speed, u16 *duplex)
@@ -2959,7 +2959,7 @@ static s32 e1000_read_phy_reg_ex(struct e1000_hw *hw, u32 reg_addr,
  * @hw: Struct containing variables accessed by shared code
  * @reg_addr: address of the PHY register to write
  * @data: data to write to the PHY
-
+ *
  * Writes a value to a PHY register
  */
 s32 e1000_write_phy_reg(struct e1000_hw *hw, u32 reg_addr, u16 phy_data)
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index 7483ca0a628..3bfbb8df898 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -721,9 +721,7 @@ void e1000_reset(struct e1000_adapter *adapter)
 	e1000_release_manageability(adapter);
 }
 
-/**
- *  Dump the eeprom for users having checksum issues
- **/
+/* Dump the eeprom for users having checksum issues */
 static void e1000_dump_eeprom(struct e1000_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
@@ -1078,18 +1076,18 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 	netdev->priv_flags |= IFF_SUPP_NOFCS;
 
 	netdev->features |= netdev->hw_features;
-	netdev->hw_features |= NETIF_F_RXCSUM;
-	netdev->hw_features |= NETIF_F_RXALL;
-	netdev->hw_features |= NETIF_F_RXFCS;
+	netdev->hw_features |= (NETIF_F_RXCSUM |
+				NETIF_F_RXALL |
+				NETIF_F_RXFCS);
 
 	if (pci_using_dac) {
 		netdev->features |= NETIF_F_HIGHDMA;
 		netdev->vlan_features |= NETIF_F_HIGHDMA;
 	}
 
-	netdev->vlan_features |= NETIF_F_TSO;
-	netdev->vlan_features |= NETIF_F_HW_CSUM;
-	netdev->vlan_features |= NETIF_F_SG;
+	netdev->vlan_features |= (NETIF_F_TSO |
+				  NETIF_F_HW_CSUM |
+				  NETIF_F_SG);
 
 	netdev->priv_flags |= IFF_UNICAST_FLT;
 
@@ -3056,14 +3054,13 @@ static void e1000_tx_queue(struct e1000_adapter *adapter,
 	mmiowb();
 }
 
-/**
- * 82547 workaround to avoid controller hang in half-duplex environment.
+/* 82547 workaround to avoid controller hang in half-duplex environment.
  * The workaround is to avoid queuing a large packet that would span
  * the internal Tx FIFO ring boundary by notifying the stack to resend
  * the packet at a later time.  This gives the Tx FIFO an opportunity to
  * flush all packets.  When that occurs, we reset the Tx FIFO pointers
  * to the beginning of the Tx FIFO.
- **/
+ */
 
 #define E1000_FIFO_HDR			0x10
 #define E1000_82547_PAD_LEN		0x3E0
diff --git a/drivers/net/ethernet/intel/e1000e/82571.c b/drivers/net/ethernet/intel/e1000e/82571.c
index 36db4df09ae..0b3bade957f 100644
--- a/drivers/net/ethernet/intel/e1000e/82571.c
+++ b/drivers/net/ethernet/intel/e1000e/82571.c
@@ -1572,6 +1572,9 @@ static s32 e1000_check_for_serdes_link_82571(struct e1000_hw *hw)
 	ctrl = er32(CTRL);
 	status = er32(STATUS);
 	rxcw = er32(RXCW);
+	/* SYNCH bit and IV bit are sticky */
+	udelay(10);
+	rxcw = er32(RXCW);
 
 	if ((rxcw & E1000_RXCW_SYNCH) && !(rxcw & E1000_RXCW_IV)) {
 
@@ -1677,16 +1680,18 @@ static s32 e1000_check_for_serdes_link_82571(struct e1000_hw *hw)
 			e_dbg("ANYSTATE  -> DOWN\n");
 		} else {
 			/*
-			 * Check several times, if Sync and Config
-			 * both are consistently 1 then simply ignore
-			 * the Invalid bit and restart Autoneg
+			 * Check several times, if SYNCH bit and CONFIG
+			 * bit both are consistently 1 then simply ignore
+			 * the IV bit and restart Autoneg
 			 */
 			for (i = 0; i < AN_RETRY_COUNT; i++) {
 				udelay(10);
 				rxcw = er32(RXCW);
-				if ((rxcw & E1000_RXCW_IV) &&
-				    !((rxcw & E1000_RXCW_SYNCH) &&
-				      (rxcw & E1000_RXCW_C))) {
+				if ((rxcw & E1000_RXCW_SYNCH) &&
+				    (rxcw & E1000_RXCW_C))
+					continue;
+
+				if (rxcw & E1000_RXCW_IV) {
 					mac->serdes_has_link = false;
 					mac->serdes_link_state =
 					    e1000_serdes_link_down;
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index 6e6fffb3458..cd153326c3c 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -514,6 +514,7 @@ 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);
 extern void e1000e_release_hw_control(struct e1000_adapter *adapter);
+extern void e1000e_write_itr(struct e1000_adapter *adapter, u32 itr);
 
 extern unsigned int copybreak;
 
diff --git a/drivers/net/ethernet/intel/e1000e/ethtool.c b/drivers/net/ethernet/intel/e1000e/ethtool.c
index 905e2147d91..0349e2478df 100644
--- a/drivers/net/ethernet/intel/e1000e/ethtool.c
+++ b/drivers/net/ethernet/intel/e1000e/ethtool.c
@@ -1897,7 +1897,6 @@ static int e1000_set_coalesce(struct net_device *netdev,
 			      struct ethtool_coalesce *ec)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
-	struct e1000_hw *hw = &adapter->hw;
 
 	if ((ec->rx_coalesce_usecs > E1000_MAX_ITR_USECS) ||
 	    ((ec->rx_coalesce_usecs > 4) &&
@@ -1916,9 +1915,9 @@ static int e1000_set_coalesce(struct net_device *netdev,
 	}
 
 	if (adapter->itr_setting != 0)
-		ew32(ITR, 1000000000 / (adapter->itr * 256));
+		e1000e_write_itr(adapter, adapter->itr);
 	else
-		ew32(ITR, 0);
+		e1000e_write_itr(adapter, 0);
 
 	return 0;
 }
@@ -2062,6 +2061,7 @@ static const struct ethtool_ops e1000_ethtool_ops = {
 	.get_coalesce		= e1000_get_coalesce,
 	.set_coalesce		= e1000_set_coalesce,
 	.get_rxnfc		= e1000_get_rxnfc,
+	.get_ts_info		= ethtool_op_get_ts_info,
 };
 
 void e1000e_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index 238ab2f8a5e..e3a7b07df62 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -325,24 +325,46 @@ static inline void __ew32flash(struct e1000_hw *hw, unsigned long reg, u32 val)
  **/
 static bool e1000_phy_is_accessible_pchlan(struct e1000_hw *hw)
 {
-	u16 phy_reg;
-	u32 phy_id;
+	u16 phy_reg = 0;
+	u32 phy_id = 0;
+	s32 ret_val;
+	u16 retry_count;
+
+	for (retry_count = 0; retry_count < 2; retry_count++) {
+		ret_val = e1e_rphy_locked(hw, PHY_ID1, &phy_reg);
+		if (ret_val || (phy_reg == 0xFFFF))
+			continue;
+		phy_id = (u32)(phy_reg << 16);
 
-	e1e_rphy_locked(hw, PHY_ID1, &phy_reg);
-	phy_id = (u32)(phy_reg << 16);
-	e1e_rphy_locked(hw, PHY_ID2, &phy_reg);
-	phy_id |= (u32)(phy_reg & PHY_REVISION_MASK);
+		ret_val = e1e_rphy_locked(hw, PHY_ID2, &phy_reg);
+		if (ret_val || (phy_reg == 0xFFFF)) {
+			phy_id = 0;
+			continue;
+		}
+		phy_id |= (u32)(phy_reg & PHY_REVISION_MASK);
+		break;
+	}
 
 	if (hw->phy.id) {
 		if (hw->phy.id == phy_id)
 			return true;
-	} else {
-		if ((phy_id != 0) && (phy_id != PHY_REVISION_MASK))
-			hw->phy.id = phy_id;
+	} else if (phy_id) {
+		hw->phy.id = phy_id;
+		hw->phy.revision = (u32)(phy_reg & ~PHY_REVISION_MASK);
 		return true;
 	}
 
-	return false;
+	/*
+	 * In case the PHY needs to be in mdio slow mode,
+	 * set slow mode and try to get the PHY id again.
+	 */
+	hw->phy.ops.release(hw);
+	ret_val = e1000_set_mdio_slow_mode_hv(hw);
+	if (!ret_val)
+		ret_val = e1000e_get_phy_id(hw);
+	hw->phy.ops.acquire(hw);
+
+	return !ret_val;
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 623e30b9964..95b245310f1 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -2159,7 +2159,7 @@ void e1000e_release_hw_control(struct e1000_adapter *adapter)
 }
 
 /**
- * @e1000_alloc_ring - allocate memory for a ring structure
+ * e1000_alloc_ring_dma - allocate memory for a ring structure
  **/
 static int e1000_alloc_ring_dma(struct e1000_adapter *adapter,
 				struct e1000_ring *ring)
@@ -2474,6 +2474,30 @@ set_itr_now:
 }
 
 /**
+ * e1000e_write_itr - write the ITR value to the appropriate registers
+ * @adapter: address of board private structure
+ * @itr: new ITR value to program
+ *
+ * e1000e_write_itr determines if the adapter is in MSI-X mode
+ * and, if so, writes the EITR registers with the ITR value.
+ * Otherwise, it writes the ITR value into the ITR register.
+ **/
+void e1000e_write_itr(struct e1000_adapter *adapter, u32 itr)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 new_itr = itr ? 1000000000 / (itr * 256) : 0;
+
+	if (adapter->msix_entries) {
+		int vector;
+
+		for (vector = 0; vector < adapter->num_vectors; vector++)
+			writel(new_itr, hw->hw_addr + E1000_EITR_82574(vector));
+	} else {
+		ew32(ITR, new_itr);
+	}
+}
+
+/**
  * e1000_alloc_queues - Allocate memory for all rings
  * @adapter: board private structure to initialize
  **/
@@ -3059,7 +3083,7 @@ static void e1000_configure_rx(struct e1000_adapter *adapter)
 	/* irq moderation */
 	ew32(RADV, adapter->rx_abs_int_delay);
 	if ((adapter->itr_setting != 0) && (adapter->itr != 0))
-		ew32(ITR, 1000000000 / (adapter->itr * 256));
+		e1000e_write_itr(adapter, adapter->itr);
 
 	ctrl_ext = er32(CTRL_EXT);
 	/* Auto-Mask interrupts upon ICR access */
@@ -3486,14 +3510,14 @@ void e1000e_reset(struct e1000_adapter *adapter)
 				dev_info(&adapter->pdev->dev,
 					"Interrupt Throttle Rate turned off\n");
 				adapter->flags2 |= FLAG2_DISABLE_AIM;
-				ew32(ITR, 0);
+				e1000e_write_itr(adapter, 0);
 			}
 		} else if (adapter->flags2 & FLAG2_DISABLE_AIM) {
 			dev_info(&adapter->pdev->dev,
 				 "Interrupt Throttle Rate turned on\n");
 			adapter->flags2 &= ~FLAG2_DISABLE_AIM;
 			adapter->itr = 20000;
-			ew32(ITR, 1000000000 / (adapter->itr * 256));
+			e1000e_write_itr(adapter, adapter->itr);
 		}
 	}
 
@@ -4576,7 +4600,7 @@ link_up:
 			    adapter->gorc - adapter->gotc) / 10000;
 		u32 itr = goc > 0 ? (dif * 6000 / goc + 2000) : 8000;
 
-		ew32(ITR, 1000000000 / (itr * 256));
+		e1000e_write_itr(adapter, itr);
 	}
 
 	/* Cause software interrupt to ensure Rx ring is cleaned */
@@ -6191,7 +6215,8 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 	}
 
 	if (hw->phy.ops.check_reset_block && hw->phy.ops.check_reset_block(hw))
-		e_info("PHY reset is blocked due to SOL/IDER session.\n");
+		dev_info(&pdev->dev,
+			 "PHY reset is blocked due to SOL/IDER session.\n");
 
 	/* Set initial default active device features */
 	netdev->features = (NETIF_F_SG |
@@ -6241,7 +6266,7 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 		if (e1000_validate_nvm_checksum(&adapter->hw) >= 0)
 			break;
 		if (i == 2) {
-			e_err("The NVM Checksum Is Not Valid\n");
+			dev_err(&pdev->dev, "The NVM Checksum Is Not Valid\n");
 			err = -EIO;
 			goto err_eeprom;
 		}
@@ -6251,13 +6276,15 @@ static int __devinit e1000_probe(struct pci_dev *pdev,
 
 	/* copy the MAC address */
 	if (e1000e_read_mac_addr(&adapter->hw))
-		e_err("NVM Read Error while reading MAC address\n");
+		dev_err(&pdev->dev,
+			"NVM Read Error while reading MAC address\n");
 
 	memcpy(netdev->dev_addr, adapter->hw.mac.addr, netdev->addr_len);
 	memcpy(netdev->perm_addr, adapter->hw.mac.addr, netdev->addr_len);
 
 	if (!is_valid_ether_addr(netdev->perm_addr)) {
-		e_err("Invalid MAC Address: %pM\n", netdev->perm_addr);
+		dev_err(&pdev->dev, "Invalid MAC Address: %pM\n",
+			netdev->perm_addr);
 		err = -EIO;
 		goto err_eeprom;
 	}
diff --git a/drivers/net/ethernet/intel/e1000e/param.c b/drivers/net/ethernet/intel/e1000e/param.c
index 55cc1565bc2..dfbfa7fd98c 100644
--- a/drivers/net/ethernet/intel/e1000e/param.c
+++ b/drivers/net/ethernet/intel/e1000e/param.c
@@ -199,16 +199,19 @@ static int __devinit e1000_validate_option(unsigned int *value,
 	case enable_option:
 		switch (*value) {
 		case OPTION_ENABLED:
-			e_info("%s Enabled\n", opt->name);
+			dev_info(&adapter->pdev->dev, "%s Enabled\n",
+				 opt->name);
 			return 0;
 		case OPTION_DISABLED:
-			e_info("%s Disabled\n", opt->name);
+			dev_info(&adapter->pdev->dev, "%s Disabled\n",
+				 opt->name);
 			return 0;
 		}
 		break;
 	case range_option:
 		if (*value >= opt->arg.r.min && *value <= opt->arg.r.max) {
-			e_info("%s set to %i\n", opt->name, *value);
+			dev_info(&adapter->pdev->dev, "%s set to %i\n",
+				 opt->name, *value);
 			return 0;
 		}
 		break;
@@ -220,7 +223,8 @@ static int __devinit e1000_validate_option(unsigned int *value,
 			ent = &opt->arg.l.p[i];
 			if (*value == ent->i) {
 				if (ent->str[0] != '\0')
-					e_info("%s\n", ent->str);
+					dev_info(&adapter->pdev->dev, "%s\n",
+						 ent->str);
 				return 0;
 			}
 		}
@@ -230,8 +234,8 @@ static int __devinit e1000_validate_option(unsigned int *value,
 		BUG();
 	}
 
-	e_info("Invalid %s value specified (%i) %s\n", opt->name, *value,
-	       opt->err);
+	dev_info(&adapter->pdev->dev, "Invalid %s value specified (%i) %s\n",
+		 opt->name, *value, opt->err);
 	*value = opt->def;
 	return -1;
 }
@@ -251,8 +255,10 @@ void __devinit e1000e_check_options(struct e1000_adapter *adapter)
 	int bd = adapter->bd_number;
 
 	if (bd >= E1000_MAX_NIC) {
-		e_notice("Warning: no configuration for board #%i\n", bd);
-		e_notice("Using defaults for all values\n");
+		dev_notice(&adapter->pdev->dev,
+			   "Warning: no configuration for board #%i\n", bd);
+		dev_notice(&adapter->pdev->dev,
+			   "Using defaults for all values\n");
 	}
 
 	{ /* Transmit Interrupt Delay */
@@ -366,27 +372,32 @@ void __devinit e1000e_check_options(struct e1000_adapter *adapter)
 			 * default values
 			 */
 			if (adapter->itr > 4)
-				e_info("%s set to default %d\n", opt.name,
-				       adapter->itr);
+				dev_info(&adapter->pdev->dev,
+					 "%s set to default %d\n", opt.name,
+					 adapter->itr);
 		}
 
 		adapter->itr_setting = adapter->itr;
 		switch (adapter->itr) {
 		case 0:
-			e_info("%s turned off\n", opt.name);
+			dev_info(&adapter->pdev->dev, "%s turned off\n",
+				 opt.name);
 			break;
 		case 1:
-			e_info("%s set to dynamic mode\n", opt.name);
+			dev_info(&adapter->pdev->dev,
+				 "%s set to dynamic mode\n", opt.name);
 			adapter->itr = 20000;
 			break;
 		case 3:
-			e_info("%s set to dynamic conservative mode\n",
-			       opt.name);
+			dev_info(&adapter->pdev->dev,
+				 "%s set to dynamic conservative mode\n",
+				 opt.name);
 			adapter->itr = 20000;
 			break;
 		case 4:
-			e_info("%s set to simplified (2000-8000 ints) mode\n",
-			       opt.name);
+			dev_info(&adapter->pdev->dev,
+				 "%s set to simplified (2000-8000 ints) mode\n",
+				 opt.name);
 			break;
 		default:
 			/*
diff --git a/drivers/net/ethernet/intel/igb/e1000_regs.h b/drivers/net/ethernet/intel/igb/e1000_regs.h
index 35d1e4f2c92..10efcd88dca 100644
--- a/drivers/net/ethernet/intel/igb/e1000_regs.h
+++ b/drivers/net/ethernet/intel/igb/e1000_regs.h
@@ -117,6 +117,7 @@
 
 /* TX Rate Limit Registers */
 #define E1000_RTTDQSEL	0x3604	/* Tx Desc Plane Queue Select - WO */
+#define E1000_RTTBCNRM	0x3690	/* Tx BCN Rate-scheduler MMW */
 #define E1000_RTTBCNRC	0x36B0	/* Tx BCN Rate-Scheduler Config - WO */
 
 /* Split and Replication RX Control - RW */
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index ae6d3f393a5..9e572dd29ab 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -65,19 +65,30 @@ struct igb_adapter;
 #define MAX_Q_VECTORS                      8
 
 /* Transmit and receive queues */
-#define IGB_MAX_RX_QUEUES		((adapter->vfs_allocated_count ? 2 : \
-					(hw->mac.type > e1000_82575 ? 8 : 4)))
-#define IGB_MAX_RX_QUEUES_I210             4
+#define IGB_MAX_RX_QUEUES                  8
+#define IGB_MAX_RX_QUEUES_82575            4
 #define IGB_MAX_RX_QUEUES_I211             2
-#define IGB_MAX_TX_QUEUES                  16
-#define IGB_MAX_TX_QUEUES_I210             4
-#define IGB_MAX_TX_QUEUES_I211             2
+#define IGB_MAX_TX_QUEUES                  8
 #define IGB_MAX_VF_MC_ENTRIES              30
 #define IGB_MAX_VF_FUNCTIONS               8
 #define IGB_MAX_VFTA_ENTRIES               128
 #define IGB_82576_VF_DEV_ID                0x10CA
 #define IGB_I350_VF_DEV_ID                 0x1520
 
+/* NVM version defines */
+#define IGB_MAJOR_MASK			0xF000
+#define IGB_MINOR_MASK			0x0FF0
+#define IGB_BUILD_MASK			0x000F
+#define IGB_COMB_VER_MASK		0x00FF
+#define IGB_MAJOR_SHIFT			12
+#define IGB_MINOR_SHIFT			4
+#define IGB_COMB_VER_SHFT		8
+#define IGB_NVM_VER_INVALID		0xFFFF
+#define IGB_ETRACK_SHIFT		16
+#define NVM_ETRACK_WORD			0x0042
+#define NVM_COMB_VER_OFF		0x0083
+#define NVM_COMB_VER_PTR		0x003d
+
 struct vf_data_storage {
 	unsigned char vf_mac_addresses[ETH_ALEN];
 	u16 vf_mc_hashes[IGB_MAX_VF_MC_ENTRIES];
@@ -371,6 +382,7 @@ struct igb_adapter {
 	spinlock_t tmreg_lock;
 	struct cyclecounter cc;
 	struct timecounter tc;
+	char fw_version[32];
 };
 
 #define IGB_FLAG_HAS_MSI           (1 << 0)
@@ -420,6 +432,7 @@ extern void igb_update_stats(struct igb_adapter *, struct rtnl_link_stats64 *);
 extern bool igb_has_link(struct igb_adapter *adapter);
 extern void igb_set_ethtool_ops(struct net_device *);
 extern void igb_power_up_link(struct igb_adapter *);
+extern void igb_set_fw_version(struct igb_adapter *);
 #ifdef CONFIG_IGB_PTP
 extern void igb_ptp_init(struct igb_adapter *adapter);
 extern void igb_ptp_remove(struct igb_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index 812d4f963bd..a19c84cad0e 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -710,6 +710,7 @@ static int igb_set_eeprom(struct net_device *netdev,
 	if ((ret_val == 0) && ((first_word <= NVM_CHECKSUM_REG)))
 		hw->nvm.ops.update(hw);
 
+	igb_set_fw_version(adapter);
 	kfree(eeprom_buff);
 	return ret_val;
 }
@@ -718,20 +719,16 @@ static void igb_get_drvinfo(struct net_device *netdev,
 			    struct ethtool_drvinfo *drvinfo)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
-	u16 eeprom_data;
 
 	strlcpy(drvinfo->driver,  igb_driver_name, sizeof(drvinfo->driver));
 	strlcpy(drvinfo->version, igb_driver_version, sizeof(drvinfo->version));
 
-	/* EEPROM image version # is reported as firmware version # for
-	 * 82575 controllers */
-	adapter->hw.nvm.ops.read(&adapter->hw, 5, 1, &eeprom_data);
-	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
-		"%d.%d-%d",
-		(eeprom_data & 0xF000) >> 12,
-		(eeprom_data & 0x0FF0) >> 4,
-		eeprom_data & 0x000F);
-
+	/*
+	 * EEPROM image version # is reported as firmware version # for
+	 * 82575 controllers
+	 */
+	strlcpy(drvinfo->fw_version, adapter->fw_version,
+		sizeof(drvinfo->fw_version));
 	strlcpy(drvinfo->bus_info, pci_name(adapter->pdev),
 		sizeof(drvinfo->bus_info));
 	drvinfo->n_stats = IGB_STATS_LEN;
@@ -2271,6 +2268,38 @@ static void igb_ethtool_complete(struct net_device *netdev)
 	pm_runtime_put(&adapter->pdev->dev);
 }
 
+#ifdef CONFIG_IGB_PTP
+static int igb_ethtool_get_ts_info(struct net_device *dev,
+				   struct ethtool_ts_info *info)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	if (adapter->ptp_clock)
+		info->phc_index = ptp_clock_index(adapter->ptp_clock);
+	else
+		info->phc_index = -1;
+
+	info->tx_types =
+		(1 << HWTSTAMP_TX_OFF) |
+		(1 << HWTSTAMP_TX_ON);
+
+	info->rx_filters =
+		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_ALL) |
+		(1 << HWTSTAMP_FILTER_SOME) |
+		(1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+		(1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
+
+	return 0;
+}
+
+#endif
 static const struct ethtool_ops igb_ethtool_ops = {
 	.get_settings           = igb_get_settings,
 	.set_settings           = igb_set_settings,
@@ -2299,6 +2328,9 @@ static const struct ethtool_ops igb_ethtool_ops = {
 	.set_coalesce           = igb_set_coalesce,
 	.begin			= igb_ethtool_begin,
 	.complete		= igb_ethtool_complete,
+#ifdef CONFIG_IGB_PTP
+	.get_ts_info		= igb_ethtool_get_ts_info,
+#endif
 };
 
 void igb_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index dd3bfe8cd36..b7c2d505057 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -59,9 +59,9 @@
 #endif
 #include "igb.h"
 
-#define MAJ 3
-#define MIN 4
-#define BUILD 7
+#define MAJ 4
+#define MIN 0
+#define BUILD 1
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 __stringify(BUILD) "-k"
 char igb_driver_name[] = "igb";
@@ -1048,11 +1048,6 @@ static int igb_set_interrupt_capability(struct igb_adapter *adapter)
 	if (!(adapter->flags & IGB_FLAG_QUEUE_PAIRS))
 		numvecs += adapter->num_tx_queues;
 
-	/* i210 and i211 can only have 4 MSIX vectors for rx/tx queues. */
-	if ((adapter->hw.mac.type == e1000_i210)
-		|| (adapter->hw.mac.type == e1000_i211))
-		numvecs = 4;
-
 	/* store the number of vectors reserved for queues */
 	adapter->num_q_vectors = numvecs;
 
@@ -1505,11 +1500,12 @@ static void igb_configure(struct igb_adapter *adapter)
  **/
 void igb_power_up_link(struct igb_adapter *adapter)
 {
+	igb_reset_phy(&adapter->hw);
+
 	if (adapter->hw.phy.media_type == e1000_media_type_copper)
 		igb_power_up_phy_copper(&adapter->hw);
 	else
 		igb_power_up_serdes_link_82575(&adapter->hw);
-	igb_reset_phy(&adapter->hw);
 }
 
 /**
@@ -1821,6 +1817,69 @@ static const struct net_device_ops igb_netdev_ops = {
 };
 
 /**
+ * igb_set_fw_version - Configure version string for ethtool
+ * @adapter: adapter struct
+ *
+ **/
+void igb_set_fw_version(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u16 eeprom_verh, eeprom_verl, comb_verh, comb_verl, comb_offset;
+	u16 major, build, patch, fw_version;
+	u32 etrack_id;
+
+	hw->nvm.ops.read(hw, 5, 1, &fw_version);
+	if (adapter->hw.mac.type != e1000_i211) {
+		hw->nvm.ops.read(hw, NVM_ETRACK_WORD, 1, &eeprom_verh);
+		hw->nvm.ops.read(hw, (NVM_ETRACK_WORD + 1), 1, &eeprom_verl);
+		etrack_id = (eeprom_verh << IGB_ETRACK_SHIFT) | eeprom_verl;
+
+		/* combo image version needs to be found */
+		hw->nvm.ops.read(hw, NVM_COMB_VER_PTR, 1, &comb_offset);
+		if ((comb_offset != 0x0) &&
+		    (comb_offset != IGB_NVM_VER_INVALID)) {
+			hw->nvm.ops.read(hw, (NVM_COMB_VER_OFF + comb_offset
+					 + 1), 1, &comb_verh);
+			hw->nvm.ops.read(hw, (NVM_COMB_VER_OFF + comb_offset),
+					 1, &comb_verl);
+
+			/* Only display Option Rom if it exists and is valid */
+			if ((comb_verh && comb_verl) &&
+			    ((comb_verh != IGB_NVM_VER_INVALID) &&
+			     (comb_verl != IGB_NVM_VER_INVALID))) {
+				major = comb_verl >> IGB_COMB_VER_SHFT;
+				build = (comb_verl << IGB_COMB_VER_SHFT) |
+					(comb_verh >> IGB_COMB_VER_SHFT);
+				patch = comb_verh & IGB_COMB_VER_MASK;
+				snprintf(adapter->fw_version,
+					 sizeof(adapter->fw_version),
+					 "%d.%d%d, 0x%08x, %d.%d.%d",
+					 (fw_version & IGB_MAJOR_MASK) >>
+					 IGB_MAJOR_SHIFT,
+					 (fw_version & IGB_MINOR_MASK) >>
+					 IGB_MINOR_SHIFT,
+					 (fw_version & IGB_BUILD_MASK),
+					 etrack_id, major, build, patch);
+				goto out;
+			}
+		}
+		snprintf(adapter->fw_version, sizeof(adapter->fw_version),
+			 "%d.%d%d, 0x%08x",
+			 (fw_version & IGB_MAJOR_MASK) >> IGB_MAJOR_SHIFT,
+			 (fw_version & IGB_MINOR_MASK) >> IGB_MINOR_SHIFT,
+			 (fw_version & IGB_BUILD_MASK), etrack_id);
+	} else {
+		snprintf(adapter->fw_version, sizeof(adapter->fw_version),
+			 "%d.%d%d",
+			 (fw_version & IGB_MAJOR_MASK) >> IGB_MAJOR_SHIFT,
+			 (fw_version & IGB_MINOR_MASK) >> IGB_MINOR_SHIFT,
+			 (fw_version & IGB_BUILD_MASK));
+	}
+out:
+	return;
+}
+
+/**
  * igb_probe - Device Initialization Routine
  * @pdev: PCI device information struct
  * @ent: entry in igb_pci_tbl
@@ -2030,6 +2089,9 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 		goto err_eeprom;
 	}
 
+	/* get firmware version for ethtool -i */
+	igb_set_fw_version(adapter);
+
 	setup_timer(&adapter->watchdog_timer, igb_watchdog,
 	            (unsigned long) adapter);
 	setup_timer(&adapter->phy_info_timer, igb_update_phy_info,
@@ -2338,6 +2400,7 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 	struct pci_dev *pdev = adapter->pdev;
+	u32 max_rss_queues;
 
 	pci_read_config_word(pdev, PCI_COMMAND, &hw->bus.pci_cmd_word);
 
@@ -2370,40 +2433,69 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
 		} else
 			adapter->vfs_allocated_count = max_vfs;
 		break;
-	case e1000_i210:
-	case e1000_i211:
-		adapter->vfs_allocated_count = 0;
-		break;
 	default:
 		break;
 	}
 #endif /* CONFIG_PCI_IOV */
+
+	/* Determine the maximum number of RSS queues supported. */
 	switch (hw->mac.type) {
+	case e1000_i211:
+		max_rss_queues = IGB_MAX_RX_QUEUES_I211;
+		break;
+	case e1000_82575:
 	case e1000_i210:
-		adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES_I210,
-			num_online_cpus());
+		max_rss_queues = IGB_MAX_RX_QUEUES_82575;
+		break;
+	case e1000_i350:
+		/* I350 cannot do RSS and SR-IOV at the same time */
+		if (!!adapter->vfs_allocated_count) {
+			max_rss_queues = 1;
+			break;
+		}
+		/* fall through */
+	case e1000_82576:
+		if (!!adapter->vfs_allocated_count) {
+			max_rss_queues = 2;
+			break;
+		}
+		/* fall through */
+	case e1000_82580:
+	default:
+		max_rss_queues = IGB_MAX_RX_QUEUES;
 		break;
+	}
+
+	adapter->rss_queues = min_t(u32, max_rss_queues, num_online_cpus());
+
+	/* Determine if we need to pair queues. */
+	switch (hw->mac.type) {
+	case e1000_82575:
 	case e1000_i211:
-		adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES_I211,
-			num_online_cpus());
+		/* Device supports enough interrupts without queue pairing. */
 		break;
+	case e1000_82576:
+		/*
+		 * If VFs are going to be allocated with RSS queues then we
+		 * should pair the queues in order to conserve interrupts due
+		 * to limited supply.
+		 */
+		if ((adapter->rss_queues > 1) &&
+		    (adapter->vfs_allocated_count > 6))
+			adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
+		/* fall through */
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i210:
 	default:
-		adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES,
-		num_online_cpus());
+		/*
+		 * If rss_queues > half of max_rss_queues, pair the queues in
+		 * order to conserve interrupts due to limited supply.
+		 */
+		if (adapter->rss_queues > (max_rss_queues / 2))
+			adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
 		break;
 	}
-	/* i350 cannot do RSS and SR-IOV at the same time */
-	if (hw->mac.type == e1000_i350 && adapter->vfs_allocated_count)
-		adapter->rss_queues = 1;
-
-	/*
-	 * if rss_queues > 4 or vfs are going to be allocated with rss_queues
-	 * then we should combine the queues into a queue pair in order to
-	 * conserve interrupts due to limited supply
-	 */
-	if ((adapter->rss_queues > 4) ||
-	    ((adapter->rss_queues > 1) && (adapter->vfs_allocated_count > 6)))
-		adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
 
 	/* Setup and initialize a copy of the hw vlan table array */
 	adapter->shadow_vfta = kzalloc(sizeof(u32) *
@@ -4917,7 +5009,7 @@ static int igb_vf_configure(struct igb_adapter *adapter, int vf)
 	unsigned int device_id;
 	u16 thisvf_devfn;
 
-	random_ether_addr(mac_addr);
+	eth_random_addr(mac_addr);
 	igb_set_vf_mac(adapter, vf, mac_addr);
 
 	switch (adapter->hw.mac.type) {
@@ -5326,7 +5418,7 @@ static void igb_vf_reset_event(struct igb_adapter *adapter, u32 vf)
 
 	/* generate a new mac address as we were hotplug removed/added */
 	if (!(adapter->vf_data[vf].flags & IGB_VF_FLAG_PF_SET_MAC))
-		random_ether_addr(vf_mac);
+		eth_random_addr(vf_mac);
 
 	/* process remaining reset events */
 	igb_vf_reset(adapter, vf);
@@ -5686,6 +5778,7 @@ static void igb_tx_hwtstamp(struct igb_q_vector *q_vector,
 /**
  * igb_clean_tx_irq - Reclaim resources after transmit completes
  * @q_vector: pointer to q_vector containing needed info
+ *
  * returns true if ring is completely cleaned
  **/
 static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
@@ -6142,7 +6235,7 @@ static bool igb_alloc_mapped_page(struct igb_ring *rx_ring,
 		return true;
 
 	if (!page) {
-		page = alloc_page(GFP_ATOMIC | __GFP_COLD);
+		page = __skb_alloc_page(GFP_ATOMIC, bi->skb);
 		bi->page = page;
 		if (unlikely(!page)) {
 			rx_ring->rx_stats.alloc_failed++;
@@ -6997,6 +7090,11 @@ static void igb_set_vf_rate_limit(struct e1000_hw *hw, int vf, int tx_rate,
 	}
 
 	wr32(E1000_RTTDQSEL, vf); /* vf X uses queue X */
+	/*
+	 * Set global transmit compensation time to the MMW_SIZE in RTTBCNRM
+	 * register. MMW_SIZE=0x014 if 9728-byte jumbo is supported.
+	 */
+	wr32(E1000_RTTBCNRM, 0x14);
 	wr32(E1000_RTTBCNRC, bcnrc_val);
 }
 
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index d5ee7fa5072..c846ea9131a 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -330,7 +330,17 @@ void igb_ptp_init(struct igb_adapter *adapter)
 
 void igb_ptp_remove(struct igb_adapter *adapter)
 {
-	cancel_delayed_work_sync(&adapter->overflow_work);
+	switch (adapter->hw.mac.type) {
+	case e1000_i211:
+	case e1000_i210:
+	case e1000_i350:
+	case e1000_82580:
+	case e1000_82576:
+		cancel_delayed_work_sync(&adapter->overflow_work);
+		break;
+	default:
+		return;
+	}
 
 	if (adapter->ptp_clock) {
 		ptp_clock_unregister(adapter->ptp_clock);
diff --git a/drivers/net/ethernet/intel/igbvf/netdev.c b/drivers/net/ethernet/intel/igbvf/netdev.c
index 8ec74b07f94..0696abfe994 100644
--- a/drivers/net/ethernet/intel/igbvf/netdev.c
+++ b/drivers/net/ethernet/intel/igbvf/netdev.c
@@ -766,6 +766,7 @@ static void igbvf_set_itr(struct igbvf_adapter *adapter)
 /**
  * igbvf_clean_tx_irq - Reclaim resources after transmit completes
  * @adapter: board private structure
+ *
  * returns true if ring is completely cleaned
  **/
 static bool igbvf_clean_tx_irq(struct igbvf_ring *tx_ring)
diff --git a/drivers/net/ethernet/intel/igbvf/vf.c b/drivers/net/ethernet/intel/igbvf/vf.c
index 30a6cc42603..eea0e10ce12 100644
--- a/drivers/net/ethernet/intel/igbvf/vf.c
+++ b/drivers/net/ethernet/intel/igbvf/vf.c
@@ -283,7 +283,8 @@ static s32 e1000_set_vfta_vf(struct e1000_hw *hw, u16 vid, bool set)
 	return err;
 }
 
-/** e1000_rlpml_set_vf - Set the maximum receive packet length
+/**
+ *  e1000_rlpml_set_vf - Set the maximum receive packet length
  *  @hw: pointer to the HW structure
  *  @max_size: value to assign to max frame size
  **/
@@ -302,7 +303,7 @@ void e1000_rlpml_set_vf(struct e1000_hw *hw, u16 max_size)
  *  e1000_rar_set_vf - set device MAC address
  *  @hw: pointer to the HW structure
  *  @addr: pointer to the receive address
- *  @index receive address array register
+ *  @index: receive address array register
  **/
 static void e1000_rar_set_vf(struct e1000_hw *hw, u8 * addr, u32 index)
 {
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_hw.c b/drivers/net/ethernet/intel/ixgb/ixgb_hw.c
index 99b69adb4a0..bf9a220f71f 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_hw.c
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_hw.c
@@ -32,6 +32,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/pci_ids.h>
 #include "ixgb_hw.h"
 #include "ixgb_ids.h"
 
@@ -96,7 +97,7 @@ static u32 ixgb_mac_reset(struct ixgb_hw *hw)
 	ASSERT(!(ctrl_reg & IXGB_CTRL0_RST));
 #endif
 
-	if (hw->subsystem_vendor_id == SUN_SUBVENDOR_ID) {
+	if (hw->subsystem_vendor_id == PCI_VENDOR_ID_SUN) {
 		ctrl_reg =  /* Enable interrupt from XFP and SerDes */
 			   IXGB_CTRL1_GPI0_EN |
 			   IXGB_CTRL1_SDP6_DIR |
@@ -271,7 +272,7 @@ ixgb_identify_phy(struct ixgb_hw *hw)
 	}
 
 	/* update phy type for sun specific board */
-	if (hw->subsystem_vendor_id == SUN_SUBVENDOR_ID)
+	if (hw->subsystem_vendor_id == PCI_VENDOR_ID_SUN)
 		phy_type = ixgb_phy_type_bcm;
 
 	return phy_type;
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_ids.h b/drivers/net/ethernet/intel/ixgb/ixgb_ids.h
index 2a58847f46e..32c1b302d79 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_ids.h
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_ids.h
@@ -33,11 +33,6 @@
 ** The Device and Vendor IDs for 10 Gigabit MACs
 **********************************************************************/
 
-#define INTEL_VENDOR_ID             0x8086
-#define INTEL_SUBVENDOR_ID          0x8086
-#define SUN_VENDOR_ID               0x108E
-#define SUN_SUBVENDOR_ID            0x108E
-
 #define IXGB_DEVICE_ID_82597EX      0x1048
 #define IXGB_DEVICE_ID_82597EX_SR   0x1A48
 #define IXGB_DEVICE_ID_82597EX_LR   0x1B48
diff --git a/drivers/net/ethernet/intel/ixgb/ixgb_main.c b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
index 5fce363d810..d05fc95befc 100644
--- a/drivers/net/ethernet/intel/ixgb/ixgb_main.c
+++ b/drivers/net/ethernet/intel/ixgb/ixgb_main.c
@@ -54,13 +54,13 @@ MODULE_PARM_DESC(copybreak,
  *   Class, Class Mask, private data (not used) }
  */
 static DEFINE_PCI_DEVICE_TABLE(ixgb_pci_tbl) = {
-	{INTEL_VENDOR_ID, IXGB_DEVICE_ID_82597EX,
+	{PCI_VENDOR_ID_INTEL, IXGB_DEVICE_ID_82597EX,
 	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{INTEL_VENDOR_ID, IXGB_DEVICE_ID_82597EX_CX4,
+	{PCI_VENDOR_ID_INTEL, IXGB_DEVICE_ID_82597EX_CX4,
 	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{INTEL_VENDOR_ID, IXGB_DEVICE_ID_82597EX_SR,
+	{PCI_VENDOR_ID_INTEL, IXGB_DEVICE_ID_82597EX_SR,
 	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{INTEL_VENDOR_ID, IXGB_DEVICE_ID_82597EX_LR,
+	{PCI_VENDOR_ID_INTEL, IXGB_DEVICE_ID_82597EX_LR,
 	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
 
 	/* required last entry */
@@ -195,7 +195,7 @@ ixgb_irq_enable(struct ixgb_adapter *adapter)
 {
 	u32 val = IXGB_INT_RXT0 | IXGB_INT_RXDMT0 |
 		  IXGB_INT_TXDW | IXGB_INT_LSC;
-	if (adapter->hw.subsystem_vendor_id == SUN_SUBVENDOR_ID)
+	if (adapter->hw.subsystem_vendor_id == PCI_VENDOR_ID_SUN)
 		val |= IXGB_INT_GPI0;
 	IXGB_WRITE_REG(&adapter->hw, IMS, val);
 	IXGB_WRITE_FLUSH(&adapter->hw);
@@ -2276,9 +2276,9 @@ static void ixgb_netpoll(struct net_device *dev)
 #endif
 
 /**
- * ixgb_io_error_detected() - called when PCI error is detected
- * @pdev    pointer to pci device with error
- * @state   pci channel state after error
+ * ixgb_io_error_detected - called when PCI error is detected
+ * @pdev:    pointer to pci device with error
+ * @state:   pci channel state after error
  *
  * This callback is called by the PCI subsystem whenever
  * a PCI bus error is detected.
diff --git a/drivers/net/ethernet/intel/ixgbe/Makefile b/drivers/net/ethernet/intel/ixgbe/Makefile
index 0bdf06bc5c4..5fd5d04c26c 100644
--- a/drivers/net/ethernet/intel/ixgbe/Makefile
+++ b/drivers/net/ethernet/intel/ixgbe/Makefile
@@ -34,11 +34,11 @@ obj-$(CONFIG_IXGBE) += ixgbe.o
 
 ixgbe-objs := ixgbe_main.o ixgbe_common.o ixgbe_ethtool.o \
               ixgbe_82599.o ixgbe_82598.o ixgbe_phy.o ixgbe_sriov.o \
-              ixgbe_mbx.o ixgbe_x540.o ixgbe_sysfs.o ixgbe_lib.o
+              ixgbe_mbx.o ixgbe_x540.o ixgbe_lib.o
 
 ixgbe-$(CONFIG_IXGBE_DCB) +=  ixgbe_dcb.o ixgbe_dcb_82598.o \
                               ixgbe_dcb_82599.o ixgbe_dcb_nl.o
 
 ixgbe-$(CONFIG_IXGBE_PTP) += ixgbe_ptp.o
-
+ixgbe-$(CONFIG_IXGBE_HWMON) += ixgbe_sysfs.o
 ixgbe-$(CONFIG_FCOE:m=y) += ixgbe_fcoe.o
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 7af291e236b..b9623e9ea89 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -77,17 +77,18 @@
 #define IXGBE_MAX_FCPAUSE		 0xFFFF
 
 /* Supported Rx Buffer Sizes */
-#define IXGBE_RXBUFFER_512   512    /* Used for packet split */
+#define IXGBE_RXBUFFER_256    256  /* Used for skb receive header */
 #define IXGBE_MAX_RXBUFFER  16384  /* largest size for a single descriptor */
 
 /*
- * NOTE: netdev_alloc_skb reserves up to 64 bytes, NET_IP_ALIGN mans we
- * reserve 2 more, and skb_shared_info adds an additional 384 bytes more,
- * this adds up to 512 bytes of extra data meaning the smallest allocation
- * we could have is 1K.
- * i.e. RXBUFFER_512 --> size-1024 slab
+ * NOTE: netdev_alloc_skb reserves up to 64 bytes, NET_IP_ALIGN means we
+ * reserve 64 more, and skb_shared_info adds an additional 320 bytes more,
+ * this adds up to 448 bytes of extra data.
+ *
+ * Since netdev_alloc_skb now allocates a page fragment we can use a value
+ * of 256 and the resultant skb will have a truesize of 960 or less.
  */
-#define IXGBE_RX_HDR_SIZE IXGBE_RXBUFFER_512
+#define IXGBE_RX_HDR_SIZE IXGBE_RXBUFFER_256
 
 #define MAXIMUM_ETHERNET_VLAN_SIZE (ETH_FRAME_LEN + ETH_FCS_LEN + VLAN_HLEN)
 
@@ -113,7 +114,7 @@
 #define IXGBE_MAX_VFTA_ENTRIES          128
 #define MAX_EMULATION_MAC_ADDRS         16
 #define IXGBE_MAX_PF_MACVLANS           15
-#define VMDQ_P(p)   ((p) + adapter->num_vfs)
+#define VMDQ_P(p)   ((p) + adapter->ring_feature[RING_F_VMDQ].offset)
 #define IXGBE_82599_VF_DEVICE_ID        0x10ED
 #define IXGBE_X540_VF_DEVICE_ID         0x1515
 
@@ -130,7 +131,6 @@ struct vf_data_storage {
 	u16 tx_rate;
 	u16 vlan_count;
 	u8 spoofchk_enabled;
-	struct pci_dev *vfdev;
 };
 
 struct vf_macvlans {
@@ -278,10 +278,16 @@ enum ixgbe_ring_f_enum {
 #define MAX_TX_QUEUES IXGBE_MAX_FDIR_INDICES
 #endif /* IXGBE_FCOE */
 struct ixgbe_ring_feature {
-	int indices;
-	int mask;
+	u16 limit;	/* upper limit on feature indices */
+	u16 indices;	/* current value of indices */
+	u16 mask;	/* Mask used for feature to ring mapping */
+	u16 offset;	/* offset to start of feature */
 } ____cacheline_internodealigned_in_smp;
 
+#define IXGBE_82599_VMDQ_8Q_MASK 0x78
+#define IXGBE_82599_VMDQ_4Q_MASK 0x7C
+#define IXGBE_82599_VMDQ_2Q_MASK 0x7E
+
 /*
  * FCoE requires that all Rx buffers be over 2200 bytes in length.  Since
  * this is twice the size of a half page we need to double the page order
@@ -315,7 +321,7 @@ struct ixgbe_ring_container {
                               ? 8 : 1)
 #define MAX_TX_PACKET_BUFFERS MAX_RX_PACKET_BUFFERS
 
-/* MAX_MSIX_Q_VECTORS of these are allocated,
+/* MAX_Q_VECTORS of these are allocated,
  * but we only use one per queue-specific vector.
  */
 struct ixgbe_q_vector {
@@ -401,11 +407,11 @@ static inline u16 ixgbe_desc_unused(struct ixgbe_ring *ring)
 #define NON_Q_VECTORS (OTHER_VECTOR)
 
 #define MAX_MSIX_VECTORS_82599 64
-#define MAX_MSIX_Q_VECTORS_82599 64
+#define MAX_Q_VECTORS_82599 64
 #define MAX_MSIX_VECTORS_82598 18
-#define MAX_MSIX_Q_VECTORS_82598 16
+#define MAX_Q_VECTORS_82598 16
 
-#define MAX_MSIX_Q_VECTORS MAX_MSIX_Q_VECTORS_82599
+#define MAX_Q_VECTORS MAX_Q_VECTORS_82599
 #define MAX_MSIX_COUNT MAX_MSIX_VECTORS_82599
 
 #define MIN_MSIX_Q_VECTORS 1
@@ -427,35 +433,33 @@ struct ixgbe_adapter {
 	 * thus the additional *_CAPABLE flags.
 	 */
 	u32 flags;
-#define IXGBE_FLAG_MSI_CAPABLE                  (u32)(1 << 1)
-#define IXGBE_FLAG_MSI_ENABLED                  (u32)(1 << 2)
-#define IXGBE_FLAG_MSIX_CAPABLE                 (u32)(1 << 3)
-#define IXGBE_FLAG_MSIX_ENABLED                 (u32)(1 << 4)
-#define IXGBE_FLAG_RX_1BUF_CAPABLE              (u32)(1 << 6)
-#define IXGBE_FLAG_RX_PS_CAPABLE                (u32)(1 << 7)
-#define IXGBE_FLAG_RX_PS_ENABLED                (u32)(1 << 8)
-#define IXGBE_FLAG_IN_NETPOLL                   (u32)(1 << 9)
-#define IXGBE_FLAG_DCA_ENABLED                  (u32)(1 << 10)
-#define IXGBE_FLAG_DCA_CAPABLE                  (u32)(1 << 11)
-#define IXGBE_FLAG_IMIR_ENABLED                 (u32)(1 << 12)
-#define IXGBE_FLAG_MQ_CAPABLE                   (u32)(1 << 13)
-#define IXGBE_FLAG_DCB_ENABLED                  (u32)(1 << 14)
-#define IXGBE_FLAG_RSS_ENABLED                  (u32)(1 << 16)
-#define IXGBE_FLAG_RSS_CAPABLE                  (u32)(1 << 17)
-#define IXGBE_FLAG_VMDQ_CAPABLE                 (u32)(1 << 18)
-#define IXGBE_FLAG_VMDQ_ENABLED                 (u32)(1 << 19)
-#define IXGBE_FLAG_FAN_FAIL_CAPABLE             (u32)(1 << 20)
-#define IXGBE_FLAG_NEED_LINK_UPDATE             (u32)(1 << 22)
-#define IXGBE_FLAG_NEED_LINK_CONFIG             (u32)(1 << 23)
-#define IXGBE_FLAG_FDIR_HASH_CAPABLE            (u32)(1 << 24)
-#define IXGBE_FLAG_FDIR_PERFECT_CAPABLE         (u32)(1 << 25)
-#define IXGBE_FLAG_FCOE_CAPABLE                 (u32)(1 << 26)
-#define IXGBE_FLAG_FCOE_ENABLED                 (u32)(1 << 27)
-#define IXGBE_FLAG_SRIOV_CAPABLE                (u32)(1 << 28)
-#define IXGBE_FLAG_SRIOV_ENABLED                (u32)(1 << 29)
+#define IXGBE_FLAG_MSI_CAPABLE                  (u32)(1 << 0)
+#define IXGBE_FLAG_MSI_ENABLED                  (u32)(1 << 1)
+#define IXGBE_FLAG_MSIX_CAPABLE                 (u32)(1 << 2)
+#define IXGBE_FLAG_MSIX_ENABLED                 (u32)(1 << 3)
+#define IXGBE_FLAG_RX_1BUF_CAPABLE              (u32)(1 << 4)
+#define IXGBE_FLAG_RX_PS_CAPABLE                (u32)(1 << 5)
+#define IXGBE_FLAG_RX_PS_ENABLED                (u32)(1 << 6)
+#define IXGBE_FLAG_IN_NETPOLL                   (u32)(1 << 7)
+#define IXGBE_FLAG_DCA_ENABLED                  (u32)(1 << 8)
+#define IXGBE_FLAG_DCA_CAPABLE                  (u32)(1 << 9)
+#define IXGBE_FLAG_IMIR_ENABLED                 (u32)(1 << 10)
+#define IXGBE_FLAG_MQ_CAPABLE                   (u32)(1 << 11)
+#define IXGBE_FLAG_DCB_ENABLED                  (u32)(1 << 12)
+#define IXGBE_FLAG_VMDQ_CAPABLE                 (u32)(1 << 13)
+#define IXGBE_FLAG_VMDQ_ENABLED                 (u32)(1 << 14)
+#define IXGBE_FLAG_FAN_FAIL_CAPABLE             (u32)(1 << 15)
+#define IXGBE_FLAG_NEED_LINK_UPDATE             (u32)(1 << 16)
+#define IXGBE_FLAG_NEED_LINK_CONFIG             (u32)(1 << 17)
+#define IXGBE_FLAG_FDIR_HASH_CAPABLE            (u32)(1 << 18)
+#define IXGBE_FLAG_FDIR_PERFECT_CAPABLE         (u32)(1 << 19)
+#define IXGBE_FLAG_FCOE_CAPABLE                 (u32)(1 << 20)
+#define IXGBE_FLAG_FCOE_ENABLED                 (u32)(1 << 21)
+#define IXGBE_FLAG_SRIOV_CAPABLE                (u32)(1 << 22)
+#define IXGBE_FLAG_SRIOV_ENABLED                (u32)(1 << 23)
 
 	u32 flags2;
-#define IXGBE_FLAG2_RSC_CAPABLE                 (u32)(1)
+#define IXGBE_FLAG2_RSC_CAPABLE                 (u32)(1 << 0)
 #define IXGBE_FLAG2_RSC_ENABLED                 (u32)(1 << 1)
 #define IXGBE_FLAG2_TEMP_SENSOR_CAPABLE         (u32)(1 << 2)
 #define IXGBE_FLAG2_TEMP_SENSOR_EVENT           (u32)(1 << 3)
@@ -496,7 +500,7 @@ struct ixgbe_adapter {
 	u32 alloc_rx_page_failed;
 	u32 alloc_rx_buff_failed;
 
-	struct ixgbe_q_vector *q_vector[MAX_MSIX_Q_VECTORS];
+	struct ixgbe_q_vector *q_vector[MAX_Q_VECTORS];
 
 	/* DCB parameters */
 	struct ieee_pfc *ixgbe_ieee_pfc;
@@ -507,8 +511,8 @@ struct ixgbe_adapter {
 	u8 dcbx_cap;
 	enum ixgbe_fc_mode last_lfc_mode;
 
-	int num_msix_vectors;
-	int max_msix_q_vectors;         /* true count of q_vectors for device */
+	int num_q_vectors;	/* current number of q_vectors for device */
+	int max_q_vectors;	/* true count of q_vectors for device */
 	struct ixgbe_ring_feature ring_feature[RING_F_ARRAY_SIZE];
 	struct msix_entry *msix_entries;
 
@@ -561,6 +565,7 @@ struct ixgbe_adapter {
 	spinlock_t tmreg_lock;
 	struct cyclecounter cc;
 	struct timecounter tc;
+	int rx_hwtstamp_filter;
 	u32 base_incval;
 	u32 cycle_speed;
 #endif /* CONFIG_IXGBE_PTP */
@@ -686,7 +691,6 @@ extern void ixgbe_configure_fcoe(struct ixgbe_adapter *adapter);
 extern int ixgbe_fso(struct ixgbe_ring *tx_ring,
 		     struct ixgbe_tx_buffer *first,
 		     u8 *hdr_len);
-extern void ixgbe_cleanup_fcoe(struct ixgbe_adapter *adapter);
 extern int ixgbe_fcoe_ddp(struct ixgbe_adapter *adapter,
 			  union ixgbe_adv_rx_desc *rx_desc,
 			  struct sk_buff *skb);
@@ -695,6 +699,8 @@ extern int ixgbe_fcoe_ddp_get(struct net_device *netdev, u16 xid,
 extern int ixgbe_fcoe_ddp_target(struct net_device *netdev, u16 xid,
 				 struct scatterlist *sgl, unsigned int sgc);
 extern int ixgbe_fcoe_ddp_put(struct net_device *netdev, u16 xid);
+extern int ixgbe_setup_fcoe_ddp_resources(struct ixgbe_adapter *adapter);
+extern void ixgbe_free_fcoe_ddp_resources(struct ixgbe_adapter *adapter);
 extern int ixgbe_fcoe_enable(struct net_device *netdev);
 extern int ixgbe_fcoe_disable(struct net_device *netdev);
 #ifdef CONFIG_IXGBE_DCB
@@ -704,6 +710,7 @@ extern u8 ixgbe_fcoe_setapp(struct ixgbe_adapter *adapter, u8 up);
 extern int ixgbe_fcoe_get_wwn(struct net_device *netdev, u64 *wwn, int type);
 extern int ixgbe_fcoe_get_hbainfo(struct net_device *netdev,
 				  struct netdev_fcoe_hbainfo *info);
+extern u8 ixgbe_fcoe_get_tc(struct ixgbe_adapter *adapter);
 #endif /* IXGBE_FCOE */
 
 static inline struct netdev_queue *txring_txq(const struct ixgbe_ring *ring)
@@ -718,6 +725,7 @@ extern void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter);
 extern void ixgbe_ptp_tx_hwtstamp(struct ixgbe_q_vector *q_vector,
 				  struct sk_buff *skb);
 extern void ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
+				  union ixgbe_adv_rx_desc *rx_desc,
 				  struct sk_buff *skb);
 extern int ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
 				    struct ifreq *ifr, int cmd);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
index dee64d2703f..50fc137501d 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -241,7 +241,9 @@ static s32 ixgbe_get_link_capabilities_82599(struct ixgbe_hw *hw,
 
 	/* Determine 1G link capabilities off of SFP+ type */
 	if (hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
-	    hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1) {
+	    hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
+	    hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0 ||
+	    hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1) {
 		*speed = IXGBE_LINK_SPEED_1GB_FULL;
 		*negotiation = true;
 		goto out;
@@ -1023,6 +1025,9 @@ mac_reset_top:
 		hw->mac.ops.set_rar(hw, hw->mac.num_rar_entries - 1,
 		                    hw->mac.san_addr, 0, IXGBE_RAH_AV);
 
+		/* Save the SAN MAC RAR index */
+		hw->mac.san_mac_rar_index = hw->mac.num_rar_entries - 1;
+
 		/* Reserve the last RAR for the SAN MAC address */
 		hw->mac.num_rar_entries--;
 	}
@@ -2104,6 +2109,7 @@ static struct ixgbe_mac_operations mac_ops_82599 = {
 	.set_rar                = &ixgbe_set_rar_generic,
 	.clear_rar              = &ixgbe_clear_rar_generic,
 	.set_vmdq               = &ixgbe_set_vmdq_generic,
+	.set_vmdq_san_mac	= &ixgbe_set_vmdq_san_mac_generic,
 	.clear_vmdq             = &ixgbe_clear_vmdq_generic,
 	.init_rx_addrs          = &ixgbe_init_rx_addrs_generic,
 	.update_mc_addr_list    = &ixgbe_update_mc_addr_list_generic,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index 77ac41feb0f..90e41db3cb6 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -2848,6 +2848,31 @@ s32 ixgbe_set_vmdq_generic(struct ixgbe_hw *hw, u32 rar, u32 vmdq)
 }
 
 /**
+ *  This function should only be involved in the IOV mode.
+ *  In IOV mode, Default pool is next pool after the number of
+ *  VFs advertized and not 0.
+ *  MPSAR table needs to be updated for SAN_MAC RAR [hw->mac.san_mac_rar_index]
+ *
+ *  ixgbe_set_vmdq_san_mac - Associate default VMDq pool index with a rx address
+ *  @hw: pointer to hardware struct
+ *  @vmdq: VMDq pool index
+ **/
+s32 ixgbe_set_vmdq_san_mac_generic(struct ixgbe_hw *hw, u32 vmdq)
+{
+	u32 rar = hw->mac.san_mac_rar_index;
+
+	if (vmdq < 32) {
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), 1 << vmdq);
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), 0);
+	} else {
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_LO(rar), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_MPSAR_HI(rar), 1 << (vmdq - 32));
+	}
+
+	return 0;
+}
+
+/**
  *  ixgbe_init_uta_tables_generic - Initialize the Unicast Table Array
  *  @hw: pointer to hardware structure
  **/
@@ -3132,7 +3157,7 @@ s32 ixgbe_check_mac_link_generic(struct ixgbe_hw *hw, ixgbe_link_speed *speed,
 }
 
 /**
- *  ixgbe_get_wwn_prefix_generic Get alternative WWNN/WWPN prefix from
+ *  ixgbe_get_wwn_prefix_generic - Get alternative WWNN/WWPN prefix from
  *  the EEPROM
  *  @hw: pointer to hardware structure
  *  @wwnn_prefix: the alternative WWNN prefix
@@ -3200,20 +3225,22 @@ void ixgbe_set_mac_anti_spoofing(struct ixgbe_hw *hw, bool enable, int pf)
 	 * PFVFSPOOF register array is size 8 with 8 bits assigned to
 	 * MAC anti-spoof enables in each register array element.
 	 */
-	for (j = 0; j < IXGBE_PFVFSPOOF_REG_COUNT; j++)
+	for (j = 0; j < pf_target_reg; j++)
 		IXGBE_WRITE_REG(hw, IXGBE_PFVFSPOOF(j), pfvfspoof);
 
-	/* If not enabling anti-spoofing then done */
-	if (!enable)
-		return;
-
 	/*
 	 * The PF should be allowed to spoof so that it can support
-	 * emulation mode NICs.  Reset the bit assigned to the PF
+	 * emulation mode NICs.  Do not set the bits assigned to the PF
 	 */
-	pfvfspoof = IXGBE_READ_REG(hw, IXGBE_PFVFSPOOF(pf_target_reg));
-	pfvfspoof ^= (1 << pf_target_shift);
-	IXGBE_WRITE_REG(hw, IXGBE_PFVFSPOOF(pf_target_reg), pfvfspoof);
+	pfvfspoof &= (1 << pf_target_shift) - 1;
+	IXGBE_WRITE_REG(hw, IXGBE_PFVFSPOOF(j), pfvfspoof);
+
+	/*
+	 * Remaining pools belong to the PF so they do not need to have
+	 * anti-spoofing enabled.
+	 */
+	for (j++; j < IXGBE_PFVFSPOOF_REG_COUNT; j++)
+		IXGBE_WRITE_REG(hw, IXGBE_PFVFSPOOF(j), 0);
 }
 
 /**
@@ -3325,6 +3352,7 @@ void ixgbe_set_rxpba_generic(struct ixgbe_hw *hw,
  *  ixgbe_calculate_checksum - Calculate checksum for buffer
  *  @buffer: pointer to EEPROM
  *  @length: size of EEPROM to calculate a checksum for
+ *
  *  Calculates the checksum for some buffer on a specified length.  The
  *  checksum calculated is returned.
  **/
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
index 6222fdb3d3f..d813d1188c3 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
@@ -85,6 +85,7 @@ s32 ixgbe_acquire_swfw_sync(struct ixgbe_hw *hw, u16 mask);
 void ixgbe_release_swfw_sync(struct ixgbe_hw *hw, u16 mask);
 s32 ixgbe_get_san_mac_addr_generic(struct ixgbe_hw *hw, u8 *san_mac_addr);
 s32 ixgbe_set_vmdq_generic(struct ixgbe_hw *hw, u32 rar, u32 vmdq);
+s32 ixgbe_set_vmdq_san_mac_generic(struct ixgbe_hw *hw, u32 vmdq);
 s32 ixgbe_clear_vmdq_generic(struct ixgbe_hw *hw, u32 rar, u32 vmdq);
 s32 ixgbe_init_uta_tables_generic(struct ixgbe_hw *hw);
 s32 ixgbe_set_vfta_generic(struct ixgbe_hw *hw, u32 vlan,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c
index 8bfaaee5ac5..9bc17c0cb97 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c
@@ -180,67 +180,83 @@ out:
 
 void ixgbe_dcb_unpack_pfc(struct ixgbe_dcb_config *cfg, u8 *pfc_en)
 {
-	int i;
+	struct tc_configuration *tc_config = &cfg->tc_config[0];
+	int tc;
 
-	*pfc_en = 0;
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
-		*pfc_en |= !!(cfg->tc_config[i].dcb_pfc & 0xF) << i;
+	for (*pfc_en = 0, tc = 0; tc < MAX_TRAFFIC_CLASS; tc++) {
+		if (tc_config[tc].dcb_pfc != pfc_disabled)
+			*pfc_en |= 1 << tc;
+	}
 }
 
 void ixgbe_dcb_unpack_refill(struct ixgbe_dcb_config *cfg, int direction,
 			     u16 *refill)
 {
-	struct tc_bw_alloc *p;
-	int i;
+	struct tc_configuration *tc_config = &cfg->tc_config[0];
+	int tc;
 
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &cfg->tc_config[i].path[direction];
-		refill[i] = p->data_credits_refill;
-	}
+	for (tc = 0; tc < MAX_TRAFFIC_CLASS; tc++)
+		refill[tc] = tc_config[tc].path[direction].data_credits_refill;
 }
 
 void ixgbe_dcb_unpack_max(struct ixgbe_dcb_config *cfg, u16 *max)
 {
-	int i;
+	struct tc_configuration *tc_config = &cfg->tc_config[0];
+	int tc;
 
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++)
-		max[i] = cfg->tc_config[i].desc_credits_max;
+	for (tc = 0; tc < MAX_TRAFFIC_CLASS; tc++)
+		max[tc] = tc_config[tc].desc_credits_max;
 }
 
 void ixgbe_dcb_unpack_bwgid(struct ixgbe_dcb_config *cfg, int direction,
 			    u8 *bwgid)
 {
-	struct tc_bw_alloc *p;
-	int i;
+	struct tc_configuration *tc_config = &cfg->tc_config[0];
+	int tc;
 
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &cfg->tc_config[i].path[direction];
-		bwgid[i] = p->bwg_id;
-	}
+	for (tc = 0; tc < MAX_TRAFFIC_CLASS; tc++)
+		bwgid[tc] = tc_config[tc].path[direction].bwg_id;
 }
 
 void ixgbe_dcb_unpack_prio(struct ixgbe_dcb_config *cfg, int direction,
 			    u8 *ptype)
 {
-	struct tc_bw_alloc *p;
-	int i;
+	struct tc_configuration *tc_config = &cfg->tc_config[0];
+	int tc;
 
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		p = &cfg->tc_config[i].path[direction];
-		ptype[i] = p->prio_type;
+	for (tc = 0; tc < MAX_TRAFFIC_CLASS; tc++)
+		ptype[tc] = tc_config[tc].path[direction].prio_type;
+}
+
+u8 ixgbe_dcb_get_tc_from_up(struct ixgbe_dcb_config *cfg, int direction, u8 up)
+{
+	struct tc_configuration *tc_config = &cfg->tc_config[0];
+	u8 prio_mask = 1 << up;
+	u8 tc = cfg->num_tcs.pg_tcs;
+
+	/* If tc is 0 then DCB is likely not enabled or supported */
+	if (!tc)
+		goto out;
+
+	/*
+	 * Test from maximum TC to 1 and report the first match we find.  If
+	 * we find no match we can assume that the TC is 0 since the TC must
+	 * be set for all user priorities
+	 */
+	for (tc--; tc; tc--) {
+		if (prio_mask & tc_config[tc].path[direction].up_to_tc_bitmap)
+			break;
 	}
+out:
+	return tc;
 }
 
 void ixgbe_dcb_unpack_map(struct ixgbe_dcb_config *cfg, int direction, u8 *map)
 {
-	int i, up;
-	unsigned long bitmap;
+	u8 up;
 
-	for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-		bitmap = cfg->tc_config[i].path[direction].up_to_tc_bitmap;
-		for_each_set_bit(up, &bitmap, MAX_USER_PRIORITY)
-			map[up] = i;
-	}
+	for (up = 0; up < MAX_USER_PRIORITY; up++)
+		map[up] = ixgbe_dcb_get_tc_from_up(cfg, direction, up);
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.h
index 24333b71816..1f4108ee154 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.h
@@ -146,6 +146,7 @@ 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 *);
 void ixgbe_dcb_unpack_map(struct ixgbe_dcb_config *, int, u8 *);
+u8 ixgbe_dcb_get_tc_from_up(struct ixgbe_dcb_config *, int, u8);
 
 /* DCB credits calculation */
 s32 ixgbe_dcb_calculate_tc_credits(struct ixgbe_hw *,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c
index 5164a21b13c..f1e002d5fa8 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_nl.c
@@ -151,34 +151,21 @@ static u8 ixgbe_dcbnl_get_state(struct net_device *netdev)
 
 static u8 ixgbe_dcbnl_set_state(struct net_device *netdev, u8 state)
 {
-	int err = 0;
-	u8 prio_tc[MAX_USER_PRIORITY] = {0};
-	int i;
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+	int err = 0;
 
 	/* Fail command if not in CEE mode */
 	if (!(adapter->dcbx_cap & DCB_CAP_DCBX_VER_CEE))
 		return 1;
 
 	/* verify there is something to do, if not then exit */
-	if (!!state != !(adapter->flags & IXGBE_FLAG_DCB_ENABLED))
-		goto out;
-
-	if (state > 0) {
-		err = ixgbe_setup_tc(netdev, adapter->dcb_cfg.num_tcs.pg_tcs);
-		ixgbe_dcb_unpack_map(&adapter->dcb_cfg, DCB_TX_CONFIG, prio_tc);
-	} else {
-		err = ixgbe_setup_tc(netdev, 0);
-	}
-
-	if (err)
+	if (!state == !(adapter->flags & IXGBE_FLAG_DCB_ENABLED))
 		goto out;
 
-	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
-		netdev_set_prio_tc_map(netdev, i, prio_tc[i]);
-
+	err = ixgbe_setup_tc(netdev,
+			     state ? adapter->dcb_cfg.num_tcs.pg_tcs : 0);
 out:
-	return err ? 1 : 0;
+	return !!err;
 }
 
 static void ixgbe_dcbnl_get_perm_hw_addr(struct net_device *netdev,
@@ -584,9 +571,6 @@ static int ixgbe_dcbnl_ieee_setets(struct net_device *dev,
 	if (err)
 		goto err_out;
 
-	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
-		netdev_set_prio_tc_map(dev, i, ets->prio_tc[i]);
-
 	err = ixgbe_dcb_hw_ets(&adapter->hw, ets, max_frame);
 err_out:
 	return err;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index 3178f1ec371..4104ea25d81 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -154,100 +154,60 @@ static int ixgbe_get_settings(struct net_device *netdev,
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
+	ixgbe_link_speed supported_link;
 	u32 link_speed = 0;
+	bool autoneg;
 	bool link_up;
 
-	ecmd->supported = SUPPORTED_10000baseT_Full;
-	ecmd->autoneg = AUTONEG_ENABLE;
-	ecmd->transceiver = XCVR_EXTERNAL;
-	if ((hw->phy.media_type == ixgbe_media_type_copper) ||
-	    (hw->phy.multispeed_fiber)) {
-		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) {
-			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)
-				ecmd->advertising |= ADVERTISED_1000baseT_Full;
-		} else {
-			/*
-			 * Default advertised modes in case
-			 * phy.autoneg_advertised isn't set.
-			 */
-			ecmd->advertising |= (ADVERTISED_10000baseT_Full |
-					      ADVERTISED_1000baseT_Full);
-			if (hw->mac.type == ixgbe_mac_X540)
-				ecmd->advertising |= ADVERTISED_100baseT_Full;
-		}
-
-		if (hw->phy.media_type == ixgbe_media_type_copper) {
-			ecmd->supported |= SUPPORTED_TP;
-			ecmd->advertising |= ADVERTISED_TP;
-			ecmd->port = PORT_TP;
-		} else {
-			ecmd->supported |= SUPPORTED_FIBRE;
-			ecmd->advertising |= ADVERTISED_FIBRE;
-			ecmd->port = PORT_FIBRE;
-		}
-	} else if (hw->phy.media_type == ixgbe_media_type_backplane) {
-		/* Set as FIBRE until SERDES defined in kernel */
-		if (hw->device_id == IXGBE_DEV_ID_82598_BX) {
-			ecmd->supported = (SUPPORTED_1000baseT_Full |
-					   SUPPORTED_FIBRE);
-			ecmd->advertising = (ADVERTISED_1000baseT_Full |
-					     ADVERTISED_FIBRE);
-			ecmd->port = PORT_FIBRE;
-			ecmd->autoneg = AUTONEG_DISABLE;
-		} else if ((hw->device_id == IXGBE_DEV_ID_82599_COMBO_BACKPLANE) ||
-			   (hw->device_id == IXGBE_DEV_ID_82599_KX4_MEZZ)) {
-			ecmd->supported |= (SUPPORTED_1000baseT_Full |
-					    SUPPORTED_Autoneg |
-					    SUPPORTED_FIBRE);
-			ecmd->advertising = (ADVERTISED_10000baseT_Full |
-					     ADVERTISED_1000baseT_Full |
-					     ADVERTISED_Autoneg |
-					     ADVERTISED_FIBRE);
-			ecmd->port = PORT_FIBRE;
-		} else {
-			ecmd->supported |= (SUPPORTED_1000baseT_Full |
-					    SUPPORTED_FIBRE);
-			ecmd->advertising = (ADVERTISED_10000baseT_Full |
-					     ADVERTISED_1000baseT_Full |
-					     ADVERTISED_FIBRE);
-			ecmd->port = PORT_FIBRE;
-		}
+	hw->mac.ops.get_link_capabilities(hw, &supported_link, &autoneg);
+
+	/* set the supported link speeds */
+	if (supported_link & IXGBE_LINK_SPEED_10GB_FULL)
+		ecmd->supported |= SUPPORTED_10000baseT_Full;
+	if (supported_link & IXGBE_LINK_SPEED_1GB_FULL)
+		ecmd->supported |= SUPPORTED_1000baseT_Full;
+	if (supported_link & IXGBE_LINK_SPEED_100_FULL)
+		ecmd->supported |= SUPPORTED_100baseT_Full;
+
+	/* set the advertised speeds */
+	if (hw->phy.autoneg_advertised) {
+		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)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
 	} else {
-		ecmd->supported |= SUPPORTED_FIBRE;
-		ecmd->advertising = (ADVERTISED_10000baseT_Full |
-		                     ADVERTISED_FIBRE);
-		ecmd->port = PORT_FIBRE;
-		ecmd->autoneg = AUTONEG_DISABLE;
+		/* default modes in case phy.autoneg_advertised isn't set */
+		if (supported_link & IXGBE_LINK_SPEED_10GB_FULL)
+			ecmd->advertising |= ADVERTISED_10000baseT_Full;
+		if (supported_link & IXGBE_LINK_SPEED_1GB_FULL)
+			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		if (supported_link & IXGBE_LINK_SPEED_100_FULL)
+			ecmd->advertising |= ADVERTISED_100baseT_Full;
 	}
 
-	/* Get PHY type */
+	if (autoneg) {
+		ecmd->supported |= SUPPORTED_Autoneg;
+		ecmd->advertising |= ADVERTISED_Autoneg;
+		ecmd->autoneg = AUTONEG_ENABLE;
+	} else
+		ecmd->autoneg = AUTONEG_DISABLE;
+
+	ecmd->transceiver = XCVR_EXTERNAL;
+
+	/* Determine the remaining settings based on the PHY type. */
 	switch (adapter->hw.phy.type) {
 	case ixgbe_phy_tn:
 	case ixgbe_phy_aq:
 	case ixgbe_phy_cu_unknown:
-		/* Copper 10G-BASET */
+		ecmd->supported |= SUPPORTED_TP;
+		ecmd->advertising |= ADVERTISED_TP;
 		ecmd->port = PORT_TP;
 		break;
 	case ixgbe_phy_qt:
+		ecmd->supported |= SUPPORTED_FIBRE;
+		ecmd->advertising |= ADVERTISED_FIBRE;
 		ecmd->port = PORT_FIBRE;
 		break;
 	case ixgbe_phy_nl:
@@ -257,42 +217,59 @@ static int ixgbe_get_settings(struct net_device *netdev,
 	case ixgbe_phy_sfp_avago:
 	case ixgbe_phy_sfp_intel:
 	case ixgbe_phy_sfp_unknown:
-		switch (adapter->hw.phy.sfp_type) {
 		/* SFP+ devices, further checking needed */
+		switch (adapter->hw.phy.sfp_type) {
 		case ixgbe_sfp_type_da_cu:
 		case ixgbe_sfp_type_da_cu_core0:
 		case ixgbe_sfp_type_da_cu_core1:
+			ecmd->supported |= SUPPORTED_FIBRE;
+			ecmd->advertising |= ADVERTISED_FIBRE;
 			ecmd->port = PORT_DA;
 			break;
 		case ixgbe_sfp_type_sr:
 		case ixgbe_sfp_type_lr:
 		case ixgbe_sfp_type_srlr_core0:
 		case ixgbe_sfp_type_srlr_core1:
+			ecmd->supported |= SUPPORTED_FIBRE;
+			ecmd->advertising |= ADVERTISED_FIBRE;
 			ecmd->port = PORT_FIBRE;
 			break;
 		case ixgbe_sfp_type_not_present:
+			ecmd->supported |= SUPPORTED_FIBRE;
+			ecmd->advertising |= ADVERTISED_FIBRE;
 			ecmd->port = PORT_NONE;
 			break;
 		case ixgbe_sfp_type_1g_cu_core0:
 		case ixgbe_sfp_type_1g_cu_core1:
+			ecmd->supported |= SUPPORTED_TP;
+			ecmd->advertising |= ADVERTISED_TP;
 			ecmd->port = PORT_TP;
-			ecmd->supported = SUPPORTED_TP;
-			ecmd->advertising = (ADVERTISED_1000baseT_Full |
-			                     ADVERTISED_TP);
+			break;
+		case ixgbe_sfp_type_1g_sx_core0:
+		case ixgbe_sfp_type_1g_sx_core1:
+			ecmd->supported |= SUPPORTED_FIBRE;
+			ecmd->advertising |= ADVERTISED_FIBRE;
+			ecmd->port = PORT_FIBRE;
 			break;
 		case ixgbe_sfp_type_unknown:
 		default:
+			ecmd->supported |= SUPPORTED_FIBRE;
+			ecmd->advertising |= ADVERTISED_FIBRE;
 			ecmd->port = PORT_OTHER;
 			break;
 		}
 		break;
 	case ixgbe_phy_xaui:
+		ecmd->supported |= SUPPORTED_FIBRE;
+		ecmd->advertising |= ADVERTISED_FIBRE;
 		ecmd->port = PORT_NONE;
 		break;
 	case ixgbe_phy_unknown:
 	case ixgbe_phy_generic:
 	case ixgbe_phy_sfp_unsupported:
 	default:
+		ecmd->supported |= SUPPORTED_FIBRE;
+		ecmd->advertising |= ADVERTISED_FIBRE;
 		ecmd->port = PORT_OTHER;
 		break;
 	}
@@ -2113,7 +2090,6 @@ static int ixgbe_set_coalesce(struct net_device *netdev,
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_q_vector *q_vector;
 	int i;
-	int num_vectors;
 	u16 tx_itr_param, rx_itr_param;
 	bool need_reset = false;
 
@@ -2149,12 +2125,7 @@ static int ixgbe_set_coalesce(struct net_device *netdev,
 	/* check the old value and enable RSC if necessary */
 	need_reset = ixgbe_update_rsc(adapter);
 
-	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED)
-		num_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-	else
-		num_vectors = 1;
-
-	for (i = 0; i < num_vectors; i++) {
+	for (i = 0; i < adapter->num_q_vectors; i++) {
 		q_vector = adapter->q_vector[i];
 		if (q_vector->tx.count && !q_vector->rx.count)
 			/* tx only */
@@ -2274,10 +2245,6 @@ static int ixgbe_get_rss_hash_opts(struct ixgbe_adapter *adapter,
 {
 	cmd->data = 0;
 
-	/* if RSS is disabled then report no hashing */
-	if (!(adapter->flags & IXGBE_FLAG_RSS_ENABLED))
-		return 0;
-
 	/* Report default options for RSS on ixgbe */
 	switch (cmd->flow_type) {
 	case TCP_V4_FLOW:
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
index bc07933d67d..ae73ef14fdf 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
@@ -38,7 +38,7 @@
 
 /**
  * ixgbe_fcoe_clear_ddp - clear the given ddp context
- * @ddp - ptr to the ixgbe_fcoe_ddp
+ * @ddp: ptr to the ixgbe_fcoe_ddp
  *
  * Returns : none
  *
@@ -104,10 +104,10 @@ int ixgbe_fcoe_ddp_put(struct net_device *netdev, u16 xid)
 			udelay(100);
 	}
 	if (ddp->sgl)
-		pci_unmap_sg(adapter->pdev, ddp->sgl, ddp->sgc,
+		dma_unmap_sg(&adapter->pdev->dev, ddp->sgl, ddp->sgc,
 			     DMA_FROM_DEVICE);
 	if (ddp->pool) {
-		pci_pool_free(ddp->pool, ddp->udl, ddp->udp);
+		dma_pool_free(ddp->pool, ddp->udl, ddp->udp);
 		ddp->pool = NULL;
 	}
 
@@ -134,6 +134,7 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 	struct ixgbe_hw *hw;
 	struct ixgbe_fcoe *fcoe;
 	struct ixgbe_fcoe_ddp *ddp;
+	struct ixgbe_fcoe_ddp_pool *ddp_pool;
 	struct scatterlist *sg;
 	unsigned int i, j, dmacount;
 	unsigned int len;
@@ -144,8 +145,6 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 	unsigned int thislen = 0;
 	u32 fcbuff, fcdmarw, fcfltrw, fcrxctl;
 	dma_addr_t addr = 0;
-	struct pci_pool *pool;
-	unsigned int cpu;
 
 	if (!netdev || !sgl)
 		return 0;
@@ -162,11 +161,6 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 		return 0;
 
 	fcoe = &adapter->fcoe;
-	if (!fcoe->pool) {
-		e_warn(drv, "xid=0x%x no ddp pool for fcoe\n", xid);
-		return 0;
-	}
-
 	ddp = &fcoe->ddp[xid];
 	if (ddp->sgl) {
 		e_err(drv, "xid 0x%x w/ non-null sgl=%p nents=%d\n",
@@ -175,22 +169,32 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 	}
 	ixgbe_fcoe_clear_ddp(ddp);
 
+
+	if (!fcoe->ddp_pool) {
+		e_warn(drv, "No ddp_pool resources allocated\n");
+		return 0;
+	}
+
+	ddp_pool = per_cpu_ptr(fcoe->ddp_pool, get_cpu());
+	if (!ddp_pool->pool) {
+		e_warn(drv, "xid=0x%x no ddp pool for fcoe\n", xid);
+		goto out_noddp;
+	}
+
 	/* setup dma from scsi command sgl */
-	dmacount = pci_map_sg(adapter->pdev, sgl, sgc, DMA_FROM_DEVICE);
+	dmacount = dma_map_sg(&adapter->pdev->dev, sgl, sgc, DMA_FROM_DEVICE);
 	if (dmacount == 0) {
 		e_err(drv, "xid 0x%x DMA map error\n", xid);
-		return 0;
+		goto out_noddp;
 	}
 
 	/* alloc the udl from per cpu ddp pool */
-	cpu = get_cpu();
-	pool = *per_cpu_ptr(fcoe->pool, cpu);
-	ddp->udl = pci_pool_alloc(pool, GFP_ATOMIC, &ddp->udp);
+	ddp->udl = dma_pool_alloc(ddp_pool->pool, GFP_ATOMIC, &ddp->udp);
 	if (!ddp->udl) {
 		e_err(drv, "failed allocated ddp context\n");
 		goto out_noddp_unmap;
 	}
-	ddp->pool = pool;
+	ddp->pool = ddp_pool->pool;
 	ddp->sgl = sgl;
 	ddp->sgc = sgc;
 
@@ -201,7 +205,7 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 		while (len) {
 			/* max number of buffers allowed in one DDP context */
 			if (j >= IXGBE_BUFFCNT_MAX) {
-				*per_cpu_ptr(fcoe->pcpu_noddp, cpu) += 1;
+				ddp_pool->noddp++;
 				goto out_noddp_free;
 			}
 
@@ -241,7 +245,7 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 	 */
 	if (lastsize == bufflen) {
 		if (j >= IXGBE_BUFFCNT_MAX) {
-			*per_cpu_ptr(fcoe->pcpu_noddp_ext_buff, cpu) += 1;
+			ddp_pool->noddp_ext_buff++;
 			goto out_noddp_free;
 		}
 
@@ -293,11 +297,12 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 	return 1;
 
 out_noddp_free:
-	pci_pool_free(pool, ddp->udl, ddp->udp);
+	dma_pool_free(ddp->pool, ddp->udl, ddp->udp);
 	ixgbe_fcoe_clear_ddp(ddp);
 
 out_noddp_unmap:
-	pci_unmap_sg(adapter->pdev, sgl, sgc, DMA_FROM_DEVICE);
+	dma_unmap_sg(&adapter->pdev->dev, sgl, sgc, DMA_FROM_DEVICE);
+out_noddp:
 	put_cpu();
 	return 0;
 }
@@ -409,7 +414,7 @@ int ixgbe_fcoe_ddp(struct ixgbe_adapter *adapter,
 		break;
 	/* unmap the sg list when FCPRSP is received */
 	case __constant_cpu_to_le32(IXGBE_RXDADV_STAT_FCSTAT_FCPRSP):
-		pci_unmap_sg(adapter->pdev, ddp->sgl,
+		dma_unmap_sg(&adapter->pdev->dev, ddp->sgl,
 			     ddp->sgc, DMA_FROM_DEVICE);
 		ddp->err = ddp_err;
 		ddp->sgl = NULL;
@@ -563,44 +568,37 @@ int ixgbe_fso(struct ixgbe_ring *tx_ring,
 	return 0;
 }
 
-static void ixgbe_fcoe_ddp_pools_free(struct ixgbe_fcoe *fcoe)
+static void ixgbe_fcoe_dma_pool_free(struct ixgbe_fcoe *fcoe, unsigned int cpu)
 {
-	unsigned int cpu;
-	struct pci_pool **pool;
+	struct ixgbe_fcoe_ddp_pool *ddp_pool;
 
-	for_each_possible_cpu(cpu) {
-		pool = per_cpu_ptr(fcoe->pool, cpu);
-		if (*pool)
-			pci_pool_destroy(*pool);
-	}
-	free_percpu(fcoe->pool);
-	fcoe->pool = NULL;
+	ddp_pool = per_cpu_ptr(fcoe->ddp_pool, cpu);
+	if (ddp_pool->pool)
+		dma_pool_destroy(ddp_pool->pool);
+	ddp_pool->pool = NULL;
 }
 
-static void ixgbe_fcoe_ddp_pools_alloc(struct ixgbe_adapter *adapter)
+static int ixgbe_fcoe_dma_pool_alloc(struct ixgbe_fcoe *fcoe,
+				     struct device *dev,
+				     unsigned int cpu)
 {
-	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
-	unsigned int cpu;
-	struct pci_pool **pool;
+	struct ixgbe_fcoe_ddp_pool *ddp_pool;
+	struct dma_pool *pool;
 	char pool_name[32];
 
-	fcoe->pool = alloc_percpu(struct pci_pool *);
-	if (!fcoe->pool)
-		return;
+	snprintf(pool_name, 32, "ixgbe_fcoe_ddp_%d", cpu);
 
-	/* allocate pci pool for each cpu */
-	for_each_possible_cpu(cpu) {
-		snprintf(pool_name, 32, "ixgbe_fcoe_ddp_%d", cpu);
-		pool = per_cpu_ptr(fcoe->pool, cpu);
-		*pool = pci_pool_create(pool_name,
-					adapter->pdev, IXGBE_FCPTR_MAX,
-					IXGBE_FCPTR_ALIGN, PAGE_SIZE);
-		if (!*pool) {
-			e_err(drv, "failed to alloc DDP pool on cpu:%d\n", cpu);
-			ixgbe_fcoe_ddp_pools_free(fcoe);
-			return;
-		}
-	}
+	pool = dma_pool_create(pool_name, dev, IXGBE_FCPTR_MAX,
+			       IXGBE_FCPTR_ALIGN, PAGE_SIZE);
+	if (!pool)
+		return -ENOMEM;
+
+	ddp_pool = per_cpu_ptr(fcoe->ddp_pool, cpu);
+	ddp_pool->pool = pool;
+	ddp_pool->noddp = 0;
+	ddp_pool->noddp_ext_buff = 0;
+
+	return 0;
 }
 
 /**
@@ -613,132 +611,171 @@ static void ixgbe_fcoe_ddp_pools_alloc(struct ixgbe_adapter *adapter)
  */
 void ixgbe_configure_fcoe(struct ixgbe_adapter *adapter)
 {
-	int i, fcoe_q, fcoe_i;
+	struct ixgbe_ring_feature *fcoe = &adapter->ring_feature[RING_F_FCOE];
 	struct ixgbe_hw *hw = &adapter->hw;
-	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
-	struct ixgbe_ring_feature *f = &adapter->ring_feature[RING_F_FCOE];
-	unsigned int cpu;
-
-	if (!fcoe->pool) {
-		spin_lock_init(&fcoe->lock);
-
-		ixgbe_fcoe_ddp_pools_alloc(adapter);
-		if (!fcoe->pool) {
-			e_err(drv, "failed to alloc percpu fcoe DDP pools\n");
-			return;
-		}
-
-		/* Extra buffer to be shared by all DDPs for HW work around */
-		fcoe->extra_ddp_buffer = kmalloc(IXGBE_FCBUFF_MIN, GFP_ATOMIC);
-		if (fcoe->extra_ddp_buffer == NULL) {
-			e_err(drv, "failed to allocated extra DDP buffer\n");
-			goto out_ddp_pools;
-		}
+	int i, fcoe_q, fcoe_i;
+	u32 etqf;
 
-		fcoe->extra_ddp_buffer_dma =
-			dma_map_single(&adapter->pdev->dev,
-				       fcoe->extra_ddp_buffer,
-				       IXGBE_FCBUFF_MIN,
-				       DMA_FROM_DEVICE);
-		if (dma_mapping_error(&adapter->pdev->dev,
-				      fcoe->extra_ddp_buffer_dma)) {
-			e_err(drv, "failed to map extra DDP buffer\n");
-			goto out_extra_ddp_buffer;
-		}
+	/* Minimal functionality for FCoE requires at least CRC offloads */
+	if (!(adapter->netdev->features & NETIF_F_FCOE_CRC))
+		return;
 
-		/* Alloc per cpu mem to count the ddp alloc failure number */
-		fcoe->pcpu_noddp = alloc_percpu(u64);
-		if (!fcoe->pcpu_noddp) {
-			e_err(drv, "failed to alloc noddp counter\n");
-			goto out_pcpu_noddp_alloc_fail;
-		}
+	/* Enable L2 EtherType filter for FCoE, needed for FCoE CRC and DDP */
+	etqf = ETH_P_FCOE | IXGBE_ETQF_FCOE | IXGBE_ETQF_FILTER_EN;
+	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
+		etqf |= IXGBE_ETQF_POOL_ENABLE;
+		etqf |= VMDQ_P(0) << IXGBE_ETQF_POOL_SHIFT;
+	}
+	IXGBE_WRITE_REG(hw, IXGBE_ETQF(IXGBE_ETQF_FILTER_FCOE), etqf);
+	IXGBE_WRITE_REG(hw, IXGBE_ETQS(IXGBE_ETQF_FILTER_FCOE), 0);
 
-		fcoe->pcpu_noddp_ext_buff = alloc_percpu(u64);
-		if (!fcoe->pcpu_noddp_ext_buff) {
-			e_err(drv, "failed to alloc noddp extra buff cnt\n");
-			goto out_pcpu_noddp_extra_buff_alloc_fail;
-		}
+	/* leave registers un-configured if FCoE is disabled */
+	if (!(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
+		return;
 
-		for_each_possible_cpu(cpu) {
-			*per_cpu_ptr(fcoe->pcpu_noddp, cpu) = 0;
-			*per_cpu_ptr(fcoe->pcpu_noddp_ext_buff, cpu) = 0;
-		}
+	/* Use one or more Rx queues for FCoE by redirection table */
+	for (i = 0; i < IXGBE_FCRETA_SIZE; i++) {
+		fcoe_i = fcoe->offset + (i % fcoe->indices);
+		fcoe_i &= IXGBE_FCRETA_ENTRY_MASK;
+		fcoe_q = adapter->rx_ring[fcoe_i]->reg_idx;
+		IXGBE_WRITE_REG(hw, IXGBE_FCRETA(i), fcoe_q);
 	}
+	IXGBE_WRITE_REG(hw, IXGBE_FCRECTL, IXGBE_FCRECTL_ENA);
 
-	/* Enable L2 eth type filter for FCoE */
-	IXGBE_WRITE_REG(hw, IXGBE_ETQF(IXGBE_ETQF_FILTER_FCOE),
-			(ETH_P_FCOE | IXGBE_ETQF_FCOE | IXGBE_ETQF_FILTER_EN));
-	/* Enable L2 eth type filter for FIP */
-	IXGBE_WRITE_REG(hw, IXGBE_ETQF(IXGBE_ETQF_FILTER_FIP),
-			(ETH_P_FIP | IXGBE_ETQF_FILTER_EN));
-	if (adapter->ring_feature[RING_F_FCOE].indices) {
-		/* Use multiple rx queues for FCoE by redirection table */
-		for (i = 0; i < IXGBE_FCRETA_SIZE; i++) {
-			fcoe_i = f->mask + i % f->indices;
-			fcoe_i &= IXGBE_FCRETA_ENTRY_MASK;
-			fcoe_q = adapter->rx_ring[fcoe_i]->reg_idx;
-			IXGBE_WRITE_REG(hw, IXGBE_FCRETA(i), fcoe_q);
-		}
-		IXGBE_WRITE_REG(hw, IXGBE_FCRECTL, IXGBE_FCRECTL_ENA);
-		IXGBE_WRITE_REG(hw, IXGBE_ETQS(IXGBE_ETQF_FILTER_FCOE), 0);
-	} else  {
-		/* Use single rx queue for FCoE */
-		fcoe_i = f->mask;
-		fcoe_q = adapter->rx_ring[fcoe_i]->reg_idx;
-		IXGBE_WRITE_REG(hw, IXGBE_FCRECTL, 0);
-		IXGBE_WRITE_REG(hw, IXGBE_ETQS(IXGBE_ETQF_FILTER_FCOE),
-				IXGBE_ETQS_QUEUE_EN |
-				(fcoe_q << IXGBE_ETQS_RX_QUEUE_SHIFT));
+	/* Enable L2 EtherType filter for FIP */
+	etqf = ETH_P_FIP | IXGBE_ETQF_FILTER_EN;
+	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
+		etqf |= IXGBE_ETQF_POOL_ENABLE;
+		etqf |= VMDQ_P(0) << IXGBE_ETQF_POOL_SHIFT;
 	}
-	/* send FIP frames to the first FCoE queue */
-	fcoe_i = f->mask;
-	fcoe_q = adapter->rx_ring[fcoe_i]->reg_idx;
+	IXGBE_WRITE_REG(hw, IXGBE_ETQF(IXGBE_ETQF_FILTER_FIP), etqf);
+
+	/* Send FIP frames to the first FCoE queue */
+	fcoe_q = adapter->rx_ring[fcoe->offset]->reg_idx;
 	IXGBE_WRITE_REG(hw, IXGBE_ETQS(IXGBE_ETQF_FILTER_FIP),
 			IXGBE_ETQS_QUEUE_EN |
 			(fcoe_q << IXGBE_ETQS_RX_QUEUE_SHIFT));
 
-	IXGBE_WRITE_REG(hw, IXGBE_FCRXCTRL, IXGBE_FCRXCTRL_FCCRCBO |
+	/* Configure FCoE Rx control */
+	IXGBE_WRITE_REG(hw, IXGBE_FCRXCTRL,
+			IXGBE_FCRXCTRL_FCCRCBO |
 			(FC_FCOE_VER << IXGBE_FCRXCTRL_FCOEVER_SHIFT));
-	return;
-out_pcpu_noddp_extra_buff_alloc_fail:
-	free_percpu(fcoe->pcpu_noddp);
-out_pcpu_noddp_alloc_fail:
-	dma_unmap_single(&adapter->pdev->dev,
-			 fcoe->extra_ddp_buffer_dma,
-			 IXGBE_FCBUFF_MIN,
-			 DMA_FROM_DEVICE);
-out_extra_ddp_buffer:
-	kfree(fcoe->extra_ddp_buffer);
-out_ddp_pools:
-	ixgbe_fcoe_ddp_pools_free(fcoe);
 }
 
 /**
- * ixgbe_cleanup_fcoe - release all fcoe ddp context resources
+ * ixgbe_free_fcoe_ddp_resources - release all fcoe ddp context resources
  * @adapter : ixgbe adapter
  *
  * Cleans up outstanding ddp context resources
  *
  * Returns : none
  */
-void ixgbe_cleanup_fcoe(struct ixgbe_adapter *adapter)
+void ixgbe_free_fcoe_ddp_resources(struct ixgbe_adapter *adapter)
 {
-	int i;
 	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
+	int cpu, i;
 
-	if (!fcoe->pool)
+	/* do nothing if no DDP pools were allocated */
+	if (!fcoe->ddp_pool)
 		return;
 
 	for (i = 0; i < IXGBE_FCOE_DDP_MAX; i++)
 		ixgbe_fcoe_ddp_put(adapter->netdev, i);
+
+	for_each_possible_cpu(cpu)
+		ixgbe_fcoe_dma_pool_free(fcoe, cpu);
+
 	dma_unmap_single(&adapter->pdev->dev,
 			 fcoe->extra_ddp_buffer_dma,
 			 IXGBE_FCBUFF_MIN,
 			 DMA_FROM_DEVICE);
-	free_percpu(fcoe->pcpu_noddp);
-	free_percpu(fcoe->pcpu_noddp_ext_buff);
 	kfree(fcoe->extra_ddp_buffer);
-	ixgbe_fcoe_ddp_pools_free(fcoe);
+
+	fcoe->extra_ddp_buffer = NULL;
+	fcoe->extra_ddp_buffer_dma = 0;
+}
+
+/**
+ * ixgbe_setup_fcoe_ddp_resources - setup all fcoe ddp context resources
+ * @adapter: ixgbe adapter
+ *
+ * Sets up ddp context resouces
+ *
+ * Returns : 0 indicates success or -EINVAL on failure
+ */
+int ixgbe_setup_fcoe_ddp_resources(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
+	struct device *dev = &adapter->pdev->dev;
+	void *buffer;
+	dma_addr_t dma;
+	unsigned int cpu;
+
+	/* do nothing if no DDP pools were allocated */
+	if (!fcoe->ddp_pool)
+		return 0;
+
+	/* Extra buffer to be shared by all DDPs for HW work around */
+	buffer = kmalloc(IXGBE_FCBUFF_MIN, GFP_ATOMIC);
+	if (!buffer) {
+		e_err(drv, "failed to allocate extra DDP buffer\n");
+		return -ENOMEM;
+	}
+
+	dma = dma_map_single(dev, buffer, IXGBE_FCBUFF_MIN, DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, dma)) {
+		e_err(drv, "failed to map extra DDP buffer\n");
+		kfree(buffer);
+		return -ENOMEM;
+	}
+
+	fcoe->extra_ddp_buffer = buffer;
+	fcoe->extra_ddp_buffer_dma = dma;
+
+	/* allocate pci pool for each cpu */
+	for_each_possible_cpu(cpu) {
+		int err = ixgbe_fcoe_dma_pool_alloc(fcoe, dev, cpu);
+		if (!err)
+			continue;
+
+		e_err(drv, "failed to alloc DDP pool on cpu:%d\n", cpu);
+		ixgbe_free_fcoe_ddp_resources(adapter);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int ixgbe_fcoe_ddp_enable(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
+
+	if (!(adapter->flags & IXGBE_FLAG_FCOE_CAPABLE))
+		return -EINVAL;
+
+	fcoe->ddp_pool = alloc_percpu(struct ixgbe_fcoe_ddp_pool);
+
+	if (!fcoe->ddp_pool) {
+		e_err(drv, "failed to allocate percpu DDP resources\n");
+		return -ENOMEM;
+	}
+
+	adapter->netdev->fcoe_ddp_xid = IXGBE_FCOE_DDP_MAX - 1;
+
+	return 0;
+}
+
+static void ixgbe_fcoe_ddp_disable(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
+
+	adapter->netdev->fcoe_ddp_xid = 0;
+
+	if (!fcoe->ddp_pool)
+		return;
+
+	free_percpu(fcoe->ddp_pool);
+	fcoe->ddp_pool = NULL;
 }
 
 /**
@@ -751,40 +788,37 @@ void ixgbe_cleanup_fcoe(struct ixgbe_adapter *adapter)
  */
 int ixgbe_fcoe_enable(struct net_device *netdev)
 {
-	int rc = -EINVAL;
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
 
+	atomic_inc(&fcoe->refcnt);
 
 	if (!(adapter->flags & IXGBE_FLAG_FCOE_CAPABLE))
-		goto out_enable;
+		return -EINVAL;
 
-	atomic_inc(&fcoe->refcnt);
 	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)
-		goto out_enable;
+		return -EINVAL;
 
 	e_info(drv, "Enabling FCoE offload features.\n");
 	if (netif_running(netdev))
 		netdev->netdev_ops->ndo_stop(netdev);
 
-	ixgbe_clear_interrupt_scheme(adapter);
+	/* Allocate per CPU memory to track DDP pools */
+	ixgbe_fcoe_ddp_enable(adapter);
 
+	/* enable FCoE and notify stack */
 	adapter->flags |= IXGBE_FLAG_FCOE_ENABLED;
-	adapter->ring_feature[RING_F_FCOE].indices = IXGBE_FCRETA_SIZE;
-	netdev->features |= NETIF_F_FCOE_CRC;
-	netdev->features |= NETIF_F_FSO;
 	netdev->features |= NETIF_F_FCOE_MTU;
-	netdev->fcoe_ddp_xid = IXGBE_FCOE_DDP_MAX - 1;
+	netdev_features_change(netdev);
 
+	/* release existing queues and reallocate them */
+	ixgbe_clear_interrupt_scheme(adapter);
 	ixgbe_init_interrupt_scheme(adapter);
-	netdev_features_change(netdev);
 
 	if (netif_running(netdev))
 		netdev->netdev_ops->ndo_open(netdev);
-	rc = 0;
 
-out_enable:
-	return rc;
+	return 0;
 }
 
 /**
@@ -797,41 +831,35 @@ out_enable:
  */
 int ixgbe_fcoe_disable(struct net_device *netdev)
 {
-	int rc = -EINVAL;
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
-	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
 
-	if (!(adapter->flags & IXGBE_FLAG_FCOE_CAPABLE))
-		goto out_disable;
+	if (!atomic_dec_and_test(&adapter->fcoe.refcnt))
+		return -EINVAL;
 
 	if (!(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
-		goto out_disable;
-
-	if (!atomic_dec_and_test(&fcoe->refcnt))
-		goto out_disable;
+		return -EINVAL;
 
 	e_info(drv, "Disabling FCoE offload features.\n");
-	netdev->features &= ~NETIF_F_FCOE_CRC;
-	netdev->features &= ~NETIF_F_FSO;
-	netdev->features &= ~NETIF_F_FCOE_MTU;
-	netdev->fcoe_ddp_xid = 0;
-	netdev_features_change(netdev);
-
 	if (netif_running(netdev))
 		netdev->netdev_ops->ndo_stop(netdev);
 
-	ixgbe_clear_interrupt_scheme(adapter);
+	/* Free per CPU memory to track DDP pools */
+	ixgbe_fcoe_ddp_disable(adapter);
+
+	/* disable FCoE and notify stack */
 	adapter->flags &= ~IXGBE_FLAG_FCOE_ENABLED;
-	adapter->ring_feature[RING_F_FCOE].indices = 0;
-	ixgbe_cleanup_fcoe(adapter);
+	netdev->features &= ~NETIF_F_FCOE_MTU;
+
+	netdev_features_change(netdev);
+
+	/* release existing queues and reallocate them */
+	ixgbe_clear_interrupt_scheme(adapter);
 	ixgbe_init_interrupt_scheme(adapter);
 
 	if (netif_running(netdev))
 		netdev->netdev_ops->ndo_open(netdev);
-	rc = 0;
 
-out_disable:
-	return rc;
+	return 0;
 }
 
 /**
@@ -960,3 +988,18 @@ int ixgbe_fcoe_get_hbainfo(struct net_device *netdev,
 
 	return 0;
 }
+
+/**
+ * ixgbe_fcoe_get_tc - get the current TC that fcoe is mapped to
+ * @adapter - pointer to the device adapter structure
+ *
+ * Return : TC that FCoE is mapped to
+ */
+u8 ixgbe_fcoe_get_tc(struct ixgbe_adapter *adapter)
+{
+#ifdef CONFIG_IXGBE_DCB
+	return netdev_get_prio_tc_map(adapter->netdev, adapter->fcoe.up);
+#else
+	return 0;
+#endif
+}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h
index 1dbed17c810..bf724da9937 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.h
@@ -62,19 +62,24 @@ struct ixgbe_fcoe_ddp {
 	struct scatterlist *sgl;
 	dma_addr_t udp;
 	u64 *udl;
-	struct pci_pool *pool;
+	struct dma_pool *pool;
+};
+
+/* per cpu variables */
+struct ixgbe_fcoe_ddp_pool {
+	struct dma_pool *pool;
+	u64 noddp;
+	u64 noddp_ext_buff;
 };
 
 struct ixgbe_fcoe {
-	struct pci_pool **pool;
+	struct ixgbe_fcoe_ddp_pool __percpu *ddp_pool;
 	atomic_t refcnt;
 	spinlock_t lock;
 	struct ixgbe_fcoe_ddp ddp[IXGBE_FCOE_DDP_MAX];
-	unsigned char *extra_ddp_buffer;
+	void *extra_ddp_buffer;
 	dma_addr_t extra_ddp_buffer_dma;
 	unsigned long mode;
-	u64 __percpu *pcpu_noddp;
-	u64 __percpu *pcpu_noddp_ext_buff;
 #ifdef CONFIG_IXGBE_DCB
 	u8 up;
 #endif
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
index c377706e81a..17ecbcedd54 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
@@ -28,28 +28,83 @@
 #include "ixgbe.h"
 #include "ixgbe_sriov.h"
 
+#ifdef CONFIG_IXGBE_DCB
 /**
- * ixgbe_cache_ring_rss - Descriptor ring to register mapping for RSS
+ * ixgbe_cache_ring_dcb_sriov - Descriptor ring to register mapping for SR-IOV
  * @adapter: board private structure to initialize
  *
- * Cache the descriptor ring offsets for RSS to the assigned rings.
+ * Cache the descriptor ring offsets for SR-IOV to the assigned rings.  It
+ * will also try to cache the proper offsets if RSS/FCoE are enabled along
+ * with VMDq.
  *
  **/
-static inline bool ixgbe_cache_ring_rss(struct ixgbe_adapter *adapter)
+static bool ixgbe_cache_ring_dcb_sriov(struct ixgbe_adapter *adapter)
 {
+#ifdef IXGBE_FCOE
+	struct ixgbe_ring_feature *fcoe = &adapter->ring_feature[RING_F_FCOE];
+#endif /* IXGBE_FCOE */
+	struct ixgbe_ring_feature *vmdq = &adapter->ring_feature[RING_F_VMDQ];
 	int i;
+	u16 reg_idx;
+	u8 tcs = netdev_get_num_tc(adapter->netdev);
 
-	if (!(adapter->flags & IXGBE_FLAG_RSS_ENABLED))
+	/* verify we have DCB queueing enabled before proceeding */
+	if (tcs <= 1)
 		return false;
 
-	for (i = 0; i < adapter->num_rx_queues; i++)
-		adapter->rx_ring[i]->reg_idx = i;
-	for (i = 0; i < adapter->num_tx_queues; i++)
-		adapter->tx_ring[i]->reg_idx = i;
+	/* verify we have VMDq enabled before proceeding */
+	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
+		return false;
+
+	/* start at VMDq register offset for SR-IOV enabled setups */
+	reg_idx = vmdq->offset * __ALIGN_MASK(1, ~vmdq->mask);
+	for (i = 0; i < adapter->num_rx_queues; i++, reg_idx++) {
+		/* If we are greater than indices move to next pool */
+		if ((reg_idx & ~vmdq->mask) >= tcs)
+			reg_idx = __ALIGN_MASK(reg_idx, ~vmdq->mask);
+		adapter->rx_ring[i]->reg_idx = reg_idx;
+	}
+
+	reg_idx = vmdq->offset * __ALIGN_MASK(1, ~vmdq->mask);
+	for (i = 0; i < adapter->num_tx_queues; i++, reg_idx++) {
+		/* If we are greater than indices move to next pool */
+		if ((reg_idx & ~vmdq->mask) >= tcs)
+			reg_idx = __ALIGN_MASK(reg_idx, ~vmdq->mask);
+		adapter->tx_ring[i]->reg_idx = reg_idx;
+	}
+
+#ifdef IXGBE_FCOE
+	/* nothing to do if FCoE is disabled */
+	if (!(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
+		return true;
+
+	/* The work is already done if the FCoE ring is shared */
+	if (fcoe->offset < tcs)
+		return true;
+
+	/* The FCoE rings exist separately, we need to move their reg_idx */
+	if (fcoe->indices) {
+		u16 queues_per_pool = __ALIGN_MASK(1, ~vmdq->mask);
+		u8 fcoe_tc = ixgbe_fcoe_get_tc(adapter);
+
+		reg_idx = (vmdq->offset + vmdq->indices) * queues_per_pool;
+		for (i = fcoe->offset; i < adapter->num_rx_queues; i++) {
+			reg_idx = __ALIGN_MASK(reg_idx, ~vmdq->mask) + fcoe_tc;
+			adapter->rx_ring[i]->reg_idx = reg_idx;
+			reg_idx++;
+		}
+
+		reg_idx = (vmdq->offset + vmdq->indices) * queues_per_pool;
+		for (i = fcoe->offset; i < adapter->num_tx_queues; i++) {
+			reg_idx = __ALIGN_MASK(reg_idx, ~vmdq->mask) + fcoe_tc;
+			adapter->tx_ring[i]->reg_idx = reg_idx;
+			reg_idx++;
+		}
+	}
 
+#endif /* IXGBE_FCOE */
 	return true;
 }
-#ifdef CONFIG_IXGBE_DCB
 
 /* ixgbe_get_first_reg_idx - Return first register index associated with ring */
 static void ixgbe_get_first_reg_idx(struct ixgbe_adapter *adapter, u8 tc,
@@ -64,42 +119,37 @@ static void ixgbe_get_first_reg_idx(struct ixgbe_adapter *adapter, u8 tc,
 
 	switch (hw->mac.type) {
 	case ixgbe_mac_82598EB:
-		*tx = tc << 2;
-		*rx = tc << 3;
+		/* TxQs/TC: 4	RxQs/TC: 8 */
+		*tx = tc << 2; /* 0, 4,  8, 12, 16, 20, 24, 28 */
+		*rx = tc << 3; /* 0, 8, 16, 24, 32, 40, 48, 56 */
 		break;
 	case ixgbe_mac_82599EB:
 	case ixgbe_mac_X540:
 		if (num_tcs > 4) {
-			if (tc < 3) {
-				*tx = tc << 5;
-				*rx = tc << 4;
-			} else if (tc <  5) {
-				*tx = ((tc + 2) << 4);
-				*rx = tc << 4;
-			} else if (tc < num_tcs) {
-				*tx = ((tc + 8) << 3);
-				*rx = tc << 4;
-			}
+			/*
+			 * TCs    : TC0/1 TC2/3 TC4-7
+			 * TxQs/TC:    32    16     8
+			 * RxQs/TC:    16    16    16
+			 */
+			*rx = tc << 4;
+			if (tc < 3)
+				*tx = tc << 5;		/*   0,  32,  64 */
+			else if (tc < 5)
+				*tx = (tc + 2) << 4;	/*  80,  96 */
+			else
+				*tx = (tc + 8) << 3;	/* 104, 112, 120 */
 		} else {
-			*rx =  tc << 5;
-			switch (tc) {
-			case 0:
-				*tx =  0;
-				break;
-			case 1:
-				*tx = 64;
-				break;
-			case 2:
-				*tx = 96;
-				break;
-			case 3:
-				*tx = 112;
-				break;
-			default:
-				break;
-			}
+			/*
+			 * TCs    : TC0 TC1 TC2/3
+			 * TxQs/TC:  64  32    16
+			 * RxQs/TC:  32  32    32
+			 */
+			*rx = tc << 5;
+			if (tc < 2)
+				*tx = tc << 6;		/*  0,  64 */
+			else
+				*tx = (tc + 4) << 4;	/* 96, 112 */
 		}
-		break;
 	default:
 		break;
 	}
@@ -112,106 +162,115 @@ static void ixgbe_get_first_reg_idx(struct ixgbe_adapter *adapter, u8 tc,
  * Cache the descriptor ring offsets for DCB to the assigned rings.
  *
  **/
-static inline bool ixgbe_cache_ring_dcb(struct ixgbe_adapter *adapter)
+static bool ixgbe_cache_ring_dcb(struct ixgbe_adapter *adapter)
 {
 	struct net_device *dev = adapter->netdev;
-	int i, j, k;
+	unsigned int tx_idx, rx_idx;
+	int tc, offset, rss_i, i;
 	u8 num_tcs = netdev_get_num_tc(dev);
 
-	if (!num_tcs)
+	/* verify we have DCB queueing enabled before proceeding */
+	if (num_tcs <= 1)
 		return false;
 
-	for (i = 0, k = 0; i < num_tcs; i++) {
-		unsigned int tx_s, rx_s;
-		u16 count = dev->tc_to_txq[i].count;
+	rss_i = adapter->ring_feature[RING_F_RSS].indices;
 
-		ixgbe_get_first_reg_idx(adapter, i, &tx_s, &rx_s);
-		for (j = 0; j < count; j++, k++) {
-			adapter->tx_ring[k]->reg_idx = tx_s + j;
-			adapter->rx_ring[k]->reg_idx = rx_s + j;
-			adapter->tx_ring[k]->dcb_tc = i;
-			adapter->rx_ring[k]->dcb_tc = i;
+	for (tc = 0, offset = 0; tc < num_tcs; tc++, offset += rss_i) {
+		ixgbe_get_first_reg_idx(adapter, tc, &tx_idx, &rx_idx);
+		for (i = 0; i < rss_i; i++, tx_idx++, rx_idx++) {
+			adapter->tx_ring[offset + i]->reg_idx = tx_idx;
+			adapter->rx_ring[offset + i]->reg_idx = rx_idx;
+			adapter->tx_ring[offset + i]->dcb_tc = tc;
+			adapter->rx_ring[offset + i]->dcb_tc = tc;
 		}
 	}
 
 	return true;
 }
-#endif
 
+#endif
 /**
- * ixgbe_cache_ring_fdir - Descriptor ring to register mapping for Flow Director
+ * ixgbe_cache_ring_sriov - Descriptor ring to register mapping for sriov
  * @adapter: board private structure to initialize
  *
- * Cache the descriptor ring offsets for Flow Director to the assigned rings.
+ * SR-IOV doesn't use any descriptor rings but changes the default if
+ * no other mapping is used.
  *
- **/
-static inline bool ixgbe_cache_ring_fdir(struct ixgbe_adapter *adapter)
+ */
+static bool ixgbe_cache_ring_sriov(struct ixgbe_adapter *adapter)
 {
+#ifdef IXGBE_FCOE
+	struct ixgbe_ring_feature *fcoe = &adapter->ring_feature[RING_F_FCOE];
+#endif /* IXGBE_FCOE */
+	struct ixgbe_ring_feature *vmdq = &adapter->ring_feature[RING_F_VMDQ];
+	struct ixgbe_ring_feature *rss = &adapter->ring_feature[RING_F_RSS];
 	int i;
-	bool ret = false;
-
-	if ((adapter->flags & IXGBE_FLAG_RSS_ENABLED) &&
-	    (adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE)) {
-		for (i = 0; i < adapter->num_rx_queues; i++)
-			adapter->rx_ring[i]->reg_idx = i;
-		for (i = 0; i < adapter->num_tx_queues; i++)
-			adapter->tx_ring[i]->reg_idx = i;
-		ret = true;
+	u16 reg_idx;
+
+	/* only proceed if VMDq is enabled */
+	if (!(adapter->flags & IXGBE_FLAG_VMDQ_ENABLED))
+		return false;
+
+	/* start at VMDq register offset for SR-IOV enabled setups */
+	reg_idx = vmdq->offset * __ALIGN_MASK(1, ~vmdq->mask);
+	for (i = 0; i < adapter->num_rx_queues; i++, reg_idx++) {
+#ifdef IXGBE_FCOE
+		/* Allow first FCoE queue to be mapped as RSS */
+		if (fcoe->offset && (i > fcoe->offset))
+			break;
+#endif
+		/* If we are greater than indices move to next pool */
+		if ((reg_idx & ~vmdq->mask) >= rss->indices)
+			reg_idx = __ALIGN_MASK(reg_idx, ~vmdq->mask);
+		adapter->rx_ring[i]->reg_idx = reg_idx;
 	}
 
-	return ret;
-}
+#ifdef IXGBE_FCOE
+	/* FCoE uses a linear block of queues so just assigning 1:1 */
+	for (; i < adapter->num_rx_queues; i++, reg_idx++)
+		adapter->rx_ring[i]->reg_idx = reg_idx;
 
+#endif
+	reg_idx = vmdq->offset * __ALIGN_MASK(1, ~vmdq->mask);
+	for (i = 0; i < adapter->num_tx_queues; i++, reg_idx++) {
 #ifdef IXGBE_FCOE
-/**
- * ixgbe_cache_ring_fcoe - Descriptor ring to register mapping for the FCoE
- * @adapter: board private structure to initialize
- *
- * Cache the descriptor ring offsets for FCoE mode to the assigned rings.
- *
- */
-static inline bool ixgbe_cache_ring_fcoe(struct ixgbe_adapter *adapter)
-{
-	struct ixgbe_ring_feature *f = &adapter->ring_feature[RING_F_FCOE];
-	int i;
-	u8 fcoe_rx_i = 0, fcoe_tx_i = 0;
+		/* Allow first FCoE queue to be mapped as RSS */
+		if (fcoe->offset && (i > fcoe->offset))
+			break;
+#endif
+		/* If we are greater than indices move to next pool */
+		if ((reg_idx & rss->mask) >= rss->indices)
+			reg_idx = __ALIGN_MASK(reg_idx, ~vmdq->mask);
+		adapter->tx_ring[i]->reg_idx = reg_idx;
+	}
 
-	if (!(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
-		return false;
+#ifdef IXGBE_FCOE
+	/* FCoE uses a linear block of queues so just assigning 1:1 */
+	for (; i < adapter->num_tx_queues; i++, reg_idx++)
+		adapter->tx_ring[i]->reg_idx = reg_idx;
 
-	if (adapter->flags & IXGBE_FLAG_RSS_ENABLED) {
-		if (adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE)
-			ixgbe_cache_ring_fdir(adapter);
-		else
-			ixgbe_cache_ring_rss(adapter);
+#endif
 
-		fcoe_rx_i = f->mask;
-		fcoe_tx_i = f->mask;
-	}
-	for (i = 0; i < f->indices; i++, fcoe_rx_i++, fcoe_tx_i++) {
-		adapter->rx_ring[f->mask + i]->reg_idx = fcoe_rx_i;
-		adapter->tx_ring[f->mask + i]->reg_idx = fcoe_tx_i;
-	}
 	return true;
 }
 
-#endif /* IXGBE_FCOE */
 /**
- * ixgbe_cache_ring_sriov - Descriptor ring to register mapping for sriov
+ * ixgbe_cache_ring_rss - Descriptor ring to register mapping for RSS
  * @adapter: board private structure to initialize
  *
- * SR-IOV doesn't use any descriptor rings but changes the default if
- * no other mapping is used.
+ * Cache the descriptor ring offsets for RSS to the assigned rings.
  *
- */
-static inline bool ixgbe_cache_ring_sriov(struct ixgbe_adapter *adapter)
+ **/
+static bool ixgbe_cache_ring_rss(struct ixgbe_adapter *adapter)
 {
-	adapter->rx_ring[0]->reg_idx = adapter->num_vfs * 2;
-	adapter->tx_ring[0]->reg_idx = adapter->num_vfs * 2;
-	if (adapter->num_vfs)
-		return true;
-	else
-		return false;
+	int i;
+
+	for (i = 0; i < adapter->num_rx_queues; i++)
+		adapter->rx_ring[i]->reg_idx = i;
+	for (i = 0; i < adapter->num_tx_queues; i++)
+		adapter->tx_ring[i]->reg_idx = i;
+
+	return true;
 }
 
 /**
@@ -231,186 +290,384 @@ static void ixgbe_cache_ring_register(struct ixgbe_adapter *adapter)
 	adapter->rx_ring[0]->reg_idx = 0;
 	adapter->tx_ring[0]->reg_idx = 0;
 
-	if (ixgbe_cache_ring_sriov(adapter))
-		return;
-
 #ifdef CONFIG_IXGBE_DCB
-	if (ixgbe_cache_ring_dcb(adapter))
+	if (ixgbe_cache_ring_dcb_sriov(adapter))
 		return;
-#endif
 
-#ifdef IXGBE_FCOE
-	if (ixgbe_cache_ring_fcoe(adapter))
+	if (ixgbe_cache_ring_dcb(adapter))
 		return;
-#endif /* IXGBE_FCOE */
 
-	if (ixgbe_cache_ring_fdir(adapter))
+#endif
+	if (ixgbe_cache_ring_sriov(adapter))
 		return;
 
-	if (ixgbe_cache_ring_rss(adapter))
-		return;
+	ixgbe_cache_ring_rss(adapter);
 }
 
-/**
- * ixgbe_set_sriov_queues: Allocate queues for IOV use
- * @adapter: board private structure to initialize
- *
- * IOV doesn't actually use anything, so just NAK the
- * request for now and let the other queue routines
- * figure out what to do.
- */
-static inline bool ixgbe_set_sriov_queues(struct ixgbe_adapter *adapter)
-{
-	return false;
-}
+#define IXGBE_RSS_16Q_MASK	0xF
+#define IXGBE_RSS_8Q_MASK	0x7
+#define IXGBE_RSS_4Q_MASK	0x3
+#define IXGBE_RSS_2Q_MASK	0x1
+#define IXGBE_RSS_DISABLED_MASK	0x0
 
+#ifdef CONFIG_IXGBE_DCB
 /**
- * ixgbe_set_rss_queues: Allocate queues for RSS
+ * ixgbe_set_dcb_sriov_queues: Allocate queues for SR-IOV devices w/ DCB
  * @adapter: board private structure to initialize
  *
- * This is our "base" multiqueue mode.  RSS (Receive Side Scaling) will try
- * to allocate one Rx queue per CPU, and if available, one Tx queue per CPU.
+ * When SR-IOV (Single Root IO Virtualiztion) is enabled, allocate queues
+ * and VM pools where appropriate.  Also assign queues based on DCB
+ * priorities and map accordingly..
  *
  **/
-static inline bool ixgbe_set_rss_queues(struct ixgbe_adapter *adapter)
+static bool ixgbe_set_dcb_sriov_queues(struct ixgbe_adapter *adapter)
 {
-	bool ret = false;
-	struct ixgbe_ring_feature *f = &adapter->ring_feature[RING_F_RSS];
-
-	if (adapter->flags & IXGBE_FLAG_RSS_ENABLED) {
-		f->mask = 0xF;
-		adapter->num_rx_queues = f->indices;
-		adapter->num_tx_queues = f->indices;
-		ret = true;
+	int i;
+	u16 vmdq_i = adapter->ring_feature[RING_F_VMDQ].limit;
+	u16 vmdq_m = 0;
+#ifdef IXGBE_FCOE
+	u16 fcoe_i = 0;
+#endif
+	u8 tcs = netdev_get_num_tc(adapter->netdev);
+
+	/* verify we have DCB queueing enabled before proceeding */
+	if (tcs <= 1)
+		return false;
+
+	/* verify we have VMDq enabled before proceeding */
+	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
+		return false;
+
+	/* Add starting offset to total pool count */
+	vmdq_i += adapter->ring_feature[RING_F_VMDQ].offset;
+
+	/* 16 pools w/ 8 TC per pool */
+	if (tcs > 4) {
+		vmdq_i = min_t(u16, vmdq_i, 16);
+		vmdq_m = IXGBE_82599_VMDQ_8Q_MASK;
+	/* 32 pools w/ 4 TC per pool */
+	} else {
+		vmdq_i = min_t(u16, vmdq_i, 32);
+		vmdq_m = IXGBE_82599_VMDQ_4Q_MASK;
 	}
 
-	return ret;
-}
+#ifdef IXGBE_FCOE
+	/* queues in the remaining pools are available for FCoE */
+	fcoe_i = (128 / __ALIGN_MASK(1, ~vmdq_m)) - vmdq_i;
 
-/**
- * ixgbe_set_fdir_queues: Allocate queues for Flow Director
- * @adapter: board private structure to initialize
- *
- * Flow Director is an advanced Rx filter, attempting to get Rx flows back
- * to the original CPU that initiated the Tx session.  This runs in addition
- * to RSS, so if a packet doesn't match an FDIR filter, we can still spread the
- * Rx load across CPUs using RSS.
- *
- **/
-static inline bool ixgbe_set_fdir_queues(struct ixgbe_adapter *adapter)
-{
-	bool ret = false;
-	struct ixgbe_ring_feature *f_fdir = &adapter->ring_feature[RING_F_FDIR];
+#endif
+	/* remove the starting offset from the pool count */
+	vmdq_i -= adapter->ring_feature[RING_F_VMDQ].offset;
 
-	f_fdir->indices = min_t(int, num_online_cpus(), f_fdir->indices);
-	f_fdir->mask = 0;
+	/* save features for later use */
+	adapter->ring_feature[RING_F_VMDQ].indices = vmdq_i;
+	adapter->ring_feature[RING_F_VMDQ].mask = vmdq_m;
 
 	/*
-	 * Use RSS in addition to Flow Director to ensure the best
-	 * distribution of flows across cores, even when an FDIR flow
-	 * isn't matched.
+	 * We do not support DCB, VMDq, and RSS all simultaneously
+	 * so we will disable RSS since it is the lowest priority
 	 */
-	if ((adapter->flags & IXGBE_FLAG_RSS_ENABLED) &&
-	    (adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE)) {
-		adapter->num_tx_queues = f_fdir->indices;
-		adapter->num_rx_queues = f_fdir->indices;
-		ret = true;
-	} else {
-		adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
+	adapter->ring_feature[RING_F_RSS].indices = 1;
+	adapter->ring_feature[RING_F_RSS].mask = IXGBE_RSS_DISABLED_MASK;
+
+	/* disable ATR as it is not supported when VMDq is enabled */
+	adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
+
+	adapter->num_rx_pools = vmdq_i;
+	adapter->num_rx_queues_per_pool = tcs;
+
+	adapter->num_tx_queues = vmdq_i * tcs;
+	adapter->num_rx_queues = vmdq_i * tcs;
+
+#ifdef IXGBE_FCOE
+	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED) {
+		struct ixgbe_ring_feature *fcoe;
+
+		fcoe = &adapter->ring_feature[RING_F_FCOE];
+
+		/* limit ourselves based on feature limits */
+		fcoe_i = min_t(u16, fcoe_i, num_online_cpus());
+		fcoe_i = min_t(u16, fcoe_i, fcoe->limit);
+
+		if (fcoe_i) {
+			/* alloc queues for FCoE separately */
+			fcoe->indices = fcoe_i;
+			fcoe->offset = vmdq_i * tcs;
+
+			/* add queues to adapter */
+			adapter->num_tx_queues += fcoe_i;
+			adapter->num_rx_queues += fcoe_i;
+		} else if (tcs > 1) {
+			/* use queue belonging to FcoE TC */
+			fcoe->indices = 1;
+			fcoe->offset = ixgbe_fcoe_get_tc(adapter);
+		} else {
+			adapter->flags &= ~IXGBE_FLAG_FCOE_ENABLED;
+
+			fcoe->indices = 0;
+			fcoe->offset = 0;
+		}
 	}
-	return ret;
+
+#endif /* IXGBE_FCOE */
+	/* configure TC to queue mapping */
+	for (i = 0; i < tcs; i++)
+		netdev_set_tc_queue(adapter->netdev, i, 1, i);
+
+	return true;
 }
 
+static bool ixgbe_set_dcb_queues(struct ixgbe_adapter *adapter)
+{
+	struct net_device *dev = adapter->netdev;
+	struct ixgbe_ring_feature *f;
+	int rss_i, rss_m, i;
+	int tcs;
+
+	/* Map queue offset and counts onto allocated tx queues */
+	tcs = netdev_get_num_tc(dev);
+
+	/* verify we have DCB queueing enabled before proceeding */
+	if (tcs <= 1)
+		return false;
+
+	/* determine the upper limit for our current DCB mode */
+	rss_i = dev->num_tx_queues / tcs;
+	if (adapter->hw.mac.type == ixgbe_mac_82598EB) {
+		/* 8 TC w/ 4 queues per TC */
+		rss_i = min_t(u16, rss_i, 4);
+		rss_m = IXGBE_RSS_4Q_MASK;
+	} else if (tcs > 4) {
+		/* 8 TC w/ 8 queues per TC */
+		rss_i = min_t(u16, rss_i, 8);
+		rss_m = IXGBE_RSS_8Q_MASK;
+	} else {
+		/* 4 TC w/ 16 queues per TC */
+		rss_i = min_t(u16, rss_i, 16);
+		rss_m = IXGBE_RSS_16Q_MASK;
+	}
+
+	/* set RSS mask and indices */
+	f = &adapter->ring_feature[RING_F_RSS];
+	rss_i = min_t(int, rss_i, f->limit);
+	f->indices = rss_i;
+	f->mask = rss_m;
+
+	/* disable ATR as it is not supported when multiple TCs are enabled */
+	adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
+
 #ifdef IXGBE_FCOE
+	/* FCoE enabled queues require special configuration indexed
+	 * by feature specific indices and offset. Here we map FCoE
+	 * indices onto the DCB queue pairs allowing FCoE to own
+	 * configuration later.
+	 */
+	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED) {
+		u8 tc = ixgbe_fcoe_get_tc(adapter);
+
+		f = &adapter->ring_feature[RING_F_FCOE];
+		f->indices = min_t(u16, rss_i, f->limit);
+		f->offset = rss_i * tc;
+	}
+
+#endif /* IXGBE_FCOE */
+	for (i = 0; i < tcs; i++)
+		netdev_set_tc_queue(dev, i, rss_i, rss_i * i);
+
+	adapter->num_tx_queues = rss_i * tcs;
+	adapter->num_rx_queues = rss_i * tcs;
+
+	return true;
+}
+
+#endif
 /**
- * ixgbe_set_fcoe_queues: Allocate queues for Fiber Channel over Ethernet (FCoE)
+ * ixgbe_set_sriov_queues - Allocate queues for SR-IOV devices
  * @adapter: board private structure to initialize
  *
- * FCoE RX FCRETA can use up to 8 rx queues for up to 8 different exchanges.
- * The ring feature mask is not used as a mask for FCoE, as it can take any 8
- * rx queues out of the max number of rx queues, instead, it is used as the
- * index of the first rx queue used by FCoE.
+ * When SR-IOV (Single Root IO Virtualiztion) is enabled, allocate queues
+ * and VM pools where appropriate.  If RSS is available, then also try and
+ * enable RSS and map accordingly.
  *
  **/
-static inline bool ixgbe_set_fcoe_queues(struct ixgbe_adapter *adapter)
+static bool ixgbe_set_sriov_queues(struct ixgbe_adapter *adapter)
 {
-	struct ixgbe_ring_feature *f = &adapter->ring_feature[RING_F_FCOE];
+	u16 vmdq_i = adapter->ring_feature[RING_F_VMDQ].limit;
+	u16 vmdq_m = 0;
+	u16 rss_i = adapter->ring_feature[RING_F_RSS].limit;
+	u16 rss_m = IXGBE_RSS_DISABLED_MASK;
+#ifdef IXGBE_FCOE
+	u16 fcoe_i = 0;
+#endif
 
-	if (!(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
+	/* only proceed if SR-IOV is enabled */
+	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
 		return false;
 
-	f->indices = min_t(int, num_online_cpus(), f->indices);
+	/* Add starting offset to total pool count */
+	vmdq_i += adapter->ring_feature[RING_F_VMDQ].offset;
 
-	adapter->num_rx_queues = 1;
-	adapter->num_tx_queues = 1;
+	/* double check we are limited to maximum pools */
+	vmdq_i = min_t(u16, IXGBE_MAX_VMDQ_INDICES, vmdq_i);
 
-	if (adapter->flags & IXGBE_FLAG_RSS_ENABLED) {
-		e_info(probe, "FCoE enabled with RSS\n");
-		if (adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE)
-			ixgbe_set_fdir_queues(adapter);
-		else
-			ixgbe_set_rss_queues(adapter);
+	/* 64 pool mode with 2 queues per pool */
+	if ((vmdq_i > 32) || (rss_i < 4)) {
+		vmdq_m = IXGBE_82599_VMDQ_2Q_MASK;
+		rss_m = IXGBE_RSS_2Q_MASK;
+		rss_i = min_t(u16, rss_i, 2);
+	/* 32 pool mode with 4 queues per pool */
+	} else {
+		vmdq_m = IXGBE_82599_VMDQ_4Q_MASK;
+		rss_m = IXGBE_RSS_4Q_MASK;
+		rss_i = 4;
 	}
 
-	/* adding FCoE rx rings to the end */
-	f->mask = adapter->num_rx_queues;
-	adapter->num_rx_queues += f->indices;
-	adapter->num_tx_queues += f->indices;
+#ifdef IXGBE_FCOE
+	/* queues in the remaining pools are available for FCoE */
+	fcoe_i = 128 - (vmdq_i * __ALIGN_MASK(1, ~vmdq_m));
+
+#endif
+	/* remove the starting offset from the pool count */
+	vmdq_i -= adapter->ring_feature[RING_F_VMDQ].offset;
+
+	/* save features for later use */
+	adapter->ring_feature[RING_F_VMDQ].indices = vmdq_i;
+	adapter->ring_feature[RING_F_VMDQ].mask = vmdq_m;
+
+	/* limit RSS based on user input and save for later use */
+	adapter->ring_feature[RING_F_RSS].indices = rss_i;
+	adapter->ring_feature[RING_F_RSS].mask = rss_m;
 
+	adapter->num_rx_pools = vmdq_i;
+	adapter->num_rx_queues_per_pool = rss_i;
+
+	adapter->num_rx_queues = vmdq_i * rss_i;
+	adapter->num_tx_queues = vmdq_i * rss_i;
+
+	/* disable ATR as it is not supported when VMDq is enabled */
+	adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
+
+#ifdef IXGBE_FCOE
+	/*
+	 * FCoE can use rings from adjacent buffers to allow RSS
+	 * like behavior.  To account for this we need to add the
+	 * FCoE indices to the total ring count.
+	 */
+	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED) {
+		struct ixgbe_ring_feature *fcoe;
+
+		fcoe = &adapter->ring_feature[RING_F_FCOE];
+
+		/* limit ourselves based on feature limits */
+		fcoe_i = min_t(u16, fcoe_i, fcoe->limit);
+
+		if (vmdq_i > 1 && fcoe_i) {
+			/* reserve no more than number of CPUs */
+			fcoe_i = min_t(u16, fcoe_i, num_online_cpus());
+
+			/* alloc queues for FCoE separately */
+			fcoe->indices = fcoe_i;
+			fcoe->offset = vmdq_i * rss_i;
+		} else {
+			/* merge FCoE queues with RSS queues */
+			fcoe_i = min_t(u16, fcoe_i + rss_i, num_online_cpus());
+
+			/* limit indices to rss_i if MSI-X is disabled */
+			if (!(adapter->flags & IXGBE_FLAG_MSIX_ENABLED))
+				fcoe_i = rss_i;
+
+			/* attempt to reserve some queues for just FCoE */
+			fcoe->indices = min_t(u16, fcoe_i, fcoe->limit);
+			fcoe->offset = fcoe_i - fcoe->indices;
+
+			fcoe_i -= rss_i;
+		}
+
+		/* add queues to adapter */
+		adapter->num_tx_queues += fcoe_i;
+		adapter->num_rx_queues += fcoe_i;
+	}
+
+#endif
 	return true;
 }
-#endif /* IXGBE_FCOE */
 
-/* Artificial max queue cap per traffic class in DCB mode */
-#define DCB_QUEUE_CAP 8
-
-#ifdef CONFIG_IXGBE_DCB
-static inline bool ixgbe_set_dcb_queues(struct ixgbe_adapter *adapter)
+/**
+ * ixgbe_set_rss_queues - Allocate queues for RSS
+ * @adapter: board private structure to initialize
+ *
+ * This is our "base" multiqueue mode.  RSS (Receive Side Scaling) will try
+ * to allocate one Rx queue per CPU, and if available, one Tx queue per CPU.
+ *
+ **/
+static bool ixgbe_set_rss_queues(struct ixgbe_adapter *adapter)
 {
-	int per_tc_q, q, i, offset = 0;
-	struct net_device *dev = adapter->netdev;
-	int tcs = netdev_get_num_tc(dev);
+	struct ixgbe_ring_feature *f;
+	u16 rss_i;
 
-	if (!tcs)
-		return false;
+	/* set mask for 16 queue limit of RSS */
+	f = &adapter->ring_feature[RING_F_RSS];
+	rss_i = f->limit;
 
-	/* Map queue offset and counts onto allocated tx queues */
-	per_tc_q = min_t(unsigned int, dev->num_tx_queues / tcs, DCB_QUEUE_CAP);
-	q = min_t(int, num_online_cpus(), per_tc_q);
+	f->indices = rss_i;
+	f->mask = IXGBE_RSS_16Q_MASK;
 
-	for (i = 0; i < tcs; i++) {
-		netdev_set_tc_queue(dev, i, q, offset);
-		offset += q;
-	}
+	/* disable ATR by default, it will be configured below */
+	adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
+
+	/*
+	 * Use Flow Director in addition to RSS to ensure the best
+	 * distribution of flows across cores, even when an FDIR flow
+	 * isn't matched.
+	 */
+	if (rss_i > 1 && adapter->atr_sample_rate) {
+		f = &adapter->ring_feature[RING_F_FDIR];
 
-	adapter->num_tx_queues = q * tcs;
-	adapter->num_rx_queues = q * tcs;
+		f->indices = min_t(u16, num_online_cpus(), f->limit);
+		rss_i = max_t(u16, rss_i, f->indices);
+
+		if (!(adapter->flags & IXGBE_FLAG_FDIR_PERFECT_CAPABLE))
+			adapter->flags |= IXGBE_FLAG_FDIR_HASH_CAPABLE;
+	}
 
 #ifdef IXGBE_FCOE
-	/* FCoE enabled queues require special configuration indexed
-	 * by feature specific indices and mask. Here we map FCoE
-	 * indices onto the DCB queue pairs allowing FCoE to own
-	 * configuration later.
+	/*
+	 * FCoE can exist on the same rings as standard network traffic
+	 * however it is preferred to avoid that if possible.  In order
+	 * to get the best performance we allocate as many FCoE queues
+	 * as we can and we place them at the end of the ring array to
+	 * avoid sharing queues with standard RSS on systems with 24 or
+	 * more CPUs.
 	 */
 	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED) {
-		u8 prio_tc[MAX_USER_PRIORITY] = {0};
-		int tc;
-		struct ixgbe_ring_feature *f =
-					&adapter->ring_feature[RING_F_FCOE];
-
-		ixgbe_dcb_unpack_map(&adapter->dcb_cfg, DCB_TX_CONFIG, prio_tc);
-		tc = prio_tc[adapter->fcoe.up];
-		f->indices = dev->tc_to_txq[tc].count;
-		f->mask = dev->tc_to_txq[tc].offset;
+		struct net_device *dev = adapter->netdev;
+		u16 fcoe_i;
+
+		f = &adapter->ring_feature[RING_F_FCOE];
+
+		/* merge FCoE queues with RSS queues */
+		fcoe_i = min_t(u16, f->limit + rss_i, num_online_cpus());
+		fcoe_i = min_t(u16, fcoe_i, dev->num_tx_queues);
+
+		/* limit indices to rss_i if MSI-X is disabled */
+		if (!(adapter->flags & IXGBE_FLAG_MSIX_ENABLED))
+			fcoe_i = rss_i;
+
+		/* attempt to reserve some queues for just FCoE */
+		f->indices = min_t(u16, fcoe_i, f->limit);
+		f->offset = fcoe_i - f->indices;
+		rss_i = max_t(u16, fcoe_i, rss_i);
 	}
-#endif
+
+#endif /* IXGBE_FCOE */
+	adapter->num_rx_queues = rss_i;
+	adapter->num_tx_queues = rss_i;
 
 	return true;
 }
-#endif
 
 /**
- * ixgbe_set_num_queues: Allocate queues for device, feature dependent
+ * ixgbe_set_num_queues - Allocate queues for device, feature dependent
  * @adapter: board private structure to initialize
  *
  * This is the top level queue allocation routine.  The order here is very
@@ -420,7 +677,7 @@ static inline bool ixgbe_set_dcb_queues(struct ixgbe_adapter *adapter)
  * fallthrough conditions.
  *
  **/
-static int ixgbe_set_num_queues(struct ixgbe_adapter *adapter)
+static void ixgbe_set_num_queues(struct ixgbe_adapter *adapter)
 {
 	/* Start with base case */
 	adapter->num_rx_queues = 1;
@@ -428,38 +685,18 @@ static int ixgbe_set_num_queues(struct ixgbe_adapter *adapter)
 	adapter->num_rx_pools = adapter->num_rx_queues;
 	adapter->num_rx_queues_per_pool = 1;
 
-	if (ixgbe_set_sriov_queues(adapter))
-		goto done;
-
 #ifdef CONFIG_IXGBE_DCB
+	if (ixgbe_set_dcb_sriov_queues(adapter))
+		return;
+
 	if (ixgbe_set_dcb_queues(adapter))
-		goto done;
+		return;
 
 #endif
-#ifdef IXGBE_FCOE
-	if (ixgbe_set_fcoe_queues(adapter))
-		goto done;
-
-#endif /* IXGBE_FCOE */
-	if (ixgbe_set_fdir_queues(adapter))
-		goto done;
-
-	if (ixgbe_set_rss_queues(adapter))
-		goto done;
-
-	/* fallback to base case */
-	adapter->num_rx_queues = 1;
-	adapter->num_tx_queues = 1;
-
-done:
-	if ((adapter->netdev->reg_state == NETREG_UNREGISTERED) ||
-	    (adapter->netdev->reg_state == NETREG_UNREGISTERING))
-		return 0;
+	if (ixgbe_set_sriov_queues(adapter))
+		return;
 
-	/* Notify the stack of the (possibly) reduced queue counts. */
-	netif_set_real_num_tx_queues(adapter->netdev, adapter->num_tx_queues);
-	return netif_set_real_num_rx_queues(adapter->netdev,
-					    adapter->num_rx_queues);
+	ixgbe_set_rss_queues(adapter);
 }
 
 static void ixgbe_acquire_msix_vectors(struct ixgbe_adapter *adapter,
@@ -507,8 +744,8 @@ static void ixgbe_acquire_msix_vectors(struct ixgbe_adapter *adapter,
 		 * of max_msix_q_vectors + NON_Q_VECTORS, or the number of
 		 * vectors we were allocated.
 		 */
-		adapter->num_msix_vectors = min(vectors,
-				   adapter->max_msix_q_vectors + NON_Q_VECTORS);
+		vectors -= NON_Q_VECTORS;
+		adapter->num_q_vectors = min(vectors, adapter->max_q_vectors);
 	}
 }
 
@@ -632,8 +869,8 @@ static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter,
 		if (adapter->netdev->features & NETIF_F_FCOE_MTU) {
 			struct ixgbe_ring_feature *f;
 			f = &adapter->ring_feature[RING_F_FCOE];
-			if ((rxr_idx >= f->mask) &&
-			    (rxr_idx < f->mask + f->indices))
+			if ((rxr_idx >= f->offset) &&
+			    (rxr_idx < f->offset + f->indices))
 				set_bit(__IXGBE_RX_FCOE, &ring->state);
 		}
 
@@ -695,7 +932,7 @@ static void ixgbe_free_q_vector(struct ixgbe_adapter *adapter, int v_idx)
  **/
 static int ixgbe_alloc_q_vectors(struct ixgbe_adapter *adapter)
 {
-	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
+	int q_vectors = adapter->num_q_vectors;
 	int rxr_remaining = adapter->num_rx_queues;
 	int txr_remaining = adapter->num_tx_queues;
 	int rxr_idx = 0, txr_idx = 0, v_idx = 0;
@@ -739,10 +976,12 @@ static int ixgbe_alloc_q_vectors(struct ixgbe_adapter *adapter)
 	return 0;
 
 err_out:
-	while (v_idx) {
-		v_idx--;
+	adapter->num_tx_queues = 0;
+	adapter->num_rx_queues = 0;
+	adapter->num_q_vectors = 0;
+
+	while (v_idx--)
 		ixgbe_free_q_vector(adapter, v_idx);
-	}
 
 	return -ENOMEM;
 }
@@ -757,14 +996,13 @@ err_out:
  **/
 static void ixgbe_free_q_vectors(struct ixgbe_adapter *adapter)
 {
-	int v_idx, q_vectors;
+	int v_idx = adapter->num_q_vectors;
 
-	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED)
-		q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-	else
-		q_vectors = 1;
+	adapter->num_tx_queues = 0;
+	adapter->num_rx_queues = 0;
+	adapter->num_q_vectors = 0;
 
-	for (v_idx = 0; v_idx < q_vectors; v_idx++)
+	while (v_idx--)
 		ixgbe_free_q_vector(adapter, v_idx);
 }
 
@@ -788,11 +1026,10 @@ static void ixgbe_reset_interrupt_capability(struct ixgbe_adapter *adapter)
  * Attempt to configure the interrupts using the best available
  * capabilities of the hardware and the kernel.
  **/
-static int ixgbe_set_interrupt_capability(struct ixgbe_adapter *adapter)
+static void ixgbe_set_interrupt_capability(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	int err = 0;
-	int vector, v_budget;
+	int vector, v_budget, err;
 
 	/*
 	 * It's easy to be greedy for MSI-X vectors, but it really
@@ -825,38 +1062,41 @@ static int ixgbe_set_interrupt_capability(struct ixgbe_adapter *adapter)
 		ixgbe_acquire_msix_vectors(adapter, v_budget);
 
 		if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED)
-			goto out;
+			return;
 	}
 
-	adapter->flags &= ~IXGBE_FLAG_DCB_ENABLED;
-	adapter->flags &= ~IXGBE_FLAG_RSS_ENABLED;
-	if (adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE) {
-		e_err(probe,
-		      "ATR is not supported while multiple "
-		      "queues are disabled.  Disabling Flow Director\n");
+	/* disable DCB if number of TCs exceeds 1 */
+	if (netdev_get_num_tc(adapter->netdev) > 1) {
+		e_err(probe, "num TCs exceeds number of queues - disabling DCB\n");
+		netdev_reset_tc(adapter->netdev);
+
+		if (adapter->hw.mac.type == ixgbe_mac_82598EB)
+			adapter->hw.fc.requested_mode = adapter->last_lfc_mode;
+
+		adapter->flags &= ~IXGBE_FLAG_DCB_ENABLED;
+		adapter->temp_dcb_cfg.pfc_mode_enable = false;
+		adapter->dcb_cfg.pfc_mode_enable = false;
 	}
-	adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
-	adapter->atr_sample_rate = 0;
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
-		ixgbe_disable_sriov(adapter);
+	adapter->dcb_cfg.num_tcs.pg_tcs = 1;
+	adapter->dcb_cfg.num_tcs.pfc_tcs = 1;
+
+	/* disable SR-IOV */
+	ixgbe_disable_sriov(adapter);
 
-	err = ixgbe_set_num_queues(adapter);
-	if (err)
-		return err;
+	/* disable RSS */
+	adapter->ring_feature[RING_F_RSS].limit = 1;
+
+	ixgbe_set_num_queues(adapter);
+	adapter->num_q_vectors = 1;
 
 	err = pci_enable_msi(adapter->pdev);
-	if (!err) {
-		adapter->flags |= IXGBE_FLAG_MSI_ENABLED;
-	} else {
+	if (err) {
 		netif_printk(adapter, hw, KERN_DEBUG, adapter->netdev,
 			     "Unable to allocate MSI interrupt, "
 			     "falling back to legacy.  Error: %d\n", err);
-		/* reset err */
-		err = 0;
+		return;
 	}
-
-out:
-	return err;
+	adapter->flags |= IXGBE_FLAG_MSI_ENABLED;
 }
 
 /**
@@ -874,15 +1114,10 @@ int ixgbe_init_interrupt_scheme(struct ixgbe_adapter *adapter)
 	int err;
 
 	/* Number of supported queues */
-	err = ixgbe_set_num_queues(adapter);
-	if (err)
-		return err;
+	ixgbe_set_num_queues(adapter);
 
-	err = ixgbe_set_interrupt_capability(adapter);
-	if (err) {
-		e_dev_err("Unable to setup interrupt capabilities\n");
-		goto err_set_interrupt;
-	}
+	/* Set interrupt mode */
+	ixgbe_set_interrupt_capability(adapter);
 
 	err = ixgbe_alloc_q_vectors(adapter);
 	if (err) {
@@ -902,7 +1137,6 @@ int ixgbe_init_interrupt_scheme(struct ixgbe_adapter *adapter)
 
 err_alloc_q_vectors:
 	ixgbe_reset_interrupt_capability(adapter);
-err_set_interrupt:
 	return err;
 }
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 18ca3bcadf0..4326f74f713 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -396,11 +396,10 @@ static void ixgbe_dump(struct ixgbe_adapter *adapter)
 				pr_cont("\n");
 
 			if (netif_msg_pktdata(adapter) &&
-			    dma_unmap_len(tx_buffer, len) != 0)
+			    tx_buffer->skb)
 				print_hex_dump(KERN_INFO, "",
 					DUMP_PREFIX_ADDRESS, 16, 1,
-					phys_to_virt(dma_unmap_addr(tx_buffer,
-								    dma)),
+					tx_buffer->skb->data,
 					dma_unmap_len(tx_buffer, len),
 					true);
 		}
@@ -474,10 +473,12 @@ rx_ring_summary:
 					(u64)rx_buffer_info->dma,
 					rx_buffer_info->skb);
 
-				if (netif_msg_pktdata(adapter)) {
+				if (netif_msg_pktdata(adapter) &&
+				    rx_buffer_info->dma) {
 					print_hex_dump(KERN_INFO, "",
 					   DUMP_PREFIX_ADDRESS, 16, 1,
-					   phys_to_virt(rx_buffer_info->dma),
+					   page_address(rx_buffer_info->page) +
+						    rx_buffer_info->page_offset,
 					   ixgbe_rx_bufsz(rx_ring), true);
 				}
 			}
@@ -516,7 +517,7 @@ static void ixgbe_get_hw_control(struct ixgbe_adapter *adapter)
 			ctrl_ext | IXGBE_CTRL_EXT_DRV_LOAD);
 }
 
-/*
+/**
  * ixgbe_set_ivar - set the IVAR registers, mapping interrupt causes to vectors
  * @adapter: pointer to adapter struct
  * @direction: 0 for Rx, 1 for Tx, -1 for other causes
@@ -790,12 +791,10 @@ static bool ixgbe_clean_tx_irq(struct ixgbe_q_vector *q_vector,
 		total_packets += tx_buffer->gso_segs;
 
 #ifdef CONFIG_IXGBE_PTP
-		if (unlikely(tx_buffer->tx_flags &
-			     IXGBE_TX_FLAGS_TSTAMP))
-			ixgbe_ptp_tx_hwtstamp(q_vector,
-					      tx_buffer->skb);
-
+		if (unlikely(tx_buffer->tx_flags & IXGBE_TX_FLAGS_TSTAMP))
+			ixgbe_ptp_tx_hwtstamp(q_vector, tx_buffer->skb);
 #endif
+
 		/* free the skb */
 		dev_kfree_skb_any(tx_buffer->skb);
 
@@ -995,7 +994,6 @@ out_no_update:
 
 static void ixgbe_setup_dca(struct ixgbe_adapter *adapter)
 {
-	int num_q_vectors;
 	int i;
 
 	if (!(adapter->flags & IXGBE_FLAG_DCA_ENABLED))
@@ -1004,12 +1002,7 @@ static void ixgbe_setup_dca(struct ixgbe_adapter *adapter)
 	/* always use CB2 mode, difference is masked in the CB driver */
 	IXGBE_WRITE_REG(&adapter->hw, IXGBE_DCA_CTRL, 2);
 
-	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED)
-		num_q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-	else
-		num_q_vectors = 1;
-
-	for (i = 0; i < num_q_vectors; i++) {
+	for (i = 0; i < adapter->num_q_vectors; i++) {
 		adapter->q_vector[i]->cpu = -1;
 		ixgbe_update_dca(adapter->q_vector[i]);
 	}
@@ -1148,8 +1141,8 @@ static bool ixgbe_alloc_mapped_page(struct ixgbe_ring *rx_ring,
 
 	/* alloc new page for storage */
 	if (likely(!page)) {
-		page = alloc_pages(GFP_ATOMIC | __GFP_COLD | __GFP_COMP,
-				   ixgbe_rx_pg_order(rx_ring));
+		page = __skb_alloc_pages(GFP_ATOMIC | __GFP_COLD | __GFP_COMP,
+					 bi->skb, ixgbe_rx_pg_order(rx_ring));
 		if (unlikely(!page)) {
 			rx_ring->rx_stats.alloc_rx_page_failed++;
 			return false;
@@ -1399,8 +1392,7 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 	ixgbe_rx_checksum(rx_ring, rx_desc, skb);
 
 #ifdef CONFIG_IXGBE_PTP
-	if (ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS))
-		ixgbe_ptp_rx_hwtstamp(rx_ring->q_vector, skb);
+	ixgbe_ptp_rx_hwtstamp(rx_ring->q_vector, rx_desc, skb);
 #endif
 
 	if ((dev->features & NETIF_F_HW_VLAN_RX) &&
@@ -1526,8 +1518,8 @@ static bool ixgbe_cleanup_headers(struct ixgbe_ring *rx_ring,
 	 * 60 bytes if the skb->len is less than 60 for skb_pad.
 	 */
 	pull_len = skb_frag_size(frag);
-	if (pull_len > 256)
-		pull_len = ixgbe_get_headlen(va, pull_len);
+	if (pull_len > IXGBE_RX_HDR_SIZE)
+		pull_len = ixgbe_get_headlen(va, IXGBE_RX_HDR_SIZE);
 
 	/* align pull length to size of long to optimize memcpy performance */
 	skb_copy_to_linear_data(skb, va, ALIGN(pull_len, sizeof(long)));
@@ -1834,11 +1826,9 @@ static bool ixgbe_clean_rx_irq(struct ixgbe_q_vector *q_vector,
 static void ixgbe_configure_msix(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_q_vector *q_vector;
-	int q_vectors, v_idx;
+	int v_idx;
 	u32 mask;
 
-	q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-
 	/* Populate MSIX to EITR Select */
 	if (adapter->num_vfs > 32) {
 		u32 eitrsel = (1 << (adapter->num_vfs - 32)) - 1;
@@ -1849,7 +1839,7 @@ static void ixgbe_configure_msix(struct ixgbe_adapter *adapter)
 	 * Populate the IVAR table and set the ITR values to the
 	 * corresponding register.
 	 */
-	for (v_idx = 0; v_idx < q_vectors; v_idx++) {
+	for (v_idx = 0; v_idx < adapter->num_q_vectors; v_idx++) {
 		struct ixgbe_ring *ring;
 		q_vector = adapter->q_vector[v_idx];
 
@@ -2413,11 +2403,10 @@ int ixgbe_poll(struct napi_struct *napi, int budget)
 static int ixgbe_request_msix_irqs(struct ixgbe_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
-	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
 	int vector, err;
 	int ri = 0, ti = 0;
 
-	for (vector = 0; vector < q_vectors; vector++) {
+	for (vector = 0; vector < adapter->num_q_vectors; vector++) {
 		struct ixgbe_q_vector *q_vector = adapter->q_vector[vector];
 		struct msix_entry *entry = &adapter->msix_entries[vector];
 
@@ -2572,30 +2561,28 @@ static int ixgbe_request_irq(struct ixgbe_adapter *adapter)
 
 static void ixgbe_free_irq(struct ixgbe_adapter *adapter)
 {
-	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED) {
-		int i, q_vectors;
+	int vector;
 
-		q_vectors = adapter->num_msix_vectors;
-		i = q_vectors - 1;
-		free_irq(adapter->msix_entries[i].vector, adapter);
-		i--;
+	if (!(adapter->flags & IXGBE_FLAG_MSIX_ENABLED)) {
+		free_irq(adapter->pdev->irq, adapter);
+		return;
+	}
 
-		for (; i >= 0; i--) {
-			/* free only the irqs that were actually requested */
-			if (!adapter->q_vector[i]->rx.ring &&
-			    !adapter->q_vector[i]->tx.ring)
-				continue;
+	for (vector = 0; vector < adapter->num_q_vectors; vector++) {
+		struct ixgbe_q_vector *q_vector = adapter->q_vector[vector];
+		struct msix_entry *entry = &adapter->msix_entries[vector];
 
-			/* clear the affinity_mask in the IRQ descriptor */
-			irq_set_affinity_hint(adapter->msix_entries[i].vector,
-					      NULL);
+		/* free only the irqs that were actually requested */
+		if (!q_vector->rx.ring && !q_vector->tx.ring)
+			continue;
 
-			free_irq(adapter->msix_entries[i].vector,
-				 adapter->q_vector[i]);
-		}
-	} else {
-		free_irq(adapter->pdev->irq, adapter);
+		/* clear the affinity_mask in the IRQ descriptor */
+		irq_set_affinity_hint(entry->vector, NULL);
+
+		free_irq(entry->vector, q_vector);
 	}
+
+	free_irq(adapter->msix_entries[vector++].vector, adapter);
 }
 
 /**
@@ -2619,9 +2606,12 @@ static inline void ixgbe_irq_disable(struct ixgbe_adapter *adapter)
 	}
 	IXGBE_WRITE_FLUSH(&adapter->hw);
 	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED) {
-		int i;
-		for (i = 0; i < adapter->num_msix_vectors; i++)
-			synchronize_irq(adapter->msix_entries[i].vector);
+		int vector;
+
+		for (vector = 0; vector < adapter->num_q_vectors; vector++)
+			synchronize_irq(adapter->msix_entries[vector].vector);
+
+		synchronize_irq(adapter->msix_entries[vector++].vector);
 	} else {
 		synchronize_irq(adapter->pdev->irq);
 	}
@@ -2699,8 +2689,7 @@ void ixgbe_configure_tx_ring(struct ixgbe_adapter *adapter,
 		   32;		/* PTHRESH = 32 */
 
 	/* reinitialize flowdirector state */
-	if ((adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE) &&
-	    adapter->atr_sample_rate) {
+	if (adapter->flags & IXGBE_FLAG_FDIR_HASH_CAPABLE) {
 		ring->atr_sample_rate = adapter->atr_sample_rate;
 		ring->atr_count = 0;
 		set_bit(__IXGBE_TX_FDIR_INIT_DONE, &ring->state);
@@ -2730,8 +2719,7 @@ void ixgbe_configure_tx_ring(struct ixgbe_adapter *adapter,
 static void ixgbe_setup_mtqc(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 rttdcs;
-	u32 reg;
+	u32 rttdcs, mtqc;
 	u8 tcs = netdev_get_num_tc(adapter->netdev);
 
 	if (hw->mac.type == ixgbe_mac_82598EB)
@@ -2743,28 +2731,32 @@ static void ixgbe_setup_mtqc(struct ixgbe_adapter *adapter)
 	IXGBE_WRITE_REG(hw, IXGBE_RTTDCS, rttdcs);
 
 	/* set transmit pool layout */
-	switch (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
-	case (IXGBE_FLAG_SRIOV_ENABLED):
-		IXGBE_WRITE_REG(hw, IXGBE_MTQC,
-				(IXGBE_MTQC_VT_ENA | IXGBE_MTQC_64VF));
-		break;
-	default:
-		if (!tcs)
-			reg = IXGBE_MTQC_64Q_1PB;
-		else if (tcs <= 4)
-			reg = IXGBE_MTQC_RT_ENA | IXGBE_MTQC_4TC_4TQ;
+	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
+		mtqc = IXGBE_MTQC_VT_ENA;
+		if (tcs > 4)
+			mtqc |= IXGBE_MTQC_RT_ENA | IXGBE_MTQC_8TC_8TQ;
+		else if (tcs > 1)
+			mtqc |= IXGBE_MTQC_RT_ENA | IXGBE_MTQC_4TC_4TQ;
+		else if (adapter->ring_feature[RING_F_RSS].indices == 4)
+			mtqc |= IXGBE_MTQC_32VF;
 		else
-			reg = IXGBE_MTQC_RT_ENA | IXGBE_MTQC_8TC_8TQ;
+			mtqc |= IXGBE_MTQC_64VF;
+	} else {
+		if (tcs > 4)
+			mtqc = IXGBE_MTQC_RT_ENA | IXGBE_MTQC_8TC_8TQ;
+		else if (tcs > 1)
+			mtqc = IXGBE_MTQC_RT_ENA | IXGBE_MTQC_4TC_4TQ;
+		else
+			mtqc = IXGBE_MTQC_64Q_1PB;
+	}
 
-		IXGBE_WRITE_REG(hw, IXGBE_MTQC, reg);
+	IXGBE_WRITE_REG(hw, IXGBE_MTQC, mtqc);
 
-		/* Enable Security TX Buffer IFG for multiple pb */
-		if (tcs) {
-			reg = IXGBE_READ_REG(hw, IXGBE_SECTXMINIFG);
-			reg |= IXGBE_SECTX_DCB;
-			IXGBE_WRITE_REG(hw, IXGBE_SECTXMINIFG, reg);
-		}
-		break;
+	/* Enable Security TX Buffer IFG for multiple pb */
+	if (tcs) {
+		u32 sectx = IXGBE_READ_REG(hw, IXGBE_SECTXMINIFG);
+		sectx |= IXGBE_SECTX_DCB;
+		IXGBE_WRITE_REG(hw, IXGBE_SECTXMINIFG, sectx);
 	}
 
 	/* re-enable the arbiter */
@@ -2858,40 +2850,34 @@ static void ixgbe_set_rx_drop_en(struct ixgbe_adapter *adapter)
 static void ixgbe_configure_srrctl(struct ixgbe_adapter *adapter,
 				   struct ixgbe_ring *rx_ring)
 {
+	struct ixgbe_hw *hw = &adapter->hw;
 	u32 srrctl;
 	u8 reg_idx = rx_ring->reg_idx;
 
-	switch (adapter->hw.mac.type) {
-	case ixgbe_mac_82598EB: {
-		struct ixgbe_ring_feature *feature = adapter->ring_feature;
-		const int mask = feature[RING_F_RSS].mask;
-		reg_idx = reg_idx & mask;
-	}
-		break;
-	case ixgbe_mac_82599EB:
-	case ixgbe_mac_X540:
-	default:
-		break;
-	}
-
-	srrctl = IXGBE_READ_REG(&adapter->hw, IXGBE_SRRCTL(reg_idx));
+	if (hw->mac.type == ixgbe_mac_82598EB) {
+		u16 mask = adapter->ring_feature[RING_F_RSS].mask;
 
-	srrctl &= ~IXGBE_SRRCTL_BSIZEHDR_MASK;
-	srrctl &= ~IXGBE_SRRCTL_BSIZEPKT_MASK;
-	if (adapter->num_vfs)
-		srrctl |= IXGBE_SRRCTL_DROP_EN;
+		/*
+		 * if VMDq is not active we must program one srrctl register
+		 * per RSS queue since we have enabled RDRXCTL.MVMEN
+		 */
+		reg_idx &= mask;
+	}
 
-	srrctl |= (IXGBE_RX_HDR_SIZE << IXGBE_SRRCTL_BSIZEHDRSIZE_SHIFT) &
-		  IXGBE_SRRCTL_BSIZEHDR_MASK;
+	/* configure header buffer length, needed for RSC */
+	srrctl = IXGBE_RX_HDR_SIZE << IXGBE_SRRCTL_BSIZEHDRSIZE_SHIFT;
 
+	/* configure the packet buffer length */
 #if PAGE_SIZE > IXGBE_MAX_RXBUFFER
 	srrctl |= IXGBE_MAX_RXBUFFER >> IXGBE_SRRCTL_BSIZEPKT_SHIFT;
 #else
 	srrctl |= ixgbe_rx_bufsz(rx_ring) >> IXGBE_SRRCTL_BSIZEPKT_SHIFT;
 #endif
+
+	/* configure descriptor type */
 	srrctl |= IXGBE_SRRCTL_DESCTYPE_ADV_ONEBUF;
 
-	IXGBE_WRITE_REG(&adapter->hw, IXGBE_SRRCTL(reg_idx), srrctl);
+	IXGBE_WRITE_REG(hw, IXGBE_SRRCTL(reg_idx), srrctl);
 }
 
 static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
@@ -2903,11 +2889,15 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 	u32 mrqc = 0, reta = 0;
 	u32 rxcsum;
 	int i, j;
-	u8 tcs = netdev_get_num_tc(adapter->netdev);
-	int maxq = adapter->ring_feature[RING_F_RSS].indices;
+	u16 rss_i = adapter->ring_feature[RING_F_RSS].indices;
 
-	if (tcs)
-		maxq = min(maxq, adapter->num_tx_queues / tcs);
+	/*
+	 * Program table for at least 2 queues w/ SR-IOV so that VFs can
+	 * make full use of any rings they may have.  We will use the
+	 * PSRTYPE register to control how many rings we use within the PF.
+	 */
+	if ((adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) && (rss_i < 2))
+		rss_i = 2;
 
 	/* Fill out hash function seeds */
 	for (i = 0; i < 10; i++)
@@ -2915,7 +2905,7 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 
 	/* Fill out redirection table */
 	for (i = 0, j = 0; i < 128; i++, j++) {
-		if (j == maxq)
+		if (j == rss_i)
 			j = 0;
 		/* reta = 4-byte sliding window of
 		 * 0x00..(indices-1)(indices-1)00..etc. */
@@ -2929,35 +2919,36 @@ static void ixgbe_setup_mrqc(struct ixgbe_adapter *adapter)
 	rxcsum |= IXGBE_RXCSUM_PCSD;
 	IXGBE_WRITE_REG(hw, IXGBE_RXCSUM, rxcsum);
 
-	if (adapter->hw.mac.type == ixgbe_mac_82598EB &&
-	    (adapter->flags & IXGBE_FLAG_RSS_ENABLED)) {
-		mrqc = IXGBE_MRQC_RSSEN;
+	if (adapter->hw.mac.type == ixgbe_mac_82598EB) {
+		if (adapter->ring_feature[RING_F_RSS].mask)
+			mrqc = IXGBE_MRQC_RSSEN;
 	} else {
-		int mask = adapter->flags & (IXGBE_FLAG_RSS_ENABLED
-					     | IXGBE_FLAG_SRIOV_ENABLED);
-
-		switch (mask) {
-		case (IXGBE_FLAG_RSS_ENABLED):
-			if (!tcs)
-				mrqc = IXGBE_MRQC_RSSEN;
-			else if (tcs <= 4)
-				mrqc = IXGBE_MRQC_RTRSS4TCEN;
+		u8 tcs = netdev_get_num_tc(adapter->netdev);
+
+		if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
+			if (tcs > 4)
+				mrqc = IXGBE_MRQC_VMDQRT8TCEN;	/* 8 TCs */
+			else if (tcs > 1)
+				mrqc = IXGBE_MRQC_VMDQRT4TCEN;	/* 4 TCs */
+			else if (adapter->ring_feature[RING_F_RSS].indices == 4)
+				mrqc = IXGBE_MRQC_VMDQRSS32EN;
 			else
+				mrqc = IXGBE_MRQC_VMDQRSS64EN;
+		} else {
+			if (tcs > 4)
 				mrqc = IXGBE_MRQC_RTRSS8TCEN;
-			break;
-		case (IXGBE_FLAG_SRIOV_ENABLED):
-			mrqc = IXGBE_MRQC_VMDQEN;
-			break;
-		default:
-			break;
+			else if (tcs > 1)
+				mrqc = IXGBE_MRQC_RTRSS4TCEN;
+			else
+				mrqc = IXGBE_MRQC_RSSEN;
 		}
 	}
 
 	/* Perform hash on these packet types */
-	mrqc |= IXGBE_MRQC_RSS_FIELD_IPV4
-	      | IXGBE_MRQC_RSS_FIELD_IPV4_TCP
-	      | IXGBE_MRQC_RSS_FIELD_IPV6
-	      | IXGBE_MRQC_RSS_FIELD_IPV6_TCP;
+	mrqc |= IXGBE_MRQC_RSS_FIELD_IPV4 |
+		IXGBE_MRQC_RSS_FIELD_IPV4_TCP |
+		IXGBE_MRQC_RSS_FIELD_IPV6 |
+		IXGBE_MRQC_RSS_FIELD_IPV6_TCP;
 
 	if (adapter->flags2 & IXGBE_FLAG2_RSS_FIELD_IPV4_UDP)
 		mrqc |= IXGBE_MRQC_RSS_FIELD_IPV4_UDP;
@@ -3108,6 +3099,7 @@ void ixgbe_configure_rx_ring(struct ixgbe_adapter *adapter,
 static void ixgbe_setup_psrtype(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
+	int rss_i = adapter->ring_feature[RING_F_RSS].indices;
 	int p;
 
 	/* PSRTYPE must be initialized in non 82598 adapters */
@@ -3120,58 +3112,69 @@ static void ixgbe_setup_psrtype(struct ixgbe_adapter *adapter)
 	if (hw->mac.type == ixgbe_mac_82598EB)
 		return;
 
-	if (adapter->flags & IXGBE_FLAG_RSS_ENABLED)
-		psrtype |= (adapter->num_rx_queues_per_pool << 29);
+	if (rss_i > 3)
+		psrtype |= 2 << 29;
+	else if (rss_i > 1)
+		psrtype |= 1 << 29;
 
 	for (p = 0; p < adapter->num_rx_pools; p++)
-		IXGBE_WRITE_REG(hw, IXGBE_PSRTYPE(adapter->num_vfs + p),
+		IXGBE_WRITE_REG(hw, IXGBE_PSRTYPE(VMDQ_P(p)),
 				psrtype);
 }
 
 static void ixgbe_configure_virtualization(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 gcr_ext;
-	u32 vt_reg_bits;
 	u32 reg_offset, vf_shift;
-	u32 vmdctl;
+	u32 gcr_ext, vmdctl;
 	int i;
 
 	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
 		return;
 
 	vmdctl = IXGBE_READ_REG(hw, IXGBE_VT_CTL);
-	vt_reg_bits = IXGBE_VMD_CTL_VMDQ_EN | IXGBE_VT_CTL_REPLEN;
-	vt_reg_bits |= (adapter->num_vfs << IXGBE_VT_CTL_POOL_SHIFT);
-	IXGBE_WRITE_REG(hw, IXGBE_VT_CTL, vmdctl | vt_reg_bits);
+	vmdctl |= IXGBE_VMD_CTL_VMDQ_EN;
+	vmdctl &= ~IXGBE_VT_CTL_POOL_MASK;
+	vmdctl |= VMDQ_P(0) << IXGBE_VT_CTL_POOL_SHIFT;
+	vmdctl |= IXGBE_VT_CTL_REPLEN;
+	IXGBE_WRITE_REG(hw, IXGBE_VT_CTL, vmdctl);
 
-	vf_shift = adapter->num_vfs % 32;
-	reg_offset = (adapter->num_vfs >= 32) ? 1 : 0;
+	vf_shift = VMDQ_P(0) % 32;
+	reg_offset = (VMDQ_P(0) >= 32) ? 1 : 0;
 
 	/* Enable only the PF's pool for Tx/Rx */
-	IXGBE_WRITE_REG(hw, IXGBE_VFRE(reg_offset), (1 << vf_shift));
-	IXGBE_WRITE_REG(hw, IXGBE_VFRE(reg_offset ^ 1), 0);
-	IXGBE_WRITE_REG(hw, IXGBE_VFTE(reg_offset), (1 << vf_shift));
-	IXGBE_WRITE_REG(hw, IXGBE_VFTE(reg_offset ^ 1), 0);
+	IXGBE_WRITE_REG(hw, IXGBE_VFRE(reg_offset), (~0) << vf_shift);
+	IXGBE_WRITE_REG(hw, IXGBE_VFRE(reg_offset ^ 1), reg_offset - 1);
+	IXGBE_WRITE_REG(hw, IXGBE_VFTE(reg_offset), (~0) << vf_shift);
+	IXGBE_WRITE_REG(hw, IXGBE_VFTE(reg_offset ^ 1), reg_offset - 1);
 	IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, IXGBE_PFDTXGSWC_VT_LBEN);
 
 	/* Map PF MAC address in RAR Entry 0 to first pool following VFs */
-	hw->mac.ops.set_vmdq(hw, 0, adapter->num_vfs);
+	hw->mac.ops.set_vmdq(hw, 0, VMDQ_P(0));
 
 	/*
 	 * Set up VF register offsets for selected VT Mode,
 	 * i.e. 32 or 64 VFs for SR-IOV
 	 */
-	gcr_ext = IXGBE_READ_REG(hw, IXGBE_GCR_EXT);
-	gcr_ext |= IXGBE_GCR_EXT_MSIX_EN;
-	gcr_ext |= IXGBE_GCR_EXT_VT_MODE_64;
+	switch (adapter->ring_feature[RING_F_VMDQ].mask) {
+	case IXGBE_82599_VMDQ_8Q_MASK:
+		gcr_ext = IXGBE_GCR_EXT_VT_MODE_16;
+		break;
+	case IXGBE_82599_VMDQ_4Q_MASK:
+		gcr_ext = IXGBE_GCR_EXT_VT_MODE_32;
+		break;
+	default:
+		gcr_ext = IXGBE_GCR_EXT_VT_MODE_64;
+		break;
+	}
+
 	IXGBE_WRITE_REG(hw, IXGBE_GCR_EXT, gcr_ext);
 
 	/* enable Tx loopback for VF/PF communication */
 	IXGBE_WRITE_REG(hw, IXGBE_PFDTXGSWC, IXGBE_PFDTXGSWC_VT_LBEN);
+
 	/* Enable MAC Anti-Spoofing */
-	hw->mac.ops.set_mac_anti_spoofing(hw,
-					   (adapter->num_vfs != 0),
+	hw->mac.ops.set_mac_anti_spoofing(hw, (adapter->num_vfs != 0),
 					  adapter->num_vfs);
 	/* For VFs that have spoof checking turned off */
 	for (i = 0; i < adapter->num_vfs; i++) {
@@ -3307,10 +3310,9 @@ static int ixgbe_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	int pool_ndx = adapter->num_vfs;
 
 	/* add VID to filter table */
-	hw->mac.ops.set_vfta(&adapter->hw, vid, pool_ndx, true);
+	hw->mac.ops.set_vfta(&adapter->hw, vid, VMDQ_P(0), true);
 	set_bit(vid, adapter->active_vlans);
 
 	return 0;
@@ -3320,10 +3322,9 @@ static int ixgbe_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	int pool_ndx = adapter->num_vfs;
 
 	/* remove VID from filter table */
-	hw->mac.ops.set_vfta(&adapter->hw, vid, pool_ndx, false);
+	hw->mac.ops.set_vfta(&adapter->hw, vid, VMDQ_P(0), false);
 	clear_bit(vid, adapter->active_vlans);
 
 	return 0;
@@ -3441,15 +3442,18 @@ static int ixgbe_write_uc_addr_list(struct net_device *netdev)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	unsigned int vfn = adapter->num_vfs;
-	unsigned int rar_entries = IXGBE_MAX_PF_MACVLANS;
+	unsigned int rar_entries = hw->mac.num_rar_entries - 1;
 	int count = 0;
 
+	/* In SR-IOV mode significantly less RAR entries are available */
+	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
+		rar_entries = IXGBE_MAX_PF_MACVLANS - 1;
+
 	/* return ENOMEM indicating insufficient memory for addresses */
 	if (netdev_uc_count(netdev) > rar_entries)
 		return -ENOMEM;
 
-	if (!netdev_uc_empty(netdev) && rar_entries) {
+	if (!netdev_uc_empty(netdev)) {
 		struct netdev_hw_addr *ha;
 		/* return error if we do not support writing to RAR table */
 		if (!hw->mac.ops.set_rar)
@@ -3459,7 +3463,7 @@ static int ixgbe_write_uc_addr_list(struct net_device *netdev)
 			if (!rar_entries)
 				break;
 			hw->mac.ops.set_rar(hw, rar_entries--, ha->addr,
-					    vfn, IXGBE_RAH_AV);
+					    VMDQ_P(0), IXGBE_RAH_AV);
 			count++;
 		}
 	}
@@ -3533,12 +3537,14 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 		vmolr |= IXGBE_VMOLR_ROPE;
 	}
 
-	if (adapter->num_vfs) {
+	if (adapter->num_vfs)
 		ixgbe_restore_vf_multicasts(adapter);
-		vmolr |= IXGBE_READ_REG(hw, IXGBE_VMOLR(adapter->num_vfs)) &
+
+	if (hw->mac.type != ixgbe_mac_82598EB) {
+		vmolr |= IXGBE_READ_REG(hw, IXGBE_VMOLR(VMDQ_P(0))) &
 			 ~(IXGBE_VMOLR_MPE | IXGBE_VMOLR_ROMPE |
 			   IXGBE_VMOLR_ROPE);
-		IXGBE_WRITE_REG(hw, IXGBE_VMOLR(adapter->num_vfs), vmolr);
+		IXGBE_WRITE_REG(hw, IXGBE_VMOLR(VMDQ_P(0)), vmolr);
 	}
 
 	/* This is useful for sniffing bad packets. */
@@ -3564,37 +3570,21 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 static void ixgbe_napi_enable_all(struct ixgbe_adapter *adapter)
 {
 	int q_idx;
-	struct ixgbe_q_vector *q_vector;
-	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-
-	/* legacy and MSI only use one vector */
-	if (!(adapter->flags & IXGBE_FLAG_MSIX_ENABLED))
-		q_vectors = 1;
 
-	for (q_idx = 0; q_idx < q_vectors; q_idx++) {
-		q_vector = adapter->q_vector[q_idx];
-		napi_enable(&q_vector->napi);
-	}
+	for (q_idx = 0; q_idx < adapter->num_q_vectors; q_idx++)
+		napi_enable(&adapter->q_vector[q_idx]->napi);
 }
 
 static void ixgbe_napi_disable_all(struct ixgbe_adapter *adapter)
 {
 	int q_idx;
-	struct ixgbe_q_vector *q_vector;
-	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
 
-	/* legacy and MSI only use one vector */
-	if (!(adapter->flags & IXGBE_FLAG_MSIX_ENABLED))
-		q_vectors = 1;
-
-	for (q_idx = 0; q_idx < q_vectors; q_idx++) {
-		q_vector = adapter->q_vector[q_idx];
-		napi_disable(&q_vector->napi);
-	}
+	for (q_idx = 0; q_idx < adapter->num_q_vectors; q_idx++)
+		napi_disable(&adapter->q_vector[q_idx]->napi);
 }
 
 #ifdef CONFIG_IXGBE_DCB
-/*
+/**
  * ixgbe_configure_dcb - Configure DCB hardware
  * @adapter: ixgbe adapter struct
  *
@@ -3641,19 +3631,16 @@ static void ixgbe_configure_dcb(struct ixgbe_adapter *adapter)
 
 	/* Enable RSS Hash per TC */
 	if (hw->mac.type != ixgbe_mac_82598EB) {
-		int i;
-		u32 reg = 0;
-
-		for (i = 0; i < MAX_TRAFFIC_CLASS; i++) {
-			u8 msb = 0;
-			u8 cnt = adapter->netdev->tc_to_txq[i].count;
+		u32 msb = 0;
+		u16 rss_i = adapter->ring_feature[RING_F_RSS].indices - 1;
 
-			while (cnt >>= 1)
-				msb++;
-
-			reg |= msb << IXGBE_RQTC_SHIFT_TC(i);
+		while (rss_i) {
+			msb++;
+			rss_i >>= 1;
 		}
-		IXGBE_WRITE_REG(hw, IXGBE_RQTC, reg);
+
+		/* write msb to all 8 TCs in one write */
+		IXGBE_WRITE_REG(hw, IXGBE_RQTC, msb * 0x11111111);
 	}
 }
 #endif
@@ -3661,11 +3648,11 @@ static void ixgbe_configure_dcb(struct ixgbe_adapter *adapter)
 /* Additional bittime to account for IXGBE framing */
 #define IXGBE_ETH_FRAMING 20
 
-/*
+/**
  * ixgbe_hpbthresh - calculate high water mark for flow control
  *
  * @adapter: board private structure to calculate for
- * @pb - packet buffer to calculate
+ * @pb: packet buffer to calculate
  */
 static int ixgbe_hpbthresh(struct ixgbe_adapter *adapter, int pb)
 {
@@ -3679,18 +3666,12 @@ static int ixgbe_hpbthresh(struct ixgbe_adapter *adapter, int pb)
 
 #ifdef IXGBE_FCOE
 	/* FCoE traffic class uses FCOE jumbo frames */
-	if (dev->features & NETIF_F_FCOE_MTU) {
-		int fcoe_pb = 0;
+	if ((dev->features & NETIF_F_FCOE_MTU) &&
+	    (tc < IXGBE_FCOE_JUMBO_FRAME_SIZE) &&
+	    (pb == ixgbe_fcoe_get_tc(adapter)))
+		tc = IXGBE_FCOE_JUMBO_FRAME_SIZE;
 
-#ifdef CONFIG_IXGBE_DCB
-		fcoe_pb = netdev_get_prio_tc_map(dev, adapter->fcoe.up);
-
-#endif
-		if (fcoe_pb == pb && tc < IXGBE_FCOE_JUMBO_FRAME_SIZE)
-			tc = IXGBE_FCOE_JUMBO_FRAME_SIZE;
-	}
 #endif
-
 	/* Calculate delay value for device */
 	switch (hw->mac.type) {
 	case ixgbe_mac_X540:
@@ -3725,11 +3706,11 @@ static int ixgbe_hpbthresh(struct ixgbe_adapter *adapter, int pb)
 	return marker;
 }
 
-/*
+/**
  * ixgbe_lpbthresh - calculate low water mark for for flow control
  *
  * @adapter: board private structure to calculate for
- * @pb - packet buffer to calculate
+ * @pb: packet buffer to calculate
  */
 static int ixgbe_lpbthresh(struct ixgbe_adapter *adapter)
 {
@@ -3830,12 +3811,6 @@ static void ixgbe_configure(struct ixgbe_adapter *adapter)
 	ixgbe_set_rx_mode(adapter->netdev);
 	ixgbe_restore_vlan(adapter);
 
-#ifdef IXGBE_FCOE
-	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)
-		ixgbe_configure_fcoe(adapter);
-
-#endif /* IXGBE_FCOE */
-
 	switch (hw->mac.type) {
 	case ixgbe_mac_82599EB:
 	case ixgbe_mac_X540:
@@ -3865,6 +3840,11 @@ static void ixgbe_configure(struct ixgbe_adapter *adapter)
 
 	ixgbe_configure_virtualization(adapter);
 
+#ifdef IXGBE_FCOE
+	/* configure FCoE L2 filters, redirection table, and Rx control */
+	ixgbe_configure_fcoe(adapter);
+
+#endif /* IXGBE_FCOE */
 	ixgbe_configure_tx(adapter);
 	ixgbe_configure_rx(adapter);
 }
@@ -3973,7 +3953,18 @@ static void ixgbe_setup_gpie(struct ixgbe_adapter *adapter)
 
 	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
 		gpie &= ~IXGBE_GPIE_VTMODE_MASK;
-		gpie |= IXGBE_GPIE_VTMODE_64;
+
+		switch (adapter->ring_feature[RING_F_VMDQ].mask) {
+		case IXGBE_82599_VMDQ_8Q_MASK:
+			gpie |= IXGBE_GPIE_VTMODE_16;
+			break;
+		case IXGBE_82599_VMDQ_4Q_MASK:
+			gpie |= IXGBE_GPIE_VTMODE_32;
+			break;
+		default:
+			gpie |= IXGBE_GPIE_VTMODE_64;
+			break;
+		}
 	}
 
 	/* Enable Thermal over heat sensor interrupt */
@@ -4131,8 +4122,11 @@ void ixgbe_reset(struct ixgbe_adapter *adapter)
 	clear_bit(__IXGBE_IN_SFP_INIT, &adapter->state);
 
 	/* reprogram the RAR[0] in case user changed it. */
-	hw->mac.ops.set_rar(hw, 0, hw->mac.addr, adapter->num_vfs,
-			    IXGBE_RAH_AV);
+	hw->mac.ops.set_rar(hw, 0, hw->mac.addr, VMDQ_P(0), IXGBE_RAH_AV);
+
+	/* update SAN MAC vmdq pool selection */
+	if (hw->mac.san_mac_rar_index)
+		hw->mac.ops.set_vmdq_san_mac(hw, VMDQ_P(0));
 }
 
 /**
@@ -4413,32 +4407,29 @@ static int __devinit ixgbe_sw_init(struct ixgbe_adapter *adapter)
 
 	/* Set capability flags */
 	rss = min_t(int, IXGBE_MAX_RSS_INDICES, num_online_cpus());
-	adapter->ring_feature[RING_F_RSS].indices = rss;
-	adapter->flags |= IXGBE_FLAG_RSS_ENABLED;
+	adapter->ring_feature[RING_F_RSS].limit = rss;
 	switch (hw->mac.type) {
 	case ixgbe_mac_82598EB:
 		if (hw->device_id == IXGBE_DEV_ID_82598AT)
 			adapter->flags |= IXGBE_FLAG_FAN_FAIL_CAPABLE;
-		adapter->max_msix_q_vectors = MAX_MSIX_Q_VECTORS_82598;
+		adapter->max_q_vectors = MAX_Q_VECTORS_82598;
 		break;
 	case ixgbe_mac_X540:
 		adapter->flags2 |= IXGBE_FLAG2_TEMP_SENSOR_CAPABLE;
 	case ixgbe_mac_82599EB:
-		adapter->max_msix_q_vectors = MAX_MSIX_Q_VECTORS_82599;
+		adapter->max_q_vectors = MAX_Q_VECTORS_82599;
 		adapter->flags2 |= IXGBE_FLAG2_RSC_CAPABLE;
 		adapter->flags2 |= IXGBE_FLAG2_RSC_ENABLED;
 		if (hw->device_id == IXGBE_DEV_ID_82599_T3_LOM)
 			adapter->flags2 |= IXGBE_FLAG2_TEMP_SENSOR_CAPABLE;
 		/* Flow Director hash filters enabled */
-		adapter->flags |= IXGBE_FLAG_FDIR_HASH_CAPABLE;
 		adapter->atr_sample_rate = 20;
-		adapter->ring_feature[RING_F_FDIR].indices =
+		adapter->ring_feature[RING_F_FDIR].limit =
 							 IXGBE_MAX_FDIR_INDICES;
 		adapter->fdir_pballoc = IXGBE_FDIR_PBALLOC_64K;
 #ifdef IXGBE_FCOE
 		adapter->flags |= IXGBE_FLAG_FCOE_CAPABLE;
 		adapter->flags &= ~IXGBE_FLAG_FCOE_ENABLED;
-		adapter->ring_feature[RING_F_FCOE].indices = 0;
 #ifdef CONFIG_IXGBE_DCB
 		/* Default traffic class to use for FCoE */
 		adapter->fcoe.up = IXGBE_FCOE_DEFTC;
@@ -4449,6 +4440,11 @@ static int __devinit ixgbe_sw_init(struct ixgbe_adapter *adapter)
 		break;
 	}
 
+#ifdef IXGBE_FCOE
+	/* FCoE support exists, always init the FCoE lock */
+	spin_lock_init(&adapter->fcoe.lock);
+
+#endif
 	/* n-tuple support exists, always init our spinlock */
 	spin_lock_init(&adapter->fdir_perfect_lock);
 
@@ -4497,6 +4493,12 @@ static int __devinit ixgbe_sw_init(struct ixgbe_adapter *adapter)
 	hw->fc.send_xon = true;
 	hw->fc.disable_fc_autoneg = false;
 
+#ifdef CONFIG_PCI_IOV
+	/* assign number of SR-IOV VFs */
+	if (hw->mac.type != ixgbe_mac_82598EB)
+		adapter->num_vfs = (max_vfs > 63) ? 0 : max_vfs;
+
+#endif
 	/* enable itr by default in dynamic mode */
 	adapter->rx_itr_setting = 1;
 	adapter->tx_itr_setting = 1;
@@ -4588,10 +4590,16 @@ static int ixgbe_setup_all_tx_resources(struct ixgbe_adapter *adapter)
 		err = ixgbe_setup_tx_resources(adapter->tx_ring[i]);
 		if (!err)
 			continue;
+
 		e_err(probe, "Allocation for Tx Queue %u failed\n", i);
-		break;
+		goto err_setup_tx;
 	}
 
+	return 0;
+err_setup_tx:
+	/* rewind the index freeing the rings as we go */
+	while (i--)
+		ixgbe_free_tx_resources(adapter->tx_ring[i]);
 	return err;
 }
 
@@ -4666,10 +4674,20 @@ static int ixgbe_setup_all_rx_resources(struct ixgbe_adapter *adapter)
 		err = ixgbe_setup_rx_resources(adapter->rx_ring[i]);
 		if (!err)
 			continue;
+
 		e_err(probe, "Allocation for Rx Queue %u failed\n", i);
-		break;
+		goto err_setup_rx;
 	}
 
+#ifdef IXGBE_FCOE
+	err = ixgbe_setup_fcoe_ddp_resources(adapter);
+	if (!err)
+#endif
+		return 0;
+err_setup_rx:
+	/* rewind the index freeing the rings as we go */
+	while (i--)
+		ixgbe_free_rx_resources(adapter->rx_ring[i]);
 	return err;
 }
 
@@ -4744,6 +4762,10 @@ static void ixgbe_free_all_rx_resources(struct ixgbe_adapter *adapter)
 {
 	int i;
 
+#ifdef IXGBE_FCOE
+	ixgbe_free_fcoe_ddp_resources(adapter);
+
+#endif
 	for (i = 0; i < adapter->num_rx_queues; i++)
 		if (adapter->rx_ring[i]->desc)
 			ixgbe_free_rx_resources(adapter->rx_ring[i]);
@@ -4825,15 +4847,31 @@ static int ixgbe_open(struct net_device *netdev)
 	if (err)
 		goto err_req_irq;
 
+	/* Notify the stack of the actual queue counts. */
+	err = netif_set_real_num_tx_queues(netdev,
+					   adapter->num_rx_pools > 1 ? 1 :
+					   adapter->num_tx_queues);
+	if (err)
+		goto err_set_queues;
+
+
+	err = netif_set_real_num_rx_queues(netdev,
+					   adapter->num_rx_pools > 1 ? 1 :
+					   adapter->num_rx_queues);
+	if (err)
+		goto err_set_queues;
+
 	ixgbe_up_complete(adapter);
 
 	return 0;
 
+err_set_queues:
+	ixgbe_free_irq(adapter);
 err_req_irq:
-err_setup_rx:
 	ixgbe_free_all_rx_resources(adapter);
-err_setup_tx:
+err_setup_rx:
 	ixgbe_free_all_tx_resources(adapter);
+err_setup_tx:
 	ixgbe_reset(adapter);
 
 	return err;
@@ -4891,23 +4929,19 @@ static int ixgbe_resume(struct pci_dev *pdev)
 
 	pci_wake_from_d3(pdev, false);
 
-	rtnl_lock();
-	err = ixgbe_init_interrupt_scheme(adapter);
-	rtnl_unlock();
-	if (err) {
-		e_dev_err("Cannot initialize interrupts for device\n");
-		return err;
-	}
-
 	ixgbe_reset(adapter);
 
 	IXGBE_WRITE_REG(&adapter->hw, IXGBE_WUS, ~0);
 
-	if (netif_running(netdev)) {
+	rtnl_lock();
+	err = ixgbe_init_interrupt_scheme(adapter);
+	if (!err && netif_running(netdev))
 		err = ixgbe_open(netdev);
-		if (err)
-			return err;
-	}
+
+	rtnl_unlock();
+
+	if (err)
+		return err;
 
 	netif_device_attach(netdev);
 
@@ -5043,11 +5077,6 @@ void ixgbe_update_stats(struct ixgbe_adapter *adapter)
 	u64 non_eop_descs = 0, restart_queue = 0, tx_busy = 0;
 	u64 alloc_rx_page_failed = 0, alloc_rx_buff_failed = 0;
 	u64 bytes = 0, packets = 0, hw_csum_rx_error = 0;
-#ifdef IXGBE_FCOE
-	struct ixgbe_fcoe *fcoe = &adapter->fcoe;
-	unsigned int cpu;
-	u64 fcoe_noddp_counts_sum = 0, fcoe_noddp_ext_buff_counts_sum = 0;
-#endif /* IXGBE_FCOE */
 
 	if (test_bit(__IXGBE_DOWN, &adapter->state) ||
 	    test_bit(__IXGBE_RESETTING, &adapter->state))
@@ -5178,17 +5207,19 @@ void ixgbe_update_stats(struct ixgbe_adapter *adapter)
 		hwstats->fcoedwrc += IXGBE_READ_REG(hw, IXGBE_FCOEDWRC);
 		hwstats->fcoedwtc += IXGBE_READ_REG(hw, IXGBE_FCOEDWTC);
 		/* Add up per cpu counters for total ddp aloc fail */
-		if (fcoe->pcpu_noddp && fcoe->pcpu_noddp_ext_buff) {
+		if (adapter->fcoe.ddp_pool) {
+			struct ixgbe_fcoe *fcoe = &adapter->fcoe;
+			struct ixgbe_fcoe_ddp_pool *ddp_pool;
+			unsigned int cpu;
+			u64 noddp = 0, noddp_ext_buff = 0;
 			for_each_possible_cpu(cpu) {
-				fcoe_noddp_counts_sum +=
-					*per_cpu_ptr(fcoe->pcpu_noddp, cpu);
-				fcoe_noddp_ext_buff_counts_sum +=
-					*per_cpu_ptr(fcoe->
-						pcpu_noddp_ext_buff, cpu);
+				ddp_pool = per_cpu_ptr(fcoe->ddp_pool, cpu);
+				noddp += ddp_pool->noddp;
+				noddp_ext_buff += ddp_pool->noddp_ext_buff;
 			}
+			hwstats->fcoe_noddp = noddp;
+			hwstats->fcoe_noddp_ext_buff = noddp_ext_buff;
 		}
-		hwstats->fcoe_noddp = fcoe_noddp_counts_sum;
-		hwstats->fcoe_noddp_ext_buff = fcoe_noddp_ext_buff_counts_sum;
 #endif /* IXGBE_FCOE */
 		break;
 	default:
@@ -5246,7 +5277,7 @@ void ixgbe_update_stats(struct ixgbe_adapter *adapter)
 
 /**
  * ixgbe_fdir_reinit_subtask - worker thread to reinit FDIR filter table
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  **/
 static void ixgbe_fdir_reinit_subtask(struct ixgbe_adapter *adapter)
 {
@@ -5282,7 +5313,7 @@ static void ixgbe_fdir_reinit_subtask(struct ixgbe_adapter *adapter)
 
 /**
  * ixgbe_check_hang_subtask - check for hung queues and dropped interrupts
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  *
  * This function serves two purposes.  First it strobes the interrupt lines
  * in order to make certain interrupts are occurring.  Secondly it sets the
@@ -5316,7 +5347,7 @@ static void ixgbe_check_hang_subtask(struct ixgbe_adapter *adapter)
 			(IXGBE_EICS_TCP_TIMER | IXGBE_EICS_OTHER));
 	} else {
 		/* get one bit for every active tx/rx interrupt vector */
-		for (i = 0; i < adapter->num_msix_vectors - NON_Q_VECTORS; i++) {
+		for (i = 0; i < adapter->num_q_vectors; i++) {
 			struct ixgbe_q_vector *qv = adapter->q_vector[i];
 			if (qv->rx.ring || qv->tx.ring)
 				eics |= ((u64)1 << i);
@@ -5330,8 +5361,8 @@ static void ixgbe_check_hang_subtask(struct ixgbe_adapter *adapter)
 
 /**
  * ixgbe_watchdog_update_link - update the link status
- * @adapter - pointer to the device adapter structure
- * @link_speed - pointer to a u32 to store the link_speed
+ * @adapter: pointer to the device adapter structure
+ * @link_speed: pointer to a u32 to store the link_speed
  **/
 static void ixgbe_watchdog_update_link(struct ixgbe_adapter *adapter)
 {
@@ -5374,7 +5405,7 @@ static void ixgbe_watchdog_update_link(struct ixgbe_adapter *adapter)
 /**
  * ixgbe_watchdog_link_is_up - update netif_carrier status and
  *                             print link up message
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  **/
 static void ixgbe_watchdog_link_is_up(struct ixgbe_adapter *adapter)
 {
@@ -5429,12 +5460,15 @@ static void ixgbe_watchdog_link_is_up(struct ixgbe_adapter *adapter)
 
 	netif_carrier_on(netdev);
 	ixgbe_check_vf_rate_limit(adapter);
+
+	/* ping all the active vfs to let them know link has changed */
+	ixgbe_ping_all_vfs(adapter);
 }
 
 /**
  * ixgbe_watchdog_link_is_down - update netif_carrier status and
  *                               print link down message
- * @adapter - pointer to the adapter structure
+ * @adapter: pointer to the adapter structure
  **/
 static void ixgbe_watchdog_link_is_down(struct ixgbe_adapter *adapter)
 {
@@ -5458,11 +5492,14 @@ static void ixgbe_watchdog_link_is_down(struct ixgbe_adapter *adapter)
 
 	e_info(drv, "NIC Link is Down\n");
 	netif_carrier_off(netdev);
+
+	/* ping all the active vfs to let them know link has changed */
+	ixgbe_ping_all_vfs(adapter);
 }
 
 /**
  * ixgbe_watchdog_flush_tx - flush queues on link down
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  **/
 static void ixgbe_watchdog_flush_tx(struct ixgbe_adapter *adapter)
 {
@@ -5511,7 +5548,7 @@ static void ixgbe_spoof_check(struct ixgbe_adapter *adapter)
 
 /**
  * ixgbe_watchdog_subtask - check and bring link up
- * @adapter - pointer to the device adapter structure
+ * @adapter: pointer to the device adapter structure
  **/
 static void ixgbe_watchdog_subtask(struct ixgbe_adapter *adapter)
 {
@@ -5535,7 +5572,7 @@ static void ixgbe_watchdog_subtask(struct ixgbe_adapter *adapter)
 
 /**
  * ixgbe_sfp_detection_subtask - poll for SFP+ cable
- * @adapter - the ixgbe adapter structure
+ * @adapter: the ixgbe adapter structure
  **/
 static void ixgbe_sfp_detection_subtask(struct ixgbe_adapter *adapter)
 {
@@ -5602,7 +5639,7 @@ sfp_out:
 
 /**
  * ixgbe_sfp_link_config_subtask - set up link SFP after module install
- * @adapter - the ixgbe adapter structure
+ * @adapter: the ixgbe adapter structure
  **/
 static void ixgbe_sfp_link_config_subtask(struct ixgbe_adapter *adapter)
 {
@@ -6233,8 +6270,14 @@ static u16 ixgbe_select_queue(struct net_device *dev, struct sk_buff *skb)
 	if (((protocol == htons(ETH_P_FCOE)) ||
 	    (protocol == htons(ETH_P_FIP))) &&
 	    (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)) {
-		txq &= (adapter->ring_feature[RING_F_FCOE].indices - 1);
-		txq += adapter->ring_feature[RING_F_FCOE].mask;
+		struct ixgbe_ring_feature *f;
+
+		f = &adapter->ring_feature[RING_F_FCOE];
+
+		while (txq >= f->indices)
+			txq -= f->indices;
+		txq += adapter->ring_feature[RING_F_FCOE].offset;
+
 		return txq;
 	}
 #endif
@@ -6348,7 +6391,7 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 #ifdef IXGBE_FCOE
 	/* setup tx offload for FCoE */
 	if ((protocol == __constant_htons(ETH_P_FCOE)) &&
-	    (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)) {
+	    (tx_ring->netdev->features & (NETIF_F_FSO | NETIF_F_FCOE_CRC))) {
 		tso = ixgbe_fso(tx_ring, first, &hdr_len);
 		if (tso < 0)
 			goto out_drop;
@@ -6389,17 +6432,12 @@ static netdev_tx_t ixgbe_xmit_frame(struct sk_buff *skb,
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_ring *tx_ring;
 
-	if (skb->len <= 0) {
-		dev_kfree_skb_any(skb);
-		return NETDEV_TX_OK;
-	}
-
 	/*
 	 * The minimum packet size for olinfo paylen is 17 so pad the skb
 	 * in order to meet this minimum size requirement.
 	 */
-	if (skb->len < 17) {
-		if (skb_padto(skb, 17))
+	if (unlikely(skb->len < 17)) {
+		if (skb_pad(skb, 17 - skb->len))
 			return NETDEV_TX_OK;
 		skb->len = 17;
 	}
@@ -6427,8 +6465,7 @@ static int ixgbe_set_mac(struct net_device *netdev, void *p)
 	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
 	memcpy(hw->mac.addr, addr->sa_data, netdev->addr_len);
 
-	hw->mac.ops.set_rar(hw, 0, hw->mac.addr, adapter->num_vfs,
-			    IXGBE_RAH_AV);
+	hw->mac.ops.set_rar(hw, 0, hw->mac.addr, VMDQ_P(0), IXGBE_RAH_AV);
 
 	return 0;
 }
@@ -6485,12 +6522,15 @@ static int ixgbe_add_sanmac_netdev(struct net_device *dev)
 {
 	int err = 0;
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
-	struct ixgbe_mac_info *mac = &adapter->hw.mac;
+	struct ixgbe_hw *hw = &adapter->hw;
 
-	if (is_valid_ether_addr(mac->san_addr)) {
+	if (is_valid_ether_addr(hw->mac.san_addr)) {
 		rtnl_lock();
-		err = dev_addr_add(dev, mac->san_addr, NETDEV_HW_ADDR_T_SAN);
+		err = dev_addr_add(dev, hw->mac.san_addr, NETDEV_HW_ADDR_T_SAN);
 		rtnl_unlock();
+
+		/* update SAN MAC vmdq pool selection */
+		hw->mac.ops.set_vmdq_san_mac(hw, VMDQ_P(0));
 	}
 	return err;
 }
@@ -6533,11 +6573,8 @@ static void ixgbe_netpoll(struct net_device *netdev)
 
 	adapter->flags |= IXGBE_FLAG_IN_NETPOLL;
 	if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED) {
-		int num_q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-		for (i = 0; i < num_q_vectors; i++) {
-			struct ixgbe_q_vector *q_vector = adapter->q_vector[i];
-			ixgbe_msix_clean_rings(0, q_vector);
-		}
+		for (i = 0; i < adapter->num_q_vectors; i++)
+			ixgbe_msix_clean_rings(0, adapter->q_vector[i]);
 	} else {
 		ixgbe_intr(adapter->pdev->irq, netdev);
 	}
@@ -6594,8 +6631,9 @@ static struct rtnl_link_stats64 *ixgbe_get_stats64(struct net_device *netdev,
 }
 
 #ifdef CONFIG_IXGBE_DCB
-/* ixgbe_validate_rtr - verify 802.1Qp to Rx packet buffer mapping is valid.
- * #adapter: pointer to ixgbe_adapter
+/**
+ * ixgbe_validate_rtr - verify 802.1Qp to Rx packet buffer mapping is valid.
+ * @adapter: pointer to ixgbe_adapter
  * @tc: number of traffic classes currently enabled
  *
  * Configure a valid 802.1Qp to Rx packet buffer mapping ie confirm
@@ -6630,8 +6668,33 @@ static void ixgbe_validate_rtr(struct ixgbe_adapter *adapter, u8 tc)
 	return;
 }
 
-/* ixgbe_setup_tc - routine to configure net_device for multiple traffic
- * classes.
+/**
+ * ixgbe_set_prio_tc_map - Configure netdev prio tc map
+ * @adapter: Pointer to adapter struct
+ *
+ * Populate the netdev user priority to tc map
+ */
+static void ixgbe_set_prio_tc_map(struct ixgbe_adapter *adapter)
+{
+	struct net_device *dev = adapter->netdev;
+	struct ixgbe_dcb_config *dcb_cfg = &adapter->dcb_cfg;
+	struct ieee_ets *ets = adapter->ixgbe_ieee_ets;
+	u8 prio;
+
+	for (prio = 0; prio < MAX_USER_PRIORITY; prio++) {
+		u8 tc = 0;
+
+		if (adapter->dcbx_cap & DCB_CAP_DCBX_VER_CEE)
+			tc = ixgbe_dcb_get_tc_from_up(dcb_cfg, 0, prio);
+		else if (ets)
+			tc = ets->prio_tc[prio];
+
+		netdev_set_prio_tc_map(dev, prio, tc);
+	}
+}
+
+/**
+ * ixgbe_setup_tc - configure net_device for multiple traffic classes
  *
  * @netdev: net device to configure
  * @tc: number of traffic classes to enable
@@ -6641,12 +6704,6 @@ int ixgbe_setup_tc(struct net_device *dev, u8 tc)
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
 	struct ixgbe_hw *hw = &adapter->hw;
 
-	/* Multiple traffic classes requires multiple queues */
-	if (!(adapter->flags & IXGBE_FLAG_MSIX_ENABLED)) {
-		e_err(drv, "Enable failed, needs MSI-X\n");
-		return -EINVAL;
-	}
-
 	/* Hardware supports up to 8 traffic classes */
 	if (tc > adapter->dcb_cfg.num_tcs.pg_tcs ||
 	    (hw->mac.type == ixgbe_mac_82598EB &&
@@ -6663,8 +6720,9 @@ int ixgbe_setup_tc(struct net_device *dev, u8 tc)
 
 	if (tc) {
 		netdev_set_num_tc(dev, tc);
+		ixgbe_set_prio_tc_map(adapter);
+
 		adapter->flags |= IXGBE_FLAG_DCB_ENABLED;
-		adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
 
 		if (adapter->hw.mac.type == ixgbe_mac_82598EB) {
 			adapter->last_lfc_mode = adapter->hw.fc.requested_mode;
@@ -6672,11 +6730,11 @@ int ixgbe_setup_tc(struct net_device *dev, u8 tc)
 		}
 	} else {
 		netdev_reset_tc(dev);
+
 		if (adapter->hw.mac.type == ixgbe_mac_82598EB)
 			adapter->hw.fc.requested_mode = adapter->last_lfc_mode;
 
 		adapter->flags &= ~IXGBE_FLAG_DCB_ENABLED;
-		adapter->flags |= IXGBE_FLAG_FDIR_HASH_CAPABLE;
 
 		adapter->temp_dcb_cfg.pfc_mode_enable = false;
 		adapter->dcb_cfg.pfc_mode_enable = false;
@@ -6706,10 +6764,6 @@ static netdev_features_t ixgbe_fix_features(struct net_device *netdev,
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 
-	/* return error if RXHASH is being enabled when RSS is not supported */
-	if (!(adapter->flags & IXGBE_FLAG_RSS_ENABLED))
-		features &= ~NETIF_F_RXHASH;
-
 	/* If Rx checksum is disabled, then RSC/LRO should also be disabled */
 	if (!(features & NETIF_F_RXCSUM))
 		features &= ~NETIF_F_LRO;
@@ -6749,20 +6803,40 @@ static int ixgbe_set_features(struct net_device *netdev,
 	 * Check if Flow Director n-tuple support was enabled or disabled.  If
 	 * the state changed, we need to reset.
 	 */
-	if (!(features & NETIF_F_NTUPLE)) {
-		if (adapter->flags & IXGBE_FLAG_FDIR_PERFECT_CAPABLE) {
-			/* turn off Flow Director, set ATR and reset */
-			if ((adapter->flags & IXGBE_FLAG_RSS_ENABLED) &&
-			    !(adapter->flags & IXGBE_FLAG_DCB_ENABLED))
-				adapter->flags |= IXGBE_FLAG_FDIR_HASH_CAPABLE;
-			need_reset = true;
-		}
-		adapter->flags &= ~IXGBE_FLAG_FDIR_PERFECT_CAPABLE;
-	} else if (!(adapter->flags & IXGBE_FLAG_FDIR_PERFECT_CAPABLE)) {
+	switch (features & NETIF_F_NTUPLE) {
+	case NETIF_F_NTUPLE:
 		/* turn off ATR, enable perfect filters and reset */
+		if (!(adapter->flags & IXGBE_FLAG_FDIR_PERFECT_CAPABLE))
+			need_reset = true;
+
 		adapter->flags &= ~IXGBE_FLAG_FDIR_HASH_CAPABLE;
 		adapter->flags |= IXGBE_FLAG_FDIR_PERFECT_CAPABLE;
-		need_reset = true;
+		break;
+	default:
+		/* turn off perfect filters, enable ATR and reset */
+		if (adapter->flags & IXGBE_FLAG_FDIR_PERFECT_CAPABLE)
+			need_reset = true;
+
+		adapter->flags &= ~IXGBE_FLAG_FDIR_PERFECT_CAPABLE;
+
+		/* We cannot enable ATR if SR-IOV is enabled */
+		if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
+			break;
+
+		/* We cannot enable ATR if we have 2 or more traffic classes */
+		if (netdev_get_num_tc(netdev) > 1)
+			break;
+
+		/* We cannot enable ATR if RSS is disabled */
+		if (adapter->ring_feature[RING_F_RSS].limit <= 1)
+			break;
+
+		/* A sample rate of 0 indicates ATR disabled */
+		if (!adapter->atr_sample_rate)
+			break;
+
+		adapter->flags |= IXGBE_FLAG_FDIR_HASH_CAPABLE;
+		break;
 	}
 
 	if (features & NETIF_F_HW_VLAN_RX)
@@ -6786,7 +6860,10 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm,
 			     u16 flags)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
-	int err = -EOPNOTSUPP;
+	int err;
+
+	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
+		return -EOPNOTSUPP;
 
 	if (ndm->ndm_state & NUD_PERMANENT) {
 		pr_info("%s: FDB only supports static addresses\n",
@@ -6794,13 +6871,17 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm,
 		return -EINVAL;
 	}
 
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
-		if (is_unicast_ether_addr(addr))
+	if (is_unicast_ether_addr(addr)) {
+		u32 rar_uc_entries = IXGBE_MAX_PF_MACVLANS;
+
+		if (netdev_uc_count(dev) < rar_uc_entries)
 			err = dev_uc_add_excl(dev, addr);
-		else if (is_multicast_ether_addr(addr))
-			err = dev_mc_add_excl(dev, addr);
 		else
-			err = -EINVAL;
+			err = -ENOMEM;
+	} else if (is_multicast_ether_addr(addr)) {
+		err = dev_mc_add_excl(dev, addr);
+	} else {
+		err = -EINVAL;
 	}
 
 	/* Only return duplicate errors if NLM_F_EXCL is set */
@@ -6889,26 +6970,6 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_fdb_dump		= ixgbe_ndo_fdb_dump,
 };
 
-static void __devinit ixgbe_probe_vf(struct ixgbe_adapter *adapter,
-				     const struct ixgbe_info *ii)
-{
-#ifdef CONFIG_PCI_IOV
-	struct ixgbe_hw *hw = &adapter->hw;
-
-	if (hw->mac.type == ixgbe_mac_82598EB)
-		return;
-
-	/* The 82599 supports up to 64 VFs per physical function
-	 * but this implementation limits allocation to 63 so that
-	 * basic networking resources are still available to the
-	 * physical function.  If the user requests greater thn
-	 * 63 VFs then it is an error - reset to default of zero.
-	 */
-	adapter->num_vfs = (max_vfs > 63) ? 0 : max_vfs;
-	ixgbe_enable_sriov(adapter, ii);
-#endif /* CONFIG_PCI_IOV */
-}
-
 /**
  * ixgbe_wol_supported - Check whether device supports WoL
  * @hw: hw specific details
@@ -6935,6 +6996,7 @@ int ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
 			if (hw->bus.func != 0)
 				break;
 		case IXGBE_SUBDEV_ID_82599_SFP:
+		case IXGBE_SUBDEV_ID_82599_RNDC:
 			is_wol_supported = 1;
 			break;
 		}
@@ -6982,6 +7044,7 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 	int i, err, pci_using_dac;
 	u8 part_str[IXGBE_PBANUM_LENGTH];
 	unsigned int indices = num_possible_cpus();
+	unsigned int dcb_max = 0;
 #ifdef IXGBE_FCOE
 	u16 device_caps;
 #endif
@@ -7031,7 +7094,12 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 	pci_save_state(pdev);
 
 #ifdef CONFIG_IXGBE_DCB
-	indices *= MAX_TRAFFIC_CLASS;
+	if (ii->mac == ixgbe_mac_82598EB)
+		dcb_max = min_t(unsigned int, indices * MAX_TRAFFIC_CLASS,
+				IXGBE_MAX_RSS_INDICES);
+	else
+		dcb_max = min_t(unsigned int, indices * MAX_TRAFFIC_CLASS,
+				IXGBE_MAX_FDIR_INDICES);
 #endif
 
 	if (ii->mac == ixgbe_mac_82598EB)
@@ -7043,6 +7111,7 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 	indices += min_t(unsigned int, num_possible_cpus(),
 			 IXGBE_MAX_FCOE_INDICES);
 #endif
+	indices = max_t(unsigned int, dcb_max, indices);
 	netdev = alloc_etherdev_mq(sizeof(struct ixgbe_adapter), indices);
 	if (!netdev) {
 		err = -ENOMEM;
@@ -7149,8 +7218,10 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 		goto err_sw_init;
 	}
 
-	ixgbe_probe_vf(adapter, ii);
+#ifdef CONFIG_PCI_IOV
+	ixgbe_enable_sriov(adapter, ii);
 
+#endif
 	netdev->features = NETIF_F_SG |
 			   NETIF_F_IP_CSUM |
 			   NETIF_F_IPV6_CSUM |
@@ -7186,10 +7257,6 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 	netdev->priv_flags |= IFF_UNICAST_FLT;
 	netdev->priv_flags |= IFF_SUPP_NOFCS;
 
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
-		adapter->flags &= ~(IXGBE_FLAG_RSS_ENABLED |
-				    IXGBE_FLAG_DCB_ENABLED);
-
 #ifdef CONFIG_IXGBE_DCB
 	netdev->dcbnl_ops = &dcbnl_ops;
 #endif
@@ -7201,11 +7268,15 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 			if (device_caps & IXGBE_DEVICE_CAPS_FCOE_OFFLOADS)
 				adapter->flags &= ~IXGBE_FLAG_FCOE_CAPABLE;
 		}
-	}
-	if (adapter->flags & IXGBE_FLAG_FCOE_CAPABLE) {
-		netdev->vlan_features |= NETIF_F_FCOE_CRC;
-		netdev->vlan_features |= NETIF_F_FSO;
-		netdev->vlan_features |= NETIF_F_FCOE_MTU;
+
+		adapter->ring_feature[RING_F_FCOE].limit = IXGBE_FCRETA_SIZE;
+
+		netdev->features |= NETIF_F_FSO |
+				    NETIF_F_FCOE_CRC;
+
+		netdev->vlan_features |= NETIF_F_FSO |
+					 NETIF_F_FCOE_CRC |
+					 NETIF_F_FCOE_MTU;
 	}
 #endif /* IXGBE_FCOE */
 	if (pci_using_dac) {
@@ -7244,11 +7315,6 @@ static int __devinit ixgbe_probe(struct pci_dev *pdev,
 	if (err)
 		goto err_sw_init;
 
-	if (!(adapter->flags & IXGBE_FLAG_RSS_ENABLED)) {
-		netdev->hw_features &= ~NETIF_F_RXHASH;
-		netdev->features &= ~NETIF_F_RXHASH;
-	}
-
 	/* WOL not supported for all devices */
 	adapter->wol = 0;
 	hw->eeprom.ops.read(hw, 0x2c, &adapter->eeprom_cap);
@@ -7359,8 +7425,7 @@ err_register:
 	ixgbe_release_hw_control(adapter);
 	ixgbe_clear_interrupt_scheme(adapter);
 err_sw_init:
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED)
-		ixgbe_disable_sriov(adapter);
+	ixgbe_disable_sriov(adapter);
 	adapter->flags2 &= ~IXGBE_FLAG2_SEARCH_FOR_SFP;
 	iounmap(hw->hw_addr);
 err_ioremap:
@@ -7407,25 +7472,13 @@ static void __devexit ixgbe_remove(struct pci_dev *pdev)
 	ixgbe_sysfs_exit(adapter);
 #endif /* CONFIG_IXGBE_HWMON */
 
-#ifdef IXGBE_FCOE
-	if (adapter->flags & IXGBE_FLAG_FCOE_ENABLED)
-		ixgbe_cleanup_fcoe(adapter);
-
-#endif /* IXGBE_FCOE */
-
 	/* remove the added san mac */
 	ixgbe_del_sanmac_netdev(netdev);
 
 	if (netdev->reg_state == NETREG_REGISTERED)
 		unregister_netdev(netdev);
 
-	if (adapter->flags & IXGBE_FLAG_SRIOV_ENABLED) {
-		if (!(ixgbe_check_vf_assignment(adapter)))
-			ixgbe_disable_sriov(adapter);
-		else
-			e_dev_warn("Unloading driver while VFs are assigned "
-				   "- VFs will not be deallocated\n");
-	}
+	ixgbe_disable_sriov(adapter);
 
 	ixgbe_clear_interrupt_scheme(adapter);
 
@@ -7516,11 +7569,11 @@ static pci_ers_result_t ixgbe_io_error_detected(struct pci_dev *pdev,
 		}
 
 		/* Find the pci device of the offending VF */
-		vfdev = pci_get_device(IXGBE_INTEL_VENDOR_ID, device_id, NULL);
+		vfdev = pci_get_device(PCI_VENDOR_ID_INTEL, device_id, NULL);
 		while (vfdev) {
 			if (vfdev->devfn == (req_id & 0xFF))
 				break;
-			vfdev = pci_get_device(IXGBE_INTEL_VENDOR_ID,
+			vfdev = pci_get_device(PCI_VENDOR_ID_INTEL,
 					       device_id, vfdev);
 		}
 		/*
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index 24117709d6a..71659edf81a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -907,6 +907,8 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 		  * 8   SFP_act_lmt_DA_CORE1 - 82599-specific
 		  * 9   SFP_1g_cu_CORE0 - 82599-specific
 		  * 10  SFP_1g_cu_CORE1 - 82599-specific
+		  * 11  SFP_1g_sx_CORE0 - 82599-specific
+		  * 12  SFP_1g_sx_CORE1 - 82599-specific
 		  */
 		if (hw->mac.type == ixgbe_mac_82598EB) {
 			if (cable_tech & IXGBE_SFF_DA_PASSIVE_CABLE)
@@ -957,6 +959,13 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 				else
 					hw->phy.sfp_type =
 						ixgbe_sfp_type_1g_cu_core1;
+			} else if (comp_codes_1g & IXGBE_SFF_1GBASESX_CAPABLE) {
+				if (hw->bus.lan_id == 0)
+					hw->phy.sfp_type =
+						ixgbe_sfp_type_1g_sx_core0;
+				else
+					hw->phy.sfp_type =
+						ixgbe_sfp_type_1g_sx_core1;
 			} else {
 				hw->phy.sfp_type = ixgbe_sfp_type_unknown;
 			}
@@ -1049,7 +1058,9 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 		/* Verify supported 1G SFP modules */
 		if (comp_codes_10g == 0 &&
 		    !(hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
-		      hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0)) {
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0 ||
+		      hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1)) {
 			hw->phy.type = ixgbe_phy_sfp_unsupported;
 			status = IXGBE_ERR_SFP_NOT_SUPPORTED;
 			goto out;
@@ -1064,7 +1075,9 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 		hw->mac.ops.get_device_caps(hw, &enforce_sfp);
 		if (!(enforce_sfp & IXGBE_DEVICE_CAPS_ALLOW_ANY_SFP) &&
 		    !((hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core0) ||
-		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1))) {
+		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_cu_core1) ||
+		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core0) ||
+		      (hw->phy.sfp_type == ixgbe_sfp_type_1g_sx_core1))) {
 			/* Make sure we're a supported PHY type */
 			if (hw->phy.type == ixgbe_phy_sfp_intel) {
 				status = 0;
@@ -1128,10 +1141,12 @@ s32 ixgbe_get_sfp_init_sequence_offsets(struct ixgbe_hw *hw,
 	 * SR modules
 	 */
 	if (sfp_type == ixgbe_sfp_type_da_act_lmt_core0 ||
-	    sfp_type == ixgbe_sfp_type_1g_cu_core0)
+	    sfp_type == ixgbe_sfp_type_1g_cu_core0 ||
+	    sfp_type == ixgbe_sfp_type_1g_sx_core0)
 		sfp_type = ixgbe_sfp_type_srlr_core0;
 	else if (sfp_type == ixgbe_sfp_type_da_act_lmt_core1 ||
-	         sfp_type == ixgbe_sfp_type_1g_cu_core1)
+		 sfp_type == ixgbe_sfp_type_1g_cu_core1 ||
+		 sfp_type == ixgbe_sfp_type_1g_sx_core1)
 		sfp_type = ixgbe_sfp_type_srlr_core1;
 
 	/* Read offset to PHY init contents */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index dcebd128bec..3456d561714 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -26,6 +26,7 @@
 *******************************************************************************/
 #include "ixgbe.h"
 #include <linux/export.h>
+#include <linux/ptp_classify.h>
 
 /*
  * The 82599 and the X540 do not have true 64bit nanosecond scale
@@ -100,9 +101,13 @@
 #define NSECS_PER_SEC 1000000000ULL
 #endif
 
+static struct sock_filter ptp_filter[] = {
+	PTP_FILTER
+};
+
 /**
  * ixgbe_ptp_read - read raw cycle counter (to be used by time counter)
- * @cc - the cyclecounter structure
+ * @cc: the cyclecounter structure
  *
  * this function reads the cyclecounter registers and is called by the
  * cyclecounter structure used to construct a ns counter from the
@@ -123,8 +128,8 @@ static cycle_t ixgbe_ptp_read(const struct cyclecounter *cc)
 
 /**
  * ixgbe_ptp_adjfreq
- * @ptp - the ptp clock structure
- * @ppb - parts per billion adjustment from base
+ * @ptp: the ptp clock structure
+ * @ppb: parts per billion adjustment from base
  *
  * adjust the frequency of the ptp cycle counter by the
  * indicated ppb from the base frequency.
@@ -170,8 +175,8 @@ static int ixgbe_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 
 /**
  * ixgbe_ptp_adjtime
- * @ptp - the ptp clock structure
- * @delta - offset to adjust the cycle counter by
+ * @ptp: the ptp clock structure
+ * @delta: offset to adjust the cycle counter by
  *
  * adjust the timer by resetting the timecounter structure.
  */
@@ -198,8 +203,8 @@ static int ixgbe_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 
 /**
  * ixgbe_ptp_gettime
- * @ptp - the ptp clock structure
- * @ts - timespec structure to hold the current time value
+ * @ptp: the ptp clock structure
+ * @ts: timespec structure to hold the current time value
  *
  * read the timecounter and return the correct value on ns,
  * after converting it into a struct timespec.
@@ -224,8 +229,8 @@ static int ixgbe_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
 
 /**
  * ixgbe_ptp_settime
- * @ptp - the ptp clock structure
- * @ts - the timespec containing the new time for the cycle counter
+ * @ptp: the ptp clock structure
+ * @ts: the timespec containing the new time for the cycle counter
  *
  * reset the timecounter to use a new base value instead of the kernel
  * wall timer value.
@@ -251,9 +256,9 @@ static int ixgbe_ptp_settime(struct ptp_clock_info *ptp,
 
 /**
  * ixgbe_ptp_enable
- * @ptp - the ptp clock structure
- * @rq - the requested feature to change
- * @on - whether to enable or disable the feature
+ * @ptp: the ptp clock structure
+ * @rq: the requested feature to change
+ * @on: whether to enable or disable the feature
  *
  * enable (or disable) ancillary features of the phc subsystem.
  * our driver only supports the PPS feature on the X540
@@ -289,8 +294,8 @@ static int ixgbe_ptp_enable(struct ptp_clock_info *ptp,
 
 /**
  * ixgbe_ptp_check_pps_event
- * @adapter - the private adapter structure
- * @eicr - the interrupt cause register value
+ * @adapter: the private adapter structure
+ * @eicr: the interrupt cause register value
  *
  * This function is called by the interrupt routine when checking for
  * interrupts. It will check and handle a pps event.
@@ -307,20 +312,21 @@ void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter, u32 eicr)
 	    !(adapter->flags2 & IXGBE_FLAG2_PTP_PPS_ENABLED))
 		return;
 
-	switch (hw->mac.type) {
-	case ixgbe_mac_X540:
-		if (eicr & IXGBE_EICR_TIMESYNC)
+	if (unlikely(eicr & IXGBE_EICR_TIMESYNC)) {
+		switch (hw->mac.type) {
+		case ixgbe_mac_X540:
 			ptp_clock_event(adapter->ptp_clock, &event);
-		break;
-	default:
-		break;
+			break;
+		default:
+			break;
+		}
 	}
 }
 
 /**
  * ixgbe_ptp_enable_sdp
- * @hw - the hardware private structure
- * @shift - the clock shift for calculating nanoseconds
+ * @hw: the hardware private structure
+ * @shift: the clock shift for calculating nanoseconds
  *
  * this function enables the clock out feature on the sdp0 for the
  * X540 device. It will create a 1second periodic output that can be
@@ -393,7 +399,7 @@ static void ixgbe_ptp_enable_sdp(struct ixgbe_hw *hw, int shift)
 
 /**
  * ixgbe_ptp_disable_sdp
- * @hw - the private hardware structure
+ * @hw: the private hardware structure
  *
  * this function disables the auxiliary SDP clock out feature
  */
@@ -425,6 +431,68 @@ void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter)
 }
 
 /**
+ * ixgbe_ptp_match - determine if this skb matches a ptp packet
+ * @skb: pointer to the skb
+ * @hwtstamp: pointer to the hwtstamp_config to check
+ *
+ * Determine whether the skb should have been timestamped, assuming the
+ * hwtstamp was set via the hwtstamp ioctl. Returns non-zero when the packet
+ * should have a timestamp waiting in the registers, and 0 otherwise.
+ *
+ * V1 packets have to check the version type to determine whether they are
+ * correct. However, we can't directly access the data because it might be
+ * fragmented in the SKB, in paged memory. In order to work around this, we
+ * use skb_copy_bits which will properly copy the data whether it is in the
+ * paged memory fragments or not. We have to copy the IP header as well as the
+ * message type.
+ */
+static int ixgbe_ptp_match(struct sk_buff *skb, int rx_filter)
+{
+	struct iphdr iph;
+	u8 msgtype;
+	unsigned int type, offset;
+
+	if (rx_filter == HWTSTAMP_FILTER_NONE)
+		return 0;
+
+	type = sk_run_filter(skb, ptp_filter);
+
+	if (likely(rx_filter == HWTSTAMP_FILTER_PTP_V2_EVENT))
+		return type & PTP_CLASS_V2;
+
+	/* For the remaining cases actually check message type */
+	switch (type) {
+	case PTP_CLASS_V1_IPV4:
+		skb_copy_bits(skb, OFF_IHL, &iph, sizeof(iph));
+		offset = ETH_HLEN + (iph.ihl << 2) + UDP_HLEN + OFF_PTP_CONTROL;
+		break;
+	case PTP_CLASS_V1_IPV6:
+		offset = OFF_PTP6 + OFF_PTP_CONTROL;
+		break;
+	default:
+		/* other cases invalid or handled above */
+		return 0;
+	}
+
+	/* Make sure our buffer is long enough */
+	if (skb->len < offset)
+		return 0;
+
+	skb_copy_bits(skb, offset, &msgtype, sizeof(msgtype));
+
+	switch (rx_filter) {
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+		return (msgtype == IXGBE_RXMTRL_V1_SYNC_MSG);
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		return (msgtype == IXGBE_RXMTRL_V1_DELAY_REQ_MSG);
+		break;
+	default:
+		return 0;
+	}
+}
+
+/**
  * ixgbe_ptp_tx_hwtstamp - utility function which checks for TX time stamp
  * @q_vector: structure containing interrupt and ring information
  * @skb: particular skb to send timestamp with
@@ -473,6 +541,7 @@ void ixgbe_ptp_tx_hwtstamp(struct ixgbe_q_vector *q_vector,
 /**
  * ixgbe_ptp_rx_hwtstamp - utility function which checks for RX time stamp
  * @q_vector: structure containing interrupt and ring information
+ * @rx_desc: the rx descriptor
  * @skb: particular skb to send timestamp with
  *
  * if the timestamp is valid, we convert it into the timecounter ns
@@ -480,6 +549,7 @@ void ixgbe_ptp_tx_hwtstamp(struct ixgbe_q_vector *q_vector,
  * is passed up the network stack
  */
 void ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
+			   union ixgbe_adv_rx_desc *rx_desc,
 			   struct sk_buff *skb)
 {
 	struct ixgbe_adapter *adapter;
@@ -497,21 +567,33 @@ void ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
 	hw = &adapter->hw;
 
 	tsyncrxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
+
+	/* Check if we have a valid timestamp and make sure the skb should
+	 * have been timestamped */
+	if (likely(!(tsyncrxctl & IXGBE_TSYNCRXCTL_VALID) ||
+		   !ixgbe_ptp_match(skb, adapter->rx_hwtstamp_filter)))
+		return;
+
+	/*
+	 * Always read the registers, in order to clear a possible fault
+	 * because of stagnant RX timestamp values for a packet that never
+	 * reached the queue.
+	 */
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPL);
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPH) << 32;
 
 	/*
-	 * If this bit is set, then the RX registers contain the time stamp. No
-	 * other packet will be time stamped until we read these registers, so
-	 * read the registers to make them available again. Because only one
-	 * packet can be time stamped at a time, we know that the register
-	 * values must belong to this one here and therefore we don't need to
-	 * compare any of the additional attributes stored for it.
+	 * If the timestamp bit is set in the packet's descriptor, we know the
+	 * timestamp belongs to this packet. No other packet can be
+	 * timestamped until the registers for timestamping have been read.
+	 * Therefor only one packet with this bit can be in the queue at a
+	 * time, and the rx timestamp values that were in the registers belong
+	 * to this packet.
 	 *
 	 * If nothing went wrong, then it should have a skb_shared_tx that we
 	 * can turn into a skb_shared_hwtstamps.
 	 */
-	if (!(tsyncrxctl & IXGBE_TSYNCRXCTL_VALID))
+	if (unlikely(!ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS)))
 		return;
 
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
@@ -539,6 +621,11 @@ void ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
  * type has to be specified. Matching the kind of event packet is
  * not supported, with the exception of "all V2 events regardless of
  * level 2 or 4".
+ *
+ * Since hardware always timestamps Path delay packets when timestamping V2
+ * packets, regardless of the type specified in the register, only use V2
+ * Event mode. This more accurately tells the user what the hardware is going
+ * to do anyways.
  */
 int ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
 			     struct ifreq *ifr, int cmd)
@@ -582,41 +669,30 @@ int ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
 		tsync_rx_mtrl = IXGBE_RXMTRL_V1_DELAY_REQ_MSG;
 		is_l4 = true;
 		break;
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
-		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L2_L4_V2;
-		tsync_rx_mtrl = IXGBE_RXMTRL_V2_SYNC_MSG;
-		is_l2 = true;
-		is_l4 = true;
-		config.rx_filter = HWTSTAMP_FILTER_SOME;
-		break;
 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
-		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L2_L4_V2;
-		tsync_rx_mtrl = IXGBE_RXMTRL_V2_DELAY_REQ_MSG;
-		is_l2 = true;
-		is_l4 = true;
-		config.rx_filter = HWTSTAMP_FILTER_SOME;
-		break;
-	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
-	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
-	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_EVENT_V2;
-		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 		is_l2 = true;
 		is_l4 = true;
+		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 	case HWTSTAMP_FILTER_ALL:
 	default:
 		/*
-		 * register RXMTRL must be set, therefore it is not
-		 * possible to time stamp both V1 Sync and Delay_Req messages
-		 * and hardware does not support timestamping all packets
-		 * => return error
+		 * register RXMTRL must be set in order to do V1 packets,
+		 * therefore it is not possible to time stamp both V1 Sync and
+		 * Delay_Req messages and hardware does not support
+		 * timestamping all packets => return error
 		 */
+		config.rx_filter = HWTSTAMP_FILTER_NONE;
 		return -ERANGE;
 	}
 
@@ -626,6 +702,9 @@ int ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
 		return 0;
 	}
 
+	/* Store filter value for later use */
+	adapter->rx_hwtstamp_filter = config.rx_filter;
+
 	/* define ethertype filter for timestamped packets */
 	if (is_l2)
 		IXGBE_WRITE_REG(hw, IXGBE_ETQF(3),
@@ -690,7 +769,7 @@ int ixgbe_ptp_hwtstamp_ioctl(struct ixgbe_adapter *adapter,
 
 /**
  * ixgbe_ptp_start_cyclecounter - create the cycle counter from hw
- * @adapter - pointer to the adapter structure
+ * @adapter: pointer to the adapter structure
  *
  * this function initializes the timecounter and cyclecounter
  * structures for use in generated a ns counter from the arbitrary
@@ -826,7 +905,7 @@ void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
 
 /**
  * ixgbe_ptp_init
- * @adapter - the ixgbe private adapter structure
+ * @adapter: the ixgbe private adapter structure
  *
  * This function performs the required steps for enabling ptp
  * support. If ptp support has already been loaded it simply calls the
@@ -870,6 +949,10 @@ void ixgbe_ptp_init(struct ixgbe_adapter *adapter)
 		return;
 	}
 
+	/* initialize the ptp filter */
+	if (ptp_filter_init(ptp_filter, ARRAY_SIZE(ptp_filter)))
+		e_dev_warn("ptp_filter_init failed\n");
+
 	spin_lock_init(&adapter->tmreg_lock);
 
 	ixgbe_ptp_start_cyclecounter(adapter);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index 2d971d18696..4fea8716ab6 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -44,50 +44,15 @@
 #include "ixgbe_sriov.h"
 
 #ifdef CONFIG_PCI_IOV
-static int ixgbe_find_enabled_vfs(struct ixgbe_adapter *adapter)
-{
-	struct pci_dev *pdev = adapter->pdev;
-	struct pci_dev *pvfdev;
-	u16 vf_devfn = 0;
-	int device_id;
-	int vfs_found = 0;
-
-	switch (adapter->hw.mac.type) {
-	case ixgbe_mac_82599EB:
-		device_id = IXGBE_DEV_ID_82599_VF;
-		break;
-	case ixgbe_mac_X540:
-		device_id = IXGBE_DEV_ID_X540_VF;
-		break;
-	default:
-		device_id = 0;
-		break;
-	}
-
-	vf_devfn = pdev->devfn + 0x80;
-	pvfdev = pci_get_device(IXGBE_INTEL_VENDOR_ID, device_id, NULL);
-	while (pvfdev) {
-		if (pvfdev->devfn == vf_devfn &&
-		    (pvfdev->bus->number >= pdev->bus->number))
-			vfs_found++;
-		vf_devfn += 2;
-		pvfdev = pci_get_device(IXGBE_INTEL_VENDOR_ID,
-					device_id, pvfdev);
-	}
-
-	return vfs_found;
-}
-
 void ixgbe_enable_sriov(struct ixgbe_adapter *adapter,
 			 const struct ixgbe_info *ii)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	int err = 0;
 	int num_vf_macvlans, i;
 	struct vf_macvlans *mv_list;
 	int pre_existing_vfs = 0;
 
-	pre_existing_vfs = ixgbe_find_enabled_vfs(adapter);
+	pre_existing_vfs = pci_num_vf(adapter->pdev);
 	if (!pre_existing_vfs && !adapter->num_vfs)
 		return;
 
@@ -106,16 +71,33 @@ void ixgbe_enable_sriov(struct ixgbe_adapter *adapter,
 			 "enabled for this device - Please reload all "
 			 "VF drivers to avoid spoofed packet errors\n");
 	} else {
+		int err;
+		/*
+		 * The 82599 supports up to 64 VFs per physical function
+		 * but this implementation limits allocation to 63 so that
+		 * basic networking resources are still available to the
+		 * physical function.  If the user requests greater thn
+		 * 63 VFs then it is an error - reset to default of zero.
+		 */
+		adapter->num_vfs = min_t(unsigned int, adapter->num_vfs, 63);
+
 		err = pci_enable_sriov(adapter->pdev, adapter->num_vfs);
+		if (err) {
+			e_err(probe, "Failed to enable PCI sriov: %d\n", err);
+			adapter->num_vfs = 0;
+			return;
+		}
 	}
-	if (err) {
-		e_err(probe, "Failed to enable PCI sriov: %d\n", err);
-		goto err_novfs;
-	}
-	adapter->flags |= IXGBE_FLAG_SRIOV_ENABLED;
 
+	adapter->flags |= IXGBE_FLAG_SRIOV_ENABLED;
 	e_info(probe, "SR-IOV enabled with %d VFs\n", adapter->num_vfs);
 
+	/* Enable VMDq flag so device will be set in VM mode */
+	adapter->flags |= IXGBE_FLAG_VMDQ_ENABLED;
+	if (!adapter->ring_feature[RING_F_VMDQ].limit)
+		adapter->ring_feature[RING_F_VMDQ].limit = 1;
+	adapter->ring_feature[RING_F_VMDQ].offset = adapter->num_vfs;
+
 	num_vf_macvlans = hw->mac.num_rar_entries -
 	(IXGBE_MAX_PF_MACVLANS + 1 + adapter->num_vfs);
 
@@ -146,12 +128,39 @@ void ixgbe_enable_sriov(struct ixgbe_adapter *adapter,
 		 * and memory allocated set up the mailbox parameters
 		 */
 		ixgbe_init_mbx_params_pf(hw);
-		memcpy(&hw->mbx.ops, ii->mbx_ops,
-		       sizeof(hw->mbx.ops));
+		memcpy(&hw->mbx.ops, ii->mbx_ops, sizeof(hw->mbx.ops));
+
+		/* limit trafffic classes based on VFs enabled */
+		if ((adapter->hw.mac.type == ixgbe_mac_82599EB) &&
+		    (adapter->num_vfs < 16)) {
+			adapter->dcb_cfg.num_tcs.pg_tcs = MAX_TRAFFIC_CLASS;
+			adapter->dcb_cfg.num_tcs.pfc_tcs = MAX_TRAFFIC_CLASS;
+		} else if (adapter->num_vfs < 32) {
+			adapter->dcb_cfg.num_tcs.pg_tcs = 4;
+			adapter->dcb_cfg.num_tcs.pfc_tcs = 4;
+		} else {
+			adapter->dcb_cfg.num_tcs.pg_tcs = 1;
+			adapter->dcb_cfg.num_tcs.pfc_tcs = 1;
+		}
+
+		/* We do not support RSS w/ SR-IOV */
+		adapter->ring_feature[RING_F_RSS].limit = 1;
 
 		/* Disable RSC when in SR-IOV mode */
 		adapter->flags2 &= ~(IXGBE_FLAG2_RSC_CAPABLE |
 				     IXGBE_FLAG2_RSC_ENABLED);
+
+#ifdef IXGBE_FCOE
+		/*
+		 * When SR-IOV is enabled 82599 cannot support jumbo frames
+		 * so we must disable FCoE because we cannot support FCoE MTU.
+		 */
+		if (adapter->hw.mac.type == ixgbe_mac_82599EB)
+			adapter->flags &= ~(IXGBE_FLAG_FCOE_ENABLED |
+					    IXGBE_FLAG_FCOE_CAPABLE);
+#endif
+
+		/* enable spoof checking for all VFs */
 		for (i = 0; i < adapter->num_vfs; i++)
 			adapter->vfinfo[i].spoofchk_enabled = true;
 		return;
@@ -160,31 +169,80 @@ void ixgbe_enable_sriov(struct ixgbe_adapter *adapter,
 	/* Oh oh */
 	e_err(probe, "Unable to allocate memory for VF Data Storage - "
 	      "SRIOV disabled\n");
-	pci_disable_sriov(adapter->pdev);
+	ixgbe_disable_sriov(adapter);
+}
 
-err_novfs:
-	adapter->flags &= ~IXGBE_FLAG_SRIOV_ENABLED;
-	adapter->num_vfs = 0;
+static bool ixgbe_vfs_are_assigned(struct ixgbe_adapter *adapter)
+{
+	struct pci_dev *pdev = adapter->pdev;
+	struct pci_dev *vfdev;
+	int dev_id;
+
+	switch (adapter->hw.mac.type) {
+	case ixgbe_mac_82599EB:
+		dev_id = IXGBE_DEV_ID_82599_VF;
+		break;
+	case ixgbe_mac_X540:
+		dev_id = IXGBE_DEV_ID_X540_VF;
+		break;
+	default:
+		return false;
+	}
+
+	/* loop through all the VFs to see if we own any that are assigned */
+	vfdev = pci_get_device(PCI_VENDOR_ID_INTEL, dev_id, NULL);
+	while (vfdev) {
+		/* if we don't own it we don't care */
+		if (vfdev->is_virtfn && vfdev->physfn == pdev) {
+			/* if it is assigned we cannot release it */
+			if (vfdev->dev_flags & PCI_DEV_FLAGS_ASSIGNED)
+				return true;
+		}
+
+		vfdev = pci_get_device(PCI_VENDOR_ID_INTEL, dev_id, vfdev);
+	}
+
+	return false;
 }
-#endif /* #ifdef CONFIG_PCI_IOV */
 
+#endif /* #ifdef CONFIG_PCI_IOV */
 void ixgbe_disable_sriov(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 gcr;
 	u32 gpie;
 	u32 vmdctl;
-	int i;
+
+	/* set num VFs to 0 to prevent access to vfinfo */
+	adapter->num_vfs = 0;
+
+	/* free VF control structures */
+	kfree(adapter->vfinfo);
+	adapter->vfinfo = NULL;
+
+	/* free macvlan list */
+	kfree(adapter->mv_list);
+	adapter->mv_list = NULL;
+
+	/* if SR-IOV is already disabled then there is nothing to do */
+	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
+		return;
 
 #ifdef CONFIG_PCI_IOV
+	/*
+	 * If our VFs are assigned we cannot shut down SR-IOV
+	 * without causing issues, so just leave the hardware
+	 * available but disabled
+	 */
+	if (ixgbe_vfs_are_assigned(adapter)) {
+		e_dev_warn("Unloading driver while VFs are assigned - VFs will not be deallocated\n");
+		return;
+	}
 	/* disable iov and allow time for transactions to clear */
 	pci_disable_sriov(adapter->pdev);
 #endif
 
 	/* turn off device IOV mode */
-	gcr = IXGBE_READ_REG(hw, IXGBE_GCR_EXT);
-	gcr &= ~(IXGBE_GCR_EXT_SRIOV);
-	IXGBE_WRITE_REG(hw, IXGBE_GCR_EXT, gcr);
+	IXGBE_WRITE_REG(hw, IXGBE_GCR_EXT, 0);
 	gpie = IXGBE_READ_REG(hw, IXGBE_GPIE);
 	gpie &= ~IXGBE_GPIE_VTMODE_MASK;
 	IXGBE_WRITE_REG(hw, IXGBE_GPIE, gpie);
@@ -195,19 +253,14 @@ void ixgbe_disable_sriov(struct ixgbe_adapter *adapter)
 	IXGBE_WRITE_REG(hw, IXGBE_VT_CTL, vmdctl);
 	IXGBE_WRITE_FLUSH(hw);
 
+	/* Disable VMDq flag so device will be set in VM mode */
+	if (adapter->ring_feature[RING_F_VMDQ].limit == 1)
+		adapter->flags &= ~IXGBE_FLAG_VMDQ_ENABLED;
+	adapter->ring_feature[RING_F_VMDQ].offset = 0;
+
 	/* take a breather then clean up driver data */
 	msleep(100);
 
-	/* Release reference to VF devices */
-	for (i = 0; i < adapter->num_vfs; i++) {
-		if (adapter->vfinfo[i].vfdev)
-			pci_dev_put(adapter->vfinfo[i].vfdev);
-	}
-	kfree(adapter->vfinfo);
-	kfree(adapter->mv_list);
-	adapter->vfinfo = NULL;
-
-	adapter->num_vfs = 0;
 	adapter->flags &= ~IXGBE_FLAG_SRIOV_ENABLED;
 }
 
@@ -441,33 +494,16 @@ static int ixgbe_set_vf_macvlan(struct ixgbe_adapter *adapter,
 	return 0;
 }
 
-int ixgbe_check_vf_assignment(struct ixgbe_adapter *adapter)
-{
-#ifdef CONFIG_PCI_IOV
-	int i;
-	for (i = 0; i < adapter->num_vfs; i++) {
-		if (adapter->vfinfo[i].vfdev->dev_flags &
-				PCI_DEV_FLAGS_ASSIGNED)
-			return true;
-	}
-#endif
-	return false;
-}
-
 int ixgbe_vf_configuration(struct pci_dev *pdev, unsigned int event_mask)
 {
 	unsigned char vf_mac_addr[6];
 	struct ixgbe_adapter *adapter = pci_get_drvdata(pdev);
 	unsigned int vfn = (event_mask & 0x3f);
-	struct pci_dev *pvfdev;
-	unsigned int device_id;
-	u16 thisvf_devfn = (pdev->devfn + 0x80 + (vfn << 1)) |
-				(pdev->devfn & 1);
 
 	bool enable = ((event_mask & 0x10000000U) != 0);
 
 	if (enable) {
-		random_ether_addr(vf_mac_addr);
+		eth_random_addr(vf_mac_addr);
 		e_info(probe, "IOV: VF %d is enabled MAC %pM\n",
 		       vfn, vf_mac_addr);
 		/*
@@ -475,31 +511,6 @@ int ixgbe_vf_configuration(struct pci_dev *pdev, unsigned int event_mask)
 		 * for it later.
 		 */
 		memcpy(adapter->vfinfo[vfn].vf_mac_addresses, vf_mac_addr, 6);
-
-		switch (adapter->hw.mac.type) {
-		case ixgbe_mac_82599EB:
-			device_id = IXGBE_DEV_ID_82599_VF;
-			break;
-		case ixgbe_mac_X540:
-			device_id = IXGBE_DEV_ID_X540_VF;
-			break;
-		default:
-			device_id = 0;
-			break;
-		}
-
-		pvfdev = pci_get_device(IXGBE_INTEL_VENDOR_ID, device_id, NULL);
-		while (pvfdev) {
-			if (pvfdev->devfn == thisvf_devfn)
-				break;
-			pvfdev = pci_get_device(IXGBE_INTEL_VENDOR_ID,
-						device_id, pvfdev);
-		}
-		if (pvfdev)
-			adapter->vfinfo[vfn].vfdev = pvfdev;
-		else
-			e_err(drv, "Couldn't find pci dev ptr for VF %4.4x\n",
-			      thisvf_devfn);
 	}
 
 	return 0;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
index 2ab38d5fda9..1be1d30e4e7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.h
@@ -42,7 +42,6 @@ int ixgbe_ndo_get_vf_config(struct net_device *netdev,
 			    int vf, struct ifla_vf_info *ivi);
 void ixgbe_check_vf_rate_limit(struct ixgbe_adapter *adapter);
 void ixgbe_disable_sriov(struct ixgbe_adapter *adapter);
-int ixgbe_check_vf_assignment(struct ixgbe_adapter *adapter);
 #ifdef CONFIG_PCI_IOV
 void ixgbe_enable_sriov(struct ixgbe_adapter *adapter,
 			const struct ixgbe_info *ii);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c
index 1d80b1cefa6..16ddf14e8ba 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sysfs.c
@@ -37,7 +37,6 @@
 #include <linux/netdevice.h>
 #include <linux/hwmon.h>
 
-#ifdef CONFIG_IXGBE_HWMON
 /* hwmon callback functions */
 static ssize_t ixgbe_hwmon_show_location(struct device *dev,
 					 struct device_attribute *attr,
@@ -96,11 +95,11 @@ static ssize_t ixgbe_hwmon_show_maxopthresh(struct device *dev,
 	return sprintf(buf, "%u\n", value);
 }
 
-/*
+/**
  * ixgbe_add_hwmon_attr - Create hwmon attr table for a hwmon sysfs file.
- * @ adapter: pointer to the adapter structure
- * @ offset: offset in the eeprom sensor data table
- * @ type: type of sensor data to display
+ * @adapter: pointer to the adapter structure
+ * @offset: offset in the eeprom sensor data table
+ * @type: type of sensor data to display
  *
  * For each file we want in hwmon's sysfs interface we need a device_attribute
  * This is included in our hwmon_attr struct that contains the references to
@@ -241,5 +240,4 @@ err:
 exit:
 	return rc;
 }
-#endif /* CONFIG_IXGBE_HWMON */
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index 204848d2448..400f86a3117 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -32,9 +32,6 @@
 #include <linux/mdio.h>
 #include <linux/netdevice.h>
 
-/* Vendor ID */
-#define IXGBE_INTEL_VENDOR_ID   0x8086
-
 /* Device IDs */
 #define IXGBE_DEV_ID_82598               0x10B6
 #define IXGBE_DEV_ID_82598_BX            0x1508
@@ -57,6 +54,7 @@
 #define IXGBE_DEV_ID_82599_BACKPLANE_FCOE       0x152a
 #define IXGBE_DEV_ID_82599_SFP_FCOE      0x1529
 #define IXGBE_SUBDEV_ID_82599_SFP        0x11A9
+#define IXGBE_SUBDEV_ID_82599_RNDC       0x1F72
 #define IXGBE_SUBDEV_ID_82599_560FLR     0x17D0
 #define IXGBE_DEV_ID_82599_SFP_EM        0x1507
 #define IXGBE_DEV_ID_82599_SFP_SF2       0x154D
@@ -1452,6 +1450,7 @@ enum {
 #define IXGBE_ETQF_1588         0x40000000 /* bit 30 */
 #define IXGBE_ETQF_FILTER_EN    0x80000000 /* bit 31 */
 #define IXGBE_ETQF_POOL_ENABLE   (1 << 26) /* bit 26 */
+#define IXGBE_ETQF_POOL_SHIFT		20
 
 #define IXGBE_ETQS_RX_QUEUE     0x007F0000 /* bits 22:16 */
 #define IXGBE_ETQS_RX_QUEUE_SHIFT       16
@@ -2419,7 +2418,7 @@ typedef u32 ixgbe_physical_layer;
  */
 
 /* BitTimes (BT) conversion */
-#define IXGBE_BT2KB(BT) ((BT + 1023) / (8 * 1024))
+#define IXGBE_BT2KB(BT) ((BT + (8 * 1024 - 1)) / (8 * 1024))
 #define IXGBE_B2BT(BT) (BT * 8)
 
 /* Calculate Delay to respond to PFC */
@@ -2450,24 +2449,31 @@ typedef u32 ixgbe_physical_layer;
 #define IXGBE_PCI_DELAY	10000
 
 /* Calculate X540 delay value in bit times */
-#define IXGBE_FILL_RATE (36 / 25)
-
-#define IXGBE_DV_X540(LINK, TC) (IXGBE_FILL_RATE * \
-				 (IXGBE_B2BT(LINK) + IXGBE_PFC_D + \
-				 (2 * IXGBE_CABLE_DC) + \
-				 (2 * IXGBE_ID_X540) + \
-				 IXGBE_HD + IXGBE_B2BT(TC)))
+#define IXGBE_DV_X540(_max_frame_link, _max_frame_tc) \
+			((36 * \
+			  (IXGBE_B2BT(_max_frame_link) + \
+			   IXGBE_PFC_D + \
+			   (2 * IXGBE_CABLE_DC) + \
+			   (2 * IXGBE_ID_X540) + \
+			   IXGBE_HD) / 25 + 1) + \
+			 2 * IXGBE_B2BT(_max_frame_tc))
 
 /* Calculate 82599, 82598 delay value in bit times */
-#define IXGBE_DV(LINK, TC) (IXGBE_FILL_RATE * \
-			    (IXGBE_B2BT(LINK) + IXGBE_PFC_D + \
-			    (2 * IXGBE_CABLE_DC) + (2 * IXGBE_ID) + \
-			    IXGBE_HD + IXGBE_B2BT(TC)))
+#define IXGBE_DV(_max_frame_link, _max_frame_tc) \
+			((36 * \
+			  (IXGBE_B2BT(_max_frame_link) + \
+			   IXGBE_PFC_D + \
+			   (2 * IXGBE_CABLE_DC) + \
+			   (2 * IXGBE_ID) + \
+			   IXGBE_HD) / 25 + 1) + \
+			 2 * IXGBE_B2BT(_max_frame_tc))
 
 /* Calculate low threshold delay values */
-#define IXGBE_LOW_DV_X540(TC) (2 * IXGBE_B2BT(TC) + \
-			       (IXGBE_FILL_RATE * IXGBE_PCI_DELAY))
-#define IXGBE_LOW_DV(TC)      (2 * IXGBE_LOW_DV_X540(TC))
+#define IXGBE_LOW_DV_X540(_max_frame_tc) \
+			(2 * IXGBE_B2BT(_max_frame_tc) + \
+			(36 * IXGBE_PCI_DELAY / 25) + 1)
+#define IXGBE_LOW_DV(_max_frame_tc) \
+			(2 * IXGBE_LOW_DV_X540(_max_frame_tc))
 
 /* Software ATR hash keys */
 #define IXGBE_ATR_BUCKET_HASH_KEY    0x3DAD14E2
@@ -2597,6 +2603,8 @@ enum ixgbe_sfp_type {
 	ixgbe_sfp_type_da_act_lmt_core1 = 8,
 	ixgbe_sfp_type_1g_cu_core0 = 9,
 	ixgbe_sfp_type_1g_cu_core1 = 10,
+	ixgbe_sfp_type_1g_sx_core0 = 11,
+	ixgbe_sfp_type_1g_sx_core1 = 12,
 	ixgbe_sfp_type_not_present = 0xFFFE,
 	ixgbe_sfp_type_unknown = 0xFFFF
 };
@@ -2837,6 +2845,7 @@ struct ixgbe_mac_operations {
 	s32 (*set_rar)(struct ixgbe_hw *, u32, u8 *, u32, u32);
 	s32 (*clear_rar)(struct ixgbe_hw *, u32);
 	s32 (*set_vmdq)(struct ixgbe_hw *, u32, u32);
+	s32 (*set_vmdq_san_mac)(struct ixgbe_hw *, u32);
 	s32 (*clear_vmdq)(struct ixgbe_hw *, u32, u32);
 	s32 (*init_rx_addrs)(struct ixgbe_hw *);
 	s32 (*update_mc_addr_list)(struct ixgbe_hw *, struct net_device *);
@@ -2912,6 +2921,7 @@ struct ixgbe_mac_info {
 	bool                            orig_link_settings_stored;
 	bool                            autotry_restart;
 	u8                              flags;
+	u8				san_mac_rar_index;
 	struct ixgbe_thermal_sensor_data  thermal_sensor_data;
 };
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
index f90ec078ece..de4da5219b7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
@@ -156,6 +156,9 @@ mac_reset_top:
 		hw->mac.ops.set_rar(hw, hw->mac.num_rar_entries - 1,
 		                    hw->mac.san_addr, 0, IXGBE_RAH_AV);
 
+		/* Save the SAN MAC RAR index */
+		hw->mac.san_mac_rar_index = hw->mac.num_rar_entries - 1;
+
 		/* Reserve the last RAR for the SAN MAC address */
 		hw->mac.num_rar_entries--;
 	}
@@ -832,6 +835,7 @@ static struct ixgbe_mac_operations mac_ops_X540 = {
 	.set_rar                = &ixgbe_set_rar_generic,
 	.clear_rar              = &ixgbe_clear_rar_generic,
 	.set_vmdq               = &ixgbe_set_vmdq_generic,
+	.set_vmdq_san_mac	= &ixgbe_set_vmdq_san_mac_generic,
 	.clear_vmdq             = &ixgbe_clear_vmdq_generic,
 	.init_rx_addrs          = &ixgbe_init_rx_addrs_generic,
 	.update_mc_addr_list    = &ixgbe_update_mc_addr_list_generic,
diff --git a/drivers/net/ethernet/intel/ixgbevf/defines.h b/drivers/net/ethernet/intel/ixgbevf/defines.h
index e09a6cc633b..418af827b23 100644
--- a/drivers/net/ethernet/intel/ixgbevf/defines.h
+++ b/drivers/net/ethernet/intel/ixgbevf/defines.h
@@ -251,6 +251,7 @@ struct ixgbe_adv_tx_context_desc {
 #define IXGBE_ADVTXD_TUCMD_L4T_TCP   0x00000800  /* L4 Packet TYPE of TCP */
 #define IXGBE_ADVTXD_TUCMD_L4T_SCTP  0x00001000  /* L4 Packet TYPE of SCTP */
 #define IXGBE_ADVTXD_IDX_SHIFT  4 /* Adv desc Index shift */
+#define IXGBE_ADVTXD_CC		0x00000080 /* Check Context */
 #define IXGBE_ADVTXD_POPTS_SHIFT      8  /* Adv desc POPTS shift */
 #define IXGBE_ADVTXD_POPTS_IXSM (IXGBE_TXD_POPTS_IXSM << \
 				 IXGBE_ADVTXD_POPTS_SHIFT)
@@ -264,32 +265,9 @@ struct ixgbe_adv_tx_context_desc {
 
 /* Interrupt register bitmasks */
 
-/* Extended Interrupt Cause Read */
-#define IXGBE_EICR_RTX_QUEUE    0x0000FFFF /* RTx Queue Interrupt */
-#define IXGBE_EICR_MAILBOX      0x00080000 /* VF to PF Mailbox Interrupt */
-#define IXGBE_EICR_OTHER        0x80000000 /* Interrupt Cause Active */
-
-/* Extended Interrupt Cause Set */
-#define IXGBE_EICS_RTX_QUEUE    IXGBE_EICR_RTX_QUEUE /* RTx Queue Interrupt */
-#define IXGBE_EICS_MAILBOX      IXGBE_EICR_MAILBOX   /* VF to PF Mailbox Int */
-#define IXGBE_EICS_OTHER        IXGBE_EICR_OTHER     /* INT Cause Active */
-
-/* Extended Interrupt Mask Set */
-#define IXGBE_EIMS_RTX_QUEUE    IXGBE_EICR_RTX_QUEUE /* RTx Queue Interrupt */
-#define IXGBE_EIMS_MAILBOX      IXGBE_EICR_MAILBOX   /* VF to PF Mailbox Int */
-#define IXGBE_EIMS_OTHER        IXGBE_EICR_OTHER     /* INT Cause Active */
-
-/* Extended Interrupt Mask Clear */
-#define IXGBE_EIMC_RTX_QUEUE    IXGBE_EICR_RTX_QUEUE /* RTx Queue Interrupt */
-#define IXGBE_EIMC_MAILBOX      IXGBE_EICR_MAILBOX   /* VF to PF Mailbox Int */
-#define IXGBE_EIMC_OTHER        IXGBE_EICR_OTHER     /* INT Cause Active */
-
-#define IXGBE_EIMS_ENABLE_MASK ( \
-				IXGBE_EIMS_RTX_QUEUE       | \
-				IXGBE_EIMS_MAILBOX         | \
-				IXGBE_EIMS_OTHER)
-
 #define IXGBE_EITR_CNT_WDIS     0x80000000
+#define IXGBE_MAX_EITR		0x00000FF8
+#define IXGBE_MIN_EITR		8
 
 /* Error Codes */
 #define IXGBE_ERR_INVALID_MAC_ADDR              -1
diff --git a/drivers/net/ethernet/intel/ixgbevf/ethtool.c b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
index e8dddf572d3..8f2070439b5 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
@@ -43,7 +43,6 @@
 
 #define IXGBE_ALL_RAR_ENTRIES 16
 
-#ifdef ETHTOOL_GSTATS
 struct ixgbe_stats {
 	char stat_string[ETH_GSTRING_LEN];
 	int sizeof_stat;
@@ -75,21 +74,17 @@ static const struct ixgbe_stats ixgbe_gstrings_stats[] = {
 						zero_base)},
 	{"tx_csum_offload_ctxt", IXGBEVF_STAT(hw_csum_tx_good, zero_base,
 					      zero_base)},
-	{"rx_header_split", IXGBEVF_STAT(rx_hdr_split, zero_base, zero_base)},
 };
 
 #define IXGBE_QUEUE_STATS_LEN 0
 #define IXGBE_GLOBAL_STATS_LEN	ARRAY_SIZE(ixgbe_gstrings_stats)
 
 #define IXGBEVF_STATS_LEN (IXGBE_GLOBAL_STATS_LEN + IXGBE_QUEUE_STATS_LEN)
-#endif /* ETHTOOL_GSTATS */
-#ifdef ETHTOOL_TEST
 static const char ixgbe_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Register test  (offline)",
 	"Link test   (on/offline)"
 };
 #define IXGBE_TEST_LEN (sizeof(ixgbe_gstrings_test) / ETH_GSTRING_LEN)
-#endif /* ETHTOOL_TEST */
 
 static int ixgbevf_get_settings(struct net_device *netdev,
 				struct ethtool_cmd *ecmd)
@@ -289,13 +284,11 @@ static void ixgbevf_get_ringparam(struct net_device *netdev,
 				  struct ethtool_ringparam *ring)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
-	struct ixgbevf_ring *tx_ring = adapter->tx_ring;
-	struct ixgbevf_ring *rx_ring = adapter->rx_ring;
 
 	ring->rx_max_pending = IXGBEVF_MAX_RXD;
 	ring->tx_max_pending = IXGBEVF_MAX_TXD;
-	ring->rx_pending = rx_ring->count;
-	ring->tx_pending = tx_ring->count;
+	ring->rx_pending = adapter->rx_ring_count;
+	ring->tx_pending = adapter->tx_ring_count;
 }
 
 static int ixgbevf_set_ringparam(struct net_device *netdev,
@@ -303,33 +296,28 @@ static int ixgbevf_set_ringparam(struct net_device *netdev,
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbevf_ring *tx_ring = NULL, *rx_ring = NULL;
-	int i, err = 0;
 	u32 new_rx_count, new_tx_count;
+	int i, err = 0;
 
 	if ((ring->rx_mini_pending) || (ring->rx_jumbo_pending))
 		return -EINVAL;
 
-	new_rx_count = max(ring->rx_pending, (u32)IXGBEVF_MIN_RXD);
-	new_rx_count = min(new_rx_count, (u32)IXGBEVF_MAX_RXD);
-	new_rx_count = ALIGN(new_rx_count, IXGBE_REQ_RX_DESCRIPTOR_MULTIPLE);
-
-	new_tx_count = max(ring->tx_pending, (u32)IXGBEVF_MIN_TXD);
-	new_tx_count = min(new_tx_count, (u32)IXGBEVF_MAX_TXD);
+	new_tx_count = max_t(u32, ring->tx_pending, IXGBEVF_MIN_TXD);
+	new_tx_count = min_t(u32, new_tx_count, IXGBEVF_MAX_TXD);
 	new_tx_count = ALIGN(new_tx_count, IXGBE_REQ_TX_DESCRIPTOR_MULTIPLE);
 
-	if ((new_tx_count == adapter->tx_ring->count) &&
-	    (new_rx_count == adapter->rx_ring->count)) {
-		/* nothing to do */
+	new_rx_count = max_t(u32, ring->rx_pending, IXGBEVF_MIN_RXD);
+	new_rx_count = min_t(u32, new_rx_count, IXGBEVF_MAX_RXD);
+	new_rx_count = ALIGN(new_rx_count, IXGBE_REQ_RX_DESCRIPTOR_MULTIPLE);
+
+	/* if nothing to do return success */
+	if ((new_tx_count == adapter->tx_ring_count) &&
+	    (new_rx_count == adapter->rx_ring_count))
 		return 0;
-	}
 
 	while (test_and_set_bit(__IXGBEVF_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 
-	/*
-	 * If the adapter isn't up and running then just set the
-	 * new parameters and scurry for the exits.
-	 */
 	if (!netif_running(adapter->netdev)) {
 		for (i = 0; i < adapter->num_tx_queues; i++)
 			adapter->tx_ring[i].count = new_tx_count;
@@ -340,82 +328,98 @@ static int ixgbevf_set_ringparam(struct net_device *netdev,
 		goto clear_reset;
 	}
 
-	tx_ring = kcalloc(adapter->num_tx_queues,
-			  sizeof(struct ixgbevf_ring), GFP_KERNEL);
-	if (!tx_ring) {
-		err = -ENOMEM;
-		goto clear_reset;
-	}
-
-	rx_ring = kcalloc(adapter->num_rx_queues,
-			  sizeof(struct ixgbevf_ring), GFP_KERNEL);
-	if (!rx_ring) {
-		err = -ENOMEM;
-		goto err_rx_setup;
-	}
-
-	ixgbevf_down(adapter);
+	if (new_tx_count != adapter->tx_ring_count) {
+		tx_ring = vmalloc(adapter->num_tx_queues * sizeof(*tx_ring));
+		if (!tx_ring) {
+			err = -ENOMEM;
+			goto clear_reset;
+		}
 
-	memcpy(tx_ring, adapter->tx_ring,
-	       adapter->num_tx_queues * sizeof(struct ixgbevf_ring));
-	for (i = 0; i < adapter->num_tx_queues; i++) {
-		tx_ring[i].count = new_tx_count;
-		err = ixgbevf_setup_tx_resources(adapter, &tx_ring[i]);
-		if (err) {
+		for (i = 0; i < adapter->num_tx_queues; i++) {
+			/* clone ring and setup updated count */
+			tx_ring[i] = adapter->tx_ring[i];
+			tx_ring[i].count = new_tx_count;
+			err = ixgbevf_setup_tx_resources(adapter, &tx_ring[i]);
+			if (!err)
+				continue;
 			while (i) {
 				i--;
-				ixgbevf_free_tx_resources(adapter,
-							  &tx_ring[i]);
+				ixgbevf_free_tx_resources(adapter, &tx_ring[i]);
 			}
-			goto err_tx_ring_setup;
+
+			vfree(tx_ring);
+			tx_ring = NULL;
+
+			goto clear_reset;
 		}
-		tx_ring[i].v_idx = adapter->tx_ring[i].v_idx;
 	}
 
-	memcpy(rx_ring, adapter->rx_ring,
-	       adapter->num_rx_queues * sizeof(struct ixgbevf_ring));
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		rx_ring[i].count = new_rx_count;
-		err = ixgbevf_setup_rx_resources(adapter, &rx_ring[i]);
-		if (err) {
+	if (new_rx_count != adapter->rx_ring_count) {
+		rx_ring = vmalloc(adapter->num_rx_queues * sizeof(*rx_ring));
+		if (!rx_ring) {
+			err = -ENOMEM;
+			goto clear_reset;
+		}
+
+		for (i = 0; i < adapter->num_rx_queues; i++) {
+			/* clone ring and setup updated count */
+			rx_ring[i] = adapter->rx_ring[i];
+			rx_ring[i].count = new_rx_count;
+			err = ixgbevf_setup_rx_resources(adapter, &rx_ring[i]);
+			if (!err)
+				continue;
 			while (i) {
 				i--;
-				ixgbevf_free_rx_resources(adapter,
-							  &rx_ring[i]);
+				ixgbevf_free_rx_resources(adapter, &rx_ring[i]);
 			}
-				goto err_rx_ring_setup;
+
+			vfree(rx_ring);
+			rx_ring = NULL;
+
+			goto clear_reset;
 		}
-		rx_ring[i].v_idx = adapter->rx_ring[i].v_idx;
 	}
 
-	/*
-	 * Only switch to new rings if all the prior allocations
-	 * and ring setups have succeeded.
-	 */
-	kfree(adapter->tx_ring);
-	adapter->tx_ring = tx_ring;
-	adapter->tx_ring_count = new_tx_count;
-
-	kfree(adapter->rx_ring);
-	adapter->rx_ring = rx_ring;
-	adapter->rx_ring_count = new_rx_count;
+	/* bring interface down to prepare for update */
+	ixgbevf_down(adapter);
 
-	/* success! */
-	ixgbevf_up(adapter);
+	/* Tx */
+	if (tx_ring) {
+		for (i = 0; i < adapter->num_tx_queues; i++) {
+			ixgbevf_free_tx_resources(adapter,
+						  &adapter->tx_ring[i]);
+			adapter->tx_ring[i] = tx_ring[i];
+		}
+		adapter->tx_ring_count = new_tx_count;
 
-	goto clear_reset;
+		vfree(tx_ring);
+		tx_ring = NULL;
+	}
 
-err_rx_ring_setup:
-	for(i = 0; i < adapter->num_tx_queues; i++)
-		ixgbevf_free_tx_resources(adapter, &tx_ring[i]);
+	/* Rx */
+	if (rx_ring) {
+		for (i = 0; i < adapter->num_rx_queues; i++) {
+			ixgbevf_free_rx_resources(adapter,
+						  &adapter->rx_ring[i]);
+			adapter->rx_ring[i] = rx_ring[i];
+		}
+		adapter->rx_ring_count = new_rx_count;
 
-err_tx_ring_setup:
-	kfree(rx_ring);
+		vfree(rx_ring);
+		rx_ring = NULL;
+	}
 
-err_rx_setup:
-	kfree(tx_ring);
+	/* restore interface using new values */
+	ixgbevf_up(adapter);
 
 clear_reset:
+	/* free Tx resources if Rx error is encountered */
+	if (tx_ring) {
+		for (i = 0; i < adapter->num_tx_queues; i++)
+			ixgbevf_free_tx_resources(adapter, &tx_ring[i]);
+		vfree(tx_ring);
+	}
+
 	clear_bit(__IXGBEVF_RESETTING, &adapter->state);
 	return err;
 }
@@ -674,10 +678,8 @@ static int ixgbevf_nway_reset(struct net_device *netdev)
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 
-	if (netif_running(netdev)) {
-		if (!adapter->dev_closed)
-			ixgbevf_reinit_locked(adapter);
-	}
+	if (netif_running(netdev))
+		ixgbevf_reinit_locked(adapter);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index 0a1b99240d4..98cadb0c4da 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -52,12 +52,12 @@ struct ixgbevf_tx_buffer {
 struct ixgbevf_rx_buffer {
 	struct sk_buff *skb;
 	dma_addr_t dma;
-	struct page *page;
-	dma_addr_t page_dma;
-	unsigned int page_offset;
 };
 
 struct ixgbevf_ring {
+	struct ixgbevf_ring *next;
+	struct net_device *netdev;
+	struct device *dev;
 	struct ixgbevf_adapter *adapter;  /* backlink */
 	void *desc;			/* descriptor ring memory */
 	dma_addr_t dma;			/* phys. address of descriptor ring */
@@ -83,29 +83,9 @@ struct ixgbevf_ring {
 		      * offset associated with this ring, which is different
 		      * for DCB and RSS modes */
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-	/* cpu for tx queue */
-	int cpu;
-#endif
-
-	u64 v_idx; /* maps directly to the index for this ring in the hardware
-		    * vector array, can also be used for finding the bit in EICR
-		    * and friends that represents the vector for this ring */
-
-	u16 work_limit;                /* max work per interrupt */
 	u16 rx_buf_len;
 };
 
-enum ixgbevf_ring_f_enum {
-	RING_F_NONE = 0,
-	RING_F_ARRAY_SIZE      /* must be last in enum set */
-};
-
-struct ixgbevf_ring_feature {
-	int indices;
-	int mask;
-};
-
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
 #define IXGBEVF_RX_BUFFER_WRITE	16	/* Must be power of 2 */
 
@@ -120,8 +100,6 @@ struct ixgbevf_ring_feature {
 #define IXGBEVF_MIN_RXD       64
 
 /* Supported Rx Buffer Sizes */
-#define IXGBEVF_RXBUFFER_64    64     /* Used for packet split */
-#define IXGBEVF_RXBUFFER_128   128    /* Used for packet split */
 #define IXGBEVF_RXBUFFER_256   256    /* Used for packet split */
 #define IXGBEVF_RXBUFFER_2048  2048
 #define IXGBEVF_MAX_RXBUFFER   16384  /* largest size for single descriptor */
@@ -140,22 +118,42 @@ struct ixgbevf_ring_feature {
 #define IXGBE_TX_FLAGS_VLAN_PRIO_MASK	0x0000e000
 #define IXGBE_TX_FLAGS_VLAN_SHIFT	16
 
+struct ixgbevf_ring_container {
+	struct ixgbevf_ring *ring;	/* pointer to linked list of rings */
+	unsigned int total_bytes;	/* total bytes processed this int */
+	unsigned int total_packets;	/* total packets processed this int */
+	u8 count;			/* total number of rings in vector */
+	u8 itr;				/* current ITR setting for ring */
+};
+
+/* iterator for handling rings in ring container */
+#define ixgbevf_for_each_ring(pos, head) \
+	for (pos = (head).ring; pos != NULL; pos = pos->next)
+
 /* MAX_MSIX_Q_VECTORS of these are allocated,
  * but we only use one per queue-specific vector.
  */
 struct ixgbevf_q_vector {
 	struct ixgbevf_adapter *adapter;
+	u16 v_idx;		/* index of q_vector within array, also used for
+				 * finding the bit in EICR and friends that
+				 * represents the vector for this ring */
+	u16 itr;		/* Interrupt throttle rate written to EITR */
 	struct napi_struct napi;
-	DECLARE_BITMAP(rxr_idx, MAX_RX_QUEUES); /* Rx ring indices */
-	DECLARE_BITMAP(txr_idx, MAX_TX_QUEUES); /* Tx ring indices */
-	u8 rxr_count;     /* Rx ring count assigned to this vector */
-	u8 txr_count;     /* Tx ring count assigned to this vector */
-	u8 tx_itr;
-	u8 rx_itr;
-	u32 eitr;
-	int v_idx;	  /* vector index in list */
+	struct ixgbevf_ring_container rx, tx;
+	char name[IFNAMSIZ + 9];
 };
 
+/*
+ * microsecond values for various ITR rates shifted by 2 to fit itr register
+ * with the first 3 bits reserved 0
+ */
+#define IXGBE_MIN_RSC_ITR	24
+#define IXGBE_100K_ITR		40
+#define IXGBE_20K_ITR		200
+#define IXGBE_10K_ITR		400
+#define IXGBE_8K_ITR		500
+
 /* Helper macros to switch between ints/sec and what the register uses.
  * And yes, it's the same math going both ways.  The lowest value
  * supported by all of the ixgbe hardware is 8.
@@ -168,12 +166,12 @@ struct ixgbevf_q_vector {
 	((((R)->next_to_clean > (R)->next_to_use) ? 0 : (R)->count) + \
 	(R)->next_to_clean - (R)->next_to_use - 1)
 
-#define IXGBE_RX_DESC_ADV(R, i)	    \
-	(&(((union ixgbe_adv_rx_desc *)((R).desc))[i]))
-#define IXGBE_TX_DESC_ADV(R, i)	    \
-	(&(((union ixgbe_adv_tx_desc *)((R).desc))[i]))
-#define IXGBE_TX_CTXTDESC_ADV(R, i)	    \
-	(&(((struct ixgbe_adv_tx_context_desc *)((R).desc))[i]))
+#define IXGBEVF_RX_DESC(R, i)	    \
+	(&(((union ixgbe_adv_rx_desc *)((R)->desc))[i]))
+#define IXGBEVF_TX_DESC(R, i)	    \
+	(&(((union ixgbe_adv_tx_desc *)((R)->desc))[i]))
+#define IXGBEVF_TX_CTXTDESC(R, i)	    \
+	(&(((struct ixgbe_adv_tx_context_desc *)((R)->desc))[i]))
 
 #define IXGBE_MAX_JUMBO_FRAME_SIZE        16128
 
@@ -181,9 +179,8 @@ struct ixgbevf_q_vector {
 #define NON_Q_VECTORS (OTHER_VECTOR)
 
 #define MAX_MSIX_Q_VECTORS 2
-#define MAX_MSIX_COUNT 2
 
-#define MIN_MSIX_Q_VECTORS 2
+#define MIN_MSIX_Q_VECTORS 1
 #define MIN_MSIX_COUNT (MIN_MSIX_Q_VECTORS + NON_Q_VECTORS)
 
 /* board specific private data structure */
@@ -193,12 +190,14 @@ struct ixgbevf_adapter {
 	u16 bd_number;
 	struct work_struct reset_task;
 	struct ixgbevf_q_vector *q_vector[MAX_MSIX_Q_VECTORS];
-	char name[MAX_MSIX_COUNT][IFNAMSIZ + 9];
 
 	/* Interrupt Throttle Rate */
-	u32 itr_setting;
-	u16 eitr_low;
-	u16 eitr_high;
+	u16 rx_itr_setting;
+	u16 tx_itr_setting;
+
+	/* interrupt masks */
+	u32 eims_enable_mask;
+	u32 eims_other;
 
 	/* TX */
 	struct ixgbevf_ring *tx_ring;	/* One per active queue */
@@ -213,18 +212,13 @@ struct ixgbevf_adapter {
 	/* RX */
 	struct ixgbevf_ring *rx_ring;	/* One per active queue */
 	int num_rx_queues;
-	int num_rx_pools;               /* == num_rx_queues in 82598 */
-	int num_rx_queues_per_pool;	/* 1 if 82598, can be many if 82599 */
 	u64 hw_csum_rx_error;
 	u64 hw_rx_no_dma_resources;
 	u64 hw_csum_rx_good;
 	u64 non_eop_descs;
 	int num_msix_vectors;
-	int max_msix_q_vectors;         /* true count of q_vectors for device */
-	struct ixgbevf_ring_feature ring_feature[RING_F_ARRAY_SIZE];
 	struct msix_entry *msix_entries;
 
-	u64 rx_hdr_split;
 	u32 alloc_rx_page_failed;
 	u32 alloc_rx_buff_failed;
 
@@ -232,15 +226,8 @@ struct ixgbevf_adapter {
 	 * thus the additional *_CAPABLE flags.
 	 */
 	u32 flags;
-#define IXGBE_FLAG_RX_CSUM_ENABLED              (u32)(1)
-#define IXGBE_FLAG_RX_1BUF_CAPABLE              (u32)(1 << 1)
-#define IXGBE_FLAG_RX_PS_CAPABLE                (u32)(1 << 2)
-#define IXGBE_FLAG_RX_PS_ENABLED                (u32)(1 << 3)
-#define IXGBE_FLAG_IN_NETPOLL                   (u32)(1 << 4)
-#define IXGBE_FLAG_IMIR_ENABLED                 (u32)(1 << 5)
-#define IXGBE_FLAG_MQ_CAPABLE                   (u32)(1 << 6)
-#define IXGBE_FLAG_NEED_LINK_UPDATE             (u32)(1 << 7)
-#define IXGBE_FLAG_IN_WATCHDOG_TASK             (u32)(1 << 8)
+#define IXGBE_FLAG_IN_WATCHDOG_TASK             (u32)(1)
+
 	/* OS defined structs */
 	struct net_device *netdev;
 	struct pci_dev *pdev;
@@ -254,18 +241,16 @@ struct ixgbevf_adapter {
 	u32 eitr_param;
 
 	unsigned long state;
-	u32 *config_space;
 	u64 tx_busy;
 	unsigned int tx_ring_count;
 	unsigned int rx_ring_count;
 
 	u32 link_speed;
 	bool link_up;
-	unsigned long link_check_timeout;
 
 	struct work_struct watchdog_task;
-	bool netdev_registered;
-	bool dev_closed;
+
+	spinlock_t mbx_lock;
 };
 
 enum ixbgevf_state_t {
@@ -301,11 +286,8 @@ extern void ixgbevf_free_rx_resources(struct ixgbevf_adapter *,
 extern void ixgbevf_free_tx_resources(struct ixgbevf_adapter *,
 				      struct ixgbevf_ring *);
 extern void ixgbevf_update_stats(struct ixgbevf_adapter *adapter);
-
-#ifdef ETHTOOL_OPS_COMPAT
 extern int ethtool_ioctl(struct ifreq *ifr);
 
-#endif
 extern void ixgbe_napi_add_all(struct ixgbevf_adapter *adapter);
 extern void ixgbe_napi_del_all(struct ixgbevf_adapter *adapter);
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index f69ec4288b1..60ef6458741 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -42,6 +42,7 @@
 #include <linux/in.h>
 #include <linux/ip.h>
 #include <linux/tcp.h>
+#include <linux/sctp.h>
 #include <linux/ipv6.h>
 #include <linux/slab.h>
 #include <net/checksum.h>
@@ -97,9 +98,7 @@ module_param(debug, int, 0);
 MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
 
 /* forward decls */
-static void ixgbevf_set_itr_msix(struct ixgbevf_q_vector *q_vector);
-static void ixgbevf_write_eitr(struct ixgbevf_adapter *adapter, int v_idx,
-			       u32 itr_reg);
+static void ixgbevf_set_itr(struct ixgbevf_q_vector *q_vector);
 
 static inline void ixgbevf_release_rx_desc(struct ixgbe_hw *hw,
 					   struct ixgbevf_ring *rx_ring,
@@ -115,7 +114,7 @@ static inline void ixgbevf_release_rx_desc(struct ixgbe_hw *hw,
 	IXGBE_WRITE_REG(hw, IXGBE_VFRDT(rx_ring->reg_idx), val);
 }
 
-/*
+/**
  * 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
@@ -146,18 +145,18 @@ static void ixgbevf_set_ivar(struct ixgbevf_adapter *adapter, s8 direction,
 	}
 }
 
-static void ixgbevf_unmap_and_free_tx_resource(struct ixgbevf_adapter *adapter,
+static void ixgbevf_unmap_and_free_tx_resource(struct ixgbevf_ring *tx_ring,
 					       struct ixgbevf_tx_buffer
 					       *tx_buffer_info)
 {
 	if (tx_buffer_info->dma) {
 		if (tx_buffer_info->mapped_as_page)
-			dma_unmap_page(&adapter->pdev->dev,
+			dma_unmap_page(tx_ring->dev,
 				       tx_buffer_info->dma,
 				       tx_buffer_info->length,
 				       DMA_TO_DEVICE);
 		else
-			dma_unmap_single(&adapter->pdev->dev,
+			dma_unmap_single(tx_ring->dev,
 					 tx_buffer_info->dma,
 					 tx_buffer_info->length,
 					 DMA_TO_DEVICE);
@@ -175,38 +174,34 @@ static void ixgbevf_unmap_and_free_tx_resource(struct ixgbevf_adapter *adapter,
 #define IXGBE_MAX_DATA_PER_TXD	(1 << IXGBE_MAX_TXD_PWR)
 
 /* Tx Descriptors needed, worst case */
-#define TXD_USE_COUNT(S) (((S) >> IXGBE_MAX_TXD_PWR) + \
-			 (((S) & (IXGBE_MAX_DATA_PER_TXD - 1)) ? 1 : 0))
-#ifdef MAX_SKB_FRAGS
-#define DESC_NEEDED (TXD_USE_COUNT(IXGBE_MAX_DATA_PER_TXD) /* skb->data */ + \
-	MAX_SKB_FRAGS * TXD_USE_COUNT(PAGE_SIZE) + 1)      /* for context */
-#else
-#define DESC_NEEDED TXD_USE_COUNT(IXGBE_MAX_DATA_PER_TXD)
-#endif
+#define TXD_USE_COUNT(S) DIV_ROUND_UP((S), IXGBE_MAX_DATA_PER_TXD)
+#define DESC_NEEDED (MAX_SKB_FRAGS + 4)
 
 static void ixgbevf_tx_timeout(struct net_device *netdev);
 
 /**
  * ixgbevf_clean_tx_irq - Reclaim resources after transmit completes
- * @adapter: board private structure
+ * @q_vector: board private structure
  * @tx_ring: tx ring to clean
  **/
-static bool ixgbevf_clean_tx_irq(struct ixgbevf_adapter *adapter,
+static bool ixgbevf_clean_tx_irq(struct ixgbevf_q_vector *q_vector,
 				 struct ixgbevf_ring *tx_ring)
 {
-	struct net_device *netdev = adapter->netdev;
-	struct ixgbe_hw *hw = &adapter->hw;
+	struct ixgbevf_adapter *adapter = q_vector->adapter;
 	union ixgbe_adv_tx_desc *tx_desc, *eop_desc;
 	struct ixgbevf_tx_buffer *tx_buffer_info;
 	unsigned int i, eop, count = 0;
 	unsigned int total_bytes = 0, total_packets = 0;
 
+	if (test_bit(__IXGBEVF_DOWN, &adapter->state))
+		return true;
+
 	i = tx_ring->next_to_clean;
 	eop = tx_ring->tx_buffer_info[i].next_to_watch;
-	eop_desc = IXGBE_TX_DESC_ADV(*tx_ring, eop);
+	eop_desc = IXGBEVF_TX_DESC(tx_ring, eop);
 
 	while ((eop_desc->wb.status & cpu_to_le32(IXGBE_TXD_STAT_DD)) &&
-	       (count < tx_ring->work_limit)) {
+	       (count < tx_ring->count)) {
 		bool cleaned = false;
 		rmb(); /* read buffer_info after eop_desc */
 		/* eop could change between read and DD-check */
@@ -214,7 +209,7 @@ static bool ixgbevf_clean_tx_irq(struct ixgbevf_adapter *adapter,
 			goto cont_loop;
 		for ( ; !cleaned; count++) {
 			struct sk_buff *skb;
-			tx_desc = IXGBE_TX_DESC_ADV(*tx_ring, i);
+			tx_desc = IXGBEVF_TX_DESC(tx_ring, i);
 			tx_buffer_info = &tx_ring->tx_buffer_info[i];
 			cleaned = (i == eop);
 			skb = tx_buffer_info->skb;
@@ -231,7 +226,7 @@ static bool ixgbevf_clean_tx_irq(struct ixgbevf_adapter *adapter,
 				total_bytes += bytecount;
 			}
 
-			ixgbevf_unmap_and_free_tx_resource(adapter,
+			ixgbevf_unmap_and_free_tx_resource(tx_ring,
 							   tx_buffer_info);
 
 			tx_desc->wb.status = 0;
@@ -243,37 +238,25 @@ static bool ixgbevf_clean_tx_irq(struct ixgbevf_adapter *adapter,
 
 cont_loop:
 		eop = tx_ring->tx_buffer_info[i].next_to_watch;
-		eop_desc = IXGBE_TX_DESC_ADV(*tx_ring, eop);
+		eop_desc = IXGBEVF_TX_DESC(tx_ring, eop);
 	}
 
 	tx_ring->next_to_clean = i;
 
 #define TX_WAKE_THRESHOLD (DESC_NEEDED * 2)
-	if (unlikely(count && netif_carrier_ok(netdev) &&
+	if (unlikely(count && netif_carrier_ok(tx_ring->netdev) &&
 		     (IXGBE_DESC_UNUSED(tx_ring) >= TX_WAKE_THRESHOLD))) {
 		/* Make sure that anybody stopping the queue after this
 		 * sees the new next_to_clean.
 		 */
 		smp_mb();
-#ifdef HAVE_TX_MQ
-		if (__netif_subqueue_stopped(netdev, tx_ring->queue_index) &&
-		    !test_bit(__IXGBEVF_DOWN, &adapter->state)) {
-			netif_wake_subqueue(netdev, tx_ring->queue_index);
-			++adapter->restart_queue;
-		}
-#else
-		if (netif_queue_stopped(netdev) &&
+		if (__netif_subqueue_stopped(tx_ring->netdev,
+					     tx_ring->queue_index) &&
 		    !test_bit(__IXGBEVF_DOWN, &adapter->state)) {
-			netif_wake_queue(netdev);
+			netif_wake_subqueue(tx_ring->netdev,
+					    tx_ring->queue_index);
 			++adapter->restart_queue;
 		}
-#endif
-	}
-
-	/* re-arm the interrupt */
-	if ((count >= tx_ring->work_limit) &&
-	    (!test_bit(__IXGBEVF_DOWN, &adapter->state))) {
-		IXGBE_WRITE_REG(hw, IXGBE_VTEICS, tx_ring->v_idx);
 	}
 
 	u64_stats_update_begin(&tx_ring->syncp);
@@ -281,7 +264,7 @@ cont_loop:
 	tx_ring->total_packets += total_packets;
 	u64_stats_update_end(&tx_ring->syncp);
 
-	return count < tx_ring->work_limit;
+	return count < tx_ring->count;
 }
 
 /**
@@ -301,13 +284,10 @@ static void ixgbevf_receive_skb(struct ixgbevf_q_vector *q_vector,
 	bool is_vlan = (status & IXGBE_RXD_STAT_VP);
 	u16 tag = le16_to_cpu(rx_desc->wb.upper.vlan);
 
-	if (is_vlan && test_bit(tag, adapter->active_vlans))
+	if (is_vlan && test_bit(tag & VLAN_VID_MASK, adapter->active_vlans))
 		__vlan_hwaccel_put_tag(skb, tag);
 
-	if (!(adapter->flags & IXGBE_FLAG_IN_NETPOLL))
-			napi_gro_receive(&q_vector->napi, skb);
-	else
-			netif_rx(skb);
+	napi_gro_receive(&q_vector->napi, skb);
 }
 
 /**
@@ -317,12 +297,13 @@ static void ixgbevf_receive_skb(struct ixgbevf_q_vector *q_vector,
  * @skb: skb currently being received and modified
  **/
 static inline void ixgbevf_rx_checksum(struct ixgbevf_adapter *adapter,
+				       struct ixgbevf_ring *ring,
 				       u32 status_err, struct sk_buff *skb)
 {
 	skb_checksum_none_assert(skb);
 
 	/* Rx csum disabled */
-	if (!(adapter->flags & IXGBE_FLAG_RX_CSUM_ENABLED))
+	if (!(ring->netdev->features & NETIF_F_RXCSUM))
 		return;
 
 	/* if IP and error */
@@ -357,52 +338,20 @@ static void ixgbevf_alloc_rx_buffers(struct ixgbevf_adapter *adapter,
 	union ixgbe_adv_rx_desc *rx_desc;
 	struct ixgbevf_rx_buffer *bi;
 	struct sk_buff *skb;
-	unsigned int i;
-	unsigned int bufsz = rx_ring->rx_buf_len + NET_IP_ALIGN;
+	unsigned int i = rx_ring->next_to_use;
 
-	i = rx_ring->next_to_use;
 	bi = &rx_ring->rx_buffer_info[i];
 
 	while (cleaned_count--) {
-		rx_desc = IXGBE_RX_DESC_ADV(*rx_ring, i);
-
-		if (!bi->page_dma &&
-		    (adapter->flags & IXGBE_FLAG_RX_PS_ENABLED)) {
-			if (!bi->page) {
-				bi->page = alloc_page(GFP_ATOMIC | __GFP_COLD);
-				if (!bi->page) {
-					adapter->alloc_rx_page_failed++;
-					goto no_buffers;
-				}
-				bi->page_offset = 0;
-			} else {
-				/* use a half page if we're re-using */
-				bi->page_offset ^= (PAGE_SIZE / 2);
-			}
-
-			bi->page_dma = dma_map_page(&pdev->dev, bi->page,
-						    bi->page_offset,
-						    (PAGE_SIZE / 2),
-						    DMA_FROM_DEVICE);
-		}
-
+		rx_desc = IXGBEVF_RX_DESC(rx_ring, i);
 		skb = bi->skb;
 		if (!skb) {
-			skb = netdev_alloc_skb(adapter->netdev,
-							       bufsz);
-
+			skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+							rx_ring->rx_buf_len);
 			if (!skb) {
 				adapter->alloc_rx_buff_failed++;
 				goto no_buffers;
 			}
-
-			/*
-			 * Make buffer alignment 2 beyond a 16 byte boundary
-			 * this will result in a 16 byte aligned IP header after
-			 * the 14 byte MAC header is removed
-			 */
-			skb_reserve(skb, NET_IP_ALIGN);
-
 			bi->skb = skb;
 		}
 		if (!bi->dma) {
@@ -410,14 +359,7 @@ static void ixgbevf_alloc_rx_buffers(struct ixgbevf_adapter *adapter,
 						 rx_ring->rx_buf_len,
 						 DMA_FROM_DEVICE);
 		}
-		/* Refresh the desc even if buffer_addrs didn't change because
-		 * each write-back erases this info. */
-		if (adapter->flags & IXGBE_FLAG_RX_PS_ENABLED) {
-			rx_desc->read.pkt_addr = cpu_to_le64(bi->page_dma);
-			rx_desc->read.hdr_addr = cpu_to_le64(bi->dma);
-		} else {
-			rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
-		}
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
 
 		i++;
 		if (i == rx_ring->count)
@@ -428,36 +370,22 @@ static void ixgbevf_alloc_rx_buffers(struct ixgbevf_adapter *adapter,
 no_buffers:
 	if (rx_ring->next_to_use != i) {
 		rx_ring->next_to_use = i;
-		if (i-- == 0)
-			i = (rx_ring->count - 1);
 
 		ixgbevf_release_rx_desc(&adapter->hw, rx_ring, i);
 	}
 }
 
 static inline void ixgbevf_irq_enable_queues(struct ixgbevf_adapter *adapter,
-					     u64 qmask)
+					     u32 qmask)
 {
-	u32 mask;
 	struct ixgbe_hw *hw = &adapter->hw;
 
-	mask = (qmask & 0xFFFFFFFF);
-	IXGBE_WRITE_REG(hw, IXGBE_VTEIMS, mask);
-}
-
-static inline u16 ixgbevf_get_hdr_info(union ixgbe_adv_rx_desc *rx_desc)
-{
-	return rx_desc->wb.lower.lo_dword.hs_rss.hdr_info;
-}
-
-static inline u16 ixgbevf_get_pkt_info(union ixgbe_adv_rx_desc *rx_desc)
-{
-	return rx_desc->wb.lower.lo_dword.hs_rss.pkt_info;
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIMS, qmask);
 }
 
 static bool ixgbevf_clean_rx_irq(struct ixgbevf_q_vector *q_vector,
 				 struct ixgbevf_ring *rx_ring,
-				 int *work_done, int work_to_do)
+				 int budget)
 {
 	struct ixgbevf_adapter *adapter = q_vector->adapter;
 	struct pci_dev *pdev = adapter->pdev;
@@ -466,36 +394,21 @@ static bool ixgbevf_clean_rx_irq(struct ixgbevf_q_vector *q_vector,
 	struct sk_buff *skb;
 	unsigned int i;
 	u32 len, staterr;
-	u16 hdr_info;
-	bool cleaned = false;
 	int cleaned_count = 0;
 	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
 
 	i = rx_ring->next_to_clean;
-	rx_desc = IXGBE_RX_DESC_ADV(*rx_ring, i);
+	rx_desc = IXGBEVF_RX_DESC(rx_ring, i);
 	staterr = le32_to_cpu(rx_desc->wb.upper.status_error);
 	rx_buffer_info = &rx_ring->rx_buffer_info[i];
 
 	while (staterr & IXGBE_RXD_STAT_DD) {
-		u32 upper_len = 0;
-		if (*work_done >= work_to_do)
+		if (!budget)
 			break;
-		(*work_done)++;
+		budget--;
 
 		rmb(); /* read descriptor and rx_buffer_info after status DD */
-		if (adapter->flags & IXGBE_FLAG_RX_PS_ENABLED) {
-			hdr_info = le16_to_cpu(ixgbevf_get_hdr_info(rx_desc));
-			len = (hdr_info & IXGBE_RXDADV_HDRBUFLEN_MASK) >>
-			       IXGBE_RXDADV_HDRBUFLEN_SHIFT;
-			if (hdr_info & IXGBE_RXDADV_SPH)
-				adapter->rx_hdr_split++;
-			if (len > IXGBEVF_RX_HDR_SIZE)
-				len = IXGBEVF_RX_HDR_SIZE;
-			upper_len = le16_to_cpu(rx_desc->wb.upper.length);
-		} else {
-			len = le16_to_cpu(rx_desc->wb.upper.length);
-		}
-		cleaned = true;
+		len = le16_to_cpu(rx_desc->wb.upper.length);
 		skb = rx_buffer_info->skb;
 		prefetch(skb->data - NET_IP_ALIGN);
 		rx_buffer_info->skb = NULL;
@@ -508,46 +421,19 @@ static bool ixgbevf_clean_rx_irq(struct ixgbevf_q_vector *q_vector,
 			skb_put(skb, len);
 		}
 
-		if (upper_len) {
-			dma_unmap_page(&pdev->dev, rx_buffer_info->page_dma,
-				       PAGE_SIZE / 2, DMA_FROM_DEVICE);
-			rx_buffer_info->page_dma = 0;
-			skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
-					   rx_buffer_info->page,
-					   rx_buffer_info->page_offset,
-					   upper_len);
-
-			if ((rx_ring->rx_buf_len > (PAGE_SIZE / 2)) ||
-			    (page_count(rx_buffer_info->page) != 1))
-				rx_buffer_info->page = NULL;
-			else
-				get_page(rx_buffer_info->page);
-
-			skb->len += upper_len;
-			skb->data_len += upper_len;
-			skb->truesize += upper_len;
-		}
-
 		i++;
 		if (i == rx_ring->count)
 			i = 0;
 
-		next_rxd = IXGBE_RX_DESC_ADV(*rx_ring, i);
+		next_rxd = IXGBEVF_RX_DESC(rx_ring, i);
 		prefetch(next_rxd);
 		cleaned_count++;
 
 		next_buffer = &rx_ring->rx_buffer_info[i];
 
 		if (!(staterr & IXGBE_RXD_STAT_EOP)) {
-			if (adapter->flags & IXGBE_FLAG_RX_PS_ENABLED) {
-				rx_buffer_info->skb = next_buffer->skb;
-				rx_buffer_info->dma = next_buffer->dma;
-				next_buffer->skb = skb;
-				next_buffer->dma = 0;
-			} else {
-				skb->next = next_buffer->skb;
-				skb->next->prev = skb;
-			}
+			skb->next = next_buffer->skb;
+			skb->next->prev = skb;
 			adapter->non_eop_descs++;
 			goto next_desc;
 		}
@@ -558,7 +444,7 @@ static bool ixgbevf_clean_rx_irq(struct ixgbevf_q_vector *q_vector,
 			goto next_desc;
 		}
 
-		ixgbevf_rx_checksum(adapter, staterr, skb);
+		ixgbevf_rx_checksum(adapter, rx_ring, staterr, skb);
 
 		/* probably a little skewed due to removing CRC */
 		total_rx_bytes += skb->len;
@@ -573,7 +459,7 @@ static bool ixgbevf_clean_rx_irq(struct ixgbevf_q_vector *q_vector,
 			if (header_fixup_len < 14)
 				skb_push(skb, header_fixup_len);
 		}
-		skb->protocol = eth_type_trans(skb, adapter->netdev);
+		skb->protocol = eth_type_trans(skb, rx_ring->netdev);
 
 		ixgbevf_receive_skb(q_vector, skb, staterr, rx_ring, rx_desc);
 
@@ -605,95 +491,74 @@ next_desc:
 	rx_ring->total_bytes += total_rx_bytes;
 	u64_stats_update_end(&rx_ring->syncp);
 
-	return cleaned;
+	return !!budget;
 }
 
 /**
- * ixgbevf_clean_rxonly - msix (aka one shot) rx clean routine
+ * ixgbevf_poll - NAPI polling calback
  * @napi: napi struct with our devices info in it
  * @budget: amount of work driver is allowed to do this pass, in packets
  *
- * This function is optimized for cleaning one queue only on a single
- * q_vector!!!
+ * This function will clean more than one or more rings associated with a
+ * q_vector.
  **/
-static int ixgbevf_clean_rxonly(struct napi_struct *napi, int budget)
+static int ixgbevf_poll(struct napi_struct *napi, int budget)
 {
 	struct ixgbevf_q_vector *q_vector =
 		container_of(napi, struct ixgbevf_q_vector, napi);
 	struct ixgbevf_adapter *adapter = q_vector->adapter;
-	struct ixgbevf_ring *rx_ring = NULL;
-	int work_done = 0;
-	long r_idx;
-
-	r_idx = find_first_bit(q_vector->rxr_idx, adapter->num_rx_queues);
-	rx_ring = &(adapter->rx_ring[r_idx]);
+	struct ixgbevf_ring *ring;
+	int per_ring_budget;
+	bool clean_complete = true;
 
-	ixgbevf_clean_rx_irq(q_vector, rx_ring, &work_done, budget);
+	ixgbevf_for_each_ring(ring, q_vector->tx)
+		clean_complete &= ixgbevf_clean_tx_irq(q_vector, ring);
 
-	/* If all Rx work done, exit the polling mode */
-	if (work_done < budget) {
-		napi_complete(napi);
-		if (adapter->itr_setting & 1)
-			ixgbevf_set_itr_msix(q_vector);
-		if (!test_bit(__IXGBEVF_DOWN, &adapter->state))
-			ixgbevf_irq_enable_queues(adapter, rx_ring->v_idx);
-	}
+	/* attempt to distribute budget to each queue fairly, but don't allow
+	 * the budget to go below 1 because we'll exit polling */
+	if (q_vector->rx.count > 1)
+		per_ring_budget = max(budget/q_vector->rx.count, 1);
+	else
+		per_ring_budget = budget;
+
+	ixgbevf_for_each_ring(ring, q_vector->rx)
+		clean_complete &= ixgbevf_clean_rx_irq(q_vector, ring,
+						       per_ring_budget);
+
+	/* If all work not completed, return budget and keep polling */
+	if (!clean_complete)
+		return budget;
+	/* all work done, exit the polling mode */
+	napi_complete(napi);
+	if (adapter->rx_itr_setting & 1)
+		ixgbevf_set_itr(q_vector);
+	if (!test_bit(__IXGBEVF_DOWN, &adapter->state))
+		ixgbevf_irq_enable_queues(adapter,
+					  1 << q_vector->v_idx);
 
-	return work_done;
+	return 0;
 }
 
 /**
- * ixgbevf_clean_rxonly_many - msix (aka one shot) rx clean routine
- * @napi: napi struct with our devices info in it
- * @budget: amount of work driver is allowed to do this pass, in packets
- *
- * This function will clean more than one rx queue associated with a
- * q_vector.
- **/
-static int ixgbevf_clean_rxonly_many(struct napi_struct *napi, int budget)
+ * ixgbevf_write_eitr - write VTEITR register in hardware specific way
+ * @q_vector: structure containing interrupt and ring information
+ */
+static void ixgbevf_write_eitr(struct ixgbevf_q_vector *q_vector)
 {
-	struct ixgbevf_q_vector *q_vector =
-		container_of(napi, struct ixgbevf_q_vector, napi);
 	struct ixgbevf_adapter *adapter = q_vector->adapter;
-	struct ixgbevf_ring *rx_ring = NULL;
-	int work_done = 0, i;
-	long r_idx;
-	u64 enable_mask = 0;
-
-	/* attempt to distribute budget to each queue fairly, but don't allow
-	 * the budget to go below 1 because we'll exit polling */
-	budget /= (q_vector->rxr_count ?: 1);
-	budget = max(budget, 1);
-	r_idx = find_first_bit(q_vector->rxr_idx, adapter->num_rx_queues);
-	for (i = 0; i < q_vector->rxr_count; i++) {
-		rx_ring = &(adapter->rx_ring[r_idx]);
-		ixgbevf_clean_rx_irq(q_vector, rx_ring, &work_done, budget);
-		enable_mask |= rx_ring->v_idx;
-		r_idx = find_next_bit(q_vector->rxr_idx, adapter->num_rx_queues,
-				      r_idx + 1);
-	}
-
-#ifndef HAVE_NETDEV_NAPI_LIST
-	if (!netif_running(adapter->netdev))
-		work_done = 0;
-
-#endif
-	r_idx = find_first_bit(q_vector->rxr_idx, adapter->num_rx_queues);
-	rx_ring = &(adapter->rx_ring[r_idx]);
+	struct ixgbe_hw *hw = &adapter->hw;
+	int v_idx = q_vector->v_idx;
+	u32 itr_reg = q_vector->itr & IXGBE_MAX_EITR;
 
-	/* If all Rx work done, exit the polling mode */
-	if (work_done < budget) {
-		napi_complete(napi);
-		if (adapter->itr_setting & 1)
-			ixgbevf_set_itr_msix(q_vector);
-		if (!test_bit(__IXGBEVF_DOWN, &adapter->state))
-			ixgbevf_irq_enable_queues(adapter, enable_mask);
-	}
+	/*
+	 * set the WDIS bit to not clear the timer bits and cause an
+	 * immediate assertion of the interrupt
+	 */
+	itr_reg |= IXGBE_EITR_CNT_WDIS;
 
-	return work_done;
+	IXGBE_WRITE_REG(hw, IXGBE_VTEITR(v_idx), itr_reg);
 }
 
-
 /**
  * ixgbevf_configure_msix - Configure MSI-X hardware
  * @adapter: board private structure
@@ -704,56 +569,49 @@ static int ixgbevf_clean_rxonly_many(struct napi_struct *napi, int budget)
 static void ixgbevf_configure_msix(struct ixgbevf_adapter *adapter)
 {
 	struct ixgbevf_q_vector *q_vector;
-	struct ixgbe_hw *hw = &adapter->hw;
-	int i, j, q_vectors, v_idx, r_idx;
-	u32 mask;
+	int q_vectors, v_idx;
 
 	q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
+	adapter->eims_enable_mask = 0;
 
 	/*
 	 * Populate the IVAR table and set the ITR values to the
 	 * corresponding register.
 	 */
 	for (v_idx = 0; v_idx < q_vectors; v_idx++) {
+		struct ixgbevf_ring *ring;
 		q_vector = adapter->q_vector[v_idx];
-		/* XXX for_each_set_bit(...) */
-		r_idx = find_first_bit(q_vector->rxr_idx,
-				       adapter->num_rx_queues);
-
-		for (i = 0; i < q_vector->rxr_count; i++) {
-			j = adapter->rx_ring[r_idx].reg_idx;
-			ixgbevf_set_ivar(adapter, 0, j, v_idx);
-			r_idx = find_next_bit(q_vector->rxr_idx,
-					      adapter->num_rx_queues,
-					      r_idx + 1);
-		}
-		r_idx = find_first_bit(q_vector->txr_idx,
-				       adapter->num_tx_queues);
-
-		for (i = 0; i < q_vector->txr_count; i++) {
-			j = adapter->tx_ring[r_idx].reg_idx;
-			ixgbevf_set_ivar(adapter, 1, j, v_idx);
-			r_idx = find_next_bit(q_vector->txr_idx,
-					      adapter->num_tx_queues,
-					      r_idx + 1);
+
+		ixgbevf_for_each_ring(ring, q_vector->rx)
+			ixgbevf_set_ivar(adapter, 0, ring->reg_idx, v_idx);
+
+		ixgbevf_for_each_ring(ring, q_vector->tx)
+			ixgbevf_set_ivar(adapter, 1, ring->reg_idx, v_idx);
+
+		if (q_vector->tx.ring && !q_vector->rx.ring) {
+			/* tx only vector */
+			if (adapter->tx_itr_setting == 1)
+				q_vector->itr = IXGBE_10K_ITR;
+			else
+				q_vector->itr = adapter->tx_itr_setting;
+		} else {
+			/* rx or rx/tx vector */
+			if (adapter->rx_itr_setting == 1)
+				q_vector->itr = IXGBE_20K_ITR;
+			else
+				q_vector->itr = adapter->rx_itr_setting;
 		}
 
-		/* if this is a tx only vector halve the interrupt rate */
-		if (q_vector->txr_count && !q_vector->rxr_count)
-			q_vector->eitr = (adapter->eitr_param >> 1);
-		else if (q_vector->rxr_count)
-			/* rx only */
-			q_vector->eitr = adapter->eitr_param;
+		/* add q_vector eims value to global eims_enable_mask */
+		adapter->eims_enable_mask |= 1 << v_idx;
 
-		ixgbevf_write_eitr(adapter, v_idx, q_vector->eitr);
+		ixgbevf_write_eitr(q_vector);
 	}
 
 	ixgbevf_set_ivar(adapter, -1, 1, v_idx);
-
-	/* set up to autoclear timer, and the vectors */
-	mask = IXGBE_EIMS_ENABLE_MASK;
-	mask &= ~IXGBE_EIMS_OTHER;
-	IXGBE_WRITE_REG(hw, IXGBE_VTEIAC, mask);
+	/* setup eims_other and add value to global eims_enable_mask */
+	adapter->eims_other = 1 << v_idx;
+	adapter->eims_enable_mask |= adapter->eims_other;
 }
 
 enum latency_range {
@@ -765,11 +623,8 @@ enum latency_range {
 
 /**
  * ixgbevf_update_itr - update the dynamic ITR value based on statistics
- * @adapter: pointer to adapter
- * @eitr: eitr setting (ints per sec) to give last timeslice
- * @itr_setting: current throttle rate in ints/second
- * @packets: the number of packets during this measurement interval
- * @bytes: the number of bytes during this measurement interval
+ * @q_vector: structure containing interrupt and ring information
+ * @ring_container: structure containing ring performance data
  *
  *      Stores a new ITR value based on packets and byte
  *      counts during the last interrupt.  The advantage of per interrupt
@@ -779,17 +634,17 @@ enum latency_range {
  *      on testing data as well as attempting to minimize response time
  *      while increasing bulk throughput.
  **/
-static u8 ixgbevf_update_itr(struct ixgbevf_adapter *adapter,
-			     u32 eitr, u8 itr_setting,
-			     int packets, int bytes)
+static void ixgbevf_update_itr(struct ixgbevf_q_vector *q_vector,
+			       struct ixgbevf_ring_container *ring_container)
 {
-	unsigned int retval = itr_setting;
+	int bytes = ring_container->total_bytes;
+	int packets = ring_container->total_packets;
 	u32 timepassed_us;
 	u64 bytes_perint;
+	u8 itr_setting = ring_container->itr;
 
 	if (packets == 0)
-		goto update_itr_done;
-
+		return;
 
 	/* simple throttlerate management
 	 *    0-20MB/s lowest (100000 ints/s)
@@ -797,134 +652,77 @@ static u8 ixgbevf_update_itr(struct ixgbevf_adapter *adapter,
 	 *  100-1249MB/s bulk (8000 ints/s)
 	 */
 	/* what was last interrupt timeslice? */
-	timepassed_us = 1000000/eitr;
+	timepassed_us = q_vector->itr >> 2;
 	bytes_perint = bytes / timepassed_us; /* bytes/usec */
 
 	switch (itr_setting) {
 	case lowest_latency:
-		if (bytes_perint > adapter->eitr_low)
-			retval = low_latency;
+		if (bytes_perint > 10)
+			itr_setting = low_latency;
 		break;
 	case low_latency:
-		if (bytes_perint > adapter->eitr_high)
-			retval = bulk_latency;
-		else if (bytes_perint <= adapter->eitr_low)
-			retval = lowest_latency;
+		if (bytes_perint > 20)
+			itr_setting = bulk_latency;
+		else if (bytes_perint <= 10)
+			itr_setting = lowest_latency;
 		break;
 	case bulk_latency:
-		if (bytes_perint <= adapter->eitr_high)
-			retval = low_latency;
+		if (bytes_perint <= 20)
+			itr_setting = low_latency;
 		break;
 	}
 
-update_itr_done:
-	return retval;
+	/* clear work counters since we have the values we need */
+	ring_container->total_bytes = 0;
+	ring_container->total_packets = 0;
+
+	/* write updated itr to ring container */
+	ring_container->itr = itr_setting;
 }
 
-/**
- * ixgbevf_write_eitr - write VTEITR register in hardware specific way
- * @adapter: pointer to adapter struct
- * @v_idx: vector index into q_vector array
- * @itr_reg: new value to be written in *register* format, not ints/s
- *
- * This function is made to be called by ethtool and by the driver
- * when it needs to update VTEITR registers at runtime.  Hardware
- * specific quirks/differences are taken care of here.
- */
-static void ixgbevf_write_eitr(struct ixgbevf_adapter *adapter, int v_idx,
-			       u32 itr_reg)
+static void ixgbevf_set_itr(struct ixgbevf_q_vector *q_vector)
 {
-	struct ixgbe_hw *hw = &adapter->hw;
-
-	itr_reg = EITR_INTS_PER_SEC_TO_REG(itr_reg);
-
-	/*
-	 * set the WDIS bit to not clear the timer bits and cause an
-	 * immediate assertion of the interrupt
-	 */
-	itr_reg |= IXGBE_EITR_CNT_WDIS;
+	u32 new_itr = q_vector->itr;
+	u8 current_itr;
 
-	IXGBE_WRITE_REG(hw, IXGBE_VTEITR(v_idx), itr_reg);
-}
+	ixgbevf_update_itr(q_vector, &q_vector->tx);
+	ixgbevf_update_itr(q_vector, &q_vector->rx);
 
-static void ixgbevf_set_itr_msix(struct ixgbevf_q_vector *q_vector)
-{
-	struct ixgbevf_adapter *adapter = q_vector->adapter;
-	u32 new_itr;
-	u8 current_itr, ret_itr;
-	int i, r_idx, v_idx = q_vector->v_idx;
-	struct ixgbevf_ring *rx_ring, *tx_ring;
-
-	r_idx = find_first_bit(q_vector->txr_idx, adapter->num_tx_queues);
-	for (i = 0; i < q_vector->txr_count; i++) {
-		tx_ring = &(adapter->tx_ring[r_idx]);
-		ret_itr = ixgbevf_update_itr(adapter, q_vector->eitr,
-					     q_vector->tx_itr,
-					     tx_ring->total_packets,
-					     tx_ring->total_bytes);
-		/* if the result for this queue would decrease interrupt
-		 * rate for this vector then use that result */
-		q_vector->tx_itr = ((q_vector->tx_itr > ret_itr) ?
-				    q_vector->tx_itr - 1 : ret_itr);
-		r_idx = find_next_bit(q_vector->txr_idx, adapter->num_tx_queues,
-				      r_idx + 1);
-	}
-
-	r_idx = find_first_bit(q_vector->rxr_idx, adapter->num_rx_queues);
-	for (i = 0; i < q_vector->rxr_count; i++) {
-		rx_ring = &(adapter->rx_ring[r_idx]);
-		ret_itr = ixgbevf_update_itr(adapter, q_vector->eitr,
-					     q_vector->rx_itr,
-					     rx_ring->total_packets,
-					     rx_ring->total_bytes);
-		/* if the result for this queue would decrease interrupt
-		 * rate for this vector then use that result */
-		q_vector->rx_itr = ((q_vector->rx_itr > ret_itr) ?
-				    q_vector->rx_itr - 1 : ret_itr);
-		r_idx = find_next_bit(q_vector->rxr_idx, adapter->num_rx_queues,
-				      r_idx + 1);
-	}
-
-	current_itr = max(q_vector->rx_itr, q_vector->tx_itr);
+	current_itr = max(q_vector->rx.itr, q_vector->tx.itr);
 
 	switch (current_itr) {
 	/* counts and packets in update_itr are dependent on these numbers */
 	case lowest_latency:
-		new_itr = 100000;
+		new_itr = IXGBE_100K_ITR;
 		break;
 	case low_latency:
-		new_itr = 20000; /* aka hwitr = ~200 */
+		new_itr = IXGBE_20K_ITR;
 		break;
 	case bulk_latency:
 	default:
-		new_itr = 8000;
+		new_itr = IXGBE_8K_ITR;
 		break;
 	}
 
-	if (new_itr != q_vector->eitr) {
-		u32 itr_reg;
-
-		/* save the algorithm value here, not the smoothed one */
-		q_vector->eitr = new_itr;
+	if (new_itr != q_vector->itr) {
 		/* do an exponential smoothing */
-		new_itr = ((q_vector->eitr * 90)/100) + ((new_itr * 10)/100);
-		itr_reg = EITR_INTS_PER_SEC_TO_REG(new_itr);
-		ixgbevf_write_eitr(adapter, v_idx, itr_reg);
+		new_itr = (10 * new_itr * q_vector->itr) /
+			  ((9 * new_itr) + q_vector->itr);
+
+		/* save the algorithm value here */
+		q_vector->itr = new_itr;
+
+		ixgbevf_write_eitr(q_vector);
 	}
 }
 
 static irqreturn_t ixgbevf_msix_mbx(int irq, void *data)
 {
-	struct net_device *netdev = data;
-	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+	struct ixgbevf_adapter *adapter = data;
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 eicr;
 	u32 msg;
 	bool got_ack = false;
 
-	eicr = IXGBE_READ_REG(hw, IXGBE_VTEICS);
-	IXGBE_WRITE_REG(hw, IXGBE_VTEICR, eicr);
-
 	if (!hw->mbx.ops.check_for_ack(hw))
 		got_ack = true;
 
@@ -953,75 +751,24 @@ static irqreturn_t ixgbevf_msix_mbx(int irq, void *data)
 	if (got_ack)
 		hw->mbx.v2p_mailbox |= IXGBE_VFMAILBOX_PFACK;
 
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t ixgbevf_msix_clean_tx(int irq, void *data)
-{
-	struct ixgbevf_q_vector *q_vector = data;
-	struct ixgbevf_adapter  *adapter = q_vector->adapter;
-	struct ixgbevf_ring     *tx_ring;
-	int i, r_idx;
-
-	if (!q_vector->txr_count)
-		return IRQ_HANDLED;
-
-	r_idx = find_first_bit(q_vector->txr_idx, adapter->num_tx_queues);
-	for (i = 0; i < q_vector->txr_count; i++) {
-		tx_ring = &(adapter->tx_ring[r_idx]);
-		tx_ring->total_bytes = 0;
-		tx_ring->total_packets = 0;
-		ixgbevf_clean_tx_irq(adapter, tx_ring);
-		r_idx = find_next_bit(q_vector->txr_idx, adapter->num_tx_queues,
-				      r_idx + 1);
-	}
-
-	if (adapter->itr_setting & 1)
-		ixgbevf_set_itr_msix(q_vector);
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIMS, adapter->eims_other);
 
 	return IRQ_HANDLED;
 }
 
+
 /**
- * ixgbevf_msix_clean_rx - single unshared vector rx clean (all queues)
+ * ixgbevf_msix_clean_rings - single unshared vector rx clean (all queues)
  * @irq: unused
  * @data: pointer to our q_vector struct for this interrupt vector
  **/
-static irqreturn_t ixgbevf_msix_clean_rx(int irq, void *data)
+static irqreturn_t ixgbevf_msix_clean_rings(int irq, void *data)
 {
 	struct ixgbevf_q_vector *q_vector = data;
-	struct ixgbevf_adapter  *adapter = q_vector->adapter;
-	struct ixgbe_hw *hw = &adapter->hw;
-	struct ixgbevf_ring  *rx_ring;
-	int r_idx;
-	int i;
-
-	r_idx = find_first_bit(q_vector->rxr_idx, adapter->num_rx_queues);
-	for (i = 0; i < q_vector->rxr_count; i++) {
-		rx_ring = &(adapter->rx_ring[r_idx]);
-		rx_ring->total_bytes = 0;
-		rx_ring->total_packets = 0;
-		r_idx = find_next_bit(q_vector->rxr_idx, adapter->num_rx_queues,
-				      r_idx + 1);
-	}
-
-	if (!q_vector->rxr_count)
-		return IRQ_HANDLED;
-
-	r_idx = find_first_bit(q_vector->rxr_idx, adapter->num_rx_queues);
-	rx_ring = &(adapter->rx_ring[r_idx]);
-	/* disable interrupts on this vector only */
-	IXGBE_WRITE_REG(hw, IXGBE_VTEIMC, rx_ring->v_idx);
-	napi_schedule(&q_vector->napi);
 
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t ixgbevf_msix_clean_many(int irq, void *data)
-{
-	ixgbevf_msix_clean_rx(irq, data);
-	ixgbevf_msix_clean_tx(irq, data);
+	/* EIAM disabled interrupts (on this vector) for us */
+	if (q_vector->rx.ring || q_vector->tx.ring)
+		napi_schedule(&q_vector->napi);
 
 	return IRQ_HANDLED;
 }
@@ -1031,9 +778,9 @@ static inline void map_vector_to_rxq(struct ixgbevf_adapter *a, int v_idx,
 {
 	struct ixgbevf_q_vector *q_vector = a->q_vector[v_idx];
 
-	set_bit(r_idx, q_vector->rxr_idx);
-	q_vector->rxr_count++;
-	a->rx_ring[r_idx].v_idx = 1 << v_idx;
+	a->rx_ring[r_idx].next = q_vector->rx.ring;
+	q_vector->rx.ring = &a->rx_ring[r_idx];
+	q_vector->rx.count++;
 }
 
 static inline void map_vector_to_txq(struct ixgbevf_adapter *a, int v_idx,
@@ -1041,9 +788,9 @@ static inline void map_vector_to_txq(struct ixgbevf_adapter *a, int v_idx,
 {
 	struct ixgbevf_q_vector *q_vector = a->q_vector[v_idx];
 
-	set_bit(t_idx, q_vector->txr_idx);
-	q_vector->txr_count++;
-	a->tx_ring[t_idx].v_idx = 1 << v_idx;
+	a->tx_ring[t_idx].next = q_vector->tx.ring;
+	q_vector->tx.ring = &a->tx_ring[t_idx];
+	q_vector->tx.count++;
 }
 
 /**
@@ -1119,37 +866,30 @@ out:
 static int ixgbevf_request_msix_irqs(struct ixgbevf_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
-	irqreturn_t (*handler)(int, void *);
-	int i, vector, q_vectors, err;
+	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
+	int vector, err;
 	int ri = 0, ti = 0;
 
-	/* Decrement for Other and TCP Timer vectors */
-	q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-
-#define SET_HANDLER(_v) (((_v)->rxr_count && (_v)->txr_count)          \
-					  ? &ixgbevf_msix_clean_many : \
-			  (_v)->rxr_count ? &ixgbevf_msix_clean_rx   : \
-			  (_v)->txr_count ? &ixgbevf_msix_clean_tx   : \
-			  NULL)
 	for (vector = 0; vector < q_vectors; vector++) {
-		handler = SET_HANDLER(adapter->q_vector[vector]);
-
-		if (handler == &ixgbevf_msix_clean_rx) {
-			sprintf(adapter->name[vector], "%s-%s-%d",
-				netdev->name, "rx", ri++);
-		} else if (handler == &ixgbevf_msix_clean_tx) {
-			sprintf(adapter->name[vector], "%s-%s-%d",
-				netdev->name, "tx", ti++);
-		} else if (handler == &ixgbevf_msix_clean_many) {
-			sprintf(adapter->name[vector], "%s-%s-%d",
-				netdev->name, "TxRx", vector);
+		struct ixgbevf_q_vector *q_vector = adapter->q_vector[vector];
+		struct msix_entry *entry = &adapter->msix_entries[vector];
+
+		if (q_vector->tx.ring && q_vector->rx.ring) {
+			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
+				 "%s-%s-%d", netdev->name, "TxRx", ri++);
+			ti++;
+		} else if (q_vector->rx.ring) {
+			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
+				 "%s-%s-%d", netdev->name, "rx", ri++);
+		} else if (q_vector->tx.ring) {
+			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
+				 "%s-%s-%d", netdev->name, "tx", ti++);
 		} else {
 			/* skip this unused q_vector */
 			continue;
 		}
-		err = request_irq(adapter->msix_entries[vector].vector,
-				  handler, 0, adapter->name[vector],
-				  adapter->q_vector[vector]);
+		err = request_irq(entry->vector, &ixgbevf_msix_clean_rings, 0,
+				  q_vector->name, q_vector);
 		if (err) {
 			hw_dbg(&adapter->hw,
 			       "request_irq failed for MSIX interrupt "
@@ -1158,9 +898,8 @@ static int ixgbevf_request_msix_irqs(struct ixgbevf_adapter *adapter)
 		}
 	}
 
-	sprintf(adapter->name[vector], "%s:mbx", netdev->name);
 	err = request_irq(adapter->msix_entries[vector].vector,
-			  &ixgbevf_msix_mbx, 0, adapter->name[vector], netdev);
+			  &ixgbevf_msix_mbx, 0, netdev->name, adapter);
 	if (err) {
 		hw_dbg(&adapter->hw,
 		       "request_irq for msix_mbx failed: %d\n", err);
@@ -1170,9 +909,11 @@ static int ixgbevf_request_msix_irqs(struct ixgbevf_adapter *adapter)
 	return 0;
 
 free_queue_irqs:
-	for (i = vector - 1; i >= 0; i--)
-		free_irq(adapter->msix_entries[--vector].vector,
-			 &(adapter->q_vector[i]));
+	while (vector) {
+		vector--;
+		free_irq(adapter->msix_entries[vector].vector,
+			 adapter->q_vector[vector]);
+	}
 	pci_disable_msix(adapter->pdev);
 	kfree(adapter->msix_entries);
 	adapter->msix_entries = NULL;
@@ -1185,11 +926,10 @@ static inline void ixgbevf_reset_q_vectors(struct ixgbevf_adapter *adapter)
 
 	for (i = 0; i < q_vectors; i++) {
 		struct ixgbevf_q_vector *q_vector = adapter->q_vector[i];
-		bitmap_zero(q_vector->rxr_idx, MAX_RX_QUEUES);
-		bitmap_zero(q_vector->txr_idx, MAX_TX_QUEUES);
-		q_vector->rxr_count = 0;
-		q_vector->txr_count = 0;
-		q_vector->eitr = adapter->eitr_param;
+		q_vector->rx.ring = NULL;
+		q_vector->tx.ring = NULL;
+		q_vector->rx.count = 0;
+		q_vector->tx.count = 0;
 	}
 }
 
@@ -1215,17 +955,20 @@ static int ixgbevf_request_irq(struct ixgbevf_adapter *adapter)
 
 static void ixgbevf_free_irq(struct ixgbevf_adapter *adapter)
 {
-	struct net_device *netdev = adapter->netdev;
 	int i, q_vectors;
 
 	q_vectors = adapter->num_msix_vectors;
-
 	i = q_vectors - 1;
 
-	free_irq(adapter->msix_entries[i].vector, netdev);
+	free_irq(adapter->msix_entries[i].vector, adapter);
 	i--;
 
 	for (; i >= 0; i--) {
+		/* free only the irqs that were actually requested */
+		if (!adapter->q_vector[i]->rx.ring &&
+		    !adapter->q_vector[i]->tx.ring)
+			continue;
+
 		free_irq(adapter->msix_entries[i].vector,
 			 adapter->q_vector[i]);
 	}
@@ -1239,10 +982,12 @@ static void ixgbevf_free_irq(struct ixgbevf_adapter *adapter)
  **/
 static inline void ixgbevf_irq_disable(struct ixgbevf_adapter *adapter)
 {
-	int i;
 	struct ixgbe_hw *hw = &adapter->hw;
+	int i;
 
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIAM, 0);
 	IXGBE_WRITE_REG(hw, IXGBE_VTEIMC, ~0);
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIAC, 0);
 
 	IXGBE_WRITE_FLUSH(hw);
 
@@ -1254,23 +999,13 @@ static inline void ixgbevf_irq_disable(struct ixgbevf_adapter *adapter)
  * ixgbevf_irq_enable - Enable default interrupt generation settings
  * @adapter: board private structure
  **/
-static inline void ixgbevf_irq_enable(struct ixgbevf_adapter *adapter,
-				      bool queues, bool flush)
+static inline void ixgbevf_irq_enable(struct ixgbevf_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 mask;
-	u64 qmask;
-
-	mask = (IXGBE_EIMS_ENABLE_MASK & ~IXGBE_EIMS_RTX_QUEUE);
-	qmask = ~0;
-
-	IXGBE_WRITE_REG(hw, IXGBE_VTEIMS, mask);
 
-	if (queues)
-		ixgbevf_irq_enable_queues(adapter, qmask);
-
-	if (flush)
-		IXGBE_WRITE_FLUSH(hw);
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIAM, adapter->eims_enable_mask);
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIAC, adapter->eims_enable_mask);
+	IXGBE_WRITE_REG(hw, IXGBE_VTEIMS, adapter->eims_enable_mask);
 }
 
 /**
@@ -1320,29 +1055,14 @@ static void ixgbevf_configure_srrctl(struct ixgbevf_adapter *adapter, int index)
 
 	srrctl = IXGBE_SRRCTL_DROP_EN;
 
-	if (adapter->flags & IXGBE_FLAG_RX_PS_ENABLED) {
-		u16 bufsz = IXGBEVF_RXBUFFER_2048;
-		/* grow the amount we can receive on large page machines */
-		if (bufsz < (PAGE_SIZE / 2))
-			bufsz = (PAGE_SIZE / 2);
-		/* cap the bufsz at our largest descriptor size */
-		bufsz = min((u16)IXGBEVF_MAX_RXBUFFER, bufsz);
-
-		srrctl |= bufsz >> IXGBE_SRRCTL_BSIZEPKT_SHIFT;
-		srrctl |= IXGBE_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS;
-		srrctl |= ((IXGBEVF_RX_HDR_SIZE <<
-			   IXGBE_SRRCTL_BSIZEHDRSIZE_SHIFT) &
-			   IXGBE_SRRCTL_BSIZEHDR_MASK);
-	} else {
-		srrctl |= IXGBE_SRRCTL_DESCTYPE_ADV_ONEBUF;
+	srrctl |= IXGBE_SRRCTL_DESCTYPE_ADV_ONEBUF;
 
-		if (rx_ring->rx_buf_len == MAXIMUM_ETHERNET_VLAN_SIZE)
-			srrctl |= IXGBEVF_RXBUFFER_2048 >>
-				IXGBE_SRRCTL_BSIZEPKT_SHIFT;
-		else
-			srrctl |= rx_ring->rx_buf_len >>
-				IXGBE_SRRCTL_BSIZEPKT_SHIFT;
-	}
+	if (rx_ring->rx_buf_len == MAXIMUM_ETHERNET_VLAN_SIZE)
+		srrctl |= IXGBEVF_RXBUFFER_2048 >>
+			IXGBE_SRRCTL_BSIZEPKT_SHIFT;
+	else
+		srrctl |= rx_ring->rx_buf_len >>
+			IXGBE_SRRCTL_BSIZEPKT_SHIFT;
 	IXGBE_WRITE_REG(hw, IXGBE_VFSRRCTL(index), srrctl);
 }
 
@@ -1362,36 +1082,12 @@ static void ixgbevf_configure_rx(struct ixgbevf_adapter *adapter)
 	u32 rdlen;
 	int rx_buf_len;
 
-	/* Decide whether to use packet split mode or not */
-	if (netdev->mtu > ETH_DATA_LEN) {
-		if (adapter->flags & IXGBE_FLAG_RX_PS_CAPABLE)
-			adapter->flags |= IXGBE_FLAG_RX_PS_ENABLED;
-		else
-			adapter->flags &= ~IXGBE_FLAG_RX_PS_ENABLED;
-	} else {
-		if (adapter->flags & IXGBE_FLAG_RX_1BUF_CAPABLE)
-			adapter->flags &= ~IXGBE_FLAG_RX_PS_ENABLED;
-		else
-			adapter->flags |= IXGBE_FLAG_RX_PS_ENABLED;
-	}
-
-	/* Set the RX buffer length according to the mode */
-	if (adapter->flags & IXGBE_FLAG_RX_PS_ENABLED) {
-		/* PSRTYPE must be initialized in 82599 */
-		u32 psrtype = IXGBE_PSRTYPE_TCPHDR |
-			IXGBE_PSRTYPE_UDPHDR |
-			IXGBE_PSRTYPE_IPV4HDR |
-			IXGBE_PSRTYPE_IPV6HDR |
-			IXGBE_PSRTYPE_L2HDR;
-		IXGBE_WRITE_REG(hw, IXGBE_VFPSRTYPE, psrtype);
-		rx_buf_len = IXGBEVF_RX_HDR_SIZE;
-	} else {
-		IXGBE_WRITE_REG(hw, IXGBE_VFPSRTYPE, 0);
-		if (netdev->mtu <= ETH_DATA_LEN)
-			rx_buf_len = MAXIMUM_ETHERNET_VLAN_SIZE;
-		else
-			rx_buf_len = ALIGN(max_frame, 1024);
-	}
+	/* PSRTYPE must be initialized in 82599 */
+	IXGBE_WRITE_REG(hw, IXGBE_VFPSRTYPE, 0);
+	if (netdev->mtu <= ETH_DATA_LEN)
+		rx_buf_len = MAXIMUM_ETHERNET_VLAN_SIZE;
+	else
+		rx_buf_len = ALIGN(max_frame, 1024);
 
 	rdlen = adapter->rx_ring[0].count * sizeof(union ixgbe_adv_rx_desc);
 	/* Setup the HW Rx Head and Tail Descriptor Pointers and
@@ -1418,9 +1114,14 @@ static int ixgbevf_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 
+	spin_lock(&adapter->mbx_lock);
+
 	/* add VID to filter table */
 	if (hw->mac.ops.set_vfta)
 		hw->mac.ops.set_vfta(hw, vid, 0, true);
+
+	spin_unlock(&adapter->mbx_lock);
+
 	set_bit(vid, adapter->active_vlans);
 
 	return 0;
@@ -1431,9 +1132,14 @@ static int ixgbevf_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 
+	spin_lock(&adapter->mbx_lock);
+
 	/* remove VID from filter table */
 	if (hw->mac.ops.set_vfta)
 		hw->mac.ops.set_vfta(hw, vid, 0, false);
+
+	spin_unlock(&adapter->mbx_lock);
+
 	clear_bit(vid, adapter->active_vlans);
 
 	return 0;
@@ -1488,11 +1194,15 @@ static void ixgbevf_set_rx_mode(struct net_device *netdev)
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 
+	spin_lock(&adapter->mbx_lock);
+
 	/* reprogram multicast list */
 	if (hw->mac.ops.update_mc_addr_list)
 		hw->mac.ops.update_mc_addr_list(hw, netdev);
 
 	ixgbevf_write_uc_addr_list(netdev);
+
+	spin_unlock(&adapter->mbx_lock);
 }
 
 static void ixgbevf_napi_enable_all(struct ixgbevf_adapter *adapter)
@@ -1502,15 +1212,8 @@ static void ixgbevf_napi_enable_all(struct ixgbevf_adapter *adapter)
 	int q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
 
 	for (q_idx = 0; q_idx < q_vectors; q_idx++) {
-		struct napi_struct *napi;
 		q_vector = adapter->q_vector[q_idx];
-		if (!q_vector->rxr_count)
-			continue;
-		napi = &q_vector->napi;
-		if (q_vector->rxr_count > 1)
-			napi->poll = &ixgbevf_clean_rxonly_many;
-
-		napi_enable(napi);
+		napi_enable(&q_vector->napi);
 	}
 }
 
@@ -1522,8 +1225,6 @@ static void ixgbevf_napi_disable_all(struct ixgbevf_adapter *adapter)
 
 	for (q_idx = 0; q_idx < q_vectors; q_idx++) {
 		q_vector = adapter->q_vector[q_idx];
-		if (!q_vector->rxr_count)
-			continue;
 		napi_disable(&q_vector->napi);
 	}
 }
@@ -1541,9 +1242,8 @@ static void ixgbevf_configure(struct ixgbevf_adapter *adapter)
 	ixgbevf_configure_rx(adapter);
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		struct ixgbevf_ring *ring = &adapter->rx_ring[i];
-		ixgbevf_alloc_rx_buffers(adapter, ring, ring->count);
-		ring->next_to_use = ring->count - 1;
-		writel(ring->next_to_use, adapter->hw.hw_addr + ring->tail);
+		ixgbevf_alloc_rx_buffers(adapter, ring,
+					 IXGBE_DESC_UNUSED(ring));
 	}
 }
 
@@ -1647,6 +1347,8 @@ static void ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 
 	ixgbevf_configure_msix(adapter);
 
+	spin_lock(&adapter->mbx_lock);
+
 	if (hw->mac.ops.set_rar) {
 		if (is_valid_ether_addr(hw->mac.addr))
 			hw->mac.ops.set_rar(hw, 0, hw->mac.addr, 0);
@@ -1658,6 +1360,8 @@ static void ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 	msg[1] = netdev->mtu + ETH_HLEN + ETH_FCS_LEN;
 	hw->mbx.ops.write_posted(hw, msg, 2);
 
+	spin_unlock(&adapter->mbx_lock);
+
 	clear_bit(__IXGBEVF_DOWN, &adapter->state);
 	ixgbevf_napi_enable_all(adapter);
 
@@ -1667,10 +1371,6 @@ static void ixgbevf_up_complete(struct ixgbevf_adapter *adapter)
 	ixgbevf_save_reset_stats(adapter);
 	ixgbevf_init_last_counter_stats(adapter);
 
-	/* bring the link up in the watchdog, this could race with our first
-	 * link up interrupt but shouldn't be a problem */
-	adapter->flags |= IXGBE_FLAG_NEED_LINK_UPDATE;
-	adapter->link_check_timeout = jiffies;
 	mod_timer(&adapter->watchdog_timer, jiffies);
 }
 
@@ -1685,7 +1385,7 @@ void ixgbevf_up(struct ixgbevf_adapter *adapter)
 	/* clear any pending interrupts, may auto mask */
 	IXGBE_READ_REG(hw, IXGBE_VTEICR);
 
-	ixgbevf_irq_enable(adapter, true, true);
+	ixgbevf_irq_enable(adapter);
 }
 
 /**
@@ -1723,14 +1423,6 @@ static void ixgbevf_clean_rx_ring(struct ixgbevf_adapter *adapter,
 				dev_kfree_skb(this);
 			} while (skb);
 		}
-		if (!rx_buffer_info->page)
-			continue;
-		dma_unmap_page(&pdev->dev, rx_buffer_info->page_dma,
-			       PAGE_SIZE / 2, DMA_FROM_DEVICE);
-		rx_buffer_info->page_dma = 0;
-		put_page(rx_buffer_info->page);
-		rx_buffer_info->page = NULL;
-		rx_buffer_info->page_offset = 0;
 	}
 
 	size = sizeof(struct ixgbevf_rx_buffer) * rx_ring->count;
@@ -1767,7 +1459,7 @@ static void ixgbevf_clean_tx_ring(struct ixgbevf_adapter *adapter,
 
 	for (i = 0; i < tx_ring->count; i++) {
 		tx_buffer_info = &tx_ring->tx_buffer_info[i];
-		ixgbevf_unmap_and_free_tx_resource(adapter, tx_buffer_info);
+		ixgbevf_unmap_and_free_tx_resource(tx_ring, tx_buffer_info);
 	}
 
 	size = sizeof(struct ixgbevf_tx_buffer) * tx_ring->count;
@@ -1882,11 +1574,15 @@ void ixgbevf_reset(struct ixgbevf_adapter *adapter)
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 
+	spin_lock(&adapter->mbx_lock);
+
 	if (hw->mac.ops.reset_hw(hw))
 		hw_dbg(hw, "PF still resetting\n");
 	else
 		hw->mac.ops.init_hw(hw);
 
+	spin_unlock(&adapter->mbx_lock);
+
 	if (is_valid_ether_addr(adapter->hw.mac.addr)) {
 		memcpy(netdev->dev_addr, adapter->hw.mac.addr,
 		       netdev->addr_len);
@@ -1900,10 +1596,9 @@ static void ixgbevf_acquire_msix_vectors(struct ixgbevf_adapter *adapter,
 {
 	int err, vector_threshold;
 
-	/* We'll want at least 3 (vector_threshold):
-	 * 1) TxQ[0] Cleanup
-	 * 2) RxQ[0] Cleanup
-	 * 3) Other (Link Status Change, etc.)
+	/* We'll want at least 2 (vector_threshold):
+	 * 1) TxQ[0] + RxQ[0] handler
+	 * 2) Other (Link Status Change, etc.)
 	 */
 	vector_threshold = MIN_MSIX_COUNT;
 
@@ -1942,8 +1637,8 @@ static void ixgbevf_acquire_msix_vectors(struct ixgbevf_adapter *adapter,
 	}
 }
 
-/*
- * ixgbevf_set_num_queues: Allocate queues for device, feature dependent
+/**
+ * ixgbevf_set_num_queues - Allocate queues for device, feature dependent
  * @adapter: board private structure to initialize
  *
  * This is the top level queue allocation routine.  The order here is very
@@ -1958,8 +1653,6 @@ static void ixgbevf_set_num_queues(struct ixgbevf_adapter *adapter)
 	/* Start with base case */
 	adapter->num_rx_queues = 1;
 	adapter->num_tx_queues = 1;
-	adapter->num_rx_pools = adapter->num_rx_queues;
-	adapter->num_rx_queues_per_pool = 1;
 }
 
 /**
@@ -1988,12 +1681,16 @@ static int ixgbevf_alloc_queues(struct ixgbevf_adapter *adapter)
 		adapter->tx_ring[i].count = adapter->tx_ring_count;
 		adapter->tx_ring[i].queue_index = i;
 		adapter->tx_ring[i].reg_idx = i;
+		adapter->tx_ring[i].dev = &adapter->pdev->dev;
+		adapter->tx_ring[i].netdev = adapter->netdev;
 	}
 
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		adapter->rx_ring[i].count = adapter->rx_ring_count;
 		adapter->rx_ring[i].queue_index = i;
 		adapter->rx_ring[i].reg_idx = i;
+		adapter->rx_ring[i].dev = &adapter->pdev->dev;
+		adapter->rx_ring[i].netdev = adapter->netdev;
 	}
 
 	return 0;
@@ -2020,10 +1717,12 @@ static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 	 * It's easy to be greedy for MSI-X vectors, but it really
 	 * doesn't do us much good if we have a lot more vectors
 	 * than CPU's.  So let's be conservative and only ask for
-	 * (roughly) twice the number of vectors as there are CPU's.
+	 * (roughly) the same number of vectors as there are CPU's.
+	 * The default is to use pairs of vectors.
 	 */
-	v_budget = min(adapter->num_rx_queues + adapter->num_tx_queues,
-		       (int)(num_online_cpus() * 2)) + NON_Q_VECTORS;
+	v_budget = max(adapter->num_rx_queues, adapter->num_tx_queues);
+	v_budget = min_t(int, v_budget, num_online_cpus());
+	v_budget += NON_Q_VECTORS;
 
 	/* A failure in MSI-X entry allocation isn't fatal, but it does
 	 * mean we disable MSI-X capabilities of the adapter. */
@@ -2054,12 +1753,8 @@ static int ixgbevf_alloc_q_vectors(struct ixgbevf_adapter *adapter)
 {
 	int q_idx, num_q_vectors;
 	struct ixgbevf_q_vector *q_vector;
-	int napi_vectors;
-	int (*poll)(struct napi_struct *, int);
 
 	num_q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
-	napi_vectors = adapter->num_rx_queues;
-	poll = &ixgbevf_clean_rxonly;
 
 	for (q_idx = 0; q_idx < num_q_vectors; q_idx++) {
 		q_vector = kzalloc(sizeof(struct ixgbevf_q_vector), GFP_KERNEL);
@@ -2067,10 +1762,8 @@ static int ixgbevf_alloc_q_vectors(struct ixgbevf_adapter *adapter)
 			goto err_out;
 		q_vector->adapter = adapter;
 		q_vector->v_idx = q_idx;
-		q_vector->eitr = adapter->eitr_param;
-		if (q_idx < napi_vectors)
-			netif_napi_add(adapter->netdev, &q_vector->napi,
-				       (*poll), 64);
+		netif_napi_add(adapter->netdev, &q_vector->napi,
+			       ixgbevf_poll, 64);
 		adapter->q_vector[q_idx] = q_vector;
 	}
 
@@ -2216,21 +1909,17 @@ static int __devinit ixgbevf_sw_init(struct ixgbevf_adapter *adapter)
 			adapter->netdev->addr_len);
 	}
 
-	/* Enable dynamic interrupt throttling rates */
-	adapter->eitr_param = 20000;
-	adapter->itr_setting = 1;
+	/* lock to protect mailbox accesses */
+	spin_lock_init(&adapter->mbx_lock);
 
-	/* set defaults for eitr in MegaBytes */
-	adapter->eitr_low = 10;
-	adapter->eitr_high = 20;
+	/* Enable dynamic interrupt throttling rates */
+	adapter->rx_itr_setting = 1;
+	adapter->tx_itr_setting = 1;
 
 	/* set default ring sizes */
 	adapter->tx_ring_count = IXGBEVF_DEFAULT_TXD;
 	adapter->rx_ring_count = IXGBEVF_DEFAULT_RXD;
 
-	/* enable rx csum by default */
-	adapter->flags |= IXGBE_FLAG_RX_CSUM_ENABLED;
-
 	set_bit(__IXGBEVF_DOWN, &adapter->state);
 	return 0;
 
@@ -2290,7 +1979,7 @@ static void ixgbevf_watchdog(unsigned long data)
 {
 	struct ixgbevf_adapter *adapter = (struct ixgbevf_adapter *)data;
 	struct ixgbe_hw *hw = &adapter->hw;
-	u64 eics = 0;
+	u32 eics = 0;
 	int i;
 
 	/*
@@ -2304,11 +1993,11 @@ static void ixgbevf_watchdog(unsigned long data)
 	/* get one bit for every active tx/rx interrupt vector */
 	for (i = 0; i < adapter->num_msix_vectors - NON_Q_VECTORS; i++) {
 		struct ixgbevf_q_vector *qv = adapter->q_vector[i];
-		if (qv->rxr_count || qv->txr_count)
-			eics |= (1 << i);
+		if (qv->rx.ring || qv->tx.ring)
+			eics |= 1 << i;
 	}
 
-	IXGBE_WRITE_REG(hw, IXGBE_VTEICS, (u32)eics);
+	IXGBE_WRITE_REG(hw, IXGBE_VTEICS, eics);
 
 watchdog_short_circuit:
 	schedule_work(&adapter->watchdog_task);
@@ -2362,8 +2051,16 @@ static void ixgbevf_watchdog_task(struct work_struct *work)
 	 * no LSC interrupt
 	 */
 	if (hw->mac.ops.check_link) {
-		if ((hw->mac.ops.check_link(hw, &link_speed,
-					    &link_up, false)) != 0) {
+		s32 need_reset;
+
+		spin_lock(&adapter->mbx_lock);
+
+		need_reset = hw->mac.ops.check_link(hw, &link_speed,
+						    &link_up, false);
+
+		spin_unlock(&adapter->mbx_lock);
+
+		if (need_reset) {
 			adapter->link_up = link_up;
 			adapter->link_speed = link_speed;
 			netif_carrier_off(netdev);
@@ -2478,7 +2175,6 @@ int ixgbevf_setup_tx_resources(struct ixgbevf_adapter *adapter,
 
 	tx_ring->next_to_use = 0;
 	tx_ring->next_to_clean = 0;
-	tx_ring->work_limit = tx_ring->count;
 	return 0;
 
 err:
@@ -2682,7 +2378,7 @@ static int ixgbevf_open(struct net_device *netdev)
 	if (err)
 		goto err_req_irq;
 
-	ixgbevf_irq_enable(adapter, true, true);
+	ixgbevf_irq_enable(adapter);
 
 	return 0;
 
@@ -2724,172 +2420,153 @@ static int ixgbevf_close(struct net_device *netdev)
 	return 0;
 }
 
-static int ixgbevf_tso(struct ixgbevf_adapter *adapter,
-		       struct ixgbevf_ring *tx_ring,
-		       struct sk_buff *skb, u32 tx_flags, u8 *hdr_len)
+static void ixgbevf_tx_ctxtdesc(struct ixgbevf_ring *tx_ring,
+				u32 vlan_macip_lens, u32 type_tucmd,
+				u32 mss_l4len_idx)
 {
 	struct ixgbe_adv_tx_context_desc *context_desc;
-	unsigned int i;
-	int err;
-	struct ixgbevf_tx_buffer *tx_buffer_info;
-	u32 vlan_macip_lens = 0, type_tucmd_mlhl;
-	u32 mss_l4len_idx, l4len;
+	u16 i = tx_ring->next_to_use;
 
-	if (skb_is_gso(skb)) {
-		if (skb_header_cloned(skb)) {
-			err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
-			if (err)
-				return err;
-		}
-		l4len = tcp_hdrlen(skb);
-		*hdr_len += l4len;
-
-		if (skb->protocol == htons(ETH_P_IP)) {
-			struct iphdr *iph = ip_hdr(skb);
-			iph->tot_len = 0;
-			iph->check = 0;
-			tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr,
-								 iph->daddr, 0,
-								 IPPROTO_TCP,
-								 0);
-			adapter->hw_tso_ctxt++;
-		} else if (skb_is_gso_v6(skb)) {
-			ipv6_hdr(skb)->payload_len = 0;
-			tcp_hdr(skb)->check =
-			    ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
-					     &ipv6_hdr(skb)->daddr,
-					     0, IPPROTO_TCP, 0);
-			adapter->hw_tso6_ctxt++;
-		}
+	context_desc = IXGBEVF_TX_CTXTDESC(tx_ring, i);
 
-		i = tx_ring->next_to_use;
+	i++;
+	tx_ring->next_to_use = (i < tx_ring->count) ? i : 0;
 
-		tx_buffer_info = &tx_ring->tx_buffer_info[i];
-		context_desc = IXGBE_TX_CTXTDESC_ADV(*tx_ring, i);
-
-		/* VLAN MACLEN IPLEN */
-		if (tx_flags & IXGBE_TX_FLAGS_VLAN)
-			vlan_macip_lens |=
-				(tx_flags & IXGBE_TX_FLAGS_VLAN_MASK);
-		vlan_macip_lens |= ((skb_network_offset(skb)) <<
-				    IXGBE_ADVTXD_MACLEN_SHIFT);
-		*hdr_len += skb_network_offset(skb);
-		vlan_macip_lens |=
-			(skb_transport_header(skb) - skb_network_header(skb));
-		*hdr_len +=
-			(skb_transport_header(skb) - skb_network_header(skb));
-		context_desc->vlan_macip_lens = cpu_to_le32(vlan_macip_lens);
-		context_desc->seqnum_seed = 0;
-
-		/* ADV DTYP TUCMD MKRLOC/ISCSIHEDLEN */
-		type_tucmd_mlhl = (IXGBE_TXD_CMD_DEXT |
-				    IXGBE_ADVTXD_DTYP_CTXT);
-
-		if (skb->protocol == htons(ETH_P_IP))
-			type_tucmd_mlhl |= IXGBE_ADVTXD_TUCMD_IPV4;
-		type_tucmd_mlhl |= IXGBE_ADVTXD_TUCMD_L4T_TCP;
-		context_desc->type_tucmd_mlhl = cpu_to_le32(type_tucmd_mlhl);
-
-		/* MSS L4LEN IDX */
-		mss_l4len_idx =
-			(skb_shinfo(skb)->gso_size << IXGBE_ADVTXD_MSS_SHIFT);
-		mss_l4len_idx |= (l4len << IXGBE_ADVTXD_L4LEN_SHIFT);
-		/* use index 1 for TSO */
-		mss_l4len_idx |= (1 << IXGBE_ADVTXD_IDX_SHIFT);
-		context_desc->mss_l4len_idx = cpu_to_le32(mss_l4len_idx);
-
-		tx_buffer_info->time_stamp = jiffies;
-		tx_buffer_info->next_to_watch = i;
+	/* set bits to identify this as an advanced context descriptor */
+	type_tucmd |= IXGBE_TXD_CMD_DEXT | IXGBE_ADVTXD_DTYP_CTXT;
 
-		i++;
-		if (i == tx_ring->count)
-			i = 0;
-		tx_ring->next_to_use = i;
+	context_desc->vlan_macip_lens	= cpu_to_le32(vlan_macip_lens);
+	context_desc->seqnum_seed	= 0;
+	context_desc->type_tucmd_mlhl	= cpu_to_le32(type_tucmd);
+	context_desc->mss_l4len_idx	= cpu_to_le32(mss_l4len_idx);
+}
 
-		return true;
+static int ixgbevf_tso(struct ixgbevf_ring *tx_ring,
+		       struct sk_buff *skb, u32 tx_flags, u8 *hdr_len)
+{
+	u32 vlan_macip_lens, type_tucmd;
+	u32 mss_l4len_idx, l4len;
+
+	if (!skb_is_gso(skb))
+		return 0;
+
+	if (skb_header_cloned(skb)) {
+		int err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
+		if (err)
+			return err;
 	}
 
-	return false;
+	/* ADV DTYP TUCMD MKRLOC/ISCSIHEDLEN */
+	type_tucmd = IXGBE_ADVTXD_TUCMD_L4T_TCP;
+
+	if (skb->protocol == htons(ETH_P_IP)) {
+		struct iphdr *iph = ip_hdr(skb);
+		iph->tot_len = 0;
+		iph->check = 0;
+		tcp_hdr(skb)->check = ~csum_tcpudp_magic(iph->saddr,
+							 iph->daddr, 0,
+							 IPPROTO_TCP,
+							 0);
+		type_tucmd |= IXGBE_ADVTXD_TUCMD_IPV4;
+	} else if (skb_is_gso_v6(skb)) {
+		ipv6_hdr(skb)->payload_len = 0;
+		tcp_hdr(skb)->check =
+		    ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
+				     &ipv6_hdr(skb)->daddr,
+				     0, IPPROTO_TCP, 0);
+	}
+
+	/* compute header lengths */
+	l4len = tcp_hdrlen(skb);
+	*hdr_len += l4len;
+	*hdr_len = skb_transport_offset(skb) + l4len;
+
+	/* mss_l4len_id: use 1 as index for TSO */
+	mss_l4len_idx = l4len << IXGBE_ADVTXD_L4LEN_SHIFT;
+	mss_l4len_idx |= skb_shinfo(skb)->gso_size << IXGBE_ADVTXD_MSS_SHIFT;
+	mss_l4len_idx |= 1 << IXGBE_ADVTXD_IDX_SHIFT;
+
+	/* vlan_macip_lens: HEADLEN, MACLEN, VLAN tag */
+	vlan_macip_lens = skb_network_header_len(skb);
+	vlan_macip_lens |= skb_network_offset(skb) << IXGBE_ADVTXD_MACLEN_SHIFT;
+	vlan_macip_lens |= tx_flags & IXGBE_TX_FLAGS_VLAN_MASK;
+
+	ixgbevf_tx_ctxtdesc(tx_ring, vlan_macip_lens,
+			    type_tucmd, mss_l4len_idx);
+
+	return 1;
 }
 
-static bool ixgbevf_tx_csum(struct ixgbevf_adapter *adapter,
-			    struct ixgbevf_ring *tx_ring,
+static bool ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 			    struct sk_buff *skb, u32 tx_flags)
 {
-	struct ixgbe_adv_tx_context_desc *context_desc;
-	unsigned int i;
-	struct ixgbevf_tx_buffer *tx_buffer_info;
-	u32 vlan_macip_lens = 0, type_tucmd_mlhl = 0;
 
-	if (skb->ip_summed == CHECKSUM_PARTIAL ||
-	    (tx_flags & IXGBE_TX_FLAGS_VLAN)) {
-		i = tx_ring->next_to_use;
-		tx_buffer_info = &tx_ring->tx_buffer_info[i];
-		context_desc = IXGBE_TX_CTXTDESC_ADV(*tx_ring, i);
-
-		if (tx_flags & IXGBE_TX_FLAGS_VLAN)
-			vlan_macip_lens |= (tx_flags &
-					    IXGBE_TX_FLAGS_VLAN_MASK);
-		vlan_macip_lens |= (skb_network_offset(skb) <<
-				    IXGBE_ADVTXD_MACLEN_SHIFT);
-		if (skb->ip_summed == CHECKSUM_PARTIAL)
-			vlan_macip_lens |= (skb_transport_header(skb) -
-					    skb_network_header(skb));
-
-		context_desc->vlan_macip_lens = cpu_to_le32(vlan_macip_lens);
-		context_desc->seqnum_seed = 0;
-
-		type_tucmd_mlhl |= (IXGBE_TXD_CMD_DEXT |
-				    IXGBE_ADVTXD_DTYP_CTXT);
-
-		if (skb->ip_summed == CHECKSUM_PARTIAL) {
-			switch (skb->protocol) {
-			case __constant_htons(ETH_P_IP):
-				type_tucmd_mlhl |= IXGBE_ADVTXD_TUCMD_IPV4;
-				if (ip_hdr(skb)->protocol == IPPROTO_TCP)
-					type_tucmd_mlhl |=
-					    IXGBE_ADVTXD_TUCMD_L4T_TCP;
-				break;
-			case __constant_htons(ETH_P_IPV6):
-				/* XXX what about other V6 headers?? */
-				if (ipv6_hdr(skb)->nexthdr == IPPROTO_TCP)
-					type_tucmd_mlhl |=
-						IXGBE_ADVTXD_TUCMD_L4T_TCP;
-				break;
-			default:
-				if (unlikely(net_ratelimit())) {
-					pr_warn("partial checksum but "
-						"proto=%x!\n", skb->protocol);
-				}
-				break;
-			}
-		}
 
-		context_desc->type_tucmd_mlhl = cpu_to_le32(type_tucmd_mlhl);
-		/* use index zero for tx checksum offload */
-		context_desc->mss_l4len_idx = 0;
 
-		tx_buffer_info->time_stamp = jiffies;
-		tx_buffer_info->next_to_watch = i;
+	u32 vlan_macip_lens = 0;
+	u32 mss_l4len_idx = 0;
+	u32 type_tucmd = 0;
 
-		adapter->hw_csum_tx_good++;
-		i++;
-		if (i == tx_ring->count)
-			i = 0;
-		tx_ring->next_to_use = i;
+	if (skb->ip_summed == CHECKSUM_PARTIAL) {
+		u8 l4_hdr = 0;
+		switch (skb->protocol) {
+		case __constant_htons(ETH_P_IP):
+			vlan_macip_lens |= skb_network_header_len(skb);
+			type_tucmd |= IXGBE_ADVTXD_TUCMD_IPV4;
+			l4_hdr = ip_hdr(skb)->protocol;
+			break;
+		case __constant_htons(ETH_P_IPV6):
+			vlan_macip_lens |= skb_network_header_len(skb);
+			l4_hdr = ipv6_hdr(skb)->nexthdr;
+			break;
+		default:
+			if (unlikely(net_ratelimit())) {
+				dev_warn(tx_ring->dev,
+				 "partial checksum but proto=%x!\n",
+				 skb->protocol);
+			}
+			break;
+		}
 
-		return true;
+		switch (l4_hdr) {
+		case IPPROTO_TCP:
+			type_tucmd |= IXGBE_ADVTXD_TUCMD_L4T_TCP;
+			mss_l4len_idx = tcp_hdrlen(skb) <<
+					IXGBE_ADVTXD_L4LEN_SHIFT;
+			break;
+		case IPPROTO_SCTP:
+			type_tucmd |= IXGBE_ADVTXD_TUCMD_L4T_SCTP;
+			mss_l4len_idx = sizeof(struct sctphdr) <<
+					IXGBE_ADVTXD_L4LEN_SHIFT;
+			break;
+		case IPPROTO_UDP:
+			mss_l4len_idx = sizeof(struct udphdr) <<
+					IXGBE_ADVTXD_L4LEN_SHIFT;
+			break;
+		default:
+			if (unlikely(net_ratelimit())) {
+				dev_warn(tx_ring->dev,
+				 "partial checksum but l4 proto=%x!\n",
+				 l4_hdr);
+			}
+			break;
+		}
 	}
 
-	return false;
+	/* vlan_macip_lens: MACLEN, VLAN tag */
+	vlan_macip_lens |= skb_network_offset(skb) << IXGBE_ADVTXD_MACLEN_SHIFT;
+	vlan_macip_lens |= tx_flags & IXGBE_TX_FLAGS_VLAN_MASK;
+
+	ixgbevf_tx_ctxtdesc(tx_ring, vlan_macip_lens,
+			    type_tucmd, mss_l4len_idx);
+
+	return (skb->ip_summed == CHECKSUM_PARTIAL);
 }
 
-static int ixgbevf_tx_map(struct ixgbevf_adapter *adapter,
-			  struct ixgbevf_ring *tx_ring,
+static int ixgbevf_tx_map(struct ixgbevf_ring *tx_ring,
 			  struct sk_buff *skb, u32 tx_flags,
 			  unsigned int first)
 {
-	struct pci_dev *pdev = adapter->pdev;
 	struct ixgbevf_tx_buffer *tx_buffer_info;
 	unsigned int len;
 	unsigned int total = skb->len;
@@ -2908,12 +2585,11 @@ static int ixgbevf_tx_map(struct ixgbevf_adapter *adapter,
 
 		tx_buffer_info->length = size;
 		tx_buffer_info->mapped_as_page = false;
-		tx_buffer_info->dma = dma_map_single(&adapter->pdev->dev,
+		tx_buffer_info->dma = dma_map_single(tx_ring->dev,
 						     skb->data + offset,
 						     size, DMA_TO_DEVICE);
-		if (dma_mapping_error(&pdev->dev, tx_buffer_info->dma))
+		if (dma_mapping_error(tx_ring->dev, tx_buffer_info->dma))
 			goto dma_error;
-		tx_buffer_info->time_stamp = jiffies;
 		tx_buffer_info->next_to_watch = i;
 
 		len -= size;
@@ -2938,12 +2614,12 @@ static int ixgbevf_tx_map(struct ixgbevf_adapter *adapter,
 
 			tx_buffer_info->length = size;
 			tx_buffer_info->dma =
-				skb_frag_dma_map(&adapter->pdev->dev, frag,
+				skb_frag_dma_map(tx_ring->dev, frag,
 						 offset, size, DMA_TO_DEVICE);
 			tx_buffer_info->mapped_as_page = true;
-			if (dma_mapping_error(&pdev->dev, tx_buffer_info->dma))
+			if (dma_mapping_error(tx_ring->dev,
+					      tx_buffer_info->dma))
 				goto dma_error;
-			tx_buffer_info->time_stamp = jiffies;
 			tx_buffer_info->next_to_watch = i;
 
 			len -= size;
@@ -2964,15 +2640,15 @@ static int ixgbevf_tx_map(struct ixgbevf_adapter *adapter,
 		i = i - 1;
 	tx_ring->tx_buffer_info[i].skb = skb;
 	tx_ring->tx_buffer_info[first].next_to_watch = i;
+	tx_ring->tx_buffer_info[first].time_stamp = jiffies;
 
 	return count;
 
 dma_error:
-	dev_err(&pdev->dev, "TX DMA map failed\n");
+	dev_err(tx_ring->dev, "TX DMA map failed\n");
 
 	/* clear timestamp and dma mappings for failed tx_buffer_info map */
 	tx_buffer_info->dma = 0;
-	tx_buffer_info->time_stamp = 0;
 	tx_buffer_info->next_to_watch = 0;
 	count--;
 
@@ -2983,14 +2659,13 @@ dma_error:
 		if (i < 0)
 			i += tx_ring->count;
 		tx_buffer_info = &tx_ring->tx_buffer_info[i];
-		ixgbevf_unmap_and_free_tx_resource(adapter, tx_buffer_info);
+		ixgbevf_unmap_and_free_tx_resource(tx_ring, tx_buffer_info);
 	}
 
 	return count;
 }
 
-static void ixgbevf_tx_queue(struct ixgbevf_adapter *adapter,
-			     struct ixgbevf_ring *tx_ring, int tx_flags,
+static void ixgbevf_tx_queue(struct ixgbevf_ring *tx_ring, int tx_flags,
 			     int count, u32 paylen, u8 hdr_len)
 {
 	union ixgbe_adv_tx_desc *tx_desc = NULL;
@@ -3007,28 +2682,31 @@ static void ixgbevf_tx_queue(struct ixgbevf_adapter *adapter,
 	if (tx_flags & IXGBE_TX_FLAGS_VLAN)
 		cmd_type_len |= IXGBE_ADVTXD_DCMD_VLE;
 
+	if (tx_flags & IXGBE_TX_FLAGS_CSUM)
+		olinfo_status |= IXGBE_ADVTXD_POPTS_TXSM;
+
 	if (tx_flags & IXGBE_TX_FLAGS_TSO) {
 		cmd_type_len |= IXGBE_ADVTXD_DCMD_TSE;
 
-		olinfo_status |= IXGBE_TXD_POPTS_TXSM <<
-			IXGBE_ADVTXD_POPTS_SHIFT;
-
 		/* use index 1 context for tso */
 		olinfo_status |= (1 << IXGBE_ADVTXD_IDX_SHIFT);
 		if (tx_flags & IXGBE_TX_FLAGS_IPV4)
-			olinfo_status |= IXGBE_TXD_POPTS_IXSM <<
-				IXGBE_ADVTXD_POPTS_SHIFT;
+			olinfo_status |= IXGBE_ADVTXD_POPTS_IXSM;
+
+	}
 
-	} else if (tx_flags & IXGBE_TX_FLAGS_CSUM)
-		olinfo_status |= IXGBE_TXD_POPTS_TXSM <<
-			IXGBE_ADVTXD_POPTS_SHIFT;
+	/*
+	 * Check Context must be set if Tx switch is enabled, which it
+	 * always is for case where virtual functions are running
+	 */
+	olinfo_status |= IXGBE_ADVTXD_CC;
 
 	olinfo_status |= ((paylen - hdr_len) << IXGBE_ADVTXD_PAYLEN_SHIFT);
 
 	i = tx_ring->next_to_use;
 	while (count--) {
 		tx_buffer_info = &tx_ring->tx_buffer_info[i];
-		tx_desc = IXGBE_TX_DESC_ADV(*tx_ring, i);
+		tx_desc = IXGBEVF_TX_DESC(tx_ring, i);
 		tx_desc->read.buffer_addr = cpu_to_le64(tx_buffer_info->dma);
 		tx_desc->read.cmd_type_len =
 			cpu_to_le32(cmd_type_len | tx_buffer_info->length);
@@ -3040,24 +2718,14 @@ static void ixgbevf_tx_queue(struct ixgbevf_adapter *adapter,
 
 	tx_desc->read.cmd_type_len |= cpu_to_le32(txd_cmd);
 
-	/*
-	 * Force memory writes to complete before letting h/w
-	 * know there are new descriptors to fetch.  (Only
-	 * applicable for weak-ordered memory model archs,
-	 * such as IA-64).
-	 */
-	wmb();
-
 	tx_ring->next_to_use = i;
-	writel(i, adapter->hw.hw_addr + tx_ring->tail);
 }
 
-static int __ixgbevf_maybe_stop_tx(struct net_device *netdev,
-				   struct ixgbevf_ring *tx_ring, int size)
+static int __ixgbevf_maybe_stop_tx(struct ixgbevf_ring *tx_ring, int size)
 {
-	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+	struct ixgbevf_adapter *adapter = netdev_priv(tx_ring->netdev);
 
-	netif_stop_subqueue(netdev, tx_ring->queue_index);
+	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
 	/* Herbert's original patch had:
 	 *  smp_mb__after_netif_stop_queue();
 	 * but since that doesn't exist yet, just open code it. */
@@ -3069,17 +2737,16 @@ static int __ixgbevf_maybe_stop_tx(struct net_device *netdev,
 		return -EBUSY;
 
 	/* A reprieve! - use start_queue because it doesn't call schedule */
-	netif_start_subqueue(netdev, tx_ring->queue_index);
+	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
 	++adapter->restart_queue;
 	return 0;
 }
 
-static int ixgbevf_maybe_stop_tx(struct net_device *netdev,
-				 struct ixgbevf_ring *tx_ring, int size)
+static int ixgbevf_maybe_stop_tx(struct ixgbevf_ring *tx_ring, int size)
 {
 	if (likely(IXGBE_DESC_UNUSED(tx_ring) >= size))
 		return 0;
-	return __ixgbevf_maybe_stop_tx(netdev, tx_ring, size);
+	return __ixgbevf_maybe_stop_tx(tx_ring, size);
 }
 
 static int ixgbevf_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
@@ -3090,54 +2757,66 @@ static int ixgbevf_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 	unsigned int tx_flags = 0;
 	u8 hdr_len = 0;
 	int r_idx = 0, tso;
-	int count = 0;
-
-	unsigned int f;
+	u16 count = TXD_USE_COUNT(skb_headlen(skb));
+#if PAGE_SIZE > IXGBE_MAX_DATA_PER_TXD
+	unsigned short f;
+#endif
 
 	tx_ring = &adapter->tx_ring[r_idx];
 
+	/*
+	 * need: 1 descriptor per page * PAGE_SIZE/IXGBE_MAX_DATA_PER_TXD,
+	 *       + 1 desc for skb_headlen/IXGBE_MAX_DATA_PER_TXD,
+	 *       + 2 desc gap to keep tail from touching head,
+	 *       + 1 desc for context descriptor,
+	 * otherwise try next time
+	 */
+#if PAGE_SIZE > IXGBE_MAX_DATA_PER_TXD
+	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
+		count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
+#else
+	count += skb_shinfo(skb)->nr_frags;
+#endif
+	if (ixgbevf_maybe_stop_tx(tx_ring, count + 3)) {
+		adapter->tx_busy++;
+		return NETDEV_TX_BUSY;
+	}
+
 	if (vlan_tx_tag_present(skb)) {
 		tx_flags |= vlan_tx_tag_get(skb);
 		tx_flags <<= IXGBE_TX_FLAGS_VLAN_SHIFT;
 		tx_flags |= IXGBE_TX_FLAGS_VLAN;
 	}
 
-	/* four things can cause us to need a context descriptor */
-	if (skb_is_gso(skb) ||
-	    (skb->ip_summed == CHECKSUM_PARTIAL) ||
-	    (tx_flags & IXGBE_TX_FLAGS_VLAN))
-		count++;
-
-	count += TXD_USE_COUNT(skb_headlen(skb));
-	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
-		count += TXD_USE_COUNT(skb_frag_size(&skb_shinfo(skb)->frags[f]));
-
-	if (ixgbevf_maybe_stop_tx(netdev, tx_ring, count)) {
-		adapter->tx_busy++;
-		return NETDEV_TX_BUSY;
-	}
-
 	first = tx_ring->next_to_use;
 
 	if (skb->protocol == htons(ETH_P_IP))
 		tx_flags |= IXGBE_TX_FLAGS_IPV4;
-	tso = ixgbevf_tso(adapter, tx_ring, skb, tx_flags, &hdr_len);
+	tso = ixgbevf_tso(tx_ring, skb, tx_flags, &hdr_len);
 	if (tso < 0) {
 		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
 	}
 
 	if (tso)
-		tx_flags |= IXGBE_TX_FLAGS_TSO;
-	else if (ixgbevf_tx_csum(adapter, tx_ring, skb, tx_flags) &&
-		 (skb->ip_summed == CHECKSUM_PARTIAL))
+		tx_flags |= IXGBE_TX_FLAGS_TSO | IXGBE_TX_FLAGS_CSUM;
+	else if (ixgbevf_tx_csum(tx_ring, skb, tx_flags))
 		tx_flags |= IXGBE_TX_FLAGS_CSUM;
 
-	ixgbevf_tx_queue(adapter, tx_ring, tx_flags,
-			 ixgbevf_tx_map(adapter, tx_ring, skb, tx_flags, first),
+	ixgbevf_tx_queue(tx_ring, tx_flags,
+			 ixgbevf_tx_map(tx_ring, skb, tx_flags, first),
 			 skb->len, hdr_len);
+	/*
+	 * Force memory writes to complete before letting h/w
+	 * know there are new descriptors to fetch.  (Only
+	 * applicable for weak-ordered memory model archs,
+	 * such as IA-64).
+	 */
+	wmb();
+
+	writel(tx_ring->next_to_use, adapter->hw.hw_addr + tx_ring->tail);
 
-	ixgbevf_maybe_stop_tx(netdev, tx_ring, DESC_NEEDED);
+	ixgbevf_maybe_stop_tx(tx_ring, DESC_NEEDED);
 
 	return NETDEV_TX_OK;
 }
@@ -3161,9 +2840,13 @@ static int ixgbevf_set_mac(struct net_device *netdev, void *p)
 	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
 	memcpy(hw->mac.addr, addr->sa_data, netdev->addr_len);
 
+	spin_lock(&adapter->mbx_lock);
+
 	if (hw->mac.ops.set_rar)
 		hw->mac.ops.set_rar(hw, 0, hw->mac.addr, 0);
 
+	spin_unlock(&adapter->mbx_lock);
+
 	return 0;
 }
 
@@ -3220,9 +2903,7 @@ static void ixgbevf_shutdown(struct pci_dev *pdev)
 		ixgbevf_free_all_rx_resources(adapter);
 	}
 
-#ifdef CONFIG_PM
 	pci_save_state(pdev);
-#endif
 
 	pci_disable_device(pdev);
 }
@@ -3265,19 +2946,6 @@ static struct rtnl_link_stats64 *ixgbevf_get_stats(struct net_device *netdev,
 	return stats;
 }
 
-static int ixgbevf_set_features(struct net_device *netdev,
-	netdev_features_t features)
-{
-	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
-
-	if (features & NETIF_F_RXCSUM)
-		adapter->flags |= IXGBE_FLAG_RX_CSUM_ENABLED;
-	else
-		adapter->flags &= ~IXGBE_FLAG_RX_CSUM_ENABLED;
-
-	return 0;
-}
-
 static const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_open		= ixgbevf_open,
 	.ndo_stop		= ixgbevf_close,
@@ -3290,7 +2958,6 @@ static const struct net_device_ops ixgbe_netdev_ops = {
 	.ndo_tx_timeout		= ixgbevf_tx_timeout,
 	.ndo_vlan_rx_add_vid	= ixgbevf_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= ixgbevf_vlan_rx_kill_vid,
-	.ndo_set_features	= ixgbevf_set_features,
 };
 
 static void ixgbevf_assign_netdev_ops(struct net_device *dev)
@@ -3350,12 +3017,8 @@ static int __devinit ixgbevf_probe(struct pci_dev *pdev,
 
 	pci_set_master(pdev);
 
-#ifdef HAVE_TX_MQ
 	netdev = alloc_etherdev_mq(sizeof(struct ixgbevf_adapter),
 				   MAX_TX_QUEUES);
-#else
-	netdev = alloc_etherdev(sizeof(struct ixgbevf_adapter));
-#endif
 	if (!netdev) {
 		err = -ENOMEM;
 		goto err_alloc_etherdev;
@@ -3396,10 +3059,6 @@ static int __devinit ixgbevf_probe(struct pci_dev *pdev,
 	memcpy(&hw->mbx.ops, &ixgbevf_mbx_ops,
 	       sizeof(struct ixgbe_mbx_operations));
 
-	adapter->flags &= ~IXGBE_FLAG_RX_PS_CAPABLE;
-	adapter->flags &= ~IXGBE_FLAG_RX_PS_ENABLED;
-	adapter->flags |= IXGBE_FLAG_RX_1BUF_CAPABLE;
-
 	/* setup the private structure */
 	err = ixgbevf_sw_init(adapter);
 	if (err)
@@ -3458,8 +3117,6 @@ static int __devinit ixgbevf_probe(struct pci_dev *pdev,
 	if (err)
 		goto err_register;
 
-	adapter->netdev_registered = true;
-
 	netif_carrier_off(netdev);
 
 	ixgbevf_init_last_counter_stats(adapter);
@@ -3469,8 +3126,6 @@ static int __devinit ixgbevf_probe(struct pci_dev *pdev,
 
 	hw_dbg(hw, "MAC: %d\n", hw->mac.type);
 
-	hw_dbg(hw, "LRO is disabled\n");
-
 	hw_dbg(hw, "Intel(R) 82599 Virtual Function\n");
 	cards_found++;
 	return 0;
@@ -3510,10 +3165,8 @@ static void __devexit ixgbevf_remove(struct pci_dev *pdev)
 	cancel_work_sync(&adapter->reset_task);
 	cancel_work_sync(&adapter->watchdog_task);
 
-	if (adapter->netdev_registered) {
+	if (netdev->reg_state == NETREG_REGISTERED)
 		unregister_netdev(netdev);
-		adapter->netdev_registered = false;
-	}
 
 	ixgbevf_reset_interrupt_capability(adapter);
 
@@ -3530,12 +3183,92 @@ static void __devexit ixgbevf_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 }
 
+/**
+ * ixgbevf_io_error_detected - called when PCI error is detected
+ * @pdev: Pointer to PCI device
+ * @state: The current pci connection state
+ *
+ * This function is called after a PCI bus error affecting
+ * this device has been detected.
+ */
+static pci_ers_result_t ixgbevf_io_error_detected(struct pci_dev *pdev,
+						  pci_channel_state_t state)
+{
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
+	netif_device_detach(netdev);
+
+	if (state == pci_channel_io_perm_failure)
+		return PCI_ERS_RESULT_DISCONNECT;
+
+	if (netif_running(netdev))
+		ixgbevf_down(adapter);
+
+	pci_disable_device(pdev);
+
+	/* Request a slot slot reset. */
+	return PCI_ERS_RESULT_NEED_RESET;
+}
+
+/**
+ * ixgbevf_io_slot_reset - called after the pci bus has been reset.
+ * @pdev: Pointer to PCI device
+ *
+ * Restart the card from scratch, as if from a cold-boot. Implementation
+ * resembles the first-half of the ixgbevf_resume routine.
+ */
+static pci_ers_result_t ixgbevf_io_slot_reset(struct pci_dev *pdev)
+{
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
+	if (pci_enable_device_mem(pdev)) {
+		dev_err(&pdev->dev,
+			"Cannot re-enable PCI device after reset.\n");
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+
+	pci_set_master(pdev);
+
+	ixgbevf_reset(adapter);
+
+	return PCI_ERS_RESULT_RECOVERED;
+}
+
+/**
+ * ixgbevf_io_resume - called when traffic can start flowing again.
+ * @pdev: Pointer to PCI device
+ *
+ * This callback is called when the error recovery driver tells us that
+ * its OK to resume normal operation. Implementation resembles the
+ * second-half of the ixgbevf_resume routine.
+ */
+static void ixgbevf_io_resume(struct pci_dev *pdev)
+{
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
+
+	if (netif_running(netdev))
+		ixgbevf_up(adapter);
+
+	netif_device_attach(netdev);
+}
+
+/* PCI Error Recovery (ERS) */
+static struct pci_error_handlers ixgbevf_err_handler = {
+	.error_detected = ixgbevf_io_error_detected,
+	.slot_reset = ixgbevf_io_slot_reset,
+	.resume = ixgbevf_io_resume,
+};
+
 static struct pci_driver ixgbevf_driver = {
 	.name     = ixgbevf_driver_name,
 	.id_table = ixgbevf_pci_tbl,
 	.probe    = ixgbevf_probe,
 	.remove   = __devexit_p(ixgbevf_remove),
 	.shutdown = ixgbevf_shutdown,
+	.err_handler = &ixgbevf_err_handler
 };
 
 /**
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index 4ea6580d3ae..c911d883c27 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -2743,6 +2743,17 @@ jme_set_features(struct net_device *netdev, netdev_features_t features)
 	return 0;
 }
 
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void jme_netpoll(struct net_device *dev)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+	jme_intr(dev->irq, dev);
+	local_irq_restore(flags);
+}
+#endif
+
 static int
 jme_nway_reset(struct net_device *netdev)
 {
@@ -2944,6 +2955,9 @@ static const struct net_device_ops jme_netdev_ops = {
 	.ndo_tx_timeout		= jme_tx_timeout,
 	.ndo_fix_features       = jme_fix_features,
 	.ndo_set_features       = jme_set_features,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= jme_netpoll,
+#endif
 };
 
 static int __devinit
diff --git a/drivers/net/ethernet/lantiq_etop.c b/drivers/net/ethernet/lantiq_etop.c
index 5dc9cbd5151..003c5bc7189 100644
--- a/drivers/net/ethernet/lantiq_etop.c
+++ b/drivers/net/ethernet/lantiq_etop.c
@@ -149,7 +149,6 @@ ltq_etop_hw_receive(struct ltq_etop_chan *ch)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	skb_put(skb, len);
-	skb->dev = ch->netdev;
 	skb->protocol = eth_type_trans(skb, ch->netdev);
 	netif_receive_skb(skb);
 }
@@ -646,7 +645,7 @@ ltq_etop_init(struct net_device *dev)
 	memcpy(&mac, &priv->pldata->mac, sizeof(struct sockaddr));
 	if (!is_valid_ether_addr(mac.sa_data)) {
 		pr_warn("etop: invalid MAC, using random\n");
-		random_ether_addr(mac.sa_data);
+		eth_random_addr(mac.sa_data);
 		random_mac = true;
 	}
 
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index f0f06b2bc28..087b9e0669f 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -1896,7 +1896,7 @@ static int rxq_init(struct mv643xx_eth_private *mp, int index)
 		goto out_free;
 	}
 
-	rx_desc = (struct rx_desc *)rxq->rx_desc_area;
+	rx_desc = rxq->rx_desc_area;
 	for (i = 0; i < rxq->rx_ring_size; i++) {
 		int nexti;
 
@@ -2001,7 +2001,7 @@ static int txq_init(struct mv643xx_eth_private *mp, int index)
 
 	txq->tx_desc_area_size = size;
 
-	tx_desc = (struct tx_desc *)txq->tx_desc_area;
+	tx_desc = txq->tx_desc_area;
 	for (i = 0; i < txq->tx_ring_size; i++) {
 		struct tx_desc *txd = tx_desc + i;
 		int nexti;
@@ -2983,6 +2983,12 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 	return 0;
 
 out:
+#if defined(CONFIG_HAVE_CLK)
+	if (!IS_ERR(mp->clk)) {
+		clk_disable_unprepare(mp->clk);
+		clk_put(mp->clk);
+	}
+#endif
 	free_netdev(dev);
 
 	return err;
diff --git a/drivers/net/ethernet/marvell/pxa168_eth.c b/drivers/net/ethernet/marvell/pxa168_eth.c
index 1db023b075a..59489722e89 100644
--- a/drivers/net/ethernet/marvell/pxa168_eth.c
+++ b/drivers/net/ethernet/marvell/pxa168_eth.c
@@ -1032,7 +1032,7 @@ static int rxq_init(struct net_device *dev)
 	}
 	memset((void *)pep->p_rx_desc_area, 0, size);
 	/* initialize the next_desc_ptr links in the Rx descriptors ring */
-	p_rx_desc = (struct rx_desc *)pep->p_rx_desc_area;
+	p_rx_desc = pep->p_rx_desc_area;
 	for (i = 0; i < rx_desc_num; i++) {
 		p_rx_desc[i].next_desc_ptr = pep->rx_desc_dma +
 		    ((i + 1) % rx_desc_num) * sizeof(struct rx_desc);
@@ -1095,7 +1095,7 @@ static int txq_init(struct net_device *dev)
 	}
 	memset((void *)pep->p_tx_desc_area, 0, pep->tx_desc_area_size);
 	/* Initialize the next_desc_ptr links in the Tx descriptors ring */
-	p_tx_desc = (struct tx_desc *)pep->p_tx_desc_area;
+	p_tx_desc = pep->p_tx_desc_area;
 	for (i = 0; i < tx_desc_num; i++) {
 		p_tx_desc[i].next_desc_ptr = pep->tx_desc_dma +
 		    ((i + 1) % tx_desc_num) * sizeof(struct tx_desc);
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 28a54451a3e..2b0748dba8b 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -141,6 +141,7 @@ static DEFINE_PCI_DEVICE_TABLE(sky2_id_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, 0x4370) }, /* 88E8075 */
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, 0x4380) }, /* 88E8057 */
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, 0x4381) }, /* 88E8059 */
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, 0x4382) }, /* 88E8079 */
 	{ 0 }
 };
 
@@ -3079,8 +3080,10 @@ static irqreturn_t sky2_intr(int irq, void *dev_id)
 
 	/* Reading this mask interrupts as side effect */
 	status = sky2_read32(hw, B0_Y2_SP_ISRC2);
-	if (status == 0 || status == ~0)
+	if (status == 0 || status == ~0) {
+		sky2_write32(hw, B0_Y2_SP_ICR, 2);
 		return IRQ_NONE;
+	}
 
 	prefetch(&hw->st_le[hw->st_idx]);
 
@@ -3349,6 +3352,17 @@ static void sky2_reset(struct sky2_hw *hw)
 			sky2_pci_write16(hw, pdev->pcie_cap + PCI_EXP_LNKCTL,
 					 reg);
 
+		if (hw->chip_id == CHIP_ID_YUKON_PRM &&
+			hw->chip_rev == CHIP_REV_YU_PRM_A0) {
+			/* change PHY Interrupt polarity to low active */
+			reg = sky2_read16(hw, GPHY_CTRL);
+			sky2_write16(hw, GPHY_CTRL, reg | GPC_INTPOL);
+
+			/* adapt HW for low active PHY Interrupt */
+			reg = sky2_read16(hw, Y2_CFG_SPC + PCI_LDO_CTRL);
+			sky2_write16(hw, Y2_CFG_SPC + PCI_LDO_CTRL, reg | PHY_M_UNDOC1);
+		}
+
 		sky2_write8(hw, B2_TST_CTRL1, TST_CFG_WRITE_OFF);
 
 		/* re-enable PEX PM in PEX PHY debug reg. 8 (clear bit 12) */
@@ -4871,7 +4885,7 @@ static const char *sky2_name(u8 chipid, char *buf, int sz)
 		"UL 2",		/* 0xba */
 		"Unknown",	/* 0xbb */
 		"Optima",	/* 0xbc */
-		"Optima Prime", /* 0xbd */
+		"OptimaEEE",    /* 0xbd */
 		"Optima 2",	/* 0xbe */
 	};
 
diff --git a/drivers/net/ethernet/marvell/sky2.h b/drivers/net/ethernet/marvell/sky2.h
index 3c896ce80b7..615ac63ea86 100644
--- a/drivers/net/ethernet/marvell/sky2.h
+++ b/drivers/net/ethernet/marvell/sky2.h
@@ -23,6 +23,7 @@ enum {
 	PSM_CONFIG_REG3  = 0x164,
 	PSM_CONFIG_REG4  = 0x168,
 
+	PCI_LDO_CTRL    = 0xbc,
 };
 
 /* Yukon-2 */
@@ -586,6 +587,10 @@ enum yukon_supr_rev {
 	CHIP_REV_YU_SU_B1    = 3,
 };
 
+enum yukon_prm_rev {
+	CHIP_REV_YU_PRM_Z1   = 1,
+	CHIP_REV_YU_PRM_A0   = 2,
+};
 
 /*	B2_Y2_CLK_GATE	 8 bit	Clock Gating (Yukon-2 only) */
 enum {
diff --git a/drivers/net/ethernet/mellanox/mlx4/catas.c b/drivers/net/ethernet/mellanox/mlx4/catas.c
index 915e947b422..9c656fe4983 100644
--- a/drivers/net/ethernet/mellanox/mlx4/catas.c
+++ b/drivers/net/ethernet/mellanox/mlx4/catas.c
@@ -69,16 +69,21 @@ static void poll_catas(unsigned long dev_ptr)
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
 	if (readl(priv->catas_err.map)) {
-		dump_err_buf(dev);
-
-		mlx4_dispatch_event(dev, MLX4_DEV_EVENT_CATASTROPHIC_ERROR, 0);
+		/* If the device is off-line, we cannot try to recover it */
+		if (pci_channel_offline(dev->pdev))
+			mod_timer(&priv->catas_err.timer,
+				  round_jiffies(jiffies + MLX4_CATAS_POLL_INTERVAL));
+		else {
+			dump_err_buf(dev);
+			mlx4_dispatch_event(dev, MLX4_DEV_EVENT_CATASTROPHIC_ERROR, 0);
 
-		if (internal_err_reset) {
-			spin_lock(&catas_lock);
-			list_add(&priv->catas_err.list, &catas_list);
-			spin_unlock(&catas_lock);
+			if (internal_err_reset) {
+				spin_lock(&catas_lock);
+				list_add(&priv->catas_err.list, &catas_list);
+				spin_unlock(&catas_lock);
 
-			queue_work(mlx4_wq, &catas_work);
+				queue_work(mlx4_wq, &catas_work);
+			}
 		}
 	} else
 		mod_timer(&priv->catas_err.timer,
@@ -100,6 +105,10 @@ static void catas_reset(struct work_struct *work)
 	list_for_each_entry_safe(priv, tmppriv, &tlist, catas_err.list) {
 		struct pci_dev *pdev = priv->dev.pdev;
 
+		/* If the device is off-line, we cannot reset it */
+		if (pci_channel_offline(pdev))
+			continue;
+
 		ret = mlx4_restart_one(priv->dev.pdev);
 		/* 'priv' now is not valid */
 		if (ret)
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index 842c8ce9494..c8fef435302 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -296,7 +296,12 @@ int mlx4_comm_cmd(struct mlx4_dev *dev, u8 cmd, u16 param,
 
 static int cmd_pending(struct mlx4_dev *dev)
 {
-	u32 status = readl(mlx4_priv(dev)->cmd.hcr + HCR_STATUS_OFFSET);
+	u32 status;
+
+	if (pci_channel_offline(dev->pdev))
+		return -EIO;
+
+	status = readl(mlx4_priv(dev)->cmd.hcr + HCR_STATUS_OFFSET);
 
 	return (status & swab32(1 << HCR_GO_BIT)) ||
 		(mlx4_priv(dev)->cmd.toggle ==
@@ -314,11 +319,29 @@ static int mlx4_cmd_post(struct mlx4_dev *dev, u64 in_param, u64 out_param,
 
 	mutex_lock(&cmd->hcr_mutex);
 
+	if (pci_channel_offline(dev->pdev)) {
+		/*
+		 * Device is going through error recovery
+		 * and cannot accept commands.
+		 */
+		ret = -EIO;
+		goto out;
+	}
+
 	end = jiffies;
 	if (event)
 		end += msecs_to_jiffies(GO_BIT_TIMEOUT_MSECS);
 
 	while (cmd_pending(dev)) {
+		if (pci_channel_offline(dev->pdev)) {
+			/*
+			 * Device is going through error recovery
+			 * and cannot accept commands.
+			 */
+			ret = -EIO;
+			goto out;
+		}
+
 		if (time_after_eq(jiffies, end)) {
 			mlx4_err(dev, "%s:cmd_pending failed\n", __func__);
 			goto out;
@@ -431,14 +454,33 @@ static int mlx4_cmd_poll(struct mlx4_dev *dev, u64 in_param, u64 *out_param,
 
 	down(&priv->cmd.poll_sem);
 
+	if (pci_channel_offline(dev->pdev)) {
+		/*
+		 * Device is going through error recovery
+		 * and cannot accept commands.
+		 */
+		err = -EIO;
+		goto out;
+	}
+
 	err = mlx4_cmd_post(dev, in_param, out_param ? *out_param : 0,
 			    in_modifier, op_modifier, op, CMD_POLL_TOKEN, 0);
 	if (err)
 		goto out;
 
 	end = msecs_to_jiffies(timeout) + jiffies;
-	while (cmd_pending(dev) && time_before(jiffies, end))
+	while (cmd_pending(dev) && time_before(jiffies, end)) {
+		if (pci_channel_offline(dev->pdev)) {
+			/*
+			 * Device is going through error recovery
+			 * and cannot accept commands.
+			 */
+			err = -EIO;
+			goto out;
+		}
+
 		cond_resched();
+	}
 
 	if (cmd_pending(dev)) {
 		err = -ETIMEDOUT;
@@ -532,6 +574,9 @@ int __mlx4_cmd(struct mlx4_dev *dev, u64 in_param, u64 *out_param,
 	       int out_is_imm, u32 in_modifier, u8 op_modifier,
 	       u16 op, unsigned long timeout, int native)
 {
+	if (pci_channel_offline(dev->pdev))
+		return -EIO;
+
 	if (!mlx4_is_mfunc(dev) || (native && mlx4_is_master(dev))) {
 		if (mlx4_priv(dev)->cmd.use_events)
 			return mlx4_cmd_wait(dev, in_param, out_param,
@@ -1080,6 +1125,25 @@ static struct mlx4_cmd_info cmd_info[] = {
 		.verify = NULL,
 		.wrapper = NULL
 	},
+	/* flow steering commands */
+	{
+		.opcode = MLX4_QP_FLOW_STEERING_ATTACH,
+		.has_inbox = true,
+		.has_outbox = false,
+		.out_is_imm = true,
+		.encode_slave_id = false,
+		.verify = NULL,
+		.wrapper = mlx4_QP_FLOW_STEERING_ATTACH_wrapper
+	},
+	{
+		.opcode = MLX4_QP_FLOW_STEERING_DETACH,
+		.has_inbox = false,
+		.has_outbox = false,
+		.out_is_imm = false,
+		.encode_slave_id = false,
+		.verify = NULL,
+		.wrapper = mlx4_QP_FLOW_STEERING_DETACH_wrapper
+	},
 };
 
 static int mlx4_master_process_vhcr(struct mlx4_dev *dev, int slave,
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index 908a460d8db..aa9c2f6cf3c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -77,6 +77,12 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	struct mlx4_en_dev *mdev = priv->mdev;
 	int err = 0;
 	char name[25];
+	struct cpu_rmap *rmap =
+#ifdef CONFIG_RFS_ACCEL
+		priv->dev->rx_cpu_rmap;
+#else
+		NULL;
+#endif
 
 	cq->dev = mdev->pndev[priv->port];
 	cq->mcq.set_ci_db  = cq->wqres.db.db;
@@ -91,7 +97,8 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 				sprintf(name, "%s-%d", priv->dev->name,
 					cq->ring);
 				/* Set IRQ for specific name (per ring) */
-				if (mlx4_assign_eq(mdev->dev, name, &cq->vector)) {
+				if (mlx4_assign_eq(mdev->dev, name, rmap,
+						   &cq->vector)) {
 					cq->vector = (cq->ring + 1 + priv->port)
 					    % mdev->dev->caps.num_comp_vectors;
 					mlx4_warn(mdev, "Failed Assigning an EQ to "
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index 72901ce2b08..9d0b88eea02 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -34,10 +34,14 @@
 #include <linux/kernel.h>
 #include <linux/ethtool.h>
 #include <linux/netdevice.h>
+#include <linux/mlx4/driver.h>
 
 #include "mlx4_en.h"
 #include "en_port.h"
 
+#define EN_ETHTOOL_QP_ATTACH (1ull << 63)
+#define EN_ETHTOOL_SHORT_MASK cpu_to_be16(0xffff)
+#define EN_ETHTOOL_WORD_MASK  cpu_to_be32(0xffffffff)
 
 static void
 mlx4_en_get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *drvinfo)
@@ -599,16 +603,369 @@ static int mlx4_en_set_rxfh_indir(struct net_device *dev,
 	return err;
 }
 
+#define all_zeros_or_all_ones(field)		\
+	((field) == 0 || (field) == (__force typeof(field))-1)
+
+static int mlx4_en_validate_flow(struct net_device *dev,
+				 struct ethtool_rxnfc *cmd)
+{
+	struct ethtool_usrip4_spec *l3_mask;
+	struct ethtool_tcpip4_spec *l4_mask;
+	struct ethhdr *eth_mask;
+	u64 full_mac = ~0ull;
+	u64 zero_mac = 0;
+
+	if (cmd->fs.location >= MAX_NUM_OF_FS_RULES)
+		return -EINVAL;
+
+	switch (cmd->fs.flow_type & ~FLOW_EXT) {
+	case TCP_V4_FLOW:
+	case UDP_V4_FLOW:
+		if (cmd->fs.m_u.tcp_ip4_spec.tos)
+			return -EINVAL;
+		l4_mask = &cmd->fs.m_u.tcp_ip4_spec;
+		/* don't allow mask which isn't all 0 or 1 */
+		if (!all_zeros_or_all_ones(l4_mask->ip4src) ||
+		    !all_zeros_or_all_ones(l4_mask->ip4dst) ||
+		    !all_zeros_or_all_ones(l4_mask->psrc) ||
+		    !all_zeros_or_all_ones(l4_mask->pdst))
+			return -EINVAL;
+		break;
+	case IP_USER_FLOW:
+		l3_mask = &cmd->fs.m_u.usr_ip4_spec;
+		if (l3_mask->l4_4_bytes || l3_mask->tos || l3_mask->proto ||
+		    cmd->fs.h_u.usr_ip4_spec.ip_ver != ETH_RX_NFC_IP4 ||
+		    (!l3_mask->ip4src && !l3_mask->ip4dst) ||
+		    !all_zeros_or_all_ones(l3_mask->ip4src) ||
+		    !all_zeros_or_all_ones(l3_mask->ip4dst))
+			return -EINVAL;
+		break;
+	case ETHER_FLOW:
+		eth_mask = &cmd->fs.m_u.ether_spec;
+		/* source mac mask must not be set */
+		if (memcmp(eth_mask->h_source, &zero_mac, ETH_ALEN))
+			return -EINVAL;
+
+		/* dest mac mask must be ff:ff:ff:ff:ff:ff */
+		if (memcmp(eth_mask->h_dest, &full_mac, ETH_ALEN))
+			return -EINVAL;
+
+		if (!all_zeros_or_all_ones(eth_mask->h_proto))
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if ((cmd->fs.flow_type & FLOW_EXT)) {
+		if (cmd->fs.m_ext.vlan_etype ||
+		    !(cmd->fs.m_ext.vlan_tci == 0 ||
+		      cmd->fs.m_ext.vlan_tci == cpu_to_be16(0xfff)))
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int add_ip_rule(struct mlx4_en_priv *priv,
+			struct ethtool_rxnfc *cmd,
+			struct list_head *list_h)
+{
+	struct mlx4_spec_list *spec_l3;
+	struct ethtool_usrip4_spec *l3_mask = &cmd->fs.m_u.usr_ip4_spec;
+
+	spec_l3 = kzalloc(sizeof *spec_l3, GFP_KERNEL);
+	if (!spec_l3) {
+		en_err(priv, "Fail to alloc ethtool rule.\n");
+		return -ENOMEM;
+	}
+
+	spec_l3->id = MLX4_NET_TRANS_RULE_ID_IPV4;
+	spec_l3->ipv4.src_ip = cmd->fs.h_u.usr_ip4_spec.ip4src;
+	if (l3_mask->ip4src)
+		spec_l3->ipv4.src_ip_msk = EN_ETHTOOL_WORD_MASK;
+	spec_l3->ipv4.dst_ip = cmd->fs.h_u.usr_ip4_spec.ip4dst;
+	if (l3_mask->ip4dst)
+		spec_l3->ipv4.dst_ip_msk = EN_ETHTOOL_WORD_MASK;
+	list_add_tail(&spec_l3->list, list_h);
+
+	return 0;
+}
+
+static int add_tcp_udp_rule(struct mlx4_en_priv *priv,
+			     struct ethtool_rxnfc *cmd,
+			     struct list_head *list_h, int proto)
+{
+	struct mlx4_spec_list *spec_l3;
+	struct mlx4_spec_list *spec_l4;
+	struct ethtool_tcpip4_spec *l4_mask = &cmd->fs.m_u.tcp_ip4_spec;
+
+	spec_l3 = kzalloc(sizeof *spec_l3, GFP_KERNEL);
+	spec_l4 = kzalloc(sizeof *spec_l4, GFP_KERNEL);
+	if (!spec_l4 || !spec_l3) {
+		en_err(priv, "Fail to alloc ethtool rule.\n");
+		kfree(spec_l3);
+		kfree(spec_l4);
+		return -ENOMEM;
+	}
+
+	spec_l3->id = MLX4_NET_TRANS_RULE_ID_IPV4;
+
+	if (proto == TCP_V4_FLOW) {
+		spec_l4->id = MLX4_NET_TRANS_RULE_ID_TCP;
+		spec_l3->ipv4.src_ip = cmd->fs.h_u.tcp_ip4_spec.ip4src;
+		spec_l3->ipv4.dst_ip = cmd->fs.h_u.tcp_ip4_spec.ip4dst;
+		spec_l4->tcp_udp.src_port = cmd->fs.h_u.tcp_ip4_spec.psrc;
+		spec_l4->tcp_udp.dst_port = cmd->fs.h_u.tcp_ip4_spec.pdst;
+	} else {
+		spec_l4->id = MLX4_NET_TRANS_RULE_ID_UDP;
+		spec_l3->ipv4.src_ip = cmd->fs.h_u.udp_ip4_spec.ip4src;
+		spec_l3->ipv4.dst_ip = cmd->fs.h_u.udp_ip4_spec.ip4dst;
+		spec_l4->tcp_udp.src_port = cmd->fs.h_u.udp_ip4_spec.psrc;
+		spec_l4->tcp_udp.dst_port = cmd->fs.h_u.udp_ip4_spec.pdst;
+	}
+
+	if (l4_mask->ip4src)
+		spec_l3->ipv4.src_ip_msk = EN_ETHTOOL_WORD_MASK;
+	if (l4_mask->ip4dst)
+		spec_l3->ipv4.dst_ip_msk = EN_ETHTOOL_WORD_MASK;
+
+	if (l4_mask->psrc)
+		spec_l4->tcp_udp.src_port_msk = EN_ETHTOOL_SHORT_MASK;
+	if (l4_mask->pdst)
+		spec_l4->tcp_udp.dst_port_msk = EN_ETHTOOL_SHORT_MASK;
+
+	list_add_tail(&spec_l3->list, list_h);
+	list_add_tail(&spec_l4->list, list_h);
+
+	return 0;
+}
+
+static int mlx4_en_ethtool_to_net_trans_rule(struct net_device *dev,
+					     struct ethtool_rxnfc *cmd,
+					     struct list_head *rule_list_h)
+{
+	int err;
+	u64 mac;
+	__be64 be_mac;
+	struct ethhdr *eth_spec;
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_spec_list *spec_l2;
+	__be64 mac_msk = cpu_to_be64(MLX4_MAC_MASK << 16);
+
+	err = mlx4_en_validate_flow(dev, cmd);
+	if (err)
+		return err;
+
+	spec_l2 = kzalloc(sizeof *spec_l2, GFP_KERNEL);
+	if (!spec_l2)
+		return -ENOMEM;
+
+	mac = priv->mac & MLX4_MAC_MASK;
+	be_mac = cpu_to_be64(mac << 16);
+
+	spec_l2->id = MLX4_NET_TRANS_RULE_ID_ETH;
+	memcpy(spec_l2->eth.dst_mac_msk, &mac_msk, ETH_ALEN);
+	if ((cmd->fs.flow_type & ~FLOW_EXT) != ETHER_FLOW)
+		memcpy(spec_l2->eth.dst_mac, &be_mac, ETH_ALEN);
+
+	if ((cmd->fs.flow_type & FLOW_EXT) && cmd->fs.m_ext.vlan_tci) {
+		spec_l2->eth.vlan_id = cmd->fs.h_ext.vlan_tci;
+		spec_l2->eth.vlan_id_msk = cpu_to_be16(0xfff);
+	}
+
+	list_add_tail(&spec_l2->list, rule_list_h);
+
+	switch (cmd->fs.flow_type & ~FLOW_EXT) {
+	case ETHER_FLOW:
+		eth_spec = &cmd->fs.h_u.ether_spec;
+		memcpy(&spec_l2->eth.dst_mac, eth_spec->h_dest, ETH_ALEN);
+		spec_l2->eth.ether_type = eth_spec->h_proto;
+		if (eth_spec->h_proto)
+			spec_l2->eth.ether_type_enable = 1;
+		break;
+	case IP_USER_FLOW:
+		err = add_ip_rule(priv, cmd, rule_list_h);
+		break;
+	case TCP_V4_FLOW:
+		err = add_tcp_udp_rule(priv, cmd, rule_list_h, TCP_V4_FLOW);
+		break;
+	case UDP_V4_FLOW:
+		err = add_tcp_udp_rule(priv, cmd, rule_list_h, UDP_V4_FLOW);
+		break;
+	}
+
+	return err;
+}
+
+static int mlx4_en_flow_replace(struct net_device *dev,
+				struct ethtool_rxnfc *cmd)
+{
+	int err;
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct ethtool_flow_id *loc_rule;
+	struct mlx4_spec_list *spec, *tmp_spec;
+	u32 qpn;
+	u64 reg_id;
+
+	struct mlx4_net_trans_rule rule = {
+		.queue_mode = MLX4_NET_TRANS_Q_FIFO,
+		.exclusive = 0,
+		.allow_loopback = 1,
+		.promisc_mode = MLX4_FS_PROMISC_NONE,
+	};
+
+	rule.port = priv->port;
+	rule.priority = MLX4_DOMAIN_ETHTOOL | cmd->fs.location;
+	INIT_LIST_HEAD(&rule.list);
+
+	/* Allow direct QP attaches if the EN_ETHTOOL_QP_ATTACH flag is set */
+	if (cmd->fs.ring_cookie == RX_CLS_FLOW_DISC)
+		qpn = priv->drop_qp.qpn;
+	else if (cmd->fs.ring_cookie & EN_ETHTOOL_QP_ATTACH) {
+		qpn = cmd->fs.ring_cookie & (EN_ETHTOOL_QP_ATTACH - 1);
+	} else {
+		if (cmd->fs.ring_cookie >= priv->rx_ring_num) {
+			en_warn(priv, "rxnfc: RX ring (%llu) doesn't exist.\n",
+				cmd->fs.ring_cookie);
+			return -EINVAL;
+		}
+		qpn = priv->rss_map.qps[cmd->fs.ring_cookie].qpn;
+		if (!qpn) {
+			en_warn(priv, "rxnfc: RX ring (%llu) is inactive.\n",
+				cmd->fs.ring_cookie);
+			return -EINVAL;
+		}
+	}
+	rule.qpn = qpn;
+	err = mlx4_en_ethtool_to_net_trans_rule(dev, cmd, &rule.list);
+	if (err)
+		goto out_free_list;
+
+	loc_rule = &priv->ethtool_rules[cmd->fs.location];
+	if (loc_rule->id) {
+		err = mlx4_flow_detach(priv->mdev->dev, loc_rule->id);
+		if (err) {
+			en_err(priv, "Fail to detach network rule at location %d. registration id = %llx\n",
+			       cmd->fs.location, loc_rule->id);
+			goto out_free_list;
+		}
+		loc_rule->id = 0;
+		memset(&loc_rule->flow_spec, 0,
+		       sizeof(struct ethtool_rx_flow_spec));
+	}
+	err = mlx4_flow_attach(priv->mdev->dev, &rule, &reg_id);
+	if (err) {
+		en_err(priv, "Fail to attach network rule at location %d.\n",
+		       cmd->fs.location);
+		goto out_free_list;
+	}
+	loc_rule->id = reg_id;
+	memcpy(&loc_rule->flow_spec, &cmd->fs,
+	       sizeof(struct ethtool_rx_flow_spec));
+
+out_free_list:
+	list_for_each_entry_safe(spec, tmp_spec, &rule.list, list) {
+		list_del(&spec->list);
+		kfree(spec);
+	}
+	return err;
+}
+
+static int mlx4_en_flow_detach(struct net_device *dev,
+			       struct ethtool_rxnfc *cmd)
+{
+	int err = 0;
+	struct ethtool_flow_id *rule;
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+
+	if (cmd->fs.location >= MAX_NUM_OF_FS_RULES)
+		return -EINVAL;
+
+	rule = &priv->ethtool_rules[cmd->fs.location];
+	if (!rule->id) {
+		err =  -ENOENT;
+		goto out;
+	}
+
+	err = mlx4_flow_detach(priv->mdev->dev, rule->id);
+	if (err) {
+		en_err(priv, "Fail to detach network rule at location %d. registration id = 0x%llx\n",
+		       cmd->fs.location, rule->id);
+		goto out;
+	}
+	rule->id = 0;
+	memset(&rule->flow_spec, 0, sizeof(struct ethtool_rx_flow_spec));
+out:
+	return err;
+
+}
+
+static int mlx4_en_get_flow(struct net_device *dev, struct ethtool_rxnfc *cmd,
+			    int loc)
+{
+	int err = 0;
+	struct ethtool_flow_id *rule;
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+
+	if (loc < 0 || loc >= MAX_NUM_OF_FS_RULES)
+		return -EINVAL;
+
+	rule = &priv->ethtool_rules[loc];
+	if (rule->id)
+		memcpy(&cmd->fs, &rule->flow_spec,
+		       sizeof(struct ethtool_rx_flow_spec));
+	else
+		err = -ENOENT;
+
+	return err;
+}
+
+static int mlx4_en_get_num_flows(struct mlx4_en_priv *priv)
+{
+
+	int i, res = 0;
+	for (i = 0; i < MAX_NUM_OF_FS_RULES; i++) {
+		if (priv->ethtool_rules[i].id)
+			res++;
+	}
+	return res;
+
+}
+
 static int mlx4_en_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
 			     u32 *rule_locs)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
 	int err = 0;
+	int i = 0, priority = 0;
+
+	if ((cmd->cmd == ETHTOOL_GRXCLSRLCNT ||
+	     cmd->cmd == ETHTOOL_GRXCLSRULE ||
+	     cmd->cmd == ETHTOOL_GRXCLSRLALL) &&
+	    mdev->dev->caps.steering_mode != MLX4_STEERING_MODE_DEVICE_MANAGED)
+		return -EINVAL;
 
 	switch (cmd->cmd) {
 	case ETHTOOL_GRXRINGS:
 		cmd->data = priv->rx_ring_num;
 		break;
+	case ETHTOOL_GRXCLSRLCNT:
+		cmd->rule_cnt = mlx4_en_get_num_flows(priv);
+		break;
+	case ETHTOOL_GRXCLSRULE:
+		err = mlx4_en_get_flow(dev, cmd, cmd->fs.location);
+		break;
+	case ETHTOOL_GRXCLSRLALL:
+		while ((!err || err == -ENOENT) && priority < cmd->rule_cnt) {
+			err = mlx4_en_get_flow(dev, cmd, i);
+			if (!err)
+				rule_locs[priority++] = i;
+			i++;
+		}
+		err = 0;
+		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -617,6 +974,30 @@ static int mlx4_en_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
 	return err;
 }
 
+static int mlx4_en_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
+{
+	int err = 0;
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = priv->mdev;
+
+	if (mdev->dev->caps.steering_mode != MLX4_STEERING_MODE_DEVICE_MANAGED)
+		return -EINVAL;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXCLSRLINS:
+		err = mlx4_en_flow_replace(dev, cmd);
+		break;
+	case ETHTOOL_SRXCLSRLDEL:
+		err = mlx4_en_flow_detach(dev, cmd);
+		break;
+	default:
+		en_warn(priv, "Unsupported ethtool command. (%d)\n", cmd->cmd);
+		return -EINVAL;
+	}
+
+	return err;
+}
+
 const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_drvinfo = mlx4_en_get_drvinfo,
 	.get_settings = mlx4_en_get_settings,
@@ -637,6 +1018,7 @@ const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_ringparam = mlx4_en_get_ringparam,
 	.set_ringparam = mlx4_en_set_ringparam,
 	.get_rxnfc = mlx4_en_get_rxnfc,
+	.set_rxnfc = mlx4_en_set_rxnfc,
 	.get_rxfh_indir_size = mlx4_en_get_rxfh_indir_size,
 	.get_rxfh_indir = mlx4_en_get_rxfh_indir,
 	.set_rxfh_indir = mlx4_en_set_rxfh_indir,
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_main.c b/drivers/net/ethernet/mellanox/mlx4/en_main.c
index 69ba5727048..a52922ed85c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_main.c
@@ -131,7 +131,7 @@ static void *mlx4_en_get_netdev(struct mlx4_dev *dev, void *ctx, u8 port)
 }
 
 static void mlx4_en_event(struct mlx4_dev *dev, void *endev_ptr,
-			  enum mlx4_dev_event event, int port)
+			  enum mlx4_dev_event event, unsigned long port)
 {
 	struct mlx4_en_dev *mdev = (struct mlx4_en_dev *) endev_ptr;
 	struct mlx4_en_priv *priv;
@@ -156,7 +156,8 @@ static void mlx4_en_event(struct mlx4_dev *dev, void *endev_ptr,
 		if (port < 1 || port > dev->caps.num_ports ||
 		    !mdev->pndev[port])
 			return;
-		mlx4_warn(mdev, "Unhandled event %d for port %d\n", event, port);
+		mlx4_warn(mdev, "Unhandled event %d for port %d\n", event,
+			  (int) port);
 	}
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 073b85b45fc..edd9cb8d3e1 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -36,6 +36,8 @@
 #include <linux/if_vlan.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
+#include <linux/hash.h>
+#include <net/ip.h>
 
 #include <linux/mlx4/driver.h>
 #include <linux/mlx4/device.h>
@@ -66,6 +68,299 @@ static int mlx4_en_setup_tc(struct net_device *dev, u8 up)
 	return 0;
 }
 
+#ifdef CONFIG_RFS_ACCEL
+
+struct mlx4_en_filter {
+	struct list_head next;
+	struct work_struct work;
+
+	__be32 src_ip;
+	__be32 dst_ip;
+	__be16 src_port;
+	__be16 dst_port;
+
+	int rxq_index;
+	struct mlx4_en_priv *priv;
+	u32 flow_id;			/* RFS infrastructure id */
+	int id;				/* mlx4_en driver id */
+	u64 reg_id;			/* Flow steering API id */
+	u8 activated;			/* Used to prevent expiry before filter
+					 * is attached
+					 */
+	struct hlist_node filter_chain;
+};
+
+static void mlx4_en_filter_rfs_expire(struct mlx4_en_priv *priv);
+
+static void mlx4_en_filter_work(struct work_struct *work)
+{
+	struct mlx4_en_filter *filter = container_of(work,
+						     struct mlx4_en_filter,
+						     work);
+	struct mlx4_en_priv *priv = filter->priv;
+	struct mlx4_spec_list spec_tcp = {
+		.id = MLX4_NET_TRANS_RULE_ID_TCP,
+		{
+			.tcp_udp = {
+				.dst_port = filter->dst_port,
+				.dst_port_msk = (__force __be16)-1,
+				.src_port = filter->src_port,
+				.src_port_msk = (__force __be16)-1,
+			},
+		},
+	};
+	struct mlx4_spec_list spec_ip = {
+		.id = MLX4_NET_TRANS_RULE_ID_IPV4,
+		{
+			.ipv4 = {
+				.dst_ip = filter->dst_ip,
+				.dst_ip_msk = (__force __be32)-1,
+				.src_ip = filter->src_ip,
+				.src_ip_msk = (__force __be32)-1,
+			},
+		},
+	};
+	struct mlx4_spec_list spec_eth = {
+		.id = MLX4_NET_TRANS_RULE_ID_ETH,
+	};
+	struct mlx4_net_trans_rule rule = {
+		.list = LIST_HEAD_INIT(rule.list),
+		.queue_mode = MLX4_NET_TRANS_Q_LIFO,
+		.exclusive = 1,
+		.allow_loopback = 1,
+		.promisc_mode = MLX4_FS_PROMISC_NONE,
+		.port = priv->port,
+		.priority = MLX4_DOMAIN_RFS,
+	};
+	int rc;
+	__be64 mac;
+	__be64 mac_mask = cpu_to_be64(MLX4_MAC_MASK << 16);
+
+	list_add_tail(&spec_eth.list, &rule.list);
+	list_add_tail(&spec_ip.list, &rule.list);
+	list_add_tail(&spec_tcp.list, &rule.list);
+
+	mac = cpu_to_be64((priv->mac & MLX4_MAC_MASK) << 16);
+
+	rule.qpn = priv->rss_map.qps[filter->rxq_index].qpn;
+	memcpy(spec_eth.eth.dst_mac, &mac, ETH_ALEN);
+	memcpy(spec_eth.eth.dst_mac_msk, &mac_mask, ETH_ALEN);
+
+	filter->activated = 0;
+
+	if (filter->reg_id) {
+		rc = mlx4_flow_detach(priv->mdev->dev, filter->reg_id);
+		if (rc && rc != -ENOENT)
+			en_err(priv, "Error detaching flow. rc = %d\n", rc);
+	}
+
+	rc = mlx4_flow_attach(priv->mdev->dev, &rule, &filter->reg_id);
+	if (rc)
+		en_err(priv, "Error attaching flow. err = %d\n", rc);
+
+	mlx4_en_filter_rfs_expire(priv);
+
+	filter->activated = 1;
+}
+
+static inline struct hlist_head *
+filter_hash_bucket(struct mlx4_en_priv *priv, __be32 src_ip, __be32 dst_ip,
+		   __be16 src_port, __be16 dst_port)
+{
+	unsigned long l;
+	int bucket_idx;
+
+	l = (__force unsigned long)src_port |
+	    ((__force unsigned long)dst_port << 2);
+	l ^= (__force unsigned long)(src_ip ^ dst_ip);
+
+	bucket_idx = hash_long(l, MLX4_EN_FILTER_HASH_SHIFT);
+
+	return &priv->filter_hash[bucket_idx];
+}
+
+static struct mlx4_en_filter *
+mlx4_en_filter_alloc(struct mlx4_en_priv *priv, int rxq_index, __be32 src_ip,
+		     __be32 dst_ip, __be16 src_port, __be16 dst_port,
+		     u32 flow_id)
+{
+	struct mlx4_en_filter *filter = NULL;
+
+	filter = kzalloc(sizeof(struct mlx4_en_filter), GFP_ATOMIC);
+	if (!filter)
+		return NULL;
+
+	filter->priv = priv;
+	filter->rxq_index = rxq_index;
+	INIT_WORK(&filter->work, mlx4_en_filter_work);
+
+	filter->src_ip = src_ip;
+	filter->dst_ip = dst_ip;
+	filter->src_port = src_port;
+	filter->dst_port = dst_port;
+
+	filter->flow_id = flow_id;
+
+	filter->id = priv->last_filter_id++ % RPS_NO_FILTER;
+
+	list_add_tail(&filter->next, &priv->filters);
+	hlist_add_head(&filter->filter_chain,
+		       filter_hash_bucket(priv, src_ip, dst_ip, src_port,
+					  dst_port));
+
+	return filter;
+}
+
+static void mlx4_en_filter_free(struct mlx4_en_filter *filter)
+{
+	struct mlx4_en_priv *priv = filter->priv;
+	int rc;
+
+	list_del(&filter->next);
+
+	rc = mlx4_flow_detach(priv->mdev->dev, filter->reg_id);
+	if (rc && rc != -ENOENT)
+		en_err(priv, "Error detaching flow. rc = %d\n", rc);
+
+	kfree(filter);
+}
+
+static inline struct mlx4_en_filter *
+mlx4_en_filter_find(struct mlx4_en_priv *priv, __be32 src_ip, __be32 dst_ip,
+		    __be16 src_port, __be16 dst_port)
+{
+	struct hlist_node *elem;
+	struct mlx4_en_filter *filter;
+	struct mlx4_en_filter *ret = NULL;
+
+	hlist_for_each_entry(filter, elem,
+			     filter_hash_bucket(priv, src_ip, dst_ip,
+						src_port, dst_port),
+			     filter_chain) {
+		if (filter->src_ip == src_ip &&
+		    filter->dst_ip == dst_ip &&
+		    filter->src_port == src_port &&
+		    filter->dst_port == dst_port) {
+			ret = filter;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static int
+mlx4_en_filter_rfs(struct net_device *net_dev, const struct sk_buff *skb,
+		   u16 rxq_index, u32 flow_id)
+{
+	struct mlx4_en_priv *priv = netdev_priv(net_dev);
+	struct mlx4_en_filter *filter;
+	const struct iphdr *ip;
+	const __be16 *ports;
+	__be32 src_ip;
+	__be32 dst_ip;
+	__be16 src_port;
+	__be16 dst_port;
+	int nhoff = skb_network_offset(skb);
+	int ret = 0;
+
+	if (skb->protocol != htons(ETH_P_IP))
+		return -EPROTONOSUPPORT;
+
+	ip = (const struct iphdr *)(skb->data + nhoff);
+	if (ip_is_fragment(ip))
+		return -EPROTONOSUPPORT;
+
+	ports = (const __be16 *)(skb->data + nhoff + 4 * ip->ihl);
+
+	src_ip = ip->saddr;
+	dst_ip = ip->daddr;
+	src_port = ports[0];
+	dst_port = ports[1];
+
+	if (ip->protocol != IPPROTO_TCP)
+		return -EPROTONOSUPPORT;
+
+	spin_lock_bh(&priv->filters_lock);
+	filter = mlx4_en_filter_find(priv, src_ip, dst_ip, src_port, dst_port);
+	if (filter) {
+		if (filter->rxq_index == rxq_index)
+			goto out;
+
+		filter->rxq_index = rxq_index;
+	} else {
+		filter = mlx4_en_filter_alloc(priv, rxq_index,
+					      src_ip, dst_ip,
+					      src_port, dst_port, flow_id);
+		if (!filter) {
+			ret = -ENOMEM;
+			goto err;
+		}
+	}
+
+	queue_work(priv->mdev->workqueue, &filter->work);
+
+out:
+	ret = filter->id;
+err:
+	spin_unlock_bh(&priv->filters_lock);
+
+	return ret;
+}
+
+void mlx4_en_cleanup_filters(struct mlx4_en_priv *priv,
+			     struct mlx4_en_rx_ring *rx_ring)
+{
+	struct mlx4_en_filter *filter, *tmp;
+	LIST_HEAD(del_list);
+
+	spin_lock_bh(&priv->filters_lock);
+	list_for_each_entry_safe(filter, tmp, &priv->filters, next) {
+		list_move(&filter->next, &del_list);
+		hlist_del(&filter->filter_chain);
+	}
+	spin_unlock_bh(&priv->filters_lock);
+
+	list_for_each_entry_safe(filter, tmp, &del_list, next) {
+		cancel_work_sync(&filter->work);
+		mlx4_en_filter_free(filter);
+	}
+}
+
+static void mlx4_en_filter_rfs_expire(struct mlx4_en_priv *priv)
+{
+	struct mlx4_en_filter *filter = NULL, *tmp, *last_filter = NULL;
+	LIST_HEAD(del_list);
+	int i = 0;
+
+	spin_lock_bh(&priv->filters_lock);
+	list_for_each_entry_safe(filter, tmp, &priv->filters, next) {
+		if (i > MLX4_EN_FILTER_EXPIRY_QUOTA)
+			break;
+
+		if (filter->activated &&
+		    !work_pending(&filter->work) &&
+		    rps_may_expire_flow(priv->dev,
+					filter->rxq_index, filter->flow_id,
+					filter->id)) {
+			list_move(&filter->next, &del_list);
+			hlist_del(&filter->filter_chain);
+		} else
+			last_filter = filter;
+
+		i++;
+	}
+
+	if (last_filter && (&last_filter->next != priv->filters.next))
+		list_move(&priv->filters, &last_filter->next);
+
+	spin_unlock_bh(&priv->filters_lock);
+
+	list_for_each_entry_safe(filter, tmp, &del_list, next)
+		mlx4_en_filter_free(filter);
+}
+#endif
+
 static int mlx4_en_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
@@ -170,33 +465,81 @@ static void mlx4_en_do_set_mac(struct work_struct *work)
 static void mlx4_en_clear_list(struct net_device *dev)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
+	struct mlx4_en_mc_list *tmp, *mc_to_del;
 
-	kfree(priv->mc_addrs);
-	priv->mc_addrs = NULL;
-	priv->mc_addrs_cnt = 0;
+	list_for_each_entry_safe(mc_to_del, tmp, &priv->mc_list, list) {
+		list_del(&mc_to_del->list);
+		kfree(mc_to_del);
+	}
 }
 
 static void mlx4_en_cache_mclist(struct net_device *dev)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct netdev_hw_addr *ha;
-	char *mc_addrs;
-	int mc_addrs_cnt = netdev_mc_count(dev);
-	int i;
+	struct mlx4_en_mc_list *tmp;
 
-	mc_addrs = kmalloc(mc_addrs_cnt * ETH_ALEN, GFP_ATOMIC);
-	if (!mc_addrs) {
-		en_err(priv, "failed to allocate multicast list\n");
-		return;
-	}
-	i = 0;
-	netdev_for_each_mc_addr(ha, dev)
-		memcpy(mc_addrs + i++ * ETH_ALEN, ha->addr, ETH_ALEN);
 	mlx4_en_clear_list(dev);
-	priv->mc_addrs = mc_addrs;
-	priv->mc_addrs_cnt = mc_addrs_cnt;
+	netdev_for_each_mc_addr(ha, dev) {
+		tmp = kzalloc(sizeof(struct mlx4_en_mc_list), GFP_ATOMIC);
+		if (!tmp) {
+			en_err(priv, "failed to allocate multicast list\n");
+			mlx4_en_clear_list(dev);
+			return;
+		}
+		memcpy(tmp->addr, ha->addr, ETH_ALEN);
+		list_add_tail(&tmp->list, &priv->mc_list);
+	}
 }
 
+static void update_mclist_flags(struct mlx4_en_priv *priv,
+				struct list_head *dst,
+				struct list_head *src)
+{
+	struct mlx4_en_mc_list *dst_tmp, *src_tmp, *new_mc;
+	bool found;
+
+	/* Find all the entries that should be removed from dst,
+	 * These are the entries that are not found in src
+	 */
+	list_for_each_entry(dst_tmp, dst, list) {
+		found = false;
+		list_for_each_entry(src_tmp, src, list) {
+			if (!memcmp(dst_tmp->addr, src_tmp->addr, ETH_ALEN)) {
+				found = true;
+				break;
+			}
+		}
+		if (!found)
+			dst_tmp->action = MCLIST_REM;
+	}
+
+	/* Add entries that exist in src but not in dst
+	 * mark them as need to add
+	 */
+	list_for_each_entry(src_tmp, src, list) {
+		found = false;
+		list_for_each_entry(dst_tmp, dst, list) {
+			if (!memcmp(dst_tmp->addr, src_tmp->addr, ETH_ALEN)) {
+				dst_tmp->action = MCLIST_NONE;
+				found = true;
+				break;
+			}
+		}
+		if (!found) {
+			new_mc = kmalloc(sizeof(struct mlx4_en_mc_list),
+					 GFP_KERNEL);
+			if (!new_mc) {
+				en_err(priv, "Failed to allocate current multicast list\n");
+				return;
+			}
+			memcpy(new_mc, src_tmp,
+			       sizeof(struct mlx4_en_mc_list));
+			new_mc->action = MCLIST_ADD;
+			list_add_tail(&new_mc->list, dst);
+		}
+	}
+}
 
 static void mlx4_en_set_multicast(struct net_device *dev)
 {
@@ -214,9 +557,10 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 						 mcast_task);
 	struct mlx4_en_dev *mdev = priv->mdev;
 	struct net_device *dev = priv->dev;
+	struct mlx4_en_mc_list *mclist, *tmp;
 	u64 mcast_addr = 0;
 	u8 mc_list[16] = {0};
-	int err;
+	int err = 0;
 
 	mutex_lock(&mdev->state_lock);
 	if (!mdev->device_up) {
@@ -251,16 +595,46 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 			priv->flags |= MLX4_EN_FLAG_PROMISC;
 
 			/* Enable promiscouos mode */
-			if (!(mdev->dev->caps.flags &
-						MLX4_DEV_CAP_FLAG_VEP_UC_STEER))
-				err = mlx4_SET_PORT_qpn_calc(mdev->dev, priv->port,
-							     priv->base_qpn, 1);
-			else
-				err = mlx4_unicast_promisc_add(mdev->dev, priv->base_qpn,
+			switch (mdev->dev->caps.steering_mode) {
+			case MLX4_STEERING_MODE_DEVICE_MANAGED:
+				err = mlx4_flow_steer_promisc_add(mdev->dev,
+								  priv->port,
+								  priv->base_qpn,
+								  MLX4_FS_PROMISC_UPLINK);
+				if (err)
+					en_err(priv, "Failed enabling promiscuous mode\n");
+				priv->flags |= MLX4_EN_FLAG_MC_PROMISC;
+				break;
+
+			case MLX4_STEERING_MODE_B0:
+				err = mlx4_unicast_promisc_add(mdev->dev,
+							       priv->base_qpn,
 							       priv->port);
-			if (err)
-				en_err(priv, "Failed enabling "
-					     "promiscuous mode\n");
+				if (err)
+					en_err(priv, "Failed enabling unicast promiscuous mode\n");
+
+				/* Add the default qp number as multicast
+				 * promisc
+				 */
+				if (!(priv->flags & MLX4_EN_FLAG_MC_PROMISC)) {
+					err = mlx4_multicast_promisc_add(mdev->dev,
+									 priv->base_qpn,
+									 priv->port);
+					if (err)
+						en_err(priv, "Failed enabling multicast promiscuous mode\n");
+					priv->flags |= MLX4_EN_FLAG_MC_PROMISC;
+				}
+				break;
+
+			case MLX4_STEERING_MODE_A0:
+				err = mlx4_SET_PORT_qpn_calc(mdev->dev,
+							     priv->port,
+							     priv->base_qpn,
+							     1);
+				if (err)
+					en_err(priv, "Failed enabling promiscuous mode\n");
+				break;
+			}
 
 			/* Disable port multicast filter (unconditionally) */
 			err = mlx4_SET_MCAST_FLTR(mdev->dev, priv->port, 0,
@@ -269,15 +643,6 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 				en_err(priv, "Failed disabling "
 					     "multicast filter\n");
 
-			/* Add the default qp number as multicast promisc */
-			if (!(priv->flags & MLX4_EN_FLAG_MC_PROMISC)) {
-				err = mlx4_multicast_promisc_add(mdev->dev, priv->base_qpn,
-								 priv->port);
-				if (err)
-					en_err(priv, "Failed entering multicast promisc mode\n");
-				priv->flags |= MLX4_EN_FLAG_MC_PROMISC;
-			}
-
 			/* Disable port VLAN filter */
 			err = mlx4_SET_VLAN_FLTR(mdev->dev, priv);
 			if (err)
@@ -296,22 +661,40 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 		priv->flags &= ~MLX4_EN_FLAG_PROMISC;
 
 		/* Disable promiscouos mode */
-		if (!(mdev->dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER))
-			err = mlx4_SET_PORT_qpn_calc(mdev->dev, priv->port,
-						     priv->base_qpn, 0);
-		else
-			err = mlx4_unicast_promisc_remove(mdev->dev, priv->base_qpn,
+		switch (mdev->dev->caps.steering_mode) {
+		case MLX4_STEERING_MODE_DEVICE_MANAGED:
+			err = mlx4_flow_steer_promisc_remove(mdev->dev,
+							     priv->port,
+							     MLX4_FS_PROMISC_UPLINK);
+			if (err)
+				en_err(priv, "Failed disabling promiscuous mode\n");
+			priv->flags &= ~MLX4_EN_FLAG_MC_PROMISC;
+			break;
+
+		case MLX4_STEERING_MODE_B0:
+			err = mlx4_unicast_promisc_remove(mdev->dev,
+							  priv->base_qpn,
 							  priv->port);
-		if (err)
-			en_err(priv, "Failed disabling promiscuous mode\n");
+			if (err)
+				en_err(priv, "Failed disabling unicast promiscuous mode\n");
+			/* Disable Multicast promisc */
+			if (priv->flags & MLX4_EN_FLAG_MC_PROMISC) {
+				err = mlx4_multicast_promisc_remove(mdev->dev,
+								    priv->base_qpn,
+								    priv->port);
+				if (err)
+					en_err(priv, "Failed disabling multicast promiscuous mode\n");
+				priv->flags &= ~MLX4_EN_FLAG_MC_PROMISC;
+			}
+			break;
 
-		/* Disable Multicast promisc */
-		if (priv->flags & MLX4_EN_FLAG_MC_PROMISC) {
-			err = mlx4_multicast_promisc_remove(mdev->dev, priv->base_qpn,
-							    priv->port);
+		case MLX4_STEERING_MODE_A0:
+			err = mlx4_SET_PORT_qpn_calc(mdev->dev,
+						     priv->port,
+						     priv->base_qpn, 0);
 			if (err)
-				en_err(priv, "Failed disabling multicast promiscuous mode\n");
-			priv->flags &= ~MLX4_EN_FLAG_MC_PROMISC;
+				en_err(priv, "Failed disabling promiscuous mode\n");
+			break;
 		}
 
 		/* Enable port VLAN filter */
@@ -329,18 +712,46 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 
 		/* Add the default qp number as multicast promisc */
 		if (!(priv->flags & MLX4_EN_FLAG_MC_PROMISC)) {
-			err = mlx4_multicast_promisc_add(mdev->dev, priv->base_qpn,
-							 priv->port);
+			switch (mdev->dev->caps.steering_mode) {
+			case MLX4_STEERING_MODE_DEVICE_MANAGED:
+				err = mlx4_flow_steer_promisc_add(mdev->dev,
+								  priv->port,
+								  priv->base_qpn,
+								  MLX4_FS_PROMISC_ALL_MULTI);
+				break;
+
+			case MLX4_STEERING_MODE_B0:
+				err = mlx4_multicast_promisc_add(mdev->dev,
+								 priv->base_qpn,
+								 priv->port);
+				break;
+
+			case MLX4_STEERING_MODE_A0:
+				break;
+			}
 			if (err)
 				en_err(priv, "Failed entering multicast promisc mode\n");
 			priv->flags |= MLX4_EN_FLAG_MC_PROMISC;
 		}
 	} else {
-		int i;
 		/* Disable Multicast promisc */
 		if (priv->flags & MLX4_EN_FLAG_MC_PROMISC) {
-			err = mlx4_multicast_promisc_remove(mdev->dev, priv->base_qpn,
-							    priv->port);
+			switch (mdev->dev->caps.steering_mode) {
+			case MLX4_STEERING_MODE_DEVICE_MANAGED:
+				err = mlx4_flow_steer_promisc_remove(mdev->dev,
+								     priv->port,
+								     MLX4_FS_PROMISC_ALL_MULTI);
+				break;
+
+			case MLX4_STEERING_MODE_B0:
+				err = mlx4_multicast_promisc_remove(mdev->dev,
+								    priv->base_qpn,
+								    priv->port);
+				break;
+
+			case MLX4_STEERING_MODE_A0:
+				break;
+			}
 			if (err)
 				en_err(priv, "Failed disabling multicast promiscuous mode\n");
 			priv->flags &= ~MLX4_EN_FLAG_MC_PROMISC;
@@ -351,13 +762,6 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 		if (err)
 			en_err(priv, "Failed disabling multicast filter\n");
 
-		/* Detach our qp from all the multicast addresses */
-		for (i = 0; i < priv->mc_addrs_cnt; i++) {
-			memcpy(&mc_list[10], priv->mc_addrs + i * ETH_ALEN, ETH_ALEN);
-			mc_list[5] = priv->port;
-			mlx4_multicast_detach(mdev->dev, &priv->rss_map.indir_qp,
-					      mc_list, MLX4_PROT_ETH);
-		}
 		/* Flush mcast filter and init it with broadcast address */
 		mlx4_SET_MCAST_FLTR(mdev->dev, priv->port, ETH_BCAST,
 				    1, MLX4_MCAST_CONFIG);
@@ -367,13 +771,8 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 		netif_tx_lock_bh(dev);
 		mlx4_en_cache_mclist(dev);
 		netif_tx_unlock_bh(dev);
-		for (i = 0; i < priv->mc_addrs_cnt; i++) {
-			mcast_addr =
-			      mlx4_en_mac_to_u64(priv->mc_addrs + i * ETH_ALEN);
-			memcpy(&mc_list[10], priv->mc_addrs + i * ETH_ALEN, ETH_ALEN);
-			mc_list[5] = priv->port;
-			mlx4_multicast_attach(mdev->dev, &priv->rss_map.indir_qp,
-					      mc_list, 0, MLX4_PROT_ETH);
+		list_for_each_entry(mclist, &priv->mc_list, list) {
+			mcast_addr = mlx4_en_mac_to_u64(mclist->addr);
 			mlx4_SET_MCAST_FLTR(mdev->dev, priv->port,
 					    mcast_addr, 0, MLX4_MCAST_CONFIG);
 		}
@@ -381,6 +780,40 @@ static void mlx4_en_do_set_multicast(struct work_struct *work)
 					  0, MLX4_MCAST_ENABLE);
 		if (err)
 			en_err(priv, "Failed enabling multicast filter\n");
+
+		update_mclist_flags(priv, &priv->curr_list, &priv->mc_list);
+		list_for_each_entry_safe(mclist, tmp, &priv->curr_list, list) {
+			if (mclist->action == MCLIST_REM) {
+				/* detach this address and delete from list */
+				memcpy(&mc_list[10], mclist->addr, ETH_ALEN);
+				mc_list[5] = priv->port;
+				err = mlx4_multicast_detach(mdev->dev,
+							    &priv->rss_map.indir_qp,
+							    mc_list,
+							    MLX4_PROT_ETH,
+							    mclist->reg_id);
+				if (err)
+					en_err(priv, "Fail to detach multicast address\n");
+
+				/* remove from list */
+				list_del(&mclist->list);
+				kfree(mclist);
+			} else if (mclist->action == MCLIST_ADD) {
+				/* attach the address */
+				memcpy(&mc_list[10], mclist->addr, ETH_ALEN);
+				/* needed for B0 steering support */
+				mc_list[5] = priv->port;
+				err = mlx4_multicast_attach(mdev->dev,
+							    &priv->rss_map.indir_qp,
+							    mc_list,
+							    priv->port, 0,
+							    MLX4_PROT_ETH,
+							    &mclist->reg_id);
+				if (err)
+					en_err(priv, "Fail to attach multicast address\n");
+
+			}
+		}
 	}
 out:
 	mutex_unlock(&mdev->state_lock);
@@ -605,6 +1038,9 @@ int mlx4_en_start_port(struct net_device *dev)
 		return 0;
 	}
 
+	INIT_LIST_HEAD(&priv->mc_list);
+	INIT_LIST_HEAD(&priv->curr_list);
+
 	/* Calculate Rx buf size */
 	dev->mtu = min(dev->mtu, priv->max_mtu);
 	mlx4_en_calc_rx_buf(dev);
@@ -653,6 +1089,10 @@ int mlx4_en_start_port(struct net_device *dev)
 		goto mac_err;
 	}
 
+	err = mlx4_en_create_drop_qp(priv);
+	if (err)
+		goto rss_err;
+
 	/* Configure tx cq's and rings */
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		/* Configure cq */
@@ -720,13 +1160,23 @@ int mlx4_en_start_port(struct net_device *dev)
 
 	/* Attach rx QP to bradcast address */
 	memset(&mc_list[10], 0xff, ETH_ALEN);
-	mc_list[5] = priv->port;
+	mc_list[5] = priv->port; /* needed for B0 steering support */
 	if (mlx4_multicast_attach(mdev->dev, &priv->rss_map.indir_qp, mc_list,
-				  0, MLX4_PROT_ETH))
+				  priv->port, 0, MLX4_PROT_ETH,
+				  &priv->broadcast_id))
 		mlx4_warn(mdev, "Failed Attaching Broadcast\n");
 
 	/* Must redo promiscuous mode setup. */
 	priv->flags &= ~(MLX4_EN_FLAG_PROMISC | MLX4_EN_FLAG_MC_PROMISC);
+	if (mdev->dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		mlx4_flow_steer_promisc_remove(mdev->dev,
+					       priv->port,
+					       MLX4_FS_PROMISC_UPLINK);
+		mlx4_flow_steer_promisc_remove(mdev->dev,
+					       priv->port,
+					       MLX4_FS_PROMISC_ALL_MULTI);
+	}
 
 	/* Schedule multicast task to populate multicast list */
 	queue_work(mdev->workqueue, &priv->mcast_task);
@@ -742,7 +1192,8 @@ tx_err:
 		mlx4_en_deactivate_tx_ring(priv, &priv->tx_ring[tx_index]);
 		mlx4_en_deactivate_cq(priv, &priv->tx_cq[tx_index]);
 	}
-
+	mlx4_en_destroy_drop_qp(priv);
+rss_err:
 	mlx4_en_release_rss_steer(priv);
 mac_err:
 	mlx4_put_eth_qp(mdev->dev, priv->port, priv->mac, priv->base_qpn);
@@ -760,6 +1211,7 @@ void mlx4_en_stop_port(struct net_device *dev)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_en_dev *mdev = priv->mdev;
+	struct mlx4_en_mc_list *mclist, *tmp;
 	int i;
 	u8 mc_list[16] = {0};
 
@@ -778,19 +1230,26 @@ void mlx4_en_stop_port(struct net_device *dev)
 
 	/* Detach All multicasts */
 	memset(&mc_list[10], 0xff, ETH_ALEN);
-	mc_list[5] = priv->port;
+	mc_list[5] = priv->port; /* needed for B0 steering support */
 	mlx4_multicast_detach(mdev->dev, &priv->rss_map.indir_qp, mc_list,
-			      MLX4_PROT_ETH);
-	for (i = 0; i < priv->mc_addrs_cnt; i++) {
-		memcpy(&mc_list[10], priv->mc_addrs + i * ETH_ALEN, ETH_ALEN);
+			      MLX4_PROT_ETH, priv->broadcast_id);
+	list_for_each_entry(mclist, &priv->curr_list, list) {
+		memcpy(&mc_list[10], mclist->addr, ETH_ALEN);
 		mc_list[5] = priv->port;
 		mlx4_multicast_detach(mdev->dev, &priv->rss_map.indir_qp,
-				      mc_list, MLX4_PROT_ETH);
+				      mc_list, MLX4_PROT_ETH, mclist->reg_id);
 	}
 	mlx4_en_clear_list(dev);
+	list_for_each_entry_safe(mclist, tmp, &priv->curr_list, list) {
+		list_del(&mclist->list);
+		kfree(mclist);
+	}
+
 	/* Flush multicast filter */
 	mlx4_SET_MCAST_FLTR(mdev->dev, priv->port, 0, 1, MLX4_MCAST_CONFIG);
 
+	mlx4_en_destroy_drop_qp(priv);
+
 	/* Free TX Rings */
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		mlx4_en_deactivate_tx_ring(priv, &priv->tx_ring[i]);
@@ -915,6 +1374,11 @@ void mlx4_en_free_resources(struct mlx4_en_priv *priv)
 {
 	int i;
 
+#ifdef CONFIG_RFS_ACCEL
+	free_irq_cpu_rmap(priv->dev->rx_cpu_rmap);
+	priv->dev->rx_cpu_rmap = NULL;
+#endif
+
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		if (priv->tx_ring[i].tx_info)
 			mlx4_en_destroy_tx_ring(priv, &priv->tx_ring[i]);
@@ -970,6 +1434,15 @@ int mlx4_en_alloc_resources(struct mlx4_en_priv *priv)
 			goto err;
 	}
 
+#ifdef CONFIG_RFS_ACCEL
+	priv->dev->rx_cpu_rmap = alloc_irq_cpu_rmap(priv->rx_ring_num);
+	if (!priv->dev->rx_cpu_rmap)
+		goto err;
+
+	INIT_LIST_HEAD(&priv->filters);
+	spin_lock_init(&priv->filters_lock);
+#endif
+
 	return 0;
 
 err:
@@ -1077,6 +1550,9 @@ static const struct net_device_ops mlx4_netdev_ops = {
 #endif
 	.ndo_set_features	= mlx4_en_set_features,
 	.ndo_setup_tc		= mlx4_en_setup_tc,
+#ifdef CONFIG_RFS_ACCEL
+	.ndo_rx_flow_steer	= mlx4_en_filter_rfs,
+#endif
 };
 
 int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
@@ -1194,6 +1670,10 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 			NETIF_F_HW_VLAN_FILTER;
 	dev->hw_features |= NETIF_F_LOOPBACK;
 
+	if (mdev->dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED)
+		dev->hw_features |= NETIF_F_NTUPLE;
+
 	mdev->pndev[port] = dev;
 
 	netif_carrier_off(dev);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index d49a7ac3187..f32e7030077 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -41,41 +41,75 @@
 
 #include "mlx4_en.h"
 
-
-static int mlx4_en_alloc_frag(struct mlx4_en_priv *priv,
-			      struct mlx4_en_rx_desc *rx_desc,
-			      struct page_frag *skb_frags,
-			      struct mlx4_en_rx_alloc *ring_alloc,
-			      int i)
+static int mlx4_en_alloc_frags(struct mlx4_en_priv *priv,
+			       struct mlx4_en_rx_desc *rx_desc,
+			       struct mlx4_en_rx_alloc *frags,
+			       struct mlx4_en_rx_alloc *ring_alloc)
 {
-	struct mlx4_en_frag_info *frag_info = &priv->frag_info[i];
-	struct mlx4_en_rx_alloc *page_alloc = &ring_alloc[i];
+	struct mlx4_en_rx_alloc page_alloc[MLX4_EN_MAX_RX_FRAGS];
+	struct mlx4_en_frag_info *frag_info;
 	struct page *page;
 	dma_addr_t dma;
+	int i;
 
-	if (page_alloc->offset == frag_info->last_offset) {
-		/* Allocate new page */
-		page = alloc_pages(GFP_ATOMIC | __GFP_COMP, MLX4_EN_ALLOC_ORDER);
-		if (!page)
-			return -ENOMEM;
-
-		skb_frags[i].page = page_alloc->page;
-		skb_frags[i].offset = page_alloc->offset;
-		page_alloc->page = page;
-		page_alloc->offset = frag_info->frag_align;
-	} else {
-		page = page_alloc->page;
-		get_page(page);
+	for (i = 0; i < priv->num_frags; i++) {
+		frag_info = &priv->frag_info[i];
+		if (ring_alloc[i].offset == frag_info->last_offset) {
+			page = alloc_pages(GFP_ATOMIC | __GFP_COMP,
+					MLX4_EN_ALLOC_ORDER);
+			if (!page)
+				goto out;
+			dma = dma_map_page(priv->ddev, page, 0,
+				MLX4_EN_ALLOC_SIZE, PCI_DMA_FROMDEVICE);
+			if (dma_mapping_error(priv->ddev, dma)) {
+				put_page(page);
+				goto out;
+			}
+			page_alloc[i].page = page;
+			page_alloc[i].dma = dma;
+			page_alloc[i].offset = frag_info->frag_align;
+		} else {
+			page_alloc[i].page = ring_alloc[i].page;
+			get_page(ring_alloc[i].page);
+			page_alloc[i].dma = ring_alloc[i].dma;
+			page_alloc[i].offset = ring_alloc[i].offset +
+						frag_info->frag_stride;
+		}
+	}
 
-		skb_frags[i].page = page;
-		skb_frags[i].offset = page_alloc->offset;
-		page_alloc->offset += frag_info->frag_stride;
+	for (i = 0; i < priv->num_frags; i++) {
+		frags[i] = ring_alloc[i];
+		dma = ring_alloc[i].dma + ring_alloc[i].offset;
+		ring_alloc[i] = page_alloc[i];
+		rx_desc->data[i].addr = cpu_to_be64(dma);
 	}
-	dma = dma_map_single(priv->ddev, page_address(skb_frags[i].page) +
-			     skb_frags[i].offset, frag_info->frag_size,
-			     PCI_DMA_FROMDEVICE);
-	rx_desc->data[i].addr = cpu_to_be64(dma);
+
 	return 0;
+
+
+out:
+	while (i--) {
+		frag_info = &priv->frag_info[i];
+		if (ring_alloc[i].offset == frag_info->last_offset)
+			dma_unmap_page(priv->ddev, page_alloc[i].dma,
+				MLX4_EN_ALLOC_SIZE, PCI_DMA_FROMDEVICE);
+		put_page(page_alloc[i].page);
+	}
+	return -ENOMEM;
+}
+
+static void mlx4_en_free_frag(struct mlx4_en_priv *priv,
+			      struct mlx4_en_rx_alloc *frags,
+			      int i)
+{
+	struct mlx4_en_frag_info *frag_info = &priv->frag_info[i];
+
+	if (frags[i].offset == frag_info->last_offset) {
+		dma_unmap_page(priv->ddev, frags[i].dma, MLX4_EN_ALLOC_SIZE,
+					 PCI_DMA_FROMDEVICE);
+	}
+	if (frags[i].page)
+		put_page(frags[i].page);
 }
 
 static int mlx4_en_init_allocator(struct mlx4_en_priv *priv,
@@ -91,6 +125,13 @@ static int mlx4_en_init_allocator(struct mlx4_en_priv *priv,
 		if (!page_alloc->page)
 			goto out;
 
+		page_alloc->dma = dma_map_page(priv->ddev, page_alloc->page, 0,
+					MLX4_EN_ALLOC_SIZE, PCI_DMA_FROMDEVICE);
+		if (dma_mapping_error(priv->ddev, page_alloc->dma)) {
+			put_page(page_alloc->page);
+			page_alloc->page = NULL;
+			goto out;
+		}
 		page_alloc->offset = priv->frag_info[i].frag_align;
 		en_dbg(DRV, priv, "Initialized allocator:%d with page:%p\n",
 		       i, page_alloc->page);
@@ -100,6 +141,8 @@ static int mlx4_en_init_allocator(struct mlx4_en_priv *priv,
 out:
 	while (i--) {
 		page_alloc = &ring->page_alloc[i];
+		dma_unmap_page(priv->ddev, page_alloc->dma,
+				MLX4_EN_ALLOC_SIZE, PCI_DMA_FROMDEVICE);
 		put_page(page_alloc->page);
 		page_alloc->page = NULL;
 	}
@@ -117,24 +160,22 @@ static void mlx4_en_destroy_allocator(struct mlx4_en_priv *priv,
 		en_dbg(DRV, priv, "Freeing allocator:%d count:%d\n",
 		       i, page_count(page_alloc->page));
 
+		dma_unmap_page(priv->ddev, page_alloc->dma,
+				MLX4_EN_ALLOC_SIZE, PCI_DMA_FROMDEVICE);
 		put_page(page_alloc->page);
 		page_alloc->page = NULL;
 	}
 }
 
-
 static void mlx4_en_init_rx_desc(struct mlx4_en_priv *priv,
 				 struct mlx4_en_rx_ring *ring, int index)
 {
 	struct mlx4_en_rx_desc *rx_desc = ring->buf + ring->stride * index;
-	struct skb_frag_struct *skb_frags = ring->rx_info +
-					    (index << priv->log_rx_info);
 	int possible_frags;
 	int i;
 
 	/* Set size and memtype fields */
 	for (i = 0; i < priv->num_frags; i++) {
-		skb_frag_size_set(&skb_frags[i], priv->frag_info[i].frag_size);
 		rx_desc->data[i].byte_count =
 			cpu_to_be32(priv->frag_info[i].frag_size);
 		rx_desc->data[i].lkey = cpu_to_be32(priv->mdev->mr.key);
@@ -151,29 +192,14 @@ static void mlx4_en_init_rx_desc(struct mlx4_en_priv *priv,
 	}
 }
 
-
 static int mlx4_en_prepare_rx_desc(struct mlx4_en_priv *priv,
 				   struct mlx4_en_rx_ring *ring, int index)
 {
 	struct mlx4_en_rx_desc *rx_desc = ring->buf + (index * ring->stride);
-	struct page_frag *skb_frags = ring->rx_info +
-				      (index << priv->log_rx_info);
-	int i;
+	struct mlx4_en_rx_alloc *frags = ring->rx_info +
+					(index << priv->log_rx_info);
 
-	for (i = 0; i < priv->num_frags; i++)
-		if (mlx4_en_alloc_frag(priv, rx_desc, skb_frags, ring->page_alloc, i))
-			goto err;
-
-	return 0;
-
-err:
-	while (i--) {
-		dma_addr_t dma = be64_to_cpu(rx_desc->data[i].addr);
-		pci_unmap_single(priv->mdev->pdev, dma, skb_frags[i].size,
-				 PCI_DMA_FROMDEVICE);
-		put_page(skb_frags[i].page);
-	}
-	return -ENOMEM;
+	return mlx4_en_alloc_frags(priv, rx_desc, frags, ring->page_alloc);
 }
 
 static inline void mlx4_en_update_rx_prod_db(struct mlx4_en_rx_ring *ring)
@@ -185,20 +211,13 @@ static void mlx4_en_free_rx_desc(struct mlx4_en_priv *priv,
 				 struct mlx4_en_rx_ring *ring,
 				 int index)
 {
-	struct page_frag *skb_frags;
-	struct mlx4_en_rx_desc *rx_desc = ring->buf + (index << ring->log_stride);
-	dma_addr_t dma;
+	struct mlx4_en_rx_alloc *frags;
 	int nr;
 
-	skb_frags = ring->rx_info + (index << priv->log_rx_info);
+	frags = ring->rx_info + (index << priv->log_rx_info);
 	for (nr = 0; nr < priv->num_frags; nr++) {
 		en_dbg(DRV, priv, "Freeing fragment:%d\n", nr);
-		dma = be64_to_cpu(rx_desc->data[nr].addr);
-
-		en_dbg(DRV, priv, "Unmapping buffer at dma:0x%llx\n", (u64) dma);
-		dma_unmap_single(priv->ddev, dma, skb_frags[nr].size,
-				 PCI_DMA_FROMDEVICE);
-		put_page(skb_frags[nr].page);
+		mlx4_en_free_frag(priv, frags, nr);
 	}
 }
 
@@ -268,10 +287,9 @@ int mlx4_en_create_rx_ring(struct mlx4_en_priv *priv,
 			   struct mlx4_en_rx_ring *ring, u32 size, u16 stride)
 {
 	struct mlx4_en_dev *mdev = priv->mdev;
-	int err;
+	int err = -ENOMEM;
 	int tmp;
 
-
 	ring->prod = 0;
 	ring->cons = 0;
 	ring->size = size;
@@ -281,7 +299,7 @@ int mlx4_en_create_rx_ring(struct mlx4_en_priv *priv,
 	ring->buf_size = ring->size * ring->stride + TXBB_SIZE;
 
 	tmp = size * roundup_pow_of_two(MLX4_EN_MAX_RX_FRAGS *
-					sizeof(struct skb_frag_struct));
+					sizeof(struct mlx4_en_rx_alloc));
 	ring->rx_info = vmalloc(tmp);
 	if (!ring->rx_info)
 		return -ENOMEM;
@@ -338,7 +356,7 @@ int mlx4_en_activate_rx_rings(struct mlx4_en_priv *priv)
 		memset(ring->buf, 0, ring->buf_size);
 		mlx4_en_update_rx_prod_db(ring);
 
-		/* Initailize all descriptors */
+		/* Initialize all descriptors */
 		for (i = 0; i < ring->size; i++)
 			mlx4_en_init_rx_desc(priv, ring, i);
 
@@ -389,6 +407,9 @@ void mlx4_en_destroy_rx_ring(struct mlx4_en_priv *priv,
 	mlx4_free_hwq_res(mdev->dev, &ring->wqres, size * stride + TXBB_SIZE);
 	vfree(ring->rx_info);
 	ring->rx_info = NULL;
+#ifdef CONFIG_RFS_ACCEL
+	mlx4_en_cleanup_filters(priv, ring);
+#endif
 }
 
 void mlx4_en_deactivate_rx_ring(struct mlx4_en_priv *priv,
@@ -401,12 +422,10 @@ void mlx4_en_deactivate_rx_ring(struct mlx4_en_priv *priv,
 }
 
 
-/* Unmap a completed descriptor and free unused pages */
 static int mlx4_en_complete_rx_desc(struct mlx4_en_priv *priv,
 				    struct mlx4_en_rx_desc *rx_desc,
-				    struct page_frag *skb_frags,
+				    struct mlx4_en_rx_alloc *frags,
 				    struct sk_buff *skb,
-				    struct mlx4_en_rx_alloc *page_alloc,
 				    int length)
 {
 	struct skb_frag_struct *skb_frags_rx = skb_shinfo(skb)->frags;
@@ -414,26 +433,24 @@ static int mlx4_en_complete_rx_desc(struct mlx4_en_priv *priv,
 	int nr;
 	dma_addr_t dma;
 
-	/* Collect used fragments while replacing them in the HW descirptors */
+	/* Collect used fragments while replacing them in the HW descriptors */
 	for (nr = 0; nr < priv->num_frags; nr++) {
 		frag_info = &priv->frag_info[nr];
 		if (length <= frag_info->frag_prefix_size)
 			break;
+		if (!frags[nr].page)
+			goto fail;
 
-		/* Save page reference in skb */
-		__skb_frag_set_page(&skb_frags_rx[nr], skb_frags[nr].page);
-		skb_frag_size_set(&skb_frags_rx[nr], skb_frags[nr].size);
-		skb_frags_rx[nr].page_offset = skb_frags[nr].offset;
-		skb->truesize += frag_info->frag_stride;
 		dma = be64_to_cpu(rx_desc->data[nr].addr);
+		dma_sync_single_for_cpu(priv->ddev, dma, frag_info->frag_size,
+					DMA_FROM_DEVICE);
 
-		/* Allocate a replacement page */
-		if (mlx4_en_alloc_frag(priv, rx_desc, skb_frags, page_alloc, nr))
-			goto fail;
-
-		/* Unmap buffer */
-		dma_unmap_single(priv->ddev, dma, skb_frag_size(&skb_frags_rx[nr]),
-				 PCI_DMA_FROMDEVICE);
+		/* Save page reference in skb */
+		get_page(frags[nr].page);
+		__skb_frag_set_page(&skb_frags_rx[nr], frags[nr].page);
+		skb_frag_size_set(&skb_frags_rx[nr], frag_info->frag_size);
+		skb_frags_rx[nr].page_offset = frags[nr].offset;
+		skb->truesize += frag_info->frag_stride;
 	}
 	/* Adjust size of last fragment to match actual length */
 	if (nr > 0)
@@ -442,8 +459,6 @@ static int mlx4_en_complete_rx_desc(struct mlx4_en_priv *priv,
 	return nr;
 
 fail:
-	/* Drop all accumulated fragments (which have already been replaced in
-	 * the descriptor) of this packet; remaining fragments are reused... */
 	while (nr > 0) {
 		nr--;
 		__skb_frag_unref(&skb_frags_rx[nr]);
@@ -454,8 +469,7 @@ fail:
 
 static struct sk_buff *mlx4_en_rx_skb(struct mlx4_en_priv *priv,
 				      struct mlx4_en_rx_desc *rx_desc,
-				      struct page_frag *skb_frags,
-				      struct mlx4_en_rx_alloc *page_alloc,
+				      struct mlx4_en_rx_alloc *frags,
 				      unsigned int length)
 {
 	struct sk_buff *skb;
@@ -473,23 +487,20 @@ static struct sk_buff *mlx4_en_rx_skb(struct mlx4_en_priv *priv,
 
 	/* Get pointer to first fragment so we could copy the headers into the
 	 * (linear part of the) skb */
-	va = page_address(skb_frags[0].page) + skb_frags[0].offset;
+	va = page_address(frags[0].page) + frags[0].offset;
 
 	if (length <= SMALL_PACKET_SIZE) {
 		/* We are copying all relevant data to the skb - temporarily
-		 * synch buffers for the copy */
+		 * sync buffers for the copy */
 		dma = be64_to_cpu(rx_desc->data[0].addr);
 		dma_sync_single_for_cpu(priv->ddev, dma, length,
 					DMA_FROM_DEVICE);
 		skb_copy_to_linear_data(skb, va, length);
-		dma_sync_single_for_device(priv->ddev, dma, length,
-					   DMA_FROM_DEVICE);
 		skb->tail += length;
 	} else {
-
 		/* Move relevant fragments to skb */
-		used_frags = mlx4_en_complete_rx_desc(priv, rx_desc, skb_frags,
-						      skb, page_alloc, length);
+		used_frags = mlx4_en_complete_rx_desc(priv, rx_desc, frags,
+							skb, length);
 		if (unlikely(!used_frags)) {
 			kfree_skb(skb);
 			return NULL;
@@ -526,12 +537,25 @@ out_loopback:
 	dev_kfree_skb_any(skb);
 }
 
+static void mlx4_en_refill_rx_buffers(struct mlx4_en_priv *priv,
+				     struct mlx4_en_rx_ring *ring)
+{
+	int index = ring->prod & ring->size_mask;
+
+	while ((u32) (ring->prod - ring->cons) < ring->actual_size) {
+		if (mlx4_en_prepare_rx_desc(priv, ring, index))
+			break;
+		ring->prod++;
+		index = ring->prod & ring->size_mask;
+	}
+}
+
 int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int budget)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_cqe *cqe;
 	struct mlx4_en_rx_ring *ring = &priv->rx_ring[cq->ring];
-	struct page_frag *skb_frags;
+	struct mlx4_en_rx_alloc *frags;
 	struct mlx4_en_rx_desc *rx_desc;
 	struct sk_buff *skb;
 	int index;
@@ -540,6 +564,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	int polled = 0;
 	int ip_summed;
 	struct ethhdr *ethh;
+	dma_addr_t dma;
 	u64 s_mac;
 
 	if (!priv->port_up)
@@ -555,7 +580,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	while (XNOR(cqe->owner_sr_opcode & MLX4_CQE_OWNER_MASK,
 		    cq->mcq.cons_index & cq->size)) {
 
-		skb_frags = ring->rx_info + (index << priv->log_rx_info);
+		frags = ring->rx_info + (index << priv->log_rx_info);
 		rx_desc = ring->buf + (index << ring->log_stride);
 
 		/*
@@ -579,8 +604,11 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 
 		/* Get pointer to first fragment since we haven't skb yet and
 		 * cast it to ethhdr struct */
-		ethh = (struct ethhdr *)(page_address(skb_frags[0].page) +
-					 skb_frags[0].offset);
+		dma = be64_to_cpu(rx_desc->data[0].addr);
+		dma_sync_single_for_cpu(priv->ddev, dma, sizeof(*ethh),
+					DMA_FROM_DEVICE);
+		ethh = (struct ethhdr *)(page_address(frags[0].page) +
+					 frags[0].offset);
 		s_mac = mlx4_en_mac_to_u64(ethh->h_source);
 
 		/* If source MAC is equal to our own MAC and not performing
@@ -612,10 +640,9 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 					if (!gro_skb)
 						goto next;
 
-					nr = mlx4_en_complete_rx_desc(
-						priv, rx_desc,
-						skb_frags, gro_skb,
-						ring->page_alloc, length);
+					nr = mlx4_en_complete_rx_desc(priv,
+						rx_desc, frags, gro_skb,
+						length);
 					if (!nr)
 						goto next;
 
@@ -651,8 +678,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 			ring->csum_none++;
 		}
 
-		skb = mlx4_en_rx_skb(priv, rx_desc, skb_frags,
-				     ring->page_alloc, length);
+		skb = mlx4_en_rx_skb(priv, rx_desc, frags, length);
 		if (!skb) {
 			priv->stats.rx_dropped++;
 			goto next;
@@ -678,6 +704,9 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 		netif_receive_skb(skb);
 
 next:
+		for (nr = 0; nr < priv->num_frags; nr++)
+			mlx4_en_free_frag(priv, frags, nr);
+
 		++cq->mcq.cons_index;
 		index = (cq->mcq.cons_index) & ring->size_mask;
 		cqe = &cq->buf[index];
@@ -693,7 +722,7 @@ out:
 	mlx4_cq_set_ci(&cq->mcq);
 	wmb(); /* ensure HW sees CQ consumer before we post new buffers */
 	ring->cons = cq->mcq.cons_index;
-	ring->prod += polled; /* Polled descriptors were realocated in place */
+	mlx4_en_refill_rx_buffers(priv, ring);
 	mlx4_en_update_rx_prod_db(ring);
 	return polled;
 }
@@ -782,7 +811,7 @@ void mlx4_en_calc_rx_buf(struct net_device *dev)
 
 	priv->num_frags = i;
 	priv->rx_skb_size = eff_mtu;
-	priv->log_rx_info = ROUNDUP_LOG2(i * sizeof(struct skb_frag_struct));
+	priv->log_rx_info = ROUNDUP_LOG2(i * sizeof(struct mlx4_en_rx_alloc));
 
 	en_dbg(DRV, priv, "Rx buffer scatter-list (effective-mtu:%d "
 		  "num_frags:%d):\n", eff_mtu, priv->num_frags);
@@ -844,6 +873,36 @@ out:
 	return err;
 }
 
+int mlx4_en_create_drop_qp(struct mlx4_en_priv *priv)
+{
+	int err;
+	u32 qpn;
+
+	err = mlx4_qp_reserve_range(priv->mdev->dev, 1, 1, &qpn);
+	if (err) {
+		en_err(priv, "Failed reserving drop qpn\n");
+		return err;
+	}
+	err = mlx4_qp_alloc(priv->mdev->dev, qpn, &priv->drop_qp);
+	if (err) {
+		en_err(priv, "Failed allocating drop qp\n");
+		mlx4_qp_release_range(priv->mdev->dev, qpn, 1);
+		return err;
+	}
+
+	return 0;
+}
+
+void mlx4_en_destroy_drop_qp(struct mlx4_en_priv *priv)
+{
+	u32 qpn;
+
+	qpn = priv->drop_qp.qpn;
+	mlx4_qp_remove(priv->mdev->dev, &priv->drop_qp);
+	mlx4_qp_free(priv->mdev->dev, &priv->drop_qp);
+	mlx4_qp_release_range(priv->mdev->dev, qpn, 1);
+}
+
 /* Allocate rx qp's and configure them according to rss map */
 int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv)
 {
@@ -954,8 +1013,3 @@ void mlx4_en_release_rss_steer(struct mlx4_en_priv *priv)
 	}
 	mlx4_qp_release_range(mdev->dev, rss_map->base_qpn, priv->rx_ring_num);
 }
-
-
-
-
-
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index bce98d9c003..99a04648fab 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -39,6 +39,7 @@
 #include <linux/dma-mapping.h>
 
 #include <linux/mlx4/cmd.h>
+#include <linux/cpu_rmap.h>
 
 #include "mlx4.h"
 #include "fw.h"
@@ -82,6 +83,15 @@ enum {
 			       (1ull << MLX4_EVENT_TYPE_FLR_EVENT)	    | \
 			       (1ull << MLX4_EVENT_TYPE_FATAL_WARNING))
 
+static u64 get_async_ev_mask(struct mlx4_dev *dev)
+{
+	u64 async_ev_mask = MLX4_ASYNC_EVENT_MASK;
+	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_PORT_MNG_CHG_EV)
+		async_ev_mask |= (1ull << MLX4_EVENT_TYPE_PORT_MNG_CHG_EVENT);
+
+	return async_ev_mask;
+}
+
 static void eq_set_ci(struct mlx4_eq *eq, int req_not)
 {
 	__raw_writel((__force u32) cpu_to_be32((eq->cons_index & 0xffffff) |
@@ -473,6 +483,11 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 
 			break;
 
+		case MLX4_EVENT_TYPE_PORT_MNG_CHG_EVENT:
+			mlx4_dispatch_event(dev, MLX4_DEV_EVENT_PORT_MGMT_CHANGE,
+					    (unsigned long) eqe);
+			break;
+
 		case MLX4_EVENT_TYPE_EEC_CATAS_ERROR:
 		case MLX4_EVENT_TYPE_ECC_DETECT:
 		default:
@@ -956,7 +971,7 @@ int mlx4_init_eq_table(struct mlx4_dev *dev)
 		priv->eq_table.have_irq = 1;
 	}
 
-	err = mlx4_MAP_EQ(dev, MLX4_ASYNC_EVENT_MASK, 0,
+	err = mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 0,
 			  priv->eq_table.eq[dev->caps.num_comp_vectors].eqn);
 	if (err)
 		mlx4_warn(dev, "MAP_EQ for async EQ %d failed (%d)\n",
@@ -996,7 +1011,7 @@ void mlx4_cleanup_eq_table(struct mlx4_dev *dev)
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int i;
 
-	mlx4_MAP_EQ(dev, MLX4_ASYNC_EVENT_MASK, 1,
+	mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 1,
 		    priv->eq_table.eq[dev->caps.num_comp_vectors].eqn);
 
 	mlx4_free_irqs(dev);
@@ -1040,7 +1055,7 @@ int mlx4_test_interrupts(struct mlx4_dev *dev)
 		mlx4_cmd_use_polling(dev);
 
 		/* Map the new eq to handle all asyncronous events */
-		err = mlx4_MAP_EQ(dev, MLX4_ASYNC_EVENT_MASK, 0,
+		err = mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 0,
 				  priv->eq_table.eq[i].eqn);
 		if (err) {
 			mlx4_warn(dev, "Failed mapping eq for interrupt test\n");
@@ -1054,13 +1069,14 @@ int mlx4_test_interrupts(struct mlx4_dev *dev)
 	}
 
 	/* Return to default */
-	mlx4_MAP_EQ(dev, MLX4_ASYNC_EVENT_MASK, 0,
+	mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 0,
 		    priv->eq_table.eq[dev->caps.num_comp_vectors].eqn);
 	return err;
 }
 EXPORT_SYMBOL(mlx4_test_interrupts);
 
-int mlx4_assign_eq(struct mlx4_dev *dev, char* name, int * vector)
+int mlx4_assign_eq(struct mlx4_dev *dev, char *name, struct cpu_rmap *rmap,
+		   int *vector)
 {
 
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -1074,6 +1090,14 @@ int mlx4_assign_eq(struct mlx4_dev *dev, char* name, int * vector)
 			snprintf(priv->eq_table.irq_names +
 					vec * MLX4_IRQNAME_SIZE,
 					MLX4_IRQNAME_SIZE, "%s", name);
+#ifdef CONFIG_RFS_ACCEL
+			if (rmap) {
+				err = irq_cpu_rmap_add(rmap,
+						       priv->eq_table.eq[vec].irq);
+				if (err)
+					mlx4_warn(dev, "Failed adding irq rmap\n");
+			}
+#endif
 			err = request_irq(priv->eq_table.eq[vec].irq,
 					  mlx4_msi_x_interrupt, 0,
 					  &priv->eq_table.irq_names[vec<<5],
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index 9c83bb8151e..c6964848732 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -109,6 +109,7 @@ static void dump_dev_cap_flags(struct mlx4_dev *dev, u64 flags)
 		[41] = "Unicast VEP steering support",
 		[42] = "Multicast VEP steering support",
 		[48] = "Counters support",
+		[59] = "Port management change event support",
 	};
 	int i;
 
@@ -123,7 +124,8 @@ static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
 	static const char * const fname[] = {
 		[0] = "RSS support",
 		[1] = "RSS Toeplitz Hash Function support",
-		[2] = "RSS XOR Hash Function support"
+		[2] = "RSS XOR Hash Function support",
+		[3] = "Device manage flow steering support"
 	};
 	int i;
 
@@ -173,6 +175,7 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 #define QUERY_FUNC_CAP_FLAGS_OFFSET		0x0
 #define QUERY_FUNC_CAP_NUM_PORTS_OFFSET		0x1
 #define QUERY_FUNC_CAP_PF_BHVR_OFFSET		0x4
+#define QUERY_FUNC_CAP_FMR_OFFSET		0x8
 #define QUERY_FUNC_CAP_QP_QUOTA_OFFSET		0x10
 #define QUERY_FUNC_CAP_CQ_QUOTA_OFFSET		0x14
 #define QUERY_FUNC_CAP_SRQ_QUOTA_OFFSET		0x18
@@ -182,25 +185,44 @@ int mlx4_QUERY_FUNC_CAP_wrapper(struct mlx4_dev *dev, int slave,
 #define QUERY_FUNC_CAP_MAX_EQ_OFFSET		0x2c
 #define QUERY_FUNC_CAP_RESERVED_EQ_OFFSET	0X30
 
+#define QUERY_FUNC_CAP_FMR_FLAG			0x80
+#define QUERY_FUNC_CAP_FLAG_RDMA		0x40
+#define QUERY_FUNC_CAP_FLAG_ETH			0x80
+
+/* when opcode modifier = 1 */
 #define QUERY_FUNC_CAP_PHYS_PORT_OFFSET		0x3
+#define QUERY_FUNC_CAP_RDMA_PROPS_OFFSET	0x8
 #define QUERY_FUNC_CAP_ETH_PROPS_OFFSET		0xc
 
+#define QUERY_FUNC_CAP_ETH_PROPS_FORCE_MAC	0x40
+#define QUERY_FUNC_CAP_ETH_PROPS_FORCE_VLAN	0x80
+
+#define QUERY_FUNC_CAP_RDMA_PROPS_FORCE_PHY_WQE_GID 0x80
+
 	if (vhcr->op_modifier == 1) {
 		field = vhcr->in_modifier;
 		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_PHYS_PORT_OFFSET);
 
-		field = 0; /* ensure fvl bit is not set */
+		field = 0;
+		/* ensure force vlan and force mac bits are not set */
 		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_ETH_PROPS_OFFSET);
+		/* ensure that phy_wqe_gid bit is not set */
+		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_RDMA_PROPS_OFFSET);
+
 	} else if (vhcr->op_modifier == 0) {
-		field = 1 << 7; /* enable only ethernet interface */
+		/* enable rdma and ethernet interfaces */
+		field = (QUERY_FUNC_CAP_FLAG_ETH | QUERY_FUNC_CAP_FLAG_RDMA);
 		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_FLAGS_OFFSET);
 
 		field = dev->caps.num_ports;
 		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_NUM_PORTS_OFFSET);
 
-		size = 0; /* no PF behavious is set for now */
+		size = 0; /* no PF behaviour is set for now */
 		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_PF_BHVR_OFFSET);
 
+		field = 0; /* protected FMR support not available as yet */
+		MLX4_PUT(outbox->buf, field, QUERY_FUNC_CAP_FMR_OFFSET);
+
 		size = dev->caps.num_qps;
 		MLX4_PUT(outbox->buf, size, QUERY_FUNC_CAP_QP_QUOTA_OFFSET);
 
@@ -253,11 +275,12 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, struct mlx4_func_cap *func_cap)
 	outbox = mailbox->buf;
 
 	MLX4_GET(field, outbox, QUERY_FUNC_CAP_FLAGS_OFFSET);
-	if (!(field & (1 << 7))) {
-		mlx4_err(dev, "The host doesn't support eth interface\n");
+	if (!(field & (QUERY_FUNC_CAP_FLAG_ETH | QUERY_FUNC_CAP_FLAG_RDMA))) {
+		mlx4_err(dev, "The host supports neither eth nor rdma interfaces\n");
 		err = -EPROTONOSUPPORT;
 		goto out;
 	}
+	func_cap->flags = field;
 
 	MLX4_GET(field, outbox, QUERY_FUNC_CAP_NUM_PORTS_OFFSET);
 	func_cap->num_ports = field;
@@ -296,17 +319,27 @@ int mlx4_QUERY_FUNC_CAP(struct mlx4_dev *dev, struct mlx4_func_cap *func_cap)
 		if (err)
 			goto out;
 
-		MLX4_GET(field, outbox, QUERY_FUNC_CAP_ETH_PROPS_OFFSET);
-		if (field & (1 << 7)) {
-			mlx4_err(dev, "VLAN is enforced on this port\n");
-			err = -EPROTONOSUPPORT;
-			goto out;
-		}
+		if (dev->caps.port_type[i] == MLX4_PORT_TYPE_ETH) {
+			MLX4_GET(field, outbox, QUERY_FUNC_CAP_ETH_PROPS_OFFSET);
+			if (field & QUERY_FUNC_CAP_ETH_PROPS_FORCE_VLAN) {
+				mlx4_err(dev, "VLAN is enforced on this port\n");
+				err = -EPROTONOSUPPORT;
+				goto out;
+			}
 
-		if (field & (1 << 6)) {
-			mlx4_err(dev, "Force mac is enabled on this port\n");
-			err = -EPROTONOSUPPORT;
-			goto out;
+			if (field & QUERY_FUNC_CAP_ETH_PROPS_FORCE_MAC) {
+				mlx4_err(dev, "Force mac is enabled on this port\n");
+				err = -EPROTONOSUPPORT;
+				goto out;
+			}
+		} else if (dev->caps.port_type[i] == MLX4_PORT_TYPE_IB) {
+			MLX4_GET(field, outbox, QUERY_FUNC_CAP_RDMA_PROPS_OFFSET);
+			if (field & QUERY_FUNC_CAP_RDMA_PROPS_FORCE_PHY_WQE_GID) {
+				mlx4_err(dev, "phy_wqe_gid is "
+					 "enforced on this ib port\n");
+				err = -EPROTONOSUPPORT;
+				goto out;
+			}
 		}
 
 		MLX4_GET(field, outbox, QUERY_FUNC_CAP_PHYS_PORT_OFFSET);
@@ -391,6 +424,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_RSVD_XRC_OFFSET		0x66
 #define QUERY_DEV_CAP_MAX_XRC_OFFSET		0x67
 #define QUERY_DEV_CAP_MAX_COUNTERS_OFFSET	0x68
+#define QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET	0x76
+#define QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET	0x77
 #define QUERY_DEV_CAP_RDMARC_ENTRY_SZ_OFFSET	0x80
 #define QUERY_DEV_CAP_QPC_ENTRY_SZ_OFFSET	0x82
 #define QUERY_DEV_CAP_AUX_ENTRY_SZ_OFFSET	0x84
@@ -474,6 +509,12 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev_cap->num_ports = field & 0xf;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_MSG_SZ_OFFSET);
 	dev_cap->max_msg_sz = 1 << (field & 0x1f);
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET);
+	if (field & 0x80)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_FS_EN;
+	dev_cap->fs_log_max_ucast_qp_range_size = field & 0x1f;
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET);
+	dev_cap->fs_max_num_qp_per_entry = field;
 	MLX4_GET(stat_rate, outbox, QUERY_DEV_CAP_RATE_SUPPORT_OFFSET);
 	dev_cap->stat_rate_support = stat_rate;
 	MLX4_GET(ext_flags, outbox, QUERY_DEV_CAP_EXT_FLAGS_OFFSET);
@@ -698,14 +739,12 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 {
 	u64 def_mac;
 	u8 port_type;
+	u16 short_field;
 	int err;
 
-#define MLX4_PORT_SUPPORT_IB		(1 << 0)
-#define MLX4_PORT_SUGGEST_TYPE		(1 << 3)
-#define MLX4_PORT_DEFAULT_SENSE		(1 << 4)
-#define MLX4_VF_PORT_ETH_ONLY_MASK	(0xff & ~MLX4_PORT_SUPPORT_IB & \
-					 ~MLX4_PORT_SUGGEST_TYPE & \
-					 ~MLX4_PORT_DEFAULT_SENSE)
+#define MLX4_VF_PORT_NO_LINK_SENSE_MASK	0xE0
+#define QUERY_PORT_CUR_MAX_PKEY_OFFSET	0x0c
+#define QUERY_PORT_CUR_MAX_GID_OFFSET	0x0e
 
 	err = mlx4_cmd_box(dev, 0, outbox->dma, vhcr->in_modifier, 0,
 			   MLX4_CMD_QUERY_PORT, MLX4_CMD_TIME_CLASS_B,
@@ -721,20 +760,58 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 		MLX4_GET(port_type, outbox->buf,
 			 QUERY_PORT_SUPPORTED_TYPE_OFFSET);
 
-		/* Allow only Eth port, no link sensing allowed */
-		port_type &= MLX4_VF_PORT_ETH_ONLY_MASK;
-
-		/* check eth is enabled for this port */
-		if (!(port_type & 2))
-			mlx4_dbg(dev, "QUERY PORT: eth not supported by host");
+		/* No link sensing allowed */
+		port_type &= MLX4_VF_PORT_NO_LINK_SENSE_MASK;
+		/* set port type to currently operating port type */
+		port_type |= (dev->caps.port_type[vhcr->in_modifier] & 0x3);
 
 		MLX4_PUT(outbox->buf, port_type,
 			 QUERY_PORT_SUPPORTED_TYPE_OFFSET);
+
+		short_field = 1; /* slave max gids */
+		MLX4_PUT(outbox->buf, short_field,
+			 QUERY_PORT_CUR_MAX_GID_OFFSET);
+
+		short_field = dev->caps.pkey_table_len[vhcr->in_modifier];
+		MLX4_PUT(outbox->buf, short_field,
+			 QUERY_PORT_CUR_MAX_PKEY_OFFSET);
 	}
 
 	return err;
 }
 
+int mlx4_get_slave_pkey_gid_tbl_len(struct mlx4_dev *dev, u8 port,
+				    int *gid_tbl_len, int *pkey_tbl_len)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	u32			*outbox;
+	u16			field;
+	int			err;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	err =  mlx4_cmd_box(dev, 0, mailbox->dma, port, 0,
+			    MLX4_CMD_QUERY_PORT, MLX4_CMD_TIME_CLASS_B,
+			    MLX4_CMD_WRAPPED);
+	if (err)
+		goto out;
+
+	outbox = mailbox->buf;
+
+	MLX4_GET(field, outbox, QUERY_PORT_CUR_MAX_GID_OFFSET);
+	*gid_tbl_len = field;
+
+	MLX4_GET(field, outbox, QUERY_PORT_CUR_MAX_PKEY_OFFSET);
+	*pkey_tbl_len = field;
+
+out:
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_get_slave_pkey_gid_tbl_len);
+
 int mlx4_map_cmd(struct mlx4_dev *dev, u16 op, struct mlx4_icm *icm, u64 virt)
 {
 	struct mlx4_cmd_mailbox *mailbox;
@@ -881,11 +958,12 @@ int mlx4_QUERY_FW(struct mlx4_dev *dev)
 		((fw_ver & 0xffff0000ull) >> 16) |
 		((fw_ver & 0x0000ffffull) << 16);
 
+	MLX4_GET(lg, outbox, QUERY_FW_PPF_ID);
+	dev->caps.function = lg;
+
 	if (mlx4_is_slave(dev))
 		goto out;
 
-	MLX4_GET(lg, outbox, QUERY_FW_PPF_ID);
-	dev->caps.function = lg;
 
 	MLX4_GET(cmd_if_rev, outbox, QUERY_FW_CMD_IF_REV_OFFSET);
 	if (cmd_if_rev < MLX4_COMMAND_INTERFACE_MIN_REV ||
@@ -966,9 +1044,12 @@ int mlx4_QUERY_FW_wrapper(struct mlx4_dev *dev, int slave,
 	if (err)
 		return err;
 
-	/* for slaves, zero out everything except FW version */
+	/* for slaves, set pci PPF ID to invalid and zero out everything
+	 * else except FW version */
 	outbuf[0] = outbuf[1] = 0;
 	memset(&outbuf[8], 0, QUERY_FW_OUT_SIZE - 8);
+	outbuf[QUERY_FW_PPF_ID] = MLX4_INVALID_SLAVE_ID;
+
 	return 0;
 }
 
@@ -1061,6 +1142,15 @@ int mlx4_INIT_HCA(struct mlx4_dev *dev, struct mlx4_init_hca_param *param)
 #define	 INIT_HCA_LOG_MC_HASH_SZ_OFFSET	 (INIT_HCA_MCAST_OFFSET + 0x16)
 #define  INIT_HCA_UC_STEERING_OFFSET	 (INIT_HCA_MCAST_OFFSET + 0x18)
 #define	 INIT_HCA_LOG_MC_TABLE_SZ_OFFSET (INIT_HCA_MCAST_OFFSET + 0x1b)
+#define  INIT_HCA_DEVICE_MANAGED_FLOW_STEERING_EN	0x6
+#define  INIT_HCA_FS_PARAM_OFFSET         0x1d0
+#define  INIT_HCA_FS_BASE_OFFSET          (INIT_HCA_FS_PARAM_OFFSET + 0x00)
+#define  INIT_HCA_FS_LOG_ENTRY_SZ_OFFSET  (INIT_HCA_FS_PARAM_OFFSET + 0x12)
+#define  INIT_HCA_FS_LOG_TABLE_SZ_OFFSET  (INIT_HCA_FS_PARAM_OFFSET + 0x1b)
+#define  INIT_HCA_FS_ETH_BITS_OFFSET      (INIT_HCA_FS_PARAM_OFFSET + 0x21)
+#define  INIT_HCA_FS_ETH_NUM_ADDRS_OFFSET (INIT_HCA_FS_PARAM_OFFSET + 0x22)
+#define  INIT_HCA_FS_IB_BITS_OFFSET       (INIT_HCA_FS_PARAM_OFFSET + 0x25)
+#define  INIT_HCA_FS_IB_NUM_ADDRS_OFFSET  (INIT_HCA_FS_PARAM_OFFSET + 0x26)
 #define INIT_HCA_TPT_OFFSET		 0x0f0
 #define	 INIT_HCA_DMPT_BASE_OFFSET	 (INIT_HCA_TPT_OFFSET + 0x00)
 #define	 INIT_HCA_LOG_MPT_SZ_OFFSET	 (INIT_HCA_TPT_OFFSET + 0x0b)
@@ -1119,14 +1209,44 @@ int mlx4_INIT_HCA(struct mlx4_dev *dev, struct mlx4_init_hca_param *param)
 	MLX4_PUT(inbox, param->rdmarc_base,   INIT_HCA_RDMARC_BASE_OFFSET);
 	MLX4_PUT(inbox, param->log_rd_per_qp, INIT_HCA_LOG_RD_OFFSET);
 
-	/* multicast attributes */
-
-	MLX4_PUT(inbox, param->mc_base,		INIT_HCA_MC_BASE_OFFSET);
-	MLX4_PUT(inbox, param->log_mc_entry_sz, INIT_HCA_LOG_MC_ENTRY_SZ_OFFSET);
-	MLX4_PUT(inbox, param->log_mc_hash_sz,  INIT_HCA_LOG_MC_HASH_SZ_OFFSET);
-	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER)
-		MLX4_PUT(inbox, (u8) (1 << 3),	INIT_HCA_UC_STEERING_OFFSET);
-	MLX4_PUT(inbox, param->log_mc_table_sz, INIT_HCA_LOG_MC_TABLE_SZ_OFFSET);
+	/* steering attributes */
+	if (dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		*(inbox + INIT_HCA_FLAGS_OFFSET / 4) |=
+			cpu_to_be32(1 <<
+				    INIT_HCA_DEVICE_MANAGED_FLOW_STEERING_EN);
+
+		MLX4_PUT(inbox, param->mc_base, INIT_HCA_FS_BASE_OFFSET);
+		MLX4_PUT(inbox, param->log_mc_entry_sz,
+			 INIT_HCA_FS_LOG_ENTRY_SZ_OFFSET);
+		MLX4_PUT(inbox, param->log_mc_table_sz,
+			 INIT_HCA_FS_LOG_TABLE_SZ_OFFSET);
+		/* Enable Ethernet flow steering
+		 * with udp unicast and tcp unicast
+		 */
+		MLX4_PUT(inbox, param->fs_hash_enable_bits,
+			 INIT_HCA_FS_ETH_BITS_OFFSET);
+		MLX4_PUT(inbox, (u16) MLX4_FS_NUM_OF_L2_ADDR,
+			 INIT_HCA_FS_ETH_NUM_ADDRS_OFFSET);
+		/* Enable IPoIB flow steering
+		 * with udp unicast and tcp unicast
+		 */
+		MLX4_PUT(inbox, param->fs_hash_enable_bits,
+			 INIT_HCA_FS_IB_BITS_OFFSET);
+		MLX4_PUT(inbox, (u16) MLX4_FS_NUM_OF_L2_ADDR,
+			 INIT_HCA_FS_IB_NUM_ADDRS_OFFSET);
+	} else {
+		MLX4_PUT(inbox, param->mc_base,	INIT_HCA_MC_BASE_OFFSET);
+		MLX4_PUT(inbox, param->log_mc_entry_sz,
+			 INIT_HCA_LOG_MC_ENTRY_SZ_OFFSET);
+		MLX4_PUT(inbox, param->log_mc_hash_sz,
+			 INIT_HCA_LOG_MC_HASH_SZ_OFFSET);
+		MLX4_PUT(inbox, param->log_mc_table_sz,
+			 INIT_HCA_LOG_MC_TABLE_SZ_OFFSET);
+		if (dev->caps.steering_mode == MLX4_STEERING_MODE_B0)
+			MLX4_PUT(inbox, (u8) (1 << 3),
+				 INIT_HCA_UC_STEERING_OFFSET);
+	}
 
 	/* TPT attributes */
 
@@ -1188,15 +1308,24 @@ int mlx4_QUERY_HCA(struct mlx4_dev *dev,
 	MLX4_GET(param->rdmarc_base,   outbox, INIT_HCA_RDMARC_BASE_OFFSET);
 	MLX4_GET(param->log_rd_per_qp, outbox, INIT_HCA_LOG_RD_OFFSET);
 
-	/* multicast attributes */
+	/* steering attributes */
+	if (dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED) {
 
-	MLX4_GET(param->mc_base,         outbox, INIT_HCA_MC_BASE_OFFSET);
-	MLX4_GET(param->log_mc_entry_sz, outbox,
-		 INIT_HCA_LOG_MC_ENTRY_SZ_OFFSET);
-	MLX4_GET(param->log_mc_hash_sz,  outbox,
-		 INIT_HCA_LOG_MC_HASH_SZ_OFFSET);
-	MLX4_GET(param->log_mc_table_sz, outbox,
-		 INIT_HCA_LOG_MC_TABLE_SZ_OFFSET);
+		MLX4_GET(param->mc_base, outbox, INIT_HCA_FS_BASE_OFFSET);
+		MLX4_GET(param->log_mc_entry_sz, outbox,
+			 INIT_HCA_FS_LOG_ENTRY_SZ_OFFSET);
+		MLX4_GET(param->log_mc_table_sz, outbox,
+			 INIT_HCA_FS_LOG_TABLE_SZ_OFFSET);
+	} else {
+		MLX4_GET(param->mc_base, outbox, INIT_HCA_MC_BASE_OFFSET);
+		MLX4_GET(param->log_mc_entry_sz, outbox,
+			 INIT_HCA_LOG_MC_ENTRY_SZ_OFFSET);
+		MLX4_GET(param->log_mc_hash_sz,  outbox,
+			 INIT_HCA_LOG_MC_HASH_SZ_OFFSET);
+		MLX4_GET(param->log_mc_table_sz, outbox,
+			 INIT_HCA_LOG_MC_TABLE_SZ_OFFSET);
+	}
 
 	/* TPT attributes */
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index 64c0399e4b7..83fcbbf1b16 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -78,6 +78,8 @@ struct mlx4_dev_cap {
 	u16 wavelength[MLX4_MAX_PORTS + 1];
 	u64 trans_code[MLX4_MAX_PORTS + 1];
 	u16 stat_rate_support;
+	int fs_log_max_ucast_qp_range_size;
+	int fs_max_num_qp_per_entry;
 	u64 flags;
 	u64 flags2;
 	int reserved_uars;
@@ -165,6 +167,7 @@ struct mlx4_init_hca_param {
 	u8  log_mpt_sz;
 	u8  log_uar_sz;
 	u8  uar_page_sz; /* log pg sz in 4k chunks */
+	u8  fs_hash_enable_bits;
 };
 
 struct mlx4_init_ib_param {
diff --git a/drivers/net/ethernet/mellanox/mlx4/icm.c b/drivers/net/ethernet/mellanox/mlx4/icm.c
index a9ade1c3cad..88b7b3e75ab 100644
--- a/drivers/net/ethernet/mellanox/mlx4/icm.c
+++ b/drivers/net/ethernet/mellanox/mlx4/icm.c
@@ -413,6 +413,8 @@ err:
 			mlx4_free_icm(dev, table->icm[i], use_coherent);
 		}
 
+	kfree(table->icm);
+
 	return -ENOMEM;
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/icm.h b/drivers/net/ethernet/mellanox/mlx4/icm.h
index b10c07a1dc1..19e4efc0b34 100644
--- a/drivers/net/ethernet/mellanox/mlx4/icm.h
+++ b/drivers/net/ethernet/mellanox/mlx4/icm.h
@@ -81,13 +81,7 @@ int mlx4_init_icm_table(struct mlx4_dev *dev, struct mlx4_icm_table *table,
 			u64 virt, int obj_size,	int nobj, int reserved,
 			int use_lowmem, int use_coherent);
 void mlx4_cleanup_icm_table(struct mlx4_dev *dev, struct mlx4_icm_table *table);
-int mlx4_table_get(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj);
-void mlx4_table_put(struct mlx4_dev *dev, struct mlx4_icm_table *table, int obj);
 void *mlx4_table_find(struct mlx4_icm_table *table, int obj, dma_addr_t *dma_handle);
-int mlx4_table_get_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
-			 int start, int end);
-void mlx4_table_put_range(struct mlx4_dev *dev, struct mlx4_icm_table *table,
-			  int start, int end);
 
 static inline void mlx4_icm_first(struct mlx4_icm *icm,
 				  struct mlx4_icm_iter *iter)
diff --git a/drivers/net/ethernet/mellanox/mlx4/intf.c b/drivers/net/ethernet/mellanox/mlx4/intf.c
index b4e9f6f5cc0..116895ac8b3 100644
--- a/drivers/net/ethernet/mellanox/mlx4/intf.c
+++ b/drivers/net/ethernet/mellanox/mlx4/intf.c
@@ -115,7 +115,8 @@ void mlx4_unregister_interface(struct mlx4_interface *intf)
 }
 EXPORT_SYMBOL_GPL(mlx4_unregister_interface);
 
-void mlx4_dispatch_event(struct mlx4_dev *dev, enum mlx4_dev_event type, int port)
+void mlx4_dispatch_event(struct mlx4_dev *dev, enum mlx4_dev_event type,
+			 unsigned long param)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_device_context *dev_ctx;
@@ -125,7 +126,7 @@ void mlx4_dispatch_event(struct mlx4_dev *dev, enum mlx4_dev_event type, int por
 
 	list_for_each_entry(dev_ctx, &priv->ctx_list, list)
 		if (dev_ctx->intf->event)
-			dev_ctx->intf->event(dev, dev_ctx->context, type, port);
+			dev_ctx->intf->event(dev, dev_ctx->context, type, param);
 
 	spin_unlock_irqrestore(&priv->ctx_lock, flags);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index a0313de122d..48d0e90194c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -41,6 +41,7 @@
 #include <linux/slab.h>
 #include <linux/io-mapping.h>
 #include <linux/delay.h>
+#include <linux/netdevice.h>
 
 #include <linux/mlx4/device.h>
 #include <linux/mlx4/doorbell.h>
@@ -90,7 +91,9 @@ module_param_named(log_num_mgm_entry_size,
 MODULE_PARM_DESC(log_num_mgm_entry_size, "log mgm size, that defines the num"
 					 " of qp per mcg, for example:"
 					 " 10 gives 248.range: 9<="
-					 " log_num_mgm_entry_size <= 12");
+					 " log_num_mgm_entry_size <= 12."
+					 " Not in use with device managed"
+					 " flow steering");
 
 #define MLX4_VF                                        (1 << 0)
 
@@ -215,6 +218,10 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	for (i = 1; i <= dev->caps.num_ports; ++i) {
 		dev->caps.vl_cap[i]	    = dev_cap->max_vl[i];
 		dev->caps.ib_mtu_cap[i]	    = dev_cap->ib_mtu[i];
+		dev->phys_caps.gid_phys_table_len[i]  = dev_cap->max_gids[i];
+		dev->phys_caps.pkey_phys_table_len[i] = dev_cap->max_pkeys[i];
+		/* set gid and pkey table operating lengths by default
+		 * to non-sriov values */
 		dev->caps.gid_table_len[i]  = dev_cap->max_gids[i];
 		dev->caps.pkey_table_len[i] = dev_cap->max_pkeys[i];
 		dev->caps.port_width_cap[i] = dev_cap->max_port_width[i];
@@ -243,7 +250,6 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev->caps.reserved_srqs	     = dev_cap->reserved_srqs;
 	dev->caps.max_sq_desc_sz     = dev_cap->max_sq_desc_sz;
 	dev->caps.max_rq_desc_sz     = dev_cap->max_rq_desc_sz;
-	dev->caps.num_qp_per_mgm     = mlx4_get_qp_per_mgm(dev);
 	/*
 	 * Subtract 1 from the limit because we need to allocate a
 	 * spare CQE so the HCA HW can tell the difference between an
@@ -274,6 +280,28 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev->caps.max_gso_sz	     = dev_cap->max_gso_sz;
 	dev->caps.max_rss_tbl_sz     = dev_cap->max_rss_tbl_sz;
 
+	if (dev_cap->flags2 & MLX4_DEV_CAP_FLAG2_FS_EN) {
+		dev->caps.steering_mode = MLX4_STEERING_MODE_DEVICE_MANAGED;
+		dev->caps.num_qp_per_mgm = dev_cap->fs_max_num_qp_per_entry;
+		dev->caps.fs_log_max_ucast_qp_range_size =
+			dev_cap->fs_log_max_ucast_qp_range_size;
+	} else {
+		if (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER &&
+		    dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER) {
+			dev->caps.steering_mode = MLX4_STEERING_MODE_B0;
+		} else {
+			dev->caps.steering_mode = MLX4_STEERING_MODE_A0;
+
+			if (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER ||
+			    dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER)
+				mlx4_warn(dev, "Must have UC_STEER and MC_STEER flags "
+						"set to use B0 steering. Falling back to A0 steering mode.\n");
+		}
+		dev->caps.num_qp_per_mgm = mlx4_get_qp_per_mgm(dev);
+	}
+	mlx4_dbg(dev, "Steering mode is: %s\n",
+		 mlx4_steering_mode_str(dev->caps.steering_mode));
+
 	/* Sense port always allowed on supported devices for ConnectX1 and 2 */
 	if (dev->pdev->device != 0x1003)
 		dev->caps.flags |= MLX4_DEV_CAP_FLAG_SENSE_SUPPORT;
@@ -288,29 +316,19 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 			/* if only ETH is supported - assign ETH */
 			if (dev->caps.supported_type[i] == MLX4_PORT_TYPE_ETH)
 				dev->caps.port_type[i] = MLX4_PORT_TYPE_ETH;
-			/* if only IB is supported,
-			 * assign IB only if SRIOV is off*/
+			/* if only IB is supported, assign IB */
 			else if (dev->caps.supported_type[i] ==
-				 MLX4_PORT_TYPE_IB) {
-				if (dev->flags & MLX4_FLAG_SRIOV)
-					dev->caps.port_type[i] =
-						MLX4_PORT_TYPE_NONE;
-				else
-					dev->caps.port_type[i] =
-						MLX4_PORT_TYPE_IB;
-			/* if IB and ETH are supported,
-			 * first of all check if SRIOV is on */
-			} else if (dev->flags & MLX4_FLAG_SRIOV)
-				dev->caps.port_type[i] = MLX4_PORT_TYPE_ETH;
+				 MLX4_PORT_TYPE_IB)
+				dev->caps.port_type[i] = MLX4_PORT_TYPE_IB;
 			else {
-				/* In non-SRIOV mode, we set the port type
-				 * according to user selection of port type,
-				 * if usere selected none, take the FW hint */
-				if (port_type_array[i-1] == MLX4_PORT_TYPE_NONE)
+				/* if IB and ETH are supported, we set the port
+				 * type according to user selection of port type;
+				 * if user selected none, take the FW hint */
+				if (port_type_array[i - 1] == MLX4_PORT_TYPE_NONE)
 					dev->caps.port_type[i] = dev->caps.suggested_type[i] ?
 						MLX4_PORT_TYPE_ETH : MLX4_PORT_TYPE_IB;
 				else
-					dev->caps.port_type[i] = port_type_array[i-1];
+					dev->caps.port_type[i] = port_type_array[i - 1];
 			}
 		}
 		/*
@@ -391,6 +409,23 @@ static int mlx4_how_many_lives_vf(struct mlx4_dev *dev)
 	return ret;
 }
 
+int mlx4_get_parav_qkey(struct mlx4_dev *dev, u32 qpn, u32 *qkey)
+{
+	u32 qk = MLX4_RESERVED_QKEY_BASE;
+	if (qpn >= dev->caps.base_tunnel_sqpn + 8 * MLX4_MFUNC_MAX ||
+	    qpn < dev->caps.sqp_start)
+		return -EINVAL;
+
+	if (qpn >= dev->caps.base_tunnel_sqpn)
+		/* tunnel qp */
+		qk += qpn - dev->caps.base_tunnel_sqpn;
+	else
+		qk += qpn - dev->caps.sqp_start;
+	*qkey = qk;
+	return 0;
+}
+EXPORT_SYMBOL(mlx4_get_parav_qkey);
+
 int mlx4_is_slave_active(struct mlx4_dev *dev, int slave)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -491,8 +526,13 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 		return -ENODEV;
 	}
 
-	for (i = 1; i <= dev->caps.num_ports; ++i)
+	for (i = 1; i <= dev->caps.num_ports; ++i) {
 		dev->caps.port_mask[i] = dev->caps.port_type[i];
+		if (mlx4_get_slave_pkey_gid_tbl_len(dev, i,
+						    &dev->caps.gid_table_len[i],
+						    &dev->caps.pkey_table_len[i]))
+			return -ENODEV;
+	}
 
 	if (dev->caps.uar_page_size * (dev->caps.num_uars -
 				       dev->caps.reserved_uars) >
@@ -529,7 +569,7 @@ int mlx4_change_port_types(struct mlx4_dev *dev,
 		for (port = 1; port <= dev->caps.num_ports; port++) {
 			mlx4_CLOSE_PORT(dev, port);
 			dev->caps.port_type[port] = port_types[port - 1];
-			err = mlx4_SET_PORT(dev, port);
+			err = mlx4_SET_PORT(dev, port, -1);
 			if (err) {
 				mlx4_err(dev, "Failed to set port %d, "
 					      "aborting\n", port);
@@ -715,7 +755,7 @@ static ssize_t set_port_ib_mtu(struct device *dev,
 	mlx4_unregister_device(mdev);
 	for (port = 1; port <= mdev->caps.num_ports; port++) {
 		mlx4_CLOSE_PORT(mdev, port);
-		err = mlx4_SET_PORT(mdev, port);
+		err = mlx4_SET_PORT(mdev, port, -1);
 		if (err) {
 			mlx4_err(mdev, "Failed to set port %d, "
 				      "aborting\n", port);
@@ -967,9 +1007,11 @@ static int mlx4_init_icm(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap,
 	}
 
 	/*
-	 * It's not strictly required, but for simplicity just map the
-	 * whole multicast group table now.  The table isn't very big
-	 * and it's a lot easier than trying to track ref counts.
+	 * For flow steering device managed mode it is required to use
+	 * mlx4_init_icm_table. For B0 steering mode it's not strictly
+	 * required, but for simplicity just map the whole multicast
+	 * group table now.  The table isn't very big and it's a lot
+	 * easier than trying to track ref counts.
 	 */
 	err = mlx4_init_icm_table(dev, &priv->mcg_table.table,
 				  init_hca->mc_base,
@@ -1166,6 +1208,17 @@ err:
 	return -EIO;
 }
 
+static void mlx4_parav_master_pf_caps(struct mlx4_dev *dev)
+{
+	int i;
+
+	for (i = 1; i <= dev->caps.num_ports; i++) {
+		dev->caps.gid_table_len[i] = 1;
+		dev->caps.pkey_table_len[i] =
+			dev->phys_caps.pkey_phys_table_len[i] - 1;
+	}
+}
+
 static int mlx4_init_hca(struct mlx4_dev *dev)
 {
 	struct mlx4_priv	  *priv = mlx4_priv(dev);
@@ -1205,7 +1258,29 @@ static int mlx4_init_hca(struct mlx4_dev *dev)
 			goto err_stop_fw;
 		}
 
+		if (mlx4_is_master(dev))
+			mlx4_parav_master_pf_caps(dev);
+
+		priv->fs_hash_mode = MLX4_FS_L2_HASH;
+
+		switch (priv->fs_hash_mode) {
+		case MLX4_FS_L2_HASH:
+			init_hca.fs_hash_enable_bits = 0;
+			break;
+
+		case MLX4_FS_L2_L3_L4_HASH:
+			/* Enable flow steering with
+			 * udp unicast and tcp unicast
+			 */
+			init_hca.fs_hash_enable_bits =
+				MLX4_FS_UDP_UC_EN | MLX4_FS_TCP_UC_EN;
+			break;
+		}
+
 		profile = default_profile;
+		if (dev->caps.steering_mode ==
+		    MLX4_STEERING_MODE_DEVICE_MANAGED)
+			profile.num_mcg = MLX4_FS_NUM_MCG;
 
 		icm_size = mlx4_make_profile(dev, &profile, &dev_cap,
 					     &init_hca);
@@ -1477,12 +1552,24 @@ static int mlx4_setup_hca(struct mlx4_dev *dev)
 					  "with caps = 0\n", port, err);
 			dev->caps.ib_port_def_cap[port] = ib_port_default_caps;
 
+			/* initialize per-slave default ib port capabilities */
+			if (mlx4_is_master(dev)) {
+				int i;
+				for (i = 0; i < dev->num_slaves; i++) {
+					if (i == mlx4_master_func_num(dev))
+						continue;
+					priv->mfunc.master.slave_state[i].ib_cap_mask[port] =
+							ib_port_default_caps;
+				}
+			}
+
 			if (mlx4_is_mfunc(dev))
 				dev->caps.port_ib_mtu[port] = IB_MTU_2048;
 			else
 				dev->caps.port_ib_mtu[port] = IB_MTU_4096;
 
-			err = mlx4_SET_PORT(dev, port);
+			err = mlx4_SET_PORT(dev, port, mlx4_is_master(dev) ?
+					    dev->caps.pkey_table_len[port] : -1);
 			if (err) {
 				mlx4_err(dev, "Failed to set port %d, aborting\n",
 					port);
@@ -1539,8 +1626,8 @@ static void mlx4_enable_msi_x(struct mlx4_dev *dev)
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct msix_entry *entries;
 	int nreq = min_t(int, dev->caps.num_ports *
-			 min_t(int, num_online_cpus() + 1, MAX_MSIX_P_PORT)
-				+ MSIX_LEGACY_SZ, MAX_MSIX);
+			 min_t(int, netif_get_num_default_rss_queues() + 1,
+			       MAX_MSIX_P_PORT) + MSIX_LEGACY_SZ, MAX_MSIX);
 	int err;
 	int i;
 
@@ -1730,6 +1817,9 @@ static int mlx4_get_ownership(struct mlx4_dev *dev)
 	void __iomem *owner;
 	u32 ret;
 
+	if (pci_channel_offline(dev->pdev))
+		return -EIO;
+
 	owner = ioremap(pci_resource_start(dev->pdev, 0) + MLX4_OWNER_BASE,
 			MLX4_OWNER_SIZE);
 	if (!owner) {
@@ -1746,6 +1836,9 @@ static void mlx4_free_ownership(struct mlx4_dev *dev)
 {
 	void __iomem *owner;
 
+	if (pci_channel_offline(dev->pdev))
+		return;
+
 	owner = ioremap(pci_resource_start(dev->pdev, 0) + MLX4_OWNER_BASE,
 			MLX4_OWNER_SIZE);
 	if (!owner) {
@@ -2192,11 +2285,33 @@ static DEFINE_PCI_DEVICE_TABLE(mlx4_pci_table) = {
 
 MODULE_DEVICE_TABLE(pci, mlx4_pci_table);
 
+static pci_ers_result_t mlx4_pci_err_detected(struct pci_dev *pdev,
+					      pci_channel_state_t state)
+{
+	mlx4_remove_one(pdev);
+
+	return state == pci_channel_io_perm_failure ?
+		PCI_ERS_RESULT_DISCONNECT : PCI_ERS_RESULT_NEED_RESET;
+}
+
+static pci_ers_result_t mlx4_pci_slot_reset(struct pci_dev *pdev)
+{
+	int ret = __mlx4_init_one(pdev, NULL);
+
+	return ret ? PCI_ERS_RESULT_DISCONNECT : PCI_ERS_RESULT_RECOVERED;
+}
+
+static struct pci_error_handlers mlx4_err_handler = {
+	.error_detected = mlx4_pci_err_detected,
+	.slot_reset     = mlx4_pci_slot_reset,
+};
+
 static struct pci_driver mlx4_driver = {
 	.name		= DRV_NAME,
 	.id_table	= mlx4_pci_table,
 	.probe		= mlx4_init_one,
-	.remove		= __devexit_p(mlx4_remove_one)
+	.remove		= __devexit_p(mlx4_remove_one),
+	.err_handler    = &mlx4_err_handler,
 };
 
 static int __init mlx4_verify_params(void)
diff --git a/drivers/net/ethernet/mellanox/mlx4/mcg.c b/drivers/net/ethernet/mellanox/mlx4/mcg.c
index f4a8f98e402..4ec3835e1bc 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mcg.c
@@ -54,7 +54,12 @@ struct mlx4_mgm {
 
 int mlx4_get_mgm_entry_size(struct mlx4_dev *dev)
 {
-	return min((1 << mlx4_log_num_mgm_entry_size), MLX4_MAX_MGM_ENTRY_SIZE);
+	if (dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED)
+		return 1 << MLX4_FS_MGM_LOG_ENTRY_SIZE;
+	else
+		return min((1 << mlx4_log_num_mgm_entry_size),
+			   MLX4_MAX_MGM_ENTRY_SIZE);
 }
 
 int mlx4_get_qp_per_mgm(struct mlx4_dev *dev)
@@ -62,6 +67,35 @@ int mlx4_get_qp_per_mgm(struct mlx4_dev *dev)
 	return 4 * (mlx4_get_mgm_entry_size(dev) / 16 - 2);
 }
 
+static int mlx4_QP_FLOW_STEERING_ATTACH(struct mlx4_dev *dev,
+					struct mlx4_cmd_mailbox *mailbox,
+					u32 size,
+					u64 *reg_id)
+{
+	u64 imm;
+	int err = 0;
+
+	err = mlx4_cmd_imm(dev, mailbox->dma, &imm, size, 0,
+			   MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
+			   MLX4_CMD_NATIVE);
+	if (err)
+		return err;
+	*reg_id = imm;
+
+	return err;
+}
+
+static int mlx4_QP_FLOW_STEERING_DETACH(struct mlx4_dev *dev, u64 regid)
+{
+	int err = 0;
+
+	err = mlx4_cmd(dev, regid, 0, 0,
+		       MLX4_QP_FLOW_STEERING_DETACH, MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+
+	return err;
+}
+
 static int mlx4_READ_ENTRY(struct mlx4_dev *dev, int index,
 			   struct mlx4_cmd_mailbox *mailbox)
 {
@@ -614,6 +648,311 @@ static int find_entry(struct mlx4_dev *dev, u8 port,
 	return err;
 }
 
+struct mlx4_net_trans_rule_hw_ctrl {
+	__be32 ctrl;
+	__be32 vf_vep_port;
+	__be32 qpn;
+	__be32 reserved;
+};
+
+static void trans_rule_ctrl_to_hw(struct mlx4_net_trans_rule *ctrl,
+				  struct mlx4_net_trans_rule_hw_ctrl *hw)
+{
+	static const u8 __promisc_mode[] = {
+		[MLX4_FS_PROMISC_NONE]   = 0x0,
+		[MLX4_FS_PROMISC_UPLINK] = 0x1,
+		[MLX4_FS_PROMISC_FUNCTION_PORT] = 0x2,
+		[MLX4_FS_PROMISC_ALL_MULTI] = 0x3,
+	};
+
+	u32 dw = 0;
+
+	dw = ctrl->queue_mode == MLX4_NET_TRANS_Q_LIFO ? 1 : 0;
+	dw |= ctrl->exclusive ? (1 << 2) : 0;
+	dw |= ctrl->allow_loopback ? (1 << 3) : 0;
+	dw |= __promisc_mode[ctrl->promisc_mode] << 8;
+	dw |= ctrl->priority << 16;
+
+	hw->ctrl = cpu_to_be32(dw);
+	hw->vf_vep_port = cpu_to_be32(ctrl->port);
+	hw->qpn = cpu_to_be32(ctrl->qpn);
+}
+
+struct mlx4_net_trans_rule_hw_ib {
+	u8	size;
+	u8	rsvd1;
+	__be16	id;
+	u32	rsvd2;
+	__be32	qpn;
+	__be32	qpn_mask;
+	u8	dst_gid[16];
+	u8	dst_gid_msk[16];
+} __packed;
+
+struct mlx4_net_trans_rule_hw_eth {
+	u8	size;
+	u8	rsvd;
+	__be16	id;
+	u8	rsvd1[6];
+	u8	dst_mac[6];
+	u16	rsvd2;
+	u8	dst_mac_msk[6];
+	u16	rsvd3;
+	u8	src_mac[6];
+	u16	rsvd4;
+	u8	src_mac_msk[6];
+	u8      rsvd5;
+	u8      ether_type_enable;
+	__be16  ether_type;
+	__be16  vlan_id_msk;
+	__be16  vlan_id;
+} __packed;
+
+struct mlx4_net_trans_rule_hw_tcp_udp {
+	u8	size;
+	u8	rsvd;
+	__be16	id;
+	__be16	rsvd1[3];
+	__be16	dst_port;
+	__be16	rsvd2;
+	__be16	dst_port_msk;
+	__be16	rsvd3;
+	__be16	src_port;
+	__be16	rsvd4;
+	__be16	src_port_msk;
+} __packed;
+
+struct mlx4_net_trans_rule_hw_ipv4 {
+	u8	size;
+	u8	rsvd;
+	__be16	id;
+	__be32	rsvd1;
+	__be32	dst_ip;
+	__be32	dst_ip_msk;
+	__be32	src_ip;
+	__be32	src_ip_msk;
+} __packed;
+
+struct _rule_hw {
+	union {
+		struct {
+			u8 size;
+			u8 rsvd;
+			__be16 id;
+		};
+		struct mlx4_net_trans_rule_hw_eth eth;
+		struct mlx4_net_trans_rule_hw_ib ib;
+		struct mlx4_net_trans_rule_hw_ipv4 ipv4;
+		struct mlx4_net_trans_rule_hw_tcp_udp tcp_udp;
+	};
+};
+
+static int parse_trans_rule(struct mlx4_dev *dev, struct mlx4_spec_list *spec,
+			    struct _rule_hw *rule_hw)
+{
+	static const u16 __sw_id_hw[] = {
+		[MLX4_NET_TRANS_RULE_ID_ETH]     = 0xE001,
+		[MLX4_NET_TRANS_RULE_ID_IB]      = 0xE005,
+		[MLX4_NET_TRANS_RULE_ID_IPV6]    = 0xE003,
+		[MLX4_NET_TRANS_RULE_ID_IPV4]    = 0xE002,
+		[MLX4_NET_TRANS_RULE_ID_TCP]     = 0xE004,
+		[MLX4_NET_TRANS_RULE_ID_UDP]     = 0xE006
+	};
+
+	static const size_t __rule_hw_sz[] = {
+		[MLX4_NET_TRANS_RULE_ID_ETH] =
+			sizeof(struct mlx4_net_trans_rule_hw_eth),
+		[MLX4_NET_TRANS_RULE_ID_IB] =
+			sizeof(struct mlx4_net_trans_rule_hw_ib),
+		[MLX4_NET_TRANS_RULE_ID_IPV6] = 0,
+		[MLX4_NET_TRANS_RULE_ID_IPV4] =
+			sizeof(struct mlx4_net_trans_rule_hw_ipv4),
+		[MLX4_NET_TRANS_RULE_ID_TCP] =
+			sizeof(struct mlx4_net_trans_rule_hw_tcp_udp),
+		[MLX4_NET_TRANS_RULE_ID_UDP] =
+			sizeof(struct mlx4_net_trans_rule_hw_tcp_udp)
+	};
+	if (spec->id >= MLX4_NET_TRANS_RULE_NUM) {
+		mlx4_err(dev, "Invalid network rule id. id = %d\n", spec->id);
+		return -EINVAL;
+	}
+	memset(rule_hw, 0, __rule_hw_sz[spec->id]);
+	rule_hw->id = cpu_to_be16(__sw_id_hw[spec->id]);
+	rule_hw->size = __rule_hw_sz[spec->id] >> 2;
+
+	switch (spec->id) {
+	case MLX4_NET_TRANS_RULE_ID_ETH:
+		memcpy(rule_hw->eth.dst_mac, spec->eth.dst_mac, ETH_ALEN);
+		memcpy(rule_hw->eth.dst_mac_msk, spec->eth.dst_mac_msk,
+		       ETH_ALEN);
+		memcpy(rule_hw->eth.src_mac, spec->eth.src_mac, ETH_ALEN);
+		memcpy(rule_hw->eth.src_mac_msk, spec->eth.src_mac_msk,
+		       ETH_ALEN);
+		if (spec->eth.ether_type_enable) {
+			rule_hw->eth.ether_type_enable = 1;
+			rule_hw->eth.ether_type = spec->eth.ether_type;
+		}
+		rule_hw->eth.vlan_id = spec->eth.vlan_id;
+		rule_hw->eth.vlan_id_msk = spec->eth.vlan_id_msk;
+		break;
+
+	case MLX4_NET_TRANS_RULE_ID_IB:
+		rule_hw->ib.qpn = spec->ib.r_qpn;
+		rule_hw->ib.qpn_mask = spec->ib.qpn_msk;
+		memcpy(&rule_hw->ib.dst_gid, &spec->ib.dst_gid, 16);
+		memcpy(&rule_hw->ib.dst_gid_msk, &spec->ib.dst_gid_msk, 16);
+		break;
+
+	case MLX4_NET_TRANS_RULE_ID_IPV6:
+		return -EOPNOTSUPP;
+
+	case MLX4_NET_TRANS_RULE_ID_IPV4:
+		rule_hw->ipv4.src_ip = spec->ipv4.src_ip;
+		rule_hw->ipv4.src_ip_msk = spec->ipv4.src_ip_msk;
+		rule_hw->ipv4.dst_ip = spec->ipv4.dst_ip;
+		rule_hw->ipv4.dst_ip_msk = spec->ipv4.dst_ip_msk;
+		break;
+
+	case MLX4_NET_TRANS_RULE_ID_TCP:
+	case MLX4_NET_TRANS_RULE_ID_UDP:
+		rule_hw->tcp_udp.dst_port = spec->tcp_udp.dst_port;
+		rule_hw->tcp_udp.dst_port_msk = spec->tcp_udp.dst_port_msk;
+		rule_hw->tcp_udp.src_port = spec->tcp_udp.src_port;
+		rule_hw->tcp_udp.src_port_msk = spec->tcp_udp.src_port_msk;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return __rule_hw_sz[spec->id];
+}
+
+static void mlx4_err_rule(struct mlx4_dev *dev, char *str,
+			  struct mlx4_net_trans_rule *rule)
+{
+#define BUF_SIZE 256
+	struct mlx4_spec_list *cur;
+	char buf[BUF_SIZE];
+	int len = 0;
+
+	mlx4_err(dev, "%s", str);
+	len += snprintf(buf + len, BUF_SIZE - len,
+			"port = %d prio = 0x%x qp = 0x%x ",
+			rule->port, rule->priority, rule->qpn);
+
+	list_for_each_entry(cur, &rule->list, list) {
+		switch (cur->id) {
+		case MLX4_NET_TRANS_RULE_ID_ETH:
+			len += snprintf(buf + len, BUF_SIZE - len,
+					"dmac = %pM ", &cur->eth.dst_mac);
+			if (cur->eth.ether_type)
+				len += snprintf(buf + len, BUF_SIZE - len,
+						"ethertype = 0x%x ",
+						be16_to_cpu(cur->eth.ether_type));
+			if (cur->eth.vlan_id)
+				len += snprintf(buf + len, BUF_SIZE - len,
+						"vlan-id = %d ",
+						be16_to_cpu(cur->eth.vlan_id));
+			break;
+
+		case MLX4_NET_TRANS_RULE_ID_IPV4:
+			if (cur->ipv4.src_ip)
+				len += snprintf(buf + len, BUF_SIZE - len,
+						"src-ip = %pI4 ",
+						&cur->ipv4.src_ip);
+			if (cur->ipv4.dst_ip)
+				len += snprintf(buf + len, BUF_SIZE - len,
+						"dst-ip = %pI4 ",
+						&cur->ipv4.dst_ip);
+			break;
+
+		case MLX4_NET_TRANS_RULE_ID_TCP:
+		case MLX4_NET_TRANS_RULE_ID_UDP:
+			if (cur->tcp_udp.src_port)
+				len += snprintf(buf + len, BUF_SIZE - len,
+						"src-port = %d ",
+						be16_to_cpu(cur->tcp_udp.src_port));
+			if (cur->tcp_udp.dst_port)
+				len += snprintf(buf + len, BUF_SIZE - len,
+						"dst-port = %d ",
+						be16_to_cpu(cur->tcp_udp.dst_port));
+			break;
+
+		case MLX4_NET_TRANS_RULE_ID_IB:
+			len += snprintf(buf + len, BUF_SIZE - len,
+					"dst-gid = %pI6\n", cur->ib.dst_gid);
+			len += snprintf(buf + len, BUF_SIZE - len,
+					"dst-gid-mask = %pI6\n",
+					cur->ib.dst_gid_msk);
+			break;
+
+		case MLX4_NET_TRANS_RULE_ID_IPV6:
+			break;
+
+		default:
+			break;
+		}
+	}
+	len += snprintf(buf + len, BUF_SIZE - len, "\n");
+	mlx4_err(dev, "%s", buf);
+
+	if (len >= BUF_SIZE)
+		mlx4_err(dev, "Network rule error message was truncated, print buffer is too small.\n");
+}
+
+int mlx4_flow_attach(struct mlx4_dev *dev,
+		     struct mlx4_net_trans_rule *rule, u64 *reg_id)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_spec_list *cur;
+	u32 size = 0;
+	int ret;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	memset(mailbox->buf, 0, sizeof(struct mlx4_net_trans_rule_hw_ctrl));
+	trans_rule_ctrl_to_hw(rule, mailbox->buf);
+
+	size += sizeof(struct mlx4_net_trans_rule_hw_ctrl);
+
+	list_for_each_entry(cur, &rule->list, list) {
+		ret = parse_trans_rule(dev, cur, mailbox->buf + size);
+		if (ret < 0) {
+			mlx4_free_cmd_mailbox(dev, mailbox);
+			return -EINVAL;
+		}
+		size += ret;
+	}
+
+	ret = mlx4_QP_FLOW_STEERING_ATTACH(dev, mailbox, size >> 2, reg_id);
+	if (ret == -ENOMEM)
+		mlx4_err_rule(dev,
+			      "mcg table is full. Fail to register network rule.\n",
+			      rule);
+	else if (ret)
+		mlx4_err_rule(dev, "Fail to register network rule.\n", rule);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mlx4_flow_attach);
+
+int mlx4_flow_detach(struct mlx4_dev *dev, u64 reg_id)
+{
+	int err;
+
+	err = mlx4_QP_FLOW_STEERING_DETACH(dev, reg_id);
+	if (err)
+		mlx4_err(dev, "Fail to detach network rule. registration id = 0x%llx\n",
+			 reg_id);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx4_flow_detach);
+
 int mlx4_qp_attach_common(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
 			  int block_mcast_loopback, enum mlx4_protocol prot,
 			  enum mlx4_steer_type steer)
@@ -866,49 +1205,159 @@ static int mlx4_QP_ATTACH(struct mlx4_dev *dev, struct mlx4_qp *qp,
 }
 
 int mlx4_multicast_attach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
-			  int block_mcast_loopback, enum mlx4_protocol prot)
+			  u8 port, int block_mcast_loopback,
+			  enum mlx4_protocol prot, u64 *reg_id)
 {
-	if (prot == MLX4_PROT_ETH &&
-			!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER))
-		return 0;
 
-	if (prot == MLX4_PROT_ETH)
-		gid[7] |= (MLX4_MC_STEER << 1);
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_A0:
+		if (prot == MLX4_PROT_ETH)
+			return 0;
+
+	case MLX4_STEERING_MODE_B0:
+		if (prot == MLX4_PROT_ETH)
+			gid[7] |= (MLX4_MC_STEER << 1);
+
+		if (mlx4_is_mfunc(dev))
+			return mlx4_QP_ATTACH(dev, qp, gid, 1,
+					      block_mcast_loopback, prot);
+		return mlx4_qp_attach_common(dev, qp, gid,
+					     block_mcast_loopback, prot,
+					     MLX4_MC_STEER);
+
+	case MLX4_STEERING_MODE_DEVICE_MANAGED: {
+		struct mlx4_spec_list spec = { {NULL} };
+		__be64 mac_mask = cpu_to_be64(MLX4_MAC_MASK << 16);
+
+		struct mlx4_net_trans_rule rule = {
+			.queue_mode = MLX4_NET_TRANS_Q_FIFO,
+			.exclusive = 0,
+			.promisc_mode = MLX4_FS_PROMISC_NONE,
+			.priority = MLX4_DOMAIN_NIC,
+		};
+
+		rule.allow_loopback = ~block_mcast_loopback;
+		rule.port = port;
+		rule.qpn = qp->qpn;
+		INIT_LIST_HEAD(&rule.list);
+
+		switch (prot) {
+		case MLX4_PROT_ETH:
+			spec.id = MLX4_NET_TRANS_RULE_ID_ETH;
+			memcpy(spec.eth.dst_mac, &gid[10], ETH_ALEN);
+			memcpy(spec.eth.dst_mac_msk, &mac_mask, ETH_ALEN);
+			break;
 
-	if (mlx4_is_mfunc(dev))
-		return mlx4_QP_ATTACH(dev, qp, gid, 1,
-					block_mcast_loopback, prot);
+		case MLX4_PROT_IB_IPV6:
+			spec.id = MLX4_NET_TRANS_RULE_ID_IB;
+			memcpy(spec.ib.dst_gid, gid, 16);
+			memset(&spec.ib.dst_gid_msk, 0xff, 16);
+			break;
+		default:
+			return -EINVAL;
+		}
+		list_add_tail(&spec.list, &rule.list);
 
-	return mlx4_qp_attach_common(dev, qp, gid, block_mcast_loopback,
-					prot, MLX4_MC_STEER);
+		return mlx4_flow_attach(dev, &rule, reg_id);
+	}
+
+	default:
+		return -EINVAL;
+	}
 }
 EXPORT_SYMBOL_GPL(mlx4_multicast_attach);
 
 int mlx4_multicast_detach(struct mlx4_dev *dev, struct mlx4_qp *qp, u8 gid[16],
-			  enum mlx4_protocol prot)
+			  enum mlx4_protocol prot, u64 reg_id)
 {
-	if (prot == MLX4_PROT_ETH &&
-			!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER))
-		return 0;
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_A0:
+		if (prot == MLX4_PROT_ETH)
+			return 0;
 
-	if (prot == MLX4_PROT_ETH)
-		gid[7] |= (MLX4_MC_STEER << 1);
+	case MLX4_STEERING_MODE_B0:
+		if (prot == MLX4_PROT_ETH)
+			gid[7] |= (MLX4_MC_STEER << 1);
 
-	if (mlx4_is_mfunc(dev))
-		return mlx4_QP_ATTACH(dev, qp, gid, 0, 0, prot);
+		if (mlx4_is_mfunc(dev))
+			return mlx4_QP_ATTACH(dev, qp, gid, 0, 0, prot);
+
+		return mlx4_qp_detach_common(dev, qp, gid, prot,
+					     MLX4_MC_STEER);
+
+	case MLX4_STEERING_MODE_DEVICE_MANAGED:
+		return mlx4_flow_detach(dev, reg_id);
 
-	return mlx4_qp_detach_common(dev, qp, gid, prot, MLX4_MC_STEER);
+	default:
+		return -EINVAL;
+	}
 }
 EXPORT_SYMBOL_GPL(mlx4_multicast_detach);
 
+int mlx4_flow_steer_promisc_add(struct mlx4_dev *dev, u8 port,
+				u32 qpn, enum mlx4_net_trans_promisc_mode mode)
+{
+	struct mlx4_net_trans_rule rule;
+	u64 *regid_p;
+
+	switch (mode) {
+	case MLX4_FS_PROMISC_UPLINK:
+	case MLX4_FS_PROMISC_FUNCTION_PORT:
+		regid_p = &dev->regid_promisc_array[port];
+		break;
+	case MLX4_FS_PROMISC_ALL_MULTI:
+		regid_p = &dev->regid_allmulti_array[port];
+		break;
+	default:
+		return -1;
+	}
+
+	if (*regid_p != 0)
+		return -1;
+
+	rule.promisc_mode = mode;
+	rule.port = port;
+	rule.qpn = qpn;
+	INIT_LIST_HEAD(&rule.list);
+	mlx4_err(dev, "going promisc on %x\n", port);
+
+	return  mlx4_flow_attach(dev, &rule, regid_p);
+}
+EXPORT_SYMBOL_GPL(mlx4_flow_steer_promisc_add);
+
+int mlx4_flow_steer_promisc_remove(struct mlx4_dev *dev, u8 port,
+				   enum mlx4_net_trans_promisc_mode mode)
+{
+	int ret;
+	u64 *regid_p;
+
+	switch (mode) {
+	case MLX4_FS_PROMISC_UPLINK:
+	case MLX4_FS_PROMISC_FUNCTION_PORT:
+		regid_p = &dev->regid_promisc_array[port];
+		break;
+	case MLX4_FS_PROMISC_ALL_MULTI:
+		regid_p = &dev->regid_allmulti_array[port];
+		break;
+	default:
+		return -1;
+	}
+
+	if (*regid_p == 0)
+		return -1;
+
+	ret =  mlx4_flow_detach(dev, *regid_p);
+	if (ret == 0)
+		*regid_p = 0;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mlx4_flow_steer_promisc_remove);
+
 int mlx4_unicast_attach(struct mlx4_dev *dev,
 			struct mlx4_qp *qp, u8 gid[16],
 			int block_mcast_loopback, enum mlx4_protocol prot)
 {
-	if (prot == MLX4_PROT_ETH &&
-			!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER))
-		return 0;
-
 	if (prot == MLX4_PROT_ETH)
 		gid[7] |= (MLX4_UC_STEER << 1);
 
@@ -924,10 +1373,6 @@ EXPORT_SYMBOL_GPL(mlx4_unicast_attach);
 int mlx4_unicast_detach(struct mlx4_dev *dev, struct mlx4_qp *qp,
 			       u8 gid[16], enum mlx4_protocol prot)
 {
-	if (prot == MLX4_PROT_ETH &&
-			!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER))
-		return 0;
-
 	if (prot == MLX4_PROT_ETH)
 		gid[7] |= (MLX4_UC_STEER << 1);
 
@@ -968,9 +1413,6 @@ static int mlx4_PROMISC(struct mlx4_dev *dev, u32 qpn,
 
 int mlx4_multicast_promisc_add(struct mlx4_dev *dev, u32 qpn, u8 port)
 {
-	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER))
-		return 0;
-
 	if (mlx4_is_mfunc(dev))
 		return mlx4_PROMISC(dev, qpn, MLX4_MC_STEER, 1, port);
 
@@ -980,9 +1422,6 @@ EXPORT_SYMBOL_GPL(mlx4_multicast_promisc_add);
 
 int mlx4_multicast_promisc_remove(struct mlx4_dev *dev, u32 qpn, u8 port)
 {
-	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER))
-		return 0;
-
 	if (mlx4_is_mfunc(dev))
 		return mlx4_PROMISC(dev, qpn, MLX4_MC_STEER, 0, port);
 
@@ -992,9 +1431,6 @@ EXPORT_SYMBOL_GPL(mlx4_multicast_promisc_remove);
 
 int mlx4_unicast_promisc_add(struct mlx4_dev *dev, u32 qpn, u8 port)
 {
-	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER))
-		return 0;
-
 	if (mlx4_is_mfunc(dev))
 		return mlx4_PROMISC(dev, qpn, MLX4_UC_STEER, 1, port);
 
@@ -1004,9 +1440,6 @@ EXPORT_SYMBOL_GPL(mlx4_unicast_promisc_add);
 
 int mlx4_unicast_promisc_remove(struct mlx4_dev *dev, u32 qpn, u8 port)
 {
-	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER))
-		return 0;
-
 	if (mlx4_is_mfunc(dev))
 		return mlx4_PROMISC(dev, qpn, MLX4_UC_STEER, 0, port);
 
@@ -1019,6 +1452,10 @@ int mlx4_init_mcg_table(struct mlx4_dev *dev)
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int err;
 
+	/* No need for mcg_table when fw managed the mcg table*/
+	if (dev->caps.steering_mode ==
+	    MLX4_STEERING_MODE_DEVICE_MANAGED)
+		return 0;
 	err = mlx4_bitmap_init(&priv->mcg_table.bitmap, dev->caps.num_amgms,
 			       dev->caps.num_amgms - 1, 0, 0);
 	if (err)
@@ -1031,5 +1468,7 @@ int mlx4_init_mcg_table(struct mlx4_dev *dev)
 
 void mlx4_cleanup_mcg_table(struct mlx4_dev *dev)
 {
-	mlx4_bitmap_cleanup(&mlx4_priv(dev)->mcg_table.bitmap);
+	if (dev->caps.steering_mode !=
+	    MLX4_STEERING_MODE_DEVICE_MANAGED)
+		mlx4_bitmap_cleanup(&mlx4_priv(dev)->mcg_table.bitmap);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index e5d20220762..59ebc033963 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -39,6 +39,7 @@
 
 #include <linux/mutex.h>
 #include <linux/radix-tree.h>
+#include <linux/rbtree.h>
 #include <linux/timer.h>
 #include <linux/semaphore.h>
 #include <linux/workqueue.h>
@@ -53,6 +54,17 @@
 #define DRV_VERSION	"1.1"
 #define DRV_RELDATE	"Dec, 2011"
 
+#define MLX4_FS_UDP_UC_EN		(1 << 1)
+#define MLX4_FS_TCP_UC_EN		(1 << 2)
+#define MLX4_FS_NUM_OF_L2_ADDR		8
+#define MLX4_FS_MGM_LOG_ENTRY_SIZE	7
+#define MLX4_FS_NUM_MCG			(1 << 17)
+
+enum {
+	MLX4_FS_L2_HASH = 0,
+	MLX4_FS_L2_L3_L4_HASH,
+};
+
 #define MLX4_NUM_UP		8
 #define MLX4_NUM_TC		8
 #define MLX4_RATELIMIT_UNITS 3 /* 100 Mbps */
@@ -137,6 +149,7 @@ enum mlx4_resource {
 	RES_VLAN,
 	RES_EQ,
 	RES_COUNTER,
+	RES_FS_RULE,
 	MLX4_NUM_OF_RESOURCE_TYPE
 };
 
@@ -338,66 +351,6 @@ struct mlx4_srq_context {
 	__be64			db_rec_addr;
 };
 
-struct mlx4_eqe {
-	u8			reserved1;
-	u8			type;
-	u8			reserved2;
-	u8			subtype;
-	union {
-		u32		raw[6];
-		struct {
-			__be32	cqn;
-		} __packed comp;
-		struct {
-			u16	reserved1;
-			__be16	token;
-			u32	reserved2;
-			u8	reserved3[3];
-			u8	status;
-			__be64	out_param;
-		} __packed cmd;
-		struct {
-			__be32	qpn;
-		} __packed qp;
-		struct {
-			__be32	srqn;
-		} __packed srq;
-		struct {
-			__be32	cqn;
-			u32	reserved1;
-			u8	reserved2[3];
-			u8	syndrome;
-		} __packed cq_err;
-		struct {
-			u32	reserved1[2];
-			__be32	port;
-		} __packed port_change;
-		struct {
-			#define COMM_CHANNEL_BIT_ARRAY_SIZE	4
-			u32 reserved;
-			u32 bit_vec[COMM_CHANNEL_BIT_ARRAY_SIZE];
-		} __packed comm_channel_arm;
-		struct {
-			u8	port;
-			u8	reserved[3];
-			__be64	mac;
-		} __packed mac_update;
-		struct {
-			u8	port;
-		} __packed sw_event;
-		struct {
-			__be32	slave_id;
-		} __packed flr_event;
-		struct {
-			__be16  current_temperature;
-			__be16  warning_threshold;
-		} __packed warming;
-	}			event;
-	u8			slave_id;
-	u8			reserved3[2];
-	u8			owner;
-} __packed;
-
 struct mlx4_eq {
 	struct mlx4_dev	       *dev;
 	void __iomem	       *doorbell;
@@ -509,7 +462,7 @@ struct slave_list {
 struct mlx4_resource_tracker {
 	spinlock_t lock;
 	/* tree for each resources */
-	struct radix_tree_root res_tree[MLX4_NUM_OF_RESOURCE_TYPE];
+	struct rb_root res_tree[MLX4_NUM_OF_RESOURCE_TYPE];
 	/* num_of_slave's lists, one per slave */
 	struct slave_list *slave_list;
 };
@@ -703,6 +656,7 @@ struct mlx4_set_port_rqp_calc_context {
 
 struct mlx4_mac_entry {
 	u64 mac;
+	u64 reg_id;
 };
 
 struct mlx4_port_info {
@@ -776,6 +730,7 @@ struct mlx4_priv {
 	struct mutex		bf_mutex;
 	struct io_mapping	*bf_mapping;
 	int			reserved_mtts;
+	int			fs_hash_mode;
 };
 
 static inline struct mlx4_priv *mlx4_priv(struct mlx4_dev *dev)
@@ -887,7 +842,8 @@ void mlx4_catas_init(void);
 int mlx4_restart_one(struct pci_dev *pdev);
 int mlx4_register_device(struct mlx4_dev *dev);
 void mlx4_unregister_device(struct mlx4_dev *dev);
-void mlx4_dispatch_event(struct mlx4_dev *dev, enum mlx4_dev_event type, int port);
+void mlx4_dispatch_event(struct mlx4_dev *dev, enum mlx4_dev_event type,
+			 unsigned long param);
 
 struct mlx4_dev_cap;
 struct mlx4_init_hca_param;
@@ -1028,11 +984,11 @@ int mlx4_change_port_types(struct mlx4_dev *dev,
 void mlx4_init_mac_table(struct mlx4_dev *dev, struct mlx4_mac_table *table);
 void mlx4_init_vlan_table(struct mlx4_dev *dev, struct mlx4_vlan_table *table);
 
-int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port);
+int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port, int pkey_tbl_sz);
 /* resource tracker functions*/
 int mlx4_get_slave_from_resource_id(struct mlx4_dev *dev,
 				    enum mlx4_resource resource_type,
-				    int resource_id, int *slave);
+				    u64 resource_id, int *slave);
 void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave_id);
 int mlx4_init_resource_tracker(struct mlx4_dev *dev);
 
@@ -1071,6 +1027,8 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 			    struct mlx4_cmd_info *cmd);
 int mlx4_get_port_ib_caps(struct mlx4_dev *dev, u8 port, __be32 *caps);
 
+int mlx4_get_slave_pkey_gid_tbl_len(struct mlx4_dev *dev, u8 port,
+				    int *gid_tbl_len, int *pkey_tbl_len);
 
 int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 			   struct mlx4_vhcr *vhcr,
@@ -1117,6 +1075,16 @@ int mlx4_QUERY_IF_STAT_wrapper(struct mlx4_dev *dev, int slave,
 			       struct mlx4_cmd_mailbox *inbox,
 			       struct mlx4_cmd_mailbox *outbox,
 			       struct mlx4_cmd_info *cmd);
+int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
+					 struct mlx4_vhcr *vhcr,
+					 struct mlx4_cmd_mailbox *inbox,
+					 struct mlx4_cmd_mailbox *outbox,
+					 struct mlx4_cmd_info *cmd);
+int mlx4_QP_FLOW_STEERING_DETACH_wrapper(struct mlx4_dev *dev, int slave,
+					 struct mlx4_vhcr *vhcr,
+					 struct mlx4_cmd_mailbox *inbox,
+					 struct mlx4_cmd_mailbox *outbox,
+					 struct mlx4_cmd_info *cmd);
 
 int mlx4_get_mgm_entry_size(struct mlx4_dev *dev);
 int mlx4_get_qp_per_mgm(struct mlx4_dev *dev);
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 225c20d4790..5f1ab105deb 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -43,6 +43,7 @@
 #ifdef CONFIG_MLX4_EN_DCB
 #include <linux/dcbnl.h>
 #endif
+#include <linux/cpu_rmap.h>
 
 #include <linux/mlx4/device.h>
 #include <linux/mlx4/qp.h>
@@ -75,6 +76,10 @@
 #define STAMP_SHIFT		31
 #define STAMP_VAL		0x7fffffff
 #define STATS_DELAY		(HZ / 4)
+#define MAX_NUM_OF_FS_RULES	256
+
+#define MLX4_EN_FILTER_HASH_SHIFT 4
+#define MLX4_EN_FILTER_EXPIRY_QUOTA 60
 
 /* Typical TSO descriptor with 16 gather entries is 352 bytes... */
 #define MAX_DESC_SIZE		512
@@ -106,7 +111,7 @@ enum {
 #define MLX4_EN_MAX_TX_SIZE	8192
 #define MLX4_EN_MAX_RX_SIZE	8192
 
-/* Minimum ring size for our page-allocation sceme to work */
+/* Minimum ring size for our page-allocation scheme to work */
 #define MLX4_EN_MIN_RX_SIZE	(MLX4_EN_ALLOC_SIZE / SMP_CACHE_BYTES)
 #define MLX4_EN_MIN_TX_SIZE	(4096 / TXBB_SIZE)
 
@@ -227,6 +232,7 @@ struct mlx4_en_tx_desc {
 
 struct mlx4_en_rx_alloc {
 	struct page *page;
+	dma_addr_t dma;
 	u16 offset;
 };
 
@@ -404,6 +410,19 @@ struct mlx4_en_perf_stats {
 #define NUM_PERF_COUNTERS		6
 };
 
+enum mlx4_en_mclist_act {
+	MCLIST_NONE,
+	MCLIST_REM,
+	MCLIST_ADD,
+};
+
+struct mlx4_en_mc_list {
+	struct list_head	list;
+	enum mlx4_en_mclist_act	action;
+	u8			addr[ETH_ALEN];
+	u64			reg_id;
+};
+
 struct mlx4_en_frag_info {
 	u16 frag_size;
 	u16 frag_prefix_size;
@@ -422,6 +441,11 @@ struct mlx4_en_frag_info {
 
 #endif
 
+struct ethtool_flow_id {
+	struct ethtool_rx_flow_spec flow_spec;
+	u64 id;
+};
+
 struct mlx4_en_priv {
 	struct mlx4_en_dev *mdev;
 	struct mlx4_en_port_profile *prof;
@@ -431,6 +455,7 @@ struct mlx4_en_priv {
 	struct net_device_stats ret_stats;
 	struct mlx4_en_port_state port_state;
 	spinlock_t stats_lock;
+	struct ethtool_flow_id ethtool_rules[MAX_NUM_OF_FS_RULES];
 
 	unsigned long last_moder_packets[MAX_RX_RINGS];
 	unsigned long last_moder_tx_packets;
@@ -480,6 +505,7 @@ struct mlx4_en_priv {
 	struct mlx4_en_rx_ring rx_ring[MAX_RX_RINGS];
 	struct mlx4_en_cq *tx_cq;
 	struct mlx4_en_cq rx_cq[MAX_RX_RINGS];
+	struct mlx4_qp drop_qp;
 	struct work_struct mcast_task;
 	struct work_struct mac_task;
 	struct work_struct watchdog_task;
@@ -489,8 +515,9 @@ struct mlx4_en_priv {
 	struct mlx4_en_pkt_stats pkstats;
 	struct mlx4_en_port_stats port_stats;
 	u64 stats_bitmap;
-	char *mc_addrs;
-	int mc_addrs_cnt;
+	struct list_head mc_list;
+	struct list_head curr_list;
+	u64 broadcast_id;
 	struct mlx4_en_stat_out_mbox hw_stats;
 	int vids[128];
 	bool wol;
@@ -501,6 +528,13 @@ struct mlx4_en_priv {
 	struct ieee_ets ets;
 	u16 maxrate[IEEE_8021QAZ_MAX_TCS];
 #endif
+#ifdef CONFIG_RFS_ACCEL
+	spinlock_t filters_lock;
+	int last_filter_id;
+	struct list_head filters;
+	struct hlist_head filter_hash[1 << MLX4_EN_FILTER_HASH_SHIFT];
+#endif
+
 };
 
 enum mlx4_en_wol {
@@ -565,6 +599,8 @@ void mlx4_en_unmap_buffer(struct mlx4_buf *buf);
 void mlx4_en_calc_rx_buf(struct net_device *dev);
 int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv);
 void mlx4_en_release_rss_steer(struct mlx4_en_priv *priv);
+int mlx4_en_create_drop_qp(struct mlx4_en_priv *priv);
+void mlx4_en_destroy_drop_qp(struct mlx4_en_priv *priv);
 int mlx4_en_free_tx_buf(struct net_device *dev, struct mlx4_en_tx_ring *ring);
 void mlx4_en_rx_irq(struct mlx4_cq *mcq);
 
@@ -578,6 +614,11 @@ int mlx4_en_QUERY_PORT(struct mlx4_en_dev *mdev, u8 port);
 extern const struct dcbnl_rtnl_ops mlx4_en_dcbnl_ops;
 #endif
 
+#ifdef CONFIG_RFS_ACCEL
+void mlx4_en_cleanup_filters(struct mlx4_en_priv *priv,
+			     struct mlx4_en_rx_ring *rx_ring);
+#endif
+
 #define MLX4_EN_NUM_SELF_TEST	5
 void mlx4_en_ex_selftest(struct net_device *dev, u32 *flags, u64 *buf);
 u64 mlx4_en_mac_to_u64(u8 *addr);
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index a8fb52992c6..e36dd0f2fa7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -39,7 +39,6 @@
 #include "mlx4.h"
 
 #define MLX4_MAC_VALID		(1ull << 63)
-#define MLX4_MAC_MASK		0xffffffffffffULL
 
 #define MLX4_VLAN_VALID		(1u << 31)
 #define MLX4_VLAN_MASK		0xfff
@@ -75,21 +74,54 @@ void mlx4_init_vlan_table(struct mlx4_dev *dev, struct mlx4_vlan_table *table)
 	table->total = 0;
 }
 
-static int mlx4_uc_steer_add(struct mlx4_dev *dev, u8 port, u64 mac, int *qpn)
+static int mlx4_uc_steer_add(struct mlx4_dev *dev, u8 port,
+			     u64 mac, int *qpn, u64 *reg_id)
 {
-	struct mlx4_qp qp;
-	u8 gid[16] = {0};
 	__be64 be_mac;
 	int err;
 
-	qp.qpn = *qpn;
-
-	mac &= 0xffffffffffffULL;
+	mac &= MLX4_MAC_MASK;
 	be_mac = cpu_to_be64(mac << 16);
-	memcpy(&gid[10], &be_mac, ETH_ALEN);
-	gid[5] = port;
 
-	err = mlx4_unicast_attach(dev, &qp, gid, 0, MLX4_PROT_ETH);
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_B0: {
+		struct mlx4_qp qp;
+		u8 gid[16] = {0};
+
+		qp.qpn = *qpn;
+		memcpy(&gid[10], &be_mac, ETH_ALEN);
+		gid[5] = port;
+
+		err = mlx4_unicast_attach(dev, &qp, gid, 0, MLX4_PROT_ETH);
+		break;
+	}
+	case MLX4_STEERING_MODE_DEVICE_MANAGED: {
+		struct mlx4_spec_list spec_eth = { {NULL} };
+		__be64 mac_mask = cpu_to_be64(MLX4_MAC_MASK << 16);
+
+		struct mlx4_net_trans_rule rule = {
+			.queue_mode = MLX4_NET_TRANS_Q_FIFO,
+			.exclusive = 0,
+			.allow_loopback = 1,
+			.promisc_mode = MLX4_FS_PROMISC_NONE,
+			.priority = MLX4_DOMAIN_NIC,
+		};
+
+		rule.port = port;
+		rule.qpn = *qpn;
+		INIT_LIST_HEAD(&rule.list);
+
+		spec_eth.id = MLX4_NET_TRANS_RULE_ID_ETH;
+		memcpy(spec_eth.eth.dst_mac, &be_mac, ETH_ALEN);
+		memcpy(spec_eth.eth.dst_mac_msk, &mac_mask, ETH_ALEN);
+		list_add_tail(&spec_eth.list, &rule.list);
+
+		err = mlx4_flow_attach(dev, &rule, reg_id);
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
 	if (err)
 		mlx4_warn(dev, "Failed Attaching Unicast\n");
 
@@ -97,19 +129,30 @@ static int mlx4_uc_steer_add(struct mlx4_dev *dev, u8 port, u64 mac, int *qpn)
 }
 
 static void mlx4_uc_steer_release(struct mlx4_dev *dev, u8 port,
-				  u64 mac, int qpn)
+				  u64 mac, int qpn, u64 reg_id)
 {
-	struct mlx4_qp qp;
-	u8 gid[16] = {0};
-	__be64 be_mac;
+	switch (dev->caps.steering_mode) {
+	case MLX4_STEERING_MODE_B0: {
+		struct mlx4_qp qp;
+		u8 gid[16] = {0};
+		__be64 be_mac;
 
-	qp.qpn = qpn;
-	mac &= 0xffffffffffffULL;
-	be_mac = cpu_to_be64(mac << 16);
-	memcpy(&gid[10], &be_mac, ETH_ALEN);
-	gid[5] = port;
+		qp.qpn = qpn;
+		mac &= MLX4_MAC_MASK;
+		be_mac = cpu_to_be64(mac << 16);
+		memcpy(&gid[10], &be_mac, ETH_ALEN);
+		gid[5] = port;
 
-	mlx4_unicast_detach(dev, &qp, gid, MLX4_PROT_ETH);
+		mlx4_unicast_detach(dev, &qp, gid, MLX4_PROT_ETH);
+		break;
+	}
+	case MLX4_STEERING_MODE_DEVICE_MANAGED: {
+		mlx4_flow_detach(dev, reg_id);
+		break;
+	}
+	default:
+		mlx4_err(dev, "Invalid steering mode.\n");
+	}
 }
 
 static int validate_index(struct mlx4_dev *dev,
@@ -144,6 +187,7 @@ int mlx4_get_eth_qp(struct mlx4_dev *dev, u8 port, u64 mac, int *qpn)
 	struct mlx4_mac_entry *entry;
 	int index = 0;
 	int err = 0;
+	u64 reg_id;
 
 	mlx4_dbg(dev, "Registering MAC: 0x%llx for adding\n",
 			(unsigned long long) mac);
@@ -155,7 +199,7 @@ int mlx4_get_eth_qp(struct mlx4_dev *dev, u8 port, u64 mac, int *qpn)
 		return err;
 	}
 
-	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER)) {
+	if (dev->caps.steering_mode == MLX4_STEERING_MODE_A0) {
 		*qpn = info->base_qpn + index;
 		return 0;
 	}
@@ -167,7 +211,7 @@ int mlx4_get_eth_qp(struct mlx4_dev *dev, u8 port, u64 mac, int *qpn)
 		goto qp_err;
 	}
 
-	err = mlx4_uc_steer_add(dev, port, mac, qpn);
+	err = mlx4_uc_steer_add(dev, port, mac, qpn, &reg_id);
 	if (err)
 		goto steer_err;
 
@@ -177,6 +221,7 @@ int mlx4_get_eth_qp(struct mlx4_dev *dev, u8 port, u64 mac, int *qpn)
 		goto alloc_err;
 	}
 	entry->mac = mac;
+	entry->reg_id = reg_id;
 	err = radix_tree_insert(&info->mac_tree, *qpn, entry);
 	if (err)
 		goto insert_err;
@@ -186,7 +231,7 @@ insert_err:
 	kfree(entry);
 
 alloc_err:
-	mlx4_uc_steer_release(dev, port, mac, *qpn);
+	mlx4_uc_steer_release(dev, port, mac, *qpn, reg_id);
 
 steer_err:
 	mlx4_qp_release_range(dev, *qpn, 1);
@@ -206,13 +251,14 @@ void mlx4_put_eth_qp(struct mlx4_dev *dev, u8 port, u64 mac, int qpn)
 		 (unsigned long long) mac);
 	mlx4_unregister_mac(dev, port, mac);
 
-	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER) {
+	if (dev->caps.steering_mode != MLX4_STEERING_MODE_A0) {
 		entry = radix_tree_lookup(&info->mac_tree, qpn);
 		if (entry) {
 			mlx4_dbg(dev, "Releasing qp: port %d, mac 0x%llx,"
 				 " qpn %d\n", port,
 				 (unsigned long long) mac, qpn);
-			mlx4_uc_steer_release(dev, port, entry->mac, qpn);
+			mlx4_uc_steer_release(dev, port, entry->mac,
+					      qpn, entry->reg_id);
 			mlx4_qp_release_range(dev, qpn, 1);
 			radix_tree_delete(&info->mac_tree, qpn);
 			kfree(entry);
@@ -359,15 +405,18 @@ int mlx4_replace_mac(struct mlx4_dev *dev, u8 port, int qpn, u64 new_mac)
 	int index = qpn - info->base_qpn;
 	int err = 0;
 
-	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER) {
+	if (dev->caps.steering_mode != MLX4_STEERING_MODE_A0) {
 		entry = radix_tree_lookup(&info->mac_tree, qpn);
 		if (!entry)
 			return -EINVAL;
-		mlx4_uc_steer_release(dev, port, entry->mac, qpn);
+		mlx4_uc_steer_release(dev, port, entry->mac,
+				      qpn, entry->reg_id);
 		mlx4_unregister_mac(dev, port, entry->mac);
 		entry->mac = new_mac;
+		entry->reg_id = 0;
 		mlx4_register_mac(dev, port, new_mac);
-		err = mlx4_uc_steer_add(dev, port, entry->mac, &qpn);
+		err = mlx4_uc_steer_add(dev, port, entry->mac,
+					&qpn, &entry->reg_id);
 		return err;
 	}
 
@@ -726,14 +775,15 @@ int mlx4_SET_PORT_wrapper(struct mlx4_dev *dev, int slave,
 enum {
 	MLX4_SET_PORT_VL_CAP	 = 4, /* bits 7:4 */
 	MLX4_SET_PORT_MTU_CAP	 = 12, /* bits 15:12 */
+	MLX4_CHANGE_PORT_PKEY_TBL_SZ = 20,
 	MLX4_CHANGE_PORT_VL_CAP	 = 21,
 	MLX4_CHANGE_PORT_MTU_CAP = 22,
 };
 
-int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port)
+int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port, int pkey_tbl_sz)
 {
 	struct mlx4_cmd_mailbox *mailbox;
-	int err, vl_cap;
+	int err, vl_cap, pkey_tbl_flag = 0;
 
 	if (dev->caps.port_type[port] == MLX4_PORT_TYPE_ETH)
 		return 0;
@@ -746,11 +796,17 @@ int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port)
 
 	((__be32 *) mailbox->buf)[1] = dev->caps.ib_port_def_cap[port];
 
+	if (pkey_tbl_sz >= 0 && mlx4_is_master(dev)) {
+		pkey_tbl_flag = 1;
+		((__be16 *) mailbox->buf)[20] = cpu_to_be16(pkey_tbl_sz);
+	}
+
 	/* IB VL CAP enum isn't used by the firmware, just numerical values */
 	for (vl_cap = 8; vl_cap >= 1; vl_cap >>= 1) {
 		((__be32 *) mailbox->buf)[0] = cpu_to_be32(
 			(1 << MLX4_CHANGE_PORT_MTU_CAP) |
 			(1 << MLX4_CHANGE_PORT_VL_CAP)  |
+			(pkey_tbl_flag << MLX4_CHANGE_PORT_PKEY_TBL_SZ) |
 			(dev->caps.port_ib_mtu[port] << MLX4_SET_PORT_MTU_CAP) |
 			(vl_cap << MLX4_SET_PORT_VL_CAP));
 		err = mlx4_cmd(dev, mailbox->dma, port, 0, MLX4_CMD_SET_PORT,
@@ -803,8 +859,7 @@ int mlx4_SET_PORT_qpn_calc(struct mlx4_dev *dev, u8 port, u32 base_qpn,
 	u32 m_promisc = (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER) ?
 		MCAST_DIRECT : MCAST_DEFAULT;
 
-	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_MC_STEER  &&
-	    dev->caps.flags & MLX4_DEV_CAP_FLAG_VEP_UC_STEER)
+	if (dev->caps.steering_mode != MLX4_STEERING_MODE_A0)
 		return 0;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
diff --git a/drivers/net/ethernet/mellanox/mlx4/profile.c b/drivers/net/ethernet/mellanox/mlx4/profile.c
index b83bc928d52..9ee4725363d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/profile.c
+++ b/drivers/net/ethernet/mellanox/mlx4/profile.c
@@ -237,13 +237,19 @@ u64 mlx4_make_profile(struct mlx4_dev *dev,
 			init_hca->mtt_base	 = profile[i].start;
 			break;
 		case MLX4_RES_MCG:
-			dev->caps.num_mgms	  = profile[i].num >> 1;
-			dev->caps.num_amgms	  = profile[i].num >> 1;
 			init_hca->mc_base	  = profile[i].start;
 			init_hca->log_mc_entry_sz =
 					ilog2(mlx4_get_mgm_entry_size(dev));
 			init_hca->log_mc_table_sz = profile[i].log_num;
-			init_hca->log_mc_hash_sz  = profile[i].log_num - 1;
+			if (dev->caps.steering_mode ==
+			    MLX4_STEERING_MODE_DEVICE_MANAGED) {
+				dev->caps.num_mgms = profile[i].num;
+			} else {
+				init_hca->log_mc_hash_sz =
+						profile[i].log_num - 1;
+				dev->caps.num_mgms = profile[i].num >> 1;
+				dev->caps.num_amgms = profile[i].num >> 1;
+			}
 			break;
 		default:
 			break;
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index b45d0e7f6ab..94ceddd17ab 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -41,13 +41,12 @@
 #include <linux/slab.h>
 #include <linux/mlx4/cmd.h>
 #include <linux/mlx4/qp.h>
+#include <linux/if_ether.h>
 
 #include "mlx4.h"
 #include "fw.h"
 
 #define MLX4_MAC_VALID		(1ull << 63)
-#define MLX4_MAC_MASK		0x7fffffffffffffffULL
-#define ETH_ALEN		6
 
 struct mac_res {
 	struct list_head list;
@@ -57,7 +56,8 @@ struct mac_res {
 
 struct res_common {
 	struct list_head	list;
-	u32		        res_id;
+	struct rb_node		node;
+	u64		        res_id;
 	int			owner;
 	int			state;
 	int			from_state;
@@ -189,6 +189,58 @@ struct res_xrcdn {
 	int			port;
 };
 
+enum res_fs_rule_states {
+	RES_FS_RULE_BUSY = RES_ANY_BUSY,
+	RES_FS_RULE_ALLOCATED,
+};
+
+struct res_fs_rule {
+	struct res_common	com;
+};
+
+static void *res_tracker_lookup(struct rb_root *root, u64 res_id)
+{
+	struct rb_node *node = root->rb_node;
+
+	while (node) {
+		struct res_common *res = container_of(node, struct res_common,
+						      node);
+
+		if (res_id < res->res_id)
+			node = node->rb_left;
+		else if (res_id > res->res_id)
+			node = node->rb_right;
+		else
+			return res;
+	}
+	return NULL;
+}
+
+static int res_tracker_insert(struct rb_root *root, struct res_common *res)
+{
+	struct rb_node **new = &(root->rb_node), *parent = NULL;
+
+	/* Figure out where to put new node */
+	while (*new) {
+		struct res_common *this = container_of(*new, struct res_common,
+						       node);
+
+		parent = *new;
+		if (res->res_id < this->res_id)
+			new = &((*new)->rb_left);
+		else if (res->res_id > this->res_id)
+			new = &((*new)->rb_right);
+		else
+			return -EEXIST;
+	}
+
+	/* Add new node and rebalance tree. */
+	rb_link_node(&res->node, parent, new);
+	rb_insert_color(&res->node, root);
+
+	return 0;
+}
+
 /* For Debug uses */
 static const char *ResourceType(enum mlx4_resource rt)
 {
@@ -201,6 +253,7 @@ static const char *ResourceType(enum mlx4_resource rt)
 	case RES_MAC: return  "RES_MAC";
 	case RES_EQ: return "RES_EQ";
 	case RES_COUNTER: return "RES_COUNTER";
+	case RES_FS_RULE: return "RES_FS_RULE";
 	case RES_XRCD: return "RES_XRCD";
 	default: return "Unknown resource type !!!";
 	};
@@ -228,8 +281,7 @@ int mlx4_init_resource_tracker(struct mlx4_dev *dev)
 	mlx4_dbg(dev, "Started init_resource_tracker: %ld slaves\n",
 		 dev->num_slaves);
 	for (i = 0 ; i < MLX4_NUM_OF_RESOURCE_TYPE; i++)
-		INIT_RADIX_TREE(&priv->mfunc.master.res_tracker.res_tree[i],
-				GFP_ATOMIC|__GFP_NOWARN);
+		priv->mfunc.master.res_tracker.res_tree[i] = RB_ROOT;
 
 	spin_lock_init(&priv->mfunc.master.res_tracker.lock);
 	return 0 ;
@@ -277,11 +329,11 @@ static void *find_res(struct mlx4_dev *dev, int res_id,
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
-	return radix_tree_lookup(&priv->mfunc.master.res_tracker.res_tree[type],
-				 res_id);
+	return res_tracker_lookup(&priv->mfunc.master.res_tracker.res_tree[type],
+				  res_id);
 }
 
-static int get_res(struct mlx4_dev *dev, int slave, int res_id,
+static int get_res(struct mlx4_dev *dev, int slave, u64 res_id,
 		   enum mlx4_resource type,
 		   void *res)
 {
@@ -307,7 +359,7 @@ static int get_res(struct mlx4_dev *dev, int slave, int res_id,
 
 	r->from_state = r->state;
 	r->state = RES_ANY_BUSY;
-	mlx4_dbg(dev, "res %s id 0x%x to busy\n",
+	mlx4_dbg(dev, "res %s id 0x%llx to busy\n",
 		 ResourceType(type), r->res_id);
 
 	if (res)
@@ -320,7 +372,7 @@ exit:
 
 int mlx4_get_slave_from_resource_id(struct mlx4_dev *dev,
 				    enum mlx4_resource type,
-				    int res_id, int *slave)
+				    u64 res_id, int *slave)
 {
 
 	struct res_common *r;
@@ -341,7 +393,7 @@ int mlx4_get_slave_from_resource_id(struct mlx4_dev *dev,
 	return err;
 }
 
-static void put_res(struct mlx4_dev *dev, int slave, int res_id,
+static void put_res(struct mlx4_dev *dev, int slave, u64 res_id,
 		    enum mlx4_resource type)
 {
 	struct res_common *r;
@@ -473,7 +525,21 @@ static struct res_common *alloc_xrcdn_tr(int id)
 	return &ret->com;
 }
 
-static struct res_common *alloc_tr(int id, enum mlx4_resource type, int slave,
+static struct res_common *alloc_fs_rule_tr(u64 id)
+{
+	struct res_fs_rule *ret;
+
+	ret = kzalloc(sizeof *ret, GFP_KERNEL);
+	if (!ret)
+		return NULL;
+
+	ret->com.res_id = id;
+	ret->com.state = RES_FS_RULE_ALLOCATED;
+
+	return &ret->com;
+}
+
+static struct res_common *alloc_tr(u64 id, enum mlx4_resource type, int slave,
 				   int extra)
 {
 	struct res_common *ret;
@@ -506,6 +572,9 @@ static struct res_common *alloc_tr(int id, enum mlx4_resource type, int slave,
 	case RES_XRCD:
 		ret = alloc_xrcdn_tr(id);
 		break;
+	case RES_FS_RULE:
+		ret = alloc_fs_rule_tr(id);
+		break;
 	default:
 		return NULL;
 	}
@@ -515,7 +584,7 @@ static struct res_common *alloc_tr(int id, enum mlx4_resource type, int slave,
 	return ret;
 }
 
-static int add_res_range(struct mlx4_dev *dev, int slave, int base, int count,
+static int add_res_range(struct mlx4_dev *dev, int slave, u64 base, int count,
 			 enum mlx4_resource type, int extra)
 {
 	int i;
@@ -523,7 +592,7 @@ static int add_res_range(struct mlx4_dev *dev, int slave, int base, int count,
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct res_common **res_arr;
 	struct mlx4_resource_tracker *tracker = &priv->mfunc.master.res_tracker;
-	struct radix_tree_root *root = &tracker->res_tree[type];
+	struct rb_root *root = &tracker->res_tree[type];
 
 	res_arr = kzalloc(count * sizeof *res_arr, GFP_KERNEL);
 	if (!res_arr)
@@ -546,7 +615,7 @@ static int add_res_range(struct mlx4_dev *dev, int slave, int base, int count,
 			err = -EEXIST;
 			goto undo;
 		}
-		err = radix_tree_insert(root, base + i, res_arr[i]);
+		err = res_tracker_insert(root, res_arr[i]);
 		if (err)
 			goto undo;
 		list_add_tail(&res_arr[i]->list,
@@ -559,7 +628,7 @@ static int add_res_range(struct mlx4_dev *dev, int slave, int base, int count,
 
 undo:
 	for (--i; i >= base; --i)
-		radix_tree_delete(&tracker->res_tree[type], i);
+		rb_erase(&res_arr[i]->node, root);
 
 	spin_unlock_irq(mlx4_tlock(dev));
 
@@ -638,6 +707,16 @@ static int remove_xrcdn_ok(struct res_xrcdn *res)
 	return 0;
 }
 
+static int remove_fs_rule_ok(struct res_fs_rule *res)
+{
+	if (res->com.state == RES_FS_RULE_BUSY)
+		return -EBUSY;
+	else if (res->com.state != RES_FS_RULE_ALLOCATED)
+		return -EPERM;
+
+	return 0;
+}
+
 static int remove_cq_ok(struct res_cq *res)
 {
 	if (res->com.state == RES_CQ_BUSY)
@@ -679,15 +758,17 @@ static int remove_ok(struct res_common *res, enum mlx4_resource type, int extra)
 		return remove_counter_ok((struct res_counter *)res);
 	case RES_XRCD:
 		return remove_xrcdn_ok((struct res_xrcdn *)res);
+	case RES_FS_RULE:
+		return remove_fs_rule_ok((struct res_fs_rule *)res);
 	default:
 		return -EINVAL;
 	}
 }
 
-static int rem_res_range(struct mlx4_dev *dev, int slave, int base, int count,
+static int rem_res_range(struct mlx4_dev *dev, int slave, u64 base, int count,
 			 enum mlx4_resource type, int extra)
 {
-	int i;
+	u64 i;
 	int err;
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct mlx4_resource_tracker *tracker = &priv->mfunc.master.res_tracker;
@@ -695,7 +776,7 @@ static int rem_res_range(struct mlx4_dev *dev, int slave, int base, int count,
 
 	spin_lock_irq(mlx4_tlock(dev));
 	for (i = base; i < base + count; ++i) {
-		r = radix_tree_lookup(&tracker->res_tree[type], i);
+		r = res_tracker_lookup(&tracker->res_tree[type], i);
 		if (!r) {
 			err = -ENOENT;
 			goto out;
@@ -710,8 +791,8 @@ static int rem_res_range(struct mlx4_dev *dev, int slave, int base, int count,
 	}
 
 	for (i = base; i < base + count; ++i) {
-		r = radix_tree_lookup(&tracker->res_tree[type], i);
-		radix_tree_delete(&tracker->res_tree[type], i);
+		r = res_tracker_lookup(&tracker->res_tree[type], i);
+		rb_erase(&r->node, &tracker->res_tree[type]);
 		list_del(&r->list);
 		kfree(r);
 	}
@@ -733,7 +814,7 @@ static int qp_res_start_move_to(struct mlx4_dev *dev, int slave, int qpn,
 	int err = 0;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[RES_QP], qpn);
+	r = res_tracker_lookup(&tracker->res_tree[RES_QP], qpn);
 	if (!r)
 		err = -ENOENT;
 	else if (r->com.owner != slave)
@@ -741,7 +822,7 @@ static int qp_res_start_move_to(struct mlx4_dev *dev, int slave, int qpn,
 	else {
 		switch (state) {
 		case RES_QP_BUSY:
-			mlx4_dbg(dev, "%s: failed RES_QP, 0x%x\n",
+			mlx4_dbg(dev, "%s: failed RES_QP, 0x%llx\n",
 				 __func__, r->com.res_id);
 			err = -EBUSY;
 			break;
@@ -750,7 +831,7 @@ static int qp_res_start_move_to(struct mlx4_dev *dev, int slave, int qpn,
 			if (r->com.state == RES_QP_MAPPED && !alloc)
 				break;
 
-			mlx4_dbg(dev, "failed RES_QP, 0x%x\n", r->com.res_id);
+			mlx4_dbg(dev, "failed RES_QP, 0x%llx\n", r->com.res_id);
 			err = -EINVAL;
 			break;
 
@@ -759,7 +840,7 @@ static int qp_res_start_move_to(struct mlx4_dev *dev, int slave, int qpn,
 			    r->com.state == RES_QP_HW)
 				break;
 			else {
-				mlx4_dbg(dev, "failed RES_QP, 0x%x\n",
+				mlx4_dbg(dev, "failed RES_QP, 0x%llx\n",
 					  r->com.res_id);
 				err = -EINVAL;
 			}
@@ -779,7 +860,7 @@ static int qp_res_start_move_to(struct mlx4_dev *dev, int slave, int qpn,
 			r->com.to_state = state;
 			r->com.state = RES_QP_BUSY;
 			if (qp)
-				*qp = (struct res_qp *)r;
+				*qp = r;
 		}
 	}
 
@@ -797,7 +878,7 @@ static int mr_res_start_move_to(struct mlx4_dev *dev, int slave, int index,
 	int err = 0;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[RES_MPT], index);
+	r = res_tracker_lookup(&tracker->res_tree[RES_MPT], index);
 	if (!r)
 		err = -ENOENT;
 	else if (r->com.owner != slave)
@@ -832,7 +913,7 @@ static int mr_res_start_move_to(struct mlx4_dev *dev, int slave, int index,
 			r->com.to_state = state;
 			r->com.state = RES_MPT_BUSY;
 			if (mpt)
-				*mpt = (struct res_mpt *)r;
+				*mpt = r;
 		}
 	}
 
@@ -850,7 +931,7 @@ static int eq_res_start_move_to(struct mlx4_dev *dev, int slave, int index,
 	int err = 0;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[RES_EQ], index);
+	r = res_tracker_lookup(&tracker->res_tree[RES_EQ], index);
 	if (!r)
 		err = -ENOENT;
 	else if (r->com.owner != slave)
@@ -898,7 +979,7 @@ static int cq_res_start_move_to(struct mlx4_dev *dev, int slave, int cqn,
 	int err;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[RES_CQ], cqn);
+	r = res_tracker_lookup(&tracker->res_tree[RES_CQ], cqn);
 	if (!r)
 		err = -ENOENT;
 	else if (r->com.owner != slave)
@@ -952,7 +1033,7 @@ static int srq_res_start_move_to(struct mlx4_dev *dev, int slave, int index,
 	int err = 0;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[RES_SRQ], index);
+	r = res_tracker_lookup(&tracker->res_tree[RES_SRQ], index);
 	if (!r)
 		err = -ENOENT;
 	else if (r->com.owner != slave)
@@ -1001,7 +1082,7 @@ static void res_abort_move(struct mlx4_dev *dev, int slave,
 	struct res_common *r;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[type], id);
+	r = res_tracker_lookup(&tracker->res_tree[type], id);
 	if (r && (r->owner == slave))
 		r->state = r->from_state;
 	spin_unlock_irq(mlx4_tlock(dev));
@@ -1015,7 +1096,7 @@ static void res_end_move(struct mlx4_dev *dev, int slave,
 	struct res_common *r;
 
 	spin_lock_irq(mlx4_tlock(dev));
-	r = radix_tree_lookup(&tracker->res_tree[type], id);
+	r = res_tracker_lookup(&tracker->res_tree[type], id);
 	if (r && (r->owner == slave))
 		r->state = r->to_state;
 	spin_unlock_irq(mlx4_tlock(dev));
@@ -2695,6 +2776,60 @@ ex_put:
 	return err;
 }
 
+int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
+					 struct mlx4_vhcr *vhcr,
+					 struct mlx4_cmd_mailbox *inbox,
+					 struct mlx4_cmd_mailbox *outbox,
+					 struct mlx4_cmd_info *cmd)
+{
+	int err;
+
+	if (dev->caps.steering_mode !=
+	    MLX4_STEERING_MODE_DEVICE_MANAGED)
+		return -EOPNOTSUPP;
+
+	err = mlx4_cmd_imm(dev, inbox->dma, &vhcr->out_param,
+			   vhcr->in_modifier, 0,
+			   MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
+			   MLX4_CMD_NATIVE);
+	if (err)
+		return err;
+
+	err = add_res_range(dev, slave, vhcr->out_param, 1, RES_FS_RULE, 0);
+	if (err) {
+		mlx4_err(dev, "Fail to add flow steering resources.\n ");
+		/* detach rule*/
+		mlx4_cmd(dev, vhcr->out_param, 0, 0,
+			 MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
+			 MLX4_CMD_NATIVE);
+	}
+	return err;
+}
+
+int mlx4_QP_FLOW_STEERING_DETACH_wrapper(struct mlx4_dev *dev, int slave,
+					 struct mlx4_vhcr *vhcr,
+					 struct mlx4_cmd_mailbox *inbox,
+					 struct mlx4_cmd_mailbox *outbox,
+					 struct mlx4_cmd_info *cmd)
+{
+	int err;
+
+	if (dev->caps.steering_mode !=
+	    MLX4_STEERING_MODE_DEVICE_MANAGED)
+		return -EOPNOTSUPP;
+
+	err = rem_res_range(dev, slave, vhcr->in_param, 1, RES_FS_RULE, 0);
+	if (err) {
+		mlx4_err(dev, "Fail to remove flow steering resources.\n ");
+		return err;
+	}
+
+	err = mlx4_cmd(dev, vhcr->in_param, 0, 0,
+		       MLX4_QP_FLOW_STEERING_DETACH, MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+	return err;
+}
+
 enum {
 	BUSY_MAX_RETRIES = 10
 };
@@ -2751,7 +2886,7 @@ static int _move_all_busy(struct mlx4_dev *dev, int slave,
 				if (r->state == RES_ANY_BUSY) {
 					if (print)
 						mlx4_dbg(dev,
-							 "%s id 0x%x is busy\n",
+							 "%s id 0x%llx is busy\n",
 							  ResourceType(type),
 							  r->res_id);
 					++busy;
@@ -2817,8 +2952,8 @@ static void rem_slave_qps(struct mlx4_dev *dev, int slave)
 				switch (state) {
 				case RES_QP_RESERVED:
 					spin_lock_irq(mlx4_tlock(dev));
-					radix_tree_delete(&tracker->res_tree[RES_QP],
-							  qp->com.res_id);
+					rb_erase(&qp->com.node,
+						 &tracker->res_tree[RES_QP]);
 					list_del(&qp->com.list);
 					spin_unlock_irq(mlx4_tlock(dev));
 					kfree(qp);
@@ -2888,8 +3023,8 @@ static void rem_slave_srqs(struct mlx4_dev *dev, int slave)
 				case RES_SRQ_ALLOCATED:
 					__mlx4_srq_free_icm(dev, srqn);
 					spin_lock_irq(mlx4_tlock(dev));
-					radix_tree_delete(&tracker->res_tree[RES_SRQ],
-							  srqn);
+					rb_erase(&srq->com.node,
+						 &tracker->res_tree[RES_SRQ]);
 					list_del(&srq->com.list);
 					spin_unlock_irq(mlx4_tlock(dev));
 					kfree(srq);
@@ -2954,8 +3089,8 @@ static void rem_slave_cqs(struct mlx4_dev *dev, int slave)
 				case RES_CQ_ALLOCATED:
 					__mlx4_cq_free_icm(dev, cqn);
 					spin_lock_irq(mlx4_tlock(dev));
-					radix_tree_delete(&tracker->res_tree[RES_CQ],
-							  cqn);
+					rb_erase(&cq->com.node,
+						 &tracker->res_tree[RES_CQ]);
 					list_del(&cq->com.list);
 					spin_unlock_irq(mlx4_tlock(dev));
 					kfree(cq);
@@ -3017,8 +3152,8 @@ static void rem_slave_mrs(struct mlx4_dev *dev, int slave)
 				case RES_MPT_RESERVED:
 					__mlx4_mr_release(dev, mpt->key);
 					spin_lock_irq(mlx4_tlock(dev));
-					radix_tree_delete(&tracker->res_tree[RES_MPT],
-							  mptn);
+					rb_erase(&mpt->com.node,
+						 &tracker->res_tree[RES_MPT]);
 					list_del(&mpt->com.list);
 					spin_unlock_irq(mlx4_tlock(dev));
 					kfree(mpt);
@@ -3086,8 +3221,8 @@ static void rem_slave_mtts(struct mlx4_dev *dev, int slave)
 					__mlx4_free_mtt_range(dev, base,
 							      mtt->order);
 					spin_lock_irq(mlx4_tlock(dev));
-					radix_tree_delete(&tracker->res_tree[RES_MTT],
-							  base);
+					rb_erase(&mtt->com.node,
+						 &tracker->res_tree[RES_MTT]);
 					list_del(&mtt->com.list);
 					spin_unlock_irq(mlx4_tlock(dev));
 					kfree(mtt);
@@ -3104,6 +3239,58 @@ static void rem_slave_mtts(struct mlx4_dev *dev, int slave)
 	spin_unlock_irq(mlx4_tlock(dev));
 }
 
+static void rem_slave_fs_rule(struct mlx4_dev *dev, int slave)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_resource_tracker *tracker =
+		&priv->mfunc.master.res_tracker;
+	struct list_head *fs_rule_list =
+		&tracker->slave_list[slave].res_list[RES_FS_RULE];
+	struct res_fs_rule *fs_rule;
+	struct res_fs_rule *tmp;
+	int state;
+	u64 base;
+	int err;
+
+	err = move_all_busy(dev, slave, RES_FS_RULE);
+	if (err)
+		mlx4_warn(dev, "rem_slave_fs_rule: Could not move all mtts to busy for slave %d\n",
+			  slave);
+
+	spin_lock_irq(mlx4_tlock(dev));
+	list_for_each_entry_safe(fs_rule, tmp, fs_rule_list, com.list) {
+		spin_unlock_irq(mlx4_tlock(dev));
+		if (fs_rule->com.owner == slave) {
+			base = fs_rule->com.res_id;
+			state = fs_rule->com.from_state;
+			while (state != 0) {
+				switch (state) {
+				case RES_FS_RULE_ALLOCATED:
+					/* detach rule */
+					err = mlx4_cmd(dev, base, 0, 0,
+						       MLX4_QP_FLOW_STEERING_DETACH,
+						       MLX4_CMD_TIME_CLASS_A,
+						       MLX4_CMD_NATIVE);
+
+					spin_lock_irq(mlx4_tlock(dev));
+					rb_erase(&fs_rule->com.node,
+						 &tracker->res_tree[RES_FS_RULE]);
+					list_del(&fs_rule->com.list);
+					spin_unlock_irq(mlx4_tlock(dev));
+					kfree(fs_rule);
+					state = 0;
+					break;
+
+				default:
+					state = 0;
+				}
+			}
+		}
+		spin_lock_irq(mlx4_tlock(dev));
+	}
+	spin_unlock_irq(mlx4_tlock(dev));
+}
+
 static void rem_slave_eqs(struct mlx4_dev *dev, int slave)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -3133,8 +3320,8 @@ static void rem_slave_eqs(struct mlx4_dev *dev, int slave)
 				switch (state) {
 				case RES_EQ_RESERVED:
 					spin_lock_irq(mlx4_tlock(dev));
-					radix_tree_delete(&tracker->res_tree[RES_EQ],
-							  eqn);
+					rb_erase(&eq->com.node,
+						 &tracker->res_tree[RES_EQ]);
 					list_del(&eq->com.list);
 					spin_unlock_irq(mlx4_tlock(dev));
 					kfree(eq);
@@ -3191,7 +3378,8 @@ static void rem_slave_counters(struct mlx4_dev *dev, int slave)
 	list_for_each_entry_safe(counter, tmp, counter_list, com.list) {
 		if (counter->com.owner == slave) {
 			index = counter->com.res_id;
-			radix_tree_delete(&tracker->res_tree[RES_COUNTER], index);
+			rb_erase(&counter->com.node,
+				 &tracker->res_tree[RES_COUNTER]);
 			list_del(&counter->com.list);
 			kfree(counter);
 			__mlx4_counter_free(dev, index);
@@ -3220,7 +3408,7 @@ static void rem_slave_xrcdns(struct mlx4_dev *dev, int slave)
 	list_for_each_entry_safe(xrcd, tmp, xrcdn_list, com.list) {
 		if (xrcd->com.owner == slave) {
 			xrcdn = xrcd->com.res_id;
-			radix_tree_delete(&tracker->res_tree[RES_XRCD], xrcdn);
+			rb_erase(&xrcd->com.node, &tracker->res_tree[RES_XRCD]);
 			list_del(&xrcd->com.list);
 			kfree(xrcd);
 			__mlx4_xrcd_free(dev, xrcdn);
@@ -3244,5 +3432,6 @@ void mlx4_delete_all_resources_for_slave(struct mlx4_dev *dev, int slave)
 	rem_slave_mtts(dev, slave);
 	rem_slave_counters(dev, slave);
 	rem_slave_xrcdns(dev, slave);
+	rem_slave_fs_rule(dev, slave);
 	mutex_unlock(&priv->mfunc.master.res_tracker.slave_list[slave].mutex);
 }
diff --git a/drivers/net/ethernet/micrel/ks8851.c b/drivers/net/ethernet/micrel/ks8851.c
index 5e313e9a252..1540ebeb866 100644
--- a/drivers/net/ethernet/micrel/ks8851.c
+++ b/drivers/net/ethernet/micrel/ks8851.c
@@ -422,7 +422,7 @@ static void ks8851_read_mac_addr(struct net_device *dev)
  *
  * Get or create the initial mac address for the device and then set that
  * into the station address register. If there is an EEPROM present, then
- * we try that. If no valid mac address is found we use random_ether_addr()
+ * we try that. If no valid mac address is found we use eth_random_addr()
  * to create a new one.
  */
 static void ks8851_init_mac(struct ks8851_net *ks)
diff --git a/drivers/net/ethernet/micrel/ks8851_mll.c b/drivers/net/ethernet/micrel/ks8851_mll.c
index 5ffde23ac8f..38529edfe35 100644
--- a/drivers/net/ethernet/micrel/ks8851_mll.c
+++ b/drivers/net/ethernet/micrel/ks8851_mll.c
@@ -16,8 +16,7 @@
  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-/**
- * Supports:
+/* Supports:
  * KS8851 16bit MLL chip from Micrel Inc.
  */
 
@@ -35,7 +34,7 @@
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
-#include <asm/io.h>
+#include <linux/ks8851_mll.h>
 
 #define	DRV_NAME	"ks8851_mll"
 
@@ -465,8 +464,7 @@ static int msg_enable;
 #define BE1             0x2000      /* Byte Enable 1 */
 #define BE0             0x1000      /* Byte Enable 0 */
 
-/**
- * register read/write calls.
+/* register read/write calls.
  *
  * All these calls issue transactions to access the chip's registers. They
  * all require that the necessary lock is held to prevent accesses when the
@@ -1103,7 +1101,7 @@ static void ks_set_grpaddr(struct ks_net *ks)
 	}
 }  /* ks_set_grpaddr */
 
-/*
+/**
 * ks_clear_mcast - clear multicast information
 *
 * @ks : The chip information
@@ -1515,6 +1513,7 @@ static int __devinit ks8851_probe(struct platform_device *pdev)
 	struct net_device *netdev;
 	struct ks_net *ks;
 	u16 id, data;
+	struct ks8851_mll_platform_data *pdata;
 
 	io_d = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	io_c = platform_get_resource(pdev, IORESOURCE_MEM, 1);
@@ -1596,17 +1595,27 @@ static int __devinit ks8851_probe(struct platform_device *pdev)
 	ks_disable_qmu(ks);
 	ks_setup(ks);
 	ks_setup_int(ks);
-	memcpy(netdev->dev_addr, ks->mac_addr, 6);
 
 	data = ks_rdreg16(ks, KS_OBCR);
 	ks_wrreg16(ks, KS_OBCR, data | OBCR_ODS_16MA);
 
-	/**
-	 * If you want to use the default MAC addr,
-	 * comment out the 2 functions below.
-	 */
+	/* overwriting the default MAC address */
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		netdev_err(netdev, "No platform data\n");
+		err = -ENODEV;
+		goto err_pdata;
+	}
+	memcpy(ks->mac_addr, pdata->mac_addr, 6);
+	if (!is_valid_ether_addr(ks->mac_addr)) {
+		/* Use random MAC address if none passed */
+		eth_random_addr(ks->mac_addr);
+		netdev_info(netdev, "Using random mac address\n");
+	}
+	netdev_info(netdev, "Mac address is: %pM\n", ks->mac_addr);
+
+	memcpy(netdev->dev_addr, ks->mac_addr, 6);
 
-	random_ether_addr(netdev->dev_addr);
 	ks_set_mac(ks, netdev->dev_addr);
 
 	id = ks_rdreg16(ks, KS_CIDER);
@@ -1615,6 +1624,8 @@ static int __devinit ks8851_probe(struct platform_device *pdev)
 		    (id >> 8) & 0xff, (id >> 4) & 0xf, (id >> 1) & 0x7);
 	return 0;
 
+err_pdata:
+	unregister_netdev(netdev);
 err_register:
 err_get_irq:
 	iounmap(ks->hw_addr_cmd);
diff --git a/drivers/net/ethernet/micrel/ksz884x.c b/drivers/net/ethernet/micrel/ksz884x.c
index eaf9ff0262a..318fee91c79 100644
--- a/drivers/net/ethernet/micrel/ksz884x.c
+++ b/drivers/net/ethernet/micrel/ksz884x.c
@@ -3913,7 +3913,7 @@ static void hw_start_rx(struct ksz_hw *hw)
 		hw->rx_stop = 2;
 }
 
-/*
+/**
  * hw_stop_rx - stop receiving
  * @hw: 	The hardware instance.
  *
@@ -4480,14 +4480,12 @@ static void ksz_init_rx_buffers(struct dev_info *adapter)
 		dma_buf->len = adapter->mtu;
 		if (!dma_buf->skb)
 			dma_buf->skb = alloc_skb(dma_buf->len, GFP_ATOMIC);
-		if (dma_buf->skb && !dma_buf->dma) {
-			dma_buf->skb->dev = adapter->dev;
+		if (dma_buf->skb && !dma_buf->dma)
 			dma_buf->dma = pci_map_single(
 				adapter->pdev,
 				skb_tail_pointer(dma_buf->skb),
 				dma_buf->len,
 				PCI_DMA_FROMDEVICE);
-		}
 
 		/* Set descriptor. */
 		set_rx_buf(desc, dma_buf->dma);
@@ -4881,8 +4879,8 @@ static netdev_tx_t netdev_tx(struct sk_buff *skb, struct net_device *dev)
 	left = hw_alloc_pkt(hw, skb->len, num);
 	if (left) {
 		if (left < num ||
-				((CHECKSUM_PARTIAL == skb->ip_summed) &&
-				(ETH_P_IPV6 == htons(skb->protocol)))) {
+		    (CHECKSUM_PARTIAL == skb->ip_summed &&
+		     skb->protocol == htons(ETH_P_IPV6))) {
 			struct sk_buff *org_skb = skb;
 
 			skb = netdev_alloc_skb(dev, org_skb->len);
diff --git a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
index 90153fc983c..fa85cf1353f 100644
--- a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
+++ b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
@@ -3775,7 +3775,7 @@ static void myri10ge_probe_slices(struct myri10ge_priv *mgp)
 
 	mgp->num_slices = 1;
 	msix_cap = pci_find_capability(pdev, PCI_CAP_ID_MSIX);
-	ncpus = num_online_cpus();
+	ncpus = netif_get_num_default_rss_queues();
 
 	if (myri10ge_max_slices == 1 || msix_cap == 0 ||
 	    (myri10ge_max_slices == -1 && ncpus < 2))
diff --git a/drivers/net/ethernet/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index bb367582c1e..d958c229937 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -3377,7 +3377,7 @@ static int wait_for_cmd_complete(void __iomem *addr, u64 busy_bit,
 	} while (cnt < 20);
 	return ret;
 }
-/*
+/**
  * check_pci_device_id - Checks if the device id is supported
  * @id : device id
  * Description: Function to check if the pci device id is supported by driver.
@@ -5238,7 +5238,7 @@ static u64 do_s2io_read_unicast_mc(struct s2io_nic *sp, int offset)
 }
 
 /**
- * s2io_set_mac_addr driver entry point
+ * s2io_set_mac_addr - driver entry point
  */
 
 static int s2io_set_mac_addr(struct net_device *dev, void *p)
@@ -6088,7 +6088,7 @@ static int s2io_bist_test(struct s2io_nic *sp, uint64_t *data)
 }
 
 /**
- * s2io-link_test - verifies the link state of the nic
+ * s2io_link_test - verifies the link state of the nic
  * @sp ; private member of the device structure, which is a pointer to the
  * s2io_nic structure.
  * @data: variable that returns the result of each of the test conducted by
@@ -6116,9 +6116,9 @@ static int s2io_link_test(struct s2io_nic *sp, uint64_t *data)
 
 /**
  * s2io_rldram_test - offline test for access to the RldRam chip on the NIC
- * @sp - private member of the device structure, which is a pointer to the
+ * @sp: private member of the device structure, which is a pointer to the
  * s2io_nic structure.
- * @data - variable that returns the result of each of the test
+ * @data: variable that returns the result of each of the test
  * conducted by the driver.
  * Description:
  *  This is one of the offline test that tests the read and write
@@ -6946,9 +6946,9 @@ static  int rxd_owner_bit_reset(struct s2io_nic *sp)
 				if (sp->rxd_mode == RXD_MODE_3B)
 					ba = &ring->ba[j][k];
 				if (set_rxd_buffer_pointer(sp, rxdp, ba, &skb,
-							   (u64 *)&temp0_64,
-							   (u64 *)&temp1_64,
-							   (u64 *)&temp2_64,
+							   &temp0_64,
+							   &temp1_64,
+							   &temp2_64,
 							   size) == -ENOMEM) {
 					return 0;
 				}
@@ -7149,7 +7149,7 @@ static int s2io_card_up(struct s2io_nic *sp)
 	int i, ret = 0;
 	struct config_param *config;
 	struct mac_info *mac_control;
-	struct net_device *dev = (struct net_device *)sp->dev;
+	struct net_device *dev = sp->dev;
 	u16 interruptible;
 
 	/* Initialize the H/W I/O registers */
@@ -7325,7 +7325,7 @@ static void s2io_tx_watchdog(struct net_device *dev)
 static int rx_osm_handler(struct ring_info *ring_data, struct RxD_t * rxdp)
 {
 	struct s2io_nic *sp = ring_data->nic;
-	struct net_device *dev = (struct net_device *)ring_data->dev;
+	struct net_device *dev = ring_data->dev;
 	struct sk_buff *skb = (struct sk_buff *)
 		((unsigned long)rxdp->Host_Control);
 	int ring_no = ring_data->ring_no;
@@ -7508,7 +7508,7 @@ aggregate:
 
 static void s2io_link(struct s2io_nic *sp, int link)
 {
-	struct net_device *dev = (struct net_device *)sp->dev;
+	struct net_device *dev = sp->dev;
 	struct swStat *swstats = &sp->mac_control.stats_info->sw_stat;
 
 	if (link != sp->last_link_state) {
@@ -8280,7 +8280,7 @@ static int check_L2_lro_capable(u8 *buffer, struct iphdr **ip,
 		return -1;
 	}
 
-	*ip = (struct iphdr *)((u8 *)buffer + ip_off);
+	*ip = (struct iphdr *)(buffer + ip_off);
 	ip_len = (u8)((*ip)->ihl);
 	ip_len <<= 2;
 	*tcp = (struct tcphdr *)((unsigned long)*ip + ip_len);
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-config.c b/drivers/net/ethernet/neterion/vxge/vxge-config.c
index 98e2c10ae08..32d06824fe3 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-config.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-config.c
@@ -2346,7 +2346,7 @@ void __vxge_hw_blockpool_blocks_add(struct __vxge_hw_blockpool *blockpool)
 
 	for (i = 0; i < nreq; i++)
 		vxge_os_dma_malloc_async(
-			((struct __vxge_hw_device *)blockpool->hldev)->pdev,
+			(blockpool->hldev)->pdev,
 			blockpool->hldev, VXGE_HW_BLOCK_SIZE);
 }
 
@@ -2428,13 +2428,13 @@ __vxge_hw_blockpool_blocks_remove(struct __vxge_hw_blockpool *blockpool)
 			break;
 
 		pci_unmap_single(
-			((struct __vxge_hw_device *)blockpool->hldev)->pdev,
+			(blockpool->hldev)->pdev,
 			((struct __vxge_hw_blockpool_entry *)p)->dma_addr,
 			((struct __vxge_hw_blockpool_entry *)p)->length,
 			PCI_DMA_BIDIRECTIONAL);
 
 		vxge_os_dma_free(
-			((struct __vxge_hw_device *)blockpool->hldev)->pdev,
+			(blockpool->hldev)->pdev,
 			((struct __vxge_hw_blockpool_entry *)p)->memblock,
 			&((struct __vxge_hw_blockpool_entry *)p)->acc_handle);
 
@@ -4059,7 +4059,7 @@ __vxge_hw_vpath_sw_reset(struct __vxge_hw_device *hldev, u32 vp_id)
 	enum vxge_hw_status status = VXGE_HW_OK;
 	struct __vxge_hw_virtualpath *vpath;
 
-	vpath = (struct __vxge_hw_virtualpath *)&hldev->virtual_paths[vp_id];
+	vpath = &hldev->virtual_paths[vp_id];
 
 	if (vpath->ringh) {
 		status = __vxge_hw_ring_reset(vpath->ringh);
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-config.h b/drivers/net/ethernet/neterion/vxge/vxge-config.h
index 5046a64f0fe..9e0c1eed5dc 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-config.h
+++ b/drivers/net/ethernet/neterion/vxge/vxge-config.h
@@ -1922,7 +1922,7 @@ realloc:
 		/* misaligned, free current one and try allocating
 		 * size + VXGE_CACHE_LINE_SIZE memory
 		 */
-		kfree((void *) vaddr);
+		kfree(vaddr);
 		size += VXGE_CACHE_LINE_SIZE;
 		realloc_flag = 1;
 		goto realloc;
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index 51387c31914..de219044351 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -1134,7 +1134,7 @@ static void vxge_set_multicast(struct net_device *dev)
 		"%s:%d", __func__, __LINE__);
 
 	vdev = netdev_priv(dev);
-	hldev = (struct __vxge_hw_device  *)vdev->devh;
+	hldev = vdev->devh;
 
 	if (unlikely(!is_vxge_card_up(vdev)))
 		return;
@@ -3131,12 +3131,12 @@ vxge_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *net_stats)
 		u64 packets, bytes, multicast;
 
 		do {
-			start = u64_stats_fetch_begin(&rxstats->syncp);
+			start = u64_stats_fetch_begin_bh(&rxstats->syncp);
 
 			packets   = rxstats->rx_frms;
 			multicast = rxstats->rx_mcast;
 			bytes     = rxstats->rx_bytes;
-		} while (u64_stats_fetch_retry(&rxstats->syncp, start));
+		} while (u64_stats_fetch_retry_bh(&rxstats->syncp, start));
 
 		net_stats->rx_packets += packets;
 		net_stats->rx_bytes += bytes;
@@ -3146,11 +3146,11 @@ vxge_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *net_stats)
 		net_stats->rx_dropped += rxstats->rx_dropped;
 
 		do {
-			start = u64_stats_fetch_begin(&txstats->syncp);
+			start = u64_stats_fetch_begin_bh(&txstats->syncp);
 
 			packets = txstats->tx_frms;
 			bytes   = txstats->tx_bytes;
-		} while (u64_stats_fetch_retry(&txstats->syncp, start));
+		} while (u64_stats_fetch_retry_bh(&txstats->syncp, start));
 
 		net_stats->tx_packets += packets;
 		net_stats->tx_bytes += bytes;
@@ -3687,7 +3687,8 @@ static int __devinit vxge_config_vpaths(
 			return 0;
 
 		if (!driver_config->g_no_cpus)
-			driver_config->g_no_cpus = num_online_cpus();
+			driver_config->g_no_cpus =
+				netif_get_num_default_rss_queues();
 
 		driver_config->vpath_per_dev = driver_config->g_no_cpus >> 1;
 		if (!driver_config->vpath_per_dev)
@@ -3989,16 +3990,16 @@ static void __devinit vxge_print_parm(struct vxgedev *vdev, u64 vpath_mask)
 			continue;
 		vxge_debug_ll_config(VXGE_TRACE,
 			"%s: MTU size - %d", vdev->ndev->name,
-			((struct __vxge_hw_device  *)(vdev->devh))->
+			((vdev->devh))->
 				config.vp_config[i].mtu);
 		vxge_debug_init(VXGE_TRACE,
 			"%s: VLAN tag stripping %s", vdev->ndev->name,
-			((struct __vxge_hw_device  *)(vdev->devh))->
+			((vdev->devh))->
 				config.vp_config[i].rpa_strip_vlan_tag
 			? "Enabled" : "Disabled");
 		vxge_debug_ll_config(VXGE_TRACE,
 			"%s: Max frags : %d", vdev->ndev->name,
-			((struct __vxge_hw_device  *)(vdev->devh))->
+			((vdev->devh))->
 				config.vp_config[i].fifo.max_frags);
 		break;
 	}
@@ -4260,9 +4261,7 @@ static int vxge_probe_fw_update(struct vxgedev *vdev)
 	if (VXGE_FW_VER(VXGE_CERT_FW_VER_MAJOR, VXGE_CERT_FW_VER_MINOR, 0) >
 	    VXGE_FW_VER(maj, min, 0)) {
 		vxge_debug_init(VXGE_ERR, "%s: Firmware %d.%d.%d is too old to"
-				" be used with this driver.\n"
-				"Please get the latest version from "
-				"ftp://ftp.s2io.com/pub/X3100-Drivers/FIRMWARE",
+				" be used with this driver.",
 				VXGE_DRIVER_NAME, maj, min, bld);
 		return -EINVAL;
 	}
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.h b/drivers/net/ethernet/neterion/vxge/vxge-main.h
index 35f3e7552ec..36ca40f8f24 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.h
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.h
@@ -430,8 +430,7 @@ void vxge_initialize_ethtool_ops(struct net_device *ndev);
 enum vxge_hw_status vxge_reset_all_vpaths(struct vxgedev *vdev);
 int vxge_fw_upgrade(struct vxgedev *vdev, char *fw_name, int override);
 
-/**
- * #define VXGE_DEBUG_INIT: debug for initialization functions
+/* #define VXGE_DEBUG_INIT: debug for initialization functions
  * #define VXGE_DEBUG_TX	 : debug transmit related functions
  * #define VXGE_DEBUG_RX  : debug recevice related functions
  * #define VXGE_DEBUG_MEM : debug memory module
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-traffic.c b/drivers/net/ethernet/neterion/vxge/vxge-traffic.c
index 5954fa264da..99749bd07d7 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-traffic.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-traffic.c
@@ -533,8 +533,7 @@ __vxge_hw_device_handle_error(struct __vxge_hw_device *hldev, u32 vp_id,
 
 	/* notify driver */
 	if (hldev->uld_callbacks->crit_err)
-		hldev->uld_callbacks->crit_err(
-			(struct __vxge_hw_device *)hldev,
+		hldev->uld_callbacks->crit_err(hldev,
 			type, vp_id);
 out:
 
@@ -1322,7 +1321,7 @@ enum vxge_hw_status vxge_hw_ring_rxd_next_completed(
 	/* check whether it is not the end */
 	if (!own || *t_code == VXGE_HW_RING_T_CODE_FRM_DROP) {
 
-		vxge_assert(((struct vxge_hw_ring_rxd_1 *)rxdp)->host_control !=
+		vxge_assert((rxdp)->host_control !=
 				0);
 
 		++ring->cmpl_cnt;
diff --git a/drivers/net/ethernet/nvidia/forcedeth.c b/drivers/net/ethernet/nvidia/forcedeth.c
index 928913c4f3f..f45def01a98 100644
--- a/drivers/net/ethernet/nvidia/forcedeth.c
+++ b/drivers/net/ethernet/nvidia/forcedeth.c
@@ -3218,7 +3218,7 @@ static void nv_force_linkspeed(struct net_device *dev, int speed, int duplex)
 }
 
 /**
- * nv_update_linkspeed: Setup the MAC according to the link partner
+ * nv_update_linkspeed - Setup the MAC according to the link partner
  * @dev: Network device to be configured
  *
  * The function queries the PHY and checks if there is a link partner.
@@ -3552,8 +3552,7 @@ static irqreturn_t nv_nic_irq(int foo, void *data)
 	return IRQ_HANDLED;
 }
 
-/**
- * All _optimized functions are used to help increase performance
+/* All _optimized functions are used to help increase performance
  * (reduce CPU and increase throughput). They use descripter version 3,
  * compiler directives, and reduce memory accesses.
  */
@@ -3776,7 +3775,7 @@ static irqreturn_t nv_nic_irq_other(int foo, void *data)
 			np->link_timeout = jiffies + LINK_TIMEOUT;
 		}
 		if (events & NVREG_IRQ_RECOVER_ERROR) {
-			spin_lock_irq(&np->lock);
+			spin_lock_irqsave(&np->lock, flags);
 			/* disable interrupts on the nic */
 			writel(NVREG_IRQ_OTHER, base + NvRegIrqMask);
 			pci_push(base);
@@ -3786,7 +3785,7 @@ static irqreturn_t nv_nic_irq_other(int foo, void *data)
 				np->recover_error = 1;
 				mod_timer(&np->nic_poll, jiffies + POLL_WAIT);
 			}
-			spin_unlock_irq(&np->lock);
+			spin_unlock_irqrestore(&np->lock, flags);
 			break;
 		}
 		if (unlikely(i > max_interrupt_work)) {
@@ -5183,6 +5182,7 @@ static const struct ethtool_ops ops = {
 	.get_ethtool_stats = nv_get_ethtool_stats,
 	.get_sset_count = nv_get_sset_count,
 	.self_test = nv_self_test,
+	.get_ts_info = ethtool_op_get_ts_info,
 };
 
 /* The mgmt unit and driver use a semaphore to access the phy during init */
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index 083d6715335..4069edab229 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -44,7 +44,6 @@
 #include <linux/of_net.h>
 #include <linux/types.h>
 
-#include <linux/delay.h>
 #include <linux/io.h>
 #include <mach/board.h>
 #include <mach/platform.h>
@@ -52,7 +51,6 @@
 
 #define MODNAME "lpc-eth"
 #define DRV_VERSION "1.00"
-#define PHYDEF_ADDR 0x00
 
 #define ENET_MAXF_SIZE 1536
 #define ENET_RX_DESC 48
@@ -416,9 +414,6 @@ static bool use_iram_for_net(struct device *dev)
 #define TXDESC_CONTROL_LAST		(1 << 30)
 #define TXDESC_CONTROL_INT		(1 << 31)
 
-static int lpc_eth_hard_start_xmit(struct sk_buff *skb,
-				   struct net_device *ndev);
-
 /*
  * Structure of a TX/RX descriptors and RX status
  */
@@ -440,7 +435,7 @@ struct netdata_local {
 	spinlock_t		lock;
 	void __iomem		*net_base;
 	u32			msg_enable;
-	struct sk_buff		*skb[ENET_TX_DESC];
+	unsigned int		skblen[ENET_TX_DESC];
 	unsigned int		last_tx_idx;
 	unsigned int		num_used_tx_buffs;
 	struct mii_bus		*mii_bus;
@@ -903,12 +898,11 @@ err_out:
 static void __lpc_handle_xmit(struct net_device *ndev)
 {
 	struct netdata_local *pldat = netdev_priv(ndev);
-	struct sk_buff *skb;
 	u32 txcidx, *ptxstat, txstat;
 
 	txcidx = readl(LPC_ENET_TXCONSUMEINDEX(pldat->net_base));
 	while (pldat->last_tx_idx != txcidx) {
-		skb = pldat->skb[pldat->last_tx_idx];
+		unsigned int skblen = pldat->skblen[pldat->last_tx_idx];
 
 		/* A buffer is available, get buffer status */
 		ptxstat = &pldat->tx_stat_v[pldat->last_tx_idx];
@@ -945,9 +939,8 @@ static void __lpc_handle_xmit(struct net_device *ndev)
 		} else {
 			/* Update stats */
 			ndev->stats.tx_packets++;
-			ndev->stats.tx_bytes += skb->len;
+			ndev->stats.tx_bytes += skblen;
 		}
-		dev_kfree_skb_irq(skb);
 
 		txcidx = readl(LPC_ENET_TXCONSUMEINDEX(pldat->net_base));
 	}
@@ -1132,7 +1125,7 @@ static int lpc_eth_hard_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	memcpy(pldat->tx_buff_v + txidx * ENET_MAXF_SIZE, skb->data, len);
 
 	/* Save the buffer and increment the buffer counter */
-	pldat->skb[txidx] = skb;
+	pldat->skblen[txidx] = len;
 	pldat->num_used_tx_buffs++;
 
 	/* Start transmit */
@@ -1147,6 +1140,7 @@ static int lpc_eth_hard_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 
 	spin_unlock_irq(&pldat->lock);
 
+	dev_kfree_skb(skb);
 	return NETDEV_TX_OK;
 }
 
@@ -1442,7 +1436,7 @@ static int lpc_eth_drv_probe(struct platform_device *pdev)
 			res->start);
 	netdev_dbg(ndev, "IO address size      :%d\n",
 			res->end - res->start + 1);
-	netdev_err(ndev, "IO address (mapped)  :0x%p\n",
+	netdev_dbg(ndev, "IO address (mapped)  :0x%p\n",
 			pldat->net_base);
 	netdev_dbg(ndev, "IRQ number           :%d\n", ndev->irq);
 	netdev_dbg(ndev, "DMA buffer size      :%d\n", pldat->dma_buff_size);
diff --git a/drivers/net/ethernet/octeon/octeon_mgmt.c b/drivers/net/ethernet/octeon/octeon_mgmt.c
index cd827ff4a02..c42bbb16cda 100644
--- a/drivers/net/ethernet/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/octeon/octeon_mgmt.c
@@ -6,19 +6,21 @@
  * Copyright (C) 2009 Cavium Networks
  */
 
-#include <linux/capability.h>
+#include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/init.h>
-#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include <linux/capability.h>
 #include <linux/interrupt.h>
-#include <linux/platform_device.h>
 #include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/if.h>
+#include <linux/spinlock.h>
 #include <linux/if_vlan.h>
+#include <linux/of_mdio.h>
+#include <linux/module.h>
+#include <linux/of_net.h>
+#include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/phy.h>
-#include <linux/spinlock.h>
+#include <linux/io.h>
 
 #include <asm/octeon/octeon.h>
 #include <asm/octeon/cvmx-mixx-defs.h>
@@ -58,8 +60,56 @@ union mgmt_port_ring_entry {
 	} s;
 };
 
+#define MIX_ORING1	0x0
+#define MIX_ORING2	0x8
+#define MIX_IRING1	0x10
+#define MIX_IRING2	0x18
+#define MIX_CTL		0x20
+#define MIX_IRHWM	0x28
+#define MIX_IRCNT	0x30
+#define MIX_ORHWM	0x38
+#define MIX_ORCNT	0x40
+#define MIX_ISR		0x48
+#define MIX_INTENA	0x50
+#define MIX_REMCNT	0x58
+#define MIX_BIST	0x78
+
+#define AGL_GMX_PRT_CFG			0x10
+#define AGL_GMX_RX_FRM_CTL		0x18
+#define AGL_GMX_RX_FRM_MAX		0x30
+#define AGL_GMX_RX_JABBER		0x38
+#define AGL_GMX_RX_STATS_CTL		0x50
+
+#define AGL_GMX_RX_STATS_PKTS_DRP	0xb0
+#define AGL_GMX_RX_STATS_OCTS_DRP	0xb8
+#define AGL_GMX_RX_STATS_PKTS_BAD	0xc0
+
+#define AGL_GMX_RX_ADR_CTL		0x100
+#define AGL_GMX_RX_ADR_CAM_EN		0x108
+#define AGL_GMX_RX_ADR_CAM0		0x180
+#define AGL_GMX_RX_ADR_CAM1		0x188
+#define AGL_GMX_RX_ADR_CAM2		0x190
+#define AGL_GMX_RX_ADR_CAM3		0x198
+#define AGL_GMX_RX_ADR_CAM4		0x1a0
+#define AGL_GMX_RX_ADR_CAM5		0x1a8
+
+#define AGL_GMX_TX_STATS_CTL		0x268
+#define AGL_GMX_TX_CTL			0x270
+#define AGL_GMX_TX_STAT0		0x280
+#define AGL_GMX_TX_STAT1		0x288
+#define AGL_GMX_TX_STAT2		0x290
+#define AGL_GMX_TX_STAT3		0x298
+#define AGL_GMX_TX_STAT4		0x2a0
+#define AGL_GMX_TX_STAT5		0x2a8
+#define AGL_GMX_TX_STAT6		0x2b0
+#define AGL_GMX_TX_STAT7		0x2b8
+#define AGL_GMX_TX_STAT8		0x2c0
+#define AGL_GMX_TX_STAT9		0x2c8
+
 struct octeon_mgmt {
 	struct net_device *netdev;
+	u64 mix;
+	u64 agl;
 	int port;
 	int irq;
 	u64 *tx_ring;
@@ -85,31 +135,34 @@ struct octeon_mgmt {
 	struct napi_struct napi;
 	struct tasklet_struct tx_clean_tasklet;
 	struct phy_device *phydev;
+	struct device_node *phy_np;
+	resource_size_t mix_phys;
+	resource_size_t mix_size;
+	resource_size_t agl_phys;
+	resource_size_t agl_size;
 };
 
 static void octeon_mgmt_set_rx_irq(struct octeon_mgmt *p, int enable)
 {
-	int port = p->port;
 	union cvmx_mixx_intena mix_intena;
 	unsigned long flags;
 
 	spin_lock_irqsave(&p->lock, flags);
-	mix_intena.u64 = cvmx_read_csr(CVMX_MIXX_INTENA(port));
+	mix_intena.u64 = cvmx_read_csr(p->mix + MIX_INTENA);
 	mix_intena.s.ithena = enable ? 1 : 0;
-	cvmx_write_csr(CVMX_MIXX_INTENA(port), mix_intena.u64);
+	cvmx_write_csr(p->mix + MIX_INTENA, mix_intena.u64);
 	spin_unlock_irqrestore(&p->lock, flags);
 }
 
 static void octeon_mgmt_set_tx_irq(struct octeon_mgmt *p, int enable)
 {
-	int port = p->port;
 	union cvmx_mixx_intena mix_intena;
 	unsigned long flags;
 
 	spin_lock_irqsave(&p->lock, flags);
-	mix_intena.u64 = cvmx_read_csr(CVMX_MIXX_INTENA(port));
+	mix_intena.u64 = cvmx_read_csr(p->mix + MIX_INTENA);
 	mix_intena.s.othena = enable ? 1 : 0;
-	cvmx_write_csr(CVMX_MIXX_INTENA(port), mix_intena.u64);
+	cvmx_write_csr(p->mix + MIX_INTENA, mix_intena.u64);
 	spin_unlock_irqrestore(&p->lock, flags);
 }
 
@@ -146,7 +199,6 @@ static unsigned int ring_size_to_bytes(unsigned int ring_size)
 static void octeon_mgmt_rx_fill_ring(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 
 	while (p->rx_current_fill < ring_max_fill(OCTEON_MGMT_RX_RING_SIZE)) {
 		unsigned int size;
@@ -177,24 +229,23 @@ static void octeon_mgmt_rx_fill_ring(struct net_device *netdev)
 			(p->rx_next_fill + 1) % OCTEON_MGMT_RX_RING_SIZE;
 		p->rx_current_fill++;
 		/* Ring the bell.  */
-		cvmx_write_csr(CVMX_MIXX_IRING2(port), 1);
+		cvmx_write_csr(p->mix + MIX_IRING2, 1);
 	}
 }
 
 static void octeon_mgmt_clean_tx_buffers(struct octeon_mgmt *p)
 {
-	int port = p->port;
 	union cvmx_mixx_orcnt mix_orcnt;
 	union mgmt_port_ring_entry re;
 	struct sk_buff *skb;
 	int cleaned = 0;
 	unsigned long flags;
 
-	mix_orcnt.u64 = cvmx_read_csr(CVMX_MIXX_ORCNT(port));
+	mix_orcnt.u64 = cvmx_read_csr(p->mix + MIX_ORCNT);
 	while (mix_orcnt.s.orcnt) {
 		spin_lock_irqsave(&p->tx_list.lock, flags);
 
-		mix_orcnt.u64 = cvmx_read_csr(CVMX_MIXX_ORCNT(port));
+		mix_orcnt.u64 = cvmx_read_csr(p->mix + MIX_ORCNT);
 
 		if (mix_orcnt.s.orcnt == 0) {
 			spin_unlock_irqrestore(&p->tx_list.lock, flags);
@@ -214,7 +265,7 @@ static void octeon_mgmt_clean_tx_buffers(struct octeon_mgmt *p)
 		mix_orcnt.s.orcnt = 1;
 
 		/* Acknowledge to hardware that we have the buffer.  */
-		cvmx_write_csr(CVMX_MIXX_ORCNT(port), mix_orcnt.u64);
+		cvmx_write_csr(p->mix + MIX_ORCNT, mix_orcnt.u64);
 		p->tx_current_fill--;
 
 		spin_unlock_irqrestore(&p->tx_list.lock, flags);
@@ -224,7 +275,7 @@ static void octeon_mgmt_clean_tx_buffers(struct octeon_mgmt *p)
 		dev_kfree_skb_any(skb);
 		cleaned++;
 
-		mix_orcnt.u64 = cvmx_read_csr(CVMX_MIXX_ORCNT(port));
+		mix_orcnt.u64 = cvmx_read_csr(p->mix + MIX_ORCNT);
 	}
 
 	if (cleaned && netif_queue_stopped(p->netdev))
@@ -241,13 +292,12 @@ static void octeon_mgmt_clean_tx_tasklet(unsigned long arg)
 static void octeon_mgmt_update_rx_stats(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	unsigned long flags;
 	u64 drop, bad;
 
 	/* These reads also clear the count registers.  */
-	drop = cvmx_read_csr(CVMX_AGL_GMX_RXX_STATS_PKTS_DRP(port));
-	bad = cvmx_read_csr(CVMX_AGL_GMX_RXX_STATS_PKTS_BAD(port));
+	drop = cvmx_read_csr(p->agl + AGL_GMX_RX_STATS_PKTS_DRP);
+	bad = cvmx_read_csr(p->agl + AGL_GMX_RX_STATS_PKTS_BAD);
 
 	if (drop || bad) {
 		/* Do an atomic update. */
@@ -261,15 +311,14 @@ static void octeon_mgmt_update_rx_stats(struct net_device *netdev)
 static void octeon_mgmt_update_tx_stats(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	unsigned long flags;
 
 	union cvmx_agl_gmx_txx_stat0 s0;
 	union cvmx_agl_gmx_txx_stat1 s1;
 
 	/* These reads also clear the count registers.  */
-	s0.u64 = cvmx_read_csr(CVMX_AGL_GMX_TXX_STAT0(port));
-	s1.u64 = cvmx_read_csr(CVMX_AGL_GMX_TXX_STAT1(port));
+	s0.u64 = cvmx_read_csr(p->agl + AGL_GMX_TX_STAT0);
+	s1.u64 = cvmx_read_csr(p->agl + AGL_GMX_TX_STAT1);
 
 	if (s0.s.xsdef || s0.s.xscol || s1.s.scol || s1.s.mcol) {
 		/* Do an atomic update. */
@@ -308,7 +357,6 @@ static u64 octeon_mgmt_dequeue_rx_buffer(struct octeon_mgmt *p,
 
 static int octeon_mgmt_receive_one(struct octeon_mgmt *p)
 {
-	int port = p->port;
 	struct net_device *netdev = p->netdev;
 	union cvmx_mixx_ircnt mix_ircnt;
 	union mgmt_port_ring_entry re;
@@ -381,18 +429,17 @@ done:
 	/* Tell the hardware we processed a packet.  */
 	mix_ircnt.u64 = 0;
 	mix_ircnt.s.ircnt = 1;
-	cvmx_write_csr(CVMX_MIXX_IRCNT(port), mix_ircnt.u64);
+	cvmx_write_csr(p->mix + MIX_IRCNT, mix_ircnt.u64);
 	return rc;
 }
 
 static int octeon_mgmt_receive_packets(struct octeon_mgmt *p, int budget)
 {
-	int port = p->port;
 	unsigned int work_done = 0;
 	union cvmx_mixx_ircnt mix_ircnt;
 	int rc;
 
-	mix_ircnt.u64 = cvmx_read_csr(CVMX_MIXX_IRCNT(port));
+	mix_ircnt.u64 = cvmx_read_csr(p->mix + MIX_IRCNT);
 	while (work_done < budget && mix_ircnt.s.ircnt) {
 
 		rc = octeon_mgmt_receive_one(p);
@@ -400,7 +447,7 @@ static int octeon_mgmt_receive_packets(struct octeon_mgmt *p, int budget)
 			work_done++;
 
 		/* Check for more packets. */
-		mix_ircnt.u64 = cvmx_read_csr(CVMX_MIXX_IRCNT(port));
+		mix_ircnt.u64 = cvmx_read_csr(p->mix + MIX_IRCNT);
 	}
 
 	octeon_mgmt_rx_fill_ring(p->netdev);
@@ -434,16 +481,16 @@ static void octeon_mgmt_reset_hw(struct octeon_mgmt *p)
 	union cvmx_agl_gmx_bist agl_gmx_bist;
 
 	mix_ctl.u64 = 0;
-	cvmx_write_csr(CVMX_MIXX_CTL(p->port), mix_ctl.u64);
+	cvmx_write_csr(p->mix + MIX_CTL, mix_ctl.u64);
 	do {
-		mix_ctl.u64 = cvmx_read_csr(CVMX_MIXX_CTL(p->port));
+		mix_ctl.u64 = cvmx_read_csr(p->mix + MIX_CTL);
 	} while (mix_ctl.s.busy);
 	mix_ctl.s.reset = 1;
-	cvmx_write_csr(CVMX_MIXX_CTL(p->port), mix_ctl.u64);
-	cvmx_read_csr(CVMX_MIXX_CTL(p->port));
+	cvmx_write_csr(p->mix + MIX_CTL, mix_ctl.u64);
+	cvmx_read_csr(p->mix + MIX_CTL);
 	cvmx_wait(64);
 
-	mix_bist.u64 = cvmx_read_csr(CVMX_MIXX_BIST(p->port));
+	mix_bist.u64 = cvmx_read_csr(p->mix + MIX_BIST);
 	if (mix_bist.u64)
 		dev_warn(p->dev, "MIX failed BIST (0x%016llx)\n",
 			(unsigned long long)mix_bist.u64);
@@ -474,7 +521,6 @@ static void octeon_mgmt_cam_state_add(struct octeon_mgmt_cam_state *cs,
 static void octeon_mgmt_set_rx_filtering(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	union cvmx_agl_gmx_rxx_adr_ctl adr_ctl;
 	union cvmx_agl_gmx_prtx_cfg agl_gmx_prtx;
 	unsigned long flags;
@@ -520,29 +566,29 @@ static void octeon_mgmt_set_rx_filtering(struct net_device *netdev)
 	spin_lock_irqsave(&p->lock, flags);
 
 	/* Disable packet I/O. */
-	agl_gmx_prtx.u64 = cvmx_read_csr(CVMX_AGL_GMX_PRTX_CFG(port));
+	agl_gmx_prtx.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
 	prev_packet_enable = agl_gmx_prtx.s.en;
 	agl_gmx_prtx.s.en = 0;
-	cvmx_write_csr(CVMX_AGL_GMX_PRTX_CFG(port), agl_gmx_prtx.u64);
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, agl_gmx_prtx.u64);
 
 	adr_ctl.u64 = 0;
 	adr_ctl.s.cam_mode = cam_mode;
 	adr_ctl.s.mcst = multicast_mode;
 	adr_ctl.s.bcst = 1;     /* Allow broadcast */
 
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CTL(port), adr_ctl.u64);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CTL, adr_ctl.u64);
 
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM0(port), cam_state.cam[0]);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM1(port), cam_state.cam[1]);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM2(port), cam_state.cam[2]);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM3(port), cam_state.cam[3]);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM4(port), cam_state.cam[4]);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM5(port), cam_state.cam[5]);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_ADR_CAM_EN(port), cam_state.cam_mask);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM0, cam_state.cam[0]);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM1, cam_state.cam[1]);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM2, cam_state.cam[2]);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM3, cam_state.cam[3]);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM4, cam_state.cam[4]);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM5, cam_state.cam[5]);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_ADR_CAM_EN, cam_state.cam_mask);
 
 	/* Restore packet I/O. */
 	agl_gmx_prtx.s.en = prev_packet_enable;
-	cvmx_write_csr(CVMX_AGL_GMX_PRTX_CFG(port), agl_gmx_prtx.u64);
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, agl_gmx_prtx.u64);
 
 	spin_unlock_irqrestore(&p->lock, flags);
 }
@@ -564,7 +610,6 @@ static int octeon_mgmt_set_mac_address(struct net_device *netdev, void *addr)
 static int octeon_mgmt_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	int size_without_fcs = new_mtu + OCTEON_MGMT_RX_HEADROOM;
 
 	/*
@@ -580,8 +625,8 @@ static int octeon_mgmt_change_mtu(struct net_device *netdev, int new_mtu)
 
 	netdev->mtu = new_mtu;
 
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_FRM_MAX(port), size_without_fcs);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_JABBER(port),
+	cvmx_write_csr(p->agl + AGL_GMX_RX_FRM_MAX, size_without_fcs);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_JABBER,
 		       (size_without_fcs + 7) & 0xfff8);
 
 	return 0;
@@ -591,14 +636,13 @@ static irqreturn_t octeon_mgmt_interrupt(int cpl, void *dev_id)
 {
 	struct net_device *netdev = dev_id;
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	union cvmx_mixx_isr mixx_isr;
 
-	mixx_isr.u64 = cvmx_read_csr(CVMX_MIXX_ISR(port));
+	mixx_isr.u64 = cvmx_read_csr(p->mix + MIX_ISR);
 
 	/* Clear any pending interrupts */
-	cvmx_write_csr(CVMX_MIXX_ISR(port), mixx_isr.u64);
-	cvmx_read_csr(CVMX_MIXX_ISR(port));
+	cvmx_write_csr(p->mix + MIX_ISR, mixx_isr.u64);
+	cvmx_read_csr(p->mix + MIX_ISR);
 
 	if (mixx_isr.s.irthresh) {
 		octeon_mgmt_disable_rx_irq(p);
@@ -629,7 +673,6 @@ static int octeon_mgmt_ioctl(struct net_device *netdev,
 static void octeon_mgmt_adjust_link(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	union cvmx_agl_gmx_prtx_cfg prtx_cfg;
 	unsigned long flags;
 	int link_changed = 0;
@@ -640,11 +683,9 @@ static void octeon_mgmt_adjust_link(struct net_device *netdev)
 			link_changed = 1;
 		if (p->last_duplex != p->phydev->duplex) {
 			p->last_duplex = p->phydev->duplex;
-			prtx_cfg.u64 =
-				cvmx_read_csr(CVMX_AGL_GMX_PRTX_CFG(port));
+			prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
 			prtx_cfg.s.duplex = p->phydev->duplex;
-			cvmx_write_csr(CVMX_AGL_GMX_PRTX_CFG(port),
-				       prtx_cfg.u64);
+			cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
 		}
 	} else {
 		if (p->last_link)
@@ -670,18 +711,16 @@ static void octeon_mgmt_adjust_link(struct net_device *netdev)
 static int octeon_mgmt_init_phy(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	char phy_id[MII_BUS_ID_SIZE + 3];
 
-	if (octeon_is_simulation()) {
+	if (octeon_is_simulation() || p->phy_np == NULL) {
 		/* No PHYs in the simulator. */
 		netif_carrier_on(netdev);
 		return 0;
 	}
 
-	snprintf(phy_id, sizeof(phy_id), PHY_ID_FMT, "mdio-octeon-0", p->port);
-
-	p->phydev = phy_connect(netdev, phy_id, octeon_mgmt_adjust_link, 0,
-				PHY_INTERFACE_MODE_MII);
+	p->phydev = of_phy_connect(netdev, p->phy_np,
+				   octeon_mgmt_adjust_link, 0,
+				   PHY_INTERFACE_MODE_MII);
 
 	if (IS_ERR(p->phydev)) {
 		p->phydev = NULL;
@@ -737,14 +776,14 @@ static int octeon_mgmt_open(struct net_device *netdev)
 
 	octeon_mgmt_reset_hw(p);
 
-	mix_ctl.u64 = cvmx_read_csr(CVMX_MIXX_CTL(port));
+	mix_ctl.u64 = cvmx_read_csr(p->mix + MIX_CTL);
 
 	/* Bring it out of reset if needed. */
 	if (mix_ctl.s.reset) {
 		mix_ctl.s.reset = 0;
-		cvmx_write_csr(CVMX_MIXX_CTL(port), mix_ctl.u64);
+		cvmx_write_csr(p->mix + MIX_CTL, mix_ctl.u64);
 		do {
-			mix_ctl.u64 = cvmx_read_csr(CVMX_MIXX_CTL(port));
+			mix_ctl.u64 = cvmx_read_csr(p->mix + MIX_CTL);
 		} while (mix_ctl.s.reset);
 	}
 
@@ -755,17 +794,17 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	oring1.u64 = 0;
 	oring1.s.obase = p->tx_ring_handle >> 3;
 	oring1.s.osize = OCTEON_MGMT_TX_RING_SIZE;
-	cvmx_write_csr(CVMX_MIXX_ORING1(port), oring1.u64);
+	cvmx_write_csr(p->mix + MIX_ORING1, oring1.u64);
 
 	iring1.u64 = 0;
 	iring1.s.ibase = p->rx_ring_handle >> 3;
 	iring1.s.isize = OCTEON_MGMT_RX_RING_SIZE;
-	cvmx_write_csr(CVMX_MIXX_IRING1(port), iring1.u64);
+	cvmx_write_csr(p->mix + MIX_IRING1, iring1.u64);
 
 	/* Disable packet I/O. */
-	prtx_cfg.u64 = cvmx_read_csr(CVMX_AGL_GMX_PRTX_CFG(port));
+	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
 	prtx_cfg.s.en = 0;
-	cvmx_write_csr(CVMX_AGL_GMX_PRTX_CFG(port), prtx_cfg.u64);
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
 
 	memcpy(sa.sa_data, netdev->dev_addr, ETH_ALEN);
 	octeon_mgmt_set_mac_address(netdev, &sa);
@@ -782,7 +821,7 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	mix_ctl.s.nbtarb = 0;       /* Arbitration mode */
 	/* MII CB-request FIFO programmable high watermark */
 	mix_ctl.s.mrq_hwm = 1;
-	cvmx_write_csr(CVMX_MIXX_CTL(port), mix_ctl.u64);
+	cvmx_write_csr(p->mix + MIX_CTL, mix_ctl.u64);
 
 	if (OCTEON_IS_MODEL(OCTEON_CN56XX_PASS1_X)
 	    || OCTEON_IS_MODEL(OCTEON_CN52XX_PASS1_X)) {
@@ -809,16 +848,16 @@ static int octeon_mgmt_open(struct net_device *netdev)
 
 	/* Clear statistics. */
 	/* Clear on read. */
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_STATS_CTL(port), 1);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_STATS_PKTS_DRP(port), 0);
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_STATS_PKTS_BAD(port), 0);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_STATS_CTL, 1);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_STATS_PKTS_DRP, 0);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_STATS_PKTS_BAD, 0);
 
-	cvmx_write_csr(CVMX_AGL_GMX_TXX_STATS_CTL(port), 1);
-	cvmx_write_csr(CVMX_AGL_GMX_TXX_STAT0(port), 0);
-	cvmx_write_csr(CVMX_AGL_GMX_TXX_STAT1(port), 0);
+	cvmx_write_csr(p->agl + AGL_GMX_TX_STATS_CTL, 1);
+	cvmx_write_csr(p->agl + AGL_GMX_TX_STAT0, 0);
+	cvmx_write_csr(p->agl + AGL_GMX_TX_STAT1, 0);
 
 	/* Clear any pending interrupts */
-	cvmx_write_csr(CVMX_MIXX_ISR(port), cvmx_read_csr(CVMX_MIXX_ISR(port)));
+	cvmx_write_csr(p->mix + MIX_ISR, cvmx_read_csr(p->mix + MIX_ISR));
 
 	if (request_irq(p->irq, octeon_mgmt_interrupt, 0, netdev->name,
 			netdev)) {
@@ -829,18 +868,18 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	/* Interrupt every single RX packet */
 	mix_irhwm.u64 = 0;
 	mix_irhwm.s.irhwm = 0;
-	cvmx_write_csr(CVMX_MIXX_IRHWM(port), mix_irhwm.u64);
+	cvmx_write_csr(p->mix + MIX_IRHWM, mix_irhwm.u64);
 
 	/* Interrupt when we have 1 or more packets to clean.  */
 	mix_orhwm.u64 = 0;
 	mix_orhwm.s.orhwm = 1;
-	cvmx_write_csr(CVMX_MIXX_ORHWM(port), mix_orhwm.u64);
+	cvmx_write_csr(p->mix + MIX_ORHWM, mix_orhwm.u64);
 
 	/* Enable receive and transmit interrupts */
 	mix_intena.u64 = 0;
 	mix_intena.s.ithena = 1;
 	mix_intena.s.othena = 1;
-	cvmx_write_csr(CVMX_MIXX_INTENA(port), mix_intena.u64);
+	cvmx_write_csr(p->mix + MIX_INTENA, mix_intena.u64);
 
 
 	/* Enable packet I/O. */
@@ -871,7 +910,7 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	 * frame.  GMX checks that the PREAMBLE is sent correctly.
 	 */
 	rxx_frm_ctl.s.pre_chk = 1;
-	cvmx_write_csr(CVMX_AGL_GMX_RXX_FRM_CTL(port), rxx_frm_ctl.u64);
+	cvmx_write_csr(p->agl + AGL_GMX_RX_FRM_CTL, rxx_frm_ctl.u64);
 
 	/* Enable the AGL block */
 	agl_gmx_inf_mode.u64 = 0;
@@ -879,13 +918,13 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	cvmx_write_csr(CVMX_AGL_GMX_INF_MODE, agl_gmx_inf_mode.u64);
 
 	/* Configure the port duplex and enables */
-	prtx_cfg.u64 = cvmx_read_csr(CVMX_AGL_GMX_PRTX_CFG(port));
+	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
 	prtx_cfg.s.tx_en = 1;
 	prtx_cfg.s.rx_en = 1;
 	prtx_cfg.s.en = 1;
 	p->last_duplex = 1;
 	prtx_cfg.s.duplex = p->last_duplex;
-	cvmx_write_csr(CVMX_AGL_GMX_PRTX_CFG(port), prtx_cfg.u64);
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
 
 	p->last_link = 0;
 	netif_carrier_off(netdev);
@@ -949,7 +988,6 @@ static int octeon_mgmt_stop(struct net_device *netdev)
 static int octeon_mgmt_xmit(struct sk_buff *skb, struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	union mgmt_port_ring_entry re;
 	unsigned long flags;
 	int rv = NETDEV_TX_BUSY;
@@ -993,7 +1031,7 @@ static int octeon_mgmt_xmit(struct sk_buff *skb, struct net_device *netdev)
 	netdev->stats.tx_bytes += skb->len;
 
 	/* Ring the bell.  */
-	cvmx_write_csr(CVMX_MIXX_ORING2(port), 1);
+	cvmx_write_csr(p->mix + MIX_ORING2, 1);
 
 	rv = NETDEV_TX_OK;
 out:
@@ -1071,10 +1109,14 @@ static const struct net_device_ops octeon_mgmt_ops = {
 
 static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 {
-	struct resource *res_irq;
 	struct net_device *netdev;
 	struct octeon_mgmt *p;
-	int i;
+	const __be32 *data;
+	const u8 *mac;
+	struct resource *res_mix;
+	struct resource *res_agl;
+	int len;
+	int result;
 
 	netdev = alloc_etherdev(sizeof(struct octeon_mgmt));
 	if (netdev == NULL)
@@ -1088,14 +1130,63 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 	p->netdev = netdev;
 	p->dev = &pdev->dev;
 
-	p->port = pdev->id;
+	data = of_get_property(pdev->dev.of_node, "cell-index", &len);
+	if (data && len == sizeof(*data)) {
+		p->port = be32_to_cpup(data);
+	} else {
+		dev_err(&pdev->dev, "no 'cell-index' property\n");
+		result = -ENXIO;
+		goto err;
+	}
+
 	snprintf(netdev->name, IFNAMSIZ, "mgmt%d", p->port);
 
-	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!res_irq)
+	result = platform_get_irq(pdev, 0);
+	if (result < 0)
+		goto err;
+
+	p->irq = result;
+
+	res_mix = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res_mix == NULL) {
+		dev_err(&pdev->dev, "no 'reg' resource\n");
+		result = -ENXIO;
+		goto err;
+	}
+
+	res_agl = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (res_agl == NULL) {
+		dev_err(&pdev->dev, "no 'reg' resource\n");
+		result = -ENXIO;
+		goto err;
+	}
+
+	p->mix_phys = res_mix->start;
+	p->mix_size = resource_size(res_mix);
+	p->agl_phys = res_agl->start;
+	p->agl_size = resource_size(res_agl);
+
+
+	if (!devm_request_mem_region(&pdev->dev, p->mix_phys, p->mix_size,
+				     res_mix->name)) {
+		dev_err(&pdev->dev, "request_mem_region (%s) failed\n",
+			res_mix->name);
+		result = -ENXIO;
+		goto err;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, p->agl_phys, p->agl_size,
+				     res_agl->name)) {
+		result = -ENXIO;
+		dev_err(&pdev->dev, "request_mem_region (%s) failed\n",
+			res_agl->name);
 		goto err;
+	}
+
+
+	p->mix = (u64)devm_ioremap(&pdev->dev, p->mix_phys, p->mix_size);
+	p->agl = (u64)devm_ioremap(&pdev->dev, p->agl_phys, p->agl_size);
 
-	p->irq = res_irq->start;
 	spin_lock_init(&p->lock);
 
 	skb_queue_head_init(&p->tx_list);
@@ -1108,24 +1199,26 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 	netdev->netdev_ops = &octeon_mgmt_ops;
 	netdev->ethtool_ops = &octeon_mgmt_ethtool_ops;
 
-	/* The mgmt ports get the first N MACs.  */
-	for (i = 0; i < 6; i++)
-		netdev->dev_addr[i] = octeon_bootinfo->mac_addr_base[i];
-	netdev->dev_addr[5] += p->port;
+	mac = of_get_mac_address(pdev->dev.of_node);
+
+	if (mac)
+		memcpy(netdev->dev_addr, mac, 6);
 
-	if (p->port >= octeon_bootinfo->mac_addr_count)
-		dev_err(&pdev->dev,
-			"Error %s: Using MAC outside of the assigned range: %pM\n",
-			netdev->name, netdev->dev_addr);
+	p->phy_np = of_parse_phandle(pdev->dev.of_node, "phy-handle", 0);
 
-	if (register_netdev(netdev))
+	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(64);
+	pdev->dev.dma_mask = &pdev->dev.coherent_dma_mask;
+
+	result = register_netdev(netdev);
+	if (result)
 		goto err;
 
 	dev_info(&pdev->dev, "Version " DRV_VERSION "\n");
 	return 0;
+
 err:
 	free_netdev(netdev);
-	return -ENOENT;
+	return result;
 }
 
 static int __devexit octeon_mgmt_remove(struct platform_device *pdev)
@@ -1137,10 +1230,19 @@ static int __devexit octeon_mgmt_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static struct of_device_id octeon_mgmt_match[] = {
+	{
+		.compatible = "cavium,octeon-5750-mix",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, octeon_mgmt_match);
+
 static struct platform_driver octeon_mgmt_driver = {
 	.driver = {
 		.name		= "octeon_mgmt",
 		.owner		= THIS_MODULE,
+		.of_match_table = octeon_mgmt_match,
 	},
 	.probe		= octeon_mgmt_probe,
 	.remove		= __devexit_p(octeon_mgmt_remove),
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_api.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_api.c
index e48f084ad22..5ae03e815ee 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_api.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_api.c
@@ -60,7 +60,7 @@ static void pch_gbe_plat_get_bus_info(struct pch_gbe_hw *hw)
 /**
  * pch_gbe_plat_init_hw - Initialize hardware
  * @hw:	Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed-EBUSY
  */
@@ -108,7 +108,7 @@ static void pch_gbe_plat_init_function_pointers(struct pch_gbe_hw *hw)
 /**
  * pch_gbe_hal_setup_init_funcs - Initializes function pointers
  * @hw:	Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:	Successfully
  *	ENOSYS:	Function is not registered
  */
@@ -137,7 +137,7 @@ inline void pch_gbe_hal_get_bus_info(struct pch_gbe_hw *hw)
 /**
  * pch_gbe_hal_init_hw - Initialize hardware
  * @hw:	Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:	Successfully
  *	ENOSYS:	Function is not registered
  */
@@ -155,7 +155,7 @@ inline s32 pch_gbe_hal_init_hw(struct pch_gbe_hw *hw)
  * @hw:	    Pointer to the HW structure
  * @offset: The register to read
  * @data:   The buffer to store the 16-bit read.
- * Returns
+ * Returns:
  *	0:	Successfully
  *	Negative value:	Failed
  */
@@ -172,7 +172,7 @@ inline s32 pch_gbe_hal_read_phy_reg(struct pch_gbe_hw *hw, u32 offset,
  * @hw:	    Pointer to the HW structure
  * @offset: The register to read
  * @data:   The value to write.
- * Returns
+ * Returns:
  *	0:	Successfully
  *	Negative value:	Failed
  */
@@ -211,7 +211,7 @@ inline void pch_gbe_hal_phy_sw_reset(struct pch_gbe_hw *hw)
 /**
  * pch_gbe_hal_read_mac_addr - Reads MAC address
  * @hw:	Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:	Successfully
  *	ENOSYS:	Function is not registered
  */
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c
index ac4e72d529e..24b787be606 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_ethtool.c
@@ -77,7 +77,7 @@ static const struct pch_gbe_stats pch_gbe_gstrings_stats[] = {
  * pch_gbe_get_settings - Get device-specific settings
  * @netdev: Network interface device structure
  * @ecmd:   Ethtool command
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
@@ -100,7 +100,7 @@ static int pch_gbe_get_settings(struct net_device *netdev,
  * pch_gbe_set_settings - Set device-specific settings
  * @netdev: Network interface device structure
  * @ecmd:   Ethtool command
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
@@ -129,7 +129,6 @@ static int pch_gbe_set_settings(struct net_device *netdev,
 	hw->mac.link_duplex = ecmd->duplex;
 	hw->phy.autoneg_advertised = ecmd->advertising;
 	hw->mac.autoneg = ecmd->autoneg;
-	pch_gbe_hal_phy_sw_reset(hw);
 
 	/* reset the link */
 	if (netif_running(adapter->netdev)) {
@@ -220,7 +219,7 @@ static void pch_gbe_get_wol(struct net_device *netdev,
  * pch_gbe_set_wol - Turn Wake-on-Lan on or off
  * @netdev: Network interface device structure
  * @wol:    Pointer of wake-on-Lan information straucture
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
@@ -248,7 +247,7 @@ static int pch_gbe_set_wol(struct net_device *netdev,
 /**
  * pch_gbe_nway_reset - Restart autonegotiation
  * @netdev: Network interface device structure
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
@@ -398,7 +397,7 @@ static void pch_gbe_get_pauseparam(struct net_device *netdev,
  * pch_gbe_set_pauseparam - Set pause paramters
  * @netdev:  Network interface device structure
  * @pause:   Pause parameters structure
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
index 3787c64ee71..feb85d56c75 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_main.c
@@ -26,7 +26,7 @@
 #include <linux/ptp_classify.h>
 #endif
 
-#define DRV_VERSION     "1.00"
+#define DRV_VERSION     "1.01"
 const char pch_driver_version[] = DRV_VERSION;
 
 #define PCI_DEVICE_ID_INTEL_IOH1_GBE	0x8802		/* Pci device ID */
@@ -35,7 +35,7 @@ const char pch_driver_version[] = DRV_VERSION;
 #define DSC_INIT16			0xC000
 #define PCH_GBE_DMA_ALIGN		0
 #define PCH_GBE_DMA_PADDING		2
-#define PCH_GBE_WATCHDOG_PERIOD		(1 * HZ)	/* watchdog time */
+#define PCH_GBE_WATCHDOG_PERIOD		(5 * HZ)	/* watchdog time */
 #define PCH_GBE_COPYBREAK_DEFAULT	256
 #define PCH_GBE_PCI_BAR			1
 #define PCH_GBE_RESERVE_MEMORY		0x200000	/* 2MB */
@@ -301,7 +301,7 @@ inline void pch_gbe_mac_load_mac_addr(struct pch_gbe_hw *hw)
 /**
  * pch_gbe_mac_read_mac_addr - Read MAC address
  * @hw:	            Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:			Successful.
  */
 s32 pch_gbe_mac_read_mac_addr(struct pch_gbe_hw *hw)
@@ -483,7 +483,7 @@ static void pch_gbe_mac_mc_addr_list_update(struct pch_gbe_hw *hw,
 /**
  * pch_gbe_mac_force_mac_fc - Force the MAC's flow control settings
  * @hw:	            Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
@@ -639,7 +639,7 @@ static void pch_gbe_mac_set_pause_packet(struct pch_gbe_hw *hw)
 /**
  * pch_gbe_alloc_queues - Allocate memory for all rings
  * @adapter:  Board private structure to initialize
- * Returns
+ * Returns:
  *	0:	Successfully
  *	Negative value:	Failed
  */
@@ -670,7 +670,7 @@ static void pch_gbe_init_stats(struct pch_gbe_adapter *adapter)
 /**
  * pch_gbe_init_phy - Initialize PHY
  * @adapter:  Board private structure to initialize
- * Returns
+ * Returns:
  *	0:	Successfully
  *	Negative value:	Failed
  */
@@ -720,7 +720,7 @@ static int pch_gbe_init_phy(struct pch_gbe_adapter *adapter)
  * @netdev: Network interface device structure
  * @addr:   Phy ID
  * @reg:    Access location
- * Returns
+ * Returns:
  *	0:	Successfully
  *	Negative value:	Failed
  */
@@ -1364,7 +1364,7 @@ static void pch_gbe_start_receive(struct pch_gbe_hw *hw)
  * pch_gbe_intr - Interrupt Handler
  * @irq:   Interrupt number
  * @data:  Pointer to a network interface device structure
- * Returns
+ * Returns:
  *	- IRQ_HANDLED:	Our interrupt
  *	- IRQ_NONE:	Not our interrupt
  */
@@ -1566,7 +1566,7 @@ static void pch_gbe_alloc_tx_buffers(struct pch_gbe_adapter *adapter,
  * pch_gbe_clean_tx - Reclaim resources after transmit completes
  * @adapter:   Board private structure
  * @tx_ring:   Tx descriptor ring
- * Returns
+ * Returns:
  *	true:  Cleaned the descriptor
  *	false: Not cleaned the descriptor
  */
@@ -1579,7 +1579,8 @@ pch_gbe_clean_tx(struct pch_gbe_adapter *adapter,
 	struct sk_buff *skb;
 	unsigned int i;
 	unsigned int cleaned_count = 0;
-	bool cleaned = true;
+	bool cleaned = false;
+	int unused, thresh;
 
 	pr_debug("next_to_clean : %d\n", tx_ring->next_to_clean);
 
@@ -1588,10 +1589,36 @@ pch_gbe_clean_tx(struct pch_gbe_adapter *adapter,
 	pr_debug("gbec_status:0x%04x  dma_status:0x%04x\n",
 		 tx_desc->gbec_status, tx_desc->dma_status);
 
+	unused = PCH_GBE_DESC_UNUSED(tx_ring);
+	thresh = tx_ring->count - PCH_GBE_TX_WEIGHT;
+	if ((tx_desc->gbec_status == DSC_INIT16) && (unused < thresh))
+	{  /* current marked clean, tx queue filling up, do extra clean */
+		int j, k;
+		if (unused < 8) {  /* tx queue nearly full */
+			pr_debug("clean_tx: transmit queue warning (%x,%x) unused=%d\n",
+				tx_ring->next_to_clean,tx_ring->next_to_use,unused);
+		}
+
+		/* current marked clean, scan for more that need cleaning. */
+		k = i;
+		for (j = 0; j < PCH_GBE_TX_WEIGHT; j++)
+		{
+			tx_desc = PCH_GBE_TX_DESC(*tx_ring, k);
+			if (tx_desc->gbec_status != DSC_INIT16) break; /*found*/
+			if (++k >= tx_ring->count) k = 0;  /*increment, wrap*/
+		}
+		if (j < PCH_GBE_TX_WEIGHT) {
+			pr_debug("clean_tx: unused=%d loops=%d found tx_desc[%x,%x:%x].gbec_status=%04x\n",
+				unused,j, i,k, tx_ring->next_to_use, tx_desc->gbec_status);
+			i = k;  /*found one to clean, usu gbec_status==2000.*/
+		}
+	}
+
 	while ((tx_desc->gbec_status & DSC_INIT16) == 0x0000) {
 		pr_debug("gbec_status:0x%04x\n", tx_desc->gbec_status);
 		buffer_info = &tx_ring->buffer_info[i];
 		skb = buffer_info->skb;
+		cleaned = true;
 
 		if ((tx_desc->gbec_status & PCH_GBE_TXD_GMAC_STAT_ABT)) {
 			adapter->stats.tx_aborted_errors++;
@@ -1639,18 +1666,21 @@ pch_gbe_clean_tx(struct pch_gbe_adapter *adapter,
 	}
 	pr_debug("called pch_gbe_unmap_and_free_tx_resource() %d count\n",
 		 cleaned_count);
-	/* Recover from running out of Tx resources in xmit_frame */
-	spin_lock(&tx_ring->tx_lock);
-	if (unlikely(cleaned && (netif_queue_stopped(adapter->netdev)))) {
-		netif_wake_queue(adapter->netdev);
-		adapter->stats.tx_restart_count++;
-		pr_debug("Tx wake queue\n");
-	}
+	if (cleaned_count > 0)  { /*skip this if nothing cleaned*/
+		/* Recover from running out of Tx resources in xmit_frame */
+		spin_lock(&tx_ring->tx_lock);
+		if (unlikely(cleaned && (netif_queue_stopped(adapter->netdev))))
+		{
+			netif_wake_queue(adapter->netdev);
+			adapter->stats.tx_restart_count++;
+			pr_debug("Tx wake queue\n");
+		}
 
-	tx_ring->next_to_clean = i;
+		tx_ring->next_to_clean = i;
 
-	pr_debug("next_to_clean : %d\n", tx_ring->next_to_clean);
-	spin_unlock(&tx_ring->tx_lock);
+		pr_debug("next_to_clean : %d\n", tx_ring->next_to_clean);
+		spin_unlock(&tx_ring->tx_lock);
+	}
 	return cleaned;
 }
 
@@ -1660,7 +1690,7 @@ pch_gbe_clean_tx(struct pch_gbe_adapter *adapter,
  * @rx_ring:     Rx descriptor ring
  * @work_done:   Completed count
  * @work_to_do:  Request count
- * Returns
+ * Returns:
  *	true:  Cleaned the descriptor
  *	false: Not cleaned the descriptor
  */
@@ -1775,7 +1805,7 @@ pch_gbe_clean_rx(struct pch_gbe_adapter *adapter,
  * pch_gbe_setup_tx_resources - Allocate Tx resources (Descriptors)
  * @adapter:  Board private structure
  * @tx_ring:  Tx descriptor ring (for a specific queue) to setup
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed
  */
@@ -1822,7 +1852,7 @@ int pch_gbe_setup_tx_resources(struct pch_gbe_adapter *adapter,
  * pch_gbe_setup_rx_resources - Allocate Rx resources (Descriptors)
  * @adapter:  Board private structure
  * @rx_ring:  Rx descriptor ring (for a specific queue) to setup
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed
  */
@@ -1899,7 +1929,7 @@ void pch_gbe_free_rx_resources(struct pch_gbe_adapter *adapter,
 /**
  * pch_gbe_request_irq - Allocate an interrupt line
  * @adapter:  Board private structure
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed
  */
@@ -1932,7 +1962,7 @@ static int pch_gbe_request_irq(struct pch_gbe_adapter *adapter)
 /**
  * pch_gbe_up - Up GbE network device
  * @adapter:  Board private structure
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed
  */
@@ -1988,6 +2018,7 @@ int pch_gbe_up(struct pch_gbe_adapter *adapter)
 void pch_gbe_down(struct pch_gbe_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
+	struct pci_dev *pdev = adapter->pdev;
 	struct pch_gbe_rx_ring *rx_ring = adapter->rx_ring;
 
 	/* signal that we're down so the interrupt handler does not
@@ -2004,7 +2035,8 @@ void pch_gbe_down(struct pch_gbe_adapter *adapter)
 	netif_carrier_off(netdev);
 	netif_stop_queue(netdev);
 
-	pch_gbe_reset(adapter);
+	if ((pdev->error_state) && (pdev->error_state != pci_channel_io_normal))
+		pch_gbe_reset(adapter);
 	pch_gbe_clean_tx_ring(adapter, adapter->tx_ring);
 	pch_gbe_clean_rx_ring(adapter, adapter->rx_ring);
 
@@ -2018,7 +2050,7 @@ void pch_gbe_down(struct pch_gbe_adapter *adapter)
 /**
  * pch_gbe_sw_init - Initialize general software structures (struct pch_gbe_adapter)
  * @adapter:  Board private structure to initialize
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed
  */
@@ -2057,7 +2089,7 @@ static int pch_gbe_sw_init(struct pch_gbe_adapter *adapter)
 /**
  * pch_gbe_open - Called when a network interface is made active
  * @netdev:	Network interface device structure
- * Returns
+ * Returns:
  *	0:		Successfully
  *	Negative value:	Failed
  */
@@ -2097,7 +2129,7 @@ err_setup_tx:
 /**
  * pch_gbe_stop - Disables a network interface
  * @netdev:  Network interface device structure
- * Returns
+ * Returns:
  *	0: Successfully
  */
 static int pch_gbe_stop(struct net_device *netdev)
@@ -2117,7 +2149,7 @@ static int pch_gbe_stop(struct net_device *netdev)
  * pch_gbe_xmit_frame - Packet transmitting start
  * @skb:     Socket buffer structure
  * @netdev:  Network interface device structure
- * Returns
+ * Returns:
  *	- NETDEV_TX_OK:   Normal end
  *	- NETDEV_TX_BUSY: Error end
  */
@@ -2127,13 +2159,6 @@ static int pch_gbe_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 	struct pch_gbe_tx_ring *tx_ring = adapter->tx_ring;
 	unsigned long flags;
 
-	if (unlikely(skb->len > (adapter->hw.mac.max_frame_size - 4))) {
-		pr_err("Transfer length Error: skb len: %d > max: %d\n",
-		       skb->len, adapter->hw.mac.max_frame_size);
-		dev_kfree_skb_any(skb);
-		adapter->stats.tx_length_errors++;
-		return NETDEV_TX_OK;
-	}
 	if (!spin_trylock_irqsave(&tx_ring->tx_lock, flags)) {
 		/* Collision - tell upper layer to requeue */
 		return NETDEV_TX_LOCKED;
@@ -2225,7 +2250,7 @@ static void pch_gbe_set_multi(struct net_device *netdev)
  * pch_gbe_set_mac - Change the Ethernet Address of the NIC
  * @netdev: Network interface device structure
  * @addr:   Pointer to an address structure
- * Returns
+ * Returns:
  *	0:		Successfully
  *	-EADDRNOTAVAIL:	Failed
  */
@@ -2256,7 +2281,7 @@ static int pch_gbe_set_mac(struct net_device *netdev, void *addr)
  * pch_gbe_change_mtu - Change the Maximum Transfer Unit
  * @netdev:   Network interface device structure
  * @new_mtu:  New value for maximum frame size
- * Returns
+ * Returns:
  *	0:		Successfully
  *	-EINVAL:	Failed
  */
@@ -2309,7 +2334,7 @@ static int pch_gbe_change_mtu(struct net_device *netdev, int new_mtu)
  * pch_gbe_set_features - Reset device after features changed
  * @netdev:   Network interface device structure
  * @features:  New features
- * Returns
+ * Returns:
  *	0:		HW state updated successfully
  */
 static int pch_gbe_set_features(struct net_device *netdev,
@@ -2334,7 +2359,7 @@ static int pch_gbe_set_features(struct net_device *netdev,
  * @netdev:   Network interface device structure
  * @ifr:      Pointer to ifr structure
  * @cmd:      Control command
- * Returns
+ * Returns:
  *	0:	Successfully
  *	Negative value:	Failed
  */
@@ -2369,7 +2394,7 @@ static void pch_gbe_tx_timeout(struct net_device *netdev)
  * pch_gbe_napi_poll - NAPI receive and transfer polling callback
  * @napi:    Pointer of polling device struct
  * @budget:  The maximum number of a packet
- * Returns
+ * Returns:
  *	false:  Exit the polling mode
  *	true:   Continue the polling mode
  */
@@ -2387,7 +2412,7 @@ static int pch_gbe_napi_poll(struct napi_struct *napi, int budget)
 	pch_gbe_clean_rx(adapter, adapter->rx_ring, &work_done, budget);
 	cleaned = pch_gbe_clean_tx(adapter, adapter->tx_ring);
 
-	if (!cleaned)
+	if (cleaned)
 		work_done = budget;
 	/* If no Tx and not enough Rx work done,
 	 * exit the polling mode
@@ -2793,6 +2818,7 @@ static int __init pch_gbe_init_module(void)
 {
 	int ret;
 
+	pr_info("EG20T PCH Gigabit Ethernet Driver - version %s\n",DRV_VERSION);
 	ret = pci_register_driver(&pch_gbe_driver);
 	if (copybreak != PCH_GBE_COPYBREAK_DEFAULT) {
 		if (copybreak == 0) {
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c
index 29e23bec809..8653c3b81f8 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c
@@ -139,7 +139,7 @@ MODULE_PARM_DESC(XsumTX, "Disable or enable Transmit Checksum offload");
 /**
  * pch_gbe_option - Force the MAC's flow control settings
  * @hw:	            Pointer to the HW structure
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
@@ -220,7 +220,7 @@ static const struct pch_gbe_opt_list fc_list[] = {
  * @value:    value
  * @opt:      option
  * @adapter:  Board private structure
- * Returns
+ * Returns:
  *	0:			Successful.
  *	Negative value:		Failed.
  */
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
index 37ccbe54e62..eb3dfdbb642 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic.h
@@ -53,8 +53,8 @@
 
 #define _NETXEN_NIC_LINUX_MAJOR 4
 #define _NETXEN_NIC_LINUX_MINOR 0
-#define _NETXEN_NIC_LINUX_SUBVERSION 79
-#define NETXEN_NIC_LINUX_VERSIONID  "4.0.79"
+#define _NETXEN_NIC_LINUX_SUBVERSION 80
+#define NETXEN_NIC_LINUX_VERSIONID  "4.0.80"
 
 #define NETXEN_VERSION_CODE(a, b, c)	(((a) << 24) + ((b) << 16) + (c))
 #define _major(v)	(((v) >> 24) & 0xff)
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c
index 39730403782..10468e7932d 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ethtool.c
@@ -489,7 +489,7 @@ netxen_nic_get_pauseparam(struct net_device *dev,
 	int port = adapter->physical_port;
 
 	if (adapter->ahw.port_type == NETXEN_NIC_GBE) {
-		if ((port < 0) || (port > NETXEN_NIU_MAX_GBE_PORTS))
+		if ((port < 0) || (port >= NETXEN_NIU_MAX_GBE_PORTS))
 			return;
 		/* get flow control settings */
 		val = NXRD32(adapter, NETXEN_NIU_GB_MAC_CONFIG_0(port));
@@ -511,7 +511,7 @@ netxen_nic_get_pauseparam(struct net_device *dev,
 				break;
 		}
 	} else if (adapter->ahw.port_type == NETXEN_NIC_XGBE) {
-		if ((port < 0) || (port > NETXEN_NIU_MAX_XG_PORTS))
+		if ((port < 0) || (port >= NETXEN_NIU_MAX_XG_PORTS))
 			return;
 		pause->rx_pause = 1;
 		val = NXRD32(adapter, NETXEN_NIU_XG_PAUSE_CTL);
@@ -534,7 +534,7 @@ netxen_nic_set_pauseparam(struct net_device *dev,
 	int port = adapter->physical_port;
 	/* read mode */
 	if (adapter->ahw.port_type == NETXEN_NIC_GBE) {
-		if ((port < 0) || (port > NETXEN_NIU_MAX_GBE_PORTS))
+		if ((port < 0) || (port >= NETXEN_NIU_MAX_GBE_PORTS))
 			return -EIO;
 		/* set flow control */
 		val = NXRD32(adapter, NETXEN_NIU_GB_MAC_CONFIG_0(port));
@@ -577,7 +577,7 @@ netxen_nic_set_pauseparam(struct net_device *dev,
 		}
 		NXWR32(adapter, NETXEN_NIU_GB_PAUSE_CTL, val);
 	} else if (adapter->ahw.port_type == NETXEN_NIC_XGBE) {
-		if ((port < 0) || (port > NETXEN_NIU_MAX_XG_PORTS))
+		if ((port < 0) || (port >= NETXEN_NIU_MAX_XG_PORTS))
 			return -EIO;
 		val = NXRD32(adapter, NETXEN_NIU_XG_PAUSE_CTL);
 		if (port == 0) {
@@ -826,7 +826,12 @@ netxen_get_dump_flag(struct net_device *netdev, struct ethtool_dump *dump)
 		dump->len = mdump->md_dump_size;
 	else
 		dump->len = 0;
-	dump->flag = mdump->md_capture_mask;
+
+	if (!mdump->md_enabled)
+		dump->flag = ETH_FW_DUMP_DISABLE;
+	else
+		dump->flag = mdump->md_capture_mask;
+
 	dump->version = adapter->fw_version;
 	return 0;
 }
@@ -840,8 +845,10 @@ netxen_set_dump(struct net_device *netdev, struct ethtool_dump *val)
 
 	switch (val->flag) {
 	case NX_FORCE_FW_DUMP_KEY:
-		if (!mdump->md_enabled)
-			mdump->md_enabled = 1;
+		if (!mdump->md_enabled) {
+			netdev_info(netdev, "FW dump not enabled\n");
+			return 0;
+		}
 		if (adapter->fw_mdump_rdy) {
 			netdev_info(netdev, "Previous dump not cleared, not forcing dump\n");
 			return 0;
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
index de96a948bb7..946160fa584 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
@@ -365,7 +365,7 @@ static int netxen_niu_disable_xg_port(struct netxen_adapter *adapter)
 	if (NX_IS_REVISION_P3(adapter->ahw.revision_id))
 		return 0;
 
-	if (port > NETXEN_NIU_MAX_XG_PORTS)
+	if (port >= NETXEN_NIU_MAX_XG_PORTS)
 		return -EINVAL;
 
 	mac_cfg = 0;
@@ -392,7 +392,7 @@ static int netxen_p2_nic_set_promisc(struct netxen_adapter *adapter, u32 mode)
 	u32 port = adapter->physical_port;
 	u16 board_type = adapter->ahw.board_type;
 
-	if (port > NETXEN_NIU_MAX_XG_PORTS)
+	if (port >= NETXEN_NIU_MAX_XG_PORTS)
 		return -EINVAL;
 
 	mac_cfg = NXRD32(adapter, NETXEN_NIU_XGE_CONFIG_0 + (0x10000 * port));
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
index 8694124ef77..bc165f4d0f6 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
@@ -1437,8 +1437,6 @@ netxen_handle_linkevent(struct netxen_adapter *adapter, nx_fw_msg_t *msg)
 				netdev->name, cable_len);
 	}
 
-	netxen_advert_link_change(adapter, link_status);
-
 	/* update link parameters */
 	if (duplex == LINKEVENT_FULL_DUPLEX)
 		adapter->link_duplex = DUPLEX_FULL;
@@ -1447,6 +1445,8 @@ netxen_handle_linkevent(struct netxen_adapter *adapter, nx_fw_msg_t *msg)
 	adapter->module_type = module;
 	adapter->link_autoneg = autoneg;
 	adapter->link_speed = link_speed;
+
+	netxen_advert_link_change(adapter, link_status);
 }
 
 static void
@@ -1532,8 +1532,6 @@ static struct sk_buff *netxen_process_rxbuf(struct netxen_adapter *adapter,
 	} else
 		skb->ip_summed = CHECKSUM_NONE;
 
-	skb->dev = adapter->netdev;
-
 	buffer->skb = NULL;
 no_skb:
 	buffer->state = NETXEN_BUFFER_FREE;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index 8680a5dae4a..eaa1db9fec3 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -36,8 +36,8 @@
 
 #define _QLCNIC_LINUX_MAJOR 5
 #define _QLCNIC_LINUX_MINOR 0
-#define _QLCNIC_LINUX_SUBVERSION 28
-#define QLCNIC_LINUX_VERSIONID  "5.0.28"
+#define _QLCNIC_LINUX_SUBVERSION 29
+#define QLCNIC_LINUX_VERSIONID  "5.0.29"
 #define QLCNIC_DRV_IDC_VER  0x01
 #define QLCNIC_DRIVER_VERSION  ((_QLCNIC_LINUX_MAJOR << 16) |\
 		 (_QLCNIC_LINUX_MINOR << 8) | (_QLCNIC_LINUX_SUBVERSION))
@@ -258,6 +258,8 @@ struct rcv_desc {
 	(((sts_data) >> 52) & 0x1)
 #define qlcnic_get_lro_sts_seq_number(sts_data)		\
 	((sts_data) & 0x0FFFFFFFF)
+#define qlcnic_get_lro_sts_mss(sts_data1)		\
+	((sts_data1 >> 32) & 0x0FFFF)
 
 
 struct status_desc {
@@ -610,7 +612,11 @@ struct qlcnic_recv_context {
 #define QLCNIC_CDRP_CMD_GET_MAC_STATS		0x00000037
 
 #define QLCNIC_RCODE_SUCCESS		0
+#define QLCNIC_RCODE_INVALID_ARGS	6
 #define QLCNIC_RCODE_NOT_SUPPORTED	9
+#define QLCNIC_RCODE_NOT_PERMITTED	10
+#define QLCNIC_RCODE_NOT_IMPL		15
+#define QLCNIC_RCODE_INVALID		16
 #define QLCNIC_RCODE_TIMEOUT		17
 #define QLCNIC_DESTROY_CTX_RESET	0
 
@@ -623,6 +629,7 @@ struct qlcnic_recv_context {
 #define QLCNIC_CAP0_JUMBO_CONTIGUOUS	(1 << 7)
 #define QLCNIC_CAP0_LRO_CONTIGUOUS	(1 << 8)
 #define QLCNIC_CAP0_VALIDOFF		(1 << 11)
+#define QLCNIC_CAP0_LRO_MSS		(1 << 21)
 
 /*
  * Context state
@@ -829,6 +836,9 @@ struct qlcnic_mac_list_s {
 #define QLCNIC_FW_CAPABILITY_FVLANTX		BIT_9
 #define QLCNIC_FW_CAPABILITY_HW_LRO		BIT_10
 #define QLCNIC_FW_CAPABILITY_MULTI_LOOPBACK	BIT_27
+#define QLCNIC_FW_CAPABILITY_MORE_CAPS		BIT_31
+
+#define QLCNIC_FW_CAPABILITY_2_LRO_MAX_TCP_SEG	BIT_2
 
 /* module types */
 #define LINKEVENT_MODULE_NOT_PRESENT			1
@@ -918,6 +928,7 @@ struct qlcnic_ipaddr {
 #define QLCNIC_NEED_FLR			0x1000
 #define QLCNIC_FW_RESET_OWNER		0x2000
 #define QLCNIC_FW_HANG			0x4000
+#define QLCNIC_FW_LRO_MSS_CAP		0x8000
 #define QLCNIC_IS_MSI_FAMILY(adapter) \
 	((adapter)->flags & (QLCNIC_MSI_ENABLED | QLCNIC_MSIX_ENABLED))
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
index 8db85244e8a..b8ead696141 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
@@ -53,12 +53,39 @@ qlcnic_issue_cmd(struct qlcnic_adapter *adapter, struct qlcnic_cmd_args *cmd)
 	rsp = qlcnic_poll_rsp(adapter);
 
 	if (rsp == QLCNIC_CDRP_RSP_TIMEOUT) {
-		dev_err(&pdev->dev, "card response timeout.\n");
+		dev_err(&pdev->dev, "CDRP response timeout.\n");
 		cmd->rsp.cmd = QLCNIC_RCODE_TIMEOUT;
 	} else if (rsp == QLCNIC_CDRP_RSP_FAIL) {
 		cmd->rsp.cmd = QLCRD32(adapter, QLCNIC_ARG1_CRB_OFFSET);
-		dev_err(&pdev->dev, "failed card response code:0x%x\n",
+		switch (cmd->rsp.cmd) {
+		case QLCNIC_RCODE_INVALID_ARGS:
+			dev_err(&pdev->dev, "CDRP invalid args: 0x%x.\n",
 				cmd->rsp.cmd);
+			break;
+		case QLCNIC_RCODE_NOT_SUPPORTED:
+		case QLCNIC_RCODE_NOT_IMPL:
+			dev_err(&pdev->dev,
+				"CDRP command not supported: 0x%x.\n",
+				cmd->rsp.cmd);
+			break;
+		case QLCNIC_RCODE_NOT_PERMITTED:
+			dev_err(&pdev->dev,
+				"CDRP requested action not permitted: 0x%x.\n",
+				cmd->rsp.cmd);
+			break;
+		case QLCNIC_RCODE_INVALID:
+			dev_err(&pdev->dev,
+				"CDRP invalid or unknown cmd received: 0x%x.\n",
+				cmd->rsp.cmd);
+			break;
+		case QLCNIC_RCODE_TIMEOUT:
+			dev_err(&pdev->dev, "CDRP command timeout: 0x%x.\n",
+				cmd->rsp.cmd);
+			break;
+		default:
+			dev_err(&pdev->dev, "CDRP command failed: 0x%x.\n",
+				cmd->rsp.cmd);
+		}
 	} else if (rsp == QLCNIC_CDRP_RSP_OK) {
 		cmd->rsp.cmd = QLCNIC_RCODE_SUCCESS;
 		if (cmd->rsp.arg2)
@@ -237,6 +264,9 @@ qlcnic_fw_cmd_create_rx_ctx(struct qlcnic_adapter *adapter)
 						| QLCNIC_CAP0_VALIDOFF);
 	cap |= (QLCNIC_CAP0_JUMBO_CONTIGUOUS | QLCNIC_CAP0_LRO_CONTIGUOUS);
 
+	if (adapter->flags & QLCNIC_FW_LRO_MSS_CAP)
+		cap |= QLCNIC_CAP0_LRO_MSS;
+
 	prq->valid_field_offset = offsetof(struct qlcnic_hostrq_rx_ctx,
 							 msix_handler);
 	prq->txrx_sds_binding = nsds_rings - 1;
@@ -954,9 +984,6 @@ int qlcnic_get_mac_stats(struct qlcnic_adapter *adapter,
 		mac_stats->mac_rx_jabber = le64_to_cpu(stats->mac_rx_jabber);
 		mac_stats->mac_rx_dropped = le64_to_cpu(stats->mac_rx_dropped);
 		mac_stats->mac_rx_crc_error = le64_to_cpu(stats->mac_rx_crc_error);
-	} else {
-		dev_info(&adapter->pdev->dev,
-			"%s: Get mac stats failed =%d.\n", __func__, err);
 	}
 
 	dma_free_coherent(&adapter->pdev->dev, stats_size, stats_addr,
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
index 6ced3195aad..28a6b28192e 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hdr.h
@@ -588,6 +588,7 @@ enum {
 #define CRB_DRIVER_VERSION		(QLCNIC_REG(0x2a0))
 
 #define CRB_FW_CAPABILITIES_1		(QLCNIC_CAM_RAM(0x128))
+#define CRB_FW_CAPABILITIES_2		(QLCNIC_CAM_RAM(0x12c))
 #define CRB_MAC_BLOCK_START		(QLCNIC_CAM_RAM(0x1c0))
 
 /*
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
index 799fd40ed03..0bcda9c51e9 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
@@ -1488,8 +1488,6 @@ static struct sk_buff *qlcnic_process_rxbuf(struct qlcnic_adapter *adapter,
 		skb_checksum_none_assert(skb);
 	}
 
-	skb->dev = adapter->netdev;
-
 	buffer->skb = NULL;
 
 	return skb;
@@ -1653,6 +1651,9 @@ qlcnic_process_lro(struct qlcnic_adapter *adapter,
 
 	length = skb->len;
 
+	if (adapter->flags & QLCNIC_FW_LRO_MSS_CAP)
+		skb_shinfo(skb)->gso_size = qlcnic_get_lro_sts_mss(sts_data1);
+
 	if (vid != 0xffff)
 		__vlan_hwaccel_put_tag(skb, vid);
 	netif_receive_skb(skb);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index ad98f4d7919..212c1219327 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -1136,6 +1136,8 @@ static int
 __qlcnic_up(struct qlcnic_adapter *adapter, struct net_device *netdev)
 {
 	int ring;
+	u32 capab2;
+
 	struct qlcnic_host_rds_ring *rds_ring;
 
 	if (adapter->is_up != QLCNIC_ADAPTER_UP_MAGIC)
@@ -1146,6 +1148,12 @@ __qlcnic_up(struct qlcnic_adapter *adapter, struct net_device *netdev)
 	if (qlcnic_set_eswitch_port_config(adapter))
 		return -EIO;
 
+	if (adapter->capabilities & QLCNIC_FW_CAPABILITY_MORE_CAPS) {
+		capab2 = QLCRD32(adapter, CRB_FW_CAPABILITIES_2);
+		if (capab2 & QLCNIC_FW_CAPABILITY_2_LRO_MAX_TCP_SEG)
+			adapter->flags |= QLCNIC_FW_LRO_MSS_CAP;
+	}
+
 	if (qlcnic_fw_create_ctx(adapter))
 		return -EIO;
 
@@ -1215,6 +1223,7 @@ __qlcnic_down(struct qlcnic_adapter *adapter, struct net_device *netdev)
 	qlcnic_napi_disable(adapter);
 
 	qlcnic_fw_destroy_ctx(adapter);
+	adapter->flags &= ~QLCNIC_FW_LRO_MSS_CAP;
 
 	qlcnic_reset_rx_buffers_list(adapter);
 	qlcnic_release_tx_buffers(adapter);
@@ -2024,6 +2033,7 @@ qlcnic_tx_pkt(struct qlcnic_adapter *adapter,
 		vh = (struct vlan_ethhdr *)skb->data;
 		flags = FLAGS_VLAN_TAGGED;
 		vlan_tci = vh->h_vlan_TCI;
+		protocol = ntohs(vh->h_vlan_encapsulated_proto);
 	} else if (vlan_tx_tag_present(skb)) {
 		flags = FLAGS_VLAN_OOB;
 		vlan_tci = vlan_tx_tag_get(skb);
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge.h b/drivers/net/ethernet/qlogic/qlge/qlge.h
index 5a639df33f1..a131d7b5d2f 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge.h
+++ b/drivers/net/ethernet/qlogic/qlge/qlge.h
@@ -18,13 +18,15 @@
  */
 #define DRV_NAME  	"qlge"
 #define DRV_STRING 	"QLogic 10 Gigabit PCI-E Ethernet Driver "
-#define DRV_VERSION	"v1.00.00.30.00.00-01"
+#define DRV_VERSION	"v1.00.00.31"
 
 #define WQ_ADDR_ALIGN	0x3	/* 4 byte alignment */
 
 #define QLGE_VENDOR_ID    0x1077
 #define QLGE_DEVICE_ID_8012	0x8012
 #define QLGE_DEVICE_ID_8000	0x8000
+#define QLGE_MEZZ_SSYS_ID_068	0x0068
+#define QLGE_MEZZ_SSYS_ID_180	0x0180
 #define MAX_CPUS 8
 #define MAX_TX_RINGS MAX_CPUS
 #define MAX_RX_RINGS ((MAX_CPUS * 2) + 1)
@@ -1397,7 +1399,6 @@ struct tx_ring {
 	struct tx_ring_desc *q;	/* descriptor list for the queue */
 	spinlock_t lock;
 	atomic_t tx_count;	/* counts down for every outstanding IO */
-	atomic_t queue_stopped;	/* Turns queue off when full. */
 	struct delayed_work tx_work;
 	struct ql_adapter *qdev;
 	u64 tx_packets;
@@ -1535,6 +1536,14 @@ struct nic_stats {
 	u64 rx_1024_to_1518_pkts;
 	u64 rx_1519_to_max_pkts;
 	u64 rx_len_err_pkts;
+	/* Receive Mac Err stats */
+	u64 rx_code_err;
+	u64 rx_oversize_err;
+	u64 rx_undersize_err;
+	u64 rx_preamble_err;
+	u64 rx_frame_len_err;
+	u64 rx_crc_err;
+	u64 rx_err_count;
 	/*
 	 * These stats come from offset 500h to 5C8h
 	 * in the XGMAC register.
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c b/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c
index 8e2c2a74f3a..6f316ab2325 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_ethtool.c
@@ -35,10 +35,152 @@
 
 #include "qlge.h"
 
+struct ql_stats {
+	char stat_string[ETH_GSTRING_LEN];
+	int sizeof_stat;
+	int stat_offset;
+};
+
+#define QL_SIZEOF(m) FIELD_SIZEOF(struct ql_adapter, m)
+#define QL_OFF(m) offsetof(struct ql_adapter, m)
+
+static const struct ql_stats ql_gstrings_stats[] = {
+	{"tx_pkts", QL_SIZEOF(nic_stats.tx_pkts), QL_OFF(nic_stats.tx_pkts)},
+	{"tx_bytes", QL_SIZEOF(nic_stats.tx_bytes), QL_OFF(nic_stats.tx_bytes)},
+	{"tx_mcast_pkts", QL_SIZEOF(nic_stats.tx_mcast_pkts),
+					QL_OFF(nic_stats.tx_mcast_pkts)},
+	{"tx_bcast_pkts", QL_SIZEOF(nic_stats.tx_bcast_pkts),
+					QL_OFF(nic_stats.tx_bcast_pkts)},
+	{"tx_ucast_pkts", QL_SIZEOF(nic_stats.tx_ucast_pkts),
+					QL_OFF(nic_stats.tx_ucast_pkts)},
+	{"tx_ctl_pkts", QL_SIZEOF(nic_stats.tx_ctl_pkts),
+					QL_OFF(nic_stats.tx_ctl_pkts)},
+	{"tx_pause_pkts", QL_SIZEOF(nic_stats.tx_pause_pkts),
+					QL_OFF(nic_stats.tx_pause_pkts)},
+	{"tx_64_pkts", QL_SIZEOF(nic_stats.tx_64_pkt),
+					QL_OFF(nic_stats.tx_64_pkt)},
+	{"tx_65_to_127_pkts", QL_SIZEOF(nic_stats.tx_65_to_127_pkt),
+					QL_OFF(nic_stats.tx_65_to_127_pkt)},
+	{"tx_128_to_255_pkts", QL_SIZEOF(nic_stats.tx_128_to_255_pkt),
+					QL_OFF(nic_stats.tx_128_to_255_pkt)},
+	{"tx_256_511_pkts", QL_SIZEOF(nic_stats.tx_256_511_pkt),
+					QL_OFF(nic_stats.tx_256_511_pkt)},
+	{"tx_512_to_1023_pkts",	QL_SIZEOF(nic_stats.tx_512_to_1023_pkt),
+					QL_OFF(nic_stats.tx_512_to_1023_pkt)},
+	{"tx_1024_to_1518_pkts", QL_SIZEOF(nic_stats.tx_1024_to_1518_pkt),
+					QL_OFF(nic_stats.tx_1024_to_1518_pkt)},
+	{"tx_1519_to_max_pkts",	QL_SIZEOF(nic_stats.tx_1519_to_max_pkt),
+					QL_OFF(nic_stats.tx_1519_to_max_pkt)},
+	{"tx_undersize_pkts", QL_SIZEOF(nic_stats.tx_undersize_pkt),
+					QL_OFF(nic_stats.tx_undersize_pkt)},
+	{"tx_oversize_pkts", QL_SIZEOF(nic_stats.tx_oversize_pkt),
+					QL_OFF(nic_stats.tx_oversize_pkt)},
+	{"rx_bytes", QL_SIZEOF(nic_stats.rx_bytes), QL_OFF(nic_stats.rx_bytes)},
+	{"rx_bytes_ok",	QL_SIZEOF(nic_stats.rx_bytes_ok),
+					QL_OFF(nic_stats.rx_bytes_ok)},
+	{"rx_pkts", QL_SIZEOF(nic_stats.rx_pkts), QL_OFF(nic_stats.rx_pkts)},
+	{"rx_pkts_ok", QL_SIZEOF(nic_stats.rx_pkts_ok),
+					QL_OFF(nic_stats.rx_pkts_ok)},
+	{"rx_bcast_pkts", QL_SIZEOF(nic_stats.rx_bcast_pkts),
+					QL_OFF(nic_stats.rx_bcast_pkts)},
+	{"rx_mcast_pkts", QL_SIZEOF(nic_stats.rx_mcast_pkts),
+					QL_OFF(nic_stats.rx_mcast_pkts)},
+	{"rx_ucast_pkts", QL_SIZEOF(nic_stats.rx_ucast_pkts),
+					QL_OFF(nic_stats.rx_ucast_pkts)},
+	{"rx_undersize_pkts", QL_SIZEOF(nic_stats.rx_undersize_pkts),
+					QL_OFF(nic_stats.rx_undersize_pkts)},
+	{"rx_oversize_pkts", QL_SIZEOF(nic_stats.rx_oversize_pkts),
+					QL_OFF(nic_stats.rx_oversize_pkts)},
+	{"rx_jabber_pkts", QL_SIZEOF(nic_stats.rx_jabber_pkts),
+					QL_OFF(nic_stats.rx_jabber_pkts)},
+	{"rx_undersize_fcerr_pkts",
+		QL_SIZEOF(nic_stats.rx_undersize_fcerr_pkts),
+				QL_OFF(nic_stats.rx_undersize_fcerr_pkts)},
+	{"rx_drop_events", QL_SIZEOF(nic_stats.rx_drop_events),
+					QL_OFF(nic_stats.rx_drop_events)},
+	{"rx_fcerr_pkts", QL_SIZEOF(nic_stats.rx_fcerr_pkts),
+					QL_OFF(nic_stats.rx_fcerr_pkts)},
+	{"rx_align_err", QL_SIZEOF(nic_stats.rx_align_err),
+					QL_OFF(nic_stats.rx_align_err)},
+	{"rx_symbol_err", QL_SIZEOF(nic_stats.rx_symbol_err),
+					QL_OFF(nic_stats.rx_symbol_err)},
+	{"rx_mac_err", QL_SIZEOF(nic_stats.rx_mac_err),
+					QL_OFF(nic_stats.rx_mac_err)},
+	{"rx_ctl_pkts",	QL_SIZEOF(nic_stats.rx_ctl_pkts),
+					QL_OFF(nic_stats.rx_ctl_pkts)},
+	{"rx_pause_pkts", QL_SIZEOF(nic_stats.rx_pause_pkts),
+					QL_OFF(nic_stats.rx_pause_pkts)},
+	{"rx_64_pkts", QL_SIZEOF(nic_stats.rx_64_pkts),
+					QL_OFF(nic_stats.rx_64_pkts)},
+	{"rx_65_to_127_pkts", QL_SIZEOF(nic_stats.rx_65_to_127_pkts),
+					QL_OFF(nic_stats.rx_65_to_127_pkts)},
+	{"rx_128_255_pkts", QL_SIZEOF(nic_stats.rx_128_255_pkts),
+					QL_OFF(nic_stats.rx_128_255_pkts)},
+	{"rx_256_511_pkts", QL_SIZEOF(nic_stats.rx_256_511_pkts),
+					QL_OFF(nic_stats.rx_256_511_pkts)},
+	{"rx_512_to_1023_pkts",	QL_SIZEOF(nic_stats.rx_512_to_1023_pkts),
+					QL_OFF(nic_stats.rx_512_to_1023_pkts)},
+	{"rx_1024_to_1518_pkts", QL_SIZEOF(nic_stats.rx_1024_to_1518_pkts),
+					QL_OFF(nic_stats.rx_1024_to_1518_pkts)},
+	{"rx_1519_to_max_pkts",	QL_SIZEOF(nic_stats.rx_1519_to_max_pkts),
+					QL_OFF(nic_stats.rx_1519_to_max_pkts)},
+	{"rx_len_err_pkts", QL_SIZEOF(nic_stats.rx_len_err_pkts),
+					QL_OFF(nic_stats.rx_len_err_pkts)},
+	{"rx_code_err",	QL_SIZEOF(nic_stats.rx_code_err),
+					QL_OFF(nic_stats.rx_code_err)},
+	{"rx_oversize_err", QL_SIZEOF(nic_stats.rx_oversize_err),
+					QL_OFF(nic_stats.rx_oversize_err)},
+	{"rx_undersize_err", QL_SIZEOF(nic_stats.rx_undersize_err),
+					QL_OFF(nic_stats.rx_undersize_err)},
+	{"rx_preamble_err", QL_SIZEOF(nic_stats.rx_preamble_err),
+					QL_OFF(nic_stats.rx_preamble_err)},
+	{"rx_frame_len_err", QL_SIZEOF(nic_stats.rx_frame_len_err),
+					QL_OFF(nic_stats.rx_frame_len_err)},
+	{"rx_crc_err", QL_SIZEOF(nic_stats.rx_crc_err),
+					QL_OFF(nic_stats.rx_crc_err)},
+	{"rx_err_count", QL_SIZEOF(nic_stats.rx_err_count),
+					QL_OFF(nic_stats.rx_err_count)},
+	{"tx_cbfc_pause_frames0", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames0),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames0)},
+	{"tx_cbfc_pause_frames1", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames1),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames1)},
+	{"tx_cbfc_pause_frames2", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames2),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames2)},
+	{"tx_cbfc_pause_frames3", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames3),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames3)},
+	{"tx_cbfc_pause_frames4", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames4),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames4)},
+	{"tx_cbfc_pause_frames5", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames5),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames5)},
+	{"tx_cbfc_pause_frames6", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames6),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames6)},
+	{"tx_cbfc_pause_frames7", QL_SIZEOF(nic_stats.tx_cbfc_pause_frames7),
+				QL_OFF(nic_stats.tx_cbfc_pause_frames7)},
+	{"rx_cbfc_pause_frames0", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames0),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames0)},
+	{"rx_cbfc_pause_frames1", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames1),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames1)},
+	{"rx_cbfc_pause_frames2", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames2),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames2)},
+	{"rx_cbfc_pause_frames3", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames3),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames3)},
+	{"rx_cbfc_pause_frames4", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames4),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames4)},
+	{"rx_cbfc_pause_frames5", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames5),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames5)},
+	{"rx_cbfc_pause_frames6", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames6),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames6)},
+	{"rx_cbfc_pause_frames7", QL_SIZEOF(nic_stats.rx_cbfc_pause_frames7),
+				QL_OFF(nic_stats.rx_cbfc_pause_frames7)},
+	{"rx_nic_fifo_drop", QL_SIZEOF(nic_stats.rx_nic_fifo_drop),
+					QL_OFF(nic_stats.rx_nic_fifo_drop)},
+};
+
 static const char ql_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Loopback test  (offline)"
 };
 #define QLGE_TEST_LEN (sizeof(ql_gstrings_test) / ETH_GSTRING_LEN)
+#define QLGE_STATS_LEN ARRAY_SIZE(ql_gstrings_stats)
 
 static int ql_update_ring_coalescing(struct ql_adapter *qdev)
 {
@@ -183,73 +325,19 @@ quit:
 	QL_DUMP_STAT(qdev);
 }
 
-static char ql_stats_str_arr[][ETH_GSTRING_LEN] = {
-	{"tx_pkts"},
-	{"tx_bytes"},
-	{"tx_mcast_pkts"},
-	{"tx_bcast_pkts"},
-	{"tx_ucast_pkts"},
-	{"tx_ctl_pkts"},
-	{"tx_pause_pkts"},
-	{"tx_64_pkts"},
-	{"tx_65_to_127_pkts"},
-	{"tx_128_to_255_pkts"},
-	{"tx_256_511_pkts"},
-	{"tx_512_to_1023_pkts"},
-	{"tx_1024_to_1518_pkts"},
-	{"tx_1519_to_max_pkts"},
-	{"tx_undersize_pkts"},
-	{"tx_oversize_pkts"},
-	{"rx_bytes"},
-	{"rx_bytes_ok"},
-	{"rx_pkts"},
-	{"rx_pkts_ok"},
-	{"rx_bcast_pkts"},
-	{"rx_mcast_pkts"},
-	{"rx_ucast_pkts"},
-	{"rx_undersize_pkts"},
-	{"rx_oversize_pkts"},
-	{"rx_jabber_pkts"},
-	{"rx_undersize_fcerr_pkts"},
-	{"rx_drop_events"},
-	{"rx_fcerr_pkts"},
-	{"rx_align_err"},
-	{"rx_symbol_err"},
-	{"rx_mac_err"},
-	{"rx_ctl_pkts"},
-	{"rx_pause_pkts"},
-	{"rx_64_pkts"},
-	{"rx_65_to_127_pkts"},
-	{"rx_128_255_pkts"},
-	{"rx_256_511_pkts"},
-	{"rx_512_to_1023_pkts"},
-	{"rx_1024_to_1518_pkts"},
-	{"rx_1519_to_max_pkts"},
-	{"rx_len_err_pkts"},
-	{"tx_cbfc_pause_frames0"},
-	{"tx_cbfc_pause_frames1"},
-	{"tx_cbfc_pause_frames2"},
-	{"tx_cbfc_pause_frames3"},
-	{"tx_cbfc_pause_frames4"},
-	{"tx_cbfc_pause_frames5"},
-	{"tx_cbfc_pause_frames6"},
-	{"tx_cbfc_pause_frames7"},
-	{"rx_cbfc_pause_frames0"},
-	{"rx_cbfc_pause_frames1"},
-	{"rx_cbfc_pause_frames2"},
-	{"rx_cbfc_pause_frames3"},
-	{"rx_cbfc_pause_frames4"},
-	{"rx_cbfc_pause_frames5"},
-	{"rx_cbfc_pause_frames6"},
-	{"rx_cbfc_pause_frames7"},
-	{"rx_nic_fifo_drop"},
-};
-
 static void ql_get_strings(struct net_device *dev, u32 stringset, u8 *buf)
 {
+	int index;
 	switch (stringset) {
+	case ETH_SS_TEST:
+		memcpy(buf, *ql_gstrings_test, QLGE_TEST_LEN * ETH_GSTRING_LEN);
+		break;
 	case ETH_SS_STATS:
-		memcpy(buf, ql_stats_str_arr, sizeof(ql_stats_str_arr));
+		for (index = 0; index < QLGE_STATS_LEN; index++) {
+			memcpy(buf + index * ETH_GSTRING_LEN,
+				ql_gstrings_stats[index].stat_string,
+				ETH_GSTRING_LEN);
+		}
 		break;
 	}
 }
@@ -260,7 +348,7 @@ static int ql_get_sset_count(struct net_device *dev, int sset)
 	case ETH_SS_TEST:
 		return QLGE_TEST_LEN;
 	case ETH_SS_STATS:
-		return ARRAY_SIZE(ql_stats_str_arr);
+		return QLGE_STATS_LEN;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -271,69 +359,17 @@ ql_get_ethtool_stats(struct net_device *ndev,
 		     struct ethtool_stats *stats, u64 *data)
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
-	struct nic_stats *s = &qdev->nic_stats;
+	int index, length;
 
+	length = QLGE_STATS_LEN;
 	ql_update_stats(qdev);
 
-	*data++ = s->tx_pkts;
-	*data++ = s->tx_bytes;
-	*data++ = s->tx_mcast_pkts;
-	*data++ = s->tx_bcast_pkts;
-	*data++ = s->tx_ucast_pkts;
-	*data++ = s->tx_ctl_pkts;
-	*data++ = s->tx_pause_pkts;
-	*data++ = s->tx_64_pkt;
-	*data++ = s->tx_65_to_127_pkt;
-	*data++ = s->tx_128_to_255_pkt;
-	*data++ = s->tx_256_511_pkt;
-	*data++ = s->tx_512_to_1023_pkt;
-	*data++ = s->tx_1024_to_1518_pkt;
-	*data++ = s->tx_1519_to_max_pkt;
-	*data++ = s->tx_undersize_pkt;
-	*data++ = s->tx_oversize_pkt;
-	*data++ = s->rx_bytes;
-	*data++ = s->rx_bytes_ok;
-	*data++ = s->rx_pkts;
-	*data++ = s->rx_pkts_ok;
-	*data++ = s->rx_bcast_pkts;
-	*data++ = s->rx_mcast_pkts;
-	*data++ = s->rx_ucast_pkts;
-	*data++ = s->rx_undersize_pkts;
-	*data++ = s->rx_oversize_pkts;
-	*data++ = s->rx_jabber_pkts;
-	*data++ = s->rx_undersize_fcerr_pkts;
-	*data++ = s->rx_drop_events;
-	*data++ = s->rx_fcerr_pkts;
-	*data++ = s->rx_align_err;
-	*data++ = s->rx_symbol_err;
-	*data++ = s->rx_mac_err;
-	*data++ = s->rx_ctl_pkts;
-	*data++ = s->rx_pause_pkts;
-	*data++ = s->rx_64_pkts;
-	*data++ = s->rx_65_to_127_pkts;
-	*data++ = s->rx_128_255_pkts;
-	*data++ = s->rx_256_511_pkts;
-	*data++ = s->rx_512_to_1023_pkts;
-	*data++ = s->rx_1024_to_1518_pkts;
-	*data++ = s->rx_1519_to_max_pkts;
-	*data++ = s->rx_len_err_pkts;
-	*data++ = s->tx_cbfc_pause_frames0;
-	*data++ = s->tx_cbfc_pause_frames1;
-	*data++ = s->tx_cbfc_pause_frames2;
-	*data++ = s->tx_cbfc_pause_frames3;
-	*data++ = s->tx_cbfc_pause_frames4;
-	*data++ = s->tx_cbfc_pause_frames5;
-	*data++ = s->tx_cbfc_pause_frames6;
-	*data++ = s->tx_cbfc_pause_frames7;
-	*data++ = s->rx_cbfc_pause_frames0;
-	*data++ = s->rx_cbfc_pause_frames1;
-	*data++ = s->rx_cbfc_pause_frames2;
-	*data++ = s->rx_cbfc_pause_frames3;
-	*data++ = s->rx_cbfc_pause_frames4;
-	*data++ = s->rx_cbfc_pause_frames5;
-	*data++ = s->rx_cbfc_pause_frames6;
-	*data++ = s->rx_cbfc_pause_frames7;
-	*data++ = s->rx_nic_fifo_drop;
+	for (index = 0; index < length; index++) {
+		char *p = (char *)qdev +
+			ql_gstrings_stats[index].stat_offset;
+		*data++ = (ql_gstrings_stats[index].sizeof_stat ==
+			sizeof(u64)) ? *(u64 *)p : (*(u32 *)p);
+	}
 }
 
 static int ql_get_settings(struct net_device *ndev,
@@ -388,30 +424,33 @@ static void ql_get_drvinfo(struct net_device *ndev,
 static void ql_get_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
-	/* What we support. */
-	wol->supported = WAKE_MAGIC;
-	/* What we've currently got set. */
-	wol->wolopts = qdev->wol;
+	unsigned short ssys_dev = qdev->pdev->subsystem_device;
+
+	/* WOL is only supported for mezz card. */
+	if (ssys_dev == QLGE_MEZZ_SSYS_ID_068 ||
+			ssys_dev == QLGE_MEZZ_SSYS_ID_180) {
+		wol->supported = WAKE_MAGIC;
+		wol->wolopts = qdev->wol;
+	}
 }
 
 static int ql_set_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
-	int status;
+	unsigned short ssys_dev = qdev->pdev->subsystem_device;
 
+	/* WOL is only supported for mezz card. */
+	if (ssys_dev != QLGE_MEZZ_SSYS_ID_068 &&
+			ssys_dev != QLGE_MEZZ_SSYS_ID_180) {
+		netif_info(qdev, drv, qdev->ndev,
+				"WOL is only supported for mezz card\n");
+		return -EOPNOTSUPP;
+	}
 	if (wol->wolopts & ~WAKE_MAGIC)
 		return -EINVAL;
 	qdev->wol = wol->wolopts;
 
 	netif_info(qdev, drv, qdev->ndev, "Set wol option 0x%x\n", qdev->wol);
-	if (!qdev->wol) {
-		u32 wol = 0;
-		status = ql_mb_wol_mode(qdev, wol);
-		netif_err(qdev, drv, qdev->ndev, "WOL %s (wol code 0x%x)\n",
-			  status == 0 ? "cleared successfully" : "clear failed",
-			  wol);
-	}
-
 	return 0;
 }
 
@@ -528,6 +567,8 @@ static void ql_self_test(struct net_device *ndev,
 {
 	struct ql_adapter *qdev = netdev_priv(ndev);
 
+	memset(data, 0, sizeof(u64) * QLGE_TEST_LEN);
+
 	if (netif_running(ndev)) {
 		set_bit(QL_SELFTEST, &qdev->flags);
 		if (eth_test->flags == ETH_TEST_FL_OFFLINE) {
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 09d8d33171d..b53a3b60b64 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -1433,6 +1433,36 @@ map_error:
 	return NETDEV_TX_BUSY;
 }
 
+/* Categorizing receive firmware frame errors */
+static void ql_categorize_rx_err(struct ql_adapter *qdev, u8 rx_err)
+{
+	struct nic_stats *stats = &qdev->nic_stats;
+
+	stats->rx_err_count++;
+
+	switch (rx_err & IB_MAC_IOCB_RSP_ERR_MASK) {
+	case IB_MAC_IOCB_RSP_ERR_CODE_ERR:
+		stats->rx_code_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_OVERSIZE:
+		stats->rx_oversize_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_UNDERSIZE:
+		stats->rx_undersize_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_PREAMBLE:
+		stats->rx_preamble_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_FRAME_LEN:
+		stats->rx_frame_len_err++;
+		break;
+	case IB_MAC_IOCB_RSP_ERR_CRC:
+		stats->rx_crc_err++;
+	default:
+		break;
+	}
+}
+
 /* Process an inbound completion from an rx ring. */
 static void ql_process_mac_rx_gro_page(struct ql_adapter *qdev,
 					struct rx_ring *rx_ring,
@@ -1499,15 +1529,6 @@ static void ql_process_mac_rx_page(struct ql_adapter *qdev,
 	addr = lbq_desc->p.pg_chunk.va;
 	prefetch(addr);
 
-
-	/* Frame error, so drop the packet. */
-	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
-		netif_info(qdev, drv, qdev->ndev,
-			  "Receive error, flags2 = 0x%x\n", ib_mac_rsp->flags2);
-		rx_ring->rx_errors++;
-		goto err_out;
-	}
-
 	/* The max framesize filter on this chip is set higher than
 	 * MTU since FCoE uses 2k frames.
 	 */
@@ -1546,7 +1567,7 @@ static void ql_process_mac_rx_page(struct ql_adapter *qdev,
 			struct iphdr *iph =
 				(struct iphdr *) ((u8 *)addr + ETH_HLEN);
 			if (!(iph->frag_off &
-				cpu_to_be16(IP_MF|IP_OFFSET))) {
+				htons(IP_MF|IP_OFFSET))) {
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
 				netif_printk(qdev, rx_status, KERN_DEBUG,
 					     qdev->ndev,
@@ -1593,15 +1614,6 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 	memcpy(skb_put(new_skb, length), skb->data, length);
 	skb = new_skb;
 
-	/* Frame error, so drop the packet. */
-	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
-		netif_info(qdev, drv, qdev->ndev,
-			  "Receive error, flags2 = 0x%x\n", ib_mac_rsp->flags2);
-		dev_kfree_skb_any(skb);
-		rx_ring->rx_errors++;
-		return;
-	}
-
 	/* loopback self test for ethtool */
 	if (test_bit(QL_SELFTEST, &qdev->flags)) {
 		ql_check_lb_frame(qdev, skb);
@@ -1619,7 +1631,6 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 	}
 
 	prefetch(skb->data);
-	skb->dev = ndev;
 	if (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) {
 		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
 			     "%s Multicast.\n",
@@ -1654,7 +1665,7 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 			/* Unfragmented ipv4 UDP frame. */
 			struct iphdr *iph = (struct iphdr *) skb->data;
 			if (!(iph->frag_off &
-				ntohs(IP_MF|IP_OFFSET))) {
+				htons(IP_MF|IP_OFFSET))) {
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
 				netif_printk(qdev, rx_status, KERN_DEBUG,
 					     qdev->ndev,
@@ -1908,15 +1919,6 @@ static void ql_process_mac_split_rx_intr(struct ql_adapter *qdev,
 		return;
 	}
 
-	/* Frame error, so drop the packet. */
-	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
-		netif_info(qdev, drv, qdev->ndev,
-			  "Receive error, flags2 = 0x%x\n", ib_mac_rsp->flags2);
-		dev_kfree_skb_any(skb);
-		rx_ring->rx_errors++;
-		return;
-	}
-
 	/* The max framesize filter on this chip is set higher than
 	 * MTU since FCoE uses 2k frames.
 	 */
@@ -1934,7 +1936,6 @@ static void ql_process_mac_split_rx_intr(struct ql_adapter *qdev,
 	}
 
 	prefetch(skb->data);
-	skb->dev = ndev;
 	if (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) {
 		netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev, "%s Multicast.\n",
 			     (ib_mac_rsp->flags1 & IB_MAC_IOCB_RSP_M_MASK) ==
@@ -1968,7 +1969,7 @@ static void ql_process_mac_split_rx_intr(struct ql_adapter *qdev,
 		/* Unfragmented ipv4 UDP frame. */
 			struct iphdr *iph = (struct iphdr *) skb->data;
 			if (!(iph->frag_off &
-				ntohs(IP_MF|IP_OFFSET))) {
+				htons(IP_MF|IP_OFFSET))) {
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
 				netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
 					     "TCP checksum done!\n");
@@ -1999,6 +2000,12 @@ static unsigned long ql_process_mac_rx_intr(struct ql_adapter *qdev,
 
 	QL_DUMP_IB_MAC_RSP(ib_mac_rsp);
 
+	/* Frame error, so drop the packet. */
+	if (ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_ERR_MASK) {
+		ql_categorize_rx_err(qdev, ib_mac_rsp->flags2);
+		return (unsigned long)length;
+	}
+
 	if (ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HV) {
 		/* The data and headers are split into
 		 * separate buffers.
@@ -2173,8 +2180,7 @@ static int ql_clean_outbound_rx_ring(struct rx_ring *rx_ring)
 	ql_write_cq_idx(rx_ring);
 	tx_ring = &qdev->tx_ring[net_rsp->txq_idx];
 	if (__netif_subqueue_stopped(qdev->ndev, tx_ring->wq_id)) {
-		if (atomic_read(&tx_ring->queue_stopped) &&
-		    (atomic_read(&tx_ring->tx_count) > (tx_ring->wq_len / 4)))
+		if ((atomic_read(&tx_ring->tx_count) > (tx_ring->wq_len / 4)))
 			/*
 			 * The queue got stopped because the tx_ring was full.
 			 * Wake it up, because it's now at least 25% empty.
@@ -2558,10 +2564,9 @@ static netdev_tx_t qlge_send(struct sk_buff *skb, struct net_device *ndev)
 
 	if (unlikely(atomic_read(&tx_ring->tx_count) < 2)) {
 		netif_info(qdev, tx_queued, qdev->ndev,
-			   "%s: shutting down tx queue %d du to lack of resources.\n",
+			   "%s: BUG! shutting down tx queue %d due to lack of resources.\n",
 			   __func__, tx_ring_idx);
 		netif_stop_subqueue(ndev, tx_ring->wq_id);
-		atomic_inc(&tx_ring->queue_stopped);
 		tx_ring->tx_errors++;
 		return NETDEV_TX_BUSY;
 	}
@@ -2612,6 +2617,16 @@ static netdev_tx_t qlge_send(struct sk_buff *skb, struct net_device *ndev)
 		     tx_ring->prod_idx, skb->len);
 
 	atomic_dec(&tx_ring->tx_count);
+
+	if (unlikely(atomic_read(&tx_ring->tx_count) < 2)) {
+		netif_stop_subqueue(ndev, tx_ring->wq_id);
+		if ((atomic_read(&tx_ring->tx_count) > (tx_ring->wq_len / 4)))
+			/*
+			 * The queue got stopped because the tx_ring was full.
+			 * Wake it up, because it's now at least 25% empty.
+			 */
+			netif_wake_subqueue(qdev->ndev, tx_ring->wq_id);
+	}
 	return NETDEV_TX_OK;
 }
 
@@ -2680,7 +2695,6 @@ static void ql_init_tx_ring(struct ql_adapter *qdev, struct tx_ring *tx_ring)
 		tx_ring_desc++;
 	}
 	atomic_set(&tx_ring->tx_count, tx_ring->wq_len);
-	atomic_set(&tx_ring->queue_stopped, 0);
 }
 
 static void ql_free_tx_resources(struct ql_adapter *qdev,
@@ -2703,10 +2717,9 @@ static int ql_alloc_tx_resources(struct ql_adapter *qdev,
 				 &tx_ring->wq_base_dma);
 
 	if ((tx_ring->wq_base == NULL) ||
-	    tx_ring->wq_base_dma & WQ_ADDR_ALIGN) {
-		netif_err(qdev, ifup, qdev->ndev, "tx_ring alloc failed.\n");
-		return -ENOMEM;
-	}
+	    tx_ring->wq_base_dma & WQ_ADDR_ALIGN)
+		goto pci_alloc_err;
+
 	tx_ring->q =
 	    kmalloc(tx_ring->wq_len * sizeof(struct tx_ring_desc), GFP_KERNEL);
 	if (tx_ring->q == NULL)
@@ -2716,6 +2729,9 @@ static int ql_alloc_tx_resources(struct ql_adapter *qdev,
 err:
 	pci_free_consistent(qdev->pdev, tx_ring->wq_size,
 			    tx_ring->wq_base, tx_ring->wq_base_dma);
+	tx_ring->wq_base = NULL;
+pci_alloc_err:
+	netif_err(qdev, ifup, qdev->ndev, "tx_ring alloc failed.\n");
 	return -ENOMEM;
 }
 
@@ -4649,7 +4665,7 @@ static int __devinit qlge_probe(struct pci_dev *pdev,
 	int err = 0;
 
 	ndev = alloc_etherdev_mq(sizeof(struct ql_adapter),
-			min(MAX_CPUS, (int)num_online_cpus()));
+			min(MAX_CPUS, netif_get_num_default_rss_queues()));
 	if (!ndev)
 		return -ENOMEM;
 
@@ -4666,6 +4682,7 @@ static int __devinit qlge_probe(struct pci_dev *pdev,
 		NETIF_F_HW_VLAN_TX | NETIF_F_RXCSUM;
 	ndev->features = ndev->hw_features |
 		NETIF_F_HW_VLAN_RX | NETIF_F_HW_VLAN_FILTER;
+	ndev->vlan_features = ndev->hw_features;
 
 	if (test_bit(QL_DMA64, &qdev->flags))
 		ndev->features |= NETIF_F_HIGHDMA;
diff --git a/drivers/net/ethernet/rdc/r6040.c b/drivers/net/ethernet/rdc/r6040.c
index d1827e887f4..557a26545d7 100644
--- a/drivers/net/ethernet/rdc/r6040.c
+++ b/drivers/net/ethernet/rdc/r6040.c
@@ -1256,7 +1256,6 @@ static void __devexit r6040_remove_one(struct pci_dev *pdev)
 	kfree(lp->mii_bus->irq);
 	mdiobus_free(lp->mii_bus);
 	netif_napi_del(&lp->napi);
-	pci_set_drvdata(pdev, NULL);
 	pci_iounmap(pdev, lp->base);
 	pci_release_regions(pdev);
 	free_netdev(dev);
@@ -1278,17 +1277,4 @@ static struct pci_driver r6040_driver = {
 	.remove		= __devexit_p(r6040_remove_one),
 };
 
-
-static int __init r6040_init(void)
-{
-	return pci_register_driver(&r6040_driver);
-}
-
-
-static void __exit r6040_cleanup(void)
-{
-	pci_unregister_driver(&r6040_driver);
-}
-
-module_init(r6040_init);
-module_exit(r6040_cleanup);
+module_pci_driver(r6040_driver);
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index d7a04e09110..b47d5b35024 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -46,6 +46,8 @@
 #define FIRMWARE_8105E_1	"rtl_nic/rtl8105e-1.fw"
 #define FIRMWARE_8402_1		"rtl_nic/rtl8402-1.fw"
 #define FIRMWARE_8411_1		"rtl_nic/rtl8411-1.fw"
+#define FIRMWARE_8106E_1	"rtl_nic/rtl8106e-1.fw"
+#define FIRMWARE_8168G_1	"rtl_nic/rtl8168g-1.fw"
 
 #ifdef RTL8169_DEBUG
 #define assert(expr) \
@@ -141,6 +143,9 @@ enum mac_version {
 	RTL_GIGA_MAC_VER_36,
 	RTL_GIGA_MAC_VER_37,
 	RTL_GIGA_MAC_VER_38,
+	RTL_GIGA_MAC_VER_39,
+	RTL_GIGA_MAC_VER_40,
+	RTL_GIGA_MAC_VER_41,
 	RTL_GIGA_MAC_NONE   = 0xff,
 };
 
@@ -259,6 +264,14 @@ static const struct {
 	[RTL_GIGA_MAC_VER_38] =
 		_R("RTL8411",		RTL_TD_1, FIRMWARE_8411_1,
 							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_39] =
+		_R("RTL8106e",		RTL_TD_1, FIRMWARE_8106E_1,
+							JUMBO_1K, true),
+	[RTL_GIGA_MAC_VER_40] =
+		_R("RTL8168g/8111g",	RTL_TD_1, FIRMWARE_8168G_1,
+							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_41] =
+		_R("RTL8168g/8111g",	RTL_TD_1, NULL, JUMBO_9K, false),
 };
 #undef _R
 
@@ -389,8 +402,12 @@ enum rtl8168_8101_registers {
 	TWSI			= 0xd2,
 	MCU			= 0xd3,
 #define	NOW_IS_OOB			(1 << 7)
+#define	TX_EMPTY			(1 << 5)
+#define	RX_EMPTY			(1 << 4)
+#define	RXTX_EMPTY			(TX_EMPTY | RX_EMPTY)
 #define	EN_NDP				(1 << 3)
 #define	EN_OOB_RESET			(1 << 2)
+#define	LINK_LIST_RDY			(1 << 1)
 	EFUSEAR			= 0xdc,
 #define	EFUSEAR_FLAG			0x80000000
 #define	EFUSEAR_WRITE_CMD		0x80000000
@@ -416,6 +433,7 @@ enum rtl8168_registers {
 #define ERIAR_MASK_SHIFT		12
 #define ERIAR_MASK_0001			(0x1 << ERIAR_MASK_SHIFT)
 #define ERIAR_MASK_0011			(0x3 << ERIAR_MASK_SHIFT)
+#define ERIAR_MASK_0101			(0x5 << ERIAR_MASK_SHIFT)
 #define ERIAR_MASK_1111			(0xf << ERIAR_MASK_SHIFT)
 	EPHY_RXER_NUM		= 0x7c,
 	OCPDR			= 0xb0,	/* OCP GPHY access */
@@ -428,10 +446,14 @@ enum rtl8168_registers {
 #define OCPAR_FLAG			0x80000000
 #define OCPAR_GPHY_WRITE_CMD		0x8000f060
 #define OCPAR_GPHY_READ_CMD		0x0000f060
+	GPHY_OCP		= 0xb8,
 	RDSAR1			= 0xd0,	/* 8168c only. Undocumented on 8168dp */
 	MISC			= 0xf0,	/* 8168e only. */
 #define TXPLA_RST			(1 << 29)
+#define DISABLE_LAN_EN			(1 << 23) /* Enable GPIO pin */
 #define PWM_EN				(1 << 22)
+#define RXDV_GATED_EN			(1 << 19)
+#define EARLY_TALLY_EN			(1 << 16)
 };
 
 enum rtl_register_content {
@@ -721,8 +743,8 @@ struct rtl8169_private {
 	u16 event_slow;
 
 	struct mdio_ops {
-		void (*write)(void __iomem *, int, int);
-		int (*read)(void __iomem *, int);
+		void (*write)(struct rtl8169_private *, int, int);
+		int (*read)(struct rtl8169_private *, int);
 	} mdio_ops;
 
 	struct pll_power_ops {
@@ -736,8 +758,8 @@ struct rtl8169_private {
 	} jumbo_ops;
 
 	struct csi_ops {
-		void (*write)(void __iomem *, int, int);
-		u32 (*read)(void __iomem *, int);
+		void (*write)(struct rtl8169_private *, int, int);
+		u32 (*read)(struct rtl8169_private *, int);
 	} csi_ops;
 
 	int (*set_speed)(struct net_device *, u8 aneg, u16 sp, u8 dpx, u32 adv);
@@ -774,6 +796,8 @@ struct rtl8169_private {
 		} phy_action;
 	} *rtl_fw;
 #define RTL_FIRMWARE_UNKNOWN	ERR_PTR(-EAGAIN)
+
+	u32 ocp_base;
 };
 
 MODULE_AUTHOR("Realtek and the Linux r8169 crew <netdev@vger.kernel.org>");
@@ -794,6 +818,8 @@ MODULE_FIRMWARE(FIRMWARE_8168F_1);
 MODULE_FIRMWARE(FIRMWARE_8168F_2);
 MODULE_FIRMWARE(FIRMWARE_8402_1);
 MODULE_FIRMWARE(FIRMWARE_8411_1);
+MODULE_FIRMWARE(FIRMWARE_8106E_1);
+MODULE_FIRMWARE(FIRMWARE_8168G_1);
 
 static void rtl_lock_work(struct rtl8169_private *tp)
 {
@@ -818,47 +844,114 @@ static void rtl_tx_performance_tweak(struct pci_dev *pdev, u16 force)
 	}
 }
 
+struct rtl_cond {
+	bool (*check)(struct rtl8169_private *);
+	const char *msg;
+};
+
+static void rtl_udelay(unsigned int d)
+{
+	udelay(d);
+}
+
+static bool rtl_loop_wait(struct rtl8169_private *tp, const struct rtl_cond *c,
+			  void (*delay)(unsigned int), unsigned int d, int n,
+			  bool high)
+{
+	int i;
+
+	for (i = 0; i < n; i++) {
+		delay(d);
+		if (c->check(tp) == high)
+			return true;
+	}
+	netif_err(tp, drv, tp->dev, "%s == %d (loop: %d, delay: %d).\n",
+		  c->msg, !high, n, d);
+	return false;
+}
+
+static bool rtl_udelay_loop_wait_high(struct rtl8169_private *tp,
+				      const struct rtl_cond *c,
+				      unsigned int d, int n)
+{
+	return rtl_loop_wait(tp, c, rtl_udelay, d, n, true);
+}
+
+static bool rtl_udelay_loop_wait_low(struct rtl8169_private *tp,
+				     const struct rtl_cond *c,
+				     unsigned int d, int n)
+{
+	return rtl_loop_wait(tp, c, rtl_udelay, d, n, false);
+}
+
+static bool rtl_msleep_loop_wait_high(struct rtl8169_private *tp,
+				      const struct rtl_cond *c,
+				      unsigned int d, int n)
+{
+	return rtl_loop_wait(tp, c, msleep, d, n, true);
+}
+
+static bool rtl_msleep_loop_wait_low(struct rtl8169_private *tp,
+				     const struct rtl_cond *c,
+				     unsigned int d, int n)
+{
+	return rtl_loop_wait(tp, c, msleep, d, n, false);
+}
+
+#define DECLARE_RTL_COND(name)				\
+static bool name ## _check(struct rtl8169_private *);	\
+							\
+static const struct rtl_cond name = {			\
+	.check	= name ## _check,			\
+	.msg	= #name					\
+};							\
+							\
+static bool name ## _check(struct rtl8169_private *tp)
+
+DECLARE_RTL_COND(rtl_ocpar_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(OCPAR) & OCPAR_FLAG;
+}
+
 static u32 ocp_read(struct rtl8169_private *tp, u8 mask, u16 reg)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
-	int i;
 
 	RTL_W32(OCPAR, ((u32)mask & 0x0f) << 12 | (reg & 0x0fff));
-	for (i = 0; i < 20; i++) {
-		udelay(100);
-		if (RTL_R32(OCPAR) & OCPAR_FLAG)
-			break;
-	}
-	return RTL_R32(OCPDR);
+
+	return rtl_udelay_loop_wait_high(tp, &rtl_ocpar_cond, 100, 20) ?
+		RTL_R32(OCPDR) : ~0;
 }
 
 static void ocp_write(struct rtl8169_private *tp, u8 mask, u16 reg, u32 data)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
-	int i;
 
 	RTL_W32(OCPDR, data);
 	RTL_W32(OCPAR, OCPAR_FLAG | ((u32)mask & 0x0f) << 12 | (reg & 0x0fff));
-	for (i = 0; i < 20; i++) {
-		udelay(100);
-		if ((RTL_R32(OCPAR) & OCPAR_FLAG) == 0)
-			break;
-	}
+
+	rtl_udelay_loop_wait_low(tp, &rtl_ocpar_cond, 100, 20);
+}
+
+DECLARE_RTL_COND(rtl_eriar_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(ERIAR) & ERIAR_FLAG;
 }
 
 static void rtl8168_oob_notify(struct rtl8169_private *tp, u8 cmd)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
-	int i;
 
 	RTL_W8(ERIDR, cmd);
 	RTL_W32(ERIAR, 0x800010e8);
 	msleep(2);
-	for (i = 0; i < 5; i++) {
-		udelay(100);
-		if (!(RTL_R32(ERIAR) & ERIAR_FLAG))
-			break;
-	}
+
+	if (!rtl_udelay_loop_wait_low(tp, &rtl_eriar_cond, 100, 5))
+		return;
 
 	ocp_write(tp, 0x1, 0x30, 0x00000001);
 }
@@ -872,36 +965,27 @@ static u16 rtl8168_get_ocp_reg(struct rtl8169_private *tp)
 	return (tp->mac_version == RTL_GIGA_MAC_VER_31) ? 0xb8 : 0x10;
 }
 
-static void rtl8168_driver_start(struct rtl8169_private *tp)
+DECLARE_RTL_COND(rtl_ocp_read_cond)
 {
 	u16 reg;
-	int i;
-
-	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_START);
 
 	reg = rtl8168_get_ocp_reg(tp);
 
-	for (i = 0; i < 10; i++) {
-		msleep(10);
-		if (ocp_read(tp, 0x0f, reg) & 0x00000800)
-			break;
-	}
+	return ocp_read(tp, 0x0f, reg) & 0x00000800;
 }
 
-static void rtl8168_driver_stop(struct rtl8169_private *tp)
+static void rtl8168_driver_start(struct rtl8169_private *tp)
 {
-	u16 reg;
-	int i;
+	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_START);
 
-	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_STOP);
+	rtl_msleep_loop_wait_high(tp, &rtl_ocp_read_cond, 10, 10);
+}
 
-	reg = rtl8168_get_ocp_reg(tp);
+static void rtl8168_driver_stop(struct rtl8169_private *tp)
+{
+	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_STOP);
 
-	for (i = 0; i < 10; i++) {
-		msleep(10);
-		if ((ocp_read(tp, 0x0f, reg) & 0x00000800) == 0)
-			break;
-	}
+	rtl_msleep_loop_wait_low(tp, &rtl_ocp_read_cond, 10, 10);
 }
 
 static int r8168dp_check_dash(struct rtl8169_private *tp)
@@ -911,21 +995,114 @@ static int r8168dp_check_dash(struct rtl8169_private *tp)
 	return (ocp_read(tp, 0x0f, reg) & 0x00008000) ? 1 : 0;
 }
 
-static void r8169_mdio_write(void __iomem *ioaddr, int reg_addr, int value)
+static bool rtl_ocp_reg_failure(struct rtl8169_private *tp, u32 reg)
 {
-	int i;
+	if (reg & 0xffff0001) {
+		netif_err(tp, drv, tp->dev, "Invalid ocp reg %x!\n", reg);
+		return true;
+	}
+	return false;
+}
 
-	RTL_W32(PHYAR, 0x80000000 | (reg_addr & 0x1f) << 16 | (value & 0xffff));
+DECLARE_RTL_COND(rtl_ocp_gphy_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
 
-	for (i = 20; i > 0; i--) {
-		/*
-		 * Check if the RTL8169 has completed writing to the specified
-		 * MII register.
-		 */
-		if (!(RTL_R32(PHYAR) & 0x80000000))
-			break;
-		udelay(25);
+	return RTL_R32(GPHY_OCP) & OCPAR_FLAG;
+}
+
+static void r8168_phy_ocp_write(struct rtl8169_private *tp, u32 reg, u32 data)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	if (rtl_ocp_reg_failure(tp, reg))
+		return;
+
+	RTL_W32(GPHY_OCP, OCPAR_FLAG | (reg << 15) | data);
+
+	rtl_udelay_loop_wait_low(tp, &rtl_ocp_gphy_cond, 25, 10);
+}
+
+static u16 r8168_phy_ocp_read(struct rtl8169_private *tp, u32 reg)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	if (rtl_ocp_reg_failure(tp, reg))
+		return 0;
+
+	RTL_W32(GPHY_OCP, reg << 15);
+
+	return rtl_udelay_loop_wait_high(tp, &rtl_ocp_gphy_cond, 25, 10) ?
+		(RTL_R32(GPHY_OCP) & 0xffff) : ~0;
+}
+
+static void rtl_w1w0_phy_ocp(struct rtl8169_private *tp, int reg, int p, int m)
+{
+	int val;
+
+	val = r8168_phy_ocp_read(tp, reg);
+	r8168_phy_ocp_write(tp, reg, (val | p) & ~m);
+}
+
+static void r8168_mac_ocp_write(struct rtl8169_private *tp, u32 reg, u32 data)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	if (rtl_ocp_reg_failure(tp, reg))
+		return;
+
+	RTL_W32(OCPDR, OCPAR_FLAG | (reg << 15) | data);
+}
+
+static u16 r8168_mac_ocp_read(struct rtl8169_private *tp, u32 reg)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	if (rtl_ocp_reg_failure(tp, reg))
+		return 0;
+
+	RTL_W32(OCPDR, reg << 15);
+
+	return RTL_R32(OCPDR);
+}
+
+#define OCP_STD_PHY_BASE	0xa400
+
+static void r8168g_mdio_write(struct rtl8169_private *tp, int reg, int value)
+{
+	if (reg == 0x1f) {
+		tp->ocp_base = value ? value << 4 : OCP_STD_PHY_BASE;
+		return;
 	}
+
+	if (tp->ocp_base != OCP_STD_PHY_BASE)
+		reg -= 0x10;
+
+	r8168_phy_ocp_write(tp, tp->ocp_base + reg * 2, value);
+}
+
+static int r8168g_mdio_read(struct rtl8169_private *tp, int reg)
+{
+	if (tp->ocp_base != OCP_STD_PHY_BASE)
+		reg -= 0x10;
+
+	return r8168_phy_ocp_read(tp, tp->ocp_base + reg * 2);
+}
+
+DECLARE_RTL_COND(rtl_phyar_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(PHYAR) & 0x80000000;
+}
+
+static void r8169_mdio_write(struct rtl8169_private *tp, int reg, int value)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	RTL_W32(PHYAR, 0x80000000 | (reg & 0x1f) << 16 | (value & 0xffff));
+
+	rtl_udelay_loop_wait_low(tp, &rtl_phyar_cond, 25, 20);
 	/*
 	 * According to hardware specs a 20us delay is required after write
 	 * complete indication, but before sending next command.
@@ -933,23 +1110,16 @@ static void r8169_mdio_write(void __iomem *ioaddr, int reg_addr, int value)
 	udelay(20);
 }
 
-static int r8169_mdio_read(void __iomem *ioaddr, int reg_addr)
+static int r8169_mdio_read(struct rtl8169_private *tp, int reg)
 {
-	int i, value = -1;
+	void __iomem *ioaddr = tp->mmio_addr;
+	int value;
 
-	RTL_W32(PHYAR, 0x0 | (reg_addr & 0x1f) << 16);
+	RTL_W32(PHYAR, 0x0 | (reg & 0x1f) << 16);
+
+	value = rtl_udelay_loop_wait_high(tp, &rtl_phyar_cond, 25, 20) ?
+		RTL_R32(PHYAR) & 0xffff : ~0;
 
-	for (i = 20; i > 0; i--) {
-		/*
-		 * Check if the RTL8169 has completed retrieving data from
-		 * the specified MII register.
-		 */
-		if (RTL_R32(PHYAR) & 0x80000000) {
-			value = RTL_R32(PHYAR) & 0xffff;
-			break;
-		}
-		udelay(25);
-	}
 	/*
 	 * According to hardware specs a 20us delay is required after read
 	 * complete indication, but before sending next command.
@@ -959,45 +1129,35 @@ static int r8169_mdio_read(void __iomem *ioaddr, int reg_addr)
 	return value;
 }
 
-static void r8168dp_1_mdio_access(void __iomem *ioaddr, int reg_addr, u32 data)
+static void r8168dp_1_mdio_access(struct rtl8169_private *tp, int reg, u32 data)
 {
-	int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
-	RTL_W32(OCPDR, data |
-		((reg_addr & OCPDR_REG_MASK) << OCPDR_GPHY_REG_SHIFT));
+	RTL_W32(OCPDR, data | ((reg & OCPDR_REG_MASK) << OCPDR_GPHY_REG_SHIFT));
 	RTL_W32(OCPAR, OCPAR_GPHY_WRITE_CMD);
 	RTL_W32(EPHY_RXER_NUM, 0);
 
-	for (i = 0; i < 100; i++) {
-		mdelay(1);
-		if (!(RTL_R32(OCPAR) & OCPAR_FLAG))
-			break;
-	}
+	rtl_udelay_loop_wait_low(tp, &rtl_ocpar_cond, 1000, 100);
 }
 
-static void r8168dp_1_mdio_write(void __iomem *ioaddr, int reg_addr, int value)
+static void r8168dp_1_mdio_write(struct rtl8169_private *tp, int reg, int value)
 {
-	r8168dp_1_mdio_access(ioaddr, reg_addr, OCPDR_WRITE_CMD |
-		(value & OCPDR_DATA_MASK));
+	r8168dp_1_mdio_access(tp, reg,
+			      OCPDR_WRITE_CMD | (value & OCPDR_DATA_MASK));
 }
 
-static int r8168dp_1_mdio_read(void __iomem *ioaddr, int reg_addr)
+static int r8168dp_1_mdio_read(struct rtl8169_private *tp, int reg)
 {
-	int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
-	r8168dp_1_mdio_access(ioaddr, reg_addr, OCPDR_READ_CMD);
+	r8168dp_1_mdio_access(tp, reg, OCPDR_READ_CMD);
 
 	mdelay(1);
 	RTL_W32(OCPAR, OCPAR_GPHY_READ_CMD);
 	RTL_W32(EPHY_RXER_NUM, 0);
 
-	for (i = 0; i < 100; i++) {
-		mdelay(1);
-		if (RTL_R32(OCPAR) & OCPAR_FLAG)
-			break;
-	}
-
-	return RTL_R32(OCPDR) & OCPDR_DATA_MASK;
+	return rtl_udelay_loop_wait_high(tp, &rtl_ocpar_cond, 1000, 100) ?
+		RTL_R32(OCPDR) & OCPDR_DATA_MASK : ~0;
 }
 
 #define R8168DP_1_MDIO_ACCESS_BIT	0x00020000
@@ -1012,22 +1172,25 @@ static void r8168dp_2_mdio_stop(void __iomem *ioaddr)
 	RTL_W32(0xd0, RTL_R32(0xd0) | R8168DP_1_MDIO_ACCESS_BIT);
 }
 
-static void r8168dp_2_mdio_write(void __iomem *ioaddr, int reg_addr, int value)
+static void r8168dp_2_mdio_write(struct rtl8169_private *tp, int reg, int value)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
+
 	r8168dp_2_mdio_start(ioaddr);
 
-	r8169_mdio_write(ioaddr, reg_addr, value);
+	r8169_mdio_write(tp, reg, value);
 
 	r8168dp_2_mdio_stop(ioaddr);
 }
 
-static int r8168dp_2_mdio_read(void __iomem *ioaddr, int reg_addr)
+static int r8168dp_2_mdio_read(struct rtl8169_private *tp, int reg)
 {
+	void __iomem *ioaddr = tp->mmio_addr;
 	int value;
 
 	r8168dp_2_mdio_start(ioaddr);
 
-	value = r8169_mdio_read(ioaddr, reg_addr);
+	value = r8169_mdio_read(tp, reg);
 
 	r8168dp_2_mdio_stop(ioaddr);
 
@@ -1036,12 +1199,12 @@ static int r8168dp_2_mdio_read(void __iomem *ioaddr, int reg_addr)
 
 static void rtl_writephy(struct rtl8169_private *tp, int location, u32 val)
 {
-	tp->mdio_ops.write(tp->mmio_addr, location, val);
+	tp->mdio_ops.write(tp, location, val);
 }
 
 static int rtl_readphy(struct rtl8169_private *tp, int location)
 {
-	return tp->mdio_ops.read(tp->mmio_addr, location);
+	return tp->mdio_ops.read(tp, location);
 }
 
 static void rtl_patchphy(struct rtl8169_private *tp, int reg_addr, int value)
@@ -1072,79 +1235,64 @@ static int rtl_mdio_read(struct net_device *dev, int phy_id, int location)
 	return rtl_readphy(tp, location);
 }
 
-static void rtl_ephy_write(void __iomem *ioaddr, int reg_addr, int value)
+DECLARE_RTL_COND(rtl_ephyar_cond)
 {
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(EPHYAR) & EPHYAR_FLAG;
+}
+
+static void rtl_ephy_write(struct rtl8169_private *tp, int reg_addr, int value)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(EPHYAR, EPHYAR_WRITE_CMD | (value & EPHYAR_DATA_MASK) |
 		(reg_addr & EPHYAR_REG_MASK) << EPHYAR_REG_SHIFT);
 
-	for (i = 0; i < 100; i++) {
-		if (!(RTL_R32(EPHYAR) & EPHYAR_FLAG))
-			break;
-		udelay(10);
-	}
+	rtl_udelay_loop_wait_low(tp, &rtl_ephyar_cond, 10, 100);
+
+	udelay(10);
 }
 
-static u16 rtl_ephy_read(void __iomem *ioaddr, int reg_addr)
+static u16 rtl_ephy_read(struct rtl8169_private *tp, int reg_addr)
 {
-	u16 value = 0xffff;
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(EPHYAR, (reg_addr & EPHYAR_REG_MASK) << EPHYAR_REG_SHIFT);
 
-	for (i = 0; i < 100; i++) {
-		if (RTL_R32(EPHYAR) & EPHYAR_FLAG) {
-			value = RTL_R32(EPHYAR) & EPHYAR_DATA_MASK;
-			break;
-		}
-		udelay(10);
-	}
-
-	return value;
+	return rtl_udelay_loop_wait_high(tp, &rtl_ephyar_cond, 10, 100) ?
+		RTL_R32(EPHYAR) & EPHYAR_DATA_MASK : ~0;
 }
 
-static
-void rtl_eri_write(void __iomem *ioaddr, int addr, u32 mask, u32 val, int type)
+static void rtl_eri_write(struct rtl8169_private *tp, int addr, u32 mask,
+			  u32 val, int type)
 {
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	BUG_ON((addr & 3) || (mask == 0));
 	RTL_W32(ERIDR, val);
 	RTL_W32(ERIAR, ERIAR_WRITE_CMD | type | mask | addr);
 
-	for (i = 0; i < 100; i++) {
-		if (!(RTL_R32(ERIAR) & ERIAR_FLAG))
-			break;
-		udelay(100);
-	}
+	rtl_udelay_loop_wait_low(tp, &rtl_eriar_cond, 100, 100);
 }
 
-static u32 rtl_eri_read(void __iomem *ioaddr, int addr, int type)
+static u32 rtl_eri_read(struct rtl8169_private *tp, int addr, int type)
 {
-	u32 value = ~0x00;
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(ERIAR, ERIAR_READ_CMD | type | ERIAR_MASK_1111 | addr);
 
-	for (i = 0; i < 100; i++) {
-		if (RTL_R32(ERIAR) & ERIAR_FLAG) {
-			value = RTL_R32(ERIDR);
-			break;
-		}
-		udelay(100);
-	}
-
-	return value;
+	return rtl_udelay_loop_wait_high(tp, &rtl_eriar_cond, 100, 100) ?
+		RTL_R32(ERIDR) : ~0;
 }
 
-static void
-rtl_w1w0_eri(void __iomem *ioaddr, int addr, u32 mask, u32 p, u32 m, int type)
+static void rtl_w1w0_eri(struct rtl8169_private *tp, int addr, u32 mask, u32 p,
+			 u32 m, int type)
 {
 	u32 val;
 
-	val = rtl_eri_read(ioaddr, addr, type);
-	rtl_eri_write(ioaddr, addr, mask, (val & ~m) | p, type);
+	val = rtl_eri_read(tp, addr, type);
+	rtl_eri_write(tp, addr, mask, (val & ~m) | p, type);
 }
 
 struct exgmac_reg {
@@ -1153,31 +1301,30 @@ struct exgmac_reg {
 	u32 val;
 };
 
-static void rtl_write_exgmac_batch(void __iomem *ioaddr,
+static void rtl_write_exgmac_batch(struct rtl8169_private *tp,
 				   const struct exgmac_reg *r, int len)
 {
 	while (len-- > 0) {
-		rtl_eri_write(ioaddr, r->addr, r->mask, r->val, ERIAR_EXGMAC);
+		rtl_eri_write(tp, r->addr, r->mask, r->val, ERIAR_EXGMAC);
 		r++;
 	}
 }
 
-static u8 rtl8168d_efuse_read(void __iomem *ioaddr, int reg_addr)
+DECLARE_RTL_COND(rtl_efusear_cond)
 {
-	u8 value = 0xff;
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
-	RTL_W32(EFUSEAR, (reg_addr & EFUSEAR_REG_MASK) << EFUSEAR_REG_SHIFT);
+	return RTL_R32(EFUSEAR) & EFUSEAR_FLAG;
+}
 
-	for (i = 0; i < 300; i++) {
-		if (RTL_R32(EFUSEAR) & EFUSEAR_FLAG) {
-			value = RTL_R32(EFUSEAR) & EFUSEAR_DATA_MASK;
-			break;
-		}
-		udelay(100);
-	}
+static u8 rtl8168d_efuse_read(struct rtl8169_private *tp, int reg_addr)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
 
-	return value;
+	RTL_W32(EFUSEAR, (reg_addr & EFUSEAR_REG_MASK) << EFUSEAR_REG_SHIFT);
+
+	return rtl_udelay_loop_wait_high(tp, &rtl_efusear_cond, 100, 300) ?
+		RTL_R32(EFUSEAR) & EFUSEAR_DATA_MASK : ~0;
 }
 
 static u16 rtl_get_events(struct rtl8169_private *tp)
@@ -1276,48 +1423,48 @@ static void rtl_link_chg_patch(struct rtl8169_private *tp)
 	if (tp->mac_version == RTL_GIGA_MAC_VER_34 ||
 	    tp->mac_version == RTL_GIGA_MAC_VER_38) {
 		if (RTL_R8(PHYstatus) & _1000bpsF) {
-			rtl_eri_write(ioaddr, 0x1bc, ERIAR_MASK_1111,
-				      0x00000011, ERIAR_EXGMAC);
-			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_1111,
-				      0x00000005, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1bc, ERIAR_MASK_1111, 0x00000011,
+				      ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1dc, ERIAR_MASK_1111, 0x00000005,
+				      ERIAR_EXGMAC);
 		} else if (RTL_R8(PHYstatus) & _100bps) {
-			rtl_eri_write(ioaddr, 0x1bc, ERIAR_MASK_1111,
-				      0x0000001f, ERIAR_EXGMAC);
-			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_1111,
-				      0x00000005, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1bc, ERIAR_MASK_1111, 0x0000001f,
+				      ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1dc, ERIAR_MASK_1111, 0x00000005,
+				      ERIAR_EXGMAC);
 		} else {
-			rtl_eri_write(ioaddr, 0x1bc, ERIAR_MASK_1111,
-				      0x0000001f, ERIAR_EXGMAC);
-			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_1111,
-				      0x0000003f, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1bc, ERIAR_MASK_1111, 0x0000001f,
+				      ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1dc, ERIAR_MASK_1111, 0x0000003f,
+				      ERIAR_EXGMAC);
 		}
 		/* Reset packet filter */
-		rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x00, 0x01,
+		rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01,
 			     ERIAR_EXGMAC);
-		rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x01, 0x00,
+		rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00,
 			     ERIAR_EXGMAC);
 	} else if (tp->mac_version == RTL_GIGA_MAC_VER_35 ||
 		   tp->mac_version == RTL_GIGA_MAC_VER_36) {
 		if (RTL_R8(PHYstatus) & _1000bpsF) {
-			rtl_eri_write(ioaddr, 0x1bc, ERIAR_MASK_1111,
-				      0x00000011, ERIAR_EXGMAC);
-			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_1111,
-				      0x00000005, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1bc, ERIAR_MASK_1111, 0x00000011,
+				      ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1dc, ERIAR_MASK_1111, 0x00000005,
+				      ERIAR_EXGMAC);
 		} else {
-			rtl_eri_write(ioaddr, 0x1bc, ERIAR_MASK_1111,
-				      0x0000001f, ERIAR_EXGMAC);
-			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_1111,
-				      0x0000003f, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1bc, ERIAR_MASK_1111, 0x0000001f,
+				      ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1dc, ERIAR_MASK_1111, 0x0000003f,
+				      ERIAR_EXGMAC);
 		}
 	} else if (tp->mac_version == RTL_GIGA_MAC_VER_37) {
 		if (RTL_R8(PHYstatus) & _10bps) {
-			rtl_eri_write(ioaddr, 0x1d0, ERIAR_MASK_0011,
-				      0x4d02, ERIAR_EXGMAC);
-			rtl_eri_write(ioaddr, 0x1dc, ERIAR_MASK_0011,
-				      0x0060, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1d0, ERIAR_MASK_0011, 0x4d02,
+				      ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1dc, ERIAR_MASK_0011, 0x0060,
+				      ERIAR_EXGMAC);
 		} else {
-			rtl_eri_write(ioaddr, 0x1d0, ERIAR_MASK_0011,
-				      0x0000, ERIAR_EXGMAC);
+			rtl_eri_write(tp, 0x1d0, ERIAR_MASK_0011, 0x0000,
+				      ERIAR_EXGMAC);
 		}
 	}
 }
@@ -1784,6 +1931,13 @@ static int rtl8169_get_sset_count(struct net_device *dev, int sset)
 	}
 }
 
+DECLARE_RTL_COND(rtl_counters_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(CounterAddrLow) & CounterDump;
+}
+
 static void rtl8169_update_counters(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -1792,7 +1946,6 @@ static void rtl8169_update_counters(struct net_device *dev)
 	struct rtl8169_counters *counters;
 	dma_addr_t paddr;
 	u32 cmd;
-	int wait = 1000;
 
 	/*
 	 * Some chips are unable to dump tally counters when the receiver
@@ -1810,13 +1963,8 @@ static void rtl8169_update_counters(struct net_device *dev)
 	RTL_W32(CounterAddrLow, cmd);
 	RTL_W32(CounterAddrLow, cmd | CounterDump);
 
-	while (wait--) {
-		if ((RTL_R32(CounterAddrLow) & CounterDump) == 0) {
-			memcpy(&tp->counters, counters, sizeof(*counters));
-			break;
-		}
-		udelay(10);
-	}
+	if (rtl_udelay_loop_wait_low(tp, &rtl_counters_cond, 10, 1000))
+		memcpy(&tp->counters, counters, sizeof(*counters));
 
 	RTL_W32(CounterAddrLow, 0);
 	RTL_W32(CounterAddrHigh, 0);
@@ -1894,6 +2042,10 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		u32 val;
 		int mac_version;
 	} mac_info[] = {
+		/* 8168G family. */
+		{ 0x7cf00000, 0x4c100000,	RTL_GIGA_MAC_VER_41 },
+		{ 0x7cf00000, 0x4c000000,	RTL_GIGA_MAC_VER_40 },
+
 		/* 8168F family. */
 		{ 0x7c800000, 0x48800000,	RTL_GIGA_MAC_VER_38 },
 		{ 0x7cf00000, 0x48100000,	RTL_GIGA_MAC_VER_36 },
@@ -1933,6 +2085,8 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		{ 0x7c800000, 0x30000000,	RTL_GIGA_MAC_VER_11 },
 
 		/* 8101 family. */
+		{ 0x7cf00000, 0x44900000,	RTL_GIGA_MAC_VER_39 },
+		{ 0x7c800000, 0x44800000,	RTL_GIGA_MAC_VER_39 },
 		{ 0x7c800000, 0x44000000,	RTL_GIGA_MAC_VER_37 },
 		{ 0x7cf00000, 0x40b00000,	RTL_GIGA_MAC_VER_30 },
 		{ 0x7cf00000, 0x40a00000,	RTL_GIGA_MAC_VER_30 },
@@ -2186,7 +2340,7 @@ static void rtl_phy_write_fw(struct rtl8169_private *tp, struct rtl_fw *rtl_fw)
 			index -= regno;
 			break;
 		case PHY_READ_EFUSE:
-			predata = rtl8168d_efuse_read(tp->mmio_addr, regno);
+			predata = rtl8168d_efuse_read(tp, regno);
 			index++;
 			break;
 		case PHY_CLEAR_READCOUNT:
@@ -2626,7 +2780,6 @@ static void rtl8168d_1_hw_phy_config(struct rtl8169_private *tp)
 		{ 0x1f, 0x0000 },
 		{ 0x0d, 0xf880 }
 	};
-	void __iomem *ioaddr = tp->mmio_addr;
 
 	rtl_writephy_batch(tp, phy_reg_init_0, ARRAY_SIZE(phy_reg_init_0));
 
@@ -2638,7 +2791,7 @@ static void rtl8168d_1_hw_phy_config(struct rtl8169_private *tp)
 	rtl_w1w0_phy(tp, 0x0b, 0x0010, 0x00ef);
 	rtl_w1w0_phy(tp, 0x0c, 0xa200, 0x5d00);
 
-	if (rtl8168d_efuse_read(ioaddr, 0x01) == 0xb1) {
+	if (rtl8168d_efuse_read(tp, 0x01) == 0xb1) {
 		static const struct phy_reg phy_reg_init[] = {
 			{ 0x1f, 0x0002 },
 			{ 0x05, 0x669a },
@@ -2738,11 +2891,10 @@ static void rtl8168d_2_hw_phy_config(struct rtl8169_private *tp)
 		{ 0x1f, 0x0000 },
 		{ 0x0d, 0xf880 }
 	};
-	void __iomem *ioaddr = tp->mmio_addr;
 
 	rtl_writephy_batch(tp, phy_reg_init_0, ARRAY_SIZE(phy_reg_init_0));
 
-	if (rtl8168d_efuse_read(ioaddr, 0x01) == 0xb1) {
+	if (rtl8168d_efuse_read(tp, 0x01) == 0xb1) {
 		static const struct phy_reg phy_reg_init[] = {
 			{ 0x1f, 0x0002 },
 			{ 0x05, 0x669a },
@@ -3010,8 +3162,7 @@ static void rtl8168e_2_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* EEE setting */
-	rtl_w1w0_eri(tp->mmio_addr, 0x1b0, ERIAR_MASK_1111, 0x0000, 0x0003,
-		     ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_1111, 0x0000, 0x0003, ERIAR_EXGMAC);
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
 	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x2000);
@@ -3115,7 +3266,6 @@ static void rtl8168f_2_hw_phy_config(struct rtl8169_private *tp)
 
 static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 {
-	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct phy_reg phy_reg_init[] = {
 		/* Channel estimation fine tune */
 		{ 0x1f, 0x0003 },
@@ -3189,7 +3339,7 @@ static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* eee setting */
-	rtl_w1w0_eri(ioaddr, 0x1b0, ERIAR_MASK_0001, 0x00, 0x03, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x00, 0x03, ERIAR_EXGMAC);
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
 	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x2000);
@@ -3211,6 +3361,55 @@ static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
+static void rtl8168g_1_hw_phy_config(struct rtl8169_private *tp)
+{
+	static const u16 mac_ocp_patch[] = {
+		0xe008, 0xe01b, 0xe01d, 0xe01f,
+		0xe021, 0xe023, 0xe025, 0xe027,
+		0x49d2, 0xf10d, 0x766c, 0x49e2,
+		0xf00a, 0x1ec0, 0x8ee1, 0xc60a,
+
+		0x77c0, 0x4870, 0x9fc0, 0x1ea0,
+		0xc707, 0x8ee1, 0x9d6c, 0xc603,
+		0xbe00, 0xb416, 0x0076, 0xe86c,
+		0xc602, 0xbe00, 0x0000, 0xc602,
+
+		0xbe00, 0x0000, 0xc602, 0xbe00,
+		0x0000, 0xc602, 0xbe00, 0x0000,
+		0xc602, 0xbe00, 0x0000, 0xc602,
+		0xbe00, 0x0000, 0xc602, 0xbe00,
+
+		0x0000, 0x0000, 0x0000, 0x0000
+	};
+	u32 i;
+
+	/* Patch code for GPHY reset */
+	for (i = 0; i < ARRAY_SIZE(mac_ocp_patch); i++)
+		r8168_mac_ocp_write(tp, 0xf800 + 2*i, mac_ocp_patch[i]);
+	r8168_mac_ocp_write(tp, 0xfc26, 0x8000);
+	r8168_mac_ocp_write(tp, 0xfc28, 0x0075);
+
+	rtl_apply_firmware(tp);
+
+	if (r8168_phy_ocp_read(tp, 0xa460) & 0x0100)
+		rtl_w1w0_phy_ocp(tp, 0xbcc4, 0x0000, 0x8000);
+	else
+		rtl_w1w0_phy_ocp(tp, 0xbcc4, 0x8000, 0x0000);
+
+	if (r8168_phy_ocp_read(tp, 0xa466) & 0x0100)
+		rtl_w1w0_phy_ocp(tp, 0xc41a, 0x0002, 0x0000);
+	else
+		rtl_w1w0_phy_ocp(tp, 0xbcc4, 0x0000, 0x0002);
+
+	rtl_w1w0_phy_ocp(tp, 0xa442, 0x000c, 0x0000);
+	rtl_w1w0_phy_ocp(tp, 0xa4b2, 0x0004, 0x0000);
+
+	r8168_phy_ocp_write(tp, 0xa436, 0x8012);
+	rtl_w1w0_phy_ocp(tp, 0xa438, 0x8000, 0x0000);
+
+	rtl_w1w0_phy_ocp(tp, 0xc422, 0x4000, 0x2000);
+}
+
 static void rtl8102e_hw_phy_config(struct rtl8169_private *tp)
 {
 	static const struct phy_reg phy_reg_init[] = {
@@ -3256,8 +3455,6 @@ static void rtl8105e_hw_phy_config(struct rtl8169_private *tp)
 
 static void rtl8402_hw_phy_config(struct rtl8169_private *tp)
 {
-	void __iomem *ioaddr = tp->mmio_addr;
-
 	/* Disable ALDPS before setting firmware */
 	rtl_writephy(tp, 0x1f, 0x0000);
 	rtl_writephy(tp, 0x18, 0x0310);
@@ -3266,13 +3463,35 @@ static void rtl8402_hw_phy_config(struct rtl8169_private *tp)
 	rtl_apply_firmware(tp);
 
 	/* EEE setting */
-	rtl_eri_write(ioaddr, 0x1b0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0x1b0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
 	rtl_writephy(tp, 0x1f, 0x0004);
 	rtl_writephy(tp, 0x10, 0x401f);
 	rtl_writephy(tp, 0x19, 0x7030);
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
+static void rtl8106e_hw_phy_config(struct rtl8169_private *tp)
+{
+	static const struct phy_reg phy_reg_init[] = {
+		{ 0x1f, 0x0004 },
+		{ 0x10, 0xc07f },
+		{ 0x19, 0x7030 },
+		{ 0x1f, 0x0000 }
+	};
+
+	/* Disable ALDPS before ram code */
+	rtl_writephy(tp, 0x1f, 0x0000);
+	rtl_writephy(tp, 0x18, 0x0310);
+	msleep(100);
+
+	rtl_apply_firmware(tp);
+
+	rtl_eri_write(tp, 0x1b0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
+
+	rtl_eri_write(tp, 0x1d0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+}
+
 static void rtl_hw_phy_config(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -3369,6 +3588,15 @@ static void rtl_hw_phy_config(struct net_device *dev)
 		rtl8411_hw_phy_config(tp);
 		break;
 
+	case RTL_GIGA_MAC_VER_39:
+		rtl8106e_hw_phy_config(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_40:
+		rtl8168g_1_hw_phy_config(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_41:
 	default:
 		break;
 	}
@@ -3426,18 +3654,16 @@ static void rtl8169_release_board(struct pci_dev *pdev, struct net_device *dev,
 	free_netdev(dev);
 }
 
+DECLARE_RTL_COND(rtl_phy_reset_cond)
+{
+	return tp->phy_reset_pending(tp);
+}
+
 static void rtl8169_phy_reset(struct net_device *dev,
 			      struct rtl8169_private *tp)
 {
-	unsigned int i;
-
 	tp->phy_reset_enable(tp);
-	for (i = 0; i < 100; i++) {
-		if (!tp->phy_reset_pending(tp))
-			return;
-		msleep(1);
-	}
-	netif_err(tp, link, dev, "PHY reset failed\n");
+	rtl_msleep_loop_wait_low(tp, &rtl_phy_reset_cond, 1, 100);
 }
 
 static bool rtl_tbi_enabled(struct rtl8169_private *tp)
@@ -3512,7 +3738,7 @@ static void rtl_rar_set(struct rtl8169_private *tp, u8 *addr)
 								low  >> 16 },
 		};
 
-		rtl_write_exgmac_batch(ioaddr, e, ARRAY_SIZE(e));
+		rtl_write_exgmac_batch(tp, e, ARRAY_SIZE(e));
 	}
 
 	RTL_W8(Cfg9346, Cfg9346_Lock);
@@ -3589,6 +3815,11 @@ static void __devinit rtl_init_mdio_ops(struct rtl8169_private *tp)
 		ops->write	= r8168dp_2_mdio_write;
 		ops->read	= r8168dp_2_mdio_read;
 		break;
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+		ops->write	= r8168g_mdio_write;
+		ops->read	= r8168g_mdio_read;
+		break;
 	default:
 		ops->write	= r8169_mdio_write;
 		ops->read	= r8169_mdio_read;
@@ -3608,6 +3839,9 @@ static void rtl_wol_suspend_quirk(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_34:
 	case RTL_GIGA_MAC_VER_37:
 	case RTL_GIGA_MAC_VER_38:
+	case RTL_GIGA_MAC_VER_39:
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
 		RTL_W32(RxConfig, RTL_R32(RxConfig) |
 			AcceptBroadcast | AcceptMulticast | AcceptMyPhys);
 		break;
@@ -3761,7 +3995,7 @@ static void r8168_pll_power_down(struct rtl8169_private *tp)
 
 	if (tp->mac_version == RTL_GIGA_MAC_VER_32 ||
 	    tp->mac_version == RTL_GIGA_MAC_VER_33)
-		rtl_ephy_write(ioaddr, 0x19, 0xff64);
+		rtl_ephy_write(tp, 0x19, 0xff64);
 
 	if (rtl_wol_pll_power_down(tp))
 		return;
@@ -3830,6 +4064,7 @@ static void __devinit rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_29:
 	case RTL_GIGA_MAC_VER_30:
 	case RTL_GIGA_MAC_VER_37:
+	case RTL_GIGA_MAC_VER_39:
 		ops->down	= r810x_pll_power_down;
 		ops->up		= r810x_pll_power_up;
 		break;
@@ -3855,6 +4090,8 @@ static void __devinit rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_35:
 	case RTL_GIGA_MAC_VER_36:
 	case RTL_GIGA_MAC_VER_38:
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
 		ops->down	= r8168_pll_power_down;
 		ops->up		= r8168_pll_power_up;
 		break;
@@ -4051,6 +4288,8 @@ static void __devinit rtl_init_jumbo_ops(struct rtl8169_private *tp)
 	 * No action needed for jumbo frames with 8169.
 	 * No jumbo for 810x at all.
 	 */
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
 	default:
 		ops->disable	= NULL;
 		ops->enable	= NULL;
@@ -4058,20 +4297,20 @@ static void __devinit rtl_init_jumbo_ops(struct rtl8169_private *tp)
 	}
 }
 
+DECLARE_RTL_COND(rtl_chipcmd_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R8(ChipCmd) & CmdReset;
+}
+
 static void rtl_hw_reset(struct rtl8169_private *tp)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
-	int i;
 
-	/* Soft reset the chip. */
 	RTL_W8(ChipCmd, CmdReset);
 
-	/* Check that the chip has finished the reset. */
-	for (i = 0; i < 100; i++) {
-		if ((RTL_R8(ChipCmd) & CmdReset) == 0)
-			break;
-		udelay(100);
-	}
+	rtl_udelay_loop_wait_low(tp, &rtl_chipcmd_cond, 100, 100);
 }
 
 static void rtl_request_uncached_firmware(struct rtl8169_private *tp)
@@ -4125,6 +4364,20 @@ static void rtl_rx_close(struct rtl8169_private *tp)
 	RTL_W32(RxConfig, RTL_R32(RxConfig) & ~RX_CONFIG_ACCEPT_MASK);
 }
 
+DECLARE_RTL_COND(rtl_npq_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R8(TxPoll) & NPQ;
+}
+
+DECLARE_RTL_COND(rtl_txcfg_empty_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(TxConfig) & TXCFG_EMPTY;
+}
+
 static void rtl8169_hw_reset(struct rtl8169_private *tp)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
@@ -4137,16 +4390,16 @@ static void rtl8169_hw_reset(struct rtl8169_private *tp)
 	if (tp->mac_version == RTL_GIGA_MAC_VER_27 ||
 	    tp->mac_version == RTL_GIGA_MAC_VER_28 ||
 	    tp->mac_version == RTL_GIGA_MAC_VER_31) {
-		while (RTL_R8(TxPoll) & NPQ)
-			udelay(20);
+		rtl_udelay_loop_wait_low(tp, &rtl_npq_cond, 20, 42*42);
 	} else if (tp->mac_version == RTL_GIGA_MAC_VER_34 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_35 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_36 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_37 ||
+	           tp->mac_version == RTL_GIGA_MAC_VER_40 ||
+	           tp->mac_version == RTL_GIGA_MAC_VER_41 ||
 	           tp->mac_version == RTL_GIGA_MAC_VER_38) {
 		RTL_W8(ChipCmd, RTL_R8(ChipCmd) | StopReq);
-		while (!(RTL_R32(TxConfig) & TXCFG_EMPTY))
-			udelay(100);
+		rtl_udelay_loop_wait_high(tp, &rtl_txcfg_empty_cond, 100, 666);
 	} else {
 		RTL_W8(ChipCmd, RTL_R8(ChipCmd) | StopReq);
 		udelay(100);
@@ -4352,15 +4605,12 @@ static void rtl_hw_start_8169(struct net_device *dev)
 static void rtl_csi_write(struct rtl8169_private *tp, int addr, int value)
 {
 	if (tp->csi_ops.write)
-		tp->csi_ops.write(tp->mmio_addr, addr, value);
+		tp->csi_ops.write(tp, addr, value);
 }
 
 static u32 rtl_csi_read(struct rtl8169_private *tp, int addr)
 {
-	if (tp->csi_ops.read)
-		return tp->csi_ops.read(tp->mmio_addr, addr);
-	else
-		return ~0;
+	return tp->csi_ops.read ? tp->csi_ops.read(tp, addr) : ~0;
 }
 
 static void rtl_csi_access_enable(struct rtl8169_private *tp, u32 bits)
@@ -4381,73 +4631,56 @@ static void rtl_csi_access_enable_2(struct rtl8169_private *tp)
 	rtl_csi_access_enable(tp, 0x27000000);
 }
 
-static void r8169_csi_write(void __iomem *ioaddr, int addr, int value)
+DECLARE_RTL_COND(rtl_csiar_cond)
 {
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(CSIAR) & CSIAR_FLAG;
+}
+
+static void r8169_csi_write(struct rtl8169_private *tp, int addr, int value)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(CSIDR, value);
 	RTL_W32(CSIAR, CSIAR_WRITE_CMD | (addr & CSIAR_ADDR_MASK) |
 		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
 
-	for (i = 0; i < 100; i++) {
-		if (!(RTL_R32(CSIAR) & CSIAR_FLAG))
-			break;
-		udelay(10);
-	}
+	rtl_udelay_loop_wait_low(tp, &rtl_csiar_cond, 10, 100);
 }
 
-static u32 r8169_csi_read(void __iomem *ioaddr, int addr)
+static u32 r8169_csi_read(struct rtl8169_private *tp, int addr)
 {
-	u32 value = ~0x00;
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(CSIAR, (addr & CSIAR_ADDR_MASK) |
 		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
 
-	for (i = 0; i < 100; i++) {
-		if (RTL_R32(CSIAR) & CSIAR_FLAG) {
-			value = RTL_R32(CSIDR);
-			break;
-		}
-		udelay(10);
-	}
-
-	return value;
+	return rtl_udelay_loop_wait_high(tp, &rtl_csiar_cond, 10, 100) ?
+		RTL_R32(CSIDR) : ~0;
 }
 
-static void r8402_csi_write(void __iomem *ioaddr, int addr, int value)
+static void r8402_csi_write(struct rtl8169_private *tp, int addr, int value)
 {
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(CSIDR, value);
 	RTL_W32(CSIAR, CSIAR_WRITE_CMD | (addr & CSIAR_ADDR_MASK) |
 		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT |
 		CSIAR_FUNC_NIC);
 
-	for (i = 0; i < 100; i++) {
-		if (!(RTL_R32(CSIAR) & CSIAR_FLAG))
-			break;
-		udelay(10);
-	}
+	rtl_udelay_loop_wait_low(tp, &rtl_csiar_cond, 10, 100);
 }
 
-static u32 r8402_csi_read(void __iomem *ioaddr, int addr)
+static u32 r8402_csi_read(struct rtl8169_private *tp, int addr)
 {
-	u32 value = ~0x00;
-	unsigned int i;
+	void __iomem *ioaddr = tp->mmio_addr;
 
 	RTL_W32(CSIAR, (addr & CSIAR_ADDR_MASK) | CSIAR_FUNC_NIC |
 		CSIAR_BYTE_ENABLE << CSIAR_BYTE_ENABLE_SHIFT);
 
-	for (i = 0; i < 100; i++) {
-		if (RTL_R32(CSIAR) & CSIAR_FLAG) {
-			value = RTL_R32(CSIDR);
-			break;
-		}
-		udelay(10);
-	}
-
-	return value;
+	return rtl_udelay_loop_wait_high(tp, &rtl_csiar_cond, 10, 100) ?
+		RTL_R32(CSIDR) : ~0;
 }
 
 static void __devinit rtl_init_csi_ops(struct rtl8169_private *tp)
@@ -4492,13 +4725,14 @@ struct ephy_info {
 	u16 bits;
 };
 
-static void rtl_ephy_init(void __iomem *ioaddr, const struct ephy_info *e, int len)
+static void rtl_ephy_init(struct rtl8169_private *tp, const struct ephy_info *e,
+			  int len)
 {
 	u16 w;
 
 	while (len-- > 0) {
-		w = (rtl_ephy_read(ioaddr, e->offset) & ~e->mask) | e->bits;
-		rtl_ephy_write(ioaddr, e->offset, w);
+		w = (rtl_ephy_read(tp, e->offset) & ~e->mask) | e->bits;
+		rtl_ephy_write(tp, e->offset, w);
 		e++;
 	}
 }
@@ -4582,7 +4816,6 @@ static void __rtl_hw_start_8168cp(struct rtl8169_private *tp)
 
 static void rtl_hw_start_8168cp_1(struct rtl8169_private *tp)
 {
-	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8168cp[] = {
 		{ 0x01, 0,	0x0001 },
 		{ 0x02, 0x0800,	0x1000 },
@@ -4593,7 +4826,7 @@ static void rtl_hw_start_8168cp_1(struct rtl8169_private *tp)
 
 	rtl_csi_access_enable_2(tp);
 
-	rtl_ephy_init(ioaddr, e_info_8168cp, ARRAY_SIZE(e_info_8168cp));
+	rtl_ephy_init(tp, e_info_8168cp, ARRAY_SIZE(e_info_8168cp));
 
 	__rtl_hw_start_8168cp(tp);
 }
@@ -4644,14 +4877,13 @@ static void rtl_hw_start_8168c_1(struct rtl8169_private *tp)
 
 	RTL_W8(DBG_REG, 0x06 | FIX_NAK_1 | FIX_NAK_2);
 
-	rtl_ephy_init(ioaddr, e_info_8168c_1, ARRAY_SIZE(e_info_8168c_1));
+	rtl_ephy_init(tp, e_info_8168c_1, ARRAY_SIZE(e_info_8168c_1));
 
 	__rtl_hw_start_8168cp(tp);
 }
 
 static void rtl_hw_start_8168c_2(struct rtl8169_private *tp)
 {
-	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8168c_2[] = {
 		{ 0x01, 0,	0x0001 },
 		{ 0x03, 0x0400,	0x0220 }
@@ -4659,7 +4891,7 @@ static void rtl_hw_start_8168c_2(struct rtl8169_private *tp)
 
 	rtl_csi_access_enable_2(tp);
 
-	rtl_ephy_init(ioaddr, e_info_8168c_2, ARRAY_SIZE(e_info_8168c_2));
+	rtl_ephy_init(tp, e_info_8168c_2, ARRAY_SIZE(e_info_8168c_2));
 
 	__rtl_hw_start_8168cp(tp);
 }
@@ -4727,8 +4959,8 @@ static void rtl_hw_start_8168d_4(struct rtl8169_private *tp)
 		const struct ephy_info *e = e_info_8168d_4 + i;
 		u16 w;
 
-		w = rtl_ephy_read(ioaddr, e->offset);
-		rtl_ephy_write(ioaddr, 0x03, (w & e->mask) | e->bits);
+		w = rtl_ephy_read(tp, e->offset);
+		rtl_ephy_write(tp, 0x03, (w & e->mask) | e->bits);
 	}
 
 	rtl_enable_clock_request(pdev);
@@ -4756,7 +4988,7 @@ static void rtl_hw_start_8168e_1(struct rtl8169_private *tp)
 
 	rtl_csi_access_enable_2(tp);
 
-	rtl_ephy_init(ioaddr, e_info_8168e_1, ARRAY_SIZE(e_info_8168e_1));
+	rtl_ephy_init(tp, e_info_8168e_1, ARRAY_SIZE(e_info_8168e_1));
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
@@ -4782,19 +5014,18 @@ static void rtl_hw_start_8168e_2(struct rtl8169_private *tp)
 
 	rtl_csi_access_enable_1(tp);
 
-	rtl_ephy_init(ioaddr, e_info_8168e_2, ARRAY_SIZE(e_info_8168e_2));
+	rtl_ephy_init(tp, e_info_8168e_2, ARRAY_SIZE(e_info_8168e_2));
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
-	rtl_eri_write(ioaddr, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xc8, ERIAR_MASK_1111, 0x00100002, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xd0, ERIAR_MASK_1111, 0x07ff0060, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00,
-		     ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xc8, ERIAR_MASK_1111, 0x00100002, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xd0, ERIAR_MASK_1111, 0x07ff0060, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00, ERIAR_EXGMAC);
 
 	RTL_W8(MaxTxPacketSize, EarlySize);
 
@@ -4820,16 +5051,16 @@ static void rtl_hw_start_8168f(struct rtl8169_private *tp)
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
-	rtl_eri_write(ioaddr, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xc8, ERIAR_MASK_1111, 0x00100002, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0x1d0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xd0, ERIAR_MASK_1111, 0x00000060, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xc8, ERIAR_MASK_1111, 0x00100002, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x1d0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xd0, ERIAR_MASK_1111, 0x00000060, ERIAR_EXGMAC);
 
 	RTL_W8(MaxTxPacketSize, EarlySize);
 
@@ -4854,10 +5085,9 @@ static void rtl_hw_start_8168f_1(struct rtl8169_private *tp)
 
 	rtl_hw_start_8168f(tp);
 
-	rtl_ephy_init(ioaddr, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
+	rtl_ephy_init(tp, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
 
-	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00,
-		     ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00, ERIAR_EXGMAC);
 
 	/* Adjust EEE LED frequency */
 	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
@@ -4865,7 +5095,6 @@ static void rtl_hw_start_8168f_1(struct rtl8169_private *tp)
 
 static void rtl_hw_start_8411(struct rtl8169_private *tp)
 {
-	void __iomem *ioaddr = tp->mmio_addr;
 	static const struct ephy_info e_info_8168f_1[] = {
 		{ 0x06, 0x00c0,	0x0020 },
 		{ 0x0f, 0xffff,	0x5200 },
@@ -4875,10 +5104,39 @@ static void rtl_hw_start_8411(struct rtl8169_private *tp)
 
 	rtl_hw_start_8168f(tp);
 
-	rtl_ephy_init(ioaddr, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
+	rtl_ephy_init(tp, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
 
-	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0x0000,
-		     ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0x0000, ERIAR_EXGMAC);
+}
+
+static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	rtl_eri_write(tp, 0xc8, ERIAR_MASK_0101, 0x080002, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xcc, ERIAR_MASK_0001, 0x38, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xd0, ERIAR_MASK_0001, 0x48, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
+
+	rtl_csi_access_enable_1(tp);
+
+	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
+
+	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+
+	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
+	RTL_W32(MISC, RTL_R32(MISC) & ~RXDV_GATED_EN);
+	RTL_W8(MaxTxPacketSize, EarlySize);
+
+	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+
+	/* Adjust EEE LED frequency */
+	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
+
+	rtl_w1w0_eri(tp, 0x2fc, ERIAR_MASK_0001, 0x01, 0x02, ERIAR_EXGMAC);
 }
 
 static void rtl_hw_start_8168(struct net_device *dev)
@@ -4982,6 +5240,11 @@ static void rtl_hw_start_8168(struct net_device *dev)
 		rtl_hw_start_8411(tp);
 		break;
 
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+		rtl_hw_start_8168g_1(tp);
+		break;
+
 	default:
 		printk(KERN_ERR PFX "%s: unknown chipset (mac_version = %d).\n",
 			dev->name, tp->mac_version);
@@ -5036,7 +5299,7 @@ static void rtl_hw_start_8102e_1(struct rtl8169_private *tp)
 	if ((cfg1 & LEDS0) && (cfg1 & LEDS1))
 		RTL_W8(Config1, cfg1 & ~LEDS0);
 
-	rtl_ephy_init(ioaddr, e_info_8102e_1, ARRAY_SIZE(e_info_8102e_1));
+	rtl_ephy_init(tp, e_info_8102e_1, ARRAY_SIZE(e_info_8102e_1));
 }
 
 static void rtl_hw_start_8102e_2(struct rtl8169_private *tp)
@@ -5056,7 +5319,7 @@ static void rtl_hw_start_8102e_3(struct rtl8169_private *tp)
 {
 	rtl_hw_start_8102e_2(tp);
 
-	rtl_ephy_write(tp->mmio_addr, 0x03, 0xc2f9);
+	rtl_ephy_write(tp, 0x03, 0xc2f9);
 }
 
 static void rtl_hw_start_8105e_1(struct rtl8169_private *tp)
@@ -5082,15 +5345,13 @@ static void rtl_hw_start_8105e_1(struct rtl8169_private *tp)
 	RTL_W8(MCU, RTL_R8(MCU) | EN_NDP | EN_OOB_RESET);
 	RTL_W8(DLLPR, RTL_R8(DLLPR) | PFM_EN);
 
-	rtl_ephy_init(ioaddr, e_info_8105e_1, ARRAY_SIZE(e_info_8105e_1));
+	rtl_ephy_init(tp, e_info_8105e_1, ARRAY_SIZE(e_info_8105e_1));
 }
 
 static void rtl_hw_start_8105e_2(struct rtl8169_private *tp)
 {
-	void __iomem *ioaddr = tp->mmio_addr;
-
 	rtl_hw_start_8105e_1(tp);
-	rtl_ephy_write(ioaddr, 0x1e, rtl_ephy_read(ioaddr, 0x1e) | 0x8000);
+	rtl_ephy_write(tp, 0x1e, rtl_ephy_read(tp, 0x1e) | 0x8000);
 }
 
 static void rtl_hw_start_8402(struct rtl8169_private *tp)
@@ -5109,18 +5370,29 @@ static void rtl_hw_start_8402(struct rtl8169_private *tp)
 	RTL_W32(TxConfig, RTL_R32(TxConfig) | TXCFG_AUTO_FIFO);
 	RTL_W8(MCU, RTL_R8(MCU) & ~NOW_IS_OOB);
 
-	rtl_ephy_init(ioaddr, e_info_8402, ARRAY_SIZE(e_info_8402));
+	rtl_ephy_init(tp, e_info_8402, ARRAY_SIZE(e_info_8402));
 
 	rtl_tx_performance_tweak(tp->pci_dev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
-	rtl_eri_write(ioaddr, 0xc8, ERIAR_MASK_1111, 0x00000002, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xe8, ERIAR_MASK_1111, 0x00000006, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_eri_write(ioaddr, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_w1w0_eri(ioaddr, 0x0d4, ERIAR_MASK_0011, 0x0e00, 0xff00,
-		     ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xc8, ERIAR_MASK_1111, 0x00000002, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00000006, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0e00, 0xff00, ERIAR_EXGMAC);
+}
+
+static void rtl_hw_start_8106(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	/* Force LAN exit from ASPM if Rx/Tx are not idle */
+	RTL_W32(FuncEvent, RTL_R32(FuncEvent) | 0x002800);
+
+	RTL_W32(MISC, (RTL_R32(MISC) | DISABLE_LAN_EN) & ~EARLY_TALLY_EN);
+	RTL_W8(MCU, RTL_R8(MCU) | EN_NDP | EN_OOB_RESET);
+	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
 }
 
 static void rtl_hw_start_8101(struct net_device *dev)
@@ -5167,6 +5439,10 @@ static void rtl_hw_start_8101(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_37:
 		rtl_hw_start_8402(tp);
 		break;
+
+	case RTL_GIGA_MAC_VER_39:
+		rtl_hw_start_8106(tp);
+		break;
 	}
 
 	RTL_W8(Cfg9346, Cfg9346_Lock);
@@ -5380,7 +5656,6 @@ static void rtl8169_tx_clear(struct rtl8169_private *tp)
 {
 	rtl8169_tx_clear_range(tp, tp->dirty_tx, NUM_TX_DESC);
 	tp->cur_tx = tp->dirty_tx = 0;
-	netdev_reset_queue(tp->dev);
 }
 
 static void rtl_reset_work(struct rtl8169_private *tp)
@@ -5535,8 +5810,6 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 
 	txd->opts2 = cpu_to_le32(opts[1]);
 
-	netdev_sent_queue(dev, skb->len);
-
 	skb_tx_timestamp(skb);
 
 	wmb();
@@ -5633,16 +5906,9 @@ static void rtl8169_pcierr_interrupt(struct net_device *dev)
 	rtl_schedule_task(tp, RTL_FLAG_TASK_RESET_PENDING);
 }
 
-struct rtl_txc {
-	int packets;
-	int bytes;
-};
-
 static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 {
-	struct rtl8169_stats *tx_stats = &tp->tx_stats;
 	unsigned int dirty_tx, tx_left;
-	struct rtl_txc txc = { 0, 0 };
 
 	dirty_tx = tp->dirty_tx;
 	smp_rmb();
@@ -5661,24 +5927,17 @@ static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 		rtl8169_unmap_tx_skb(&tp->pci_dev->dev, tx_skb,
 				     tp->TxDescArray + entry);
 		if (status & LastFrag) {
-			struct sk_buff *skb = tx_skb->skb;
-
-			txc.packets++;
-			txc.bytes += skb->len;
-			dev_kfree_skb(skb);
+			u64_stats_update_begin(&tp->tx_stats.syncp);
+			tp->tx_stats.packets++;
+			tp->tx_stats.bytes += tx_skb->skb->len;
+			u64_stats_update_end(&tp->tx_stats.syncp);
+			dev_kfree_skb(tx_skb->skb);
 			tx_skb->skb = NULL;
 		}
 		dirty_tx++;
 		tx_left--;
 	}
 
-	u64_stats_update_begin(&tx_stats->syncp);
-	tx_stats->packets += txc.packets;
-	tx_stats->bytes += txc.bytes;
-	u64_stats_update_end(&tx_stats->syncp);
-
-	netdev_completed_queue(dev, txc.packets, txc.bytes);
-
 	if (tp->dirty_tx != dirty_tx) {
 		tp->dirty_tx = dirty_tx;
 		/* Sync with rtl8169_start_xmit:
@@ -6435,6 +6694,67 @@ static unsigned rtl_try_msi(struct rtl8169_private *tp,
 	return msi;
 }
 
+DECLARE_RTL_COND(rtl_link_list_ready_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R8(MCU) & LINK_LIST_RDY;
+}
+
+DECLARE_RTL_COND(rtl_rxtx_empty_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return (RTL_R8(MCU) & RXTX_EMPTY) == RXTX_EMPTY;
+}
+
+static void __devinit rtl_hw_init_8168g(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	u32 data;
+
+	tp->ocp_base = OCP_STD_PHY_BASE;
+
+	RTL_W32(MISC, RTL_R32(MISC) | RXDV_GATED_EN);
+
+	if (!rtl_udelay_loop_wait_high(tp, &rtl_txcfg_empty_cond, 100, 42))
+		return;
+
+	if (!rtl_udelay_loop_wait_high(tp, &rtl_rxtx_empty_cond, 100, 42))
+		return;
+
+	RTL_W8(ChipCmd, RTL_R8(ChipCmd) & ~(CmdTxEnb | CmdRxEnb));
+	msleep(1);
+	RTL_W8(MCU, RTL_R8(MCU) & ~NOW_IS_OOB);
+
+	data = r8168_mac_ocp_read(tp, 0xe8de);
+	data &= ~(1 << 14);
+	r8168_mac_ocp_write(tp, 0xe8de, data);
+
+	if (!rtl_udelay_loop_wait_high(tp, &rtl_link_list_ready_cond, 100, 42))
+		return;
+
+	data = r8168_mac_ocp_read(tp, 0xe8de);
+	data |= (1 << 15);
+	r8168_mac_ocp_write(tp, 0xe8de, data);
+
+	if (!rtl_udelay_loop_wait_high(tp, &rtl_link_list_ready_cond, 100, 42))
+		return;
+}
+
+static void __devinit rtl_hw_initialize(struct rtl8169_private *tp)
+{
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+		rtl_hw_init_8168g(tp);
+		break;
+
+	default:
+		break;
+	}
+}
+
 static int __devinit
 rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -6544,6 +6864,8 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	rtl_irq_disable(tp);
 
+	rtl_hw_initialize(tp);
+
 	rtl_hw_reset(tp);
 
 	rtl_ack_events(tp, 0xffff);
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 79bf09b4197..af0b867a6cf 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -49,6 +49,34 @@
 		NETIF_MSG_RX_ERR| \
 		NETIF_MSG_TX_ERR)
 
+#if defined(CONFIG_CPU_SUBTYPE_SH7734) || \
+	defined(CONFIG_CPU_SUBTYPE_SH7763) || \
+	defined(CONFIG_ARCH_R8A7740)
+static void sh_eth_select_mii(struct net_device *ndev)
+{
+	u32 value = 0x0;
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	switch (mdp->phy_interface) {
+	case PHY_INTERFACE_MODE_GMII:
+		value = 0x2;
+		break;
+	case PHY_INTERFACE_MODE_MII:
+		value = 0x1;
+		break;
+	case PHY_INTERFACE_MODE_RMII:
+		value = 0x0;
+		break;
+	default:
+		pr_warn("PHY interface mode was not setup. Set to MII.\n");
+		value = 0x1;
+		break;
+	}
+
+	sh_eth_write(ndev, value, RMII_MII);
+}
+#endif
+
 /* There is CPU dependent code */
 #if defined(CONFIG_CPU_SUBTYPE_SH7724)
 #define SH_ETH_RESET_DEFAULT	1
@@ -102,6 +130,8 @@ static struct sh_eth_cpu_data sh_eth_my_cpu_data = {
 #elif defined(CONFIG_CPU_SUBTYPE_SH7757)
 #define SH_ETH_HAS_BOTH_MODULES	1
 #define SH_ETH_HAS_TSU	1
+static int sh_eth_check_reset(struct net_device *ndev);
+
 static void sh_eth_set_duplex(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
@@ -176,23 +206,19 @@ static void sh_eth_chip_reset_giga(struct net_device *ndev)
 }
 
 static int sh_eth_is_gether(struct sh_eth_private *mdp);
-static void sh_eth_reset(struct net_device *ndev)
+static int sh_eth_reset(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	int cnt = 100;
+	int ret = 0;
 
 	if (sh_eth_is_gether(mdp)) {
 		sh_eth_write(ndev, 0x03, EDSR);
 		sh_eth_write(ndev, sh_eth_read(ndev, EDMR) | EDMR_SRST_GETHER,
 				EDMR);
-		while (cnt > 0) {
-			if (!(sh_eth_read(ndev, EDMR) & 0x3))
-				break;
-			mdelay(1);
-			cnt--;
-		}
-		if (cnt < 0)
-			printk(KERN_ERR "Device reset fail\n");
+
+		ret = sh_eth_check_reset(ndev);
+		if (ret)
+			goto out;
 
 		/* Table Init */
 		sh_eth_write(ndev, 0x0, TDLAR);
@@ -210,6 +236,9 @@ static void sh_eth_reset(struct net_device *ndev)
 		sh_eth_write(ndev, sh_eth_read(ndev, EDMR) & ~EDMR_SRST_ETHER,
 				EDMR);
 	}
+
+out:
+	return ret;
 }
 
 static void sh_eth_set_duplex_giga(struct net_device *ndev)
@@ -282,7 +311,9 @@ static struct sh_eth_cpu_data *sh_eth_get_cpu_data(struct sh_eth_private *mdp)
 
 #elif defined(CONFIG_CPU_SUBTYPE_SH7734) || defined(CONFIG_CPU_SUBTYPE_SH7763)
 #define SH_ETH_HAS_TSU	1
+static int sh_eth_check_reset(struct net_device *ndev);
 static void sh_eth_reset_hw_crc(struct net_device *ndev);
+
 static void sh_eth_chip_reset(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
@@ -292,35 +323,6 @@ static void sh_eth_chip_reset(struct net_device *ndev)
 	mdelay(1);
 }
 
-static void sh_eth_reset(struct net_device *ndev)
-{
-	int cnt = 100;
-
-	sh_eth_write(ndev, EDSR_ENALL, EDSR);
-	sh_eth_write(ndev, sh_eth_read(ndev, EDMR) | EDMR_SRST_GETHER, EDMR);
-	while (cnt > 0) {
-		if (!(sh_eth_read(ndev, EDMR) & 0x3))
-			break;
-		mdelay(1);
-		cnt--;
-	}
-	if (cnt == 0)
-		printk(KERN_ERR "Device reset fail\n");
-
-	/* Table Init */
-	sh_eth_write(ndev, 0x0, TDLAR);
-	sh_eth_write(ndev, 0x0, TDFAR);
-	sh_eth_write(ndev, 0x0, TDFXR);
-	sh_eth_write(ndev, 0x0, TDFFR);
-	sh_eth_write(ndev, 0x0, RDLAR);
-	sh_eth_write(ndev, 0x0, RDFAR);
-	sh_eth_write(ndev, 0x0, RDFXR);
-	sh_eth_write(ndev, 0x0, RDFFR);
-
-	/* Reset HW CRC register */
-	sh_eth_reset_hw_crc(ndev);
-}
-
 static void sh_eth_set_duplex(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
@@ -377,9 +379,41 @@ static struct sh_eth_cpu_data sh_eth_my_cpu_data = {
 	.tsu		= 1,
 #if defined(CONFIG_CPU_SUBTYPE_SH7734)
 	.hw_crc     = 1,
+	.select_mii = 1,
 #endif
 };
 
+static int sh_eth_reset(struct net_device *ndev)
+{
+	int ret = 0;
+
+	sh_eth_write(ndev, EDSR_ENALL, EDSR);
+	sh_eth_write(ndev, sh_eth_read(ndev, EDMR) | EDMR_SRST_GETHER, EDMR);
+
+	ret = sh_eth_check_reset(ndev);
+	if (ret)
+		goto out;
+
+	/* Table Init */
+	sh_eth_write(ndev, 0x0, TDLAR);
+	sh_eth_write(ndev, 0x0, TDFAR);
+	sh_eth_write(ndev, 0x0, TDFXR);
+	sh_eth_write(ndev, 0x0, TDFFR);
+	sh_eth_write(ndev, 0x0, RDLAR);
+	sh_eth_write(ndev, 0x0, RDFAR);
+	sh_eth_write(ndev, 0x0, RDFXR);
+	sh_eth_write(ndev, 0x0, RDFFR);
+
+	/* Reset HW CRC register */
+	sh_eth_reset_hw_crc(ndev);
+
+	/* Select MII mode */
+	if (sh_eth_my_cpu_data.select_mii)
+		sh_eth_select_mii(ndev);
+out:
+	return ret;
+}
+
 static void sh_eth_reset_hw_crc(struct net_device *ndev)
 {
 	if (sh_eth_my_cpu_data.hw_crc)
@@ -388,44 +422,29 @@ static void sh_eth_reset_hw_crc(struct net_device *ndev)
 
 #elif defined(CONFIG_ARCH_R8A7740)
 #define SH_ETH_HAS_TSU	1
+static int sh_eth_check_reset(struct net_device *ndev);
+
 static void sh_eth_chip_reset(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	unsigned long mii;
 
 	/* reset device */
 	sh_eth_tsu_write(mdp, ARSTR_ARSTR, ARSTR);
 	mdelay(1);
 
-	switch (mdp->phy_interface) {
-	case PHY_INTERFACE_MODE_GMII:
-		mii = 2;
-		break;
-	case PHY_INTERFACE_MODE_MII:
-		mii = 1;
-		break;
-	case PHY_INTERFACE_MODE_RMII:
-	default:
-		mii = 0;
-		break;
-	}
-	sh_eth_write(ndev, mii, RMII_MII);
+	sh_eth_select_mii(ndev);
 }
 
-static void sh_eth_reset(struct net_device *ndev)
+static int sh_eth_reset(struct net_device *ndev)
 {
-	int cnt = 100;
+	int ret = 0;
 
 	sh_eth_write(ndev, EDSR_ENALL, EDSR);
 	sh_eth_write(ndev, sh_eth_read(ndev, EDMR) | EDMR_SRST_GETHER, EDMR);
-	while (cnt > 0) {
-		if (!(sh_eth_read(ndev, EDMR) & 0x3))
-			break;
-		mdelay(1);
-		cnt--;
-	}
-	if (cnt == 0)
-		printk(KERN_ERR "Device reset fail\n");
+
+	ret = sh_eth_check_reset(ndev);
+	if (ret)
+		goto out;
 
 	/* Table Init */
 	sh_eth_write(ndev, 0x0, TDLAR);
@@ -436,6 +455,9 @@ static void sh_eth_reset(struct net_device *ndev)
 	sh_eth_write(ndev, 0x0, RDFAR);
 	sh_eth_write(ndev, 0x0, RDFXR);
 	sh_eth_write(ndev, 0x0, RDFFR);
+
+out:
+	return ret;
 }
 
 static void sh_eth_set_duplex(struct net_device *ndev)
@@ -492,6 +514,7 @@ static struct sh_eth_cpu_data sh_eth_my_cpu_data = {
 	.no_trimd	= 1,
 	.no_ade		= 1,
 	.tsu		= 1,
+	.select_mii	= 1,
 };
 
 #elif defined(CONFIG_CPU_SUBTYPE_SH7619)
@@ -543,11 +566,31 @@ static void sh_eth_set_default_cpu_data(struct sh_eth_cpu_data *cd)
 
 #if defined(SH_ETH_RESET_DEFAULT)
 /* Chip Reset */
-static void sh_eth_reset(struct net_device *ndev)
+static int  sh_eth_reset(struct net_device *ndev)
 {
 	sh_eth_write(ndev, sh_eth_read(ndev, EDMR) | EDMR_SRST_ETHER, EDMR);
 	mdelay(3);
 	sh_eth_write(ndev, sh_eth_read(ndev, EDMR) & ~EDMR_SRST_ETHER, EDMR);
+
+	return 0;
+}
+#else
+static int sh_eth_check_reset(struct net_device *ndev)
+{
+	int ret = 0;
+	int cnt = 100;
+
+	while (cnt > 0) {
+		if (!(sh_eth_read(ndev, EDMR) & 0x3))
+			break;
+		mdelay(1);
+		cnt--;
+	}
+	if (cnt < 0) {
+		printk(KERN_ERR "Device reset fail\n");
+		ret = -ETIMEDOUT;
+	}
+	return ret;
 }
 #endif
 
@@ -739,21 +782,23 @@ static void sh_eth_ring_free(struct net_device *ndev)
 
 	/* Free Rx skb ringbuffer */
 	if (mdp->rx_skbuff) {
-		for (i = 0; i < RX_RING_SIZE; i++) {
+		for (i = 0; i < mdp->num_rx_ring; i++) {
 			if (mdp->rx_skbuff[i])
 				dev_kfree_skb(mdp->rx_skbuff[i]);
 		}
 	}
 	kfree(mdp->rx_skbuff);
+	mdp->rx_skbuff = NULL;
 
 	/* Free Tx skb ringbuffer */
 	if (mdp->tx_skbuff) {
-		for (i = 0; i < TX_RING_SIZE; i++) {
+		for (i = 0; i < mdp->num_tx_ring; i++) {
 			if (mdp->tx_skbuff[i])
 				dev_kfree_skb(mdp->tx_skbuff[i]);
 		}
 	}
 	kfree(mdp->tx_skbuff);
+	mdp->tx_skbuff = NULL;
 }
 
 /* format skb and descriptor buffer */
@@ -764,8 +809,8 @@ static void sh_eth_ring_format(struct net_device *ndev)
 	struct sk_buff *skb;
 	struct sh_eth_rxdesc *rxdesc = NULL;
 	struct sh_eth_txdesc *txdesc = NULL;
-	int rx_ringsize = sizeof(*rxdesc) * RX_RING_SIZE;
-	int tx_ringsize = sizeof(*txdesc) * TX_RING_SIZE;
+	int rx_ringsize = sizeof(*rxdesc) * mdp->num_rx_ring;
+	int tx_ringsize = sizeof(*txdesc) * mdp->num_tx_ring;
 
 	mdp->cur_rx = mdp->cur_tx = 0;
 	mdp->dirty_rx = mdp->dirty_tx = 0;
@@ -773,7 +818,7 @@ static void sh_eth_ring_format(struct net_device *ndev)
 	memset(mdp->rx_ring, 0, rx_ringsize);
 
 	/* build Rx ring buffer */
-	for (i = 0; i < RX_RING_SIZE; i++) {
+	for (i = 0; i < mdp->num_rx_ring; i++) {
 		/* skb */
 		mdp->rx_skbuff[i] = NULL;
 		skb = netdev_alloc_skb(ndev, mdp->rx_buf_sz);
@@ -799,7 +844,7 @@ static void sh_eth_ring_format(struct net_device *ndev)
 		}
 	}
 
-	mdp->dirty_rx = (u32) (i - RX_RING_SIZE);
+	mdp->dirty_rx = (u32) (i - mdp->num_rx_ring);
 
 	/* Mark the last entry as wrapping the ring. */
 	rxdesc->status |= cpu_to_edmac(mdp, RD_RDEL);
@@ -807,7 +852,7 @@ static void sh_eth_ring_format(struct net_device *ndev)
 	memset(mdp->tx_ring, 0, tx_ringsize);
 
 	/* build Tx ring buffer */
-	for (i = 0; i < TX_RING_SIZE; i++) {
+	for (i = 0; i < mdp->num_tx_ring; i++) {
 		mdp->tx_skbuff[i] = NULL;
 		txdesc = &mdp->tx_ring[i];
 		txdesc->status = cpu_to_edmac(mdp, TD_TFP);
@@ -841,7 +886,7 @@ static int sh_eth_ring_init(struct net_device *ndev)
 		mdp->rx_buf_sz += NET_IP_ALIGN;
 
 	/* Allocate RX and TX skb rings */
-	mdp->rx_skbuff = kmalloc(sizeof(*mdp->rx_skbuff) * RX_RING_SIZE,
+	mdp->rx_skbuff = kmalloc(sizeof(*mdp->rx_skbuff) * mdp->num_rx_ring,
 				GFP_KERNEL);
 	if (!mdp->rx_skbuff) {
 		dev_err(&ndev->dev, "Cannot allocate Rx skb\n");
@@ -849,7 +894,7 @@ static int sh_eth_ring_init(struct net_device *ndev)
 		return ret;
 	}
 
-	mdp->tx_skbuff = kmalloc(sizeof(*mdp->tx_skbuff) * TX_RING_SIZE,
+	mdp->tx_skbuff = kmalloc(sizeof(*mdp->tx_skbuff) * mdp->num_tx_ring,
 				GFP_KERNEL);
 	if (!mdp->tx_skbuff) {
 		dev_err(&ndev->dev, "Cannot allocate Tx skb\n");
@@ -858,7 +903,7 @@ static int sh_eth_ring_init(struct net_device *ndev)
 	}
 
 	/* Allocate all Rx descriptors. */
-	rx_ringsize = sizeof(struct sh_eth_rxdesc) * RX_RING_SIZE;
+	rx_ringsize = sizeof(struct sh_eth_rxdesc) * mdp->num_rx_ring;
 	mdp->rx_ring = dma_alloc_coherent(NULL, rx_ringsize, &mdp->rx_desc_dma,
 			GFP_KERNEL);
 
@@ -872,7 +917,7 @@ static int sh_eth_ring_init(struct net_device *ndev)
 	mdp->dirty_rx = 0;
 
 	/* Allocate all Tx descriptors. */
-	tx_ringsize = sizeof(struct sh_eth_txdesc) * TX_RING_SIZE;
+	tx_ringsize = sizeof(struct sh_eth_txdesc) * mdp->num_tx_ring;
 	mdp->tx_ring = dma_alloc_coherent(NULL, tx_ringsize, &mdp->tx_desc_dma,
 			GFP_KERNEL);
 	if (!mdp->tx_ring) {
@@ -890,19 +935,41 @@ desc_ring_free:
 skb_ring_free:
 	/* Free Rx and Tx skb ring buffer */
 	sh_eth_ring_free(ndev);
+	mdp->tx_ring = NULL;
+	mdp->rx_ring = NULL;
 
 	return ret;
 }
 
-static int sh_eth_dev_init(struct net_device *ndev)
+static void sh_eth_free_dma_buffer(struct sh_eth_private *mdp)
+{
+	int ringsize;
+
+	if (mdp->rx_ring) {
+		ringsize = sizeof(struct sh_eth_rxdesc) * mdp->num_rx_ring;
+		dma_free_coherent(NULL, ringsize, mdp->rx_ring,
+				  mdp->rx_desc_dma);
+		mdp->rx_ring = NULL;
+	}
+
+	if (mdp->tx_ring) {
+		ringsize = sizeof(struct sh_eth_txdesc) * mdp->num_tx_ring;
+		dma_free_coherent(NULL, ringsize, mdp->tx_ring,
+				  mdp->tx_desc_dma);
+		mdp->tx_ring = NULL;
+	}
+}
+
+static int sh_eth_dev_init(struct net_device *ndev, bool start)
 {
 	int ret = 0;
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	u_int32_t rx_int_var, tx_int_var;
 	u32 val;
 
 	/* Soft Reset */
-	sh_eth_reset(ndev);
+	ret = sh_eth_reset(ndev);
+	if (ret)
+		goto out;
 
 	/* Descriptor format */
 	sh_eth_ring_format(ndev);
@@ -926,9 +993,7 @@ static int sh_eth_dev_init(struct net_device *ndev)
 	/* Frame recv control */
 	sh_eth_write(ndev, mdp->cd->rmcr_value, RMCR);
 
-	rx_int_var = mdp->rx_int_var = DESC_I_RINT8 | DESC_I_RINT5;
-	tx_int_var = mdp->tx_int_var = DESC_I_TINT2;
-	sh_eth_write(ndev, rx_int_var | tx_int_var, TRSCER);
+	sh_eth_write(ndev, DESC_I_RINT8 | DESC_I_RINT5 | DESC_I_TINT2, TRSCER);
 
 	if (mdp->cd->bculr)
 		sh_eth_write(ndev, 0x800, BCULR);	/* Burst sycle set */
@@ -943,7 +1008,8 @@ static int sh_eth_dev_init(struct net_device *ndev)
 		     RFLR);
 
 	sh_eth_write(ndev, sh_eth_read(ndev, EESR), EESR);
-	sh_eth_write(ndev, mdp->cd->eesipr_value, EESIPR);
+	if (start)
+		sh_eth_write(ndev, mdp->cd->eesipr_value, EESIPR);
 
 	/* PAUSE Prohibition */
 	val = (sh_eth_read(ndev, ECMR) & ECMR_DM) |
@@ -958,7 +1024,8 @@ static int sh_eth_dev_init(struct net_device *ndev)
 	sh_eth_write(ndev, mdp->cd->ecsr_value, ECSR);
 
 	/* E-MAC Interrupt Enable register */
-	sh_eth_write(ndev, mdp->cd->ecsipr_value, ECSIPR);
+	if (start)
+		sh_eth_write(ndev, mdp->cd->ecsipr_value, ECSIPR);
 
 	/* Set MAC address */
 	update_mac_address(ndev);
@@ -971,11 +1038,14 @@ static int sh_eth_dev_init(struct net_device *ndev)
 	if (mdp->cd->tpauser)
 		sh_eth_write(ndev, TPAUSER_UNLIMITED, TPAUSER);
 
-	/* Setting the Rx mode will start the Rx process. */
-	sh_eth_write(ndev, EDRRR_R, EDRRR);
+	if (start) {
+		/* Setting the Rx mode will start the Rx process. */
+		sh_eth_write(ndev, EDRRR_R, EDRRR);
 
-	netif_start_queue(ndev);
+		netif_start_queue(ndev);
+	}
 
+out:
 	return ret;
 }
 
@@ -988,7 +1058,7 @@ static int sh_eth_txfree(struct net_device *ndev)
 	int entry = 0;
 
 	for (; mdp->cur_tx - mdp->dirty_tx > 0; mdp->dirty_tx++) {
-		entry = mdp->dirty_tx % TX_RING_SIZE;
+		entry = mdp->dirty_tx % mdp->num_tx_ring;
 		txdesc = &mdp->tx_ring[entry];
 		if (txdesc->status & cpu_to_edmac(mdp, TD_TACT))
 			break;
@@ -1001,7 +1071,7 @@ static int sh_eth_txfree(struct net_device *ndev)
 			freeNum++;
 		}
 		txdesc->status = cpu_to_edmac(mdp, TD_TFP);
-		if (entry >= TX_RING_SIZE - 1)
+		if (entry >= mdp->num_tx_ring - 1)
 			txdesc->status |= cpu_to_edmac(mdp, TD_TDLE);
 
 		ndev->stats.tx_packets++;
@@ -1016,8 +1086,8 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status)
 	struct sh_eth_private *mdp = netdev_priv(ndev);
 	struct sh_eth_rxdesc *rxdesc;
 
-	int entry = mdp->cur_rx % RX_RING_SIZE;
-	int boguscnt = (mdp->dirty_rx + RX_RING_SIZE) - mdp->cur_rx;
+	int entry = mdp->cur_rx % mdp->num_rx_ring;
+	int boguscnt = (mdp->dirty_rx + mdp->num_rx_ring) - mdp->cur_rx;
 	struct sk_buff *skb;
 	u16 pkt_len = 0;
 	u32 desc_status;
@@ -1068,13 +1138,13 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status)
 			ndev->stats.rx_bytes += pkt_len;
 		}
 		rxdesc->status |= cpu_to_edmac(mdp, RD_RACT);
-		entry = (++mdp->cur_rx) % RX_RING_SIZE;
+		entry = (++mdp->cur_rx) % mdp->num_rx_ring;
 		rxdesc = &mdp->rx_ring[entry];
 	}
 
 	/* Refill the Rx ring buffers. */
 	for (; mdp->cur_rx - mdp->dirty_rx > 0; mdp->dirty_rx++) {
-		entry = mdp->dirty_rx % RX_RING_SIZE;
+		entry = mdp->dirty_rx % mdp->num_rx_ring;
 		rxdesc = &mdp->rx_ring[entry];
 		/* The size of the buffer is 16 byte boundary. */
 		rxdesc->buffer_length = ALIGN(mdp->rx_buf_sz, 16);
@@ -1091,7 +1161,7 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status)
 			skb_checksum_none_assert(skb);
 			rxdesc->addr = virt_to_phys(PTR_ALIGN(skb->data, 4));
 		}
-		if (entry >= RX_RING_SIZE - 1)
+		if (entry >= mdp->num_rx_ring - 1)
 			rxdesc->status |=
 				cpu_to_edmac(mdp, RD_RACT | RD_RFP | RD_RDEL);
 		else
@@ -1293,14 +1363,6 @@ other_irq:
 	return ret;
 }
 
-static void sh_eth_timer(unsigned long data)
-{
-	struct net_device *ndev = (struct net_device *)data;
-	struct sh_eth_private *mdp = netdev_priv(ndev);
-
-	mod_timer(&mdp->timer, jiffies + (10 * HZ));
-}
-
 /* PHY state control function */
 static void sh_eth_adjust_link(struct net_device *ndev)
 {
@@ -1499,6 +1561,71 @@ static void sh_eth_get_strings(struct net_device *ndev, u32 stringset, u8 *data)
 	}
 }
 
+static void sh_eth_get_ringparam(struct net_device *ndev,
+				 struct ethtool_ringparam *ring)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	ring->rx_max_pending = RX_RING_MAX;
+	ring->tx_max_pending = TX_RING_MAX;
+	ring->rx_pending = mdp->num_rx_ring;
+	ring->tx_pending = mdp->num_tx_ring;
+}
+
+static int sh_eth_set_ringparam(struct net_device *ndev,
+				struct ethtool_ringparam *ring)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+	int ret;
+
+	if (ring->tx_pending > TX_RING_MAX ||
+	    ring->rx_pending > RX_RING_MAX ||
+	    ring->tx_pending < TX_RING_MIN ||
+	    ring->rx_pending < RX_RING_MIN)
+		return -EINVAL;
+	if (ring->rx_mini_pending || ring->rx_jumbo_pending)
+		return -EINVAL;
+
+	if (netif_running(ndev)) {
+		netif_tx_disable(ndev);
+		/* Disable interrupts by clearing the interrupt mask. */
+		sh_eth_write(ndev, 0x0000, EESIPR);
+		/* Stop the chip's Tx and Rx processes. */
+		sh_eth_write(ndev, 0, EDTRR);
+		sh_eth_write(ndev, 0, EDRRR);
+		synchronize_irq(ndev->irq);
+	}
+
+	/* Free all the skbuffs in the Rx queue. */
+	sh_eth_ring_free(ndev);
+	/* Free DMA buffer */
+	sh_eth_free_dma_buffer(mdp);
+
+	/* Set new parameters */
+	mdp->num_rx_ring = ring->rx_pending;
+	mdp->num_tx_ring = ring->tx_pending;
+
+	ret = sh_eth_ring_init(ndev);
+	if (ret < 0) {
+		dev_err(&ndev->dev, "%s: sh_eth_ring_init failed.\n", __func__);
+		return ret;
+	}
+	ret = sh_eth_dev_init(ndev, false);
+	if (ret < 0) {
+		dev_err(&ndev->dev, "%s: sh_eth_dev_init failed.\n", __func__);
+		return ret;
+	}
+
+	if (netif_running(ndev)) {
+		sh_eth_write(ndev, mdp->cd->eesipr_value, EESIPR);
+		/* Setting the Rx mode will start the Rx process. */
+		sh_eth_write(ndev, EDRRR_R, EDRRR);
+		netif_wake_queue(ndev);
+	}
+
+	return 0;
+}
+
 static const struct ethtool_ops sh_eth_ethtool_ops = {
 	.get_settings	= sh_eth_get_settings,
 	.set_settings	= sh_eth_set_settings,
@@ -1509,6 +1636,8 @@ static const struct ethtool_ops sh_eth_ethtool_ops = {
 	.get_strings	= sh_eth_get_strings,
 	.get_ethtool_stats  = sh_eth_get_ethtool_stats,
 	.get_sset_count     = sh_eth_get_sset_count,
+	.get_ringparam	= sh_eth_get_ringparam,
+	.set_ringparam	= sh_eth_set_ringparam,
 };
 
 /* network device open function */
@@ -1539,7 +1668,7 @@ static int sh_eth_open(struct net_device *ndev)
 		goto out_free_irq;
 
 	/* device init */
-	ret = sh_eth_dev_init(ndev);
+	ret = sh_eth_dev_init(ndev, true);
 	if (ret)
 		goto out_free_irq;
 
@@ -1548,11 +1677,6 @@ static int sh_eth_open(struct net_device *ndev)
 	if (ret)
 		goto out_free_irq;
 
-	/* Set the timer to check for link beat. */
-	init_timer(&mdp->timer);
-	mdp->timer.expires = (jiffies + (24 * HZ)) / 10;/* 2.4 sec. */
-	setup_timer(&mdp->timer, sh_eth_timer, (unsigned long)ndev);
-
 	return ret;
 
 out_free_irq:
@@ -1577,11 +1701,8 @@ static void sh_eth_tx_timeout(struct net_device *ndev)
 	/* tx_errors count up */
 	ndev->stats.tx_errors++;
 
-	/* timer off */
-	del_timer_sync(&mdp->timer);
-
 	/* Free all the skbuffs in the Rx queue. */
-	for (i = 0; i < RX_RING_SIZE; i++) {
+	for (i = 0; i < mdp->num_rx_ring; i++) {
 		rxdesc = &mdp->rx_ring[i];
 		rxdesc->status = 0;
 		rxdesc->addr = 0xBADF00D0;
@@ -1589,18 +1710,14 @@ static void sh_eth_tx_timeout(struct net_device *ndev)
 			dev_kfree_skb(mdp->rx_skbuff[i]);
 		mdp->rx_skbuff[i] = NULL;
 	}
-	for (i = 0; i < TX_RING_SIZE; i++) {
+	for (i = 0; i < mdp->num_tx_ring; i++) {
 		if (mdp->tx_skbuff[i])
 			dev_kfree_skb(mdp->tx_skbuff[i]);
 		mdp->tx_skbuff[i] = NULL;
 	}
 
 	/* device init */
-	sh_eth_dev_init(ndev);
-
-	/* timer on */
-	mdp->timer.expires = (jiffies + (24 * HZ)) / 10;/* 2.4 sec. */
-	add_timer(&mdp->timer);
+	sh_eth_dev_init(ndev, true);
 }
 
 /* Packet transmit function */
@@ -1612,7 +1729,7 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	unsigned long flags;
 
 	spin_lock_irqsave(&mdp->lock, flags);
-	if ((mdp->cur_tx - mdp->dirty_tx) >= (TX_RING_SIZE - 4)) {
+	if ((mdp->cur_tx - mdp->dirty_tx) >= (mdp->num_tx_ring - 4)) {
 		if (!sh_eth_txfree(ndev)) {
 			if (netif_msg_tx_queued(mdp))
 				dev_warn(&ndev->dev, "TxFD exhausted.\n");
@@ -1623,7 +1740,7 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	}
 	spin_unlock_irqrestore(&mdp->lock, flags);
 
-	entry = mdp->cur_tx % TX_RING_SIZE;
+	entry = mdp->cur_tx % mdp->num_tx_ring;
 	mdp->tx_skbuff[entry] = skb;
 	txdesc = &mdp->tx_ring[entry];
 	/* soft swap. */
@@ -1637,7 +1754,7 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	else
 		txdesc->buffer_length = skb->len;
 
-	if (entry >= TX_RING_SIZE - 1)
+	if (entry >= mdp->num_tx_ring - 1)
 		txdesc->status |= cpu_to_edmac(mdp, TD_TACT | TD_TDLE);
 	else
 		txdesc->status |= cpu_to_edmac(mdp, TD_TACT);
@@ -1654,7 +1771,6 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 static int sh_eth_close(struct net_device *ndev)
 {
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	int ringsize;
 
 	netif_stop_queue(ndev);
 
@@ -1673,18 +1789,11 @@ static int sh_eth_close(struct net_device *ndev)
 
 	free_irq(ndev->irq, ndev);
 
-	del_timer_sync(&mdp->timer);
-
 	/* Free all the skbuffs in the Rx queue. */
 	sh_eth_ring_free(ndev);
 
 	/* free DMA buffer */
-	ringsize = sizeof(struct sh_eth_rxdesc) * RX_RING_SIZE;
-	dma_free_coherent(NULL, ringsize, mdp->rx_ring, mdp->rx_desc_dma);
-
-	/* free DMA buffer */
-	ringsize = sizeof(struct sh_eth_txdesc) * TX_RING_SIZE;
-	dma_free_coherent(NULL, ringsize, mdp->tx_ring, mdp->tx_desc_dma);
+	sh_eth_free_dma_buffer(mdp);
 
 	pm_runtime_put_sync(&mdp->pdev->dev);
 
@@ -2275,6 +2384,8 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	ether_setup(ndev);
 
 	mdp = netdev_priv(ndev);
+	mdp->num_tx_ring = TX_RING_SIZE;
+	mdp->num_rx_ring = RX_RING_SIZE;
 	mdp->addr = ioremap(res->start, resource_size(res));
 	if (mdp->addr == NULL) {
 		ret = -ENOMEM;
@@ -2312,8 +2423,6 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 
 	/* 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);
 
 	/* read and set MAC address */
 	read_mac_address(ndev, pd->mac_addr);
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index 57b8e1fc5d1..bae84fd2e73 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -27,6 +27,10 @@
 #define TX_TIMEOUT	(5*HZ)
 #define TX_RING_SIZE	64	/* Tx ring size */
 #define RX_RING_SIZE	64	/* Rx ring size */
+#define TX_RING_MIN	64
+#define RX_RING_MIN	64
+#define TX_RING_MAX	1024
+#define RX_RING_MAX	1024
 #define ETHERSMALL		60
 #define PKT_BUF_SZ		1538
 #define SH_ETH_TSU_TIMEOUT_MS	500
@@ -585,71 +589,6 @@ enum RPADIR_BIT {
 /* FDR */
 #define DEFAULT_FDR_INIT	0x00000707
 
-enum phy_offsets {
-	PHY_CTRL = 0, PHY_STAT = 1, PHY_IDT1 = 2, PHY_IDT2 = 3,
-	PHY_ANA = 4, PHY_ANL = 5, PHY_ANE = 6,
-	PHY_16 = 16,
-};
-
-/* PHY_CTRL */
-enum PHY_CTRL_BIT {
-	PHY_C_RESET = 0x8000, PHY_C_LOOPBK = 0x4000, PHY_C_SPEEDSL = 0x2000,
-	PHY_C_ANEGEN = 0x1000, PHY_C_PWRDN = 0x0800, PHY_C_ISO = 0x0400,
-	PHY_C_RANEG = 0x0200, PHY_C_DUPLEX = 0x0100, PHY_C_COLT = 0x0080,
-};
-#define DM9161_PHY_C_ANEGEN 0	/* auto nego special */
-
-/* PHY_STAT */
-enum PHY_STAT_BIT {
-	PHY_S_100T4 = 0x8000, PHY_S_100X_F = 0x4000, PHY_S_100X_H = 0x2000,
-	PHY_S_10T_F = 0x1000, PHY_S_10T_H = 0x0800, PHY_S_ANEGC = 0x0020,
-	PHY_S_RFAULT = 0x0010, PHY_S_ANEGA = 0x0008, PHY_S_LINK = 0x0004,
-	PHY_S_JAB = 0x0002, PHY_S_EXTD = 0x0001,
-};
-
-/* PHY_ANA */
-enum PHY_ANA_BIT {
-	PHY_A_NP = 0x8000, PHY_A_ACK = 0x4000, PHY_A_RF = 0x2000,
-	PHY_A_FCS = 0x0400, PHY_A_T4 = 0x0200, PHY_A_FDX = 0x0100,
-	PHY_A_HDX = 0x0080, PHY_A_10FDX = 0x0040, PHY_A_10HDX = 0x0020,
-	PHY_A_SEL = 0x001e,
-};
-/* PHY_ANL */
-enum PHY_ANL_BIT {
-	PHY_L_NP = 0x8000, PHY_L_ACK = 0x4000, PHY_L_RF = 0x2000,
-	PHY_L_FCS = 0x0400, PHY_L_T4 = 0x0200, PHY_L_FDX = 0x0100,
-	PHY_L_HDX = 0x0080, PHY_L_10FDX = 0x0040, PHY_L_10HDX = 0x0020,
-	PHY_L_SEL = 0x001f,
-};
-
-/* PHY_ANE */
-enum PHY_ANE_BIT {
-	PHY_E_PDF = 0x0010, PHY_E_LPNPA = 0x0008, PHY_E_NPA = 0x0004,
-	PHY_E_PRX = 0x0002, PHY_E_LPANEGA = 0x0001,
-};
-
-/* DM9161 */
-enum PHY_16_BIT {
-	PHY_16_BP4B45 = 0x8000, PHY_16_BPSCR = 0x4000, PHY_16_BPALIGN = 0x2000,
-	PHY_16_BP_ADPOK = 0x1000, PHY_16_Repeatmode = 0x0800,
-	PHY_16_TXselect = 0x0400,
-	PHY_16_Rsvd = 0x0200, PHY_16_RMIIEnable = 0x0100,
-	PHY_16_Force100LNK = 0x0080,
-	PHY_16_APDLED_CTL = 0x0040, PHY_16_COLLED_CTL = 0x0020,
-	PHY_16_RPDCTR_EN = 0x0010,
-	PHY_16_ResetStMch = 0x0008, PHY_16_PreamSupr = 0x0004,
-	PHY_16_Sleepmode = 0x0002,
-	PHY_16_RemoteLoopOut = 0x0001,
-};
-
-#define POST_RX		0x08
-#define POST_FW		0x04
-#define POST0_RX	(POST_RX)
-#define POST0_FW	(POST_FW)
-#define POST1_RX	(POST_RX >> 2)
-#define POST1_FW	(POST_FW >> 2)
-#define POST_ALL	(POST0_RX | POST0_FW | POST1_RX | POST1_FW)
-
 /* ARSTR */
 enum ARSTR_BIT { ARSTR_ARSTR = 0x00000001, };
 
@@ -757,6 +696,7 @@ struct sh_eth_cpu_data {
 	unsigned no_trimd:1;		/* E-DMAC DO NOT have TRIMD */
 	unsigned no_ade:1;	/* E-DMAC DO NOT have ADE bit in EESR */
 	unsigned hw_crc:1;	/* E-DMAC have CSMR */
+	unsigned select_mii:1;	/* EtherC have RMII_MII (MII select register) */
 };
 
 struct sh_eth_private {
@@ -765,13 +705,14 @@ struct sh_eth_private {
 	const u16 *reg_offset;
 	void __iomem *addr;
 	void __iomem *tsu_addr;
+	u32 num_rx_ring;
+	u32 num_tx_ring;
 	dma_addr_t rx_desc_dma;
 	dma_addr_t tx_desc_dma;
 	struct sh_eth_rxdesc *rx_ring;
 	struct sh_eth_txdesc *tx_ring;
 	struct sk_buff **rx_skbuff;
 	struct sk_buff **tx_skbuff;
-	struct timer_list timer;
 	spinlock_t lock;
 	u32 cur_rx, dirty_rx;	/* Producer/consumer ring indices */
 	u32 cur_tx, dirty_tx;
@@ -786,10 +727,6 @@ struct sh_eth_private {
 	int msg_enable;
 	int speed;
 	int duplex;
-	u32 rx_int_var, tx_int_var;	/* interrupt control variables */
-	char post_rx;		/* POST receive */
-	char post_fw;		/* POST forward */
-	struct net_device_stats tsu_stats;	/* TSU forward status */
 	int port;		/* for TSU */
 	int vlan_num_ids;	/* for VLAN tag filter */
 
diff --git a/drivers/net/ethernet/seeq/seeq8005.c b/drivers/net/ethernet/seeq/seeq8005.c
index 698edbbfc14..d6e50de7118 100644
--- a/drivers/net/ethernet/seeq/seeq8005.c
+++ b/drivers/net/ethernet/seeq/seeq8005.c
@@ -736,9 +736,7 @@ MODULE_PARM_DESC(irq, "SEEQ 8005 IRQ number");
 int __init init_module(void)
 {
 	dev_seeq = seeq8005_probe(-1);
-	if (IS_ERR(dev_seeq))
-		return PTR_ERR(dev_seeq);
-	return 0;
+	return PTR_RET(dev_seeq);
 }
 
 void __exit cleanup_module(void)
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index b95f2e1b33f..70554a1b2b0 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -1103,8 +1103,8 @@ static int efx_init_io(struct efx_nic *efx)
 	 * masks event though they reject 46 bit masks.
 	 */
 	while (dma_mask > 0x7fffffffUL) {
-		if (pci_dma_supported(pci_dev, dma_mask)) {
-			rc = pci_set_dma_mask(pci_dev, dma_mask);
+		if (dma_supported(&pci_dev->dev, dma_mask)) {
+			rc = dma_set_mask(&pci_dev->dev, dma_mask);
 			if (rc == 0)
 				break;
 		}
@@ -1117,10 +1117,10 @@ static int efx_init_io(struct efx_nic *efx)
 	}
 	netif_dbg(efx, probe, efx->net_dev,
 		  "using DMA mask %llx\n", (unsigned long long) dma_mask);
-	rc = pci_set_consistent_dma_mask(pci_dev, dma_mask);
+	rc = dma_set_coherent_mask(&pci_dev->dev, dma_mask);
 	if (rc) {
-		/* pci_set_consistent_dma_mask() is not *allowed* to
-		 * fail with a mask that pci_set_dma_mask() accepted,
+		/* dma_set_coherent_mask() is not *allowed* to
+		 * fail with a mask that dma_set_mask() accepted,
 		 * but just in case...
 		 */
 		netif_err(efx, probe, efx->net_dev,
diff --git a/drivers/net/ethernet/sfc/enum.h b/drivers/net/ethernet/sfc/enum.h
index d725a8fbe1a..182dbe2cc6e 100644
--- a/drivers/net/ethernet/sfc/enum.h
+++ b/drivers/net/ethernet/sfc/enum.h
@@ -136,10 +136,10 @@ enum efx_loopback_mode {
  *
  * Reset methods are numbered in order of increasing scope.
  *
- * @RESET_TYPE_INVISIBLE: don't reset the PHYs or interrupts
- * @RESET_TYPE_ALL: reset everything but PCI core blocks
- * @RESET_TYPE_WORLD: reset everything, save & restore PCI config
- * @RESET_TYPE_DISABLE: disable NIC
+ * @RESET_TYPE_INVISIBLE: Reset datapath and MAC (Falcon only)
+ * @RESET_TYPE_ALL: Reset datapath, MAC and PHY
+ * @RESET_TYPE_WORLD: Reset as much as possible
+ * @RESET_TYPE_DISABLE: Reset datapath, MAC and PHY; leave NIC disabled
  * @RESET_TYPE_TX_WATCHDOG: reset due to TX watchdog
  * @RESET_TYPE_INT_ERROR: reset due to internal error
  * @RESET_TYPE_RX_RECOVERY: reset to recover from RX datapath errors
diff --git a/drivers/net/ethernet/sfc/ethtool.c b/drivers/net/ethernet/sfc/ethtool.c
index 03ded364c8d..10536f93b56 100644
--- a/drivers/net/ethernet/sfc/ethtool.c
+++ b/drivers/net/ethernet/sfc/ethtool.c
@@ -453,7 +453,7 @@ static void efx_ethtool_get_strings(struct net_device *net_dev,
 	switch (string_set) {
 	case ETH_SS_STATS:
 		for (i = 0; i < EFX_ETHTOOL_NUM_STATS; i++)
-			strncpy(ethtool_strings[i].name,
+			strlcpy(ethtool_strings[i].name,
 				efx_ethtool_stats[i].name,
 				sizeof(ethtool_strings[i].name));
 		break;
diff --git a/drivers/net/ethernet/sfc/falcon.c b/drivers/net/ethernet/sfc/falcon.c
index 3a1ca2bd154..12b573a8e82 100644
--- a/drivers/net/ethernet/sfc/falcon.c
+++ b/drivers/net/ethernet/sfc/falcon.c
@@ -25,9 +25,12 @@
 #include "io.h"
 #include "phy.h"
 #include "workarounds.h"
+#include "selftest.h"
 
 /* Hardware control for SFC4000 (aka Falcon). */
 
+static int falcon_reset_hw(struct efx_nic *efx, enum reset_type method);
+
 static const unsigned int
 /* "Large" EEPROM device: Atmel AT25640 or similar
  * 8 KB, 16-bit address, 32 B write block */
@@ -1034,10 +1037,34 @@ static const struct efx_nic_register_test falcon_b0_register_tests[] = {
 	  EFX_OWORD32(0x0003FF0F, 0x00000000, 0x00000000, 0x00000000) },
 };
 
-static int falcon_b0_test_registers(struct efx_nic *efx)
+static int
+falcon_b0_test_chip(struct efx_nic *efx, struct efx_self_tests *tests)
 {
-	return efx_nic_test_registers(efx, falcon_b0_register_tests,
-				      ARRAY_SIZE(falcon_b0_register_tests));
+	enum reset_type reset_method = RESET_TYPE_INVISIBLE;
+	int rc, rc2;
+
+	mutex_lock(&efx->mac_lock);
+	if (efx->loopback_modes) {
+		/* We need the 312 clock from the PHY to test the XMAC
+		 * registers, so move into XGMII loopback if available */
+		if (efx->loopback_modes & (1 << LOOPBACK_XGMII))
+			efx->loopback_mode = LOOPBACK_XGMII;
+		else
+			efx->loopback_mode = __ffs(efx->loopback_modes);
+	}
+	__efx_reconfigure_port(efx);
+	mutex_unlock(&efx->mac_lock);
+
+	efx_reset_down(efx, reset_method);
+
+	tests->registers =
+		efx_nic_test_registers(efx, falcon_b0_register_tests,
+				       ARRAY_SIZE(falcon_b0_register_tests))
+		? -1 : 1;
+
+	rc = falcon_reset_hw(efx, reset_method);
+	rc2 = efx_reset_up(efx, reset_method, rc == 0);
+	return rc ? rc : rc2;
 }
 
 /**************************************************************************
@@ -1818,7 +1845,7 @@ const struct efx_nic_type falcon_b0_nic_type = {
 	.get_wol = falcon_get_wol,
 	.set_wol = falcon_set_wol,
 	.resume_wol = efx_port_dummy_op_void,
-	.test_registers = falcon_b0_test_registers,
+	.test_chip = falcon_b0_test_chip,
 	.test_nvram = falcon_test_nvram,
 
 	.revision = EFX_REV_FALCON_B0,
diff --git a/drivers/net/ethernet/sfc/falcon_xmac.c b/drivers/net/ethernet/sfc/falcon_xmac.c
index 6106ef15dee..8333865d4c9 100644
--- a/drivers/net/ethernet/sfc/falcon_xmac.c
+++ b/drivers/net/ethernet/sfc/falcon_xmac.c
@@ -341,12 +341,12 @@ void falcon_update_stats_xmac(struct efx_nic *efx)
 	FALCON_STAT(efx, XgTxIpSrcErrPkt, tx_ip_src_error);
 
 	/* Update derived statistics */
-	mac_stats->tx_good_bytes =
-		(mac_stats->tx_bytes - mac_stats->tx_bad_bytes -
-		 mac_stats->tx_control * 64);
-	mac_stats->rx_bad_bytes =
-		(mac_stats->rx_bytes - mac_stats->rx_good_bytes -
-		 mac_stats->rx_control * 64);
+	efx_update_diff_stat(&mac_stats->tx_good_bytes,
+			     mac_stats->tx_bytes - mac_stats->tx_bad_bytes -
+			     mac_stats->tx_control * 64);
+	efx_update_diff_stat(&mac_stats->rx_bad_bytes,
+			     mac_stats->rx_bytes - mac_stats->rx_good_bytes -
+			     mac_stats->rx_control * 64);
 }
 
 void falcon_poll_xmac(struct efx_nic *efx)
diff --git a/drivers/net/ethernet/sfc/filter.c b/drivers/net/ethernet/sfc/filter.c
index fea7f730067..c3fd61f0a95 100644
--- a/drivers/net/ethernet/sfc/filter.c
+++ b/drivers/net/ethernet/sfc/filter.c
@@ -662,7 +662,7 @@ s32 efx_filter_insert_filter(struct efx_nic *efx, struct efx_filter_spec *spec,
 	struct efx_filter_table *table = efx_filter_spec_table(state, spec);
 	struct efx_filter_spec *saved_spec;
 	efx_oword_t filter;
-	unsigned int filter_idx, depth;
+	unsigned int filter_idx, depth = 0;
 	u32 key;
 	int rc;
 
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index 17b6463e459..fc5e7bbcbc9 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -1001,12 +1001,17 @@ static void efx_mcdi_exit_assertion(struct efx_nic *efx)
 {
 	u8 inbuf[MC_CMD_REBOOT_IN_LEN];
 
-	/* Atomically reboot the mcfw out of the assertion handler */
+	/* If the MC is running debug firmware, it might now be
+	 * waiting for a debugger to attach, but we just want it to
+	 * reboot.  We set a flag that makes the command a no-op if it
+	 * has already done so.  We don't know what return code to
+	 * expect (0 or -EIO), so ignore it.
+	 */
 	BUILD_BUG_ON(MC_CMD_REBOOT_OUT_LEN != 0);
 	MCDI_SET_DWORD(inbuf, REBOOT_IN_FLAGS,
 		       MC_CMD_REBOOT_FLAGS_AFTER_ASSERTION);
-	efx_mcdi_rpc(efx, MC_CMD_REBOOT, inbuf, MC_CMD_REBOOT_IN_LEN,
-		     NULL, 0, NULL);
+	(void) efx_mcdi_rpc(efx, MC_CMD_REBOOT, inbuf, MC_CMD_REBOOT_IN_LEN,
+			    NULL, 0, NULL);
 }
 
 int efx_mcdi_handle_assertion(struct efx_nic *efx)
diff --git a/drivers/net/ethernet/sfc/mcdi_mon.c b/drivers/net/ethernet/sfc/mcdi_mon.c
index fb7f65b59eb..1d552f0664d 100644
--- a/drivers/net/ethernet/sfc/mcdi_mon.c
+++ b/drivers/net/ethernet/sfc/mcdi_mon.c
@@ -222,6 +222,7 @@ efx_mcdi_mon_add_attr(struct efx_nic *efx, const char *name,
 	attr->index = index;
 	attr->type = type;
 	attr->limit_value = limit_value;
+	sysfs_attr_init(&attr->dev_attr.attr);
 	attr->dev_attr.attr.name = attr->name;
 	attr->dev_attr.attr.mode = S_IRUGO;
 	attr->dev_attr.show = reader;
diff --git a/drivers/net/ethernet/sfc/mcdi_pcol.h b/drivers/net/ethernet/sfc/mcdi_pcol.h
index 0310b9f08c9..db4beed9766 100644
--- a/drivers/net/ethernet/sfc/mcdi_pcol.h
+++ b/drivers/net/ethernet/sfc/mcdi_pcol.h
@@ -48,8 +48,7 @@
 
 /* Unused commands: 0x23, 0x27, 0x30, 0x31 */
 
-/**
- * MCDI version 1
+/* MCDI version 1
  *
  * Each MCDI request starts with an MCDI_HEADER, which is a 32byte
  * structure, filled in by the client.
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index 0e575359af1..cd9c0a98969 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -68,6 +68,8 @@
 #define EFX_TXQ_TYPES		4
 #define EFX_MAX_TX_QUEUES	(EFX_TXQ_TYPES * EFX_MAX_CHANNELS)
 
+struct efx_self_tests;
+
 /**
  * struct efx_special_buffer - An Efx special buffer
  * @addr: CPU base address of the buffer
@@ -100,7 +102,7 @@ struct efx_special_buffer {
  * @len: Length of this fragment.
  *	This field is zero when the queue slot is empty.
  * @continuation: True if this fragment is not the end of a packet.
- * @unmap_single: True if pci_unmap_single should be used.
+ * @unmap_single: True if dma_unmap_single should be used.
  * @unmap_len: Length of this fragment to unmap
  */
 struct efx_tx_buffer {
@@ -527,7 +529,7 @@ struct efx_phy_operations {
 };
 
 /**
- * @enum efx_phy_mode - PHY operating mode flags
+ * enum efx_phy_mode - PHY operating mode flags
  * @PHY_MODE_NORMAL: on and should pass traffic
  * @PHY_MODE_TX_DISABLED: on with TX disabled
  * @PHY_MODE_LOW_POWER: set to low power through MDIO
@@ -901,7 +903,8 @@ static inline unsigned int efx_port_num(struct efx_nic *efx)
  * @get_wol: Get WoL configuration from driver state
  * @set_wol: Push WoL configuration to the NIC
  * @resume_wol: Synchronise WoL state between driver and MC (e.g. after resume)
- * @test_registers: Test read/write functionality of control registers
+ * @test_chip: Test registers.  Should use efx_nic_test_registers(), and is
+ *	expected to reset the NIC.
  * @test_nvram: Test validity of NVRAM contents
  * @revision: Hardware architecture revision
  * @mem_map_size: Memory BAR mapped size
@@ -946,7 +949,7 @@ struct efx_nic_type {
 	void (*get_wol)(struct efx_nic *efx, struct ethtool_wolinfo *wol);
 	int (*set_wol)(struct efx_nic *efx, u32 type);
 	void (*resume_wol)(struct efx_nic *efx);
-	int (*test_registers)(struct efx_nic *efx);
+	int (*test_chip)(struct efx_nic *efx, struct efx_self_tests *tests);
 	int (*test_nvram)(struct efx_nic *efx);
 
 	int revision;
diff --git a/drivers/net/ethernet/sfc/nic.c b/drivers/net/ethernet/sfc/nic.c
index 4a9a5beec8f..326d799762d 100644
--- a/drivers/net/ethernet/sfc/nic.c
+++ b/drivers/net/ethernet/sfc/nic.c
@@ -124,9 +124,6 @@ int efx_nic_test_registers(struct efx_nic *efx,
 	unsigned address = 0, i, j;
 	efx_oword_t mask, imask, original, reg, buf;
 
-	/* Falcon should be in loopback to isolate the XMAC from the PHY */
-	WARN_ON(!LOOPBACK_INTERNAL(efx));
-
 	for (i = 0; i < n_regs; ++i) {
 		address = regs[i].address;
 		mask = imask = regs[i].mask;
@@ -308,8 +305,8 @@ efx_free_special_buffer(struct efx_nic *efx, struct efx_special_buffer *buffer)
 int efx_nic_alloc_buffer(struct efx_nic *efx, struct efx_buffer *buffer,
 			 unsigned int len)
 {
-	buffer->addr = pci_alloc_consistent(efx->pci_dev, len,
-					    &buffer->dma_addr);
+	buffer->addr = dma_alloc_coherent(&efx->pci_dev->dev, len,
+					  &buffer->dma_addr, GFP_ATOMIC);
 	if (!buffer->addr)
 		return -ENOMEM;
 	buffer->len = len;
@@ -320,8 +317,8 @@ int efx_nic_alloc_buffer(struct efx_nic *efx, struct efx_buffer *buffer,
 void efx_nic_free_buffer(struct efx_nic *efx, struct efx_buffer *buffer)
 {
 	if (buffer->addr) {
-		pci_free_consistent(efx->pci_dev, buffer->len,
-				    buffer->addr, buffer->dma_addr);
+		dma_free_coherent(&efx->pci_dev->dev, buffer->len,
+				  buffer->addr, buffer->dma_addr);
 		buffer->addr = NULL;
 	}
 }
diff --git a/drivers/net/ethernet/sfc/nic.h b/drivers/net/ethernet/sfc/nic.h
index f48ccf6bb3b..bab5cd9f574 100644
--- a/drivers/net/ethernet/sfc/nic.h
+++ b/drivers/net/ethernet/sfc/nic.h
@@ -294,6 +294,24 @@ extern bool falcon_xmac_check_fault(struct efx_nic *efx);
 extern int falcon_reconfigure_xmac(struct efx_nic *efx);
 extern void falcon_update_stats_xmac(struct efx_nic *efx);
 
+/* Some statistics are computed as A - B where A and B each increase
+ * linearly with some hardware counter(s) and the counters are read
+ * asynchronously.  If the counters contributing to B are always read
+ * after those contributing to A, the computed value may be lower than
+ * the true value by some variable amount, and may decrease between
+ * subsequent computations.
+ *
+ * We should never allow statistics to decrease or to exceed the true
+ * value.  Since the computed value will never be greater than the
+ * true value, we can achieve this by only storing the computed value
+ * when it increases.
+ */
+static inline void efx_update_diff_stat(u64 *stat, u64 diff)
+{
+	if ((s64)(diff - *stat) > 0)
+		*stat = diff;
+}
+
 /* Interrupts and test events */
 extern int efx_nic_init_interrupt(struct efx_nic *efx);
 extern void efx_nic_enable_interrupts(struct efx_nic *efx);
diff --git a/drivers/net/ethernet/sfc/rx.c b/drivers/net/ethernet/sfc/rx.c
index 243e91f3dff..719319b89d7 100644
--- a/drivers/net/ethernet/sfc/rx.c
+++ b/drivers/net/ethernet/sfc/rx.c
@@ -155,11 +155,11 @@ static int efx_init_rx_buffers_skb(struct efx_rx_queue *rx_queue)
 		rx_buf->len = skb_len - NET_IP_ALIGN;
 		rx_buf->flags = 0;
 
-		rx_buf->dma_addr = pci_map_single(efx->pci_dev,
+		rx_buf->dma_addr = dma_map_single(&efx->pci_dev->dev,
 						  skb->data, rx_buf->len,
-						  PCI_DMA_FROMDEVICE);
-		if (unlikely(pci_dma_mapping_error(efx->pci_dev,
-						   rx_buf->dma_addr))) {
+						  DMA_FROM_DEVICE);
+		if (unlikely(dma_mapping_error(&efx->pci_dev->dev,
+					       rx_buf->dma_addr))) {
 			dev_kfree_skb_any(skb);
 			rx_buf->u.skb = NULL;
 			return -EIO;
@@ -200,10 +200,10 @@ static int efx_init_rx_buffers_page(struct efx_rx_queue *rx_queue)
 				   efx->rx_buffer_order);
 		if (unlikely(page == NULL))
 			return -ENOMEM;
-		dma_addr = pci_map_page(efx->pci_dev, page, 0,
+		dma_addr = dma_map_page(&efx->pci_dev->dev, page, 0,
 					efx_rx_buf_size(efx),
-					PCI_DMA_FROMDEVICE);
-		if (unlikely(pci_dma_mapping_error(efx->pci_dev, dma_addr))) {
+					DMA_FROM_DEVICE);
+		if (unlikely(dma_mapping_error(&efx->pci_dev->dev, dma_addr))) {
 			__free_pages(page, efx->rx_buffer_order);
 			return -EIO;
 		}
@@ -247,14 +247,14 @@ static void efx_unmap_rx_buffer(struct efx_nic *efx,
 
 		state = page_address(rx_buf->u.page);
 		if (--state->refcnt == 0) {
-			pci_unmap_page(efx->pci_dev,
+			dma_unmap_page(&efx->pci_dev->dev,
 				       state->dma_addr,
 				       efx_rx_buf_size(efx),
-				       PCI_DMA_FROMDEVICE);
+				       DMA_FROM_DEVICE);
 		}
 	} else if (!(rx_buf->flags & EFX_RX_BUF_PAGE) && rx_buf->u.skb) {
-		pci_unmap_single(efx->pci_dev, rx_buf->dma_addr,
-				 rx_buf->len, PCI_DMA_FROMDEVICE);
+		dma_unmap_single(&efx->pci_dev->dev, rx_buf->dma_addr,
+				 rx_buf->len, DMA_FROM_DEVICE);
 	}
 }
 
@@ -336,6 +336,7 @@ static void efx_recycle_rx_buffer(struct efx_channel *channel,
 /**
  * efx_fast_push_rx_descriptors - push new RX descriptors quickly
  * @rx_queue:		RX descriptor queue
+ *
  * This will aim to fill the RX descriptor queue up to
  * @rx_queue->@max_fill. If there is insufficient atomic
  * memory to do so, a slow fill will be scheduled.
diff --git a/drivers/net/ethernet/sfc/selftest.c b/drivers/net/ethernet/sfc/selftest.c
index de4c0069f5b..96068d15b60 100644
--- a/drivers/net/ethernet/sfc/selftest.c
+++ b/drivers/net/ethernet/sfc/selftest.c
@@ -120,19 +120,6 @@ static int efx_test_nvram(struct efx_nic *efx, struct efx_self_tests *tests)
 	return rc;
 }
 
-static int efx_test_chip(struct efx_nic *efx, struct efx_self_tests *tests)
-{
-	int rc = 0;
-
-	/* Test register access */
-	if (efx->type->test_registers) {
-		rc = efx->type->test_registers(efx);
-		tests->registers = rc ? -1 : 1;
-	}
-
-	return rc;
-}
-
 /**************************************************************************
  *
  * Interrupt and event queue testing
@@ -488,7 +475,7 @@ static int efx_end_loopback(struct efx_tx_queue *tx_queue,
 		skb = state->skbs[i];
 		if (skb && !skb_shared(skb))
 			++tx_done;
-		dev_kfree_skb_any(skb);
+		dev_kfree_skb(skb);
 	}
 
 	netif_tx_unlock_bh(efx->net_dev);
@@ -699,8 +686,7 @@ int efx_selftest(struct efx_nic *efx, struct efx_self_tests *tests,
 {
 	enum efx_loopback_mode loopback_mode = efx->loopback_mode;
 	int phy_mode = efx->phy_mode;
-	enum reset_type reset_method = RESET_TYPE_INVISIBLE;
-	int rc_test = 0, rc_reset = 0, rc;
+	int rc_test = 0, rc_reset, rc;
 
 	efx_selftest_async_cancel(efx);
 
@@ -737,44 +723,26 @@ int efx_selftest(struct efx_nic *efx, struct efx_self_tests *tests,
 	 */
 	netif_device_detach(efx->net_dev);
 
-	mutex_lock(&efx->mac_lock);
-	if (efx->loopback_modes) {
-		/* We need the 312 clock from the PHY to test the XMAC
-		 * registers, so move into XGMII loopback if available */
-		if (efx->loopback_modes & (1 << LOOPBACK_XGMII))
-			efx->loopback_mode = LOOPBACK_XGMII;
-		else
-			efx->loopback_mode = __ffs(efx->loopback_modes);
-	}
-
-	__efx_reconfigure_port(efx);
-	mutex_unlock(&efx->mac_lock);
-
-	/* free up all consumers of SRAM (including all the queues) */
-	efx_reset_down(efx, reset_method);
-
-	rc = efx_test_chip(efx, tests);
-	if (rc && !rc_test)
-		rc_test = rc;
+	if (efx->type->test_chip) {
+		rc_reset = efx->type->test_chip(efx, tests);
+		if (rc_reset) {
+			netif_err(efx, hw, efx->net_dev,
+				  "Unable to recover from chip test\n");
+			efx_schedule_reset(efx, RESET_TYPE_DISABLE);
+			return rc_reset;
+		}
 
-	/* reset the chip to recover from the register test */
-	rc_reset = efx->type->reset(efx, reset_method);
+		if ((tests->registers < 0) && !rc_test)
+			rc_test = -EIO;
+	}
 
 	/* Ensure that the phy is powered and out of loopback
 	 * for the bist and loopback tests */
+	mutex_lock(&efx->mac_lock);
 	efx->phy_mode &= ~PHY_MODE_LOW_POWER;
 	efx->loopback_mode = LOOPBACK_NONE;
-
-	rc = efx_reset_up(efx, reset_method, rc_reset == 0);
-	if (rc && !rc_reset)
-		rc_reset = rc;
-
-	if (rc_reset) {
-		netif_err(efx, drv, efx->net_dev,
-			  "Unable to recover from chip test\n");
-		efx_schedule_reset(efx, RESET_TYPE_DISABLE);
-		return rc_reset;
-	}
+	__efx_reconfigure_port(efx);
+	mutex_unlock(&efx->mac_lock);
 
 	rc = efx_test_phy(efx, tests, flags);
 	if (rc && !rc_test)
diff --git a/drivers/net/ethernet/sfc/siena.c b/drivers/net/ethernet/sfc/siena.c
index 9f8d7cea396..6bafd216e55 100644
--- a/drivers/net/ethernet/sfc/siena.c
+++ b/drivers/net/ethernet/sfc/siena.c
@@ -25,10 +25,12 @@
 #include "workarounds.h"
 #include "mcdi.h"
 #include "mcdi_pcol.h"
+#include "selftest.h"
 
 /* Hardware control for SFC9000 family including SFL9021 (aka Siena). */
 
 static void siena_init_wol(struct efx_nic *efx);
+static int siena_reset_hw(struct efx_nic *efx, enum reset_type method);
 
 
 static void siena_push_irq_moderation(struct efx_channel *channel)
@@ -154,10 +156,29 @@ static const struct efx_nic_register_test siena_register_tests[] = {
 	  EFX_OWORD32(0xFFFFFFFF, 0xFFFFFFFF, 0x00000007, 0x00000000) },
 };
 
-static int siena_test_registers(struct efx_nic *efx)
+static int siena_test_chip(struct efx_nic *efx, struct efx_self_tests *tests)
 {
-	return efx_nic_test_registers(efx, siena_register_tests,
-				      ARRAY_SIZE(siena_register_tests));
+	enum reset_type reset_method = reset_method;
+	int rc, rc2;
+
+	efx_reset_down(efx, reset_method);
+
+	/* Reset the chip immediately so that it is completely
+	 * quiescent regardless of what any VF driver does.
+	 */
+	rc = siena_reset_hw(efx, reset_method);
+	if (rc)
+		goto out;
+
+	tests->registers =
+		efx_nic_test_registers(efx, siena_register_tests,
+				       ARRAY_SIZE(siena_register_tests))
+		? -1 : 1;
+
+	rc = siena_reset_hw(efx, reset_method);
+out:
+	rc2 = efx_reset_up(efx, reset_method, rc == 0);
+	return rc ? rc : rc2;
 }
 
 /**************************************************************************
@@ -437,8 +458,8 @@ static int siena_try_update_nic_stats(struct efx_nic *efx)
 
 	MAC_STAT(tx_bytes, TX_BYTES);
 	MAC_STAT(tx_bad_bytes, TX_BAD_BYTES);
-	mac_stats->tx_good_bytes = (mac_stats->tx_bytes -
-				    mac_stats->tx_bad_bytes);
+	efx_update_diff_stat(&mac_stats->tx_good_bytes,
+			     mac_stats->tx_bytes - mac_stats->tx_bad_bytes);
 	MAC_STAT(tx_packets, TX_PKTS);
 	MAC_STAT(tx_bad, TX_BAD_FCS_PKTS);
 	MAC_STAT(tx_pause, TX_PAUSE_PKTS);
@@ -471,8 +492,8 @@ static int siena_try_update_nic_stats(struct efx_nic *efx)
 	MAC_STAT(tx_ip_src_error, TX_IP_SRC_ERR_PKTS);
 	MAC_STAT(rx_bytes, RX_BYTES);
 	MAC_STAT(rx_bad_bytes, RX_BAD_BYTES);
-	mac_stats->rx_good_bytes = (mac_stats->rx_bytes -
-				    mac_stats->rx_bad_bytes);
+	efx_update_diff_stat(&mac_stats->rx_good_bytes,
+			     mac_stats->rx_bytes - mac_stats->rx_bad_bytes);
 	MAC_STAT(rx_packets, RX_PKTS);
 	MAC_STAT(rx_good, RX_GOOD_PKTS);
 	MAC_STAT(rx_bad, RX_BAD_FCS_PKTS);
@@ -649,7 +670,7 @@ const struct efx_nic_type siena_a0_nic_type = {
 	.get_wol = siena_get_wol,
 	.set_wol = siena_set_wol,
 	.resume_wol = siena_init_wol,
-	.test_registers = siena_test_registers,
+	.test_chip = siena_test_chip,
 	.test_nvram = efx_mcdi_nvram_test_all,
 
 	.revision = EFX_REV_SIENA_A0,
diff --git a/drivers/net/ethernet/sfc/tx.c b/drivers/net/ethernet/sfc/tx.c
index 94d0365b31c..9b225a7769f 100644
--- a/drivers/net/ethernet/sfc/tx.c
+++ b/drivers/net/ethernet/sfc/tx.c
@@ -36,15 +36,15 @@ static void efx_dequeue_buffer(struct efx_tx_queue *tx_queue,
 			       unsigned int *bytes_compl)
 {
 	if (buffer->unmap_len) {
-		struct pci_dev *pci_dev = tx_queue->efx->pci_dev;
+		struct device *dma_dev = &tx_queue->efx->pci_dev->dev;
 		dma_addr_t unmap_addr = (buffer->dma_addr + buffer->len -
 					 buffer->unmap_len);
 		if (buffer->unmap_single)
-			pci_unmap_single(pci_dev, unmap_addr, buffer->unmap_len,
-					 PCI_DMA_TODEVICE);
+			dma_unmap_single(dma_dev, unmap_addr, buffer->unmap_len,
+					 DMA_TO_DEVICE);
 		else
-			pci_unmap_page(pci_dev, unmap_addr, buffer->unmap_len,
-				       PCI_DMA_TODEVICE);
+			dma_unmap_page(dma_dev, unmap_addr, buffer->unmap_len,
+				       DMA_TO_DEVICE);
 		buffer->unmap_len = 0;
 		buffer->unmap_single = false;
 	}
@@ -138,7 +138,7 @@ efx_max_tx_len(struct efx_nic *efx, dma_addr_t dma_addr)
 netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 {
 	struct efx_nic *efx = tx_queue->efx;
-	struct pci_dev *pci_dev = efx->pci_dev;
+	struct device *dma_dev = &efx->pci_dev->dev;
 	struct efx_tx_buffer *buffer;
 	skb_frag_t *fragment;
 	unsigned int len, unmap_len = 0, fill_level, insert_ptr;
@@ -167,17 +167,17 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 	fill_level = tx_queue->insert_count - tx_queue->old_read_count;
 	q_space = efx->txq_entries - 1 - fill_level;
 
-	/* Map for DMA.  Use pci_map_single rather than pci_map_page
+	/* Map for DMA.  Use dma_map_single rather than dma_map_page
 	 * since this is more efficient on machines with sparse
 	 * memory.
 	 */
 	unmap_single = true;
-	dma_addr = pci_map_single(pci_dev, skb->data, len, PCI_DMA_TODEVICE);
+	dma_addr = dma_map_single(dma_dev, skb->data, len, PCI_DMA_TODEVICE);
 
 	/* Process all fragments */
 	while (1) {
-		if (unlikely(pci_dma_mapping_error(pci_dev, dma_addr)))
-			goto pci_err;
+		if (unlikely(dma_mapping_error(dma_dev, dma_addr)))
+			goto dma_err;
 
 		/* Store fields for marking in the per-fragment final
 		 * descriptor */
@@ -246,7 +246,7 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 		i++;
 		/* Map for DMA */
 		unmap_single = false;
-		dma_addr = skb_frag_dma_map(&pci_dev->dev, fragment, 0, len,
+		dma_addr = skb_frag_dma_map(dma_dev, fragment, 0, len,
 					    DMA_TO_DEVICE);
 	}
 
@@ -261,7 +261,7 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 
 	return NETDEV_TX_OK;
 
- pci_err:
+ dma_err:
 	netif_err(efx, tx_err, efx->net_dev,
 		  " TX queue %d could not map skb with %d bytes %d "
 		  "fragments for DMA\n", tx_queue->queue, skb->len,
@@ -284,11 +284,11 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 	/* Free the fragment we were mid-way through pushing */
 	if (unmap_len) {
 		if (unmap_single)
-			pci_unmap_single(pci_dev, unmap_addr, unmap_len,
-					 PCI_DMA_TODEVICE);
+			dma_unmap_single(dma_dev, unmap_addr, unmap_len,
+					 DMA_TO_DEVICE);
 		else
-			pci_unmap_page(pci_dev, unmap_addr, unmap_len,
-				       PCI_DMA_TODEVICE);
+			dma_unmap_page(dma_dev, unmap_addr, unmap_len,
+				       DMA_TO_DEVICE);
 	}
 
 	return rc;
@@ -651,17 +651,8 @@ static __be16 efx_tso_check_protocol(struct sk_buff *skb)
 	EFX_BUG_ON_PARANOID(((struct ethhdr *)skb->data)->h_proto !=
 			    protocol);
 	if (protocol == htons(ETH_P_8021Q)) {
-		/* Find the encapsulated protocol; reset network header
-		 * and transport header based on that. */
 		struct vlan_ethhdr *veh = (struct vlan_ethhdr *)skb->data;
 		protocol = veh->h_vlan_encapsulated_proto;
-		skb_set_network_header(skb, sizeof(*veh));
-		if (protocol == htons(ETH_P_IP))
-			skb_set_transport_header(skb, sizeof(*veh) +
-						 4 * ip_hdr(skb)->ihl);
-		else if (protocol == htons(ETH_P_IPV6))
-			skb_set_transport_header(skb, sizeof(*veh) +
-						 sizeof(struct ipv6hdr));
 	}
 
 	if (protocol == htons(ETH_P_IP)) {
@@ -684,20 +675,19 @@ static __be16 efx_tso_check_protocol(struct sk_buff *skb)
  */
 static int efx_tsoh_block_alloc(struct efx_tx_queue *tx_queue)
 {
-
-	struct pci_dev *pci_dev = tx_queue->efx->pci_dev;
+	struct device *dma_dev = &tx_queue->efx->pci_dev->dev;
 	struct efx_tso_header *tsoh;
 	dma_addr_t dma_addr;
 	u8 *base_kva, *kva;
 
-	base_kva = pci_alloc_consistent(pci_dev, PAGE_SIZE, &dma_addr);
+	base_kva = dma_alloc_coherent(dma_dev, PAGE_SIZE, &dma_addr, GFP_ATOMIC);
 	if (base_kva == NULL) {
 		netif_err(tx_queue->efx, tx_err, tx_queue->efx->net_dev,
 			  "Unable to allocate page for TSO headers\n");
 		return -ENOMEM;
 	}
 
-	/* pci_alloc_consistent() allocates pages. */
+	/* dma_alloc_coherent() allocates pages. */
 	EFX_BUG_ON_PARANOID(dma_addr & (PAGE_SIZE - 1u));
 
 	for (kva = base_kva; kva < base_kva + PAGE_SIZE; kva += TSOH_STD_SIZE) {
@@ -714,7 +704,7 @@ static int efx_tsoh_block_alloc(struct efx_tx_queue *tx_queue)
 /* Free up a TSO header, and all others in the same page. */
 static void efx_tsoh_block_free(struct efx_tx_queue *tx_queue,
 				struct efx_tso_header *tsoh,
-				struct pci_dev *pci_dev)
+				struct device *dma_dev)
 {
 	struct efx_tso_header **p;
 	unsigned long base_kva;
@@ -731,7 +721,7 @@ static void efx_tsoh_block_free(struct efx_tx_queue *tx_queue,
 			p = &(*p)->next;
 	}
 
-	pci_free_consistent(pci_dev, PAGE_SIZE, (void *)base_kva, base_dma);
+	dma_free_coherent(dma_dev, PAGE_SIZE, (void *)base_kva, base_dma);
 }
 
 static struct efx_tso_header *
@@ -743,11 +733,11 @@ efx_tsoh_heap_alloc(struct efx_tx_queue *tx_queue, size_t header_len)
 	if (unlikely(!tsoh))
 		return NULL;
 
-	tsoh->dma_addr = pci_map_single(tx_queue->efx->pci_dev,
+	tsoh->dma_addr = dma_map_single(&tx_queue->efx->pci_dev->dev,
 					TSOH_BUFFER(tsoh), header_len,
-					PCI_DMA_TODEVICE);
-	if (unlikely(pci_dma_mapping_error(tx_queue->efx->pci_dev,
-					   tsoh->dma_addr))) {
+					DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(&tx_queue->efx->pci_dev->dev,
+				       tsoh->dma_addr))) {
 		kfree(tsoh);
 		return NULL;
 	}
@@ -759,9 +749,9 @@ efx_tsoh_heap_alloc(struct efx_tx_queue *tx_queue, size_t header_len)
 static void
 efx_tsoh_heap_free(struct efx_tx_queue *tx_queue, struct efx_tso_header *tsoh)
 {
-	pci_unmap_single(tx_queue->efx->pci_dev,
+	dma_unmap_single(&tx_queue->efx->pci_dev->dev,
 			 tsoh->dma_addr, tsoh->unmap_len,
-			 PCI_DMA_TODEVICE);
+			 DMA_TO_DEVICE);
 	kfree(tsoh);
 }
 
@@ -892,13 +882,13 @@ static void efx_enqueue_unwind(struct efx_tx_queue *tx_queue)
 			unmap_addr = (buffer->dma_addr + buffer->len -
 				      buffer->unmap_len);
 			if (buffer->unmap_single)
-				pci_unmap_single(tx_queue->efx->pci_dev,
+				dma_unmap_single(&tx_queue->efx->pci_dev->dev,
 						 unmap_addr, buffer->unmap_len,
-						 PCI_DMA_TODEVICE);
+						 DMA_TO_DEVICE);
 			else
-				pci_unmap_page(tx_queue->efx->pci_dev,
+				dma_unmap_page(&tx_queue->efx->pci_dev->dev,
 					       unmap_addr, buffer->unmap_len,
-					       PCI_DMA_TODEVICE);
+					       DMA_TO_DEVICE);
 			buffer->unmap_len = 0;
 		}
 		buffer->len = 0;
@@ -927,7 +917,6 @@ static void tso_start(struct tso_state *st, const struct sk_buff *skb)
 	EFX_BUG_ON_PARANOID(tcp_hdr(skb)->syn);
 	EFX_BUG_ON_PARANOID(tcp_hdr(skb)->rst);
 
-	st->packet_space = st->full_packet_size;
 	st->out_len = skb->len - st->header_len;
 	st->unmap_len = 0;
 	st->unmap_single = false;
@@ -954,9 +943,9 @@ static int tso_get_head_fragment(struct tso_state *st, struct efx_nic *efx,
 	int hl = st->header_len;
 	int len = skb_headlen(skb) - hl;
 
-	st->unmap_addr = pci_map_single(efx->pci_dev, skb->data + hl,
-					len, PCI_DMA_TODEVICE);
-	if (likely(!pci_dma_mapping_error(efx->pci_dev, st->unmap_addr))) {
+	st->unmap_addr = dma_map_single(&efx->pci_dev->dev, skb->data + hl,
+					len, DMA_TO_DEVICE);
+	if (likely(!dma_mapping_error(&efx->pci_dev->dev, st->unmap_addr))) {
 		st->unmap_single = true;
 		st->unmap_len = len;
 		st->in_len = len;
@@ -1008,7 +997,7 @@ static int tso_fill_packet_with_fragment(struct efx_tx_queue *tx_queue,
 		buffer->continuation = !end_of_packet;
 
 		if (st->in_len == 0) {
-			/* Transfer ownership of the pci mapping */
+			/* Transfer ownership of the DMA mapping */
 			buffer->unmap_len = st->unmap_len;
 			buffer->unmap_single = st->unmap_single;
 			st->unmap_len = 0;
@@ -1181,18 +1170,18 @@ static int efx_enqueue_skb_tso(struct efx_tx_queue *tx_queue,
 
  mem_err:
 	netif_err(efx, tx_err, efx->net_dev,
-		  "Out of memory for TSO headers, or PCI mapping error\n");
+		  "Out of memory for TSO headers, or DMA mapping error\n");
 	dev_kfree_skb_any(skb);
 
  unwind:
 	/* Free the DMA mapping we were in the process of writing out */
 	if (state.unmap_len) {
 		if (state.unmap_single)
-			pci_unmap_single(efx->pci_dev, state.unmap_addr,
-					 state.unmap_len, PCI_DMA_TODEVICE);
+			dma_unmap_single(&efx->pci_dev->dev, state.unmap_addr,
+					 state.unmap_len, DMA_TO_DEVICE);
 		else
-			pci_unmap_page(efx->pci_dev, state.unmap_addr,
-				       state.unmap_len, PCI_DMA_TODEVICE);
+			dma_unmap_page(&efx->pci_dev->dev, state.unmap_addr,
+				       state.unmap_len, DMA_TO_DEVICE);
 	}
 
 	efx_enqueue_unwind(tx_queue);
@@ -1216,5 +1205,5 @@ static void efx_fini_tso(struct efx_tx_queue *tx_queue)
 
 	while (tx_queue->tso_headers_free != NULL)
 		efx_tsoh_block_free(tx_queue, tx_queue->tso_headers_free,
-				    tx_queue->efx->pci_dev);
+				    &tx_queue->efx->pci_dev->dev);
 }
diff --git a/drivers/net/ethernet/sgi/ioc3-eth.c b/drivers/net/ethernet/sgi/ioc3-eth.c
index fcb5b0e0f26..3e5519a0acc 100644
--- a/drivers/net/ethernet/sgi/ioc3-eth.c
+++ b/drivers/net/ethernet/sgi/ioc3-eth.c
@@ -583,7 +583,7 @@ static inline void ioc3_rx(struct net_device *dev)
 	unsigned long *rxr;
 	u32 w0, err;
 
-	rxr = (unsigned long *) ip->rxr;		/* Ring base */
+	rxr = ip->rxr;		/* Ring base */
 	rx_entry = ip->rx_ci;				/* RX consume index */
 	n_entry = ip->rx_pi;
 
@@ -903,7 +903,7 @@ static void ioc3_alloc_rings(struct net_device *dev)
 	if (ip->rxr == NULL) {
 		/* Allocate and initialize rx ring.  4kb = 512 entries  */
 		ip->rxr = (unsigned long *) get_zeroed_page(GFP_ATOMIC);
-		rxr = (unsigned long *) ip->rxr;
+		rxr = ip->rxr;
 		if (!rxr)
 			printk("ioc3_alloc_rings(): get_zeroed_page() failed!\n");
 
diff --git a/drivers/net/ethernet/smsc/smc911x.c b/drivers/net/ethernet/smsc/smc911x.c
index 8814b2f5d46..8d15f7a74b4 100644
--- a/drivers/net/ethernet/smsc/smc911x.c
+++ b/drivers/net/ethernet/smsc/smc911x.c
@@ -773,7 +773,7 @@ static int smc911x_phy_fixed(struct net_device *dev)
 	return 1;
 }
 
-/*
+/**
  * smc911x_phy_reset - reset the phy
  * @dev: net device
  * @phy: phy address
@@ -819,7 +819,7 @@ static int smc911x_phy_reset(struct net_device *dev, int phy)
 	return reg & PMT_CTRL_PHY_RST_;
 }
 
-/*
+/**
  * smc911x_phy_powerdown - powerdown phy
  * @dev: net device
  * @phy: phy address
@@ -837,7 +837,7 @@ static void smc911x_phy_powerdown(struct net_device *dev, int phy)
 	SMC_SET_PHY_BMCR(lp, phy, bmcr);
 }
 
-/*
+/**
  * smc911x_phy_check_media - check the media status and adjust BMCR
  * @dev: net device
  * @init: set true for initialisation
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index fee44935501..318adc935a5 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -942,7 +942,7 @@ static int smc_phy_fixed(struct net_device *dev)
 	return 1;
 }
 
-/*
+/**
  * smc_phy_reset - reset the phy
  * @dev: net device
  * @phy: phy address
@@ -976,7 +976,7 @@ static int smc_phy_reset(struct net_device *dev, int phy)
 	return bmcr & BMCR_RESET;
 }
 
-/*
+/**
  * smc_phy_powerdown - powerdown phy
  * @dev: net device
  *
@@ -1000,7 +1000,7 @@ static void smc_phy_powerdown(struct net_device *dev)
 	smc_phy_write(dev, phy, MII_BMCR, bmcr | BMCR_PDOWN);
 }
 
-/*
+/**
  * smc_phy_check_media - check the media status and adjust TCR
  * @dev: net device
  * @init: set true for initialisation
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index 1466e5d2af4..62d1baf111e 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -1442,6 +1442,14 @@ smsc911x_set_hw_mac_address(struct smsc911x_data *pdata, u8 dev_addr[6])
 	smsc911x_mac_write(pdata, ADDRL, mac_low32);
 }
 
+static void smsc911x_disable_irq_chip(struct net_device *dev)
+{
+	struct smsc911x_data *pdata = netdev_priv(dev);
+
+	smsc911x_reg_write(pdata, INT_EN, 0);
+	smsc911x_reg_write(pdata, INT_STS, 0xFFFFFFFF);
+}
+
 static int smsc911x_open(struct net_device *dev)
 {
 	struct smsc911x_data *pdata = netdev_priv(dev);
@@ -1494,8 +1502,7 @@ static int smsc911x_open(struct net_device *dev)
 	spin_unlock_irq(&pdata->mac_lock);
 
 	/* Initialise irqs, but leave all sources disabled */
-	smsc911x_reg_write(pdata, INT_EN, 0);
-	smsc911x_reg_write(pdata, INT_STS, 0xFFFFFFFF);
+	smsc911x_disable_irq_chip(dev);
 
 	/* Set interrupt deassertion to 100uS */
 	intcfg = ((10 << 24) | INT_CFG_IRQ_EN_);
@@ -2215,9 +2222,6 @@ static int __devinit smsc911x_init(struct net_device *dev)
 	if (smsc911x_soft_reset(pdata))
 		return -ENODEV;
 
-	/* Disable all interrupt sources until we bring the device up */
-	smsc911x_reg_write(pdata, INT_EN, 0);
-
 	ether_setup(dev);
 	dev->flags |= IFF_MULTICAST;
 	netif_napi_add(dev, &pdata->napi, smsc911x_poll, SMSC_NAPI_WEIGHT);
@@ -2434,8 +2438,7 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 	smsc911x_reg_write(pdata, INT_CFG, intcfg);
 
 	/* Ensure interrupts are globally disabled before connecting ISR */
-	smsc911x_reg_write(pdata, INT_EN, 0);
-	smsc911x_reg_write(pdata, INT_STS, 0xFFFFFFFF);
+	smsc911x_disable_irq_chip(dev);
 
 	retval = request_irq(dev->irq, smsc911x_irqhandler,
 			     irq_flags | IRQF_SHARED, dev->name, dev);
@@ -2485,7 +2488,7 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 			eth_hw_addr_random(dev);
 			smsc911x_set_hw_mac_address(pdata, dev->dev_addr);
 			SMSC_TRACE(pdata, probe,
-				   "MAC Address is set to random_ether_addr");
+				   "MAC Address is set to eth_random_addr");
 		}
 	}
 
diff --git a/drivers/net/ethernet/smsc/smsc9420.c b/drivers/net/ethernet/smsc/smsc9420.c
index fd33b21f6c9..1fcd914ec39 100644
--- a/drivers/net/ethernet/smsc/smsc9420.c
+++ b/drivers/net/ethernet/smsc/smsc9420.c
@@ -1640,8 +1640,7 @@ smsc9420_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto out_free_io_4;
 
 	/* descriptors are aligned due to the nature of pci_alloc_consistent */
-	pd->tx_ring = (struct smsc9420_dma_desc *)
-	    (pd->rx_ring + RX_RING_SIZE);
+	pd->tx_ring = (pd->rx_ring + RX_RING_SIZE);
 	pd->tx_dma_addr = pd->rx_dma_addr +
 	    sizeof(struct smsc9420_dma_desc) * RX_RING_SIZE;
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index bcd54d6e94f..e2d083228f3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -95,6 +95,16 @@ struct stmmac_extra_stats {
 	unsigned long poll_n;
 	unsigned long sched_timer_n;
 	unsigned long normal_irq_n;
+	unsigned long mmc_tx_irq_n;
+	unsigned long mmc_rx_irq_n;
+	unsigned long mmc_rx_csum_offload_irq_n;
+	/* EEE */
+	unsigned long irq_receive_pmt_irq_n;
+	unsigned long irq_tx_path_in_lpi_mode_n;
+	unsigned long irq_tx_path_exit_lpi_mode_n;
+	unsigned long irq_rx_path_in_lpi_mode_n;
+	unsigned long irq_rx_path_exit_lpi_mode_n;
+	unsigned long phy_eee_wakeup_error_n;
 };
 
 /* CSR Frequency Access Defines*/
@@ -162,6 +172,17 @@ enum tx_dma_irq_status {
 	handle_tx_rx = 3,
 };
 
+enum core_specific_irq_mask {
+	core_mmc_tx_irq = 1,
+	core_mmc_rx_irq = 2,
+	core_mmc_rx_csum_offload_irq = 4,
+	core_irq_receive_pmt_irq = 8,
+	core_irq_tx_path_in_lpi_mode = 16,
+	core_irq_tx_path_exit_lpi_mode = 32,
+	core_irq_rx_path_in_lpi_mode = 64,
+	core_irq_rx_path_exit_lpi_mode = 128,
+};
+
 /* DMA HW capabilities */
 struct dma_features {
 	unsigned int mbps_10_100;
@@ -208,6 +229,10 @@ struct dma_features {
 #define MAC_ENABLE_TX		0x00000008	/* Transmitter Enable */
 #define MAC_RNABLE_RX		0x00000004	/* Receiver Enable */
 
+/* Default LPI timers */
+#define STMMAC_DEFAULT_LIT_LS_TIMER	0x3E8
+#define STMMAC_DEFAULT_TWT_LS_TIMER	0x0
+
 struct stmmac_desc_ops {
 	/* DMA RX descriptor ring initialization */
 	void (*init_rx_desc) (struct dma_desc *p, unsigned int ring_size,
@@ -278,7 +303,7 @@ struct stmmac_ops {
 	/* Dump MAC registers */
 	void (*dump_regs) (void __iomem *ioaddr);
 	/* Handle extra events on specific interrupts hw dependent */
-	void (*host_irq_status) (void __iomem *ioaddr);
+	int (*host_irq_status) (void __iomem *ioaddr);
 	/* Multicast filter setting */
 	void (*set_filter) (struct net_device *dev, int id);
 	/* Flow control setting */
@@ -291,6 +316,10 @@ struct stmmac_ops {
 			       unsigned int reg_n);
 	void (*get_umac_addr) (void __iomem *ioaddr, unsigned char *addr,
 			       unsigned int reg_n);
+	void (*set_eee_mode) (void __iomem *ioaddr);
+	void (*reset_eee_mode) (void __iomem *ioaddr);
+	void (*set_eee_timer) (void __iomem *ioaddr, int ls, int tw);
+	void (*set_eee_pls) (void __iomem *ioaddr, int link);
 };
 
 struct mac_link {
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index 23478bf4ed7..f90fcb5f957 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -36,6 +36,7 @@
 
 #define GMAC_INT_STATUS		0x00000038	/* interrupt status register */
 enum dwmac1000_irq_status {
+	lpiis_irq = 0x400,
 	time_stamp_irq = 0x0200,
 	mmc_rx_csum_offload_irq = 0x0080,
 	mmc_tx_irq = 0x0040,
@@ -60,6 +61,25 @@ enum power_event {
 	power_down = 0x00000001,
 };
 
+/* Energy Efficient Ethernet (EEE)
+ *
+ * LPI status, timer and control register offset
+ */
+#define LPI_CTRL_STATUS	0x0030
+#define LPI_TIMER_CTRL	0x0034
+
+/* LPI control and status defines */
+#define LPI_CTRL_STATUS_LPITXA	0x00080000	/* Enable LPI TX Automate */
+#define LPI_CTRL_STATUS_PLSEN	0x00040000	/* Enable PHY Link Status */
+#define LPI_CTRL_STATUS_PLS	0x00020000	/* PHY Link Status */
+#define LPI_CTRL_STATUS_LPIEN	0x00010000	/* LPI Enable */
+#define LPI_CTRL_STATUS_RLPIST	0x00000200	/* Receive LPI state */
+#define LPI_CTRL_STATUS_TLPIST	0x00000100	/* Transmit LPI state */
+#define LPI_CTRL_STATUS_RLPIEX	0x00000008	/* Receive LPI Exit */
+#define LPI_CTRL_STATUS_RLPIEN	0x00000004	/* Receive LPI Entry */
+#define LPI_CTRL_STATUS_TLPIEX	0x00000002	/* Transmit LPI Exit */
+#define LPI_CTRL_STATUS_TLPIEN	0x00000001	/* Transmit LPI Entry */
+
 /* GMAC HW ADDR regs */
 #define GMAC_ADDR_HIGH(reg)	(((reg > 15) ? 0x00000800 : 0x00000040) + \
 				(reg * 8))
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index b5e4d02f15c..bfe02260549 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -194,26 +194,107 @@ static void dwmac1000_pmt(void __iomem *ioaddr, unsigned long mode)
 }
 
 
-static void dwmac1000_irq_status(void __iomem *ioaddr)
+static int dwmac1000_irq_status(void __iomem *ioaddr)
 {
 	u32 intr_status = readl(ioaddr + GMAC_INT_STATUS);
+	int status = 0;
 
 	/* Not used events (e.g. MMC interrupts) are not handled. */
-	if ((intr_status & mmc_tx_irq))
-		CHIP_DBG(KERN_DEBUG "GMAC: MMC tx interrupt: 0x%08x\n",
+	if ((intr_status & mmc_tx_irq)) {
+		CHIP_DBG(KERN_INFO "GMAC: MMC tx interrupt: 0x%08x\n",
 		    readl(ioaddr + GMAC_MMC_TX_INTR));
-	if (unlikely(intr_status & mmc_rx_irq))
-		CHIP_DBG(KERN_DEBUG "GMAC: MMC rx interrupt: 0x%08x\n",
+		status |= core_mmc_tx_irq;
+	}
+	if (unlikely(intr_status & mmc_rx_irq)) {
+		CHIP_DBG(KERN_INFO "GMAC: MMC rx interrupt: 0x%08x\n",
 		    readl(ioaddr + GMAC_MMC_RX_INTR));
-	if (unlikely(intr_status & mmc_rx_csum_offload_irq))
-		CHIP_DBG(KERN_DEBUG "GMAC: MMC rx csum offload: 0x%08x\n",
+		status |= core_mmc_rx_irq;
+	}
+	if (unlikely(intr_status & mmc_rx_csum_offload_irq)) {
+		CHIP_DBG(KERN_INFO "GMAC: MMC rx csum offload: 0x%08x\n",
 		    readl(ioaddr + GMAC_MMC_RX_CSUM_OFFLOAD));
+		status |= core_mmc_rx_csum_offload_irq;
+	}
 	if (unlikely(intr_status & pmt_irq)) {
-		CHIP_DBG(KERN_DEBUG "GMAC: received Magic frame\n");
+		CHIP_DBG(KERN_INFO "GMAC: received Magic frame\n");
 		/* clear the PMT bits 5 and 6 by reading the PMT
 		 * status register. */
 		readl(ioaddr + GMAC_PMT);
+		status |= core_irq_receive_pmt_irq;
 	}
+	/* MAC trx/rx EEE LPI entry/exit interrupts */
+	if (intr_status & lpiis_irq) {
+		/* Clean LPI interrupt by reading the Reg 12 */
+		u32 lpi_status = readl(ioaddr + LPI_CTRL_STATUS);
+
+		if (lpi_status & LPI_CTRL_STATUS_TLPIEN) {
+			CHIP_DBG(KERN_INFO "GMAC TX entered in LPI\n");
+			status |= core_irq_tx_path_in_lpi_mode;
+		}
+		if (lpi_status & LPI_CTRL_STATUS_TLPIEX) {
+			CHIP_DBG(KERN_INFO "GMAC TX exit from LPI\n");
+			status |= core_irq_tx_path_exit_lpi_mode;
+		}
+		if (lpi_status & LPI_CTRL_STATUS_RLPIEN) {
+			CHIP_DBG(KERN_INFO "GMAC RX entered in LPI\n");
+			status |= core_irq_rx_path_in_lpi_mode;
+		}
+		if (lpi_status & LPI_CTRL_STATUS_RLPIEX) {
+			CHIP_DBG(KERN_INFO "GMAC RX exit from LPI\n");
+			status |= core_irq_rx_path_exit_lpi_mode;
+		}
+	}
+
+	return status;
+}
+
+static void  dwmac1000_set_eee_mode(void __iomem *ioaddr)
+{
+	u32 value;
+
+	/* Enable the link status receive on RGMII, SGMII ore SMII
+	 * receive path and instruct the transmit to enter in LPI
+	 * state. */
+	value = readl(ioaddr + LPI_CTRL_STATUS);
+	value |= LPI_CTRL_STATUS_LPIEN | LPI_CTRL_STATUS_LPITXA;
+	writel(value, ioaddr + LPI_CTRL_STATUS);
+}
+
+static void  dwmac1000_reset_eee_mode(void __iomem *ioaddr)
+{
+	u32 value;
+
+	value = readl(ioaddr + LPI_CTRL_STATUS);
+	value &= ~(LPI_CTRL_STATUS_LPIEN | LPI_CTRL_STATUS_LPITXA);
+	writel(value, ioaddr + LPI_CTRL_STATUS);
+}
+
+static void  dwmac1000_set_eee_pls(void __iomem *ioaddr, int link)
+{
+	u32 value;
+
+	value = readl(ioaddr + LPI_CTRL_STATUS);
+
+	if (link)
+		value |= LPI_CTRL_STATUS_PLS;
+	else
+		value &= ~LPI_CTRL_STATUS_PLS;
+
+	writel(value, ioaddr + LPI_CTRL_STATUS);
+}
+
+static void  dwmac1000_set_eee_timer(void __iomem *ioaddr, int ls, int tw)
+{
+	int value = ((tw & 0xffff)) | ((ls & 0x7ff) << 16);
+
+	/* Program the timers in the LPI timer control register:
+	 * LS: minimum time (ms) for which the link
+	 *  status from PHY should be ok before transmitting
+	 *  the LPI pattern.
+	 * TW: minimum time (us) for which the core waits
+	 *  after it has stopped transmitting the LPI pattern.
+	 */
+	writel(value, ioaddr + LPI_TIMER_CTRL);
 }
 
 static const struct stmmac_ops dwmac1000_ops = {
@@ -226,6 +307,10 @@ static const struct stmmac_ops dwmac1000_ops = {
 	.pmt = dwmac1000_pmt,
 	.set_umac_addr = dwmac1000_set_umac_addr,
 	.get_umac_addr = dwmac1000_get_umac_addr,
+	.set_eee_mode =  dwmac1000_set_eee_mode,
+	.reset_eee_mode =  dwmac1000_reset_eee_mode,
+	.set_eee_timer =  dwmac1000_set_eee_timer,
+	.set_eee_pls =  dwmac1000_set_eee_pls,
 };
 
 struct mac_device_info *dwmac1000_setup(void __iomem *ioaddr)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
index 19e0f4eed2b..f83210e7c22 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac100_core.c
@@ -72,9 +72,9 @@ static int dwmac100_rx_ipc_enable(void __iomem *ioaddr)
 	return 0;
 }
 
-static void dwmac100_irq_status(void __iomem *ioaddr)
+static int dwmac100_irq_status(void __iomem *ioaddr)
 {
-	return;
+	return 0;
 }
 
 static void dwmac100_set_umac_addr(void __iomem *ioaddr, unsigned char *addr,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
index 6e0360f9cfd..e678ce39d01 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
@@ -70,6 +70,7 @@
 #define DMA_INTR_DEFAULT_MASK	(DMA_INTR_NORMAL | DMA_INTR_ABNORMAL)
 
 /* DMA Status register defines */
+#define DMA_STATUS_GLPII	0x40000000	/* GMAC LPI interrupt */
 #define DMA_STATUS_GPI		0x10000000	/* PMT interrupt */
 #define DMA_STATUS_GMI		0x08000000	/* MMC interrupt */
 #define DMA_STATUS_GLI		0x04000000	/* GMAC Line interface int */
diff --git a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
index fb8377da168..4b785e10f2e 100644
--- a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
+++ b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
@@ -51,7 +51,7 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
 		priv->hw->desc->prepare_tx_desc(desc, 1, bmax,
 						csum);
-
+		wmb();
 		entry = (++priv->cur_tx) % txsize;
 		desc = priv->dma_tx + entry;
 
@@ -59,6 +59,7 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 					    len, DMA_TO_DEVICE);
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
 		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum);
+		wmb();
 		priv->hw->desc->set_tx_owner(desc);
 		priv->tx_skbuff[entry] = NULL;
 	} else {
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index dc20c56efc9..f2d3665430a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -82,11 +82,15 @@ struct stmmac_priv {
 	struct stmmac_counters mmc;
 	struct dma_features dma_cap;
 	int hw_cap_support;
-#ifdef CONFIG_HAVE_CLK
 	struct clk *stmmac_clk;
-#endif
 	int clk_csr;
 	int synopsys_id;
+	struct timer_list eee_ctrl_timer;
+	bool tx_path_in_lpi_mode;
+	int lpi_irq;
+	int eee_enabled;
+	int eee_active;
+	int tx_lpi_timer;
 };
 
 extern int phyaddr;
@@ -104,46 +108,8 @@ int stmmac_dvr_remove(struct net_device *ndev);
 struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 				     struct plat_stmmacenet_data *plat_dat,
 				     void __iomem *addr);
-
-#ifdef CONFIG_HAVE_CLK
-static inline int stmmac_clk_enable(struct stmmac_priv *priv)
-{
-	if (!IS_ERR(priv->stmmac_clk))
-		return clk_prepare_enable(priv->stmmac_clk);
-
-	return 0;
-}
-
-static inline void stmmac_clk_disable(struct stmmac_priv *priv)
-{
-	if (IS_ERR(priv->stmmac_clk))
-		return;
-
-	clk_disable_unprepare(priv->stmmac_clk);
-}
-static inline int stmmac_clk_get(struct stmmac_priv *priv)
-{
-	priv->stmmac_clk = clk_get(priv->device, NULL);
-
-	if (IS_ERR(priv->stmmac_clk))
-		return PTR_ERR(priv->stmmac_clk);
-
-	return 0;
-}
-#else
-static inline int stmmac_clk_enable(struct stmmac_priv *priv)
-{
-	return 0;
-}
-static inline void stmmac_clk_disable(struct stmmac_priv *priv)
-{
-}
-static inline int stmmac_clk_get(struct stmmac_priv *priv)
-{
-	return 0;
-}
-#endif /* CONFIG_HAVE_CLK */
-
+void stmmac_disable_eee_mode(struct stmmac_priv *priv);
+bool stmmac_eee_init(struct stmmac_priv *priv);
 
 #ifdef CONFIG_STMMAC_PLATFORM
 extern struct platform_driver stmmac_pltfr_driver;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index ce431846fc6..76fd61aa005 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -93,6 +93,16 @@ static const struct stmmac_stats stmmac_gstrings_stats[] = {
 	STMMAC_STAT(poll_n),
 	STMMAC_STAT(sched_timer_n),
 	STMMAC_STAT(normal_irq_n),
+	STMMAC_STAT(normal_irq_n),
+	STMMAC_STAT(mmc_tx_irq_n),
+	STMMAC_STAT(mmc_rx_irq_n),
+	STMMAC_STAT(mmc_rx_csum_offload_irq_n),
+	STMMAC_STAT(irq_receive_pmt_irq_n),
+	STMMAC_STAT(irq_tx_path_in_lpi_mode_n),
+	STMMAC_STAT(irq_tx_path_exit_lpi_mode_n),
+	STMMAC_STAT(irq_rx_path_in_lpi_mode_n),
+	STMMAC_STAT(irq_rx_path_exit_lpi_mode_n),
+	STMMAC_STAT(phy_eee_wakeup_error_n),
 };
 #define STMMAC_STATS_LEN ARRAY_SIZE(stmmac_gstrings_stats)
 
@@ -366,6 +376,11 @@ static void stmmac_get_ethtool_stats(struct net_device *dev,
 					     (*(u32 *)p);
 			}
 		}
+		if (priv->eee_enabled) {
+			int val = phy_get_eee_err(priv->phydev);
+			if (val)
+				priv->xstats.phy_eee_wakeup_error_n = val;
+		}
 	}
 	for (i = 0; i < STMMAC_STATS_LEN; i++) {
 		char *p = (char *)priv + stmmac_gstrings_stats[i].stat_offset;
@@ -464,6 +479,46 @@ static int stmmac_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 	return 0;
 }
 
+static int stmmac_ethtool_op_get_eee(struct net_device *dev,
+				     struct ethtool_eee *edata)
+{
+	struct stmmac_priv *priv = netdev_priv(dev);
+
+	if (!priv->dma_cap.eee)
+		return -EOPNOTSUPP;
+
+	edata->eee_enabled = priv->eee_enabled;
+	edata->eee_active = priv->eee_active;
+	edata->tx_lpi_timer = priv->tx_lpi_timer;
+
+	return phy_ethtool_get_eee(priv->phydev, edata);
+}
+
+static int stmmac_ethtool_op_set_eee(struct net_device *dev,
+				     struct ethtool_eee *edata)
+{
+	struct stmmac_priv *priv = netdev_priv(dev);
+
+	priv->eee_enabled = edata->eee_enabled;
+
+	if (!priv->eee_enabled)
+		stmmac_disable_eee_mode(priv);
+	else {
+		/* We are asking for enabling the EEE but it is safe
+		 * to verify all by invoking the eee_init function.
+		 * In case of failure it will return an error.
+		 */
+		priv->eee_enabled = stmmac_eee_init(priv);
+		if (!priv->eee_enabled)
+			return -EOPNOTSUPP;
+
+		/* Do not change tx_lpi_timer in case of failure */
+		priv->tx_lpi_timer = edata->tx_lpi_timer;
+	}
+
+	return phy_ethtool_set_eee(priv->phydev, edata);
+}
+
 static const struct ethtool_ops stmmac_ethtool_ops = {
 	.begin = stmmac_check_if_running,
 	.get_drvinfo = stmmac_ethtool_getdrvinfo,
@@ -480,6 +535,8 @@ static const struct ethtool_ops stmmac_ethtool_ops = {
 	.get_strings = stmmac_get_strings,
 	.get_wol = stmmac_get_wol,
 	.set_wol = stmmac_set_wol,
+	.get_eee = stmmac_ethtool_op_get_eee,
+	.set_eee = stmmac_ethtool_op_set_eee,
 	.get_sset_count	= stmmac_get_sset_count,
 	.get_ts_info = ethtool_op_get_ts_info,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 51b3b68528e..fd8882f9602 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -28,6 +28,7 @@
 	https://bugzilla.stlinux.com/
 *******************************************************************************/
 
+#include <linux/clk.h>
 #include <linux/kernel.h>
 #include <linux/interrupt.h>
 #include <linux/ip.h>
@@ -133,6 +134,12 @@ static const u32 default_msg_level = (NETIF_MSG_DRV | NETIF_MSG_PROBE |
 				      NETIF_MSG_LINK | NETIF_MSG_IFUP |
 				      NETIF_MSG_IFDOWN | NETIF_MSG_TIMER);
 
+#define STMMAC_DEFAULT_LPI_TIMER	1000
+static int eee_timer = STMMAC_DEFAULT_LPI_TIMER;
+module_param(eee_timer, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(eee_timer, "LPI tx expiration time in msec");
+#define STMMAC_LPI_TIMER(x) (jiffies + msecs_to_jiffies(x))
+
 static irqreturn_t stmmac_interrupt(int irq, void *dev_id);
 
 #ifdef CONFIG_STMMAC_DEBUG_FS
@@ -161,16 +168,14 @@ static void stmmac_verify_args(void)
 		flow_ctrl = FLOW_OFF;
 	if (unlikely((pause < 0) || (pause > 0xffff)))
 		pause = PAUSE_TIME;
+	if (eee_timer < 0)
+		eee_timer = STMMAC_DEFAULT_LPI_TIMER;
 }
 
 static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 {
-#ifdef CONFIG_HAVE_CLK
 	u32 clk_rate;
 
-	if (IS_ERR(priv->stmmac_clk))
-		return;
-
 	clk_rate = clk_get_rate(priv->stmmac_clk);
 
 	/* Platform provided default clk_csr would be assumed valid
@@ -192,7 +197,6 @@ static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 	   * we can not estimate the proper divider as it is not known
 	   * the frequency of clk_csr_i. So we do not change the default
 	   * divider. */
-#endif
 }
 
 #if defined(STMMAC_XMIT_DEBUG) || defined(STMMAC_RX_DEBUG)
@@ -229,6 +233,85 @@ static inline void stmmac_hw_fix_mac_speed(struct stmmac_priv *priv)
 					  phydev->speed);
 }
 
+static void stmmac_enable_eee_mode(struct stmmac_priv *priv)
+{
+	/* Check and enter in LPI mode */
+	if ((priv->dirty_tx == priv->cur_tx) &&
+	    (priv->tx_path_in_lpi_mode == false))
+		priv->hw->mac->set_eee_mode(priv->ioaddr);
+}
+
+void stmmac_disable_eee_mode(struct stmmac_priv *priv)
+{
+	/* Exit and disable EEE in case of we are are in LPI state. */
+	priv->hw->mac->reset_eee_mode(priv->ioaddr);
+	del_timer_sync(&priv->eee_ctrl_timer);
+	priv->tx_path_in_lpi_mode = false;
+}
+
+/**
+ * stmmac_eee_ctrl_timer
+ * @arg : data hook
+ * Description:
+ *  If there is no data transfer and if we are not in LPI state,
+ *  then MAC Transmitter can be moved to LPI state.
+ */
+static void stmmac_eee_ctrl_timer(unsigned long arg)
+{
+	struct stmmac_priv *priv = (struct stmmac_priv *)arg;
+
+	stmmac_enable_eee_mode(priv);
+	mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_TIMER(eee_timer));
+}
+
+/**
+ * stmmac_eee_init
+ * @priv: private device pointer
+ * Description:
+ *  If the EEE support has been enabled while configuring the driver,
+ *  if the GMAC actually supports the EEE (from the HW cap reg) and the
+ *  phy can also manage EEE, so enable the LPI state and start the timer
+ *  to verify if the tx path can enter in LPI state.
+ */
+bool stmmac_eee_init(struct stmmac_priv *priv)
+{
+	bool ret = false;
+
+	/* MAC core supports the EEE feature. */
+	if (priv->dma_cap.eee) {
+		/* Check if the PHY supports EEE */
+		if (phy_init_eee(priv->phydev, 1))
+			goto out;
+
+		priv->eee_active = 1;
+		init_timer(&priv->eee_ctrl_timer);
+		priv->eee_ctrl_timer.function = stmmac_eee_ctrl_timer;
+		priv->eee_ctrl_timer.data = (unsigned long)priv;
+		priv->eee_ctrl_timer.expires = STMMAC_LPI_TIMER(eee_timer);
+		add_timer(&priv->eee_ctrl_timer);
+
+		priv->hw->mac->set_eee_timer(priv->ioaddr,
+					     STMMAC_DEFAULT_LIT_LS_TIMER,
+					     priv->tx_lpi_timer);
+
+		pr_info("stmmac: Energy-Efficient Ethernet initialized\n");
+
+		ret = true;
+	}
+out:
+	return ret;
+}
+
+static void stmmac_eee_adjust(struct stmmac_priv *priv)
+{
+	/* When the EEE has been already initialised we have to
+	 * modify the PLS bit in the LPI ctrl & status reg according
+	 * to the PHY link status. For this reason.
+	 */
+	if (priv->eee_enabled)
+		priv->hw->mac->set_eee_pls(priv->ioaddr, priv->phydev->link);
+}
+
 /**
  * stmmac_adjust_link
  * @dev: net device structure
@@ -249,6 +332,7 @@ static void stmmac_adjust_link(struct net_device *dev)
 	    phydev->addr, phydev->link);
 
 	spin_lock_irqsave(&priv->lock, flags);
+
 	if (phydev->link) {
 		u32 ctrl = readl(priv->ioaddr + MAC_CTRL_REG);
 
@@ -315,6 +399,8 @@ static void stmmac_adjust_link(struct net_device *dev)
 	if (new_state && netif_msg_link(priv))
 		phy_print_status(phydev);
 
+	stmmac_eee_adjust(priv);
+
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	DBG(probe, DEBUG, "stmmac_adjust_link: exiting\n");
@@ -332,7 +418,7 @@ static int stmmac_init_phy(struct net_device *dev)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 	struct phy_device *phydev;
-	char phy_id[MII_BUS_ID_SIZE + 3];
+	char phy_id_fmt[MII_BUS_ID_SIZE + 3];
 	char bus_id[MII_BUS_ID_SIZE];
 	int interface = priv->plat->interface;
 	priv->oldlink = 0;
@@ -346,11 +432,12 @@ static int stmmac_init_phy(struct net_device *dev)
 		snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x",
 				priv->plat->bus_id);
 
-	snprintf(phy_id, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
+	snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
 		 priv->plat->phy_addr);
-	pr_debug("stmmac_init_phy:  trying to attach to %s\n", phy_id);
+	pr_debug("stmmac_init_phy:  trying to attach to %s\n", phy_id_fmt);
 
-	phydev = phy_connect(dev, phy_id, &stmmac_adjust_link, 0, interface);
+	phydev = phy_connect(dev, phy_id_fmt, &stmmac_adjust_link, 0,
+			     interface);
 
 	if (IS_ERR(phydev)) {
 		pr_err("%s: Could not attach to PHY\n", dev->name);
@@ -677,7 +764,7 @@ static void stmmac_tx(struct stmmac_priv *priv)
 
 		priv->hw->desc->release_tx_desc(p);
 
-		entry = (++priv->dirty_tx) % txsize;
+		priv->dirty_tx++;
 	}
 	if (unlikely(netif_queue_stopped(priv->dev) &&
 		     stmmac_tx_avail(priv) > STMMAC_TX_THRESH(priv))) {
@@ -689,6 +776,11 @@ static void stmmac_tx(struct stmmac_priv *priv)
 		}
 		netif_tx_unlock(priv->dev);
 	}
+
+	if ((priv->eee_enabled) && (!priv->tx_path_in_lpi_mode)) {
+		stmmac_enable_eee_mode(priv);
+		mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_TIMER(eee_timer));
+	}
 	spin_unlock(&priv->tx_lock);
 }
 
@@ -974,7 +1066,7 @@ static int stmmac_open(struct net_device *dev)
 	} else
 		priv->tm->enable = 1;
 #endif
-	stmmac_clk_enable(priv);
+	clk_enable(priv->stmmac_clk);
 
 	stmmac_check_ether_addr(priv);
 
@@ -1027,6 +1119,17 @@ static int stmmac_open(struct net_device *dev)
 		}
 	}
 
+	/* Request the IRQ lines */
+	if (priv->lpi_irq != -ENXIO) {
+		ret = request_irq(priv->lpi_irq, stmmac_interrupt, IRQF_SHARED,
+				  dev->name, dev);
+		if (unlikely(ret < 0)) {
+			pr_err("%s: ERROR: allocating the LPI IRQ %d (%d)\n",
+			       __func__, priv->lpi_irq, ret);
+			goto open_error_lpiirq;
+		}
+	}
+
 	/* Enable the MAC Rx/Tx */
 	stmmac_set_mac(priv->ioaddr, true);
 
@@ -1062,12 +1165,19 @@ static int stmmac_open(struct net_device *dev)
 	if (priv->phydev)
 		phy_start(priv->phydev);
 
+	priv->tx_lpi_timer = STMMAC_DEFAULT_TWT_LS_TIMER;
+	priv->eee_enabled = stmmac_eee_init(priv);
+
 	napi_enable(&priv->napi);
 	skb_queue_head_init(&priv->rx_recycle);
 	netif_start_queue(dev);
 
 	return 0;
 
+open_error_lpiirq:
+	if (priv->wol_irq != dev->irq)
+		free_irq(priv->wol_irq, dev);
+
 open_error_wolirq:
 	free_irq(dev->irq, dev);
 
@@ -1078,7 +1188,7 @@ open_error:
 	if (priv->phydev)
 		phy_disconnect(priv->phydev);
 
-	stmmac_clk_disable(priv);
+	clk_disable(priv->stmmac_clk);
 
 	return ret;
 }
@@ -1093,6 +1203,9 @@ static int stmmac_release(struct net_device *dev)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
 
+	if (priv->eee_enabled)
+		del_timer_sync(&priv->eee_ctrl_timer);
+
 	/* Stop and disconnect the PHY */
 	if (priv->phydev) {
 		phy_stop(priv->phydev);
@@ -1115,6 +1228,8 @@ static int stmmac_release(struct net_device *dev)
 	free_irq(dev->irq, dev);
 	if (priv->wol_irq != dev->irq)
 		free_irq(priv->wol_irq, dev);
+	if (priv->lpi_irq != -ENXIO)
+		free_irq(priv->lpi_irq, dev);
 
 	/* Stop TX/RX DMA and clear the descriptors */
 	priv->hw->dma->stop_tx(priv->ioaddr);
@@ -1131,7 +1246,7 @@ static int stmmac_release(struct net_device *dev)
 #ifdef CONFIG_STMMAC_DEBUG_FS
 	stmmac_exit_fs();
 #endif
-	stmmac_clk_disable(priv);
+	clk_disable(priv->stmmac_clk);
 
 	return 0;
 }
@@ -1164,6 +1279,9 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	spin_lock(&priv->tx_lock);
 
+	if (priv->tx_path_in_lpi_mode)
+		stmmac_disable_eee_mode(priv);
+
 	entry = priv->cur_tx % txsize;
 
 #ifdef STMMAC_XMIT_DEBUG
@@ -1212,6 +1330,7 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum_insertion);
 		wmb();
 		priv->hw->desc->set_tx_owner(desc);
+		wmb();
 	}
 
 	/* Interrupt on completition only for the latest segment */
@@ -1227,6 +1346,7 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* To avoid raise condition */
 	priv->hw->desc->set_tx_owner(first);
+	wmb();
 
 	priv->cur_tx++;
 
@@ -1290,6 +1410,7 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv)
 		}
 		wmb();
 		priv->hw->desc->set_rx_owner(p + entry);
+		wmb();
 	}
 }
 
@@ -1308,7 +1429,6 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit)
 		display_ring(priv->dma_rx, rxsize);
 	}
 #endif
-	count = 0;
 	while (!priv->hw->desc->get_rx_owner(p)) {
 		int status;
 
@@ -1541,10 +1661,37 @@ static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
 		return IRQ_NONE;
 	}
 
-	if (priv->plat->has_gmac)
-		/* To handle GMAC own interrupts */
-		priv->hw->mac->host_irq_status((void __iomem *) dev->base_addr);
+	/* To handle GMAC own interrupts */
+	if (priv->plat->has_gmac) {
+		int status = priv->hw->mac->host_irq_status((void __iomem *)
+							    dev->base_addr);
+		if (unlikely(status)) {
+			if (status & core_mmc_tx_irq)
+				priv->xstats.mmc_tx_irq_n++;
+			if (status & core_mmc_rx_irq)
+				priv->xstats.mmc_rx_irq_n++;
+			if (status & core_mmc_rx_csum_offload_irq)
+				priv->xstats.mmc_rx_csum_offload_irq_n++;
+			if (status & core_irq_receive_pmt_irq)
+				priv->xstats.irq_receive_pmt_irq_n++;
+
+			/* For LPI we need to save the tx status */
+			if (status & core_irq_tx_path_in_lpi_mode) {
+				priv->xstats.irq_tx_path_in_lpi_mode_n++;
+				priv->tx_path_in_lpi_mode = true;
+			}
+			if (status & core_irq_tx_path_exit_lpi_mode) {
+				priv->xstats.irq_tx_path_exit_lpi_mode_n++;
+				priv->tx_path_in_lpi_mode = false;
+			}
+			if (status & core_irq_rx_path_in_lpi_mode)
+				priv->xstats.irq_rx_path_in_lpi_mode_n++;
+			if (status & core_irq_rx_path_exit_lpi_mode)
+				priv->xstats.irq_rx_path_exit_lpi_mode_n++;
+		}
+	}
 
+	/* To handle DMA interrupts */
 	stmmac_dma_interrupt(priv);
 
 	return IRQ_HANDLED;
@@ -1927,11 +2074,14 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	ret = register_netdev(ndev);
 	if (ret) {
 		pr_err("%s: ERROR %i registering the device\n", __func__, ret);
-		goto error;
+		goto error_netdev_register;
 	}
 
-	if (stmmac_clk_get(priv))
+	priv->stmmac_clk = clk_get(priv->device, NULL);
+	if (IS_ERR(priv->stmmac_clk)) {
 		pr_warning("%s: warning: cannot get CSR clock\n", __func__);
+		goto error_clk_get;
+	}
 
 	/* If a specific clk_csr value is passed from the platform
 	 * this means that the CSR Clock Range selection cannot be
@@ -1949,15 +2099,17 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	if (ret < 0) {
 		pr_debug("%s: MDIO bus (id: %d) registration failed",
 			 __func__, priv->plat->bus_id);
-		goto error;
+		goto error_mdio_register;
 	}
 
 	return priv;
 
-error:
-	netif_napi_del(&priv->napi);
-
+error_mdio_register:
+	clk_put(priv->stmmac_clk);
+error_clk_get:
 	unregister_netdev(ndev);
+error_netdev_register:
+	netif_napi_del(&priv->napi);
 	free_netdev(ndev);
 
 	return NULL;
@@ -2026,7 +2178,7 @@ int stmmac_suspend(struct net_device *ndev)
 	else {
 		stmmac_set_mac(priv->ioaddr, false);
 		/* Disable clock in case of PWM is off */
-		stmmac_clk_disable(priv);
+		clk_disable(priv->stmmac_clk);
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
 	return 0;
@@ -2051,7 +2203,7 @@ int stmmac_resume(struct net_device *ndev)
 		priv->hw->mac->pmt(priv->ioaddr, 0);
 	else
 		/* enable the clk prevously disabled */
-		stmmac_clk_enable(priv);
+		clk_enable(priv->stmmac_clk);
 
 	netif_device_attach(ndev);
 
@@ -2130,42 +2282,38 @@ static int __init stmmac_cmdline_opt(char *str)
 		return -EINVAL;
 	while ((opt = strsep(&str, ",")) != NULL) {
 		if (!strncmp(opt, "debug:", 6)) {
-			if (strict_strtoul(opt + 6, 0, (unsigned long *)&debug))
+			if (kstrtoint(opt + 6, 0, &debug))
 				goto err;
 		} else if (!strncmp(opt, "phyaddr:", 8)) {
-			if (strict_strtoul(opt + 8, 0,
-					   (unsigned long *)&phyaddr))
+			if (kstrtoint(opt + 8, 0, &phyaddr))
 				goto err;
 		} else if (!strncmp(opt, "dma_txsize:", 11)) {
-			if (strict_strtoul(opt + 11, 0,
-					   (unsigned long *)&dma_txsize))
+			if (kstrtoint(opt + 11, 0, &dma_txsize))
 				goto err;
 		} else if (!strncmp(opt, "dma_rxsize:", 11)) {
-			if (strict_strtoul(opt + 11, 0,
-					   (unsigned long *)&dma_rxsize))
+			if (kstrtoint(opt + 11, 0, &dma_rxsize))
 				goto err;
 		} else if (!strncmp(opt, "buf_sz:", 7)) {
-			if (strict_strtoul(opt + 7, 0,
-					   (unsigned long *)&buf_sz))
+			if (kstrtoint(opt + 7, 0, &buf_sz))
 				goto err;
 		} else if (!strncmp(opt, "tc:", 3)) {
-			if (strict_strtoul(opt + 3, 0, (unsigned long *)&tc))
+			if (kstrtoint(opt + 3, 0, &tc))
 				goto err;
 		} else if (!strncmp(opt, "watchdog:", 9)) {
-			if (strict_strtoul(opt + 9, 0,
-					   (unsigned long *)&watchdog))
+			if (kstrtoint(opt + 9, 0, &watchdog))
 				goto err;
 		} else if (!strncmp(opt, "flow_ctrl:", 10)) {
-			if (strict_strtoul(opt + 10, 0,
-					   (unsigned long *)&flow_ctrl))
+			if (kstrtoint(opt + 10, 0, &flow_ctrl))
 				goto err;
 		} else if (!strncmp(opt, "pause:", 6)) {
-			if (strict_strtoul(opt + 6, 0, (unsigned long *)&pause))
+			if (kstrtoint(opt + 6, 0, &pause))
+				goto err;
+		} else if (!strncmp(opt, "eee_timer:", 6)) {
+			if (kstrtoint(opt + 10, 0, &eee_timer))
 				goto err;
 #ifdef CONFIG_STMMAC_TIMER
 		} else if (!strncmp(opt, "tmrate:", 7)) {
-			if (strict_strtoul(opt + 7, 0,
-					   (unsigned long *)&tmrate))
+			if (kstrtoint(opt + 7, 0, &tmrate))
 				goto err;
 #endif
 		}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
index cf826e6b6aa..13afb8edfad 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
@@ -125,7 +125,7 @@ err_out_req_reg_failed:
 }
 
 /**
- * stmmac_dvr_remove
+ * stmmac_pci_remove
  *
  * @pdev: platform device pointer
  * Description: this function calls the main to free the net resources
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 680d2b8dfe2..cd01ee7ecef 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -49,7 +49,9 @@ static int __devinit stmmac_probe_config_dt(struct platform_device *pdev,
 	 * are provided. All other properties should be added
 	 * once needed on other platforms.
 	 */
-	if (of_device_is_compatible(np, "st,spear600-gmac")) {
+	if (of_device_is_compatible(np, "st,spear600-gmac") ||
+		of_device_is_compatible(np, "snps,dwmac-3.70a") ||
+		of_device_is_compatible(np, "snps,dwmac")) {
 		plat->has_gmac = 1;
 		plat->pmt = 1;
 	}
@@ -156,6 +158,8 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 	if (priv->wol_irq == -ENXIO)
 		priv->wol_irq = priv->dev->irq;
 
+	priv->lpi_irq = platform_get_irq_byname(pdev, "eth_lpi");
+
 	platform_set_drvdata(pdev, priv->dev);
 
 	pr_debug("STMMAC platform driver registration completed");
@@ -190,7 +194,7 @@ static int stmmac_pltfr_remove(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, NULL);
 
-	iounmap((void *)priv->ioaddr);
+	iounmap((void __force __iomem *)priv->ioaddr);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
 
@@ -250,7 +254,9 @@ static const struct dev_pm_ops stmmac_pltfr_pm_ops;
 #endif /* CONFIG_PM */
 
 static const struct of_device_id stmmac_dt_ids[] = {
-	{ .compatible = "st,spear600-gmac", },
+	{ .compatible = "st,spear600-gmac"},
+	{ .compatible = "snps,dwmac-3.70a"},
+	{ .compatible = "snps,dwmac"},
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, stmmac_dt_ids);
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 8c726b7004d..c2a0fe39326 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -3335,6 +3335,10 @@ static int niu_rbr_add_page(struct niu *np, struct rx_ring_info *rp,
 
 	addr = np->ops->map_page(np->device, page, 0,
 				 PAGE_SIZE, DMA_FROM_DEVICE);
+	if (!addr) {
+		__free_page(page);
+		return -ENOMEM;
+	}
 
 	niu_hash_page(rp, page, addr);
 	if (rp->rbr_blocks_per_page > 1)
@@ -3513,7 +3517,7 @@ static int niu_rbr_fill(struct niu *np, struct rx_ring_info *rp, gfp_t mask)
 	err = 0;
 	while (index < (rp->rbr_table_size - blocks_per_page)) {
 		err = niu_rbr_add_page(np, rp, mask, index);
-		if (err)
+		if (unlikely(err))
 			break;
 
 		index += blocks_per_page;
diff --git a/drivers/net/ethernet/sun/sunbmac.c b/drivers/net/ethernet/sun/sunbmac.c
index 2a83fc57edb..967fe8cb476 100644
--- a/drivers/net/ethernet/sun/sunbmac.c
+++ b/drivers/net/ethernet/sun/sunbmac.c
@@ -233,7 +233,6 @@ static void bigmac_init_rings(struct bigmac *bp, int from_irq)
 			continue;
 
 		bp->rx_skbs[i] = skb;
-		skb->dev = dev;
 
 		/* Because we reserve afterwards. */
 		skb_put(skb, ETH_FRAME_LEN);
@@ -838,7 +837,6 @@ static void bigmac_rx(struct bigmac *bp)
 					 RX_BUF_ALLOC_SIZE - 34,
 					 DMA_FROM_DEVICE);
 			bp->rx_skbs[elem] = new_skb;
-			new_skb->dev = bp->dev;
 			skb_put(new_skb, ETH_FRAME_LEN);
 			skb_reserve(new_skb, 34);
 			this->rx_addr =
diff --git a/drivers/net/ethernet/sun/sungem.c b/drivers/net/ethernet/sun/sungem.c
index 3cf4ab75583..9ae12d0c963 100644
--- a/drivers/net/ethernet/sun/sungem.c
+++ b/drivers/net/ethernet/sun/sungem.c
@@ -752,7 +752,6 @@ static __inline__ struct sk_buff *gem_alloc_skb(struct net_device *dev, int size
 	if (likely(skb)) {
 		unsigned long offset = ALIGNED_RX_SKB_ADDR(skb->data);
 		skb_reserve(skb, offset);
-		skb->dev = dev;
 	}
 	return skb;
 }
diff --git a/drivers/net/ethernet/sun/sunhme.c b/drivers/net/ethernet/sun/sunhme.c
index dfc00c4683e..73f341b8bef 100644
--- a/drivers/net/ethernet/sun/sunhme.c
+++ b/drivers/net/ethernet/sun/sunhme.c
@@ -1249,7 +1249,6 @@ static void happy_meal_clean_rings(struct happy_meal *hp)
 static void happy_meal_init_rings(struct happy_meal *hp)
 {
 	struct hmeal_init_block *hb = hp->happy_block;
-	struct net_device *dev = hp->dev;
 	int i;
 
 	HMD(("happy_meal_init_rings: counters to zero, "));
@@ -1270,7 +1269,6 @@ static void happy_meal_init_rings(struct happy_meal *hp)
 			continue;
 		}
 		hp->rx_skbs[i] = skb;
-		skb->dev = dev;
 
 		/* Because we reserve afterwards. */
 		skb_put(skb, (ETH_FRAME_LEN + RX_OFFSET + 4));
@@ -2031,7 +2029,6 @@ static void happy_meal_rx(struct happy_meal *hp, struct net_device *dev)
 			}
 			dma_unmap_single(hp->dma_dev, dma_addr, RX_BUF_ALLOC_SIZE, DMA_FROM_DEVICE);
 			hp->rx_skbs[elem] = new_skb;
-			new_skb->dev = dev;
 			skb_put(new_skb, (ETH_FRAME_LEN + RX_OFFSET + 4));
 			hme_write_rxd(hp, this,
 				      (RXFLAG_OWN|((RX_BUF_ALLOC_SIZE-RX_OFFSET)<<16)),
diff --git a/drivers/net/ethernet/sun/sunqe.c b/drivers/net/ethernet/sun/sunqe.c
index 7d4a040d84a..aeded7ff1c8 100644
--- a/drivers/net/ethernet/sun/sunqe.c
+++ b/drivers/net/ethernet/sun/sunqe.c
@@ -441,7 +441,7 @@ static void qe_rx(struct sunqe *qep)
 			} else {
 				skb_reserve(skb, 2);
 				skb_put(skb, len);
-				skb_copy_to_linear_data(skb, (unsigned char *) this_qbuf,
+				skb_copy_to_linear_data(skb, this_qbuf,
 						 len);
 				skb->protocol = eth_type_trans(skb, qep->dev);
 				netif_rx(skb);
diff --git a/drivers/net/ethernet/tehuti/tehuti.c b/drivers/net/ethernet/tehuti/tehuti.c
index 447a6932cab..6ce9edd95c0 100644
--- a/drivers/net/ethernet/tehuti/tehuti.c
+++ b/drivers/net/ethernet/tehuti/tehuti.c
@@ -137,14 +137,15 @@ static void print_eth_id(struct net_device *ndev)
 #define bdx_disable_interrupts(priv)	\
 	do { WRITE_REG(priv, regIMR, 0); } while (0)
 
-/* bdx_fifo_init
- * create TX/RX descriptor fifo for host-NIC communication.
+/**
+ * bdx_fifo_init - create TX/RX descriptor fifo for host-NIC communication.
+ * @priv: NIC private structure
+ * @f: fifo to initialize
+ * @fsz_type: fifo size type: 0-4KB, 1-8KB, 2-16KB, 3-32KB
+ * @reg_XXX: offsets of registers relative to base address
+ *
  * 1K extra space is allocated at the end of the fifo to simplify
  * processing of descriptors that wraps around fifo's end
- * @priv - NIC private structure
- * @f - fifo to initialize
- * @fsz_type - fifo size type: 0-4KB, 1-8KB, 2-16KB, 3-32KB
- * @reg_XXX - offsets of registers relative to base address
  *
  * Returns 0 on success, negative value on failure
  *
@@ -177,9 +178,10 @@ bdx_fifo_init(struct bdx_priv *priv, struct fifo *f, int fsz_type,
 	RET(0);
 }
 
-/* bdx_fifo_free - free all resources used by fifo
- * @priv - NIC private structure
- * @f - fifo to release
+/**
+ * bdx_fifo_free - free all resources used by fifo
+ * @priv: NIC private structure
+ * @f: fifo to release
  */
 static void bdx_fifo_free(struct bdx_priv *priv, struct fifo *f)
 {
@@ -192,9 +194,9 @@ static void bdx_fifo_free(struct bdx_priv *priv, struct fifo *f)
 	RET();
 }
 
-/*
+/**
  * bdx_link_changed - notifies OS about hw link state.
- * @bdx_priv - hw adapter structure
+ * @priv: hw adapter structure
  */
 static void bdx_link_changed(struct bdx_priv *priv)
 {
@@ -233,10 +235,10 @@ static void bdx_isr_extra(struct bdx_priv *priv, u32 isr)
 
 }
 
-/* bdx_isr - Interrupt Service Routine for Bordeaux NIC
- * @irq - interrupt number
- * @ndev - network device
- * @regs - CPU registers
+/**
+ * bdx_isr_napi - Interrupt Service Routine for Bordeaux NIC
+ * @irq: interrupt number
+ * @dev: network device
  *
  * Return IRQ_NONE if it was not our interrupt, IRQ_HANDLED - otherwise
  *
@@ -307,8 +309,10 @@ static int bdx_poll(struct napi_struct *napi, int budget)
 	return work_done;
 }
 
-/* bdx_fw_load - loads firmware to NIC
- * @priv - NIC private structure
+/**
+ * bdx_fw_load - loads firmware to NIC
+ * @priv: NIC private structure
+ *
  * Firmware is loaded via TXD fifo, so it must be initialized first.
  * Firware must be loaded once per NIC not per PCI device provided by NIC (NIC
  * can have few of them). So all drivers use semaphore register to choose one
@@ -380,8 +384,9 @@ static void bdx_restore_mac(struct net_device *ndev, struct bdx_priv *priv)
 	RET();
 }
 
-/* bdx_hw_start - inits registers and starts HW's Rx and Tx engines
- * @priv - NIC private structure
+/**
+ * bdx_hw_start - inits registers and starts HW's Rx and Tx engines
+ * @priv: NIC private structure
  */
 static int bdx_hw_start(struct bdx_priv *priv)
 {
@@ -691,12 +696,13 @@ static int bdx_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
 		RET(-EOPNOTSUPP);
 }
 
-/*
+/**
  * __bdx_vlan_rx_vid - private helper for adding/killing VLAN vid
- *                     by passing VLAN filter table to hardware
- * @ndev network device
- * @vid  VLAN vid
- * @op   add or kill operation
+ * @ndev: network device
+ * @vid:  VLAN vid
+ * @op:   add or kill operation
+ *
+ * Passes VLAN filter table to hardware
  */
 static void __bdx_vlan_rx_vid(struct net_device *ndev, uint16_t vid, int enable)
 {
@@ -722,10 +728,10 @@ static void __bdx_vlan_rx_vid(struct net_device *ndev, uint16_t vid, int enable)
 	RET();
 }
 
-/*
+/**
  * bdx_vlan_rx_add_vid - kernel hook for adding VLAN vid to hw filtering table
- * @ndev network device
- * @vid  VLAN vid to add
+ * @ndev: network device
+ * @vid:  VLAN vid to add
  */
 static int bdx_vlan_rx_add_vid(struct net_device *ndev, uint16_t vid)
 {
@@ -733,10 +739,10 @@ static int bdx_vlan_rx_add_vid(struct net_device *ndev, uint16_t vid)
 	return 0;
 }
 
-/*
+/**
  * bdx_vlan_rx_kill_vid - kernel hook for killing VLAN vid in hw filtering table
- * @ndev network device
- * @vid  VLAN vid to kill
+ * @ndev: network device
+ * @vid:  VLAN vid to kill
  */
 static int bdx_vlan_rx_kill_vid(struct net_device *ndev, unsigned short vid)
 {
@@ -974,8 +980,9 @@ static inline void bdx_rxdb_free_elem(struct rxdb *db, int n)
  *     Rx Init                                                           *
  *************************************************************************/
 
-/* bdx_rx_init - initialize RX all related HW and SW resources
- * @priv - NIC private structure
+/**
+ * bdx_rx_init - initialize RX all related HW and SW resources
+ * @priv: NIC private structure
  *
  * Returns 0 on success, negative value on failure
  *
@@ -1016,9 +1023,10 @@ err_mem:
 	return -ENOMEM;
 }
 
-/* bdx_rx_free_skbs - frees and unmaps all skbs allocated for the fifo
- * @priv - NIC private structure
- * @f - RXF fifo
+/**
+ * bdx_rx_free_skbs - frees and unmaps all skbs allocated for the fifo
+ * @priv: NIC private structure
+ * @f: RXF fifo
  */
 static void bdx_rx_free_skbs(struct bdx_priv *priv, struct rxf_fifo *f)
 {
@@ -1045,8 +1053,10 @@ static void bdx_rx_free_skbs(struct bdx_priv *priv, struct rxf_fifo *f)
 	}
 }
 
-/* bdx_rx_free - release all Rx resources
- * @priv - NIC private structure
+/**
+ * bdx_rx_free - release all Rx resources
+ * @priv: NIC private structure
+ *
  * It assumes that Rx is desabled in HW
  */
 static void bdx_rx_free(struct bdx_priv *priv)
@@ -1067,9 +1077,11 @@ static void bdx_rx_free(struct bdx_priv *priv)
  *     Rx Engine                                                         *
  *************************************************************************/
 
-/* bdx_rx_alloc_skbs - fill rxf fifo with new skbs
- * @priv - nic's private structure
- * @f - RXF fifo that needs skbs
+/**
+ * bdx_rx_alloc_skbs - fill rxf fifo with new skbs
+ * @priv: nic's private structure
+ * @f: RXF fifo that needs skbs
+ *
  * It allocates skbs, build rxf descs and push it (rxf descr) into rxf fifo.
  * skb's virtual and physical addresses are stored in skb db.
  * To calculate free space, func uses cached values of RPTR and WPTR
@@ -1179,13 +1191,15 @@ static void bdx_recycle_skb(struct bdx_priv *priv, struct rxd_desc *rxdd)
 	RET();
 }
 
-/* bdx_rx_receive - receives full packets from RXD fifo and pass them to OS
+/**
+ * bdx_rx_receive - receives full packets from RXD fifo and pass them to OS
  * NOTE: a special treatment is given to non-continuous descriptors
  * that start near the end, wraps around and continue at the beginning. a second
  * part is copied right after the first, and then descriptor is interpreted as
  * normal. fifo has an extra space to allow such operations
- * @priv - nic's private structure
- * @f - RXF fifo that needs skbs
+ * @priv: nic's private structure
+ * @f: RXF fifo that needs skbs
+ * @budget: maximum number of packets to receive
  */
 
 /* TBD: replace memcpy func call by explicite inline asm */
@@ -1375,9 +1389,10 @@ static inline int bdx_tx_db_size(struct txdb *db)
 	return db->size - taken;
 }
 
-/* __bdx_tx_ptr_next - helper function, increment read/write pointer + wrap
- * @d   - tx data base
- * @ptr - read or write pointer
+/**
+ * __bdx_tx_db_ptr_next - helper function, increment read/write pointer + wrap
+ * @db: tx data base
+ * @pptr: read or write pointer
  */
 static inline void __bdx_tx_db_ptr_next(struct txdb *db, struct tx_map **pptr)
 {
@@ -1394,8 +1409,9 @@ static inline void __bdx_tx_db_ptr_next(struct txdb *db, struct tx_map **pptr)
 		*pptr = db->start;
 }
 
-/* bdx_tx_db_inc_rptr - increment read pointer
- * @d   - tx data base
+/**
+ * bdx_tx_db_inc_rptr - increment read pointer
+ * @db: tx data base
  */
 static inline void bdx_tx_db_inc_rptr(struct txdb *db)
 {
@@ -1403,8 +1419,9 @@ static inline void bdx_tx_db_inc_rptr(struct txdb *db)
 	__bdx_tx_db_ptr_next(db, &db->rptr);
 }
 
-/* bdx_tx_db_inc_rptr - increment write pointer
- * @d   - tx data base
+/**
+ * bdx_tx_db_inc_wptr - increment write pointer
+ * @db: tx data base
  */
 static inline void bdx_tx_db_inc_wptr(struct txdb *db)
 {
@@ -1413,9 +1430,11 @@ static inline void bdx_tx_db_inc_wptr(struct txdb *db)
 						   a result of write */
 }
 
-/* bdx_tx_db_init - creates and initializes tx db
- * @d       - tx data base
- * @sz_type - size of tx fifo
+/**
+ * bdx_tx_db_init - creates and initializes tx db
+ * @d: tx data base
+ * @sz_type: size of tx fifo
+ *
  * Returns 0 on success, error code otherwise
  */
 static int bdx_tx_db_init(struct txdb *d, int sz_type)
@@ -1441,8 +1460,9 @@ static int bdx_tx_db_init(struct txdb *d, int sz_type)
 	return 0;
 }
 
-/* bdx_tx_db_close - closes tx db and frees all memory
- * @d - tx data base
+/**
+ * bdx_tx_db_close - closes tx db and frees all memory
+ * @d: tx data base
  */
 static void bdx_tx_db_close(struct txdb *d)
 {
@@ -1463,9 +1483,11 @@ static struct {
 	u16 qwords;		/* qword = 64 bit */
 } txd_sizes[MAX_SKB_FRAGS + 1];
 
-/* txdb_map_skb - creates and stores dma mappings for skb's data blocks
- * @priv - NIC private structure
- * @skb  - socket buffer to map
+/**
+ * bdx_tx_map_skb - creates and stores dma mappings for skb's data blocks
+ * @priv: NIC private structure
+ * @skb: socket buffer to map
+ * @txdd: TX descriptor to use
  *
  * It makes dma mappings for skb's data blocks and writes them to PBL of
  * new tx descriptor. It also stores them in the tx db, so they could be
@@ -1562,9 +1584,10 @@ err_mem:
 	return -ENOMEM;
 }
 
-/*
+/**
  * bdx_tx_space - calculates available space in TX fifo
- * @priv - NIC private structure
+ * @priv: NIC private structure
+ *
  * Returns available space in TX fifo in bytes
  */
 static inline int bdx_tx_space(struct bdx_priv *priv)
@@ -1579,9 +1602,10 @@ static inline int bdx_tx_space(struct bdx_priv *priv)
 	return fsize;
 }
 
-/* bdx_tx_transmit - send packet to NIC
- * @skb - packet to send
- * ndev - network device assigned to NIC
+/**
+ * bdx_tx_transmit - send packet to NIC
+ * @skb: packet to send
+ * @ndev: network device assigned to NIC
  * Return codes:
  * o NETDEV_TX_OK everything ok.
  * o NETDEV_TX_BUSY Cannot transmit packet, try later
@@ -1699,8 +1723,10 @@ static netdev_tx_t bdx_tx_transmit(struct sk_buff *skb,
 	return NETDEV_TX_OK;
 }
 
-/* bdx_tx_cleanup - clean TXF fifo, run in the context of IRQ.
- * @priv - bdx adapter
+/**
+ * bdx_tx_cleanup - clean TXF fifo, run in the context of IRQ.
+ * @priv: bdx adapter
+ *
  * It scans TXF fifo for descriptors, frees DMA mappings and reports to OS
  * that those packets were sent
  */
@@ -1761,7 +1787,8 @@ static void bdx_tx_cleanup(struct bdx_priv *priv)
 	spin_unlock(&priv->tx_lock);
 }
 
-/* bdx_tx_free_skbs - frees all skbs from TXD fifo.
+/**
+ * bdx_tx_free_skbs - frees all skbs from TXD fifo.
  * It gets called when OS stops this dev, eg upon "ifconfig down" or rmmod
  */
 static void bdx_tx_free_skbs(struct bdx_priv *priv)
@@ -1790,10 +1817,11 @@ static void bdx_tx_free(struct bdx_priv *priv)
 	bdx_tx_db_close(&priv->txdb);
 }
 
-/* bdx_tx_push_desc - push descriptor to TxD fifo
- * @priv - NIC private structure
- * @data - desc's data
- * @size - desc's size
+/**
+ * bdx_tx_push_desc - push descriptor to TxD fifo
+ * @priv: NIC private structure
+ * @data: desc's data
+ * @size: desc's size
  *
  * Pushes desc to TxD fifo and overlaps it if needed.
  * NOTE: this func does not check for available space. this is responsibility
@@ -1819,10 +1847,11 @@ static void bdx_tx_push_desc(struct bdx_priv *priv, void *data, int size)
 	WRITE_REG(priv, f->m.reg_WPTR, f->m.wptr & TXF_WPTR_WR_PTR);
 }
 
-/* bdx_tx_push_desc_safe - push descriptor to TxD fifo in a safe way
- * @priv - NIC private structure
- * @data - desc's data
- * @size - desc's size
+/**
+ * bdx_tx_push_desc_safe - push descriptor to TxD fifo in a safe way
+ * @priv: NIC private structure
+ * @data: desc's data
+ * @size: desc's size
  *
  * NOTE: this func does check for available space and, if necessary, waits for
  *   NIC to read existing data before writing new one.
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 6685bbb5705..1e5d85b06e7 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -27,6 +27,7 @@
 #include <linux/phy.h>
 #include <linux/workqueue.h>
 #include <linux/delay.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/platform_data/cpsw.h>
 
@@ -494,11 +495,7 @@ static int cpsw_ndo_open(struct net_device *ndev)
 	cpsw_intr_disable(priv);
 	netif_carrier_off(ndev);
 
-	ret = clk_enable(priv->clk);
-	if (ret < 0) {
-		dev_err(priv->dev, "unable to turn on device clock\n");
-		return ret;
-	}
+	pm_runtime_get_sync(&priv->pdev->dev);
 
 	reg = __raw_readl(&priv->regs->id_ver);
 
@@ -569,7 +566,7 @@ static int cpsw_ndo_stop(struct net_device *ndev)
 	netif_carrier_off(priv->ndev);
 	cpsw_ale_stop(priv->ale);
 	for_each_slave(priv, cpsw_slave_stop, priv);
-	clk_disable(priv->clk);
+	pm_runtime_put_sync(&priv->pdev->dev);
 	return 0;
 }
 
@@ -748,7 +745,7 @@ static int __devinit cpsw_probe(struct platform_device *pdev)
 		memcpy(priv->mac_addr, data->slave_data[0].mac_addr, ETH_ALEN);
 		pr_info("Detected MACID = %pM", priv->mac_addr);
 	} else {
-		random_ether_addr(priv->mac_addr);
+		eth_random_addr(priv->mac_addr);
 		pr_info("Random MACID = %pM", priv->mac_addr);
 	}
 
@@ -763,10 +760,12 @@ static int __devinit cpsw_probe(struct platform_device *pdev)
 	for (i = 0; i < data->slaves; i++)
 		priv->slaves[i].slave_num = i;
 
-	priv->clk = clk_get(&pdev->dev, NULL);
+	pm_runtime_enable(&pdev->dev);
+	priv->clk = clk_get(&pdev->dev, "fck");
 	if (IS_ERR(priv->clk)) {
-		dev_err(priv->dev, "failed to get device clock)\n");
-		ret = -EBUSY;
+		dev_err(&pdev->dev, "fck is not found\n");
+		ret = -ENODEV;
+		goto clean_slave_ret;
 	}
 
 	priv->cpsw_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -935,6 +934,8 @@ clean_cpsw_iores_ret:
 			   resource_size(priv->cpsw_res));
 clean_clk_ret:
 	clk_put(priv->clk);
+clean_slave_ret:
+	pm_runtime_disable(&pdev->dev);
 	kfree(priv->slaves);
 clean_ndev_ret:
 	free_netdev(ndev);
@@ -959,6 +960,7 @@ static int __devexit cpsw_remove(struct platform_device *pdev)
 			   resource_size(priv->cpsw_res));
 	release_mem_region(priv->cpsw_ss_res->start,
 			   resource_size(priv->cpsw_ss_res));
+	pm_runtime_disable(&pdev->dev);
 	clk_put(priv->clk);
 	kfree(priv->slaves);
 	free_netdev(ndev);
@@ -973,6 +975,8 @@ static int cpsw_suspend(struct device *dev)
 
 	if (netif_running(ndev))
 		cpsw_ndo_stop(ndev);
+	pm_runtime_put_sync(&pdev->dev);
+
 	return 0;
 }
 
@@ -981,6 +985,7 @@ static int cpsw_resume(struct device *dev)
 	struct platform_device	*pdev = to_platform_device(dev);
 	struct net_device	*ndev = platform_get_drvdata(pdev);
 
+	pm_runtime_get_sync(&pdev->dev);
 	if (netif_running(ndev))
 		cpsw_ndo_open(ndev);
 	return 0;
diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index 4da93a5d7ec..fce89a0ab06 100644
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -57,7 +57,12 @@
 #include <linux/bitops.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
+#include <linux/pm_runtime.h>
 #include <linux/davinci_emac.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_net.h>
 
 #include <asm/irq.h>
 #include <asm/page.h>
@@ -339,6 +344,9 @@ struct emac_priv {
 	u32 rx_addr_type;
 	atomic_t cur_tx;
 	const char *phy_id;
+#ifdef CONFIG_OF
+	struct device_node *phy_node;
+#endif
 	struct phy_device *phydev;
 	spinlock_t lock;
 	/*platform specific members*/
@@ -346,10 +354,6 @@ struct emac_priv {
 	void (*int_disable) (void);
 };
 
-/* clock frequency for EMAC */
-static struct clk *emac_clk;
-static unsigned long emac_bus_frequency;
-
 /* EMAC TX Host Error description strings */
 static char *emac_txhost_errcodes[16] = {
 	"No error", "SOP error", "Ownership bit not set in SOP buffer",
@@ -375,7 +379,7 @@ static char *emac_rxhost_errcodes[16] = {
 #define emac_ctrl_write(reg, val) iowrite32(val, (priv->ctrl_base + (reg)))
 
 /**
- * emac_dump_regs: Dump important EMAC registers to debug terminal
+ * emac_dump_regs - Dump important EMAC registers to debug terminal
  * @priv: The DaVinci EMAC private adapter structure
  *
  * Executes ethtool set cmd & sets phy mode
@@ -466,7 +470,7 @@ static void emac_dump_regs(struct emac_priv *priv)
 }
 
 /**
- * emac_get_drvinfo: Get EMAC driver information
+ * emac_get_drvinfo - Get EMAC driver information
  * @ndev: The DaVinci EMAC network adapter
  * @info: ethtool info structure containing name and version
  *
@@ -481,7 +485,7 @@ static void emac_get_drvinfo(struct net_device *ndev,
 }
 
 /**
- * emac_get_settings: Get EMAC settings
+ * emac_get_settings - Get EMAC settings
  * @ndev: The DaVinci EMAC network adapter
  * @ecmd: ethtool command
  *
@@ -500,7 +504,7 @@ static int emac_get_settings(struct net_device *ndev,
 }
 
 /**
- * emac_set_settings: Set EMAC settings
+ * emac_set_settings - Set EMAC settings
  * @ndev: The DaVinci EMAC network adapter
  * @ecmd: ethtool command
  *
@@ -518,7 +522,7 @@ static int emac_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
 }
 
 /**
- * emac_get_coalesce : Get interrupt coalesce settings for this device
+ * emac_get_coalesce - Get interrupt coalesce settings for this device
  * @ndev : The DaVinci EMAC network adapter
  * @coal : ethtool coalesce settings structure
  *
@@ -536,7 +540,7 @@ static int emac_get_coalesce(struct net_device *ndev,
 }
 
 /**
- * emac_set_coalesce : Set interrupt coalesce settings for this device
+ * emac_set_coalesce - Set interrupt coalesce settings for this device
  * @ndev : The DaVinci EMAC network adapter
  * @coal : ethtool coalesce settings structure
  *
@@ -614,11 +618,9 @@ static int emac_set_coalesce(struct net_device *ndev,
 }
 
 
-/**
- * ethtool_ops: DaVinci EMAC Ethtool structure
+/* ethtool_ops: DaVinci EMAC Ethtool structure
  *
  * Ethtool support for EMAC adapter
- *
  */
 static const struct ethtool_ops ethtool_ops = {
 	.get_drvinfo = emac_get_drvinfo,
@@ -631,7 +633,7 @@ static const struct ethtool_ops ethtool_ops = {
 };
 
 /**
- * emac_update_phystatus: Update Phy status
+ * emac_update_phystatus - Update Phy status
  * @priv: The DaVinci EMAC private adapter structure
  *
  * Updates phy status and takes action for network queue if required
@@ -697,7 +699,7 @@ static void emac_update_phystatus(struct emac_priv *priv)
 }
 
 /**
- * hash_get: Calculate hash value from mac address
+ * hash_get - Calculate hash value from mac address
  * @addr: mac address to delete from hash table
  *
  * Calculates hash value from mac address
@@ -723,9 +725,9 @@ static u32 hash_get(u8 *addr)
 }
 
 /**
- * hash_add: Hash function to add mac addr from hash table
+ * hash_add - Hash function to add mac addr from hash table
  * @priv: The DaVinci EMAC private adapter structure
- * mac_addr: mac address to delete from hash table
+ * @mac_addr: mac address to delete from hash table
  *
  * Adds mac address to the internal hash table
  *
@@ -765,9 +767,9 @@ static int hash_add(struct emac_priv *priv, u8 *mac_addr)
 }
 
 /**
- * hash_del: Hash function to delete mac addr from hash table
+ * hash_del - Hash function to delete mac addr from hash table
  * @priv: The DaVinci EMAC private adapter structure
- * mac_addr: mac address to delete from hash table
+ * @mac_addr: mac address to delete from hash table
  *
  * Removes mac address from the internal hash table
  *
@@ -807,7 +809,7 @@ static int hash_del(struct emac_priv *priv, u8 *mac_addr)
 #define EMAC_ALL_MULTI_CLR	3
 
 /**
- * emac_add_mcast: Set multicast address in the EMAC adapter (Internal)
+ * emac_add_mcast - Set multicast address in the EMAC adapter (Internal)
  * @priv: The DaVinci EMAC private adapter structure
  * @action: multicast operation to perform
  * mac_addr: mac address to set
@@ -855,7 +857,7 @@ static void emac_add_mcast(struct emac_priv *priv, u32 action, u8 *mac_addr)
 }
 
 /**
- * emac_dev_mcast_set: Set multicast address in the EMAC adapter
+ * emac_dev_mcast_set - Set multicast address in the EMAC adapter
  * @ndev: The DaVinci EMAC network adapter
  *
  * Set multicast addresses in EMAC adapter
@@ -901,7 +903,7 @@ static void emac_dev_mcast_set(struct net_device *ndev)
  *************************************************************************/
 
 /**
- * emac_int_disable: Disable EMAC module interrupt (from adapter)
+ * emac_int_disable - Disable EMAC module interrupt (from adapter)
  * @priv: The DaVinci EMAC private adapter structure
  *
  * Disable EMAC interrupt on the adapter
@@ -931,7 +933,7 @@ static void emac_int_disable(struct emac_priv *priv)
 }
 
 /**
- * emac_int_enable: Enable EMAC module interrupt (from adapter)
+ * emac_int_enable - Enable EMAC module interrupt (from adapter)
  * @priv: The DaVinci EMAC private adapter structure
  *
  * Enable EMAC interrupt on the adapter
@@ -967,7 +969,7 @@ static void emac_int_enable(struct emac_priv *priv)
 }
 
 /**
- * emac_irq: EMAC interrupt handler
+ * emac_irq - EMAC interrupt handler
  * @irq: interrupt number
  * @dev_id: EMAC network adapter data structure ptr
  *
@@ -1060,7 +1062,7 @@ static void emac_tx_handler(void *token, int len, int status)
 }
 
 /**
- * emac_dev_xmit: EMAC Transmit function
+ * emac_dev_xmit - EMAC Transmit function
  * @skb: SKB pointer
  * @ndev: The DaVinci EMAC network adapter
  *
@@ -1111,7 +1113,7 @@ fail_tx:
 }
 
 /**
- * emac_dev_tx_timeout: EMAC Transmit timeout function
+ * emac_dev_tx_timeout - EMAC Transmit timeout function
  * @ndev: The DaVinci EMAC network adapter
  *
  * Called when system detects that a skb timeout period has expired
@@ -1138,7 +1140,7 @@ static void emac_dev_tx_timeout(struct net_device *ndev)
 }
 
 /**
- * emac_set_type0addr: Set EMAC Type0 mac address
+ * emac_set_type0addr - Set EMAC Type0 mac address
  * @priv: The DaVinci EMAC private adapter structure
  * @ch: RX channel number
  * @mac_addr: MAC address to set in device
@@ -1165,7 +1167,7 @@ static void emac_set_type0addr(struct emac_priv *priv, u32 ch, char *mac_addr)
 }
 
 /**
- * emac_set_type1addr: Set EMAC Type1 mac address
+ * emac_set_type1addr - Set EMAC Type1 mac address
  * @priv: The DaVinci EMAC private adapter structure
  * @ch: RX channel number
  * @mac_addr: MAC address to set in device
@@ -1187,7 +1189,7 @@ static void emac_set_type1addr(struct emac_priv *priv, u32 ch, char *mac_addr)
 }
 
 /**
- * emac_set_type2addr: Set EMAC Type2 mac address
+ * emac_set_type2addr - Set EMAC Type2 mac address
  * @priv: The DaVinci EMAC private adapter structure
  * @ch: RX channel number
  * @mac_addr: MAC address to set in device
@@ -1213,7 +1215,7 @@ static void emac_set_type2addr(struct emac_priv *priv, u32 ch,
 }
 
 /**
- * emac_setmac: Set mac address in the adapter (internal function)
+ * emac_setmac - Set mac address in the adapter (internal function)
  * @priv: The DaVinci EMAC private adapter structure
  * @ch: RX channel number
  * @mac_addr: MAC address to set in device
@@ -1242,7 +1244,7 @@ static void emac_setmac(struct emac_priv *priv, u32 ch, char *mac_addr)
 }
 
 /**
- * emac_dev_setmac_addr: Set mac address in the adapter
+ * emac_dev_setmac_addr - Set mac address in the adapter
  * @ndev: The DaVinci EMAC network adapter
  * @addr: MAC address to set in device
  *
@@ -1277,7 +1279,7 @@ static int emac_dev_setmac_addr(struct net_device *ndev, void *addr)
 }
 
 /**
- * emac_hw_enable: Enable EMAC hardware for packet transmission/reception
+ * emac_hw_enable - Enable EMAC hardware for packet transmission/reception
  * @priv: The DaVinci EMAC private adapter structure
  *
  * Enables EMAC hardware for packet processing - enables PHY, enables RX
@@ -1347,7 +1349,7 @@ static int emac_hw_enable(struct emac_priv *priv)
 }
 
 /**
- * emac_poll: EMAC NAPI Poll function
+ * emac_poll - EMAC NAPI Poll function
  * @ndev: The DaVinci EMAC network adapter
  * @budget: Number of receive packets to process (as told by NAPI layer)
  *
@@ -1430,7 +1432,7 @@ static int emac_poll(struct napi_struct *napi, int budget)
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
 /**
- * emac_poll_controller: EMAC Poll controller function
+ * emac_poll_controller - EMAC Poll controller function
  * @ndev: The DaVinci EMAC network adapter
  *
  * Polled functionality used by netconsole and others in non interrupt mode
@@ -1489,7 +1491,7 @@ static void emac_adjust_link(struct net_device *ndev)
  *************************************************************************/
 
 /**
- * emac_devioctl: EMAC adapter ioctl
+ * emac_devioctl - EMAC adapter ioctl
  * @ndev: The DaVinci EMAC network adapter
  * @ifrq: request parameter
  * @cmd: command parameter
@@ -1516,7 +1518,7 @@ static int match_first_device(struct device *dev, void *data)
 }
 
 /**
- * emac_dev_open: EMAC device open
+ * emac_dev_open - EMAC device open
  * @ndev: The DaVinci EMAC network adapter
  *
  * Called when system wants to start the interface. We init TX/RX channels
@@ -1535,6 +1537,8 @@ static int emac_dev_open(struct net_device *ndev)
 	int k = 0;
 	struct emac_priv *priv = netdev_priv(ndev);
 
+	pm_runtime_get(&priv->pdev->dev);
+
 	netif_carrier_off(ndev);
 	for (cnt = 0; cnt < ETH_ALEN; cnt++)
 		ndev->dev_addr[cnt] = priv->mac_addr[cnt];
@@ -1604,7 +1608,7 @@ static int emac_dev_open(struct net_device *ndev)
 				priv->phy_id);
 			ret = PTR_ERR(priv->phydev);
 			priv->phydev = NULL;
-			return ret;
+			goto err;
 		}
 
 		priv->link = 0;
@@ -1645,11 +1649,15 @@ rollback:
 		res = platform_get_resource(priv->pdev, IORESOURCE_IRQ, k-1);
 		m = res->end;
 	}
-	return -EBUSY;
+
+	ret = -EBUSY;
+err:
+	pm_runtime_put(&priv->pdev->dev);
+	return ret;
 }
 
 /**
- * emac_dev_stop: EMAC device stop
+ * emac_dev_stop - EMAC device stop
  * @ndev: The DaVinci EMAC network adapter
  *
  * Called when system wants to stop or down the interface. We stop the network
@@ -1687,11 +1695,12 @@ static int emac_dev_stop(struct net_device *ndev)
 	if (netif_msg_drv(priv))
 		dev_notice(emac_dev, "DaVinci EMAC: %s stopped\n", ndev->name);
 
+	pm_runtime_put(&priv->pdev->dev);
 	return 0;
 }
 
 /**
- * emac_dev_getnetstats: EMAC get statistics function
+ * emac_dev_getnetstats - EMAC get statistics function
  * @ndev: The DaVinci EMAC network adapter
  *
  * Called when system wants to get statistics from the device.
@@ -1762,8 +1771,79 @@ static const struct net_device_ops emac_netdev_ops = {
 #endif
 };
 
+#ifdef CONFIG_OF
+static struct emac_platform_data
+	*davinci_emac_of_get_pdata(struct platform_device *pdev,
+	struct emac_priv *priv)
+{
+	struct device_node *np;
+	struct emac_platform_data *pdata = NULL;
+	const u8 *mac_addr;
+	u32 data;
+	int ret;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+		if (!pdata)
+			goto nodata;
+	}
+
+	np = pdev->dev.of_node;
+	if (!np)
+		goto nodata;
+	else
+		pdata->version = EMAC_VERSION_2;
+
+	if (!is_valid_ether_addr(pdata->mac_addr)) {
+		mac_addr = of_get_mac_address(np);
+		if (mac_addr)
+			memcpy(pdata->mac_addr, mac_addr, ETH_ALEN);
+	}
+
+	ret = of_property_read_u32(np, "ti,davinci-ctrl-reg-offset", &data);
+	if (!ret)
+		pdata->ctrl_reg_offset = data;
+
+	ret = of_property_read_u32(np, "ti,davinci-ctrl-mod-reg-offset",
+		&data);
+	if (!ret)
+		pdata->ctrl_mod_reg_offset = data;
+
+	ret = of_property_read_u32(np, "ti,davinci-ctrl-ram-offset", &data);
+	if (!ret)
+		pdata->ctrl_ram_offset = data;
+
+	ret = of_property_read_u32(np, "ti,davinci-ctrl-ram-size", &data);
+	if (!ret)
+		pdata->ctrl_ram_size = data;
+
+	ret = of_property_read_u32(np, "ti,davinci-rmii-en", &data);
+	if (!ret)
+		pdata->rmii_en = data;
+
+	ret = of_property_read_u32(np, "ti,davinci-no-bd-ram", &data);
+	if (!ret)
+		pdata->no_bd_ram = data;
+
+	priv->phy_node = of_parse_phandle(np, "phy-handle", 0);
+	if (!priv->phy_node)
+		pdata->phy_id = "";
+
+	pdev->dev.platform_data = pdata;
+nodata:
+	return  pdata;
+}
+#else
+static struct emac_platform_data
+	*davinci_emac_of_get_pdata(struct platform_device *pdev,
+	struct emac_priv *priv)
+{
+	return  pdev->dev.platform_data;
+}
+#endif
 /**
- * davinci_emac_probe: EMAC device probe
+ * davinci_emac_probe - EMAC device probe
  * @pdev: The DaVinci EMAC device that we are removing
  *
  * Called when probing for emac devicesr. We get details of instances and
@@ -1780,6 +1860,9 @@ static int __devinit davinci_emac_probe(struct platform_device *pdev)
 	struct emac_platform_data *pdata;
 	struct device *emac_dev;
 	struct cpdma_params dma_params;
+	struct clk *emac_clk;
+	unsigned long emac_bus_frequency;
+
 
 	/* obtain emac clock from kernel */
 	emac_clk = clk_get(&pdev->dev, NULL);
@@ -1788,12 +1871,14 @@ static int __devinit davinci_emac_probe(struct platform_device *pdev)
 		return -EBUSY;
 	}
 	emac_bus_frequency = clk_get_rate(emac_clk);
+	clk_put(emac_clk);
+
 	/* TODO: Probe PHY here if possible */
 
 	ndev = alloc_etherdev(sizeof(struct emac_priv));
 	if (!ndev) {
 		rc = -ENOMEM;
-		goto free_clk;
+		goto no_ndev;
 	}
 
 	platform_set_drvdata(pdev, ndev);
@@ -1804,7 +1889,7 @@ static int __devinit davinci_emac_probe(struct platform_device *pdev)
 
 	spin_lock_init(&priv->lock);
 
-	pdata = pdev->dev.platform_data;
+	pdata = davinci_emac_of_get_pdata(pdev, priv);
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform data\n");
 		rc = -ENODEV;
@@ -1909,15 +1994,13 @@ static int __devinit davinci_emac_probe(struct platform_device *pdev)
 	SET_ETHTOOL_OPS(ndev, &ethtool_ops);
 	netif_napi_add(ndev, &priv->napi, emac_poll, EMAC_POLL_WEIGHT);
 
-	clk_enable(emac_clk);
-
 	/* register the network device */
 	SET_NETDEV_DEV(ndev, &pdev->dev);
 	rc = register_netdev(ndev);
 	if (rc) {
 		dev_err(&pdev->dev, "error in register_netdev\n");
 		rc = -ENODEV;
-		goto netdev_reg_err;
+		goto no_irq_res;
 	}
 
 
@@ -1926,10 +2009,12 @@ static int __devinit davinci_emac_probe(struct platform_device *pdev)
 			   "(regs: %p, irq: %d)\n",
 			   (void *)priv->emac_base_phys, ndev->irq);
 	}
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_resume(&pdev->dev);
+
 	return 0;
 
-netdev_reg_err:
-	clk_disable(emac_clk);
 no_irq_res:
 	if (priv->txchan)
 		cpdma_chan_destroy(priv->txchan);
@@ -1943,13 +2028,12 @@ no_dma:
 
 probe_quit:
 	free_netdev(ndev);
-free_clk:
-	clk_put(emac_clk);
+no_ndev:
 	return rc;
 }
 
 /**
- * davinci_emac_remove: EMAC device remove
+ * davinci_emac_remove - EMAC device remove
  * @pdev: The DaVinci EMAC device that we are removing
  *
  * Called when removing the device driver. We disable clock usage and release
@@ -1978,9 +2062,6 @@ static int __devexit davinci_emac_remove(struct platform_device *pdev)
 	iounmap(priv->remap_addr);
 	free_netdev(ndev);
 
-	clk_disable(emac_clk);
-	clk_put(emac_clk);
-
 	return 0;
 }
 
@@ -1992,8 +2073,6 @@ static int davinci_emac_suspend(struct device *dev)
 	if (netif_running(ndev))
 		emac_dev_stop(ndev);
 
-	clk_disable(emac_clk);
-
 	return 0;
 }
 
@@ -2002,8 +2081,6 @@ static int davinci_emac_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct net_device *ndev = platform_get_drvdata(pdev);
 
-	clk_enable(emac_clk);
-
 	if (netif_running(ndev))
 		emac_dev_open(ndev);
 
@@ -2015,21 +2092,26 @@ static const struct dev_pm_ops davinci_emac_pm_ops = {
 	.resume		= davinci_emac_resume,
 };
 
-/**
- * davinci_emac_driver: EMAC platform driver structure
- */
+static const struct of_device_id davinci_emac_of_match[] = {
+	{.compatible = "ti,davinci-dm6467-emac", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, davinci_emac_of_match);
+
+/* davinci_emac_driver: EMAC platform driver structure */
 static struct platform_driver davinci_emac_driver = {
 	.driver = {
 		.name	 = "davinci_emac",
 		.owner	 = THIS_MODULE,
 		.pm	 = &davinci_emac_pm_ops,
+		.of_match_table = of_match_ptr(davinci_emac_of_match),
 	},
 	.probe = davinci_emac_probe,
 	.remove = __devexit_p(davinci_emac_remove),
 };
 
 /**
- * davinci_emac_init: EMAC driver module init
+ * davinci_emac_init - EMAC driver module init
  *
  * Called when initializing the driver. We register the driver with
  * the platform.
@@ -2041,7 +2123,7 @@ static int __init davinci_emac_init(void)
 late_initcall(davinci_emac_init);
 
 /**
- * davinci_emac_exit: EMAC driver module exit
+ * davinci_emac_exit - EMAC driver module exit
  *
  * Called when exiting the driver completely. We unregister the driver with
  * the platform and exit
diff --git a/drivers/net/ethernet/ti/davinci_mdio.c b/drivers/net/ethernet/ti/davinci_mdio.c
index e4e47088e26..cd7ee204e94 100644
--- a/drivers/net/ethernet/ti/davinci_mdio.c
+++ b/drivers/net/ethernet/ti/davinci_mdio.c
@@ -34,6 +34,7 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/io.h>
+#include <linux/pm_runtime.h>
 #include <linux/davinci_emac.h>
 
 /*
@@ -321,7 +322,9 @@ static int __devinit davinci_mdio_probe(struct platform_device *pdev)
 	snprintf(data->bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		pdev->name, pdev->id);
 
-	data->clk = clk_get(dev, NULL);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+	data->clk = clk_get(&pdev->dev, "fck");
 	if (IS_ERR(data->clk)) {
 		dev_err(dev, "failed to get device clock\n");
 		ret = PTR_ERR(data->clk);
@@ -329,8 +332,6 @@ static int __devinit davinci_mdio_probe(struct platform_device *pdev)
 		goto bail_out;
 	}
 
-	clk_enable(data->clk);
-
 	dev_set_drvdata(dev, data);
 	data->dev = dev;
 	spin_lock_init(&data->lock);
@@ -378,10 +379,10 @@ bail_out:
 	if (data->bus)
 		mdiobus_free(data->bus);
 
-	if (data->clk) {
-		clk_disable(data->clk);
+	if (data->clk)
 		clk_put(data->clk);
-	}
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 
 	kfree(data);
 
@@ -396,10 +397,10 @@ static int __devexit davinci_mdio_remove(struct platform_device *pdev)
 	if (data->bus)
 		mdiobus_free(data->bus);
 
-	if (data->clk) {
-		clk_disable(data->clk);
+	if (data->clk)
 		clk_put(data->clk);
-	}
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 
 	dev_set_drvdata(dev, NULL);
 
@@ -421,8 +422,7 @@ static int davinci_mdio_suspend(struct device *dev)
 	__raw_writel(ctrl, &data->regs->control);
 	wait_for_idle(data);
 
-	if (data->clk)
-		clk_disable(data->clk);
+	pm_runtime_put_sync(data->dev);
 
 	data->suspended = true;
 	spin_unlock(&data->lock);
@@ -436,8 +436,7 @@ static int davinci_mdio_resume(struct device *dev)
 	u32 ctrl;
 
 	spin_lock(&data->lock);
-	if (data->clk)
-		clk_enable(data->clk);
+	pm_runtime_put_sync(data->dev);
 
 	/* restart the scan state machine */
 	ctrl = __raw_readl(&data->regs->control);
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index 83b4b388ad4..4e2a1628484 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -123,6 +123,7 @@ struct tile_net_comps {
 
 /* The transmit wake timer for a given cpu and echannel. */
 struct tile_net_tx_wake {
+	int tx_queue_idx;
 	struct hrtimer timer;
 	struct net_device *dev;
 };
@@ -573,12 +574,14 @@ static void add_comp(gxio_mpipe_equeue_t *equeue,
 	comps->comp_next++;
 }
 
-static void tile_net_schedule_tx_wake_timer(struct net_device *dev)
+static void tile_net_schedule_tx_wake_timer(struct net_device *dev,
+                                            int tx_queue_idx)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = &per_cpu(per_cpu_info, tx_queue_idx);
 	struct tile_net_priv *priv = netdev_priv(dev);
+	struct tile_net_tx_wake *tx_wake = &info->tx_wake[priv->echannel];
 
-	hrtimer_start(&info->tx_wake[priv->echannel].timer,
+	hrtimer_start(&tx_wake->timer,
 		      ktime_set(0, TX_TIMER_DELAY_USEC * 1000UL),
 		      HRTIMER_MODE_REL_PINNED);
 }
@@ -587,7 +590,7 @@ static enum hrtimer_restart tile_net_handle_tx_wake_timer(struct hrtimer *t)
 {
 	struct tile_net_tx_wake *tx_wake =
 		container_of(t, struct tile_net_tx_wake, timer);
-	netif_wake_subqueue(tx_wake->dev, smp_processor_id());
+	netif_wake_subqueue(tx_wake->dev, tx_wake->tx_queue_idx);
 	return HRTIMER_NORESTART;
 }
 
@@ -1218,6 +1221,7 @@ static int tile_net_open(struct net_device *dev)
 
 		hrtimer_init(&tx_wake->timer, CLOCK_MONOTONIC,
 			     HRTIMER_MODE_REL);
+		tx_wake->tx_queue_idx = cpu;
 		tx_wake->timer.function = tile_net_handle_tx_wake_timer;
 		tx_wake->dev = dev;
 	}
@@ -1291,6 +1295,7 @@ static inline void *tile_net_frag_buf(skb_frag_t *f)
  * stop the queue and schedule the tx_wake timer.
  */
 static s64 tile_net_equeue_try_reserve(struct net_device *dev,
+				       int tx_queue_idx,
 				       struct tile_net_comps *comps,
 				       gxio_mpipe_equeue_t *equeue,
 				       int num_edescs)
@@ -1313,8 +1318,8 @@ static s64 tile_net_equeue_try_reserve(struct net_device *dev,
 	}
 
 	/* Still nothing; give up and stop the queue for a short while. */
-	netif_stop_subqueue(dev, smp_processor_id());
-	tile_net_schedule_tx_wake_timer(dev);
+	netif_stop_subqueue(dev, tx_queue_idx);
+	tile_net_schedule_tx_wake_timer(dev, tx_queue_idx);
 	return -1;
 }
 
@@ -1328,11 +1333,12 @@ static s64 tile_net_equeue_try_reserve(struct net_device *dev,
 static int tso_count_edescs(struct sk_buff *skb)
 {
 	struct skb_shared_info *sh = skb_shinfo(skb);
-	unsigned int data_len = skb->data_len;
+	unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+	unsigned int data_len = skb->data_len + skb->hdr_len - sh_len;
 	unsigned int p_len = sh->gso_size;
 	long f_id = -1;    /* id of the current fragment */
-	long f_size = -1;  /* size of the current fragment */
-	long f_used = -1;  /* bytes used from the current fragment */
+	long f_size = skb->hdr_len;  /* size of the current fragment */
+	long f_used = sh_len;  /* bytes used from the current fragment */
 	long n;            /* size of the current piece of payload */
 	int num_edescs = 0;
 	int segment;
@@ -1377,13 +1383,14 @@ static void tso_headers_prepare(struct sk_buff *skb, unsigned char *headers,
 	struct skb_shared_info *sh = skb_shinfo(skb);
 	struct iphdr *ih;
 	struct tcphdr *th;
-	unsigned int data_len = skb->data_len;
+	unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+	unsigned int data_len = skb->data_len + skb->hdr_len - sh_len;
 	unsigned char *data = skb->data;
-	unsigned int ih_off, th_off, sh_len, p_len;
+	unsigned int ih_off, th_off, p_len;
 	unsigned int isum_seed, tsum_seed, id, seq;
 	long f_id = -1;    /* id of the current fragment */
-	long f_size = -1;  /* size of the current fragment */
-	long f_used = -1;  /* bytes used from the current fragment */
+	long f_size = skb->hdr_len;  /* size of the current fragment */
+	long f_used = sh_len;  /* bytes used from the current fragment */
 	long n;            /* size of the current piece of payload */
 	int segment;
 
@@ -1392,14 +1399,13 @@ static void tso_headers_prepare(struct sk_buff *skb, unsigned char *headers,
 	th = tcp_hdr(skb);
 	ih_off = skb_network_offset(skb);
 	th_off = skb_transport_offset(skb);
-	sh_len = th_off + tcp_hdrlen(skb);
 	p_len = sh->gso_size;
 
 	/* Set up seed values for IP and TCP csum and initialize id and seq. */
 	isum_seed = ((0xFFFF - ih->check) +
 		     (0xFFFF - ih->tot_len) +
 		     (0xFFFF - ih->id));
-	tsum_seed = th->check + (0xFFFF ^ htons(skb->len));
+	tsum_seed = th->check + (0xFFFF ^ htons(sh_len + data_len));
 	id = ntohs(ih->id);
 	seq = ntohl(th->seq);
 
@@ -1471,21 +1477,22 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
 {
 	struct tile_net_priv *priv = netdev_priv(dev);
 	struct skb_shared_info *sh = skb_shinfo(skb);
-	unsigned int data_len = skb->data_len;
+	unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+	unsigned int data_len = skb->data_len + skb->hdr_len - sh_len;
 	unsigned int p_len = sh->gso_size;
 	gxio_mpipe_edesc_t edesc_head = { { 0 } };
 	gxio_mpipe_edesc_t edesc_body = { { 0 } };
 	long f_id = -1;    /* id of the current fragment */
-	long f_size = -1;  /* size of the current fragment */
-	long f_used = -1;  /* bytes used from the current fragment */
+	long f_size = skb->hdr_len;  /* size of the current fragment */
+	long f_used = sh_len;  /* bytes used from the current fragment */
+	void *f_data = skb->data;
 	long n;            /* size of the current piece of payload */
 	unsigned long tx_packets = 0, tx_bytes = 0;
-	unsigned int csum_start, sh_len;
+	unsigned int csum_start;
 	int segment;
 
 	/* Prepare to egress the headers: set up header edesc. */
 	csum_start = skb_checksum_start_offset(skb);
-	sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
 	edesc_head.csum = 1;
 	edesc_head.csum_start = csum_start;
 	edesc_head.csum_dest = csum_start + skb->csum_offset;
@@ -1497,7 +1504,6 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
 
 	/* Egress all the edescs. */
 	for (segment = 0; segment < sh->gso_segs; segment++) {
-		void *va;
 		unsigned char *buf;
 		unsigned int p_used = 0;
 
@@ -1516,10 +1522,9 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
 				f_id++;
 				f_size = sh->frags[f_id].size;
 				f_used = 0;
+				f_data = tile_net_frag_buf(&sh->frags[f_id]);
 			}
 
-			va = tile_net_frag_buf(&sh->frags[f_id]) + f_used;
-
 			/* Use bytes from the current fragment. */
 			n = p_len - p_used;
 			if (n > f_size - f_used)
@@ -1528,7 +1533,7 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
 			p_used += n;
 
 			/* Egress a piece of the payload. */
-			edesc_body.va = va_to_tile_io_addr(va);
+			edesc_body.va = va_to_tile_io_addr(f_data) + f_used;
 			edesc_body.xfer_size = n;
 			edesc_body.bound = !(p_used < p_len);
 			gxio_mpipe_equeue_put_at(equeue, edesc_body, slot);
@@ -1580,7 +1585,8 @@ static int tile_net_tx_tso(struct sk_buff *skb, struct net_device *dev)
 	local_irq_save(irqflags);
 
 	/* Try to acquire a completion entry and an egress slot. */
-	slot = tile_net_equeue_try_reserve(dev, comps, equeue, num_edescs);
+	slot = tile_net_equeue_try_reserve(dev, skb->queue_mapping, comps,
+					   equeue, num_edescs);
 	if (slot < 0) {
 		local_irq_restore(irqflags);
 		return NETDEV_TX_BUSY;
@@ -1674,7 +1680,8 @@ static int tile_net_tx(struct sk_buff *skb, struct net_device *dev)
 	local_irq_save(irqflags);
 
 	/* Try to acquire a completion entry and an egress slot. */
-	slot = tile_net_equeue_try_reserve(dev, comps, equeue, num_edescs);
+	slot = tile_net_equeue_try_reserve(dev, skb->queue_mapping, comps,
+					   equeue, num_edescs);
 	if (slot < 0) {
 		local_irq_restore(irqflags);
 		return NETDEV_TX_BUSY;
@@ -1844,7 +1851,7 @@ static void tile_net_dev_init(const char *name, const uint8_t *mac)
 		memcpy(dev->dev_addr, mac, 6);
 		dev->addr_len = 6;
 	} else {
-		random_ether_addr(dev->dev_addr);
+		eth_hw_addr_random(dev);
 	}
 
 	/* Register the network device. */
diff --git a/drivers/net/ethernet/toshiba/spider_net.c b/drivers/net/ethernet/toshiba/spider_net.c
index 6199f6b387b..c1ebfe9efcb 100644
--- a/drivers/net/ethernet/toshiba/spider_net.c
+++ b/drivers/net/ethernet/toshiba/spider_net.c
@@ -114,7 +114,8 @@ spider_net_write_reg(struct spider_net_card *card, u32 reg, u32 value)
 	out_be32(card->regs + reg, value);
 }
 
-/** spider_net_write_phy - write to phy register
+/**
+ * spider_net_write_phy - write to phy register
  * @netdev: adapter to be written to
  * @mii_id: id of MII
  * @reg: PHY register
@@ -137,7 +138,8 @@ spider_net_write_phy(struct net_device *netdev, int mii_id,
 	spider_net_write_reg(card, SPIDER_NET_GPCWOPCMD, writevalue);
 }
 
-/** spider_net_read_phy - read from phy register
+/**
+ * spider_net_read_phy - read from phy register
  * @netdev: network device to be read from
  * @mii_id: id of MII
  * @reg: PHY register
diff --git a/drivers/net/ethernet/via/via-velocity.c b/drivers/net/ethernet/via/via-velocity.c
index ea3e0a21ba7..a46c1985968 100644
--- a/drivers/net/ethernet/via/via-velocity.c
+++ b/drivers/net/ethernet/via/via-velocity.c
@@ -486,7 +486,7 @@ static void __devinit velocity_get_options(struct velocity_opt *opts, int index,
 	velocity_set_bool_opt(&opts->flags, IP_byte_align[index], IP_ALIG_DEF, VELOCITY_FLAGS_IP_ALIGN, "IP_byte_align", devname);
 	velocity_set_bool_opt(&opts->flags, ValPktLen[index], VAL_PKT_LEN_DEF, VELOCITY_FLAGS_VAL_PKT_LEN, "ValPktLen", devname);
 	velocity_set_int_opt((int *) &opts->spd_dpx, speed_duplex[index], MED_LNK_MIN, MED_LNK_MAX, MED_LNK_DEF, "Media link mode", devname);
-	velocity_set_int_opt((int *) &opts->wol_opts, wol_opts[index], WOL_OPT_MIN, WOL_OPT_MAX, WOL_OPT_DEF, "Wake On Lan options", devname);
+	velocity_set_int_opt(&opts->wol_opts, wol_opts[index], WOL_OPT_MIN, WOL_OPT_MAX, WOL_OPT_DEF, "Wake On Lan options", devname);
 	opts->numrx = (opts->numrx & ~3);
 }
 
diff --git a/drivers/net/ethernet/wiznet/Kconfig b/drivers/net/ethernet/wiznet/Kconfig
index cb18043f583..b4d281626fb 100644
--- a/drivers/net/ethernet/wiznet/Kconfig
+++ b/drivers/net/ethernet/wiznet/Kconfig
@@ -4,6 +4,7 @@
 
 config NET_VENDOR_WIZNET
 	bool "WIZnet devices"
+	depends on HAS_IOMEM
 	default y
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
index a75e9ef5a4c..a5826a3111a 100644
--- a/drivers/net/ethernet/wiznet/w5100.c
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -637,7 +637,7 @@ static int __devinit w5100_hw_probe(struct platform_device *pdev)
 	if (data && is_valid_ether_addr(data->mac_addr)) {
 		memcpy(ndev->dev_addr, data->mac_addr, ETH_ALEN);
 	} else {
-		random_ether_addr(ndev->dev_addr);
+		eth_random_addr(ndev->dev_addr);
 		ndev->addr_assign_type |= NET_ADDR_RANDOM;
 	}
 
diff --git a/drivers/net/ethernet/wiznet/w5300.c b/drivers/net/ethernet/wiznet/w5300.c
index 3306a20ec21..bdd8891c215 100644
--- a/drivers/net/ethernet/wiznet/w5300.c
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -557,7 +557,7 @@ static int __devinit w5300_hw_probe(struct platform_device *pdev)
 	if (data && is_valid_ether_addr(data->mac_addr)) {
 		memcpy(ndev->dev_addr, data->mac_addr, ETH_ALEN);
 	} else {
-		random_ether_addr(ndev->dev_addr);
+		eth_random_addr(ndev->dev_addr);
 		ndev->addr_assign_type |= NET_ADDR_RANDOM;
 	}
 
diff --git a/drivers/net/ethernet/xilinx/ll_temac_main.c b/drivers/net/ethernet/xilinx/ll_temac_main.c
index 1eaf7128afe..f8e35188011 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_main.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_main.c
@@ -197,7 +197,7 @@ static int temac_dcr_setup(struct temac_local *lp, struct platform_device *op,
 #endif
 
 /**
- *  * temac_dma_bd_release - Release buffer descriptor rings
+ * temac_dma_bd_release - Release buffer descriptor rings
  */
 static void temac_dma_bd_release(struct net_device *ndev)
 {
@@ -768,7 +768,6 @@ static void ll_temac_recv(struct net_device *ndev)
 				 DMA_FROM_DEVICE);
 
 		skb_put(skb, length);
-		skb->dev = ndev;
 		skb->protocol = eth_type_trans(skb, ndev);
 		skb_checksum_none_assert(skb);
 
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
index 9c365e192a3..0793299bd39 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
@@ -312,7 +312,7 @@ static void axienet_set_mac_address(struct net_device *ndev, void *address)
 	if (address)
 		memcpy(ndev->dev_addr, address, ETH_ALEN);
 	if (!is_valid_ether_addr(ndev->dev_addr))
-		random_ether_addr(ndev->dev_addr);
+		eth_random_addr(ndev->dev_addr);
 
 	/* Set up unicast MAC address filter set its mac address */
 	axienet_iow(lp, XAE_UAW0_OFFSET,
diff --git a/drivers/net/fddi/defxx.c b/drivers/net/fddi/defxx.c
index 4ad80f77109..6695a1dadf4 100644
--- a/drivers/net/fddi/defxx.c
+++ b/drivers/net/fddi/defxx.c
@@ -2962,7 +2962,7 @@ static int dfx_rcv_init(DFX_board_t *bp, int get_buffers)
 			bp->descr_block_virt->rcv_data[i+j].long_0 = (u32) (PI_RCV_DESCR_M_SOP |
 				((PI_RCV_DATA_K_SIZE_MAX / PI_ALIGN_K_RCV_DATA_BUFF) << PI_RCV_DESCR_V_SEG_LEN));
 			bp->descr_block_virt->rcv_data[i+j].long_1 = (u32) (bp->rcv_block_phys + (i * PI_RCV_DATA_K_SIZE_MAX));
-			bp->p_rcv_buff_va[i+j] = (char *) (bp->rcv_block_virt + (i * PI_RCV_DATA_K_SIZE_MAX));
+			bp->p_rcv_buff_va[i+j] = (bp->rcv_block_virt + (i * PI_RCV_DATA_K_SIZE_MAX));
 			}
 #endif
 	}
@@ -3030,7 +3030,7 @@ static void dfx_rcv_queue_process(
 #ifdef DYNAMIC_BUFFERS
 		p_buff = (char *) (((struct sk_buff *)bp->p_rcv_buff_va[entry])->data);
 #else
-		p_buff = (char *) bp->p_rcv_buff_va[entry];
+		p_buff = bp->p_rcv_buff_va[entry];
 #endif
 		memcpy(&descr, p_buff + RCV_BUFF_K_DESCR, sizeof(u32));
 
diff --git a/drivers/net/fddi/skfp/pmf.c b/drivers/net/fddi/skfp/pmf.c
index 9ac4665d741..24d8566cfd8 100644
--- a/drivers/net/fddi/skfp/pmf.c
+++ b/drivers/net/fddi/skfp/pmf.c
@@ -1242,7 +1242,7 @@ static int smt_set_para(struct s_smc *smc, struct smt_para *pa, int index,
 			if (len < 8)
 				goto len_error ;
 			if (set)
-				memcpy((char *) to,(char *) from+2,6) ;
+				memcpy(to,from+2,6) ;
 			to += 8 ;
 			from += 8 ;
 			len -= 8 ;
@@ -1251,7 +1251,7 @@ static int smt_set_para(struct s_smc *smc, struct smt_para *pa, int index,
 			if (len < 4)
 				goto len_error ;
 			if (set)
-				memcpy((char *) to,(char *) from,4) ;
+				memcpy(to,from,4) ;
 			to += 4 ;
 			from += 4 ;
 			len -= 4 ;
@@ -1260,7 +1260,7 @@ static int smt_set_para(struct s_smc *smc, struct smt_para *pa, int index,
 			if (len < 8)
 				goto len_error ;
 			if (set)
-				memcpy((char *) to,(char *) from,8) ;
+				memcpy(to,from,8) ;
 			to += 8 ;
 			from += 8 ;
 			len -= 8 ;
@@ -1269,7 +1269,7 @@ static int smt_set_para(struct s_smc *smc, struct smt_para *pa, int index,
 			if (len < 32)
 				goto len_error ;
 			if (set)
-				memcpy((char *) to,(char *) from,32) ;
+				memcpy(to,from,32) ;
 			to += 32 ;
 			from += 32 ;
 			len -= 32 ;
diff --git a/drivers/net/hamradio/mkiss.c b/drivers/net/hamradio/mkiss.c
index aed1a6105b2..2c0894a92ab 100644
--- a/drivers/net/hamradio/mkiss.c
+++ b/drivers/net/hamradio/mkiss.c
@@ -485,7 +485,7 @@ static void ax_encaps(struct net_device *dev, unsigned char *icp, int len)
 
 			return;
 		default:
-			count = kiss_esc(p, (unsigned char *)ax->xbuff, len);
+			count = kiss_esc(p, ax->xbuff, len);
 		}
 	} else {
 		unsigned short crc;
@@ -497,7 +497,7 @@ static void ax_encaps(struct net_device *dev, unsigned char *icp, int len)
 		case CRC_MODE_SMACK:
 			*p |= 0x80;
 			crc = swab16(crc16(0, p, len));
-			count = kiss_esc_crc(p, (unsigned char *)ax->xbuff, crc, len+2);
+			count = kiss_esc_crc(p, ax->xbuff, crc, len+2);
 			break;
 		case CRC_MODE_FLEX_TEST:
 			ax->crcmode = CRC_MODE_NONE;
@@ -506,11 +506,11 @@ static void ax_encaps(struct net_device *dev, unsigned char *icp, int len)
 		case CRC_MODE_FLEX:
 			*p |= 0x20;
 			crc = calc_crc_flex(p, len);
-			count = kiss_esc_crc(p, (unsigned char *)ax->xbuff, crc, len+2);
+			count = kiss_esc_crc(p, ax->xbuff, crc, len+2);
 			break;
 
 		default:
-			count = kiss_esc(p, (unsigned char *)ax->xbuff, len);
+			count = kiss_esc(p, ax->xbuff, len);
 		}
   	}
 	spin_unlock_bh(&ax->buflock);
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index 2857ab078aa..95ceb359304 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -131,6 +131,7 @@ int rndis_filter_send(struct hv_device *dev,
 			struct hv_netvsc_packet *pkt);
 
 int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter);
+int rndis_filter_set_device_mac(struct hv_device *hdev, char *mac);
 
 
 #define NVSP_INVALID_PROTOCOL_VERSION	((u32)0xFFFFFFFF)
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index 0c569831db5..6cee2917eb0 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -614,7 +614,7 @@ retry_send_cmplt:
 static void netvsc_receive_completion(void *context)
 {
 	struct hv_netvsc_packet *packet = context;
-	struct hv_device *device = (struct hv_device *)packet->device;
+	struct hv_device *device = packet->device;
 	struct netvsc_device *net_device;
 	u64 transaction_id = 0;
 	bool fsend_receive_comp = false;
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 8f8ed332042..8c5a1c43c81 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -47,7 +47,7 @@ struct net_device_context {
 	struct work_struct work;
 };
 
-
+#define RING_SIZE_MIN 64
 static int ring_size = 128;
 module_param(ring_size, int, S_IRUGO);
 MODULE_PARM_DESC(ring_size, "Ring buffer size (# of pages)");
@@ -341,6 +341,34 @@ static int netvsc_change_mtu(struct net_device *ndev, int mtu)
 	return 0;
 }
 
+
+static int netvsc_set_mac_addr(struct net_device *ndev, void *p)
+{
+	struct net_device_context *ndevctx = netdev_priv(ndev);
+	struct hv_device *hdev =  ndevctx->device_ctx;
+	struct sockaddr *addr = p;
+	char save_adr[14];
+	unsigned char save_aatype;
+	int err;
+
+	memcpy(save_adr, ndev->dev_addr, ETH_ALEN);
+	save_aatype = ndev->addr_assign_type;
+
+	err = eth_mac_addr(ndev, p);
+	if (err != 0)
+		return err;
+
+	err = rndis_filter_set_device_mac(hdev, addr->sa_data);
+	if (err != 0) {
+		/* roll back to saved MAC */
+		memcpy(ndev->dev_addr, save_adr, ETH_ALEN);
+		ndev->addr_assign_type = save_aatype;
+	}
+
+	return err;
+}
+
+
 static const struct ethtool_ops ethtool_ops = {
 	.get_drvinfo	= netvsc_get_drvinfo,
 	.get_link	= ethtool_op_get_link,
@@ -353,7 +381,7 @@ static const struct net_device_ops device_ops = {
 	.ndo_set_rx_mode =		netvsc_set_multicast_list,
 	.ndo_change_mtu =		netvsc_change_mtu,
 	.ndo_validate_addr =		eth_validate_addr,
-	.ndo_set_mac_address =		eth_mac_addr,
+	.ndo_set_mac_address =		netvsc_set_mac_addr,
 };
 
 /*
@@ -490,6 +518,11 @@ static void __exit netvsc_drv_exit(void)
 
 static int __init netvsc_drv_init(void)
 {
+	if (ring_size < RING_SIZE_MIN) {
+		ring_size = RING_SIZE_MIN;
+		pr_info("Increased ring_size to %d (min allowed)\n",
+			ring_size);
+	}
 	return vmbus_driver_register(&netvsc_drv);
 }
 
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index 981ebb11563..e5d6146937f 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -27,6 +27,7 @@
 #include <linux/if_ether.h>
 #include <linux/netdevice.h>
 #include <linux/if_vlan.h>
+#include <linux/nls.h>
 
 #include "hyperv_net.h"
 
@@ -47,6 +48,7 @@ struct rndis_request {
 	struct hv_page_buffer buf;
 	/* FIXME: We assumed a fixed size request here. */
 	struct rndis_message request_msg;
+	u8 ext[100];
 };
 
 static void rndis_filter_send_completion(void *ctx);
@@ -511,6 +513,83 @@ static int rndis_filter_query_device_mac(struct rndis_device *dev)
 				      dev->hw_mac_adr, &size);
 }
 
+#define NWADR_STR "NetworkAddress"
+#define NWADR_STRLEN 14
+
+int rndis_filter_set_device_mac(struct hv_device *hdev, char *mac)
+{
+	struct netvsc_device *nvdev = hv_get_drvdata(hdev);
+	struct rndis_device *rdev = nvdev->extension;
+	struct net_device *ndev = nvdev->ndev;
+	struct rndis_request *request;
+	struct rndis_set_request *set;
+	struct rndis_config_parameter_info *cpi;
+	wchar_t *cfg_nwadr, *cfg_mac;
+	struct rndis_set_complete *set_complete;
+	char macstr[2*ETH_ALEN+1];
+	u32 extlen = sizeof(struct rndis_config_parameter_info) +
+		2*NWADR_STRLEN + 4*ETH_ALEN;
+	int ret, t;
+
+	request = get_rndis_request(rdev, RNDIS_MSG_SET,
+		RNDIS_MESSAGE_SIZE(struct rndis_set_request) + extlen);
+	if (!request)
+		return -ENOMEM;
+
+	set = &request->request_msg.msg.set_req;
+	set->oid = RNDIS_OID_GEN_RNDIS_CONFIG_PARAMETER;
+	set->info_buflen = extlen;
+	set->info_buf_offset = sizeof(struct rndis_set_request);
+	set->dev_vc_handle = 0;
+
+	cpi = (struct rndis_config_parameter_info *)((ulong)set +
+		set->info_buf_offset);
+	cpi->parameter_name_offset =
+		sizeof(struct rndis_config_parameter_info);
+	/* Multiply by 2 because host needs 2 bytes (utf16) for each char */
+	cpi->parameter_name_length = 2*NWADR_STRLEN;
+	cpi->parameter_type = RNDIS_CONFIG_PARAM_TYPE_STRING;
+	cpi->parameter_value_offset =
+		cpi->parameter_name_offset + cpi->parameter_name_length;
+	/* Multiply by 4 because each MAC byte displayed as 2 utf16 chars */
+	cpi->parameter_value_length = 4*ETH_ALEN;
+
+	cfg_nwadr = (wchar_t *)((ulong)cpi + cpi->parameter_name_offset);
+	cfg_mac = (wchar_t *)((ulong)cpi + cpi->parameter_value_offset);
+	ret = utf8s_to_utf16s(NWADR_STR, NWADR_STRLEN, UTF16_HOST_ENDIAN,
+			      cfg_nwadr, NWADR_STRLEN);
+	if (ret < 0)
+		goto cleanup;
+	snprintf(macstr, 2*ETH_ALEN+1, "%pm", mac);
+	ret = utf8s_to_utf16s(macstr, 2*ETH_ALEN, UTF16_HOST_ENDIAN,
+			      cfg_mac, 2*ETH_ALEN);
+	if (ret < 0)
+		goto cleanup;
+
+	ret = rndis_filter_send_request(rdev, request);
+	if (ret != 0)
+		goto cleanup;
+
+	t = wait_for_completion_timeout(&request->wait_event, 5*HZ);
+	if (t == 0) {
+		netdev_err(ndev, "timeout before we got a set response...\n");
+		/*
+		 * can't put_rndis_request, since we may still receive a
+		 * send-completion.
+		 */
+		return -EBUSY;
+	} else {
+		set_complete = &request->response_msg.msg.set_complete;
+		if (set_complete->status != RNDIS_STATUS_SUCCESS)
+			ret = -EINVAL;
+	}
+
+cleanup:
+	put_rndis_request(rdev, request);
+	return ret;
+}
+
+
 static int rndis_filter_query_device_link_status(struct rndis_device *dev)
 {
 	u32 size = sizeof(u32);
@@ -725,18 +804,15 @@ int rndis_filter_device_add(struct hv_device *dev,
 	/* Send the rndis initialization message */
 	ret = rndis_filter_init_device(rndis_device);
 	if (ret != 0) {
-		/*
-		 * TODO: If rndis init failed, we will need to shut down the
-		 * channel
-		 */
+		rndis_filter_device_remove(dev);
+		return ret;
 	}
 
 	/* Get the mac address */
 	ret = rndis_filter_query_device_mac(rndis_device);
 	if (ret != 0) {
-		/*
-		 * TODO: shutdown rndis device and the channel
-		 */
+		rndis_filter_device_remove(dev);
+		return ret;
 	}
 
 	memcpy(device_info->mac_adr, rndis_device->hw_mac_adr, ETH_ALEN);
diff --git a/drivers/net/irda/ali-ircc.c b/drivers/net/irda/ali-ircc.c
index dcc80d652b7..84872043b5c 100644
--- a/drivers/net/irda/ali-ircc.c
+++ b/drivers/net/irda/ali-ircc.c
@@ -1017,7 +1017,7 @@ static void ali_ircc_fir_change_speed(struct ali_ircc_cb *priv, __u32 baud)
 {
 		
 	int iobase; 
-	struct ali_ircc_cb *self = (struct ali_ircc_cb *) priv;
+	struct ali_ircc_cb *self = priv;
 	struct net_device *dev;
 
 	IRDA_DEBUG(1, "%s(), ---------------- Start ----------------\n", __func__ );
@@ -1052,7 +1052,7 @@ static void ali_ircc_fir_change_speed(struct ali_ircc_cb *priv, __u32 baud)
  */
 static void ali_ircc_sir_change_speed(struct ali_ircc_cb *priv, __u32 speed)
 {
-	struct ali_ircc_cb *self = (struct ali_ircc_cb *) priv;
+	struct ali_ircc_cb *self = priv;
 	unsigned long flags;
 	int iobase; 
 	int fcr;    /* FIFO control reg */
@@ -1121,7 +1121,7 @@ static void ali_ircc_sir_change_speed(struct ali_ircc_cb *priv, __u32 speed)
 static void ali_ircc_change_dongle_speed(struct ali_ircc_cb *priv, int speed)
 {
 	
-	struct ali_ircc_cb *self = (struct ali_ircc_cb *) priv;
+	struct ali_ircc_cb *self = priv;
 	int iobase,dongle_id;
 	int tmp = 0;
 			
diff --git a/drivers/net/irda/au1k_ir.c b/drivers/net/irda/au1k_ir.c
index fc503aa5288..e09417df8f3 100644
--- a/drivers/net/irda/au1k_ir.c
+++ b/drivers/net/irda/au1k_ir.c
@@ -794,7 +794,7 @@ static int __devinit au1k_irda_net_init(struct net_device *dev)
 
 	/* allocate the data buffers */
 	aup->db[0].vaddr =
-		(void *)dma_alloc(MAX_BUF_SIZE * 2 * NUM_IR_DESC, &temp);
+		dma_alloc(MAX_BUF_SIZE * 2 * NUM_IR_DESC, &temp);
 	if (!aup->db[0].vaddr)
 		goto out3;
 
diff --git a/drivers/net/irda/pxaficp_ir.c b/drivers/net/irda/pxaficp_ir.c
index ff16daf33ae..8d547670791 100644
--- a/drivers/net/irda/pxaficp_ir.c
+++ b/drivers/net/irda/pxaficp_ir.c
@@ -289,7 +289,7 @@ static irqreturn_t pxa_irda_sir_irq(int irq, void *dev_id)
 			}
 			lsr = STLSR;
 		}
-		si->last_oscr = OSCR;
+		si->last_oscr = readl_relaxed(OSCR);
 		break;
 
 	case 0x04: /* Received Data Available */
@@ -300,7 +300,7 @@ static irqreturn_t pxa_irda_sir_irq(int irq, void *dev_id)
 		    dev->stats.rx_bytes++;
 	            async_unwrap_char(dev, &dev->stats, &si->rx_buff, STRBR);
 	  	} while (STLSR & LSR_DR);
-		si->last_oscr = OSCR;
+		si->last_oscr = readl_relaxed(OSCR);
 	  	break;
 
 	case 0x02: /* Transmit FIFO Data Request */
@@ -316,7 +316,7 @@ static irqreturn_t pxa_irda_sir_irq(int irq, void *dev_id)
                         /* We need to ensure that the transmitter has finished. */
 			while ((STLSR & LSR_TEMT) == 0)
 				cpu_relax();
-			si->last_oscr = OSCR;
+			si->last_oscr = readl_relaxed(OSCR);
 
 			/*
 		 	* Ok, we've finished transmitting.  Now enable
@@ -370,7 +370,7 @@ static void pxa_irda_fir_dma_tx_irq(int channel, void *data)
 
 	while (ICSR1 & ICSR1_TBY)
 		cpu_relax();
-	si->last_oscr = OSCR;
+	si->last_oscr = readl_relaxed(OSCR);
 
 	/*
 	 * HACK: It looks like the TBY bit is dropped too soon.
@@ -470,7 +470,7 @@ static irqreturn_t pxa_irda_fir_irq(int irq, void *dev_id)
 
 	/* stop RX DMA */
 	DCSR(si->rxdma) &= ~DCSR_RUN;
-	si->last_oscr = OSCR;
+	si->last_oscr = readl_relaxed(OSCR);
 	icsr0 = ICSR0;
 
 	if (icsr0 & (ICSR0_FRE | ICSR0_RAB)) {
@@ -546,7 +546,7 @@ static int pxa_irda_hard_xmit(struct sk_buff *skb, struct net_device *dev)
 		skb_copy_from_linear_data(skb, si->dma_tx_buff, skb->len);
 
 		if (mtt)
-			while ((unsigned)(OSCR - si->last_oscr)/4 < mtt)
+			while ((unsigned)(readl_relaxed(OSCR) - si->last_oscr)/4 < mtt)
 				cpu_relax();
 
 		/* stop RX DMA,  disable FICP */
diff --git a/drivers/net/loopback.c b/drivers/net/loopback.c
index 32eb94ece6c..e2a06fd996d 100644
--- a/drivers/net/loopback.c
+++ b/drivers/net/loopback.c
@@ -107,10 +107,10 @@ static struct rtnl_link_stats64 *loopback_get_stats64(struct net_device *dev,
 
 		lb_stats = per_cpu_ptr(dev->lstats, i);
 		do {
-			start = u64_stats_fetch_begin(&lb_stats->syncp);
+			start = u64_stats_fetch_begin_bh(&lb_stats->syncp);
 			tbytes = lb_stats->bytes;
 			tpackets = lb_stats->packets;
-		} while (u64_stats_fetch_retry(&lb_stats->syncp, start));
+		} while (u64_stats_fetch_retry_bh(&lb_stats->syncp, start));
 		bytes   += tbytes;
 		packets += tpackets;
 	}
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 2ee56de7b0c..0737bd4d166 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -847,13 +847,12 @@ static ssize_t macvtap_do_read(struct macvtap_queue *q, struct kiocb *iocb,
 			       const struct iovec *iv, unsigned long len,
 			       int noblock)
 {
-	DECLARE_WAITQUEUE(wait, current);
+	DEFINE_WAIT(wait);
 	struct sk_buff *skb;
 	ssize_t ret = 0;
 
-	add_wait_queue(sk_sleep(&q->sk), &wait);
 	while (len) {
-		current->state = TASK_INTERRUPTIBLE;
+		prepare_to_wait(sk_sleep(&q->sk), &wait, TASK_INTERRUPTIBLE);
 
 		/* Read frames from the queue */
 		skb = skb_dequeue(&q->sk.sk_receive_queue);
@@ -875,8 +874,7 @@ static ssize_t macvtap_do_read(struct macvtap_queue *q, struct kiocb *iocb,
 		break;
 	}
 
-	current->state = TASK_RUNNING;
-	remove_wait_queue(sk_sleep(&q->sk), &wait);
+	finish_wait(sk_sleep(&q->sk), &wait);
 	return ret;
 }
 
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 944cdfb80fe..3090dc65a6f 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -67,6 +67,11 @@ config BCM63XX_PHY
 	---help---
 	  Currently supports the 6348 and 6358 PHYs.
 
+config BCM87XX_PHY
+	tristate "Driver for Broadcom BCM8706 and BCM8727 PHYs"
+	help
+	  Currently supports the BCM8706 and BCM8727 10G Ethernet PHYs.
+
 config ICPLUS_PHY
 	tristate "Drivers for ICPlus PHYs"
 	---help---
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index f51af688ef8..6d2dc6c94f2 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_SMSC_PHY)		+= smsc.o
 obj-$(CONFIG_VITESSE_PHY)	+= vitesse.o
 obj-$(CONFIG_BROADCOM_PHY)	+= broadcom.o
 obj-$(CONFIG_BCM63XX_PHY)	+= bcm63xx.o
+obj-$(CONFIG_BCM87XX_PHY)	+= bcm87xx.o
 obj-$(CONFIG_ICPLUS_PHY)	+= icplus.o
 obj-$(CONFIG_REALTEK_PHY)	+= realtek.o
 obj-$(CONFIG_LSI_ET1011C_PHY)	+= et1011c.o
diff --git a/drivers/net/phy/amd.c b/drivers/net/phy/amd.c
index cfabd5fe537..a3fb5ceb648 100644
--- a/drivers/net/phy/amd.c
+++ b/drivers/net/phy/amd.c
@@ -77,13 +77,7 @@ static struct phy_driver am79c_driver = {
 
 static int __init am79c_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&am79c_driver);
-	if (ret)
-		return ret;
-
-	return 0;
+	return phy_driver_register(&am79c_driver);
 }
 
 static void __exit am79c_exit(void)
diff --git a/drivers/net/phy/bcm63xx.c b/drivers/net/phy/bcm63xx.c
index cd802eb25fd..84c7a39b1c6 100644
--- a/drivers/net/phy/bcm63xx.c
+++ b/drivers/net/phy/bcm63xx.c
@@ -71,7 +71,8 @@ static int bcm63xx_config_intr(struct phy_device *phydev)
 	return err;
 }
 
-static struct phy_driver bcm63xx_1_driver = {
+static struct phy_driver bcm63xx_driver[] = {
+{
 	.phy_id		= 0x00406000,
 	.phy_id_mask	= 0xfffffc00,
 	.name		= "Broadcom BCM63XX (1)",
@@ -84,10 +85,8 @@ static struct phy_driver bcm63xx_1_driver = {
 	.ack_interrupt	= bcm63xx_ack_interrupt,
 	.config_intr	= bcm63xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-/* same phy as above, with just a different OUI */
-static struct phy_driver bcm63xx_2_driver = {
+}, {
+	/* same phy as above, with just a different OUI */
 	.phy_id		= 0x002bdc00,
 	.phy_id_mask	= 0xfffffc00,
 	.name		= "Broadcom BCM63XX (2)",
@@ -99,30 +98,18 @@ static struct phy_driver bcm63xx_2_driver = {
 	.ack_interrupt	= bcm63xx_ack_interrupt,
 	.config_intr	= bcm63xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
+} };
 
 static int __init bcm63xx_phy_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&bcm63xx_1_driver);
-	if (ret)
-		goto out_63xx_1;
-	ret = phy_driver_register(&bcm63xx_2_driver);
-	if (ret)
-		goto out_63xx_2;
-	return ret;
-
-out_63xx_2:
-	phy_driver_unregister(&bcm63xx_1_driver);
-out_63xx_1:
-	return ret;
+	return phy_drivers_register(bcm63xx_driver,
+		ARRAY_SIZE(bcm63xx_driver));
 }
 
 static void __exit bcm63xx_phy_exit(void)
 {
-	phy_driver_unregister(&bcm63xx_1_driver);
-	phy_driver_unregister(&bcm63xx_2_driver);
+	phy_drivers_unregister(bcm63xx_driver,
+		ARRAY_SIZE(bcm63xx_driver));
 }
 
 module_init(bcm63xx_phy_init);
diff --git a/drivers/net/phy/bcm87xx.c b/drivers/net/phy/bcm87xx.c
new file mode 100644
index 00000000000..2346b38b983
--- /dev/null
+++ b/drivers/net/phy/bcm87xx.c
@@ -0,0 +1,231 @@
+/*
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 2011 - 2012 Cavium, Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/phy.h>
+#include <linux/of.h>
+
+#define PHY_ID_BCM8706	0x0143bdc1
+#define PHY_ID_BCM8727	0x0143bff0
+
+#define BCM87XX_PMD_RX_SIGNAL_DETECT	(MII_ADDR_C45 | 0x1000a)
+#define BCM87XX_10GBASER_PCS_STATUS	(MII_ADDR_C45 | 0x30020)
+#define BCM87XX_XGXS_LANE_STATUS	(MII_ADDR_C45 | 0x40018)
+
+#define BCM87XX_LASI_CONTROL (MII_ADDR_C45 | 0x39002)
+#define BCM87XX_LASI_STATUS (MII_ADDR_C45 | 0x39005)
+
+#if IS_ENABLED(CONFIG_OF_MDIO)
+/* Set and/or override some configuration registers based on the
+ * broadcom,c45-reg-init property stored in the of_node for the phydev.
+ *
+ * broadcom,c45-reg-init = <devid reg mask value>,...;
+ *
+ * There may be one or more sets of <devid reg mask value>:
+ *
+ * devid: which sub-device to use.
+ * reg: the register.
+ * mask: if non-zero, ANDed with existing register value.
+ * value: ORed with the masked value and written to the regiser.
+ *
+ */
+static int bcm87xx_of_reg_init(struct phy_device *phydev)
+{
+	const __be32 *paddr;
+	const __be32 *paddr_end;
+	int len, ret;
+
+	if (!phydev->dev.of_node)
+		return 0;
+
+	paddr = of_get_property(phydev->dev.of_node,
+				"broadcom,c45-reg-init", &len);
+	if (!paddr)
+		return 0;
+
+	paddr_end = paddr + (len /= sizeof(*paddr));
+
+	ret = 0;
+
+	while (paddr + 3 < paddr_end) {
+		u16 devid	= be32_to_cpup(paddr++);
+		u16 reg		= be32_to_cpup(paddr++);
+		u16 mask	= be32_to_cpup(paddr++);
+		u16 val_bits	= be32_to_cpup(paddr++);
+		int val;
+		u32 regnum = MII_ADDR_C45 | (devid << 16) | reg;
+		val = 0;
+		if (mask) {
+			val = phy_read(phydev, regnum);
+			if (val < 0) {
+				ret = val;
+				goto err;
+			}
+			val &= mask;
+		}
+		val |= val_bits;
+
+		ret = phy_write(phydev, regnum, val);
+		if (ret < 0)
+			goto err;
+	}
+err:
+	return ret;
+}
+#else
+static int bcm87xx_of_reg_init(struct phy_device *phydev)
+{
+	return 0;
+}
+#endif /* CONFIG_OF_MDIO */
+
+static int bcm87xx_config_init(struct phy_device *phydev)
+{
+	phydev->supported = SUPPORTED_10000baseR_FEC;
+	phydev->advertising = ADVERTISED_10000baseR_FEC;
+	phydev->state = PHY_NOLINK;
+	phydev->autoneg = AUTONEG_DISABLE;
+
+	bcm87xx_of_reg_init(phydev);
+
+	return 0;
+}
+
+static int bcm87xx_config_aneg(struct phy_device *phydev)
+{
+	return -EINVAL;
+}
+
+static int bcm87xx_read_status(struct phy_device *phydev)
+{
+	int rx_signal_detect;
+	int pcs_status;
+	int xgxs_lane_status;
+
+	rx_signal_detect = phy_read(phydev, BCM87XX_PMD_RX_SIGNAL_DETECT);
+	if (rx_signal_detect < 0)
+		return rx_signal_detect;
+
+	if ((rx_signal_detect & 1) == 0)
+		goto no_link;
+
+	pcs_status = phy_read(phydev, BCM87XX_10GBASER_PCS_STATUS);
+	if (pcs_status < 0)
+		return pcs_status;
+
+	if ((pcs_status & 1) == 0)
+		goto no_link;
+
+	xgxs_lane_status = phy_read(phydev, BCM87XX_XGXS_LANE_STATUS);
+	if (xgxs_lane_status < 0)
+		return xgxs_lane_status;
+
+	if ((xgxs_lane_status & 0x1000) == 0)
+		goto no_link;
+
+	phydev->speed = 10000;
+	phydev->link = 1;
+	phydev->duplex = 1;
+	return 0;
+
+no_link:
+	phydev->link = 0;
+	return 0;
+}
+
+static int bcm87xx_config_intr(struct phy_device *phydev)
+{
+	int reg, err;
+
+	reg = phy_read(phydev, BCM87XX_LASI_CONTROL);
+
+	if (reg < 0)
+		return reg;
+
+	if (phydev->interrupts == PHY_INTERRUPT_ENABLED)
+		reg |= 1;
+	else
+		reg &= ~1;
+
+	err = phy_write(phydev, BCM87XX_LASI_CONTROL, reg);
+	return err;
+}
+
+static int bcm87xx_did_interrupt(struct phy_device *phydev)
+{
+	int reg;
+
+	reg = phy_read(phydev, BCM87XX_LASI_STATUS);
+
+	if (reg < 0) {
+		dev_err(&phydev->dev,
+			"Error: Read of BCM87XX_LASI_STATUS failed: %d\n", reg);
+		return 0;
+	}
+	return (reg & 1) != 0;
+}
+
+static int bcm87xx_ack_interrupt(struct phy_device *phydev)
+{
+	/* Reading the LASI status clears it. */
+	bcm87xx_did_interrupt(phydev);
+	return 0;
+}
+
+static int bcm8706_match_phy_device(struct phy_device *phydev)
+{
+	return phydev->c45_ids.device_ids[4] == PHY_ID_BCM8706;
+}
+
+static int bcm8727_match_phy_device(struct phy_device *phydev)
+{
+	return phydev->c45_ids.device_ids[4] == PHY_ID_BCM8727;
+}
+
+static struct phy_driver bcm87xx_driver[] = {
+{
+	.phy_id		= PHY_ID_BCM8706,
+	.phy_id_mask	= 0xffffffff,
+	.name		= "Broadcom BCM8706",
+	.flags		= PHY_HAS_INTERRUPT,
+	.config_init	= bcm87xx_config_init,
+	.config_aneg	= bcm87xx_config_aneg,
+	.read_status	= bcm87xx_read_status,
+	.ack_interrupt	= bcm87xx_ack_interrupt,
+	.config_intr	= bcm87xx_config_intr,
+	.did_interrupt	= bcm87xx_did_interrupt,
+	.match_phy_device = bcm8706_match_phy_device,
+	.driver		= { .owner = THIS_MODULE },
+}, {
+	.phy_id		= PHY_ID_BCM8727,
+	.phy_id_mask	= 0xffffffff,
+	.name		= "Broadcom BCM8727",
+	.flags		= PHY_HAS_INTERRUPT,
+	.config_init	= bcm87xx_config_init,
+	.config_aneg	= bcm87xx_config_aneg,
+	.read_status	= bcm87xx_read_status,
+	.ack_interrupt	= bcm87xx_ack_interrupt,
+	.config_intr	= bcm87xx_config_intr,
+	.did_interrupt	= bcm87xx_did_interrupt,
+	.match_phy_device = bcm8727_match_phy_device,
+	.driver		= { .owner = THIS_MODULE },
+} };
+
+static int __init bcm87xx_init(void)
+{
+	return phy_drivers_register(bcm87xx_driver,
+		ARRAY_SIZE(bcm87xx_driver));
+}
+module_init(bcm87xx_init);
+
+static void __exit bcm87xx_exit(void)
+{
+	phy_drivers_unregister(bcm87xx_driver,
+		ARRAY_SIZE(bcm87xx_driver));
+}
+module_exit(bcm87xx_exit);
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 60338ff6309..f8c90ea7510 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -682,7 +682,8 @@ static int brcm_fet_config_intr(struct phy_device *phydev)
 	return err;
 }
 
-static struct phy_driver bcm5411_driver = {
+static struct phy_driver broadcom_drivers[] = {
+{
 	.phy_id		= PHY_ID_BCM5411,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5411",
@@ -695,9 +696,7 @@ static struct phy_driver bcm5411_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm5421_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM5421,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5421",
@@ -710,9 +709,7 @@ static struct phy_driver bcm5421_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm5461_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM5461,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5461",
@@ -725,9 +722,7 @@ static struct phy_driver bcm5461_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm5464_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM5464,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5464",
@@ -740,9 +735,7 @@ static struct phy_driver bcm5464_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm5481_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM5481,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5481",
@@ -755,9 +748,7 @@ static struct phy_driver bcm5481_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm5482_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM5482,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5482",
@@ -770,9 +761,7 @@ static struct phy_driver bcm5482_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm50610_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM50610,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM50610",
@@ -785,9 +774,7 @@ static struct phy_driver bcm50610_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm50610m_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM50610M,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM50610M",
@@ -800,9 +787,7 @@ static struct phy_driver bcm50610m_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm57780_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM57780,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM57780",
@@ -815,9 +800,7 @@ static struct phy_driver bcm57780_driver = {
 	.ack_interrupt	= bcm54xx_ack_interrupt,
 	.config_intr	= bcm54xx_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcmac131_driver = {
+}, {
 	.phy_id		= PHY_ID_BCMAC131,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCMAC131",
@@ -830,9 +813,7 @@ static struct phy_driver bcmac131_driver = {
 	.ack_interrupt	= brcm_fet_ack_interrupt,
 	.config_intr	= brcm_fet_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
-
-static struct phy_driver bcm5241_driver = {
+}, {
 	.phy_id		= PHY_ID_BCM5241,
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "Broadcom BCM5241",
@@ -845,84 +826,18 @@ static struct phy_driver bcm5241_driver = {
 	.ack_interrupt	= brcm_fet_ack_interrupt,
 	.config_intr	= brcm_fet_config_intr,
 	.driver		= { .owner = THIS_MODULE },
-};
+} };
 
 static int __init broadcom_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&bcm5411_driver);
-	if (ret)
-		goto out_5411;
-	ret = phy_driver_register(&bcm5421_driver);
-	if (ret)
-		goto out_5421;
-	ret = phy_driver_register(&bcm5461_driver);
-	if (ret)
-		goto out_5461;
-	ret = phy_driver_register(&bcm5464_driver);
-	if (ret)
-		goto out_5464;
-	ret = phy_driver_register(&bcm5481_driver);
-	if (ret)
-		goto out_5481;
-	ret = phy_driver_register(&bcm5482_driver);
-	if (ret)
-		goto out_5482;
-	ret = phy_driver_register(&bcm50610_driver);
-	if (ret)
-		goto out_50610;
-	ret = phy_driver_register(&bcm50610m_driver);
-	if (ret)
-		goto out_50610m;
-	ret = phy_driver_register(&bcm57780_driver);
-	if (ret)
-		goto out_57780;
-	ret = phy_driver_register(&bcmac131_driver);
-	if (ret)
-		goto out_ac131;
-	ret = phy_driver_register(&bcm5241_driver);
-	if (ret)
-		goto out_5241;
-	return ret;
-
-out_5241:
-	phy_driver_unregister(&bcmac131_driver);
-out_ac131:
-	phy_driver_unregister(&bcm57780_driver);
-out_57780:
-	phy_driver_unregister(&bcm50610m_driver);
-out_50610m:
-	phy_driver_unregister(&bcm50610_driver);
-out_50610:
-	phy_driver_unregister(&bcm5482_driver);
-out_5482:
-	phy_driver_unregister(&bcm5481_driver);
-out_5481:
-	phy_driver_unregister(&bcm5464_driver);
-out_5464:
-	phy_driver_unregister(&bcm5461_driver);
-out_5461:
-	phy_driver_unregister(&bcm5421_driver);
-out_5421:
-	phy_driver_unregister(&bcm5411_driver);
-out_5411:
-	return ret;
+	return phy_drivers_register(broadcom_drivers,
+		ARRAY_SIZE(broadcom_drivers));
 }
 
 static void __exit broadcom_exit(void)
 {
-	phy_driver_unregister(&bcm5241_driver);
-	phy_driver_unregister(&bcmac131_driver);
-	phy_driver_unregister(&bcm57780_driver);
-	phy_driver_unregister(&bcm50610m_driver);
-	phy_driver_unregister(&bcm50610_driver);
-	phy_driver_unregister(&bcm5482_driver);
-	phy_driver_unregister(&bcm5481_driver);
-	phy_driver_unregister(&bcm5464_driver);
-	phy_driver_unregister(&bcm5461_driver);
-	phy_driver_unregister(&bcm5421_driver);
-	phy_driver_unregister(&bcm5411_driver);
+	phy_drivers_unregister(broadcom_drivers,
+		ARRAY_SIZE(broadcom_drivers));
 }
 
 module_init(broadcom_init);
diff --git a/drivers/net/phy/cicada.c b/drivers/net/phy/cicada.c
index d28173161c2..db472ffb6e8 100644
--- a/drivers/net/phy/cicada.c
+++ b/drivers/net/phy/cicada.c
@@ -102,7 +102,8 @@ static int cis820x_config_intr(struct phy_device *phydev)
 }
 
 /* Cicada 8201, a.k.a Vitesse VSC8201 */
-static struct phy_driver cis8201_driver = {
+static struct phy_driver cis820x_driver[] = {
+{
 	.phy_id		= 0x000fc410,
 	.name		= "Cicada Cis8201",
 	.phy_id_mask	= 0x000ffff0,
@@ -113,11 +114,8 @@ static struct phy_driver cis8201_driver = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &cis820x_ack_interrupt,
 	.config_intr	= &cis820x_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
-};
-
-/* Cicada 8204 */
-static struct phy_driver cis8204_driver = {
+	.driver		= { .owner = THIS_MODULE,},
+}, {
 	.phy_id		= 0x000fc440,
 	.name		= "Cicada Cis8204",
 	.phy_id_mask	= 0x000fffc0,
@@ -128,32 +126,19 @@ static struct phy_driver cis8204_driver = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &cis820x_ack_interrupt,
 	.config_intr	= &cis820x_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
-};
+	.driver		= { .owner = THIS_MODULE,},
+} };
 
 static int __init cicada_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&cis8204_driver);
-	if (ret)
-		goto err1;
-
-	ret = phy_driver_register(&cis8201_driver);
-	if (ret)
-		goto err2;
-	return 0;
-
-err2:
-	phy_driver_unregister(&cis8204_driver);
-err1:
-	return ret;
+	return phy_drivers_register(cis820x_driver,
+		ARRAY_SIZE(cis820x_driver));
 }
 
 static void __exit cicada_exit(void)
 {
-	phy_driver_unregister(&cis8204_driver);
-	phy_driver_unregister(&cis8201_driver);
+	phy_drivers_unregister(cis820x_driver,
+		ARRAY_SIZE(cis820x_driver));
 }
 
 module_init(cicada_init);
diff --git a/drivers/net/phy/davicom.c b/drivers/net/phy/davicom.c
index 5f59cc06477..81c7bc010dd 100644
--- a/drivers/net/phy/davicom.c
+++ b/drivers/net/phy/davicom.c
@@ -144,7 +144,8 @@ static int dm9161_ack_interrupt(struct phy_device *phydev)
 	return (err < 0) ? err : 0;
 }
 
-static struct phy_driver dm9161e_driver = {
+static struct phy_driver dm91xx_driver[] = {
+{
 	.phy_id		= 0x0181b880,
 	.name		= "Davicom DM9161E",
 	.phy_id_mask	= 0x0ffffff0,
@@ -153,9 +154,7 @@ static struct phy_driver dm9161e_driver = {
 	.config_aneg	= dm9161_config_aneg,
 	.read_status	= genphy_read_status,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver dm9161a_driver = {
+}, {
 	.phy_id		= 0x0181b8a0,
 	.name		= "Davicom DM9161A",
 	.phy_id_mask	= 0x0ffffff0,
@@ -164,9 +163,7 @@ static struct phy_driver dm9161a_driver = {
 	.config_aneg	= dm9161_config_aneg,
 	.read_status	= genphy_read_status,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver dm9131_driver = {
+}, {
 	.phy_id		= 0x00181b80,
 	.name		= "Davicom DM9131",
 	.phy_id_mask	= 0x0ffffff0,
@@ -177,38 +174,18 @@ static struct phy_driver dm9131_driver = {
 	.ack_interrupt	= dm9161_ack_interrupt,
 	.config_intr	= dm9161_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
-};
+} };
 
 static int __init davicom_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&dm9161e_driver);
-	if (ret)
-		goto err1;
-
-	ret = phy_driver_register(&dm9161a_driver);
-	if (ret)
-		goto err2;
-
-	ret = phy_driver_register(&dm9131_driver);
-	if (ret)
-		goto err3;
-	return 0;
-
- err3:
-	phy_driver_unregister(&dm9161a_driver);
- err2:
-	phy_driver_unregister(&dm9161e_driver);
- err1:
-	return ret;
+	return phy_drivers_register(dm91xx_driver,
+		ARRAY_SIZE(dm91xx_driver));
 }
 
 static void __exit davicom_exit(void)
 {
-	phy_driver_unregister(&dm9161e_driver);
-	phy_driver_unregister(&dm9161a_driver);
-	phy_driver_unregister(&dm9131_driver);
+	phy_drivers_unregister(dm91xx_driver,
+		ARRAY_SIZE(dm91xx_driver));
 }
 
 module_init(davicom_init);
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index 940b29022d0..b0da0226661 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -17,6 +17,9 @@
  *  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/ethtool.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
@@ -453,16 +456,16 @@ static void enable_status_frames(struct phy_device *phydev, bool on)
 	ext_write(0, phydev, PAGE6, PSF_CFG1, ver);
 
 	if (!phydev->attached_dev) {
-		pr_warning("dp83640: expected to find an attached netdevice\n");
+		pr_warn("expected to find an attached netdevice\n");
 		return;
 	}
 
 	if (on) {
 		if (dev_mc_add(phydev->attached_dev, status_frame_dst))
-			pr_warning("dp83640: failed to add mc address\n");
+			pr_warn("failed to add mc address\n");
 	} else {
 		if (dev_mc_del(phydev->attached_dev, status_frame_dst))
-			pr_warning("dp83640: failed to delete mc address\n");
+			pr_warn("failed to delete mc address\n");
 	}
 }
 
@@ -582,9 +585,9 @@ static void recalibrate(struct dp83640_clock *clock)
 	 * read out and correct offsets
 	 */
 	val = ext_read(master, PAGE4, PTP_STS);
-	pr_info("master PTP_STS  0x%04hx", val);
+	pr_info("master PTP_STS  0x%04hx\n", val);
 	val = ext_read(master, PAGE4, PTP_ESTS);
-	pr_info("master PTP_ESTS 0x%04hx", val);
+	pr_info("master PTP_ESTS 0x%04hx\n", val);
 	event_ts.ns_lo  = ext_read(master, PAGE4, PTP_EDATA);
 	event_ts.ns_hi  = ext_read(master, PAGE4, PTP_EDATA);
 	event_ts.sec_lo = ext_read(master, PAGE4, PTP_EDATA);
@@ -594,9 +597,9 @@ static void recalibrate(struct dp83640_clock *clock)
 	list_for_each(this, &clock->phylist) {
 		tmp = list_entry(this, struct dp83640_private, list);
 		val = ext_read(tmp->phydev, PAGE4, PTP_STS);
-		pr_info("slave  PTP_STS  0x%04hx", val);
+		pr_info("slave  PTP_STS  0x%04hx\n", val);
 		val = ext_read(tmp->phydev, PAGE4, PTP_ESTS);
-		pr_info("slave  PTP_ESTS 0x%04hx", val);
+		pr_info("slave  PTP_ESTS 0x%04hx\n", val);
 		event_ts.ns_lo  = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
 		event_ts.ns_hi  = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
 		event_ts.sec_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
@@ -686,7 +689,7 @@ static void decode_rxts(struct dp83640_private *dp83640,
 	prune_rx_ts(dp83640);
 
 	if (list_empty(&dp83640->rxpool)) {
-		pr_debug("dp83640: rx timestamp pool is empty\n");
+		pr_debug("rx timestamp pool is empty\n");
 		goto out;
 	}
 	rxts = list_first_entry(&dp83640->rxpool, struct rxts, list);
@@ -709,7 +712,7 @@ static void decode_txts(struct dp83640_private *dp83640,
 	skb = skb_dequeue(&dp83640->tx_queue);
 
 	if (!skb) {
-		pr_debug("dp83640: have timestamp but tx_queue empty\n");
+		pr_debug("have timestamp but tx_queue empty\n");
 		return;
 	}
 	ns = phy2txts(phy_txts);
@@ -847,7 +850,7 @@ static void dp83640_free_clocks(void)
 	list_for_each_safe(this, next, &phyter_clocks) {
 		clock = list_entry(this, struct dp83640_clock, list);
 		if (!list_empty(&clock->phylist)) {
-			pr_warning("phy list non-empty while unloading");
+			pr_warn("phy list non-empty while unloading\n");
 			BUG();
 		}
 		list_del(&clock->list);
diff --git a/drivers/net/phy/fixed.c b/drivers/net/phy/fixed.c
index 633680d0828..ba55adfc7aa 100644
--- a/drivers/net/phy/fixed.c
+++ b/drivers/net/phy/fixed.c
@@ -70,7 +70,7 @@ static int fixed_phy_update_regs(struct fixed_phy *fp)
 			lpa |= LPA_10FULL;
 			break;
 		default:
-			printk(KERN_WARNING "fixed phy: unknown speed\n");
+			pr_warn("fixed phy: unknown speed\n");
 			return -EINVAL;
 		}
 	} else {
@@ -90,7 +90,7 @@ static int fixed_phy_update_regs(struct fixed_phy *fp)
 			lpa |= LPA_10HALF;
 			break;
 		default:
-			printk(KERN_WARNING "fixed phy: unknown speed\n");
+			pr_warn("fixed phy: unknown speed\n");
 			return -EINVAL;
 		}
 	}
diff --git a/drivers/net/phy/icplus.c b/drivers/net/phy/icplus.c
index 47f8e893926..d5199cb4cae 100644
--- a/drivers/net/phy/icplus.c
+++ b/drivers/net/phy/icplus.c
@@ -202,7 +202,8 @@ static int ip101a_g_ack_interrupt(struct phy_device *phydev)
 	return 0;
 }
 
-static struct phy_driver ip175c_driver = {
+static struct phy_driver icplus_driver[] = {
+{
 	.phy_id		= 0x02430d80,
 	.name		= "ICPlus IP175C",
 	.phy_id_mask	= 0x0ffffff0,
@@ -213,9 +214,7 @@ static struct phy_driver ip175c_driver = {
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver ip1001_driver = {
+}, {
 	.phy_id		= 0x02430d90,
 	.name		= "ICPlus IP1001",
 	.phy_id_mask	= 0x0ffffff0,
@@ -227,9 +226,7 @@ static struct phy_driver ip1001_driver = {
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver ip101a_g_driver = {
+}, {
 	.phy_id		= 0x02430c54,
 	.name		= "ICPlus IP101A/G",
 	.phy_id_mask	= 0x0ffffff0,
@@ -243,28 +240,18 @@ static struct phy_driver ip101a_g_driver = {
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 	.driver		= { .owner = THIS_MODULE,},
-};
+} };
 
 static int __init icplus_init(void)
 {
-	int ret = 0;
-
-	ret = phy_driver_register(&ip1001_driver);
-	if (ret < 0)
-		return -ENODEV;
-
-	ret = phy_driver_register(&ip101a_g_driver);
-	if (ret < 0)
-		return -ENODEV;
-
-	return phy_driver_register(&ip175c_driver);
+	return phy_drivers_register(icplus_driver,
+		ARRAY_SIZE(icplus_driver));
 }
 
 static void __exit icplus_exit(void)
 {
-	phy_driver_unregister(&ip1001_driver);
-	phy_driver_unregister(&ip101a_g_driver);
-	phy_driver_unregister(&ip175c_driver);
+	phy_drivers_unregister(icplus_driver,
+		ARRAY_SIZE(icplus_driver));
 }
 
 module_init(icplus_init);
diff --git a/drivers/net/phy/lxt.c b/drivers/net/phy/lxt.c
index 6f6e8b616a6..6d1e3fcc43e 100644
--- a/drivers/net/phy/lxt.c
+++ b/drivers/net/phy/lxt.c
@@ -149,7 +149,8 @@ static int lxt973_config_aneg(struct phy_device *phydev)
 	return phydev->priv ? 0 : genphy_config_aneg(phydev);
 }
 
-static struct phy_driver lxt970_driver = {
+static struct phy_driver lxt97x_driver[] = {
+{
 	.phy_id		= 0x78100000,
 	.name		= "LXT970",
 	.phy_id_mask	= 0xfffffff0,
@@ -160,10 +161,8 @@ static struct phy_driver lxt970_driver = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= lxt970_ack_interrupt,
 	.config_intr	= lxt970_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver lxt971_driver = {
+	.driver		= { .owner = THIS_MODULE,},
+}, {
 	.phy_id		= 0x001378e0,
 	.name		= "LXT971",
 	.phy_id_mask	= 0xfffffff0,
@@ -173,10 +172,8 @@ static struct phy_driver lxt971_driver = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= lxt971_ack_interrupt,
 	.config_intr	= lxt971_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver lxt973_driver = {
+	.driver		= { .owner = THIS_MODULE,},
+}, {
 	.phy_id		= 0x00137a10,
 	.name		= "LXT973",
 	.phy_id_mask	= 0xfffffff0,
@@ -185,39 +182,19 @@ static struct phy_driver lxt973_driver = {
 	.probe		= lxt973_probe,
 	.config_aneg	= lxt973_config_aneg,
 	.read_status	= genphy_read_status,
-	.driver 	= { .owner = THIS_MODULE,},
-};
+	.driver		= { .owner = THIS_MODULE,},
+} };
 
 static int __init lxt_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&lxt970_driver);
-	if (ret)
-		goto err1;
-
-	ret = phy_driver_register(&lxt971_driver);
-	if (ret)
-		goto err2;
-
-	ret = phy_driver_register(&lxt973_driver);
-	if (ret)
-		goto err3;
-	return 0;
-
- err3:
-	phy_driver_unregister(&lxt971_driver);
- err2:
-	phy_driver_unregister(&lxt970_driver);
- err1:
-	return ret;
+	return phy_drivers_register(lxt97x_driver,
+		ARRAY_SIZE(lxt97x_driver));
 }
 
 static void __exit lxt_exit(void)
 {
-	phy_driver_unregister(&lxt970_driver);
-	phy_driver_unregister(&lxt971_driver);
-	phy_driver_unregister(&lxt973_driver);
+	phy_drivers_unregister(lxt97x_driver,
+		ARRAY_SIZE(lxt97x_driver));
 }
 
 module_init(lxt_init);
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index 418928d644b..5d2a3f21588 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -826,28 +826,14 @@ static struct phy_driver marvell_drivers[] = {
 
 static int __init marvell_init(void)
 {
-	int ret;
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(marvell_drivers); i++) {
-		ret = phy_driver_register(&marvell_drivers[i]);
-
-		if (ret) {
-			while (i-- > 0)
-				phy_driver_unregister(&marvell_drivers[i]);
-			return ret;
-		}
-	}
-
-	return 0;
+	return phy_drivers_register(marvell_drivers,
+		 ARRAY_SIZE(marvell_drivers));
 }
 
 static void __exit marvell_exit(void)
 {
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(marvell_drivers); i++)
-		phy_driver_unregister(&marvell_drivers[i]);
+	phy_drivers_unregister(marvell_drivers,
+		 ARRAY_SIZE(marvell_drivers));
 }
 
 module_init(marvell_init);
diff --git a/drivers/net/phy/mdio-mux.c b/drivers/net/phy/mdio-mux.c
index 39ea0674dcd..5c120189ec8 100644
--- a/drivers/net/phy/mdio-mux.c
+++ b/drivers/net/phy/mdio-mux.c
@@ -46,7 +46,13 @@ static int mdio_mux_read(struct mii_bus *bus, int phy_id, int regnum)
 	struct mdio_mux_parent_bus *pb = cb->parent;
 	int r;
 
-	mutex_lock(&pb->mii_bus->mdio_lock);
+	/* In theory multiple mdio_mux could be stacked, thus creating
+	 * more than a single level of nesting.  But in practice,
+	 * SINGLE_DEPTH_NESTING will cover the vast majority of use
+	 * cases.  We use it, instead of trying to handle the general
+	 * case.
+	 */
+	mutex_lock_nested(&pb->mii_bus->mdio_lock, SINGLE_DEPTH_NESTING);
 	r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
 	if (r)
 		goto out;
@@ -71,7 +77,7 @@ static int mdio_mux_write(struct mii_bus *bus, int phy_id,
 
 	int r;
 
-	mutex_lock(&pb->mii_bus->mdio_lock);
+	mutex_lock_nested(&pb->mii_bus->mdio_lock, SINGLE_DEPTH_NESTING);
 	r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
 	if (r)
 		goto out;
diff --git a/drivers/net/phy/mdio-octeon.c b/drivers/net/phy/mdio-octeon.c
index 826d961f39f..d4015aa663e 100644
--- a/drivers/net/phy/mdio-octeon.c
+++ b/drivers/net/phy/mdio-octeon.c
@@ -3,14 +3,17 @@
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  *
- * Copyright (C) 2009 Cavium Networks
+ * Copyright (C) 2009,2011 Cavium, Inc.
  */
 
-#include <linux/gfp.h>
-#include <linux/init.h>
-#include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/of_mdio.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/gfp.h>
 #include <linux/phy.h>
+#include <linux/io.h>
 
 #include <asm/octeon/octeon.h>
 #include <asm/octeon/cvmx-smix-defs.h>
@@ -18,9 +21,17 @@
 #define DRV_VERSION "1.0"
 #define DRV_DESCRIPTION "Cavium Networks Octeon SMI/MDIO driver"
 
+#define SMI_CMD		0x0
+#define SMI_WR_DAT	0x8
+#define SMI_RD_DAT	0x10
+#define SMI_CLK		0x18
+#define SMI_EN		0x20
+
 struct octeon_mdiobus {
 	struct mii_bus *mii_bus;
-	int unit;
+	u64 register_base;
+	resource_size_t mdio_phys;
+	resource_size_t regsize;
 	int phy_irq[PHY_MAX_ADDR];
 };
 
@@ -35,15 +46,15 @@ static int octeon_mdiobus_read(struct mii_bus *bus, int phy_id, int regnum)
 	smi_cmd.s.phy_op = 1; /* MDIO_CLAUSE_22_READ */
 	smi_cmd.s.phy_adr = phy_id;
 	smi_cmd.s.reg_adr = regnum;
-	cvmx_write_csr(CVMX_SMIX_CMD(p->unit), smi_cmd.u64);
+	cvmx_write_csr(p->register_base + SMI_CMD, smi_cmd.u64);
 
 	do {
 		/*
 		 * Wait 1000 clocks so we don't saturate the RSL bus
 		 * doing reads.
 		 */
-		cvmx_wait(1000);
-		smi_rd.u64 = cvmx_read_csr(CVMX_SMIX_RD_DAT(p->unit));
+		__delay(1000);
+		smi_rd.u64 = cvmx_read_csr(p->register_base + SMI_RD_DAT);
 	} while (smi_rd.s.pending && --timeout);
 
 	if (smi_rd.s.val)
@@ -62,21 +73,21 @@ static int octeon_mdiobus_write(struct mii_bus *bus, int phy_id,
 
 	smi_wr.u64 = 0;
 	smi_wr.s.dat = val;
-	cvmx_write_csr(CVMX_SMIX_WR_DAT(p->unit), smi_wr.u64);
+	cvmx_write_csr(p->register_base + SMI_WR_DAT, smi_wr.u64);
 
 	smi_cmd.u64 = 0;
 	smi_cmd.s.phy_op = 0; /* MDIO_CLAUSE_22_WRITE */
 	smi_cmd.s.phy_adr = phy_id;
 	smi_cmd.s.reg_adr = regnum;
-	cvmx_write_csr(CVMX_SMIX_CMD(p->unit), smi_cmd.u64);
+	cvmx_write_csr(p->register_base + SMI_CMD, smi_cmd.u64);
 
 	do {
 		/*
 		 * Wait 1000 clocks so we don't saturate the RSL bus
 		 * doing reads.
 		 */
-		cvmx_wait(1000);
-		smi_wr.u64 = cvmx_read_csr(CVMX_SMIX_WR_DAT(p->unit));
+		__delay(1000);
+		smi_wr.u64 = cvmx_read_csr(p->register_base + SMI_WR_DAT);
 	} while (smi_wr.s.pending && --timeout);
 
 	if (timeout <= 0)
@@ -88,38 +99,44 @@ static int octeon_mdiobus_write(struct mii_bus *bus, int phy_id,
 static int __devinit octeon_mdiobus_probe(struct platform_device *pdev)
 {
 	struct octeon_mdiobus *bus;
+	struct resource *res_mem;
 	union cvmx_smix_en smi_en;
-	int i;
 	int err = -ENOENT;
 
 	bus = devm_kzalloc(&pdev->dev, sizeof(*bus), GFP_KERNEL);
 	if (!bus)
 		return -ENOMEM;
 
-	/* The platform_device id is our unit number.  */
-	bus->unit = pdev->id;
+	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (res_mem == NULL) {
+		dev_err(&pdev->dev, "found no memory resource\n");
+		err = -ENXIO;
+		goto fail;
+	}
+	bus->mdio_phys = res_mem->start;
+	bus->regsize = resource_size(res_mem);
+	if (!devm_request_mem_region(&pdev->dev, bus->mdio_phys, bus->regsize,
+				     res_mem->name)) {
+		dev_err(&pdev->dev, "request_mem_region failed\n");
+		goto fail;
+	}
+	bus->register_base =
+		(u64)devm_ioremap(&pdev->dev, bus->mdio_phys, bus->regsize);
 
 	bus->mii_bus = mdiobus_alloc();
 
 	if (!bus->mii_bus)
-		goto err;
+		goto fail;
 
 	smi_en.u64 = 0;
 	smi_en.s.en = 1;
-	cvmx_write_csr(CVMX_SMIX_EN(bus->unit), smi_en.u64);
-
-	/*
-	 * Standard Octeon evaluation boards don't support phy
-	 * interrupts, we need to poll.
-	 */
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		bus->phy_irq[i] = PHY_POLL;
+	cvmx_write_csr(bus->register_base + SMI_EN, smi_en.u64);
 
 	bus->mii_bus->priv = bus;
 	bus->mii_bus->irq = bus->phy_irq;
 	bus->mii_bus->name = "mdio-octeon";
-	snprintf(bus->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
-		bus->mii_bus->name, bus->unit);
+	snprintf(bus->mii_bus->id, MII_BUS_ID_SIZE, "%llx", bus->register_base);
 	bus->mii_bus->parent = &pdev->dev;
 
 	bus->mii_bus->read = octeon_mdiobus_read;
@@ -127,20 +144,18 @@ static int __devinit octeon_mdiobus_probe(struct platform_device *pdev)
 
 	dev_set_drvdata(&pdev->dev, bus);
 
-	err = mdiobus_register(bus->mii_bus);
+	err = of_mdiobus_register(bus->mii_bus, pdev->dev.of_node);
 	if (err)
-		goto err_register;
+		goto fail_register;
 
 	dev_info(&pdev->dev, "Version " DRV_VERSION "\n");
 
 	return 0;
-err_register:
+fail_register:
 	mdiobus_free(bus->mii_bus);
-
-err:
-	devm_kfree(&pdev->dev, bus);
+fail:
 	smi_en.u64 = 0;
-	cvmx_write_csr(CVMX_SMIX_EN(bus->unit), smi_en.u64);
+	cvmx_write_csr(bus->register_base + SMI_EN, smi_en.u64);
 	return err;
 }
 
@@ -154,14 +169,23 @@ static int __devexit octeon_mdiobus_remove(struct platform_device *pdev)
 	mdiobus_unregister(bus->mii_bus);
 	mdiobus_free(bus->mii_bus);
 	smi_en.u64 = 0;
-	cvmx_write_csr(CVMX_SMIX_EN(bus->unit), smi_en.u64);
+	cvmx_write_csr(bus->register_base + SMI_EN, smi_en.u64);
 	return 0;
 }
 
+static struct of_device_id octeon_mdiobus_match[] = {
+	{
+		.compatible = "cavium,octeon-3860-mdio",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, octeon_mdiobus_match);
+
 static struct platform_driver octeon_mdiobus_driver = {
 	.driver = {
 		.name		= "mdio-octeon",
 		.owner		= THIS_MODULE,
+		.of_match_table = octeon_mdiobus_match,
 	},
 	.probe		= octeon_mdiobus_probe,
 	.remove		= __devexit_p(octeon_mdiobus_remove),
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 5061608f408..170eb411ab5 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -13,6 +13,9 @@
  * option) any later version.
  *
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/string.h>
 #include <linux/errno.h>
@@ -22,6 +25,7 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/of_device.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
@@ -148,7 +152,7 @@ int mdiobus_register(struct mii_bus *bus)
 
 	err = device_register(&bus->dev);
 	if (err) {
-		printk(KERN_ERR "mii_bus %s failed to register\n", bus->id);
+		pr_err("mii_bus %s failed to register\n", bus->id);
 		return -EINVAL;
 	}
 
@@ -229,7 +233,7 @@ struct phy_device *mdiobus_scan(struct mii_bus *bus, int addr)
 	struct phy_device *phydev;
 	int err;
 
-	phydev = get_phy_device(bus, addr);
+	phydev = get_phy_device(bus, addr, false);
 	if (IS_ERR(phydev) || phydev == NULL)
 		return phydev;
 
@@ -305,6 +309,12 @@ static int mdio_bus_match(struct device *dev, struct device_driver *drv)
 	struct phy_device *phydev = to_phy_device(dev);
 	struct phy_driver *phydrv = to_phy_driver(drv);
 
+	if (of_driver_match_device(dev, drv))
+		return 1;
+
+	if (phydrv->match_phy_device)
+		return phydrv->match_phy_device(phydev);
+
 	return ((phydrv->phy_id & phydrv->phy_id_mask) ==
 		(phydev->phy_id & phydrv->phy_id_mask));
 }
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 9d6c80c8a0c..cf287e0eb40 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -114,7 +114,8 @@ static int ks8051_config_init(struct phy_device *phydev)
 	return 0;
 }
 
-static struct phy_driver ks8737_driver = {
+static struct phy_driver ksphy_driver[] = {
+{
 	.phy_id		= PHY_ID_KS8737,
 	.phy_id_mask	= 0x00fffff0,
 	.name		= "Micrel KS8737",
@@ -126,9 +127,7 @@ static struct phy_driver ks8737_driver = {
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= ks8737_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver ks8041_driver = {
+}, {
 	.phy_id		= PHY_ID_KS8041,
 	.phy_id_mask	= 0x00fffff0,
 	.name		= "Micrel KS8041",
@@ -141,9 +140,7 @@ static struct phy_driver ks8041_driver = {
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver ks8051_driver = {
+}, {
 	.phy_id		= PHY_ID_KS8051,
 	.phy_id_mask	= 0x00fffff0,
 	.name		= "Micrel KS8051",
@@ -156,9 +153,7 @@ static struct phy_driver ks8051_driver = {
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver ks8001_driver = {
+}, {
 	.phy_id		= PHY_ID_KS8001,
 	.name		= "Micrel KS8001 or KS8721",
 	.phy_id_mask	= 0x00ffffff,
@@ -170,9 +165,7 @@ static struct phy_driver ks8001_driver = {
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
-};
-
-static struct phy_driver ksz9021_driver = {
+}, {
 	.phy_id		= PHY_ID_KSZ9021,
 	.phy_id_mask	= 0x000ffffe,
 	.name		= "Micrel KSZ9021 Gigabit PHY",
@@ -185,51 +178,18 @@ static struct phy_driver ksz9021_driver = {
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= ksz9021_config_intr,
 	.driver		= { .owner = THIS_MODULE, },
-};
+} };
 
 static int __init ksphy_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&ks8001_driver);
-	if (ret)
-		goto err1;
-
-	ret = phy_driver_register(&ksz9021_driver);
-	if (ret)
-		goto err2;
-
-	ret = phy_driver_register(&ks8737_driver);
-	if (ret)
-		goto err3;
-	ret = phy_driver_register(&ks8041_driver);
-	if (ret)
-		goto err4;
-	ret = phy_driver_register(&ks8051_driver);
-	if (ret)
-		goto err5;
-
-	return 0;
-
-err5:
-	phy_driver_unregister(&ks8041_driver);
-err4:
-	phy_driver_unregister(&ks8737_driver);
-err3:
-	phy_driver_unregister(&ksz9021_driver);
-err2:
-	phy_driver_unregister(&ks8001_driver);
-err1:
-	return ret;
+	return phy_drivers_register(ksphy_driver,
+		ARRAY_SIZE(ksphy_driver));
 }
 
 static void __exit ksphy_exit(void)
 {
-	phy_driver_unregister(&ks8001_driver);
-	phy_driver_unregister(&ks8737_driver);
-	phy_driver_unregister(&ksz9021_driver);
-	phy_driver_unregister(&ks8041_driver);
-	phy_driver_unregister(&ks8051_driver);
+	phy_drivers_unregister(ksphy_driver,
+		ARRAY_SIZE(ksphy_driver));
 }
 
 module_init(ksphy_init);
diff --git a/drivers/net/phy/national.c b/drivers/net/phy/national.c
index 04bb8fcc0cb..9a5f234d95b 100644
--- a/drivers/net/phy/national.c
+++ b/drivers/net/phy/national.c
@@ -15,6 +15,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/mii.h>
@@ -22,6 +24,8 @@
 #include <linux/phy.h>
 #include <linux/netdevice.h>
 
+#define DEBUG
+
 /* DP83865 phy identifier values */
 #define DP83865_PHY_ID	0x20005c7a
 
@@ -112,8 +116,8 @@ static void ns_10_base_t_hdx_loopack(struct phy_device *phydev, int disable)
 		ns_exp_write(phydev, 0x1c0,
 			     ns_exp_read(phydev, 0x1c0) & 0xfffe);
 
-	printk(KERN_DEBUG "DP83865 PHY: 10BASE-T HDX loopback %s\n",
-	       (ns_exp_read(phydev, 0x1c0) & 0x0001) ? "off" : "on");
+	pr_debug("10BASE-T HDX loopback %s\n",
+		 (ns_exp_read(phydev, 0x1c0) & 0x0001) ? "off" : "on");
 }
 
 static int ns_config_init(struct phy_device *phydev)
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 3cbda0851f8..7ca2ff97c36 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -15,6 +15,9 @@
  * option) any later version.
  *
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/string.h>
 #include <linux/errno.h>
@@ -32,6 +35,7 @@
 #include <linux/phy.h>
 #include <linux/timer.h>
 #include <linux/workqueue.h>
+#include <linux/mdio.h>
 
 #include <linux/atomic.h>
 #include <asm/io.h>
@@ -44,18 +48,16 @@
  */
 void phy_print_status(struct phy_device *phydev)
 {
-	pr_info("PHY: %s - Link is %s", dev_name(&phydev->dev),
-			phydev->link ? "Up" : "Down");
 	if (phydev->link)
-		printk(KERN_CONT " - %d/%s", phydev->speed,
-				DUPLEX_FULL == phydev->duplex ?
-				"Full" : "Half");
-
-	printk(KERN_CONT "\n");
+		pr_info("%s - Link is Up - %d/%s\n",
+			dev_name(&phydev->dev),
+			phydev->speed,
+			DUPLEX_FULL == phydev->duplex ? "Full" : "Half");
+	else
+		pr_info("%s - Link is Down\n", dev_name(&phydev->dev));
 }
 EXPORT_SYMBOL(phy_print_status);
 
-
 /**
  * phy_clear_interrupt - Ack the phy device's interrupt
  * @phydev: the phy_device struct
@@ -482,9 +484,8 @@ static void phy_force_reduction(struct phy_device *phydev)
 	phydev->speed = settings[idx].speed;
 	phydev->duplex = settings[idx].duplex;
 
-	pr_info("Trying %d/%s\n", phydev->speed,
-			DUPLEX_FULL == phydev->duplex ?
-			"FULL" : "HALF");
+	pr_info("Trying %d/%s\n",
+		phydev->speed, DUPLEX_FULL == phydev->duplex ? "FULL" : "HALF");
 }
 
 
@@ -598,9 +599,8 @@ int phy_start_interrupts(struct phy_device *phydev)
 				IRQF_SHARED,
 				"phy_interrupt",
 				phydev) < 0) {
-		printk(KERN_WARNING "%s: Can't get IRQ %d (PHY)\n",
-				phydev->bus->name,
-				phydev->irq);
+		pr_warn("%s: Can't get IRQ %d (PHY)\n",
+			phydev->bus->name, phydev->irq);
 		phydev->irq = PHY_POLL;
 		return 0;
 	}
@@ -838,10 +838,10 @@ void phy_state_machine(struct work_struct *work)
 
 				phydev->autoneg = AUTONEG_DISABLE;
 
-				pr_info("Trying %d/%s\n", phydev->speed,
-						DUPLEX_FULL ==
-						phydev->duplex ?
-						"FULL" : "HALF");
+				pr_info("Trying %d/%s\n",
+					phydev->speed,
+					DUPLEX_FULL == phydev->duplex ?
+					"FULL" : "HALF");
 			}
 			break;
 		case PHY_NOLINK:
@@ -968,3 +968,283 @@ void phy_state_machine(struct work_struct *work)
 
 	schedule_delayed_work(&phydev->state_queue, PHY_STATE_TIME * HZ);
 }
+
+static inline void mmd_phy_indirect(struct mii_bus *bus, int prtad, int devad,
+				    int addr)
+{
+	/* Write the desired MMD Devad */
+	bus->write(bus, addr, MII_MMD_CTRL, devad);
+
+	/* Write the desired MMD register address */
+	bus->write(bus, addr, MII_MMD_DATA, prtad);
+
+	/* Select the Function : DATA with no post increment */
+	bus->write(bus, addr, MII_MMD_CTRL, (devad | MII_MMD_CTRL_NOINCR));
+}
+
+/**
+ * phy_read_mmd_indirect - reads data from the MMD registers
+ * @bus: the target MII bus
+ * @prtad: MMD Address
+ * @devad: MMD DEVAD
+ * @addr: PHY address on the MII bus
+ *
+ * Description: it reads data from the MMD registers (clause 22 to access to
+ * clause 45) of the specified phy address.
+ * To read these register we have:
+ * 1) Write reg 13 // DEVAD
+ * 2) Write reg 14 // MMD Address
+ * 3) Write reg 13 // MMD Data Command for MMD DEVAD
+ * 3) Read  reg 14 // Read MMD data
+ */
+static int phy_read_mmd_indirect(struct mii_bus *bus, int prtad, int devad,
+				 int addr)
+{
+	u32 ret;
+
+	mmd_phy_indirect(bus, prtad, devad, addr);
+
+	/* Read the content of the MMD's selected register */
+	ret = bus->read(bus, addr, MII_MMD_DATA);
+
+	return ret;
+}
+
+/**
+ * phy_write_mmd_indirect - writes data to the MMD registers
+ * @bus: the target MII bus
+ * @prtad: MMD Address
+ * @devad: MMD DEVAD
+ * @addr: PHY address on the MII bus
+ * @data: data to write in the MMD register
+ *
+ * Description: Write data from the MMD registers of the specified
+ * phy address.
+ * To write these register we have:
+ * 1) Write reg 13 // DEVAD
+ * 2) Write reg 14 // MMD Address
+ * 3) Write reg 13 // MMD Data Command for MMD DEVAD
+ * 3) Write reg 14 // Write MMD data
+ */
+static void phy_write_mmd_indirect(struct mii_bus *bus, int prtad, int devad,
+				   int addr, u32 data)
+{
+	mmd_phy_indirect(bus, prtad, devad, addr);
+
+	/* Write the data into MMD's selected register */
+	bus->write(bus, addr, MII_MMD_DATA, data);
+}
+
+static u32 phy_eee_to_adv(u16 eee_adv)
+{
+	u32 adv = 0;
+
+	if (eee_adv & MDIO_EEE_100TX)
+		adv |= ADVERTISED_100baseT_Full;
+	if (eee_adv & MDIO_EEE_1000T)
+		adv |= ADVERTISED_1000baseT_Full;
+	if (eee_adv & MDIO_EEE_10GT)
+		adv |= ADVERTISED_10000baseT_Full;
+	if (eee_adv & MDIO_EEE_1000KX)
+		adv |= ADVERTISED_1000baseKX_Full;
+	if (eee_adv & MDIO_EEE_10GKX4)
+		adv |= ADVERTISED_10000baseKX4_Full;
+	if (eee_adv & MDIO_EEE_10GKR)
+		adv |= ADVERTISED_10000baseKR_Full;
+
+	return adv;
+}
+
+static u32 phy_eee_to_supported(u16 eee_caported)
+{
+	u32 supported = 0;
+
+	if (eee_caported & MDIO_EEE_100TX)
+		supported |= SUPPORTED_100baseT_Full;
+	if (eee_caported & MDIO_EEE_1000T)
+		supported |= SUPPORTED_1000baseT_Full;
+	if (eee_caported & MDIO_EEE_10GT)
+		supported |= SUPPORTED_10000baseT_Full;
+	if (eee_caported & MDIO_EEE_1000KX)
+		supported |= SUPPORTED_1000baseKX_Full;
+	if (eee_caported & MDIO_EEE_10GKX4)
+		supported |= SUPPORTED_10000baseKX4_Full;
+	if (eee_caported & MDIO_EEE_10GKR)
+		supported |= SUPPORTED_10000baseKR_Full;
+
+	return supported;
+}
+
+static u16 phy_adv_to_eee(u32 adv)
+{
+	u16 reg = 0;
+
+	if (adv & ADVERTISED_100baseT_Full)
+		reg |= MDIO_EEE_100TX;
+	if (adv & ADVERTISED_1000baseT_Full)
+		reg |= MDIO_EEE_1000T;
+	if (adv & ADVERTISED_10000baseT_Full)
+		reg |= MDIO_EEE_10GT;
+	if (adv & ADVERTISED_1000baseKX_Full)
+		reg |= MDIO_EEE_1000KX;
+	if (adv & ADVERTISED_10000baseKX4_Full)
+		reg |= MDIO_EEE_10GKX4;
+	if (adv & ADVERTISED_10000baseKR_Full)
+		reg |= MDIO_EEE_10GKR;
+
+	return reg;
+}
+
+/**
+ * phy_init_eee - init and check the EEE feature
+ * @phydev: target phy_device struct
+ * @clk_stop_enable: PHY may stop the clock during LPI
+ *
+ * Description: it checks if the Energy-Efficient Ethernet (EEE)
+ * is supported by looking at the MMD registers 3.20 and 7.60/61
+ * and it programs the MMD register 3.0 setting the "Clock stop enable"
+ * bit if required.
+ */
+int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
+{
+	int ret = -EPROTONOSUPPORT;
+
+	/* According to 802.3az,the EEE is supported only in full duplex-mode.
+	 * Also EEE feature is active when core is operating with MII, GMII
+	 * or RGMII.
+	 */
+	if ((phydev->duplex == DUPLEX_FULL) &&
+	    ((phydev->interface == PHY_INTERFACE_MODE_MII) ||
+	    (phydev->interface == PHY_INTERFACE_MODE_GMII) ||
+	    (phydev->interface == PHY_INTERFACE_MODE_RGMII))) {
+		int eee_lp, eee_cap, eee_adv;
+		u32 lp, cap, adv;
+		int idx, status;
+
+		/* Read phy status to properly get the right settings */
+		status = phy_read_status(phydev);
+		if (status)
+			return status;
+
+		/* First check if the EEE ability is supported */
+		eee_cap = phy_read_mmd_indirect(phydev->bus, MDIO_PCS_EEE_ABLE,
+						MDIO_MMD_PCS, phydev->addr);
+		if (eee_cap < 0)
+			return eee_cap;
+
+		cap = phy_eee_to_supported(eee_cap);
+		if (!cap)
+			goto eee_exit;
+
+		/* Check which link settings negotiated and verify it in
+		 * the EEE advertising registers.
+		 */
+		eee_lp = phy_read_mmd_indirect(phydev->bus, MDIO_AN_EEE_LPABLE,
+					       MDIO_MMD_AN, phydev->addr);
+		if (eee_lp < 0)
+			return eee_lp;
+
+		eee_adv = phy_read_mmd_indirect(phydev->bus, MDIO_AN_EEE_ADV,
+						MDIO_MMD_AN, phydev->addr);
+		if (eee_adv < 0)
+			return eee_adv;
+
+		adv = phy_eee_to_adv(eee_adv);
+		lp = phy_eee_to_adv(eee_lp);
+		idx = phy_find_setting(phydev->speed, phydev->duplex);
+		if ((lp & adv & settings[idx].setting))
+			goto eee_exit;
+
+		if (clk_stop_enable) {
+			/* Configure the PHY to stop receiving xMII
+			 * clock while it is signaling LPI.
+			 */
+			int val = phy_read_mmd_indirect(phydev->bus, MDIO_CTRL1,
+							MDIO_MMD_PCS,
+							phydev->addr);
+			if (val < 0)
+				return val;
+
+			val |= MDIO_PCS_CTRL1_CLKSTOP_EN;
+			phy_write_mmd_indirect(phydev->bus, MDIO_CTRL1,
+					       MDIO_MMD_PCS, phydev->addr, val);
+		}
+
+		ret = 0; /* EEE supported */
+	}
+
+eee_exit:
+	return ret;
+}
+EXPORT_SYMBOL(phy_init_eee);
+
+/**
+ * phy_get_eee_err - report the EEE wake error count
+ * @phydev: target phy_device struct
+ *
+ * Description: it is to report the number of time where the PHY
+ * failed to complete its normal wake sequence.
+ */
+int phy_get_eee_err(struct phy_device *phydev)
+{
+	return phy_read_mmd_indirect(phydev->bus, MDIO_PCS_EEE_WK_ERR,
+				     MDIO_MMD_PCS, phydev->addr);
+
+}
+EXPORT_SYMBOL(phy_get_eee_err);
+
+/**
+ * phy_ethtool_get_eee - get EEE supported and status
+ * @phydev: target phy_device struct
+ * @data: ethtool_eee data
+ *
+ * Description: it reportes the Supported/Advertisement/LP Advertisement
+ * capabilities.
+ */
+int phy_ethtool_get_eee(struct phy_device *phydev, struct ethtool_eee *data)
+{
+	int val;
+
+	/* Get Supported EEE */
+	val = phy_read_mmd_indirect(phydev->bus, MDIO_PCS_EEE_ABLE,
+				    MDIO_MMD_PCS, phydev->addr);
+	if (val < 0)
+		return val;
+	data->supported = phy_eee_to_supported(val);
+
+	/* Get advertisement EEE */
+	val = phy_read_mmd_indirect(phydev->bus, MDIO_AN_EEE_ADV,
+				    MDIO_MMD_AN, phydev->addr);
+	if (val < 0)
+		return val;
+	data->advertised = phy_eee_to_adv(val);
+
+	/* Get LP advertisement EEE */
+	val = phy_read_mmd_indirect(phydev->bus, MDIO_AN_EEE_LPABLE,
+				    MDIO_MMD_AN, phydev->addr);
+	if (val < 0)
+		return val;
+	data->lp_advertised = phy_eee_to_adv(val);
+
+	return 0;
+}
+EXPORT_SYMBOL(phy_ethtool_get_eee);
+
+/**
+ * phy_ethtool_set_eee - set EEE supported and status
+ * @phydev: target phy_device struct
+ * @data: ethtool_eee data
+ *
+ * Description: it is to program the Advertisement EEE register.
+ */
+int phy_ethtool_set_eee(struct phy_device *phydev, struct ethtool_eee *data)
+{
+	int val;
+
+	val = phy_adv_to_eee(data->advertised);
+	phy_write_mmd_indirect(phydev->bus, MDIO_AN_EEE_ADV, MDIO_MMD_AN,
+			       phydev->addr, val);
+
+	return 0;
+}
+EXPORT_SYMBOL(phy_ethtool_set_eee);
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index de86a558222..8af46e88a18 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -14,6 +14,9 @@
  * option) any later version.
  *
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/string.h>
 #include <linux/errno.h>
@@ -149,8 +152,8 @@ int phy_scan_fixups(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(phy_scan_fixups);
 
-static struct phy_device* phy_device_create(struct mii_bus *bus,
-					    int addr, int phy_id)
+struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id,
+			bool is_c45, struct phy_c45_device_ids *c45_ids)
 {
 	struct phy_device *dev;
 
@@ -171,8 +174,11 @@ static struct phy_device* phy_device_create(struct mii_bus *bus,
 
 	dev->autoneg = AUTONEG_ENABLE;
 
+	dev->is_c45 = is_c45;
 	dev->addr = addr;
 	dev->phy_id = phy_id;
+	if (c45_ids)
+		dev->c45_ids = *c45_ids;
 	dev->bus = bus;
 	dev->dev.parent = bus->parent;
 	dev->dev.bus = &mdio_bus_type;
@@ -197,20 +203,99 @@ static struct phy_device* phy_device_create(struct mii_bus *bus,
 
 	return dev;
 }
+EXPORT_SYMBOL(phy_device_create);
+
+/**
+ * get_phy_c45_ids - reads the specified addr for its 802.3-c45 IDs.
+ * @bus: the target MII bus
+ * @addr: PHY address on the MII bus
+ * @phy_id: where to store the ID retrieved.
+ * @c45_ids: where to store the c45 ID information.
+ *
+ *   If the PHY devices-in-package appears to be valid, it and the
+ *   corresponding identifiers are stored in @c45_ids, zero is stored
+ *   in @phy_id.  Otherwise 0xffffffff is stored in @phy_id.  Returns
+ *   zero on success.
+ *
+ */
+static int get_phy_c45_ids(struct mii_bus *bus, int addr, u32 *phy_id,
+			   struct phy_c45_device_ids *c45_ids) {
+	int phy_reg;
+	int i, reg_addr;
+	const int num_ids = ARRAY_SIZE(c45_ids->device_ids);
+
+	/* Find first non-zero Devices In package.  Device
+	 * zero is reserved, so don't probe it.
+	 */
+	for (i = 1;
+	     i < num_ids && c45_ids->devices_in_package == 0;
+	     i++) {
+		reg_addr = MII_ADDR_C45 | i << 16 | 6;
+		phy_reg = mdiobus_read(bus, addr, reg_addr);
+		if (phy_reg < 0)
+			return -EIO;
+		c45_ids->devices_in_package = (phy_reg & 0xffff) << 16;
+
+		reg_addr = MII_ADDR_C45 | i << 16 | 5;
+		phy_reg = mdiobus_read(bus, addr, reg_addr);
+		if (phy_reg < 0)
+			return -EIO;
+		c45_ids->devices_in_package |= (phy_reg & 0xffff);
+
+		/* If mostly Fs, there is no device there,
+		 * let's get out of here.
+		 */
+		if ((c45_ids->devices_in_package & 0x1fffffff) == 0x1fffffff) {
+			*phy_id = 0xffffffff;
+			return 0;
+		}
+	}
+
+	/* Now probe Device Identifiers for each device present. */
+	for (i = 1; i < num_ids; i++) {
+		if (!(c45_ids->devices_in_package & (1 << i)))
+			continue;
+
+		reg_addr = MII_ADDR_C45 | i << 16 | MII_PHYSID1;
+		phy_reg = mdiobus_read(bus, addr, reg_addr);
+		if (phy_reg < 0)
+			return -EIO;
+		c45_ids->device_ids[i] = (phy_reg & 0xffff) << 16;
+
+		reg_addr = MII_ADDR_C45 | i << 16 | MII_PHYSID2;
+		phy_reg = mdiobus_read(bus, addr, reg_addr);
+		if (phy_reg < 0)
+			return -EIO;
+		c45_ids->device_ids[i] |= (phy_reg & 0xffff);
+	}
+	*phy_id = 0;
+	return 0;
+}
 
 /**
  * get_phy_id - reads the specified addr for its ID.
  * @bus: the target MII bus
  * @addr: PHY address on the MII bus
  * @phy_id: where to store the ID retrieved.
+ * @is_c45: If true the PHY uses the 802.3 clause 45 protocol
+ * @c45_ids: where to store the c45 ID information.
+ *
+ * Description: In the case of a 802.3-c22 PHY, reads the ID registers
+ *   of the PHY at @addr on the @bus, stores it in @phy_id and returns
+ *   zero on success.
+ *
+ *   In the case of a 802.3-c45 PHY, get_phy_c45_ids() is invoked, and
+ *   its return value is in turn returned.
  *
- * Description: Reads the ID registers of the PHY at @addr on the
- *   @bus, stores it in @phy_id and returns zero on success.
  */
-static int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
+static int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id,
+		      bool is_c45, struct phy_c45_device_ids *c45_ids)
 {
 	int phy_reg;
 
+	if (is_c45)
+		return get_phy_c45_ids(bus, addr, phy_id, c45_ids);
+
 	/* Grab the bits from PHYIR1, and put them
 	 * in the upper half */
 	phy_reg = mdiobus_read(bus, addr, MII_PHYSID1);
@@ -235,17 +320,19 @@ static int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
  * get_phy_device - reads the specified PHY device and returns its @phy_device struct
  * @bus: the target MII bus
  * @addr: PHY address on the MII bus
+ * @is_c45: If true the PHY uses the 802.3 clause 45 protocol
  *
  * Description: Reads the ID registers of the PHY at @addr on the
  *   @bus, then allocates and returns the phy_device to represent it.
  */
-struct phy_device * get_phy_device(struct mii_bus *bus, int addr)
+struct phy_device *get_phy_device(struct mii_bus *bus, int addr, bool is_c45)
 {
+	struct phy_c45_device_ids c45_ids = {0};
 	struct phy_device *dev = NULL;
-	u32 phy_id;
+	u32 phy_id = 0;
 	int r;
 
-	r = get_phy_id(bus, addr, &phy_id);
+	r = get_phy_id(bus, addr, &phy_id, is_c45, &c45_ids);
 	if (r)
 		return ERR_PTR(r);
 
@@ -253,7 +340,7 @@ struct phy_device * get_phy_device(struct mii_bus *bus, int addr)
 	if ((phy_id & 0x1fffffff) == 0x1fffffff)
 		return NULL;
 
-	dev = phy_device_create(bus, addr, phy_id);
+	dev = phy_device_create(bus, addr, phy_id, is_c45, &c45_ids);
 
 	return dev;
 }
@@ -446,6 +533,11 @@ static int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	/* Assume that if there is no driver, that it doesn't
 	 * exist, and we should use the genphy driver. */
 	if (NULL == d->driver) {
+		if (phydev->is_c45) {
+			pr_err("No driver for phy %x\n", phydev->phy_id);
+			return -ENODEV;
+		}
+
 		d->driver = &genphy_driver.driver;
 
 		err = d->driver->probe(d);
@@ -975,8 +1067,8 @@ int phy_driver_register(struct phy_driver *new_driver)
 	retval = driver_register(&new_driver->driver);
 
 	if (retval) {
-		printk(KERN_ERR "%s: Error %d in registering driver\n",
-				new_driver->name, retval);
+		pr_err("%s: Error %d in registering driver\n",
+		       new_driver->name, retval);
 
 		return retval;
 	}
@@ -987,12 +1079,37 @@ int phy_driver_register(struct phy_driver *new_driver)
 }
 EXPORT_SYMBOL(phy_driver_register);
 
+int phy_drivers_register(struct phy_driver *new_driver, int n)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < n; i++) {
+		ret = phy_driver_register(new_driver + i);
+		if (ret) {
+			while (i-- > 0)
+				phy_driver_unregister(new_driver + i);
+			break;
+		}
+	}
+	return ret;
+}
+EXPORT_SYMBOL(phy_drivers_register);
+
 void phy_driver_unregister(struct phy_driver *drv)
 {
 	driver_unregister(&drv->driver);
 }
 EXPORT_SYMBOL(phy_driver_unregister);
 
+void phy_drivers_unregister(struct phy_driver *drv, int n)
+{
+	int i;
+	for (i = 0; i < n; i++) {
+		phy_driver_unregister(drv + i);
+	}
+}
+EXPORT_SYMBOL(phy_drivers_unregister);
+
 static struct phy_driver genphy_driver = {
 	.phy_id		= 0xffffffff,
 	.phy_id_mask	= 0xffffffff,
diff --git a/drivers/net/phy/realtek.c b/drivers/net/phy/realtek.c
index f414ffb5b72..72f93470ea3 100644
--- a/drivers/net/phy/realtek.c
+++ b/drivers/net/phy/realtek.c
@@ -65,11 +65,7 @@ static struct phy_driver rtl821x_driver = {
 
 static int __init realtek_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register(&rtl821x_driver);
-
-	return ret;
+	return phy_driver_register(&rtl821x_driver);
 }
 
 static void __exit realtek_exit(void)
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index fc3e7e96c88..6d6192316b3 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -12,7 +12,7 @@
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
  *
- * Support added for SMSC LAN8187 and LAN8700 by steve.glendinning@smsc.com
+ * Support added for SMSC LAN8187 and LAN8700 by steve.glendinning@shawell.net
  *
  */
 
@@ -61,7 +61,8 @@ static int lan911x_config_init(struct phy_device *phydev)
 	return smsc_phy_ack_interrupt(phydev);
 }
 
-static struct phy_driver lan83c185_driver = {
+static struct phy_driver smsc_phy_driver[] = {
+{
 	.phy_id		= 0x0007c0a0, /* OUI=0x00800f, Model#=0x0a */
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "SMSC LAN83C185",
@@ -83,9 +84,7 @@ static struct phy_driver lan83c185_driver = {
 	.resume		= genphy_resume,
 
 	.driver		= { .owner = THIS_MODULE, }
-};
-
-static struct phy_driver lan8187_driver = {
+}, {
 	.phy_id		= 0x0007c0b0, /* OUI=0x00800f, Model#=0x0b */
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "SMSC LAN8187",
@@ -107,9 +106,7 @@ static struct phy_driver lan8187_driver = {
 	.resume		= genphy_resume,
 
 	.driver		= { .owner = THIS_MODULE, }
-};
-
-static struct phy_driver lan8700_driver = {
+}, {
 	.phy_id		= 0x0007c0c0, /* OUI=0x00800f, Model#=0x0c */
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "SMSC LAN8700",
@@ -131,9 +128,7 @@ static struct phy_driver lan8700_driver = {
 	.resume		= genphy_resume,
 
 	.driver		= { .owner = THIS_MODULE, }
-};
-
-static struct phy_driver lan911x_int_driver = {
+}, {
 	.phy_id		= 0x0007c0d0, /* OUI=0x00800f, Model#=0x0d */
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "SMSC LAN911x Internal PHY",
@@ -155,9 +150,7 @@ static struct phy_driver lan911x_int_driver = {
 	.resume		= genphy_resume,
 
 	.driver		= { .owner = THIS_MODULE, }
-};
-
-static struct phy_driver lan8710_driver = {
+}, {
 	.phy_id		= 0x0007c0f0, /* OUI=0x00800f, Model#=0x0f */
 	.phy_id_mask	= 0xfffffff0,
 	.name		= "SMSC LAN8710/LAN8720",
@@ -179,53 +172,18 @@ static struct phy_driver lan8710_driver = {
 	.resume		= genphy_resume,
 
 	.driver		= { .owner = THIS_MODULE, }
-};
+} };
 
 static int __init smsc_init(void)
 {
-	int ret;
-
-	ret = phy_driver_register (&lan83c185_driver);
-	if (ret)
-		goto err1;
-
-	ret = phy_driver_register (&lan8187_driver);
-	if (ret)
-		goto err2;
-
-	ret = phy_driver_register (&lan8700_driver);
-	if (ret)
-		goto err3;
-
-	ret = phy_driver_register (&lan911x_int_driver);
-	if (ret)
-		goto err4;
-
-	ret = phy_driver_register (&lan8710_driver);
-	if (ret)
-		goto err5;
-
-	return 0;
-
-err5:
-	phy_driver_unregister (&lan911x_int_driver);
-err4:
-	phy_driver_unregister (&lan8700_driver);
-err3:
-	phy_driver_unregister (&lan8187_driver);
-err2:
-	phy_driver_unregister (&lan83c185_driver);
-err1:
-	return ret;
+	return phy_drivers_register(smsc_phy_driver,
+		ARRAY_SIZE(smsc_phy_driver));
 }
 
 static void __exit smsc_exit(void)
 {
-	phy_driver_unregister (&lan8710_driver);
-	phy_driver_unregister (&lan911x_int_driver);
-	phy_driver_unregister (&lan8700_driver);
-	phy_driver_unregister (&lan8187_driver);
-	phy_driver_unregister (&lan83c185_driver);
+	return phy_drivers_unregister(smsc_phy_driver,
+		ARRAY_SIZE(smsc_phy_driver));
 }
 
 MODULE_DESCRIPTION("SMSC PHY driver");
diff --git a/drivers/net/phy/spi_ks8995.c b/drivers/net/phy/spi_ks8995.c
index 4eb98bc52a0..1c3abce78b6 100644
--- a/drivers/net/phy/spi_ks8995.c
+++ b/drivers/net/phy/spi_ks8995.c
@@ -11,6 +11,8 @@
  * by the Free Software Foundation.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -356,7 +358,7 @@ static struct spi_driver ks8995_driver = {
 
 static int __init ks8995_init(void)
 {
-	printk(KERN_INFO DRV_DESC " version " DRV_VERSION"\n");
+	pr_info(DRV_DESC " version " DRV_VERSION "\n");
 
 	return spi_register_driver(&ks8995_driver);
 }
diff --git a/drivers/net/phy/ste10Xp.c b/drivers/net/phy/ste10Xp.c
index 187a2fa814f..5e1eb138916 100644
--- a/drivers/net/phy/ste10Xp.c
+++ b/drivers/net/phy/ste10Xp.c
@@ -81,7 +81,8 @@ static int ste10Xp_ack_interrupt(struct phy_device *phydev)
 	return 0;
 }
 
-static struct phy_driver ste101p_pdriver = {
+static struct phy_driver ste10xp_pdriver[] = {
+{
 	.phy_id = STE101P_PHY_ID,
 	.phy_id_mask = 0xfffffff0,
 	.name = "STe101p",
@@ -95,9 +96,7 @@ static struct phy_driver ste101p_pdriver = {
 	.suspend = genphy_suspend,
 	.resume = genphy_resume,
 	.driver = {.owner = THIS_MODULE,}
-};
-
-static struct phy_driver ste100p_pdriver = {
+}, {
 	.phy_id = STE100P_PHY_ID,
 	.phy_id_mask = 0xffffffff,
 	.name = "STe100p",
@@ -111,22 +110,18 @@ static struct phy_driver ste100p_pdriver = {
 	.suspend = genphy_suspend,
 	.resume = genphy_resume,
 	.driver = {.owner = THIS_MODULE,}
-};
+} };
 
 static int __init ste10Xp_init(void)
 {
-	int retval;
-
-	retval = phy_driver_register(&ste100p_pdriver);
-	if (retval < 0)
-		return retval;
-	return phy_driver_register(&ste101p_pdriver);
+	return phy_drivers_register(ste10xp_pdriver,
+		ARRAY_SIZE(ste10xp_pdriver));
 }
 
 static void __exit ste10Xp_exit(void)
 {
-	phy_driver_unregister(&ste100p_pdriver);
-	phy_driver_unregister(&ste101p_pdriver);
+	phy_drivers_unregister(ste10xp_pdriver,
+		ARRAY_SIZE(ste10xp_pdriver));
 }
 
 module_init(ste10Xp_init);
diff --git a/drivers/net/phy/vitesse.c b/drivers/net/phy/vitesse.c
index 0ec8e09cc2a..2585c383e62 100644
--- a/drivers/net/phy/vitesse.c
+++ b/drivers/net/phy/vitesse.c
@@ -138,21 +138,6 @@ static int vsc82xx_config_intr(struct phy_device *phydev)
 	return err;
 }
 
-/* Vitesse 824x */
-static struct phy_driver vsc8244_driver = {
-	.phy_id		= PHY_ID_VSC8244,
-	.name		= "Vitesse VSC8244",
-	.phy_id_mask	= 0x000fffc0,
-	.features	= PHY_GBIT_FEATURES,
-	.flags		= PHY_HAS_INTERRUPT,
-	.config_init	= &vsc824x_config_init,
-	.config_aneg	= &genphy_config_aneg,
-	.read_status	= &genphy_read_status,
-	.ack_interrupt	= &vsc824x_ack_interrupt,
-	.config_intr	= &vsc82xx_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
-};
-
 static int vsc8221_config_init(struct phy_device *phydev)
 {
 	int err;
@@ -165,8 +150,22 @@ static int vsc8221_config_init(struct phy_device *phydev)
 	   Options are 802.3Z SerDes or SGMII */
 }
 
-/* Vitesse 8221 */
-static struct phy_driver vsc8221_driver = {
+/* Vitesse 824x */
+static struct phy_driver vsc82xx_driver[] = {
+{
+	.phy_id		= PHY_ID_VSC8244,
+	.name		= "Vitesse VSC8244",
+	.phy_id_mask	= 0x000fffc0,
+	.features	= PHY_GBIT_FEATURES,
+	.flags		= PHY_HAS_INTERRUPT,
+	.config_init	= &vsc824x_config_init,
+	.config_aneg	= &genphy_config_aneg,
+	.read_status	= &genphy_read_status,
+	.ack_interrupt	= &vsc824x_ack_interrupt,
+	.config_intr	= &vsc82xx_config_intr,
+	.driver		= { .owner = THIS_MODULE,},
+}, {
+	/* Vitesse 8221 */
 	.phy_id		= PHY_ID_VSC8221,
 	.phy_id_mask	= 0x000ffff0,
 	.name		= "Vitesse VSC8221",
@@ -177,26 +176,19 @@ static struct phy_driver vsc8221_driver = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &vsc824x_ack_interrupt,
 	.config_intr	= &vsc82xx_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
-};
+	.driver		= { .owner = THIS_MODULE,},
+} };
 
 static int __init vsc82xx_init(void)
 {
-	int err;
-
-	err = phy_driver_register(&vsc8244_driver);
-	if (err < 0)
-		return err;
-	err = phy_driver_register(&vsc8221_driver);
-	if (err < 0)
-		phy_driver_unregister(&vsc8244_driver);
-	return err;
+	return phy_drivers_register(vsc82xx_driver,
+		ARRAY_SIZE(vsc82xx_driver));
 }
 
 static void __exit vsc82xx_exit(void)
 {
-	phy_driver_unregister(&vsc8244_driver);
-	phy_driver_unregister(&vsc8221_driver);
+	return phy_drivers_unregister(vsc82xx_driver,
+		ARRAY_SIZE(vsc82xx_driver));
 }
 
 module_init(vsc82xx_init);
diff --git a/drivers/net/slip/slip.c b/drivers/net/slip/slip.c
index d4c9db3da22..a34d6bf5e43 100644
--- a/drivers/net/slip/slip.c
+++ b/drivers/net/slip/slip.c
@@ -390,10 +390,10 @@ static void sl_encaps(struct slip *sl, unsigned char *icp, int len)
 #endif
 #ifdef CONFIG_SLIP_MODE_SLIP6
 	if (sl->mode & SL_MODE_SLIP6)
-		count = slip_esc6(p, (unsigned char *) sl->xbuff, len);
+		count = slip_esc6(p, sl->xbuff, len);
 	else
 #endif
-		count = slip_esc(p, (unsigned char *) sl->xbuff, len);
+		count = slip_esc(p, sl->xbuff, len);
 
 	/* Order of next two lines is *very* important.
 	 * When we are sending a little amount of data,
diff --git a/drivers/net/team/Kconfig b/drivers/net/team/Kconfig
index 89024d5fc33..6a7260b03a1 100644
--- a/drivers/net/team/Kconfig
+++ b/drivers/net/team/Kconfig
@@ -15,6 +15,17 @@ menuconfig NET_TEAM
 
 if NET_TEAM
 
+config NET_TEAM_MODE_BROADCAST
+	tristate "Broadcast mode support"
+	depends on NET_TEAM
+	---help---
+	  Basic mode where packets are transmitted always by all suitable ports.
+
+	  All added ports are setup to have team's mac address.
+
+	  To compile this team mode as a module, choose M here: the module
+	  will be called team_mode_broadcast.
+
 config NET_TEAM_MODE_ROUNDROBIN
 	tristate "Round-robin mode support"
 	depends on NET_TEAM
@@ -22,7 +33,7 @@ config NET_TEAM_MODE_ROUNDROBIN
 	  Basic mode where port used for transmitting packets is selected in
 	  round-robin fashion using packet counter.
 
-	  All added ports are setup to have bond's mac address.
+	  All added ports are setup to have team's mac address.
 
 	  To compile this team mode as a module, choose M here: the module
 	  will be called team_mode_roundrobin.
diff --git a/drivers/net/team/Makefile b/drivers/net/team/Makefile
index fb9f4c1c51f..975763014e5 100644
--- a/drivers/net/team/Makefile
+++ b/drivers/net/team/Makefile
@@ -3,6 +3,7 @@
 #
 
 obj-$(CONFIG_NET_TEAM) += team.o
+obj-$(CONFIG_NET_TEAM_MODE_BROADCAST) += team_mode_broadcast.o
 obj-$(CONFIG_NET_TEAM_MODE_ROUNDROBIN) += team_mode_roundrobin.o
 obj-$(CONFIG_NET_TEAM_MODE_ACTIVEBACKUP) += team_mode_activebackup.o
 obj-$(CONFIG_NET_TEAM_MODE_LOADBALANCE) += team_mode_loadbalance.o
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index c61ae35a53c..87707ab3943 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -1,5 +1,5 @@
 /*
- * net/drivers/team/team.c - Network team device driver
+ * drivers/net/team/team.c - Network team device driver
  * Copyright (c) 2011 Jiri Pirko <jpirko@redhat.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,7 @@
 #include <linux/ctype.h>
 #include <linux/notifier.h>
 #include <linux/netdevice.h>
+#include <linux/netpoll.h>
 #include <linux/if_vlan.h>
 #include <linux/if_arp.h>
 #include <linux/socket.h>
@@ -26,6 +27,7 @@
 #include <net/rtnetlink.h>
 #include <net/genetlink.h>
 #include <net/netlink.h>
+#include <net/sch_generic.h>
 #include <linux/if_team.h>
 
 #define DRV_NAME "team"
@@ -82,14 +84,16 @@ static void team_refresh_port_linkup(struct team_port *port)
 						   port->state.linkup;
 }
 
+
 /*******************
  * Options handling
  *******************/
 
 struct team_option_inst { /* One for each option instance */
 	struct list_head list;
+	struct list_head tmp_list;
 	struct team_option *option;
-	struct team_port *port; /* != NULL if per-port */
+	struct team_option_inst_info info;
 	bool changed;
 	bool removed;
 };
@@ -106,22 +110,6 @@ static struct team_option *__team_find_option(struct team *team,
 	return NULL;
 }
 
-static int __team_option_inst_add(struct team *team, struct team_option *option,
-				  struct team_port *port)
-{
-	struct team_option_inst *opt_inst;
-
-	opt_inst = kmalloc(sizeof(*opt_inst), GFP_KERNEL);
-	if (!opt_inst)
-		return -ENOMEM;
-	opt_inst->option = option;
-	opt_inst->port = port;
-	opt_inst->changed = true;
-	opt_inst->removed = false;
-	list_add_tail(&opt_inst->list, &team->option_inst_list);
-	return 0;
-}
-
 static void __team_option_inst_del(struct team_option_inst *opt_inst)
 {
 	list_del(&opt_inst->list);
@@ -139,14 +127,49 @@ static void __team_option_inst_del_option(struct team *team,
 	}
 }
 
+static int __team_option_inst_add(struct team *team, struct team_option *option,
+				  struct team_port *port)
+{
+	struct team_option_inst *opt_inst;
+	unsigned int array_size;
+	unsigned int i;
+	int err;
+
+	array_size = option->array_size;
+	if (!array_size)
+		array_size = 1; /* No array but still need one instance */
+
+	for (i = 0; i < array_size; i++) {
+		opt_inst = kmalloc(sizeof(*opt_inst), GFP_KERNEL);
+		if (!opt_inst)
+			return -ENOMEM;
+		opt_inst->option = option;
+		opt_inst->info.port = port;
+		opt_inst->info.array_index = i;
+		opt_inst->changed = true;
+		opt_inst->removed = false;
+		list_add_tail(&opt_inst->list, &team->option_inst_list);
+		if (option->init) {
+			err = option->init(team, &opt_inst->info);
+			if (err)
+				return err;
+		}
+
+	}
+	return 0;
+}
+
 static int __team_option_inst_add_option(struct team *team,
 					 struct team_option *option)
 {
 	struct team_port *port;
 	int err;
 
-	if (!option->per_port)
-		return __team_option_inst_add(team, option, 0);
+	if (!option->per_port) {
+		err = __team_option_inst_add(team, option, NULL);
+		if (err)
+			goto inst_del_option;
+	}
 
 	list_for_each_entry(port, &team->port_list, list) {
 		err = __team_option_inst_add(team, option, port);
@@ -180,7 +203,7 @@ static void __team_option_inst_del_port(struct team *team,
 
 	list_for_each_entry_safe(opt_inst, tmp, &team->option_inst_list, list) {
 		if (opt_inst->option->per_port &&
-		    opt_inst->port == port)
+		    opt_inst->info.port == port)
 			__team_option_inst_del(opt_inst);
 	}
 }
@@ -211,7 +234,7 @@ static void __team_option_inst_mark_removed_port(struct team *team,
 	struct team_option_inst *opt_inst;
 
 	list_for_each_entry(opt_inst, &team->option_inst_list, list) {
-		if (opt_inst->port == port) {
+		if (opt_inst->info.port == port) {
 			opt_inst->changed = true;
 			opt_inst->removed = true;
 		}
@@ -324,28 +347,12 @@ void team_options_unregister(struct team *team,
 }
 EXPORT_SYMBOL(team_options_unregister);
 
-static int team_option_port_add(struct team *team, struct team_port *port)
-{
-	int err;
-
-	err = __team_option_inst_add_port(team, port);
-	if (err)
-		return err;
-	__team_options_change_check(team);
-	return 0;
-}
-
-static void team_option_port_del(struct team *team, struct team_port *port)
-{
-	__team_option_inst_mark_removed_port(team, port);
-	__team_options_change_check(team);
-	__team_option_inst_del_port(team, port);
-}
-
 static int team_option_get(struct team *team,
 			   struct team_option_inst *opt_inst,
 			   struct team_gsetter_ctx *ctx)
 {
+	if (!opt_inst->option->getter)
+		return -EOPNOTSUPP;
 	return opt_inst->option->getter(team, ctx);
 }
 
@@ -353,16 +360,26 @@ static int team_option_set(struct team *team,
 			   struct team_option_inst *opt_inst,
 			   struct team_gsetter_ctx *ctx)
 {
-	int err;
+	if (!opt_inst->option->setter)
+		return -EOPNOTSUPP;
+	return opt_inst->option->setter(team, ctx);
+}
 
-	err = opt_inst->option->setter(team, ctx);
-	if (err)
-		return err;
+void team_option_inst_set_change(struct team_option_inst_info *opt_inst_info)
+{
+	struct team_option_inst *opt_inst;
 
+	opt_inst = container_of(opt_inst_info, struct team_option_inst, info);
 	opt_inst->changed = true;
+}
+EXPORT_SYMBOL(team_option_inst_set_change);
+
+void team_options_change_check(struct team *team)
+{
 	__team_options_change_check(team);
-	return err;
 }
+EXPORT_SYMBOL(team_options_change_check);
+
 
 /****************
  * Mode handling
@@ -371,13 +388,18 @@ static int team_option_set(struct team *team,
 static LIST_HEAD(mode_list);
 static DEFINE_SPINLOCK(mode_list_lock);
 
-static struct team_mode *__find_mode(const char *kind)
+struct team_mode_item {
+	struct list_head list;
+	const struct team_mode *mode;
+};
+
+static struct team_mode_item *__find_mode(const char *kind)
 {
-	struct team_mode *mode;
+	struct team_mode_item *mitem;
 
-	list_for_each_entry(mode, &mode_list, list) {
-		if (strcmp(mode->kind, kind) == 0)
-			return mode;
+	list_for_each_entry(mitem, &mode_list, list) {
+		if (strcmp(mitem->mode->kind, kind) == 0)
+			return mitem;
 	}
 	return NULL;
 }
@@ -392,49 +414,65 @@ static bool is_good_mode_name(const char *name)
 	return true;
 }
 
-int team_mode_register(struct team_mode *mode)
+int team_mode_register(const struct team_mode *mode)
 {
 	int err = 0;
+	struct team_mode_item *mitem;
 
 	if (!is_good_mode_name(mode->kind) ||
 	    mode->priv_size > TEAM_MODE_PRIV_SIZE)
 		return -EINVAL;
+
+	mitem = kmalloc(sizeof(*mitem), GFP_KERNEL);
+	if (!mitem)
+		return -ENOMEM;
+
 	spin_lock(&mode_list_lock);
 	if (__find_mode(mode->kind)) {
 		err = -EEXIST;
+		kfree(mitem);
 		goto unlock;
 	}
-	list_add_tail(&mode->list, &mode_list);
+	mitem->mode = mode;
+	list_add_tail(&mitem->list, &mode_list);
 unlock:
 	spin_unlock(&mode_list_lock);
 	return err;
 }
 EXPORT_SYMBOL(team_mode_register);
 
-int team_mode_unregister(struct team_mode *mode)
+void team_mode_unregister(const struct team_mode *mode)
 {
+	struct team_mode_item *mitem;
+
 	spin_lock(&mode_list_lock);
-	list_del_init(&mode->list);
+	mitem = __find_mode(mode->kind);
+	if (mitem) {
+		list_del_init(&mitem->list);
+		kfree(mitem);
+	}
 	spin_unlock(&mode_list_lock);
-	return 0;
 }
 EXPORT_SYMBOL(team_mode_unregister);
 
-static struct team_mode *team_mode_get(const char *kind)
+static const struct team_mode *team_mode_get(const char *kind)
 {
-	struct team_mode *mode;
+	struct team_mode_item *mitem;
+	const struct team_mode *mode = NULL;
 
 	spin_lock(&mode_list_lock);
-	mode = __find_mode(kind);
-	if (!mode) {
+	mitem = __find_mode(kind);
+	if (!mitem) {
 		spin_unlock(&mode_list_lock);
 		request_module("team-mode-%s", kind);
 		spin_lock(&mode_list_lock);
-		mode = __find_mode(kind);
+		mitem = __find_mode(kind);
 	}
-	if (mode)
+	if (mitem) {
+		mode = mitem->mode;
 		if (!try_module_get(mode->owner))
 			mode = NULL;
+	}
 
 	spin_unlock(&mode_list_lock);
 	return mode;
@@ -458,26 +496,45 @@ rx_handler_result_t team_dummy_receive(struct team *team,
 	return RX_HANDLER_ANOTHER;
 }
 
-static void team_adjust_ops(struct team *team)
+static const struct team_mode __team_no_mode = {
+	.kind		= "*NOMODE*",
+};
+
+static bool team_is_mode_set(struct team *team)
+{
+	return team->mode != &__team_no_mode;
+}
+
+static void team_set_no_mode(struct team *team)
+{
+	team->mode = &__team_no_mode;
+}
+
+static void __team_adjust_ops(struct team *team, int en_port_count)
 {
 	/*
 	 * To avoid checks in rx/tx skb paths, ensure here that non-null and
 	 * correct ops are always set.
 	 */
 
-	if (list_empty(&team->port_list) ||
-	    !team->mode || !team->mode->ops->transmit)
+	if (!en_port_count || !team_is_mode_set(team) ||
+	    !team->mode->ops->transmit)
 		team->ops.transmit = team_dummy_transmit;
 	else
 		team->ops.transmit = team->mode->ops->transmit;
 
-	if (list_empty(&team->port_list) ||
-	    !team->mode || !team->mode->ops->receive)
+	if (!en_port_count || !team_is_mode_set(team) ||
+	    !team->mode->ops->receive)
 		team->ops.receive = team_dummy_receive;
 	else
 		team->ops.receive = team->mode->ops->receive;
 }
 
+static void team_adjust_ops(struct team *team)
+{
+	__team_adjust_ops(team, team->en_port_count);
+}
+
 /*
  * We can benefit from the fact that it's ensured no port is present
  * at the time of mode change. Therefore no packets are in fly so there's no
@@ -487,7 +544,7 @@ static int __team_change_mode(struct team *team,
 			      const struct team_mode *new_mode)
 {
 	/* Check if mode was previously set and do cleanup if so */
-	if (team->mode) {
+	if (team_is_mode_set(team)) {
 		void (*exit_op)(struct team *team) = team->ops.exit;
 
 		/* Clear ops area so no callback is called any longer */
@@ -497,7 +554,7 @@ static int __team_change_mode(struct team *team,
 		if (exit_op)
 			exit_op(team);
 		team_mode_put(team->mode);
-		team->mode = NULL;
+		team_set_no_mode(team);
 		/* zero private data area */
 		memset(&team->mode_priv, 0,
 		       sizeof(struct team) - offsetof(struct team, mode_priv));
@@ -523,7 +580,7 @@ static int __team_change_mode(struct team *team,
 
 static int team_change_mode(struct team *team, const char *kind)
 {
-	struct team_mode *new_mode;
+	const struct team_mode *new_mode;
 	struct net_device *dev = team->dev;
 	int err;
 
@@ -532,7 +589,7 @@ static int team_change_mode(struct team *team, const char *kind)
 		return -EBUSY;
 	}
 
-	if (team->mode && strcmp(team->mode->kind, kind) == 0) {
+	if (team_is_mode_set(team) && strcmp(team->mode->kind, kind) == 0) {
 		netdev_err(dev, "Unable to change to the same mode the team is in\n");
 		return -EINVAL;
 	}
@@ -559,8 +616,6 @@ static int team_change_mode(struct team *team, const char *kind)
  * Rx path frame handler
  ************************/
 
-static bool team_port_enabled(struct team_port *port);
-
 /* note: already called with rcu_read_lock */
 static rx_handler_result_t team_handle_frame(struct sk_buff **pskb)
 {
@@ -618,11 +673,6 @@ static bool team_port_find(const struct team *team,
 	return false;
 }
 
-static bool team_port_enabled(struct team_port *port)
-{
-	return port->index != -1;
-}
-
 /*
  * Enable/disable port by adding to enabled port hashlist and setting
  * port->index (Might be racy so reader could see incorrect ifindex when
@@ -637,6 +687,9 @@ static void team_port_enable(struct team *team,
 	port->index = team->en_port_count++;
 	hlist_add_head_rcu(&port->hlist,
 			   team_port_index_hash(team, port->index));
+	team_adjust_ops(team);
+	if (team->ops.port_enabled)
+		team->ops.port_enabled(team, port);
 }
 
 static void __reconstruct_port_hlist(struct team *team, int rm_index)
@@ -656,14 +709,20 @@ static void __reconstruct_port_hlist(struct team *team, int rm_index)
 static void team_port_disable(struct team *team,
 			      struct team_port *port)
 {
-	int rm_index = port->index;
-
 	if (!team_port_enabled(port))
 		return;
+	if (team->ops.port_disabled)
+		team->ops.port_disabled(team, port);
 	hlist_del_rcu(&port->hlist);
-	__reconstruct_port_hlist(team, rm_index);
-	team->en_port_count--;
+	__reconstruct_port_hlist(team, port->index);
 	port->index = -1;
+	__team_adjust_ops(team, team->en_port_count - 1);
+	/*
+	 * Wait until readers see adjusted ops. This ensures that
+	 * readers never see team->en_port_count == 0
+	 */
+	synchronize_rcu();
+	team->en_port_count--;
 }
 
 #define TEAM_VLAN_FEATURES (NETIF_F_ALL_CSUM | NETIF_F_SG | \
@@ -675,12 +734,14 @@ static void __team_compute_features(struct team *team)
 	struct team_port *port;
 	u32 vlan_features = TEAM_VLAN_FEATURES;
 	unsigned short max_hard_header_len = ETH_HLEN;
+	unsigned int flags, dst_release_flag = IFF_XMIT_DST_RELEASE;
 
 	list_for_each_entry(port, &team->port_list, list) {
 		vlan_features = netdev_increment_features(vlan_features,
 					port->dev->vlan_features,
 					TEAM_VLAN_FEATURES);
 
+		dst_release_flag &= port->dev->priv_flags;
 		if (port->dev->hard_header_len > max_hard_header_len)
 			max_hard_header_len = port->dev->hard_header_len;
 	}
@@ -688,6 +749,9 @@ static void __team_compute_features(struct team *team)
 	team->dev->vlan_features = vlan_features;
 	team->dev->hard_header_len = max_hard_header_len;
 
+	flags = team->dev->priv_flags & ~IFF_XMIT_DST_RELEASE;
+	team->dev->priv_flags = flags | dst_release_flag;
+
 	netdev_change_features(team->dev);
 }
 
@@ -730,6 +794,58 @@ static void team_port_leave(struct team *team, struct team_port *port)
 	dev_put(team->dev);
 }
 
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static int team_port_enable_netpoll(struct team *team, struct team_port *port)
+{
+	struct netpoll *np;
+	int err;
+
+	np = kzalloc(sizeof(*np), GFP_KERNEL);
+	if (!np)
+		return -ENOMEM;
+
+	err = __netpoll_setup(np, port->dev);
+	if (err) {
+		kfree(np);
+		return err;
+	}
+	port->np = np;
+	return err;
+}
+
+static void team_port_disable_netpoll(struct team_port *port)
+{
+	struct netpoll *np = port->np;
+
+	if (!np)
+		return;
+	port->np = NULL;
+
+	/* Wait for transmitting packets to finish before freeing. */
+	synchronize_rcu_bh();
+	__netpoll_cleanup(np);
+	kfree(np);
+}
+
+static struct netpoll_info *team_netpoll_info(struct team *team)
+{
+	return team->dev->npinfo;
+}
+
+#else
+static int team_port_enable_netpoll(struct team *team, struct team_port *port)
+{
+	return 0;
+}
+static void team_port_disable_netpoll(struct team_port *port)
+{
+}
+static struct netpoll_info *team_netpoll_info(struct team *team)
+{
+	return NULL;
+}
+#endif
+
 static void __team_port_change_check(struct team_port *port, bool linkup);
 
 static int team_port_add(struct team *team, struct net_device *port_dev)
@@ -758,7 +874,8 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		return -EBUSY;
 	}
 
-	port = kzalloc(sizeof(struct team_port), GFP_KERNEL);
+	port = kzalloc(sizeof(struct team_port) + team->mode->port_priv_size,
+		       GFP_KERNEL);
 	if (!port)
 		return -ENOMEM;
 
@@ -795,6 +912,15 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		goto err_vids_add;
 	}
 
+	if (team_netpoll_info(team)) {
+		err = team_port_enable_netpoll(team, port);
+		if (err) {
+			netdev_err(dev, "Failed to enable netpoll on device %s\n",
+				   portname);
+			goto err_enable_netpoll;
+		}
+	}
+
 	err = netdev_set_master(port_dev, dev);
 	if (err) {
 		netdev_err(dev, "Device %s failed to set master\n", portname);
@@ -809,7 +935,7 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		goto err_handler_register;
 	}
 
-	err = team_option_port_add(team, port);
+	err = __team_option_inst_add_port(team, port);
 	if (err) {
 		netdev_err(dev, "Device %s failed to add per-port options\n",
 			   portname);
@@ -819,9 +945,9 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 	port->index = -1;
 	team_port_enable(team, port);
 	list_add_tail_rcu(&port->list, &team->port_list);
-	team_adjust_ops(team);
 	__team_compute_features(team);
 	__team_port_change_check(port, !!netif_carrier_ok(port_dev));
+	__team_options_change_check(team);
 
 	netdev_info(dev, "Port device %s added\n", portname);
 
@@ -834,6 +960,9 @@ err_handler_register:
 	netdev_set_master(port_dev, NULL);
 
 err_set_master:
+	team_port_disable_netpoll(port);
+
+err_enable_netpoll:
 	vlan_vids_del_by_dev(port_dev, dev);
 
 err_vids_add:
@@ -865,14 +994,16 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
 		return -ENOENT;
 	}
 
+	__team_option_inst_mark_removed_port(team, port);
+	__team_options_change_check(team);
+	__team_option_inst_del_port(team, port);
 	port->removed = true;
 	__team_port_change_check(port, false);
 	team_port_disable(team, port);
 	list_del_rcu(&port->list);
-	team_adjust_ops(team);
-	team_option_port_del(team, port);
 	netdev_rx_handler_unregister(port_dev);
 	netdev_set_master(port_dev, NULL);
+	team_port_disable_netpoll(port);
 	vlan_vids_del_by_dev(port_dev, dev);
 	dev_close(port_dev);
 	team_port_leave(team, port);
@@ -891,11 +1022,9 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
  * Net device ops
  *****************/
 
-static const char team_no_mode_kind[] = "*NOMODE*";
-
 static int team_mode_option_get(struct team *team, struct team_gsetter_ctx *ctx)
 {
-	ctx->data.str_val = team->mode ? team->mode->kind : team_no_mode_kind;
+	ctx->data.str_val = team->mode->kind;
 	return 0;
 }
 
@@ -907,39 +1036,47 @@ static int team_mode_option_set(struct team *team, struct team_gsetter_ctx *ctx)
 static int team_port_en_option_get(struct team *team,
 				   struct team_gsetter_ctx *ctx)
 {
-	ctx->data.bool_val = team_port_enabled(ctx->port);
+	struct team_port *port = ctx->info->port;
+
+	ctx->data.bool_val = team_port_enabled(port);
 	return 0;
 }
 
 static int team_port_en_option_set(struct team *team,
 				   struct team_gsetter_ctx *ctx)
 {
+	struct team_port *port = ctx->info->port;
+
 	if (ctx->data.bool_val)
-		team_port_enable(team, ctx->port);
+		team_port_enable(team, port);
 	else
-		team_port_disable(team, ctx->port);
+		team_port_disable(team, port);
 	return 0;
 }
 
 static int team_user_linkup_option_get(struct team *team,
 				       struct team_gsetter_ctx *ctx)
 {
-	ctx->data.bool_val = ctx->port->user.linkup;
+	struct team_port *port = ctx->info->port;
+
+	ctx->data.bool_val = port->user.linkup;
 	return 0;
 }
 
 static int team_user_linkup_option_set(struct team *team,
 				       struct team_gsetter_ctx *ctx)
 {
-	ctx->port->user.linkup = ctx->data.bool_val;
-	team_refresh_port_linkup(ctx->port);
+	struct team_port *port = ctx->info->port;
+
+	port->user.linkup = ctx->data.bool_val;
+	team_refresh_port_linkup(port);
 	return 0;
 }
 
 static int team_user_linkup_en_option_get(struct team *team,
 					  struct team_gsetter_ctx *ctx)
 {
-	struct team_port *port = ctx->port;
+	struct team_port *port = ctx->info->port;
 
 	ctx->data.bool_val = port->user.linkup_enabled;
 	return 0;
@@ -948,10 +1085,10 @@ static int team_user_linkup_en_option_get(struct team *team,
 static int team_user_linkup_en_option_set(struct team *team,
 					  struct team_gsetter_ctx *ctx)
 {
-	struct team_port *port = ctx->port;
+	struct team_port *port = ctx->info->port;
 
 	port->user.linkup_enabled = ctx->data.bool_val;
-	team_refresh_port_linkup(ctx->port);
+	team_refresh_port_linkup(port);
 	return 0;
 }
 
@@ -985,6 +1122,22 @@ static const struct team_option team_options[] = {
 	},
 };
 
+static struct lock_class_key team_netdev_xmit_lock_key;
+static struct lock_class_key team_netdev_addr_lock_key;
+
+static void team_set_lockdep_class_one(struct net_device *dev,
+				       struct netdev_queue *txq,
+				       void *unused)
+{
+	lockdep_set_class(&txq->_xmit_lock, &team_netdev_xmit_lock_key);
+}
+
+static void team_set_lockdep_class(struct net_device *dev)
+{
+	lockdep_set_class(&dev->addr_list_lock, &team_netdev_addr_lock_key);
+	netdev_for_each_tx_queue(dev, team_set_lockdep_class_one, NULL);
+}
+
 static int team_init(struct net_device *dev)
 {
 	struct team *team = netdev_priv(dev);
@@ -993,6 +1146,7 @@ static int team_init(struct net_device *dev)
 
 	team->dev = dev;
 	mutex_init(&team->lock);
+	team_set_no_mode(team);
 
 	team->pcpu_stats = alloc_percpu(struct team_pcpu_stats);
 	if (!team->pcpu_stats)
@@ -1011,6 +1165,8 @@ static int team_init(struct net_device *dev)
 		goto err_options_register;
 	netif_carrier_off(dev);
 
+	team_set_lockdep_class(dev);
+
 	return 0;
 
 err_options_register:
@@ -1079,6 +1235,29 @@ static netdev_tx_t team_xmit(struct sk_buff *skb, struct net_device *dev)
 	return NETDEV_TX_OK;
 }
 
+static u16 team_select_queue(struct net_device *dev, struct sk_buff *skb)
+{
+	/*
+	 * This helper function exists to help dev_pick_tx get the correct
+	 * destination queue.  Using a helper function skips a call to
+	 * skb_tx_hash and will put the skbs in the queue we expect on their
+	 * way down to the team driver.
+	 */
+	u16 txq = skb_rx_queue_recorded(skb) ? skb_get_rx_queue(skb) : 0;
+
+	/*
+	 * Save the original txq to restore before passing to the driver
+	 */
+	qdisc_skb_cb(skb)->slave_dev_queue_mapping = skb->queue_mapping;
+
+	if (unlikely(txq >= dev->real_num_tx_queues)) {
+		do {
+			txq -= dev->real_num_tx_queues;
+		} while (txq >= dev->real_num_tx_queues);
+	}
+	return txq;
+}
+
 static void team_change_rx_flags(struct net_device *dev, int change)
 {
 	struct team *team = netdev_priv(dev);
@@ -1116,10 +1295,11 @@ static int team_set_mac_address(struct net_device *dev, void *p)
 {
 	struct team *team = netdev_priv(dev);
 	struct team_port *port;
-	struct sockaddr *addr = p;
+	int err;
 
-	dev->addr_assign_type &= ~NET_ADDR_RANDOM;
-	memcpy(dev->dev_addr, addr->sa_data, ETH_ALEN);
+	err = eth_mac_addr(dev, p);
+	if (err)
+		return err;
 	rcu_read_lock();
 	list_for_each_entry_rcu(port, &team->port_list, list)
 		if (team->ops.port_change_mac)
@@ -1240,6 +1420,48 @@ static int team_vlan_rx_kill_vid(struct net_device *dev, uint16_t vid)
 	return 0;
 }
 
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void team_poll_controller(struct net_device *dev)
+{
+}
+
+static void __team_netpoll_cleanup(struct team *team)
+{
+	struct team_port *port;
+
+	list_for_each_entry(port, &team->port_list, list)
+		team_port_disable_netpoll(port);
+}
+
+static void team_netpoll_cleanup(struct net_device *dev)
+{
+	struct team *team = netdev_priv(dev);
+
+	mutex_lock(&team->lock);
+	__team_netpoll_cleanup(team);
+	mutex_unlock(&team->lock);
+}
+
+static int team_netpoll_setup(struct net_device *dev,
+			      struct netpoll_info *npifo)
+{
+	struct team *team = netdev_priv(dev);
+	struct team_port *port;
+	int err = 0;
+
+	mutex_lock(&team->lock);
+	list_for_each_entry(port, &team->port_list, list) {
+		err = team_port_enable_netpoll(team, port);
+		if (err) {
+			__team_netpoll_cleanup(team);
+			break;
+		}
+	}
+	mutex_unlock(&team->lock);
+	return err;
+}
+#endif
+
 static int team_add_slave(struct net_device *dev, struct net_device *port_dev)
 {
 	struct team *team = netdev_priv(dev);
@@ -1289,6 +1511,7 @@ static const struct net_device_ops team_netdev_ops = {
 	.ndo_open		= team_open,
 	.ndo_stop		= team_close,
 	.ndo_start_xmit		= team_xmit,
+	.ndo_select_queue	= team_select_queue,
 	.ndo_change_rx_flags	= team_change_rx_flags,
 	.ndo_set_rx_mode	= team_set_rx_mode,
 	.ndo_set_mac_address	= team_set_mac_address,
@@ -1296,6 +1519,11 @@ static const struct net_device_ops team_netdev_ops = {
 	.ndo_get_stats64	= team_get_stats64,
 	.ndo_vlan_rx_add_vid	= team_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= team_vlan_rx_kill_vid,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= team_poll_controller,
+	.ndo_netpoll_setup	= team_netpoll_setup,
+	.ndo_netpoll_cleanup	= team_netpoll_cleanup,
+#endif
 	.ndo_add_slave		= team_add_slave,
 	.ndo_del_slave		= team_del_slave,
 	.ndo_fix_features	= team_fix_features,
@@ -1321,7 +1549,7 @@ static void team_setup(struct net_device *dev)
 	 * bring us to promisc mode in case a unicast addr is added.
 	 * Let this up to underlay drivers.
 	 */
-	dev->priv_flags |= IFF_UNICAST_FLT;
+	dev->priv_flags |= IFF_UNICAST_FLT | IFF_LIVE_ADDR_CHANGE;
 
 	dev->features |= NETIF_F_LLTX;
 	dev->features |= NETIF_F_GRO;
@@ -1358,12 +1586,24 @@ static int team_validate(struct nlattr *tb[], struct nlattr *data[])
 	return 0;
 }
 
+static unsigned int team_get_num_tx_queues(void)
+{
+	return TEAM_DEFAULT_NUM_TX_QUEUES;
+}
+
+static unsigned int team_get_num_rx_queues(void)
+{
+	return TEAM_DEFAULT_NUM_RX_QUEUES;
+}
+
 static struct rtnl_link_ops team_link_ops __read_mostly = {
-	.kind		= DRV_NAME,
-	.priv_size	= sizeof(struct team),
-	.setup		= team_setup,
-	.newlink	= team_newlink,
-	.validate	= team_validate,
+	.kind			= DRV_NAME,
+	.priv_size		= sizeof(struct team),
+	.setup			= team_setup,
+	.newlink		= team_newlink,
+	.validate		= team_validate,
+	.get_num_tx_queues	= team_get_num_tx_queues,
+	.get_num_rx_queues	= team_get_num_rx_queues,
 };
 
 
@@ -1404,7 +1644,7 @@ static int team_nl_cmd_noop(struct sk_buff *skb, struct genl_info *info)
 	void *hdr;
 	int err;
 
-	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 	if (!msg)
 		return -ENOMEM;
 
@@ -1466,7 +1706,7 @@ static int team_nl_send_generic(struct genl_info *info, struct team *team,
 	struct sk_buff *skb;
 	int err;
 
-	skb = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 	if (!skb)
 		return -ENOMEM;
 
@@ -1482,16 +1722,128 @@ err_fill:
 	return err;
 }
 
-static int team_nl_fill_options_get(struct sk_buff *skb,
-				    u32 pid, u32 seq, int flags,
-				    struct team *team, bool fillall)
+typedef int team_nl_send_func_t(struct sk_buff *skb,
+				struct team *team, u32 pid);
+
+static int team_nl_send_unicast(struct sk_buff *skb, struct team *team, u32 pid)
+{
+	return genlmsg_unicast(dev_net(team->dev), skb, pid);
+}
+
+static int team_nl_fill_one_option_get(struct sk_buff *skb, struct team *team,
+				       struct team_option_inst *opt_inst)
+{
+	struct nlattr *option_item;
+	struct team_option *option = opt_inst->option;
+	struct team_option_inst_info *opt_inst_info = &opt_inst->info;
+	struct team_gsetter_ctx ctx;
+	int err;
+
+	ctx.info = opt_inst_info;
+	err = team_option_get(team, opt_inst, &ctx);
+	if (err)
+		return err;
+
+	option_item = nla_nest_start(skb, TEAM_ATTR_ITEM_OPTION);
+	if (!option_item)
+		return -EMSGSIZE;
+
+	if (nla_put_string(skb, TEAM_ATTR_OPTION_NAME, option->name))
+		goto nest_cancel;
+	if (opt_inst_info->port &&
+	    nla_put_u32(skb, TEAM_ATTR_OPTION_PORT_IFINDEX,
+			opt_inst_info->port->dev->ifindex))
+		goto nest_cancel;
+	if (opt_inst->option->array_size &&
+	    nla_put_u32(skb, TEAM_ATTR_OPTION_ARRAY_INDEX,
+			opt_inst_info->array_index))
+		goto nest_cancel;
+
+	switch (option->type) {
+	case TEAM_OPTION_TYPE_U32:
+		if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_U32))
+			goto nest_cancel;
+		if (nla_put_u32(skb, TEAM_ATTR_OPTION_DATA, ctx.data.u32_val))
+			goto nest_cancel;
+		break;
+	case TEAM_OPTION_TYPE_STRING:
+		if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_STRING))
+			goto nest_cancel;
+		if (nla_put_string(skb, TEAM_ATTR_OPTION_DATA,
+				   ctx.data.str_val))
+			goto nest_cancel;
+		break;
+	case TEAM_OPTION_TYPE_BINARY:
+		if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_BINARY))
+			goto nest_cancel;
+		if (nla_put(skb, TEAM_ATTR_OPTION_DATA, ctx.data.bin_val.len,
+			    ctx.data.bin_val.ptr))
+			goto nest_cancel;
+		break;
+	case TEAM_OPTION_TYPE_BOOL:
+		if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_FLAG))
+			goto nest_cancel;
+		if (ctx.data.bool_val &&
+		    nla_put_flag(skb, TEAM_ATTR_OPTION_DATA))
+			goto nest_cancel;
+		break;
+	default:
+		BUG();
+	}
+	if (opt_inst->removed && nla_put_flag(skb, TEAM_ATTR_OPTION_REMOVED))
+		goto nest_cancel;
+	if (opt_inst->changed) {
+		if (nla_put_flag(skb, TEAM_ATTR_OPTION_CHANGED))
+			goto nest_cancel;
+		opt_inst->changed = false;
+	}
+	nla_nest_end(skb, option_item);
+	return 0;
+
+nest_cancel:
+	nla_nest_cancel(skb, option_item);
+	return -EMSGSIZE;
+}
+
+static int __send_and_alloc_skb(struct sk_buff **pskb,
+				struct team *team, u32 pid,
+				team_nl_send_func_t *send_func)
+{
+	int err;
+
+	if (*pskb) {
+		err = send_func(*pskb, team, pid);
+		if (err)
+			return err;
+	}
+	*pskb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!*pskb)
+		return -ENOMEM;
+	return 0;
+}
+
+static int team_nl_send_options_get(struct team *team, u32 pid, u32 seq,
+				    int flags, team_nl_send_func_t *send_func,
+				    struct list_head *sel_opt_inst_list)
 {
 	struct nlattr *option_list;
+	struct nlmsghdr *nlh;
 	void *hdr;
 	struct team_option_inst *opt_inst;
 	int err;
+	struct sk_buff *skb = NULL;
+	bool incomplete;
+	int i;
 
-	hdr = genlmsg_put(skb, pid, seq, &team_nl_family, flags,
+	opt_inst = list_first_entry(sel_opt_inst_list,
+				    struct team_option_inst, tmp_list);
+
+start_again:
+	err = __send_and_alloc_skb(&skb, team, pid, send_func);
+	if (err)
+		return err;
+
+	hdr = genlmsg_put(skb, pid, seq, &team_nl_family, flags | NLM_F_MULTI,
 			  TEAM_CMD_OPTIONS_GET);
 	if (IS_ERR(hdr))
 		return PTR_ERR(hdr);
@@ -1500,122 +1852,80 @@ static int team_nl_fill_options_get(struct sk_buff *skb,
 		goto nla_put_failure;
 	option_list = nla_nest_start(skb, TEAM_ATTR_LIST_OPTION);
 	if (!option_list)
-		return -EMSGSIZE;
-
-	list_for_each_entry(opt_inst, &team->option_inst_list, list) {
-		struct nlattr *option_item;
-		struct team_option *option = opt_inst->option;
-		struct team_gsetter_ctx ctx;
+		goto nla_put_failure;
 
-		/* Include only changed options if fill all mode is not on */
-		if (!fillall && !opt_inst->changed)
-			continue;
-		option_item = nla_nest_start(skb, TEAM_ATTR_ITEM_OPTION);
-		if (!option_item)
-			goto nla_put_failure;
-		if (nla_put_string(skb, TEAM_ATTR_OPTION_NAME, option->name))
-			goto nla_put_failure;
-		if (opt_inst->changed) {
-			if (nla_put_flag(skb, TEAM_ATTR_OPTION_CHANGED))
-				goto nla_put_failure;
-			opt_inst->changed = false;
-		}
-		if (opt_inst->removed &&
-		    nla_put_flag(skb, TEAM_ATTR_OPTION_REMOVED))
-			goto nla_put_failure;
-		if (opt_inst->port &&
-		    nla_put_u32(skb, TEAM_ATTR_OPTION_PORT_IFINDEX,
-				opt_inst->port->dev->ifindex))
-			goto nla_put_failure;
-		ctx.port = opt_inst->port;
-		switch (option->type) {
-		case TEAM_OPTION_TYPE_U32:
-			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_U32))
-				goto nla_put_failure;
-			err = team_option_get(team, opt_inst, &ctx);
-			if (err)
-				goto errout;
-			if (nla_put_u32(skb, TEAM_ATTR_OPTION_DATA,
-					ctx.data.u32_val))
-				goto nla_put_failure;
-			break;
-		case TEAM_OPTION_TYPE_STRING:
-			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_STRING))
-				goto nla_put_failure;
-			err = team_option_get(team, opt_inst, &ctx);
-			if (err)
-				goto errout;
-			if (nla_put_string(skb, TEAM_ATTR_OPTION_DATA,
-					   ctx.data.str_val))
-				goto nla_put_failure;
-			break;
-		case TEAM_OPTION_TYPE_BINARY:
-			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_BINARY))
-				goto nla_put_failure;
-			err = team_option_get(team, opt_inst, &ctx);
-			if (err)
-				goto errout;
-			if (nla_put(skb, TEAM_ATTR_OPTION_DATA,
-				    ctx.data.bin_val.len, ctx.data.bin_val.ptr))
-				goto nla_put_failure;
-			break;
-		case TEAM_OPTION_TYPE_BOOL:
-			if (nla_put_u8(skb, TEAM_ATTR_OPTION_TYPE, NLA_FLAG))
-				goto nla_put_failure;
-			err = team_option_get(team, opt_inst, &ctx);
-			if (err)
-				goto errout;
-			if (ctx.data.bool_val &&
-			    nla_put_flag(skb, TEAM_ATTR_OPTION_DATA))
-				goto nla_put_failure;
-			break;
-		default:
-			BUG();
+	i = 0;
+	incomplete = false;
+	list_for_each_entry_from(opt_inst, sel_opt_inst_list, tmp_list) {
+		err = team_nl_fill_one_option_get(skb, team, opt_inst);
+		if (err) {
+			if (err == -EMSGSIZE) {
+				if (!i)
+					goto errout;
+				incomplete = true;
+				break;
+			}
+			goto errout;
 		}
-		nla_nest_end(skb, option_item);
+		i++;
 	}
 
 	nla_nest_end(skb, option_list);
-	return genlmsg_end(skb, hdr);
+	genlmsg_end(skb, hdr);
+	if (incomplete)
+		goto start_again;
+
+send_done:
+	nlh = nlmsg_put(skb, pid, seq, NLMSG_DONE, 0, flags | NLM_F_MULTI);
+	if (!nlh) {
+		err = __send_and_alloc_skb(&skb, team, pid, send_func);
+		if (err)
+			goto errout;
+		goto send_done;
+	}
+
+	return send_func(skb, team, pid);
 
 nla_put_failure:
 	err = -EMSGSIZE;
 errout:
 	genlmsg_cancel(skb, hdr);
+	nlmsg_free(skb);
 	return err;
 }
 
-static int team_nl_fill_options_get_all(struct sk_buff *skb,
-					struct genl_info *info, int flags,
-					struct team *team)
-{
-	return team_nl_fill_options_get(skb, info->snd_pid,
-					info->snd_seq, NLM_F_ACK,
-					team, true);
-}
-
 static int team_nl_cmd_options_get(struct sk_buff *skb, struct genl_info *info)
 {
 	struct team *team;
+	struct team_option_inst *opt_inst;
 	int err;
+	LIST_HEAD(sel_opt_inst_list);
 
 	team = team_nl_team_get(info);
 	if (!team)
 		return -EINVAL;
 
-	err = team_nl_send_generic(info, team, team_nl_fill_options_get_all);
+	list_for_each_entry(opt_inst, &team->option_inst_list, list)
+		list_add_tail(&opt_inst->tmp_list, &sel_opt_inst_list);
+	err = team_nl_send_options_get(team, info->snd_pid, info->snd_seq,
+				       NLM_F_ACK, team_nl_send_unicast,
+				       &sel_opt_inst_list);
 
 	team_nl_team_put(team);
 
 	return err;
 }
 
+static int team_nl_send_event_options_get(struct team *team,
+					  struct list_head *sel_opt_inst_list);
+
 static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 {
 	struct team *team;
 	int err = 0;
 	int i;
 	struct nlattr *nl_option;
+	LIST_HEAD(opt_inst_list);
 
 	team = team_nl_team_get(info);
 	if (!team)
@@ -1629,10 +1939,12 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 
 	nla_for_each_nested(nl_option, info->attrs[TEAM_ATTR_LIST_OPTION], i) {
 		struct nlattr *opt_attrs[TEAM_ATTR_OPTION_MAX + 1];
-		struct nlattr *attr_port_ifindex;
+		struct nlattr *attr;
 		struct nlattr *attr_data;
 		enum team_option_type opt_type;
 		int opt_port_ifindex = 0; /* != 0 for per-port options */
+		u32 opt_array_index = 0;
+		bool opt_is_array = false;
 		struct team_option_inst *opt_inst;
 		char *opt_name;
 		bool opt_found = false;
@@ -1674,23 +1986,33 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 		}
 
 		opt_name = nla_data(opt_attrs[TEAM_ATTR_OPTION_NAME]);
-		attr_port_ifindex = opt_attrs[TEAM_ATTR_OPTION_PORT_IFINDEX];
-		if (attr_port_ifindex)
-			opt_port_ifindex = nla_get_u32(attr_port_ifindex);
+		attr = opt_attrs[TEAM_ATTR_OPTION_PORT_IFINDEX];
+		if (attr)
+			opt_port_ifindex = nla_get_u32(attr);
+
+		attr = opt_attrs[TEAM_ATTR_OPTION_ARRAY_INDEX];
+		if (attr) {
+			opt_is_array = true;
+			opt_array_index = nla_get_u32(attr);
+		}
 
 		list_for_each_entry(opt_inst, &team->option_inst_list, list) {
 			struct team_option *option = opt_inst->option;
 			struct team_gsetter_ctx ctx;
+			struct team_option_inst_info *opt_inst_info;
 			int tmp_ifindex;
 
-			tmp_ifindex = opt_inst->port ?
-				      opt_inst->port->dev->ifindex : 0;
+			opt_inst_info = &opt_inst->info;
+			tmp_ifindex = opt_inst_info->port ?
+				      opt_inst_info->port->dev->ifindex : 0;
 			if (option->type != opt_type ||
 			    strcmp(option->name, opt_name) ||
-			    tmp_ifindex != opt_port_ifindex)
+			    tmp_ifindex != opt_port_ifindex ||
+			    (option->array_size && !opt_is_array) ||
+			    opt_inst_info->array_index != opt_array_index)
 				continue;
 			opt_found = true;
-			ctx.port = opt_inst->port;
+			ctx.info = opt_inst_info;
 			switch (opt_type) {
 			case TEAM_OPTION_TYPE_U32:
 				ctx.data.u32_val = nla_get_u32(attr_data);
@@ -1715,6 +2037,8 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 			err = team_option_set(team, opt_inst, &ctx);
 			if (err)
 				goto team_put;
+			opt_inst->changed = true;
+			list_add(&opt_inst->tmp_list, &opt_inst_list);
 		}
 		if (!opt_found) {
 			err = -ENOENT;
@@ -1722,6 +2046,8 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 		}
 	}
 
+	err = team_nl_send_event_options_get(team, &opt_inst_list);
+
 team_put:
 	team_nl_team_put(team);
 
@@ -1746,7 +2072,7 @@ static int team_nl_fill_port_list_get(struct sk_buff *skb,
 		goto nla_put_failure;
 	port_list = nla_nest_start(skb, TEAM_ATTR_LIST_PORT);
 	if (!port_list)
-		return -EMSGSIZE;
+		goto nla_put_failure;
 
 	list_for_each_entry(port, &team->port_list, list) {
 		struct nlattr *port_item;
@@ -1838,27 +2164,18 @@ static struct genl_multicast_group team_change_event_mcgrp = {
 	.name = TEAM_GENL_CHANGE_EVENT_MC_GRP_NAME,
 };
 
-static int team_nl_send_event_options_get(struct team *team)
+static int team_nl_send_multicast(struct sk_buff *skb,
+				  struct team *team, u32 pid)
 {
-	struct sk_buff *skb;
-	int err;
-	struct net *net = dev_net(team->dev);
-
-	skb = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
-	if (!skb)
-		return -ENOMEM;
-
-	err = team_nl_fill_options_get(skb, 0, 0, 0, team, false);
-	if (err < 0)
-		goto err_fill;
-
-	err = genlmsg_multicast_netns(net, skb, 0, team_change_event_mcgrp.id,
-				      GFP_KERNEL);
-	return err;
+	return genlmsg_multicast_netns(dev_net(team->dev), skb, 0,
+				       team_change_event_mcgrp.id, GFP_KERNEL);
+}
 
-err_fill:
-	nlmsg_free(skb);
-	return err;
+static int team_nl_send_event_options_get(struct team *team,
+					  struct list_head *sel_opt_inst_list)
+{
+	return team_nl_send_options_get(team, 0, 0, 0, team_nl_send_multicast,
+					sel_opt_inst_list);
 }
 
 static int team_nl_send_event_port_list_get(struct team *team)
@@ -1867,7 +2184,7 @@ static int team_nl_send_event_port_list_get(struct team *team)
 	int err;
 	struct net *net = dev_net(team->dev);
 
-	skb = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
 	if (!skb)
 		return -ENOMEM;
 
@@ -1918,10 +2235,17 @@ static void team_nl_fini(void)
 static void __team_options_change_check(struct team *team)
 {
 	int err;
+	struct team_option_inst *opt_inst;
+	LIST_HEAD(sel_opt_inst_list);
 
-	err = team_nl_send_event_options_get(team);
+	list_for_each_entry(opt_inst, &team->option_inst_list, list) {
+		if (opt_inst->changed)
+			list_add_tail(&opt_inst->tmp_list, &sel_opt_inst_list);
+	}
+	err = team_nl_send_event_options_get(team, &sel_opt_inst_list);
 	if (err)
-		netdev_warn(team->dev, "Failed to send options change via netlink\n");
+		netdev_warn(team->dev, "Failed to send options change via netlink (err %d)\n",
+			    err);
 }
 
 /* rtnl lock is held */
@@ -1965,6 +2289,7 @@ static void team_port_change_check(struct team_port *port, bool linkup)
 	mutex_unlock(&team->lock);
 }
 
+
 /************************************
  * Net device notifier event handler
  ************************************/
diff --git a/drivers/net/team/team_mode_activebackup.c b/drivers/net/team/team_mode_activebackup.c
index fd6bd03aaa8..6262b4defd9 100644
--- a/drivers/net/team/team_mode_activebackup.c
+++ b/drivers/net/team/team_mode_activebackup.c
@@ -1,5 +1,5 @@
 /*
- * net/drivers/team/team_mode_activebackup.c - Active-backup mode for team
+ * drivers/net/team/team_mode_activebackup.c - Active-backup mode for team
  * Copyright (c) 2011 Jiri Pirko <jpirko@redhat.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -40,11 +40,10 @@ static bool ab_transmit(struct team *team, struct sk_buff *skb)
 {
 	struct team_port *active_port;
 
-	active_port = rcu_dereference(ab_priv(team)->active_port);
+	active_port = rcu_dereference_bh(ab_priv(team)->active_port);
 	if (unlikely(!active_port))
 		goto drop;
-	skb->dev = active_port->dev;
-	if (dev_queue_xmit(skb))
+	if (team_dev_queue_xmit(team, active_port, skb))
 		return false;
 	return true;
 
@@ -61,8 +60,12 @@ static void ab_port_leave(struct team *team, struct team_port *port)
 
 static int ab_active_port_get(struct team *team, struct team_gsetter_ctx *ctx)
 {
-	if (ab_priv(team)->active_port)
-		ctx->data.u32_val = ab_priv(team)->active_port->dev->ifindex;
+	struct team_port *active_port;
+
+	active_port = rcu_dereference_protected(ab_priv(team)->active_port,
+						lockdep_is_held(&team->lock));
+	if (active_port)
+		ctx->data.u32_val = active_port->dev->ifindex;
 	else
 		ctx->data.u32_val = 0;
 	return 0;
@@ -108,7 +111,7 @@ static const struct team_mode_ops ab_mode_ops = {
 	.port_leave		= ab_port_leave,
 };
 
-static struct team_mode ab_mode = {
+static const struct team_mode ab_mode = {
 	.kind		= "activebackup",
 	.owner		= THIS_MODULE,
 	.priv_size	= sizeof(struct ab_priv),
diff --git a/drivers/net/team/team_mode_broadcast.c b/drivers/net/team/team_mode_broadcast.c
new file mode 100644
index 00000000000..c96e4d2967f
--- /dev/null
+++ b/drivers/net/team/team_mode_broadcast.c
@@ -0,0 +1,87 @@
+/*
+ * drivers/net/team/team_mode_broadcast.c - Broadcast mode for team
+ * Copyright (c) 2012 Jiri Pirko <jpirko@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/netdevice.h>
+#include <linux/if_team.h>
+
+static bool bc_transmit(struct team *team, struct sk_buff *skb)
+{
+	struct team_port *cur;
+	struct team_port *last = NULL;
+	struct sk_buff *skb2;
+	bool ret;
+	bool sum_ret = false;
+
+	list_for_each_entry_rcu(cur, &team->port_list, list) {
+		if (team_port_txable(cur)) {
+			if (last) {
+				skb2 = skb_clone(skb, GFP_ATOMIC);
+				if (skb2) {
+					ret = team_dev_queue_xmit(team, last,
+								  skb2);
+					if (!sum_ret)
+						sum_ret = ret;
+				}
+			}
+			last = cur;
+		}
+	}
+	if (last) {
+		ret = team_dev_queue_xmit(team, last, skb);
+		if (!sum_ret)
+			sum_ret = ret;
+	}
+	return sum_ret;
+}
+
+static int bc_port_enter(struct team *team, struct team_port *port)
+{
+	return team_port_set_team_mac(port);
+}
+
+static void bc_port_change_mac(struct team *team, struct team_port *port)
+{
+	team_port_set_team_mac(port);
+}
+
+static const struct team_mode_ops bc_mode_ops = {
+	.transmit		= bc_transmit,
+	.port_enter		= bc_port_enter,
+	.port_change_mac	= bc_port_change_mac,
+};
+
+static const struct team_mode bc_mode = {
+	.kind		= "broadcast",
+	.owner		= THIS_MODULE,
+	.ops		= &bc_mode_ops,
+};
+
+static int __init bc_init_module(void)
+{
+	return team_mode_register(&bc_mode);
+}
+
+static void __exit bc_cleanup_module(void)
+{
+	team_mode_unregister(&bc_mode);
+}
+
+module_init(bc_init_module);
+module_exit(bc_cleanup_module);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Jiri Pirko <jpirko@redhat.com>");
+MODULE_DESCRIPTION("Broadcast mode for team");
+MODULE_ALIAS("team-mode-broadcast");
diff --git a/drivers/net/team/team_mode_loadbalance.c b/drivers/net/team/team_mode_loadbalance.c
index 86e8183c8e3..cdc31b5ea15 100644
--- a/drivers/net/team/team_mode_loadbalance.c
+++ b/drivers/net/team/team_mode_loadbalance.c
@@ -17,34 +17,209 @@
 #include <linux/filter.h>
 #include <linux/if_team.h>
 
+struct lb_priv;
+
+typedef struct team_port *lb_select_tx_port_func_t(struct team *,
+						   struct lb_priv *,
+						   struct sk_buff *,
+						   unsigned char);
+
+#define LB_TX_HASHTABLE_SIZE 256 /* hash is a char */
+
+struct lb_stats {
+	u64 tx_bytes;
+};
+
+struct lb_pcpu_stats {
+	struct lb_stats hash_stats[LB_TX_HASHTABLE_SIZE];
+	struct u64_stats_sync syncp;
+};
+
+struct lb_stats_info {
+	struct lb_stats stats;
+	struct lb_stats last_stats;
+	struct team_option_inst_info *opt_inst_info;
+};
+
+struct lb_port_mapping {
+	struct team_port __rcu *port;
+	struct team_option_inst_info *opt_inst_info;
+};
+
+struct lb_priv_ex {
+	struct team *team;
+	struct lb_port_mapping tx_hash_to_port_mapping[LB_TX_HASHTABLE_SIZE];
+	struct sock_fprog *orig_fprog;
+	struct {
+		unsigned int refresh_interval; /* in tenths of second */
+		struct delayed_work refresh_dw;
+		struct lb_stats_info info[LB_TX_HASHTABLE_SIZE];
+	} stats;
+};
+
 struct lb_priv {
 	struct sk_filter __rcu *fp;
-	struct sock_fprog *orig_fprog;
+	lb_select_tx_port_func_t __rcu *select_tx_port_func;
+	struct lb_pcpu_stats __percpu *pcpu_stats;
+	struct lb_priv_ex *ex; /* priv extension */
 };
 
-static struct lb_priv *lb_priv(struct team *team)
+static struct lb_priv *get_lb_priv(struct team *team)
 {
 	return (struct lb_priv *) &team->mode_priv;
 }
 
-static bool lb_transmit(struct team *team, struct sk_buff *skb)
+struct lb_port_priv {
+	struct lb_stats __percpu *pcpu_stats;
+	struct lb_stats_info stats_info;
+};
+
+static struct lb_port_priv *get_lb_port_priv(struct team_port *port)
+{
+	return (struct lb_port_priv *) &port->mode_priv;
+}
+
+#define LB_HTPM_PORT_BY_HASH(lp_priv, hash) \
+	(lb_priv)->ex->tx_hash_to_port_mapping[hash].port
+
+#define LB_HTPM_OPT_INST_INFO_BY_HASH(lp_priv, hash) \
+	(lb_priv)->ex->tx_hash_to_port_mapping[hash].opt_inst_info
+
+static void lb_tx_hash_to_port_mapping_null_port(struct team *team,
+						 struct team_port *port)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	bool changed = false;
+	int i;
+
+	for (i = 0; i < LB_TX_HASHTABLE_SIZE; i++) {
+		struct lb_port_mapping *pm;
+
+		pm = &lb_priv->ex->tx_hash_to_port_mapping[i];
+		if (rcu_access_pointer(pm->port) == port) {
+			RCU_INIT_POINTER(pm->port, NULL);
+			team_option_inst_set_change(pm->opt_inst_info);
+			changed = true;
+		}
+	}
+	if (changed)
+		team_options_change_check(team);
+}
+
+/* Basic tx selection based solely by hash */
+static struct team_port *lb_hash_select_tx_port(struct team *team,
+						struct lb_priv *lb_priv,
+						struct sk_buff *skb,
+						unsigned char hash)
 {
-	struct sk_filter *fp;
-	struct team_port *port;
-	unsigned int hash;
 	int port_index;
 
-	fp = rcu_dereference(lb_priv(team)->fp);
-	if (unlikely(!fp))
-		goto drop;
-	hash = SK_RUN_FILTER(fp, skb);
 	port_index = hash % team->en_port_count;
-	port = team_get_port_by_index_rcu(team, port_index);
+	return team_get_port_by_index_rcu(team, port_index);
+}
+
+/* Hash to port mapping select tx port */
+static struct team_port *lb_htpm_select_tx_port(struct team *team,
+						struct lb_priv *lb_priv,
+						struct sk_buff *skb,
+						unsigned char hash)
+{
+	return rcu_dereference_bh(LB_HTPM_PORT_BY_HASH(lb_priv, hash));
+}
+
+struct lb_select_tx_port {
+	char *name;
+	lb_select_tx_port_func_t *func;
+};
+
+static const struct lb_select_tx_port lb_select_tx_port_list[] = {
+	{
+		.name = "hash",
+		.func = lb_hash_select_tx_port,
+	},
+	{
+		.name = "hash_to_port_mapping",
+		.func = lb_htpm_select_tx_port,
+	},
+};
+#define LB_SELECT_TX_PORT_LIST_COUNT ARRAY_SIZE(lb_select_tx_port_list)
+
+static char *lb_select_tx_port_get_name(lb_select_tx_port_func_t *func)
+{
+	int i;
+
+	for (i = 0; i < LB_SELECT_TX_PORT_LIST_COUNT; i++) {
+		const struct lb_select_tx_port *item;
+
+		item = &lb_select_tx_port_list[i];
+		if (item->func == func)
+			return item->name;
+	}
+	return NULL;
+}
+
+static lb_select_tx_port_func_t *lb_select_tx_port_get_func(const char *name)
+{
+	int i;
+
+	for (i = 0; i < LB_SELECT_TX_PORT_LIST_COUNT; i++) {
+		const struct lb_select_tx_port *item;
+
+		item = &lb_select_tx_port_list[i];
+		if (!strcmp(item->name, name))
+			return item->func;
+	}
+	return NULL;
+}
+
+static unsigned int lb_get_skb_hash(struct lb_priv *lb_priv,
+				    struct sk_buff *skb)
+{
+	struct sk_filter *fp;
+	uint32_t lhash;
+	unsigned char *c;
+
+	fp = rcu_dereference_bh(lb_priv->fp);
+	if (unlikely(!fp))
+		return 0;
+	lhash = SK_RUN_FILTER(fp, skb);
+	c = (char *) &lhash;
+	return c[0] ^ c[1] ^ c[2] ^ c[3];
+}
+
+static void lb_update_tx_stats(unsigned int tx_bytes, struct lb_priv *lb_priv,
+			       struct lb_port_priv *lb_port_priv,
+			       unsigned char hash)
+{
+	struct lb_pcpu_stats *pcpu_stats;
+	struct lb_stats *port_stats;
+	struct lb_stats *hash_stats;
+
+	pcpu_stats = this_cpu_ptr(lb_priv->pcpu_stats);
+	port_stats = this_cpu_ptr(lb_port_priv->pcpu_stats);
+	hash_stats = &pcpu_stats->hash_stats[hash];
+	u64_stats_update_begin(&pcpu_stats->syncp);
+	port_stats->tx_bytes += tx_bytes;
+	hash_stats->tx_bytes += tx_bytes;
+	u64_stats_update_end(&pcpu_stats->syncp);
+}
+
+static bool lb_transmit(struct team *team, struct sk_buff *skb)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	lb_select_tx_port_func_t *select_tx_port_func;
+	struct team_port *port;
+	unsigned char hash;
+	unsigned int tx_bytes = skb->len;
+
+	hash = lb_get_skb_hash(lb_priv, skb);
+	select_tx_port_func = rcu_dereference_bh(lb_priv->select_tx_port_func);
+	port = select_tx_port_func(team, lb_priv, skb, hash);
 	if (unlikely(!port))
 		goto drop;
-	skb->dev = port->dev;
-	if (dev_queue_xmit(skb))
+	if (team_dev_queue_xmit(team, port, skb))
 		return false;
+	lb_update_tx_stats(tx_bytes, lb_priv, get_lb_port_priv(port), hash);
 	return true;
 
 drop:
@@ -54,14 +229,16 @@ drop:
 
 static int lb_bpf_func_get(struct team *team, struct team_gsetter_ctx *ctx)
 {
-	if (!lb_priv(team)->orig_fprog) {
+	struct lb_priv *lb_priv = get_lb_priv(team);
+
+	if (!lb_priv->ex->orig_fprog) {
 		ctx->data.bin_val.len = 0;
 		ctx->data.bin_val.ptr = NULL;
 		return 0;
 	}
-	ctx->data.bin_val.len = lb_priv(team)->orig_fprog->len *
+	ctx->data.bin_val.len = lb_priv->ex->orig_fprog->len *
 				sizeof(struct sock_filter);
-	ctx->data.bin_val.ptr = lb_priv(team)->orig_fprog->filter;
+	ctx->data.bin_val.ptr = lb_priv->ex->orig_fprog->filter;
 	return 0;
 }
 
@@ -94,7 +271,9 @@ static void __fprog_destroy(struct sock_fprog *fprog)
 
 static int lb_bpf_func_set(struct team *team, struct team_gsetter_ctx *ctx)
 {
+	struct lb_priv *lb_priv = get_lb_priv(team);
 	struct sk_filter *fp = NULL;
+	struct sk_filter *orig_fp;
 	struct sock_fprog *fprog = NULL;
 	int err;
 
@@ -110,14 +289,238 @@ static int lb_bpf_func_set(struct team *team, struct team_gsetter_ctx *ctx)
 		}
 	}
 
-	if (lb_priv(team)->orig_fprog) {
+	if (lb_priv->ex->orig_fprog) {
 		/* Clear old filter data */
-		__fprog_destroy(lb_priv(team)->orig_fprog);
-		sk_unattached_filter_destroy(lb_priv(team)->fp);
+		__fprog_destroy(lb_priv->ex->orig_fprog);
+		orig_fp = rcu_dereference_protected(lb_priv->fp,
+						lockdep_is_held(&team->lock));
+		sk_unattached_filter_destroy(orig_fp);
 	}
 
-	rcu_assign_pointer(lb_priv(team)->fp, fp);
-	lb_priv(team)->orig_fprog = fprog;
+	rcu_assign_pointer(lb_priv->fp, fp);
+	lb_priv->ex->orig_fprog = fprog;
+	return 0;
+}
+
+static int lb_tx_method_get(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	lb_select_tx_port_func_t *func;
+	char *name;
+
+	func = rcu_dereference_protected(lb_priv->select_tx_port_func,
+					 lockdep_is_held(&team->lock));
+	name = lb_select_tx_port_get_name(func);
+	BUG_ON(!name);
+	ctx->data.str_val = name;
+	return 0;
+}
+
+static int lb_tx_method_set(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	lb_select_tx_port_func_t *func;
+
+	func = lb_select_tx_port_get_func(ctx->data.str_val);
+	if (!func)
+		return -EINVAL;
+	rcu_assign_pointer(lb_priv->select_tx_port_func, func);
+	return 0;
+}
+
+static int lb_tx_hash_to_port_mapping_init(struct team *team,
+					   struct team_option_inst_info *info)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	unsigned char hash = info->array_index;
+
+	LB_HTPM_OPT_INST_INFO_BY_HASH(lb_priv, hash) = info;
+	return 0;
+}
+
+static int lb_tx_hash_to_port_mapping_get(struct team *team,
+					  struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	struct team_port *port;
+	unsigned char hash = ctx->info->array_index;
+
+	port = LB_HTPM_PORT_BY_HASH(lb_priv, hash);
+	ctx->data.u32_val = port ? port->dev->ifindex : 0;
+	return 0;
+}
+
+static int lb_tx_hash_to_port_mapping_set(struct team *team,
+					  struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	struct team_port *port;
+	unsigned char hash = ctx->info->array_index;
+
+	list_for_each_entry(port, &team->port_list, list) {
+		if (ctx->data.u32_val == port->dev->ifindex &&
+		    team_port_enabled(port)) {
+			rcu_assign_pointer(LB_HTPM_PORT_BY_HASH(lb_priv, hash),
+					   port);
+			return 0;
+		}
+	}
+	return -ENODEV;
+}
+
+static int lb_hash_stats_init(struct team *team,
+			      struct team_option_inst_info *info)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	unsigned char hash = info->array_index;
+
+	lb_priv->ex->stats.info[hash].opt_inst_info = info;
+	return 0;
+}
+
+static int lb_hash_stats_get(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	unsigned char hash = ctx->info->array_index;
+
+	ctx->data.bin_val.ptr = &lb_priv->ex->stats.info[hash].stats;
+	ctx->data.bin_val.len = sizeof(struct lb_stats);
+	return 0;
+}
+
+static int lb_port_stats_init(struct team *team,
+			      struct team_option_inst_info *info)
+{
+	struct team_port *port = info->port;
+	struct lb_port_priv *lb_port_priv = get_lb_port_priv(port);
+
+	lb_port_priv->stats_info.opt_inst_info = info;
+	return 0;
+}
+
+static int lb_port_stats_get(struct team *team, struct team_gsetter_ctx *ctx)
+{
+	struct team_port *port = ctx->info->port;
+	struct lb_port_priv *lb_port_priv = get_lb_port_priv(port);
+
+	ctx->data.bin_val.ptr = &lb_port_priv->stats_info.stats;
+	ctx->data.bin_val.len = sizeof(struct lb_stats);
+	return 0;
+}
+
+static void __lb_stats_info_refresh_prepare(struct lb_stats_info *s_info)
+{
+	memcpy(&s_info->last_stats, &s_info->stats, sizeof(struct lb_stats));
+	memset(&s_info->stats, 0, sizeof(struct lb_stats));
+}
+
+static bool __lb_stats_info_refresh_check(struct lb_stats_info *s_info,
+					  struct team *team)
+{
+	if (memcmp(&s_info->last_stats, &s_info->stats,
+	    sizeof(struct lb_stats))) {
+		team_option_inst_set_change(s_info->opt_inst_info);
+		return true;
+	}
+	return false;
+}
+
+static void __lb_one_cpu_stats_add(struct lb_stats *acc_stats,
+				   struct lb_stats *cpu_stats,
+				   struct u64_stats_sync *syncp)
+{
+	unsigned int start;
+	struct lb_stats tmp;
+
+	do {
+		start = u64_stats_fetch_begin_bh(syncp);
+		tmp.tx_bytes = cpu_stats->tx_bytes;
+	} while (u64_stats_fetch_retry_bh(syncp, start));
+	acc_stats->tx_bytes += tmp.tx_bytes;
+}
+
+static void lb_stats_refresh(struct work_struct *work)
+{
+	struct team *team;
+	struct lb_priv *lb_priv;
+	struct lb_priv_ex *lb_priv_ex;
+	struct lb_pcpu_stats *pcpu_stats;
+	struct lb_stats *stats;
+	struct lb_stats_info *s_info;
+	struct team_port *port;
+	bool changed = false;
+	int i;
+	int j;
+
+	lb_priv_ex = container_of(work, struct lb_priv_ex,
+				  stats.refresh_dw.work);
+
+	team = lb_priv_ex->team;
+	lb_priv = get_lb_priv(team);
+
+	if (!mutex_trylock(&team->lock)) {
+		schedule_delayed_work(&lb_priv_ex->stats.refresh_dw, 0);
+		return;
+	}
+
+	for (j = 0; j < LB_TX_HASHTABLE_SIZE; j++) {
+		s_info = &lb_priv->ex->stats.info[j];
+		__lb_stats_info_refresh_prepare(s_info);
+		for_each_possible_cpu(i) {
+			pcpu_stats = per_cpu_ptr(lb_priv->pcpu_stats, i);
+			stats = &pcpu_stats->hash_stats[j];
+			__lb_one_cpu_stats_add(&s_info->stats, stats,
+					       &pcpu_stats->syncp);
+		}
+		changed |= __lb_stats_info_refresh_check(s_info, team);
+	}
+
+	list_for_each_entry(port, &team->port_list, list) {
+		struct lb_port_priv *lb_port_priv = get_lb_port_priv(port);
+
+		s_info = &lb_port_priv->stats_info;
+		__lb_stats_info_refresh_prepare(s_info);
+		for_each_possible_cpu(i) {
+			pcpu_stats = per_cpu_ptr(lb_priv->pcpu_stats, i);
+			stats = per_cpu_ptr(lb_port_priv->pcpu_stats, i);
+			__lb_one_cpu_stats_add(&s_info->stats, stats,
+					       &pcpu_stats->syncp);
+		}
+		changed |= __lb_stats_info_refresh_check(s_info, team);
+	}
+
+	if (changed)
+		team_options_change_check(team);
+
+	schedule_delayed_work(&lb_priv_ex->stats.refresh_dw,
+			      (lb_priv_ex->stats.refresh_interval * HZ) / 10);
+
+	mutex_unlock(&team->lock);
+}
+
+static int lb_stats_refresh_interval_get(struct team *team,
+					 struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+
+	ctx->data.u32_val = lb_priv->ex->stats.refresh_interval;
+	return 0;
+}
+
+static int lb_stats_refresh_interval_set(struct team *team,
+					 struct team_gsetter_ctx *ctx)
+{
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	unsigned int interval;
+
+	interval = ctx->data.u32_val;
+	if (lb_priv->ex->stats.refresh_interval == interval)
+		return 0;
+	lb_priv->ex->stats.refresh_interval = interval;
+	if (interval)
+		schedule_delayed_work(&lb_priv->ex->stats.refresh_dw, 0);
+	else
+		cancel_delayed_work(&lb_priv->ex->stats.refresh_dw);
 	return 0;
 }
 
@@ -128,30 +531,125 @@ static const struct team_option lb_options[] = {
 		.getter = lb_bpf_func_get,
 		.setter = lb_bpf_func_set,
 	},
+	{
+		.name = "lb_tx_method",
+		.type = TEAM_OPTION_TYPE_STRING,
+		.getter = lb_tx_method_get,
+		.setter = lb_tx_method_set,
+	},
+	{
+		.name = "lb_tx_hash_to_port_mapping",
+		.array_size = LB_TX_HASHTABLE_SIZE,
+		.type = TEAM_OPTION_TYPE_U32,
+		.init = lb_tx_hash_to_port_mapping_init,
+		.getter = lb_tx_hash_to_port_mapping_get,
+		.setter = lb_tx_hash_to_port_mapping_set,
+	},
+	{
+		.name = "lb_hash_stats",
+		.array_size = LB_TX_HASHTABLE_SIZE,
+		.type = TEAM_OPTION_TYPE_BINARY,
+		.init = lb_hash_stats_init,
+		.getter = lb_hash_stats_get,
+	},
+	{
+		.name = "lb_port_stats",
+		.per_port = true,
+		.type = TEAM_OPTION_TYPE_BINARY,
+		.init = lb_port_stats_init,
+		.getter = lb_port_stats_get,
+	},
+	{
+		.name = "lb_stats_refresh_interval",
+		.type = TEAM_OPTION_TYPE_U32,
+		.getter = lb_stats_refresh_interval_get,
+		.setter = lb_stats_refresh_interval_set,
+	},
 };
 
 static int lb_init(struct team *team)
 {
-	return team_options_register(team, lb_options,
-				     ARRAY_SIZE(lb_options));
+	struct lb_priv *lb_priv = get_lb_priv(team);
+	lb_select_tx_port_func_t *func;
+	int err;
+
+	/* set default tx port selector */
+	func = lb_select_tx_port_get_func("hash");
+	BUG_ON(!func);
+	rcu_assign_pointer(lb_priv->select_tx_port_func, func);
+
+	lb_priv->ex = kzalloc(sizeof(*lb_priv->ex), GFP_KERNEL);
+	if (!lb_priv->ex)
+		return -ENOMEM;
+	lb_priv->ex->team = team;
+
+	lb_priv->pcpu_stats = alloc_percpu(struct lb_pcpu_stats);
+	if (!lb_priv->pcpu_stats) {
+		err = -ENOMEM;
+		goto err_alloc_pcpu_stats;
+	}
+
+	INIT_DELAYED_WORK(&lb_priv->ex->stats.refresh_dw, lb_stats_refresh);
+
+	err = team_options_register(team, lb_options, ARRAY_SIZE(lb_options));
+	if (err)
+		goto err_options_register;
+	return 0;
+
+err_options_register:
+	free_percpu(lb_priv->pcpu_stats);
+err_alloc_pcpu_stats:
+	kfree(lb_priv->ex);
+	return err;
 }
 
 static void lb_exit(struct team *team)
 {
+	struct lb_priv *lb_priv = get_lb_priv(team);
+
 	team_options_unregister(team, lb_options,
 				ARRAY_SIZE(lb_options));
+	cancel_delayed_work_sync(&lb_priv->ex->stats.refresh_dw);
+	free_percpu(lb_priv->pcpu_stats);
+	kfree(lb_priv->ex);
+}
+
+static int lb_port_enter(struct team *team, struct team_port *port)
+{
+	struct lb_port_priv *lb_port_priv = get_lb_port_priv(port);
+
+	lb_port_priv->pcpu_stats = alloc_percpu(struct lb_stats);
+	if (!lb_port_priv->pcpu_stats)
+		return -ENOMEM;
+	return 0;
+}
+
+static void lb_port_leave(struct team *team, struct team_port *port)
+{
+	struct lb_port_priv *lb_port_priv = get_lb_port_priv(port);
+
+	free_percpu(lb_port_priv->pcpu_stats);
+}
+
+static void lb_port_disabled(struct team *team, struct team_port *port)
+{
+	lb_tx_hash_to_port_mapping_null_port(team, port);
 }
 
 static const struct team_mode_ops lb_mode_ops = {
 	.init			= lb_init,
 	.exit			= lb_exit,
+	.port_enter		= lb_port_enter,
+	.port_leave		= lb_port_leave,
+	.port_disabled		= lb_port_disabled,
 	.transmit		= lb_transmit,
 };
 
-static struct team_mode lb_mode = {
+static const struct team_mode lb_mode = {
 	.kind		= "loadbalance",
 	.owner		= THIS_MODULE,
 	.priv_size	= sizeof(struct lb_priv),
+	.port_priv_size	= sizeof(struct lb_port_priv),
 	.ops		= &lb_mode_ops,
 };
 
diff --git a/drivers/net/team/team_mode_roundrobin.c b/drivers/net/team/team_mode_roundrobin.c
index 6abfbdc96be..ad7ed0ec544 100644
--- a/drivers/net/team/team_mode_roundrobin.c
+++ b/drivers/net/team/team_mode_roundrobin.c
@@ -1,5 +1,5 @@
 /*
- * net/drivers/team/team_mode_roundrobin.c - Round-robin mode for team
+ * drivers/net/team/team_mode_roundrobin.c - Round-robin mode for team
  * Copyright (c) 2011 Jiri Pirko <jpirko@redhat.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -30,16 +30,16 @@ static struct team_port *__get_first_port_up(struct team *team,
 {
 	struct team_port *cur;
 
-	if (port->linkup)
+	if (team_port_txable(port))
 		return port;
 	cur = port;
 	list_for_each_entry_continue_rcu(cur, &team->port_list, list)
-		if (cur->linkup)
+		if (team_port_txable(port))
 			return cur;
 	list_for_each_entry_rcu(cur, &team->port_list, list) {
 		if (cur == port)
 			break;
-		if (cur->linkup)
+		if (team_port_txable(port))
 			return cur;
 	}
 	return NULL;
@@ -55,8 +55,7 @@ static bool rr_transmit(struct team *team, struct sk_buff *skb)
 	port = __get_first_port_up(team, port);
 	if (unlikely(!port))
 		goto drop;
-	skb->dev = port->dev;
-	if (dev_queue_xmit(skb))
+	if (team_dev_queue_xmit(team, port, skb))
 		return false;
 	return true;
 
@@ -81,7 +80,7 @@ static const struct team_mode_ops rr_mode_ops = {
 	.port_change_mac	= rr_port_change_mac,
 };
 
-static struct team_mode rr_mode = {
+static const struct team_mode rr_mode = {
 	.kind		= "roundrobin",
 	.owner		= THIS_MODULE,
 	.priv_size	= sizeof(struct rr_priv),
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index 987aeefbc77..926d4db5cb3 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -22,7 +22,7 @@
  *    Add TUNSETLINK ioctl to set the link encapsulation
  *
  *  Mark Smith <markzzzsmith@yahoo.com.au>
- *    Use random_ether_addr() for tap MAC address.
+ *    Use eth_random_addr() for tap MAC address.
  *
  *  Harald Roelle <harald.roelle@ifi.lmu.de>  2004/04/20
  *    Fixes in packet dropping, queue length setting and queue wakeup.
@@ -100,6 +100,8 @@ do {								\
 } while (0)
 #endif
 
+#define GOODCOPY_LEN 128
+
 #define FLT_EXACT_COUNT 8
 struct tap_filter {
 	unsigned int    count;    /* Number of addrs. Zero means disabled */
@@ -358,6 +360,8 @@ static void tun_free_netdev(struct net_device *dev)
 {
 	struct tun_struct *tun = netdev_priv(dev);
 
+	BUG_ON(!test_bit(SOCK_EXTERNALLY_ALLOCATED, &tun->socket.flags));
+
 	sk_release_kernel(tun->socket.sk);
 }
 
@@ -414,6 +418,8 @@ static netdev_tx_t tun_net_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Orphan the skb - required as we might hang on to it
 	 * for indefinite time. */
+	if (unlikely(skb_orphan_frags(skb, GFP_ATOMIC)))
+		goto drop;
 	skb_orphan(skb);
 
 	/* Enqueue packet */
@@ -600,19 +606,100 @@ static struct sk_buff *tun_alloc_skb(struct tun_struct *tun,
 	return skb;
 }
 
+/* set skb frags from iovec, this can move to core network code for reuse */
+static int zerocopy_sg_from_iovec(struct sk_buff *skb, const struct iovec *from,
+				  int offset, size_t count)
+{
+	int len = iov_length(from, count) - offset;
+	int copy = skb_headlen(skb);
+	int size, offset1 = 0;
+	int i = 0;
+
+	/* Skip over from offset */
+	while (count && (offset >= from->iov_len)) {
+		offset -= from->iov_len;
+		++from;
+		--count;
+	}
+
+	/* copy up to skb headlen */
+	while (count && (copy > 0)) {
+		size = min_t(unsigned int, copy, from->iov_len - offset);
+		if (copy_from_user(skb->data + offset1, from->iov_base + offset,
+				   size))
+			return -EFAULT;
+		if (copy > size) {
+			++from;
+			--count;
+			offset = 0;
+		} else
+			offset += size;
+		copy -= size;
+		offset1 += size;
+	}
+
+	if (len == offset1)
+		return 0;
+
+	while (count--) {
+		struct page *page[MAX_SKB_FRAGS];
+		int num_pages;
+		unsigned long base;
+		unsigned long truesize;
+
+		len = from->iov_len - offset;
+		if (!len) {
+			offset = 0;
+			++from;
+			continue;
+		}
+		base = (unsigned long)from->iov_base + offset;
+		size = ((base & ~PAGE_MASK) + len + ~PAGE_MASK) >> PAGE_SHIFT;
+		if (i + size > MAX_SKB_FRAGS)
+			return -EMSGSIZE;
+		num_pages = get_user_pages_fast(base, size, 0, &page[i]);
+		if (num_pages != size) {
+			for (i = 0; i < num_pages; i++)
+				put_page(page[i]);
+			return -EFAULT;
+		}
+		truesize = size * PAGE_SIZE;
+		skb->data_len += len;
+		skb->len += len;
+		skb->truesize += truesize;
+		atomic_add(truesize, &skb->sk->sk_wmem_alloc);
+		while (len) {
+			int off = base & ~PAGE_MASK;
+			int size = min_t(int, len, PAGE_SIZE - off);
+			__skb_fill_page_desc(skb, i, page[i], off, size);
+			skb_shinfo(skb)->nr_frags++;
+			/* increase sk_wmem_alloc */
+			base += size;
+			len -= size;
+			i++;
+		}
+		offset = 0;
+		++from;
+	}
+	return 0;
+}
+
 /* Get packet from user space buffer */
-static ssize_t tun_get_user(struct tun_struct *tun,
-			    const struct iovec *iv, size_t count,
-			    int noblock)
+static ssize_t tun_get_user(struct tun_struct *tun, void *msg_control,
+			    const struct iovec *iv, size_t total_len,
+			    size_t count, int noblock)
 {
 	struct tun_pi pi = { 0, cpu_to_be16(ETH_P_IP) };
 	struct sk_buff *skb;
-	size_t len = count, align = NET_SKB_PAD;
+	size_t len = total_len, align = NET_SKB_PAD;
 	struct virtio_net_hdr gso = { 0 };
 	int offset = 0;
+	int copylen;
+	bool zerocopy = false;
+	int err;
 
 	if (!(tun->flags & TUN_NO_PI)) {
-		if ((len -= sizeof(pi)) > count)
+		if ((len -= sizeof(pi)) > total_len)
 			return -EINVAL;
 
 		if (memcpy_fromiovecend((void *)&pi, iv, 0, sizeof(pi)))
@@ -621,7 +708,7 @@ static ssize_t tun_get_user(struct tun_struct *tun,
 	}
 
 	if (tun->flags & TUN_VNET_HDR) {
-		if ((len -= tun->vnet_hdr_sz) > count)
+		if ((len -= tun->vnet_hdr_sz) > total_len)
 			return -EINVAL;
 
 		if (memcpy_fromiovecend((void *)&gso, iv, offset, sizeof(gso)))
@@ -643,14 +730,46 @@ static ssize_t tun_get_user(struct tun_struct *tun,
 			return -EINVAL;
 	}
 
-	skb = tun_alloc_skb(tun, align, len, gso.hdr_len, noblock);
+	if (msg_control)
+		zerocopy = true;
+
+	if (zerocopy) {
+		/* Userspace may produce vectors with count greater than
+		 * MAX_SKB_FRAGS, so we need to linearize parts of the skb
+		 * to let the rest of data to be fit in the frags.
+		 */
+		if (count > MAX_SKB_FRAGS) {
+			copylen = iov_length(iv, count - MAX_SKB_FRAGS);
+			if (copylen < offset)
+				copylen = 0;
+			else
+				copylen -= offset;
+		} else
+				copylen = 0;
+		/* There are 256 bytes to be copied in skb, so there is enough
+		 * room for skb expand head in case it is used.
+		 * The rest of the buffer is mapped from userspace.
+		 */
+		if (copylen < gso.hdr_len)
+			copylen = gso.hdr_len;
+		if (!copylen)
+			copylen = GOODCOPY_LEN;
+	} else
+		copylen = len;
+
+	skb = tun_alloc_skb(tun, align, copylen, gso.hdr_len, noblock);
 	if (IS_ERR(skb)) {
 		if (PTR_ERR(skb) != -EAGAIN)
 			tun->dev->stats.rx_dropped++;
 		return PTR_ERR(skb);
 	}
 
-	if (skb_copy_datagram_from_iovec(skb, 0, iv, offset, len)) {
+	if (zerocopy)
+		err = zerocopy_sg_from_iovec(skb, iv, offset, count);
+	else
+		err = skb_copy_datagram_from_iovec(skb, 0, iv, offset, len);
+
+	if (err) {
 		tun->dev->stats.rx_dropped++;
 		kfree_skb(skb);
 		return -EFAULT;
@@ -724,12 +843,18 @@ static ssize_t tun_get_user(struct tun_struct *tun,
 		skb_shinfo(skb)->gso_segs = 0;
 	}
 
+	/* copy skb_ubuf_info for callback when skb has no error */
+	if (zerocopy) {
+		skb_shinfo(skb)->destructor_arg = msg_control;
+		skb_shinfo(skb)->tx_flags |= SKBTX_DEV_ZEROCOPY;
+	}
+
 	netif_rx_ni(skb);
 
 	tun->dev->stats.rx_packets++;
 	tun->dev->stats.rx_bytes += len;
 
-	return count;
+	return total_len;
 }
 
 static ssize_t tun_chr_aio_write(struct kiocb *iocb, const struct iovec *iv,
@@ -744,7 +869,7 @@ static ssize_t tun_chr_aio_write(struct kiocb *iocb, const struct iovec *iv,
 
 	tun_debug(KERN_INFO, tun, "tun_chr_write %ld\n", count);
 
-	result = tun_get_user(tun, iv, iov_length(iv, count),
+	result = tun_get_user(tun, NULL, iv, iov_length(iv, count), count,
 			      file->f_flags & O_NONBLOCK);
 
 	tun_put(tun);
@@ -958,8 +1083,8 @@ static int tun_sendmsg(struct kiocb *iocb, struct socket *sock,
 		       struct msghdr *m, size_t total_len)
 {
 	struct tun_struct *tun = container_of(sock, struct tun_struct, socket);
-	return tun_get_user(tun, m->msg_iov, total_len,
-			    m->msg_flags & MSG_DONTWAIT);
+	return tun_get_user(tun, m->msg_control, m->msg_iov, total_len,
+			    m->msg_iovlen, m->msg_flags & MSG_DONTWAIT);
 }
 
 static int tun_recvmsg(struct kiocb *iocb, struct socket *sock,
@@ -1115,6 +1240,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 		tun->flags = flags;
 		tun->txflt.count = 0;
 		tun->vnet_hdr_sz = sizeof(struct virtio_net_hdr);
+		set_bit(SOCK_EXTERNALLY_ALLOCATED, &tun->socket.flags);
 
 		err = -ENOMEM;
 		sk = sk_alloc(&init_net, AF_UNSPEC, GFP_KERNEL, &tun_proto);
@@ -1128,6 +1254,7 @@ static int tun_set_iff(struct net *net, struct file *file, struct ifreq *ifr)
 		sock_init_data(&tun->socket, sk);
 		sk->sk_write_space = tun_sock_write_space;
 		sk->sk_sndbuf = INT_MAX;
+		sock_set_flag(sk, SOCK_ZEROCOPY);
 
 		tun_sk(sk)->tun = tun;
 
@@ -1252,10 +1379,12 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 	int vnet_hdr_sz;
 	int ret;
 
-	if (cmd == TUNSETIFF || _IOC_TYPE(cmd) == 0x89)
+	if (cmd == TUNSETIFF || _IOC_TYPE(cmd) == 0x89) {
 		if (copy_from_user(&ifr, argp, ifreq_len))
 			return -EFAULT;
-
+	} else {
+		memset(&ifr, 0, sizeof(ifr));
+	}
 	if (cmd == TUNGETFEATURES) {
 		/* Currently this just means: "what IFF flags are valid?".
 		 * This is needed because we never checked for invalid flags on
diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index 833e32f8d63..c1ae76968f4 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -134,6 +134,7 @@ config USB_NET_AX8817X
 	tristate "ASIX AX88xxx Based USB 2.0 Ethernet Adapters"
 	depends on USB_USBNET
 	select CRC32
+	select PHYLIB
 	default y
 	help
 	  This option adds support for ASIX AX88xxx based USB 2.0
diff --git a/drivers/net/usb/Makefile b/drivers/net/usb/Makefile
index a2e2d72c52a..bf063008c1a 100644
--- a/drivers/net/usb/Makefile
+++ b/drivers/net/usb/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_USB_PEGASUS)	+= pegasus.o
 obj-$(CONFIG_USB_RTL8150)	+= rtl8150.o
 obj-$(CONFIG_USB_HSO)		+= hso.o
 obj-$(CONFIG_USB_NET_AX8817X)	+= asix.o
+asix-y := asix_devices.o asix_common.o ax88172a.o
 obj-$(CONFIG_USB_NET_CDCETHER)	+= cdc_ether.o
 obj-$(CONFIG_USB_NET_CDC_EEM)	+= cdc_eem.o
 obj-$(CONFIG_USB_NET_DM9601)	+= dm9601.o
diff --git a/drivers/net/usb/asix.h b/drivers/net/usb/asix.h
new file mode 100644
index 00000000000..e889631161b
--- /dev/null
+++ b/drivers/net/usb/asix.h
@@ -0,0 +1,218 @@
+/*
+ * ASIX AX8817X based USB 2.0 Ethernet Devices
+ * Copyright (C) 2003-2006 David Hollis <dhollis@davehollis.com>
+ * Copyright (C) 2005 Phil Chang <pchang23@sbcglobal.net>
+ * Copyright (C) 2006 James Painter <jamie.painter@iname.com>
+ * Copyright (c) 2002-2003 TiVo 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef _ASIX_H
+#define _ASIX_H
+
+// #define	DEBUG			// error path messages, extra info
+// #define	VERBOSE			// more; success messages
+
+#include <linux/module.h>
+#include <linux/kmod.h>
+#include <linux/init.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/workqueue.h>
+#include <linux/mii.h>
+#include <linux/usb.h>
+#include <linux/crc32.h>
+#include <linux/usb/usbnet.h>
+#include <linux/slab.h>
+#include <linux/if_vlan.h>
+
+#define DRIVER_VERSION "22-Dec-2011"
+#define DRIVER_NAME "asix"
+
+/* ASIX AX8817X based USB 2.0 Ethernet Devices */
+
+#define AX_CMD_SET_SW_MII		0x06
+#define AX_CMD_READ_MII_REG		0x07
+#define AX_CMD_WRITE_MII_REG		0x08
+#define AX_CMD_SET_HW_MII		0x0a
+#define AX_CMD_READ_EEPROM		0x0b
+#define AX_CMD_WRITE_EEPROM		0x0c
+#define AX_CMD_WRITE_ENABLE		0x0d
+#define AX_CMD_WRITE_DISABLE		0x0e
+#define AX_CMD_READ_RX_CTL		0x0f
+#define AX_CMD_WRITE_RX_CTL		0x10
+#define AX_CMD_READ_IPG012		0x11
+#define AX_CMD_WRITE_IPG0		0x12
+#define AX_CMD_WRITE_IPG1		0x13
+#define AX_CMD_READ_NODE_ID		0x13
+#define AX_CMD_WRITE_NODE_ID		0x14
+#define AX_CMD_WRITE_IPG2		0x14
+#define AX_CMD_WRITE_MULTI_FILTER	0x16
+#define AX88172_CMD_READ_NODE_ID	0x17
+#define AX_CMD_READ_PHY_ID		0x19
+#define AX_CMD_READ_MEDIUM_STATUS	0x1a
+#define AX_CMD_WRITE_MEDIUM_MODE	0x1b
+#define AX_CMD_READ_MONITOR_MODE	0x1c
+#define AX_CMD_WRITE_MONITOR_MODE	0x1d
+#define AX_CMD_READ_GPIOS		0x1e
+#define AX_CMD_WRITE_GPIOS		0x1f
+#define AX_CMD_SW_RESET			0x20
+#define AX_CMD_SW_PHY_STATUS		0x21
+#define AX_CMD_SW_PHY_SELECT		0x22
+
+#define AX_PHY_SELECT_MASK		(BIT(3) | BIT(2))
+#define AX_PHY_SELECT_INTERNAL		0
+#define AX_PHY_SELECT_EXTERNAL		BIT(2)
+
+#define AX_MONITOR_MODE			0x01
+#define AX_MONITOR_LINK			0x02
+#define AX_MONITOR_MAGIC		0x04
+#define AX_MONITOR_HSFS			0x10
+
+/* AX88172 Medium Status Register values */
+#define AX88172_MEDIUM_FD		0x02
+#define AX88172_MEDIUM_TX		0x04
+#define AX88172_MEDIUM_FC		0x10
+#define AX88172_MEDIUM_DEFAULT \
+		( AX88172_MEDIUM_FD | AX88172_MEDIUM_TX | AX88172_MEDIUM_FC )
+
+#define AX_MCAST_FILTER_SIZE		8
+#define AX_MAX_MCAST			64
+
+#define AX_SWRESET_CLEAR		0x00
+#define AX_SWRESET_RR			0x01
+#define AX_SWRESET_RT			0x02
+#define AX_SWRESET_PRTE			0x04
+#define AX_SWRESET_PRL			0x08
+#define AX_SWRESET_BZ			0x10
+#define AX_SWRESET_IPRL			0x20
+#define AX_SWRESET_IPPD			0x40
+
+#define AX88772_IPG0_DEFAULT		0x15
+#define AX88772_IPG1_DEFAULT		0x0c
+#define AX88772_IPG2_DEFAULT		0x12
+
+/* AX88772 & AX88178 Medium Mode Register */
+#define AX_MEDIUM_PF		0x0080
+#define AX_MEDIUM_JFE		0x0040
+#define AX_MEDIUM_TFC		0x0020
+#define AX_MEDIUM_RFC		0x0010
+#define AX_MEDIUM_ENCK		0x0008
+#define AX_MEDIUM_AC		0x0004
+#define AX_MEDIUM_FD		0x0002
+#define AX_MEDIUM_GM		0x0001
+#define AX_MEDIUM_SM		0x1000
+#define AX_MEDIUM_SBP		0x0800
+#define AX_MEDIUM_PS		0x0200
+#define AX_MEDIUM_RE		0x0100
+
+#define AX88178_MEDIUM_DEFAULT	\
+	(AX_MEDIUM_PS | AX_MEDIUM_FD | AX_MEDIUM_AC | \
+	 AX_MEDIUM_RFC | AX_MEDIUM_TFC | AX_MEDIUM_JFE | \
+	 AX_MEDIUM_RE)
+
+#define AX88772_MEDIUM_DEFAULT	\
+	(AX_MEDIUM_FD | AX_MEDIUM_RFC | \
+	 AX_MEDIUM_TFC | AX_MEDIUM_PS | \
+	 AX_MEDIUM_AC | AX_MEDIUM_RE)
+
+/* AX88772 & AX88178 RX_CTL values */
+#define AX_RX_CTL_SO		0x0080
+#define AX_RX_CTL_AP		0x0020
+#define AX_RX_CTL_AM		0x0010
+#define AX_RX_CTL_AB		0x0008
+#define AX_RX_CTL_SEP		0x0004
+#define AX_RX_CTL_AMALL		0x0002
+#define AX_RX_CTL_PRO		0x0001
+#define AX_RX_CTL_MFB_2048	0x0000
+#define AX_RX_CTL_MFB_4096	0x0100
+#define AX_RX_CTL_MFB_8192	0x0200
+#define AX_RX_CTL_MFB_16384	0x0300
+
+#define AX_DEFAULT_RX_CTL	(AX_RX_CTL_SO | AX_RX_CTL_AB)
+
+/* GPIO 0 .. 2 toggles */
+#define AX_GPIO_GPO0EN		0x01	/* GPIO0 Output enable */
+#define AX_GPIO_GPO_0		0x02	/* GPIO0 Output value */
+#define AX_GPIO_GPO1EN		0x04	/* GPIO1 Output enable */
+#define AX_GPIO_GPO_1		0x08	/* GPIO1 Output value */
+#define AX_GPIO_GPO2EN		0x10	/* GPIO2 Output enable */
+#define AX_GPIO_GPO_2		0x20	/* GPIO2 Output value */
+#define AX_GPIO_RESERVED	0x40	/* Reserved */
+#define AX_GPIO_RSE		0x80	/* Reload serial EEPROM */
+
+#define AX_EEPROM_MAGIC		0xdeadbeef
+#define AX_EEPROM_LEN		0x200
+
+/* This structure cannot exceed sizeof(unsigned long [5]) AKA 20 bytes */
+struct asix_data {
+	u8 multi_filter[AX_MCAST_FILTER_SIZE];
+	u8 mac_addr[ETH_ALEN];
+	u8 phymode;
+	u8 ledmode;
+	u8 res;
+};
+
+int asix_read_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+		  u16 size, void *data);
+
+int asix_write_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+		   u16 size, void *data);
+
+void asix_write_cmd_async(struct usbnet *dev, u8 cmd, u16 value,
+			  u16 index, u16 size, void *data);
+
+int asix_rx_fixup(struct usbnet *dev, struct sk_buff *skb);
+
+struct sk_buff *asix_tx_fixup(struct usbnet *dev, struct sk_buff *skb,
+			      gfp_t flags);
+
+int asix_set_sw_mii(struct usbnet *dev);
+int asix_set_hw_mii(struct usbnet *dev);
+
+int asix_read_phy_addr(struct usbnet *dev, int internal);
+int asix_get_phy_addr(struct usbnet *dev);
+
+int asix_sw_reset(struct usbnet *dev, u8 flags);
+
+u16 asix_read_rx_ctl(struct usbnet *dev);
+int asix_write_rx_ctl(struct usbnet *dev, u16 mode);
+
+u16 asix_read_medium_status(struct usbnet *dev);
+int asix_write_medium_mode(struct usbnet *dev, u16 mode);
+
+int asix_write_gpio(struct usbnet *dev, u16 value, int sleep);
+
+void asix_set_multicast(struct net_device *net);
+
+int asix_mdio_read(struct net_device *netdev, int phy_id, int loc);
+void asix_mdio_write(struct net_device *netdev, int phy_id, int loc, int val);
+
+void asix_get_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo);
+int asix_set_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo);
+
+int asix_get_eeprom_len(struct net_device *net);
+int asix_get_eeprom(struct net_device *net, struct ethtool_eeprom *eeprom,
+		    u8 *data);
+int asix_set_eeprom(struct net_device *net, struct ethtool_eeprom *eeprom,
+		    u8 *data);
+
+void asix_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *info);
+
+int asix_set_mac_address(struct net_device *net, void *p);
+
+#endif /* _ASIX_H */
diff --git a/drivers/net/usb/asix_common.c b/drivers/net/usb/asix_common.c
new file mode 100644
index 00000000000..774d9ce2daf
--- /dev/null
+++ b/drivers/net/usb/asix_common.c
@@ -0,0 +1,631 @@
+/*
+ * ASIX AX8817X based USB 2.0 Ethernet Devices
+ * Copyright (C) 2003-2006 David Hollis <dhollis@davehollis.com>
+ * Copyright (C) 2005 Phil Chang <pchang23@sbcglobal.net>
+ * Copyright (C) 2006 James Painter <jamie.painter@iname.com>
+ * Copyright (c) 2002-2003 TiVo 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include "asix.h"
+
+int asix_read_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+		  u16 size, void *data)
+{
+	void *buf;
+	int err = -ENOMEM;
+
+	netdev_dbg(dev->net, "asix_read_cmd() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
+		   cmd, value, index, size);
+
+	buf = kmalloc(size, GFP_KERNEL);
+	if (!buf)
+		goto out;
+
+	err = usb_control_msg(
+		dev->udev,
+		usb_rcvctrlpipe(dev->udev, 0),
+		cmd,
+		USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+		value,
+		index,
+		buf,
+		size,
+		USB_CTRL_GET_TIMEOUT);
+	if (err == size)
+		memcpy(data, buf, size);
+	else if (err >= 0)
+		err = -EINVAL;
+	kfree(buf);
+
+out:
+	return err;
+}
+
+int asix_write_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+		   u16 size, void *data)
+{
+	void *buf = NULL;
+	int err = -ENOMEM;
+
+	netdev_dbg(dev->net, "asix_write_cmd() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
+		   cmd, value, index, size);
+
+	if (data) {
+		buf = kmemdup(data, size, GFP_KERNEL);
+		if (!buf)
+			goto out;
+	}
+
+	err = usb_control_msg(
+		dev->udev,
+		usb_sndctrlpipe(dev->udev, 0),
+		cmd,
+		USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+		value,
+		index,
+		buf,
+		size,
+		USB_CTRL_SET_TIMEOUT);
+	kfree(buf);
+
+out:
+	return err;
+}
+
+static void asix_async_cmd_callback(struct urb *urb)
+{
+	struct usb_ctrlrequest *req = (struct usb_ctrlrequest *)urb->context;
+	int status = urb->status;
+
+	if (status < 0)
+		printk(KERN_DEBUG "asix_async_cmd_callback() failed with %d",
+			status);
+
+	kfree(req);
+	usb_free_urb(urb);
+}
+
+void asix_write_cmd_async(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+			  u16 size, void *data)
+{
+	struct usb_ctrlrequest *req;
+	int status;
+	struct urb *urb;
+
+	netdev_dbg(dev->net, "asix_write_cmd_async() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
+		   cmd, value, index, size);
+
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!urb) {
+		netdev_err(dev->net, "Error allocating URB in write_cmd_async!\n");
+		return;
+	}
+
+	req = kmalloc(sizeof(struct usb_ctrlrequest), GFP_ATOMIC);
+	if (!req) {
+		netdev_err(dev->net, "Failed to allocate memory for control request\n");
+		usb_free_urb(urb);
+		return;
+	}
+
+	req->bRequestType = USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
+	req->bRequest = cmd;
+	req->wValue = cpu_to_le16(value);
+	req->wIndex = cpu_to_le16(index);
+	req->wLength = cpu_to_le16(size);
+
+	usb_fill_control_urb(urb, dev->udev,
+			     usb_sndctrlpipe(dev->udev, 0),
+			     (void *)req, data, size,
+			     asix_async_cmd_callback, req);
+
+	status = usb_submit_urb(urb, GFP_ATOMIC);
+	if (status < 0) {
+		netdev_err(dev->net, "Error submitting the control message: status=%d\n",
+			   status);
+		kfree(req);
+		usb_free_urb(urb);
+	}
+}
+
+int asix_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
+{
+	int offset = 0;
+
+	while (offset + sizeof(u32) < skb->len) {
+		struct sk_buff *ax_skb;
+		u16 size;
+		u32 header = get_unaligned_le32(skb->data + offset);
+
+		offset += sizeof(u32);
+
+		/* get the packet length */
+		size = (u16) (header & 0x7ff);
+		if (size != ((~header >> 16) & 0x07ff)) {
+			netdev_err(dev->net, "asix_rx_fixup() Bad Header Length\n");
+			return 0;
+		}
+
+		if ((size > dev->net->mtu + ETH_HLEN + VLAN_HLEN) ||
+		    (size + offset > skb->len)) {
+			netdev_err(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
+				   size);
+			return 0;
+		}
+		ax_skb = netdev_alloc_skb_ip_align(dev->net, size);
+		if (!ax_skb)
+			return 0;
+
+		skb_put(ax_skb, size);
+		memcpy(ax_skb->data, skb->data + offset, size);
+		usbnet_skb_return(dev, ax_skb);
+
+		offset += (size + 1) & 0xfffe;
+	}
+
+	if (skb->len != offset) {
+		netdev_err(dev->net, "asix_rx_fixup() Bad SKB Length %d\n",
+			   skb->len);
+		return 0;
+	}
+	return 1;
+}
+
+struct sk_buff *asix_tx_fixup(struct usbnet *dev, struct sk_buff *skb,
+			      gfp_t flags)
+{
+	int padlen;
+	int headroom = skb_headroom(skb);
+	int tailroom = skb_tailroom(skb);
+	u32 packet_len;
+	u32 padbytes = 0xffff0000;
+
+	padlen = ((skb->len + 4) & (dev->maxpacket - 1)) ? 0 : 4;
+
+	/* We need to push 4 bytes in front of frame (packet_len)
+	 * and maybe add 4 bytes after the end (if padlen is 4)
+	 *
+	 * Avoid skb_copy_expand() expensive call, using following rules :
+	 * - We are allowed to push 4 bytes in headroom if skb_header_cloned()
+	 *   is false (and if we have 4 bytes of headroom)
+	 * - We are allowed to put 4 bytes at tail if skb_cloned()
+	 *   is false (and if we have 4 bytes of tailroom)
+	 *
+	 * TCP packets for example are cloned, but skb_header_release()
+	 * was called in tcp stack, allowing us to use headroom for our needs.
+	 */
+	if (!skb_header_cloned(skb) &&
+	    !(padlen && skb_cloned(skb)) &&
+	    headroom + tailroom >= 4 + padlen) {
+		/* following should not happen, but better be safe */
+		if (headroom < 4 ||
+		    tailroom < padlen) {
+			skb->data = memmove(skb->head + 4, skb->data, skb->len);
+			skb_set_tail_pointer(skb, skb->len);
+		}
+	} else {
+		struct sk_buff *skb2;
+
+		skb2 = skb_copy_expand(skb, 4, padlen, flags);
+		dev_kfree_skb_any(skb);
+		skb = skb2;
+		if (!skb)
+			return NULL;
+	}
+
+	packet_len = ((skb->len ^ 0x0000ffff) << 16) + skb->len;
+	skb_push(skb, 4);
+	cpu_to_le32s(&packet_len);
+	skb_copy_to_linear_data(skb, &packet_len, sizeof(packet_len));
+
+	if (padlen) {
+		cpu_to_le32s(&padbytes);
+		memcpy(skb_tail_pointer(skb), &padbytes, sizeof(padbytes));
+		skb_put(skb, sizeof(padbytes));
+	}
+	return skb;
+}
+
+int asix_set_sw_mii(struct usbnet *dev)
+{
+	int ret;
+	ret = asix_write_cmd(dev, AX_CMD_SET_SW_MII, 0x0000, 0, 0, NULL);
+	if (ret < 0)
+		netdev_err(dev->net, "Failed to enable software MII access\n");
+	return ret;
+}
+
+int asix_set_hw_mii(struct usbnet *dev)
+{
+	int ret;
+	ret = asix_write_cmd(dev, AX_CMD_SET_HW_MII, 0x0000, 0, 0, NULL);
+	if (ret < 0)
+		netdev_err(dev->net, "Failed to enable hardware MII access\n");
+	return ret;
+}
+
+int asix_read_phy_addr(struct usbnet *dev, int internal)
+{
+	int offset = (internal ? 1 : 0);
+	u8 buf[2];
+	int ret = asix_read_cmd(dev, AX_CMD_READ_PHY_ID, 0, 0, 2, buf);
+
+	netdev_dbg(dev->net, "asix_get_phy_addr()\n");
+
+	if (ret < 0) {
+		netdev_err(dev->net, "Error reading PHYID register: %02x\n", ret);
+		goto out;
+	}
+	netdev_dbg(dev->net, "asix_get_phy_addr() returning 0x%04x\n",
+		   *((__le16 *)buf));
+	ret = buf[offset];
+
+out:
+	return ret;
+}
+
+int asix_get_phy_addr(struct usbnet *dev)
+{
+	/* return the address of the internal phy */
+	return asix_read_phy_addr(dev, 1);
+}
+
+
+int asix_sw_reset(struct usbnet *dev, u8 flags)
+{
+	int ret;
+
+        ret = asix_write_cmd(dev, AX_CMD_SW_RESET, flags, 0, 0, NULL);
+	if (ret < 0)
+		netdev_err(dev->net, "Failed to send software reset: %02x\n", ret);
+
+	return ret;
+}
+
+u16 asix_read_rx_ctl(struct usbnet *dev)
+{
+	__le16 v;
+	int ret = asix_read_cmd(dev, AX_CMD_READ_RX_CTL, 0, 0, 2, &v);
+
+	if (ret < 0) {
+		netdev_err(dev->net, "Error reading RX_CTL register: %02x\n", ret);
+		goto out;
+	}
+	ret = le16_to_cpu(v);
+out:
+	return ret;
+}
+
+int asix_write_rx_ctl(struct usbnet *dev, u16 mode)
+{
+	int ret;
+
+	netdev_dbg(dev->net, "asix_write_rx_ctl() - mode = 0x%04x\n", mode);
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_RX_CTL, mode, 0, 0, NULL);
+	if (ret < 0)
+		netdev_err(dev->net, "Failed to write RX_CTL mode to 0x%04x: %02x\n",
+			   mode, ret);
+
+	return ret;
+}
+
+u16 asix_read_medium_status(struct usbnet *dev)
+{
+	__le16 v;
+	int ret = asix_read_cmd(dev, AX_CMD_READ_MEDIUM_STATUS, 0, 0, 2, &v);
+
+	if (ret < 0) {
+		netdev_err(dev->net, "Error reading Medium Status register: %02x\n",
+			   ret);
+		return ret;	/* TODO: callers not checking for error ret */
+	}
+
+	return le16_to_cpu(v);
+
+}
+
+int asix_write_medium_mode(struct usbnet *dev, u16 mode)
+{
+	int ret;
+
+	netdev_dbg(dev->net, "asix_write_medium_mode() - mode = 0x%04x\n", mode);
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, mode, 0, 0, NULL);
+	if (ret < 0)
+		netdev_err(dev->net, "Failed to write Medium Mode mode to 0x%04x: %02x\n",
+			   mode, ret);
+
+	return ret;
+}
+
+int asix_write_gpio(struct usbnet *dev, u16 value, int sleep)
+{
+	int ret;
+
+	netdev_dbg(dev->net, "asix_write_gpio() - value = 0x%04x\n", value);
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_GPIOS, value, 0, 0, NULL);
+	if (ret < 0)
+		netdev_err(dev->net, "Failed to write GPIO value 0x%04x: %02x\n",
+			   value, ret);
+
+	if (sleep)
+		msleep(sleep);
+
+	return ret;
+}
+
+/*
+ * AX88772 & AX88178 have a 16-bit RX_CTL value
+ */
+void asix_set_multicast(struct net_device *net)
+{
+	struct usbnet *dev = netdev_priv(net);
+	struct asix_data *data = (struct asix_data *)&dev->data;
+	u16 rx_ctl = AX_DEFAULT_RX_CTL;
+
+	if (net->flags & IFF_PROMISC) {
+		rx_ctl |= AX_RX_CTL_PRO;
+	} else if (net->flags & IFF_ALLMULTI ||
+		   netdev_mc_count(net) > AX_MAX_MCAST) {
+		rx_ctl |= AX_RX_CTL_AMALL;
+	} else if (netdev_mc_empty(net)) {
+		/* just broadcast and directed */
+	} else {
+		/* We use the 20 byte dev->data
+		 * for our 8 byte filter buffer
+		 * to avoid allocating memory that
+		 * is tricky to free later */
+		struct netdev_hw_addr *ha;
+		u32 crc_bits;
+
+		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
+
+		/* Build the multicast hash filter. */
+		netdev_for_each_mc_addr(ha, net) {
+			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
+			data->multi_filter[crc_bits >> 3] |=
+			    1 << (crc_bits & 7);
+		}
+
+		asix_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
+				   AX_MCAST_FILTER_SIZE, data->multi_filter);
+
+		rx_ctl |= AX_RX_CTL_AM;
+	}
+
+	asix_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
+}
+
+int asix_mdio_read(struct net_device *netdev, int phy_id, int loc)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	__le16 res;
+
+	mutex_lock(&dev->phy_mutex);
+	asix_set_sw_mii(dev);
+	asix_read_cmd(dev, AX_CMD_READ_MII_REG, phy_id,
+				(__u16)loc, 2, &res);
+	asix_set_hw_mii(dev);
+	mutex_unlock(&dev->phy_mutex);
+
+	netdev_dbg(dev->net, "asix_mdio_read() phy_id=0x%02x, loc=0x%02x, returns=0x%04x\n",
+		   phy_id, loc, le16_to_cpu(res));
+
+	return le16_to_cpu(res);
+}
+
+void asix_mdio_write(struct net_device *netdev, int phy_id, int loc, int val)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	__le16 res = cpu_to_le16(val);
+
+	netdev_dbg(dev->net, "asix_mdio_write() phy_id=0x%02x, loc=0x%02x, val=0x%04x\n",
+		   phy_id, loc, val);
+	mutex_lock(&dev->phy_mutex);
+	asix_set_sw_mii(dev);
+	asix_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id, (__u16)loc, 2, &res);
+	asix_set_hw_mii(dev);
+	mutex_unlock(&dev->phy_mutex);
+}
+
+void asix_get_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
+{
+	struct usbnet *dev = netdev_priv(net);
+	u8 opt;
+
+	if (asix_read_cmd(dev, AX_CMD_READ_MONITOR_MODE, 0, 0, 1, &opt) < 0) {
+		wolinfo->supported = 0;
+		wolinfo->wolopts = 0;
+		return;
+	}
+	wolinfo->supported = WAKE_PHY | WAKE_MAGIC;
+	wolinfo->wolopts = 0;
+	if (opt & AX_MONITOR_LINK)
+		wolinfo->wolopts |= WAKE_PHY;
+	if (opt & AX_MONITOR_MAGIC)
+		wolinfo->wolopts |= WAKE_MAGIC;
+}
+
+int asix_set_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
+{
+	struct usbnet *dev = netdev_priv(net);
+	u8 opt = 0;
+
+	if (wolinfo->wolopts & WAKE_PHY)
+		opt |= AX_MONITOR_LINK;
+	if (wolinfo->wolopts & WAKE_MAGIC)
+		opt |= AX_MONITOR_MAGIC;
+
+	if (asix_write_cmd(dev, AX_CMD_WRITE_MONITOR_MODE,
+			      opt, 0, 0, NULL) < 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+int asix_get_eeprom_len(struct net_device *net)
+{
+	return AX_EEPROM_LEN;
+}
+
+int asix_get_eeprom(struct net_device *net, struct ethtool_eeprom *eeprom,
+		    u8 *data)
+{
+	struct usbnet *dev = netdev_priv(net);
+	u16 *eeprom_buff;
+	int first_word, last_word;
+	int i;
+
+	if (eeprom->len == 0)
+		return -EINVAL;
+
+	eeprom->magic = AX_EEPROM_MAGIC;
+
+	first_word = eeprom->offset >> 1;
+	last_word = (eeprom->offset + eeprom->len - 1) >> 1;
+
+	eeprom_buff = kmalloc(sizeof(u16) * (last_word - first_word + 1),
+			      GFP_KERNEL);
+	if (!eeprom_buff)
+		return -ENOMEM;
+
+	/* ax8817x returns 2 bytes from eeprom on read */
+	for (i = first_word; i <= last_word; i++) {
+		if (asix_read_cmd(dev, AX_CMD_READ_EEPROM, i, 0, 2,
+				  &(eeprom_buff[i - first_word])) < 0) {
+			kfree(eeprom_buff);
+			return -EIO;
+		}
+	}
+
+	memcpy(data, (u8 *)eeprom_buff + (eeprom->offset & 1), eeprom->len);
+	kfree(eeprom_buff);
+	return 0;
+}
+
+int asix_set_eeprom(struct net_device *net, struct ethtool_eeprom *eeprom,
+		    u8 *data)
+{
+	struct usbnet *dev = netdev_priv(net);
+	u16 *eeprom_buff;
+	int first_word, last_word;
+	int i;
+	int ret;
+
+	netdev_dbg(net, "write EEPROM len %d, offset %d, magic 0x%x\n",
+		   eeprom->len, eeprom->offset, eeprom->magic);
+
+	if (eeprom->len == 0)
+		return -EINVAL;
+
+	if (eeprom->magic != AX_EEPROM_MAGIC)
+		return -EINVAL;
+
+	first_word = eeprom->offset >> 1;
+	last_word = (eeprom->offset + eeprom->len - 1) >> 1;
+
+	eeprom_buff = kmalloc(sizeof(u16) * (last_word - first_word + 1),
+			      GFP_KERNEL);
+	if (!eeprom_buff)
+		return -ENOMEM;
+
+	/* align data to 16 bit boundaries, read the missing data from
+	   the EEPROM */
+	if (eeprom->offset & 1) {
+		ret = asix_read_cmd(dev, AX_CMD_READ_EEPROM, first_word, 0, 2,
+				    &(eeprom_buff[0]));
+		if (ret < 0) {
+			netdev_err(net, "Failed to read EEPROM at offset 0x%02x.\n", first_word);
+			goto free;
+		}
+	}
+
+	if ((eeprom->offset + eeprom->len) & 1) {
+		ret = asix_read_cmd(dev, AX_CMD_READ_EEPROM, last_word, 0, 2,
+				    &(eeprom_buff[last_word - first_word]));
+		if (ret < 0) {
+			netdev_err(net, "Failed to read EEPROM at offset 0x%02x.\n", last_word);
+			goto free;
+		}
+	}
+
+	memcpy((u8 *)eeprom_buff + (eeprom->offset & 1), data, eeprom->len);
+
+	/* write data to EEPROM */
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_ENABLE, 0x0000, 0, 0, NULL);
+	if (ret < 0) {
+		netdev_err(net, "Failed to enable EEPROM write\n");
+		goto free;
+	}
+	msleep(20);
+
+	for (i = first_word; i <= last_word; i++) {
+		netdev_dbg(net, "write to EEPROM at offset 0x%02x, data 0x%04x\n",
+			   i, eeprom_buff[i - first_word]);
+		ret = asix_write_cmd(dev, AX_CMD_WRITE_EEPROM, i,
+				     eeprom_buff[i - first_word], 0, NULL);
+		if (ret < 0) {
+			netdev_err(net, "Failed to write EEPROM at offset 0x%02x.\n",
+				   i);
+			goto free;
+		}
+		msleep(20);
+	}
+
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_DISABLE, 0x0000, 0, 0, NULL);
+	if (ret < 0) {
+		netdev_err(net, "Failed to disable EEPROM write\n");
+		goto free;
+	}
+
+	ret = 0;
+free:
+	kfree(eeprom_buff);
+	return ret;
+}
+
+void asix_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *info)
+{
+	/* Inherit standard device info */
+	usbnet_get_drvinfo(net, info);
+	strncpy (info->driver, DRIVER_NAME, sizeof info->driver);
+	strncpy (info->version, DRIVER_VERSION, sizeof info->version);
+	info->eedump_len = AX_EEPROM_LEN;
+}
+
+int asix_set_mac_address(struct net_device *net, void *p)
+{
+	struct usbnet *dev = netdev_priv(net);
+	struct asix_data *data = (struct asix_data *)&dev->data;
+	struct sockaddr *addr = p;
+
+	if (netif_running(net))
+		return -EBUSY;
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	memcpy(net->dev_addr, addr->sa_data, ETH_ALEN);
+
+	/* We use the 20 byte dev->data
+	 * for our 6 byte mac buffer
+	 * to avoid allocating memory that
+	 * is tricky to free later */
+	memcpy(data->mac_addr, addr->sa_data, ETH_ALEN);
+	asix_write_cmd_async(dev, AX_CMD_WRITE_NODE_ID, 0, 0, ETH_ALEN,
+							data->mac_addr);
+
+	return 0;
+}
diff --git a/drivers/net/usb/asix.c b/drivers/net/usb/asix_devices.c
index 3ae80eccd0e..4fd48df6b98 100644
--- a/drivers/net/usb/asix.c
+++ b/drivers/net/usb/asix_devices.c
@@ -20,137 +20,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
-// #define	DEBUG			// error path messages, extra info
-// #define	VERBOSE			// more; success messages
-
-#include <linux/module.h>
-#include <linux/kmod.h>
-#include <linux/init.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/ethtool.h>
-#include <linux/workqueue.h>
-#include <linux/mii.h>
-#include <linux/usb.h>
-#include <linux/crc32.h>
-#include <linux/usb/usbnet.h>
-#include <linux/slab.h>
-#include <linux/if_vlan.h>
-
-#define DRIVER_VERSION "22-Dec-2011"
-#define DRIVER_NAME "asix"
-
-/* ASIX AX8817X based USB 2.0 Ethernet Devices */
-
-#define AX_CMD_SET_SW_MII		0x06
-#define AX_CMD_READ_MII_REG		0x07
-#define AX_CMD_WRITE_MII_REG		0x08
-#define AX_CMD_SET_HW_MII		0x0a
-#define AX_CMD_READ_EEPROM		0x0b
-#define AX_CMD_WRITE_EEPROM		0x0c
-#define AX_CMD_WRITE_ENABLE		0x0d
-#define AX_CMD_WRITE_DISABLE		0x0e
-#define AX_CMD_READ_RX_CTL		0x0f
-#define AX_CMD_WRITE_RX_CTL		0x10
-#define AX_CMD_READ_IPG012		0x11
-#define AX_CMD_WRITE_IPG0		0x12
-#define AX_CMD_WRITE_IPG1		0x13
-#define AX_CMD_READ_NODE_ID		0x13
-#define AX_CMD_WRITE_NODE_ID		0x14
-#define AX_CMD_WRITE_IPG2		0x14
-#define AX_CMD_WRITE_MULTI_FILTER	0x16
-#define AX88172_CMD_READ_NODE_ID	0x17
-#define AX_CMD_READ_PHY_ID		0x19
-#define AX_CMD_READ_MEDIUM_STATUS	0x1a
-#define AX_CMD_WRITE_MEDIUM_MODE	0x1b
-#define AX_CMD_READ_MONITOR_MODE	0x1c
-#define AX_CMD_WRITE_MONITOR_MODE	0x1d
-#define AX_CMD_READ_GPIOS		0x1e
-#define AX_CMD_WRITE_GPIOS		0x1f
-#define AX_CMD_SW_RESET			0x20
-#define AX_CMD_SW_PHY_STATUS		0x21
-#define AX_CMD_SW_PHY_SELECT		0x22
-
-#define AX_MONITOR_MODE			0x01
-#define AX_MONITOR_LINK			0x02
-#define AX_MONITOR_MAGIC		0x04
-#define AX_MONITOR_HSFS			0x10
-
-/* AX88172 Medium Status Register values */
-#define AX88172_MEDIUM_FD		0x02
-#define AX88172_MEDIUM_TX		0x04
-#define AX88172_MEDIUM_FC		0x10
-#define AX88172_MEDIUM_DEFAULT \
-		( AX88172_MEDIUM_FD | AX88172_MEDIUM_TX | AX88172_MEDIUM_FC )
-
-#define AX_MCAST_FILTER_SIZE		8
-#define AX_MAX_MCAST			64
-
-#define AX_SWRESET_CLEAR		0x00
-#define AX_SWRESET_RR			0x01
-#define AX_SWRESET_RT			0x02
-#define AX_SWRESET_PRTE			0x04
-#define AX_SWRESET_PRL			0x08
-#define AX_SWRESET_BZ			0x10
-#define AX_SWRESET_IPRL			0x20
-#define AX_SWRESET_IPPD			0x40
-
-#define AX88772_IPG0_DEFAULT		0x15
-#define AX88772_IPG1_DEFAULT		0x0c
-#define AX88772_IPG2_DEFAULT		0x12
-
-/* AX88772 & AX88178 Medium Mode Register */
-#define AX_MEDIUM_PF		0x0080
-#define AX_MEDIUM_JFE		0x0040
-#define AX_MEDIUM_TFC		0x0020
-#define AX_MEDIUM_RFC		0x0010
-#define AX_MEDIUM_ENCK		0x0008
-#define AX_MEDIUM_AC		0x0004
-#define AX_MEDIUM_FD		0x0002
-#define AX_MEDIUM_GM		0x0001
-#define AX_MEDIUM_SM		0x1000
-#define AX_MEDIUM_SBP		0x0800
-#define AX_MEDIUM_PS		0x0200
-#define AX_MEDIUM_RE		0x0100
-
-#define AX88178_MEDIUM_DEFAULT	\
-	(AX_MEDIUM_PS | AX_MEDIUM_FD | AX_MEDIUM_AC | \
-	 AX_MEDIUM_RFC | AX_MEDIUM_TFC | AX_MEDIUM_JFE | \
-	 AX_MEDIUM_RE)
-
-#define AX88772_MEDIUM_DEFAULT	\
-	(AX_MEDIUM_FD | AX_MEDIUM_RFC | \
-	 AX_MEDIUM_TFC | AX_MEDIUM_PS | \
-	 AX_MEDIUM_AC | AX_MEDIUM_RE)
-
-/* AX88772 & AX88178 RX_CTL values */
-#define AX_RX_CTL_SO		0x0080
-#define AX_RX_CTL_AP		0x0020
-#define AX_RX_CTL_AM		0x0010
-#define AX_RX_CTL_AB		0x0008
-#define AX_RX_CTL_SEP		0x0004
-#define AX_RX_CTL_AMALL		0x0002
-#define AX_RX_CTL_PRO		0x0001
-#define AX_RX_CTL_MFB_2048	0x0000
-#define AX_RX_CTL_MFB_4096	0x0100
-#define AX_RX_CTL_MFB_8192	0x0200
-#define AX_RX_CTL_MFB_16384	0x0300
-
-#define AX_DEFAULT_RX_CTL	(AX_RX_CTL_SO | AX_RX_CTL_AB)
-
-/* GPIO 0 .. 2 toggles */
-#define AX_GPIO_GPO0EN		0x01	/* GPIO0 Output enable */
-#define AX_GPIO_GPO_0		0x02	/* GPIO0 Output value */
-#define AX_GPIO_GPO1EN		0x04	/* GPIO1 Output enable */
-#define AX_GPIO_GPO_1		0x08	/* GPIO1 Output value */
-#define AX_GPIO_GPO2EN		0x10	/* GPIO2 Output enable */
-#define AX_GPIO_GPO_2		0x20	/* GPIO2 Output value */
-#define AX_GPIO_RESERVED	0x40	/* Reserved */
-#define AX_GPIO_RSE		0x80	/* Reload serial EEPROM */
-
-#define AX_EEPROM_MAGIC		0xdeadbeef
-#define AX88172_EEPROM_LEN	0x40
-#define AX88772_EEPROM_LEN	0xff
+#include "asix.h"
 
 #define PHY_MODE_MARVELL	0x0000
 #define MII_MARVELL_LED_CTRL	0x0018
@@ -166,15 +36,6 @@
 
 #define	PHY_MODE_RTL8211CL	0x000C
 
-/* This structure cannot exceed sizeof(unsigned long [5]) AKA 20 bytes */
-struct asix_data {
-	u8 multi_filter[AX_MCAST_FILTER_SIZE];
-	u8 mac_addr[ETH_ALEN];
-	u8 phymode;
-	u8 ledmode;
-	u8 eeprom_len;
-};
-
 struct ax88172_int_data {
 	__le16 res1;
 	u8 link;
@@ -183,209 +44,6 @@ struct ax88172_int_data {
 	__le16 res3;
 } __packed;
 
-static int asix_read_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
-			    u16 size, void *data)
-{
-	void *buf;
-	int err = -ENOMEM;
-
-	netdev_dbg(dev->net, "asix_read_cmd() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
-		   cmd, value, index, size);
-
-	buf = kmalloc(size, GFP_KERNEL);
-	if (!buf)
-		goto out;
-
-	err = usb_control_msg(
-		dev->udev,
-		usb_rcvctrlpipe(dev->udev, 0),
-		cmd,
-		USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		value,
-		index,
-		buf,
-		size,
-		USB_CTRL_GET_TIMEOUT);
-	if (err == size)
-		memcpy(data, buf, size);
-	else if (err >= 0)
-		err = -EINVAL;
-	kfree(buf);
-
-out:
-	return err;
-}
-
-static int asix_write_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
-			     u16 size, void *data)
-{
-	void *buf = NULL;
-	int err = -ENOMEM;
-
-	netdev_dbg(dev->net, "asix_write_cmd() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
-		   cmd, value, index, size);
-
-	if (data) {
-		buf = kmemdup(data, size, GFP_KERNEL);
-		if (!buf)
-			goto out;
-	}
-
-	err = usb_control_msg(
-		dev->udev,
-		usb_sndctrlpipe(dev->udev, 0),
-		cmd,
-		USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-		value,
-		index,
-		buf,
-		size,
-		USB_CTRL_SET_TIMEOUT);
-	kfree(buf);
-
-out:
-	return err;
-}
-
-static void asix_async_cmd_callback(struct urb *urb)
-{
-	struct usb_ctrlrequest *req = (struct usb_ctrlrequest *)urb->context;
-	int status = urb->status;
-
-	if (status < 0)
-		printk(KERN_DEBUG "asix_async_cmd_callback() failed with %d",
-			status);
-
-	kfree(req);
-	usb_free_urb(urb);
-}
-
-static void
-asix_write_cmd_async(struct usbnet *dev, u8 cmd, u16 value, u16 index,
-				    u16 size, void *data)
-{
-	struct usb_ctrlrequest *req;
-	int status;
-	struct urb *urb;
-
-	netdev_dbg(dev->net, "asix_write_cmd_async() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
-		   cmd, value, index, size);
-
-	urb = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!urb) {
-		netdev_err(dev->net, "Error allocating URB in write_cmd_async!\n");
-		return;
-	}
-
-	req = kmalloc(sizeof(struct usb_ctrlrequest), GFP_ATOMIC);
-	if (!req) {
-		netdev_err(dev->net, "Failed to allocate memory for control request\n");
-		usb_free_urb(urb);
-		return;
-	}
-
-	req->bRequestType = USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
-	req->bRequest = cmd;
-	req->wValue = cpu_to_le16(value);
-	req->wIndex = cpu_to_le16(index);
-	req->wLength = cpu_to_le16(size);
-
-	usb_fill_control_urb(urb, dev->udev,
-			     usb_sndctrlpipe(dev->udev, 0),
-			     (void *)req, data, size,
-			     asix_async_cmd_callback, req);
-
-	status = usb_submit_urb(urb, GFP_ATOMIC);
-	if (status < 0) {
-		netdev_err(dev->net, "Error submitting the control message: status=%d\n",
-			   status);
-		kfree(req);
-		usb_free_urb(urb);
-	}
-}
-
-static int asix_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
-{
-	int offset = 0;
-
-	while (offset + sizeof(u32) < skb->len) {
-		struct sk_buff *ax_skb;
-		u16 size;
-		u32 header = get_unaligned_le32(skb->data + offset);
-
-		offset += sizeof(u32);
-
-		/* get the packet length */
-		size = (u16) (header & 0x7ff);
-		if (size != ((~header >> 16) & 0x07ff)) {
-			netdev_err(dev->net, "asix_rx_fixup() Bad Header Length\n");
-			return 0;
-		}
-
-		if ((size > dev->net->mtu + ETH_HLEN + VLAN_HLEN) ||
-		    (size + offset > skb->len)) {
-			netdev_err(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
-				   size);
-			return 0;
-		}
-		ax_skb = netdev_alloc_skb_ip_align(dev->net, size);
-		if (!ax_skb)
-			return 0;
-
-		skb_put(ax_skb, size);
-		memcpy(ax_skb->data, skb->data + offset, size);
-		usbnet_skb_return(dev, ax_skb);
-
-		offset += (size + 1) & 0xfffe;
-	}
-
-	if (skb->len != offset) {
-		netdev_err(dev->net, "asix_rx_fixup() Bad SKB Length %d\n",
-			   skb->len);
-		return 0;
-	}
-	return 1;
-}
-
-static struct sk_buff *asix_tx_fixup(struct usbnet *dev, struct sk_buff *skb,
-					gfp_t flags)
-{
-	int padlen;
-	int headroom = skb_headroom(skb);
-	int tailroom = skb_tailroom(skb);
-	u32 packet_len;
-	u32 padbytes = 0xffff0000;
-
-	padlen = ((skb->len + 4) & (dev->maxpacket - 1)) ? 0 : 4;
-
-	if ((!skb_cloned(skb)) &&
-	    ((headroom + tailroom) >= (4 + padlen))) {
-		if ((headroom < 4) || (tailroom < padlen)) {
-			skb->data = memmove(skb->head + 4, skb->data, skb->len);
-			skb_set_tail_pointer(skb, skb->len);
-		}
-	} else {
-		struct sk_buff *skb2;
-		skb2 = skb_copy_expand(skb, 4, padlen, flags);
-		dev_kfree_skb_any(skb);
-		skb = skb2;
-		if (!skb)
-			return NULL;
-	}
-
-	skb_push(skb, 4);
-	packet_len = (((skb->len - 4) ^ 0x0000ffff) << 16) + (skb->len - 4);
-	cpu_to_le32s(&packet_len);
-	skb_copy_to_linear_data(skb, &packet_len, sizeof(packet_len));
-
-	if (padlen) {
-		cpu_to_le32s(&padbytes);
-		memcpy(skb_tail_pointer(skb), &padbytes, sizeof(padbytes));
-		skb_put(skb, sizeof(padbytes));
-	}
-	return skb;
-}
-
 static void asix_status(struct usbnet *dev, struct urb *urb)
 {
 	struct ax88172_int_data *event;
@@ -406,200 +64,6 @@ static void asix_status(struct usbnet *dev, struct urb *urb)
 	}
 }
 
-static inline int asix_set_sw_mii(struct usbnet *dev)
-{
-	int ret;
-	ret = asix_write_cmd(dev, AX_CMD_SET_SW_MII, 0x0000, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to enable software MII access\n");
-	return ret;
-}
-
-static inline int asix_set_hw_mii(struct usbnet *dev)
-{
-	int ret;
-	ret = asix_write_cmd(dev, AX_CMD_SET_HW_MII, 0x0000, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to enable hardware MII access\n");
-	return ret;
-}
-
-static inline int asix_get_phy_addr(struct usbnet *dev)
-{
-	u8 buf[2];
-	int ret = asix_read_cmd(dev, AX_CMD_READ_PHY_ID, 0, 0, 2, buf);
-
-	netdev_dbg(dev->net, "asix_get_phy_addr()\n");
-
-	if (ret < 0) {
-		netdev_err(dev->net, "Error reading PHYID register: %02x\n", ret);
-		goto out;
-	}
-	netdev_dbg(dev->net, "asix_get_phy_addr() returning 0x%04x\n",
-		   *((__le16 *)buf));
-	ret = buf[1];
-
-out:
-	return ret;
-}
-
-static int asix_sw_reset(struct usbnet *dev, u8 flags)
-{
-	int ret;
-
-        ret = asix_write_cmd(dev, AX_CMD_SW_RESET, flags, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to send software reset: %02x\n", ret);
-
-	return ret;
-}
-
-static u16 asix_read_rx_ctl(struct usbnet *dev)
-{
-	__le16 v;
-	int ret = asix_read_cmd(dev, AX_CMD_READ_RX_CTL, 0, 0, 2, &v);
-
-	if (ret < 0) {
-		netdev_err(dev->net, "Error reading RX_CTL register: %02x\n", ret);
-		goto out;
-	}
-	ret = le16_to_cpu(v);
-out:
-	return ret;
-}
-
-static int asix_write_rx_ctl(struct usbnet *dev, u16 mode)
-{
-	int ret;
-
-	netdev_dbg(dev->net, "asix_write_rx_ctl() - mode = 0x%04x\n", mode);
-	ret = asix_write_cmd(dev, AX_CMD_WRITE_RX_CTL, mode, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to write RX_CTL mode to 0x%04x: %02x\n",
-			   mode, ret);
-
-	return ret;
-}
-
-static u16 asix_read_medium_status(struct usbnet *dev)
-{
-	__le16 v;
-	int ret = asix_read_cmd(dev, AX_CMD_READ_MEDIUM_STATUS, 0, 0, 2, &v);
-
-	if (ret < 0) {
-		netdev_err(dev->net, "Error reading Medium Status register: %02x\n",
-			   ret);
-		return ret;	/* TODO: callers not checking for error ret */
-	}
-
-	return le16_to_cpu(v);
-
-}
-
-static int asix_write_medium_mode(struct usbnet *dev, u16 mode)
-{
-	int ret;
-
-	netdev_dbg(dev->net, "asix_write_medium_mode() - mode = 0x%04x\n", mode);
-	ret = asix_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, mode, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to write Medium Mode mode to 0x%04x: %02x\n",
-			   mode, ret);
-
-	return ret;
-}
-
-static int asix_write_gpio(struct usbnet *dev, u16 value, int sleep)
-{
-	int ret;
-
-	netdev_dbg(dev->net, "asix_write_gpio() - value = 0x%04x\n", value);
-	ret = asix_write_cmd(dev, AX_CMD_WRITE_GPIOS, value, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to write GPIO value 0x%04x: %02x\n",
-			   value, ret);
-
-	if (sleep)
-		msleep(sleep);
-
-	return ret;
-}
-
-/*
- * AX88772 & AX88178 have a 16-bit RX_CTL value
- */
-static void asix_set_multicast(struct net_device *net)
-{
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	u16 rx_ctl = AX_DEFAULT_RX_CTL;
-
-	if (net->flags & IFF_PROMISC) {
-		rx_ctl |= AX_RX_CTL_PRO;
-	} else if (net->flags & IFF_ALLMULTI ||
-		   netdev_mc_count(net) > AX_MAX_MCAST) {
-		rx_ctl |= AX_RX_CTL_AMALL;
-	} else if (netdev_mc_empty(net)) {
-		/* just broadcast and directed */
-	} else {
-		/* We use the 20 byte dev->data
-		 * for our 8 byte filter buffer
-		 * to avoid allocating memory that
-		 * is tricky to free later */
-		struct netdev_hw_addr *ha;
-		u32 crc_bits;
-
-		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
-
-		/* Build the multicast hash filter. */
-		netdev_for_each_mc_addr(ha, net) {
-			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
-			data->multi_filter[crc_bits >> 3] |=
-			    1 << (crc_bits & 7);
-		}
-
-		asix_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
-				   AX_MCAST_FILTER_SIZE, data->multi_filter);
-
-		rx_ctl |= AX_RX_CTL_AM;
-	}
-
-	asix_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
-}
-
-static int asix_mdio_read(struct net_device *netdev, int phy_id, int loc)
-{
-	struct usbnet *dev = netdev_priv(netdev);
-	__le16 res;
-
-	mutex_lock(&dev->phy_mutex);
-	asix_set_sw_mii(dev);
-	asix_read_cmd(dev, AX_CMD_READ_MII_REG, phy_id,
-				(__u16)loc, 2, &res);
-	asix_set_hw_mii(dev);
-	mutex_unlock(&dev->phy_mutex);
-
-	netdev_dbg(dev->net, "asix_mdio_read() phy_id=0x%02x, loc=0x%02x, returns=0x%04x\n",
-		   phy_id, loc, le16_to_cpu(res));
-
-	return le16_to_cpu(res);
-}
-
-static void
-asix_mdio_write(struct net_device *netdev, int phy_id, int loc, int val)
-{
-	struct usbnet *dev = netdev_priv(netdev);
-	__le16 res = cpu_to_le16(val);
-
-	netdev_dbg(dev->net, "asix_mdio_write() phy_id=0x%02x, loc=0x%02x, val=0x%04x\n",
-		   phy_id, loc, val);
-	mutex_lock(&dev->phy_mutex);
-	asix_set_sw_mii(dev);
-	asix_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id, (__u16)loc, 2, &res);
-	asix_set_hw_mii(dev);
-	mutex_unlock(&dev->phy_mutex);
-}
-
 /* Get the PHY Identifier from the PHYSID1 & PHYSID2 MII registers */
 static u32 asix_get_phyid(struct usbnet *dev)
 {
@@ -629,88 +93,6 @@ static u32 asix_get_phyid(struct usbnet *dev)
 	return phy_id;
 }
 
-static void
-asix_get_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
-{
-	struct usbnet *dev = netdev_priv(net);
-	u8 opt;
-
-	if (asix_read_cmd(dev, AX_CMD_READ_MONITOR_MODE, 0, 0, 1, &opt) < 0) {
-		wolinfo->supported = 0;
-		wolinfo->wolopts = 0;
-		return;
-	}
-	wolinfo->supported = WAKE_PHY | WAKE_MAGIC;
-	wolinfo->wolopts = 0;
-	if (opt & AX_MONITOR_LINK)
-		wolinfo->wolopts |= WAKE_PHY;
-	if (opt & AX_MONITOR_MAGIC)
-		wolinfo->wolopts |= WAKE_MAGIC;
-}
-
-static int
-asix_set_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
-{
-	struct usbnet *dev = netdev_priv(net);
-	u8 opt = 0;
-
-	if (wolinfo->wolopts & WAKE_PHY)
-		opt |= AX_MONITOR_LINK;
-	if (wolinfo->wolopts & WAKE_MAGIC)
-		opt |= AX_MONITOR_MAGIC;
-
-	if (asix_write_cmd(dev, AX_CMD_WRITE_MONITOR_MODE,
-			      opt, 0, 0, NULL) < 0)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int asix_get_eeprom_len(struct net_device *net)
-{
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-
-	return data->eeprom_len;
-}
-
-static int asix_get_eeprom(struct net_device *net,
-			      struct ethtool_eeprom *eeprom, u8 *data)
-{
-	struct usbnet *dev = netdev_priv(net);
-	__le16 *ebuf = (__le16 *)data;
-	int i;
-
-	/* Crude hack to ensure that we don't overwrite memory
-	 * if an odd length is supplied
-	 */
-	if (eeprom->len % 2)
-		return -EINVAL;
-
-	eeprom->magic = AX_EEPROM_MAGIC;
-
-	/* ax8817x returns 2 bytes from eeprom on read */
-	for (i=0; i < eeprom->len / 2; i++) {
-		if (asix_read_cmd(dev, AX_CMD_READ_EEPROM,
-			eeprom->offset + i, 0, 2, &ebuf[i]) < 0)
-			return -EINVAL;
-	}
-	return 0;
-}
-
-static void asix_get_drvinfo (struct net_device *net,
-				 struct ethtool_drvinfo *info)
-{
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-
-	/* Inherit standard device info */
-	usbnet_get_drvinfo(net, info);
-	strncpy (info->driver, DRIVER_NAME, sizeof info->driver);
-	strncpy (info->version, DRIVER_VERSION, sizeof info->version);
-	info->eedump_len = data->eeprom_len;
-}
-
 static u32 asix_get_link(struct net_device *net)
 {
 	struct usbnet *dev = netdev_priv(net);
@@ -725,30 +107,6 @@ static int asix_ioctl (struct net_device *net, struct ifreq *rq, int cmd)
 	return generic_mii_ioctl(&dev->mii, if_mii(rq), cmd, NULL);
 }
 
-static int asix_set_mac_address(struct net_device *net, void *p)
-{
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	struct sockaddr *addr = p;
-
-	if (netif_running(net))
-		return -EBUSY;
-	if (!is_valid_ether_addr(addr->sa_data))
-		return -EADDRNOTAVAIL;
-
-	memcpy(net->dev_addr, addr->sa_data, ETH_ALEN);
-
-	/* We use the 20 byte dev->data
-	 * for our 6 byte mac buffer
-	 * to avoid allocating memory that
-	 * is tricky to free later */
-	memcpy(data->mac_addr, addr->sa_data, ETH_ALEN);
-	asix_write_cmd_async(dev, AX_CMD_WRITE_NODE_ID, 0, 0, ETH_ALEN,
-							data->mac_addr);
-
-	return 0;
-}
-
 /* We need to override some ethtool_ops so we require our
    own structure so we don't interfere with other usbnet
    devices that may be connected at the same time. */
@@ -761,6 +119,7 @@ static const struct ethtool_ops ax88172_ethtool_ops = {
 	.set_wol		= asix_set_wol,
 	.get_eeprom_len		= asix_get_eeprom_len,
 	.get_eeprom		= asix_get_eeprom,
+	.set_eeprom		= asix_set_eeprom,
 	.get_settings		= usbnet_get_settings,
 	.set_settings		= usbnet_set_settings,
 	.nway_reset		= usbnet_nway_reset,
@@ -843,9 +202,6 @@ static int ax88172_bind(struct usbnet *dev, struct usb_interface *intf)
 	u8 buf[ETH_ALEN];
 	int i;
 	unsigned long gpio_bits = dev->driver_info->data;
-	struct asix_data *data = (struct asix_data *)&dev->data;
-
-	data->eeprom_len = AX88172_EEPROM_LEN;
 
 	usbnet_get_endpoints(dev,intf);
 
@@ -880,6 +236,8 @@ static int ax88172_bind(struct usbnet *dev, struct usb_interface *intf)
 
 	dev->net->netdev_ops = &ax88172_netdev_ops;
 	dev->net->ethtool_ops = &ax88172_ethtool_ops;
+	dev->net->needed_headroom = 4; /* cf asix_tx_fixup() */
+	dev->net->needed_tailroom = 4; /* cf asix_tx_fixup() */
 
 	asix_mdio_write(dev->net, dev->mii.phy_id, MII_BMCR, BMCR_RESET);
 	asix_mdio_write(dev->net, dev->mii.phy_id, MII_ADVERTISE,
@@ -901,6 +259,7 @@ static const struct ethtool_ops ax88772_ethtool_ops = {
 	.set_wol		= asix_set_wol,
 	.get_eeprom_len		= asix_get_eeprom_len,
 	.get_eeprom		= asix_get_eeprom,
+	.set_eeprom		= asix_set_eeprom,
 	.get_settings		= usbnet_get_settings,
 	.set_settings		= usbnet_set_settings,
 	.nway_reset		= usbnet_nway_reset,
@@ -1049,12 +408,9 @@ static const struct net_device_ops ax88772_netdev_ops = {
 static int ax88772_bind(struct usbnet *dev, struct usb_interface *intf)
 {
 	int ret, embd_phy;
-	struct asix_data *data = (struct asix_data *)&dev->data;
 	u8 buf[ETH_ALEN];
 	u32 phyid;
 
-	data->eeprom_len = AX88772_EEPROM_LEN;
-
 	usbnet_get_endpoints(dev,intf);
 
 	/* Get the MAC address */
@@ -1075,6 +431,8 @@ static int ax88772_bind(struct usbnet *dev, struct usb_interface *intf)
 
 	dev->net->netdev_ops = &ax88772_netdev_ops;
 	dev->net->ethtool_ops = &ax88772_ethtool_ops;
+	dev->net->needed_headroom = 4; /* cf asix_tx_fixup() */
+	dev->net->needed_tailroom = 4; /* cf asix_tx_fixup() */
 
 	embd_phy = ((dev->mii.phy_id & 0x1f) == 0x10 ? 1 : 0);
 
@@ -1122,6 +480,7 @@ static const struct ethtool_ops ax88178_ethtool_ops = {
 	.set_wol		= asix_set_wol,
 	.get_eeprom_len		= asix_get_eeprom_len,
 	.get_eeprom		= asix_get_eeprom,
+	.set_eeprom		= asix_set_eeprom,
 	.get_settings		= usbnet_get_settings,
 	.set_settings		= usbnet_set_settings,
 	.nway_reset		= usbnet_nway_reset,
@@ -1405,9 +764,6 @@ static int ax88178_bind(struct usbnet *dev, struct usb_interface *intf)
 {
 	int ret;
 	u8 buf[ETH_ALEN];
-	struct asix_data *data = (struct asix_data *)&dev->data;
-
-	data->eeprom_len = AX88772_EEPROM_LEN;
 
 	usbnet_get_endpoints(dev,intf);
 
@@ -1510,6 +866,8 @@ static const struct driver_info ax88178_info = {
 	.tx_fixup = asix_tx_fixup,
 };
 
+extern const struct driver_info ax88172a_info;
+
 static const struct usb_device_id	products [] = {
 {
 	// Linksys USB200M
@@ -1635,6 +993,10 @@ static const struct usb_device_id	products [] = {
 	// Asus USB Ethernet Adapter
 	USB_DEVICE (0x0b95, 0x7e2b),
 	.driver_info = (unsigned long) &ax88772_info,
+}, {
+	/* ASIX 88172a demo board */
+	USB_DEVICE(0x0b95, 0x172a),
+	.driver_info = (unsigned long) &ax88172a_info,
 },
 	{ },		// END
 };
diff --git a/drivers/net/usb/ax88172a.c b/drivers/net/usb/ax88172a.c
new file mode 100644
index 00000000000..c8e0aa85fb8
--- /dev/null
+++ b/drivers/net/usb/ax88172a.c
@@ -0,0 +1,414 @@
+/*
+ * ASIX AX88172A based USB 2.0 Ethernet Devices
+ * Copyright (C) 2012 OMICRON electronics GmbH
+ *
+ * Supports external PHYs via phylib. Based on the driver for the
+ * AX88772. Original copyrights follow:
+ *
+ * Copyright (C) 2003-2006 David Hollis <dhollis@davehollis.com>
+ * Copyright (C) 2005 Phil Chang <pchang23@sbcglobal.net>
+ * Copyright (C) 2006 James Painter <jamie.painter@iname.com>
+ * Copyright (c) 2002-2003 TiVo 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include "asix.h"
+#include <linux/phy.h>
+
+struct ax88172a_private {
+	struct mii_bus *mdio;
+	struct phy_device *phydev;
+	char phy_name[20];
+	u16 phy_addr;
+	u16 oldmode;
+	int use_embdphy;
+};
+
+/* MDIO read and write wrappers for phylib */
+static int asix_mdio_bus_read(struct mii_bus *bus, int phy_id, int regnum)
+{
+	return asix_mdio_read(((struct usbnet *)bus->priv)->net, phy_id,
+			      regnum);
+}
+
+static int asix_mdio_bus_write(struct mii_bus *bus, int phy_id, int regnum,
+			       u16 val)
+{
+	asix_mdio_write(((struct usbnet *)bus->priv)->net, phy_id, regnum, val);
+	return 0;
+}
+
+static int ax88172a_ioctl(struct net_device *net, struct ifreq *rq, int cmd)
+{
+	if (!netif_running(net))
+		return -EINVAL;
+
+	if (!net->phydev)
+		return -ENODEV;
+
+	return phy_mii_ioctl(net->phydev, rq, cmd);
+}
+
+/* set MAC link settings according to information from phylib */
+static void ax88172a_adjust_link(struct net_device *netdev)
+{
+	struct phy_device *phydev = netdev->phydev;
+	struct usbnet *dev = netdev_priv(netdev);
+	struct ax88172a_private *priv = dev->driver_priv;
+	u16 mode = 0;
+
+	if (phydev->link) {
+		mode = AX88772_MEDIUM_DEFAULT;
+
+		if (phydev->duplex == DUPLEX_HALF)
+			mode &= ~AX_MEDIUM_FD;
+
+		if (phydev->speed != SPEED_100)
+			mode &= ~AX_MEDIUM_PS;
+	}
+
+	if (mode != priv->oldmode) {
+		asix_write_medium_mode(dev, mode);
+		priv->oldmode = mode;
+		netdev_dbg(netdev, "speed %u duplex %d, setting mode to 0x%04x\n",
+			   phydev->speed, phydev->duplex, mode);
+		phy_print_status(phydev);
+	}
+}
+
+static void ax88172a_status(struct usbnet *dev, struct urb *urb)
+{
+	/* link changes are detected by polling the phy */
+}
+
+/* use phylib infrastructure */
+static int ax88172a_init_mdio(struct usbnet *dev)
+{
+	struct ax88172a_private *priv = dev->driver_priv;
+	int ret, i;
+
+	priv->mdio = mdiobus_alloc();
+	if (!priv->mdio) {
+		netdev_err(dev->net, "Could not allocate MDIO bus\n");
+		return -ENOMEM;
+	}
+
+	priv->mdio->priv = (void *)dev;
+	priv->mdio->read = &asix_mdio_bus_read;
+	priv->mdio->write = &asix_mdio_bus_write;
+	priv->mdio->name = "Asix MDIO Bus";
+	/* mii bus name is usb-<usb bus number>-<usb device number> */
+	snprintf(priv->mdio->id, MII_BUS_ID_SIZE, "usb-%03d:%03d",
+		 dev->udev->bus->busnum, dev->udev->devnum);
+
+	priv->mdio->irq = kzalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
+	if (!priv->mdio->irq) {
+		netdev_err(dev->net, "Could not allocate mdio->irq\n");
+		ret = -ENOMEM;
+		goto mfree;
+	}
+	for (i = 0; i < PHY_MAX_ADDR; i++)
+		priv->mdio->irq[i] = PHY_POLL;
+
+	ret = mdiobus_register(priv->mdio);
+	if (ret) {
+		netdev_err(dev->net, "Could not register MDIO bus\n");
+		goto ifree;
+	}
+
+	netdev_info(dev->net, "registered mdio bus %s\n", priv->mdio->id);
+	return 0;
+
+ifree:
+	kfree(priv->mdio->irq);
+mfree:
+	mdiobus_free(priv->mdio);
+	return ret;
+}
+
+static void ax88172a_remove_mdio(struct usbnet *dev)
+{
+	struct ax88172a_private *priv = dev->driver_priv;
+
+	netdev_info(dev->net, "deregistering mdio bus %s\n", priv->mdio->id);
+	mdiobus_unregister(priv->mdio);
+	kfree(priv->mdio->irq);
+	mdiobus_free(priv->mdio);
+}
+
+static const struct net_device_ops ax88172a_netdev_ops = {
+	.ndo_open		= usbnet_open,
+	.ndo_stop		= usbnet_stop,
+	.ndo_start_xmit		= usbnet_start_xmit,
+	.ndo_tx_timeout		= usbnet_tx_timeout,
+	.ndo_change_mtu		= usbnet_change_mtu,
+	.ndo_set_mac_address	= asix_set_mac_address,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_do_ioctl		= ax88172a_ioctl,
+	.ndo_set_rx_mode        = asix_set_multicast,
+};
+
+int ax88172a_get_settings(struct net_device *net, struct ethtool_cmd *cmd)
+{
+	if (!net->phydev)
+		return -ENODEV;
+
+	return phy_ethtool_gset(net->phydev, cmd);
+}
+
+int ax88172a_set_settings(struct net_device *net, struct ethtool_cmd *cmd)
+{
+	if (!net->phydev)
+		return -ENODEV;
+
+	return phy_ethtool_sset(net->phydev, cmd);
+}
+
+int ax88172a_nway_reset(struct net_device *net)
+{
+	if (!net->phydev)
+		return -ENODEV;
+
+	return phy_start_aneg(net->phydev);
+}
+
+static const struct ethtool_ops ax88172a_ethtool_ops = {
+	.get_drvinfo		= asix_get_drvinfo,
+	.get_link		= usbnet_get_link,
+	.get_msglevel		= usbnet_get_msglevel,
+	.set_msglevel		= usbnet_set_msglevel,
+	.get_wol		= asix_get_wol,
+	.set_wol		= asix_set_wol,
+	.get_eeprom_len		= asix_get_eeprom_len,
+	.get_eeprom		= asix_get_eeprom,
+	.set_eeprom		= asix_set_eeprom,
+	.get_settings		= ax88172a_get_settings,
+	.set_settings		= ax88172a_set_settings,
+	.nway_reset		= ax88172a_nway_reset,
+};
+
+static int ax88172a_reset_phy(struct usbnet *dev, int embd_phy)
+{
+	int ret;
+
+	ret = asix_sw_reset(dev, AX_SWRESET_IPPD);
+	if (ret < 0)
+		goto err;
+
+	msleep(150);
+	ret = asix_sw_reset(dev, AX_SWRESET_CLEAR);
+	if (ret < 0)
+		goto err;
+
+	msleep(150);
+
+	ret = asix_sw_reset(dev, embd_phy ? AX_SWRESET_IPRL : AX_SWRESET_IPPD);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	return ret;
+}
+
+
+static int ax88172a_bind(struct usbnet *dev, struct usb_interface *intf)
+{
+	int ret;
+	u8 buf[ETH_ALEN];
+	struct ax88172a_private *priv;
+
+	usbnet_get_endpoints(dev, intf);
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		netdev_err(dev->net, "Could not allocate memory for private data\n");
+		return -ENOMEM;
+	}
+	dev->driver_priv = priv;
+
+	/* Get the MAC address */
+	ret = asix_read_cmd(dev, AX_CMD_READ_NODE_ID, 0, 0, ETH_ALEN, buf);
+	if (ret < 0) {
+		netdev_err(dev->net, "Failed to read MAC address: %d\n", ret);
+		goto free;
+	}
+	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
+
+	dev->net->netdev_ops = &ax88172a_netdev_ops;
+	dev->net->ethtool_ops = &ax88172a_ethtool_ops;
+
+	/* are we using the internal or the external phy? */
+	ret = asix_read_cmd(dev, AX_CMD_SW_PHY_STATUS, 0, 0, 1, buf);
+	if (ret < 0) {
+		netdev_err(dev->net, "Failed to read software interface selection register: %d\n",
+			   ret);
+		goto free;
+	}
+
+	netdev_dbg(dev->net, "AX_CMD_SW_PHY_STATUS = 0x%02x\n", buf[0]);
+	switch (buf[0] & AX_PHY_SELECT_MASK) {
+	case AX_PHY_SELECT_INTERNAL:
+		netdev_dbg(dev->net, "use internal phy\n");
+		priv->use_embdphy = 1;
+		break;
+	case AX_PHY_SELECT_EXTERNAL:
+		netdev_dbg(dev->net, "use external phy\n");
+		priv->use_embdphy = 0;
+		break;
+	default:
+		netdev_err(dev->net, "Interface mode not supported by driver\n");
+		ret = -ENOTSUPP;
+		goto free;
+	}
+
+	priv->phy_addr = asix_read_phy_addr(dev, priv->use_embdphy);
+	ax88172a_reset_phy(dev, priv->use_embdphy);
+
+	/* Asix framing packs multiple eth frames into a 2K usb bulk transfer */
+	if (dev->driver_info->flags & FLAG_FRAMING_AX) {
+		/* hard_mtu  is still the default - the device does not support
+		   jumbo eth frames */
+		dev->rx_urb_size = 2048;
+	}
+
+	/* init MDIO bus */
+	ret = ax88172a_init_mdio(dev);
+	if (ret)
+		goto free;
+
+	return 0;
+
+free:
+	kfree(priv);
+	return ret;
+}
+
+static int ax88172a_stop(struct usbnet *dev)
+{
+	struct ax88172a_private *priv = dev->driver_priv;
+
+	netdev_dbg(dev->net, "Stopping interface\n");
+
+	if (priv->phydev) {
+		netdev_info(dev->net, "Disconnecting from phy %s\n",
+			    priv->phy_name);
+		phy_stop(priv->phydev);
+		phy_disconnect(priv->phydev);
+	}
+
+	return 0;
+}
+
+static void ax88172a_unbind(struct usbnet *dev, struct usb_interface *intf)
+{
+	struct ax88172a_private *priv = dev->driver_priv;
+
+	ax88172a_remove_mdio(dev);
+	kfree(priv);
+}
+
+static int ax88172a_reset(struct usbnet *dev)
+{
+	struct asix_data *data = (struct asix_data *)&dev->data;
+	struct ax88172a_private *priv = dev->driver_priv;
+	int ret;
+	u16 rx_ctl;
+
+	ax88172a_reset_phy(dev, priv->use_embdphy);
+
+	msleep(150);
+	rx_ctl = asix_read_rx_ctl(dev);
+	netdev_dbg(dev->net, "RX_CTL is 0x%04x after software reset\n", rx_ctl);
+	ret = asix_write_rx_ctl(dev, 0x0000);
+	if (ret < 0)
+		goto out;
+
+	rx_ctl = asix_read_rx_ctl(dev);
+	netdev_dbg(dev->net, "RX_CTL is 0x%04x setting to 0x0000\n", rx_ctl);
+
+	msleep(150);
+
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_IPG0,
+			     AX88772_IPG0_DEFAULT | AX88772_IPG1_DEFAULT,
+			     AX88772_IPG2_DEFAULT, 0, NULL);
+	if (ret < 0) {
+		netdev_err(dev->net, "Write IPG,IPG1,IPG2 failed: %d\n", ret);
+		goto out;
+	}
+
+	/* Rewrite MAC address */
+	memcpy(data->mac_addr, dev->net->dev_addr, ETH_ALEN);
+	ret = asix_write_cmd(dev, AX_CMD_WRITE_NODE_ID, 0, 0, ETH_ALEN,
+			     data->mac_addr);
+	if (ret < 0)
+		goto out;
+
+	/* Set RX_CTL to default values with 2k buffer, and enable cactus */
+	ret = asix_write_rx_ctl(dev, AX_DEFAULT_RX_CTL);
+	if (ret < 0)
+		goto out;
+
+	rx_ctl = asix_read_rx_ctl(dev);
+	netdev_dbg(dev->net, "RX_CTL is 0x%04x after all initializations\n",
+		   rx_ctl);
+
+	rx_ctl = asix_read_medium_status(dev);
+	netdev_dbg(dev->net, "Medium Status is 0x%04x after all initializations\n",
+		   rx_ctl);
+
+	/* Connect to PHY */
+	snprintf(priv->phy_name, 20, PHY_ID_FMT,
+		 priv->mdio->id, priv->phy_addr);
+
+	priv->phydev = phy_connect(dev->net, priv->phy_name,
+				   &ax88172a_adjust_link,
+				   0, PHY_INTERFACE_MODE_MII);
+	if (IS_ERR(priv->phydev)) {
+		netdev_err(dev->net, "Could not connect to PHY device %s\n",
+			   priv->phy_name);
+		ret = PTR_ERR(priv->phydev);
+		goto out;
+	}
+
+	netdev_info(dev->net, "Connected to phy %s\n", priv->phy_name);
+
+	/* During power-up, the AX88172A set the power down (BMCR_PDOWN)
+	 * bit of the PHY. Bring the PHY up again.
+	 */
+	genphy_resume(priv->phydev);
+	phy_start(priv->phydev);
+
+	return 0;
+
+out:
+	return ret;
+
+}
+
+const struct driver_info ax88172a_info = {
+	.description = "ASIX AX88172A USB 2.0 Ethernet",
+	.bind = ax88172a_bind,
+	.reset = ax88172a_reset,
+	.stop = ax88172a_stop,
+	.unbind = ax88172a_unbind,
+	.status = ax88172a_status,
+	.flags = FLAG_ETHER | FLAG_FRAMING_AX | FLAG_LINK_INTR |
+		 FLAG_MULTI_PACKET,
+	.rx_fixup = asix_rx_fixup,
+	.tx_fixup = asix_tx_fixup,
+};
diff --git a/drivers/net/usb/cdc-phonet.c b/drivers/net/usb/cdc-phonet.c
index d848d4dd575..64610048ce8 100644
--- a/drivers/net/usb/cdc-phonet.c
+++ b/drivers/net/usb/cdc-phonet.c
@@ -130,7 +130,7 @@ static int rx_submit(struct usbpn_dev *pnd, struct urb *req, gfp_t gfp_flags)
 	struct page *page;
 	int err;
 
-	page = alloc_page(gfp_flags);
+	page = __skb_alloc_page(gfp_flags | __GFP_NOMEMALLOC, NULL);
 	if (!page)
 		return -ENOMEM;
 
@@ -394,7 +394,7 @@ int usbpn_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	SET_NETDEV_DEV(dev, &intf->dev);
 
 	pnd->dev = dev;
-	pnd->usb = usb_get_dev(usbdev);
+	pnd->usb = usbdev;
 	pnd->intf = intf;
 	pnd->data_intf = data_intf;
 	spin_lock_init(&pnd->tx_lock);
@@ -440,7 +440,6 @@ out:
 static void usbpn_disconnect(struct usb_interface *intf)
 {
 	struct usbpn_dev *pnd = usb_get_intfdata(intf);
-	struct usb_device *usb = pnd->usb;
 
 	if (pnd->disconnected)
 		return;
@@ -449,7 +448,6 @@ static void usbpn_disconnect(struct usb_interface *intf)
 	usb_driver_release_interface(&usbpn_driver,
 			(pnd->intf == intf) ? pnd->data_intf : pnd->intf);
 	unregister_netdev(pnd->dev);
-	usb_put_dev(usb);
 }
 
 static struct usb_driver usbpn_driver = {
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index 4b9513fcf27..f4ce5957df3 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -138,20 +138,7 @@ struct cdc_ncm_ctx {
 static void cdc_ncm_txpath_bh(unsigned long param);
 static void cdc_ncm_tx_timeout_start(struct cdc_ncm_ctx *ctx);
 static enum hrtimer_restart cdc_ncm_tx_timer_cb(struct hrtimer *hr_timer);
-static const struct driver_info cdc_ncm_info;
 static struct usb_driver cdc_ncm_driver;
-static const struct ethtool_ops cdc_ncm_ethtool_ops;
-
-static const struct usb_device_id cdc_devs[] = {
-	{ USB_INTERFACE_INFO(USB_CLASS_COMM,
-		USB_CDC_SUBCLASS_NCM, USB_CDC_PROTO_NONE),
-		.driver_info = (unsigned long)&cdc_ncm_info,
-	},
-	{
-	},
-};
-
-MODULE_DEVICE_TABLE(usb, cdc_devs);
 
 static void
 cdc_ncm_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *info)
@@ -454,6 +441,16 @@ static void cdc_ncm_free(struct cdc_ncm_ctx *ctx)
 	kfree(ctx);
 }
 
+static const struct ethtool_ops cdc_ncm_ethtool_ops = {
+	.get_drvinfo = cdc_ncm_get_drvinfo,
+	.get_link = usbnet_get_link,
+	.get_msglevel = usbnet_get_msglevel,
+	.set_msglevel = usbnet_set_msglevel,
+	.get_settings = usbnet_get_settings,
+	.set_settings = usbnet_set_settings,
+	.nway_reset = usbnet_nway_reset,
+};
+
 static int cdc_ncm_bind(struct usbnet *dev, struct usb_interface *intf)
 {
 	struct cdc_ncm_ctx *ctx;
@@ -1203,6 +1200,41 @@ static const struct driver_info cdc_ncm_info = {
 	.tx_fixup = cdc_ncm_tx_fixup,
 };
 
+/* Same as cdc_ncm_info, but with FLAG_WWAN */
+static const struct driver_info wwan_info = {
+	.description = "Mobile Broadband Network Device",
+	.flags = FLAG_POINTTOPOINT | FLAG_NO_SETINT | FLAG_MULTI_PACKET
+			| FLAG_WWAN,
+	.bind = cdc_ncm_bind,
+	.unbind = cdc_ncm_unbind,
+	.check_connect = cdc_ncm_check_connect,
+	.manage_power = cdc_ncm_manage_power,
+	.status = cdc_ncm_status,
+	.rx_fixup = cdc_ncm_rx_fixup,
+	.tx_fixup = cdc_ncm_tx_fixup,
+};
+
+static const struct usb_device_id cdc_devs[] = {
+	/* Ericsson MBM devices like F5521gw */
+	{ .match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+		| USB_DEVICE_ID_MATCH_VENDOR,
+	  .idVendor = 0x0bdb,
+	  .bInterfaceClass = USB_CLASS_COMM,
+	  .bInterfaceSubClass = USB_CDC_SUBCLASS_NCM,
+	  .bInterfaceProtocol = USB_CDC_PROTO_NONE,
+	  .driver_info = (unsigned long) &wwan_info,
+	},
+
+	/* Generic CDC-NCM devices */
+	{ USB_INTERFACE_INFO(USB_CLASS_COMM,
+		USB_CDC_SUBCLASS_NCM, USB_CDC_PROTO_NONE),
+		.driver_info = (unsigned long)&cdc_ncm_info,
+	},
+	{
+	},
+};
+MODULE_DEVICE_TABLE(usb, cdc_devs);
+
 static struct usb_driver cdc_ncm_driver = {
 	.name = "cdc_ncm",
 	.id_table = cdc_devs,
@@ -1215,16 +1247,6 @@ static struct usb_driver cdc_ncm_driver = {
 	.disable_hub_initiated_lpm = 1,
 };
 
-static const struct ethtool_ops cdc_ncm_ethtool_ops = {
-	.get_drvinfo = cdc_ncm_get_drvinfo,
-	.get_link = usbnet_get_link,
-	.get_msglevel = usbnet_get_msglevel,
-	.set_msglevel = usbnet_set_msglevel,
-	.get_settings = usbnet_get_settings,
-	.set_settings = usbnet_set_settings,
-	.nway_reset = usbnet_nway_reset,
-};
-
 module_usb_driver(cdc_ncm_driver);
 
 MODULE_AUTHOR("Hans Petter Selasky");
diff --git a/drivers/net/usb/kaweth.c b/drivers/net/usb/kaweth.c
index d8ad5528438..c3d03490c97 100644
--- a/drivers/net/usb/kaweth.c
+++ b/drivers/net/usb/kaweth.c
@@ -1314,7 +1314,7 @@ static int kaweth_internal_control_msg(struct usb_device *usb_dev,
         int retv;
         int length = 0; /* shut up GCC */
 
-        urb = usb_alloc_urb(0, GFP_NOIO);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
         if (!urb)
                 return -ENOMEM;
 
diff --git a/drivers/net/usb/pegasus.c b/drivers/net/usb/pegasus.c
index 7023220456c..a0b5807b30d 100644
--- a/drivers/net/usb/pegasus.c
+++ b/drivers/net/usb/pegasus.c
@@ -1329,8 +1329,6 @@ static int pegasus_probe(struct usb_interface *intf,
 	}
 	pegasus_count++;
 
-	usb_get_dev(dev);
-
 	net = alloc_etherdev(sizeof(struct pegasus));
 	if (!net)
 		goto out;
@@ -1407,7 +1405,6 @@ out2:
 out1:
 	free_netdev(net);
 out:
-	usb_put_dev(dev);
 	pegasus_dec_workqueue();
 	return res;
 }
@@ -1425,7 +1422,6 @@ static void pegasus_disconnect(struct usb_interface *intf)
 	pegasus->flags |= PEGASUS_UNPLUG;
 	cancel_delayed_work(&pegasus->carrier_check);
 	unregister_netdev(pegasus->net);
-	usb_put_dev(interface_to_usbdev(intf));
 	unlink_all_urbs(pegasus);
 	free_all_urbs(pegasus);
 	free_skb_pool(pegasus);
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index b01960fcfbc..2ea126a16d7 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -1,6 +1,10 @@
 /*
  * Copyright (c) 2012  Bjørn Mork <bjorn@mork.no>
  *
+ * The probing code is heavily inspired by cdc_ether, which is:
+ * Copyright (C) 2003-2005 by David Brownell
+ * Copyright (C) 2006 by Ole Andre Vadla Ravnas (ActiveSync)
+ *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
  * version 2 as published by the Free Software Foundation.
@@ -15,11 +19,7 @@
 #include <linux/usb/usbnet.h>
 #include <linux/usb/cdc-wdm.h>
 
-/* The name of the CDC Device Management driver */
-#define DM_DRIVER "cdc_wdm"
-
-/*
- * This driver supports wwan (3G/LTE/?) devices using a vendor
+/* This driver supports wwan (3G/LTE/?) devices using a vendor
  * specific management protocol called Qualcomm MSM Interface (QMI) -
  * in addition to the more common AT commands over serial interface
  * management
@@ -31,59 +31,117 @@
  * management protocol is used in place of the standard CDC
  * notifications NOTIFY_NETWORK_CONNECTION and NOTIFY_SPEED_CHANGE
  *
+ * Alternatively, control and data functions can be combined in a
+ * single USB interface.
+ *
  * Handling a protocol like QMI is out of the scope for any driver.
- * It can be exported as a character device using the cdc-wdm driver,
- * which will enable userspace applications ("modem managers") to
- * handle it.  This may be required to use the network interface
- * provided by the driver.
+ * It is exported as a character device using the cdc-wdm driver as
+ * a subdriver, enabling userspace applications ("modem managers") to
+ * handle it.
  *
  * These devices may alternatively/additionally be configured using AT
- * commands on any of the serial interfaces driven by the option driver
- *
- * This driver binds only to the data ("slave") interface to enable
- * the cdc-wdm driver to bind to the control interface.  It still
- * parses the CDC functional descriptors on the control interface to
- *  a) verify that this is indeed a handled interface (CDC Union
- *     header lists it as slave)
- *  b) get MAC address and other ethernet config from the CDC Ethernet
- *     header
- *  c) enable user bind requests against the control interface, which
- *     is the common way to bind to CDC Ethernet Control Model type
- *     interfaces
- *  d) provide a hint to the user about which interface is the
- *     corresponding management interface
+ * commands on a serial interface
  */
 
+/* driver specific data */
+struct qmi_wwan_state {
+	struct usb_driver *subdriver;
+	atomic_t pmcount;
+	unsigned long unused;
+	struct usb_interface *control;
+	struct usb_interface *data;
+};
+
+/* using a counter to merge subdriver requests with our own into a combined state */
+static int qmi_wwan_manage_power(struct usbnet *dev, int on)
+{
+	struct qmi_wwan_state *info = (void *)&dev->data;
+	int rv = 0;
+
+	dev_dbg(&dev->intf->dev, "%s() pmcount=%d, on=%d\n", __func__, atomic_read(&info->pmcount), on);
+
+	if ((on && atomic_add_return(1, &info->pmcount) == 1) || (!on && atomic_dec_and_test(&info->pmcount))) {
+		/* need autopm_get/put here to ensure the usbcore sees the new value */
+		rv = usb_autopm_get_interface(dev->intf);
+		if (rv < 0)
+			goto err;
+		dev->intf->needs_remote_wakeup = on;
+		usb_autopm_put_interface(dev->intf);
+	}
+err:
+	return rv;
+}
+
+static int qmi_wwan_cdc_wdm_manage_power(struct usb_interface *intf, int on)
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+
+	/* can be called while disconnecting */
+	if (!dev)
+		return 0;
+	return qmi_wwan_manage_power(dev, on);
+}
+
+/* collect all three endpoints and register subdriver */
+static int qmi_wwan_register_subdriver(struct usbnet *dev)
+{
+	int rv;
+	struct usb_driver *subdriver = NULL;
+	struct qmi_wwan_state *info = (void *)&dev->data;
+
+	/* collect bulk endpoints */
+	rv = usbnet_get_endpoints(dev, info->data);
+	if (rv < 0)
+		goto err;
+
+	/* update status endpoint if separate control interface */
+	if (info->control != info->data)
+		dev->status = &info->control->cur_altsetting->endpoint[0];
+
+	/* require interrupt endpoint for subdriver */
+	if (!dev->status) {
+		rv = -EINVAL;
+		goto err;
+	}
+
+	/* for subdriver power management */
+	atomic_set(&info->pmcount, 0);
+
+	/* register subdriver */
+	subdriver = usb_cdc_wdm_register(info->control, &dev->status->desc, 512, &qmi_wwan_cdc_wdm_manage_power);
+	if (IS_ERR(subdriver)) {
+		dev_err(&info->control->dev, "subdriver registration failed\n");
+		rv = PTR_ERR(subdriver);
+		goto err;
+	}
+
+	/* prevent usbnet from using status endpoint */
+	dev->status = NULL;
+
+	/* save subdriver struct for suspend/resume wrappers */
+	info->subdriver = subdriver;
+
+err:
+	return rv;
+}
+
 static int qmi_wwan_bind(struct usbnet *dev, struct usb_interface *intf)
 {
 	int status = -1;
-	struct usb_interface *control = NULL;
 	u8 *buf = intf->cur_altsetting->extra;
 	int len = intf->cur_altsetting->extralen;
 	struct usb_interface_descriptor *desc = &intf->cur_altsetting->desc;
 	struct usb_cdc_union_desc *cdc_union = NULL;
 	struct usb_cdc_ether_desc *cdc_ether = NULL;
-	u32 required = 1 << USB_CDC_HEADER_TYPE | 1 << USB_CDC_UNION_TYPE;
 	u32 found = 0;
-	atomic_t *pmcount = (void *)&dev->data[1];
-
-	atomic_set(pmcount, 0);
+	struct usb_driver *driver = driver_of(intf);
+	struct qmi_wwan_state *info = (void *)&dev->data;
 
-	/*
-	 * assume a data interface has no additional descriptors and
-	 * that the control and data interface are numbered
-	 * consecutively - this holds for the Huawei device at least
-	 */
-	if (len == 0 && desc->bInterfaceNumber > 0) {
-		control = usb_ifnum_to_if(dev->udev, desc->bInterfaceNumber - 1);
-		if (!control)
-			goto err;
+	BUILD_BUG_ON((sizeof(((struct usbnet *)0)->data) < sizeof(struct qmi_wwan_state)));
 
-		buf = control->cur_altsetting->extra;
-		len = control->cur_altsetting->extralen;
-		dev_dbg(&intf->dev, "guessing \"control\" => %s, \"data\" => this\n",
-			dev_name(&control->dev));
-	}
+	/* require a single interrupt status endpoint for subdriver */
+	if (intf->cur_altsetting->desc.bNumEndpoints != 1)
+		goto err;
 
 	while (len > 3) {
 		struct usb_descriptor_header *h = (void *)buf;
@@ -142,15 +200,23 @@ next_desc:
 	}
 
 	/* did we find all the required ones? */
-	if ((found & required) != required) {
+	if (!(found & (1 << USB_CDC_HEADER_TYPE)) ||
+	    !(found & (1 << USB_CDC_UNION_TYPE))) {
 		dev_err(&intf->dev, "CDC functional descriptors missing\n");
 		goto err;
 	}
 
-	/* give the user a helpful hint if trying to bind to the wrong interface */
-	if (cdc_union && desc->bInterfaceNumber == cdc_union->bMasterInterface0) {
-		dev_err(&intf->dev, "leaving \"control\" interface for " DM_DRIVER " - try binding to %s instead!\n",
-			dev_name(&usb_ifnum_to_if(dev->udev, cdc_union->bSlaveInterface0)->dev));
+	/* verify CDC Union */
+	if (desc->bInterfaceNumber != cdc_union->bMasterInterface0) {
+		dev_err(&intf->dev, "bogus CDC Union: master=%u\n", cdc_union->bMasterInterface0);
+		goto err;
+	}
+
+	/* need to save these for unbind */
+	info->control = intf;
+	info->data = usb_ifnum_to_if(dev->udev,	cdc_union->bSlaveInterface0);
+	if (!info->data) {
+		dev_err(&intf->dev, "bogus CDC Union: slave=%u\n", cdc_union->bSlaveInterface0);
 		goto err;
 	}
 
@@ -160,63 +226,29 @@ next_desc:
 		usbnet_get_ethernet_addr(dev, cdc_ether->iMACAddress);
 	}
 
-	/* success! point the user to the management interface */
-	if (control)
-		dev_info(&intf->dev, "Use \"" DM_DRIVER "\" for QMI interface %s\n",
-			dev_name(&control->dev));
-
-	/* XXX: add a sysfs symlink somewhere to help management applications find it? */
+	/* claim data interface and set it up */
+	status = usb_driver_claim_interface(driver, info->data, dev);
+	if (status < 0)
+		goto err;
 
-	/* collect bulk endpoints now that we know intf == "data" interface */
-	status = usbnet_get_endpoints(dev, intf);
+	status = qmi_wwan_register_subdriver(dev);
+	if (status < 0) {
+		usb_set_intfdata(info->data, NULL);
+		usb_driver_release_interface(driver, info->data);
+	}
 
 err:
 	return status;
 }
 
-/* using a counter to merge subdriver requests with our own into a combined state */
-static int qmi_wwan_manage_power(struct usbnet *dev, int on)
-{
-	atomic_t *pmcount = (void *)&dev->data[1];
-	int rv = 0;
-
-	dev_dbg(&dev->intf->dev, "%s() pmcount=%d, on=%d\n", __func__, atomic_read(pmcount), on);
-
-	if ((on && atomic_add_return(1, pmcount) == 1) || (!on && atomic_dec_and_test(pmcount))) {
-		/* need autopm_get/put here to ensure the usbcore sees the new value */
-		rv = usb_autopm_get_interface(dev->intf);
-		if (rv < 0)
-			goto err;
-		dev->intf->needs_remote_wakeup = on;
-		usb_autopm_put_interface(dev->intf);
-	}
-err:
-	return rv;
-}
-
-static int qmi_wwan_cdc_wdm_manage_power(struct usb_interface *intf, int on)
-{
-	struct usbnet *dev = usb_get_intfdata(intf);
-
-	/* can be called while disconnecting */
-	if (!dev)
-		return 0;
-	return qmi_wwan_manage_power(dev, on);
-}
-
 /* Some devices combine the "control" and "data" functions into a
  * single interface with all three endpoints: interrupt + bulk in and
  * out
- *
- * Setting up cdc-wdm as a subdriver owning the interrupt endpoint
- * will let it provide userspace access to the encapsulated QMI
- * protocol without interfering with the usbnet operations.
-  */
+ */
 static int qmi_wwan_bind_shared(struct usbnet *dev, struct usb_interface *intf)
 {
 	int rv;
-	struct usb_driver *subdriver = NULL;
-	atomic_t *pmcount = (void *)&dev->data[1];
+	struct qmi_wwan_state *info = (void *)&dev->data;
 
 	/* ZTE makes devices where the interface descriptors and endpoint
 	 * configurations of two or more interfaces are identical, even
@@ -232,43 +264,39 @@ static int qmi_wwan_bind_shared(struct usbnet *dev, struct usb_interface *intf)
 		goto err;
 	}
 
-	atomic_set(pmcount, 0);
-
-	/* collect all three endpoints */
-	rv = usbnet_get_endpoints(dev, intf);
-	if (rv < 0)
-		goto err;
-
-	/* require interrupt endpoint for subdriver */
-	if (!dev->status) {
-		rv = -EINVAL;
-		goto err;
-	}
-
-	subdriver = usb_cdc_wdm_register(intf, &dev->status->desc, 512, &qmi_wwan_cdc_wdm_manage_power);
-	if (IS_ERR(subdriver)) {
-		rv = PTR_ERR(subdriver);
-		goto err;
-	}
-
-	/* can't let usbnet use the interrupt endpoint */
-	dev->status = NULL;
-
-	/* save subdriver struct for suspend/resume wrappers */
-	dev->data[0] = (unsigned long)subdriver;
+	/*  control and data is shared */
+	info->control = intf;
+	info->data = intf;
+	rv = qmi_wwan_register_subdriver(dev);
 
 err:
 	return rv;
 }
 
-static void qmi_wwan_unbind_shared(struct usbnet *dev, struct usb_interface *intf)
+static void qmi_wwan_unbind(struct usbnet *dev, struct usb_interface *intf)
 {
-	struct usb_driver *subdriver = (void *)dev->data[0];
-
-	if (subdriver && subdriver->disconnect)
-		subdriver->disconnect(intf);
+	struct qmi_wwan_state *info = (void *)&dev->data;
+	struct usb_driver *driver = driver_of(intf);
+	struct usb_interface *other;
+
+	if (info->subdriver && info->subdriver->disconnect)
+		info->subdriver->disconnect(info->control);
+
+	/* allow user to unbind using either control or data */
+	if (intf == info->control)
+		other = info->data;
+	else
+		other = info->control;
+
+	/* only if not shared */
+	if (other && intf != other) {
+		usb_set_intfdata(other, NULL);
+		usb_driver_release_interface(driver, other);
+	}
 
-	dev->data[0] = (unsigned long)NULL;
+	info->subdriver = NULL;
+	info->data = NULL;
+	info->control = NULL;
 }
 
 /* suspend/resume wrappers calling both usbnet and the cdc-wdm
@@ -280,15 +308,15 @@ static void qmi_wwan_unbind_shared(struct usbnet *dev, struct usb_interface *int
 static int qmi_wwan_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct usbnet *dev = usb_get_intfdata(intf);
-	struct usb_driver *subdriver = (void *)dev->data[0];
+	struct qmi_wwan_state *info = (void *)&dev->data;
 	int ret;
 
 	ret = usbnet_suspend(intf, message);
 	if (ret < 0)
 		goto err;
 
-	if (subdriver && subdriver->suspend)
-		ret = subdriver->suspend(intf, message);
+	if (info->subdriver && info->subdriver->suspend)
+		ret = info->subdriver->suspend(intf, message);
 	if (ret < 0)
 		usbnet_resume(intf);
 err:
@@ -298,33 +326,33 @@ err:
 static int qmi_wwan_resume(struct usb_interface *intf)
 {
 	struct usbnet *dev = usb_get_intfdata(intf);
-	struct usb_driver *subdriver = (void *)dev->data[0];
+	struct qmi_wwan_state *info = (void *)&dev->data;
 	int ret = 0;
 
-	if (subdriver && subdriver->resume)
-		ret = subdriver->resume(intf);
+	if (info->subdriver && info->subdriver->resume)
+		ret = info->subdriver->resume(intf);
 	if (ret < 0)
 		goto err;
 	ret = usbnet_resume(intf);
-	if (ret < 0 && subdriver && subdriver->resume && subdriver->suspend)
-		subdriver->suspend(intf, PMSG_SUSPEND);
+	if (ret < 0 && info->subdriver && info->subdriver->resume && info->subdriver->suspend)
+		info->subdriver->suspend(intf, PMSG_SUSPEND);
 err:
 	return ret;
 }
 
-
 static const struct driver_info	qmi_wwan_info = {
-	.description	= "QMI speaking wwan device",
+	.description	= "WWAN/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 };
 
 static const struct driver_info	qmi_wwan_shared = {
-	.description	= "QMI speaking wwan device with combined interface",
+	.description	= "WWAN/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind_shared,
-	.unbind		= qmi_wwan_unbind_shared,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 };
 
@@ -332,7 +360,7 @@ static const struct driver_info	qmi_wwan_force_int0 = {
 	.description	= "Qualcomm WWAN/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind_shared,
-	.unbind		= qmi_wwan_unbind_shared,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 	.data		= BIT(0), /* interface whitelist bitmap */
 };
@@ -341,16 +369,25 @@ static const struct driver_info	qmi_wwan_force_int1 = {
 	.description	= "Qualcomm WWAN/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind_shared,
-	.unbind		= qmi_wwan_unbind_shared,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 	.data		= BIT(1), /* interface whitelist bitmap */
 };
 
+static const struct driver_info qmi_wwan_force_int2 = {
+	.description	= "Qualcomm WWAN/QMI device",
+	.flags		= FLAG_WWAN,
+	.bind		= qmi_wwan_bind_shared,
+	.unbind		= qmi_wwan_unbind,
+	.manage_power	= qmi_wwan_manage_power,
+	.data		= BIT(2), /* interface whitelist bitmap */
+};
+
 static const struct driver_info	qmi_wwan_force_int3 = {
 	.description	= "Qualcomm WWAN/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind_shared,
-	.unbind		= qmi_wwan_unbind_shared,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 	.data		= BIT(3), /* interface whitelist bitmap */
 };
@@ -359,7 +396,7 @@ static const struct driver_info	qmi_wwan_force_int4 = {
 	.description	= "Qualcomm WWAN/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind_shared,
-	.unbind		= qmi_wwan_unbind_shared,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 	.data		= BIT(4), /* interface whitelist bitmap */
 };
@@ -381,7 +418,7 @@ static const struct driver_info	qmi_wwan_sierra = {
 	.description	= "Sierra Wireless wwan/QMI device",
 	.flags		= FLAG_WWAN,
 	.bind		= qmi_wwan_bind_shared,
-	.unbind		= qmi_wwan_unbind_shared,
+	.unbind		= qmi_wwan_unbind,
 	.manage_power	= qmi_wwan_manage_power,
 	.data		= BIT(8) | BIT(19), /* interface whitelist bitmap */
 };
@@ -404,7 +441,7 @@ static const struct usb_device_id products[] = {
 		.idVendor           = HUAWEI_VENDOR_ID,
 		.bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
 		.bInterfaceSubClass = 1,
-		.bInterfaceProtocol = 8, /* NOTE: This is the *slave* interface of the CDC Union! */
+		.bInterfaceProtocol = 9, /* CDC Ethernet *control* interface */
 		.driver_info        = (unsigned long)&qmi_wwan_info,
 	},
 	{	/* Vodafone/Huawei K5005 (12d1:14c8) and similar modems */
@@ -412,7 +449,7 @@ static const struct usb_device_id products[] = {
 		.idVendor           = HUAWEI_VENDOR_ID,
 		.bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
 		.bInterfaceSubClass = 1,
-		.bInterfaceProtocol = 56, /* NOTE: This is the *slave* interface of the CDC Union! */
+		.bInterfaceProtocol = 57, /* CDC Ethernet *control* interface */
 		.driver_info        = (unsigned long)&qmi_wwan_info,
 	},
 	{	/* Huawei E392, E398 and possibly others in "Windows mode"
@@ -444,6 +481,15 @@ static const struct usb_device_id products[] = {
 		.bInterfaceProtocol = 0xff,
 		.driver_info        = (unsigned long)&qmi_wwan_force_int4,
 	},
+	{	/* ZTE MF821D */
+		.match_flags        = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
+		.idVendor           = 0x19d2,
+		.idProduct          = 0x0326,
+		.bInterfaceClass    = 0xff,
+		.bInterfaceSubClass = 0xff,
+		.bInterfaceProtocol = 0xff,
+		.driver_info        = (unsigned long)&qmi_wwan_force_int4,
+	},
 	{	/* ZTE (Vodafone) K3520-Z */
 		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
 		.idVendor           = 0x19d2,
@@ -498,6 +544,15 @@ static const struct usb_device_id products[] = {
 		.bInterfaceProtocol = 0xff,
 		.driver_info        = (unsigned long)&qmi_wwan_force_int4,
 	},
+	{	/* ZTE MF60 */
+		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
+		.idVendor           = 0x19d2,
+		.idProduct          = 0x1402,
+		.bInterfaceClass    = 0xff,
+		.bInterfaceSubClass = 0xff,
+		.bInterfaceProtocol = 0xff,
+		.driver_info        = (unsigned long)&qmi_wwan_force_int2,
+	},
 	{	/* Sierra Wireless MC77xx in QMI mode */
 		.match_flags	    = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO,
 		.idVendor           = 0x1199,
@@ -554,10 +609,27 @@ static const struct usb_device_id products[] = {
 };
 MODULE_DEVICE_TABLE(usb, products);
 
+static int qmi_wwan_probe(struct usb_interface *intf, const struct usb_device_id *prod)
+{
+	struct usb_device_id *id = (struct usb_device_id *)prod;
+
+	/* Workaround to enable dynamic IDs.  This disables usbnet
+	 * blacklisting functionality.  Which, if required, can be
+	 * reimplemented here by using a magic "blacklist" value
+	 * instead of 0 in the static device id table
+	 */
+	if (!id->driver_info) {
+		dev_dbg(&intf->dev, "setting defaults for dynamic device id\n");
+		id->driver_info = (unsigned long)&qmi_wwan_shared;
+	}
+
+	return usbnet_probe(intf, id);
+}
+
 static struct usb_driver qmi_wwan_driver = {
 	.name		      = "qmi_wwan",
 	.id_table	      = products,
-	.probe		      =	usbnet_probe,
+	.probe		      = qmi_wwan_probe,
 	.disconnect	      = usbnet_disconnect,
 	.suspend	      = qmi_wwan_suspend,
 	.resume		      =	qmi_wwan_resume,
@@ -566,17 +638,7 @@ static struct usb_driver qmi_wwan_driver = {
 	.disable_hub_initiated_lpm = 1,
 };
 
-static int __init qmi_wwan_init(void)
-{
-	return usb_register(&qmi_wwan_driver);
-}
-module_init(qmi_wwan_init);
-
-static void __exit qmi_wwan_exit(void)
-{
-	usb_deregister(&qmi_wwan_driver);
-}
-module_exit(qmi_wwan_exit);
+module_usb_driver(qmi_wwan_driver);
 
 MODULE_AUTHOR("Bjørn Mork <bjorn@mork.no>");
 MODULE_DESCRIPTION("Qualcomm MSM Interface (QMI) WWAN driver");
diff --git a/drivers/net/usb/smsc75xx.c b/drivers/net/usb/smsc75xx.c
index 1c6e51588da..f5ab6e613ec 100644
--- a/drivers/net/usb/smsc75xx.c
+++ b/drivers/net/usb/smsc75xx.c
@@ -616,7 +616,7 @@ static void smsc75xx_init_mac_address(struct usbnet *dev)
 
 	/* no eeprom, or eeprom values are invalid. generate random MAC */
 	eth_hw_addr_random(dev->net);
-	netif_dbg(dev, ifup, dev->net, "MAC address set to random_ether_addr");
+	netif_dbg(dev, ifup, dev->net, "MAC address set to eth_random_addr");
 }
 
 static int smsc75xx_set_mac_address(struct usbnet *dev)
@@ -1260,6 +1260,6 @@ static struct usb_driver smsc75xx_driver = {
 module_usb_driver(smsc75xx_driver);
 
 MODULE_AUTHOR("Nancy Lin");
-MODULE_AUTHOR("Steve Glendinning <steve.glendinning@smsc.com>");
+MODULE_AUTHOR("Steve Glendinning <steve.glendinning@shawell.net>");
 MODULE_DESCRIPTION("SMSC75XX USB 2.0 Gigabit Ethernet Devices");
 MODULE_LICENSE("GPL");
diff --git a/drivers/net/usb/smsc95xx.c b/drivers/net/usb/smsc95xx.c
index b1112e75385..d45e539a84b 100644
--- a/drivers/net/usb/smsc95xx.c
+++ b/drivers/net/usb/smsc95xx.c
@@ -578,6 +578,36 @@ static int smsc95xx_ethtool_set_eeprom(struct net_device *netdev,
 	return smsc95xx_write_eeprom(dev, ee->offset, ee->len, data);
 }
 
+static int smsc95xx_ethtool_getregslen(struct net_device *netdev)
+{
+	/* all smsc95xx registers */
+	return COE_CR - ID_REV + 1;
+}
+
+static void
+smsc95xx_ethtool_getregs(struct net_device *netdev, struct ethtool_regs *regs,
+			 void *buf)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	unsigned int i, j;
+	int retval;
+	u32 *data = buf;
+
+	retval = smsc95xx_read_reg(dev, ID_REV, &regs->version);
+	if (retval < 0) {
+		netdev_warn(netdev, "REGS: cannot read ID_REV\n");
+		return;
+	}
+
+	for (i = ID_REV, j = 0; i <= COE_CR; i += (sizeof(u32)), j++) {
+		retval = smsc95xx_read_reg(dev, i, &data[j]);
+		if (retval < 0) {
+			netdev_warn(netdev, "REGS: cannot read reg[%x]\n", i);
+			return;
+		}
+	}
+}
+
 static const struct ethtool_ops smsc95xx_ethtool_ops = {
 	.get_link	= usbnet_get_link,
 	.nway_reset	= usbnet_nway_reset,
@@ -589,6 +619,8 @@ static const struct ethtool_ops smsc95xx_ethtool_ops = {
 	.get_eeprom_len	= smsc95xx_ethtool_get_eeprom_len,
 	.get_eeprom	= smsc95xx_ethtool_get_eeprom,
 	.set_eeprom	= smsc95xx_ethtool_set_eeprom,
+	.get_regs_len	= smsc95xx_ethtool_getregslen,
+	.get_regs	= smsc95xx_ethtool_getregs,
 };
 
 static int smsc95xx_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)
@@ -615,7 +647,7 @@ static void smsc95xx_init_mac_address(struct usbnet *dev)
 
 	/* no eeprom, or eeprom values are invalid. generate random MAC */
 	eth_hw_addr_random(dev->net);
-	netif_dbg(dev, ifup, dev->net, "MAC address set to random_ether_addr\n");
+	netif_dbg(dev, ifup, dev->net, "MAC address set to eth_random_addr\n");
 }
 
 static int smsc95xx_set_mac_address(struct usbnet *dev)
@@ -1303,6 +1335,6 @@ static struct usb_driver smsc95xx_driver = {
 module_usb_driver(smsc95xx_driver);
 
 MODULE_AUTHOR("Nancy Lin");
-MODULE_AUTHOR("Steve Glendinning <steve.glendinning@smsc.com>");
+MODULE_AUTHOR("Steve Glendinning <steve.glendinning@shawell.net>");
 MODULE_DESCRIPTION("SMSC95XX USB 2.0 Ethernet Devices");
 MODULE_LICENSE("GPL");
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index aba769d7745..8531c1caac2 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -180,7 +180,40 @@ int usbnet_get_ethernet_addr(struct usbnet *dev, int iMACAddress)
 }
 EXPORT_SYMBOL_GPL(usbnet_get_ethernet_addr);
 
-static void intr_complete (struct urb *urb);
+static void intr_complete (struct urb *urb)
+{
+	struct usbnet	*dev = urb->context;
+	int		status = urb->status;
+
+	switch (status) {
+	/* success */
+	case 0:
+		dev->driver_info->status(dev, urb);
+		break;
+
+	/* software-driven interface shutdown */
+	case -ENOENT:		/* urb killed */
+	case -ESHUTDOWN:	/* hardware gone */
+		netif_dbg(dev, ifdown, dev->net,
+			  "intr shutdown, code %d\n", status);
+		return;
+
+	/* NOTE:  not throttling like RX/TX, since this endpoint
+	 * already polls infrequently
+	 */
+	default:
+		netdev_dbg(dev->net, "intr status %d\n", status);
+		break;
+	}
+
+	if (!netif_running (dev->net))
+		return;
+
+	status = usb_submit_urb (urb, GFP_ATOMIC);
+	if (status != 0)
+		netif_err(dev, timer, dev->net,
+			  "intr resubmit --> %d\n", status);
+}
 
 static int init_status (struct usbnet *dev, struct usb_interface *intf)
 {
@@ -519,42 +552,6 @@ block:
 	netif_dbg(dev, rx_err, dev->net, "no read resubmitted\n");
 }
 
-static void intr_complete (struct urb *urb)
-{
-	struct usbnet	*dev = urb->context;
-	int		status = urb->status;
-
-	switch (status) {
-	/* success */
-	case 0:
-		dev->driver_info->status(dev, urb);
-		break;
-
-	/* software-driven interface shutdown */
-	case -ENOENT:		/* urb killed */
-	case -ESHUTDOWN:	/* hardware gone */
-		netif_dbg(dev, ifdown, dev->net,
-			  "intr shutdown, code %d\n", status);
-		return;
-
-	/* NOTE:  not throttling like RX/TX, since this endpoint
-	 * already polls infrequently
-	 */
-	default:
-		netdev_dbg(dev->net, "intr status %d\n", status);
-		break;
-	}
-
-	if (!netif_running (dev->net))
-		return;
-
-	memset(urb->transfer_buffer, 0, urb->transfer_buffer_length);
-	status = usb_submit_urb (urb, GFP_ATOMIC);
-	if (status != 0)
-		netif_err(dev, timer, dev->net,
-			  "intr resubmit --> %d\n", status);
-}
-
 /*-------------------------------------------------------------------------*/
 void usbnet_pause_rx(struct usbnet *dev)
 {
@@ -1312,7 +1309,6 @@ void usbnet_disconnect (struct usb_interface *intf)
 	usb_free_urb(dev->interrupt);
 
 	free_netdev(net);
-	usb_put_dev (xdev);
 }
 EXPORT_SYMBOL_GPL(usbnet_disconnect);
 
@@ -1368,8 +1364,6 @@ usbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
 	xdev = interface_to_usbdev (udev);
 	interface = udev->cur_altsetting;
 
-	usb_get_dev (xdev);
-
 	status = -ENOMEM;
 
 	// set up our own records
@@ -1498,7 +1492,6 @@ out3:
 out1:
 	free_netdev(net);
 out:
-	usb_put_dev(xdev);
 	return status;
 }
 EXPORT_SYMBOL_GPL(usbnet_probe);
@@ -1600,7 +1593,7 @@ static int __init usbnet_init(void)
 	BUILD_BUG_ON(
 		FIELD_SIZEOF(struct sk_buff, cb) < sizeof(struct skb_data));
 
-	random_ether_addr(node_id);
+	eth_random_addr(node_id);
 	return 0;
 }
 module_init(usbnet_init);
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index f18149ae258..83d2b0c34c5 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -704,16 +704,16 @@ static struct rtnl_link_stats64 *virtnet_stats(struct net_device *dev,
 		u64 tpackets, tbytes, rpackets, rbytes;
 
 		do {
-			start = u64_stats_fetch_begin(&stats->tx_syncp);
+			start = u64_stats_fetch_begin_bh(&stats->tx_syncp);
 			tpackets = stats->tx_packets;
 			tbytes   = stats->tx_bytes;
-		} while (u64_stats_fetch_retry(&stats->tx_syncp, start));
+		} while (u64_stats_fetch_retry_bh(&stats->tx_syncp, start));
 
 		do {
-			start = u64_stats_fetch_begin(&stats->rx_syncp);
+			start = u64_stats_fetch_begin_bh(&stats->rx_syncp);
 			rpackets = stats->rx_packets;
 			rbytes   = stats->rx_bytes;
-		} while (u64_stats_fetch_retry(&stats->rx_syncp, start));
+		} while (u64_stats_fetch_retry_bh(&stats->rx_syncp, start));
 
 		tot->rx_packets += rpackets;
 		tot->tx_packets += tpackets;
@@ -1062,7 +1062,7 @@ static int virtnet_probe(struct virtio_device *vdev)
 		return -ENOMEM;
 
 	/* Set up network device as normal. */
-	dev->priv_flags |= IFF_UNICAST_FLT;
+	dev->priv_flags |= IFF_UNICAST_FLT | IFF_LIVE_ADDR_CHANGE;
 	dev->netdev_ops = &virtnet_netdev;
 	dev->features = NETIF_F_HIGHDMA;
 
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index 3f04ba0a545..93e0cfb739b 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -1037,7 +1037,7 @@ vmxnet3_tq_xmit(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 #endif
 	dev_dbg(&adapter->netdev->dev,
 		"txd[%u]: SOP 0x%Lx 0x%x 0x%x\n",
-		(u32)((union Vmxnet3_GenericDesc *)ctx.sop_txd -
+		(u32)(ctx.sop_txd -
 		tq->tx_ring.base), le64_to_cpu(gdesc->txd.addr),
 		le32_to_cpu(gdesc->dword[2]), le32_to_cpu(gdesc->dword[3]));
 
diff --git a/drivers/net/wan/x25_asy.c b/drivers/net/wan/x25_asy.c
index d7a65e141d1..44db8b75a53 100644
--- a/drivers/net/wan/x25_asy.c
+++ b/drivers/net/wan/x25_asy.c
@@ -231,7 +231,7 @@ static void x25_asy_encaps(struct x25_asy *sl, unsigned char *icp, int len)
 	}
 
 	p = icp;
-	count = x25_asy_esc(p, (unsigned char *) sl->xbuff, len);
+	count = x25_asy_esc(p, sl->xbuff, len);
 
 	/* Order of next two lines is *very* important.
 	 * When we are sending a little amount of data,
diff --git a/drivers/net/wimax/i2400m/Kconfig b/drivers/net/wimax/i2400m/Kconfig
index 672de18a776..71453db1425 100644
--- a/drivers/net/wimax/i2400m/Kconfig
+++ b/drivers/net/wimax/i2400m/Kconfig
@@ -7,9 +7,6 @@ config WIMAX_I2400M
 comment "Enable USB support to see WiMAX USB drivers"
 	depends on USB = n
 
-comment "Enable MMC support to see WiMAX SDIO drivers"
-	depends on MMC = n
-
 config WIMAX_I2400M_USB
 	tristate "Intel Wireless WiMAX Connection 2400 over USB (including 5x50)"
 	depends on WIMAX && USB
@@ -21,25 +18,6 @@ config WIMAX_I2400M_USB
 
 	  If unsure, it is safe to select M (module).
 
-config WIMAX_I2400M_SDIO
-	tristate "Intel Wireless WiMAX Connection 2400 over SDIO"
-	depends on WIMAX && MMC
-	select WIMAX_I2400M
-	help
-	  Select if you have a device based on the Intel WiMAX
-	  Connection 2400 over SDIO.
-
-	  If unsure, it is safe to select M (module).
-
-config WIMAX_IWMC3200_SDIO
-	bool "Intel Wireless Multicom WiMAX Connection 3200 over SDIO (EXPERIMENTAL)"
-	depends on WIMAX_I2400M_SDIO
-	depends on EXPERIMENTAL
-	select IWMC3200TOP
-	help
-	  Select if you have a device based on the Intel Multicom WiMAX
-          Connection 3200 over SDIO.
-
 config WIMAX_I2400M_DEBUG_LEVEL
 	int "WiMAX i2400m debug level"
 	depends on WIMAX_I2400M
diff --git a/drivers/net/wimax/i2400m/Makefile b/drivers/net/wimax/i2400m/Makefile
index 5d9e018d31a..f6d19c34808 100644
--- a/drivers/net/wimax/i2400m/Makefile
+++ b/drivers/net/wimax/i2400m/Makefile
@@ -1,7 +1,6 @@
 
 obj-$(CONFIG_WIMAX_I2400M) += i2400m.o
 obj-$(CONFIG_WIMAX_I2400M_USB) += i2400m-usb.o
-obj-$(CONFIG_WIMAX_I2400M_SDIO) += i2400m-sdio.o
 
 i2400m-y :=		\
 	control.o	\
@@ -21,10 +20,3 @@ i2400m-usb-y :=			\
 	usb-tx.o		\
 	usb-rx.o		\
 	usb.o
-
-
-i2400m-sdio-y := 		\
-	sdio.o      		\
-	sdio-tx.o   		\
-	sdio-fw.o	 	\
-	sdio-rx.o
diff --git a/drivers/net/wimax/i2400m/control.c b/drivers/net/wimax/i2400m/control.c
index 2fea02b35b2..4a01e5c7fe0 100644
--- a/drivers/net/wimax/i2400m/control.c
+++ b/drivers/net/wimax/i2400m/control.c
@@ -130,7 +130,7 @@ ssize_t i2400m_tlv_match(const struct i2400m_tlv_hdr *tlv,
 	    && le16_to_cpu(tlv->length) + sizeof(*tlv) != tlv_size) {
 		size_t size = le16_to_cpu(tlv->length) + sizeof(*tlv);
 		printk(KERN_WARNING "W: tlv type 0x%x mismatched because of "
-		       "size (got %zu vs %zu expected)\n",
+		       "size (got %zu vs %zd expected)\n",
 		       tlv_type, size, tlv_size);
 		return size;
 	}
@@ -235,7 +235,7 @@ const struct i2400m_tlv_hdr *i2400m_tlv_find(
 			break;
 		if (match > 0)
 			dev_warn(dev, "TLV type 0x%04x found with size "
-				 "mismatch (%zu vs %zu needed)\n",
+				 "mismatch (%zu vs %zd needed)\n",
 				 tlv_type, match, tlv_size);
 	}
 	return tlv;
diff --git a/drivers/net/wimax/i2400m/driver.c b/drivers/net/wimax/i2400m/driver.c
index 47cae7150bc..02542613275 100644
--- a/drivers/net/wimax/i2400m/driver.c
+++ b/drivers/net/wimax/i2400m/driver.c
@@ -754,8 +754,7 @@ EXPORT_SYMBOL_GPL(i2400m_error_recovery);
 /*
  * Alloc the command and ack buffers for boot mode
  *
- * Get the buffers needed to deal with boot mode messages.  These
- * buffers need to be allocated before the sdio receive irq is setup.
+ * Get the buffers needed to deal with boot mode messages.
  */
 static
 int i2400m_bm_buf_alloc(struct i2400m *i2400m)
@@ -897,7 +896,7 @@ int i2400m_setup(struct i2400m *i2400m, enum i2400m_bri bm_flags)
 	result = i2400m_read_mac_addr(i2400m);
 	if (result < 0)
 		goto error_read_mac_addr;
-	random_ether_addr(i2400m->src_mac_addr);
+	eth_random_addr(i2400m->src_mac_addr);
 
 	i2400m->pm_notifier.notifier_call = i2400m_pm_notifier;
 	register_pm_notifier(&i2400m->pm_notifier);
diff --git a/drivers/net/wimax/i2400m/fw.c b/drivers/net/wimax/i2400m/fw.c
index 7cbd7d231e1..283237f6f07 100644
--- a/drivers/net/wimax/i2400m/fw.c
+++ b/drivers/net/wimax/i2400m/fw.c
@@ -51,8 +51,7 @@
  * firmware. Normal hardware takes only signed firmware.
  *
  * On boot mode, in USB, we write to the device using the bulk out
- * endpoint and read from it in the notification endpoint. In SDIO we
- * talk to it via the write address and read from the read address.
+ * endpoint and read from it in the notification endpoint.
  *
  * Upon entrance to boot mode, the device sends (preceded with a few
  * zero length packets (ZLPs) on the notification endpoint in USB) a
@@ -1268,7 +1267,7 @@ int i2400m_fw_check(struct i2400m *i2400m, const void *bcf, size_t bcf_size)
 		size_t leftover, offset, header_len, size;
 
 		leftover = top - itr;
-		offset = itr - (const void *) bcf;
+		offset = itr - bcf;
 		if (leftover <= sizeof(*bcf_hdr)) {
 			dev_err(dev, "firmware %s: %zu B left at @%zx, "
 				"not enough for BCF header\n",
diff --git a/drivers/net/wimax/i2400m/i2400m-sdio.h b/drivers/net/wimax/i2400m/i2400m-sdio.h
deleted file mode 100644
index 1d63ffdedfd..00000000000
--- a/drivers/net/wimax/i2400m/i2400m-sdio.h
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * Intel Wireless WiMAX Connection 2400m
- * SDIO-specific i2400m driver definitions
- *
- *
- * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <linux-wimax@intel.com>
- * Brian Bian <brian.bian@intel.com>
- * Dirk Brandewie <dirk.j.brandewie@intel.com>
- * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
- * Yanir Lubetkin <yanirx.lubetkin@intel.com>
- *  - Initial implementation
- *
- *
- * This driver implements the bus-specific part of the i2400m for
- * SDIO. Check i2400m.h for a generic driver description.
- *
- * ARCHITECTURE
- *
- * This driver sits under the bus-generic i2400m driver, providing the
- * connection to the device.
- *
- * When probed, all the function pointers are setup and then the
- * bus-generic code called. The generic driver will then use the
- * provided pointers for uploading firmware (i2400ms_bus_bm*() in
- * sdio-fw.c) and then setting up the device (i2400ms_dev_*() in
- * sdio.c).
- *
- * Once firmware is uploaded, TX functions (sdio-tx.c) are called when
- * data is ready for transmission in the TX fifo; then the SDIO IRQ is
- * fired and data is available (sdio-rx.c), it is sent to the generic
- * driver for processing with i2400m_rx.
- */
-
-#ifndef __I2400M_SDIO_H__
-#define __I2400M_SDIO_H__
-
-#include "i2400m.h"
-
-/* Host-Device interface for SDIO */
-enum {
-	I2400M_SDIO_BOOT_RETRIES = 3,
-	I2400MS_BLK_SIZE = 256,
-	I2400MS_PL_SIZE_MAX = 0x3E00,
-
-	I2400MS_DATA_ADDR = 0x0,
-	I2400MS_INTR_STATUS_ADDR = 0x13,
-	I2400MS_INTR_CLEAR_ADDR = 0x13,
-	I2400MS_INTR_ENABLE_ADDR = 0x14,
-	I2400MS_INTR_GET_SIZE_ADDR = 0x2C,
-	/* The number of ticks to wait for the device to signal that
-	 * it is ready */
-	I2400MS_INIT_SLEEP_INTERVAL = 100,
-	/* How long to wait for the device to settle after reset */
-	I2400MS_SETTLE_TIME = 40,
-	/* The number of msec to wait for IOR after sending IOE */
-	IWMC3200_IOR_TIMEOUT = 10,
-};
-
-
-/**
- * struct i2400ms - descriptor for a SDIO connected i2400m
- *
- * @i2400m: bus-generic i2400m implementation; has to be first (see
- *     it's documentation in i2400m.h).
- *
- * @func: pointer to our SDIO function
- *
- * @tx_worker: workqueue struct used to TX data when the bus-generic
- *     code signals packets are pending for transmission to the device.
- *
- * @tx_workqueue: workqeueue used for data TX; we don't use the
- *     system's workqueue as that might cause deadlocks with code in
- *     the bus-generic driver. The read/write operation to the queue
- *     is protected with spinlock (tx_lock in struct i2400m) to avoid
- *     the queue being destroyed in the middle of a the queue read/write
- *     operation.
- *
- * @debugfs_dentry: dentry for the SDIO specific debugfs files
- *
- *     Note this value is set to NULL upon destruction; this is
- *     because some routinges use it to determine if we are inside the
- *     probe() path or some other path. When debugfs is disabled,
- *     creation sets the dentry to '(void*) -ENODEV', which is valid
- *     for the test.
- */
-struct i2400ms {
-	struct i2400m i2400m;		/* FIRST! See doc */
-	struct sdio_func *func;
-
-	struct work_struct tx_worker;
-	struct workqueue_struct *tx_workqueue;
-	char tx_wq_name[32];
-
-	struct dentry *debugfs_dentry;
-
-	wait_queue_head_t bm_wfa_wq;
-	int bm_wait_result;
-	size_t bm_ack_size;
-
-	/* Device is any of the iwmc3200 SKUs */
-	unsigned iwmc3200:1;
-};
-
-
-static inline
-void i2400ms_init(struct i2400ms *i2400ms)
-{
-	i2400m_init(&i2400ms->i2400m);
-}
-
-
-extern int i2400ms_rx_setup(struct i2400ms *);
-extern void i2400ms_rx_release(struct i2400ms *);
-
-extern int i2400ms_tx_setup(struct i2400ms *);
-extern void i2400ms_tx_release(struct i2400ms *);
-extern void i2400ms_bus_tx_kick(struct i2400m *);
-
-extern ssize_t i2400ms_bus_bm_cmd_send(struct i2400m *,
-				       const struct i2400m_bootrom_header *,
-				       size_t, int);
-extern ssize_t i2400ms_bus_bm_wait_for_ack(struct i2400m *,
-					   struct i2400m_bootrom_header *,
-					   size_t);
-extern void i2400ms_bus_bm_release(struct i2400m *);
-extern int i2400ms_bus_bm_setup(struct i2400m *);
-
-#endif /* #ifndef __I2400M_SDIO_H__ */
diff --git a/drivers/net/wimax/i2400m/i2400m.h b/drivers/net/wimax/i2400m/i2400m.h
index c806d455021..79c6505b5c2 100644
--- a/drivers/net/wimax/i2400m/i2400m.h
+++ b/drivers/net/wimax/i2400m/i2400m.h
@@ -46,7 +46,7 @@
  *  - bus generic driver (this part)
  *
  * The bus specific driver sets up stuff specific to the bus the
- * device is connected to (USB, SDIO, PCI, tam-tam...non-authoritative
+ * device is connected to (USB, PCI, tam-tam...non-authoritative
  * nor binding list) which is basically the device-model management
  * (probe/disconnect, etc), moving data from device to kernel and
  * back, doing the power saving details and reseting the device.
@@ -238,14 +238,13 @@ struct i2400m_barker_db;
  * amount needed for loading firmware, where us dev_start/stop setup
  * the rest needed to do full data/control traffic.
  *
- * @bus_tx_block_size: [fill] SDIO imposes a 256 block size, USB 16,
- *     so we have a tx_blk_size variable that the bus layer sets to
- *     tell the engine how much of that we need.
+ * @bus_tx_block_size: [fill] USB imposes a 16 block size, but other
+ *     busses will differ.  So we have a tx_blk_size variable that the
+ *     bus layer sets to tell the engine how much of that we need.
  *
  * @bus_tx_room_min: [fill] Minimum room required while allocating
- *     TX queue's buffer space for message header. SDIO requires
- *     224 bytes and USB 16 bytes. Refer bus specific driver code
- *     for details.
+ *     TX queue's buffer space for message header. USB requires
+ *     16 bytes. Refer to bus specific driver code for details.
  *
  * @bus_pl_size_max: [fill] Maximum payload size.
  *
diff --git a/drivers/net/wimax/i2400m/sdio-debug-levels.h b/drivers/net/wimax/i2400m/sdio-debug-levels.h
deleted file mode 100644
index c5199874130..00000000000
--- a/drivers/net/wimax/i2400m/sdio-debug-levels.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * debug levels control file for the i2400m module's
- */
-#ifndef __debug_levels__h__
-#define __debug_levels__h__
-
-/* Maximum compile and run time debug level for all submodules */
-#define D_MODULENAME i2400m_sdio
-#define D_MASTER CONFIG_WIMAX_I2400M_DEBUG_LEVEL
-
-#include <linux/wimax/debug.h>
-
-/* List of all the enabled modules */
-enum d_module {
-	D_SUBMODULE_DECLARE(main),
-	D_SUBMODULE_DECLARE(tx),
-	D_SUBMODULE_DECLARE(rx),
-	D_SUBMODULE_DECLARE(fw)
-};
-
-
-#endif /* #ifndef __debug_levels__h__ */
diff --git a/drivers/net/wimax/i2400m/sdio-fw.c b/drivers/net/wimax/i2400m/sdio-fw.c
deleted file mode 100644
index 8e025418f5b..00000000000
--- a/drivers/net/wimax/i2400m/sdio-fw.c
+++ /dev/null
@@ -1,210 +0,0 @@
-/*
- * Intel Wireless WiMAX Connection 2400m
- * Firmware uploader's SDIO specifics
- *
- *
- * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <linux-wimax@intel.com>
- * Yanir Lubetkin <yanirx.lubetkin@intel.com>
- * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
- *  - Initial implementation
- *
- * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
- *  - Bus generic/specific split for USB
- *
- * Dirk Brandewie <dirk.j.brandewie@intel.com>
- *  - Initial implementation for SDIO
- *
- * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
- *  - SDIO rehash for changes in the bus-driver model
- *
- * Dirk Brandewie <dirk.j.brandewie@intel.com>
- *  - Make it IRQ based, not polling
- *
- * THE PROCEDURE
- *
- * See fw.c for the generic description of this procedure.
- *
- * This file implements only the SDIO specifics. It boils down to how
- * to send a command and waiting for an acknowledgement from the
- * device.
- *
- * All this code is sequential -- all i2400ms_bus_bm_*() functions are
- * executed in the same thread, except i2400ms_bm_irq() [on its own by
- * the SDIO driver]. This makes it possible to avoid locking.
- *
- * COMMAND EXECUTION
- *
- * The generic firmware upload code will call i2400m_bus_bm_cmd_send()
- * to send commands.
- *
- * The SDIO devices expects things in 256 byte blocks, so it will pad
- * it, compute the checksum (if needed) and pass it to SDIO.
- *
- * ACK RECEPTION
- *
- * This works in IRQ mode -- the fw loader says when to wait for data
- * and for that it calls i2400ms_bus_bm_wait_for_ack().
- *
- * This checks if there is any data available (RX size > 0); if not,
- * waits for the IRQ handler to notify about it. Once there is data,
- * it is read and passed to the caller. Doing it this way we don't
- * need much coordination/locking, and it makes it much more difficult
- * for an interrupt to be lost and the wait_for_ack() function getting
- * stuck even when data is pending.
- */
-#include <linux/mmc/sdio_func.h>
-#include "i2400m-sdio.h"
-
-
-#define D_SUBMODULE fw
-#include "sdio-debug-levels.h"
-
-
-/*
- * Send a boot-mode command to the SDIO function
- *
- * We use a bounce buffer (i2400m->bm_cmd_buf) because we need to
- * touch the header if the RAW flag is not set.
- *
- * @flags: pass thru from i2400m_bm_cmd()
- * @return: cmd_size if ok, < 0 errno code on error.
- *
- * Note the command is padded to the SDIO block size for the device.
- */
-ssize_t i2400ms_bus_bm_cmd_send(struct i2400m *i2400m,
-				const struct i2400m_bootrom_header *_cmd,
-				size_t cmd_size, int flags)
-{
-	ssize_t result;
-	struct device *dev = i2400m_dev(i2400m);
-	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
-	int opcode = _cmd == NULL ? -1 : i2400m_brh_get_opcode(_cmd);
-	struct i2400m_bootrom_header *cmd;
-	/* SDIO restriction */
-	size_t cmd_size_a = ALIGN(cmd_size, I2400MS_BLK_SIZE);
-
-	d_fnstart(5, dev, "(i2400m %p cmd %p size %zu)\n",
-		  i2400m, _cmd, cmd_size);
-	result = -E2BIG;
-	if (cmd_size > I2400M_BM_CMD_BUF_SIZE)
-		goto error_too_big;
-
-	if (_cmd != i2400m->bm_cmd_buf)
-		memmove(i2400m->bm_cmd_buf, _cmd, cmd_size);
-	cmd = i2400m->bm_cmd_buf;
-	if (cmd_size_a > cmd_size)			/* Zero pad space */
-		memset(i2400m->bm_cmd_buf + cmd_size, 0, cmd_size_a - cmd_size);
-	if ((flags & I2400M_BM_CMD_RAW) == 0) {
-		if (WARN_ON(i2400m_brh_get_response_required(cmd) == 0))
-			dev_warn(dev, "SW BUG: response_required == 0\n");
-		i2400m_bm_cmd_prepare(cmd);
-	}
-	d_printf(4, dev, "BM cmd %d: %zu bytes (%zu padded)\n",
-		 opcode, cmd_size, cmd_size_a);
-	d_dump(5, dev, cmd, cmd_size);
-
-	sdio_claim_host(i2400ms->func);			/* Send & check */
-	result = sdio_memcpy_toio(i2400ms->func, I2400MS_DATA_ADDR,
-				  i2400m->bm_cmd_buf, cmd_size_a);
-	sdio_release_host(i2400ms->func);
-	if (result < 0) {
-		dev_err(dev, "BM cmd %d: cannot send: %ld\n",
-			opcode, (long) result);
-		goto error_cmd_send;
-	}
-	result = cmd_size;
-error_cmd_send:
-error_too_big:
-	d_fnend(5, dev, "(i2400m %p cmd %p size %zu) = %d\n",
-		i2400m, _cmd, cmd_size, (int) result);
-	return result;
-}
-
-
-/*
- * Read an ack from the device's boot-mode
- *
- * @i2400m:
- * @_ack: pointer to where to store the read data
- * @ack_size: how many bytes we should read
- *
- * Returns: < 0 errno code on error; otherwise, amount of received bytes.
- *
- * The ACK for a BM command is always at least sizeof(*ack) bytes, so
- * check for that. We don't need to check for device reboots
- *
- */
-ssize_t i2400ms_bus_bm_wait_for_ack(struct i2400m *i2400m,
-				    struct i2400m_bootrom_header *ack,
-				    size_t ack_size)
-{
-	ssize_t result;
-	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-	int size;
-
-	BUG_ON(sizeof(*ack) > ack_size);
-
-	d_fnstart(5, dev, "(i2400m %p ack %p size %zu)\n",
-		  i2400m, ack, ack_size);
-
-	result = wait_event_timeout(i2400ms->bm_wfa_wq,
-				    i2400ms->bm_ack_size != -EINPROGRESS,
-				    2 * HZ);
-	if (result == 0) {
-		result = -ETIMEDOUT;
-		dev_err(dev, "BM: error waiting for an ack\n");
-		goto error_timeout;
-	}
-
-	spin_lock(&i2400m->rx_lock);
-	result = i2400ms->bm_ack_size;
-	BUG_ON(result == -EINPROGRESS);
-	if (result < 0)        /* so we exit when rx_release() is called */
-		dev_err(dev, "BM: %s failed: %zd\n", __func__, result);
-	else {
-		size = min(ack_size, i2400ms->bm_ack_size);
-		memcpy(ack, i2400m->bm_ack_buf, size);
-	}
-	/*
-	 * Remember always to clear the bm_ack_size to -EINPROGRESS
-	 * after the RX data is processed
-	 */
-	i2400ms->bm_ack_size = -EINPROGRESS;
-	spin_unlock(&i2400m->rx_lock);
-
-error_timeout:
-	d_fnend(5, dev, "(i2400m %p ack %p size %zu) = %zd\n",
-		i2400m, ack, ack_size, result);
-	return result;
-}
diff --git a/drivers/net/wimax/i2400m/sdio-rx.c b/drivers/net/wimax/i2400m/sdio-rx.c
deleted file mode 100644
index fb6396dd115..00000000000
--- a/drivers/net/wimax/i2400m/sdio-rx.c
+++ /dev/null
@@ -1,301 +0,0 @@
-/*
- * Intel Wireless WiMAX Connection 2400m
- * SDIO RX handling
- *
- *
- * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <linux-wimax@intel.com>
- * Dirk Brandewie <dirk.j.brandewie@intel.com>
- *  - Initial implementation
- *
- *
- * This handles the RX path on SDIO.
- *
- * The SDIO bus driver calls the "irq" routine when data is available.
- * This is not a traditional interrupt routine since the SDIO bus
- * driver calls us from its irq thread context.  Because of this
- * sleeping in the SDIO RX IRQ routine is okay.
- *
- * From there on, we obtain the size of the data that is available,
- * allocate an skb, copy it and then pass it to the generic driver's
- * RX routine [i2400m_rx()].
- *
- * ROADMAP
- *
- * i2400ms_irq()
- *   i2400ms_rx()
- *     __i2400ms_rx_get_size()
- *     i2400m_is_boot_barker()
- *     i2400m_rx()
- *
- * i2400ms_rx_setup()
- *
- * i2400ms_rx_release()
- */
-#include <linux/workqueue.h>
-#include <linux/wait.h>
-#include <linux/skbuff.h>
-#include <linux/mmc/sdio.h>
-#include <linux/mmc/sdio_func.h>
-#include <linux/slab.h>
-#include "i2400m-sdio.h"
-
-#define D_SUBMODULE rx
-#include "sdio-debug-levels.h"
-
-static const __le32 i2400m_ACK_BARKER[4] = {
-	__constant_cpu_to_le32(I2400M_ACK_BARKER),
-	__constant_cpu_to_le32(I2400M_ACK_BARKER),
-	__constant_cpu_to_le32(I2400M_ACK_BARKER),
-	__constant_cpu_to_le32(I2400M_ACK_BARKER)
-};
-
-
-/*
- * Read and return the amount of bytes available for RX
- *
- * The RX size has to be read like this: byte reads of three
- * sequential locations; then glue'em together.
- *
- * sdio_readl() doesn't work.
- */
-static ssize_t __i2400ms_rx_get_size(struct i2400ms *i2400ms)
-{
-	int ret, cnt, val;
-	ssize_t rx_size;
-	unsigned xfer_size_addr;
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &i2400ms->func->dev;
-
-	d_fnstart(7, dev, "(i2400ms %p)\n", i2400ms);
-	xfer_size_addr = I2400MS_INTR_GET_SIZE_ADDR;
-	rx_size = 0;
-	for (cnt = 0; cnt < 3; cnt++) {
-		val = sdio_readb(func, xfer_size_addr + cnt, &ret);
-		if (ret < 0) {
-			dev_err(dev, "RX: Can't read byte %d of RX size from "
-				"0x%08x: %d\n", cnt, xfer_size_addr + cnt, ret);
-			rx_size = ret;
-			goto error_read;
-		}
-		rx_size = rx_size << 8 | (val & 0xff);
-	}
-	d_printf(6, dev, "RX: rx_size is %ld\n", (long) rx_size);
-error_read:
-	d_fnend(7, dev, "(i2400ms %p) = %ld\n", i2400ms, (long) rx_size);
-	return rx_size;
-}
-
-
-/*
- * Read data from the device (when in normal)
- *
- * Allocate an SKB of the right size, read the data in and then
- * deliver it to the generic layer.
- *
- * We also check for a reboot barker. That means the device died and
- * we have to reboot it.
- */
-static
-void i2400ms_rx(struct i2400ms *i2400ms)
-{
-	int ret;
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-	struct i2400m *i2400m = &i2400ms->i2400m;
-	struct sk_buff *skb;
-	ssize_t rx_size;
-
-	d_fnstart(7, dev, "(i2400ms %p)\n", i2400ms);
-	rx_size = __i2400ms_rx_get_size(i2400ms);
-	if (rx_size < 0) {
-		ret = rx_size;
-		goto error_get_size;
-	}
-	/*
-	 * Hardware quirk: make sure to clear the INTR status register
-	 * AFTER getting the data transfer size.
-	 */
-	sdio_writeb(func, 1, I2400MS_INTR_CLEAR_ADDR, &ret);
-
-	ret = -ENOMEM;
-	skb = alloc_skb(rx_size, GFP_ATOMIC);
-	if (NULL == skb) {
-		dev_err(dev, "RX: unable to alloc skb\n");
-		goto error_alloc_skb;
-	}
-	ret = sdio_memcpy_fromio(func, skb->data,
-				 I2400MS_DATA_ADDR, rx_size);
-	if (ret < 0) {
-		dev_err(dev, "RX: SDIO data read failed: %d\n", ret);
-		goto error_memcpy_fromio;
-	}
-
-	rmb();	/* make sure we get boot_mode from dev_reset_handle */
-	if (unlikely(i2400m->boot_mode == 1)) {
-		spin_lock(&i2400m->rx_lock);
-		i2400ms->bm_ack_size = rx_size;
-		spin_unlock(&i2400m->rx_lock);
-		memcpy(i2400m->bm_ack_buf, skb->data, rx_size);
-		wake_up(&i2400ms->bm_wfa_wq);
-		d_printf(5, dev, "RX: SDIO boot mode message\n");
-		kfree_skb(skb);
-		goto out;
-	}
-	ret = -EIO;
-	if (unlikely(rx_size < sizeof(__le32))) {
-		dev_err(dev, "HW BUG? only %zu bytes received\n", rx_size);
-		goto error_bad_size;
-	}
-	if (likely(i2400m_is_d2h_barker(skb->data))) {
-		skb_put(skb, rx_size);
-		i2400m_rx(i2400m, skb);
-	} else if (unlikely(i2400m_is_boot_barker(i2400m,
-						  skb->data, rx_size))) {
-		ret = i2400m_dev_reset_handle(i2400m, "device rebooted");
-		dev_err(dev, "RX: SDIO reboot barker\n");
-		kfree_skb(skb);
-	} else {
-		i2400m_unknown_barker(i2400m, skb->data, rx_size);
-		kfree_skb(skb);
-	}
-out:
-	d_fnend(7, dev, "(i2400ms %p) = void\n", i2400ms);
-	return;
-
-error_memcpy_fromio:
-	kfree_skb(skb);
-error_alloc_skb:
-error_get_size:
-error_bad_size:
-	d_fnend(7, dev, "(i2400ms %p) = %d\n", i2400ms, ret);
-}
-
-
-/*
- * Process an interrupt from the SDIO card
- *
- * FIXME: need to process other events that are not just ready-to-read
- *
- * Checks there is data ready and then proceeds to read it.
- */
-static
-void i2400ms_irq(struct sdio_func *func)
-{
-	int ret;
-	struct i2400ms *i2400ms = sdio_get_drvdata(func);
-	struct device *dev = &func->dev;
-	int val;
-
-	d_fnstart(6, dev, "(i2400ms %p)\n", i2400ms);
-	val = sdio_readb(func, I2400MS_INTR_STATUS_ADDR, &ret);
-	if (ret < 0) {
-		dev_err(dev, "RX: Can't read interrupt status: %d\n", ret);
-		goto error_no_irq;
-	}
-	if (!val) {
-		dev_err(dev, "RX: BUG? got IRQ but no interrupt ready?\n");
-		goto error_no_irq;
-	}
-	i2400ms_rx(i2400ms);
-error_no_irq:
-	d_fnend(6, dev, "(i2400ms %p) = void\n", i2400ms);
-}
-
-
-/*
- * Setup SDIO RX
- *
- * Hooks up the IRQ handler and then enables IRQs.
- */
-int i2400ms_rx_setup(struct i2400ms *i2400ms)
-{
-	int result;
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-	struct i2400m *i2400m = &i2400ms->i2400m;
-
-	d_fnstart(5, dev, "(i2400ms %p)\n", i2400ms);
-
-	init_waitqueue_head(&i2400ms->bm_wfa_wq);
-	spin_lock(&i2400m->rx_lock);
-	i2400ms->bm_wait_result = -EINPROGRESS;
-	/*
-	 * Before we are about to enable the RX interrupt, make sure
-	 * bm_ack_size is cleared to -EINPROGRESS which indicates
-	 * no RX interrupt happened yet or the previous interrupt
-	 * has been handled, we are ready to take the new interrupt
-	 */
-	i2400ms->bm_ack_size = -EINPROGRESS;
-	spin_unlock(&i2400m->rx_lock);
-
-	sdio_claim_host(func);
-	result = sdio_claim_irq(func, i2400ms_irq);
-	if (result < 0) {
-		dev_err(dev, "Cannot claim IRQ: %d\n", result);
-		goto error_irq_claim;
-	}
-	result = 0;
-	sdio_writeb(func, 1, I2400MS_INTR_ENABLE_ADDR, &result);
-	if (result < 0) {
-		sdio_release_irq(func);
-		dev_err(dev, "Failed to enable interrupts %d\n", result);
-	}
-error_irq_claim:
-	sdio_release_host(func);
-	d_fnend(5, dev, "(i2400ms %p) = %d\n", i2400ms, result);
-	return result;
-}
-
-
-/*
- * Tear down SDIO RX
- *
- * Disables IRQs in the device and removes the IRQ handler.
- */
-void i2400ms_rx_release(struct i2400ms *i2400ms)
-{
-	int result;
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-	struct i2400m *i2400m = &i2400ms->i2400m;
-
-	d_fnstart(5, dev, "(i2400ms %p)\n", i2400ms);
-	spin_lock(&i2400m->rx_lock);
-	i2400ms->bm_ack_size = -EINTR;
-	spin_unlock(&i2400m->rx_lock);
-	wake_up_all(&i2400ms->bm_wfa_wq);
-	sdio_claim_host(func);
-	sdio_writeb(func, 0, I2400MS_INTR_ENABLE_ADDR, &result);
-	sdio_release_irq(func);
-	sdio_release_host(func);
-	d_fnend(5, dev, "(i2400ms %p) = %d\n", i2400ms, result);
-}
diff --git a/drivers/net/wimax/i2400m/sdio-tx.c b/drivers/net/wimax/i2400m/sdio-tx.c
deleted file mode 100644
index b53cd1c80e3..00000000000
--- a/drivers/net/wimax/i2400m/sdio-tx.c
+++ /dev/null
@@ -1,177 +0,0 @@
-/*
- * Intel Wireless WiMAX Connection 2400m
- * SDIO TX transaction backends
- *
- *
- * Copyright (C) 2007-2008 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <linux-wimax@intel.com>
- * Dirk Brandewie <dirk.j.brandewie@intel.com>
- *  - Initial implementation
- *
- *
- * Takes the TX messages in the i2400m's driver TX FIFO and sends them
- * to the device until there are no more.
- *
- * If we fail sending the message, we just drop it. There isn't much
- * we can do at this point. Most of the traffic is network, which has
- * recovery methods for dropped packets.
- *
- * The SDIO functions are not atomic, so we can't run from the context
- * where i2400m->bus_tx_kick() [i2400ms_bus_tx_kick()] is being called
- * (some times atomic). Thus, the actual TX work is deferred to a
- * workqueue.
- *
- * ROADMAP
- *
- * i2400ms_bus_tx_kick()
- *   i2400ms_tx_submit()     [through workqueue]
- *
- * i2400m_tx_setup()
- *
- * i2400m_tx_release()
- */
-#include <linux/mmc/sdio_func.h>
-#include "i2400m-sdio.h"
-
-#define D_SUBMODULE tx
-#include "sdio-debug-levels.h"
-
-
-/*
- * Pull TX transations from the TX FIFO and send them to the device
- * until there are no more.
- */
-static
-void i2400ms_tx_submit(struct work_struct *ws)
-{
-	int result;
-	struct i2400ms *i2400ms = container_of(ws, struct i2400ms, tx_worker);
-	struct i2400m *i2400m = &i2400ms->i2400m;
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-	struct i2400m_msg_hdr *tx_msg;
-	size_t tx_msg_size;
-
-	d_fnstart(4, dev, "(i2400ms %p, i2400m %p)\n", i2400ms, i2400ms);
-
-	while (NULL != (tx_msg = i2400m_tx_msg_get(i2400m, &tx_msg_size))) {
-		d_printf(2, dev, "TX: submitting %zu bytes\n", tx_msg_size);
-		d_dump(5, dev, tx_msg, tx_msg_size);
-
-		sdio_claim_host(func);
-		result = sdio_memcpy_toio(func, 0, tx_msg, tx_msg_size);
-		sdio_release_host(func);
-
-		i2400m_tx_msg_sent(i2400m);
-
-		if (result < 0) {
-			dev_err(dev, "TX: cannot submit TX; tx_msg @%zu %zu B:"
-				" %d\n", (void *) tx_msg - i2400m->tx_buf,
-				tx_msg_size, result);
-		}
-
-		if (result == -ETIMEDOUT) {
-			i2400m_error_recovery(i2400m);
-			break;
-		}
-		d_printf(2, dev, "TX: %zub submitted\n", tx_msg_size);
-	}
-
-	d_fnend(4, dev, "(i2400ms %p) = void\n", i2400ms);
-}
-
-
-/*
- * The generic driver notifies us that there is data ready for TX
- *
- * Schedule a run of i2400ms_tx_submit() to handle it.
- */
-void i2400ms_bus_tx_kick(struct i2400m *i2400m)
-{
-	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
-	struct device *dev = &i2400ms->func->dev;
-	unsigned long flags;
-
-	d_fnstart(3, dev, "(i2400m %p) = void\n", i2400m);
-
-	/* schedule tx work, this is because tx may block, therefore
-	 * it has to run in a thread context.
-	 */
-	spin_lock_irqsave(&i2400m->tx_lock, flags);
-	if (i2400ms->tx_workqueue != NULL)
-		queue_work(i2400ms->tx_workqueue, &i2400ms->tx_worker);
-	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
-
-	d_fnend(3, dev, "(i2400m %p) = void\n", i2400m);
-}
-
-int i2400ms_tx_setup(struct i2400ms *i2400ms)
-{
-	int result;
-	struct device *dev = &i2400ms->func->dev;
-	struct i2400m *i2400m = &i2400ms->i2400m;
-	struct workqueue_struct *tx_workqueue;
-	unsigned long flags;
-
-	d_fnstart(5, dev, "(i2400ms %p)\n", i2400ms);
-
-	INIT_WORK(&i2400ms->tx_worker, i2400ms_tx_submit);
-	snprintf(i2400ms->tx_wq_name, sizeof(i2400ms->tx_wq_name),
-		 "%s-tx", i2400m->wimax_dev.name);
-	tx_workqueue =
-		create_singlethread_workqueue(i2400ms->tx_wq_name);
-	if (tx_workqueue == NULL) {
-		dev_err(dev, "TX: failed to create workqueue\n");
-		result = -ENOMEM;
-	} else
-		result = 0;
-	spin_lock_irqsave(&i2400m->tx_lock, flags);
-	i2400ms->tx_workqueue = tx_workqueue;
-	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
-	d_fnend(5, dev, "(i2400ms %p) = %d\n", i2400ms, result);
-	return result;
-}
-
-void i2400ms_tx_release(struct i2400ms *i2400ms)
-{
-	struct i2400m *i2400m = &i2400ms->i2400m;
-	struct workqueue_struct *tx_workqueue;
-	unsigned long flags;
-
-	tx_workqueue = i2400ms->tx_workqueue;
-
-	spin_lock_irqsave(&i2400m->tx_lock, flags);
-	i2400ms->tx_workqueue = NULL;
-	spin_unlock_irqrestore(&i2400m->tx_lock, flags);
-
-	if (tx_workqueue)
-		destroy_workqueue(tx_workqueue);
-}
diff --git a/drivers/net/wimax/i2400m/sdio.c b/drivers/net/wimax/i2400m/sdio.c
deleted file mode 100644
index 21a9edd6e75..00000000000
--- a/drivers/net/wimax/i2400m/sdio.c
+++ /dev/null
@@ -1,602 +0,0 @@
-/*
- * Intel Wireless WiMAX Connection 2400m
- * Linux driver model glue for the SDIO device, reset & fw upload
- *
- *
- * Copyright (C) 2007-2008 Intel Corporation <linux-wimax@intel.com>
- * Dirk Brandewie <dirk.j.brandewie@intel.com>
- * Inaky Perez-Gonzalez <inaky.perez-gonzalez@intel.com>
- * Yanir Lubetkin <yanirx.lubetkin@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- *
- * See i2400m-sdio.h for a general description of this driver.
- *
- * This file implements driver model glue, and hook ups for the
- * generic driver to implement the bus-specific functions (device
- * communication setup/tear down, firmware upload and resetting).
- *
- * ROADMAP
- *
- * i2400m_probe()
- *   alloc_netdev()
- *     i2400ms_netdev_setup()
- *       i2400ms_init()
- *       i2400m_netdev_setup()
- *   i2400ms_enable_function()
- *   i2400m_setup()
- *
- * i2400m_remove()
- *     i2400m_release()
- *     free_netdev(net_dev)
- *
- * i2400ms_bus_reset()            Called by i2400m_reset
- *   __i2400ms_reset()
- *     __i2400ms_send_barker()
- */
-
-#include <linux/slab.h>
-#include <linux/debugfs.h>
-#include <linux/mmc/sdio_ids.h>
-#include <linux/mmc/sdio.h>
-#include <linux/mmc/sdio_func.h>
-#include "i2400m-sdio.h"
-#include <linux/wimax/i2400m.h>
-#include <linux/module.h>
-
-#define D_SUBMODULE main
-#include "sdio-debug-levels.h"
-
-/* IOE WiMAX function timeout in seconds */
-static int ioe_timeout = 2;
-module_param(ioe_timeout, int, 0);
-
-static char i2400ms_debug_params[128];
-module_param_string(debug, i2400ms_debug_params, sizeof(i2400ms_debug_params),
-		    0644);
-MODULE_PARM_DESC(debug,
-		 "String of space-separated NAME:VALUE pairs, where NAMEs "
-		 "are the different debug submodules and VALUE are the "
-		 "initial debug value to set.");
-
-/* Our firmware file name list */
-static const char *i2400ms_bus_fw_names[] = {
-#define I2400MS_FW_FILE_NAME "i2400m-fw-sdio-1.3.sbcf"
-	I2400MS_FW_FILE_NAME,
-	NULL
-};
-
-
-static const struct i2400m_poke_table i2400ms_pokes[] = {
-	I2400M_FW_POKE(0x6BE260, 0x00000088),
-	I2400M_FW_POKE(0x080550, 0x00000005),
-	I2400M_FW_POKE(0xAE0000, 0x00000000),
-	I2400M_FW_POKE(0x000000, 0x00000000), /* MUST be 0 terminated or bad
-					       * things will happen */
-};
-
-/*
- * Enable the SDIO function
- *
- * Tries to enable the SDIO function; might fail if it is still not
- * ready (in some hardware, the SDIO WiMAX function is only enabled
- * when we ask it to explicitly doing). Tries until a timeout is
- * reached.
- *
- * The @maxtries argument indicates how many times (at most) it should
- * be tried to enable the function. 0 means forever. This acts along
- * with the timeout (ie: it'll stop trying as soon as the maximum
- * number of tries is reached _or_ as soon as the timeout is reached).
- *
- * The reverse of this is...sdio_disable_function()
- *
- * Returns: 0 if the SDIO function was enabled, < 0 errno code on
- *     error (-ENODEV when it was unable to enable the function).
- */
-static
-int i2400ms_enable_function(struct i2400ms *i2400ms, unsigned maxtries)
-{
-	struct sdio_func *func = i2400ms->func;
-	u64 timeout;
-	int err;
-	struct device *dev = &func->dev;
-	unsigned tries = 0;
-
-	d_fnstart(3, dev, "(func %p)\n", func);
-	/* Setup timeout (FIXME: This needs to read the CIS table to
-	 * get a real timeout) and then wait for the device to signal
-	 * it is ready */
-	timeout = get_jiffies_64() + ioe_timeout * HZ;
-	err = -ENODEV;
-	while (err != 0 && time_before64(get_jiffies_64(), timeout)) {
-		sdio_claim_host(func);
-		/*
-		 * There is a sillicon bug on the IWMC3200, where the
-		 * IOE timeout will cause problems on Moorestown
-		 * platforms (system hang). We explicitly overwrite
-		 * func->enable_timeout here to work around the issue.
-		 */
-		if (i2400ms->iwmc3200)
-			func->enable_timeout = IWMC3200_IOR_TIMEOUT;
-		err = sdio_enable_func(func);
-		if (0 == err) {
-			sdio_release_host(func);
-			d_printf(2, dev, "SDIO function enabled\n");
-			goto function_enabled;
-		}
-		d_printf(2, dev, "SDIO function failed to enable: %d\n", err);
-		sdio_release_host(func);
-		if (maxtries > 0 && ++tries >= maxtries) {
-			err = -ETIME;
-			break;
-		}
-		msleep(I2400MS_INIT_SLEEP_INTERVAL);
-	}
-	/* If timed out, device is not there yet -- get -ENODEV so
-	 * the device driver core will retry later on. */
-	if (err == -ETIME) {
-		dev_err(dev, "Can't enable WiMAX function; "
-			" has the function been enabled?\n");
-		err = -ENODEV;
-	}
-function_enabled:
-	d_fnend(3, dev, "(func %p) = %d\n", func, err);
-	return err;
-}
-
-
-/*
- * Setup minimal device communication infrastructure needed to at
- * least be able to update the firmware.
- *
- * Note the ugly trick: if we are in the probe path
- * (i2400ms->debugfs_dentry == NULL), we only retry function
- * enablement one, to avoid racing with the iwmc3200 top controller.
- */
-static
-int i2400ms_bus_setup(struct i2400m *i2400m)
-{
-	int result;
-	struct i2400ms *i2400ms =
-		container_of(i2400m, struct i2400ms, i2400m);
-	struct device *dev = i2400m_dev(i2400m);
-	struct sdio_func *func = i2400ms->func;
-	int retries;
-
-	sdio_claim_host(func);
-	result = sdio_set_block_size(func, I2400MS_BLK_SIZE);
-	sdio_release_host(func);
-	if (result < 0) {
-		dev_err(dev, "Failed to set block size: %d\n", result);
-		goto error_set_blk_size;
-	}
-
-	if (i2400ms->iwmc3200 && i2400ms->debugfs_dentry == NULL)
-		retries = 1;
-	else
-		retries = 0;
-	result = i2400ms_enable_function(i2400ms, retries);
-	if (result < 0) {
-		dev_err(dev, "Cannot enable SDIO function: %d\n", result);
-		goto error_func_enable;
-	}
-
-	result = i2400ms_tx_setup(i2400ms);
-	if (result < 0)
-		goto error_tx_setup;
-	result = i2400ms_rx_setup(i2400ms);
-	if (result < 0)
-		goto error_rx_setup;
-	return 0;
-
-error_rx_setup:
-	i2400ms_tx_release(i2400ms);
-error_tx_setup:
-	sdio_claim_host(func);
-	sdio_disable_func(func);
-	sdio_release_host(func);
-error_func_enable:
-error_set_blk_size:
-	return result;
-}
-
-
-/*
- * Tear down minimal device communication infrastructure needed to at
- * least be able to update the firmware.
- */
-static
-void i2400ms_bus_release(struct i2400m *i2400m)
-{
-	struct i2400ms *i2400ms =
-		container_of(i2400m, struct i2400ms, i2400m);
-	struct sdio_func *func = i2400ms->func;
-
-	i2400ms_rx_release(i2400ms);
-	i2400ms_tx_release(i2400ms);
-	sdio_claim_host(func);
-	sdio_disable_func(func);
-	sdio_release_host(func);
-}
-
-
-/*
- * Setup driver resources needed to communicate with the device
- *
- * The fw needs some time to settle, and it was just uploaded,
- * so give it a break first. I'd prefer to just wait for the device to
- * send something, but seems the poking we do to enable SDIO stuff
- * interferes with it, so just give it a break before starting...
- */
-static
-int i2400ms_bus_dev_start(struct i2400m *i2400m)
-{
-	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-
-	d_fnstart(3, dev, "(i2400m %p)\n", i2400m);
-	msleep(200);
-	d_fnend(3, dev, "(i2400m %p) = %d\n", i2400m, 0);
-	return 0;
-}
-
-
-/*
- * Sends a barker buffer to the device
- *
- * This helper will allocate a kmalloced buffer and use it to transmit
- * (then free it). Reason for this is that the SDIO host controller
- * expects alignment (unknown exactly which) which the stack won't
- * really provide and certain arches/host-controller combinations
- * cannot use stack/vmalloc/text areas for DMA transfers.
- */
-static
-int __i2400ms_send_barker(struct i2400ms *i2400ms,
-			  const __le32 *barker, size_t barker_size)
-{
-	int  ret;
-	struct sdio_func *func = i2400ms->func;
-	struct device *dev = &func->dev;
-	void *buffer;
-
-	ret = -ENOMEM;
-	buffer = kmalloc(I2400MS_BLK_SIZE, GFP_KERNEL);
-	if (buffer == NULL)
-		goto error_kzalloc;
-
-	memcpy(buffer, barker, barker_size);
-	sdio_claim_host(func);
-	ret = sdio_memcpy_toio(func, 0, buffer, I2400MS_BLK_SIZE);
-	sdio_release_host(func);
-
-	if (ret < 0)
-		d_printf(0, dev, "E: barker error: %d\n", ret);
-
-	kfree(buffer);
-error_kzalloc:
-	return ret;
-}
-
-
-/*
- * Reset a device at different levels (warm, cold or bus)
- *
- * @i2400ms: device descriptor
- * @reset_type: soft, warm or bus reset (I2400M_RT_WARM/SOFT/BUS)
- *
- * FIXME: not tested -- need to confirm expected effects
- *
- * Warm and cold resets get an SDIO reset if they fail (unimplemented)
- *
- * Warm reset:
- *
- * The device will be fully reset internally, but won't be
- * disconnected from the bus (so no reenumeration will
- * happen). Firmware upload will be necessary.
- *
- * The device will send a reboot barker that will trigger the driver
- * to reinitialize the state via __i2400m_dev_reset_handle.
- *
- *
- * Cold and bus reset:
- *
- * The device will be fully reset internally, disconnected from the
- * bus an a reenumeration will happen. Firmware upload will be
- * necessary. Thus, we don't do any locking or struct
- * reinitialization, as we are going to be fully disconnected and
- * reenumerated.
- *
- * Note we need to return -ENODEV if a warm reset was requested and we
- * had to resort to a bus reset. See i2400m_op_reset(), wimax_reset()
- * and wimax_dev->op_reset.
- *
- * WARNING: no driver state saved/fixed
- */
-static
-int i2400ms_bus_reset(struct i2400m *i2400m, enum i2400m_reset_type rt)
-{
-	int result = 0;
-	struct i2400ms *i2400ms =
-		container_of(i2400m, struct i2400ms, i2400m);
-	struct device *dev = i2400m_dev(i2400m);
-	static const __le32 i2400m_WARM_BOOT_BARKER[4] = {
-		cpu_to_le32(I2400M_WARM_RESET_BARKER),
-		cpu_to_le32(I2400M_WARM_RESET_BARKER),
-		cpu_to_le32(I2400M_WARM_RESET_BARKER),
-		cpu_to_le32(I2400M_WARM_RESET_BARKER),
-	};
-	static const __le32 i2400m_COLD_BOOT_BARKER[4] = {
-		cpu_to_le32(I2400M_COLD_RESET_BARKER),
-		cpu_to_le32(I2400M_COLD_RESET_BARKER),
-		cpu_to_le32(I2400M_COLD_RESET_BARKER),
-		cpu_to_le32(I2400M_COLD_RESET_BARKER),
-	};
-
-	if (rt == I2400M_RT_WARM)
-		result = __i2400ms_send_barker(i2400ms, i2400m_WARM_BOOT_BARKER,
-					       sizeof(i2400m_WARM_BOOT_BARKER));
-	else if (rt == I2400M_RT_COLD)
-		result = __i2400ms_send_barker(i2400ms, i2400m_COLD_BOOT_BARKER,
-					       sizeof(i2400m_COLD_BOOT_BARKER));
-	else if (rt == I2400M_RT_BUS) {
-do_bus_reset:
-
-		i2400ms_bus_release(i2400m);
-
-		/* Wait for the device to settle */
-		msleep(40);
-
-		result =  i2400ms_bus_setup(i2400m);
-	} else
-		BUG();
-	if (result < 0 && rt != I2400M_RT_BUS) {
-		dev_err(dev, "%s reset failed (%d); trying SDIO reset\n",
-			rt == I2400M_RT_WARM ? "warm" : "cold", result);
-		rt = I2400M_RT_BUS;
-		goto do_bus_reset;
-	}
-	return result;
-}
-
-
-static
-void i2400ms_netdev_setup(struct net_device *net_dev)
-{
-	struct i2400m *i2400m = net_dev_to_i2400m(net_dev);
-	struct i2400ms *i2400ms = container_of(i2400m, struct i2400ms, i2400m);
-	i2400ms_init(i2400ms);
-	i2400m_netdev_setup(net_dev);
-}
-
-
-/*
- * Debug levels control; see debug.h
- */
-struct d_level D_LEVEL[] = {
-	D_SUBMODULE_DEFINE(main),
-	D_SUBMODULE_DEFINE(tx),
-	D_SUBMODULE_DEFINE(rx),
-	D_SUBMODULE_DEFINE(fw),
-};
-size_t D_LEVEL_SIZE = ARRAY_SIZE(D_LEVEL);
-
-
-#define __debugfs_register(prefix, name, parent)			\
-do {									\
-	result = d_level_register_debugfs(prefix, name, parent);	\
-	if (result < 0)							\
-		goto error;						\
-} while (0)
-
-
-static
-int i2400ms_debugfs_add(struct i2400ms *i2400ms)
-{
-	int result;
-	struct dentry *dentry = i2400ms->i2400m.wimax_dev.debugfs_dentry;
-
-	dentry = debugfs_create_dir("i2400m-sdio", dentry);
-	result = PTR_ERR(dentry);
-	if (IS_ERR(dentry)) {
-		if (result == -ENODEV)
-			result = 0;	/* No debugfs support */
-		goto error;
-	}
-	i2400ms->debugfs_dentry = dentry;
-	__debugfs_register("dl_", main, dentry);
-	__debugfs_register("dl_", tx, dentry);
-	__debugfs_register("dl_", rx, dentry);
-	__debugfs_register("dl_", fw, dentry);
-
-	return 0;
-
-error:
-	debugfs_remove_recursive(i2400ms->debugfs_dentry);
-	i2400ms->debugfs_dentry = NULL;
-	return result;
-}
-
-
-static struct device_type i2400ms_type = {
-	.name	= "wimax",
-};
-
-/*
- * Probe a i2400m interface and register it
- *
- * @func:    SDIO function
- * @id:      SDIO device ID
- * @returns: 0 if ok, < 0 errno code on error.
- *
- * Alloc a net device, initialize the bus-specific details and then
- * calls the bus-generic initialization routine. That will register
- * the wimax and netdev devices, upload the firmware [using
- * _bus_bm_*()], call _bus_dev_start() to finalize the setup of the
- * communication with the device and then will start to talk to it to
- * finnish setting it up.
- *
- * Initialization is tricky; some instances of the hw are packed with
- * others in a way that requires a third driver that enables the WiMAX
- * function. In those cases, we can't enable the SDIO function and
- * we'll return with -ENODEV. When the driver that enables the WiMAX
- * function does its thing, it has to do a bus_rescan_devices() on the
- * SDIO bus so this driver is called again to enumerate the WiMAX
- * function.
- */
-static
-int i2400ms_probe(struct sdio_func *func,
-		  const struct sdio_device_id *id)
-{
-	int result;
-	struct net_device *net_dev;
-	struct device *dev = &func->dev;
-	struct i2400m *i2400m;
-	struct i2400ms *i2400ms;
-
-	/* Allocate instance [calls i2400m_netdev_setup() on it]. */
-	result = -ENOMEM;
-	net_dev = alloc_netdev(sizeof(*i2400ms), "wmx%d",
-			       i2400ms_netdev_setup);
-	if (net_dev == NULL) {
-		dev_err(dev, "no memory for network device instance\n");
-		goto error_alloc_netdev;
-	}
-	SET_NETDEV_DEV(net_dev, dev);
-	SET_NETDEV_DEVTYPE(net_dev, &i2400ms_type);
-	i2400m = net_dev_to_i2400m(net_dev);
-	i2400ms = container_of(i2400m, struct i2400ms, i2400m);
-	i2400m->wimax_dev.net_dev = net_dev;
-	i2400ms->func = func;
-	sdio_set_drvdata(func, i2400ms);
-
-	i2400m->bus_tx_block_size = I2400MS_BLK_SIZE;
-	/*
-	 * Room required in the TX queue for SDIO message to accommodate
-	 * a smallest payload while allocating header space is 224 bytes,
-	 * which is the smallest message size(the block size 256 bytes)
-	 * minus the smallest message header size(32 bytes).
-	 */
-	i2400m->bus_tx_room_min = I2400MS_BLK_SIZE - I2400M_PL_ALIGN * 2;
-	i2400m->bus_pl_size_max = I2400MS_PL_SIZE_MAX;
-	i2400m->bus_setup = i2400ms_bus_setup;
-	i2400m->bus_dev_start = i2400ms_bus_dev_start;
-	i2400m->bus_dev_stop = NULL;
-	i2400m->bus_release = i2400ms_bus_release;
-	i2400m->bus_tx_kick = i2400ms_bus_tx_kick;
-	i2400m->bus_reset = i2400ms_bus_reset;
-	/* The iwmc3200-wimax sometimes requires the driver to try
-	 * hard when we paint it into a corner. */
-	i2400m->bus_bm_retries = I2400M_SDIO_BOOT_RETRIES;
-	i2400m->bus_bm_cmd_send = i2400ms_bus_bm_cmd_send;
-	i2400m->bus_bm_wait_for_ack = i2400ms_bus_bm_wait_for_ack;
-	i2400m->bus_fw_names = i2400ms_bus_fw_names;
-	i2400m->bus_bm_mac_addr_impaired = 1;
-	i2400m->bus_bm_pokes_table = &i2400ms_pokes[0];
-
-	switch (func->device) {
-	case SDIO_DEVICE_ID_INTEL_IWMC3200WIMAX:
-	case SDIO_DEVICE_ID_INTEL_IWMC3200WIMAX_2G5:
-		i2400ms->iwmc3200 = 1;
-		break;
-	default:
-		i2400ms->iwmc3200 = 0;
-	}
-
-	result = i2400m_setup(i2400m, I2400M_BRI_NO_REBOOT);
-	if (result < 0) {
-		dev_err(dev, "cannot setup device: %d\n", result);
-		goto error_setup;
-	}
-
-	result = i2400ms_debugfs_add(i2400ms);
-	if (result < 0) {
-		dev_err(dev, "cannot create SDIO debugfs: %d\n",
-			result);
-		goto error_debugfs_add;
-	}
-	return 0;
-
-error_debugfs_add:
-	i2400m_release(i2400m);
-error_setup:
-	sdio_set_drvdata(func, NULL);
-	free_netdev(net_dev);
-error_alloc_netdev:
-	return result;
-}
-
-
-static
-void i2400ms_remove(struct sdio_func *func)
-{
-	struct device *dev = &func->dev;
-	struct i2400ms *i2400ms = sdio_get_drvdata(func);
-	struct i2400m *i2400m = &i2400ms->i2400m;
-	struct net_device *net_dev = i2400m->wimax_dev.net_dev;
-
-	d_fnstart(3, dev, "SDIO func %p\n", func);
-	debugfs_remove_recursive(i2400ms->debugfs_dentry);
-	i2400ms->debugfs_dentry = NULL;
-	i2400m_release(i2400m);
-	sdio_set_drvdata(func, NULL);
-	free_netdev(net_dev);
-	d_fnend(3, dev, "SDIO func %p\n", func);
-}
-
-static
-const struct sdio_device_id i2400ms_sdio_ids[] = {
-	/* Intel: i2400m WiMAX (iwmc3200) over SDIO */
-	{ SDIO_DEVICE(SDIO_VENDOR_ID_INTEL,
-		      SDIO_DEVICE_ID_INTEL_IWMC3200WIMAX) },
-	{ SDIO_DEVICE(SDIO_VENDOR_ID_INTEL,
-		      SDIO_DEVICE_ID_INTEL_IWMC3200WIMAX_2G5) },
-	{ /* end: all zeroes */ },
-};
-MODULE_DEVICE_TABLE(sdio, i2400ms_sdio_ids);
-
-
-static
-struct sdio_driver i2400m_sdio_driver = {
-	.name		= KBUILD_MODNAME,
-	.probe		= i2400ms_probe,
-	.remove		= i2400ms_remove,
-	.id_table	= i2400ms_sdio_ids,
-};
-
-
-static
-int __init i2400ms_driver_init(void)
-{
-	d_parse_params(D_LEVEL, D_LEVEL_SIZE, i2400ms_debug_params,
-		       "i2400m_sdio.debug");
-	return sdio_register_driver(&i2400m_sdio_driver);
-}
-module_init(i2400ms_driver_init);
-
-
-static
-void __exit i2400ms_driver_exit(void)
-{
-	sdio_unregister_driver(&i2400m_sdio_driver);
-}
-module_exit(i2400ms_driver_exit);
-
-
-MODULE_AUTHOR("Intel Corporation <linux-wimax@intel.com>");
-MODULE_DESCRIPTION("Intel 2400M WiMAX networking for SDIO");
-MODULE_LICENSE("GPL");
-MODULE_FIRMWARE(I2400MS_FW_FILE_NAME);
diff --git a/drivers/net/wimax/i2400m/usb-fw.c b/drivers/net/wimax/i2400m/usb-fw.c
index 1fda46c55eb..e74664b8492 100644
--- a/drivers/net/wimax/i2400m/usb-fw.c
+++ b/drivers/net/wimax/i2400m/usb-fw.c
@@ -212,7 +212,7 @@ ssize_t i2400mu_bus_bm_cmd_send(struct i2400m *i2400m,
 	}
 	if (result != cmd_size) {		/* all was transferred? */
 		dev_err(dev, "boot-mode cmd %d: incomplete transfer "
-			"(%zu vs %zu submitted)\n",  opcode, result, cmd_size);
+			"(%zd vs %zu submitted)\n",  opcode, result, cmd_size);
 		result = -EIO;
 		goto error_cmd_size;
 	}
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index 5f58fa53238..6deaae18db5 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -276,7 +276,6 @@ 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"
 source "drivers/net/wireless/p54/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 0ce218b931d..062dfdff636 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -53,8 +53,6 @@ obj-$(CONFIG_MAC80211_HWSIM)	+= mac80211_hwsim.o
 
 obj-$(CONFIG_WL_TI)	+= ti/
 
-obj-$(CONFIG_IWM)	+= iwmc3200wifi/
-
 obj-$(CONFIG_MWIFIEX)	+= mwifiex/
 
 obj-$(CONFIG_BRCMFMAC)	+= brcm80211/
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index 0ac09a2bd14..689a71c1af7 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -1738,8 +1738,7 @@ static int adm8211_alloc_rings(struct ieee80211_hw *dev)
 		return -ENOMEM;
 	}
 
-	priv->tx_ring = (struct adm8211_desc *)(priv->rx_ring +
-						priv->rx_ring_size);
+	priv->tx_ring = priv->rx_ring + priv->rx_ring_size;
 	priv->tx_ring_dma = priv->rx_ring_dma +
 			    sizeof(struct adm8211_desc) * priv->rx_ring_size;
 
@@ -1855,7 +1854,7 @@ static int __devinit adm8211_probe(struct pci_dev *pdev,
 	if (!is_valid_ether_addr(perm_addr)) {
 		printk(KERN_WARNING "%s (adm8211): Invalid hwaddr in EEPROM!\n",
 		       pci_name(pdev));
-		random_ether_addr(perm_addr);
+		eth_random_addr(perm_addr);
 	}
 	SET_IEEE80211_PERM_ADDR(dev, perm_addr);
 
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index a747c632597..f9f15bb3f03 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -1997,7 +1997,7 @@ static int mpi_send_packet (struct net_device *dev)
  *                         ------------------------------------------------
  */
 
-	memcpy((char *)ai->txfids[0].virtual_host_addr,
+	memcpy(ai->txfids[0].virtual_host_addr,
 		(char *)&wifictlhdr8023, sizeof(wifictlhdr8023));
 
 	payloadLen = (__le16 *)(ai->txfids[0].virtual_host_addr +
@@ -4212,7 +4212,7 @@ static int PC4500_writerid(struct airo_info *ai, u16 rid,
 			airo_print_err(ai->dev->name, "%s: len=%d", __func__, len);
 			rc = -1;
 		} else {
-			memcpy((char *)ai->config_desc.virtual_host_addr,
+			memcpy(ai->config_desc.virtual_host_addr,
 				pBuf, len);
 
 			rc = issuecommand(ai, &cmd, &rsp);
diff --git a/drivers/net/wireless/ath/ath.h b/drivers/net/wireless/ath/ath.h
index 420d69b2674..6169fbd23ed 100644
--- a/drivers/net/wireless/ath/ath.h
+++ b/drivers/net/wireless/ath/ath.h
@@ -216,6 +216,7 @@ void ath_printk(const char *level, const struct ath_common *common,
  *	used exclusively for WLAN-BT coexistence starting from
  *	AR9462.
  * @ATH_DBG_DFS: radar datection
+ * @ATH_DBG_WOW: Wake on Wireless
  * @ATH_DBG_ANY: enable all debugging
  *
  * The debug level is used to control the amount and type of debugging output
@@ -243,6 +244,7 @@ enum ATH_DEBUG {
 	ATH_DBG_BSTUCK		= 0x00008000,
 	ATH_DBG_MCI		= 0x00010000,
 	ATH_DBG_DFS		= 0x00020000,
+	ATH_DBG_WOW		= 0x00040000,
 	ATH_DBG_ANY		= 0xffffffff
 };
 
diff --git a/drivers/net/wireless/ath/ath5k/Kconfig b/drivers/net/wireless/ath/ath5k/Kconfig
index e18a9aa7b6c..338c5c42357 100644
--- a/drivers/net/wireless/ath/ath5k/Kconfig
+++ b/drivers/net/wireless/ath/ath5k/Kconfig
@@ -64,3 +64,11 @@ config ATH5K_PCI
 	---help---
 	  This adds support for PCI type chipsets of the 5xxx Atheros
 	  family.
+
+config ATH5K_TEST_CHANNELS
+	bool "Enables testing channels on ath5k"
+	depends on ATH5K && CFG80211_CERTIFICATION_ONUS
+	---help---
+	  This enables non-standard IEEE 802.11 channels on ath5k, which
+	  can be used for research purposes. This option should be disabled
+	  unless doing research.
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 44ad6fe0278..8c4c040a47b 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -74,10 +74,6 @@ bool ath5k_modparam_nohwcrypt;
 module_param_named(nohwcrypt, ath5k_modparam_nohwcrypt, bool, S_IRUGO);
 MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
 
-static bool modparam_all_channels;
-module_param_named(all_channels, modparam_all_channels, bool, S_IRUGO);
-MODULE_PARM_DESC(all_channels, "Expose all channels the device can use.");
-
 static bool modparam_fastchanswitch;
 module_param_named(fastchanswitch, modparam_fastchanswitch, bool, S_IRUGO);
 MODULE_PARM_DESC(fastchanswitch, "Enable fast channel switching for AR2413/AR5413 radios.");
@@ -258,8 +254,15 @@ static int ath5k_reg_notifier(struct wiphy *wiphy, struct regulatory_request *re
 \********************/
 
 /*
- * Returns true for the channel numbers used without all_channels modparam.
+ * Returns true for the channel numbers used.
  */
+#ifdef CONFIG_ATH5K_TEST_CHANNELS
+static bool ath5k_is_standard_channel(short chan, enum ieee80211_band band)
+{
+	return true;
+}
+
+#else
 static bool ath5k_is_standard_channel(short chan, enum ieee80211_band band)
 {
 	if (band == IEEE80211_BAND_2GHZ && chan <= 14)
@@ -276,6 +279,7 @@ static bool ath5k_is_standard_channel(short chan, enum ieee80211_band band)
 		/* 802.11j 4.9GHz (20MHz) */
 		(chan == 184 || chan == 188 || chan == 192 || chan == 196));
 }
+#endif
 
 static unsigned int
 ath5k_setup_channels(struct ath5k_hw *ah, struct ieee80211_channel *channels,
@@ -316,8 +320,7 @@ ath5k_setup_channels(struct ath5k_hw *ah, struct ieee80211_channel *channels,
 		if (!ath5k_channel_ok(ah, &channels[count]))
 			continue;
 
-		if (!modparam_all_channels &&
-		    !ath5k_is_standard_channel(ch, band))
+		if (!ath5k_is_standard_channel(ch, band))
 			continue;
 
 		count++;
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index 22b80af0f47..260e7dc7f75 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -594,7 +594,7 @@ ath5k_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif, u16 queue,
 	qi.tqi_aifs = params->aifs;
 	qi.tqi_cw_min = params->cw_min;
 	qi.tqi_cw_max = params->cw_max;
-	qi.tqi_burst_time = params->txop;
+	qi.tqi_burst_time = params->txop * 32;
 
 	ATH5K_DBG(ah, ATH5K_DEBUG_ANY,
 		  "Configure tx [queue %d],  "
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index b869a358ce4..86aeef4b9d7 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -53,6 +53,11 @@
 
 #define DEFAULT_BG_SCAN_PERIOD 60
 
+struct ath6kl_cfg80211_match_probe_ssid {
+	struct cfg80211_ssid ssid;
+	u8 flag;
+};
+
 static struct ieee80211_rate ath6kl_rates[] = {
 	RATETAB_ENT(10, 0x1, 0),
 	RATETAB_ENT(20, 0x2, 0),
@@ -576,6 +581,9 @@ static int ath6kl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 
 	vif->nw_type = vif->next_mode;
 
+	/* enable enhanced bmiss detection if applicable */
+	ath6kl_cfg80211_sta_bmiss_enhance(vif, true);
+
 	if (vif->wdev.iftype == NL80211_IFTYPE_P2P_CLIENT)
 		nw_subtype = SUBTYPE_P2PCLIENT;
 
@@ -852,20 +860,6 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
 		}
 	}
 
-	/*
-	 * Send a disconnect command to target when a disconnect event is
-	 * received with reason code other than 3 (DISCONNECT_CMD - disconnect
-	 * request from host) to make the firmware stop trying to connect even
-	 * after giving disconnect event. There will be one more disconnect
-	 * event for this disconnect command with reason code DISCONNECT_CMD
-	 * which will be notified to cfg80211.
-	 */
-
-	if (reason != DISCONNECT_CMD) {
-		ath6kl_wmi_disconnect_cmd(ar->wmi, vif->fw_vif_idx);
-		return;
-	}
-
 	clear_bit(CONNECT_PEND, &vif->flags);
 
 	if (vif->sme_state == SME_CONNECTING) {
@@ -875,32 +869,96 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
 					WLAN_STATUS_UNSPECIFIED_FAILURE,
 					GFP_KERNEL);
 	} else if (vif->sme_state == SME_CONNECTED) {
-		cfg80211_disconnected(vif->ndev, reason,
+		cfg80211_disconnected(vif->ndev, proto_reason,
 				      NULL, 0, GFP_KERNEL);
 	}
 
 	vif->sme_state = SME_DISCONNECTED;
+
+	/*
+	 * Send a disconnect command to target when a disconnect event is
+	 * received with reason code other than 3 (DISCONNECT_CMD - disconnect
+	 * request from host) to make the firmware stop trying to connect even
+	 * after giving disconnect event. There will be one more disconnect
+	 * event for this disconnect command with reason code DISCONNECT_CMD
+	 * which won't be notified to cfg80211.
+	 */
+	if (reason != DISCONNECT_CMD)
+		ath6kl_wmi_disconnect_cmd(ar->wmi, vif->fw_vif_idx);
 }
 
 static int ath6kl_set_probed_ssids(struct ath6kl *ar,
 				   struct ath6kl_vif *vif,
-				   struct cfg80211_ssid *ssids, int n_ssids)
+				   struct cfg80211_ssid *ssids, int n_ssids,
+				   struct cfg80211_match_set *match_set,
+				   int n_match_ssid)
 {
-	u8 i;
+	u8 i, j, index_to_add, ssid_found = false;
+	struct ath6kl_cfg80211_match_probe_ssid ssid_list[MAX_PROBED_SSIDS];
+
+	memset(ssid_list, 0, sizeof(ssid_list));
 
-	if (n_ssids > MAX_PROBED_SSID_INDEX)
+	if (n_ssids > MAX_PROBED_SSIDS ||
+	    n_match_ssid > MAX_PROBED_SSIDS)
 		return -EINVAL;
 
 	for (i = 0; i < n_ssids; i++) {
+		memcpy(ssid_list[i].ssid.ssid,
+		       ssids[i].ssid,
+		       ssids[i].ssid_len);
+		ssid_list[i].ssid.ssid_len = ssids[i].ssid_len;
+
+		if (ssids[i].ssid_len)
+			ssid_list[i].flag = SPECIFIC_SSID_FLAG;
+		else
+			ssid_list[i].flag = ANY_SSID_FLAG;
+
+		if (n_match_ssid == 0)
+			ssid_list[i].flag |= MATCH_SSID_FLAG;
+	}
+
+	index_to_add = i;
+
+	for (i = 0; i < n_match_ssid; i++) {
+		ssid_found = false;
+
+		for (j = 0; j < n_ssids; j++) {
+			if ((match_set[i].ssid.ssid_len ==
+			     ssid_list[j].ssid.ssid_len) &&
+			    (!memcmp(ssid_list[j].ssid.ssid,
+				     match_set[i].ssid.ssid,
+				     match_set[i].ssid.ssid_len))) {
+				ssid_list[j].flag |= MATCH_SSID_FLAG;
+				ssid_found = true;
+				break;
+			}
+		}
+
+		if (ssid_found)
+			continue;
+
+		if (index_to_add >= MAX_PROBED_SSIDS)
+			continue;
+
+		ssid_list[index_to_add].ssid.ssid_len =
+			match_set[i].ssid.ssid_len;
+		memcpy(ssid_list[index_to_add].ssid.ssid,
+		       match_set[i].ssid.ssid,
+		       match_set[i].ssid.ssid_len);
+		ssid_list[index_to_add].flag |= MATCH_SSID_FLAG;
+		index_to_add++;
+	}
+
+	for (i = 0; i < index_to_add; i++) {
 		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
-					  ssids[i].ssid_len ?
-					  SPECIFIC_SSID_FLAG : ANY_SSID_FLAG,
-					  ssids[i].ssid_len,
-					  ssids[i].ssid);
+					  ssid_list[i].flag,
+					  ssid_list[i].ssid.ssid_len,
+					  ssid_list[i].ssid.ssid);
+
 	}
 
 	/* Make sure no old entries are left behind */
-	for (i = n_ssids; i < MAX_PROBED_SSID_INDEX; i++) {
+	for (i = index_to_add; i < MAX_PROBED_SSIDS; i++) {
 		ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
 					  DISABLE_SSID_FLAG, 0, NULL);
 	}
@@ -908,11 +966,11 @@ static int ath6kl_set_probed_ssids(struct ath6kl *ar,
 	return 0;
 }
 
-static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
+static int ath6kl_cfg80211_scan(struct wiphy *wiphy,
 				struct cfg80211_scan_request *request)
 {
-	struct ath6kl *ar = ath6kl_priv(ndev);
-	struct ath6kl_vif *vif = netdev_priv(ndev);
+	struct ath6kl_vif *vif = ath6kl_vif_from_wdev(request->wdev);
+	struct ath6kl *ar = ath6kl_priv(vif->ndev);
 	s8 n_channels = 0;
 	u16 *channels = NULL;
 	int ret = 0;
@@ -934,7 +992,7 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	}
 
 	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
-				      request->n_ssids);
+				      request->n_ssids, NULL, 0);
 	if (ret < 0)
 		return ret;
 
@@ -943,7 +1001,7 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 				       WMI_FRAME_PROBE_REQ,
 				       request->ie, request->ie_len);
 	if (ret) {
-		ath6kl_err("failed to set Probe Request appie for scan");
+		ath6kl_err("failed to set Probe Request appie for scan\n");
 		return ret;
 	}
 
@@ -1429,14 +1487,14 @@ static int ath6kl_cfg80211_set_power_mgmt(struct wiphy *wiphy,
 	return 0;
 }
 
-static struct net_device *ath6kl_cfg80211_add_iface(struct wiphy *wiphy,
-						    char *name,
-						    enum nl80211_iftype type,
-						    u32 *flags,
-						    struct vif_params *params)
+static struct wireless_dev *ath6kl_cfg80211_add_iface(struct wiphy *wiphy,
+						      char *name,
+						      enum nl80211_iftype type,
+						      u32 *flags,
+						      struct vif_params *params)
 {
 	struct ath6kl *ar = wiphy_priv(wiphy);
-	struct net_device *ndev;
+	struct wireless_dev *wdev;
 	u8 if_idx, nw_type;
 
 	if (ar->num_vif == ar->vif_max) {
@@ -1449,20 +1507,20 @@ static struct net_device *ath6kl_cfg80211_add_iface(struct wiphy *wiphy,
 		return ERR_PTR(-EINVAL);
 	}
 
-	ndev = ath6kl_interface_add(ar, name, type, if_idx, nw_type);
-	if (!ndev)
+	wdev = ath6kl_interface_add(ar, name, type, if_idx, nw_type);
+	if (!wdev)
 		return ERR_PTR(-ENOMEM);
 
 	ar->num_vif++;
 
-	return ndev;
+	return wdev;
 }
 
 static int ath6kl_cfg80211_del_iface(struct wiphy *wiphy,
-				     struct net_device *ndev)
+				     struct wireless_dev *wdev)
 {
 	struct ath6kl *ar = wiphy_priv(wiphy);
-	struct ath6kl_vif *vif = netdev_priv(ndev);
+	struct ath6kl_vif *vif = netdev_priv(wdev->netdev);
 
 	spin_lock_bh(&ar->list_lock);
 	list_del(&vif->list);
@@ -1512,6 +1570,9 @@ static int ath6kl_cfg80211_change_iface(struct wiphy *wiphy,
 		}
 	}
 
+	/* need to clean up enhanced bmiss detection fw state */
+	ath6kl_cfg80211_sta_bmiss_enhance(vif, false);
+
 set_iface_type:
 	switch (type) {
 	case NL80211_IFTYPE_STATION:
@@ -2074,7 +2135,9 @@ static int ath6kl_wow_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
 	if (wow && (wow->n_patterns > WOW_MAX_FILTERS_PER_LIST))
 		return -EINVAL;
 
-	if (!test_bit(NETDEV_MCAST_ALL_ON, &vif->flags)) {
+	if (!test_bit(NETDEV_MCAST_ALL_ON, &vif->flags) &&
+	    test_bit(ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+		     ar->fw_capabilities)) {
 		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
 						vif->fw_vif_idx, false);
 		if (ret)
@@ -2209,7 +2272,9 @@ static int ath6kl_wow_resume(struct ath6kl *ar)
 
 	ar->state = ATH6KL_STATE_ON;
 
-	if (!test_bit(NETDEV_MCAST_ALL_OFF, &vif->flags)) {
+	if (!test_bit(NETDEV_MCAST_ALL_OFF, &vif->flags) &&
+	    test_bit(ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+		     ar->fw_capabilities)) {
 		ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
 					vif->fw_vif_idx, true);
 		if (ret)
@@ -2475,7 +2540,7 @@ void ath6kl_check_wow_status(struct ath6kl *ar)
 static int ath6kl_set_htcap(struct ath6kl_vif *vif, enum ieee80211_band band,
 			    bool ht_enable)
 {
-	struct ath6kl_htcap *htcap = &vif->htcap;
+	struct ath6kl_htcap *htcap = &vif->htcap[band];
 
 	if (htcap->ht_enable == ht_enable)
 		return 0;
@@ -2585,33 +2650,28 @@ static int ath6kl_set_ies(struct ath6kl_vif *vif,
 	return 0;
 }
 
-static int ath6kl_set_channel(struct wiphy *wiphy, struct net_device *dev,
-			      struct ieee80211_channel *chan,
-			      enum nl80211_channel_type channel_type)
+void ath6kl_cfg80211_sta_bmiss_enhance(struct ath6kl_vif *vif, bool enable)
 {
-	struct ath6kl_vif *vif;
+	int err;
 
-	/*
-	 * 'dev' could be NULL if a channel change is required for the hardware
-	 * device itself, instead of a particular VIF.
-	 *
-	 * FIXME: To be handled properly when monitor mode is supported.
-	 */
-	if (!dev)
-		return -EBUSY;
+	if (WARN_ON(!test_bit(WMI_READY, &vif->ar->flag)))
+		return;
 
-	vif = netdev_priv(dev);
+	if (vif->nw_type != INFRA_NETWORK)
+		return;
 
-	if (!ath6kl_cfg80211_ready(vif))
-		return -EIO;
+	if (!test_bit(ATH6KL_FW_CAPABILITY_BMISS_ENHANCE,
+		      vif->ar->fw_capabilities))
+		return;
 
-	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s: center_freq=%u hw_value=%u\n",
-		   __func__, chan->center_freq, chan->hw_value);
-	vif->next_chan = chan->center_freq;
-	vif->next_ch_type = channel_type;
-	vif->next_ch_band = chan->band;
+	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s fw bmiss enhance\n",
+		   enable ? "enable" : "disable");
 
-	return 0;
+	err = ath6kl_wmi_sta_bmiss_enhance_cmd(vif->ar->wmi,
+					       vif->fw_vif_idx, enable);
+	if (err)
+		ath6kl_err("failed to %s enhanced bmiss detection: %d\n",
+			   enable ? "enable" : "disable", err);
 }
 
 static int ath6kl_get_rsn_capab(struct cfg80211_beacon_data *beacon,
@@ -2694,9 +2754,15 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 
 	/* TODO:
 	 * info->interval
-	 * info->dtim_period
 	 */
 
+	ret = ath6kl_wmi_ap_set_dtim_cmd(ar->wmi, vif->fw_vif_idx,
+					 info->dtim_period);
+
+	/* ignore error, just print a warning and continue normally */
+	if (ret)
+		ath6kl_warn("Failed to set dtim_period in beacon: %d\n", ret);
+
 	if (info->beacon.head == NULL)
 		return -EINVAL;
 	mgmt = (struct ieee80211_mgmt *) info->beacon.head;
@@ -2791,7 +2857,7 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 	p.ssid_len = vif->ssid_len;
 	memcpy(p.ssid, vif->ssid, vif->ssid_len);
 	p.dot11_auth_mode = vif->dot11_auth_mode;
-	p.ch = cpu_to_le16(vif->next_chan);
+	p.ch = cpu_to_le16(info->channel->center_freq);
 
 	/* Enable uAPSD support by default */
 	res = ath6kl_wmi_ap_set_apsd(ar->wmi, vif->fw_vif_idx, true);
@@ -2815,8 +2881,8 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 			return res;
 	}
 
-	if (ath6kl_set_htcap(vif, vif->next_ch_band,
-			     vif->next_ch_type != NL80211_CHAN_NO_HT))
+	if (ath6kl_set_htcap(vif, info->channel->band,
+			     info->channel_type != NL80211_CHAN_NO_HT))
 		return -EIO;
 
 	/*
@@ -2909,14 +2975,14 @@ static int ath6kl_change_station(struct wiphy *wiphy, struct net_device *dev,
 }
 
 static int ath6kl_remain_on_channel(struct wiphy *wiphy,
-				    struct net_device *dev,
+				    struct wireless_dev *wdev,
 				    struct ieee80211_channel *chan,
 				    enum nl80211_channel_type channel_type,
 				    unsigned int duration,
 				    u64 *cookie)
 {
-	struct ath6kl *ar = ath6kl_priv(dev);
-	struct ath6kl_vif *vif = netdev_priv(dev);
+	struct ath6kl_vif *vif = ath6kl_vif_from_wdev(wdev);
+	struct ath6kl *ar = ath6kl_priv(vif->ndev);
 	u32 id;
 
 	/* TODO: if already pending or ongoing remain-on-channel,
@@ -2933,11 +2999,11 @@ static int ath6kl_remain_on_channel(struct wiphy *wiphy,
 }
 
 static int ath6kl_cancel_remain_on_channel(struct wiphy *wiphy,
-					   struct net_device *dev,
+					   struct wireless_dev *wdev,
 					   u64 cookie)
 {
-	struct ath6kl *ar = ath6kl_priv(dev);
-	struct ath6kl_vif *vif = netdev_priv(dev);
+	struct ath6kl_vif *vif = ath6kl_vif_from_wdev(wdev);
+	struct ath6kl *ar = ath6kl_priv(vif->ndev);
 
 	if (cookie != vif->last_roc_id)
 		return -ENOENT;
@@ -3068,15 +3134,15 @@ static bool ath6kl_is_p2p_go_ssid(const u8 *buf, size_t len)
 	return false;
 }
 
-static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
+static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 			  struct ieee80211_channel *chan, bool offchan,
 			  enum nl80211_channel_type channel_type,
 			  bool channel_type_valid, unsigned int wait,
 			  const u8 *buf, size_t len, bool no_cck,
 			  bool dont_wait_for_ack, u64 *cookie)
 {
-	struct ath6kl *ar = ath6kl_priv(dev);
-	struct ath6kl_vif *vif = netdev_priv(dev);
+	struct ath6kl_vif *vif = ath6kl_vif_from_wdev(wdev);
+	struct ath6kl *ar = ath6kl_priv(vif->ndev);
 	u32 id;
 	const struct ieee80211_mgmt *mgmt;
 	bool more_data, queued;
@@ -3121,10 +3187,10 @@ static int ath6kl_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
 }
 
 static void ath6kl_mgmt_frame_register(struct wiphy *wiphy,
-				       struct net_device *dev,
+				       struct wireless_dev *wdev,
 				       u16 frame_type, bool reg)
 {
-	struct ath6kl_vif *vif = netdev_priv(dev);
+	struct ath6kl_vif *vif = ath6kl_vif_from_wdev(wdev);
 
 	ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s: frame_type=0x%x reg=%d\n",
 		   __func__, frame_type, reg);
@@ -3160,10 +3226,24 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 	ath6kl_cfg80211_scan_complete_event(vif, true);
 
 	ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
-				      request->n_ssids);
+				      request->n_ssids,
+				      request->match_sets,
+				      request->n_match_sets);
 	if (ret < 0)
 		return ret;
 
+	if (!request->n_match_sets) {
+		ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
+					       ALL_BSS_FILTER, 0);
+		if (ret < 0)
+			return ret;
+	} else {
+		 ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
+						MATCHED_SSID_FILTER, 0);
+		if (ret < 0)
+			return ret;
+	}
+
 	/* fw uses seconds, also make sure that it's >0 */
 	interval = max_t(u16, 1, request->interval / 1000);
 
@@ -3185,7 +3265,7 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
 				       WMI_FRAME_PROBE_REQ,
 				       request->ie, request->ie_len);
 	if (ret) {
-		ath6kl_warn("Failed to set probe request IE for scheduled scan: %d",
+		ath6kl_warn("Failed to set probe request IE for scheduled scan: %d\n",
 			    ret);
 		return ret;
 	}
@@ -3217,6 +3297,18 @@ static int ath6kl_cfg80211_sscan_stop(struct wiphy *wiphy,
 	return 0;
 }
 
+static int ath6kl_cfg80211_set_bitrate(struct wiphy *wiphy,
+				       struct net_device *dev,
+				       const u8 *addr,
+				       const struct cfg80211_bitrate_mask *mask)
+{
+	struct ath6kl *ar = ath6kl_priv(dev);
+	struct ath6kl_vif *vif = netdev_priv(dev);
+
+	return ath6kl_wmi_set_bitrate_mask(ar->wmi, vif->fw_vif_idx,
+					   mask);
+}
+
 static const struct ieee80211_txrx_stypes
 ath6kl_mgmt_stypes[NUM_NL80211_IFTYPES] = {
 	[NL80211_IFTYPE_STATION] = {
@@ -3271,7 +3363,6 @@ static struct cfg80211_ops ath6kl_cfg80211_ops = {
 	.suspend = __ath6kl_cfg80211_suspend,
 	.resume = __ath6kl_cfg80211_resume,
 #endif
-	.set_channel = ath6kl_set_channel,
 	.start_ap = ath6kl_start_ap,
 	.change_beacon = ath6kl_change_beacon,
 	.stop_ap = ath6kl_stop_ap,
@@ -3283,6 +3374,7 @@ static struct cfg80211_ops ath6kl_cfg80211_ops = {
 	.mgmt_frame_register = ath6kl_mgmt_frame_register,
 	.sched_scan_start = ath6kl_cfg80211_sscan_start,
 	.sched_scan_stop = ath6kl_cfg80211_sscan_stop,
+	.set_bitrate_mask = ath6kl_cfg80211_set_bitrate,
 };
 
 void ath6kl_cfg80211_stop(struct ath6kl_vif *vif)
@@ -3385,9 +3477,9 @@ void ath6kl_cfg80211_vif_cleanup(struct ath6kl_vif *vif)
 	ar->num_vif--;
 }
 
-struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
-					enum nl80211_iftype type, u8 fw_vif_idx,
-					u8 nw_type)
+struct wireless_dev *ath6kl_interface_add(struct ath6kl *ar, char *name,
+					  enum nl80211_iftype type,
+					  u8 fw_vif_idx, u8 nw_type)
 {
 	struct net_device *ndev;
 	struct ath6kl_vif *vif;
@@ -3410,7 +3502,8 @@ struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
 	vif->listen_intvl_t = ATH6KL_DEFAULT_LISTEN_INTVAL;
 	vif->bmiss_time_t = ATH6KL_DEFAULT_BMISS_TIME;
 	vif->bg_scan_period = 0;
-	vif->htcap.ht_enable = true;
+	vif->htcap[IEEE80211_BAND_2GHZ].ht_enable = true;
+	vif->htcap[IEEE80211_BAND_5GHZ].ht_enable = true;
 
 	memcpy(ndev->dev_addr, ar->mac_addr, ETH_ALEN);
 	if (fw_vif_idx != 0)
@@ -3440,7 +3533,7 @@ struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
 	list_add_tail(&vif->list, &ar->vif_list);
 	spin_unlock_bh(&ar->list_lock);
 
-	return ndev;
+	return &vif->wdev;
 
 err:
 	aggr_module_destroy(vif->aggr_cntxt);
@@ -3470,7 +3563,13 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	}
 
 	/* max num of ssids that can be probed during scanning */
-	wiphy->max_scan_ssids = MAX_PROBED_SSID_INDEX;
+	wiphy->max_scan_ssids = MAX_PROBED_SSIDS;
+
+	/* max num of ssids that can be matched after scan */
+	if (test_bit(ATH6KL_FW_CAPABILITY_SCHED_SCAN_MATCH_LIST,
+		     ar->fw_capabilities))
+		wiphy->max_match_sets = MAX_PROBED_SSIDS;
+
 	wiphy->max_scan_ie_len = 1000; /* FIX: what is correct limit? */
 	switch (ar->hw.cap) {
 	case WMI_11AN_CAP:
@@ -3507,6 +3606,17 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 		ath6kl_band_5ghz.ht_cap.cap = 0;
 		ath6kl_band_5ghz.ht_cap.ht_supported = false;
 	}
+
+	if (ar->hw.flags & ATH6KL_HW_FLAG_64BIT_RATES) {
+		ath6kl_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+		ath6kl_band_5ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+		ath6kl_band_2ghz.ht_cap.mcs.rx_mask[1] = 0xff;
+		ath6kl_band_5ghz.ht_cap.mcs.rx_mask[1] = 0xff;
+	} else {
+		ath6kl_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+		ath6kl_band_5ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+	}
+
 	if (band_2gig)
 		wiphy->bands[IEEE80211_BAND_2GHZ] = &ath6kl_band_2ghz;
 	if (band_5gig)
@@ -3517,6 +3627,7 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	wiphy->cipher_suites = cipher_suites;
 	wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
 
+#ifdef CONFIG_PM
 	wiphy->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT |
 			      WIPHY_WOWLAN_DISCONNECT |
 			      WIPHY_WOWLAN_GTK_REKEY_FAILURE  |
@@ -3526,8 +3637,9 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 	wiphy->wowlan.n_patterns = WOW_MAX_FILTERS_PER_LIST;
 	wiphy->wowlan.pattern_min_len = 1;
 	wiphy->wowlan.pattern_max_len = WOW_PATTERN_SIZE;
+#endif
 
-	wiphy->max_sched_scan_ssids = MAX_PROBED_SSID_INDEX;
+	wiphy->max_sched_scan_ssids = MAX_PROBED_SSIDS;
 
 	ar->wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM |
 			    WIPHY_FLAG_HAVE_AP_SME |
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.h b/drivers/net/wireless/ath/ath6kl/cfg80211.h
index 5ea8cbb79f4..56b1ebe7981 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.h
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.h
@@ -25,9 +25,9 @@ enum ath6kl_cfg_suspend_mode {
 	ATH6KL_CFG_SUSPEND_SCHED_SCAN,
 };
 
-struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
-					enum nl80211_iftype type,
-					u8 fw_vif_idx, u8 nw_type);
+struct wireless_dev *ath6kl_interface_add(struct ath6kl *ar, char *name,
+					  enum nl80211_iftype type,
+					  u8 fw_vif_idx, u8 nw_type);
 void ath6kl_cfg80211_ch_switch_notify(struct ath6kl_vif *vif, int freq,
 				      enum wmi_phy_mode mode);
 void ath6kl_cfg80211_scan_complete_event(struct ath6kl_vif *vif, bool aborted);
@@ -62,5 +62,7 @@ void ath6kl_cfg80211_cleanup(struct ath6kl *ar);
 
 struct ath6kl *ath6kl_cfg80211_create(void);
 void ath6kl_cfg80211_destroy(struct ath6kl *ar);
+/* TODO: remove this once ath6kl_vif_cleanup() is moved to cfg80211.c */
+void ath6kl_cfg80211_sta_bmiss_enhance(struct ath6kl_vif *vif, bool enable);
 
 #endif /* ATH6KL_CFG80211_H */
diff --git a/drivers/net/wireless/ath/ath6kl/core.c b/drivers/net/wireless/ath/ath6kl/core.c
index fdb3b1decc7..82c4dd2a960 100644
--- a/drivers/net/wireless/ath/ath6kl/core.c
+++ b/drivers/net/wireless/ath/ath6kl/core.c
@@ -56,7 +56,7 @@ EXPORT_SYMBOL(ath6kl_core_rx_complete);
 int ath6kl_core_init(struct ath6kl *ar, enum ath6kl_htc_type htc_type)
 {
 	struct ath6kl_bmi_target_info targ_info;
-	struct net_device *ndev;
+	struct wireless_dev *wdev;
 	int ret = 0, i;
 
 	switch (htc_type) {
@@ -187,12 +187,12 @@ int ath6kl_core_init(struct ath6kl *ar, enum ath6kl_htc_type htc_type)
 	rtnl_lock();
 
 	/* Add an initial station interface */
-	ndev = ath6kl_interface_add(ar, "wlan%d", NL80211_IFTYPE_STATION, 0,
+	wdev = ath6kl_interface_add(ar, "wlan%d", NL80211_IFTYPE_STATION, 0,
 				    INFRA_NETWORK);
 
 	rtnl_unlock();
 
-	if (!ndev) {
+	if (!wdev) {
 		ath6kl_err("Failed to instantiate a network device\n");
 		ret = -ENOMEM;
 		wiphy_unregister(ar->wiphy);
@@ -200,7 +200,7 @@ int ath6kl_core_init(struct ath6kl *ar, enum ath6kl_htc_type htc_type)
 	}
 
 	ath6kl_dbg(ATH6KL_DBG_TRC, "%s: name=%s dev=0x%p, ar=0x%p\n",
-		   __func__, ndev->name, ndev, ar);
+		   __func__, wdev->netdev->name, wdev->netdev, ar);
 
 	return ret;
 
diff --git a/drivers/net/wireless/ath/ath6kl/core.h b/drivers/net/wireless/ath/ath6kl/core.h
index 4d9c6f14269..cec49a31029 100644
--- a/drivers/net/wireless/ath/ath6kl/core.h
+++ b/drivers/net/wireless/ath/ath6kl/core.h
@@ -100,6 +100,21 @@ enum ath6kl_fw_capability {
 	/* Firmware has support to override rsn cap of rsn ie */
 	ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE,
 
+	/*
+	 * Multicast support in WOW and host awake mode.
+	 * Allow all multicast in host awake mode.
+	 * Apply multicast filter in WOW mode.
+	 */
+	ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+
+	/* Firmware supports enhanced bmiss detection */
+	ATH6KL_FW_CAPABILITY_BMISS_ENHANCE,
+
+	/*
+	 * FW supports matching of ssid in schedule scan
+	 */
+	ATH6KL_FW_CAPABILITY_SCHED_SCAN_MATCH_LIST,
+
 	/* this needs to be last */
 	ATH6KL_FW_CAPABILITY_MAX,
 };
@@ -112,6 +127,10 @@ struct ath6kl_fw_ie {
 	u8 data[0];
 };
 
+enum ath6kl_hw_flags {
+	ATH6KL_HW_FLAG_64BIT_RATES	= BIT(0),
+};
+
 #define ATH6KL_FW_API2_FILE "fw-2.bin"
 #define ATH6KL_FW_API3_FILE "fw-3.bin"
 
@@ -196,7 +215,7 @@ struct ath6kl_fw_ie {
 
 #define AGGR_NUM_OF_FREE_NETBUFS    16
 
-#define AGGR_RX_TIMEOUT     400	/* in ms */
+#define AGGR_RX_TIMEOUT     100	/* in ms */
 
 #define WMI_TIMEOUT (2 * HZ)
 
@@ -245,7 +264,6 @@ struct skb_hold_q {
 
 struct rxtid {
 	bool aggr;
-	bool progress;
 	bool timer_mon;
 	u16 win_sz;
 	u16 seq_next;
@@ -254,9 +272,15 @@ struct rxtid {
 	struct sk_buff_head q;
 
 	/*
-	 * FIXME: No clue what this should protect. Apparently it should
-	 * protect some of the fields above but they are also accessed
-	 * without taking the lock.
+	 * lock mainly protects seq_next and hold_q. Movement of seq_next
+	 * needs to be protected between aggr_timeout() and
+	 * aggr_process_recv_frm(). hold_q will be holding the pending
+	 * reorder frames and it's access should also be protected.
+	 * Some of the other fields like hold_q_sz, win_sz and aggr are
+	 * initialized/reset when receiving addba/delba req, also while
+	 * deleting aggr state all the pending buffers are flushed before
+	 * resetting these fields, so there should not be any race in accessing
+	 * these fields.
 	 */
 	spinlock_t lock;
 };
@@ -541,7 +565,7 @@ struct ath6kl_vif {
 	struct ath6kl_wep_key wep_key_list[WMI_MAX_KEY_INDEX + 1];
 	struct ath6kl_key keys[WMI_MAX_KEY_INDEX + 1];
 	struct aggr_info *aggr_cntxt;
-	struct ath6kl_htcap htcap;
+	struct ath6kl_htcap htcap[IEEE80211_NUM_BANDS];
 
 	struct timer_list disconnect_timer;
 	struct timer_list sched_scan_timer;
@@ -553,9 +577,6 @@ struct ath6kl_vif {
 	u32 last_cancel_roc_id;
 	u32 send_action_id;
 	bool probe_req_report;
-	u16 next_chan;
-	enum nl80211_channel_type next_ch_type;
-	enum ieee80211_band next_ch_band;
 	u16 assoc_bss_beacon_int;
 	u16 listen_intvl_t;
 	u16 bmiss_time_t;
@@ -568,6 +589,11 @@ struct ath6kl_vif {
 	struct list_head mc_filter;
 };
 
+static inline struct ath6kl_vif *ath6kl_vif_from_wdev(struct wireless_dev *wdev)
+{
+	return container_of(wdev, struct ath6kl_vif, wdev);
+}
+
 #define WOW_LIST_ID		0
 #define WOW_HOST_REQ_DELAY	500 /* ms */
 
@@ -687,6 +713,8 @@ struct ath6kl {
 		u32 testscript_addr;
 		enum wmi_phy_cap cap;
 
+		u32 flags;
+
 		struct ath6kl_hw_fw {
 			const char *dir;
 			const char *otp;
diff --git a/drivers/net/wireless/ath/ath6kl/htc_mbox.c b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
index 2798624d3a9..cd0e1ba410d 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_mbox.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
@@ -1309,7 +1309,7 @@ static int ath6kl_htc_rx_packet(struct htc_target *target,
 	}
 
 	ath6kl_dbg(ATH6KL_DBG_HTC,
-		   "htc rx 0x%p hdr x%x len %d mbox 0x%x\n",
+		   "htc rx 0x%p hdr 0x%x len %d mbox 0x%x\n",
 		   packet, packet->info.rx.exp_hdr,
 		   padded_len, dev->ar->mbox_info.htc_addr);
 
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index 7eb0515f458..f90b5db741c 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -42,6 +42,7 @@ static const struct ath6kl_hw hw_list[] = {
 		.reserved_ram_size		= 6912,
 		.refclk_hz			= 26000000,
 		.uarttx_pin			= 8,
+		.flags				= 0,
 
 		/* hw2.0 needs override address hardcoded */
 		.app_start_override_addr	= 0x944C00,
@@ -67,6 +68,7 @@ static const struct ath6kl_hw hw_list[] = {
 		.refclk_hz			= 26000000,
 		.uarttx_pin			= 8,
 		.testscript_addr		= 0x57ef74,
+		.flags				= 0,
 
 		.fw = {
 			.dir		= AR6003_HW_2_1_1_FW_DIR,
@@ -91,6 +93,7 @@ static const struct ath6kl_hw hw_list[] = {
 		.board_addr			= 0x433900,
 		.refclk_hz			= 26000000,
 		.uarttx_pin			= 11,
+		.flags				= ATH6KL_HW_FLAG_64BIT_RATES,
 
 		.fw = {
 			.dir		= AR6004_HW_1_0_FW_DIR,
@@ -110,6 +113,7 @@ static const struct ath6kl_hw hw_list[] = {
 		.board_addr			= 0x43d400,
 		.refclk_hz			= 40000000,
 		.uarttx_pin			= 11,
+		.flags				= ATH6KL_HW_FLAG_64BIT_RATES,
 
 		.fw = {
 			.dir		= AR6004_HW_1_1_FW_DIR,
@@ -129,6 +133,7 @@ static const struct ath6kl_hw hw_list[] = {
 		.board_addr			= 0x435c00,
 		.refclk_hz			= 40000000,
 		.uarttx_pin			= 11,
+		.flags				= ATH6KL_HW_FLAG_64BIT_RATES,
 
 		.fw = {
 			.dir		= AR6004_HW_1_2_FW_DIR,
@@ -938,6 +943,14 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 		}
 
 		switch (ie_id) {
+		case ATH6KL_FW_IE_FW_VERSION:
+			strlcpy(ar->wiphy->fw_version, data,
+				sizeof(ar->wiphy->fw_version));
+
+			ath6kl_dbg(ATH6KL_DBG_BOOT,
+				   "found fw version %s\n",
+				    ar->wiphy->fw_version);
+			break;
 		case ATH6KL_FW_IE_OTP_IMAGE:
 			ath6kl_dbg(ATH6KL_DBG_BOOT, "found otp image ie (%zd B)\n",
 				   ie_len);
@@ -991,9 +1004,6 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 				   ar->hw.reserved_ram_size);
 			break;
 		case ATH6KL_FW_IE_CAPABILITIES:
-			if (ie_len < DIV_ROUND_UP(ATH6KL_FW_CAPABILITY_MAX, 8))
-				break;
-
 			ath6kl_dbg(ATH6KL_DBG_BOOT,
 				   "found firmware capabilities ie (%zd B)\n",
 				   ie_len);
@@ -1002,6 +1012,9 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 				index = i / 8;
 				bit = i % 8;
 
+				if (index == ie_len)
+					break;
+
 				if (data[index] & (1 << bit))
 					__set_bit(i, ar->fw_capabilities);
 			}
@@ -1392,6 +1405,12 @@ static int ath6kl_init_upload(struct ath6kl *ar)
 	    ar->version.target_ver == AR6003_HW_2_1_1_VERSION) {
 		ath6kl_err("temporary war to avoid sdio crc error\n");
 
+		param = 0x28;
+		address = GPIO_BASE_ADDRESS + GPIO_PIN9_ADDRESS;
+		status = ath6kl_bmi_reg_write(ar, address, param);
+		if (status)
+			return status;
+
 		param = 0x20;
 
 		address = GPIO_BASE_ADDRESS + GPIO_PIN10_ADDRESS;
@@ -1659,6 +1678,9 @@ void ath6kl_cleanup_vif(struct ath6kl_vif *vif, bool wmi_ready)
 		cfg80211_scan_done(vif->scan_req, true);
 		vif->scan_req = NULL;
 	}
+
+	/* need to clean up enhanced bmiss detection fw state */
+	ath6kl_cfg80211_sta_bmiss_enhance(vif, false);
 }
 
 void ath6kl_stop_txrx(struct ath6kl *ar)
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index e5524470529..c189e28e86a 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -554,20 +554,24 @@ void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver,
 	struct ath6kl *ar = devt;
 
 	memcpy(ar->mac_addr, datap, ETH_ALEN);
-	ath6kl_dbg(ATH6KL_DBG_TRC, "%s: mac addr = %pM\n",
-		   __func__, ar->mac_addr);
+
+	ath6kl_dbg(ATH6KL_DBG_BOOT,
+		   "ready event mac addr %pM sw_ver 0x%x abi_ver 0x%x cap 0x%x\n",
+		   ar->mac_addr, sw_ver, abi_ver, cap);
 
 	ar->version.wlan_ver = sw_ver;
 	ar->version.abi_ver = abi_ver;
 	ar->hw.cap = cap;
 
-	snprintf(ar->wiphy->fw_version,
-		 sizeof(ar->wiphy->fw_version),
-		 "%u.%u.%u.%u",
-		 (ar->version.wlan_ver & 0xf0000000) >> 28,
-		 (ar->version.wlan_ver & 0x0f000000) >> 24,
-		 (ar->version.wlan_ver & 0x00ff0000) >> 16,
-		 (ar->version.wlan_ver & 0x0000ffff));
+	if (strlen(ar->wiphy->fw_version) == 0) {
+		snprintf(ar->wiphy->fw_version,
+			 sizeof(ar->wiphy->fw_version),
+			 "%u.%u.%u.%u",
+			 (ar->version.wlan_ver & 0xf0000000) >> 28,
+			 (ar->version.wlan_ver & 0x0f000000) >> 24,
+			 (ar->version.wlan_ver & 0x00ff0000) >> 16,
+			 (ar->version.wlan_ver & 0x0000ffff));
+	}
 
 	/* indicate to the waiting thread that the ready event was received */
 	set_bit(WMI_READY, &ar->flag);
@@ -598,7 +602,6 @@ static int ath6kl_commit_ch_switch(struct ath6kl_vif *vif, u16 channel)
 
 	struct ath6kl *ar = vif->ar;
 
-	vif->next_chan = channel;
 	vif->profile.ch = cpu_to_le16(channel);
 
 	switch (vif->nw_type) {
@@ -1167,7 +1170,10 @@ static void ath6kl_set_multicast_list(struct net_device *ndev)
 	else
 		clear_bit(NETDEV_MCAST_ALL_ON, &vif->flags);
 
-	mc_all_on = mc_all_on || (vif->ar->state == ATH6KL_STATE_ON);
+	if (test_bit(ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+		     vif->ar->fw_capabilities)) {
+		mc_all_on = mc_all_on || (vif->ar->state == ATH6KL_STATE_ON);
+	}
 
 	if (!(ndev->flags & IFF_MULTICAST)) {
 		mc_all_on = false;
diff --git a/drivers/net/wireless/ath/ath6kl/target.h b/drivers/net/wireless/ath/ath6kl/target.h
index 78e0ef4567a..a98c12ba70c 100644
--- a/drivers/net/wireless/ath/ath6kl/target.h
+++ b/drivers/net/wireless/ath/ath6kl/target.h
@@ -45,6 +45,7 @@
 #define LPO_CAL_ENABLE_S		20
 #define LPO_CAL_ENABLE			0x00100000
 
+#define GPIO_PIN9_ADDRESS		0x0000004c
 #define GPIO_PIN10_ADDRESS		0x00000050
 #define GPIO_PIN11_ADDRESS		0x00000054
 #define GPIO_PIN12_ADDRESS		0x00000058
diff --git a/drivers/net/wireless/ath/ath6kl/txrx.c b/drivers/net/wireless/ath/ath6kl/txrx.c
index 67206aedea6..7dfa0fd86d7 100644
--- a/drivers/net/wireless/ath/ath6kl/txrx.c
+++ b/drivers/net/wireless/ath/ath6kl/txrx.c
@@ -1036,6 +1036,7 @@ static void aggr_deque_frms(struct aggr_info_conn *agg_conn, u8 tid,
 	rxtid = &agg_conn->rx_tid[tid];
 	stats = &agg_conn->stat[tid];
 
+	spin_lock_bh(&rxtid->lock);
 	idx = AGGR_WIN_IDX(rxtid->seq_next, rxtid->hold_q_sz);
 
 	/*
@@ -1054,8 +1055,6 @@ static void aggr_deque_frms(struct aggr_info_conn *agg_conn, u8 tid,
 	seq_end = seq_no ? seq_no : rxtid->seq_next;
 	idx_end = AGGR_WIN_IDX(seq_end, rxtid->hold_q_sz);
 
-	spin_lock_bh(&rxtid->lock);
-
 	do {
 		node = &rxtid->hold_q[idx];
 		if ((order == 1) && (!node->skb))
@@ -1127,11 +1126,13 @@ static bool aggr_process_recv_frm(struct aggr_info_conn *agg_conn, u8 tid,
 		    ((end > extended_end) && (cur > extended_end) &&
 		     (cur < end))) {
 			aggr_deque_frms(agg_conn, tid, 0, 0);
+			spin_lock_bh(&rxtid->lock);
 			if (cur >= rxtid->hold_q_sz - 1)
 				rxtid->seq_next = cur - (rxtid->hold_q_sz - 1);
 			else
 				rxtid->seq_next = ATH6KL_MAX_SEQ_NO -
 						  (rxtid->hold_q_sz - 2 - cur);
+			spin_unlock_bh(&rxtid->lock);
 		} else {
 			/*
 			 * Dequeue only those frames that are outside the
@@ -1185,25 +1186,25 @@ static bool aggr_process_recv_frm(struct aggr_info_conn *agg_conn, u8 tid,
 	aggr_deque_frms(agg_conn, tid, 0, 1);
 
 	if (agg_conn->timer_scheduled)
-		rxtid->progress = true;
-	else
-		for (idx = 0 ; idx < rxtid->hold_q_sz; idx++) {
-			if (rxtid->hold_q[idx].skb) {
-				/*
-				 * There is a frame in the queue and no
-				 * timer so start a timer to ensure that
-				 * the frame doesn't remain stuck
-				 * forever.
-				 */
-				agg_conn->timer_scheduled = true;
-				mod_timer(&agg_conn->timer,
-					  (jiffies +
-					   HZ * (AGGR_RX_TIMEOUT) / 1000));
-				rxtid->progress = false;
-				rxtid->timer_mon = true;
-				break;
-			}
+		return is_queued;
+
+	spin_lock_bh(&rxtid->lock);
+	for (idx = 0 ; idx < rxtid->hold_q_sz; idx++) {
+		if (rxtid->hold_q[idx].skb) {
+			/*
+			 * There is a frame in the queue and no
+			 * timer so start a timer to ensure that
+			 * the frame doesn't remain stuck
+			 * forever.
+			 */
+			agg_conn->timer_scheduled = true;
+			mod_timer(&agg_conn->timer,
+				  (jiffies + (HZ * AGGR_RX_TIMEOUT) / 1000));
+			rxtid->timer_mon = true;
+			break;
 		}
+	}
+	spin_unlock_bh(&rxtid->lock);
 
 	return is_queued;
 }
@@ -1608,7 +1609,7 @@ static void aggr_timeout(unsigned long arg)
 		rxtid = &aggr_conn->rx_tid[i];
 		stats = &aggr_conn->stat[i];
 
-		if (!rxtid->aggr || !rxtid->timer_mon || rxtid->progress)
+		if (!rxtid->aggr || !rxtid->timer_mon)
 			continue;
 
 		stats->num_timeouts++;
@@ -1626,14 +1627,15 @@ static void aggr_timeout(unsigned long arg)
 		rxtid = &aggr_conn->rx_tid[i];
 
 		if (rxtid->aggr && rxtid->hold_q) {
+			spin_lock_bh(&rxtid->lock);
 			for (j = 0; j < rxtid->hold_q_sz; j++) {
 				if (rxtid->hold_q[j].skb) {
 					aggr_conn->timer_scheduled = true;
 					rxtid->timer_mon = true;
-					rxtid->progress = false;
 					break;
 				}
 			}
+			spin_unlock_bh(&rxtid->lock);
 
 			if (j >= rxtid->hold_q_sz)
 				rxtid->timer_mon = false;
@@ -1660,7 +1662,6 @@ static void aggr_delete_tid_state(struct aggr_info_conn *aggr_conn, u8 tid)
 		aggr_deque_frms(aggr_conn, tid, 0, 0);
 
 	rxtid->aggr = false;
-	rxtid->progress = false;
 	rxtid->timer_mon = false;
 	rxtid->win_sz = 0;
 	rxtid->seq_next = 0;
@@ -1739,7 +1740,6 @@ void aggr_conn_init(struct ath6kl_vif *vif, struct aggr_info *aggr_info,
 	for (i = 0; i < NUM_OF_TIDS; i++) {
 		rxtid = &aggr_conn->rx_tid[i];
 		rxtid->aggr = false;
-		rxtid->progress = false;
 		rxtid->timer_mon = false;
 		skb_queue_head_init(&rxtid->q);
 		spin_lock_init(&rxtid->lock);
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index ee8ec2394c2..c30ab4b11d6 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -474,7 +474,7 @@ static int ath6kl_wmi_remain_on_chnl_event_rx(struct wmi *wmi, u8 *datap,
 		return -EINVAL;
 	}
 	id = vif->last_roc_id;
-	cfg80211_ready_on_channel(vif->ndev, id, chan, NL80211_CHAN_NO_HT,
+	cfg80211_ready_on_channel(&vif->wdev, id, chan, NL80211_CHAN_NO_HT,
 				  dur, GFP_ATOMIC);
 
 	return 0;
@@ -513,7 +513,7 @@ static int ath6kl_wmi_cancel_remain_on_chnl_event_rx(struct wmi *wmi,
 	else
 		id = vif->last_roc_id; /* timeout on uncanceled r-o-c */
 	vif->last_cancel_roc_id = 0;
-	cfg80211_remain_on_channel_expired(vif->ndev, id, chan,
+	cfg80211_remain_on_channel_expired(&vif->wdev, id, chan,
 					   NL80211_CHAN_NO_HT, GFP_ATOMIC);
 
 	return 0;
@@ -533,7 +533,7 @@ static int ath6kl_wmi_tx_status_event_rx(struct wmi *wmi, u8 *datap, int len,
 	ath6kl_dbg(ATH6KL_DBG_WMI, "tx_status: id=%x ack_status=%u\n",
 		   id, ev->ack_status);
 	if (wmi->last_mgmt_tx_frame) {
-		cfg80211_mgmt_tx_status(vif->ndev, id,
+		cfg80211_mgmt_tx_status(&vif->wdev, id,
 					wmi->last_mgmt_tx_frame,
 					wmi->last_mgmt_tx_frame_len,
 					!!ev->ack_status, GFP_ATOMIC);
@@ -568,7 +568,7 @@ static int ath6kl_wmi_rx_probe_req_event_rx(struct wmi *wmi, u8 *datap, int len,
 		   dlen, freq, vif->probe_req_report);
 
 	if (vif->probe_req_report || vif->nw_type == AP_NETWORK)
-		cfg80211_rx_mgmt(vif->ndev, freq, 0,
+		cfg80211_rx_mgmt(&vif->wdev, freq, 0,
 				 ev->data, dlen, GFP_ATOMIC);
 
 	return 0;
@@ -608,7 +608,7 @@ static int ath6kl_wmi_rx_action_event_rx(struct wmi *wmi, u8 *datap, int len,
 		return -EINVAL;
 	}
 	ath6kl_dbg(ATH6KL_DBG_WMI, "rx_action: len=%u freq=%u\n", dlen, freq);
-	cfg80211_rx_mgmt(vif->ndev, freq, 0,
+	cfg80211_rx_mgmt(&vif->wdev, freq, 0,
 			 ev->data, dlen, GFP_ATOMIC);
 
 	return 0;
@@ -743,7 +743,6 @@ int ath6kl_wmi_force_roam_cmd(struct wmi *wmi, const u8 *bssid)
 		return -ENOMEM;
 
 	cmd = (struct roam_ctrl_cmd *) skb->data;
-	memset(cmd, 0, sizeof(*cmd));
 
 	memcpy(cmd->info.bssid, bssid, ETH_ALEN);
 	cmd->roam_ctrl = WMI_FORCE_ROAM;
@@ -753,6 +752,22 @@ int ath6kl_wmi_force_roam_cmd(struct wmi *wmi, const u8 *bssid)
 				   NO_SYNC_WMIFLAG);
 }
 
+int ath6kl_wmi_ap_set_dtim_cmd(struct wmi *wmi, u8 if_idx, u32 dtim_period)
+{
+	struct sk_buff *skb;
+	struct set_dtim_cmd *cmd;
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*cmd));
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct set_dtim_cmd *) skb->data;
+
+	cmd->dtim_period = cpu_to_le32(dtim_period);
+	return ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+				   WMI_AP_SET_DTIM_CMDID, NO_SYNC_WMIFLAG);
+}
+
 int ath6kl_wmi_set_roam_mode_cmd(struct wmi *wmi, enum wmi_roam_mode mode)
 {
 	struct sk_buff *skb;
@@ -763,7 +778,6 @@ int ath6kl_wmi_set_roam_mode_cmd(struct wmi *wmi, enum wmi_roam_mode mode)
 		return -ENOMEM;
 
 	cmd = (struct roam_ctrl_cmd *) skb->data;
-	memset(cmd, 0, sizeof(*cmd));
 
 	cmd->info.roam_mode = mode;
 	cmd->roam_ctrl = WMI_SET_ROAM_MODE;
@@ -1995,7 +2009,7 @@ int ath6kl_wmi_probedssid_cmd(struct wmi *wmi, u8 if_idx, u8 index, u8 flag,
 	struct wmi_probed_ssid_cmd *cmd;
 	int ret;
 
-	if (index > MAX_PROBED_SSID_INDEX)
+	if (index >= MAX_PROBED_SSIDS)
 		return -EINVAL;
 
 	if (ssid_len > sizeof(cmd->ssid))
@@ -2599,6 +2613,115 @@ static void ath6kl_wmi_relinquish_implicit_pstream_credits(struct wmi *wmi)
 	spin_unlock_bh(&wmi->lock);
 }
 
+static int ath6kl_set_bitrate_mask64(struct wmi *wmi, u8 if_idx,
+				     const struct cfg80211_bitrate_mask *mask)
+{
+	struct sk_buff *skb;
+	int ret, mode, band;
+	u64 mcsrate, ratemask[IEEE80211_NUM_BANDS];
+	struct wmi_set_tx_select_rates64_cmd *cmd;
+
+	memset(&ratemask, 0, sizeof(ratemask));
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		/* copy legacy rate mask */
+		ratemask[band] = mask->control[band].legacy;
+		if (band == IEEE80211_BAND_5GHZ)
+			ratemask[band] =
+				mask->control[band].legacy << 4;
+
+		/* copy mcs rate mask */
+		mcsrate = mask->control[band].mcs[1];
+		mcsrate <<= 8;
+		mcsrate |= mask->control[band].mcs[0];
+		ratemask[band] |= mcsrate << 12;
+		ratemask[band] |= mcsrate << 28;
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "Ratemask 64 bit: 2.4:%llx 5:%llx\n",
+		   ratemask[0], ratemask[1]);
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*cmd) * WMI_RATES_MODE_MAX);
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_set_tx_select_rates64_cmd *) skb->data;
+	for (mode = 0; mode < WMI_RATES_MODE_MAX; mode++) {
+		/* A mode operate in 5GHZ band */
+		if (mode == WMI_RATES_MODE_11A ||
+		    mode == WMI_RATES_MODE_11A_HT20 ||
+		    mode == WMI_RATES_MODE_11A_HT40)
+			band = IEEE80211_BAND_5GHZ;
+		else
+			band = IEEE80211_BAND_2GHZ;
+		cmd->ratemask[mode] = cpu_to_le64(ratemask[band]);
+	}
+
+	ret = ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+				  WMI_SET_TX_SELECT_RATES_CMDID,
+				  NO_SYNC_WMIFLAG);
+	return ret;
+}
+
+static int ath6kl_set_bitrate_mask32(struct wmi *wmi, u8 if_idx,
+				     const struct cfg80211_bitrate_mask *mask)
+{
+	struct sk_buff *skb;
+	int ret, mode, band;
+	u32 mcsrate, ratemask[IEEE80211_NUM_BANDS];
+	struct wmi_set_tx_select_rates32_cmd *cmd;
+
+	memset(&ratemask, 0, sizeof(ratemask));
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		/* copy legacy rate mask */
+		ratemask[band] = mask->control[band].legacy;
+		if (band == IEEE80211_BAND_5GHZ)
+			ratemask[band] =
+				mask->control[band].legacy << 4;
+
+		/* copy mcs rate mask */
+		mcsrate = mask->control[band].mcs[0];
+		ratemask[band] |= mcsrate << 12;
+		ratemask[band] |= mcsrate << 20;
+	}
+
+	ath6kl_dbg(ATH6KL_DBG_WMI,
+		   "Ratemask 32 bit: 2.4:%x 5:%x\n",
+		   ratemask[0], ratemask[1]);
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*cmd) * WMI_RATES_MODE_MAX);
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_set_tx_select_rates32_cmd *) skb->data;
+	for (mode = 0; mode < WMI_RATES_MODE_MAX; mode++) {
+		/* A mode operate in 5GHZ band */
+		if (mode == WMI_RATES_MODE_11A ||
+		    mode == WMI_RATES_MODE_11A_HT20 ||
+		    mode == WMI_RATES_MODE_11A_HT40)
+			band = IEEE80211_BAND_5GHZ;
+		else
+			band = IEEE80211_BAND_2GHZ;
+		cmd->ratemask[mode] = cpu_to_le32(ratemask[band]);
+	}
+
+	ret = ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+				  WMI_SET_TX_SELECT_RATES_CMDID,
+				  NO_SYNC_WMIFLAG);
+	return ret;
+}
+
+int ath6kl_wmi_set_bitrate_mask(struct wmi *wmi, u8 if_idx,
+				const struct cfg80211_bitrate_mask *mask)
+{
+	struct ath6kl *ar = wmi->parent_dev;
+
+	if (ar->hw.flags & ATH6KL_HW_FLAG_64BIT_RATES)
+		return ath6kl_set_bitrate_mask64(wmi, if_idx, mask);
+	else
+		return ath6kl_set_bitrate_mask32(wmi, if_idx, mask);
+}
+
 int ath6kl_wmi_set_host_sleep_mode_cmd(struct wmi *wmi, u8 if_idx,
 				       enum ath6kl_host_mode host_mode)
 {
@@ -2997,6 +3120,25 @@ int ath6kl_wmi_add_del_mcast_filter_cmd(struct wmi *wmi, u8 if_idx,
 	return ret;
 }
 
+int ath6kl_wmi_sta_bmiss_enhance_cmd(struct wmi *wmi, u8 if_idx, bool enhance)
+{
+	struct sk_buff *skb;
+	struct wmi_sta_bmiss_enhance_cmd *cmd;
+	int ret;
+
+	skb = ath6kl_wmi_get_new_buf(sizeof(*cmd));
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_sta_bmiss_enhance_cmd *) skb->data;
+	cmd->enable = enhance ? 1 : 0;
+
+	ret = ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+				  WMI_STA_BMISS_ENHANCE_CMDID,
+				  NO_SYNC_WMIFLAG);
+	return ret;
+}
+
 s32 ath6kl_wmi_get_rate(s8 rate_index)
 {
 	if (rate_index == RATE_AUTO)
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.h b/drivers/net/wireless/ath/ath6kl/wmi.h
index 9076bec3a2b..43339aca585 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.h
+++ b/drivers/net/wireless/ath/ath6kl/wmi.h
@@ -624,6 +624,10 @@ enum wmi_cmd_id {
 	WMI_SEND_MGMT_CMDID,
 	WMI_BEGIN_SCAN_CMDID,
 
+	WMI_SET_BLACK_LIST,
+	WMI_SET_MCASTRATE,
+
+	WMI_STA_BMISS_ENHANCE_CMDID,
 };
 
 enum wmi_mgmt_frame_type {
@@ -960,6 +964,9 @@ enum wmi_bss_filter {
 	/* beacons matching probed ssid */
 	PROBED_SSID_FILTER,
 
+	/* beacons matching matched ssid */
+	MATCHED_SSID_FILTER,
+
 	/* marker only */
 	LAST_BSS_FILTER,
 };
@@ -978,7 +985,7 @@ struct wmi_bss_filter_cmd {
 } __packed;
 
 /* WMI_SET_PROBED_SSID_CMDID */
-#define MAX_PROBED_SSID_INDEX   9
+#define MAX_PROBED_SSIDS   16
 
 enum wmi_ssid_flag {
 	/* disables entry */
@@ -989,10 +996,13 @@ enum wmi_ssid_flag {
 
 	/* probes for any ssid */
 	ANY_SSID_FLAG = 0x02,
+
+	/* match for ssid */
+	MATCH_SSID_FLAG = 0x08,
 };
 
 struct wmi_probed_ssid_cmd {
-	/* 0 to MAX_PROBED_SSID_INDEX */
+	/* 0 to MAX_PROBED_SSIDS - 1 */
 	u8 entry_index;
 
 	/* see, enum wmi_ssid_flg */
@@ -1017,6 +1027,11 @@ struct wmi_bmiss_time_cmd {
 	__le16 num_beacons;
 };
 
+/* WMI_STA_ENHANCE_BMISS_CMDID */
+struct wmi_sta_bmiss_enhance_cmd {
+	u8 enable;
+} __packed;
+
 /* WMI_SET_POWER_MODE_CMDID */
 enum wmi_power_mode {
 	REC_POWER = 0x01,
@@ -1048,6 +1063,36 @@ struct wmi_power_params_cmd {
 	__le16 ps_fail_event_policy;
 } __packed;
 
+/*
+ * Ratemask for below modes should be passed
+ * to WMI_SET_TX_SELECT_RATES_CMDID.
+ * AR6003 has 32 bit mask for each modes.
+ * First 12 bits for legacy rates, 13 to 20
+ * bits for HT 20 rates and 21 to 28 bits for
+ * HT 40 rates
+ */
+enum wmi_mode_phy {
+	WMI_RATES_MODE_11A = 0,
+	WMI_RATES_MODE_11G,
+	WMI_RATES_MODE_11B,
+	WMI_RATES_MODE_11GONLY,
+	WMI_RATES_MODE_11A_HT20,
+	WMI_RATES_MODE_11G_HT20,
+	WMI_RATES_MODE_11A_HT40,
+	WMI_RATES_MODE_11G_HT40,
+	WMI_RATES_MODE_MAX
+};
+
+/* WMI_SET_TX_SELECT_RATES_CMDID */
+struct wmi_set_tx_select_rates32_cmd {
+	__le32 ratemask[WMI_RATES_MODE_MAX];
+} __packed;
+
+/* WMI_SET_TX_SELECT_RATES_CMDID */
+struct wmi_set_tx_select_rates64_cmd {
+	__le64 ratemask[WMI_RATES_MODE_MAX];
+} __packed;
+
 /* WMI_SET_DISC_TIMEOUT_CMDID */
 struct wmi_disc_timeout_cmd {
 	/* seconds */
@@ -1572,6 +1617,10 @@ struct roam_ctrl_cmd {
 	u8 roam_ctrl;
 } __packed;
 
+struct set_dtim_cmd {
+	__le32 dtim_period;
+} __packed;
+
 /* BSS INFO HDR version 2.0 */
 struct wmi_bss_info_hdr2 {
 	__le16 ch; /* frequency in MHz */
@@ -2532,6 +2581,8 @@ int ath6kl_wmi_set_ip_cmd(struct wmi *wmi, u8 if_idx,
 			  __be32 ips0, __be32 ips1);
 int ath6kl_wmi_set_host_sleep_mode_cmd(struct wmi *wmi, u8 if_idx,
 				       enum ath6kl_host_mode host_mode);
+int ath6kl_wmi_set_bitrate_mask(struct wmi *wmi, u8 if_idx,
+				const struct cfg80211_bitrate_mask *mask);
 int ath6kl_wmi_set_wow_mode_cmd(struct wmi *wmi, u8 if_idx,
 				enum ath6kl_wow_mode wow_mode,
 				u32 filter, u16 host_req_delay);
@@ -2542,11 +2593,14 @@ int ath6kl_wmi_add_wow_pattern_cmd(struct wmi *wmi, u8 if_idx,
 int ath6kl_wmi_del_wow_pattern_cmd(struct wmi *wmi, u8 if_idx,
 				   u16 list_id, u16 filter_id);
 int ath6kl_wmi_set_roam_lrssi_cmd(struct wmi *wmi, u8 lrssi);
+int ath6kl_wmi_ap_set_dtim_cmd(struct wmi *wmi, u8 if_idx, u32 dtim_period);
 int ath6kl_wmi_force_roam_cmd(struct wmi *wmi, const u8 *bssid);
 int ath6kl_wmi_set_roam_mode_cmd(struct wmi *wmi, enum wmi_roam_mode mode);
 int ath6kl_wmi_mcast_filter_cmd(struct wmi *wmi, u8 if_idx, bool mc_all_on);
 int ath6kl_wmi_add_del_mcast_filter_cmd(struct wmi *wmi, u8 if_idx,
 					u8 *filter, bool add_filter);
+int ath6kl_wmi_sta_bmiss_enhance_cmd(struct wmi *wmi, u8 if_idx, bool enable);
+
 /* AP mode uAPSD */
 int ath6kl_wmi_ap_set_apsd(struct wmi *wmi, u8 if_idx, u8 enable);
 
diff --git a/drivers/net/wireless/ath/ath9k/Kconfig b/drivers/net/wireless/ath/ath9k/Kconfig
index e507e78398f..c7aa6646123 100644
--- a/drivers/net/wireless/ath/ath9k/Kconfig
+++ b/drivers/net/wireless/ath/ath9k/Kconfig
@@ -64,7 +64,7 @@ config ATH9K_DEBUGFS
 
 config ATH9K_DFS_CERTIFIED
 	bool "Atheros DFS support for certified platforms"
-	depends on ATH9K && EXPERT
+	depends on ATH9K && CFG80211_CERTIFICATION_ONUS
 	default n
 	---help---
 	  This option enables DFS support for initiating radiation on
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index 3f0b8472378..2ad8f9474ba 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -3,7 +3,9 @@ ath9k-y +=	beacon.o \
 		init.o \
 		main.o \
 		recv.o \
-		xmit.o
+		xmit.o \
+		link.o \
+		antenna.o
 
 ath9k-$(CONFIG_ATH9K_BTCOEX_SUPPORT) += mci.o
 ath9k-$(CONFIG_ATH9K_RATE_CONTROL) += rc.o
@@ -15,6 +17,7 @@ ath9k-$(CONFIG_ATH9K_DFS_CERTIFIED) += \
 		dfs.o \
 		dfs_pattern_detector.o \
 		dfs_pri_detector.o
+ath9k-$(CONFIG_PM_SLEEP) += wow.o
 
 obj-$(CONFIG_ATH9K) += ath9k.o
 
diff --git a/drivers/net/wireless/ath/ath9k/ahb.c b/drivers/net/wireless/ath/ath9k/ahb.c
index 5e47ca6d16a..3a69804f4c1 100644
--- a/drivers/net/wireless/ath/ath9k/ahb.c
+++ b/drivers/net/wireless/ath/ath9k/ahb.c
@@ -35,6 +35,10 @@ static const struct platform_device_id ath9k_platform_id_table[] = {
 		.name = "ar934x_wmac",
 		.driver_data = AR9300_DEVID_AR9340,
 	},
+	{
+		.name = "qca955x_wmac",
+		.driver_data = AR9300_DEVID_QCA955X,
+	},
 	{},
 };
 
@@ -126,7 +130,7 @@ static int ath_ahb_probe(struct platform_device *pdev)
 	sc->irq = irq;
 
 	/* Will be cleared in ath9k_start() */
-	sc->sc_flags |= SC_OP_INVALID;
+	set_bit(SC_OP_INVALID, &sc->sc_flags);
 
 	ret = request_irq(irq, ath_isr, IRQF_SHARED, "ath9k", sc);
 	if (ret) {
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index b4c77f9d747..ff007f500fe 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -104,11 +104,6 @@ static const struct ani_cck_level_entry cck_level_table[] = {
 #define ATH9K_ANI_CCK_DEF_LEVEL \
 	2 /* default level - matches the INI settings */
 
-static bool use_new_ani(struct ath_hw *ah)
-{
-	return AR_SREV_9300_20_OR_LATER(ah) || modparam_force_new_ani;
-}
-
 static void ath9k_hw_update_mibstats(struct ath_hw *ah,
 				     struct ath9k_mib_stats *stats)
 {
@@ -122,8 +117,6 @@ static void ath9k_hw_update_mibstats(struct ath_hw *ah,
 static void ath9k_ani_restart(struct ath_hw *ah)
 {
 	struct ar5416AniState *aniState;
-	struct ath_common *common = ath9k_hw_common(ah);
-	u32 ofdm_base = 0, cck_base = 0;
 
 	if (!DO_ANI(ah))
 		return;
@@ -131,18 +124,10 @@ static void ath9k_ani_restart(struct ath_hw *ah)
 	aniState = &ah->curchan->ani;
 	aniState->listenTime = 0;
 
-	if (!use_new_ani(ah)) {
-		ofdm_base = AR_PHY_COUNTMAX - ah->config.ofdm_trig_high;
-		cck_base = AR_PHY_COUNTMAX - ah->config.cck_trig_high;
-	}
-
-	ath_dbg(common, ANI, "Writing ofdmbase=%u   cckbase=%u\n",
-		ofdm_base, cck_base);
-
 	ENABLE_REGWRITE_BUFFER(ah);
 
-	REG_WRITE(ah, AR_PHY_ERR_1, ofdm_base);
-	REG_WRITE(ah, AR_PHY_ERR_2, cck_base);
+	REG_WRITE(ah, AR_PHY_ERR_1, 0);
+	REG_WRITE(ah, AR_PHY_ERR_2, 0);
 	REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
 	REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
 
@@ -154,129 +139,23 @@ static void ath9k_ani_restart(struct ath_hw *ah)
 	aniState->cckPhyErrCount = 0;
 }
 
-static void ath9k_hw_ani_ofdm_err_trigger_old(struct ath_hw *ah)
-{
-	struct ieee80211_conf *conf = &ath9k_hw_common(ah)->hw->conf;
-	struct ar5416AniState *aniState;
-	int32_t rssi;
-
-	aniState = &ah->curchan->ani;
-
-	if (aniState->noiseImmunityLevel < HAL_NOISE_IMMUNE_MAX) {
-		if (ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-					 aniState->noiseImmunityLevel + 1)) {
-			return;
-		}
-	}
-
-	if (aniState->spurImmunityLevel < HAL_SPUR_IMMUNE_MAX) {
-		if (ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL,
-					 aniState->spurImmunityLevel + 1)) {
-			return;
-		}
-	}
-
-	if (ah->opmode == NL80211_IFTYPE_AP) {
-		if (aniState->firstepLevel < HAL_FIRST_STEP_MAX) {
-			ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-					     aniState->firstepLevel + 1);
-		}
-		return;
-	}
-	rssi = BEACON_RSSI(ah);
-	if (rssi > aniState->rssiThrHigh) {
-		if (!aniState->ofdmWeakSigDetectOff) {
-			if (ath9k_hw_ani_control(ah,
-					 ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-					 false)) {
-				ath9k_hw_ani_control(ah,
-					ATH9K_ANI_SPUR_IMMUNITY_LEVEL, 0);
-				return;
-			}
-		}
-		if (aniState->firstepLevel < HAL_FIRST_STEP_MAX) {
-			ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-					     aniState->firstepLevel + 1);
-			return;
-		}
-	} else if (rssi > aniState->rssiThrLow) {
-		if (aniState->ofdmWeakSigDetectOff)
-			ath9k_hw_ani_control(ah,
-				     ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-				     true);
-		if (aniState->firstepLevel < HAL_FIRST_STEP_MAX)
-			ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-					     aniState->firstepLevel + 1);
-		return;
-	} else {
-		if ((conf->channel->band == IEEE80211_BAND_2GHZ) &&
-		    !conf_is_ht(conf)) {
-			if (!aniState->ofdmWeakSigDetectOff)
-				ath9k_hw_ani_control(ah,
-				     ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-				     false);
-			if (aniState->firstepLevel > 0)
-				ath9k_hw_ani_control(ah,
-					     ATH9K_ANI_FIRSTEP_LEVEL, 0);
-			return;
-		}
-	}
-}
-
-static void ath9k_hw_ani_cck_err_trigger_old(struct ath_hw *ah)
-{
-	struct ieee80211_conf *conf = &ath9k_hw_common(ah)->hw->conf;
-	struct ar5416AniState *aniState;
-	int32_t rssi;
-
-	aniState = &ah->curchan->ani;
-	if (aniState->noiseImmunityLevel < HAL_NOISE_IMMUNE_MAX) {
-		if (ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-					 aniState->noiseImmunityLevel + 1)) {
-			return;
-		}
-	}
-	if (ah->opmode == NL80211_IFTYPE_AP) {
-		if (aniState->firstepLevel < HAL_FIRST_STEP_MAX) {
-			ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-					     aniState->firstepLevel + 1);
-		}
-		return;
-	}
-	rssi = BEACON_RSSI(ah);
-	if (rssi > aniState->rssiThrLow) {
-		if (aniState->firstepLevel < HAL_FIRST_STEP_MAX)
-			ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-					     aniState->firstepLevel + 1);
-	} else {
-		if ((conf->channel->band == IEEE80211_BAND_2GHZ) &&
-		    !conf_is_ht(conf)) {
-			if (aniState->firstepLevel > 0)
-				ath9k_hw_ani_control(ah,
-					     ATH9K_ANI_FIRSTEP_LEVEL, 0);
-		}
-	}
-}
-
 /* Adjust the OFDM Noise Immunity Level */
-static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
+static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel,
+				  bool scan)
 {
 	struct ar5416AniState *aniState = &ah->curchan->ani;
 	struct ath_common *common = ath9k_hw_common(ah);
 	const struct ani_ofdm_level_entry *entry_ofdm;
 	const struct ani_cck_level_entry *entry_cck;
-
-	aniState->noiseFloor = BEACON_RSSI(ah);
+	bool weak_sig;
 
 	ath_dbg(common, ANI, "**** ofdmlevel %d=>%d, rssi=%d[lo=%d hi=%d]\n",
 		aniState->ofdmNoiseImmunityLevel,
-		immunityLevel, aniState->noiseFloor,
+		immunityLevel, BEACON_RSSI(ah),
 		aniState->rssiThrLow, aniState->rssiThrHigh);
 
-	if (aniState->update_ani)
-		aniState->ofdmNoiseImmunityLevel =
-			(immunityLevel > ATH9K_ANI_OFDM_DEF_LEVEL) ?
-			immunityLevel : ATH9K_ANI_OFDM_DEF_LEVEL;
+	if (!scan)
+		aniState->ofdmNoiseImmunityLevel = immunityLevel;
 
 	entry_ofdm = &ofdm_level_table[aniState->ofdmNoiseImmunityLevel];
 	entry_cck = &cck_level_table[aniState->cckNoiseImmunityLevel];
@@ -292,12 +171,22 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
 				     ATH9K_ANI_FIRSTEP_LEVEL,
 				     entry_ofdm->fir_step_level);
 
-	if ((aniState->noiseFloor >= aniState->rssiThrHigh) &&
-	    (!aniState->ofdmWeakSigDetectOff !=
-	     entry_ofdm->ofdm_weak_signal_on)) {
+	weak_sig = entry_ofdm->ofdm_weak_signal_on;
+	if (ah->opmode == NL80211_IFTYPE_STATION &&
+	    BEACON_RSSI(ah) <= aniState->rssiThrHigh)
+		weak_sig = true;
+
+	if (aniState->ofdmWeakSigDetect != weak_sig)
 			ath9k_hw_ani_control(ah,
 				ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
 				entry_ofdm->ofdm_weak_signal_on);
+
+	if (aniState->ofdmNoiseImmunityLevel >= ATH9K_ANI_OFDM_DEF_LEVEL) {
+		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
+		ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI;
+	} else {
+		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI;
+		ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW;
 	}
 }
 
@@ -308,43 +197,35 @@ static void ath9k_hw_ani_ofdm_err_trigger(struct ath_hw *ah)
 	if (!DO_ANI(ah))
 		return;
 
-	if (!use_new_ani(ah)) {
-		ath9k_hw_ani_ofdm_err_trigger_old(ah);
-		return;
-	}
-
 	aniState = &ah->curchan->ani;
 
 	if (aniState->ofdmNoiseImmunityLevel < ATH9K_ANI_OFDM_MAX_LEVEL)
-		ath9k_hw_set_ofdm_nil(ah, aniState->ofdmNoiseImmunityLevel + 1);
+		ath9k_hw_set_ofdm_nil(ah, aniState->ofdmNoiseImmunityLevel + 1, false);
 }
 
 /*
  * Set the ANI settings to match an CCK level.
  */
-static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel)
+static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel,
+				 bool scan)
 {
 	struct ar5416AniState *aniState = &ah->curchan->ani;
 	struct ath_common *common = ath9k_hw_common(ah);
 	const struct ani_ofdm_level_entry *entry_ofdm;
 	const struct ani_cck_level_entry *entry_cck;
 
-	aniState->noiseFloor = BEACON_RSSI(ah);
 	ath_dbg(common, ANI, "**** ccklevel %d=>%d, rssi=%d[lo=%d hi=%d]\n",
 		aniState->cckNoiseImmunityLevel, immunityLevel,
-		aniState->noiseFloor, aniState->rssiThrLow,
+		BEACON_RSSI(ah), aniState->rssiThrLow,
 		aniState->rssiThrHigh);
 
-	if ((ah->opmode == NL80211_IFTYPE_STATION ||
-	     ah->opmode == NL80211_IFTYPE_ADHOC) &&
-	    aniState->noiseFloor <= aniState->rssiThrLow &&
+	if (ah->opmode == NL80211_IFTYPE_STATION &&
+	    BEACON_RSSI(ah) <= aniState->rssiThrLow &&
 	    immunityLevel > ATH9K_ANI_CCK_MAX_LEVEL_LOW_RSSI)
 		immunityLevel = ATH9K_ANI_CCK_MAX_LEVEL_LOW_RSSI;
 
-	if (aniState->update_ani)
-		aniState->cckNoiseImmunityLevel =
-			(immunityLevel > ATH9K_ANI_CCK_DEF_LEVEL) ?
-			immunityLevel : ATH9K_ANI_CCK_DEF_LEVEL;
+	if (!scan)
+		aniState->cckNoiseImmunityLevel = immunityLevel;
 
 	entry_ofdm = &ofdm_level_table[aniState->ofdmNoiseImmunityLevel];
 	entry_cck = &cck_level_table[aniState->cckNoiseImmunityLevel];
@@ -359,7 +240,7 @@ static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel)
 	if (!AR_SREV_9300_20_OR_LATER(ah) || AR_SREV_9485(ah))
 		return;
 
-	if (aniState->mrcCCKOff == entry_cck->mrc_cck_on)
+	if (aniState->mrcCCK != entry_cck->mrc_cck_on)
 		ath9k_hw_ani_control(ah,
 				     ATH9K_ANI_MRC_CCK,
 				     entry_cck->mrc_cck_on);
@@ -372,68 +253,11 @@ static void ath9k_hw_ani_cck_err_trigger(struct ath_hw *ah)
 	if (!DO_ANI(ah))
 		return;
 
-	if (!use_new_ani(ah)) {
-		ath9k_hw_ani_cck_err_trigger_old(ah);
-		return;
-	}
-
 	aniState = &ah->curchan->ani;
 
 	if (aniState->cckNoiseImmunityLevel < ATH9K_ANI_CCK_MAX_LEVEL)
-		ath9k_hw_set_cck_nil(ah, aniState->cckNoiseImmunityLevel + 1);
-}
-
-static void ath9k_hw_ani_lower_immunity_old(struct ath_hw *ah)
-{
-	struct ar5416AniState *aniState;
-	int32_t rssi;
-
-	aniState = &ah->curchan->ani;
-
-	if (ah->opmode == NL80211_IFTYPE_AP) {
-		if (aniState->firstepLevel > 0) {
-			if (ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-						 aniState->firstepLevel - 1))
-				return;
-		}
-	} else {
-		rssi = BEACON_RSSI(ah);
-		if (rssi > aniState->rssiThrHigh) {
-			/* XXX: Handle me */
-		} else if (rssi > aniState->rssiThrLow) {
-			if (aniState->ofdmWeakSigDetectOff) {
-				if (ath9k_hw_ani_control(ah,
-					 ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-					 true))
-					return;
-			}
-			if (aniState->firstepLevel > 0) {
-				if (ath9k_hw_ani_control(ah,
-					 ATH9K_ANI_FIRSTEP_LEVEL,
-					 aniState->firstepLevel - 1))
-					return;
-			}
-		} else {
-			if (aniState->firstepLevel > 0) {
-				if (ath9k_hw_ani_control(ah,
-					 ATH9K_ANI_FIRSTEP_LEVEL,
-					 aniState->firstepLevel - 1))
-					return;
-			}
-		}
-	}
-
-	if (aniState->spurImmunityLevel > 0) {
-		if (ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL,
-					 aniState->spurImmunityLevel - 1))
-			return;
-	}
-
-	if (aniState->noiseImmunityLevel > 0) {
-		ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-				     aniState->noiseImmunityLevel - 1);
-		return;
-	}
+		ath9k_hw_set_cck_nil(ah, aniState->cckNoiseImmunityLevel + 1,
+				     false);
 }
 
 /*
@@ -446,87 +270,18 @@ static void ath9k_hw_ani_lower_immunity(struct ath_hw *ah)
 
 	aniState = &ah->curchan->ani;
 
-	if (!use_new_ani(ah)) {
-		ath9k_hw_ani_lower_immunity_old(ah);
-		return;
-	}
-
 	/* lower OFDM noise immunity */
 	if (aniState->ofdmNoiseImmunityLevel > 0 &&
 	    (aniState->ofdmsTurn || aniState->cckNoiseImmunityLevel == 0)) {
-		ath9k_hw_set_ofdm_nil(ah, aniState->ofdmNoiseImmunityLevel - 1);
+		ath9k_hw_set_ofdm_nil(ah, aniState->ofdmNoiseImmunityLevel - 1,
+				      false);
 		return;
 	}
 
 	/* lower CCK noise immunity */
 	if (aniState->cckNoiseImmunityLevel > 0)
-		ath9k_hw_set_cck_nil(ah, aniState->cckNoiseImmunityLevel - 1);
-}
-
-static void ath9k_ani_reset_old(struct ath_hw *ah, bool is_scanning)
-{
-	struct ar5416AniState *aniState;
-	struct ath9k_channel *chan = ah->curchan;
-	struct ath_common *common = ath9k_hw_common(ah);
-
-	if (!DO_ANI(ah))
-		return;
-
-	aniState = &ah->curchan->ani;
-
-	if (ah->opmode != NL80211_IFTYPE_STATION
-	    && ah->opmode != NL80211_IFTYPE_ADHOC) {
-		ath_dbg(common, ANI, "Reset ANI state opmode %u\n", ah->opmode);
-		ah->stats.ast_ani_reset++;
-
-		if (ah->opmode == NL80211_IFTYPE_AP) {
-			/*
-			 * ath9k_hw_ani_control() will only process items set on
-			 * ah->ani_function
-			 */
-			if (IS_CHAN_2GHZ(chan))
-				ah->ani_function = (ATH9K_ANI_SPUR_IMMUNITY_LEVEL |
-						    ATH9K_ANI_FIRSTEP_LEVEL);
-			else
-				ah->ani_function = 0;
-		}
-
-		ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL, 0);
-		ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL, 0);
-		ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL, 0);
-		ath9k_hw_ani_control(ah, ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-				     !ATH9K_ANI_USE_OFDM_WEAK_SIG);
-		ath9k_hw_ani_control(ah, ATH9K_ANI_CCK_WEAK_SIGNAL_THR,
-				     ATH9K_ANI_CCK_WEAK_SIG_THR);
-
-		ath9k_ani_restart(ah);
-		return;
-	}
-
-	if (aniState->noiseImmunityLevel != 0)
-		ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-				     aniState->noiseImmunityLevel);
-	if (aniState->spurImmunityLevel != 0)
-		ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL,
-				     aniState->spurImmunityLevel);
-	if (aniState->ofdmWeakSigDetectOff)
-		ath9k_hw_ani_control(ah, ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-				     !aniState->ofdmWeakSigDetectOff);
-	if (aniState->cckWeakSigThreshold)
-		ath9k_hw_ani_control(ah, ATH9K_ANI_CCK_WEAK_SIGNAL_THR,
-				     aniState->cckWeakSigThreshold);
-	if (aniState->firstepLevel != 0)
-		ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-				     aniState->firstepLevel);
-
-	ath9k_ani_restart(ah);
-
-	ENABLE_REGWRITE_BUFFER(ah);
-
-	REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
-	REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
-
-	REGWRITE_BUFFER_FLUSH(ah);
+		ath9k_hw_set_cck_nil(ah, aniState->cckNoiseImmunityLevel - 1,
+				     false);
 }
 
 /*
@@ -539,13 +294,11 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
 	struct ar5416AniState *aniState = &ah->curchan->ani;
 	struct ath9k_channel *chan = ah->curchan;
 	struct ath_common *common = ath9k_hw_common(ah);
+	int ofdm_nil, cck_nil;
 
 	if (!DO_ANI(ah))
 		return;
 
-	if (!use_new_ani(ah))
-		return ath9k_ani_reset_old(ah, is_scanning);
-
 	BUG_ON(aniState == NULL);
 	ah->stats.ast_ani_reset++;
 
@@ -563,6 +316,11 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
 	/* always allow mode (on/off) to be controlled */
 	ah->ani_function |= ATH9K_ANI_MODE;
 
+	ofdm_nil = max_t(int, ATH9K_ANI_OFDM_DEF_LEVEL,
+			 aniState->ofdmNoiseImmunityLevel);
+	cck_nil = max_t(int, ATH9K_ANI_CCK_DEF_LEVEL,
+			 aniState->cckNoiseImmunityLevel);
+
 	if (is_scanning ||
 	    (ah->opmode != NL80211_IFTYPE_STATION &&
 	     ah->opmode != NL80211_IFTYPE_ADHOC)) {
@@ -585,9 +343,8 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
 				aniState->ofdmNoiseImmunityLevel,
 				aniState->cckNoiseImmunityLevel);
 
-			aniState->update_ani = false;
-			ath9k_hw_set_ofdm_nil(ah, ATH9K_ANI_OFDM_DEF_LEVEL);
-			ath9k_hw_set_cck_nil(ah, ATH9K_ANI_CCK_DEF_LEVEL);
+			ofdm_nil = ATH9K_ANI_OFDM_DEF_LEVEL;
+			cck_nil = ATH9K_ANI_CCK_DEF_LEVEL;
 		}
 	} else {
 		/*
@@ -601,13 +358,9 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
 			is_scanning,
 			aniState->ofdmNoiseImmunityLevel,
 			aniState->cckNoiseImmunityLevel);
-
-			aniState->update_ani = true;
-			ath9k_hw_set_ofdm_nil(ah,
-					      aniState->ofdmNoiseImmunityLevel);
-			ath9k_hw_set_cck_nil(ah,
-					     aniState->cckNoiseImmunityLevel);
 	}
+	ath9k_hw_set_ofdm_nil(ah, ofdm_nil, is_scanning);
+	ath9k_hw_set_cck_nil(ah, cck_nil, is_scanning);
 
 	/*
 	 * enable phy counters if hw supports or if not, enable phy
@@ -627,9 +380,6 @@ static bool ath9k_hw_ani_read_counters(struct ath_hw *ah)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ar5416AniState *aniState = &ah->curchan->ani;
-	u32 ofdm_base = 0;
-	u32 cck_base = 0;
-	u32 ofdmPhyErrCnt, cckPhyErrCnt;
 	u32 phyCnt1, phyCnt2;
 	int32_t listenTime;
 
@@ -642,11 +392,6 @@ static bool ath9k_hw_ani_read_counters(struct ath_hw *ah)
 		return false;
 	}
 
-	if (!use_new_ani(ah)) {
-		ofdm_base = AR_PHY_COUNTMAX - ah->config.ofdm_trig_high;
-		cck_base = AR_PHY_COUNTMAX - ah->config.cck_trig_high;
-	}
-
 	aniState->listenTime += listenTime;
 
 	ath9k_hw_update_mibstats(ah, &ah->ah_mibStats);
@@ -654,35 +399,12 @@ static bool ath9k_hw_ani_read_counters(struct ath_hw *ah)
 	phyCnt1 = REG_READ(ah, AR_PHY_ERR_1);
 	phyCnt2 = REG_READ(ah, AR_PHY_ERR_2);
 
-	if (!use_new_ani(ah) && (phyCnt1 < ofdm_base || phyCnt2 < cck_base)) {
-		if (phyCnt1 < ofdm_base) {
-			ath_dbg(common, ANI,
-				"phyCnt1 0x%x, resetting counter value to 0x%x\n",
-				phyCnt1, ofdm_base);
-			REG_WRITE(ah, AR_PHY_ERR_1, ofdm_base);
-			REG_WRITE(ah, AR_PHY_ERR_MASK_1,
-				  AR_PHY_ERR_OFDM_TIMING);
-		}
-		if (phyCnt2 < cck_base) {
-			ath_dbg(common, ANI,
-				"phyCnt2 0x%x, resetting counter value to 0x%x\n",
-				phyCnt2, cck_base);
-			REG_WRITE(ah, AR_PHY_ERR_2, cck_base);
-			REG_WRITE(ah, AR_PHY_ERR_MASK_2,
-				  AR_PHY_ERR_CCK_TIMING);
-		}
-		return false;
-	}
+	ah->stats.ast_ani_ofdmerrs += phyCnt1 - aniState->ofdmPhyErrCount;
+	aniState->ofdmPhyErrCount = phyCnt1;
 
-	ofdmPhyErrCnt = phyCnt1 - ofdm_base;
-	ah->stats.ast_ani_ofdmerrs +=
-		ofdmPhyErrCnt - aniState->ofdmPhyErrCount;
-	aniState->ofdmPhyErrCount = ofdmPhyErrCnt;
+	ah->stats.ast_ani_cckerrs += phyCnt2 - aniState->cckPhyErrCount;
+	aniState->cckPhyErrCount = phyCnt2;
 
-	cckPhyErrCnt = phyCnt2 - cck_base;
-	ah->stats.ast_ani_cckerrs +=
-		cckPhyErrCnt - aniState->cckPhyErrCount;
-	aniState->cckPhyErrCount = cckPhyErrCnt;
 	return true;
 }
 
@@ -716,21 +438,10 @@ void ath9k_hw_ani_monitor(struct ath_hw *ah, struct ath9k_channel *chan)
 
 	if (aniState->listenTime > ah->aniperiod) {
 		if (cckPhyErrRate < ah->config.cck_trig_low &&
-		    ((ofdmPhyErrRate < ah->config.ofdm_trig_low &&
-		      aniState->ofdmNoiseImmunityLevel <
-		      ATH9K_ANI_OFDM_DEF_LEVEL) ||
-		     (ofdmPhyErrRate < ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI &&
-		      aniState->ofdmNoiseImmunityLevel >=
-		      ATH9K_ANI_OFDM_DEF_LEVEL))) {
+		    ofdmPhyErrRate < ah->config.ofdm_trig_low) {
 			ath9k_hw_ani_lower_immunity(ah);
 			aniState->ofdmsTurn = !aniState->ofdmsTurn;
-		} else if ((ofdmPhyErrRate > ah->config.ofdm_trig_high &&
-			    aniState->ofdmNoiseImmunityLevel >=
-			    ATH9K_ANI_OFDM_DEF_LEVEL) ||
-			   (ofdmPhyErrRate >
-			    ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI &&
-			    aniState->ofdmNoiseImmunityLevel <
-			    ATH9K_ANI_OFDM_DEF_LEVEL)) {
+		} else if (ofdmPhyErrRate > ah->config.ofdm_trig_high) {
 			ath9k_hw_ani_ofdm_err_trigger(ah);
 			aniState->ofdmsTurn = false;
 		} else if (cckPhyErrRate > ah->config.cck_trig_high) {
@@ -778,49 +489,6 @@ void ath9k_hw_disable_mib_counters(struct ath_hw *ah)
 }
 EXPORT_SYMBOL(ath9k_hw_disable_mib_counters);
 
-/*
- * Process a MIB interrupt.  We may potentially be invoked because
- * any of the MIB counters overflow/trigger so don't assume we're
- * here because a PHY error counter triggered.
- */
-void ath9k_hw_proc_mib_event(struct ath_hw *ah)
-{
-	u32 phyCnt1, phyCnt2;
-
-	/* Reset these counters regardless */
-	REG_WRITE(ah, AR_FILT_OFDM, 0);
-	REG_WRITE(ah, AR_FILT_CCK, 0);
-	if (!(REG_READ(ah, AR_SLP_MIB_CTRL) & AR_SLP_MIB_PENDING))
-		REG_WRITE(ah, AR_SLP_MIB_CTRL, AR_SLP_MIB_CLEAR);
-
-	/* Clear the mib counters and save them in the stats */
-	ath9k_hw_update_mibstats(ah, &ah->ah_mibStats);
-
-	if (!DO_ANI(ah)) {
-		/*
-		 * We must always clear the interrupt cause by
-		 * resetting the phy error regs.
-		 */
-		REG_WRITE(ah, AR_PHY_ERR_1, 0);
-		REG_WRITE(ah, AR_PHY_ERR_2, 0);
-		return;
-	}
-
-	/* NB: these are not reset-on-read */
-	phyCnt1 = REG_READ(ah, AR_PHY_ERR_1);
-	phyCnt2 = REG_READ(ah, AR_PHY_ERR_2);
-	if (((phyCnt1 & AR_MIBCNT_INTRMASK) == AR_MIBCNT_INTRMASK) ||
-	    ((phyCnt2 & AR_MIBCNT_INTRMASK) == AR_MIBCNT_INTRMASK)) {
-
-		if (!use_new_ani(ah))
-			ath9k_hw_ani_read_counters(ah);
-
-		/* NB: always restart to insure the h/w counters are reset */
-		ath9k_ani_restart(ah);
-	}
-}
-EXPORT_SYMBOL(ath9k_hw_proc_mib_event);
-
 void ath9k_hw_ani_setup(struct ath_hw *ah)
 {
 	int i;
@@ -845,66 +513,37 @@ void ath9k_hw_ani_init(struct ath_hw *ah)
 
 	ath_dbg(common, ANI, "Initialize ANI\n");
 
-	if (use_new_ani(ah)) {
-		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_NEW;
-		ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_NEW;
+	ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
+	ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW;
 
-		ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH_NEW;
-		ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW_NEW;
-	} else {
-		ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_OLD;
-		ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_OLD;
-
-		ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH_OLD;
-		ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW_OLD;
-	}
+	ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH;
+	ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW;
 
 	for (i = 0; i < ARRAY_SIZE(ah->channels); i++) {
 		struct ath9k_channel *chan = &ah->channels[i];
 		struct ar5416AniState *ani = &chan->ani;
 
-		if (use_new_ani(ah)) {
-			ani->spurImmunityLevel =
-				ATH9K_ANI_SPUR_IMMUNE_LVL_NEW;
+		ani->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
 
-			ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_NEW;
+		ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
 
-			if (AR_SREV_9300_20_OR_LATER(ah))
-				ani->mrcCCKOff =
-					!ATH9K_ANI_ENABLE_MRC_CCK;
-			else
-				ani->mrcCCKOff = true;
-
-			ani->ofdmsTurn = true;
-		} else {
-			ani->spurImmunityLevel =
-				ATH9K_ANI_SPUR_IMMUNE_LVL_OLD;
-			ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_OLD;
-
-			ani->cckWeakSigThreshold =
-				ATH9K_ANI_CCK_WEAK_SIG_THR;
-		}
+		ani->mrcCCK = AR_SREV_9300_20_OR_LATER(ah) ? true : false;
+
+		ani->ofdmsTurn = true;
 
 		ani->rssiThrHigh = ATH9K_ANI_RSSI_THR_HIGH;
 		ani->rssiThrLow = ATH9K_ANI_RSSI_THR_LOW;
-		ani->ofdmWeakSigDetectOff =
-			!ATH9K_ANI_USE_OFDM_WEAK_SIG;
+		ani->ofdmWeakSigDetect = ATH9K_ANI_USE_OFDM_WEAK_SIG;
 		ani->cckNoiseImmunityLevel = ATH9K_ANI_CCK_DEF_LEVEL;
 		ani->ofdmNoiseImmunityLevel = ATH9K_ANI_OFDM_DEF_LEVEL;
-		ani->update_ani = false;
 	}
 
 	/*
 	 * since we expect some ongoing maintenance on the tables, let's sanity
 	 * check here default level should not modify INI setting.
 	 */
-	if (use_new_ani(ah)) {
-		ah->aniperiod = ATH9K_ANI_PERIOD_NEW;
-		ah->config.ani_poll_interval = ATH9K_ANI_POLLINTERVAL_NEW;
-	} else {
-		ah->aniperiod = ATH9K_ANI_PERIOD_OLD;
-		ah->config.ani_poll_interval = ATH9K_ANI_POLLINTERVAL_OLD;
-	}
+	ah->aniperiod = ATH9K_ANI_PERIOD;
+	ah->config.ani_poll_interval = ATH9K_ANI_POLLINTERVAL;
 
 	if (ah->config.enable_ani)
 		ah->proc_phyerr |= HAL_PROCESS_ANI;
diff --git a/drivers/net/wireless/ath/ath9k/ani.h b/drivers/net/wireless/ath/ath9k/ani.h
index 72e2b874e17..1485bf5e351 100644
--- a/drivers/net/wireless/ath/ath9k/ani.h
+++ b/drivers/net/wireless/ath/ath9k/ani.h
@@ -24,42 +24,34 @@
 #define BEACON_RSSI(ahp) (ahp->stats.avgbrssi)
 
 /* units are errors per second */
-#define ATH9K_ANI_OFDM_TRIG_HIGH_OLD      500
-#define ATH9K_ANI_OFDM_TRIG_HIGH_NEW      3500
+#define ATH9K_ANI_OFDM_TRIG_HIGH          3500
 #define ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI 1000
 
 /* units are errors per second */
-#define ATH9K_ANI_OFDM_TRIG_LOW_OLD       200
-#define ATH9K_ANI_OFDM_TRIG_LOW_NEW       400
+#define ATH9K_ANI_OFDM_TRIG_LOW           400
 #define ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI 900
 
 /* units are errors per second */
-#define ATH9K_ANI_CCK_TRIG_HIGH_OLD       200
-#define ATH9K_ANI_CCK_TRIG_HIGH_NEW       600
+#define ATH9K_ANI_CCK_TRIG_HIGH           600
 
 /* units are errors per second */
-#define ATH9K_ANI_CCK_TRIG_LOW_OLD        100
-#define ATH9K_ANI_CCK_TRIG_LOW_NEW        300
+#define ATH9K_ANI_CCK_TRIG_LOW            300
 
 #define ATH9K_ANI_NOISE_IMMUNE_LVL        4
 #define ATH9K_ANI_USE_OFDM_WEAK_SIG       true
 #define ATH9K_ANI_CCK_WEAK_SIG_THR        false
 
-#define ATH9K_ANI_SPUR_IMMUNE_LVL_OLD     7
-#define ATH9K_ANI_SPUR_IMMUNE_LVL_NEW     3
+#define ATH9K_ANI_SPUR_IMMUNE_LVL         3
 
-#define ATH9K_ANI_FIRSTEP_LVL_OLD         0
-#define ATH9K_ANI_FIRSTEP_LVL_NEW         2
+#define ATH9K_ANI_FIRSTEP_LVL             2
 
 #define ATH9K_ANI_RSSI_THR_HIGH           40
 #define ATH9K_ANI_RSSI_THR_LOW            7
 
-#define ATH9K_ANI_PERIOD_OLD              100
-#define ATH9K_ANI_PERIOD_NEW              300
+#define ATH9K_ANI_PERIOD                  300
 
 /* in ms */
-#define ATH9K_ANI_POLLINTERVAL_OLD        100
-#define ATH9K_ANI_POLLINTERVAL_NEW        1000
+#define ATH9K_ANI_POLLINTERVAL            1000
 
 #define HAL_NOISE_IMMUNE_MAX              4
 #define HAL_SPUR_IMMUNE_MAX               7
@@ -70,8 +62,6 @@
 #define ATH9K_SIG_SPUR_IMM_SETTING_MIN    0
 #define ATH9K_SIG_SPUR_IMM_SETTING_MAX    22
 
-#define ATH9K_ANI_ENABLE_MRC_CCK          true
-
 /* values here are relative to the INI */
 
 enum ath9k_ani_cmd {
@@ -119,16 +109,14 @@ struct ar5416AniState {
 	u8 ofdmNoiseImmunityLevel;
 	u8 cckNoiseImmunityLevel;
 	bool ofdmsTurn;
-	u8 mrcCCKOff;
+	u8 mrcCCK;
 	u8 spurImmunityLevel;
 	u8 firstepLevel;
-	u8 ofdmWeakSigDetectOff;
+	u8 ofdmWeakSigDetect;
 	u8 cckWeakSigThreshold;
-	bool update_ani;
 	u32 listenTime;
 	int32_t rssiThrLow;
 	int32_t rssiThrHigh;
-	u32 noiseFloor;
 	u32 ofdmPhyErrCount;
 	u32 cckPhyErrCount;
 	int16_t pktRssi[2];
diff --git a/drivers/net/wireless/ath/ath9k/antenna.c b/drivers/net/wireless/ath/ath9k/antenna.c
new file mode 100644
index 00000000000..bbcfeb3b2a6
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/antenna.c
@@ -0,0 +1,776 @@
+/*
+ * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "ath9k.h"
+
+static inline bool ath_is_alt_ant_ratio_better(int alt_ratio, int maxdelta,
+					       int mindelta, int main_rssi_avg,
+					       int alt_rssi_avg, int pkt_count)
+{
+	return (((alt_ratio >= ATH_ANT_DIV_COMB_ALT_ANT_RATIO2) &&
+		 (alt_rssi_avg > main_rssi_avg + maxdelta)) ||
+		(alt_rssi_avg > main_rssi_avg + mindelta)) && (pkt_count > 50);
+}
+
+static inline bool ath_ant_div_comb_alt_check(u8 div_group, int alt_ratio,
+					      int curr_main_set, int curr_alt_set,
+					      int alt_rssi_avg, int main_rssi_avg)
+{
+	bool result = false;
+	switch (div_group) {
+	case 0:
+		if (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO)
+			result = true;
+		break;
+	case 1:
+	case 2:
+		if ((((curr_main_set == ATH_ANT_DIV_COMB_LNA2) &&
+		      (curr_alt_set == ATH_ANT_DIV_COMB_LNA1) &&
+		      (alt_rssi_avg >= (main_rssi_avg - 5))) ||
+		     ((curr_main_set == ATH_ANT_DIV_COMB_LNA1) &&
+		      (curr_alt_set == ATH_ANT_DIV_COMB_LNA2) &&
+		      (alt_rssi_avg >= (main_rssi_avg - 2)))) &&
+		    (alt_rssi_avg >= 4))
+			result = true;
+		else
+			result = false;
+		break;
+	}
+
+	return result;
+}
+
+static void ath_lnaconf_alt_good_scan(struct ath_ant_comb *antcomb,
+				      struct ath_hw_antcomb_conf ant_conf,
+				      int main_rssi_avg)
+{
+	antcomb->quick_scan_cnt = 0;
+
+	if (ant_conf.main_lna_conf == ATH_ANT_DIV_COMB_LNA2)
+		antcomb->rssi_lna2 = main_rssi_avg;
+	else if (ant_conf.main_lna_conf == ATH_ANT_DIV_COMB_LNA1)
+		antcomb->rssi_lna1 = main_rssi_avg;
+
+	switch ((ant_conf.main_lna_conf << 4) | ant_conf.alt_lna_conf) {
+	case 0x10: /* LNA2 A-B */
+		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+		antcomb->first_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA1;
+		break;
+	case 0x20: /* LNA1 A-B */
+		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+		antcomb->first_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA2;
+		break;
+	case 0x21: /* LNA1 LNA2 */
+		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA2;
+		antcomb->first_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+		antcomb->second_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+		break;
+	case 0x12: /* LNA2 LNA1 */
+		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1;
+		antcomb->first_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+		antcomb->second_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+		break;
+	case 0x13: /* LNA2 A+B */
+		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+		antcomb->first_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA1;
+		break;
+	case 0x23: /* LNA1 A+B */
+		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+		antcomb->first_quick_scan_conf =
+			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA2;
+		break;
+	default:
+		break;
+	}
+}
+
+static void ath_select_ant_div_from_quick_scan(struct ath_ant_comb *antcomb,
+				       struct ath_hw_antcomb_conf *div_ant_conf,
+				       int main_rssi_avg, int alt_rssi_avg,
+				       int alt_ratio)
+{
+	/* alt_good */
+	switch (antcomb->quick_scan_cnt) {
+	case 0:
+		/* set alt to main, and alt to first conf */
+		div_ant_conf->main_lna_conf = antcomb->main_conf;
+		div_ant_conf->alt_lna_conf = antcomb->first_quick_scan_conf;
+		break;
+	case 1:
+		/* set alt to main, and alt to first conf */
+		div_ant_conf->main_lna_conf = antcomb->main_conf;
+		div_ant_conf->alt_lna_conf = antcomb->second_quick_scan_conf;
+		antcomb->rssi_first = main_rssi_avg;
+		antcomb->rssi_second = alt_rssi_avg;
+
+		if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1) {
+			/* main is LNA1 */
+			if (ath_is_alt_ant_ratio_better(alt_ratio,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_HI,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
+						main_rssi_avg, alt_rssi_avg,
+						antcomb->total_pkt_count))
+				antcomb->first_ratio = true;
+			else
+				antcomb->first_ratio = false;
+		} else if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2) {
+			if (ath_is_alt_ant_ratio_better(alt_ratio,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_MID,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
+						main_rssi_avg, alt_rssi_avg,
+						antcomb->total_pkt_count))
+				antcomb->first_ratio = true;
+			else
+				antcomb->first_ratio = false;
+		} else {
+			if ((((alt_ratio >= ATH_ANT_DIV_COMB_ALT_ANT_RATIO2) &&
+			      (alt_rssi_avg > main_rssi_avg +
+			       ATH_ANT_DIV_COMB_LNA1_DELTA_HI)) ||
+			     (alt_rssi_avg > main_rssi_avg)) &&
+			    (antcomb->total_pkt_count > 50))
+				antcomb->first_ratio = true;
+			else
+				antcomb->first_ratio = false;
+		}
+		break;
+	case 2:
+		antcomb->alt_good = false;
+		antcomb->scan_not_start = false;
+		antcomb->scan = false;
+		antcomb->rssi_first = main_rssi_avg;
+		antcomb->rssi_third = alt_rssi_avg;
+
+		if (antcomb->second_quick_scan_conf == ATH_ANT_DIV_COMB_LNA1)
+			antcomb->rssi_lna1 = alt_rssi_avg;
+		else if (antcomb->second_quick_scan_conf ==
+			 ATH_ANT_DIV_COMB_LNA2)
+			antcomb->rssi_lna2 = alt_rssi_avg;
+		else if (antcomb->second_quick_scan_conf ==
+			 ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2) {
+			if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2)
+				antcomb->rssi_lna2 = main_rssi_avg;
+			else if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1)
+				antcomb->rssi_lna1 = main_rssi_avg;
+		}
+
+		if (antcomb->rssi_lna2 > antcomb->rssi_lna1 +
+		    ATH_ANT_DIV_COMB_LNA1_LNA2_SWITCH_DELTA)
+			div_ant_conf->main_lna_conf = ATH_ANT_DIV_COMB_LNA2;
+		else
+			div_ant_conf->main_lna_conf = ATH_ANT_DIV_COMB_LNA1;
+
+		if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1) {
+			if (ath_is_alt_ant_ratio_better(alt_ratio,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_HI,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
+						main_rssi_avg, alt_rssi_avg,
+						antcomb->total_pkt_count))
+				antcomb->second_ratio = true;
+			else
+				antcomb->second_ratio = false;
+		} else if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2) {
+			if (ath_is_alt_ant_ratio_better(alt_ratio,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_MID,
+						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
+						main_rssi_avg, alt_rssi_avg,
+						antcomb->total_pkt_count))
+				antcomb->second_ratio = true;
+			else
+				antcomb->second_ratio = false;
+		} else {
+			if ((((alt_ratio >= ATH_ANT_DIV_COMB_ALT_ANT_RATIO2) &&
+			      (alt_rssi_avg > main_rssi_avg +
+			       ATH_ANT_DIV_COMB_LNA1_DELTA_HI)) ||
+			     (alt_rssi_avg > main_rssi_avg)) &&
+			    (antcomb->total_pkt_count > 50))
+				antcomb->second_ratio = true;
+			else
+				antcomb->second_ratio = false;
+		}
+
+		/* set alt to the conf with maximun ratio */
+		if (antcomb->first_ratio && antcomb->second_ratio) {
+			if (antcomb->rssi_second > antcomb->rssi_third) {
+				/* first alt*/
+				if ((antcomb->first_quick_scan_conf ==
+				    ATH_ANT_DIV_COMB_LNA1) ||
+				    (antcomb->first_quick_scan_conf ==
+				    ATH_ANT_DIV_COMB_LNA2))
+					/* Set alt LNA1 or LNA2*/
+					if (div_ant_conf->main_lna_conf ==
+					    ATH_ANT_DIV_COMB_LNA2)
+						div_ant_conf->alt_lna_conf =
+							ATH_ANT_DIV_COMB_LNA1;
+					else
+						div_ant_conf->alt_lna_conf =
+							ATH_ANT_DIV_COMB_LNA2;
+				else
+					/* Set alt to A+B or A-B */
+					div_ant_conf->alt_lna_conf =
+						antcomb->first_quick_scan_conf;
+			} else if ((antcomb->second_quick_scan_conf ==
+				   ATH_ANT_DIV_COMB_LNA1) ||
+				   (antcomb->second_quick_scan_conf ==
+				   ATH_ANT_DIV_COMB_LNA2)) {
+				/* Set alt LNA1 or LNA2 */
+				if (div_ant_conf->main_lna_conf ==
+				    ATH_ANT_DIV_COMB_LNA2)
+					div_ant_conf->alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+				else
+					div_ant_conf->alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+			} else {
+				/* Set alt to A+B or A-B */
+				div_ant_conf->alt_lna_conf =
+					antcomb->second_quick_scan_conf;
+			}
+		} else if (antcomb->first_ratio) {
+			/* first alt */
+			if ((antcomb->first_quick_scan_conf ==
+			    ATH_ANT_DIV_COMB_LNA1) ||
+			    (antcomb->first_quick_scan_conf ==
+			    ATH_ANT_DIV_COMB_LNA2))
+					/* Set alt LNA1 or LNA2 */
+				if (div_ant_conf->main_lna_conf ==
+				    ATH_ANT_DIV_COMB_LNA2)
+					div_ant_conf->alt_lna_conf =
+							ATH_ANT_DIV_COMB_LNA1;
+				else
+					div_ant_conf->alt_lna_conf =
+							ATH_ANT_DIV_COMB_LNA2;
+			else
+				/* Set alt to A+B or A-B */
+				div_ant_conf->alt_lna_conf =
+						antcomb->first_quick_scan_conf;
+		} else if (antcomb->second_ratio) {
+				/* second alt */
+			if ((antcomb->second_quick_scan_conf ==
+			    ATH_ANT_DIV_COMB_LNA1) ||
+			    (antcomb->second_quick_scan_conf ==
+			    ATH_ANT_DIV_COMB_LNA2))
+				/* Set alt LNA1 or LNA2 */
+				if (div_ant_conf->main_lna_conf ==
+				    ATH_ANT_DIV_COMB_LNA2)
+					div_ant_conf->alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+				else
+					div_ant_conf->alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+			else
+				/* Set alt to A+B or A-B */
+				div_ant_conf->alt_lna_conf =
+						antcomb->second_quick_scan_conf;
+		} else {
+			/* main is largest */
+			if ((antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1) ||
+			    (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2))
+				/* Set alt LNA1 or LNA2 */
+				if (div_ant_conf->main_lna_conf ==
+				    ATH_ANT_DIV_COMB_LNA2)
+					div_ant_conf->alt_lna_conf =
+							ATH_ANT_DIV_COMB_LNA1;
+				else
+					div_ant_conf->alt_lna_conf =
+							ATH_ANT_DIV_COMB_LNA2;
+			else
+				/* Set alt to A+B or A-B */
+				div_ant_conf->alt_lna_conf = antcomb->main_conf;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static void ath_ant_div_conf_fast_divbias(struct ath_hw_antcomb_conf *ant_conf,
+					  struct ath_ant_comb *antcomb,
+					  int alt_ratio)
+{
+	if (ant_conf->div_group == 0) {
+		/* Adjust the fast_div_bias based on main and alt lna conf */
+		switch ((ant_conf->main_lna_conf << 4) |
+				ant_conf->alt_lna_conf) {
+		case 0x01: /* A-B LNA2 */
+			ant_conf->fast_div_bias = 0x3b;
+			break;
+		case 0x02: /* A-B LNA1 */
+			ant_conf->fast_div_bias = 0x3d;
+			break;
+		case 0x03: /* A-B A+B */
+			ant_conf->fast_div_bias = 0x1;
+			break;
+		case 0x10: /* LNA2 A-B */
+			ant_conf->fast_div_bias = 0x7;
+			break;
+		case 0x12: /* LNA2 LNA1 */
+			ant_conf->fast_div_bias = 0x2;
+			break;
+		case 0x13: /* LNA2 A+B */
+			ant_conf->fast_div_bias = 0x7;
+			break;
+		case 0x20: /* LNA1 A-B */
+			ant_conf->fast_div_bias = 0x6;
+			break;
+		case 0x21: /* LNA1 LNA2 */
+			ant_conf->fast_div_bias = 0x0;
+			break;
+		case 0x23: /* LNA1 A+B */
+			ant_conf->fast_div_bias = 0x6;
+			break;
+		case 0x30: /* A+B A-B */
+			ant_conf->fast_div_bias = 0x1;
+			break;
+		case 0x31: /* A+B LNA2 */
+			ant_conf->fast_div_bias = 0x3b;
+			break;
+		case 0x32: /* A+B LNA1 */
+			ant_conf->fast_div_bias = 0x3d;
+			break;
+		default:
+			break;
+		}
+	} else if (ant_conf->div_group == 1) {
+		/* Adjust the fast_div_bias based on main and alt_lna_conf */
+		switch ((ant_conf->main_lna_conf << 4) |
+			ant_conf->alt_lna_conf) {
+		case 0x01: /* A-B LNA2 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x02: /* A-B LNA1 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x03: /* A-B A+B */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x10: /* LNA2 A-B */
+			if (!(antcomb->scan) &&
+			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x3f;
+			else
+				ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x12: /* LNA2 LNA1 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x13: /* LNA2 A+B */
+			if (!(antcomb->scan) &&
+			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x3f;
+			else
+				ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x20: /* LNA1 A-B */
+			if (!(antcomb->scan) &&
+			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x3f;
+			else
+				ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x21: /* LNA1 LNA2 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x23: /* LNA1 A+B */
+			if (!(antcomb->scan) &&
+			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x3f;
+			else
+				ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x30: /* A+B A-B */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x31: /* A+B LNA2 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x32: /* A+B LNA1 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		default:
+			break;
+		}
+	} else if (ant_conf->div_group == 2) {
+		/* Adjust the fast_div_bias based on main and alt_lna_conf */
+		switch ((ant_conf->main_lna_conf << 4) |
+				ant_conf->alt_lna_conf) {
+		case 0x01: /* A-B LNA2 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x02: /* A-B LNA1 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x03: /* A-B A+B */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x10: /* LNA2 A-B */
+			if (!(antcomb->scan) &&
+				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x1;
+			else
+				ant_conf->fast_div_bias = 0x2;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x12: /* LNA2 LNA1 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x13: /* LNA2 A+B */
+			if (!(antcomb->scan) &&
+				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x1;
+			else
+				ant_conf->fast_div_bias = 0x2;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x20: /* LNA1 A-B */
+			if (!(antcomb->scan) &&
+				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x1;
+			else
+				ant_conf->fast_div_bias = 0x2;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x21: /* LNA1 LNA2 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x23: /* LNA1 A+B */
+			if (!(antcomb->scan) &&
+				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
+				ant_conf->fast_div_bias = 0x1;
+			else
+				ant_conf->fast_div_bias = 0x2;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x30: /* A+B A-B */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x31: /* A+B LNA2 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		case 0x32: /* A+B LNA1 */
+			ant_conf->fast_div_bias = 0x1;
+			ant_conf->main_gaintb = 0;
+			ant_conf->alt_gaintb = 0;
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+void ath_ant_comb_scan(struct ath_softc *sc, struct ath_rx_status *rs)
+{
+	struct ath_hw_antcomb_conf div_ant_conf;
+	struct ath_ant_comb *antcomb = &sc->ant_comb;
+	int alt_ratio = 0, alt_rssi_avg = 0, main_rssi_avg = 0, curr_alt_set;
+	int curr_main_set;
+	int main_rssi = rs->rs_rssi_ctl0;
+	int alt_rssi = rs->rs_rssi_ctl1;
+	int rx_ant_conf,  main_ant_conf;
+	bool short_scan = false;
+
+	rx_ant_conf = (rs->rs_rssi_ctl2 >> ATH_ANT_RX_CURRENT_SHIFT) &
+		       ATH_ANT_RX_MASK;
+	main_ant_conf = (rs->rs_rssi_ctl2 >> ATH_ANT_RX_MAIN_SHIFT) &
+			 ATH_ANT_RX_MASK;
+
+	/* Record packet only when both main_rssi and  alt_rssi is positive */
+	if (main_rssi > 0 && alt_rssi > 0) {
+		antcomb->total_pkt_count++;
+		antcomb->main_total_rssi += main_rssi;
+		antcomb->alt_total_rssi  += alt_rssi;
+		if (main_ant_conf == rx_ant_conf)
+			antcomb->main_recv_cnt++;
+		else
+			antcomb->alt_recv_cnt++;
+	}
+
+	/* Short scan check */
+	if (antcomb->scan && antcomb->alt_good) {
+		if (time_after(jiffies, antcomb->scan_start_time +
+		    msecs_to_jiffies(ATH_ANT_DIV_COMB_SHORT_SCAN_INTR)))
+			short_scan = true;
+		else
+			if (antcomb->total_pkt_count ==
+			    ATH_ANT_DIV_COMB_SHORT_SCAN_PKTCOUNT) {
+				alt_ratio = ((antcomb->alt_recv_cnt * 100) /
+					    antcomb->total_pkt_count);
+				if (alt_ratio < ATH_ANT_DIV_COMB_ALT_ANT_RATIO)
+					short_scan = true;
+			}
+	}
+
+	if (((antcomb->total_pkt_count < ATH_ANT_DIV_COMB_MAX_PKTCOUNT) ||
+	    rs->rs_moreaggr) && !short_scan)
+		return;
+
+	if (antcomb->total_pkt_count) {
+		alt_ratio = ((antcomb->alt_recv_cnt * 100) /
+			     antcomb->total_pkt_count);
+		main_rssi_avg = (antcomb->main_total_rssi /
+				 antcomb->total_pkt_count);
+		alt_rssi_avg = (antcomb->alt_total_rssi /
+				 antcomb->total_pkt_count);
+	}
+
+
+	ath9k_hw_antdiv_comb_conf_get(sc->sc_ah, &div_ant_conf);
+	curr_alt_set = div_ant_conf.alt_lna_conf;
+	curr_main_set = div_ant_conf.main_lna_conf;
+
+	antcomb->count++;
+
+	if (antcomb->count == ATH_ANT_DIV_COMB_MAX_COUNT) {
+		if (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO) {
+			ath_lnaconf_alt_good_scan(antcomb, div_ant_conf,
+						  main_rssi_avg);
+			antcomb->alt_good = true;
+		} else {
+			antcomb->alt_good = false;
+		}
+
+		antcomb->count = 0;
+		antcomb->scan = true;
+		antcomb->scan_not_start = true;
+	}
+
+	if (!antcomb->scan) {
+		if (ath_ant_div_comb_alt_check(div_ant_conf.div_group,
+					alt_ratio, curr_main_set, curr_alt_set,
+					alt_rssi_avg, main_rssi_avg)) {
+			if (curr_alt_set == ATH_ANT_DIV_COMB_LNA2) {
+				/* Switch main and alt LNA */
+				div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+				div_ant_conf.alt_lna_conf  =
+						ATH_ANT_DIV_COMB_LNA1;
+			} else if (curr_alt_set == ATH_ANT_DIV_COMB_LNA1) {
+				div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+				div_ant_conf.alt_lna_conf  =
+						ATH_ANT_DIV_COMB_LNA2;
+			}
+
+			goto div_comb_done;
+		} else if ((curr_alt_set != ATH_ANT_DIV_COMB_LNA1) &&
+			   (curr_alt_set != ATH_ANT_DIV_COMB_LNA2)) {
+			/* Set alt to another LNA */
+			if (curr_main_set == ATH_ANT_DIV_COMB_LNA2)
+				div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+			else if (curr_main_set == ATH_ANT_DIV_COMB_LNA1)
+				div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+
+			goto div_comb_done;
+		}
+
+		if ((alt_rssi_avg < (main_rssi_avg +
+				     div_ant_conf.lna1_lna2_delta)))
+			goto div_comb_done;
+	}
+
+	if (!antcomb->scan_not_start) {
+		switch (curr_alt_set) {
+		case ATH_ANT_DIV_COMB_LNA2:
+			antcomb->rssi_lna2 = alt_rssi_avg;
+			antcomb->rssi_lna1 = main_rssi_avg;
+			antcomb->scan = true;
+			/* set to A+B */
+			div_ant_conf.main_lna_conf =
+				ATH_ANT_DIV_COMB_LNA1;
+			div_ant_conf.alt_lna_conf  =
+				ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+			break;
+		case ATH_ANT_DIV_COMB_LNA1:
+			antcomb->rssi_lna1 = alt_rssi_avg;
+			antcomb->rssi_lna2 = main_rssi_avg;
+			antcomb->scan = true;
+			/* set to A+B */
+			div_ant_conf.main_lna_conf = ATH_ANT_DIV_COMB_LNA2;
+			div_ant_conf.alt_lna_conf  =
+				ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+			break;
+		case ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2:
+			antcomb->rssi_add = alt_rssi_avg;
+			antcomb->scan = true;
+			/* set to A-B */
+			div_ant_conf.alt_lna_conf =
+				ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+			break;
+		case ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2:
+			antcomb->rssi_sub = alt_rssi_avg;
+			antcomb->scan = false;
+			if (antcomb->rssi_lna2 >
+			    (antcomb->rssi_lna1 +
+			    ATH_ANT_DIV_COMB_LNA1_LNA2_SWITCH_DELTA)) {
+				/* use LNA2 as main LNA */
+				if ((antcomb->rssi_add > antcomb->rssi_lna1) &&
+				    (antcomb->rssi_add > antcomb->rssi_sub)) {
+					/* set to A+B */
+					div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+					div_ant_conf.alt_lna_conf  =
+						ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+				} else if (antcomb->rssi_sub >
+					   antcomb->rssi_lna1) {
+					/* set to A-B */
+					div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+					div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+				} else {
+					/* set to LNA1 */
+					div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+					div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+				}
+			} else {
+				/* use LNA1 as main LNA */
+				if ((antcomb->rssi_add > antcomb->rssi_lna2) &&
+				    (antcomb->rssi_add > antcomb->rssi_sub)) {
+					/* set to A+B */
+					div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+					div_ant_conf.alt_lna_conf  =
+						ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
+				} else if (antcomb->rssi_sub >
+					   antcomb->rssi_lna1) {
+					/* set to A-B */
+					div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+					div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
+				} else {
+					/* set to LNA2 */
+					div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+					div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+				}
+			}
+			break;
+		default:
+			break;
+		}
+	} else {
+		if (!antcomb->alt_good) {
+			antcomb->scan_not_start = false;
+			/* Set alt to another LNA */
+			if (curr_main_set == ATH_ANT_DIV_COMB_LNA2) {
+				div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+				div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+			} else if (curr_main_set == ATH_ANT_DIV_COMB_LNA1) {
+				div_ant_conf.main_lna_conf =
+						ATH_ANT_DIV_COMB_LNA1;
+				div_ant_conf.alt_lna_conf =
+						ATH_ANT_DIV_COMB_LNA2;
+			}
+			goto div_comb_done;
+		}
+	}
+
+	ath_select_ant_div_from_quick_scan(antcomb, &div_ant_conf,
+					   main_rssi_avg, alt_rssi_avg,
+					   alt_ratio);
+
+	antcomb->quick_scan_cnt++;
+
+div_comb_done:
+	ath_ant_div_conf_fast_divbias(&div_ant_conf, antcomb, alt_ratio);
+	ath9k_hw_antdiv_comb_conf_set(sc->sc_ah, &div_ant_conf);
+
+	antcomb->scan_start_time = jiffies;
+	antcomb->total_pkt_count = 0;
+	antcomb->main_total_rssi = 0;
+	antcomb->alt_total_rssi = 0;
+	antcomb->main_recv_cnt = 0;
+	antcomb->alt_recv_cnt = 0;
+}
+
+void ath_ant_comb_update(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_hw_antcomb_conf div_ant_conf;
+	u8 lna_conf;
+
+	ath9k_hw_antdiv_comb_conf_get(ah, &div_ant_conf);
+
+	if (sc->ant_rx == 1)
+		lna_conf = ATH_ANT_DIV_COMB_LNA1;
+	else
+		lna_conf = ATH_ANT_DIV_COMB_LNA2;
+
+	div_ant_conf.main_lna_conf = lna_conf;
+	div_ant_conf.alt_lna_conf = lna_conf;
+
+	ath9k_hw_antdiv_comb_conf_set(ah, &div_ant_conf);
+}
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index c7492c6a251..874186bfda4 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -995,141 +995,6 @@ static u32 ar5008_hw_compute_pll_control(struct ath_hw *ah,
 	return pll;
 }
 
-static bool ar5008_hw_ani_control_old(struct ath_hw *ah,
-				      enum ath9k_ani_cmd cmd,
-				      int param)
-{
-	struct ar5416AniState *aniState = &ah->curchan->ani;
-	struct ath_common *common = ath9k_hw_common(ah);
-
-	switch (cmd & ah->ani_function) {
-	case ATH9K_ANI_NOISE_IMMUNITY_LEVEL:{
-		u32 level = param;
-
-		if (level >= ARRAY_SIZE(ah->totalSizeDesired)) {
-			ath_dbg(common, ANI, "level out of range (%u > %zu)\n",
-				level, ARRAY_SIZE(ah->totalSizeDesired));
-			return false;
-		}
-
-		REG_RMW_FIELD(ah, AR_PHY_DESIRED_SZ,
-			      AR_PHY_DESIRED_SZ_TOT_DES,
-			      ah->totalSizeDesired[level]);
-		REG_RMW_FIELD(ah, AR_PHY_AGC_CTL1,
-			      AR_PHY_AGC_CTL1_COARSE_LOW,
-			      ah->coarse_low[level]);
-		REG_RMW_FIELD(ah, AR_PHY_AGC_CTL1,
-			      AR_PHY_AGC_CTL1_COARSE_HIGH,
-			      ah->coarse_high[level]);
-		REG_RMW_FIELD(ah, AR_PHY_FIND_SIG,
-			      AR_PHY_FIND_SIG_FIRPWR,
-			      ah->firpwr[level]);
-
-		if (level > aniState->noiseImmunityLevel)
-			ah->stats.ast_ani_niup++;
-		else if (level < aniState->noiseImmunityLevel)
-			ah->stats.ast_ani_nidown++;
-		aniState->noiseImmunityLevel = level;
-		break;
-	}
-	case ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION:{
-		u32 on = param ? 1 : 0;
-
-		if (on)
-			REG_SET_BIT(ah, AR_PHY_SFCORR_LOW,
-				    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
-		else
-			REG_CLR_BIT(ah, AR_PHY_SFCORR_LOW,
-				    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
-
-		if (!on != aniState->ofdmWeakSigDetectOff) {
-			if (on)
-				ah->stats.ast_ani_ofdmon++;
-			else
-				ah->stats.ast_ani_ofdmoff++;
-			aniState->ofdmWeakSigDetectOff = !on;
-		}
-		break;
-	}
-	case ATH9K_ANI_CCK_WEAK_SIGNAL_THR:{
-		static const int weakSigThrCck[] = { 8, 6 };
-		u32 high = param ? 1 : 0;
-
-		REG_RMW_FIELD(ah, AR_PHY_CCK_DETECT,
-			      AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK,
-			      weakSigThrCck[high]);
-		if (high != aniState->cckWeakSigThreshold) {
-			if (high)
-				ah->stats.ast_ani_cckhigh++;
-			else
-				ah->stats.ast_ani_ccklow++;
-			aniState->cckWeakSigThreshold = high;
-		}
-		break;
-	}
-	case ATH9K_ANI_FIRSTEP_LEVEL:{
-		static const int firstep[] = { 0, 4, 8 };
-		u32 level = param;
-
-		if (level >= ARRAY_SIZE(firstep)) {
-			ath_dbg(common, ANI, "level out of range (%u > %zu)\n",
-				level, ARRAY_SIZE(firstep));
-			return false;
-		}
-		REG_RMW_FIELD(ah, AR_PHY_FIND_SIG,
-			      AR_PHY_FIND_SIG_FIRSTEP,
-			      firstep[level]);
-		if (level > aniState->firstepLevel)
-			ah->stats.ast_ani_stepup++;
-		else if (level < aniState->firstepLevel)
-			ah->stats.ast_ani_stepdown++;
-		aniState->firstepLevel = level;
-		break;
-	}
-	case ATH9K_ANI_SPUR_IMMUNITY_LEVEL:{
-		static const int cycpwrThr1[] = { 2, 4, 6, 8, 10, 12, 14, 16 };
-		u32 level = param;
-
-		if (level >= ARRAY_SIZE(cycpwrThr1)) {
-			ath_dbg(common, ANI, "level out of range (%u > %zu)\n",
-				level, ARRAY_SIZE(cycpwrThr1));
-			return false;
-		}
-		REG_RMW_FIELD(ah, AR_PHY_TIMING5,
-			      AR_PHY_TIMING5_CYCPWR_THR1,
-			      cycpwrThr1[level]);
-		if (level > aniState->spurImmunityLevel)
-			ah->stats.ast_ani_spurup++;
-		else if (level < aniState->spurImmunityLevel)
-			ah->stats.ast_ani_spurdown++;
-		aniState->spurImmunityLevel = level;
-		break;
-	}
-	case ATH9K_ANI_PRESENT:
-		break;
-	default:
-		ath_dbg(common, ANI, "invalid cmd %u\n", cmd);
-		return false;
-	}
-
-	ath_dbg(common, ANI, "ANI parameters:\n");
-	ath_dbg(common, ANI,
-		"noiseImmunityLevel=%d, spurImmunityLevel=%d, ofdmWeakSigDetectOff=%d\n",
-		aniState->noiseImmunityLevel,
-		aniState->spurImmunityLevel,
-		!aniState->ofdmWeakSigDetectOff);
-	ath_dbg(common, ANI,
-		"cckWeakSigThreshold=%d, firstepLevel=%d, listenTime=%d\n",
-		aniState->cckWeakSigThreshold,
-		aniState->firstepLevel,
-		aniState->listenTime);
-	ath_dbg(common, ANI, "ofdmPhyErrCount=%d, cckPhyErrCount=%d\n\n",
-		aniState->ofdmPhyErrCount,
-		aniState->cckPhyErrCount);
-
-	return true;
-}
-
 static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 				      enum ath9k_ani_cmd cmd,
 				      int param)
@@ -1206,18 +1071,18 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 			REG_CLR_BIT(ah, AR_PHY_SFCORR_LOW,
 				    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
 
-		if (!on != aniState->ofdmWeakSigDetectOff) {
+		if (on != aniState->ofdmWeakSigDetect) {
 			ath_dbg(common, ANI,
 				"** ch %d: ofdm weak signal: %s=>%s\n",
 				chan->channel,
-				!aniState->ofdmWeakSigDetectOff ?
+				aniState->ofdmWeakSigDetect ?
 				"on" : "off",
 				on ? "on" : "off");
 			if (on)
 				ah->stats.ast_ani_ofdmon++;
 			else
 				ah->stats.ast_ani_ofdmoff++;
-			aniState->ofdmWeakSigDetectOff = !on;
+			aniState->ofdmWeakSigDetect = on;
 		}
 		break;
 	}
@@ -1236,7 +1101,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value = firstep_table[level] -
-			firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+			firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
 			aniState->iniDef.firstep;
 		if (value < ATH9K_SIG_FIRSTEP_SETTING_MIN)
 			value = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -1251,7 +1116,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value2 = firstep_table[level] -
-			 firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+			 firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
 			 aniState->iniDef.firstepLow;
 		if (value2 < ATH9K_SIG_FIRSTEP_SETTING_MIN)
 			value2 = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -1267,7 +1132,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 				chan->channel,
 				aniState->firstepLevel,
 				level,
-				ATH9K_ANI_FIRSTEP_LVL_NEW,
+				ATH9K_ANI_FIRSTEP_LVL,
 				value,
 				aniState->iniDef.firstep);
 			ath_dbg(common, ANI,
@@ -1275,7 +1140,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 				chan->channel,
 				aniState->firstepLevel,
 				level,
-				ATH9K_ANI_FIRSTEP_LVL_NEW,
+				ATH9K_ANI_FIRSTEP_LVL,
 				value2,
 				aniState->iniDef.firstepLow);
 			if (level > aniState->firstepLevel)
@@ -1300,7 +1165,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value = cycpwrThr1_table[level] -
-			cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+			cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
 			aniState->iniDef.cycpwrThr1;
 		if (value < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
 			value = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -1316,7 +1181,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value2 = cycpwrThr1_table[level] -
-			 cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+			 cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
 			 aniState->iniDef.cycpwrThr1Ext;
 		if (value2 < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
 			value2 = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -1331,7 +1196,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 				chan->channel,
 				aniState->spurImmunityLevel,
 				level,
-				ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+				ATH9K_ANI_SPUR_IMMUNE_LVL,
 				value,
 				aniState->iniDef.cycpwrThr1);
 			ath_dbg(common, ANI,
@@ -1339,7 +1204,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 				chan->channel,
 				aniState->spurImmunityLevel,
 				level,
-				ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+				ATH9K_ANI_SPUR_IMMUNE_LVL,
 				value2,
 				aniState->iniDef.cycpwrThr1Ext);
 			if (level > aniState->spurImmunityLevel)
@@ -1367,9 +1232,9 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 	ath_dbg(common, ANI,
 		"ANI parameters: SI=%d, ofdmWS=%s FS=%d MRCcck=%s listenTime=%d ofdmErrs=%d cckErrs=%d\n",
 		aniState->spurImmunityLevel,
-		!aniState->ofdmWeakSigDetectOff ? "on" : "off",
+		aniState->ofdmWeakSigDetect ? "on" : "off",
 		aniState->firstepLevel,
-		!aniState->mrcCCKOff ? "on" : "off",
+		aniState->mrcCCK ? "on" : "off",
 		aniState->listenTime,
 		aniState->ofdmPhyErrCount,
 		aniState->cckPhyErrCount);
@@ -1454,10 +1319,10 @@ static void ar5008_hw_ani_cache_ini_regs(struct ath_hw *ah)
 					       AR_PHY_EXT_TIMING5_CYCPWR_THR1);
 
 	/* these levels just got reset to defaults by the INI */
-	aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL_NEW;
-	aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_NEW;
-	aniState->ofdmWeakSigDetectOff = !ATH9K_ANI_USE_OFDM_WEAK_SIG;
-	aniState->mrcCCKOff = true; /* not available on pre AR9003 */
+	aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
+	aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
+	aniState->ofdmWeakSigDetect = ATH9K_ANI_USE_OFDM_WEAK_SIG;
+	aniState->mrcCCK = false; /* not available on pre AR9003 */
 }
 
 static void ar5008_hw_set_nf_limits(struct ath_hw *ah)
@@ -1545,11 +1410,8 @@ void ar5008_hw_attach_phy_ops(struct ath_hw *ah)
 	priv_ops->do_getnf = ar5008_hw_do_getnf;
 	priv_ops->set_radar_params = ar5008_hw_set_radar_params;
 
-	if (modparam_force_new_ani) {
-		priv_ops->ani_control = ar5008_hw_ani_control_new;
-		priv_ops->ani_cache_ini_regs = ar5008_hw_ani_cache_ini_regs;
-	} else
-		priv_ops->ani_control = ar5008_hw_ani_control_old;
+	priv_ops->ani_control = ar5008_hw_ani_control_new;
+	priv_ops->ani_cache_ini_regs = ar5008_hw_ani_cache_ini_regs;
 
 	if (AR_SREV_9100(ah) || AR_SREV_9160_10_OR_LATER(ah))
 		priv_ops->compute_pll_control = ar9160_hw_compute_pll_control;
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_hw.c b/drivers/net/wireless/ath/ath9k/ar9002_hw.c
index d9a69fc470c..648da3e885e 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_hw.c
@@ -21,110 +21,79 @@
 #include "ar9002_initvals.h"
 #include "ar9002_phy.h"
 
-int modparam_force_new_ani;
-module_param_named(force_new_ani, modparam_force_new_ani, int, 0444);
-MODULE_PARM_DESC(force_new_ani, "Force new ANI for AR5008, AR9001, AR9002");
-
 /* General hardware code for the A5008/AR9001/AR9002 hadware families */
 
 static void ar9002_hw_init_mode_regs(struct ath_hw *ah)
 {
 	if (AR_SREV_9271(ah)) {
-		INIT_INI_ARRAY(&ah->iniModes, ar9271Modes_9271,
-			       ARRAY_SIZE(ar9271Modes_9271), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar9271Common_9271,
-			       ARRAY_SIZE(ar9271Common_9271), 2);
-		INIT_INI_ARRAY(&ah->iniModes_9271_ANI_reg, ar9271Modes_9271_ANI_reg,
-			       ARRAY_SIZE(ar9271Modes_9271_ANI_reg), 5);
+		INIT_INI_ARRAY(&ah->iniModes, ar9271Modes_9271);
+		INIT_INI_ARRAY(&ah->iniCommon, ar9271Common_9271);
+		INIT_INI_ARRAY(&ah->iniModes_9271_ANI_reg, ar9271Modes_9271_ANI_reg);
 		return;
 	}
 
 	if (ah->config.pcie_clock_req)
 		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-			   ar9280PciePhy_clkreq_off_L1_9280,
-			   ARRAY_SIZE(ar9280PciePhy_clkreq_off_L1_9280), 2);
+			   ar9280PciePhy_clkreq_off_L1_9280);
 	else
 		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-			   ar9280PciePhy_clkreq_always_on_L1_9280,
-			   ARRAY_SIZE(ar9280PciePhy_clkreq_always_on_L1_9280), 2);
+			   ar9280PciePhy_clkreq_always_on_L1_9280);
+#ifdef CONFIG_PM_SLEEP
+		INIT_INI_ARRAY(&ah->iniPcieSerdesWow,
+			       ar9280PciePhy_awow);
+#endif
 
 	if (AR_SREV_9287_11_OR_LATER(ah)) {
-		INIT_INI_ARRAY(&ah->iniModes, ar9287Modes_9287_1_1,
-				ARRAY_SIZE(ar9287Modes_9287_1_1), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar9287Common_9287_1_1,
-				ARRAY_SIZE(ar9287Common_9287_1_1), 2);
+		INIT_INI_ARRAY(&ah->iniModes, ar9287Modes_9287_1_1);
+		INIT_INI_ARRAY(&ah->iniCommon, ar9287Common_9287_1_1);
 	} else if (AR_SREV_9285_12_OR_LATER(ah)) {
-		INIT_INI_ARRAY(&ah->iniModes, ar9285Modes_9285_1_2,
-			       ARRAY_SIZE(ar9285Modes_9285_1_2), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar9285Common_9285_1_2,
-			       ARRAY_SIZE(ar9285Common_9285_1_2), 2);
+		INIT_INI_ARRAY(&ah->iniModes, ar9285Modes_9285_1_2);
+		INIT_INI_ARRAY(&ah->iniCommon, ar9285Common_9285_1_2);
 	} else if (AR_SREV_9280_20_OR_LATER(ah)) {
-		INIT_INI_ARRAY(&ah->iniModes, ar9280Modes_9280_2,
-			       ARRAY_SIZE(ar9280Modes_9280_2), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar9280Common_9280_2,
-			       ARRAY_SIZE(ar9280Common_9280_2), 2);
+		INIT_INI_ARRAY(&ah->iniModes, ar9280Modes_9280_2);
+		INIT_INI_ARRAY(&ah->iniCommon, ar9280Common_9280_2);
 
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
-			       ar9280Modes_fast_clock_9280_2,
-			       ARRAY_SIZE(ar9280Modes_fast_clock_9280_2), 3);
+			       ar9280Modes_fast_clock_9280_2);
 	} else if (AR_SREV_9160_10_OR_LATER(ah)) {
-		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes_9160,
-			       ARRAY_SIZE(ar5416Modes_9160), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common_9160,
-			       ARRAY_SIZE(ar5416Common_9160), 2);
+		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes_9160);
+		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common_9160);
 		if (AR_SREV_9160_11(ah)) {
 			INIT_INI_ARRAY(&ah->iniAddac,
-				       ar5416Addac_9160_1_1,
-				       ARRAY_SIZE(ar5416Addac_9160_1_1), 2);
+				       ar5416Addac_9160_1_1);
 		} else {
-			INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac_9160,
-				       ARRAY_SIZE(ar5416Addac_9160), 2);
+			INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac_9160);
 		}
 	} else if (AR_SREV_9100_OR_LATER(ah)) {
-		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes_9100,
-			       ARRAY_SIZE(ar5416Modes_9100), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common_9100,
-			       ARRAY_SIZE(ar5416Common_9100), 2);
-		INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6_9100,
-			       ARRAY_SIZE(ar5416Bank6_9100), 3);
-		INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac_9100,
-			       ARRAY_SIZE(ar5416Addac_9100), 2);
+		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes_9100);
+		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common_9100);
+		INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6_9100);
+		INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac_9100);
 	} else {
-		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes,
-			       ARRAY_SIZE(ar5416Modes), 5);
-		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common,
-			       ARRAY_SIZE(ar5416Common), 2);
-		INIT_INI_ARRAY(&ah->iniBank6TPC, ar5416Bank6TPC,
-			       ARRAY_SIZE(ar5416Bank6TPC), 3);
-		INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac,
-			       ARRAY_SIZE(ar5416Addac), 2);
+		INIT_INI_ARRAY(&ah->iniModes, ar5416Modes);
+		INIT_INI_ARRAY(&ah->iniCommon, ar5416Common);
+		INIT_INI_ARRAY(&ah->iniBank6TPC, ar5416Bank6TPC);
+		INIT_INI_ARRAY(&ah->iniAddac, ar5416Addac);
 	}
 
 	if (!AR_SREV_9280_20_OR_LATER(ah)) {
 		/* Common for AR5416, AR913x, AR9160 */
-		INIT_INI_ARRAY(&ah->iniBB_RfGain, ar5416BB_RfGain,
-			       ARRAY_SIZE(ar5416BB_RfGain), 3);
-
-		INIT_INI_ARRAY(&ah->iniBank0, ar5416Bank0,
-			       ARRAY_SIZE(ar5416Bank0), 2);
-		INIT_INI_ARRAY(&ah->iniBank1, ar5416Bank1,
-			       ARRAY_SIZE(ar5416Bank1), 2);
-		INIT_INI_ARRAY(&ah->iniBank2, ar5416Bank2,
-			       ARRAY_SIZE(ar5416Bank2), 2);
-		INIT_INI_ARRAY(&ah->iniBank3, ar5416Bank3,
-			       ARRAY_SIZE(ar5416Bank3), 3);
-		INIT_INI_ARRAY(&ah->iniBank7, ar5416Bank7,
-			       ARRAY_SIZE(ar5416Bank7), 2);
+		INIT_INI_ARRAY(&ah->iniBB_RfGain, ar5416BB_RfGain);
+
+		INIT_INI_ARRAY(&ah->iniBank0, ar5416Bank0);
+		INIT_INI_ARRAY(&ah->iniBank1, ar5416Bank1);
+		INIT_INI_ARRAY(&ah->iniBank2, ar5416Bank2);
+		INIT_INI_ARRAY(&ah->iniBank3, ar5416Bank3);
+		INIT_INI_ARRAY(&ah->iniBank7, ar5416Bank7);
 
 		/* Common for AR5416, AR9160 */
 		if (!AR_SREV_9100(ah))
-			INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6,
-				       ARRAY_SIZE(ar5416Bank6), 3);
+			INIT_INI_ARRAY(&ah->iniBank6, ar5416Bank6);
 
 		/* Common for AR913x, AR9160 */
 		if (!AR_SREV_5416(ah))
-			INIT_INI_ARRAY(&ah->iniBank6TPC, ar5416Bank6TPC_9100,
-				       ARRAY_SIZE(ar5416Bank6TPC_9100), 3);
+			INIT_INI_ARRAY(&ah->iniBank6TPC,
+				      ar5416Bank6TPC_9100);
 	}
 
 	/* iniAddac needs to be modified for these chips */
@@ -147,13 +116,9 @@ static void ar9002_hw_init_mode_regs(struct ath_hw *ah)
 	}
 	if (AR_SREV_9287_11_OR_LATER(ah)) {
 		INIT_INI_ARRAY(&ah->iniCckfirNormal,
-		       ar9287Common_normal_cck_fir_coeff_9287_1_1,
-		       ARRAY_SIZE(ar9287Common_normal_cck_fir_coeff_9287_1_1),
-		       2);
+		       ar9287Common_normal_cck_fir_coeff_9287_1_1);
 		INIT_INI_ARRAY(&ah->iniCckfirJapan2484,
-		       ar9287Common_japan_2484_cck_fir_coeff_9287_1_1,
-		       ARRAY_SIZE(ar9287Common_japan_2484_cck_fir_coeff_9287_1_1),
-		       2);
+		       ar9287Common_japan_2484_cck_fir_coeff_9287_1_1);
 	}
 }
 
@@ -167,20 +132,16 @@ static void ar9280_20_hw_init_rxgain_ini(struct ath_hw *ah)
 
 		if (rxgain_type == AR5416_EEP_RXGAIN_13DB_BACKOFF)
 			INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9280Modes_backoff_13db_rxgain_9280_2,
-			ARRAY_SIZE(ar9280Modes_backoff_13db_rxgain_9280_2), 5);
+				       ar9280Modes_backoff_13db_rxgain_9280_2);
 		else if (rxgain_type == AR5416_EEP_RXGAIN_23DB_BACKOFF)
 			INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9280Modes_backoff_23db_rxgain_9280_2,
-			ARRAY_SIZE(ar9280Modes_backoff_23db_rxgain_9280_2), 5);
+				       ar9280Modes_backoff_23db_rxgain_9280_2);
 		else
 			INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9280Modes_original_rxgain_9280_2,
-			ARRAY_SIZE(ar9280Modes_original_rxgain_9280_2), 5);
+				       ar9280Modes_original_rxgain_9280_2);
 	} else {
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9280Modes_original_rxgain_9280_2,
-			ARRAY_SIZE(ar9280Modes_original_rxgain_9280_2), 5);
+			       ar9280Modes_original_rxgain_9280_2);
 	}
 }
 
@@ -190,16 +151,13 @@ static void ar9280_20_hw_init_txgain_ini(struct ath_hw *ah, u32 txgain_type)
 	    AR5416_EEP_MINOR_VER_19) {
 		if (txgain_type == AR5416_EEP_TXGAIN_HIGH_POWER)
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9280Modes_high_power_tx_gain_9280_2,
-			ARRAY_SIZE(ar9280Modes_high_power_tx_gain_9280_2), 5);
+				       ar9280Modes_high_power_tx_gain_9280_2);
 		else
 			INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9280Modes_original_tx_gain_9280_2,
-			ARRAY_SIZE(ar9280Modes_original_tx_gain_9280_2), 5);
+				       ar9280Modes_original_tx_gain_9280_2);
 	} else {
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-		ar9280Modes_original_tx_gain_9280_2,
-		ARRAY_SIZE(ar9280Modes_original_tx_gain_9280_2), 5);
+			       ar9280Modes_original_tx_gain_9280_2);
 	}
 }
 
@@ -207,12 +165,10 @@ static void ar9271_hw_init_txgain_ini(struct ath_hw *ah, u32 txgain_type)
 {
 	if (txgain_type == AR5416_EEP_TXGAIN_HIGH_POWER)
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			       ar9271Modes_high_power_tx_gain_9271,
-			       ARRAY_SIZE(ar9271Modes_high_power_tx_gain_9271), 5);
+			       ar9271Modes_high_power_tx_gain_9271);
 	else
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			       ar9271Modes_normal_power_tx_gain_9271,
-			       ARRAY_SIZE(ar9271Modes_normal_power_tx_gain_9271), 5);
+			       ar9271Modes_normal_power_tx_gain_9271);
 }
 
 static void ar9002_hw_init_mode_gain_regs(struct ath_hw *ah)
@@ -221,8 +177,7 @@ static void ar9002_hw_init_mode_gain_regs(struct ath_hw *ah)
 
 	if (AR_SREV_9287_11_OR_LATER(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-		ar9287Modes_rx_gain_9287_1_1,
-		ARRAY_SIZE(ar9287Modes_rx_gain_9287_1_1), 5);
+			       ar9287Modes_rx_gain_9287_1_1);
 	else if (AR_SREV_9280_20(ah))
 		ar9280_20_hw_init_rxgain_ini(ah);
 
@@ -230,8 +185,7 @@ static void ar9002_hw_init_mode_gain_regs(struct ath_hw *ah)
 		ar9271_hw_init_txgain_ini(ah, txgain_type);
 	} else if (AR_SREV_9287_11_OR_LATER(ah)) {
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-		ar9287Modes_tx_gain_9287_1_1,
-		ARRAY_SIZE(ar9287Modes_tx_gain_9287_1_1), 5);
+			       ar9287Modes_tx_gain_9287_1_1);
 	} else if (AR_SREV_9280_20(ah)) {
 		ar9280_20_hw_init_txgain_ini(ah, txgain_type);
 	} else if (AR_SREV_9285_12_OR_LATER(ah)) {
@@ -239,26 +193,18 @@ static void ar9002_hw_init_mode_gain_regs(struct ath_hw *ah)
 		if (txgain_type == AR5416_EEP_TXGAIN_HIGH_POWER) {
 			if (AR_SREV_9285E_20(ah)) {
 				INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9285Modes_XE2_0_high_power,
-				ARRAY_SIZE(
-				  ar9285Modes_XE2_0_high_power), 5);
+					       ar9285Modes_XE2_0_high_power);
 			} else {
 				INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9285Modes_high_power_tx_gain_9285_1_2,
-				ARRAY_SIZE(
-				  ar9285Modes_high_power_tx_gain_9285_1_2), 5);
+					ar9285Modes_high_power_tx_gain_9285_1_2);
 			}
 		} else {
 			if (AR_SREV_9285E_20(ah)) {
 				INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9285Modes_XE2_0_normal_power,
-				ARRAY_SIZE(
-				  ar9285Modes_XE2_0_normal_power), 5);
+					       ar9285Modes_XE2_0_normal_power);
 			} else {
 				INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9285Modes_original_tx_gain_9285_1_2,
-				ARRAY_SIZE(
-				  ar9285Modes_original_tx_gain_9285_1_2), 5);
+					ar9285Modes_original_tx_gain_9285_1_2);
 			}
 		}
 	}
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_initvals.h b/drivers/net/wireless/ath/ath9k/ar9002_initvals.h
index 4d18c66a679..beb6162cf97 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9002_initvals.h
@@ -925,6 +925,20 @@ static const u32 ar9280PciePhy_clkreq_always_on_L1_9280[][2] = {
 	{0x00004044, 0x00000000},
 };
 
+static const u32 ar9280PciePhy_awow[][2] = {
+	/* Addr      allmodes  */
+	{0x00004040, 0x9248fd00},
+	{0x00004040, 0x24924924},
+	{0x00004040, 0xa8000019},
+	{0x00004040, 0x13160820},
+	{0x00004040, 0xe5980560},
+	{0x00004040, 0xc01dcffd},
+	{0x00004040, 0x1aaabe41},
+	{0x00004040, 0xbe105554},
+	{0x00004040, 0x00043007},
+	{0x00004044, 0x00000000},
+};
+
 static const u32 ar9285Modes_9285_1_2[][5] = {
 	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x00001030, 0x00000230, 0x00000460, 0x000002c0, 0x00000160},
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
index 952cb2b4656..89bf94d4d8a 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index 9fdd70fcaf5..84b558d126c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -159,14 +159,11 @@ static bool ar9003_hw_calibrate(struct ath_hw *ah,
 		}
 	}
 
-	/* Do NF cal only at longer intervals */
-	if (longcal) {
-		/*
-		 * Get the value from the previous NF cal and update
-		 * history buffer.
-		 */
-		ath9k_hw_getnf(ah, chan);
-
+	/*
+	 * Do NF cal only at longer intervals. Get the value from
+	 * the previous NF cal and update history buffer.
+	 */
+	if (longcal && ath9k_hw_getnf(ah, chan)) {
 		/*
 		 * Load the NF from history buffer of the current channel.
 		 * NF is slow time-variant, so it is OK to use a historical
@@ -653,7 +650,6 @@ static void ar9003_hw_detect_outlier(int *mp_coeff, int nmeasurement,
 }
 
 static void ar9003_hw_tx_iqcal_load_avg_2_passes(struct ath_hw *ah,
-						 u8 num_chains,
 						 struct coeff *coeff,
 						 bool is_reusable)
 {
@@ -677,7 +673,9 @@ static void ar9003_hw_tx_iqcal_load_avg_2_passes(struct ath_hw *ah,
 	}
 
 	/* Load the average of 2 passes */
-	for (i = 0; i < num_chains; i++) {
+	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
+		if (!(ah->txchainmask & (1 << i)))
+			continue;
 		nmeasurement = REG_READ_FIELD(ah,
 				AR_PHY_TX_IQCAL_STATUS_B0,
 				AR_PHY_CALIBRATED_GAINS_0);
@@ -767,16 +765,13 @@ static void ar9003_hw_tx_iq_cal_post_proc(struct ath_hw *ah, bool is_reusable)
 	};
 	struct coeff coeff;
 	s32 iq_res[6];
-	u8 num_chains = 0;
 	int i, im, j;
 	int nmeasurement;
 
 	for (i = 0; i < AR9300_MAX_CHAINS; i++) {
-		if (ah->txchainmask & (1 << i))
-			num_chains++;
-	}
+		if (!(ah->txchainmask & (1 << i)))
+			continue;
 
-	for (i = 0; i < num_chains; i++) {
 		nmeasurement = REG_READ_FIELD(ah,
 				AR_PHY_TX_IQCAL_STATUS_B0,
 				AR_PHY_CALIBRATED_GAINS_0);
@@ -839,8 +834,7 @@ static void ar9003_hw_tx_iq_cal_post_proc(struct ath_hw *ah, bool is_reusable)
 				coeff.phs_coeff[i][im] -= 128;
 		}
 	}
-	ar9003_hw_tx_iqcal_load_avg_2_passes(ah, num_chains,
-					     &coeff, is_reusable);
+	ar9003_hw_tx_iqcal_load_avg_2_passes(ah, &coeff, is_reusable);
 
 	return;
 
@@ -901,7 +895,6 @@ static bool ar9003_hw_init_cal(struct ath_hw *ah,
 	bool is_reusable = true, status = true;
 	bool run_rtt_cal = false, run_agc_cal;
 	bool rtt = !!(ah->caps.hw_caps & ATH9K_HW_CAP_RTT);
-	bool mci = !!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI);
 	u32 agc_ctrl = 0, agc_supp_cals = AR_PHY_AGC_CONTROL_OFFSET_CAL |
 					  AR_PHY_AGC_CONTROL_FLTR_CAL   |
 					  AR_PHY_AGC_CONTROL_PKDET_CAL;
@@ -970,7 +963,7 @@ static bool ar9003_hw_init_cal(struct ath_hw *ah,
 	} else if (caldata && !caldata->done_txiqcal_once)
 		run_agc_cal = true;
 
-	if (mci && IS_CHAN_2GHZ(chan) && run_agc_cal)
+	if (ath9k_hw_mci_is_enabled(ah) && IS_CHAN_2GHZ(chan) && run_agc_cal)
 		ar9003_mci_init_cal_req(ah, &is_reusable);
 
 	if (!(IS_CHAN_HALF_RATE(chan) || IS_CHAN_QUARTER_RATE(chan))) {
@@ -993,7 +986,7 @@ skip_tx_iqcal:
 				       0, AH_WAIT_TIMEOUT);
 	}
 
-	if (mci && IS_CHAN_2GHZ(chan) && run_agc_cal)
+	if (ath9k_hw_mci_is_enabled(ah) && IS_CHAN_2GHZ(chan) && run_agc_cal)
 		ar9003_mci_init_cal_done(ah);
 
 	if (rtt && !run_rtt_cal) {
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index dfb0441f406..2588848f4a8 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -131,8 +131,9 @@ static const struct ar9300_eeprom ar9300_default = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0cf0e0e0),
 		.papdRateMaskHt40 = LE32(0x6cf0e0e0),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	 },
 	.base_ext1 = {
@@ -331,8 +332,9 @@ static const struct ar9300_eeprom ar9300_default = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0c80c080),
 		.papdRateMaskHt40 = LE32(0x0080c080),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	 },
 	.base_ext2 = {
@@ -704,8 +706,9 @@ static const struct ar9300_eeprom ar9300_x113 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0c80c080),
 		.papdRateMaskHt40 = LE32(0x0080c080),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	 },
 	 .base_ext1 = {
@@ -904,8 +907,9 @@ static const struct ar9300_eeprom ar9300_x113 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0cf0e0e0),
 		.papdRateMaskHt40 = LE32(0x6cf0e0e0),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	 },
 	.base_ext2 = {
@@ -1278,8 +1282,9 @@ static const struct ar9300_eeprom ar9300_h112 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0c80c080),
 		.papdRateMaskHt40 = LE32(0x0080c080),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	},
 	.base_ext1 = {
@@ -1478,8 +1483,9 @@ static const struct ar9300_eeprom ar9300_h112 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0cf0e0e0),
 		.papdRateMaskHt40 = LE32(0x6cf0e0e0),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	},
 	.base_ext2 = {
@@ -1852,8 +1858,9 @@ static const struct ar9300_eeprom ar9300_x112 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0c80c080),
 		.papdRateMaskHt40 = LE32(0x0080c080),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	},
 	.base_ext1 = {
@@ -2052,8 +2059,9 @@ static const struct ar9300_eeprom ar9300_x112 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0cf0e0e0),
 		.papdRateMaskHt40 = LE32(0x6cf0e0e0),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	},
 	.base_ext2 = {
@@ -2425,8 +2433,9 @@ static const struct ar9300_eeprom ar9300_h116 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0c80C080),
 		.papdRateMaskHt40 = LE32(0x0080C080),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	 },
 	 .base_ext1 = {
@@ -2625,8 +2634,9 @@ static const struct ar9300_eeprom ar9300_h116 = {
 		.thresh62 = 28,
 		.papdRateMaskHt20 = LE32(0x0cf0e0e0),
 		.papdRateMaskHt40 = LE32(0x6cf0e0e0),
+		.xlna_bias_strength = 0,
 		.futureModal = {
-			0, 0, 0, 0, 0, 0, 0, 0,
+			0, 0, 0, 0, 0, 0, 0,
 		},
 	 },
 	.base_ext2 = {
@@ -2971,14 +2981,6 @@ static u32 ath9k_hw_ar9300_get_eeprom(struct ath_hw *ah,
 		return (pBase->txrxMask >> 4) & 0xf;
 	case EEP_RX_MASK:
 		return pBase->txrxMask & 0xf;
-	case EEP_DRIVE_STRENGTH:
-#define AR9300_EEP_BASE_DRIV_STRENGTH	0x1
-		return pBase->miscConfiguration & AR9300_EEP_BASE_DRIV_STRENGTH;
-	case EEP_INTERNAL_REGULATOR:
-		/* Bit 4 is internal regulator flag */
-		return (pBase->featureEnable & 0x10) >> 4;
-	case EEP_SWREG:
-		return le32_to_cpu(pBase->swreg);
 	case EEP_PAPRD:
 		return !!(pBase->featureEnable & BIT(5));
 	case EEP_CHAIN_MASK_REDUCE:
@@ -2989,8 +2991,6 @@ static u32 ath9k_hw_ar9300_get_eeprom(struct ath_hw *ah,
 		return eep->modalHeader5G.antennaGain;
 	case EEP_ANTENNA_GAIN_2G:
 		return eep->modalHeader2G.antennaGain;
-	case EEP_QUICK_DROP:
-		return pBase->miscConfiguration & BIT(1);
 	default:
 		return 0;
 	}
@@ -3178,7 +3178,7 @@ static int ar9300_compress_decision(struct ath_hw *ah,
 				mdata_size, length);
 			return -1;
 		}
-		memcpy(mptr, (u8 *) (word + COMP_HDR_LEN), length);
+		memcpy(mptr, word + COMP_HDR_LEN, length);
 		ath_dbg(common, EEPROM,
 			"restored eeprom %d: uncompressed, length %d\n",
 			it, length);
@@ -3199,7 +3199,7 @@ static int ar9300_compress_decision(struct ath_hw *ah,
 			"restore eeprom %d: block, reference %d, length %d\n",
 			it, reference, length);
 		ar9300_uncompress_block(ah, mptr, mdata_size,
-					(u8 *) (word + COMP_HDR_LEN), length);
+					(word + COMP_HDR_LEN), length);
 		break;
 	default:
 		ath_dbg(common, EEPROM, "unknown compression code %d\n", code);
@@ -3260,10 +3260,20 @@ static int ar9300_eeprom_restore_internal(struct ath_hw *ah,
 	int it;
 	u16 checksum, mchecksum;
 	struct ath_common *common = ath9k_hw_common(ah);
+	struct ar9300_eeprom *eep;
 	eeprom_read_op read;
 
-	if (ath9k_hw_use_flash(ah))
-		return ar9300_eeprom_restore_flash(ah, mptr, mdata_size);
+	if (ath9k_hw_use_flash(ah)) {
+		u8 txrx;
+
+		ar9300_eeprom_restore_flash(ah, mptr, mdata_size);
+
+		/* check if eeprom contains valid data */
+		eep = (struct ar9300_eeprom *) mptr;
+		txrx = eep->baseEepHeader.txrxMask;
+		if (txrx != 0 && txrx != 0xff)
+			return 0;
+	}
 
 	word = kzalloc(2048, GFP_KERNEL);
 	if (!word)
@@ -3412,11 +3422,11 @@ static u32 ath9k_hw_ar9003_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 	if (!dump_base_hdr) {
 		len += snprintf(buf + len, size - len,
 				"%20s :\n", "2GHz modal Header");
-		len += ar9003_dump_modal_eeprom(buf, len, size,
+		len = ar9003_dump_modal_eeprom(buf, len, size,
 						&eep->modalHeader2G);
 		len += snprintf(buf + len, size - len,
 				"%20s :\n", "5GHz modal Header");
-		len += ar9003_dump_modal_eeprom(buf, len, size,
+		len = ar9003_dump_modal_eeprom(buf, len, size,
 						&eep->modalHeader5G);
 		goto out;
 	}
@@ -3493,23 +3503,24 @@ static int ath9k_hw_ar9300_get_eeprom_rev(struct ath_hw *ah)
 	return 0;
 }
 
-static s32 ar9003_hw_xpa_bias_level_get(struct ath_hw *ah, bool is2ghz)
+static struct ar9300_modal_eep_header *ar9003_modal_header(struct ath_hw *ah,
+							   bool is2ghz)
 {
 	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
 
 	if (is2ghz)
-		return eep->modalHeader2G.xpaBiasLvl;
+		return &eep->modalHeader2G;
 	else
-		return eep->modalHeader5G.xpaBiasLvl;
+		return &eep->modalHeader5G;
 }
 
 static void ar9003_hw_xpa_bias_level_apply(struct ath_hw *ah, bool is2ghz)
 {
-	int bias = ar9003_hw_xpa_bias_level_get(ah, is2ghz);
+	int bias = ar9003_modal_header(ah, is2ghz)->xpaBiasLvl;
 
 	if (AR_SREV_9485(ah) || AR_SREV_9330(ah) || AR_SREV_9340(ah))
 		REG_RMW_FIELD(ah, AR_CH0_TOP2, AR_CH0_TOP2_XPABIASLVL, bias);
-	else if (AR_SREV_9462(ah))
+	else if (AR_SREV_9462(ah) || AR_SREV_9550(ah))
 		REG_RMW_FIELD(ah, AR_CH0_TOP, AR_CH0_TOP_XPABIASLVL, bias);
 	else {
 		REG_RMW_FIELD(ah, AR_CH0_TOP, AR_CH0_TOP_XPABIASLVL, bias);
@@ -3521,57 +3532,26 @@ static void ar9003_hw_xpa_bias_level_apply(struct ath_hw *ah, bool is2ghz)
 	}
 }
 
-static u16 ar9003_switch_com_spdt_get(struct ath_hw *ah, bool is_2ghz)
+static u16 ar9003_switch_com_spdt_get(struct ath_hw *ah, bool is2ghz)
 {
-	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
-	__le16 val;
-
-	if (is_2ghz)
-		val = eep->modalHeader2G.switchcomspdt;
-	else
-		val = eep->modalHeader5G.switchcomspdt;
-	return le16_to_cpu(val);
+	return le16_to_cpu(ar9003_modal_header(ah, is2ghz)->switchcomspdt);
 }
 
 
 static u32 ar9003_hw_ant_ctrl_common_get(struct ath_hw *ah, bool is2ghz)
 {
-	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
-	__le32 val;
-
-	if (is2ghz)
-		val = eep->modalHeader2G.antCtrlCommon;
-	else
-		val = eep->modalHeader5G.antCtrlCommon;
-	return le32_to_cpu(val);
+	return le32_to_cpu(ar9003_modal_header(ah, is2ghz)->antCtrlCommon);
 }
 
 static u32 ar9003_hw_ant_ctrl_common_2_get(struct ath_hw *ah, bool is2ghz)
 {
-	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
-	__le32 val;
-
-	if (is2ghz)
-		val = eep->modalHeader2G.antCtrlCommon2;
-	else
-		val = eep->modalHeader5G.antCtrlCommon2;
-	return le32_to_cpu(val);
+	return le32_to_cpu(ar9003_modal_header(ah, is2ghz)->antCtrlCommon2);
 }
 
-static u16 ar9003_hw_ant_ctrl_chain_get(struct ath_hw *ah,
-					int chain,
+static u16 ar9003_hw_ant_ctrl_chain_get(struct ath_hw *ah, int chain,
 					bool is2ghz)
 {
-	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
-	__le16 val = 0;
-
-	if (chain >= 0 && chain < AR9300_MAX_CHAINS) {
-		if (is2ghz)
-			val = eep->modalHeader2G.antCtrlChain[chain];
-		else
-			val = eep->modalHeader5G.antCtrlChain[chain];
-	}
-
+	__le16 val = ar9003_modal_header(ah, is2ghz)->antCtrlChain[chain];
 	return le16_to_cpu(val);
 }
 
@@ -3591,6 +3571,9 @@ static void ar9003_hw_ant_ctrl_apply(struct ath_hw *ah, bool is2ghz)
 	if (AR_SREV_9462(ah)) {
 		REG_RMW_FIELD(ah, AR_PHY_SWITCH_COM,
 				AR_SWITCH_TABLE_COM_AR9462_ALL, value);
+	} else if (AR_SREV_9550(ah)) {
+		REG_RMW_FIELD(ah, AR_PHY_SWITCH_COM,
+				AR_SWITCH_TABLE_COM_AR9550_ALL, value);
 	} else
 		REG_RMW_FIELD(ah, AR_PHY_SWITCH_COM,
 			      AR_SWITCH_TABLE_COM_ALL, value);
@@ -3613,6 +3596,7 @@ static void ar9003_hw_ant_ctrl_apply(struct ath_hw *ah, bool is2ghz)
 		value = ar9003_switch_com_spdt_get(ah, is2ghz);
 		REG_RMW_FIELD(ah, AR_PHY_GLB_CONTROL,
 				AR_SWITCH_TABLE_COM_SPDT_ALL, value);
+		REG_SET_BIT(ah, AR_PHY_GLB_CONTROL, AR_BTCOEX_CTRL_SPDT_ENABLE);
 	}
 
 	value = ar9003_hw_ant_ctrl_common_2_get(ah, is2ghz);
@@ -3677,11 +3661,12 @@ static void ar9003_hw_ant_ctrl_apply(struct ath_hw *ah, bool is2ghz)
 
 static void ar9003_hw_drive_strength_apply(struct ath_hw *ah)
 {
+	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
+	struct ar9300_base_eep_hdr *pBase = &eep->baseEepHeader;
 	int drive_strength;
 	unsigned long reg;
 
-	drive_strength = ath9k_hw_ar9300_get_eeprom(ah, EEP_DRIVE_STRENGTH);
-
+	drive_strength = pBase->miscConfiguration & BIT(0);
 	if (!drive_strength)
 		return;
 
@@ -3811,11 +3796,11 @@ static bool is_pmu_set(struct ath_hw *ah, u32 pmu_reg, int pmu_set)
 
 void ar9003_hw_internal_regulator_apply(struct ath_hw *ah)
 {
-	int internal_regulator =
-		ath9k_hw_ar9300_get_eeprom(ah, EEP_INTERNAL_REGULATOR);
+	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
+	struct ar9300_base_eep_hdr *pBase = &eep->baseEepHeader;
 	u32 reg_val;
 
-	if (internal_regulator) {
+	if (pBase->featureEnable & BIT(4)) {
 		if (AR_SREV_9330(ah) || AR_SREV_9485(ah)) {
 			int reg_pmu_set;
 
@@ -3859,11 +3844,11 @@ void ar9003_hw_internal_regulator_apply(struct ath_hw *ah)
 			if (!is_pmu_set(ah, AR_PHY_PMU2, reg_pmu_set))
 				return;
 		} else if (AR_SREV_9462(ah)) {
-			reg_val = ath9k_hw_ar9300_get_eeprom(ah, EEP_SWREG);
+			reg_val = le32_to_cpu(pBase->swreg);
 			REG_WRITE(ah, AR_PHY_PMU1, reg_val);
 		} else {
 			/* Internal regulator is ON. Write swreg register. */
-			reg_val = ath9k_hw_ar9300_get_eeprom(ah, EEP_SWREG);
+			reg_val = le32_to_cpu(pBase->swreg);
 			REG_WRITE(ah, AR_RTC_REG_CONTROL1,
 				  REG_READ(ah, AR_RTC_REG_CONTROL1) &
 				  (~AR_RTC_REG_CONTROL1_SWREG_PROGRAM));
@@ -3905,6 +3890,9 @@ static void ar9003_hw_apply_tuning_caps(struct ath_hw *ah)
 	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
 	u8 tuning_caps_param = eep->baseEepHeader.params_for_tuning_caps[0];
 
+	if (AR_SREV_9485(ah) || AR_SREV_9330(ah) || AR_SREV_9340(ah))
+		return;
+
 	if (eep->baseEepHeader.featureEnable & 0x40) {
 		tuning_caps_param &= 0x7f;
 		REG_RMW_FIELD(ah, AR_CH0_XTAL, AR_CH0_XTAL_CAPINDAC,
@@ -3917,10 +3905,11 @@ static void ar9003_hw_apply_tuning_caps(struct ath_hw *ah)
 static void ar9003_hw_quick_drop_apply(struct ath_hw *ah, u16 freq)
 {
 	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
-	int quick_drop = ath9k_hw_ar9300_get_eeprom(ah, EEP_QUICK_DROP);
+	struct ar9300_base_eep_hdr *pBase = &eep->baseEepHeader;
+	int quick_drop;
 	s32 t[3], f[3] = {5180, 5500, 5785};
 
-	if (!quick_drop)
+	if (!(pBase->miscConfiguration & BIT(1)))
 		return;
 
 	if (freq < 4000)
@@ -3934,13 +3923,11 @@ static void ar9003_hw_quick_drop_apply(struct ath_hw *ah, u16 freq)
 	REG_RMW_FIELD(ah, AR_PHY_AGC, AR_PHY_AGC_QUICK_DROP, quick_drop);
 }
 
-static void ar9003_hw_txend_to_xpa_off_apply(struct ath_hw *ah, u16 freq)
+static void ar9003_hw_txend_to_xpa_off_apply(struct ath_hw *ah, bool is2ghz)
 {
-	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
 	u32 value;
 
-	value = (freq < 4000) ? eep->modalHeader2G.txEndToXpaOff :
-				eep->modalHeader5G.txEndToXpaOff;
+	value = ar9003_modal_header(ah, is2ghz)->txEndToXpaOff;
 
 	REG_RMW_FIELD(ah, AR_PHY_XPA_TIMING_CTL,
 		      AR_PHY_XPA_TIMING_CTL_TX_END_XPAB_OFF, value);
@@ -3948,19 +3935,63 @@ static void ar9003_hw_txend_to_xpa_off_apply(struct ath_hw *ah, u16 freq)
 		      AR_PHY_XPA_TIMING_CTL_TX_END_XPAA_OFF, value);
 }
 
+static void ar9003_hw_xpa_timing_control_apply(struct ath_hw *ah, bool is2ghz)
+{
+	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
+	u8 xpa_ctl;
+
+	if (!(eep->baseEepHeader.featureEnable & 0x80))
+		return;
+
+	if (!AR_SREV_9300(ah) && !AR_SREV_9340(ah) && !AR_SREV_9580(ah))
+		return;
+
+	xpa_ctl = ar9003_modal_header(ah, is2ghz)->txFrameToXpaOn;
+	if (is2ghz)
+		REG_RMW_FIELD(ah, AR_PHY_XPA_TIMING_CTL,
+			      AR_PHY_XPA_TIMING_CTL_FRAME_XPAB_ON, xpa_ctl);
+	else
+		REG_RMW_FIELD(ah, AR_PHY_XPA_TIMING_CTL,
+			      AR_PHY_XPA_TIMING_CTL_FRAME_XPAA_ON, xpa_ctl);
+}
+
+static void ar9003_hw_xlna_bias_strength_apply(struct ath_hw *ah, bool is2ghz)
+{
+	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
+	u8 bias;
+
+	if (!(eep->baseEepHeader.featureEnable & 0x40))
+		return;
+
+	if (!AR_SREV_9300(ah))
+		return;
+
+	bias = ar9003_modal_header(ah, is2ghz)->xlna_bias_strength;
+	REG_RMW_FIELD(ah, AR_PHY_65NM_CH0_RXTX4, AR_PHY_65NM_RXTX4_XLNA_BIAS,
+		      bias & 0x3);
+	bias >>= 2;
+	REG_RMW_FIELD(ah, AR_PHY_65NM_CH1_RXTX4, AR_PHY_65NM_RXTX4_XLNA_BIAS,
+		      bias & 0x3);
+	bias >>= 2;
+	REG_RMW_FIELD(ah, AR_PHY_65NM_CH2_RXTX4, AR_PHY_65NM_RXTX4_XLNA_BIAS,
+		      bias & 0x3);
+}
+
 static void ath9k_hw_ar9300_set_board_values(struct ath_hw *ah,
 					     struct ath9k_channel *chan)
 {
-	ar9003_hw_xpa_bias_level_apply(ah, IS_CHAN_2GHZ(chan));
-	ar9003_hw_ant_ctrl_apply(ah, IS_CHAN_2GHZ(chan));
+	bool is2ghz = IS_CHAN_2GHZ(chan);
+	ar9003_hw_xpa_timing_control_apply(ah, is2ghz);
+	ar9003_hw_xpa_bias_level_apply(ah, is2ghz);
+	ar9003_hw_ant_ctrl_apply(ah, is2ghz);
 	ar9003_hw_drive_strength_apply(ah);
+	ar9003_hw_xlna_bias_strength_apply(ah, is2ghz);
 	ar9003_hw_atten_apply(ah, chan);
 	ar9003_hw_quick_drop_apply(ah, chan->channel);
-	if (!AR_SREV_9330(ah) && !AR_SREV_9340(ah))
+	if (!AR_SREV_9330(ah) && !AR_SREV_9340(ah) && !AR_SREV_9550(ah))
 		ar9003_hw_internal_regulator_apply(ah);
-	if (AR_SREV_9485(ah) || AR_SREV_9330(ah) || AR_SREV_9340(ah))
-		ar9003_hw_apply_tuning_caps(ah);
-	ar9003_hw_txend_to_xpa_off_apply(ah, chan->channel);
+	ar9003_hw_apply_tuning_caps(ah);
+	ar9003_hw_txend_to_xpa_off_apply(ah, is2ghz);
 }
 
 static void ath9k_hw_ar9300_set_addac(struct ath_hw *ah,
@@ -5096,14 +5127,9 @@ s32 ar9003_hw_get_rx_gain_idx(struct ath_hw *ah)
 	return (eep->baseEepHeader.txrxgain) & 0xf; /* bits 3:0 */
 }
 
-u8 *ar9003_get_spur_chan_ptr(struct ath_hw *ah, bool is_2ghz)
+u8 *ar9003_get_spur_chan_ptr(struct ath_hw *ah, bool is2ghz)
 {
-	struct ar9300_eeprom *eep = &ah->eeprom.ar9300_eep;
-
-	if (is_2ghz)
-		return eep->modalHeader2G.spurChans;
-	else
-		return eep->modalHeader5G.spurChans;
+	return ar9003_modal_header(ah, is2ghz)->spurChans;
 }
 
 unsigned int ar9003_get_paprd_scale_factor(struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h
index 8396d150ce0..3a1ff55bceb 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.h
@@ -231,7 +231,8 @@ struct ar9300_modal_eep_header {
 	__le32 papdRateMaskHt20;
 	__le32 papdRateMaskHt40;
 	__le16 switchcomspdt;
-	u8 futureModal[8];
+	u8 xlna_bias_strength;
+	u8 futureModal[7];
 } __packed;
 
 struct ar9300_cal_data_per_freq_op_loop {
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_hw.c b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
index a0e3394b10d..1e8a4da5952 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
@@ -21,6 +21,7 @@
 #include "ar9340_initvals.h"
 #include "ar9330_1p1_initvals.h"
 #include "ar9330_1p2_initvals.h"
+#include "ar955x_1p0_initvals.h"
 #include "ar9580_1p0_initvals.h"
 #include "ar9462_2p0_initvals.h"
 
@@ -43,408 +44,310 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 		ar9462_2p0_baseband_core_txfir_coeff_japan_2484
 	if (AR_SREV_9330_11(ah)) {
 		/* mac */
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
-				ar9331_1p1_mac_core,
-				ARRAY_SIZE(ar9331_1p1_mac_core), 2);
+				ar9331_1p1_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9331_1p1_mac_postamble,
-				ARRAY_SIZE(ar9331_1p1_mac_postamble), 5);
+				ar9331_1p1_mac_postamble);
 
 		/* bb */
-		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9331_1p1_baseband_core,
-				ARRAY_SIZE(ar9331_1p1_baseband_core), 2);
+				ar9331_1p1_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9331_1p1_baseband_postamble,
-				ARRAY_SIZE(ar9331_1p1_baseband_postamble), 5);
+				ar9331_1p1_baseband_postamble);
 
 		/* radio */
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
-				ar9331_1p1_radio_core,
-				ARRAY_SIZE(ar9331_1p1_radio_core), 2);
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST], NULL, 0, 0);
+				ar9331_1p1_radio_core);
 
 		/* soc */
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
-				ar9331_1p1_soc_preamble,
-				ARRAY_SIZE(ar9331_1p1_soc_preamble), 2);
-		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+				ar9331_1p1_soc_preamble);
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
-				ar9331_1p1_soc_postamble,
-				ARRAY_SIZE(ar9331_1p1_soc_postamble), 2);
+				ar9331_1p1_soc_postamble);
 
 		/* rx/tx gain */
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9331_common_rx_gain_1p1,
-				ARRAY_SIZE(ar9331_common_rx_gain_1p1), 2);
+				ar9331_common_rx_gain_1p1);
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_lowest_ob_db_tx_gain_1p1,
-			ARRAY_SIZE(ar9331_modes_lowest_ob_db_tx_gain_1p1),
-			5);
+				ar9331_modes_lowest_ob_db_tx_gain_1p1);
 
 		/* additional clock settings */
 		if (ah->is_clk_25mhz)
 			INIT_INI_ARRAY(&ah->iniAdditional,
-					ar9331_1p1_xtal_25M,
-					ARRAY_SIZE(ar9331_1p1_xtal_25M), 2);
+					ar9331_1p1_xtal_25M);
 		else
 			INIT_INI_ARRAY(&ah->iniAdditional,
-					ar9331_1p1_xtal_40M,
-					ARRAY_SIZE(ar9331_1p1_xtal_40M), 2);
+					ar9331_1p1_xtal_40M);
 	} else if (AR_SREV_9330_12(ah)) {
 		/* mac */
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
-				ar9331_1p2_mac_core,
-				ARRAY_SIZE(ar9331_1p2_mac_core), 2);
+				ar9331_1p2_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9331_1p2_mac_postamble,
-				ARRAY_SIZE(ar9331_1p2_mac_postamble), 5);
+				ar9331_1p2_mac_postamble);
 
 		/* bb */
-		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9331_1p2_baseband_core,
-				ARRAY_SIZE(ar9331_1p2_baseband_core), 2);
+				ar9331_1p2_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9331_1p2_baseband_postamble,
-				ARRAY_SIZE(ar9331_1p2_baseband_postamble), 5);
+				ar9331_1p2_baseband_postamble);
 
 		/* radio */
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
-				ar9331_1p2_radio_core,
-				ARRAY_SIZE(ar9331_1p2_radio_core), 2);
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST], NULL, 0, 0);
+				ar9331_1p2_radio_core);
 
 		/* soc */
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
-				ar9331_1p2_soc_preamble,
-				ARRAY_SIZE(ar9331_1p2_soc_preamble), 2);
-		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+				ar9331_1p2_soc_preamble);
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
-				ar9331_1p2_soc_postamble,
-				ARRAY_SIZE(ar9331_1p2_soc_postamble), 2);
+				ar9331_1p2_soc_postamble);
 
 		/* rx/tx gain */
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9331_common_rx_gain_1p2,
-				ARRAY_SIZE(ar9331_common_rx_gain_1p2), 2);
+				ar9331_common_rx_gain_1p2);
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_lowest_ob_db_tx_gain_1p2,
-			ARRAY_SIZE(ar9331_modes_lowest_ob_db_tx_gain_1p2),
-			5);
+				ar9331_modes_lowest_ob_db_tx_gain_1p2);
 
 		/* additional clock settings */
 		if (ah->is_clk_25mhz)
 			INIT_INI_ARRAY(&ah->iniAdditional,
-					ar9331_1p2_xtal_25M,
-					ARRAY_SIZE(ar9331_1p2_xtal_25M), 2);
+					ar9331_1p2_xtal_25M);
 		else
 			INIT_INI_ARRAY(&ah->iniAdditional,
-					ar9331_1p2_xtal_40M,
-					ARRAY_SIZE(ar9331_1p2_xtal_40M), 2);
+					ar9331_1p2_xtal_40M);
 	} else if (AR_SREV_9340(ah)) {
 		/* mac */
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
-				ar9340_1p0_mac_core,
-				ARRAY_SIZE(ar9340_1p0_mac_core), 2);
+				ar9340_1p0_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9340_1p0_mac_postamble,
-				ARRAY_SIZE(ar9340_1p0_mac_postamble), 5);
+				ar9340_1p0_mac_postamble);
 
 		/* bb */
-		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9340_1p0_baseband_core,
-				ARRAY_SIZE(ar9340_1p0_baseband_core), 2);
+				ar9340_1p0_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9340_1p0_baseband_postamble,
-				ARRAY_SIZE(ar9340_1p0_baseband_postamble), 5);
+				ar9340_1p0_baseband_postamble);
 
 		/* radio */
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
-				ar9340_1p0_radio_core,
-				ARRAY_SIZE(ar9340_1p0_radio_core), 2);
+				ar9340_1p0_radio_core);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
-				ar9340_1p0_radio_postamble,
-				ARRAY_SIZE(ar9340_1p0_radio_postamble), 5);
+				ar9340_1p0_radio_postamble);
 
 		/* soc */
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
-				ar9340_1p0_soc_preamble,
-				ARRAY_SIZE(ar9340_1p0_soc_preamble), 2);
-		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+				ar9340_1p0_soc_preamble);
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
-				ar9340_1p0_soc_postamble,
-				ARRAY_SIZE(ar9340_1p0_soc_postamble), 5);
+				ar9340_1p0_soc_postamble);
 
 		/* rx/tx gain */
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9340Common_wo_xlna_rx_gain_table_1p0,
-				ARRAY_SIZE(ar9340Common_wo_xlna_rx_gain_table_1p0),
-				5);
+				ar9340Common_wo_xlna_rx_gain_table_1p0);
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9340Modes_high_ob_db_tx_gain_table_1p0,
-				ARRAY_SIZE(ar9340Modes_high_ob_db_tx_gain_table_1p0),
-				5);
+				ar9340Modes_high_ob_db_tx_gain_table_1p0);
 
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
-				ar9340Modes_fast_clock_1p0,
-				ARRAY_SIZE(ar9340Modes_fast_clock_1p0),
-				3);
+				ar9340Modes_fast_clock_1p0);
 
 		if (!ah->is_clk_25mhz)
 			INIT_INI_ARRAY(&ah->iniAdditional,
-				       ar9340_1p0_radio_core_40M,
-				       ARRAY_SIZE(ar9340_1p0_radio_core_40M),
-				       2);
+				       ar9340_1p0_radio_core_40M);
 	} else 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);
+				ar9485_1_1_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9485_1_1_mac_postamble,
-				ARRAY_SIZE(ar9485_1_1_mac_postamble), 5);
+				ar9485_1_1_mac_postamble);
 
 		/* 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_PRE], ar9485_1_1);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9485_1_1_baseband_core,
-				ARRAY_SIZE(ar9485_1_1_baseband_core), 2);
+				ar9485_1_1_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9485_1_1_baseband_postamble,
-				ARRAY_SIZE(ar9485_1_1_baseband_postamble), 5);
+				ar9485_1_1_baseband_postamble);
 
 		/* 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);
+				ar9485_1_1_radio_core);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
-				ar9485_1_1_radio_postamble,
-				ARRAY_SIZE(ar9485_1_1_radio_postamble), 2);
+				ar9485_1_1_radio_postamble);
 
 		/* 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);
+				ar9485_1_1_soc_preamble);
 
 		/* rx/tx gain */
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9485Common_wo_xlna_rx_gain_1_1,
-				ARRAY_SIZE(ar9485Common_wo_xlna_rx_gain_1_1), 2);
+				ar9485Common_wo_xlna_rx_gain_1_1);
 		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);
+				ar9485_modes_lowest_ob_db_tx_gain_1_1);
 
 		/* 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);
+				ar9485_1_1_pcie_phy_clkreq_disable_L1);
 
 		/* 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);
+				ar9485_1_1_pcie_phy_clkreq_disable_L1);
 	} else if (AR_SREV_9462_20(ah)) {
 
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE], ar9462_2p0_mac_core,
-				ARRAY_SIZE(ar9462_2p0_mac_core), 2);
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE], ar9462_2p0_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9462_2p0_mac_postamble,
-				ARRAY_SIZE(ar9462_2p0_mac_postamble), 5);
+				ar9462_2p0_mac_postamble);
 
-		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9462_2p0_baseband_core,
-				ARRAY_SIZE(ar9462_2p0_baseband_core), 2);
+				ar9462_2p0_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9462_2p0_baseband_postamble,
-				ARRAY_SIZE(ar9462_2p0_baseband_postamble), 5);
+				ar9462_2p0_baseband_postamble);
 
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
-				ar9462_2p0_radio_core,
-				ARRAY_SIZE(ar9462_2p0_radio_core), 2);
+				ar9462_2p0_radio_core);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
-				ar9462_2p0_radio_postamble,
-				ARRAY_SIZE(ar9462_2p0_radio_postamble), 5);
+				ar9462_2p0_radio_postamble);
 		INIT_INI_ARRAY(&ah->ini_radio_post_sys2ant,
-				ar9462_2p0_radio_postamble_sys2ant,
-				ARRAY_SIZE(ar9462_2p0_radio_postamble_sys2ant),
-				5);
+				ar9462_2p0_radio_postamble_sys2ant);
 
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
-				ar9462_2p0_soc_preamble,
-				ARRAY_SIZE(ar9462_2p0_soc_preamble), 2);
-		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+				ar9462_2p0_soc_preamble);
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
-				ar9462_2p0_soc_postamble,
-				ARRAY_SIZE(ar9462_2p0_soc_postamble), 5);
+				ar9462_2p0_soc_postamble);
 
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9462_common_rx_gain_table_2p0,
-				ARRAY_SIZE(ar9462_common_rx_gain_table_2p0), 2);
+				ar9462_common_rx_gain_table_2p0);
 
 		/* Awake -> Sleep Setting */
 		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-				PCIE_PLL_ON_CREQ_DIS_L1_2P0,
-				ARRAY_SIZE(PCIE_PLL_ON_CREQ_DIS_L1_2P0),
-				2);
+				PCIE_PLL_ON_CREQ_DIS_L1_2P0);
 		/* Sleep -> Awake Setting */
 		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-				PCIE_PLL_ON_CREQ_DIS_L1_2P0,
-				ARRAY_SIZE(PCIE_PLL_ON_CREQ_DIS_L1_2P0),
-				2);
+				PCIE_PLL_ON_CREQ_DIS_L1_2P0);
 
 		/* Fast clock modal settings */
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
-				ar9462_modes_fast_clock_2p0,
-				ARRAY_SIZE(ar9462_modes_fast_clock_2p0), 3);
+				ar9462_modes_fast_clock_2p0);
 
 		INIT_INI_ARRAY(&ah->iniCckfirJapan2484,
-				AR9462_BB_CTX_COEFJ(2p0),
-				ARRAY_SIZE(AR9462_BB_CTX_COEFJ(2p0)), 2);
+				AR9462_BB_CTX_COEFJ(2p0));
 
-		INIT_INI_ARRAY(&ah->ini_japan2484, AR9462_BBC_TXIFR_COEFFJ,
-				ARRAY_SIZE(AR9462_BBC_TXIFR_COEFFJ), 2);
+		INIT_INI_ARRAY(&ah->ini_japan2484, AR9462_BBC_TXIFR_COEFFJ);
+	} else if (AR_SREV_9550(ah)) {
+		/* mac */
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
+				ar955x_1p0_mac_core);
+		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
+				ar955x_1p0_mac_postamble);
+
+		/* bb */
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
+				ar955x_1p0_baseband_core);
+		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
+				ar955x_1p0_baseband_postamble);
+
+		/* radio */
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
+				ar955x_1p0_radio_core);
+		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
+				ar955x_1p0_radio_postamble);
+
+		/* soc */
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
+				ar955x_1p0_soc_preamble);
+		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
+				ar955x_1p0_soc_postamble);
+
+		/* rx/tx gain */
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+			ar955x_1p0_common_wo_xlna_rx_gain_table);
+		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
+			ar955x_1p0_common_wo_xlna_rx_gain_bounds);
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+				ar955x_1p0_modes_xpa_tx_gain_table);
 
+		/* Fast clock modal settings */
+		INIT_INI_ARRAY(&ah->iniModesFastClock,
+				ar955x_1p0_modes_fast_clock);
 	} else if (AR_SREV_9580(ah)) {
 		/* mac */
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
-				ar9580_1p0_mac_core,
-				ARRAY_SIZE(ar9580_1p0_mac_core), 2);
+				ar9580_1p0_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9580_1p0_mac_postamble,
-				ARRAY_SIZE(ar9580_1p0_mac_postamble), 5);
+				ar9580_1p0_mac_postamble);
 
 		/* bb */
-		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9580_1p0_baseband_core,
-				ARRAY_SIZE(ar9580_1p0_baseband_core), 2);
+				ar9580_1p0_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9580_1p0_baseband_postamble,
-				ARRAY_SIZE(ar9580_1p0_baseband_postamble), 5);
+				ar9580_1p0_baseband_postamble);
 
 		/* radio */
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
-				ar9580_1p0_radio_core,
-				ARRAY_SIZE(ar9580_1p0_radio_core), 2);
+				ar9580_1p0_radio_core);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
-				ar9580_1p0_radio_postamble,
-				ARRAY_SIZE(ar9580_1p0_radio_postamble), 5);
+				ar9580_1p0_radio_postamble);
 
 		/* soc */
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
-				ar9580_1p0_soc_preamble,
-				ARRAY_SIZE(ar9580_1p0_soc_preamble), 2);
-		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+				ar9580_1p0_soc_preamble);
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
-				ar9580_1p0_soc_postamble,
-				ARRAY_SIZE(ar9580_1p0_soc_postamble), 5);
+				ar9580_1p0_soc_postamble);
 
 		/* rx/tx gain */
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9580_1p0_rx_gain_table,
-				ARRAY_SIZE(ar9580_1p0_rx_gain_table), 2);
+				ar9580_1p0_rx_gain_table);
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9580_1p0_low_ob_db_tx_gain_table,
-				ARRAY_SIZE(ar9580_1p0_low_ob_db_tx_gain_table),
-				5);
+				ar9580_1p0_low_ob_db_tx_gain_table);
 
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
-				ar9580_1p0_modes_fast_clock,
-				ARRAY_SIZE(ar9580_1p0_modes_fast_clock),
-				3);
+				ar9580_1p0_modes_fast_clock);
 	} else {
 		/* mac */
-		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_CORE],
-				ar9300_2p2_mac_core,
-				ARRAY_SIZE(ar9300_2p2_mac_core), 2);
+				ar9300_2p2_mac_core);
 		INIT_INI_ARRAY(&ah->iniMac[ATH_INI_POST],
-				ar9300_2p2_mac_postamble,
-				ARRAY_SIZE(ar9300_2p2_mac_postamble), 5);
+				ar9300_2p2_mac_postamble);
 
 		/* bb */
-		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_CORE],
-				ar9300_2p2_baseband_core,
-				ARRAY_SIZE(ar9300_2p2_baseband_core), 2);
+				ar9300_2p2_baseband_core);
 		INIT_INI_ARRAY(&ah->iniBB[ATH_INI_POST],
-				ar9300_2p2_baseband_postamble,
-				ARRAY_SIZE(ar9300_2p2_baseband_postamble), 5);
+				ar9300_2p2_baseband_postamble);
 
 		/* radio */
-		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_PRE], NULL, 0, 0);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_CORE],
-				ar9300_2p2_radio_core,
-				ARRAY_SIZE(ar9300_2p2_radio_core), 2);
+				ar9300_2p2_radio_core);
 		INIT_INI_ARRAY(&ah->iniRadio[ATH_INI_POST],
-				ar9300_2p2_radio_postamble,
-				ARRAY_SIZE(ar9300_2p2_radio_postamble), 5);
+				ar9300_2p2_radio_postamble);
 
 		/* soc */
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_PRE],
-				ar9300_2p2_soc_preamble,
-				ARRAY_SIZE(ar9300_2p2_soc_preamble), 2);
-		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_CORE], NULL, 0, 0);
+				ar9300_2p2_soc_preamble);
 		INIT_INI_ARRAY(&ah->iniSOC[ATH_INI_POST],
-				ar9300_2p2_soc_postamble,
-				ARRAY_SIZE(ar9300_2p2_soc_postamble), 5);
+				ar9300_2p2_soc_postamble);
 
 		/* rx/tx gain */
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9300Common_rx_gain_table_2p2,
-				ARRAY_SIZE(ar9300Common_rx_gain_table_2p2), 2);
+				ar9300Common_rx_gain_table_2p2);
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-				ar9300Modes_lowest_ob_db_tx_gain_table_2p2,
-				ARRAY_SIZE(ar9300Modes_lowest_ob_db_tx_gain_table_2p2),
-				5);
+				ar9300Modes_lowest_ob_db_tx_gain_table_2p2);
 
 		/* Load PCIE SERDES settings from INI */
 
 		/* Awake Setting */
 
 		INIT_INI_ARRAY(&ah->iniPcieSerdes,
-				ar9300PciePhy_pll_on_clkreq_disable_L1_2p2,
-				ARRAY_SIZE(ar9300PciePhy_pll_on_clkreq_disable_L1_2p2),
-				2);
+				ar9300PciePhy_pll_on_clkreq_disable_L1_2p2);
 
 		/* Sleep Setting */
 
 		INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-				ar9300PciePhy_pll_on_clkreq_disable_L1_2p2,
-				ARRAY_SIZE(ar9300PciePhy_pll_on_clkreq_disable_L1_2p2),
-				2);
+				ar9300PciePhy_pll_on_clkreq_disable_L1_2p2);
 
 		/* Fast clock modal settings */
 		INIT_INI_ARRAY(&ah->iniModesFastClock,
-				ar9300Modes_fast_clock_2p2,
-				ARRAY_SIZE(ar9300Modes_fast_clock_2p2),
-				3);
+				ar9300Modes_fast_clock_2p2);
 	}
 }
 
@@ -452,146 +355,110 @@ static void ar9003_tx_gain_table_mode0(struct ath_hw *ah)
 {
 	if (AR_SREV_9330_12(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_lowest_ob_db_tx_gain_1p2,
-			ARRAY_SIZE(ar9331_modes_lowest_ob_db_tx_gain_1p2),
-			5);
+			ar9331_modes_lowest_ob_db_tx_gain_1p2);
 	else if (AR_SREV_9330_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_lowest_ob_db_tx_gain_1p1,
-			ARRAY_SIZE(ar9331_modes_lowest_ob_db_tx_gain_1p1),
-			5);
+			ar9331_modes_lowest_ob_db_tx_gain_1p1);
 	else if (AR_SREV_9340(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9340Modes_lowest_ob_db_tx_gain_table_1p0,
-			ARRAY_SIZE(ar9340Modes_lowest_ob_db_tx_gain_table_1p0),
-			5);
+			ar9340Modes_lowest_ob_db_tx_gain_table_1p0);
 	else 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);
+			ar9485_modes_lowest_ob_db_tx_gain_1_1);
+	else if (AR_SREV_9550(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			ar955x_1p0_modes_xpa_tx_gain_table);
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9580_1p0_lowest_ob_db_tx_gain_table,
-			ARRAY_SIZE(ar9580_1p0_lowest_ob_db_tx_gain_table),
-			5);
+			ar9580_1p0_lowest_ob_db_tx_gain_table);
 	else if (AR_SREV_9462_20(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9462_modes_low_ob_db_tx_gain_table_2p0,
-			ARRAY_SIZE(ar9462_modes_low_ob_db_tx_gain_table_2p0),
-			5);
+			ar9462_modes_low_ob_db_tx_gain_table_2p0);
 	else
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9300Modes_lowest_ob_db_tx_gain_table_2p2,
-			ARRAY_SIZE(ar9300Modes_lowest_ob_db_tx_gain_table_2p2),
-			5);
+			ar9300Modes_lowest_ob_db_tx_gain_table_2p2);
 }
 
 static void ar9003_tx_gain_table_mode1(struct ath_hw *ah)
 {
 	if (AR_SREV_9330_12(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_high_ob_db_tx_gain_1p2,
-			ARRAY_SIZE(ar9331_modes_high_ob_db_tx_gain_1p2),
-			5);
+			ar9331_modes_high_ob_db_tx_gain_1p2);
 	else if (AR_SREV_9330_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_high_ob_db_tx_gain_1p1,
-			ARRAY_SIZE(ar9331_modes_high_ob_db_tx_gain_1p1),
-			5);
+			ar9331_modes_high_ob_db_tx_gain_1p1);
 	else if (AR_SREV_9340(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9340Modes_lowest_ob_db_tx_gain_table_1p0,
-			ARRAY_SIZE(ar9340Modes_lowest_ob_db_tx_gain_table_1p0),
-			5);
+			ar9340Modes_high_ob_db_tx_gain_table_1p0);
 	else 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);
+			ar9485Modes_high_ob_db_tx_gain_1_1);
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9580_1p0_high_ob_db_tx_gain_table,
-			ARRAY_SIZE(ar9580_1p0_high_ob_db_tx_gain_table),
-			5);
+			ar9580_1p0_high_ob_db_tx_gain_table);
+	else if (AR_SREV_9550(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			ar955x_1p0_modes_no_xpa_tx_gain_table);
 	else if (AR_SREV_9462_20(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9462_modes_high_ob_db_tx_gain_table_2p0,
-			ARRAY_SIZE(ar9462_modes_high_ob_db_tx_gain_table_2p0),
-			5);
+			ar9462_modes_high_ob_db_tx_gain_table_2p0);
 	else
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9300Modes_high_ob_db_tx_gain_table_2p2,
-			ARRAY_SIZE(ar9300Modes_high_ob_db_tx_gain_table_2p2),
-			5);
+			ar9300Modes_high_ob_db_tx_gain_table_2p2);
 }
 
 static void ar9003_tx_gain_table_mode2(struct ath_hw *ah)
 {
 	if (AR_SREV_9330_12(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_low_ob_db_tx_gain_1p2,
-			ARRAY_SIZE(ar9331_modes_low_ob_db_tx_gain_1p2),
-			5);
+			ar9331_modes_low_ob_db_tx_gain_1p2);
 	else if (AR_SREV_9330_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_low_ob_db_tx_gain_1p1,
-			ARRAY_SIZE(ar9331_modes_low_ob_db_tx_gain_1p1),
-			5);
+			ar9331_modes_low_ob_db_tx_gain_1p1);
 	else if (AR_SREV_9340(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9340Modes_lowest_ob_db_tx_gain_table_1p0,
-			ARRAY_SIZE(ar9340Modes_lowest_ob_db_tx_gain_table_1p0),
-			5);
+			ar9340Modes_low_ob_db_tx_gain_table_1p0);
 	else 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);
+			ar9485Modes_low_ob_db_tx_gain_1_1);
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9580_1p0_low_ob_db_tx_gain_table,
-			ARRAY_SIZE(ar9580_1p0_low_ob_db_tx_gain_table),
-			5);
+			ar9580_1p0_low_ob_db_tx_gain_table);
 	else
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9300Modes_low_ob_db_tx_gain_table_2p2,
-			ARRAY_SIZE(ar9300Modes_low_ob_db_tx_gain_table_2p2),
-			5);
+			ar9300Modes_low_ob_db_tx_gain_table_2p2);
 }
 
 static void ar9003_tx_gain_table_mode3(struct ath_hw *ah)
 {
 	if (AR_SREV_9330_12(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_high_power_tx_gain_1p2,
-			ARRAY_SIZE(ar9331_modes_high_power_tx_gain_1p2),
-			5);
+			ar9331_modes_high_power_tx_gain_1p2);
 	else if (AR_SREV_9330_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9331_modes_high_power_tx_gain_1p1,
-			ARRAY_SIZE(ar9331_modes_high_power_tx_gain_1p1),
-			5);
+			ar9331_modes_high_power_tx_gain_1p1);
 	else if (AR_SREV_9340(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9340Modes_lowest_ob_db_tx_gain_table_1p0,
-			ARRAY_SIZE(ar9340Modes_lowest_ob_db_tx_gain_table_1p0),
-			5);
+			ar9340Modes_high_power_tx_gain_table_1p0);
 	else 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);
+			ar9485Modes_high_power_tx_gain_1_1);
 	else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9580_1p0_high_power_tx_gain_table,
-			ARRAY_SIZE(ar9580_1p0_high_power_tx_gain_table),
-			5);
+			ar9580_1p0_high_power_tx_gain_table);
 	else
 		INIT_INI_ARRAY(&ah->iniModesTxGain,
-			ar9300Modes_high_power_tx_gain_table_2p2,
-			ARRAY_SIZE(ar9300Modes_high_power_tx_gain_table_2p2),
-			5);
+			ar9300Modes_high_power_tx_gain_table_2p2);
+}
+
+static void ar9003_tx_gain_table_mode4(struct ath_hw *ah)
+{
+	if (AR_SREV_9340(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			ar9340Modes_mixed_ob_db_tx_gain_table_1p0);
+	else if (AR_SREV_9580(ah))
+		INIT_INI_ARRAY(&ah->iniModesTxGain,
+			ar9580_1p0_mixed_ob_db_tx_gain_table);
 }
 
 static void ar9003_tx_gain_table_apply(struct ath_hw *ah)
@@ -610,6 +477,9 @@ static void ar9003_tx_gain_table_apply(struct ath_hw *ah)
 	case 3:
 		ar9003_tx_gain_table_mode3(ah);
 		break;
+	case 4:
+		ar9003_tx_gain_table_mode4(ah);
+		break;
 	}
 }
 
@@ -617,86 +487,67 @@ static void ar9003_rx_gain_table_mode0(struct ath_hw *ah)
 {
 	if (AR_SREV_9330_12(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9331_common_rx_gain_1p2,
-				ARRAY_SIZE(ar9331_common_rx_gain_1p2),
-				2);
+				ar9331_common_rx_gain_1p2);
 	else if (AR_SREV_9330_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9331_common_rx_gain_1p1,
-				ARRAY_SIZE(ar9331_common_rx_gain_1p1),
-				2);
+				ar9331_common_rx_gain_1p1);
 	else if (AR_SREV_9340(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9340Common_rx_gain_table_1p0,
-				ARRAY_SIZE(ar9340Common_rx_gain_table_1p0),
-				2);
+				ar9340Common_rx_gain_table_1p0);
 	else 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_9580(ah))
+				ar9485Common_wo_xlna_rx_gain_1_1);
+	else if (AR_SREV_9550(ah)) {
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+				ar955x_1p0_common_rx_gain_table);
+		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
+				ar955x_1p0_common_rx_gain_bounds);
+	} else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9580_1p0_rx_gain_table,
-				ARRAY_SIZE(ar9580_1p0_rx_gain_table),
-				2);
+				ar9580_1p0_rx_gain_table);
 	else if (AR_SREV_9462_20(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9462_common_rx_gain_table_2p0,
-				ARRAY_SIZE(ar9462_common_rx_gain_table_2p0),
-				2);
+				ar9462_common_rx_gain_table_2p0);
 	else
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-				ar9300Common_rx_gain_table_2p2,
-				ARRAY_SIZE(ar9300Common_rx_gain_table_2p2),
-				2);
+				ar9300Common_rx_gain_table_2p2);
 }
 
 static void ar9003_rx_gain_table_mode1(struct ath_hw *ah)
 {
 	if (AR_SREV_9330_12(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9331_common_wo_xlna_rx_gain_1p2,
-			ARRAY_SIZE(ar9331_common_wo_xlna_rx_gain_1p2),
-			2);
+			ar9331_common_wo_xlna_rx_gain_1p2);
 	else if (AR_SREV_9330_11(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9331_common_wo_xlna_rx_gain_1p1,
-			ARRAY_SIZE(ar9331_common_wo_xlna_rx_gain_1p1),
-			2);
+			ar9331_common_wo_xlna_rx_gain_1p1);
 	else if (AR_SREV_9340(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9340Common_wo_xlna_rx_gain_table_1p0,
-			ARRAY_SIZE(ar9340Common_wo_xlna_rx_gain_table_1p0),
-			2);
+			ar9340Common_wo_xlna_rx_gain_table_1p0);
 	else 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);
+			ar9485Common_wo_xlna_rx_gain_1_1);
 	else if (AR_SREV_9462_20(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9462_common_wo_xlna_rx_gain_table_2p0,
-			ARRAY_SIZE(ar9462_common_wo_xlna_rx_gain_table_2p0),
-			2);
-	else if (AR_SREV_9580(ah))
+			ar9462_common_wo_xlna_rx_gain_table_2p0);
+	else if (AR_SREV_9550(ah)) {
+		INIT_INI_ARRAY(&ah->iniModesRxGain,
+			ar955x_1p0_common_wo_xlna_rx_gain_table);
+		INIT_INI_ARRAY(&ah->ini_modes_rx_gain_bounds,
+			ar955x_1p0_common_wo_xlna_rx_gain_bounds);
+	} else if (AR_SREV_9580(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9580_1p0_wo_xlna_rx_gain_table,
-			ARRAY_SIZE(ar9580_1p0_wo_xlna_rx_gain_table),
-			2);
+			ar9580_1p0_wo_xlna_rx_gain_table);
 	else
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			ar9300Common_wo_xlna_rx_gain_table_2p2,
-			ARRAY_SIZE(ar9300Common_wo_xlna_rx_gain_table_2p2),
-			2);
+			ar9300Common_wo_xlna_rx_gain_table_2p2);
 }
 
 static void ar9003_rx_gain_table_mode2(struct ath_hw *ah)
 {
 	if (AR_SREV_9462_20(ah))
 		INIT_INI_ARRAY(&ah->iniModesRxGain,
-			       ar9462_common_mixed_rx_gain_table_2p0,
-			       ARRAY_SIZE(ar9462_common_mixed_rx_gain_table_2p0), 2);
+			       ar9462_common_mixed_rx_gain_table_2p0);
 }
 
 static void ar9003_rx_gain_table_apply(struct ath_hw *ah)
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index d9e0824af09..78816b8b217 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
@@ -181,11 +181,14 @@ static bool ar9003_hw_get_isr(struct ath_hw *ah, enum ath9k_int *masked)
 	u32 mask2 = 0;
 	struct ath9k_hw_capabilities *pCap = &ah->caps;
 	struct ath_common *common = ath9k_hw_common(ah);
-	u32 sync_cause = 0, async_cause;
+	u32 sync_cause = 0, async_cause, async_mask = AR_INTR_MAC_IRQ;
+
+	if (ath9k_hw_mci_is_enabled(ah))
+		async_mask |= AR_INTR_ASYNC_MASK_MCI;
 
 	async_cause = REG_READ(ah, AR_INTR_ASYNC_CAUSE);
 
-	if (async_cause & (AR_INTR_MAC_IRQ | AR_INTR_ASYNC_MASK_MCI)) {
+	if (async_cause & async_mask) {
 		if ((REG_READ(ah, AR_RTC_STATUS) & AR_RTC_STATUS_M)
 				== AR_RTC_STATUS_ON)
 			isr = REG_READ(ah, AR_ISR);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.c b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
index ffbb180f91e..9a34fcaae3f 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
@@ -35,31 +35,30 @@ static int ar9003_mci_wait_for_interrupt(struct ath_hw *ah, u32 address,
 	struct ath_common *common = ath9k_hw_common(ah);
 
 	while (time_out) {
-		if (REG_READ(ah, address) & bit_position) {
-			REG_WRITE(ah, address, bit_position);
-
-			if (address == AR_MCI_INTERRUPT_RX_MSG_RAW) {
-				if (bit_position &
-				    AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE)
-					ar9003_mci_reset_req_wakeup(ah);
-
-				if (bit_position &
-				    (AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING |
-				     AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING))
-					REG_WRITE(ah, AR_MCI_INTERRUPT_RAW,
-					AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE);
-
-				REG_WRITE(ah, AR_MCI_INTERRUPT_RAW,
-					  AR_MCI_INTERRUPT_RX_MSG);
-			}
-			break;
-		}
+		if (!(REG_READ(ah, address) & bit_position)) {
+			udelay(10);
+			time_out -= 10;
 
-		udelay(10);
-		time_out -= 10;
+			if (time_out < 0)
+				break;
+			else
+				continue;
+		}
+		REG_WRITE(ah, address, bit_position);
 
-		if (time_out < 0)
+		if (address != AR_MCI_INTERRUPT_RX_MSG_RAW)
 			break;
+
+		if (bit_position & AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE)
+			ar9003_mci_reset_req_wakeup(ah);
+
+		if (bit_position & (AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING |
+				    AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING))
+			REG_WRITE(ah, AR_MCI_INTERRUPT_RAW,
+				  AR_MCI_INTERRUPT_REMOTE_SLEEP_UPDATE);
+
+		REG_WRITE(ah, AR_MCI_INTERRUPT_RAW, AR_MCI_INTERRUPT_RX_MSG);
+		break;
 	}
 
 	if (time_out <= 0) {
@@ -127,14 +126,13 @@ static void ar9003_mci_send_coex_version_query(struct ath_hw *ah,
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 payload[4] = {0, 0, 0, 0};
 
-	if (!mci->bt_version_known &&
-	    (mci->bt_state != MCI_BT_SLEEP)) {
-		MCI_GPM_SET_TYPE_OPCODE(payload,
-					MCI_GPM_COEX_AGENT,
-					MCI_GPM_COEX_VERSION_QUERY);
-		ar9003_mci_send_message(ah, MCI_GPM, 0, payload, 16,
-					wait_done, true);
-	}
+	if (mci->bt_version_known ||
+	    (mci->bt_state == MCI_BT_SLEEP))
+		return;
+
+	MCI_GPM_SET_TYPE_OPCODE(payload, MCI_GPM_COEX_AGENT,
+				MCI_GPM_COEX_VERSION_QUERY);
+	ar9003_mci_send_message(ah, MCI_GPM, 0, payload, 16, wait_done, true);
 }
 
 static void ar9003_mci_send_coex_version_response(struct ath_hw *ah,
@@ -158,15 +156,14 @@ static void ar9003_mci_send_coex_wlan_channels(struct ath_hw *ah,
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 *payload = &mci->wlan_channels[0];
 
-	if ((mci->wlan_channels_update == true) &&
-	    (mci->bt_state != MCI_BT_SLEEP)) {
-		MCI_GPM_SET_TYPE_OPCODE(payload,
-					MCI_GPM_COEX_AGENT,
-					MCI_GPM_COEX_WLAN_CHANNELS);
-		ar9003_mci_send_message(ah, MCI_GPM, 0, payload, 16,
-					wait_done, true);
-		MCI_GPM_SET_TYPE_OPCODE(payload, 0xff, 0xff);
-	}
+	if (!mci->wlan_channels_update ||
+	    (mci->bt_state == MCI_BT_SLEEP))
+		return;
+
+	MCI_GPM_SET_TYPE_OPCODE(payload, MCI_GPM_COEX_AGENT,
+				MCI_GPM_COEX_WLAN_CHANNELS);
+	ar9003_mci_send_message(ah, MCI_GPM, 0, payload, 16, wait_done, true);
+	MCI_GPM_SET_TYPE_OPCODE(payload, 0xff, 0xff);
 }
 
 static void ar9003_mci_send_coex_bt_status_query(struct ath_hw *ah,
@@ -174,29 +171,30 @@ static void ar9003_mci_send_coex_bt_status_query(struct ath_hw *ah,
 {
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 payload[4] = {0, 0, 0, 0};
-	bool query_btinfo = !!(query_type & (MCI_GPM_COEX_QUERY_BT_ALL_INFO |
-					     MCI_GPM_COEX_QUERY_BT_TOPOLOGY));
+	bool query_btinfo;
 
-	if (mci->bt_state != MCI_BT_SLEEP) {
+	if (mci->bt_state == MCI_BT_SLEEP)
+		return;
 
-		MCI_GPM_SET_TYPE_OPCODE(payload, MCI_GPM_COEX_AGENT,
-					MCI_GPM_COEX_STATUS_QUERY);
+	query_btinfo = !!(query_type & (MCI_GPM_COEX_QUERY_BT_ALL_INFO |
+					MCI_GPM_COEX_QUERY_BT_TOPOLOGY));
+	MCI_GPM_SET_TYPE_OPCODE(payload, MCI_GPM_COEX_AGENT,
+				MCI_GPM_COEX_STATUS_QUERY);
 
-		*(((u8 *)payload) + MCI_GPM_COEX_B_BT_BITMAP) = query_type;
-
-		/*
-		 * If bt_status_query message is  not sent successfully,
-		 * then need_flush_btinfo should be set again.
-		 */
-		if (!ar9003_mci_send_message(ah, MCI_GPM, 0, payload, 16,
-					     wait_done, true)) {
-			if (query_btinfo)
-				mci->need_flush_btinfo = true;
-		}
+	*(((u8 *)payload) + MCI_GPM_COEX_B_BT_BITMAP) = query_type;
 
+	/*
+	 * If bt_status_query message is  not sent successfully,
+	 * then need_flush_btinfo should be set again.
+	 */
+	if (!ar9003_mci_send_message(ah, MCI_GPM, 0, payload, 16,
+				wait_done, true)) {
 		if (query_btinfo)
-			mci->query_bt = false;
+			mci->need_flush_btinfo = true;
 	}
+
+	if (query_btinfo)
+		mci->query_bt = false;
 }
 
 static void ar9003_mci_send_coex_halt_bt_gpm(struct ath_hw *ah, bool halt,
@@ -241,73 +239,73 @@ static void ar9003_mci_prep_interface(struct ath_hw *ah)
 	ar9003_mci_remote_reset(ah, true);
 	ar9003_mci_send_req_wake(ah, true);
 
-	if (ar9003_mci_wait_for_interrupt(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
-				  AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING, 500)) {
+	if (!ar9003_mci_wait_for_interrupt(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
+				  AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING, 500))
+		goto clear_redunt;
 
-		mci->bt_state = MCI_BT_AWAKE;
+	mci->bt_state = MCI_BT_AWAKE;
 
-		/*
-		 * we don't need to send more remote_reset at this moment.
-		 * If BT receive first remote_reset, then BT HW will
-		 * be cleaned up and will be able to receive req_wake
-		 * and BT HW will respond sys_waking.
-		 * In this case, WLAN will receive BT's HW sys_waking.
-		 * Otherwise, if BT SW missed initial remote_reset,
-		 * that remote_reset will still clean up BT MCI RX,
-		 * and the req_wake will wake BT up,
-		 * and BT SW will respond this req_wake with a remote_reset and
-		 * sys_waking. In this case, WLAN will receive BT's SW
-		 * sys_waking. In either case, BT's RX is cleaned up. So we
-		 * don't need to reply BT's remote_reset now, if any.
-		 * Similarly, if in any case, WLAN can receive BT's sys_waking,
-		 * that means WLAN's RX is also fine.
-		 */
-		ar9003_mci_send_sys_waking(ah, true);
-		udelay(10);
+	/*
+	 * we don't need to send more remote_reset at this moment.
+	 * If BT receive first remote_reset, then BT HW will
+	 * be cleaned up and will be able to receive req_wake
+	 * and BT HW will respond sys_waking.
+	 * In this case, WLAN will receive BT's HW sys_waking.
+	 * Otherwise, if BT SW missed initial remote_reset,
+	 * that remote_reset will still clean up BT MCI RX,
+	 * and the req_wake will wake BT up,
+	 * and BT SW will respond this req_wake with a remote_reset and
+	 * sys_waking. In this case, WLAN will receive BT's SW
+	 * sys_waking. In either case, BT's RX is cleaned up. So we
+	 * don't need to reply BT's remote_reset now, if any.
+	 * Similarly, if in any case, WLAN can receive BT's sys_waking,
+	 * that means WLAN's RX is also fine.
+	 */
+	ar9003_mci_send_sys_waking(ah, true);
+	udelay(10);
 
-		/*
-		 * Set BT priority interrupt value to be 0xff to
-		 * avoid having too many BT PRIORITY interrupts.
-		 */
-		REG_WRITE(ah, AR_MCI_BT_PRI0, 0xFFFFFFFF);
-		REG_WRITE(ah, AR_MCI_BT_PRI1, 0xFFFFFFFF);
-		REG_WRITE(ah, AR_MCI_BT_PRI2, 0xFFFFFFFF);
-		REG_WRITE(ah, AR_MCI_BT_PRI3, 0xFFFFFFFF);
-		REG_WRITE(ah, AR_MCI_BT_PRI, 0X000000FF);
+	/*
+	 * Set BT priority interrupt value to be 0xff to
+	 * avoid having too many BT PRIORITY interrupts.
+	 */
+	REG_WRITE(ah, AR_MCI_BT_PRI0, 0xFFFFFFFF);
+	REG_WRITE(ah, AR_MCI_BT_PRI1, 0xFFFFFFFF);
+	REG_WRITE(ah, AR_MCI_BT_PRI2, 0xFFFFFFFF);
+	REG_WRITE(ah, AR_MCI_BT_PRI3, 0xFFFFFFFF);
+	REG_WRITE(ah, AR_MCI_BT_PRI, 0X000000FF);
 
-		/*
-		 * A contention reset will be received after send out
-		 * sys_waking. Also BT priority interrupt bits will be set.
-		 * Clear those bits before the next step.
-		 */
+	/*
+	 * A contention reset will be received after send out
+	 * sys_waking. Also BT priority interrupt bits will be set.
+	 * Clear those bits before the next step.
+	 */
 
-		REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
-			  AR_MCI_INTERRUPT_RX_MSG_CONT_RST);
-		REG_WRITE(ah, AR_MCI_INTERRUPT_RAW,
-			  AR_MCI_INTERRUPT_BT_PRI);
+	REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
+		  AR_MCI_INTERRUPT_RX_MSG_CONT_RST);
+	REG_WRITE(ah, AR_MCI_INTERRUPT_RAW, AR_MCI_INTERRUPT_BT_PRI);
 
-		if (mci->is_2g) {
-			ar9003_mci_send_lna_transfer(ah, true);
-			udelay(5);
-		}
+	if (mci->is_2g) {
+		ar9003_mci_send_lna_transfer(ah, true);
+		udelay(5);
+	}
 
-		if ((mci->is_2g && !mci->update_2g5g)) {
-			if (ar9003_mci_wait_for_interrupt(ah,
-					  AR_MCI_INTERRUPT_RX_MSG_RAW,
-					  AR_MCI_INTERRUPT_RX_MSG_LNA_INFO,
-					  mci_timeout))
-				ath_dbg(common, MCI,
-					"MCI WLAN has control over the LNA & BT obeys it\n");
-			else
-				ath_dbg(common, MCI,
-					"MCI BT didn't respond to LNA_TRANS\n");
-		}
+	if ((mci->is_2g && !mci->update_2g5g)) {
+		if (ar9003_mci_wait_for_interrupt(ah,
+					AR_MCI_INTERRUPT_RX_MSG_RAW,
+					AR_MCI_INTERRUPT_RX_MSG_LNA_INFO,
+					mci_timeout))
+			ath_dbg(common, MCI,
+				"MCI WLAN has control over the LNA & BT obeys it\n");
+		else
+			ath_dbg(common, MCI,
+				"MCI BT didn't respond to LNA_TRANS\n");
 	}
 
+clear_redunt:
 	/* Clear the extra redundant SYS_WAKING from BT */
 	if ((mci->bt_state == MCI_BT_AWAKE) &&
-		(REG_READ_FIELD(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
-				AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING)) &&
+	    (REG_READ_FIELD(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
+			    AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING)) &&
 	    (REG_READ_FIELD(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
 			    AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING) == 0)) {
 		REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
@@ -323,14 +321,13 @@ void ar9003_mci_set_full_sleep(struct ath_hw *ah)
 {
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 
-	if (ar9003_mci_state(ah, MCI_STATE_ENABLE, NULL) &&
+	if (ar9003_mci_state(ah, MCI_STATE_ENABLE) &&
 	    (mci->bt_state != MCI_BT_SLEEP) &&
 	    !mci->halted_bt_gpm) {
 		ar9003_mci_send_coex_halt_bt_gpm(ah, true, true);
 	}
 
 	mci->ready = false;
-	REG_WRITE(ah, AR_RTC_KEEP_AWAKE, 0x2);
 }
 
 static void ar9003_mci_disable_interrupt(struct ath_hw *ah)
@@ -487,7 +484,7 @@ static void ar9003_mci_sync_bt_state(struct ath_hw *ah)
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 cur_bt_state;
 
-	cur_bt_state = ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP, NULL);
+	cur_bt_state = ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP);
 
 	if (mci->bt_state != cur_bt_state)
 		mci->bt_state = cur_bt_state;
@@ -596,8 +593,7 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
 		if (!time_out)
 			break;
 
-		offset = ar9003_mci_state(ah, MCI_STATE_NEXT_GPM_OFFSET,
-					  &more_data);
+		offset = ar9003_mci_get_next_gpm_offset(ah, false, &more_data);
 
 		if (offset == MCI_GPM_INVALID)
 			continue;
@@ -615,9 +611,9 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
 				}
 				break;
 			}
-		} else if ((recv_type == gpm_type) && (recv_opcode == gpm_opcode)) {
+		} else if ((recv_type == gpm_type) &&
+			   (recv_opcode == gpm_opcode))
 			break;
-		}
 
 		/*
 		 * check if it's cal_grant
@@ -661,8 +657,7 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
 		time_out = 0;
 
 	while (more_data == MCI_GPM_MORE) {
-		offset = ar9003_mci_state(ah, MCI_STATE_NEXT_GPM_OFFSET,
-					  &more_data);
+		offset = ar9003_mci_get_next_gpm_offset(ah, false, &more_data);
 		if (offset == MCI_GPM_INVALID)
 			break;
 
@@ -731,38 +726,38 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (!IS_CHAN_2GHZ(chan) || (mci_hw->bt_state != MCI_BT_SLEEP))
 		goto exit;
 
-	if (ar9003_mci_check_int(ah, AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET) ||
-	    ar9003_mci_check_int(ah, AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE)) {
+	if (!ar9003_mci_check_int(ah, AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET) &&
+	    !ar9003_mci_check_int(ah, AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE))
+		goto exit;
 
-		/*
-		 * BT is sleeping. Check if BT wakes up during
-		 * WLAN calibration. If BT wakes up during
-		 * WLAN calibration, need to go through all
-		 * message exchanges again and recal.
-		 */
-		REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
-			  AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET |
-			  AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE);
+	/*
+	 * BT is sleeping. Check if BT wakes up during
+	 * WLAN calibration. If BT wakes up during
+	 * WLAN calibration, need to go through all
+	 * message exchanges again and recal.
+	 */
+	REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
+		  (AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET |
+		   AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE));
 
-		ar9003_mci_remote_reset(ah, true);
-		ar9003_mci_send_sys_waking(ah, true);
-		udelay(1);
+	ar9003_mci_remote_reset(ah, true);
+	ar9003_mci_send_sys_waking(ah, true);
+	udelay(1);
 
-		if (IS_CHAN_2GHZ(chan))
-			ar9003_mci_send_lna_transfer(ah, true);
+	if (IS_CHAN_2GHZ(chan))
+		ar9003_mci_send_lna_transfer(ah, true);
 
-		mci_hw->bt_state = MCI_BT_AWAKE;
+	mci_hw->bt_state = MCI_BT_AWAKE;
 
-		if (caldata) {
-			caldata->done_txiqcal_once = false;
-			caldata->done_txclcal_once = false;
-			caldata->rtt_done = false;
-		}
+	if (caldata) {
+		caldata->done_txiqcal_once = false;
+		caldata->done_txclcal_once = false;
+		caldata->rtt_done = false;
+	}
 
-		if (!ath9k_hw_init_cal(ah, chan))
-			return -EIO;
+	if (!ath9k_hw_init_cal(ah, chan))
+		return -EIO;
 
-	}
 exit:
 	ar9003_mci_enable_interrupt(ah);
 	return 0;
@@ -772,10 +767,6 @@ static void ar9003_mci_mute_bt(struct ath_hw *ah)
 {
 	/* disable all MCI messages */
 	REG_WRITE(ah, AR_MCI_MSG_ATTRIBUTES_TABLE, 0xffff0000);
-	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS0, 0xffffffff);
-	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS1, 0xffffffff);
-	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS2, 0xffffffff);
-	REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS3, 0xffffffff);
 	REG_SET_BIT(ah, AR_MCI_TX_CTRL, AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
 
 	/* wait pending HW messages to flush out */
@@ -798,29 +789,27 @@ static void ar9003_mci_osla_setup(struct ath_hw *ah, bool enable)
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 thresh;
 
-	if (enable) {
-		REG_RMW_FIELD(ah, AR_MCI_SCHD_TABLE_2,
-			      AR_MCI_SCHD_TABLE_2_HW_BASED, 1);
-		REG_RMW_FIELD(ah, AR_MCI_SCHD_TABLE_2,
-			      AR_MCI_SCHD_TABLE_2_MEM_BASED, 1);
-
-		if (!(mci->config & ATH_MCI_CONFIG_DISABLE_AGGR_THRESH)) {
-			thresh = MS(mci->config, ATH_MCI_CONFIG_AGGR_THRESH);
-			REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
-				      AR_BTCOEX_CTRL_AGGR_THRESH, thresh);
-			REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
-				      AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN, 1);
-		} else {
-			REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
-				      AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN, 0);
-		}
-
-		REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
-			      AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN, 1);
-	} else {
+	if (!enable) {
 		REG_CLR_BIT(ah, AR_BTCOEX_CTRL,
 			    AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+		return;
 	}
+	REG_RMW_FIELD(ah, AR_MCI_SCHD_TABLE_2, AR_MCI_SCHD_TABLE_2_HW_BASED, 1);
+	REG_RMW_FIELD(ah, AR_MCI_SCHD_TABLE_2,
+		      AR_MCI_SCHD_TABLE_2_MEM_BASED, 1);
+
+	if (!(mci->config & ATH_MCI_CONFIG_DISABLE_AGGR_THRESH)) {
+		thresh = MS(mci->config, ATH_MCI_CONFIG_AGGR_THRESH);
+		REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
+			      AR_BTCOEX_CTRL_AGGR_THRESH, thresh);
+		REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
+			      AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN, 1);
+	} else
+		REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
+			      AR_BTCOEX_CTRL_TIME_TO_NEXT_BT_THRESH_EN, 0);
+
+	REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
+		      AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN, 1);
 }
 
 void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
@@ -898,13 +887,16 @@ void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 		udelay(100);
 	}
 
+	/* Check pending GPM msg before MCI Reset Rx */
+	ar9003_mci_check_gpm_offset(ah);
+
 	regval |= SM(1, AR_MCI_COMMAND2_RESET_RX);
 	REG_WRITE(ah, AR_MCI_COMMAND2, regval);
 	udelay(1);
 	regval &= ~SM(1, AR_MCI_COMMAND2_RESET_RX);
 	REG_WRITE(ah, AR_MCI_COMMAND2, regval);
 
-	ar9003_mci_state(ah, MCI_STATE_INIT_GPM_OFFSET, NULL);
+	ar9003_mci_get_next_gpm_offset(ah, true, NULL);
 
 	REG_WRITE(ah, AR_MCI_MSG_ATTRIBUTES_TABLE,
 		  (SM(0xe801, AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR) |
@@ -943,26 +935,27 @@ static void ar9003_mci_send_2g5g_status(struct ath_hw *ah, bool wait_done)
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 	u32 new_flags, to_set, to_clear;
 
-	if (mci->update_2g5g && (mci->bt_state != MCI_BT_SLEEP)) {
-		if (mci->is_2g) {
-			new_flags = MCI_2G_FLAGS;
-			to_clear = MCI_2G_FLAGS_CLEAR_MASK;
-			to_set = MCI_2G_FLAGS_SET_MASK;
-		} else {
-			new_flags = MCI_5G_FLAGS;
-			to_clear = MCI_5G_FLAGS_CLEAR_MASK;
-			to_set = MCI_5G_FLAGS_SET_MASK;
-		}
+	if (!mci->update_2g5g || (mci->bt_state == MCI_BT_SLEEP))
+		return;
+
+	if (mci->is_2g) {
+		new_flags = MCI_2G_FLAGS;
+		to_clear = MCI_2G_FLAGS_CLEAR_MASK;
+		to_set = MCI_2G_FLAGS_SET_MASK;
+	} else {
+		new_flags = MCI_5G_FLAGS;
+		to_clear = MCI_5G_FLAGS_CLEAR_MASK;
+		to_set = MCI_5G_FLAGS_SET_MASK;
+	}
 
-		if (to_clear)
-			ar9003_mci_send_coex_bt_flags(ah, wait_done,
+	if (to_clear)
+		ar9003_mci_send_coex_bt_flags(ah, wait_done,
 					      MCI_GPM_COEX_BT_FLAGS_CLEAR,
 					      to_clear);
-		if (to_set)
-			ar9003_mci_send_coex_bt_flags(ah, wait_done,
+	if (to_set)
+		ar9003_mci_send_coex_bt_flags(ah, wait_done,
 					      MCI_GPM_COEX_BT_FLAGS_SET,
 					      to_set);
-	}
 }
 
 static void ar9003_mci_queue_unsent_gpm(struct ath_hw *ah, u8 header,
@@ -1014,38 +1007,36 @@ static void ar9003_mci_queue_unsent_gpm(struct ath_hw *ah, u8 header,
 	}
 }
 
-void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool wait_done)
+void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool force)
 {
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 
-	if (mci->update_2g5g) {
-		if (mci->is_2g) {
-			ar9003_mci_send_2g5g_status(ah, true);
-			ar9003_mci_send_lna_transfer(ah, true);
-			udelay(5);
+	if (!mci->update_2g5g && !force)
+		return;
 
-			REG_CLR_BIT(ah, AR_MCI_TX_CTRL,
-				    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
-			REG_CLR_BIT(ah, AR_PHY_GLB_CONTROL,
-				    AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
+	if (mci->is_2g) {
+		ar9003_mci_send_2g5g_status(ah, true);
+		ar9003_mci_send_lna_transfer(ah, true);
+		udelay(5);
 
-			if (!(mci->config & ATH_MCI_CONFIG_DISABLE_OSLA)) {
-				REG_SET_BIT(ah, AR_BTCOEX_CTRL,
-					    AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
-			}
-		} else {
-			ar9003_mci_send_lna_take(ah, true);
-			udelay(5);
+		REG_CLR_BIT(ah, AR_MCI_TX_CTRL,
+			    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
+		REG_CLR_BIT(ah, AR_PHY_GLB_CONTROL,
+			    AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
+
+		if (!(mci->config & ATH_MCI_CONFIG_DISABLE_OSLA))
+			ar9003_mci_osla_setup(ah, true);
+	} else {
+		ar9003_mci_send_lna_take(ah, true);
+		udelay(5);
 
-			REG_SET_BIT(ah, AR_MCI_TX_CTRL,
-				    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
-			REG_SET_BIT(ah, AR_PHY_GLB_CONTROL,
-				    AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
-			REG_CLR_BIT(ah, AR_BTCOEX_CTRL,
-				    AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+		REG_SET_BIT(ah, AR_MCI_TX_CTRL,
+			    AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
+		REG_SET_BIT(ah, AR_PHY_GLB_CONTROL,
+			    AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
 
-			ar9003_mci_send_2g5g_status(ah, true);
-		}
+		ar9003_mci_osla_setup(ah, false);
+		ar9003_mci_send_2g5g_status(ah, true);
 	}
 }
 
@@ -1132,7 +1123,7 @@ void ar9003_mci_init_cal_req(struct ath_hw *ah, bool *is_reusable)
 	if (ar9003_mci_wait_for_gpm(ah, MCI_GPM_BT_CAL_GRANT, 0, 50000)) {
 		ath_dbg(common, MCI, "MCI BT_CAL_GRANT received\n");
 	} else {
-		is_reusable = false;
+		*is_reusable = false;
 		ath_dbg(common, MCI, "MCI BT_CAL_GRANT not received\n");
 	}
 }
@@ -1173,11 +1164,10 @@ void ar9003_mci_cleanup(struct ath_hw *ah)
 }
 EXPORT_SYMBOL(ar9003_mci_cleanup);
 
-u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
+u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type)
 {
-	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
-	u32 value = 0, more_gpm = 0, gpm_ptr;
+	u32 value = 0;
 	u8 query_type;
 
 	switch (state_type) {
@@ -1190,81 +1180,6 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
 		}
 		value &= AR_BTCOEX_CTRL_MCI_MODE_EN;
 		break;
-	case MCI_STATE_INIT_GPM_OFFSET:
-		value = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
-		mci->gpm_idx = value;
-		break;
-	case MCI_STATE_NEXT_GPM_OFFSET:
-	case MCI_STATE_LAST_GPM_OFFSET:
-		/*
-		* This could be useful to avoid new GPM message interrupt which
-		* may lead to spurious interrupt after power sleep, or multiple
-		* entry of ath_mci_intr().
-		* Adding empty GPM check by returning HAL_MCI_GPM_INVALID can
-		* alleviate this effect, but clearing GPM RX interrupt bit is
-		* safe, because whether this is called from hw or driver code
-		* there must be an interrupt bit set/triggered initially
-		*/
-		REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
-			  AR_MCI_INTERRUPT_RX_MSG_GPM);
-
-		gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
-		value = gpm_ptr;
-
-		if (value == 0)
-			value = mci->gpm_len - 1;
-		else if (value >= mci->gpm_len) {
-			if (value != 0xFFFF)
-				value = 0;
-		} else {
-			value--;
-		}
-
-		if (value == 0xFFFF) {
-			value = MCI_GPM_INVALID;
-			more_gpm = MCI_GPM_NOMORE;
-		} else if (state_type == MCI_STATE_NEXT_GPM_OFFSET) {
-			if (gpm_ptr == mci->gpm_idx) {
-				value = MCI_GPM_INVALID;
-				more_gpm = MCI_GPM_NOMORE;
-			} else {
-				for (;;) {
-					u32 temp_index;
-
-					/* skip reserved GPM if any */
-
-					if (value != mci->gpm_idx)
-						more_gpm = MCI_GPM_MORE;
-					else
-						more_gpm = MCI_GPM_NOMORE;
-
-					temp_index = mci->gpm_idx;
-					mci->gpm_idx++;
-
-					if (mci->gpm_idx >=
-					    mci->gpm_len)
-						mci->gpm_idx = 0;
-
-					if (ar9003_mci_is_gpm_valid(ah,
-								    temp_index)) {
-						value = temp_index;
-						break;
-					}
-
-					if (more_gpm == MCI_GPM_NOMORE) {
-						value = MCI_GPM_INVALID;
-						break;
-					}
-				}
-			}
-			if (p_data)
-				*p_data = more_gpm;
-			}
-
-			if (value != MCI_GPM_INVALID)
-				value <<= 4;
-
-			break;
 	case MCI_STATE_LAST_SCHD_MSG_OFFSET:
 		value = MS(REG_READ(ah, AR_MCI_RX_STATUS),
 				    AR_MCI_RX_LAST_SCHD_MSG_INDEX);
@@ -1276,21 +1191,6 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
 			   AR_MCI_RX_REMOTE_SLEEP) ?
 			MCI_BT_SLEEP : MCI_BT_AWAKE;
 		break;
-	case MCI_STATE_CONT_RSSI_POWER:
-		value = MS(mci->cont_status, AR_MCI_CONT_RSSI_POWER);
-		break;
-	case MCI_STATE_CONT_PRIORITY:
-		value = MS(mci->cont_status, AR_MCI_CONT_RRIORITY);
-		break;
-	case MCI_STATE_CONT_TXRX:
-		value = MS(mci->cont_status, AR_MCI_CONT_TXRX);
-		break;
-	case MCI_STATE_BT:
-		value = mci->bt_state;
-		break;
-	case MCI_STATE_SET_BT_SLEEP:
-		mci->bt_state = MCI_BT_SLEEP;
-		break;
 	case MCI_STATE_SET_BT_AWAKE:
 		mci->bt_state = MCI_BT_AWAKE;
 		ar9003_mci_send_coex_version_query(ah, true);
@@ -1299,7 +1199,7 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
 		if (mci->unhalt_bt_gpm)
 			ar9003_mci_send_coex_halt_bt_gpm(ah, false, true);
 
-		ar9003_mci_2g5g_switch(ah, true);
+		ar9003_mci_2g5g_switch(ah, false);
 		break;
 	case MCI_STATE_SET_BT_CAL_START:
 		mci->bt_state = MCI_BT_CAL_START;
@@ -1323,34 +1223,6 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
 	case MCI_STATE_SEND_WLAN_COEX_VERSION:
 		ar9003_mci_send_coex_version_response(ah, true);
 		break;
-	case MCI_STATE_SET_BT_COEX_VERSION:
-		if (!p_data)
-			ath_dbg(common, MCI,
-				"MCI Set BT Coex version with NULL data!!\n");
-		else {
-			mci->bt_ver_major = (*p_data >> 8) & 0xff;
-			mci->bt_ver_minor = (*p_data) & 0xff;
-			mci->bt_version_known = true;
-			ath_dbg(common, MCI, "MCI BT version set: %d.%d\n",
-				mci->bt_ver_major, mci->bt_ver_minor);
-		}
-		break;
-	case MCI_STATE_SEND_WLAN_CHANNELS:
-		if (p_data) {
-			if (((mci->wlan_channels[1] & 0xffff0000) ==
-			     (*(p_data + 1) & 0xffff0000)) &&
-			    (mci->wlan_channels[2] == *(p_data + 2)) &&
-			    (mci->wlan_channels[3] == *(p_data + 3)))
-				break;
-
-			mci->wlan_channels[0] = *p_data++;
-			mci->wlan_channels[1] = *p_data++;
-			mci->wlan_channels[2] = *p_data++;
-			mci->wlan_channels[3] = *p_data++;
-		}
-		mci->wlan_channels_update = true;
-		ar9003_mci_send_coex_wlan_channels(ah, true);
-		break;
 	case MCI_STATE_SEND_VERSION_QUERY:
 		ar9003_mci_send_coex_version_query(ah, true);
 		break;
@@ -1358,38 +1230,16 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
 		query_type = MCI_GPM_COEX_QUERY_BT_TOPOLOGY;
 		ar9003_mci_send_coex_bt_status_query(ah, true, query_type);
 		break;
-	case MCI_STATE_NEED_FLUSH_BT_INFO:
-			/*
-			 * btcoex_hw.mci.unhalt_bt_gpm means whether it's
-			 * needed to send UNHALT message. It's set whenever
-			 * there's a request to send HALT message.
-			 * mci_halted_bt_gpm means whether HALT message is sent
-			 * out successfully.
-			 *
-			 * Checking (mci_unhalt_bt_gpm == false) instead of
-			 * checking (ah->mci_halted_bt_gpm == false) will make
-			 * sure currently is in UNHALT-ed mode and BT can
-			 * respond to status query.
-			 */
-			value = (!mci->unhalt_bt_gpm &&
-				 mci->need_flush_btinfo) ? 1 : 0;
-			if (p_data)
-				mci->need_flush_btinfo =
-					(*p_data != 0) ? true : false;
-			break;
 	case MCI_STATE_RECOVER_RX:
 		ar9003_mci_prep_interface(ah);
 		mci->query_bt = true;
 		mci->need_flush_btinfo = true;
 		ar9003_mci_send_coex_wlan_channels(ah, true);
-		ar9003_mci_2g5g_switch(ah, true);
+		ar9003_mci_2g5g_switch(ah, false);
 		break;
 	case MCI_STATE_NEED_FTP_STOMP:
 		value = !(mci->config & ATH_MCI_CONFIG_DISABLE_FTP_STOMP);
 		break;
-	case MCI_STATE_NEED_TUNING:
-		value = !(mci->config & ATH_MCI_CONFIG_DISABLE_TUNING);
-		break;
 	default:
 		break;
 	}
@@ -1397,3 +1247,173 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
 	return value;
 }
 EXPORT_SYMBOL(ar9003_mci_state);
+
+void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
+	ath_dbg(common, MCI, "Give LNA and SPDT control to BT\n");
+
+	ar9003_mci_send_lna_take(ah, true);
+	udelay(50);
+
+	REG_SET_BIT(ah, AR_PHY_GLB_CONTROL, AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
+	mci->is_2g = false;
+	mci->update_2g5g = true;
+	ar9003_mci_send_2g5g_status(ah, true);
+
+	/* Force another 2g5g update at next scanning */
+	mci->update_2g5g = true;
+}
+
+void ar9003_mci_set_power_awake(struct ath_hw *ah)
+{
+	u32 btcoex_ctrl2, diag_sw;
+	int i;
+	u8 lna_ctrl, bt_sleep;
+
+	for (i = 0; i < AH_WAIT_TIMEOUT; i++) {
+		btcoex_ctrl2 = REG_READ(ah, AR_BTCOEX_CTRL2);
+		if (btcoex_ctrl2 != 0xdeadbeef)
+			break;
+		udelay(AH_TIME_QUANTUM);
+	}
+	REG_WRITE(ah, AR_BTCOEX_CTRL2, (btcoex_ctrl2 | BIT(23)));
+
+	for (i = 0; i < AH_WAIT_TIMEOUT; i++) {
+		diag_sw = REG_READ(ah, AR_DIAG_SW);
+		if (diag_sw != 0xdeadbeef)
+			break;
+		udelay(AH_TIME_QUANTUM);
+	}
+	REG_WRITE(ah, AR_DIAG_SW, (diag_sw | BIT(27) | BIT(19) | BIT(18)));
+	lna_ctrl = REG_READ(ah, AR_OBS_BUS_CTRL) & 0x3;
+	bt_sleep = REG_READ(ah, AR_MCI_RX_STATUS) & AR_MCI_RX_REMOTE_SLEEP;
+
+	REG_WRITE(ah, AR_BTCOEX_CTRL2, btcoex_ctrl2);
+	REG_WRITE(ah, AR_DIAG_SW, diag_sw);
+
+	if (bt_sleep && (lna_ctrl == 2)) {
+		REG_SET_BIT(ah, AR_BTCOEX_RC, 0x1);
+		REG_CLR_BIT(ah, AR_BTCOEX_RC, 0x1);
+		udelay(50);
+	}
+}
+
+void ar9003_mci_check_gpm_offset(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+	u32 offset;
+
+	/*
+	 * This should only be called before "MAC Warm Reset" or "MCI Reset Rx".
+	 */
+	offset = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+	if (mci->gpm_idx == offset)
+		return;
+	ath_dbg(common, MCI, "GPM cached write pointer mismatch %d %d\n",
+		mci->gpm_idx, offset);
+	mci->query_bt = true;
+	mci->need_flush_btinfo = true;
+	mci->gpm_idx = 0;
+}
+
+u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, bool first, u32 *more)
+{
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+	u32 offset, more_gpm = 0, gpm_ptr;
+
+	if (first) {
+		gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+		mci->gpm_idx = gpm_ptr;
+		return gpm_ptr;
+	}
+
+	/*
+	 * This could be useful to avoid new GPM message interrupt which
+	 * may lead to spurious interrupt after power sleep, or multiple
+	 * entry of ath_mci_intr().
+	 * Adding empty GPM check by returning HAL_MCI_GPM_INVALID can
+	 * alleviate this effect, but clearing GPM RX interrupt bit is
+	 * safe, because whether this is called from hw or driver code
+	 * there must be an interrupt bit set/triggered initially
+	 */
+	REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
+			AR_MCI_INTERRUPT_RX_MSG_GPM);
+
+	gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+	offset = gpm_ptr;
+
+	if (!offset)
+		offset = mci->gpm_len - 1;
+	else if (offset >= mci->gpm_len) {
+		if (offset != 0xFFFF)
+			offset = 0;
+	} else {
+		offset--;
+	}
+
+	if ((offset == 0xFFFF) || (gpm_ptr == mci->gpm_idx)) {
+		offset = MCI_GPM_INVALID;
+		more_gpm = MCI_GPM_NOMORE;
+		goto out;
+	}
+	for (;;) {
+		u32 temp_index;
+
+		/* skip reserved GPM if any */
+
+		if (offset != mci->gpm_idx)
+			more_gpm = MCI_GPM_MORE;
+		else
+			more_gpm = MCI_GPM_NOMORE;
+
+		temp_index = mci->gpm_idx;
+		mci->gpm_idx++;
+
+		if (mci->gpm_idx >= mci->gpm_len)
+			mci->gpm_idx = 0;
+
+		if (ar9003_mci_is_gpm_valid(ah, temp_index)) {
+			offset = temp_index;
+			break;
+		}
+
+		if (more_gpm == MCI_GPM_NOMORE) {
+			offset = MCI_GPM_INVALID;
+			break;
+		}
+	}
+
+	if (offset != MCI_GPM_INVALID)
+		offset <<= 4;
+out:
+	if (more)
+		*more = more_gpm;
+
+	return offset;
+}
+EXPORT_SYMBOL(ar9003_mci_get_next_gpm_offset);
+
+void ar9003_mci_set_bt_version(struct ath_hw *ah, u8 major, u8 minor)
+{
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
+	mci->bt_ver_major = major;
+	mci->bt_ver_minor = minor;
+	mci->bt_version_known = true;
+	ath_dbg(ath9k_hw_common(ah), MCI, "MCI BT version set: %d.%d\n",
+		mci->bt_ver_major, mci->bt_ver_minor);
+}
+EXPORT_SYMBOL(ar9003_mci_set_bt_version);
+
+void ar9003_mci_send_wlan_channels(struct ath_hw *ah)
+{
+	struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
+	mci->wlan_channels_update = true;
+	ar9003_mci_send_coex_wlan_channels(ah, true);
+}
+EXPORT_SYMBOL(ar9003_mci_send_wlan_channels);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.h b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
index 4842f6c06b8..d33b8e12885 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
@@ -189,30 +189,18 @@ enum mci_bt_state {
 /* Type of state query */
 enum mci_state_type {
 	MCI_STATE_ENABLE,
-	MCI_STATE_INIT_GPM_OFFSET,
-	MCI_STATE_NEXT_GPM_OFFSET,
-	MCI_STATE_LAST_GPM_OFFSET,
-	MCI_STATE_BT,
-	MCI_STATE_SET_BT_SLEEP,
 	MCI_STATE_SET_BT_AWAKE,
 	MCI_STATE_SET_BT_CAL_START,
 	MCI_STATE_SET_BT_CAL,
 	MCI_STATE_LAST_SCHD_MSG_OFFSET,
 	MCI_STATE_REMOTE_SLEEP,
-	MCI_STATE_CONT_RSSI_POWER,
-	MCI_STATE_CONT_PRIORITY,
-	MCI_STATE_CONT_TXRX,
 	MCI_STATE_RESET_REQ_WAKE,
 	MCI_STATE_SEND_WLAN_COEX_VERSION,
-	MCI_STATE_SET_BT_COEX_VERSION,
-	MCI_STATE_SEND_WLAN_CHANNELS,
 	MCI_STATE_SEND_VERSION_QUERY,
 	MCI_STATE_SEND_STATUS_QUERY,
-	MCI_STATE_NEED_FLUSH_BT_INFO,
 	MCI_STATE_SET_CONCUR_TX_PRI,
 	MCI_STATE_RECOVER_RX,
 	MCI_STATE_NEED_FTP_STOMP,
-	MCI_STATE_NEED_TUNING,
 	MCI_STATE_DEBUG,
 	MCI_STATE_MAX
 };
@@ -260,28 +248,26 @@ enum mci_gpm_coex_opcode {
 bool ar9003_mci_send_message(struct ath_hw *ah, u8 header, u32 flag,
 			     u32 *payload, u8 len, bool wait_done,
 			     bool check_bt);
-u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data);
+u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type);
 void ar9003_mci_setup(struct ath_hw *ah, u32 gpm_addr, void *gpm_buf,
 		      u16 len, u32 sched_addr);
 void ar9003_mci_cleanup(struct ath_hw *ah);
 void ar9003_mci_get_interrupt(struct ath_hw *ah, u32 *raw_intr,
 			      u32 *rx_msg_intr);
-
+u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, bool first, u32 *more);
+void ar9003_mci_set_bt_version(struct ath_hw *ah, u8 major, u8 minor);
+void ar9003_mci_send_wlan_channels(struct ath_hw *ah);
 /*
  * These functions are used by ath9k_hw.
  */
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
 
-static inline bool ar9003_mci_is_ready(struct ath_hw *ah)
-{
-	return ah->btcoex_hw.mci.ready;
-}
 void ar9003_mci_stop_bt(struct ath_hw *ah, bool save_fullsleep);
 void ar9003_mci_init_cal_req(struct ath_hw *ah, bool *is_reusable);
 void ar9003_mci_init_cal_done(struct ath_hw *ah);
 void ar9003_mci_set_full_sleep(struct ath_hw *ah);
-void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool wait_done);
+void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool force);
 void ar9003_mci_check_bt(struct ath_hw *ah);
 bool ar9003_mci_start_reset(struct ath_hw *ah, struct ath9k_channel *chan);
 int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
@@ -289,13 +275,12 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 		      bool is_full_sleep);
 void ar9003_mci_get_isr(struct ath_hw *ah, enum ath9k_int *masked);
+void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah);
+void ar9003_mci_set_power_awake(struct ath_hw *ah);
+void ar9003_mci_check_gpm_offset(struct ath_hw *ah);
 
 #else
 
-static inline bool ar9003_mci_is_ready(struct ath_hw *ah)
-{
-	return false;
-}
 static inline void ar9003_mci_stop_bt(struct ath_hw *ah, bool save_fullsleep)
 {
 }
@@ -330,6 +315,15 @@ static inline void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 static inline void ar9003_mci_get_isr(struct ath_hw *ah, enum ath9k_int *masked)
 {
 }
+static inline void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah)
+{
+}
+static inline void ar9003_mci_set_power_awake(struct ath_hw *ah)
+{
+}
+static inline void ar9003_mci_check_gpm_offset(struct ath_hw *ah)
+{
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_paprd.c b/drivers/net/wireless/ath/ath9k/ar9003_paprd.c
index 3d400e8d653..2c9f7d7ed4c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_paprd.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_paprd.c
@@ -211,7 +211,7 @@ static int ar9003_paprd_setup_single_table(struct ath_hw *ah)
 		      AR_PHY_PAPRD_TRAINER_CNTL3_CF_PAPRD_NUM_CORR_STAGES, 7);
 	REG_RMW_FIELD(ah, AR_PHY_PAPRD_TRAINER_CNTL3,
 		      AR_PHY_PAPRD_TRAINER_CNTL3_CF_PAPRD_MIN_LOOPBACK_DEL, 1);
-	if (AR_SREV_9485(ah) || AR_SREV_9462(ah))
+	if (AR_SREV_9485(ah) || AR_SREV_9462(ah) || AR_SREV_9550(ah))
 		REG_RMW_FIELD(ah, AR_PHY_PAPRD_TRAINER_CNTL3,
 			      AR_PHY_PAPRD_TRAINER_CNTL3_CF_PAPRD_QUICK_DROP,
 			      -3);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index 11abb972be1..e476f9f92ce 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -99,7 +99,7 @@ static int ar9003_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 			channelSel = (freq * 4) / 120;
 			chan_frac = (((freq * 4) % 120) * 0x20000) / 120;
 			channelSel = (channelSel << 17) | chan_frac;
-		} else if (AR_SREV_9340(ah)) {
+		} else if (AR_SREV_9340(ah) || AR_SREV_9550(ah)) {
 			if (ah->is_clk_25mhz) {
 				u32 chan_frac;
 
@@ -113,11 +113,12 @@ static int ar9003_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 		/* Set to 2G mode */
 		bMode = 1;
 	} else {
-		if (AR_SREV_9340(ah) && ah->is_clk_25mhz) {
+		if ((AR_SREV_9340(ah) || AR_SREV_9550(ah)) &&
+		    ah->is_clk_25mhz) {
 			u32 chan_frac;
 
-			channelSel = (freq * 2) / 75;
-			chan_frac = (((freq * 2) % 75) * 0x20000) / 75;
+			channelSel = freq / 75;
+			chan_frac = ((freq % 75) * 0x20000) / 75;
 			channelSel = (channelSel << 17) | chan_frac;
 		} else {
 			channelSel = CHANSEL_5G(freq);
@@ -173,16 +174,15 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
 	int cur_bb_spur, negative = 0, cck_spur_freq;
 	int i;
 	int range, max_spur_cnts, synth_freq;
-	u8 *spur_fbin_ptr = NULL;
+	u8 *spur_fbin_ptr = ar9003_get_spur_chan_ptr(ah, IS_CHAN_2GHZ(chan));
 
 	/*
 	 * Need to verify range +/- 10 MHz in control channel, otherwise spur
 	 * is out-of-band and can be ignored.
 	 */
 
-	if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah)) {
-		spur_fbin_ptr = ar9003_get_spur_chan_ptr(ah,
-							 IS_CHAN_2GHZ(chan));
+	if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah) ||
+	    AR_SREV_9550(ah)) {
 		if (spur_fbin_ptr[0] == 0) /* No spur */
 			return;
 		max_spur_cnts = 5;
@@ -207,7 +207,8 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
 		if (AR_SREV_9462(ah) && (i == 0 || i == 3))
 			continue;
 		negative = 0;
-		if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah))
+		if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah) ||
+		    AR_SREV_9550(ah))
 			cur_bb_spur = ath9k_hw_fbin2freq(spur_fbin_ptr[i],
 							 IS_CHAN_2GHZ(chan));
 		else
@@ -620,6 +621,50 @@ static void ar9003_hw_prog_ini(struct ath_hw *ah,
 	}
 }
 
+static int ar9550_hw_get_modes_txgain_index(struct ath_hw *ah,
+					    struct ath9k_channel *chan)
+{
+	int ret;
+
+	switch (chan->chanmode) {
+	case CHANNEL_A:
+	case CHANNEL_A_HT20:
+		if (chan->channel <= 5350)
+			ret = 1;
+		else if ((chan->channel > 5350) && (chan->channel <= 5600))
+			ret = 3;
+		else
+			ret = 5;
+		break;
+
+	case CHANNEL_A_HT40PLUS:
+	case CHANNEL_A_HT40MINUS:
+		if (chan->channel <= 5350)
+			ret = 2;
+		else if ((chan->channel > 5350) && (chan->channel <= 5600))
+			ret = 4;
+		else
+			ret = 6;
+		break;
+
+	case CHANNEL_G:
+	case CHANNEL_G_HT20:
+	case CHANNEL_B:
+		ret = 8;
+		break;
+
+	case CHANNEL_G_HT40PLUS:
+	case CHANNEL_G_HT40MINUS:
+		ret = 7;
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
 static int ar9003_hw_process_ini(struct ath_hw *ah,
 				 struct ath9k_channel *chan)
 {
@@ -661,7 +706,22 @@ static int ar9003_hw_process_ini(struct ath_hw *ah,
 	}
 
 	REG_WRITE_ARRAY(&ah->iniModesRxGain, 1, regWrites);
-	REG_WRITE_ARRAY(&ah->iniModesTxGain, modesIndex, regWrites);
+	if (AR_SREV_9550(ah))
+		REG_WRITE_ARRAY(&ah->ini_modes_rx_gain_bounds, modesIndex,
+				regWrites);
+
+	if (AR_SREV_9550(ah)) {
+		int modes_txgain_index;
+
+		modes_txgain_index = ar9550_hw_get_modes_txgain_index(ah, chan);
+		if (modes_txgain_index < 0)
+			return -EINVAL;
+
+		REG_WRITE_ARRAY(&ah->iniModesTxGain, modes_txgain_index,
+				regWrites);
+	} else {
+		REG_WRITE_ARRAY(&ah->iniModesTxGain, modesIndex, regWrites);
+	}
 
 	/*
 	 * For 5GHz channels requiring Fast Clock, apply
@@ -676,6 +736,10 @@ static int ar9003_hw_process_ini(struct ath_hw *ah,
 	if (chan->channel == 2484)
 		ar9003_hw_prog_ini(ah, &ah->ini_japan2484, 1);
 
+	if (AR_SREV_9462(ah))
+		REG_WRITE(ah, AR_GLB_SWREG_DISCONT_MODE,
+			  AR_GLB_SWREG_DISCONT_EN_BT_WLAN);
+
 	ah->modes_index = modesIndex;
 	ar9003_hw_override_ini(ah);
 	ar9003_hw_set_channel_regs(ah, chan);
@@ -821,18 +885,18 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 			REG_CLR_BIT(ah, AR_PHY_SFCORR_LOW,
 				    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
 
-		if (!on != aniState->ofdmWeakSigDetectOff) {
+		if (on != aniState->ofdmWeakSigDetect) {
 			ath_dbg(common, ANI,
 				"** ch %d: ofdm weak signal: %s=>%s\n",
 				chan->channel,
-				!aniState->ofdmWeakSigDetectOff ?
+				aniState->ofdmWeakSigDetect ?
 				"on" : "off",
 				on ? "on" : "off");
 			if (on)
 				ah->stats.ast_ani_ofdmon++;
 			else
 				ah->stats.ast_ani_ofdmoff++;
-			aniState->ofdmWeakSigDetectOff = !on;
+			aniState->ofdmWeakSigDetect = on;
 		}
 		break;
 	}
@@ -851,7 +915,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value = firstep_table[level] -
-			firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+			firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
 			aniState->iniDef.firstep;
 		if (value < ATH9K_SIG_FIRSTEP_SETTING_MIN)
 			value = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -866,7 +930,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value2 = firstep_table[level] -
-			 firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+			 firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
 			 aniState->iniDef.firstepLow;
 		if (value2 < ATH9K_SIG_FIRSTEP_SETTING_MIN)
 			value2 = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -882,7 +946,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 				chan->channel,
 				aniState->firstepLevel,
 				level,
-				ATH9K_ANI_FIRSTEP_LVL_NEW,
+				ATH9K_ANI_FIRSTEP_LVL,
 				value,
 				aniState->iniDef.firstep);
 			ath_dbg(common, ANI,
@@ -890,7 +954,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 				chan->channel,
 				aniState->firstepLevel,
 				level,
-				ATH9K_ANI_FIRSTEP_LVL_NEW,
+				ATH9K_ANI_FIRSTEP_LVL,
 				value2,
 				aniState->iniDef.firstepLow);
 			if (level > aniState->firstepLevel)
@@ -915,7 +979,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value = cycpwrThr1_table[level] -
-			cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+			cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
 			aniState->iniDef.cycpwrThr1;
 		if (value < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
 			value = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -931,7 +995,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 		 * from INI file & cap value
 		 */
 		value2 = cycpwrThr1_table[level] -
-			 cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+			 cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
 			 aniState->iniDef.cycpwrThr1Ext;
 		if (value2 < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
 			value2 = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -946,7 +1010,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 				chan->channel,
 				aniState->spurImmunityLevel,
 				level,
-				ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+				ATH9K_ANI_SPUR_IMMUNE_LVL,
 				value,
 				aniState->iniDef.cycpwrThr1);
 			ath_dbg(common, ANI,
@@ -954,7 +1018,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 				chan->channel,
 				aniState->spurImmunityLevel,
 				level,
-				ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+				ATH9K_ANI_SPUR_IMMUNE_LVL,
 				value2,
 				aniState->iniDef.cycpwrThr1Ext);
 			if (level > aniState->spurImmunityLevel)
@@ -975,16 +1039,16 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 			      AR_PHY_MRC_CCK_ENABLE, is_on);
 		REG_RMW_FIELD(ah, AR_PHY_MRC_CCK_CTRL,
 			      AR_PHY_MRC_CCK_MUX_REG, is_on);
-		if (!is_on != aniState->mrcCCKOff) {
+		if (is_on != aniState->mrcCCK) {
 			ath_dbg(common, ANI, "** ch %d: MRC CCK: %s=>%s\n",
 				chan->channel,
-				!aniState->mrcCCKOff ? "on" : "off",
+				aniState->mrcCCK ? "on" : "off",
 				is_on ? "on" : "off");
 		if (is_on)
 			ah->stats.ast_ani_ccklow++;
 		else
 			ah->stats.ast_ani_cckhigh++;
-		aniState->mrcCCKOff = !is_on;
+		aniState->mrcCCK = is_on;
 		}
 	break;
 	}
@@ -998,9 +1062,9 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
 	ath_dbg(common, ANI,
 		"ANI parameters: SI=%d, ofdmWS=%s FS=%d MRCcck=%s listenTime=%d ofdmErrs=%d cckErrs=%d\n",
 		aniState->spurImmunityLevel,
-		!aniState->ofdmWeakSigDetectOff ? "on" : "off",
+		aniState->ofdmWeakSigDetect ? "on" : "off",
 		aniState->firstepLevel,
-		!aniState->mrcCCKOff ? "on" : "off",
+		aniState->mrcCCK ? "on" : "off",
 		aniState->listenTime,
 		aniState->ofdmPhyErrCount,
 		aniState->cckPhyErrCount);
@@ -1107,10 +1171,10 @@ static void ar9003_hw_ani_cache_ini_regs(struct ath_hw *ah)
 					       AR_PHY_EXT_CYCPWR_THR1);
 
 	/* these levels just got reset to defaults by the INI */
-	aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL_NEW;
-	aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_NEW;
-	aniState->ofdmWeakSigDetectOff = !ATH9K_ANI_USE_OFDM_WEAK_SIG;
-	aniState->mrcCCKOff = !ATH9K_ANI_ENABLE_MRC_CCK;
+	aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
+	aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
+	aniState->ofdmWeakSigDetect = ATH9K_ANI_USE_OFDM_WEAK_SIG;
+	aniState->mrcCCK = true;
 }
 
 static void ar9003_hw_set_radar_params(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 7268a48a92a..7bfbaf065a4 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -633,11 +633,13 @@
 #define AR_PHY_65NM_CH0_BIAS2       0x160c4
 #define AR_PHY_65NM_CH0_BIAS4       0x160cc
 #define AR_PHY_65NM_CH0_RXTX4       0x1610c
+#define AR_PHY_65NM_CH1_RXTX4       0x1650c
+#define AR_PHY_65NM_CH2_RXTX4       0x1690c
 
 #define AR_CH0_TOP	(AR_SREV_9300(ah) ? 0x16288 : \
 				((AR_SREV_9462(ah) ? 0x1628c : 0x16280)))
-#define AR_CH0_TOP_XPABIASLVL (0x300)
-#define AR_CH0_TOP_XPABIASLVL_S (8)
+#define AR_CH0_TOP_XPABIASLVL (AR_SREV_9550(ah) ? 0x3c0 : 0x300)
+#define AR_CH0_TOP_XPABIASLVL_S (AR_SREV_9550(ah) ? 6 : 8)
 
 #define AR_CH0_THERM	(AR_SREV_9300(ah) ? 0x16290 : \
 				((AR_SREV_9485(ah) ? 0x1628c : 0x16294)))
@@ -650,6 +652,8 @@
 #define AR_SWITCH_TABLE_COM_ALL_S (0)
 #define AR_SWITCH_TABLE_COM_AR9462_ALL (0xffffff)
 #define AR_SWITCH_TABLE_COM_AR9462_ALL_S (0)
+#define AR_SWITCH_TABLE_COM_AR9550_ALL (0xffffff)
+#define AR_SWITCH_TABLE_COM_AR9550_ALL_S (0)
 #define AR_SWITCH_TABLE_COM_SPDT (0x00f00000)
 #define AR_SWITCH_TABLE_COM_SPDT_ALL (0x0000fff0)
 #define AR_SWITCH_TABLE_COM_SPDT_ALL_S (4)
@@ -820,18 +824,26 @@
 #define AR_PHY_CHAN_INFO_MEMORY_CAPTURE_MASK         0x0001
 #define AR_PHY_RX_DELAY_DELAY   0x00003FFF
 #define AR_PHY_CCK_TX_CTRL_JAPAN    0x00000010
-#define AR_PHY_SPECTRAL_SCAN_ENABLE         0x00000001
-#define AR_PHY_SPECTRAL_SCAN_ENABLE_S       0
-#define AR_PHY_SPECTRAL_SCAN_ACTIVE         0x00000002
-#define AR_PHY_SPECTRAL_SCAN_ACTIVE_S       1
-#define AR_PHY_SPECTRAL_SCAN_FFT_PERIOD     0x000000F0
-#define AR_PHY_SPECTRAL_SCAN_FFT_PERIOD_S   4
-#define AR_PHY_SPECTRAL_SCAN_PERIOD         0x0000FF00
-#define AR_PHY_SPECTRAL_SCAN_PERIOD_S       8
-#define AR_PHY_SPECTRAL_SCAN_COUNT          0x00FF0000
-#define AR_PHY_SPECTRAL_SCAN_COUNT_S        16
-#define AR_PHY_SPECTRAL_SCAN_SHORT_REPEAT   0x01000000
-#define AR_PHY_SPECTRAL_SCAN_SHORT_REPEAT_S 24
+
+#define AR_PHY_SPECTRAL_SCAN_ENABLE           0x00000001
+#define AR_PHY_SPECTRAL_SCAN_ENABLE_S         0
+#define AR_PHY_SPECTRAL_SCAN_ACTIVE           0x00000002
+#define AR_PHY_SPECTRAL_SCAN_ACTIVE_S         1
+#define AR_PHY_SPECTRAL_SCAN_FFT_PERIOD       0x000000F0
+#define AR_PHY_SPECTRAL_SCAN_FFT_PERIOD_S     4
+#define AR_PHY_SPECTRAL_SCAN_PERIOD           0x0000FF00
+#define AR_PHY_SPECTRAL_SCAN_PERIOD_S         8
+#define AR_PHY_SPECTRAL_SCAN_COUNT            0x0FFF0000
+#define AR_PHY_SPECTRAL_SCAN_COUNT_S          16
+#define AR_PHY_SPECTRAL_SCAN_SHORT_REPEAT     0x10000000
+#define AR_PHY_SPECTRAL_SCAN_SHORT_REPEAT_S   28
+#define AR_PHY_SPECTRAL_SCAN_PRIORITY         0x20000000
+#define AR_PHY_SPECTRAL_SCAN_PRIORITY_S       29
+#define AR_PHY_SPECTRAL_SCAN_USE_ERR5         0x40000000
+#define AR_PHY_SPECTRAL_SCAN_USE_ERR5_S       30
+#define AR_PHY_SPECTRAL_SCAN_COMPRESSED_RPT   0x80000000
+#define AR_PHY_SPECTRAL_SCAN_COMPRESSED_RPT_S 31
+
 #define AR_PHY_CHANNEL_STATUS_RX_CLEAR      0x00000004
 #define AR_PHY_RTT_CTRL_ENA_RADIO_RETENTION     0x00000001
 #define AR_PHY_RTT_CTRL_ENA_RADIO_RETENTION_S   0
@@ -866,6 +878,9 @@
 #define AR_PHY_65NM_CH0_RXTX4_THERM_ON          0x10000000
 #define AR_PHY_65NM_CH0_RXTX4_THERM_ON_S        28
 
+#define AR_PHY_65NM_RXTX4_XLNA_BIAS		0xC0000000
+#define AR_PHY_65NM_RXTX4_XLNA_BIAS_S		30
+
 /*
  * Channel 1 Register Map
  */
diff --git a/drivers/net/wireless/ath/ath9k/ar9330_1p1_initvals.h b/drivers/net/wireless/ath/ath9k/ar9330_1p1_initvals.h
index 1bd3a3d2210..6e1756bc383 100644
--- a/drivers/net/wireless/ath/ath9k/ar9330_1p1_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9330_1p1_initvals.h
@@ -337,12 +337,7 @@ static const u32 ar9331_modes_low_ob_db_tx_gain_1p1[][5] = {
 	{0x00016284, 0x14d3f000, 0x14d3f000, 0x14d3f000, 0x14d3f000},
 };
 
-static const u32 ar9331_1p1_baseband_core_txfir_coeff_japan_2484[][2] = {
-	/* Addr      allmodes  */
-	{0x0000a398, 0x00000000},
-	{0x0000a39c, 0x6f7f0301},
-	{0x0000a3a0, 0xca9228ee},
-};
+#define ar9331_1p1_baseband_core_txfir_coeff_japan_2484 ar9462_2p0_baseband_core_txfir_coeff_japan_2484
 
 static const u32 ar9331_1p1_xtal_25M[][2] = {
 	/* Addr      allmodes  */
@@ -783,17 +778,7 @@ static const u32 ar9331_modes_high_power_tx_gain_1p1[][5] = {
 	{0x00016284, 0x14d3f000, 0x14d3f000, 0x14d3f000, 0x14d3f000},
 };
 
-static const u32 ar9331_1p1_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},
-};
+#define ar9331_1p1_mac_postamble ar9300_2p2_mac_postamble
 
 static const u32 ar9331_1p1_soc_preamble[][2] = {
 	/* Addr      allmodes  */
@@ -1112,38 +1097,4 @@ static const u32 ar9331_common_tx_gain_offset1_1[][1] = {
 	{0x00000000},
 };
 
-static const u32 ar9331_1p1_chansel_xtal_25M[] = {
-	0x0101479e,
-	0x0101d027,
-	0x010258af,
-	0x0102e138,
-	0x010369c0,
-	0x0103f249,
-	0x01047ad1,
-	0x0105035a,
-	0x01058be2,
-	0x0106146b,
-	0x01069cf3,
-	0x0107257c,
-	0x0107ae04,
-	0x0108f5b2,
-};
-
-static const u32 ar9331_1p1_chansel_xtal_40M[] = {
-	0x00a0ccbe,
-	0x00a12213,
-	0x00a17769,
-	0x00a1ccbe,
-	0x00a22213,
-	0x00a27769,
-	0x00a2ccbe,
-	0x00a32213,
-	0x00a37769,
-	0x00a3ccbe,
-	0x00a42213,
-	0x00a47769,
-	0x00a4ccbe,
-	0x00a5998b,
-};
-
 #endif /* INITVALS_9330_1P1_H */
diff --git a/drivers/net/wireless/ath/ath9k/ar9330_1p2_initvals.h b/drivers/net/wireless/ath/ath9k/ar9330_1p2_initvals.h
index 0e6ca0834b3..57ed8a11217 100644
--- a/drivers/net/wireless/ath/ath9k/ar9330_1p2_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9330_1p2_initvals.h
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2011 Atheros Communications Inc.
+ * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -17,8 +18,8 @@
 #ifndef INITVALS_9330_1P2_H
 #define INITVALS_9330_1P2_H
 
-static const u32 ar9331_modes_lowest_ob_db_tx_gain_1p2[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+static const u32 ar9331_modes_high_ob_db_tx_gain_1p2[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x0000a410, 0x000050d7, 0x000050d7, 0x000050d7, 0x000050d7},
 	{0x0000a500, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
 	{0x0000a504, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
@@ -102,8 +103,14 @@ static const u32 ar9331_modes_lowest_ob_db_tx_gain_1p2[][5] = {
 	{0x0000a63c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
 };
 
+#define ar9331_modes_high_power_tx_gain_1p2 ar9331_modes_high_ob_db_tx_gain_1p2
+
+#define ar9331_modes_low_ob_db_tx_gain_1p2 ar9331_modes_high_power_tx_gain_1p2
+
+#define ar9331_modes_lowest_ob_db_tx_gain_1p2 ar9331_modes_low_ob_db_tx_gain_1p2
+
 static const u32 ar9331_1p2_baseband_postamble[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8005, 0xd00a8005},
 	{0x00009820, 0x206a002e, 0x206a002e, 0x206a002e, 0x206a002e},
 	{0x00009824, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0},
@@ -147,191 +154,6 @@ static const u32 ar9331_1p2_baseband_postamble[][5] = {
 	{0x0000ae18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 };
 
-static const u32 ar9331_modes_high_ob_db_tx_gain_1p2[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
-	{0x0000a410, 0x000050d7, 0x000050d7, 0x000050d7, 0x000050d7},
-	{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, 0x23000a00, 0x23000a00},
-	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x27000a02, 0x27000a02},
-	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2b000a04, 0x2b000a04},
-	{0x0000a52c, 0x41023e85, 0x41023e85, 0x3f001620, 0x3f001620},
-	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x41001621, 0x41001621},
-	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x44001640, 0x44001640},
-	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x46001641, 0x46001641},
-	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x48001642, 0x48001642},
-	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x4b001644, 0x4b001644},
-	{0x0000a544, 0x6502feca, 0x6502feca, 0x4e001a81, 0x4e001a81},
-	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x51001a83, 0x51001a83},
-	{0x0000a54c, 0x7203feca, 0x7203feca, 0x54001c84, 0x54001c84},
-	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x57001ce3, 0x57001ce3},
-	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x5b001ce5, 0x5b001ce5},
-	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5f001ce9, 0x5f001ce9},
-	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x66001eec, 0x66001eec},
-	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x66001eec, 0x66001eec},
-	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a580, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
-	{0x0000a584, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
-	{0x0000a588, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
-	{0x0000a58c, 0x11062202, 0x11062202, 0x0b000200, 0x0b000200},
-	{0x0000a590, 0x17022e00, 0x17022e00, 0x0f000202, 0x0f000202},
-	{0x0000a594, 0x1d000ec2, 0x1d000ec2, 0x11000400, 0x11000400},
-	{0x0000a598, 0x25020ec0, 0x25020ec0, 0x15000402, 0x15000402},
-	{0x0000a59c, 0x2b020ec3, 0x2b020ec3, 0x19000404, 0x19000404},
-	{0x0000a5a0, 0x2f001f04, 0x2f001f04, 0x1b000603, 0x1b000603},
-	{0x0000a5a4, 0x35001fc4, 0x35001fc4, 0x1f000a02, 0x1f000a02},
-	{0x0000a5a8, 0x3c022f04, 0x3c022f04, 0x23000a04, 0x23000a04},
-	{0x0000a5ac, 0x41023e85, 0x41023e85, 0x26000a20, 0x26000a20},
-	{0x0000a5b0, 0x48023ec6, 0x48023ec6, 0x2a000e20, 0x2a000e20},
-	{0x0000a5b4, 0x4d023f01, 0x4d023f01, 0x2e000e22, 0x2e000e22},
-	{0x0000a5b8, 0x53023f4b, 0x53023f4b, 0x31000e24, 0x31000e24},
-	{0x0000a5bc, 0x5a027f09, 0x5a027f09, 0x34001640, 0x34001640},
-	{0x0000a5c0, 0x5f027fc9, 0x5f027fc9, 0x38001660, 0x38001660},
-	{0x0000a5c4, 0x6502feca, 0x6502feca, 0x3b001861, 0x3b001861},
-	{0x0000a5c8, 0x6b02ff4a, 0x6b02ff4a, 0x3e001a81, 0x3e001a81},
-	{0x0000a5cc, 0x7203feca, 0x7203feca, 0x42001a83, 0x42001a83},
-	{0x0000a5d0, 0x7703ff0b, 0x7703ff0b, 0x44001c84, 0x44001c84},
-	{0x0000a5d4, 0x7d06ffcb, 0x7d06ffcb, 0x48001ce3, 0x48001ce3},
-	{0x0000a5d8, 0x8407ff0b, 0x8407ff0b, 0x4c001ce5, 0x4c001ce5},
-	{0x0000a5dc, 0x8907ffcb, 0x8907ffcb, 0x50001ce9, 0x50001ce9},
-	{0x0000a5e0, 0x900fff0b, 0x900fff0b, 0x54001ceb, 0x54001ceb},
-	{0x0000a5e4, 0x960fffcb, 0x960fffcb, 0x56001eec, 0x56001eec},
-	{0x0000a5e8, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5ec, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f0, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f4, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f8, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5fc, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
-	{0x0000a618, 0x02008501, 0x02008501, 0x02008501, 0x02008501},
-	{0x0000a61c, 0x02008802, 0x02008802, 0x02008802, 0x02008802},
-	{0x0000a620, 0x0300c802, 0x0300c802, 0x0300c802, 0x0300c802},
-	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x0300cc03, 0x0300cc03},
-	{0x0000a628, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a62c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a630, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a634, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a638, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a63c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-};
-
-static const u32 ar9331_modes_low_ob_db_tx_gain_1p2[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
-	{0x0000a410, 0x000050d7, 0x000050d7, 0x000050d7, 0x000050d7},
-	{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, 0x23000a00, 0x23000a00},
-	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x27000a02, 0x27000a02},
-	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2b000a04, 0x2b000a04},
-	{0x0000a52c, 0x41023e85, 0x41023e85, 0x3f001620, 0x3f001620},
-	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x41001621, 0x41001621},
-	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x44001640, 0x44001640},
-	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x46001641, 0x46001641},
-	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x48001642, 0x48001642},
-	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x4b001644, 0x4b001644},
-	{0x0000a544, 0x6502feca, 0x6502feca, 0x4e001a81, 0x4e001a81},
-	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x51001a83, 0x51001a83},
-	{0x0000a54c, 0x7203feca, 0x7203feca, 0x54001c84, 0x54001c84},
-	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x57001ce3, 0x57001ce3},
-	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x5b001ce5, 0x5b001ce5},
-	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5f001ce9, 0x5f001ce9},
-	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x66001eec, 0x66001eec},
-	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x66001eec, 0x66001eec},
-	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a580, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
-	{0x0000a584, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
-	{0x0000a588, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
-	{0x0000a58c, 0x11062202, 0x11062202, 0x0b000200, 0x0b000200},
-	{0x0000a590, 0x17022e00, 0x17022e00, 0x0f000202, 0x0f000202},
-	{0x0000a594, 0x1d000ec2, 0x1d000ec2, 0x11000400, 0x11000400},
-	{0x0000a598, 0x25020ec0, 0x25020ec0, 0x15000402, 0x15000402},
-	{0x0000a59c, 0x2b020ec3, 0x2b020ec3, 0x19000404, 0x19000404},
-	{0x0000a5a0, 0x2f001f04, 0x2f001f04, 0x1b000603, 0x1b000603},
-	{0x0000a5a4, 0x35001fc4, 0x35001fc4, 0x1f000a02, 0x1f000a02},
-	{0x0000a5a8, 0x3c022f04, 0x3c022f04, 0x23000a04, 0x23000a04},
-	{0x0000a5ac, 0x41023e85, 0x41023e85, 0x26000a20, 0x26000a20},
-	{0x0000a5b0, 0x48023ec6, 0x48023ec6, 0x2a000e20, 0x2a000e20},
-	{0x0000a5b4, 0x4d023f01, 0x4d023f01, 0x2e000e22, 0x2e000e22},
-	{0x0000a5b8, 0x53023f4b, 0x53023f4b, 0x31000e24, 0x31000e24},
-	{0x0000a5bc, 0x5a027f09, 0x5a027f09, 0x34001640, 0x34001640},
-	{0x0000a5c0, 0x5f027fc9, 0x5f027fc9, 0x38001660, 0x38001660},
-	{0x0000a5c4, 0x6502feca, 0x6502feca, 0x3b001861, 0x3b001861},
-	{0x0000a5c8, 0x6b02ff4a, 0x6b02ff4a, 0x3e001a81, 0x3e001a81},
-	{0x0000a5cc, 0x7203feca, 0x7203feca, 0x42001a83, 0x42001a83},
-	{0x0000a5d0, 0x7703ff0b, 0x7703ff0b, 0x44001c84, 0x44001c84},
-	{0x0000a5d4, 0x7d06ffcb, 0x7d06ffcb, 0x48001ce3, 0x48001ce3},
-	{0x0000a5d8, 0x8407ff0b, 0x8407ff0b, 0x4c001ce5, 0x4c001ce5},
-	{0x0000a5dc, 0x8907ffcb, 0x8907ffcb, 0x50001ce9, 0x50001ce9},
-	{0x0000a5e0, 0x900fff0b, 0x900fff0b, 0x54001ceb, 0x54001ceb},
-	{0x0000a5e4, 0x960fffcb, 0x960fffcb, 0x56001eec, 0x56001eec},
-	{0x0000a5e8, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5ec, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f0, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f4, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f8, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5fc, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
-	{0x0000a618, 0x02008501, 0x02008501, 0x02008501, 0x02008501},
-	{0x0000a61c, 0x02008802, 0x02008802, 0x02008802, 0x02008802},
-	{0x0000a620, 0x0300c802, 0x0300c802, 0x0300c802, 0x0300c802},
-	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x0300cc03, 0x0300cc03},
-	{0x0000a628, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a62c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a630, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a634, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a638, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a63c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-};
-
-static const u32 ar9331_1p2_baseband_core_txfir_coeff_japan_2484[][2] = {
-	/* Addr      allmodes  */
-	{0x0000a398, 0x00000000},
-	{0x0000a39c, 0x6f7f0301},
-	{0x0000a3a0, 0xca9228ee},
-};
-
-static const u32 ar9331_1p2_xtal_25M[][2] = {
-	/* Addr      allmodes  */
-	{0x00007038, 0x000002f8},
-	{0x00008244, 0x0010f3d7},
-	{0x0000824c, 0x0001e7ae},
-	{0x0001609c, 0x0f508f29},
-};
-
 static const u32 ar9331_1p2_radio_core[][2] = {
 	/* Addr      allmodes  */
 	{0x00016000, 0x36db6db6},
@@ -397,684 +219,24 @@ static const u32 ar9331_1p2_radio_core[][2] = {
 	{0x000163d4, 0x00000000},
 };
 
-static const u32 ar9331_1p2_soc_postamble[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
-	{0x00007010, 0x00000022, 0x00000022, 0x00000022, 0x00000022},
-};
+#define ar9331_1p2_baseband_core_txfir_coeff_japan_2484 ar9331_1p1_baseband_core_txfir_coeff_japan_2484
 
-static const u32 ar9331_common_wo_xlna_rx_gain_1p2[][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},
-};
+#define ar9331_1p2_xtal_25M ar9331_1p1_xtal_25M
 
-static const u32 ar9331_1p2_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, 0x32840bbe},
-	{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},
-	{0x00009e08, 0x0038233c},
-	{0x00009e24, 0x9927b515},
-	{0x00009e28, 0x12ef0200},
-	{0x00009e30, 0x06336f77},
-	{0x00009e34, 0x6af6532f},
-	{0x00009e38, 0x0cc80c00},
-	{0x00009e40, 0x0d261820},
-	{0x00009e4c, 0x00001004},
-	{0x00009e50, 0x00ff03f1},
-	{0x00009fc0, 0x803e4788},
-	{0x00009fc4, 0x0001efb5},
-	{0x00009fcc, 0x40000014},
-	{0x0000a20c, 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, 0x00000000},
-	{0x0000a3a8, 0xaaaaaaaa},
-	{0x0000a3ac, 0x3c466478},
-	{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, 0x1ce739ce},
-	{0x0000a418, 0x2d001dce},
-	{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},
-	{0x0000a458, 0x00000000},
-	{0x0000a640, 0x00000000},
-	{0x0000a644, 0x3fad9d74},
-	{0x0000a648, 0x0048060a},
-	{0x0000a64c, 0x00003c37},
-	{0x0000a670, 0x03020100},
-	{0x0000a674, 0x09080504},
-	{0x0000a678, 0x0d0c0b0a},
-	{0x0000a67c, 0x13121110},
-	{0x0000a680, 0x31301514},
-	{0x0000a684, 0x35343332},
-	{0x0000a688, 0x00000036},
-	{0x0000a690, 0x00000838},
-	{0x0000a7c0, 0x00000000},
-	{0x0000a7c4, 0xfffffffc},
-	{0x0000a7c8, 0x00000000},
-	{0x0000a7cc, 0x00000000},
-	{0x0000a7d0, 0x00000000},
-	{0x0000a7d4, 0x00000004},
-	{0x0000a7dc, 0x00000001},
-};
+#define ar9331_1p2_xtal_40M ar9331_1p1_xtal_40M
 
-static const u32 ar9331_modes_high_power_tx_gain_1p2[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
-	{0x0000a410, 0x000050d7, 0x000050d7, 0x000050d7, 0x000050d7},
-	{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, 0x23000a00, 0x23000a00},
-	{0x0000a524, 0x35001fc4, 0x35001fc4, 0x27000a02, 0x27000a02},
-	{0x0000a528, 0x3c022f04, 0x3c022f04, 0x2b000a04, 0x2b000a04},
-	{0x0000a52c, 0x41023e85, 0x41023e85, 0x3f001620, 0x3f001620},
-	{0x0000a530, 0x48023ec6, 0x48023ec6, 0x41001621, 0x41001621},
-	{0x0000a534, 0x4d023f01, 0x4d023f01, 0x44001640, 0x44001640},
-	{0x0000a538, 0x53023f4b, 0x53023f4b, 0x46001641, 0x46001641},
-	{0x0000a53c, 0x5a027f09, 0x5a027f09, 0x48001642, 0x48001642},
-	{0x0000a540, 0x5f027fc9, 0x5f027fc9, 0x4b001644, 0x4b001644},
-	{0x0000a544, 0x6502feca, 0x6502feca, 0x4e001a81, 0x4e001a81},
-	{0x0000a548, 0x6b02ff4a, 0x6b02ff4a, 0x51001a83, 0x51001a83},
-	{0x0000a54c, 0x7203feca, 0x7203feca, 0x54001c84, 0x54001c84},
-	{0x0000a550, 0x7703ff0b, 0x7703ff0b, 0x57001ce3, 0x57001ce3},
-	{0x0000a554, 0x7d06ffcb, 0x7d06ffcb, 0x5b001ce5, 0x5b001ce5},
-	{0x0000a558, 0x8407ff0b, 0x8407ff0b, 0x5f001ce9, 0x5f001ce9},
-	{0x0000a55c, 0x8907ffcb, 0x8907ffcb, 0x66001eec, 0x66001eec},
-	{0x0000a560, 0x900fff0b, 0x900fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a564, 0x960fffcb, 0x960fffcb, 0x66001eec, 0x66001eec},
-	{0x0000a568, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a56c, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a570, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a574, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a578, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a57c, 0x9c1fff0b, 0x9c1fff0b, 0x66001eec, 0x66001eec},
-	{0x0000a580, 0x00022200, 0x00022200, 0x00000000, 0x00000000},
-	{0x0000a584, 0x05062002, 0x05062002, 0x04000002, 0x04000002},
-	{0x0000a588, 0x0c002e00, 0x0c002e00, 0x08000004, 0x08000004},
-	{0x0000a58c, 0x11062202, 0x11062202, 0x0b000200, 0x0b000200},
-	{0x0000a590, 0x17022e00, 0x17022e00, 0x0f000202, 0x0f000202},
-	{0x0000a594, 0x1d000ec2, 0x1d000ec2, 0x11000400, 0x11000400},
-	{0x0000a598, 0x25020ec0, 0x25020ec0, 0x15000402, 0x15000402},
-	{0x0000a59c, 0x2b020ec3, 0x2b020ec3, 0x19000404, 0x19000404},
-	{0x0000a5a0, 0x2f001f04, 0x2f001f04, 0x1b000603, 0x1b000603},
-	{0x0000a5a4, 0x35001fc4, 0x35001fc4, 0x1f000a02, 0x1f000a02},
-	{0x0000a5a8, 0x3c022f04, 0x3c022f04, 0x23000a04, 0x23000a04},
-	{0x0000a5ac, 0x41023e85, 0x41023e85, 0x26000a20, 0x26000a20},
-	{0x0000a5b0, 0x48023ec6, 0x48023ec6, 0x2a000e20, 0x2a000e20},
-	{0x0000a5b4, 0x4d023f01, 0x4d023f01, 0x2e000e22, 0x2e000e22},
-	{0x0000a5b8, 0x53023f4b, 0x53023f4b, 0x31000e24, 0x31000e24},
-	{0x0000a5bc, 0x5a027f09, 0x5a027f09, 0x34001640, 0x34001640},
-	{0x0000a5c0, 0x5f027fc9, 0x5f027fc9, 0x38001660, 0x38001660},
-	{0x0000a5c4, 0x6502feca, 0x6502feca, 0x3b001861, 0x3b001861},
-	{0x0000a5c8, 0x6b02ff4a, 0x6b02ff4a, 0x3e001a81, 0x3e001a81},
-	{0x0000a5cc, 0x7203feca, 0x7203feca, 0x42001a83, 0x42001a83},
-	{0x0000a5d0, 0x7703ff0b, 0x7703ff0b, 0x44001c84, 0x44001c84},
-	{0x0000a5d4, 0x7d06ffcb, 0x7d06ffcb, 0x48001ce3, 0x48001ce3},
-	{0x0000a5d8, 0x8407ff0b, 0x8407ff0b, 0x4c001ce5, 0x4c001ce5},
-	{0x0000a5dc, 0x8907ffcb, 0x8907ffcb, 0x50001ce9, 0x50001ce9},
-	{0x0000a5e0, 0x900fff0b, 0x900fff0b, 0x54001ceb, 0x54001ceb},
-	{0x0000a5e4, 0x960fffcb, 0x960fffcb, 0x56001eec, 0x56001eec},
-	{0x0000a5e8, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5ec, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f0, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f4, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5f8, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a5fc, 0x9c1fff0b, 0x9c1fff0b, 0x56001eec, 0x56001eec},
-	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
-	{0x0000a618, 0x02008501, 0x02008501, 0x02008501, 0x02008501},
-	{0x0000a61c, 0x02008802, 0x02008802, 0x02008802, 0x02008802},
-	{0x0000a620, 0x0300c802, 0x0300c802, 0x0300c802, 0x0300c802},
-	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x0300cc03, 0x0300cc03},
-	{0x0000a628, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a62c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a630, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a634, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a638, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-	{0x0000a63c, 0x04011004, 0x04011004, 0x04011004, 0x04011004},
-};
+#define ar9331_1p2_baseband_core ar9331_1p1_baseband_core
 
-static const u32 ar9331_1p2_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},
-};
+#define ar9331_1p2_soc_postamble ar9331_1p1_soc_postamble
 
-static const u32 ar9331_1p2_soc_preamble[][2] = {
-	/* Addr      allmodes  */
-	{0x00007020, 0x00000000},
-	{0x00007034, 0x00000002},
-	{0x00007038, 0x000002f8},
-};
+#define ar9331_1p2_mac_postamble ar9331_1p1_mac_postamble
 
-static const u32 ar9331_1p2_xtal_40M[][2] = {
-	/* Addr      allmodes  */
-	{0x00007038, 0x000004c2},
-	{0x00008244, 0x0010f400},
-	{0x0000824c, 0x0001e800},
-	{0x0001609c, 0x0b283f31},
-};
+#define ar9331_1p2_soc_preamble ar9331_1p1_soc_preamble
 
-static const u32 ar9331_1p2_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, 0x13881c20},
-	{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},
-	{0x000081c8, 0x00000000},
-	{0x000081cc, 0x00000000},
-	{0x000081d4, 0x00000000},
-	{0x000081ec, 0x00000000},
-	{0x000081f0, 0x00000000},
-	{0x000081f4, 0x00000000},
-	{0x000081f8, 0x00000000},
-	{0x000081fc, 0x00000000},
-	{0x00008240, 0x00100000},
-	{0x00008248, 0x00000800},
-	{0x00008250, 0x00000000},
-	{0x00008254, 0x00000000},
-	{0x00008258, 0x00000000},
-	{0x0000825c, 0x40000000},
-	{0x00008260, 0x00080922},
-	{0x00008264, 0x9d400010},
-	{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, 0xaa48105b},
-	{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},
-};
+#define ar9331_1p2_mac_core ar9331_1p1_mac_core
 
-static const u32 ar9331_common_rx_gain_1p2[][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},
-};
+#define ar9331_common_wo_xlna_rx_gain_1p2 ar9331_common_wo_xlna_rx_gain_1p1
+
+#define ar9331_common_rx_gain_1p2 ar9485_common_rx_gain_1_1
 
 #endif /* INITVALS_9330_1P2_H */
diff --git a/drivers/net/wireless/ath/ath9k/ar9340_initvals.h b/drivers/net/wireless/ath/ath9k/ar9340_initvals.h
index 815a8af1bee..1d8235e19f0 100644
--- a/drivers/net/wireless/ath/ath9k/ar9340_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9340_initvals.h
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2011 Atheros Communications Inc.
+ * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -18,16 +19,16 @@
 #define INITVALS_9340_H
 
 static const u32 ar9340_1p0_radio_postamble[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x000160ac, 0xa4646800, 0xa4646800, 0xa4646800, 0xa4646800},
-	{0x0001610c, 0x08000000, 0x08000000, 0x00000000, 0x00000000},
+	{0x0001610c, 0x08000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x00016140, 0x10804000, 0x10804000, 0x50804000, 0x50804000},
-	{0x0001650c, 0x08000000, 0x08000000, 0x00000000, 0x00000000},
+	{0x0001650c, 0x08000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x00016540, 0x10804000, 0x10804000, 0x50804000, 0x50804000},
 };
 
 static const u32 ar9340Modes_lowest_ob_db_tx_gain_table_1p0[][5] = {
-	/*   Addr     5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
 	{0x0000a500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a504, 0x06000003, 0x06000003, 0x04000002, 0x04000002},
@@ -99,21 +100,10 @@ static const u32 ar9340Modes_lowest_ob_db_tx_gain_table_1p0[][5] = {
 	{0x00016448, 0x24925266, 0x24925266, 0x24925266, 0x24925266},
 };
 
-static const u32 ar9340Modes_fast_clock_1p0[][3] = {
-	/*  Addr      5G_HT20     5G_HT40  */
-	{0x00001030, 0x00000268, 0x000004d0},
-	{0x00001070, 0x0000018c, 0x00000318},
-	{0x000010b0, 0x00000fd0, 0x00001fa0},
-	{0x00008014, 0x044c044c, 0x08980898},
-	{0x0000801c, 0x148ec02b, 0x148ec057},
-	{0x00008318, 0x000044c0, 0x00008980},
-	{0x00009e00, 0x03721821, 0x03721821},
-	{0x0000a230, 0x0000000b, 0x00000016},
-	{0x0000a254, 0x00000898, 0x00001130},
-};
+#define ar9340Modes_fast_clock_1p0 ar9300Modes_fast_clock_2p2
 
 static const u32 ar9340_1p0_radio_core[][2] = {
-	/*  Addr     allmodes  */
+	/* Addr      allmodes  */
 	{0x00016000, 0x36db6db6},
 	{0x00016004, 0x6db6db40},
 	{0x00016008, 0x73f00000},
@@ -146,15 +136,13 @@ static const u32 ar9340_1p0_radio_core[][2] = {
 	{0x00016100, 0x04cb0001},
 	{0x00016104, 0xfff80000},
 	{0x00016108, 0x00080010},
-	{0x0001610c, 0x00000000},
 	{0x00016140, 0x50804008},
 	{0x00016144, 0x01884080},
 	{0x00016148, 0x000080c0},
 	{0x00016280, 0x01000015},
-	{0x00016284, 0x05530000},
+	{0x00016284, 0x15530000},
 	{0x00016288, 0x00318000},
 	{0x0001628c, 0x50000000},
-	{0x00016290, 0x4080294f},
 	{0x00016380, 0x00000000},
 	{0x00016384, 0x00000000},
 	{0x00016388, 0x00800700},
@@ -219,52 +207,43 @@ static const u32 ar9340_1p0_radio_core[][2] = {
 };
 
 static const u32 ar9340_1p0_radio_core_40M[][2] = {
+	/* Addr      allmodes  */
 	{0x0001609c, 0x02566f3a},
 	{0x000160ac, 0xa4647c00},
 	{0x000160b0, 0x01885f5a},
 };
 
-static const u32 ar9340_1p0_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},
-};
+#define ar9340_1p0_mac_postamble ar9300_2p2_mac_postamble
 
-static const u32 ar9340_1p0_soc_postamble[][5] = {
-	/*   Addr     5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
-	{0x00007010, 0x00000023, 0x00000023, 0x00000023, 0x00000023},
-};
+#define ar9340_1p0_soc_postamble ar9300_2p2_soc_postamble
 
 static const u32 ar9340_1p0_baseband_postamble[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
 	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8011, 0xd00a8011},
 	{0x00009820, 0x206a022e, 0x206a022e, 0x206a022e, 0x206a022e},
 	{0x00009824, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0},
 	{0x00009828, 0x06903081, 0x06903081, 0x06903881, 0x06903881},
 	{0x0000982c, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4},
 	{0x00009830, 0x0000059c, 0x0000059c, 0x0000119c, 0x0000119c},
-	{0x00009c00, 0x00000044, 0x000000c4, 0x000000c4, 0x00000044},
-	{0x00009e00, 0x0372161e, 0x0372161e, 0x037216a0, 0x037216a0},
-	{0x00009e04, 0x00182020, 0x00182020, 0x00182020, 0x00182020},
+	{0x00009c00, 0x000000c4, 0x000000c4, 0x000000c4, 0x000000c4},
+	{0x00009e00, 0x0372111a, 0x0372111a, 0x037216a0, 0x037216a0},
+	{0x00009e04, 0x001c2020, 0x001c2020, 0x001c2020, 0x001c2020},
 	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000e2, 0x6c4000e2},
 	{0x00009e10, 0x7ec88d2e, 0x7ec88d2e, 0x7ec88d2e, 0x7ec88d2e},
-	{0x00009e14, 0x31395d5e, 0x3139605e, 0x3139605e, 0x31395d5e},
+	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3379605e, 0x33795d5e},
 	{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, 0x02291e27, 0x02291e27},
 	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
 	{0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
-	{0x0000a204, 0x00003fc0, 0x00003fc4, 0x00003fc4, 0x00003fc0},
+	{0x0000a204, 0x00003ec0, 0x00003ec4, 0x00003ec4, 0x00003ec0},
 	{0x0000a208, 0x00000104, 0x00000104, 0x00000004, 0x00000004},
+	{0x0000a22c, 0x07e26a2f, 0x07e26a2f, 0x01026a2f, 0x01026a2f},
 	{0x0000a230, 0x0000000a, 0x00000014, 0x00000016, 0x0000000b},
+	{0x0000a234, 0x00000fff, 0x10000fff, 0x10000fff, 0x00000fff},
 	{0x0000a238, 0xffb81018, 0xffb81018, 0xffb81018, 0xffb81018},
 	{0x0000a250, 0x00000000, 0x00000000, 0x00000210, 0x00000108},
 	{0x0000a254, 0x000007d0, 0x00000fa0, 0x00001130, 0x00000898},
@@ -277,11 +256,11 @@ static const u32 ar9340_1p0_baseband_postamble[][5] = {
 	{0x0000a288, 0x00000220, 0x00000220, 0x00000110, 0x00000110},
 	{0x0000a28c, 0x00011111, 0x00011111, 0x00022222, 0x00022222},
 	{0x0000a2c4, 0x00158d18, 0x00158d18, 0x00158d18, 0x00158d18},
-	{0x0000a2d0, 0x00071981, 0x00071981, 0x00071981, 0x00071982},
-	{0x0000a2d8, 0xf999a83a, 0xf999a83a, 0xf999a83a, 0xf999a83a},
+	{0x0000a2d0, 0x00041983, 0x00041983, 0x00041982, 0x00041982},
+	{0x0000a2d8, 0x7999a83a, 0x7999a83a, 0x7999a83a, 0x7999a83a},
 	{0x0000a358, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a830, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
-	{0x0000ae04, 0x00180000, 0x00180000, 0x00180000, 0x00180000},
+	{0x0000ae04, 0x001c0000, 0x001c0000, 0x001c0000, 0x001c0000},
 	{0x0000ae18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000ae1c, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
 	{0x0000ae20, 0x000001b5, 0x000001b5, 0x000001ce, 0x000001ce},
@@ -289,21 +268,21 @@ static const u32 ar9340_1p0_baseband_postamble[][5] = {
 };
 
 static const u32 ar9340_1p0_baseband_core[][2] = {
-	/*  Addr     allmodes  */
+	/* Addr      allmodes  */
 	{0x00009800, 0xafe68e30},
 	{0x00009804, 0xfd14e000},
 	{0x00009808, 0x9c0a9f6b},
 	{0x0000980c, 0x04900000},
-	{0x00009814, 0xb280c00a},
+	{0x00009814, 0x3280c00a},
 	{0x00009818, 0x00000000},
 	{0x0000981c, 0x00020028},
-	{0x00009834, 0x5f3ca3de},
+	{0x00009834, 0x6400a190},
 	{0x00009838, 0x0108ecff},
-	{0x0000983c, 0x14750600},
+	{0x0000983c, 0x14000600},
 	{0x00009880, 0x201fff00},
 	{0x00009884, 0x00001042},
 	{0x000098a4, 0x00200400},
-	{0x000098b0, 0x52440bbe},
+	{0x000098b0, 0x32840bbe},
 	{0x000098d0, 0x004b6a8e},
 	{0x000098d4, 0x00000820},
 	{0x000098dc, 0x00000000},
@@ -329,7 +308,6 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 	{0x00009e30, 0x06336f77},
 	{0x00009e34, 0x6af6532f},
 	{0x00009e38, 0x0cc80c00},
-	{0x00009e3c, 0xcf946222},
 	{0x00009e40, 0x0d261820},
 	{0x00009e4c, 0x00001004},
 	{0x00009e50, 0x00ff03f1},
@@ -342,8 +320,6 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 	{0x0000a220, 0x00000000},
 	{0x0000a224, 0x00000000},
 	{0x0000a228, 0x10002310},
-	{0x0000a22c, 0x01036a1e},
-	{0x0000a234, 0x10000fff},
 	{0x0000a23c, 0x00000000},
 	{0x0000a244, 0x0c000000},
 	{0x0000a2a0, 0x00000001},
@@ -351,10 +327,6 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 	{0x0000a2c8, 0x00000000},
 	{0x0000a2cc, 0x18c43433},
 	{0x0000a2d4, 0x00000000},
-	{0x0000a2dc, 0x00000000},
-	{0x0000a2e0, 0x00000000},
-	{0x0000a2e4, 0x00000000},
-	{0x0000a2e8, 0x00000000},
 	{0x0000a2ec, 0x00000000},
 	{0x0000a2f0, 0x00000000},
 	{0x0000a2f4, 0x00000000},
@@ -385,7 +357,7 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 	{0x0000a3e8, 0x20202020},
 	{0x0000a3ec, 0x20202020},
 	{0x0000a3f0, 0x00000000},
-	{0x0000a3f4, 0x00000246},
+	{0x0000a3f4, 0x00000000},
 	{0x0000a3f8, 0x0cdbd380},
 	{0x0000a3fc, 0x000f0f01},
 	{0x0000a400, 0x8fa91f01},
@@ -402,33 +374,17 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 	{0x0000a430, 0x1ce739ce},
 	{0x0000a434, 0x00000000},
 	{0x0000a438, 0x00001801},
-	{0x0000a43c, 0x00000000},
+	{0x0000a43c, 0x00100000},
 	{0x0000a440, 0x00000000},
 	{0x0000a444, 0x00000000},
-	{0x0000a448, 0x04000080},
+	{0x0000a448, 0x05000080},
 	{0x0000a44c, 0x00000001},
 	{0x0000a450, 0x00010000},
 	{0x0000a458, 0x00000000},
-	{0x0000a600, 0x00000000},
-	{0x0000a604, 0x00000000},
-	{0x0000a608, 0x00000000},
-	{0x0000a60c, 0x00000000},
-	{0x0000a610, 0x00000000},
-	{0x0000a614, 0x00000000},
-	{0x0000a618, 0x00000000},
-	{0x0000a61c, 0x00000000},
-	{0x0000a620, 0x00000000},
-	{0x0000a624, 0x00000000},
-	{0x0000a628, 0x00000000},
-	{0x0000a62c, 0x00000000},
-	{0x0000a630, 0x00000000},
-	{0x0000a634, 0x00000000},
-	{0x0000a638, 0x00000000},
-	{0x0000a63c, 0x00000000},
 	{0x0000a640, 0x00000000},
 	{0x0000a644, 0x3fad9d74},
 	{0x0000a648, 0x0048060a},
-	{0x0000a64c, 0x00000637},
+	{0x0000a64c, 0x00003c37},
 	{0x0000a670, 0x03020100},
 	{0x0000a674, 0x09080504},
 	{0x0000a678, 0x0d0c0b0a},
@@ -451,10 +407,6 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 	{0x0000a8f4, 0x00000000},
 	{0x0000b2d0, 0x00000080},
 	{0x0000b2d4, 0x00000000},
-	{0x0000b2dc, 0x00000000},
-	{0x0000b2e0, 0x00000000},
-	{0x0000b2e4, 0x00000000},
-	{0x0000b2e8, 0x00000000},
 	{0x0000b2ec, 0x00000000},
 	{0x0000b2f0, 0x00000000},
 	{0x0000b2f4, 0x00000000},
@@ -465,80 +417,108 @@ static const u32 ar9340_1p0_baseband_core[][2] = {
 };
 
 static const u32 ar9340Modes_high_power_tx_gain_table_1p0[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x0000a2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
+	{0x0000a2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
+	{0x0000a2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
+	{0x0000a618, 0x01404501, 0x01404501, 0x01404501, 0x01404501},
+	{0x0000a61c, 0x02008802, 0x02008802, 0x02008501, 0x02008501},
+	{0x0000a620, 0x0300cc03, 0x0300cc03, 0x0280ca03, 0x0280ca03},
+	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x03010c04, 0x03010c04},
+	{0x0000a628, 0x0300cc03, 0x0300cc03, 0x04014c04, 0x04014c04},
+	{0x0000a62c, 0x03810c03, 0x03810c03, 0x04015005, 0x04015005},
+	{0x0000a630, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a634, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a638, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a63c, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000b2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
+	{0x0000b2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
+	{0x0000b2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x0000a410, 0x000050d8, 0x000050d8, 0x000050d9, 0x000050d9},
 	{0x0000a500, 0x00002220, 0x00002220, 0x00000000, 0x00000000},
-	{0x0000a504, 0x04002222, 0x04002222, 0x04000002, 0x04000002},
-	{0x0000a508, 0x09002421, 0x09002421, 0x08000004, 0x08000004},
-	{0x0000a50c, 0x0d002621, 0x0d002621, 0x0b000200, 0x0b000200},
-	{0x0000a510, 0x13004620, 0x13004620, 0x0f000202, 0x0f000202},
-	{0x0000a514, 0x19004a20, 0x19004a20, 0x11000400, 0x11000400},
-	{0x0000a518, 0x1d004e20, 0x1d004e20, 0x15000402, 0x15000402},
-	{0x0000a51c, 0x21005420, 0x21005420, 0x19000404, 0x19000404},
-	{0x0000a520, 0x26005e20, 0x26005e20, 0x1b000603, 0x1b000603},
-	{0x0000a524, 0x2b005e40, 0x2b005e40, 0x1f000a02, 0x1f000a02},
-	{0x0000a528, 0x2f005e42, 0x2f005e42, 0x23000a04, 0x23000a04},
-	{0x0000a52c, 0x33005e44, 0x33005e44, 0x26000a20, 0x26000a20},
-	{0x0000a530, 0x38005e65, 0x38005e65, 0x2a000e20, 0x2a000e20},
-	{0x0000a534, 0x3c005e69, 0x3c005e69, 0x2e000e22, 0x2e000e22},
-	{0x0000a538, 0x40005e6b, 0x40005e6b, 0x31000e24, 0x31000e24},
-	{0x0000a53c, 0x44005e6d, 0x44005e6d, 0x34001640, 0x34001640},
-	{0x0000a540, 0x49005e72, 0x49005e72, 0x38001660, 0x38001660},
-	{0x0000a544, 0x4e005eb2, 0x4e005eb2, 0x3b001861, 0x3b001861},
-	{0x0000a548, 0x53005f12, 0x53005f12, 0x3e001a81, 0x3e001a81},
-	{0x0000a54c, 0x59025eb5, 0x59025eb5, 0x42001a83, 0x42001a83},
-	{0x0000a550, 0x5e025f12, 0x5e025f12, 0x44001c84, 0x44001c84},
-	{0x0000a554, 0x61027f12, 0x61027f12, 0x48001ce3, 0x48001ce3},
-	{0x0000a558, 0x6702bf12, 0x6702bf12, 0x4c001ce5, 0x4c001ce5},
-	{0x0000a55c, 0x6b02bf14, 0x6b02bf14, 0x50001ce9, 0x50001ce9},
-	{0x0000a560, 0x6f02bf16, 0x6f02bf16, 0x54001ceb, 0x54001ceb},
-	{0x0000a564, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a568, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a56c, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a570, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a574, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a578, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a57c, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
+	{0x0000a504, 0x04002222, 0x04002222, 0x02000001, 0x02000001},
+	{0x0000a508, 0x09002421, 0x09002421, 0x05000003, 0x05000003},
+	{0x0000a50c, 0x0d002621, 0x0d002621, 0x0a000005, 0x0a000005},
+	{0x0000a510, 0x13004620, 0x13004620, 0x0e000201, 0x0e000201},
+	{0x0000a514, 0x19004a20, 0x19004a20, 0x11000203, 0x11000203},
+	{0x0000a518, 0x1d004e20, 0x1d004e20, 0x14000401, 0x14000401},
+	{0x0000a51c, 0x21005420, 0x21005420, 0x18000403, 0x18000403},
+	{0x0000a520, 0x26005e20, 0x26005e20, 0x1b000602, 0x1b000602},
+	{0x0000a524, 0x2b005e40, 0x2b005e40, 0x1f000802, 0x1f000802},
+	{0x0000a528, 0x2f005e42, 0x2f005e42, 0x21000620, 0x21000620},
+	{0x0000a52c, 0x33005e44, 0x33005e44, 0x25000820, 0x25000820},
+	{0x0000a530, 0x38005e65, 0x38005e65, 0x29000822, 0x29000822},
+	{0x0000a534, 0x3c005e69, 0x3c005e69, 0x2d000824, 0x2d000824},
+	{0x0000a538, 0x40005e6b, 0x40005e6b, 0x30000828, 0x30000828},
+	{0x0000a53c, 0x44005e6d, 0x44005e6d, 0x3400082a, 0x3400082a},
+	{0x0000a540, 0x49005e72, 0x49005e72, 0x38000849, 0x38000849},
+	{0x0000a544, 0x4e005eb2, 0x4e005eb2, 0x3b000a2c, 0x3b000a2c},
+	{0x0000a548, 0x53005f12, 0x53005f12, 0x3e000e2b, 0x3e000e2b},
+	{0x0000a54c, 0x59025eb5, 0x59025eb5, 0x42000e2d, 0x42000e2d},
+	{0x0000a550, 0x5e025f12, 0x5e025f12, 0x4500124a, 0x4500124a},
+	{0x0000a554, 0x61027f12, 0x61027f12, 0x4900124c, 0x4900124c},
+	{0x0000a558, 0x6702bf12, 0x6702bf12, 0x4c00126c, 0x4c00126c},
+	{0x0000a55c, 0x6b02bf14, 0x6b02bf14, 0x4f00128c, 0x4f00128c},
+	{0x0000a560, 0x6f02bf16, 0x6f02bf16, 0x52001290, 0x52001290},
+	{0x0000a564, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
+	{0x0000a568, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
+	{0x0000a56c, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
+	{0x0000a570, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
+	{0x0000a574, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
+	{0x0000a578, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
+	{0x0000a57c, 0x6f02bf16, 0x6f02bf16, 0x56001292, 0x56001292},
 	{0x0000a580, 0x00802220, 0x00802220, 0x00800000, 0x00800000},
-	{0x0000a584, 0x04802222, 0x04802222, 0x04800002, 0x04800002},
-	{0x0000a588, 0x09802421, 0x09802421, 0x08800004, 0x08800004},
-	{0x0000a58c, 0x0d802621, 0x0d802621, 0x0b800200, 0x0b800200},
-	{0x0000a590, 0x13804620, 0x13804620, 0x0f800202, 0x0f800202},
-	{0x0000a594, 0x19804a20, 0x19804a20, 0x11800400, 0x11800400},
-	{0x0000a598, 0x1d804e20, 0x1d804e20, 0x15800402, 0x15800402},
-	{0x0000a59c, 0x21805420, 0x21805420, 0x19800404, 0x19800404},
-	{0x0000a5a0, 0x26805e20, 0x26805e20, 0x1b800603, 0x1b800603},
-	{0x0000a5a4, 0x2b805e40, 0x2b805e40, 0x1f800a02, 0x1f800a02},
-	{0x0000a5a8, 0x2f805e42, 0x2f805e42, 0x23800a04, 0x23800a04},
-	{0x0000a5ac, 0x33805e44, 0x33805e44, 0x26800a20, 0x26800a20},
-	{0x0000a5b0, 0x38805e65, 0x38805e65, 0x2a800e20, 0x2a800e20},
-	{0x0000a5b4, 0x3c805e69, 0x3c805e69, 0x2e800e22, 0x2e800e22},
-	{0x0000a5b8, 0x40805e6b, 0x40805e6b, 0x31800e24, 0x31800e24},
-	{0x0000a5bc, 0x44805e6d, 0x44805e6d, 0x34801640, 0x34801640},
-	{0x0000a5c0, 0x49805e72, 0x49805e72, 0x38801660, 0x38801660},
-	{0x0000a5c4, 0x4e805eb2, 0x4e805eb2, 0x3b801861, 0x3b801861},
-	{0x0000a5c8, 0x53805f12, 0x53805f12, 0x3e801a81, 0x3e801a81},
-	{0x0000a5cc, 0x59825eb2, 0x59825eb2, 0x42801a83, 0x42801a83},
-	{0x0000a5d0, 0x5e825f12, 0x5e825f12, 0x44801c84, 0x44801c84},
-	{0x0000a5d4, 0x61827f12, 0x61827f12, 0x48801ce3, 0x48801ce3},
-	{0x0000a5d8, 0x6782bf12, 0x6782bf12, 0x4c801ce5, 0x4c801ce5},
-	{0x0000a5dc, 0x6b82bf14, 0x6b82bf14, 0x50801ce9, 0x50801ce9},
-	{0x0000a5e0, 0x6f82bf16, 0x6f82bf16, 0x54801ceb, 0x54801ceb},
-	{0x0000a5e4, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5e8, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5ec, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5f0, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5f4, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5f8, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5fc, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x00016044, 0x056db2db, 0x056db2db, 0x056db2db, 0x056db2db},
+	{0x0000a584, 0x04802222, 0x04802222, 0x02800001, 0x02800001},
+	{0x0000a588, 0x09802421, 0x09802421, 0x05800003, 0x05800003},
+	{0x0000a58c, 0x0d802621, 0x0d802621, 0x0a800005, 0x0a800005},
+	{0x0000a590, 0x13804620, 0x13804620, 0x0e800201, 0x0e800201},
+	{0x0000a594, 0x19804a20, 0x19804a20, 0x11800203, 0x11800203},
+	{0x0000a598, 0x1d804e20, 0x1d804e20, 0x14800401, 0x14800401},
+	{0x0000a59c, 0x21805420, 0x21805420, 0x18800403, 0x18800403},
+	{0x0000a5a0, 0x26805e20, 0x26805e20, 0x1b800602, 0x1b800602},
+	{0x0000a5a4, 0x2b805e40, 0x2b805e40, 0x1f800802, 0x1f800802},
+	{0x0000a5a8, 0x2f805e42, 0x2f805e42, 0x21800620, 0x21800620},
+	{0x0000a5ac, 0x33805e44, 0x33805e44, 0x25800820, 0x25800820},
+	{0x0000a5b0, 0x38805e65, 0x38805e65, 0x29800822, 0x29800822},
+	{0x0000a5b4, 0x3c805e69, 0x3c805e69, 0x2d800824, 0x2d800824},
+	{0x0000a5b8, 0x40805e6b, 0x40805e6b, 0x30800828, 0x30800828},
+	{0x0000a5bc, 0x44805e6d, 0x44805e6d, 0x3480082a, 0x3480082a},
+	{0x0000a5c0, 0x49805e72, 0x49805e72, 0x38800849, 0x38800849},
+	{0x0000a5c4, 0x4e805eb2, 0x4e805eb2, 0x3b800a2c, 0x3b800a2c},
+	{0x0000a5c8, 0x53805f12, 0x53805f12, 0x3e800e2b, 0x3e800e2b},
+	{0x0000a5cc, 0x59825eb2, 0x59825eb2, 0x42800e2d, 0x42800e2d},
+	{0x0000a5d0, 0x5e825f12, 0x5e825f12, 0x4580124a, 0x4580124a},
+	{0x0000a5d4, 0x61827f12, 0x61827f12, 0x4980124c, 0x4980124c},
+	{0x0000a5d8, 0x6782bf12, 0x6782bf12, 0x4c80126c, 0x4c80126c},
+	{0x0000a5dc, 0x6b82bf14, 0x6b82bf14, 0x4f80128c, 0x4f80128c},
+	{0x0000a5e0, 0x6f82bf16, 0x6f82bf16, 0x52801290, 0x52801290},
+	{0x0000a5e4, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x0000a5e8, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x0000a5ec, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x0000a5f0, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x0000a5f4, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x0000a5f8, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x0000a5fc, 0x6f82bf16, 0x6f82bf16, 0x56801292, 0x56801292},
+	{0x00016044, 0x056db2db, 0x056db2db, 0x022492db, 0x022492db},
 	{0x00016048, 0x24925266, 0x24925266, 0x24925266, 0x24925266},
-	{0x00016444, 0x056db2db, 0x056db2db, 0x056db2db, 0x056db2db},
+	{0x00016444, 0x056db2db, 0x056db2db, 0x022492db, 0x022492db},
 	{0x00016448, 0x24925266, 0x24925266, 0x24925266, 0x24925266},
 };
 
 static const u32 ar9340Modes_high_ob_db_tx_gain_table_1p0[][5] = {
-	/*  Addr       5G_HT20    5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x0000a2dc, 0x01feee00, 0x01feee00, 0x03aaa352, 0x03aaa352},
+	{0x0000a2e0, 0x0000f000, 0x0000f000, 0x03ccc584, 0x03ccc584},
+	{0x0000a2e4, 0x01ff0000, 0x01ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x0000a410, 0x000050d8, 0x000050d8, 0x000050d9, 0x000050d9},
 	{0x0000a500, 0x00002220, 0x00002220, 0x00000000, 0x00000000},
 	{0x0000a504, 0x04002222, 0x04002222, 0x04000002, 0x04000002},
@@ -559,7 +539,7 @@ static const u32 ar9340Modes_high_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a540, 0x49005e72, 0x49005e72, 0x38001660, 0x38001660},
 	{0x0000a544, 0x4e005eb2, 0x4e005eb2, 0x3b001861, 0x3b001861},
 	{0x0000a548, 0x53005f12, 0x53005f12, 0x3e001a81, 0x3e001a81},
-	{0x0000a54c, 0x59025eb5, 0x59025eb5, 0x42001a83, 0x42001a83},
+	{0x0000a54c, 0x59025eb2, 0x59025eb2, 0x42001a83, 0x42001a83},
 	{0x0000a550, 0x5e025f12, 0x5e025f12, 0x44001c84, 0x44001c84},
 	{0x0000a554, 0x61027f12, 0x61027f12, 0x48001ce3, 0x48001ce3},
 	{0x0000a558, 0x6702bf12, 0x6702bf12, 0x4c001ce5, 0x4c001ce5},
@@ -604,13 +584,43 @@ static const u32 ar9340Modes_high_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a5f4, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
 	{0x0000a5f8, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
 	{0x0000a5fc, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00804000, 0x00804000, 0x00000000, 0x00000000},
+	{0x0000a614, 0x00804201, 0x00804201, 0x01404000, 0x01404000},
+	{0x0000a618, 0x0280c802, 0x0280c802, 0x01404501, 0x01404501},
+	{0x0000a61c, 0x0280ca03, 0x0280ca03, 0x02008501, 0x02008501},
+	{0x0000a620, 0x04c15104, 0x04c15104, 0x0280ca03, 0x0280ca03},
+	{0x0000a624, 0x04c15305, 0x04c15305, 0x03010c04, 0x03010c04},
+	{0x0000a628, 0x04c15305, 0x04c15305, 0x04014c04, 0x04014c04},
+	{0x0000a62c, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a630, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a634, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a638, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a63c, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000b2dc, 0x01feee00, 0x01feee00, 0x03aaa352, 0x03aaa352},
+	{0x0000b2e0, 0x0000f000, 0x0000f000, 0x03ccc584, 0x03ccc584},
+	{0x0000b2e4, 0x01ff0000, 0x01ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x00016044, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4},
-	{0x00016048, 0x8e481266, 0x8e481266, 0x8e481266, 0x8e481266},
+	{0x00016048, 0x8e481666, 0x8e481666, 0x8e481266, 0x8e481266},
+	{0x00016280, 0x01000015, 0x01000015, 0x01001015, 0x01001015},
 	{0x00016444, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4},
-	{0x00016448, 0x8e481266, 0x8e481266, 0x8e481266, 0x8e481266},
+	{0x00016448, 0x8e481666, 0x8e481666, 0x8e481266, 0x8e481266},
 };
+
 static const u32 ar9340Modes_ub124_tx_gain_table_1p0[][5] = {
-	/*  Addr      5G_HT20      5G_HT40     2G_HT40    2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8005, 0xd00a8005},
+	{0x00009820, 0x206a022e, 0x206a022e, 0x206a00ae, 0x206a00ae},
+	{0x00009830, 0x0000059c, 0x0000059c, 0x0000059c, 0x0000059c},
+	{0x00009e10, 0x7ec88d2e, 0x7ec88d2e, 0x7ec82d2e, 0x7ec82d2e},
+	{0x0000a2dc, 0xfef5d402, 0xfef5d402, 0xfdab5b52, 0xfdab5b52},
+	{0x0000a2e0, 0xfe896600, 0xfe896600, 0xfd339c84, 0xfd339c84},
+	{0x0000a2e4, 0xff01f800, 0xff01f800, 0xfec3e000, 0xfec3e000},
+	{0x0000a2e8, 0xfffe0000, 0xfffe0000, 0xfffc0000, 0xfffc0000},
 	{0x0000a410, 0x000050d8, 0x000050d8, 0x000050d9, 0x000050d9},
 	{0x0000a500, 0x00002220, 0x00002220, 0x00000000, 0x00000000},
 	{0x0000a504, 0x04002222, 0x04002222, 0x04000002, 0x04000002},
@@ -676,15 +686,34 @@ static const u32 ar9340Modes_ub124_tx_gain_table_1p0[][5] = {
 	{0x0000a5f4, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
 	{0x0000a5f8, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
 	{0x0000a5fc, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x00016044, 0x036db2db, 0x036db2db, 0x036db2db, 0x036db2db},
-	{0x00016048, 0x69b65266, 0x69b65266, 0x69b65266, 0x69b65266},
-	{0x00016444, 0x036db2db, 0x036db2db, 0x036db2db, 0x036db2db},
-	{0x00016448, 0x69b65266, 0x69b65266, 0x69b65266, 0x69b65266},
+	{0x00016044, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4},
+	{0x00016048, 0x8e480086, 0x8e480086, 0x8e480086, 0x8e480086},
+	{0x00016444, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4, 0x03b6d2e4},
+	{0x00016448, 0x8e480086, 0x8e480086, 0x8e480086, 0x8e480086},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00804000, 0x00804000, 0x00000000, 0x00000000},
+	{0x0000a614, 0x00804201, 0x00804201, 0x01404000, 0x01404000},
+	{0x0000a618, 0x0280c802, 0x0280c802, 0x01404501, 0x01404501},
+	{0x0000a61c, 0x0280ca03, 0x0280ca03, 0x02008501, 0x02008501},
+	{0x0000a620, 0x04c15104, 0x04c15104, 0x0280ca03, 0x0280ca03},
+	{0x0000a624, 0x04c15305, 0x04c15305, 0x03010c04, 0x03010c04},
+	{0x0000a628, 0x04c15305, 0x04c15305, 0x04014c04, 0x04014c04},
+	{0x0000a62c, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a630, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a634, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a638, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000a63c, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
+	{0x0000b2dc, 0xfef5d402, 0xfef5d402, 0xfdab5b52, 0xfdab5b52},
+	{0x0000b2e0, 0xfe896600, 0xfe896600, 0xfd339c84, 0xfd339c84},
+	{0x0000b2e4, 0xff01f800, 0xff01f800, 0xfec3e000, 0xfec3e000},
+	{0x0000b2e8, 0xfffe0000, 0xfffe0000, 0xfffc0000, 0xfffc0000},
 };
 
-
 static const u32 ar9340Common_rx_gain_table_1p0[][2] = {
-	/*   Addr     allmodes */
+	/* Addr      allmodes  */
 	{0x0000a000, 0x00010000},
 	{0x0000a004, 0x00030002},
 	{0x0000a008, 0x00050004},
@@ -845,14 +874,14 @@ static const u32 ar9340Common_rx_gain_table_1p0[][2] = {
 	{0x0000b074, 0x00000000},
 	{0x0000b078, 0x00000000},
 	{0x0000b07c, 0x00000000},
-	{0x0000b080, 0x32323232},
-	{0x0000b084, 0x2f2f3232},
-	{0x0000b088, 0x23282a2d},
-	{0x0000b08c, 0x1c1e2123},
-	{0x0000b090, 0x14171919},
-	{0x0000b094, 0x0e0e1214},
-	{0x0000b098, 0x03050707},
-	{0x0000b09c, 0x00030303},
+	{0x0000b080, 0x23232323},
+	{0x0000b084, 0x21232323},
+	{0x0000b088, 0x19191c1e},
+	{0x0000b08c, 0x12141417},
+	{0x0000b090, 0x07070e0e},
+	{0x0000b094, 0x03030305},
+	{0x0000b098, 0x00000003},
+	{0x0000b09c, 0x00000000},
 	{0x0000b0a0, 0x00000000},
 	{0x0000b0a4, 0x00000000},
 	{0x0000b0a8, 0x00000000},
@@ -944,7 +973,11 @@ static const u32 ar9340Common_rx_gain_table_1p0[][2] = {
 };
 
 static const u32 ar9340Modes_low_ob_db_tx_gain_table_1p0[][5] = {
-	/*  Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x0000a2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
+	{0x0000a2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
+	{0x0000a2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
 	{0x0000a500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a504, 0x06000003, 0x06000003, 0x04000002, 0x04000002},
@@ -952,8 +985,8 @@ static const u32 ar9340Modes_low_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a50c, 0x10000023, 0x10000023, 0x0b000200, 0x0b000200},
 	{0x0000a510, 0x16000220, 0x16000220, 0x0f000202, 0x0f000202},
 	{0x0000a514, 0x1c000223, 0x1c000223, 0x12000400, 0x12000400},
-	{0x0000a518, 0x21020220, 0x21020220, 0x16000402, 0x16000402},
-	{0x0000a51c, 0x27020223, 0x27020223, 0x19000404, 0x19000404},
+	{0x0000a518, 0x21002220, 0x21002220, 0x16000402, 0x16000402},
+	{0x0000a51c, 0x27002223, 0x27002223, 0x19000404, 0x19000404},
 	{0x0000a520, 0x2b022220, 0x2b022220, 0x1c000603, 0x1c000603},
 	{0x0000a524, 0x2f022222, 0x2f022222, 0x21000a02, 0x21000a02},
 	{0x0000a528, 0x34022225, 0x34022225, 0x25000a04, 0x25000a04},
@@ -965,19 +998,19 @@ static const u32 ar9340Modes_low_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a540, 0x4e02246c, 0x4e02246c, 0x3c001660, 0x3c001660},
 	{0x0000a544, 0x5302266c, 0x5302266c, 0x3f001861, 0x3f001861},
 	{0x0000a548, 0x5702286c, 0x5702286c, 0x43001a81, 0x43001a81},
-	{0x0000a54c, 0x5c04286b, 0x5c04286b, 0x47001a83, 0x47001a83},
-	{0x0000a550, 0x61042a6c, 0x61042a6c, 0x4a001c84, 0x4a001c84},
-	{0x0000a554, 0x66062a6c, 0x66062a6c, 0x4e001ce3, 0x4e001ce3},
-	{0x0000a558, 0x6b062e6c, 0x6b062e6c, 0x52001ce5, 0x52001ce5},
-	{0x0000a55c, 0x7006308c, 0x7006308c, 0x56001ce9, 0x56001ce9},
-	{0x0000a560, 0x730a308a, 0x730a308a, 0x5a001ceb, 0x5a001ceb},
-	{0x0000a564, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
-	{0x0000a568, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
-	{0x0000a56c, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
-	{0x0000a570, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
-	{0x0000a574, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
-	{0x0000a578, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
-	{0x0000a57c, 0x770a308c, 0x770a308c, 0x5d001eec, 0x5d001eec},
+	{0x0000a54c, 0x5c02486b, 0x5c02486b, 0x47001a83, 0x47001a83},
+	{0x0000a550, 0x61024a6c, 0x61024a6c, 0x4a001c84, 0x4a001c84},
+	{0x0000a554, 0x66026a6c, 0x66026a6c, 0x4e001ce3, 0x4e001ce3},
+	{0x0000a558, 0x6b026e6c, 0x6b026e6c, 0x52001ce5, 0x52001ce5},
+	{0x0000a55c, 0x7002708c, 0x7002708c, 0x56001ce9, 0x56001ce9},
+	{0x0000a560, 0x7302b08a, 0x7302b08a, 0x5a001ceb, 0x5a001ceb},
+	{0x0000a564, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
+	{0x0000a568, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
+	{0x0000a56c, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
+	{0x0000a570, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
+	{0x0000a574, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
+	{0x0000a578, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
+	{0x0000a57c, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
 	{0x0000a580, 0x00800000, 0x00800000, 0x00800000, 0x00800000},
 	{0x0000a584, 0x06800003, 0x06800003, 0x04800002, 0x04800002},
 	{0x0000a588, 0x0a800020, 0x0a800020, 0x08800004, 0x08800004},
@@ -1010,14 +1043,40 @@ static const u32 ar9340Modes_low_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a5f4, 0x778a308c, 0x778a308c, 0x5d801eec, 0x5d801eec},
 	{0x0000a5f8, 0x778a308c, 0x778a308c, 0x5d801eec, 0x5d801eec},
 	{0x0000a5fc, 0x778a308c, 0x778a308c, 0x5d801eec, 0x5d801eec},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
+	{0x0000a618, 0x01404501, 0x01404501, 0x01404501, 0x01404501},
+	{0x0000a61c, 0x02008802, 0x02008802, 0x02008501, 0x02008501},
+	{0x0000a620, 0x0300cc03, 0x0300cc03, 0x0280ca03, 0x0280ca03},
+	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x03010c04, 0x03010c04},
+	{0x0000a628, 0x0300cc03, 0x0300cc03, 0x04014c04, 0x04014c04},
+	{0x0000a62c, 0x03810c03, 0x03810c03, 0x04015005, 0x04015005},
+	{0x0000a630, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a634, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a638, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a63c, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000b2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
+	{0x0000b2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
+	{0x0000b2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x00016044, 0x056db2db, 0x056db2db, 0x056db2db, 0x056db2db},
-	{0x00016048, 0x24925266, 0x24925266, 0x24925266, 0x24925266},
+	{0x00016048, 0x24925666, 0x24925666, 0x24925266, 0x24925266},
+	{0x00016280, 0x01000015, 0x01000015, 0x01001015, 0x01001015},
+	{0x00016288, 0xf0318000, 0xf0318000, 0xf0318000, 0xf0318000},
 	{0x00016444, 0x056db2db, 0x056db2db, 0x056db2db, 0x056db2db},
-	{0x00016448, 0x24925266, 0x24925266, 0x24925266, 0x24925266},
+	{0x00016448, 0x24925666, 0x24925666, 0x24925266, 0x24925266},
 };
 
 static const u32 ar9340Modes_mixed_ob_db_tx_gain_table_1p0[][5] = {
-	/*  Addr       5G_HT20     5G_HT40     2G_HT40    2G_HT20  */
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x0000a2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
+	{0x0000a2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
+	{0x0000a2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
 	{0x0000a500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x0000a504, 0x06000003, 0x06000003, 0x04000002, 0x04000002},
@@ -1025,8 +1084,8 @@ static const u32 ar9340Modes_mixed_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a50c, 0x10000023, 0x10000023, 0x0b000200, 0x0b000200},
 	{0x0000a510, 0x16000220, 0x16000220, 0x0f000202, 0x0f000202},
 	{0x0000a514, 0x1c000223, 0x1c000223, 0x11000400, 0x11000400},
-	{0x0000a518, 0x21020220, 0x21020220, 0x15000402, 0x15000402},
-	{0x0000a51c, 0x27020223, 0x27020223, 0x19000404, 0x19000404},
+	{0x0000a518, 0x21002220, 0x21002220, 0x15000402, 0x15000402},
+	{0x0000a51c, 0x27002223, 0x27002223, 0x19000404, 0x19000404},
 	{0x0000a520, 0x2b022220, 0x2b022220, 0x1b000603, 0x1b000603},
 	{0x0000a524, 0x2f022222, 0x2f022222, 0x1f000a02, 0x1f000a02},
 	{0x0000a528, 0x34022225, 0x34022225, 0x23000a04, 0x23000a04},
@@ -1038,19 +1097,19 @@ static const u32 ar9340Modes_mixed_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a540, 0x4e02246c, 0x4e02246c, 0x38001660, 0x38001660},
 	{0x0000a544, 0x5302266c, 0x5302266c, 0x3b001861, 0x3b001861},
 	{0x0000a548, 0x5702286c, 0x5702286c, 0x3e001a81, 0x3e001a81},
-	{0x0000a54c, 0x5c04286b, 0x5c04286b, 0x42001a83, 0x42001a83},
-	{0x0000a550, 0x61042a6c, 0x61042a6c, 0x44001c84, 0x44001c84},
-	{0x0000a554, 0x66062a6c, 0x66062a6c, 0x48001ce3, 0x48001ce3},
-	{0x0000a558, 0x6b062e6c, 0x6b062e6c, 0x4c001ce5, 0x4c001ce5},
-	{0x0000a55c, 0x7006308c, 0x7006308c, 0x50001ce9, 0x50001ce9},
-	{0x0000a560, 0x730a308a, 0x730a308a, 0x54001ceb, 0x54001ceb},
-	{0x0000a564, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
-	{0x0000a568, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
-	{0x0000a56c, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
-	{0x0000a570, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
-	{0x0000a574, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
-	{0x0000a578, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
-	{0x0000a57c, 0x770a308c, 0x770a308c, 0x56001eec, 0x56001eec},
+	{0x0000a54c, 0x5c02486b, 0x5c02486b, 0x42001a83, 0x42001a83},
+	{0x0000a550, 0x61024a6c, 0x61024a6c, 0x44001c84, 0x44001c84},
+	{0x0000a554, 0x66026a6c, 0x66026a6c, 0x48001ce3, 0x48001ce3},
+	{0x0000a558, 0x6b026e6c, 0x6b026e6c, 0x4c001ce5, 0x4c001ce5},
+	{0x0000a55c, 0x7002708c, 0x7002708c, 0x50001ce9, 0x50001ce9},
+	{0x0000a560, 0x7302b08a, 0x7302b08a, 0x54001ceb, 0x54001ceb},
+	{0x0000a564, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
+	{0x0000a568, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
+	{0x0000a56c, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
+	{0x0000a570, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
+	{0x0000a574, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
+	{0x0000a578, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
+	{0x0000a57c, 0x7702b08c, 0x7702b08c, 0x56001eec, 0x56001eec},
 	{0x0000a580, 0x00800000, 0x00800000, 0x00800000, 0x00800000},
 	{0x0000a584, 0x06800003, 0x06800003, 0x04800002, 0x04800002},
 	{0x0000a588, 0x0a800020, 0x0a800020, 0x08800004, 0x08800004},
@@ -1083,14 +1142,36 @@ static const u32 ar9340Modes_mixed_ob_db_tx_gain_table_1p0[][5] = {
 	{0x0000a5f4, 0x778a308c, 0x778a308c, 0x56801eec, 0x56801eec},
 	{0x0000a5f8, 0x778a308c, 0x778a308c, 0x56801eec, 0x56801eec},
 	{0x0000a5fc, 0x778a308c, 0x778a308c, 0x56801eec, 0x56801eec},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
+	{0x0000a618, 0x01404501, 0x01404501, 0x01404501, 0x01404501},
+	{0x0000a61c, 0x02008802, 0x02008802, 0x02008501, 0x02008501},
+	{0x0000a620, 0x0300cc03, 0x0300cc03, 0x0280ca03, 0x0280ca03},
+	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x03010c04, 0x03010c04},
+	{0x0000a628, 0x0300cc03, 0x0300cc03, 0x04014c04, 0x04014c04},
+	{0x0000a62c, 0x03810c03, 0x03810c03, 0x04015005, 0x04015005},
+	{0x0000a630, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a634, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a638, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000a63c, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
+	{0x0000b2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
+	{0x0000b2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
+	{0x0000b2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
+	{0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
 	{0x00016044, 0x056db2db, 0x056db2db, 0x03b6d2e4, 0x03b6d2e4},
-	{0x00016048, 0x24927266, 0x24927266, 0x8e483266, 0x8e483266},
+	{0x00016048, 0x24925666, 0x24925666, 0x8e481266, 0x8e481266},
+	{0x00016280, 0x01000015, 0x01000015, 0x01001015, 0x01001015},
+	{0x00016288, 0x30318000, 0x30318000, 0x00318000, 0x00318000},
 	{0x00016444, 0x056db2db, 0x056db2db, 0x03b6d2e4, 0x03b6d2e4},
-	{0x00016448, 0x24927266, 0x24927266, 0x8e482266, 0x8e482266},
+	{0x00016448, 0x24925666, 0x24925666, 0x8e481266, 0x8e481266},
 };
 
 static const u32 ar9340_1p0_mac_core[][2] = {
-	/*    Addr        allmodes        */
+	/* Addr      allmodes  */
 	{0x00000008, 0x00000000},
 	{0x00000030, 0x00020085},
 	{0x00000034, 0x00000005},
@@ -1119,6 +1200,7 @@ static const u32 ar9340_1p0_mac_core[][2] = {
 	{0x00008004, 0x00000000},
 	{0x00008008, 0x00000000},
 	{0x0000800c, 0x00000000},
+	{0x00008010, 0x00080800},
 	{0x00008018, 0x00000000},
 	{0x00008020, 0x00000000},
 	{0x00008038, 0x00000000},
@@ -1146,7 +1228,7 @@ static const u32 ar9340_1p0_mac_core[][2] = {
 	{0x000080bc, 0x00000000},
 	{0x000080c0, 0x2a800000},
 	{0x000080c4, 0x06900168},
-	{0x000080c8, 0x13881c20},
+	{0x000080c8, 0x13881c22},
 	{0x000080cc, 0x01f40000},
 	{0x000080d0, 0x00252500},
 	{0x000080d4, 0x00a00000},
@@ -1250,276 +1332,17 @@ static const u32 ar9340_1p0_mac_core[][2] = {
 	{0x000083c4, 0x00000000},
 	{0x000083c8, 0x00000000},
 	{0x000083cc, 0x00000200},
-	{0x000083d0, 0x000301ff},
+	{0x000083d0, 0x000101ff},
 };
 
-static const u32 ar9340Common_wo_xlna_rx_gain_table_1p0[][2] = {
-	/*    Addr        allmodes        */
-	{0x0000a000, 0x00010000},
-	{0x0000a004, 0x00030002},
-	{0x0000a008, 0x00050004},
-	{0x0000a00c, 0x00810080},
-	{0x0000a010, 0x00830082},
-	{0x0000a014, 0x01810180},
-	{0x0000a018, 0x01830182},
-	{0x0000a01c, 0x01850184},
-	{0x0000a020, 0x01890188},
-	{0x0000a024, 0x018b018a},
-	{0x0000a028, 0x018d018c},
-	{0x0000a02c, 0x03820190},
-	{0x0000a030, 0x03840383},
-	{0x0000a034, 0x03880385},
-	{0x0000a038, 0x038a0389},
-	{0x0000a03c, 0x038c038b},
-	{0x0000a040, 0x0390038d},
-	{0x0000a044, 0x03920391},
-	{0x0000a048, 0x03940393},
-	{0x0000a04c, 0x03960395},
-	{0x0000a050, 0x00000000},
-	{0x0000a054, 0x00000000},
-	{0x0000a058, 0x00000000},
-	{0x0000a05c, 0x00000000},
-	{0x0000a060, 0x00000000},
-	{0x0000a064, 0x00000000},
-	{0x0000a068, 0x00000000},
-	{0x0000a06c, 0x00000000},
-	{0x0000a070, 0x00000000},
-	{0x0000a074, 0x00000000},
-	{0x0000a078, 0x00000000},
-	{0x0000a07c, 0x00000000},
-	{0x0000a080, 0x29292929},
-	{0x0000a084, 0x29292929},
-	{0x0000a088, 0x29292929},
-	{0x0000a08c, 0x29292929},
-	{0x0000a090, 0x22292929},
-	{0x0000a094, 0x1d1d2222},
-	{0x0000a098, 0x0c111117},
-	{0x0000a09c, 0x00030303},
-	{0x0000a0a0, 0x00000000},
-	{0x0000a0a4, 0x00000000},
-	{0x0000a0a8, 0x00000000},
-	{0x0000a0ac, 0x00000000},
-	{0x0000a0b0, 0x00000000},
-	{0x0000a0b4, 0x00000000},
-	{0x0000a0b8, 0x00000000},
-	{0x0000a0bc, 0x00000000},
-	{0x0000a0c0, 0x001f0000},
-	{0x0000a0c4, 0x01000101},
-	{0x0000a0c8, 0x011e011f},
-	{0x0000a0cc, 0x011c011d},
-	{0x0000a0d0, 0x02030204},
-	{0x0000a0d4, 0x02010202},
-	{0x0000a0d8, 0x021f0200},
-	{0x0000a0dc, 0x0302021e},
-	{0x0000a0e0, 0x03000301},
-	{0x0000a0e4, 0x031e031f},
-	{0x0000a0e8, 0x0402031d},
-	{0x0000a0ec, 0x04000401},
-	{0x0000a0f0, 0x041e041f},
-	{0x0000a0f4, 0x0502041d},
-	{0x0000a0f8, 0x05000501},
-	{0x0000a0fc, 0x051e051f},
-	{0x0000a100, 0x06010602},
-	{0x0000a104, 0x061f0600},
-	{0x0000a108, 0x061d061e},
-	{0x0000a10c, 0x07020703},
-	{0x0000a110, 0x07000701},
-	{0x0000a114, 0x00000000},
-	{0x0000a118, 0x00000000},
-	{0x0000a11c, 0x00000000},
-	{0x0000a120, 0x00000000},
-	{0x0000a124, 0x00000000},
-	{0x0000a128, 0x00000000},
-	{0x0000a12c, 0x00000000},
-	{0x0000a130, 0x00000000},
-	{0x0000a134, 0x00000000},
-	{0x0000a138, 0x00000000},
-	{0x0000a13c, 0x00000000},
-	{0x0000a140, 0x001f0000},
-	{0x0000a144, 0x01000101},
-	{0x0000a148, 0x011e011f},
-	{0x0000a14c, 0x011c011d},
-	{0x0000a150, 0x02030204},
-	{0x0000a154, 0x02010202},
-	{0x0000a158, 0x021f0200},
-	{0x0000a15c, 0x0302021e},
-	{0x0000a160, 0x03000301},
-	{0x0000a164, 0x031e031f},
-	{0x0000a168, 0x0402031d},
-	{0x0000a16c, 0x04000401},
-	{0x0000a170, 0x041e041f},
-	{0x0000a174, 0x0502041d},
-	{0x0000a178, 0x05000501},
-	{0x0000a17c, 0x051e051f},
-	{0x0000a180, 0x06010602},
-	{0x0000a184, 0x061f0600},
-	{0x0000a188, 0x061d061e},
-	{0x0000a18c, 0x07020703},
-	{0x0000a190, 0x07000701},
-	{0x0000a194, 0x00000000},
-	{0x0000a198, 0x00000000},
-	{0x0000a19c, 0x00000000},
-	{0x0000a1a0, 0x00000000},
-	{0x0000a1a4, 0x00000000},
-	{0x0000a1a8, 0x00000000},
-	{0x0000a1ac, 0x00000000},
-	{0x0000a1b0, 0x00000000},
-	{0x0000a1b4, 0x00000000},
-	{0x0000a1b8, 0x00000000},
-	{0x0000a1bc, 0x00000000},
-	{0x0000a1c0, 0x00000000},
-	{0x0000a1c4, 0x00000000},
-	{0x0000a1c8, 0x00000000},
-	{0x0000a1cc, 0x00000000},
-	{0x0000a1d0, 0x00000000},
-	{0x0000a1d4, 0x00000000},
-	{0x0000a1d8, 0x00000000},
-	{0x0000a1dc, 0x00000000},
-	{0x0000a1e0, 0x00000000},
-	{0x0000a1e4, 0x00000000},
-	{0x0000a1e8, 0x00000000},
-	{0x0000a1ec, 0x00000000},
-	{0x0000a1f0, 0x00000396},
-	{0x0000a1f4, 0x00000396},
-	{0x0000a1f8, 0x00000396},
-	{0x0000a1fc, 0x00000196},
-	{0x0000b000, 0x00010000},
-	{0x0000b004, 0x00030002},
-	{0x0000b008, 0x00050004},
-	{0x0000b00c, 0x00810080},
-	{0x0000b010, 0x00830082},
-	{0x0000b014, 0x01810180},
-	{0x0000b018, 0x01830182},
-	{0x0000b01c, 0x01850184},
-	{0x0000b020, 0x02810280},
-	{0x0000b024, 0x02830282},
-	{0x0000b028, 0x02850284},
-	{0x0000b02c, 0x02890288},
-	{0x0000b030, 0x028b028a},
-	{0x0000b034, 0x0388028c},
-	{0x0000b038, 0x038a0389},
-	{0x0000b03c, 0x038c038b},
-	{0x0000b040, 0x0390038d},
-	{0x0000b044, 0x03920391},
-	{0x0000b048, 0x03940393},
-	{0x0000b04c, 0x03960395},
-	{0x0000b050, 0x00000000},
-	{0x0000b054, 0x00000000},
-	{0x0000b058, 0x00000000},
-	{0x0000b05c, 0x00000000},
-	{0x0000b060, 0x00000000},
-	{0x0000b064, 0x00000000},
-	{0x0000b068, 0x00000000},
-	{0x0000b06c, 0x00000000},
-	{0x0000b070, 0x00000000},
-	{0x0000b074, 0x00000000},
-	{0x0000b078, 0x00000000},
-	{0x0000b07c, 0x00000000},
-	{0x0000b080, 0x32323232},
-	{0x0000b084, 0x2f2f3232},
-	{0x0000b088, 0x23282a2d},
-	{0x0000b08c, 0x1c1e2123},
-	{0x0000b090, 0x14171919},
-	{0x0000b094, 0x0e0e1214},
-	{0x0000b098, 0x03050707},
-	{0x0000b09c, 0x00030303},
-	{0x0000b0a0, 0x00000000},
-	{0x0000b0a4, 0x00000000},
-	{0x0000b0a8, 0x00000000},
-	{0x0000b0ac, 0x00000000},
-	{0x0000b0b0, 0x00000000},
-	{0x0000b0b4, 0x00000000},
-	{0x0000b0b8, 0x00000000},
-	{0x0000b0bc, 0x00000000},
-	{0x0000b0c0, 0x003f0020},
-	{0x0000b0c4, 0x00400041},
-	{0x0000b0c8, 0x0140005f},
-	{0x0000b0cc, 0x0160015f},
-	{0x0000b0d0, 0x017e017f},
-	{0x0000b0d4, 0x02410242},
-	{0x0000b0d8, 0x025f0240},
-	{0x0000b0dc, 0x027f0260},
-	{0x0000b0e0, 0x0341027e},
-	{0x0000b0e4, 0x035f0340},
-	{0x0000b0e8, 0x037f0360},
-	{0x0000b0ec, 0x04400441},
-	{0x0000b0f0, 0x0460045f},
-	{0x0000b0f4, 0x0541047f},
-	{0x0000b0f8, 0x055f0540},
-	{0x0000b0fc, 0x057f0560},
-	{0x0000b100, 0x06400641},
-	{0x0000b104, 0x0660065f},
-	{0x0000b108, 0x067e067f},
-	{0x0000b10c, 0x07410742},
-	{0x0000b110, 0x075f0740},
-	{0x0000b114, 0x077f0760},
-	{0x0000b118, 0x07800781},
-	{0x0000b11c, 0x07a0079f},
-	{0x0000b120, 0x07c107bf},
-	{0x0000b124, 0x000007c0},
-	{0x0000b128, 0x00000000},
-	{0x0000b12c, 0x00000000},
-	{0x0000b130, 0x00000000},
-	{0x0000b134, 0x00000000},
-	{0x0000b138, 0x00000000},
-	{0x0000b13c, 0x00000000},
-	{0x0000b140, 0x003f0020},
-	{0x0000b144, 0x00400041},
-	{0x0000b148, 0x0140005f},
-	{0x0000b14c, 0x0160015f},
-	{0x0000b150, 0x017e017f},
-	{0x0000b154, 0x02410242},
-	{0x0000b158, 0x025f0240},
-	{0x0000b15c, 0x027f0260},
-	{0x0000b160, 0x0341027e},
-	{0x0000b164, 0x035f0340},
-	{0x0000b168, 0x037f0360},
-	{0x0000b16c, 0x04400441},
-	{0x0000b170, 0x0460045f},
-	{0x0000b174, 0x0541047f},
-	{0x0000b178, 0x055f0540},
-	{0x0000b17c, 0x057f0560},
-	{0x0000b180, 0x06400641},
-	{0x0000b184, 0x0660065f},
-	{0x0000b188, 0x067e067f},
-	{0x0000b18c, 0x07410742},
-	{0x0000b190, 0x075f0740},
-	{0x0000b194, 0x077f0760},
-	{0x0000b198, 0x07800781},
-	{0x0000b19c, 0x07a0079f},
-	{0x0000b1a0, 0x07c107bf},
-	{0x0000b1a4, 0x000007c0},
-	{0x0000b1a8, 0x00000000},
-	{0x0000b1ac, 0x00000000},
-	{0x0000b1b0, 0x00000000},
-	{0x0000b1b4, 0x00000000},
-	{0x0000b1b8, 0x00000000},
-	{0x0000b1bc, 0x00000000},
-	{0x0000b1c0, 0x00000000},
-	{0x0000b1c4, 0x00000000},
-	{0x0000b1c8, 0x00000000},
-	{0x0000b1cc, 0x00000000},
-	{0x0000b1d0, 0x00000000},
-	{0x0000b1d4, 0x00000000},
-	{0x0000b1d8, 0x00000000},
-	{0x0000b1dc, 0x00000000},
-	{0x0000b1e0, 0x00000000},
-	{0x0000b1e4, 0x00000000},
-	{0x0000b1e8, 0x00000000},
-	{0x0000b1ec, 0x00000000},
-	{0x0000b1f0, 0x00000396},
-	{0x0000b1f4, 0x00000396},
-	{0x0000b1f8, 0x00000396},
-	{0x0000b1fc, 0x00000196},
-};
+#define ar9340Common_wo_xlna_rx_gain_table_1p0 ar9300Common_wo_xlna_rx_gain_table_2p2
 
 static const u32 ar9340_1p0_soc_preamble[][2] = {
-	/*    Addr        allmodes        */
-	{0x000040a4, 0x00a0c1c9},
+	/* Addr      allmodes  */
 	{0x00007008, 0x00000000},
 	{0x00007020, 0x00000000},
 	{0x00007034, 0x00000002},
 	{0x00007038, 0x000004c2},
 };
 
-#endif
+#endif /* INITVALS_9340_H */
diff --git a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
index 1d6658e139b..4ef7dcccaa2 100644
--- a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2010 Atheros Communications Inc.
+ * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -52,7 +53,7 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
 	{0x00009e04, 0x001c2020, 0x001c2020, 0x001c2020, 0x001c2020},
 	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000e2, 0x6c4000d8},
 	{0x00009e10, 0x92c88d2e, 0x7ec88d2e, 0x7ec84d2e, 0x7ec86d2e},
-	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3376605e, 0x33795d5e},
+	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3376605e, 0x32395d5e},
 	{0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 	{0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
 	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003ce, 0x000003ce},
@@ -61,7 +62,7 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
 	{0x00009e44, 0x62321e27, 0x62321e27, 0xfe291e27, 0xfe291e27},
 	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
 	{0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
-	{0x0000a204, 0x013187c0, 0x013187c4, 0x013187c4, 0x013187c0},
+	{0x0000a204, 0x01318fc0, 0x01318fc4, 0x01318fc4, 0x01318fc0},
 	{0x0000a208, 0x00000104, 0x00000104, 0x00000004, 0x00000004},
 	{0x0000a22c, 0x01026a2f, 0x01026a27, 0x01026a2f, 0x01026a2f},
 	{0x0000a230, 0x0000400a, 0x00004014, 0x00004016, 0x0000400b},
@@ -958,7 +959,7 @@ static const u32 ar9462_2p0_radio_core[][2] = {
 	{0x0001604c, 0x2699e04f},
 	{0x00016050, 0x6db6db6c},
 	{0x00016058, 0x6c200000},
-	{0x00016080, 0x00040000},
+	{0x00016080, 0x000c0000},
 	{0x00016084, 0x9a68048c},
 	{0x00016088, 0x54214514},
 	{0x0001608c, 0x1203040b},
@@ -981,7 +982,7 @@ static const u32 ar9462_2p0_radio_core[][2] = {
 	{0x00016144, 0x02084080},
 	{0x00016148, 0x000080c0},
 	{0x00016280, 0x050a0001},
-	{0x00016284, 0x3d841400},
+	{0x00016284, 0x3d841418},
 	{0x00016288, 0x00000000},
 	{0x0001628c, 0xe3000000},
 	{0x00016290, 0xa1005080},
@@ -1007,6 +1008,7 @@ static const u32 ar9462_2p0_radio_core[][2] = {
 
 static const u32 ar9462_2p0_soc_preamble[][2] = {
 	/* Addr      allmodes  */
+	{0x000040a4, 0x00a0c1c9},
 	{0x00007020, 0x00000000},
 	{0x00007034, 0x00000002},
 	{0x00007038, 0x000004c2},
diff --git a/drivers/net/wireless/ath/ath9k/ar9485_initvals.h b/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
index d16d029f81a..fb4497fc7a3 100644
--- a/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -17,360 +18,151 @@
 #ifndef INITVALS_9485_H
 #define INITVALS_9485_H
 
-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},
-};
+/* AR9485 1.0 */
 
-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},
-};
+#define ar9485_1_1_mac_postamble ar9300_2p2_mac_postamble
 
-static const u32 ar9485Common_1_1[][2] = {
-	/*  Addr      allmodes */
-	{0x00007010, 0x00000022},
-	{0x00007020, 0x00000000},
-	{0x00007034, 0x00000002},
-	{0x00007038, 0x000004c2},
+static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_disable_L1[][2] = {
+	/* Addr      allmodes  */
+	{0x00018c00, 0x18012e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
 };
 
-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 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},
 };
 
-static const u32 ar9485Modes_high_ob_db_tx_gain_1_1[][5] = {
-	/* Addr        5G_HT20       5G_HT40       2G_HT40       2G_HT20 */
+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},
@@ -442,102 +234,34 @@ static const u32 ar9485Modes_high_ob_db_tx_gain_1_1[][5] = {
 	{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, 0x5a001ceb, 0x5a001ceb},
-	{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},
-};
+#define ar9485Modes_high_ob_db_tx_gain_1_1 ar9485Modes_high_power_tx_gain_1_1
 
-static const u32 ar9485_1_1_radio_postamble[][2] = {
-	/* Addr        allmodes */
-	{0x0001609c, 0x0b283f31},
-	{0x000160ac, 0x24611800},
-	{0x000160b0, 0x03284f3e},
-	{0x0001610c, 0x00170000},
-	{0x00016140, 0x50804008},
-};
+#define ar9485Modes_low_ob_db_tx_gain_1_1 ar9485Modes_high_ob_db_tx_gain_1_1
 
-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},
+#define ar9485_modes_lowest_ob_db_tx_gain_1_1 ar9485Modes_low_ob_db_tx_gain_1_1
+
+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_1_1_radio_core[][2] = {
-	/* Addr        allmodes */
+	/* Addr      allmodes  */
 	{0x00016000, 0x36db6db6},
 	{0x00016004, 0x6db6db40},
 	{0x00016008, 0x73800000},
@@ -601,294 +325,145 @@ static const u32 ar9485_1_1_radio_core[][2] = {
 	{0x00016c44, 0x12000000},
 };
 
-static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_enable_L1[][2] = {
-	/* Addr        allmodes */
-	{0x00018c00, 0x18052e5e},
-	{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, 0x5a001ceb, 0x5a001ceb},
-	{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, 0x5a001ceb, 0x5a001ceb},
-	{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, 0x18013e5e},
-	{0x00018c04, 0x000801d8},
-	{0x00018c08, 0x0000080c},
-};
-
-static const u32 ar9485_1_1_soc_preamble[][2] = {
-	/* Addr        allmodes */
-	{0x00004014, 0xba280400},
-	{0x00004090, 0x00aa10aa},
-	{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, 0x5a001ceb, 0x5a001ceb},
-	{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, 0x18012e5e},
-	{0x00018c04, 0x000801d8},
-	{0x00018c08, 0x0000080c},
+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 ar9485_common_rx_gain_1_1[][2] = {
-	/* Addr        allmodes */
+	/* Addr      allmodes  */
 	{0x0000a000, 0x00010000},
 	{0x0000a004, 0x00030002},
 	{0x0000a008, 0x00050004},
@@ -1019,143 +594,260 @@ static const u32 ar9485_common_rx_gain_1_1[][2] = {
 	{0x0000a1fc, 0x00000296},
 };
 
+static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_enable_L1[][2] = {
+	/* Addr      allmodes  */
+	{0x00018c00, 0x18052e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
+};
+
 static const u32 ar9485_1_1_pcie_phy_clkreq_enable_L1[][2] = {
-	/* Addr        allmodes */
+	/* Addr      allmodes  */
 	{0x00018c00, 0x18053e5e},
 	{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},
+static const u32 ar9485_1_1_soc_preamble[][2] = {
+	/* Addr      allmodes  */
+	{0x00004014, 0xba280400},
+	{0x00004090, 0x00aa10aa},
+	{0x000040a4, 0x00a0c9c9},
+	{0x00007010, 0x00000022},
+	{0x00007020, 0x00000000},
+	{0x00007034, 0x00000002},
+	{0x00007038, 0x000004c2},
+	{0x00007048, 0x00000002},
+};
+
+static const u32 ar9485_fast_clock_1_1_baseband_postamble[][3] = {
+	/* Addr      5G_HT20     5G_HT40   */
+	{0x00009e00, 0x03721821, 0x03721821},
+	{0x0000a230, 0x0000400b, 0x00004016},
+	{0x0000a254, 0x00000898, 0x00001130},
+};
+
+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 ar9485_1_1_pcie_phy_clkreq_disable_L1[][2] = {
+	/* Addr      allmodes  */
+	{0x00018c00, 0x18013e5e},
+	{0x00018c04, 0x000801d8},
+	{0x00018c08, 0x0000080c},
+};
+
+static const u32 ar9485_1_1_radio_postamble[][2] = {
+	/* Addr      allmodes  */
+	{0x0001609c, 0x0b283f31},
+	{0x000160ac, 0x24611800},
+	{0x000160b0, 0x03284f3e},
+	{0x0001610c, 0x00170000},
+	{0x00016140, 0x50804008},
+};
+
+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},
 };
 
-#endif
+#endif /* INITVALS_9485_H */
diff --git a/drivers/net/wireless/ath/ath9k/ar955x_1p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar955x_1p0_initvals.h
new file mode 100644
index 00000000000..df97f21c52d
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/ar955x_1p0_initvals.h
@@ -0,0 +1,1284 @@
+/*
+ * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef INITVALS_955X_1P0_H
+#define INITVALS_955X_1P0_H
+
+/* AR955X 1.0 */
+
+static const u32 ar955x_1p0_radio_postamble[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00016098, 0xd2dd5554, 0xd2dd5554, 0xd28b3330, 0xd28b3330},
+	{0x0001609c, 0x0a566f3a, 0x0a566f3a, 0x06345f2a, 0x06345f2a},
+	{0x000160ac, 0xa4647c00, 0xa4647c00, 0xa4646800, 0xa4646800},
+	{0x000160b0, 0x01885f52, 0x01885f52, 0x04accf3a, 0x04accf3a},
+	{0x00016104, 0xb7a00001, 0xb7a00001, 0xb7a00001, 0xb7a00001},
+	{0x0001610c, 0xc0000000, 0xc0000000, 0xc0000000, 0xc0000000},
+	{0x00016140, 0x10804008, 0x10804008, 0x10804008, 0x10804008},
+	{0x00016504, 0xb7a00001, 0xb7a00001, 0xb7a00001, 0xb7a00001},
+	{0x0001650c, 0xc0000000, 0xc0000000, 0xc0000000, 0xc0000000},
+	{0x00016540, 0x10804008, 0x10804008, 0x10804008, 0x10804008},
+	{0x00016904, 0xb7a00001, 0xb7a00001, 0xb7a00001, 0xb7a00001},
+	{0x0001690c, 0xc0000000, 0xc0000000, 0xc0000000, 0xc0000000},
+	{0x00016940, 0x10804008, 0x10804008, 0x10804008, 0x10804008},
+};
+
+static const u32 ar955x_1p0_baseband_core_txfir_coeff_japan_2484[][2] = {
+	/* Addr      allmodes  */
+	{0x0000a398, 0x00000000},
+	{0x0000a39c, 0x6f7f0301},
+	{0x0000a3a0, 0xca9228ee},
+};
+
+static const u32 ar955x_1p0_baseband_postamble[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00009810, 0xd00a8005, 0xd00a8005, 0xd00a8011, 0xd00a8011},
+	{0x00009820, 0x206a022e, 0x206a022e, 0x206a012e, 0x206a012e},
+	{0x00009824, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0, 0x5ac640d0},
+	{0x00009828, 0x06903081, 0x06903081, 0x06903881, 0x06903881},
+	{0x0000982c, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4, 0x05eea6d4},
+	{0x00009830, 0x0000059c, 0x0000059c, 0x0000119c, 0x0000119c},
+	{0x00009c00, 0x000000c4, 0x000000c4, 0x000000c4, 0x000000c4},
+	{0x00009e00, 0x0372111a, 0x0372111a, 0x037216a0, 0x037216a0},
+	{0x00009e04, 0x001c2020, 0x001c2020, 0x001c2020, 0x001c2020},
+	{0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000e2, 0x6c4000e2},
+	{0x00009e10, 0x7ec88d2e, 0x7ec88d2e, 0x7ec84d2e, 0x7ec84d2e},
+	{0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3379605e, 0x33795d5e},
+	{0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
+	{0x00009e20, 0x000003b5, 0x000003b5, 0x000003ce, 0x000003ce},
+	{0x00009e2c, 0x0000001c, 0x0000001c, 0x00000021, 0x00000021},
+	{0x00009e3c, 0xcfa10820, 0xcfa10820, 0xcfa10822, 0xcfa10822},
+	{0x00009e44, 0xfe321e27, 0xfe321e27, 0xfe291e27, 0xfe291e27},
+	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
+	{0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
+	{0x0000a204, 0x005c0ec0, 0x005c0ec4, 0x005c0ec4, 0x005c0ec0},
+	{0x0000a208, 0x00000104, 0x00000104, 0x00000004, 0x00000004},
+	{0x0000a22c, 0x07e26a2f, 0x07e26a2f, 0x01026a2f, 0x01026a2f},
+	{0x0000a230, 0x0000000a, 0x00000014, 0x00000016, 0x0000000b},
+	{0x0000a234, 0x00000fff, 0x10000fff, 0x10000fff, 0x00000fff},
+	{0x0000a238, 0xffb01018, 0xffb01018, 0xffb01018, 0xffb01018},
+	{0x0000a250, 0x00000000, 0x00000000, 0x00000210, 0x00000108},
+	{0x0000a254, 0x000007d0, 0x00000fa0, 0x00001130, 0x00000898},
+	{0x0000a258, 0x02020002, 0x02020002, 0x02020002, 0x02020002},
+	{0x0000a25c, 0x01000e0e, 0x01000e0e, 0x01000e0e, 0x01000e0e},
+	{0x0000a260, 0x0a021501, 0x0a021501, 0x3a021501, 0x3a021501},
+	{0x0000a264, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0x00000e0e},
+	{0x0000a280, 0x00000007, 0x00000007, 0x0000000b, 0x0000000b},
+	{0x0000a284, 0x00000000, 0x00000000, 0x00000010, 0x00000010},
+	{0x0000a288, 0x00000110, 0x00000110, 0x00000110, 0x00000110},
+	{0x0000a28c, 0x00022222, 0x00022222, 0x00022222, 0x00022222},
+	{0x0000a2c4, 0x00158d18, 0x00158d18, 0x00158d18, 0x00158d18},
+	{0x0000a2cc, 0x18c50033, 0x18c43433, 0x18c41033, 0x18c44c33},
+	{0x0000a2d0, 0x00041982, 0x00041982, 0x00041982, 0x00041982},
+	{0x0000a2d8, 0x7999a83b, 0x7999a83b, 0x7999a83b, 0x7999a83b},
+	{0x0000a358, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a830, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000ae04, 0x001c0000, 0x001c0000, 0x001c0000, 0x001c0000},
+	{0x0000ae18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000ae1c, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000ae20, 0x000001b5, 0x000001b5, 0x000001ce, 0x000001ce},
+	{0x0000b284, 0x00000000, 0x00000000, 0x00000010, 0x00000010},
+	{0x0000b830, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000be04, 0x001c0000, 0x001c0000, 0x001c0000, 0x001c0000},
+	{0x0000be18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000be1c, 0x0000019c, 0x0000019c, 0x0000019c, 0x0000019c},
+	{0x0000be20, 0x000001b5, 0x000001b5, 0x000001ce, 0x000001ce},
+	{0x0000c284, 0x00000000, 0x00000000, 0x00000010, 0x00000010},
+};
+
+static const u32 ar955x_1p0_radio_core[][2] = {
+	/* Addr      allmodes  */
+	{0x00016000, 0x36db6db6},
+	{0x00016004, 0x6db6db40},
+	{0x00016008, 0x73f00000},
+	{0x0001600c, 0x00000000},
+	{0x00016040, 0x7f80fff8},
+	{0x0001604c, 0x76d005b5},
+	{0x00016050, 0x557cf031},
+	{0x00016054, 0x13449440},
+	{0x00016058, 0x0c51c92c},
+	{0x0001605c, 0x3db7fffc},
+	{0x00016060, 0xfffffffc},
+	{0x00016064, 0x000f0278},
+	{0x00016068, 0x6db6db6c},
+	{0x0001606c, 0x6db60000},
+	{0x00016080, 0x00080000},
+	{0x00016084, 0x0e48048c},
+	{0x00016088, 0x14214514},
+	{0x0001608c, 0x119f101e},
+	{0x00016090, 0x24926490},
+	{0x00016094, 0x00000000},
+	{0x000160a0, 0x0a108ffe},
+	{0x000160a4, 0x812fc370},
+	{0x000160a8, 0x423c8000},
+	{0x000160b4, 0x92480080},
+	{0x000160c0, 0x006db6d0},
+	{0x000160c4, 0x6db6db60},
+	{0x000160c8, 0x6db6db6c},
+	{0x000160cc, 0x01e6c000},
+	{0x00016100, 0x11999601},
+	{0x00016108, 0x00080010},
+	{0x00016144, 0x02084080},
+	{0x00016148, 0x000080c0},
+	{0x00016280, 0x01800804},
+	{0x00016284, 0x00038dc5},
+	{0x00016288, 0x00000000},
+	{0x0001628c, 0x00000040},
+	{0x00016380, 0x00000000},
+	{0x00016384, 0x00000000},
+	{0x00016388, 0x00400705},
+	{0x0001638c, 0x00800700},
+	{0x00016390, 0x00800700},
+	{0x00016394, 0x00000000},
+	{0x00016398, 0x00000000},
+	{0x0001639c, 0x00000000},
+	{0x000163a0, 0x00000001},
+	{0x000163a4, 0x00000001},
+	{0x000163a8, 0x00000000},
+	{0x000163ac, 0x00000000},
+	{0x000163b0, 0x00000000},
+	{0x000163b4, 0x00000000},
+	{0x000163b8, 0x00000000},
+	{0x000163bc, 0x00000000},
+	{0x000163c0, 0x000000a0},
+	{0x000163c4, 0x000c0000},
+	{0x000163c8, 0x14021402},
+	{0x000163cc, 0x00001402},
+	{0x000163d0, 0x00000000},
+	{0x000163d4, 0x00000000},
+	{0x00016400, 0x36db6db6},
+	{0x00016404, 0x6db6db40},
+	{0x00016408, 0x73f00000},
+	{0x0001640c, 0x00000000},
+	{0x00016440, 0x7f80fff8},
+	{0x0001644c, 0x76d005b5},
+	{0x00016450, 0x557cf031},
+	{0x00016454, 0x13449440},
+	{0x00016458, 0x0c51c92c},
+	{0x0001645c, 0x3db7fffc},
+	{0x00016460, 0xfffffffc},
+	{0x00016464, 0x000f0278},
+	{0x00016468, 0x6db6db6c},
+	{0x0001646c, 0x6db60000},
+	{0x00016500, 0x11999601},
+	{0x00016508, 0x00080010},
+	{0x00016544, 0x02084080},
+	{0x00016548, 0x000080c0},
+	{0x00016780, 0x00000000},
+	{0x00016784, 0x00000000},
+	{0x00016788, 0x00400705},
+	{0x0001678c, 0x00800700},
+	{0x00016790, 0x00800700},
+	{0x00016794, 0x00000000},
+	{0x00016798, 0x00000000},
+	{0x0001679c, 0x00000000},
+	{0x000167a0, 0x00000001},
+	{0x000167a4, 0x00000001},
+	{0x000167a8, 0x00000000},
+	{0x000167ac, 0x00000000},
+	{0x000167b0, 0x00000000},
+	{0x000167b4, 0x00000000},
+	{0x000167b8, 0x00000000},
+	{0x000167bc, 0x00000000},
+	{0x000167c0, 0x000000a0},
+	{0x000167c4, 0x000c0000},
+	{0x000167c8, 0x14021402},
+	{0x000167cc, 0x00001402},
+	{0x000167d0, 0x00000000},
+	{0x000167d4, 0x00000000},
+	{0x00016800, 0x36db6db6},
+	{0x00016804, 0x6db6db40},
+	{0x00016808, 0x73f00000},
+	{0x0001680c, 0x00000000},
+	{0x00016840, 0x7f80fff8},
+	{0x0001684c, 0x76d005b5},
+	{0x00016850, 0x557cf031},
+	{0x00016854, 0x13449440},
+	{0x00016858, 0x0c51c92c},
+	{0x0001685c, 0x3db7fffc},
+	{0x00016860, 0xfffffffc},
+	{0x00016864, 0x000f0278},
+	{0x00016868, 0x6db6db6c},
+	{0x0001686c, 0x6db60000},
+	{0x00016900, 0x11999601},
+	{0x00016908, 0x00080010},
+	{0x00016944, 0x02084080},
+	{0x00016948, 0x000080c0},
+	{0x00016b80, 0x00000000},
+	{0x00016b84, 0x00000000},
+	{0x00016b88, 0x00400705},
+	{0x00016b8c, 0x00800700},
+	{0x00016b90, 0x00800700},
+	{0x00016b94, 0x00000000},
+	{0x00016b98, 0x00000000},
+	{0x00016b9c, 0x00000000},
+	{0x00016ba0, 0x00000001},
+	{0x00016ba4, 0x00000001},
+	{0x00016ba8, 0x00000000},
+	{0x00016bac, 0x00000000},
+	{0x00016bb0, 0x00000000},
+	{0x00016bb4, 0x00000000},
+	{0x00016bb8, 0x00000000},
+	{0x00016bbc, 0x00000000},
+	{0x00016bc0, 0x000000a0},
+	{0x00016bc4, 0x000c0000},
+	{0x00016bc8, 0x14021402},
+	{0x00016bcc, 0x00001402},
+	{0x00016bd0, 0x00000000},
+	{0x00016bd4, 0x00000000},
+};
+
+static const u32 ar955x_1p0_modes_xpa_tx_gain_table[][9] = {
+	/* Addr      5G_HT20_L   5G_HT40_L   5G_HT20_M   5G_HT40_M   5G_HT20_H   5G_HT40_H   2G_HT40     2G_HT20  */
+	{0x0000a2dc, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xfffd5aaa, 0xfffd5aaa},
+	{0x0000a2e0, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xfffe9ccc, 0xfffe9ccc},
+	{0x0000a2e4, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xffffe0f0, 0xffffe0f0},
+	{0x0000a2e8, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xfffcff00, 0xfffcff00},
+	{0x0000a410, 0x000050de, 0x000050de, 0x000050de, 0x000050de, 0x000050de, 0x000050de, 0x000050da, 0x000050da},
+	{0x0000a500, 0x00000003, 0x00000003, 0x00000003, 0x00000003, 0x00000003, 0x00000003, 0x00000000, 0x00000000},
+	{0x0000a504, 0x04000005, 0x04000005, 0x04000005, 0x04000005, 0x04000005, 0x04000005, 0x04000002, 0x04000002},
+	{0x0000a508, 0x08000009, 0x08000009, 0x08000009, 0x08000009, 0x08000009, 0x08000009, 0x08000004, 0x08000004},
+	{0x0000a50c, 0x0c00000b, 0x0c00000b, 0x0c00000b, 0x0c00000b, 0x0c00000b, 0x0c00000b, 0x0c000006, 0x0c000006},
+	{0x0000a510, 0x1000000d, 0x1000000d, 0x1000000d, 0x1000000d, 0x1000000d, 0x1000000d, 0x0f00000a, 0x0f00000a},
+	{0x0000a514, 0x14000011, 0x14000011, 0x14000011, 0x14000011, 0x14000011, 0x14000011, 0x1300000c, 0x1300000c},
+	{0x0000a518, 0x19004008, 0x19004008, 0x19004008, 0x19004008, 0x18004008, 0x18004008, 0x1700000e, 0x1700000e},
+	{0x0000a51c, 0x1d00400a, 0x1d00400a, 0x1d00400a, 0x1d00400a, 0x1c00400a, 0x1c00400a, 0x1b000064, 0x1b000064},
+	{0x0000a520, 0x230020a2, 0x230020a2, 0x210020a2, 0x210020a2, 0x200020a2, 0x200020a2, 0x1f000242, 0x1f000242},
+	{0x0000a524, 0x2500006e, 0x2500006e, 0x2500006e, 0x2500006e, 0x2400006e, 0x2400006e, 0x23000229, 0x23000229},
+	{0x0000a528, 0x29022221, 0x29022221, 0x28022221, 0x28022221, 0x27022221, 0x27022221, 0x270002a2, 0x270002a2},
+	{0x0000a52c, 0x2d00062a, 0x2d00062a, 0x2c00062a, 0x2c00062a, 0x2a00062a, 0x2a00062a, 0x2c001203, 0x2c001203},
+	{0x0000a530, 0x340220a5, 0x340220a5, 0x320220a5, 0x320220a5, 0x2f0220a5, 0x2f0220a5, 0x30001803, 0x30001803},
+	{0x0000a534, 0x380022c5, 0x380022c5, 0x350022c5, 0x350022c5, 0x320022c5, 0x320022c5, 0x33000881, 0x33000881},
+	{0x0000a538, 0x3b002486, 0x3b002486, 0x39002486, 0x39002486, 0x36002486, 0x36002486, 0x38001809, 0x38001809},
+	{0x0000a53c, 0x3f00248a, 0x3f00248a, 0x3d00248a, 0x3d00248a, 0x3a00248a, 0x3a00248a, 0x3a000814, 0x3a000814},
+	{0x0000a540, 0x4202242c, 0x4202242c, 0x4102242c, 0x4102242c, 0x3f02242c, 0x3f02242c, 0x3f001a0c, 0x3f001a0c},
+	{0x0000a544, 0x490044c6, 0x490044c6, 0x460044c6, 0x460044c6, 0x420044c6, 0x420044c6, 0x43001a0e, 0x43001a0e},
+	{0x0000a548, 0x4d024485, 0x4d024485, 0x4a024485, 0x4a024485, 0x46024485, 0x46024485, 0x46001812, 0x46001812},
+	{0x0000a54c, 0x51044483, 0x51044483, 0x4e044483, 0x4e044483, 0x4a044483, 0x4a044483, 0x49001884, 0x49001884},
+	{0x0000a550, 0x5404a40c, 0x5404a40c, 0x5204a40c, 0x5204a40c, 0x4d04a40c, 0x4d04a40c, 0x4d001e84, 0x4d001e84},
+	{0x0000a554, 0x57024632, 0x57024632, 0x55024632, 0x55024632, 0x52024632, 0x52024632, 0x50001e69, 0x50001e69},
+	{0x0000a558, 0x5c00a634, 0x5c00a634, 0x5900a634, 0x5900a634, 0x5600a634, 0x5600a634, 0x550006f4, 0x550006f4},
+	{0x0000a55c, 0x5f026832, 0x5f026832, 0x5d026832, 0x5d026832, 0x5a026832, 0x5a026832, 0x59000ad3, 0x59000ad3},
+	{0x0000a560, 0x6602b012, 0x6602b012, 0x6202b012, 0x6202b012, 0x5d02b012, 0x5d02b012, 0x5e000ad5, 0x5e000ad5},
+	{0x0000a564, 0x6e02d0e1, 0x6e02d0e1, 0x6802d0e1, 0x6802d0e1, 0x6002d0e1, 0x6002d0e1, 0x61001ced, 0x61001ced},
+	{0x0000a568, 0x7202b4c4, 0x7202b4c4, 0x6c02b4c4, 0x6c02b4c4, 0x6502b4c4, 0x6502b4c4, 0x660018d4, 0x660018d4},
+	{0x0000a56c, 0x75007894, 0x75007894, 0x70007894, 0x70007894, 0x6b007894, 0x6b007894, 0x660018d4, 0x660018d4},
+	{0x0000a570, 0x7b025c74, 0x7b025c74, 0x75025c74, 0x75025c74, 0x70025c74, 0x70025c74, 0x660018d4, 0x660018d4},
+	{0x0000a574, 0x8300bcb5, 0x8300bcb5, 0x7a00bcb5, 0x7a00bcb5, 0x7600bcb5, 0x7600bcb5, 0x660018d4, 0x660018d4},
+	{0x0000a578, 0x8a04dc74, 0x8a04dc74, 0x7f04dc74, 0x7f04dc74, 0x7c04dc74, 0x7c04dc74, 0x660018d4, 0x660018d4},
+	{0x0000a57c, 0x8a04dc74, 0x8a04dc74, 0x7f04dc74, 0x7f04dc74, 0x7c04dc74, 0x7c04dc74, 0x660018d4, 0x660018d4},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x03804000, 0x03804000},
+	{0x0000a610, 0x04c08c01, 0x04c08c01, 0x04808b01, 0x04808b01, 0x04808a01, 0x04808a01, 0x0300ca02, 0x0300ca02},
+	{0x0000a614, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00000e04, 0x00000e04},
+	{0x0000a618, 0x04010c01, 0x04010c01, 0x03c10b01, 0x03c10b01, 0x03810a01, 0x03810a01, 0x03014000, 0x03014000},
+	{0x0000a61c, 0x03814e05, 0x03814e05, 0x03414d05, 0x03414d05, 0x03414d05, 0x03414d05, 0x00000000, 0x00000000},
+	{0x0000a620, 0x04010303, 0x04010303, 0x03c10303, 0x03c10303, 0x03810303, 0x03810303, 0x00000000, 0x00000000},
+	{0x0000a624, 0x03814e05, 0x03814e05, 0x03414d05, 0x03414d05, 0x03414d05, 0x03414d05, 0x03014000, 0x03014000},
+	{0x0000a628, 0x00c0c000, 0x00c0c000, 0x00c0c000, 0x00c0c000, 0x00c0c000, 0x00c0c000, 0x03804c05, 0x03804c05},
+	{0x0000a62c, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x00c0c303, 0x0701de06, 0x0701de06},
+	{0x0000a630, 0x03418000, 0x03418000, 0x03018000, 0x03018000, 0x02c18000, 0x02c18000, 0x07819c07, 0x07819c07},
+	{0x0000a634, 0x03815004, 0x03815004, 0x03414f04, 0x03414f04, 0x03414e04, 0x03414e04, 0x0701dc07, 0x0701dc07},
+	{0x0000a638, 0x03005302, 0x03005302, 0x02c05202, 0x02c05202, 0x02805202, 0x02805202, 0x0701dc07, 0x0701dc07},
+	{0x0000a63c, 0x04c09302, 0x04c09302, 0x04809202, 0x04809202, 0x04809202, 0x04809202, 0x0701dc07, 0x0701dc07},
+	{0x0000b2dc, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xfffd5aaa, 0xfffd5aaa},
+	{0x0000b2e0, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xfffe9ccc, 0xfffe9ccc},
+	{0x0000b2e4, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xffffe0f0, 0xffffe0f0},
+	{0x0000b2e8, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xfffcff00, 0xfffcff00},
+	{0x0000c2dc, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xffffaaaa, 0xfffd5aaa, 0xfffd5aaa},
+	{0x0000c2e0, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xffffcccc, 0xfffe9ccc, 0xfffe9ccc},
+	{0x0000c2e4, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xfffff0f0, 0xffffe0f0, 0xffffe0f0},
+	{0x0000c2e8, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xffffff00, 0xfffcff00, 0xfffcff00},
+	{0x00016044, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x010002d4, 0x010002d4},
+	{0x00016048, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x66482401, 0x66482401},
+	{0x00016280, 0x01801e84, 0x01801e84, 0x01801e84, 0x01801e84, 0x01801e84, 0x01801e84, 0x01808e84, 0x01808e84},
+	{0x00016444, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x010002d4, 0x010002d4},
+	{0x00016448, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x66482401, 0x66482401},
+	{0x00016844, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x010002d4, 0x010002d4},
+	{0x00016848, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x62482401, 0x66482401, 0x66482401},
+};
+
+static const u32 ar955x_1p0_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},
+	{0x00008140, 0x000000fe},
+	{0x00008144, 0xffffffff},
+	{0x00008168, 0x00000000},
+	{0x0000816c, 0x00000000},
+	{0x000081c0, 0x00000000},
+	{0x000081c4, 0x33332210},
+	{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, 0x9d400010},
+	{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, 0x00001d40},
+	{0x00008314, 0x00000000},
+	{0x0000831c, 0x0000010d},
+	{0x00008328, 0x00000000},
+	{0x0000832c, 0x0000001f},
+	{0x00008330, 0x00000302},
+	{0x00008334, 0x00000700},
+	{0x00008338, 0xffff0000},
+	{0x0000833c, 0x02400000},
+	{0x00008340, 0x000107ff},
+	{0x00008344, 0xaa48107b},
+	{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, 0x8c7901ff},
+};
+
+static const u32 ar955x_1p0_common_rx_gain_table[][2] = {
+	/* Addr      allmodes  */
+	{0x0000a000, 0x00010000},
+	{0x0000a004, 0x00030002},
+	{0x0000a008, 0x00050004},
+	{0x0000a00c, 0x00810080},
+	{0x0000a010, 0x00830082},
+	{0x0000a014, 0x01810180},
+	{0x0000a018, 0x01830182},
+	{0x0000a01c, 0x01850184},
+	{0x0000a020, 0x01890188},
+	{0x0000a024, 0x018b018a},
+	{0x0000a028, 0x018d018c},
+	{0x0000a02c, 0x01910190},
+	{0x0000a030, 0x01930192},
+	{0x0000a034, 0x01950194},
+	{0x0000a038, 0x038a0196},
+	{0x0000a03c, 0x038c038b},
+	{0x0000a040, 0x0390038d},
+	{0x0000a044, 0x03920391},
+	{0x0000a048, 0x03940393},
+	{0x0000a04c, 0x03960395},
+	{0x0000a050, 0x00000000},
+	{0x0000a054, 0x00000000},
+	{0x0000a058, 0x00000000},
+	{0x0000a05c, 0x00000000},
+	{0x0000a060, 0x00000000},
+	{0x0000a064, 0x00000000},
+	{0x0000a068, 0x00000000},
+	{0x0000a06c, 0x00000000},
+	{0x0000a070, 0x00000000},
+	{0x0000a074, 0x00000000},
+	{0x0000a078, 0x00000000},
+	{0x0000a07c, 0x00000000},
+	{0x0000a080, 0x22222229},
+	{0x0000a084, 0x1d1d1d1d},
+	{0x0000a088, 0x1d1d1d1d},
+	{0x0000a08c, 0x1d1d1d1d},
+	{0x0000a090, 0x171d1d1d},
+	{0x0000a094, 0x11111717},
+	{0x0000a098, 0x00030311},
+	{0x0000a09c, 0x00000000},
+	{0x0000a0a0, 0x00000000},
+	{0x0000a0a4, 0x00000000},
+	{0x0000a0a8, 0x00000000},
+	{0x0000a0ac, 0x00000000},
+	{0x0000a0b0, 0x00000000},
+	{0x0000a0b4, 0x00000000},
+	{0x0000a0b8, 0x00000000},
+	{0x0000a0bc, 0x00000000},
+	{0x0000a0c0, 0x001f0000},
+	{0x0000a0c4, 0x01000101},
+	{0x0000a0c8, 0x011e011f},
+	{0x0000a0cc, 0x011c011d},
+	{0x0000a0d0, 0x02030204},
+	{0x0000a0d4, 0x02010202},
+	{0x0000a0d8, 0x021f0200},
+	{0x0000a0dc, 0x0302021e},
+	{0x0000a0e0, 0x03000301},
+	{0x0000a0e4, 0x031e031f},
+	{0x0000a0e8, 0x0402031d},
+	{0x0000a0ec, 0x04000401},
+	{0x0000a0f0, 0x041e041f},
+	{0x0000a0f4, 0x0502041d},
+	{0x0000a0f8, 0x05000501},
+	{0x0000a0fc, 0x051e051f},
+	{0x0000a100, 0x06010602},
+	{0x0000a104, 0x061f0600},
+	{0x0000a108, 0x061d061e},
+	{0x0000a10c, 0x07020703},
+	{0x0000a110, 0x07000701},
+	{0x0000a114, 0x00000000},
+	{0x0000a118, 0x00000000},
+	{0x0000a11c, 0x00000000},
+	{0x0000a120, 0x00000000},
+	{0x0000a124, 0x00000000},
+	{0x0000a128, 0x00000000},
+	{0x0000a12c, 0x00000000},
+	{0x0000a130, 0x00000000},
+	{0x0000a134, 0x00000000},
+	{0x0000a138, 0x00000000},
+	{0x0000a13c, 0x00000000},
+	{0x0000a140, 0x001f0000},
+	{0x0000a144, 0x01000101},
+	{0x0000a148, 0x011e011f},
+	{0x0000a14c, 0x011c011d},
+	{0x0000a150, 0x02030204},
+	{0x0000a154, 0x02010202},
+	{0x0000a158, 0x021f0200},
+	{0x0000a15c, 0x0302021e},
+	{0x0000a160, 0x03000301},
+	{0x0000a164, 0x031e031f},
+	{0x0000a168, 0x0402031d},
+	{0x0000a16c, 0x04000401},
+	{0x0000a170, 0x041e041f},
+	{0x0000a174, 0x0502041d},
+	{0x0000a178, 0x05000501},
+	{0x0000a17c, 0x051e051f},
+	{0x0000a180, 0x06010602},
+	{0x0000a184, 0x061f0600},
+	{0x0000a188, 0x061d061e},
+	{0x0000a18c, 0x07020703},
+	{0x0000a190, 0x07000701},
+	{0x0000a194, 0x00000000},
+	{0x0000a198, 0x00000000},
+	{0x0000a19c, 0x00000000},
+	{0x0000a1a0, 0x00000000},
+	{0x0000a1a4, 0x00000000},
+	{0x0000a1a8, 0x00000000},
+	{0x0000a1ac, 0x00000000},
+	{0x0000a1b0, 0x00000000},
+	{0x0000a1b4, 0x00000000},
+	{0x0000a1b8, 0x00000000},
+	{0x0000a1bc, 0x00000000},
+	{0x0000a1c0, 0x00000000},
+	{0x0000a1c4, 0x00000000},
+	{0x0000a1c8, 0x00000000},
+	{0x0000a1cc, 0x00000000},
+	{0x0000a1d0, 0x00000000},
+	{0x0000a1d4, 0x00000000},
+	{0x0000a1d8, 0x00000000},
+	{0x0000a1dc, 0x00000000},
+	{0x0000a1e0, 0x00000000},
+	{0x0000a1e4, 0x00000000},
+	{0x0000a1e8, 0x00000000},
+	{0x0000a1ec, 0x00000000},
+	{0x0000a1f0, 0x00000396},
+	{0x0000a1f4, 0x00000396},
+	{0x0000a1f8, 0x00000396},
+	{0x0000a1fc, 0x00000196},
+	{0x0000b000, 0x00010000},
+	{0x0000b004, 0x00030002},
+	{0x0000b008, 0x00050004},
+	{0x0000b00c, 0x00810080},
+	{0x0000b010, 0x00830082},
+	{0x0000b014, 0x01810180},
+	{0x0000b018, 0x01830182},
+	{0x0000b01c, 0x01850184},
+	{0x0000b020, 0x02810280},
+	{0x0000b024, 0x02830282},
+	{0x0000b028, 0x02850284},
+	{0x0000b02c, 0x02890288},
+	{0x0000b030, 0x028b028a},
+	{0x0000b034, 0x0388028c},
+	{0x0000b038, 0x038a0389},
+	{0x0000b03c, 0x038c038b},
+	{0x0000b040, 0x0390038d},
+	{0x0000b044, 0x03920391},
+	{0x0000b048, 0x03940393},
+	{0x0000b04c, 0x03960395},
+	{0x0000b050, 0x00000000},
+	{0x0000b054, 0x00000000},
+	{0x0000b058, 0x00000000},
+	{0x0000b05c, 0x00000000},
+	{0x0000b060, 0x00000000},
+	{0x0000b064, 0x00000000},
+	{0x0000b068, 0x00000000},
+	{0x0000b06c, 0x00000000},
+	{0x0000b070, 0x00000000},
+	{0x0000b074, 0x00000000},
+	{0x0000b078, 0x00000000},
+	{0x0000b07c, 0x00000000},
+	{0x0000b080, 0x23232323},
+	{0x0000b084, 0x21232323},
+	{0x0000b088, 0x19191c1e},
+	{0x0000b08c, 0x12141417},
+	{0x0000b090, 0x07070e0e},
+	{0x0000b094, 0x03030305},
+	{0x0000b098, 0x00000003},
+	{0x0000b09c, 0x00000000},
+	{0x0000b0a0, 0x00000000},
+	{0x0000b0a4, 0x00000000},
+	{0x0000b0a8, 0x00000000},
+	{0x0000b0ac, 0x00000000},
+	{0x0000b0b0, 0x00000000},
+	{0x0000b0b4, 0x00000000},
+	{0x0000b0b8, 0x00000000},
+	{0x0000b0bc, 0x00000000},
+	{0x0000b0c0, 0x003f0020},
+	{0x0000b0c4, 0x00400041},
+	{0x0000b0c8, 0x0140005f},
+	{0x0000b0cc, 0x0160015f},
+	{0x0000b0d0, 0x017e017f},
+	{0x0000b0d4, 0x02410242},
+	{0x0000b0d8, 0x025f0240},
+	{0x0000b0dc, 0x027f0260},
+	{0x0000b0e0, 0x0341027e},
+	{0x0000b0e4, 0x035f0340},
+	{0x0000b0e8, 0x037f0360},
+	{0x0000b0ec, 0x04400441},
+	{0x0000b0f0, 0x0460045f},
+	{0x0000b0f4, 0x0541047f},
+	{0x0000b0f8, 0x055f0540},
+	{0x0000b0fc, 0x057f0560},
+	{0x0000b100, 0x06400641},
+	{0x0000b104, 0x0660065f},
+	{0x0000b108, 0x067e067f},
+	{0x0000b10c, 0x07410742},
+	{0x0000b110, 0x075f0740},
+	{0x0000b114, 0x077f0760},
+	{0x0000b118, 0x07800781},
+	{0x0000b11c, 0x07a0079f},
+	{0x0000b120, 0x07c107bf},
+	{0x0000b124, 0x000007c0},
+	{0x0000b128, 0x00000000},
+	{0x0000b12c, 0x00000000},
+	{0x0000b130, 0x00000000},
+	{0x0000b134, 0x00000000},
+	{0x0000b138, 0x00000000},
+	{0x0000b13c, 0x00000000},
+	{0x0000b140, 0x003f0020},
+	{0x0000b144, 0x00400041},
+	{0x0000b148, 0x0140005f},
+	{0x0000b14c, 0x0160015f},
+	{0x0000b150, 0x017e017f},
+	{0x0000b154, 0x02410242},
+	{0x0000b158, 0x025f0240},
+	{0x0000b15c, 0x027f0260},
+	{0x0000b160, 0x0341027e},
+	{0x0000b164, 0x035f0340},
+	{0x0000b168, 0x037f0360},
+	{0x0000b16c, 0x04400441},
+	{0x0000b170, 0x0460045f},
+	{0x0000b174, 0x0541047f},
+	{0x0000b178, 0x055f0540},
+	{0x0000b17c, 0x057f0560},
+	{0x0000b180, 0x06400641},
+	{0x0000b184, 0x0660065f},
+	{0x0000b188, 0x067e067f},
+	{0x0000b18c, 0x07410742},
+	{0x0000b190, 0x075f0740},
+	{0x0000b194, 0x077f0760},
+	{0x0000b198, 0x07800781},
+	{0x0000b19c, 0x07a0079f},
+	{0x0000b1a0, 0x07c107bf},
+	{0x0000b1a4, 0x000007c0},
+	{0x0000b1a8, 0x00000000},
+	{0x0000b1ac, 0x00000000},
+	{0x0000b1b0, 0x00000000},
+	{0x0000b1b4, 0x00000000},
+	{0x0000b1b8, 0x00000000},
+	{0x0000b1bc, 0x00000000},
+	{0x0000b1c0, 0x00000000},
+	{0x0000b1c4, 0x00000000},
+	{0x0000b1c8, 0x00000000},
+	{0x0000b1cc, 0x00000000},
+	{0x0000b1d0, 0x00000000},
+	{0x0000b1d4, 0x00000000},
+	{0x0000b1d8, 0x00000000},
+	{0x0000b1dc, 0x00000000},
+	{0x0000b1e0, 0x00000000},
+	{0x0000b1e4, 0x00000000},
+	{0x0000b1e8, 0x00000000},
+	{0x0000b1ec, 0x00000000},
+	{0x0000b1f0, 0x00000396},
+	{0x0000b1f4, 0x00000396},
+	{0x0000b1f8, 0x00000396},
+	{0x0000b1fc, 0x00000196},
+};
+
+static const u32 ar955x_1p0_baseband_core[][2] = {
+	/* Addr      allmodes  */
+	{0x00009800, 0xafe68e30},
+	{0x00009804, 0xfd14e000},
+	{0x00009808, 0x9c0a9f6b},
+	{0x0000980c, 0x04900000},
+	{0x00009814, 0x0280c00a},
+	{0x00009818, 0x00000000},
+	{0x0000981c, 0x00020028},
+	{0x00009834, 0x6400a190},
+	{0x00009838, 0x0108ecff},
+	{0x0000983c, 0x14000600},
+	{0x00009880, 0x201fff00},
+	{0x00009884, 0x00001042},
+	{0x000098a4, 0x00200400},
+	{0x000098b0, 0x32840bbe},
+	{0x000098bc, 0x00000002},
+	{0x000098d0, 0x004b6a8e},
+	{0x000098d4, 0x00000820},
+	{0x000098dc, 0x00000000},
+	{0x000098f0, 0x00000000},
+	{0x000098f4, 0x00000000},
+	{0x00009c04, 0xff55ff55},
+	{0x00009c08, 0x0320ff55},
+	{0x00009c0c, 0x00000000},
+	{0x00009c10, 0x00000000},
+	{0x00009c14, 0x00046384},
+	{0x00009c18, 0x05b6b440},
+	{0x00009c1c, 0x00b6b440},
+	{0x00009d00, 0xc080a333},
+	{0x00009d04, 0x40206c10},
+	{0x00009d08, 0x009c4060},
+	{0x00009d0c, 0x9883800a},
+	{0x00009d10, 0x01834061},
+	{0x00009d14, 0x00c0040b},
+	{0x00009d18, 0x00000000},
+	{0x00009e08, 0x0038230c},
+	{0x00009e24, 0x990bb515},
+	{0x00009e28, 0x0c6f0000},
+	{0x00009e30, 0x06336f77},
+	{0x00009e34, 0x6af6532f},
+	{0x00009e38, 0x0cc80c00},
+	{0x00009e40, 0x0d261820},
+	{0x00009e4c, 0x00001004},
+	{0x00009e50, 0x00ff03f1},
+	{0x00009fc0, 0x813e4788},
+	{0x00009fc4, 0x0001efb5},
+	{0x00009fcc, 0x40000014},
+	{0x00009fd0, 0x01193b93},
+	{0x0000a20c, 0x00000000},
+	{0x0000a220, 0x00000000},
+	{0x0000a224, 0x00000000},
+	{0x0000a228, 0x10002310},
+	{0x0000a23c, 0x00000000},
+	{0x0000a244, 0x0c000000},
+	{0x0000a248, 0x00000140},
+	{0x0000a2a0, 0x00000007},
+	{0x0000a2c0, 0x00000007},
+	{0x0000a2c8, 0x00000000},
+	{0x0000a2d4, 0x00000000},
+	{0x0000a2ec, 0x00000000},
+	{0x0000a2f0, 0x00000000},
+	{0x0000a2f4, 0x00000000},
+	{0x0000a2f8, 0x00000000},
+	{0x0000a344, 0x00000000},
+	{0x0000a34c, 0x00000000},
+	{0x0000a350, 0x0000a000},
+	{0x0000a364, 0x00000000},
+	{0x0000a370, 0x00000000},
+	{0x0000a390, 0x00000001},
+	{0x0000a394, 0x00000444},
+	{0x0000a398, 0x1f020503},
+	{0x0000a39c, 0x29180c03},
+	{0x0000a3a0, 0x9a8b6844},
+	{0x0000a3a4, 0x00000000},
+	{0x0000a3a8, 0xaaaaaaaa},
+	{0x0000a3ac, 0x3c466478},
+	{0x0000a3c0, 0x20202020},
+	{0x0000a3c4, 0x22222220},
+	{0x0000a3c8, 0x20200020},
+	{0x0000a3cc, 0x20202020},
+	{0x0000a3d0, 0x20202020},
+	{0x0000a3d4, 0x20202020},
+	{0x0000a3d8, 0x20202020},
+	{0x0000a3dc, 0x20202020},
+	{0x0000a3e0, 0x20202020},
+	{0x0000a3e4, 0x20202020},
+	{0x0000a3e8, 0x20202020},
+	{0x0000a3ec, 0x20202020},
+	{0x0000a3f0, 0x00000000},
+	{0x0000a3f4, 0x00000000},
+	{0x0000a3f8, 0x0c9bd380},
+	{0x0000a3fc, 0x000f0f01},
+	{0x0000a400, 0x8fa91f01},
+	{0x0000a404, 0x00000000},
+	{0x0000a408, 0x0e79e5c6},
+	{0x0000a40c, 0x00820820},
+	{0x0000a414, 0x1ce739ce},
+	{0x0000a418, 0x2d001dce},
+	{0x0000a41c, 0x1ce739ce},
+	{0x0000a420, 0x000001ce},
+	{0x0000a424, 0x1ce739ce},
+	{0x0000a428, 0x000001ce},
+	{0x0000a42c, 0x1ce739ce},
+	{0x0000a430, 0x1ce739ce},
+	{0x0000a434, 0x00000000},
+	{0x0000a438, 0x00001801},
+	{0x0000a43c, 0x00100000},
+	{0x0000a444, 0x00000000},
+	{0x0000a448, 0x05000080},
+	{0x0000a44c, 0x00000001},
+	{0x0000a450, 0x00010000},
+	{0x0000a458, 0x00000000},
+	{0x0000a644, 0x3fad9d74},
+	{0x0000a648, 0x0048060a},
+	{0x0000a64c, 0x00003c37},
+	{0x0000a670, 0x03020100},
+	{0x0000a674, 0x09080504},
+	{0x0000a678, 0x0d0c0b0a},
+	{0x0000a67c, 0x13121110},
+	{0x0000a680, 0x31301514},
+	{0x0000a684, 0x35343332},
+	{0x0000a688, 0x00000036},
+	{0x0000a690, 0x00000838},
+	{0x0000a7cc, 0x00000000},
+	{0x0000a7d0, 0x00000000},
+	{0x0000a7d4, 0x00000004},
+	{0x0000a7dc, 0x00000000},
+	{0x0000a8d0, 0x004b6a8e},
+	{0x0000a8d4, 0x00000820},
+	{0x0000a8dc, 0x00000000},
+	{0x0000a8f0, 0x00000000},
+	{0x0000a8f4, 0x00000000},
+	{0x0000b2d0, 0x00000080},
+	{0x0000b2d4, 0x00000000},
+	{0x0000b2ec, 0x00000000},
+	{0x0000b2f0, 0x00000000},
+	{0x0000b2f4, 0x00000000},
+	{0x0000b2f8, 0x00000000},
+	{0x0000b408, 0x0e79e5c0},
+	{0x0000b40c, 0x00820820},
+	{0x0000b420, 0x00000000},
+	{0x0000b8d0, 0x004b6a8e},
+	{0x0000b8d4, 0x00000820},
+	{0x0000b8dc, 0x00000000},
+	{0x0000b8f0, 0x00000000},
+	{0x0000b8f4, 0x00000000},
+	{0x0000c2d0, 0x00000080},
+	{0x0000c2d4, 0x00000000},
+	{0x0000c2ec, 0x00000000},
+	{0x0000c2f0, 0x00000000},
+	{0x0000c2f4, 0x00000000},
+	{0x0000c2f8, 0x00000000},
+	{0x0000c408, 0x0e79e5c0},
+	{0x0000c40c, 0x00820820},
+	{0x0000c420, 0x00000000},
+};
+
+static const u32 ar955x_1p0_common_wo_xlna_rx_gain_table[][2] = {
+	/* Addr      allmodes  */
+	{0x0000a000, 0x00010000},
+	{0x0000a004, 0x00030002},
+	{0x0000a008, 0x00050004},
+	{0x0000a00c, 0x00810080},
+	{0x0000a010, 0x00830082},
+	{0x0000a014, 0x01810180},
+	{0x0000a018, 0x01830182},
+	{0x0000a01c, 0x01850184},
+	{0x0000a020, 0x01890188},
+	{0x0000a024, 0x018b018a},
+	{0x0000a028, 0x018d018c},
+	{0x0000a02c, 0x03820190},
+	{0x0000a030, 0x03840383},
+	{0x0000a034, 0x03880385},
+	{0x0000a038, 0x038a0389},
+	{0x0000a03c, 0x038c038b},
+	{0x0000a040, 0x0390038d},
+	{0x0000a044, 0x03920391},
+	{0x0000a048, 0x03940393},
+	{0x0000a04c, 0x03960395},
+	{0x0000a050, 0x00000000},
+	{0x0000a054, 0x00000000},
+	{0x0000a058, 0x00000000},
+	{0x0000a05c, 0x00000000},
+	{0x0000a060, 0x00000000},
+	{0x0000a064, 0x00000000},
+	{0x0000a068, 0x00000000},
+	{0x0000a06c, 0x00000000},
+	{0x0000a070, 0x00000000},
+	{0x0000a074, 0x00000000},
+	{0x0000a078, 0x00000000},
+	{0x0000a07c, 0x00000000},
+	{0x0000a080, 0x29292929},
+	{0x0000a084, 0x29292929},
+	{0x0000a088, 0x29292929},
+	{0x0000a08c, 0x29292929},
+	{0x0000a090, 0x22292929},
+	{0x0000a094, 0x1d1d2222},
+	{0x0000a098, 0x0c111117},
+	{0x0000a09c, 0x00030303},
+	{0x0000a0a0, 0x00000000},
+	{0x0000a0a4, 0x00000000},
+	{0x0000a0a8, 0x00000000},
+	{0x0000a0ac, 0x00000000},
+	{0x0000a0b0, 0x00000000},
+	{0x0000a0b4, 0x00000000},
+	{0x0000a0b8, 0x00000000},
+	{0x0000a0bc, 0x00000000},
+	{0x0000a0c0, 0x001f0000},
+	{0x0000a0c4, 0x01000101},
+	{0x0000a0c8, 0x011e011f},
+	{0x0000a0cc, 0x011c011d},
+	{0x0000a0d0, 0x02030204},
+	{0x0000a0d4, 0x02010202},
+	{0x0000a0d8, 0x021f0200},
+	{0x0000a0dc, 0x0302021e},
+	{0x0000a0e0, 0x03000301},
+	{0x0000a0e4, 0x031e031f},
+	{0x0000a0e8, 0x0402031d},
+	{0x0000a0ec, 0x04000401},
+	{0x0000a0f0, 0x041e041f},
+	{0x0000a0f4, 0x0502041d},
+	{0x0000a0f8, 0x05000501},
+	{0x0000a0fc, 0x051e051f},
+	{0x0000a100, 0x06010602},
+	{0x0000a104, 0x061f0600},
+	{0x0000a108, 0x061d061e},
+	{0x0000a10c, 0x07020703},
+	{0x0000a110, 0x07000701},
+	{0x0000a114, 0x00000000},
+	{0x0000a118, 0x00000000},
+	{0x0000a11c, 0x00000000},
+	{0x0000a120, 0x00000000},
+	{0x0000a124, 0x00000000},
+	{0x0000a128, 0x00000000},
+	{0x0000a12c, 0x00000000},
+	{0x0000a130, 0x00000000},
+	{0x0000a134, 0x00000000},
+	{0x0000a138, 0x00000000},
+	{0x0000a13c, 0x00000000},
+	{0x0000a140, 0x001f0000},
+	{0x0000a144, 0x01000101},
+	{0x0000a148, 0x011e011f},
+	{0x0000a14c, 0x011c011d},
+	{0x0000a150, 0x02030204},
+	{0x0000a154, 0x02010202},
+	{0x0000a158, 0x021f0200},
+	{0x0000a15c, 0x0302021e},
+	{0x0000a160, 0x03000301},
+	{0x0000a164, 0x031e031f},
+	{0x0000a168, 0x0402031d},
+	{0x0000a16c, 0x04000401},
+	{0x0000a170, 0x041e041f},
+	{0x0000a174, 0x0502041d},
+	{0x0000a178, 0x05000501},
+	{0x0000a17c, 0x051e051f},
+	{0x0000a180, 0x06010602},
+	{0x0000a184, 0x061f0600},
+	{0x0000a188, 0x061d061e},
+	{0x0000a18c, 0x07020703},
+	{0x0000a190, 0x07000701},
+	{0x0000a194, 0x00000000},
+	{0x0000a198, 0x00000000},
+	{0x0000a19c, 0x00000000},
+	{0x0000a1a0, 0x00000000},
+	{0x0000a1a4, 0x00000000},
+	{0x0000a1a8, 0x00000000},
+	{0x0000a1ac, 0x00000000},
+	{0x0000a1b0, 0x00000000},
+	{0x0000a1b4, 0x00000000},
+	{0x0000a1b8, 0x00000000},
+	{0x0000a1bc, 0x00000000},
+	{0x0000a1c0, 0x00000000},
+	{0x0000a1c4, 0x00000000},
+	{0x0000a1c8, 0x00000000},
+	{0x0000a1cc, 0x00000000},
+	{0x0000a1d0, 0x00000000},
+	{0x0000a1d4, 0x00000000},
+	{0x0000a1d8, 0x00000000},
+	{0x0000a1dc, 0x00000000},
+	{0x0000a1e0, 0x00000000},
+	{0x0000a1e4, 0x00000000},
+	{0x0000a1e8, 0x00000000},
+	{0x0000a1ec, 0x00000000},
+	{0x0000a1f0, 0x00000396},
+	{0x0000a1f4, 0x00000396},
+	{0x0000a1f8, 0x00000396},
+	{0x0000a1fc, 0x00000196},
+	{0x0000b000, 0x00010000},
+	{0x0000b004, 0x00030002},
+	{0x0000b008, 0x00050004},
+	{0x0000b00c, 0x00810080},
+	{0x0000b010, 0x00830082},
+	{0x0000b014, 0x01810180},
+	{0x0000b018, 0x01830182},
+	{0x0000b01c, 0x01850184},
+	{0x0000b020, 0x02810280},
+	{0x0000b024, 0x02830282},
+	{0x0000b028, 0x02850284},
+	{0x0000b02c, 0x02890288},
+	{0x0000b030, 0x028b028a},
+	{0x0000b034, 0x0388028c},
+	{0x0000b038, 0x038a0389},
+	{0x0000b03c, 0x038c038b},
+	{0x0000b040, 0x0390038d},
+	{0x0000b044, 0x03920391},
+	{0x0000b048, 0x03940393},
+	{0x0000b04c, 0x03960395},
+	{0x0000b050, 0x00000000},
+	{0x0000b054, 0x00000000},
+	{0x0000b058, 0x00000000},
+	{0x0000b05c, 0x00000000},
+	{0x0000b060, 0x00000000},
+	{0x0000b064, 0x00000000},
+	{0x0000b068, 0x00000000},
+	{0x0000b06c, 0x00000000},
+	{0x0000b070, 0x00000000},
+	{0x0000b074, 0x00000000},
+	{0x0000b078, 0x00000000},
+	{0x0000b07c, 0x00000000},
+	{0x0000b080, 0x32323232},
+	{0x0000b084, 0x2f2f3232},
+	{0x0000b088, 0x23282a2d},
+	{0x0000b08c, 0x1c1e2123},
+	{0x0000b090, 0x14171919},
+	{0x0000b094, 0x0e0e1214},
+	{0x0000b098, 0x03050707},
+	{0x0000b09c, 0x00030303},
+	{0x0000b0a0, 0x00000000},
+	{0x0000b0a4, 0x00000000},
+	{0x0000b0a8, 0x00000000},
+	{0x0000b0ac, 0x00000000},
+	{0x0000b0b0, 0x00000000},
+	{0x0000b0b4, 0x00000000},
+	{0x0000b0b8, 0x00000000},
+	{0x0000b0bc, 0x00000000},
+	{0x0000b0c0, 0x003f0020},
+	{0x0000b0c4, 0x00400041},
+	{0x0000b0c8, 0x0140005f},
+	{0x0000b0cc, 0x0160015f},
+	{0x0000b0d0, 0x017e017f},
+	{0x0000b0d4, 0x02410242},
+	{0x0000b0d8, 0x025f0240},
+	{0x0000b0dc, 0x027f0260},
+	{0x0000b0e0, 0x0341027e},
+	{0x0000b0e4, 0x035f0340},
+	{0x0000b0e8, 0x037f0360},
+	{0x0000b0ec, 0x04400441},
+	{0x0000b0f0, 0x0460045f},
+	{0x0000b0f4, 0x0541047f},
+	{0x0000b0f8, 0x055f0540},
+	{0x0000b0fc, 0x057f0560},
+	{0x0000b100, 0x06400641},
+	{0x0000b104, 0x0660065f},
+	{0x0000b108, 0x067e067f},
+	{0x0000b10c, 0x07410742},
+	{0x0000b110, 0x075f0740},
+	{0x0000b114, 0x077f0760},
+	{0x0000b118, 0x07800781},
+	{0x0000b11c, 0x07a0079f},
+	{0x0000b120, 0x07c107bf},
+	{0x0000b124, 0x000007c0},
+	{0x0000b128, 0x00000000},
+	{0x0000b12c, 0x00000000},
+	{0x0000b130, 0x00000000},
+	{0x0000b134, 0x00000000},
+	{0x0000b138, 0x00000000},
+	{0x0000b13c, 0x00000000},
+	{0x0000b140, 0x003f0020},
+	{0x0000b144, 0x00400041},
+	{0x0000b148, 0x0140005f},
+	{0x0000b14c, 0x0160015f},
+	{0x0000b150, 0x017e017f},
+	{0x0000b154, 0x02410242},
+	{0x0000b158, 0x025f0240},
+	{0x0000b15c, 0x027f0260},
+	{0x0000b160, 0x0341027e},
+	{0x0000b164, 0x035f0340},
+	{0x0000b168, 0x037f0360},
+	{0x0000b16c, 0x04400441},
+	{0x0000b170, 0x0460045f},
+	{0x0000b174, 0x0541047f},
+	{0x0000b178, 0x055f0540},
+	{0x0000b17c, 0x057f0560},
+	{0x0000b180, 0x06400641},
+	{0x0000b184, 0x0660065f},
+	{0x0000b188, 0x067e067f},
+	{0x0000b18c, 0x07410742},
+	{0x0000b190, 0x075f0740},
+	{0x0000b194, 0x077f0760},
+	{0x0000b198, 0x07800781},
+	{0x0000b19c, 0x07a0079f},
+	{0x0000b1a0, 0x07c107bf},
+	{0x0000b1a4, 0x000007c0},
+	{0x0000b1a8, 0x00000000},
+	{0x0000b1ac, 0x00000000},
+	{0x0000b1b0, 0x00000000},
+	{0x0000b1b4, 0x00000000},
+	{0x0000b1b8, 0x00000000},
+	{0x0000b1bc, 0x00000000},
+	{0x0000b1c0, 0x00000000},
+	{0x0000b1c4, 0x00000000},
+	{0x0000b1c8, 0x00000000},
+	{0x0000b1cc, 0x00000000},
+	{0x0000b1d0, 0x00000000},
+	{0x0000b1d4, 0x00000000},
+	{0x0000b1d8, 0x00000000},
+	{0x0000b1dc, 0x00000000},
+	{0x0000b1e0, 0x00000000},
+	{0x0000b1e4, 0x00000000},
+	{0x0000b1e8, 0x00000000},
+	{0x0000b1ec, 0x00000000},
+	{0x0000b1f0, 0x00000396},
+	{0x0000b1f4, 0x00000396},
+	{0x0000b1f8, 0x00000396},
+	{0x0000b1fc, 0x00000196},
+};
+
+static const u32 ar955x_1p0_soc_preamble[][2] = {
+	/* Addr      allmodes  */
+	{0x00007000, 0x00000000},
+	{0x00007004, 0x00000000},
+	{0x00007008, 0x00000000},
+	{0x0000700c, 0x00000000},
+	{0x0000701c, 0x00000000},
+	{0x00007020, 0x00000000},
+	{0x00007024, 0x00000000},
+	{0x00007028, 0x00000000},
+	{0x0000702c, 0x00000000},
+	{0x00007030, 0x00000000},
+	{0x00007034, 0x00000002},
+	{0x00007038, 0x000004c2},
+	{0x00007048, 0x00000000},
+};
+
+static const u32 ar955x_1p0_common_wo_xlna_rx_gain_bounds[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00009e44, 0xfe321e27, 0xfe321e27, 0xfe291e27, 0xfe291e27},
+	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
+};
+
+static const u32 ar955x_1p0_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 ar955x_1p0_common_rx_gain_bounds[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00009e44, 0xfe321e27, 0xfe321e27, 0xfe291e27, 0xfe291e27},
+	{0x00009e48, 0x5030201a, 0x5030201a, 0x50302018, 0x50302018},
+};
+
+static const u32 ar955x_1p0_modes_no_xpa_tx_gain_table[][9] = {
+	/* Addr      5G_HT20_L   5G_HT40_L   5G_HT20_M   5G_HT40_M   5G_HT20_H   5G_HT40_H   2G_HT40     2G_HT20  */
+	{0x0000a2dc, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0xfffe5aaa, 0xfffe5aaa},
+	{0x0000a2e0, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0xfffe9ccc, 0xfffe9ccc},
+	{0x0000a2e4, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0xffffe0f0, 0xffffe0f0},
+	{0x0000a2e8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0xffffef00, 0xffffef00},
+	{0x0000a410, 0x000050d8, 0x000050d8, 0x000050d8, 0x000050d8, 0x000050d8, 0x000050d8, 0x000050d7, 0x000050d7},
+	{0x0000a500, 0x00002220, 0x00002220, 0x00002220, 0x00002220, 0x00002220, 0x00002220, 0x00000000, 0x00000000},
+	{0x0000a504, 0x04002222, 0x04002222, 0x04002222, 0x04002222, 0x04002222, 0x04002222, 0x04000002, 0x04000002},
+	{0x0000a508, 0x09002421, 0x09002421, 0x09002421, 0x09002421, 0x09002421, 0x09002421, 0x08000004, 0x08000004},
+	{0x0000a50c, 0x0d002621, 0x0d002621, 0x0d002621, 0x0d002621, 0x0d002621, 0x0d002621, 0x0b000006, 0x0b000006},
+	{0x0000a510, 0x13004620, 0x13004620, 0x13004620, 0x13004620, 0x13004620, 0x13004620, 0x0f00000a, 0x0f00000a},
+	{0x0000a514, 0x19004a20, 0x19004a20, 0x19004a20, 0x19004a20, 0x19004a20, 0x19004a20, 0x1300000c, 0x1300000c},
+	{0x0000a518, 0x1d004e20, 0x1d004e20, 0x1d004e20, 0x1d004e20, 0x1d004e20, 0x1d004e20, 0x1700000e, 0x1700000e},
+	{0x0000a51c, 0x21005420, 0x21005420, 0x21005420, 0x21005420, 0x21005420, 0x21005420, 0x1b000012, 0x1b000012},
+	{0x0000a520, 0x26005e20, 0x26005e20, 0x26005e20, 0x26005e20, 0x26005e20, 0x26005e20, 0x1f00004a, 0x1f00004a},
+	{0x0000a524, 0x2b005e40, 0x2b005e40, 0x2b005e40, 0x2b005e40, 0x2b005e40, 0x2b005e40, 0x23000244, 0x23000244},
+	{0x0000a528, 0x2f005e42, 0x2f005e42, 0x2f005e42, 0x2f005e42, 0x2f005e42, 0x2f005e42, 0x2700022b, 0x2700022b},
+	{0x0000a52c, 0x33005e44, 0x33005e44, 0x33005e44, 0x33005e44, 0x33005e44, 0x33005e44, 0x2b000625, 0x2b000625},
+	{0x0000a530, 0x38005e65, 0x38005e65, 0x38005e65, 0x38005e65, 0x38005e65, 0x38005e65, 0x2f001006, 0x2f001006},
+	{0x0000a534, 0x3c005e69, 0x3c005e69, 0x3c005e69, 0x3c005e69, 0x3c005e69, 0x3c005e69, 0x330008a0, 0x330008a0},
+	{0x0000a538, 0x40005e6b, 0x40005e6b, 0x40005e6b, 0x40005e6b, 0x40005e6b, 0x40005e6b, 0x37000a2a, 0x37000a2a},
+	{0x0000a53c, 0x44005e6d, 0x44005e6d, 0x44005e6d, 0x44005e6d, 0x44005e6d, 0x44005e6d, 0x3b001c23, 0x3b001c23},
+	{0x0000a540, 0x49005e72, 0x49005e72, 0x49005e72, 0x49005e72, 0x49005e72, 0x49005e72, 0x3f0014a0, 0x3f0014a0},
+	{0x0000a544, 0x4e005eb2, 0x4e005eb2, 0x4e005eb2, 0x4e005eb2, 0x4e005eb2, 0x4e005eb2, 0x43001882, 0x43001882},
+	{0x0000a548, 0x53005f12, 0x53005f12, 0x53005f12, 0x53005f12, 0x53005f12, 0x53005f12, 0x47001ca2, 0x47001ca2},
+	{0x0000a54c, 0x59025eb2, 0x59025eb2, 0x59025eb2, 0x59025eb2, 0x59025eb2, 0x59025eb2, 0x4b001ec3, 0x4b001ec3},
+	{0x0000a550, 0x5e025f12, 0x5e025f12, 0x5e025f12, 0x5e025f12, 0x5e025f12, 0x5e025f12, 0x4f00148c, 0x4f00148c},
+	{0x0000a554, 0x61027f12, 0x61027f12, 0x61027f12, 0x61027f12, 0x61027f12, 0x61027f12, 0x53001c6e, 0x53001c6e},
+	{0x0000a558, 0x6702bf12, 0x6702bf12, 0x6702bf12, 0x6702bf12, 0x6702bf12, 0x6702bf12, 0x57001c92, 0x57001c92},
+	{0x0000a55c, 0x6b02bf14, 0x6b02bf14, 0x6b02bf14, 0x6b02bf14, 0x6b02bf14, 0x6b02bf14, 0x5c001af6, 0x5c001af6},
+	{0x0000a560, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a564, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a568, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a56c, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a570, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a574, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a578, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a57c, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x6f02bf16, 0x5c001af6, 0x5c001af6},
+	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+	{0x0000a610, 0x00804000, 0x00804000, 0x00804000, 0x00804000, 0x00804000, 0x00804000, 0x04005001, 0x04005001},
+	{0x0000a614, 0x00804201, 0x00804201, 0x00804201, 0x00804201, 0x00804201, 0x00804201, 0x03808e02, 0x03808e02},
+	{0x0000a618, 0x0280c802, 0x0280c802, 0x0280c802, 0x0280c802, 0x0280c802, 0x0280c802, 0x0300c000, 0x0300c000},
+	{0x0000a61c, 0x0280ca03, 0x0280ca03, 0x0280ca03, 0x0280ca03, 0x0280ca03, 0x0280ca03, 0x03808e02, 0x03808e02},
+	{0x0000a620, 0x04c15104, 0x04c15104, 0x04c15104, 0x04c15104, 0x04c15104, 0x04c15104, 0x03410c03, 0x03410c03},
+	{0x0000a624, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04014c03, 0x04014c03},
+	{0x0000a628, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x05818d04, 0x05818d04},
+	{0x0000a62c, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x0801cd04, 0x0801cd04},
+	{0x0000a630, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x0801e007, 0x0801e007},
+	{0x0000a634, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x0801e007, 0x0801e007},
+	{0x0000a638, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x0801e007, 0x0801e007},
+	{0x0000a63c, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x04c15305, 0x0801e007, 0x0801e007},
+	{0x0000b2dc, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0xfffe5aaa, 0xfffe5aaa},
+	{0x0000b2e0, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0xfffe9ccc, 0xfffe9ccc},
+	{0x0000b2e4, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0xffffe0f0, 0xffffe0f0},
+	{0x0000b2e8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0xffffef00, 0xffffef00},
+	{0x0000c2dc, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0x01feee00, 0xfffe5aaa, 0xfffe5aaa},
+	{0x0000c2e0, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0x0000f000, 0xfffe9ccc, 0xfffe9ccc},
+	{0x0000c2e4, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0x01ff0000, 0xffffe0f0, 0xffffe0f0},
+	{0x0000c2e8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0xffffef00, 0xffffef00},
+	{0x00016044, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x054922d4, 0x054922d4},
+	{0x00016048, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401},
+	{0x00016444, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x054922d4, 0x054922d4},
+	{0x00016448, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401},
+	{0x00016844, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x056db2d4, 0x054922d4, 0x054922d4},
+	{0x00016848, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401, 0x66482401},
+};
+
+static const u32 ar955x_1p0_soc_postamble[][5] = {
+	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
+	{0x00007010, 0x00000023, 0x00000023, 0x00000023, 0x00000023},
+};
+
+static const u32 ar955x_1p0_modes_fast_clock[][3] = {
+	/* Addr      5G_HT20     5G_HT40   */
+	{0x00001030, 0x00000268, 0x000004d0},
+	{0x00001070, 0x0000018c, 0x00000318},
+	{0x000010b0, 0x00000fd0, 0x00001fa0},
+	{0x00008014, 0x044c044c, 0x08980898},
+	{0x0000801c, 0x148ec02b, 0x148ec057},
+	{0x00008318, 0x000044c0, 0x00008980},
+	{0x00009e00, 0x0372131c, 0x0372131c},
+	{0x0000a230, 0x0000000b, 0x00000016},
+	{0x0000a254, 0x00000898, 0x00001130},
+};
+
+#endif /* INITVALS_955X_1P0_H */
diff --git a/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h
index 06b3f0df9fa..6e1915aee71 100644
--- a/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9580_1p0_initvals.h
@@ -1,5 +1,6 @@
 /*
- * Copyright (c) 2010 Atheros Communications Inc.
+ * Copyright (c) 2010-2011 Atheros Communications Inc.
+ * Copyright (c) 2011-2012 Qualcomm Atheros Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -19,18 +20,7 @@
 
 /* AR9580 1.0 */
 
-static const u32 ar9580_1p0_modes_fast_clock[][3] = {
-	/* Addr      5G_HT20     5G_HT40   */
-	{0x00001030, 0x00000268, 0x000004d0},
-	{0x00001070, 0x0000018c, 0x00000318},
-	{0x000010b0, 0x00000fd0, 0x00001fa0},
-	{0x00008014, 0x044c044c, 0x08980898},
-	{0x0000801c, 0x148ec02b, 0x148ec057},
-	{0x00008318, 0x000044c0, 0x00008980},
-	{0x00009e00, 0x0372131c, 0x0372131c},
-	{0x0000a230, 0x0000000b, 0x00000016},
-	{0x0000a254, 0x00000898, 0x00001130},
-};
+#define ar9580_1p0_modes_fast_clock ar9300Modes_fast_clock_2p2
 
 static const u32 ar9580_1p0_radio_postamble[][5] = {
 	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
@@ -208,17 +198,7 @@ static const u32 ar9580_1p0_baseband_core[][2] = {
 	{0x0000c420, 0x00000000},
 };
 
-static const u32 ar9580_1p0_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},
-};
+#define ar9580_1p0_mac_postamble ar9300_2p2_mac_postamble
 
 static const u32 ar9580_1p0_low_ob_db_tx_gain_table[][5] = {
 	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
@@ -326,111 +306,7 @@ static const u32 ar9580_1p0_low_ob_db_tx_gain_table[][5] = {
 	{0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
 };
 
-static const u32 ar9580_1p0_high_power_tx_gain_table[][5] = {
-	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
-	{0x0000a2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
-	{0x0000a2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
-	{0x0000a2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
-	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
-	{0x0000a500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a504, 0x06000003, 0x06000003, 0x04000002, 0x04000002},
-	{0x0000a508, 0x0a000020, 0x0a000020, 0x08000004, 0x08000004},
-	{0x0000a50c, 0x10000023, 0x10000023, 0x0b000200, 0x0b000200},
-	{0x0000a510, 0x16000220, 0x16000220, 0x0f000202, 0x0f000202},
-	{0x0000a514, 0x1c000223, 0x1c000223, 0x12000400, 0x12000400},
-	{0x0000a518, 0x21002220, 0x21002220, 0x16000402, 0x16000402},
-	{0x0000a51c, 0x27002223, 0x27002223, 0x19000404, 0x19000404},
-	{0x0000a520, 0x2b022220, 0x2b022220, 0x1c000603, 0x1c000603},
-	{0x0000a524, 0x2f022222, 0x2f022222, 0x21000a02, 0x21000a02},
-	{0x0000a528, 0x34022225, 0x34022225, 0x25000a04, 0x25000a04},
-	{0x0000a52c, 0x3a02222a, 0x3a02222a, 0x28000a20, 0x28000a20},
-	{0x0000a530, 0x3e02222c, 0x3e02222c, 0x2c000e20, 0x2c000e20},
-	{0x0000a534, 0x4202242a, 0x4202242a, 0x30000e22, 0x30000e22},
-	{0x0000a538, 0x4702244a, 0x4702244a, 0x34000e24, 0x34000e24},
-	{0x0000a53c, 0x4b02244c, 0x4b02244c, 0x38001640, 0x38001640},
-	{0x0000a540, 0x4e02246c, 0x4e02246c, 0x3c001660, 0x3c001660},
-	{0x0000a544, 0x5302266c, 0x5302266c, 0x3f001861, 0x3f001861},
-	{0x0000a548, 0x5702286c, 0x5702286c, 0x43001a81, 0x43001a81},
-	{0x0000a54c, 0x5c02486b, 0x5c02486b, 0x47001a83, 0x47001a83},
-	{0x0000a550, 0x61024a6c, 0x61024a6c, 0x4a001c84, 0x4a001c84},
-	{0x0000a554, 0x66026a6c, 0x66026a6c, 0x4e001ce3, 0x4e001ce3},
-	{0x0000a558, 0x6b026e6c, 0x6b026e6c, 0x52001ce5, 0x52001ce5},
-	{0x0000a55c, 0x7002708c, 0x7002708c, 0x56001ce9, 0x56001ce9},
-	{0x0000a560, 0x7302b08a, 0x7302b08a, 0x5a001ceb, 0x5a001ceb},
-	{0x0000a564, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a568, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a56c, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a570, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a574, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a578, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a57c, 0x7702b08c, 0x7702b08c, 0x5d001eec, 0x5d001eec},
-	{0x0000a580, 0x00800000, 0x00800000, 0x00800000, 0x00800000},
-	{0x0000a584, 0x06800003, 0x06800003, 0x04800002, 0x04800002},
-	{0x0000a588, 0x0a800020, 0x0a800020, 0x08800004, 0x08800004},
-	{0x0000a58c, 0x10800023, 0x10800023, 0x0b800200, 0x0b800200},
-	{0x0000a590, 0x16800220, 0x16800220, 0x0f800202, 0x0f800202},
-	{0x0000a594, 0x1c800223, 0x1c800223, 0x12800400, 0x12800400},
-	{0x0000a598, 0x21802220, 0x21802220, 0x16800402, 0x16800402},
-	{0x0000a59c, 0x27802223, 0x27802223, 0x19800404, 0x19800404},
-	{0x0000a5a0, 0x2b822220, 0x2b822220, 0x1c800603, 0x1c800603},
-	{0x0000a5a4, 0x2f822222, 0x2f822222, 0x21800a02, 0x21800a02},
-	{0x0000a5a8, 0x34822225, 0x34822225, 0x25800a04, 0x25800a04},
-	{0x0000a5ac, 0x3a82222a, 0x3a82222a, 0x28800a20, 0x28800a20},
-	{0x0000a5b0, 0x3e82222c, 0x3e82222c, 0x2c800e20, 0x2c800e20},
-	{0x0000a5b4, 0x4282242a, 0x4282242a, 0x30800e22, 0x30800e22},
-	{0x0000a5b8, 0x4782244a, 0x4782244a, 0x34800e24, 0x34800e24},
-	{0x0000a5bc, 0x4b82244c, 0x4b82244c, 0x38801640, 0x38801640},
-	{0x0000a5c0, 0x4e82246c, 0x4e82246c, 0x3c801660, 0x3c801660},
-	{0x0000a5c4, 0x5382266c, 0x5382266c, 0x3f801861, 0x3f801861},
-	{0x0000a5c8, 0x5782286c, 0x5782286c, 0x43801a81, 0x43801a81},
-	{0x0000a5cc, 0x5c82486b, 0x5c82486b, 0x47801a83, 0x47801a83},
-	{0x0000a5d0, 0x61824a6c, 0x61824a6c, 0x4a801c84, 0x4a801c84},
-	{0x0000a5d4, 0x66826a6c, 0x66826a6c, 0x4e801ce3, 0x4e801ce3},
-	{0x0000a5d8, 0x6b826e6c, 0x6b826e6c, 0x52801ce5, 0x52801ce5},
-	{0x0000a5dc, 0x7082708c, 0x7082708c, 0x56801ce9, 0x56801ce9},
-	{0x0000a5e0, 0x7382b08a, 0x7382b08a, 0x5a801ceb, 0x5a801ceb},
-	{0x0000a5e4, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a5e8, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a5ec, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a5f0, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a5f4, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a5f8, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a5fc, 0x7782b08c, 0x7782b08c, 0x5d801eec, 0x5d801eec},
-	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a614, 0x01404000, 0x01404000, 0x01404000, 0x01404000},
-	{0x0000a618, 0x01404501, 0x01404501, 0x01404501, 0x01404501},
-	{0x0000a61c, 0x02008802, 0x02008802, 0x02008501, 0x02008501},
-	{0x0000a620, 0x0300cc03, 0x0300cc03, 0x0280ca03, 0x0280ca03},
-	{0x0000a624, 0x0300cc03, 0x0300cc03, 0x03010c04, 0x03010c04},
-	{0x0000a628, 0x0300cc03, 0x0300cc03, 0x04014c04, 0x04014c04},
-	{0x0000a62c, 0x03810c03, 0x03810c03, 0x04015005, 0x04015005},
-	{0x0000a630, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
-	{0x0000a634, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
-	{0x0000a638, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
-	{0x0000a63c, 0x03810e04, 0x03810e04, 0x04015005, 0x04015005},
-	{0x0000b2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
-	{0x0000b2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
-	{0x0000b2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
-	{0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x0000c2dc, 0x0380c7fc, 0x0380c7fc, 0x03aaa352, 0x03aaa352},
-	{0x0000c2e0, 0x0000f800, 0x0000f800, 0x03ccc584, 0x03ccc584},
-	{0x0000c2e4, 0x03ff0000, 0x03ff0000, 0x03f0f800, 0x03f0f800},
-	{0x0000c2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x00016044, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-	{0x00016048, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
-	{0x00016068, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
-	{0x00016444, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-	{0x00016448, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
-	{0x00016468, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
-	{0x00016844, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-	{0x00016848, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
-	{0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
-};
+#define ar9580_1p0_high_power_tx_gain_table ar9580_1p0_low_ob_db_tx_gain_table
 
 static const u32 ar9580_1p0_lowest_ob_db_tx_gain_table[][5] = {
 	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
@@ -538,12 +414,7 @@ static const u32 ar9580_1p0_lowest_ob_db_tx_gain_table[][5] = {
 	{0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
 };
 
-static const u32 ar9580_1p0_baseband_core_txfir_coeff_japan_2484[][2] = {
-	/* Addr      allmodes  */
-	{0x0000a398, 0x00000000},
-	{0x0000a39c, 0x6f7f0301},
-	{0x0000a3a0, 0xca9228ee},
-};
+#define ar9580_1p0_baseband_core_txfir_coeff_japan_2484 ar9462_2p0_baseband_core_txfir_coeff_japan_2484
 
 static const u32 ar9580_1p0_mac_core[][2] = {
 	/* Addr      allmodes  */
@@ -808,376 +679,11 @@ static const u32 ar9580_1p0_mixed_ob_db_tx_gain_table[][5] = {
 	{0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
 };
 
-static const u32 ar9580_1p0_wo_xlna_rx_gain_table[][2] = {
-	/* Addr      allmodes  */
-	{0x0000a000, 0x00010000},
-	{0x0000a004, 0x00030002},
-	{0x0000a008, 0x00050004},
-	{0x0000a00c, 0x00810080},
-	{0x0000a010, 0x00830082},
-	{0x0000a014, 0x01810180},
-	{0x0000a018, 0x01830182},
-	{0x0000a01c, 0x01850184},
-	{0x0000a020, 0x01890188},
-	{0x0000a024, 0x018b018a},
-	{0x0000a028, 0x018d018c},
-	{0x0000a02c, 0x03820190},
-	{0x0000a030, 0x03840383},
-	{0x0000a034, 0x03880385},
-	{0x0000a038, 0x038a0389},
-	{0x0000a03c, 0x038c038b},
-	{0x0000a040, 0x0390038d},
-	{0x0000a044, 0x03920391},
-	{0x0000a048, 0x03940393},
-	{0x0000a04c, 0x03960395},
-	{0x0000a050, 0x00000000},
-	{0x0000a054, 0x00000000},
-	{0x0000a058, 0x00000000},
-	{0x0000a05c, 0x00000000},
-	{0x0000a060, 0x00000000},
-	{0x0000a064, 0x00000000},
-	{0x0000a068, 0x00000000},
-	{0x0000a06c, 0x00000000},
-	{0x0000a070, 0x00000000},
-	{0x0000a074, 0x00000000},
-	{0x0000a078, 0x00000000},
-	{0x0000a07c, 0x00000000},
-	{0x0000a080, 0x29292929},
-	{0x0000a084, 0x29292929},
-	{0x0000a088, 0x29292929},
-	{0x0000a08c, 0x29292929},
-	{0x0000a090, 0x22292929},
-	{0x0000a094, 0x1d1d2222},
-	{0x0000a098, 0x0c111117},
-	{0x0000a09c, 0x00030303},
-	{0x0000a0a0, 0x00000000},
-	{0x0000a0a4, 0x00000000},
-	{0x0000a0a8, 0x00000000},
-	{0x0000a0ac, 0x00000000},
-	{0x0000a0b0, 0x00000000},
-	{0x0000a0b4, 0x00000000},
-	{0x0000a0b8, 0x00000000},
-	{0x0000a0bc, 0x00000000},
-	{0x0000a0c0, 0x001f0000},
-	{0x0000a0c4, 0x01000101},
-	{0x0000a0c8, 0x011e011f},
-	{0x0000a0cc, 0x011c011d},
-	{0x0000a0d0, 0x02030204},
-	{0x0000a0d4, 0x02010202},
-	{0x0000a0d8, 0x021f0200},
-	{0x0000a0dc, 0x0302021e},
-	{0x0000a0e0, 0x03000301},
-	{0x0000a0e4, 0x031e031f},
-	{0x0000a0e8, 0x0402031d},
-	{0x0000a0ec, 0x04000401},
-	{0x0000a0f0, 0x041e041f},
-	{0x0000a0f4, 0x0502041d},
-	{0x0000a0f8, 0x05000501},
-	{0x0000a0fc, 0x051e051f},
-	{0x0000a100, 0x06010602},
-	{0x0000a104, 0x061f0600},
-	{0x0000a108, 0x061d061e},
-	{0x0000a10c, 0x07020703},
-	{0x0000a110, 0x07000701},
-	{0x0000a114, 0x00000000},
-	{0x0000a118, 0x00000000},
-	{0x0000a11c, 0x00000000},
-	{0x0000a120, 0x00000000},
-	{0x0000a124, 0x00000000},
-	{0x0000a128, 0x00000000},
-	{0x0000a12c, 0x00000000},
-	{0x0000a130, 0x00000000},
-	{0x0000a134, 0x00000000},
-	{0x0000a138, 0x00000000},
-	{0x0000a13c, 0x00000000},
-	{0x0000a140, 0x001f0000},
-	{0x0000a144, 0x01000101},
-	{0x0000a148, 0x011e011f},
-	{0x0000a14c, 0x011c011d},
-	{0x0000a150, 0x02030204},
-	{0x0000a154, 0x02010202},
-	{0x0000a158, 0x021f0200},
-	{0x0000a15c, 0x0302021e},
-	{0x0000a160, 0x03000301},
-	{0x0000a164, 0x031e031f},
-	{0x0000a168, 0x0402031d},
-	{0x0000a16c, 0x04000401},
-	{0x0000a170, 0x041e041f},
-	{0x0000a174, 0x0502041d},
-	{0x0000a178, 0x05000501},
-	{0x0000a17c, 0x051e051f},
-	{0x0000a180, 0x06010602},
-	{0x0000a184, 0x061f0600},
-	{0x0000a188, 0x061d061e},
-	{0x0000a18c, 0x07020703},
-	{0x0000a190, 0x07000701},
-	{0x0000a194, 0x00000000},
-	{0x0000a198, 0x00000000},
-	{0x0000a19c, 0x00000000},
-	{0x0000a1a0, 0x00000000},
-	{0x0000a1a4, 0x00000000},
-	{0x0000a1a8, 0x00000000},
-	{0x0000a1ac, 0x00000000},
-	{0x0000a1b0, 0x00000000},
-	{0x0000a1b4, 0x00000000},
-	{0x0000a1b8, 0x00000000},
-	{0x0000a1bc, 0x00000000},
-	{0x0000a1c0, 0x00000000},
-	{0x0000a1c4, 0x00000000},
-	{0x0000a1c8, 0x00000000},
-	{0x0000a1cc, 0x00000000},
-	{0x0000a1d0, 0x00000000},
-	{0x0000a1d4, 0x00000000},
-	{0x0000a1d8, 0x00000000},
-	{0x0000a1dc, 0x00000000},
-	{0x0000a1e0, 0x00000000},
-	{0x0000a1e4, 0x00000000},
-	{0x0000a1e8, 0x00000000},
-	{0x0000a1ec, 0x00000000},
-	{0x0000a1f0, 0x00000396},
-	{0x0000a1f4, 0x00000396},
-	{0x0000a1f8, 0x00000396},
-	{0x0000a1fc, 0x00000196},
-	{0x0000b000, 0x00010000},
-	{0x0000b004, 0x00030002},
-	{0x0000b008, 0x00050004},
-	{0x0000b00c, 0x00810080},
-	{0x0000b010, 0x00830082},
-	{0x0000b014, 0x01810180},
-	{0x0000b018, 0x01830182},
-	{0x0000b01c, 0x01850184},
-	{0x0000b020, 0x02810280},
-	{0x0000b024, 0x02830282},
-	{0x0000b028, 0x02850284},
-	{0x0000b02c, 0x02890288},
-	{0x0000b030, 0x028b028a},
-	{0x0000b034, 0x0388028c},
-	{0x0000b038, 0x038a0389},
-	{0x0000b03c, 0x038c038b},
-	{0x0000b040, 0x0390038d},
-	{0x0000b044, 0x03920391},
-	{0x0000b048, 0x03940393},
-	{0x0000b04c, 0x03960395},
-	{0x0000b050, 0x00000000},
-	{0x0000b054, 0x00000000},
-	{0x0000b058, 0x00000000},
-	{0x0000b05c, 0x00000000},
-	{0x0000b060, 0x00000000},
-	{0x0000b064, 0x00000000},
-	{0x0000b068, 0x00000000},
-	{0x0000b06c, 0x00000000},
-	{0x0000b070, 0x00000000},
-	{0x0000b074, 0x00000000},
-	{0x0000b078, 0x00000000},
-	{0x0000b07c, 0x00000000},
-	{0x0000b080, 0x32323232},
-	{0x0000b084, 0x2f2f3232},
-	{0x0000b088, 0x23282a2d},
-	{0x0000b08c, 0x1c1e2123},
-	{0x0000b090, 0x14171919},
-	{0x0000b094, 0x0e0e1214},
-	{0x0000b098, 0x03050707},
-	{0x0000b09c, 0x00030303},
-	{0x0000b0a0, 0x00000000},
-	{0x0000b0a4, 0x00000000},
-	{0x0000b0a8, 0x00000000},
-	{0x0000b0ac, 0x00000000},
-	{0x0000b0b0, 0x00000000},
-	{0x0000b0b4, 0x00000000},
-	{0x0000b0b8, 0x00000000},
-	{0x0000b0bc, 0x00000000},
-	{0x0000b0c0, 0x003f0020},
-	{0x0000b0c4, 0x00400041},
-	{0x0000b0c8, 0x0140005f},
-	{0x0000b0cc, 0x0160015f},
-	{0x0000b0d0, 0x017e017f},
-	{0x0000b0d4, 0x02410242},
-	{0x0000b0d8, 0x025f0240},
-	{0x0000b0dc, 0x027f0260},
-	{0x0000b0e0, 0x0341027e},
-	{0x0000b0e4, 0x035f0340},
-	{0x0000b0e8, 0x037f0360},
-	{0x0000b0ec, 0x04400441},
-	{0x0000b0f0, 0x0460045f},
-	{0x0000b0f4, 0x0541047f},
-	{0x0000b0f8, 0x055f0540},
-	{0x0000b0fc, 0x057f0560},
-	{0x0000b100, 0x06400641},
-	{0x0000b104, 0x0660065f},
-	{0x0000b108, 0x067e067f},
-	{0x0000b10c, 0x07410742},
-	{0x0000b110, 0x075f0740},
-	{0x0000b114, 0x077f0760},
-	{0x0000b118, 0x07800781},
-	{0x0000b11c, 0x07a0079f},
-	{0x0000b120, 0x07c107bf},
-	{0x0000b124, 0x000007c0},
-	{0x0000b128, 0x00000000},
-	{0x0000b12c, 0x00000000},
-	{0x0000b130, 0x00000000},
-	{0x0000b134, 0x00000000},
-	{0x0000b138, 0x00000000},
-	{0x0000b13c, 0x00000000},
-	{0x0000b140, 0x003f0020},
-	{0x0000b144, 0x00400041},
-	{0x0000b148, 0x0140005f},
-	{0x0000b14c, 0x0160015f},
-	{0x0000b150, 0x017e017f},
-	{0x0000b154, 0x02410242},
-	{0x0000b158, 0x025f0240},
-	{0x0000b15c, 0x027f0260},
-	{0x0000b160, 0x0341027e},
-	{0x0000b164, 0x035f0340},
-	{0x0000b168, 0x037f0360},
-	{0x0000b16c, 0x04400441},
-	{0x0000b170, 0x0460045f},
-	{0x0000b174, 0x0541047f},
-	{0x0000b178, 0x055f0540},
-	{0x0000b17c, 0x057f0560},
-	{0x0000b180, 0x06400641},
-	{0x0000b184, 0x0660065f},
-	{0x0000b188, 0x067e067f},
-	{0x0000b18c, 0x07410742},
-	{0x0000b190, 0x075f0740},
-	{0x0000b194, 0x077f0760},
-	{0x0000b198, 0x07800781},
-	{0x0000b19c, 0x07a0079f},
-	{0x0000b1a0, 0x07c107bf},
-	{0x0000b1a4, 0x000007c0},
-	{0x0000b1a8, 0x00000000},
-	{0x0000b1ac, 0x00000000},
-	{0x0000b1b0, 0x00000000},
-	{0x0000b1b4, 0x00000000},
-	{0x0000b1b8, 0x00000000},
-	{0x0000b1bc, 0x00000000},
-	{0x0000b1c0, 0x00000000},
-	{0x0000b1c4, 0x00000000},
-	{0x0000b1c8, 0x00000000},
-	{0x0000b1cc, 0x00000000},
-	{0x0000b1d0, 0x00000000},
-	{0x0000b1d4, 0x00000000},
-	{0x0000b1d8, 0x00000000},
-	{0x0000b1dc, 0x00000000},
-	{0x0000b1e0, 0x00000000},
-	{0x0000b1e4, 0x00000000},
-	{0x0000b1e8, 0x00000000},
-	{0x0000b1ec, 0x00000000},
-	{0x0000b1f0, 0x00000396},
-	{0x0000b1f4, 0x00000396},
-	{0x0000b1f8, 0x00000396},
-	{0x0000b1fc, 0x00000196},
-};
+#define ar9580_1p0_wo_xlna_rx_gain_table ar9300Common_wo_xlna_rx_gain_table_2p2
 
-static const u32 ar9580_1p0_soc_postamble[][5] = {
-	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
-	{0x00007010, 0x00000023, 0x00000023, 0x00000023, 0x00000023},
-};
+#define ar9580_1p0_soc_postamble ar9300_2p2_soc_postamble
 
-static const u32 ar9580_1p0_high_ob_db_tx_gain_table[][5] = {
-	/* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
-	{0x0000a2dc, 0x01feee00, 0x01feee00, 0x03aaa352, 0x03aaa352},
-	{0x0000a2e0, 0x0000f000, 0x0000f000, 0x03ccc584, 0x03ccc584},
-	{0x0000a2e4, 0x01ff0000, 0x01ff0000, 0x03f0f800, 0x03f0f800},
-	{0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x0000a410, 0x000050d8, 0x000050d8, 0x000050d9, 0x000050d9},
-	{0x0000a500, 0x00002220, 0x00002220, 0x00000000, 0x00000000},
-	{0x0000a504, 0x04002222, 0x04002222, 0x04000002, 0x04000002},
-	{0x0000a508, 0x09002421, 0x09002421, 0x08000004, 0x08000004},
-	{0x0000a50c, 0x0d002621, 0x0d002621, 0x0b000200, 0x0b000200},
-	{0x0000a510, 0x13004620, 0x13004620, 0x0f000202, 0x0f000202},
-	{0x0000a514, 0x19004a20, 0x19004a20, 0x11000400, 0x11000400},
-	{0x0000a518, 0x1d004e20, 0x1d004e20, 0x15000402, 0x15000402},
-	{0x0000a51c, 0x21005420, 0x21005420, 0x19000404, 0x19000404},
-	{0x0000a520, 0x26005e20, 0x26005e20, 0x1b000603, 0x1b000603},
-	{0x0000a524, 0x2b005e40, 0x2b005e40, 0x1f000a02, 0x1f000a02},
-	{0x0000a528, 0x2f005e42, 0x2f005e42, 0x23000a04, 0x23000a04},
-	{0x0000a52c, 0x33005e44, 0x33005e44, 0x26000a20, 0x26000a20},
-	{0x0000a530, 0x38005e65, 0x38005e65, 0x2a000e20, 0x2a000e20},
-	{0x0000a534, 0x3c005e69, 0x3c005e69, 0x2e000e22, 0x2e000e22},
-	{0x0000a538, 0x40005e6b, 0x40005e6b, 0x31000e24, 0x31000e24},
-	{0x0000a53c, 0x44005e6d, 0x44005e6d, 0x34001640, 0x34001640},
-	{0x0000a540, 0x49005e72, 0x49005e72, 0x38001660, 0x38001660},
-	{0x0000a544, 0x4e005eb2, 0x4e005eb2, 0x3b001861, 0x3b001861},
-	{0x0000a548, 0x53005f12, 0x53005f12, 0x3e001a81, 0x3e001a81},
-	{0x0000a54c, 0x59025eb2, 0x59025eb2, 0x42001a83, 0x42001a83},
-	{0x0000a550, 0x5e025f12, 0x5e025f12, 0x44001c84, 0x44001c84},
-	{0x0000a554, 0x61027f12, 0x61027f12, 0x48001ce3, 0x48001ce3},
-	{0x0000a558, 0x6702bf12, 0x6702bf12, 0x4c001ce5, 0x4c001ce5},
-	{0x0000a55c, 0x6b02bf14, 0x6b02bf14, 0x50001ce9, 0x50001ce9},
-	{0x0000a560, 0x6f02bf16, 0x6f02bf16, 0x54001ceb, 0x54001ceb},
-	{0x0000a564, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a568, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a56c, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a570, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a574, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a578, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a57c, 0x6f02bf16, 0x6f02bf16, 0x56001eec, 0x56001eec},
-	{0x0000a580, 0x00802220, 0x00802220, 0x00800000, 0x00800000},
-	{0x0000a584, 0x04802222, 0x04802222, 0x04800002, 0x04800002},
-	{0x0000a588, 0x09802421, 0x09802421, 0x08800004, 0x08800004},
-	{0x0000a58c, 0x0d802621, 0x0d802621, 0x0b800200, 0x0b800200},
-	{0x0000a590, 0x13804620, 0x13804620, 0x0f800202, 0x0f800202},
-	{0x0000a594, 0x19804a20, 0x19804a20, 0x11800400, 0x11800400},
-	{0x0000a598, 0x1d804e20, 0x1d804e20, 0x15800402, 0x15800402},
-	{0x0000a59c, 0x21805420, 0x21805420, 0x19800404, 0x19800404},
-	{0x0000a5a0, 0x26805e20, 0x26805e20, 0x1b800603, 0x1b800603},
-	{0x0000a5a4, 0x2b805e40, 0x2b805e40, 0x1f800a02, 0x1f800a02},
-	{0x0000a5a8, 0x2f805e42, 0x2f805e42, 0x23800a04, 0x23800a04},
-	{0x0000a5ac, 0x33805e44, 0x33805e44, 0x26800a20, 0x26800a20},
-	{0x0000a5b0, 0x38805e65, 0x38805e65, 0x2a800e20, 0x2a800e20},
-	{0x0000a5b4, 0x3c805e69, 0x3c805e69, 0x2e800e22, 0x2e800e22},
-	{0x0000a5b8, 0x40805e6b, 0x40805e6b, 0x31800e24, 0x31800e24},
-	{0x0000a5bc, 0x44805e6d, 0x44805e6d, 0x34801640, 0x34801640},
-	{0x0000a5c0, 0x49805e72, 0x49805e72, 0x38801660, 0x38801660},
-	{0x0000a5c4, 0x4e805eb2, 0x4e805eb2, 0x3b801861, 0x3b801861},
-	{0x0000a5c8, 0x53805f12, 0x53805f12, 0x3e801a81, 0x3e801a81},
-	{0x0000a5cc, 0x59825eb2, 0x59825eb2, 0x42801a83, 0x42801a83},
-	{0x0000a5d0, 0x5e825f12, 0x5e825f12, 0x44801c84, 0x44801c84},
-	{0x0000a5d4, 0x61827f12, 0x61827f12, 0x48801ce3, 0x48801ce3},
-	{0x0000a5d8, 0x6782bf12, 0x6782bf12, 0x4c801ce5, 0x4c801ce5},
-	{0x0000a5dc, 0x6b82bf14, 0x6b82bf14, 0x50801ce9, 0x50801ce9},
-	{0x0000a5e0, 0x6f82bf16, 0x6f82bf16, 0x54801ceb, 0x54801ceb},
-	{0x0000a5e4, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5e8, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5ec, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5f0, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5f4, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5f8, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a5fc, 0x6f82bf16, 0x6f82bf16, 0x56801eec, 0x56801eec},
-	{0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-	{0x0000a610, 0x00804000, 0x00804000, 0x00000000, 0x00000000},
-	{0x0000a614, 0x00804201, 0x00804201, 0x01404000, 0x01404000},
-	{0x0000a618, 0x0280c802, 0x0280c802, 0x01404501, 0x01404501},
-	{0x0000a61c, 0x0280ca03, 0x0280ca03, 0x02008501, 0x02008501},
-	{0x0000a620, 0x04c15104, 0x04c15104, 0x0280ca03, 0x0280ca03},
-	{0x0000a624, 0x04c15305, 0x04c15305, 0x03010c04, 0x03010c04},
-	{0x0000a628, 0x04c15305, 0x04c15305, 0x04014c04, 0x04014c04},
-	{0x0000a62c, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
-	{0x0000a630, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
-	{0x0000a634, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
-	{0x0000a638, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
-	{0x0000a63c, 0x04c15305, 0x04c15305, 0x04015005, 0x04015005},
-	{0x0000b2dc, 0x01feee00, 0x01feee00, 0x03aaa352, 0x03aaa352},
-	{0x0000b2e0, 0x0000f000, 0x0000f000, 0x03ccc584, 0x03ccc584},
-	{0x0000b2e4, 0x01ff0000, 0x01ff0000, 0x03f0f800, 0x03f0f800},
-	{0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x0000c2dc, 0x01feee00, 0x01feee00, 0x03aaa352, 0x03aaa352},
-	{0x0000c2e0, 0x0000f000, 0x0000f000, 0x03ccc584, 0x03ccc584},
-	{0x0000c2e4, 0x01ff0000, 0x01ff0000, 0x03f0f800, 0x03f0f800},
-	{0x0000c2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-	{0x00016044, 0x056db2e4, 0x056db2e4, 0x056db2e4, 0x056db2e4},
-	{0x00016048, 0x8e480001, 0x8e480001, 0x8e480001, 0x8e480001},
-	{0x00016068, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
-	{0x00016444, 0x056db2e4, 0x056db2e4, 0x056db2e4, 0x056db2e4},
-	{0x00016448, 0x8e480001, 0x8e480001, 0x8e480001, 0x8e480001},
-	{0x00016468, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
-	{0x00016844, 0x056db2e4, 0x056db2e4, 0x056db2e4, 0x056db2e4},
-	{0x00016848, 0x8e480001, 0x8e480001, 0x8e480001, 0x8e480001},
-	{0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
-};
+#define ar9580_1p0_high_ob_db_tx_gain_table ar9300Modes_high_ob_db_tx_gain_table_2p2
 
 static const u32 ar9580_1p0_soc_preamble[][2] = {
 	/* Addr      allmodes  */
@@ -1189,265 +695,7 @@ static const u32 ar9580_1p0_soc_preamble[][2] = {
 	{0x00007048, 0x00000008},
 };
 
-static const u32 ar9580_1p0_rx_gain_table[][2] = {
-	/* Addr      allmodes  */
-	{0x0000a000, 0x00010000},
-	{0x0000a004, 0x00030002},
-	{0x0000a008, 0x00050004},
-	{0x0000a00c, 0x00810080},
-	{0x0000a010, 0x00830082},
-	{0x0000a014, 0x01810180},
-	{0x0000a018, 0x01830182},
-	{0x0000a01c, 0x01850184},
-	{0x0000a020, 0x01890188},
-	{0x0000a024, 0x018b018a},
-	{0x0000a028, 0x018d018c},
-	{0x0000a02c, 0x01910190},
-	{0x0000a030, 0x01930192},
-	{0x0000a034, 0x01950194},
-	{0x0000a038, 0x038a0196},
-	{0x0000a03c, 0x038c038b},
-	{0x0000a040, 0x0390038d},
-	{0x0000a044, 0x03920391},
-	{0x0000a048, 0x03940393},
-	{0x0000a04c, 0x03960395},
-	{0x0000a050, 0x00000000},
-	{0x0000a054, 0x00000000},
-	{0x0000a058, 0x00000000},
-	{0x0000a05c, 0x00000000},
-	{0x0000a060, 0x00000000},
-	{0x0000a064, 0x00000000},
-	{0x0000a068, 0x00000000},
-	{0x0000a06c, 0x00000000},
-	{0x0000a070, 0x00000000},
-	{0x0000a074, 0x00000000},
-	{0x0000a078, 0x00000000},
-	{0x0000a07c, 0x00000000},
-	{0x0000a080, 0x22222229},
-	{0x0000a084, 0x1d1d1d1d},
-	{0x0000a088, 0x1d1d1d1d},
-	{0x0000a08c, 0x1d1d1d1d},
-	{0x0000a090, 0x171d1d1d},
-	{0x0000a094, 0x11111717},
-	{0x0000a098, 0x00030311},
-	{0x0000a09c, 0x00000000},
-	{0x0000a0a0, 0x00000000},
-	{0x0000a0a4, 0x00000000},
-	{0x0000a0a8, 0x00000000},
-	{0x0000a0ac, 0x00000000},
-	{0x0000a0b0, 0x00000000},
-	{0x0000a0b4, 0x00000000},
-	{0x0000a0b8, 0x00000000},
-	{0x0000a0bc, 0x00000000},
-	{0x0000a0c0, 0x001f0000},
-	{0x0000a0c4, 0x01000101},
-	{0x0000a0c8, 0x011e011f},
-	{0x0000a0cc, 0x011c011d},
-	{0x0000a0d0, 0x02030204},
-	{0x0000a0d4, 0x02010202},
-	{0x0000a0d8, 0x021f0200},
-	{0x0000a0dc, 0x0302021e},
-	{0x0000a0e0, 0x03000301},
-	{0x0000a0e4, 0x031e031f},
-	{0x0000a0e8, 0x0402031d},
-	{0x0000a0ec, 0x04000401},
-	{0x0000a0f0, 0x041e041f},
-	{0x0000a0f4, 0x0502041d},
-	{0x0000a0f8, 0x05000501},
-	{0x0000a0fc, 0x051e051f},
-	{0x0000a100, 0x06010602},
-	{0x0000a104, 0x061f0600},
-	{0x0000a108, 0x061d061e},
-	{0x0000a10c, 0x07020703},
-	{0x0000a110, 0x07000701},
-	{0x0000a114, 0x00000000},
-	{0x0000a118, 0x00000000},
-	{0x0000a11c, 0x00000000},
-	{0x0000a120, 0x00000000},
-	{0x0000a124, 0x00000000},
-	{0x0000a128, 0x00000000},
-	{0x0000a12c, 0x00000000},
-	{0x0000a130, 0x00000000},
-	{0x0000a134, 0x00000000},
-	{0x0000a138, 0x00000000},
-	{0x0000a13c, 0x00000000},
-	{0x0000a140, 0x001f0000},
-	{0x0000a144, 0x01000101},
-	{0x0000a148, 0x011e011f},
-	{0x0000a14c, 0x011c011d},
-	{0x0000a150, 0x02030204},
-	{0x0000a154, 0x02010202},
-	{0x0000a158, 0x021f0200},
-	{0x0000a15c, 0x0302021e},
-	{0x0000a160, 0x03000301},
-	{0x0000a164, 0x031e031f},
-	{0x0000a168, 0x0402031d},
-	{0x0000a16c, 0x04000401},
-	{0x0000a170, 0x041e041f},
-	{0x0000a174, 0x0502041d},
-	{0x0000a178, 0x05000501},
-	{0x0000a17c, 0x051e051f},
-	{0x0000a180, 0x06010602},
-	{0x0000a184, 0x061f0600},
-	{0x0000a188, 0x061d061e},
-	{0x0000a18c, 0x07020703},
-	{0x0000a190, 0x07000701},
-	{0x0000a194, 0x00000000},
-	{0x0000a198, 0x00000000},
-	{0x0000a19c, 0x00000000},
-	{0x0000a1a0, 0x00000000},
-	{0x0000a1a4, 0x00000000},
-	{0x0000a1a8, 0x00000000},
-	{0x0000a1ac, 0x00000000},
-	{0x0000a1b0, 0x00000000},
-	{0x0000a1b4, 0x00000000},
-	{0x0000a1b8, 0x00000000},
-	{0x0000a1bc, 0x00000000},
-	{0x0000a1c0, 0x00000000},
-	{0x0000a1c4, 0x00000000},
-	{0x0000a1c8, 0x00000000},
-	{0x0000a1cc, 0x00000000},
-	{0x0000a1d0, 0x00000000},
-	{0x0000a1d4, 0x00000000},
-	{0x0000a1d8, 0x00000000},
-	{0x0000a1dc, 0x00000000},
-	{0x0000a1e0, 0x00000000},
-	{0x0000a1e4, 0x00000000},
-	{0x0000a1e8, 0x00000000},
-	{0x0000a1ec, 0x00000000},
-	{0x0000a1f0, 0x00000396},
-	{0x0000a1f4, 0x00000396},
-	{0x0000a1f8, 0x00000396},
-	{0x0000a1fc, 0x00000196},
-	{0x0000b000, 0x00010000},
-	{0x0000b004, 0x00030002},
-	{0x0000b008, 0x00050004},
-	{0x0000b00c, 0x00810080},
-	{0x0000b010, 0x00830082},
-	{0x0000b014, 0x01810180},
-	{0x0000b018, 0x01830182},
-	{0x0000b01c, 0x01850184},
-	{0x0000b020, 0x02810280},
-	{0x0000b024, 0x02830282},
-	{0x0000b028, 0x02850284},
-	{0x0000b02c, 0x02890288},
-	{0x0000b030, 0x028b028a},
-	{0x0000b034, 0x0388028c},
-	{0x0000b038, 0x038a0389},
-	{0x0000b03c, 0x038c038b},
-	{0x0000b040, 0x0390038d},
-	{0x0000b044, 0x03920391},
-	{0x0000b048, 0x03940393},
-	{0x0000b04c, 0x03960395},
-	{0x0000b050, 0x00000000},
-	{0x0000b054, 0x00000000},
-	{0x0000b058, 0x00000000},
-	{0x0000b05c, 0x00000000},
-	{0x0000b060, 0x00000000},
-	{0x0000b064, 0x00000000},
-	{0x0000b068, 0x00000000},
-	{0x0000b06c, 0x00000000},
-	{0x0000b070, 0x00000000},
-	{0x0000b074, 0x00000000},
-	{0x0000b078, 0x00000000},
-	{0x0000b07c, 0x00000000},
-	{0x0000b080, 0x2a2d2f32},
-	{0x0000b084, 0x21232328},
-	{0x0000b088, 0x19191c1e},
-	{0x0000b08c, 0x12141417},
-	{0x0000b090, 0x07070e0e},
-	{0x0000b094, 0x03030305},
-	{0x0000b098, 0x00000003},
-	{0x0000b09c, 0x00000000},
-	{0x0000b0a0, 0x00000000},
-	{0x0000b0a4, 0x00000000},
-	{0x0000b0a8, 0x00000000},
-	{0x0000b0ac, 0x00000000},
-	{0x0000b0b0, 0x00000000},
-	{0x0000b0b4, 0x00000000},
-	{0x0000b0b8, 0x00000000},
-	{0x0000b0bc, 0x00000000},
-	{0x0000b0c0, 0x003f0020},
-	{0x0000b0c4, 0x00400041},
-	{0x0000b0c8, 0x0140005f},
-	{0x0000b0cc, 0x0160015f},
-	{0x0000b0d0, 0x017e017f},
-	{0x0000b0d4, 0x02410242},
-	{0x0000b0d8, 0x025f0240},
-	{0x0000b0dc, 0x027f0260},
-	{0x0000b0e0, 0x0341027e},
-	{0x0000b0e4, 0x035f0340},
-	{0x0000b0e8, 0x037f0360},
-	{0x0000b0ec, 0x04400441},
-	{0x0000b0f0, 0x0460045f},
-	{0x0000b0f4, 0x0541047f},
-	{0x0000b0f8, 0x055f0540},
-	{0x0000b0fc, 0x057f0560},
-	{0x0000b100, 0x06400641},
-	{0x0000b104, 0x0660065f},
-	{0x0000b108, 0x067e067f},
-	{0x0000b10c, 0x07410742},
-	{0x0000b110, 0x075f0740},
-	{0x0000b114, 0x077f0760},
-	{0x0000b118, 0x07800781},
-	{0x0000b11c, 0x07a0079f},
-	{0x0000b120, 0x07c107bf},
-	{0x0000b124, 0x000007c0},
-	{0x0000b128, 0x00000000},
-	{0x0000b12c, 0x00000000},
-	{0x0000b130, 0x00000000},
-	{0x0000b134, 0x00000000},
-	{0x0000b138, 0x00000000},
-	{0x0000b13c, 0x00000000},
-	{0x0000b140, 0x003f0020},
-	{0x0000b144, 0x00400041},
-	{0x0000b148, 0x0140005f},
-	{0x0000b14c, 0x0160015f},
-	{0x0000b150, 0x017e017f},
-	{0x0000b154, 0x02410242},
-	{0x0000b158, 0x025f0240},
-	{0x0000b15c, 0x027f0260},
-	{0x0000b160, 0x0341027e},
-	{0x0000b164, 0x035f0340},
-	{0x0000b168, 0x037f0360},
-	{0x0000b16c, 0x04400441},
-	{0x0000b170, 0x0460045f},
-	{0x0000b174, 0x0541047f},
-	{0x0000b178, 0x055f0540},
-	{0x0000b17c, 0x057f0560},
-	{0x0000b180, 0x06400641},
-	{0x0000b184, 0x0660065f},
-	{0x0000b188, 0x067e067f},
-	{0x0000b18c, 0x07410742},
-	{0x0000b190, 0x075f0740},
-	{0x0000b194, 0x077f0760},
-	{0x0000b198, 0x07800781},
-	{0x0000b19c, 0x07a0079f},
-	{0x0000b1a0, 0x07c107bf},
-	{0x0000b1a4, 0x000007c0},
-	{0x0000b1a8, 0x00000000},
-	{0x0000b1ac, 0x00000000},
-	{0x0000b1b0, 0x00000000},
-	{0x0000b1b4, 0x00000000},
-	{0x0000b1b8, 0x00000000},
-	{0x0000b1bc, 0x00000000},
-	{0x0000b1c0, 0x00000000},
-	{0x0000b1c4, 0x00000000},
-	{0x0000b1c8, 0x00000000},
-	{0x0000b1cc, 0x00000000},
-	{0x0000b1d0, 0x00000000},
-	{0x0000b1d4, 0x00000000},
-	{0x0000b1d8, 0x00000000},
-	{0x0000b1dc, 0x00000000},
-	{0x0000b1e0, 0x00000000},
-	{0x0000b1e4, 0x00000000},
-	{0x0000b1e8, 0x00000000},
-	{0x0000b1ec, 0x00000000},
-	{0x0000b1f0, 0x00000396},
-	{0x0000b1f4, 0x00000396},
-	{0x0000b1f8, 0x00000396},
-	{0x0000b1fc, 0x00000196},
-};
+#define ar9580_1p0_rx_gain_table ar9462_common_rx_gain_table_2p0
 
 static const u32 ar9580_1p0_radio_core[][2] = {
 	/* Addr      allmodes  */
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 4866550ddd9..b09285c36c4 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -297,6 +297,8 @@ struct ath_tx {
 	struct ath_txq txq[ATH9K_NUM_TX_QUEUES];
 	struct ath_descdma txdma;
 	struct ath_txq *txq_map[WME_NUM_AC];
+	u32 txq_max_pending[WME_NUM_AC];
+	u16 max_aggr_framelen[WME_NUM_AC][4][32];
 };
 
 struct ath_rx_edma {
@@ -308,6 +310,7 @@ struct ath_rx {
 	u8 defant;
 	u8 rxotherant;
 	u32 *rxlink;
+	u32 num_pkts;
 	unsigned int rxfilter;
 	spinlock_t rxbuflock;
 	struct list_head rxbuf;
@@ -326,6 +329,9 @@ int ath_rx_init(struct ath_softc *sc, int nbufs);
 void ath_rx_cleanup(struct ath_softc *sc);
 int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp);
 struct ath_txq *ath_txq_setup(struct ath_softc *sc, int qtype, int subtype);
+void ath_txq_lock(struct ath_softc *sc, struct ath_txq *txq);
+void ath_txq_unlock(struct ath_softc *sc, struct ath_txq *txq);
+void ath_txq_unlock_complete(struct ath_softc *sc, struct ath_txq *txq);
 void ath_tx_cleanupq(struct ath_softc *sc, struct ath_txq *txq);
 bool ath_drain_all_txq(struct ath_softc *sc, bool retry_tx);
 void ath_draintxq(struct ath_softc *sc,
@@ -337,6 +343,7 @@ int ath_tx_init(struct ath_softc *sc, int nbufs);
 void ath_tx_cleanup(struct ath_softc *sc);
 int ath_txq_update(struct ath_softc *sc, int qnum,
 		   struct ath9k_tx_queue_info *q);
+void ath_update_max_aggr_framelen(struct ath_softc *sc, int queue, int txop);
 int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 		 struct ath_tx_control *txctl);
 void ath_tx_tasklet(struct ath_softc *sc);
@@ -356,7 +363,7 @@ void ath_tx_aggr_sleep(struct ieee80211_sta *sta, struct ath_softc *sc,
 
 struct ath_vif {
 	int av_bslot;
-	bool is_bslot_active, primary_sta_vif;
+	bool primary_sta_vif;
 	__le64 tsf_adjust; /* TSF adjustment for staggered beacons */
 	struct ath_buf *av_bcbuf;
 };
@@ -382,6 +389,7 @@ struct ath_beacon_config {
 	u16 dtim_period;
 	u16 bmiss_timeout;
 	u8 dtim_count;
+	bool enable_beacon;
 };
 
 struct ath_beacon {
@@ -393,7 +401,6 @@ struct ath_beacon {
 
 	u32 beaconq;
 	u32 bmisscnt;
-	u32 ast_be_xmit;
 	u32 bc_tstamp;
 	struct ieee80211_vif *bslot[ATH_BCBUF];
 	int slottime;
@@ -407,17 +414,19 @@ struct ath_beacon {
 	bool tx_last;
 };
 
-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_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 ath_set_beacon(struct ath_softc *sc);
+void ath9k_beacon_tasklet(unsigned long data);
+bool ath9k_allow_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif);
+void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif,
+			 u32 changed);
+void ath9k_beacon_assign_slot(struct ath_softc *sc, struct ieee80211_vif *vif);
+void ath9k_beacon_remove_slot(struct ath_softc *sc, struct ieee80211_vif *vif);
+void ath9k_set_tsfadjust(struct ath_softc *sc, struct ieee80211_vif *vif);
+void ath9k_set_beacon(struct ath_softc *sc);
 void ath9k_set_beaconing_status(struct ath_softc *sc, bool status);
 
-/*******/
-/* ANI */
-/*******/
+/*******************/
+/* Link Monitoring */
+/*******************/
 
 #define ATH_STA_SHORT_CALINTERVAL 1000    /* 1 second */
 #define ATH_AP_SHORT_CALINTERVAL  100     /* 100 ms */
@@ -428,7 +437,9 @@ void ath9k_set_beaconing_status(struct ath_softc *sc, bool status);
 #define ATH_RESTART_CALINTERVAL   1200000 /* 20 minutes */
 
 #define ATH_PAPRD_TIMEOUT	100 /* msecs */
+#define ATH_PLL_WORK_INTERVAL   100
 
+void ath_tx_complete_poll_work(struct work_struct *work);
 void ath_reset_work(struct work_struct *work);
 void ath_hw_check(struct work_struct *work);
 void ath_hw_pll_work(struct work_struct *work);
@@ -436,23 +447,35 @@ void ath_rx_poll(unsigned long data);
 void ath_start_rx_poll(struct ath_softc *sc, u8 nbeacon);
 void ath_paprd_calibrate(struct work_struct *work);
 void ath_ani_calibrate(unsigned long data);
-void ath_start_ani(struct ath_common *common);
+void ath_start_ani(struct ath_softc *sc);
+void ath_stop_ani(struct ath_softc *sc);
+void ath_check_ani(struct ath_softc *sc);
+int ath_update_survey_stats(struct ath_softc *sc);
+void ath_update_survey_nf(struct ath_softc *sc, int channel);
+void ath9k_queue_reset(struct ath_softc *sc, enum ath_reset_type type);
 
 /**********/
 /* BTCOEX */
 /**********/
 
+enum bt_op_flags {
+	BT_OP_PRIORITY_DETECTED,
+	BT_OP_SCAN,
+};
+
 struct ath_btcoex {
 	bool hw_timer_enabled;
 	spinlock_t btcoex_lock;
 	struct timer_list period_timer; /* Timer for BT period */
 	u32 bt_priority_cnt;
 	unsigned long bt_priority_time;
+	unsigned long op_flags;
 	int bt_stomp_type; /* Types of BT stomping */
 	u32 btcoex_no_stomp; /* in usec */
 	u32 btcoex_period; /* in usec */
 	u32 btscan_no_stomp; /* in usec */
 	u32 duty_cycle;
+	u32 bt_wait_time;
 	struct ath_gen_timer *no_stomp_timer; /* Timer for no BT stomping */
 	struct ath_mci_profile mci;
 };
@@ -466,6 +489,7 @@ void ath9k_btcoex_timer_resume(struct ath_softc *sc);
 void ath9k_btcoex_timer_pause(struct ath_softc *sc);
 void ath9k_btcoex_handle_interrupt(struct ath_softc *sc, u32 status);
 u16 ath9k_btcoex_aggr_limit(struct ath_softc *sc, u32 max_4ms_framelen);
+void ath9k_btcoex_stop_gen_timer(struct ath_softc *sc);
 #else
 static inline int ath9k_init_btcoex(struct ath_softc *sc)
 {
@@ -489,8 +513,17 @@ static inline u16 ath9k_btcoex_aggr_limit(struct ath_softc *sc,
 {
 	return 0;
 }
+static inline void ath9k_btcoex_stop_gen_timer(struct ath_softc *sc)
+{
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
+struct ath9k_wow_pattern {
+	u8 pattern_bytes[MAX_PATTERN_SIZE];
+	u8 mask_bytes[MAX_PATTERN_SIZE];
+	u32 pattern_len;
+};
+
 /********************/
 /*   LED Control    */
 /********************/
@@ -514,8 +547,10 @@ static inline void ath_deinit_leds(struct ath_softc *sc)
 }
 #endif
 
-
+/*******************************/
 /* Antenna diversity/combining */
+/*******************************/
+
 #define ATH_ANT_RX_CURRENT_SHIFT 4
 #define ATH_ANT_RX_MAIN_SHIFT 2
 #define ATH_ANT_RX_MASK 0x3
@@ -568,6 +603,9 @@ struct ath_ant_comb {
 	unsigned long scan_start_time;
 };
 
+void ath_ant_comb_scan(struct ath_softc *sc, struct ath_rx_status *rs);
+void ath_ant_comb_update(struct ath_softc *sc);
+
 /********************/
 /* Main driver core */
 /********************/
@@ -585,15 +623,14 @@ struct ath_ant_comb {
 #define ATH_TXPOWER_MAX         100     /* .5 dBm units */
 #define ATH_RATE_DUMMY_MARKER   0
 
-#define SC_OP_INVALID                BIT(0)
-#define SC_OP_BEACONS                BIT(1)
-#define SC_OP_OFFCHANNEL             BIT(2)
-#define SC_OP_RXFLUSH                BIT(3)
-#define SC_OP_TSF_RESET              BIT(4)
-#define SC_OP_BT_PRIORITY_DETECTED   BIT(5)
-#define SC_OP_BT_SCAN                BIT(6)
-#define SC_OP_ANI_RUN                BIT(7)
-#define SC_OP_PRIM_STA_VIF           BIT(8)
+enum sc_op_flags {
+	SC_OP_INVALID,
+	SC_OP_BEACONS,
+	SC_OP_RXFLUSH,
+	SC_OP_ANI_RUN,
+	SC_OP_PRIM_STA_VIF,
+	SC_OP_HW_RESET,
+};
 
 /* Powersave flags */
 #define PS_WAIT_FOR_BEACON        BIT(0)
@@ -639,9 +676,9 @@ struct ath_softc {
 	struct completion paprd_complete;
 
 	unsigned int hw_busy_count;
+	unsigned long sc_flags;
 
 	u32 intrstatus;
-	u32 sc_flags; /* SC_OP_* */
 	u16 ps_flags; /* PS_* */
 	u16 curtxpow;
 	bool ps_enabled;
@@ -679,6 +716,7 @@ struct ath_softc {
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
 	struct ath_btcoex btcoex;
 	struct ath_mci_coex mci_coex;
+	struct work_struct mci_work;
 #endif
 
 	struct ath_descdma txsdma;
@@ -686,6 +724,13 @@ struct ath_softc {
 	struct ath_ant_comb ant_comb;
 	u8 ant_tx, ant_rx;
 	struct dfs_pattern_detector *dfs_detector;
+	u32 wow_enabled;
+
+#ifdef CONFIG_PM_SLEEP
+	atomic_t wow_got_bmiss_intr;
+	atomic_t wow_sleep_proc_intr; /* in the middle of WoW sleep ? */
+	u32 wow_intr_before_sleep;
+#endif
 };
 
 void ath9k_tasklet(unsigned long data);
@@ -701,6 +746,7 @@ extern int ath9k_modparam_nohwcrypt;
 extern int led_blink;
 extern bool is_ath9k_unloaded;
 
+u8 ath9k_parse_mpdudensity(u8 mpdudensity);
 irqreturn_t ath_isr(int irq, void *dev);
 int ath9k_init_device(u16 devid, struct ath_softc *sc,
 		    const struct ath_bus_ops *bus_ops);
@@ -737,5 +783,4 @@ 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 11bc55e3d69..76f07d8c272 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -30,7 +30,7 @@ static void ath9k_reset_beacon_status(struct ath_softc *sc)
  *  the operating mode of the station (AP or AdHoc).  Parameters are AIFS
  *  settings and channel width min/max
 */
-int ath_beaconq_config(struct ath_softc *sc)
+static void ath9k_beaconq_config(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -38,6 +38,7 @@ int ath_beaconq_config(struct ath_softc *sc)
 	struct ath_txq *txq;
 
 	ath9k_hw_get_txq_props(ah, sc->beacon.beaconq, &qi);
+
 	if (sc->sc_ah->opmode == NL80211_IFTYPE_AP) {
 		/* Always burst out beacon and CAB traffic. */
 		qi.tqi_aifs = 1;
@@ -48,17 +49,17 @@ int ath_beaconq_config(struct ath_softc *sc)
 		txq = sc->tx.txq_map[WME_AC_BE];
 		ath9k_hw_get_txq_props(ah, txq->axq_qnum, &qi_be);
 		qi.tqi_aifs = qi_be.tqi_aifs;
-		qi.tqi_cwmin = 4*qi_be.tqi_cwmin;
+		if (ah->slottime == ATH9K_SLOT_TIME_20)
+			qi.tqi_cwmin = 2*qi_be.tqi_cwmin;
+		else
+			qi.tqi_cwmin = 4*qi_be.tqi_cwmin;
 		qi.tqi_cwmax = qi_be.tqi_cwmax;
 	}
 
 	if (!ath9k_hw_set_txq_props(ah, sc->beacon.beaconq, &qi)) {
-		ath_err(common,
-			"Unable to update h/w beacon queue parameters\n");
-		return 0;
+		ath_err(common, "Unable to update h/w beacon queue parameters\n");
 	} else {
 		ath9k_hw_resettxqueue(ah, sc->beacon.beaconq);
-		return 1;
 	}
 }
 
@@ -67,7 +68,7 @@ int ath_beaconq_config(struct ath_softc *sc)
  *  up rate codes, and channel flags. Beacons are always sent out at the
  *  lowest rate, and are not retried.
 */
-static void ath_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
+static void ath9k_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 			     struct ath_buf *bf, int rateidx)
 {
 	struct sk_buff *skb = bf->bf_mpdu;
@@ -78,8 +79,6 @@ static void ath_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 	u8 chainmask = ah->txchainmask;
 	u8 rate = 0;
 
-	ath9k_reset_beacon_status(sc);
-
 	sband = &sc->sbands[common->hw->conf.channel->band];
 	rate = sband->bitrates[rateidx].hw_value;
 	if (vif->bss_conf.use_short_preamble)
@@ -108,7 +107,7 @@ static void ath_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 	ath9k_hw_set_txdesc(ah, bf->bf_desc, &info);
 }
 
-static void ath_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void ath9k_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
@@ -125,28 +124,22 @@ 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)
+static struct ath_buf *ath9k_beacon_generate(struct ieee80211_hw *hw,
+					     struct ieee80211_vif *vif)
 {
 	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_buf *bf;
-	struct ath_vif *avp;
+	struct ath_vif *avp = (void *)vif->drv_priv;
 	struct sk_buff *skb;
-	struct ath_txq *cabq;
+	struct ath_txq *cabq = sc->beacon.cabq;
 	struct ieee80211_tx_info *info;
+	struct ieee80211_mgmt *mgmt_hdr;
 	int cabq_depth;
 
-	ath9k_reset_beacon_status(sc);
-
-	avp = (void *)vif->drv_priv;
-	cabq = sc->beacon.cabq;
-
-	if ((avp->av_bcbuf == NULL) || !avp->is_bslot_active)
+	if (avp->av_bcbuf == NULL)
 		return NULL;
 
-	/* Release the old beacon first */
-
 	bf = avp->av_bcbuf;
 	skb = bf->bf_mpdu;
 	if (skb) {
@@ -156,14 +149,14 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
 		bf->bf_buf_addr = 0;
 	}
 
-	/* Get a new beacon from mac80211 */
-
 	skb = ieee80211_beacon_get(hw, vif);
-	bf->bf_mpdu = skb;
 	if (skb == NULL)
 		return NULL;
-	((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp =
-		avp->tsf_adjust;
+
+	bf->bf_mpdu = skb;
+
+	mgmt_hdr = (struct ieee80211_mgmt *)skb->data;
+	mgmt_hdr->u.beacon.timestamp = avp->tsf_adjust;
 
 	info = IEEE80211_SKB_CB(skb);
 	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
@@ -209,61 +202,52 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
 		}
 	}
 
-	ath_beacon_setup(sc, vif, bf, info->control.rates[0].idx);
+	ath9k_beacon_setup(sc, vif, bf, info->control.rates[0].idx);
 
 	while (skb) {
-		ath_tx_cabq(hw, skb);
+		ath9k_tx_cabq(hw, skb);
 		skb = ieee80211_get_buffered_bc(hw, vif);
 	}
 
 	return bf;
 }
 
-int ath_beacon_alloc(struct ath_softc *sc, struct ieee80211_vif *vif)
+void ath9k_beacon_assign_slot(struct ath_softc *sc, struct ieee80211_vif *vif)
 {
 	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;
-
-	/* Allocate a beacon descriptor if we haven't done so. */
-	if (!avp->av_bcbuf) {
-		/* Allocate beacon state for hostap/ibss.  We know
-		 * a buffer is available. */
-		avp->av_bcbuf = list_first_entry(&sc->beacon.bbuf,
-						 struct ath_buf, list);
-		list_del(&avp->av_bcbuf->list);
-
-		if (ath9k_uses_beacons(vif->type)) {
-			int slot;
-			/*
-			 * Assign the vif to a beacon xmit slot. As
-			 * above, this cannot fail to find one.
-			 */
-			avp->av_bslot = 0;
-			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])
-						break;
-				}
-			BUG_ON(sc->beacon.bslot[avp->av_bslot] != NULL);
-			sc->beacon.bslot[avp->av_bslot] = vif;
-			sc->nbcnvifs++;
+	struct ath_vif *avp = (void *)vif->drv_priv;
+	int slot;
+
+	avp->av_bcbuf = list_first_entry(&sc->beacon.bbuf, struct ath_buf, list);
+	list_del(&avp->av_bcbuf->list);
+
+	for (slot = 0; slot < ATH_BCBUF; slot++) {
+		if (sc->beacon.bslot[slot] == NULL) {
+			avp->av_bslot = slot;
+			break;
 		}
 	}
 
-	/* release the previous beacon frame, if it already exists. */
-	bf = avp->av_bcbuf;
-	if (bf->bf_mpdu != NULL) {
-		skb = bf->bf_mpdu;
+	sc->beacon.bslot[avp->av_bslot] = vif;
+	sc->nbcnvifs++;
+
+	ath_dbg(common, CONFIG, "Added interface at beacon slot: %d\n",
+		avp->av_bslot);
+}
+
+void ath9k_beacon_remove_slot(struct ath_softc *sc, struct ieee80211_vif *vif)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_vif *avp = (void *)vif->drv_priv;
+	struct ath_buf *bf = avp->av_bcbuf;
+
+	ath_dbg(common, CONFIG, "Removing interface at beacon slot: %d\n",
+		avp->av_bslot);
+
+	tasklet_disable(&sc->bcon_tasklet);
+
+	if (bf && bf->bf_mpdu) {
+		struct sk_buff *skb = bf->bf_mpdu;
 		dma_unmap_single(sc->dev, bf->bf_buf_addr,
 				 skb->len, DMA_TO_DEVICE);
 		dev_kfree_skb_any(skb);
@@ -271,99 +255,74 @@ int ath_beacon_alloc(struct ath_softc *sc, struct ieee80211_vif *vif)
 		bf->bf_buf_addr = 0;
 	}
 
-	/* NB: the beacon data buffer must be 32-bit aligned. */
-	skb = ieee80211_beacon_get(sc->hw, vif);
-	if (skb == NULL)
-		return -ENOMEM;
+	avp->av_bcbuf = NULL;
+	sc->beacon.bslot[avp->av_bslot] = NULL;
+	sc->nbcnvifs--;
+	list_add_tail(&bf->list, &sc->beacon.bbuf);
 
-	tstamp = ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp;
-	sc->beacon.bc_tstamp = (u32) le64_to_cpu(tstamp);
-	/* Calculate a TSF adjustment factor required for staggered beacons. */
-	if (avp->av_bslot > 0) {
-		u64 tsfadjust;
-		int intval;
-
-		intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
+	tasklet_enable(&sc->bcon_tasklet);
+}
 
-		/*
-		 * Calculate the TSF offset for this beacon slot, i.e., the
-		 * number of usecs that need to be added to the timestamp field
-		 * in Beacon and Probe Response frames. Beacon slot 0 is
-		 * processed at the correct offset, so it does not require TSF
-		 * adjustment. Other slots are adjusted to get the timestamp
-		 * close to the TBTT for the BSS.
-		 */
-		tsfadjust = TU_TO_USEC(intval * avp->av_bslot) / ATH_BCBUF;
-		avp->tsf_adjust = cpu_to_le64(tsfadjust);
+static int ath9k_beacon_choose_slot(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
+	u16 intval;
+	u32 tsftu;
+	u64 tsf;
+	int slot;
 
-		ath_dbg(common, BEACON,
-			"stagger beacons, bslot %d intval %u tsfadjust %llu\n",
-			avp->av_bslot, intval, (unsigned long long)tsfadjust);
+	if (sc->sc_ah->opmode != NL80211_IFTYPE_AP) {
+		ath_dbg(common, BEACON, "slot 0, tsf: %llu\n",
+			ath9k_hw_gettsf64(sc->sc_ah));
+		return 0;
+	}
 
-		((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp =
-			avp->tsf_adjust;
-	} else
-		avp->tsf_adjust = cpu_to_le64(0);
+	intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
+	tsf = ath9k_hw_gettsf64(sc->sc_ah);
+	tsf += TU_TO_USEC(sc->sc_ah->config.sw_beacon_response_time);
+	tsftu = TSF_TO_TU((tsf * ATH_BCBUF) >>32, tsf * ATH_BCBUF);
+	slot = (tsftu % (intval * ATH_BCBUF)) / intval;
 
-	bf->bf_mpdu = skb;
-	bf->bf_buf_addr = dma_map_single(sc->dev, skb->data,
-					 skb->len, DMA_TO_DEVICE);
-	if (unlikely(dma_mapping_error(sc->dev, bf->bf_buf_addr))) {
-		dev_kfree_skb_any(skb);
-		bf->bf_mpdu = NULL;
-		bf->bf_buf_addr = 0;
-		ath_err(common, "dma_mapping_error on beacon alloc\n");
-		return -ENOMEM;
-	}
-	avp->is_bslot_active = true;
+	ath_dbg(common, BEACON, "slot: %d tsf: %llu tsftu: %u\n",
+		slot, tsf, tsftu / ATH_BCBUF);
 
-	return 0;
+	return slot;
 }
 
-void ath_beacon_return(struct ath_softc *sc, struct ath_vif *avp)
+void ath9k_set_tsfadjust(struct ath_softc *sc, struct ieee80211_vif *vif)
 {
-	if (avp->av_bcbuf != NULL) {
-		struct ath_buf *bf;
-
-		avp->is_bslot_active = false;
-		if (avp->av_bslot != -1) {
-			sc->beacon.bslot[avp->av_bslot] = NULL;
-			sc->nbcnvifs--;
-			avp->av_bslot = -1;
-		}
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
+	struct ath_vif *avp = (void *)vif->drv_priv;
+	u64 tsfadjust;
 
-		bf = avp->av_bcbuf;
-		if (bf->bf_mpdu != NULL) {
-			struct sk_buff *skb = bf->bf_mpdu;
-			dma_unmap_single(sc->dev, bf->bf_buf_addr,
-					 skb->len, DMA_TO_DEVICE);
-			dev_kfree_skb_any(skb);
-			bf->bf_mpdu = NULL;
-			bf->bf_buf_addr = 0;
-		}
-		list_add_tail(&bf->list, &sc->beacon.bbuf);
+	if (avp->av_bslot == 0)
+		return;
 
-		avp->av_bcbuf = NULL;
-	}
+	tsfadjust = cur_conf->beacon_interval * avp->av_bslot / ATH_BCBUF;
+	avp->tsf_adjust = cpu_to_le64(TU_TO_USEC(tsfadjust));
+
+	ath_dbg(common, CONFIG, "tsfadjust is: %llu for bslot: %d\n",
+		(unsigned long long)tsfadjust, avp->av_bslot);
 }
 
-void ath_beacon_tasklet(unsigned long data)
+void ath9k_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;
 	bool edma = !!(ah->caps.hw_caps & ATH9K_HW_CAP_EDMA);
 	int slot;
-	u32 bfaddr, bc = 0;
 
-	if (work_pending(&sc->hw_reset_work)) {
+	if (test_bit(SC_OP_HW_RESET, &sc->sc_flags)) {
 		ath_dbg(common, RESET,
 			"reset work is pending, skip beaconing now\n");
 		return;
 	}
+
 	/*
 	 * Check if the previous beacon has gone out.  If
 	 * not don't try to post another, skip this period
@@ -387,55 +346,25 @@ void ath_beacon_tasklet(unsigned long data)
 		} else if (sc->beacon.bmisscnt >= BSTUCK_THRESH) {
 			ath_dbg(common, BSTUCK, "beacon is officially stuck\n");
 			sc->beacon.bmisscnt = 0;
-			sc->sc_flags |= SC_OP_TSF_RESET;
-			ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
+			ath9k_queue_reset(sc, RESET_TYPE_BEACON_STUCK);
 		}
 
 		return;
 	}
 
-	/*
-	 * Generate beacon frames. we are sending frames
-	 * staggered so calculate the slot for this frame based
-	 * on the tsf to safeguard against missing an swba.
-	 */
-
-
-	if (ah->opmode == NL80211_IFTYPE_AP) {
-		u16 intval;
-		u32 tsftu;
-		u64 tsf;
-
-		intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
-		tsf = ath9k_hw_gettsf64(ah);
-		tsf += TU_TO_USEC(ah->config.sw_beacon_response_time);
-		tsftu = TSF_TO_TU((tsf * ATH_BCBUF) >>32, tsf * ATH_BCBUF);
-		slot = (tsftu % (intval * ATH_BCBUF)) / intval;
-		vif = sc->beacon.bslot[slot];
-
-		ath_dbg(common, BEACON,
-			"slot %d [tsf %llu tsftu %u intval %u] vif %p\n",
-			slot, tsf, tsftu / ATH_BCBUF, intval, vif);
-	} else {
-		slot = 0;
-		vif = sc->beacon.bslot[slot];
-	}
+	slot = ath9k_beacon_choose_slot(sc);
+	vif = sc->beacon.bslot[slot];
 
+	if (!vif || !vif->bss_conf.enable_beacon)
+		return;
 
-	bfaddr = 0;
-	if (vif) {
-		bf = ath_beacon_generate(sc->hw, vif);
-		if (bf != NULL) {
-			bfaddr = bf->bf_daddr;
-			bc = 1;
-		}
+	bf = ath9k_beacon_generate(sc->hw, vif);
+	WARN_ON(!bf);
 
-		if (sc->beacon.bmisscnt != 0) {
-			ath_dbg(common, BSTUCK,
-				"resume beacon xmit after %u misses\n",
-				sc->beacon.bmisscnt);
-			sc->beacon.bmisscnt = 0;
-		}
+	if (sc->beacon.bmisscnt != 0) {
+		ath_dbg(common, BSTUCK, "resume beacon xmit after %u misses\n",
+			sc->beacon.bmisscnt);
+		sc->beacon.bmisscnt = 0;
 	}
 
 	/*
@@ -455,39 +384,40 @@ void ath_beacon_tasklet(unsigned long data)
 	 *     set to ATH_BCBUF so this check is a noop.
 	 */
 	if (sc->beacon.updateslot == UPDATE) {
-		sc->beacon.updateslot = COMMIT; /* commit next beacon */
+		sc->beacon.updateslot = COMMIT;
 		sc->beacon.slotupdate = slot;
-	} else if (sc->beacon.updateslot == COMMIT && sc->beacon.slotupdate == slot) {
+	} else if (sc->beacon.updateslot == COMMIT &&
+		   sc->beacon.slotupdate == slot) {
 		ah->slottime = sc->beacon.slottime;
 		ath9k_hw_init_global_settings(ah);
 		sc->beacon.updateslot = OK;
 	}
-	if (bfaddr != 0) {
+
+	if (bf) {
+		ath9k_reset_beacon_status(sc);
+
+		ath_dbg(common, BEACON,
+			"Transmitting beacon for slot: %d\n", slot);
+
 		/* NB: cabq traffic should already be queued and primed */
-		ath9k_hw_puttxbuf(ah, sc->beacon.beaconq, bfaddr);
+		ath9k_hw_puttxbuf(ah, sc->beacon.beaconq, bf->bf_daddr);
 
 		if (!edma)
 			ath9k_hw_txstart(ah, sc->beacon.beaconq);
-
-		sc->beacon.ast_be_xmit += bc;     /* XXX per-vif? */
 	}
 }
 
-static void ath9k_beacon_init(struct ath_softc *sc,
-			      u32 next_beacon,
-			      u32 beacon_period)
+static void ath9k_beacon_init(struct ath_softc *sc, u32 nexttbtt, u32 intval)
 {
-	if (sc->sc_flags & SC_OP_TSF_RESET) {
-		ath9k_ps_wakeup(sc);
-		ath9k_hw_reset_tsf(sc->sc_ah);
-	}
-
-	ath9k_hw_beaconinit(sc->sc_ah, next_beacon, beacon_period);
+	struct ath_hw *ah = sc->sc_ah;
 
-	if (sc->sc_flags & SC_OP_TSF_RESET) {
-		ath9k_ps_restore(sc);
-		sc->sc_flags &= ~SC_OP_TSF_RESET;
-	}
+	ath9k_hw_disable_interrupts(ah);
+	ath9k_hw_reset_tsf(ah);
+	ath9k_beaconq_config(sc);
+	ath9k_hw_beaconinit(ah, nexttbtt, intval);
+	sc->beacon.bmisscnt = 0;
+	ath9k_hw_set_interrupts(ah);
+	ath9k_hw_enable_interrupts(ah);
 }
 
 /*
@@ -495,32 +425,27 @@ static void ath9k_beacon_init(struct ath_softc *sc,
  * burst together.  For the former arrange for the SWBA to be delivered for each
  * slot. Slots that are not occupied will generate nothing.
  */
-static void ath_beacon_config_ap(struct ath_softc *sc,
-				 struct ath_beacon_config *conf)
+static void ath9k_beacon_config_ap(struct ath_softc *sc,
+				   struct ath_beacon_config *conf)
 {
 	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 	u32 nexttbtt, intval;
 
 	/* NB: the beacon interval is kept internally in TU's */
 	intval = TU_TO_USEC(conf->beacon_interval);
-	intval /= ATH_BCBUF;    /* for staggered beacons */
+	intval /= ATH_BCBUF;
 	nexttbtt = intval;
 
-	/*
-	 * In AP mode we enable the beacon timers and SWBA interrupts to
-	 * prepare beacon frames.
-	 */
-	ah->imask |= ATH9K_INT_SWBA;
-	ath_beaconq_config(sc);
+	if (conf->enable_beacon)
+		ah->imask |= ATH9K_INT_SWBA;
+	else
+		ah->imask &= ~ATH9K_INT_SWBA;
 
-	/* Set the computed AP beacon timers */
+	ath_dbg(common, BEACON, "AP nexttbtt: %u intval: %u conf_intval: %u\n",
+		nexttbtt, intval, conf->beacon_interval);
 
-	ath9k_hw_disable_interrupts(ah);
-	sc->sc_flags |= SC_OP_TSF_RESET;
 	ath9k_beacon_init(sc, nexttbtt, intval);
-	sc->beacon.bmisscnt = 0;
-	ath9k_hw_set_interrupts(ah);
-	ath9k_hw_enable_interrupts(ah);
 }
 
 /*
@@ -531,8 +456,8 @@ static void ath_beacon_config_ap(struct ath_softc *sc,
  * we'll receive a BMISS interrupt when we stop seeing beacons from the AP
  * we've associated with.
  */
-static void ath_beacon_config_sta(struct ath_softc *sc,
-				  struct ath_beacon_config *conf)
+static void ath9k_beacon_config_sta(struct ath_softc *sc,
+				    struct ath_beacon_config *conf)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -544,7 +469,7 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
 	int num_beacons, offset, dtim_dec_count, cfp_dec_count;
 
 	/* No need to configure beacon if we are not associated */
-	if (!common->curaid) {
+	if (!test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags)) {
 		ath_dbg(common, BEACON,
 			"STA is not yet associated..skipping beacon config\n");
 		return;
@@ -651,97 +576,65 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
 	ath9k_hw_enable_interrupts(ah);
 }
 
-static void ath_beacon_config_adhoc(struct ath_softc *sc,
-				    struct ath_beacon_config *conf)
+static void ath9k_beacon_config_adhoc(struct ath_softc *sc,
+				      struct ath_beacon_config *conf)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
-	u32 tsf, intval, nexttbtt;
+	u32 intval, nexttbtt;
 
 	ath9k_reset_beacon_status(sc);
-	if (!(sc->sc_flags & SC_OP_BEACONS))
-		ath9k_hw_settsf64(ah, sc->beacon.bc_tstamp);
 
 	intval = TU_TO_USEC(conf->beacon_interval);
-	tsf = roundup(ath9k_hw_gettsf32(ah) + TU_TO_USEC(FUDGE), intval);
-	nexttbtt = tsf + intval;
-
-	ath_dbg(common, BEACON, "IBSS nexttbtt %u intval %u (%u)\n",
-		nexttbtt, intval, conf->beacon_interval);
-
-	/*
-	 * In IBSS mode enable the beacon timers but only enable SWBA interrupts
-	 * if we need to manually prepare beacon frames.  Otherwise we use a
-	 * self-linked tx descriptor and let the hardware deal with things.
-	 */
-	ah->imask |= ATH9K_INT_SWBA;
+	nexttbtt = intval;
 
-	ath_beaconq_config(sc);
+	if (conf->enable_beacon)
+		ah->imask |= ATH9K_INT_SWBA;
+	else
+		ah->imask &= ~ATH9K_INT_SWBA;
 
-	/* Set the computed ADHOC beacon timers */
+	ath_dbg(common, BEACON, "IBSS nexttbtt: %u intval: %u conf_intval: %u\n",
+		nexttbtt, intval, conf->beacon_interval);
 
-	ath9k_hw_disable_interrupts(ah);
 	ath9k_beacon_init(sc, nexttbtt, intval);
-	sc->beacon.bmisscnt = 0;
-
-	ath9k_hw_set_interrupts(ah);
-	ath9k_hw_enable_interrupts(ah);
 }
 
-static bool ath9k_allow_beacon_config(struct ath_softc *sc,
-				      struct ieee80211_vif *vif)
+bool ath9k_allow_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
 {
-	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
 	struct ath_vif *avp = (void *)vif->drv_priv;
 
-	/*
-	 * Can not have different beacon interval on multiple
-	 * AP interface case
-	 */
-	if ((sc->sc_ah->opmode == NL80211_IFTYPE_AP) &&
-	    (sc->nbcnvifs > 1) &&
-	    (vif->type == NL80211_IFTYPE_AP) &&
-	    (cur_conf->beacon_interval != bss_conf->beacon_int)) {
-		ath_dbg(common, CONFIG,
-			"Changing beacon interval of multiple AP interfaces !\n");
-		return false;
-	}
-	/*
-	 * Can not configure station vif's beacon config
-	 * while on AP opmode
-	 */
-	if ((sc->sc_ah->opmode == NL80211_IFTYPE_AP) &&
-	    (vif->type != NL80211_IFTYPE_AP)) {
-		ath_dbg(common, CONFIG,
-			"STA vif's beacon not allowed on AP mode\n");
-		return false;
+	if (sc->sc_ah->opmode == NL80211_IFTYPE_AP) {
+		if ((vif->type != NL80211_IFTYPE_AP) ||
+		    (sc->nbcnvifs > 1)) {
+			ath_dbg(common, CONFIG,
+				"An AP interface is already present !\n");
+			return false;
+		}
 	}
-	/*
-	 * Do not allow beacon config if HW was already configured
-	 * with another STA vif
-	 */
-	if ((sc->sc_ah->opmode == NL80211_IFTYPE_STATION) &&
-	    (vif->type == NL80211_IFTYPE_STATION) &&
-	    (sc->sc_flags & SC_OP_BEACONS) &&
-	    !avp->primary_sta_vif) {
-		ath_dbg(common, CONFIG,
-			"Beacon already configured for a station interface\n");
-		return false;
+
+	if (sc->sc_ah->opmode == NL80211_IFTYPE_STATION) {
+		if ((vif->type == NL80211_IFTYPE_STATION) &&
+		    test_bit(SC_OP_BEACONS, &sc->sc_flags) &&
+		    !avp->primary_sta_vif) {
+			ath_dbg(common, CONFIG,
+				"Beacon already configured for a station interface\n");
+			return false;
+		}
 	}
+
 	return true;
 }
 
-void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
+static void ath9k_cache_beacon_config(struct ath_softc *sc,
+				      struct ieee80211_bss_conf *bss_conf)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
-	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
 
-	if (!ath9k_allow_beacon_config(sc, vif))
-		return;
+	ath_dbg(common, BEACON,
+		"Caching beacon data for BSS: %pM\n", bss_conf->bssid);
 
-	/* Setup the beacon configuration parameters */
 	cur_conf->beacon_interval = bss_conf->beacon_int;
 	cur_conf->dtim_period = bss_conf->dtim_period;
 	cur_conf->listen_interval = 1;
@@ -766,73 +659,62 @@ void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
 	if (cur_conf->dtim_period == 0)
 		cur_conf->dtim_period = 1;
 
-	ath_set_beacon(sc);
 }
 
-static bool ath_has_valid_bslot(struct ath_softc *sc)
+void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif,
+			 u32 changed)
 {
-	struct ath_vif *avp;
-	int slot;
-	bool found = false;
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 
-	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 (sc->sc_ah->opmode == NL80211_IFTYPE_STATION) {
+		ath9k_cache_beacon_config(sc, bss_conf);
+		ath9k_set_beacon(sc);
+		set_bit(SC_OP_BEACONS, &sc->sc_flags);
+	} else {
+		/*
+		 * Take care of multiple interfaces when
+		 * enabling/disabling SWBA.
+		 */
+		if (changed & BSS_CHANGED_BEACON_ENABLED) {
+			if (!bss_conf->enable_beacon &&
+			    (sc->nbcnvifs <= 1)) {
+				cur_conf->enable_beacon = false;
+			} else if (bss_conf->enable_beacon) {
+				cur_conf->enable_beacon = true;
+				ath9k_cache_beacon_config(sc, bss_conf);
 			}
 		}
+
+		if (cur_conf->beacon_interval) {
+			ath9k_set_beacon(sc);
+
+			if (cur_conf->enable_beacon)
+				set_bit(SC_OP_BEACONS, &sc->sc_flags);
+			else
+				clear_bit(SC_OP_BEACONS, &sc->sc_flags);
+		}
 	}
-	return found;
 }
 
-
-void ath_set_beacon(struct ath_softc *sc)
+void ath9k_set_beacon(struct ath_softc *sc)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
 
 	switch (sc->sc_ah->opmode) {
 	case NL80211_IFTYPE_AP:
-		if (ath_has_valid_bslot(sc))
-			ath_beacon_config_ap(sc, cur_conf);
+		ath9k_beacon_config_ap(sc, cur_conf);
 		break;
 	case NL80211_IFTYPE_ADHOC:
 	case NL80211_IFTYPE_MESH_POINT:
-		ath_beacon_config_adhoc(sc, cur_conf);
+		ath9k_beacon_config_adhoc(sc, cur_conf);
 		break;
 	case NL80211_IFTYPE_STATION:
-		ath_beacon_config_sta(sc, cur_conf);
+		ath9k_beacon_config_sta(sc, cur_conf);
 		break;
 	default:
 		ath_dbg(common, CONFIG, "Unsupported beaconing mode\n");
 		return;
 	}
-
-	sc->sc_flags |= SC_OP_BEACONS;
-}
-
-void ath9k_set_beaconing_status(struct ath_softc *sc, bool status)
-{
-	struct ath_hw *ah = sc->sc_ah;
-
-	if (!ath_has_valid_bslot(sc)) {
-		sc->sc_flags &= ~SC_OP_BEACONS;
-		return;
-	}
-
-	ath9k_ps_wakeup(sc);
-	if (status) {
-		/* Re-enable beaconing */
-		ah->imask |= ATH9K_INT_SWBA;
-		ath9k_hw_set_interrupts(ah);
-	} else {
-		/* Disable SWBA interrupt */
-		ah->imask &= ~ATH9K_INT_SWBA;
-		ath9k_hw_set_interrupts(ah);
-		tasklet_kill(&sc->bcon_tasklet);
-		ath9k_hw_stop_dma_queue(ah, sc->beacon.beaconq);
-	}
-	ath9k_ps_restore(sc);
 }
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.c b/drivers/net/wireless/ath/ath9k/btcoex.c
index 1ca6da80d4a..acd437384fe 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.c
+++ b/drivers/net/wireless/ath/ath9k/btcoex.c
@@ -336,10 +336,16 @@ static void ar9003_btcoex_bt_stomp(struct ath_hw *ah,
 			 enum ath_stomp_type stomp_type)
 {
 	struct ath_btcoex_hw *btcoex = &ah->btcoex_hw;
-	const u32 *weight = AR_SREV_9462(ah) ? ar9003_wlan_weights[stomp_type] :
-					       ar9462_wlan_weights[stomp_type];
+	const u32 *weight = ar9003_wlan_weights[stomp_type];
 	int i;
 
+	if (AR_SREV_9462(ah)) {
+		if ((stomp_type == ATH_BTCOEX_STOMP_LOW) &&
+		    btcoex->mci.stomp_ftp)
+			stomp_type = ATH_BTCOEX_STOMP_LOW_FTP;
+		weight = ar9462_wlan_weights[stomp_type];
+	}
+
 	for (i = 0; i < AR9300_NUM_WLAN_WEIGHTS; i++) {
 		btcoex->bt_weight[i] = AR9300_BT_WGHT;
 		btcoex->wlan_weight[i] = weight[i];
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.h b/drivers/net/wireless/ath/ath9k/btcoex.h
index 3a1e1cfabd5..20092f98658 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.h
+++ b/drivers/net/wireless/ath/ath9k/btcoex.h
@@ -36,6 +36,9 @@
 #define ATH_BT_CNT_THRESHOLD	       3
 #define ATH_BT_CNT_SCAN_THRESHOLD      15
 
+#define ATH_BTCOEX_RX_WAIT_TIME       100
+#define ATH_BTCOEX_STOMP_FTP_THRESH   5
+
 #define AR9300_NUM_BT_WEIGHTS   4
 #define AR9300_NUM_WLAN_WEIGHTS 4
 /* Defines the BT AR_BT_COEX_WGHT used */
@@ -80,6 +83,7 @@ struct ath9k_hw_mci {
 	u8 bt_ver_major;
 	u8 bt_ver_minor;
 	u8 bt_state;
+	u8 stomp_ftp;
 };
 
 struct ath_btcoex_hw {
diff --git a/drivers/net/wireless/ath/ath9k/calib.h b/drivers/net/wireless/ath/ath9k/calib.h
index 3b33996d97d..1060c19a501 100644
--- a/drivers/net/wireless/ath/ath9k/calib.h
+++ b/drivers/net/wireless/ath/ath9k/calib.h
@@ -30,10 +30,10 @@ struct ar5416IniArray {
 	u32 ia_columns;
 };
 
-#define INIT_INI_ARRAY(iniarray, array, rows, columns) do {	\
+#define INIT_INI_ARRAY(iniarray, array) do {	\
 		(iniarray)->ia_array = (u32 *)(array);		\
-		(iniarray)->ia_rows = (rows);			\
-		(iniarray)->ia_columns = (columns);		\
+		(iniarray)->ia_rows = ARRAY_SIZE(array);	\
+		(iniarray)->ia_columns = ARRAY_SIZE(array[0]);	\
 	} while (0)
 
 #define INI_RA(iniarray, row, column) \
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index fde700c4e49..68b643c8943 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -205,11 +205,10 @@ static ssize_t write_file_disable_ani(struct file *file,
 	common->disable_ani = !!disable_ani;
 
 	if (disable_ani) {
-		sc->sc_flags &= ~SC_OP_ANI_RUN;
-		del_timer_sync(&common->ani.timer);
+		clear_bit(SC_OP_ANI_RUN, &sc->sc_flags);
+		ath_stop_ani(sc);
 	} else {
-		sc->sc_flags |= SC_OP_ANI_RUN;
-		ath_start_ani(common);
+		ath_check_ani(sc);
 	}
 
 	return count;
@@ -348,8 +347,6 @@ void ath_debug_stat_interrupt(struct ath_softc *sc, enum ath9k_int status)
 		sc->debug.stats.istats.txok++;
 	if (status & ATH9K_INT_TXURN)
 		sc->debug.stats.istats.txurn++;
-	if (status & ATH9K_INT_MIB)
-		sc->debug.stats.istats.mib++;
 	if (status & ATH9K_INT_RXPHY)
 		sc->debug.stats.istats.rxphyerr++;
 	if (status & ATH9K_INT_RXKCM)
@@ -374,6 +371,8 @@ void ath_debug_stat_interrupt(struct ath_softc *sc, enum ath9k_int status)
 		sc->debug.stats.istats.dtim++;
 	if (status & ATH9K_INT_TSFOOR)
 		sc->debug.stats.istats.tsfoor++;
+	if (status & ATH9K_INT_MCI)
+		sc->debug.stats.istats.mci++;
 }
 
 static ssize_t read_file_interrupt(struct file *file, char __user *user_buf,
@@ -418,6 +417,7 @@ static ssize_t read_file_interrupt(struct file *file, char __user *user_buf,
 	PR_IS("DTIMSYNC", dtimsync);
 	PR_IS("DTIM", dtim);
 	PR_IS("TSFOOR", tsfoor);
+	PR_IS("MCI", mci);
 	PR_IS("TOTAL", total);
 
 	len += snprintf(buf + len, mxlen - len,
@@ -1318,7 +1318,7 @@ static int open_file_bb_mac_samps(struct inode *inode, struct file *file)
 	u8 chainmask = (ah->rxchainmask << 3) | ah->rxchainmask;
 	u8 nread;
 
-	if (sc->sc_flags & SC_OP_INVALID)
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags))
 		return -EAGAIN;
 
 	buf = vmalloc(size);
@@ -1555,6 +1555,14 @@ int ath9k_init_debug(struct ath_hw *ah)
 			    &fops_interrupt);
 	debugfs_create_file("xmit", S_IRUSR, sc->debug.debugfs_phy, sc,
 			    &fops_xmit);
+	debugfs_create_u32("qlen_bk", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
+			   &sc->tx.txq_max_pending[WME_AC_BK]);
+	debugfs_create_u32("qlen_be", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
+			   &sc->tx.txq_max_pending[WME_AC_BE]);
+	debugfs_create_u32("qlen_vi", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
+			   &sc->tx.txq_max_pending[WME_AC_VI]);
+	debugfs_create_u32("qlen_vo", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
+			   &sc->tx.txq_max_pending[WME_AC_VO]);
 	debugfs_create_file("stations", S_IRUSR, sc->debug.debugfs_phy, sc,
 			    &fops_stations);
 	debugfs_create_file("misc", S_IRUSR, sc->debug.debugfs_phy, sc,
diff --git a/drivers/net/wireless/ath/ath9k/debug.h b/drivers/net/wireless/ath/ath9k/debug.h
index c34da09d910..8b9d080d89d 100644
--- a/drivers/net/wireless/ath/ath9k/debug.h
+++ b/drivers/net/wireless/ath/ath9k/debug.h
@@ -32,6 +32,19 @@ struct ath_buf;
 #define RESET_STAT_INC(sc, type) do { } while (0)
 #endif
 
+enum ath_reset_type {
+	RESET_TYPE_BB_HANG,
+	RESET_TYPE_BB_WATCHDOG,
+	RESET_TYPE_FATAL_INT,
+	RESET_TYPE_TX_ERROR,
+	RESET_TYPE_TX_HANG,
+	RESET_TYPE_PLL_HANG,
+	RESET_TYPE_MAC_HANG,
+	RESET_TYPE_BEACON_STUCK,
+	RESET_TYPE_MCI,
+	__RESET_TYPE_MAX
+};
+
 #ifdef CONFIG_ATH9K_DEBUGFS
 
 /**
@@ -86,6 +99,7 @@ struct ath_interrupt_stats {
 	u32 dtim;
 	u32 bb_watchdog;
 	u32 tsfoor;
+	u32 mci;
 
 	/* Sync-cause stats */
 	u32 sync_cause_all;
@@ -208,17 +222,6 @@ struct ath_rx_stats {
 	u32 rx_frags;
 };
 
-enum ath_reset_type {
-	RESET_TYPE_BB_HANG,
-	RESET_TYPE_BB_WATCHDOG,
-	RESET_TYPE_FATAL_INT,
-	RESET_TYPE_TX_ERROR,
-	RESET_TYPE_TX_HANG,
-	RESET_TYPE_PLL_HANG,
-	RESET_TYPE_MAC_HANG,
-	__RESET_TYPE_MAX
-};
-
 struct ath_stats {
 	struct ath_interrupt_stats istats;
 	struct ath_tx_stats txstats[ATH9K_NUM_TX_QUEUES];
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.h b/drivers/net/wireless/ath/ath9k/eeprom.h
index 33acb920ed3..484b3130590 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.h
+++ b/drivers/net/wireless/ath/ath9k/eeprom.h
@@ -241,16 +241,12 @@ enum eeprom_param {
 	EEP_TEMPSENSE_SLOPE,
 	EEP_TEMPSENSE_SLOPE_PAL_ON,
 	EEP_PWR_TABLE_OFFSET,
-	EEP_DRIVE_STRENGTH,
-	EEP_INTERNAL_REGULATOR,
-	EEP_SWREG,
 	EEP_PAPRD,
 	EEP_MODAL_VER,
 	EEP_ANT_DIV_CTL1,
 	EEP_CHAIN_MASK_REDUCE,
 	EEP_ANTENNA_GAIN_2G,
 	EEP_ANTENNA_GAIN_5G,
-	EEP_QUICK_DROP
 };
 
 enum ar5416_rates {
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_4k.c b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
index 4322ac80c20..7d075105a85 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_4k.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
@@ -135,7 +135,7 @@ static u32 ath9k_hw_4k_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 	if (!dump_base_hdr) {
 		len += snprintf(buf + len, size - len,
 				"%20s :\n", "2GHz modal Header");
-		len += ath9k_dump_4k_modal_eeprom(buf, len, size,
+		len = ath9k_dump_4k_modal_eeprom(buf, len, size,
 						  &eep->modalHeader);
 		goto out;
 	}
@@ -188,8 +188,7 @@ static int ath9k_hw_4k_check_eeprom(struct ath_hw *ah)
 {
 #define EEPROM_4K_SIZE (sizeof(struct ar5416_eeprom_4k) / sizeof(u16))
 	struct ath_common *common = ath9k_hw_common(ah);
-	struct ar5416_eeprom_4k *eep =
-		(struct ar5416_eeprom_4k *) &ah->eeprom.map4k;
+	struct ar5416_eeprom_4k *eep = &ah->eeprom.map4k;
 	u16 *eepdata, temp, magic, magic2;
 	u32 sum = 0, el;
 	bool need_swap = false;
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_9287.c b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
index aa614767adf..cd742fb944c 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_9287.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
@@ -132,7 +132,7 @@ static u32 ath9k_hw_ar9287_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 	if (!dump_base_hdr) {
 		len += snprintf(buf + len, size - len,
 				"%20s :\n", "2GHz modal Header");
-		len += ar9287_dump_modal_eeprom(buf, len, size,
+		len = ar9287_dump_modal_eeprom(buf, len, size,
 						&eep->modalHeader);
 		goto out;
 	}
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_def.c b/drivers/net/wireless/ath/ath9k/eeprom_def.c
index b5fba8b18b8..a8ac30a0072 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_def.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_def.c
@@ -211,11 +211,11 @@ static u32 ath9k_hw_def_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 	if (!dump_base_hdr) {
 		len += snprintf(buf + len, size - len,
 				"%20s :\n", "2GHz modal Header");
-		len += ath9k_def_dump_modal_eeprom(buf, len, size,
+		len = ath9k_def_dump_modal_eeprom(buf, len, size,
 						   &eep->modalHeader[0]);
 		len += snprintf(buf + len, size - len,
 				"%20s :\n", "5GHz modal Header");
-		len += ath9k_def_dump_modal_eeprom(buf, len, size,
+		len = ath9k_def_dump_modal_eeprom(buf, len, size,
 						   &eep->modalHeader[1]);
 		goto out;
 	}
@@ -264,8 +264,7 @@ static u32 ath9k_hw_def_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 
 static int ath9k_hw_def_check_eeprom(struct ath_hw *ah)
 {
-	struct ar5416_eeprom_def *eep =
-		(struct ar5416_eeprom_def *) &ah->eeprom.def;
+	struct ar5416_eeprom_def *eep = &ah->eeprom.def;
 	struct ath_common *common = ath9k_hw_common(ah);
 	u16 *eepdata, temp, magic, magic2;
 	u32 sum = 0, el;
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index 281a9af0f1b..bacdb8fb4ef 100644
--- a/drivers/net/wireless/ath/ath9k/gpio.c
+++ b/drivers/net/wireless/ath/ath9k/gpio.c
@@ -132,17 +132,18 @@ static void ath_detect_bt_priority(struct ath_softc *sc)
 
 	if (time_after(jiffies, btcoex->bt_priority_time +
 			msecs_to_jiffies(ATH_BT_PRIORITY_TIME_THRESHOLD))) {
-		sc->sc_flags &= ~(SC_OP_BT_PRIORITY_DETECTED | SC_OP_BT_SCAN);
+		clear_bit(BT_OP_PRIORITY_DETECTED, &btcoex->op_flags);
+		clear_bit(BT_OP_SCAN, &btcoex->op_flags);
 		/* Detect if colocated bt started scanning */
 		if (btcoex->bt_priority_cnt >= ATH_BT_CNT_SCAN_THRESHOLD) {
 			ath_dbg(ath9k_hw_common(sc->sc_ah), BTCOEX,
 				"BT scan detected\n");
-			sc->sc_flags |= (SC_OP_BT_SCAN |
-					 SC_OP_BT_PRIORITY_DETECTED);
+			set_bit(BT_OP_PRIORITY_DETECTED, &btcoex->op_flags);
+			set_bit(BT_OP_SCAN, &btcoex->op_flags);
 		} else if (btcoex->bt_priority_cnt >= ATH_BT_CNT_THRESHOLD) {
 			ath_dbg(ath9k_hw_common(sc->sc_ah), BTCOEX,
 				"BT priority traffic detected\n");
-			sc->sc_flags |= SC_OP_BT_PRIORITY_DETECTED;
+			set_bit(BT_OP_PRIORITY_DETECTED, &btcoex->op_flags);
 		}
 
 		btcoex->bt_priority_cnt = 0;
@@ -190,13 +191,34 @@ static void ath_btcoex_period_timer(unsigned long data)
 	struct ath_softc *sc = (struct ath_softc *) data;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_btcoex *btcoex = &sc->btcoex;
+	struct ath_mci_profile *mci = &btcoex->mci;
 	u32 timer_period;
 	bool is_btscan;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sc->sc_pm_lock, flags);
+	if (sc->sc_ah->power_mode == ATH9K_PM_NETWORK_SLEEP) {
+		spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
+		goto skip_hw_wakeup;
+	}
+	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
 	ath9k_ps_wakeup(sc);
 	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI))
 		ath_detect_bt_priority(sc);
-	is_btscan = sc->sc_flags & SC_OP_BT_SCAN;
+	is_btscan = test_bit(BT_OP_SCAN, &btcoex->op_flags);
+
+	btcoex->bt_wait_time += btcoex->btcoex_period;
+	if (btcoex->bt_wait_time > ATH_BTCOEX_RX_WAIT_TIME) {
+		if (ar9003_mci_state(ah, MCI_STATE_NEED_FTP_STOMP) &&
+		    (mci->num_pan || mci->num_other_acl))
+			ah->btcoex_hw.mci.stomp_ftp =
+				(sc->rx.num_pkts < ATH_BTCOEX_STOMP_FTP_THRESH);
+		else
+			ah->btcoex_hw.mci.stomp_ftp = false;
+		btcoex->bt_wait_time = 0;
+		sc->rx.num_pkts = 0;
+	}
 
 	spin_lock_bh(&btcoex->btcoex_lock);
 
@@ -218,9 +240,9 @@ static void ath_btcoex_period_timer(unsigned long data)
 	}
 
 	ath9k_ps_restore(sc);
-	timer_period = btcoex->btcoex_period / 1000;
-	mod_timer(&btcoex->period_timer, jiffies +
-				  msecs_to_jiffies(timer_period));
+skip_hw_wakeup:
+	timer_period = btcoex->btcoex_period;
+	mod_timer(&btcoex->period_timer, jiffies + msecs_to_jiffies(timer_period));
 }
 
 /*
@@ -233,14 +255,14 @@ static void ath_btcoex_no_stomp_timer(void *arg)
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_btcoex *btcoex = &sc->btcoex;
 	struct ath_common *common = ath9k_hw_common(ah);
-	bool is_btscan = sc->sc_flags & SC_OP_BT_SCAN;
 
 	ath_dbg(common, BTCOEX, "no stomp timer running\n");
 
 	ath9k_ps_wakeup(sc);
 	spin_lock_bh(&btcoex->btcoex_lock);
 
-	if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW || is_btscan)
+	if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW ||
+	    test_bit(BT_OP_SCAN, &btcoex->op_flags))
 		ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_NONE);
 	 else if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_ALL)
 		ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_LOW);
@@ -254,10 +276,10 @@ static int ath_init_btcoex_timer(struct ath_softc *sc)
 {
 	struct ath_btcoex *btcoex = &sc->btcoex;
 
-	btcoex->btcoex_period = ATH_BTCOEX_DEF_BT_PERIOD * 1000;
-	btcoex->btcoex_no_stomp = (100 - ATH_BTCOEX_DEF_DUTY_CYCLE) *
+	btcoex->btcoex_period = ATH_BTCOEX_DEF_BT_PERIOD;
+	btcoex->btcoex_no_stomp = (100 - ATH_BTCOEX_DEF_DUTY_CYCLE) * 1000 *
 		btcoex->btcoex_period / 100;
-	btcoex->btscan_no_stomp = (100 - ATH_BTCOEX_BTSCAN_DUTY_CYCLE) *
+	btcoex->btscan_no_stomp = (100 - ATH_BTCOEX_BTSCAN_DUTY_CYCLE) * 1000 *
 				   btcoex->btcoex_period / 100;
 
 	setup_timer(&btcoex->period_timer, ath_btcoex_period_timer,
@@ -292,7 +314,8 @@ void ath9k_btcoex_timer_resume(struct ath_softc *sc)
 
 	btcoex->bt_priority_cnt = 0;
 	btcoex->bt_priority_time = jiffies;
-	sc->sc_flags &= ~(SC_OP_BT_PRIORITY_DETECTED | SC_OP_BT_SCAN);
+	clear_bit(BT_OP_PRIORITY_DETECTED, &btcoex->op_flags);
+	clear_bit(BT_OP_SCAN, &btcoex->op_flags);
 
 	mod_timer(&btcoex->period_timer, jiffies);
 }
@@ -314,14 +337,22 @@ void ath9k_btcoex_timer_pause(struct ath_softc *sc)
 	btcoex->hw_timer_enabled = false;
 }
 
+void ath9k_btcoex_stop_gen_timer(struct ath_softc *sc)
+{
+	struct ath_btcoex *btcoex = &sc->btcoex;
+
+	ath9k_gen_timer_stop(sc->sc_ah, btcoex->no_stomp_timer);
+}
+
 u16 ath9k_btcoex_aggr_limit(struct ath_softc *sc, u32 max_4ms_framelen)
 {
+	struct ath_btcoex *btcoex = &sc->btcoex;
 	struct ath_mci_profile *mci = &sc->btcoex.mci;
 	u16 aggr_limit = 0;
 
 	if ((sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_MCI) && mci->aggr_limit)
 		aggr_limit = (max_4ms_framelen * mci->aggr_limit) >> 4;
-	else if (sc->sc_flags & SC_OP_BT_PRIORITY_DETECTED)
+	else if (test_bit(BT_OP_PRIORITY_DETECTED, &btcoex->op_flags))
 		aggr_limit = min((max_4ms_framelen * 3) / 8,
 				 (u32)ATH_AMPDU_LIMIT_MAX);
 
@@ -362,9 +393,9 @@ void ath9k_stop_btcoex(struct ath_softc *sc)
 
 	if (ah->btcoex_hw.enabled &&
 	    ath9k_hw_get_btcoex_scheme(ah) != ATH_BTCOEX_CFG_NONE) {
-		ath9k_hw_btcoex_disable(ah);
 		if (ath9k_hw_get_btcoex_scheme(ah) == ATH_BTCOEX_CFG_3WIRE)
 			ath9k_btcoex_timer_pause(sc);
+		ath9k_hw_btcoex_disable(ah);
 		if (AR_SREV_9462(ah))
 			ath_mci_flush_profile(&sc->btcoex.mci);
 	}
@@ -372,11 +403,13 @@ void ath9k_stop_btcoex(struct ath_softc *sc)
 
 void ath9k_deinit_btcoex(struct ath_softc *sc)
 {
+	struct ath_hw *ah = sc->sc_ah;
+
         if ((sc->btcoex.no_stomp_timer) &&
 	    ath9k_hw_get_btcoex_scheme(sc->sc_ah) == ATH_BTCOEX_CFG_3WIRE)
 		ath_gen_timer_free(sc->sc_ah, sc->btcoex.no_stomp_timer);
 
-	if (AR_SREV_9462(sc->sc_ah))
+	if (ath9k_hw_mci_is_enabled(ah))
 		ath_mci_cleanup(sc);
 }
 
@@ -402,7 +435,7 @@ int ath9k_init_btcoex(struct ath_softc *sc)
 		txq = sc->tx.txq_map[WME_AC_BE];
 		ath9k_hw_init_btcoex_hw(sc->sc_ah, txq->axq_qnum);
 		sc->btcoex.bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
-		if (AR_SREV_9462(ah)) {
+		if (ath9k_hw_mci_is_enabled(ah)) {
 			sc->btcoex.duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
 			INIT_LIST_HEAD(&sc->btcoex.mci.info);
 
diff --git a/drivers/net/wireless/ath/ath9k/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index 135795257d9..936e920fb88 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -453,7 +453,6 @@ struct ath9k_htc_priv {
 	u8 num_sta_assoc_vif;
 	u8 num_ap_vif;
 
-	u16 op_flags;
 	u16 curtxpow;
 	u16 txpowlimit;
 	u16 nvifs;
@@ -461,6 +460,7 @@ struct ath9k_htc_priv {
 	bool rearm_ani;
 	bool reconfig_beacon;
 	unsigned int rxfilter;
+	unsigned long op_flags;
 
 	struct ath9k_hw_cal_data caldata;
 	struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
@@ -572,8 +572,6 @@ bool ath9k_htc_setpower(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);
-void ath9k_htc_radio_enable(struct ieee80211_hw *hw);
-void ath9k_htc_radio_disable(struct ieee80211_hw *hw);
 
 #ifdef CONFIG_MAC80211_LEDS
 void ath9k_init_leds(struct ath9k_htc_priv *priv);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
index 2eadffb7971..77d541feb91 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
@@ -207,9 +207,9 @@ static void ath9k_htc_beacon_config_ap(struct ath9k_htc_priv *priv,
 	else
 		priv->ah->config.sw_beacon_response_time = MIN_SWBA_RESPONSE;
 
-	if (priv->op_flags & OP_TSF_RESET) {
+	if (test_bit(OP_TSF_RESET, &priv->op_flags)) {
 		ath9k_hw_reset_tsf(priv->ah);
-		priv->op_flags &= ~OP_TSF_RESET;
+		clear_bit(OP_TSF_RESET, &priv->op_flags);
 	} else {
 		/*
 		 * Pull nexttbtt forward to reflect the current TSF.
@@ -221,7 +221,7 @@ static void ath9k_htc_beacon_config_ap(struct ath9k_htc_priv *priv,
 		} while (nexttbtt < tsftu);
 	}
 
-	if (priv->op_flags & OP_ENABLE_BEACON)
+	if (test_bit(OP_ENABLE_BEACON, &priv->op_flags))
 		imask |= ATH9K_INT_SWBA;
 
 	ath_dbg(common, CONFIG,
@@ -269,7 +269,7 @@ static void ath9k_htc_beacon_config_adhoc(struct ath9k_htc_priv *priv,
 	else
 		priv->ah->config.sw_beacon_response_time = MIN_SWBA_RESPONSE;
 
-	if (priv->op_flags & OP_ENABLE_BEACON)
+	if (test_bit(OP_ENABLE_BEACON, &priv->op_flags))
 		imask |= ATH9K_INT_SWBA;
 
 	ath_dbg(common, CONFIG,
@@ -365,7 +365,7 @@ static void ath9k_htc_send_beacon(struct ath9k_htc_priv *priv,
 	vif = priv->cur_beacon_conf.bslot[slot];
 	avp = (struct ath9k_htc_vif *)vif->drv_priv;
 
-	if (unlikely(priv->op_flags & OP_SCANNING)) {
+	if (unlikely(test_bit(OP_SCANNING, &priv->op_flags))) {
 		spin_unlock_bh(&priv->beacon_lock);
 		return;
 	}
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
index 1c10e2e5c23..07df279c8d4 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
@@ -37,17 +37,18 @@ static void ath_detect_bt_priority(struct ath9k_htc_priv *priv)
 
 	if (time_after(jiffies, btcoex->bt_priority_time +
 			msecs_to_jiffies(ATH_BT_PRIORITY_TIME_THRESHOLD))) {
-		priv->op_flags &= ~(OP_BT_PRIORITY_DETECTED | OP_BT_SCAN);
+		clear_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
+		clear_bit(OP_BT_SCAN, &priv->op_flags);
 		/* Detect if colocated bt started scanning */
 		if (btcoex->bt_priority_cnt >= ATH_BT_CNT_SCAN_THRESHOLD) {
 			ath_dbg(ath9k_hw_common(ah), BTCOEX,
 				"BT scan detected\n");
-			priv->op_flags |= (OP_BT_SCAN |
-					 OP_BT_PRIORITY_DETECTED);
+			set_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
+			set_bit(OP_BT_SCAN, &priv->op_flags);
 		} else if (btcoex->bt_priority_cnt >= ATH_BT_CNT_THRESHOLD) {
 			ath_dbg(ath9k_hw_common(ah), BTCOEX,
 				"BT priority traffic detected\n");
-			priv->op_flags |= OP_BT_PRIORITY_DETECTED;
+			set_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
 		}
 
 		btcoex->bt_priority_cnt = 0;
@@ -67,26 +68,23 @@ static void ath_btcoex_period_work(struct work_struct *work)
 	struct ath_btcoex *btcoex = &priv->btcoex;
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	u32 timer_period;
-	bool is_btscan;
 	int ret;
 
 	ath_detect_bt_priority(priv);
 
-	is_btscan = !!(priv->op_flags & OP_BT_SCAN);
-
 	ret = ath9k_htc_update_cap_target(priv,
-				  !!(priv->op_flags & OP_BT_PRIORITY_DETECTED));
+			  test_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags));
 	if (ret) {
 		ath_err(common, "Unable to set BTCOEX parameters\n");
 		return;
 	}
 
-	ath9k_hw_btcoex_bt_stomp(priv->ah, is_btscan ? ATH_BTCOEX_STOMP_ALL :
-			btcoex->bt_stomp_type);
+	ath9k_hw_btcoex_bt_stomp(priv->ah, test_bit(OP_BT_SCAN, &priv->op_flags) ?
+				 ATH_BTCOEX_STOMP_ALL : btcoex->bt_stomp_type);
 
 	ath9k_hw_btcoex_enable(priv->ah);
-	timer_period = is_btscan ? btcoex->btscan_no_stomp :
-		btcoex->btcoex_no_stomp;
+	timer_period = test_bit(OP_BT_SCAN, &priv->op_flags) ?
+		btcoex->btscan_no_stomp : btcoex->btcoex_no_stomp;
 	ieee80211_queue_delayed_work(priv->hw, &priv->duty_cycle_work,
 				     msecs_to_jiffies(timer_period));
 	ieee80211_queue_delayed_work(priv->hw, &priv->coex_period_work,
@@ -104,14 +102,15 @@ static void ath_btcoex_duty_cycle_work(struct work_struct *work)
 	struct ath_hw *ah = priv->ah;
 	struct ath_btcoex *btcoex = &priv->btcoex;
 	struct ath_common *common = ath9k_hw_common(ah);
-	bool is_btscan = priv->op_flags & OP_BT_SCAN;
 
 	ath_dbg(common, BTCOEX, "time slice work for bt and wlan\n");
 
-	if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW || is_btscan)
+	if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW ||
+	    test_bit(OP_BT_SCAN, &priv->op_flags))
 		ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_NONE);
 	else if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_ALL)
 		ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_LOW);
+
 	ath9k_hw_btcoex_enable(priv->ah);
 }
 
@@ -141,7 +140,8 @@ static void ath_htc_resume_btcoex_work(struct ath9k_htc_priv *priv)
 
 	btcoex->bt_priority_cnt = 0;
 	btcoex->bt_priority_time = jiffies;
-	priv->op_flags &= ~(OP_BT_PRIORITY_DETECTED | OP_BT_SCAN);
+	clear_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
+	clear_bit(OP_BT_SCAN, &priv->op_flags);
 	ieee80211_queue_delayed_work(priv->hw, &priv->coex_period_work, 0);
 }
 
@@ -310,95 +310,3 @@ void ath9k_start_rfkill_poll(struct ath9k_htc_priv *priv)
 	if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_RFSILENT)
 		wiphy_rfkill_start_polling(priv->hw->wiphy);
 }
-
-void ath9k_htc_radio_enable(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);
-	int ret;
-	u8 cmd_rsp;
-
-	if (!ah->curchan)
-		ah->curchan = ath9k_cmn_get_curchannel(hw, ah);
-
-	/* Reset the HW */
-	ret = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
-	if (ret) {
-		ath_err(common,
-			"Unable to reset hardware; reset status %d (freq %u MHz)\n",
-			ret, ah->curchan->channel);
-	}
-
-	ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
-			       &priv->curtxpow);
-
-	/* Start RX */
-	WMI_CMD(WMI_START_RECV_CMDID);
-	ath9k_host_rx_init(priv);
-
-	/* Start TX */
-	htc_start(priv->htc);
-	spin_lock_bh(&priv->tx.tx_lock);
-	priv->tx.flags &= ~ATH9K_HTC_OP_TX_QUEUES_STOP;
-	spin_unlock_bh(&priv->tx.tx_lock);
-	ieee80211_wake_queues(hw);
-
-	WMI_CMD(WMI_ENABLE_INTR_CMDID);
-
-	/* Enable LED */
-	ath9k_hw_cfg_output(ah, ah->led_pin,
-			    AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
-	ath9k_hw_set_gpio(ah, ah->led_pin, 0);
-}
-
-void ath9k_htc_radio_disable(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);
-	int ret;
-	u8 cmd_rsp;
-
-	ath9k_htc_ps_wakeup(priv);
-
-	/* Disable LED */
-	ath9k_hw_set_gpio(ah, ah->led_pin, 1);
-	ath9k_hw_cfg_gpio_input(ah, ah->led_pin);
-
-	WMI_CMD(WMI_DISABLE_INTR_CMDID);
-
-	/* Stop TX */
-	ieee80211_stop_queues(hw);
-	ath9k_htc_tx_drain(priv);
-	WMI_CMD(WMI_DRAIN_TXQ_ALL_CMDID);
-
-	/* Stop RX */
-	WMI_CMD(WMI_STOP_RECV_CMDID);
-
-	/* Clear the WMI event queue */
-	ath9k_wmi_event_drain(priv);
-
-	/*
-	 * The MIB counters have to be disabled here,
-	 * since the target doesn't do it.
-	 */
-	ath9k_hw_disable_mib_counters(ah);
-
-	if (!ah->curchan)
-		ah->curchan = ath9k_cmn_get_curchannel(hw, ah);
-
-	/* Reset the HW */
-	ret = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
-	if (ret) {
-		ath_err(common,
-			"Unable to reset hardware; reset status %d (freq %u MHz)\n",
-			ret, ah->curchan->channel);
-	}
-
-	/* Disable the PHY */
-	ath9k_hw_phy_disable(ah);
-
-	ath9k_htc_ps_restore(priv);
-	ath9k_htc_setpower(priv, ATH9K_PM_FULL_SLEEP);
-}
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index 25213d521bc..a035a380d66 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -611,7 +611,7 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
 	struct ath_common *common;
 	int i, ret = 0, csz = 0;
 
-	priv->op_flags |= OP_INVALID;
+	set_bit(OP_INVALID, &priv->op_flags);
 
 	ah = kzalloc(sizeof(struct ath_hw), GFP_KERNEL);
 	if (!ah)
@@ -718,7 +718,7 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 
 	hw->queues = 4;
 	hw->channel_change_time = 5000;
-	hw->max_listen_interval = 10;
+	hw->max_listen_interval = 1;
 
 	hw->vif_data_size = sizeof(struct ath9k_htc_vif);
 	hw->sta_data_size = sizeof(struct ath9k_htc_sta);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index abbd6effd60..c785129692f 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -75,14 +75,19 @@ unlock:
 
 void ath9k_htc_ps_restore(struct ath9k_htc_priv *priv)
 {
+	bool reset;
+
 	mutex_lock(&priv->htc_pm_lock);
 	if (--priv->ps_usecount != 0)
 		goto unlock;
 
-	if (priv->ps_idle)
+	if (priv->ps_idle) {
+		ath9k_hw_setrxabort(priv->ah, true);
+		ath9k_hw_stopdmarecv(priv->ah, &reset);
 		ath9k_hw_setpower(priv->ah, ATH9K_PM_FULL_SLEEP);
-	else if (priv->ps_enabled)
+	} else if (priv->ps_enabled) {
 		ath9k_hw_setpower(priv->ah, ATH9K_PM_NETWORK_SLEEP);
+	}
 
 unlock:
 	mutex_unlock(&priv->htc_pm_lock);
@@ -250,7 +255,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 	u8 cmd_rsp;
 	int ret;
 
-	if (priv->op_flags & OP_INVALID)
+	if (test_bit(OP_INVALID, &priv->op_flags))
 		return -EIO;
 
 	fastcc = !!(hw->conf.flags & IEEE80211_CONF_OFFCHANNEL);
@@ -304,7 +309,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 
 	htc_start(priv->htc);
 
-	if (!(priv->op_flags & OP_SCANNING) &&
+	if (!test_bit(OP_SCANNING, &priv->op_flags) &&
 	    !(hw->conf.flags & IEEE80211_CONF_OFFCHANNEL))
 		ath9k_htc_vif_reconfig(priv);
 
@@ -750,7 +755,7 @@ void ath9k_htc_start_ani(struct ath9k_htc_priv *priv)
 	common->ani.shortcal_timer = timestamp;
 	common->ani.checkani_timer = timestamp;
 
-	priv->op_flags |= OP_ANI_RUNNING;
+	set_bit(OP_ANI_RUNNING, &priv->op_flags);
 
 	ieee80211_queue_delayed_work(common->hw, &priv->ani_work,
 				     msecs_to_jiffies(ATH_ANI_POLLINTERVAL));
@@ -759,7 +764,7 @@ void ath9k_htc_start_ani(struct ath9k_htc_priv *priv)
 void ath9k_htc_stop_ani(struct ath9k_htc_priv *priv)
 {
 	cancel_delayed_work_sync(&priv->ani_work);
-	priv->op_flags &= ~OP_ANI_RUNNING;
+	clear_bit(OP_ANI_RUNNING, &priv->op_flags);
 }
 
 void ath9k_htc_ani_work(struct work_struct *work)
@@ -944,7 +949,7 @@ static int ath9k_htc_start(struct ieee80211_hw *hw)
 		ath_dbg(common, CONFIG,
 			"Failed to update capability in target\n");
 
-	priv->op_flags &= ~OP_INVALID;
+	clear_bit(OP_INVALID, &priv->op_flags);
 	htc_start(priv->htc);
 
 	spin_lock_bh(&priv->tx.tx_lock);
@@ -973,7 +978,7 @@ static void ath9k_htc_stop(struct ieee80211_hw *hw)
 
 	mutex_lock(&priv->mutex);
 
-	if (priv->op_flags & OP_INVALID) {
+	if (test_bit(OP_INVALID, &priv->op_flags)) {
 		ath_dbg(common, ANY, "Device not present\n");
 		mutex_unlock(&priv->mutex);
 		return;
@@ -1015,7 +1020,7 @@ static void ath9k_htc_stop(struct ieee80211_hw *hw)
 	ath9k_htc_ps_restore(priv);
 	ath9k_htc_setpower(priv, ATH9K_PM_FULL_SLEEP);
 
-	priv->op_flags |= OP_INVALID;
+	set_bit(OP_INVALID, &priv->op_flags);
 
 	ath_dbg(common, CONFIG, "Driver halt\n");
 	mutex_unlock(&priv->mutex);
@@ -1105,8 +1110,8 @@ static int ath9k_htc_add_interface(struct ieee80211_hw *hw,
 	ath9k_htc_set_opmode(priv);
 
 	if ((priv->ah->opmode == NL80211_IFTYPE_AP) &&
-	    !(priv->op_flags & OP_ANI_RUNNING)) {
-		ath9k_hw_set_tsfadjust(priv->ah, 1);
+	    !test_bit(OP_ANI_RUNNING, &priv->op_flags)) {
+		ath9k_hw_set_tsfadjust(priv->ah, true);
 		ath9k_htc_start_ani(priv);
 	}
 
@@ -1178,24 +1183,20 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 	struct ath9k_htc_priv *priv = hw->priv;
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct ieee80211_conf *conf = &hw->conf;
+	bool chip_reset = false;
+	int ret = 0;
 
 	mutex_lock(&priv->mutex);
+	ath9k_htc_ps_wakeup(priv);
 
 	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-		bool enable_radio = false;
-		bool idle = !!(conf->flags & IEEE80211_CONF_IDLE);
-
 		mutex_lock(&priv->htc_pm_lock);
-		if (!idle && priv->ps_idle)
-			enable_radio = true;
-		priv->ps_idle = idle;
-		mutex_unlock(&priv->htc_pm_lock);
 
-		if (enable_radio) {
-			ath_dbg(common, CONFIG, "not-idle: enabling radio\n");
-			ath9k_htc_setpower(priv, ATH9K_PM_AWAKE);
-			ath9k_htc_radio_enable(hw);
-		}
+		priv->ps_idle = !!(conf->flags & IEEE80211_CONF_IDLE);
+		if (priv->ps_idle)
+			chip_reset = true;
+
+		mutex_unlock(&priv->htc_pm_lock);
 	}
 
 	/*
@@ -1210,7 +1211,7 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 			ath9k_htc_remove_monitor_interface(priv);
 	}
 
-	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) || chip_reset) {
 		struct ieee80211_channel *curchan = hw->conf.channel;
 		int pos = curchan->hw_value;
 
@@ -1223,8 +1224,8 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 
 		if (ath9k_htc_set_channel(priv, hw, &priv->ah->channels[pos]) < 0) {
 			ath_err(common, "Unable to set channel\n");
-			mutex_unlock(&priv->mutex);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto out;
 		}
 
 	}
@@ -1246,21 +1247,10 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 				       priv->txpowlimit, &priv->curtxpow);
 	}
 
-	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-		mutex_lock(&priv->htc_pm_lock);
-		if (!priv->ps_idle) {
-			mutex_unlock(&priv->htc_pm_lock);
-			goto out;
-		}
-		mutex_unlock(&priv->htc_pm_lock);
-
-		ath_dbg(common, CONFIG, "idle: disabling radio\n");
-		ath9k_htc_radio_disable(hw);
-	}
-
 out:
+	ath9k_htc_ps_restore(priv);
 	mutex_unlock(&priv->mutex);
-	return 0;
+	return ret;
 }
 
 #define SUPPORTED_FILTERS			\
@@ -1285,7 +1275,7 @@ static void ath9k_htc_configure_filter(struct ieee80211_hw *hw,
 	changed_flags &= SUPPORTED_FILTERS;
 	*total_flags &= SUPPORTED_FILTERS;
 
-	if (priv->op_flags & OP_INVALID) {
+	if (test_bit(OP_INVALID, &priv->op_flags)) {
 		ath_dbg(ath9k_hw_common(priv->ah), ANY,
 			"Unable to configure filter on invalid state\n");
 		mutex_unlock(&priv->mutex);
@@ -1361,7 +1351,7 @@ static int ath9k_htc_conf_tx(struct ieee80211_hw *hw,
 	qi.tqi_aifs = params->aifs;
 	qi.tqi_cwmin = params->cw_min;
 	qi.tqi_cwmax = params->cw_max;
-	qi.tqi_burstTime = params->txop;
+	qi.tqi_burstTime = params->txop * 32;
 
 	qnum = get_hw_qnum(queue, priv->hwq_map);
 
@@ -1516,7 +1506,7 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
 		ath_dbg(common, CONFIG, "Beacon enabled for BSS: %pM\n",
 			bss_conf->bssid);
 		ath9k_htc_set_tsfadjust(priv, vif);
-		priv->op_flags |= OP_ENABLE_BEACON;
+		set_bit(OP_ENABLE_BEACON, &priv->op_flags);
 		ath9k_htc_beacon_config(priv, vif);
 	}
 
@@ -1529,7 +1519,7 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
 			ath_dbg(common, CONFIG,
 				"Beacon disabled for BSS: %pM\n",
 				bss_conf->bssid);
-			priv->op_flags &= ~OP_ENABLE_BEACON;
+			clear_bit(OP_ENABLE_BEACON, &priv->op_flags);
 			ath9k_htc_beacon_config(priv, vif);
 		}
 	}
@@ -1542,7 +1532,7 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
 		    (priv->nvifs == 1) &&
 		    (priv->num_ap_vif == 1) &&
 		    (vif->type == NL80211_IFTYPE_AP)) {
-			priv->op_flags |= OP_TSF_RESET;
+			set_bit(OP_TSF_RESET, &priv->op_flags);
 		}
 		ath_dbg(common, CONFIG,
 			"Beacon interval changed for BSS: %pM\n",
@@ -1654,7 +1644,7 @@ static void ath9k_htc_sw_scan_start(struct ieee80211_hw *hw)
 
 	mutex_lock(&priv->mutex);
 	spin_lock_bh(&priv->beacon_lock);
-	priv->op_flags |= OP_SCANNING;
+	set_bit(OP_SCANNING, &priv->op_flags);
 	spin_unlock_bh(&priv->beacon_lock);
 	cancel_work_sync(&priv->ps_work);
 	ath9k_htc_stop_ani(priv);
@@ -1667,7 +1657,7 @@ static void ath9k_htc_sw_scan_complete(struct ieee80211_hw *hw)
 
 	mutex_lock(&priv->mutex);
 	spin_lock_bh(&priv->beacon_lock);
-	priv->op_flags &= ~OP_SCANNING;
+	clear_bit(OP_SCANNING, &priv->op_flags);
 	spin_unlock_bh(&priv->beacon_lock);
 	ath9k_htc_ps_wakeup(priv);
 	ath9k_htc_vif_reconfig(priv);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index 3e40a646151..47e61d0da33 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -916,7 +916,7 @@ void ath9k_host_rx_init(struct ath9k_htc_priv *priv)
 {
 	ath9k_hw_rxena(priv->ah);
 	ath9k_htc_opmode_init(priv);
-	ath9k_hw_startpcureceive(priv->ah, (priv->op_flags & OP_SCANNING));
+	ath9k_hw_startpcureceive(priv->ah, test_bit(OP_SCANNING, &priv->op_flags));
 	priv->rx.last_rssi = ATH_RSSI_DUMMY_MARKER;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 995ca8e1302..cfa91ab7acf 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -342,6 +342,9 @@ static void ath9k_hw_read_revisions(struct ath_hw *ah)
 		val = REG_READ(ah, AR_SREV);
 		ah->hw_version.macRev = MS(val, AR_SREV_REVISION2);
 		return;
+	case AR9300_DEVID_QCA955X:
+		ah->hw_version.macVersion = AR_SREV_VERSION_9550;
+		return;
 	}
 
 	val = REG_READ(ah, AR_SREV) & AR_SREV_ID;
@@ -390,14 +393,6 @@ static void ath9k_hw_disablepcie(struct ath_hw *ah)
 	REG_WRITE(ah, AR_PCIE_SERDES2, 0x00000000);
 }
 
-static void ath9k_hw_aspm_init(struct ath_hw *ah)
-{
-	struct ath_common *common = ath9k_hw_common(ah);
-
-	if (common->bus_ops->aspm_init)
-		common->bus_ops->aspm_init(common);
-}
-
 /* This should work for all families including legacy */
 static bool ath9k_hw_chip_test(struct ath_hw *ah)
 {
@@ -654,6 +649,7 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 	case AR_SREV_VERSION_9485:
 	case AR_SREV_VERSION_9340:
 	case AR_SREV_VERSION_9462:
+	case AR_SREV_VERSION_9550:
 		break;
 	default:
 		ath_err(common,
@@ -663,7 +659,7 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 	}
 
 	if (AR_SREV_9271(ah) || AR_SREV_9100(ah) || AR_SREV_9340(ah) ||
-	    AR_SREV_9330(ah))
+	    AR_SREV_9330(ah) || AR_SREV_9550(ah))
 		ah->is_pciexpress = false;
 
 	ah->hw_version.phyRev = REG_READ(ah, AR_PHY_CHIP_ID);
@@ -675,10 +671,6 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 	if (!AR_SREV_9300_20_OR_LATER(ah))
 		ah->ani_function &= ~ATH9K_ANI_MRC_CCK;
 
-	/* disable ANI for 9340 */
-	if (AR_SREV_9340(ah))
-		ah->config.enable_ani = false;
-
 	ath9k_hw_init_mode_regs(ah);
 
 	if (!ah->is_pciexpress)
@@ -693,9 +685,6 @@ static int __ath9k_hw_init(struct ath_hw *ah)
 	if (r)
 		return r;
 
-	if (ah->is_pciexpress)
-		ath9k_hw_aspm_init(ah);
-
 	r = ath9k_hw_init_macaddr(ah);
 	if (r) {
 		ath_err(common, "Failed to initialize MAC address\n");
@@ -738,6 +727,7 @@ int ath9k_hw_init(struct ath_hw *ah)
 	case AR9300_DEVID_AR9485_PCIE:
 	case AR9300_DEVID_AR9330:
 	case AR9300_DEVID_AR9340:
+	case AR9300_DEVID_QCA955X:
 	case AR9300_DEVID_AR9580:
 	case AR9300_DEVID_AR9462:
 		break;
@@ -876,7 +866,7 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 		/* program BB PLL phase_shift */
 		REG_RMW_FIELD(ah, AR_CH0_BB_DPLL3,
 			      AR_CH0_BB_DPLL3_PHASE_SHIFT, 0x1);
-	} else if (AR_SREV_9340(ah)) {
+	} else if (AR_SREV_9340(ah) || AR_SREV_9550(ah)) {
 		u32 regval, pll2_divint, pll2_divfrac, refdiv;
 
 		REG_WRITE(ah, AR_RTC_PLL_CONTROL, 0x1142c);
@@ -890,9 +880,15 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 			pll2_divfrac = 0x1eb85;
 			refdiv = 3;
 		} else {
-			pll2_divint = 88;
-			pll2_divfrac = 0;
-			refdiv = 5;
+			if (AR_SREV_9340(ah)) {
+				pll2_divint = 88;
+				pll2_divfrac = 0;
+				refdiv = 5;
+			} else {
+				pll2_divint = 0x11;
+				pll2_divfrac = 0x26666;
+				refdiv = 1;
+			}
 		}
 
 		regval = REG_READ(ah, AR_PHY_PLL_MODE);
@@ -905,8 +901,12 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 		udelay(100);
 
 		regval = REG_READ(ah, AR_PHY_PLL_MODE);
-		regval = (regval & 0x80071fff) | (0x1 << 30) | (0x1 << 13) |
-			 (0x4 << 26) | (0x18 << 19);
+		if (AR_SREV_9340(ah))
+			regval = (regval & 0x80071fff) | (0x1 << 30) |
+				 (0x1 << 13) | (0x4 << 26) | (0x18 << 19);
+		else
+			regval = (regval & 0x80071fff) | (0x3 << 30) |
+				 (0x1 << 13) | (0x4 << 26) | (0x60 << 19);
 		REG_WRITE(ah, AR_PHY_PLL_MODE, regval);
 		REG_WRITE(ah, AR_PHY_PLL_MODE,
 			  REG_READ(ah, AR_PHY_PLL_MODE) & 0xfffeffff);
@@ -917,7 +917,8 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 
 	REG_WRITE(ah, AR_RTC_PLL_CONTROL, pll);
 
-	if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah))
+	if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah) ||
+	    AR_SREV_9550(ah))
 		udelay(1000);
 
 	/* Switch the core clock for ar9271 to 117Mhz */
@@ -930,7 +931,7 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 
 	REG_WRITE(ah, AR_RTC_SLEEP_CLK, AR_RTC_FORCE_DERIVED_CLK);
 
-	if (AR_SREV_9340(ah)) {
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah)) {
 		if (ah->is_clk_25mhz) {
 			REG_WRITE(ah, AR_RTC_DERIVED_CLK, 0x17c << 1);
 			REG_WRITE(ah, AR_SLP32_MODE, 0x0010f3d7);
@@ -954,7 +955,7 @@ static void ath9k_hw_init_interrupt_masks(struct ath_hw *ah,
 		AR_IMR_RXORN |
 		AR_IMR_BCNMISC;
 
-	if (AR_SREV_9340(ah))
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah))
 		sync_default &= ~AR_INTR_SYNC_HOST1_FATAL;
 
 	if (AR_SREV_9300_20_OR_LATER(ah)) {
@@ -1371,6 +1372,9 @@ static bool ath9k_hw_set_reset(struct ath_hw *ah, int type)
 		}
 	}
 
+	if (ath9k_hw_mci_is_enabled(ah))
+		ar9003_mci_check_gpm_offset(ah);
+
 	REG_WRITE(ah, AR_RTC_RC, rst_flags);
 
 	REGWRITE_BUFFER_FLUSH(ah);
@@ -1455,9 +1459,6 @@ static bool ath9k_hw_set_reset_reg(struct ath_hw *ah, u32 type)
 		break;
 	}
 
-	if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
-		REG_WRITE(ah, AR_RTC_KEEP_AWAKE, 0x2);
-
 	return ret;
 }
 
@@ -1733,8 +1734,8 @@ static int ath9k_hw_do_fastcc(struct ath_hw *ah, struct ath9k_channel *chan)
 	ath9k_hw_loadnf(ah, ah->curchan);
 	ath9k_hw_start_nfcal(ah, true);
 
-	if ((ah->caps.hw_caps & ATH9K_HW_CAP_MCI) && ar9003_mci_is_ready(ah))
-		ar9003_mci_2g5g_switch(ah, true);
+	if (ath9k_hw_mci_is_enabled(ah))
+		ar9003_mci_2g5g_switch(ah, false);
 
 	if (AR_SREV_9271(ah))
 		ar9002_hw_load_ani_reg(ah, chan);
@@ -1754,10 +1755,9 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	u64 tsf = 0;
 	int i, r;
 	bool start_mci_reset = false;
-	bool mci = !!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI);
 	bool save_fullsleep = ah->chip_fullsleep;
 
-	if (mci) {
+	if (ath9k_hw_mci_is_enabled(ah)) {
 		start_mci_reset = ar9003_mci_start_reset(ah, chan);
 		if (start_mci_reset)
 			return 0;
@@ -1786,7 +1786,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 			return r;
 	}
 
-	if (mci)
+	if (ath9k_hw_mci_is_enabled(ah))
 		ar9003_mci_stop_bt(ah, save_fullsleep);
 
 	saveDefAntenna = REG_READ(ah, AR_DEF_ANTENNA);
@@ -1844,7 +1844,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (r)
 		return r;
 
-	if (mci)
+	if (ath9k_hw_mci_is_enabled(ah))
 		ar9003_mci_reset(ah, false, IS_CHAN_2GHZ(chan), save_fullsleep);
 
 	/*
@@ -1939,7 +1939,8 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
 	ath9k_hw_set_dma(ah);
 
-	REG_WRITE(ah, AR_OBS, 8);
+	if (!ath9k_hw_mci_is_enabled(ah))
+		REG_WRITE(ah, AR_OBS, 8);
 
 	if (ah->config.rx_intr_mitigation) {
 		REG_RMW_FIELD(ah, AR_RIMT, AR_RIMT_LAST, 500);
@@ -1960,10 +1961,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (!ath9k_hw_init_cal(ah, chan))
 		return -EIO;
 
-	ath9k_hw_loadnf(ah, chan);
-	ath9k_hw_start_nfcal(ah, true);
-
-	if (mci && ar9003_mci_end_reset(ah, chan, caldata))
+	if (ath9k_hw_mci_is_enabled(ah) && ar9003_mci_end_reset(ah, chan, caldata))
 		return -EIO;
 
 	ENABLE_REGWRITE_BUFFER(ah);
@@ -1998,7 +1996,8 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 				REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
 		}
 #ifdef __BIG_ENDIAN
-		else if (AR_SREV_9330(ah) || AR_SREV_9340(ah))
+		else if (AR_SREV_9330(ah) || AR_SREV_9340(ah) ||
+			 AR_SREV_9550(ah))
 			REG_RMW(ah, AR_CFG, AR_CFG_SWRB | AR_CFG_SWTB, 0);
 		else
 			REG_WRITE(ah, AR_CFG, AR_CFG_SWTD | AR_CFG_SWRD);
@@ -2008,9 +2007,12 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (ath9k_hw_btcoex_is_enabled(ah))
 		ath9k_hw_btcoex_enable(ah);
 
-	if (mci)
+	if (ath9k_hw_mci_is_enabled(ah))
 		ar9003_mci_check_bt(ah);
 
+	ath9k_hw_loadnf(ah, chan);
+	ath9k_hw_start_nfcal(ah, true);
+
 	if (AR_SREV_9300_20_OR_LATER(ah)) {
 		ar9003_hw_bb_watchdog_config(ah);
 
@@ -2031,39 +2033,35 @@ EXPORT_SYMBOL(ath9k_hw_reset);
  * Notify Power Mgt is disabled in self-generated frames.
  * If requested, force chip to sleep.
  */
-static void ath9k_set_power_sleep(struct ath_hw *ah, int setChip)
+static void ath9k_set_power_sleep(struct ath_hw *ah)
 {
 	REG_SET_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
-	if (setChip) {
-		if (AR_SREV_9462(ah)) {
-			REG_WRITE(ah, AR_TIMER_MODE,
-				  REG_READ(ah, AR_TIMER_MODE) & 0xFFFFFF00);
-			REG_WRITE(ah, AR_NDP2_TIMER_MODE, REG_READ(ah,
-				  AR_NDP2_TIMER_MODE) & 0xFFFFFF00);
-			REG_WRITE(ah, AR_SLP32_INC,
-				  REG_READ(ah, AR_SLP32_INC) & 0xFFF00000);
-			/* xxx Required for WLAN only case ? */
-			REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_EN, 0);
-			udelay(100);
-		}
 
-		/*
-		 * Clear the RTC force wake bit to allow the
-		 * mac to go to sleep.
-		 */
-		REG_CLR_BIT(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_EN);
+	if (AR_SREV_9462(ah)) {
+		REG_CLR_BIT(ah, AR_TIMER_MODE, 0xff);
+		REG_CLR_BIT(ah, AR_NDP2_TIMER_MODE, 0xff);
+		REG_CLR_BIT(ah, AR_SLP32_INC, 0xfffff);
+		/* xxx Required for WLAN only case ? */
+		REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_EN, 0);
+		udelay(100);
+	}
 
-		if (AR_SREV_9462(ah))
-			udelay(100);
+	/*
+	 * Clear the RTC force wake bit to allow the
+	 * mac to go to sleep.
+	 */
+	REG_CLR_BIT(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_EN);
 
-		if (!AR_SREV_9100(ah) && !AR_SREV_9300_20_OR_LATER(ah))
-			REG_WRITE(ah, AR_RC, AR_RC_AHB | AR_RC_HOSTIF);
+	if (ath9k_hw_mci_is_enabled(ah))
+		udelay(100);
 
-		/* Shutdown chip. Active low */
-		if (!AR_SREV_5416(ah) && !AR_SREV_9271(ah)) {
-			REG_CLR_BIT(ah, AR_RTC_RESET, AR_RTC_RESET_EN);
-			udelay(2);
-		}
+	if (!AR_SREV_9100(ah) && !AR_SREV_9300_20_OR_LATER(ah))
+		REG_WRITE(ah, AR_RC, AR_RC_AHB | AR_RC_HOSTIF);
+
+	/* Shutdown chip. Active low */
+	if (!AR_SREV_5416(ah) && !AR_SREV_9271(ah)) {
+		REG_CLR_BIT(ah, AR_RTC_RESET, AR_RTC_RESET_EN);
+		udelay(2);
 	}
 
 	/* Clear Bit 14 of AR_WA after putting chip into Full Sleep mode. */
@@ -2076,44 +2074,38 @@ static void ath9k_set_power_sleep(struct ath_hw *ah, int setChip)
  * frames. If request, set power mode of chip to
  * auto/normal.  Duration in units of 128us (1/8 TU).
  */
-static void ath9k_set_power_network_sleep(struct ath_hw *ah, int setChip)
+static void ath9k_set_power_network_sleep(struct ath_hw *ah)
 {
-	u32 val;
+	struct ath9k_hw_capabilities *pCap = &ah->caps;
 
 	REG_SET_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
-	if (setChip) {
-		struct ath9k_hw_capabilities *pCap = &ah->caps;
 
-		if (!(pCap->hw_caps & ATH9K_HW_CAP_AUTOSLEEP)) {
-			/* Set WakeOnInterrupt bit; clear ForceWake bit */
-			REG_WRITE(ah, AR_RTC_FORCE_WAKE,
-				  AR_RTC_FORCE_WAKE_ON_INT);
-		} else {
+	if (!(pCap->hw_caps & ATH9K_HW_CAP_AUTOSLEEP)) {
+		/* Set WakeOnInterrupt bit; clear ForceWake bit */
+		REG_WRITE(ah, AR_RTC_FORCE_WAKE,
+			  AR_RTC_FORCE_WAKE_ON_INT);
+	} else {
 
-			/* When chip goes into network sleep, it could be waken
-			 * up by MCI_INT interrupt caused by BT's HW messages
-			 * (LNA_xxx, CONT_xxx) which chould be in a very fast
-			 * rate (~100us). This will cause chip to leave and
-			 * re-enter network sleep mode frequently, which in
-			 * consequence will have WLAN MCI HW to generate lots of
-			 * SYS_WAKING and SYS_SLEEPING messages which will make
-			 * BT CPU to busy to process.
-			 */
-			if (AR_SREV_9462(ah)) {
-				val = REG_READ(ah, AR_MCI_INTERRUPT_RX_MSG_EN) &
-					~AR_MCI_INTERRUPT_RX_HW_MSG_MASK;
-				REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_EN, val);
-			}
-			/*
-			 * Clear the RTC force wake bit to allow the
-			 * mac to go to sleep.
-			 */
-			REG_CLR_BIT(ah, AR_RTC_FORCE_WAKE,
-				    AR_RTC_FORCE_WAKE_EN);
-
-			if (AR_SREV_9462(ah))
-				udelay(30);
-		}
+		/* When chip goes into network sleep, it could be waken
+		 * up by MCI_INT interrupt caused by BT's HW messages
+		 * (LNA_xxx, CONT_xxx) which chould be in a very fast
+		 * rate (~100us). This will cause chip to leave and
+		 * re-enter network sleep mode frequently, which in
+		 * consequence will have WLAN MCI HW to generate lots of
+		 * SYS_WAKING and SYS_SLEEPING messages which will make
+		 * BT CPU to busy to process.
+		 */
+		if (ath9k_hw_mci_is_enabled(ah))
+			REG_CLR_BIT(ah, AR_MCI_INTERRUPT_RX_MSG_EN,
+				    AR_MCI_INTERRUPT_RX_HW_MSG_MASK);
+		/*
+		 * Clear the RTC force wake bit to allow the
+		 * mac to go to sleep.
+		 */
+		REG_CLR_BIT(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_EN);
+
+		if (ath9k_hw_mci_is_enabled(ah))
+			udelay(30);
 	}
 
 	/* Clear Bit 14 of AR_WA after putting chip into Net Sleep mode. */
@@ -2121,7 +2113,7 @@ static void ath9k_set_power_network_sleep(struct ath_hw *ah, int setChip)
 		REG_WRITE(ah, AR_WA, ah->WARegVal & ~AR_WA_D3_L1_DISABLE);
 }
 
-static bool ath9k_hw_set_power_awake(struct ath_hw *ah, int setChip)
+static bool ath9k_hw_set_power_awake(struct ath_hw *ah)
 {
 	u32 val;
 	int i;
@@ -2132,37 +2124,38 @@ static bool ath9k_hw_set_power_awake(struct ath_hw *ah, int setChip)
 		udelay(10);
 	}
 
-	if (setChip) {
-		if ((REG_READ(ah, AR_RTC_STATUS) &
-		     AR_RTC_STATUS_M) == AR_RTC_STATUS_SHUTDOWN) {
-			if (!ath9k_hw_set_reset_reg(ah, ATH9K_RESET_POWER_ON)) {
-				return false;
-			}
-			if (!AR_SREV_9300_20_OR_LATER(ah))
-				ath9k_hw_init_pll(ah, NULL);
+	if ((REG_READ(ah, AR_RTC_STATUS) &
+	     AR_RTC_STATUS_M) == AR_RTC_STATUS_SHUTDOWN) {
+		if (!ath9k_hw_set_reset_reg(ah, ATH9K_RESET_POWER_ON)) {
+			return false;
 		}
-		if (AR_SREV_9100(ah))
-			REG_SET_BIT(ah, AR_RTC_RESET,
-				    AR_RTC_RESET_EN);
+		if (!AR_SREV_9300_20_OR_LATER(ah))
+			ath9k_hw_init_pll(ah, NULL);
+	}
+	if (AR_SREV_9100(ah))
+		REG_SET_BIT(ah, AR_RTC_RESET,
+			    AR_RTC_RESET_EN);
 
+	REG_SET_BIT(ah, AR_RTC_FORCE_WAKE,
+		    AR_RTC_FORCE_WAKE_EN);
+	udelay(50);
+
+	if (ath9k_hw_mci_is_enabled(ah))
+		ar9003_mci_set_power_awake(ah);
+
+	for (i = POWER_UP_TIME / 50; i > 0; i--) {
+		val = REG_READ(ah, AR_RTC_STATUS) & AR_RTC_STATUS_M;
+		if (val == AR_RTC_STATUS_ON)
+			break;
+		udelay(50);
 		REG_SET_BIT(ah, AR_RTC_FORCE_WAKE,
 			    AR_RTC_FORCE_WAKE_EN);
-		udelay(50);
-
-		for (i = POWER_UP_TIME / 50; i > 0; i--) {
-			val = REG_READ(ah, AR_RTC_STATUS) & AR_RTC_STATUS_M;
-			if (val == AR_RTC_STATUS_ON)
-				break;
-			udelay(50);
-			REG_SET_BIT(ah, AR_RTC_FORCE_WAKE,
-				    AR_RTC_FORCE_WAKE_EN);
-		}
-		if (i == 0) {
-			ath_err(ath9k_hw_common(ah),
-				"Failed to wakeup in %uus\n",
-				POWER_UP_TIME / 20);
-			return false;
-		}
+	}
+	if (i == 0) {
+		ath_err(ath9k_hw_common(ah),
+			"Failed to wakeup in %uus\n",
+			POWER_UP_TIME / 20);
+		return false;
 	}
 
 	REG_CLR_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
@@ -2173,7 +2166,7 @@ static bool ath9k_hw_set_power_awake(struct ath_hw *ah, int setChip)
 bool ath9k_hw_setpower(struct ath_hw *ah, enum ath9k_power_mode mode)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
-	int status = true, setChip = true;
+	int status = true;
 	static const char *modes[] = {
 		"AWAKE",
 		"FULL-SLEEP",
@@ -2189,25 +2182,17 @@ bool ath9k_hw_setpower(struct ath_hw *ah, enum ath9k_power_mode mode)
 
 	switch (mode) {
 	case ATH9K_PM_AWAKE:
-		status = ath9k_hw_set_power_awake(ah, setChip);
-
-		if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
-			REG_WRITE(ah, AR_RTC_KEEP_AWAKE, 0x2);
-
+		status = ath9k_hw_set_power_awake(ah);
 		break;
 	case ATH9K_PM_FULL_SLEEP:
-		if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
+		if (ath9k_hw_mci_is_enabled(ah))
 			ar9003_mci_set_full_sleep(ah);
 
-		ath9k_set_power_sleep(ah, setChip);
+		ath9k_set_power_sleep(ah);
 		ah->chip_fullsleep = true;
 		break;
 	case ATH9K_PM_NETWORK_SLEEP:
-
-		if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
-			REG_WRITE(ah, AR_RTC_KEEP_AWAKE, 0x2);
-
-		ath9k_set_power_network_sleep(ah, setChip);
+		ath9k_set_power_network_sleep(ah);
 		break;
 	default:
 		ath_err(common, "Unknown power mode %u\n", mode);
@@ -2600,6 +2585,14 @@ int ath9k_hw_fill_cap_info(struct ath_hw *ah)
 	}
 
 
+	if (AR_SREV_9280_20_OR_LATER(ah)) {
+		pCap->hw_caps |= ATH9K_HW_WOW_DEVICE_CAPABLE |
+				 ATH9K_HW_WOW_PATTERN_MATCH_EXACT;
+
+		if (AR_SREV_9280(ah))
+			pCap->hw_caps |= ATH9K_HW_WOW_PATTERN_MATCH_DWORD;
+	}
+
 	return 0;
 }
 
@@ -2777,6 +2770,9 @@ EXPORT_SYMBOL(ath9k_hw_setrxfilter);
 
 bool ath9k_hw_phy_disable(struct ath_hw *ah)
 {
+	if (ath9k_hw_mci_is_enabled(ah))
+		ar9003_mci_bt_gain_ctrl(ah);
+
 	if (!ath9k_hw_set_reset_reg(ah, ATH9K_RESET_WARM))
 		return false;
 
@@ -2916,9 +2912,9 @@ void ath9k_hw_reset_tsf(struct ath_hw *ah)
 }
 EXPORT_SYMBOL(ath9k_hw_reset_tsf);
 
-void ath9k_hw_set_tsfadjust(struct ath_hw *ah, u32 setting)
+void ath9k_hw_set_tsfadjust(struct ath_hw *ah, bool set)
 {
-	if (setting)
+	if (set)
 		ah->misc_mode |= AR_PCU_TX_ADD_TSF;
 	else
 		ah->misc_mode &= ~AR_PCU_TX_ADD_TSF;
@@ -3162,6 +3158,7 @@ static struct {
 	{ AR_SREV_VERSION_9340,		"9340" },
 	{ AR_SREV_VERSION_9485,         "9485" },
 	{ AR_SREV_VERSION_9462,         "9462" },
+	{ AR_SREV_VERSION_9550,         "9550" },
 };
 
 /* For devices with external radios */
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index b620c557c2a..dd0c146d81d 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -48,6 +48,7 @@
 #define AR9300_DEVID_AR9580	0x0033
 #define AR9300_DEVID_AR9462	0x0034
 #define AR9300_DEVID_AR9330	0x0035
+#define AR9300_DEVID_QCA955X	0x0038
 
 #define AR5416_AR9100_DEVID	0x000b
 
@@ -179,6 +180,37 @@
 #define PAPRD_TABLE_SZ			24
 #define PAPRD_IDEAL_AGC2_PWR_RANGE	0xe0
 
+/*
+ * Wake on Wireless
+ */
+
+/* Keep Alive Frame */
+#define KAL_FRAME_LEN		28
+#define KAL_FRAME_TYPE		0x2	/* data frame */
+#define KAL_FRAME_SUB_TYPE	0x4	/* null data frame */
+#define KAL_DURATION_ID		0x3d
+#define KAL_NUM_DATA_WORDS	6
+#define KAL_NUM_DESC_WORDS	12
+#define KAL_ANTENNA_MODE	1
+#define KAL_TO_DS		1
+#define KAL_DELAY		4	/*delay of 4ms between 2 KAL frames */
+#define KAL_TIMEOUT		900
+
+#define MAX_PATTERN_SIZE		256
+#define MAX_PATTERN_MASK_SIZE		32
+#define MAX_NUM_PATTERN			8
+#define MAX_NUM_USER_PATTERN		6 /*  deducting the disassociate and
+					      deauthenticate packets */
+
+/*
+ * WoW trigger mapping to hardware code
+ */
+
+#define AH_WOW_USER_PATTERN_EN		BIT(0)
+#define AH_WOW_MAGIC_PATTERN_EN		BIT(1)
+#define AH_WOW_LINK_CHANGE		BIT(2)
+#define AH_WOW_BEACON_MISS		BIT(3)
+
 enum ath_hw_txq_subtype {
 	ATH_TXQ_AC_BE = 0,
 	ATH_TXQ_AC_BK = 1,
@@ -211,8 +243,22 @@ enum ath9k_hw_caps {
 	ATH9K_HW_CAP_RTT			= BIT(14),
 	ATH9K_HW_CAP_MCI			= BIT(15),
 	ATH9K_HW_CAP_DFS			= BIT(16),
+	ATH9K_HW_WOW_DEVICE_CAPABLE		= BIT(17),
+	ATH9K_HW_WOW_PATTERN_MATCH_EXACT	= BIT(18),
+	ATH9K_HW_WOW_PATTERN_MATCH_DWORD	= BIT(19),
 };
 
+/*
+ * WoW device capabilities
+ * @ATH9K_HW_WOW_DEVICE_CAPABLE: device revision is capable of WoW.
+ * @ATH9K_HW_WOW_PATTERN_MATCH_EXACT: device is capable of matching
+ * an exact user defined pattern or de-authentication/disassoc pattern.
+ * @ATH9K_HW_WOW_PATTERN_MATCH_DWORD: device requires the first four
+ * bytes of the pattern for user defined pattern, de-authentication and
+ * disassociation patterns for all types of possible frames recieved
+ * of those types.
+ */
+
 struct ath9k_hw_capabilities {
 	u32 hw_caps; /* ATH9K_HW_CAP_* from ath9k_hw_caps */
 	u16 rts_aggr_limit;
@@ -814,17 +860,20 @@ struct ath_hw {
 	struct ar5416IniArray iniBank7;
 	struct ar5416IniArray iniAddac;
 	struct ar5416IniArray iniPcieSerdes;
+#ifdef CONFIG_PM_SLEEP
+	struct ar5416IniArray iniPcieSerdesWow;
+#endif
 	struct ar5416IniArray iniPcieSerdesLowPower;
 	struct ar5416IniArray iniModesFastClock;
 	struct ar5416IniArray iniAdditional;
 	struct ar5416IniArray iniModesRxGain;
+	struct ar5416IniArray ini_modes_rx_gain_bounds;
 	struct ar5416IniArray iniModesTxGain;
 	struct ar5416IniArray iniCckfirNormal;
 	struct ar5416IniArray iniCckfirJapan2484;
 	struct ar5416IniArray ini_japan2484;
 	struct ar5416IniArray iniModes_9271_ANI_reg;
 	struct ar5416IniArray ini_radio_post_sys2ant;
-	struct ar5416IniArray ini_BTCOEX_MAX_TXPWR;
 
 	struct ar5416IniArray iniMac[ATH_INI_NUM_SPLIT];
 	struct ar5416IniArray iniBB[ATH_INI_NUM_SPLIT];
@@ -862,6 +911,9 @@ struct ath_hw {
 	/* Enterprise mode cap */
 	u32 ent_mode;
 
+#ifdef CONFIG_PM_SLEEP
+	u32 wow_event_mask;
+#endif
 	bool is_clk_25mhz;
 	int (*get_mac_revision)(void);
 	int (*external_reset)(void);
@@ -942,7 +994,7 @@ u32 ath9k_hw_gettsf32(struct ath_hw *ah);
 u64 ath9k_hw_gettsf64(struct ath_hw *ah);
 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_set_tsfadjust(struct ath_hw *ah, bool set);
 void ath9k_hw_init_global_settings(struct ath_hw *ah);
 u32 ar9003_get_pll_sqsum_dvc(struct ath_hw *ah);
 void ath9k_hw_set11nmac2040(struct ath_hw *ah);
@@ -1020,16 +1072,8 @@ void ar9002_hw_attach_ops(struct ath_hw *ah);
 void ar9003_hw_attach_ops(struct ath_hw *ah);
 
 void ar9002_hw_load_ani_reg(struct ath_hw *ah, struct ath9k_channel *chan);
-/*
- * ANI work can be shared between all families but a next
- * generation implementation of ANI will be used only for AR9003 only
- * for now as the other families still need to be tested with the same
- * next generation ANI. Feel free to start testing it though for the
- * older families (AR5008, AR9001, AR9002) by using modparam_force_new_ani.
- */
-extern int modparam_force_new_ani;
+
 void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning);
-void ath9k_hw_proc_mib_event(struct ath_hw *ah);
 void ath9k_hw_ani_monitor(struct ath_hw *ah, struct ath9k_channel *chan);
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
@@ -1037,6 +1081,12 @@ static inline bool ath9k_hw_btcoex_is_enabled(struct ath_hw *ah)
 {
 	return ah->btcoex_hw.enabled;
 }
+static inline bool ath9k_hw_mci_is_enabled(struct ath_hw *ah)
+{
+	return ah->common.btcoex_enabled &&
+	       (ah->caps.hw_caps & ATH9K_HW_CAP_MCI);
+
+}
 void ath9k_hw_btcoex_enable(struct ath_hw *ah);
 static inline enum ath_btcoex_scheme
 ath9k_hw_get_btcoex_scheme(struct ath_hw *ah)
@@ -1048,6 +1098,10 @@ static inline bool ath9k_hw_btcoex_is_enabled(struct ath_hw *ah)
 {
 	return false;
 }
+static inline bool ath9k_hw_mci_is_enabled(struct ath_hw *ah)
+{
+	return false;
+}
 static inline void ath9k_hw_btcoex_enable(struct ath_hw *ah)
 {
 }
@@ -1058,6 +1112,37 @@ ath9k_hw_get_btcoex_scheme(struct ath_hw *ah)
 }
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
+
+#ifdef CONFIG_PM_SLEEP
+const char *ath9k_hw_wow_event_to_string(u32 wow_event);
+void ath9k_hw_wow_apply_pattern(struct ath_hw *ah, u8 *user_pattern,
+				u8 *user_mask, int pattern_count,
+				int pattern_len);
+u32 ath9k_hw_wow_wakeup(struct ath_hw *ah);
+void ath9k_hw_wow_enable(struct ath_hw *ah, u32 pattern_enable);
+#else
+static inline const char *ath9k_hw_wow_event_to_string(u32 wow_event)
+{
+	return NULL;
+}
+static inline void ath9k_hw_wow_apply_pattern(struct ath_hw *ah,
+					      u8 *user_pattern,
+					      u8 *user_mask,
+					      int pattern_count,
+					      int pattern_len)
+{
+}
+static inline u32 ath9k_hw_wow_wakeup(struct ath_hw *ah)
+{
+	return 0;
+}
+static inline void ath9k_hw_wow_enable(struct ath_hw *ah, u32 pattern_enable)
+{
+}
+#endif
+
+
+
 #define ATH9K_CLOCK_RATE_CCK		22
 #define ATH9K_CLOCK_RATE_5GHZ_OFDM	40
 #define ATH9K_CLOCK_RATE_2GHZ_OFDM	44
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index dee9e092449..f33712140fa 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -434,6 +434,7 @@ static int ath9k_init_queues(struct ath_softc *sc)
 	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;
+		sc->tx.txq_max_pending[i] = ATH_MAX_QDEPTH;
 	}
 	return 0;
 }
@@ -489,6 +490,7 @@ static void ath9k_init_misc(struct ath_softc *sc)
 
 	setup_timer(&common->ani.timer, ath_ani_calibrate, (unsigned long)sc);
 
+	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
 	sc->config.txpowlimit = ATH_TXPOWER_MAX;
 	memcpy(common->bssidmask, ath_bcast_mac, ETH_ALEN);
 	sc->beacon.slottime = ATH9K_SLOT_TIME_9;
@@ -557,9 +559,15 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	spin_lock_init(&sc->debug.samp_lock);
 #endif
 	tasklet_init(&sc->intr_tq, ath9k_tasklet, (unsigned long)sc);
-	tasklet_init(&sc->bcon_tasklet, ath_beacon_tasklet,
+	tasklet_init(&sc->bcon_tasklet, ath9k_beacon_tasklet,
 		     (unsigned long)sc);
 
+	INIT_WORK(&sc->hw_reset_work, ath_reset_work);
+	INIT_WORK(&sc->hw_check_work, ath_hw_check);
+	INIT_WORK(&sc->paprd_work, ath_paprd_calibrate);
+	INIT_DELAYED_WORK(&sc->hw_pll_work, ath_hw_pll_work);
+	setup_timer(&sc->rx_poll_timer, ath_rx_poll, (unsigned long)sc);
+
 	/*
 	 * Cache line size is used to size and align various
 	 * structures used to communicate with the hardware.
@@ -590,6 +598,9 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	ath9k_cmn_init_crypto(sc->sc_ah);
 	ath9k_init_misc(sc);
 
+	if (common->bus_ops->aspm_init)
+		common->bus_ops->aspm_init(common);
+
 	return 0;
 
 err_btcoex:
@@ -703,6 +714,24 @@ void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
 	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 
+#ifdef CONFIG_PM_SLEEP
+
+	if ((ah->caps.hw_caps & ATH9K_HW_WOW_DEVICE_CAPABLE) &&
+	    device_can_wakeup(sc->dev)) {
+
+		hw->wiphy->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT |
+					  WIPHY_WOWLAN_DISCONNECT;
+		hw->wiphy->wowlan.n_patterns = MAX_NUM_USER_PATTERN;
+		hw->wiphy->wowlan.pattern_min_len = 1;
+		hw->wiphy->wowlan.pattern_max_len = MAX_PATTERN_SIZE;
+
+	}
+
+	atomic_set(&sc->wow_sleep_proc_intr, -1);
+	atomic_set(&sc->wow_got_bmiss_intr, -1);
+
+#endif
+
 	hw->queues = 4;
 	hw->max_rates = 4;
 	hw->channel_change_time = 5000;
@@ -782,11 +811,6 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc,
 		ARRAY_SIZE(ath9k_tpt_blink));
 #endif
 
-	INIT_WORK(&sc->hw_reset_work, ath_reset_work);
-	INIT_WORK(&sc->hw_check_work, ath_hw_check);
-	INIT_WORK(&sc->paprd_work, ath_paprd_calibrate);
-	INIT_DELAYED_WORK(&sc->hw_pll_work, ath_hw_pll_work);
-
 	/* Register with mac80211 */
 	error = ieee80211_register_hw(hw);
 	if (error)
@@ -805,9 +829,6 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc,
 			goto error_world;
 	}
 
-	setup_timer(&sc->rx_poll_timer, ath_rx_poll, (unsigned long)sc);
-	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
-
 	ath_init_leds(sc);
 	ath_start_rfkill_poll(sc);
 
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
new file mode 100644
index 00000000000..d4549e9aac5
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -0,0 +1,555 @@
+/*
+ * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "ath9k.h"
+
+/*
+ * TX polling - checks if the TX engine is stuck somewhere
+ * and issues a chip reset if so.
+ */
+void ath_tx_complete_poll_work(struct work_struct *work)
+{
+	struct ath_softc *sc = container_of(work, struct ath_softc,
+					    tx_complete_work.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)) {
+			txq = &sc->tx.txq[i];
+			ath_txq_lock(sc, txq);
+			if (txq->axq_depth) {
+				if (txq->axq_tx_inprogress) {
+					needreset = true;
+					ath_txq_unlock(sc, txq);
+					break;
+				} else {
+					txq->axq_tx_inprogress = true;
+				}
+			}
+			ath_txq_unlock_complete(sc, txq);
+		}
+
+	if (needreset) {
+		ath_dbg(ath9k_hw_common(sc->sc_ah), RESET,
+			"tx hung, resetting the chip\n");
+		ath9k_queue_reset(sc, RESET_TYPE_TX_HANG);
+		return;
+	}
+
+	ieee80211_queue_delayed_work(sc->hw, &sc->tx_complete_work,
+				     msecs_to_jiffies(ATH_TX_COMPLETE_POLL_INT));
+}
+
+/*
+ * Checks if the BB/MAC is hung.
+ */
+void ath_hw_check(struct work_struct *work)
+{
+	struct ath_softc *sc = container_of(work, struct ath_softc, hw_check_work);
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	unsigned long flags;
+	int busy;
+	u8 is_alive, nbeacon = 1;
+	enum ath_reset_type type;
+
+	ath9k_ps_wakeup(sc);
+	is_alive = ath9k_hw_check_alive(sc->sc_ah);
+
+	if (is_alive && !AR_SREV_9300(sc->sc_ah))
+		goto out;
+	else if (!is_alive && AR_SREV_9300(sc->sc_ah)) {
+		ath_dbg(common, RESET,
+			"DCU stuck is detected. Schedule chip reset\n");
+		type = RESET_TYPE_MAC_HANG;
+		goto sched_reset;
+	}
+
+	spin_lock_irqsave(&common->cc_lock, flags);
+	busy = ath_update_survey_stats(sc);
+	spin_unlock_irqrestore(&common->cc_lock, flags);
+
+	ath_dbg(common, RESET, "Possible baseband hang, busy=%d (try %d)\n",
+		busy, sc->hw_busy_count + 1);
+	if (busy >= 99) {
+		if (++sc->hw_busy_count >= 3) {
+			type = RESET_TYPE_BB_HANG;
+			goto sched_reset;
+		}
+	} else if (busy >= 0) {
+		sc->hw_busy_count = 0;
+		nbeacon = 3;
+	}
+
+	ath_start_rx_poll(sc, nbeacon);
+	goto out;
+
+sched_reset:
+	ath9k_queue_reset(sc, type);
+out:
+	ath9k_ps_restore(sc);
+}
+
+/*
+ * PLL-WAR for AR9485/AR9340
+ */
+static bool ath_hw_pll_rx_hang_check(struct ath_softc *sc, u32 pll_sqsum)
+{
+	static int count;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	if (pll_sqsum >= 0x40000) {
+		count++;
+		if (count == 3) {
+			ath_dbg(common, RESET, "PLL WAR, resetting the chip\n");
+			ath9k_queue_reset(sc, RESET_TYPE_PLL_HANG);
+			count = 0;
+			return true;
+		}
+	} else {
+		count = 0;
+	}
+
+	return false;
+}
+
+void ath_hw_pll_work(struct work_struct *work)
+{
+	u32 pll_sqsum;
+	struct ath_softc *sc = container_of(work, struct ath_softc,
+					    hw_pll_work.work);
+	/*
+	 * ensure that the PLL WAR is executed only
+	 * after the STA is associated (or) if the
+	 * beaconing had started in interfaces that
+	 * uses beacons.
+	 */
+	if (!test_bit(SC_OP_BEACONS, &sc->sc_flags))
+		return;
+
+	ath9k_ps_wakeup(sc);
+	pll_sqsum = ar9003_get_pll_sqsum_dvc(sc->sc_ah);
+	ath9k_ps_restore(sc);
+	if (ath_hw_pll_rx_hang_check(sc, pll_sqsum))
+		return;
+
+	ieee80211_queue_delayed_work(sc->hw, &sc->hw_pll_work,
+				     msecs_to_jiffies(ATH_PLL_WORK_INTERVAL));
+}
+
+/*
+ * RX Polling - monitors baseband hangs.
+ */
+void ath_start_rx_poll(struct ath_softc *sc, u8 nbeacon)
+{
+	if (!AR_SREV_9300(sc->sc_ah))
+		return;
+
+	if (!test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags))
+		return;
+
+	mod_timer(&sc->rx_poll_timer, jiffies + msecs_to_jiffies
+		  (nbeacon * sc->cur_beacon_conf.beacon_interval));
+}
+
+void ath_rx_poll(unsigned long data)
+{
+	struct ath_softc *sc = (struct ath_softc *)data;
+
+	ieee80211_queue_work(sc->hw, &sc->hw_check_work);
+}
+
+/*
+ * PA Pre-distortion.
+ */
+static void ath_paprd_activate(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath9k_hw_cal_data *caldata = ah->caldata;
+	int chain;
+
+	if (!caldata || !caldata->paprd_done)
+		return;
+
+	ath9k_ps_wakeup(sc);
+	ar9003_paprd_enable(ah, false);
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->txchainmask & BIT(chain)))
+			continue;
+
+		ar9003_paprd_populate_single_table(ah, caldata, chain);
+	}
+
+	ar9003_paprd_enable(ah, true);
+	ath9k_ps_restore(sc);
+}
+
+static bool ath_paprd_send_frame(struct ath_softc *sc, struct sk_buff *skb, int chain)
+{
+	struct ieee80211_hw *hw = sc->hw;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath_tx_control txctl;
+	int time_left;
+
+	memset(&txctl, 0, sizeof(txctl));
+	txctl.txq = sc->tx.txq_map[WME_AC_BE];
+
+	memset(tx_info, 0, sizeof(*tx_info));
+	tx_info->band = hw->conf.channel->band;
+	tx_info->flags |= IEEE80211_TX_CTL_NO_ACK;
+	tx_info->control.rates[0].idx = 0;
+	tx_info->control.rates[0].count = 1;
+	tx_info->control.rates[0].flags = IEEE80211_TX_RC_MCS;
+	tx_info->control.rates[1].idx = -1;
+
+	init_completion(&sc->paprd_complete);
+	txctl.paprd = BIT(chain);
+
+	if (ath_tx_start(hw, skb, &txctl) != 0) {
+		ath_dbg(common, CALIBRATE, "PAPRD TX failed\n");
+		dev_kfree_skb_any(skb);
+		return false;
+	}
+
+	time_left = wait_for_completion_timeout(&sc->paprd_complete,
+			msecs_to_jiffies(ATH_PAPRD_TIMEOUT));
+
+	if (!time_left)
+		ath_dbg(common, CALIBRATE,
+			"Timeout waiting for paprd training on TX chain %d\n",
+			chain);
+
+	return !!time_left;
+}
+
+void ath_paprd_calibrate(struct work_struct *work)
+{
+	struct ath_softc *sc = container_of(work, struct ath_softc, paprd_work);
+	struct ieee80211_hw *hw = sc->hw;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ieee80211_hdr *hdr;
+	struct sk_buff *skb = NULL;
+	struct ath9k_hw_cal_data *caldata = ah->caldata;
+	struct ath_common *common = ath9k_hw_common(ah);
+	int ftype;
+	int chain_ok = 0;
+	int chain;
+	int len = 1800;
+
+	if (!caldata)
+		return;
+
+	ath9k_ps_wakeup(sc);
+
+	if (ar9003_paprd_init_table(ah) < 0)
+		goto fail_paprd;
+
+	skb = alloc_skb(len, GFP_KERNEL);
+	if (!skb)
+		goto fail_paprd;
+
+	skb_put(skb, len);
+	memset(skb->data, 0, len);
+	hdr = (struct ieee80211_hdr *)skb->data;
+	ftype = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC;
+	hdr->frame_control = cpu_to_le16(ftype);
+	hdr->duration_id = cpu_to_le16(10);
+	memcpy(hdr->addr1, hw->wiphy->perm_addr, ETH_ALEN);
+	memcpy(hdr->addr2, hw->wiphy->perm_addr, ETH_ALEN);
+	memcpy(hdr->addr3, hw->wiphy->perm_addr, ETH_ALEN);
+
+	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
+		if (!(ah->txchainmask & BIT(chain)))
+			continue;
+
+		chain_ok = 0;
+
+		ath_dbg(common, CALIBRATE,
+			"Sending PAPRD frame for thermal measurement on chain %d\n",
+			chain);
+		if (!ath_paprd_send_frame(sc, skb, chain))
+			goto fail_paprd;
+
+		ar9003_paprd_setup_gain_table(ah, chain);
+
+		ath_dbg(common, CALIBRATE,
+			"Sending PAPRD training frame on chain %d\n", chain);
+		if (!ath_paprd_send_frame(sc, skb, chain))
+			goto fail_paprd;
+
+		if (!ar9003_paprd_is_done(ah)) {
+			ath_dbg(common, CALIBRATE,
+				"PAPRD not yet done on chain %d\n", chain);
+			break;
+		}
+
+		if (ar9003_paprd_create_curve(ah, caldata, chain)) {
+			ath_dbg(common, CALIBRATE,
+				"PAPRD create curve failed on chain %d\n",
+				chain);
+			break;
+		}
+
+		chain_ok = 1;
+	}
+	kfree_skb(skb);
+
+	if (chain_ok) {
+		caldata->paprd_done = true;
+		ath_paprd_activate(sc);
+	}
+
+fail_paprd:
+	ath9k_ps_restore(sc);
+}
+
+/*
+ *  ANI performs periodic noise floor calibration
+ *  that is used to adjust and optimize the chip performance.  This
+ *  takes environmental changes (location, temperature) into account.
+ *  When the task is complete, it reschedules itself depending on the
+ *  appropriate interval that was calculated.
+ */
+void ath_ani_calibrate(unsigned long data)
+{
+	struct ath_softc *sc = (struct ath_softc *)data;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	bool longcal = false;
+	bool shortcal = false;
+	bool aniflag = false;
+	unsigned int timestamp = jiffies_to_msecs(jiffies);
+	u32 cal_interval, short_cal_interval, long_cal_interval;
+	unsigned long flags;
+
+	if (ah->caldata && ah->caldata->nfcal_interference)
+		long_cal_interval = ATH_LONG_CALINTERVAL_INT;
+	else
+		long_cal_interval = ATH_LONG_CALINTERVAL;
+
+	short_cal_interval = (ah->opmode == NL80211_IFTYPE_AP) ?
+		ATH_AP_SHORT_CALINTERVAL : ATH_STA_SHORT_CALINTERVAL;
+
+	/* Only calibrate if awake */
+	if (sc->sc_ah->power_mode != ATH9K_PM_AWAKE)
+		goto set_timer;
+
+	ath9k_ps_wakeup(sc);
+
+	/* Long calibration runs independently of short calibration. */
+	if ((timestamp - common->ani.longcal_timer) >= long_cal_interval) {
+		longcal = true;
+		common->ani.longcal_timer = timestamp;
+	}
+
+	/* Short calibration applies only while caldone is false */
+	if (!common->ani.caldone) {
+		if ((timestamp - common->ani.shortcal_timer) >= short_cal_interval) {
+			shortcal = true;
+			common->ani.shortcal_timer = timestamp;
+			common->ani.resetcal_timer = timestamp;
+		}
+	} else {
+		if ((timestamp - common->ani.resetcal_timer) >=
+		    ATH_RESTART_CALINTERVAL) {
+			common->ani.caldone = ath9k_hw_reset_calvalid(ah);
+			if (common->ani.caldone)
+				common->ani.resetcal_timer = timestamp;
+		}
+	}
+
+	/* Verify whether we must check ANI */
+	if (sc->sc_ah->config.enable_ani
+	    && (timestamp - common->ani.checkani_timer) >=
+	    ah->config.ani_poll_interval) {
+		aniflag = true;
+		common->ani.checkani_timer = timestamp;
+	}
+
+	/* Call ANI routine if necessary */
+	if (aniflag) {
+		spin_lock_irqsave(&common->cc_lock, flags);
+		ath9k_hw_ani_monitor(ah, ah->curchan);
+		ath_update_survey_stats(sc);
+		spin_unlock_irqrestore(&common->cc_lock, flags);
+	}
+
+	/* Perform calibration if necessary */
+	if (longcal || shortcal) {
+		common->ani.caldone =
+			ath9k_hw_calibrate(ah, ah->curchan,
+					   ah->rxchainmask, longcal);
+	}
+
+	ath_dbg(common, ANI,
+		"Calibration @%lu finished: %s %s %s, caldone: %s\n",
+		jiffies,
+		longcal ? "long" : "", shortcal ? "short" : "",
+		aniflag ? "ani" : "", common->ani.caldone ? "true" : "false");
+
+	ath9k_debug_samp_bb_mac(sc);
+	ath9k_ps_restore(sc);
+
+set_timer:
+	/*
+	* Set timer interval based on previous results.
+	* The interval must be the shortest necessary to satisfy ANI,
+	* short calibration and long calibration.
+	*/
+	cal_interval = ATH_LONG_CALINTERVAL;
+	if (sc->sc_ah->config.enable_ani)
+		cal_interval = min(cal_interval,
+				   (u32)ah->config.ani_poll_interval);
+	if (!common->ani.caldone)
+		cal_interval = min(cal_interval, (u32)short_cal_interval);
+
+	mod_timer(&common->ani.timer, jiffies + msecs_to_jiffies(cal_interval));
+	if ((sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_PAPRD) && ah->caldata) {
+		if (!ah->caldata->paprd_done)
+			ieee80211_queue_work(sc->hw, &sc->paprd_work);
+		else if (!ah->paprd_table_write_done)
+			ath_paprd_activate(sc);
+	}
+}
+
+void ath_start_ani(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	unsigned long timestamp = jiffies_to_msecs(jiffies);
+
+	if (common->disable_ani ||
+	    !test_bit(SC_OP_ANI_RUN, &sc->sc_flags) ||
+	    (sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL))
+		return;
+
+	common->ani.longcal_timer = timestamp;
+	common->ani.shortcal_timer = timestamp;
+	common->ani.checkani_timer = timestamp;
+
+	ath_dbg(common, ANI, "Starting ANI\n");
+	mod_timer(&common->ani.timer,
+		  jiffies + msecs_to_jiffies((u32)ah->config.ani_poll_interval));
+}
+
+void ath_stop_ani(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	ath_dbg(common, ANI, "Stopping ANI\n");
+	del_timer_sync(&common->ani.timer);
+}
+
+void ath_check_ani(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
+
+	/*
+	 * Check for the various conditions in which ANI has to
+	 * be stopped.
+	 */
+	if (ah->opmode == NL80211_IFTYPE_ADHOC) {
+		if (!cur_conf->enable_beacon)
+			goto stop_ani;
+	} else if (ah->opmode == NL80211_IFTYPE_AP) {
+		if (!cur_conf->enable_beacon) {
+			/*
+			 * Disable ANI only when there are no
+			 * associated stations.
+			 */
+			if (!test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags))
+				goto stop_ani;
+		}
+	} else if (ah->opmode == NL80211_IFTYPE_STATION) {
+		if (!test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags))
+			goto stop_ani;
+	}
+
+	if (!test_bit(SC_OP_ANI_RUN, &sc->sc_flags)) {
+		set_bit(SC_OP_ANI_RUN, &sc->sc_flags);
+		ath_start_ani(sc);
+	}
+
+	return;
+
+stop_ani:
+	clear_bit(SC_OP_ANI_RUN, &sc->sc_flags);
+	ath_stop_ani(sc);
+}
+
+void ath_update_survey_nf(struct ath_softc *sc, int channel)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath9k_channel *chan = &ah->channels[channel];
+	struct survey_info *survey = &sc->survey[channel];
+
+	if (chan->noisefloor) {
+		survey->filled |= SURVEY_INFO_NOISE_DBM;
+		survey->noise = ath9k_hw_getchan_noise(ah, chan);
+	}
+}
+
+/*
+ * 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.
+ */
+int ath_update_survey_stats(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	int pos = ah->curchan - &ah->channels[0];
+	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 -1;
+
+	if (ah->power_mode == ATH9K_PM_AWAKE)
+		ath_hw_cycle_counters_update(common);
+
+	if (cc->cycles > 0) {
+		survey->filled |= SURVEY_INFO_CHANNEL_TIME |
+			SURVEY_INFO_CHANNEL_TIME_BUSY |
+			SURVEY_INFO_CHANNEL_TIME_RX |
+			SURVEY_INFO_CHANNEL_TIME_TX;
+		survey->channel_time += cc->cycles / div;
+		survey->channel_time_busy += cc->rx_busy / div;
+		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;
+}
diff --git a/drivers/net/wireless/ath/ath9k/mac.c b/drivers/net/wireless/ath/ath9k/mac.c
index 04ef775ccee..7990cd55599 100644
--- a/drivers/net/wireless/ath/ath9k/mac.c
+++ b/drivers/net/wireless/ath/ath9k/mac.c
@@ -810,7 +810,7 @@ void ath9k_hw_enable_interrupts(struct ath_hw *ah)
 		return;
 	}
 
-	if (AR_SREV_9340(ah))
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah))
 		sync_default &= ~AR_INTR_SYNC_HOST1_FATAL;
 
 	async_mask = AR_INTR_MAC_IRQ;
diff --git a/drivers/net/wireless/ath/ath9k/mac.h b/drivers/net/wireless/ath/ath9k/mac.h
index 21c955609e6..0eba36dca6f 100644
--- a/drivers/net/wireless/ath/ath9k/mac.h
+++ b/drivers/net/wireless/ath/ath9k/mac.h
@@ -646,6 +646,7 @@ enum ath9k_rx_filter {
 	ATH9K_RX_FILTER_PHYRADAR = 0x00002000,
 	ATH9K_RX_FILTER_MCAST_BCAST_ALL = 0x00008000,
 	ATH9K_RX_FILTER_CONTROL_WRAPPER = 0x00080000,
+	ATH9K_RX_FILTER_4ADDRESS = 0x00100000,
 };
 
 #define ATH9K_RATESERIES_RTS_CTS  0x0001
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index dac1a2709e3..6049d8b8285 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -19,7 +19,10 @@
 #include "ath9k.h"
 #include "btcoex.h"
 
-static u8 parse_mpdudensity(u8 mpdudensity)
+static void ath9k_set_assoc_state(struct ath_softc *sc,
+				  struct ieee80211_vif *vif);
+
+u8 ath9k_parse_mpdudensity(u8 mpdudensity)
 {
 	/*
 	 * 802.11n D2.0 defined values for "Minimum MPDU Start Spacing":
@@ -101,6 +104,7 @@ void ath9k_ps_wakeup(struct ath_softc *sc)
 		spin_lock(&common->cc_lock);
 		ath_hw_cycle_counters_update(common);
 		memset(&common->cc_survey, 0, sizeof(common->cc_survey));
+		memset(&common->cc_ani, 0, sizeof(common->cc_ani));
 		spin_unlock(&common->cc_lock);
 	}
 
@@ -129,6 +133,8 @@ void ath9k_ps_restore(struct ath_softc *sc)
 				     PS_WAIT_FOR_PSPOLL_DATA |
 				     PS_WAIT_FOR_TX_ACK))) {
 		mode = ATH9K_PM_NETWORK_SLEEP;
+		if (ath9k_hw_btcoex_is_enabled(sc->sc_ah))
+			ath9k_btcoex_stop_gen_timer(sc);
 	} else {
 		goto unlock;
 	}
@@ -143,90 +149,17 @@ void ath9k_ps_restore(struct ath_softc *sc)
 	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 }
 
-void ath_start_ani(struct ath_common *common)
-{
-	struct ath_hw *ah = common->ah;
-	unsigned long timestamp = jiffies_to_msecs(jiffies);
-	struct ath_softc *sc = (struct ath_softc *) common->priv;
-
-	if (!(sc->sc_flags & SC_OP_ANI_RUN))
-		return;
-
-	if (sc->sc_flags & SC_OP_OFFCHANNEL)
-		return;
-
-	common->ani.longcal_timer = timestamp;
-	common->ani.shortcal_timer = timestamp;
-	common->ani.checkani_timer = timestamp;
-
-	mod_timer(&common->ani.timer,
-		  jiffies +
-			msecs_to_jiffies((u32)ah->config.ani_poll_interval));
-}
-
-static void ath_update_survey_nf(struct ath_softc *sc, int channel)
-{
-	struct ath_hw *ah = sc->sc_ah;
-	struct ath9k_channel *chan = &ah->channels[channel];
-	struct survey_info *survey = &sc->survey[channel];
-
-	if (chan->noisefloor) {
-		survey->filled |= SURVEY_INFO_NOISE_DBM;
-		survey->noise = ath9k_hw_getchan_noise(ah, chan);
-	}
-}
-
-/*
- * 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);
-	int pos = ah->curchan - &ah->channels[0];
-	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 -1;
-
-	if (ah->power_mode == ATH9K_PM_AWAKE)
-		ath_hw_cycle_counters_update(common);
-
-	if (cc->cycles > 0) {
-		survey->filled |= SURVEY_INFO_CHANNEL_TIME |
-			SURVEY_INFO_CHANNEL_TIME_BUSY |
-			SURVEY_INFO_CHANNEL_TIME_RX |
-			SURVEY_INFO_CHANNEL_TIME_TX;
-		survey->channel_time += cc->cycles / div;
-		survey->channel_time_busy += cc->rx_busy / div;
-		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;
-}
-
 static void __ath_cancel_work(struct ath_softc *sc)
 {
 	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);
+
+#ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+	if (ath9k_hw_mci_is_enabled(sc->sc_ah))
+		cancel_work_sync(&sc->mci_work);
+#endif
 }
 
 static void ath_cancel_work(struct ath_softc *sc)
@@ -235,16 +168,28 @@ static void ath_cancel_work(struct ath_softc *sc)
 	cancel_work_sync(&sc->hw_reset_work);
 }
 
+static void ath_restart_work(struct ath_softc *sc)
+{
+	ieee80211_queue_delayed_work(sc->hw, &sc->tx_complete_work, 0);
+
+	if (AR_SREV_9340(sc->sc_ah) || AR_SREV_9485(sc->sc_ah) ||
+	    AR_SREV_9550(sc->sc_ah))
+		ieee80211_queue_delayed_work(sc->hw, &sc->hw_pll_work,
+				     msecs_to_jiffies(ATH_PLL_WORK_INTERVAL));
+
+	ath_start_rx_poll(sc, 3);
+	ath_start_ani(sc);
+}
+
 static bool ath_prepare_reset(struct ath_softc *sc, bool retry_tx, bool flush)
 {
 	struct ath_hw *ah = sc->sc_ah;
-	struct ath_common *common = ath9k_hw_common(ah);
 	bool ret = true;
 
 	ieee80211_stop_queues(sc->hw);
 
 	sc->hw_busy_count = 0;
-	del_timer_sync(&common->ani.timer);
+	ath_stop_ani(sc);
 	del_timer_sync(&sc->rx_poll_timer);
 
 	ath9k_debug_samp_bb_mac(sc);
@@ -271,6 +216,7 @@ static bool ath_complete_reset(struct ath_softc *sc, bool start)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
+	unsigned long flags;
 
 	if (ath_startrecv(sc) != 0) {
 		ath_err(common, "Unable to restart recv logic\n");
@@ -279,36 +225,30 @@ static bool ath_complete_reset(struct ath_softc *sc, bool start)
 
 	ath9k_cmn_update_txpow(ah, sc->curtxpow,
 			       sc->config.txpowlimit, &sc->curtxpow);
+
+	clear_bit(SC_OP_HW_RESET, &sc->sc_flags);
 	ath9k_hw_set_interrupts(ah);
 	ath9k_hw_enable_interrupts(ah);
 
-	if (!(sc->sc_flags & (SC_OP_OFFCHANNEL)) && start) {
-		if (sc->sc_flags & SC_OP_BEACONS)
-			ath_set_beacon(sc);
-
-		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_rx_poll(sc, 3);
-		if (!common->disable_ani)
-			ath_start_ani(common);
-	}
-
-	if ((ah->caps.hw_caps & ATH9K_HW_CAP_ANT_DIV_COMB) && sc->ant_rx != 3) {
-		struct ath_hw_antcomb_conf div_ant_conf;
-		u8 lna_conf;
-
-		ath9k_hw_antdiv_comb_conf_get(ah, &div_ant_conf);
+	if (!(sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL) && start) {
+		if (!test_bit(SC_OP_BEACONS, &sc->sc_flags))
+			goto work;
 
-		if (sc->ant_rx == 1)
-			lna_conf = ATH_ANT_DIV_COMB_LNA1;
-		else
-			lna_conf = ATH_ANT_DIV_COMB_LNA2;
-		div_ant_conf.main_lna_conf = lna_conf;
-		div_ant_conf.alt_lna_conf = lna_conf;
+		ath9k_set_beacon(sc);
 
-		ath9k_hw_antdiv_comb_conf_set(ah, &div_ant_conf);
+		if (ah->opmode == NL80211_IFTYPE_STATION &&
+		    test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags)) {
+			spin_lock_irqsave(&sc->sc_pm_lock, flags);
+			sc->ps_flags |= PS_BEACON_SYNC | PS_WAIT_FOR_BEACON;
+			spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
+		}
+	work:
+		ath_restart_work(sc);
 	}
 
+	if ((ah->caps.hw_caps & ATH9K_HW_CAP_ANT_DIV_COMB) && sc->ant_rx != 3)
+		ath_ant_comb_update(sc);
+
 	ieee80211_wake_queues(sc->hw);
 
 	return true;
@@ -328,7 +268,7 @@ static int ath_reset_internal(struct ath_softc *sc, struct ath9k_channel *hchan,
 
 	spin_lock_bh(&sc->sc_pcu_lock);
 
-	if (!(sc->sc_flags & SC_OP_OFFCHANNEL)) {
+	if (!(sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)) {
 		fastcc = false;
 		caldata = &sc->caldata;
 	}
@@ -371,7 +311,7 @@ static int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 {
 	int r;
 
-	if (sc->sc_flags & SC_OP_INVALID)
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags))
 		return -EIO;
 
 	r = ath_reset_internal(sc, hchan, false);
@@ -379,262 +319,11 @@ static int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
 	return r;
 }
 
-static void ath_paprd_activate(struct ath_softc *sc)
-{
-	struct ath_hw *ah = sc->sc_ah;
-	struct ath9k_hw_cal_data *caldata = ah->caldata;
-	int chain;
-
-	if (!caldata || !caldata->paprd_done)
-		return;
-
-	ath9k_ps_wakeup(sc);
-	ar9003_paprd_enable(ah, false);
-	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
-		if (!(ah->txchainmask & BIT(chain)))
-			continue;
-
-		ar9003_paprd_populate_single_table(ah, caldata, chain);
-	}
-
-	ar9003_paprd_enable(ah, true);
-	ath9k_ps_restore(sc);
-}
-
-static bool ath_paprd_send_frame(struct ath_softc *sc, struct sk_buff *skb, int chain)
-{
-	struct ieee80211_hw *hw = sc->hw;
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ath_hw *ah = sc->sc_ah;
-	struct ath_common *common = ath9k_hw_common(ah);
-	struct ath_tx_control txctl;
-	int time_left;
-
-	memset(&txctl, 0, sizeof(txctl));
-	txctl.txq = sc->tx.txq_map[WME_AC_BE];
-
-	memset(tx_info, 0, sizeof(*tx_info));
-	tx_info->band = hw->conf.channel->band;
-	tx_info->flags |= IEEE80211_TX_CTL_NO_ACK;
-	tx_info->control.rates[0].idx = 0;
-	tx_info->control.rates[0].count = 1;
-	tx_info->control.rates[0].flags = IEEE80211_TX_RC_MCS;
-	tx_info->control.rates[1].idx = -1;
-
-	init_completion(&sc->paprd_complete);
-	txctl.paprd = BIT(chain);
-
-	if (ath_tx_start(hw, skb, &txctl) != 0) {
-		ath_dbg(common, CALIBRATE, "PAPRD TX failed\n");
-		dev_kfree_skb_any(skb);
-		return false;
-	}
-
-	time_left = wait_for_completion_timeout(&sc->paprd_complete,
-			msecs_to_jiffies(ATH_PAPRD_TIMEOUT));
-
-	if (!time_left)
-		ath_dbg(common, CALIBRATE,
-			"Timeout waiting for paprd training on TX chain %d\n",
-			chain);
-
-	return !!time_left;
-}
-
-void ath_paprd_calibrate(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc, paprd_work);
-	struct ieee80211_hw *hw = sc->hw;
-	struct ath_hw *ah = sc->sc_ah;
-	struct ieee80211_hdr *hdr;
-	struct sk_buff *skb = NULL;
-	struct ath9k_hw_cal_data *caldata = ah->caldata;
-	struct ath_common *common = ath9k_hw_common(ah);
-	int ftype;
-	int chain_ok = 0;
-	int chain;
-	int len = 1800;
-
-	if (!caldata)
-		return;
-
-	ath9k_ps_wakeup(sc);
-
-	if (ar9003_paprd_init_table(ah) < 0)
-		goto fail_paprd;
-
-	skb = alloc_skb(len, GFP_KERNEL);
-	if (!skb)
-		goto fail_paprd;
-
-	skb_put(skb, len);
-	memset(skb->data, 0, len);
-	hdr = (struct ieee80211_hdr *)skb->data;
-	ftype = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC;
-	hdr->frame_control = cpu_to_le16(ftype);
-	hdr->duration_id = cpu_to_le16(10);
-	memcpy(hdr->addr1, hw->wiphy->perm_addr, ETH_ALEN);
-	memcpy(hdr->addr2, hw->wiphy->perm_addr, ETH_ALEN);
-	memcpy(hdr->addr3, hw->wiphy->perm_addr, ETH_ALEN);
-
-	for (chain = 0; chain < AR9300_MAX_CHAINS; chain++) {
-		if (!(ah->txchainmask & BIT(chain)))
-			continue;
-
-		chain_ok = 0;
-
-		ath_dbg(common, CALIBRATE,
-			"Sending PAPRD frame for thermal measurement on chain %d\n",
-			chain);
-		if (!ath_paprd_send_frame(sc, skb, chain))
-			goto fail_paprd;
-
-		ar9003_paprd_setup_gain_table(ah, chain);
-
-		ath_dbg(common, CALIBRATE,
-			"Sending PAPRD training frame on chain %d\n", chain);
-		if (!ath_paprd_send_frame(sc, skb, chain))
-			goto fail_paprd;
-
-		if (!ar9003_paprd_is_done(ah)) {
-			ath_dbg(common, CALIBRATE,
-				"PAPRD not yet done on chain %d\n", chain);
-			break;
-		}
-
-		if (ar9003_paprd_create_curve(ah, caldata, chain)) {
-			ath_dbg(common, CALIBRATE,
-				"PAPRD create curve failed on chain %d\n",
-								   chain);
-			break;
-		}
-
-		chain_ok = 1;
-	}
-	kfree_skb(skb);
-
-	if (chain_ok) {
-		caldata->paprd_done = true;
-		ath_paprd_activate(sc);
-	}
-
-fail_paprd:
-	ath9k_ps_restore(sc);
-}
-
-/*
- *  This routine performs the periodic noise floor calibration function
- *  that is used to adjust and optimize the chip performance.  This
- *  takes environmental changes (location, temperature) into account.
- *  When the task is complete, it reschedules itself depending on the
- *  appropriate interval that was calculated.
- */
-void ath_ani_calibrate(unsigned long data)
-{
-	struct ath_softc *sc = (struct ath_softc *)data;
-	struct ath_hw *ah = sc->sc_ah;
-	struct ath_common *common = ath9k_hw_common(ah);
-	bool longcal = false;
-	bool shortcal = false;
-	bool aniflag = false;
-	unsigned int timestamp = jiffies_to_msecs(jiffies);
-	u32 cal_interval, short_cal_interval, long_cal_interval;
-	unsigned long flags;
-
-	if (ah->caldata && ah->caldata->nfcal_interference)
-		long_cal_interval = ATH_LONG_CALINTERVAL_INT;
-	else
-		long_cal_interval = ATH_LONG_CALINTERVAL;
-
-	short_cal_interval = (ah->opmode == NL80211_IFTYPE_AP) ?
-		ATH_AP_SHORT_CALINTERVAL : ATH_STA_SHORT_CALINTERVAL;
-
-	/* Only calibrate if awake */
-	if (sc->sc_ah->power_mode != ATH9K_PM_AWAKE)
-		goto set_timer;
-
-	ath9k_ps_wakeup(sc);
-
-	/* Long calibration runs independently of short calibration. */
-	if ((timestamp - common->ani.longcal_timer) >= long_cal_interval) {
-		longcal = true;
-		common->ani.longcal_timer = timestamp;
-	}
-
-	/* Short calibration applies only while caldone is false */
-	if (!common->ani.caldone) {
-		if ((timestamp - common->ani.shortcal_timer) >= short_cal_interval) {
-			shortcal = true;
-			common->ani.shortcal_timer = timestamp;
-			common->ani.resetcal_timer = timestamp;
-		}
-	} else {
-		if ((timestamp - common->ani.resetcal_timer) >=
-		    ATH_RESTART_CALINTERVAL) {
-			common->ani.caldone = ath9k_hw_reset_calvalid(ah);
-			if (common->ani.caldone)
-				common->ani.resetcal_timer = timestamp;
-		}
-	}
-
-	/* Verify whether we must check ANI */
-	if (sc->sc_ah->config.enable_ani
-	    && (timestamp - common->ani.checkani_timer) >=
-	    ah->config.ani_poll_interval) {
-		aniflag = true;
-		common->ani.checkani_timer = timestamp;
-	}
-
-	/* Call ANI routine if necessary */
-	if (aniflag) {
-		spin_lock_irqsave(&common->cc_lock, flags);
-		ath9k_hw_ani_monitor(ah, ah->curchan);
-		ath_update_survey_stats(sc);
-		spin_unlock_irqrestore(&common->cc_lock, flags);
-	}
-
-	/* Perform calibration if necessary */
-	if (longcal || shortcal) {
-		common->ani.caldone =
-			ath9k_hw_calibrate(ah, ah->curchan,
-						ah->rxchainmask, longcal);
-	}
-
-	ath_dbg(common, ANI,
-		"Calibration @%lu finished: %s %s %s, caldone: %s\n",
-		jiffies,
-		longcal ? "long" : "", shortcal ? "short" : "",
-		aniflag ? "ani" : "", common->ani.caldone ? "true" : "false");
-
-	ath9k_ps_restore(sc);
-
-set_timer:
-	/*
-	* Set timer interval based on previous results.
-	* The interval must be the shortest necessary to satisfy ANI,
-	* short calibration and long calibration.
-	*/
-	ath9k_debug_samp_bb_mac(sc);
-	cal_interval = ATH_LONG_CALINTERVAL;
-	if (sc->sc_ah->config.enable_ani)
-		cal_interval = min(cal_interval,
-				   (u32)ah->config.ani_poll_interval);
-	if (!common->ani.caldone)
-		cal_interval = min(cal_interval, (u32)short_cal_interval);
-
-	mod_timer(&common->ani.timer, jiffies + msecs_to_jiffies(cal_interval));
-	if ((sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_PAPRD) && ah->caldata) {
-		if (!ah->caldata->paprd_done)
-			ieee80211_queue_work(sc->hw, &sc->paprd_work);
-		else if (!ah->paprd_table_write_done)
-			ath_paprd_activate(sc);
-	}
-}
-
 static void ath_node_attach(struct ath_softc *sc, struct ieee80211_sta *sta,
 			    struct ieee80211_vif *vif)
 {
 	struct ath_node *an;
+	u8 density;
 	an = (struct ath_node *)sta->drv_priv;
 
 #ifdef CONFIG_ATH9K_DEBUGFS
@@ -649,7 +338,8 @@ static void ath_node_attach(struct ath_softc *sc, struct ieee80211_sta *sta,
 		ath_tx_node_init(sc, an);
 		an->maxampdu = 1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
 				     sta->ht_cap.ampdu_factor);
-		an->mpdudensity = parse_mpdudensity(sta->ht_cap.ampdu_density);
+		density = ath9k_parse_mpdudensity(sta->ht_cap.ampdu_density);
+		an->mpdudensity = density;
 	}
 }
 
@@ -668,13 +358,13 @@ static void ath_node_detach(struct ath_softc *sc, struct ieee80211_sta *sta)
 		ath_tx_node_cleanup(sc, an);
 }
 
-
 void ath9k_tasklet(unsigned long data)
 {
 	struct ath_softc *sc = (struct ath_softc *)data;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
-
+	enum ath_reset_type type;
+	unsigned long flags;
 	u32 status = sc->intrstatus;
 	u32 rxmask;
 
@@ -683,20 +373,17 @@ void ath9k_tasklet(unsigned long data)
 
 	if ((status & ATH9K_INT_FATAL) ||
 	    (status & ATH9K_INT_BB_WATCHDOG)) {
-#ifdef CONFIG_ATH9K_DEBUGFS
-		enum ath_reset_type type;
 
 		if (status & ATH9K_INT_FATAL)
 			type = RESET_TYPE_FATAL_INT;
 		else
 			type = RESET_TYPE_BB_WATCHDOG;
 
-		RESET_STAT_INC(sc, type);
-#endif
-		ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
+		ath9k_queue_reset(sc, type);
 		goto out;
 	}
 
+	spin_lock_irqsave(&sc->sc_pm_lock, flags);
 	if ((status & ATH9K_INT_TSFOOR) && sc->ps_enabled) {
 		/*
 		 * TSF sync does not look correct; remain awake to sync with
@@ -705,6 +392,7 @@ void ath9k_tasklet(unsigned long data)
 		ath_dbg(common, PS, "TSFOOR - Sync with next Beacon\n");
 		sc->ps_flags |= PS_WAIT_FOR_BEACON | PS_BEACON_SYNC;
 	}
+	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
 	if (ah->caps.hw_caps & ATH9K_HW_CAP_EDMA)
 		rxmask = (ATH9K_INT_RXHP | ATH9K_INT_RXLP | ATH9K_INT_RXEOL |
@@ -766,15 +454,17 @@ irqreturn_t ath_isr(int irq, void *dev)
 	 * touch anything. Note this can happen early
 	 * on if the IRQ is shared.
 	 */
-	if (sc->sc_flags & SC_OP_INVALID)
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags))
 		return IRQ_NONE;
 
-
 	/* shared irq, not for us */
 
 	if (!ath9k_hw_intrpend(ah))
 		return IRQ_NONE;
 
+	if(test_bit(SC_OP_HW_RESET, &sc->sc_flags))
+		return IRQ_HANDLED;
+
 	/*
 	 * Figure out the reason(s) for the interrupt.  Note
 	 * that the hal returns a pseudo-ISR that may include
@@ -797,6 +487,17 @@ irqreturn_t ath_isr(int irq, void *dev)
 	if (status & SCHED_INTR)
 		sched = true;
 
+#ifdef CONFIG_PM_SLEEP
+	if (status & ATH9K_INT_BMISS) {
+		if (atomic_read(&sc->wow_sleep_proc_intr) == 0) {
+			ath_dbg(common, ANY, "during WoW we got a BMISS\n");
+			atomic_inc(&sc->wow_got_bmiss_intr);
+			atomic_dec(&sc->wow_sleep_proc_intr);
+		}
+	ath_dbg(common, INTERRUPT, "beacon miss interrupt\n");
+	}
+#endif
+
 	/*
 	 * If a FATAL or RXORN interrupt is received, we have to reset the
 	 * chip immediately.
@@ -827,24 +528,6 @@ irqreturn_t ath_isr(int irq, void *dev)
 		ath9k_hw_set_interrupts(ah);
 	}
 
-	if (status & ATH9K_INT_MIB) {
-		/*
-		 * Disable interrupts until we service the MIB
-		 * interrupt; otherwise it will continue to
-		 * fire.
-		 */
-		ath9k_hw_disable_interrupts(ah);
-		/*
-		 * Let the hal handle the event. We assume
-		 * it will clear whatever condition caused
-		 * the interrupt.
-		 */
-		spin_lock(&common->cc_lock);
-		ath9k_hw_proc_mib_event(ah);
-		spin_unlock(&common->cc_lock);
-		ath9k_hw_enable_interrupts(ah);
-	}
-
 	if (!(ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP))
 		if (status & ATH9K_INT_TIM_TIMER) {
 			if (ATH_DBG_WARN_ON_ONCE(sc->ps_idle))
@@ -852,8 +535,10 @@ irqreturn_t ath_isr(int irq, void *dev)
 			/* Clear RxAbort bit so that we can
 			 * receive frames */
 			ath9k_setpower(sc, ATH9K_PM_AWAKE);
+			spin_lock(&sc->sc_pm_lock);
 			ath9k_hw_setrxabort(sc->sc_ah, 0);
 			sc->ps_flags |= PS_WAIT_FOR_BEACON;
+			spin_unlock(&sc->sc_pm_lock);
 		}
 
 chip_reset:
@@ -895,101 +580,20 @@ static int ath_reset(struct ath_softc *sc, bool retry_tx)
 	return r;
 }
 
-void ath_reset_work(struct work_struct *work)
+void ath9k_queue_reset(struct ath_softc *sc, enum ath_reset_type type)
 {
-	struct ath_softc *sc = container_of(work, struct ath_softc, hw_reset_work);
-
-	ath_reset(sc, true);
-}
-
-void ath_hw_check(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc, hw_check_work);
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	unsigned long flags;
-	int busy;
-	u8 is_alive, nbeacon = 1;
-
-	ath9k_ps_wakeup(sc);
-	is_alive = ath9k_hw_check_alive(sc->sc_ah);
-
-	if (is_alive && !AR_SREV_9300(sc->sc_ah))
-		goto out;
-	else if (!is_alive && AR_SREV_9300(sc->sc_ah)) {
-		ath_dbg(common, RESET,
-			"DCU stuck is detected. Schedule chip reset\n");
-		RESET_STAT_INC(sc, RESET_TYPE_MAC_HANG);
-		goto sched_reset;
-	}
-
-	spin_lock_irqsave(&common->cc_lock, flags);
-	busy = ath_update_survey_stats(sc);
-	spin_unlock_irqrestore(&common->cc_lock, flags);
-
-	ath_dbg(common, RESET, "Possible baseband hang, busy=%d (try %d)\n",
-		busy, sc->hw_busy_count + 1);
-	if (busy >= 99) {
-		if (++sc->hw_busy_count >= 3) {
-			RESET_STAT_INC(sc, RESET_TYPE_BB_HANG);
-			goto sched_reset;
-		}
-	} else if (busy >= 0) {
-		sc->hw_busy_count = 0;
-		nbeacon = 3;
-	}
-
-	ath_start_rx_poll(sc, nbeacon);
-	goto out;
-
-sched_reset:
+#ifdef CONFIG_ATH9K_DEBUGFS
+	RESET_STAT_INC(sc, type);
+#endif
+	set_bit(SC_OP_HW_RESET, &sc->sc_flags);
 	ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
-out:
-	ath9k_ps_restore(sc);
-}
-
-static void ath_hw_pll_rx_hang_check(struct ath_softc *sc, u32 pll_sqsum)
-{
-	static int count;
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-
-	if (pll_sqsum >= 0x40000) {
-		count++;
-		if (count == 3) {
-			/* Rx is hung for more than 500ms. Reset it */
-			ath_dbg(common, RESET, "Possible RX hang, resetting\n");
-			RESET_STAT_INC(sc, RESET_TYPE_PLL_HANG);
-			ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
-			count = 0;
-		}
-	} else
-		count = 0;
 }
 
-void ath_hw_pll_work(struct work_struct *work)
+void ath_reset_work(struct work_struct *work)
 {
-	struct ath_softc *sc = container_of(work, struct ath_softc,
-					    hw_pll_work.work);
-	u32 pll_sqsum;
-
-	/*
-	 * ensure that the PLL WAR is executed only
-	 * after the STA is associated (or) if the
-	 * beaconing had started in interfaces that
-	 * uses beacons.
-	 */
-	if (!(sc->sc_flags & SC_OP_BEACONS))
-		return;
-
-	if (AR_SREV_9485(sc->sc_ah)) {
-
-		ath9k_ps_wakeup(sc);
-		pll_sqsum = ar9003_get_pll_sqsum_dvc(sc->sc_ah);
-		ath9k_ps_restore(sc);
-
-		ath_hw_pll_rx_hang_check(sc, pll_sqsum);
+	struct ath_softc *sc = container_of(work, struct ath_softc, hw_reset_work);
 
-		ieee80211_queue_delayed_work(sc->hw, &sc->hw_pll_work, HZ/5);
-	}
+	ath_reset(sc, true);
 }
 
 /**********************/
@@ -1054,10 +658,9 @@ static int ath9k_start(struct ieee80211_hw *hw)
 	if (ah->caps.hw_caps & ATH9K_HW_CAP_HT)
 		ah->imask |= ATH9K_INT_CST;
 
-	if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
-		ah->imask |= ATH9K_INT_MCI;
+	ath_mci_enable(sc);
 
-	sc->sc_flags &= ~SC_OP_INVALID;
+	clear_bit(SC_OP_INVALID, &sc->sc_flags);
 	sc->sc_ah->is_monitoring = false;
 
 	if (!ath_complete_reset(sc, false)) {
@@ -1080,8 +683,6 @@ static int ath9k_start(struct ieee80211_hw *hw)
 
 	spin_unlock_bh(&sc->sc_pcu_lock);
 
-	ath9k_start_btcoex(sc);
-
 	if (ah->caps.pcie_lcr_extsync_en && common->bus_ops->extn_synch_en)
 		common->bus_ops->extn_synch_en(common);
 
@@ -1099,6 +700,7 @@ static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_tx_control txctl;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	unsigned long flags;
 
 	if (sc->ps_enabled) {
 		/*
@@ -1121,6 +723,7 @@ static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		 * completed and if needed, also for RX of buffered frames.
 		 */
 		ath9k_ps_wakeup(sc);
+		spin_lock_irqsave(&sc->sc_pm_lock, flags);
 		if (!(sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP))
 			ath9k_hw_setrxabort(sc->sc_ah, 0);
 		if (ieee80211_is_pspoll(hdr->frame_control)) {
@@ -1136,6 +739,7 @@ static void ath9k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		 * the ps_flags bit is cleared. We are just dropping
 		 * the ps_usecount here.
 		 */
+		spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 		ath9k_ps_restore(sc);
 	}
 
@@ -1176,7 +780,7 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 	ath_cancel_work(sc);
 	del_timer_sync(&sc->rx_poll_timer);
 
-	if (sc->sc_flags & SC_OP_INVALID) {
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags)) {
 		ath_dbg(common, ANY, "Device not present\n");
 		mutex_unlock(&sc->mutex);
 		return;
@@ -1185,8 +789,6 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 	/* Ensure HW is awake when we try to shut it down. */
 	ath9k_ps_wakeup(sc);
 
-	ath9k_stop_btcoex(sc);
-
 	spin_lock_bh(&sc->sc_pcu_lock);
 
 	/* prevent tasklets to enable interrupts once we disable them */
@@ -1233,7 +835,7 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 
 	ath9k_ps_restore(sc);
 
-	sc->sc_flags |= SC_OP_INVALID;
+	set_bit(SC_OP_INVALID, &sc->sc_flags);
 	sc->ps_idle = prev_idle;
 
 	mutex_unlock(&sc->mutex);
@@ -1253,16 +855,6 @@ bool ath9k_uses_beacons(int type)
 	}
 }
 
-static void ath9k_reclaim_beacon(struct ath_softc *sc,
-				 struct ieee80211_vif *vif)
-{
-	struct ath_vif *avp = (void *)vif->drv_priv;
-
-	ath9k_set_beaconing_status(sc, false);
-	ath_beacon_return(sc, avp);
-	ath9k_set_beaconing_status(sc, true);
-}
-
 static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 {
 	struct ath9k_vif_iter_data *iter_data = data;
@@ -1294,6 +886,18 @@ static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 	}
 }
 
+static void ath9k_sta_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+{
+	struct ath_softc *sc = data;
+	struct ath_vif *avp = (void *)vif->drv_priv;
+
+	if (vif->type != NL80211_IFTYPE_STATION)
+		return;
+
+	if (avp->primary_sta_vif)
+		ath9k_set_assoc_state(sc, vif);
+}
+
 /* Called with sc->mutex held. */
 void ath9k_calculate_iter_data(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif,
@@ -1327,21 +931,18 @@ static void ath9k_calculate_summary_state(struct ieee80211_hw *hw,
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_vif_iter_data iter_data;
+	enum nl80211_iftype old_opmode = ah->opmode;
 
 	ath9k_calculate_iter_data(hw, vif, &iter_data);
 
-	/* 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;
+		ath9k_hw_set_tsfadjust(ah, true);
 		ah->opmode = NL80211_IFTYPE_AP;
 	} else {
-		ath9k_hw_set_tsfadjust(ah, 0);
-		sc->sc_flags &= ~SC_OP_TSF_RESET;
+		ath9k_hw_set_tsfadjust(ah, false);
 
 		if (iter_data.nmeshes)
 			ah->opmode = NL80211_IFTYPE_MESH_POINT;
@@ -1353,70 +954,27 @@ static void ath9k_calculate_summary_state(struct ieee80211_hw *hw,
 			ah->opmode = NL80211_IFTYPE_STATION;
 	}
 
-	/*
-	 * Enable MIB interrupts when there are hardware phy counters.
-	 */
-	if ((iter_data.nstations + iter_data.nadhocs + iter_data.nmeshes) > 0) {
-		if (ah->config.enable_ani)
-			ah->imask |= ATH9K_INT_MIB;
+	ath9k_hw_setopmode(ah);
+
+	if ((iter_data.nstations + iter_data.nadhocs + iter_data.nmeshes) > 0)
 		ah->imask |= ATH9K_INT_TSFOOR;
-	} else {
-		ah->imask &= ~ATH9K_INT_MIB;
+	else
 		ah->imask &= ~ATH9K_INT_TSFOOR;
-	}
 
 	ath9k_hw_set_interrupts(ah);
 
-	/* Set up ANI */
-	if (iter_data.naps > 0) {
-		sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
-
-		if (!common->disable_ani) {
-			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);
-	}
-}
-
-/* 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)) {
-		/* Reserve a beacon slot for the vif */
-		ath9k_set_beaconing_status(sc, false);
-		ath_beacon_alloc(sc, vif);
-		ath9k_set_beaconing_status(sc, true);
+	/*
+	 * If we are changing the opmode to STATION,
+	 * a beacon sync needs to be done.
+	 */
+	if (ah->opmode == NL80211_IFTYPE_STATION &&
+	    old_opmode == NL80211_IFTYPE_AP &&
+	    test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags)) {
+		ieee80211_iterate_active_interfaces_atomic(sc->hw,
+						   ath9k_sta_vif_iter, sc);
 	}
 }
 
-void ath_start_rx_poll(struct ath_softc *sc, u8 nbeacon)
-{
-	if (!AR_SREV_9300(sc->sc_ah))
-		return;
-
-	if (!(sc->sc_flags & SC_OP_PRIM_STA_VIF))
-		return;
-
-	mod_timer(&sc->rx_poll_timer, jiffies + msecs_to_jiffies
-			(nbeacon * sc->cur_beacon_conf.beacon_interval));
-}
-
-void ath_rx_poll(unsigned long data)
-{
-	struct ath_softc *sc = (struct ath_softc *)data;
-
-	ieee80211_queue_work(sc->hw, &sc->hw_check_work);
-}
-
 static int ath9k_add_interface(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif)
 {
@@ -1456,7 +1014,10 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
 
 	sc->nvifs++;
 
-	ath9k_do_vif_add_setup(hw, vif);
+	ath9k_calculate_summary_state(hw, vif);
+	if (ath9k_uses_beacons(vif->type))
+		ath9k_beacon_assign_slot(sc, vif);
+
 out:
 	mutex_unlock(&sc->mutex);
 	ath9k_ps_restore(sc);
@@ -1473,6 +1034,7 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
 	int ret = 0;
 
 	ath_dbg(common, CONFIG, "Change Interface\n");
+
 	mutex_lock(&sc->mutex);
 	ath9k_ps_wakeup(sc);
 
@@ -1485,15 +1047,16 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
 		}
 	}
 
-	/* Clean up old vif stuff */
 	if (ath9k_uses_beacons(vif->type))
-		ath9k_reclaim_beacon(sc, vif);
+		ath9k_beacon_remove_slot(sc, vif);
 
-	/* Add new settings */
 	vif->type = new_type;
 	vif->p2p = p2p;
 
-	ath9k_do_vif_add_setup(hw, vif);
+	ath9k_calculate_summary_state(hw, vif);
+	if (ath9k_uses_beacons(vif->type))
+		ath9k_beacon_assign_slot(sc, vif);
+
 out:
 	ath9k_ps_restore(sc);
 	mutex_unlock(&sc->mutex);
@@ -1513,9 +1076,8 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
 
 	sc->nvifs--;
 
-	/* Reclaim beacon resources */
 	if (ath9k_uses_beacons(vif->type))
-		ath9k_reclaim_beacon(sc, vif);
+		ath9k_beacon_remove_slot(sc, vif);
 
 	ath9k_calculate_summary_state(hw, NULL);
 
@@ -1573,14 +1135,17 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 
 	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
 		sc->ps_idle = !!(conf->flags & IEEE80211_CONF_IDLE);
-		if (sc->ps_idle)
+		if (sc->ps_idle) {
 			ath_cancel_work(sc);
-		else
+			ath9k_stop_btcoex(sc);
+		} else {
+			ath9k_start_btcoex(sc);
 			/*
 			 * The chip needs a reset to properly wake up from
 			 * full sleep
 			 */
 			reset_channel = ah->chip_fullsleep;
+		}
 	}
 
 	/*
@@ -1618,11 +1183,6 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 		if (ah->curchan)
 			old_pos = ah->curchan - &ah->channels[0];
 
-		if (hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)
-			sc->sc_flags |= SC_OP_OFFCHANNEL;
-		else
-			sc->sc_flags &= ~SC_OP_OFFCHANNEL;
-
 		ath_dbg(common, CONFIG, "Set channel: %d MHz type: %d\n",
 			curchan->center_freq, conf->channel_type);
 
@@ -1664,6 +1224,7 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 		if (ath_set_channel(sc, hw, &sc->sc_ah->channels[pos]) < 0) {
 			ath_err(common, "Unable to set channel\n");
 			mutex_unlock(&sc->mutex);
+			ath9k_ps_restore(sc);
 			return -EINVAL;
 		}
 
@@ -1813,21 +1374,18 @@ static int ath9k_conf_tx(struct ieee80211_hw *hw,
 	qi.tqi_aifs = params->aifs;
 	qi.tqi_cwmin = params->cw_min;
 	qi.tqi_cwmax = params->cw_max;
-	qi.tqi_burstTime = params->txop;
+	qi.tqi_burstTime = params->txop * 32;
 
 	ath_dbg(common, CONFIG,
 		"Configure tx [queue/halq] [%d/%d], aifs: %d, cw_min: %d, cw_max: %d, txop: %d\n",
 		queue, txq->axq_qnum, params->aifs, params->cw_min,
 		params->cw_max, params->txop);
 
+	ath_update_max_aggr_framelen(sc, queue, qi.tqi_burstTime);
 	ret = ath_txq_update(sc, txq->axq_qnum, &qi);
 	if (ret)
 		ath_err(common, "TXQ Update failed\n");
 
-	if (sc->sc_ah->opmode == NL80211_IFTYPE_ADHOC)
-		if (queue == WME_AC_BE && !ret)
-			ath_beaconq_config(sc);
-
 	mutex_unlock(&sc->mutex);
 	ath9k_ps_restore(sc);
 
@@ -1896,83 +1454,53 @@ static int ath9k_set_key(struct ieee80211_hw *hw,
 
 	return ret;
 }
-static void ath9k_bss_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+
+static void ath9k_set_assoc_state(struct ath_softc *sc,
+				  struct ieee80211_vif *vif)
 {
-	struct ath_softc *sc = data;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
 	struct ath_vif *avp = (void *)vif->drv_priv;
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	unsigned long flags;
+
+	set_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags);
+	avp->primary_sta_vif = true;
 
 	/*
-	 * Skip iteration if primary station vif's bss info
-	 * was not changed
+	 * Set the AID, BSSID and do beacon-sync only when
+	 * the HW opmode is STATION.
+	 *
+	 * But the primary bit is set above in any case.
 	 */
-	if (sc->sc_flags & SC_OP_PRIM_STA_VIF)
+	if (sc->sc_ah->opmode != NL80211_IFTYPE_STATION)
 		return;
 
-	if (bss_conf->assoc) {
-		sc->sc_flags |= SC_OP_PRIM_STA_VIF;
-		avp->primary_sta_vif = true;
-		memcpy(common->curbssid, bss_conf->bssid, ETH_ALEN);
-		common->curaid = bss_conf->aid;
-		ath9k_hw_write_associd(sc->sc_ah);
-		ath_dbg(common, CONFIG, "Bss Info ASSOC %d, bssid: %pM\n",
-			bss_conf->aid, common->curbssid);
-		ath_beacon_config(sc, vif);
-		/*
-		 * Request a re-configuration of Beacon related timers
-		 * on the receipt of the first Beacon frame (i.e.,
-		 * after time sync with the AP).
-		 */
-		sc->ps_flags |= PS_BEACON_SYNC | PS_WAIT_FOR_BEACON;
-		/* Reset rssi stats */
-		sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
-		sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
+	memcpy(common->curbssid, bss_conf->bssid, ETH_ALEN);
+	common->curaid = bss_conf->aid;
+	ath9k_hw_write_associd(sc->sc_ah);
 
-		ath_start_rx_poll(sc, 3);
+	sc->last_rssi = ATH_RSSI_DUMMY_MARKER;
+	sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
 
-		if (!common->disable_ani) {
-			sc->sc_flags |= SC_OP_ANI_RUN;
-			ath_start_ani(common);
-		}
+	spin_lock_irqsave(&sc->sc_pm_lock, flags);
+	sc->ps_flags |= PS_BEACON_SYNC | PS_WAIT_FOR_BEACON;
+	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
-	}
+	ath_dbg(common, CONFIG,
+		"Primary Station interface: %pM, BSSID: %pM\n",
+		vif->addr, common->curbssid);
 }
 
-static void ath9k_config_bss(struct ath_softc *sc, struct ieee80211_vif *vif)
+static void ath9k_bss_assoc_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 {
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_softc *sc = data;
 	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
-	struct ath_vif *avp = (void *)vif->drv_priv;
 
-	if (sc->sc_ah->opmode != NL80211_IFTYPE_STATION)
+	if (test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags))
 		return;
 
-	/* Reconfigure bss info */
-	if (avp->primary_sta_vif && !bss_conf->assoc) {
-		ath_dbg(common, CONFIG, "Bss Info DISASSOC %d, bssid %pM\n",
-			common->curaid, common->curbssid);
-		sc->sc_flags &= ~(SC_OP_PRIM_STA_VIF | SC_OP_BEACONS);
-		avp->primary_sta_vif = false;
-		memset(common->curbssid, 0, ETH_ALEN);
-		common->curaid = 0;
-	}
-
-	ieee80211_iterate_active_interfaces_atomic(
-			sc->hw, ath9k_bss_iter, sc);
-
-	/*
-	 * None of station vifs are associated.
-	 * Clear bssid & aid
-	 */
-	if (!(sc->sc_flags & SC_OP_PRIM_STA_VIF)) {
-		ath9k_hw_write_associd(sc->sc_ah);
-		/* Stop ANI */
-		sc->sc_flags &= ~SC_OP_ANI_RUN;
-		del_timer_sync(&common->ani.timer);
-		del_timer_sync(&sc->rx_poll_timer);
-		memset(&sc->caldata, 0, sizeof(sc->caldata));
-	}
+	if (bss_conf->assoc)
+		ath9k_set_assoc_state(sc, vif);
 }
 
 static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
@@ -1980,6 +1508,11 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 				   struct ieee80211_bss_conf *bss_conf,
 				   u32 changed)
 {
+#define CHECK_ANI				\
+	(BSS_CHANGED_ASSOC |			\
+	 BSS_CHANGED_IBSS |			\
+	 BSS_CHANGED_BEACON_ENABLED)
+
 	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
@@ -1990,53 +1523,41 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	mutex_lock(&sc->mutex);
 
 	if (changed & BSS_CHANGED_ASSOC) {
-		ath9k_config_bss(sc, vif);
+		ath_dbg(common, CONFIG, "BSSID %pM Changed ASSOC %d\n",
+			bss_conf->bssid, bss_conf->assoc);
+
+		if (avp->primary_sta_vif && !bss_conf->assoc) {
+			clear_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags);
+			avp->primary_sta_vif = false;
 
-		ath_dbg(common, CONFIG, "BSSID: %pM aid: 0x%x\n",
-			common->curbssid, common->curaid);
+			if (ah->opmode == NL80211_IFTYPE_STATION)
+				clear_bit(SC_OP_BEACONS, &sc->sc_flags);
+		}
+
+		ieee80211_iterate_active_interfaces_atomic(sc->hw,
+						   ath9k_bss_assoc_iter, sc);
+
+		if (!test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags) &&
+		    ah->opmode == NL80211_IFTYPE_STATION) {
+			memset(common->curbssid, 0, ETH_ALEN);
+			common->curaid = 0;
+			ath9k_hw_write_associd(sc->sc_ah);
+		}
 	}
 
 	if (changed & BSS_CHANGED_IBSS) {
-		/* There can be only one vif available */
 		memcpy(common->curbssid, bss_conf->bssid, ETH_ALEN);
 		common->curaid = bss_conf->aid;
 		ath9k_hw_write_associd(sc->sc_ah);
-
-		if (bss_conf->ibss_joined) {
-			sc->sc_ah->stats.avgbrssi = ATH_RSSI_DUMMY_MARKER;
-
-			if (!common->disable_ani) {
-				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);
-			del_timer_sync(&sc->rx_poll_timer);
-		}
 	}
 
-	/*
-	 * In case of AP mode, the HW TSF has to be reset
-	 * when the beacon interval changes.
-	 */
-	if ((changed & BSS_CHANGED_BEACON_INT) &&
-	    (vif->type == NL80211_IFTYPE_AP))
-		sc->sc_flags |= SC_OP_TSF_RESET;
-
-	/* Configure beaconing (AP, IBSS, MESH) */
-	if (ath9k_uses_beacons(vif->type) &&
-	    ((changed & BSS_CHANGED_BEACON) ||
-	     (changed & BSS_CHANGED_BEACON_ENABLED) ||
-	     (changed & BSS_CHANGED_BEACON_INT))) {
-		ath9k_set_beaconing_status(sc, false);
-		if (bss_conf->enable_beacon)
-			ath_beacon_alloc(sc, vif);
-		else
-			avp->is_bslot_active = false;
-		ath_beacon_config(sc, vif);
-		ath9k_set_beaconing_status(sc, true);
+	if ((changed & BSS_CHANGED_BEACON_ENABLED) ||
+	    (changed & BSS_CHANGED_BEACON_INT)) {
+		if (ah->opmode == NL80211_IFTYPE_AP &&
+		    bss_conf->enable_beacon)
+			ath9k_set_tsfadjust(sc, vif);
+		if (ath9k_allow_beacon_config(sc, vif))
+			ath9k_beacon_config(sc, vif, changed);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -2058,8 +1579,13 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 		}
 	}
 
+	if (changed & CHECK_ANI)
+		ath_check_ani(sc);
+
 	mutex_unlock(&sc->mutex);
 	ath9k_ps_restore(sc);
+
+#undef CHECK_ANI
 }
 
 static u64 ath9k_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
@@ -2215,7 +1741,7 @@ static void ath9k_flush(struct ieee80211_hw *hw, bool drop)
 		return;
 	}
 
-	if (sc->sc_flags & SC_OP_INVALID) {
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags)) {
 		ath_dbg(common, ANY, "Device not present\n");
 		mutex_unlock(&sc->mutex);
 		return;
@@ -2288,10 +1814,11 @@ static int ath9k_tx_last_beacon(struct ieee80211_hw *hw)
 	if (!vif)
 		return 0;
 
-	avp = (void *)vif->drv_priv;
-	if (!avp->is_bslot_active)
+	if (!vif->bss_conf.enable_beacon)
 		return 0;
 
+	avp = (void *)vif->drv_priv;
+
 	if (!sc->beacon.tx_processed && !edma) {
 		tasklet_disable(&sc->bcon_tasklet);
 
@@ -2345,12 +1872,29 @@ static u32 fill_chainmask(u32 cap, u32 new)
 	return filled;
 }
 
+static bool validate_antenna_mask(struct ath_hw *ah, u32 val)
+{
+	switch (val & 0x7) {
+	case 0x1:
+	case 0x3:
+	case 0x7:
+		return true;
+	case 0x2:
+		return (ah->caps.rx_chainmask == 1);
+	default:
+		return false;
+	}
+}
+
 static int ath9k_set_antenna(struct ieee80211_hw *hw, u32 tx_ant, u32 rx_ant)
 {
 	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
 
-	if (!rx_ant || !tx_ant)
+	if (ah->caps.rx_chainmask != 1)
+		rx_ant |= tx_ant;
+
+	if (!validate_antenna_mask(ah, rx_ant) || !tx_ant)
 		return -EINVAL;
 
 	sc->ant_rx = rx_ant;
@@ -2380,6 +1924,490 @@ static int ath9k_get_antenna(struct ieee80211_hw *hw, u32 *tx_ant, u32 *rx_ant)
 	return 0;
 }
 
+#ifdef CONFIG_ATH9K_DEBUGFS
+
+/* Ethtool support for get-stats */
+
+#define AMKSTR(nm) #nm "_BE", #nm "_BK", #nm "_VI", #nm "_VO"
+static const char ath9k_gstrings_stats[][ETH_GSTRING_LEN] = {
+	"tx_pkts_nic",
+	"tx_bytes_nic",
+	"rx_pkts_nic",
+	"rx_bytes_nic",
+	AMKSTR(d_tx_pkts),
+	AMKSTR(d_tx_bytes),
+	AMKSTR(d_tx_mpdus_queued),
+	AMKSTR(d_tx_mpdus_completed),
+	AMKSTR(d_tx_mpdu_xretries),
+	AMKSTR(d_tx_aggregates),
+	AMKSTR(d_tx_ampdus_queued_hw),
+	AMKSTR(d_tx_ampdus_queued_sw),
+	AMKSTR(d_tx_ampdus_completed),
+	AMKSTR(d_tx_ampdu_retries),
+	AMKSTR(d_tx_ampdu_xretries),
+	AMKSTR(d_tx_fifo_underrun),
+	AMKSTR(d_tx_op_exceeded),
+	AMKSTR(d_tx_timer_expiry),
+	AMKSTR(d_tx_desc_cfg_err),
+	AMKSTR(d_tx_data_underrun),
+	AMKSTR(d_tx_delim_underrun),
+
+	"d_rx_decrypt_crc_err",
+	"d_rx_phy_err",
+	"d_rx_mic_err",
+	"d_rx_pre_delim_crc_err",
+	"d_rx_post_delim_crc_err",
+	"d_rx_decrypt_busy_err",
+
+	"d_rx_phyerr_radar",
+	"d_rx_phyerr_ofdm_timing",
+	"d_rx_phyerr_cck_timing",
+
+};
+#define ATH9K_SSTATS_LEN ARRAY_SIZE(ath9k_gstrings_stats)
+
+static void ath9k_get_et_strings(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 u32 sset, u8 *data)
+{
+	if (sset == ETH_SS_STATS)
+		memcpy(data, *ath9k_gstrings_stats,
+		       sizeof(ath9k_gstrings_stats));
+}
+
+static int ath9k_get_et_sset_count(struct ieee80211_hw *hw,
+				   struct ieee80211_vif *vif, int sset)
+{
+	if (sset == ETH_SS_STATS)
+		return ATH9K_SSTATS_LEN;
+	return 0;
+}
+
+#define PR_QNUM(_n) (sc->tx.txq_map[_n]->axq_qnum)
+#define AWDATA(elem)							\
+	do {								\
+		data[i++] = sc->debug.stats.txstats[PR_QNUM(WME_AC_BE)].elem; \
+		data[i++] = sc->debug.stats.txstats[PR_QNUM(WME_AC_BK)].elem; \
+		data[i++] = sc->debug.stats.txstats[PR_QNUM(WME_AC_VI)].elem; \
+		data[i++] = sc->debug.stats.txstats[PR_QNUM(WME_AC_VO)].elem; \
+	} while (0)
+
+#define AWDATA_RX(elem)						\
+	do {							\
+		data[i++] = sc->debug.stats.rxstats.elem;	\
+	} while (0)
+
+static void ath9k_get_et_stats(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct ethtool_stats *stats, u64 *data)
+{
+	struct ath_softc *sc = hw->priv;
+	int i = 0;
+
+	data[i++] = (sc->debug.stats.txstats[PR_QNUM(WME_AC_BE)].tx_pkts_all +
+		     sc->debug.stats.txstats[PR_QNUM(WME_AC_BK)].tx_pkts_all +
+		     sc->debug.stats.txstats[PR_QNUM(WME_AC_VI)].tx_pkts_all +
+		     sc->debug.stats.txstats[PR_QNUM(WME_AC_VO)].tx_pkts_all);
+	data[i++] = (sc->debug.stats.txstats[PR_QNUM(WME_AC_BE)].tx_bytes_all +
+		     sc->debug.stats.txstats[PR_QNUM(WME_AC_BK)].tx_bytes_all +
+		     sc->debug.stats.txstats[PR_QNUM(WME_AC_VI)].tx_bytes_all +
+		     sc->debug.stats.txstats[PR_QNUM(WME_AC_VO)].tx_bytes_all);
+	AWDATA_RX(rx_pkts_all);
+	AWDATA_RX(rx_bytes_all);
+
+	AWDATA(tx_pkts_all);
+	AWDATA(tx_bytes_all);
+	AWDATA(queued);
+	AWDATA(completed);
+	AWDATA(xretries);
+	AWDATA(a_aggr);
+	AWDATA(a_queued_hw);
+	AWDATA(a_queued_sw);
+	AWDATA(a_completed);
+	AWDATA(a_retries);
+	AWDATA(a_xretries);
+	AWDATA(fifo_underrun);
+	AWDATA(xtxop);
+	AWDATA(timer_exp);
+	AWDATA(desc_cfg_err);
+	AWDATA(data_underrun);
+	AWDATA(delim_underrun);
+
+	AWDATA_RX(decrypt_crc_err);
+	AWDATA_RX(phy_err);
+	AWDATA_RX(mic_err);
+	AWDATA_RX(pre_delim_crc_err);
+	AWDATA_RX(post_delim_crc_err);
+	AWDATA_RX(decrypt_busy_err);
+
+	AWDATA_RX(phy_err_stats[ATH9K_PHYERR_RADAR]);
+	AWDATA_RX(phy_err_stats[ATH9K_PHYERR_OFDM_TIMING]);
+	AWDATA_RX(phy_err_stats[ATH9K_PHYERR_CCK_TIMING]);
+
+	WARN_ON(i != ATH9K_SSTATS_LEN);
+}
+
+/* End of ethtool get-stats functions */
+
+#endif
+
+
+#ifdef CONFIG_PM_SLEEP
+
+static void ath9k_wow_map_triggers(struct ath_softc *sc,
+				   struct cfg80211_wowlan *wowlan,
+				   u32 *wow_triggers)
+{
+	if (wowlan->disconnect)
+		*wow_triggers |= AH_WOW_LINK_CHANGE |
+				 AH_WOW_BEACON_MISS;
+	if (wowlan->magic_pkt)
+		*wow_triggers |= AH_WOW_MAGIC_PATTERN_EN;
+
+	if (wowlan->n_patterns)
+		*wow_triggers |= AH_WOW_USER_PATTERN_EN;
+
+	sc->wow_enabled = *wow_triggers;
+
+}
+
+static void ath9k_wow_add_disassoc_deauth_pattern(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_hw_capabilities *pcaps = &ah->caps;
+	int pattern_count = 0;
+	int i, byte_cnt;
+	u8 dis_deauth_pattern[MAX_PATTERN_SIZE];
+	u8 dis_deauth_mask[MAX_PATTERN_SIZE];
+
+	memset(dis_deauth_pattern, 0, MAX_PATTERN_SIZE);
+	memset(dis_deauth_mask, 0, MAX_PATTERN_SIZE);
+
+	/*
+	 * Create Dissassociate / Deauthenticate packet filter
+	 *
+	 *     2 bytes        2 byte    6 bytes   6 bytes  6 bytes
+	 *  +--------------+----------+---------+--------+--------+----
+	 *  + Frame Control+ Duration +   DA    +  SA    +  BSSID +
+	 *  +--------------+----------+---------+--------+--------+----
+	 *
+	 * The above is the management frame format for disassociate/
+	 * deauthenticate pattern, from this we need to match the first byte
+	 * of 'Frame Control' and DA, SA, and BSSID fields
+	 * (skipping 2nd byte of FC and Duration feild.
+	 *
+	 * Disassociate pattern
+	 * --------------------
+	 * Frame control = 00 00 1010
+	 * DA, SA, BSSID = x:x:x:x:x:x
+	 * Pattern will be A0000000 | x:x:x:x:x:x | x:x:x:x:x:x
+	 *			    | x:x:x:x:x:x  -- 22 bytes
+	 *
+	 * Deauthenticate pattern
+	 * ----------------------
+	 * Frame control = 00 00 1100
+	 * DA, SA, BSSID = x:x:x:x:x:x
+	 * Pattern will be C0000000 | x:x:x:x:x:x | x:x:x:x:x:x
+	 *			    | x:x:x:x:x:x  -- 22 bytes
+	 */
+
+	/* Create Disassociate Pattern first */
+
+	byte_cnt = 0;
+
+	/* Fill out the mask with all FF's */
+
+	for (i = 0; i < MAX_PATTERN_MASK_SIZE; i++)
+		dis_deauth_mask[i] = 0xff;
+
+	/* copy the first byte of frame control field */
+	dis_deauth_pattern[byte_cnt] = 0xa0;
+	byte_cnt++;
+
+	/* skip 2nd byte of frame control and Duration field */
+	byte_cnt += 3;
+
+	/*
+	 * need not match the destination mac address, it can be a broadcast
+	 * mac address or an unicast to this station
+	 */
+	byte_cnt += 6;
+
+	/* copy the source mac address */
+	memcpy((dis_deauth_pattern + byte_cnt), common->curbssid, ETH_ALEN);
+
+	byte_cnt += 6;
+
+	/* copy the bssid, its same as the source mac address */
+
+	memcpy((dis_deauth_pattern + byte_cnt), common->curbssid, ETH_ALEN);
+
+	/* Create Disassociate pattern mask */
+
+	if (pcaps->hw_caps & ATH9K_HW_WOW_PATTERN_MATCH_EXACT) {
+
+		if (pcaps->hw_caps & ATH9K_HW_WOW_PATTERN_MATCH_DWORD) {
+			/*
+			 * for AR9280, because of hardware limitation, the
+			 * first 4 bytes have to be matched for all patterns.
+			 * the mask for disassociation and de-auth pattern
+			 * matching need to enable the first 4 bytes.
+			 * also the duration field needs to be filled.
+			 */
+			dis_deauth_mask[0] = 0xf0;
+
+			/*
+			 * fill in duration field
+			 FIXME: what is the exact value ?
+			 */
+			dis_deauth_pattern[2] = 0xff;
+			dis_deauth_pattern[3] = 0xff;
+		} else {
+			dis_deauth_mask[0] = 0xfe;
+		}
+
+		dis_deauth_mask[1] = 0x03;
+		dis_deauth_mask[2] = 0xc0;
+	} else {
+		dis_deauth_mask[0] = 0xef;
+		dis_deauth_mask[1] = 0x3f;
+		dis_deauth_mask[2] = 0x00;
+		dis_deauth_mask[3] = 0xfc;
+	}
+
+	ath_dbg(common, WOW, "Adding disassoc/deauth patterns for WoW\n");
+
+	ath9k_hw_wow_apply_pattern(ah, dis_deauth_pattern, dis_deauth_mask,
+				   pattern_count, byte_cnt);
+
+	pattern_count++;
+	/*
+	 * for de-authenticate pattern, only the first byte of the frame
+	 * control field gets changed from 0xA0 to 0xC0
+	 */
+	dis_deauth_pattern[0] = 0xC0;
+
+	ath9k_hw_wow_apply_pattern(ah, dis_deauth_pattern, dis_deauth_mask,
+				   pattern_count, byte_cnt);
+
+}
+
+static void ath9k_wow_add_pattern(struct ath_softc *sc,
+				  struct cfg80211_wowlan *wowlan)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath9k_wow_pattern *wow_pattern = NULL;
+	struct cfg80211_wowlan_trig_pkt_pattern *patterns = wowlan->patterns;
+	int mask_len;
+	s8 i = 0;
+
+	if (!wowlan->n_patterns)
+		return;
+
+	/*
+	 * Add the new user configured patterns
+	 */
+	for (i = 0; i < wowlan->n_patterns; i++) {
+
+		wow_pattern = kzalloc(sizeof(*wow_pattern), GFP_KERNEL);
+
+		if (!wow_pattern)
+			return;
+
+		/*
+		 * TODO: convert the generic user space pattern to
+		 * appropriate chip specific/802.11 pattern.
+		 */
+
+		mask_len = DIV_ROUND_UP(wowlan->patterns[i].pattern_len, 8);
+		memset(wow_pattern->pattern_bytes, 0, MAX_PATTERN_SIZE);
+		memset(wow_pattern->mask_bytes, 0, MAX_PATTERN_SIZE);
+		memcpy(wow_pattern->pattern_bytes, patterns[i].pattern,
+		       patterns[i].pattern_len);
+		memcpy(wow_pattern->mask_bytes, patterns[i].mask, mask_len);
+		wow_pattern->pattern_len = patterns[i].pattern_len;
+
+		/*
+		 * just need to take care of deauth and disssoc pattern,
+		 * make sure we don't overwrite them.
+		 */
+
+		ath9k_hw_wow_apply_pattern(ah, wow_pattern->pattern_bytes,
+					   wow_pattern->mask_bytes,
+					   i + 2,
+					   wow_pattern->pattern_len);
+		kfree(wow_pattern);
+
+	}
+
+}
+
+static int ath9k_suspend(struct ieee80211_hw *hw,
+			 struct cfg80211_wowlan *wowlan)
+{
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	u32 wow_triggers_enabled = 0;
+	int ret = 0;
+
+	mutex_lock(&sc->mutex);
+
+	ath_cancel_work(sc);
+	del_timer_sync(&common->ani.timer);
+	del_timer_sync(&sc->rx_poll_timer);
+
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags)) {
+		ath_dbg(common, ANY, "Device not present\n");
+		ret = -EINVAL;
+		goto fail_wow;
+	}
+
+	if (WARN_ON(!wowlan)) {
+		ath_dbg(common, WOW, "None of the WoW triggers enabled\n");
+		ret = -EINVAL;
+		goto fail_wow;
+	}
+
+	if (!device_can_wakeup(sc->dev)) {
+		ath_dbg(common, WOW, "device_can_wakeup failed, WoW is not enabled\n");
+		ret = 1;
+		goto fail_wow;
+	}
+
+	/*
+	 * none of the sta vifs are associated
+	 * and we are not currently handling multivif
+	 * cases, for instance we have to seperately
+	 * configure 'keep alive frame' for each
+	 * STA.
+	 */
+
+	if (!test_bit(SC_OP_PRIM_STA_VIF, &sc->sc_flags)) {
+		ath_dbg(common, WOW, "None of the STA vifs are associated\n");
+		ret = 1;
+		goto fail_wow;
+	}
+
+	if (sc->nvifs > 1) {
+		ath_dbg(common, WOW, "WoW for multivif is not yet supported\n");
+		ret = 1;
+		goto fail_wow;
+	}
+
+	ath9k_wow_map_triggers(sc, wowlan, &wow_triggers_enabled);
+
+	ath_dbg(common, WOW, "WoW triggers enabled 0x%x\n",
+		wow_triggers_enabled);
+
+	ath9k_ps_wakeup(sc);
+
+	ath9k_stop_btcoex(sc);
+
+	/*
+	 * Enable wake up on recieving disassoc/deauth
+	 * frame by default.
+	 */
+	ath9k_wow_add_disassoc_deauth_pattern(sc);
+
+	if (wow_triggers_enabled & AH_WOW_USER_PATTERN_EN)
+		ath9k_wow_add_pattern(sc, wowlan);
+
+	spin_lock_bh(&sc->sc_pcu_lock);
+	/*
+	 * To avoid false wake, we enable beacon miss interrupt only
+	 * when we go to sleep. We save the current interrupt mask
+	 * so we can restore it after the system wakes up
+	 */
+	sc->wow_intr_before_sleep = ah->imask;
+	ah->imask &= ~ATH9K_INT_GLOBAL;
+	ath9k_hw_disable_interrupts(ah);
+	ah->imask = ATH9K_INT_BMISS | ATH9K_INT_GLOBAL;
+	ath9k_hw_set_interrupts(ah);
+	ath9k_hw_enable_interrupts(ah);
+
+	spin_unlock_bh(&sc->sc_pcu_lock);
+
+	/*
+	 * we can now sync irq and kill any running tasklets, since we already
+	 * disabled interrupts and not holding a spin lock
+	 */
+	synchronize_irq(sc->irq);
+	tasklet_kill(&sc->intr_tq);
+
+	ath9k_hw_wow_enable(ah, wow_triggers_enabled);
+
+	ath9k_ps_restore(sc);
+	ath_dbg(common, ANY, "WoW enabled in ath9k\n");
+	atomic_inc(&sc->wow_sleep_proc_intr);
+
+fail_wow:
+	mutex_unlock(&sc->mutex);
+	return ret;
+}
+
+static int ath9k_resume(struct ieee80211_hw *hw)
+{
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	u32 wow_status;
+
+	mutex_lock(&sc->mutex);
+
+	ath9k_ps_wakeup(sc);
+
+	spin_lock_bh(&sc->sc_pcu_lock);
+
+	ath9k_hw_disable_interrupts(ah);
+	ah->imask = sc->wow_intr_before_sleep;
+	ath9k_hw_set_interrupts(ah);
+	ath9k_hw_enable_interrupts(ah);
+
+	spin_unlock_bh(&sc->sc_pcu_lock);
+
+	wow_status = ath9k_hw_wow_wakeup(ah);
+
+	if (atomic_read(&sc->wow_got_bmiss_intr) == 0) {
+		/*
+		 * some devices may not pick beacon miss
+		 * as the reason they woke up so we add
+		 * that here for that shortcoming.
+		 */
+		wow_status |= AH_WOW_BEACON_MISS;
+		atomic_dec(&sc->wow_got_bmiss_intr);
+		ath_dbg(common, ANY, "Beacon miss interrupt picked up during WoW sleep\n");
+	}
+
+	atomic_dec(&sc->wow_sleep_proc_intr);
+
+	if (wow_status) {
+		ath_dbg(common, ANY, "Waking up due to WoW triggers %s with WoW status = %x\n",
+			ath9k_hw_wow_event_to_string(wow_status), wow_status);
+	}
+
+	ath_restart_work(sc);
+	ath9k_start_btcoex(sc);
+
+	ath9k_ps_restore(sc);
+	mutex_unlock(&sc->mutex);
+
+	return 0;
+}
+
+static void ath9k_set_wakeup(struct ieee80211_hw *hw, bool enabled)
+{
+	struct ath_softc *sc = hw->priv;
+
+	mutex_lock(&sc->mutex);
+	device_init_wakeup(sc->dev, 1);
+	device_set_wakeup_enable(sc->dev, enabled);
+	mutex_unlock(&sc->mutex);
+}
+
+#endif
+
 struct ieee80211_ops ath9k_ops = {
 	.tx 		    = ath9k_tx,
 	.start 		    = ath9k_start,
@@ -2408,4 +2436,16 @@ struct ieee80211_ops ath9k_ops = {
 	.get_stats	    = ath9k_get_stats,
 	.set_antenna	    = ath9k_set_antenna,
 	.get_antenna	    = ath9k_get_antenna,
+
+#ifdef CONFIG_PM_SLEEP
+	.suspend	    = ath9k_suspend,
+	.resume		    = ath9k_resume,
+	.set_wakeup	    = ath9k_set_wakeup,
+#endif
+
+#ifdef CONFIG_ATH9K_DEBUGFS
+	.get_et_sset_count  = ath9k_get_et_sset_count,
+	.get_et_stats  = ath9k_get_et_stats,
+	.get_et_strings  = ath9k_get_et_strings,
+#endif
 };
diff --git a/drivers/net/wireless/ath/ath9k/mci.c b/drivers/net/wireless/ath/ath9k/mci.c
index 29fe52d6997..fb536e7e661 100644
--- a/drivers/net/wireless/ath/ath9k/mci.c
+++ b/drivers/net/wireless/ath/ath9k/mci.c
@@ -20,7 +20,7 @@
 #include "ath9k.h"
 #include "mci.h"
 
-static const u8 ath_mci_duty_cycle[] = { 0, 50, 60, 70, 80, 85, 90, 95, 98 };
+static const u8 ath_mci_duty_cycle[] = { 55, 50, 60, 70, 80, 85, 90, 95, 98 };
 
 static struct ath_mci_profile_info*
 ath_mci_find_profile(struct ath_mci_profile *mci,
@@ -28,11 +28,14 @@ ath_mci_find_profile(struct ath_mci_profile *mci,
 {
 	struct ath_mci_profile_info *entry;
 
+	if (list_empty(&mci->info))
+		return NULL;
+
 	list_for_each_entry(entry, &mci->info, list) {
 		if (entry->conn_handle == info->conn_handle)
-			break;
+			return entry;
 	}
-	return entry;
+	return NULL;
 }
 
 static bool ath_mci_add_profile(struct ath_common *common,
@@ -49,31 +52,21 @@ static bool ath_mci_add_profile(struct ath_common *common,
 	    (info->type != MCI_GPM_COEX_PROFILE_VOICE))
 		return false;
 
-	entry = ath_mci_find_profile(mci, info);
+	entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
+	if (!entry)
+		return false;
 
-	if (entry) {
-		memcpy(entry, info, 10);
-	} else {
-		entry = kzalloc(sizeof(*entry), GFP_KERNEL);
-		if (!entry)
-			return false;
-
-		memcpy(entry, info, 10);
-		INC_PROF(mci, info);
-		list_add_tail(&info->list, &mci->info);
-	}
+	memcpy(entry, info, 10);
+	INC_PROF(mci, info);
+	list_add_tail(&entry->list, &mci->info);
 
 	return true;
 }
 
 static void ath_mci_del_profile(struct ath_common *common,
 				struct ath_mci_profile *mci,
-				struct ath_mci_profile_info *info)
+				struct ath_mci_profile_info *entry)
 {
-	struct ath_mci_profile_info *entry;
-
-	entry = ath_mci_find_profile(mci, info);
-
 	if (!entry)
 		return;
 
@@ -86,12 +79,16 @@ void ath_mci_flush_profile(struct ath_mci_profile *mci)
 {
 	struct ath_mci_profile_info *info, *tinfo;
 
+	mci->aggr_limit = 0;
+
+	if (list_empty(&mci->info))
+		return;
+
 	list_for_each_entry_safe(info, tinfo, &mci->info, list) {
 		list_del(&info->list);
 		DEC_PROF(mci, info);
 		kfree(info);
 	}
-	mci->aggr_limit = 0;
 }
 
 static void ath_mci_adjust_aggr_limit(struct ath_btcoex *btcoex)
@@ -116,42 +113,60 @@ static void ath_mci_update_scheme(struct ath_softc *sc)
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_btcoex *btcoex = &sc->btcoex;
 	struct ath_mci_profile *mci = &btcoex->mci;
+	struct ath9k_hw_mci *mci_hw = &sc->sc_ah->btcoex_hw.mci;
 	struct ath_mci_profile_info *info;
 	u32 num_profile = NUM_PROF(mci);
 
+	if (mci_hw->config & ATH_MCI_CONFIG_DISABLE_TUNING)
+		goto skip_tuning;
+
+	btcoex->duty_cycle = ath_mci_duty_cycle[num_profile];
+
 	if (num_profile == 1) {
 		info = list_first_entry(&mci->info,
 					struct ath_mci_profile_info,
 					list);
-		if (mci->num_sco && info->T == 12) {
-			mci->aggr_limit = 8;
+		if (mci->num_sco) {
+			if (info->T == 12)
+				mci->aggr_limit = 8;
+			else if (info->T == 6) {
+				mci->aggr_limit = 6;
+				btcoex->duty_cycle = 30;
+			}
 			ath_dbg(common, MCI,
-				"Single SCO, aggregation limit 2 ms\n");
-		} else if ((info->type == MCI_GPM_COEX_PROFILE_BNEP) &&
-			   !info->master) {
-			btcoex->btcoex_period = 60;
+				"Single SCO, aggregation limit %d 1/4 ms\n",
+				mci->aggr_limit);
+		} else if (mci->num_pan || mci->num_other_acl) {
+			/*
+			 * For single PAN/FTP profile, allocate 35% for BT
+			 * to improve WLAN throughput.
+			 */
+			btcoex->duty_cycle = 35;
+			btcoex->btcoex_period = 53;
 			ath_dbg(common, MCI,
-				"Single slave PAN/FTP, bt period 60 ms\n");
-		} else if ((info->type == MCI_GPM_COEX_PROFILE_HID) &&
-			 (info->T > 0 && info->T < 50) &&
-			 (info->A > 1 || info->W > 1)) {
+				"Single PAN/FTP bt period %d ms dutycycle %d\n",
+				btcoex->duty_cycle, btcoex->btcoex_period);
+		} else if (mci->num_hid) {
 			btcoex->duty_cycle = 30;
-			mci->aggr_limit = 8;
+			mci->aggr_limit = 6;
 			ath_dbg(common, MCI,
 				"Multiple attempt/timeout single HID "
-				"aggregation limit 2 ms dutycycle 30%%\n");
+				"aggregation limit 1.5 ms dutycycle 30%%\n");
 		}
-	} else if ((num_profile == 2) && (mci->num_hid == 2)) {
-		btcoex->duty_cycle = 30;
-		mci->aggr_limit = 8;
-		ath_dbg(common, MCI,
-			"Two HIDs aggregation limit 2 ms dutycycle 30%%\n");
-	} else if (num_profile > 3) {
+	} else if (num_profile == 2) {
+		if (mci->num_hid == 2)
+			btcoex->duty_cycle = 30;
 		mci->aggr_limit = 6;
 		ath_dbg(common, MCI,
-			"Three or more profiles aggregation limit 1.5 ms\n");
+			"Two BT profiles aggr limit 1.5 ms dutycycle %d%%\n",
+			btcoex->duty_cycle);
+	} else if (num_profile >= 3) {
+		mci->aggr_limit = 4;
+		ath_dbg(common, MCI,
+			"Three or more profiles aggregation limit 1 ms\n");
 	}
 
+skip_tuning:
 	if (IS_CHAN_2GHZ(sc->sc_ah->curchan)) {
 		if (IS_CHAN_HT(sc->sc_ah->curchan))
 			ath_mci_adjust_aggr_limit(btcoex);
@@ -159,18 +174,17 @@ static void ath_mci_update_scheme(struct ath_softc *sc)
 			btcoex->btcoex_period >>= 1;
 	}
 
-	ath9k_hw_btcoex_disable(sc->sc_ah);
 	ath9k_btcoex_timer_pause(sc);
+	ath9k_hw_btcoex_disable(sc->sc_ah);
 
 	if (IS_CHAN_5GHZ(sc->sc_ah->curchan))
 		return;
 
-	btcoex->duty_cycle += (mci->num_bdr ? ATH_MCI_MAX_DUTY_CYCLE : 0);
+	btcoex->duty_cycle += (mci->num_bdr ? ATH_MCI_BDR_DUTY_CYCLE : 0);
 	if (btcoex->duty_cycle > ATH_MCI_MAX_DUTY_CYCLE)
 		btcoex->duty_cycle = ATH_MCI_MAX_DUTY_CYCLE;
 
-	btcoex->btcoex_period *= 1000;
-	btcoex->btcoex_no_stomp =  btcoex->btcoex_period *
+	btcoex->btcoex_no_stomp =  btcoex->btcoex_period * 1000 *
 		(100 - btcoex->duty_cycle) / 100;
 
 	ath9k_hw_btcoex_enable(sc->sc_ah);
@@ -181,20 +195,16 @@ static void ath_mci_cal_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
 	u32 payload[4] = {0, 0, 0, 0};
 
 	switch (opcode) {
 	case MCI_GPM_BT_CAL_REQ:
-		if (ar9003_mci_state(ah, MCI_STATE_BT, NULL) == MCI_BT_AWAKE) {
-			ar9003_mci_state(ah, MCI_STATE_SET_BT_CAL_START, NULL);
-			ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
-		} else {
-			ath_dbg(common, MCI, "MCI State mismatch: %d\n",
-				ar9003_mci_state(ah, MCI_STATE_BT, NULL));
+		if (mci_hw->bt_state == MCI_BT_AWAKE) {
+			ar9003_mci_state(ah, MCI_STATE_SET_BT_CAL_START);
+			ath9k_queue_reset(sc, RESET_TYPE_MCI);
 		}
-		break;
-	case MCI_GPM_BT_CAL_DONE:
-		ar9003_mci_state(ah, MCI_STATE_BT, NULL);
+		ath_dbg(common, MCI, "MCI State : %d\n", mci_hw->bt_state);
 		break;
 	case MCI_GPM_BT_CAL_GRANT:
 		MCI_GPM_SET_CAL_TYPE(payload, MCI_GPM_WLAN_CAL_DONE);
@@ -207,32 +217,55 @@ static void ath_mci_cal_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
 	}
 }
 
+static void ath9k_mci_work(struct work_struct *work)
+{
+	struct ath_softc *sc = container_of(work, struct ath_softc, mci_work);
+
+	ath_mci_update_scheme(sc);
+}
+
 static void ath_mci_process_profile(struct ath_softc *sc,
 				    struct ath_mci_profile_info *info)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_btcoex *btcoex = &sc->btcoex;
 	struct ath_mci_profile *mci = &btcoex->mci;
+	struct ath_mci_profile_info *entry = NULL;
+
+	entry = ath_mci_find_profile(mci, info);
+	if (entry) {
+		/*
+		 * Two MCI interrupts are generated while connecting to
+		 * headset and A2DP profile, but only one MCI interrupt
+		 * is generated with last added profile type while disconnecting
+		 * both profiles.
+		 * So while adding second profile type decrement
+		 * the first one.
+		 */
+		if (entry->type != info->type) {
+			DEC_PROF(mci, entry);
+			INC_PROF(mci, info);
+		}
+		memcpy(entry, info, 10);
+	}
 
 	if (info->start) {
-		if (!ath_mci_add_profile(common, mci, info))
+		if (!entry && !ath_mci_add_profile(common, mci, info))
 			return;
 	} else
-		ath_mci_del_profile(common, mci, info);
+		ath_mci_del_profile(common, mci, entry);
 
 	btcoex->btcoex_period = ATH_MCI_DEF_BT_PERIOD;
 	mci->aggr_limit = mci->num_sco ? 6 : 0;
 
-	if (NUM_PROF(mci)) {
+	btcoex->duty_cycle = ath_mci_duty_cycle[NUM_PROF(mci)];
+	if (NUM_PROF(mci))
 		btcoex->bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
-		btcoex->duty_cycle = ath_mci_duty_cycle[NUM_PROF(mci)];
-	} else {
+	else
 		btcoex->bt_stomp_type = mci->num_mgmt ? ATH_BTCOEX_STOMP_ALL :
 							ATH_BTCOEX_STOMP_LOW;
-		btcoex->duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
-	}
 
-	ath_mci_update_scheme(sc);
+	ieee80211_queue_work(sc->hw, &sc->mci_work);
 }
 
 static void ath_mci_process_status(struct ath_softc *sc,
@@ -247,8 +280,6 @@ static void ath_mci_process_status(struct ath_softc *sc,
 	if (status->is_link)
 		return;
 
-	memset(&info, 0, sizeof(struct ath_mci_profile_info));
-
 	info.conn_handle = status->conn_handle;
 	if (ath_mci_find_profile(mci, &info))
 		return;
@@ -268,7 +299,7 @@ static void ath_mci_process_status(struct ath_softc *sc,
 	} while (++i < ATH_MCI_MAX_PROFILE);
 
 	if (old_num_mgmt != mci->num_mgmt)
-		ath_mci_update_scheme(sc);
+		ieee80211_queue_work(sc->hw, &sc->mci_work);
 }
 
 static void ath_mci_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
@@ -277,25 +308,20 @@ static void ath_mci_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
 	struct ath_mci_profile_info profile_info;
 	struct ath_mci_profile_status profile_status;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	u32 version;
-	u8 major;
-	u8 minor;
+	u8 major, minor;
 	u32 seq_num;
 
 	switch (opcode) {
 	case MCI_GPM_COEX_VERSION_QUERY:
-		version = ar9003_mci_state(ah, MCI_STATE_SEND_WLAN_COEX_VERSION,
-					   NULL);
+		ar9003_mci_state(ah, MCI_STATE_SEND_WLAN_COEX_VERSION);
 		break;
 	case MCI_GPM_COEX_VERSION_RESPONSE:
 		major = *(rx_payload + MCI_GPM_COEX_B_MAJOR_VERSION);
 		minor = *(rx_payload + MCI_GPM_COEX_B_MINOR_VERSION);
-		version = (major << 8) + minor;
-		version = ar9003_mci_state(ah, MCI_STATE_SET_BT_COEX_VERSION,
-					   &version);
+		ar9003_mci_set_bt_version(ah, major, minor);
 		break;
 	case MCI_GPM_COEX_STATUS_QUERY:
-		ar9003_mci_state(ah, MCI_STATE_SEND_WLAN_CHANNELS, NULL);
+		ar9003_mci_send_wlan_channels(ah);
 		break;
 	case MCI_GPM_COEX_BT_PROFILE_INFO:
 		memcpy(&profile_info,
@@ -322,7 +348,7 @@ static void ath_mci_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
 
 		seq_num = *((u32 *)(rx_payload + 12));
 		ath_dbg(common, MCI,
-			"BT_Status_Update: is_link=%d, linkId=%d, state=%d, SEQ=%d\n",
+			"BT_Status_Update: is_link=%d, linkId=%d, state=%d, SEQ=%u\n",
 			profile_status.is_link, profile_status.conn_handle,
 			profile_status.is_critical, seq_num);
 
@@ -362,6 +388,7 @@ int ath_mci_setup(struct ath_softc *sc)
 			 mci->gpm_buf.bf_addr, (mci->gpm_buf.bf_len >> 4),
 			 mci->sched_buf.bf_paddr);
 
+	INIT_WORK(&sc->mci_work, ath9k_mci_work);
 	ath_dbg(common, MCI, "MCI Initialized\n");
 
 	return 0;
@@ -389,6 +416,7 @@ void ath_mci_intr(struct ath_softc *sc)
 	struct ath_mci_coex *mci = &sc->mci_coex;
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
 	u32 mci_int, mci_int_rxmsg;
 	u32 offset, subtype, opcode;
 	u32 *pgpm;
@@ -397,8 +425,8 @@ void ath_mci_intr(struct ath_softc *sc)
 
 	ar9003_mci_get_interrupt(sc->sc_ah, &mci_int, &mci_int_rxmsg);
 
-	if (ar9003_mci_state(ah, MCI_STATE_ENABLE, NULL) == 0) {
-		ar9003_mci_state(ah, MCI_STATE_INIT_GPM_OFFSET, NULL);
+	if (ar9003_mci_state(ah, MCI_STATE_ENABLE) == 0) {
+		ar9003_mci_get_next_gpm_offset(ah, true, NULL);
 		return;
 	}
 
@@ -417,46 +445,41 @@ void ath_mci_intr(struct ath_softc *sc)
 					NULL, 0, true, false);
 
 		mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE;
-		ar9003_mci_state(ah, MCI_STATE_RESET_REQ_WAKE, NULL);
+		ar9003_mci_state(ah, MCI_STATE_RESET_REQ_WAKE);
 
 		/*
 		 * always do this for recovery and 2G/5G toggling and LNA_TRANS
 		 */
-		ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE, NULL);
+		ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE);
 	}
 
 	if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING) {
 		mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING;
 
-		if (ar9003_mci_state(ah, MCI_STATE_BT, NULL) == MCI_BT_SLEEP) {
-			if (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP, NULL) !=
-			    MCI_BT_SLEEP)
-				ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE,
-						 NULL);
-		}
+		if ((mci_hw->bt_state == MCI_BT_SLEEP) &&
+		    (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP) !=
+		     MCI_BT_SLEEP))
+			ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE);
 	}
 
 	if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING) {
 		mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING;
 
-		if (ar9003_mci_state(ah, MCI_STATE_BT, NULL) == MCI_BT_AWAKE) {
-			if (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP, NULL) !=
-			    MCI_BT_AWAKE)
-				ar9003_mci_state(ah, MCI_STATE_SET_BT_SLEEP,
-						 NULL);
-		}
+		if ((mci_hw->bt_state == MCI_BT_AWAKE) &&
+		    (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP) !=
+		     MCI_BT_AWAKE))
+			mci_hw->bt_state = MCI_BT_SLEEP;
 	}
 
 	if ((mci_int & AR_MCI_INTERRUPT_RX_INVALID_HDR) ||
 	    (mci_int & AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT)) {
-		ar9003_mci_state(ah, MCI_STATE_RECOVER_RX, NULL);
+		ar9003_mci_state(ah, MCI_STATE_RECOVER_RX);
 		skip_gpm = true;
 	}
 
 	if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO) {
 		mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO;
-		offset = ar9003_mci_state(ah, MCI_STATE_LAST_SCHD_MSG_OFFSET,
-					  NULL);
+		offset = ar9003_mci_state(ah, MCI_STATE_LAST_SCHD_MSG_OFFSET);
 	}
 
 	if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_GPM) {
@@ -465,8 +488,8 @@ void ath_mci_intr(struct ath_softc *sc)
 		while (more_data == MCI_GPM_MORE) {
 
 			pgpm = mci->gpm_buf.bf_addr;
-			offset = ar9003_mci_state(ah, MCI_STATE_NEXT_GPM_OFFSET,
-						  &more_data);
+			offset = ar9003_mci_get_next_gpm_offset(ah, false,
+								&more_data);
 
 			if (offset == MCI_GPM_INVALID)
 				break;
@@ -507,23 +530,17 @@ void ath_mci_intr(struct ath_softc *sc)
 			mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_LNA_INFO;
 
 		if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_CONT_INFO) {
-			int value_dbm = ar9003_mci_state(ah,
-						 MCI_STATE_CONT_RSSI_POWER, NULL);
+			int value_dbm = MS(mci_hw->cont_status,
+					   AR_MCI_CONT_RSSI_POWER);
 
 			mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_CONT_INFO;
 
-			if (ar9003_mci_state(ah, MCI_STATE_CONT_TXRX, NULL))
-				ath_dbg(common, MCI,
-					"MCI CONT_INFO: (tx) pri = %d, pwr = %d dBm\n",
-					ar9003_mci_state(ah,
-						 MCI_STATE_CONT_PRIORITY, NULL),
-					value_dbm);
-			else
-				ath_dbg(common, MCI,
-					"MCI CONT_INFO: (rx) pri = %d,pwr = %d dBm\n",
-					ar9003_mci_state(ah,
-						 MCI_STATE_CONT_PRIORITY, NULL),
-					value_dbm);
+			ath_dbg(common, MCI,
+				"MCI CONT_INFO: (%s) pri = %d pwr = %d dBm\n",
+				MS(mci_hw->cont_status, AR_MCI_CONT_TXRX) ?
+				"tx" : "rx",
+				MS(mci_hw->cont_status, AR_MCI_CONT_PRIORITY),
+				value_dbm);
 		}
 
 		if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_CONT_NACK)
@@ -538,3 +555,14 @@ void ath_mci_intr(struct ath_softc *sc)
 		mci_int &= ~(AR_MCI_INTERRUPT_RX_INVALID_HDR |
 			     AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT);
 }
+
+void ath_mci_enable(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	if (!common->btcoex_enabled)
+		return;
+
+	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
+		sc->sc_ah->imask |= ATH9K_INT_MCI;
+}
diff --git a/drivers/net/wireless/ath/ath9k/mci.h b/drivers/net/wireless/ath/ath9k/mci.h
index c841444f53c..fc14eea034e 100644
--- a/drivers/net/wireless/ath/ath9k/mci.h
+++ b/drivers/net/wireless/ath/ath9k/mci.h
@@ -130,4 +130,13 @@ void ath_mci_flush_profile(struct ath_mci_profile *mci);
 int ath_mci_setup(struct ath_softc *sc);
 void ath_mci_cleanup(struct ath_softc *sc);
 void ath_mci_intr(struct ath_softc *sc);
-#endif
+
+#ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+void ath_mci_enable(struct ath_softc *sc);
+#else
+static inline void ath_mci_enable(struct ath_softc *sc)
+{
+}
+#endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
+
+#endif /* MCI_H*/
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index a856b51255f..87b89d55e63 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -115,6 +115,9 @@ static void ath_pci_aspm_init(struct ath_common *common)
 	int pos;
 	u8 aspm;
 
+	if (!ah->is_pciexpress)
+		return;
+
 	pos = pci_pcie_cap(pdev);
 	if (!pos)
 		return;
@@ -138,6 +141,7 @@ static void ath_pci_aspm_init(struct ath_common *common)
 		aspm &= ~(PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
 		pci_write_config_byte(parent, pos + PCI_EXP_LNKCTL, aspm);
 
+		ath_info(common, "Disabling ASPM since BTCOEX is enabled\n");
 		return;
 	}
 
@@ -147,6 +151,7 @@ static void ath_pci_aspm_init(struct ath_common *common)
 		ah->aspm_enabled = true;
 		/* Initialize PCIe PM and SERDES registers. */
 		ath9k_hw_configpcipowersave(ah, false);
+		ath_info(common, "ASPM enabled: 0x%x\n", aspm);
 	}
 }
 
@@ -246,7 +251,7 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	sc->mem = mem;
 
 	/* Will be cleared in ath9k_start() */
-	sc->sc_flags |= SC_OP_INVALID;
+	set_bit(SC_OP_INVALID, &sc->sc_flags);
 
 	ret = request_irq(pdev->irq, ath_isr, IRQF_SHARED, "ath9k", sc);
 	if (ret) {
@@ -308,6 +313,9 @@ static int ath_pci_suspend(struct device *device)
 	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
 	struct ath_softc *sc = hw->priv;
 
+	if (sc->wow_enabled)
+		return 0;
+
 	/* The device has to be moved to FULLSLEEP forcibly.
 	 * Otherwise the chip never moved to full sleep,
 	 * when no interface is up.
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
index 92a6c0a87f8..e034add9cd5 100644
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ b/drivers/net/wireless/ath/ath9k/rc.c
@@ -770,7 +770,7 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
 	struct ieee80211_tx_rate *rates = tx_info->control.rates;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	__le16 fc = hdr->frame_control;
-	u8 try_per_rate, i = 0, rix, high_rix;
+	u8 try_per_rate, i = 0, rix;
 	int is_probe = 0;
 
 	if (rate_control_send_low(sta, priv_sta, txrc))
@@ -791,7 +791,6 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
 	rate_table = ath_rc_priv->rate_table;
 	rix = ath_rc_get_highest_rix(sc, ath_rc_priv, rate_table,
 				     &is_probe, false);
-	high_rix = rix;
 
 	/*
 	 * If we're in HT mode and both us and our peer supports LDPC.
@@ -839,16 +838,16 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
 	try_per_rate = 8;
 
 	/*
-	 * Use a legacy rate as last retry to ensure that the frame
-	 * is tried in both MCS and legacy rates.
+	 * If the last rate in the rate series is MCS and has
+	 * more than 80% of per thresh, then use a legacy rate
+	 * as last retry to ensure that the frame is tried in both
+	 * MCS and legacy rate.
 	 */
-	if ((rates[2].flags & IEEE80211_TX_RC_MCS) &&
-	    (!(tx_info->flags & IEEE80211_TX_CTL_AMPDU) ||
-	    (ath_rc_priv->per[high_rix] > 45)))
+	ath_rc_get_lower_rix(rate_table, ath_rc_priv, rix, &rix);
+	if (WLAN_RC_PHY_HT(rate_table->info[rix].phy) &&
+	    (ath_rc_priv->per[rix] > 45))
 		rix = ath_rc_get_highest_rix(sc, ath_rc_priv, rate_table,
 				&is_probe, true);
-	else
-		ath_rc_get_lower_rix(rate_table, ath_rc_priv, rix, &rix);
 
 	/* All other rates in the series have RTS enabled */
 	ath_rc_rate_set_series(rate_table, &rates[i], txrc,
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 0735aeb3b26..12aca02228c 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -20,43 +20,6 @@
 
 #define SKB_CB_ATHBUF(__skb)	(*((struct ath_buf **)__skb->cb))
 
-static inline bool ath_is_alt_ant_ratio_better(int alt_ratio, int maxdelta,
-					       int mindelta, int main_rssi_avg,
-					       int alt_rssi_avg, int pkt_count)
-{
-	return (((alt_ratio >= ATH_ANT_DIV_COMB_ALT_ANT_RATIO2) &&
-		(alt_rssi_avg > main_rssi_avg + maxdelta)) ||
-		(alt_rssi_avg > main_rssi_avg + mindelta)) && (pkt_count > 50);
-}
-
-static inline bool ath_ant_div_comb_alt_check(u8 div_group, int alt_ratio,
-					int curr_main_set, int curr_alt_set,
-					int alt_rssi_avg, int main_rssi_avg)
-{
-	bool result = false;
-	switch (div_group) {
-	case 0:
-		if (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO)
-			result = true;
-		break;
-	case 1:
-	case 2:
-		if ((((curr_main_set == ATH_ANT_DIV_COMB_LNA2) &&
-			(curr_alt_set == ATH_ANT_DIV_COMB_LNA1) &&
-				(alt_rssi_avg >= (main_rssi_avg - 5))) ||
-			((curr_main_set == ATH_ANT_DIV_COMB_LNA1) &&
-			(curr_alt_set == ATH_ANT_DIV_COMB_LNA2) &&
-				(alt_rssi_avg >= (main_rssi_avg - 2)))) &&
-							(alt_rssi_avg >= 4))
-			result = true;
-		else
-			result = false;
-		break;
-	}
-
-	return result;
-}
-
 static inline bool ath9k_check_auto_sleep(struct ath_softc *sc)
 {
 	return sc->ps_enabled &&
@@ -303,7 +266,7 @@ static void ath_edma_start_recv(struct ath_softc *sc)
 
 	ath_opmode_init(sc);
 
-	ath9k_hw_startpcureceive(sc->sc_ah, (sc->sc_flags & SC_OP_OFFCHANNEL));
+	ath9k_hw_startpcureceive(sc->sc_ah, !!(sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL));
 
 	spin_unlock_bh(&sc->rx.rxbuflock);
 }
@@ -322,8 +285,8 @@ int ath_rx_init(struct ath_softc *sc, int nbufs)
 	int error = 0;
 
 	spin_lock_init(&sc->sc_pcu_lock);
-	sc->sc_flags &= ~SC_OP_RXFLUSH;
 	spin_lock_init(&sc->rx.rxbuflock);
+	clear_bit(SC_OP_RXFLUSH, &sc->sc_flags);
 
 	common->rx_bufsize = IEEE80211_MAX_MPDU_LEN / 2 +
 			     sc->sc_ah->caps.rx_status_len;
@@ -467,6 +430,9 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 		rfilt |= ATH9K_RX_FILTER_MCAST_BCAST_ALL;
 	}
 
+	if (AR_SREV_9550(sc->sc_ah))
+		rfilt |= ATH9K_RX_FILTER_4ADDRESS;
+
 	return rfilt;
 
 }
@@ -500,7 +466,7 @@ int ath_startrecv(struct ath_softc *sc)
 
 start_recv:
 	ath_opmode_init(sc);
-	ath9k_hw_startpcureceive(ah, (sc->sc_flags & SC_OP_OFFCHANNEL));
+	ath9k_hw_startpcureceive(ah, !!(sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL));
 
 	spin_unlock_bh(&sc->rx.rxbuflock);
 
@@ -535,11 +501,11 @@ bool ath_stoprecv(struct ath_softc *sc)
 
 void ath_flushrecv(struct ath_softc *sc)
 {
-	sc->sc_flags |= SC_OP_RXFLUSH;
+	set_bit(SC_OP_RXFLUSH, &sc->sc_flags);
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_EDMA)
 		ath_rx_tasklet(sc, 1, true);
 	ath_rx_tasklet(sc, 1, false);
-	sc->sc_flags &= ~SC_OP_RXFLUSH;
+	clear_bit(SC_OP_RXFLUSH, &sc->sc_flags);
 }
 
 static bool ath_beacon_dtim_pending_cab(struct sk_buff *skb)
@@ -587,7 +553,7 @@ static void ath_rx_ps_beacon(struct ath_softc *sc, struct sk_buff *skb)
 		sc->ps_flags &= ~PS_BEACON_SYNC;
 		ath_dbg(common, PS,
 			"Reconfigure Beacon timers based on timestamp from the AP\n");
-		ath_set_beacon(sc);
+		ath9k_set_beacon(sc);
 	}
 
 	if (ath_beacon_dtim_pending_cab(skb)) {
@@ -624,13 +590,13 @@ static void ath_rx_ps(struct ath_softc *sc, struct sk_buff *skb, bool mybeacon)
 
 	/* Process Beacon and CAB receive in PS state */
 	if (((sc->ps_flags & PS_WAIT_FOR_BEACON) || ath9k_check_auto_sleep(sc))
-	    && mybeacon)
+	    && mybeacon) {
 		ath_rx_ps_beacon(sc, skb);
-	else if ((sc->ps_flags & PS_WAIT_FOR_CAB) &&
-		 (ieee80211_is_data(hdr->frame_control) ||
-		  ieee80211_is_action(hdr->frame_control)) &&
-		 is_multicast_ether_addr(hdr->addr1) &&
-		 !ieee80211_has_moredata(hdr->frame_control)) {
+	} else if ((sc->ps_flags & PS_WAIT_FOR_CAB) &&
+		   (ieee80211_is_data(hdr->frame_control) ||
+		    ieee80211_is_action(hdr->frame_control)) &&
+		   is_multicast_ether_addr(hdr->addr1) &&
+		   !ieee80211_has_moredata(hdr->frame_control)) {
 		/*
 		 * No more broadcast/multicast frames to be received at this
 		 * point.
@@ -1068,709 +1034,6 @@ static void ath9k_rx_skb_postprocess(struct ath_common *common,
 		rxs->flag &= ~RX_FLAG_DECRYPTED;
 }
 
-static void ath_lnaconf_alt_good_scan(struct ath_ant_comb *antcomb,
-				      struct ath_hw_antcomb_conf ant_conf,
-				      int main_rssi_avg)
-{
-	antcomb->quick_scan_cnt = 0;
-
-	if (ant_conf.main_lna_conf == ATH_ANT_DIV_COMB_LNA2)
-		antcomb->rssi_lna2 = main_rssi_avg;
-	else if (ant_conf.main_lna_conf == ATH_ANT_DIV_COMB_LNA1)
-		antcomb->rssi_lna1 = main_rssi_avg;
-
-	switch ((ant_conf.main_lna_conf << 4) | ant_conf.alt_lna_conf) {
-	case 0x10: /* LNA2 A-B */
-		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-		antcomb->first_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA1;
-		break;
-	case 0x20: /* LNA1 A-B */
-		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-		antcomb->first_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA2;
-		break;
-	case 0x21: /* LNA1 LNA2 */
-		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA2;
-		antcomb->first_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-		antcomb->second_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-		break;
-	case 0x12: /* LNA2 LNA1 */
-		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1;
-		antcomb->first_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-		antcomb->second_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-		break;
-	case 0x13: /* LNA2 A+B */
-		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-		antcomb->first_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA1;
-		break;
-	case 0x23: /* LNA1 A+B */
-		antcomb->main_conf = ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-		antcomb->first_quick_scan_conf =
-			ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-		antcomb->second_quick_scan_conf = ATH_ANT_DIV_COMB_LNA2;
-		break;
-	default:
-		break;
-	}
-}
-
-static void ath_select_ant_div_from_quick_scan(struct ath_ant_comb *antcomb,
-				struct ath_hw_antcomb_conf *div_ant_conf,
-				int main_rssi_avg, int alt_rssi_avg,
-				int alt_ratio)
-{
-	/* alt_good */
-	switch (antcomb->quick_scan_cnt) {
-	case 0:
-		/* set alt to main, and alt to first conf */
-		div_ant_conf->main_lna_conf = antcomb->main_conf;
-		div_ant_conf->alt_lna_conf = antcomb->first_quick_scan_conf;
-		break;
-	case 1:
-		/* set alt to main, and alt to first conf */
-		div_ant_conf->main_lna_conf = antcomb->main_conf;
-		div_ant_conf->alt_lna_conf = antcomb->second_quick_scan_conf;
-		antcomb->rssi_first = main_rssi_avg;
-		antcomb->rssi_second = alt_rssi_avg;
-
-		if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1) {
-			/* main is LNA1 */
-			if (ath_is_alt_ant_ratio_better(alt_ratio,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_HI,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
-						main_rssi_avg, alt_rssi_avg,
-						antcomb->total_pkt_count))
-				antcomb->first_ratio = true;
-			else
-				antcomb->first_ratio = false;
-		} else if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2) {
-			if (ath_is_alt_ant_ratio_better(alt_ratio,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_MID,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
-						main_rssi_avg, alt_rssi_avg,
-						antcomb->total_pkt_count))
-				antcomb->first_ratio = true;
-			else
-				antcomb->first_ratio = false;
-		} else {
-			if ((((alt_ratio >= ATH_ANT_DIV_COMB_ALT_ANT_RATIO2) &&
-			    (alt_rssi_avg > main_rssi_avg +
-			    ATH_ANT_DIV_COMB_LNA1_DELTA_HI)) ||
-			    (alt_rssi_avg > main_rssi_avg)) &&
-			    (antcomb->total_pkt_count > 50))
-				antcomb->first_ratio = true;
-			else
-				antcomb->first_ratio = false;
-		}
-		break;
-	case 2:
-		antcomb->alt_good = false;
-		antcomb->scan_not_start = false;
-		antcomb->scan = false;
-		antcomb->rssi_first = main_rssi_avg;
-		antcomb->rssi_third = alt_rssi_avg;
-
-		if (antcomb->second_quick_scan_conf == ATH_ANT_DIV_COMB_LNA1)
-			antcomb->rssi_lna1 = alt_rssi_avg;
-		else if (antcomb->second_quick_scan_conf ==
-			 ATH_ANT_DIV_COMB_LNA2)
-			antcomb->rssi_lna2 = alt_rssi_avg;
-		else if (antcomb->second_quick_scan_conf ==
-			 ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2) {
-			if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2)
-				antcomb->rssi_lna2 = main_rssi_avg;
-			else if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1)
-				antcomb->rssi_lna1 = main_rssi_avg;
-		}
-
-		if (antcomb->rssi_lna2 > antcomb->rssi_lna1 +
-		    ATH_ANT_DIV_COMB_LNA1_LNA2_SWITCH_DELTA)
-			div_ant_conf->main_lna_conf = ATH_ANT_DIV_COMB_LNA2;
-		else
-			div_ant_conf->main_lna_conf = ATH_ANT_DIV_COMB_LNA1;
-
-		if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1) {
-			if (ath_is_alt_ant_ratio_better(alt_ratio,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_HI,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
-						main_rssi_avg, alt_rssi_avg,
-						antcomb->total_pkt_count))
-				antcomb->second_ratio = true;
-			else
-				antcomb->second_ratio = false;
-		} else if (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2) {
-			if (ath_is_alt_ant_ratio_better(alt_ratio,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_MID,
-						ATH_ANT_DIV_COMB_LNA1_DELTA_LOW,
-						main_rssi_avg, alt_rssi_avg,
-						antcomb->total_pkt_count))
-				antcomb->second_ratio = true;
-			else
-				antcomb->second_ratio = false;
-		} else {
-			if ((((alt_ratio >= ATH_ANT_DIV_COMB_ALT_ANT_RATIO2) &&
-			    (alt_rssi_avg > main_rssi_avg +
-			    ATH_ANT_DIV_COMB_LNA1_DELTA_HI)) ||
-			    (alt_rssi_avg > main_rssi_avg)) &&
-			    (antcomb->total_pkt_count > 50))
-				antcomb->second_ratio = true;
-			else
-				antcomb->second_ratio = false;
-		}
-
-		/* set alt to the conf with maximun ratio */
-		if (antcomb->first_ratio && antcomb->second_ratio) {
-			if (antcomb->rssi_second > antcomb->rssi_third) {
-				/* first alt*/
-				if ((antcomb->first_quick_scan_conf ==
-				    ATH_ANT_DIV_COMB_LNA1) ||
-				    (antcomb->first_quick_scan_conf ==
-				    ATH_ANT_DIV_COMB_LNA2))
-					/* Set alt LNA1 or LNA2*/
-					if (div_ant_conf->main_lna_conf ==
-					    ATH_ANT_DIV_COMB_LNA2)
-						div_ant_conf->alt_lna_conf =
-							ATH_ANT_DIV_COMB_LNA1;
-					else
-						div_ant_conf->alt_lna_conf =
-							ATH_ANT_DIV_COMB_LNA2;
-				else
-					/* Set alt to A+B or A-B */
-					div_ant_conf->alt_lna_conf =
-						antcomb->first_quick_scan_conf;
-			} else if ((antcomb->second_quick_scan_conf ==
-				   ATH_ANT_DIV_COMB_LNA1) ||
-				   (antcomb->second_quick_scan_conf ==
-				   ATH_ANT_DIV_COMB_LNA2)) {
-				/* Set alt LNA1 or LNA2 */
-				if (div_ant_conf->main_lna_conf ==
-				    ATH_ANT_DIV_COMB_LNA2)
-					div_ant_conf->alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-				else
-					div_ant_conf->alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-			} else {
-				/* Set alt to A+B or A-B */
-				div_ant_conf->alt_lna_conf =
-					antcomb->second_quick_scan_conf;
-			}
-		} else if (antcomb->first_ratio) {
-			/* first alt */
-			if ((antcomb->first_quick_scan_conf ==
-			    ATH_ANT_DIV_COMB_LNA1) ||
-			    (antcomb->first_quick_scan_conf ==
-			    ATH_ANT_DIV_COMB_LNA2))
-					/* Set alt LNA1 or LNA2 */
-				if (div_ant_conf->main_lna_conf ==
-				    ATH_ANT_DIV_COMB_LNA2)
-					div_ant_conf->alt_lna_conf =
-							ATH_ANT_DIV_COMB_LNA1;
-				else
-					div_ant_conf->alt_lna_conf =
-							ATH_ANT_DIV_COMB_LNA2;
-			else
-				/* Set alt to A+B or A-B */
-				div_ant_conf->alt_lna_conf =
-						antcomb->first_quick_scan_conf;
-		} else if (antcomb->second_ratio) {
-				/* second alt */
-			if ((antcomb->second_quick_scan_conf ==
-			    ATH_ANT_DIV_COMB_LNA1) ||
-			    (antcomb->second_quick_scan_conf ==
-			    ATH_ANT_DIV_COMB_LNA2))
-				/* Set alt LNA1 or LNA2 */
-				if (div_ant_conf->main_lna_conf ==
-				    ATH_ANT_DIV_COMB_LNA2)
-					div_ant_conf->alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-				else
-					div_ant_conf->alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-			else
-				/* Set alt to A+B or A-B */
-				div_ant_conf->alt_lna_conf =
-						antcomb->second_quick_scan_conf;
-		} else {
-			/* main is largest */
-			if ((antcomb->main_conf == ATH_ANT_DIV_COMB_LNA1) ||
-			    (antcomb->main_conf == ATH_ANT_DIV_COMB_LNA2))
-				/* Set alt LNA1 or LNA2 */
-				if (div_ant_conf->main_lna_conf ==
-				    ATH_ANT_DIV_COMB_LNA2)
-					div_ant_conf->alt_lna_conf =
-							ATH_ANT_DIV_COMB_LNA1;
-				else
-					div_ant_conf->alt_lna_conf =
-							ATH_ANT_DIV_COMB_LNA2;
-			else
-				/* Set alt to A+B or A-B */
-				div_ant_conf->alt_lna_conf = antcomb->main_conf;
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static void ath_ant_div_conf_fast_divbias(struct ath_hw_antcomb_conf *ant_conf,
-		struct ath_ant_comb *antcomb, int alt_ratio)
-{
-	if (ant_conf->div_group == 0) {
-		/* Adjust the fast_div_bias based on main and alt lna conf */
-		switch ((ant_conf->main_lna_conf << 4) |
-				ant_conf->alt_lna_conf) {
-		case 0x01: /* A-B LNA2 */
-			ant_conf->fast_div_bias = 0x3b;
-			break;
-		case 0x02: /* A-B LNA1 */
-			ant_conf->fast_div_bias = 0x3d;
-			break;
-		case 0x03: /* A-B A+B */
-			ant_conf->fast_div_bias = 0x1;
-			break;
-		case 0x10: /* LNA2 A-B */
-			ant_conf->fast_div_bias = 0x7;
-			break;
-		case 0x12: /* LNA2 LNA1 */
-			ant_conf->fast_div_bias = 0x2;
-			break;
-		case 0x13: /* LNA2 A+B */
-			ant_conf->fast_div_bias = 0x7;
-			break;
-		case 0x20: /* LNA1 A-B */
-			ant_conf->fast_div_bias = 0x6;
-			break;
-		case 0x21: /* LNA1 LNA2 */
-			ant_conf->fast_div_bias = 0x0;
-			break;
-		case 0x23: /* LNA1 A+B */
-			ant_conf->fast_div_bias = 0x6;
-			break;
-		case 0x30: /* A+B A-B */
-			ant_conf->fast_div_bias = 0x1;
-			break;
-		case 0x31: /* A+B LNA2 */
-			ant_conf->fast_div_bias = 0x3b;
-			break;
-		case 0x32: /* A+B LNA1 */
-			ant_conf->fast_div_bias = 0x3d;
-			break;
-		default:
-			break;
-		}
-	} else if (ant_conf->div_group == 1) {
-		/* Adjust the fast_div_bias based on main and alt_lna_conf */
-		switch ((ant_conf->main_lna_conf << 4) |
-			ant_conf->alt_lna_conf) {
-		case 0x01: /* A-B LNA2 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x02: /* A-B LNA1 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x03: /* A-B A+B */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x10: /* LNA2 A-B */
-			if (!(antcomb->scan) &&
-			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x3f;
-			else
-				ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x12: /* LNA2 LNA1 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x13: /* LNA2 A+B */
-			if (!(antcomb->scan) &&
-			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x3f;
-			else
-				ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x20: /* LNA1 A-B */
-			if (!(antcomb->scan) &&
-			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x3f;
-			else
-				ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x21: /* LNA1 LNA2 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x23: /* LNA1 A+B */
-			if (!(antcomb->scan) &&
-			    (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x3f;
-			else
-				ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x30: /* A+B A-B */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x31: /* A+B LNA2 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x32: /* A+B LNA1 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		default:
-			break;
-		}
-	} else if (ant_conf->div_group == 2) {
-		/* Adjust the fast_div_bias based on main and alt_lna_conf */
-		switch ((ant_conf->main_lna_conf << 4) |
-				ant_conf->alt_lna_conf) {
-		case 0x01: /* A-B LNA2 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x02: /* A-B LNA1 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x03: /* A-B A+B */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x10: /* LNA2 A-B */
-			if (!(antcomb->scan) &&
-				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x1;
-			else
-				ant_conf->fast_div_bias = 0x2;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x12: /* LNA2 LNA1 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x13: /* LNA2 A+B */
-			if (!(antcomb->scan) &&
-				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x1;
-			else
-				ant_conf->fast_div_bias = 0x2;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x20: /* LNA1 A-B */
-			if (!(antcomb->scan) &&
-				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x1;
-			else
-				ant_conf->fast_div_bias = 0x2;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x21: /* LNA1 LNA2 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x23: /* LNA1 A+B */
-			if (!(antcomb->scan) &&
-				(alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO))
-				ant_conf->fast_div_bias = 0x1;
-			else
-				ant_conf->fast_div_bias = 0x2;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x30: /* A+B A-B */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x31: /* A+B LNA2 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		case 0x32: /* A+B LNA1 */
-			ant_conf->fast_div_bias = 0x1;
-			ant_conf->main_gaintb = 0;
-			ant_conf->alt_gaintb = 0;
-			break;
-		default:
-			break;
-		}
-	}
-}
-
-/* Antenna diversity and combining */
-static void ath_ant_comb_scan(struct ath_softc *sc, struct ath_rx_status *rs)
-{
-	struct ath_hw_antcomb_conf div_ant_conf;
-	struct ath_ant_comb *antcomb = &sc->ant_comb;
-	int alt_ratio = 0, alt_rssi_avg = 0, main_rssi_avg = 0, curr_alt_set;
-	int curr_main_set;
-	int main_rssi = rs->rs_rssi_ctl0;
-	int alt_rssi = rs->rs_rssi_ctl1;
-	int rx_ant_conf,  main_ant_conf;
-	bool short_scan = false;
-
-	rx_ant_conf = (rs->rs_rssi_ctl2 >> ATH_ANT_RX_CURRENT_SHIFT) &
-		       ATH_ANT_RX_MASK;
-	main_ant_conf = (rs->rs_rssi_ctl2 >> ATH_ANT_RX_MAIN_SHIFT) &
-			 ATH_ANT_RX_MASK;
-
-	/* Record packet only when both main_rssi and  alt_rssi is positive */
-	if (main_rssi > 0 && alt_rssi > 0) {
-		antcomb->total_pkt_count++;
-		antcomb->main_total_rssi += main_rssi;
-		antcomb->alt_total_rssi  += alt_rssi;
-		if (main_ant_conf == rx_ant_conf)
-			antcomb->main_recv_cnt++;
-		else
-			antcomb->alt_recv_cnt++;
-	}
-
-	/* Short scan check */
-	if (antcomb->scan && antcomb->alt_good) {
-		if (time_after(jiffies, antcomb->scan_start_time +
-		    msecs_to_jiffies(ATH_ANT_DIV_COMB_SHORT_SCAN_INTR)))
-			short_scan = true;
-		else
-			if (antcomb->total_pkt_count ==
-			    ATH_ANT_DIV_COMB_SHORT_SCAN_PKTCOUNT) {
-				alt_ratio = ((antcomb->alt_recv_cnt * 100) /
-					    antcomb->total_pkt_count);
-				if (alt_ratio < ATH_ANT_DIV_COMB_ALT_ANT_RATIO)
-					short_scan = true;
-			}
-	}
-
-	if (((antcomb->total_pkt_count < ATH_ANT_DIV_COMB_MAX_PKTCOUNT) ||
-	    rs->rs_moreaggr) && !short_scan)
-		return;
-
-	if (antcomb->total_pkt_count) {
-		alt_ratio = ((antcomb->alt_recv_cnt * 100) /
-			     antcomb->total_pkt_count);
-		main_rssi_avg = (antcomb->main_total_rssi /
-				 antcomb->total_pkt_count);
-		alt_rssi_avg = (antcomb->alt_total_rssi /
-				 antcomb->total_pkt_count);
-	}
-
-
-	ath9k_hw_antdiv_comb_conf_get(sc->sc_ah, &div_ant_conf);
-	curr_alt_set = div_ant_conf.alt_lna_conf;
-	curr_main_set = div_ant_conf.main_lna_conf;
-
-	antcomb->count++;
-
-	if (antcomb->count == ATH_ANT_DIV_COMB_MAX_COUNT) {
-		if (alt_ratio > ATH_ANT_DIV_COMB_ALT_ANT_RATIO) {
-			ath_lnaconf_alt_good_scan(antcomb, div_ant_conf,
-						  main_rssi_avg);
-			antcomb->alt_good = true;
-		} else {
-			antcomb->alt_good = false;
-		}
-
-		antcomb->count = 0;
-		antcomb->scan = true;
-		antcomb->scan_not_start = true;
-	}
-
-	if (!antcomb->scan) {
-		if (ath_ant_div_comb_alt_check(div_ant_conf.div_group,
-					alt_ratio, curr_main_set, curr_alt_set,
-					alt_rssi_avg, main_rssi_avg)) {
-			if (curr_alt_set == ATH_ANT_DIV_COMB_LNA2) {
-				/* Switch main and alt LNA */
-				div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-				div_ant_conf.alt_lna_conf  =
-						ATH_ANT_DIV_COMB_LNA1;
-			} else if (curr_alt_set == ATH_ANT_DIV_COMB_LNA1) {
-				div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-				div_ant_conf.alt_lna_conf  =
-						ATH_ANT_DIV_COMB_LNA2;
-			}
-
-			goto div_comb_done;
-		} else if ((curr_alt_set != ATH_ANT_DIV_COMB_LNA1) &&
-			   (curr_alt_set != ATH_ANT_DIV_COMB_LNA2)) {
-			/* Set alt to another LNA */
-			if (curr_main_set == ATH_ANT_DIV_COMB_LNA2)
-				div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-			else if (curr_main_set == ATH_ANT_DIV_COMB_LNA1)
-				div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-
-			goto div_comb_done;
-		}
-
-		if ((alt_rssi_avg < (main_rssi_avg +
-						div_ant_conf.lna1_lna2_delta)))
-			goto div_comb_done;
-	}
-
-	if (!antcomb->scan_not_start) {
-		switch (curr_alt_set) {
-		case ATH_ANT_DIV_COMB_LNA2:
-			antcomb->rssi_lna2 = alt_rssi_avg;
-			antcomb->rssi_lna1 = main_rssi_avg;
-			antcomb->scan = true;
-			/* set to A+B */
-			div_ant_conf.main_lna_conf =
-				ATH_ANT_DIV_COMB_LNA1;
-			div_ant_conf.alt_lna_conf  =
-				ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-			break;
-		case ATH_ANT_DIV_COMB_LNA1:
-			antcomb->rssi_lna1 = alt_rssi_avg;
-			antcomb->rssi_lna2 = main_rssi_avg;
-			antcomb->scan = true;
-			/* set to A+B */
-			div_ant_conf.main_lna_conf = ATH_ANT_DIV_COMB_LNA2;
-			div_ant_conf.alt_lna_conf  =
-				ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-			break;
-		case ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2:
-			antcomb->rssi_add = alt_rssi_avg;
-			antcomb->scan = true;
-			/* set to A-B */
-			div_ant_conf.alt_lna_conf =
-				ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-			break;
-		case ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2:
-			antcomb->rssi_sub = alt_rssi_avg;
-			antcomb->scan = false;
-			if (antcomb->rssi_lna2 >
-			    (antcomb->rssi_lna1 +
-			    ATH_ANT_DIV_COMB_LNA1_LNA2_SWITCH_DELTA)) {
-				/* use LNA2 as main LNA */
-				if ((antcomb->rssi_add > antcomb->rssi_lna1) &&
-				    (antcomb->rssi_add > antcomb->rssi_sub)) {
-					/* set to A+B */
-					div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-					div_ant_conf.alt_lna_conf  =
-						ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-				} else if (antcomb->rssi_sub >
-					   antcomb->rssi_lna1) {
-					/* set to A-B */
-					div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-					div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-				} else {
-					/* set to LNA1 */
-					div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-					div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-				}
-			} else {
-				/* use LNA1 as main LNA */
-				if ((antcomb->rssi_add > antcomb->rssi_lna2) &&
-				    (antcomb->rssi_add > antcomb->rssi_sub)) {
-					/* set to A+B */
-					div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-					div_ant_conf.alt_lna_conf  =
-						ATH_ANT_DIV_COMB_LNA1_PLUS_LNA2;
-				} else if (antcomb->rssi_sub >
-					   antcomb->rssi_lna1) {
-					/* set to A-B */
-					div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-					div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1_MINUS_LNA2;
-				} else {
-					/* set to LNA2 */
-					div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-					div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-				}
-			}
-			break;
-		default:
-			break;
-		}
-	} else {
-		if (!antcomb->alt_good) {
-			antcomb->scan_not_start = false;
-			/* Set alt to another LNA */
-			if (curr_main_set == ATH_ANT_DIV_COMB_LNA2) {
-				div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-				div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-			} else if (curr_main_set == ATH_ANT_DIV_COMB_LNA1) {
-				div_ant_conf.main_lna_conf =
-						ATH_ANT_DIV_COMB_LNA1;
-				div_ant_conf.alt_lna_conf =
-						ATH_ANT_DIV_COMB_LNA2;
-			}
-			goto div_comb_done;
-		}
-	}
-
-	ath_select_ant_div_from_quick_scan(antcomb, &div_ant_conf,
-					   main_rssi_avg, alt_rssi_avg,
-					   alt_ratio);
-
-	antcomb->quick_scan_cnt++;
-
-div_comb_done:
-	ath_ant_div_conf_fast_divbias(&div_ant_conf, antcomb, alt_ratio);
-	ath9k_hw_antdiv_comb_conf_set(sc->sc_ah, &div_ant_conf);
-
-	antcomb->scan_start_time = jiffies;
-	antcomb->total_pkt_count = 0;
-	antcomb->main_total_rssi = 0;
-	antcomb->alt_total_rssi = 0;
-	antcomb->main_recv_cnt = 0;
-	antcomb->alt_recv_cnt = 0;
-}
-
 int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 {
 	struct ath_buf *bf;
@@ -1804,7 +1067,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 
 	do {
 		/* If handling rx interrupt and flush is in progress => exit */
-		if ((sc->sc_flags & SC_OP_RXFLUSH) && (flush == 0))
+		if (test_bit(SC_OP_RXFLUSH, &sc->sc_flags) && (flush == 0))
 			break;
 
 		memset(&rs, 0, sizeof(rs));
@@ -1842,13 +1105,14 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		else
 			rs.is_mybeacon = false;
 
+		sc->rx.num_pkts++;
 		ath_debug_stat_rx(sc, &rs);
 
 		/*
 		 * If we're asked to flush receive queue, directly
 		 * chain it back at the queue without processing it.
 		 */
-		if (sc->sc_flags & SC_OP_RXFLUSH) {
+		if (test_bit(SC_OP_RXFLUSH, &sc->sc_flags)) {
 			RX_STAT_INC(rx_drop_rxflush);
 			goto requeue_drop_frag;
 		}
@@ -1969,7 +1233,6 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 			skb_trim(skb, skb->len - 8);
 
 		spin_lock_irqsave(&sc->sc_pm_lock, flags);
-
 		if ((sc->ps_flags & (PS_WAIT_FOR_BEACON |
 				     PS_WAIT_FOR_CAB |
 				     PS_WAIT_FOR_PSPOLL_DATA)) ||
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index 458f81b4a7c..87cac8eb783 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -696,9 +696,12 @@
 #define AR_WA_BIT7			(1 << 7)
 #define AR_WA_BIT23			(1 << 23)
 #define AR_WA_D3_L1_DISABLE		(1 << 14)
+#define AR_WA_UNTIE_RESET_EN		(1 << 15) /* Enable PCI Reset
+						     to POR (power-on-reset) */
 #define AR_WA_D3_TO_L1_DISABLE_REAL     (1 << 16)
 #define AR_WA_ASPM_TIMER_BASED_DISABLE  (1 << 17)
-#define AR_WA_RESET_EN                  (1 << 18) /* Sw Control to enable PCI-Reset to POR (bit 15) */
+#define AR_WA_RESET_EN                  (1 << 18) /* Enable PCI-Reset to
+						     POR (bit 15) */
 #define AR_WA_ANALOG_SHIFT              (1 << 20)
 #define AR_WA_POR_SHORT                 (1 << 21) /* PCI-E Phy reset control */
 #define AR_WA_BIT22			(1 << 22)
@@ -798,6 +801,7 @@
 #define AR_SREV_REVISION_9580_10	4 /* AR9580 1.0 */
 #define AR_SREV_VERSION_9462		0x280
 #define AR_SREV_REVISION_9462_20	2
+#define AR_SREV_VERSION_9550		0x400
 
 #define AR_SREV_5416(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_5416_PCI) || \
@@ -905,6 +909,9 @@
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9462) && \
 	((_ah)->hw_version.macRev >= AR_SREV_REVISION_9462_20))
 
+#define AR_SREV_9550(_ah) \
+	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9550))
+
 #define AR_SREV_9580(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9580) && \
 	((_ah)->hw_version.macRev >= AR_SREV_REVISION_9580_10))
@@ -1028,6 +1035,8 @@ enum {
 #define AR_PCIE_PM_CTRL                          (AR_SREV_9340(ah) ? 0x4004 : 0x4014)
 #define AR_PCIE_PM_CTRL_ENA                      0x00080000
 
+#define AR_PCIE_PHY_REG3			 0x18c08
+
 #define AR_NUM_GPIO                              14
 #define AR928X_NUM_GPIO                          10
 #define AR9285_NUM_GPIO                          12
@@ -1231,6 +1240,8 @@ enum {
 #define AR_RTC_PLL_CLKSEL       0x00000300
 #define AR_RTC_PLL_CLKSEL_S     8
 #define AR_RTC_PLL_BYPASS	0x00010000
+#define AR_RTC_PLL_NOPWD	0x00040000
+#define AR_RTC_PLL_NOPWD_S	18
 
 #define PLL3 0x16188
 #define PLL3_DO_MEAS_MASK 0x40000000
@@ -1643,11 +1654,11 @@ enum {
 
 #define AR_TPC                 0x80e8
 #define AR_TPC_ACK             0x0000003f
-#define AR_TPC_ACK_S           0x00
+#define AR_TPC_ACK_S           0
 #define AR_TPC_CTS             0x00003f00
-#define AR_TPC_CTS_S           0x08
+#define AR_TPC_CTS_S           8
 #define AR_TPC_CHIRP           0x003f0000
-#define AR_TPC_CHIRP_S         0x16
+#define AR_TPC_CHIRP_S         16
 
 #define AR_QUIET1          0x80fc
 #define AR_QUIET1_NEXT_QUIET_S         0
@@ -1883,6 +1894,8 @@ enum {
 #define AR_PCU_MISC_MODE2_HWWAR2                       0x02000000
 #define AR_PCU_MISC_MODE2_RESERVED2                    0xFFFE0000
 
+#define AR_PCU_MISC_MODE3			       0x83d0
+
 #define AR_MAC_PCU_ASYNC_FIFO_REG3			0x8358
 #define AR_MAC_PCU_ASYNC_FIFO_REG3_DATAPATH_SEL		0x00000400
 #define AR_MAC_PCU_ASYNC_FIFO_REG3_SOFT_RESET		0x80000000
@@ -1905,6 +1918,140 @@ enum {
 #define AR_RATE_DURATION_32     0x8780
 #define AR_RATE_DURATION(_n)    (AR_RATE_DURATION_0 + ((_n)<<2))
 
+/* WoW - Wake On Wireless */
+
+#define AR_PMCTRL_AUX_PWR_DET		0x10000000 /* Puts Chip in L2 state */
+#define AR_PMCTRL_D3COLD_VAUX		0x00800000
+#define AR_PMCTRL_HOST_PME_EN		0x00400000 /* Send OOB WAKE_L on WoW
+						      event */
+#define AR_PMCTRL_WOW_PME_CLR		0x00200000 /* Clear WoW event */
+#define AR_PMCTRL_PWR_STATE_MASK	0x0f000000 /* Power State Mask */
+#define AR_PMCTRL_PWR_STATE_D1D3	0x0f000000 /* Activate D1 and D3 */
+#define AR_PMCTRL_PWR_STATE_D1D3_REAL	0x0f000000 /* Activate D1 and D3 */
+#define AR_PMCTRL_PWR_STATE_D0		0x08000000 /* Activate D0 */
+#define AR_PMCTRL_PWR_PM_CTRL_ENA	0x00008000 /* Enable power mgmt */
+
+#define AR_WOW_BEACON_TIMO_MAX		0xffffffff
+
+/*
+ * MAC WoW Registers
+ */
+
+#define AR_WOW_PATTERN			0x825C
+#define AR_WOW_COUNT			0x8260
+#define AR_WOW_BCN_EN			0x8270
+#define AR_WOW_BCN_TIMO			0x8274
+#define AR_WOW_KEEP_ALIVE_TIMO		0x8278
+#define AR_WOW_KEEP_ALIVE		0x827c
+#define AR_WOW_US_SCALAR		0x8284
+#define AR_WOW_KEEP_ALIVE_DELAY		0x8288
+#define AR_WOW_PATTERN_MATCH		0x828c
+#define AR_WOW_PATTERN_OFF1		0x8290	/* pattern bytes 0 -> 3 */
+#define AR_WOW_PATTERN_OFF2		0x8294	/* pattern bytes 4 -> 7 */
+
+/* for AR9285 or later version of chips */
+#define AR_WOW_EXACT			0x829c
+#define AR_WOW_LENGTH1			0x8360
+#define AR_WOW_LENGTH2			0X8364
+/* register to enable match for less than 256 bytes packets */
+#define AR_WOW_PATTERN_MATCH_LT_256B	0x8368
+
+#define AR_SW_WOW_CONTROL		0x20018
+#define AR_SW_WOW_ENABLE		0x1
+#define AR_SWITCH_TO_REFCLK		0x2
+#define AR_RESET_CONTROL		0x4
+#define AR_RESET_VALUE_MASK		0x8
+#define AR_HW_WOW_DISABLE		0x10
+#define AR_CLR_MAC_INTERRUPT		0x20
+#define AR_CLR_KA_INTERRUPT		0x40
+
+/* AR_WOW_PATTERN register values */
+#define AR_WOW_BACK_OFF_SHIFT(x)	((x & 0xf) << 28) /* in usecs */
+#define AR_WOW_MAC_INTR_EN		0x00040000
+#define AR_WOW_MAGIC_EN			0x00010000
+#define AR_WOW_PATTERN_EN(x)		(x & 0xff)
+#define AR_WOW_PAT_FOUND_SHIFT	8
+#define AR_WOW_PATTERN_FOUND(x)		(x & (0xff << AR_WOW_PAT_FOUND_SHIFT))
+#define AR_WOW_PATTERN_FOUND_MASK	((0xff) << AR_WOW_PAT_FOUND_SHIFT)
+#define AR_WOW_MAGIC_PAT_FOUND		0x00020000
+#define AR_WOW_MAC_INTR			0x00080000
+#define AR_WOW_KEEP_ALIVE_FAIL		0x00100000
+#define AR_WOW_BEACON_FAIL		0x00200000
+
+#define AR_WOW_STATUS(x)		(x & (AR_WOW_PATTERN_FOUND_MASK | \
+					      AR_WOW_MAGIC_PAT_FOUND	| \
+					      AR_WOW_KEEP_ALIVE_FAIL	| \
+					      AR_WOW_BEACON_FAIL))
+#define AR_WOW_CLEAR_EVENTS(x)		(x & ~(AR_WOW_PATTERN_EN(0xff) | \
+					       AR_WOW_MAGIC_EN | \
+					       AR_WOW_MAC_INTR_EN | \
+					       AR_WOW_BEACON_FAIL | \
+					       AR_WOW_KEEP_ALIVE_FAIL))
+
+/* AR_WOW_COUNT register values */
+#define AR_WOW_AIFS_CNT(x)		(x & 0xff)
+#define AR_WOW_SLOT_CNT(x)		((x & 0xff) << 8)
+#define AR_WOW_KEEP_ALIVE_CNT(x)	((x & 0xff) << 16)
+
+/* AR_WOW_BCN_EN register */
+#define AR_WOW_BEACON_FAIL_EN		0x00000001
+
+/* AR_WOW_BCN_TIMO rgister */
+#define AR_WOW_BEACON_TIMO		0x40000000 /* valid if BCN_EN is set */
+
+/* AR_WOW_KEEP_ALIVE_TIMO register */
+#define AR_WOW_KEEP_ALIVE_TIMO_VALUE
+#define AR_WOW_KEEP_ALIVE_NEVER		0xffffffff
+
+/* AR_WOW_KEEP_ALIVE register  */
+#define AR_WOW_KEEP_ALIVE_AUTO_DIS	0x00000001
+#define AR_WOW_KEEP_ALIVE_FAIL_DIS	0x00000002
+
+/* AR_WOW_KEEP_ALIVE_DELAY register */
+#define AR_WOW_KEEP_ALIVE_DELAY_VALUE	0x000003e8 /* 1 msec */
+
+
+/*
+ * keep it long for beacon workaround - ensure no false alarm
+ */
+#define AR_WOW_BMISSTHRESHOLD		0x20
+
+/* AR_WOW_PATTERN_MATCH register */
+#define AR_WOW_PAT_END_OF_PKT(x)	(x & 0xf)
+#define AR_WOW_PAT_OFF_MATCH(x)		((x & 0xf) << 8)
+
+/*
+ * default values for Wow Configuration for backoff, aifs, slot, keep-alive
+ * to be programmed into various registers.
+ */
+#define AR_WOW_PAT_BACKOFF	0x00000004 /* AR_WOW_PATTERN_REG */
+#define AR_WOW_CNT_AIFS_CNT	0x00000022 /* AR_WOW_COUNT_REG */
+#define AR_WOW_CNT_SLOT_CNT	0x00000009 /* AR_WOW_COUNT_REG */
+/*
+ * Keepalive count applicable for AR9280 2.0 and above.
+ */
+#define AR_WOW_CNT_KA_CNT 0x00000008    /* AR_WOW_COUNT register */
+
+/* WoW - Transmit buffer for keep alive frames */
+#define AR_WOW_TRANSMIT_BUFFER	0xe000 /* E000 - EFFC */
+
+#define AR_WOW_TXBUF(i)		(AR_WOW_TRANSMIT_BUFFER + ((i) << 2))
+
+#define AR_WOW_KA_DESC_WORD2	0xe000
+
+#define AR_WOW_KA_DATA_WORD0	0xe030
+
+/* WoW Transmit Buffer for patterns */
+#define AR_WOW_TB_PATTERN(i)	(0xe100 + (i << 8))
+#define AR_WOW_TB_MASK(i)	(0xec00 + (i << 5))
+
+/* Currently Pattern 0-7 are supported - so bit 0-7 are set */
+#define AR_WOW_PATTERN_SUPPORTED	0xff
+#define AR_WOW_LENGTH_MAX		0xff
+#define AR_WOW_LEN1_SHIFT(_i)	((0x3 - ((_i) & 0x3)) << 0x3)
+#define AR_WOW_LENGTH1_MASK(_i)	(AR_WOW_LENGTH_MAX << AR_WOW_LEN1_SHIFT(_i))
+#define AR_WOW_LEN2_SHIFT(_i)	((0x7 - ((_i) & 0x7)) << 0x3)
+#define AR_WOW_LENGTH2_MASK(_i)	(AR_WOW_LENGTH_MAX << AR_WOW_LEN2_SHIFT(_i))
 
 #define AR9271_CORE_CLOCK	117   /* clock to 117Mhz */
 #define AR9271_TARGET_BAUD_RATE	19200 /* 115200 */
@@ -2077,12 +2224,6 @@ enum {
 					 AR_MCI_INTERRUPT_RX_MSG_REMOTE_RESET| \
 					 AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING  | \
 					 AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING| \
-					 AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO   | \
-					 AR_MCI_INTERRUPT_RX_MSG_LNA_CONTROL | \
-					 AR_MCI_INTERRUPT_RX_MSG_LNA_INFO    | \
-					 AR_MCI_INTERRUPT_RX_MSG_CONT_NACK   | \
-					 AR_MCI_INTERRUPT_RX_MSG_CONT_INFO   | \
-					 AR_MCI_INTERRUPT_RX_MSG_CONT_RST    | \
 					 AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE)
 
 #define AR_MCI_CPU_INT					0x1840
@@ -2098,8 +2239,8 @@ enum {
 #define AR_MCI_CONT_STATUS			0x1848
 #define AR_MCI_CONT_RSSI_POWER			0x000000FF
 #define AR_MCI_CONT_RSSI_POWER_S		0
-#define AR_MCI_CONT_RRIORITY			0x0000FF00
-#define AR_MCI_CONT_RRIORITY_S			8
+#define AR_MCI_CONT_PRIORITY			0x0000FF00
+#define AR_MCI_CONT_PRIORITY_S			8
 #define AR_MCI_CONT_TXRX			0x00010000
 #define AR_MCI_CONT_TXRX_S			16
 
@@ -2162,10 +2303,6 @@ enum {
 #define AR_BTCOEX_CTRL_SPDT_POLARITY			0x80000000
 #define AR_BTCOEX_CTRL_SPDT_POLARITY_S			31
 
-#define AR_BTCOEX_WL_WEIGHTS0				0x18b0
-#define AR_BTCOEX_WL_WEIGHTS1				0x18b4
-#define AR_BTCOEX_WL_WEIGHTS2				0x18b8
-#define AR_BTCOEX_WL_WEIGHTS3				0x18bc
 #define AR_BTCOEX_MAX_TXPWR(_x)				(0x18c0 + ((_x) << 2))
 #define AR_BTCOEX_WL_LNA				0x1940
 #define AR_BTCOEX_RFGAIN_CTRL				0x1944
@@ -2211,5 +2348,7 @@ enum {
 #define AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT	0x00000fff
 #define AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT_S	0
 
+#define AR_GLB_SWREG_DISCONT_MODE         0x2002c
+#define AR_GLB_SWREG_DISCONT_EN_BT_WLAN   0x3
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/wow.c b/drivers/net/wireless/ath/ath9k/wow.c
new file mode 100644
index 00000000000..44a08eb53c6
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/wow.c
@@ -0,0 +1,532 @@
+/*
+ * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/export.h>
+#include "ath9k.h"
+#include "reg.h"
+#include "hw-ops.h"
+
+const char *ath9k_hw_wow_event_to_string(u32 wow_event)
+{
+	if (wow_event & AH_WOW_MAGIC_PATTERN_EN)
+		return "Magic pattern";
+	if (wow_event & AH_WOW_USER_PATTERN_EN)
+		return "User pattern";
+	if (wow_event & AH_WOW_LINK_CHANGE)
+		return "Link change";
+	if (wow_event & AH_WOW_BEACON_MISS)
+		return "Beacon miss";
+
+	return  "unknown reason";
+}
+EXPORT_SYMBOL(ath9k_hw_wow_event_to_string);
+
+static void ath9k_hw_config_serdes_wow_sleep(struct ath_hw *ah)
+{
+	int i;
+
+	for (i = 0; i < ah->iniPcieSerdesWow.ia_rows; i++)
+		REG_WRITE(ah, INI_RA(&ah->iniPcieSerdesWow, i, 0),
+			  INI_RA(&ah->iniPcieSerdesWow, i, 1));
+
+	usleep_range(1000, 1500);
+}
+
+static void ath9k_hw_set_powermode_wow_sleep(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	REG_SET_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
+
+	/* set rx disable bit */
+	REG_WRITE(ah, AR_CR, AR_CR_RXD);
+
+	if (!ath9k_hw_wait(ah, AR_CR, AR_CR_RXE, 0, AH_WAIT_TIMEOUT)) {
+		ath_err(common, "Failed to stop Rx DMA in 10ms AR_CR=0x%08x AR_DIAG_SW=0x%08x\n",
+			REG_READ(ah, AR_CR), REG_READ(ah, AR_DIAG_SW));
+		return;
+	} else {
+		if (!AR_SREV_9300_20_OR_LATER(ah))
+			REG_WRITE(ah, AR_RXDP, 0x0);
+	}
+
+	/* AR9280 WoW has sleep issue, do not set it to sleep */
+	if (AR_SREV_9280_20(ah))
+		return;
+
+	REG_WRITE(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_ON_INT);
+}
+
+static void ath9k_wow_create_keep_alive_pattern(struct ath_hw *ah)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+	u8 sta_mac_addr[ETH_ALEN], ap_mac_addr[ETH_ALEN];
+	u32 ctl[13] = {0};
+	u32 data_word[KAL_NUM_DATA_WORDS];
+	u8 i;
+	u32 wow_ka_data_word0;
+
+	memcpy(sta_mac_addr, common->macaddr, ETH_ALEN);
+	memcpy(ap_mac_addr, common->curbssid, ETH_ALEN);
+
+	/* set the transmit buffer */
+	ctl[0] = (KAL_FRAME_LEN | (MAX_RATE_POWER << 16));
+
+	if (!(AR_SREV_9300_20_OR_LATER(ah)))
+		ctl[0] += (KAL_ANTENNA_MODE << 25);
+
+	ctl[1] = 0;
+	ctl[3] = 0xb;	/* OFDM_6M hardware value for this rate */
+	ctl[4] = 0;
+	ctl[7] = (ah->txchainmask) << 2;
+
+	if (AR_SREV_9300_20_OR_LATER(ah))
+		ctl[2] = 0xf << 16; /* tx_tries 0 */
+	else
+		ctl[2] = 0x7 << 16; /* tx_tries 0 */
+
+
+	for (i = 0; i < KAL_NUM_DESC_WORDS; i++)
+		REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + i * 4), ctl[i]);
+
+	/* for AR9300 family 13 descriptor words */
+	if (AR_SREV_9300_20_OR_LATER(ah))
+		REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + i * 4), ctl[i]);
+
+	data_word[0] = (KAL_FRAME_TYPE << 2) | (KAL_FRAME_SUB_TYPE << 4) |
+		       (KAL_TO_DS << 8) | (KAL_DURATION_ID << 16);
+	data_word[1] = (ap_mac_addr[3] << 24) | (ap_mac_addr[2] << 16) |
+		       (ap_mac_addr[1] << 8) | (ap_mac_addr[0]);
+	data_word[2] = (sta_mac_addr[1] << 24) | (sta_mac_addr[0] << 16) |
+		       (ap_mac_addr[5] << 8) | (ap_mac_addr[4]);
+	data_word[3] = (sta_mac_addr[5] << 24) | (sta_mac_addr[4] << 16) |
+		       (sta_mac_addr[3] << 8) | (sta_mac_addr[2]);
+	data_word[4] = (ap_mac_addr[3] << 24) | (ap_mac_addr[2] << 16) |
+		       (ap_mac_addr[1] << 8) | (ap_mac_addr[0]);
+	data_word[5] = (ap_mac_addr[5] << 8) | (ap_mac_addr[4]);
+
+	if (AR_SREV_9462_20_OR_LATER(ah)) {
+		/* AR9462 2.0 has an extra descriptor word (time based
+		 * discard) compared to other chips */
+		REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + (12 * 4)), 0);
+		wow_ka_data_word0 = AR_WOW_TXBUF(13);
+	} else {
+		wow_ka_data_word0 = AR_WOW_TXBUF(12);
+	}
+
+	for (i = 0; i < KAL_NUM_DATA_WORDS; i++)
+		REG_WRITE(ah, (wow_ka_data_word0 + i*4), data_word[i]);
+
+}
+
+void ath9k_hw_wow_apply_pattern(struct ath_hw *ah, u8 *user_pattern,
+				u8 *user_mask, int pattern_count,
+				int pattern_len)
+{
+	int i;
+	u32 pattern_val, mask_val;
+	u32 set, clr;
+
+	/* FIXME: should check count by querying the hardware capability */
+	if (pattern_count >= MAX_NUM_PATTERN)
+		return;
+
+	REG_SET_BIT(ah, AR_WOW_PATTERN, BIT(pattern_count));
+
+	/* set the registers for pattern */
+	for (i = 0; i < MAX_PATTERN_SIZE; i += 4) {
+		memcpy(&pattern_val, user_pattern, 4);
+		REG_WRITE(ah, (AR_WOW_TB_PATTERN(pattern_count) + i),
+			  pattern_val);
+		user_pattern += 4;
+	}
+
+	/* set the registers for mask */
+	for (i = 0; i < MAX_PATTERN_MASK_SIZE; i += 4) {
+		memcpy(&mask_val, user_mask, 4);
+		REG_WRITE(ah, (AR_WOW_TB_MASK(pattern_count) + i), mask_val);
+		user_mask += 4;
+	}
+
+	/* set the pattern length to be matched
+	 *
+	 * AR_WOW_LENGTH1_REG1
+	 * bit 31:24 pattern 0 length
+	 * bit 23:16 pattern 1 length
+	 * bit 15:8 pattern 2 length
+	 * bit 7:0 pattern 3 length
+	 *
+	 * AR_WOW_LENGTH1_REG2
+	 * bit 31:24 pattern 4 length
+	 * bit 23:16 pattern 5 length
+	 * bit 15:8 pattern 6 length
+	 * bit 7:0 pattern 7 length
+	 *
+	 * the below logic writes out the new
+	 * pattern length for the corresponding
+	 * pattern_count, while masking out the
+	 * other fields
+	 */
+
+	ah->wow_event_mask |= BIT(pattern_count + AR_WOW_PAT_FOUND_SHIFT);
+
+	if (!AR_SREV_9285_12_OR_LATER(ah))
+		return;
+
+	if (pattern_count < 4) {
+		/* Pattern 0-3 uses AR_WOW_LENGTH1 register */
+		set = (pattern_len & AR_WOW_LENGTH_MAX) <<
+		       AR_WOW_LEN1_SHIFT(pattern_count);
+		clr = AR_WOW_LENGTH1_MASK(pattern_count);
+		REG_RMW(ah, AR_WOW_LENGTH1, set, clr);
+	} else {
+		/* Pattern 4-7 uses AR_WOW_LENGTH2 register */
+		set = (pattern_len & AR_WOW_LENGTH_MAX) <<
+		       AR_WOW_LEN2_SHIFT(pattern_count);
+		clr = AR_WOW_LENGTH2_MASK(pattern_count);
+		REG_RMW(ah, AR_WOW_LENGTH2, set, clr);
+	}
+
+}
+EXPORT_SYMBOL(ath9k_hw_wow_apply_pattern);
+
+u32 ath9k_hw_wow_wakeup(struct ath_hw *ah)
+{
+	u32 wow_status = 0;
+	u32 val = 0, rval;
+	/*
+	 * read the WoW status register to know
+	 * the wakeup reason
+	 */
+	rval = REG_READ(ah, AR_WOW_PATTERN);
+	val = AR_WOW_STATUS(rval);
+
+	/*
+	 * mask only the WoW events that we have enabled. Sometimes
+	 * we have spurious WoW events from the AR_WOW_PATTERN
+	 * register. This mask will clean it up.
+	 */
+
+	val &= ah->wow_event_mask;
+
+	if (val) {
+
+		if (val & AR_WOW_MAGIC_PAT_FOUND)
+			wow_status |= AH_WOW_MAGIC_PATTERN_EN;
+
+		if (AR_WOW_PATTERN_FOUND(val))
+			wow_status |= AH_WOW_USER_PATTERN_EN;
+
+		if (val & AR_WOW_KEEP_ALIVE_FAIL)
+			wow_status |= AH_WOW_LINK_CHANGE;
+
+		if (val & AR_WOW_BEACON_FAIL)
+			wow_status |= AH_WOW_BEACON_MISS;
+
+	}
+
+	/*
+	 * set and clear WOW_PME_CLEAR registers for the chip to
+	 * generate next wow signal.
+	 * disable D3 before accessing other registers ?
+	 */
+
+	/* do we need to check the bit value 0x01000000 (7-10) ?? */
+	REG_RMW(ah, AR_PCIE_PM_CTRL, AR_PMCTRL_WOW_PME_CLR,
+		AR_PMCTRL_PWR_STATE_D1D3);
+
+	/*
+	 * clear all events
+	 */
+	REG_WRITE(ah, AR_WOW_PATTERN,
+		  AR_WOW_CLEAR_EVENTS(REG_READ(ah, AR_WOW_PATTERN)));
+
+	/*
+	 * tie reset register for AR9002 family of chipsets
+	 * NB: not tieing it back might have some repurcussions.
+	 */
+
+	if (!AR_SREV_9300_20_OR_LATER(ah)) {
+		REG_SET_BIT(ah, AR_WA, AR_WA_UNTIE_RESET_EN |
+			    AR_WA_POR_SHORT | AR_WA_RESET_EN);
+	}
+
+
+	/*
+	 * restore the beacon threshold to init value
+	 */
+	REG_WRITE(ah, AR_RSSI_THR, INIT_RSSI_THR);
+
+	/*
+	 * Restore the way the PCI-E reset, Power-On-Reset, external
+	 * PCIE_POR_SHORT pins are tied to its original value.
+	 * Previously just before WoW sleep, we untie the PCI-E
+	 * reset to our Chip's Power On Reset so that any PCI-E
+	 * reset from the bus will not reset our chip
+	 */
+
+	if (AR_SREV_9280_20_OR_LATER(ah) && ah->is_pciexpress)
+		ath9k_hw_configpcipowersave(ah, false);
+
+	ah->wow_event_mask = 0;
+
+	return wow_status;
+}
+EXPORT_SYMBOL(ath9k_hw_wow_wakeup);
+
+void ath9k_hw_wow_enable(struct ath_hw *ah, u32 pattern_enable)
+{
+	u32 wow_event_mask;
+	u32 set, clr;
+
+	/*
+	 * wow_event_mask is a mask to the AR_WOW_PATTERN register to
+	 * indicate which WoW events we have enabled. The WoW events
+	 * are from the 'pattern_enable' in this function and
+	 * 'pattern_count' of ath9k_hw_wow_apply_pattern()
+	 */
+
+	wow_event_mask = ah->wow_event_mask;
+
+	/*
+	 * Untie Power-on-Reset from the PCI-E-Reset. When we are in
+	 * WOW sleep, we do want the Reset from the PCI-E to disturb
+	 * our hw state
+	 */
+
+	if (ah->is_pciexpress) {
+
+		/*
+		 * we need to untie the internal POR (power-on-reset)
+		 * to the external PCI-E reset. We also need to tie
+		 * the PCI-E Phy reset to the PCI-E reset.
+		 */
+
+		if (AR_SREV_9300_20_OR_LATER(ah)) {
+			set = AR_WA_RESET_EN | AR_WA_POR_SHORT;
+			clr = AR_WA_UNTIE_RESET_EN | AR_WA_D3_L1_DISABLE;
+			REG_RMW(ah, AR_WA, set, clr);
+		} else {
+			if (AR_SREV_9285(ah) || AR_SREV_9287(ah))
+				set = AR9285_WA_DEFAULT;
+			else
+				set = AR9280_WA_DEFAULT;
+
+			/*
+			 * In AR9280 and AR9285, bit 14 in WA register
+			 * (disable L1) should only be set when device
+			 * enters D3 state and be cleared when device
+			 * comes back to D0
+			 */
+
+			if (ah->config.pcie_waen & AR_WA_D3_L1_DISABLE)
+				set |= AR_WA_D3_L1_DISABLE;
+
+			clr = AR_WA_UNTIE_RESET_EN;
+			set |= AR_WA_RESET_EN | AR_WA_POR_SHORT;
+			REG_RMW(ah, AR_WA, set, clr);
+
+			/*
+			 * for WoW sleep, we reprogram the SerDes so that the
+			 * PLL and CLK REQ are both enabled. This uses more
+			 * power but otherwise WoW sleep is unstable and the
+			 * chip may disappear.
+			 */
+
+			if (AR_SREV_9285_12_OR_LATER(ah))
+				ath9k_hw_config_serdes_wow_sleep(ah);
+
+		}
+	}
+
+	/*
+	 * set the power states appropriately and enable PME
+	 */
+	set = AR_PMCTRL_HOST_PME_EN | AR_PMCTRL_PWR_PM_CTRL_ENA |
+	      AR_PMCTRL_AUX_PWR_DET | AR_PMCTRL_WOW_PME_CLR;
+
+	/*
+	 * set and clear WOW_PME_CLEAR registers for the chip
+	 * to generate next wow signal.
+	 */
+	REG_SET_BIT(ah, AR_PCIE_PM_CTRL, set);
+	clr = AR_PMCTRL_WOW_PME_CLR;
+	REG_CLR_BIT(ah, AR_PCIE_PM_CTRL, clr);
+
+	/*
+	 * Setup for:
+	 *	- beacon misses
+	 *	- magic pattern
+	 *	- keep alive timeout
+	 *	- pattern matching
+	 */
+
+	/*
+	 * Program default values for pattern backoff, aifs/slot/KAL count,
+	 * beacon miss timeout, KAL timeout, etc.
+	 */
+
+	set = AR_WOW_BACK_OFF_SHIFT(AR_WOW_PAT_BACKOFF);
+	REG_SET_BIT(ah, AR_WOW_PATTERN, set);
+
+	set = AR_WOW_AIFS_CNT(AR_WOW_CNT_AIFS_CNT) |
+	      AR_WOW_SLOT_CNT(AR_WOW_CNT_SLOT_CNT) |
+	      AR_WOW_KEEP_ALIVE_CNT(AR_WOW_CNT_KA_CNT);
+	REG_SET_BIT(ah, AR_WOW_COUNT, set);
+
+	if (pattern_enable & AH_WOW_BEACON_MISS)
+		set = AR_WOW_BEACON_TIMO;
+	/* We are not using beacon miss, program a large value */
+	else
+		set = AR_WOW_BEACON_TIMO_MAX;
+
+	REG_WRITE(ah, AR_WOW_BCN_TIMO, set);
+
+	/*
+	 * Keep alive timo in ms except AR9280
+	 */
+	if (!pattern_enable || AR_SREV_9280(ah))
+		set = AR_WOW_KEEP_ALIVE_NEVER;
+	else
+		set = KAL_TIMEOUT * 32;
+
+	REG_WRITE(ah, AR_WOW_KEEP_ALIVE_TIMO, set);
+
+	/*
+	 * Keep alive delay in us. based on 'power on clock',
+	 * therefore in usec
+	 */
+	set = KAL_DELAY * 1000;
+	REG_WRITE(ah, AR_WOW_KEEP_ALIVE_DELAY, set);
+
+	/*
+	 * Create keep alive pattern to respond to beacons
+	 */
+	ath9k_wow_create_keep_alive_pattern(ah);
+
+	/*
+	 * Configure MAC WoW Registers
+	 */
+
+	set = 0;
+	/* Send keep alive timeouts anyway */
+	clr = AR_WOW_KEEP_ALIVE_AUTO_DIS;
+
+	if (pattern_enable & AH_WOW_LINK_CHANGE)
+		wow_event_mask |= AR_WOW_KEEP_ALIVE_FAIL;
+	else
+		set = AR_WOW_KEEP_ALIVE_FAIL_DIS;
+
+	/*
+	 * FIXME: For now disable keep alive frame
+	 * failure. This seems to sometimes trigger
+	 * unnecessary wake up with AR9485 chipsets.
+	 */
+	set = AR_WOW_KEEP_ALIVE_FAIL_DIS;
+
+	REG_RMW(ah, AR_WOW_KEEP_ALIVE, set, clr);
+
+
+	/*
+	 * we are relying on a bmiss failure. ensure we have
+	 * enough threshold to prevent false positives
+	 */
+	REG_RMW_FIELD(ah, AR_RSSI_THR, AR_RSSI_THR_BM_THR,
+		      AR_WOW_BMISSTHRESHOLD);
+
+	set = 0;
+	clr = 0;
+
+	if (pattern_enable & AH_WOW_BEACON_MISS) {
+		set = AR_WOW_BEACON_FAIL_EN;
+		wow_event_mask |= AR_WOW_BEACON_FAIL;
+	} else {
+		clr = AR_WOW_BEACON_FAIL_EN;
+	}
+
+	REG_RMW(ah, AR_WOW_BCN_EN, set, clr);
+
+	set = 0;
+	clr = 0;
+	/*
+	 * Enable the magic packet registers
+	 */
+	if (pattern_enable & AH_WOW_MAGIC_PATTERN_EN) {
+		set = AR_WOW_MAGIC_EN;
+		wow_event_mask |= AR_WOW_MAGIC_PAT_FOUND;
+	} else {
+		clr = AR_WOW_MAGIC_EN;
+	}
+	set |= AR_WOW_MAC_INTR_EN;
+	REG_RMW(ah, AR_WOW_PATTERN, set, clr);
+
+	/*
+	 * For AR9285 and later version of chipsets
+	 * enable WoW pattern match for packets less
+	 * than 256 bytes for all patterns
+	 */
+	if (AR_SREV_9285_12_OR_LATER(ah))
+		REG_WRITE(ah, AR_WOW_PATTERN_MATCH_LT_256B,
+			  AR_WOW_PATTERN_SUPPORTED);
+
+	/*
+	 * Set the power states appropriately and enable PME
+	 */
+	clr = 0;
+	set = AR_PMCTRL_PWR_STATE_D1D3 | AR_PMCTRL_HOST_PME_EN |
+	      AR_PMCTRL_PWR_PM_CTRL_ENA;
+	/*
+	 * This is needed for AR9300 chipsets to wake-up
+	 * the host.
+	 */
+	if (AR_SREV_9300_20_OR_LATER(ah))
+		clr = AR_PCIE_PM_CTRL_ENA;
+
+	REG_RMW(ah, AR_PCIE_PM_CTRL, set, clr);
+
+	if (AR_SREV_9462(ah)) {
+		/*
+		 * this is needed to prevent the chip waking up
+		 * the host within 3-4 seconds with certain
+		 * platform/BIOS. The fix is to enable
+		 * D1 & D3 to match original definition and
+		 * also match the OTP value. Anyway this
+		 * is more related to SW WOW.
+		 */
+		clr = AR_PMCTRL_PWR_STATE_D1D3;
+		REG_CLR_BIT(ah, AR_PCIE_PM_CTRL, clr);
+
+		set = AR_PMCTRL_PWR_STATE_D1D3_REAL;
+		REG_SET_BIT(ah, AR_PCIE_PM_CTRL, set);
+	}
+
+
+
+	REG_CLR_BIT(ah, AR_STA_ID1, AR_STA_ID1_PRESERVE_SEQNUM);
+
+	if (AR_SREV_9300_20_OR_LATER(ah)) {
+		/* to bring down WOW power low margin */
+		set = BIT(13);
+		REG_SET_BIT(ah, AR_PCIE_PHY_REG3, set);
+		/* HW WoW */
+		clr = BIT(5);
+		REG_CLR_BIT(ah, AR_PCU_MISC_MODE3, clr);
+	}
+
+	ath9k_hw_set_powermode_wow_sleep(ah);
+	ah->wow_event_mask = wow_event_mask;
+}
+EXPORT_SYMBOL(ath9k_hw_wow_enable);
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 4d571394c7a..2c9da6b2ecb 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -29,6 +29,8 @@
 #define HT_LTF(_ns)             (4 * (_ns))
 #define SYMBOL_TIME(_ns)        ((_ns) << 2) /* ns * 4 us */
 #define SYMBOL_TIME_HALFGI(_ns) (((_ns) * 18 + 4) / 5)  /* ns * 3.6 us */
+#define TIME_SYMBOLS(t)         ((t) >> 2)
+#define TIME_SYMBOLS_HALFGI(t)  (((t) * 5 - 4) / 18)
 #define NUM_SYMBOLS_PER_USEC(_usec) (_usec >> 2)
 #define NUM_SYMBOLS_PER_USEC_HALFGI(_usec) (((_usec*5)-4)/18)
 
@@ -74,50 +76,23 @@ enum {
 	MCS_HT40_SGI,
 };
 
-static int ath_max_4ms_framelen[4][32] = {
-	[MCS_HT20] = {
-		3212,  6432,  9648,  12864,  19300,  25736,  28952,  32172,
-		6424,  12852, 19280, 25708,  38568,  51424,  57852,  64280,
-		9628,  19260, 28896, 38528,  57792,  65532,  65532,  65532,
-		12828, 25656, 38488, 51320,  65532,  65532,  65532,  65532,
-	},
-	[MCS_HT20_SGI] = {
-		3572,  7144,  10720,  14296,  21444,  28596,  32172,  35744,
-		7140,  14284, 21428,  28568,  42856,  57144,  64288,  65532,
-		10700, 21408, 32112,  42816,  64228,  65532,  65532,  65532,
-		14256, 28516, 42780,  57040,  65532,  65532,  65532,  65532,
-	},
-	[MCS_HT40] = {
-		6680,  13360,  20044,  26724,  40092,  53456,  60140,  65532,
-		13348, 26700,  40052,  53400,  65532,  65532,  65532,  65532,
-		20004, 40008,  60016,  65532,  65532,  65532,  65532,  65532,
-		26644, 53292,  65532,  65532,  65532,  65532,  65532,  65532,
-	},
-	[MCS_HT40_SGI] = {
-		7420,  14844,  22272,  29696,  44544,  59396,  65532,  65532,
-		14832, 29668,  44504,  59340,  65532,  65532,  65532,  65532,
-		22232, 44464,  65532,  65532,  65532,  65532,  65532,  65532,
-		29616, 59232,  65532,  65532,  65532,  65532,  65532,  65532,
-	}
-};
-
 /*********************/
 /* Aggregation logic */
 /*********************/
 
-static void ath_txq_lock(struct ath_softc *sc, struct ath_txq *txq)
+void ath_txq_lock(struct ath_softc *sc, struct ath_txq *txq)
 	__acquires(&txq->axq_lock)
 {
 	spin_lock_bh(&txq->axq_lock);
 }
 
-static void ath_txq_unlock(struct ath_softc *sc, struct ath_txq *txq)
+void ath_txq_unlock(struct ath_softc *sc, struct ath_txq *txq)
 	__releases(&txq->axq_lock)
 {
 	spin_unlock_bh(&txq->axq_lock);
 }
 
-static void ath_txq_unlock_complete(struct ath_softc *sc, struct ath_txq *txq)
+void ath_txq_unlock_complete(struct ath_softc *sc, struct ath_txq *txq)
 	__releases(&txq->axq_lock)
 {
 	struct sk_buff_head q;
@@ -614,10 +589,8 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 
 	rcu_read_unlock();
 
-	if (needreset) {
-		RESET_STAT_INC(sc, RESET_TYPE_TX_ERROR);
-		ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
-	}
+	if (needreset)
+		ath9k_queue_reset(sc, RESET_TYPE_TX_ERROR);
 }
 
 static bool ath_lookup_legacy(struct ath_buf *bf)
@@ -650,6 +623,7 @@ static u32 ath_lookup_rate(struct ath_softc *sc, struct ath_buf *bf,
 	struct ieee80211_tx_rate *rates;
 	u32 max_4ms_framelen, frmlen;
 	u16 aggr_limit, bt_aggr_limit, legacy = 0;
+	int q = tid->ac->txq->mac80211_qnum;
 	int i;
 
 	skb = bf->bf_mpdu;
@@ -658,8 +632,7 @@ static u32 ath_lookup_rate(struct ath_softc *sc, struct ath_buf *bf,
 
 	/*
 	 * Find the lowest frame length among the rate series that will have a
-	 * 4ms transmit duration.
-	 * TODO - TXOP limit needs to be considered.
+	 * 4ms (or TXOP limited) transmit duration.
 	 */
 	max_4ms_framelen = ATH_AMPDU_LIMIT_MAX;
 
@@ -682,7 +655,7 @@ static u32 ath_lookup_rate(struct ath_softc *sc, struct ath_buf *bf,
 		if (rates[i].flags & IEEE80211_TX_RC_SHORT_GI)
 			modeidx++;
 
-		frmlen = ath_max_4ms_framelen[modeidx][rates[i].idx];
+		frmlen = sc->tx.max_aggr_framelen[q][modeidx][rates[i].idx];
 		max_4ms_framelen = min(max_4ms_framelen, frmlen);
 	}
 
@@ -929,6 +902,44 @@ static u32 ath_pkt_duration(struct ath_softc *sc, u8 rix, int pktlen,
 	return duration;
 }
 
+static int ath_max_framelen(int usec, int mcs, bool ht40, bool sgi)
+{
+	int streams = HT_RC_2_STREAMS(mcs);
+	int symbols, bits;
+	int bytes = 0;
+
+	symbols = sgi ? TIME_SYMBOLS_HALFGI(usec) : TIME_SYMBOLS(usec);
+	bits = symbols * bits_per_symbol[mcs % 8][ht40] * streams;
+	bits -= OFDM_PLCP_BITS;
+	bytes = bits / 8;
+	bytes -= L_STF + L_LTF + L_SIG + HT_SIG + HT_STF + HT_LTF(streams);
+	if (bytes > 65532)
+		bytes = 65532;
+
+	return bytes;
+}
+
+void ath_update_max_aggr_framelen(struct ath_softc *sc, int queue, int txop)
+{
+	u16 *cur_ht20, *cur_ht20_sgi, *cur_ht40, *cur_ht40_sgi;
+	int mcs;
+
+	/* 4ms is the default (and maximum) duration */
+	if (!txop || txop > 4096)
+		txop = 4096;
+
+	cur_ht20 = sc->tx.max_aggr_framelen[queue][MCS_HT20];
+	cur_ht20_sgi = sc->tx.max_aggr_framelen[queue][MCS_HT20_SGI];
+	cur_ht40 = sc->tx.max_aggr_framelen[queue][MCS_HT40];
+	cur_ht40_sgi = sc->tx.max_aggr_framelen[queue][MCS_HT40_SGI];
+	for (mcs = 0; mcs < 32; mcs++) {
+		cur_ht20[mcs] = ath_max_framelen(txop, mcs, false, false);
+		cur_ht20_sgi[mcs] = ath_max_framelen(txop, mcs, false, true);
+		cur_ht40[mcs] = ath_max_framelen(txop, mcs, true, false);
+		cur_ht40_sgi[mcs] = ath_max_framelen(txop, mcs, true, true);
+	}
+}
+
 static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf,
 			     struct ath_tx_info *info, int len)
 {
@@ -1165,6 +1176,7 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 {
 	struct ath_atx_tid *txtid;
 	struct ath_node *an;
+	u8 density;
 
 	an = (struct ath_node *)sta->drv_priv;
 	txtid = ATH_AN_2_TID(an, tid);
@@ -1172,6 +1184,17 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 	if (txtid->state & (AGGR_CLEANUP | AGGR_ADDBA_COMPLETE))
 		return -EAGAIN;
 
+	/* update ampdu factor/density, they may have changed. This may happen
+	 * in HT IBSS when a beacon with HT-info is received after the station
+	 * has already been added.
+	 */
+	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_HT) {
+		an->maxampdu = 1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
+				     sta->ht_cap.ampdu_factor);
+		density = ath9k_parse_mpdudensity(sta->ht_cap.ampdu_density);
+		an->mpdudensity = density;
+	}
+
 	txtid->state |= AGGR_ADDBA_PROGRESS;
 	txtid->paused = true;
 	*ssn = txtid->seq_start = txtid->seq_next;
@@ -1391,16 +1414,6 @@ int ath_txq_update(struct ath_softc *sc, int qnum,
 	int error = 0;
 	struct ath9k_tx_queue_info qi;
 
-	if (qnum == sc->beacon.beaconq) {
-		/*
-		 * XXX: for beacon queue, we just save the parameter.
-		 * It will be picked up by ath_beaconq_config when
-		 * it's necessary.
-		 */
-		sc->beacon.beacon_qi = *qinfo;
-		return 0;
-	}
-
 	BUG_ON(sc->tx.txq[qnum].axq_qnum != qnum);
 
 	ath9k_hw_get_txq_props(ah, qnum, &qi);
@@ -1526,7 +1539,7 @@ bool ath_drain_all_txq(struct ath_softc *sc, bool retry_tx)
 	int i;
 	u32 npend = 0;
 
-	if (sc->sc_flags & SC_OP_INVALID)
+	if (test_bit(SC_OP_INVALID, &sc->sc_flags))
 		return true;
 
 	ath9k_hw_abort_tx_dma(ah);
@@ -1574,7 +1587,8 @@ void ath_txq_schedule(struct ath_softc *sc, struct ath_txq *txq)
 	struct ath_atx_ac *ac, *ac_tmp, *last_ac;
 	struct ath_atx_tid *tid, *last_tid;
 
-	if (work_pending(&sc->hw_reset_work) || list_empty(&txq->axq_acq) ||
+	if (test_bit(SC_OP_HW_RESET, &sc->sc_flags) ||
+	    list_empty(&txq->axq_acq) ||
 	    txq->axq_ampdu_depth >= ATH_AGGR_MIN_QDEPTH)
 		return;
 
@@ -1976,7 +1990,8 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 
 	ath_txq_lock(sc, txq);
 	if (txq == sc->tx.txq_map[q] &&
-	    ++txq->pending_frames > ATH_MAX_QDEPTH && !txq->stopped) {
+	    ++txq->pending_frames > sc->tx.txq_max_pending[q] &&
+	    !txq->stopped) {
 		ieee80211_stop_queue(sc->hw, q);
 		txq->stopped = true;
 	}
@@ -1999,6 +2014,7 @@ static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ieee80211_hdr * hdr = (struct ieee80211_hdr *)skb->data;
 	int q, padpos, padsize;
+	unsigned long flags;
 
 	ath_dbg(common, XMIT, "TX complete: skb: %p\n", skb);
 
@@ -2017,6 +2033,7 @@ static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 		skb_pull(skb, padsize);
 	}
 
+	spin_lock_irqsave(&sc->sc_pm_lock, flags);
 	if ((sc->ps_flags & PS_WAIT_FOR_TX_ACK) && !txq->axq_depth) {
 		sc->ps_flags &= ~PS_WAIT_FOR_TX_ACK;
 		ath_dbg(common, PS,
@@ -2026,13 +2043,15 @@ static void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 					PS_WAIT_FOR_PSPOLL_DATA |
 					PS_WAIT_FOR_TX_ACK));
 	}
+	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
 	q = skb_get_queue_mapping(skb);
 	if (txq == sc->tx.txq_map[q]) {
 		if (WARN_ON(--txq->pending_frames < 0))
 			txq->pending_frames = 0;
 
-		if (txq->stopped && txq->pending_frames < ATH_MAX_QDEPTH) {
+		if (txq->stopped &&
+		    txq->pending_frames < sc->tx.txq_max_pending[q]) {
 			ieee80211_wake_queue(sc->hw, q);
 			txq->stopped = false;
 		}
@@ -2176,7 +2195,7 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 
 	ath_txq_lock(sc, txq);
 	for (;;) {
-		if (work_pending(&sc->hw_reset_work))
+		if (test_bit(SC_OP_HW_RESET, &sc->sc_flags))
 			break;
 
 		if (list_empty(&txq->axq_q)) {
@@ -2236,46 +2255,6 @@ static void ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 	ath_txq_unlock_complete(sc, txq);
 }
 
-static void ath_tx_complete_poll_work(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc,
-			tx_complete_work.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)) {
-			txq = &sc->tx.txq[i];
-			ath_txq_lock(sc, txq);
-			if (txq->axq_depth) {
-				if (txq->axq_tx_inprogress) {
-					needreset = true;
-					ath_txq_unlock(sc, txq);
-					break;
-				} else {
-					txq->axq_tx_inprogress = true;
-				}
-			}
-			ath_txq_unlock_complete(sc, txq);
-		}
-
-	if (needreset) {
-		ath_dbg(ath9k_hw_common(sc->sc_ah), RESET,
-			"tx hung, resetting the chip\n");
-		RESET_STAT_INC(sc, RESET_TYPE_TX_HANG);
-		ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
-	}
-
-	ieee80211_queue_delayed_work(sc->hw, &sc->tx_complete_work,
-			msecs_to_jiffies(ATH_TX_COMPLETE_POLL_INT));
-}
-
-
-
 void ath_tx_tasklet(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
@@ -2299,7 +2278,7 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 	int status;
 
 	for (;;) {
-		if (work_pending(&sc->hw_reset_work))
+		if (test_bit(SC_OP_HW_RESET, &sc->sc_flags))
 			break;
 
 		status = ath9k_hw_txprocdesc(ah, NULL, (void *)&ts);
diff --git a/drivers/net/wireless/ath/carl9170/carl9170.h b/drivers/net/wireless/ath/carl9170/carl9170.h
index 0cea20e3e25..376be11161c 100644
--- a/drivers/net/wireless/ath/carl9170/carl9170.h
+++ b/drivers/net/wireless/ath/carl9170/carl9170.h
@@ -289,6 +289,7 @@ struct ar9170 {
 		unsigned int mem_block_size;
 		unsigned int rx_size;
 		unsigned int tx_seq_table;
+		bool ba_filter;
 	} fw;
 
 	/* interface configuration combinations */
@@ -425,6 +426,10 @@ struct ar9170 {
 	struct sk_buff *rx_failover;
 	int rx_failover_missing;
 
+	/* FIFO for collecting outstanding BlockAckRequest */
+	struct list_head bar_list[__AR9170_NUM_TXQ];
+	spinlock_t bar_list_lock[__AR9170_NUM_TXQ];
+
 #ifdef CONFIG_CARL9170_WPC
 	struct {
 		bool pbc_state;
@@ -468,6 +473,12 @@ enum carl9170_ps_off_override_reasons {
 	PS_OFF_BCN	= BIT(1),
 };
 
+struct carl9170_bar_list_entry {
+	struct list_head list;
+	struct rcu_head head;
+	struct sk_buff *skb;
+};
+
 struct carl9170_ba_stats {
 	u8 ampdu_len;
 	u8 ampdu_ack_len;
diff --git a/drivers/net/wireless/ath/carl9170/cmd.c b/drivers/net/wireless/ath/carl9170/cmd.c
index 195dc653811..39a63874b27 100644
--- a/drivers/net/wireless/ath/carl9170/cmd.c
+++ b/drivers/net/wireless/ath/carl9170/cmd.c
@@ -138,7 +138,7 @@ int carl9170_reboot(struct ar9170 *ar)
 	if (!cmd)
 		return -ENOMEM;
 
-	err = __carl9170_exec_cmd(ar, (struct carl9170_cmd *)cmd, true);
+	err = __carl9170_exec_cmd(ar, cmd, true);
 	return err;
 }
 
diff --git a/drivers/net/wireless/ath/carl9170/fw.c b/drivers/net/wireless/ath/carl9170/fw.c
index 5c73c03872f..c5ca6f1f583 100644
--- a/drivers/net/wireless/ath/carl9170/fw.c
+++ b/drivers/net/wireless/ath/carl9170/fw.c
@@ -307,6 +307,9 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 	if (SUPP(CARL9170FW_WOL))
 		device_set_wakeup_enable(&ar->udev->dev, true);
 
+	if (SUPP(CARL9170FW_RX_BA_FILTER))
+		ar->fw.ba_filter = true;
+
 	if_comb_types = BIT(NL80211_IFTYPE_STATION) |
 			BIT(NL80211_IFTYPE_P2P_CLIENT);
 
diff --git a/drivers/net/wireless/ath/carl9170/fwdesc.h b/drivers/net/wireless/ath/carl9170/fwdesc.h
index 6d9c0891ce7..66848d47c88 100644
--- a/drivers/net/wireless/ath/carl9170/fwdesc.h
+++ b/drivers/net/wireless/ath/carl9170/fwdesc.h
@@ -78,6 +78,9 @@ enum carl9170fw_feature_list {
 	/* HW (ANI, CCA, MIB) tally counters */
 	CARL9170FW_HW_COUNTERS,
 
+	/* Firmware will pass BA when BARs are queued */
+	CARL9170FW_RX_BA_FILTER,
+
 	/* KEEP LAST */
 	__CARL9170FW_FEATURE_NUM
 };
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index 8d2523b3f72..858e58dfc4d 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -949,6 +949,9 @@ static void carl9170_op_configure_filter(struct ieee80211_hw *hw,
 	if (ar->fw.rx_filter && changed_flags & ar->rx_filter_caps) {
 		u32 rx_filter = 0;
 
+		if (!ar->fw.ba_filter)
+			rx_filter |= CARL9170_RX_FILTER_CTL_OTHER;
+
 		if (!(*new_flags & (FIF_FCSFAIL | FIF_PLCPFAIL)))
 			rx_filter |= CARL9170_RX_FILTER_BAD;
 
@@ -1753,6 +1756,9 @@ void *carl9170_alloc(size_t priv_size)
 	for (i = 0; i < ar->hw->queues; i++) {
 		skb_queue_head_init(&ar->tx_status[i]);
 		skb_queue_head_init(&ar->tx_pending[i]);
+
+		INIT_LIST_HEAD(&ar->bar_list[i]);
+		spin_lock_init(&ar->bar_list_lock[i]);
 	}
 	INIT_WORK(&ar->ps_work, carl9170_ps_work);
 	INIT_WORK(&ar->ping_work, carl9170_ping_work);
diff --git a/drivers/net/wireless/ath/carl9170/rx.c b/drivers/net/wireless/ath/carl9170/rx.c
index 84b22eec7ab..6f6a3415566 100644
--- a/drivers/net/wireless/ath/carl9170/rx.c
+++ b/drivers/net/wireless/ath/carl9170/rx.c
@@ -161,7 +161,7 @@ static void carl9170_cmd_callback(struct ar9170 *ar, u32 len, void *buffer)
 
 void carl9170_handle_command_response(struct ar9170 *ar, void *buf, u32 len)
 {
-	struct carl9170_rsp *cmd = (void *) buf;
+	struct carl9170_rsp *cmd = buf;
 	struct ieee80211_vif *vif;
 
 	if (carl9170_check_sequence(ar, cmd->hdr.seq))
@@ -520,7 +520,7 @@ static u8 *carl9170_find_ie(u8 *data, unsigned int len, u8 ie)
  */
 static void carl9170_ps_beacon(struct ar9170 *ar, void *data, unsigned int len)
 {
-	struct ieee80211_hdr *hdr = (void *) data;
+	struct ieee80211_hdr *hdr = data;
 	struct ieee80211_tim_ie *tim_ie;
 	u8 *tim;
 	u8 tim_len;
@@ -576,6 +576,53 @@ static void carl9170_ps_beacon(struct ar9170 *ar, void *data, unsigned int len)
 	}
 }
 
+static void carl9170_ba_check(struct ar9170 *ar, void *data, unsigned int len)
+{
+	struct ieee80211_bar *bar = (void *) data;
+	struct carl9170_bar_list_entry *entry;
+	unsigned int queue;
+
+	if (likely(!ieee80211_is_back(bar->frame_control)))
+		return;
+
+	if (len <= sizeof(*bar) + FCS_LEN)
+		return;
+
+	queue = TID_TO_WME_AC(((le16_to_cpu(bar->control) &
+		IEEE80211_BAR_CTRL_TID_INFO_MASK) >>
+		IEEE80211_BAR_CTRL_TID_INFO_SHIFT) & 7);
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(entry, &ar->bar_list[queue], list) {
+		struct sk_buff *entry_skb = entry->skb;
+		struct _carl9170_tx_superframe *super = (void *)entry_skb->data;
+		struct ieee80211_bar *entry_bar = (void *)super->frame_data;
+
+#define TID_CHECK(a, b) (						\
+	((a) & cpu_to_le16(IEEE80211_BAR_CTRL_TID_INFO_MASK)) ==	\
+	((b) & cpu_to_le16(IEEE80211_BAR_CTRL_TID_INFO_MASK)))		\
+
+		if (bar->start_seq_num == entry_bar->start_seq_num &&
+		    TID_CHECK(bar->control, entry_bar->control) &&
+		    compare_ether_addr(bar->ra, entry_bar->ta) == 0 &&
+		    compare_ether_addr(bar->ta, entry_bar->ra) == 0) {
+			struct ieee80211_tx_info *tx_info;
+
+			tx_info = IEEE80211_SKB_CB(entry_skb);
+			tx_info->flags |= IEEE80211_TX_STAT_ACK;
+
+			spin_lock_bh(&ar->bar_list_lock[queue]);
+			list_del_rcu(&entry->list);
+			spin_unlock_bh(&ar->bar_list_lock[queue]);
+			kfree_rcu(entry, head);
+			break;
+		}
+	}
+	rcu_read_unlock();
+
+#undef TID_CHECK
+}
+
 static bool carl9170_ampdu_check(struct ar9170 *ar, u8 *buf, u8 ms)
 {
 	__le16 fc;
@@ -738,6 +785,8 @@ static void carl9170_handle_mpdu(struct ar9170 *ar, u8 *buf, int len)
 
 	carl9170_ps_beacon(ar, buf, mpdu_len);
 
+	carl9170_ba_check(ar, buf, mpdu_len);
+
 	skb = carl9170_rx_copy_data(buf, mpdu_len);
 	if (!skb)
 		goto drop;
diff --git a/drivers/net/wireless/ath/carl9170/tx.c b/drivers/net/wireless/ath/carl9170/tx.c
index aed305177af..6a8681407a1 100644
--- a/drivers/net/wireless/ath/carl9170/tx.c
+++ b/drivers/net/wireless/ath/carl9170/tx.c
@@ -277,11 +277,11 @@ static void carl9170_tx_release(struct kref *ref)
 		return;
 
 	BUILD_BUG_ON(
-	    offsetof(struct ieee80211_tx_info, status.ampdu_ack_len) != 23);
+	    offsetof(struct ieee80211_tx_info, status.ack_signal) != 20);
 
-	memset(&txinfo->status.ampdu_ack_len, 0,
+	memset(&txinfo->status.ack_signal, 0,
 	       sizeof(struct ieee80211_tx_info) -
-	       offsetof(struct ieee80211_tx_info, status.ampdu_ack_len));
+	       offsetof(struct ieee80211_tx_info, status.ack_signal));
 
 	if (atomic_read(&ar->tx_total_queued))
 		ar->tx_schedule = true;
@@ -436,6 +436,45 @@ out_rcu:
 	rcu_read_unlock();
 }
 
+static void carl9170_tx_bar_status(struct ar9170 *ar, struct sk_buff *skb,
+	struct ieee80211_tx_info *tx_info)
+{
+	struct _carl9170_tx_superframe *super = (void *) skb->data;
+	struct ieee80211_bar *bar = (void *) super->frame_data;
+
+	/*
+	 * Unlike all other frames, the status report for BARs does
+	 * not directly come from the hardware as it is incapable of
+	 * matching a BA to a previously send BAR.
+	 * Instead the RX-path will scan for incoming BAs and set the
+	 * IEEE80211_TX_STAT_ACK if it sees one that was likely
+	 * caused by a BAR from us.
+	 */
+
+	if (unlikely(ieee80211_is_back_req(bar->frame_control)) &&
+	   !(tx_info->flags & IEEE80211_TX_STAT_ACK)) {
+		struct carl9170_bar_list_entry *entry;
+		int queue = skb_get_queue_mapping(skb);
+
+		rcu_read_lock();
+		list_for_each_entry_rcu(entry, &ar->bar_list[queue], list) {
+			if (entry->skb == skb) {
+				spin_lock_bh(&ar->bar_list_lock[queue]);
+				list_del_rcu(&entry->list);
+				spin_unlock_bh(&ar->bar_list_lock[queue]);
+				kfree_rcu(entry, head);
+				goto out;
+			}
+		}
+
+		WARN(1, "bar not found in %d - ra:%pM ta:%pM c:%x ssn:%x\n",
+		       queue, bar->ra, bar->ta, bar->control,
+			bar->start_seq_num);
+out:
+		rcu_read_unlock();
+	}
+}
+
 void carl9170_tx_status(struct ar9170 *ar, struct sk_buff *skb,
 			const bool success)
 {
@@ -445,6 +484,8 @@ void carl9170_tx_status(struct ar9170 *ar, struct sk_buff *skb,
 
 	txinfo = IEEE80211_SKB_CB(skb);
 
+	carl9170_tx_bar_status(ar, skb, txinfo);
+
 	if (success)
 		txinfo->flags |= IEEE80211_TX_STAT_ACK;
 	else
@@ -1265,6 +1306,26 @@ out_rcu:
 	return false;
 }
 
+static void carl9170_bar_check(struct ar9170 *ar, struct sk_buff *skb)
+{
+	struct _carl9170_tx_superframe *super = (void *) skb->data;
+	struct ieee80211_bar *bar = (void *) super->frame_data;
+
+	if (unlikely(ieee80211_is_back_req(bar->frame_control)) &&
+	    skb->len >= sizeof(struct ieee80211_bar)) {
+		struct carl9170_bar_list_entry *entry;
+		unsigned int queue = skb_get_queue_mapping(skb);
+
+		entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
+		if (!WARN_ON_ONCE(!entry)) {
+			entry->skb = skb;
+			spin_lock_bh(&ar->bar_list_lock[queue]);
+			list_add_tail_rcu(&entry->list, &ar->bar_list[queue]);
+			spin_unlock_bh(&ar->bar_list_lock[queue]);
+		}
+	}
+}
+
 static void carl9170_tx(struct ar9170 *ar)
 {
 	struct sk_buff *skb;
@@ -1287,6 +1348,8 @@ static void carl9170_tx(struct ar9170 *ar)
 			if (unlikely(carl9170_tx_ps_drop(ar, skb)))
 				continue;
 
+			carl9170_bar_check(ar, skb);
+
 			atomic_inc(&ar->tx_total_pending);
 
 			q = __carl9170_get_queue(ar, i);
diff --git a/drivers/net/wireless/ath/carl9170/version.h b/drivers/net/wireless/ath/carl9170/version.h
index e651db85634..2ec3e9191e4 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 11
-#define CARL9170FW_VERSION_MONTH 8
-#define CARL9170FW_VERSION_DAY 15
-#define CARL9170FW_VERSION_GIT "1.9.4"
+#define CARL9170FW_VERSION_YEAR 12
+#define CARL9170FW_VERSION_MONTH 7
+#define CARL9170FW_VERSION_DAY 7
+#define CARL9170FW_VERSION_GIT "1.9.6"
 #endif /* __CARL9170_SHARED_VERSION_H */
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index d07c0301da6..4a4e98f7180 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -2952,10 +2952,10 @@ static void send_association_request(struct atmel_private *priv, int is_reassoc)
 	/* current AP address - only in reassoc frame */
 	if (is_reassoc) {
 		memcpy(body.ap, priv->CurrentBSSID, 6);
-		ssid_el_p = (u8 *)&body.ssid_el_id;
+		ssid_el_p = &body.ssid_el_id;
 		bodysize = 18 + priv->SSID_size;
 	} else {
-		ssid_el_p = (u8 *)&body.ap[0];
+		ssid_el_p = &body.ap[0];
 		bodysize = 12 + priv->SSID_size;
 	}
 
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index c06b6cb5c91..7c899fc7ddd 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -870,13 +870,6 @@ struct b43_wl {
 	 * handler, only. This basically is just the IRQ mask register. */
 	spinlock_t hardirq_lock;
 
-	/* The number of queues that were registered with the mac80211 subsystem
-	 * initially. This is a backup copy of hw->queues in case hw->queues has
-	 * to be dynamically lowered at runtime (Firmware does not support QoS).
-	 * hw->queues has to be restored to the original value before unregistering
-	 * from the mac80211 subsystem. */
-	u16 mac80211_initially_registered_queues;
-
 	/* Set this if we call ieee80211_register_hw() and check if we call
 	 * ieee80211_unregister_hw(). */
 	bool hw_registred;
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 1b988f26bdf..b80352b308d 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -2359,6 +2359,8 @@ static int b43_try_request_fw(struct b43_request_fw_context *ctx)
 	if (err)
 		goto err_load;
 
+	fw->opensource = (ctx->req_type == B43_FWTYPE_OPENSOURCE);
+
 	return 0;
 
 err_no_ucode:
@@ -2434,6 +2436,10 @@ static void b43_request_firmware(struct work_struct *work)
 	goto out;
 
 start_ieee80211:
+	wl->hw->queues = B43_QOS_QUEUE_NUM;
+	if (!modparam_qos || dev->fw.opensource)
+		wl->hw->queues = 1;
+
 	err = ieee80211_register_hw(wl->hw);
 	if (err)
 		goto err_one_core_detach;
@@ -2537,11 +2543,9 @@ static int b43_upload_microcode(struct b43_wldev *dev)
 		dev->fw.hdr_format = B43_FW_HDR_410;
 	else
 		dev->fw.hdr_format = B43_FW_HDR_351;
-	dev->fw.opensource = (fwdate == 0xFFFF);
+	WARN_ON(dev->fw.opensource != (fwdate == 0xFFFF));
 
-	/* Default to use-all-queues. */
-	dev->wl->hw->queues = dev->wl->mac80211_initially_registered_queues;
-	dev->qos_enabled = !!modparam_qos;
+	dev->qos_enabled = dev->wl->hw->queues > 1;
 	/* Default to firmware/hardware crypto acceleration. */
 	dev->hwcrypto_enabled = true;
 
@@ -2559,14 +2563,8 @@ static int b43_upload_microcode(struct b43_wldev *dev)
 			/* Disable hardware crypto and fall back to software crypto. */
 			dev->hwcrypto_enabled = false;
 		}
-		if (!(fwcapa & B43_FWCAPA_QOS)) {
-			b43info(dev->wl, "QoS not supported by firmware\n");
-			/* Disable QoS. Tweak hw->queues to 1. It will be restored before
-			 * ieee80211_unregister to make sure the networking core can
-			 * properly free possible resources. */
-			dev->wl->hw->queues = 1;
-			dev->qos_enabled = false;
-		}
+		/* adding QoS support should use an offline discovery mechanism */
+		WARN(fwcapa & B43_FWCAPA_QOS, "QoS in OpenFW not supported\n");
 	} else {
 		b43info(dev->wl, "Loading firmware version %u.%u "
 			"(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
@@ -5298,8 +5296,6 @@ static struct b43_wl *b43_wireless_init(struct b43_bus_dev *dev)
 
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
 
-	hw->queues = modparam_qos ? B43_QOS_QUEUE_NUM : 1;
-	wl->mac80211_initially_registered_queues = hw->queues;
 	wl->hw_registred = false;
 	hw->max_rates = 2;
 	SET_IEEE80211_DEV(hw, dev->dev);
@@ -5374,10 +5370,6 @@ static void b43_bcma_remove(struct bcma_device *core)
 
 	B43_WARN_ON(!wl);
 	if (wl->current_dev == wldev && wl->hw_registred) {
-		/* Restore the queues count before unregistering, because firmware detect
-		 * might have modified it. Restoring is important, so the networking
-		 * stack can properly free resources. */
-		wl->hw->queues = wl->mac80211_initially_registered_queues;
 		b43_leds_stop(wldev);
 		ieee80211_unregister_hw(wl->hw);
 	}
@@ -5452,10 +5444,6 @@ static void b43_ssb_remove(struct ssb_device *sdev)
 
 	B43_WARN_ON(!wl);
 	if (wl->current_dev == wldev && wl->hw_registred) {
-		/* Restore the queues count before unregistering, because firmware detect
-		 * might have modified it. Restoring is important, so the networking
-		 * stack can properly free resources. */
-		wl->hw->queues = wl->mac80211_initially_registered_queues;
 		b43_leds_stop(wldev);
 		ieee80211_unregister_hw(wl->hw);
 	}
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index 108118820b3..b92bb9c92ad 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -1369,7 +1369,7 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 					i << 2);
 			b43_nphy_poll_rssi(dev, 2, results[i], 8);
 		}
-		for (i = 0; i < 4; i++) {
+		for (i = 0; i < 4; i += 2) {
 			s32 curr;
 			s32 mind = 40;
 			s32 minpoll = 249;
@@ -1415,14 +1415,15 @@ static void b43_nphy_rev3_rssi_cal(struct b43_wldev *dev)
 			b43_nphy_scale_offset_rssi(dev, 0, 0, core + 1, 1, i);
 			b43_nphy_poll_rssi(dev, i, poll_results, 8);
 			for (j = 0; j < 4; j++) {
-				if (j / 2 == core)
+				if (j / 2 == core) {
 					offset[j] = 232 - poll_results[j];
-				if (offset[j] < 0)
-					offset[j] = -(abs(offset[j] + 4) / 8);
-				else
-					offset[j] = (offset[j] + 4) / 8;
-				b43_nphy_scale_offset_rssi(dev, 0,
-					offset[2 * core], core + 1, j % 2, i);
+					if (offset[j] < 0)
+						offset[j] = -(abs(offset[j] + 4) / 8);
+					else
+						offset[j] = (offset[j] + 4) / 8;
+					b43_nphy_scale_offset_rssi(dev, 0,
+						offset[2 * core], core + 1, j % 2, i);
+				}
 			}
 		}
 	}
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index b31ccc02fa2..136510edf3c 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -663,7 +663,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 	u32 uninitialized_var(macstat);
 	u16 chanid;
 	u16 phytype;
-	int padding;
+	int padding, rate_idx;
 
 	memset(&status, 0, sizeof(status));
 
@@ -766,16 +766,17 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 	}
 
 	if (phystat0 & B43_RX_PHYST0_OFDM)
-		status.rate_idx = b43_plcp_get_bitrate_idx_ofdm(plcp,
+		rate_idx = b43_plcp_get_bitrate_idx_ofdm(plcp,
 						phytype == B43_PHYTYPE_A);
 	else
-		status.rate_idx = b43_plcp_get_bitrate_idx_cck(plcp);
-	if (unlikely(status.rate_idx == -1)) {
+		rate_idx = b43_plcp_get_bitrate_idx_cck(plcp);
+	if (unlikely(rate_idx == -1)) {
 		/* PLCP seems to be corrupted.
 		 * Drop the frame, if we are not interested in corrupted frames. */
 		if (!(dev->wl->filter_flags & FIF_PLCPFAIL))
 			goto drop;
 	}
+	status.rate_idx = rate_idx;
 	status.antenna = !!(phystat0 & B43_RX_PHYST0_ANT);
 
 	/*
diff --git a/drivers/net/wireless/b43legacy/dma.c b/drivers/net/wireless/b43legacy/dma.c
index f1f8bd09bd8..2d3c6644f82 100644
--- a/drivers/net/wireless/b43legacy/dma.c
+++ b/drivers/net/wireless/b43legacy/dma.c
@@ -52,7 +52,7 @@ struct b43legacy_dmadesc32 *op32_idx2desc(struct b43legacy_dmaring *ring,
 	desc = ring->descbase;
 	desc = &(desc[slot]);
 
-	return (struct b43legacy_dmadesc32 *)desc;
+	return desc;
 }
 
 static void op32_fill_descriptor(struct b43legacy_dmaring *ring,
@@ -1072,7 +1072,7 @@ static int dma_tx_fragment(struct b43legacy_dmaring *ring,
 	meta->dmaaddr = map_descbuffer(ring, skb->data, skb->len, 1);
 	/* create a bounce buffer in zone_dma on mapping failure. */
 	if (b43legacy_dma_mapping_error(ring, meta->dmaaddr, skb->len, 1)) {
-		bounce_skb = __dev_alloc_skb(skb->len, GFP_ATOMIC | GFP_DMA);
+		bounce_skb = alloc_skb(skb->len, GFP_ATOMIC | GFP_DMA);
 		if (!bounce_skb) {
 			ring->current_slot = old_top_slot;
 			ring->used_slots = old_used_slots;
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index eae691e2f7d..8156135a059 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -1508,7 +1508,7 @@ static void b43legacy_release_firmware(struct b43legacy_wldev *dev)
 
 static void b43legacy_print_fw_helptext(struct b43legacy_wl *wl)
 {
-	b43legacyerr(wl, "You must go to http://linuxwireless.org/en/users/"
+	b43legacyerr(wl, "You must go to http://wireless.kernel.org/en/users/"
 		     "Drivers/b43#devicefirmware "
 		     "and download the correct firmware (version 3).\n");
 }
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
index a8012f2749e..b8ffea6f5c6 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -269,8 +269,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 	b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
 				    (&txhdr->plcp), plcp_fragment_len,
 				    rate);
-	b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
-				    (&txhdr->plcp_fb), plcp_fragment_len,
+	b43legacy_generate_plcp_hdr(&txhdr->plcp_fb, plcp_fragment_len,
 				    rate_fb->hw_value);
 
 	/* PHY TX Control word */
@@ -340,8 +339,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 		b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
 					    (&txhdr->rts_plcp),
 					    len, rts_rate);
-		b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
-					    (&txhdr->rts_plcp_fb),
+		b43legacy_generate_plcp_hdr(&txhdr->rts_plcp_fb,
 					    len, rts_rate_fb);
 		hdr = (struct ieee80211_hdr *)(&txhdr->rts_frame);
 		txhdr->rts_dur_fb = hdr->duration_id;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index abb48032753..9d5170b6df5 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
@@ -34,3 +34,5 @@ brcmfmac-$(CONFIG_BRCMFMAC_SDIO) += \
 		sdio_chip.o
 brcmfmac-$(CONFIG_BRCMFMAC_USB) += \
 		usb.o
+brcmfmac-$(CONFIG_BRCMDBG) += \
+		dhd_dbg.o
\ No newline at end of file
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index 82f51dbd0d6..49765d34b4e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -44,6 +44,7 @@
 
 #define SDIO_DEVICE_ID_BROADCOM_4329	0x4329
 #define SDIO_DEVICE_ID_BROADCOM_4330	0x4330
+#define SDIO_DEVICE_ID_BROADCOM_4334	0x4334
 
 #define SDIO_FUNC1_BLOCKSIZE		64
 #define SDIO_FUNC2_BLOCKSIZE		512
@@ -52,6 +53,7 @@
 static const struct sdio_device_id brcmf_sdmmc_ids[] = {
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4329)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4330)},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4334)},
 	{ /* end: all zeroes */ },
 };
 MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
index 9f637014486..a11fe54f595 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
@@ -613,6 +613,9 @@ struct brcmf_pub {
 	struct work_struct multicast_work;
 	u8 macvalue[ETH_ALEN];
 	atomic_t pend_8021x_cnt;
+#ifdef DEBUG
+	struct dentry *dbgfs_dir;
+#endif
 };
 
 struct brcmf_if_event {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
index 366916494be..537f499cc5d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
@@ -36,6 +36,13 @@ struct dngl_stats {
 	unsigned long multicast;	/* multicast packets received */
 };
 
+struct brcmf_bus_dcmd {
+	char *name;
+	char *param;
+	int param_len;
+	struct list_head list;
+};
+
 /* interface structure between common and bus layer */
 struct brcmf_bus {
 	u8 type;		/* bus type */
@@ -50,6 +57,7 @@ struct brcmf_bus {
 	unsigned long tx_realloc;	/* Tx packets realloced for headroom */
 	struct dngl_stats dstats;	/* Stats for dongle-based data */
 	u8 align;		/* bus alignment requirement */
+	struct list_head dcmd_list;
 
 	/* interface functions pointers */
 	/* Stop bus module: clear pending frames, disable data flow */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
index 236cb9fa460..2621dd3d7dc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
@@ -800,13 +800,13 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 	char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];	/*  Room for
 				 "event_msgs" + '\0' + bitvec  */
 	char buf[128], *ptr;
-	u32 dongle_align = drvr->bus_if->align;
-	u32 glom = 0;
 	u32 roaming = 1;
 	uint bcn_timeout = 3;
 	int scan_assoc_time = 40;
 	int scan_unassoc_time = 40;
 	int i;
+	struct brcmf_bus_dcmd *cmdlst;
+	struct list_head *cur, *q;
 
 	mutex_lock(&drvr->proto_block);
 
@@ -827,17 +827,6 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 	/* Print fw version info */
 	brcmf_dbg(ERROR, "Firmware version = %s\n", buf);
 
-	/* Match Host and Dongle rx alignment */
-	brcmf_c_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf,
-		    sizeof(iovbuf));
-	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
-				  sizeof(iovbuf));
-
-	/* disable glom option per default */
-	brcmf_c_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
-	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
-				  sizeof(iovbuf));
-
 	/* Setup timeout if Beacons are lost and roam is off to report
 		 link down */
 	brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf,
@@ -874,6 +863,20 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 						 0, true);
 	}
 
+	/* set bus specific command if there is any */
+	list_for_each_safe(cur, q, &drvr->bus_if->dcmd_list) {
+		cmdlst = list_entry(cur, struct brcmf_bus_dcmd, list);
+		if (cmdlst->name && cmdlst->param && cmdlst->param_len) {
+			brcmf_c_mkiovar(cmdlst->name, cmdlst->param,
+					cmdlst->param_len, iovbuf,
+					sizeof(iovbuf));
+			brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR,
+						 iovbuf, sizeof(iovbuf));
+		}
+		list_del(cur);
+		kfree(cmdlst);
+	}
+
 	mutex_unlock(&drvr->proto_block);
 
 	return 0;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
new file mode 100644
index 00000000000..7f89540b56d
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2012 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include <linux/debugfs.h>
+#include <linux/if_ether.h>
+#include <linux/if.h>
+#include <linux/ieee80211.h>
+#include <linux/module.h>
+
+#include <defs.h>
+#include <brcmu_wifi.h>
+#include <brcmu_utils.h>
+#include "dhd.h"
+#include "dhd_bus.h"
+#include "dhd_dbg.h"
+
+static struct dentry *root_folder;
+
+void brcmf_debugfs_init(void)
+{
+	root_folder = debugfs_create_dir(KBUILD_MODNAME, NULL);
+	if (IS_ERR(root_folder))
+		root_folder = NULL;
+}
+
+void brcmf_debugfs_exit(void)
+{
+	if (!root_folder)
+		return;
+
+	debugfs_remove_recursive(root_folder);
+	root_folder = NULL;
+}
+
+int brcmf_debugfs_attach(struct brcmf_pub *drvr)
+{
+	if (!root_folder)
+		return -ENODEV;
+
+	drvr->dbgfs_dir = debugfs_create_dir(dev_name(drvr->dev), root_folder);
+	return PTR_RET(drvr->dbgfs_dir);
+}
+
+void brcmf_debugfs_detach(struct brcmf_pub *drvr)
+{
+	if (!IS_ERR_OR_NULL(drvr->dbgfs_dir))
+		debugfs_remove_recursive(drvr->dbgfs_dir);
+}
+
+struct dentry *brcmf_debugfs_get_devdir(struct brcmf_pub *drvr)
+{
+	return drvr->dbgfs_dir;
+}
+
+static
+ssize_t brcmf_debugfs_sdio_counter_read(struct file *f, char __user *data,
+					size_t count, loff_t *ppos)
+{
+	struct brcmf_sdio_count *sdcnt = f->private_data;
+	char buf[750];
+	int res;
+
+	/* only allow read from start */
+	if (*ppos > 0)
+		return 0;
+
+	res = scnprintf(buf, sizeof(buf),
+			"intrcount:    %u\nlastintrs:    %u\n"
+			"pollcnt:      %u\nregfails:     %u\n"
+			"tx_sderrs:    %u\nfcqueued:     %u\n"
+			"rxrtx:        %u\nrx_toolong:   %u\n"
+			"rxc_errors:   %u\nrx_hdrfail:   %u\n"
+			"rx_badhdr:    %u\nrx_badseq:    %u\n"
+			"fc_rcvd:      %u\nfc_xoff:      %u\n"
+			"fc_xon:       %u\nrxglomfail:   %u\n"
+			"rxglomframes: %u\nrxglompkts:   %u\n"
+			"f2rxhdrs:     %u\nf2rxdata:     %u\n"
+			"f2txdata:     %u\nf1regdata:    %u\n"
+			"tickcnt:      %u\ntx_ctlerrs:   %lu\n"
+			"tx_ctlpkts:   %lu\nrx_ctlerrs:   %lu\n"
+			"rx_ctlpkts:   %lu\nrx_readahead: %lu\n",
+			sdcnt->intrcount, sdcnt->lastintrs,
+			sdcnt->pollcnt, sdcnt->regfails,
+			sdcnt->tx_sderrs, sdcnt->fcqueued,
+			sdcnt->rxrtx, sdcnt->rx_toolong,
+			sdcnt->rxc_errors, sdcnt->rx_hdrfail,
+			sdcnt->rx_badhdr, sdcnt->rx_badseq,
+			sdcnt->fc_rcvd, sdcnt->fc_xoff,
+			sdcnt->fc_xon, sdcnt->rxglomfail,
+			sdcnt->rxglomframes, sdcnt->rxglompkts,
+			sdcnt->f2rxhdrs, sdcnt->f2rxdata,
+			sdcnt->f2txdata, sdcnt->f1regdata,
+			sdcnt->tickcnt, sdcnt->tx_ctlerrs,
+			sdcnt->tx_ctlpkts, sdcnt->rx_ctlerrs,
+			sdcnt->rx_ctlpkts, sdcnt->rx_readahead_cnt);
+
+	return simple_read_from_buffer(data, count, ppos, buf, res);
+}
+
+static const struct file_operations brcmf_debugfs_sdio_counter_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = brcmf_debugfs_sdio_counter_read
+};
+
+void brcmf_debugfs_create_sdio_count(struct brcmf_pub *drvr,
+				     struct brcmf_sdio_count *sdcnt)
+{
+	struct dentry *dentry = drvr->dbgfs_dir;
+
+	if (!IS_ERR_OR_NULL(dentry))
+		debugfs_create_file("counters", S_IRUGO, dentry,
+				    sdcnt, &brcmf_debugfs_sdio_counter_ops);
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
index a2c4576cf9f..b784920532d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
@@ -76,4 +76,63 @@ do {									\
 
 extern int brcmf_msg_level;
 
+/*
+ * hold counter variables used in brcmfmac sdio driver.
+ */
+struct brcmf_sdio_count {
+	uint intrcount;		/* Count of device interrupt callbacks */
+	uint lastintrs;		/* Count as of last watchdog timer */
+	uint pollcnt;		/* Count of active polls */
+	uint regfails;		/* Count of R_REG failures */
+	uint tx_sderrs;		/* Count of tx attempts with sd errors */
+	uint fcqueued;		/* Tx packets that got queued */
+	uint rxrtx;		/* Count of rtx requests (NAK to dongle) */
+	uint rx_toolong;	/* Receive frames too long to receive */
+	uint rxc_errors;	/* SDIO errors when reading control frames */
+	uint rx_hdrfail;	/* SDIO errors on header reads */
+	uint rx_badhdr;		/* Bad received headers (roosync?) */
+	uint rx_badseq;		/* Mismatched rx sequence number */
+	uint fc_rcvd;		/* Number of flow-control events received */
+	uint fc_xoff;		/* Number which turned on flow-control */
+	uint fc_xon;		/* Number which turned off flow-control */
+	uint rxglomfail;	/* Failed deglom attempts */
+	uint rxglomframes;	/* Number of glom frames (superframes) */
+	uint rxglompkts;	/* Number of packets from glom frames */
+	uint f2rxhdrs;		/* Number of header reads */
+	uint f2rxdata;		/* Number of frame data reads */
+	uint f2txdata;		/* Number of f2 frame writes */
+	uint f1regdata;		/* Number of f1 register accesses */
+	uint tickcnt;		/* Number of watchdog been schedule */
+	ulong tx_ctlerrs;	/* Err of sending ctrl frames */
+	ulong tx_ctlpkts;	/* Ctrl frames sent to dongle */
+	ulong rx_ctlerrs;	/* Err of processing rx ctrl frames */
+	ulong rx_ctlpkts;	/* Ctrl frames processed from dongle */
+	ulong rx_readahead_cnt;	/* packets where header read-ahead was used */
+};
+
+struct brcmf_pub;
+#ifdef DEBUG
+void brcmf_debugfs_init(void);
+void brcmf_debugfs_exit(void);
+int brcmf_debugfs_attach(struct brcmf_pub *drvr);
+void brcmf_debugfs_detach(struct brcmf_pub *drvr);
+struct dentry *brcmf_debugfs_get_devdir(struct brcmf_pub *drvr);
+void brcmf_debugfs_create_sdio_count(struct brcmf_pub *drvr,
+				     struct brcmf_sdio_count *sdcnt);
+#else
+static inline void brcmf_debugfs_init(void)
+{
+}
+static inline void brcmf_debugfs_exit(void)
+{
+}
+static inline int brcmf_debugfs_attach(struct brcmf_pub *drvr)
+{
+	return 0;
+}
+static inline void brcmf_debugfs_detach(struct brcmf_pub *drvr)
+{
+}
+#endif
+
 #endif				/* _BRCMF_DBG_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
index 8933f9b31a9..9ab24528f9b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
@@ -1007,6 +1007,9 @@ int brcmf_attach(uint bus_hdrlen, struct device *dev)
 	drvr->bus_if->drvr = drvr;
 	drvr->dev = dev;
 
+	/* create device debugfs folder */
+	brcmf_debugfs_attach(drvr);
+
 	/* Attach and link in the protocol */
 	ret = brcmf_proto_attach(drvr);
 	if (ret != 0) {
@@ -1017,6 +1020,8 @@ int brcmf_attach(uint bus_hdrlen, struct device *dev)
 	INIT_WORK(&drvr->setmacaddr_work, _brcmf_set_mac_address);
 	INIT_WORK(&drvr->multicast_work, _brcmf_set_multicast_list);
 
+	INIT_LIST_HEAD(&drvr->bus_if->dcmd_list);
+
 	return ret;
 
 fail:
@@ -1123,6 +1128,7 @@ void brcmf_detach(struct device *dev)
 		brcmf_proto_detach(drvr);
 	}
 
+	brcmf_debugfs_detach(drvr);
 	bus_if->drvr = NULL;
 	kfree(drvr);
 }
@@ -1182,7 +1188,7 @@ exit:
 	kfree(buf);
 	/* close file before return */
 	if (fp)
-		filp_close(fp, current->files);
+		filp_close(fp, NULL);
 	/* restore previous address limit */
 	set_fs(old_fs);
 
@@ -1192,6 +1198,8 @@ exit:
 
 static void brcmf_driver_init(struct work_struct *work)
 {
+	brcmf_debugfs_init();
+
 #ifdef CONFIG_BRCMFMAC_SDIO
 	brcmf_sdio_init();
 #endif
@@ -1219,6 +1227,7 @@ static void __exit brcmfmac_module_exit(void)
 #ifdef CONFIG_BRCMFMAC_USB
 	brcmf_usb_exit();
 #endif
+	brcmf_debugfs_exit();
 }
 
 module_init(brcmfmac_module_init);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 1dbf2be478c..472f2ef5c65 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -31,6 +31,8 @@
 #include <linux/firmware.h>
 #include <linux/module.h>
 #include <linux/bcma/bcma.h>
+#include <linux/debugfs.h>
+#include <linux/vmalloc.h>
 #include <asm/unaligned.h>
 #include <defs.h>
 #include <brcmu_wifi.h>
@@ -48,6 +50,9 @@
 
 #define CBUF_LEN	(128)
 
+/* Device console log buffer state */
+#define CONSOLE_BUFFER_MAX	2024
+
 struct rte_log_le {
 	__le32 buf;		/* Can't be pointer on (64-bit) hosts */
 	__le32 buf_size;
@@ -281,7 +286,7 @@ struct rte_console {
  * Shared structure between dongle and the host.
  * The structure contains pointers to trap or assert information.
  */
-#define SDPCM_SHARED_VERSION       0x0002
+#define SDPCM_SHARED_VERSION       0x0003
 #define SDPCM_SHARED_VERSION_MASK  0x00FF
 #define SDPCM_SHARED_ASSERT_BUILT  0x0100
 #define SDPCM_SHARED_ASSERT        0x0200
@@ -428,6 +433,29 @@ struct brcmf_console {
 	u8 *buf;		/* Log buffer (host copy) */
 	uint last;		/* Last buffer read index */
 };
+
+struct brcmf_trap_info {
+	__le32		type;
+	__le32		epc;
+	__le32		cpsr;
+	__le32		spsr;
+	__le32		r0;	/* a1 */
+	__le32		r1;	/* a2 */
+	__le32		r2;	/* a3 */
+	__le32		r3;	/* a4 */
+	__le32		r4;	/* v1 */
+	__le32		r5;	/* v2 */
+	__le32		r6;	/* v3 */
+	__le32		r7;	/* v4 */
+	__le32		r8;	/* v5 */
+	__le32		r9;	/* sb/v6 */
+	__le32		r10;	/* sl/v7 */
+	__le32		r11;	/* fp/v8 */
+	__le32		r12;	/* ip */
+	__le32		r13;	/* sp */
+	__le32		r14;	/* lr */
+	__le32		pc;	/* r15 */
+};
 #endif				/* DEBUG */
 
 struct sdpcm_shared {
@@ -439,6 +467,7 @@ struct sdpcm_shared {
 	u32 console_addr;	/* Address of struct rte_console */
 	u32 msgtrace_addr;
 	u8 tag[32];
+	u32 brpt_addr;
 };
 
 struct sdpcm_shared_le {
@@ -450,6 +479,7 @@ struct sdpcm_shared_le {
 	__le32 console_addr;	/* Address of struct rte_console */
 	__le32 msgtrace_addr;
 	u8 tag[32];
+	__le32 brpt_addr;
 };
 
 
@@ -502,12 +532,9 @@ struct brcmf_sdio {
 	bool intr;		/* Use interrupts */
 	bool poll;		/* Use polling */
 	bool ipend;		/* Device interrupt is pending */
-	uint intrcount;		/* Count of device interrupt callbacks */
-	uint lastintrs;		/* Count as of last watchdog timer */
 	uint spurious;		/* Count of spurious interrupts */
 	uint pollrate;		/* Ticks between device polls */
 	uint polltick;		/* Tick counter */
-	uint pollcnt;		/* Count of active polls */
 
 #ifdef DEBUG
 	uint console_interval;
@@ -515,8 +542,6 @@ struct brcmf_sdio {
 	uint console_addr;	/* Console address from shared struct */
 #endif				/* DEBUG */
 
-	uint regfails;		/* Count of R_REG failures */
-
 	uint clkstate;		/* State of sd and backplane clock(s) */
 	bool activity;		/* Activity flag for clock down */
 	s32 idletime;		/* Control for activity timeout */
@@ -531,33 +556,6 @@ struct brcmf_sdio {
 /* Field to decide if rx of control frames happen in rxbuf or lb-pool */
 	bool usebufpool;
 
-	/* Some additional counters */
-	uint tx_sderrs;		/* Count of tx attempts with sd errors */
-	uint fcqueued;		/* Tx packets that got queued */
-	uint rxrtx;		/* Count of rtx requests (NAK to dongle) */
-	uint rx_toolong;	/* Receive frames too long to receive */
-	uint rxc_errors;	/* SDIO errors when reading control frames */
-	uint rx_hdrfail;	/* SDIO errors on header reads */
-	uint rx_badhdr;		/* Bad received headers (roosync?) */
-	uint rx_badseq;		/* Mismatched rx sequence number */
-	uint fc_rcvd;		/* Number of flow-control events received */
-	uint fc_xoff;		/* Number which turned on flow-control */
-	uint fc_xon;		/* Number which turned off flow-control */
-	uint rxglomfail;	/* Failed deglom attempts */
-	uint rxglomframes;	/* Number of glom frames (superframes) */
-	uint rxglompkts;	/* Number of packets from glom frames */
-	uint f2rxhdrs;		/* Number of header reads */
-	uint f2rxdata;		/* Number of frame data reads */
-	uint f2txdata;		/* Number of f2 frame writes */
-	uint f1regdata;		/* Number of f1 register accesses */
-	uint tickcnt;		/* Number of watchdog been schedule */
-	unsigned long tx_ctlerrs;	/* Err of sending ctrl frames */
-	unsigned long tx_ctlpkts;	/* Ctrl frames sent to dongle */
-	unsigned long rx_ctlerrs;	/* Err of processing rx ctrl frames */
-	unsigned long rx_ctlpkts;	/* Ctrl frames processed from dongle */
-	unsigned long rx_readahead_cnt;	/* Number of packets where header
-					 * read-ahead was used. */
-
 	u8 *ctrl_frame_buf;
 	u32 ctrl_frame_len;
 	bool ctrl_frame_stat;
@@ -583,6 +581,7 @@ struct brcmf_sdio {
 	u32 fw_ptr;
 
 	bool txoff;		/* Transmit flow-controlled */
+	struct brcmf_sdio_count sdcnt;
 };
 
 /* clkstate */
@@ -945,7 +944,7 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 	if (ret == 0)
 		w_sdreg32(bus, SMB_INT_ACK,
 			  offsetof(struct sdpcmd_regs, tosbmailbox));
-	bus->f1regdata += 2;
+	bus->sdcnt.f1regdata += 2;
 
 	/* Dongle recomposed rx frames, accept them again */
 	if (hmb_data & HMB_DATA_NAKHANDLED) {
@@ -984,12 +983,12 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
 							HMB_DATA_FCDATA_SHIFT;
 
 		if (fcbits & ~bus->flowcontrol)
-			bus->fc_xoff++;
+			bus->sdcnt.fc_xoff++;
 
 		if (bus->flowcontrol & ~fcbits)
-			bus->fc_xon++;
+			bus->sdcnt.fc_xon++;
 
-		bus->fc_rcvd++;
+		bus->sdcnt.fc_rcvd++;
 		bus->flowcontrol = fcbits;
 	}
 
@@ -1021,7 +1020,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 
 	brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
 			 SFC_RF_TERM, &err);
-	bus->f1regdata++;
+	bus->sdcnt.f1regdata++;
 
 	/* Wait until the packet has been flushed (device/FIFO stable) */
 	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
@@ -1029,7 +1028,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 				      SBSDIO_FUNC1_RFRAMEBCHI, &err);
 		lo = brcmf_sdio_regrb(bus->sdiodev,
 				      SBSDIO_FUNC1_RFRAMEBCLO, &err);
-		bus->f1regdata += 2;
+		bus->sdcnt.f1regdata += 2;
 
 		if ((hi == 0) && (lo == 0))
 			break;
@@ -1047,11 +1046,11 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 		brcmf_dbg(INFO, "flush took %d iterations\n", 0xffff - retries);
 
 	if (rtx) {
-		bus->rxrtx++;
+		bus->sdcnt.rxrtx++;
 		err = w_sdreg32(bus, SMB_NAK,
 				offsetof(struct sdpcmd_regs, tosbmailbox));
 
-		bus->f1regdata++;
+		bus->sdcnt.f1regdata++;
 		if (err == 0)
 			bus->rxskip = true;
 	}
@@ -1243,7 +1242,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 				  dlen);
 			errcode = -1;
 		}
-		bus->f2rxdata++;
+		bus->sdcnt.f2rxdata++;
 
 		/* On failure, kill the superframe, allow a couple retries */
 		if (errcode < 0) {
@@ -1256,7 +1255,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 			} else {
 				bus->glomerr = 0;
 				brcmf_sdbrcm_rxfail(bus, true, false);
-				bus->rxglomfail++;
+				bus->sdcnt.rxglomfail++;
 				brcmf_sdbrcm_free_glom(bus);
 			}
 			return 0;
@@ -1312,7 +1311,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 		if (rxseq != seq) {
 			brcmf_dbg(INFO, "(superframe) rx_seq %d, expected %d\n",
 				  seq, rxseq);
-			bus->rx_badseq++;
+			bus->sdcnt.rx_badseq++;
 			rxseq = seq;
 		}
 
@@ -1376,7 +1375,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 			} else {
 				bus->glomerr = 0;
 				brcmf_sdbrcm_rxfail(bus, true, false);
-				bus->rxglomfail++;
+				bus->sdcnt.rxglomfail++;
 				brcmf_sdbrcm_free_glom(bus);
 			}
 			bus->nextlen = 0;
@@ -1402,7 +1401,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 			if (rxseq != seq) {
 				brcmf_dbg(GLOM, "rx_seq %d, expected %d\n",
 					  seq, rxseq);
-				bus->rx_badseq++;
+				bus->sdcnt.rx_badseq++;
 				rxseq = seq;
 			}
 			rxseq++;
@@ -1441,8 +1440,8 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 			down(&bus->sdsem);
 		}
 
-		bus->rxglomframes++;
-		bus->rxglompkts += bus->glom.qlen;
+		bus->sdcnt.rxglomframes++;
+		bus->sdcnt.rxglompkts += bus->glom.qlen;
 	}
 	return num;
 }
@@ -1526,7 +1525,7 @@ brcmf_sdbrcm_read_control(struct brcmf_sdio *bus, u8 *hdr, uint len, uint doff)
 		brcmf_dbg(ERROR, "%d-byte ctl frame (%d-byte ctl data) exceeds %d-byte limit\n",
 			  len, len - doff, bus->sdiodev->bus_if->maxctl);
 		bus->sdiodev->bus_if->dstats.rx_errors++;
-		bus->rx_toolong++;
+		bus->sdcnt.rx_toolong++;
 		brcmf_sdbrcm_rxfail(bus, false, false);
 		goto done;
 	}
@@ -1536,13 +1535,13 @@ brcmf_sdbrcm_read_control(struct brcmf_sdio *bus, u8 *hdr, uint len, uint doff)
 				bus->sdiodev->sbwad,
 				SDIO_FUNC_2,
 				F2SYNC, (bus->rxctl + BRCMF_FIRSTREAD), rdlen);
-	bus->f2rxdata++;
+	bus->sdcnt.f2rxdata++;
 
 	/* Control frame failures need retransmission */
 	if (sdret < 0) {
 		brcmf_dbg(ERROR, "read %d control bytes failed: %d\n",
 			  rdlen, sdret);
-		bus->rxc_errors++;
+		bus->sdcnt.rxc_errors++;
 		brcmf_sdbrcm_rxfail(bus, true, true);
 		goto done;
 	}
@@ -1589,7 +1588,7 @@ brcmf_alloc_pkt_and_read(struct brcmf_sdio *bus, u16 rdlen,
 	/* Read the entire frame */
 	sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
 				      SDIO_FUNC_2, F2SYNC, *pkt);
-	bus->f2rxdata++;
+	bus->sdcnt.f2rxdata++;
 
 	if (sdret < 0) {
 		brcmf_dbg(ERROR, "(nextlen): read %d bytes failed: %d\n",
@@ -1630,7 +1629,7 @@ brcmf_check_rxbuf(struct brcmf_sdio *bus, struct sk_buff *pkt, u8 *rxbuf,
 	if ((u16)~(*len ^ check)) {
 		brcmf_dbg(ERROR, "(nextlen): HW hdr error: nextlen/len/check 0x%04x/0x%04x/0x%04x\n",
 			  nextlen, *len, check);
-		bus->rx_badhdr++;
+		bus->sdcnt.rx_badhdr++;
 		brcmf_sdbrcm_rxfail(bus, false, false);
 		goto fail;
 	}
@@ -1746,7 +1745,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 				bus->nextlen = 0;
 			}
 
-			bus->rx_readahead_cnt++;
+			bus->sdcnt.rx_readahead_cnt++;
 
 			/* Handle Flow Control */
 			fcbits = SDPCM_FCMASK_VALUE(
@@ -1754,12 +1753,12 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 
 			if (bus->flowcontrol != fcbits) {
 				if (~bus->flowcontrol & fcbits)
-					bus->fc_xoff++;
+					bus->sdcnt.fc_xoff++;
 
 				if (bus->flowcontrol & ~fcbits)
-					bus->fc_xon++;
+					bus->sdcnt.fc_xon++;
 
-				bus->fc_rcvd++;
+				bus->sdcnt.fc_rcvd++;
 				bus->flowcontrol = fcbits;
 			}
 
@@ -1767,7 +1766,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 			if (rxseq != seq) {
 				brcmf_dbg(INFO, "(nextlen): rx_seq %d, expected %d\n",
 					  seq, rxseq);
-				bus->rx_badseq++;
+				bus->sdcnt.rx_badseq++;
 				rxseq = seq;
 			}
 
@@ -1814,11 +1813,11 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 		sdret = brcmf_sdcard_recv_buf(bus->sdiodev, bus->sdiodev->sbwad,
 					      SDIO_FUNC_2, F2SYNC, bus->rxhdr,
 					      BRCMF_FIRSTREAD);
-		bus->f2rxhdrs++;
+		bus->sdcnt.f2rxhdrs++;
 
 		if (sdret < 0) {
 			brcmf_dbg(ERROR, "RXHEADER FAILED: %d\n", sdret);
-			bus->rx_hdrfail++;
+			bus->sdcnt.rx_hdrfail++;
 			brcmf_sdbrcm_rxfail(bus, true, true);
 			continue;
 		}
@@ -1840,7 +1839,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 		if ((u16) ~(len ^ check)) {
 			brcmf_dbg(ERROR, "HW hdr err: len/check 0x%04x/0x%04x\n",
 				  len, check);
-			bus->rx_badhdr++;
+			bus->sdcnt.rx_badhdr++;
 			brcmf_sdbrcm_rxfail(bus, false, false);
 			continue;
 		}
@@ -1861,7 +1860,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 		if ((doff < SDPCM_HDRLEN) || (doff > len)) {
 			brcmf_dbg(ERROR, "Bad data offset %d: HW len %d, min %d seq %d\n",
 				  doff, len, SDPCM_HDRLEN, seq);
-			bus->rx_badhdr++;
+			bus->sdcnt.rx_badhdr++;
 			brcmf_sdbrcm_rxfail(bus, false, false);
 			continue;
 		}
@@ -1880,19 +1879,19 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 
 		if (bus->flowcontrol != fcbits) {
 			if (~bus->flowcontrol & fcbits)
-				bus->fc_xoff++;
+				bus->sdcnt.fc_xoff++;
 
 			if (bus->flowcontrol & ~fcbits)
-				bus->fc_xon++;
+				bus->sdcnt.fc_xon++;
 
-			bus->fc_rcvd++;
+			bus->sdcnt.fc_rcvd++;
 			bus->flowcontrol = fcbits;
 		}
 
 		/* Check and update sequence number */
 		if (rxseq != seq) {
 			brcmf_dbg(INFO, "rx_seq %d, expected %d\n", seq, rxseq);
-			bus->rx_badseq++;
+			bus->sdcnt.rx_badseq++;
 			rxseq = seq;
 		}
 
@@ -1937,7 +1936,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 			brcmf_dbg(ERROR, "too long: len %d rdlen %d\n",
 				  len, rdlen);
 			bus->sdiodev->bus_if->dstats.rx_errors++;
-			bus->rx_toolong++;
+			bus->sdcnt.rx_toolong++;
 			brcmf_sdbrcm_rxfail(bus, false, false);
 			continue;
 		}
@@ -1960,7 +1959,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 		/* Read the remaining frame data */
 		sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
 					      SDIO_FUNC_2, F2SYNC, pkt);
-		bus->f2rxdata++;
+		bus->sdcnt.f2rxdata++;
 
 		if (sdret < 0) {
 			brcmf_dbg(ERROR, "read %d %s bytes failed: %d\n", rdlen,
@@ -2147,18 +2146,18 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 
 	ret = brcmf_sdcard_send_pkt(bus->sdiodev, bus->sdiodev->sbwad,
 				    SDIO_FUNC_2, F2SYNC, pkt);
-	bus->f2txdata++;
+	bus->sdcnt.f2txdata++;
 
 	if (ret < 0) {
 		/* On failure, abort the command and terminate the frame */
 		brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
 			  ret);
-		bus->tx_sderrs++;
+		bus->sdcnt.tx_sderrs++;
 
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
 				 SFC_WF_TERM, NULL);
-		bus->f1regdata++;
+		bus->sdcnt.f1regdata++;
 
 		for (i = 0; i < 3; i++) {
 			u8 hi, lo;
@@ -2166,7 +2165,7 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 					      SBSDIO_FUNC1_WFRAMEBCHI, NULL);
 			lo = brcmf_sdio_regrb(bus->sdiodev,
 					      SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-			bus->f1regdata += 2;
+			bus->sdcnt.f1regdata += 2;
 			if ((hi == 0) && (lo == 0))
 				break;
 		}
@@ -2224,7 +2223,7 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
 			ret = r_sdreg32(bus, &intstatus,
 					offsetof(struct sdpcmd_regs,
 						 intstatus));
-			bus->f2txdata++;
+			bus->sdcnt.f2txdata++;
 			if (ret != 0)
 				break;
 			if (intstatus & bus->hostintmask)
@@ -2417,7 +2416,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 		bus->ipend = false;
 		err = r_sdreg32(bus, &newstatus,
 				offsetof(struct sdpcmd_regs, intstatus));
-		bus->f1regdata++;
+		bus->sdcnt.f1regdata++;
 		if (err != 0)
 			newstatus = 0;
 		newstatus &= bus->hostintmask;
@@ -2426,7 +2425,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 			err = w_sdreg32(bus, newstatus,
 					offsetof(struct sdpcmd_regs,
 						 intstatus));
-			bus->f1regdata++;
+			bus->sdcnt.f1regdata++;
 		}
 	}
 
@@ -2445,7 +2444,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 
 		err = r_sdreg32(bus, &newstatus,
 				offsetof(struct sdpcmd_regs, intstatus));
-		bus->f1regdata += 2;
+		bus->sdcnt.f1regdata += 2;
 		bus->fcstate =
 		    !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
 		intstatus |= (newstatus & bus->hostintmask);
@@ -2502,7 +2501,7 @@ clkwait:
 		int ret, i;
 
 		ret = brcmf_sdcard_send_buf(bus->sdiodev, bus->sdiodev->sbwad,
-			SDIO_FUNC_2, F2SYNC, (u8 *) bus->ctrl_frame_buf,
+			SDIO_FUNC_2, F2SYNC, bus->ctrl_frame_buf,
 			(u32) bus->ctrl_frame_len);
 
 		if (ret < 0) {
@@ -2510,13 +2509,13 @@ clkwait:
 				terminate the frame */
 			brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
 				  ret);
-			bus->tx_sderrs++;
+			bus->sdcnt.tx_sderrs++;
 
 			brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
 			brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
 					 SFC_WF_TERM, &err);
-			bus->f1regdata++;
+			bus->sdcnt.f1regdata++;
 
 			for (i = 0; i < 3; i++) {
 				u8 hi, lo;
@@ -2526,7 +2525,7 @@ clkwait:
 				lo = brcmf_sdio_regrb(bus->sdiodev,
 						      SBSDIO_FUNC1_WFRAMEBCLO,
 						      &err);
-				bus->f1regdata += 2;
+				bus->sdcnt.f1regdata += 2;
 				if ((hi == 0) && (lo == 0))
 					break;
 			}
@@ -2657,7 +2656,7 @@ static int brcmf_sdbrcm_bus_txdata(struct device *dev, struct sk_buff *pkt)
 	/* Check for existing queue, current flow-control,
 			 pending event, or pending clock */
 	brcmf_dbg(TRACE, "deferring pktq len %d\n", pktq_len(&bus->txq));
-	bus->fcqueued++;
+	bus->sdcnt.fcqueued++;
 
 	/* Priority based enq */
 	spin_lock_bh(&bus->txqlock);
@@ -2845,13 +2844,13 @@ static int brcmf_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
 		/* On failure, abort the command and terminate the frame */
 		brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
 			  ret);
-		bus->tx_sderrs++;
+		bus->sdcnt.tx_sderrs++;
 
 		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
 		brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
 				 SFC_WF_TERM, NULL);
-		bus->f1regdata++;
+		bus->sdcnt.f1regdata++;
 
 		for (i = 0; i < 3; i++) {
 			u8 hi, lo;
@@ -2859,7 +2858,7 @@ static int brcmf_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
 					      SBSDIO_FUNC1_WFRAMEBCHI, NULL);
 			lo = brcmf_sdio_regrb(bus->sdiodev,
 					      SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-			bus->f1regdata += 2;
+			bus->sdcnt.f1regdata += 2;
 			if (hi == 0 && lo == 0)
 				break;
 		}
@@ -2976,13 +2975,324 @@ brcmf_sdbrcm_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 	up(&bus->sdsem);
 
 	if (ret)
-		bus->tx_ctlerrs++;
+		bus->sdcnt.tx_ctlerrs++;
 	else
-		bus->tx_ctlpkts++;
+		bus->sdcnt.tx_ctlpkts++;
 
 	return ret ? -EIO : 0;
 }
 
+#ifdef DEBUG
+static inline bool brcmf_sdio_valid_shared_address(u32 addr)
+{
+	return !(addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff));
+}
+
+static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
+				 struct sdpcm_shared *sh)
+{
+	u32 addr;
+	int rv;
+	u32 shaddr = 0;
+	struct sdpcm_shared_le sh_le;
+	__le32 addr_le;
+
+	shaddr = bus->ramsize - 4;
+
+	/*
+	 * Read last word in socram to determine
+	 * address of sdpcm_shared structure
+	 */
+	rv = brcmf_sdbrcm_membytes(bus, false, shaddr,
+				   (u8 *)&addr_le, 4);
+	if (rv < 0)
+		return rv;
+
+	addr = le32_to_cpu(addr_le);
+
+	brcmf_dbg(INFO, "sdpcm_shared address 0x%08X\n", addr);
+
+	/*
+	 * Check if addr is valid.
+	 * NVRAM length at the end of memory should have been overwritten.
+	 */
+	if (!brcmf_sdio_valid_shared_address(addr)) {
+			brcmf_dbg(ERROR, "invalid sdpcm_shared address 0x%08X\n",
+				  addr);
+			return -EINVAL;
+	}
+
+	/* Read hndrte_shared structure */
+	rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *)&sh_le,
+				   sizeof(struct sdpcm_shared_le));
+	if (rv < 0)
+		return rv;
+
+	/* Endianness */
+	sh->flags = le32_to_cpu(sh_le.flags);
+	sh->trap_addr = le32_to_cpu(sh_le.trap_addr);
+	sh->assert_exp_addr = le32_to_cpu(sh_le.assert_exp_addr);
+	sh->assert_file_addr = le32_to_cpu(sh_le.assert_file_addr);
+	sh->assert_line = le32_to_cpu(sh_le.assert_line);
+	sh->console_addr = le32_to_cpu(sh_le.console_addr);
+	sh->msgtrace_addr = le32_to_cpu(sh_le.msgtrace_addr);
+
+	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
+		brcmf_dbg(ERROR,
+			  "sdpcm_shared version mismatch: dhd %d dongle %d\n",
+			  SDPCM_SHARED_VERSION,
+			  sh->flags & SDPCM_SHARED_VERSION_MASK);
+		return -EPROTO;
+	}
+
+	return 0;
+}
+
+static int brcmf_sdio_dump_console(struct brcmf_sdio *bus,
+				   struct sdpcm_shared *sh, char __user *data,
+				   size_t count)
+{
+	u32 addr, console_ptr, console_size, console_index;
+	char *conbuf = NULL;
+	__le32 sh_val;
+	int rv;
+	loff_t pos = 0;
+	int nbytes = 0;
+
+	/* obtain console information from device memory */
+	addr = sh->console_addr + offsetof(struct rte_console, log_le);
+	rv = brcmf_sdbrcm_membytes(bus, false, addr,
+			(u8 *)&sh_val, sizeof(u32));
+	if (rv < 0)
+		return rv;
+	console_ptr = le32_to_cpu(sh_val);
+
+	addr = sh->console_addr + offsetof(struct rte_console, log_le.buf_size);
+	rv = brcmf_sdbrcm_membytes(bus, false, addr,
+			(u8 *)&sh_val, sizeof(u32));
+	if (rv < 0)
+		return rv;
+	console_size = le32_to_cpu(sh_val);
+
+	addr = sh->console_addr + offsetof(struct rte_console, log_le.idx);
+	rv = brcmf_sdbrcm_membytes(bus, false, addr,
+			(u8 *)&sh_val, sizeof(u32));
+	if (rv < 0)
+		return rv;
+	console_index = le32_to_cpu(sh_val);
+
+	/* allocate buffer for console data */
+	if (console_size <= CONSOLE_BUFFER_MAX)
+		conbuf = vzalloc(console_size+1);
+
+	if (!conbuf)
+		return -ENOMEM;
+
+	/* obtain the console data from device */
+	conbuf[console_size] = '\0';
+	rv = brcmf_sdbrcm_membytes(bus, false, console_ptr, (u8 *)conbuf,
+				   console_size);
+	if (rv < 0)
+		goto done;
+
+	rv = simple_read_from_buffer(data, count, &pos,
+				     conbuf + console_index,
+				     console_size - console_index);
+	if (rv < 0)
+		goto done;
+
+	nbytes = rv;
+	if (console_index > 0) {
+		pos = 0;
+		rv = simple_read_from_buffer(data+nbytes, count, &pos,
+					     conbuf, console_index - 1);
+		if (rv < 0)
+			goto done;
+		rv += nbytes;
+	}
+done:
+	vfree(conbuf);
+	return rv;
+}
+
+static int brcmf_sdio_trap_info(struct brcmf_sdio *bus, struct sdpcm_shared *sh,
+				char __user *data, size_t count)
+{
+	int error, res;
+	char buf[350];
+	struct brcmf_trap_info tr;
+	int nbytes;
+	loff_t pos = 0;
+
+	if ((sh->flags & SDPCM_SHARED_TRAP) == 0)
+		return 0;
+
+	error = brcmf_sdbrcm_membytes(bus, false, sh->trap_addr, (u8 *)&tr,
+				      sizeof(struct brcmf_trap_info));
+	if (error < 0)
+		return error;
+
+	nbytes = brcmf_sdio_dump_console(bus, sh, data, count);
+	if (nbytes < 0)
+		return nbytes;
+
+	res = scnprintf(buf, sizeof(buf),
+			"dongle trap info: type 0x%x @ epc 0x%08x\n"
+			"  cpsr 0x%08x spsr 0x%08x sp 0x%08x\n"
+			"  lr   0x%08x pc   0x%08x offset 0x%x\n"
+			"  r0   0x%08x r1   0x%08x r2 0x%08x r3 0x%08x\n"
+			"  r4   0x%08x r5   0x%08x r6 0x%08x r7 0x%08x\n",
+			le32_to_cpu(tr.type), le32_to_cpu(tr.epc),
+			le32_to_cpu(tr.cpsr), le32_to_cpu(tr.spsr),
+			le32_to_cpu(tr.r13), le32_to_cpu(tr.r14),
+			le32_to_cpu(tr.pc), sh->trap_addr,
+			le32_to_cpu(tr.r0), le32_to_cpu(tr.r1),
+			le32_to_cpu(tr.r2), le32_to_cpu(tr.r3),
+			le32_to_cpu(tr.r4), le32_to_cpu(tr.r5),
+			le32_to_cpu(tr.r6), le32_to_cpu(tr.r7));
+
+	error = simple_read_from_buffer(data+nbytes, count, &pos, buf, res);
+	if (error < 0)
+		return error;
+
+	nbytes += error;
+	return nbytes;
+}
+
+static int brcmf_sdio_assert_info(struct brcmf_sdio *bus,
+				  struct sdpcm_shared *sh, char __user *data,
+				  size_t count)
+{
+	int error = 0;
+	char buf[200];
+	char file[80] = "?";
+	char expr[80] = "<???>";
+	int res;
+	loff_t pos = 0;
+
+	if ((sh->flags & SDPCM_SHARED_ASSERT_BUILT) == 0) {
+		brcmf_dbg(INFO, "firmware not built with -assert\n");
+		return 0;
+	} else if ((sh->flags & SDPCM_SHARED_ASSERT) == 0) {
+		brcmf_dbg(INFO, "no assert in dongle\n");
+		return 0;
+	}
+
+	if (sh->assert_file_addr != 0) {
+		error = brcmf_sdbrcm_membytes(bus, false, sh->assert_file_addr,
+					      (u8 *)file, 80);
+		if (error < 0)
+			return error;
+	}
+	if (sh->assert_exp_addr != 0) {
+		error = brcmf_sdbrcm_membytes(bus, false, sh->assert_exp_addr,
+					      (u8 *)expr, 80);
+		if (error < 0)
+			return error;
+	}
+
+	res = scnprintf(buf, sizeof(buf),
+			"dongle assert: %s:%d: assert(%s)\n",
+			file, sh->assert_line, expr);
+	return simple_read_from_buffer(data, count, &pos, buf, res);
+}
+
+static int brcmf_sdbrcm_checkdied(struct brcmf_sdio *bus)
+{
+	int error;
+	struct sdpcm_shared sh;
+
+	down(&bus->sdsem);
+	error = brcmf_sdio_readshared(bus, &sh);
+	up(&bus->sdsem);
+
+	if (error < 0)
+		return error;
+
+	if ((sh.flags & SDPCM_SHARED_ASSERT_BUILT) == 0)
+		brcmf_dbg(INFO, "firmware not built with -assert\n");
+	else if (sh.flags & SDPCM_SHARED_ASSERT)
+		brcmf_dbg(ERROR, "assertion in dongle\n");
+
+	if (sh.flags & SDPCM_SHARED_TRAP)
+		brcmf_dbg(ERROR, "firmware trap in dongle\n");
+
+	return 0;
+}
+
+static int brcmf_sdbrcm_died_dump(struct brcmf_sdio *bus, char __user *data,
+				  size_t count, loff_t *ppos)
+{
+	int error = 0;
+	struct sdpcm_shared sh;
+	int nbytes = 0;
+	loff_t pos = *ppos;
+
+	if (pos != 0)
+		return 0;
+
+	down(&bus->sdsem);
+	error = brcmf_sdio_readshared(bus, &sh);
+	if (error < 0)
+		goto done;
+
+	error = brcmf_sdio_assert_info(bus, &sh, data, count);
+	if (error < 0)
+		goto done;
+
+	nbytes = error;
+	error = brcmf_sdio_trap_info(bus, &sh, data, count);
+	if (error < 0)
+		goto done;
+
+	error += nbytes;
+	*ppos += error;
+done:
+	up(&bus->sdsem);
+	return error;
+}
+
+static ssize_t brcmf_sdio_forensic_read(struct file *f, char __user *data,
+					size_t count, loff_t *ppos)
+{
+	struct brcmf_sdio *bus = f->private_data;
+	int res;
+
+	res = brcmf_sdbrcm_died_dump(bus, data, count, ppos);
+	if (res > 0)
+		*ppos += res;
+	return (ssize_t)res;
+}
+
+static const struct file_operations brcmf_sdio_forensic_ops = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = brcmf_sdio_forensic_read
+};
+
+static void brcmf_sdio_debugfs_create(struct brcmf_sdio *bus)
+{
+	struct brcmf_pub *drvr = bus->sdiodev->bus_if->drvr;
+	struct dentry *dentry = brcmf_debugfs_get_devdir(drvr);
+
+	if (IS_ERR_OR_NULL(dentry))
+		return;
+
+	debugfs_create_file("forensics", S_IRUGO, dentry, bus,
+			    &brcmf_sdio_forensic_ops);
+	brcmf_debugfs_create_sdio_count(drvr, &bus->sdcnt);
+}
+#else
+static int brcmf_sdbrcm_checkdied(struct brcmf_sdio *bus)
+{
+	return 0;
+}
+
+static void brcmf_sdio_debugfs_create(struct brcmf_sdio *bus)
+{
+}
+#endif /* DEBUG */
+
 static int
 brcmf_sdbrcm_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
 {
@@ -3009,60 +3319,27 @@ brcmf_sdbrcm_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
 			  rxlen, msglen);
 	} else if (timeleft == 0) {
 		brcmf_dbg(ERROR, "resumed on timeout\n");
+		brcmf_sdbrcm_checkdied(bus);
 	} else if (pending) {
 		brcmf_dbg(CTL, "cancelled\n");
 		return -ERESTARTSYS;
 	} else {
 		brcmf_dbg(CTL, "resumed for unknown reason?\n");
+		brcmf_sdbrcm_checkdied(bus);
 	}
 
 	if (rxlen)
-		bus->rx_ctlpkts++;
+		bus->sdcnt.rx_ctlpkts++;
 	else
-		bus->rx_ctlerrs++;
+		bus->sdcnt.rx_ctlerrs++;
 
 	return rxlen ? (int)rxlen : -ETIMEDOUT;
 }
 
-static int brcmf_sdbrcm_downloadvars(struct brcmf_sdio *bus, void *arg, int len)
-{
-	int bcmerror = 0;
-
-	brcmf_dbg(TRACE, "Enter\n");
-
-	/* Basic sanity checks */
-	if (bus->sdiodev->bus_if->drvr_up) {
-		bcmerror = -EISCONN;
-		goto err;
-	}
-	if (!len) {
-		bcmerror = -EOVERFLOW;
-		goto err;
-	}
-
-	/* Free the old ones and replace with passed variables */
-	kfree(bus->vars);
-
-	bus->vars = kmalloc(len, GFP_ATOMIC);
-	bus->varsz = bus->vars ? len : 0;
-	if (bus->vars == NULL) {
-		bcmerror = -ENOMEM;
-		goto err;
-	}
-
-	/* Copy the passed variables, which should include the
-		 terminating double-null */
-	memcpy(bus->vars, arg, bus->varsz);
-err:
-	return bcmerror;
-}
-
 static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 {
 	int bcmerror = 0;
-	u32 varsize;
 	u32 varaddr;
-	u8 *vbuffer;
 	u32 varsizew;
 	__le32 varsizew_le;
 #ifdef DEBUG
@@ -3071,56 +3348,44 @@ static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 
 	/* Even if there are no vars are to be written, we still
 		 need to set the ramsize. */
-	varsize = bus->varsz ? roundup(bus->varsz, 4) : 0;
-	varaddr = (bus->ramsize - 4) - varsize;
+	varaddr = (bus->ramsize - 4) - bus->varsz;
 
 	if (bus->vars) {
-		vbuffer = kzalloc(varsize, GFP_ATOMIC);
-		if (!vbuffer)
-			return -ENOMEM;
-
-		memcpy(vbuffer, bus->vars, bus->varsz);
-
 		/* Write the vars list */
-		bcmerror =
-		    brcmf_sdbrcm_membytes(bus, true, varaddr, vbuffer, varsize);
+		bcmerror = brcmf_sdbrcm_membytes(bus, true, varaddr,
+						 bus->vars, bus->varsz);
 #ifdef DEBUG
 		/* Verify NVRAM bytes */
-		brcmf_dbg(INFO, "Compare NVRAM dl & ul; varsize=%d\n", varsize);
-		nvram_ularray = kmalloc(varsize, GFP_ATOMIC);
-		if (!nvram_ularray) {
-			kfree(vbuffer);
+		brcmf_dbg(INFO, "Compare NVRAM dl & ul; varsize=%d\n",
+			  bus->varsz);
+		nvram_ularray = kmalloc(bus->varsz, GFP_ATOMIC);
+		if (!nvram_ularray)
 			return -ENOMEM;
-		}
 
 		/* Upload image to verify downloaded contents. */
-		memset(nvram_ularray, 0xaa, varsize);
+		memset(nvram_ularray, 0xaa, bus->varsz);
 
 		/* Read the vars list to temp buffer for comparison */
-		bcmerror =
-		    brcmf_sdbrcm_membytes(bus, false, varaddr, nvram_ularray,
-				     varsize);
+		bcmerror = brcmf_sdbrcm_membytes(bus, false, varaddr,
+						 nvram_ularray, bus->varsz);
 		if (bcmerror) {
 			brcmf_dbg(ERROR, "error %d on reading %d nvram bytes at 0x%08x\n",
-				  bcmerror, varsize, varaddr);
+				  bcmerror, bus->varsz, varaddr);
 		}
 		/* Compare the org NVRAM with the one read from RAM */
-		if (memcmp(vbuffer, nvram_ularray, varsize))
+		if (memcmp(bus->vars, nvram_ularray, bus->varsz))
 			brcmf_dbg(ERROR, "Downloaded NVRAM image is corrupted\n");
 		else
 			brcmf_dbg(ERROR, "Download/Upload/Compare of NVRAM ok\n");
 
 		kfree(nvram_ularray);
 #endif				/* DEBUG */
-
-		kfree(vbuffer);
 	}
 
 	/* adjust to the user specified RAM */
 	brcmf_dbg(INFO, "Physical memory size: %d\n", bus->ramsize);
 	brcmf_dbg(INFO, "Vars are at %d, orig varsize is %d\n",
-		  varaddr, varsize);
-	varsize = ((bus->ramsize - 4) - varaddr);
+		  varaddr, bus->varsz);
 
 	/*
 	 * Determine the length token:
@@ -3131,13 +3396,13 @@ static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 		varsizew = 0;
 		varsizew_le = cpu_to_le32(0);
 	} else {
-		varsizew = varsize / 4;
+		varsizew = bus->varsz / 4;
 		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
 		varsizew_le = cpu_to_le32(varsizew);
 	}
 
 	brcmf_dbg(INFO, "New varsize is %d, length token=0x%08x\n",
-		  varsize, varsizew);
+		  bus->varsz, varsizew);
 
 	/* Write the length token to the last word */
 	bcmerror = brcmf_sdbrcm_membytes(bus, true, (bus->ramsize - 4),
@@ -3261,13 +3526,21 @@ err:
  * by two NULs.
 */
 
-static uint brcmf_process_nvram_vars(char *varbuf, uint len)
+static int brcmf_process_nvram_vars(struct brcmf_sdio *bus)
 {
+	char *varbuf;
 	char *dp;
 	bool findNewline;
 	int column;
-	uint buf_len, n;
+	int ret = 0;
+	uint buf_len, n, len;
 
+	len = bus->firmware->size;
+	varbuf = vmalloc(len);
+	if (!varbuf)
+		return -ENOMEM;
+
+	memcpy(varbuf, bus->firmware->data, len);
 	dp = varbuf;
 
 	findNewline = false;
@@ -3296,56 +3569,44 @@ static uint brcmf_process_nvram_vars(char *varbuf, uint len)
 		column++;
 	}
 	buf_len = dp - varbuf;
-
 	while (dp < varbuf + n)
 		*dp++ = 0;
 
-	return buf_len;
+	kfree(bus->vars);
+	/* roundup needed for download to device */
+	bus->varsz = roundup(buf_len + 1, 4);
+	bus->vars = kmalloc(bus->varsz, GFP_KERNEL);
+	if (bus->vars == NULL) {
+		bus->varsz = 0;
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* copy the processed variables and add null termination */
+	memcpy(bus->vars, varbuf, buf_len);
+	bus->vars[buf_len] = 0;
+err:
+	vfree(varbuf);
+	return ret;
 }
 
 static int brcmf_sdbrcm_download_nvram(struct brcmf_sdio *bus)
 {
-	uint len;
-	char *memblock = NULL;
-	char *bufp;
 	int ret;
 
+	if (bus->sdiodev->bus_if->drvr_up)
+		return -EISCONN;
+
 	ret = request_firmware(&bus->firmware, BRCMF_SDIO_NV_NAME,
 			       &bus->sdiodev->func[2]->dev);
 	if (ret) {
 		brcmf_dbg(ERROR, "Fail to request nvram %d\n", ret);
 		return ret;
 	}
-	bus->fw_ptr = 0;
-
-	memblock = kmalloc(MEMBLOCK, GFP_ATOMIC);
-	if (memblock == NULL) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	len = brcmf_sdbrcm_get_image(memblock, MEMBLOCK, bus);
-
-	if (len > 0 && len < MEMBLOCK) {
-		bufp = (char *)memblock;
-		bufp[len] = 0;
-		len = brcmf_process_nvram_vars(bufp, len);
-		bufp += len;
-		*bufp++ = 0;
-		if (len)
-			ret = brcmf_sdbrcm_downloadvars(bus, memblock, len + 1);
-		if (ret)
-			brcmf_dbg(ERROR, "error downloading vars: %d\n", ret);
-	} else {
-		brcmf_dbg(ERROR, "error reading nvram file: %d\n", len);
-		ret = -EIO;
-	}
 
-err:
-	kfree(memblock);
+	ret = brcmf_process_nvram_vars(bus);
 
 	release_firmware(bus->firmware);
-	bus->fw_ptr = 0;
 
 	return ret;
 }
@@ -3419,7 +3680,7 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
 		return 0;
 
 	/* Start the watchdog timer */
-	bus->tickcnt = 0;
+	bus->sdcnt.tickcnt = 0;
 	brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 
 	down(&bus->sdsem);
@@ -3512,7 +3773,7 @@ void brcmf_sdbrcm_isr(void *arg)
 		return;
 	}
 	/* Count the interrupt call */
-	bus->intrcount++;
+	bus->sdcnt.intrcount++;
 	bus->ipend = true;
 
 	/* Shouldn't get this interrupt if we're sleeping? */
@@ -3554,7 +3815,8 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 		bus->polltick = 0;
 
 		/* Check device if no interrupts */
-		if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
+		if (!bus->intr ||
+		    (bus->sdcnt.intrcount == bus->sdcnt.lastintrs)) {
 
 			if (!bus->dpc_sched) {
 				u8 devpend;
@@ -3569,7 +3831,7 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 			/* If there is something, make like the ISR and
 				 schedule the DPC */
 			if (intstatus) {
-				bus->pollcnt++;
+				bus->sdcnt.pollcnt++;
 				bus->ipend = true;
 
 				bus->dpc_sched = true;
@@ -3581,7 +3843,7 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
 		}
 
 		/* Update interrupt tracking */
-		bus->lastintrs = bus->intrcount;
+		bus->sdcnt.lastintrs = bus->sdcnt.intrcount;
 	}
 #ifdef DEBUG
 	/* Poll for console output periodically */
@@ -3623,6 +3885,8 @@ static bool brcmf_sdbrcm_chipmatch(u16 chipid)
 		return true;
 	if (chipid == BCM4330_CHIP_ID)
 		return true;
+	if (chipid == BCM4334_CHIP_ID)
+		return true;
 	return false;
 }
 
@@ -3793,7 +4057,7 @@ brcmf_sdbrcm_watchdog_thread(void *data)
 		if (!wait_for_completion_interruptible(&bus->watchdog_wait)) {
 			brcmf_sdbrcm_bus_watchdog(bus);
 			/* Count the tick for reference */
-			bus->tickcnt++;
+			bus->sdcnt.tickcnt++;
 		} else
 			break;
 	}
@@ -3856,6 +4120,10 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
 {
 	int ret;
 	struct brcmf_sdio *bus;
+	struct brcmf_bus_dcmd *dlst;
+	u32 dngl_txglom;
+	u32 dngl_txglomalign;
+	u8 idx;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
@@ -3938,8 +4206,29 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
 		goto fail;
 	}
 
+	brcmf_sdio_debugfs_create(bus);
 	brcmf_dbg(INFO, "completed!!\n");
 
+	/* sdio bus core specific dcmd */
+	idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
+	dlst = kzalloc(sizeof(struct brcmf_bus_dcmd), GFP_KERNEL);
+	if (dlst) {
+		if (bus->ci->c_inf[idx].rev < 12) {
+			/* for sdio core rev < 12, disable txgloming */
+			dngl_txglom = 0;
+			dlst->name = "bus:txglom";
+			dlst->param = (char *)&dngl_txglom;
+			dlst->param_len = sizeof(u32);
+		} else {
+			/* otherwise, set txglomalign */
+			dngl_txglomalign = bus->sdiodev->bus_if->align;
+			dlst->name = "bus:txglomalign";
+			dlst->param = (char *)&dngl_txglomalign;
+			dlst->param_len = sizeof(u32);
+		}
+		list_add(&dlst->list, &bus->sdiodev->bus_if->dcmd_list);
+	}
+
 	/* if firmware path present try to download and bring up bus */
 	ret = brcmf_bus_start(bus->sdiodev->dev);
 	if (ret != 0) {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
index f8e1f1c84d0..58155e23d22 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
@@ -403,6 +403,23 @@ static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
 		ci->c_inf[3].cib = 0x03004211;
 		ci->ramsize = 0x48000;
 		break;
+	case BCM4334_CHIP_ID:
+		ci->c_inf[0].wrapbase = 0x18100000;
+		ci->c_inf[0].cib = 0x29004211;
+		ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
+		ci->c_inf[1].base = 0x18002000;
+		ci->c_inf[1].wrapbase = 0x18102000;
+		ci->c_inf[1].cib = 0x0d004211;
+		ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
+		ci->c_inf[2].base = 0x18004000;
+		ci->c_inf[2].wrapbase = 0x18104000;
+		ci->c_inf[2].cib = 0x13080401;
+		ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
+		ci->c_inf[3].base = 0x18003000;
+		ci->c_inf[3].wrapbase = 0x18103000;
+		ci->c_inf[3].cib = 0x07004211;
+		ci->ramsize = 0x80000;
+		break;
 	default:
 		brcmf_dbg(ERROR, "chipid 0x%x is not supported\n", ci->chip);
 		return -ENODEV;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index d13ae9c299f..28c5fbb4af2 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -691,9 +691,10 @@ scan_out:
 }
 
 static s32
-brcmf_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
+brcmf_cfg80211_scan(struct wiphy *wiphy,
 		 struct cfg80211_scan_request *request)
 {
+	struct net_device *ndev = request->wdev->netdev;
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
@@ -919,9 +920,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 	set_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
 
 	if (params->bssid)
-		WL_CONN("BSSID: %02X %02X %02X %02X %02X %02X\n",
-		params->bssid[0], params->bssid[1], params->bssid[2],
-		params->bssid[3], params->bssid[4], params->bssid[5]);
+		WL_CONN("BSSID: %pM\n", params->bssid);
 	else
 		WL_CONN("No BSSID specified\n");
 
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
index 6d8b7213643..8c9345dd37d 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
@@ -318,10 +318,6 @@
 #define	IS_SIM(chippkg)	\
 	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
 
-#define PCIE(sih)	(ai_get_buscoretype(sih) == PCIE_CORE_ID)
-
-#define PCI_FORCEHT(sih) (PCIE(sih) && (ai_get_chip_id(sih) == BCM4716_CHIP_ID))
-
 #ifdef DEBUG
 #define	SI_MSG(fmt, ...)	pr_debug(fmt, ##__VA_ARGS__)
 #else
@@ -473,9 +469,6 @@ ai_buscore_setup(struct si_info *sii, struct bcma_device *cc)
 		sii->pub.pmurev = sii->pub.pmucaps & PCAP_REV_MASK;
 	}
 
-	/* figure out buscore */
-	sii->buscore = ai_findcore(&sii->pub, PCIE_CORE_ID, 0);
-
 	return true;
 }
 
@@ -483,11 +476,7 @@ static struct si_info *ai_doattach(struct si_info *sii,
 				   struct bcma_bus *pbus)
 {
 	struct si_pub *sih = &sii->pub;
-	u32 w, savewin;
 	struct bcma_device *cc;
-	struct ssb_sprom *sprom = &pbus->sprom;
-
-	savewin = 0;
 
 	sii->icbus = pbus;
 	sii->pcibus = pbus->host_pci;
@@ -510,47 +499,7 @@ static struct si_info *ai_doattach(struct si_info *sii,
 
 	/* PMU specific initializations */
 	if (ai_get_cccaps(sih) & CC_CAP_PMU) {
-		si_pmu_init(sih);
 		(void)si_pmu_measure_alpclk(sih);
-		si_pmu_res_init(sih);
-	}
-
-	/* setup the GPIO based LED powersave register */
-	w = (sprom->leddc_on_time << BCMA_CC_GPIOTIMER_ONTIME_SHIFT) |
-		 (sprom->leddc_off_time << BCMA_CC_GPIOTIMER_OFFTIME_SHIFT);
-	if (w == 0)
-		w = DEFAULT_GPIOTIMERVAL;
-	ai_cc_reg(sih, offsetof(struct chipcregs, gpiotimerval),
-		  ~0, w);
-
-	if (ai_get_chip_id(sih) == BCM43224_CHIP_ID) {
-		/*
-		 * enable 12 mA drive strenth for 43224 and
-		 * set chipControl register bit 15
-		 */
-		if (ai_get_chiprev(sih) == 0) {
-			SI_MSG("Applying 43224A0 WARs\n");
-			ai_cc_reg(sih, offsetof(struct chipcregs, chipcontrol),
-				  CCTRL43224_GPIO_TOGGLE,
-				  CCTRL43224_GPIO_TOGGLE);
-			si_pmu_chipcontrol(sih, 0, CCTRL_43224A0_12MA_LED_DRIVE,
-					   CCTRL_43224A0_12MA_LED_DRIVE);
-		}
-		if (ai_get_chiprev(sih) >= 1) {
-			SI_MSG("Applying 43224B0+ WARs\n");
-			si_pmu_chipcontrol(sih, 0, CCTRL_43224B0_12MA_LED_DRIVE,
-					   CCTRL_43224B0_12MA_LED_DRIVE);
-		}
-	}
-
-	if (ai_get_chip_id(sih) == BCM4313_CHIP_ID) {
-		/*
-		 * enable 12 mA drive strenth for 4313 and
-		 * set chipControl register bit 1
-		 */
-		SI_MSG("Applying 4313 WARs\n");
-		si_pmu_chipcontrol(sih, 0, CCTRL_4313_12MA_LED_DRIVE,
-				   CCTRL_4313_12MA_LED_DRIVE);
 	}
 
 	return sii;
@@ -589,7 +538,7 @@ void ai_detach(struct si_pub *sih)
 	struct si_pub *si_local = NULL;
 	memcpy(&si_local, &sih, sizeof(struct si_pub **));
 
-	sii = (struct si_info *)sih;
+	sii = container_of(sih, struct si_info, pub);
 
 	if (sii == NULL)
 		return;
@@ -597,27 +546,6 @@ void ai_detach(struct si_pub *sih)
 	kfree(sii);
 }
 
-/* return index of coreid or BADIDX if not found */
-struct bcma_device *ai_findcore(struct si_pub *sih, u16 coreid, u16 coreunit)
-{
-	struct bcma_device *core;
-	struct si_info *sii;
-	uint found;
-
-	sii = (struct si_info *)sih;
-
-	found = 0;
-
-	list_for_each_entry(core, &sii->icbus->cores, list)
-		if (core->id.id == coreid) {
-			if (found == coreunit)
-				return core;
-			found++;
-		}
-
-	return NULL;
-}
-
 /*
  * read/modify chipcommon core register.
  */
@@ -627,13 +555,12 @@ uint ai_cc_reg(struct si_pub *sih, uint regoff, u32 mask, u32 val)
 	u32 w;
 	struct si_info *sii;
 
-	sii = (struct si_info *)sih;
+	sii = container_of(sih, struct si_info, pub);
 	cc = sii->icbus->drv_cc.core;
 
 	/* mask and set */
-	if (mask || val) {
+	if (mask || val)
 		bcma_maskset32(cc, regoff, ~mask, val);
-	}
 
 	/* readback */
 	w = bcma_read32(cc, regoff);
@@ -694,12 +621,13 @@ ai_clkctl_setdelay(struct si_pub *sih, struct bcma_device *cc)
 /* initialize power control delay registers */
 void ai_clkctl_init(struct si_pub *sih)
 {
+	struct si_info *sii = container_of(sih, struct si_info, pub);
 	struct bcma_device *cc;
 
 	if (!(ai_get_cccaps(sih) & CC_CAP_PWR_CTL))
 		return;
 
-	cc = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
+	cc = sii->icbus->drv_cc.core;
 	if (cc == NULL)
 		return;
 
@@ -721,7 +649,7 @@ u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih)
 	uint slowminfreq;
 	u16 fpdelay;
 
-	sii = (struct si_info *)sih;
+	sii = container_of(sih, struct si_info, pub);
 	if (ai_get_cccaps(sih) & CC_CAP_PMU) {
 		fpdelay = si_pmu_fast_pwrup_delay(sih);
 		return fpdelay;
@@ -731,7 +659,7 @@ u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih)
 		return 0;
 
 	fpdelay = 0;
-	cc = ai_findcore(sih, CC_CORE_ID, 0);
+	cc = sii->icbus->drv_cc.core;
 	if (cc) {
 		slowminfreq = ai_slowclk_freq(sih, false, cc);
 		fpdelay = (((bcma_read32(cc, CHIPCREGOFFS(pll_on_delay)) + 2)
@@ -753,12 +681,9 @@ bool ai_clkctl_cc(struct si_pub *sih, enum bcma_clkmode mode)
 	struct si_info *sii;
 	struct bcma_device *cc;
 
-	sii = (struct si_info *)sih;
-
-	if (PCI_FORCEHT(sih))
-		return mode == BCMA_CLKMODE_FAST;
+	sii = container_of(sih, struct si_info, pub);
 
-	cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
+	cc = sii->icbus->drv_cc.core;
 	bcma_core_set_clockmode(cc, mode);
 	return mode == BCMA_CLKMODE_FAST;
 }
@@ -766,16 +691,10 @@ bool ai_clkctl_cc(struct si_pub *sih, enum bcma_clkmode mode)
 void ai_pci_up(struct si_pub *sih)
 {
 	struct si_info *sii;
-	struct bcma_device *cc;
 
-	sii = (struct si_info *)sih;
+	sii = container_of(sih, struct si_info, pub);
 
-	if (PCI_FORCEHT(sih)) {
-		cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
-		bcma_core_set_clockmode(cc, BCMA_CLKMODE_FAST);
-	}
-
-	if (PCIE(sih))
+	if (sii->icbus->hosttype == BCMA_HOSTTYPE_PCI)
 		bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, true);
 }
 
@@ -783,26 +702,20 @@ void ai_pci_up(struct si_pub *sih)
 void ai_pci_down(struct si_pub *sih)
 {
 	struct si_info *sii;
-	struct bcma_device *cc;
 
-	sii = (struct si_info *)sih;
+	sii = container_of(sih, struct si_info, pub);
 
-	/* release FORCEHT since chip is going to "down" state */
-	if (PCI_FORCEHT(sih)) {
-		cc = ai_findcore(&sii->pub, BCMA_CORE_CHIPCOMMON, 0);
-		bcma_core_set_clockmode(cc, BCMA_CLKMODE_DYNAMIC);
-	}
-
-	if (PCIE(sih))
+	if (sii->icbus->hosttype == BCMA_HOSTTYPE_PCI)
 		bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, false);
 }
 
 /* Enable BT-COEX & Ex-PA for 4313 */
 void ai_epa_4313war(struct si_pub *sih)
 {
+	struct si_info *sii = container_of(sih, struct si_info, pub);
 	struct bcma_device *cc;
 
-	cc = ai_findcore(sih, CC_CORE_ID, 0);
+	cc = sii->icbus->drv_cc.core;
 
 	/* EPA Fix */
 	bcma_set32(cc, CHIPCREGOFFS(gpiocontrol), GPIO_CTRL_EPA_EN_MASK);
@@ -814,7 +727,7 @@ bool ai_deviceremoved(struct si_pub *sih)
 	u32 w;
 	struct si_info *sii;
 
-	sii = (struct si_info *)sih;
+	sii = container_of(sih, struct si_info, pub);
 
 	if (sii->icbus->hosttype != BCMA_HOSTTYPE_PCI)
 		return false;
@@ -825,15 +738,3 @@ bool ai_deviceremoved(struct si_pub *sih)
 
 	return false;
 }
-
-uint ai_get_buscoretype(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-	return sii->buscore->id.id;
-}
-
-uint ai_get_buscorerev(struct si_pub *sih)
-{
-	struct si_info *sii = (struct si_info *)sih;
-	return sii->buscore->id.rev;
-}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
index d9f04a683bd..89562c1fbf4 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
@@ -88,16 +88,6 @@
 #define	CLKD_OTP		0x000f0000
 #define	CLKD_OTP_SHIFT		16
 
-/* Package IDs */
-#define	BCM4717_PKG_ID		9	/* 4717 package id */
-#define	BCM4718_PKG_ID		10	/* 4718 package id */
-#define BCM43224_FAB_SMIC	0xa	/* the chip is manufactured by SMIC */
-
-/* these are router chips */
-#define	BCM4716_CHIP_ID		0x4716	/* 4716 chipcommon chipid */
-#define	BCM47162_CHIP_ID	47162	/* 47162 chipcommon chipid */
-#define	BCM4748_CHIP_ID		0x4748	/* 4716 chipcommon chipid (OTP, RBBU) */
-
 /* dynamic clock control defines */
 #define	LPOMINFREQ		25000	/* low power oscillator min */
 #define	LPOMAXFREQ		43000	/* low power oscillator max */
@@ -168,7 +158,6 @@ struct si_info {
 	struct si_pub pub;	/* back plane public state (must be first) */
 	struct bcma_bus *icbus;	/* handle to soc interconnect bus */
 	struct pci_dev *pcibus;	/* handle to pci bus */
-	struct bcma_device *buscore;
 
 	u32 chipst;		/* chip status */
 };
@@ -183,8 +172,6 @@ struct si_info {
 
 
 /* AMBA Interconnect exported externs */
-extern struct bcma_device *ai_findcore(struct si_pub *sih,
-				       u16 coreid, u16 coreunit);
 extern u32 ai_core_cflags(struct bcma_device *core, u32 mask, u32 val);
 
 /* === exported functions === */
@@ -193,7 +180,7 @@ extern void ai_detach(struct si_pub *sih);
 extern uint ai_cc_reg(struct si_pub *sih, uint regoff, u32 mask, u32 val);
 extern void ai_clkctl_init(struct si_pub *sih);
 extern u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih);
-extern bool ai_clkctl_cc(struct si_pub *sih, uint mode);
+extern bool ai_clkctl_cc(struct si_pub *sih, enum bcma_clkmode mode);
 extern bool ai_deviceremoved(struct si_pub *sih);
 
 extern void ai_pci_down(struct si_pub *sih);
@@ -202,9 +189,6 @@ extern void ai_pci_up(struct si_pub *sih);
 /* Enable Ex-PA for 4313 */
 extern void ai_epa_4313war(struct si_pub *sih);
 
-extern uint ai_get_buscoretype(struct si_pub *sih);
-extern uint ai_get_buscorerev(struct si_pub *sih);
-
 static inline u32 ai_get_cccaps(struct si_pub *sih)
 {
 	return sih->cccaps;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c b/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
index 95b5902bc4b..be5bcfb9153 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
@@ -663,9 +663,6 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 		/* patch the first MPDU */
 		if (count == 1) {
 			u8 plcp0, plcp3, is40, sgi;
-			struct ieee80211_sta *sta;
-
-			sta = tx_info->control.sta;
 
 			if (rr) {
 				plcp0 = plcp[0];
@@ -735,10 +732,8 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 		 * a candidate for aggregation
 		 */
 		p = pktq_ppeek(&qi->q, prec);
-		/* tx_info must be checked with current p */
-		tx_info = IEEE80211_SKB_CB(p);
-
 		if (p) {
+			tx_info = IEEE80211_SKB_CB(p);
 			if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) &&
 			    ((u8) (p->priority) == tid)) {
 				plen = p->len + AMPDU_MAX_MPDU_OVERHEAD;
@@ -759,6 +754,7 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 					p = NULL;
 					continue;
 				}
+				/* next packet fit for aggregation so dequeue */
 				p = brcmu_pktq_pdeq(&qi->q, prec);
 			} else {
 				p = NULL;
@@ -1196,8 +1192,8 @@ static bool cb_del_ampdu_pkt(struct sk_buff *mpdu, void *arg_a)
 	bool rc;
 
 	rc = tx_info->flags & IEEE80211_TX_CTL_AMPDU ? true : false;
-	rc = rc && (tx_info->control.sta == NULL || ampdu_pars->sta == NULL ||
-		    tx_info->control.sta == ampdu_pars->sta);
+	rc = rc && (tx_info->rate_driver_data[0] == NULL || ampdu_pars->sta == NULL ||
+		    tx_info->rate_driver_data[0] == ampdu_pars->sta);
 	rc = rc && ((u8)(mpdu->priority) == ampdu_pars->tid);
 	return rc;
 }
@@ -1211,8 +1207,8 @@ static void dma_cb_fn_ampdu(void *txi, void *arg_a)
 	struct ieee80211_tx_info *tx_info = (struct ieee80211_tx_info *)txi;
 
 	if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) &&
-	    (tx_info->control.sta == sta || sta == NULL))
-		tx_info->control.sta = NULL;
+	    (tx_info->rate_driver_data[0] == sta || sta == NULL))
+		tx_info->rate_driver_data[0] = NULL;
 }
 
 /*
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.c b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
index eb77ac3cfb6..9a4c63f927c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
@@ -15,7 +15,9 @@
  */
 
 #include <linux/types.h>
+#include <net/cfg80211.h>
 #include <net/mac80211.h>
+#include <net/regulatory.h>
 
 #include <defs.h>
 #include "pub.h"
@@ -23,73 +25,17 @@
 #include "main.h"
 #include "stf.h"
 #include "channel.h"
+#include "mac80211_if.h"
 
 /* QDB() macro takes a dB value and converts to a quarter dB value */
 #define QDB(n) ((n) * BRCMS_TXPWR_DB_FACTOR)
 
-#define  LOCALE_CHAN_01_11	 (1<<0)
-#define  LOCALE_CHAN_12_13	 (1<<1)
-#define  LOCALE_CHAN_14		 (1<<2)
-#define  LOCALE_SET_5G_LOW_JP1   (1<<3)	/* 34-48, step 2 */
-#define  LOCALE_SET_5G_LOW_JP2   (1<<4)	/* 34-46, step 4 */
-#define  LOCALE_SET_5G_LOW1      (1<<5)	/* 36-48, step 4 */
-#define  LOCALE_SET_5G_LOW2      (1<<6)	/* 52 */
-#define  LOCALE_SET_5G_LOW3      (1<<7)	/* 56-64, step 4 */
-#define  LOCALE_SET_5G_MID1      (1<<8)	/* 100-116, step 4 */
-#define  LOCALE_SET_5G_MID2	 (1<<9)	/* 120-124, step 4 */
-#define  LOCALE_SET_5G_MID3      (1<<10)	/* 128 */
-#define  LOCALE_SET_5G_HIGH1     (1<<11)	/* 132-140, step 4 */
-#define  LOCALE_SET_5G_HIGH2     (1<<12)	/* 149-161, step 4 */
-#define  LOCALE_SET_5G_HIGH3     (1<<13)	/* 165 */
-#define  LOCALE_CHAN_52_140_ALL  (1<<14)
-#define  LOCALE_SET_5G_HIGH4     (1<<15)	/* 184-216 */
-
-#define  LOCALE_CHAN_36_64	(LOCALE_SET_5G_LOW1 | \
-				 LOCALE_SET_5G_LOW2 | \
-				 LOCALE_SET_5G_LOW3)
-#define  LOCALE_CHAN_52_64	(LOCALE_SET_5G_LOW2 | LOCALE_SET_5G_LOW3)
-#define  LOCALE_CHAN_100_124	(LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2)
-#define  LOCALE_CHAN_100_140	(LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2 | \
-				  LOCALE_SET_5G_MID3 | LOCALE_SET_5G_HIGH1)
-#define  LOCALE_CHAN_149_165	(LOCALE_SET_5G_HIGH2 | LOCALE_SET_5G_HIGH3)
-#define  LOCALE_CHAN_184_216	LOCALE_SET_5G_HIGH4
-
-#define  LOCALE_CHAN_01_14	(LOCALE_CHAN_01_11 | \
-				 LOCALE_CHAN_12_13 | \
-				 LOCALE_CHAN_14)
-
-#define  LOCALE_RADAR_SET_NONE		  0
-#define  LOCALE_RADAR_SET_1		  1
-
-#define  LOCALE_RESTRICTED_NONE		  0
-#define  LOCALE_RESTRICTED_SET_2G_SHORT   1
-#define  LOCALE_RESTRICTED_CHAN_165       2
-#define  LOCALE_CHAN_ALL_5G		  3
-#define  LOCALE_RESTRICTED_JAPAN_LEGACY   4
-#define  LOCALE_RESTRICTED_11D_2G	  5
-#define  LOCALE_RESTRICTED_11D_5G	  6
-#define  LOCALE_RESTRICTED_LOW_HI	  7
-#define  LOCALE_RESTRICTED_12_13_14	  8
-
-#define LOCALE_2G_IDX_i			0
-#define LOCALE_5G_IDX_11		0
 #define LOCALE_MIMO_IDX_bn		0
 #define LOCALE_MIMO_IDX_11n		0
 
-/* max of BAND_5G_PWR_LVLS and 6 for 2.4 GHz */
-#define BRCMS_MAXPWR_TBL_SIZE		6
 /* max of BAND_5G_PWR_LVLS and 14 for 2.4 GHz */
 #define BRCMS_MAXPWR_MIMO_TBL_SIZE	14
 
-/* power level in group of 2.4GHz band channels:
- * maxpwr[0] - CCK  channels [1]
- * maxpwr[1] - CCK  channels [2-10]
- * maxpwr[2] - CCK  channels [11-14]
- * maxpwr[3] - OFDM channels [1]
- * maxpwr[4] - OFDM channels [2-10]
- * maxpwr[5] - OFDM channels [11-14]
- */
-
 /* maxpwr mapping to 5GHz band channels:
  * maxpwr[0] - channels [34-48]
  * maxpwr[1] - channels [52-60]
@@ -101,16 +47,8 @@
 
 #define LC(id)	LOCALE_MIMO_IDX_ ## id
 
-#define LC_2G(id)	LOCALE_2G_IDX_ ## id
-
-#define LC_5G(id)	LOCALE_5G_IDX_ ## id
-
-#define LOCALES(band2, band5, mimo2, mimo5) \
-		{LC_2G(band2), LC_5G(band5), LC(mimo2), LC(mimo5)}
-
-/* macro to get 2.4 GHz channel group index for tx power */
-#define CHANNEL_POWER_IDX_2G_CCK(c) (((c) < 2) ? 0 : (((c) < 11) ? 1 : 2))
-#define CHANNEL_POWER_IDX_2G_OFDM(c) (((c) < 2) ? 3 : (((c) < 11) ? 4 : 5))
+#define LOCALES(mimo2, mimo5) \
+		{LC(mimo2), LC(mimo5)}
 
 /* macro to get 5 GHz channel group index for tx power */
 #define CHANNEL_POWER_IDX_5G(c) (((c) < 52) ? 0 : \
@@ -118,18 +56,37 @@
 				 (((c) < 100) ? 2 : \
 				 (((c) < 149) ? 3 : 4))))
 
-#define ISDFS_EU(fl)		(((fl) & BRCMS_DFS_EU) == BRCMS_DFS_EU)
-
-struct brcms_cm_band {
-	/* struct locale_info flags */
-	u8 locale_flags;
-	/* List of valid channels in the country */
-	struct brcms_chanvec valid_channels;
-	/* List of restricted use channels */
-	const struct brcms_chanvec *restricted_channels;
-	/* List of radar sensitive channels */
-	const struct brcms_chanvec *radar_channels;
-	u8 PAD[8];
+#define BRCM_2GHZ_2412_2462	REG_RULE(2412-10, 2462+10, 40, 0, 19, 0)
+#define BRCM_2GHZ_2467_2472	REG_RULE(2467-10, 2472+10, 20, 0, 19, \
+					 NL80211_RRF_PASSIVE_SCAN | \
+					 NL80211_RRF_NO_IBSS)
+
+#define BRCM_5GHZ_5180_5240	REG_RULE(5180-10, 5240+10, 40, 0, 21, \
+					 NL80211_RRF_PASSIVE_SCAN | \
+					 NL80211_RRF_NO_IBSS)
+#define BRCM_5GHZ_5260_5320	REG_RULE(5260-10, 5320+10, 40, 0, 21, \
+					 NL80211_RRF_PASSIVE_SCAN | \
+					 NL80211_RRF_DFS | \
+					 NL80211_RRF_NO_IBSS)
+#define BRCM_5GHZ_5500_5700	REG_RULE(5500-10, 5700+10, 40, 0, 21, \
+					 NL80211_RRF_PASSIVE_SCAN | \
+					 NL80211_RRF_DFS | \
+					 NL80211_RRF_NO_IBSS)
+#define BRCM_5GHZ_5745_5825	REG_RULE(5745-10, 5825+10, 40, 0, 21, \
+					 NL80211_RRF_PASSIVE_SCAN | \
+					 NL80211_RRF_NO_IBSS)
+
+static const struct ieee80211_regdomain brcms_regdom_x2 = {
+	.n_reg_rules = 7,
+	.alpha2 = "X2",
+	.reg_rules = {
+		BRCM_2GHZ_2412_2462,
+		BRCM_2GHZ_2467_2472,
+		BRCM_5GHZ_5180_5240,
+		BRCM_5GHZ_5260_5320,
+		BRCM_5GHZ_5500_5700,
+		BRCM_5GHZ_5745_5825,
+	}
 };
 
  /* locale per-channel tx power limits for MIMO frames
@@ -141,337 +98,23 @@ struct locale_mimo_info {
 	s8 maxpwr20[BRCMS_MAXPWR_MIMO_TBL_SIZE];
 	/* tx 40 MHz power limits, qdBm units */
 	s8 maxpwr40[BRCMS_MAXPWR_MIMO_TBL_SIZE];
-	u8 flags;
 };
 
 /* Country names and abbreviations with locale defined from ISO 3166 */
 struct country_info {
-	const u8 locale_2G;	/* 2.4G band locale */
-	const u8 locale_5G;	/* 5G band locale */
 	const u8 locale_mimo_2G;	/* 2.4G mimo info */
 	const u8 locale_mimo_5G;	/* 5G mimo info */
 };
 
+struct brcms_regd {
+	struct country_info country;
+	const struct ieee80211_regdomain *regdomain;
+};
+
 struct brcms_cm_info {
 	struct brcms_pub *pub;
 	struct brcms_c_info *wlc;
-	char srom_ccode[BRCM_CNTRY_BUF_SZ];	/* Country Code in SROM */
-	uint srom_regrev;	/* Regulatory Rev for the SROM ccode */
-	const struct country_info *country;	/* current country def */
-	char ccode[BRCM_CNTRY_BUF_SZ];	/* current internal Country Code */
-	uint regrev;		/* current Regulatory Revision */
-	char country_abbrev[BRCM_CNTRY_BUF_SZ];	/* current advertised ccode */
-	/* per-band state (one per phy/radio) */
-	struct brcms_cm_band bandstate[MAXBANDS];
-	/* quiet channels currently for radar sensitivity or 11h support */
-	/* channels on which we cannot transmit */
-	struct brcms_chanvec quiet_channels;
-};
-
-/* locale channel and power info. */
-struct locale_info {
-	u32 valid_channels;
-	/* List of radar sensitive channels */
-	u8 radar_channels;
-	/* List of channels used only if APs are detected */
-	u8 restricted_channels;
-	/* Max tx pwr in qdBm for each sub-band */
-	s8 maxpwr[BRCMS_MAXPWR_TBL_SIZE];
-	/* Country IE advertised max tx pwr in dBm per sub-band */
-	s8 pub_maxpwr[BAND_5G_PWR_LVLS];
-	u8 flags;
-};
-
-/* Regulatory Matrix Spreadsheet (CLM) MIMO v3.7.9 */
-
-/*
- * Some common channel sets
- */
-
-/* No channels */
-static const struct brcms_chanvec chanvec_none = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* All 2.4 GHz HW channels */
-static const struct brcms_chanvec chanvec_all_2G = {
-	{0xfe, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* All 5 GHz HW channels */
-static const struct brcms_chanvec chanvec_all_5G = {
-	{0x00, 0x00, 0x00, 0x00, 0x54, 0x55, 0x11, 0x11,
-	 0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,
-	 0x11, 0x11, 0x20, 0x22, 0x22, 0x00, 0x00, 0x11,
-	 0x11, 0x11, 0x11, 0x01}
-};
-
-/*
- * Radar channel sets
- */
-
-/* Channels 52 - 64, 100 - 140 */
-static const struct brcms_chanvec radar_set1 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,  /* 52 - 60 */
-	 0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,  /* 64, 100 - 124 */
-	 0x11, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  /* 128 - 140 */
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/*
- * Restricted channel sets
- */
-
-/* Channels 34, 38, 42, 46 */
-static const struct brcms_chanvec restricted_set_japan_legacy = {
-	{0x00, 0x00, 0x00, 0x00, 0x44, 0x44, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channels 12, 13 */
-static const struct brcms_chanvec restricted_set_2g_short = {
-	{0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channel 165 */
-static const struct brcms_chanvec restricted_chan_165 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channels 36 - 48 & 149 - 165 */
-static const struct brcms_chanvec restricted_low_hi = {
-	{0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x01, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x20, 0x22, 0x22, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channels 12 - 14 */
-static const struct brcms_chanvec restricted_set_12_13_14 = {
-	{0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-/* global memory to provide working buffer for expanded locale */
-
-static const struct brcms_chanvec *g_table_radar_set[] = {
-	&chanvec_none,
-	&radar_set1
-};
-
-static const struct brcms_chanvec *g_table_restricted_chan[] = {
-	&chanvec_none,		/* restricted_set_none */
-	&restricted_set_2g_short,
-	&restricted_chan_165,
-	&chanvec_all_5G,
-	&restricted_set_japan_legacy,
-	&chanvec_all_2G,	/* restricted_set_11d_2G */
-	&chanvec_all_5G,	/* restricted_set_11d_5G */
-	&restricted_low_hi,
-	&restricted_set_12_13_14
-};
-
-static const struct brcms_chanvec locale_2g_01_11 = {
-	{0xfe, 0x0f, 0x00, 0x00, 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 struct brcms_chanvec locale_2g_12_13 = {
-	{0x00, 0x30, 0x00, 0x00, 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 struct brcms_chanvec locale_2g_14 = {
-	{0x00, 0x40, 0x00, 0x00, 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 struct brcms_chanvec locale_5g_LOW_JP1 = {
-	{0x00, 0x00, 0x00, 0x00, 0x54, 0x55, 0x01, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW_JP2 = {
-	{0x00, 0x00, 0x00, 0x00, 0x44, 0x44, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW1 = {
-	{0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x01, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW2 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW3 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
-	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_MID1 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_MID2 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_MID3 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH1 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x10, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH2 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x20, 0x22, 0x02, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH3 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_52_140_ALL = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,
-	 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
-	 0x11, 0x11, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH4 = {
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
-	 0x11, 0x11, 0x11, 0x11}
-};
-
-static const struct brcms_chanvec *g_table_locale_base[] = {
-	&locale_2g_01_11,
-	&locale_2g_12_13,
-	&locale_2g_14,
-	&locale_5g_LOW_JP1,
-	&locale_5g_LOW_JP2,
-	&locale_5g_LOW1,
-	&locale_5g_LOW2,
-	&locale_5g_LOW3,
-	&locale_5g_MID1,
-	&locale_5g_MID2,
-	&locale_5g_MID3,
-	&locale_5g_HIGH1,
-	&locale_5g_HIGH2,
-	&locale_5g_HIGH3,
-	&locale_5g_52_140_ALL,
-	&locale_5g_HIGH4
-};
-
-static void brcms_c_locale_add_channels(struct brcms_chanvec *target,
-				    const struct brcms_chanvec *channels)
-{
-	u8 i;
-	for (i = 0; i < sizeof(struct brcms_chanvec); i++)
-		target->vec[i] |= channels->vec[i];
-}
-
-static void brcms_c_locale_get_channels(const struct locale_info *locale,
-				    struct brcms_chanvec *channels)
-{
-	u8 i;
-
-	memset(channels, 0, sizeof(struct brcms_chanvec));
-
-	for (i = 0; i < ARRAY_SIZE(g_table_locale_base); i++) {
-		if (locale->valid_channels & (1 << i))
-			brcms_c_locale_add_channels(channels,
-						g_table_locale_base[i]);
-	}
-}
-
-/*
- * Locale Definitions - 2.4 GHz
- */
-static const struct locale_info locale_i = {	/* locale i. channel 1 - 13 */
-	LOCALE_CHAN_01_11 | LOCALE_CHAN_12_13,
-	LOCALE_RADAR_SET_NONE,
-	LOCALE_RESTRICTED_SET_2G_SHORT,
-	{QDB(19), QDB(19), QDB(19),
-	 QDB(19), QDB(19), QDB(19)},
-	{20, 20, 20, 0},
-	BRCMS_EIRP
-};
-
-/*
- * Locale Definitions - 5 GHz
- */
-static const struct locale_info locale_11 = {
-	/* locale 11. channel 36 - 48, 52 - 64, 100 - 140, 149 - 165 */
-	LOCALE_CHAN_36_64 | LOCALE_CHAN_100_140 | LOCALE_CHAN_149_165,
-	LOCALE_RADAR_SET_1,
-	LOCALE_RESTRICTED_NONE,
-	{QDB(21), QDB(21), QDB(21), QDB(21), QDB(21)},
-	{23, 23, 23, 30, 30},
-	BRCMS_EIRP | BRCMS_DFS_EU
-};
-
-static const struct locale_info *g_locale_2g_table[] = {
-	&locale_i
-};
-
-static const struct locale_info *g_locale_5g_table[] = {
-	&locale_11
+	const struct brcms_regd *world_regd;
 };
 
 /*
@@ -484,7 +127,6 @@ static const struct locale_mimo_info locale_bn = {
 	{0, 0, QDB(13), QDB(13), QDB(13),
 	 QDB(13), QDB(13), QDB(13), QDB(13), QDB(13),
 	 QDB(13), 0, 0},
-	0
 };
 
 static const struct locale_mimo_info *g_mimo_2g_table[] = {
@@ -497,114 +139,20 @@ static const struct locale_mimo_info *g_mimo_2g_table[] = {
 static const struct locale_mimo_info locale_11n = {
 	{ /* 12.5 dBm */ 50, 50, 50, QDB(15), QDB(15)},
 	{QDB(14), QDB(15), QDB(15), QDB(15), QDB(15)},
-	0
 };
 
 static const struct locale_mimo_info *g_mimo_5g_table[] = {
 	&locale_11n
 };
 
-static const struct {
-	char abbrev[BRCM_CNTRY_BUF_SZ];	/* country abbreviation */
-	struct country_info country;
-} cntry_locales[] = {
+static const struct brcms_regd cntry_locales[] = {
+	/* Worldwide RoW 2, must always be at index 0 */
 	{
-	"X2", LOCALES(i, 11, bn, 11n)},	/* Worldwide RoW 2 */
-};
-
-#ifdef SUPPORT_40MHZ
-/* 20MHz channel info for 40MHz pairing support */
-struct chan20_info {
-	u8 sb;
-	u8 adj_sbs;
+		.country = LOCALES(bn, 11n),
+		.regdomain = &brcms_regdom_x2,
+	},
 };
 
-/* indicates adjacent channels that are allowed for a 40 Mhz channel and
- * those that permitted by the HT
- */
-struct chan20_info chan20_info[] = {
-	/* 11b/11g */
-/* 0 */ {1, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 1 */ {2, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 2 */ {3, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 3 */ {4, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 4 */ {5, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 5 */ {6, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 6 */ {7, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 7 */ {8, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 8 */ {9, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 9 */ {10, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 10 */ {11, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 11 */ {12, (CH_LOWER_SB)},
-/* 12 */ {13, (CH_LOWER_SB)},
-/* 13 */ {14, (CH_LOWER_SB)},
-
-/* 11a japan high */
-/* 14 */ {34, (CH_UPPER_SB)},
-/* 15 */ {38, (CH_LOWER_SB)},
-/* 16 */ {42, (CH_LOWER_SB)},
-/* 17 */ {46, (CH_LOWER_SB)},
-
-/* 11a usa low */
-/* 18 */ {36, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 19 */ {40, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 20 */ {44, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 21 */ {48, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 22 */ {52, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 23 */ {56, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 24 */ {60, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 25 */ {64, (CH_LOWER_SB | CH_EWA_VALID)},
-
-/* 11a Europe */
-/* 26 */ {100, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 27 */ {104, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 28 */ {108, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 29 */ {112, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 30 */ {116, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 31 */ {120, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 32 */ {124, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 33 */ {128, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 34 */ {132, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 35 */ {136, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 36 */ {140, (CH_LOWER_SB)},
-
-/* 11a usa high, ref5 only */
-/* The 0x80 bit in pdiv means these are REF5, other entries are REF20 */
-/* 37 */ {149, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 38 */ {153, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 39 */ {157, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 40 */ {161, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 41 */ {165, (CH_LOWER_SB)},
-
-/* 11a japan */
-/* 42 */ {184, (CH_UPPER_SB)},
-/* 43 */ {188, (CH_LOWER_SB)},
-/* 44 */ {192, (CH_UPPER_SB)},
-/* 45 */ {196, (CH_LOWER_SB)},
-/* 46 */ {200, (CH_UPPER_SB)},
-/* 47 */ {204, (CH_LOWER_SB)},
-/* 48 */ {208, (CH_UPPER_SB)},
-/* 49 */ {212, (CH_LOWER_SB)},
-/* 50 */ {216, (CH_LOWER_SB)}
-};
-#endif				/* SUPPORT_40MHZ */
-
-static const struct locale_info *brcms_c_get_locale_2g(u8 locale_idx)
-{
-	if (locale_idx >= ARRAY_SIZE(g_locale_2g_table))
-		return NULL; /* error condition */
-
-	return g_locale_2g_table[locale_idx];
-}
-
-static const struct locale_info *brcms_c_get_locale_5g(u8 locale_idx)
-{
-	if (locale_idx >= ARRAY_SIZE(g_locale_5g_table))
-		return NULL; /* error condition */
-
-	return g_locale_5g_table[locale_idx];
-}
-
 static const struct locale_mimo_info *brcms_c_get_mimo_2g(u8 locale_idx)
 {
 	if (locale_idx >= ARRAY_SIZE(g_mimo_2g_table))
@@ -621,13 +169,6 @@ static const struct locale_mimo_info *brcms_c_get_mimo_5g(u8 locale_idx)
 	return g_mimo_5g_table[locale_idx];
 }
 
-static int
-brcms_c_country_aggregate_map(struct brcms_cm_info *wlc_cm, const char *ccode,
-			  char *mapped_ccode, uint *mapped_regrev)
-{
-	return false;
-}
-
 /*
  * Indicates whether the country provided is valid to pass
  * to cfg80211 or not.
@@ -662,155 +203,24 @@ static bool brcms_c_country_valid(const char *ccode)
 	return true;
 }
 
-/* Lookup a country info structure from a null terminated country
- * abbreviation and regrev directly with no translation.
- */
-static const struct country_info *
-brcms_c_country_lookup_direct(const char *ccode, uint regrev)
+static const struct brcms_regd *brcms_world_regd(const char *regdom, int len)
 {
-	uint size, i;
-
-	/* Should just return 0 for single locale driver. */
-	/* Keep it this way in case we add more locales. (for now anyway) */
-
-	/*
-	 * all other country def arrays are for regrev == 0, so if
-	 * regrev is non-zero, fail
-	 */
-	if (regrev > 0)
-		return NULL;
-
-	/* find matched table entry from country code */
-	size = ARRAY_SIZE(cntry_locales);
-	for (i = 0; i < size; i++) {
-		if (strcmp(ccode, cntry_locales[i].abbrev) == 0)
-			return &cntry_locales[i].country;
-	}
-	return NULL;
-}
-
-static const struct country_info *
-brcms_c_countrycode_map(struct brcms_cm_info *wlc_cm, const char *ccode,
-			char *mapped_ccode, uint *mapped_regrev)
-{
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	const struct country_info *country;
-	uint srom_regrev = wlc_cm->srom_regrev;
-	const char *srom_ccode = wlc_cm->srom_ccode;
-	int mapped;
-
-	/* check for currently supported ccode size */
-	if (strlen(ccode) > (BRCM_CNTRY_BUF_SZ - 1)) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: ccode \"%s\" too long for "
-			  "match\n", wlc->pub->unit, __func__, ccode);
-		return NULL;
-	}
-
-	/* default mapping is the given ccode and regrev 0 */
-	strncpy(mapped_ccode, ccode, BRCM_CNTRY_BUF_SZ);
-	*mapped_regrev = 0;
-
-	/* If the desired country code matches the srom country code,
-	 * then the mapped country is the srom regulatory rev.
-	 * Otherwise look for an aggregate mapping.
-	 */
-	if (!strcmp(srom_ccode, ccode)) {
-		*mapped_regrev = srom_regrev;
-		mapped = 0;
-		wiphy_err(wlc->wiphy, "srom_code == ccode %s\n", __func__);
-	} else {
-		mapped =
-		    brcms_c_country_aggregate_map(wlc_cm, ccode, mapped_ccode,
-					      mapped_regrev);
-	}
-
-	/* find the matching built-in country definition */
-	country = brcms_c_country_lookup_direct(mapped_ccode, *mapped_regrev);
-
-	/* if there is not an exact rev match, default to rev zero */
-	if (country == NULL && *mapped_regrev != 0) {
-		*mapped_regrev = 0;
-		country =
-		    brcms_c_country_lookup_direct(mapped_ccode, *mapped_regrev);
-	}
-
-	return country;
-}
-
-/* Lookup a country info structure from a null terminated country code
- * The lookup is case sensitive.
- */
-static const struct country_info *
-brcms_c_country_lookup(struct brcms_c_info *wlc, const char *ccode)
-{
-	const struct country_info *country;
-	char mapped_ccode[BRCM_CNTRY_BUF_SZ];
-	uint mapped_regrev;
-
-	/*
-	 * map the country code to a built-in country code, regrev, and
-	 * country_info struct
-	 */
-	country = brcms_c_countrycode_map(wlc->cmi, ccode, mapped_ccode,
-					  &mapped_regrev);
-
-	return country;
-}
-
-/*
- * reset the quiet channels vector to the union
- * of the restricted and radar channel sets
- */
-static void brcms_c_quiet_channels_reset(struct brcms_cm_info *wlc_cm)
-{
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	uint i, j;
-	struct brcms_band *band;
-	const struct brcms_chanvec *chanvec;
-
-	memset(&wlc_cm->quiet_channels, 0, sizeof(struct brcms_chanvec));
-
-	band = wlc->band;
-	for (i = 0; i < wlc->pub->_nbands;
-	     i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
-
-		/* initialize quiet channels for restricted channels */
-		chanvec = wlc_cm->bandstate[band->bandunit].restricted_channels;
-		for (j = 0; j < sizeof(struct brcms_chanvec); j++)
-			wlc_cm->quiet_channels.vec[j] |= chanvec->vec[j];
+	const struct brcms_regd *regd = NULL;
+	int i;
 
+	for (i = 0; i < ARRAY_SIZE(cntry_locales); i++) {
+		if (!strncmp(regdom, cntry_locales[i].regdomain->alpha2, len)) {
+			regd = &cntry_locales[i];
+			break;
+		}
 	}
-}
-
-/* Is the channel valid for the current locale and current band? */
-static bool brcms_c_valid_channel20(struct brcms_cm_info *wlc_cm, uint val)
-{
-	struct brcms_c_info *wlc = wlc_cm->wlc;
 
-	return ((val < MAXCHANNEL) &&
-		isset(wlc_cm->bandstate[wlc->band->bandunit].valid_channels.vec,
-		      val));
+	return regd;
 }
 
-/* Is the channel valid for the current locale and specified band? */
-static bool brcms_c_valid_channel20_in_band(struct brcms_cm_info *wlc_cm,
-					    uint bandunit, uint val)
-{
-	return ((val < MAXCHANNEL)
-		&& isset(wlc_cm->bandstate[bandunit].valid_channels.vec, val));
-}
-
-/* Is the channel valid for the current locale? (but don't consider channels not
- *   available due to bandlocking)
- */
-static bool brcms_c_valid_channel20_db(struct brcms_cm_info *wlc_cm, uint val)
+static const struct brcms_regd *brcms_default_world_regd(void)
 {
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-
-	return brcms_c_valid_channel20(wlc->cmi, val) ||
-		(!wlc->bandlocked
-		 && brcms_c_valid_channel20_in_band(wlc->cmi,
-						    OTHERBANDUNIT(wlc), val));
+	return &cntry_locales[0];
 }
 
 /* JP, J1 - J10 are Japan ccodes */
@@ -820,12 +230,6 @@ static bool brcms_c_japan_ccode(const char *ccode)
 		(ccode[1] == 'P' || (ccode[1] >= '1' && ccode[1] <= '9')));
 }
 
-/* Returns true if currently set country is Japan or variant */
-static bool brcms_c_japan(struct brcms_c_info *wlc)
-{
-	return brcms_c_japan_ccode(wlc->cmi->country_abbrev);
-}
-
 static void
 brcms_c_channel_min_txpower_limits_with_local_constraint(
 		struct brcms_cm_info *wlc_cm, struct txpwr_limits *txpwr,
@@ -901,140 +305,16 @@ brcms_c_channel_min_txpower_limits_with_local_constraint(
 
 }
 
-/* Update the radio state (enable/disable) and tx power targets
- * based on a new set of channel/regulatory information
- */
-static void brcms_c_channels_commit(struct brcms_cm_info *wlc_cm)
-{
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	uint chan;
-	struct txpwr_limits txpwr;
-
-	/* search for the existence of any valid channel */
-	for (chan = 0; chan < MAXCHANNEL; chan++) {
-		if (brcms_c_valid_channel20_db(wlc->cmi, chan))
-			break;
-	}
-	if (chan == MAXCHANNEL)
-		chan = INVCHANNEL;
-
-	/*
-	 * based on the channel search above, set or
-	 * clear WL_RADIO_COUNTRY_DISABLE.
-	 */
-	if (chan == INVCHANNEL) {
-		/*
-		 * country/locale with no valid channels, set
-		 * the radio disable bit
-		 */
-		mboolset(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
-		wiphy_err(wlc->wiphy, "wl%d: %s: no valid channel for \"%s\" "
-			  "nbands %d bandlocked %d\n", wlc->pub->unit,
-			  __func__, wlc_cm->country_abbrev, wlc->pub->_nbands,
-			  wlc->bandlocked);
-	} else if (mboolisset(wlc->pub->radio_disabled,
-			      WL_RADIO_COUNTRY_DISABLE)) {
-		/*
-		 * country/locale with valid channel, clear
-		 * the radio disable bit
-		 */
-		mboolclr(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
-	}
-
-	/*
-	 * Now that the country abbreviation is set, if the radio supports 2G,
-	 * then set channel 14 restrictions based on the new locale.
-	 */
-	if (wlc->pub->_nbands > 1 || wlc->band->bandtype == BRCM_BAND_2G)
-		wlc_phy_chanspec_ch14_widefilter_set(wlc->band->pi,
-						     brcms_c_japan(wlc) ? true :
-						     false);
-
-	if (wlc->pub->up && chan != INVCHANNEL) {
-		brcms_c_channel_reg_limits(wlc_cm, wlc->chanspec, &txpwr);
-		brcms_c_channel_min_txpower_limits_with_local_constraint(wlc_cm,
-			&txpwr, BRCMS_TXPWR_MAX);
-		wlc_phy_txpower_limit_set(wlc->band->pi, &txpwr, wlc->chanspec);
-	}
-}
-
-static int
-brcms_c_channels_init(struct brcms_cm_info *wlc_cm,
-		      const struct country_info *country)
-{
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	uint i, j;
-	struct brcms_band *band;
-	const struct locale_info *li;
-	struct brcms_chanvec sup_chan;
-	const struct locale_mimo_info *li_mimo;
-
-	band = wlc->band;
-	for (i = 0; i < wlc->pub->_nbands;
-	     i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
-
-		li = (band->bandtype == BRCM_BAND_5G) ?
-		    brcms_c_get_locale_5g(country->locale_5G) :
-		    brcms_c_get_locale_2g(country->locale_2G);
-		wlc_cm->bandstate[band->bandunit].locale_flags = li->flags;
-		li_mimo = (band->bandtype == BRCM_BAND_5G) ?
-		    brcms_c_get_mimo_5g(country->locale_mimo_5G) :
-		    brcms_c_get_mimo_2g(country->locale_mimo_2G);
-
-		/* merge the mimo non-mimo locale flags */
-		wlc_cm->bandstate[band->bandunit].locale_flags |=
-		    li_mimo->flags;
-
-		wlc_cm->bandstate[band->bandunit].restricted_channels =
-		    g_table_restricted_chan[li->restricted_channels];
-		wlc_cm->bandstate[band->bandunit].radar_channels =
-		    g_table_radar_set[li->radar_channels];
-
-		/*
-		 * set the channel availability, masking out the channels
-		 * that may not be supported on this phy.
-		 */
-		wlc_phy_chanspec_band_validch(band->pi, band->bandtype,
-					      &sup_chan);
-		brcms_c_locale_get_channels(li,
-					&wlc_cm->bandstate[band->bandunit].
-					valid_channels);
-		for (j = 0; j < sizeof(struct brcms_chanvec); j++)
-			wlc_cm->bandstate[band->bandunit].valid_channels.
-			    vec[j] &= sup_chan.vec[j];
-	}
-
-	brcms_c_quiet_channels_reset(wlc_cm);
-	brcms_c_channels_commit(wlc_cm);
-
-	return 0;
-}
-
 /*
  * set the driver's current country and regulatory information
  * using a country code as the source. Look up built in country
  * information found with the country code.
  */
 static void
-brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
-		       const char *country_abbrev,
-		       const char *ccode, uint regrev,
-		       const struct country_info *country)
+brcms_c_set_country(struct brcms_cm_info *wlc_cm,
+		    const struct brcms_regd *regd)
 {
-	const struct locale_info *locale;
 	struct brcms_c_info *wlc = wlc_cm->wlc;
-	char prev_country_abbrev[BRCM_CNTRY_BUF_SZ];
-
-	/* save current country state */
-	wlc_cm->country = country;
-
-	memset(&prev_country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
-	strncpy(prev_country_abbrev, wlc_cm->country_abbrev,
-		BRCM_CNTRY_BUF_SZ - 1);
-
-	strncpy(wlc_cm->country_abbrev, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
-	strncpy(wlc_cm->ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
-	wlc_cm->regrev = regrev;
 
 	if ((wlc->pub->_n_enab & SUPPORT_11N) !=
 	    wlc->protection->nmode_user)
@@ -1042,75 +322,19 @@ brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
 
 	brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_2G_INDEX]);
 	brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_5G_INDEX]);
-	/* set or restore gmode as required by regulatory */
-	locale = brcms_c_get_locale_2g(country->locale_2G);
-	if (locale && (locale->flags & BRCMS_NO_OFDM))
-		brcms_c_set_gmode(wlc, GMODE_LEGACY_B, false);
-	else
-		brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
 
-	brcms_c_channels_init(wlc_cm, country);
+	brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
 
 	return;
 }
 
-static int
-brcms_c_set_countrycode_rev(struct brcms_cm_info *wlc_cm,
-			const char *country_abbrev,
-			const char *ccode, int regrev)
-{
-	const struct country_info *country;
-	char mapped_ccode[BRCM_CNTRY_BUF_SZ];
-	uint mapped_regrev;
-
-	/* if regrev is -1, lookup the mapped country code,
-	 * otherwise use the ccode and regrev directly
-	 */
-	if (regrev == -1) {
-		/*
-		 * map the country code to a built-in country
-		 * code, regrev, and country_info
-		 */
-		country =
-		    brcms_c_countrycode_map(wlc_cm, ccode, mapped_ccode,
-					&mapped_regrev);
-	} else {
-		/* find the matching built-in country definition */
-		country = brcms_c_country_lookup_direct(ccode, regrev);
-		strncpy(mapped_ccode, ccode, BRCM_CNTRY_BUF_SZ);
-		mapped_regrev = regrev;
-	}
-
-	if (country == NULL)
-		return -EINVAL;
-
-	/* set the driver state for the country */
-	brcms_c_set_country_common(wlc_cm, country_abbrev, mapped_ccode,
-			       mapped_regrev, country);
-
-	return 0;
-}
-
-/*
- * set the driver's current country and regulatory information using
- * a country code as the source. Lookup built in country information
- * found with the country code.
- */
-static int
-brcms_c_set_countrycode(struct brcms_cm_info *wlc_cm, const char *ccode)
-{
-	char country_abbrev[BRCM_CNTRY_BUF_SZ];
-	strncpy(country_abbrev, ccode, BRCM_CNTRY_BUF_SZ);
-	return brcms_c_set_countrycode_rev(wlc_cm, country_abbrev, ccode, -1);
-}
-
 struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 {
 	struct brcms_cm_info *wlc_cm;
-	char country_abbrev[BRCM_CNTRY_BUF_SZ];
-	const struct country_info *country;
 	struct brcms_pub *pub = wlc->pub;
 	struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
+	const char *ccode = sprom->alpha2;
+	int ccode_len = sizeof(sprom->alpha2);
 
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
@@ -1122,24 +346,27 @@ struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 	wlc->cmi = wlc_cm;
 
 	/* store the country code for passing up as a regulatory hint */
-	if (sprom->alpha2 && brcms_c_country_valid(sprom->alpha2))
-		strncpy(wlc->pub->srom_ccode, sprom->alpha2, sizeof(sprom->alpha2));
+	wlc_cm->world_regd = brcms_world_regd(ccode, ccode_len);
+	if (brcms_c_country_valid(ccode))
+		strncpy(wlc->pub->srom_ccode, ccode, ccode_len);
 
 	/*
-	 * internal country information which must match
-	 * regulatory constraints in firmware
+	 * If no custom world domain is found in the SROM, use the
+	 * default "X2" domain.
 	 */
-	memset(country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
-	strncpy(country_abbrev, "X2", sizeof(country_abbrev) - 1);
-	country = brcms_c_country_lookup(wlc, country_abbrev);
+	if (!wlc_cm->world_regd) {
+		wlc_cm->world_regd = brcms_default_world_regd();
+		ccode = wlc_cm->world_regd->regdomain->alpha2;
+		ccode_len = BRCM_CNTRY_BUF_SZ - 1;
+	}
 
 	/* save default country for exiting 11d regulatory mode */
-	strncpy(wlc->country_default, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
+	strncpy(wlc->country_default, ccode, ccode_len);
 
 	/* initialize autocountry_default to driver default */
-	strncpy(wlc->autocountry_default, "X2", BRCM_CNTRY_BUF_SZ - 1);
+	strncpy(wlc->autocountry_default, ccode, ccode_len);
 
-	brcms_c_set_countrycode(wlc_cm, country_abbrev);
+	brcms_c_set_country(wlc_cm, wlc_cm->world_regd);
 
 	return wlc_cm;
 }
@@ -1149,31 +376,15 @@ void brcms_c_channel_mgr_detach(struct brcms_cm_info *wlc_cm)
 	kfree(wlc_cm);
 }
 
-u8
-brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
-				     uint bandunit)
-{
-	return wlc_cm->bandstate[bandunit].locale_flags;
-}
-
-static bool
-brcms_c_quiet_chanspec(struct brcms_cm_info *wlc_cm, u16 chspec)
-{
-	return (wlc_cm->wlc->pub->_n_enab & SUPPORT_11N) &&
-		CHSPEC_IS40(chspec) ?
-		(isset(wlc_cm->quiet_channels.vec,
-		       lower_20_sb(CHSPEC_CHANNEL(chspec))) ||
-		 isset(wlc_cm->quiet_channels.vec,
-		       upper_20_sb(CHSPEC_CHANNEL(chspec)))) :
-		isset(wlc_cm->quiet_channels.vec, CHSPEC_CHANNEL(chspec));
-}
-
 void
 brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, u16 chanspec,
 			 u8 local_constraint_qdbm)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
+	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
+	const struct ieee80211_reg_rule *reg_rule;
 	struct txpwr_limits txpwr;
+	int ret;
 
 	brcms_c_channel_reg_limits(wlc_cm, chanspec, &txpwr);
 
@@ -1181,8 +392,15 @@ brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, u16 chanspec,
 		wlc_cm, &txpwr, local_constraint_qdbm
 	);
 
+	/* set or restore gmode as required by regulatory */
+	ret = freq_reg_info(wlc->wiphy, ch->center_freq, 0, &reg_rule);
+	if (!ret && (reg_rule->flags & NL80211_RRF_NO_OFDM))
+		brcms_c_set_gmode(wlc, GMODE_LEGACY_B, false);
+	else
+		brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
+
 	brcms_b_set_chanspec(wlc->hw, chanspec,
-			      (brcms_c_quiet_chanspec(wlc_cm, chanspec) != 0),
+			      !!(ch->flags & IEEE80211_CHAN_PASSIVE_SCAN),
 			      &txpwr);
 }
 
@@ -1191,15 +409,14 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
 		       struct txpwr_limits *txpwr)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
+	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
 	uint i;
 	uint chan;
 	int maxpwr;
 	int delta;
 	const struct country_info *country;
 	struct brcms_band *band;
-	const struct locale_info *li;
 	int conducted_max = BRCMS_TXPWR_MAX;
-	int conducted_ofdm_max = BRCMS_TXPWR_MAX;
 	const struct locale_mimo_info *li_mimo;
 	int maxpwr20, maxpwr40;
 	int maxpwr_idx;
@@ -1207,67 +424,35 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
 
 	memset(txpwr, 0, sizeof(struct txpwr_limits));
 
-	if (!brcms_c_valid_chanspec_db(wlc_cm, chanspec)) {
-		country = brcms_c_country_lookup(wlc, wlc->autocountry_default);
-		if (country == NULL)
-			return;
-	} else {
-		country = wlc_cm->country;
-	}
+	if (WARN_ON(!ch))
+		return;
+
+	country = &wlc_cm->world_regd->country;
 
 	chan = CHSPEC_CHANNEL(chanspec);
 	band = wlc->bandstate[chspec_bandunit(chanspec)];
-	li = (band->bandtype == BRCM_BAND_5G) ?
-	    brcms_c_get_locale_5g(country->locale_5G) :
-	    brcms_c_get_locale_2g(country->locale_2G);
-
 	li_mimo = (band->bandtype == BRCM_BAND_5G) ?
 	    brcms_c_get_mimo_5g(country->locale_mimo_5G) :
 	    brcms_c_get_mimo_2g(country->locale_mimo_2G);
 
-	if (li->flags & BRCMS_EIRP) {
-		delta = band->antgain;
-	} else {
-		delta = 0;
-		if (band->antgain > QDB(6))
-			delta = band->antgain - QDB(6);	/* Excess over 6 dB */
-	}
+	delta = band->antgain;
 
-	if (li == &locale_i) {
+	if (band->bandtype == BRCM_BAND_2G)
 		conducted_max = QDB(22);
-		conducted_ofdm_max = QDB(22);
-	}
+
+	maxpwr = QDB(ch->max_power) - delta;
+	maxpwr = max(maxpwr, 0);
+	maxpwr = min(maxpwr, conducted_max);
 
 	/* CCK txpwr limits for 2.4G band */
 	if (band->bandtype == BRCM_BAND_2G) {
-		maxpwr = li->maxpwr[CHANNEL_POWER_IDX_2G_CCK(chan)];
-
-		maxpwr = maxpwr - delta;
-		maxpwr = max(maxpwr, 0);
-		maxpwr = min(maxpwr, conducted_max);
-
 		for (i = 0; i < BRCMS_NUM_RATES_CCK; i++)
 			txpwr->cck[i] = (u8) maxpwr;
 	}
 
-	/* OFDM txpwr limits for 2.4G or 5G bands */
-	if (band->bandtype == BRCM_BAND_2G)
-		maxpwr = li->maxpwr[CHANNEL_POWER_IDX_2G_OFDM(chan)];
-	else
-		maxpwr = li->maxpwr[CHANNEL_POWER_IDX_5G(chan)];
-
-	maxpwr = maxpwr - delta;
-	maxpwr = max(maxpwr, 0);
-	maxpwr = min(maxpwr, conducted_ofdm_max);
-
-	/* Keep OFDM lmit below CCK limit */
-	if (band->bandtype == BRCM_BAND_2G)
-		maxpwr = min_t(int, maxpwr, txpwr->cck[0]);
-
-	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
+	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
 		txpwr->ofdm[i] = (u8) maxpwr;
 
-	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
 		/*
 		 * OFDM 40 MHz SISO has the same power as the corresponding
 		 * MCS0-7 rate unless overriden by the locale specific code.
@@ -1282,14 +467,9 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
 		txpwr->ofdm_40_cdd[i] = 0;
 	}
 
-	/* MIMO/HT specific limits */
-	if (li_mimo->flags & BRCMS_EIRP) {
-		delta = band->antgain;
-	} else {
-		delta = 0;
-		if (band->antgain > QDB(6))
-			delta = band->antgain - QDB(6);	/* Excess over 6 dB */
-	}
+	delta = 0;
+	if (band->antgain > QDB(6))
+		delta = band->antgain - QDB(6);	/* Excess over 6 dB */
 
 	if (band->bandtype == BRCM_BAND_2G)
 		maxpwr_idx = (chan - 1);
@@ -1431,8 +611,7 @@ static bool brcms_c_chspec_malformed(u16 chanspec)
  * and they are also a legal HT combination
  */
 static bool
-brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec,
-			   bool dualband)
+brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
 	u8 channel = CHSPEC_CHANNEL(chspec);
@@ -1448,59 +627,163 @@ brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec,
 	    chspec_bandunit(chspec))
 		return false;
 
-	/* Check a 20Mhz channel */
-	if (CHSPEC_IS20(chspec)) {
-		if (dualband)
-			return brcms_c_valid_channel20_db(wlc_cm->wlc->cmi,
-							  channel);
-		else
-			return brcms_c_valid_channel20(wlc_cm->wlc->cmi,
-						       channel);
+	return true;
+}
+
+bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm, u16 chspec)
+{
+	return brcms_c_valid_chanspec_ext(wlc_cm, chspec);
+}
+
+static bool brcms_is_radar_freq(u16 center_freq)
+{
+	return center_freq >= 5260 && center_freq <= 5700;
+}
+
+static void brcms_reg_apply_radar_flags(struct wiphy *wiphy)
+{
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	int i;
+
+	sband = wiphy->bands[IEEE80211_BAND_5GHZ];
+	if (!sband)
+		return;
+
+	for (i = 0; i < sband->n_channels; i++) {
+		ch = &sband->channels[i];
+
+		if (!brcms_is_radar_freq(ch->center_freq))
+			continue;
+
+		/*
+		 * All channels in this range should be passive and have
+		 * DFS enabled.
+		 */
+		if (!(ch->flags & IEEE80211_CHAN_DISABLED))
+			ch->flags |= IEEE80211_CHAN_RADAR |
+				     IEEE80211_CHAN_NO_IBSS |
+				     IEEE80211_CHAN_PASSIVE_SCAN;
 	}
-#ifdef SUPPORT_40MHZ
-	/*
-	 * We know we are now checking a 40MHZ channel, so we should
-	 * only be here for NPHYS
-	 */
-	if (BRCMS_ISNPHY(wlc->band) || BRCMS_ISSSLPNPHY(wlc->band)) {
-		u8 upper_sideband = 0, idx;
-		u8 num_ch20_entries =
-		    sizeof(chan20_info) / sizeof(struct chan20_info);
-
-		if (!VALID_40CHANSPEC_IN_BAND(wlc, chspec_bandunit(chspec)))
-			return false;
-
-		if (dualband) {
-			if (!brcms_c_valid_channel20_db(wlc->cmi,
-							lower_20_sb(channel)) ||
-			    !brcms_c_valid_channel20_db(wlc->cmi,
-							upper_20_sb(channel)))
-				return false;
-		} else {
-			if (!brcms_c_valid_channel20(wlc->cmi,
-						     lower_20_sb(channel)) ||
-			    !brcms_c_valid_channel20(wlc->cmi,
-						     upper_20_sb(channel)))
-				return false;
+}
+
+static void
+brcms_reg_apply_beaconing_flags(struct wiphy *wiphy,
+				enum nl80211_reg_initiator initiator)
+{
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	const struct ieee80211_reg_rule *rule;
+	int band, i, ret;
+
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		sband = wiphy->bands[band];
+		if (!sband)
+			continue;
+
+		for (i = 0; i < sband->n_channels; i++) {
+			ch = &sband->channels[i];
+
+			if (ch->flags &
+			    (IEEE80211_CHAN_DISABLED | IEEE80211_CHAN_RADAR))
+				continue;
+
+			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
+				ret = freq_reg_info(wiphy, ch->center_freq,
+						    0, &rule);
+				if (ret)
+					continue;
+
+				if (!(rule->flags & NL80211_RRF_NO_IBSS))
+					ch->flags &= ~IEEE80211_CHAN_NO_IBSS;
+				if (!(rule->flags & NL80211_RRF_PASSIVE_SCAN))
+					ch->flags &=
+						~IEEE80211_CHAN_PASSIVE_SCAN;
+			} else if (ch->beacon_found) {
+				ch->flags &= ~(IEEE80211_CHAN_NO_IBSS |
+					       IEEE80211_CHAN_PASSIVE_SCAN);
+			}
 		}
+	}
+}
 
-		/* find the lower sideband info in the sideband array */
-		for (idx = 0; idx < num_ch20_entries; idx++) {
-			if (chan20_info[idx].sb == lower_20_sb(channel))
-				upper_sideband = chan20_info[idx].adj_sbs;
+static int brcms_reg_notifier(struct wiphy *wiphy,
+			      struct regulatory_request *request)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct brcms_info *wl = hw->priv;
+	struct brcms_c_info *wlc = wl->wlc;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	int band, i;
+	bool ch_found = false;
+
+	brcms_reg_apply_radar_flags(wiphy);
+
+	if (request->initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE)
+		brcms_reg_apply_beaconing_flags(wiphy, request->initiator);
+
+	/* Disable radio if all channels disallowed by regulatory */
+	for (band = 0; !ch_found && band < IEEE80211_NUM_BANDS; band++) {
+		sband = wiphy->bands[band];
+		if (!sband)
+			continue;
+
+		for (i = 0; !ch_found && i < sband->n_channels; i++) {
+			ch = &sband->channels[i];
+
+			if (!(ch->flags & IEEE80211_CHAN_DISABLED))
+				ch_found = true;
 		}
-		/* check that the lower sideband allows an upper sideband */
-		if ((upper_sideband & (CH_UPPER_SB | CH_EWA_VALID)) ==
-		    (CH_UPPER_SB | CH_EWA_VALID))
-			return true;
-		return false;
 	}
-#endif				/* 40 MHZ */
 
-	return false;
+	if (ch_found) {
+		mboolclr(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
+	} else {
+		mboolset(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
+		wiphy_err(wlc->wiphy, "wl%d: %s: no valid channel for \"%s\"\n",
+			  wlc->pub->unit, __func__, request->alpha2);
+	}
+
+	if (wlc->pub->_nbands > 1 || wlc->band->bandtype == BRCM_BAND_2G)
+		wlc_phy_chanspec_ch14_widefilter_set(wlc->band->pi,
+					brcms_c_japan_ccode(request->alpha2));
+
+	return 0;
 }
 
-bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm, u16 chspec)
+void brcms_c_regd_init(struct brcms_c_info *wlc)
 {
-	return brcms_c_valid_chanspec_ext(wlc_cm, chspec, true);
+	struct wiphy *wiphy = wlc->wiphy;
+	const struct brcms_regd *regd = wlc->cmi->world_regd;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	struct brcms_chanvec sup_chan;
+	struct brcms_band *band;
+	int band_idx, i;
+
+	/* Disable any channels not supported by the phy */
+	for (band_idx = 0; band_idx < wlc->pub->_nbands; band_idx++) {
+		band = wlc->bandstate[band_idx];
+
+		wlc_phy_chanspec_band_validch(band->pi, band->bandtype,
+					      &sup_chan);
+
+		if (band_idx == BAND_2G_INDEX)
+			sband = wiphy->bands[IEEE80211_BAND_2GHZ];
+		else
+			sband = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+		for (i = 0; i < sband->n_channels; i++) {
+			ch = &sband->channels[i];
+			if (!isset(sup_chan.vec, ch->hw_value))
+				ch->flags |= IEEE80211_CHAN_DISABLED;
+		}
+	}
+
+	wlc->wiphy->reg_notifier = brcms_reg_notifier;
+	wlc->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
+			     WIPHY_FLAG_STRICT_REGULATORY;
+	wiphy_apply_custom_regulatory(wlc->wiphy, regd->regdomain);
+	brcms_reg_apply_beaconing_flags(wiphy, NL80211_REGDOM_SET_BY_DRIVER);
 }
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.h b/drivers/net/wireless/brcm80211/brcmsmac/channel.h
index 808cb4fbfbe..006483a0abe 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.h
@@ -37,9 +37,6 @@ brcms_c_channel_mgr_attach(struct brcms_c_info *wlc);
 
 extern void brcms_c_channel_mgr_detach(struct brcms_cm_info *wlc_cm);
 
-extern u8 brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
-					   uint bandunit);
-
 extern bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm,
 				      u16 chspec);
 
@@ -49,5 +46,6 @@ extern void brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm,
 extern void brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm,
 				     u16 chanspec,
 				     u8 local_constraint_qdbm);
+extern void brcms_c_regd_init(struct brcms_c_info *wlc);
 
 #endif				/* _WLC_CHANNEL_H */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/dma.c b/drivers/net/wireless/brcm80211/brcmsmac/dma.c
index 11054ae9d4f..5e53305bd9a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/dma.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/dma.c
@@ -573,6 +573,7 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 	struct dma_info *di;
 	u8 rev = core->id.rev;
 	uint size;
+	struct si_info *sii = container_of(sih, struct si_info, pub);
 
 	/* allocate private info structure */
 	di = kzalloc(sizeof(struct dma_info), GFP_ATOMIC);
@@ -633,16 +634,20 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 	 */
 	di->ddoffsetlow = 0;
 	di->dataoffsetlow = 0;
-	/* add offset for pcie with DMA64 bus */
-	di->ddoffsetlow = 0;
-	di->ddoffsethigh = SI_PCIE_DMA_H32;
+	/* for pci bus, add offset */
+	if (sii->icbus->hosttype == BCMA_HOSTTYPE_PCI) {
+		/* add offset for pcie with DMA64 bus */
+		di->ddoffsetlow = 0;
+		di->ddoffsethigh = SI_PCIE_DMA_H32;
+	}
 	di->dataoffsetlow = di->ddoffsetlow;
 	di->dataoffsethigh = di->ddoffsethigh;
+
 	/* WAR64450 : DMACtl.Addr ext fields are not supported in SDIOD core. */
-	if ((core->id.id == SDIOD_CORE_ID)
+	if ((core->id.id == BCMA_CORE_SDIO_DEV)
 	    && ((rev > 0) && (rev <= 2)))
 		di->addrext = false;
-	else if ((core->id.id == I2S_CORE_ID) &&
+	else if ((core->id.id == BCMA_CORE_I2S) &&
 		 ((rev == 0) || (rev == 1)))
 		di->addrext = false;
 	else
@@ -1433,7 +1438,7 @@ void dma_walk_packets(struct dma_pub *dmah, void (*callback_fnc)
 	struct ieee80211_tx_info *tx_info;
 
 	while (i != end) {
-		skb = (struct sk_buff *)di->txp[i];
+		skb = di->txp[i];
 		if (skb != NULL) {
 			tx_info = (struct ieee80211_tx_info *)skb->cb;
 			(callback_fnc)(tx_info, arg_a);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index 50f92a0b7c4..9e79d47e077 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -267,6 +267,7 @@ static void brcms_set_basic_rate(struct brcm_rateset *rs, u16 rate, bool is_br)
 static void brcms_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct brcms_info *wl = hw->priv;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 
 	spin_lock_bh(&wl->lock);
 	if (!wl->pub->up) {
@@ -275,6 +276,7 @@ static void brcms_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		goto done;
 	}
 	brcms_c_sendpkt_mac80211(wl->wlc, skb, hw);
+	tx_info->rate_driver_data[0] = tx_info->control.sta;
  done:
 	spin_unlock_bh(&wl->lock);
 }
@@ -319,8 +321,7 @@ static void brcms_ops_stop(struct ieee80211_hw *hw)
 		return;
 
 	spin_lock_bh(&wl->lock);
-	status = brcms_c_chipmatch(wl->wlc->hw->vendorid,
-				   wl->wlc->hw->deviceid);
+	status = brcms_c_chipmatch(wl->wlc->hw->d11core);
 	spin_unlock_bh(&wl->lock);
 	if (!status) {
 		wiphy_err(wl->wiphy,
@@ -721,14 +722,6 @@ static const struct ieee80211_ops brcms_ops = {
 	.flush = brcms_ops_flush,
 };
 
-/*
- * is called in brcms_bcma_probe() context, therefore no locking required.
- */
-static int brcms_set_hint(struct brcms_info *wl, char *abbrev)
-{
-	return regulatory_hint(wl->pub->ieee_hw->wiphy, abbrev);
-}
-
 void brcms_dpc(unsigned long data)
 {
 	struct brcms_info *wl;
@@ -1058,6 +1051,8 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 		goto fail;
 	}
 
+	brcms_c_regd_init(wl->wlc);
+
 	memcpy(perm, &wl->pub->cur_etheraddr, ETH_ALEN);
 	if (WARN_ON(!is_valid_ether_addr(perm)))
 		goto fail;
@@ -1068,9 +1063,9 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
 		wiphy_err(wl->wiphy, "%s: ieee80211_register_hw failed, status"
 			  "%d\n", __func__, err);
 
-	if (wl->pub->srom_ccode[0] && brcms_set_hint(wl, wl->pub->srom_ccode))
-		wiphy_err(wl->wiphy, "%s: regulatory_hint failed, status %d\n",
-			  __func__, err);
+	if (wl->pub->srom_ccode[0] &&
+	    regulatory_hint(wl->wiphy, wl->pub->srom_ccode))
+		wiphy_err(wl->wiphy, "%s: regulatory hint failed\n", __func__);
 
 	n_adapters_found++;
 	return wl;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index 19db4052c44..03ca6532484 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -18,6 +18,7 @@
 
 #include <linux/pci_ids.h>
 #include <linux/if_ether.h>
+#include <net/cfg80211.h>
 #include <net/mac80211.h>
 #include <brcm_hw_ids.h>
 #include <aiutils.h>
@@ -268,7 +269,7 @@ struct brcms_c_bit_desc {
  */
 
 /* Starting corerev for the fifo size table */
-#define XMTFIFOTBL_STARTREV	20
+#define XMTFIFOTBL_STARTREV	17
 
 struct d11init {
 	__le16 addr;
@@ -332,6 +333,12 @@ const u8 wlc_prio2prec_map[] = {
 };
 
 static const u16 xmtfifo_sz[][NFIFO] = {
+	/* corerev 17: 5120, 49152, 49152, 5376, 4352, 1280 */
+	{20, 192, 192, 21, 17, 5},
+	/* corerev 18: */
+	{0, 0, 0, 0, 0, 0},
+	/* corerev 19: */
+	{0, 0, 0, 0, 0, 0},
 	/* corerev 20: 5120, 49152, 49152, 5376, 4352, 1280 */
 	{20, 192, 192, 21, 17, 5},
 	/* corerev 21: 2304, 14848, 5632, 3584, 3584, 1280 */
@@ -342,6 +349,14 @@ static const u16 xmtfifo_sz[][NFIFO] = {
 	{20, 192, 192, 21, 17, 5},
 	/* corerev 24: 2304, 14848, 5632, 3584, 3584, 1280 */
 	{9, 58, 22, 14, 14, 5},
+	/* corerev 25: */
+	{0, 0, 0, 0, 0, 0},
+	/* corerev 26: */
+	{0, 0, 0, 0, 0, 0},
+	/* corerev 27: */
+	{0, 0, 0, 0, 0, 0},
+	/* corerev 28: 2304, 14848, 5632, 3584, 3584, 1280 */
+	{9, 58, 22, 14, 14, 5},
 };
 
 #ifdef DEBUG
@@ -878,7 +893,7 @@ brcms_c_dotxstatus(struct brcms_c_info *wlc, struct tx_status *txs)
 	tx_info = IEEE80211_SKB_CB(p);
 	h = (struct ieee80211_hdr *)((u8 *) (txh + 1) + D11_PHY_HDR_LEN);
 
-	if (tx_info->control.sta)
+	if (tx_info->rate_driver_data[0])
 		scb = &wlc->pri_scb;
 
 	if (tx_info->flags & IEEE80211_TX_CTL_AMPDU) {
@@ -1941,7 +1956,8 @@ static bool brcms_b_radio_read_hwdisabled(struct brcms_hardware *wlc_hw)
 		 * accesses phyreg throughput mac. This can be skipped since
 		 * only mac reg is accessed below
 		 */
-		flags |= SICF_PCLKE;
+		if (D11REV_GE(wlc_hw->corerev, 18))
+			flags |= SICF_PCLKE;
 
 		/*
 		 * TODO: test suspend/resume
@@ -2022,7 +2038,8 @@ void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
 	 * phyreg throughput mac, AND phy_reset is skipped at early stage when
 	 * band->pi is invalid. need to enable PHY CLK
 	 */
-	flags |= SICF_PCLKE;
+	if (D11REV_GE(wlc_hw->corerev, 18))
+		flags |= SICF_PCLKE;
 
 	/*
 	 * reset the core
@@ -2125,8 +2142,8 @@ void brcms_b_switch_macfreq(struct brcms_hardware *wlc_hw, u8 spurmode)
 {
 	struct bcma_device *core = wlc_hw->d11core;
 
-	if ((ai_get_chip_id(wlc_hw->sih) == BCM43224_CHIP_ID) ||
-	    (ai_get_chip_id(wlc_hw->sih) == BCM43225_CHIP_ID)) {
+	if ((ai_get_chip_id(wlc_hw->sih) == BCMA_CHIP_ID_BCM43224) ||
+	    (ai_get_chip_id(wlc_hw->sih) == BCMA_CHIP_ID_BCM43225)) {
 		if (spurmode == WL_SPURAVOID_ON2) {	/* 126Mhz */
 			bcma_write16(core, D11REGOFFS(tsf_clk_frac_l), 0x2082);
 			bcma_write16(core, D11REGOFFS(tsf_clk_frac_h), 0x8);
@@ -2790,7 +2807,7 @@ void brcms_b_core_phypll_ctl(struct brcms_hardware *wlc_hw, bool on)
 	tmp = 0;
 
 	if (on) {
-		if ((ai_get_chip_id(wlc_hw->sih) == BCM4313_CHIP_ID)) {
+		if ((ai_get_chip_id(wlc_hw->sih) == BCMA_CHIP_ID_BCM4313)) {
 			bcma_set32(core, D11REGOFFS(clk_ctl_st),
 				   CCS_ERSRC_REQ_HT |
 				   CCS_ERSRC_REQ_D11PLL |
@@ -3139,20 +3156,6 @@ void brcms_c_reset(struct brcms_c_info *wlc)
 	brcms_b_reset(wlc->hw);
 }
 
-/* Return the channel the driver should initialize during brcms_c_init.
- * the channel may have to be changed from the currently configured channel
- * if other configurations are in conflict (bandlocked, 11n mode disabled,
- * invalid channel for current country, etc.)
- */
-static u16 brcms_c_init_chanspec(struct brcms_c_info *wlc)
-{
-	u16 chanspec =
-	    1 | WL_CHANSPEC_BW_20 | WL_CHANSPEC_CTL_SB_NONE |
-	    WL_CHANSPEC_BAND_2G;
-
-	return chanspec;
-}
-
 void brcms_c_init_scb(struct scb *scb)
 {
 	int i;
@@ -4231,9 +4234,8 @@ static void brcms_c_radio_timer(void *arg)
 }
 
 /* common low-level watchdog code */
-static void brcms_b_watchdog(void *arg)
+static void brcms_b_watchdog(struct brcms_c_info *wlc)
 {
-	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
 	struct brcms_hardware *wlc_hw = wlc->hw;
 
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
@@ -4254,10 +4256,8 @@ static void brcms_b_watchdog(void *arg)
 }
 
 /* common watchdog code */
-static void brcms_c_watchdog(void *arg)
+static void brcms_c_watchdog(struct brcms_c_info *wlc)
 {
-	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
-
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
 	if (!wlc->pub->up)
@@ -4297,7 +4297,9 @@ static void brcms_c_watchdog(void *arg)
 
 static void brcms_c_watchdog_by_timer(void *arg)
 {
-	brcms_c_watchdog(arg);
+	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
+
+	brcms_c_watchdog(wlc);
 }
 
 static bool brcms_c_timers_init(struct brcms_c_info *wlc, int unit)
@@ -4467,11 +4469,9 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	}
 
 	/* verify again the device is supported */
-	if (core->bus->hosttype == BCMA_HOSTTYPE_PCI &&
-	    !brcms_c_chipmatch(pcidev->vendor, pcidev->device)) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported "
-			"vendor/device (0x%x/0x%x)\n",
-			 unit, pcidev->vendor, pcidev->device);
+	if (!brcms_c_chipmatch(core)) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported device\n",
+			 unit);
 		err = 12;
 		goto fail;
 	}
@@ -4541,7 +4541,7 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 	else
 		wlc_hw->_nbands = 1;
 
-	if ((ai_get_chip_id(wlc_hw->sih) == BCM43225_CHIP_ID))
+	if ((ai_get_chip_id(wlc_hw->sih) == BCMA_CHIP_ID_BCM43225))
 		wlc_hw->_nbands = 1;
 
 	/* BMAC_NOTE: remove init of pub values when brcms_c_attach()
@@ -4608,8 +4608,12 @@ static int brcms_b_attach(struct brcms_c_info *wlc, struct bcma_device *core,
 		wlc_hw->machwcap_backup = wlc_hw->machwcap;
 
 		/* init tx fifo size */
+		WARN_ON((wlc_hw->corerev - XMTFIFOTBL_STARTREV) < 0 ||
+			(wlc_hw->corerev - XMTFIFOTBL_STARTREV) >
+				ARRAY_SIZE(xmtfifo_sz));
 		wlc_hw->xmtfifo_sz =
 		    xmtfifo_sz[(wlc_hw->corerev - XMTFIFOTBL_STARTREV)];
+		WARN_ON(!wlc_hw->xmtfifo_sz[0]);
 
 		/* Get a phy for this band */
 		wlc_hw->band->pi =
@@ -5049,7 +5053,7 @@ static void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
 	wlc_hw->wlc->pub->hw_up = true;
 
 	if ((wlc_hw->boardflags & BFL_FEM)
-	    && (ai_get_chip_id(wlc_hw->sih) == BCM4313_CHIP_ID)) {
+	    && (ai_get_chip_id(wlc_hw->sih) == BCMA_CHIP_ID_BCM4313)) {
 		if (!
 		    (wlc_hw->boardrev >= 0x1250
 		     && (wlc_hw->boardflags & BFL_FEM_BT)))
@@ -5129,6 +5133,8 @@ static void brcms_c_wme_retries_write(struct brcms_c_info *wlc)
 /* make interface operational */
 int brcms_c_up(struct brcms_c_info *wlc)
 {
+	struct ieee80211_channel *ch;
+
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
 	/* HW is turned off so don't try to access it */
@@ -5141,7 +5147,7 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	}
 
 	if ((wlc->pub->boardflags & BFL_FEM)
-	    && (ai_get_chip_id(wlc->hw->sih) == BCM4313_CHIP_ID)) {
+	    && (ai_get_chip_id(wlc->hw->sih) == BCMA_CHIP_ID_BCM4313)) {
 		if (wlc->pub->boardrev >= 0x1250
 		    && (wlc->pub->boardflags & BFL_FEM_BT))
 			brcms_b_mhf(wlc->hw, MHF5, MHF5_4313_GPIOCTRL,
@@ -5195,8 +5201,9 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	wlc->pub->up = true;
 
 	if (wlc->bandinit_pending) {
+		ch = wlc->pub->ieee_hw->conf.channel;
 		brcms_c_suspend_mac_and_wait(wlc);
-		brcms_c_set_chanspec(wlc, wlc->default_bss->chanspec);
+		brcms_c_set_chanspec(wlc, ch20mhz_chspec(ch->hw_value));
 		wlc->bandinit_pending = false;
 		brcms_c_enable_mac(wlc);
 	}
@@ -5397,11 +5404,6 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 	else
 		return -EINVAL;
 
-	/* Legacy or bust when no OFDM is supported by regulatory */
-	if ((brcms_c_channel_locale_flags_in_band(wlc->cmi, band->bandunit) &
-	     BRCMS_NO_OFDM) && (gmode != GMODE_LEGACY_B))
-		return -EINVAL;
-
 	/* update configuration value */
 	if (config)
 		brcms_c_protection_upd(wlc, BRCMS_PROT_G_USER, gmode);
@@ -5782,8 +5784,12 @@ void brcms_c_print_txstatus(struct tx_status *txs)
 		 (txs->ackphyrxsh & PRXS1_SQ_MASK) >> PRXS1_SQ_SHIFT);
 }
 
-bool brcms_c_chipmatch(u16 vendor, u16 device)
+static bool brcms_c_chipmatch_pci(struct bcma_device *core)
 {
+	struct pci_dev *pcidev = core->bus->host_pci;
+	u16 vendor = pcidev->vendor;
+	u16 device = pcidev->device;
+
 	if (vendor != PCI_VENDOR_ID_BROADCOM) {
 		pr_err("unknown vendor id %04x\n", vendor);
 		return false;
@@ -5802,6 +5808,30 @@ bool brcms_c_chipmatch(u16 vendor, u16 device)
 	return false;
 }
 
+static bool brcms_c_chipmatch_soc(struct bcma_device *core)
+{
+	struct bcma_chipinfo *chipinfo = &core->bus->chipinfo;
+
+	if (chipinfo->id == BCMA_CHIP_ID_BCM4716)
+		return true;
+
+	pr_err("unknown chip id %04x\n", chipinfo->id);
+	return false;
+}
+
+bool brcms_c_chipmatch(struct bcma_device *core)
+{
+	switch (core->bus->hosttype) {
+	case BCMA_HOSTTYPE_PCI:
+		return brcms_c_chipmatch_pci(core);
+	case BCMA_HOSTTYPE_SOC:
+		return brcms_c_chipmatch_soc(core);
+	default:
+		pr_err("unknown host type: %i\n", core->bus->hosttype);
+		return false;
+	}
+}
+
 #if defined(DEBUG)
 void brcms_c_print_txdesc(struct d11txh *txh)
 {
@@ -8201,19 +8231,12 @@ bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
 void brcms_c_init(struct brcms_c_info *wlc, bool mute_tx)
 {
 	struct bcma_device *core = wlc->hw->d11core;
+	struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
 	u16 chanspec;
 
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
-	/*
-	 * This will happen if a big-hammer was executed. In
-	 * that case, we want to go back to the channel that
-	 * we were on and not new channel
-	 */
-	if (wlc->pub->associated)
-		chanspec = wlc->home_chanspec;
-	else
-		chanspec = brcms_c_init_chanspec(wlc);
+	chanspec = ch20mhz_chspec(ch->hw_value);
 
 	brcms_b_init(wlc->hw, chanspec);
 
@@ -8318,7 +8341,7 @@ brcms_c_attach(struct brcms_info *wl, struct bcma_device *core, uint unit,
 	struct brcms_pub *pub;
 
 	/* allocate struct brcms_c_info state and its substructures */
-	wlc = (struct brcms_c_info *) brcms_c_attach_malloc(unit, &err, 0);
+	wlc = brcms_c_attach_malloc(unit, &err, 0);
 	if (wlc == NULL)
 		goto fail;
 	wlc->wiphy = wl->wiphy;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
index 264f8c4c703..91937c5025c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
@@ -198,6 +198,8 @@ u16 read_radio_reg(struct brcms_phy *pi, u16 addr)
 
 void write_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
+	struct si_info *sii = container_of(pi->sh->sih, struct si_info, pub);
+
 	if ((D11REV_GE(pi->sh->corerev, 24)) ||
 	    (D11REV_IS(pi->sh->corerev, 22)
 	     && (pi->pubpi.phy_type != PHY_TYPE_SSN))) {
@@ -209,7 +211,8 @@ void write_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 		bcma_write16(pi->d11core, D11REGOFFS(phy4wdatalo), val);
 	}
 
-	if (++pi->phy_wreg >= pi->phy_wreg_limit) {
+	if ((sii->icbus->hosttype == BCMA_HOSTTYPE_PCI) &&
+	    (++pi->phy_wreg >= pi->phy_wreg_limit)) {
 		(void)bcma_read32(pi->d11core, D11REGOFFS(maccontrol));
 		pi->phy_wreg = 0;
 	}
@@ -292,10 +295,13 @@ void write_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 	bcma_wflush16(pi->d11core, D11REGOFFS(phyregaddr), addr);
 	bcma_write16(pi->d11core, D11REGOFFS(phyregdata), val);
 	if (addr == 0x72)
-		(void)bcma_read16(pi->d11core, D11REGOFFS(phyversion));
+		(void)bcma_read16(pi->d11core, D11REGOFFS(phyregdata));
 #else
+	struct si_info *sii = container_of(pi->sh->sih, struct si_info, pub);
+
 	bcma_write32(pi->d11core, D11REGOFFS(phyregaddr), addr | (val << 16));
-	if (++pi->phy_wreg >= pi->phy_wreg_limit) {
+	if ((sii->icbus->hosttype == BCMA_HOSTTYPE_PCI) &&
+	    (++pi->phy_wreg >= pi->phy_wreg_limit)) {
 		pi->phy_wreg = 0;
 		(void)bcma_read16(pi->d11core, D11REGOFFS(phyversion));
 	}
@@ -837,7 +843,7 @@ wlc_phy_table_addr(struct brcms_phy *pi, uint tbl_id, uint tbl_offset,
 	pi->tbl_data_hi = tblDataHi;
 	pi->tbl_data_lo = tblDataLo;
 
-	if (pi->sh->chip == BCM43224_CHIP_ID &&
+	if (pi->sh->chip == BCMA_CHIP_ID_BCM43224 &&
 	    pi->sh->chiprev == 1) {
 		pi->tbl_addr = tblAddr;
 		pi->tbl_save_id = tbl_id;
@@ -847,7 +853,7 @@ wlc_phy_table_addr(struct brcms_phy *pi, uint tbl_id, uint tbl_offset,
 
 void wlc_phy_table_data_write(struct brcms_phy *pi, uint width, u32 val)
 {
-	if ((pi->sh->chip == BCM43224_CHIP_ID) &&
+	if ((pi->sh->chip == BCMA_CHIP_ID_BCM43224) &&
 	    (pi->sh->chiprev == 1) &&
 	    (pi->tbl_save_id == NPHY_TBL_ID_ANTSWCTRLLUT)) {
 		read_phy_reg(pi, pi->tbl_data_lo);
@@ -881,7 +887,7 @@ wlc_phy_write_table(struct brcms_phy *pi, const struct phytbl_info *ptbl_info,
 
 	for (idx = 0; idx < ptbl_info->tbl_len; idx++) {
 
-		if ((pi->sh->chip == BCM43224_CHIP_ID) &&
+		if ((pi->sh->chip == BCMA_CHIP_ID_BCM43224) &&
 		    (pi->sh->chiprev == 1) &&
 		    (tbl_id == NPHY_TBL_ID_ANTSWCTRLLUT)) {
 			read_phy_reg(pi, tblDataLo);
@@ -918,7 +924,7 @@ wlc_phy_read_table(struct brcms_phy *pi, const struct phytbl_info *ptbl_info,
 
 	for (idx = 0; idx < ptbl_info->tbl_len; idx++) {
 
-		if ((pi->sh->chip == BCM43224_CHIP_ID) &&
+		if ((pi->sh->chip == BCMA_CHIP_ID_BCM43224) &&
 		    (pi->sh->chiprev == 1)) {
 			(void)read_phy_reg(pi, tblDataLo);
 
@@ -2894,7 +2900,7 @@ const u8 *wlc_phy_get_ofdm_rate_lookup(void)
 
 void wlc_lcnphy_epa_switch(struct brcms_phy *pi, bool mode)
 {
-	if ((pi->sh->chip == BCM4313_CHIP_ID) &&
+	if ((pi->sh->chip == BCMA_CHIP_ID_BCM4313) &&
 	    (pi->sh->boardflags & BFL_FEM)) {
 		if (mode) {
 			u16 txant = 0;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
index 13b261517cc..65db9b7458d 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
@@ -14358,7 +14358,7 @@ void wlc_phy_nphy_tkip_rifs_war(struct brcms_phy *pi, u8 rifs)
 
 	wlc_phy_write_txmacreg_nphy(pi, holdoff, delay);
 
-	if (pi && pi->sh && (pi->sh->_rifs_phy != rifs))
+	if (pi->sh && (pi->sh->_rifs_phy != rifs))
 		pi->sh->_rifs_phy = rifs;
 }
 
@@ -17893,6 +17893,8 @@ static u32 *wlc_phy_get_ipa_gaintbl_nphy(struct brcms_phy *pi)
 					nphy_tpc_txgain_ipa_2g_2057rev7;
 		} else if (NREV_IS(pi->pubpi.phy_rev, 6)) {
 			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev6;
+			if (pi->sh->chip == BCMA_CHIP_ID_BCM47162)
+				tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev5;
 		} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
 			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev5;
 		} else {
@@ -19254,8 +19256,14 @@ static void wlc_phy_spurwar_nphy(struct brcms_phy *pi)
 			case 38:
 			case 102:
 			case 118:
-				nphy_adj_tone_id_buf[0] = 0;
-				nphy_adj_noise_var_buf[0] = 0x0;
+				if ((pi->sh->chip == BCMA_CHIP_ID_BCM4716) &&
+				    (pi->sh->chippkg == BCMA_PKG_ID_BCM4717)) {
+					nphy_adj_tone_id_buf[0] = 32;
+					nphy_adj_noise_var_buf[0] = 0x21f;
+				} else {
+					nphy_adj_tone_id_buf[0] = 0;
+					nphy_adj_noise_var_buf[0] = 0x0;
+				}
 				break;
 			case 134:
 				nphy_adj_tone_id_buf[0] = 32;
@@ -19309,8 +19317,8 @@ void wlc_phy_init_nphy(struct brcms_phy *pi)
 		pi->measure_hold |= PHY_HOLD_FOR_NOT_ASSOC;
 
 	if ((ISNPHY(pi)) && (NREV_GE(pi->pubpi.phy_rev, 5)) &&
-	    ((pi->sh->chippkg == BCM4717_PKG_ID) ||
-	     (pi->sh->chippkg == BCM4718_PKG_ID))) {
+	    ((pi->sh->chippkg == BCMA_PKG_ID_BCM4717) ||
+	     (pi->sh->chippkg == BCMA_PKG_ID_BCM4718))) {
 		if ((pi->sh->boardflags & BFL_EXTLNA) &&
 		    (CHSPEC_IS2G(pi->radio_chanspec)))
 			ai_cc_reg(pi->sh->sih,
@@ -19318,6 +19326,10 @@ void wlc_phy_init_nphy(struct brcms_phy *pi)
 				  0x40, 0x40);
 	}
 
+	if ((!PHY_IPA(pi)) && (pi->sh->chip == BCMA_CHIP_ID_BCM5357))
+		si_pmu_chipcontrol(pi->sh->sih, 1, CCTRL5357_EXTPA,
+				   CCTRL5357_EXTPA);
+
 	if ((pi->nphy_gband_spurwar2_en) && CHSPEC_IS2G(pi->radio_chanspec) &&
 	    CHSPEC_IS40(pi->radio_chanspec)) {
 
@@ -20695,12 +20707,22 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 			write_radio_reg(pi, RADIO_2056_SYN_PLL_LOOPFILTER2 |
 					RADIO_2056_SYN, 0x1f);
 
-			write_radio_reg(pi,
-					RADIO_2056_SYN_PLL_LOOPFILTER4 |
-					RADIO_2056_SYN, 0xb);
-			write_radio_reg(pi,
-					RADIO_2056_SYN_PLL_CP2 |
-					RADIO_2056_SYN, 0x14);
+			if ((pi->sh->chip == BCMA_CHIP_ID_BCM4716) ||
+			    (pi->sh->chip == BCMA_CHIP_ID_BCM47162)) {
+				write_radio_reg(pi,
+						RADIO_2056_SYN_PLL_LOOPFILTER4 |
+						RADIO_2056_SYN, 0x14);
+				write_radio_reg(pi,
+						RADIO_2056_SYN_PLL_CP2 |
+						RADIO_2056_SYN, 0x00);
+			} else {
+				write_radio_reg(pi,
+						RADIO_2056_SYN_PLL_LOOPFILTER4 |
+						RADIO_2056_SYN, 0xb);
+				write_radio_reg(pi,
+						RADIO_2056_SYN_PLL_CP2 |
+						RADIO_2056_SYN, 0x14);
+			}
 		}
 	}
 
@@ -20747,24 +20769,30 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
 						 PADG_IDAC, 0xcc);
 
-				bias = 0x25;
-				cascbias = 0x20;
+				if ((pi->sh->chip == BCMA_CHIP_ID_BCM4716) ||
+				    (pi->sh->chip == BCMA_CHIP_ID_BCM47162)) {
+					bias = 0x40;
+					cascbias = 0x45;
+					pag_boost_tune = 0x5;
+					pgag_boost_tune = 0x33;
+					padg_boost_tune = 0x77;
+					mixg_boost_tune = 0x55;
+				} else {
+					bias = 0x25;
+					cascbias = 0x20;
 
-				if ((pi->sh->chip ==
-				     BCM43224_CHIP_ID)
-				    || (pi->sh->chip ==
-					BCM43225_CHIP_ID)) {
-					if (pi->sh->chippkg ==
-					    BCM43224_FAB_SMIC) {
+					if ((pi->sh->chip == BCMA_CHIP_ID_BCM43224 ||
+					     pi->sh->chip == BCMA_CHIP_ID_BCM43225) &&
+					    pi->sh->chippkg == BCMA_PKG_ID_BCM43224_FAB_SMIC) {
 						bias = 0x2a;
 						cascbias = 0x38;
 					}
-				}
 
-				pag_boost_tune = 0x4;
-				pgag_boost_tune = 0x03;
-				padg_boost_tune = 0x77;
-				mixg_boost_tune = 0x65;
+					pag_boost_tune = 0x4;
+					pgag_boost_tune = 0x03;
+					padg_boost_tune = 0x77;
+					mixg_boost_tune = 0x65;
+				}
 
 				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
 						 INTPAG_IMAIN_STAT, bias);
@@ -20863,11 +20891,10 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 
 			cascbias = 0x30;
 
-			if ((pi->sh->chip == BCM43224_CHIP_ID) ||
-			    (pi->sh->chip == BCM43225_CHIP_ID)) {
-				if (pi->sh->chippkg == BCM43224_FAB_SMIC)
-					cascbias = 0x35;
-			}
+			if ((pi->sh->chip == BCMA_CHIP_ID_BCM43224 ||
+			     pi->sh->chip == BCMA_CHIP_ID_BCM43225) &&
+			    pi->sh->chippkg == BCMA_PKG_ID_BCM43224_FAB_SMIC)
+				cascbias = 0x35;
 
 			pabias = (pi->phy_pabias == 0) ? 0x30 : pi->phy_pabias;
 
@@ -21106,6 +21133,7 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, u16 chanspec,
 			    const struct nphy_sfo_cfg *ci)
 {
 	u16 val;
+	struct si_info *sii = container_of(pi->sh->sih, struct si_info, pub);
 
 	val = read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand;
 	if (CHSPEC_IS5G(chanspec) && !val) {
@@ -21178,22 +21206,32 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, u16 chanspec,
 		} else if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 			if (val == 54)
 				spuravoid = 1;
-		} else {
-			if (pi->nphy_aband_spurwar_en &&
-			    ((val == 38) || (val == 102)
-			     || (val == 118)))
+		} else if (pi->nphy_aband_spurwar_en &&
+		    ((val == 38) || (val == 102) || (val == 118))) {
+			if ((pi->sh->chip == BCMA_CHIP_ID_BCM4716)
+			    && (pi->sh->chippkg == BCMA_PKG_ID_BCM4717)) {
+				spuravoid = 0;
+			} else {
 				spuravoid = 1;
+			}
 		}
 
 		if (pi->phy_spuravoid == SPURAVOID_FORCEON)
 			spuravoid = 1;
 
-		wlapi_bmac_core_phypll_ctl(pi->sh->physhim, false);
-		si_pmu_spuravoid_pllupdate(pi->sh->sih, spuravoid);
-		wlapi_bmac_core_phypll_ctl(pi->sh->physhim, true);
+		if ((pi->sh->chip == BCMA_CHIP_ID_BCM4716) ||
+		    (pi->sh->chip == BCMA_CHIP_ID_BCM43225)) {
+			bcma_pmu_spuravoid_pllupdate(&sii->icbus->drv_cc,
+						     spuravoid);
+		} else {
+			wlapi_bmac_core_phypll_ctl(pi->sh->physhim, false);
+			bcma_pmu_spuravoid_pllupdate(&sii->icbus->drv_cc,
+						     spuravoid);
+			wlapi_bmac_core_phypll_ctl(pi->sh->physhim, true);
+		}
 
-		if ((pi->sh->chip == BCM43224_CHIP_ID) ||
-		    (pi->sh->chip == BCM43225_CHIP_ID)) {
+		if ((pi->sh->chip == BCMA_CHIP_ID_BCM43224) ||
+		    (pi->sh->chip == BCMA_CHIP_ID_BCM43225)) {
 			if (spuravoid == 1) {
 				bcma_write16(pi->d11core,
 					     D11REGOFFS(tsf_clk_frac_l),
@@ -21209,7 +21247,9 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, u16 chanspec,
 			}
 		}
 
-		wlapi_bmac_core_phypll_reset(pi->sh->physhim);
+		if (!((pi->sh->chip == BCMA_CHIP_ID_BCM4716) ||
+		      (pi->sh->chip == BCMA_CHIP_ID_BCM47162)))
+			wlapi_bmac_core_phypll_reset(pi->sh->physhim);
 
 		mod_phy_reg(pi, 0x01, (0x1 << 15),
 			    ((spuravoid > 0) ? (0x1 << 15) : 0));
@@ -22171,9 +22211,15 @@ s16 wlc_phy_tempsense_nphy(struct brcms_phy *pi)
 		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_AFECTRL, 1, 0x03, 16,
 					 &auxADC_rssi_ctrlH_save);
 
-		radio_temp[0] = (179 * (radio_temp[1] + radio_temp2[1])
-				 + 82 * (auxADC_Vl) - 28861 +
-				 128) / 256;
+		if (pi->sh->chip == BCMA_CHIP_ID_BCM5357) {
+			radio_temp[0] = (193 * (radio_temp[1] + radio_temp2[1])
+					 + 88 * (auxADC_Vl) - 27111 +
+					 128) / 256;
+		} else {
+			radio_temp[0] = (179 * (radio_temp[1] + radio_temp2[1])
+					 + 82 * (auxADC_Vl) - 28861 +
+					 128) / 256;
+		}
 
 		offset = (s16) pi->phy_tempsense_offset;
 
@@ -24923,14 +24969,16 @@ wlc_phy_a2_nphy(struct brcms_phy *pi, struct nphy_ipa_txcalgains *txgains,
 			if (txgains->useindex) {
 				phy_a4 = 15 - ((txgains->index) >> 3);
 				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					if (NREV_GE(pi->pubpi.phy_rev, 6))
+					if (NREV_GE(pi->pubpi.phy_rev, 6) &&
+					    pi->sh->chip == BCMA_CHIP_ID_BCM47162) {
+						phy_a5 = 0x10f7 | (phy_a4 << 8);
+					} else if (NREV_GE(pi->pubpi.phy_rev, 6)) {
 						phy_a5 = 0x00f7 | (phy_a4 << 8);
-
-					else
-					if (NREV_IS(pi->pubpi.phy_rev, 5))
+					} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
 						phy_a5 = 0x10f7 | (phy_a4 << 8);
-					else
+					} else {
 						phy_a5 = 0x50f7 | (phy_a4 << 8);
+					}
 				} else {
 					phy_a5 = 0x70f7 | (phy_a4 << 8);
 				}
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pmu.c b/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
index 4931d29d077..7e9df566c73 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
@@ -74,16 +74,6 @@
  * PMU<rev>_PLL<num>_XX where <rev> is PMU corerev and <num> is an arbitrary
  * number to differentiate different PLLs controlled by the same PMU rev.
  */
-/* pllcontrol registers:
- * ndiv_pwrdn, pwrdn_ch<x>, refcomp_pwrdn, dly_ch<x>,
- * p1div, p2div, _bypass_sdmod
- */
-#define PMU1_PLL0_PLLCTL0		0
-#define PMU1_PLL0_PLLCTL1		1
-#define PMU1_PLL0_PLLCTL2		2
-#define PMU1_PLL0_PLLCTL3		3
-#define PMU1_PLL0_PLLCTL4		4
-#define PMU1_PLL0_PLLCTL5		5
 
 /* pmu XtalFreqRatio */
 #define	PMU_XTALFREQ_REG_ILPCTR_MASK	0x00001FFF
@@ -108,118 +98,14 @@
 #define	RES4313_HT_AVAIL_RSRC		14
 #define	RES4313_MACPHY_CLK_AVAIL_RSRC	15
 
-/* Determine min/max rsrc masks. Value 0 leaves hardware at default. */
-static void si_pmu_res_masks(struct si_pub *sih, u32 * pmin, u32 * pmax)
-{
-	u32 min_mask = 0, max_mask = 0;
-	uint rsrcs;
-
-	/* # resources */
-	rsrcs = (ai_get_pmucaps(sih) & PCAP_RC_MASK) >> PCAP_RC_SHIFT;
-
-	/* determine min/max rsrc masks */
-	switch (ai_get_chip_id(sih)) {
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-		/* ??? */
-		break;
-
-	case BCM4313_CHIP_ID:
-		min_mask = PMURES_BIT(RES4313_BB_PU_RSRC) |
-		    PMURES_BIT(RES4313_XTAL_PU_RSRC) |
-		    PMURES_BIT(RES4313_ALP_AVAIL_RSRC) |
-		    PMURES_BIT(RES4313_BB_PLL_PWRSW_RSRC);
-		max_mask = 0xffff;
-		break;
-	default:
-		break;
-	}
-
-	*pmin = min_mask;
-	*pmax = max_mask;
-}
-
-void si_pmu_spuravoid_pllupdate(struct si_pub *sih, u8 spuravoid)
-{
-	u32 tmp = 0;
-	struct bcma_device *core;
-
-	/* switch to chipc */
-	core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-
-	switch (ai_get_chip_id(sih)) {
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-		if (spuravoid == 1) {
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL0);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x11500010);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL1);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x000C0C06);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL2);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x0F600a08);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL3);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x00000000);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL4);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x2001E920);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL5);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x88888815);
-		} else {
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL0);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x11100010);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL1);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x000c0c06);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL2);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x03000a08);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL3);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x00000000);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL4);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x200005c0);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_addr),
-				     PMU1_PLL0_PLLCTL5);
-			bcma_write32(core, CHIPCREGOFFS(pllcontrol_data),
-				     0x88888815);
-		}
-		tmp = 1 << 10;
-		break;
-
-	default:
-		/* bail out */
-		return;
-	}
-
-	bcma_set32(core, CHIPCREGOFFS(pmucontrol), tmp);
-}
-
 u16 si_pmu_fast_pwrup_delay(struct si_pub *sih)
 {
 	uint delay = PMU_MAX_TRANSITION_DLY;
 
 	switch (ai_get_chip_id(sih)) {
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-	case BCM4313_CHIP_ID:
+	case BCMA_CHIP_ID_BCM43224:
+	case BCMA_CHIP_ID_BCM43225:
+	case BCMA_CHIP_ID_BCM4313:
 		delay = 3700;
 		break;
 	default:
@@ -270,9 +156,9 @@ u32 si_pmu_alp_clock(struct si_pub *sih)
 		return clock;
 
 	switch (ai_get_chip_id(sih)) {
-	case BCM43224_CHIP_ID:
-	case BCM43225_CHIP_ID:
-	case BCM4313_CHIP_ID:
+	case BCMA_CHIP_ID_BCM43224:
+	case BCMA_CHIP_ID_BCM43225:
+	case BCMA_CHIP_ID_BCM4313:
 		/* always 20Mhz */
 		clock = 20000 * 1000;
 		break;
@@ -283,51 +169,9 @@ u32 si_pmu_alp_clock(struct si_pub *sih)
 	return clock;
 }
 
-/* initialize PMU */
-void si_pmu_init(struct si_pub *sih)
-{
-	struct bcma_device *core;
-
-	/* select chipc */
-	core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-
-	if (ai_get_pmurev(sih) == 1)
-		bcma_mask32(core, CHIPCREGOFFS(pmucontrol),
-			    ~PCTL_NOILP_ON_WAIT);
-	else if (ai_get_pmurev(sih) >= 2)
-		bcma_set32(core, CHIPCREGOFFS(pmucontrol), PCTL_NOILP_ON_WAIT);
-}
-
-/* initialize PMU resources */
-void si_pmu_res_init(struct si_pub *sih)
-{
-	struct bcma_device *core;
-	u32 min_mask = 0, max_mask = 0;
-
-	/* select to chipc */
-	core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
-
-	/* Determine min/max rsrc masks */
-	si_pmu_res_masks(sih, &min_mask, &max_mask);
-
-	/* It is required to program max_mask first and then min_mask */
-
-	/* Program max resource mask */
-
-	if (max_mask)
-		bcma_write32(core, CHIPCREGOFFS(max_res_mask), max_mask);
-
-	/* Program min resource mask */
-
-	if (min_mask)
-		bcma_write32(core, CHIPCREGOFFS(min_res_mask), min_mask);
-
-	/* Add some delay; allow resources to come up and settle. */
-	mdelay(2);
-}
-
 u32 si_pmu_measure_alpclk(struct si_pub *sih)
 {
+	struct si_info *sii = container_of(sih, struct si_info, pub);
 	struct bcma_device *core;
 	u32 alp_khz;
 
@@ -335,7 +179,7 @@ u32 si_pmu_measure_alpclk(struct si_pub *sih)
 		return 0;
 
 	/* Remember original core before switch to chipc */
-	core = ai_findcore(sih, BCMA_CORE_CHIPCOMMON, 0);
+	core = sii->icbus->drv_cc.core;
 
 	if (bcma_read32(core, CHIPCREGOFFS(pmustatus)) & PST_EXTLPOAVAIL) {
 		u32 ilp_ctr, alp_hz;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pmu.h b/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
index 3e39c5e0f9f..f7cff873578 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
@@ -26,10 +26,7 @@ extern u32 si_pmu_chipcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
 extern u32 si_pmu_regcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
 extern u32 si_pmu_alp_clock(struct si_pub *sih);
 extern void si_pmu_pllupd(struct si_pub *sih);
-extern void si_pmu_spuravoid_pllupdate(struct si_pub *sih, u8 spuravoid);
 extern u32 si_pmu_pllcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
-extern void si_pmu_init(struct si_pub *sih);
-extern void si_pmu_res_init(struct si_pub *sih);
 extern u32 si_pmu_measure_alpclk(struct si_pub *sih);
 
 #endif /* _BRCM_PMU_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pub.h b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
index aa5d67f8d87..5855f4fd16d 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pub.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
@@ -311,7 +311,7 @@ extern uint brcms_c_detach(struct brcms_c_info *wlc);
 extern int brcms_c_up(struct brcms_c_info *wlc);
 extern uint brcms_c_down(struct brcms_c_info *wlc);
 
-extern bool brcms_c_chipmatch(u16 vendor, u16 device);
+extern bool brcms_c_chipmatch(struct bcma_device *core);
 extern void brcms_c_init(struct brcms_c_info *wlc, bool mute_tx);
 extern void brcms_c_reset(struct brcms_c_info *wlc);
 
diff --git a/drivers/net/wireless/brcm80211/brcmutil/utils.c b/drivers/net/wireless/brcm80211/brcmutil/utils.c
index b45ab34cdfd..3e6405e06ac 100644
--- a/drivers/net/wireless/brcm80211/brcmutil/utils.c
+++ b/drivers/net/wireless/brcm80211/brcmutil/utils.c
@@ -43,6 +43,8 @@ EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
 /* Free the driver packet. Free the tag if present */
 void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
 {
+	if (!skb)
+		return;
 	WARN_ON(skb->next);
 	if (skb->destructor)
 		/* cannot kfree_skb() on hard IRQ (net/core/skbuff.c) if
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index 333193f20e1..bcc79b4e326 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -37,5 +37,6 @@
 #define BCM4329_CHIP_ID		0x4329
 #define BCM4330_CHIP_ID		0x4330
 #define BCM4331_CHIP_ID		0x4331
+#define BCM4334_CHIP_ID		0x4334
 
 #endif				/* _BRCM_HW_IDS_H_ */
diff --git a/drivers/net/wireless/brcm80211/include/soc.h b/drivers/net/wireless/brcm80211/include/soc.h
index 4e9b7e4827e..123cfa854a0 100644
--- a/drivers/net/wireless/brcm80211/include/soc.h
+++ b/drivers/net/wireless/brcm80211/include/soc.h
@@ -19,68 +19,6 @@
 
 #define SI_ENUM_BASE		0x18000000	/* Enumeration space base */
 
-/* core codes */
-#define	NODEV_CORE_ID		0x700	/* Invalid coreid */
-#define	CC_CORE_ID		0x800	/* chipcommon core */
-#define	ILINE20_CORE_ID		0x801	/* iline20 core */
-#define	SRAM_CORE_ID		0x802	/* sram core */
-#define	SDRAM_CORE_ID		0x803	/* sdram core */
-#define	PCI_CORE_ID		0x804	/* pci core */
-#define	MIPS_CORE_ID		0x805	/* mips core */
-#define	ENET_CORE_ID		0x806	/* enet mac core */
-#define	CODEC_CORE_ID		0x807	/* v90 codec core */
-#define	USB_CORE_ID		0x808	/* usb 1.1 host/device core */
-#define	ADSL_CORE_ID		0x809	/* ADSL core */
-#define	ILINE100_CORE_ID	0x80a	/* iline100 core */
-#define	IPSEC_CORE_ID		0x80b	/* ipsec core */
-#define	UTOPIA_CORE_ID		0x80c	/* utopia core */
-#define	PCMCIA_CORE_ID		0x80d	/* pcmcia core */
-#define	SOCRAM_CORE_ID		0x80e	/* internal memory core */
-#define	MEMC_CORE_ID		0x80f	/* memc sdram core */
-#define	OFDM_CORE_ID		0x810	/* OFDM phy core */
-#define	EXTIF_CORE_ID		0x811	/* external interface core */
-#define	D11_CORE_ID		0x812	/* 802.11 MAC core */
-#define	APHY_CORE_ID		0x813	/* 802.11a phy core */
-#define	BPHY_CORE_ID		0x814	/* 802.11b phy core */
-#define	GPHY_CORE_ID		0x815	/* 802.11g phy core */
-#define	MIPS33_CORE_ID		0x816	/* mips3302 core */
-#define	USB11H_CORE_ID		0x817	/* usb 1.1 host core */
-#define	USB11D_CORE_ID		0x818	/* usb 1.1 device core */
-#define	USB20H_CORE_ID		0x819	/* usb 2.0 host core */
-#define	USB20D_CORE_ID		0x81a	/* usb 2.0 device core */
-#define	SDIOH_CORE_ID		0x81b	/* sdio host core */
-#define	ROBO_CORE_ID		0x81c	/* roboswitch core */
-#define	ATA100_CORE_ID		0x81d	/* parallel ATA core */
-#define	SATAXOR_CORE_ID		0x81e	/* serial ATA & XOR DMA core */
-#define	GIGETH_CORE_ID		0x81f	/* gigabit ethernet core */
-#define	PCIE_CORE_ID		0x820	/* pci express core */
-#define	NPHY_CORE_ID		0x821	/* 802.11n 2x2 phy core */
-#define	SRAMC_CORE_ID		0x822	/* SRAM controller core */
-#define	MINIMAC_CORE_ID		0x823	/* MINI MAC/phy core */
-#define	ARM11_CORE_ID		0x824	/* ARM 1176 core */
-#define	ARM7S_CORE_ID		0x825	/* ARM7tdmi-s core */
-#define	LPPHY_CORE_ID		0x826	/* 802.11a/b/g phy core */
-#define	PMU_CORE_ID		0x827	/* PMU core */
-#define	SSNPHY_CORE_ID		0x828	/* 802.11n single-stream phy core */
-#define	SDIOD_CORE_ID		0x829	/* SDIO device core */
-#define	ARMCM3_CORE_ID		0x82a	/* ARM Cortex M3 core */
-#define	HTPHY_CORE_ID		0x82b	/* 802.11n 4x4 phy core */
-#define	MIPS74K_CORE_ID		0x82c	/* mips 74k core */
-#define	GMAC_CORE_ID		0x82d	/* Gigabit MAC core */
-#define	DMEMC_CORE_ID		0x82e	/* DDR1/2 memory controller core */
-#define	PCIERC_CORE_ID		0x82f	/* PCIE Root Complex core */
-#define	OCP_CORE_ID		0x830	/* OCP2OCP bridge core */
-#define	SC_CORE_ID		0x831	/* shared common core */
-#define	AHB_CORE_ID		0x832	/* OCP2AHB bridge core */
-#define	SPIH_CORE_ID		0x833	/* SPI host core */
-#define	I2S_CORE_ID		0x834	/* I2S core */
-#define	DMEMS_CORE_ID		0x835	/* SDR/DDR1 memory controller core */
-#define	DEF_SHIM_COMP		0x837	/* SHIM component in ubus/6362 */
-#define OOB_ROUTER_CORE_ID	0x367	/* OOB router core ID */
-#define	DEF_AI_COMP		0xfff	/* Default component, in ai chips it
-					 * maps all unused address ranges
-					 */
-
 /* Common core control flags */
 #define	SICF_BIST_EN		0x8000
 #define	SICF_PME_EN		0x4000
diff --git a/drivers/net/wireless/hostap/hostap_proc.c b/drivers/net/wireless/hostap/hostap_proc.c
index 75ef8f04aab..dc447c1b5ab 100644
--- a/drivers/net/wireless/hostap/hostap_proc.c
+++ b/drivers/net/wireless/hostap/hostap_proc.c
@@ -58,8 +58,7 @@ static int prism2_stats_proc_read(char *page, char **start, off_t off,
 {
 	char *p = page;
 	local_info_t *local = (local_info_t *) data;
-	struct comm_tallies_sums *sums = (struct comm_tallies_sums *)
-		&local->comm_tallies;
+	struct comm_tallies_sums *sums = &local->comm_tallies;
 
 	if (off != 0) {
 		*eof = 1;
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index 0036737fe8e..0df45914739 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -2701,6 +2701,20 @@ static void eeprom_parse_mac(struct ipw_priv *priv, u8 * mac)
 	memcpy(mac, &priv->eeprom[EEPROM_MAC_ADDRESS], 6);
 }
 
+static void ipw_read_eeprom(struct ipw_priv *priv)
+{
+	int i;
+	__le16 *eeprom = (__le16 *) priv->eeprom;
+
+	IPW_DEBUG_TRACE(">>\n");
+
+	/* read entire contents of eeprom into private buffer */
+	for (i = 0; i < 128; i++)
+		eeprom[i] = cpu_to_le16(eeprom_read_u16(priv, (u8) i));
+
+	IPW_DEBUG_TRACE("<<\n");
+}
+
 /*
  * Either the device driver (i.e. the host) or the firmware can
  * load eeprom data into the designated region in SRAM.  If neither
@@ -2712,14 +2726,9 @@ static void eeprom_parse_mac(struct ipw_priv *priv, u8 * mac)
 static void ipw_eeprom_init_sram(struct ipw_priv *priv)
 {
 	int i;
-	__le16 *eeprom = (__le16 *) priv->eeprom;
 
 	IPW_DEBUG_TRACE(">>\n");
 
-	/* read entire contents of eeprom into private buffer */
-	for (i = 0; i < 128; i++)
-		eeprom[i] = cpu_to_le16(eeprom_read_u16(priv, (u8) i));
-
 	/*
 	   If the data looks correct, then copy it to our private
 	   copy.  Otherwise let the firmware know to perform the operation
@@ -3643,8 +3652,10 @@ static int ipw_load(struct ipw_priv *priv)
 	/* ack fw init done interrupt */
 	ipw_write32(priv, IPW_INTA_RW, IPW_INTA_BIT_FW_INITIALIZATION_DONE);
 
-	/* read eeprom data and initialize the eeprom region of sram */
+	/* read eeprom data */
 	priv->eeprom_delay = 1;
+	ipw_read_eeprom(priv);
+	/* initialize the eeprom region of sram */
 	ipw_eeprom_init_sram(priv);
 
 	/* enable interrupts */
@@ -7069,9 +7080,7 @@ static int ipw_qos_activate(struct ipw_priv *priv,
 	}
 
 	IPW_DEBUG_QOS("QoS sending IPW_CMD_QOS_PARAMETERS\n");
-	err = ipw_send_qos_params_command(priv,
-					  (struct libipw_qos_parameters *)
-					  &(qos_parameters[0]));
+	err = ipw_send_qos_params_command(priv, &qos_parameters[0]);
 	if (err)
 		IPW_DEBUG_QOS("QoS IPW_CMD_QOS_PARAMETERS failed\n");
 
diff --git a/drivers/net/wireless/iwlegacy/3945-rs.c b/drivers/net/wireless/iwlegacy/3945-rs.c
index 4b10157d868..d4fd29ad90d 100644
--- a/drivers/net/wireless/iwlegacy/3945-rs.c
+++ b/drivers/net/wireless/iwlegacy/3945-rs.c
@@ -946,7 +946,7 @@ il3945_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id)
 	case IEEE80211_BAND_5GHZ:
 		rs_sta->expected_tpt = il3945_expected_tpt_a;
 		break;
-	case IEEE80211_NUM_BANDS:
+	default:
 		BUG();
 		break;
 	}
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index 509301a5e7e..34f61a0581a 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -3405,7 +3405,7 @@ il4965_remove_dynamic_key(struct il_priv *il,
 		return 0;
 	}
 
-	if (il->stations[sta_id].sta.key.key_offset == WEP_INVALID_OFFSET) {
+	if (il->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_INVALID) {
 		IL_WARN("Removing wrong key %d 0x%x\n", keyconf->keyidx,
 			key_flags);
 		spin_unlock_irqrestore(&il->sta_lock, flags);
@@ -3420,7 +3420,7 @@ il4965_remove_dynamic_key(struct il_priv *il,
 	memset(&il->stations[sta_id].sta.key, 0, sizeof(struct il4965_keyinfo));
 	il->stations[sta_id].sta.key.key_flags =
 	    STA_KEY_FLG_NO_ENC | STA_KEY_FLG_INVALID;
-	il->stations[sta_id].sta.key.key_offset = WEP_INVALID_OFFSET;
+	il->stations[sta_id].sta.key.key_offset = keyconf->hw_key_idx;
 	il->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
 	il->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
 
@@ -5724,7 +5724,8 @@ il4965_mac_setup_register(struct il_priv *il, u32 max_probe_length)
 	    BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_ADHOC);
 
 	hw->wiphy->flags |=
-	    WIPHY_FLAG_CUSTOM_REGULATORY | WIPHY_FLAG_DISABLE_BEACON_HINTS;
+	    WIPHY_FLAG_CUSTOM_REGULATORY | WIPHY_FLAG_DISABLE_BEACON_HINTS |
+	    WIPHY_FLAG_IBSS_RSN;
 
 	/*
 	 * For now, disable PS by default because it affects
@@ -5873,6 +5874,16 @@ il4965_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)) {
+		D_MAC80211("leave - ad-hoc group key\n");
+		return -EOPNOTSUPP;
+	}
+
 	sta_id = il_sta_id_or_broadcast(il, sta);
 	if (sta_id == IL_INVALID_STATION)
 		return -EINVAL;
diff --git a/drivers/net/wireless/iwlegacy/common.c b/drivers/net/wireless/iwlegacy/common.c
index cbf2dc18341..0370403fd0b 100644
--- a/drivers/net/wireless/iwlegacy/common.c
+++ b/drivers/net/wireless/iwlegacy/common.c
@@ -4717,10 +4717,11 @@ il_check_stuck_queue(struct il_priv *il, int cnt)
 	struct il_tx_queue *txq = &il->txq[cnt];
 	struct il_queue *q = &txq->q;
 	unsigned long timeout;
+	unsigned long now = jiffies;
 	int ret;
 
 	if (q->read_ptr == q->write_ptr) {
-		txq->time_stamp = jiffies;
+		txq->time_stamp = now;
 		return 0;
 	}
 
@@ -4728,9 +4729,9 @@ il_check_stuck_queue(struct il_priv *il, int cnt)
 	    txq->time_stamp +
 	    msecs_to_jiffies(il->cfg->wd_timeout);
 
-	if (time_after(jiffies, timeout)) {
+	if (time_after(now, timeout)) {
 		IL_ERR("Queue %d stuck for %u ms.\n", q->id,
-		       il->cfg->wd_timeout);
+		       jiffies_to_msecs(now - txq->time_stamp));
 		ret = il_force_reset(il, false);
 		return (ret == -EAGAIN) ? 0 : 1;
 	}
@@ -4767,14 +4768,12 @@ il_bg_watchdog(unsigned long data)
 		return;
 
 	/* monitor and check for other stuck queues */
-	if (il_is_any_associated(il)) {
-		for (cnt = 0; cnt < il->hw_params.max_txq_num; cnt++) {
-			/* skip as we already checked the command queue */
-			if (cnt == il->cmd_queue)
-				continue;
-			if (il_check_stuck_queue(il, cnt))
-				return;
-		}
+	for (cnt = 0; cnt < il->hw_params.max_txq_num; cnt++) {
+		/* skip as we already checked the command queue */
+		if (cnt == il->cmd_queue)
+			continue;
+		if (il_check_stuck_queue(il, cnt))
+			return;
 	}
 
 	mod_timer(&il->watchdog,
@@ -5360,7 +5359,7 @@ il_mac_bss_info_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	if (changes & BSS_CHANGED_ASSOC) {
 		D_MAC80211("ASSOC %d\n", bss_conf->assoc);
 		if (bss_conf->assoc) {
-			il->timestamp = bss_conf->last_tsf;
+			il->timestamp = bss_conf->sync_tsf;
 
 			if (!il_is_rfkill(il))
 				il->ops->post_associate(il);
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index 2463c062643..727fbb5db9d 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -6,6 +6,7 @@ config IWLWIFI
 	select LEDS_CLASS
 	select LEDS_TRIGGERS
 	select MAC80211_LEDS
+	select IWLDVM
 	---help---
 	  Select to build the driver supporting the:
 
@@ -41,6 +42,10 @@ config IWLWIFI
 	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
 	  module will be called iwlwifi.
 
+config IWLDVM
+	tristate "Intel Wireless WiFi"
+	depends on IWLWIFI
+
 menu "Debugging Options"
 	depends on IWLWIFI
 
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index d615eacbf05..170ec330d2a 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -1,27 +1,19 @@
-# WIFI
+# common
 obj-$(CONFIG_IWLWIFI)	+= iwlwifi.o
-iwlwifi-objs		:= iwl-agn.o iwl-agn-rs.o iwl-mac80211.o
-iwlwifi-objs		+= iwl-ucode.o iwl-agn-tx.o iwl-debug.o
-iwlwifi-objs		+= iwl-agn-lib.o iwl-agn-calib.o iwl-io.o
-iwlwifi-objs		+= iwl-agn-tt.o iwl-agn-sta.o iwl-agn-rx.o
-
-iwlwifi-objs		+= iwl-eeprom.o iwl-power.o
-iwlwifi-objs		+= iwl-scan.o iwl-led.o
-iwlwifi-objs		+= iwl-agn-rxon.o iwl-agn-devices.o
-iwlwifi-objs		+= iwl-5000.o
-iwlwifi-objs		+= iwl-6000.o
-iwlwifi-objs		+= iwl-1000.o
-iwlwifi-objs		+= iwl-2000.o
-iwlwifi-objs		+= iwl-pci.o
+iwlwifi-objs		+= iwl-io.o
 iwlwifi-objs		+= iwl-drv.o
+iwlwifi-objs		+= iwl-debug.o
 iwlwifi-objs		+= iwl-notif-wait.o
-iwlwifi-objs		+= iwl-trans-pcie.o iwl-trans-pcie-rx.o iwl-trans-pcie-tx.o
-
+iwlwifi-objs		+= iwl-eeprom-read.o iwl-eeprom-parse.o
+iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
+iwlwifi-objs		+= pcie/1000.o pcie/2000.o pcie/5000.o pcie/6000.o
 
-iwlwifi-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
 iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TRACING) += iwl-devtrace.o
-iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += iwl-testmode.o
+iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += iwl-test.o
 
-CFLAGS_iwl-devtrace.o := -I$(src)
+ccflags-y += -D__CHECK_ENDIAN__ -I$(src)
 
-ccflags-y += -D__CHECK_ENDIAN__
+
+obj-$(CONFIG_IWLDVM)	+= dvm/
+
+CFLAGS_iwl-devtrace.o := -I$(src)
diff --git a/drivers/net/wireless/iwlwifi/dvm/Makefile b/drivers/net/wireless/iwlwifi/dvm/Makefile
new file mode 100644
index 00000000000..5ff76b20414
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/dvm/Makefile
@@ -0,0 +1,13 @@
+# DVM
+obj-$(CONFIG_IWLDVM)	+= iwldvm.o
+iwldvm-objs		+= main.o rs.o mac80211.o ucode.o tx.o
+iwldvm-objs		+= lib.o calib.o tt.o sta.o rx.o
+
+iwldvm-objs		+= power.o
+iwldvm-objs		+= scan.o led.o
+iwldvm-objs		+= rxon.o devices.o
+
+iwldvm-$(CONFIG_IWLWIFI_DEBUGFS) += debugfs.o
+iwldvm-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += testmode.o
+
+ccflags-y += -D__CHECK_ENDIAN__ -I$(src)/../
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.h b/drivers/net/wireless/iwlwifi/dvm/agn.h
index 79c0fe06f4d..9bb16bdf6d2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.h
+++ b/drivers/net/wireless/iwlwifi/dvm/agn.h
@@ -63,9 +63,10 @@
 #ifndef __iwl_agn_h__
 #define __iwl_agn_h__
 
-#include "iwl-dev.h"
 #include "iwl-config.h"
 
+#include "dev.h"
+
 /* The first 11 queues (0-10) are used otherwise */
 #define IWLAGN_FIRST_AMPDU_QUEUE	11
 
@@ -91,7 +92,6 @@ extern struct iwl_lib_ops iwl6030_lib;
 #define STATUS_CT_KILL		1
 #define STATUS_ALIVE		2
 #define STATUS_READY		3
-#define STATUS_GEO_CONFIGURED	4
 #define STATUS_EXIT_PENDING	5
 #define STATUS_STATISTICS	6
 #define STATUS_SCANNING		7
@@ -101,6 +101,7 @@ extern struct iwl_lib_ops iwl6030_lib;
 #define STATUS_CHANNEL_SWITCH_PENDING 11
 #define STATUS_SCAN_COMPLETE	12
 #define STATUS_POWER_PMI	13
+#define STATUS_SCAN_ROC_EXPIRED 14
 
 struct iwl_ucode_capabilities;
 
@@ -255,6 +256,10 @@ int __must_check iwl_scan_initiate(struct iwl_priv *priv,
 				   enum iwl_scan_type scan_type,
 				   enum ieee80211_band band);
 
+void iwl_scan_roc_expired(struct iwl_priv *priv);
+void iwl_scan_offchannel_skb(struct iwl_priv *priv);
+void iwl_scan_offchannel_skb_status(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
@@ -264,7 +269,7 @@ int __must_check iwl_scan_initiate(struct iwl_priv *priv,
 #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)
+#define IWL_SCAN_CHECK_WATCHDOG		(HZ * 15)
 
 
 /* bt coex */
@@ -390,8 +395,10 @@ static inline __le32 iwl_hw_set_rate_n_flags(u8 rate, u32 flags)
 }
 
 extern int iwl_alive_start(struct iwl_priv *priv);
-/* svtool */
+
+/* testmode support */
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
+
 extern int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data,
 				   int len);
 extern int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw,
@@ -399,13 +406,16 @@ extern int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw,
 				    struct netlink_callback *cb,
 				    void *data, int len);
 extern void iwl_testmode_init(struct iwl_priv *priv);
-extern void iwl_testmode_cleanup(struct iwl_priv *priv);
+extern void iwl_testmode_free(struct iwl_priv *priv);
+
 #else
+
 static inline
 int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
 {
 	return -ENOSYS;
 }
+
 static inline
 int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
 		      struct netlink_callback *cb,
@@ -413,12 +423,12 @@ int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
 {
 	return -ENOSYS;
 }
-static inline
-void iwl_testmode_init(struct iwl_priv *priv)
+
+static inline void iwl_testmode_init(struct iwl_priv *priv)
 {
 }
-static inline
-void iwl_testmode_cleanup(struct iwl_priv *priv)
+
+static inline void iwl_testmode_free(struct iwl_priv *priv)
 {
 }
 #endif
@@ -437,10 +447,8 @@ static inline void iwl_print_rx_config_cmd(struct iwl_priv *priv,
 
 static inline int iwl_is_ready(struct iwl_priv *priv)
 {
-	/* The adapter is 'ready' if READY and GEO_CONFIGURED bits are
-	 * set but EXIT_PENDING is not */
+	/* The adapter is 'ready' if READY EXIT_PENDING is not set */
 	return test_bit(STATUS_READY, &priv->status) &&
-	       test_bit(STATUS_GEO_CONFIGURED, &priv->status) &&
 	       !test_bit(STATUS_EXIT_PENDING, &priv->status);
 }
 
@@ -518,85 +526,4 @@ static inline const char *iwl_dvm_get_cmd_string(u8 cmd)
 		return s;
 	return "UNKNOWN";
 }
-
-/* API method exported for mvm hybrid state */
-void iwl_setup_deferred_work(struct iwl_priv *priv);
-int iwl_send_wimax_coex(struct iwl_priv *priv);
-int iwl_send_bt_env(struct iwl_priv *priv, u8 action, u8 type);
-void iwl_option_config(struct iwl_priv *priv);
-void iwl_set_hw_params(struct iwl_priv *priv);
-void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags);
-int iwl_init_drv(struct iwl_priv *priv);
-void iwl_uninit_drv(struct iwl_priv *priv);
-void iwl_send_bt_config(struct iwl_priv *priv);
-void iwl_rf_kill_ct_config(struct iwl_priv *priv);
-int iwl_setup_interface(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwl_teardown_interface(struct iwl_priv *priv,
-			    struct ieee80211_vif *vif,
-			    bool mode_change);
-int iwl_full_rxon_required(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwlagn_check_needed_chains(struct iwl_priv *priv,
-				struct iwl_rxon_context *ctx,
-				struct ieee80211_bss_conf *bss_conf);
-void iwlagn_chain_noise_reset(struct iwl_priv *priv);
-int iwlagn_update_beacon(struct iwl_priv *priv,
-			 struct ieee80211_vif *vif);
-void iwl_tt_handler(struct iwl_priv *priv);
-void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode);
-void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue);
-void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state);
-void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb);
-void iwl_nic_error(struct iwl_op_mode *op_mode);
-void iwl_cmd_queue_full(struct iwl_op_mode *op_mode);
-void iwl_nic_config(struct iwl_op_mode *op_mode);
-int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
-		       struct ieee80211_sta *sta, bool set);
-void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
-			      enum ieee80211_rssi_event rssi_event);
-int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw);
-int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw);
-void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop);
-void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue);
-void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
-			       struct ieee80211_channel_switch *ch_switch);
-int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
-			 struct ieee80211_vif *vif,
-			 struct ieee80211_sta *sta,
-			 enum ieee80211_sta_state old_state,
-			 enum ieee80211_sta_state new_state);
-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,
-			    u8 buf_size);
-int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
-		       struct ieee80211_vif *vif,
-		       struct cfg80211_scan_request *req);
-void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
-			   struct ieee80211_vif *vif,
-			   enum sta_notify_cmd cmd,
-			   struct ieee80211_sta *sta);
-void iwlagn_configure_filter(struct ieee80211_hw *hw,
-			     unsigned int changed_flags,
-			     unsigned int *total_flags,
-			     u64 multicast);
-int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
-		       struct ieee80211_vif *vif, u16 queue,
-		       const struct ieee80211_tx_queue_params *params);
-void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif,
-			       struct cfg80211_gtk_rekey_data *data);
-void iwlagn_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 iwlagn_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 iwlagn_mac_stop(struct ieee80211_hw *hw);
-void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
-int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan);
 #endif /* __iwl_agn_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c b/drivers/net/wireless/iwlwifi/dvm/calib.c
index 95f27f1a423..f2dd671d7dc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/calib.c
@@ -63,10 +63,11 @@
 #include <linux/slab.h>
 #include <net/mac80211.h>
 
-#include "iwl-dev.h"
-#include "iwl-agn-calib.h"
 #include "iwl-trans.h"
-#include "iwl-agn.h"
+
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
 
 /*****************************************************************************
  * INIT calibrations framework
@@ -832,14 +833,14 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
 	 * To be safe, simply mask out any chains that we know
 	 * are not on the device.
 	 */
-	active_chains &= priv->hw_params.valid_rx_ant;
+	active_chains &= priv->eeprom_data->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))
+		if (!(priv->eeprom_data->valid_tx_ant & ant_msk))
 			continue;
 
 		num_tx_chains++;
@@ -853,7 +854,7 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
 			 * connect the first valid tx chain
 			 */
 			first_chain =
-				find_first_chain(priv->hw_params.valid_tx_ant);
+				find_first_chain(priv->eeprom_data->valid_tx_ant);
 			data->disconn_array[first_chain] = 0;
 			active_chains |= BIT(first_chain);
 			IWL_DEBUG_CALIB(priv,
@@ -863,13 +864,13 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
 		}
 	}
 
-	if (active_chains != priv->hw_params.valid_rx_ant &&
+	if (active_chains != priv->eeprom_data->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);
+				priv->eeprom_data->valid_rx_ant);
 
 	/* Save for use within RXON, TX, SCAN commands, etc. */
 	data->active_chains = active_chains;
@@ -1054,7 +1055,7 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
 	    priv->cfg->bt_params->advanced_bt_coexist) {
 		/* Disable disconnected antenna algorithm for advanced
 		   bt coex, assuming valid antennas are connected */
-		data->active_chains = priv->hw_params.valid_rx_ant;
+		data->active_chains = priv->eeprom_data->valid_rx_ant;
 		for (i = 0; i < NUM_RX_CHAINS; i++)
 			if (!(data->active_chains & (1<<i)))
 				data->disconn_array[i] = 1;
@@ -1083,8 +1084,9 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
 	IWL_DEBUG_CALIB(priv, "min_average_noise = %d, antenna %d\n",
 			min_average_noise, min_average_noise_antenna_i);
 
-	iwlagn_gain_computation(priv, average_noise,
-				find_first_chain(priv->hw_params.valid_rx_ant));
+	iwlagn_gain_computation(
+		priv, average_noise,
+		find_first_chain(priv->eeprom_data->valid_rx_ant));
 
 	/* Some power changes may have been made during the calibration.
 	 * Update and commit the RXON
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.h b/drivers/net/wireless/iwlwifi/dvm/calib.h
index dbe13787f27..2349f393cc4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.h
+++ b/drivers/net/wireless/iwlwifi/dvm/calib.h
@@ -62,8 +62,8 @@
 #ifndef __iwl_calib_h__
 #define __iwl_calib_h__
 
-#include "iwl-dev.h"
-#include "iwl-commands.h"
+#include "dev.h"
+#include "commands.h"
 
 void iwl_chain_noise_calibration(struct iwl_priv *priv);
 void iwl_sensitivity_calibration(struct iwl_priv *priv);
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/dvm/commands.h
index 9af6a239b38..4a361c55c54 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/dvm/commands.h
@@ -61,9 +61,9 @@
  *
  *****************************************************************************/
 /*
- * Please use this file (iwl-commands.h) only for uCode API definitions.
+ * Please use this file (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.
+ * Please use dev.h for driver implementation definitions.
  */
 
 #ifndef __iwl_commands_h__
@@ -190,6 +190,44 @@ enum {
 	REPLY_MAX = 0xff
 };
 
+/*
+ * Minimum number of queues. MAX_NUM is defined in hw specific files.
+ * Set the minimum to accommodate
+ *  - 4 standard TX queues
+ *  - the command queue
+ *  - 4 PAN TX queues
+ *  - the PAN multicast queue, and
+ *  - the AUX (TX during scan dwell) queue.
+ */
+#define IWL_MIN_NUM_QUEUES	11
+
+/*
+ * Command queue depends on iPAN support.
+ */
+#define IWL_DEFAULT_CMD_QUEUE_NUM	4
+#define IWL_IPAN_CMD_QUEUE_NUM		9
+
+#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_BK_IPAN	IWL_TX_FIFO_BK
+#define IWL_TX_FIFO_BE_IPAN	4
+#define IWL_TX_FIFO_VI_IPAN	IWL_TX_FIFO_VI
+#define IWL_TX_FIFO_VO_IPAN	5
+/* re-uses the VO FIFO, uCode will properly flush/schedule */
+#define IWL_TX_FIFO_AUX		5
+#define IWL_TX_FIFO_UNUSED	255
+
+#define IWLAGN_CMD_FIFO_NUM	7
+
+/*
+ * This queue number is required for proper operation
+ * because the ucode will stop/start the scheduler as
+ * required.
+ */
+#define IWL_IPAN_MCAST_QUEUE	8
+
 /******************************************************************************
  * (0)
  * Commonly used structures and definitions:
@@ -197,9 +235,6 @@ enum {
  *
  *****************************************************************************/
 
-/* iwl_cmd_header flags value */
-#define IWL_CMD_FAILED_MSK 0x40
-
 /**
  * iwlagn rate_n_flags bit fields
  *
@@ -758,8 +793,6 @@ struct iwl_qosparam_cmd {
 #define IWLAGN_BROADCAST_ID	15
 #define	IWLAGN_STATION_COUNT	16
 
-#define	IWL_INVALID_STATION 	255
-#define IWL_MAX_TID_COUNT	8
 #define IWL_TID_NON_QOS IWL_MAX_TID_COUNT
 
 #define STA_FLG_TX_RATE_MSK		cpu_to_le32(1 << 2)
@@ -1872,6 +1905,7 @@ struct iwl_bt_cmd {
 #define IWLAGN_BT_PRIO_BOOST_MAX	0xFF
 #define IWLAGN_BT_PRIO_BOOST_MIN	0x00
 #define IWLAGN_BT_PRIO_BOOST_DEFAULT	0xF0
+#define IWLAGN_BT_PRIO_BOOST_DEFAULT32	0xF0F0F0F0
 
 #define IWLAGN_BT_MAX_KILL_DEFAULT	5
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/dvm/debugfs.c
index 7f97dec8534..46782f1102a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/debugfs.c
@@ -30,16 +30,12 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/debugfs.h>
-
 #include <linux/ieee80211.h>
 #include <net/mac80211.h>
-
-
-#include "iwl-dev.h"
 #include "iwl-debug.h"
 #include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
 
 /* create and remove of files */
 #define DEBUGFS_ADD_FILE(name, parent, mode) do {			\
@@ -87,7 +83,7 @@ static ssize_t iwl_dbgfs_##name##_write(struct file *file,              \
 #define DEBUGFS_READ_FILE_OPS(name)                                     \
 	DEBUGFS_READ_FUNC(name);                                        \
 static const struct file_operations iwl_dbgfs_##name##_ops = {          \
-	.read = iwl_dbgfs_##name##_read,                       		\
+	.read = iwl_dbgfs_##name##_read,				\
 	.open = simple_open,						\
 	.llseek = generic_file_llseek,					\
 };
@@ -307,13 +303,13 @@ static ssize_t iwl_dbgfs_nvm_read(struct file *file,
 	const u8 *ptr;
 	char *buf;
 	u16 eeprom_ver;
-	size_t eeprom_len = priv->cfg->base_params->eeprom_size;
+	size_t eeprom_len = priv->eeprom_blob_size;
 	buf_size = 4 * eeprom_len + 256;
 
 	if (eeprom_len % 16)
 		return -ENODATA;
 
-	ptr = priv->eeprom;
+	ptr = priv->eeprom_blob;
 	if (!ptr)
 		return -ENOMEM;
 
@@ -322,11 +318,9 @@ static ssize_t iwl_dbgfs_nvm_read(struct file *file,
 	if (!buf)
 		return -ENOMEM;
 
-	eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
-	pos += scnprintf(buf + pos, buf_size - pos, "NVM Type: %s, "
-			"version: 0x%x\n",
-			(priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
-			 ? "OTP" : "EEPROM", eeprom_ver);
+	eeprom_ver = priv->eeprom_data->eeprom_version;
+	pos += scnprintf(buf + pos, buf_size - pos,
+			 "NVM 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,
@@ -351,9 +345,6 @@ static ssize_t iwl_dbgfs_channels_read(struct file *file, char __user *user_buf,
 	char *buf;
 	ssize_t ret;
 
-	if (!test_bit(STATUS_GEO_CONFIGURED, &priv->status))
-		return -EAGAIN;
-
 	buf = kzalloc(bufsz, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -426,8 +417,6 @@ static ssize_t iwl_dbgfs_status_read(struct file *file,
 		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_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",
@@ -1341,17 +1330,17 @@ static ssize_t iwl_dbgfs_ucode_tx_stats_read(struct file *file,
 	if (tx->tx_power.ant_a || tx->tx_power.ant_b || tx->tx_power.ant_c) {
 		pos += scnprintf(buf + pos, bufsz - pos,
 			"tx power: (1/2 dB step)\n");
-		if ((priv->hw_params.valid_tx_ant & ANT_A) &&
+		if ((priv->eeprom_data->valid_tx_ant & ANT_A) &&
 		    tx->tx_power.ant_a)
 			pos += scnprintf(buf + pos, bufsz - pos,
 					fmt_hex, "antenna A:",
 					tx->tx_power.ant_a);
-		if ((priv->hw_params.valid_tx_ant & ANT_B) &&
+		if ((priv->eeprom_data->valid_tx_ant & ANT_B) &&
 		    tx->tx_power.ant_b)
 			pos += scnprintf(buf + pos, bufsz - pos,
 					fmt_hex, "antenna B:",
 					tx->tx_power.ant_b);
-		if ((priv->hw_params.valid_tx_ant & ANT_C) &&
+		if ((priv->eeprom_data->valid_tx_ant & ANT_C) &&
 		    tx->tx_power.ant_c)
 			pos += scnprintf(buf + pos, bufsz - pos,
 					fmt_hex, "antenna C:",
@@ -2266,6 +2255,10 @@ static ssize_t iwl_dbgfs_log_event_write(struct file *file,
 	char buf[8];
 	int buf_size;
 
+	/* check that the interface is up */
+	if (!iwl_is_ready(priv))
+		return -EAGAIN;
+
 	memset(buf, 0, sizeof(buf));
 	buf_size = min(count, sizeof(buf) -  1);
 	if (copy_from_user(buf, user_buf, buf_size))
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/dvm/dev.h
index 70062379d0e..054f728f626 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/dvm/dev.h
@@ -24,8 +24,8 @@
  *
  *****************************************************************************/
 /*
- * Please use this file (iwl-dev.h) for driver implementation definitions.
- * Please use iwl-commands.h for uCode API definitions.
+ * Please use this file (dev.h) for driver implementation definitions.
+ * Please use commands.h for uCode API definitions.
  */
 
 #ifndef __iwl_dev_h__
@@ -39,17 +39,20 @@
 #include <linux/mutex.h>
 
 #include "iwl-fw.h"
-#include "iwl-eeprom.h"
+#include "iwl-eeprom-parse.h"
 #include "iwl-csr.h"
 #include "iwl-debug.h"
 #include "iwl-agn-hw.h"
-#include "iwl-led.h"
-#include "iwl-power.h"
-#include "iwl-agn-rs.h"
-#include "iwl-agn-tt.h"
-#include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-notif-wait.h"
+#include "iwl-trans.h"
+
+#include "led.h"
+#include "power.h"
+#include "rs.h"
+#include "tt.h"
+
+#include "iwl-test.h"
 
 /* CT-KILL constants */
 #define CT_KILL_THRESHOLD_LEGACY   110 /* in Celsius */
@@ -87,49 +90,6 @@
 
 #define IWL_NUM_SCAN_RATES         (2)
 
-/*
- * 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 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_* */
-};
-
-/*
- * Minimum number of queues. MAX_NUM is defined in hw specific files.
- * Set the minimum to accommodate
- *  - 4 standard TX queues
- *  - the command queue
- *  - 4 PAN TX queues
- *  - the PAN multicast queue, and
- *  - the AUX (TX during scan dwell) queue.
- */
-#define IWL_MIN_NUM_QUEUES	11
-
-/*
- * Command queue depends on iPAN support.
- */
-#define IWL_DEFAULT_CMD_QUEUE_NUM	4
-#define IWL_IPAN_CMD_QUEUE_NUM		9
 
 #define IEEE80211_DATA_LEN              2304
 #define IEEE80211_4ADDR_LEN             30
@@ -153,29 +113,6 @@ union iwl_ht_rate_supp {
 	};
 };
 
-#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 */
@@ -445,23 +382,6 @@ enum {
 	MEASUREMENT_ACTIVE = (1 << 1),
 };
 
-enum iwl_nvm_type {
-	NVM_DEVICE_TYPE_EEPROM = 0,
-	NVM_DEVICE_TYPE_OTP,
-};
-
-/*
- * Two types of OTP memory access modes
- *   IWL_OTP_ACCESS_ABSOLUTE - absolute address mode,
- * 			        based on physical memory addressing
- *   IWL_OTP_ACCESS_RELATIVE - relative address mode,
- * 			       based on logical memory addressing
- */
-enum iwl_access_mode {
-	IWL_OTP_ACCESS_ABSOLUTE,
-	IWL_OTP_ACCESS_RELATIVE,
-};
-
 /* reply_tx_statistics (for _agn devices) */
 struct reply_tx_error_statistics {
 	u32 pp_delay;
@@ -632,10 +552,6 @@ enum iwl_scan_type {
  *
  * @tx_chains_num: Number of TX chains
  * @rx_chains_num: Number of RX chains
- * @valid_tx_ant: usable antennas for TX
- * @valid_rx_ant: usable antennas for RX
- * @ht40_channel: is 40MHz width possible: BIT(IEEE80211_BAND_XXX)
- * @sku: sku read from EEPROM
  * @ct_kill_threshold: temperature threshold - in hw dependent unit
  * @ct_kill_exit_threshold: when to reeable the device - in hw dependent unit
  *	relevant for 1000, 6000 and up
@@ -645,11 +561,7 @@ enum iwl_scan_type {
 struct iwl_hw_params {
 	u8  tx_chains_num;
 	u8  rx_chains_num;
-	u8  valid_tx_ant;
-	u8  valid_rx_ant;
-	u8  ht40_channel;
 	bool use_rts_for_aggregation;
-	u16 sku;
 	u32 ct_kill_threshold;
 	u32 ct_kill_exit_threshold;
 
@@ -664,31 +576,10 @@ struct iwl_lib_ops {
 	/* device specific configuration */
 	void (*nic_config)(struct iwl_priv *priv);
 
-	/* eeprom operations (as defined in iwl-eeprom.h) */
-	struct iwl_eeprom_ops eeprom_ops;
-
 	/* temperature */
 	void (*temperature)(struct iwl_priv *priv);
 };
 
-#ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
-struct iwl_testmode_trace {
-	u32 buff_size;
-	u32 total_size;
-	u32 num_chunks;
-	u8 *cpu_addr;
-	u8 *trace_addr;
-	dma_addr_t dma_addr;
-	bool trace_enabled;
-};
-struct iwl_testmode_mem {
-	u32 buff_size;
-	u32 num_chunks;
-	u8 *buff_addr;
-	bool read_in_progress;
-};
-#endif
-
 struct iwl_wipan_noa_data {
 	struct rcu_head rcu_head;
 	u32 length;
@@ -735,8 +626,6 @@ 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 list_head calib_results;
 
@@ -747,16 +636,12 @@ struct iwl_priv {
 	enum ieee80211_band band;
 	u8 valid_contexts;
 
-	void (*pre_rx_handler)(struct iwl_priv *priv,
-			       struct iwl_rx_cmd_buffer *rxb);
 	int (*rx_handlers[REPLY_MAX])(struct iwl_priv *priv,
 				       struct iwl_rx_cmd_buffer *rxb,
 				       struct iwl_device_cmd *cmd);
 
 	struct iwl_notif_wait_data notif_wait;
 
-	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
-
 	/* spectrum measurement report caching */
 	struct iwl_spectrum_notification measure_report;
 	u8 measurement_status;
@@ -787,11 +672,6 @@ struct iwl_priv {
 	bool ucode_loaded;
 	bool init_ucode_run;		/* Don't run init uCode again */
 
-	/* 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 */
-
 	u8 plcp_delta_threshold;
 
 	/* thermal calibration */
@@ -846,6 +726,7 @@ struct iwl_priv {
 	struct iwl_station_entry stations[IWLAGN_STATION_COUNT];
 	unsigned long ucode_key_table;
 	struct iwl_tid_data tid_data[IWLAGN_STATION_COUNT][IWL_MAX_TID_COUNT];
+	atomic_t num_aux_in_flight;
 
 	u8 mac80211_registered;
 
@@ -950,10 +831,8 @@ struct iwl_priv {
 
 	struct delayed_work scan_check;
 
-	/* TX Power */
+	/* TX Power settings */
 	s8 tx_power_user_lmt;
-	s8 tx_power_device_lmt;
-	s8 tx_power_lmt_in_half_dbm; /* max tx power in half-dBm format */
 	s8 tx_power_next;
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
@@ -964,9 +843,10 @@ struct iwl_priv {
 	void *wowlan_sram;
 #endif /* CONFIG_IWLWIFI_DEBUGFS */
 
-	/* eeprom -- this is in the card's little endian byte order */
-	u8 *eeprom;
-	enum iwl_nvm_type nvm_device_type;
+	struct iwl_eeprom_data *eeprom_data;
+	/* eeprom blob for debugfs/testmode */
+	u8 *eeprom_blob;
+	size_t eeprom_blob_size;
 
 	struct work_struct txpower_work;
 	u32 calib_disabled;
@@ -979,9 +859,9 @@ struct iwl_priv {
 	struct led_classdev led;
 	unsigned long blink_on, blink_off;
 	bool led_registered;
+
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
-	struct iwl_testmode_trace testmode_trace;
-	struct iwl_testmode_mem testmode_mem;
+	struct iwl_test tst;
 	u32 tm_fixed_rate;
 #endif
 
@@ -1001,8 +881,6 @@ struct iwl_priv {
 	enum iwl_ucode_type cur_ucode;
 }; /*iwl_priv */
 
-extern struct kmem_cache *iwl_tx_cmd_pool;
-
 static inline struct iwl_rxon_context *
 iwl_rxon_ctx_from_vif(struct ieee80211_vif *vif)
 {
@@ -1036,36 +914,4 @@ static inline int iwl_is_any_associated(struct iwl_priv *priv)
 	return false;
 }
 
-static inline int 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 is_channel_radar(const struct iwl_channel_info *ch_info)
-{
-	return (ch_info->flags & EEPROM_CHANNEL_RADAR) ? 1 : 0;
-}
-
-static inline u8 is_channel_a_band(const struct iwl_channel_info *ch_info)
-{
-	return ch_info->band == IEEE80211_BAND_5GHZ;
-}
-
-static inline u8 is_channel_bg_band(const struct iwl_channel_info *ch_info)
-{
-	return ch_info->band == IEEE80211_BAND_2GHZ;
-}
-
-static inline int is_channel_passive(const struct iwl_channel_info *ch)
-{
-	return (!(ch->flags & EEPROM_CHANNEL_ACTIVE)) ? 1 : 0;
-}
-
-static inline int is_channel_ibss(const struct iwl_channel_info *ch)
-{
-	return ((ch->flags & EEPROM_CHANNEL_IBSS)) ? 1 : 0;
-}
-
 #endif				/* __iwl_dev_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-devices.c b/drivers/net/wireless/iwlwifi/dvm/devices.c
index 48533b3a0f9..349c205d5f6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-devices.c
+++ b/drivers/net/wireless/iwlwifi/dvm/devices.c
@@ -27,11 +27,14 @@
 /*
  * DVM device-specific data & functions
  */
-#include "iwl-agn.h"
-#include "iwl-dev.h"
-#include "iwl-commands.h"
 #include "iwl-io.h"
 #include "iwl-prph.h"
+#include "iwl-eeprom-parse.h"
+
+#include "agn.h"
+#include "dev.h"
+#include "commands.h"
+
 
 /*
  * 1000 series
@@ -58,11 +61,6 @@ static void iwl1000_set_ct_threshold(struct iwl_priv *priv)
 /* NIC configuration for 1000 series */
 static void iwl1000_nic_config(struct iwl_priv *priv)
 {
-	/* set CSR_HW_CONFIG_REG for uCode use */
-	iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
-		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
-
 	/* Setting digital SVR for 1000 card to 1.32V */
 	/* locking is acquired in iwl_set_bits_mask_prph() function */
 	iwl_set_bits_mask_prph(priv->trans, APMG_DIGITAL_SVR_REG,
@@ -170,16 +168,6 @@ static const struct iwl_sensitivity_ranges iwl1000_sensitivity = {
 
 static void iwl1000_hw_set_hw_params(struct iwl_priv *priv)
 {
-	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
-
-	priv->hw_params.tx_chains_num =
-		num_of_ant(priv->hw_params.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->hw_params.valid_rx_ant);
-
 	iwl1000_set_ct_threshold(priv);
 
 	/* Set initial sensitivity parameters */
@@ -189,17 +177,6 @@ static void iwl1000_hw_set_hw_params(struct iwl_priv *priv)
 struct iwl_lib_ops iwl1000_lib = {
 	.set_hw_params = iwl1000_hw_set_hw_params,
 	.nic_config = iwl1000_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_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REGULATORY_BAND_NO_HT40,
-		},
-	},
 	.temperature = iwlagn_temperature,
 };
 
@@ -219,8 +196,6 @@ static void iwl2000_set_ct_threshold(struct iwl_priv *priv)
 /* NIC configuration for 2000 series */
 static void iwl2000_nic_config(struct iwl_priv *priv)
 {
-	iwl_rf_config(priv);
-
 	iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
 		    CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER);
 }
@@ -251,16 +226,6 @@ static const struct iwl_sensitivity_ranges iwl2000_sensitivity = {
 
 static void iwl2000_hw_set_hw_params(struct iwl_priv *priv)
 {
-	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
-
-	priv->hw_params.tx_chains_num =
-		num_of_ant(priv->hw_params.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->hw_params.valid_rx_ant);
-
 	iwl2000_set_ct_threshold(priv);
 
 	/* Set initial sensitivity parameters */
@@ -270,36 +235,12 @@ static void iwl2000_hw_set_hw_params(struct iwl_priv *priv)
 struct iwl_lib_ops iwl2000_lib = {
 	.set_hw_params = iwl2000_hw_set_hw_params,
 	.nic_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_REGULATORY_BAND_NO_HT40,
-		},
-		.enhanced_txpower = true,
-	},
 	.temperature = iwlagn_temperature,
 };
 
 struct iwl_lib_ops iwl2030_lib = {
 	.set_hw_params = iwl2000_hw_set_hw_params,
 	.nic_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_REGULATORY_BAND_NO_HT40,
-		},
-		.enhanced_txpower = true,
-	},
 	.temperature = iwlagn_temperature,
 };
 
@@ -309,19 +250,6 @@ struct iwl_lib_ops iwl2030_lib = {
  */
 
 /* NIC configuration for 5000 series */
-static void iwl5000_nic_config(struct iwl_priv *priv)
-{
-	iwl_rf_config(priv);
-
-	/* W/A : NIC is stuck in a reset state after Early PCIe power off
-	 * (PCIe power is lost before PERST# is asserted),
-	 * causing ME FW to lose ownership and not being able to obtain it back.
-	 */
-	iwl_set_bits_mask_prph(priv->trans, APMG_PS_CTRL_REG,
-				APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
-				~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
-}
-
 static const struct iwl_sensitivity_ranges iwl5000_sensitivity = {
 	.min_nrg_cck = 100,
 	.auto_corr_min_ofdm = 90,
@@ -376,11 +304,9 @@ static struct iwl_sensitivity_ranges iwl5150_sensitivity = {
 static s32 iwl_temp_calib_to_offset(struct iwl_priv *priv)
 {
 	u16 temperature, voltage;
-	__le16 *temp_calib = (__le16 *)iwl_eeprom_query_addr(priv,
-				EEPROM_KELVIN_TEMPERATURE);
 
-	temperature = le16_to_cpu(temp_calib[0]);
-	voltage = le16_to_cpu(temp_calib[1]);
+	temperature = le16_to_cpu(priv->eeprom_data->kelvin_temperature);
+	voltage = le16_to_cpu(priv->eeprom_data->kelvin_voltage);
 
 	/* offset = temp - volt / coeff */
 	return (s32)(temperature -
@@ -404,14 +330,6 @@ static void iwl5000_set_ct_threshold(struct iwl_priv *priv)
 
 static void iwl5000_hw_set_hw_params(struct iwl_priv *priv)
 {
-	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-					BIT(IEEE80211_BAND_5GHZ);
-
-	priv->hw_params.tx_chains_num =
-		num_of_ant(priv->hw_params.valid_tx_ant);
-	priv->hw_params.rx_chains_num =
-		num_of_ant(priv->hw_params.valid_rx_ant);
-
 	iwl5000_set_ct_threshold(priv);
 
 	/* Set initial sensitivity parameters */
@@ -420,14 +338,6 @@ static void iwl5000_hw_set_hw_params(struct iwl_priv *priv)
 
 static void iwl5150_hw_set_hw_params(struct iwl_priv *priv)
 {
-	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-					BIT(IEEE80211_BAND_5GHZ);
-
-	priv->hw_params.tx_chains_num =
-		num_of_ant(priv->hw_params.valid_tx_ant);
-	priv->hw_params.rx_chains_num =
-		num_of_ant(priv->hw_params.valid_rx_ant);
-
 	iwl5150_set_ct_threshold(priv);
 
 	/* Set initial sensitivity parameters */
@@ -455,7 +365,6 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
 	 */
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
 	struct iwl5000_channel_switch_cmd cmd;
-	const struct iwl_channel_info *ch_info;
 	u32 switch_time_in_usec, ucode_switch_time;
 	u16 ch;
 	u32 tsf_low;
@@ -505,14 +414,7 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
 	}
 	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;
-	}
+	cmd.expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
 
 	return iwl_dvm_send_cmd(priv, &hcmd);
 }
@@ -520,36 +422,12 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
 struct iwl_lib_ops iwl5000_lib = {
 	.set_hw_params = iwl5000_hw_set_hw_params,
 	.set_channel_switch = iwl5000_hw_channel_switch,
-	.nic_config = iwl5000_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_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REG_BAND_52_HT40_CHANNELS
-		},
-	},
 	.temperature = iwlagn_temperature,
 };
 
 struct iwl_lib_ops iwl5150_lib = {
 	.set_hw_params = iwl5150_hw_set_hw_params,
 	.set_channel_switch = iwl5000_hw_channel_switch,
-	.nic_config = iwl5000_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_REG_BAND_24_HT40_CHANNELS,
-			EEPROM_REG_BAND_52_HT40_CHANNELS
-		},
-	},
 	.temperature = iwl5150_temperature,
 };
 
@@ -570,8 +448,6 @@ static void iwl6000_set_ct_threshold(struct iwl_priv *priv)
 /* NIC configuration for 6000 series */
 static void iwl6000_nic_config(struct iwl_priv *priv)
 {
-	iwl_rf_config(priv);
-
 	switch (priv->cfg->device_family) {
 	case IWL_DEVICE_FAMILY_6005:
 	case IWL_DEVICE_FAMILY_6030:
@@ -584,13 +460,13 @@ static void iwl6000_nic_config(struct iwl_priv *priv)
 		break;
 	case IWL_DEVICE_FAMILY_6050:
 		/* Indicate calibration version to uCode. */
-		if (iwl_eeprom_calib_version(priv) >= 6)
+		if (priv->eeprom_data->calib_version >= 6)
 			iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
 					CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
 		break;
 	case IWL_DEVICE_FAMILY_6150:
 		/* Indicate calibration version to uCode. */
-		if (iwl_eeprom_calib_version(priv) >= 6)
+		if (priv->eeprom_data->calib_version >= 6)
 			iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
 					CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
 		iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
@@ -627,17 +503,6 @@ static const struct iwl_sensitivity_ranges iwl6000_sensitivity = {
 
 static void iwl6000_hw_set_hw_params(struct iwl_priv *priv)
 {
-	priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-					BIT(IEEE80211_BAND_5GHZ);
-
-	priv->hw_params.tx_chains_num =
-		num_of_ant(priv->hw_params.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->hw_params.valid_rx_ant);
-
 	iwl6000_set_ct_threshold(priv);
 
 	/* Set initial sensitivity parameters */
@@ -654,7 +519,6 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
 	 */
 	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;
@@ -704,14 +568,7 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
 	}
 	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;
-	}
+	cmd.expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
 
 	return iwl_dvm_send_cmd(priv, &hcmd);
 }
@@ -720,18 +577,6 @@ struct iwl_lib_ops iwl6000_lib = {
 	.set_hw_params = iwl6000_hw_set_hw_params,
 	.set_channel_switch = iwl6000_hw_channel_switch,
 	.nic_config = iwl6000_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
-		},
-		.enhanced_txpower = true,
-	},
 	.temperature = iwlagn_temperature,
 };
 
@@ -739,17 +584,5 @@ struct iwl_lib_ops iwl6030_lib = {
 	.set_hw_params = iwl6000_hw_set_hw_params,
 	.set_channel_switch = iwl6000_hw_channel_switch,
 	.nic_config = iwl6000_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
-		},
-		.enhanced_txpower = true,
-	},
 	.temperature = iwlagn_temperature,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.c b/drivers/net/wireless/iwlwifi/dvm/led.c
index 47000419f91..bf479f70909 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.c
+++ b/drivers/net/wireless/iwlwifi/dvm/led.c
@@ -34,12 +34,11 @@
 #include <net/mac80211.h>
 #include <linux/etherdevice.h>
 #include <asm/unaligned.h>
-
-#include "iwl-dev.h"
-#include "iwl-agn.h"
 #include "iwl-io.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
 
 /* Throughput		OFF time(ms)	ON time (ms)
  *	>300			25		25
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.h b/drivers/net/wireless/iwlwifi/dvm/led.h
index b02a853103d..b02a853103d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.h
+++ b/drivers/net/wireless/iwlwifi/dvm/led.h
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/dvm/lib.c
index e55ec6c8a92..bef88c1a2c9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/lib.c
@@ -33,13 +33,14 @@
 #include <linux/sched.h>
 #include <net/mac80211.h>
 
-#include "iwl-dev.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
 
+#include "dev.h"
+#include "agn.h"
+
 int iwlagn_hw_valid_rtc_data_addr(u32 addr)
 {
 	return (addr >= IWLAGN_RTC_DATA_LOWER_BOUND) &&
@@ -58,8 +59,7 @@ int iwlagn_send_tx_power(struct iwl_priv *priv)
 	/* half dBm need to multiply */
 	tx_power_cmd.global_lmt = (s8)(2 * priv->tx_power_user_lmt);
 
-	if (priv->tx_power_lmt_in_half_dbm &&
-	    priv->tx_power_lmt_in_half_dbm < tx_power_cmd.global_lmt) {
+	if (tx_power_cmd.global_lmt > priv->eeprom_data->max_tx_pwr_half_dbm) {
 		/*
 		 * For the newer devices which using enhanced/extend tx power
 		 * table in EEPROM, the format is in half dBm. driver need to
@@ -71,7 +71,8 @@ int iwlagn_send_tx_power(struct iwl_priv *priv)
 		 * "tx_power_user_lmt" is higher than EEPROM value (in
 		 * half-dBm format), lower the tx power based on EEPROM
 		 */
-		tx_power_cmd.global_lmt = priv->tx_power_lmt_in_half_dbm;
+		tx_power_cmd.global_lmt =
+			priv->eeprom_data->max_tx_pwr_half_dbm;
 	}
 	tx_power_cmd.flags = IWLAGN_TX_POWER_NO_CLOSED;
 	tx_power_cmd.srv_chan_lmt = IWLAGN_TX_POWER_AUTO;
@@ -159,7 +160,7 @@ int iwlagn_txfifo_flush(struct iwl_priv *priv, u16 flush_control)
 				IWL_PAN_SCD_BK_MSK | IWL_PAN_SCD_MGMT_MSK |
 				IWL_PAN_SCD_MULTICAST_MSK;
 
-	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
+	if (priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE)
 		flush_cmd.fifo_control |= IWL_AGG_TX_QUEUE_MSK;
 
 	IWL_DEBUG_INFO(priv, "fifo queue control: 0X%x\n",
@@ -264,6 +265,8 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
 			bt_cmd_v2.tx_prio_boost = 0;
 			bt_cmd_v2.rx_prio_boost = 0;
 		} else {
+			/* older version only has 8 bits */
+			WARN_ON(priv->cfg->bt_params->bt_prio_boost & ~0xFF);
 			bt_cmd_v1.prio_boost =
 				priv->cfg->bt_params->bt_prio_boost;
 			bt_cmd_v1.tx_prio_boost = 0;
@@ -617,6 +620,11 @@ static bool iwlagn_fill_txpower_mode(struct iwl_priv *priv,
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
 	int ave_rssi;
 
+	if (!ctx->vif || (ctx->vif->type != NL80211_IFTYPE_STATION)) {
+		IWL_DEBUG_INFO(priv, "BSS ctx not active or not in sta mode\n");
+		return false;
+	}
+
 	ave_rssi = ieee80211_ave_rssi(ctx->vif);
 	if (!ave_rssi) {
 		/* no rssi data, no changes to reduce tx power */
@@ -818,7 +826,7 @@ void iwlagn_set_rxon_chain(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	if (priv->chain_noise_data.active_chains)
 		active_chains = priv->chain_noise_data.active_chains;
 	else
-		active_chains = priv->hw_params.valid_rx_ant;
+		active_chains = priv->eeprom_data->valid_rx_ant;
 
 	if (priv->cfg->bt_params &&
 	    priv->cfg->bt_params->advanced_bt_coexist &&
@@ -1259,7 +1267,7 @@ int iwl_dvm_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 	 * the mutex, this ensures we don't try to send two
 	 * (or more) synchronous commands at a time.
 	 */
-	if (cmd->flags & CMD_SYNC)
+	if (!(cmd->flags & CMD_ASYNC))
 		lockdep_assert_held(&priv->mutex);
 
 	if (priv->ucode_owner == IWL_OWNERSHIP_TM &&
diff --git a/drivers/net/wireless/iwlwifi/iwl-mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index 013680332f0..a5f7bce9632 100644
--- a/drivers/net/wireless/iwlwifi/iwl-mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -38,19 +38,20 @@
 #include <linux/etherdevice.h>
 #include <linux/if_arp.h>
 
+#include <net/ieee80211_radiotap.h>
 #include <net/mac80211.h>
 
 #include <asm/div64.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-agn-calib.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-modparams.h"
 
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
+
 /*****************************************************************************
  *
  * mac80211 entry point functions
@@ -154,6 +155,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 		    IEEE80211_HW_SCAN_WHILE_IDLE;
 
 	hw->offchannel_tx_hw_queue = IWL_AUX_QUEUE;
+	hw->radiotap_mcs_details |= IEEE80211_RADIOTAP_MCS_HAVE_FMT;
 
 	/*
 	 * Including the following line will crash some AP's.  This
@@ -162,7 +164,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	hw->max_tx_aggregation_subframes = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
 	 */
 
-	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
+	if (priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE)
 		hw->flags |= IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
 			     IEEE80211_HW_SUPPORTS_STATIC_SMPS;
 
@@ -237,12 +239,12 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 
 	hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
 
-	if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
+	if (priv->eeprom_data->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->eeprom_data->bands[IEEE80211_BAND_2GHZ];
+	if (priv->eeprom_data->bands[IEEE80211_BAND_5GHZ].n_channels)
 		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-			&priv->bands[IEEE80211_BAND_5GHZ];
+			&priv->eeprom_data->bands[IEEE80211_BAND_5GHZ];
 
 	hw->wiphy->hw_version = priv->trans->hw_id;
 
@@ -341,7 +343,7 @@ static int iwlagn_mac_start(struct ieee80211_hw *hw)
 	return 0;
 }
 
-void iwlagn_mac_stop(struct ieee80211_hw *hw)
+static void iwlagn_mac_stop(struct ieee80211_hw *hw)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -369,9 +371,9 @@ void iwlagn_mac_stop(struct ieee80211_hw *hw)
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif,
-			       struct cfg80211_gtk_rekey_data *data)
+static void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
+				      struct ieee80211_vif *vif,
+				      struct cfg80211_gtk_rekey_data *data)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -397,7 +399,8 @@ void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
 
 #ifdef CONFIG_PM_SLEEP
 
-int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
+static int iwlagn_mac_suspend(struct ieee80211_hw *hw,
+			      struct cfg80211_wowlan *wowlan)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
@@ -420,8 +423,6 @@ int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 	if (ret)
 		goto error;
 
-	device_set_wakeup_enable(priv->trans->dev, true);
-
 	iwl_trans_wowlan_suspend(priv->trans);
 
 	goto out;
@@ -475,7 +476,7 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 			}
 
 			if (priv->wowlan_sram)
-				_iwl_read_targ_mem_words(
+				_iwl_read_targ_mem_dwords(
 				      priv->trans, 0x800000,
 				      priv->wowlan_sram,
 				      img->sec[IWL_UCODE_SECTION_DATA].len / 4);
@@ -488,8 +489,6 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 
 	priv->wowlan = false;
 
-	device_set_wakeup_enable(priv->trans->dev, false);
-
 	iwlagn_prepare_restart(priv);
 
 	memset((void *)&ctx->active, 0, sizeof(ctx->active));
@@ -504,9 +503,15 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
 	return 1;
 }
 
+static void iwlagn_mac_set_wakeup(struct ieee80211_hw *hw, bool enabled)
+{
+	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+
+	device_set_wakeup_enable(priv->trans->dev, enabled);
+}
 #endif
 
-void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -517,21 +522,21 @@ void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 		dev_kfree_skb_any(skb);
 }
 
-void iwlagn_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)
+static void iwlagn_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 = IWL_MAC80211_GET_DVM(hw);
 
 	iwl_update_tkip_key(priv, vif, keyconf, sta, iv32, phase1key);
 }
 
-int iwlagn_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)
+static int iwlagn_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 = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -631,11 +636,11 @@ int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return ret;
 }
 
-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,
-			    u8 buf_size)
+static 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,
+				   u8 buf_size)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	int ret = -EINVAL;
@@ -644,7 +649,7 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 	IWL_DEBUG_HT(priv, "A-MPDU action on addr %pM tid %d\n",
 		     sta->addr, tid);
 
-	if (!(priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE))
+	if (!(priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE))
 		return -EACCES;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
@@ -662,7 +667,7 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 		ret = iwl_sta_rx_agg_stop(priv, sta, tid);
 		break;
 	case IEEE80211_AMPDU_TX_START:
-		if (!priv->trans->ops->tx_agg_setup)
+		if (!priv->trans->ops->txq_enable)
 			break;
 		if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_TXAGG)
 			break;
@@ -757,11 +762,11 @@ static int iwlagn_mac_sta_remove(struct ieee80211_hw *hw,
 	return ret;
 }
 
-int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
-			 struct ieee80211_vif *vif,
-			 struct ieee80211_sta *sta,
-			 enum ieee80211_sta_state old_state,
-			 enum ieee80211_sta_state new_state)
+static int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				struct ieee80211_sta *sta,
+				enum ieee80211_sta_state old_state,
+				enum ieee80211_sta_state new_state)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -852,11 +857,10 @@ int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
 	return ret;
 }
 
-void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
-			       struct ieee80211_channel_switch *ch_switch)
+static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
+				      struct ieee80211_channel_switch *ch_switch)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	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;
@@ -893,12 +897,6 @@ void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
 	if (le16_to_cpu(ctx->active.channel) == ch)
 		goto out;
 
-	ch_info = iwl_get_channel_info(priv, channel->band, ch);
-	if (!is_channel_valid(ch_info)) {
-		IWL_DEBUG_MAC80211(priv, "invalid channel\n");
-		goto out;
-	}
-
 	priv->current_ht_config.smps = conf->smps_mode;
 
 	/* Configure HT40 channels */
@@ -947,10 +945,10 @@ void iwl_chswitch_done(struct iwl_priv *priv, bool is_success)
 		ieee80211_chswitch_done(ctx->vif, is_success);
 }
 
-void iwlagn_configure_filter(struct ieee80211_hw *hw,
-			     unsigned int changed_flags,
-			     unsigned int *total_flags,
-			     u64 multicast)
+static void iwlagn_configure_filter(struct ieee80211_hw *hw,
+				    unsigned int changed_flags,
+				    unsigned int *total_flags,
+				    u64 multicast)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	__le32 filter_or = 0, filter_nand = 0;
@@ -997,7 +995,7 @@ void iwlagn_configure_filter(struct ieee80211_hw *hw,
 			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
-void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
+static void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1050,8 +1048,18 @@ static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
 	mutex_lock(&priv->mutex);
 
 	if (test_bit(STATUS_SCAN_HW, &priv->status)) {
-		err = -EBUSY;
-		goto out;
+		/* mac80211 should not scan while ROC or ROC while scanning */
+		if (WARN_ON_ONCE(priv->scan_type != IWL_SCAN_RADIO_RESET)) {
+			err = -EBUSY;
+			goto out;
+		}
+
+		iwl_scan_cancel_timeout(priv, 100);
+
+		if (test_bit(STATUS_SCAN_HW, &priv->status)) {
+			err = -EBUSY;
+			goto out;
+		}
 	}
 
 	priv->hw_roc_channel = channel;
@@ -1124,7 +1132,7 @@ static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
 	return err;
 }
 
-int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
+static int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1141,8 +1149,8 @@ int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
 	return 0;
 }
 
-void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
-			      enum ieee80211_rssi_event rssi_event)
+static void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
+				     enum ieee80211_rssi_event rssi_event)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1166,8 +1174,8 @@ void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
-		       struct ieee80211_sta *sta, bool set)
+static int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
+			      struct ieee80211_sta *sta, bool set)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1176,9 +1184,9 @@ int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
 	return 0;
 }
 
-int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
-		       struct ieee80211_vif *vif, u16 queue,
-		       const struct ieee80211_tx_queue_params *params)
+static int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
+			      struct ieee80211_vif *vif, u16 queue,
+			      const struct ieee80211_tx_queue_params *params)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -1220,7 +1228,7 @@ int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
 	return 0;
 }
 
-int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw)
+static int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1236,7 +1244,8 @@ static int iwl_set_mode(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	return iwlagn_commit_rxon(priv, ctx);
 }
 
-int iwl_setup_interface(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+static int iwl_setup_interface(struct iwl_priv *priv,
+			       struct iwl_rxon_context *ctx)
 {
 	struct ieee80211_vif *vif = ctx->vif;
 	int err, ac;
@@ -1356,9 +1365,9 @@ static int iwlagn_mac_add_interface(struct ieee80211_hw *hw,
 	return err;
 }
 
-void iwl_teardown_interface(struct iwl_priv *priv,
-			    struct ieee80211_vif *vif,
-			    bool mode_change)
+static void iwl_teardown_interface(struct iwl_priv *priv,
+				   struct ieee80211_vif *vif,
+				   bool mode_change)
 {
 	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
 
@@ -1414,13 +1423,11 @@ static void iwlagn_mac_remove_interface(struct ieee80211_hw *hw,
 }
 
 static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif,
-				enum nl80211_iftype newtype, bool newp2p)
+				       struct ieee80211_vif *vif,
+				       enum nl80211_iftype newtype, bool newp2p)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
-	struct iwl_rxon_context *bss_ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-	struct iwl_rxon_context *tmp;
+	struct iwl_rxon_context *ctx, *tmp;
 	enum nl80211_iftype newviftype = newtype;
 	u32 interface_modes;
 	int err;
@@ -1431,6 +1438,18 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&priv->mutex);
 
+	ctx = iwl_rxon_ctx_from_vif(vif);
+
+	/*
+	 * To simplify this code, only support changes on the
+	 * BSS context. The PAN context is usually reassigned
+	 * by creating/removing P2P interfaces anyway.
+	 */
+	if (ctx->ctxid != IWL_RXON_CTX_BSS) {
+		err = -EBUSY;
+		goto out;
+	}
+
 	if (!ctx->vif || !iwl_is_ready_rf(priv)) {
 		/*
 		 * Huh? But wait ... this can maybe happen when
@@ -1440,32 +1459,19 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
 		goto out;
 	}
 
+	/* Check if the switch is supported in the same context */
 	interface_modes = ctx->interface_modes | ctx->exclusive_interface_modes;
-
 	if (!(interface_modes & BIT(newtype))) {
 		err = -EBUSY;
 		goto out;
 	}
 
-	/*
-	 * Refuse a change that should be done by moving from the PAN
-	 * context to the BSS context instead, if the BSS context is
-	 * available and can support the new interface type.
-	 */
-	if (ctx->ctxid == IWL_RXON_CTX_PAN && !bss_ctx->vif &&
-	    (bss_ctx->interface_modes & BIT(newtype) ||
-	     bss_ctx->exclusive_interface_modes & BIT(newtype))) {
-		BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
-		err = -EBUSY;
-		goto out;
-	}
-
 	if (ctx->exclusive_interface_modes & BIT(newtype)) {
 		for_each_context(priv, tmp) {
 			if (ctx == tmp)
 				continue;
 
-			if (!tmp->vif)
+			if (!tmp->is_active)
 				continue;
 
 			/*
@@ -1499,9 +1505,9 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
 	return err;
 }
 
-int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
-		       struct ieee80211_vif *vif,
-		       struct cfg80211_scan_request *req)
+static int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
+			      struct ieee80211_vif *vif,
+			      struct cfg80211_scan_request *req)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	int ret;
@@ -1556,10 +1562,10 @@ static void iwl_sta_modify_ps_wake(struct iwl_priv *priv, int sta_id)
 	iwl_send_add_sta(priv, &cmd, CMD_ASYNC);
 }
 
-void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
-			   struct ieee80211_vif *vif,
-			   enum sta_notify_cmd cmd,
-			   struct ieee80211_sta *sta)
+static void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
+				  struct ieee80211_vif *vif,
+				  enum sta_notify_cmd cmd,
+				  struct ieee80211_sta *sta)
 {
 	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 	struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
@@ -1596,6 +1602,7 @@ struct ieee80211_ops iwlagn_hw_ops = {
 #ifdef CONFIG_PM_SLEEP
 	.suspend = iwlagn_mac_suspend,
 	.resume = iwlagn_mac_resume,
+	.set_wakeup = iwlagn_mac_set_wakeup,
 #endif
 	.add_interface = iwlagn_mac_add_interface,
 	.remove_interface = iwlagn_mac_remove_interface,
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index ec36e2b020b..84d3db5aa50 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -44,15 +44,19 @@
 
 #include <asm/div64.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
+#include "iwl-eeprom-read.h"
+#include "iwl-eeprom-parse.h"
 #include "iwl-io.h"
-#include "iwl-agn-calib.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-drv.h"
 #include "iwl-modparams.h"
+#include "iwl-prph.h"
+
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
+
 
 /******************************************************************************
  *
@@ -78,7 +82,8 @@ MODULE_DESCRIPTION(DRV_DESCRIPTION);
 MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("iwlagn");
+
+static const struct iwl_op_mode_ops iwl_dvm_ops;
 
 void iwl_update_chain_flags(struct iwl_priv *priv)
 {
@@ -180,7 +185,7 @@ int iwlagn_send_beacon_cmd(struct iwl_priv *priv)
 		rate = info->control.rates[0].idx;
 
 	priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-					      priv->hw_params.valid_tx_ant);
+					      priv->eeprom_data->valid_tx_ant);
 	rate_flags = iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
 
 	/* In mac80211, rates for 5 GHz start at 0 */
@@ -403,7 +408,7 @@ static void iwl_continuous_event_trace(struct iwl_priv *priv)
 
 	base = priv->device_pointers.log_event_table;
 	if (iwlagn_hw_valid_rtc_data_addr(base)) {
-		iwl_read_targ_mem_words(priv->trans, base, &read, sizeof(read));
+		iwl_read_targ_mem_bytes(priv->trans, base, &read, sizeof(read));
 		capacity = read.capacity;
 		mode = read.mode;
 		num_wraps = read.wrap_counter;
@@ -513,49 +518,6 @@ static void iwl_bg_tx_flush(struct work_struct *work)
  * queue/FIFO/AC mapping definitions
  */
 
-#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_BK_IPAN	IWL_TX_FIFO_BK
-#define IWL_TX_FIFO_BE_IPAN	4
-#define IWL_TX_FIFO_VI_IPAN	IWL_TX_FIFO_VI
-#define IWL_TX_FIFO_VO_IPAN	5
-/* re-uses the VO FIFO, uCode will properly flush/schedule */
-#define IWL_TX_FIFO_AUX		5
-#define IWL_TX_FIFO_UNUSED	-1
-
-#define IWLAGN_CMD_FIFO_NUM	7
-
-/*
- * This queue number is required for proper operation
- * because the ucode will stop/start the scheduler as
- * required.
- */
-#define IWL_IPAN_MCAST_QUEUE	8
-
-static const u8 iwlagn_default_queue_to_tx_fifo[] = {
-	IWL_TX_FIFO_VO,
-	IWL_TX_FIFO_VI,
-	IWL_TX_FIFO_BE,
-	IWL_TX_FIFO_BK,
-	IWLAGN_CMD_FIFO_NUM,
-};
-
-static const u8 iwlagn_ipan_queue_to_tx_fifo[] = {
-	IWL_TX_FIFO_VO,
-	IWL_TX_FIFO_VI,
-	IWL_TX_FIFO_BE,
-	IWL_TX_FIFO_BK,
-	IWL_TX_FIFO_BK_IPAN,
-	IWL_TX_FIFO_BE_IPAN,
-	IWL_TX_FIFO_VI_IPAN,
-	IWL_TX_FIFO_VO_IPAN,
-	IWL_TX_FIFO_BE_IPAN,
-	IWLAGN_CMD_FIFO_NUM,
-	IWL_TX_FIFO_AUX,
-};
-
 static const u8 iwlagn_bss_ac_to_fifo[] = {
 	IWL_TX_FIFO_VO,
 	IWL_TX_FIFO_VI,
@@ -578,7 +540,7 @@ static const u8 iwlagn_pan_ac_to_queue[] = {
 	7, 6, 5, 4,
 };
 
-void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
+static void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 {
 	int i;
 
@@ -645,7 +607,7 @@ void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 	BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
 }
 
-void iwl_rf_kill_ct_config(struct iwl_priv *priv)
+static void iwl_rf_kill_ct_config(struct iwl_priv *priv)
 {
 	struct iwl_ct_kill_config cmd;
 	struct iwl_ct_kill_throttling_config adv_cmd;
@@ -726,7 +688,7 @@ static int iwlagn_send_tx_ant_config(struct iwl_priv *priv, u8 valid_tx_ant)
 	}
 }
 
-void iwl_send_bt_config(struct iwl_priv *priv)
+static void iwl_send_bt_config(struct iwl_priv *priv)
 {
 	struct iwl_bt_cmd bt_cmd = {
 		.lead_time = BT_LEAD_TIME_DEF,
@@ -814,7 +776,7 @@ int iwl_alive_start(struct iwl_priv *priv)
 	ieee80211_wake_queues(priv->hw);
 
 	/* Configure Tx antenna selection based on H/W config */
-	iwlagn_send_tx_ant_config(priv, priv->hw_params.valid_tx_ant);
+	iwlagn_send_tx_ant_config(priv, priv->eeprom_data->valid_tx_ant);
 
 	if (iwl_is_associated_ctx(ctx) && !priv->wowlan) {
 		struct iwl_rxon_cmd *active_rxon =
@@ -932,11 +894,12 @@ void iwl_down(struct iwl_priv *priv)
 	priv->ucode_loaded = false;
 	iwl_trans_stop_device(priv->trans);
 
+	/* Set num_aux_in_flight must be done after the transport is stopped */
+	atomic_set(&priv->num_aux_in_flight, 0);
+
 	/* Clear out all status bits but a few that are stable across reset */
 	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) <<
@@ -1078,7 +1041,7 @@ static void iwlagn_disable_roc_work(struct work_struct *work)
  *
  *****************************************************************************/
 
-void iwl_setup_deferred_work(struct iwl_priv *priv)
+static void iwl_setup_deferred_work(struct iwl_priv *priv)
 {
 	priv->workqueue = create_singlethread_workqueue(DRV_NAME);
 
@@ -1123,224 +1086,14 @@ void iwl_cancel_deferred_work(struct iwl_priv *priv)
 	del_timer_sync(&priv->ucode_trace);
 }
 
-static void iwl_init_hw_rates(struct ieee80211_rate *rates)
-{
-	int i;
-
-	for (i = 0; i < IWL_RATE_COUNT_LEGACY; i++) {
-		rates[i].bitrate = iwl_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 |=
-				(iwl_rates[i].plcp == IWL_RATE_1M_PLCP) ?
-					0 : IEEE80211_RATE_SHORT_PREAMBLE;
-		}
-	}
-}
-
-#define MAX_BIT_RATE_40_MHZ 150 /* Mbps */
-#define MAX_BIT_RATE_20_MHZ 72 /* Mbps */
-static void iwl_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;
-
-	if (priv->cfg->ht_params &&
-	    priv->cfg->ht_params->ht_greenfield_support)
-		ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
-	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 (iwlwifi_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_init_geos - Initialize mac80211's geo/channel info based from eeprom
- */
-static int iwl_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;
-	s8 max_tx_power = IWLAGN_TX_POWER_TARGET_POWER_MIN;
-
-	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 = kcalloc(priv->channel_count,
-			   sizeof(struct ieee80211_channel), GFP_KERNEL);
-	if (!channels)
-		return -ENOMEM;
-
-	rates = kcalloc(IWL_RATE_COUNT_LEGACY, sizeof(struct ieee80211_rate),
-			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(iwl_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->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
-		iwl_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->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
-		iwl_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];
-
-		/* FIXME: might be removed if scan is OK */
-		if (!is_channel_valid(ch))
-			continue;
-
-		sband =  &priv->bands[ch->band];
-
-		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 (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 > max_tx_power)
-				max_tx_power = 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,
-				is_channel_a_band(ch) ?  "5.2" : "2.4",
-				geo_ch->flags & IEEE80211_CHAN_DISABLED ?
-				"restricted" : "valid",
-				 geo_ch->flags);
-	}
-
-	priv->tx_power_device_lmt = max_tx_power;
-	priv->tx_power_user_lmt = max_tx_power;
-	priv->tx_power_next = max_tx_power;
-
-	if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
-	     priv->hw_params.sku & EEPROM_SKU_CAP_BAND_52GHZ) {
-		IWL_INFO(priv, "Incorrectly detected BG card as ABG. "
-			"Please send your %s to maintainer.\n",
-			priv->trans->hw_id_str);
-		priv->hw_params.sku &= ~EEPROM_SKU_CAP_BAND_52GHZ;
-	}
-
-	if (iwlwifi_mod_params.disable_5ghz)
-		priv->bands[IEEE80211_BAND_5GHZ].n_channels = 0;
-
-	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;
-}
-
-/*
- * iwl_free_geos - undo allocations in iwl_init_geos
- */
-static void iwl_free_geos(struct iwl_priv *priv)
-{
-	kfree(priv->ieee_channels);
-	kfree(priv->ieee_rates);
-	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
-}
-
-int iwl_init_drv(struct iwl_priv *priv)
+static int iwl_init_drv(struct iwl_priv *priv)
 {
-	int ret;
-
 	spin_lock_init(&priv->sta_lock);
 
 	mutex_init(&priv->mutex);
 
 	INIT_LIST_HEAD(&priv->calib_results);
 
-	priv->ieee_channels = NULL;
-	priv->ieee_rates = NULL;
 	priv->band = IEEE80211_BAND_2GHZ;
 
 	priv->plcp_delta_threshold =
@@ -1371,31 +1124,11 @@ int iwl_init_drv(struct iwl_priv *priv)
 		priv->dynamic_frag_thresh = BT_FRAG_THRESHOLD_DEF;
 	}
 
-	ret = iwl_init_channel_map(priv);
-	if (ret) {
-		IWL_ERR(priv, "initializing regulatory failed: %d\n", ret);
-		goto err;
-	}
-
-	ret = iwl_init_geos(priv);
-	if (ret) {
-		IWL_ERR(priv, "initializing geos failed: %d\n", ret);
-		goto err_free_channel_map;
-	}
-	iwl_init_hw_rates(priv->ieee_rates);
-
 	return 0;
-
-err_free_channel_map:
-	iwl_free_channel_map(priv);
-err:
-	return ret;
 }
 
-void iwl_uninit_drv(struct iwl_priv *priv)
+static void iwl_uninit_drv(struct iwl_priv *priv)
 {
-	iwl_free_geos(priv);
-	iwl_free_channel_map(priv);
 	kfree(priv->scan_cmd);
 	kfree(priv->beacon_cmd);
 	kfree(rcu_dereference_raw(priv->noa_data));
@@ -1405,15 +1138,12 @@ void iwl_uninit_drv(struct iwl_priv *priv)
 #endif
 }
 
-void iwl_set_hw_params(struct iwl_priv *priv)
+static void iwl_set_hw_params(struct iwl_priv *priv)
 {
 	if (priv->cfg->ht_params)
 		priv->hw_params.use_rts_for_aggregation =
 			priv->cfg->ht_params->use_rts_for_aggregation;
 
-	if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
-		priv->hw_params.sku &= ~EEPROM_SKU_CAP_11N_ENABLE;
-
 	/* Device-specific setup */
 	priv->lib->set_hw_params(priv);
 }
@@ -1421,7 +1151,7 @@ void iwl_set_hw_params(struct iwl_priv *priv)
 
 
 /* show what optional capabilities we have */
-void iwl_option_config(struct iwl_priv *priv)
+static void iwl_option_config(struct iwl_priv *priv)
 {
 #ifdef CONFIG_IWLWIFI_DEBUG
 	IWL_INFO(priv, "CONFIG_IWLWIFI_DEBUG enabled\n");
@@ -1454,6 +1184,42 @@ void iwl_option_config(struct iwl_priv *priv)
 #endif
 }
 
+static int iwl_eeprom_init_hw_params(struct iwl_priv *priv)
+{
+	u16 radio_cfg;
+
+	priv->eeprom_data->sku = priv->eeprom_data->sku;
+
+	if (priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE &&
+	    !priv->cfg->ht_params) {
+		IWL_ERR(priv, "Invalid 11n configuration\n");
+		return -EINVAL;
+	}
+
+	if (!priv->eeprom_data->sku) {
+		IWL_ERR(priv, "Invalid device sku\n");
+		return -EINVAL;
+	}
+
+	IWL_INFO(priv, "Device SKU: 0x%X\n", priv->eeprom_data->sku);
+
+	radio_cfg = priv->eeprom_data->radio_cfg;
+
+	priv->hw_params.tx_chains_num =
+		num_of_ant(priv->eeprom_data->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->eeprom_data->valid_rx_ant);
+
+	IWL_INFO(priv, "Valid Tx ant: 0x%X, Valid Rx ant: 0x%X\n",
+		 priv->eeprom_data->valid_tx_ant,
+		 priv->eeprom_data->valid_rx_ant);
+
+	return 0;
+}
+
 static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 						 const struct iwl_cfg *cfg,
 						 const struct iwl_fw *fw)
@@ -1466,7 +1232,6 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	struct iwl_trans_config trans_cfg;
 	static const u8 no_reclaim_cmds[] = {
 		REPLY_RX_PHY_CMD,
-		REPLY_RX,
 		REPLY_RX_MPDU_CMD,
 		REPLY_COMPRESSED_BA,
 		STATISTICS_NOTIFICATION,
@@ -1539,8 +1304,12 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 		trans_cfg.queue_watchdog_timeout =
 			priv->cfg->base_params->wd_timeout;
 	else
-		trans_cfg.queue_watchdog_timeout = IWL_WATCHHDOG_DISABLED;
+		trans_cfg.queue_watchdog_timeout = IWL_WATCHDOG_DISABLED;
 	trans_cfg.command_names = iwl_dvm_cmd_strings;
+	trans_cfg.cmd_fifo = IWLAGN_CMD_FIFO_NUM;
+
+	WARN_ON(sizeof(priv->transport_queue_stop) * BITS_PER_BYTE <
+		priv->cfg->base_params->num_of_queues);
 
 	ucode_flags = fw->ucode_capa.flags;
 
@@ -1551,15 +1320,9 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	if (ucode_flags & IWL_UCODE_TLV_FLAGS_PAN) {
 		priv->sta_key_max_num = STA_KEY_MAX_NUM_PAN;
 		trans_cfg.cmd_queue = IWL_IPAN_CMD_QUEUE_NUM;
-		trans_cfg.queue_to_fifo = iwlagn_ipan_queue_to_tx_fifo;
-		trans_cfg.n_queue_to_fifo =
-			ARRAY_SIZE(iwlagn_ipan_queue_to_tx_fifo);
 	} else {
 		priv->sta_key_max_num = STA_KEY_MAX_NUM;
 		trans_cfg.cmd_queue = IWL_DEFAULT_CMD_QUEUE_NUM;
-		trans_cfg.queue_to_fifo = iwlagn_default_queue_to_tx_fifo;
-		trans_cfg.n_queue_to_fifo =
-			ARRAY_SIZE(iwlagn_default_queue_to_tx_fifo);
 	}
 
 	/* Configure transport layer */
@@ -1599,25 +1362,33 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 		goto out_free_hw;
 
 	/* Read the EEPROM */
-	if (iwl_eeprom_init(priv, priv->trans->hw_rev)) {
+	if (iwl_read_eeprom(priv->trans, &priv->eeprom_blob,
+			    &priv->eeprom_blob_size)) {
 		IWL_ERR(priv, "Unable to init EEPROM\n");
 		goto out_free_hw;
 	}
+
 	/* Reset chip to save power until we load uCode during "up". */
 	iwl_trans_stop_hw(priv->trans, false);
 
-	if (iwl_eeprom_check_version(priv))
+	priv->eeprom_data = iwl_parse_eeprom_data(priv->trans->dev, priv->cfg,
+						  priv->eeprom_blob,
+						  priv->eeprom_blob_size);
+	if (!priv->eeprom_data)
+		goto out_free_eeprom_blob;
+
+	if (iwl_eeprom_check_version(priv->eeprom_data, priv->trans))
 		goto out_free_eeprom;
 
 	if (iwl_eeprom_init_hw_params(priv))
 		goto out_free_eeprom;
 
 	/* extract MAC Address */
-	iwl_eeprom_get_mac(priv, priv->addresses[0].addr);
+	memcpy(priv->addresses[0].addr, priv->eeprom_data->hw_addr, ETH_ALEN);
 	IWL_DEBUG_INFO(priv, "MAC address: %pM\n", priv->addresses[0].addr);
 	priv->hw->wiphy->addresses = priv->addresses;
 	priv->hw->wiphy->n_addresses = 1;
-	num_mac = iwl_eeprom_query16(priv, EEPROM_NUM_MAC_ADDRESS);
+	num_mac = priv->eeprom_data->n_hw_addrs;
 	if (num_mac > 1) {
 		memcpy(priv->addresses[1].addr, priv->addresses[0].addr,
 		       ETH_ALEN);
@@ -1630,7 +1401,7 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	 ************************/
 	iwl_set_hw_params(priv);
 
-	if (!(priv->hw_params.sku & EEPROM_SKU_CAP_IPAN_ENABLE)) {
+	if (!(priv->eeprom_data->sku & EEPROM_SKU_CAP_IPAN_ENABLE)) {
 		IWL_DEBUG_INFO(priv, "Your EEPROM disabled PAN");
 		ucode_flags &= ~IWL_UCODE_TLV_FLAGS_PAN;
 		/*
@@ -1640,9 +1411,6 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 		ucode_flags &= ~IWL_UCODE_TLV_FLAGS_P2P;
 		priv->sta_key_max_num = STA_KEY_MAX_NUM;
 		trans_cfg.cmd_queue = IWL_DEFAULT_CMD_QUEUE_NUM;
-		trans_cfg.queue_to_fifo = iwlagn_default_queue_to_tx_fifo;
-		trans_cfg.n_queue_to_fifo =
-			ARRAY_SIZE(iwlagn_default_queue_to_tx_fifo);
 
 		/* Configure transport layer again*/
 		iwl_trans_configure(priv->trans, &trans_cfg);
@@ -1660,9 +1428,6 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 		atomic_set(&priv->queue_stop_count[i], 0);
 	}
 
-	WARN_ON(trans_cfg.queue_to_fifo[trans_cfg.cmd_queue] !=
-						IWLAGN_CMD_FIFO_NUM);
-
 	if (iwl_init_drv(priv))
 		goto out_free_eeprom;
 
@@ -1711,8 +1476,10 @@ out_destroy_workqueue:
 	destroy_workqueue(priv->workqueue);
 	priv->workqueue = NULL;
 	iwl_uninit_drv(priv);
+out_free_eeprom_blob:
+	kfree(priv->eeprom_blob);
 out_free_eeprom:
-	iwl_eeprom_free(priv);
+	iwl_free_eeprom_data(priv->eeprom_data);
 out_free_hw:
 	ieee80211_free_hw(priv->hw);
 out:
@@ -1720,7 +1487,7 @@ out:
 	return op_mode;
 }
 
-void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
+static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -1728,7 +1495,7 @@ void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 
 	iwl_dbgfs_unregister(priv);
 
-	iwl_testmode_cleanup(priv);
+	iwl_testmode_free(priv);
 	iwlagn_mac_unregister(priv);
 
 	iwl_tt_exit(priv);
@@ -1737,7 +1504,8 @@ void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 	priv->ucode_loaded = false;
 	iwl_trans_stop_device(priv->trans);
 
-	iwl_eeprom_free(priv);
+	kfree(priv->eeprom_blob);
+	iwl_free_eeprom_data(priv->eeprom_data);
 
 	/*netif_stop_queue(dev); */
 	flush_workqueue(priv->workqueue);
@@ -1850,7 +1618,7 @@ static void iwl_dump_nic_error_log(struct iwl_priv *priv)
 	}
 
 	/*TODO: Update dbgfs with ISR error stats obtained below */
-	iwl_read_targ_mem_words(trans, base, &table, sizeof(table));
+	iwl_read_targ_mem_bytes(trans, base, &table, sizeof(table));
 
 	if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
 		IWL_ERR(trans, "Start IWL Error Log Dump:\n");
@@ -2185,7 +1953,7 @@ static void iwlagn_fw_error(struct iwl_priv *priv, bool ondemand)
 	}
 }
 
-void iwl_nic_error(struct iwl_op_mode *op_mode)
+static void iwl_nic_error(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -2198,7 +1966,7 @@ void iwl_nic_error(struct iwl_op_mode *op_mode)
 	iwlagn_fw_error(priv, false);
 }
 
-void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
+static void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -2208,11 +1976,60 @@ void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
 	}
 }
 
-void iwl_nic_config(struct iwl_op_mode *op_mode)
+#define EEPROM_RF_CONFIG_TYPE_MAX      0x3
+
+static void iwl_nic_config(struct iwl_op_mode *op_mode)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	u16 radio_cfg = priv->eeprom_data->radio_cfg;
+
+	/* SKU Control */
+	iwl_set_bits_mask(priv->trans, CSR_HW_IF_CONFIG_REG,
+			  CSR_HW_IF_CONFIG_REG_MSK_MAC_DASH |
+			  CSR_HW_IF_CONFIG_REG_MSK_MAC_STEP,
+			  (CSR_HW_REV_STEP(priv->trans->hw_rev) <<
+				CSR_HW_IF_CONFIG_REG_POS_MAC_STEP) |
+			  (CSR_HW_REV_DASH(priv->trans->hw_rev) <<
+				CSR_HW_IF_CONFIG_REG_POS_MAC_DASH));
+
+	/* write radio config values to register */
+	if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <= EEPROM_RF_CONFIG_TYPE_MAX) {
+		u32 reg_val =
+			EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <<
+				CSR_HW_IF_CONFIG_REG_POS_PHY_TYPE |
+			EEPROM_RF_CFG_STEP_MSK(radio_cfg) <<
+				CSR_HW_IF_CONFIG_REG_POS_PHY_STEP |
+			EEPROM_RF_CFG_DASH_MSK(radio_cfg) <<
+				CSR_HW_IF_CONFIG_REG_POS_PHY_DASH;
+
+		iwl_set_bits_mask(priv->trans, CSR_HW_IF_CONFIG_REG,
+				  CSR_HW_IF_CONFIG_REG_MSK_PHY_TYPE |
+				  CSR_HW_IF_CONFIG_REG_MSK_PHY_STEP |
+				  CSR_HW_IF_CONFIG_REG_MSK_PHY_DASH, reg_val);
+
+		IWL_INFO(priv, "Radio type=0x%x-0x%x-0x%x\n",
+			 EEPROM_RF_CFG_TYPE_MSK(radio_cfg),
+			 EEPROM_RF_CFG_STEP_MSK(radio_cfg),
+			 EEPROM_RF_CFG_DASH_MSK(radio_cfg));
+	} else {
+		WARN_ON(1);
+	}
 
-	priv->lib->nic_config(priv);
+	/* set CSR_HW_CONFIG_REG for uCode use */
+	iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
+		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+
+	/* W/A : NIC is stuck in a reset state after Early PCIe power off
+	 * (PCIe power is lost before PERST# is asserted),
+	 * causing ME FW to lose ownership and not being able to obtain it back.
+	 */
+	iwl_set_bits_mask_prph(priv->trans, APMG_PS_CTRL_REG,
+			       APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
+			       ~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
+
+	if (priv->lib->nic_config)
+		priv->lib->nic_config(priv);
 }
 
 static void iwl_wimax_active(struct iwl_op_mode *op_mode)
@@ -2223,7 +2040,7 @@ static void iwl_wimax_active(struct iwl_op_mode *op_mode)
 	IWL_ERR(priv, "RF is used by WiMAX\n");
 }
 
-void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
+static void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 	int mq = priv->queue_to_mac80211[queue];
@@ -2242,7 +2059,7 @@ void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
 	ieee80211_stop_queue(priv->hw, mq);
 }
 
-void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
+static void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 	int mq = priv->queue_to_mac80211[queue];
@@ -2282,16 +2099,17 @@ void iwlagn_lift_passive_no_rx(struct iwl_priv *priv)
 	priv->passive_no_rx = false;
 }
 
-void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+static void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb)
 {
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 	struct ieee80211_tx_info *info;
 
 	info = IEEE80211_SKB_CB(skb);
-	kmem_cache_free(iwl_tx_cmd_pool, (info->driver_data[1]));
+	iwl_trans_free_tx_cmd(priv->trans, info->driver_data[1]);
 	dev_kfree_skb_any(skb);
 }
 
-void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
+static void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
 {
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -2303,7 +2121,7 @@ void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
 	wiphy_rfkill_set_hw_state(priv->hw->wiphy, state);
 }
 
-const struct iwl_op_mode_ops iwl_dvm_ops = {
+static const struct iwl_op_mode_ops iwl_dvm_ops = {
 	.start = iwl_op_mode_dvm_start,
 	.stop = iwl_op_mode_dvm_stop,
 	.rx = iwl_rx_dispatch,
@@ -2322,9 +2140,6 @@ const struct iwl_op_mode_ops iwl_dvm_ops = {
  * driver and module entry point
  *
  *****************************************************************************/
-
-struct kmem_cache *iwl_tx_cmd_pool;
-
 static int __init iwl_init(void)
 {
 
@@ -2332,36 +2147,25 @@ static int __init iwl_init(void)
 	pr_info(DRV_DESCRIPTION ", " DRV_VERSION "\n");
 	pr_info(DRV_COPYRIGHT "\n");
 
-	iwl_tx_cmd_pool = kmem_cache_create("iwl_dev_cmd",
-					    sizeof(struct iwl_device_cmd),
-					    sizeof(void *), 0, NULL);
-	if (!iwl_tx_cmd_pool)
-		return -ENOMEM;
-
 	ret = iwlagn_rate_control_register();
 	if (ret) {
 		pr_err("Unable to register rate control algorithm: %d\n", ret);
-		goto error_rc_register;
+		return ret;
 	}
 
-	ret = iwl_pci_register_driver();
-	if (ret)
-		goto error_pci_register;
-	return ret;
+	ret = iwl_opmode_register("iwldvm", &iwl_dvm_ops);
+	if (ret) {
+		pr_err("Unable to register op_mode: %d\n", ret);
+		iwlagn_rate_control_unregister();
+	}
 
-error_pci_register:
-	iwlagn_rate_control_unregister();
-error_rc_register:
-	kmem_cache_destroy(iwl_tx_cmd_pool);
 	return ret;
 }
+module_init(iwl_init);
 
 static void __exit iwl_exit(void)
 {
-	iwl_pci_unregister_driver();
+	iwl_opmode_deregister("iwldvm");
 	iwlagn_rate_control_unregister();
-	kmem_cache_destroy(iwl_tx_cmd_pool);
 }
-
 module_exit(iwl_exit);
-module_init(iwl_init);
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/dvm/power.c
index 544ddf17f5b..518cf371580 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/dvm/power.c
@@ -31,18 +31,15 @@
 #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-agn.h"
 #include "iwl-io.h"
-#include "iwl-commands.h"
 #include "iwl-debug.h"
-#include "iwl-power.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
+#include "commands.h"
+#include "power.h"
 
 /*
  * Setting power level allows the card to go to sleep when not busy.
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.h b/drivers/net/wireless/iwlwifi/dvm/power.h
index 21afc92efac..a2cee7f0484 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.h
+++ b/drivers/net/wireless/iwlwifi/dvm/power.h
@@ -28,7 +28,7 @@
 #ifndef __iwl_power_setting_h__
 #define __iwl_power_setting_h__
 
-#include "iwl-commands.h"
+#include "commands.h"
 
 struct iwl_power_mgr {
 	struct iwl_powertable_cmd sleep_cmd;
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c b/drivers/net/wireless/iwlwifi/dvm/rs.c
index 8cebd7c363f..6fddd2785e6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.c
@@ -35,10 +35,8 @@
 
 #include <linux/workqueue.h>
 
-#include "iwl-dev.h"
-#include "iwl-agn.h"
-#include "iwl-op-mode.h"
-#include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
 
 #define RS_NAME "iwl-agn-rs"
 
@@ -819,7 +817,7 @@ static u32 rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
 
 		if (num_of_ant(tbl->ant_type) > 1)
 			tbl->ant_type =
-			    first_antenna(priv->hw_params.valid_tx_ant);
+			    first_antenna(priv->eeprom_data->valid_tx_ant);
 
 		tbl->is_ht40 = 0;
 		tbl->is_SGI = 0;
@@ -1447,7 +1445,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 	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 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	int ret = 0;
 	u8 update_search_tbl_counter = 0;
@@ -1465,7 +1463,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 	case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
 		/* avoid antenna B and MIMO */
 		valid_tx_ant =
-			first_antenna(priv->hw_params.valid_tx_ant);
+			first_antenna(priv->eeprom_data->valid_tx_ant);
 		if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2 &&
 		    tbl->action != IWL_LEGACY_SWITCH_SISO)
 			tbl->action = IWL_LEGACY_SWITCH_SISO;
@@ -1489,7 +1487,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 		else if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2)
 			tbl->action = IWL_LEGACY_SWITCH_SISO;
 		valid_tx_ant =
-			first_antenna(priv->hw_params.valid_tx_ant);
+			first_antenna(priv->eeprom_data->valid_tx_ant);
 	}
 
 	start_action = tbl->action;
@@ -1623,7 +1621,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	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 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	u8 update_search_tbl_counter = 0;
 	int ret;
@@ -1641,7 +1639,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
 		/* avoid antenna B and MIMO */
 		valid_tx_ant =
-			first_antenna(priv->hw_params.valid_tx_ant);
+			first_antenna(priv->eeprom_data->valid_tx_ant);
 		if (tbl->action != IWL_SISO_SWITCH_ANTENNA1)
 			tbl->action = IWL_SISO_SWITCH_ANTENNA1;
 		break;
@@ -1659,7 +1657,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	/* configure as 1x1 if bt full concurrency */
 	if (priv->bt_full_concurrent) {
 		valid_tx_ant =
-			first_antenna(priv->hw_params.valid_tx_ant);
+			first_antenna(priv->eeprom_data->valid_tx_ant);
 		if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2)
 			tbl->action = IWL_SISO_SWITCH_ANTENNA1;
 	}
@@ -1795,7 +1793,7 @@ static int rs_move_mimo2_to_other(struct iwl_priv *priv,
 	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 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	u8 update_search_tbl_counter = 0;
 	int ret;
@@ -1965,7 +1963,7 @@ static int rs_move_mimo3_to_other(struct iwl_priv *priv,
 	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 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 	u8 tx_chains_num = priv->hw_params.tx_chains_num;
 	int ret;
 	u8 update_search_tbl_counter = 0;
@@ -2699,7 +2697,7 @@ static void rs_initialize_lq(struct iwl_priv *priv,
 
 	i = lq_sta->last_txrate_idx;
 
-	valid_tx_ant = priv->hw_params.valid_tx_ant;
+	valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 
 	if (!lq_sta->search_better_tbl)
 		active_tbl = lq_sta->active_tbl;
@@ -2893,15 +2891,15 @@ void iwl_rs_rate_init(struct iwl_priv *priv, struct ieee80211_sta *sta, u8 sta_i
 
 	/* These values will be overridden later */
 	lq_sta->lq.general_params.single_stream_ant_msk =
-		first_antenna(priv->hw_params.valid_tx_ant);
+		first_antenna(priv->eeprom_data->valid_tx_ant);
 	lq_sta->lq.general_params.dual_stream_ant_msk =
-		priv->hw_params.valid_tx_ant &
-		~first_antenna(priv->hw_params.valid_tx_ant);
+		priv->eeprom_data->valid_tx_ant &
+		~first_antenna(priv->eeprom_data->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 (num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+	} else if (num_of_ant(priv->eeprom_data->valid_tx_ant) == 2) {
 		lq_sta->lq.general_params.dual_stream_ant_msk =
-			priv->hw_params.valid_tx_ant;
+			priv->eeprom_data->valid_tx_ant;
 	}
 
 	/* as default allow aggregation for all tids */
@@ -2947,7 +2945,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 	if (priv && priv->bt_full_concurrent) {
 		/* 1x1 only */
 		tbl_type.ant_type =
-			first_antenna(priv->hw_params.valid_tx_ant);
+			first_antenna(priv->eeprom_data->valid_tx_ant);
 	}
 
 	/* How many times should we repeat the initial rate? */
@@ -2979,7 +2977,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 		if (priv->bt_full_concurrent)
 			valid_tx_ant = ANT_A;
 		else
-			valid_tx_ant = priv->hw_params.valid_tx_ant;
+			valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 	}
 
 	/* Fill rest of rate table */
@@ -3013,7 +3011,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
 		if (priv && priv->bt_full_concurrent) {
 			/* 1x1 only */
 			tbl_type.ant_type =
-			    first_antenna(priv->hw_params.valid_tx_ant);
+			    first_antenna(priv->eeprom_data->valid_tx_ant);
 		}
 
 		/* Indicate to uCode which entries might be MIMO.
@@ -3100,7 +3098,7 @@ static void rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
 	u8 ant_sel_tx;
 
 	priv = lq_sta->drv;
-	valid_tx_ant = priv->hw_params.valid_tx_ant;
+	valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 	if (lq_sta->dbg_fixed_rate) {
 		ant_sel_tx =
 		  ((lq_sta->dbg_fixed_rate & RATE_MCS_ANT_ABC_MSK)
@@ -3171,9 +3169,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	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" : "");
+	    (priv->eeprom_data->valid_tx_ant & ANT_A) ? "ANT_A," : "",
+	    (priv->eeprom_data->valid_tx_ant & ANT_B) ? "ANT_B," : "",
+	    (priv->eeprom_data->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)) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h b/drivers/net/wireless/iwlwifi/dvm/rs.h
index 82d02e1ae89..ad3aea8f626 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.h
@@ -29,9 +29,10 @@
 
 #include <net/mac80211.h>
 
-#include "iwl-commands.h"
 #include "iwl-config.h"
 
+#include "commands.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. */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c b/drivers/net/wireless/iwlwifi/dvm/rx.c
index 403de96f974..fee5cffa166 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rx.c
@@ -32,12 +32,10 @@
 #include <linux/sched.h>
 #include <net/mac80211.h>
 #include <asm/unaligned.h>
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-agn-calib.h"
-#include "iwl-agn.h"
-#include "iwl-modparams.h"
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
 
 #define IWL_CMD_ENTRY(x) [x] = #x
 
@@ -90,7 +88,6 @@ const char *iwl_dvm_cmd_strings[REPLY_MAX] = {
 	IWL_CMD_ENTRY(REPLY_PHY_CALIBRATION_CMD),
 	IWL_CMD_ENTRY(REPLY_RX_PHY_CMD),
 	IWL_CMD_ENTRY(REPLY_RX_MPDU_CMD),
-	IWL_CMD_ENTRY(REPLY_RX),
 	IWL_CMD_ENTRY(REPLY_COMPRESSED_BA),
 	IWL_CMD_ENTRY(CALIBRATION_CFG_CMD),
 	IWL_CMD_ENTRY(CALIBRATION_RES_NOTIFICATION),
@@ -897,8 +894,7 @@ static int iwlagn_calc_rssi(struct iwl_priv *priv,
 	return max_rssi - agc - IWLAGN_RSSI_OFFSET;
 }
 
-/* Called for REPLY_RX (legacy ABG frames), or
- * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
+/* Called for REPLY_RX_MPDU_CMD */
 static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
 			    struct iwl_rx_cmd_buffer *rxb,
 			    struct iwl_device_cmd *cmd)
@@ -913,37 +909,17 @@ static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
 	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->data;
-		header = (struct ieee80211_hdr *)(pkt->data + sizeof(*phy_res)
-				+ phy_res->cfg_phy_cnt);
-
-		len = le16_to_cpu(phy_res->byte_count);
-		rx_pkt_status = *(__le32 *)(pkt->data + sizeof(*phy_res) +
-				phy_res->cfg_phy_cnt + len);
-		ampdu_status = le32_to_cpu(rx_pkt_status);
-	} else {
-		if (!priv->last_phy_res_valid) {
-			IWL_ERR(priv, "MPDU frame without cached PHY data\n");
-			return 0;
-		}
-		phy_res = &priv->last_phy_res;
-		amsdu = (struct iwl_rx_mpdu_res_start *)pkt->data;
-		header = (struct ieee80211_hdr *)(pkt->data + sizeof(*amsdu));
-		len = le16_to_cpu(amsdu->byte_count);
-		rx_pkt_status = *(__le32 *)(pkt->data + sizeof(*amsdu) + len);
-		ampdu_status = iwlagn_translate_rx_status(priv,
-						le32_to_cpu(rx_pkt_status));
+	if (!priv->last_phy_res_valid) {
+		IWL_ERR(priv, "MPDU frame without cached PHY data\n");
+		return 0;
 	}
+	phy_res = &priv->last_phy_res;
+	amsdu = (struct iwl_rx_mpdu_res_start *)pkt->data;
+	header = (struct ieee80211_hdr *)(pkt->data + sizeof(*amsdu));
+	len = le16_to_cpu(amsdu->byte_count);
+	rx_pkt_status = *(__le32 *)(pkt->data + 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",
@@ -1012,6 +988,8 @@ static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
 		rx_status.flag |= RX_FLAG_40MHZ;
 	if (rate_n_flags & RATE_MCS_SGI_MSK)
 		rx_status.flag |= RX_FLAG_SHORT_GI;
+	if (rate_n_flags & RATE_MCS_GF_MSK)
+		rx_status.flag |= RX_FLAG_HT_GF;
 
 	iwlagn_pass_packet_to_mac80211(priv, header, len, ampdu_status,
 				    rxb, &rx_status);
@@ -1124,8 +1102,6 @@ int iwl_rx_dispatch(struct iwl_op_mode *op_mode, struct iwl_rx_cmd_buffer *rxb,
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
-	void (*pre_rx_handler)(struct iwl_priv *,
-			       struct iwl_rx_cmd_buffer *);
 	int err = 0;
 
 	/*
@@ -1135,19 +1111,19 @@ int iwl_rx_dispatch(struct iwl_op_mode *op_mode, struct iwl_rx_cmd_buffer *rxb,
 	 */
 	iwl_notification_wait_notify(&priv->notif_wait, pkt);
 
-	/* RX data may be forwarded to userspace (using pre_rx_handler) in one
-	 * of two cases: the first, that the user owns the uCode through
-	 * testmode - in such case the pre_rx_handler is set and no further
-	 * processing takes place. The other case is when the user want to
-	 * monitor the rx w/o affecting the regular flow - the pre_rx_handler
-	 * will be set but the ownership flag != IWL_OWNERSHIP_TM and the flow
+#ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
+	/*
+	 * RX data may be forwarded to userspace in one
+	 * of two cases: the user owns the fw through testmode or when
+	 * the user requested to monitor the rx w/o affecting the regular flow.
+	 * In these cases the iwl_test object will handle forwarding the rx
+	 * data to user space.
+	 * Note that if the ownership flag != IWL_OWNERSHIP_TM the flow
 	 * continues.
-	 * We need to use ACCESS_ONCE to prevent a case where the handler
-	 * changes between the check and the call.
 	 */
-	pre_rx_handler = ACCESS_ONCE(priv->pre_rx_handler);
-	if (pre_rx_handler)
-		pre_rx_handler(priv, rxb);
+	iwl_test_rx(&priv->tst, rxb);
+#endif
+
 	if (priv->ucode_owner != IWL_OWNERSHIP_TM) {
 		/* Based on type of command response or notification,
 		 *   handle those that need handling via function in
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c b/drivers/net/wireless/iwlwifi/dvm/rxon.c
index 0a3aa7c8300..10896393e5a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rxon.c
@@ -25,11 +25,11 @@
  *****************************************************************************/
 
 #include <linux/etherdevice.h>
-#include "iwl-dev.h"
-#include "iwl-agn.h"
-#include "iwl-agn-calib.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
+#include "calib.h"
 
 /*
  * initialize rxon structure with default values from eeprom
@@ -37,8 +37,6 @@
 void iwl_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) {
@@ -80,14 +78,8 @@ void iwl_connection_init_rx_config(struct iwl_priv *priv,
 		ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
 #endif
 
-	ch_info = iwl_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;
+	ctx->staging.channel = cpu_to_le16(priv->hw->conf.channel->hw_value);
+	priv->band = priv->hw->conf.channel->band;
 
 	iwl_set_flags_for_band(priv, ctx, priv->band, ctx->vif);
 
@@ -175,7 +167,8 @@ static int iwlagn_disconn_pan(struct iwl_priv *priv,
 	return ret;
 }
 
-void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+static void iwlagn_update_qos(struct iwl_priv *priv,
+			      struct iwl_rxon_context *ctx)
 {
 	int ret;
 
@@ -202,8 +195,8 @@ void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 		IWL_DEBUG_QUIET_RFKILL(priv, "Failed to update QoS\n");
 }
 
-int iwlagn_update_beacon(struct iwl_priv *priv,
-			 struct ieee80211_vif *vif)
+static int iwlagn_update_beacon(struct iwl_priv *priv,
+				struct ieee80211_vif *vif)
 {
 	lockdep_assert_held(&priv->mutex);
 
@@ -215,7 +208,7 @@ int iwlagn_update_beacon(struct iwl_priv *priv,
 }
 
 static int iwlagn_send_rxon_assoc(struct iwl_priv *priv,
-			   struct iwl_rxon_context *ctx)
+				  struct iwl_rxon_context *ctx)
 {
 	int ret = 0;
 	struct iwl_rxon_assoc_cmd rxon_assoc;
@@ -427,10 +420,10 @@ static int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
 		return -EINVAL;
 	}
 
-	if (tx_power > priv->tx_power_device_lmt) {
+	if (tx_power > DIV_ROUND_UP(priv->eeprom_data->max_tx_pwr_half_dbm, 2)) {
 		IWL_WARN(priv,
 			"Requested user TXPOWER %d above upper limit %d.\n",
-			 tx_power, priv->tx_power_device_lmt);
+			 tx_power, priv->eeprom_data->max_tx_pwr_half_dbm);
 		return -EINVAL;
 	}
 
@@ -863,8 +856,8 @@ static int iwl_check_rxon_cmd(struct iwl_priv *priv,
  * 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_full_rxon_required(struct iwl_priv *priv,
-			   struct iwl_rxon_context *ctx)
+static int iwl_full_rxon_required(struct iwl_priv *priv,
+				  struct iwl_rxon_context *ctx)
 {
 	const struct iwl_rxon_cmd *staging = &ctx->staging;
 	const struct iwl_rxon_cmd *active = &ctx->active;
@@ -1189,7 +1182,6 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 	struct iwl_rxon_context *ctx;
 	struct ieee80211_conf *conf = &hw->conf;
 	struct ieee80211_channel *channel = conf->channel;
-	const struct iwl_channel_info *ch_info;
 	int ret = 0;
 
 	IWL_DEBUG_MAC80211(priv, "enter: changed %#x\n", changed);
@@ -1223,14 +1215,6 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		ch_info = iwl_get_channel_info(priv, channel->band,
-					       channel->hw_value);
-		if (!is_channel_valid(ch_info)) {
-			IWL_DEBUG_MAC80211(priv, "leave - invalid channel\n");
-			ret = -EINVAL;
-			goto out;
-		}
-
 		for_each_context(priv, ctx) {
 			/* Configure HT40 channels */
 			if (ctx->ht.enabled != conf_is_ht(conf))
@@ -1294,9 +1278,9 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 	return ret;
 }
 
-void iwlagn_check_needed_chains(struct iwl_priv *priv,
-				struct iwl_rxon_context *ctx,
-				struct ieee80211_bss_conf *bss_conf)
+static void iwlagn_check_needed_chains(struct iwl_priv *priv,
+				       struct iwl_rxon_context *ctx,
+				       struct ieee80211_bss_conf *bss_conf)
 {
 	struct ieee80211_vif *vif = ctx->vif;
 	struct iwl_rxon_context *tmp;
@@ -1388,7 +1372,7 @@ void iwlagn_check_needed_chains(struct iwl_priv *priv,
 	ht_conf->single_chain_sufficient = !need_multiple;
 }
 
-void iwlagn_chain_noise_reset(struct iwl_priv *priv)
+static void iwlagn_chain_noise_reset(struct iwl_priv *priv)
 {
 	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
 	int ret;
@@ -1463,7 +1447,7 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 
 	if (changes & BSS_CHANGED_ASSOC) {
 		if (bss_conf->assoc) {
-			priv->timestamp = bss_conf->last_tsf;
+			priv->timestamp = bss_conf->sync_tsf;
 			ctx->staging.filter_flags |= RXON_FILTER_ASSOC_MSK;
 		} else {
 			/*
diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/dvm/scan.c
index 031d8e21f82..e3467fa8689 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scan.c
+++ b/drivers/net/wireless/iwlwifi/dvm/scan.c
@@ -30,11 +30,8 @@
 #include <linux/etherdevice.h>
 #include <net/mac80211.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-trans.h"
+#include "dev.h"
+#include "agn.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
@@ -54,6 +51,9 @@
 #define IWL_CHANNEL_TUNE_TIME       5
 #define MAX_SCAN_CHANNEL	    50
 
+/* For reset radio, need minimal dwell time only */
+#define IWL_RADIO_RESET_DWELL_TIME	5
+
 static int iwl_send_scan_abort(struct iwl_priv *priv)
 {
 	int ret;
@@ -67,7 +67,6 @@ static int iwl_send_scan_abort(struct iwl_priv *priv)
 	 * 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))
 		return -EIO;
@@ -101,11 +100,8 @@ static void iwl_complete_scan(struct iwl_priv *priv, bool aborted)
 		ieee80211_scan_completed(priv->hw, aborted);
 	}
 
-	if (priv->scan_type == IWL_SCAN_ROC) {
-		ieee80211_remain_on_channel_expired(priv->hw);
-		priv->hw_roc_channel = NULL;
-		schedule_delayed_work(&priv->hw_roc_disable_work, 10 * HZ);
-	}
+	if (priv->scan_type == IWL_SCAN_ROC)
+		iwl_scan_roc_expired(priv);
 
 	priv->scan_type = IWL_SCAN_NORMAL;
 	priv->scan_vif = NULL;
@@ -134,11 +130,8 @@ static void iwl_process_scan_complete(struct iwl_priv *priv)
 		goto out_settings;
 	}
 
-	if (priv->scan_type == IWL_SCAN_ROC) {
-		ieee80211_remain_on_channel_expired(priv->hw);
-		priv->hw_roc_channel = NULL;
-		schedule_delayed_work(&priv->hw_roc_disable_work, 10 * HZ);
-	}
+	if (priv->scan_type == IWL_SCAN_ROC)
+		iwl_scan_roc_expired(priv);
 
 	if (priv->scan_type != IWL_SCAN_NORMAL && !aborted) {
 		int err;
@@ -403,15 +396,21 @@ static u16 iwl_get_active_dwell_time(struct iwl_priv *priv,
 static u16 iwl_limit_dwell(struct iwl_priv *priv, u16 dwell_time)
 {
 	struct iwl_rxon_context *ctx;
+	int limits[NUM_IWL_RXON_CTX] = {};
+	int n_active = 0;
+	u16 limit;
+
+	BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
 
 	/*
 	 * If we're associated, we clamp the dwell time 98%
-	 * of the smallest beacon interval (minus 2 * channel
-	 * tune time)
+	 * of the beacon interval (minus 2 * channel tune time)
+	 * If both contexts are active, we have to restrict to
+	 * 1/2 of the minimum of them, because they might be in
+	 * lock-step with the time inbetween only half of what
+	 * time we'd have in each of them.
 	 */
 	for_each_context(priv, ctx) {
-		u16 value;
-
 		switch (ctx->staging.dev_type) {
 		case RXON_DEV_TYPE_P2P:
 			/* no timing constraints */
@@ -431,14 +430,25 @@ static u16 iwl_limit_dwell(struct iwl_priv *priv, u16 dwell_time)
 			break;
 		}
 
-		value = ctx->beacon_int;
-		if (!value)
-			value = IWL_PASSIVE_DWELL_BASE;
-		value = (value * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
-		dwell_time = min(value, dwell_time);
+		limits[n_active++] = ctx->beacon_int ?: IWL_PASSIVE_DWELL_BASE;
 	}
 
-	return dwell_time;
+	switch (n_active) {
+	case 0:
+		return dwell_time;
+	case 2:
+		limit = (limits[1] * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
+		limit /= 2;
+		dwell_time = min(limit, dwell_time);
+		/* fall through to limit further */
+	case 1:
+		limit = (limits[0] * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
+		limit /= n_active;
+		return min(limit, dwell_time);
+	default:
+		WARN_ON_ONCE(1);
+		return dwell_time;
+	}
 }
 
 static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
@@ -453,27 +463,17 @@ static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
 
 /* Return valid, unused, channel for a passive scan to reset the RF */
 static u8 iwl_get_single_channel_number(struct iwl_priv *priv,
-				 enum ieee80211_band band)
+					enum ieee80211_band band)
 {
-	const struct iwl_channel_info *ch_info;
-	int i;
-	u8 channel = 0;
-	u8 min, max;
+	struct ieee80211_supported_band *sband = priv->hw->wiphy->bands[band];
 	struct iwl_rxon_context *ctx;
+	int i;
 
-	if (band == IEEE80211_BAND_5GHZ) {
-		min = 14;
-		max = priv->channel_count;
-	} else {
-		min = 0;
-		max = 14;
-	}
-
-	for (i = min; i < max; i++) {
+	for (i = 0; i < sband->n_channels; i++) {
 		bool busy = false;
 
 		for_each_context(priv, ctx) {
-			busy = priv->channel_info[i].channel ==
+			busy = sband->channels[i].hw_value ==
 				le16_to_cpu(ctx->staging.channel);
 			if (busy)
 				break;
@@ -482,54 +482,46 @@ static u8 iwl_get_single_channel_number(struct iwl_priv *priv,
 		if (busy)
 			continue;
 
-		channel = priv->channel_info[i].channel;
-		ch_info = iwl_get_channel_info(priv, band, channel);
-		if (is_channel_valid(ch_info))
-			break;
+		if (!(sband->channels[i].flags & IEEE80211_CHAN_DISABLED))
+			return sband->channels[i].hw_value;
 	}
 
-	return channel;
+	return 0;
 }
 
-static int iwl_get_single_channel_for_scan(struct iwl_priv *priv,
-					   struct ieee80211_vif *vif,
-					   enum ieee80211_band band,
-					   struct iwl_scan_channel *scan_ch)
+static int iwl_get_channel_for_reset_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;
+	u16 channel;
 
 	sband = iwl_get_hw_mode(priv, band);
 	if (!sband) {
 		IWL_ERR(priv, "invalid band\n");
-		return added;
+		return 0;
 	}
 
-	active_dwell = iwl_get_active_dwell_time(priv, band, 0);
-	passive_dwell = iwl_get_passive_dwell_time(priv, band);
-
-	if (passive_dwell <= active_dwell)
-		passive_dwell = active_dwell + 1;
-
 	channel = iwl_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);
+		scan_ch->active_dwell =
+			cpu_to_le16(IWL_RADIO_RESET_DWELL_TIME);
+		scan_ch->passive_dwell =
+			cpu_to_le16(IWL_RADIO_RESET_DWELL_TIME);
 		/* 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;
+		return 1;
+	}
+
+	IWL_ERR(priv, "no valid channel found\n");
+	return 0;
 }
 
 static int iwl_get_channels_for_scan(struct iwl_priv *priv,
@@ -540,7 +532,6 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
 {
 	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;
@@ -565,16 +556,7 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
 		channel = chan->hw_value;
 		scan_ch->channel = cpu_to_le16(channel);
 
-		ch_info = iwl_get_channel_info(priv, band, channel);
-		if (!is_channel_valid(ch_info)) {
-			IWL_DEBUG_SCAN(priv,
-				       "Channel %d is INVALID for this band.\n",
-				       channel);
-			continue;
-		}
-
-		if (!is_active || is_channel_passive(ch_info) ||
-		    (chan->flags & IEEE80211_CHAN_PASSIVE_SCAN))
+		if (!is_active || (chan->flags & IEEE80211_CHAN_PASSIVE_SCAN))
 			scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
 		else
 			scan_ch->type = SCAN_CHANNEL_TYPE_ACTIVE;
@@ -678,12 +660,12 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	u16 rx_chain = 0;
 	enum ieee80211_band band;
 	u8 n_probes = 0;
-	u8 rx_ant = priv->hw_params.valid_rx_ant;
+	u8 rx_ant = priv->eeprom_data->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;
+	u8 scan_tx_antennas = priv->eeprom_data->valid_tx_ant;
 	int ret;
 	int scan_cmd_size = sizeof(struct iwl_scan_cmd) +
 			    MAX_SCAN_CHANNEL * sizeof(struct iwl_scan_channel) +
@@ -755,6 +737,12 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	switch (priv->scan_type) {
 	case IWL_SCAN_RADIO_RESET:
 		IWL_DEBUG_SCAN(priv, "Start internal passive scan.\n");
+		/*
+		 * Override quiet time as firmware checks that active
+		 * dwell is >= quiet; since we use passive scan it'll
+		 * not actually be used.
+		 */
+		scan->quiet_time = cpu_to_le16(IWL_RADIO_RESET_DWELL_TIME);
 		break;
 	case IWL_SCAN_NORMAL:
 		if (priv->scan_request->n_ssids) {
@@ -893,7 +881,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 	/* MIMO is not used here, but value is required */
 	rx_chain |=
-		priv->hw_params.valid_rx_ant << RXON_RX_CHAIN_VALID_POS;
+		priv->eeprom_data->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;
@@ -928,7 +916,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	switch (priv->scan_type) {
 	case IWL_SCAN_RADIO_RESET:
 		scan->channel_count =
-			iwl_get_single_channel_for_scan(priv, vif, band,
+			iwl_get_channel_for_reset_scan(priv, vif, band,
 				(void *)&scan->data[cmd_len]);
 		break;
 	case IWL_SCAN_NORMAL:
@@ -994,8 +982,10 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 	set_bit(STATUS_SCAN_HW, &priv->status);
 
 	ret = iwlagn_set_pan_params(priv);
-	if (ret)
+	if (ret) {
+		clear_bit(STATUS_SCAN_HW, &priv->status);
 		return ret;
+	}
 
 	ret = iwl_dvm_send_cmd(priv, &cmd);
 	if (ret) {
@@ -1008,7 +998,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 void iwl_init_scan_params(struct iwl_priv *priv)
 {
-	u8 ant_idx = fls(priv->hw_params.valid_tx_ant) - 1;
+	u8 ant_idx = fls(priv->eeprom_data->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])
@@ -1158,3 +1148,40 @@ void iwl_cancel_scan_deferred_work(struct iwl_priv *priv)
 		mutex_unlock(&priv->mutex);
 	}
 }
+
+void iwl_scan_roc_expired(struct iwl_priv *priv)
+{
+	/*
+	 * The status bit should be set here, to prevent a race
+	 * where the atomic_read returns 1, but before the execution continues
+	 * iwl_scan_offchannel_skb_status() checks if the status bit is set
+	 */
+	set_bit(STATUS_SCAN_ROC_EXPIRED, &priv->status);
+
+	if (atomic_read(&priv->num_aux_in_flight) == 0) {
+		ieee80211_remain_on_channel_expired(priv->hw);
+		priv->hw_roc_channel = NULL;
+		schedule_delayed_work(&priv->hw_roc_disable_work,
+				      10 * HZ);
+
+		clear_bit(STATUS_SCAN_ROC_EXPIRED, &priv->status);
+	} else {
+		IWL_DEBUG_SCAN(priv, "ROC done with %d frames in aux\n",
+			       atomic_read(&priv->num_aux_in_flight));
+	}
+}
+
+void iwl_scan_offchannel_skb(struct iwl_priv *priv)
+{
+	WARN_ON(!priv->hw_roc_start_notified);
+	atomic_inc(&priv->num_aux_in_flight);
+}
+
+void iwl_scan_offchannel_skb_status(struct iwl_priv *priv)
+{
+	if (atomic_dec_return(&priv->num_aux_in_flight) == 0 &&
+	    test_bit(STATUS_SCAN_ROC_EXPIRED, &priv->status)) {
+		IWL_DEBUG_SCAN(priv, "0 aux frames. Calling ROC expired\n");
+		iwl_scan_roc_expired(priv);
+	}
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c b/drivers/net/wireless/iwlwifi/dvm/sta.c
index eb6a8eaf42f..b29b798f755 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
+++ b/drivers/net/wireless/iwlwifi/dvm/sta.c
@@ -28,10 +28,9 @@
  *****************************************************************************/
 #include <linux/etherdevice.h>
 #include <net/mac80211.h>
-
-#include "iwl-dev.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
+#include "dev.h"
+#include "agn.h"
 
 const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 
@@ -171,26 +170,6 @@ int iwl_send_add_sta(struct iwl_priv *priv,
 	return cmd.handler_status;
 }
 
-static bool iwl_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_get_channel_info(priv, band, channel);
-	if (!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_is_ht40_tx_allowed(struct iwl_priv *priv,
 			    struct iwl_rxon_context *ctx,
 			    struct ieee80211_sta_ht_cap *ht_cap)
@@ -198,21 +177,25 @@ bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
 	if (!ctx->ht.enabled || !ctx->ht.is_40mhz)
 		return false;
 
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (priv->disable_ht40)
+		return false;
+#endif
+
 	/*
-	 * We do not check for IEEE80211_HT_CAP_SUP_WIDTH_20_40
-	 * the bit will not set if it is pure 40MHz case
+	 * Remainder of this function checks ht_cap, but if it's
+	 * NULL then we can do HT40 (special case for RXON)
 	 */
-	if (ht_cap && !ht_cap->ht_supported)
+	if (!ht_cap)
+		return true;
+
+	if (!ht_cap->ht_supported)
 		return false;
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (priv->disable_ht40)
+	if (!(ht_cap->cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
 		return false;
-#endif
 
-	return iwl_is_channel_extension(priv, priv->band,
-			le16_to_cpu(ctx->staging.channel),
-			ctx->ht.extension_chan_offset);
+	return true;
 }
 
 static void iwl_sta_calc_ht_flags(struct iwl_priv *priv,
@@ -236,6 +219,7 @@ static void iwl_sta_calc_ht_flags(struct iwl_priv *priv,
 	mimo_ps_mode = (sta_ht_inf->cap & IEEE80211_HT_CAP_SM_PS) >> 2;
 
 	IWL_DEBUG_INFO(priv, "STA %pM SM PS mode: %s\n",
+			sta->addr,
 			(mimo_ps_mode == WLAN_HT_CAP_SM_PS_STATIC) ?
 			"static" :
 			(mimo_ps_mode == WLAN_HT_CAP_SM_PS_DYNAMIC) ?
@@ -649,23 +633,23 @@ static void iwl_sta_fill_lq(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
 	if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
 		rate_flags |= RATE_MCS_CCK_MSK;
 
-	rate_flags |= first_antenna(priv->hw_params.valid_tx_ant) <<
+	rate_flags |= first_antenna(priv->eeprom_data->valid_tx_ant) <<
 				RATE_MCS_ANT_POS;
 	rate_n_flags = iwl_hw_set_rate_n_flags(iwl_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 =
-			first_antenna(priv->hw_params.valid_tx_ant);
+			first_antenna(priv->eeprom_data->valid_tx_ant);
 
 	link_cmd->general_params.dual_stream_ant_msk =
-		priv->hw_params.valid_tx_ant &
-		~first_antenna(priv->hw_params.valid_tx_ant);
+		priv->eeprom_data->valid_tx_ant &
+		~first_antenna(priv->eeprom_data->valid_tx_ant);
 	if (!link_cmd->general_params.dual_stream_ant_msk) {
 		link_cmd->general_params.dual_stream_ant_msk = ANT_AB;
-	} else if (num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+	} else if (num_of_ant(priv->eeprom_data->valid_tx_ant) == 2) {
 		link_cmd->general_params.dual_stream_ant_msk =
-			priv->hw_params.valid_tx_ant;
+			priv->eeprom_data->valid_tx_ant;
 	}
 
 	link_cmd->agg_params.agg_dis_start_th =
diff --git a/drivers/net/wireless/iwlwifi/dvm/testmode.c b/drivers/net/wireless/iwlwifi/dvm/testmode.c
new file mode 100644
index 00000000000..57b918ce3b5
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/dvm/testmode.c
@@ -0,0 +1,471 @@
+/******************************************************************************
+ *
+ * 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) 2010 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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) 2010 - 2012 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/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/dma-mapping.h>
+#include <net/net_namespace.h>
+#include <linux/netdevice.h>
+#include <net/cfg80211.h>
+#include <net/mac80211.h>
+#include <net/netlink.h>
+
+#include "iwl-debug.h"
+#include "iwl-trans.h"
+#include "dev.h"
+#include "agn.h"
+#include "iwl-test.h"
+#include "iwl-testmode.h"
+
+static int iwl_testmode_send_cmd(struct iwl_op_mode *op_mode,
+				 struct iwl_host_cmd *cmd)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	return iwl_dvm_send_cmd(priv, cmd);
+}
+
+static bool iwl_testmode_valid_hw_addr(u32 addr)
+{
+	if (iwlagn_hw_valid_rtc_data_addr(addr))
+		return true;
+
+	if (IWLAGN_RTC_INST_LOWER_BOUND <= addr &&
+	    addr < IWLAGN_RTC_INST_UPPER_BOUND)
+		return true;
+
+	return false;
+}
+
+static u32 iwl_testmode_get_fw_ver(struct iwl_op_mode *op_mode)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	return priv->fw->ucode_ver;
+}
+
+static struct sk_buff*
+iwl_testmode_alloc_reply(struct iwl_op_mode *op_mode, int len)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	return cfg80211_testmode_alloc_reply_skb(priv->hw->wiphy, len);
+}
+
+static int iwl_testmode_reply(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+{
+	return cfg80211_testmode_reply(skb);
+}
+
+static struct sk_buff *iwl_testmode_alloc_event(struct iwl_op_mode *op_mode,
+						int len)
+{
+	struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+	return cfg80211_testmode_alloc_event_skb(priv->hw->wiphy, len,
+						 GFP_ATOMIC);
+}
+
+static void iwl_testmode_event(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+{
+	return cfg80211_testmode_event(skb, GFP_ATOMIC);
+}
+
+static struct iwl_test_ops tst_ops = {
+	.send_cmd = iwl_testmode_send_cmd,
+	.valid_hw_addr = iwl_testmode_valid_hw_addr,
+	.get_fw_ver = iwl_testmode_get_fw_ver,
+	.alloc_reply = iwl_testmode_alloc_reply,
+	.reply = iwl_testmode_reply,
+	.alloc_event = iwl_testmode_alloc_event,
+	.event = iwl_testmode_event,
+};
+
+void iwl_testmode_init(struct iwl_priv *priv)
+{
+	iwl_test_init(&priv->tst, priv->trans, &tst_ops);
+}
+
+void iwl_testmode_free(struct iwl_priv *priv)
+{
+	iwl_test_free(&priv->tst);
+}
+
+static int iwl_testmode_cfg_init_calib(struct iwl_priv *priv)
+{
+	struct iwl_notification_wait calib_wait;
+	static const u8 calib_complete[] = {
+		CALIBRATION_COMPLETE_NOTIFICATION
+	};
+	int ret;
+
+	iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
+				   calib_complete, ARRAY_SIZE(calib_complete),
+				   NULL, NULL);
+	ret = iwl_init_alive_start(priv);
+	if (ret) {
+		IWL_ERR(priv, "Fail init calibration: %d\n", ret);
+		goto cfg_init_calib_error;
+	}
+
+	ret = iwl_wait_notification(&priv->notif_wait, &calib_wait, 2 * HZ);
+	if (ret)
+		IWL_ERR(priv, "Error detecting"
+			" CALIBRATION_COMPLETE_NOTIFICATION: %d\n", ret);
+	return ret;
+
+cfg_init_calib_error:
+	iwl_remove_notification(&priv->notif_wait, &calib_wait);
+	return ret;
+}
+
+/*
+ * This function handles the user application commands for driver.
+ *
+ * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
+ * handlers respectively.
+ *
+ * If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
+ * value of the actual command execution is replied to the user application.
+ *
+ * If there's any message responding to the user space, IWL_TM_ATTR_SYNC_RSP
+ * is used for carry the message while IWL_TM_ATTR_COMMAND must set to
+ * IWL_TM_CMD_DEV2APP_SYNC_RSP.
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @tb: gnl message fields from the user space
+ */
+static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
+{
+	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+	struct iwl_trans *trans = priv->trans;
+	struct sk_buff *skb;
+	unsigned char *rsp_data_ptr = NULL;
+	int status = 0, rsp_data_len = 0;
+	u32 inst_size = 0, data_size = 0;
+	const struct fw_img *img;
+
+	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
+	case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
+		rsp_data_ptr = (unsigned char *)priv->cfg->name;
+		rsp_data_len = strlen(priv->cfg->name);
+		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+							rsp_data_len + 20);
+		if (!skb) {
+			IWL_ERR(priv, "Memory allocation fail\n");
+			return -ENOMEM;
+		}
+		if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+				IWL_TM_CMD_DEV2APP_SYNC_RSP) ||
+		    nla_put(skb, IWL_TM_ATTR_SYNC_RSP,
+			    rsp_data_len, rsp_data_ptr))
+			goto nla_put_failure;
+		status = cfg80211_testmode_reply(skb);
+		if (status < 0)
+			IWL_ERR(priv, "Error sending msg : %d\n", status);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
+		status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_INIT);
+		if (status)
+			IWL_ERR(priv, "Error loading init ucode: %d\n", status);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
+		iwl_testmode_cfg_init_calib(priv);
+		priv->ucode_loaded = false;
+		iwl_trans_stop_device(trans);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
+		status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_REGULAR);
+		if (status) {
+			IWL_ERR(priv,
+				"Error loading runtime ucode: %d\n", status);
+			break;
+		}
+		status = iwl_alive_start(priv);
+		if (status)
+			IWL_ERR(priv,
+				"Error starting the device: %d\n", status);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
+		iwl_scan_cancel_timeout(priv, 200);
+		priv->ucode_loaded = false;
+		iwl_trans_stop_device(trans);
+		status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_WOWLAN);
+		if (status) {
+			IWL_ERR(priv,
+				"Error loading WOWLAN ucode: %d\n", status);
+			break;
+		}
+		status = iwl_alive_start(priv);
+		if (status)
+			IWL_ERR(priv,
+				"Error starting the device: %d\n", status);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
+		if (priv->eeprom_blob) {
+			skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+				priv->eeprom_blob_size + 20);
+			if (!skb) {
+				IWL_ERR(priv, "Memory allocation fail\n");
+				return -ENOMEM;
+			}
+			if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+					IWL_TM_CMD_DEV2APP_EEPROM_RSP) ||
+			    nla_put(skb, IWL_TM_ATTR_EEPROM,
+				    priv->eeprom_blob_size,
+				    priv->eeprom_blob))
+				goto nla_put_failure;
+			status = cfg80211_testmode_reply(skb);
+			if (status < 0)
+				IWL_ERR(priv, "Error sending msg : %d\n",
+					status);
+		} else
+			return -ENODATA;
+		break;
+
+	case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
+		if (!tb[IWL_TM_ATTR_FIXRATE]) {
+			IWL_ERR(priv, "Missing fixrate setting\n");
+			return -ENOMSG;
+		}
+		priv->tm_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
+		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20 + 8);
+		if (!skb) {
+			IWL_ERR(priv, "Memory allocation fail\n");
+			return -ENOMEM;
+		}
+		if (!priv->ucode_loaded) {
+			IWL_ERR(priv, "No uCode has not been loaded\n");
+			return -EINVAL;
+		} else {
+			img = &priv->fw->img[priv->cur_ucode];
+			inst_size = img->sec[IWL_UCODE_SECTION_INST].len;
+			data_size = img->sec[IWL_UCODE_SECTION_DATA].len;
+		}
+		if (nla_put_u32(skb, IWL_TM_ATTR_FW_TYPE, priv->cur_ucode) ||
+		    nla_put_u32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size) ||
+		    nla_put_u32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size))
+			goto nla_put_failure;
+		status = cfg80211_testmode_reply(skb);
+		if (status < 0)
+			IWL_ERR(priv, "Error sending msg : %d\n", status);
+		break;
+
+	default:
+		IWL_ERR(priv, "Unknown testmode driver command ID\n");
+		return -ENOSYS;
+	}
+	return status;
+
+nla_put_failure:
+	kfree_skb(skb);
+	return -EMSGSIZE;
+}
+
+/*
+ * This function handles the user application switch ucode ownership.
+ *
+ * It retrieves the mandatory fields IWL_TM_ATTR_UCODE_OWNER and
+ * decide who the current owner of the uCode
+ *
+ * If the current owner is OWNERSHIP_TM, then the only host command
+ * can deliver to uCode is from testmode, all the other host commands
+ * will dropped.
+ *
+ * default driver is the owner of uCode in normal operational mode
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @tb: gnl message fields from the user space
+ */
+static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
+{
+	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+	u8 owner;
+
+	if (!tb[IWL_TM_ATTR_UCODE_OWNER]) {
+		IWL_ERR(priv, "Missing ucode owner\n");
+		return -ENOMSG;
+	}
+
+	owner = nla_get_u8(tb[IWL_TM_ATTR_UCODE_OWNER]);
+	if (owner == IWL_OWNERSHIP_DRIVER) {
+		priv->ucode_owner = owner;
+		iwl_test_enable_notifications(&priv->tst, false);
+	} else if (owner == IWL_OWNERSHIP_TM) {
+		priv->ucode_owner = owner;
+		iwl_test_enable_notifications(&priv->tst, true);
+	} else {
+		IWL_ERR(priv, "Invalid owner\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* The testmode gnl message handler that takes the gnl message from the
+ * user space and parses it per the policy iwl_testmode_gnl_msg_policy, then
+ * invoke the corresponding handlers.
+ *
+ * This function is invoked when there is user space application sending
+ * gnl message through the testmode tunnel NL80211_CMD_TESTMODE regulated
+ * by nl80211.
+ *
+ * It retrieves the mandatory field, IWL_TM_ATTR_COMMAND, before
+ * dispatching it to the corresponding handler.
+ *
+ * If IWL_TM_ATTR_COMMAND is missing, -ENOMSG is replied to user application;
+ * -ENOSYS is replied to the user application if the command is unknown;
+ * Otherwise, the command is dispatched to the respective handler.
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @data: pointer to user space message
+ * @len: length in byte of @data
+ */
+int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
+{
+	struct nlattr *tb[IWL_TM_ATTR_MAX];
+	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+	int result;
+
+	result = iwl_test_parse(&priv->tst, tb, data, len);
+	if (result)
+		return result;
+
+	/* in case multiple accesses to the device happens */
+	mutex_lock(&priv->mutex);
+	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
+	case IWL_TM_CMD_APP2DEV_UCODE:
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
+	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
+	case IWL_TM_CMD_APP2DEV_END_TRACE:
+	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ:
+	case IWL_TM_CMD_APP2DEV_NOTIFICATIONS:
+	case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
+	case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
+	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_WRITE:
+		result = iwl_test_handle_cmd(&priv->tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
+	case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
+	case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
+	case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
+	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
+	case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
+	case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
+	case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
+		IWL_DEBUG_INFO(priv, "testmode cmd to driver\n");
+		result = iwl_testmode_driver(hw, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_OWNERSHIP:
+		IWL_DEBUG_INFO(priv, "testmode change uCode ownership\n");
+		result = iwl_testmode_ownership(hw, tb);
+		break;
+
+	default:
+		IWL_ERR(priv, "Unknown testmode command\n");
+		result = -ENOSYS;
+		break;
+	}
+	mutex_unlock(&priv->mutex);
+
+	if (result)
+		IWL_ERR(priv, "Test cmd failed result=%d\n", result);
+	return result;
+}
+
+int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
+		      struct netlink_callback *cb,
+		      void *data, int len)
+{
+	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+	int result;
+	u32 cmd;
+
+	if (cb->args[3]) {
+		/* offset by 1 since commands start at 0 */
+		cmd = cb->args[3] - 1;
+	} else {
+		struct nlattr *tb[IWL_TM_ATTR_MAX];
+
+		result = iwl_test_parse(&priv->tst, tb, data, len);
+		if (result)
+			return result;
+
+		cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
+		cb->args[3] = cmd + 1;
+	}
+
+	/* in case multiple accesses to the device happens */
+	mutex_lock(&priv->mutex);
+	result = iwl_test_dump(&priv->tst, cmd, skb, cb);
+	mutex_unlock(&priv->mutex);
+	return result;
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tt.c b/drivers/net/wireless/iwlwifi/dvm/tt.c
index a5cfe0aceed..eb864433e59 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tt.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tt.c
@@ -31,17 +31,14 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/init.h>
-
 #include <net/mac80211.h>
-
-#include "iwl-agn.h"
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-commands.h"
-#include "iwl-debug.h"
-#include "iwl-agn-tt.h"
 #include "iwl-modparams.h"
+#include "iwl-debug.h"
+#include "agn.h"
+#include "dev.h"
+#include "commands.h"
+#include "tt.h"
 
 /* default Thermal Throttling transaction table
  * Current state   |         Throttling Down               |  Throttling Up
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tt.h b/drivers/net/wireless/iwlwifi/dvm/tt.h
index 86bbf47501c..44c7c8f30a2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tt.h
+++ b/drivers/net/wireless/iwlwifi/dvm/tt.h
@@ -28,7 +28,7 @@
 #ifndef __iwl_tt_setting_h__
 #define __iwl_tt_setting_h__
 
-#include "iwl-commands.h"
+#include "commands.h"
 
 #define IWL_ABSOLUTE_ZERO		0
 #define IWL_ABSOLUTE_MAX		0xFFFFFFFF
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index 3366e2e2f00..5971a23aa47 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -32,12 +32,11 @@
 #include <linux/init.h>
 #include <linux/sched.h>
 #include <linux/ieee80211.h>
-
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-agn-hw.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
+#include "iwl-agn-hw.h"
+#include "dev.h"
+#include "agn.h"
 
 static const u8 tid_to_ac[] = {
 	IEEE80211_AC_BE,
@@ -187,7 +186,8 @@ static void iwlagn_tx_cmd_build_rate(struct iwl_priv *priv,
 	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],
+		rate_idx = rate_lowest_index(
+				&priv->eeprom_data->bands[info->band],
 				info->control.sta);
 	/* For 5 GHZ band, remap mac80211 rate indices into driver indices */
 	if (info->band == IEEE80211_BAND_5GHZ)
@@ -207,10 +207,11 @@ static void iwlagn_tx_cmd_build_rate(struct iwl_priv *priv,
 	     priv->bt_full_concurrent) {
 		/* operated as 1x1 in full concurrency mode */
 		priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-				first_antenna(priv->hw_params.valid_tx_ant));
+				first_antenna(priv->eeprom_data->valid_tx_ant));
 	} else
-		priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-						priv->hw_params.valid_tx_ant);
+		priv->mgmt_tx_ant = iwl_toggle_tx_ant(
+					priv, priv->mgmt_tx_ant,
+					priv->eeprom_data->valid_tx_ant);
 	rate_flags |= iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
 
 	/* Set the rate in the TX cmd */
@@ -296,7 +297,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct iwl_station_priv *sta_priv = NULL;
 	struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-	struct iwl_device_cmd *dev_cmd = NULL;
+	struct iwl_device_cmd *dev_cmd;
 	struct iwl_tx_cmd *tx_cmd;
 	__le16 fc;
 	u8 hdr_len;
@@ -378,7 +379,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	if (info->flags & IEEE80211_TX_CTL_AMPDU)
 		is_agg = true;
 
-	dev_cmd = kmem_cache_alloc(iwl_tx_cmd_pool, GFP_ATOMIC);
+	dev_cmd = iwl_trans_alloc_tx_cmd(priv->trans);
 
 	if (unlikely(!dev_cmd))
 		goto drop_unlock_priv;
@@ -402,6 +403,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 
 	info->driver_data[0] = ctx;
 	info->driver_data[1] = dev_cmd;
+	/* From now on, we cannot access info->control */
 
 	spin_lock(&priv->sta_lock);
 
@@ -486,11 +488,14 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 	if (sta_priv && sta_priv->client && !is_agg)
 		atomic_inc(&sta_priv->pending_frames);
 
+	if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
+		iwl_scan_offchannel_skb(priv);
+
 	return 0;
 
 drop_unlock_sta:
 	if (dev_cmd)
-		kmem_cache_free(iwl_tx_cmd_pool, dev_cmd);
+		iwl_trans_free_tx_cmd(priv->trans, dev_cmd);
 	spin_unlock(&priv->sta_lock);
 drop_unlock_priv:
 	return -1;
@@ -597,7 +602,7 @@ turn_off:
 		 * time, or we hadn't time to drain the AC queues.
 		 */
 		if (agg_state == IWL_AGG_ON)
-			iwl_trans_tx_agg_disable(priv->trans, txq_id);
+			iwl_trans_txq_disable(priv->trans, txq_id);
 		else
 			IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
 					    agg_state);
@@ -686,9 +691,8 @@ int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
 
 	fifo = ctx->ac_to_fifo[tid_to_ac[tid]];
 
-	iwl_trans_tx_agg_setup(priv->trans, q, fifo,
-			       sta_priv->sta_id, tid,
-			       buf_size, ssn);
+	iwl_trans_txq_enable(priv->trans, q, fifo, sta_priv->sta_id, tid,
+			     buf_size, ssn);
 
 	/*
 	 * If the limit is 0, then it wasn't initialised yet,
@@ -753,8 +757,8 @@ static void iwlagn_check_ratid_empty(struct iwl_priv *priv, int sta_id, u8 tid)
 			IWL_DEBUG_TX_QUEUES(priv,
 				"Can continue DELBA flow ssn = next_recl ="
 				" %d", tid_data->next_reclaimed);
-			iwl_trans_tx_agg_disable(priv->trans,
-						 tid_data->agg.txq_id);
+			iwl_trans_txq_disable(priv->trans,
+					      tid_data->agg.txq_id);
 			iwlagn_dealloc_agg_txq(priv, tid_data->agg.txq_id);
 			tid_data->agg.state = IWL_AGG_OFF;
 			ieee80211_stop_tx_ba_cb_irqsafe(vif, addr, tid);
@@ -1136,6 +1140,7 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 	struct sk_buff *skb;
 	struct iwl_rxon_context *ctx;
 	bool is_agg = (txq_id >= IWLAGN_FIRST_AMPDU_QUEUE);
+	bool is_offchannel_skb;
 
 	tid = (tx_resp->ra_tid & IWLAGN_TX_RES_TID_MSK) >>
 		IWLAGN_TX_RES_TID_POS;
@@ -1149,6 +1154,8 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 
 	__skb_queue_head_init(&skbs);
 
+	is_offchannel_skb = false;
+
 	if (tx_resp->frame_count == 1) {
 		u16 next_reclaimed = le16_to_cpu(tx_resp->seq_ctl);
 		next_reclaimed = SEQ_TO_SN(next_reclaimed + 0x10);
@@ -1176,7 +1183,8 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 		}
 
 		/*we can free until ssn % q.n_bd not inclusive */
-		WARN_ON(iwl_reclaim(priv, sta_id, tid, txq_id, ssn, &skbs));
+		WARN_ON_ONCE(iwl_reclaim(priv, sta_id, tid,
+					 txq_id, ssn, &skbs));
 		iwlagn_check_ratid_empty(priv, sta_id, tid);
 		freed = 0;
 
@@ -1189,8 +1197,8 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 
 			info = IEEE80211_SKB_CB(skb);
 			ctx = info->driver_data[0];
-			kmem_cache_free(iwl_tx_cmd_pool,
-					(info->driver_data[1]));
+			iwl_trans_free_tx_cmd(priv->trans,
+					      info->driver_data[1]);
 
 			memset(&info->status, 0, sizeof(info->status));
 
@@ -1225,10 +1233,19 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 			if (!is_agg)
 				iwlagn_non_agg_tx_status(priv, ctx, hdr->addr1);
 
+			is_offchannel_skb =
+				(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN);
 			freed++;
 		}
 
 		WARN_ON(!is_agg && freed != 1);
+
+		/*
+		 * An offchannel frame can be send only on the AUX queue, where
+		 * there is no aggregation (and reordering) so it only is single
+		 * skb is expected to be processed.
+		 */
+		WARN_ON(is_offchannel_skb && freed != 1);
 	}
 
 	iwl_check_abort_status(priv, tx_resp->frame_count, status);
@@ -1239,6 +1256,9 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 		ieee80211_tx_status(priv->hw, skb);
 	}
 
+	if (is_offchannel_skb)
+		iwl_scan_offchannel_skb_status(priv);
+
 	return 0;
 }
 
@@ -1341,7 +1361,7 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
 			WARN_ON_ONCE(1);
 
 		info = IEEE80211_SKB_CB(skb);
-		kmem_cache_free(iwl_tx_cmd_pool, (info->driver_data[1]));
+		iwl_trans_free_tx_cmd(priv->trans, info->driver_data[1]);
 
 		if (freed == 1) {
 			/* this is the first skb we deliver in this batch */
diff --git a/drivers/net/wireless/iwlwifi/iwl-ucode.c b/drivers/net/wireless/iwlwifi/dvm/ucode.c
index bc40dc68b0f..6d8d6dd7943 100644
--- a/drivers/net/wireless/iwlwifi/iwl-ucode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/ucode.c
@@ -30,15 +30,16 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 
-#include "iwl-dev.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
-#include "iwl-agn.h"
-#include "iwl-agn-calib.h"
 #include "iwl-trans.h"
 #include "iwl-fh.h"
 #include "iwl-op-mode.h"
 
+#include "dev.h"
+#include "agn.h"
+#include "calib.h"
+
 /******************************************************************************
  *
  * uCode download functions
@@ -60,8 +61,7 @@ iwl_get_ucode_image(struct iwl_priv *priv, enum iwl_ucode_type ucode_type)
 static int iwl_set_Xtal_calib(struct iwl_priv *priv)
 {
 	struct iwl_calib_xtal_freq_cmd cmd;
-	__le16 *xtal_calib =
-		(__le16 *)iwl_eeprom_query_addr(priv, EEPROM_XTAL);
+	__le16 *xtal_calib = priv->eeprom_data->xtal_calib;
 
 	iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_CRYSTAL_FRQ_CMD);
 	cmd.cap_pin1 = le16_to_cpu(xtal_calib[0]);
@@ -72,12 +72,10 @@ static int iwl_set_Xtal_calib(struct iwl_priv *priv)
 static int iwl_set_temperature_offset_calib(struct iwl_priv *priv)
 {
 	struct iwl_calib_temperature_offset_cmd cmd;
-	__le16 *offset_calib =
-		(__le16 *)iwl_eeprom_query_addr(priv, EEPROM_RAW_TEMPERATURE);
 
 	memset(&cmd, 0, sizeof(cmd));
 	iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_TEMP_OFFSET_CMD);
-	memcpy(&cmd.radio_sensor_offset, offset_calib, sizeof(*offset_calib));
+	cmd.radio_sensor_offset = priv->eeprom_data->raw_temperature;
 	if (!(cmd.radio_sensor_offset))
 		cmd.radio_sensor_offset = DEFAULT_RADIO_SENSOR_OFFSET;
 
@@ -89,27 +87,17 @@ static int iwl_set_temperature_offset_calib(struct iwl_priv *priv)
 static int iwl_set_temperature_offset_calib_v2(struct iwl_priv *priv)
 {
 	struct iwl_calib_temperature_offset_v2_cmd cmd;
-	__le16 *offset_calib_high = (__le16 *)iwl_eeprom_query_addr(priv,
-				     EEPROM_KELVIN_TEMPERATURE);
-	__le16 *offset_calib_low =
-		(__le16 *)iwl_eeprom_query_addr(priv, EEPROM_RAW_TEMPERATURE);
-	struct iwl_eeprom_calib_hdr *hdr;
 
 	memset(&cmd, 0, sizeof(cmd));
 	iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_TEMP_OFFSET_CMD);
-	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
-							EEPROM_CALIB_ALL);
-	memcpy(&cmd.radio_sensor_offset_high, offset_calib_high,
-		sizeof(*offset_calib_high));
-	memcpy(&cmd.radio_sensor_offset_low, offset_calib_low,
-		sizeof(*offset_calib_low));
-	if (!(cmd.radio_sensor_offset_low)) {
+	cmd.radio_sensor_offset_high = priv->eeprom_data->kelvin_temperature;
+	cmd.radio_sensor_offset_low = priv->eeprom_data->raw_temperature;
+	if (!cmd.radio_sensor_offset_low) {
 		IWL_DEBUG_CALIB(priv, "no info in EEPROM, use default\n");
 		cmd.radio_sensor_offset_low = DEFAULT_RADIO_SENSOR_OFFSET;
 		cmd.radio_sensor_offset_high = DEFAULT_RADIO_SENSOR_OFFSET;
 	}
-	memcpy(&cmd.burntVoltageRef, &hdr->voltage,
-		sizeof(hdr->voltage));
+	cmd.burntVoltageRef = priv->eeprom_data->calib_voltage;
 
 	IWL_DEBUG_CALIB(priv, "Radio sensor offset high: %d\n",
 			le16_to_cpu(cmd.radio_sensor_offset_high));
@@ -177,7 +165,7 @@ int iwl_init_alive_start(struct iwl_priv *priv)
 	return 0;
 }
 
-int iwl_send_wimax_coex(struct iwl_priv *priv)
+static int iwl_send_wimax_coex(struct iwl_priv *priv)
 {
 	struct iwl_wimax_coex_cmd coex_cmd;
 
@@ -238,13 +226,50 @@ int iwl_send_bt_env(struct iwl_priv *priv, u8 action, u8 type)
 	return ret;
 }
 
+static const u8 iwlagn_default_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+};
+
+static const u8 iwlagn_ipan_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_VO,
+	IWL_TX_FIFO_VI,
+	IWL_TX_FIFO_BE,
+	IWL_TX_FIFO_BK,
+	IWL_TX_FIFO_BK_IPAN,
+	IWL_TX_FIFO_BE_IPAN,
+	IWL_TX_FIFO_VI_IPAN,
+	IWL_TX_FIFO_VO_IPAN,
+	IWL_TX_FIFO_BE_IPAN,
+	IWL_TX_FIFO_UNUSED,
+	IWL_TX_FIFO_AUX,
+};
 
 static int iwl_alive_notify(struct iwl_priv *priv)
 {
+	const u8 *queue_to_txf;
+	u8 n_queues;
 	int ret;
+	int i;
 
 	iwl_trans_fw_alive(priv->trans);
 
+	if (priv->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_PAN &&
+	    priv->eeprom_data->sku & EEPROM_SKU_CAP_IPAN_ENABLE) {
+		n_queues = ARRAY_SIZE(iwlagn_ipan_queue_to_tx_fifo);
+		queue_to_txf = iwlagn_ipan_queue_to_tx_fifo;
+	} else {
+		n_queues = ARRAY_SIZE(iwlagn_default_queue_to_tx_fifo);
+		queue_to_txf = iwlagn_default_queue_to_tx_fifo;
+	}
+
+	for (i = 0; i < n_queues; i++)
+		if (queue_to_txf[i] != IWL_TX_FIFO_UNUSED)
+			iwl_trans_ac_txq_enable(priv->trans, i,
+						queue_to_txf[i]);
+
 	priv->passive_no_rx = false;
 	priv->transport_queue_stop = 0;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index 67b28aa7f9b..87f465a49df 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -113,7 +113,7 @@ enum iwl_led_mode {
 #define IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE	0
 
 /* TX queue watchdog timeouts in mSecs */
-#define IWL_WATCHHDOG_DISABLED	0
+#define IWL_WATCHDOG_DISABLED	0
 #define IWL_DEF_WD_TIMEOUT	2000
 #define IWL_LONG_WD_TIMEOUT	10000
 #define IWL_MAX_WD_TIMEOUT	120000
@@ -143,7 +143,7 @@ enum iwl_led_mode {
  * @chain_noise_scale: default chain noise scale used for gain computation
  * @wd_timeout: TX queues watchdog timeout
  * @max_event_log_size: size of event log buffer size for ucode event logging
- * @shadow_reg_enable: HW shadhow register bit
+ * @shadow_reg_enable: HW shadow register support
  * @hd_v2: v2 of enhanced sensitivity value, used for 2000 series and up
  * @no_idle_support: do not support idle mode
  */
@@ -177,18 +177,39 @@ struct iwl_base_params {
 struct iwl_bt_params {
 	bool advanced_bt_coexist;
 	u8 bt_init_traffic_load;
-	u8 bt_prio_boost;
+	u32 bt_prio_boost;
 	u16 agg_time_limit;
 	bool bt_sco_disable;
 	bool bt_session_2;
 };
+
 /*
  * @use_rts_for_aggregation: use rts/cts protection for HT traffic
+ * @ht40_bands: bitmap of bands (using %IEEE80211_BAND_*) that support HT40
  */
 struct iwl_ht_params {
+	enum ieee80211_smps_mode smps_mode;
 	const bool ht_greenfield_support; /* if used set to true */
 	bool use_rts_for_aggregation;
-	enum ieee80211_smps_mode smps_mode;
+	u8 ht40_bands;
+};
+
+/*
+ * information on how to parse the EEPROM
+ */
+#define EEPROM_REG_BAND_1_CHANNELS		0x08
+#define EEPROM_REG_BAND_2_CHANNELS		0x26
+#define EEPROM_REG_BAND_3_CHANNELS		0x42
+#define EEPROM_REG_BAND_4_CHANNELS		0x5C
+#define EEPROM_REG_BAND_5_CHANNELS		0x74
+#define EEPROM_REG_BAND_24_HT40_CHANNELS	0x82
+#define EEPROM_REG_BAND_52_HT40_CHANNELS	0x92
+#define EEPROM_6000_REG_BAND_24_HT40_CHANNELS	0x80
+#define EEPROM_REGULATORY_BAND_NO_HT40		0
+
+struct iwl_eeprom_params {
+	const u8 regulatory_bands[7];
+	bool enhanced_txpower;
 };
 
 /**
@@ -243,6 +264,7 @@ struct iwl_cfg {
 	/* params likely to change within a device family */
 	const struct iwl_ht_params *ht_params;
 	const struct iwl_bt_params *bt_params;
+	const struct iwl_eeprom_params *eeprom_params;
 	const bool need_temp_offset_calib; /* if used set to true */
 	const bool no_xtal_calib;
 	enum iwl_led_mode led_mode;
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 59750543fce..34a5287dfc2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -97,13 +97,10 @@
 /*
  * Hardware revision info
  * Bit fields:
- * 31-8:  Reserved
- *  7-4:  Type of device:  see CSR_HW_REV_TYPE_xxx definitions
+ * 31-16:  Reserved
+ *  15-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)
 
@@ -155,9 +152,21 @@
 #define CSR_DBG_LINK_PWR_MGMT_REG	(CSR_BASE+0x250)
 
 /* Bits for CSR_HW_IF_CONFIG_REG */
-#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_MSK_MAC_DASH	(0x00000003)
+#define CSR_HW_IF_CONFIG_REG_MSK_MAC_STEP	(0x0000000C)
+#define CSR_HW_IF_CONFIG_REG_MSK_BOARD_VER	(0x000000C0)
+#define CSR_HW_IF_CONFIG_REG_BIT_MAC_SI		(0x00000100)
 #define CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI	(0x00000200)
+#define CSR_HW_IF_CONFIG_REG_MSK_PHY_TYPE	(0x00000C00)
+#define CSR_HW_IF_CONFIG_REG_MSK_PHY_DASH	(0x00003000)
+#define CSR_HW_IF_CONFIG_REG_MSK_PHY_STEP	(0x0000C000)
+
+#define CSR_HW_IF_CONFIG_REG_POS_MAC_DASH	(0)
+#define CSR_HW_IF_CONFIG_REG_POS_MAC_STEP	(2)
+#define CSR_HW_IF_CONFIG_REG_POS_BOARD_VER	(6)
+#define CSR_HW_IF_CONFIG_REG_POS_PHY_TYPE	(10)
+#define CSR_HW_IF_CONFIG_REG_POS_PHY_DASH	(12)
+#define CSR_HW_IF_CONFIG_REG_POS_PHY_STEP	(14)
 
 #define CSR_HW_IF_CONFIG_REG_BIT_HAP_WAKE_L1A	(0x00080000)
 #define CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM	(0x00200000)
@@ -270,7 +279,10 @@
 
 
 /* HW REV */
-#define CSR_HW_REV_TYPE_MSK            (0x00001F0)
+#define CSR_HW_REV_DASH(_val)          (((_val) & 0x0000003) >> 0)
+#define CSR_HW_REV_STEP(_val)          (((_val) & 0x000000C) >> 2)
+
+#define CSR_HW_REV_TYPE_MSK            (0x000FFF0)
 #define CSR_HW_REV_TYPE_5300           (0x0000020)
 #define CSR_HW_REV_TYPE_5350           (0x0000030)
 #define CSR_HW_REV_TYPE_5100           (0x0000050)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.c b/drivers/net/wireless/iwlwifi/iwl-debug.c
index 2d1b42847b9..87535a67de7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.c
@@ -61,7 +61,11 @@
  *
  *****************************************************************************/
 
+#define DEBUG
+
+#include <linux/device.h>
 #include <linux/interrupt.h>
+#include <linux/export.h>
 #include "iwl-debug.h"
 #include "iwl-devtrace.h"
 
@@ -81,8 +85,11 @@ void __iwl_ ##fn(struct device *dev, const char *fmt, ...)	\
 }
 
 __iwl_fn(warn)
+EXPORT_SYMBOL_GPL(__iwl_warn);
 __iwl_fn(info)
+EXPORT_SYMBOL_GPL(__iwl_info);
 __iwl_fn(crit)
+EXPORT_SYMBOL_GPL(__iwl_crit);
 
 void __iwl_err(struct device *dev, bool rfkill_prefix, bool trace_only,
 		const char *fmt, ...)
@@ -103,6 +110,7 @@ void __iwl_err(struct device *dev, bool rfkill_prefix, bool trace_only,
 	trace_iwlwifi_err(&vaf);
 	va_end(args);
 }
+EXPORT_SYMBOL_GPL(__iwl_err);
 
 #if defined(CONFIG_IWLWIFI_DEBUG) || defined(CONFIG_IWLWIFI_DEVICE_TRACING)
 void __iwl_dbg(struct device *dev,
@@ -119,10 +127,11 @@ void __iwl_dbg(struct device *dev,
 #ifdef CONFIG_IWLWIFI_DEBUG
 	if (iwl_have_debug_level(level) &&
 	    (!limit || net_ratelimit()))
-		dev_err(dev, "%c %s %pV", in_interrupt() ? 'I' : 'U',
+		dev_dbg(dev, "%c %s %pV", in_interrupt() ? 'I' : 'U',
 			function, &vaf);
 #endif
 	trace_iwlwifi_dbg(level, in_interrupt(), function, &vaf);
 	va_end(args);
 }
+EXPORT_SYMBOL_GPL(__iwl_dbg);
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index 8376b842bdb..42b20b0e83b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -38,13 +38,14 @@ static inline bool iwl_have_debug_level(u32 level)
 }
 
 void __iwl_err(struct device *dev, bool rfkill_prefix, bool only_trace,
-		const char *fmt, ...);
-void __iwl_warn(struct device *dev, const char *fmt, ...);
-void __iwl_info(struct device *dev, const char *fmt, ...);
-void __iwl_crit(struct device *dev, const char *fmt, ...);
+		const char *fmt, ...) __printf(4, 5);
+void __iwl_warn(struct device *dev, const char *fmt, ...) __printf(2, 3);
+void __iwl_info(struct device *dev, const char *fmt, ...) __printf(2, 3);
+void __iwl_crit(struct device *dev, const char *fmt, ...) __printf(2, 3);
 
 /* No matter what is m (priv, bus, trans), this will work */
 #define IWL_ERR(m, f, a...) __iwl_err((m)->dev, false, false, f, ## a)
+#define IWL_ERR_DEV(d, f, a...) __iwl_err((d), false, false, f, ## a)
 #define IWL_WARN(m, f, a...) __iwl_warn((m)->dev, f, ## a)
 #define IWL_INFO(m, f, a...) __iwl_info((m)->dev, f, ## a)
 #define IWL_CRIT(m, f, a...) __iwl_crit((m)->dev, f, ## a)
@@ -52,9 +53,9 @@ void __iwl_crit(struct device *dev, const char *fmt, ...);
 #if defined(CONFIG_IWLWIFI_DEBUG) || defined(CONFIG_IWLWIFI_DEVICE_TRACING)
 void __iwl_dbg(struct device *dev,
 	       u32 level, bool limit, const char *function,
-	       const char *fmt, ...);
+	       const char *fmt, ...) __printf(5, 6);
 #else
-static inline void
+__printf(5, 6) static inline void
 __iwl_dbg(struct device *dev,
 	  u32 level, bool limit, const char *function,
 	  const char *fmt, ...)
@@ -69,6 +70,8 @@ do {									\
 
 #define IWL_DEBUG(m, level, fmt, args...)				\
 	__iwl_dbg((m)->dev, level, false, __func__, fmt, ##args)
+#define IWL_DEBUG_DEV(dev, level, fmt, args...)				\
+	__iwl_dbg((dev), level, false, __func__, fmt, ##args)
 #define IWL_DEBUG_LIMIT(m, level, fmt, args...)				\
 	__iwl_dbg((m)->dev, level, true, __func__, fmt, ##args)
 
@@ -153,7 +156,7 @@ do {                                            			\
 #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_EEPROM(p, f, a...)	IWL_DEBUG(p, IWL_DL_EEPROM, f, ## a)
+#define IWL_DEBUG_EEPROM(d, f, a...)	IWL_DEBUG_DEV(d, 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)
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.c b/drivers/net/wireless/iwlwifi/iwl-devtrace.c
index 91f45e71e0a..70191ddbd8f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.c
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.c
@@ -42,4 +42,9 @@ EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_event);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_error);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_cont_event);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_wrap_event);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_info);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_warn);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_crit);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_err);
+EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dbg);
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.h b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
index 06203d6a1d8..06ca505bb2c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.h
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
@@ -28,6 +28,7 @@
 #define __IWLWIFI_DEVICE_TRACE
 
 #include <linux/tracepoint.h>
+#include <linux/device.h>
 
 
 #if !defined(CONFIG_IWLWIFI_DEVICE_TRACING) || defined(__CHECKER__)
@@ -175,7 +176,7 @@ TRACE_EVENT(iwlwifi_dev_ucode_wrap_event,
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM iwlwifi_msg
 
-#define MAX_MSG_LEN	100
+#define MAX_MSG_LEN	110
 
 DECLARE_EVENT_CLASS(iwlwifi_msg_event,
 	TP_PROTO(struct va_format *vaf),
@@ -188,7 +189,7 @@ DECLARE_EVENT_CLASS(iwlwifi_msg_event,
 				       MAX_MSG_LEN, vaf->fmt,
 				       *vaf->va) >= MAX_MSG_LEN);
 	),
-	TP_printk("%s", (char *)__get_dynamic_array(msg))
+	TP_printk("%s", __get_str(msg))
 );
 
 DEFINE_EVENT(iwlwifi_msg_event, iwlwifi_err,
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index fac67a526a3..cc41cfaedfb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -77,8 +77,33 @@
 /* private includes */
 #include "iwl-fw-file.h"
 
+/******************************************************************************
+ *
+ * module boiler plate
+ *
+ ******************************************************************************/
+
+/*
+ * module name, copyright, version, etc.
+ */
+#define DRV_DESCRIPTION	"Intel(R) Wireless WiFi driver for Linux"
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+#define VD "d"
+#else
+#define VD
+#endif
+
+#define DRV_VERSION     IWLWIFI_VERSION VD
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
+MODULE_LICENSE("GPL");
+
 /**
  * struct iwl_drv - drv common data
+ * @list: list of drv structures using this opmode
  * @fw: the iwl_fw structure
  * @op_mode: the running op_mode
  * @trans: transport layer
@@ -89,6 +114,7 @@
  * @request_firmware_complete: the firmware has been obtained from user space
  */
 struct iwl_drv {
+	struct list_head list;
 	struct iwl_fw fw;
 
 	struct iwl_op_mode *op_mode;
@@ -102,7 +128,19 @@ struct iwl_drv {
 	struct completion request_firmware_complete;
 };
 
-
+#define DVM_OP_MODE	0
+#define MVM_OP_MODE	1
+
+/* Protects the table contents, i.e. the ops pointer & drv list */
+static struct mutex iwlwifi_opmode_table_mtx;
+static struct iwlwifi_opmode_table {
+	const char *name;			/* name: iwldvm, iwlmvm, etc */
+	const struct iwl_op_mode_ops *ops;	/* pointer to op_mode ops */
+	struct list_head drv;		/* list of devices using this op_mode */
+} iwlwifi_opmode_table[] = {		/* ops set when driver is initialized */
+	{ .name = "iwldvm", .ops = NULL },
+	{ .name = "iwlmvm", .ops = NULL },
+};
 
 /*
  * struct fw_sec: Just for the image parsing proccess.
@@ -721,7 +759,6 @@ static int validate_sec_sizes(struct iwl_drv *drv,
 	return 0;
 }
 
-
 /**
  * iwl_ucode_callback - callback when firmware was loaded
  *
@@ -733,6 +770,7 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	struct iwl_drv *drv = context;
 	struct iwl_fw *fw = &drv->fw;
 	struct iwl_ucode_header *ucode;
+	struct iwlwifi_opmode_table *op;
 	int err;
 	struct iwl_firmware_pieces pieces;
 	const unsigned int api_max = drv->cfg->ucode_api_max;
@@ -740,6 +778,7 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	const unsigned int api_min = drv->cfg->ucode_api_min;
 	u32 api_ver;
 	int i;
+	bool load_module = false;
 
 	fw->ucode_capa.max_probe_length = 200;
 	fw->ucode_capa.standard_phy_calibration_size =
@@ -862,10 +901,24 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	/* We have our copies now, allow OS release its copies */
 	release_firmware(ucode_raw);
 
-	drv->op_mode = iwl_dvm_ops.start(drv->trans, drv->cfg, &drv->fw);
+	mutex_lock(&iwlwifi_opmode_table_mtx);
+	op = &iwlwifi_opmode_table[DVM_OP_MODE];
 
-	if (!drv->op_mode)
-		goto out_unbind;
+	/* add this device to the list of devices using this op_mode */
+	list_add_tail(&drv->list, &op->drv);
+
+	if (op->ops) {
+		const struct iwl_op_mode_ops *ops = op->ops;
+		drv->op_mode = ops->start(drv->trans, drv->cfg, &drv->fw);
+
+		if (!drv->op_mode) {
+			mutex_unlock(&iwlwifi_opmode_table_mtx);
+			goto out_unbind;
+		}
+	} else {
+		load_module = true;
+	}
+	mutex_unlock(&iwlwifi_opmode_table_mtx);
 
 	/*
 	 * Complete the firmware request last so that
@@ -873,6 +926,14 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
 	 * are doing the start() above.
 	 */
 	complete(&drv->request_firmware_complete);
+
+	/*
+	 * Load the module last so we don't block anything
+	 * else from proceeding if the module fails to load
+	 * or hangs loading.
+	 */
+	if (load_module)
+		request_module("%s", op->name);
 	return;
 
  try_again:
@@ -906,6 +967,7 @@ struct iwl_drv *iwl_drv_start(struct iwl_trans *trans,
 	drv->cfg = cfg;
 
 	init_completion(&drv->request_firmware_complete);
+	INIT_LIST_HEAD(&drv->list);
 
 	ret = iwl_request_firmware(drv, true);
 
@@ -928,6 +990,16 @@ void iwl_drv_stop(struct iwl_drv *drv)
 
 	iwl_dealloc_ucode(drv);
 
+	mutex_lock(&iwlwifi_opmode_table_mtx);
+	/*
+	 * List is empty (this item wasn't added)
+	 * when firmware loading failed -- in that
+	 * case we can't remove it from any list.
+	 */
+	if (!list_empty(&drv->list))
+		list_del(&drv->list);
+	mutex_unlock(&iwlwifi_opmode_table_mtx);
+
 	kfree(drv);
 }
 
@@ -941,8 +1013,78 @@ struct iwl_mod_params iwlwifi_mod_params = {
 	.power_level = IWL_POWER_INDEX_1,
 	.bt_ch_announce = true,
 	.auto_agg = true,
+	.wd_disable = true,
 	/* the rest are 0 by default */
 };
+EXPORT_SYMBOL_GPL(iwlwifi_mod_params);
+
+int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops)
+{
+	int i;
+	struct iwl_drv *drv;
+
+	mutex_lock(&iwlwifi_opmode_table_mtx);
+	for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++) {
+		if (strcmp(iwlwifi_opmode_table[i].name, name))
+			continue;
+		iwlwifi_opmode_table[i].ops = ops;
+		list_for_each_entry(drv, &iwlwifi_opmode_table[i].drv, list)
+			drv->op_mode = ops->start(drv->trans, drv->cfg,
+						  &drv->fw);
+		mutex_unlock(&iwlwifi_opmode_table_mtx);
+		return 0;
+	}
+	mutex_unlock(&iwlwifi_opmode_table_mtx);
+	return -EIO;
+}
+EXPORT_SYMBOL_GPL(iwl_opmode_register);
+
+void iwl_opmode_deregister(const char *name)
+{
+	int i;
+	struct iwl_drv *drv;
+
+	mutex_lock(&iwlwifi_opmode_table_mtx);
+	for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++) {
+		if (strcmp(iwlwifi_opmode_table[i].name, name))
+			continue;
+		iwlwifi_opmode_table[i].ops = NULL;
+
+		/* call the stop routine for all devices */
+		list_for_each_entry(drv, &iwlwifi_opmode_table[i].drv, list) {
+			if (drv->op_mode) {
+				iwl_op_mode_stop(drv->op_mode);
+				drv->op_mode = NULL;
+			}
+		}
+		mutex_unlock(&iwlwifi_opmode_table_mtx);
+		return;
+	}
+	mutex_unlock(&iwlwifi_opmode_table_mtx);
+}
+EXPORT_SYMBOL_GPL(iwl_opmode_deregister);
+
+static int __init iwl_drv_init(void)
+{
+	int i;
+
+	mutex_init(&iwlwifi_opmode_table_mtx);
+
+	for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++)
+		INIT_LIST_HEAD(&iwlwifi_opmode_table[i].drv);
+
+	pr_info(DRV_DESCRIPTION ", " DRV_VERSION "\n");
+	pr_info(DRV_COPYRIGHT "\n");
+
+	return iwl_pci_register_driver();
+}
+module_init(iwl_drv_init);
+
+static void __exit iwl_drv_exit(void)
+{
+	iwl_pci_unregister_driver();
+}
+module_exit(iwl_drv_exit);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
 module_param_named(debug, iwlwifi_mod_params.debug_level, uint,
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
new file mode 100644
index 00000000000..f10170fe879
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
@@ -0,0 +1,903 @@
+/******************************************************************************
+ *
+ * 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 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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 - 2012 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/types.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+#include "iwl-modparams.h"
+#include "iwl-eeprom-parse.h"
+
+/* EEPROM offset definitions */
+
+/* indirect access definitions */
+#define ADDRESS_MSK                 0x0000FFFF
+#define INDIRECT_TYPE_MSK           0x000F0000
+#define INDIRECT_HOST               0x00010000
+#define INDIRECT_GENERAL            0x00020000
+#define INDIRECT_REGULATORY         0x00030000
+#define INDIRECT_CALIBRATION        0x00040000
+#define INDIRECT_PROCESS_ADJST      0x00050000
+#define INDIRECT_OTHERS             0x00060000
+#define INDIRECT_TXP_LIMIT          0x00070000
+#define INDIRECT_TXP_LIMIT_SIZE     0x00080000
+#define INDIRECT_ADDRESS            0x00100000
+
+/* corresponding link offsets in EEPROM */
+#define EEPROM_LINK_HOST             (2*0x64)
+#define EEPROM_LINK_GENERAL          (2*0x65)
+#define EEPROM_LINK_REGULATORY       (2*0x66)
+#define EEPROM_LINK_CALIBRATION      (2*0x67)
+#define EEPROM_LINK_PROCESS_ADJST    (2*0x68)
+#define EEPROM_LINK_OTHERS           (2*0x69)
+#define EEPROM_LINK_TXP_LIMIT        (2*0x6a)
+#define EEPROM_LINK_TXP_LIMIT_SIZE   (2*0x6b)
+
+/* General */
+#define EEPROM_DEVICE_ID                    (2*0x08)	/* 2 bytes */
+#define EEPROM_SUBSYSTEM_ID		    (2*0x0A)	/* 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_RADIO_CONFIG                 (2*0x48)	/* 2  bytes */
+#define EEPROM_NUM_MAC_ADDRESS              (2*0x4C)	/* 2  bytes */
+
+/* calibration */
+struct iwl_eeprom_calib_hdr {
+	u8 version;
+	u8 pa_type;
+	__le16 voltage;
+} __packed;
+
+#define EEPROM_CALIB_ALL	(INDIRECT_ADDRESS | INDIRECT_CALIBRATION)
+#define EEPROM_XTAL		((2*0x128) | EEPROM_CALIB_ALL)
+
+/* temperature */
+#define EEPROM_KELVIN_TEMPERATURE	((2*0x12A) | EEPROM_CALIB_ALL)
+#define EEPROM_RAW_TEMPERATURE		((2*0x12B) | EEPROM_CALIB_ALL)
+
+/*
+ * EEPROM bands
+ * These are the channel numbers from each band in the order
+ * that they are stored in the EEPROM band information. Note
+ * that EEPROM bands aren't the same as mac80211 bands, and
+ * there are even special "ht40 bands" in the EEPROM.
+ */
+static const u8 iwl_eeprom_band_1[14] = { /* 2.4 GHz */
+	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
+};
+
+static const u8 iwl_eeprom_band_2[] = {	/* 4915-5080MHz */
+	183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
+};
+
+static const u8 iwl_eeprom_band_3[] = {	/* 5170-5320MHz */
+	34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
+};
+
+static const u8 iwl_eeprom_band_4[] = {	/* 5500-5700MHz */
+	100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+};
+
+static const u8 iwl_eeprom_band_5[] = {	/* 5725-5825MHz */
+	145, 149, 153, 157, 161, 165
+};
+
+static const u8 iwl_eeprom_band_6[] = {	/* 2.4 ht40 channel */
+	1, 2, 3, 4, 5, 6, 7
+};
+
+static const u8 iwl_eeprom_band_7[] = {	/* 5.2 ht40 channel */
+	36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157
+};
+
+#define IWL_NUM_CHANNELS	(ARRAY_SIZE(iwl_eeprom_band_1) + \
+				 ARRAY_SIZE(iwl_eeprom_band_2) + \
+				 ARRAY_SIZE(iwl_eeprom_band_3) + \
+				 ARRAY_SIZE(iwl_eeprom_band_4) + \
+				 ARRAY_SIZE(iwl_eeprom_band_5))
+
+/* rate data (static) */
+static struct ieee80211_rate iwl_cfg80211_rates[] = {
+	{ .bitrate = 1 * 10, .hw_value = 0, .hw_value_short = 0, },
+	{ .bitrate = 2 * 10, .hw_value = 1, .hw_value_short = 1,
+	  .flags = IEEE80211_RATE_SHORT_PREAMBLE, },
+	{ .bitrate = 5.5 * 10, .hw_value = 2, .hw_value_short = 2,
+	  .flags = IEEE80211_RATE_SHORT_PREAMBLE, },
+	{ .bitrate = 11 * 10, .hw_value = 3, .hw_value_short = 3,
+	  .flags = IEEE80211_RATE_SHORT_PREAMBLE, },
+	{ .bitrate = 6 * 10, .hw_value = 4, .hw_value_short = 4, },
+	{ .bitrate = 9 * 10, .hw_value = 5, .hw_value_short = 5, },
+	{ .bitrate = 12 * 10, .hw_value = 6, .hw_value_short = 6, },
+	{ .bitrate = 18 * 10, .hw_value = 7, .hw_value_short = 7, },
+	{ .bitrate = 24 * 10, .hw_value = 8, .hw_value_short = 8, },
+	{ .bitrate = 36 * 10, .hw_value = 9, .hw_value_short = 9, },
+	{ .bitrate = 48 * 10, .hw_value = 10, .hw_value_short = 10, },
+	{ .bitrate = 54 * 10, .hw_value = 11, .hw_value_short = 11, },
+};
+#define RATES_24_OFFS	0
+#define N_RATES_24	ARRAY_SIZE(iwl_cfg80211_rates)
+#define RATES_52_OFFS	4
+#define N_RATES_52	(N_RATES_24 - RATES_52_OFFS)
+
+/* EEPROM reading functions */
+
+static u16 iwl_eeprom_query16(const u8 *eeprom, size_t eeprom_size, int offset)
+{
+	if (WARN_ON(offset + sizeof(u16) > eeprom_size))
+		return 0;
+	return le16_to_cpup((__le16 *)(eeprom + offset));
+}
+
+static u32 eeprom_indirect_address(const u8 *eeprom, size_t eeprom_size,
+				   u32 address)
+{
+	u16 offset = 0;
+
+	if ((address & INDIRECT_ADDRESS) == 0)
+		return address;
+
+	switch (address & INDIRECT_TYPE_MSK) {
+	case INDIRECT_HOST:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_HOST);
+		break;
+	case INDIRECT_GENERAL:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_GENERAL);
+		break;
+	case INDIRECT_REGULATORY:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_REGULATORY);
+		break;
+	case INDIRECT_TXP_LIMIT:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_TXP_LIMIT);
+		break;
+	case INDIRECT_TXP_LIMIT_SIZE:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_TXP_LIMIT_SIZE);
+		break;
+	case INDIRECT_CALIBRATION:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_CALIBRATION);
+		break;
+	case INDIRECT_PROCESS_ADJST:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_PROCESS_ADJST);
+		break;
+	case INDIRECT_OTHERS:
+		offset = iwl_eeprom_query16(eeprom, eeprom_size,
+					    EEPROM_LINK_OTHERS);
+		break;
+	default:
+		WARN_ON(1);
+		break;
+	}
+
+	/* translate the offset from words to byte */
+	return (address & ADDRESS_MSK) + (offset << 1);
+}
+
+static const u8 *iwl_eeprom_query_addr(const u8 *eeprom, size_t eeprom_size,
+				       u32 offset)
+{
+	u32 address = eeprom_indirect_address(eeprom, eeprom_size, offset);
+
+	if (WARN_ON(address >= eeprom_size))
+		return NULL;
+
+	return &eeprom[address];
+}
+
+static int iwl_eeprom_read_calib(const u8 *eeprom, size_t eeprom_size,
+				 struct iwl_eeprom_data *data)
+{
+	struct iwl_eeprom_calib_hdr *hdr;
+
+	hdr = (void *)iwl_eeprom_query_addr(eeprom, eeprom_size,
+					    EEPROM_CALIB_ALL);
+	if (!hdr)
+		return -ENODATA;
+	data->calib_version = hdr->version;
+	data->calib_voltage = hdr->voltage;
+
+	return 0;
+}
+
+/**
+ * enum iwl_eeprom_channel_flags - channel flags in EEPROM
+ * @EEPROM_CHANNEL_VALID: channel is usable for this SKU/geo
+ * @EEPROM_CHANNEL_IBSS: usable as an IBSS channel
+ * @EEPROM_CHANNEL_ACTIVE: active scanning allowed
+ * @EEPROM_CHANNEL_RADAR: radar detection required
+ * @EEPROM_CHANNEL_WIDE: 20 MHz channel okay (?)
+ * @EEPROM_CHANNEL_DFS: dynamic freq selection candidate
+ */
+enum iwl_eeprom_channel_flags {
+	EEPROM_CHANNEL_VALID = BIT(0),
+	EEPROM_CHANNEL_IBSS = BIT(1),
+	EEPROM_CHANNEL_ACTIVE = BIT(3),
+	EEPROM_CHANNEL_RADAR = BIT(4),
+	EEPROM_CHANNEL_WIDE = BIT(5),
+	EEPROM_CHANNEL_DFS = BIT(7),
+};
+
+/**
+ * struct iwl_eeprom_channel - EEPROM channel data
+ * @flags: %EEPROM_CHANNEL_* flags
+ * @max_power_avg: max power (in dBm) on this channel, at most 31 dBm
+ */
+struct iwl_eeprom_channel {
+	u8 flags;
+	s8 max_power_avg;
+} __packed;
+
+
+enum iwl_eeprom_enhanced_txpwr_flags {
+	IWL_EEPROM_ENH_TXP_FL_VALID = BIT(0),
+	IWL_EEPROM_ENH_TXP_FL_BAND_52G = BIT(1),
+	IWL_EEPROM_ENH_TXP_FL_OFDM = BIT(2),
+	IWL_EEPROM_ENH_TXP_FL_40MHZ = BIT(3),
+	IWL_EEPROM_ENH_TXP_FL_HT_AP = BIT(4),
+	IWL_EEPROM_ENH_TXP_FL_RES1 = BIT(5),
+	IWL_EEPROM_ENH_TXP_FL_RES2 = BIT(6),
+	IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE = BIT(7),
+};
+
+/**
+ * iwl_eeprom_enhanced_txpwr structure
+ * @flags: entry flags
+ * @channel: channel number
+ * @chain_a_max_pwr: chain a max power in 1/2 dBm
+ * @chain_b_max_pwr: chain b max power in 1/2 dBm
+ * @chain_c_max_pwr: chain c max power in 1/2 dBm
+ * @delta_20_in_40: 20-in-40 deltas (hi/lo)
+ * @mimo2_max_pwr: mimo2 max power in 1/2 dBm
+ * @mimo3_max_pwr: mimo3 max power in 1/2 dBm
+ *
+ * This structure presents the enhanced regulatory tx power limit layout
+ * in an EEPROM image.
+ */
+struct iwl_eeprom_enhanced_txpwr {
+	u8 flags;
+	u8 channel;
+	s8 chain_a_max;
+	s8 chain_b_max;
+	s8 chain_c_max;
+	u8 delta_20_in_40;
+	s8 mimo2_max;
+	s8 mimo3_max;
+} __packed;
+
+static s8 iwl_get_max_txpwr_half_dbm(const struct iwl_eeprom_data *data,
+				     struct iwl_eeprom_enhanced_txpwr *txp)
+{
+	s8 result = 0; /* (.5 dBm) */
+
+	/* Take the highest tx power from any valid chains */
+	if (data->valid_tx_ant & ANT_A && txp->chain_a_max > result)
+		result = txp->chain_a_max;
+
+	if (data->valid_tx_ant & ANT_B && txp->chain_b_max > result)
+		result = txp->chain_b_max;
+
+	if (data->valid_tx_ant & ANT_C && txp->chain_c_max > result)
+		result = txp->chain_c_max;
+
+	if ((data->valid_tx_ant == ANT_AB ||
+	     data->valid_tx_ant == ANT_BC ||
+	     data->valid_tx_ant == ANT_AC) && txp->mimo2_max > result)
+		result = txp->mimo2_max;
+
+	if (data->valid_tx_ant == ANT_ABC && txp->mimo3_max > result)
+		result = txp->mimo3_max;
+
+	return result;
+}
+
+#define EEPROM_TXP_OFFS	(0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT)
+#define EEPROM_TXP_ENTRY_LEN sizeof(struct iwl_eeprom_enhanced_txpwr)
+#define EEPROM_TXP_SZ_OFFS (0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT_SIZE)
+
+#define TXP_CHECK_AND_PRINT(x) \
+	((txp->flags & IWL_EEPROM_ENH_TXP_FL_##x) ? # x " " : "")
+
+static void
+iwl_eeprom_enh_txp_read_element(struct iwl_eeprom_data *data,
+				struct iwl_eeprom_enhanced_txpwr *txp,
+				int n_channels, s8 max_txpower_avg)
+{
+	int ch_idx;
+	enum ieee80211_band band;
+
+	band = txp->flags & IWL_EEPROM_ENH_TXP_FL_BAND_52G ?
+		IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
+
+	for (ch_idx = 0; ch_idx < n_channels; ch_idx++) {
+		struct ieee80211_channel *chan = &data->channels[ch_idx];
+
+		/* update matching channel or from common data only */
+		if (txp->channel != 0 && chan->hw_value != txp->channel)
+			continue;
+
+		/* update matching band only */
+		if (band != chan->band)
+			continue;
+
+		if (chan->max_power < max_txpower_avg &&
+		    !(txp->flags & IWL_EEPROM_ENH_TXP_FL_40MHZ))
+			chan->max_power = max_txpower_avg;
+	}
+}
+
+static void iwl_eeprom_enhanced_txpower(struct device *dev,
+					struct iwl_eeprom_data *data,
+					const u8 *eeprom, size_t eeprom_size,
+					int n_channels)
+{
+	struct iwl_eeprom_enhanced_txpwr *txp_array, *txp;
+	int idx, entries;
+	__le16 *txp_len;
+	s8 max_txp_avg_halfdbm;
+
+	BUILD_BUG_ON(sizeof(struct iwl_eeprom_enhanced_txpwr) != 8);
+
+	/* the length is in 16-bit words, but we want entries */
+	txp_len = (__le16 *)iwl_eeprom_query_addr(eeprom, eeprom_size,
+						  EEPROM_TXP_SZ_OFFS);
+	entries = le16_to_cpup(txp_len) * 2 / EEPROM_TXP_ENTRY_LEN;
+
+	txp_array = (void *)iwl_eeprom_query_addr(eeprom, eeprom_size,
+						  EEPROM_TXP_OFFS);
+
+	for (idx = 0; idx < entries; idx++) {
+		txp = &txp_array[idx];
+		/* skip invalid entries */
+		if (!(txp->flags & IWL_EEPROM_ENH_TXP_FL_VALID))
+			continue;
+
+		IWL_DEBUG_EEPROM(dev, "%s %d:\t %s%s%s%s%s%s%s%s (0x%02x)\n",
+				 (txp->channel && (txp->flags &
+					IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE)) ?
+					"Common " : (txp->channel) ?
+					"Channel" : "Common",
+				 (txp->channel),
+				 TXP_CHECK_AND_PRINT(VALID),
+				 TXP_CHECK_AND_PRINT(BAND_52G),
+				 TXP_CHECK_AND_PRINT(OFDM),
+				 TXP_CHECK_AND_PRINT(40MHZ),
+				 TXP_CHECK_AND_PRINT(HT_AP),
+				 TXP_CHECK_AND_PRINT(RES1),
+				 TXP_CHECK_AND_PRINT(RES2),
+				 TXP_CHECK_AND_PRINT(COMMON_TYPE),
+				 txp->flags);
+		IWL_DEBUG_EEPROM(dev,
+				 "\t\t chain_A: 0x%02x chain_B: 0X%02x chain_C: 0X%02x\n",
+				 txp->chain_a_max, txp->chain_b_max,
+				 txp->chain_c_max);
+		IWL_DEBUG_EEPROM(dev,
+				 "\t\t MIMO2: 0x%02x MIMO3: 0x%02x High 20_on_40: 0x%02x Low 20_on_40: 0x%02x\n",
+				 txp->mimo2_max, txp->mimo3_max,
+				 ((txp->delta_20_in_40 & 0xf0) >> 4),
+				 (txp->delta_20_in_40 & 0x0f));
+
+		max_txp_avg_halfdbm = iwl_get_max_txpwr_half_dbm(data, txp);
+
+		iwl_eeprom_enh_txp_read_element(data, txp, n_channels,
+				DIV_ROUND_UP(max_txp_avg_halfdbm, 2));
+
+		if (max_txp_avg_halfdbm > data->max_tx_pwr_half_dbm)
+			data->max_tx_pwr_half_dbm = max_txp_avg_halfdbm;
+	}
+}
+
+static void iwl_init_band_reference(const struct iwl_cfg *cfg,
+				    const u8 *eeprom, size_t eeprom_size,
+				    int eeprom_band, int *eeprom_ch_count,
+				    const struct iwl_eeprom_channel **ch_info,
+				    const u8 **eeprom_ch_array)
+{
+	u32 offset = cfg->eeprom_params->regulatory_bands[eeprom_band - 1];
+
+	offset |= INDIRECT_ADDRESS | INDIRECT_REGULATORY;
+
+	*ch_info = (void *)iwl_eeprom_query_addr(eeprom, eeprom_size, offset);
+
+	switch (eeprom_band) {
+	case 1:		/* 2.4GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
+		*eeprom_ch_array = iwl_eeprom_band_1;
+		break;
+	case 2:		/* 4.9GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
+		*eeprom_ch_array = iwl_eeprom_band_2;
+		break;
+	case 3:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
+		*eeprom_ch_array = iwl_eeprom_band_3;
+		break;
+	case 4:		/* 5.5GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
+		*eeprom_ch_array = iwl_eeprom_band_4;
+		break;
+	case 5:		/* 5.7GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
+		*eeprom_ch_array = iwl_eeprom_band_5;
+		break;
+	case 6:		/* 2.4GHz ht40 channels */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
+		*eeprom_ch_array = iwl_eeprom_band_6;
+		break;
+	case 7:		/* 5 GHz ht40 channels */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
+		*eeprom_ch_array = iwl_eeprom_band_7;
+		break;
+	default:
+		*eeprom_ch_count = 0;
+		*eeprom_ch_array = NULL;
+		WARN_ON(1);
+	}
+}
+
+#define CHECK_AND_PRINT(x) \
+	((eeprom_ch->flags & EEPROM_CHANNEL_##x) ? # x " " : "")
+
+static void iwl_mod_ht40_chan_info(struct device *dev,
+				   struct iwl_eeprom_data *data, int n_channels,
+				   enum ieee80211_band band, u16 channel,
+				   const struct iwl_eeprom_channel *eeprom_ch,
+				   u8 clear_ht40_extension_channel)
+{
+	struct ieee80211_channel *chan = NULL;
+	int i;
+
+	for (i = 0; i < n_channels; i++) {
+		if (data->channels[i].band != band)
+			continue;
+		if (data->channels[i].hw_value != channel)
+			continue;
+		chan = &data->channels[i];
+		break;
+	}
+
+	if (!chan)
+		return;
+
+	IWL_DEBUG_EEPROM(dev,
+			 "HT40 Ch. %d [%sGHz] %s%s%s%s%s(0x%02x %ddBm): Ad-Hoc %ssupported\n",
+			 channel,
+			 band == IEEE80211_BAND_5GHZ ? "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 ");
+
+	if (eeprom_ch->flags & EEPROM_CHANNEL_VALID)
+		chan->flags &= ~clear_ht40_extension_channel;
+}
+
+#define CHECK_AND_PRINT_I(x)	\
+	((eeprom_ch_info[ch_idx].flags & EEPROM_CHANNEL_##x) ? # x " " : "")
+
+static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
+				struct iwl_eeprom_data *data,
+				const u8 *eeprom, size_t eeprom_size)
+{
+	int band, ch_idx;
+	const struct iwl_eeprom_channel *eeprom_ch_info;
+	const u8 *eeprom_ch_array;
+	int eeprom_ch_count;
+	int n_channels = 0;
+
+	/*
+	 * Loop through the 5 EEPROM bands and add them to the parse list
+	 */
+	for (band = 1; band <= 5; band++) {
+		struct ieee80211_channel *channel;
+
+		iwl_init_band_reference(cfg, eeprom, eeprom_size, band,
+					&eeprom_ch_count, &eeprom_ch_info,
+					&eeprom_ch_array);
+
+		/* Loop through each band adding each of the channels */
+		for (ch_idx = 0; ch_idx < eeprom_ch_count; ch_idx++) {
+			const struct iwl_eeprom_channel *eeprom_ch;
+
+			eeprom_ch = &eeprom_ch_info[ch_idx];
+
+			if (!(eeprom_ch->flags & EEPROM_CHANNEL_VALID)) {
+				IWL_DEBUG_EEPROM(dev,
+						 "Ch. %d Flags %x [%sGHz] - No traffic\n",
+						 eeprom_ch_array[ch_idx],
+						 eeprom_ch_info[ch_idx].flags,
+						 (band != 1) ? "5.2" : "2.4");
+				continue;
+			}
+
+			channel = &data->channels[n_channels];
+			n_channels++;
+
+			channel->hw_value = eeprom_ch_array[ch_idx];
+			channel->band = (band == 1) ? IEEE80211_BAND_2GHZ
+						    : IEEE80211_BAND_5GHZ;
+			channel->center_freq =
+				ieee80211_channel_to_frequency(
+					channel->hw_value, channel->band);
+
+			/* set no-HT40, will enable as appropriate later */
+			channel->flags = IEEE80211_CHAN_NO_HT40;
+
+			if (!(eeprom_ch->flags & EEPROM_CHANNEL_IBSS))
+				channel->flags |= IEEE80211_CHAN_NO_IBSS;
+
+			if (!(eeprom_ch->flags & EEPROM_CHANNEL_ACTIVE))
+				channel->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
+
+			if (eeprom_ch->flags & EEPROM_CHANNEL_RADAR)
+				channel->flags |= IEEE80211_CHAN_RADAR;
+
+			/* Initialize regulatory-based run-time data */
+			channel->max_power =
+				eeprom_ch_info[ch_idx].max_power_avg;
+			IWL_DEBUG_EEPROM(dev,
+					 "Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x %ddBm): Ad-Hoc %ssupported\n",
+					 channel->hw_value,
+					 (band != 1) ? "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_idx].flags,
+					 eeprom_ch_info[ch_idx].max_power_avg,
+					 ((eeprom_ch_info[ch_idx].flags &
+							EEPROM_CHANNEL_IBSS) &&
+					  !(eeprom_ch_info[ch_idx].flags &
+							EEPROM_CHANNEL_RADAR))
+						? "" : "not ");
+		}
+	}
+
+	if (cfg->eeprom_params->enhanced_txpower) {
+		/*
+		 * for newer device (6000 series and up)
+		 * EEPROM contain enhanced tx power information
+		 * driver need to process addition information
+		 * to determine the max channel tx power limits
+		 */
+		iwl_eeprom_enhanced_txpower(dev, data, eeprom, eeprom_size,
+					    n_channels);
+	} else {
+		/* All others use data from channel map */
+		int i;
+
+		data->max_tx_pwr_half_dbm = -128;
+
+		for (i = 0; i < n_channels; i++)
+			data->max_tx_pwr_half_dbm =
+				max_t(s8, data->max_tx_pwr_half_dbm,
+				      data->channels[i].max_power * 2);
+	}
+
+	/* Check if we do have HT40 channels */
+	if (cfg->eeprom_params->regulatory_bands[5] ==
+				EEPROM_REGULATORY_BAND_NO_HT40 &&
+	    cfg->eeprom_params->regulatory_bands[6] ==
+				EEPROM_REGULATORY_BAND_NO_HT40)
+		return n_channels;
+
+	/* Two additional EEPROM bands for 2.4 and 5 GHz HT40 channels */
+	for (band = 6; band <= 7; band++) {
+		enum ieee80211_band ieeeband;
+
+		iwl_init_band_reference(cfg, eeprom, eeprom_size, band,
+					&eeprom_ch_count, &eeprom_ch_info,
+					&eeprom_ch_array);
+
+		/* 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_idx = 0; ch_idx < eeprom_ch_count; ch_idx++) {
+			/* Set up driver's info for lower half */
+			iwl_mod_ht40_chan_info(dev, data, n_channels, ieeeband,
+					       eeprom_ch_array[ch_idx],
+					       &eeprom_ch_info[ch_idx],
+					       IEEE80211_CHAN_NO_HT40PLUS);
+
+			/* Set up driver's info for upper half */
+			iwl_mod_ht40_chan_info(dev, data, n_channels, ieeeband,
+					       eeprom_ch_array[ch_idx] + 4,
+					       &eeprom_ch_info[ch_idx],
+					       IEEE80211_CHAN_NO_HT40MINUS);
+		}
+	}
+
+	return n_channels;
+}
+
+static int iwl_init_sband_channels(struct iwl_eeprom_data *data,
+				   struct ieee80211_supported_band *sband,
+				   int n_channels, enum ieee80211_band band)
+{
+	struct ieee80211_channel *chan = &data->channels[0];
+	int n = 0, idx = 0;
+
+	while (chan->band != band && idx < n_channels)
+		chan = &data->channels[++idx];
+
+	sband->channels = &data->channels[idx];
+
+	while (chan->band == band && idx < n_channels) {
+		chan = &data->channels[++idx];
+		n++;
+	}
+
+	sband->n_channels = n;
+
+	return n;
+}
+
+#define MAX_BIT_RATE_40_MHZ	150 /* Mbps */
+#define MAX_BIT_RATE_20_MHZ	72 /* Mbps */
+
+static void iwl_init_ht_hw_capab(const struct iwl_cfg *cfg,
+				 struct iwl_eeprom_data *data,
+				 struct ieee80211_sta_ht_cap *ht_info,
+				 enum ieee80211_band band)
+{
+	int max_bit_rate = 0;
+	u8 rx_chains;
+	u8 tx_chains;
+
+	tx_chains = hweight8(data->valid_tx_ant);
+	if (cfg->rx_with_siso_diversity)
+		rx_chains = 1;
+	else
+		rx_chains = hweight8(data->valid_rx_ant);
+
+	if (!(data->sku & EEPROM_SKU_CAP_11N_ENABLE) || !cfg->ht_params) {
+		ht_info->ht_supported = false;
+		return;
+	}
+
+	ht_info->ht_supported = true;
+	ht_info->cap = 0;
+
+	if (iwlwifi_mod_params.amsdu_size_8K)
+		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
+
+	ht_info->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+	ht_info->ampdu_density = IEEE80211_HT_MPDU_DENSITY_4;
+
+	ht_info->mcs.rx_mask[0] = 0xFF;
+	if (rx_chains >= 2)
+		ht_info->mcs.rx_mask[1] = 0xFF;
+	if (rx_chains >= 3)
+		ht_info->mcs.rx_mask[2] = 0xFF;
+
+	if (cfg->ht_params->ht_greenfield_support)
+		ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
+	ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+
+	max_bit_rate = MAX_BIT_RATE_20_MHZ;
+
+	if (cfg->ht_params->ht40_bands & 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;
+	}
+
+	/* Highest supported Rx data rate */
+	max_bit_rate *= rx_chains;
+	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 != rx_chains) {
+		ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
+		ht_info->mcs.tx_params |= ((tx_chains - 1) <<
+				IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
+	}
+}
+
+static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
+			    struct iwl_eeprom_data *data,
+			    const u8 *eeprom, size_t eeprom_size)
+{
+	int n_channels = iwl_init_channel_map(dev, cfg, data,
+					      eeprom, eeprom_size);
+	int n_used = 0;
+	struct ieee80211_supported_band *sband;
+
+	sband = &data->bands[IEEE80211_BAND_2GHZ];
+	sband->band = IEEE80211_BAND_2GHZ;
+	sband->bitrates = &iwl_cfg80211_rates[RATES_24_OFFS];
+	sband->n_bitrates = N_RATES_24;
+	n_used += iwl_init_sband_channels(data, sband, n_channels,
+					  IEEE80211_BAND_2GHZ);
+	iwl_init_ht_hw_capab(cfg, data, &sband->ht_cap, IEEE80211_BAND_2GHZ);
+
+	sband = &data->bands[IEEE80211_BAND_5GHZ];
+	sband->band = IEEE80211_BAND_5GHZ;
+	sband->bitrates = &iwl_cfg80211_rates[RATES_52_OFFS];
+	sband->n_bitrates = N_RATES_52;
+	n_used += iwl_init_sband_channels(data, sband, n_channels,
+					  IEEE80211_BAND_5GHZ);
+	iwl_init_ht_hw_capab(cfg, data, &sband->ht_cap, IEEE80211_BAND_5GHZ);
+
+	if (n_channels != n_used)
+		IWL_ERR_DEV(dev, "EEPROM: used only %d of %d channels\n",
+			    n_used, n_channels);
+}
+
+/* EEPROM data functions */
+
+struct iwl_eeprom_data *
+iwl_parse_eeprom_data(struct device *dev, const struct iwl_cfg *cfg,
+		      const u8 *eeprom, size_t eeprom_size)
+{
+	struct iwl_eeprom_data *data;
+	const void *tmp;
+
+	if (WARN_ON(!cfg || !cfg->eeprom_params))
+		return NULL;
+
+	data = kzalloc(sizeof(*data) +
+		       sizeof(struct ieee80211_channel) * IWL_NUM_CHANNELS,
+		       GFP_KERNEL);
+	if (!data)
+		return NULL;
+
+	/* get MAC address(es) */
+	tmp = iwl_eeprom_query_addr(eeprom, eeprom_size, EEPROM_MAC_ADDRESS);
+	if (!tmp)
+		goto err_free;
+	memcpy(data->hw_addr, tmp, ETH_ALEN);
+	data->n_hw_addrs = iwl_eeprom_query16(eeprom, eeprom_size,
+					      EEPROM_NUM_MAC_ADDRESS);
+
+	if (iwl_eeprom_read_calib(eeprom, eeprom_size, data))
+		goto err_free;
+
+	tmp = iwl_eeprom_query_addr(eeprom, eeprom_size, EEPROM_XTAL);
+	if (!tmp)
+		goto err_free;
+	memcpy(data->xtal_calib, tmp, sizeof(data->xtal_calib));
+
+	tmp = iwl_eeprom_query_addr(eeprom, eeprom_size,
+				    EEPROM_RAW_TEMPERATURE);
+	if (!tmp)
+		goto err_free;
+	data->raw_temperature = *(__le16 *)tmp;
+
+	tmp = iwl_eeprom_query_addr(eeprom, eeprom_size,
+				    EEPROM_KELVIN_TEMPERATURE);
+	if (!tmp)
+		goto err_free;
+	data->kelvin_temperature = *(__le16 *)tmp;
+	data->kelvin_voltage = *((__le16 *)tmp + 1);
+
+	data->radio_cfg = iwl_eeprom_query16(eeprom, eeprom_size,
+					     EEPROM_RADIO_CONFIG);
+	data->sku = iwl_eeprom_query16(eeprom, eeprom_size,
+				       EEPROM_SKU_CAP);
+	if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
+		data->sku &= ~EEPROM_SKU_CAP_11N_ENABLE;
+
+	data->eeprom_version = iwl_eeprom_query16(eeprom, eeprom_size,
+						  EEPROM_VERSION);
+
+	data->valid_tx_ant = EEPROM_RF_CFG_TX_ANT_MSK(data->radio_cfg);
+	data->valid_rx_ant = EEPROM_RF_CFG_RX_ANT_MSK(data->radio_cfg);
+
+	/* check overrides (some devices have wrong EEPROM) */
+	if (cfg->valid_tx_ant)
+		data->valid_tx_ant = cfg->valid_tx_ant;
+	if (cfg->valid_rx_ant)
+		data->valid_rx_ant = cfg->valid_rx_ant;
+
+	if (!data->valid_tx_ant || !data->valid_rx_ant) {
+		IWL_ERR_DEV(dev, "invalid antennas (0x%x, 0x%x)\n",
+			    data->valid_tx_ant, data->valid_rx_ant);
+		goto err_free;
+	}
+
+	iwl_init_sbands(dev, cfg, data, eeprom, eeprom_size);
+
+	return data;
+ err_free:
+	kfree(data);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(iwl_parse_eeprom_data);
+
+/* helper functions */
+int iwl_eeprom_check_version(struct iwl_eeprom_data *data,
+			     struct iwl_trans *trans)
+{
+	if (data->eeprom_version >= trans->cfg->eeprom_ver ||
+	    data->calib_version >= trans->cfg->eeprom_calib_ver) {
+		IWL_INFO(trans, "device EEPROM VER=0x%x, CALIB=0x%x\n",
+			 data->eeprom_version, data->calib_version);
+		return 0;
+	}
+
+	IWL_ERR(trans,
+		"Unsupported (too old) EEPROM VER=0x%x < 0x%x CALIB=0x%x < 0x%x\n",
+		data->eeprom_version, trans->cfg->eeprom_ver,
+		data->calib_version,  trans->cfg->eeprom_calib_ver);
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(iwl_eeprom_check_version);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
new file mode 100644
index 00000000000..9c07c670a1c
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
@@ -0,0 +1,138 @@
+/******************************************************************************
+ *
+ * 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 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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 - 2012 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_eeprom_parse_h__
+#define __iwl_eeprom_parse_h__
+
+#include <linux/types.h>
+#include <linux/if_ether.h>
+#include "iwl-trans.h"
+
+/* SKU Capabilities (actual values from EEPROM definition) */
+#define EEPROM_SKU_CAP_BAND_24GHZ	(1 << 4)
+#define EEPROM_SKU_CAP_BAND_52GHZ	(1 << 5)
+#define EEPROM_SKU_CAP_11N_ENABLE	(1 << 6)
+#define EEPROM_SKU_CAP_AMT_ENABLE	(1 << 7)
+#define EEPROM_SKU_CAP_IPAN_ENABLE	(1 << 8)
+
+/* radio config bits (actual values from EEPROM definition) */
+#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 */
+
+struct iwl_eeprom_data {
+	int n_hw_addrs;
+	u8 hw_addr[ETH_ALEN];
+
+	u16 radio_config;
+
+	u8 calib_version;
+	__le16 calib_voltage;
+
+	__le16 raw_temperature;
+	__le16 kelvin_temperature;
+	__le16 kelvin_voltage;
+	__le16 xtal_calib[2];
+
+	u16 sku;
+	u16 radio_cfg;
+	u16 eeprom_version;
+	s8 max_tx_pwr_half_dbm;
+
+	u8 valid_tx_ant, valid_rx_ant;
+
+	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
+	struct ieee80211_channel channels[];
+};
+
+/**
+ * iwl_parse_eeprom_data - parse EEPROM data and return values
+ *
+ * @dev: device pointer we're parsing for, for debug only
+ * @cfg: device configuration for parsing and overrides
+ * @eeprom: the EEPROM data
+ * @eeprom_size: length of the EEPROM data
+ *
+ * This function parses all EEPROM values we need and then
+ * returns a (newly allocated) struct containing all the
+ * relevant values for driver use. The struct must be freed
+ * later with iwl_free_eeprom_data().
+ */
+struct iwl_eeprom_data *
+iwl_parse_eeprom_data(struct device *dev, const struct iwl_cfg *cfg,
+		      const u8 *eeprom, size_t eeprom_size);
+
+/**
+ * iwl_free_eeprom_data - free EEPROM data
+ * @data: the data to free
+ */
+static inline void iwl_free_eeprom_data(struct iwl_eeprom_data *data)
+{
+	kfree(data);
+}
+
+int iwl_eeprom_check_version(struct iwl_eeprom_data *data,
+			     struct iwl_trans *trans);
+
+#endif /* __iwl_eeprom_parse_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
new file mode 100644
index 00000000000..27c7da3c6ed
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
@@ -0,0 +1,463 @@
+/******************************************************************************
+ *
+ * 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 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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 - 2012 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/types.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+
+#include "iwl-debug.h"
+#include "iwl-eeprom-read.h"
+#include "iwl-io.h"
+#include "iwl-prph.h"
+#include "iwl-csr.h"
+
+/*
+ * 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) */
+
+
+/*
+ * 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.
+ */
+
+#define	EEPROM_SEM_TIMEOUT 10		/* milliseconds */
+#define EEPROM_SEM_RETRY_LIMIT 1000	/* number of attempts (not time) */
+
+static int iwl_eeprom_acquire_semaphore(struct iwl_trans *trans)
+{
+	u16 count;
+	int ret;
+
+	for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
+		/* Request semaphore */
+		iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+
+		/* See if we got it */
+		ret = iwl_poll_bit(trans, 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_EEPROM(trans->dev,
+					 "Acquired semaphore after %d tries.\n",
+					 count+1);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+static void iwl_eeprom_release_semaphore(struct iwl_trans *trans)
+{
+	iwl_clear_bit(trans, CSR_HW_IF_CONFIG_REG,
+		      CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+}
+
+static int iwl_eeprom_verify_signature(struct iwl_trans *trans, bool nvm_is_otp)
+{
+	u32 gp = iwl_read32(trans, CSR_EEPROM_GP) & CSR_EEPROM_GP_VALID_MSK;
+
+	IWL_DEBUG_EEPROM(trans->dev, "EEPROM signature=0x%08x\n", gp);
+
+	switch (gp) {
+	case CSR_EEPROM_GP_BAD_SIG_EEP_GOOD_SIG_OTP:
+		if (!nvm_is_otp) {
+			IWL_ERR(trans, "EEPROM with bad signature: 0x%08x\n",
+				gp);
+			return -ENOENT;
+		}
+		return 0;
+	case CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K:
+	case CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K:
+		if (nvm_is_otp) {
+			IWL_ERR(trans, "OTP with bad signature: 0x%08x\n", gp);
+			return -ENOENT;
+		}
+		return 0;
+	case CSR_EEPROM_GP_BAD_SIGNATURE_BOTH_EEP_AND_OTP:
+	default:
+		IWL_ERR(trans,
+			"bad EEPROM/OTP signature, type=%s, EEPROM_GP=0x%08x\n",
+			nvm_is_otp ? "OTP" : "EEPROM", gp);
+		return -ENOENT;
+	}
+}
+
+/******************************************************************************
+ *
+ * OTP related functions
+ *
+******************************************************************************/
+
+static void iwl_set_otp_access_absolute(struct iwl_trans *trans)
+{
+	iwl_read32(trans, CSR_OTP_GP_REG);
+
+	iwl_clear_bit(trans, CSR_OTP_GP_REG,
+		      CSR_OTP_GP_REG_OTP_ACCESS_MODE);
+}
+
+static int iwl_nvm_is_otp(struct iwl_trans *trans)
+{
+	u32 otpgp;
+
+	/* OTP only valid for CP/PP and after */
+	switch (trans->hw_rev & CSR_HW_REV_TYPE_MSK) {
+	case CSR_HW_REV_TYPE_NONE:
+		IWL_ERR(trans, "Unknown hardware type\n");
+		return -EIO;
+	case CSR_HW_REV_TYPE_5300:
+	case CSR_HW_REV_TYPE_5350:
+	case CSR_HW_REV_TYPE_5100:
+	case CSR_HW_REV_TYPE_5150:
+		return 0;
+	default:
+		otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
+		if (otpgp & CSR_OTP_GP_REG_DEVICE_SELECT)
+			return 1;
+		return 0;
+	}
+}
+
+static int iwl_init_otp_access(struct iwl_trans *trans)
+{
+	int ret;
+
+	/* Enable 40MHz radio clock */
+	iwl_write32(trans, CSR_GP_CNTRL,
+		    iwl_read32(trans, CSR_GP_CNTRL) |
+		    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+	/* wait for clock to be ready */
+	ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
+			   CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			   CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			   25000);
+	if (ret < 0) {
+		IWL_ERR(trans, "Time out access OTP\n");
+	} else {
+		iwl_set_bits_prph(trans, APMG_PS_CTRL_REG,
+				  APMG_PS_CTRL_VAL_RESET_REQ);
+		udelay(5);
+		iwl_clear_bits_prph(trans, APMG_PS_CTRL_REG,
+				    APMG_PS_CTRL_VAL_RESET_REQ);
+
+		/*
+		 * CSR auto clock gate disable bit -
+		 * this is only applicable for HW with OTP shadow RAM
+		 */
+		if (trans->cfg->base_params->shadow_ram_support)
+			iwl_set_bit(trans, CSR_DBG_LINK_PWR_MGMT_REG,
+				    CSR_RESET_LINK_PWR_MGMT_DISABLED);
+	}
+	return ret;
+}
+
+static int iwl_read_otp_word(struct iwl_trans *trans, u16 addr,
+			     __le16 *eeprom_data)
+{
+	int ret = 0;
+	u32 r;
+	u32 otpgp;
+
+	iwl_write32(trans, CSR_EEPROM_REG,
+		    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
+	ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
+				 CSR_EEPROM_REG_READ_VALID_MSK,
+				 CSR_EEPROM_REG_READ_VALID_MSK,
+				 IWL_EEPROM_ACCESS_TIMEOUT);
+	if (ret < 0) {
+		IWL_ERR(trans, "Time out reading OTP[%d]\n", addr);
+		return ret;
+	}
+	r = iwl_read32(trans, CSR_EEPROM_REG);
+	/* check for ECC errors: */
+	otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
+	if (otpgp & CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK) {
+		/* stop in this case */
+		/* set the uncorrectable OTP ECC bit for acknowledgement */
+		iwl_set_bit(trans, CSR_OTP_GP_REG,
+			    CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
+		IWL_ERR(trans, "Uncorrectable OTP ECC error, abort OTP read\n");
+		return -EINVAL;
+	}
+	if (otpgp & CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK) {
+		/* continue in this case */
+		/* set the correctable OTP ECC bit for acknowledgement */
+		iwl_set_bit(trans, CSR_OTP_GP_REG,
+			    CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK);
+		IWL_ERR(trans, "Correctable OTP ECC error, continue read\n");
+	}
+	*eeprom_data = cpu_to_le16(r >> 16);
+	return 0;
+}
+
+/*
+ * iwl_is_otp_empty: check for empty OTP
+ */
+static bool iwl_is_otp_empty(struct iwl_trans *trans)
+{
+	u16 next_link_addr = 0;
+	__le16 link_value;
+	bool is_empty = false;
+
+	/* locate the beginning of OTP link list */
+	if (!iwl_read_otp_word(trans, next_link_addr, &link_value)) {
+		if (!link_value) {
+			IWL_ERR(trans, "OTP is empty\n");
+			is_empty = true;
+		}
+	} else {
+		IWL_ERR(trans, "Unable to read first block of OTP list.\n");
+		is_empty = true;
+	}
+
+	return is_empty;
+}
+
+
+/*
+ * iwl_find_otp_image: find EEPROM image in OTP
+ *   finding the OTP block that contains the EEPROM image.
+ *   the last valid block on the link list (the block _before_ the last block)
+ *   is the block we should read and used to configure the device.
+ *   If all the available OTP blocks are full, the last block will be the block
+ *   we should read and used to configure the device.
+ *   only perform this operation if shadow RAM is disabled
+ */
+static int iwl_find_otp_image(struct iwl_trans *trans,
+					u16 *validblockaddr)
+{
+	u16 next_link_addr = 0, valid_addr;
+	__le16 link_value = 0;
+	int usedblocks = 0;
+
+	/* set addressing mode to absolute to traverse the link list */
+	iwl_set_otp_access_absolute(trans);
+
+	/* checking for empty OTP or error */
+	if (iwl_is_otp_empty(trans))
+		return -EINVAL;
+
+	/*
+	 * start traverse link list
+	 * until reach the max number of OTP blocks
+	 * different devices have different number of OTP blocks
+	 */
+	do {
+		/* save current valid block address
+		 * check for more block on the link list
+		 */
+		valid_addr = next_link_addr;
+		next_link_addr = le16_to_cpu(link_value) * sizeof(u16);
+		IWL_DEBUG_EEPROM(trans->dev, "OTP blocks %d addr 0x%x\n",
+				 usedblocks, next_link_addr);
+		if (iwl_read_otp_word(trans, next_link_addr, &link_value))
+			return -EINVAL;
+		if (!link_value) {
+			/*
+			 * reach the end of link list, return success and
+			 * set address point to the starting address
+			 * of the image
+			 */
+			*validblockaddr = valid_addr;
+			/* skip first 2 bytes (link list pointer) */
+			*validblockaddr += 2;
+			return 0;
+		}
+		/* more in the link list, continue */
+		usedblocks++;
+	} while (usedblocks <= trans->cfg->base_params->max_ll_items);
+
+	/* OTP has no valid blocks */
+	IWL_DEBUG_EEPROM(trans->dev, "OTP has no valid blocks\n");
+	return -EINVAL;
+}
+
+/**
+ * iwl_read_eeprom - read EEPROM contents
+ *
+ * Load the EEPROM contents from adapter and return it
+ * and its size.
+ *
+ * NOTE:  This routine uses the non-debug IO access functions.
+ */
+int iwl_read_eeprom(struct iwl_trans *trans, u8 **eeprom, size_t *eeprom_size)
+{
+	__le16 *e;
+	u32 gp = iwl_read32(trans, CSR_EEPROM_GP);
+	int sz;
+	int ret;
+	u16 addr;
+	u16 validblockaddr = 0;
+	u16 cache_addr = 0;
+	int nvm_is_otp;
+
+	if (!eeprom || !eeprom_size)
+		return -EINVAL;
+
+	nvm_is_otp = iwl_nvm_is_otp(trans);
+	if (nvm_is_otp < 0)
+		return nvm_is_otp;
+
+	sz = trans->cfg->base_params->eeprom_size;
+	IWL_DEBUG_EEPROM(trans->dev, "NVM size = %d\n", sz);
+
+	e = kmalloc(sz, GFP_KERNEL);
+	if (!e)
+		return -ENOMEM;
+
+	ret = iwl_eeprom_verify_signature(trans, nvm_is_otp);
+	if (ret < 0) {
+		IWL_ERR(trans, "EEPROM not found, EEPROM_GP=0x%08x\n", gp);
+		goto err_free;
+	}
+
+	/* Make sure driver (instead of uCode) is allowed to read EEPROM */
+	ret = iwl_eeprom_acquire_semaphore(trans);
+	if (ret < 0) {
+		IWL_ERR(trans, "Failed to acquire EEPROM semaphore.\n");
+		goto err_free;
+	}
+
+	if (nvm_is_otp) {
+		ret = iwl_init_otp_access(trans);
+		if (ret) {
+			IWL_ERR(trans, "Failed to initialize OTP access.\n");
+			goto err_unlock;
+		}
+
+		iwl_write32(trans, CSR_EEPROM_GP,
+			    iwl_read32(trans, CSR_EEPROM_GP) &
+			    ~CSR_EEPROM_GP_IF_OWNER_MSK);
+
+		iwl_set_bit(trans, CSR_OTP_GP_REG,
+			    CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK |
+			    CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
+		/* traversing the linked list if no shadow ram supported */
+		if (!trans->cfg->base_params->shadow_ram_support) {
+			ret = iwl_find_otp_image(trans, &validblockaddr);
+			if (ret)
+				goto err_unlock;
+		}
+		for (addr = validblockaddr; addr < validblockaddr + sz;
+		     addr += sizeof(u16)) {
+			__le16 eeprom_data;
+
+			ret = iwl_read_otp_word(trans, addr, &eeprom_data);
+			if (ret)
+				goto err_unlock;
+			e[cache_addr / 2] = eeprom_data;
+			cache_addr += sizeof(u16);
+		}
+	} else {
+		/* eeprom is an array of 16bit values */
+		for (addr = 0; addr < sz; addr += sizeof(u16)) {
+			u32 r;
+
+			iwl_write32(trans, CSR_EEPROM_REG,
+				    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
+
+			ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
+					   CSR_EEPROM_REG_READ_VALID_MSK,
+					   CSR_EEPROM_REG_READ_VALID_MSK,
+					   IWL_EEPROM_ACCESS_TIMEOUT);
+			if (ret < 0) {
+				IWL_ERR(trans,
+					"Time out reading EEPROM[%d]\n", addr);
+				goto err_unlock;
+			}
+			r = iwl_read32(trans, CSR_EEPROM_REG);
+			e[addr / 2] = cpu_to_le16(r >> 16);
+		}
+	}
+
+	IWL_DEBUG_EEPROM(trans->dev, "NVM Type: %s\n",
+			 nvm_is_otp ? "OTP" : "EEPROM");
+
+	iwl_eeprom_release_semaphore(trans);
+
+	*eeprom_size = sz;
+	*eeprom = (u8 *)e;
+	return 0;
+
+ err_unlock:
+	iwl_eeprom_release_semaphore(trans);
+ err_free:
+	kfree(e);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iwl_read_eeprom);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
new file mode 100644
index 00000000000..1337c9d36fe
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
@@ -0,0 +1,70 @@
+/******************************************************************************
+ *
+ * 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 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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 - 2012 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_eeprom_h__
+#define __iwl_eeprom_h__
+
+#include "iwl-trans.h"
+
+int iwl_read_eeprom(struct iwl_trans *trans, u8 **eeprom, size_t *eeprom_size);
+
+#endif  /* __iwl_eeprom_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
deleted file mode 100644
index b8e2b223ac3..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ /dev/null
@@ -1,1148 +0,0 @@
-/******************************************************************************
- *
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- * redistributing this file, you may do so under either license.
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2008 - 2012 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You 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 - 2012 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-dev.h"
-#include "iwl-debug.h"
-#include "iwl-agn.h"
-#include "iwl-eeprom.h"
-#include "iwl-io.h"
-#include "iwl-prph.h"
-
-/************************** EEPROM BANDS ****************************
- *
- * The iwl_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 iwl_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 iwl_eeprom_band_2[] = {	/* 4915-5080MHz */
-	183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
-};
-
-static const u8 iwl_eeprom_band_3[] = {	/* 5170-5320MHz */
-	34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
-};
-
-static const u8 iwl_eeprom_band_4[] = {	/* 5500-5700MHz */
-	100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
-};
-
-static const u8 iwl_eeprom_band_5[] = {	/* 5725-5825MHz */
-	145, 149, 153, 157, 161, 165
-};
-
-static const u8 iwl_eeprom_band_6[] = {       /* 2.4 ht40 channel */
-	1, 2, 3, 4, 5, 6, 7
-};
-
-static const u8 iwl_eeprom_band_7[] = {       /* 5.2 ht40 channel */
-	36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157
-};
-
-/******************************************************************************
- *
- * generic NVM 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.
- */
-
-#define	EEPROM_SEM_TIMEOUT 10		/* milliseconds */
-#define EEPROM_SEM_RETRY_LIMIT 1000	/* number of attempts (not time) */
-
-static int iwl_eeprom_acquire_semaphore(struct iwl_trans *trans)
-{
-	u16 count;
-	int ret;
-
-	for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
-		/* Request semaphore */
-		iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
-			    CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
-
-		/* See if we got it */
-		ret = iwl_poll_bit(trans, 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_EEPROM(trans,
-				"Acquired semaphore after %d tries.\n",
-				count+1);
-			return ret;
-		}
-	}
-
-	return ret;
-}
-
-static void iwl_eeprom_release_semaphore(struct iwl_trans *trans)
-{
-	iwl_clear_bit(trans, CSR_HW_IF_CONFIG_REG,
-		CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
-
-}
-
-static int iwl_eeprom_verify_signature(struct iwl_priv *priv)
-{
-	u32 gp = iwl_read32(priv->trans, 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_BAD_SIG_EEP_GOOD_SIG_OTP:
-		if (priv->nvm_device_type != NVM_DEVICE_TYPE_OTP) {
-			IWL_ERR(priv, "EEPROM with bad signature: 0x%08x\n",
-				gp);
-			ret = -ENOENT;
-		}
-		break;
-	case CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K:
-	case CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K:
-		if (priv->nvm_device_type != NVM_DEVICE_TYPE_EEPROM) {
-			IWL_ERR(priv, "OTP with bad signature: 0x%08x\n", gp);
-			ret = -ENOENT;
-		}
-		break;
-	case CSR_EEPROM_GP_BAD_SIGNATURE_BOTH_EEP_AND_OTP:
-	default:
-		IWL_ERR(priv, "bad EEPROM/OTP signature, type=%s, "
-			"EEPROM_GP=0x%08x\n",
-			(priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
-			? "OTP" : "EEPROM", gp);
-		ret = -ENOENT;
-		break;
-	}
-	return ret;
-}
-
-u16 iwl_eeprom_query16(struct iwl_priv *priv, size_t offset)
-{
-	if (!priv->eeprom)
-		return 0;
-	return (u16)priv->eeprom[offset] | ((u16)priv->eeprom[offset + 1] << 8);
-}
-
-int iwl_eeprom_check_version(struct iwl_priv *priv)
-{
-	u16 eeprom_ver;
-	u16 calib_ver;
-
-	eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
-	calib_ver = iwl_eeprom_calib_version(priv);
-
-	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;
-
-}
-
-int iwl_eeprom_init_hw_params(struct iwl_priv *priv)
-{
-	u16 radio_cfg;
-
-	priv->hw_params.sku = iwl_eeprom_query16(priv, EEPROM_SKU_CAP);
-	if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE &&
-	    !priv->cfg->ht_params) {
-		IWL_ERR(priv, "Invalid 11n configuration\n");
-		return -EINVAL;
-	}
-
-	if (!priv->hw_params.sku) {
-		IWL_ERR(priv, "Invalid device sku\n");
-		return -EINVAL;
-	}
-
-	IWL_INFO(priv, "Device SKU: 0x%X\n", priv->hw_params.sku);
-
-	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
-
-	priv->hw_params.valid_tx_ant = EEPROM_RF_CFG_TX_ANT_MSK(radio_cfg);
-	priv->hw_params.valid_rx_ant = EEPROM_RF_CFG_RX_ANT_MSK(radio_cfg);
-
-	/* check overrides (some devices have wrong EEPROM) */
-	if (priv->cfg->valid_tx_ant)
-		priv->hw_params.valid_tx_ant = priv->cfg->valid_tx_ant;
-	if (priv->cfg->valid_rx_ant)
-		priv->hw_params.valid_rx_ant = priv->cfg->valid_rx_ant;
-
-	if (!priv->hw_params.valid_tx_ant || !priv->hw_params.valid_rx_ant) {
-		IWL_ERR(priv, "Invalid chain (0x%X, 0x%X)\n",
-			priv->hw_params.valid_tx_ant,
-			priv->hw_params.valid_rx_ant);
-		return -EINVAL;
-	}
-
-	IWL_INFO(priv, "Valid Tx ant: 0x%X, Valid Rx ant: 0x%X\n",
-		 priv->hw_params.valid_tx_ant, priv->hw_params.valid_rx_ant);
-
-	return 0;
-}
-
-u16 iwl_eeprom_calib_version(struct iwl_priv *priv)
-{
-	struct iwl_eeprom_calib_hdr *hdr;
-
-	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
-							EEPROM_CALIB_ALL);
-	return hdr->version;
-}
-
-static u32 eeprom_indirect_address(struct iwl_priv *priv, u32 address)
-{
-	u16 offset = 0;
-
-	if ((address & INDIRECT_ADDRESS) == 0)
-		return address;
-
-	switch (address & INDIRECT_TYPE_MSK) {
-	case INDIRECT_HOST:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_HOST);
-		break;
-	case INDIRECT_GENERAL:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_GENERAL);
-		break;
-	case INDIRECT_REGULATORY:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_REGULATORY);
-		break;
-	case INDIRECT_TXP_LIMIT:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_TXP_LIMIT);
-		break;
-	case INDIRECT_TXP_LIMIT_SIZE:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_TXP_LIMIT_SIZE);
-		break;
-	case INDIRECT_CALIBRATION:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_CALIBRATION);
-		break;
-	case INDIRECT_PROCESS_ADJST:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_PROCESS_ADJST);
-		break;
-	case INDIRECT_OTHERS:
-		offset = iwl_eeprom_query16(priv, EEPROM_LINK_OTHERS);
-		break;
-	default:
-		IWL_ERR(priv, "illegal indirect type: 0x%X\n",
-		address & INDIRECT_TYPE_MSK);
-		break;
-	}
-
-	/* translate the offset from words to byte */
-	return (address & ADDRESS_MSK) + (offset << 1);
-}
-
-const u8 *iwl_eeprom_query_addr(struct iwl_priv *priv, size_t offset)
-{
-	u32 address = eeprom_indirect_address(priv, offset);
-	BUG_ON(address >= priv->cfg->base_params->eeprom_size);
-	return &priv->eeprom[address];
-}
-
-void iwl_eeprom_get_mac(struct iwl_priv *priv, u8 *mac)
-{
-	const u8 *addr = iwl_eeprom_query_addr(priv,
-					EEPROM_MAC_ADDRESS);
-	memcpy(mac, addr, ETH_ALEN);
-}
-
-/******************************************************************************
- *
- * OTP related functions
- *
-******************************************************************************/
-
-static void iwl_set_otp_access(struct iwl_trans *trans,
-			       enum iwl_access_mode mode)
-{
-	iwl_read32(trans, CSR_OTP_GP_REG);
-
-	if (mode == IWL_OTP_ACCESS_ABSOLUTE)
-		iwl_clear_bit(trans, CSR_OTP_GP_REG,
-			      CSR_OTP_GP_REG_OTP_ACCESS_MODE);
-	else
-		iwl_set_bit(trans, CSR_OTP_GP_REG,
-			    CSR_OTP_GP_REG_OTP_ACCESS_MODE);
-}
-
-static int iwl_get_nvm_type(struct iwl_trans *trans, u32 hw_rev)
-{
-	u32 otpgp;
-	int nvm_type;
-
-	/* OTP only valid for CP/PP and after */
-	switch (hw_rev & CSR_HW_REV_TYPE_MSK) {
-	case CSR_HW_REV_TYPE_NONE:
-		IWL_ERR(trans, "Unknown hardware type\n");
-		return -ENOENT;
-	case CSR_HW_REV_TYPE_5300:
-	case CSR_HW_REV_TYPE_5350:
-	case CSR_HW_REV_TYPE_5100:
-	case CSR_HW_REV_TYPE_5150:
-		nvm_type = NVM_DEVICE_TYPE_EEPROM;
-		break;
-	default:
-		otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
-		if (otpgp & CSR_OTP_GP_REG_DEVICE_SELECT)
-			nvm_type = NVM_DEVICE_TYPE_OTP;
-		else
-			nvm_type = NVM_DEVICE_TYPE_EEPROM;
-		break;
-	}
-	return  nvm_type;
-}
-
-static int iwl_init_otp_access(struct iwl_trans *trans)
-{
-	int ret;
-
-	/* Enable 40MHz radio clock */
-	iwl_write32(trans, CSR_GP_CNTRL,
-		    iwl_read32(trans, CSR_GP_CNTRL) |
-		    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
-
-	/* wait for clock to be ready */
-	ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
-				 CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
-				 CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
-				 25000);
-	if (ret < 0)
-		IWL_ERR(trans, "Time out access OTP\n");
-	else {
-		iwl_set_bits_prph(trans, APMG_PS_CTRL_REG,
-				  APMG_PS_CTRL_VAL_RESET_REQ);
-		udelay(5);
-		iwl_clear_bits_prph(trans, APMG_PS_CTRL_REG,
-				    APMG_PS_CTRL_VAL_RESET_REQ);
-
-		/*
-		 * CSR auto clock gate disable bit -
-		 * this is only applicable for HW with OTP shadow RAM
-		 */
-		if (trans->cfg->base_params->shadow_ram_support)
-			iwl_set_bit(trans, CSR_DBG_LINK_PWR_MGMT_REG,
-				CSR_RESET_LINK_PWR_MGMT_DISABLED);
-	}
-	return ret;
-}
-
-static int iwl_read_otp_word(struct iwl_trans *trans, u16 addr,
-			     __le16 *eeprom_data)
-{
-	int ret = 0;
-	u32 r;
-	u32 otpgp;
-
-	iwl_write32(trans, CSR_EEPROM_REG,
-		    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
-	ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
-				 CSR_EEPROM_REG_READ_VALID_MSK,
-				 CSR_EEPROM_REG_READ_VALID_MSK,
-				 IWL_EEPROM_ACCESS_TIMEOUT);
-	if (ret < 0) {
-		IWL_ERR(trans, "Time out reading OTP[%d]\n", addr);
-		return ret;
-	}
-	r = iwl_read32(trans, CSR_EEPROM_REG);
-	/* check for ECC errors: */
-	otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
-	if (otpgp & CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK) {
-		/* stop in this case */
-		/* set the uncorrectable OTP ECC bit for acknowledgement */
-		iwl_set_bit(trans, CSR_OTP_GP_REG,
-			CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
-		IWL_ERR(trans, "Uncorrectable OTP ECC error, abort OTP read\n");
-		return -EINVAL;
-	}
-	if (otpgp & CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK) {
-		/* continue in this case */
-		/* set the correctable OTP ECC bit for acknowledgement */
-		iwl_set_bit(trans, CSR_OTP_GP_REG,
-				CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK);
-		IWL_ERR(trans, "Correctable OTP ECC error, continue read\n");
-	}
-	*eeprom_data = cpu_to_le16(r >> 16);
-	return 0;
-}
-
-/*
- * iwl_is_otp_empty: check for empty OTP
- */
-static bool iwl_is_otp_empty(struct iwl_trans *trans)
-{
-	u16 next_link_addr = 0;
-	__le16 link_value;
-	bool is_empty = false;
-
-	/* locate the beginning of OTP link list */
-	if (!iwl_read_otp_word(trans, next_link_addr, &link_value)) {
-		if (!link_value) {
-			IWL_ERR(trans, "OTP is empty\n");
-			is_empty = true;
-		}
-	} else {
-		IWL_ERR(trans, "Unable to read first block of OTP list.\n");
-		is_empty = true;
-	}
-
-	return is_empty;
-}
-
-
-/*
- * iwl_find_otp_image: find EEPROM image in OTP
- *   finding the OTP block that contains the EEPROM image.
- *   the last valid block on the link list (the block _before_ the last block)
- *   is the block we should read and used to configure the device.
- *   If all the available OTP blocks are full, the last block will be the block
- *   we should read and used to configure the device.
- *   only perform this operation if shadow RAM is disabled
- */
-static int iwl_find_otp_image(struct iwl_trans *trans,
-					u16 *validblockaddr)
-{
-	u16 next_link_addr = 0, valid_addr;
-	__le16 link_value = 0;
-	int usedblocks = 0;
-
-	/* set addressing mode to absolute to traverse the link list */
-	iwl_set_otp_access(trans, IWL_OTP_ACCESS_ABSOLUTE);
-
-	/* checking for empty OTP or error */
-	if (iwl_is_otp_empty(trans))
-		return -EINVAL;
-
-	/*
-	 * start traverse link list
-	 * until reach the max number of OTP blocks
-	 * different devices have different number of OTP blocks
-	 */
-	do {
-		/* save current valid block address
-		 * check for more block on the link list
-		 */
-		valid_addr = next_link_addr;
-		next_link_addr = le16_to_cpu(link_value) * sizeof(u16);
-		IWL_DEBUG_EEPROM(trans, "OTP blocks %d addr 0x%x\n",
-			       usedblocks, next_link_addr);
-		if (iwl_read_otp_word(trans, next_link_addr, &link_value))
-			return -EINVAL;
-		if (!link_value) {
-			/*
-			 * reach the end of link list, return success and
-			 * set address point to the starting address
-			 * of the image
-			 */
-			*validblockaddr = valid_addr;
-			/* skip first 2 bytes (link list pointer) */
-			*validblockaddr += 2;
-			return 0;
-		}
-		/* more in the link list, continue */
-		usedblocks++;
-	} while (usedblocks <= trans->cfg->base_params->max_ll_items);
-
-	/* OTP has no valid blocks */
-	IWL_DEBUG_EEPROM(trans, "OTP has no valid blocks\n");
-	return -EINVAL;
-}
-
-/******************************************************************************
- *
- * Tx Power related functions
- *
-******************************************************************************/
-/**
- * iwl_get_max_txpower_avg - get the highest tx power from all chains.
- *     find the highest tx power from all chains for the channel
- */
-static s8 iwl_get_max_txpower_avg(struct iwl_priv *priv,
-		struct iwl_eeprom_enhanced_txpwr *enhanced_txpower,
-		int element, s8 *max_txpower_in_half_dbm)
-{
-	s8 max_txpower_avg = 0; /* (dBm) */
-
-	/* Take the highest tx power from any valid chains */
-	if ((priv->hw_params.valid_tx_ant & ANT_A) &&
-	    (enhanced_txpower[element].chain_a_max > max_txpower_avg))
-		max_txpower_avg = enhanced_txpower[element].chain_a_max;
-	if ((priv->hw_params.valid_tx_ant & ANT_B) &&
-	    (enhanced_txpower[element].chain_b_max > max_txpower_avg))
-		max_txpower_avg = enhanced_txpower[element].chain_b_max;
-	if ((priv->hw_params.valid_tx_ant & ANT_C) &&
-	    (enhanced_txpower[element].chain_c_max > max_txpower_avg))
-		max_txpower_avg = enhanced_txpower[element].chain_c_max;
-	if (((priv->hw_params.valid_tx_ant == ANT_AB) |
-	    (priv->hw_params.valid_tx_ant == ANT_BC) |
-	    (priv->hw_params.valid_tx_ant == ANT_AC)) &&
-	    (enhanced_txpower[element].mimo2_max > max_txpower_avg))
-		max_txpower_avg =  enhanced_txpower[element].mimo2_max;
-	if ((priv->hw_params.valid_tx_ant == ANT_ABC) &&
-	    (enhanced_txpower[element].mimo3_max > max_txpower_avg))
-		max_txpower_avg = enhanced_txpower[element].mimo3_max;
-
-	/*
-	 * max. tx power in EEPROM is in 1/2 dBm format
-	 * convert from 1/2 dBm to dBm (round-up convert)
-	 * but we also do not want to loss 1/2 dBm resolution which
-	 * will impact performance
-	 */
-	*max_txpower_in_half_dbm = max_txpower_avg;
-	return (max_txpower_avg & 0x01) + (max_txpower_avg >> 1);
-}
-
-static void
-iwl_eeprom_enh_txp_read_element(struct iwl_priv *priv,
-				    struct iwl_eeprom_enhanced_txpwr *txp,
-				    s8 max_txpower_avg)
-{
-	int ch_idx;
-	bool is_ht40 = txp->flags & IWL_EEPROM_ENH_TXP_FL_40MHZ;
-	enum ieee80211_band band;
-
-	band = txp->flags & IWL_EEPROM_ENH_TXP_FL_BAND_52G ?
-		IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
-
-	for (ch_idx = 0; ch_idx < priv->channel_count; ch_idx++) {
-		struct iwl_channel_info *ch_info = &priv->channel_info[ch_idx];
-
-		/* update matching channel or from common data only */
-		if (txp->channel != 0 && ch_info->channel != txp->channel)
-			continue;
-
-		/* update matching band only */
-		if (band != ch_info->band)
-			continue;
-
-		if (ch_info->max_power_avg < max_txpower_avg && !is_ht40) {
-			ch_info->max_power_avg = max_txpower_avg;
-			ch_info->curr_txpow = max_txpower_avg;
-			ch_info->scan_power = max_txpower_avg;
-		}
-
-		if (is_ht40 && ch_info->ht40_max_power_avg < max_txpower_avg)
-			ch_info->ht40_max_power_avg = max_txpower_avg;
-	}
-}
-
-#define EEPROM_TXP_OFFS	(0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT)
-#define EEPROM_TXP_ENTRY_LEN sizeof(struct iwl_eeprom_enhanced_txpwr)
-#define EEPROM_TXP_SZ_OFFS (0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT_SIZE)
-
-#define TXP_CHECK_AND_PRINT(x) ((txp->flags & IWL_EEPROM_ENH_TXP_FL_##x) \
-			    ? # x " " : "")
-
-static void iwl_eeprom_enhanced_txpower(struct iwl_priv *priv)
-{
-	struct iwl_eeprom_enhanced_txpwr *txp_array, *txp;
-	int idx, entries;
-	__le16 *txp_len;
-	s8 max_txp_avg, max_txp_avg_halfdbm;
-
-	BUILD_BUG_ON(sizeof(struct iwl_eeprom_enhanced_txpwr) != 8);
-
-	/* the length is in 16-bit words, but we want entries */
-	txp_len = (__le16 *) iwl_eeprom_query_addr(priv, EEPROM_TXP_SZ_OFFS);
-	entries = le16_to_cpup(txp_len) * 2 / EEPROM_TXP_ENTRY_LEN;
-
-	txp_array = (void *) iwl_eeprom_query_addr(priv, EEPROM_TXP_OFFS);
-
-	for (idx = 0; idx < entries; idx++) {
-		txp = &txp_array[idx];
-		/* skip invalid entries */
-		if (!(txp->flags & IWL_EEPROM_ENH_TXP_FL_VALID))
-			continue;
-
-		IWL_DEBUG_EEPROM(priv, "%s %d:\t %s%s%s%s%s%s%s%s (0x%02x)\n",
-				 (txp->channel && (txp->flags &
-					IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE)) ?
-					"Common " : (txp->channel) ?
-					"Channel" : "Common",
-				 (txp->channel),
-				 TXP_CHECK_AND_PRINT(VALID),
-				 TXP_CHECK_AND_PRINT(BAND_52G),
-				 TXP_CHECK_AND_PRINT(OFDM),
-				 TXP_CHECK_AND_PRINT(40MHZ),
-				 TXP_CHECK_AND_PRINT(HT_AP),
-				 TXP_CHECK_AND_PRINT(RES1),
-				 TXP_CHECK_AND_PRINT(RES2),
-				 TXP_CHECK_AND_PRINT(COMMON_TYPE),
-				 txp->flags);
-		IWL_DEBUG_EEPROM(priv, "\t\t chain_A: 0x%02x "
-				 "chain_B: 0X%02x chain_C: 0X%02x\n",
-				 txp->chain_a_max, txp->chain_b_max,
-				 txp->chain_c_max);
-		IWL_DEBUG_EEPROM(priv, "\t\t MIMO2: 0x%02x "
-				 "MIMO3: 0x%02x High 20_on_40: 0x%02x "
-				 "Low 20_on_40: 0x%02x\n",
-				 txp->mimo2_max, txp->mimo3_max,
-				 ((txp->delta_20_in_40 & 0xf0) >> 4),
-				 (txp->delta_20_in_40 & 0x0f));
-
-		max_txp_avg = iwl_get_max_txpower_avg(priv, txp_array, idx,
-						      &max_txp_avg_halfdbm);
-
-		/*
-		 * Update the user limit values values to the highest
-		 * power supported by any channel
-		 */
-		if (max_txp_avg > priv->tx_power_user_lmt)
-			priv->tx_power_user_lmt = max_txp_avg;
-		if (max_txp_avg_halfdbm > priv->tx_power_lmt_in_half_dbm)
-			priv->tx_power_lmt_in_half_dbm = max_txp_avg_halfdbm;
-
-		iwl_eeprom_enh_txp_read_element(priv, txp, max_txp_avg);
-	}
-}
-
-/**
- * iwl_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_eeprom_init(struct iwl_priv *priv, u32 hw_rev)
-{
-	__le16 *e;
-	u32 gp = iwl_read32(priv->trans, CSR_EEPROM_GP);
-	int sz;
-	int ret;
-	u16 addr;
-	u16 validblockaddr = 0;
-	u16 cache_addr = 0;
-
-	priv->nvm_device_type = iwl_get_nvm_type(priv->trans, hw_rev);
-	if (priv->nvm_device_type == -ENOENT)
-		return -ENOENT;
-	/* 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;
-
-	ret = iwl_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 = iwl_eeprom_acquire_semaphore(priv->trans);
-	if (ret < 0) {
-		IWL_ERR(priv, "Failed to acquire EEPROM semaphore.\n");
-		ret = -ENOENT;
-		goto err;
-	}
-
-	if (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP) {
-
-		ret = iwl_init_otp_access(priv->trans);
-		if (ret) {
-			IWL_ERR(priv, "Failed to initialize OTP access.\n");
-			ret = -ENOENT;
-			goto done;
-		}
-		iwl_write32(priv->trans, CSR_EEPROM_GP,
-			    iwl_read32(priv->trans, CSR_EEPROM_GP) &
-			    ~CSR_EEPROM_GP_IF_OWNER_MSK);
-
-		iwl_set_bit(priv->trans, CSR_OTP_GP_REG,
-			     CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK |
-			     CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
-		/* traversing the linked list if no shadow ram supported */
-		if (!priv->cfg->base_params->shadow_ram_support) {
-			if (iwl_find_otp_image(priv->trans, &validblockaddr)) {
-				ret = -ENOENT;
-				goto done;
-			}
-		}
-		for (addr = validblockaddr; addr < validblockaddr + sz;
-		     addr += sizeof(u16)) {
-			__le16 eeprom_data;
-
-			ret = iwl_read_otp_word(priv->trans, addr,
-						&eeprom_data);
-			if (ret)
-				goto done;
-			e[cache_addr / 2] = eeprom_data;
-			cache_addr += sizeof(u16);
-		}
-	} else {
-		/* eeprom is an array of 16bit values */
-		for (addr = 0; addr < sz; addr += sizeof(u16)) {
-			u32 r;
-
-			iwl_write32(priv->trans, CSR_EEPROM_REG,
-				    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
-
-			ret = iwl_poll_bit(priv->trans, 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_read32(priv->trans, CSR_EEPROM_REG);
-			e[addr / 2] = cpu_to_le16(r >> 16);
-		}
-	}
-
-	IWL_DEBUG_EEPROM(priv, "NVM Type: %s, version: 0x%x\n",
-		       (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
-		       ? "OTP" : "EEPROM",
-		       iwl_eeprom_query16(priv, EEPROM_VERSION));
-
-	ret = 0;
-done:
-	iwl_eeprom_release_semaphore(priv->trans);
-
-err:
-	if (ret)
-		iwl_eeprom_free(priv);
-alloc_err:
-	return ret;
-}
-
-void iwl_eeprom_free(struct iwl_priv *priv)
-{
-	kfree(priv->eeprom);
-	priv->eeprom = NULL;
-}
-
-static void iwl_init_band_reference(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->lib->
-			eeprom_ops.regulatory_bands[eep_band - 1];
-	switch (eep_band) {
-	case 1:		/* 2.4GHz band */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_1;
-		break;
-	case 2:		/* 4.9GHz band */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_2;
-		break;
-	case 3:		/* 5.2GHz band */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_3;
-		break;
-	case 4:		/* 5.5GHz band */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_4;
-		break;
-	case 5:		/* 5.7GHz band */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_5;
-		break;
-	case 6:		/* 2.4GHz ht40 channels */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_6;
-		break;
-	case 7:		/* 5 GHz ht40 channels */
-		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
-		*eeprom_ch_info = (struct iwl_eeprom_channel *)
-				iwl_eeprom_query_addr(priv, offset);
-		*eeprom_ch_index = iwl_eeprom_band_7;
-		break;
-	default:
-		BUG();
-		return;
-	}
-}
-
-#define CHECK_AND_PRINT(x) ((eeprom_ch->flags & EEPROM_CHANNEL_##x) \
-			    ? # x " " : "")
-/**
- * iwl_mod_ht40_chan_info - Copy ht40 channel info into driver's priv.
- *
- * Does not set up a command, or touch hardware.
- */
-static int iwl_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_get_channel_info(priv, band, channel);
-
-	if (!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,
-			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_init_channel_map - Set up driver's info for all possible channels
- */
-int iwl_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(iwl_eeprom_band_1) +
-	    ARRAY_SIZE(iwl_eeprom_band_2) +
-	    ARRAY_SIZE(iwl_eeprom_band_3) +
-	    ARRAY_SIZE(iwl_eeprom_band_4) +
-	    ARRAY_SIZE(iwl_eeprom_band_5);
-
-	IWL_DEBUG_EEPROM(priv, "Parsing data for %d channels.\n",
-			priv->channel_count);
-
-	priv->channel_info = kcalloc(priv->channel_count,
-				     sizeof(struct iwl_channel_info),
-				     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_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 (!(is_channel_valid(ch_info))) {
-				IWL_DEBUG_EEPROM(priv,
-					       "Ch. %d Flags %x [%sGHz] - "
-					       "No traffic\n",
-					       ch_info->channel,
-					       ch_info->flags,
-					       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,
-				       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 ");
-
-			ch_info++;
-		}
-	}
-
-	/* Check if we do have HT40 channels */
-	if (priv->lib->eeprom_ops.regulatory_bands[5] ==
-	    EEPROM_REGULATORY_BAND_NO_HT40 &&
-	    priv->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_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_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_mod_ht40_chan_info(priv, ieeeband,
-						eeprom_ch_index[ch] + 4,
-						&eeprom_ch_info[ch],
-						IEEE80211_CHAN_NO_HT40MINUS);
-		}
-	}
-
-	/* for newer device (6000 series and up)
-	 * EEPROM contain enhanced tx power information
-	 * driver need to process addition information
-	 * to determine the max channel tx power limits
-	 */
-	if (priv->lib->eeprom_ops.enhanced_txpower)
-		iwl_eeprom_enhanced_txpower(priv);
-
-	return 0;
-}
-
-/*
- * iwl_free_channel_map - undo allocations in iwl_init_channel_map
- */
-void iwl_free_channel_map(struct iwl_priv *priv)
-{
-	kfree(priv->channel_info);
-	priv->channel_count = 0;
-}
-
-/**
- * iwl_get_channel_info - Find driver's private channel info
- *
- * Based on band and channel number.
- */
-const struct iwl_channel_info *iwl_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;
-}
-
-void iwl_rf_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->trans, 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));
-		IWL_INFO(priv, "Radio type=0x%x-0x%x-0x%x\n",
-			 EEPROM_RF_CFG_TYPE_MSK(radio_cfg),
-			 EEPROM_RF_CFG_STEP_MSK(radio_cfg),
-			 EEPROM_RF_CFG_DASH_MSK(radio_cfg));
-	} else
-		WARN_ON(1);
-
-	/* set CSR_HW_CONFIG_REG for uCode use */
-	iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
-		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
-}
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
deleted file mode 100644
index 64bfd947cae..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.h
+++ /dev/null
@@ -1,269 +0,0 @@
-/******************************************************************************
- *
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- * redistributing this file, you may do so under either license.
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2008 - 2012 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You 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 - 2012 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_eeprom_h__
-#define __iwl_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 */
-#define EEPROM_SKU_CAP_BAND_24GHZ			(1 << 4)
-#define EEPROM_SKU_CAP_BAND_52GHZ			(1 << 5)
-#define EEPROM_SKU_CAP_11N_ENABLE	                (1 << 6)
-#define EEPROM_SKU_CAP_AMT_ENABLE			(1 << 7)
-#define EEPROM_SKU_CAP_IPAN_ENABLE	                (1 << 8)
-
-/* *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;
-
-enum iwl_eeprom_enhanced_txpwr_flags {
-	IWL_EEPROM_ENH_TXP_FL_VALID		= BIT(0),
-	IWL_EEPROM_ENH_TXP_FL_BAND_52G		= BIT(1),
-	IWL_EEPROM_ENH_TXP_FL_OFDM		= BIT(2),
-	IWL_EEPROM_ENH_TXP_FL_40MHZ		= BIT(3),
-	IWL_EEPROM_ENH_TXP_FL_HT_AP		= BIT(4),
-	IWL_EEPROM_ENH_TXP_FL_RES1		= BIT(5),
-	IWL_EEPROM_ENH_TXP_FL_RES2		= BIT(6),
-	IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE	= BIT(7),
-};
-
-/**
- * iwl_eeprom_enhanced_txpwr structure
- *    This structure presents the enhanced regulatory tx power limit layout
- *    in eeprom image
- *    Enhanced regulatory tx power portion of eeprom image can be broken down
- *    into individual structures; each one is 8 bytes in size and contain the
- *    following information
- * @flags: entry flags
- * @channel: channel number
- * @chain_a_max_pwr: chain a max power in 1/2 dBm
- * @chain_b_max_pwr: chain b max power in 1/2 dBm
- * @chain_c_max_pwr: chain c max power in 1/2 dBm
- * @delta_20_in_40: 20-in-40 deltas (hi/lo)
- * @mimo2_max_pwr: mimo2 max power in 1/2 dBm
- * @mimo3_max_pwr: mimo3 max power in 1/2 dBm
- *
- */
-struct iwl_eeprom_enhanced_txpwr {
-	u8 flags;
-	u8 channel;
-	s8 chain_a_max;
-	s8 chain_b_max;
-	s8 chain_c_max;
-	u8 delta_20_in_40;
-	s8 mimo2_max;
-	s8 mimo3_max;
-} __packed;
-
-/* calibration */
-struct iwl_eeprom_calib_hdr {
-	u8 version;
-	u8 pa_type;
-	__le16 voltage;
-} __packed;
-
-#define EEPROM_CALIB_ALL	(INDIRECT_ADDRESS | INDIRECT_CALIBRATION)
-#define EEPROM_XTAL		((2*0x128) | EEPROM_CALIB_ALL)
-
-/* temperature */
-#define EEPROM_KELVIN_TEMPERATURE	((2*0x12A) | EEPROM_CALIB_ALL)
-#define EEPROM_RAW_TEMPERATURE		((2*0x12B) | EEPROM_CALIB_ALL)
-
-
-/* agn links */
-#define EEPROM_LINK_HOST             (2*0x64)
-#define EEPROM_LINK_GENERAL          (2*0x65)
-#define EEPROM_LINK_REGULATORY       (2*0x66)
-#define EEPROM_LINK_CALIBRATION      (2*0x67)
-#define EEPROM_LINK_PROCESS_ADJST    (2*0x68)
-#define EEPROM_LINK_OTHERS           (2*0x69)
-#define EEPROM_LINK_TXP_LIMIT        (2*0x6a)
-#define EEPROM_LINK_TXP_LIMIT_SIZE   (2*0x6b)
-
-/* agn regulatory - indirect access */
-#define EEPROM_REG_BAND_1_CHANNELS       ((0x08)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 28 bytes */
-#define EEPROM_REG_BAND_2_CHANNELS       ((0x26)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 26 bytes */
-#define EEPROM_REG_BAND_3_CHANNELS       ((0x42)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 24 bytes */
-#define EEPROM_REG_BAND_4_CHANNELS       ((0x5C)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 22 bytes */
-#define EEPROM_REG_BAND_5_CHANNELS       ((0x74)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 12 bytes */
-#define EEPROM_REG_BAND_24_HT40_CHANNELS  ((0x82)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 14  bytes */
-#define EEPROM_REG_BAND_52_HT40_CHANNELS  ((0x92)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 22  bytes */
-
-/* 6000 regulatory - indirect access */
-#define EEPROM_6000_REG_BAND_24_HT40_CHANNELS  ((0x80)\
-		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 14  bytes */
-/* 2.4 GHz */
-extern const u8 iwl_eeprom_band_1[14];
-
-#define ADDRESS_MSK                 0x0000FFFF
-#define INDIRECT_TYPE_MSK           0x000F0000
-#define INDIRECT_HOST               0x00010000
-#define INDIRECT_GENERAL            0x00020000
-#define INDIRECT_REGULATORY         0x00030000
-#define INDIRECT_CALIBRATION        0x00040000
-#define INDIRECT_PROCESS_ADJST      0x00050000
-#define INDIRECT_OTHERS             0x00060000
-#define INDIRECT_TXP_LIMIT          0x00070000
-#define INDIRECT_TXP_LIMIT_SIZE     0x00080000
-#define INDIRECT_ADDRESS            0x00100000
-
-/* General */
-#define EEPROM_DEVICE_ID                    (2*0x08)	/* 2 bytes */
-#define EEPROM_SUBSYSTEM_ID		    (2*0x0A)	/* 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_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_RF_CONFIG_TYPE_MAX	0x3
-
-#define EEPROM_REGULATORY_BAND_NO_HT40			(0)
-
-struct iwl_eeprom_ops {
-	const u32 regulatory_bands[7];
-	bool enhanced_txpower;
-};
-
-
-int iwl_eeprom_init(struct iwl_priv *priv, u32 hw_rev);
-void iwl_eeprom_free(struct iwl_priv *priv);
-int iwl_eeprom_check_version(struct iwl_priv *priv);
-int iwl_eeprom_init_hw_params(struct iwl_priv *priv);
-u16 iwl_eeprom_calib_version(struct iwl_priv *priv);
-const u8 *iwl_eeprom_query_addr(struct iwl_priv *priv, size_t offset);
-u16 iwl_eeprom_query16(struct iwl_priv *priv, size_t offset);
-void iwl_eeprom_get_mac(struct iwl_priv *priv, u8 *mac);
-int iwl_init_channel_map(struct iwl_priv *priv);
-void iwl_free_channel_map(struct iwl_priv *priv);
-const struct iwl_channel_info *iwl_get_channel_info(
-		const struct iwl_priv *priv,
-		enum ieee80211_band band, u16 channel);
-void iwl_rf_config(struct iwl_priv *priv);
-
-#endif  /* __iwl_eeprom_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index 74bce97a860..80604664174 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -421,6 +421,8 @@ static inline unsigned int FH_MEM_CBBC_QUEUE(unsigned int chnl)
 		(FH_SRVC_LOWER_BOUND + ((_chnl) - 9) * 0x4)
 
 #define FH_TX_CHICKEN_BITS_REG	(FH_MEM_LOWER_BOUND + 0xE98)
+#define FH_TX_TRB_REG(_chan)	(FH_MEM_LOWER_BOUND + 0x958 + (_chan) * 4)
+
 /* Instruct FH to increment the retry count of a packet when
  * it is brought from the memory to TX-FIFO
  */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index 081dd34d238..3dfebfb8434 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -27,9 +27,10 @@
  *****************************************************************************/
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/export.h>
 
 #include "iwl-io.h"
-#include"iwl-csr.h"
+#include "iwl-csr.h"
 #include "iwl-debug.h"
 
 #define IWL_POLL_INTERVAL 10	/* microseconds */
@@ -52,6 +53,7 @@ void iwl_set_bit(struct iwl_trans *trans, u32 reg, u32 mask)
 	__iwl_set_bit(trans, reg, mask);
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_set_bit);
 
 void iwl_clear_bit(struct iwl_trans *trans, u32 reg, u32 mask)
 {
@@ -61,6 +63,25 @@ void iwl_clear_bit(struct iwl_trans *trans, u32 reg, u32 mask)
 	__iwl_clear_bit(trans, reg, mask);
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_clear_bit);
+
+void iwl_set_bits_mask(struct iwl_trans *trans, u32 reg, u32 mask, u32 value)
+{
+	unsigned long flags;
+	u32 v;
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	WARN_ON_ONCE(value & ~mask);
+#endif
+
+	spin_lock_irqsave(&trans->reg_lock, flags);
+	v = iwl_read32(trans, reg);
+	v &= ~mask;
+	v |= value;
+	iwl_write32(trans, reg, v);
+	spin_unlock_irqrestore(&trans->reg_lock, flags);
+}
+EXPORT_SYMBOL_GPL(iwl_set_bits_mask);
 
 int iwl_poll_bit(struct iwl_trans *trans, u32 addr,
 		 u32 bits, u32 mask, int timeout)
@@ -76,6 +97,7 @@ int iwl_poll_bit(struct iwl_trans *trans, u32 addr,
 
 	return -ETIMEDOUT;
 }
+EXPORT_SYMBOL_GPL(iwl_poll_bit);
 
 int iwl_grab_nic_access_silent(struct iwl_trans *trans)
 {
@@ -117,6 +139,7 @@ int iwl_grab_nic_access_silent(struct iwl_trans *trans)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(iwl_grab_nic_access_silent);
 
 bool iwl_grab_nic_access(struct iwl_trans *trans)
 {
@@ -130,6 +153,7 @@ bool iwl_grab_nic_access(struct iwl_trans *trans)
 
 	return true;
 }
+EXPORT_SYMBOL_GPL(iwl_grab_nic_access);
 
 void iwl_release_nic_access(struct iwl_trans *trans)
 {
@@ -144,6 +168,7 @@ void iwl_release_nic_access(struct iwl_trans *trans)
 	 */
 	mmiowb();
 }
+EXPORT_SYMBOL_GPL(iwl_release_nic_access);
 
 u32 iwl_read_direct32(struct iwl_trans *trans, u32 reg)
 {
@@ -158,6 +183,7 @@ u32 iwl_read_direct32(struct iwl_trans *trans, u32 reg)
 
 	return value;
 }
+EXPORT_SYMBOL_GPL(iwl_read_direct32);
 
 void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value)
 {
@@ -170,6 +196,7 @@ void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value)
 	}
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_write_direct32);
 
 int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
 			int timeout)
@@ -185,6 +212,7 @@ int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
 
 	return -ETIMEDOUT;
 }
+EXPORT_SYMBOL_GPL(iwl_poll_direct_bit);
 
 static inline u32 __iwl_read_prph(struct iwl_trans *trans, u32 reg)
 {
@@ -211,6 +239,7 @@ u32 iwl_read_prph(struct iwl_trans *trans, u32 reg)
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 	return val;
 }
+EXPORT_SYMBOL_GPL(iwl_read_prph);
 
 void iwl_write_prph(struct iwl_trans *trans, u32 addr, u32 val)
 {
@@ -223,6 +252,7 @@ void iwl_write_prph(struct iwl_trans *trans, u32 addr, u32 val)
 	}
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_write_prph);
 
 void iwl_set_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask)
 {
@@ -236,6 +266,7 @@ void iwl_set_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask)
 	}
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_set_bits_prph);
 
 void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 reg,
 			    u32 bits, u32 mask)
@@ -250,6 +281,7 @@ void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 reg,
 	}
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_set_bits_mask_prph);
 
 void iwl_clear_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask)
 {
@@ -264,9 +296,10 @@ void iwl_clear_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask)
 	}
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(iwl_clear_bits_prph);
 
-void _iwl_read_targ_mem_words(struct iwl_trans *trans, u32 addr,
-			      void *buf, int words)
+void _iwl_read_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+			       void *buf, int dwords)
 {
 	unsigned long flags;
 	int offs;
@@ -275,24 +308,26 @@ void _iwl_read_targ_mem_words(struct iwl_trans *trans, u32 addr,
 	spin_lock_irqsave(&trans->reg_lock, flags);
 	if (likely(iwl_grab_nic_access(trans))) {
 		iwl_write32(trans, HBUS_TARG_MEM_RADDR, addr);
-		for (offs = 0; offs < words; offs++)
+		for (offs = 0; offs < dwords; offs++)
 			vals[offs] = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
 		iwl_release_nic_access(trans);
 	}
 	spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
+EXPORT_SYMBOL_GPL(_iwl_read_targ_mem_dwords);
 
 u32 iwl_read_targ_mem(struct iwl_trans *trans, u32 addr)
 {
 	u32 value;
 
-	_iwl_read_targ_mem_words(trans, addr, &value, 1);
+	_iwl_read_targ_mem_dwords(trans, addr, &value, 1);
 
 	return value;
 }
+EXPORT_SYMBOL_GPL(iwl_read_targ_mem);
 
-int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
-				void *buf, int words)
+int _iwl_write_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+			       void *buf, int dwords)
 {
 	unsigned long flags;
 	int offs, result = 0;
@@ -301,7 +336,7 @@ int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
 	spin_lock_irqsave(&trans->reg_lock, flags);
 	if (likely(iwl_grab_nic_access(trans))) {
 		iwl_write32(trans, HBUS_TARG_MEM_WADDR, addr);
-		for (offs = 0; offs < words; offs++)
+		for (offs = 0; offs < dwords; offs++)
 			iwl_write32(trans, HBUS_TARG_MEM_WDAT, vals[offs]);
 		iwl_release_nic_access(trans);
 	} else
@@ -310,8 +345,10 @@ int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
 
 	return result;
 }
+EXPORT_SYMBOL_GPL(_iwl_write_targ_mem_dwords);
 
 int iwl_write_targ_mem(struct iwl_trans *trans, u32 addr, u32 val)
 {
-	return _iwl_write_targ_mem_words(trans, addr, &val, 1);
+	return _iwl_write_targ_mem_dwords(trans, addr, &val, 1);
 }
+EXPORT_SYMBOL_GPL(iwl_write_targ_mem);
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/iwlwifi/iwl-io.h
index abb3250164b..50d3819739d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -54,6 +54,8 @@ static inline u32 iwl_read32(struct iwl_trans *trans, u32 ofs)
 void iwl_set_bit(struct iwl_trans *trans, u32 reg, u32 mask);
 void iwl_clear_bit(struct iwl_trans *trans, u32 reg, u32 mask);
 
+void iwl_set_bits_mask(struct iwl_trans *trans, u32 reg, u32 mask, u32 value);
+
 int iwl_poll_bit(struct iwl_trans *trans, u32 addr,
 		 u32 bits, u32 mask, int timeout);
 int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
@@ -74,18 +76,18 @@ void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 reg,
 			    u32 bits, u32 mask);
 void iwl_clear_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask);
 
-void _iwl_read_targ_mem_words(struct iwl_trans *trans, u32 addr,
-			      void *buf, int words);
+void _iwl_read_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+			       void *buf, int dwords);
 
-#define iwl_read_targ_mem_words(trans, addr, buf, bufsize)	\
+#define iwl_read_targ_mem_bytes(trans, addr, buf, bufsize)	\
 	do {							\
 		BUILD_BUG_ON((bufsize) % sizeof(u32));		\
-		_iwl_read_targ_mem_words(trans, addr, buf,	\
-					 (bufsize) / sizeof(u32));\
+		_iwl_read_targ_mem_dwords(trans, addr, buf,	\
+					  (bufsize) / sizeof(u32));\
 	} while (0)
 
-int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
-			      void *buf, int words);
+int _iwl_write_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+			       void *buf, int dwords);
 
 u32 iwl_read_targ_mem(struct iwl_trans *trans, u32 addr);
 int iwl_write_targ_mem(struct iwl_trans *trans, u32 addr, u32 val);
diff --git a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
index 0066b899fe5..c61f2070f15 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
+++ b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
@@ -61,6 +61,7 @@
  *
  *****************************************************************************/
 #include <linux/sched.h>
+#include <linux/export.h>
 
 #include "iwl-notif-wait.h"
 
@@ -71,6 +72,7 @@ void iwl_notification_wait_init(struct iwl_notif_wait_data *notif_wait)
 	INIT_LIST_HEAD(&notif_wait->notif_waits);
 	init_waitqueue_head(&notif_wait->notif_waitq);
 }
+EXPORT_SYMBOL_GPL(iwl_notification_wait_init);
 
 void iwl_notification_wait_notify(struct iwl_notif_wait_data *notif_wait,
 				  struct iwl_rx_packet *pkt)
@@ -115,20 +117,20 @@ void iwl_notification_wait_notify(struct iwl_notif_wait_data *notif_wait,
 	if (triggered)
 		wake_up_all(&notif_wait->notif_waitq);
 }
+EXPORT_SYMBOL_GPL(iwl_notification_wait_notify);
 
 void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_wait)
 {
-	unsigned long flags;
 	struct iwl_notification_wait *wait_entry;
 
-	spin_lock_irqsave(&notif_wait->notif_wait_lock, flags);
+	spin_lock(&notif_wait->notif_wait_lock);
 	list_for_each_entry(wait_entry, &notif_wait->notif_waits, list)
 		wait_entry->aborted = true;
-	spin_unlock_irqrestore(&notif_wait->notif_wait_lock, flags);
+	spin_unlock(&notif_wait->notif_wait_lock);
 
 	wake_up_all(&notif_wait->notif_waitq);
 }
-
+EXPORT_SYMBOL_GPL(iwl_abort_notification_waits);
 
 void
 iwl_init_notification_wait(struct iwl_notif_wait_data *notif_wait,
@@ -152,6 +154,7 @@ iwl_init_notification_wait(struct iwl_notif_wait_data *notif_wait,
 	list_add(&wait_entry->list, &notif_wait->notif_waits);
 	spin_unlock_bh(&notif_wait->notif_wait_lock);
 }
+EXPORT_SYMBOL_GPL(iwl_init_notification_wait);
 
 int iwl_wait_notification(struct iwl_notif_wait_data *notif_wait,
 			  struct iwl_notification_wait *wait_entry,
@@ -175,6 +178,7 @@ int iwl_wait_notification(struct iwl_notif_wait_data *notif_wait,
 		return -ETIMEDOUT;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(iwl_wait_notification);
 
 void iwl_remove_notification(struct iwl_notif_wait_data *notif_wait,
 			     struct iwl_notification_wait *wait_entry)
@@ -183,3 +187,4 @@ void iwl_remove_notification(struct iwl_notif_wait_data *notif_wait,
 	list_del(&wait_entry->list);
 	spin_unlock_bh(&notif_wait->notif_wait_lock);
 }
+EXPORT_SYMBOL_GPL(iwl_remove_notification);
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index 4ef742b28e0..64886f95664 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -111,22 +111,25 @@ struct iwl_cfg;
  *	May sleep
  * @rx: Rx notification to the op_mode. rxb is the Rx buffer itself. Cmd is the
  *	HCMD the this Rx responds to.
- *	Must be atomic.
+ *	Must be atomic and called with BH disabled.
  * @queue_full: notifies that a HW queue is full.
- *	Must be atomic
+ *	Must be atomic and called with BH disabled.
  * @queue_not_full: notifies that a HW queue is not full any more.
- *	Must be atomic
+ *	Must be atomic and called with BH disabled.
  * @hw_rf_kill:notifies of a change in the HW rf kill switch. True means that
  *	the radio is killed. Must be atomic.
  * @free_skb: allows the transport layer to free skbs that haven't been
  *	reclaimed by the op_mode. This can happen when the driver is freed and
  *	there are Tx packets pending in the transport layer.
  *	Must be atomic
- * @nic_error: error notification. Must be atomic
- * @cmd_queue_full: Called when the command queue gets full. Must be atomic.
+ * @nic_error: error notification. Must be atomic and must be called with BH
+ *	disabled.
+ * @cmd_queue_full: Called when the command queue gets full. Must be atomic and
+ *	called with BH disabled.
  * @nic_config: configure NIC, called before firmware is started.
  *	May sleep
- * @wimax_active: invoked when WiMax becomes active.  Must be atomic.
+ * @wimax_active: invoked when WiMax becomes active.  Must be atomic and called
+ *	with BH disabled.
  */
 struct iwl_op_mode_ops {
 	struct iwl_op_mode *(*start)(struct iwl_trans *trans,
@@ -145,6 +148,9 @@ struct iwl_op_mode_ops {
 	void (*wimax_active)(struct iwl_op_mode *op_mode);
 };
 
+int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops);
+void iwl_opmode_deregister(const char *name);
+
 /**
  * struct iwl_op_mode - operational mode
  *
@@ -162,7 +168,6 @@ struct iwl_op_mode {
 static inline void iwl_op_mode_stop(struct iwl_op_mode *op_mode)
 {
 	might_sleep();
-
 	op_mode->ops->stop(op_mode);
 }
 
@@ -218,9 +223,4 @@ static inline void iwl_op_mode_wimax_active(struct iwl_op_mode *op_mode)
 	op_mode->ops->wimax_active(op_mode);
 }
 
-/*****************************************************
-* Op mode layers implementations
-******************************************************/
-extern const struct iwl_op_mode_ops iwl_dvm_ops;
-
 #endif /* __iwl_op_mode_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index dfd54662e3e..9253ef1dba7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -187,7 +187,7 @@
 #define SCD_QUEUE_STTS_REG_POS_ACTIVE	(3)
 #define SCD_QUEUE_STTS_REG_POS_WSL	(4)
 #define SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN (19)
-#define SCD_QUEUE_STTS_REG_MSK		(0x00FF0000)
+#define SCD_QUEUE_STTS_REG_MSK		(0x017F0000)
 
 #define SCD_QUEUE_CTX_REG1_CREDIT_POS		(8)
 #define SCD_QUEUE_CTX_REG1_CREDIT_MSK		(0x00FFFF00)
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.c b/drivers/net/wireless/iwlwifi/iwl-test.c
new file mode 100644
index 00000000000..81e8c7126d7
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-test.c
@@ -0,0 +1,856 @@
+/******************************************************************************
+ *
+ * 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) 2010 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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) 2010 - 2012 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/export.h>
+#include <net/netlink.h>
+
+#include "iwl-io.h"
+#include "iwl-fh.h"
+#include "iwl-prph.h"
+#include "iwl-trans.h"
+#include "iwl-test.h"
+#include "iwl-csr.h"
+#include "iwl-testmode.h"
+
+/*
+ * Periphery registers absolute lower bound. This is used in order to
+ * differentiate registery access through HBUS_TARG_PRPH_* and
+ * HBUS_TARG_MEM_* accesses.
+ */
+#define IWL_ABS_PRPH_START (0xA00000)
+
+/*
+ * The TLVs used in the gnl message policy between the kernel module and
+ * user space application. iwl_testmode_gnl_msg_policy is to be carried
+ * through the NL80211_CMD_TESTMODE channel regulated by nl80211.
+ * See iwl-testmode.h
+ */
+static
+struct nla_policy iwl_testmode_gnl_msg_policy[IWL_TM_ATTR_MAX] = {
+	[IWL_TM_ATTR_COMMAND] = { .type = NLA_U32, },
+
+	[IWL_TM_ATTR_UCODE_CMD_ID] = { .type = NLA_U8, },
+	[IWL_TM_ATTR_UCODE_CMD_DATA] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_REG_OFFSET] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_REG_VALUE8] = { .type = NLA_U8, },
+	[IWL_TM_ATTR_REG_VALUE32] = { .type = NLA_U32, },
+
+	[IWL_TM_ATTR_SYNC_RSP] = { .type = NLA_UNSPEC, },
+	[IWL_TM_ATTR_UCODE_RX_PKT] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_EEPROM] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_TRACE_ADDR] = { .type = NLA_UNSPEC, },
+	[IWL_TM_ATTR_TRACE_DUMP] = { .type = NLA_UNSPEC, },
+	[IWL_TM_ATTR_TRACE_SIZE] = { .type = NLA_U32, },
+
+	[IWL_TM_ATTR_FIXRATE] = { .type = NLA_U32, },
+
+	[IWL_TM_ATTR_UCODE_OWNER] = { .type = NLA_U8, },
+
+	[IWL_TM_ATTR_MEM_ADDR] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_BUFFER_SIZE] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_BUFFER_DUMP] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_FW_VERSION] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_DEVICE_ID] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_FW_TYPE] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_FW_INST_SIZE] = { .type = NLA_U32, },
+	[IWL_TM_ATTR_FW_DATA_SIZE] = { .type = NLA_U32, },
+
+	[IWL_TM_ATTR_ENABLE_NOTIFICATION] = {.type = NLA_FLAG, },
+};
+
+static inline void iwl_test_trace_clear(struct iwl_test *tst)
+{
+	memset(&tst->trace, 0, sizeof(struct iwl_test_trace));
+}
+
+static void iwl_test_trace_stop(struct iwl_test *tst)
+{
+	if (!tst->trace.enabled)
+		return;
+
+	if (tst->trace.cpu_addr && tst->trace.dma_addr)
+		dma_free_coherent(tst->trans->dev,
+				  tst->trace.tsize,
+				  tst->trace.cpu_addr,
+				  tst->trace.dma_addr);
+
+	iwl_test_trace_clear(tst);
+}
+
+static inline void iwl_test_mem_clear(struct iwl_test *tst)
+{
+	memset(&tst->mem, 0, sizeof(struct iwl_test_mem));
+}
+
+static inline void iwl_test_mem_stop(struct iwl_test *tst)
+{
+	if (!tst->mem.in_read)
+		return;
+
+	iwl_test_mem_clear(tst);
+}
+
+/*
+ * Initializes the test object
+ * During the lifetime of the test object it is assumed that the transport is
+ * started. The test object should be stopped before the transport is stopped.
+ */
+void iwl_test_init(struct iwl_test *tst, struct iwl_trans *trans,
+		   struct iwl_test_ops *ops)
+{
+	tst->trans = trans;
+	tst->ops = ops;
+
+	iwl_test_trace_clear(tst);
+	iwl_test_mem_clear(tst);
+}
+EXPORT_SYMBOL_GPL(iwl_test_init);
+
+/*
+ * Stop the test object
+ */
+void iwl_test_free(struct iwl_test *tst)
+{
+	iwl_test_mem_stop(tst);
+	iwl_test_trace_stop(tst);
+}
+EXPORT_SYMBOL_GPL(iwl_test_free);
+
+static inline int iwl_test_send_cmd(struct iwl_test *tst,
+				    struct iwl_host_cmd *cmd)
+{
+	return tst->ops->send_cmd(tst->trans->op_mode, cmd);
+}
+
+static inline bool iwl_test_valid_hw_addr(struct iwl_test *tst, u32 addr)
+{
+	return tst->ops->valid_hw_addr(addr);
+}
+
+static inline u32 iwl_test_fw_ver(struct iwl_test *tst)
+{
+	return tst->ops->get_fw_ver(tst->trans->op_mode);
+}
+
+static inline struct sk_buff*
+iwl_test_alloc_reply(struct iwl_test *tst, int len)
+{
+	return tst->ops->alloc_reply(tst->trans->op_mode, len);
+}
+
+static inline int iwl_test_reply(struct iwl_test *tst, struct sk_buff *skb)
+{
+	return tst->ops->reply(tst->trans->op_mode, skb);
+}
+
+static inline struct sk_buff*
+iwl_test_alloc_event(struct iwl_test *tst, int len)
+{
+	return tst->ops->alloc_event(tst->trans->op_mode, len);
+}
+
+static inline void
+iwl_test_event(struct iwl_test *tst, struct sk_buff *skb)
+{
+	return tst->ops->event(tst->trans->op_mode, skb);
+}
+
+/*
+ * This function handles the user application commands to the fw. The fw
+ * commands are sent in a synchronuous manner. In case that the user requested
+ * to get commands response, it is send to the user.
+ */
+static int iwl_test_fw_cmd(struct iwl_test *tst, struct nlattr **tb)
+{
+	struct iwl_host_cmd cmd;
+	struct iwl_rx_packet *pkt;
+	struct sk_buff *skb;
+	void *reply_buf;
+	u32 reply_len;
+	int ret;
+	bool cmd_want_skb;
+
+	memset(&cmd, 0, sizeof(struct iwl_host_cmd));
+
+	if (!tb[IWL_TM_ATTR_UCODE_CMD_ID] ||
+	    !tb[IWL_TM_ATTR_UCODE_CMD_DATA]) {
+		IWL_ERR(tst->trans, "Missing fw command mandatory fields\n");
+		return -ENOMSG;
+	}
+
+	cmd.flags = CMD_ON_DEMAND | CMD_SYNC;
+	cmd_want_skb = nla_get_flag(tb[IWL_TM_ATTR_UCODE_CMD_SKB]);
+	if (cmd_want_skb)
+		cmd.flags |= CMD_WANT_SKB;
+
+	cmd.id = nla_get_u8(tb[IWL_TM_ATTR_UCODE_CMD_ID]);
+	cmd.data[0] = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+	cmd.len[0] = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+	cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
+	IWL_DEBUG_INFO(tst->trans, "test fw cmd=0x%x, flags 0x%x, len %d\n",
+		       cmd.id, cmd.flags, cmd.len[0]);
+
+	ret = iwl_test_send_cmd(tst, &cmd);
+	if (ret) {
+		IWL_ERR(tst->trans, "Failed to send hcmd\n");
+		return ret;
+	}
+	if (!cmd_want_skb)
+		return ret;
+
+	/* Handling return of SKB to the user */
+	pkt = cmd.resp_pkt;
+	if (!pkt) {
+		IWL_ERR(tst->trans, "HCMD received a null response packet\n");
+		return ret;
+	}
+
+	reply_len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+	skb = iwl_test_alloc_reply(tst, reply_len + 20);
+	reply_buf = kmalloc(reply_len, GFP_KERNEL);
+	if (!skb || !reply_buf) {
+		kfree_skb(skb);
+		kfree(reply_buf);
+		return -ENOMEM;
+	}
+
+	/* The reply is in a page, that we cannot send to user space. */
+	memcpy(reply_buf, &(pkt->hdr), reply_len);
+	iwl_free_resp(&cmd);
+
+	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+			IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
+	    nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf))
+		goto nla_put_failure;
+	return iwl_test_reply(tst, skb);
+
+nla_put_failure:
+	IWL_DEBUG_INFO(tst->trans, "Failed creating NL attributes\n");
+	kfree(reply_buf);
+	kfree_skb(skb);
+	return -ENOMSG;
+}
+
+/*
+ * Handles the user application commands for register access.
+ */
+static int iwl_test_reg(struct iwl_test *tst, struct nlattr **tb)
+{
+	u32 ofs, val32, cmd;
+	u8 val8;
+	struct sk_buff *skb;
+	int status = 0;
+	struct iwl_trans *trans = tst->trans;
+
+	if (!tb[IWL_TM_ATTR_REG_OFFSET]) {
+		IWL_ERR(trans, "Missing reg offset\n");
+		return -ENOMSG;
+	}
+
+	ofs = nla_get_u32(tb[IWL_TM_ATTR_REG_OFFSET]);
+	IWL_DEBUG_INFO(trans, "test reg access cmd offset=0x%x\n", ofs);
+
+	cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
+
+	/*
+	 * Allow access only to FH/CSR/HBUS in direct mode.
+	 * Since we don't have the upper bounds for the CSR and HBUS segments,
+	 * we will use only the upper bound of FH for sanity check.
+	 */
+	if (ofs >= FH_MEM_UPPER_BOUND) {
+		IWL_ERR(trans, "offset out of segment (0x0 - 0x%x)\n",
+			FH_MEM_UPPER_BOUND);
+		return -EINVAL;
+	}
+
+	switch (cmd) {
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
+		val32 = iwl_read_direct32(tst->trans, ofs);
+		IWL_DEBUG_INFO(trans, "32 value to read 0x%x\n", val32);
+
+		skb = iwl_test_alloc_reply(tst, 20);
+		if (!skb) {
+			IWL_ERR(trans, "Memory allocation fail\n");
+			return -ENOMEM;
+		}
+		if (nla_put_u32(skb, IWL_TM_ATTR_REG_VALUE32, val32))
+			goto nla_put_failure;
+		status = iwl_test_reply(tst, skb);
+		if (status < 0)
+			IWL_ERR(trans, "Error sending msg : %d\n", status);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
+		if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
+			IWL_ERR(trans, "Missing value to write\n");
+			return -ENOMSG;
+		} else {
+			val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
+			IWL_DEBUG_INFO(trans, "32b write val=0x%x\n", val32);
+			iwl_write_direct32(tst->trans, ofs, val32);
+		}
+		break;
+
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
+		if (!tb[IWL_TM_ATTR_REG_VALUE8]) {
+			IWL_ERR(trans, "Missing value to write\n");
+			return -ENOMSG;
+		} else {
+			val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
+			IWL_DEBUG_INFO(trans, "8b write val=0x%x\n", val8);
+			iwl_write8(tst->trans, ofs, val8);
+		}
+		break;
+
+	default:
+		IWL_ERR(trans, "Unknown test register cmd ID\n");
+		return -ENOMSG;
+	}
+
+	return status;
+
+nla_put_failure:
+	kfree_skb(skb);
+	return -EMSGSIZE;
+}
+
+/*
+ * Handles the request to start FW tracing. Allocates of the trace buffer
+ * and sends a reply to user space with the address of the allocated buffer.
+ */
+static int iwl_test_trace_begin(struct iwl_test *tst, struct nlattr **tb)
+{
+	struct sk_buff *skb;
+	int status = 0;
+
+	if (tst->trace.enabled)
+		return -EBUSY;
+
+	if (!tb[IWL_TM_ATTR_TRACE_SIZE])
+		tst->trace.size = TRACE_BUFF_SIZE_DEF;
+	else
+		tst->trace.size =
+			nla_get_u32(tb[IWL_TM_ATTR_TRACE_SIZE]);
+
+	if (!tst->trace.size)
+		return -EINVAL;
+
+	if (tst->trace.size < TRACE_BUFF_SIZE_MIN ||
+	    tst->trace.size > TRACE_BUFF_SIZE_MAX)
+		return -EINVAL;
+
+	tst->trace.tsize = tst->trace.size + TRACE_BUFF_PADD;
+	tst->trace.cpu_addr = dma_alloc_coherent(tst->trans->dev,
+						 tst->trace.tsize,
+						 &tst->trace.dma_addr,
+						 GFP_KERNEL);
+	if (!tst->trace.cpu_addr)
+		return -ENOMEM;
+
+	tst->trace.enabled = true;
+	tst->trace.trace_addr = (u8 *)PTR_ALIGN(tst->trace.cpu_addr, 0x100);
+
+	memset(tst->trace.trace_addr, 0x03B, tst->trace.size);
+
+	skb = iwl_test_alloc_reply(tst, sizeof(tst->trace.dma_addr) + 20);
+	if (!skb) {
+		IWL_ERR(tst->trans, "Memory allocation fail\n");
+		iwl_test_trace_stop(tst);
+		return -ENOMEM;
+	}
+
+	if (nla_put(skb, IWL_TM_ATTR_TRACE_ADDR,
+		    sizeof(tst->trace.dma_addr),
+		    (u64 *)&tst->trace.dma_addr))
+		goto nla_put_failure;
+
+	status = iwl_test_reply(tst, skb);
+	if (status < 0)
+		IWL_ERR(tst->trans, "Error sending msg : %d\n", status);
+
+	tst->trace.nchunks = DIV_ROUND_UP(tst->trace.size,
+					  DUMP_CHUNK_SIZE);
+
+	return status;
+
+nla_put_failure:
+	kfree_skb(skb);
+	if (nla_get_u32(tb[IWL_TM_ATTR_COMMAND]) ==
+	    IWL_TM_CMD_APP2DEV_BEGIN_TRACE)
+		iwl_test_trace_stop(tst);
+	return -EMSGSIZE;
+}
+
+/*
+ * Handles indirect read from the periphery or the SRAM. The read is performed
+ * to a temporary buffer. The user space application should later issue a dump
+ */
+static int iwl_test_indirect_read(struct iwl_test *tst, u32 addr, u32 size)
+{
+	struct iwl_trans *trans = tst->trans;
+	unsigned long flags;
+	int i;
+
+	if (size & 0x3)
+		return -EINVAL;
+
+	tst->mem.size = size;
+	tst->mem.addr = kmalloc(tst->mem.size, GFP_KERNEL);
+	if (tst->mem.addr == NULL)
+		return -ENOMEM;
+
+	/* Hard-coded periphery absolute address */
+	if (IWL_ABS_PRPH_START <= addr &&
+	    addr < IWL_ABS_PRPH_START + PRPH_END) {
+			spin_lock_irqsave(&trans->reg_lock, flags);
+			iwl_grab_nic_access(trans);
+			iwl_write32(trans, HBUS_TARG_PRPH_RADDR,
+				    addr | (3 << 24));
+			for (i = 0; i < size; i += 4)
+				*(u32 *)(tst->mem.addr + i) =
+					iwl_read32(trans, HBUS_TARG_PRPH_RDAT);
+			iwl_release_nic_access(trans);
+			spin_unlock_irqrestore(&trans->reg_lock, flags);
+	} else { /* target memory (SRAM) */
+		_iwl_read_targ_mem_dwords(trans, addr,
+					  tst->mem.addr,
+					  tst->mem.size / 4);
+	}
+
+	tst->mem.nchunks =
+		DIV_ROUND_UP(tst->mem.size, DUMP_CHUNK_SIZE);
+	tst->mem.in_read = true;
+	return 0;
+
+}
+
+/*
+ * Handles indirect write to the periphery or SRAM. The  is performed to a
+ * temporary buffer.
+ */
+static int iwl_test_indirect_write(struct iwl_test *tst, u32 addr,
+	u32 size, unsigned char *buf)
+{
+	struct iwl_trans *trans = tst->trans;
+	u32 val, i;
+	unsigned long flags;
+
+	if (IWL_ABS_PRPH_START <= addr &&
+	    addr < IWL_ABS_PRPH_START + PRPH_END) {
+			/* Periphery writes can be 1-3 bytes long, or DWORDs */
+			if (size < 4) {
+				memcpy(&val, buf, size);
+				spin_lock_irqsave(&trans->reg_lock, flags);
+				iwl_grab_nic_access(trans);
+				iwl_write32(trans, HBUS_TARG_PRPH_WADDR,
+					    (addr & 0x0000FFFF) |
+					    ((size - 1) << 24));
+				iwl_write32(trans, HBUS_TARG_PRPH_WDAT, val);
+				iwl_release_nic_access(trans);
+				/* needed after consecutive writes w/o read */
+				mmiowb();
+				spin_unlock_irqrestore(&trans->reg_lock, flags);
+			} else {
+				if (size % 4)
+					return -EINVAL;
+				for (i = 0; i < size; i += 4)
+					iwl_write_prph(trans, addr+i,
+						       *(u32 *)(buf+i));
+			}
+	} else if (iwl_test_valid_hw_addr(tst, addr)) {
+		_iwl_write_targ_mem_dwords(trans, addr, buf, size / 4);
+	} else {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * Handles the user application commands for indirect read/write
+ * to/from the periphery or the SRAM.
+ */
+static int iwl_test_indirect_mem(struct iwl_test *tst, struct nlattr **tb)
+{
+	u32 addr, size, cmd;
+	unsigned char *buf;
+
+	/* Both read and write should be blocked, for atomicity */
+	if (tst->mem.in_read)
+		return -EBUSY;
+
+	cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
+	if (!tb[IWL_TM_ATTR_MEM_ADDR]) {
+		IWL_ERR(tst->trans, "Error finding memory offset address\n");
+		return -ENOMSG;
+	}
+	addr = nla_get_u32(tb[IWL_TM_ATTR_MEM_ADDR]);
+	if (!tb[IWL_TM_ATTR_BUFFER_SIZE]) {
+		IWL_ERR(tst->trans, "Error finding size for memory reading\n");
+		return -ENOMSG;
+	}
+	size = nla_get_u32(tb[IWL_TM_ATTR_BUFFER_SIZE]);
+
+	if (cmd == IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ) {
+		return iwl_test_indirect_read(tst, addr,  size);
+	} else {
+		if (!tb[IWL_TM_ATTR_BUFFER_DUMP])
+			return -EINVAL;
+		buf = (unsigned char *)nla_data(tb[IWL_TM_ATTR_BUFFER_DUMP]);
+		return iwl_test_indirect_write(tst, addr, size, buf);
+	}
+}
+
+/*
+ * Enable notifications to user space
+ */
+static int iwl_test_notifications(struct iwl_test *tst,
+				  struct nlattr **tb)
+{
+	tst->notify = nla_get_flag(tb[IWL_TM_ATTR_ENABLE_NOTIFICATION]);
+	return 0;
+}
+
+/*
+ * Handles the request to get the device id
+ */
+static int iwl_test_get_dev_id(struct iwl_test *tst, struct nlattr **tb)
+{
+	u32 devid = tst->trans->hw_id;
+	struct sk_buff *skb;
+	int status;
+
+	IWL_DEBUG_INFO(tst->trans, "hw version: 0x%x\n", devid);
+
+	skb = iwl_test_alloc_reply(tst, 20);
+	if (!skb) {
+		IWL_ERR(tst->trans, "Memory allocation fail\n");
+		return -ENOMEM;
+	}
+
+	if (nla_put_u32(skb, IWL_TM_ATTR_DEVICE_ID, devid))
+		goto nla_put_failure;
+	status = iwl_test_reply(tst, skb);
+	if (status < 0)
+		IWL_ERR(tst->trans, "Error sending msg : %d\n", status);
+
+	return 0;
+
+nla_put_failure:
+	kfree_skb(skb);
+	return -EMSGSIZE;
+}
+
+/*
+ * Handles the request to get the FW version
+ */
+static int iwl_test_get_fw_ver(struct iwl_test *tst, struct nlattr **tb)
+{
+	struct sk_buff *skb;
+	int status;
+	u32 ver = iwl_test_fw_ver(tst);
+
+	IWL_DEBUG_INFO(tst->trans, "uCode version raw: 0x%x\n", ver);
+
+	skb = iwl_test_alloc_reply(tst, 20);
+	if (!skb) {
+		IWL_ERR(tst->trans, "Memory allocation fail\n");
+		return -ENOMEM;
+	}
+
+	if (nla_put_u32(skb, IWL_TM_ATTR_FW_VERSION, ver))
+		goto nla_put_failure;
+
+	status = iwl_test_reply(tst, skb);
+	if (status < 0)
+		IWL_ERR(tst->trans, "Error sending msg : %d\n", status);
+
+	return 0;
+
+nla_put_failure:
+	kfree_skb(skb);
+	return -EMSGSIZE;
+}
+
+/*
+ * Parse the netlink message and validate that the IWL_TM_ATTR_CMD exists
+ */
+int iwl_test_parse(struct iwl_test *tst, struct nlattr **tb,
+		   void *data, int len)
+{
+	int result;
+
+	result = nla_parse(tb, IWL_TM_ATTR_MAX - 1, data, len,
+			iwl_testmode_gnl_msg_policy);
+	if (result) {
+		IWL_ERR(tst->trans, "Fail parse gnl msg: %d\n", result);
+		return result;
+	}
+
+	/* IWL_TM_ATTR_COMMAND is absolutely mandatory */
+	if (!tb[IWL_TM_ATTR_COMMAND]) {
+		IWL_ERR(tst->trans, "Missing testmode command type\n");
+		return -ENOMSG;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iwl_test_parse);
+
+/*
+ * Handle test commands.
+ * Returns 1 for unknown commands (not handled by the test object); negative
+ * value in case of error.
+ */
+int iwl_test_handle_cmd(struct iwl_test *tst, struct nlattr **tb)
+{
+	int result;
+
+	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
+	case IWL_TM_CMD_APP2DEV_UCODE:
+		IWL_DEBUG_INFO(tst->trans, "test cmd to uCode\n");
+		result = iwl_test_fw_cmd(tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
+	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
+		IWL_DEBUG_INFO(tst->trans, "test cmd to register\n");
+		result = iwl_test_reg(tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
+		IWL_DEBUG_INFO(tst->trans, "test uCode trace cmd to driver\n");
+		result = iwl_test_trace_begin(tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_END_TRACE:
+		iwl_test_trace_stop(tst);
+		result = 0;
+		break;
+
+	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ:
+	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_WRITE:
+		IWL_DEBUG_INFO(tst->trans, "test indirect memory cmd\n");
+		result = iwl_test_indirect_mem(tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_NOTIFICATIONS:
+		IWL_DEBUG_INFO(tst->trans, "test notifications cmd\n");
+		result = iwl_test_notifications(tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
+		IWL_DEBUG_INFO(tst->trans, "test get FW ver cmd\n");
+		result = iwl_test_get_fw_ver(tst, tb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
+		IWL_DEBUG_INFO(tst->trans, "test Get device ID cmd\n");
+		result = iwl_test_get_dev_id(tst, tb);
+		break;
+
+	default:
+		IWL_DEBUG_INFO(tst->trans, "Unknown test command\n");
+		result = 1;
+		break;
+	}
+	return result;
+}
+EXPORT_SYMBOL_GPL(iwl_test_handle_cmd);
+
+static int iwl_test_trace_dump(struct iwl_test *tst, struct sk_buff *skb,
+			       struct netlink_callback *cb)
+{
+	int idx, length;
+
+	if (!tst->trace.enabled || !tst->trace.trace_addr)
+		return -EFAULT;
+
+	idx = cb->args[4];
+	if (idx >= tst->trace.nchunks)
+		return -ENOENT;
+
+	length = DUMP_CHUNK_SIZE;
+	if (((idx + 1) == tst->trace.nchunks) &&
+	    (tst->trace.size % DUMP_CHUNK_SIZE))
+		length = tst->trace.size %
+			DUMP_CHUNK_SIZE;
+
+	if (nla_put(skb, IWL_TM_ATTR_TRACE_DUMP, length,
+		    tst->trace.trace_addr + (DUMP_CHUNK_SIZE * idx)))
+		goto nla_put_failure;
+
+	cb->args[4] = ++idx;
+	return 0;
+
+ nla_put_failure:
+	return -ENOBUFS;
+}
+
+static int iwl_test_buffer_dump(struct iwl_test *tst, struct sk_buff *skb,
+				struct netlink_callback *cb)
+{
+	int idx, length;
+
+	if (!tst->mem.in_read)
+		return -EFAULT;
+
+	idx = cb->args[4];
+	if (idx >= tst->mem.nchunks) {
+		iwl_test_mem_stop(tst);
+		return -ENOENT;
+	}
+
+	length = DUMP_CHUNK_SIZE;
+	if (((idx + 1) == tst->mem.nchunks) &&
+	    (tst->mem.size % DUMP_CHUNK_SIZE))
+		length = tst->mem.size % DUMP_CHUNK_SIZE;
+
+	if (nla_put(skb, IWL_TM_ATTR_BUFFER_DUMP, length,
+		    tst->mem.addr + (DUMP_CHUNK_SIZE * idx)))
+		goto nla_put_failure;
+
+	cb->args[4] = ++idx;
+	return 0;
+
+ nla_put_failure:
+	return -ENOBUFS;
+}
+
+/*
+ * Handle dump commands.
+ * Returns 1 for unknown commands (not handled by the test object); negative
+ * value in case of error.
+ */
+int iwl_test_dump(struct iwl_test *tst, u32 cmd, struct sk_buff *skb,
+		  struct netlink_callback *cb)
+{
+	int result;
+
+	switch (cmd) {
+	case IWL_TM_CMD_APP2DEV_READ_TRACE:
+		IWL_DEBUG_INFO(tst->trans, "uCode trace cmd\n");
+		result = iwl_test_trace_dump(tst, skb, cb);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_DUMP:
+		IWL_DEBUG_INFO(tst->trans, "testmode sram dump cmd\n");
+		result = iwl_test_buffer_dump(tst, skb, cb);
+		break;
+
+	default:
+		result = 1;
+		break;
+	}
+	return result;
+}
+EXPORT_SYMBOL_GPL(iwl_test_dump);
+
+/*
+ * Multicast a spontaneous messages from the device to the user space.
+ */
+static void iwl_test_send_rx(struct iwl_test *tst,
+			     struct iwl_rx_cmd_buffer *rxb)
+{
+	struct sk_buff *skb;
+	struct iwl_rx_packet *data;
+	int length;
+
+	data = rxb_addr(rxb);
+	length = le32_to_cpu(data->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+
+	/* the length doesn't include len_n_flags field, so add it manually */
+	length += sizeof(__le32);
+
+	skb = iwl_test_alloc_event(tst, length + 20);
+	if (skb == NULL) {
+		IWL_ERR(tst->trans, "Out of memory for message to user\n");
+		return;
+	}
+
+	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+			IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
+	    nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, length, data))
+		goto nla_put_failure;
+
+	iwl_test_event(tst, skb);
+	return;
+
+nla_put_failure:
+	kfree_skb(skb);
+	IWL_ERR(tst->trans, "Ouch, overran buffer, check allocation!\n");
+}
+
+/*
+ * Called whenever a Rx frames is recevied from the device. If notifications to
+ * the user space are requested, sends the frames to the user.
+ */
+void iwl_test_rx(struct iwl_test *tst, struct iwl_rx_cmd_buffer *rxb)
+{
+	if (tst->notify)
+		iwl_test_send_rx(tst, rxb);
+}
+EXPORT_SYMBOL_GPL(iwl_test_rx);
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.h b/drivers/net/wireless/iwlwifi/iwl-test.h
new file mode 100644
index 00000000000..e13ffa8acc0
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-test.h
@@ -0,0 +1,161 @@
+/******************************************************************************
+ *
+ * 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) 2010 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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) 2010 - 2012 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_TEST_H__
+#define __IWL_TEST_H__
+
+#include <linux/types.h>
+#include "iwl-trans.h"
+
+struct iwl_test_trace {
+	u32 size;
+	u32 tsize;
+	u32 nchunks;
+	u8 *cpu_addr;
+	u8 *trace_addr;
+	dma_addr_t dma_addr;
+	bool enabled;
+};
+
+struct iwl_test_mem {
+	u32 size;
+	u32 nchunks;
+	u8 *addr;
+	bool in_read;
+};
+
+/*
+ * struct iwl_test_ops: callback to the op mode
+ *
+ * The structure defines the callbacks that the op_mode should handle,
+ * inorder to handle logic that is out of the scope of iwl_test. The
+ * op_mode must set all the callbacks.
+
+ * @send_cmd: handler that is used by the test object to request the
+ *  op_mode to send a command to the fw.
+ *
+ * @valid_hw_addr: handler that is used by the test object to request the
+ *  op_mode to check if the given address is a valid address.
+ *
+ * @get_fw_ver: handler used to get the FW version.
+ *
+ * @alloc_reply: handler used by the test object to request the op_mode
+ *  to allocate an skb for sending a reply to the user, and initialize
+ *  the skb. It is assumed that the test object only fills the required
+ *  attributes.
+ *
+ * @reply: handler used by the test object to request the op_mode to reply
+ *  to a request. The skb is an skb previously allocated by the the
+ *  alloc_reply callback.
+ I
+ * @alloc_event: handler used by the test object to request the op_mode
+ *  to allocate an skb for sending an event, and initialize
+ *  the skb. It is assumed that the test object only fills the required
+ *  attributes.
+ *
+ * @reply: handler used by the test object to request the op_mode to send
+ *  an event. The skb is an skb previously allocated by the the
+ *  alloc_event callback.
+ */
+struct iwl_test_ops {
+	int (*send_cmd)(struct iwl_op_mode *op_modes,
+			struct iwl_host_cmd *cmd);
+	bool (*valid_hw_addr)(u32 addr);
+	u32 (*get_fw_ver)(struct iwl_op_mode *op_mode);
+
+	struct sk_buff *(*alloc_reply)(struct iwl_op_mode *op_mode, int len);
+	int (*reply)(struct iwl_op_mode *op_mode, struct sk_buff *skb);
+	struct sk_buff* (*alloc_event)(struct iwl_op_mode *op_mode, int len);
+	void (*event)(struct iwl_op_mode *op_mode, struct sk_buff *skb);
+};
+
+struct iwl_test {
+	struct iwl_trans *trans;
+	struct iwl_test_ops *ops;
+	struct iwl_test_trace trace;
+	struct iwl_test_mem mem;
+	bool notify;
+};
+
+void iwl_test_init(struct iwl_test *tst, struct iwl_trans *trans,
+		   struct iwl_test_ops *ops);
+
+void iwl_test_free(struct iwl_test *tst);
+
+int iwl_test_parse(struct iwl_test *tst, struct nlattr **tb,
+		   void *data, int len);
+
+int iwl_test_handle_cmd(struct iwl_test *tst, struct nlattr **tb);
+
+int iwl_test_dump(struct iwl_test *tst, u32 cmd, struct sk_buff *skb,
+		  struct netlink_callback *cb);
+
+void iwl_test_rx(struct iwl_test *tst, struct iwl_rx_cmd_buffer *rxb);
+
+static inline void iwl_test_enable_notifications(struct iwl_test *tst,
+						 bool enable)
+{
+	tst->notify = enable;
+}
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-testmode.c b/drivers/net/wireless/iwlwifi/iwl-testmode.c
deleted file mode 100644
index 060aac3e22f..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-testmode.c
+++ /dev/null
@@ -1,1114 +0,0 @@
-/******************************************************************************
- *
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- * redistributing this file, you may do so under either license.
- *
- * GPL LICENSE SUMMARY
- *
- * Copyright(c) 2010 - 2012 Intel Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You 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) 2010 - 2012 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/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/dma-mapping.h>
-#include <net/net_namespace.h>
-#include <linux/netdevice.h>
-#include <net/cfg80211.h>
-#include <net/mac80211.h>
-#include <net/netlink.h>
-
-#include "iwl-dev.h"
-#include "iwl-debug.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-testmode.h"
-#include "iwl-trans.h"
-#include "iwl-fh.h"
-#include "iwl-prph.h"
-
-
-/* Periphery registers absolute lower bound. This is used in order to
- * differentiate registery access through HBUS_TARG_PRPH_* and
- * HBUS_TARG_MEM_* accesses.
- */
-#define IWL_TM_ABS_PRPH_START (0xA00000)
-
-/* The TLVs used in the gnl message policy between the kernel module and
- * user space application. iwl_testmode_gnl_msg_policy is to be carried
- * through the NL80211_CMD_TESTMODE channel regulated by nl80211.
- * See iwl-testmode.h
- */
-static
-struct nla_policy iwl_testmode_gnl_msg_policy[IWL_TM_ATTR_MAX] = {
-	[IWL_TM_ATTR_COMMAND] = { .type = NLA_U32, },
-
-	[IWL_TM_ATTR_UCODE_CMD_ID] = { .type = NLA_U8, },
-	[IWL_TM_ATTR_UCODE_CMD_DATA] = { .type = NLA_UNSPEC, },
-
-	[IWL_TM_ATTR_REG_OFFSET] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_REG_VALUE8] = { .type = NLA_U8, },
-	[IWL_TM_ATTR_REG_VALUE32] = { .type = NLA_U32, },
-
-	[IWL_TM_ATTR_SYNC_RSP] = { .type = NLA_UNSPEC, },
-	[IWL_TM_ATTR_UCODE_RX_PKT] = { .type = NLA_UNSPEC, },
-
-	[IWL_TM_ATTR_EEPROM] = { .type = NLA_UNSPEC, },
-
-	[IWL_TM_ATTR_TRACE_ADDR] = { .type = NLA_UNSPEC, },
-	[IWL_TM_ATTR_TRACE_DUMP] = { .type = NLA_UNSPEC, },
-	[IWL_TM_ATTR_TRACE_SIZE] = { .type = NLA_U32, },
-
-	[IWL_TM_ATTR_FIXRATE] = { .type = NLA_U32, },
-
-	[IWL_TM_ATTR_UCODE_OWNER] = { .type = NLA_U8, },
-
-	[IWL_TM_ATTR_MEM_ADDR] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_BUFFER_SIZE] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_BUFFER_DUMP] = { .type = NLA_UNSPEC, },
-
-	[IWL_TM_ATTR_FW_VERSION] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_DEVICE_ID] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_FW_TYPE] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_FW_INST_SIZE] = { .type = NLA_U32, },
-	[IWL_TM_ATTR_FW_DATA_SIZE] = { .type = NLA_U32, },
-
-	[IWL_TM_ATTR_ENABLE_NOTIFICATION] = {.type = NLA_FLAG, },
-};
-
-/*
- * See the struct iwl_rx_packet in iwl-commands.h for the format of the
- * received events from the device
- */
-static inline int get_event_length(struct iwl_rx_cmd_buffer *rxb)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	if (pkt)
-		return le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-	else
-		return 0;
-}
-
-
-/*
- * This function multicasts the spontaneous messages from the device to the
- * user space. It is invoked whenever there is a received messages
- * from the device. This function is called within the ISR of the rx handlers
- * in iwlagn driver.
- *
- * The parsing of the message content is left to the user space application,
- * The message content is treated as unattacked raw data and is encapsulated
- * with IWL_TM_ATTR_UCODE_RX_PKT multicasting to the user space.
- *
- * @priv: the instance of iwlwifi device
- * @rxb: pointer to rx data content received by the ISR
- *
- * See the message policies and TLVs in iwl_testmode_gnl_msg_policy[].
- * For the messages multicasting to the user application, the mandatory
- * TLV fields are :
- *	IWL_TM_ATTR_COMMAND must be IWL_TM_CMD_DEV2APP_UCODE_RX_PKT
- *	IWL_TM_ATTR_UCODE_RX_PKT for carrying the message content
- */
-
-static void iwl_testmode_ucode_rx_pkt(struct iwl_priv *priv,
-				      struct iwl_rx_cmd_buffer *rxb)
-{
-	struct ieee80211_hw *hw = priv->hw;
-	struct sk_buff *skb;
-	void *data;
-	int length;
-
-	data = (void *)rxb_addr(rxb);
-	length = get_event_length(rxb);
-
-	if (!data || length == 0)
-		return;
-
-	skb = cfg80211_testmode_alloc_event_skb(hw->wiphy, 20 + length,
-								GFP_ATOMIC);
-	if (skb == NULL) {
-		IWL_ERR(priv,
-			 "Run out of memory for messages to user space ?\n");
-		return;
-	}
-	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
-	    /* the length doesn't include len_n_flags field, so add it manually */
-	    nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, length + sizeof(__le32), data))
-		goto nla_put_failure;
-	cfg80211_testmode_event(skb, GFP_ATOMIC);
-	return;
-
-nla_put_failure:
-	kfree_skb(skb);
-	IWL_ERR(priv, "Ouch, overran buffer, check allocation!\n");
-}
-
-void iwl_testmode_init(struct iwl_priv *priv)
-{
-	priv->pre_rx_handler = NULL;
-	priv->testmode_trace.trace_enabled = false;
-	priv->testmode_mem.read_in_progress = false;
-}
-
-static void iwl_mem_cleanup(struct iwl_priv *priv)
-{
-	if (priv->testmode_mem.read_in_progress) {
-		kfree(priv->testmode_mem.buff_addr);
-		priv->testmode_mem.buff_addr = NULL;
-		priv->testmode_mem.buff_size = 0;
-		priv->testmode_mem.num_chunks = 0;
-		priv->testmode_mem.read_in_progress = false;
-	}
-}
-
-static void iwl_trace_cleanup(struct iwl_priv *priv)
-{
-	if (priv->testmode_trace.trace_enabled) {
-		if (priv->testmode_trace.cpu_addr &&
-		    priv->testmode_trace.dma_addr)
-			dma_free_coherent(priv->trans->dev,
-					priv->testmode_trace.total_size,
-					priv->testmode_trace.cpu_addr,
-					priv->testmode_trace.dma_addr);
-		priv->testmode_trace.trace_enabled = false;
-		priv->testmode_trace.cpu_addr = NULL;
-		priv->testmode_trace.trace_addr = NULL;
-		priv->testmode_trace.dma_addr = 0;
-		priv->testmode_trace.buff_size = 0;
-		priv->testmode_trace.total_size = 0;
-	}
-}
-
-
-void iwl_testmode_cleanup(struct iwl_priv *priv)
-{
-	iwl_trace_cleanup(priv);
-	iwl_mem_cleanup(priv);
-}
-
-
-/*
- * This function handles the user application commands to the ucode.
- *
- * It retrieves the mandatory fields IWL_TM_ATTR_UCODE_CMD_ID and
- * IWL_TM_ATTR_UCODE_CMD_DATA and calls to the handler to send the
- * host command to the ucode.
- *
- * If any mandatory field is missing, -ENOMSG is replied to the user space
- * application; otherwise, waits for the host command to be sent and checks
- * the return code. In case or error, it is returned, otherwise a reply is
- * allocated and the reply RX packet
- * is returned.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_ucode(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	struct iwl_host_cmd cmd;
-	struct iwl_rx_packet *pkt;
-	struct sk_buff *skb;
-	void *reply_buf;
-	u32 reply_len;
-	int ret;
-	bool cmd_want_skb;
-
-	memset(&cmd, 0, sizeof(struct iwl_host_cmd));
-
-	if (!tb[IWL_TM_ATTR_UCODE_CMD_ID] ||
-	    !tb[IWL_TM_ATTR_UCODE_CMD_DATA]) {
-		IWL_ERR(priv, "Missing ucode command mandatory fields\n");
-		return -ENOMSG;
-	}
-
-	cmd.flags = CMD_ON_DEMAND | CMD_SYNC;
-	cmd_want_skb = nla_get_flag(tb[IWL_TM_ATTR_UCODE_CMD_SKB]);
-	if (cmd_want_skb)
-		cmd.flags |= CMD_WANT_SKB;
-
-	cmd.id = nla_get_u8(tb[IWL_TM_ATTR_UCODE_CMD_ID]);
-	cmd.data[0] = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
-	cmd.len[0] = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
-	cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
-	IWL_DEBUG_INFO(priv, "testmode ucode command ID 0x%x, flags 0x%x,"
-				" len %d\n", cmd.id, cmd.flags, cmd.len[0]);
-
-	ret = iwl_dvm_send_cmd(priv, &cmd);
-	if (ret) {
-		IWL_ERR(priv, "Failed to send hcmd\n");
-		return ret;
-	}
-	if (!cmd_want_skb)
-		return ret;
-
-	/* Handling return of SKB to the user */
-	pkt = cmd.resp_pkt;
-	if (!pkt) {
-		IWL_ERR(priv, "HCMD received a null response packet\n");
-		return ret;
-	}
-
-	reply_len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-	skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, reply_len + 20);
-	reply_buf = kmalloc(reply_len, GFP_KERNEL);
-	if (!skb || !reply_buf) {
-		kfree_skb(skb);
-		kfree(reply_buf);
-		return -ENOMEM;
-	}
-
-	/* The reply is in a page, that we cannot send to user space. */
-	memcpy(reply_buf, &(pkt->hdr), reply_len);
-	iwl_free_resp(&cmd);
-
-	if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
-	    nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf))
-		goto nla_put_failure;
-	return cfg80211_testmode_reply(skb);
-
-nla_put_failure:
-	IWL_DEBUG_INFO(priv, "Failed creating NL attributes\n");
-	return -ENOMSG;
-}
-
-
-/*
- * This function handles the user application commands for register access.
- *
- * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
- * handlers respectively.
- *
- * If it's an unknown commdn ID, -ENOSYS is returned; or -ENOMSG if the
- * mandatory fields(IWL_TM_ATTR_REG_OFFSET,IWL_TM_ATTR_REG_VALUE32,
- * IWL_TM_ATTR_REG_VALUE8) are missing; Otherwise 0 is replied indicating
- * the success of the command execution.
- *
- * If IWL_TM_ATTR_COMMAND is IWL_TM_CMD_APP2DEV_REG_READ32, the register read
- * value is returned with IWL_TM_ATTR_REG_VALUE32.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	u32 ofs, val32, cmd;
-	u8 val8;
-	struct sk_buff *skb;
-	int status = 0;
-
-	if (!tb[IWL_TM_ATTR_REG_OFFSET]) {
-		IWL_ERR(priv, "Missing register offset\n");
-		return -ENOMSG;
-	}
-	ofs = nla_get_u32(tb[IWL_TM_ATTR_REG_OFFSET]);
-	IWL_INFO(priv, "testmode register access command offset 0x%x\n", ofs);
-
-	/* Allow access only to FH/CSR/HBUS in direct mode.
-	Since we don't have the upper bounds for the CSR and HBUS segments,
-	we will use only the upper bound of FH for sanity check. */
-	cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
-	if ((cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32 ||
-		cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32 ||
-		cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8) &&
-		(ofs >= FH_MEM_UPPER_BOUND)) {
-		IWL_ERR(priv, "offset out of segment (0x0 - 0x%x)\n",
-			FH_MEM_UPPER_BOUND);
-		return -EINVAL;
-	}
-
-	switch (cmd) {
-	case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
-		val32 = iwl_read_direct32(priv->trans, ofs);
-		IWL_INFO(priv, "32bit value to read 0x%x\n", val32);
-
-		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
-		if (!skb) {
-			IWL_ERR(priv, "Memory allocation fail\n");
-			return -ENOMEM;
-		}
-		if (nla_put_u32(skb, IWL_TM_ATTR_REG_VALUE32, val32))
-			goto nla_put_failure;
-		status = cfg80211_testmode_reply(skb);
-		if (status < 0)
-			IWL_ERR(priv, "Error sending msg : %d\n", status);
-		break;
-	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
-		if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
-			IWL_ERR(priv, "Missing value to write\n");
-			return -ENOMSG;
-		} else {
-			val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
-			IWL_INFO(priv, "32bit value to write 0x%x\n", val32);
-			iwl_write_direct32(priv->trans, ofs, val32);
-		}
-		break;
-	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
-		if (!tb[IWL_TM_ATTR_REG_VALUE8]) {
-			IWL_ERR(priv, "Missing value to write\n");
-			return -ENOMSG;
-		} else {
-			val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
-			IWL_INFO(priv, "8bit value to write 0x%x\n", val8);
-			iwl_write8(priv->trans, ofs, val8);
-		}
-		break;
-	default:
-		IWL_ERR(priv, "Unknown testmode register command ID\n");
-		return -ENOSYS;
-	}
-
-	return status;
-
-nla_put_failure:
-	kfree_skb(skb);
-	return -EMSGSIZE;
-}
-
-
-static int iwl_testmode_cfg_init_calib(struct iwl_priv *priv)
-{
-	struct iwl_notification_wait calib_wait;
-	static const u8 calib_complete[] = {
-		CALIBRATION_COMPLETE_NOTIFICATION
-	};
-	int ret;
-
-	iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
-				   calib_complete, ARRAY_SIZE(calib_complete),
-				   NULL, NULL);
-	ret = iwl_init_alive_start(priv);
-	if (ret) {
-		IWL_ERR(priv, "Fail init calibration: %d\n", ret);
-		goto cfg_init_calib_error;
-	}
-
-	ret = iwl_wait_notification(&priv->notif_wait, &calib_wait, 2 * HZ);
-	if (ret)
-		IWL_ERR(priv, "Error detecting"
-			" CALIBRATION_COMPLETE_NOTIFICATION: %d\n", ret);
-	return ret;
-
-cfg_init_calib_error:
-	iwl_remove_notification(&priv->notif_wait, &calib_wait);
-	return ret;
-}
-
-/*
- * This function handles the user application commands for driver.
- *
- * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
- * handlers respectively.
- *
- * If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
- * value of the actual command execution is replied to the user application.
- *
- * If there's any message responding to the user space, IWL_TM_ATTR_SYNC_RSP
- * is used for carry the message while IWL_TM_ATTR_COMMAND must set to
- * IWL_TM_CMD_DEV2APP_SYNC_RSP.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	struct iwl_trans *trans = priv->trans;
-	struct sk_buff *skb;
-	unsigned char *rsp_data_ptr = NULL;
-	int status = 0, rsp_data_len = 0;
-	u32 devid, inst_size = 0, data_size = 0;
-	const struct fw_img *img;
-
-	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
-	case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
-		rsp_data_ptr = (unsigned char *)priv->cfg->name;
-		rsp_data_len = strlen(priv->cfg->name);
-		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-							rsp_data_len + 20);
-		if (!skb) {
-			IWL_ERR(priv, "Memory allocation fail\n");
-			return -ENOMEM;
-		}
-		if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
-				IWL_TM_CMD_DEV2APP_SYNC_RSP) ||
-		    nla_put(skb, IWL_TM_ATTR_SYNC_RSP,
-			    rsp_data_len, rsp_data_ptr))
-			goto nla_put_failure;
-		status = cfg80211_testmode_reply(skb);
-		if (status < 0)
-			IWL_ERR(priv, "Error sending msg : %d\n", status);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
-		status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_INIT);
-		if (status)
-			IWL_ERR(priv, "Error loading init ucode: %d\n", status);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
-		iwl_testmode_cfg_init_calib(priv);
-		priv->ucode_loaded = false;
-		iwl_trans_stop_device(trans);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
-		status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_REGULAR);
-		if (status) {
-			IWL_ERR(priv,
-				"Error loading runtime ucode: %d\n", status);
-			break;
-		}
-		status = iwl_alive_start(priv);
-		if (status)
-			IWL_ERR(priv,
-				"Error starting the device: %d\n", status);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
-		iwl_scan_cancel_timeout(priv, 200);
-		priv->ucode_loaded = false;
-		iwl_trans_stop_device(trans);
-		status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_WOWLAN);
-		if (status) {
-			IWL_ERR(priv,
-				"Error loading WOWLAN ucode: %d\n", status);
-			break;
-		}
-		status = iwl_alive_start(priv);
-		if (status)
-			IWL_ERR(priv,
-				"Error starting the device: %d\n", status);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
-		if (priv->eeprom) {
-			skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-				priv->cfg->base_params->eeprom_size + 20);
-			if (!skb) {
-				IWL_ERR(priv, "Memory allocation fail\n");
-				return -ENOMEM;
-			}
-			if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
-					IWL_TM_CMD_DEV2APP_EEPROM_RSP) ||
-			    nla_put(skb, IWL_TM_ATTR_EEPROM,
-				    priv->cfg->base_params->eeprom_size,
-				    priv->eeprom))
-				goto nla_put_failure;
-			status = cfg80211_testmode_reply(skb);
-			if (status < 0)
-				IWL_ERR(priv, "Error sending msg : %d\n",
-					status);
-		} else
-			return -EFAULT;
-		break;
-
-	case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
-		if (!tb[IWL_TM_ATTR_FIXRATE]) {
-			IWL_ERR(priv, "Missing fixrate setting\n");
-			return -ENOMSG;
-		}
-		priv->tm_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
-		IWL_INFO(priv, "uCode version raw: 0x%x\n",
-			 priv->fw->ucode_ver);
-
-		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
-		if (!skb) {
-			IWL_ERR(priv, "Memory allocation fail\n");
-			return -ENOMEM;
-		}
-		if (nla_put_u32(skb, IWL_TM_ATTR_FW_VERSION,
-				priv->fw->ucode_ver))
-			goto nla_put_failure;
-		status = cfg80211_testmode_reply(skb);
-		if (status < 0)
-			IWL_ERR(priv, "Error sending msg : %d\n", status);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
-		devid = priv->trans->hw_id;
-		IWL_INFO(priv, "hw version: 0x%x\n", devid);
-
-		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
-		if (!skb) {
-			IWL_ERR(priv, "Memory allocation fail\n");
-			return -ENOMEM;
-		}
-		if (nla_put_u32(skb, IWL_TM_ATTR_DEVICE_ID, devid))
-			goto nla_put_failure;
-		status = cfg80211_testmode_reply(skb);
-		if (status < 0)
-			IWL_ERR(priv, "Error sending msg : %d\n", status);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
-		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20 + 8);
-		if (!skb) {
-			IWL_ERR(priv, "Memory allocation fail\n");
-			return -ENOMEM;
-		}
-		if (!priv->ucode_loaded) {
-			IWL_ERR(priv, "No uCode has not been loaded\n");
-			return -EINVAL;
-		} else {
-			img = &priv->fw->img[priv->cur_ucode];
-			inst_size = img->sec[IWL_UCODE_SECTION_INST].len;
-			data_size = img->sec[IWL_UCODE_SECTION_DATA].len;
-		}
-		if (nla_put_u32(skb, IWL_TM_ATTR_FW_TYPE, priv->cur_ucode) ||
-		    nla_put_u32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size) ||
-		    nla_put_u32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size))
-			goto nla_put_failure;
-		status = cfg80211_testmode_reply(skb);
-		if (status < 0)
-			IWL_ERR(priv, "Error sending msg : %d\n", status);
-		break;
-
-	default:
-		IWL_ERR(priv, "Unknown testmode driver command ID\n");
-		return -ENOSYS;
-	}
-	return status;
-
-nla_put_failure:
-	kfree_skb(skb);
-	return -EMSGSIZE;
-}
-
-
-/*
- * This function handles the user application commands for uCode trace
- *
- * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
- * handlers respectively.
- *
- * If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
- * value of the actual command execution is replied to the user application.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_trace(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	struct sk_buff *skb;
-	int status = 0;
-	struct device *dev = priv->trans->dev;
-
-	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
-	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
-		if (priv->testmode_trace.trace_enabled)
-			return -EBUSY;
-
-		if (!tb[IWL_TM_ATTR_TRACE_SIZE])
-			priv->testmode_trace.buff_size = TRACE_BUFF_SIZE_DEF;
-		else
-			priv->testmode_trace.buff_size =
-				nla_get_u32(tb[IWL_TM_ATTR_TRACE_SIZE]);
-		if (!priv->testmode_trace.buff_size)
-			return -EINVAL;
-		if (priv->testmode_trace.buff_size < TRACE_BUFF_SIZE_MIN ||
-		    priv->testmode_trace.buff_size > TRACE_BUFF_SIZE_MAX)
-			return -EINVAL;
-
-		priv->testmode_trace.total_size =
-			priv->testmode_trace.buff_size + TRACE_BUFF_PADD;
-		priv->testmode_trace.cpu_addr =
-			dma_alloc_coherent(dev,
-					   priv->testmode_trace.total_size,
-					   &priv->testmode_trace.dma_addr,
-					   GFP_KERNEL);
-		if (!priv->testmode_trace.cpu_addr)
-			return -ENOMEM;
-		priv->testmode_trace.trace_enabled = true;
-		priv->testmode_trace.trace_addr = (u8 *)PTR_ALIGN(
-			priv->testmode_trace.cpu_addr, 0x100);
-		memset(priv->testmode_trace.trace_addr, 0x03B,
-			priv->testmode_trace.buff_size);
-		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-			sizeof(priv->testmode_trace.dma_addr) + 20);
-		if (!skb) {
-			IWL_ERR(priv, "Memory allocation fail\n");
-			iwl_trace_cleanup(priv);
-			return -ENOMEM;
-		}
-		if (nla_put(skb, IWL_TM_ATTR_TRACE_ADDR,
-			    sizeof(priv->testmode_trace.dma_addr),
-			    (u64 *)&priv->testmode_trace.dma_addr))
-			goto nla_put_failure;
-		status = cfg80211_testmode_reply(skb);
-		if (status < 0) {
-			IWL_ERR(priv, "Error sending msg : %d\n", status);
-		}
-		priv->testmode_trace.num_chunks =
-			DIV_ROUND_UP(priv->testmode_trace.buff_size,
-				     DUMP_CHUNK_SIZE);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_END_TRACE:
-		iwl_trace_cleanup(priv);
-		break;
-	default:
-		IWL_ERR(priv, "Unknown testmode mem command ID\n");
-		return -ENOSYS;
-	}
-	return status;
-
-nla_put_failure:
-	kfree_skb(skb);
-	if (nla_get_u32(tb[IWL_TM_ATTR_COMMAND]) ==
-	    IWL_TM_CMD_APP2DEV_BEGIN_TRACE)
-		iwl_trace_cleanup(priv);
-	return -EMSGSIZE;
-}
-
-static int iwl_testmode_trace_dump(struct ieee80211_hw *hw,
-				   struct sk_buff *skb,
-				   struct netlink_callback *cb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	int idx, length;
-
-	if (priv->testmode_trace.trace_enabled &&
-	    priv->testmode_trace.trace_addr) {
-		idx = cb->args[4];
-		if (idx >= priv->testmode_trace.num_chunks)
-			return -ENOENT;
-		length = DUMP_CHUNK_SIZE;
-		if (((idx + 1) == priv->testmode_trace.num_chunks) &&
-		    (priv->testmode_trace.buff_size % DUMP_CHUNK_SIZE))
-			length = priv->testmode_trace.buff_size %
-				DUMP_CHUNK_SIZE;
-
-		if (nla_put(skb, IWL_TM_ATTR_TRACE_DUMP, length,
-			    priv->testmode_trace.trace_addr +
-			    (DUMP_CHUNK_SIZE * idx)))
-			goto nla_put_failure;
-		idx++;
-		cb->args[4] = idx;
-		return 0;
-	} else
-		return -EFAULT;
-
- nla_put_failure:
-	return -ENOBUFS;
-}
-
-/*
- * This function handles the user application switch ucode ownership.
- *
- * It retrieves the mandatory fields IWL_TM_ATTR_UCODE_OWNER and
- * decide who the current owner of the uCode
- *
- * If the current owner is OWNERSHIP_TM, then the only host command
- * can deliver to uCode is from testmode, all the other host commands
- * will dropped.
- *
- * default driver is the owner of uCode in normal operational mode
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	u8 owner;
-
-	if (!tb[IWL_TM_ATTR_UCODE_OWNER]) {
-		IWL_ERR(priv, "Missing ucode owner\n");
-		return -ENOMSG;
-	}
-
-	owner = nla_get_u8(tb[IWL_TM_ATTR_UCODE_OWNER]);
-	if (owner == IWL_OWNERSHIP_DRIVER) {
-		priv->ucode_owner = owner;
-		priv->pre_rx_handler = NULL;
-	} else if (owner == IWL_OWNERSHIP_TM) {
-		priv->pre_rx_handler = iwl_testmode_ucode_rx_pkt;
-		priv->ucode_owner = owner;
-	} else {
-		IWL_ERR(priv, "Invalid owner\n");
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int iwl_testmode_indirect_read(struct iwl_priv *priv, u32 addr, u32 size)
-{
-	struct iwl_trans *trans = priv->trans;
-	unsigned long flags;
-	int i;
-
-	if (size & 0x3)
-		return -EINVAL;
-	priv->testmode_mem.buff_size = size;
-	priv->testmode_mem.buff_addr =
-		kmalloc(priv->testmode_mem.buff_size, GFP_KERNEL);
-	if (priv->testmode_mem.buff_addr == NULL)
-		return -ENOMEM;
-
-	/* Hard-coded periphery absolute address */
-	if (IWL_TM_ABS_PRPH_START <= addr &&
-		addr < IWL_TM_ABS_PRPH_START + PRPH_END) {
-			spin_lock_irqsave(&trans->reg_lock, flags);
-			iwl_grab_nic_access(trans);
-			iwl_write32(trans, HBUS_TARG_PRPH_RADDR,
-				addr | (3 << 24));
-			for (i = 0; i < size; i += 4)
-				*(u32 *)(priv->testmode_mem.buff_addr + i) =
-					iwl_read32(trans, HBUS_TARG_PRPH_RDAT);
-			iwl_release_nic_access(trans);
-			spin_unlock_irqrestore(&trans->reg_lock, flags);
-	} else { /* target memory (SRAM) */
-		_iwl_read_targ_mem_words(trans, addr,
-			priv->testmode_mem.buff_addr,
-			priv->testmode_mem.buff_size / 4);
-	}
-
-	priv->testmode_mem.num_chunks =
-		DIV_ROUND_UP(priv->testmode_mem.buff_size, DUMP_CHUNK_SIZE);
-	priv->testmode_mem.read_in_progress = true;
-	return 0;
-
-}
-
-static int iwl_testmode_indirect_write(struct iwl_priv *priv, u32 addr,
-	u32 size, unsigned char *buf)
-{
-	struct iwl_trans *trans = priv->trans;
-	u32 val, i;
-	unsigned long flags;
-
-	if (IWL_TM_ABS_PRPH_START <= addr &&
-		addr < IWL_TM_ABS_PRPH_START + PRPH_END) {
-			/* Periphery writes can be 1-3 bytes long, or DWORDs */
-			if (size < 4) {
-				memcpy(&val, buf, size);
-				spin_lock_irqsave(&trans->reg_lock, flags);
-				iwl_grab_nic_access(trans);
-				iwl_write32(trans, HBUS_TARG_PRPH_WADDR,
-					    (addr & 0x0000FFFF) |
-					    ((size - 1) << 24));
-				iwl_write32(trans, HBUS_TARG_PRPH_WDAT, val);
-				iwl_release_nic_access(trans);
-				/* needed after consecutive writes w/o read */
-				mmiowb();
-				spin_unlock_irqrestore(&trans->reg_lock, flags);
-			} else {
-				if (size % 4)
-					return -EINVAL;
-				for (i = 0; i < size; i += 4)
-					iwl_write_prph(trans, addr+i,
-						*(u32 *)(buf+i));
-			}
-	} else if (iwlagn_hw_valid_rtc_data_addr(addr) ||
-		(IWLAGN_RTC_INST_LOWER_BOUND <= addr &&
-		addr < IWLAGN_RTC_INST_UPPER_BOUND)) {
-			_iwl_write_targ_mem_words(trans, addr, buf, size/4);
-	} else
-		return -EINVAL;
-	return 0;
-}
-
-/*
- * This function handles the user application commands for SRAM data dump
- *
- * It retrieves the mandatory fields IWL_TM_ATTR_SRAM_ADDR and
- * IWL_TM_ATTR_SRAM_SIZE to decide the memory area for SRAM data reading
- *
- * Several error will be retured, -EBUSY if the SRAM data retrieved by
- * previous command has not been delivered to userspace, or -ENOMSG if
- * the mandatory fields (IWL_TM_ATTR_SRAM_ADDR,IWL_TM_ATTR_SRAM_SIZE)
- * are missing, or -ENOMEM if the buffer allocation fails.
- *
- * Otherwise 0 is replied indicating the success of the SRAM reading.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_indirect_mem(struct ieee80211_hw *hw,
-	struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	u32 addr, size, cmd;
-	unsigned char *buf;
-
-	/* Both read and write should be blocked, for atomicity */
-	if (priv->testmode_mem.read_in_progress)
-		return -EBUSY;
-
-	cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
-	if (!tb[IWL_TM_ATTR_MEM_ADDR]) {
-		IWL_ERR(priv, "Error finding memory offset address\n");
-		return -ENOMSG;
-	}
-	addr = nla_get_u32(tb[IWL_TM_ATTR_MEM_ADDR]);
-	if (!tb[IWL_TM_ATTR_BUFFER_SIZE]) {
-		IWL_ERR(priv, "Error finding size for memory reading\n");
-		return -ENOMSG;
-	}
-	size = nla_get_u32(tb[IWL_TM_ATTR_BUFFER_SIZE]);
-
-	if (cmd == IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ)
-		return iwl_testmode_indirect_read(priv, addr,  size);
-	else {
-		if (!tb[IWL_TM_ATTR_BUFFER_DUMP])
-			return -EINVAL;
-		buf = (unsigned char *) nla_data(tb[IWL_TM_ATTR_BUFFER_DUMP]);
-		return iwl_testmode_indirect_write(priv, addr, size, buf);
-	}
-}
-
-static int iwl_testmode_buffer_dump(struct ieee80211_hw *hw,
-				    struct sk_buff *skb,
-				    struct netlink_callback *cb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	int idx, length;
-
-	if (priv->testmode_mem.read_in_progress) {
-		idx = cb->args[4];
-		if (idx >= priv->testmode_mem.num_chunks) {
-			iwl_mem_cleanup(priv);
-			return -ENOENT;
-		}
-		length = DUMP_CHUNK_SIZE;
-		if (((idx + 1) == priv->testmode_mem.num_chunks) &&
-		    (priv->testmode_mem.buff_size % DUMP_CHUNK_SIZE))
-			length = priv->testmode_mem.buff_size %
-				DUMP_CHUNK_SIZE;
-
-		if (nla_put(skb, IWL_TM_ATTR_BUFFER_DUMP, length,
-			    priv->testmode_mem.buff_addr +
-			    (DUMP_CHUNK_SIZE * idx)))
-			goto nla_put_failure;
-		idx++;
-		cb->args[4] = idx;
-		return 0;
-	} else
-		return -EFAULT;
-
- nla_put_failure:
-	return -ENOBUFS;
-}
-
-static int iwl_testmode_notifications(struct ieee80211_hw *hw,
-	struct nlattr **tb)
-{
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	bool enable;
-
-	enable = nla_get_flag(tb[IWL_TM_ATTR_ENABLE_NOTIFICATION]);
-	if (enable)
-		priv->pre_rx_handler = iwl_testmode_ucode_rx_pkt;
-	else
-		priv->pre_rx_handler = NULL;
-	return 0;
-}
-
-
-/* The testmode gnl message handler that takes the gnl message from the
- * user space and parses it per the policy iwl_testmode_gnl_msg_policy, then
- * invoke the corresponding handlers.
- *
- * This function is invoked when there is user space application sending
- * gnl message through the testmode tunnel NL80211_CMD_TESTMODE regulated
- * by nl80211.
- *
- * It retrieves the mandatory field, IWL_TM_ATTR_COMMAND, before
- * dispatching it to the corresponding handler.
- *
- * If IWL_TM_ATTR_COMMAND is missing, -ENOMSG is replied to user application;
- * -ENOSYS is replied to the user application if the command is unknown;
- * Otherwise, the command is dispatched to the respective handler.
- *
- * @hw: ieee80211_hw object that represents the device
- * @data: pointer to user space message
- * @len: length in byte of @data
- */
-int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
-{
-	struct nlattr *tb[IWL_TM_ATTR_MAX];
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	int result;
-
-	result = nla_parse(tb, IWL_TM_ATTR_MAX - 1, data, len,
-			iwl_testmode_gnl_msg_policy);
-	if (result != 0) {
-		IWL_ERR(priv, "Error parsing the gnl message : %d\n", result);
-		return result;
-	}
-
-	/* IWL_TM_ATTR_COMMAND is absolutely mandatory */
-	if (!tb[IWL_TM_ATTR_COMMAND]) {
-		IWL_ERR(priv, "Missing testmode command type\n");
-		return -ENOMSG;
-	}
-	/* in case multiple accesses to the device happens */
-	mutex_lock(&priv->mutex);
-
-	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
-	case IWL_TM_CMD_APP2DEV_UCODE:
-		IWL_DEBUG_INFO(priv, "testmode cmd to uCode\n");
-		result = iwl_testmode_ucode(hw, tb);
-		break;
-	case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
-	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
-	case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
-		IWL_DEBUG_INFO(priv, "testmode cmd to register\n");
-		result = iwl_testmode_reg(hw, tb);
-		break;
-	case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
-	case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
-	case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
-	case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
-	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
-	case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
-	case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
-	case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
-	case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
-	case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
-		IWL_DEBUG_INFO(priv, "testmode cmd to driver\n");
-		result = iwl_testmode_driver(hw, tb);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
-	case IWL_TM_CMD_APP2DEV_END_TRACE:
-	case IWL_TM_CMD_APP2DEV_READ_TRACE:
-		IWL_DEBUG_INFO(priv, "testmode uCode trace cmd to driver\n");
-		result = iwl_testmode_trace(hw, tb);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_OWNERSHIP:
-		IWL_DEBUG_INFO(priv, "testmode change uCode ownership\n");
-		result = iwl_testmode_ownership(hw, tb);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ:
-	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_WRITE:
-		IWL_DEBUG_INFO(priv, "testmode indirect memory cmd "
-			"to driver\n");
-		result = iwl_testmode_indirect_mem(hw, tb);
-		break;
-
-	case IWL_TM_CMD_APP2DEV_NOTIFICATIONS:
-		IWL_DEBUG_INFO(priv, "testmode notifications cmd "
-			"to driver\n");
-		result = iwl_testmode_notifications(hw, tb);
-		break;
-
-	default:
-		IWL_ERR(priv, "Unknown testmode command\n");
-		result = -ENOSYS;
-		break;
-	}
-
-	mutex_unlock(&priv->mutex);
-	return result;
-}
-
-int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
-		      struct netlink_callback *cb,
-		      void *data, int len)
-{
-	struct nlattr *tb[IWL_TM_ATTR_MAX];
-	struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-	int result;
-	u32 cmd;
-
-	if (cb->args[3]) {
-		/* offset by 1 since commands start at 0 */
-		cmd = cb->args[3] - 1;
-	} else {
-		result = nla_parse(tb, IWL_TM_ATTR_MAX - 1, data, len,
-				iwl_testmode_gnl_msg_policy);
-		if (result) {
-			IWL_ERR(priv,
-				"Error parsing the gnl message : %d\n", result);
-			return result;
-		}
-
-		/* IWL_TM_ATTR_COMMAND is absolutely mandatory */
-		if (!tb[IWL_TM_ATTR_COMMAND]) {
-			IWL_ERR(priv, "Missing testmode command type\n");
-			return -ENOMSG;
-		}
-		cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
-		cb->args[3] = cmd + 1;
-	}
-
-	/* in case multiple accesses to the device happens */
-	mutex_lock(&priv->mutex);
-	switch (cmd) {
-	case IWL_TM_CMD_APP2DEV_READ_TRACE:
-		IWL_DEBUG_INFO(priv, "uCode trace cmd to driver\n");
-		result = iwl_testmode_trace_dump(hw, skb, cb);
-		break;
-	case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_DUMP:
-		IWL_DEBUG_INFO(priv, "testmode sram dump cmd to driver\n");
-		result = iwl_testmode_buffer_dump(hw, skb, cb);
-		break;
-	default:
-		result = -EINVAL;
-		break;
-	}
-
-	mutex_unlock(&priv->mutex);
-	return result;
-}
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 79a1e7ae499..92576a3e84e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -154,6 +154,9 @@ struct iwl_cmd_header {
 	__le16 sequence;
 } __packed;
 
+/* iwl_cmd_header flags value */
+#define IWL_CMD_FAILED_MSK 0x40
+
 
 #define FH_RSCSR_FRAME_SIZE_MSK		0x00003FFF	/* bits 0-13 */
 #define FH_RSCSR_FRAME_INVALID		0x55550000
@@ -280,21 +283,24 @@ static inline struct page *rxb_steal_page(struct iwl_rx_cmd_buffer *r)
 
 #define MAX_NO_RECLAIM_CMDS	6
 
+#define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
+
 /*
  * Maximum number of HW queues the transport layer
  * currently supports
  */
 #define IWL_MAX_HW_QUEUES		32
+#define IWL_INVALID_STATION	255
+#define IWL_MAX_TID_COUNT	8
+#define IWL_FRAME_LIMIT	64
 
 /**
  * struct iwl_trans_config - transport configuration
  *
  * @op_mode: pointer to the upper layer.
- * @queue_to_fifo: queue to FIFO mapping to set up by
- *	default
- * @n_queue_to_fifo: number of queues to set up
  * @cmd_queue: the index of the command queue.
  *	Must be set before start_fw.
+ * @cmd_fifo: the fifo for host commands
  * @no_reclaim_cmds: Some devices erroneously don't set the
  *	SEQ_RX_FRAME bit on some notifications, this is the
  *	list of such notifications to filter. Max length is
@@ -309,10 +315,9 @@ static inline struct page *rxb_steal_page(struct iwl_rx_cmd_buffer *r)
  */
 struct iwl_trans_config {
 	struct iwl_op_mode *op_mode;
-	const u8 *queue_to_fifo;
-	u8 n_queue_to_fifo;
 
 	u8 cmd_queue;
+	u8 cmd_fifo;
 	const u8 *no_reclaim_cmds;
 	int n_no_reclaim_cmds;
 
@@ -350,10 +355,10 @@ struct iwl_trans;
  *	Must be atomic
  * @reclaim: free packet until ssn. Returns a list of freed packets.
  *	Must be atomic
- * @tx_agg_setup: setup a tx queue for AMPDU - will be called once the HW is
- *	ready and a successful ADDBA response has been received.
- *	May sleep
- * @tx_agg_disable: de-configure a Tx queue to send AMPDUs
+ * @txq_enable: setup a queue. To setup an AC queue, use the
+ *	iwl_trans_ac_txq_enable wrapper. fw_alive must have been called before
+ *	this one. The op_mode must not configure the HCMD queue. May sleep.
+ * @txq_disable: de-configure a Tx queue to send AMPDUs
  *	Must be atomic
  * @wait_tx_queue_empty: wait until all tx queues are empty
  *	May sleep
@@ -386,9 +391,9 @@ struct iwl_trans_ops {
 	void (*reclaim)(struct iwl_trans *trans, int queue, int ssn,
 			struct sk_buff_head *skbs);
 
-	void (*tx_agg_setup)(struct iwl_trans *trans, int queue, int fifo,
-			     int sta_id, int tid, int frame_limit, u16 ssn);
-	void (*tx_agg_disable)(struct iwl_trans *trans, int queue);
+	void (*txq_enable)(struct iwl_trans *trans, int queue, int fifo,
+			   int sta_id, int tid, int frame_limit, u16 ssn);
+	void (*txq_disable)(struct iwl_trans *trans, int queue);
 
 	int (*dbgfs_register)(struct iwl_trans *trans, struct dentry* dir);
 	int (*wait_tx_queue_empty)(struct iwl_trans *trans);
@@ -428,6 +433,11 @@ enum iwl_trans_state {
  * @hw_id_str: a string with info about HW ID. Set during transport allocation.
  * @pm_support: set to true in start_hw if link pm is supported
  * @wait_command_queue: the wait_queue for SYNC host commands
+ * @dev_cmd_pool: pool for Tx cmd allocation - for internal use only.
+ *	The user should use iwl_trans_{alloc,free}_tx_cmd.
+ * @dev_cmd_headroom: room needed for the transport's private use before the
+ *	device_cmd for Tx - for internal use only
+ *	The user should use iwl_trans_{alloc,free}_tx_cmd.
  */
 struct iwl_trans {
 	const struct iwl_trans_ops *ops;
@@ -445,6 +455,11 @@ struct iwl_trans {
 
 	wait_queue_head_t wait_command_queue;
 
+	/* The following fields are internal only */
+	struct kmem_cache *dev_cmd_pool;
+	size_t dev_cmd_headroom;
+	char dev_cmd_pool_name[50];
+
 	/* pointer to trans specific struct */
 	/*Ensure that this pointer will always be aligned to sizeof pointer */
 	char trans_specific[0] __aligned(sizeof(void *));
@@ -483,9 +498,9 @@ static inline void iwl_trans_fw_alive(struct iwl_trans *trans)
 {
 	might_sleep();
 
-	trans->ops->fw_alive(trans);
-
 	trans->state = IWL_TRANS_FW_ALIVE;
+
+	trans->ops->fw_alive(trans);
 }
 
 static inline int iwl_trans_start_fw(struct iwl_trans *trans,
@@ -520,6 +535,26 @@ static inline int iwl_trans_send_cmd(struct iwl_trans *trans,
 	return trans->ops->send_cmd(trans, cmd);
 }
 
+static inline struct iwl_device_cmd *
+iwl_trans_alloc_tx_cmd(struct iwl_trans *trans)
+{
+	u8 *dev_cmd_ptr = kmem_cache_alloc(trans->dev_cmd_pool, GFP_ATOMIC);
+
+	if (unlikely(dev_cmd_ptr == NULL))
+		return NULL;
+
+	return (struct iwl_device_cmd *)
+			(dev_cmd_ptr + trans->dev_cmd_headroom);
+}
+
+static inline void iwl_trans_free_tx_cmd(struct iwl_trans *trans,
+					 struct iwl_device_cmd *dev_cmd)
+{
+	u8 *dev_cmd_ptr = (u8 *)dev_cmd - trans->dev_cmd_headroom;
+
+	kmem_cache_free(trans->dev_cmd_pool, dev_cmd_ptr);
+}
+
 static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
 			       struct iwl_device_cmd *dev_cmd, int queue)
 {
@@ -538,27 +573,34 @@ static inline void iwl_trans_reclaim(struct iwl_trans *trans, int queue,
 	trans->ops->reclaim(trans, queue, ssn, skbs);
 }
 
-static inline void iwl_trans_tx_agg_disable(struct iwl_trans *trans, int queue)
+static inline void iwl_trans_txq_disable(struct iwl_trans *trans, int queue)
 {
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
 		  "%s bad state = %d", __func__, trans->state);
 
-	trans->ops->tx_agg_disable(trans, queue);
+	trans->ops->txq_disable(trans, queue);
 }
 
-static inline void iwl_trans_tx_agg_setup(struct iwl_trans *trans, int queue,
-					  int fifo, int sta_id, int tid,
-					  int frame_limit, u16 ssn)
+static inline void iwl_trans_txq_enable(struct iwl_trans *trans, int queue,
+					int fifo, int sta_id, int tid,
+					int frame_limit, u16 ssn)
 {
 	might_sleep();
 
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
 		  "%s bad state = %d", __func__, trans->state);
 
-	trans->ops->tx_agg_setup(trans, queue, fifo, sta_id, tid,
+	trans->ops->txq_enable(trans, queue, fifo, sta_id, tid,
 				 frame_limit, ssn);
 }
 
+static inline void iwl_trans_ac_txq_enable(struct iwl_trans *trans, int queue,
+					   int fifo)
+{
+	iwl_trans_txq_enable(trans, queue, fifo, IWL_INVALID_STATION,
+			     IWL_MAX_TID_COUNT, IWL_FRAME_LIMIT, 0);
+}
+
 static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans)
 {
 	WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/iwlwifi/pcie/1000.c
index 2629a6602df..81b83f484f0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/1000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-csr.h"
 #include "iwl-agn-hw.h"
+#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL1000_UCODE_API_MAX 5
@@ -64,13 +64,26 @@ static const struct iwl_base_params iwl1000_base_params = {
 	.support_ct_kill_exit = true,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_EXT_LONG_THRESHOLD_DEF,
 	.chain_noise_scale = 1000,
-	.wd_timeout = IWL_WATCHHDOG_DISABLED,
+	.wd_timeout = IWL_WATCHDOG_DISABLED,
 	.max_event_log_size = 128,
 };
 
 static const struct iwl_ht_params iwl1000_ht_params = {
 	.ht_greenfield_support = true,
 	.use_rts_for_aggregation = true, /* use rts/cts protection */
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ),
+};
+
+static const struct iwl_eeprom_params iwl1000_eeprom_params = {
+	.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_REG_BAND_24_HT40_CHANNELS,
+		EEPROM_REGULATORY_BAND_NO_HT40,
+	}
 };
 
 #define IWL_DEVICE_1000						\
@@ -84,6 +97,7 @@ static const struct iwl_ht_params iwl1000_ht_params = {
 	.eeprom_ver = EEPROM_1000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_1000_TX_POWER_VERSION,	\
 	.base_params = &iwl1000_base_params,			\
+	.eeprom_params = &iwl1000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK
 
 const struct iwl_cfg iwl1000_bgn_cfg = {
@@ -108,6 +122,7 @@ const struct iwl_cfg iwl1000_bg_cfg = {
 	.eeprom_ver = EEPROM_1000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_1000_TX_POWER_VERSION,	\
 	.base_params = &iwl1000_base_params,			\
+	.eeprom_params = &iwl1000_eeprom_params,		\
 	.led_mode = IWL_LED_RF_STATE,				\
 	.rx_with_siso_diversity = true
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/iwlwifi/pcie/2000.c
index 8133105ac64..9fbde32f755 100644
--- a/drivers/net/wireless/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/2000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-agn-hw.h"
-#include "iwl-commands.h" /* needed for BT for now */
+#include "cfg.h"
+#include "dvm/commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
 #define IWL2030_UCODE_API_MAX 6
@@ -104,6 +104,7 @@ static const struct iwl_base_params iwl2030_base_params = {
 static const struct iwl_ht_params iwl2000_ht_params = {
 	.ht_greenfield_support = true,
 	.use_rts_for_aggregation = true, /* use rts/cts protection */
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ),
 };
 
 static const struct iwl_bt_params iwl2030_bt_params = {
@@ -111,11 +112,24 @@ static const struct iwl_bt_params iwl2030_bt_params = {
 	.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_prio_boost = IWLAGN_BT_PRIO_BOOST_DEFAULT32,
 	.bt_sco_disable = true,
 	.bt_session_2 = true,
 };
 
+static const struct iwl_eeprom_params iwl20x0_eeprom_params = {
+	.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_REGULATORY_BAND_NO_HT40,
+	},
+	.enhanced_txpower = true,
+};
+
 #define IWL_DEVICE_2000						\
 	.fw_name_pre = IWL2000_FW_PRE,				\
 	.ucode_api_max = IWL2000_UCODE_API_MAX,			\
@@ -127,6 +141,7 @@ static const struct iwl_bt_params iwl2030_bt_params = {
 	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
 	.base_params = &iwl2000_base_params,			\
+	.eeprom_params = &iwl20x0_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE
@@ -155,6 +170,7 @@ const struct iwl_cfg iwl2000_2bgn_d_cfg = {
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
 	.base_params = &iwl2030_base_params,			\
 	.bt_params = &iwl2030_bt_params,			\
+	.eeprom_params = &iwl20x0_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE,				\
@@ -177,6 +193,7 @@ const struct iwl_cfg iwl2030_2bgn_cfg = {
 	.eeprom_ver = EEPROM_2000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
 	.base_params = &iwl2000_base_params,			\
+	.eeprom_params = &iwl20x0_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE,				\
@@ -207,6 +224,7 @@ const struct iwl_cfg iwl105_bgn_d_cfg = {
 	.eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,	\
 	.base_params = &iwl2030_base_params,			\
 	.bt_params = &iwl2030_bt_params,			\
+	.eeprom_params = &iwl20x0_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.temp_offset_v2 = true,					\
 	.led_mode = IWL_LED_RF_STATE,				\
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/pcie/5000.c
index 8e26bc825f2..d1665fa6d15 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/5000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-agn-hw.h"
 #include "iwl-csr.h"
+#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL5000_UCODE_API_MAX 5
@@ -62,13 +62,26 @@ static const struct iwl_base_params iwl5000_base_params = {
 	.led_compensation = 51,
 	.plcp_delta_threshold = IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF,
 	.chain_noise_scale = 1000,
-	.wd_timeout = IWL_WATCHHDOG_DISABLED,
+	.wd_timeout = IWL_WATCHDOG_DISABLED,
 	.max_event_log_size = 512,
 	.no_idle_support = true,
 };
 
 static const struct iwl_ht_params iwl5000_ht_params = {
 	.ht_greenfield_support = true,
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
+};
+
+static const struct iwl_eeprom_params iwl5000_eeprom_params = {
+	.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_REG_BAND_24_HT40_CHANNELS,
+		EEPROM_REG_BAND_52_HT40_CHANNELS
+	},
 };
 
 #define IWL_DEVICE_5000						\
@@ -82,6 +95,7 @@ static const struct iwl_ht_params iwl5000_ht_params = {
 	.eeprom_ver = EEPROM_5000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_5000_TX_POWER_VERSION,	\
 	.base_params = &iwl5000_base_params,			\
+	.eeprom_params = &iwl5000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK
 
 const struct iwl_cfg iwl5300_agn_cfg = {
@@ -128,6 +142,7 @@ const struct iwl_cfg iwl5350_agn_cfg = {
 	.eeprom_ver = EEPROM_5050_EEPROM_VERSION,
 	.eeprom_calib_ver = EEPROM_5050_TX_POWER_VERSION,
 	.base_params = &iwl5000_base_params,
+	.eeprom_params = &iwl5000_eeprom_params,
 	.ht_params = &iwl5000_ht_params,
 	.led_mode = IWL_LED_BLINK,
 	.internal_wimax_coex = true,
@@ -144,6 +159,7 @@ const struct iwl_cfg iwl5350_agn_cfg = {
 	.eeprom_ver = EEPROM_5050_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_5050_TX_POWER_VERSION,	\
 	.base_params = &iwl5000_base_params,			\
+	.eeprom_params = &iwl5000_eeprom_params,		\
 	.no_xtal_calib = true,					\
 	.led_mode = IWL_LED_BLINK,				\
 	.internal_wimax_coex = true
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/iwlwifi/pcie/6000.c
index e5e8ada4aaf..4a57624afc4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/6000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-agn-hw.h"
-#include "iwl-commands.h" /* needed for BT for now */
+#include "cfg.h"
+#include "dvm/commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
 #define IWL6000_UCODE_API_MAX 6
@@ -127,6 +127,7 @@ static const struct iwl_base_params iwl6000_g2_base_params = {
 static const struct iwl_ht_params iwl6000_ht_params = {
 	.ht_greenfield_support = true,
 	.use_rts_for_aggregation = true, /* use rts/cts protection */
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
 };
 
 static const struct iwl_bt_params iwl6000_bt_params = {
@@ -138,6 +139,19 @@ static const struct iwl_bt_params iwl6000_bt_params = {
 	.bt_sco_disable = true,
 };
 
+static const struct iwl_eeprom_params iwl6000_eeprom_params = {
+	.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
+	},
+	.enhanced_txpower = true,
+};
+
 #define IWL_DEVICE_6005						\
 	.fw_name_pre = IWL6005_FW_PRE,				\
 	.ucode_api_max = IWL6000G2_UCODE_API_MAX,		\
@@ -149,6 +163,7 @@ static const struct iwl_bt_params iwl6000_bt_params = {
 	.eeprom_ver = EEPROM_6005_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6005_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_g2_base_params,			\
+	.eeprom_params = &iwl6000_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.led_mode = IWL_LED_RF_STATE
 
@@ -204,6 +219,7 @@ const struct iwl_cfg iwl6005_2agn_mow2_cfg = {
 	.eeprom_calib_ver = EEPROM_6030_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_g2_base_params,			\
 	.bt_params = &iwl6000_bt_params,			\
+	.eeprom_params = &iwl6000_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.led_mode = IWL_LED_RF_STATE,				\
 	.adv_pm = true						\
@@ -242,6 +258,7 @@ const struct iwl_cfg iwl6030_2bg_cfg = {
 	.eeprom_calib_ver = EEPROM_6030_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_g2_base_params,			\
 	.bt_params = &iwl6000_bt_params,			\
+	.eeprom_params = &iwl6000_eeprom_params,		\
 	.need_temp_offset_calib = true,				\
 	.led_mode = IWL_LED_RF_STATE,				\
 	.adv_pm = true
@@ -292,6 +309,7 @@ const struct iwl_cfg iwl130_bg_cfg = {
 	.eeprom_ver = EEPROM_6000_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6000_TX_POWER_VERSION,	\
 	.base_params = &iwl6000_base_params,			\
+	.eeprom_params = &iwl6000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK
 
 const struct iwl_cfg iwl6000i_2agn_cfg = {
@@ -322,6 +340,7 @@ const struct iwl_cfg iwl6000i_2bg_cfg = {
 	.eeprom_ver = EEPROM_6050_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6050_TX_POWER_VERSION,	\
 	.base_params = &iwl6050_base_params,			\
+	.eeprom_params = &iwl6000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK,				\
 	.internal_wimax_coex = true
 
@@ -346,6 +365,7 @@ const struct iwl_cfg iwl6050_2abg_cfg = {
 	.eeprom_ver = EEPROM_6150_EEPROM_VERSION,		\
 	.eeprom_calib_ver = EEPROM_6150_TX_POWER_VERSION,	\
 	.base_params = &iwl6050_base_params,			\
+	.eeprom_params = &iwl6000_eeprom_params,		\
 	.led_mode = IWL_LED_BLINK,				\
 	.internal_wimax_coex = true
 
@@ -372,6 +392,7 @@ const struct iwl_cfg iwl6000_3agn_cfg = {
 	.eeprom_ver = EEPROM_6000_EEPROM_VERSION,
 	.eeprom_calib_ver = EEPROM_6000_TX_POWER_VERSION,
 	.base_params = &iwl6000_base_params,
+	.eeprom_params = &iwl6000_eeprom_params,
 	.ht_params = &iwl6000_ht_params,
 	.led_mode = IWL_LED_BLINK,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-cfg.h b/drivers/net/wireless/iwlwifi/pcie/cfg.h
index 82152311d73..82152311d73 100644
--- a/drivers/net/wireless/iwlwifi/iwl-cfg.h
+++ b/drivers/net/wireless/iwlwifi/pcie/cfg.h
diff --git a/drivers/net/wireless/iwlwifi/iwl-pci.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index 0c8a1c2d884..f4c3500b68c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-pci.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -68,10 +68,11 @@
 #include <linux/pci-aspm.h>
 
 #include "iwl-trans.h"
-#include "iwl-cfg.h"
 #include "iwl-drv.h"
 #include "iwl-trans.h"
-#include "iwl-trans-pcie-int.h"
+
+#include "cfg.h"
+#include "internal.h"
 
 #define IWL_PCI_DEVICE(dev, subdev, cfg) \
 	.vendor = PCI_VENDOR_ID_INTEL,  .device = (dev), \
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index e959207c630..d9694c58208 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -269,10 +269,9 @@ struct iwl_trans_pcie {
 	wait_queue_head_t ucode_write_waitq;
 	unsigned long status;
 	u8 cmd_queue;
+	u8 cmd_fifo;
 	u8 n_no_reclaim_cmds;
 	u8 no_reclaim_cmds[MAX_NO_RECLAIM_CMDS];
-	u8 setup_q_to_fifo[IWL_MAX_HW_QUEUES];
-	u8 n_q_to_fifo;
 
 	bool rx_buf_size_8k;
 	u32 rx_page_order;
@@ -313,7 +312,7 @@ void iwl_bg_rx_replenish(struct work_struct *data);
 void iwl_irq_tasklet(struct iwl_trans *trans);
 void iwlagn_rx_replenish(struct iwl_trans *trans);
 void iwl_rx_queue_update_write_ptr(struct iwl_trans *trans,
-			struct iwl_rx_queue *q);
+				   struct iwl_rx_queue *q);
 
 /*****************************************************
 * ICT
@@ -328,7 +327,7 @@ irqreturn_t iwl_isr_ict(int irq, void *data);
 * TX / HCMD
 ******************************************************/
 void iwl_txq_update_write_ptr(struct iwl_trans *trans,
-			struct iwl_tx_queue *txq);
+			      struct iwl_tx_queue *txq);
 int iwlagn_txq_attach_buf_to_tfd(struct iwl_trans *trans,
 				 struct iwl_tx_queue *txq,
 				 dma_addr_t addr, u16 len, u8 reset);
@@ -337,17 +336,13 @@ int iwl_trans_pcie_send_cmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd);
 void iwl_tx_cmd_complete(struct iwl_trans *trans,
 			 struct iwl_rx_cmd_buffer *rxb, int handler_status);
 void iwl_trans_txq_update_byte_cnt_tbl(struct iwl_trans *trans,
-					   struct iwl_tx_queue *txq,
-					   u16 byte_cnt);
-void iwl_trans_pcie_tx_agg_disable(struct iwl_trans *trans, int queue);
-void iwl_trans_set_wr_ptrs(struct iwl_trans *trans, int txq_id, u32 index);
-void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
-				   struct iwl_tx_queue *txq,
-				   int tx_fifo_id, bool active);
-void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans, int queue, int fifo,
-				 int sta_id, int tid, int frame_limit, u16 ssn);
-void iwlagn_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
-			 enum dma_data_direction dma_dir);
+				       struct iwl_tx_queue *txq,
+				       u16 byte_cnt);
+void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
+			       int sta_id, int tid, int frame_limit, u16 ssn);
+void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int queue);
+void iwl_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
+		      enum dma_data_direction dma_dir);
 int iwl_tx_queue_reclaim(struct iwl_trans *trans, int txq_id, int index,
 			 struct sk_buff_head *skbs);
 int iwl_queue_space(const struct iwl_queue *q);
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index 08517d3c80b..39a6ca1f009 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -32,7 +32,7 @@
 
 #include "iwl-prph.h"
 #include "iwl-io.h"
-#include "iwl-trans-pcie-int.h"
+#include "internal.h"
 #include "iwl-op-mode.h"
 
 #ifdef CONFIG_IWLWIFI_IDI
@@ -130,7 +130,7 @@ static int iwl_rx_queue_space(const struct iwl_rx_queue *q)
  * iwl_rx_queue_update_write_ptr - Update the write pointer for the RX queue
  */
 void iwl_rx_queue_update_write_ptr(struct iwl_trans *trans,
-			struct iwl_rx_queue *q)
+				   struct iwl_rx_queue *q)
 {
 	unsigned long flags;
 	u32 reg;
@@ -201,9 +201,7 @@ static inline __le32 iwlagn_dma_addr2rbd_ptr(dma_addr_t dma_addr)
  */
 static void iwlagn_rx_queue_restock(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 	struct list_head *element;
 	struct iwl_rx_mem_buffer *rxb;
@@ -253,9 +251,7 @@ static void iwlagn_rx_queue_restock(struct iwl_trans *trans)
  */
 static void iwlagn_rx_allocate(struct iwl_trans *trans, gfp_t priority)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 	struct list_head *element;
 	struct iwl_rx_mem_buffer *rxb;
@@ -278,8 +274,7 @@ static void iwlagn_rx_allocate(struct iwl_trans *trans, gfp_t priority)
 			gfp_mask |= __GFP_COMP;
 
 		/* Alloc a new receive buffer */
-		page = alloc_pages(gfp_mask,
-				  trans_pcie->rx_page_order);
+		page = alloc_pages(gfp_mask, trans_pcie->rx_page_order);
 		if (!page) {
 			if (net_ratelimit())
 				IWL_DEBUG_INFO(trans, "alloc_pages failed, "
@@ -315,9 +310,10 @@ static void iwlagn_rx_allocate(struct iwl_trans *trans, gfp_t priority)
 		BUG_ON(rxb->page);
 		rxb->page = page;
 		/* Get physical address of the RB */
-		rxb->page_dma = dma_map_page(trans->dev, page, 0,
-				PAGE_SIZE << trans_pcie->rx_page_order,
-				DMA_FROM_DEVICE);
+		rxb->page_dma =
+			dma_map_page(trans->dev, page, 0,
+				     PAGE_SIZE << trans_pcie->rx_page_order,
+				     DMA_FROM_DEVICE);
 		/* dma address must be no more than 36 bits */
 		BUG_ON(rxb->page_dma & ~DMA_BIT_MASK(36));
 		/* and also 256 byte aligned! */
@@ -465,8 +461,8 @@ static void iwl_rx_handle_rxbuf(struct iwl_trans *trans,
 	if (rxb->page != NULL) {
 		rxb->page_dma =
 			dma_map_page(trans->dev, rxb->page, 0,
-				PAGE_SIZE << trans_pcie->rx_page_order,
-				DMA_FROM_DEVICE);
+				     PAGE_SIZE << trans_pcie->rx_page_order,
+				     DMA_FROM_DEVICE);
 		list_add_tail(&rxb->list, &rxq->rx_free);
 		rxq->free_count++;
 	} else
@@ -497,7 +493,7 @@ static void iwl_rx_handle(struct iwl_trans *trans)
 
 	/* Rx interrupt, but nothing sent from uCode */
 	if (i == r)
-		IWL_DEBUG_RX(trans, "r = %d, i = %d\n", r, i);
+		IWL_DEBUG_RX(trans, "HW = SW = %d\n", r);
 
 	/* calculate total frames need to be restock after handling RX */
 	total_empty = r - rxq->write_actual;
@@ -513,8 +509,8 @@ static void iwl_rx_handle(struct iwl_trans *trans)
 		rxb = rxq->queue[i];
 		rxq->queue[i] = NULL;
 
-		IWL_DEBUG_RX(trans, "rxbuf: r = %d, i = %d (%p)\n", rxb);
-
+		IWL_DEBUG_RX(trans, "rxbuf: HW = %d, SW = %d (%p)\n",
+			     r, i, rxb);
 		iwl_rx_handle_rxbuf(trans, rxb);
 
 		i = (i + 1) & RX_QUEUE_MASK;
@@ -546,12 +542,12 @@ static void iwl_irq_handle_error(struct iwl_trans *trans)
 	/* W/A for WiFi/WiMAX coex and WiMAX own the RF */
 	if (trans->cfg->internal_wimax_coex &&
 	    (!(iwl_read_prph(trans, APMG_CLK_CTRL_REG) &
-			APMS_CLK_VAL_MRB_FUNC_MODE) ||
+			     APMS_CLK_VAL_MRB_FUNC_MODE) ||
 	     (iwl_read_prph(trans, APMG_PS_CTRL_REG) &
-			APMG_PS_CTRL_VAL_RESET_REQ))) {
-		struct iwl_trans_pcie *trans_pcie;
+			    APMG_PS_CTRL_VAL_RESET_REQ))) {
+		struct iwl_trans_pcie *trans_pcie =
+			IWL_TRANS_GET_PCIE_TRANS(trans);
 
-		trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 		clear_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status);
 		iwl_op_mode_wimax_active(trans->op_mode);
 		wake_up(&trans->wait_command_queue);
@@ -567,6 +563,8 @@ static void iwl_irq_handle_error(struct iwl_trans *trans)
 /* tasklet for iwlagn interrupt */
 void iwl_irq_tasklet(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct isr_statistics *isr_stats = &trans_pcie->isr_stats;
 	u32 inta = 0;
 	u32 handled = 0;
 	unsigned long flags;
@@ -575,10 +573,6 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 	u32 inta_mask;
 #endif
 
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	struct isr_statistics *isr_stats = &trans_pcie->isr_stats;
-
-
 	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
 
 	/* Ack/clear/reset pending uCode interrupts.
@@ -593,7 +587,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 	 * interrupt coalescing can still be achieved.
 	 */
 	iwl_write32(trans, CSR_INT,
-		trans_pcie->inta | ~trans_pcie->inta_mask);
+		    trans_pcie->inta | ~trans_pcie->inta_mask);
 
 	inta = trans_pcie->inta;
 
@@ -602,7 +596,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 		/* just for debug */
 		inta_mask = iwl_read32(trans, CSR_INT_MASK);
 		IWL_DEBUG_ISR(trans, "inta 0x%08x, enabled 0x%08x\n",
-				inta, inta_mask);
+			      inta, inta_mask);
 	}
 #endif
 
@@ -651,7 +645,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 
 		hw_rfkill = iwl_is_rfkill_set(trans);
 		IWL_WARN(trans, "RF_KILL bit toggled to %s.\n",
-				hw_rfkill ? "disable radio" : "enable radio");
+			 hw_rfkill ? "disable radio" : "enable radio");
 
 		isr_stats->rfkill++;
 
@@ -693,7 +687,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 	 * 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 |
-			CSR_INT_BIT_RX_PERIODIC)) {
+		    CSR_INT_BIT_RX_PERIODIC)) {
 		IWL_DEBUG_ISR(trans, "Rx interrupt\n");
 		if (inta & (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX)) {
 			handled |= (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX);
@@ -733,7 +727,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 		 */
 		if (inta & (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX))
 			iwl_write8(trans, CSR_INT_PERIODIC_REG,
-				    CSR_INT_PERIODIC_ENA);
+				   CSR_INT_PERIODIC_ENA);
 
 		isr_stats->rx++;
 	}
@@ -782,8 +776,7 @@ void iwl_irq_tasklet(struct iwl_trans *trans)
 /* Free dram table */
 void iwl_free_isr_ict(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	if (trans_pcie->ict_tbl) {
 		dma_free_coherent(trans->dev, ICT_SIZE,
@@ -802,8 +795,7 @@ void iwl_free_isr_ict(struct iwl_trans *trans)
  */
 int iwl_alloc_isr_ict(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	trans_pcie->ict_tbl =
 		dma_alloc_coherent(trans->dev, ICT_SIZE,
@@ -837,10 +829,9 @@ int iwl_alloc_isr_ict(struct iwl_trans *trans)
  */
 void iwl_reset_ict(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 val;
 	unsigned long flags;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	if (!trans_pcie->ict_tbl)
 		return;
@@ -868,9 +859,7 @@ void iwl_reset_ict(struct iwl_trans *trans)
 /* Device is going down disable ict interrupt usage */
 void iwl_disable_ict(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	unsigned long flags;
 
 	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
@@ -878,23 +867,19 @@ void iwl_disable_ict(struct iwl_trans *trans)
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 }
 
+/* legacy (non-ICT) ISR. Assumes that trans_pcie->irq_lock is held */
 static irqreturn_t iwl_isr(int irq, void *data)
 {
 	struct iwl_trans *trans = data;
-	struct iwl_trans_pcie *trans_pcie;
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 inta, inta_mask;
-	unsigned long flags;
 #ifdef CONFIG_IWLWIFI_DEBUG
 	u32 inta_fh;
 #endif
-	if (!trans)
-		return IRQ_NONE;
 
-	trace_iwlwifi_dev_irq(trans->dev);
+	lockdep_assert_held(&trans_pcie->irq_lock);
 
-	trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-
-	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
+	trace_iwlwifi_dev_irq(trans->dev);
 
 	/* Disable (but don't clear!) interrupts here to avoid
 	 *    back-to-back ISRs and sporadic interrupts from our NIC.
@@ -918,7 +903,7 @@ static irqreturn_t iwl_isr(int irq, void *data)
 		/* Hardware disappeared. It might have already raised
 		 * an interrupt */
 		IWL_WARN(trans, "HARDWARE GONE?? INTA == 0x%08x\n", inta);
-		goto unplugged;
+		return IRQ_HANDLED;
 	}
 
 #ifdef CONFIG_IWLWIFI_DEBUG
@@ -934,21 +919,16 @@ static irqreturn_t iwl_isr(int irq, void *data)
 	if (likely(inta))
 		tasklet_schedule(&trans_pcie->irq_tasklet);
 	else if (test_bit(STATUS_INT_ENABLED, &trans_pcie->status) &&
-			!trans_pcie->inta)
+		 !trans_pcie->inta)
 		iwl_enable_interrupts(trans);
 
- unplugged:
-	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
-	return IRQ_HANDLED;
-
- none:
+none:
 	/* re-enable interrupts here since we don't have anything to service. */
 	/* only Re-enable if disabled by irq  and no schedules tasklet. */
 	if (test_bit(STATUS_INT_ENABLED, &trans_pcie->status) &&
-		!trans_pcie->inta)
+	    !trans_pcie->inta)
 		iwl_enable_interrupts(trans);
 
-	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 	return IRQ_NONE;
 }
 
@@ -974,15 +954,19 @@ irqreturn_t iwl_isr_ict(int irq, void *data)
 
 	trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
+	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
+
 	/* dram interrupt table not set yet,
 	 * use legacy interrupt.
 	 */
-	if (!trans_pcie->use_ict)
-		return iwl_isr(irq, data);
+	if (unlikely(!trans_pcie->use_ict)) {
+		irqreturn_t ret = iwl_isr(irq, data);
+		spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
+		return ret;
+	}
 
 	trace_iwlwifi_dev_irq(trans->dev);
 
-	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
 
 	/* Disable (but don't clear!) interrupts here to avoid
 	 * back-to-back ISRs and sporadic interrupts from our NIC.
@@ -1036,7 +1020,7 @@ irqreturn_t iwl_isr_ict(int irq, void *data)
 
 	inta = (0xff & val) | ((0xff00 & val) << 16);
 	IWL_DEBUG_ISR(trans, "ISR inta 0x%08x, enabled 0x%08x ict 0x%08x\n",
-			inta, inta_mask, val);
+		      inta, inta_mask, val);
 
 	inta &= trans_pcie->inta_mask;
 	trans_pcie->inta |= inta;
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 79c6b91417f..939c2f78df5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -70,15 +70,12 @@
 
 #include "iwl-drv.h"
 #include "iwl-trans.h"
-#include "iwl-trans-pcie-int.h"
 #include "iwl-csr.h"
 #include "iwl-prph.h"
-#include "iwl-eeprom.h"
 #include "iwl-agn-hw.h"
+#include "internal.h"
 /* FIXME: need to abstract out TX command (once we know what it looks like) */
-#include "iwl-commands.h"
-
-#define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
+#include "dvm/commands.h"
 
 #define SCD_QUEUECHAIN_SEL_ALL(trans, trans_pcie)	\
 	(((1<<trans->cfg->base_params->num_of_queues) - 1) &\
@@ -86,8 +83,7 @@
 
 static int iwl_trans_rx_alloc(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 	struct device *dev = trans->dev;
 
@@ -114,7 +110,7 @@ static int iwl_trans_rx_alloc(struct iwl_trans *trans)
 
 err_rb_stts:
 	dma_free_coherent(dev, sizeof(__le32) * RX_QUEUE_SIZE,
-			rxq->bd, rxq->bd_dma);
+			  rxq->bd, rxq->bd_dma);
 	memset(&rxq->bd_dma, 0, sizeof(rxq->bd_dma));
 	rxq->bd = NULL;
 err_bd:
@@ -123,8 +119,7 @@ err_bd:
 
 static void iwl_trans_rxq_free_rx_bufs(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 	int i;
 
@@ -134,8 +129,8 @@ static void iwl_trans_rxq_free_rx_bufs(struct iwl_trans *trans)
 		 * to an SKB, so we need to unmap and free potential storage */
 		if (rxq->pool[i].page != NULL) {
 			dma_unmap_page(trans->dev, rxq->pool[i].page_dma,
-				PAGE_SIZE << trans_pcie->rx_page_order,
-				DMA_FROM_DEVICE);
+				       PAGE_SIZE << trans_pcie->rx_page_order,
+				       DMA_FROM_DEVICE);
 			__free_pages(rxq->pool[i].page,
 				     trans_pcie->rx_page_order);
 			rxq->pool[i].page = NULL;
@@ -193,8 +188,7 @@ static void iwl_trans_rx_hw_init(struct iwl_trans *trans,
 
 static int iwl_rx_init(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 
 	int i, err;
@@ -236,10 +230,8 @@ static int iwl_rx_init(struct iwl_trans *trans)
 
 static void iwl_trans_pcie_rx_free(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
-
 	unsigned long flags;
 
 	/*if rxq->bd is NULL, it means that nothing has been allocated,
@@ -274,11 +266,11 @@ static int iwl_trans_rx_stop(struct iwl_trans *trans)
 	/* stop Rx DMA */
 	iwl_write_direct32(trans, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
 	return iwl_poll_direct_bit(trans, FH_MEM_RSSR_RX_STATUS_REG,
-			    FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE, 1000);
+				   FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE, 1000);
 }
 
-static inline int iwlagn_alloc_dma_ptr(struct iwl_trans *trans,
-				    struct iwl_dma_ptr *ptr, size_t size)
+static int iwlagn_alloc_dma_ptr(struct iwl_trans *trans,
+				struct iwl_dma_ptr *ptr, size_t size)
 {
 	if (WARN_ON(ptr->addr))
 		return -EINVAL;
@@ -291,8 +283,8 @@ static inline int iwlagn_alloc_dma_ptr(struct iwl_trans *trans,
 	return 0;
 }
 
-static inline void iwlagn_free_dma_ptr(struct iwl_trans *trans,
-				    struct iwl_dma_ptr *ptr)
+static void iwlagn_free_dma_ptr(struct iwl_trans *trans,
+				struct iwl_dma_ptr *ptr)
 {
 	if (unlikely(!ptr->addr))
 		return;
@@ -304,8 +296,13 @@ static inline void iwlagn_free_dma_ptr(struct iwl_trans *trans,
 static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
 {
 	struct iwl_tx_queue *txq = (void *)data;
+	struct iwl_queue *q = &txq->q;
 	struct iwl_trans_pcie *trans_pcie = txq->trans_pcie;
 	struct iwl_trans *trans = iwl_trans_pcie_get_trans(trans_pcie);
+	u32 scd_sram_addr = trans_pcie->scd_base_addr +
+		SCD_TX_STTS_MEM_LOWER_BOUND + (16 * txq->q.id);
+	u8 buf[16];
+	int i;
 
 	spin_lock(&txq->lock);
 	/* check if triggered erroneously */
@@ -315,26 +312,59 @@ static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
 	}
 	spin_unlock(&txq->lock);
 
-
 	IWL_ERR(trans, "Queue %d stuck for %u ms.\n", txq->q.id,
 		jiffies_to_msecs(trans_pcie->wd_timeout));
 	IWL_ERR(trans, "Current SW read_ptr %d write_ptr %d\n",
 		txq->q.read_ptr, txq->q.write_ptr);
-	IWL_ERR(trans, "Current HW read_ptr %d write_ptr %d\n",
-		iwl_read_prph(trans, SCD_QUEUE_RDPTR(txq->q.id))
-					& (TFD_QUEUE_SIZE_MAX - 1),
-		iwl_read_prph(trans, SCD_QUEUE_WRPTR(txq->q.id)));
+
+	iwl_read_targ_mem_bytes(trans, scd_sram_addr, buf, sizeof(buf));
+
+	iwl_print_hex_error(trans, buf, sizeof(buf));
+
+	for (i = 0; i < FH_TCSR_CHNL_NUM; i++)
+		IWL_ERR(trans, "FH TRBs(%d) = 0x%08x\n", i,
+			iwl_read_direct32(trans, FH_TX_TRB_REG(i)));
+
+	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
+		u32 status = iwl_read_prph(trans, SCD_QUEUE_STATUS_BITS(i));
+		u8 fifo = (status >> SCD_QUEUE_STTS_REG_POS_TXF) & 0x7;
+		bool active = !!(status & BIT(SCD_QUEUE_STTS_REG_POS_ACTIVE));
+		u32 tbl_dw =
+			iwl_read_targ_mem(trans,
+					  trans_pcie->scd_base_addr +
+					  SCD_TRANS_TBL_OFFSET_QUEUE(i));
+
+		if (i & 0x1)
+			tbl_dw = (tbl_dw & 0xFFFF0000) >> 16;
+		else
+			tbl_dw = tbl_dw & 0x0000FFFF;
+
+		IWL_ERR(trans,
+			"Q %d is %sactive and mapped to fifo %d ra_tid 0x%04x [%d,%d]\n",
+			i, active ? "" : "in", fifo, tbl_dw,
+			iwl_read_prph(trans,
+				      SCD_QUEUE_RDPTR(i)) & (txq->q.n_bd - 1),
+			iwl_read_prph(trans, SCD_QUEUE_WRPTR(i)));
+	}
+
+	for (i = q->read_ptr; i != q->write_ptr;
+	     i = iwl_queue_inc_wrap(i, q->n_bd)) {
+		struct iwl_tx_cmd *tx_cmd =
+			(struct iwl_tx_cmd *)txq->entries[i].cmd->payload;
+		IWL_ERR(trans, "scratch %d = 0x%08x\n", i,
+			get_unaligned_le32(&tx_cmd->scratch));
+	}
 
 	iwl_op_mode_nic_error(trans->op_mode);
 }
 
 static int iwl_trans_txq_alloc(struct iwl_trans *trans,
-				struct iwl_tx_queue *txq, int slots_num,
-				u32 txq_id)
+			       struct iwl_tx_queue *txq, int slots_num,
+			       u32 txq_id)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	size_t tfd_sz = sizeof(struct iwl_tfd) * TFD_QUEUE_SIZE_MAX;
 	int i;
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	if (WARN_ON(txq->entries || txq->tfds))
 		return -EINVAL;
@@ -435,7 +465,7 @@ static void iwl_tx_queue_unmap(struct iwl_trans *trans, int txq_id)
 
 	spin_lock_bh(&txq->lock);
 	while (q->write_ptr != q->read_ptr) {
-		iwlagn_txq_free_tfd(trans, txq, dma_dir);
+		iwl_txq_free_tfd(trans, txq, dma_dir);
 		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd);
 	}
 	spin_unlock_bh(&txq->lock);
@@ -455,6 +485,7 @@ static void iwl_tx_queue_free(struct iwl_trans *trans, int txq_id)
 	struct iwl_tx_queue *txq = &trans_pcie->txq[txq_id];
 	struct device *dev = trans->dev;
 	int i;
+
 	if (WARN_ON(!txq))
 		return;
 
@@ -574,11 +605,11 @@ error:
 }
 static int iwl_tx_init(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ret;
 	int txq_id, slots_num;
 	unsigned long flags;
 	bool alloc = false;
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	if (!trans_pcie->txq) {
 		ret = iwl_trans_tx_alloc(trans);
@@ -643,10 +674,9 @@ static void iwl_set_pwr_vmain(struct iwl_trans *trans)
 
 static u16 iwl_pciexp_link_ctrl(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int pos;
 	u16 pci_lnk_ctl;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	struct pci_dev *pci_dev = trans_pcie->pci_dev;
 
@@ -700,14 +730,14 @@ static int iwl_apm_init(struct iwl_trans *trans)
 
 	/* Disable L0S exit timer (platform NMI Work/Around) */
 	iwl_set_bit(trans, CSR_GIO_CHICKEN_BITS,
-			  CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
+		    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_set_bit(trans, CSR_GIO_CHICKEN_BITS,
-			  CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
+		    CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
 
 	/* Set FH wait threshold to maximum (HW error during stress W/A) */
 	iwl_set_bit(trans, CSR_DBG_HPET_MEM_REG, CSR_DBG_HPET_MEM_REG_VAL);
@@ -717,7 +747,7 @@ static int iwl_apm_init(struct iwl_trans *trans)
 	 * wake device's PCI Express link L1a -> L0s
 	 */
 	iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
-				    CSR_HW_IF_CONFIG_REG_BIT_HAP_WAKE_L1A);
+		    CSR_HW_IF_CONFIG_REG_BIT_HAP_WAKE_L1A);
 
 	iwl_apm_config(trans);
 
@@ -738,8 +768,8 @@ static int iwl_apm_init(struct iwl_trans *trans)
 	 * and accesses to uCode SRAM.
 	 */
 	ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
-			CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
-			CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+			   CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			   CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
 	if (ret < 0) {
 		IWL_DEBUG_INFO(trans, "Failed to init the card\n");
 		goto out;
@@ -773,8 +803,8 @@ static int iwl_apm_stop_master(struct iwl_trans *trans)
 	iwl_set_bit(trans, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
 
 	ret = iwl_poll_bit(trans, CSR_RESET,
-			CSR_RESET_REG_FLAG_MASTER_DISABLED,
-			CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
+			   CSR_RESET_REG_FLAG_MASTER_DISABLED,
+			   CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
 	if (ret)
 		IWL_WARN(trans, "Master Disable Timed Out, 100 usec\n");
 
@@ -816,8 +846,7 @@ static int iwl_nic_init(struct iwl_trans *trans)
 	iwl_apm_init(trans);
 
 	/* Set interrupt coalescing calibration timer to default (512 usecs) */
-	iwl_write8(trans, CSR_INT_COALESCING,
-		IWL_HOST_INT_CALIB_TIMEOUT_DEF);
+	iwl_write8(trans, CSR_INT_COALESCING, IWL_HOST_INT_CALIB_TIMEOUT_DEF);
 
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 
@@ -836,8 +865,8 @@ static int iwl_nic_init(struct iwl_trans *trans)
 
 	if (trans->cfg->base_params->shadow_reg_enable) {
 		/* enable shadow regs in HW */
-		iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTRL,
-			0x800FFFFF);
+		iwl_set_bit(trans, CSR_MAC_SHADOW_REG_CTRL, 0x800FFFFF);
+		IWL_DEBUG_INFO(trans, "Enabling shadow registers in device\n");
 	}
 
 	return 0;
@@ -851,13 +880,13 @@ static int iwl_set_hw_ready(struct iwl_trans *trans)
 	int ret;
 
 	iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
-		CSR_HW_IF_CONFIG_REG_BIT_NIC_READY);
+		    CSR_HW_IF_CONFIG_REG_BIT_NIC_READY);
 
 	/* See if we got it */
 	ret = iwl_poll_bit(trans, CSR_HW_IF_CONFIG_REG,
-				CSR_HW_IF_CONFIG_REG_BIT_NIC_READY,
-				CSR_HW_IF_CONFIG_REG_BIT_NIC_READY,
-				HW_READY_TIMEOUT);
+			   CSR_HW_IF_CONFIG_REG_BIT_NIC_READY,
+			   CSR_HW_IF_CONFIG_REG_BIT_NIC_READY,
+			   HW_READY_TIMEOUT);
 
 	IWL_DEBUG_INFO(trans, "hardware%s ready\n", ret < 0 ? " not" : "");
 	return ret;
@@ -877,11 +906,11 @@ static int iwl_prepare_card_hw(struct iwl_trans *trans)
 
 	/* If HW is not ready, prepare the conditions to check again */
 	iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
-			CSR_HW_IF_CONFIG_REG_PREPARE);
+		    CSR_HW_IF_CONFIG_REG_PREPARE);
 
 	ret = iwl_poll_bit(trans, CSR_HW_IF_CONFIG_REG,
-			~CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE,
-			CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE, 150000);
+			   ~CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE,
+			   CSR_HW_IF_CONFIG_REG_BIT_NIC_PREPARE_DONE, 150000);
 
 	if (ret < 0)
 		return ret;
@@ -908,32 +937,33 @@ static int iwl_load_section(struct iwl_trans *trans, u8 section_num,
 	trans_pcie->ucode_write_complete = false;
 
 	iwl_write_direct32(trans,
-		FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
-		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE);
+			   FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
+			   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE);
 
 	iwl_write_direct32(trans,
-		FH_SRVC_CHNL_SRAM_ADDR_REG(FH_SRVC_CHNL), dst_addr);
+			   FH_SRVC_CHNL_SRAM_ADDR_REG(FH_SRVC_CHNL),
+			   dst_addr);
 
 	iwl_write_direct32(trans,
 		FH_TFDIB_CTRL0_REG(FH_SRVC_CHNL),
 		phy_addr & FH_MEM_TFDIB_DRAM_ADDR_LSB_MSK);
 
 	iwl_write_direct32(trans,
-		FH_TFDIB_CTRL1_REG(FH_SRVC_CHNL),
-		(iwl_get_dma_hi_addr(phy_addr)
-			<< FH_MEM_TFDIB_REG1_ADDR_BITSHIFT) | byte_cnt);
+			   FH_TFDIB_CTRL1_REG(FH_SRVC_CHNL),
+			   (iwl_get_dma_hi_addr(phy_addr)
+				<< FH_MEM_TFDIB_REG1_ADDR_BITSHIFT) | byte_cnt);
 
 	iwl_write_direct32(trans,
-		FH_TCSR_CHNL_TX_BUF_STS_REG(FH_SRVC_CHNL),
-		1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM |
-		1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX |
-		FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID);
+			   FH_TCSR_CHNL_TX_BUF_STS_REG(FH_SRVC_CHNL),
+			   1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM |
+			   1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX |
+			   FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID);
 
 	iwl_write_direct32(trans,
-		FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
-		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE	|
-		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE	|
-		FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD);
+			   FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
+			   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE	|
+			   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE	|
+			   FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD);
 
 	IWL_DEBUG_FW(trans, "[%d] uCode section being loaded...\n",
 		     section_num);
@@ -1016,15 +1046,12 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 
 /*
  * Activate/Deactivate Tx DMA/FIFO channels according tx fifos mask
- * must be called under the irq lock and with MAC access
  */
 static void iwl_trans_txq_set_sched(struct iwl_trans *trans, u32 mask)
 {
 	struct iwl_trans_pcie __maybe_unused *trans_pcie =
 		IWL_TRANS_GET_PCIE_TRANS(trans);
 
-	lockdep_assert_held(&trans_pcie->irq_lock);
-
 	iwl_write_prph(trans, SCD_TXFACT, mask);
 }
 
@@ -1032,11 +1059,12 @@ static void iwl_tx_start(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 a;
-	unsigned long flags;
-	int i, chan;
+	int chan;
 	u32 reg_val;
 
-	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
+	/* make sure all queue are not stopped/used */
+	memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
+	memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
 
 	trans_pcie->scd_base_addr =
 		iwl_read_prph(trans, SCD_SRAM_BASE_ADDR);
@@ -1063,64 +1091,26 @@ static void iwl_tx_start(struct iwl_trans *trans)
 	 */
 	iwl_write_prph(trans, SCD_CHAINEXT_EN, 0);
 
+	iwl_trans_ac_txq_enable(trans, trans_pcie->cmd_queue,
+				trans_pcie->cmd_fifo);
+
+	/* Activate all Tx DMA/FIFO channels */
+	iwl_trans_txq_set_sched(trans, IWL_MASK(0, 7));
+
 	/* Enable DMA channel */
 	for (chan = 0; chan < FH_TCSR_CHNL_NUM ; chan++)
 		iwl_write_direct32(trans, 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);
+				   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(trans, FH_TX_CHICKEN_BITS_REG);
 	iwl_write_direct32(trans, FH_TX_CHICKEN_BITS_REG,
 			   reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
 
-	iwl_write_prph(trans, SCD_QUEUECHAIN_SEL,
-		SCD_QUEUECHAIN_SEL_ALL(trans, trans_pcie));
-	iwl_write_prph(trans, SCD_AGGR_SEL, 0);
-
-	/* initiate the queues */
-	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
-		iwl_write_prph(trans, SCD_QUEUE_RDPTR(i), 0);
-		iwl_write_direct32(trans, HBUS_TARG_WRPTR, 0 | (i << 8));
-		iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
-				SCD_CONTEXT_QUEUE_OFFSET(i), 0);
-		iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
-				SCD_CONTEXT_QUEUE_OFFSET(i) +
-				sizeof(u32),
-				((SCD_WIN_SIZE <<
-				SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
-				SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
-				((SCD_FRAME_LIMIT <<
-				SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
-				SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
-	}
-
-	iwl_write_prph(trans, SCD_INTERRUPT_MASK,
-			IWL_MASK(0, trans->cfg->base_params->num_of_queues));
-
-	/* Activate all Tx DMA/FIFO channels */
-	iwl_trans_txq_set_sched(trans, IWL_MASK(0, 7));
-
-	iwl_trans_set_wr_ptrs(trans, trans_pcie->cmd_queue, 0);
-
-	/* make sure all queue are not stopped/used */
-	memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
-	memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
-
-	for (i = 0; i < trans_pcie->n_q_to_fifo; i++) {
-		int fifo = trans_pcie->setup_q_to_fifo[i];
-
-		set_bit(i, trans_pcie->queue_used);
-
-		iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[i],
-					      fifo, true);
-	}
-
-	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
-
 	/* Enable L1-Active */
 	iwl_clear_bits_prph(trans, APMG_PCIDEV_STT_REG,
-			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+			    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 }
 
 static void iwl_trans_pcie_fw_alive(struct iwl_trans *trans)
@@ -1134,9 +1124,9 @@ static void iwl_trans_pcie_fw_alive(struct iwl_trans *trans)
  */
 static int iwl_trans_tx_stop(struct iwl_trans *trans)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ch, txq_id, ret;
 	unsigned long flags;
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	/* Turn off all Tx DMA fifos */
 	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
@@ -1148,18 +1138,19 @@ static int iwl_trans_tx_stop(struct iwl_trans *trans)
 		iwl_write_direct32(trans,
 				   FH_TCSR_CHNL_TX_CONFIG_REG(ch), 0x0);
 		ret = iwl_poll_direct_bit(trans, FH_TSSR_TX_STATUS_REG,
-				    FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(ch),
-				    1000);
+			FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(ch), 1000);
 		if (ret < 0)
-			IWL_ERR(trans, "Failing on timeout while stopping"
-			    " DMA channel %d [0x%08x]", ch,
-			    iwl_read_direct32(trans,
-					      FH_TSSR_TX_STATUS_REG));
+			IWL_ERR(trans,
+				"Failing on timeout while stopping DMA channel %d [0x%08x]\n",
+				ch,
+				iwl_read_direct32(trans,
+						  FH_TSSR_TX_STATUS_REG));
 	}
 	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 
 	if (!trans_pcie->txq) {
-		IWL_WARN(trans, "Stopping tx queues that aren't allocated...");
+		IWL_WARN(trans,
+			 "Stopping tx queues that aren't allocated...\n");
 		return 0;
 	}
 
@@ -1173,8 +1164,8 @@ static int iwl_trans_tx_stop(struct iwl_trans *trans)
 
 static void iwl_trans_pcie_stop_device(struct iwl_trans *trans)
 {
-	unsigned long flags;
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	unsigned long flags;
 
 	/* tell the device to stop sending interrupts */
 	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
@@ -1204,7 +1195,7 @@ static void iwl_trans_pcie_stop_device(struct iwl_trans *trans)
 
 	/* Make sure (redundant) we've released our request to stay awake */
 	iwl_clear_bit(trans, CSR_GP_CNTRL,
-			CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+		      CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
 
 	/* Stop the device, and put it in low power state */
 	iwl_apm_stop(trans);
@@ -1273,13 +1264,27 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 
 	spin_lock(&txq->lock);
 
+	/* In AGG mode, the index in the ring must correspond to the WiFi
+	 * sequence number. This is a HW requirements to help the SCD to parse
+	 * the BA.
+	 * Check here that the packets are in the right place on the ring.
+	 */
+#ifdef CONFIG_IWLWIFI_DEBUG
+	wifi_seq = SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
+	WARN_ONCE((iwl_read_prph(trans, SCD_AGGR_SEL) & BIT(txq_id)) &&
+		  ((wifi_seq & 0xff) != q->write_ptr),
+		  "Q: %d WiFi Seq %d tfdNum %d",
+		  txq_id, wifi_seq, q->write_ptr);
+#endif
+
 	/* Set up driver data for this TFD */
 	txq->entries[q->write_ptr].skb = skb;
 	txq->entries[q->write_ptr].cmd = dev_cmd;
 
 	dev_cmd->hdr.cmd = REPLY_TX;
-	dev_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
-				INDEX_TO_SEQ(q->write_ptr)));
+	dev_cmd->hdr.sequence =
+		cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
+			    INDEX_TO_SEQ(q->write_ptr)));
 
 	/* Set up first empty entry in queue's array of Tx/cmd buffers */
 	out_meta = &txq->entries[q->write_ptr].meta;
@@ -1344,7 +1349,7 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 
 	/* take back ownership of DMA buffer to enable update */
 	dma_sync_single_for_cpu(trans->dev, txcmd_phys, firstlen,
-			DMA_BIDIRECTIONAL);
+				DMA_BIDIRECTIONAL);
 	tx_cmd->dram_lsb_ptr = cpu_to_le32(scratch_phys);
 	tx_cmd->dram_msb_ptr = iwl_get_dma_hi_addr(scratch_phys);
 
@@ -1356,16 +1361,17 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 	iwl_trans_txq_update_byte_cnt_tbl(trans, txq, le16_to_cpu(tx_cmd->len));
 
 	dma_sync_single_for_device(trans->dev, txcmd_phys, firstlen,
-			DMA_BIDIRECTIONAL);
+				   DMA_BIDIRECTIONAL);
 
 	trace_iwlwifi_dev_tx(trans->dev,
-			     &((struct iwl_tfd *)txq->tfds)[txq->q.write_ptr],
+			     &txq->tfds[txq->q.write_ptr],
 			     sizeof(struct iwl_tfd),
 			     &dev_cmd->hdr, firstlen,
 			     skb->data + hdr_len, secondlen);
 
 	/* start timer if queue currently empty */
-	if (q->read_ptr == q->write_ptr && trans_pcie->wd_timeout)
+	if (txq->need_update && q->read_ptr == q->write_ptr &&
+	    trans_pcie->wd_timeout)
 		mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
 
 	/* Tell device the write index *just past* this latest filled TFD */
@@ -1395,8 +1401,7 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 
 static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int err;
 	bool hw_rfkill;
 
@@ -1409,7 +1414,7 @@ static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 		iwl_alloc_isr_ict(trans);
 
 		err = request_irq(trans_pcie->irq, iwl_isr_ict, IRQF_SHARED,
-			DRV_NAME, trans);
+				  DRV_NAME, trans);
 		if (err) {
 			IWL_ERR(trans, "Error allocating IRQ %d\n",
 				trans_pcie->irq);
@@ -1422,7 +1427,7 @@ static int iwl_trans_pcie_start_hw(struct iwl_trans *trans)
 
 	err = iwl_prepare_card_hw(trans);
 	if (err) {
-		IWL_ERR(trans, "Error while preparing HW: %d", err);
+		IWL_ERR(trans, "Error while preparing HW: %d\n", err);
 		goto err_free_irq;
 	}
 
@@ -1447,9 +1452,9 @@ error:
 static void iwl_trans_pcie_stop_hw(struct iwl_trans *trans,
 				   bool op_mode_leaving)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	bool hw_rfkill;
 	unsigned long flags;
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	iwl_apm_stop(trans);
 
@@ -1520,6 +1525,7 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	trans_pcie->cmd_queue = trans_cfg->cmd_queue;
+	trans_pcie->cmd_fifo = trans_cfg->cmd_fifo;
 	if (WARN_ON(trans_cfg->n_no_reclaim_cmds > MAX_NO_RECLAIM_CMDS))
 		trans_pcie->n_no_reclaim_cmds = 0;
 	else
@@ -1528,17 +1534,6 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 		memcpy(trans_pcie->no_reclaim_cmds, trans_cfg->no_reclaim_cmds,
 		       trans_pcie->n_no_reclaim_cmds * sizeof(u8));
 
-	trans_pcie->n_q_to_fifo = trans_cfg->n_queue_to_fifo;
-
-	if (WARN_ON(trans_pcie->n_q_to_fifo > IWL_MAX_HW_QUEUES))
-		trans_pcie->n_q_to_fifo = IWL_MAX_HW_QUEUES;
-
-	/* at least the command queue must be mapped */
-	WARN_ON(!trans_pcie->n_q_to_fifo);
-
-	memcpy(trans_pcie->setup_q_to_fifo, trans_cfg->queue_to_fifo,
-	       trans_pcie->n_q_to_fifo * sizeof(u8));
-
 	trans_pcie->rx_buf_size_8k = trans_cfg->rx_buf_size_8k;
 	if (trans_pcie->rx_buf_size_8k)
 		trans_pcie->rx_page_order = get_order(8 * 1024);
@@ -1553,8 +1548,7 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 
 void iwl_trans_pcie_free(struct iwl_trans *trans)
 {
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
 	iwl_trans_pcie_tx_free(trans);
 #ifndef CONFIG_IWLWIFI_IDI
@@ -1569,6 +1563,7 @@ void iwl_trans_pcie_free(struct iwl_trans *trans)
 	iounmap(trans_pcie->hw_base);
 	pci_release_regions(trans_pcie->pci_dev);
 	pci_disable_device(trans_pcie->pci_dev);
+	kmem_cache_destroy(trans->dev_cmd_pool);
 
 	kfree(trans);
 }
@@ -1816,8 +1811,8 @@ static const struct file_operations iwl_dbgfs_##name##_ops = {		\
 };
 
 static ssize_t iwl_dbgfs_tx_queue_read(struct file *file,
-						char __user *user_buf,
-						size_t count, loff_t *ppos)
+				       char __user *user_buf,
+				       size_t count, loff_t *ppos)
 {
 	struct iwl_trans *trans = file->private_data;
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -1853,11 +1848,11 @@ static ssize_t iwl_dbgfs_tx_queue_read(struct file *file,
 }
 
 static ssize_t iwl_dbgfs_rx_queue_read(struct file *file,
-						char __user *user_buf,
-						size_t count, loff_t *ppos) {
+				       char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
 	struct iwl_trans *trans = file->private_data;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rx_queue *rxq = &trans_pcie->rxq;
 	char buf[256];
 	int pos = 0;
@@ -1881,11 +1876,10 @@ static ssize_t iwl_dbgfs_rx_queue_read(struct file *file,
 
 static ssize_t iwl_dbgfs_interrupt_read(struct file *file,
 					char __user *user_buf,
-					size_t count, loff_t *ppos) {
-
+					size_t count, loff_t *ppos)
+{
 	struct iwl_trans *trans = file->private_data;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct isr_statistics *isr_stats = &trans_pcie->isr_stats;
 
 	int pos = 0;
@@ -1943,8 +1937,7 @@ static ssize_t iwl_dbgfs_interrupt_write(struct file *file,
 					 size_t count, loff_t *ppos)
 {
 	struct iwl_trans *trans = file->private_data;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct isr_statistics *isr_stats = &trans_pcie->isr_stats;
 
 	char buf[8];
@@ -1964,8 +1957,8 @@ static ssize_t iwl_dbgfs_interrupt_write(struct file *file,
 }
 
 static ssize_t iwl_dbgfs_csr_write(struct file *file,
-					 const char __user *user_buf,
-					 size_t count, loff_t *ppos)
+				   const char __user *user_buf,
+				   size_t count, loff_t *ppos)
 {
 	struct iwl_trans *trans = file->private_data;
 	char buf[8];
@@ -1985,8 +1978,8 @@ static ssize_t iwl_dbgfs_csr_write(struct file *file,
 }
 
 static ssize_t iwl_dbgfs_fh_reg_read(struct file *file,
-					 char __user *user_buf,
-					 size_t count, loff_t *ppos)
+				     char __user *user_buf,
+				     size_t count, loff_t *ppos)
 {
 	struct iwl_trans *trans = file->private_data;
 	char *buf;
@@ -2012,7 +2005,9 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
 	if (!trans->op_mode)
 		return -EAGAIN;
 
+	local_bh_disable();
 	iwl_op_mode_nic_error(trans->op_mode);
+	local_bh_enable();
 
 	return count;
 }
@@ -2029,7 +2024,7 @@ DEBUGFS_WRITE_FILE_OPS(fw_restart);
  *
  */
 static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
-					struct dentry *dir)
+					 struct dentry *dir)
 {
 	DEBUGFS_ADD_FILE(rx_queue, dir, S_IRUSR);
 	DEBUGFS_ADD_FILE(tx_queue, dir, S_IRUSR);
@@ -2041,9 +2036,10 @@ static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
 }
 #else
 static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
-					struct dentry *dir)
-{ return 0; }
-
+					 struct dentry *dir)
+{
+	return 0;
+}
 #endif /*CONFIG_IWLWIFI_DEBUGFS */
 
 static const struct iwl_trans_ops trans_ops_pcie = {
@@ -2060,8 +2056,8 @@ static const struct iwl_trans_ops trans_ops_pcie = {
 	.tx = iwl_trans_pcie_tx,
 	.reclaim = iwl_trans_pcie_reclaim,
 
-	.tx_agg_disable = iwl_trans_pcie_tx_agg_disable,
-	.tx_agg_setup = iwl_trans_pcie_tx_agg_setup,
+	.txq_disable = iwl_trans_pcie_txq_disable,
+	.txq_enable = iwl_trans_pcie_txq_enable,
 
 	.dbgfs_register = iwl_trans_pcie_dbgfs_register,
 
@@ -2088,7 +2084,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	int err;
 
 	trans = kzalloc(sizeof(struct iwl_trans) +
-			     sizeof(struct iwl_trans_pcie), GFP_KERNEL);
+			sizeof(struct iwl_trans_pcie), GFP_KERNEL);
 
 	if (WARN_ON(!trans))
 		return NULL;
@@ -2104,7 +2100,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	/* W/A - seems to solve weird behavior. We need to remove this if we
 	 * don't want to stay in L1 all the time. This wastes a lot of power */
 	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1 |
-				PCIE_LINK_STATE_CLKPM);
+			       PCIE_LINK_STATE_CLKPM);
 
 	if (pci_enable_device(pdev)) {
 		err = -ENODEV;
@@ -2120,7 +2116,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (!err)
 			err = pci_set_consistent_dma_mask(pdev,
-							DMA_BIT_MASK(32));
+							  DMA_BIT_MASK(32));
 		/* both attempts failed: */
 		if (err) {
 			dev_printk(KERN_ERR, &pdev->dev,
@@ -2131,25 +2127,26 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 
 	err = pci_request_regions(pdev, DRV_NAME);
 	if (err) {
-		dev_printk(KERN_ERR, &pdev->dev, "pci_request_regions failed");
+		dev_printk(KERN_ERR, &pdev->dev,
+			   "pci_request_regions failed\n");
 		goto out_pci_disable_device;
 	}
 
 	trans_pcie->hw_base = pci_ioremap_bar(pdev, 0);
 	if (!trans_pcie->hw_base) {
-		dev_printk(KERN_ERR, &pdev->dev, "pci_ioremap_bar failed");
+		dev_printk(KERN_ERR, &pdev->dev, "pci_ioremap_bar failed\n");
 		err = -ENODEV;
 		goto out_pci_release_regions;
 	}
 
 	dev_printk(KERN_INFO, &pdev->dev,
-		"pci_resource_len = 0x%08llx\n",
-		(unsigned long long) pci_resource_len(pdev, 0));
+		   "pci_resource_len = 0x%08llx\n",
+		   (unsigned long long) pci_resource_len(pdev, 0));
 	dev_printk(KERN_INFO, &pdev->dev,
-		"pci_resource_base = %p\n", trans_pcie->hw_base);
+		   "pci_resource_base = %p\n", trans_pcie->hw_base);
 
 	dev_printk(KERN_INFO, &pdev->dev,
-		"HW Revision ID = 0x%X\n", pdev->revision);
+		   "HW Revision ID = 0x%X\n", pdev->revision);
 
 	/* We disable the RETRY_TIMEOUT register (0x41) to keep
 	 * PCI Tx retries from interfering with C3 CPU state */
@@ -2158,7 +2155,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	err = pci_enable_msi(pdev);
 	if (err)
 		dev_printk(KERN_ERR, &pdev->dev,
-			"pci_enable_msi failed(0X%x)", err);
+			   "pci_enable_msi failed(0X%x)\n", err);
 
 	trans->dev = &pdev->dev;
 	trans_pcie->irq = pdev->irq;
@@ -2180,8 +2177,25 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	init_waitqueue_head(&trans->wait_command_queue);
 	spin_lock_init(&trans->reg_lock);
 
+	snprintf(trans->dev_cmd_pool_name, sizeof(trans->dev_cmd_pool_name),
+		 "iwl_cmd_pool:%s", dev_name(trans->dev));
+
+	trans->dev_cmd_headroom = 0;
+	trans->dev_cmd_pool =
+		kmem_cache_create(trans->dev_cmd_pool_name,
+				  sizeof(struct iwl_device_cmd)
+				  + trans->dev_cmd_headroom,
+				  sizeof(void *),
+				  SLAB_HWCACHE_ALIGN,
+				  NULL);
+
+	if (!trans->dev_cmd_pool)
+		goto out_pci_disable_msi;
+
 	return trans;
 
+out_pci_disable_msi:
+	pci_disable_msi(pdev);
 out_pci_release_regions:
 	pci_release_regions(pdev);
 out_pci_disable_device:
@@ -2190,4 +2204,3 @@ out_no_pci:
 	kfree(trans);
 	return NULL;
 }
-
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index a8750238ee0..6baf8deef51 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -34,11 +34,10 @@
 #include "iwl-csr.h"
 #include "iwl-prph.h"
 #include "iwl-io.h"
-#include "iwl-agn-hw.h"
 #include "iwl-op-mode.h"
-#include "iwl-trans-pcie-int.h"
+#include "internal.h"
 /* FIXME: need to abstract out TX command (once we know what it looks like) */
-#include "iwl-commands.h"
+#include "dvm/commands.h"
 
 #define IWL_TX_CRC_SIZE 4
 #define IWL_TX_DELIMITER_SIZE 4
@@ -47,12 +46,11 @@
  * iwl_trans_txq_update_byte_cnt_tbl - Set up entry in Tx byte-count array
  */
 void iwl_trans_txq_update_byte_cnt_tbl(struct iwl_trans *trans,
-					   struct iwl_tx_queue *txq,
-					   u16 byte_cnt)
+				       struct iwl_tx_queue *txq,
+				       u16 byte_cnt)
 {
 	struct iwlagn_scd_bc_tbl *scd_bc_tbl;
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int write_ptr = txq->q.write_ptr;
 	int txq_id = txq->q.id;
 	u8 sec_ctl = 0;
@@ -178,8 +176,8 @@ static inline u8 iwl_tfd_get_num_tbs(struct iwl_tfd *tfd)
 	return tfd->num_tbs & 0x1f;
 }
 
-static void iwlagn_unmap_tfd(struct iwl_trans *trans, struct iwl_cmd_meta *meta,
-		     struct iwl_tfd *tfd, enum dma_data_direction dma_dir)
+static void iwl_unmap_tfd(struct iwl_trans *trans, struct iwl_cmd_meta *meta,
+			  struct iwl_tfd *tfd, enum dma_data_direction dma_dir)
 {
 	int i;
 	int num_tbs;
@@ -209,7 +207,7 @@ static void iwlagn_unmap_tfd(struct iwl_trans *trans, struct iwl_cmd_meta *meta,
 }
 
 /**
- * iwlagn_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
+ * iwl_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
  * @trans - transport private data
  * @txq - tx queue
  * @dma_dir - the direction of the DMA mapping
@@ -217,8 +215,8 @@ static void iwlagn_unmap_tfd(struct iwl_trans *trans, struct iwl_cmd_meta *meta,
  * Does NOT advance any TFD circular buffer read/write indexes
  * Does NOT free the TFD itself (which is within circular buffer)
  */
-void iwlagn_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
-			 enum dma_data_direction dma_dir)
+void iwl_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
+		      enum dma_data_direction dma_dir)
 {
 	struct iwl_tfd *tfd_tmp = txq->tfds;
 
@@ -229,8 +227,8 @@ void iwlagn_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
 	lockdep_assert_held(&txq->lock);
 
 	/* We have only q->n_window txq->entries, but we use q->n_bd tfds */
-	iwlagn_unmap_tfd(trans, &txq->entries[idx].meta,
-			 &tfd_tmp[rd_ptr], dma_dir);
+	iwl_unmap_tfd(trans, &txq->entries[idx].meta, &tfd_tmp[rd_ptr],
+		      dma_dir);
 
 	/* free SKB */
 	if (txq->entries) {
@@ -270,7 +268,7 @@ int iwlagn_txq_attach_buf_to_tfd(struct iwl_trans *trans,
 	/* Each TFD can point to a maximum 20 Tx buffers */
 	if (num_tbs >= IWL_NUM_OF_TBS) {
 		IWL_ERR(trans, "Error can not send more than %d chunks\n",
-			  IWL_NUM_OF_TBS);
+			IWL_NUM_OF_TBS);
 		return -EINVAL;
 	}
 
@@ -279,7 +277,7 @@ int iwlagn_txq_attach_buf_to_tfd(struct iwl_trans *trans,
 
 	if (unlikely(addr & ~IWL_TX_DMA_MASK))
 		IWL_ERR(trans, "Unaligned address = %llx\n",
-			  (unsigned long long)addr);
+			(unsigned long long)addr);
 
 	iwl_tfd_set_tb(tfd, num_tbs, addr, len);
 
@@ -382,16 +380,14 @@ static void iwlagn_txq_inval_byte_cnt_tbl(struct iwl_trans *trans,
 			tfd_offset[TFD_QUEUE_SIZE_MAX + read_ptr] = bc_ent;
 }
 
-static int iwlagn_tx_queue_set_q2ratid(struct iwl_trans *trans, u16 ra_tid,
-					u16 txq_id)
+static int iwl_txq_set_ratid_map(struct iwl_trans *trans, u16 ra_tid,
+				 u16 txq_id)
 {
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 tbl_dw_addr;
 	u32 tbl_dw;
 	u16 scd_q2ratid;
 
-	struct iwl_trans_pcie *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
 	scd_q2ratid = ra_tid & SCD_QUEUE_RA_TID_MAP_RATID_MSK;
 
 	tbl_dw_addr = trans_pcie->scd_base_addr +
@@ -409,7 +405,7 @@ static int iwlagn_tx_queue_set_q2ratid(struct iwl_trans *trans, u16 ra_tid,
 	return 0;
 }
 
-static void iwlagn_tx_queue_stop_scheduler(struct iwl_trans *trans, u16 txq_id)
+static inline void iwl_txq_set_inactive(struct iwl_trans *trans, 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. */
@@ -419,102 +415,87 @@ static void iwlagn_tx_queue_stop_scheduler(struct iwl_trans *trans, u16 txq_id)
 		(1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
 }
 
-void iwl_trans_set_wr_ptrs(struct iwl_trans *trans,
-				int txq_id, u32 index)
-{
-	IWL_DEBUG_TX_QUEUES(trans, "Q %d  WrPtr: %d\n", txq_id, index & 0xff);
-	iwl_write_direct32(trans, HBUS_TARG_WRPTR,
-			(index & 0xff) | (txq_id << 8));
-	iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), index);
-}
-
-void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
-				   struct iwl_tx_queue *txq,
-				   int tx_fifo_id, bool active)
-{
-	int txq_id = txq->q.id;
-
-	iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
-			(active << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
-			(tx_fifo_id << SCD_QUEUE_STTS_REG_POS_TXF) |
-			(1 << SCD_QUEUE_STTS_REG_POS_WSL) |
-			SCD_QUEUE_STTS_REG_MSK);
-
-	if (active)
-		IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d on FIFO %d\n",
-				    txq_id, tx_fifo_id);
-	else
-		IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
-}
-
-void iwl_trans_pcie_tx_agg_setup(struct iwl_trans *trans, int txq_id, int fifo,
-				 int sta_id, int tid, int frame_limit, u16 ssn)
+void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
+			       int sta_id, int tid, int frame_limit, u16 ssn)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	unsigned long flags;
-	u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
 	if (test_and_set_bit(txq_id, trans_pcie->queue_used))
 		WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
 
-	spin_lock_irqsave(&trans_pcie->irq_lock, flags);
-
 	/* Stop this Tx queue before configuring it */
-	iwlagn_tx_queue_stop_scheduler(trans, txq_id);
+	iwl_txq_set_inactive(trans, txq_id);
 
-	/* Map receiver-address / traffic-ID to this queue */
-	iwlagn_tx_queue_set_q2ratid(trans, ra_tid, txq_id);
+	/* Set this queue as a chain-building queue unless it is CMD queue */
+	if (txq_id != trans_pcie->cmd_queue)
+		iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
 
-	/* Set this queue as a chain-building queue */
-	iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
+	/* If this queue is mapped to a certain station: it is an AGG queue */
+	if (sta_id != IWL_INVALID_STATION) {
+		u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
-	/* enable aggregations for the queue */
-	iwl_set_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+		/* Map receiver-address / traffic-ID to this queue */
+		iwl_txq_set_ratid_map(trans, ra_tid, txq_id);
+
+		/* enable aggregations for the queue */
+		iwl_set_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+	} else {
+		/*
+		 * disable aggregations for the queue, this will also make the
+		 * ra_tid mapping configuration irrelevant since it is now a
+		 * non-AGG queue.
+		 */
+		iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+	}
 
 	/* Place first TFD at index corresponding to start sequence number.
 	 * Assumes that ssn_idx is valid (!= 0xFFF) */
 	trans_pcie->txq[txq_id].q.read_ptr = (ssn & 0xff);
 	trans_pcie->txq[txq_id].q.write_ptr = (ssn & 0xff);
-	iwl_trans_set_wr_ptrs(trans, txq_id, ssn);
+
+	iwl_write_direct32(trans, HBUS_TARG_WRPTR,
+			   (ssn & 0xff) | (txq_id << 8));
+	iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), ssn);
 
 	/* Set up Tx window size and frame limit for this queue */
 	iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
+			SCD_CONTEXT_QUEUE_OFFSET(txq_id), 0);
+	iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
 			SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
 			((frame_limit << SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
 				SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
 			((frame_limit << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
 				SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
 
-	iwl_set_bits_prph(trans, SCD_INTERRUPT_MASK, (1 << txq_id));
-
 	/* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
-	iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id],
-				      fifo, true);
-
-	spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
+	iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
+		       (1 << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+		       (fifo << SCD_QUEUE_STTS_REG_POS_TXF) |
+		       (1 << SCD_QUEUE_STTS_REG_POS_WSL) |
+		       SCD_QUEUE_STTS_REG_MSK);
+	IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d on FIFO %d WrPtr: %d\n",
+			    txq_id, fifo, ssn & 0xff);
 }
 
-void iwl_trans_pcie_tx_agg_disable(struct iwl_trans *trans, int txq_id)
+void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	u16 rd_ptr, wr_ptr;
+	int n_bd = trans_pcie->txq[txq_id].q.n_bd;
 
 	if (!test_and_clear_bit(txq_id, trans_pcie->queue_used)) {
 		WARN_ONCE(1, "queue %d not used", txq_id);
 		return;
 	}
 
-	iwlagn_tx_queue_stop_scheduler(trans, txq_id);
-
-	iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+	rd_ptr = iwl_read_prph(trans, SCD_QUEUE_RDPTR(txq_id)) & (n_bd - 1);
+	wr_ptr = iwl_read_prph(trans, SCD_QUEUE_WRPTR(txq_id));
 
-	trans_pcie->txq[txq_id].q.read_ptr = 0;
-	trans_pcie->txq[txq_id].q.write_ptr = 0;
-	iwl_trans_set_wr_ptrs(trans, txq_id, 0);
+	WARN_ONCE(rd_ptr != wr_ptr, "queue %d isn't empty: [%d,%d]",
+		  txq_id, rd_ptr, wr_ptr);
 
-	iwl_clear_bits_prph(trans, SCD_INTERRUPT_MASK, BIT(txq_id));
-
-	iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id],
-				      0, false);
+	iwl_txq_set_inactive(trans, txq_id);
+	IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
 }
 
 /*************** HOST COMMAND QUEUE FUNCTIONS   *****/
@@ -615,13 +596,13 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	}
 
 	IWL_DEBUG_HC(trans,
-		"Sending command %s (#%x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
-		trans_pcie_get_cmd_string(trans_pcie, out_cmd->hdr.cmd),
-		out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence), cmd_size,
-		q->write_ptr, idx, trans_pcie->cmd_queue);
+		     "Sending command %s (#%x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
+		     trans_pcie_get_cmd_string(trans_pcie, out_cmd->hdr.cmd),
+		     out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
+		     cmd_size, q->write_ptr, idx, trans_pcie->cmd_queue);
 
 	phys_addr = dma_map_single(trans->dev, &out_cmd->hdr, copy_size,
-				DMA_BIDIRECTIONAL);
+				   DMA_BIDIRECTIONAL);
 	if (unlikely(dma_mapping_error(trans->dev, phys_addr))) {
 		idx = -ENOMEM;
 		goto out;
@@ -630,8 +611,7 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	dma_unmap_addr_set(out_meta, mapping, phys_addr);
 	dma_unmap_len_set(out_meta, len, copy_size);
 
-	iwlagn_txq_attach_buf_to_tfd(trans, txq,
-					phys_addr, copy_size, 1);
+	iwlagn_txq_attach_buf_to_tfd(trans, txq, phys_addr, copy_size, 1);
 #ifdef CONFIG_IWLWIFI_DEVICE_TRACING
 	trace_bufs[0] = &out_cmd->hdr;
 	trace_lens[0] = copy_size;
@@ -643,13 +623,12 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 			continue;
 		if (!(cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY))
 			continue;
-		phys_addr = dma_map_single(trans->dev,
-					   (void *)cmd->data[i],
+		phys_addr = dma_map_single(trans->dev, (void *)cmd->data[i],
 					   cmd->len[i], DMA_BIDIRECTIONAL);
 		if (dma_mapping_error(trans->dev, phys_addr)) {
-			iwlagn_unmap_tfd(trans, out_meta,
-					 &txq->tfds[q->write_ptr],
-					 DMA_BIDIRECTIONAL);
+			iwl_unmap_tfd(trans, out_meta,
+				      &txq->tfds[q->write_ptr],
+				      DMA_BIDIRECTIONAL);
 			idx = -ENOMEM;
 			goto out;
 		}
@@ -723,9 +702,10 @@ static void iwl_hcmd_queue_reclaim(struct iwl_trans *trans, int txq_id,
 	lockdep_assert_held(&txq->lock);
 
 	if ((idx >= q->n_bd) || (iwl_queue_used(q, idx) == 0)) {
-		IWL_ERR(trans, "%s: Read index for DMA queue txq id (%d), "
-			  "index %d is out of range [0-%d] %d %d.\n", __func__,
-			  txq_id, idx, q->n_bd, q->write_ptr, q->read_ptr);
+		IWL_ERR(trans,
+			"%s: Read index for DMA queue txq id (%d), index %d is out of range [0-%d] %d %d.\n",
+			__func__, txq_id, idx, q->n_bd,
+			q->write_ptr, q->read_ptr);
 		return;
 	}
 
@@ -733,8 +713,8 @@ static void iwl_hcmd_queue_reclaim(struct iwl_trans *trans, int txq_id,
 	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
 
 		if (nfreed++ > 0) {
-			IWL_ERR(trans, "HCMD skipped: index (%d) %d %d\n", idx,
-					q->write_ptr, q->read_ptr);
+			IWL_ERR(trans, "HCMD skipped: index (%d) %d %d\n",
+				idx, q->write_ptr, q->read_ptr);
 			iwl_op_mode_nic_error(trans->op_mode);
 		}
 
@@ -771,9 +751,9 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans, struct iwl_rx_cmd_buffer *rxb,
 	 * in the queue management code. */
 	if (WARN(txq_id != trans_pcie->cmd_queue,
 		 "wrong command queue %d (should be %d), sequence 0x%X readp=%d writep=%d\n",
-		  txq_id, trans_pcie->cmd_queue, sequence,
-		  trans_pcie->txq[trans_pcie->cmd_queue].q.read_ptr,
-		  trans_pcie->txq[trans_pcie->cmd_queue].q.write_ptr)) {
+		 txq_id, trans_pcie->cmd_queue, sequence,
+		 trans_pcie->txq[trans_pcie->cmd_queue].q.read_ptr,
+		 trans_pcie->txq[trans_pcie->cmd_queue].q.write_ptr)) {
 		iwl_print_hex_error(trans, pkt, 32);
 		return;
 	}
@@ -784,8 +764,7 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans, struct iwl_rx_cmd_buffer *rxb,
 	cmd = txq->entries[cmd_index].cmd;
 	meta = &txq->entries[cmd_index].meta;
 
-	iwlagn_unmap_tfd(trans, meta, &txq->tfds[index],
-			 DMA_BIDIRECTIONAL);
+	iwl_unmap_tfd(trans, meta, &txq->tfds[index], DMA_BIDIRECTIONAL);
 
 	/* Input error checking is done when commands are added to queue. */
 	if (meta->flags & CMD_WANT_SKB) {
@@ -870,8 +849,9 @@ static int iwl_send_cmd_sync(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	}
 
 	ret = wait_event_timeout(trans->wait_command_queue,
-			!test_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status),
-			HOST_COMPLETE_TIMEOUT);
+				 !test_bit(STATUS_HCMD_ACTIVE,
+					   &trans_pcie->status),
+				 HOST_COMPLETE_TIMEOUT);
 	if (!ret) {
 		if (test_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status)) {
 			struct iwl_tx_queue *txq =
@@ -956,10 +936,10 @@ int iwl_tx_queue_reclaim(struct iwl_trans *trans, int txq_id, int index,
 
 	if ((index >= q->n_bd) ||
 	   (iwl_queue_used(q, last_to_free) == 0)) {
-		IWL_ERR(trans, "%s: Read index for DMA queue txq id (%d), "
-			  "last_to_free %d is out of range [0-%d] %d %d.\n",
-			  __func__, txq_id, last_to_free, q->n_bd,
-			  q->write_ptr, q->read_ptr);
+		IWL_ERR(trans,
+			"%s: Read index for DMA queue txq id (%d), last_to_free %d is out of range [0-%d] %d %d.\n",
+			__func__, txq_id, last_to_free, q->n_bd,
+			q->write_ptr, q->read_ptr);
 		return 0;
 	}
 
@@ -979,7 +959,7 @@ int iwl_tx_queue_reclaim(struct iwl_trans *trans, int txq_id, int index,
 
 		iwlagn_txq_inval_byte_cnt_tbl(trans, txq);
 
-		iwlagn_txq_free_tfd(trans, txq, DMA_TO_DEVICE);
+		iwl_txq_free_tfd(trans, txq, DMA_TO_DEVICE);
 		freed++;
 	}
 
diff --git a/drivers/net/wireless/iwmc3200wifi/Kconfig b/drivers/net/wireless/iwmc3200wifi/Kconfig
deleted file mode 100644
index 7107ce53d4d..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/Kconfig
+++ /dev/null
@@ -1,39 +0,0 @@
-config IWM
-	tristate "Intel Wireless Multicomm 3200 WiFi driver (EXPERIMENTAL)"
-	depends on MMC && EXPERIMENTAL
-	depends on CFG80211
-	select FW_LOADER
-	select IWMC3200TOP
-	help
-	  The Intel Wireless Multicomm 3200 hardware is a combo
-	  card with GPS, Bluetooth, WiMax and 802.11 radios. It
-	  runs over SDIO and is typically found on Moorestown
-	  based platform. This driver takes care of the 802.11
-	  part, which is a fullmac one.
-
-	  If you choose to build it as a module, it'll be called
-	  iwmc3200wifi.ko.
-
-config IWM_DEBUG
-	bool "Enable full debugging output in iwmc3200wifi"
-	depends on IWM && DEBUG_FS
-	help
-	  This option will enable debug tracing and setting for iwm
-
-	  You can set the debug level and module through debugfs. By
-	  default all modules are set to the IWL_DL_ERR level.
-	  To see the list of debug modules and levels, see iwm/debug.h
-
-	  For example, if you want the full MLME debug output:
-	  echo 0xff > /sys/kernel/debug/iwm/phyN/debug/mlme
-
-	  Or, if you want the full debug, for all modules:
-	  echo 0xff > /sys/kernel/debug/iwm/phyN/debug/level
-	  echo 0xff > /sys/kernel/debug/iwm/phyN/debug/modules
-
-config IWM_TRACING
-	bool "Enable event tracing for iwmc3200wifi"
-	depends on IWM && EVENT_TRACING
-	help
-	  Say Y here to trace all the commands and responses between
-	  the driver and firmware (including TX/RX frames) with ftrace.
diff --git a/drivers/net/wireless/iwmc3200wifi/Makefile b/drivers/net/wireless/iwmc3200wifi/Makefile
deleted file mode 100644
index cdc7e07ba11..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/Makefile
+++ /dev/null
@@ -1,10 +0,0 @@
-obj-$(CONFIG_IWM) := iwmc3200wifi.o
-iwmc3200wifi-objs += main.o netdev.o rx.o tx.o sdio.o hal.o fw.o
-iwmc3200wifi-objs += commands.o cfg80211.o eeprom.o
-
-iwmc3200wifi-$(CONFIG_IWM_DEBUG) += debugfs.o
-iwmc3200wifi-$(CONFIG_IWM_TRACING) += trace.o
-
-CFLAGS_trace.o := -I$(src)
-
-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/iwmc3200wifi/bus.h b/drivers/net/wireless/iwmc3200wifi/bus.h
deleted file mode 100644
index 62edd5888a7..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/bus.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- * 02110-1301, USA.
- *
- */
-
-#ifndef __IWM_BUS_H__
-#define __IWM_BUS_H__
-
-#include "iwm.h"
-
-struct iwm_if_ops {
-	int (*enable)(struct iwm_priv *iwm);
-	int (*disable)(struct iwm_priv *iwm);
-	int (*send_chunk)(struct iwm_priv *iwm, u8* buf, int count);
-
-	void (*debugfs_init)(struct iwm_priv *iwm, struct dentry *parent_dir);
-	void (*debugfs_exit)(struct iwm_priv *iwm);
-
-	const char *umac_name;
-	const char *calib_lmac_name;
-	const char *lmac_name;
-};
-
-static inline int iwm_bus_send_chunk(struct iwm_priv *iwm, u8 *buf, int count)
-{
-	return iwm->bus_ops->send_chunk(iwm, buf, count);
-}
-
-static inline int iwm_bus_enable(struct iwm_priv *iwm)
-{
-	return iwm->bus_ops->enable(iwm);
-}
-
-static inline int iwm_bus_disable(struct iwm_priv *iwm)
-{
-	return iwm->bus_ops->disable(iwm);
-}
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/cfg80211.c b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
deleted file mode 100644
index 48e8218fd23..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/cfg80211.c
+++ /dev/null
@@ -1,882 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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/netdevice.h>
-#include <linux/sched.h>
-#include <linux/etherdevice.h>
-#include <linux/wireless.h>
-#include <linux/ieee80211.h>
-#include <linux/slab.h>
-#include <net/cfg80211.h>
-
-#include "iwm.h"
-#include "commands.h"
-#include "cfg80211.h"
-#include "debug.h"
-
-#define RATETAB_ENT(_rate, _rateid, _flags) \
-	{								\
-		.bitrate	= (_rate),				\
-		.hw_value	= (_rateid),				\
-		.flags		= (_flags),				\
-	}
-
-#define CHAN2G(_channel, _freq, _flags) {			\
-	.band			= IEEE80211_BAND_2GHZ,		\
-	.center_freq		= (_freq),			\
-	.hw_value		= (_channel),			\
-	.flags			= (_flags),			\
-	.max_antenna_gain	= 0,				\
-	.max_power		= 30,				\
-}
-
-#define CHAN5G(_channel, _flags) {				\
-	.band			= IEEE80211_BAND_5GHZ,		\
-	.center_freq		= 5000 + (5 * (_channel)),	\
-	.hw_value		= (_channel),			\
-	.flags			= (_flags),			\
-	.max_antenna_gain	= 0,				\
-	.max_power		= 30,				\
-}
-
-static struct ieee80211_rate iwm_rates[] = {
-	RATETAB_ENT(10,  0x1,   0),
-	RATETAB_ENT(20,  0x2,   0),
-	RATETAB_ENT(55,  0x4,   0),
-	RATETAB_ENT(110, 0x8,   0),
-	RATETAB_ENT(60,  0x10,  0),
-	RATETAB_ENT(90,  0x20,  0),
-	RATETAB_ENT(120, 0x40,  0),
-	RATETAB_ENT(180, 0x80,  0),
-	RATETAB_ENT(240, 0x100, 0),
-	RATETAB_ENT(360, 0x200, 0),
-	RATETAB_ENT(480, 0x400, 0),
-	RATETAB_ENT(540, 0x800, 0),
-};
-
-#define iwm_a_rates		(iwm_rates + 4)
-#define iwm_a_rates_size	8
-#define iwm_g_rates		(iwm_rates + 0)
-#define iwm_g_rates_size	12
-
-static struct ieee80211_channel iwm_2ghz_channels[] = {
-	CHAN2G(1, 2412, 0),
-	CHAN2G(2, 2417, 0),
-	CHAN2G(3, 2422, 0),
-	CHAN2G(4, 2427, 0),
-	CHAN2G(5, 2432, 0),
-	CHAN2G(6, 2437, 0),
-	CHAN2G(7, 2442, 0),
-	CHAN2G(8, 2447, 0),
-	CHAN2G(9, 2452, 0),
-	CHAN2G(10, 2457, 0),
-	CHAN2G(11, 2462, 0),
-	CHAN2G(12, 2467, 0),
-	CHAN2G(13, 2472, 0),
-	CHAN2G(14, 2484, 0),
-};
-
-static struct ieee80211_channel iwm_5ghz_a_channels[] = {
-	CHAN5G(34, 0),		CHAN5G(36, 0),
-	CHAN5G(38, 0),		CHAN5G(40, 0),
-	CHAN5G(42, 0),		CHAN5G(44, 0),
-	CHAN5G(46, 0),		CHAN5G(48, 0),
-	CHAN5G(52, 0),		CHAN5G(56, 0),
-	CHAN5G(60, 0),		CHAN5G(64, 0),
-	CHAN5G(100, 0),		CHAN5G(104, 0),
-	CHAN5G(108, 0),		CHAN5G(112, 0),
-	CHAN5G(116, 0),		CHAN5G(120, 0),
-	CHAN5G(124, 0),		CHAN5G(128, 0),
-	CHAN5G(132, 0),		CHAN5G(136, 0),
-	CHAN5G(140, 0),		CHAN5G(149, 0),
-	CHAN5G(153, 0),		CHAN5G(157, 0),
-	CHAN5G(161, 0),		CHAN5G(165, 0),
-	CHAN5G(184, 0),		CHAN5G(188, 0),
-	CHAN5G(192, 0),		CHAN5G(196, 0),
-	CHAN5G(200, 0),		CHAN5G(204, 0),
-	CHAN5G(208, 0),		CHAN5G(212, 0),
-	CHAN5G(216, 0),
-};
-
-static struct ieee80211_supported_band iwm_band_2ghz = {
-	.channels = iwm_2ghz_channels,
-	.n_channels = ARRAY_SIZE(iwm_2ghz_channels),
-	.bitrates = iwm_g_rates,
-	.n_bitrates = iwm_g_rates_size,
-};
-
-static struct ieee80211_supported_band iwm_band_5ghz = {
-	.channels = iwm_5ghz_a_channels,
-	.n_channels = ARRAY_SIZE(iwm_5ghz_a_channels),
-	.bitrates = iwm_a_rates,
-	.n_bitrates = iwm_a_rates_size,
-};
-
-static int iwm_key_init(struct iwm_key *key, u8 key_index,
-			const u8 *mac_addr, struct key_params *params)
-{
-	key->hdr.key_idx = key_index;
-	if (!mac_addr || is_broadcast_ether_addr(mac_addr)) {
-		key->hdr.multicast = 1;
-		memset(key->hdr.mac, 0xff, ETH_ALEN);
-	} else {
-		key->hdr.multicast = 0;
-		memcpy(key->hdr.mac, mac_addr, ETH_ALEN);
-	}
-
-	if (params) {
-		if (params->key_len > WLAN_MAX_KEY_LEN ||
-		    params->seq_len > IW_ENCODE_SEQ_MAX_SIZE)
-			return -EINVAL;
-
-		key->cipher = params->cipher;
-		key->key_len = params->key_len;
-		key->seq_len = params->seq_len;
-		memcpy(key->key, params->key, key->key_len);
-		memcpy(key->seq, params->seq, key->seq_len);
-	}
-
-	return 0;
-}
-
-static int iwm_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
-				u8 key_index, bool pairwise, const u8 *mac_addr,
-				struct key_params *params)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-	struct iwm_key *key;
-	int ret;
-
-	IWM_DBG_WEXT(iwm, DBG, "Adding key for %pM\n", mac_addr);
-
-	if (key_index >= IWM_NUM_KEYS)
-		return -ENOENT;
-
-	key = &iwm->keys[key_index];
-	memset(key, 0, sizeof(struct iwm_key));
-	ret = iwm_key_init(key, key_index, mac_addr, params);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Invalid key_params\n");
-		return ret;
-	}
-
-	return iwm_set_key(iwm, 0, key);
-}
-
-static int iwm_cfg80211_get_key(struct wiphy *wiphy, struct net_device *ndev,
-				u8 key_index, bool pairwise, const u8 *mac_addr,
-				void *cookie,
-				void (*callback)(void *cookie,
-						 struct key_params*))
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-	struct iwm_key *key;
-	struct key_params params;
-
-	IWM_DBG_WEXT(iwm, DBG, "Getting key %d\n", key_index);
-
-	if (key_index >= IWM_NUM_KEYS)
-		return -ENOENT;
-
-	memset(&params, 0, sizeof(params));
-
-	key = &iwm->keys[key_index];
-	params.cipher = key->cipher;
-	params.key_len = key->key_len;
-	params.seq_len = key->seq_len;
-	params.seq = key->seq;
-	params.key = key->key;
-
-	callback(cookie, &params);
-
-	return key->key_len ? 0 : -ENOENT;
-}
-
-
-static int iwm_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
-				u8 key_index, bool pairwise, const u8 *mac_addr)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-	struct iwm_key *key;
-
-	if (key_index >= IWM_NUM_KEYS)
-		return -ENOENT;
-
-	key = &iwm->keys[key_index];
-	if (!iwm->keys[key_index].key_len) {
-		IWM_DBG_WEXT(iwm, DBG, "Key %d not used\n", key_index);
-		return 0;
-	}
-
-	if (key_index == iwm->default_key)
-		iwm->default_key = -1;
-
-	return iwm_set_key(iwm, 1, key);
-}
-
-static int iwm_cfg80211_set_default_key(struct wiphy *wiphy,
-					struct net_device *ndev,
-					u8 key_index, bool unicast,
-					bool multicast)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-
-	IWM_DBG_WEXT(iwm, DBG, "Default key index is: %d\n", key_index);
-
-	if (key_index >= IWM_NUM_KEYS)
-		return -ENOENT;
-
-	if (!iwm->keys[key_index].key_len) {
-		IWM_ERR(iwm, "Key %d not used\n", key_index);
-		return -EINVAL;
-	}
-
-	iwm->default_key = key_index;
-
-	return iwm_set_tx_key(iwm, key_index);
-}
-
-static int iwm_cfg80211_get_station(struct wiphy *wiphy,
-				    struct net_device *ndev,
-				    u8 *mac, struct station_info *sinfo)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-
-	if (memcmp(mac, iwm->bssid, ETH_ALEN))
-		return -ENOENT;
-
-	sinfo->filled |= STATION_INFO_TX_BITRATE;
-	sinfo->txrate.legacy = iwm->rate * 10;
-
-	if (test_bit(IWM_STATUS_ASSOCIATED, &iwm->status)) {
-		sinfo->filled |= STATION_INFO_SIGNAL;
-		sinfo->signal = iwm->wstats.qual.level;
-	}
-
-	return 0;
-}
-
-
-int iwm_cfg80211_inform_bss(struct iwm_priv *iwm)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct iwm_bss_info *bss;
-	struct iwm_umac_notif_bss_info *umac_bss;
-	struct ieee80211_mgmt *mgmt;
-	struct ieee80211_channel *channel;
-	struct ieee80211_supported_band *band;
-	s32 signal;
-	int freq;
-
-	list_for_each_entry(bss, &iwm->bss_list, node) {
-		umac_bss = bss->bss;
-		mgmt = (struct ieee80211_mgmt *)(umac_bss->frame_buf);
-
-		if (umac_bss->band == UMAC_BAND_2GHZ)
-			band = wiphy->bands[IEEE80211_BAND_2GHZ];
-		else if (umac_bss->band == UMAC_BAND_5GHZ)
-			band = wiphy->bands[IEEE80211_BAND_5GHZ];
-		else {
-			IWM_ERR(iwm, "Invalid band: %d\n", umac_bss->band);
-			return -EINVAL;
-		}
-
-		freq = ieee80211_channel_to_frequency(umac_bss->channel,
-						      band->band);
-		channel = ieee80211_get_channel(wiphy, freq);
-		signal = umac_bss->rssi * 100;
-
-		if (!cfg80211_inform_bss_frame(wiphy, channel, mgmt,
-					       le16_to_cpu(umac_bss->frame_len),
-					       signal, GFP_KERNEL))
-			return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int iwm_cfg80211_change_iface(struct wiphy *wiphy,
-				     struct net_device *ndev,
-				     enum nl80211_iftype type, u32 *flags,
-				     struct vif_params *params)
-{
-	struct wireless_dev *wdev;
-	struct iwm_priv *iwm;
-	u32 old_mode;
-
-	wdev = ndev->ieee80211_ptr;
-	iwm = ndev_to_iwm(ndev);
-	old_mode = iwm->conf.mode;
-
-	switch (type) {
-	case NL80211_IFTYPE_STATION:
-		iwm->conf.mode = UMAC_MODE_BSS;
-		break;
-	case NL80211_IFTYPE_ADHOC:
-		iwm->conf.mode = UMAC_MODE_IBSS;
-		break;
-	default:
-		return -EOPNOTSUPP;
-	}
-
-	wdev->iftype = type;
-
-	if ((old_mode == iwm->conf.mode) || !iwm->umac_profile)
-		return 0;
-
-	iwm->umac_profile->mode = cpu_to_le32(iwm->conf.mode);
-
-	if (iwm->umac_profile_active)
-		iwm_invalidate_mlme_profile(iwm);
-
-	return 0;
-}
-
-static int iwm_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-			     struct cfg80211_scan_request *request)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-	int ret;
-
-	if (!test_bit(IWM_STATUS_READY, &iwm->status)) {
-		IWM_ERR(iwm, "Scan while device is not ready\n");
-		return -EIO;
-	}
-
-	if (test_bit(IWM_STATUS_SCANNING, &iwm->status)) {
-		IWM_ERR(iwm, "Scanning already\n");
-		return -EAGAIN;
-	}
-
-	if (test_bit(IWM_STATUS_SCAN_ABORTING, &iwm->status)) {
-		IWM_ERR(iwm, "Scanning being aborted\n");
-		return -EAGAIN;
-	}
-
-	set_bit(IWM_STATUS_SCANNING, &iwm->status);
-
-	ret = iwm_scan_ssids(iwm, request->ssids, request->n_ssids);
-	if (ret) {
-		clear_bit(IWM_STATUS_SCANNING, &iwm->status);
-		return ret;
-	}
-
-	iwm->scan_request = request;
-	return 0;
-}
-
-static int iwm_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-
-	if (changed & WIPHY_PARAM_RTS_THRESHOLD &&
-	    (iwm->conf.rts_threshold != wiphy->rts_threshold)) {
-		int ret;
-
-		iwm->conf.rts_threshold = wiphy->rts_threshold;
-
-		ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-					     CFG_RTS_THRESHOLD,
-					     iwm->conf.rts_threshold);
-		if (ret < 0)
-			return ret;
-	}
-
-	if (changed & WIPHY_PARAM_FRAG_THRESHOLD &&
-	    (iwm->conf.frag_threshold != wiphy->frag_threshold)) {
-		int ret;
-
-		iwm->conf.frag_threshold = wiphy->frag_threshold;
-
-		ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_FA_CFG_FIX,
-					     CFG_FRAG_THRESHOLD,
-					     iwm->conf.frag_threshold);
-		if (ret < 0)
-			return ret;
-	}
-
-	return 0;
-}
-
-static int iwm_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
-				  struct cfg80211_ibss_params *params)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-	struct ieee80211_channel *chan = params->channel;
-
-	if (!test_bit(IWM_STATUS_READY, &iwm->status))
-		return -EIO;
-
-	/* UMAC doesn't support creating or joining an IBSS network
-	 * with specified bssid. */
-	if (params->bssid)
-		return -EOPNOTSUPP;
-
-	iwm->channel = ieee80211_frequency_to_channel(chan->center_freq);
-	iwm->umac_profile->ibss.band = chan->band;
-	iwm->umac_profile->ibss.channel = iwm->channel;
-	iwm->umac_profile->ssid.ssid_len = params->ssid_len;
-	memcpy(iwm->umac_profile->ssid.ssid, params->ssid, params->ssid_len);
-
-	return iwm_send_mlme_profile(iwm);
-}
-
-static int iwm_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-
-	if (iwm->umac_profile_active)
-		return iwm_invalidate_mlme_profile(iwm);
-
-	return 0;
-}
-
-static int iwm_set_auth_type(struct iwm_priv *iwm,
-			     enum nl80211_auth_type sme_auth_type)
-{
-	u8 *auth_type = &iwm->umac_profile->sec.auth_type;
-
-	switch (sme_auth_type) {
-	case NL80211_AUTHTYPE_AUTOMATIC:
-	case NL80211_AUTHTYPE_OPEN_SYSTEM:
-		IWM_DBG_WEXT(iwm, DBG, "OPEN auth\n");
-		*auth_type = UMAC_AUTH_TYPE_OPEN;
-		break;
-	case NL80211_AUTHTYPE_SHARED_KEY:
-		if (iwm->umac_profile->sec.flags &
-		    (UMAC_SEC_FLG_WPA_ON_MSK | UMAC_SEC_FLG_RSNA_ON_MSK)) {
-			IWM_DBG_WEXT(iwm, DBG, "WPA auth alg\n");
-			*auth_type = UMAC_AUTH_TYPE_RSNA_PSK;
-		} else {
-			IWM_DBG_WEXT(iwm, DBG, "WEP shared key auth alg\n");
-			*auth_type = UMAC_AUTH_TYPE_LEGACY_PSK;
-		}
-
-		break;
-	default:
-		IWM_ERR(iwm, "Unsupported auth alg: 0x%x\n", sme_auth_type);
-		return -ENOTSUPP;
-	}
-
-	return 0;
-}
-
-static int iwm_set_wpa_version(struct iwm_priv *iwm, u32 wpa_version)
-{
-	IWM_DBG_WEXT(iwm, DBG, "wpa_version: %d\n", wpa_version);
-
-	if (!wpa_version) {
-		iwm->umac_profile->sec.flags = UMAC_SEC_FLG_LEGACY_PROFILE;
-		return 0;
-	}
-
-	if (wpa_version & NL80211_WPA_VERSION_1)
-		iwm->umac_profile->sec.flags = UMAC_SEC_FLG_WPA_ON_MSK;
-
-	if (wpa_version & NL80211_WPA_VERSION_2)
-		iwm->umac_profile->sec.flags = UMAC_SEC_FLG_RSNA_ON_MSK;
-
-	return 0;
-}
-
-static int iwm_set_cipher(struct iwm_priv *iwm, u32 cipher, bool ucast)
-{
-	u8 *profile_cipher = ucast ? &iwm->umac_profile->sec.ucast_cipher :
-		&iwm->umac_profile->sec.mcast_cipher;
-
-	if (!cipher) {
-		*profile_cipher = UMAC_CIPHER_TYPE_NONE;
-		return 0;
-	}
-
-	IWM_DBG_WEXT(iwm, DBG, "%ccast cipher is 0x%x\n", ucast ? 'u' : 'm',
-		     cipher);
-
-	switch (cipher) {
-	case IW_AUTH_CIPHER_NONE:
-		*profile_cipher = UMAC_CIPHER_TYPE_NONE;
-		break;
-	case WLAN_CIPHER_SUITE_WEP40:
-		*profile_cipher = UMAC_CIPHER_TYPE_WEP_40;
-		break;
-	case WLAN_CIPHER_SUITE_WEP104:
-		*profile_cipher = UMAC_CIPHER_TYPE_WEP_104;
-		break;
-	case WLAN_CIPHER_SUITE_TKIP:
-		*profile_cipher = UMAC_CIPHER_TYPE_TKIP;
-		break;
-	case WLAN_CIPHER_SUITE_CCMP:
-		*profile_cipher = UMAC_CIPHER_TYPE_CCMP;
-		break;
-	default:
-		IWM_ERR(iwm, "Unsupported cipher: 0x%x\n", cipher);
-		return -ENOTSUPP;
-	}
-
-	return 0;
-}
-
-static int iwm_set_key_mgt(struct iwm_priv *iwm, u32 key_mgt)
-{
-	u8 *auth_type = &iwm->umac_profile->sec.auth_type;
-
-	IWM_DBG_WEXT(iwm, DBG, "key_mgt: 0x%x\n", key_mgt);
-
-	if (key_mgt == WLAN_AKM_SUITE_8021X)
-		*auth_type = UMAC_AUTH_TYPE_8021X;
-	else if (key_mgt == WLAN_AKM_SUITE_PSK) {
-		if (iwm->umac_profile->sec.flags &
-		    (UMAC_SEC_FLG_WPA_ON_MSK | UMAC_SEC_FLG_RSNA_ON_MSK))
-			*auth_type = UMAC_AUTH_TYPE_RSNA_PSK;
-		else
-			*auth_type = UMAC_AUTH_TYPE_LEGACY_PSK;
-	} else {
-		IWM_ERR(iwm, "Invalid key mgt: 0x%x\n", key_mgt);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-
-static int iwm_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
-				 struct cfg80211_connect_params *sme)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-	struct ieee80211_channel *chan = sme->channel;
-	struct key_params key_param;
-	int ret;
-
-	if (!test_bit(IWM_STATUS_READY, &iwm->status))
-		return -EIO;
-
-	if (!sme->ssid)
-		return -EINVAL;
-
-	if (iwm->umac_profile_active) {
-		ret = iwm_invalidate_mlme_profile(iwm);
-		if (ret) {
-			IWM_ERR(iwm, "Couldn't invalidate profile\n");
-			return ret;
-		}
-	}
-
-	if (chan)
-		iwm->channel =
-			ieee80211_frequency_to_channel(chan->center_freq);
-
-	iwm->umac_profile->ssid.ssid_len = sme->ssid_len;
-	memcpy(iwm->umac_profile->ssid.ssid, sme->ssid, sme->ssid_len);
-
-	if (sme->bssid) {
-		IWM_DBG_WEXT(iwm, DBG, "BSSID: %pM\n", sme->bssid);
-		memcpy(&iwm->umac_profile->bssid[0], sme->bssid, ETH_ALEN);
-		iwm->umac_profile->bss_num = 1;
-	} else {
-		memset(&iwm->umac_profile->bssid[0], 0, ETH_ALEN);
-		iwm->umac_profile->bss_num = 0;
-	}
-
-	ret = iwm_set_wpa_version(iwm, sme->crypto.wpa_versions);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_set_auth_type(iwm, sme->auth_type);
-	if (ret < 0)
-		return ret;
-
-	if (sme->crypto.n_ciphers_pairwise) {
-		ret = iwm_set_cipher(iwm, sme->crypto.ciphers_pairwise[0],
-				     true);
-		if (ret < 0)
-			return ret;
-	}
-
-	ret = iwm_set_cipher(iwm, sme->crypto.cipher_group, false);
-	if (ret < 0)
-		return ret;
-
-	if (sme->crypto.n_akm_suites) {
-		ret = iwm_set_key_mgt(iwm, sme->crypto.akm_suites[0]);
-		if (ret < 0)
-			return ret;
-	}
-
-	/*
-	 * We save the WEP key in case we want to do shared authentication.
-	 * We have to do it so because UMAC will assert whenever it gets a
-	 * key before a profile.
-	 */
-	if (sme->key) {
-		key_param.key = kmemdup(sme->key, sme->key_len, GFP_KERNEL);
-		if (key_param.key == NULL)
-			return -ENOMEM;
-		key_param.key_len = sme->key_len;
-		key_param.seq_len = 0;
-		key_param.cipher = sme->crypto.ciphers_pairwise[0];
-
-		ret = iwm_key_init(&iwm->keys[sme->key_idx], sme->key_idx,
-				   NULL, &key_param);
-		kfree(key_param.key);
-		if (ret < 0) {
-			IWM_ERR(iwm, "Invalid key_params\n");
-			return ret;
-		}
-
-		iwm->default_key = sme->key_idx;
-	}
-
-	/* WPA and open AUTH type from wpa_s means WPS (a.k.a. WSC) */
-	if ((iwm->umac_profile->sec.flags &
-	     (UMAC_SEC_FLG_WPA_ON_MSK | UMAC_SEC_FLG_RSNA_ON_MSK)) &&
-	    iwm->umac_profile->sec.auth_type == UMAC_AUTH_TYPE_OPEN) {
-			iwm->umac_profile->sec.flags = UMAC_SEC_FLG_WSC_ON_MSK;
-	}
-
-	ret = iwm_send_mlme_profile(iwm);
-
-	if (iwm->umac_profile->sec.auth_type != UMAC_AUTH_TYPE_LEGACY_PSK ||
-	    sme->key == NULL)
-		return ret;
-
-	/*
-	 * We want to do shared auth.
-	 * We need to actually set the key we previously cached,
-	 * and then tell the UMAC it's the default one.
-	 * That will trigger the auth+assoc UMAC machinery, and again,
-	 * this must be done after setting the profile.
-	 */
-	ret = iwm_set_key(iwm, 0, &iwm->keys[sme->key_idx]);
-	if (ret < 0)
-		return ret;
-
-	return iwm_set_tx_key(iwm, iwm->default_key);
-}
-
-static int iwm_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
-				   u16 reason_code)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-
-	IWM_DBG_WEXT(iwm, DBG, "Active: %d\n", iwm->umac_profile_active);
-
-	if (iwm->umac_profile_active)
-		iwm_invalidate_mlme_profile(iwm);
-
-	return 0;
-}
-
-static int iwm_cfg80211_set_txpower(struct wiphy *wiphy,
-				    enum nl80211_tx_power_setting type, int mbm)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-	int ret;
-
-	switch (type) {
-	case NL80211_TX_POWER_AUTOMATIC:
-		return 0;
-	case NL80211_TX_POWER_FIXED:
-		if (mbm < 0 || (mbm % 100))
-			return -EOPNOTSUPP;
-
-		if (!test_bit(IWM_STATUS_READY, &iwm->status))
-			return 0;
-
-		ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-					      CFG_TX_PWR_LIMIT_USR,
-					      MBM_TO_DBM(mbm) * 2);
-		if (ret < 0)
-			return ret;
-
-		return iwm_tx_power_trigger(iwm);
-	default:
-		IWM_ERR(iwm, "Unsupported power type: %d\n", type);
-		return -EOPNOTSUPP;
-	}
-
-	return 0;
-}
-
-static int iwm_cfg80211_get_txpower(struct wiphy *wiphy, int *dbm)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-
-	*dbm = iwm->txpower >> 1;
-
-	return 0;
-}
-
-static int iwm_cfg80211_set_power_mgmt(struct wiphy *wiphy,
-				       struct net_device *dev,
-				       bool enabled, int timeout)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-	u32 power_index;
-
-	if (enabled)
-		power_index = IWM_POWER_INDEX_DEFAULT;
-	else
-		power_index = IWM_POWER_INDEX_MIN;
-
-	if (power_index == iwm->conf.power_index)
-		return 0;
-
-	iwm->conf.power_index = power_index;
-
-	return iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				       CFG_POWER_INDEX, iwm->conf.power_index);
-}
-
-static int iwm_cfg80211_set_pmksa(struct wiphy *wiphy,
-				  struct net_device *netdev,
-				  struct cfg80211_pmksa *pmksa)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-
-	return iwm_send_pmkid_update(iwm, pmksa, IWM_CMD_PMKID_ADD);
-}
-
-static int iwm_cfg80211_del_pmksa(struct wiphy *wiphy,
-				  struct net_device *netdev,
-				  struct cfg80211_pmksa *pmksa)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-
-	return iwm_send_pmkid_update(iwm, pmksa, IWM_CMD_PMKID_DEL);
-}
-
-static int iwm_cfg80211_flush_pmksa(struct wiphy *wiphy,
-				    struct net_device *netdev)
-{
-	struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
-	struct cfg80211_pmksa pmksa;
-
-	memset(&pmksa, 0, sizeof(struct cfg80211_pmksa));
-
-	return iwm_send_pmkid_update(iwm, &pmksa, IWM_CMD_PMKID_FLUSH);
-}
-
-
-static struct cfg80211_ops iwm_cfg80211_ops = {
-	.change_virtual_intf = iwm_cfg80211_change_iface,
-	.add_key = iwm_cfg80211_add_key,
-	.get_key = iwm_cfg80211_get_key,
-	.del_key = iwm_cfg80211_del_key,
-	.set_default_key = iwm_cfg80211_set_default_key,
-	.get_station = iwm_cfg80211_get_station,
-	.scan = iwm_cfg80211_scan,
-	.set_wiphy_params = iwm_cfg80211_set_wiphy_params,
-	.connect = iwm_cfg80211_connect,
-	.disconnect = iwm_cfg80211_disconnect,
-	.join_ibss = iwm_cfg80211_join_ibss,
-	.leave_ibss = iwm_cfg80211_leave_ibss,
-	.set_tx_power = iwm_cfg80211_set_txpower,
-	.get_tx_power = iwm_cfg80211_get_txpower,
-	.set_power_mgmt = iwm_cfg80211_set_power_mgmt,
-	.set_pmksa = iwm_cfg80211_set_pmksa,
-	.del_pmksa = iwm_cfg80211_del_pmksa,
-	.flush_pmksa = iwm_cfg80211_flush_pmksa,
-};
-
-static const u32 cipher_suites[] = {
-	WLAN_CIPHER_SUITE_WEP40,
-	WLAN_CIPHER_SUITE_WEP104,
-	WLAN_CIPHER_SUITE_TKIP,
-	WLAN_CIPHER_SUITE_CCMP,
-};
-
-struct wireless_dev *iwm_wdev_alloc(int sizeof_bus, struct device *dev)
-{
-	int ret = 0;
-	struct wireless_dev *wdev;
-
-	/*
-	 * We're trying to have the following memory
-	 * layout:
-	 *
-	 * +-------------------------+
-	 * | struct wiphy	     |
-	 * +-------------------------+
-	 * | struct iwm_priv         |
-	 * +-------------------------+
-	 * | bus private data        |
-	 * | (e.g. iwm_priv_sdio)    |
-	 * +-------------------------+
-	 *
-	 */
-
-	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
-	if (!wdev) {
-		dev_err(dev, "Couldn't allocate wireless device\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	wdev->wiphy = wiphy_new(&iwm_cfg80211_ops,
-				sizeof(struct iwm_priv) + sizeof_bus);
-	if (!wdev->wiphy) {
-		dev_err(dev, "Couldn't allocate wiphy device\n");
-		ret = -ENOMEM;
-		goto out_err_new;
-	}
-
-	set_wiphy_dev(wdev->wiphy, dev);
-	wdev->wiphy->max_scan_ssids = UMAC_WIFI_IF_PROBE_OPTION_MAX;
-	wdev->wiphy->max_num_pmkids = UMAC_MAX_NUM_PMKIDS;
-	wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
-				       BIT(NL80211_IFTYPE_ADHOC);
-	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &iwm_band_2ghz;
-	wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &iwm_band_5ghz;
-	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
-
-	wdev->wiphy->cipher_suites = cipher_suites;
-	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
-
-	ret = wiphy_register(wdev->wiphy);
-	if (ret < 0) {
-		dev_err(dev, "Couldn't register wiphy device\n");
-		goto out_err_register;
-	}
-
-	return wdev;
-
- out_err_register:
-	wiphy_free(wdev->wiphy);
-
- out_err_new:
-	kfree(wdev);
-
-	return ERR_PTR(ret);
-}
-
-void iwm_wdev_free(struct iwm_priv *iwm)
-{
-	struct wireless_dev *wdev = iwm_to_wdev(iwm);
-
-	if (!wdev)
-		return;
-
-	wiphy_unregister(wdev->wiphy);
-	wiphy_free(wdev->wiphy);
-	kfree(wdev);
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/cfg80211.h b/drivers/net/wireless/iwmc3200wifi/cfg80211.h
deleted file mode 100644
index 56a34145acb..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/cfg80211.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- * 02110-1301, USA.
- *
- */
-
-#ifndef __IWM_CFG80211_H__
-#define __IWM_CFG80211_H__
-
-int iwm_cfg80211_inform_bss(struct iwm_priv *iwm);
-struct wireless_dev *iwm_wdev_alloc(int sizeof_bus, struct device *dev);
-void iwm_wdev_free(struct iwm_priv *iwm);
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/commands.c b/drivers/net/wireless/iwmc3200wifi/commands.c
deleted file mode 100644
index bd75078c454..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/commands.c
+++ /dev/null
@@ -1,1002 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/wireless.h>
-#include <linux/etherdevice.h>
-#include <linux/ieee80211.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/moduleparam.h>
-
-#include "iwm.h"
-#include "bus.h"
-#include "hal.h"
-#include "umac.h"
-#include "commands.h"
-#include "debug.h"
-
-static int iwm_send_lmac_ptrough_cmd(struct iwm_priv *iwm,
-				     u8 lmac_cmd_id,
-				     const void *lmac_payload,
-				     u16 lmac_payload_size,
-				     u8 resp)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_LMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_lmac_cmd lmac_cmd;
-
-	lmac_cmd.id = lmac_cmd_id;
-
-	umac_cmd.id = UMAC_CMD_OPCODE_WIFI_PASS_THROUGH;
-	umac_cmd.resp = resp;
-
-	return iwm_hal_send_host_cmd(iwm, &udma_cmd, &umac_cmd, &lmac_cmd,
-				     lmac_payload, lmac_payload_size);
-}
-
-int iwm_send_wifi_if_cmd(struct iwm_priv *iwm, void *payload, u16 payload_size,
-			 bool resp)
-{
-	struct iwm_umac_wifi_if *hdr = (struct iwm_umac_wifi_if *)payload;
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	int ret;
-	u8 oid = hdr->oid;
-
-	if (!test_bit(IWM_STATUS_READY, &iwm->status)) {
-		IWM_ERR(iwm, "Interface is not ready yet");
-		return -EAGAIN;
-	}
-
-	umac_cmd.id = UMAC_CMD_OPCODE_WIFI_IF_WRAPPER;
-	umac_cmd.resp = resp;
-
-	ret = iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd,
-				    payload, payload_size);
-
-	if (resp) {
-		ret = wait_event_interruptible_timeout(iwm->wifi_ntfy_queue,
-				   test_and_clear_bit(oid, &iwm->wifi_ntfy[0]),
-				   3 * HZ);
-
-		return ret ? 0 : -EBUSY;
-	}
-
-	return ret;
-}
-
-static int modparam_wiwi = COEX_MODE_CM;
-module_param_named(wiwi, modparam_wiwi, int, 0644);
-MODULE_PARM_DESC(wiwi, "Wifi-WiMAX coexistence: 1=SA, 2=XOR, 3=CM (default)");
-
-static struct coex_event iwm_sta_xor_prio_tbl[COEX_EVENTS_NUM] =
-{
-	{4, 3, 0, COEX_UNASSOC_IDLE_FLAGS},
-	{4, 3, 0, COEX_UNASSOC_MANUAL_SCAN_FLAGS},
-	{4, 3, 0, COEX_UNASSOC_AUTO_SCAN_FLAGS},
-	{4, 3, 0, COEX_CALIBRATION_FLAGS},
-	{4, 3, 0, COEX_PERIODIC_CALIBRATION_FLAGS},
-	{4, 3, 0, COEX_CONNECTION_ESTAB_FLAGS},
-	{4, 3, 0, COEX_ASSOCIATED_IDLE_FLAGS},
-	{4, 3, 0, COEX_ASSOC_MANUAL_SCAN_FLAGS},
-	{4, 3, 0, COEX_ASSOC_AUTO_SCAN_FLAGS},
-	{4, 3, 0, COEX_ASSOC_ACTIVE_LEVEL_FLAGS},
-	{6, 3, 0, COEX_XOR_RF_ON_FLAGS},
-	{4, 3, 0, COEX_RF_OFF_FLAGS},
-	{6, 6, 0, COEX_STAND_ALONE_DEBUG_FLAGS},
-	{4, 3, 0, COEX_IPAN_ASSOC_LEVEL_FLAGS},
-	{4, 3, 0, COEX_RSRVD1_FLAGS},
-	{4, 3, 0, COEX_RSRVD2_FLAGS}
-};
-
-static struct coex_event iwm_sta_cm_prio_tbl[COEX_EVENTS_NUM] =
-{
-	{1, 1, 0, COEX_UNASSOC_IDLE_FLAGS},
-	{4, 4, 0, COEX_UNASSOC_MANUAL_SCAN_FLAGS},
-	{3, 3, 0, COEX_UNASSOC_AUTO_SCAN_FLAGS},
-	{6, 6, 0, COEX_CALIBRATION_FLAGS},
-	{3, 3, 0, COEX_PERIODIC_CALIBRATION_FLAGS},
-	{6, 5, 0, COEX_CONNECTION_ESTAB_FLAGS},
-	{4, 4, 0, COEX_ASSOCIATED_IDLE_FLAGS},
-	{4, 4, 0, COEX_ASSOC_MANUAL_SCAN_FLAGS},
-	{4, 4, 0, COEX_ASSOC_AUTO_SCAN_FLAGS},
-	{4, 4, 0, COEX_ASSOC_ACTIVE_LEVEL_FLAGS},
-	{1, 1, 0, COEX_RF_ON_FLAGS},
-	{1, 1, 0, COEX_RF_OFF_FLAGS},
-	{7, 7, 0, COEX_STAND_ALONE_DEBUG_FLAGS},
-	{5, 4, 0, COEX_IPAN_ASSOC_LEVEL_FLAGS},
-	{1, 1, 0, COEX_RSRVD1_FLAGS},
-	{1, 1, 0, COEX_RSRVD2_FLAGS}
-};
-
-int iwm_send_prio_table(struct iwm_priv *iwm)
-{
-	struct iwm_coex_prio_table_cmd coex_table_cmd;
-	u32 coex_enabled, mode_enabled;
-
-	memset(&coex_table_cmd, 0, sizeof(struct iwm_coex_prio_table_cmd));
-
-	coex_table_cmd.flags = COEX_FLAGS_STA_TABLE_VALID_MSK;
-
-	switch (modparam_wiwi) {
-	case COEX_MODE_XOR:
-	case COEX_MODE_CM:
-		coex_enabled = 1;
-		break;
-	default:
-		coex_enabled = 0;
-		break;
-	}
-
-	switch (iwm->conf.mode) {
-	case UMAC_MODE_BSS:
-	case UMAC_MODE_IBSS:
-		mode_enabled = 1;
-		break;
-	default:
-		mode_enabled = 0;
-		break;
-	}
-
-	if (coex_enabled && mode_enabled) {
-		coex_table_cmd.flags |= COEX_FLAGS_COEX_ENABLE_MSK |
-					COEX_FLAGS_ASSOC_WAKEUP_UMASK_MSK |
-					COEX_FLAGS_UNASSOC_WAKEUP_UMASK_MSK;
-
-		switch (modparam_wiwi) {
-		case COEX_MODE_XOR:
-			memcpy(coex_table_cmd.sta_prio, iwm_sta_xor_prio_tbl,
-			       sizeof(iwm_sta_xor_prio_tbl));
-			break;
-		case COEX_MODE_CM:
-			memcpy(coex_table_cmd.sta_prio, iwm_sta_cm_prio_tbl,
-			       sizeof(iwm_sta_cm_prio_tbl));
-			break;
-		default:
-			IWM_ERR(iwm, "Invalid coex_mode 0x%x\n",
-				modparam_wiwi);
-			break;
-		}
-	} else
-		IWM_WARN(iwm, "coexistense disabled\n");
-
-	return iwm_send_lmac_ptrough_cmd(iwm, COEX_PRIORITY_TABLE_CMD,
-				&coex_table_cmd,
-				sizeof(struct iwm_coex_prio_table_cmd), 0);
-}
-
-int iwm_send_init_calib_cfg(struct iwm_priv *iwm, u8 calib_requested)
-{
-	struct iwm_lmac_cal_cfg_cmd cal_cfg_cmd;
-
-	memset(&cal_cfg_cmd, 0, sizeof(struct iwm_lmac_cal_cfg_cmd));
-
-	cal_cfg_cmd.ucode_cfg.init.enable = cpu_to_le32(calib_requested);
-	cal_cfg_cmd.ucode_cfg.init.start = cpu_to_le32(calib_requested);
-	cal_cfg_cmd.ucode_cfg.init.send_res = cpu_to_le32(calib_requested);
-	cal_cfg_cmd.ucode_cfg.flags =
-		cpu_to_le32(CALIB_CFG_FLAG_SEND_COMPLETE_NTFY_AFTER_MSK);
-
-	return iwm_send_lmac_ptrough_cmd(iwm, CALIBRATION_CFG_CMD, &cal_cfg_cmd,
-				sizeof(struct iwm_lmac_cal_cfg_cmd), 1);
-}
-
-int iwm_send_periodic_calib_cfg(struct iwm_priv *iwm, u8 calib_requested)
-{
-	struct iwm_lmac_cal_cfg_cmd cal_cfg_cmd;
-
-	memset(&cal_cfg_cmd, 0, sizeof(struct iwm_lmac_cal_cfg_cmd));
-
-	cal_cfg_cmd.ucode_cfg.periodic.enable = cpu_to_le32(calib_requested);
-	cal_cfg_cmd.ucode_cfg.periodic.start = cpu_to_le32(calib_requested);
-
-	return iwm_send_lmac_ptrough_cmd(iwm, CALIBRATION_CFG_CMD, &cal_cfg_cmd,
-				sizeof(struct iwm_lmac_cal_cfg_cmd), 0);
-}
-
-int iwm_store_rxiq_calib_result(struct iwm_priv *iwm)
-{
-	struct iwm_calib_rxiq *rxiq;
-	u8 *eeprom_rxiq = iwm_eeprom_access(iwm, IWM_EEPROM_CALIB_RXIQ);
-	int grplen = sizeof(struct iwm_calib_rxiq_group);
-
-	rxiq = kzalloc(sizeof(struct iwm_calib_rxiq), GFP_KERNEL);
-	if (!rxiq) {
-		IWM_ERR(iwm, "Couldn't alloc memory for RX IQ\n");
-		return -ENOMEM;
-	}
-
-	eeprom_rxiq = iwm_eeprom_access(iwm, IWM_EEPROM_CALIB_RXIQ);
-	if (IS_ERR(eeprom_rxiq)) {
-		IWM_ERR(iwm, "Couldn't access EEPROM RX IQ entry\n");
-		kfree(rxiq);
-		return PTR_ERR(eeprom_rxiq);
-	}
-
-	iwm->calib_res[SHILOH_PHY_CALIBRATE_RX_IQ_CMD].buf = (u8 *)rxiq;
-	iwm->calib_res[SHILOH_PHY_CALIBRATE_RX_IQ_CMD].size = sizeof(*rxiq);
-
-	rxiq->hdr.opcode = SHILOH_PHY_CALIBRATE_RX_IQ_CMD;
-	rxiq->hdr.first_grp = 0;
-	rxiq->hdr.grp_num = 1;
-	rxiq->hdr.all_data_valid = 1;
-
-	memcpy(&rxiq->group[0], eeprom_rxiq, 4 * grplen);
-	memcpy(&rxiq->group[4], eeprom_rxiq + 6 * grplen, grplen);
-
-	return 0;
-}
-
-int iwm_send_calib_results(struct iwm_priv *iwm)
-{
-	int i, ret = 0;
-
-	for (i = PHY_CALIBRATE_OPCODES_NUM; i < CALIBRATION_CMD_NUM; i++) {
-		if (test_bit(i - PHY_CALIBRATE_OPCODES_NUM,
-			     &iwm->calib_done_map)) {
-			IWM_DBG_CMD(iwm, DBG,
-				    "Send calibration %d result\n", i);
-			ret |= iwm_send_lmac_ptrough_cmd(iwm,
-					REPLY_PHY_CALIBRATION_CMD,
-					iwm->calib_res[i].buf,
-					iwm->calib_res[i].size, 0);
-
-			kfree(iwm->calib_res[i].buf);
-			iwm->calib_res[i].buf = NULL;
-			iwm->calib_res[i].size = 0;
-		}
-	}
-
-	return ret;
-}
-
-int iwm_send_ct_kill_cfg(struct iwm_priv *iwm, u8 entry, u8 exit)
-{
-	struct iwm_ct_kill_cfg_cmd cmd;
-
-	cmd.entry_threshold = entry;
-	cmd.exit_threshold = exit;
-
-	return iwm_send_lmac_ptrough_cmd(iwm, REPLY_CT_KILL_CONFIG_CMD, &cmd,
-					 sizeof(struct iwm_ct_kill_cfg_cmd), 0);
-}
-
-int iwm_send_umac_reset(struct iwm_priv *iwm, __le32 reset_flags, bool resp)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_reset reset;
-
-	reset.flags = reset_flags;
-
-	umac_cmd.id = UMAC_CMD_OPCODE_RESET;
-	umac_cmd.resp = resp;
-
-	return iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, &reset,
-				     sizeof(struct iwm_umac_cmd_reset));
-}
-
-int iwm_umac_set_config_fix(struct iwm_priv *iwm, u16 tbl, u16 key, u32 value)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_set_param_fix param;
-
-	if ((tbl != UMAC_PARAM_TBL_CFG_FIX) &&
-	    (tbl != UMAC_PARAM_TBL_FA_CFG_FIX))
-		return -EINVAL;
-
-	umac_cmd.id = UMAC_CMD_OPCODE_SET_PARAM_FIX;
-	umac_cmd.resp = 0;
-
-	param.tbl = cpu_to_le16(tbl);
-	param.key = cpu_to_le16(key);
-	param.value = cpu_to_le32(value);
-
-	return iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, &param,
-				     sizeof(struct iwm_umac_cmd_set_param_fix));
-}
-
-int iwm_umac_set_config_var(struct iwm_priv *iwm, u16 key,
-			    void *payload, u16 payload_size)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_set_param_var *param_hdr;
-	u8 *param;
-	int ret;
-
-	param = kzalloc(payload_size +
-			sizeof(struct iwm_umac_cmd_set_param_var), GFP_KERNEL);
-	if (!param) {
-		IWM_ERR(iwm, "Couldn't allocate param\n");
-		return -ENOMEM;
-	}
-
-	param_hdr = (struct iwm_umac_cmd_set_param_var *)param;
-
-	umac_cmd.id = UMAC_CMD_OPCODE_SET_PARAM_VAR;
-	umac_cmd.resp = 0;
-
-	param_hdr->tbl = cpu_to_le16(UMAC_PARAM_TBL_CFG_VAR);
-	param_hdr->key = cpu_to_le16(key);
-	param_hdr->len = cpu_to_le16(payload_size);
-	memcpy(param + sizeof(struct iwm_umac_cmd_set_param_var),
-	       payload, payload_size);
-
-	ret = iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, param,
-				    sizeof(struct iwm_umac_cmd_set_param_var) +
-				    payload_size);
-	kfree(param);
-
-	return ret;
-}
-
-int iwm_send_umac_config(struct iwm_priv *iwm, __le32 reset_flags)
-{
-	int ret;
-
-	/* Use UMAC default values */
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_POWER_INDEX, iwm->conf.power_index);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_FA_CFG_FIX,
-				      CFG_FRAG_THRESHOLD,
-				      iwm->conf.frag_threshold);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_RTS_THRESHOLD,
-				      iwm->conf.rts_threshold);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_CTS_TO_SELF, iwm->conf.cts_to_self);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_WIRELESS_MODE,
-				      iwm->conf.wireless_mode);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_COEX_MODE, modparam_wiwi);
-	if (ret < 0)
-		return ret;
-
-	/*
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_ASSOCIATION_TIMEOUT,
-				      iwm->conf.assoc_timeout);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_ROAM_TIMEOUT,
-				      iwm->conf.roam_timeout);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_WIRELESS_MODE,
-				      WIRELESS_MODE_11A | WIRELESS_MODE_11G);
-	if (ret < 0)
-		return ret;
-	*/
-
-	ret = iwm_umac_set_config_var(iwm, CFG_NET_ADDR,
-				      iwm_to_ndev(iwm)->dev_addr, ETH_ALEN);
-	if (ret < 0)
-		return ret;
-
-	/* UMAC PM static configurations */
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_PM_LEGACY_RX_TIMEOUT, 0x12C);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_PM_LEGACY_TX_TIMEOUT, 0x15E);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_PM_CTRL_FLAGS, 0x1);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-				      CFG_PM_KEEP_ALIVE_IN_BEACONS, 0x80);
-	if (ret < 0)
-		return ret;
-
-	/* reset UMAC */
-	ret = iwm_send_umac_reset(iwm, reset_flags, 1);
-	if (ret < 0)
-		return ret;
-
-	ret = iwm_notif_handle(iwm, UMAC_CMD_OPCODE_RESET, IWM_SRC_UMAC,
-			       WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Wait for UMAC RESET timeout\n");
-		return ret;
-	}
-
-	return ret;
-}
-
-int iwm_send_packet(struct iwm_priv *iwm, struct sk_buff *skb, int pool_id)
-{
-	struct iwm_udma_wifi_cmd udma_cmd;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_tx_info *tx_info = skb_to_tx_info(skb);
-
-	udma_cmd.eop = 1; /* always set eop for non-concatenated Tx */
-	udma_cmd.credit_group = pool_id;
-	udma_cmd.ra_tid = tx_info->sta << 4 | tx_info->tid;
-	udma_cmd.lmac_offset = 0;
-
-	umac_cmd.id = REPLY_TX;
-	umac_cmd.color = tx_info->color;
-	umac_cmd.resp = 0;
-
-	return iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd,
-				     skb->data, skb->len);
-}
-
-static int iwm_target_read(struct iwm_priv *iwm, __le32 address,
-			   u8 *response, u32 resp_size)
-{
-	struct iwm_udma_nonwifi_cmd target_cmd;
-	struct iwm_nonwifi_cmd *cmd;
-	u16 seq_num;
-	int ret = 0;
-
-	target_cmd.opcode = UMAC_HDI_OUT_OPCODE_READ;
-	target_cmd.addr = address;
-	target_cmd.op1_sz = cpu_to_le32(resp_size);
-	target_cmd.op2 = 0;
-	target_cmd.handle_by_hw = 0;
-	target_cmd.resp = 1;
-	target_cmd.eop = 1;
-
-	ret = iwm_hal_send_target_cmd(iwm, &target_cmd, NULL);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't send READ command\n");
-		return ret;
-	}
-
-	/* When succeeding, the send_target routine returns the seq number */
-	seq_num = ret;
-
-	ret = wait_event_interruptible_timeout(iwm->nonwifi_queue,
-		(cmd = iwm_get_pending_nonwifi_cmd(iwm, seq_num,
-					  UMAC_HDI_OUT_OPCODE_READ)) != NULL,
-					       2 * HZ);
-
-	if (!ret) {
-		IWM_ERR(iwm, "Didn't receive a target READ answer\n");
-		return ret;
-	}
-
-	memcpy(response, cmd->buf.hdr + sizeof(struct iwm_udma_in_hdr),
-	       resp_size);
-
-	kfree(cmd);
-
-	return 0;
-}
-
-int iwm_read_mac(struct iwm_priv *iwm, u8 *mac)
-{
-	int ret;
-	u8 mac_align[ALIGN(ETH_ALEN, 8)];
-
-	ret = iwm_target_read(iwm, cpu_to_le32(WICO_MAC_ADDRESS_ADDR),
-			      mac_align, sizeof(mac_align));
-	if (ret)
-		return ret;
-
-	if (is_valid_ether_addr(mac_align))
-		memcpy(mac, mac_align, ETH_ALEN);
-	else {
-		IWM_ERR(iwm, "Invalid EEPROM MAC\n");
-		memcpy(mac, iwm->conf.mac_addr, ETH_ALEN);
-		get_random_bytes(&mac[3], 3);
-	}
-
-	return 0;
-}
-
-static int iwm_check_profile(struct iwm_priv *iwm)
-{
-	if (!iwm->umac_profile_active)
-		return -EAGAIN;
-
-	if (iwm->umac_profile->sec.ucast_cipher != UMAC_CIPHER_TYPE_WEP_40 &&
-	    iwm->umac_profile->sec.ucast_cipher != UMAC_CIPHER_TYPE_WEP_104 &&
-	    iwm->umac_profile->sec.ucast_cipher != UMAC_CIPHER_TYPE_TKIP &&
-	    iwm->umac_profile->sec.ucast_cipher != UMAC_CIPHER_TYPE_CCMP) {
-		IWM_ERR(iwm, "Wrong unicast cipher: 0x%x\n",
-			iwm->umac_profile->sec.ucast_cipher);
-		return -EAGAIN;
-	}
-
-	if (iwm->umac_profile->sec.mcast_cipher != UMAC_CIPHER_TYPE_WEP_40 &&
-	    iwm->umac_profile->sec.mcast_cipher != UMAC_CIPHER_TYPE_WEP_104 &&
-	    iwm->umac_profile->sec.mcast_cipher != UMAC_CIPHER_TYPE_TKIP &&
-	    iwm->umac_profile->sec.mcast_cipher != UMAC_CIPHER_TYPE_CCMP) {
-		IWM_ERR(iwm, "Wrong multicast cipher: 0x%x\n",
-			iwm->umac_profile->sec.mcast_cipher);
-		return -EAGAIN;
-	}
-
-	if ((iwm->umac_profile->sec.ucast_cipher == UMAC_CIPHER_TYPE_WEP_40 ||
-	     iwm->umac_profile->sec.ucast_cipher == UMAC_CIPHER_TYPE_WEP_104) &&
-	    (iwm->umac_profile->sec.ucast_cipher !=
-	     iwm->umac_profile->sec.mcast_cipher)) {
-		IWM_ERR(iwm, "Unicast and multicast ciphers differ for WEP\n");
-	}
-
-	return 0;
-}
-
-int iwm_set_tx_key(struct iwm_priv *iwm, u8 key_idx)
-{
-	struct iwm_umac_tx_key_id tx_key_id;
-	int ret;
-
-	ret = iwm_check_profile(iwm);
-	if (ret < 0)
-		return ret;
-
-	/* UMAC only allows to set default key for WEP and auth type is
-	 * NOT 802.1X or RSNA. */
-	if ((iwm->umac_profile->sec.ucast_cipher != UMAC_CIPHER_TYPE_WEP_40 &&
-	     iwm->umac_profile->sec.ucast_cipher != UMAC_CIPHER_TYPE_WEP_104) ||
-	    iwm->umac_profile->sec.auth_type == UMAC_AUTH_TYPE_8021X ||
-	    iwm->umac_profile->sec.auth_type == UMAC_AUTH_TYPE_RSNA_PSK)
-		return 0;
-
-	tx_key_id.hdr.oid = UMAC_WIFI_IF_CMD_GLOBAL_TX_KEY_ID;
-	tx_key_id.hdr.buf_size = cpu_to_le16(sizeof(struct iwm_umac_tx_key_id) -
-					     sizeof(struct iwm_umac_wifi_if));
-
-	tx_key_id.key_idx = key_idx;
-
-	return iwm_send_wifi_if_cmd(iwm, &tx_key_id, sizeof(tx_key_id), 1);
-}
-
-int iwm_set_key(struct iwm_priv *iwm, bool remove, struct iwm_key *key)
-{
-	int ret = 0;
-	u8 cmd[64], *sta_addr, *key_data, key_len;
-	s8 key_idx;
-	u16 cmd_size = 0;
-	struct iwm_umac_key_hdr *key_hdr = &key->hdr;
-	struct iwm_umac_key_wep40 *wep40 = (struct iwm_umac_key_wep40 *)cmd;
-	struct iwm_umac_key_wep104 *wep104 = (struct iwm_umac_key_wep104 *)cmd;
-	struct iwm_umac_key_tkip *tkip = (struct iwm_umac_key_tkip *)cmd;
-	struct iwm_umac_key_ccmp *ccmp = (struct iwm_umac_key_ccmp *)cmd;
-
-	if (!remove) {
-		ret = iwm_check_profile(iwm);
-		if (ret < 0)
-			return ret;
-	}
-
-	sta_addr = key->hdr.mac;
-	key_data = key->key;
-	key_len = key->key_len;
-	key_idx = key->hdr.key_idx;
-
-	if (!remove) {
-		u8 auth_type = iwm->umac_profile->sec.auth_type;
-
-		IWM_DBG_WEXT(iwm, DBG, "key_idx:%d\n", key_idx);
-		IWM_DBG_WEXT(iwm, DBG, "key_len:%d\n", key_len);
-		IWM_DBG_WEXT(iwm, DBG, "MAC:%pM, idx:%d, multicast:%d\n",
-		       key_hdr->mac, key_hdr->key_idx, key_hdr->multicast);
-
-		IWM_DBG_WEXT(iwm, DBG, "profile: mcast:0x%x, ucast:0x%x\n",
-			     iwm->umac_profile->sec.mcast_cipher,
-			     iwm->umac_profile->sec.ucast_cipher);
-		IWM_DBG_WEXT(iwm, DBG, "profile: auth_type:0x%x, flags:0x%x\n",
-			     iwm->umac_profile->sec.auth_type,
-			     iwm->umac_profile->sec.flags);
-
-		switch (key->cipher) {
-		case WLAN_CIPHER_SUITE_WEP40:
-			wep40->hdr.oid = UMAC_WIFI_IF_CMD_ADD_WEP40_KEY;
-			wep40->hdr.buf_size =
-				cpu_to_le16(sizeof(struct iwm_umac_key_wep40) -
-					    sizeof(struct iwm_umac_wifi_if));
-
-			memcpy(&wep40->key_hdr, key_hdr,
-			       sizeof(struct iwm_umac_key_hdr));
-			memcpy(wep40->key, key_data, key_len);
-			wep40->static_key =
-				!!((auth_type != UMAC_AUTH_TYPE_8021X) &&
-				   (auth_type != UMAC_AUTH_TYPE_RSNA_PSK));
-
-			cmd_size = sizeof(struct iwm_umac_key_wep40);
-			break;
-
-		case WLAN_CIPHER_SUITE_WEP104:
-			wep104->hdr.oid = UMAC_WIFI_IF_CMD_ADD_WEP104_KEY;
-			wep104->hdr.buf_size =
-				cpu_to_le16(sizeof(struct iwm_umac_key_wep104) -
-					    sizeof(struct iwm_umac_wifi_if));
-
-			memcpy(&wep104->key_hdr, key_hdr,
-			       sizeof(struct iwm_umac_key_hdr));
-			memcpy(wep104->key, key_data, key_len);
-			wep104->static_key =
-				!!((auth_type != UMAC_AUTH_TYPE_8021X) &&
-				   (auth_type != UMAC_AUTH_TYPE_RSNA_PSK));
-
-			cmd_size = sizeof(struct iwm_umac_key_wep104);
-			break;
-
-		case WLAN_CIPHER_SUITE_CCMP:
-			key_hdr->key_idx++;
-			ccmp->hdr.oid = UMAC_WIFI_IF_CMD_ADD_CCMP_KEY;
-			ccmp->hdr.buf_size =
-				cpu_to_le16(sizeof(struct iwm_umac_key_ccmp) -
-					    sizeof(struct iwm_umac_wifi_if));
-
-			memcpy(&ccmp->key_hdr, key_hdr,
-			       sizeof(struct iwm_umac_key_hdr));
-
-			memcpy(ccmp->key, key_data, key_len);
-
-			if (key->seq_len)
-				memcpy(ccmp->iv_count, key->seq, key->seq_len);
-
-			cmd_size = sizeof(struct iwm_umac_key_ccmp);
-			break;
-
-		case WLAN_CIPHER_SUITE_TKIP:
-			key_hdr->key_idx++;
-			tkip->hdr.oid = UMAC_WIFI_IF_CMD_ADD_TKIP_KEY;
-			tkip->hdr.buf_size =
-				cpu_to_le16(sizeof(struct iwm_umac_key_tkip) -
-					    sizeof(struct iwm_umac_wifi_if));
-
-			memcpy(&tkip->key_hdr, key_hdr,
-			       sizeof(struct iwm_umac_key_hdr));
-
-			memcpy(tkip->tkip_key, key_data, IWM_TKIP_KEY_SIZE);
-			memcpy(tkip->mic_tx_key, key_data + IWM_TKIP_KEY_SIZE,
-			       IWM_TKIP_MIC_SIZE);
-			memcpy(tkip->mic_rx_key,
-			       key_data + IWM_TKIP_KEY_SIZE + IWM_TKIP_MIC_SIZE,
-			       IWM_TKIP_MIC_SIZE);
-
-			if (key->seq_len)
-				memcpy(ccmp->iv_count, key->seq, key->seq_len);
-
-			cmd_size = sizeof(struct iwm_umac_key_tkip);
-			break;
-
-		default:
-			return -ENOTSUPP;
-		}
-
-		if ((key->cipher == WLAN_CIPHER_SUITE_TKIP) ||
-		    (key->cipher == WLAN_CIPHER_SUITE_CCMP))
-			/*
-			 * UGLY_UGLY_UGLY
-			 * Copied HACK from the MWG driver.
-			 * Without it, the key is set before the second
-			 * EAPOL frame is sent, and the latter is thus
-			 * encrypted.
-			 */
-			schedule_timeout_interruptible(usecs_to_jiffies(300));
-
-		ret =  iwm_send_wifi_if_cmd(iwm, cmd, cmd_size, 1);
-	} else {
-		struct iwm_umac_key_remove key_remove;
-
-		IWM_DBG_WEXT(iwm, ERR, "Removing key_idx:%d\n", key_idx);
-
-		key_remove.hdr.oid = UMAC_WIFI_IF_CMD_REMOVE_KEY;
-		key_remove.hdr.buf_size =
-			cpu_to_le16(sizeof(struct iwm_umac_key_remove) -
-				    sizeof(struct iwm_umac_wifi_if));
-		memcpy(&key_remove.key_hdr, key_hdr,
-		       sizeof(struct iwm_umac_key_hdr));
-
-		ret =  iwm_send_wifi_if_cmd(iwm, &key_remove,
-					    sizeof(struct iwm_umac_key_remove),
-					    1);
-		if (ret)
-			return ret;
-
-		iwm->keys[key_idx].key_len = 0;
-	}
-
-	return ret;
-}
-
-
-int iwm_send_mlme_profile(struct iwm_priv *iwm)
-{
-	int ret;
-	struct iwm_umac_profile profile;
-
-	memcpy(&profile, iwm->umac_profile, sizeof(profile));
-
-	profile.hdr.oid = UMAC_WIFI_IF_CMD_SET_PROFILE;
-	profile.hdr.buf_size = cpu_to_le16(sizeof(struct iwm_umac_profile) -
-					   sizeof(struct iwm_umac_wifi_if));
-
-	ret = iwm_send_wifi_if_cmd(iwm, &profile, sizeof(profile), 1);
-	if (ret) {
-		IWM_ERR(iwm, "Send profile command failed\n");
-		return ret;
-	}
-
-	set_bit(IWM_STATUS_SME_CONNECTING, &iwm->status);
-	return 0;
-}
-
-int __iwm_invalidate_mlme_profile(struct iwm_priv *iwm)
-{
-	struct iwm_umac_invalidate_profile invalid;
-
-	invalid.hdr.oid = UMAC_WIFI_IF_CMD_INVALIDATE_PROFILE;
-	invalid.hdr.buf_size =
-		cpu_to_le16(sizeof(struct iwm_umac_invalidate_profile) -
-			    sizeof(struct iwm_umac_wifi_if));
-
-	invalid.reason = WLAN_REASON_UNSPECIFIED;
-
-	return iwm_send_wifi_if_cmd(iwm, &invalid, sizeof(invalid), 1);
-}
-
-int iwm_invalidate_mlme_profile(struct iwm_priv *iwm)
-{
-	int ret;
-
-	ret = __iwm_invalidate_mlme_profile(iwm);
-	if (ret)
-		return ret;
-
-	ret = wait_event_interruptible_timeout(iwm->mlme_queue,
-				(iwm->umac_profile_active == 0), 5 * HZ);
-
-	return ret ? 0 : -EBUSY;
-}
-
-int iwm_tx_power_trigger(struct iwm_priv *iwm)
-{
-	struct iwm_umac_pwr_trigger pwr_trigger;
-
-	pwr_trigger.hdr.oid = UMAC_WIFI_IF_CMD_TX_PWR_TRIGGER;
-	pwr_trigger.hdr.buf_size =
-		cpu_to_le16(sizeof(struct iwm_umac_pwr_trigger) -
-			    sizeof(struct iwm_umac_wifi_if));
-
-
-	return iwm_send_wifi_if_cmd(iwm, &pwr_trigger, sizeof(pwr_trigger), 1);
-}
-
-int iwm_send_umac_stats_req(struct iwm_priv *iwm, u32 flags)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_stats_req stats_req;
-
-	stats_req.flags = cpu_to_le32(flags);
-
-	umac_cmd.id = UMAC_CMD_OPCODE_STATISTIC_REQUEST;
-	umac_cmd.resp = 0;
-
-	return iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, &stats_req,
-				     sizeof(struct iwm_umac_cmd_stats_req));
-}
-
-int iwm_send_umac_channel_list(struct iwm_priv *iwm)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_get_channel_list *ch_list;
-	int size = sizeof(struct iwm_umac_cmd_get_channel_list) +
-		   sizeof(struct iwm_umac_channel_info) * 4;
-	int ret;
-
-	ch_list = kzalloc(size, GFP_KERNEL);
-	if (!ch_list) {
-		IWM_ERR(iwm, "Couldn't allocate channel list cmd\n");
-		return -ENOMEM;
-	}
-
-	ch_list->ch[0].band = UMAC_BAND_2GHZ;
-	ch_list->ch[0].type = UMAC_CHANNEL_WIDTH_20MHZ;
-	ch_list->ch[0].flags = UMAC_CHANNEL_FLAG_VALID;
-
-	ch_list->ch[1].band = UMAC_BAND_5GHZ;
-	ch_list->ch[1].type = UMAC_CHANNEL_WIDTH_20MHZ;
-	ch_list->ch[1].flags = UMAC_CHANNEL_FLAG_VALID;
-
-	ch_list->ch[2].band = UMAC_BAND_2GHZ;
-	ch_list->ch[2].type = UMAC_CHANNEL_WIDTH_20MHZ;
-	ch_list->ch[2].flags = UMAC_CHANNEL_FLAG_VALID | UMAC_CHANNEL_FLAG_IBSS;
-
-	ch_list->ch[3].band = UMAC_BAND_5GHZ;
-	ch_list->ch[3].type = UMAC_CHANNEL_WIDTH_20MHZ;
-	ch_list->ch[3].flags = UMAC_CHANNEL_FLAG_VALID | UMAC_CHANNEL_FLAG_IBSS;
-
-	ch_list->count = cpu_to_le16(4);
-
-	umac_cmd.id = UMAC_CMD_OPCODE_GET_CHAN_INFO_LIST;
-	umac_cmd.resp = 1;
-
-	ret = iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, ch_list, size);
-
-	kfree(ch_list);
-
-	return ret;
-}
-
-int iwm_scan_ssids(struct iwm_priv *iwm, struct cfg80211_ssid *ssids,
-		   int ssid_num)
-{
-	struct iwm_umac_cmd_scan_request req;
-	int i, ret;
-
-	memset(&req, 0, sizeof(struct iwm_umac_cmd_scan_request));
-
-	req.hdr.oid = UMAC_WIFI_IF_CMD_SCAN_REQUEST;
-	req.hdr.buf_size = cpu_to_le16(sizeof(struct iwm_umac_cmd_scan_request)
-				       - sizeof(struct iwm_umac_wifi_if));
-	req.type = UMAC_WIFI_IF_SCAN_TYPE_USER;
-	req.timeout = 2;
-	req.seq_num = iwm->scan_id;
-	req.ssid_num = min(ssid_num, UMAC_WIFI_IF_PROBE_OPTION_MAX);
-
-	for (i = 0; i < req.ssid_num; i++) {
-		memcpy(req.ssids[i].ssid, ssids[i].ssid, ssids[i].ssid_len);
-		req.ssids[i].ssid_len = ssids[i].ssid_len;
-	}
-
-	ret = iwm_send_wifi_if_cmd(iwm, &req, sizeof(req), 0);
-	if (ret) {
-		IWM_ERR(iwm, "Couldn't send scan request\n");
-		return ret;
-	}
-
-	iwm->scan_id = (iwm->scan_id + 1) % IWM_SCAN_ID_MAX;
-
-	return 0;
-}
-
-int iwm_scan_one_ssid(struct iwm_priv *iwm, u8 *ssid, int ssid_len)
-{
-	struct cfg80211_ssid one_ssid;
-
-	if (test_and_set_bit(IWM_STATUS_SCANNING, &iwm->status))
-		return 0;
-
-	one_ssid.ssid_len = min(ssid_len, IEEE80211_MAX_SSID_LEN);
-	memcpy(&one_ssid.ssid, ssid, one_ssid.ssid_len);
-
-	return iwm_scan_ssids(iwm, &one_ssid, 1);
-}
-
-int iwm_target_reset(struct iwm_priv *iwm)
-{
-	struct iwm_udma_nonwifi_cmd target_cmd;
-
-	target_cmd.opcode = UMAC_HDI_OUT_OPCODE_REBOOT;
-	target_cmd.addr = 0;
-	target_cmd.op1_sz = 0;
-	target_cmd.op2 = 0;
-	target_cmd.handle_by_hw = 0;
-	target_cmd.resp = 0;
-	target_cmd.eop = 1;
-
-	return iwm_hal_send_target_cmd(iwm, &target_cmd, NULL);
-}
-
-int iwm_send_umac_stop_resume_tx(struct iwm_priv *iwm,
-				 struct iwm_umac_notif_stop_resume_tx *ntf)
-{
-	struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_stop_resume_tx stp_res_cmd;
-	struct iwm_sta_info *sta_info;
-	u8 sta_id = STA_ID_N_COLOR_ID(ntf->sta_id);
-	int i;
-
-	sta_info = &iwm->sta_table[sta_id];
-	if (!sta_info->valid) {
-		IWM_ERR(iwm, "Invalid STA: %d\n", sta_id);
-		return -EINVAL;
-	}
-
-	umac_cmd.id = UMAC_CMD_OPCODE_STOP_RESUME_STA_TX;
-	umac_cmd.resp = 0;
-
-	stp_res_cmd.flags = ntf->flags;
-	stp_res_cmd.sta_id = ntf->sta_id;
-	stp_res_cmd.stop_resume_tid_msk = ntf->stop_resume_tid_msk;
-	for (i = 0; i < IWM_UMAC_TID_NR; i++)
-		stp_res_cmd.last_seq_num[i] =
-			sta_info->tid_info[i].last_seq_num;
-
-	return iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, &stp_res_cmd,
-				 sizeof(struct iwm_umac_cmd_stop_resume_tx));
-
-}
-
-int iwm_send_pmkid_update(struct iwm_priv *iwm,
-			  struct cfg80211_pmksa *pmksa, u32 command)
-{
-	struct iwm_umac_pmkid_update update;
-	int ret;
-
-	memset(&update, 0, sizeof(struct iwm_umac_pmkid_update));
-
-	update.hdr.oid = UMAC_WIFI_IF_CMD_PMKID_UPDATE;
-	update.hdr.buf_size = cpu_to_le16(sizeof(struct iwm_umac_pmkid_update) -
-					  sizeof(struct iwm_umac_wifi_if));
-
-	update.command = cpu_to_le32(command);
-	if (pmksa->bssid)
-		memcpy(&update.bssid, pmksa->bssid, ETH_ALEN);
-	if (pmksa->pmkid)
-		memcpy(&update.pmkid, pmksa->pmkid, WLAN_PMKID_LEN);
-
-	ret = iwm_send_wifi_if_cmd(iwm, &update,
-				   sizeof(struct iwm_umac_pmkid_update), 0);
-	if (ret) {
-		IWM_ERR(iwm, "PMKID update command failed\n");
-		return ret;
-	}
-
-	return 0;
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/commands.h b/drivers/net/wireless/iwmc3200wifi/commands.h
deleted file mode 100644
index 6421689f5e8..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/commands.h
+++ /dev/null
@@ -1,509 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_COMMANDS_H__
-#define __IWM_COMMANDS_H__
-
-#include <linux/ieee80211.h>
-
-#define IWM_BARKER_REBOOT_NOTIFICATION	0xF
-#define IWM_ACK_BARKER_NOTIFICATION	0x10
-
-/* UMAC commands */
-#define UMAC_RST_CTRL_FLG_LARC_CLK_EN	0x0001
-#define UMAC_RST_CTRL_FLG_LARC_RESET	0x0002
-#define UMAC_RST_CTRL_FLG_FUNC_RESET	0x0004
-#define UMAC_RST_CTRL_FLG_DEV_RESET	0x0008
-#define UMAC_RST_CTRL_FLG_WIFI_CORE_EN	0x0010
-#define UMAC_RST_CTRL_FLG_WIFI_LINK_EN	0x0040
-#define UMAC_RST_CTRL_FLG_WIFI_MLME_EN	0x0080
-#define UMAC_RST_CTRL_FLG_NVM_RELOAD	0x0100
-
-struct iwm_umac_cmd_reset {
-	__le32 flags;
-} __packed;
-
-#define UMAC_PARAM_TBL_ORD_FIX    0x0
-#define UMAC_PARAM_TBL_ORD_VAR    0x1
-#define UMAC_PARAM_TBL_CFG_FIX    0x2
-#define UMAC_PARAM_TBL_CFG_VAR    0x3
-#define UMAC_PARAM_TBL_BSS_TRK    0x4
-#define UMAC_PARAM_TBL_FA_CFG_FIX 0x5
-#define UMAC_PARAM_TBL_STA        0x6
-#define UMAC_PARAM_TBL_CHN        0x7
-#define UMAC_PARAM_TBL_STATISTICS 0x8
-
-/* fast access table */
-enum {
-	CFG_FRAG_THRESHOLD = 0,
-	CFG_FRAME_RETRY_LIMIT,
-	CFG_OS_QUEUE_UTIL_TH,
-	CFG_RX_FILTER,
-	/* <-- LAST --> */
-	FAST_ACCESS_CFG_TBL_FIX_LAST
-};
-
-/* fixed size table */
-enum {
-	CFG_POWER_INDEX = 0,
-	CFG_PM_LEGACY_RX_TIMEOUT,
-	CFG_PM_LEGACY_TX_TIMEOUT,
-	CFG_PM_CTRL_FLAGS,
-	CFG_PM_KEEP_ALIVE_IN_BEACONS,
-	CFG_BT_ON_THRESHOLD,
-	CFG_RTS_THRESHOLD,
-	CFG_CTS_TO_SELF,
-	CFG_COEX_MODE,
-	CFG_WIRELESS_MODE,
-	CFG_ASSOCIATION_TIMEOUT,
-	CFG_ROAM_TIMEOUT,
-	CFG_CAPABILITY_SUPPORTED_RATES,
-	CFG_SCAN_ALLOWED_UNASSOC_FLAGS,
-	CFG_SCAN_ALLOWED_MAIN_ASSOC_FLAGS,
-	CFG_SCAN_ALLOWED_PAN_ASSOC_FLAGS,
-	CFG_SCAN_INTERNAL_PERIODIC_ENABLED,
-	CFG_SCAN_IMM_INTERNAL_PERIODIC_SCAN_ON_INIT,
-	CFG_SCAN_DEFAULT_PERIODIC_FREQ_SEC,
-	CFG_SCAN_NUM_PASSIVE_CHAN_PER_PARTIAL_SCAN,
-	CFG_TLC_SUPPORTED_TX_HT_RATES,
-	CFG_TLC_SUPPORTED_TX_RATES,
-	CFG_TLC_SPATIAL_STREAM_SUPPORTED,
-	CFG_TLC_RETRY_PER_RATE,
-	CFG_TLC_RETRY_PER_HT_RATE,
-	CFG_TLC_FIXED_MCS,
-	CFG_TLC_CONTROL_FLAGS,
-	CFG_TLC_SR_MIN_FAIL,
-	CFG_TLC_SR_MIN_PASS,
-	CFG_TLC_HT_STAY_IN_COL_PASS_THRESH,
-	CFG_TLC_HT_STAY_IN_COL_FAIL_THRESH,
-	CFG_TLC_LEGACY_STAY_IN_COL_PASS_THRESH,
-	CFG_TLC_LEGACY_STAY_IN_COL_FAIL_THRESH,
-	CFG_TLC_HT_FLUSH_STATS_PACKETS,
-	CFG_TLC_LEGACY_FLUSH_STATS_PACKETS,
-	CFG_TLC_LEGACY_FLUSH_STATS_MS,
-	CFG_TLC_HT_FLUSH_STATS_MS,
-	CFG_TLC_STAY_IN_COL_TIME_OUT,
-	CFG_TLC_AGG_SHORT_LIM,
-	CFG_TLC_AGG_LONG_LIM,
-	CFG_TLC_HT_SR_NO_DECREASE,
-	CFG_TLC_LEGACY_SR_NO_DECREASE,
-	CFG_TLC_SR_FORCE_DECREASE,
-	CFG_TLC_SR_ALLOW_INCREASE,
-	CFG_TLC_AGG_SET_LONG,
-	CFG_TLC_AUTO_AGGREGATION,
-	CFG_TLC_AGG_THRESHOLD,
-	CFG_TLC_TID_LOAD_THRESHOLD,
-	CFG_TLC_BLOCK_ACK_TIMEOUT,
-	CFG_TLC_NO_BA_COUNTED_AS_ONE,
-	CFG_TLC_NUM_BA_STREAMS_ALLOWED,
-	CFG_TLC_NUM_BA_STREAMS_PRESENT,
-	CFG_TLC_RENEW_ADDBA_DELAY,
-	CFG_TLC_NUM_OF_MULTISEC_TO_COUN_LOAD,
-	CFG_TLC_IS_STABLE_IN_HT,
-	CFG_TLC_SR_SIC_1ST_FAIL,
-	CFG_TLC_SR_SIC_1ST_PASS,
-	CFG_TLC_SR_SIC_TOTAL_FAIL,
-	CFG_TLC_SR_SIC_TOTAL_PASS,
-	CFG_RLC_CHAIN_CTRL,
-	CFG_TRK_TABLE_OP_MODE,
-	CFG_TRK_TABLE_RSSI_THRESHOLD,
-	CFG_TX_PWR_TARGET, /* Used By xVT */
-	CFG_TX_PWR_LIMIT_USR,
-	CFG_TX_PWR_LIMIT_BSS, /* 11d limit */
-	CFG_TX_PWR_LIMIT_BSS_CONSTRAINT, /* 11h constraint */
-	CFG_TX_PWR_MODE,
-	CFG_MLME_DBG_NOTIF_BLOCK,
-	CFG_BT_OFF_BECONS_INTERVALS,
-	CFG_BT_FRAG_DURATION,
-	CFG_ACTIVE_CHAINS,
-	CFG_CALIB_CTRL,
-	CFG_CAPABILITY_SUPPORTED_HT_RATES,
-	CFG_HT_MAC_PARAM_INFO,
-	CFG_MIMO_PS_MODE,
-	CFG_HT_DEFAULT_CAPABILIES_INFO,
-	CFG_LED_SC_RESOLUTION_FACTOR,
-	CFG_PTAM_ENERGY_CCK_DET_DEFAULT,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_MRC_DEFAULT,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_DEFAULT,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_MRC_DEFAULT,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_DEFAULT,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_MRC_DEFAULT,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_DEFAULT,
-	CFG_PTAM_ENERGY_CCK_DET_MIN_VAL,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_MRC_MIN_VAL,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_MIN_VAL,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_MRC_MIN_VAL,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_MIN_VAL,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_MRC_MIN_VAL,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_MIN_VAL,
-	CFG_PTAM_ENERGY_CCK_DET_MAX_VAL,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_MRC_MAX_VAL,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_MAX_VAL,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_MRC_MAX_VAL,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_MAX_VAL,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_MRC_MAX_VAL,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_MAX_VAL,
-	CFG_PTAM_ENERGY_CCK_DET_STEP_VAL,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_MRC_STEP_VAL,
-	CFG_PTAM_CORR40_4_TH_ADD_MIN_STEP_VAL,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_MRC_STEP_VAL,
-	CFG_PTAM_CORR32_4_TH_ADD_MIN_STEP_VAL,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_MRC_STEP_VAL,
-	CFG_PTAM_CORR32_1_TH_ADD_MIN_STEP_VAL,
-	CFG_PTAM_LINK_SENS_FA_OFDM_MAX,
-	CFG_PTAM_LINK_SENS_FA_OFDM_MIN,
-	CFG_PTAM_LINK_SENS_FA_CCK_MAX,
-	CFG_PTAM_LINK_SENS_FA_CCK_MIN,
-	CFG_PTAM_LINK_SENS_NRG_DIFF,
-	CFG_PTAM_LINK_SENS_NRG_MARGIN,
-	CFG_PTAM_LINK_SENS_MAX_NUMBER_OF_TIMES_IN_CCK_NO_FA,
-	CFG_PTAM_LINK_SENS_AUTO_CORR_MAX_TH_CCK,
-	CFG_AGG_MGG_TID_LOAD_ADDBA_THRESHOLD,
-	CFG_AGG_MGG_TID_LOAD_DELBA_THRESHOLD,
-	CFG_AGG_MGG_ADDBA_BUF_SIZE,
-	CFG_AGG_MGG_ADDBA_INACTIVE_TIMEOUT,
-	CFG_AGG_MGG_ADDBA_DEBUG_FLAGS,
-	CFG_SCAN_PERIODIC_RSSI_HIGH_THRESHOLD,
-	CFG_SCAN_PERIODIC_COEF_RSSI_HIGH,
-	CFG_11D_ENABLED,
-	CFG_11H_FEATURE_FLAGS,
-
-	/* <-- LAST --> */
-	CFG_TBL_FIX_LAST
-};
-
-/* variable size table */
-enum {
-	CFG_NET_ADDR = 0,
-	CFG_LED_PATTERN_TABLE,
-
-	/* <-- LAST --> */
-	CFG_TBL_VAR_LAST
-};
-
-struct iwm_umac_cmd_set_param_fix {
-	__le16 tbl;
-	__le16 key;
-	__le32 value;
-} __packed;
-
-struct iwm_umac_cmd_set_param_var {
-	__le16 tbl;
-	__le16 key;
-	__le16 len;
-	__le16 reserved;
-} __packed;
-
-struct iwm_umac_cmd_get_param {
-	__le16 tbl;
-	__le16 key;
-} __packed;
-
-struct iwm_umac_cmd_get_param_resp {
-	__le16 tbl;
-	__le16 key;
-	__le16 len;
-	__le16 reserved;
-} __packed;
-
-struct iwm_umac_cmd_eeprom_proxy_hdr {
-	__le32 type;
-	__le32 offset;
-	__le32 len;
-} __packed;
-
-struct iwm_umac_cmd_eeprom_proxy {
-	struct iwm_umac_cmd_eeprom_proxy_hdr hdr;
-	u8 buf[0];
-} __packed;
-
-#define IWM_UMAC_CMD_EEPROM_TYPE_READ       0x1
-#define IWM_UMAC_CMD_EEPROM_TYPE_WRITE      0x2
-
-#define UMAC_CHANNEL_FLAG_VALID		BIT(0)
-#define UMAC_CHANNEL_FLAG_IBSS		BIT(1)
-#define UMAC_CHANNEL_FLAG_ACTIVE	BIT(3)
-#define UMAC_CHANNEL_FLAG_RADAR		BIT(4)
-#define UMAC_CHANNEL_FLAG_DFS		BIT(7)
-
-struct iwm_umac_channel_info {
-	u8 band;
-	u8 type;
-	u8 reserved;
-	u8 flags;
-	__le32 channels_mask;
-} __packed;
-
-struct iwm_umac_cmd_get_channel_list {
-	__le16 count;
-	__le16 reserved;
-	struct iwm_umac_channel_info ch[0];
-} __packed;
-
-
-/* UMAC WiFi interface commands */
-
-/* Coexistence mode */
-#define COEX_MODE_SA  0x1
-#define COEX_MODE_XOR 0x2
-#define COEX_MODE_CM  0x3
-#define COEX_MODE_MAX 0x4
-
-/* Wireless mode */
-#define WIRELESS_MODE_11A  0x1
-#define WIRELESS_MODE_11G  0x2
-#define WIRELESS_MODE_11N  0x4
-
-#define UMAC_PROFILE_EX_IE_REQUIRED	0x1
-#define UMAC_PROFILE_QOS_ALLOWED	0x2
-
-/* Scanning */
-#define UMAC_WIFI_IF_PROBE_OPTION_MAX        10
-
-#define UMAC_WIFI_IF_SCAN_TYPE_USER          0x0
-#define UMAC_WIFI_IF_SCAN_TYPE_UMAC_RESERVED 0x1
-#define UMAC_WIFI_IF_SCAN_TYPE_HOST_PERIODIC 0x2
-#define UMAC_WIFI_IF_SCAN_TYPE_MAX           0x3
-
-struct iwm_umac_ssid {
-	u8 ssid_len;
-	u8 ssid[IEEE80211_MAX_SSID_LEN];
-	u8 reserved[3];
-} __packed;
-
-struct iwm_umac_cmd_scan_request {
-	struct iwm_umac_wifi_if hdr;
-	__le32 type; /* UMAC_WIFI_IF_SCAN_TYPE_* */
-	u8 ssid_num;
-	u8 seq_num;
-	u8 timeout; /* In seconds */
-	u8 reserved;
-	struct iwm_umac_ssid ssids[UMAC_WIFI_IF_PROBE_OPTION_MAX];
-} __packed;
-
-#define UMAC_CIPHER_TYPE_NONE		0xFF
-#define UMAC_CIPHER_TYPE_USE_GROUPCAST	0x00
-#define UMAC_CIPHER_TYPE_WEP_40		0x01
-#define UMAC_CIPHER_TYPE_WEP_104	0x02
-#define UMAC_CIPHER_TYPE_TKIP		0x04
-#define UMAC_CIPHER_TYPE_CCMP		0x08
-
-/* Supported authentication types - bitmap */
-#define UMAC_AUTH_TYPE_OPEN		0x00
-#define UMAC_AUTH_TYPE_LEGACY_PSK	0x01
-#define UMAC_AUTH_TYPE_8021X		0x02
-#define UMAC_AUTH_TYPE_RSNA_PSK		0x04
-
-/* iwm_umac_security.flag is WPA supported -- bits[0:0] */
-#define UMAC_SEC_FLG_WPA_ON_POS		0
-#define UMAC_SEC_FLG_WPA_ON_SEED	1
-#define UMAC_SEC_FLG_WPA_ON_MSK (UMAC_SEC_FLG_WPA_ON_SEED << \
-				 UMAC_SEC_FLG_WPA_ON_POS)
-
-/* iwm_umac_security.flag is WPA2 supported -- bits [1:1] */
-#define UMAC_SEC_FLG_RSNA_ON_POS	1
-#define UMAC_SEC_FLG_RSNA_ON_SEED	1
-#define UMAC_SEC_FLG_RSNA_ON_MSK        (UMAC_SEC_FLG_RSNA_ON_SEED << \
-					 UMAC_SEC_FLG_RSNA_ON_POS)
-
-/* iwm_umac_security.flag is WSC mode on -- bits [2:2] */
-#define UMAC_SEC_FLG_WSC_ON_POS		2
-#define UMAC_SEC_FLG_WSC_ON_SEED	1
-#define UMAC_SEC_FLG_WSC_ON_MSK         (UMAC_SEC_FLG_WSC_ON_SEED << \
-					 UMAC_SEC_FLG_WSC_ON_POS)
-
-
-/* Legacy profile can use only WEP40 and WEP104 for encryption and
- * OPEN or PSK for authentication */
-#define UMAC_SEC_FLG_LEGACY_PROFILE	0
-
-struct iwm_umac_security {
-	u8 auth_type;
-	u8 ucast_cipher;
-	u8 mcast_cipher;
-	u8 flags;
-} __packed;
-
-struct iwm_umac_ibss {
-	u8 beacon_interval;	/* in millisecond */
-	u8 atim;		/* in millisecond */
-	s8 join_only;
-	u8 band;
-	u8 channel;
-	u8 reserved[3];
-} __packed;
-
-#define UMAC_MODE_BSS	0
-#define UMAC_MODE_IBSS	1
-
-#define UMAC_BSSID_MAX	4
-
-struct iwm_umac_profile {
-	struct iwm_umac_wifi_if hdr;
-	__le32 mode;
-	struct iwm_umac_ssid ssid;
-	u8 bssid[UMAC_BSSID_MAX][ETH_ALEN];
-	struct iwm_umac_security sec;
-	struct iwm_umac_ibss ibss;
-	__le32 channel_2ghz;
-	__le32 channel_5ghz;
-	__le16 flags;
-	u8 wireless_mode;
-	u8 bss_num;
-} __packed;
-
-struct iwm_umac_invalidate_profile {
-	struct iwm_umac_wifi_if hdr;
-	u8 reason;
-	u8 reserved[3];
-} __packed;
-
-/* Encryption key commands */
-struct iwm_umac_key_wep40 {
-	struct iwm_umac_wifi_if hdr;
-	struct iwm_umac_key_hdr key_hdr;
-	u8 key[WLAN_KEY_LEN_WEP40];
-	u8 static_key;
-	u8 reserved[2];
-} __packed;
-
-struct iwm_umac_key_wep104 {
-	struct iwm_umac_wifi_if hdr;
-	struct iwm_umac_key_hdr key_hdr;
-	u8 key[WLAN_KEY_LEN_WEP104];
-	u8 static_key;
-	u8 reserved[2];
-} __packed;
-
-#define IWM_TKIP_KEY_SIZE 16
-#define IWM_TKIP_MIC_SIZE 8
-struct iwm_umac_key_tkip {
-	struct iwm_umac_wifi_if hdr;
-	struct iwm_umac_key_hdr key_hdr;
-	u8 iv_count[6];
-	u8 reserved[2];
-	u8 tkip_key[IWM_TKIP_KEY_SIZE];
-	u8 mic_rx_key[IWM_TKIP_MIC_SIZE];
-	u8 mic_tx_key[IWM_TKIP_MIC_SIZE];
-} __packed;
-
-struct iwm_umac_key_ccmp {
-	struct iwm_umac_wifi_if hdr;
-	struct iwm_umac_key_hdr key_hdr;
-	u8 iv_count[6];
-	u8 reserved[2];
-	u8 key[WLAN_KEY_LEN_CCMP];
-} __packed;
-
-struct iwm_umac_key_remove {
-	struct iwm_umac_wifi_if hdr;
-	struct iwm_umac_key_hdr key_hdr;
-} __packed;
-
-struct iwm_umac_tx_key_id {
-	struct iwm_umac_wifi_if hdr;
-	u8 key_idx;
-	u8 reserved[3];
-} __packed;
-
-struct iwm_umac_pwr_trigger {
-	struct iwm_umac_wifi_if hdr;
-	__le32 reseved;
-} __packed;
-
-struct iwm_umac_cmd_stats_req {
-	__le32 flags;
-} __packed;
-
-struct iwm_umac_cmd_stop_resume_tx {
-	u8 flags;
-	u8 sta_id;
-	__le16 stop_resume_tid_msk;
-	__le16 last_seq_num[IWM_UMAC_TID_NR];
-	u16 reserved;
-} __packed;
-
-#define IWM_CMD_PMKID_ADD   1
-#define IWM_CMD_PMKID_DEL   2
-#define IWM_CMD_PMKID_FLUSH 3
-
-struct iwm_umac_pmkid_update {
-	struct iwm_umac_wifi_if hdr;
-	__le32 command;
-	u8 bssid[ETH_ALEN];
-	__le16 reserved;
-	u8 pmkid[WLAN_PMKID_LEN];
-} __packed;
-
-/* LMAC commands */
-int iwm_read_mac(struct iwm_priv *iwm, u8 *mac);
-int iwm_send_prio_table(struct iwm_priv *iwm);
-int iwm_send_init_calib_cfg(struct iwm_priv *iwm, u8 calib_requested);
-int iwm_send_periodic_calib_cfg(struct iwm_priv *iwm, u8 calib_requested);
-int iwm_send_calib_results(struct iwm_priv *iwm);
-int iwm_store_rxiq_calib_result(struct iwm_priv *iwm);
-int iwm_send_ct_kill_cfg(struct iwm_priv *iwm, u8 entry, u8 exit);
-
-/* UMAC commands */
-int iwm_send_wifi_if_cmd(struct iwm_priv *iwm, void *payload, u16 payload_size,
-			 bool resp);
-int iwm_send_umac_reset(struct iwm_priv *iwm, __le32 reset_flags, bool resp);
-int iwm_umac_set_config_fix(struct iwm_priv *iwm, u16 tbl, u16 key, u32 value);
-int iwm_umac_set_config_var(struct iwm_priv *iwm, u16 key,
-			    void *payload, u16 payload_size);
-int iwm_send_umac_config(struct iwm_priv *iwm, __le32 reset_flags);
-int iwm_send_mlme_profile(struct iwm_priv *iwm);
-int __iwm_invalidate_mlme_profile(struct iwm_priv *iwm);
-int iwm_invalidate_mlme_profile(struct iwm_priv *iwm);
-int iwm_send_packet(struct iwm_priv *iwm, struct sk_buff *skb, int pool_id);
-int iwm_set_tx_key(struct iwm_priv *iwm, u8 key_idx);
-int iwm_set_key(struct iwm_priv *iwm, bool remove, struct iwm_key *key);
-int iwm_tx_power_trigger(struct iwm_priv *iwm);
-int iwm_send_umac_stats_req(struct iwm_priv *iwm, u32 flags);
-int iwm_send_umac_channel_list(struct iwm_priv *iwm);
-int iwm_scan_ssids(struct iwm_priv *iwm, struct cfg80211_ssid *ssids,
-		   int ssid_num);
-int iwm_scan_one_ssid(struct iwm_priv *iwm, u8 *ssid, int ssid_len);
-int iwm_send_umac_stop_resume_tx(struct iwm_priv *iwm,
-				 struct iwm_umac_notif_stop_resume_tx *ntf);
-int iwm_send_pmkid_update(struct iwm_priv *iwm,
-			  struct cfg80211_pmksa *pmksa, u32 command);
-
-/* UDMA commands */
-int iwm_target_reset(struct iwm_priv *iwm);
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/debug.h b/drivers/net/wireless/iwmc3200wifi/debug.h
deleted file mode 100644
index a0c13a49ab3..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/debug.h
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- * 02110-1301, USA.
- *
- */
-
-#ifndef __IWM_DEBUG_H__
-#define __IWM_DEBUG_H__
-
-#define IWM_ERR(p, f, a...) dev_err(iwm_to_dev(p), f, ## a)
-#define IWM_WARN(p, f, a...) dev_warn(iwm_to_dev(p), f, ## a)
-#define IWM_INFO(p, f, a...) dev_info(iwm_to_dev(p), f, ## a)
-#define IWM_CRIT(p, f, a...) dev_crit(iwm_to_dev(p), f, ## a)
-
-#ifdef CONFIG_IWM_DEBUG
-
-#define IWM_DEBUG_MODULE(i, level, module, f, a...)			     \
-do {									     \
-	if (unlikely(i->dbg.dbg_module[IWM_DM_##module] >= (IWM_DL_##level)))\
-		dev_printk(KERN_INFO, (iwm_to_dev(i)),	             \
-			   "%s " f, __func__ , ## a);			     \
-} while (0)
-
-#define IWM_HEXDUMP(i, level, module, pref, buf, len)		             \
-do {									     \
-	if (unlikely(i->dbg.dbg_module[IWM_DM_##module] >= (IWM_DL_##level)))\
-		print_hex_dump(KERN_INFO, pref, DUMP_PREFIX_OFFSET,	     \
-			       16, 1, buf, len, 1);			     \
-} while (0)
-
-#else
-
-#define IWM_DEBUG_MODULE(i, level, module, f, a...)
-#define IWM_HEXDUMP(i, level, module, pref, buf, len)
-
-#endif /* CONFIG_IWM_DEBUG */
-
-/* Debug modules */
-enum iwm_debug_module_id {
-	IWM_DM_BOOT = 0,
-	IWM_DM_FW,
-	IWM_DM_SDIO,
-	IWM_DM_NTF,
-	IWM_DM_RX,
-	IWM_DM_TX,
-	IWM_DM_MLME,
-	IWM_DM_CMD,
-	IWM_DM_WEXT,
-	__IWM_DM_NR,
-};
-#define IWM_DM_DEFAULT 0
-
-#define IWM_DBG_BOOT(i, l, f, a...) IWM_DEBUG_MODULE(i, l, BOOT, f, ## a)
-#define IWM_DBG_FW(i, l, f, a...)   IWM_DEBUG_MODULE(i, l, FW, f, ## a)
-#define IWM_DBG_SDIO(i, l, f, a...) IWM_DEBUG_MODULE(i, l, SDIO, f, ## a)
-#define IWM_DBG_NTF(i, l, f, a...)  IWM_DEBUG_MODULE(i, l, NTF, f, ## a)
-#define IWM_DBG_RX(i, l, f, a...)   IWM_DEBUG_MODULE(i, l, RX, f, ## a)
-#define IWM_DBG_TX(i, l, f, a...)   IWM_DEBUG_MODULE(i, l, TX, f, ## a)
-#define IWM_DBG_MLME(i, l, f, a...) IWM_DEBUG_MODULE(i, l, MLME, f, ## a)
-#define IWM_DBG_CMD(i, l, f, a...)  IWM_DEBUG_MODULE(i, l, CMD, f, ## a)
-#define IWM_DBG_WEXT(i, l, f, a...) IWM_DEBUG_MODULE(i, l, WEXT, f, ## a)
-
-/* Debug levels */
-enum iwm_debug_level {
-	IWM_DL_NONE = 0,
-	IWM_DL_ERR,
-	IWM_DL_WARN,
-	IWM_DL_INFO,
-	IWM_DL_DBG,
-};
-#define IWM_DL_DEFAULT IWM_DL_ERR
-
-struct iwm_debugfs {
-	struct iwm_priv *iwm;
-	struct dentry *rootdir;
-	struct dentry *devdir;
-	struct dentry *dbgdir;
-	struct dentry *txdir;
-	struct dentry *rxdir;
-	struct dentry *busdir;
-
-	u32 dbg_level;
-	struct dentry *dbg_level_dentry;
-
-	unsigned long dbg_modules;
-	struct dentry *dbg_modules_dentry;
-
-	u8 dbg_module[__IWM_DM_NR];
-	struct dentry *dbg_module_dentries[__IWM_DM_NR];
-
-	struct dentry *txq_dentry;
-	struct dentry *tx_credit_dentry;
-	struct dentry *rx_ticket_dentry;
-
-	struct dentry *fw_err_dentry;
-};
-
-#ifdef CONFIG_IWM_DEBUG
-void iwm_debugfs_init(struct iwm_priv *iwm);
-void iwm_debugfs_exit(struct iwm_priv *iwm);
-#else
-static inline void iwm_debugfs_init(struct iwm_priv *iwm) {}
-static inline void iwm_debugfs_exit(struct iwm_priv *iwm) {}
-#endif
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/debugfs.c b/drivers/net/wireless/iwmc3200wifi/debugfs.c
deleted file mode 100644
index b6199d124bb..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/debugfs.c
+++ /dev/null
@@ -1,488 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- * 02110-1301, USA.
- *
- */
-
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/bitops.h>
-#include <linux/debugfs.h>
-#include <linux/export.h>
-
-#include "iwm.h"
-#include "bus.h"
-#include "rx.h"
-#include "debug.h"
-
-static struct {
-	u8 id;
-	char *name;
-} iwm_debug_module[__IWM_DM_NR] = {
-	 {IWM_DM_BOOT, "boot"},
-	 {IWM_DM_FW,   "fw"},
-	 {IWM_DM_SDIO, "sdio"},
-	 {IWM_DM_NTF,  "ntf"},
-	 {IWM_DM_RX,   "rx"},
-	 {IWM_DM_TX,   "tx"},
-	 {IWM_DM_MLME, "mlme"},
-	 {IWM_DM_CMD,  "cmd"},
-	 {IWM_DM_WEXT,  "wext"},
-};
-
-#define add_dbg_module(dbg, name, id, initlevel) 	\
-do {							\
-	dbg.dbg_module[id] = (initlevel);		\
-	dbg.dbg_module_dentries[id] =			\
-		debugfs_create_x8(name, 0600,		\
-				dbg.dbgdir,		\
-				&(dbg.dbg_module[id]));	\
-} while (0)
-
-static int iwm_debugfs_u32_read(void *data, u64 *val)
-{
-	struct iwm_priv *iwm = data;
-
-	*val = iwm->dbg.dbg_level;
-	return 0;
-}
-
-static int iwm_debugfs_dbg_level_write(void *data, u64 val)
-{
-	struct iwm_priv *iwm = data;
-	int i;
-
-	iwm->dbg.dbg_level = val;
-
-	for (i = 0; i < __IWM_DM_NR; i++)
-		iwm->dbg.dbg_module[i] = val;
-
-	return 0;
-}
-DEFINE_SIMPLE_ATTRIBUTE(fops_iwm_dbg_level,
-			iwm_debugfs_u32_read, iwm_debugfs_dbg_level_write,
-			"%llu\n");
-
-static int iwm_debugfs_dbg_modules_write(void *data, u64 val)
-{
-	struct iwm_priv *iwm = data;
-	int i, bit;
-
-	iwm->dbg.dbg_modules = val;
-
-	for (i = 0; i < __IWM_DM_NR; i++)
-		iwm->dbg.dbg_module[i] = 0;
-
-	for_each_set_bit(bit, &iwm->dbg.dbg_modules, __IWM_DM_NR)
-		iwm->dbg.dbg_module[bit] = iwm->dbg.dbg_level;
-
-	return 0;
-}
-DEFINE_SIMPLE_ATTRIBUTE(fops_iwm_dbg_modules,
-			iwm_debugfs_u32_read, iwm_debugfs_dbg_modules_write,
-			"%llu\n");
-
-
-static ssize_t iwm_debugfs_txq_read(struct file *filp, char __user *buffer,
-				   size_t count, loff_t *ppos)
-{
-	struct iwm_priv *iwm = filp->private_data;
-	char *buf;
-	int i, buf_len = 4096;
-	size_t len = 0;
-	ssize_t ret;
-
-	if (*ppos != 0)
-		return 0;
-	if (count < sizeof(buf))
-		return -ENOSPC;
-
-	buf = kzalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	for (i = 0; i < IWM_TX_QUEUES; i++) {
-		struct iwm_tx_queue *txq = &iwm->txq[i];
-		struct sk_buff *skb;
-		int j;
-		unsigned long flags;
-
-		spin_lock_irqsave(&txq->queue.lock, flags);
-
-		skb = (struct sk_buff *)&txq->queue;
-
-		len += snprintf(buf + len, buf_len - len, "TXQ #%d\n", i);
-		len += snprintf(buf + len, buf_len - len, "\tStopped:     %d\n",
-				__netif_subqueue_stopped(iwm_to_ndev(iwm),
-							 txq->id));
-		len += snprintf(buf + len, buf_len - len, "\tConcat count:%d\n",
-				txq->concat_count);
-		len += snprintf(buf + len, buf_len - len, "\tQueue len:   %d\n",
-				skb_queue_len(&txq->queue));
-		for (j = 0; j < skb_queue_len(&txq->queue); j++) {
-			struct iwm_tx_info *tx_info;
-
-			skb = skb->next;
-			tx_info = skb_to_tx_info(skb);
-
-			len += snprintf(buf + len, buf_len - len,
-					"\tSKB #%d\n", j);
-			len += snprintf(buf + len, buf_len - len,
-					"\t\tsta:   %d\n", tx_info->sta);
-			len += snprintf(buf + len, buf_len - len,
-					"\t\tcolor: %d\n", tx_info->color);
-			len += snprintf(buf + len, buf_len - len,
-					"\t\ttid:   %d\n", tx_info->tid);
-		}
-
-		spin_unlock_irqrestore(&txq->queue.lock, flags);
-
-		spin_lock_irqsave(&txq->stopped_queue.lock, flags);
-
-		len += snprintf(buf + len, buf_len - len,
-				"\tStopped Queue len:   %d\n",
-				skb_queue_len(&txq->stopped_queue));
-		for (j = 0; j < skb_queue_len(&txq->stopped_queue); j++) {
-			struct iwm_tx_info *tx_info;
-
-			skb = skb->next;
-			tx_info = skb_to_tx_info(skb);
-
-			len += snprintf(buf + len, buf_len - len,
-					"\tSKB #%d\n", j);
-			len += snprintf(buf + len, buf_len - len,
-					"\t\tsta:   %d\n", tx_info->sta);
-			len += snprintf(buf + len, buf_len - len,
-					"\t\tcolor: %d\n", tx_info->color);
-			len += snprintf(buf + len, buf_len - len,
-					"\t\ttid:   %d\n", tx_info->tid);
-		}
-
-		spin_unlock_irqrestore(&txq->stopped_queue.lock, flags);
-	}
-
-	ret = simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
-	kfree(buf);
-
-	return ret;
-}
-
-static ssize_t iwm_debugfs_tx_credit_read(struct file *filp,
-					  char __user *buffer,
-					  size_t count, loff_t *ppos)
-{
-	struct iwm_priv *iwm = filp->private_data;
-	struct iwm_tx_credit *credit = &iwm->tx_credit;
-	char *buf;
-	int i, buf_len = 4096;
-	size_t len = 0;
-	ssize_t ret;
-
-	if (*ppos != 0)
-		return 0;
-	if (count < sizeof(buf))
-		return -ENOSPC;
-
-	buf = kzalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	len += snprintf(buf + len, buf_len - len,
-			"NR pools:  %d\n", credit->pool_nr);
-	len += snprintf(buf + len, buf_len - len,
-			"pools map: 0x%lx\n", credit->full_pools_map);
-
-	len += snprintf(buf + len, buf_len - len, "\n### POOLS ###\n");
-	for (i = 0; i < IWM_MACS_OUT_GROUPS; i++) {
-		len += snprintf(buf + len, buf_len - len,
-				"pools entry #%d\n", i);
-		len += snprintf(buf + len, buf_len - len,
-				"\tid:          %d\n",
-				credit->pools[i].id);
-		len += snprintf(buf + len, buf_len - len,
-				"\tsid:         %d\n",
-				credit->pools[i].sid);
-		len += snprintf(buf + len, buf_len - len,
-				"\tmin_pages:   %d\n",
-				credit->pools[i].min_pages);
-		len += snprintf(buf + len, buf_len - len,
-				"\tmax_pages:   %d\n",
-				credit->pools[i].max_pages);
-		len += snprintf(buf + len, buf_len - len,
-				"\talloc_pages: %d\n",
-				credit->pools[i].alloc_pages);
-		len += snprintf(buf + len, buf_len - len,
-				"\tfreed_pages: %d\n",
-				credit->pools[i].total_freed_pages);
-	}
-
-	len += snprintf(buf + len, buf_len - len, "\n### SPOOLS ###\n");
-	for (i = 0; i < IWM_MACS_OUT_SGROUPS; i++) {
-		len += snprintf(buf + len, buf_len - len,
-				"spools entry #%d\n", i);
-		len += snprintf(buf + len, buf_len - len,
-				"\tid:          %d\n",
-				credit->spools[i].id);
-		len += snprintf(buf + len, buf_len - len,
-				"\tmax_pages:   %d\n",
-				credit->spools[i].max_pages);
-		len += snprintf(buf + len, buf_len - len,
-				"\talloc_pages: %d\n",
-				credit->spools[i].alloc_pages);
-
-	}
-
-	ret = simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
-	kfree(buf);
-
-	return ret;
-}
-
-static ssize_t iwm_debugfs_rx_ticket_read(struct file *filp,
-					  char __user *buffer,
-					  size_t count, loff_t *ppos)
-{
-	struct iwm_priv *iwm = filp->private_data;
-	struct iwm_rx_ticket_node *ticket;
-	char *buf;
-	int buf_len = 4096, i;
-	size_t len = 0;
-	ssize_t ret;
-
-	if (*ppos != 0)
-		return 0;
-	if (count < sizeof(buf))
-		return -ENOSPC;
-
-	buf = kzalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	spin_lock(&iwm->ticket_lock);
-	list_for_each_entry(ticket, &iwm->rx_tickets, node) {
-		len += snprintf(buf + len, buf_len - len, "Ticket #%d\n",
-				ticket->ticket->id);
-		len += snprintf(buf + len, buf_len - len, "\taction: 0x%x\n",
-				ticket->ticket->action);
-		len += snprintf(buf + len, buf_len - len, "\tflags:  0x%x\n",
-				ticket->ticket->flags);
-	}
-	spin_unlock(&iwm->ticket_lock);
-
-	for (i = 0; i < IWM_RX_ID_HASH; i++) {
-		struct iwm_rx_packet *packet;
-		struct list_head *pkt_list = &iwm->rx_packets[i];
-
-		if (!list_empty(pkt_list)) {
-			len += snprintf(buf + len, buf_len - len,
-					"Packet hash #%d\n", i);
-			spin_lock(&iwm->packet_lock[i]);
-			list_for_each_entry(packet, pkt_list, node) {
-				len += snprintf(buf + len, buf_len - len,
-						"\tPacket id:     %d\n",
-						packet->id);
-				len += snprintf(buf + len, buf_len - len,
-						"\tPacket length: %lu\n",
-						packet->pkt_size);
-			}
-			spin_unlock(&iwm->packet_lock[i]);
-		}
-	}
-
-	ret = simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
-	kfree(buf);
-
-	return ret;
-}
-
-static ssize_t iwm_debugfs_fw_err_read(struct file *filp,
-				       char __user *buffer,
-				       size_t count, loff_t *ppos)
-{
-
-	struct iwm_priv *iwm = filp->private_data;
-	char buf[512];
-	int buf_len = 512;
-	size_t len = 0;
-
-	if (*ppos != 0)
-		return 0;
-	if (count < sizeof(buf))
-		return -ENOSPC;
-
-	if (!iwm->last_fw_err)
-		return -ENOMEM;
-
-	if (iwm->last_fw_err->line_num == 0)
-		goto out;
-
-	len += snprintf(buf + len, buf_len - len, "%cMAC FW ERROR:\n",
-	     (le32_to_cpu(iwm->last_fw_err->category) == UMAC_SYS_ERR_CAT_LMAC)
-			? 'L' : 'U');
-	len += snprintf(buf + len, buf_len - len,
-			"\tCategory:    %d\n",
-			le32_to_cpu(iwm->last_fw_err->category));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tStatus:      0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->status));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tPC:          0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->pc));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tblink1:      %d\n",
-			le32_to_cpu(iwm->last_fw_err->blink1));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tblink2:      %d\n",
-			le32_to_cpu(iwm->last_fw_err->blink2));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tilink1:      %d\n",
-			le32_to_cpu(iwm->last_fw_err->ilink1));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tilink2:      %d\n",
-			le32_to_cpu(iwm->last_fw_err->ilink2));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tData1:       0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->data1));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tData2:       0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->data2));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tLine number: %d\n",
-			le32_to_cpu(iwm->last_fw_err->line_num));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tUMAC status: 0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->umac_status));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tLMAC status: 0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->lmac_status));
-
-	len += snprintf(buf + len, buf_len - len,
-			"\tSDIO status: 0x%x\n",
-			le32_to_cpu(iwm->last_fw_err->sdio_status));
-
-out:
-
-	return simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
-}
-
-static const struct file_operations iwm_debugfs_txq_fops = {
-	.owner =	THIS_MODULE,
-	.open =		simple_open,
-	.read =		iwm_debugfs_txq_read,
-	.llseek =	default_llseek,
-};
-
-static const struct file_operations iwm_debugfs_tx_credit_fops = {
-	.owner =	THIS_MODULE,
-	.open =		simple_open,
-	.read =		iwm_debugfs_tx_credit_read,
-	.llseek =	default_llseek,
-};
-
-static const struct file_operations iwm_debugfs_rx_ticket_fops = {
-	.owner =	THIS_MODULE,
-	.open =		simple_open,
-	.read =		iwm_debugfs_rx_ticket_read,
-	.llseek =	default_llseek,
-};
-
-static const struct file_operations iwm_debugfs_fw_err_fops = {
-	.owner =	THIS_MODULE,
-	.open =		simple_open,
-	.read =		iwm_debugfs_fw_err_read,
-	.llseek =	default_llseek,
-};
-
-void iwm_debugfs_init(struct iwm_priv *iwm)
-{
-	int i;
-
-	iwm->dbg.rootdir = debugfs_create_dir(KBUILD_MODNAME, NULL);
-	iwm->dbg.devdir = debugfs_create_dir(wiphy_name(iwm_to_wiphy(iwm)),
-					     iwm->dbg.rootdir);
-	iwm->dbg.dbgdir = debugfs_create_dir("debug", iwm->dbg.devdir);
-	iwm->dbg.rxdir = debugfs_create_dir("rx", iwm->dbg.devdir);
-	iwm->dbg.txdir = debugfs_create_dir("tx", iwm->dbg.devdir);
-	iwm->dbg.busdir = debugfs_create_dir("bus", iwm->dbg.devdir);
-	if (iwm->bus_ops->debugfs_init)
-		iwm->bus_ops->debugfs_init(iwm, iwm->dbg.busdir);
-
-	iwm->dbg.dbg_level = IWM_DL_NONE;
-	iwm->dbg.dbg_level_dentry =
-		debugfs_create_file("level", 0200, iwm->dbg.dbgdir, iwm,
-				    &fops_iwm_dbg_level);
-
-	iwm->dbg.dbg_modules = IWM_DM_DEFAULT;
-	iwm->dbg.dbg_modules_dentry =
-		debugfs_create_file("modules", 0200, iwm->dbg.dbgdir, iwm,
-				    &fops_iwm_dbg_modules);
-
-	for (i = 0; i < __IWM_DM_NR; i++)
-		add_dbg_module(iwm->dbg, iwm_debug_module[i].name,
-			       iwm_debug_module[i].id, IWM_DL_DEFAULT);
-
-	iwm->dbg.txq_dentry = debugfs_create_file("queues", 0200,
-						  iwm->dbg.txdir, iwm,
-						  &iwm_debugfs_txq_fops);
-	iwm->dbg.tx_credit_dentry = debugfs_create_file("credits", 0200,
-						   iwm->dbg.txdir, iwm,
-						   &iwm_debugfs_tx_credit_fops);
-	iwm->dbg.rx_ticket_dentry = debugfs_create_file("tickets", 0200,
-						  iwm->dbg.rxdir, iwm,
-						  &iwm_debugfs_rx_ticket_fops);
-	iwm->dbg.fw_err_dentry = debugfs_create_file("last_fw_err", 0200,
-						     iwm->dbg.dbgdir, iwm,
-						     &iwm_debugfs_fw_err_fops);
-}
-
-void iwm_debugfs_exit(struct iwm_priv *iwm)
-{
-	int i;
-
-	for (i = 0; i < __IWM_DM_NR; i++)
-		debugfs_remove(iwm->dbg.dbg_module_dentries[i]);
-
-	debugfs_remove(iwm->dbg.dbg_modules_dentry);
-	debugfs_remove(iwm->dbg.dbg_level_dentry);
-	debugfs_remove(iwm->dbg.txq_dentry);
-	debugfs_remove(iwm->dbg.tx_credit_dentry);
-	debugfs_remove(iwm->dbg.rx_ticket_dentry);
-	debugfs_remove(iwm->dbg.fw_err_dentry);
-	if (iwm->bus_ops->debugfs_exit)
-		iwm->bus_ops->debugfs_exit(iwm);
-
-	debugfs_remove(iwm->dbg.busdir);
-	debugfs_remove(iwm->dbg.dbgdir);
-	debugfs_remove(iwm->dbg.txdir);
-	debugfs_remove(iwm->dbg.rxdir);
-	debugfs_remove(iwm->dbg.devdir);
-	debugfs_remove(iwm->dbg.rootdir);
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/eeprom.c b/drivers/net/wireless/iwmc3200wifi/eeprom.c
deleted file mode 100644
index e80e776b74f..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/eeprom.c
+++ /dev/null
@@ -1,234 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/slab.h>
-
-#include "iwm.h"
-#include "umac.h"
-#include "commands.h"
-#include "eeprom.h"
-
-static struct iwm_eeprom_entry eeprom_map[] = {
-	[IWM_EEPROM_SIG] =
-	{"Signature", IWM_EEPROM_SIG_OFF, IWM_EEPROM_SIG_LEN},
-
-	[IWM_EEPROM_VERSION] =
-	{"Version", IWM_EEPROM_VERSION_OFF, IWM_EEPROM_VERSION_LEN},
-
-	[IWM_EEPROM_OEM_HW_VERSION] =
-	{"OEM HW version", IWM_EEPROM_OEM_HW_VERSION_OFF,
-	 IWM_EEPROM_OEM_HW_VERSION_LEN},
-
-	[IWM_EEPROM_MAC_VERSION] =
-	{"MAC version", IWM_EEPROM_MAC_VERSION_OFF, IWM_EEPROM_MAC_VERSION_LEN},
-
-	[IWM_EEPROM_CARD_ID] =
-	{"Card ID", IWM_EEPROM_CARD_ID_OFF, IWM_EEPROM_CARD_ID_LEN},
-
-	[IWM_EEPROM_RADIO_CONF] =
-	{"Radio config", IWM_EEPROM_RADIO_CONF_OFF, IWM_EEPROM_RADIO_CONF_LEN},
-
-	[IWM_EEPROM_SKU_CAP] =
-	{"SKU capabilities", IWM_EEPROM_SKU_CAP_OFF, IWM_EEPROM_SKU_CAP_LEN},
-
-	[IWM_EEPROM_FAT_CHANNELS_CAP] =
-	{"HT channels capabilities", IWM_EEPROM_FAT_CHANNELS_CAP_OFF,
-	 IWM_EEPROM_FAT_CHANNELS_CAP_LEN},
-
-	[IWM_EEPROM_CALIB_RXIQ_OFFSET] =
-	{"RX IQ offset", IWM_EEPROM_CALIB_RXIQ_OFF, IWM_EEPROM_INDIRECT_LEN},
-
-	[IWM_EEPROM_CALIB_RXIQ] =
-	{"Calib RX IQ", 0, IWM_EEPROM_CALIB_RXIQ_LEN},
-};
-
-
-static int iwm_eeprom_read(struct iwm_priv *iwm, u8 eeprom_id)
-{
-	int ret;
-	u32 entry_size, chunk_size, data_offset = 0, addr_offset = 0;
-	u32 addr;
-	struct iwm_udma_wifi_cmd udma_cmd;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_umac_cmd_eeprom_proxy eeprom_cmd;
-
-	if (eeprom_id > (IWM_EEPROM_LAST - 1))
-		return -EINVAL;
-
-	entry_size = eeprom_map[eeprom_id].length;
-
-	if (eeprom_id >= IWM_EEPROM_INDIRECT_DATA) {
-		/* indirect data */
-		u32 off_id = eeprom_id - IWM_EEPROM_INDIRECT_DATA +
-			     IWM_EEPROM_INDIRECT_OFFSET;
-
-		eeprom_map[eeprom_id].offset =
-			*(u16 *)(iwm->eeprom + eeprom_map[off_id].offset) << 1;
-	}
-
-	addr = eeprom_map[eeprom_id].offset;
-
-	udma_cmd.eop = 1;
-	udma_cmd.credit_group = 0x4;
-	udma_cmd.ra_tid = UMAC_HDI_ACT_TBL_IDX_HOST_CMD;
-	udma_cmd.lmac_offset = 0;
-
-	umac_cmd.id = UMAC_CMD_OPCODE_EEPROM_PROXY;
-	umac_cmd.resp = 1;
-
-	while (entry_size > 0) {
-		chunk_size = min_t(u32, entry_size, IWM_MAX_EEPROM_DATA_LEN);
-
-		eeprom_cmd.hdr.type =
-			cpu_to_le32(IWM_UMAC_CMD_EEPROM_TYPE_READ);
-		eeprom_cmd.hdr.offset = cpu_to_le32(addr + addr_offset);
-		eeprom_cmd.hdr.len = cpu_to_le32(chunk_size);
-
-		ret = iwm_hal_send_umac_cmd(iwm, &udma_cmd,
-					    &umac_cmd, &eeprom_cmd,
-				     sizeof(struct iwm_umac_cmd_eeprom_proxy));
-		if (ret < 0) {
-			IWM_ERR(iwm, "Couldn't read eeprom\n");
-			return ret;
-		}
-
-		ret = iwm_notif_handle(iwm, UMAC_CMD_OPCODE_EEPROM_PROXY,
-				       IWM_SRC_UMAC, 2*HZ);
-		if (ret < 0) {
-			IWM_ERR(iwm, "Did not get any eeprom answer\n");
-			return ret;
-		}
-
-		data_offset += chunk_size;
-		addr_offset += chunk_size;
-		entry_size -= chunk_size;
-	}
-
-	return 0;
-}
-
-u8 *iwm_eeprom_access(struct iwm_priv *iwm, u8 eeprom_id)
-{
-	if (!iwm->eeprom)
-		return ERR_PTR(-ENODEV);
-
-	return iwm->eeprom + eeprom_map[eeprom_id].offset;
-}
-
-int iwm_eeprom_fat_channels(struct iwm_priv *iwm)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct ieee80211_supported_band *band;
-	u16 *channels, i;
-
-	channels = (u16 *)iwm_eeprom_access(iwm, IWM_EEPROM_FAT_CHANNELS_CAP);
-	if (IS_ERR(channels))
-		return PTR_ERR(channels);
-
-	band = wiphy->bands[IEEE80211_BAND_2GHZ];
-	band->ht_cap.ht_supported = true;
-
-	for (i = 0; i < IWM_EEPROM_FAT_CHANNELS_24; i++)
-		if (!(channels[i] & IWM_EEPROM_FAT_CHANNEL_ENABLED))
-			band->ht_cap.ht_supported = false;
-
-	band = wiphy->bands[IEEE80211_BAND_5GHZ];
-	band->ht_cap.ht_supported = true;
-	for (i = IWM_EEPROM_FAT_CHANNELS_24; i < IWM_EEPROM_FAT_CHANNELS; i++)
-		if (!(channels[i] & IWM_EEPROM_FAT_CHANNEL_ENABLED))
-			band->ht_cap.ht_supported = false;
-
-	return 0;
-}
-
-u32 iwm_eeprom_wireless_mode(struct iwm_priv *iwm)
-{
-	u16 sku_cap;
-	u32 wireless_mode = 0;
-
-	sku_cap = *((u16 *)iwm_eeprom_access(iwm, IWM_EEPROM_SKU_CAP));
-
-	if (sku_cap & IWM_EEPROM_SKU_CAP_BAND_24GHZ)
-		wireless_mode |= WIRELESS_MODE_11G;
-
-	if (sku_cap & IWM_EEPROM_SKU_CAP_BAND_52GHZ)
-		wireless_mode |= WIRELESS_MODE_11A;
-
-	if (sku_cap & IWM_EEPROM_SKU_CAP_11N_ENABLE)
-		wireless_mode |= WIRELESS_MODE_11N;
-
-	return wireless_mode;
-}
-
-
-int iwm_eeprom_init(struct iwm_priv *iwm)
-{
-	int i, ret = 0;
-	char name[32];
-
-	iwm->eeprom = kzalloc(IWM_EEPROM_LEN, GFP_KERNEL);
-	if (!iwm->eeprom)
-		return -ENOMEM;
-
-	for (i = IWM_EEPROM_FIRST; i < IWM_EEPROM_LAST; i++) {
-		ret = iwm_eeprom_read(iwm, i);
-		if (ret < 0) {
-			IWM_ERR(iwm, "Couldn't read eeprom entry #%d: %s\n",
-				i, eeprom_map[i].name);
-			break;
-		}
-	}
-
-	IWM_DBG_BOOT(iwm, DBG, "EEPROM dump:\n");
-	for (i = IWM_EEPROM_FIRST; i < IWM_EEPROM_LAST; i++) {
-		memset(name, 0, 32);
-		sprintf(name, "%s: ", eeprom_map[i].name);
-
-		IWM_HEXDUMP(iwm, DBG, BOOT, name,
-			    iwm->eeprom + eeprom_map[i].offset,
-			    eeprom_map[i].length);
-	}
-
-	return ret;
-}
-
-void iwm_eeprom_exit(struct iwm_priv *iwm)
-{
-	kfree(iwm->eeprom);
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/eeprom.h b/drivers/net/wireless/iwmc3200wifi/eeprom.h
deleted file mode 100644
index 4e3a3fdab0d..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/eeprom.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_EEPROM_H__
-#define __IWM_EEPROM_H__
-
-enum {
-	IWM_EEPROM_SIG = 0,
-	IWM_EEPROM_FIRST = IWM_EEPROM_SIG,
-	IWM_EEPROM_VERSION,
-	IWM_EEPROM_OEM_HW_VERSION,
-	IWM_EEPROM_MAC_VERSION,
-	IWM_EEPROM_CARD_ID,
-	IWM_EEPROM_RADIO_CONF,
-	IWM_EEPROM_SKU_CAP,
-	IWM_EEPROM_FAT_CHANNELS_CAP,
-
-	IWM_EEPROM_INDIRECT_OFFSET,
-	IWM_EEPROM_CALIB_RXIQ_OFFSET = IWM_EEPROM_INDIRECT_OFFSET,
-
-	IWM_EEPROM_INDIRECT_DATA,
-	IWM_EEPROM_CALIB_RXIQ = IWM_EEPROM_INDIRECT_DATA,
-
-	IWM_EEPROM_LAST,
-};
-
-#define IWM_EEPROM_SIG_OFF                 0x00
-#define IWM_EEPROM_VERSION_OFF            (0x54 << 1)
-#define IWM_EEPROM_OEM_HW_VERSION_OFF     (0x56 << 1)
-#define IWM_EEPROM_MAC_VERSION_OFF        (0x30 << 1)
-#define IWM_EEPROM_CARD_ID_OFF            (0x5d << 1)
-#define IWM_EEPROM_RADIO_CONF_OFF         (0x58 << 1)
-#define IWM_EEPROM_SKU_CAP_OFF            (0x55 << 1)
-#define IWM_EEPROM_CALIB_CONFIG_OFF       (0x7c << 1)
-#define IWM_EEPROM_FAT_CHANNELS_CAP_OFF   (0xde << 1)
-
-#define IWM_EEPROM_SIG_LEN              4
-#define IWM_EEPROM_VERSION_LEN          2
-#define IWM_EEPROM_OEM_HW_VERSION_LEN   2
-#define IWM_EEPROM_MAC_VERSION_LEN      1
-#define IWM_EEPROM_CARD_ID_LEN          2
-#define IWM_EEPROM_RADIO_CONF_LEN       2
-#define IWM_EEPROM_SKU_CAP_LEN          2
-#define IWM_EEPROM_FAT_CHANNELS_CAP_LEN 40
-#define IWM_EEPROM_INDIRECT_LEN		2
-
-#define IWM_MAX_EEPROM_DATA_LEN         240
-#define IWM_EEPROM_LEN                  0x800
-
-#define IWM_EEPROM_MIN_ALLOWED_VERSION          0x0610
-#define IWM_EEPROM_MAX_ALLOWED_VERSION          0x0700
-#define IWM_EEPROM_CURRENT_VERSION              0x0612
-
-#define IWM_EEPROM_SKU_CAP_BAND_24GHZ           (1 << 4)
-#define IWM_EEPROM_SKU_CAP_BAND_52GHZ           (1 << 5)
-#define IWM_EEPROM_SKU_CAP_11N_ENABLE           (1 << 6)
-
-#define IWM_EEPROM_FAT_CHANNELS 20
-/* 2.4 gHz FAT primary channels: 1, 2, 3, 4, 5, 6, 7, 8, 9 */
-#define IWM_EEPROM_FAT_CHANNELS_24 9
-/* 5.2 gHz FAT primary channels: 36,44,52,60,100,108,116,124,132,149,157 */
-#define IWM_EEPROM_FAT_CHANNELS_52 11
-
-#define IWM_EEPROM_FAT_CHANNEL_ENABLED (1 << 0)
-
-enum {
-	IWM_EEPROM_CALIB_CAL_HDR,
-	IWM_EEPROM_CALIB_TX_POWER,
-	IWM_EEPROM_CALIB_XTAL,
-	IWM_EEPROM_CALIB_TEMPERATURE,
-	IWM_EEPROM_CALIB_RX_BB_FILTER,
-	IWM_EEPROM_CALIB_RX_IQ,
-	IWM_EEPROM_CALIB_MAX,
-};
-
-#define IWM_EEPROM_CALIB_RXIQ_OFF	(IWM_EEPROM_CALIB_CONFIG_OFF + \
-					 (IWM_EEPROM_CALIB_RX_IQ << 1))
-#define IWM_EEPROM_CALIB_RXIQ_LEN	sizeof(struct iwm_lmac_calib_rxiq)
-
-struct iwm_eeprom_entry {
-	char *name;
-	u32 offset;
-	u32 length;
-};
-
-int iwm_eeprom_init(struct iwm_priv *iwm);
-void iwm_eeprom_exit(struct iwm_priv *iwm);
-u8 *iwm_eeprom_access(struct iwm_priv *iwm, u8 eeprom_id);
-int iwm_eeprom_fat_channels(struct iwm_priv *iwm);
-u32 iwm_eeprom_wireless_mode(struct iwm_priv *iwm);
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/fw.c b/drivers/net/wireless/iwmc3200wifi/fw.c
deleted file mode 100644
index 6f1afe6bbc8..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/fw.c
+++ /dev/null
@@ -1,416 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/firmware.h>
-
-#include "iwm.h"
-#include "bus.h"
-#include "hal.h"
-#include "umac.h"
-#include "debug.h"
-#include "fw.h"
-#include "commands.h"
-
-static const char fw_barker[] = "*WESTOPFORNOONE*";
-
-/*
- * @op_code: Op code we're looking for.
- * @index: There can be several instances of the same opcode within
- *         the firmware. Index specifies which one we're looking for.
- */
-static int iwm_fw_op_offset(struct iwm_priv *iwm, const struct firmware *fw,
-			    u16 op_code, u32 index)
-{
-	int offset = -EINVAL, fw_offset;
-	u32 op_index = 0;
-	const u8 *fw_ptr;
-	struct iwm_fw_hdr_rec *rec;
-
-	fw_offset = 0;
-	fw_ptr = fw->data;
-
-	/* We first need to look for the firmware barker */
-	if (memcmp(fw_ptr, fw_barker, IWM_HDR_BARKER_LEN)) {
-		IWM_ERR(iwm, "No barker string in this FW\n");
-		return -EINVAL;
-	}
-
-	if (fw->size < IWM_HDR_LEN) {
-		IWM_ERR(iwm, "FW is too small (%zu)\n", fw->size);
-		return -EINVAL;
-	}
-
-	fw_offset += IWM_HDR_BARKER_LEN;
-
-	while (fw_offset < fw->size) {
-		rec = (struct iwm_fw_hdr_rec *)(fw_ptr + fw_offset);
-
-		IWM_DBG_FW(iwm, DBG, "FW: op_code: 0x%x, len: %d @ 0x%x\n",
-			   rec->op_code, rec->len, fw_offset);
-
-		if (rec->op_code == IWM_HDR_REC_OP_INVALID) {
-			IWM_DBG_FW(iwm, DBG, "Reached INVALID op code\n");
-			break;
-		}
-
-		if (rec->op_code == op_code) {
-			if (op_index == index) {
-				fw_offset += sizeof(struct iwm_fw_hdr_rec);
-				offset = fw_offset;
-				goto out;
-			}
-			op_index++;
-		}
-
-		fw_offset += sizeof(struct iwm_fw_hdr_rec) + rec->len;
-	}
-
- out:
-	return offset;
-}
-
-static int iwm_load_firmware_chunk(struct iwm_priv *iwm,
-				   const struct firmware *fw,
-				   struct iwm_fw_img_desc *img_desc)
-{
-	struct iwm_udma_nonwifi_cmd target_cmd;
-	u32 chunk_size;
-	const u8 *chunk_ptr;
-	int ret = 0;
-
-	IWM_DBG_FW(iwm, INFO, "Loading FW chunk: %d bytes @ 0x%x\n",
-		   img_desc->length, img_desc->address);
-
-	target_cmd.opcode = UMAC_HDI_OUT_OPCODE_WRITE;
-	target_cmd.handle_by_hw = 1;
-	target_cmd.op2 = 0;
-	target_cmd.resp = 0;
-	target_cmd.eop = 1;
-
-	chunk_size = img_desc->length;
-	chunk_ptr = fw->data + img_desc->offset;
-
-	while (chunk_size > 0) {
-		u32 tmp_chunk_size;
-
-		tmp_chunk_size = min_t(u32, chunk_size,
-				       IWM_MAX_NONWIFI_CMD_BUFF_SIZE);
-
-		target_cmd.addr = cpu_to_le32(img_desc->address +
-				    (chunk_ptr - fw->data - img_desc->offset));
-		target_cmd.op1_sz = cpu_to_le32(tmp_chunk_size);
-
-		IWM_DBG_FW(iwm, DBG, "\t%d bytes @ 0x%x\n",
-			   tmp_chunk_size, target_cmd.addr);
-
-		ret = iwm_hal_send_target_cmd(iwm, &target_cmd, chunk_ptr);
-		if (ret < 0) {
-			IWM_ERR(iwm, "Couldn't load FW chunk\n");
-			break;
-		}
-
-		chunk_size -= tmp_chunk_size;
-		chunk_ptr += tmp_chunk_size;
-	}
-
-	return ret;
-}
-/*
- * To load a fw image to the target, we basically go through the
- * fw, looking for OP_MEM_DESC records. Once we found one, we
- * pass it to iwm_load_firmware_chunk().
- * The OP_MEM_DESC records contain the actuall memory chunk to be
- * sent, but also the destination address.
- */
-static int iwm_load_img(struct iwm_priv *iwm, const char *img_name)
-{
-	const struct firmware *fw;
-	struct iwm_fw_img_desc *img_desc;
-	struct iwm_fw_img_ver *ver;
-	int ret = 0, fw_offset;
-	u32 opcode_idx = 0, build_date;
-	char *build_tag;
-
-	ret = request_firmware(&fw, img_name, iwm_to_dev(iwm));
-	if (ret) {
-		IWM_ERR(iwm, "Request firmware failed");
-		return ret;
-	}
-
-	IWM_DBG_FW(iwm, INFO, "Start to load FW %s\n", img_name);
-
-	while (1) {
-		fw_offset = iwm_fw_op_offset(iwm, fw,
-					     IWM_HDR_REC_OP_MEM_DESC,
-					     opcode_idx);
-		if (fw_offset < 0)
-			break;
-
-		img_desc = (struct iwm_fw_img_desc *)(fw->data + fw_offset);
-		ret = iwm_load_firmware_chunk(iwm, fw, img_desc);
-		if (ret < 0)
-			goto err_release_fw;
-		opcode_idx++;
-	}
-
-	/* Read firmware version */
-	fw_offset = iwm_fw_op_offset(iwm, fw, IWM_HDR_REC_OP_SW_VER, 0);
-	if (fw_offset < 0)
-		goto err_release_fw;
-
-	ver = (struct iwm_fw_img_ver *)(fw->data + fw_offset);
-
-	/* Read build tag */
-	fw_offset = iwm_fw_op_offset(iwm, fw, IWM_HDR_REC_OP_BUILD_TAG, 0);
-	if (fw_offset < 0)
-		goto err_release_fw;
-
-	build_tag = (char *)(fw->data + fw_offset);
-
-	/* Read build date */
-	fw_offset = iwm_fw_op_offset(iwm, fw, IWM_HDR_REC_OP_BUILD_DATE, 0);
-	if (fw_offset < 0)
-		goto err_release_fw;
-
-	build_date = *(u32 *)(fw->data + fw_offset);
-
-	IWM_INFO(iwm, "%s:\n", img_name);
-	IWM_INFO(iwm, "\tVersion:    %02X.%02X\n", ver->major, ver->minor);
-	IWM_INFO(iwm, "\tBuild tag:  %s\n", build_tag);
-	IWM_INFO(iwm, "\tBuild date: %x-%x-%x\n",
-		 IWM_BUILD_YEAR(build_date), IWM_BUILD_MONTH(build_date),
-		 IWM_BUILD_DAY(build_date));
-
-	if (!strcmp(img_name, iwm->bus_ops->umac_name))
-		sprintf(iwm->umac_version, "%02X.%02X",
-			ver->major, ver->minor);
-
-	if (!strcmp(img_name, iwm->bus_ops->lmac_name))
-		sprintf(iwm->lmac_version, "%02X.%02X",
-			ver->major, ver->minor);
-
- err_release_fw:
-	release_firmware(fw);
-
-	return ret;
-}
-
-static int iwm_load_umac(struct iwm_priv *iwm)
-{
-	struct iwm_udma_nonwifi_cmd target_cmd;
-	int ret;
-
-	ret = iwm_load_img(iwm, iwm->bus_ops->umac_name);
-	if (ret < 0)
-		return ret;
-
-	/* We've loaded the UMAC, we can tell the target to jump there */
-	target_cmd.opcode = UMAC_HDI_OUT_OPCODE_JUMP;
-	target_cmd.addr = cpu_to_le32(UMAC_MU_FW_INST_DATA_12_ADDR);
-	target_cmd.op1_sz = 0;
-	target_cmd.op2 = 0;
-	target_cmd.handle_by_hw = 0;
-	target_cmd.resp = 1 ;
-	target_cmd.eop = 1;
-
-	ret = iwm_hal_send_target_cmd(iwm, &target_cmd, NULL);
-	if (ret < 0)
-		IWM_ERR(iwm, "Couldn't send JMP command\n");
-
-	return ret;
-}
-
-static int iwm_load_lmac(struct iwm_priv *iwm, const char *img_name)
-{
-	int ret;
-
-	ret = iwm_load_img(iwm, img_name);
-	if (ret < 0)
-		return ret;
-
-	return iwm_send_umac_reset(iwm,
-			cpu_to_le32(UMAC_RST_CTRL_FLG_LARC_CLK_EN), 0);
-}
-
-static int iwm_init_calib(struct iwm_priv *iwm, unsigned long cfg_bitmap,
-			  unsigned long expected_bitmap, u8 rx_iq_cmd)
-{
-	/* Read RX IQ calibration result from EEPROM */
-	if (test_bit(rx_iq_cmd, &cfg_bitmap)) {
-		iwm_store_rxiq_calib_result(iwm);
-		set_bit(PHY_CALIBRATE_RX_IQ_CMD, &iwm->calib_done_map);
-	}
-
-	iwm_send_prio_table(iwm);
-	iwm_send_init_calib_cfg(iwm, cfg_bitmap);
-
-	while (iwm->calib_done_map != expected_bitmap) {
-		if (iwm_notif_handle(iwm, CALIBRATION_RES_NOTIFICATION,
-				     IWM_SRC_LMAC, WAIT_NOTIF_TIMEOUT)) {
-			IWM_DBG_FW(iwm, DBG, "Initial calibration timeout\n");
-			return -ETIMEDOUT;
-		}
-
-		IWM_DBG_FW(iwm, DBG, "Got calibration result. calib_done_map: "
-			   "0x%lx, expected calibrations: 0x%lx\n",
-			   iwm->calib_done_map, expected_bitmap);
-	}
-
-	return 0;
-}
-
-/*
- * We currently have to load 3 FWs:
- * 1) The UMAC (Upper MAC).
- * 2) The calibration LMAC (Lower MAC).
- *    We then send the calibration init command, so that the device can
- *    run a first calibration round.
- * 3) The operational LMAC, which replaces the calibration one when it's
- *    done with the first calibration round.
- *
- * Once those 3 FWs have been loaded, we send the periodic calibration
- * command, and then the device is available for regular 802.11 operations.
- */
-int iwm_load_fw(struct iwm_priv *iwm)
-{
-	unsigned long init_calib_map, periodic_calib_map;
-	unsigned long expected_calib_map;
-	int ret;
-
-	/* We first start downloading the UMAC */
-	ret = iwm_load_umac(iwm);
-	if (ret < 0) {
-		IWM_ERR(iwm, "UMAC loading failed\n");
-		return ret;
-	}
-
-	/* Handle UMAC_ALIVE notification */
-	ret = iwm_notif_handle(iwm, UMAC_NOTIFY_OPCODE_ALIVE, IWM_SRC_UMAC,
-			       WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Handle UMAC_ALIVE failed: %d\n", ret);
-		return ret;
-	}
-
-	/* UMAC is alive, we can download the calibration LMAC */
-	ret = iwm_load_lmac(iwm, iwm->bus_ops->calib_lmac_name);
-	if (ret) {
-		IWM_ERR(iwm, "Calibration LMAC loading failed\n");
-		return ret;
-	}
-
-	/* Handle UMAC_INIT_COMPLETE notification */
-	ret = iwm_notif_handle(iwm, UMAC_NOTIFY_OPCODE_INIT_COMPLETE,
-			       IWM_SRC_UMAC, WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Handle INIT_COMPLETE failed for calibration "
-			"LMAC: %d\n", ret);
-		return ret;
-	}
-
-	/* Read EEPROM data */
-	ret = iwm_eeprom_init(iwm);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't init eeprom array\n");
-		return ret;
-	}
-
-	init_calib_map = iwm->conf.calib_map & IWM_CALIB_MAP_INIT_MSK;
-	expected_calib_map = iwm->conf.expected_calib_map &
-		IWM_CALIB_MAP_INIT_MSK;
-	periodic_calib_map = IWM_CALIB_MAP_PER_LMAC(iwm->conf.calib_map);
-
-	ret = iwm_init_calib(iwm, init_calib_map, expected_calib_map,
-			     CALIB_CFG_RX_IQ_IDX);
-	if (ret < 0) {
-		/* Let's try the old way */
-		ret = iwm_init_calib(iwm, expected_calib_map,
-				     expected_calib_map,
-				     PHY_CALIBRATE_RX_IQ_CMD);
-		if (ret < 0) {
-			IWM_ERR(iwm, "Calibration result timeout\n");
-			goto out;
-		}
-	}
-
-	/* Handle LMAC CALIBRATION_COMPLETE notification */
-	ret = iwm_notif_handle(iwm, CALIBRATION_COMPLETE_NOTIFICATION,
-			       IWM_SRC_LMAC, WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Wait for CALIBRATION_COMPLETE timeout\n");
-		goto out;
-	}
-
-	IWM_INFO(iwm, "LMAC calibration done: 0x%lx\n", iwm->calib_done_map);
-
-	iwm_send_umac_reset(iwm, cpu_to_le32(UMAC_RST_CTRL_FLG_LARC_RESET), 1);
-
-	ret = iwm_notif_handle(iwm, UMAC_CMD_OPCODE_RESET, IWM_SRC_UMAC,
-			       WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Wait for UMAC RESET timeout\n");
-		goto out;
-	}
-
-	/* Download the operational LMAC */
-	ret = iwm_load_lmac(iwm, iwm->bus_ops->lmac_name);
-	if (ret) {
-		IWM_ERR(iwm, "LMAC loading failed\n");
-		goto out;
-	}
-
-	ret = iwm_notif_handle(iwm, UMAC_NOTIFY_OPCODE_INIT_COMPLETE,
-			       IWM_SRC_UMAC, WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Handle INIT_COMPLETE failed for LMAC: %d\n", ret);
-		goto out;
-	}
-
-	iwm_send_prio_table(iwm);
-	iwm_send_calib_results(iwm);
-	iwm_send_periodic_calib_cfg(iwm, periodic_calib_map);
-	iwm_send_ct_kill_cfg(iwm, iwm->conf.ct_kill_entry,
-			     iwm->conf.ct_kill_exit);
-
-	return 0;
-
- out:
-	iwm_eeprom_exit(iwm);
-	return ret;
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/fw.h b/drivers/net/wireless/iwmc3200wifi/fw.h
deleted file mode 100644
index c70a3b40dad..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/fw.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_FW_H__
-#define __IWM_FW_H__
-
-/**
- * struct iwm_fw_hdr_rec - An iwm firmware image is a
- * concatenation of various records. Each of them is
- * defined by an ID (aka op code), a length, and the
- * actual data.
- * @op_code: The record ID, see IWM_HDR_REC_OP_*
- *
- * @len: The record payload length
- *
- * @buf: The record payload
- */
-struct iwm_fw_hdr_rec {
-	u16 op_code;
-	u16 len;
-	u8 buf[0];
-};
-
-/* Header's definitions */
-#define IWM_HDR_LEN                          (512)
-#define IWM_HDR_BARKER_LEN                   (16)
-
-/* Header's opcodes */
-#define IWM_HDR_REC_OP_INVALID             (0x00)
-#define IWM_HDR_REC_OP_BUILD_DATE          (0x01)
-#define IWM_HDR_REC_OP_BUILD_TAG           (0x02)
-#define IWM_HDR_REC_OP_SW_VER              (0x03)
-#define IWM_HDR_REC_OP_HW_SKU              (0x04)
-#define IWM_HDR_REC_OP_BUILD_OPT           (0x05)
-#define IWM_HDR_REC_OP_MEM_DESC            (0x06)
-#define IWM_HDR_REC_USERDEFS               (0x07)
-
-/* Header's records length (in bytes) */
-#define IWM_HDR_REC_LEN_BUILD_DATE           (4)
-#define IWM_HDR_REC_LEN_BUILD_TAG            (64)
-#define IWM_HDR_REC_LEN_SW_VER               (4)
-#define IWM_HDR_REC_LEN_HW_SKU               (4)
-#define IWM_HDR_REC_LEN_BUILD_OPT            (4)
-#define IWM_HDR_REC_LEN_MEM_DESC             (12)
-#define IWM_HDR_REC_LEN_USERDEF              (64)
-
-#define IWM_BUILD_YEAR(date) ((date >> 16) & 0xffff)
-#define IWM_BUILD_MONTH(date) ((date >> 8) & 0xff)
-#define IWM_BUILD_DAY(date) (date & 0xff)
-
-struct iwm_fw_img_desc {
-	u32 offset;
-	u32 address;
-	u32 length;
-};
-
-struct iwm_fw_img_ver {
-	u8 minor;
-	u8 major;
-	u16 reserved;
-};
-
-int iwm_load_fw(struct iwm_priv *iwm);
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/hal.c b/drivers/net/wireless/iwmc3200wifi/hal.c
deleted file mode 100644
index 1cabcb39643..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/hal.c
+++ /dev/null
@@ -1,470 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-/*
- * Hardware Abstraction Layer for iwm.
- *
- * This file mostly defines an abstraction API for
- * sending various commands to the target.
- *
- * We have 2 types of commands: wifi and non-wifi ones.
- *
- * - wifi commands:
- *   They are used for sending LMAC and UMAC commands,
- *   and thus are the most commonly used ones.
- *   There are 2 different wifi command types, the regular
- *   one and the LMAC one. The former is used to send
- *   UMAC commands (see UMAC_CMD_OPCODE_* from umac.h)
- *   while the latter is used for sending commands to the
- *   LMAC. If you look at LMAC commands you'll se that they
- *   are actually regular iwlwifi target commands encapsulated
- *   into a special UMAC command called UMAC passthrough.
- *   This is due to the fact the host talks exclusively
- *   to the UMAC and so there needs to be a special UMAC
- *   command for talking to the LMAC.
- *   This is how a wifi command is laid out:
- *    ------------------------
- *   | iwm_udma_out_wifi_hdr  |
- *    ------------------------
- *   | SW meta_data (32 bits) |
- *    ------------------------
- *   | iwm_dev_cmd_hdr        |
- *    ------------------------
- *   | payload                |
- *   | ....                   |
- *
- * - non-wifi, or general commands:
- *   Those commands are handled by the device's bootrom,
- *   and are typically sent when the UMAC and the LMAC
- *   are not yet available.
- *    *   This is how a non-wifi command is laid out:
- *    ---------------------------
- *   | iwm_udma_out_nonwifi_hdr  |
- *    ---------------------------
- *   | payload                   |
- *   | ....                      |
-
- *
- * All the commands start with a UDMA header, which is
- * basically a 32 bits field. The 4 LSB there define
- * an opcode that allows the target to differentiate
- * between wifi (opcode is 0xf) and non-wifi commands
- * (opcode is [0..0xe]).
- *
- * When a command (wifi or non-wifi) is supposed to receive
- * an answer, we queue the command buffer. When we do receive
- * a command response from the UMAC, we go through the list
- * of pending command, and pass both the command and the answer
- * to the rx handler. Each command is sent with a unique
- * sequence id, and the answer is sent with the same one. This
- * is how we're supposed to match an answer with its command.
- * See rx.c:iwm_rx_handle_[non]wifi() and iwm_get_pending_[non]wifi()
- * for the implementation details.
- */
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/slab.h>
-
-#include "iwm.h"
-#include "bus.h"
-#include "hal.h"
-#include "umac.h"
-#include "debug.h"
-#include "trace.h"
-
-static int iwm_nonwifi_cmd_init(struct iwm_priv *iwm,
-				struct iwm_nonwifi_cmd *cmd,
-				struct iwm_udma_nonwifi_cmd *udma_cmd)
-{
-	INIT_LIST_HEAD(&cmd->pending);
-
-	spin_lock(&iwm->cmd_lock);
-
-	cmd->resp_received = 0;
-
-	cmd->seq_num = iwm->nonwifi_seq_num;
-	udma_cmd->seq_num = cpu_to_le16(cmd->seq_num);
-
-	iwm->nonwifi_seq_num++;
-	iwm->nonwifi_seq_num %= UMAC_NONWIFI_SEQ_NUM_MAX;
-
-	if (udma_cmd->resp)
-		list_add_tail(&cmd->pending, &iwm->nonwifi_pending_cmd);
-
-	spin_unlock(&iwm->cmd_lock);
-
-	cmd->buf.start = cmd->buf.payload;
-	cmd->buf.len = 0;
-
-	memcpy(&cmd->udma_cmd, udma_cmd, sizeof(*udma_cmd));
-
-	return cmd->seq_num;
-}
-
-u16 iwm_alloc_wifi_cmd_seq(struct iwm_priv *iwm)
-{
-	u16 seq_num = iwm->wifi_seq_num;
-
-	iwm->wifi_seq_num++;
-	iwm->wifi_seq_num %= UMAC_WIFI_SEQ_NUM_MAX;
-
-	return seq_num;
-}
-
-static void iwm_wifi_cmd_init(struct iwm_priv *iwm,
-			      struct iwm_wifi_cmd *cmd,
-			      struct iwm_udma_wifi_cmd *udma_cmd,
-			      struct iwm_umac_cmd *umac_cmd,
-			      struct iwm_lmac_cmd *lmac_cmd,
-			      u16 payload_size)
-{
-	INIT_LIST_HEAD(&cmd->pending);
-
-	spin_lock(&iwm->cmd_lock);
-
-	cmd->seq_num = iwm_alloc_wifi_cmd_seq(iwm);
-	umac_cmd->seq_num = cpu_to_le16(cmd->seq_num);
-
-	if (umac_cmd->resp)
-		list_add_tail(&cmd->pending, &iwm->wifi_pending_cmd);
-
-	spin_unlock(&iwm->cmd_lock);
-
-	cmd->buf.start = cmd->buf.payload;
-	cmd->buf.len = 0;
-
-	if (lmac_cmd) {
-		cmd->buf.start -= sizeof(struct iwm_lmac_hdr);
-
-		lmac_cmd->seq_num = cpu_to_le16(cmd->seq_num);
-		lmac_cmd->count = cpu_to_le16(payload_size);
-
-		memcpy(&cmd->lmac_cmd, lmac_cmd, sizeof(*lmac_cmd));
-
-		umac_cmd->count = cpu_to_le16(sizeof(struct iwm_lmac_hdr));
-	} else
-		umac_cmd->count = 0;
-
-	umac_cmd->count = cpu_to_le16(payload_size +
-				      le16_to_cpu(umac_cmd->count));
-	udma_cmd->count = cpu_to_le16(sizeof(struct iwm_umac_fw_cmd_hdr) +
-				      le16_to_cpu(umac_cmd->count));
-
-	memcpy(&cmd->udma_cmd, udma_cmd, sizeof(*udma_cmd));
-	memcpy(&cmd->umac_cmd, umac_cmd, sizeof(*umac_cmd));
-}
-
-void iwm_cmd_flush(struct iwm_priv *iwm)
-{
-	struct iwm_wifi_cmd *wcmd, *wnext;
-	struct iwm_nonwifi_cmd *nwcmd, *nwnext;
-
-	list_for_each_entry_safe(wcmd, wnext, &iwm->wifi_pending_cmd, pending) {
-		list_del(&wcmd->pending);
-		kfree(wcmd);
-	}
-
-	list_for_each_entry_safe(nwcmd, nwnext, &iwm->nonwifi_pending_cmd,
-				 pending) {
-		list_del(&nwcmd->pending);
-		kfree(nwcmd);
-	}
-}
-
-struct iwm_wifi_cmd *iwm_get_pending_wifi_cmd(struct iwm_priv *iwm, u16 seq_num)
-{
-	struct iwm_wifi_cmd *cmd;
-
-	list_for_each_entry(cmd, &iwm->wifi_pending_cmd, pending)
-		if (cmd->seq_num == seq_num) {
-			list_del(&cmd->pending);
-			return cmd;
-		}
-
-	return NULL;
-}
-
-struct iwm_nonwifi_cmd *iwm_get_pending_nonwifi_cmd(struct iwm_priv *iwm,
-						    u8 seq_num, u8 cmd_opcode)
-{
-	struct iwm_nonwifi_cmd *cmd;
-
-	list_for_each_entry(cmd, &iwm->nonwifi_pending_cmd, pending)
-		if ((cmd->seq_num == seq_num) &&
-		    (cmd->udma_cmd.opcode == cmd_opcode) &&
-		    (cmd->resp_received)) {
-			list_del(&cmd->pending);
-			return cmd;
-		}
-
-	return NULL;
-}
-
-static void iwm_build_udma_nonwifi_hdr(struct iwm_priv *iwm,
-				       struct iwm_udma_out_nonwifi_hdr *hdr,
-				       struct iwm_udma_nonwifi_cmd *cmd)
-{
-	memset(hdr, 0, sizeof(*hdr));
-
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_OPCODE, cmd->opcode);
-	SET_VAL32(hdr->cmd, UDMA_HDI_OUT_NW_CMD_RESP, cmd->resp);
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_EOT, 1);
-	SET_VAL32(hdr->cmd, UDMA_HDI_OUT_NW_CMD_HANDLE_BY_HW,
-		  cmd->handle_by_hw);
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_SIGNATURE, UMAC_HDI_OUT_SIGNATURE);
-	SET_VAL32(hdr->cmd, UDMA_HDI_OUT_CMD_NON_WIFI_HW_SEQ_NUM,
-		  le16_to_cpu(cmd->seq_num));
-
-	hdr->addr = cmd->addr;
-	hdr->op1_sz = cmd->op1_sz;
-	hdr->op2 = cmd->op2;
-}
-
-static int iwm_send_udma_nonwifi_cmd(struct iwm_priv *iwm,
-				     struct iwm_nonwifi_cmd *cmd)
-{
-	struct iwm_udma_out_nonwifi_hdr *udma_hdr;
-	struct iwm_nonwifi_cmd_buff *buf;
-	struct iwm_udma_nonwifi_cmd *udma_cmd = &cmd->udma_cmd;
-
-	buf = &cmd->buf;
-
-	buf->start -= sizeof(struct iwm_umac_nonwifi_out_hdr);
-	buf->len += sizeof(struct iwm_umac_nonwifi_out_hdr);
-
-	udma_hdr = (struct iwm_udma_out_nonwifi_hdr *)(buf->start);
-
-	iwm_build_udma_nonwifi_hdr(iwm, udma_hdr, udma_cmd);
-
-	IWM_DBG_CMD(iwm, DBG,
-		    "Send UDMA nonwifi cmd: opcode = 0x%x, resp = 0x%x, "
-		    "hw = 0x%x, seqnum = %d, addr = 0x%x, op1_sz = 0x%x, "
-		    "op2 = 0x%x\n", udma_cmd->opcode, udma_cmd->resp,
-		    udma_cmd->handle_by_hw, cmd->seq_num, udma_cmd->addr,
-		    udma_cmd->op1_sz, udma_cmd->op2);
-
-	trace_iwm_tx_nonwifi_cmd(iwm, udma_hdr);
-	return iwm_bus_send_chunk(iwm, buf->start, buf->len);
-}
-
-void iwm_udma_wifi_hdr_set_eop(struct iwm_priv *iwm, u8 *buf, u8 eop)
-{
-	struct iwm_udma_out_wifi_hdr *hdr = (struct iwm_udma_out_wifi_hdr *)buf;
-
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_EOT, eop);
-}
-
-void iwm_build_udma_wifi_hdr(struct iwm_priv *iwm,
-			     struct iwm_udma_out_wifi_hdr *hdr,
-			     struct iwm_udma_wifi_cmd *cmd)
-{
-	memset(hdr, 0, sizeof(*hdr));
-
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_OPCODE, UMAC_HDI_OUT_OPCODE_WIFI);
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_EOT, cmd->eop);
-	SET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_SIGNATURE, UMAC_HDI_OUT_SIGNATURE);
-
-	SET_VAL32(hdr->meta_data, UMAC_HDI_OUT_BYTE_COUNT,
-		  le16_to_cpu(cmd->count));
-	SET_VAL32(hdr->meta_data, UMAC_HDI_OUT_CREDIT_GRP, cmd->credit_group);
-	SET_VAL32(hdr->meta_data, UMAC_HDI_OUT_RATID, cmd->ra_tid);
-	SET_VAL32(hdr->meta_data, UMAC_HDI_OUT_LMAC_OFFSET, cmd->lmac_offset);
-}
-
-void iwm_build_umac_hdr(struct iwm_priv *iwm,
-			struct iwm_umac_fw_cmd_hdr *hdr,
-			struct iwm_umac_cmd *cmd)
-{
-	memset(hdr, 0, sizeof(*hdr));
-
-	SET_VAL32(hdr->meta_data, UMAC_FW_CMD_BYTE_COUNT,
-		  le16_to_cpu(cmd->count));
-	SET_VAL32(hdr->meta_data, UMAC_FW_CMD_TX_STA_COLOR, cmd->color);
-	SET_VAL8(hdr->cmd.flags, UMAC_DEV_CMD_FLAGS_RESP_REQ, cmd->resp);
-
-	hdr->cmd.cmd = cmd->id;
-	hdr->cmd.seq_num = cmd->seq_num;
-}
-
-static int iwm_send_udma_wifi_cmd(struct iwm_priv *iwm,
-				  struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_wifi_out_hdr *umac_hdr;
-	struct iwm_wifi_cmd_buff *buf;
-	struct iwm_udma_wifi_cmd *udma_cmd = &cmd->udma_cmd;
-	struct iwm_umac_cmd *umac_cmd = &cmd->umac_cmd;
-	int ret;
-
-	buf = &cmd->buf;
-
-	buf->start -= sizeof(struct iwm_umac_wifi_out_hdr);
-	buf->len += sizeof(struct iwm_umac_wifi_out_hdr);
-
-	umac_hdr = (struct iwm_umac_wifi_out_hdr *)(buf->start);
-
-	iwm_build_udma_wifi_hdr(iwm, &umac_hdr->hw_hdr, udma_cmd);
-	iwm_build_umac_hdr(iwm, &umac_hdr->sw_hdr, umac_cmd);
-
-	IWM_DBG_CMD(iwm, DBG,
-		    "Send UDMA wifi cmd: opcode = 0x%x, UMAC opcode = 0x%x, "
-		    "eop = 0x%x, count = 0x%x, credit_group = 0x%x, "
-		    "ra_tid = 0x%x, lmac_offset = 0x%x, seqnum = %d\n",
-		    UMAC_HDI_OUT_OPCODE_WIFI, umac_cmd->id,
-		    udma_cmd->eop, udma_cmd->count, udma_cmd->credit_group,
-		    udma_cmd->ra_tid, udma_cmd->lmac_offset, cmd->seq_num);
-
-	if (umac_cmd->id == UMAC_CMD_OPCODE_WIFI_PASS_THROUGH)
-		IWM_DBG_CMD(iwm, DBG, "\tLMAC opcode: 0x%x\n",
-			    cmd->lmac_cmd.id);
-
-	ret = iwm_tx_credit_alloc(iwm, udma_cmd->credit_group, buf->len);
-
-	/* We keep sending UMAC reset regardless of the command credits.
-	 * The UMAC is supposed to be reset anyway and the Tx credits are
-	 * reinitialized afterwards. If we are lucky, the reset could
-	 * still be done even though we have run out of credits for the
-	 * command pool at this moment.*/
-	if (ret && (umac_cmd->id != UMAC_CMD_OPCODE_RESET)) {
-		IWM_DBG_TX(iwm, DBG, "Failed to alloc tx credit for cmd %d\n",
-			   umac_cmd->id);
-		return ret;
-	}
-
-	trace_iwm_tx_wifi_cmd(iwm, umac_hdr);
-	return iwm_bus_send_chunk(iwm, buf->start, buf->len);
-}
-
-/* target_cmd a.k.a udma_nonwifi_cmd can be sent when UMAC is not available */
-int iwm_hal_send_target_cmd(struct iwm_priv *iwm,
-			    struct iwm_udma_nonwifi_cmd *udma_cmd,
-			    const void *payload)
-{
-	struct iwm_nonwifi_cmd *cmd;
-	int ret, seq_num;
-
-	cmd = kzalloc(sizeof(struct iwm_nonwifi_cmd), GFP_KERNEL);
-	if (!cmd) {
-		IWM_ERR(iwm, "Couldn't alloc memory for hal cmd\n");
-		return -ENOMEM;
-	}
-
-	seq_num = iwm_nonwifi_cmd_init(iwm, cmd, udma_cmd);
-
-	if (cmd->udma_cmd.opcode == UMAC_HDI_OUT_OPCODE_WRITE ||
-	    cmd->udma_cmd.opcode == UMAC_HDI_OUT_OPCODE_WRITE_PERSISTENT) {
-		cmd->buf.len = le32_to_cpu(cmd->udma_cmd.op1_sz);
-		memcpy(&cmd->buf.payload, payload, cmd->buf.len);
-	}
-
-	ret = iwm_send_udma_nonwifi_cmd(iwm, cmd);
-
-	if (!udma_cmd->resp)
-		kfree(cmd);
-
-	if (ret < 0)
-		return ret;
-
-	return seq_num;
-}
-
-static void iwm_build_lmac_hdr(struct iwm_priv *iwm, struct iwm_lmac_hdr *hdr,
-			       struct iwm_lmac_cmd *cmd)
-{
-	memset(hdr, 0, sizeof(*hdr));
-
-	hdr->id = cmd->id;
-	hdr->flags = 0; /* Is this ever used? */
-	hdr->seq_num = cmd->seq_num;
-}
-
-/*
- * iwm_hal_send_host_cmd(): sends commands to the UMAC or the LMAC.
- * Sending command to the LMAC is equivalent to sending a
- * regular UMAC command with the LMAC passthrough or the LMAC
- * wrapper UMAC command IDs.
- */
-int iwm_hal_send_host_cmd(struct iwm_priv *iwm,
-			  struct iwm_udma_wifi_cmd *udma_cmd,
-			  struct iwm_umac_cmd *umac_cmd,
-			  struct iwm_lmac_cmd *lmac_cmd,
-			  const void *payload, u16 payload_size)
-{
-	struct iwm_wifi_cmd *cmd;
-	struct iwm_lmac_hdr *hdr;
-	int lmac_hdr_len = 0;
-	int ret;
-
-	cmd = kzalloc(sizeof(struct iwm_wifi_cmd), GFP_KERNEL);
-	if (!cmd) {
-		IWM_ERR(iwm, "Couldn't alloc memory for wifi hal cmd\n");
-		return -ENOMEM;
-	}
-
-	iwm_wifi_cmd_init(iwm, cmd, udma_cmd, umac_cmd, lmac_cmd, payload_size);
-
-	if (lmac_cmd) {
-		hdr = (struct iwm_lmac_hdr *)(cmd->buf.start);
-
-		iwm_build_lmac_hdr(iwm, hdr, &cmd->lmac_cmd);
-		lmac_hdr_len = sizeof(struct iwm_lmac_hdr);
-	}
-
-	memcpy(cmd->buf.payload, payload, payload_size);
-	cmd->buf.len = le16_to_cpu(umac_cmd->count);
-
-	ret = iwm_send_udma_wifi_cmd(iwm, cmd);
-
-	/* We free the cmd if we're not expecting any response */
-	if (!umac_cmd->resp)
-		kfree(cmd);
-	return ret;
-}
-
-/*
- * iwm_hal_send_umac_cmd(): This is a special case for
- * iwm_hal_send_host_cmd() to send direct UMAC cmd (without
- * LMAC involved).
- */
-int iwm_hal_send_umac_cmd(struct iwm_priv *iwm,
-			  struct iwm_udma_wifi_cmd *udma_cmd,
-			  struct iwm_umac_cmd *umac_cmd,
-			  const void *payload, u16 payload_size)
-{
-	return iwm_hal_send_host_cmd(iwm, udma_cmd, umac_cmd, NULL,
-				     payload, payload_size);
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/hal.h b/drivers/net/wireless/iwmc3200wifi/hal.h
deleted file mode 100644
index c20936d9b6b..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/hal.h
+++ /dev/null
@@ -1,237 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef _IWM_HAL_H_
-#define _IWM_HAL_H_
-
-#include "umac.h"
-
-#define GET_VAL8(s, name)	((s >> name##_POS) & name##_SEED)
-#define GET_VAL16(s, name)	((le16_to_cpu(s) >> name##_POS) & name##_SEED)
-#define GET_VAL32(s, name)	((le32_to_cpu(s) >> name##_POS) & name##_SEED)
-
-#define SET_VAL8(s, name, val)						  \
-do {									  \
-	s = (s & ~(name##_SEED << name##_POS)) |			  \
-	    ((val & name##_SEED) << name##_POS);			  \
-} while (0)
-
-#define SET_VAL16(s, name, val)						  \
-do {									  \
-	s = cpu_to_le16((le16_to_cpu(s) & ~(name##_SEED << name##_POS)) | \
-			((val & name##_SEED) << name##_POS));		  \
-} while (0)
-
-#define SET_VAL32(s, name, val)						  \
-do {									  \
-	s = cpu_to_le32((le32_to_cpu(s) & ~(name##_SEED << name##_POS)) | \
-			((val & name##_SEED) << name##_POS));		  \
-} while (0)
-
-
-#define UDMA_UMAC_INIT	{	.eop = 1,				  \
-				.credit_group = 0x4,			  \
-				.ra_tid = UMAC_HDI_ACT_TBL_IDX_HOST_CMD,  \
-				.lmac_offset = 0 }
-#define UDMA_LMAC_INIT	{	.eop = 1,				  \
-				.credit_group = 0x4,			  \
-				.ra_tid = UMAC_HDI_ACT_TBL_IDX_HOST_CMD,  \
-				.lmac_offset = 4 }
-
-
-/* UDMA IN OP CODE -- cmd bits [3:0] */
-#define UDMA_HDI_IN_NW_CMD_OPCODE_POS		0
-#define UDMA_HDI_IN_NW_CMD_OPCODE_SEED		0xF
-
-#define UDMA_IN_OPCODE_GENERAL_RESP		0x0
-#define UDMA_IN_OPCODE_READ_RESP		0x1
-#define UDMA_IN_OPCODE_WRITE_RESP		0x2
-#define UDMA_IN_OPCODE_PERS_WRITE_RESP		0x5
-#define UDMA_IN_OPCODE_PERS_READ_RESP		0x6
-#define UDMA_IN_OPCODE_RD_MDFY_WR_RESP		0x7
-#define UDMA_IN_OPCODE_EP_MNGMT_MSG		0x8
-#define UDMA_IN_OPCODE_CRDT_CHNG_MSG		0x9
-#define UDMA_IN_OPCODE_CNTRL_DATABASE_MSG	0xA
-#define UDMA_IN_OPCODE_SW_MSG			0xB
-#define UDMA_IN_OPCODE_WIFI			0xF
-#define UDMA_IN_OPCODE_WIFI_LMAC		0x1F
-#define UDMA_IN_OPCODE_WIFI_UMAC		0x2F
-
-/* HW API: udma_hdi_nonwifi API (OUT and IN) */
-
-/* iwm_udma_nonwifi_cmd request response -- bits [9:9] */
-#define UDMA_HDI_OUT_NW_CMD_RESP_POS		9
-#define UDMA_HDI_OUT_NW_CMD_RESP_SEED		0x1
-
-/* iwm_udma_nonwifi_cmd handle by HW -- bits [11:11] */
-#define UDMA_HDI_OUT_NW_CMD_HANDLE_BY_HW_POS	11
-#define UDMA_HDI_OUT_NW_CMD_HANDLE_BY_HW_SEED	0x1
-
-/* iwm_udma_nonwifi_cmd sequence-number -- bits [12:15] */
-#define UDMA_HDI_OUT_NW_CMD_SEQ_NUM_POS		12
-#define UDMA_HDI_OUT_NW_CMD_SEQ_NUM_SEED	0xF
-
-/* UDMA IN Non-WIFI HW sequence number -- bits [12:15] */
-#define UDMA_IN_NW_HW_SEQ_NUM_POS		12
-#define UDMA_IN_NW_HW_SEQ_NUM_SEED		0xF
-
-/* UDMA IN Non-WIFI HW signature -- bits [16:31] */
-#define UDMA_IN_NW_HW_SIG_POS			16
-#define UDMA_IN_NW_HW_SIG_SEED			0xFFFF
-
-/* fixed signature */
-#define UDMA_IN_NW_HW_SIG			0xCBBC
-
-/* UDMA IN Non-WIFI HW block length -- bits [32:35] */
-#define UDMA_IN_NW_HW_LENGTH_SEED		0xF
-#define UDMA_IN_NW_HW_LENGTH_POS		32
-
-/* End of HW API: udma_hdi_nonwifi API (OUT and IN) */
-
-#define IWM_SDIO_FW_MAX_CHUNK_SIZE	2032
-#define IWM_MAX_WIFI_HEADERS_SIZE	32
-#define IWM_MAX_NONWIFI_HEADERS_SIZE	16
-#define IWM_MAX_NONWIFI_CMD_BUFF_SIZE	(IWM_SDIO_FW_MAX_CHUNK_SIZE - \
-					 IWM_MAX_NONWIFI_HEADERS_SIZE)
-#define IWM_MAX_WIFI_CMD_BUFF_SIZE	(IWM_SDIO_FW_MAX_CHUNK_SIZE - \
-					 IWM_MAX_WIFI_HEADERS_SIZE)
-
-#define IWM_HAL_CONCATENATE_BUF_SIZE	(32 * 1024)
-
-struct iwm_wifi_cmd_buff {
-	u16 len;
-	u8 *start;
-	u8 hdr[IWM_MAX_WIFI_HEADERS_SIZE];
-	u8 payload[IWM_MAX_WIFI_CMD_BUFF_SIZE];
-};
-
-struct iwm_nonwifi_cmd_buff {
-	u16 len;
-	u8 *start;
-	u8 hdr[IWM_MAX_NONWIFI_HEADERS_SIZE];
-	u8 payload[IWM_MAX_NONWIFI_CMD_BUFF_SIZE];
-};
-
-struct iwm_udma_nonwifi_cmd {
-	u8 opcode;
-	u8 eop;
-	u8 resp;
-	u8 handle_by_hw;
-	__le32 addr;
-	__le32 op1_sz;
-	__le32 op2;
-	__le16 seq_num;
-};
-
-struct iwm_udma_wifi_cmd {
-	__le16 count;
-	u8 eop;
-	u8 credit_group;
-	u8 ra_tid;
-	u8 lmac_offset;
-};
-
-struct iwm_umac_cmd {
-	u8 id;
-	__le16 count;
-	u8 resp;
-	__le16 seq_num;
-	u8 color;
-};
-
-struct iwm_lmac_cmd {
-	u8 id;
-	__le16 count;
-	u8 resp;
-	__le16 seq_num;
-};
-
-struct iwm_nonwifi_cmd {
-	u16 seq_num;
-	bool resp_received;
-	struct list_head pending;
-	struct iwm_udma_nonwifi_cmd udma_cmd;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_lmac_cmd lmac_cmd;
-	struct iwm_nonwifi_cmd_buff buf;
-	u32 flags;
-};
-
-struct iwm_wifi_cmd {
-	u16 seq_num;
-	struct list_head pending;
-	struct iwm_udma_wifi_cmd udma_cmd;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_lmac_cmd lmac_cmd;
-	struct iwm_wifi_cmd_buff buf;
-	u32 flags;
-};
-
-void iwm_cmd_flush(struct iwm_priv *iwm);
-
-struct iwm_wifi_cmd *iwm_get_pending_wifi_cmd(struct iwm_priv *iwm,
-					      u16 seq_num);
-struct iwm_nonwifi_cmd *iwm_get_pending_nonwifi_cmd(struct iwm_priv *iwm,
-						    u8 seq_num, u8 cmd_opcode);
-
-
-int iwm_hal_send_target_cmd(struct iwm_priv *iwm,
-			    struct iwm_udma_nonwifi_cmd *ucmd,
-			    const void *payload);
-
-int iwm_hal_send_host_cmd(struct iwm_priv *iwm,
-			  struct iwm_udma_wifi_cmd *udma_cmd,
-			  struct iwm_umac_cmd *umac_cmd,
-			  struct iwm_lmac_cmd *lmac_cmd,
-			  const void *payload, u16 payload_size);
-
-int iwm_hal_send_umac_cmd(struct iwm_priv *iwm,
-			  struct iwm_udma_wifi_cmd *udma_cmd,
-			  struct iwm_umac_cmd *umac_cmd,
-			  const void *payload, u16 payload_size);
-
-u16 iwm_alloc_wifi_cmd_seq(struct iwm_priv *iwm);
-
-void iwm_udma_wifi_hdr_set_eop(struct iwm_priv *iwm, u8 *buf, u8 eop);
-void iwm_build_udma_wifi_hdr(struct iwm_priv *iwm,
-			     struct iwm_udma_out_wifi_hdr *hdr,
-			     struct iwm_udma_wifi_cmd *cmd);
-void iwm_build_umac_hdr(struct iwm_priv *iwm,
-			struct iwm_umac_fw_cmd_hdr *hdr,
-			struct iwm_umac_cmd *cmd);
-#endif /* _IWM_HAL_H_ */
diff --git a/drivers/net/wireless/iwmc3200wifi/iwm.h b/drivers/net/wireless/iwmc3200wifi/iwm.h
deleted file mode 100644
index 51d7efa15ae..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/iwm.h
+++ /dev/null
@@ -1,367 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_H__
-#define __IWM_H__
-
-#include <linux/netdevice.h>
-#include <linux/wireless.h>
-#include <net/cfg80211.h>
-
-#include "debug.h"
-#include "hal.h"
-#include "umac.h"
-#include "lmac.h"
-#include "eeprom.h"
-#include "trace.h"
-
-#define IWM_COPYRIGHT "Copyright(c) 2009 Intel Corporation"
-#define IWM_AUTHOR "<ilw@linux.intel.com>"
-
-#define IWM_SRC_LMAC	UMAC_HDI_IN_SOURCE_FHRX
-#define IWM_SRC_UDMA	UMAC_HDI_IN_SOURCE_UDMA
-#define IWM_SRC_UMAC	UMAC_HDI_IN_SOURCE_FW
-#define IWM_SRC_NUM	3
-
-#define IWM_POWER_INDEX_MIN	0
-#define IWM_POWER_INDEX_MAX	5
-#define IWM_POWER_INDEX_DEFAULT	3
-
-struct iwm_conf {
-	u32 sdio_ior_timeout;
-	unsigned long calib_map;
-	unsigned long expected_calib_map;
-	u8 ct_kill_entry;
-	u8 ct_kill_exit;
-	bool reset_on_fatal_err;
-	bool auto_connect;
-	bool wimax_not_present;
-	bool enable_qos;
-	u32 mode;
-
-	u32 power_index;
-	u32 frag_threshold;
-	u32 rts_threshold;
-	bool cts_to_self;
-
-	u32 assoc_timeout;
-	u32 roam_timeout;
-	u32 wireless_mode;
-
-	u8 ibss_band;
-	u8 ibss_channel;
-
-	u8 mac_addr[ETH_ALEN];
-};
-
-enum {
-	COEX_MODE_SA = 1,
-	COEX_MODE_XOR,
-	COEX_MODE_CM,
-	COEX_MODE_MAX,
-};
-
-struct iwm_if_ops;
-struct iwm_wifi_cmd;
-
-struct pool_entry {
-	int id;		/* group id */
-	int sid;	/* super group id */
-	int min_pages;	/* min capacity in pages */
-	int max_pages;	/* max capacity in pages */
-	int alloc_pages;	/* allocated # of pages. incresed by driver */
-	int total_freed_pages;	/* total freed # of pages. incresed by UMAC */
-};
-
-struct spool_entry {
-	int id;
-	int max_pages;
-	int alloc_pages;
-};
-
-struct iwm_tx_credit {
-	spinlock_t lock;
-	int pool_nr;
-	unsigned long full_pools_map; /* bitmap for # of filled tx pools */
-	struct pool_entry pools[IWM_MACS_OUT_GROUPS];
-	struct spool_entry spools[IWM_MACS_OUT_SGROUPS];
-};
-
-struct iwm_notif {
-	struct list_head pending;
-	u32 cmd_id;
-	void *cmd;
-	u8 src;
-	void *buf;
-	unsigned long buf_size;
-};
-
-struct iwm_tid_info {
-	__le16 last_seq_num;
-	bool stopped;
-	struct mutex mutex;
-};
-
-struct iwm_sta_info {
-	u8 addr[ETH_ALEN];
-	bool valid;
-	bool qos;
-	u8 color;
-	struct iwm_tid_info tid_info[IWM_UMAC_TID_NR];
-};
-
-struct iwm_tx_info {
-	u8 sta;
-	u8 color;
-	u8 tid;
-};
-
-struct iwm_rx_info {
-	unsigned long rx_size;
-	unsigned long rx_buf_size;
-};
-
-#define IWM_NUM_KEYS 4
-
-struct iwm_umac_key_hdr {
-	u8 mac[ETH_ALEN];
-	u8 key_idx;
-	u8 multicast; /* BCast encrypt & BCast decrypt of frames FROM mac */
-} __packed;
-
-struct iwm_key {
-	struct iwm_umac_key_hdr hdr;
-	u32 cipher;
-	u8 key[WLAN_MAX_KEY_LEN];
-	u8 seq[IW_ENCODE_SEQ_MAX_SIZE];
-	int key_len;
-	int seq_len;
-};
-
-#define IWM_RX_ID_HASH  0xff
-#define IWM_RX_ID_GET_HASH(id) ((id) % IWM_RX_ID_HASH)
-
-#define IWM_STA_TABLE_NUM	16
-#define IWM_TX_LIST_SIZE	64
-#define IWM_RX_LIST_SIZE        256
-
-#define IWM_SCAN_ID_MAX 0xff
-
-#define IWM_STATUS_READY		0
-#define IWM_STATUS_SCANNING		1
-#define IWM_STATUS_SCAN_ABORTING	2
-#define IWM_STATUS_SME_CONNECTING	3
-#define IWM_STATUS_ASSOCIATED		4
-#define IWM_STATUS_RESETTING		5
-
-struct iwm_tx_queue {
-	int id;
-	struct sk_buff_head queue;
-	struct sk_buff_head stopped_queue;
-	spinlock_t lock;
-	struct workqueue_struct *wq;
-	struct work_struct worker;
-	u8 concat_buf[IWM_HAL_CONCATENATE_BUF_SIZE];
-	int concat_count;
-	u8 *concat_ptr;
-};
-
-/* Queues 0 ~ 3 for AC data, 5 for iPAN */
-#define IWM_TX_QUEUES		5
-#define IWM_TX_DATA_QUEUES	4
-#define IWM_TX_CMD_QUEUE	4
-
-struct iwm_bss_info {
-	struct list_head node;
-	struct cfg80211_bss *cfg_bss;
-	struct iwm_umac_notif_bss_info *bss;
-};
-
-typedef int (*iwm_handler)(struct iwm_priv *priv, u8 *buf,
-			   unsigned long buf_size, struct iwm_wifi_cmd *cmd);
-
-#define IWM_WATCHDOG_PERIOD	(6 * HZ)
-
-struct iwm_priv {
-	struct wireless_dev *wdev;
-	struct iwm_if_ops *bus_ops;
-
-	struct iwm_conf conf;
-
-	unsigned long status;
-
-	struct list_head pending_notif;
-	wait_queue_head_t notif_queue;
-
-	wait_queue_head_t nonwifi_queue;
-
-	unsigned long calib_done_map;
-	struct {
-		u8 *buf;
-		u32 size;
-	} calib_res[CALIBRATION_CMD_NUM];
-
-	struct iwm_umac_profile *umac_profile;
-	bool umac_profile_active;
-
-	u8 bssid[ETH_ALEN];
-	u8 channel;
-	u16 rate;
-	u32 txpower;
-
-	struct iwm_sta_info sta_table[IWM_STA_TABLE_NUM];
-	struct list_head bss_list;
-
-	void (*nonwifi_rx_handlers[UMAC_HDI_IN_OPCODE_NONWIFI_MAX])
-	(struct iwm_priv *priv, u8 *buf, unsigned long buf_size);
-
-	const iwm_handler *umac_handlers;
-	const iwm_handler *lmac_handlers;
-	DECLARE_BITMAP(lmac_handler_map, LMAC_COMMAND_ID_NUM);
-	DECLARE_BITMAP(umac_handler_map, LMAC_COMMAND_ID_NUM);
-	DECLARE_BITMAP(udma_handler_map, LMAC_COMMAND_ID_NUM);
-
-	struct list_head wifi_pending_cmd;
-	struct list_head nonwifi_pending_cmd;
-	u16 wifi_seq_num;
-	u8 nonwifi_seq_num;
-	spinlock_t cmd_lock;
-
-	u32 core_enabled;
-
-	u8 scan_id;
-	struct cfg80211_scan_request *scan_request;
-
-	struct sk_buff_head rx_list;
-	struct list_head rx_tickets;
-	spinlock_t ticket_lock;
-	struct list_head rx_packets[IWM_RX_ID_HASH];
-	spinlock_t packet_lock[IWM_RX_ID_HASH];
-	struct workqueue_struct *rx_wq;
-	struct work_struct rx_worker;
-
-	struct iwm_tx_credit tx_credit;
-	struct iwm_tx_queue txq[IWM_TX_QUEUES];
-
-	struct iwm_key keys[IWM_NUM_KEYS];
-	s8 default_key;
-
-	DECLARE_BITMAP(wifi_ntfy, WIFI_IF_NTFY_MAX);
-	wait_queue_head_t wifi_ntfy_queue;
-
-	wait_queue_head_t mlme_queue;
-
-	struct iw_statistics wstats;
-	struct delayed_work stats_request;
-	struct delayed_work disconnect;
-	struct delayed_work ct_kill_delay;
-
-	struct iwm_debugfs dbg;
-
-	u8 *eeprom;
-	struct timer_list watchdog;
-	struct work_struct reset_worker;
-	struct work_struct auth_retry_worker;
-	struct mutex mutex;
-
-	u8 *req_ie;
-	int req_ie_len;
-	u8 *resp_ie;
-	int resp_ie_len;
-
-	struct iwm_fw_error_hdr *last_fw_err;
-	char umac_version[8];
-	char lmac_version[8];
-
-	char private[0] __attribute__((__aligned__(NETDEV_ALIGN)));
-};
-
-static inline void *iwm_private(struct iwm_priv *iwm)
-{
-	BUG_ON(!iwm);
-	return &iwm->private;
-}
-
-#define hw_to_iwm(h) (h->iwm)
-#define iwm_to_dev(i) (wiphy_dev(i->wdev->wiphy))
-#define iwm_to_wiphy(i) (i->wdev->wiphy)
-#define wiphy_to_iwm(w) (struct iwm_priv *)(wiphy_priv(w))
-#define iwm_to_wdev(i) (i->wdev)
-#define wdev_to_iwm(w) (struct iwm_priv *)(wdev_priv(w))
-#define iwm_to_ndev(i) (i->wdev->netdev)
-#define ndev_to_iwm(n) (wdev_to_iwm(n->ieee80211_ptr))
-#define skb_to_rx_info(s) ((struct iwm_rx_info *)(s->cb))
-#define skb_to_tx_info(s) ((struct iwm_tx_info *)s->cb)
-
-void *iwm_if_alloc(int sizeof_bus, struct device *dev,
-		   struct iwm_if_ops *if_ops);
-void iwm_if_free(struct iwm_priv *iwm);
-int iwm_if_add(struct iwm_priv *iwm);
-void iwm_if_remove(struct iwm_priv *iwm);
-int iwm_mode_to_nl80211_iftype(int mode);
-int iwm_priv_init(struct iwm_priv *iwm);
-void iwm_priv_deinit(struct iwm_priv *iwm);
-void iwm_reset(struct iwm_priv *iwm);
-void iwm_resetting(struct iwm_priv *iwm);
-void iwm_tx_credit_init_pools(struct iwm_priv *iwm,
-			      struct iwm_umac_notif_alive *alive);
-int iwm_tx_credit_alloc(struct iwm_priv *iwm, int id, int nb);
-int iwm_notif_send(struct iwm_priv *iwm, struct iwm_wifi_cmd *cmd,
-		   u8 cmd_id, u8 source, u8 *buf, unsigned long buf_size);
-int iwm_notif_handle(struct iwm_priv *iwm, u32 cmd, u8 source, long timeout);
-void iwm_init_default_profile(struct iwm_priv *iwm,
-			      struct iwm_umac_profile *profile);
-void iwm_link_on(struct iwm_priv *iwm);
-void iwm_link_off(struct iwm_priv *iwm);
-int iwm_up(struct iwm_priv *iwm);
-int iwm_down(struct iwm_priv *iwm);
-
-/* TX API */
-int iwm_tid_to_queue(u16 tid);
-void iwm_tx_credit_inc(struct iwm_priv *iwm, int id, int total_freed_pages);
-void iwm_tx_worker(struct work_struct *work);
-int iwm_xmit_frame(struct sk_buff *skb, struct net_device *netdev);
-
-/* RX API */
-void iwm_rx_setup_handlers(struct iwm_priv *iwm);
-int iwm_rx_handle(struct iwm_priv *iwm, u8 *buf, unsigned long buf_size);
-int iwm_rx_handle_resp(struct iwm_priv *iwm, u8 *buf, unsigned long buf_size,
-		       struct iwm_wifi_cmd *cmd);
-void iwm_rx_free(struct iwm_priv *iwm);
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/lmac.h b/drivers/net/wireless/iwmc3200wifi/lmac.h
deleted file mode 100644
index 5ddcdf8c70c..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/lmac.h
+++ /dev/null
@@ -1,484 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_LMAC_H__
-#define __IWM_LMAC_H__
-
-struct iwm_lmac_hdr {
-	u8 id;
-	u8 flags;
-	__le16 seq_num;
-} __packed;
-
-/* LMAC commands */
-#define CALIB_CFG_FLAG_SEND_COMPLETE_NTFY_AFTER_MSK  0x1
-
-struct iwm_lmac_cal_cfg_elt {
-	__le32 enable; /* 1 means LMAC needs to do something */
-	__le32 start;  /* 1 to start calibration, 0 to stop */
-	__le32 send_res; /* 1 for sending back results */
-	__le32 apply_res; /* 1 for applying calibration results to HW */
-	__le32 reserved;
-} __packed;
-
-struct iwm_lmac_cal_cfg_status {
-	struct iwm_lmac_cal_cfg_elt init;
-	struct iwm_lmac_cal_cfg_elt periodic;
-	__le32 flags; /* CALIB_CFG_FLAG_SEND_COMPLETE_NTFY_AFTER_MSK */
-} __packed;
-
-struct iwm_lmac_cal_cfg_cmd {
-	struct iwm_lmac_cal_cfg_status ucode_cfg;
-	struct iwm_lmac_cal_cfg_status driver_cfg;
-	__le32 reserved;
-} __packed;
-
-struct iwm_lmac_cal_cfg_resp {
-	__le32 status;
-} __packed;
-
-#define IWM_CARD_STATE_SW_HW_ENABLED	0x00
-#define IWM_CARD_STATE_HW_DISABLED	0x01
-#define IWM_CARD_STATE_SW_DISABLED	0x02
-#define IWM_CARD_STATE_CTKILL_DISABLED	0x04
-#define IWM_CARD_STATE_IS_RXON		0x10
-
-struct iwm_lmac_card_state {
-	__le32 flags;
-} __packed;
-
-/**
- * COEX_PRIORITY_TABLE_CMD
- *
- * Priority entry for each state
- * Will keep two tables, for STA and WIPAN
- */
-enum {
-	/* UN-ASSOCIATION PART */
-	COEX_UNASSOC_IDLE = 0,
-	COEX_UNASSOC_MANUAL_SCAN,
-	COEX_UNASSOC_AUTO_SCAN,
-
-	/* CALIBRATION */
-	COEX_CALIBRATION,
-	COEX_PERIODIC_CALIBRATION,
-
-	/* CONNECTION */
-	COEX_CONNECTION_ESTAB,
-
-	/* ASSOCIATION PART */
-	COEX_ASSOCIATED_IDLE,
-	COEX_ASSOC_MANUAL_SCAN,
-	COEX_ASSOC_AUTO_SCAN,
-	COEX_ASSOC_ACTIVE_LEVEL,
-
-	/* RF ON/OFF */
-	COEX_RF_ON,
-	COEX_RF_OFF,
-	COEX_STAND_ALONE_DEBUG,
-
-	/* IPNN */
-	COEX_IPAN_ASSOC_LEVEL,
-
-	/* RESERVED */
-	COEX_RSRVD1,
-	COEX_RSRVD2,
-
-	COEX_EVENTS_NUM
-};
-
-#define COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK	0x1
-#define COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK	0x2
-#define COEX_EVT_FLAG_DELAY_MEDIUM_FREE_NTFY_MSK	0x4
-
-struct coex_event {
-	u8 req_prio;
-	u8 win_med_prio;
-	u8 reserved;
-	u8 flags;
-} __packed;
-
-#define COEX_FLAGS_STA_TABLE_VALID_MSK		0x1
-#define COEX_FLAGS_UNASSOC_WAKEUP_UMASK_MSK	0x4
-#define COEX_FLAGS_ASSOC_WAKEUP_UMASK_MSK	0x8
-#define COEX_FLAGS_COEX_ENABLE_MSK		0x80
-
-struct iwm_coex_prio_table_cmd {
-	u8 flags;
-	u8 reserved[3];
-	struct coex_event sta_prio[COEX_EVENTS_NUM];
-} __packed;
-
-/* Coexistence definitions
- *
- * Constants to fill in the Priorities' Tables
- * RP - Requested Priority
- * WP - Win Medium Priority: priority assigned when the contention has been won
- * FLAGS - Combination of COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK and
- * 	   COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK
- */
-
-#define COEX_UNASSOC_IDLE_FLAGS		0
-#define COEX_UNASSOC_MANUAL_SCAN_FLAGS	(COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-					 COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK)
-#define COEX_UNASSOC_AUTO_SCAN_FLAGS	(COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-					 COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK)
-#define COEX_CALIBRATION_FLAGS		(COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-					 COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK)
-#define COEX_PERIODIC_CALIBRATION_FLAGS	0
-/* COEX_CONNECTION_ESTAB: we need DELAY_MEDIUM_FREE_NTFY to let WiMAX
- * disconnect from network. */
-#define COEX_CONNECTION_ESTAB_FLAGS (COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-				     COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK | \
-				     COEX_EVT_FLAG_DELAY_MEDIUM_FREE_NTFY_MSK)
-#define COEX_ASSOCIATED_IDLE_FLAGS	0
-#define COEX_ASSOC_MANUAL_SCAN_FLAGS	(COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-					 COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK)
-#define COEX_ASSOC_AUTO_SCAN_FLAGS	(COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-					 COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK)
-#define COEX_ASSOC_ACTIVE_LEVEL_FLAGS	0
-#define COEX_RF_ON_FLAGS		0
-#define COEX_RF_OFF_FLAGS		0
-#define COEX_STAND_ALONE_DEBUG_FLAGS	(COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-					 COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK)
-#define COEX_IPAN_ASSOC_LEVEL_FLAGS (COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-				     COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK | \
-				     COEX_EVT_FLAG_DELAY_MEDIUM_FREE_NTFY_MSK)
-#define COEX_RSRVD1_FLAGS		0
-#define COEX_RSRVD2_FLAGS		0
-/* XOR_RF_ON is the event wrapping all radio ownership. We need
- * DELAY_MEDIUM_FREE_NTFY to let WiMAX disconnect from network. */
-#define COEX_XOR_RF_ON_FLAGS	    (COEX_EVT_FLAG_MEDIUM_FREE_NTFY_MSK | \
-				     COEX_EVT_FLAG_MEDIUM_ACTV_NTFY_MSK | \
-				     COEX_EVT_FLAG_DELAY_MEDIUM_FREE_NTFY_MSK)
-
-/* CT kill config command */
-struct iwm_ct_kill_cfg_cmd {
-	u32 exit_threshold;
-	u32 reserved;
-	u32 entry_threshold;
-} __packed;
-
-
-/* LMAC OP CODES */
-#define REPLY_PAD			0x0
-#define REPLY_ALIVE			0x1
-#define REPLY_ERROR			0x2
-#define REPLY_ECHO			0x3
-#define REPLY_HALT			0x6
-
-/* RXON state commands */
-#define REPLY_RX_ON			0x10
-#define REPLY_RX_ON_ASSOC		0x11
-#define REPLY_RX_OFF			0x12
-#define REPLY_QOS_PARAM			0x13
-#define REPLY_RX_ON_TIMING		0x14
-#define REPLY_INTERNAL_QOS_PARAM	0x15
-#define REPLY_RX_INT_TIMEOUT_CNFG	0x16
-#define REPLY_NULL			0x17
-
-/* Multi-Station support */
-#define REPLY_ADD_STA			0x18
-#define REPLY_REMOVE_STA		0x19
-#define REPLY_RESET_ALL_STA		0x1a
-
-/* RX, TX */
-#define REPLY_ALM_RX			0x1b
-#define REPLY_TX			0x1c
-#define REPLY_TXFIFO_FLUSH		0x1e
-
-/* MISC commands */
-#define REPLY_MGMT_MCAST_KEY		0x1f
-#define REPLY_WEPKEY			0x20
-#define REPLY_INIT_IV			0x21
-#define REPLY_WRITE_MIB			0x22
-#define REPLY_READ_MIB			0x23
-#define REPLY_RADIO_FE			0x24
-#define REPLY_TXFIFO_CFG		0x25
-#define REPLY_WRITE_READ		0x26
-#define REPLY_INSTALL_SEC_KEY		0x27
-
-
-#define REPLY_RATE_SCALE		0x47
-#define REPLY_LEDS_CMD			0x48
-#define REPLY_TX_LINK_QUALITY_CMD	0x4e
-#define REPLY_ANA_MIB_OVERRIDE_CMD	0x4f
-#define REPLY_WRITE2REG_CMD		0x50
-
-/* winfi-wifi coexistence */
-#define COEX_PRIORITY_TABLE_CMD		0x5a
-#define COEX_MEDIUM_NOTIFICATION	0x5b
-#define COEX_EVENT_CMD			0x5c
-
-/* more Protocol and Protocol-test commands */
-#define REPLY_MAX_SLEEP_TIME_CMD	0x61
-#define CALIBRATION_CFG_CMD		0x65
-#define CALIBRATION_RES_NOTIFICATION	0x66
-#define CALIBRATION_COMPLETE_NOTIFICATION	0x67
-
-/* Measurements */
-#define REPLY_QUIET_CMD			0x71
-#define REPLY_CHANNEL_SWITCH		0x72
-#define CHANNEL_SWITCH_NOTIFICATION	0x73
-
-#define REPLY_SPECTRUM_MEASUREMENT_CMD	0x74
-#define SPECTRUM_MEASURE_NOTIFICATION	0x75
-#define REPLY_MEASUREMENT_ABORT_CMD	0x76
-
-/* Power Management */
-#define POWER_TABLE_CMD			0x77
-#define SAVE_RESTORE_ADDRESS_CMD		0x78
-#define REPLY_WATERMARK_CMD		0x79
-#define PM_DEBUG_STATISTIC_NOTIFIC	0x7B
-#define PD_FLUSH_N_NOTIFICATION		0x7C
-
-/* Scan commands and notifications */
-#define REPLY_SCAN_REQUEST_CMD		0x80
-#define REPLY_SCAN_ABORT_CMD		0x81
-#define SCAN_START_NOTIFICATION		0x82
-#define SCAN_RESULTS_NOTIFICATION	0x83
-#define SCAN_COMPLETE_NOTIFICATION	0x84
-
-/* Continuous TX commands */
-#define REPLY_CONT_TX_CMD		0x85
-#define END_OF_CONT_TX_NOTIFICATION	0x86
-
-/* Timer/Eeprom commands */
-#define TIMER_CMD			0x87
-#define EEPROM_WRITE_CMD		0x88
-
-/* PAPD commands */
-#define FEEDBACK_REQUEST_NOTIFICATION	0x8b
-#define REPLY_CW_CMD			0x8c
-
-/* IBSS/AP commands Continue */
-#define BEACON_NOTIFICATION		0x90
-#define REPLY_TX_BEACON			0x91
-#define REPLY_REQUEST_ATIM		0x93
-#define WHO_IS_AWAKE_NOTIFICATION	0x94
-#define TX_PWR_DBM_LIMIT_CMD		0x95
-#define QUIET_NOTIFICATION		0x96
-#define TX_PWR_TABLE_CMD		0x97
-#define TX_ANT_CONFIGURATION_CMD	0x98
-#define MEASURE_ABORT_NOTIFICATION	0x99
-#define REPLY_CALIBRATION_TUNE		0x9a
-
-/* bt config command */
-#define REPLY_BT_CONFIG			0x9b
-#define REPLY_STATISTICS_CMD		0x9c
-#define STATISTICS_NOTIFICATION		0x9d
-
-/* RF-KILL commands and notifications */
-#define REPLY_CARD_STATE_CMD		0xa0
-#define CARD_STATE_NOTIFICATION		0xa1
-
-/* Missed beacons notification */
-#define MISSED_BEACONS_NOTIFICATION	0xa2
-#define MISSED_BEACONS_NOTIFICATION_TH_CMD	0xa3
-
-#define REPLY_CT_KILL_CONFIG_CMD	0xa4
-
-/* HD commands and notifications */
-#define REPLY_HD_PARAMS_CMD		0xa6
-#define HD_PARAMS_NOTIFICATION		0xa7
-#define SENSITIVITY_CMD			0xa8
-#define U_APSD_PARAMS_CMD		0xa9
-#define NOISY_PLATFORM_CMD		0xaa
-#define ILLEGAL_CMD			0xac
-#define REPLY_PHY_CALIBRATION_CMD	0xb0
-#define REPLAY_RX_GAIN_CALIB_CMD	0xb1
-
-/* WiPAN commands */
-#define REPLY_WIPAN_PARAMS_CMD		0xb2
-#define REPLY_WIPAN_RX_ON_CMD		0xb3
-#define REPLY_WIPAN_RX_ON_TIMING	0xb4
-#define REPLY_WIPAN_TX_PWR_TABLE_CMD	0xb5
-#define REPLY_WIPAN_RXON_ASSOC_CMD	0xb6
-#define REPLY_WIPAN_QOS_PARAM		0xb7
-#define WIPAN_REPLY_WEPKEY		0xb8
-
-/* BeamForming commands */
-#define BEAMFORMER_CFG_CMD		0xba
-#define BEAMFORMEE_NOTIFICATION		0xbb
-
-/* TGn new Commands */
-#define REPLY_RX_PHY_CMD		0xc0
-#define REPLY_RX_MPDU_CMD		0xc1
-#define REPLY_MULTICAST_HASH		0xc2
-#define REPLY_KDR_RX			0xc3
-#define REPLY_RX_DSP_EXT_INFO		0xc4
-#define REPLY_COMPRESSED_BA		0xc5
-
-/* PNC commands */
-#define PNC_CONFIG_CMD			0xc8
-#define PNC_UPDATE_TABLE_CMD		0xc9
-#define XVT_GENERAL_CTRL_CMD		0xca
-#define REPLY_LEGACY_RADIO_FE		0xdd
-
-/* WoWLAN commands */
-#define WOWLAN_PATTERNS			0xe0
-#define WOWLAN_WAKEUP_FILTER		0xe1
-#define WOWLAN_TSC_RSC_PARAM		0xe2
-#define WOWLAN_TKIP_PARAM		0xe3
-#define WOWLAN_KEK_KCK_MATERIAL		0xe4
-#define WOWLAN_GET_STATUSES		0xe5
-#define WOWLAN_TX_POWER_PER_DB		0xe6
-#define REPLY_WOWLAN_GET_STATUSES       WOWLAN_GET_STATUSES
-
-#define REPLY_DEBUG_CMD			0xf0
-#define REPLY_DSP_DEBUG_CMD		0xf1
-#define REPLY_DEBUG_MONITOR_CMD		0xf2
-#define REPLY_DEBUG_XVT_CMD		0xf3
-#define REPLY_DEBUG_DC_CALIB		0xf4
-#define REPLY_DYNAMIC_BP		0xf5
-
-/* General purpose Commands */
-#define REPLY_GP1_CMD			0xfa
-#define REPLY_GP2_CMD			0xfb
-#define REPLY_GP3_CMD			0xfc
-#define REPLY_GP4_CMD			0xfd
-#define REPLY_REPLAY_WRAPPER		0xfe
-#define REPLY_FRAME_DURATION_CALC_CMD	0xff
-
-#define LMAC_COMMAND_ID_MAX		0xff
-#define LMAC_COMMAND_ID_NUM		(LMAC_COMMAND_ID_MAX + 1)
-
-
-/* Calibration */
-
-enum {
-	PHY_CALIBRATE_DC_CMD			= 0,
-	PHY_CALIBRATE_LO_CMD			= 1,
-	PHY_CALIBRATE_RX_BB_CMD			= 2,
-	PHY_CALIBRATE_TX_IQ_CMD			= 3,
-	PHY_CALIBRATE_RX_IQ_CMD			= 4,
-	PHY_CALIBRATION_NOISE_CMD		= 5,
-	PHY_CALIBRATE_AGC_TABLE_CMD		= 6,
-	PHY_CALIBRATE_CRYSTAL_FRQ_CMD		= 7,
-	PHY_CALIBRATE_OPCODES_NUM,
-	SHILOH_PHY_CALIBRATE_DC_CMD		= 8,
-	SHILOH_PHY_CALIBRATE_LO_CMD		= 9,
-	SHILOH_PHY_CALIBRATE_RX_BB_CMD		= 10,
-	SHILOH_PHY_CALIBRATE_TX_IQ_CMD		= 11,
-	SHILOH_PHY_CALIBRATE_RX_IQ_CMD		= 12,
-	SHILOH_PHY_CALIBRATION_NOISE_CMD	= 13,
-	SHILOH_PHY_CALIBRATE_AGC_TABLE_CMD	= 14,
-	SHILOH_PHY_CALIBRATE_CRYSTAL_FRQ_CMD	= 15,
-	SHILOH_PHY_CALIBRATE_BASE_BAND_CMD	= 16,
-	SHILOH_PHY_CALIBRATE_TXIQ_PERIODIC_CMD	= 17,
-	CALIBRATION_CMD_NUM,
-};
-
-enum {
-	CALIB_CFG_RX_BB_IDX       = 0,
-	CALIB_CFG_DC_IDX          = 1,
-	CALIB_CFG_LO_IDX          = 2,
-	CALIB_CFG_TX_IQ_IDX       = 3,
-	CALIB_CFG_RX_IQ_IDX       = 4,
-	CALIB_CFG_NOISE_IDX       = 5,
-	CALIB_CFG_CRYSTAL_IDX     = 6,
-	CALIB_CFG_TEMPERATURE_IDX = 7,
-	CALIB_CFG_PAPD_IDX        = 8,
-	CALIB_CFG_LAST_IDX        = CALIB_CFG_PAPD_IDX,
-	CALIB_CFG_MODULE_NUM,
-};
-
-#define IWM_CALIB_MAP_INIT_MSK		0xFFFF
-#define IWM_CALIB_MAP_PER_LMAC(m)	((m & 0xFF0000) >> 16)
-#define IWM_CALIB_MAP_PER_UMAC(m)	((m & 0xFF000000) >> 24)
-#define IWM_CALIB_OPCODE_TO_INDEX(op)   (op - PHY_CALIBRATE_OPCODES_NUM)
-
-struct iwm_lmac_calib_hdr {
-	u8 opcode;
-	u8 first_grp;
-	u8 grp_num;
-	u8 all_data_valid;
-} __packed;
-
-#define IWM_LMAC_CALIB_FREQ_GROUPS_NR	7
-#define IWM_CALIB_FREQ_GROUPS_NR	5
-#define IWM_CALIB_DC_MODES_NR		12
-
-struct iwm_calib_rxiq_entry {
-	u16 ptam_postdist_ars;
-	u16 ptam_postdist_arc;
-} __packed;
-
-struct iwm_calib_rxiq_group {
-	struct iwm_calib_rxiq_entry mode[IWM_CALIB_DC_MODES_NR];
-} __packed;
-
-struct iwm_lmac_calib_rxiq {
-	struct iwm_calib_rxiq_group group[IWM_LMAC_CALIB_FREQ_GROUPS_NR];
-} __packed;
-
-struct iwm_calib_rxiq {
-	struct iwm_lmac_calib_hdr hdr;
-	struct iwm_calib_rxiq_group group[IWM_CALIB_FREQ_GROUPS_NR];
-} __packed;
-
-#define LMAC_STA_ID_SEED	0x0f
-#define LMAC_STA_ID_POS		0
-
-#define LMAC_STA_COLOR_SEED	0x7
-#define LMAC_STA_COLOR_POS	4
-
-struct iwm_lmac_power_report {
-	u8 pa_status;
-	u8 pa_integ_res_A[3];
-	u8 pa_integ_res_B[3];
-	u8 pa_integ_res_C[3];
-} __packed;
-
-struct iwm_lmac_tx_resp {
-	u8 frame_cnt; /* 1-no aggregation, greater then 1 - aggregation */
-	u8 bt_kill_cnt;
-	__le16 retry_cnt;
-	__le32 initial_tx_rate;
-	__le16 wireless_media_time;
-	struct iwm_lmac_power_report power_report;
-	__le32 tfd_info;
-	__le16 seq_ctl;
-	__le16 byte_cnt;
-	u8 tlc_rate_info;
-	u8 ra_tid;
-	__le16 frame_ctl;
-	__le32 status;
-} __packed;
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/main.c b/drivers/net/wireless/iwmc3200wifi/main.c
deleted file mode 100644
index 1f868b166d1..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/main.c
+++ /dev/null
@@ -1,847 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/sched.h>
-#include <linux/ieee80211.h>
-#include <linux/wireless.h>
-#include <linux/slab.h>
-#include <linux/moduleparam.h>
-
-#include "iwm.h"
-#include "debug.h"
-#include "bus.h"
-#include "umac.h"
-#include "commands.h"
-#include "hal.h"
-#include "fw.h"
-#include "rx.h"
-
-static struct iwm_conf def_iwm_conf = {
-
-	.sdio_ior_timeout	= 5000,
-	.calib_map		= BIT(CALIB_CFG_DC_IDX)	|
-				  BIT(CALIB_CFG_LO_IDX)	|
-				  BIT(CALIB_CFG_TX_IQ_IDX)	|
-				  BIT(CALIB_CFG_RX_IQ_IDX)	|
-				  BIT(SHILOH_PHY_CALIBRATE_BASE_BAND_CMD),
-	.expected_calib_map	= BIT(PHY_CALIBRATE_DC_CMD)	|
-				  BIT(PHY_CALIBRATE_LO_CMD)	|
-				  BIT(PHY_CALIBRATE_TX_IQ_CMD)	|
-				  BIT(PHY_CALIBRATE_RX_IQ_CMD)	|
-				  BIT(SHILOH_PHY_CALIBRATE_BASE_BAND_CMD),
-	.ct_kill_entry		= 110,
-	.ct_kill_exit		= 110,
-	.reset_on_fatal_err	= 1,
-	.auto_connect		= 1,
-	.enable_qos		= 1,
-	.mode			= UMAC_MODE_BSS,
-
-	/* UMAC configuration */
-	.power_index		= 0,
-	.frag_threshold		= IEEE80211_MAX_FRAG_THRESHOLD,
-	.rts_threshold		= IEEE80211_MAX_RTS_THRESHOLD,
-	.cts_to_self		= 0,
-
-	.assoc_timeout		= 2,
-	.roam_timeout		= 10,
-	.wireless_mode		= WIRELESS_MODE_11A | WIRELESS_MODE_11G |
-				  WIRELESS_MODE_11N,
-
-	/* IBSS */
-	.ibss_band		= UMAC_BAND_2GHZ,
-	.ibss_channel		= 1,
-
-	.mac_addr		= {0x00, 0x02, 0xb3, 0x01, 0x02, 0x03},
-};
-
-static bool modparam_reset;
-module_param_named(reset, modparam_reset, bool, 0644);
-MODULE_PARM_DESC(reset, "reset on firmware errors (default 0 [not reset])");
-
-static bool modparam_wimax_enable = true;
-module_param_named(wimax_enable, modparam_wimax_enable, bool, 0644);
-MODULE_PARM_DESC(wimax_enable, "Enable wimax core (default 1 [wimax enabled])");
-
-int iwm_mode_to_nl80211_iftype(int mode)
-{
-	switch (mode) {
-	case UMAC_MODE_BSS:
-		return NL80211_IFTYPE_STATION;
-	case UMAC_MODE_IBSS:
-		return NL80211_IFTYPE_ADHOC;
-	default:
-		return NL80211_IFTYPE_UNSPECIFIED;
-	}
-
-	return 0;
-}
-
-static void iwm_statistics_request(struct work_struct *work)
-{
-	struct iwm_priv *iwm =
-		container_of(work, struct iwm_priv, stats_request.work);
-
-	iwm_send_umac_stats_req(iwm, 0);
-}
-
-static void iwm_disconnect_work(struct work_struct *work)
-{
-	struct iwm_priv *iwm =
-		container_of(work, struct iwm_priv, disconnect.work);
-
-	if (iwm->umac_profile_active)
-		iwm_invalidate_mlme_profile(iwm);
-
-	clear_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
-	iwm->umac_profile_active = false;
-	memset(iwm->bssid, 0, ETH_ALEN);
-	iwm->channel = 0;
-
-	iwm_link_off(iwm);
-
-	wake_up_interruptible(&iwm->mlme_queue);
-
-	cfg80211_disconnected(iwm_to_ndev(iwm), 0, NULL, 0, GFP_KERNEL);
-}
-
-static void iwm_ct_kill_work(struct work_struct *work)
-{
-	struct iwm_priv *iwm =
-		container_of(work, struct iwm_priv, ct_kill_delay.work);
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-
-	IWM_INFO(iwm, "CT kill delay timeout\n");
-
-	wiphy_rfkill_set_hw_state(wiphy, false);
-}
-
-static int __iwm_up(struct iwm_priv *iwm);
-static int __iwm_down(struct iwm_priv *iwm);
-
-static void iwm_reset_worker(struct work_struct *work)
-{
-	struct iwm_priv *iwm;
-	struct iwm_umac_profile *profile = NULL;
-	int uninitialized_var(ret), retry = 0;
-
-	iwm = container_of(work, struct iwm_priv, reset_worker);
-
-	/*
-	 * XXX: The iwm->mutex is introduced purely for this reset work,
-	 * because the other users for iwm_up and iwm_down are only netdev
-	 * ndo_open and ndo_stop which are already protected by rtnl.
-	 * Please remove iwm->mutex together if iwm_reset_worker() is not
-	 * required in the future.
-	 */
-	if (!mutex_trylock(&iwm->mutex)) {
-		IWM_WARN(iwm, "We are in the middle of interface bringing "
-			 "UP/DOWN. Skip driver resetting.\n");
-		return;
-	}
-
-	if (iwm->umac_profile_active) {
-		profile = kmalloc(sizeof(struct iwm_umac_profile), GFP_KERNEL);
-		if (profile)
-			memcpy(profile, iwm->umac_profile, sizeof(*profile));
-		else
-			IWM_ERR(iwm, "Couldn't alloc memory for profile\n");
-	}
-
-	__iwm_down(iwm);
-
-	while (retry++ < 3) {
-		ret = __iwm_up(iwm);
-		if (!ret)
-			break;
-
-		schedule_timeout_uninterruptible(10 * HZ);
-	}
-
-	if (ret) {
-		IWM_WARN(iwm, "iwm_up() failed: %d\n", ret);
-
-		kfree(profile);
-		goto out;
-	}
-
-	if (profile) {
-		IWM_DBG_MLME(iwm, DBG, "Resend UMAC profile\n");
-		memcpy(iwm->umac_profile, profile, sizeof(*profile));
-		iwm_send_mlme_profile(iwm);
-		kfree(profile);
-	} else
-		clear_bit(IWM_STATUS_RESETTING, &iwm->status);
-
- out:
-	mutex_unlock(&iwm->mutex);
-}
-
-static void iwm_auth_retry_worker(struct work_struct *work)
-{
-	struct iwm_priv *iwm;
-	int i, ret;
-
-	iwm = container_of(work, struct iwm_priv, auth_retry_worker);
-	if (iwm->umac_profile_active) {
-		ret = iwm_invalidate_mlme_profile(iwm);
-		if (ret < 0)
-			return;
-	}
-
-	iwm->umac_profile->sec.auth_type = UMAC_AUTH_TYPE_LEGACY_PSK;
-
-	ret = iwm_send_mlme_profile(iwm);
-	if (ret < 0)
-		return;
-
-	for (i = 0; i < IWM_NUM_KEYS; i++)
-		if (iwm->keys[i].key_len)
-			iwm_set_key(iwm, 0, &iwm->keys[i]);
-
-	iwm_set_tx_key(iwm, iwm->default_key);
-}
-
-
-
-static void iwm_watchdog(unsigned long data)
-{
-	struct iwm_priv *iwm = (struct iwm_priv *)data;
-
-	IWM_WARN(iwm, "Watchdog expired: UMAC stalls!\n");
-
-	if (modparam_reset)
-		iwm_resetting(iwm);
-}
-
-int iwm_priv_init(struct iwm_priv *iwm)
-{
-	int i, j;
-	char name[32];
-
-	iwm->status = 0;
-	INIT_LIST_HEAD(&iwm->pending_notif);
-	init_waitqueue_head(&iwm->notif_queue);
-	init_waitqueue_head(&iwm->nonwifi_queue);
-	init_waitqueue_head(&iwm->wifi_ntfy_queue);
-	init_waitqueue_head(&iwm->mlme_queue);
-	memcpy(&iwm->conf, &def_iwm_conf, sizeof(struct iwm_conf));
-	spin_lock_init(&iwm->tx_credit.lock);
-	INIT_LIST_HEAD(&iwm->wifi_pending_cmd);
-	INIT_LIST_HEAD(&iwm->nonwifi_pending_cmd);
-	iwm->wifi_seq_num = UMAC_WIFI_SEQ_NUM_BASE;
-	iwm->nonwifi_seq_num = UMAC_NONWIFI_SEQ_NUM_BASE;
-	spin_lock_init(&iwm->cmd_lock);
-	iwm->scan_id = 1;
-	INIT_DELAYED_WORK(&iwm->stats_request, iwm_statistics_request);
-	INIT_DELAYED_WORK(&iwm->disconnect, iwm_disconnect_work);
-	INIT_DELAYED_WORK(&iwm->ct_kill_delay, iwm_ct_kill_work);
-	INIT_WORK(&iwm->reset_worker, iwm_reset_worker);
-	INIT_WORK(&iwm->auth_retry_worker, iwm_auth_retry_worker);
-	INIT_LIST_HEAD(&iwm->bss_list);
-
-	skb_queue_head_init(&iwm->rx_list);
-	INIT_LIST_HEAD(&iwm->rx_tickets);
-	spin_lock_init(&iwm->ticket_lock);
-	for (i = 0; i < IWM_RX_ID_HASH; i++) {
-		INIT_LIST_HEAD(&iwm->rx_packets[i]);
-		spin_lock_init(&iwm->packet_lock[i]);
-	}
-
-	INIT_WORK(&iwm->rx_worker, iwm_rx_worker);
-
-	iwm->rx_wq = create_singlethread_workqueue(KBUILD_MODNAME "_rx");
-	if (!iwm->rx_wq)
-		return -EAGAIN;
-
-	for (i = 0; i < IWM_TX_QUEUES; i++) {
-		INIT_WORK(&iwm->txq[i].worker, iwm_tx_worker);
-		snprintf(name, 32, KBUILD_MODNAME "_tx_%d", i);
-		iwm->txq[i].id = i;
-		iwm->txq[i].wq = create_singlethread_workqueue(name);
-		if (!iwm->txq[i].wq)
-			return -EAGAIN;
-
-		skb_queue_head_init(&iwm->txq[i].queue);
-		skb_queue_head_init(&iwm->txq[i].stopped_queue);
-		spin_lock_init(&iwm->txq[i].lock);
-	}
-
-	for (i = 0; i < IWM_NUM_KEYS; i++)
-		memset(&iwm->keys[i], 0, sizeof(struct iwm_key));
-
-	iwm->default_key = -1;
-
-	for (i = 0; i < IWM_STA_TABLE_NUM; i++)
-		for (j = 0; j < IWM_UMAC_TID_NR; j++) {
-			mutex_init(&iwm->sta_table[i].tid_info[j].mutex);
-			iwm->sta_table[i].tid_info[j].stopped = false;
-		}
-
-	init_timer(&iwm->watchdog);
-	iwm->watchdog.function = iwm_watchdog;
-	iwm->watchdog.data = (unsigned long)iwm;
-	mutex_init(&iwm->mutex);
-
-	iwm->last_fw_err = kzalloc(sizeof(struct iwm_fw_error_hdr),
-				   GFP_KERNEL);
-	if (iwm->last_fw_err == NULL)
-		return -ENOMEM;
-
-	return 0;
-}
-
-void iwm_priv_deinit(struct iwm_priv *iwm)
-{
-	int i;
-
-	for (i = 0; i < IWM_TX_QUEUES; i++)
-		destroy_workqueue(iwm->txq[i].wq);
-
-	destroy_workqueue(iwm->rx_wq);
-	kfree(iwm->last_fw_err);
-}
-
-/*
- * We reset all the structures, and we reset the UMAC.
- * After calling this routine, you're expected to reload
- * the firmware.
- */
-void iwm_reset(struct iwm_priv *iwm)
-{
-	struct iwm_notif *notif, *next;
-
-	if (test_bit(IWM_STATUS_READY, &iwm->status))
-		iwm_target_reset(iwm);
-
-	if (test_bit(IWM_STATUS_RESETTING, &iwm->status)) {
-		iwm->status = 0;
-		set_bit(IWM_STATUS_RESETTING, &iwm->status);
-	} else
-		iwm->status = 0;
-	iwm->scan_id = 1;
-
-	list_for_each_entry_safe(notif, next, &iwm->pending_notif, pending) {
-		list_del(&notif->pending);
-		kfree(notif->buf);
-		kfree(notif);
-	}
-
-	iwm_cmd_flush(iwm);
-
-	flush_workqueue(iwm->rx_wq);
-
-	iwm_link_off(iwm);
-}
-
-void iwm_resetting(struct iwm_priv *iwm)
-{
-	set_bit(IWM_STATUS_RESETTING, &iwm->status);
-
-	schedule_work(&iwm->reset_worker);
-}
-
-/*
- * Notification code:
- *
- * We're faced with the following issue: Any host command can
- * have an answer or not, and if there's an answer to expect,
- * it can be treated synchronously or asynchronously.
- * To work around the synchronous answer case, we implemented
- * our notification mechanism.
- * When a code path needs to wait for a command response
- * synchronously, it calls notif_handle(), which waits for the
- * right notification to show up, and then process it. Before
- * starting to wait, it registered as a waiter for this specific
- * answer (by toggling a bit in on of the handler_map), so that
- * the rx code knows that it needs to send a notification to the
- * waiting processes. It does so by calling iwm_notif_send(),
- * which adds the notification to the pending notifications list,
- * and then wakes the waiting processes up.
- */
-int iwm_notif_send(struct iwm_priv *iwm, struct iwm_wifi_cmd *cmd,
-		   u8 cmd_id, u8 source, u8 *buf, unsigned long buf_size)
-{
-	struct iwm_notif *notif;
-
-	notif = kzalloc(sizeof(struct iwm_notif), GFP_KERNEL);
-	if (!notif) {
-		IWM_ERR(iwm, "Couldn't alloc memory for notification\n");
-		return -ENOMEM;
-	}
-
-	INIT_LIST_HEAD(&notif->pending);
-	notif->cmd = cmd;
-	notif->cmd_id = cmd_id;
-	notif->src = source;
-	notif->buf = kzalloc(buf_size, GFP_KERNEL);
-	if (!notif->buf) {
-		IWM_ERR(iwm, "Couldn't alloc notification buffer\n");
-		kfree(notif);
-		return -ENOMEM;
-	}
-	notif->buf_size = buf_size;
-	memcpy(notif->buf, buf, buf_size);
-	list_add_tail(&notif->pending, &iwm->pending_notif);
-
-	wake_up_interruptible(&iwm->notif_queue);
-
-	return 0;
-}
-
-static struct iwm_notif *iwm_notif_find(struct iwm_priv *iwm, u32 cmd,
-					u8 source)
-{
-	struct iwm_notif *notif;
-
-	list_for_each_entry(notif, &iwm->pending_notif, pending) {
-		if ((notif->cmd_id == cmd) && (notif->src == source)) {
-			list_del(&notif->pending);
-			return notif;
-		}
-	}
-
-	return NULL;
-}
-
-static struct iwm_notif *iwm_notif_wait(struct iwm_priv *iwm, u32 cmd,
-					u8 source, long timeout)
-{
-	int ret;
-	struct iwm_notif *notif;
-	unsigned long *map = NULL;
-
-	switch (source) {
-	case IWM_SRC_LMAC:
-		map = &iwm->lmac_handler_map[0];
-		break;
-	case IWM_SRC_UMAC:
-		map = &iwm->umac_handler_map[0];
-		break;
-	case IWM_SRC_UDMA:
-		map = &iwm->udma_handler_map[0];
-		break;
-	}
-
-	set_bit(cmd, map);
-
-	ret = wait_event_interruptible_timeout(iwm->notif_queue,
-			 ((notif = iwm_notif_find(iwm, cmd, source)) != NULL),
-					       timeout);
-	clear_bit(cmd, map);
-
-	if (!ret)
-		return NULL;
-
-	return notif;
-}
-
-int iwm_notif_handle(struct iwm_priv *iwm, u32 cmd, u8 source, long timeout)
-{
-	int ret;
-	struct iwm_notif *notif;
-
-	notif = iwm_notif_wait(iwm, cmd, source, timeout);
-	if (!notif)
-		return -ETIME;
-
-	ret = iwm_rx_handle_resp(iwm, notif->buf, notif->buf_size, notif->cmd);
-	kfree(notif->buf);
-	kfree(notif);
-
-	return ret;
-}
-
-static int iwm_config_boot_params(struct iwm_priv *iwm)
-{
-	struct iwm_udma_nonwifi_cmd target_cmd;
-	int ret;
-
-	/* check Wimax is off and config debug monitor */
-	if (!modparam_wimax_enable) {
-		u32 data1 = 0x1f;
-		u32 addr1 = 0x606BE258;
-
-		u32 data2_set = 0x0;
-		u32 data2_clr = 0x1;
-		u32 addr2 = 0x606BE100;
-
-		u32 data3 = 0x1;
-		u32 addr3 = 0x606BEC00;
-
-		target_cmd.resp = 0;
-		target_cmd.handle_by_hw = 0;
-		target_cmd.eop = 1;
-
-		target_cmd.opcode = UMAC_HDI_OUT_OPCODE_WRITE;
-		target_cmd.addr = cpu_to_le32(addr1);
-		target_cmd.op1_sz = cpu_to_le32(sizeof(u32));
-		target_cmd.op2 = 0;
-
-		ret = iwm_hal_send_target_cmd(iwm, &target_cmd, &data1);
-		if (ret < 0) {
-			IWM_ERR(iwm, "iwm_hal_send_target_cmd failed\n");
-			return ret;
-		}
-
-		target_cmd.opcode = UMAC_HDI_OUT_OPCODE_READ_MODIFY_WRITE;
-		target_cmd.addr = cpu_to_le32(addr2);
-		target_cmd.op1_sz = cpu_to_le32(data2_set);
-		target_cmd.op2 = cpu_to_le32(data2_clr);
-
-		ret = iwm_hal_send_target_cmd(iwm, &target_cmd, &data1);
-		if (ret < 0) {
-			IWM_ERR(iwm, "iwm_hal_send_target_cmd failed\n");
-			return ret;
-		}
-
-		target_cmd.opcode = UMAC_HDI_OUT_OPCODE_WRITE;
-		target_cmd.addr = cpu_to_le32(addr3);
-		target_cmd.op1_sz = cpu_to_le32(sizeof(u32));
-		target_cmd.op2 = 0;
-
-		ret = iwm_hal_send_target_cmd(iwm, &target_cmd, &data3);
-		if (ret < 0) {
-			IWM_ERR(iwm, "iwm_hal_send_target_cmd failed\n");
-			return ret;
-		}
-	}
-
-	return 0;
-}
-
-void iwm_init_default_profile(struct iwm_priv *iwm,
-			      struct iwm_umac_profile *profile)
-{
-	memset(profile, 0, sizeof(struct iwm_umac_profile));
-
-	profile->sec.auth_type = UMAC_AUTH_TYPE_OPEN;
-	profile->sec.flags = UMAC_SEC_FLG_LEGACY_PROFILE;
-	profile->sec.ucast_cipher = UMAC_CIPHER_TYPE_NONE;
-	profile->sec.mcast_cipher = UMAC_CIPHER_TYPE_NONE;
-
-	if (iwm->conf.enable_qos)
-		profile->flags |= cpu_to_le16(UMAC_PROFILE_QOS_ALLOWED);
-
-	profile->wireless_mode = iwm->conf.wireless_mode;
-	profile->mode = cpu_to_le32(iwm->conf.mode);
-
-	profile->ibss.atim = 0;
-	profile->ibss.beacon_interval = 100;
-	profile->ibss.join_only = 0;
-	profile->ibss.band = iwm->conf.ibss_band;
-	profile->ibss.channel = iwm->conf.ibss_channel;
-}
-
-void iwm_link_on(struct iwm_priv *iwm)
-{
-	netif_carrier_on(iwm_to_ndev(iwm));
-	netif_tx_wake_all_queues(iwm_to_ndev(iwm));
-
-	iwm_send_umac_stats_req(iwm, 0);
-}
-
-void iwm_link_off(struct iwm_priv *iwm)
-{
-	struct iw_statistics *wstats = &iwm->wstats;
-	int i;
-
-	netif_tx_stop_all_queues(iwm_to_ndev(iwm));
-	netif_carrier_off(iwm_to_ndev(iwm));
-
-	for (i = 0; i < IWM_TX_QUEUES; i++) {
-		skb_queue_purge(&iwm->txq[i].queue);
-		skb_queue_purge(&iwm->txq[i].stopped_queue);
-
-		iwm->txq[i].concat_count = 0;
-		iwm->txq[i].concat_ptr = iwm->txq[i].concat_buf;
-
-		flush_workqueue(iwm->txq[i].wq);
-	}
-
-	iwm_rx_free(iwm);
-
-	cancel_delayed_work_sync(&iwm->stats_request);
-	memset(wstats, 0, sizeof(struct iw_statistics));
-	wstats->qual.updated = IW_QUAL_ALL_INVALID;
-
-	kfree(iwm->req_ie);
-	iwm->req_ie = NULL;
-	iwm->req_ie_len = 0;
-	kfree(iwm->resp_ie);
-	iwm->resp_ie = NULL;
-	iwm->resp_ie_len = 0;
-
-	del_timer_sync(&iwm->watchdog);
-}
-
-static void iwm_bss_list_clean(struct iwm_priv *iwm)
-{
-	struct iwm_bss_info *bss, *next;
-
-	list_for_each_entry_safe(bss, next, &iwm->bss_list, node) {
-		list_del(&bss->node);
-		kfree(bss->bss);
-		kfree(bss);
-	}
-}
-
-static int iwm_channels_init(struct iwm_priv *iwm)
-{
-	int ret;
-
-	ret = iwm_send_umac_channel_list(iwm);
-	if (ret) {
-		IWM_ERR(iwm, "Send channel list failed\n");
-		return ret;
-	}
-
-	ret = iwm_notif_handle(iwm, UMAC_CMD_OPCODE_GET_CHAN_INFO_LIST,
-			       IWM_SRC_UMAC, WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Didn't get a channel list notification\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-static int __iwm_up(struct iwm_priv *iwm)
-{
-	int ret;
-	struct iwm_notif *notif_reboot, *notif_ack = NULL;
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	u32 wireless_mode;
-
-	ret = iwm_bus_enable(iwm);
-	if (ret) {
-		IWM_ERR(iwm, "Couldn't enable function\n");
-		return ret;
-	}
-
-	iwm_rx_setup_handlers(iwm);
-
-	/* Wait for initial BARKER_REBOOT from hardware */
-	notif_reboot = iwm_notif_wait(iwm, IWM_BARKER_REBOOT_NOTIFICATION,
-				      IWM_SRC_UDMA, 2 * HZ);
-	if (!notif_reboot) {
-		IWM_ERR(iwm, "Wait for REBOOT_BARKER timeout\n");
-		goto err_disable;
-	}
-
-	/* We send the barker back */
-	ret = iwm_bus_send_chunk(iwm, notif_reboot->buf, 16);
-	if (ret) {
-		IWM_ERR(iwm, "REBOOT barker response failed\n");
-		kfree(notif_reboot);
-		goto err_disable;
-	}
-
-	kfree(notif_reboot->buf);
-	kfree(notif_reboot);
-
-	/* Wait for ACK_BARKER from hardware */
-	notif_ack = iwm_notif_wait(iwm, IWM_ACK_BARKER_NOTIFICATION,
-				   IWM_SRC_UDMA, 2 * HZ);
-	if (!notif_ack) {
-		IWM_ERR(iwm, "Wait for ACK_BARKER timeout\n");
-		goto err_disable;
-	}
-
-	kfree(notif_ack->buf);
-	kfree(notif_ack);
-
-	/* We start to config static boot parameters */
-	ret = iwm_config_boot_params(iwm);
-	if (ret) {
-		IWM_ERR(iwm, "Config boot parameters failed\n");
-		goto err_disable;
-	}
-
-	ret = iwm_read_mac(iwm, iwm_to_ndev(iwm)->dev_addr);
-	if (ret) {
-		IWM_ERR(iwm, "MAC reading failed\n");
-		goto err_disable;
-	}
-	memcpy(iwm_to_ndev(iwm)->perm_addr, iwm_to_ndev(iwm)->dev_addr,
-		ETH_ALEN);
-
-	/* We can load the FWs */
-	ret = iwm_load_fw(iwm);
-	if (ret) {
-		IWM_ERR(iwm, "FW loading failed\n");
-		goto err_disable;
-	}
-
-	ret = iwm_eeprom_fat_channels(iwm);
-	if (ret) {
-		IWM_ERR(iwm, "Couldnt read HT channels EEPROM entries\n");
-		goto err_fw;
-	}
-
-	/*
-	 * Read our SKU capabilities.
-	 * If it's valid, we AND the configured wireless mode with the
-	 * device EEPROM value as the current profile wireless mode.
-	 */
-	wireless_mode = iwm_eeprom_wireless_mode(iwm);
-	if (wireless_mode) {
-		iwm->conf.wireless_mode &= wireless_mode;
-		if (iwm->umac_profile)
-			iwm->umac_profile->wireless_mode =
-					iwm->conf.wireless_mode;
-	} else
-		IWM_ERR(iwm, "Wrong SKU capabilities: 0x%x\n",
-			*((u16 *)iwm_eeprom_access(iwm, IWM_EEPROM_SKU_CAP)));
-
-	snprintf(wiphy->fw_version, sizeof(wiphy->fw_version), "L%s_U%s",
-		 iwm->lmac_version, iwm->umac_version);
-
-	/* We configure the UMAC and enable the wifi module */
-	ret = iwm_send_umac_config(iwm,
-			cpu_to_le32(UMAC_RST_CTRL_FLG_WIFI_CORE_EN) |
-			cpu_to_le32(UMAC_RST_CTRL_FLG_WIFI_LINK_EN) |
-			cpu_to_le32(UMAC_RST_CTRL_FLG_WIFI_MLME_EN));
-	if (ret) {
-		IWM_ERR(iwm, "UMAC config failed\n");
-		goto err_fw;
-	}
-
-	ret = iwm_notif_handle(iwm, UMAC_NOTIFY_OPCODE_WIFI_CORE_STATUS,
-			       IWM_SRC_UMAC, WAIT_NOTIF_TIMEOUT);
-	if (ret) {
-		IWM_ERR(iwm, "Didn't get a wifi core status notification\n");
-		goto err_fw;
-	}
-
-	if (iwm->core_enabled != (UMAC_NTFY_WIFI_CORE_STATUS_LINK_EN |
-				  UMAC_NTFY_WIFI_CORE_STATUS_MLME_EN)) {
-		IWM_DBG_BOOT(iwm, DBG, "Not all cores enabled:0x%x\n",
-			     iwm->core_enabled);
-		ret = iwm_notif_handle(iwm, UMAC_NOTIFY_OPCODE_WIFI_CORE_STATUS,
-			       IWM_SRC_UMAC, WAIT_NOTIF_TIMEOUT);
-		if (ret) {
-			IWM_ERR(iwm, "Didn't get a core status notification\n");
-			goto err_fw;
-		}
-
-		if (iwm->core_enabled != (UMAC_NTFY_WIFI_CORE_STATUS_LINK_EN |
-					  UMAC_NTFY_WIFI_CORE_STATUS_MLME_EN)) {
-			IWM_ERR(iwm, "Not all cores enabled: 0x%x\n",
-				iwm->core_enabled);
-			goto err_fw;
-		} else {
-			IWM_INFO(iwm, "All cores enabled\n");
-		}
-	}
-
-	ret = iwm_channels_init(iwm);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't init channels\n");
-		goto err_fw;
-	}
-
-	/* Set the READY bit to indicate interface is brought up successfully */
-	set_bit(IWM_STATUS_READY, &iwm->status);
-
-	return 0;
-
- err_fw:
-	iwm_eeprom_exit(iwm);
-
- err_disable:
-	ret = iwm_bus_disable(iwm);
-	if (ret < 0)
-		IWM_ERR(iwm, "Couldn't disable function\n");
-
-	return -EIO;
-}
-
-int iwm_up(struct iwm_priv *iwm)
-{
-	int ret;
-
-	mutex_lock(&iwm->mutex);
-	ret = __iwm_up(iwm);
-	mutex_unlock(&iwm->mutex);
-
-	return ret;
-}
-
-static int __iwm_down(struct iwm_priv *iwm)
-{
-	int ret;
-
-	/* The interface is already down */
-	if (!test_bit(IWM_STATUS_READY, &iwm->status))
-		return 0;
-
-	if (iwm->scan_request) {
-		cfg80211_scan_done(iwm->scan_request, true);
-		iwm->scan_request = NULL;
-	}
-
-	clear_bit(IWM_STATUS_READY, &iwm->status);
-
-	iwm_eeprom_exit(iwm);
-	iwm_bss_list_clean(iwm);
-	iwm_init_default_profile(iwm, iwm->umac_profile);
-	iwm->umac_profile_active = false;
-	iwm->default_key = -1;
-	iwm->core_enabled = 0;
-
-	ret = iwm_bus_disable(iwm);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't disable function\n");
-		return ret;
-	}
-
-	return 0;
-}
-
-int iwm_down(struct iwm_priv *iwm)
-{
-	int ret;
-
-	mutex_lock(&iwm->mutex);
-	ret = __iwm_down(iwm);
-	mutex_unlock(&iwm->mutex);
-
-	return ret;
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/netdev.c b/drivers/net/wireless/iwmc3200wifi/netdev.c
deleted file mode 100644
index 5091d77e02c..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/netdev.c
+++ /dev/null
@@ -1,191 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License version
- * 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You 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.
- *
- */
-
-/*
- * This is the netdev related hooks for iwm.
- *
- * Some interesting code paths:
- *
- * iwm_open() (Called at netdev interface bringup time)
- *  -> iwm_up() (main.c)
- *      -> iwm_bus_enable()
- *          -> if_sdio_enable() (In case of an SDIO bus)
- *              -> sdio_enable_func()
- *      -> iwm_notif_wait(BARKER_REBOOT) (wait for reboot barker)
- *      -> iwm_notif_wait(ACK_BARKER) (wait for ACK barker)
- *      -> iwm_load_fw() (fw.c)
- *          -> iwm_load_umac()
- *          -> iwm_load_lmac() (Calibration LMAC)
- *          -> iwm_load_lmac() (Operational LMAC)
- *      -> iwm_send_umac_config()
- *
- * iwm_stop() (Called at netdev interface bringdown time)
- *  -> iwm_down()
- *      -> iwm_bus_disable()
- *          -> if_sdio_disable() (In case of an SDIO bus)
- *              -> sdio_disable_func()
- */
-#include <linux/netdevice.h>
-#include <linux/slab.h>
-
-#include "iwm.h"
-#include "commands.h"
-#include "cfg80211.h"
-#include "debug.h"
-
-static int iwm_open(struct net_device *ndev)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-
-	return iwm_up(iwm);
-}
-
-static int iwm_stop(struct net_device *ndev)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(ndev);
-
-	return iwm_down(iwm);
-}
-
-/*
- * iwm AC to queue mapping
- *
- * AC_VO -> queue 3
- * AC_VI -> queue 2
- * AC_BE -> queue 1
- * AC_BK -> queue 0
- */
-static const u16 iwm_1d_to_queue[8] = { 1, 0, 0, 1, 2, 2, 3, 3 };
-
-int iwm_tid_to_queue(u16 tid)
-{
-	if (tid > IWM_UMAC_TID_NR - 2)
-		return -EINVAL;
-
-	return iwm_1d_to_queue[tid];
-}
-
-static u16 iwm_select_queue(struct net_device *dev, struct sk_buff *skb)
-{
-	skb->priority = cfg80211_classify8021d(skb);
-
-	return iwm_1d_to_queue[skb->priority];
-}
-
-static const struct net_device_ops iwm_netdev_ops = {
-	.ndo_open		= iwm_open,
-	.ndo_stop		= iwm_stop,
-	.ndo_start_xmit		= iwm_xmit_frame,
-	.ndo_select_queue	= iwm_select_queue,
-};
-
-void *iwm_if_alloc(int sizeof_bus, struct device *dev,
-		   struct iwm_if_ops *if_ops)
-{
-	struct net_device *ndev;
-	struct wireless_dev *wdev;
-	struct iwm_priv *iwm;
-	int ret = 0;
-
-	wdev = iwm_wdev_alloc(sizeof_bus, dev);
-	if (IS_ERR(wdev))
-		return wdev;
-
-	iwm = wdev_to_iwm(wdev);
-	iwm->bus_ops = if_ops;
-	iwm->wdev = wdev;
-
-	ret = iwm_priv_init(iwm);
-	if (ret) {
-		dev_err(dev, "failed to init iwm_priv\n");
-		goto out_wdev;
-	}
-
-	wdev->iftype = iwm_mode_to_nl80211_iftype(iwm->conf.mode);
-
-	ndev = alloc_netdev_mq(0, "wlan%d", ether_setup, IWM_TX_QUEUES);
-	if (!ndev) {
-		dev_err(dev, "no memory for network device instance\n");
-		ret = -ENOMEM;
-		goto out_priv;
-	}
-
-	ndev->netdev_ops = &iwm_netdev_ops;
-	ndev->ieee80211_ptr = wdev;
-	SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
-	wdev->netdev = ndev;
-
-	iwm->umac_profile = kmalloc(sizeof(struct iwm_umac_profile),
-				    GFP_KERNEL);
-	if (!iwm->umac_profile) {
-		dev_err(dev, "Couldn't alloc memory for profile\n");
-		ret = -ENOMEM;
-		goto out_profile;
-	}
-
-	iwm_init_default_profile(iwm, iwm->umac_profile);
-
-	return iwm;
-
- out_profile:
-	free_netdev(ndev);
-
- out_priv:
-	iwm_priv_deinit(iwm);
-
- out_wdev:
-	iwm_wdev_free(iwm);
-	return ERR_PTR(ret);
-}
-
-void iwm_if_free(struct iwm_priv *iwm)
-{
-	if (!iwm_to_ndev(iwm))
-		return;
-
-	cancel_delayed_work_sync(&iwm->ct_kill_delay);
-	free_netdev(iwm_to_ndev(iwm));
-	iwm_priv_deinit(iwm);
-	kfree(iwm->umac_profile);
-	iwm->umac_profile = NULL;
-	iwm_wdev_free(iwm);
-}
-
-int iwm_if_add(struct iwm_priv *iwm)
-{
-	struct net_device *ndev = iwm_to_ndev(iwm);
-	int ret;
-
-	ret = register_netdev(ndev);
-	if (ret < 0) {
-		dev_err(&ndev->dev, "Failed to register netdev: %d\n", ret);
-		return ret;
-	}
-
-	return 0;
-}
-
-void iwm_if_remove(struct iwm_priv *iwm)
-{
-	unregister_netdev(iwm_to_ndev(iwm));
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/rx.c b/drivers/net/wireless/iwmc3200wifi/rx.c
deleted file mode 100644
index 7d708f4395f..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/rx.c
+++ /dev/null
@@ -1,1701 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
-#include <linux/sched.h>
-#include <linux/etherdevice.h>
-#include <linux/wireless.h>
-#include <linux/ieee80211.h>
-#include <linux/if_arp.h>
-#include <linux/list.h>
-#include <linux/slab.h>
-#include <net/iw_handler.h>
-
-#include "iwm.h"
-#include "debug.h"
-#include "hal.h"
-#include "umac.h"
-#include "lmac.h"
-#include "commands.h"
-#include "rx.h"
-#include "cfg80211.h"
-#include "eeprom.h"
-
-static int iwm_rx_check_udma_hdr(struct iwm_udma_in_hdr *hdr)
-{
-	if ((le32_to_cpu(hdr->cmd) == UMAC_PAD_TERMINAL) ||
-	    (le32_to_cpu(hdr->size) == UMAC_PAD_TERMINAL))
-		return -EINVAL;
-
-	return 0;
-}
-
-static inline int iwm_rx_resp_size(struct iwm_udma_in_hdr *hdr)
-{
-	return ALIGN(le32_to_cpu(hdr->size) + sizeof(struct iwm_udma_in_hdr),
-		     16);
-}
-
-/*
- * Notification handlers:
- *
- * For every possible notification we can receive from the
- * target, we have a handler.
- * When we get a target notification, and there is no one
- * waiting for it, it's just processed through the rx code
- * path:
- *
- * iwm_rx_handle()
- *  -> iwm_rx_handle_umac()
- *      -> iwm_rx_handle_wifi()
- *          -> iwm_rx_handle_resp()
- *              -> iwm_ntf_*()
- *
- *      OR
- *
- *      -> iwm_rx_handle_non_wifi()
- *
- * If there are processes waiting for this notification, then
- * iwm_rx_handle_wifi() just wakes those processes up and they
- * grab the pending notification.
- */
-static int iwm_ntf_error(struct iwm_priv *iwm, u8 *buf,
-			 unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_error *error;
-	struct iwm_fw_error_hdr *fw_err;
-
-	error = (struct iwm_umac_notif_error *)buf;
-	fw_err = &error->err;
-
-	memcpy(iwm->last_fw_err, fw_err, sizeof(struct iwm_fw_error_hdr));
-
-	IWM_ERR(iwm, "%cMAC FW ERROR:\n",
-	 (le32_to_cpu(fw_err->category) == UMAC_SYS_ERR_CAT_LMAC) ? 'L' : 'U');
-	IWM_ERR(iwm, "\tCategory:    %d\n", le32_to_cpu(fw_err->category));
-	IWM_ERR(iwm, "\tStatus:      0x%x\n", le32_to_cpu(fw_err->status));
-	IWM_ERR(iwm, "\tPC:          0x%x\n", le32_to_cpu(fw_err->pc));
-	IWM_ERR(iwm, "\tblink1:      %d\n", le32_to_cpu(fw_err->blink1));
-	IWM_ERR(iwm, "\tblink2:      %d\n", le32_to_cpu(fw_err->blink2));
-	IWM_ERR(iwm, "\tilink1:      %d\n", le32_to_cpu(fw_err->ilink1));
-	IWM_ERR(iwm, "\tilink2:      %d\n", le32_to_cpu(fw_err->ilink2));
-	IWM_ERR(iwm, "\tData1:       0x%x\n", le32_to_cpu(fw_err->data1));
-	IWM_ERR(iwm, "\tData2:       0x%x\n", le32_to_cpu(fw_err->data2));
-	IWM_ERR(iwm, "\tLine number: %d\n", le32_to_cpu(fw_err->line_num));
-	IWM_ERR(iwm, "\tUMAC status: 0x%x\n", le32_to_cpu(fw_err->umac_status));
-	IWM_ERR(iwm, "\tLMAC status: 0x%x\n", le32_to_cpu(fw_err->lmac_status));
-	IWM_ERR(iwm, "\tSDIO status: 0x%x\n", le32_to_cpu(fw_err->sdio_status));
-
-	iwm_resetting(iwm);
-
-	return 0;
-}
-
-static int iwm_ntf_umac_alive(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_alive *alive_resp =
-			(struct iwm_umac_notif_alive *)(buf);
-	u16 status = le16_to_cpu(alive_resp->status);
-
-	if (status == UMAC_NTFY_ALIVE_STATUS_ERR) {
-		IWM_ERR(iwm, "Receive error UMAC_ALIVE\n");
-		return -EIO;
-	}
-
-	iwm_tx_credit_init_pools(iwm, alive_resp);
-
-	return 0;
-}
-
-static int iwm_ntf_init_complete(struct iwm_priv *iwm, u8 *buf,
-				 unsigned long buf_size,
-				 struct iwm_wifi_cmd *cmd)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct iwm_umac_notif_init_complete *init_complete =
-			(struct iwm_umac_notif_init_complete *)(buf);
-	u16 status = le16_to_cpu(init_complete->status);
-	bool blocked = (status == UMAC_NTFY_INIT_COMPLETE_STATUS_ERR);
-
-	if (blocked)
-		IWM_DBG_NTF(iwm, DBG, "Hardware rf kill is on (radio off)\n");
-	else
-		IWM_DBG_NTF(iwm, DBG, "Hardware rf kill is off (radio on)\n");
-
-	wiphy_rfkill_set_hw_state(wiphy, blocked);
-
-	return 0;
-}
-
-static int iwm_ntf_tx_credit_update(struct iwm_priv *iwm, u8 *buf,
-				    unsigned long buf_size,
-				    struct iwm_wifi_cmd *cmd)
-{
-	int pool_nr, total_freed_pages;
-	unsigned long pool_map;
-	int i, id;
-	struct iwm_umac_notif_page_dealloc *dealloc =
-			(struct iwm_umac_notif_page_dealloc *)buf;
-
-	pool_nr = GET_VAL32(dealloc->changes, UMAC_DEALLOC_NTFY_CHANGES_CNT);
-	pool_map = GET_VAL32(dealloc->changes, UMAC_DEALLOC_NTFY_CHANGES_MSK);
-
-	IWM_DBG_TX(iwm, DBG, "UMAC dealloc notification: pool nr %d, "
-		   "update map 0x%lx\n", pool_nr, pool_map);
-
-	spin_lock(&iwm->tx_credit.lock);
-
-	for (i = 0; i < pool_nr; i++) {
-		id = GET_VAL32(dealloc->grp_info[i],
-			       UMAC_DEALLOC_NTFY_GROUP_NUM);
-		if (test_bit(id, &pool_map)) {
-			total_freed_pages = GET_VAL32(dealloc->grp_info[i],
-					      UMAC_DEALLOC_NTFY_PAGE_CNT);
-			iwm_tx_credit_inc(iwm, id, total_freed_pages);
-		}
-	}
-
-	spin_unlock(&iwm->tx_credit.lock);
-
-	return 0;
-}
-
-static int iwm_ntf_umac_reset(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	IWM_DBG_NTF(iwm, DBG, "UMAC RESET done\n");
-
-	return 0;
-}
-
-static int iwm_ntf_lmac_version(struct iwm_priv *iwm, u8 *buf,
-				unsigned long buf_size,
-				struct iwm_wifi_cmd *cmd)
-{
-	IWM_DBG_NTF(iwm, INFO, "LMAC Version: %x.%x\n", buf[9], buf[8]);
-
-	return 0;
-}
-
-static int iwm_ntf_tx(struct iwm_priv *iwm, u8 *buf,
-		      unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_lmac_tx_resp *tx_resp;
-	struct iwm_umac_wifi_in_hdr *hdr;
-
-	tx_resp = (struct iwm_lmac_tx_resp *)
-		(buf + sizeof(struct iwm_umac_wifi_in_hdr));
-	hdr = (struct iwm_umac_wifi_in_hdr *)buf;
-
-	IWM_DBG_TX(iwm, DBG, "REPLY_TX, buf size: %lu\n", buf_size);
-
-	IWM_DBG_TX(iwm, DBG, "Seqnum: %d\n",
-		   le16_to_cpu(hdr->sw_hdr.cmd.seq_num));
-	IWM_DBG_TX(iwm, DBG, "\tFrame cnt: %d\n", tx_resp->frame_cnt);
-	IWM_DBG_TX(iwm, DBG, "\tRetry cnt: %d\n",
-		   le16_to_cpu(tx_resp->retry_cnt));
-	IWM_DBG_TX(iwm, DBG, "\tSeq ctl: %d\n", le16_to_cpu(tx_resp->seq_ctl));
-	IWM_DBG_TX(iwm, DBG, "\tByte cnt: %d\n",
-		   le16_to_cpu(tx_resp->byte_cnt));
-	IWM_DBG_TX(iwm, DBG, "\tStatus: 0x%x\n", le32_to_cpu(tx_resp->status));
-
-	return 0;
-}
-
-
-static int iwm_ntf_calib_res(struct iwm_priv *iwm, u8 *buf,
-			     unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	u8 opcode;
-	u8 *calib_buf;
-	struct iwm_lmac_calib_hdr *hdr = (struct iwm_lmac_calib_hdr *)
-				(buf + sizeof(struct iwm_umac_wifi_in_hdr));
-
-	opcode = hdr->opcode;
-
-	BUG_ON(opcode >= CALIBRATION_CMD_NUM ||
-	       opcode < PHY_CALIBRATE_OPCODES_NUM);
-
-	IWM_DBG_NTF(iwm, DBG, "Store calibration result for opcode: %d\n",
-		    opcode);
-
-	buf_size -= sizeof(struct iwm_umac_wifi_in_hdr);
-	calib_buf = iwm->calib_res[opcode].buf;
-
-	if (!calib_buf || (iwm->calib_res[opcode].size < buf_size)) {
-		kfree(calib_buf);
-		calib_buf = kzalloc(buf_size, GFP_KERNEL);
-		if (!calib_buf) {
-			IWM_ERR(iwm, "Memory allocation failed: calib_res\n");
-			return -ENOMEM;
-		}
-		iwm->calib_res[opcode].buf = calib_buf;
-		iwm->calib_res[opcode].size = buf_size;
-	}
-
-	memcpy(calib_buf, hdr, buf_size);
-	set_bit(opcode - PHY_CALIBRATE_OPCODES_NUM, &iwm->calib_done_map);
-
-	return 0;
-}
-
-static int iwm_ntf_calib_complete(struct iwm_priv *iwm, u8 *buf,
-				  unsigned long buf_size,
-				  struct iwm_wifi_cmd *cmd)
-{
-	IWM_DBG_NTF(iwm, DBG, "Calibration completed\n");
-
-	return 0;
-}
-
-static int iwm_ntf_calib_cfg(struct iwm_priv *iwm, u8 *buf,
-			     unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_lmac_cal_cfg_resp *cal_resp;
-
-	cal_resp = (struct iwm_lmac_cal_cfg_resp *)
-			(buf + sizeof(struct iwm_umac_wifi_in_hdr));
-
-	IWM_DBG_NTF(iwm, DBG, "Calibration CFG command status: %d\n",
-		    le32_to_cpu(cal_resp->status));
-
-	return 0;
-}
-
-static int iwm_ntf_wifi_status(struct iwm_priv *iwm, u8 *buf,
-			       unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_wifi_status *status =
-		(struct iwm_umac_notif_wifi_status *)buf;
-
-	iwm->core_enabled |= le16_to_cpu(status->status);
-
-	return 0;
-}
-
-static struct iwm_rx_ticket_node *
-iwm_rx_ticket_node_alloc(struct iwm_priv *iwm, struct iwm_rx_ticket *ticket)
-{
-	struct iwm_rx_ticket_node *ticket_node;
-
-	ticket_node = kzalloc(sizeof(struct iwm_rx_ticket_node), GFP_KERNEL);
-	if (!ticket_node) {
-		IWM_ERR(iwm, "Couldn't allocate ticket node\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	ticket_node->ticket = kmemdup(ticket, sizeof(struct iwm_rx_ticket),
-				      GFP_KERNEL);
-	if (!ticket_node->ticket) {
-		IWM_ERR(iwm, "Couldn't allocate RX ticket\n");
-		kfree(ticket_node);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	INIT_LIST_HEAD(&ticket_node->node);
-
-	return ticket_node;
-}
-
-static void iwm_rx_ticket_node_free(struct iwm_rx_ticket_node *ticket_node)
-{
-	kfree(ticket_node->ticket);
-	kfree(ticket_node);
-}
-
-static struct iwm_rx_packet *iwm_rx_packet_get(struct iwm_priv *iwm, u16 id)
-{
-	u8 id_hash = IWM_RX_ID_GET_HASH(id);
-	struct iwm_rx_packet *packet;
-
-	spin_lock(&iwm->packet_lock[id_hash]);
-	list_for_each_entry(packet, &iwm->rx_packets[id_hash], node)
-		if (packet->id == id) {
-			list_del(&packet->node);
-			spin_unlock(&iwm->packet_lock[id_hash]);
-			return packet;
-		}
-
-	spin_unlock(&iwm->packet_lock[id_hash]);
-	return NULL;
-}
-
-static struct iwm_rx_packet *iwm_rx_packet_alloc(struct iwm_priv *iwm, u8 *buf,
-						 u32 size, u16 id)
-{
-	struct iwm_rx_packet *packet;
-
-	packet = kzalloc(sizeof(struct iwm_rx_packet), GFP_KERNEL);
-	if (!packet) {
-		IWM_ERR(iwm, "Couldn't allocate packet\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	packet->skb = dev_alloc_skb(size);
-	if (!packet->skb) {
-		IWM_ERR(iwm, "Couldn't allocate packet SKB\n");
-		kfree(packet);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	packet->pkt_size = size;
-
-	skb_put(packet->skb, size);
-	memcpy(packet->skb->data, buf, size);
-	INIT_LIST_HEAD(&packet->node);
-	packet->id = id;
-
-	return packet;
-}
-
-void iwm_rx_free(struct iwm_priv *iwm)
-{
-	struct iwm_rx_ticket_node *ticket, *nt;
-	struct iwm_rx_packet *packet, *np;
-	int i;
-
-	spin_lock(&iwm->ticket_lock);
-	list_for_each_entry_safe(ticket, nt, &iwm->rx_tickets, node) {
-		list_del(&ticket->node);
-		iwm_rx_ticket_node_free(ticket);
-	}
-	spin_unlock(&iwm->ticket_lock);
-
-	for (i = 0; i < IWM_RX_ID_HASH; i++) {
-		spin_lock(&iwm->packet_lock[i]);
-		list_for_each_entry_safe(packet, np, &iwm->rx_packets[i],
-					 node) {
-			list_del(&packet->node);
-			kfree_skb(packet->skb);
-			kfree(packet);
-		}
-		spin_unlock(&iwm->packet_lock[i]);
-	}
-}
-
-static int iwm_ntf_rx_ticket(struct iwm_priv *iwm, u8 *buf,
-			     unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_rx_ticket *ntf_rx_ticket =
-		(struct iwm_umac_notif_rx_ticket *)buf;
-	struct iwm_rx_ticket *ticket =
-		(struct iwm_rx_ticket *)ntf_rx_ticket->tickets;
-	int i, schedule_rx = 0;
-
-	for (i = 0; i < ntf_rx_ticket->num_tickets; i++) {
-		struct iwm_rx_ticket_node *ticket_node;
-
-		switch (le16_to_cpu(ticket->action)) {
-		case IWM_RX_TICKET_RELEASE:
-		case IWM_RX_TICKET_DROP:
-			/* We can push the packet to the stack */
-			ticket_node = iwm_rx_ticket_node_alloc(iwm, ticket);
-			if (IS_ERR(ticket_node))
-				return PTR_ERR(ticket_node);
-
-			IWM_DBG_RX(iwm, DBG, "TICKET %s(%d)\n",
-				   __le16_to_cpu(ticket->action) ==
-							IWM_RX_TICKET_RELEASE ?
-				   "RELEASE" : "DROP",
-				   ticket->id);
-			spin_lock(&iwm->ticket_lock);
-			list_add_tail(&ticket_node->node, &iwm->rx_tickets);
-			spin_unlock(&iwm->ticket_lock);
-
-			/*
-			 * We received an Rx ticket, most likely there's
-			 * a packet pending for it, it's not worth going
-			 * through the packet hash list to double check.
-			 * Let's just fire the rx worker..
-			 */
-			schedule_rx = 1;
-
-			break;
-
-		default:
-			IWM_ERR(iwm, "Invalid RX ticket action: 0x%x\n",
-				ticket->action);
-		}
-
-		ticket++;
-	}
-
-	if (schedule_rx)
-		queue_work(iwm->rx_wq, &iwm->rx_worker);
-
-	return 0;
-}
-
-static int iwm_ntf_rx_packet(struct iwm_priv *iwm, u8 *buf,
-			     unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_wifi_in_hdr *wifi_hdr;
-	struct iwm_rx_packet *packet;
-	u16 id, buf_offset;
-	u32 packet_size;
-	u8 id_hash;
-
-	IWM_DBG_RX(iwm, DBG, "\n");
-
-	wifi_hdr = (struct iwm_umac_wifi_in_hdr *)buf;
-	id = le16_to_cpu(wifi_hdr->sw_hdr.cmd.seq_num);
-	buf_offset = sizeof(struct iwm_umac_wifi_in_hdr);
-	packet_size = buf_size - sizeof(struct iwm_umac_wifi_in_hdr);
-
-	IWM_DBG_RX(iwm, DBG, "CMD:0x%x, seqnum: %d, packet size: %d\n",
-		   wifi_hdr->sw_hdr.cmd.cmd, id, packet_size);
-	IWM_DBG_RX(iwm, DBG, "Packet id: %d\n", id);
-	IWM_HEXDUMP(iwm, DBG, RX, "PACKET: ", buf + buf_offset, packet_size);
-
-	packet = iwm_rx_packet_alloc(iwm, buf + buf_offset, packet_size, id);
-	if (IS_ERR(packet))
-		return PTR_ERR(packet);
-
-	id_hash = IWM_RX_ID_GET_HASH(id);
-	spin_lock(&iwm->packet_lock[id_hash]);
-	list_add_tail(&packet->node, &iwm->rx_packets[id_hash]);
-	spin_unlock(&iwm->packet_lock[id_hash]);
-
-	/* We might (unlikely) have received the packet _after_ the ticket */
-	queue_work(iwm->rx_wq, &iwm->rx_worker);
-
-	return 0;
-}
-
-/* MLME handlers */
-static int iwm_mlme_assoc_start(struct iwm_priv *iwm, u8 *buf,
-				unsigned long buf_size,
-				struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_assoc_start *start;
-
-	start = (struct iwm_umac_notif_assoc_start *)buf;
-
-	IWM_DBG_MLME(iwm, INFO, "Association with %pM Started, reason: %d\n",
-		     start->bssid, le32_to_cpu(start->roam_reason));
-
-	wake_up_interruptible(&iwm->mlme_queue);
-
-	return 0;
-}
-
-static u8 iwm_is_open_wep_profile(struct iwm_priv *iwm)
-{
-	if ((iwm->umac_profile->sec.ucast_cipher == UMAC_CIPHER_TYPE_WEP_40 ||
-	     iwm->umac_profile->sec.ucast_cipher == UMAC_CIPHER_TYPE_WEP_104) &&
-	    (iwm->umac_profile->sec.ucast_cipher ==
-	     iwm->umac_profile->sec.mcast_cipher) &&
-	    (iwm->umac_profile->sec.auth_type == UMAC_AUTH_TYPE_OPEN))
-	       return 1;
-
-       return 0;
-}
-
-static int iwm_mlme_assoc_complete(struct iwm_priv *iwm, u8 *buf,
-				   unsigned long buf_size,
-				   struct iwm_wifi_cmd *cmd)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct ieee80211_channel *chan;
-	struct iwm_umac_notif_assoc_complete *complete =
-		(struct iwm_umac_notif_assoc_complete *)buf;
-
-	IWM_DBG_MLME(iwm, INFO, "Association with %pM completed, status: %d\n",
-		     complete->bssid, complete->status);
-
-	switch (le32_to_cpu(complete->status)) {
-	case UMAC_ASSOC_COMPLETE_SUCCESS:
-		chan = ieee80211_get_channel(wiphy,
-			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);
-			IWM_WARN(iwm, "Couldn't associate with %pM due to "
-				 "channel %d is disabled. Check your local "
-				 "regulatory setting.\n",
-				 complete->bssid, complete->channel);
-			goto failure;
-		}
-
-		set_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
-		memcpy(iwm->bssid, complete->bssid, ETH_ALEN);
-		iwm->channel = complete->channel;
-
-		/* Internal roaming state, avoid notifying SME. */
-		if (!test_and_clear_bit(IWM_STATUS_SME_CONNECTING, &iwm->status)
-		    && iwm->conf.mode == UMAC_MODE_BSS) {
-			cancel_delayed_work(&iwm->disconnect);
-			cfg80211_roamed(iwm_to_ndev(iwm), NULL,
-					complete->bssid,
-					iwm->req_ie, iwm->req_ie_len,
-					iwm->resp_ie, iwm->resp_ie_len,
-					GFP_KERNEL);
-			break;
-		}
-
-		iwm_link_on(iwm);
-
-		if (iwm->conf.mode == UMAC_MODE_IBSS)
-			goto ibss;
-
-		if (!test_bit(IWM_STATUS_RESETTING, &iwm->status))
-			cfg80211_connect_result(iwm_to_ndev(iwm),
-						complete->bssid,
-						iwm->req_ie, iwm->req_ie_len,
-						iwm->resp_ie, iwm->resp_ie_len,
-						WLAN_STATUS_SUCCESS,
-						GFP_KERNEL);
-		else
-			cfg80211_roamed(iwm_to_ndev(iwm), NULL,
-					complete->bssid,
-					iwm->req_ie, iwm->req_ie_len,
-					iwm->resp_ie, iwm->resp_ie_len,
-					GFP_KERNEL);
-		break;
-	case UMAC_ASSOC_COMPLETE_FAILURE:
- failure:
-		clear_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
-		memset(iwm->bssid, 0, ETH_ALEN);
-		iwm->channel = 0;
-
-		/* Internal roaming state, avoid notifying SME. */
-		if (!test_and_clear_bit(IWM_STATUS_SME_CONNECTING, &iwm->status)
-		    && iwm->conf.mode == UMAC_MODE_BSS) {
-			cancel_delayed_work(&iwm->disconnect);
-			break;
-		}
-
-		iwm_link_off(iwm);
-
-		if (iwm->conf.mode == UMAC_MODE_IBSS)
-			goto ibss;
-
-		if (!test_bit(IWM_STATUS_RESETTING, &iwm->status))
-			if (!iwm_is_open_wep_profile(iwm)) {
-				cfg80211_connect_result(iwm_to_ndev(iwm),
-					       complete->bssid,
-					       NULL, 0, NULL, 0,
-					       WLAN_STATUS_UNSPECIFIED_FAILURE,
-					       GFP_KERNEL);
-			} else {
-				/* Let's try shared WEP auth */
-				IWM_ERR(iwm, "Trying WEP shared auth\n");
-				schedule_work(&iwm->auth_retry_worker);
-			}
-		else
-			cfg80211_disconnected(iwm_to_ndev(iwm), 0, NULL, 0,
-					      GFP_KERNEL);
-		break;
-	default:
-		break;
-	}
-
-	clear_bit(IWM_STATUS_RESETTING, &iwm->status);
-	return 0;
-
- ibss:
-	cfg80211_ibss_joined(iwm_to_ndev(iwm), iwm->bssid, GFP_KERNEL);
-	clear_bit(IWM_STATUS_RESETTING, &iwm->status);
-	return 0;
-}
-
-static int iwm_mlme_profile_invalidate(struct iwm_priv *iwm, u8 *buf,
-				       unsigned long buf_size,
-				       struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_profile_invalidate *invalid;
-	u32 reason;
-
-	invalid = (struct iwm_umac_notif_profile_invalidate *)buf;
-	reason = le32_to_cpu(invalid->reason);
-
-	IWM_DBG_MLME(iwm, INFO, "Profile Invalidated. Reason: %d\n", reason);
-
-	if (reason != UMAC_PROFILE_INVALID_REQUEST &&
-	    test_bit(IWM_STATUS_SME_CONNECTING, &iwm->status))
-		cfg80211_connect_result(iwm_to_ndev(iwm), NULL, NULL, 0, NULL,
-					0, WLAN_STATUS_UNSPECIFIED_FAILURE,
-					GFP_KERNEL);
-
-	clear_bit(IWM_STATUS_SME_CONNECTING, &iwm->status);
-	clear_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
-
-	iwm->umac_profile_active = false;
-	memset(iwm->bssid, 0, ETH_ALEN);
-	iwm->channel = 0;
-
-	iwm_link_off(iwm);
-
-	wake_up_interruptible(&iwm->mlme_queue);
-
-	return 0;
-}
-
-#define IWM_DISCONNECT_INTERVAL	(5 * HZ)
-
-static int iwm_mlme_connection_terminated(struct iwm_priv *iwm, u8 *buf,
-					  unsigned long buf_size,
-					  struct iwm_wifi_cmd *cmd)
-{
-	IWM_DBG_MLME(iwm, DBG, "Connection terminated\n");
-
-	schedule_delayed_work(&iwm->disconnect, IWM_DISCONNECT_INTERVAL);
-
-	return 0;
-}
-
-static int iwm_mlme_scan_complete(struct iwm_priv *iwm, u8 *buf,
-				  unsigned long buf_size,
-				  struct iwm_wifi_cmd *cmd)
-{
-	int ret;
-	struct iwm_umac_notif_scan_complete *scan_complete =
-		(struct iwm_umac_notif_scan_complete *)buf;
-	u32 result = le32_to_cpu(scan_complete->result);
-
-	IWM_DBG_MLME(iwm, INFO, "type:0x%x result:0x%x seq:%d\n",
-		     le32_to_cpu(scan_complete->type),
-		     le32_to_cpu(scan_complete->result),
-		     scan_complete->seq_num);
-
-	if (!test_and_clear_bit(IWM_STATUS_SCANNING, &iwm->status)) {
-		IWM_ERR(iwm, "Scan complete while device not scanning\n");
-		return -EIO;
-	}
-	if (!iwm->scan_request)
-		return 0;
-
-	ret = iwm_cfg80211_inform_bss(iwm);
-
-	cfg80211_scan_done(iwm->scan_request,
-			   (result & UMAC_SCAN_RESULT_ABORTED) ? 1 : !!ret);
-	iwm->scan_request = NULL;
-
-	return ret;
-}
-
-static int iwm_mlme_update_sta_table(struct iwm_priv *iwm, u8 *buf,
-				     unsigned long buf_size,
-				     struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_sta_info *umac_sta =
-			(struct iwm_umac_notif_sta_info *)buf;
-	struct iwm_sta_info *sta;
-	int i;
-
-	switch (le32_to_cpu(umac_sta->opcode)) {
-	case UMAC_OPCODE_ADD_MODIFY:
-		sta = &iwm->sta_table[GET_VAL8(umac_sta->sta_id, LMAC_STA_ID)];
-
-		IWM_DBG_MLME(iwm, INFO, "%s STA: ID = %d, Color = %d, "
-			     "addr = %pM, qos = %d\n",
-			     sta->valid ? "Modify" : "Add",
-			     GET_VAL8(umac_sta->sta_id, LMAC_STA_ID),
-			     GET_VAL8(umac_sta->sta_id, LMAC_STA_COLOR),
-			     umac_sta->mac_addr,
-			     umac_sta->flags & UMAC_STA_FLAG_QOS);
-
-		sta->valid = true;
-		sta->qos = umac_sta->flags & UMAC_STA_FLAG_QOS;
-		sta->color = GET_VAL8(umac_sta->sta_id, LMAC_STA_COLOR);
-		memcpy(sta->addr, umac_sta->mac_addr, ETH_ALEN);
-		break;
-	case UMAC_OPCODE_REMOVE:
-		IWM_DBG_MLME(iwm, INFO, "Remove STA: ID = %d, Color = %d, "
-			     "addr = %pM\n",
-			     GET_VAL8(umac_sta->sta_id, LMAC_STA_ID),
-			     GET_VAL8(umac_sta->sta_id, LMAC_STA_COLOR),
-			     umac_sta->mac_addr);
-
-		sta = &iwm->sta_table[GET_VAL8(umac_sta->sta_id, LMAC_STA_ID)];
-
-		if (!memcmp(sta->addr, umac_sta->mac_addr, ETH_ALEN))
-			sta->valid = false;
-
-		break;
-	case UMAC_OPCODE_CLEAR_ALL:
-		for (i = 0; i < IWM_STA_TABLE_NUM; i++)
-			iwm->sta_table[i].valid = false;
-
-		break;
-	default:
-		break;
-	}
-
-	return 0;
-}
-
-static int iwm_mlme_medium_lost(struct iwm_priv *iwm, u8 *buf,
-				unsigned long buf_size,
-				struct iwm_wifi_cmd *cmd)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-
-	IWM_DBG_NTF(iwm, DBG, "WiFi/WiMax coexistence radio is OFF\n");
-
-	wiphy_rfkill_set_hw_state(wiphy, true);
-
-	return 0;
-}
-
-static int iwm_mlme_update_bss_table(struct iwm_priv *iwm, u8 *buf,
-				     unsigned long buf_size,
-				     struct iwm_wifi_cmd *cmd)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct ieee80211_mgmt *mgmt;
-	struct iwm_umac_notif_bss_info *umac_bss =
-			(struct iwm_umac_notif_bss_info *)buf;
-	struct ieee80211_channel *channel;
-	struct ieee80211_supported_band *band;
-	struct iwm_bss_info *bss;
-	s32 signal;
-	int freq;
-	u16 frame_len = le16_to_cpu(umac_bss->frame_len);
-	size_t bss_len = sizeof(struct iwm_umac_notif_bss_info) + frame_len;
-
-	mgmt = (struct ieee80211_mgmt *)(umac_bss->frame_buf);
-
-	IWM_DBG_MLME(iwm, DBG, "New BSS info entry: %pM\n", mgmt->bssid);
-	IWM_DBG_MLME(iwm, DBG, "\tType: 0x%x\n", le32_to_cpu(umac_bss->type));
-	IWM_DBG_MLME(iwm, DBG, "\tTimestamp: %d\n",
-		     le32_to_cpu(umac_bss->timestamp));
-	IWM_DBG_MLME(iwm, DBG, "\tTable Index: %d\n",
-		     le16_to_cpu(umac_bss->table_idx));
-	IWM_DBG_MLME(iwm, DBG, "\tBand: %d\n", umac_bss->band);
-	IWM_DBG_MLME(iwm, DBG, "\tChannel: %d\n", umac_bss->channel);
-	IWM_DBG_MLME(iwm, DBG, "\tRSSI: %d\n", umac_bss->rssi);
-	IWM_DBG_MLME(iwm, DBG, "\tFrame Length: %d\n", frame_len);
-
-	list_for_each_entry(bss, &iwm->bss_list, node)
-		if (bss->bss->table_idx == umac_bss->table_idx)
-			break;
-
-	if (&bss->node != &iwm->bss_list) {
-		/* Remove the old BSS entry, we will add it back later. */
-		list_del(&bss->node);
-		kfree(bss->bss);
-	} else {
-		/* New BSS entry */
-
-		bss = kzalloc(sizeof(struct iwm_bss_info), GFP_KERNEL);
-		if (!bss) {
-			IWM_ERR(iwm, "Couldn't allocate bss_info\n");
-			return -ENOMEM;
-		}
-	}
-
-	bss->bss = kzalloc(bss_len, GFP_KERNEL);
-	if (!bss->bss) {
-		kfree(bss);
-		IWM_ERR(iwm, "Couldn't allocate bss\n");
-		return -ENOMEM;
-	}
-
-	INIT_LIST_HEAD(&bss->node);
-	memcpy(bss->bss, umac_bss, bss_len);
-
-	if (umac_bss->band == UMAC_BAND_2GHZ)
-		band = wiphy->bands[IEEE80211_BAND_2GHZ];
-	else if (umac_bss->band == UMAC_BAND_5GHZ)
-		band = wiphy->bands[IEEE80211_BAND_5GHZ];
-	else {
-		IWM_ERR(iwm, "Invalid band: %d\n", umac_bss->band);
-		goto err;
-	}
-
-	freq = ieee80211_channel_to_frequency(umac_bss->channel, band->band);
-	channel = ieee80211_get_channel(wiphy, freq);
-	signal = umac_bss->rssi * 100;
-
-	bss->cfg_bss = cfg80211_inform_bss_frame(wiphy, channel,
-						 mgmt, frame_len,
-						 signal, GFP_KERNEL);
-	if (!bss->cfg_bss)
-		goto err;
-
-	list_add_tail(&bss->node, &iwm->bss_list);
-
-	return 0;
- err:
-	kfree(bss->bss);
-	kfree(bss);
-
-	return -EINVAL;
-}
-
-static int iwm_mlme_remove_bss(struct iwm_priv *iwm, u8 *buf,
-			       unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_bss_removed *bss_rm =
-		(struct iwm_umac_notif_bss_removed *)buf;
-	struct iwm_bss_info *bss, *next;
-	u16 table_idx;
-	int i;
-
-	for (i = 0; i < le32_to_cpu(bss_rm->count); i++) {
-		table_idx = le16_to_cpu(bss_rm->entries[i]) &
-			    IWM_BSS_REMOVE_INDEX_MSK;
-		list_for_each_entry_safe(bss, next, &iwm->bss_list, node)
-			if (bss->bss->table_idx == cpu_to_le16(table_idx)) {
-				struct ieee80211_mgmt *mgmt;
-
-				mgmt = (struct ieee80211_mgmt *)
-					(bss->bss->frame_buf);
-				IWM_DBG_MLME(iwm, ERR, "BSS removed: %pM\n",
-					     mgmt->bssid);
-				list_del(&bss->node);
-				kfree(bss->bss);
-				kfree(bss);
-			}
-	}
-
-	return 0;
-}
-
-static int iwm_mlme_mgt_frame(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_mgt_frame *mgt_frame =
-			(struct iwm_umac_notif_mgt_frame *)buf;
-	struct ieee80211_mgmt *mgt = (struct ieee80211_mgmt *)mgt_frame->frame;
-
-	IWM_HEXDUMP(iwm, DBG, MLME, "MGT: ", mgt_frame->frame,
-		    le16_to_cpu(mgt_frame->len));
-
-	if (ieee80211_is_assoc_req(mgt->frame_control)) {
-		iwm->req_ie_len = le16_to_cpu(mgt_frame->len)
-				  - offsetof(struct ieee80211_mgmt,
-					     u.assoc_req.variable);
-		kfree(iwm->req_ie);
-		iwm->req_ie = kmemdup(mgt->u.assoc_req.variable,
-				      iwm->req_ie_len, GFP_KERNEL);
-	} else if (ieee80211_is_reassoc_req(mgt->frame_control)) {
-		iwm->req_ie_len = le16_to_cpu(mgt_frame->len)
-				  - offsetof(struct ieee80211_mgmt,
-					     u.reassoc_req.variable);
-		kfree(iwm->req_ie);
-		iwm->req_ie = kmemdup(mgt->u.reassoc_req.variable,
-				      iwm->req_ie_len, GFP_KERNEL);
-	} else if (ieee80211_is_assoc_resp(mgt->frame_control)) {
-		iwm->resp_ie_len = le16_to_cpu(mgt_frame->len)
-				   - offsetof(struct ieee80211_mgmt,
-					      u.assoc_resp.variable);
-		kfree(iwm->resp_ie);
-		iwm->resp_ie = kmemdup(mgt->u.assoc_resp.variable,
-				       iwm->resp_ie_len, GFP_KERNEL);
-	} else if (ieee80211_is_reassoc_resp(mgt->frame_control)) {
-		iwm->resp_ie_len = le16_to_cpu(mgt_frame->len)
-				   - offsetof(struct ieee80211_mgmt,
-					      u.reassoc_resp.variable);
-		kfree(iwm->resp_ie);
-		iwm->resp_ie = kmemdup(mgt->u.reassoc_resp.variable,
-				       iwm->resp_ie_len, GFP_KERNEL);
-	} else {
-		IWM_ERR(iwm, "Unsupported management frame: 0x%x",
-			le16_to_cpu(mgt->frame_control));
-		return 0;
-	}
-
-	return 0;
-}
-
-static int iwm_ntf_mlme(struct iwm_priv *iwm, u8 *buf,
-			unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_wifi_if *notif =
-		(struct iwm_umac_notif_wifi_if *)buf;
-
-	switch (notif->status) {
-	case WIFI_IF_NTFY_ASSOC_START:
-		return iwm_mlme_assoc_start(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_ASSOC_COMPLETE:
-		return iwm_mlme_assoc_complete(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_PROFILE_INVALIDATE_COMPLETE:
-		return iwm_mlme_profile_invalidate(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_CONNECTION_TERMINATED:
-		return iwm_mlme_connection_terminated(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_SCAN_COMPLETE:
-		return iwm_mlme_scan_complete(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_STA_TABLE_CHANGE:
-		return iwm_mlme_update_sta_table(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_EXTENDED_IE_REQUIRED:
-		IWM_DBG_MLME(iwm, DBG, "Extended IE required\n");
-		break;
-	case WIFI_IF_NTFY_RADIO_PREEMPTION:
-		return iwm_mlme_medium_lost(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_BSS_TRK_TABLE_CHANGED:
-		return iwm_mlme_update_bss_table(iwm, buf, buf_size, cmd);
-	case WIFI_IF_NTFY_BSS_TRK_ENTRIES_REMOVED:
-		return iwm_mlme_remove_bss(iwm, buf, buf_size, cmd);
-		break;
-	case WIFI_IF_NTFY_MGMT_FRAME:
-		return iwm_mlme_mgt_frame(iwm, buf, buf_size, cmd);
-	case WIFI_DBG_IF_NTFY_SCAN_SUPER_JOB_START:
-	case WIFI_DBG_IF_NTFY_SCAN_SUPER_JOB_COMPLETE:
-	case WIFI_DBG_IF_NTFY_SCAN_CHANNEL_START:
-	case WIFI_DBG_IF_NTFY_SCAN_CHANNEL_RESULT:
-	case WIFI_DBG_IF_NTFY_SCAN_MINI_JOB_START:
-	case WIFI_DBG_IF_NTFY_SCAN_MINI_JOB_COMPLETE:
-	case WIFI_DBG_IF_NTFY_CNCT_ATC_START:
-	case WIFI_DBG_IF_NTFY_COEX_NOTIFICATION:
-	case WIFI_DBG_IF_NTFY_COEX_HANDLE_ENVELOP:
-	case WIFI_DBG_IF_NTFY_COEX_HANDLE_RELEASE_ENVELOP:
-		IWM_DBG_MLME(iwm, DBG, "MLME debug notification: 0x%x\n",
-			     notif->status);
-		break;
-	default:
-		IWM_ERR(iwm, "Unhandled notification: 0x%x\n", notif->status);
-		break;
-	}
-
-	return 0;
-}
-
-#define IWM_STATS_UPDATE_INTERVAL		(2 * HZ)
-
-static int iwm_ntf_statistics(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_stats *stats = (struct iwm_umac_notif_stats *)buf;
-	struct iw_statistics *wstats = &iwm->wstats;
-	u16 max_rate = 0;
-	int i;
-
-	IWM_DBG_MLME(iwm, DBG, "Statistics notification received\n");
-
-	if (test_bit(IWM_STATUS_ASSOCIATED, &iwm->status)) {
-		for (i = 0; i < UMAC_NTF_RATE_SAMPLE_NR; i++) {
-			max_rate = max_t(u16, max_rate,
-					 max(le16_to_cpu(stats->tx_rate[i]),
-					     le16_to_cpu(stats->rx_rate[i])));
-		}
-		/* UMAC passes rate info multiplies by 2 */
-		iwm->rate = max_rate >> 1;
-	}
-	iwm->txpower = le32_to_cpu(stats->tx_power);
-
-	wstats->status = 0;
-
-	wstats->discard.nwid = le32_to_cpu(stats->rx_drop_other_bssid);
-	wstats->discard.code = le32_to_cpu(stats->rx_drop_decode);
-	wstats->discard.fragment = le32_to_cpu(stats->rx_drop_reassembly);
-	wstats->discard.retries = le32_to_cpu(stats->tx_drop_max_retry);
-
-	wstats->miss.beacon = le32_to_cpu(stats->missed_beacons);
-
-	/* according to cfg80211 */
-	if (stats->rssi_dbm < -110)
-		wstats->qual.qual = 0;
-	else if (stats->rssi_dbm > -40)
-		wstats->qual.qual = 70;
-	else
-		wstats->qual.qual = stats->rssi_dbm + 110;
-
-	wstats->qual.level = stats->rssi_dbm;
-	wstats->qual.noise = stats->noise_dbm;
-	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-
-	schedule_delayed_work(&iwm->stats_request, IWM_STATS_UPDATE_INTERVAL);
-
-	mod_timer(&iwm->watchdog, round_jiffies(jiffies + IWM_WATCHDOG_PERIOD));
-
-	return 0;
-}
-
-static int iwm_ntf_eeprom_proxy(struct iwm_priv *iwm, u8 *buf,
-				unsigned long buf_size,
-				struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_cmd_eeprom_proxy *eeprom_proxy =
-		(struct iwm_umac_cmd_eeprom_proxy *)
-		(buf + sizeof(struct iwm_umac_wifi_in_hdr));
-	struct iwm_umac_cmd_eeprom_proxy_hdr *hdr = &eeprom_proxy->hdr;
-	u32 hdr_offset = le32_to_cpu(hdr->offset);
-	u32 hdr_len = le32_to_cpu(hdr->len);
-	u32 hdr_type = le32_to_cpu(hdr->type);
-
-	IWM_DBG_NTF(iwm, DBG, "type: 0x%x, len: %d, offset: 0x%x\n",
-		    hdr_type, hdr_len, hdr_offset);
-
-	if ((hdr_offset + hdr_len) > IWM_EEPROM_LEN)
-		return -EINVAL;
-
-	switch (hdr_type) {
-	case IWM_UMAC_CMD_EEPROM_TYPE_READ:
-		memcpy(iwm->eeprom + hdr_offset, eeprom_proxy->buf, hdr_len);
-		break;
-	case IWM_UMAC_CMD_EEPROM_TYPE_WRITE:
-	default:
-		return -ENOTSUPP;
-	}
-
-	return 0;
-}
-
-static int iwm_ntf_channel_info_list(struct iwm_priv *iwm, u8 *buf,
-				     unsigned long buf_size,
-				     struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_cmd_get_channel_list *ch_list =
-			(struct iwm_umac_cmd_get_channel_list *)
-			(buf + sizeof(struct iwm_umac_wifi_in_hdr));
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct ieee80211_supported_band *band;
-	int i;
-
-	band = wiphy->bands[IEEE80211_BAND_2GHZ];
-
-	for (i = 0; i < band->n_channels; i++) {
-		unsigned long ch_mask_0 =
-			le32_to_cpu(ch_list->ch[0].channels_mask);
-		unsigned long ch_mask_2 =
-			le32_to_cpu(ch_list->ch[2].channels_mask);
-
-		if (!test_bit(i, &ch_mask_0))
-			band->channels[i].flags |= IEEE80211_CHAN_DISABLED;
-
-		if (!test_bit(i, &ch_mask_2))
-			band->channels[i].flags |= IEEE80211_CHAN_NO_IBSS;
-	}
-
-	band = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-	for (i = 0; i < min(band->n_channels, 32); i++) {
-		unsigned long ch_mask_1 =
-			le32_to_cpu(ch_list->ch[1].channels_mask);
-		unsigned long ch_mask_3 =
-			le32_to_cpu(ch_list->ch[3].channels_mask);
-
-		if (!test_bit(i, &ch_mask_1))
-			band->channels[i].flags |= IEEE80211_CHAN_DISABLED;
-
-		if (!test_bit(i, &ch_mask_3))
-			band->channels[i].flags |= IEEE80211_CHAN_NO_IBSS;
-	}
-
-	return 0;
-}
-
-static int iwm_ntf_stop_resume_tx(struct iwm_priv *iwm, u8 *buf,
-				  unsigned long buf_size,
-				  struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_notif_stop_resume_tx *stp_res_tx =
-		(struct iwm_umac_notif_stop_resume_tx *)buf;
-	struct iwm_sta_info *sta_info;
-	struct iwm_tid_info *tid_info;
-	u8 sta_id = STA_ID_N_COLOR_ID(stp_res_tx->sta_id);
-	u16 tid_msk = le16_to_cpu(stp_res_tx->stop_resume_tid_msk);
-	int bit, ret = 0;
-	bool stop = false;
-
-	IWM_DBG_NTF(iwm, DBG, "stop/resume notification:\n"
-		    "\tflags:       0x%x\n"
-		    "\tSTA id:      %d\n"
-		    "\tTID bitmask: 0x%x\n",
-		    stp_res_tx->flags, stp_res_tx->sta_id,
-		    stp_res_tx->stop_resume_tid_msk);
-
-	if (stp_res_tx->flags & UMAC_STOP_TX_FLAG)
-		stop = true;
-
-	sta_info = &iwm->sta_table[sta_id];
-	if (!sta_info->valid) {
-		IWM_ERR(iwm, "Stoping an invalid STA: %d %d\n",
-			sta_id, stp_res_tx->sta_id);
-		return -EINVAL;
-	}
-
-	for_each_set_bit(bit, (unsigned long *)&tid_msk, IWM_UMAC_TID_NR) {
-		tid_info = &sta_info->tid_info[bit];
-
-		mutex_lock(&tid_info->mutex);
-		tid_info->stopped = stop;
-		mutex_unlock(&tid_info->mutex);
-
-		if (!stop) {
-			struct iwm_tx_queue *txq;
-			int queue = iwm_tid_to_queue(bit);
-
-			if (queue < 0)
-				continue;
-
-			txq = &iwm->txq[queue];
-			/*
-			 * If we resume, we have to move our SKBs
-			 * back to the tx queue and queue some work.
-			 */
-			spin_lock_bh(&txq->lock);
-			skb_queue_splice_init(&txq->queue, &txq->stopped_queue);
-			spin_unlock_bh(&txq->lock);
-
-			queue_work(txq->wq, &txq->worker);
-		}
-
-	}
-
-	/* We send an ACK only for the stop case */
-	if (stop)
-		ret = iwm_send_umac_stop_resume_tx(iwm, stp_res_tx);
-
-	return ret;
-}
-
-static int iwm_ntf_wifi_if_wrapper(struct iwm_priv *iwm, u8 *buf,
-				   unsigned long buf_size,
-				   struct iwm_wifi_cmd *cmd)
-{
-	struct iwm_umac_wifi_if *hdr;
-
-	if (cmd == NULL) {
-		IWM_ERR(iwm, "Couldn't find expected wifi command\n");
-		return -EINVAL;
-	}
-
-	hdr = (struct iwm_umac_wifi_if *)cmd->buf.payload;
-
-	IWM_DBG_NTF(iwm, DBG, "WIFI_IF_WRAPPER cmd is delivered to UMAC: "
-		    "oid is 0x%x\n", hdr->oid);
-
-	set_bit(hdr->oid, &iwm->wifi_ntfy[0]);
-	wake_up_interruptible(&iwm->wifi_ntfy_queue);
-
-	switch (hdr->oid) {
-	case UMAC_WIFI_IF_CMD_SET_PROFILE:
-		iwm->umac_profile_active = true;
-		break;
-	default:
-		break;
-	}
-
-	return 0;
-}
-
-#define CT_KILL_DELAY (30 * HZ)
-static int iwm_ntf_card_state(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size, struct iwm_wifi_cmd *cmd)
-{
-	struct wiphy *wiphy = iwm_to_wiphy(iwm);
-	struct iwm_lmac_card_state *state = (struct iwm_lmac_card_state *)
-				(buf + sizeof(struct iwm_umac_wifi_in_hdr));
-	u32 flags = le32_to_cpu(state->flags);
-
-	IWM_INFO(iwm, "HW RF Kill %s, CT Kill %s\n",
-		 flags & IWM_CARD_STATE_HW_DISABLED ? "ON" : "OFF",
-		 flags & IWM_CARD_STATE_CTKILL_DISABLED ? "ON" : "OFF");
-
-	if (flags & IWM_CARD_STATE_CTKILL_DISABLED) {
-		/*
-		 * We got a CTKILL event: We bring the interface down in
-		 * oder to cool the device down, and try to bring it up
-		 * 30 seconds later. If it's still too hot, we'll go through
-		 * this code path again.
-		 */
-		cancel_delayed_work_sync(&iwm->ct_kill_delay);
-		schedule_delayed_work(&iwm->ct_kill_delay, CT_KILL_DELAY);
-	}
-
-	wiphy_rfkill_set_hw_state(wiphy, flags &
-				  (IWM_CARD_STATE_HW_DISABLED |
-				   IWM_CARD_STATE_CTKILL_DISABLED));
-
-	return 0;
-}
-
-static int iwm_rx_handle_wifi(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size)
-{
-	struct iwm_umac_wifi_in_hdr *wifi_hdr;
-	struct iwm_wifi_cmd *cmd;
-	u8 source, cmd_id;
-	u16 seq_num;
-	u32 count;
-
-	wifi_hdr = (struct iwm_umac_wifi_in_hdr *)buf;
-	cmd_id = wifi_hdr->sw_hdr.cmd.cmd;
-	source = GET_VAL32(wifi_hdr->hw_hdr.cmd, UMAC_HDI_IN_CMD_SOURCE);
-	if (source >= IWM_SRC_NUM) {
-		IWM_CRIT(iwm, "invalid source %d\n", source);
-		return -EINVAL;
-	}
-
-	if (cmd_id == REPLY_RX_MPDU_CMD)
-		trace_iwm_rx_packet(iwm, buf, buf_size);
-	else if ((cmd_id == UMAC_NOTIFY_OPCODE_RX_TICKET) &&
-		 (source == UMAC_HDI_IN_SOURCE_FW))
-		trace_iwm_rx_ticket(iwm, buf, buf_size);
-	else
-		trace_iwm_rx_wifi_cmd(iwm, wifi_hdr);
-
-	count = GET_VAL32(wifi_hdr->sw_hdr.meta_data, UMAC_FW_CMD_BYTE_COUNT);
-	count += sizeof(struct iwm_umac_wifi_in_hdr) -
-		 sizeof(struct iwm_dev_cmd_hdr);
-	if (count > buf_size) {
-		IWM_CRIT(iwm, "count %d, buf size:%ld\n", count, buf_size);
-		return -EINVAL;
-	}
-
-	seq_num = le16_to_cpu(wifi_hdr->sw_hdr.cmd.seq_num);
-
-	IWM_DBG_RX(iwm, DBG, "CMD:0x%x, source: 0x%x, seqnum: %d\n",
-		   cmd_id, source, seq_num);
-
-	/*
-	 * If this is a response to a previously sent command, there must
-	 * be a pending command for this sequence number.
-	 */
-	cmd = iwm_get_pending_wifi_cmd(iwm, seq_num);
-
-	/* Notify the caller only for sync commands. */
-	switch (source) {
-	case UMAC_HDI_IN_SOURCE_FHRX:
-		if (iwm->lmac_handlers[cmd_id] &&
-		    test_bit(cmd_id, &iwm->lmac_handler_map[0]))
-			return iwm_notif_send(iwm, cmd, cmd_id, source,
-					      buf, count);
-		break;
-	case UMAC_HDI_IN_SOURCE_FW:
-		if (iwm->umac_handlers[cmd_id] &&
-		    test_bit(cmd_id, &iwm->umac_handler_map[0]))
-			return iwm_notif_send(iwm, cmd, cmd_id, source,
-					      buf, count);
-		break;
-	case UMAC_HDI_IN_SOURCE_UDMA:
-		break;
-	}
-
-	return iwm_rx_handle_resp(iwm, buf, count, cmd);
-}
-
-int iwm_rx_handle_resp(struct iwm_priv *iwm, u8 *buf, unsigned long buf_size,
-		       struct iwm_wifi_cmd *cmd)
-{
-	u8 source, cmd_id;
-	struct iwm_umac_wifi_in_hdr *wifi_hdr;
-	int ret = 0;
-
-	wifi_hdr = (struct iwm_umac_wifi_in_hdr *)buf;
-	cmd_id = wifi_hdr->sw_hdr.cmd.cmd;
-
-	source = GET_VAL32(wifi_hdr->hw_hdr.cmd, UMAC_HDI_IN_CMD_SOURCE);
-
-	IWM_DBG_RX(iwm, DBG, "CMD:0x%x, source: 0x%x\n", cmd_id, source);
-
-	switch (source) {
-	case UMAC_HDI_IN_SOURCE_FHRX:
-		if (iwm->lmac_handlers[cmd_id])
-			ret = iwm->lmac_handlers[cmd_id]
-					(iwm, buf, buf_size, cmd);
-		break;
-	case UMAC_HDI_IN_SOURCE_FW:
-		if (iwm->umac_handlers[cmd_id])
-			ret = iwm->umac_handlers[cmd_id]
-					(iwm, buf, buf_size, cmd);
-		break;
-	case UMAC_HDI_IN_SOURCE_UDMA:
-		ret = -EINVAL;
-		break;
-	}
-
-	kfree(cmd);
-
-	return ret;
-}
-
-static int iwm_rx_handle_nonwifi(struct iwm_priv *iwm, u8 *buf,
-				 unsigned long buf_size)
-{
-	u8 seq_num;
-	struct iwm_udma_in_hdr *hdr = (struct iwm_udma_in_hdr *)buf;
-	struct iwm_nonwifi_cmd *cmd;
-
-	trace_iwm_rx_nonwifi_cmd(iwm, buf, buf_size);
-	seq_num = GET_VAL32(hdr->cmd, UDMA_HDI_IN_CMD_NON_WIFI_HW_SEQ_NUM);
-
-	/*
-	 * We received a non wifi answer.
-	 * Let's check if there's a pending command for it, and if so
-	 * replace the command payload with the buffer, and then wake the
-	 * callers up.
-	 * That means we only support synchronised non wifi command response
-	 * schemes.
-	 */
-	list_for_each_entry(cmd, &iwm->nonwifi_pending_cmd, pending)
-		if (cmd->seq_num == seq_num) {
-			cmd->resp_received = true;
-			cmd->buf.len = buf_size;
-			memcpy(cmd->buf.hdr, buf, buf_size);
-			wake_up_interruptible(&iwm->nonwifi_queue);
-		}
-
-	return 0;
-}
-
-static int iwm_rx_handle_umac(struct iwm_priv *iwm, u8 *buf,
-			      unsigned long buf_size)
-{
-	int ret = 0;
-	u8 op_code;
-	unsigned long buf_offset = 0;
-	struct iwm_udma_in_hdr *hdr;
-
-	/*
-	 * To allow for a more efficient bus usage, UMAC
-	 * messages are encapsulated into UDMA ones. This
-	 * way we can have several UMAC messages in one bus
-	 * transfer.
-	 * A UDMA frame size is always aligned on 16 bytes,
-	 * and a UDMA frame must not start with a UMAC_PAD_TERMINAL
-	 * word. This is how we parse a bus frame into several
-	 * UDMA ones.
-	 */
-	while (buf_offset < buf_size) {
-
-		hdr = (struct iwm_udma_in_hdr *)(buf + buf_offset);
-
-		if (iwm_rx_check_udma_hdr(hdr) < 0) {
-			IWM_DBG_RX(iwm, DBG, "End of frame\n");
-			break;
-		}
-
-		op_code = GET_VAL32(hdr->cmd, UMAC_HDI_IN_CMD_OPCODE);
-
-		IWM_DBG_RX(iwm, DBG, "Op code: 0x%x\n", op_code);
-
-		if (op_code == UMAC_HDI_IN_OPCODE_WIFI) {
-			ret |= iwm_rx_handle_wifi(iwm, buf + buf_offset,
-						  buf_size - buf_offset);
-		} else if (op_code < UMAC_HDI_IN_OPCODE_NONWIFI_MAX) {
-			if (GET_VAL32(hdr->cmd,
-				      UDMA_HDI_IN_CMD_NON_WIFI_HW_SIG) !=
-			    UDMA_HDI_IN_CMD_NON_WIFI_HW_SIG) {
-				IWM_ERR(iwm, "Incorrect hw signature\n");
-				return -EINVAL;
-			}
-			ret |= iwm_rx_handle_nonwifi(iwm, buf + buf_offset,
-						     buf_size - buf_offset);
-		} else {
-			IWM_ERR(iwm, "Invalid RX opcode: 0x%x\n", op_code);
-			ret |= -EINVAL;
-		}
-
-		buf_offset += iwm_rx_resp_size(hdr);
-	}
-
-	return ret;
-}
-
-int iwm_rx_handle(struct iwm_priv *iwm, u8 *buf, unsigned long buf_size)
-{
-	struct iwm_udma_in_hdr *hdr;
-
-	hdr = (struct iwm_udma_in_hdr *)buf;
-
-	switch (le32_to_cpu(hdr->cmd)) {
-	case UMAC_REBOOT_BARKER:
-		if (test_bit(IWM_STATUS_READY, &iwm->status)) {
-			IWM_ERR(iwm, "Unexpected BARKER\n");
-
-			schedule_work(&iwm->reset_worker);
-
-			return 0;
-		}
-
-		return iwm_notif_send(iwm, NULL, IWM_BARKER_REBOOT_NOTIFICATION,
-				      IWM_SRC_UDMA, buf, buf_size);
-	case UMAC_ACK_BARKER:
-		return iwm_notif_send(iwm, NULL, IWM_ACK_BARKER_NOTIFICATION,
-				      IWM_SRC_UDMA, NULL, 0);
-	default:
-		IWM_DBG_RX(iwm, DBG, "Received cmd: 0x%x\n", hdr->cmd);
-		return iwm_rx_handle_umac(iwm, buf, buf_size);
-	}
-
-	return 0;
-}
-
-static const iwm_handler iwm_umac_handlers[] =
-{
-	[UMAC_NOTIFY_OPCODE_ERROR]		= iwm_ntf_error,
-	[UMAC_NOTIFY_OPCODE_ALIVE]		= iwm_ntf_umac_alive,
-	[UMAC_NOTIFY_OPCODE_INIT_COMPLETE]	= iwm_ntf_init_complete,
-	[UMAC_NOTIFY_OPCODE_WIFI_CORE_STATUS]	= iwm_ntf_wifi_status,
-	[UMAC_NOTIFY_OPCODE_WIFI_IF_WRAPPER]	= iwm_ntf_mlme,
-	[UMAC_NOTIFY_OPCODE_PAGE_DEALLOC]	= iwm_ntf_tx_credit_update,
-	[UMAC_NOTIFY_OPCODE_RX_TICKET]		= iwm_ntf_rx_ticket,
-	[UMAC_CMD_OPCODE_RESET]			= iwm_ntf_umac_reset,
-	[UMAC_NOTIFY_OPCODE_STATS]		= iwm_ntf_statistics,
-	[UMAC_CMD_OPCODE_EEPROM_PROXY]		= iwm_ntf_eeprom_proxy,
-	[UMAC_CMD_OPCODE_GET_CHAN_INFO_LIST]	= iwm_ntf_channel_info_list,
-	[UMAC_CMD_OPCODE_STOP_RESUME_STA_TX]	= iwm_ntf_stop_resume_tx,
-	[REPLY_RX_MPDU_CMD]			= iwm_ntf_rx_packet,
-	[UMAC_CMD_OPCODE_WIFI_IF_WRAPPER]	= iwm_ntf_wifi_if_wrapper,
-};
-
-static const iwm_handler iwm_lmac_handlers[] =
-{
-	[REPLY_TX]				= iwm_ntf_tx,
-	[REPLY_ALIVE]				= iwm_ntf_lmac_version,
-	[CALIBRATION_RES_NOTIFICATION]		= iwm_ntf_calib_res,
-	[CALIBRATION_COMPLETE_NOTIFICATION]	= iwm_ntf_calib_complete,
-	[CALIBRATION_CFG_CMD]			= iwm_ntf_calib_cfg,
-	[REPLY_RX_MPDU_CMD]			= iwm_ntf_rx_packet,
-	[CARD_STATE_NOTIFICATION]		= iwm_ntf_card_state,
-};
-
-void iwm_rx_setup_handlers(struct iwm_priv *iwm)
-{
-	iwm->umac_handlers = (iwm_handler *) iwm_umac_handlers;
-	iwm->lmac_handlers = (iwm_handler *) iwm_lmac_handlers;
-}
-
-static void iwm_remove_iv(struct sk_buff *skb, u32 hdr_total_len)
-{
-	struct ieee80211_hdr *hdr;
-	unsigned int hdr_len;
-
-	hdr = (struct ieee80211_hdr *)skb->data;
-
-	if (!ieee80211_has_protected(hdr->frame_control))
-		return;
-
-	hdr_len = ieee80211_hdrlen(hdr->frame_control);
-	if (hdr_total_len <= hdr_len)
-		return;
-
-	memmove(skb->data + (hdr_total_len - hdr_len), skb->data, hdr_len);
-	skb_pull(skb, (hdr_total_len - hdr_len));
-}
-
-static void iwm_rx_adjust_packet(struct iwm_priv *iwm,
-				 struct iwm_rx_packet *packet,
-				 struct iwm_rx_ticket_node *ticket_node)
-{
-	u32 payload_offset = 0, payload_len;
-	struct iwm_rx_ticket *ticket = ticket_node->ticket;
-	struct iwm_rx_mpdu_hdr *mpdu_hdr;
-	struct ieee80211_hdr *hdr;
-
-	mpdu_hdr = (struct iwm_rx_mpdu_hdr *)packet->skb->data;
-	payload_offset += sizeof(struct iwm_rx_mpdu_hdr);
-	/* Padding is 0 or 2 bytes */
-	payload_len = le16_to_cpu(mpdu_hdr->len) +
-		(le16_to_cpu(ticket->flags) & IWM_RX_TICKET_PAD_SIZE_MSK);
-	payload_len -= ticket->tail_len;
-
-	IWM_DBG_RX(iwm, DBG, "Packet adjusted, len:%d, offset:%d, "
-		   "ticket offset:%d ticket tail len:%d\n",
-		   payload_len, payload_offset, ticket->payload_offset,
-		   ticket->tail_len);
-
-	IWM_HEXDUMP(iwm, DBG, RX, "RAW: ", packet->skb->data, packet->skb->len);
-
-	skb_pull(packet->skb, payload_offset);
-	skb_trim(packet->skb, payload_len);
-
-	iwm_remove_iv(packet->skb, ticket->payload_offset);
-
-	hdr = (struct ieee80211_hdr *) packet->skb->data;
-	if (ieee80211_is_data_qos(hdr->frame_control)) {
-		/* UMAC handed QOS_DATA frame with 2 padding bytes appended
-		 * to the qos_ctl field in IEEE 802.11 headers. */
-		memmove(packet->skb->data + IEEE80211_QOS_CTL_LEN + 2,
-			packet->skb->data,
-			ieee80211_hdrlen(hdr->frame_control) -
-			IEEE80211_QOS_CTL_LEN);
-		hdr = (struct ieee80211_hdr *) skb_pull(packet->skb,
-				IEEE80211_QOS_CTL_LEN + 2);
-		hdr->frame_control &= ~cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
-	}
-
-	IWM_HEXDUMP(iwm, DBG, RX, "ADJUSTED: ",
-		    packet->skb->data, packet->skb->len);
-}
-
-static void classify8023(struct sk_buff *skb)
-{
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-
-	if (ieee80211_is_data_qos(hdr->frame_control)) {
-		u8 *qc = ieee80211_get_qos_ctl(hdr);
-		/* frame has qos control */
-		skb->priority = *qc & IEEE80211_QOS_CTL_TID_MASK;
-	} else {
-		skb->priority = 0;
-	}
-}
-
-static void iwm_rx_process_amsdu(struct iwm_priv *iwm, struct sk_buff *skb)
-{
-	struct wireless_dev *wdev = iwm_to_wdev(iwm);
-	struct net_device *ndev = iwm_to_ndev(iwm);
-	struct sk_buff_head list;
-	struct sk_buff *frame;
-
-	IWM_HEXDUMP(iwm, DBG, RX, "A-MSDU: ", skb->data, skb->len);
-
-	__skb_queue_head_init(&list);
-	ieee80211_amsdu_to_8023s(skb, &list, ndev->dev_addr, wdev->iftype, 0,
-									true);
-
-	while ((frame = __skb_dequeue(&list))) {
-		ndev->stats.rx_packets++;
-		ndev->stats.rx_bytes += frame->len;
-
-		frame->protocol = eth_type_trans(frame, ndev);
-		frame->ip_summed = CHECKSUM_NONE;
-		memset(frame->cb, 0, sizeof(frame->cb));
-
-		if (netif_rx_ni(frame) == NET_RX_DROP) {
-			IWM_ERR(iwm, "Packet dropped\n");
-			ndev->stats.rx_dropped++;
-		}
-	}
-}
-
-static void iwm_rx_process_packet(struct iwm_priv *iwm,
-				  struct iwm_rx_packet *packet,
-				  struct iwm_rx_ticket_node *ticket_node)
-{
-	int ret;
-	struct sk_buff *skb = packet->skb;
-	struct wireless_dev *wdev = iwm_to_wdev(iwm);
-	struct net_device *ndev = iwm_to_ndev(iwm);
-
-	IWM_DBG_RX(iwm, DBG, "Processing packet ID %d\n", packet->id);
-
-	switch (le16_to_cpu(ticket_node->ticket->action)) {
-	case IWM_RX_TICKET_RELEASE:
-		IWM_DBG_RX(iwm, DBG, "RELEASE packet\n");
-
-		iwm_rx_adjust_packet(iwm, packet, ticket_node);
-		skb->dev = iwm_to_ndev(iwm);
-		classify8023(skb);
-
-		if (le16_to_cpu(ticket_node->ticket->flags) &
-		    IWM_RX_TICKET_AMSDU_MSK) {
-			iwm_rx_process_amsdu(iwm, skb);
-			break;
-		}
-
-		ret = ieee80211_data_to_8023(skb, ndev->dev_addr, wdev->iftype);
-		if (ret < 0) {
-			IWM_DBG_RX(iwm, DBG, "Couldn't convert 802.11 header - "
-				   "%d\n", ret);
-			kfree_skb(packet->skb);
-			break;
-		}
-
-		IWM_HEXDUMP(iwm, DBG, RX, "802.3: ", skb->data, skb->len);
-
-		ndev->stats.rx_packets++;
-		ndev->stats.rx_bytes += skb->len;
-
-		skb->protocol = eth_type_trans(skb, ndev);
-		skb->ip_summed = CHECKSUM_NONE;
-		memset(skb->cb, 0, sizeof(skb->cb));
-
-		if (netif_rx_ni(skb) == NET_RX_DROP) {
-			IWM_ERR(iwm, "Packet dropped\n");
-			ndev->stats.rx_dropped++;
-		}
-		break;
-	case IWM_RX_TICKET_DROP:
-		IWM_DBG_RX(iwm, DBG, "DROP packet: 0x%x\n",
-			   le16_to_cpu(ticket_node->ticket->flags));
-		kfree_skb(packet->skb);
-		break;
-	default:
-		IWM_ERR(iwm, "Unknown ticket action: %d\n",
-			le16_to_cpu(ticket_node->ticket->action));
-		kfree_skb(packet->skb);
-	}
-
-	kfree(packet);
-	iwm_rx_ticket_node_free(ticket_node);
-}
-
-/*
- * Rx data processing:
- *
- * We're receiving Rx packet from the LMAC, and Rx ticket from
- * the UMAC.
- * To forward a target data packet upstream (i.e. to the
- * kernel network stack), we must have received an Rx ticket
- * that tells us we're allowed to release this packet (ticket
- * action is IWM_RX_TICKET_RELEASE). The Rx ticket also indicates,
- * among other things, where valid data actually starts in the Rx
- * packet.
- */
-void iwm_rx_worker(struct work_struct *work)
-{
-	struct iwm_priv *iwm;
-	struct iwm_rx_ticket_node *ticket, *next;
-
-	iwm = container_of(work, struct iwm_priv, rx_worker);
-
-	/*
-	 * We go through the tickets list and if there is a pending
-	 * packet for it, we push it upstream.
-	 * We stop whenever a ticket is missing its packet, as we're
-	 * supposed to send the packets in order.
-	 */
-	spin_lock(&iwm->ticket_lock);
-	list_for_each_entry_safe(ticket, next, &iwm->rx_tickets, node) {
-		struct iwm_rx_packet *packet =
-			iwm_rx_packet_get(iwm, le16_to_cpu(ticket->ticket->id));
-
-		if (!packet) {
-			IWM_DBG_RX(iwm, DBG, "Skip rx_work: Wait for ticket %d "
-				   "to be handled first\n",
-				   le16_to_cpu(ticket->ticket->id));
-			break;
-		}
-
-		list_del(&ticket->node);
-		iwm_rx_process_packet(iwm, packet, ticket);
-	}
-	spin_unlock(&iwm->ticket_lock);
-}
-
diff --git a/drivers/net/wireless/iwmc3200wifi/rx.h b/drivers/net/wireless/iwmc3200wifi/rx.h
deleted file mode 100644
index da0db91cee5..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/rx.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_RX_H__
-#define __IWM_RX_H__
-
-#include <linux/skbuff.h>
-
-#include "umac.h"
-
-struct iwm_rx_ticket_node {
-	struct list_head node;
-	struct iwm_rx_ticket *ticket;
-};
-
-struct iwm_rx_packet {
-	struct list_head node;
-	u16 id;
-	struct sk_buff *skb;
-	unsigned long pkt_size;
-};
-
-void iwm_rx_worker(struct work_struct *work);
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/sdio.c b/drivers/net/wireless/iwmc3200wifi/sdio.c
deleted file mode 100644
index 0042f204b07..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/sdio.c
+++ /dev/null
@@ -1,509 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-/*
- * This is the SDIO bus specific hooks for iwm.
- * It also is the module's entry point.
- *
- * Interesting code paths:
- * iwm_sdio_probe() (Called by an SDIO bus scan)
- *  -> iwm_if_alloc() (netdev.c)
- *      -> iwm_wdev_alloc() (cfg80211.c, allocates and register our wiphy)
- *          -> wiphy_new()
- *          -> wiphy_register()
- *      -> alloc_netdev_mq()
- *      -> register_netdev()
- *
- * iwm_sdio_remove()
- *  -> iwm_if_free() (netdev.c)
- *      -> unregister_netdev()
- *      -> iwm_wdev_free() (cfg80211.c)
- *          -> wiphy_unregister()
- *          -> wiphy_free()
- *
- * iwm_sdio_isr() (called in process context from the SDIO core code)
- *  -> queue_work(.., isr_worker)
- *      -- [async] --> iwm_sdio_isr_worker()
- *                      -> iwm_rx_handle()
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/netdevice.h>
-#include <linux/debugfs.h>
-#include <linux/mmc/sdio_ids.h>
-#include <linux/mmc/sdio.h>
-#include <linux/mmc/sdio_func.h>
-
-#include "iwm.h"
-#include "debug.h"
-#include "bus.h"
-#include "sdio.h"
-
-static void iwm_sdio_isr_worker(struct work_struct *work)
-{
-	struct iwm_sdio_priv *hw;
-	struct iwm_priv *iwm;
-	struct iwm_rx_info *rx_info;
-	struct sk_buff *skb;
-	u8 *rx_buf;
-	unsigned long rx_size;
-
-	hw = container_of(work, struct iwm_sdio_priv, isr_worker);
-	iwm = hw_to_iwm(hw);
-
-	while (!skb_queue_empty(&iwm->rx_list)) {
-		skb = skb_dequeue(&iwm->rx_list);
-		rx_info = skb_to_rx_info(skb);
-		rx_size = rx_info->rx_size;
-		rx_buf = skb->data;
-
-		IWM_HEXDUMP(iwm, DBG, SDIO, "RX: ", rx_buf, rx_size);
-		if (iwm_rx_handle(iwm, rx_buf, rx_size) < 0)
-			IWM_WARN(iwm, "RX error\n");
-
-		kfree_skb(skb);
-	}
-}
-
-static void iwm_sdio_isr(struct sdio_func *func)
-{
-	struct iwm_priv *iwm;
-	struct iwm_sdio_priv *hw;
-	struct iwm_rx_info *rx_info;
-	struct sk_buff *skb;
-	unsigned long buf_size, read_size;
-	int ret;
-	u8 val;
-
-	hw = sdio_get_drvdata(func);
-	iwm = hw_to_iwm(hw);
-
-	buf_size = hw->blk_size;
-
-	/* We're checking the status */
-	val = sdio_readb(func, IWM_SDIO_INTR_STATUS_ADDR, &ret);
-	if (val == 0 || ret < 0) {
-		IWM_ERR(iwm, "Wrong INTR_STATUS\n");
-		return;
-	}
-
-	/* See if we have free buffers */
-	if (skb_queue_len(&iwm->rx_list) > IWM_RX_LIST_SIZE) {
-		IWM_ERR(iwm, "No buffer for more Rx frames\n");
-		return;
-	}
-
-	/* We first read the transaction size */
-	read_size = sdio_readb(func, IWM_SDIO_INTR_GET_SIZE_ADDR + 1, &ret);
-	read_size = read_size << 8;
-
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't read the xfer size\n");
-		return;
-	}
-
-	/* We need to clear the INT register */
-	sdio_writeb(func, 1, IWM_SDIO_INTR_CLEAR_ADDR, &ret);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't clear the INT register\n");
-		return;
-	}
-
-	while (buf_size < read_size)
-		buf_size <<= 1;
-
-	skb = dev_alloc_skb(buf_size);
-	if (!skb) {
-		IWM_ERR(iwm, "Couldn't alloc RX skb\n");
-		return;
-	}
-	rx_info = skb_to_rx_info(skb);
-	rx_info->rx_size = read_size;
-	rx_info->rx_buf_size = buf_size;
-
-	/* Now we can read the actual buffer */
-	ret = sdio_memcpy_fromio(func, skb_put(skb, read_size),
-				 IWM_SDIO_DATA_ADDR, read_size);
-
-	/* The skb is put on a driver's specific Rx SKB list */
-	skb_queue_tail(&iwm->rx_list, skb);
-
-	/* We can now schedule the actual worker */
-	queue_work(hw->isr_wq, &hw->isr_worker);
-}
-
-static void iwm_sdio_rx_free(struct iwm_sdio_priv *hw)
-{
-	struct iwm_priv *iwm = hw_to_iwm(hw);
-
-	flush_workqueue(hw->isr_wq);
-
-	skb_queue_purge(&iwm->rx_list);
-}
-
-/* Bus ops */
-static int if_sdio_enable(struct iwm_priv *iwm)
-{
-	struct iwm_sdio_priv *hw = iwm_to_if_sdio(iwm);
-	int ret;
-
-	sdio_claim_host(hw->func);
-
-	ret = sdio_enable_func(hw->func);
-	if (ret) {
-		IWM_ERR(iwm, "Couldn't enable the device: is TOP driver "
-			"loaded and functional?\n");
-		goto release_host;
-	}
-
-	iwm_reset(iwm);
-
-	ret = sdio_claim_irq(hw->func, iwm_sdio_isr);
-	if (ret) {
-		IWM_ERR(iwm, "Failed to claim irq: %d\n", ret);
-		goto release_host;
-	}
-
-	sdio_writeb(hw->func, 1, IWM_SDIO_INTR_ENABLE_ADDR, &ret);
-	if (ret < 0) {
-		IWM_ERR(iwm, "Couldn't enable INTR: %d\n", ret);
-		goto release_irq;
-	}
-
-	sdio_release_host(hw->func);
-
-	IWM_DBG_SDIO(iwm, INFO, "IWM SDIO enable\n");
-
-	return 0;
-
- release_irq:
-	sdio_release_irq(hw->func);
- release_host:
-	sdio_release_host(hw->func);
-
-	return ret;
-}
-
-static int if_sdio_disable(struct iwm_priv *iwm)
-{
-	struct iwm_sdio_priv *hw = iwm_to_if_sdio(iwm);
-	int ret;
-
-	sdio_claim_host(hw->func);
-	sdio_writeb(hw->func, 0, IWM_SDIO_INTR_ENABLE_ADDR, &ret);
-	if (ret < 0)
-		IWM_WARN(iwm, "Couldn't disable INTR: %d\n", ret);
-
-	sdio_release_irq(hw->func);
-	sdio_disable_func(hw->func);
-	sdio_release_host(hw->func);
-
-	iwm_sdio_rx_free(hw);
-
-	iwm_reset(iwm);
-
-	IWM_DBG_SDIO(iwm, INFO, "IWM SDIO disable\n");
-
-	return 0;
-}
-
-static int if_sdio_send_chunk(struct iwm_priv *iwm, u8 *buf, int count)
-{
-	struct iwm_sdio_priv *hw = iwm_to_if_sdio(iwm);
-	int aligned_count = ALIGN(count, hw->blk_size);
-	int ret;
-
-	if ((unsigned long)buf & 0x3) {
-		IWM_ERR(iwm, "buf <%p> is not dword aligned\n", buf);
-		/* TODO: Is this a hardware limitation? use get_unligned */
-		return -EINVAL;
-	}
-
-	sdio_claim_host(hw->func);
-	ret = sdio_memcpy_toio(hw->func, IWM_SDIO_DATA_ADDR, buf,
-			       aligned_count);
-	sdio_release_host(hw->func);
-
-	return ret;
-}
-
-static ssize_t iwm_debugfs_sdio_read(struct file *filp, char __user *buffer,
-				     size_t count, loff_t *ppos)
-{
-	struct iwm_priv *iwm = filp->private_data;
-	struct iwm_sdio_priv *hw = iwm_to_if_sdio(iwm);
-	char *buf;
-	u8 cccr;
-	int buf_len = 4096, ret;
-	size_t len = 0;
-
-	if (*ppos != 0)
-		return 0;
-	if (count < sizeof(buf))
-		return -ENOSPC;
-
-	buf = kzalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	sdio_claim_host(hw->func);
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_IOEx, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_IOEx\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_IOEx:  0x%x\n", cccr);
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_IORx, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_IORx\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_IORx:  0x%x\n", cccr);
-
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_IENx, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_IENx\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_IENx:  0x%x\n", cccr);
-
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_INTx, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_INTx\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_INTx:  0x%x\n", cccr);
-
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_ABORT, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_ABORTx\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_ABORT: 0x%x\n", cccr);
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_IF, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_IF\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_IF:    0x%x\n", cccr);
-
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_CAPS, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_CAPS\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_CAPS:  0x%x\n", cccr);
-
-	cccr =  sdio_f0_readb(hw->func, SDIO_CCCR_CIS, &ret);
-	if (ret) {
-		IWM_ERR(iwm, "Could not read SDIO_CCCR_CIS\n");
-		goto err;
-	}
-	len += snprintf(buf + len, buf_len - len, "CCCR_CIS:   0x%x\n", cccr);
-
-	ret = simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
-err:
-	sdio_release_host(hw->func);
-
-	kfree(buf);
-
-	return ret;
-}
-
-static const struct file_operations iwm_debugfs_sdio_fops = {
-	.owner =	THIS_MODULE,
-	.open =		simple_open,
-	.read =		iwm_debugfs_sdio_read,
-	.llseek =	default_llseek,
-};
-
-static void if_sdio_debugfs_init(struct iwm_priv *iwm, struct dentry *parent_dir)
-{
-	struct iwm_sdio_priv *hw = iwm_to_if_sdio(iwm);
-
-	hw->cccr_dentry = debugfs_create_file("cccr", 0200,
-					      parent_dir, iwm,
-					      &iwm_debugfs_sdio_fops);
-}
-
-static void if_sdio_debugfs_exit(struct iwm_priv *iwm)
-{
-	struct iwm_sdio_priv *hw = iwm_to_if_sdio(iwm);
-
-	debugfs_remove(hw->cccr_dentry);
-}
-
-static struct iwm_if_ops if_sdio_ops = {
-	.enable = if_sdio_enable,
-	.disable = if_sdio_disable,
-	.send_chunk = if_sdio_send_chunk,
-	.debugfs_init = if_sdio_debugfs_init,
-	.debugfs_exit = if_sdio_debugfs_exit,
-	.umac_name = "iwmc3200wifi-umac-sdio.bin",
-	.calib_lmac_name = "iwmc3200wifi-calib-sdio.bin",
-	.lmac_name = "iwmc3200wifi-lmac-sdio.bin",
-};
-MODULE_FIRMWARE("iwmc3200wifi-umac-sdio.bin");
-MODULE_FIRMWARE("iwmc3200wifi-calib-sdio.bin");
-MODULE_FIRMWARE("iwmc3200wifi-lmac-sdio.bin");
-
-static int iwm_sdio_probe(struct sdio_func *func,
-			  const struct sdio_device_id *id)
-{
-	struct iwm_priv *iwm;
-	struct iwm_sdio_priv *hw;
-	struct device *dev = &func->dev;
-	int ret;
-
-	/* check if TOP has already initialized the card */
-	sdio_claim_host(func);
-	ret = sdio_enable_func(func);
-	if (ret) {
-		dev_err(dev, "wait for TOP to enable the device\n");
-		sdio_release_host(func);
-		return ret;
-	}
-
-	ret = sdio_set_block_size(func, IWM_SDIO_BLK_SIZE);
-
-	sdio_disable_func(func);
-	sdio_release_host(func);
-
-	if (ret < 0) {
-		dev_err(dev, "Failed to set block size: %d\n", ret);
-		return ret;
-	}
-
-	iwm = iwm_if_alloc(sizeof(struct iwm_sdio_priv), dev, &if_sdio_ops);
-	if (IS_ERR(iwm)) {
-		dev_err(dev, "allocate SDIO interface failed\n");
-		return PTR_ERR(iwm);
-	}
-
-	hw = iwm_private(iwm);
-	hw->iwm = iwm;
-
-	iwm_debugfs_init(iwm);
-
-	sdio_set_drvdata(func, hw);
-
-	hw->func = func;
-	hw->blk_size = IWM_SDIO_BLK_SIZE;
-
-	hw->isr_wq = create_singlethread_workqueue(KBUILD_MODNAME "_sdio");
-	if (!hw->isr_wq) {
-		ret = -ENOMEM;
-		goto debugfs_exit;
-	}
-
-	INIT_WORK(&hw->isr_worker, iwm_sdio_isr_worker);
-
-	ret = iwm_if_add(iwm);
-	if (ret) {
-		dev_err(dev, "add SDIO interface failed\n");
-		goto destroy_wq;
-	}
-
-	dev_info(dev, "IWM SDIO probe\n");
-
-	return 0;
-
- destroy_wq:
-	destroy_workqueue(hw->isr_wq);
- debugfs_exit:
-	iwm_debugfs_exit(iwm);
-	iwm_if_free(iwm);
-	return ret;
-}
-
-static void iwm_sdio_remove(struct sdio_func *func)
-{
-	struct iwm_sdio_priv *hw = sdio_get_drvdata(func);
-	struct iwm_priv *iwm = hw_to_iwm(hw);
-	struct device *dev = &func->dev;
-
-	iwm_if_remove(iwm);
-	destroy_workqueue(hw->isr_wq);
-	iwm_debugfs_exit(iwm);
-	iwm_if_free(iwm);
-
-	sdio_set_drvdata(func, NULL);
-
-	dev_info(dev, "IWM SDIO remove\n");
-}
-
-static const struct sdio_device_id iwm_sdio_ids[] = {
-	/* Global/AGN SKU */
-	{ SDIO_DEVICE(SDIO_VENDOR_ID_INTEL, 0x1403) },
-	/* BGN SKU */
-	{ SDIO_DEVICE(SDIO_VENDOR_ID_INTEL, 0x1408) },
-	{ /* end: all zeroes */	},
-};
-MODULE_DEVICE_TABLE(sdio, iwm_sdio_ids);
-
-static struct sdio_driver iwm_sdio_driver = {
-	.name		= "iwm_sdio",
-	.id_table	= iwm_sdio_ids,
-	.probe		= iwm_sdio_probe,
-	.remove		= iwm_sdio_remove,
-};
-
-static int __init iwm_sdio_init_module(void)
-{
-	return sdio_register_driver(&iwm_sdio_driver);
-}
-
-static void __exit iwm_sdio_exit_module(void)
-{
-	sdio_unregister_driver(&iwm_sdio_driver);
-}
-
-module_init(iwm_sdio_init_module);
-module_exit(iwm_sdio_exit_module);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR(IWM_COPYRIGHT " " IWM_AUTHOR);
diff --git a/drivers/net/wireless/iwmc3200wifi/sdio.h b/drivers/net/wireless/iwmc3200wifi/sdio.h
deleted file mode 100644
index aab6b6892e4..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/sdio.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_SDIO_H__
-#define __IWM_SDIO_H__
-
-#define IWM_SDIO_DATA_ADDR           0x0
-#define IWM_SDIO_INTR_ENABLE_ADDR    0x14
-#define IWM_SDIO_INTR_STATUS_ADDR    0x13
-#define IWM_SDIO_INTR_CLEAR_ADDR     0x13
-#define IWM_SDIO_INTR_GET_SIZE_ADDR  0x2C
-
-#define IWM_SDIO_BLK_SIZE       256
-
-#define iwm_to_if_sdio(i) (struct iwm_sdio_priv *)(iwm->private)
-
-struct iwm_sdio_priv {
-	struct sdio_func *func;
-	struct iwm_priv *iwm;
-
-	struct workqueue_struct *isr_wq;
-	struct work_struct isr_worker;
-
-	struct dentry *cccr_dentry;
-
-	unsigned int blk_size;
-};
-
-#endif
diff --git a/drivers/net/wireless/iwmc3200wifi/trace.c b/drivers/net/wireless/iwmc3200wifi/trace.c
deleted file mode 100644
index 904d36f2231..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/trace.c
+++ /dev/null
@@ -1,3 +0,0 @@
-#include "iwm.h"
-#define CREATE_TRACE_POINTS
-#include "trace.h"
diff --git a/drivers/net/wireless/iwmc3200wifi/trace.h b/drivers/net/wireless/iwmc3200wifi/trace.h
deleted file mode 100644
index f5f7070b7e2..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/trace.h
+++ /dev/null
@@ -1,283 +0,0 @@
-#if !defined(__IWM_TRACE_H__) || defined(TRACE_HEADER_MULTI_READ)
-#define __IWM_TRACE_H__
-
-#include <linux/tracepoint.h>
-
-#if !defined(CONFIG_IWM_TRACING)
-#undef TRACE_EVENT
-#define TRACE_EVENT(name, proto, ...) \
-static inline void trace_ ## name(proto) {}
-#endif
-
-#undef TRACE_SYSTEM
-#define TRACE_SYSTEM iwm
-
-#define IWM_ENTRY	__array(char, ndev_name, 16)
-#define IWM_ASSIGN	strlcpy(__entry->ndev_name, iwm_to_ndev(iwm)->name, 16)
-#define IWM_PR_FMT	"%s"
-#define IWM_PR_ARG	__entry->ndev_name
-
-TRACE_EVENT(iwm_tx_nonwifi_cmd,
-	TP_PROTO(struct iwm_priv *iwm, struct iwm_udma_out_nonwifi_hdr *hdr),
-
-	TP_ARGS(iwm, hdr),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, opcode)
-		__field(u8, resp)
-		__field(u8, eot)
-		__field(u8, hw)
-		__field(u16, seq)
-		__field(u32, addr)
-		__field(u32, op1)
-		__field(u32, op2)
-	),
-
-	TP_fast_assign(
-		IWM_ASSIGN;
-		__entry->opcode = GET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_OPCODE);
-		__entry->resp = GET_VAL32(hdr->cmd, UDMA_HDI_OUT_NW_CMD_RESP);
-		__entry->eot = GET_VAL32(hdr->cmd, UMAC_HDI_OUT_CMD_EOT);
-		__entry->hw = GET_VAL32(hdr->cmd, UDMA_HDI_OUT_NW_CMD_HANDLE_BY_HW);
-		__entry->seq = GET_VAL32(hdr->cmd, UDMA_HDI_OUT_CMD_NON_WIFI_HW_SEQ_NUM);
-		__entry->addr = le32_to_cpu(hdr->addr);
-		__entry->op1 = le32_to_cpu(hdr->op1_sz);
-		__entry->op2 = le32_to_cpu(hdr->op2);
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Tx TARGET CMD: opcode 0x%x, resp %d, eot %d, "
-		"hw %d, seq 0x%x, addr 0x%x, op1 0x%x, op2 0x%x",
-		IWM_PR_ARG, __entry->opcode, __entry->resp, __entry->eot,
-		__entry->hw, __entry->seq, __entry->addr, __entry->op1,
-		__entry->op2
-	)
-);
-
-TRACE_EVENT(iwm_tx_wifi_cmd,
-	TP_PROTO(struct iwm_priv *iwm, struct iwm_umac_wifi_out_hdr *hdr),
-
-	TP_ARGS(iwm, hdr),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, opcode)
-		__field(u8, lmac)
-		__field(u8, resp)
-		__field(u8, eot)
-		__field(u8, ra_tid)
-		__field(u8, credit_group)
-		__field(u8, color)
-		__field(u16, seq)
-	),
-
-	TP_fast_assign(
-		IWM_ASSIGN;
-		__entry->opcode = hdr->sw_hdr.cmd.cmd;
-		__entry->lmac = 0;
-		__entry->seq = __le16_to_cpu(hdr->sw_hdr.cmd.seq_num);
-		__entry->resp = GET_VAL8(hdr->sw_hdr.cmd.flags, UMAC_DEV_CMD_FLAGS_RESP_REQ);
-		__entry->color = GET_VAL32(hdr->sw_hdr.meta_data, UMAC_FW_CMD_TX_STA_COLOR);
-		__entry->eot = GET_VAL32(hdr->hw_hdr.cmd, UMAC_HDI_OUT_CMD_EOT);
-		__entry->ra_tid = GET_VAL32(hdr->hw_hdr.meta_data, UMAC_HDI_OUT_RATID);
-		__entry->credit_group = GET_VAL32(hdr->hw_hdr.meta_data, UMAC_HDI_OUT_CREDIT_GRP);
-		if (__entry->opcode == UMAC_CMD_OPCODE_WIFI_PASS_THROUGH ||
-		    __entry->opcode == UMAC_CMD_OPCODE_WIFI_IF_WRAPPER) {
-			__entry->lmac = 1;
-			__entry->opcode = ((struct iwm_lmac_hdr *)(hdr + 1))->id;
-		}
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Tx %cMAC CMD: opcode 0x%x, resp %d, eot %d, "
-		"seq 0x%x, sta_color 0x%x, ra_tid 0x%x, credit_group 0x%x",
-		IWM_PR_ARG, __entry->lmac ? 'L' : 'U', __entry->opcode,
-		__entry->resp, __entry->eot, __entry->seq, __entry->color,
-		__entry->ra_tid, __entry->credit_group
-	)
-);
-
-TRACE_EVENT(iwm_tx_packets,
-	TP_PROTO(struct iwm_priv *iwm, u8 *buf, int len),
-
-	TP_ARGS(iwm, buf, len),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, eot)
-		__field(u8, ra_tid)
-		__field(u8, credit_group)
-		__field(u8, color)
-		__field(u16, seq)
-		__field(u8, npkt)
-		__field(u32, bytes)
-	),
-
-	TP_fast_assign(
-		struct iwm_umac_wifi_out_hdr *hdr =
-			(struct iwm_umac_wifi_out_hdr *)buf;
-
-		IWM_ASSIGN;
-		__entry->eot = GET_VAL32(hdr->hw_hdr.cmd, UMAC_HDI_OUT_CMD_EOT);
-		__entry->ra_tid = GET_VAL32(hdr->hw_hdr.meta_data, UMAC_HDI_OUT_RATID);
-		__entry->credit_group = GET_VAL32(hdr->hw_hdr.meta_data, UMAC_HDI_OUT_CREDIT_GRP);
-		__entry->color = GET_VAL32(hdr->sw_hdr.meta_data, UMAC_FW_CMD_TX_STA_COLOR);
-		__entry->seq = __le16_to_cpu(hdr->sw_hdr.cmd.seq_num);
-		__entry->npkt = 1;
-		__entry->bytes = len;
-
-		if (!__entry->eot) {
-			int count;
-			u8 *ptr = buf;
-
-			__entry->npkt = 0;
-			while (ptr < buf + len) {
-				count = GET_VAL32(hdr->sw_hdr.meta_data,
-						  UMAC_FW_CMD_BYTE_COUNT);
-				ptr += ALIGN(sizeof(*hdr) + count, 16);
-				hdr = (struct iwm_umac_wifi_out_hdr *)ptr;
-				__entry->npkt++;
-			}
-		}
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Tx %spacket: eot %d, seq 0x%x, sta_color 0x%x, "
-		"ra_tid 0x%x, credit_group 0x%x, embedded_packets %d, %d bytes",
-		IWM_PR_ARG, !__entry->eot ? "concatenated " : "",
-		__entry->eot, __entry->seq, __entry->color, __entry->ra_tid,
-		__entry->credit_group, __entry->npkt, __entry->bytes
-	)
-);
-
-TRACE_EVENT(iwm_rx_nonwifi_cmd,
-	TP_PROTO(struct iwm_priv *iwm, void *buf, int len),
-
-	TP_ARGS(iwm, buf, len),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, opcode)
-		__field(u16, seq)
-		__field(u32, len)
-	),
-
-	TP_fast_assign(
-		struct iwm_udma_in_hdr *hdr = buf;
-
-		IWM_ASSIGN;
-		__entry->opcode = GET_VAL32(hdr->cmd, UDMA_HDI_IN_NW_CMD_OPCODE);
-		__entry->seq = GET_VAL32(hdr->cmd, UDMA_HDI_IN_CMD_NON_WIFI_HW_SEQ_NUM);
-		__entry->len = len;
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Rx TARGET RESP: opcode 0x%x, seq 0x%x, len 0x%x",
-		IWM_PR_ARG, __entry->opcode, __entry->seq, __entry->len
-	)
-);
-
-TRACE_EVENT(iwm_rx_wifi_cmd,
-	TP_PROTO(struct iwm_priv *iwm, struct iwm_umac_wifi_in_hdr *hdr),
-
-	TP_ARGS(iwm, hdr),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, cmd)
-		__field(u8, source)
-		__field(u16, seq)
-		__field(u32, count)
-	),
-
-	TP_fast_assign(
-		IWM_ASSIGN;
-		__entry->cmd = hdr->sw_hdr.cmd.cmd;
-		__entry->source = GET_VAL32(hdr->hw_hdr.cmd, UMAC_HDI_IN_CMD_SOURCE);
-		__entry->count = GET_VAL32(hdr->sw_hdr.meta_data, UMAC_FW_CMD_BYTE_COUNT);
-		__entry->seq = le16_to_cpu(hdr->sw_hdr.cmd.seq_num);
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Rx %s RESP: cmd 0x%x, seq 0x%x, count 0x%x",
-		IWM_PR_ARG, __entry->source == UMAC_HDI_IN_SOURCE_FHRX ? "LMAC" :
-		__entry->source == UMAC_HDI_IN_SOURCE_FW ? "UMAC" : "UDMA",
-		__entry->cmd, __entry->seq, __entry->count
-	)
-);
-
-#define iwm_ticket_action_symbol		\
-	{ IWM_RX_TICKET_DROP, "DROP" },		\
-	{ IWM_RX_TICKET_RELEASE, "RELEASE" },	\
-	{ IWM_RX_TICKET_SNIFFER, "SNIFFER" },	\
-	{ IWM_RX_TICKET_ENQUEUE, "ENQUEUE" }
-
-TRACE_EVENT(iwm_rx_ticket,
-	TP_PROTO(struct iwm_priv *iwm, void *buf, int len),
-
-	TP_ARGS(iwm, buf, len),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, action)
-		__field(u8, reason)
-		__field(u16, id)
-		__field(u16, flags)
-	),
-
-	TP_fast_assign(
-		struct iwm_rx_ticket *ticket =
-			((struct iwm_umac_notif_rx_ticket *)buf)->tickets;
-
-		IWM_ASSIGN;
-		__entry->id = le16_to_cpu(ticket->id);
-		__entry->action = le16_to_cpu(ticket->action);
-		__entry->flags = le16_to_cpu(ticket->flags);
-		__entry->reason = (__entry->flags & IWM_RX_TICKET_DROP_REASON_MSK) >> IWM_RX_TICKET_DROP_REASON_POS;
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Rx ticket: id 0x%x, action %s, %s 0x%x%s",
-		IWM_PR_ARG, __entry->id,
-		__print_symbolic(__entry->action, iwm_ticket_action_symbol),
-		__entry->reason ? "reason" : "flags",
-		__entry->reason ? __entry->reason : __entry->flags,
-		__entry->flags & IWM_RX_TICKET_AMSDU_MSK ? ", AMSDU frame" : ""
-	)
-);
-
-TRACE_EVENT(iwm_rx_packet,
-	TP_PROTO(struct iwm_priv *iwm, void *buf, int len),
-
-	TP_ARGS(iwm, buf, len),
-
-	TP_STRUCT__entry(
-		IWM_ENTRY
-		__field(u8, source)
-		__field(u16, id)
-		__field(u32, len)
-	),
-
-	TP_fast_assign(
-		struct iwm_umac_wifi_in_hdr *hdr = buf;
-
-		IWM_ASSIGN;
-		__entry->source = GET_VAL32(hdr->hw_hdr.cmd, UMAC_HDI_IN_CMD_SOURCE);
-		__entry->id = le16_to_cpu(hdr->sw_hdr.cmd.seq_num);
-		__entry->len = len - sizeof(*hdr);
-	),
-
-	TP_printk(
-		IWM_PR_FMT " Rx %s packet: id 0x%x, %d bytes",
-		IWM_PR_ARG, __entry->source == UMAC_HDI_IN_SOURCE_FHRX ?
-		"LMAC" : "UMAC", __entry->id, __entry->len
-	)
-);
-#endif
-
-#undef TRACE_INCLUDE_PATH
-#define TRACE_INCLUDE_PATH .
-#undef TRACE_INCLUDE_FILE
-#define TRACE_INCLUDE_FILE trace
-#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/iwmc3200wifi/tx.c b/drivers/net/wireless/iwmc3200wifi/tx.c
deleted file mode 100644
index be98074c060..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/tx.c
+++ /dev/null
@@ -1,529 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-/*
- * iwm Tx theory of operation:
- *
- * 1) We receive a 802.3 frame from the stack
- * 2) We convert it to a 802.11 frame [iwm_xmit_frame]
- * 3) We queue it to its corresponding tx queue [iwm_xmit_frame]
- * 4) We schedule the tx worker. There is one worker per tx
- *    queue. [iwm_xmit_frame]
- * 5) The tx worker is scheduled
- * 6) We go through every queued skb on the tx queue, and for each
- *    and every one of them: [iwm_tx_worker]
- *    a) We check if we have enough Tx credits (see below for a Tx
- *       credits description) for the frame length. [iwm_tx_worker]
- *    b) If we do, we aggregate the Tx frame into a UDMA one, by
- *       concatenating one REPLY_TX command per Tx frame. [iwm_tx_worker]
- *    c) When we run out of credits, or when we reach the maximum
- *       concatenation size, we actually send the concatenated UDMA
- *       frame. [iwm_tx_worker]
- *
- * When we run out of Tx credits, the skbs are filling the tx queue,
- * and eventually we will stop the netdev queue. [iwm_tx_worker]
- * The tx queue is emptied as we're getting new tx credits, by
- * scheduling the tx_worker. [iwm_tx_credit_inc]
- * The netdev queue is started again when we have enough tx credits,
- * and when our tx queue has some reasonable amout of space available
- * (i.e. half of the max size). [iwm_tx_worker]
- */
-
-#include <linux/slab.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <linux/ieee80211.h>
-
-#include "iwm.h"
-#include "debug.h"
-#include "commands.h"
-#include "hal.h"
-#include "umac.h"
-#include "bus.h"
-
-#define IWM_UMAC_PAGE_ALLOC_WRAP 0xffff
-
-#define BYTES_TO_PAGES(n)	 (1 + ((n) >> ilog2(IWM_UMAC_PAGE_SIZE)) - \
-				 (((n) & (IWM_UMAC_PAGE_SIZE - 1)) == 0))
-
-#define pool_id_to_queue(id)	 ((id < IWM_TX_CMD_QUEUE) ? id : id - 1)
-#define queue_to_pool_id(q)	 ((q < IWM_TX_CMD_QUEUE) ? q : q + 1)
-
-/* require to hold tx_credit lock */
-static int iwm_tx_credit_get(struct iwm_tx_credit *tx_credit, int id)
-{
-	struct pool_entry *pool = &tx_credit->pools[id];
-	struct spool_entry *spool = &tx_credit->spools[pool->sid];
-	int spool_pages;
-
-	/* number of pages can be taken from spool by this pool */
-	spool_pages = spool->max_pages - spool->alloc_pages +
-		      max(pool->min_pages - pool->alloc_pages, 0);
-
-	return min(pool->max_pages - pool->alloc_pages, spool_pages);
-}
-
-static bool iwm_tx_credit_ok(struct iwm_priv *iwm, int id, int nb)
-{
-	u32 npages = BYTES_TO_PAGES(nb);
-
-	if (npages <= iwm_tx_credit_get(&iwm->tx_credit, id))
-		return 1;
-
-	set_bit(id, &iwm->tx_credit.full_pools_map);
-
-	IWM_DBG_TX(iwm, DBG, "LINK: stop txq[%d], available credit: %d\n",
-		   pool_id_to_queue(id),
-		   iwm_tx_credit_get(&iwm->tx_credit, id));
-
-	return 0;
-}
-
-void iwm_tx_credit_inc(struct iwm_priv *iwm, int id, int total_freed_pages)
-{
-	struct pool_entry *pool;
-	struct spool_entry *spool;
-	int freed_pages;
-	int queue;
-
-	BUG_ON(id >= IWM_MACS_OUT_GROUPS);
-
-	pool = &iwm->tx_credit.pools[id];
-	spool = &iwm->tx_credit.spools[pool->sid];
-
-	freed_pages = total_freed_pages - pool->total_freed_pages;
-	IWM_DBG_TX(iwm, DBG, "Free %d pages for pool[%d]\n", freed_pages, id);
-
-	if (!freed_pages) {
-		IWM_DBG_TX(iwm, DBG, "No pages are freed by UMAC\n");
-		return;
-	} else if (freed_pages < 0)
-		freed_pages += IWM_UMAC_PAGE_ALLOC_WRAP + 1;
-
-	if (pool->alloc_pages > pool->min_pages) {
-		int spool_pages = pool->alloc_pages - pool->min_pages;
-		spool_pages = min(spool_pages, freed_pages);
-		spool->alloc_pages -= spool_pages;
-	}
-
-	pool->alloc_pages -= freed_pages;
-	pool->total_freed_pages = total_freed_pages;
-
-	IWM_DBG_TX(iwm, DBG, "Pool[%d] pages alloc: %d, total_freed: %d, "
-		   "Spool[%d] pages alloc: %d\n", id, pool->alloc_pages,
-		   pool->total_freed_pages, pool->sid, spool->alloc_pages);
-
-	if (test_bit(id, &iwm->tx_credit.full_pools_map) &&
-	    (pool->alloc_pages < pool->max_pages / 2)) {
-		clear_bit(id, &iwm->tx_credit.full_pools_map);
-
-		queue = pool_id_to_queue(id);
-
-		IWM_DBG_TX(iwm, DBG, "LINK: start txq[%d], available "
-			   "credit: %d\n", queue,
-			   iwm_tx_credit_get(&iwm->tx_credit, id));
-		queue_work(iwm->txq[queue].wq, &iwm->txq[queue].worker);
-	}
-}
-
-static void iwm_tx_credit_dec(struct iwm_priv *iwm, int id, int alloc_pages)
-{
-	struct pool_entry *pool;
-	struct spool_entry *spool;
-	int spool_pages;
-
-	IWM_DBG_TX(iwm, DBG, "Allocate %d pages for pool[%d]\n",
-		   alloc_pages, id);
-
-	BUG_ON(id >= IWM_MACS_OUT_GROUPS);
-
-	pool = &iwm->tx_credit.pools[id];
-	spool = &iwm->tx_credit.spools[pool->sid];
-
-	spool_pages = pool->alloc_pages + alloc_pages - pool->min_pages;
-
-	if (pool->alloc_pages >= pool->min_pages)
-		spool->alloc_pages += alloc_pages;
-	else if (spool_pages > 0)
-		spool->alloc_pages += spool_pages;
-
-	pool->alloc_pages += alloc_pages;
-
-	IWM_DBG_TX(iwm, DBG, "Pool[%d] pages alloc: %d, total_freed: %d, "
-		   "Spool[%d] pages alloc: %d\n", id, pool->alloc_pages,
-		   pool->total_freed_pages, pool->sid, spool->alloc_pages);
-}
-
-int iwm_tx_credit_alloc(struct iwm_priv *iwm, int id, int nb)
-{
-	u32 npages = BYTES_TO_PAGES(nb);
-	int ret = 0;
-
-	spin_lock(&iwm->tx_credit.lock);
-
-	if (!iwm_tx_credit_ok(iwm, id, nb)) {
-		IWM_DBG_TX(iwm, DBG, "No credit available for pool[%d]\n", id);
-		ret = -ENOSPC;
-		goto out;
-	}
-
-	iwm_tx_credit_dec(iwm, id, npages);
-
- out:
-	spin_unlock(&iwm->tx_credit.lock);
-	return ret;
-}
-
-/*
- * Since we're on an SDIO or USB bus, we are not sharing memory
- * for storing to be transmitted frames. The host needs to push
- * them upstream. As a consequence there needs to be a way for
- * the target to let us know if it can actually take more TX frames
- * or not. This is what Tx credits are for.
- *
- * For each Tx HW queue, we have a Tx pool, and then we have one
- * unique super pool (spool), which is actually a global pool of
- * all the UMAC pages.
- * For each Tx pool we have a min_pages, a max_pages fields, and a
- * alloc_pages fields. The alloc_pages tracks the number of pages
- * currently allocated from the tx pool.
- * Here are the rules to check if given a tx frame we have enough
- * tx credits for it:
- * 1) We translate the frame length into a number of UMAC pages.
- *    Let's call them n_pages.
- * 2) For the corresponding tx pool, we check if n_pages +
- *    pool->alloc_pages is higher than pool->min_pages. min_pages
- *    represent a set of pre-allocated pages on the tx pool. If
- *    that's the case, then we need to allocate those pages from
- *    the spool. We can do so until we reach spool->max_pages.
- * 3) Each tx pool is not allowed to allocate more than pool->max_pages
- *    from the spool, so once we're over min_pages, we can allocate
- *    pages from the spool, but not more than max_pages.
- *
- * When the tx code path needs to send a tx frame, it checks first
- * if it has enough tx credits, following those rules. [iwm_tx_credit_get]
- * If it does, it then updates the pool and spool counters and
- * then send the frame. [iwm_tx_credit_alloc and iwm_tx_credit_dec]
- * On the other side, when the UMAC is done transmitting frames, it
- * will send a credit update notification to the host. This is when
- * the pool and spool counters gets to be decreased. [iwm_tx_credit_inc,
- * called from rx.c:iwm_ntf_tx_credit_update]
- *
- */
-void iwm_tx_credit_init_pools(struct iwm_priv *iwm,
-			      struct iwm_umac_notif_alive *alive)
-{
-	int i, sid, pool_pages;
-
-	spin_lock(&iwm->tx_credit.lock);
-
-	iwm->tx_credit.pool_nr = le16_to_cpu(alive->page_grp_count);
-	iwm->tx_credit.full_pools_map = 0;
-	memset(&iwm->tx_credit.spools[0], 0, sizeof(struct spool_entry));
-
-	IWM_DBG_TX(iwm, DBG, "Pools number is %d\n", iwm->tx_credit.pool_nr);
-
-	for (i = 0; i < iwm->tx_credit.pool_nr; i++) {
-		__le32 page_grp_state = alive->page_grp_state[i];
-
-		iwm->tx_credit.pools[i].id = GET_VAL32(page_grp_state,
-				UMAC_ALIVE_PAGE_STS_GRP_NUM);
-		iwm->tx_credit.pools[i].sid = GET_VAL32(page_grp_state,
-				UMAC_ALIVE_PAGE_STS_SGRP_NUM);
-		iwm->tx_credit.pools[i].min_pages = GET_VAL32(page_grp_state,
-				UMAC_ALIVE_PAGE_STS_GRP_MIN_SIZE);
-		iwm->tx_credit.pools[i].max_pages = GET_VAL32(page_grp_state,
-				UMAC_ALIVE_PAGE_STS_GRP_MAX_SIZE);
-		iwm->tx_credit.pools[i].alloc_pages = 0;
-		iwm->tx_credit.pools[i].total_freed_pages = 0;
-
-		sid = iwm->tx_credit.pools[i].sid;
-		pool_pages = iwm->tx_credit.pools[i].min_pages;
-
-		if (iwm->tx_credit.spools[sid].max_pages == 0) {
-			iwm->tx_credit.spools[sid].id = sid;
-			iwm->tx_credit.spools[sid].max_pages =
-				GET_VAL32(page_grp_state,
-					  UMAC_ALIVE_PAGE_STS_SGRP_MAX_SIZE);
-			iwm->tx_credit.spools[sid].alloc_pages = 0;
-		}
-
-		iwm->tx_credit.spools[sid].alloc_pages += pool_pages;
-
-		IWM_DBG_TX(iwm, DBG, "Pool idx: %d, id: %d, sid: %d, capacity "
-			   "min: %d, max: %d, pool alloc: %d, total_free: %d, "
-			   "super poll alloc: %d\n",
-			   i, iwm->tx_credit.pools[i].id,
-			   iwm->tx_credit.pools[i].sid,
-			   iwm->tx_credit.pools[i].min_pages,
-			   iwm->tx_credit.pools[i].max_pages,
-			   iwm->tx_credit.pools[i].alloc_pages,
-			   iwm->tx_credit.pools[i].total_freed_pages,
-			   iwm->tx_credit.spools[sid].alloc_pages);
-	}
-
-	spin_unlock(&iwm->tx_credit.lock);
-}
-
-#define IWM_UDMA_HDR_LEN	sizeof(struct iwm_umac_wifi_out_hdr)
-
-static __le16 iwm_tx_build_packet(struct iwm_priv *iwm, struct sk_buff *skb,
-				  int pool_id, u8 *buf)
-{
-	struct iwm_umac_wifi_out_hdr *hdr = (struct iwm_umac_wifi_out_hdr *)buf;
-	struct iwm_udma_wifi_cmd udma_cmd;
-	struct iwm_umac_cmd umac_cmd;
-	struct iwm_tx_info *tx_info = skb_to_tx_info(skb);
-
-	udma_cmd.count = cpu_to_le16(skb->len +
-				     sizeof(struct iwm_umac_fw_cmd_hdr));
-	/* set EOP to 0 here. iwm_udma_wifi_hdr_set_eop() will be
-	 * called later to set EOP for the last packet. */
-	udma_cmd.eop = 0;
-	udma_cmd.credit_group = pool_id;
-	udma_cmd.ra_tid = tx_info->sta << 4 | tx_info->tid;
-	udma_cmd.lmac_offset = 0;
-
-	umac_cmd.id = REPLY_TX;
-	umac_cmd.count = cpu_to_le16(skb->len);
-	umac_cmd.color = tx_info->color;
-	umac_cmd.resp = 0;
-	umac_cmd.seq_num = cpu_to_le16(iwm_alloc_wifi_cmd_seq(iwm));
-
-	iwm_build_udma_wifi_hdr(iwm, &hdr->hw_hdr, &udma_cmd);
-	iwm_build_umac_hdr(iwm, &hdr->sw_hdr, &umac_cmd);
-
-	memcpy(buf + sizeof(*hdr), skb->data, skb->len);
-
-	return umac_cmd.seq_num;
-}
-
-static int iwm_tx_send_concat_packets(struct iwm_priv *iwm,
-				      struct iwm_tx_queue *txq)
-{
-	int ret;
-
-	if (!txq->concat_count)
-		return 0;
-
-	IWM_DBG_TX(iwm, DBG, "Send concatenated Tx: queue %d, %d bytes\n",
-		   txq->id, txq->concat_count);
-
-	/* mark EOP for the last packet */
-	iwm_udma_wifi_hdr_set_eop(iwm, txq->concat_ptr, 1);
-
-	trace_iwm_tx_packets(iwm, txq->concat_buf, txq->concat_count);
-	ret = iwm_bus_send_chunk(iwm, txq->concat_buf, txq->concat_count);
-
-	txq->concat_count = 0;
-	txq->concat_ptr = txq->concat_buf;
-
-	return ret;
-}
-
-void iwm_tx_worker(struct work_struct *work)
-{
-	struct iwm_priv *iwm;
-	struct iwm_tx_info *tx_info = NULL;
-	struct sk_buff *skb;
-	struct iwm_tx_queue *txq;
-	struct iwm_sta_info *sta_info;
-	struct iwm_tid_info *tid_info;
-	int cmdlen, ret, pool_id;
-
-	txq = container_of(work, struct iwm_tx_queue, worker);
-	iwm = container_of(txq, struct iwm_priv, txq[txq->id]);
-
-	pool_id = queue_to_pool_id(txq->id);
-
-	while (!test_bit(pool_id, &iwm->tx_credit.full_pools_map) &&
-	       !skb_queue_empty(&txq->queue)) {
-
-		spin_lock_bh(&txq->lock);
-		skb = skb_dequeue(&txq->queue);
-		spin_unlock_bh(&txq->lock);
-
-		tx_info = skb_to_tx_info(skb);
-		sta_info = &iwm->sta_table[tx_info->sta];
-		if (!sta_info->valid) {
-			IWM_ERR(iwm, "Trying to send a frame to unknown STA\n");
-			kfree_skb(skb);
-			continue;
-		}
-
-		tid_info = &sta_info->tid_info[tx_info->tid];
-
-		mutex_lock(&tid_info->mutex);
-
-		/*
-		 * If the RAxTID is stopped, we queue the skb to the stopped
-		 * queue.
-		 * Whenever we'll get a UMAC notification to resume the tx flow
-		 * for this RAxTID, we'll merge back the stopped queue into the
-		 * regular queue. See iwm_ntf_stop_resume_tx() from rx.c.
-		 */
-		if (tid_info->stopped) {
-			IWM_DBG_TX(iwm, DBG, "%dx%d stopped\n",
-				   tx_info->sta, tx_info->tid);
-			spin_lock_bh(&txq->lock);
-			skb_queue_tail(&txq->stopped_queue, skb);
-			spin_unlock_bh(&txq->lock);
-
-			mutex_unlock(&tid_info->mutex);
-			continue;
-		}
-
-		cmdlen = IWM_UDMA_HDR_LEN + skb->len;
-
-		IWM_DBG_TX(iwm, DBG, "Tx frame on queue %d: skb: 0x%p, sta: "
-			   "%d, color: %d\n", txq->id, skb, tx_info->sta,
-			   tx_info->color);
-
-		if (txq->concat_count + cmdlen > IWM_HAL_CONCATENATE_BUF_SIZE)
-			iwm_tx_send_concat_packets(iwm, txq);
-
-		ret = iwm_tx_credit_alloc(iwm, pool_id, cmdlen);
-		if (ret) {
-			IWM_DBG_TX(iwm, DBG, "not enough tx_credit for queue "
-				   "%d, Tx worker stopped\n", txq->id);
-			spin_lock_bh(&txq->lock);
-			skb_queue_head(&txq->queue, skb);
-			spin_unlock_bh(&txq->lock);
-
-			mutex_unlock(&tid_info->mutex);
-			break;
-		}
-
-		txq->concat_ptr = txq->concat_buf + txq->concat_count;
-		tid_info->last_seq_num =
-			iwm_tx_build_packet(iwm, skb, pool_id, txq->concat_ptr);
-		txq->concat_count += ALIGN(cmdlen, 16);
-
-		mutex_unlock(&tid_info->mutex);
-
-		kfree_skb(skb);
-	}
-
-	iwm_tx_send_concat_packets(iwm, txq);
-
-	if (__netif_subqueue_stopped(iwm_to_ndev(iwm), txq->id) &&
-	    !test_bit(pool_id, &iwm->tx_credit.full_pools_map) &&
-	    (skb_queue_len(&txq->queue) < IWM_TX_LIST_SIZE / 2)) {
-		IWM_DBG_TX(iwm, DBG, "LINK: start netif_subqueue[%d]", txq->id);
-		netif_wake_subqueue(iwm_to_ndev(iwm), txq->id);
-	}
-}
-
-int iwm_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
-{
-	struct iwm_priv *iwm = ndev_to_iwm(netdev);
-	struct wireless_dev *wdev = iwm_to_wdev(iwm);
-	struct iwm_tx_info *tx_info;
-	struct iwm_tx_queue *txq;
-	struct iwm_sta_info *sta_info;
-	u8 *dst_addr, sta_id;
-	u16 queue;
-	int ret;
-
-
-	if (!test_bit(IWM_STATUS_ASSOCIATED, &iwm->status)) {
-		IWM_DBG_TX(iwm, DBG, "LINK: stop netif_all_queues: "
-			   "not associated\n");
-		netif_tx_stop_all_queues(netdev);
-		goto drop;
-	}
-
-	queue = skb_get_queue_mapping(skb);
-	BUG_ON(queue >= IWM_TX_DATA_QUEUES); /* no iPAN yet */
-
-	txq = &iwm->txq[queue];
-
-	/* No free space for Tx, tx_worker is too slow */
-	if ((skb_queue_len(&txq->queue) > IWM_TX_LIST_SIZE) ||
-	    (skb_queue_len(&txq->stopped_queue) > IWM_TX_LIST_SIZE)) {
-		IWM_DBG_TX(iwm, DBG, "LINK: stop netif_subqueue[%d]\n", queue);
-		netif_stop_subqueue(netdev, queue);
-		return NETDEV_TX_BUSY;
-	}
-
-	ret = ieee80211_data_from_8023(skb, netdev->dev_addr, wdev->iftype,
-				       iwm->bssid, 0);
-	if (ret) {
-		IWM_ERR(iwm, "build wifi header failed\n");
-		goto drop;
-	}
-
-	dst_addr = ((struct ieee80211_hdr *)(skb->data))->addr1;
-
-	for (sta_id = 0; sta_id < IWM_STA_TABLE_NUM; sta_id++) {
-		sta_info = &iwm->sta_table[sta_id];
-		if (sta_info->valid &&
-		    !memcmp(dst_addr, sta_info->addr, ETH_ALEN))
-			break;
-	}
-
-	if (sta_id == IWM_STA_TABLE_NUM) {
-		IWM_ERR(iwm, "STA %pM not found in sta_table, Tx ignored\n",
-			dst_addr);
-		goto drop;
-	}
-
-	tx_info = skb_to_tx_info(skb);
-	tx_info->sta = sta_id;
-	tx_info->color = sta_info->color;
-	/* UMAC uses TID 8 (vs. 0) for non QoS packets */
-	if (sta_info->qos)
-		tx_info->tid = skb->priority;
-	else
-		tx_info->tid = IWM_UMAC_MGMT_TID;
-
-	spin_lock_bh(&iwm->txq[queue].lock);
-	skb_queue_tail(&iwm->txq[queue].queue, skb);
-	spin_unlock_bh(&iwm->txq[queue].lock);
-
-	queue_work(iwm->txq[queue].wq, &iwm->txq[queue].worker);
-
-	netdev->stats.tx_packets++;
-	netdev->stats.tx_bytes += skb->len;
-	return NETDEV_TX_OK;
-
- drop:
-	netdev->stats.tx_dropped++;
-	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
-}
diff --git a/drivers/net/wireless/iwmc3200wifi/umac.h b/drivers/net/wireless/iwmc3200wifi/umac.h
deleted file mode 100644
index 4a137d334a4..00000000000
--- a/drivers/net/wireless/iwmc3200wifi/umac.h
+++ /dev/null
@@ -1,789 +0,0 @@
-/*
- * Intel Wireless Multicomm 3200 WiFi driver
- *
- * Copyright (C) 2009 Intel Corporation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above copyright
- *     notice, this list of conditions and the following disclaimer in
- *     the documentation and/or other materials provided with the
- *     distribution.
- *   * Neither the name of Intel Corporation nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *
- * Intel Corporation <ilw@linux.intel.com>
- * Samuel Ortiz <samuel.ortiz@intel.com>
- * Zhu Yi <yi.zhu@intel.com>
- *
- */
-
-#ifndef __IWM_UMAC_H__
-#define __IWM_UMAC_H__
-
-struct iwm_udma_in_hdr {
-	__le32 cmd;
-	__le32 size;
-} __packed;
-
-struct iwm_udma_out_nonwifi_hdr {
-	__le32 cmd;
-	__le32 addr;
-	__le32 op1_sz;
-	__le32 op2;
-} __packed;
-
-struct iwm_udma_out_wifi_hdr {
-	__le32 cmd;
-	__le32 meta_data;
-} __packed;
-
-/* Sequence numbering */
-#define UMAC_WIFI_SEQ_NUM_BASE		1
-#define UMAC_WIFI_SEQ_NUM_MAX		0x4000
-#define UMAC_NONWIFI_SEQ_NUM_BASE	1
-#define UMAC_NONWIFI_SEQ_NUM_MAX	0x10
-
-/* MAC address address */
-#define WICO_MAC_ADDRESS_ADDR               0x604008F8
-
-/* RA / TID */
-#define UMAC_HDI_ACT_TBL_IDX_TID_POS                  0
-#define UMAC_HDI_ACT_TBL_IDX_TID_SEED                 0xF
-
-#define UMAC_HDI_ACT_TBL_IDX_RA_POS                   4
-#define UMAC_HDI_ACT_TBL_IDX_RA_SEED                  0xF
-
-#define UMAC_HDI_ACT_TBL_IDX_RA_UMAC                  0xF
-#define UMAC_HDI_ACT_TBL_IDX_TID_UMAC                 0x9
-#define UMAC_HDI_ACT_TBL_IDX_TID_LMAC                 0xA
-
-#define UMAC_HDI_ACT_TBL_IDX_HOST_CMD \
-	((UMAC_HDI_ACT_TBL_IDX_RA_UMAC << UMAC_HDI_ACT_TBL_IDX_RA_POS) |\
-	 (UMAC_HDI_ACT_TBL_IDX_TID_UMAC << UMAC_HDI_ACT_TBL_IDX_TID_POS))
-#define UMAC_HDI_ACT_TBL_IDX_UMAC_CMD \
-	((UMAC_HDI_ACT_TBL_IDX_RA_UMAC << UMAC_HDI_ACT_TBL_IDX_RA_POS) |\
-	(UMAC_HDI_ACT_TBL_IDX_TID_LMAC << UMAC_HDI_ACT_TBL_IDX_TID_POS))
-
-/* STA ID and color */
-#define STA_ID_SEED                        (0x0f)
-#define STA_ID_POS                         (0)
-#define STA_ID_MSK                         (STA_ID_SEED << STA_ID_POS)
-
-#define STA_COLOR_SEED                     (0x7)
-#define STA_COLOR_POS                      (4)
-#define STA_COLOR_MSK                      (STA_COLOR_SEED << STA_COLOR_POS)
-
-#define STA_ID_N_COLOR_COLOR(id_n_color) \
-	(((id_n_color) & STA_COLOR_MSK) >> STA_COLOR_POS)
-#define STA_ID_N_COLOR_ID(id_n_color) \
-	(((id_n_color) & STA_ID_MSK) >> STA_ID_POS)
-
-/* iwm_umac_notif_alive.page_grp_state Group number -- bits [3:0] */
-#define UMAC_ALIVE_PAGE_STS_GRP_NUM_POS		0
-#define UMAC_ALIVE_PAGE_STS_GRP_NUM_SEED	0xF
-
-/* iwm_umac_notif_alive.page_grp_state Super group number -- bits [7:4] */
-#define UMAC_ALIVE_PAGE_STS_SGRP_NUM_POS	4
-#define UMAC_ALIVE_PAGE_STS_SGRP_NUM_SEED	0xF
-
-/* iwm_umac_notif_alive.page_grp_state Group min size -- bits [15:8] */
-#define UMAC_ALIVE_PAGE_STS_GRP_MIN_SIZE_POS	8
-#define UMAC_ALIVE_PAGE_STS_GRP_MIN_SIZE_SEED	0xFF
-
-/* iwm_umac_notif_alive.page_grp_state Group max size -- bits [23:16] */
-#define UMAC_ALIVE_PAGE_STS_GRP_MAX_SIZE_POS	16
-#define UMAC_ALIVE_PAGE_STS_GRP_MAX_SIZE_SEED	0xFF
-
-/* iwm_umac_notif_alive.page_grp_state Super group max size -- bits [31:24] */
-#define UMAC_ALIVE_PAGE_STS_SGRP_MAX_SIZE_POS	24
-#define UMAC_ALIVE_PAGE_STS_SGRP_MAX_SIZE_SEED	0xFF
-
-/* Barkers */
-#define UMAC_REBOOT_BARKER		0xdeadbeef
-#define UMAC_ACK_BARKER			0xfeedbabe
-#define UMAC_PAD_TERMINAL		0xadadadad
-
-/* UMAC JMP address */
-#define UMAC_MU_FW_INST_DATA_12_ADDR    0xBF0000
-
-/* iwm_umac_hdi_out_hdr.cmd OP code -- bits [3:0] */
-#define UMAC_HDI_OUT_CMD_OPCODE_POS	0
-#define UMAC_HDI_OUT_CMD_OPCODE_SEED	0xF
-
-/* iwm_umac_hdi_out_hdr.cmd End-Of-Transfer -- bits [10:10] */
-#define UMAC_HDI_OUT_CMD_EOT_POS	10
-#define UMAC_HDI_OUT_CMD_EOT_SEED	0x1
-
-/* iwm_umac_hdi_out_hdr.cmd UTFD only usage -- bits [11:11] */
-#define UMAC_HDI_OUT_CMD_UTFD_ONLY_POS	11
-#define UMAC_HDI_OUT_CMD_UTFD_ONLY_SEED	0x1
-
-/* iwm_umac_hdi_out_hdr.cmd Non-WiFi HW sequence number -- bits [12:15] */
-#define UDMA_HDI_OUT_CMD_NON_WIFI_HW_SEQ_NUM_POS   12
-#define UDMA_HDI_OUT_CMD_NON_WIFI_HW_SEQ_NUM_SEED  0xF
-
-/* iwm_umac_hdi_out_hdr.cmd Signature -- bits [31:16] */
-#define UMAC_HDI_OUT_CMD_SIGNATURE_POS	16
-#define UMAC_HDI_OUT_CMD_SIGNATURE_SEED	0xFFFF
-
-/* iwm_umac_hdi_out_hdr.meta_data Byte count -- bits [11:0] */
-#define UMAC_HDI_OUT_BYTE_COUNT_POS	0
-#define UMAC_HDI_OUT_BYTE_COUNT_SEED	0xFFF
-
-/* iwm_umac_hdi_out_hdr.meta_data Credit group -- bits [15:12] */
-#define UMAC_HDI_OUT_CREDIT_GRP_POS	12
-#define UMAC_HDI_OUT_CREDIT_GRP_SEED	0xF
-
-/* iwm_umac_hdi_out_hdr.meta_data RA/TID -- bits [23:16] */
-#define UMAC_HDI_OUT_RATID_POS		16
-#define UMAC_HDI_OUT_RATID_SEED		0xFF
-
-/* iwm_umac_hdi_out_hdr.meta_data LMAC offset -- bits [31:24] */
-#define UMAC_HDI_OUT_LMAC_OFFSET_POS	24
-#define UMAC_HDI_OUT_LMAC_OFFSET_SEED	0xFF
-
-/* Signature */
-#define UMAC_HDI_OUT_SIGNATURE		0xCBBC
-
-/* buffer alignment */
-#define UMAC_HDI_BUF_ALIGN_MSK		0xF
-
-/*  iwm_umac_hdi_in_hdr.cmd OP code -- bits [3:0] */
-#define UMAC_HDI_IN_CMD_OPCODE_POS                0
-#define UMAC_HDI_IN_CMD_OPCODE_SEED               0xF
-
-/*  iwm_umac_hdi_in_hdr.cmd Non-WiFi API response -- bits [6:4] */
-#define UMAC_HDI_IN_CMD_NON_WIFI_RESP_POS         4
-#define UMAC_HDI_IN_CMD_NON_WIFI_RESP_SEED        0x7
-
-/* iwm_umac_hdi_in_hdr.cmd WiFi API source -- bits [5:4] */
-#define UMAC_HDI_IN_CMD_SOURCE_POS                4
-#define UMAC_HDI_IN_CMD_SOURCE_SEED               0x3
-
-/* iwm_umac_hdi_in_hdr.cmd WiFi API EOT -- bits [6:6] */
-#define UMAC_HDI_IN_CMD_EOT_POS                   6
-#define UMAC_HDI_IN_CMD_EOT_SEED                  0x1
-
-/* iwm_umac_hdi_in_hdr.cmd timestamp present -- bits [7:7] */
-#define UMAC_HDI_IN_CMD_TIME_STAMP_PRESENT_POS    7
-#define UMAC_HDI_IN_CMD_TIME_STAMP_PRESENT_SEED   0x1
-
-/* iwm_umac_hdi_in_hdr.cmd WiFi Non-last AMSDU -- bits [8:8] */
-#define UMAC_HDI_IN_CMD_NON_LAST_AMSDU_POS        8
-#define UMAC_HDI_IN_CMD_NON_LAST_AMSDU_SEED       0x1
-
-/* iwm_umac_hdi_in_hdr.cmd WiFi HW sequence number -- bits [31:9] */
-#define UMAC_HDI_IN_CMD_HW_SEQ_NUM_POS            9
-#define UMAC_HDI_IN_CMD_HW_SEQ_NUM_SEED           0x7FFFFF
-
-/* iwm_umac_hdi_in_hdr.cmd Non-WiFi HW sequence number -- bits [12:15] */
-#define UDMA_HDI_IN_CMD_NON_WIFI_HW_SEQ_NUM_POS   12
-#define UDMA_HDI_IN_CMD_NON_WIFI_HW_SEQ_NUM_SEED  0xF
-
-/* iwm_umac_hdi_in_hdr.cmd Non-WiFi HW signature -- bits [16:31] */
-#define UDMA_HDI_IN_CMD_NON_WIFI_HW_SIG_POS       16
-#define UDMA_HDI_IN_CMD_NON_WIFI_HW_SIG_SEED      0xFFFF
-
-/* Fixed Non-WiFi signature */
-#define UDMA_HDI_IN_CMD_NON_WIFI_HW_SIG           0xCBBC
-
-/* IN NTFY op-codes */
-#define UMAC_NOTIFY_OPCODE_ALIVE		0xA1
-#define UMAC_NOTIFY_OPCODE_INIT_COMPLETE	0xA2
-#define UMAC_NOTIFY_OPCODE_WIFI_CORE_STATUS	0xA3
-#define UMAC_NOTIFY_OPCODE_ERROR		0xA4
-#define UMAC_NOTIFY_OPCODE_DEBUG		0xA5
-#define UMAC_NOTIFY_OPCODE_WIFI_IF_WRAPPER	0xB0
-#define UMAC_NOTIFY_OPCODE_STATS		0xB1
-#define UMAC_NOTIFY_OPCODE_PAGE_DEALLOC		0xB3
-#define UMAC_NOTIFY_OPCODE_RX_TICKET		0xB4
-#define UMAC_NOTIFY_OPCODE_MAX		        (UMAC_NOTIFY_OPCODE_RX_TICKET -\
-						UMAC_NOTIFY_OPCODE_ALIVE + 1)
-#define UMAC_NOTIFY_OPCODE_FIRST		(UMAC_NOTIFY_OPCODE_ALIVE)
-
-/* HDI OUT OP CODE */
-#define UMAC_HDI_OUT_OPCODE_PING		0x0
-#define UMAC_HDI_OUT_OPCODE_READ		0x1
-#define UMAC_HDI_OUT_OPCODE_WRITE		0x2
-#define UMAC_HDI_OUT_OPCODE_JUMP		0x3
-#define UMAC_HDI_OUT_OPCODE_REBOOT		0x4
-#define UMAC_HDI_OUT_OPCODE_WRITE_PERSISTENT	0x5
-#define UMAC_HDI_OUT_OPCODE_READ_PERSISTENT	0x6
-#define UMAC_HDI_OUT_OPCODE_READ_MODIFY_WRITE	0x7
-/* #define UMAC_HDI_OUT_OPCODE_RESERVED		0x8..0xA */
-#define UMAC_HDI_OUT_OPCODE_WRITE_AUX_REG	0xB
-#define UMAC_HDI_OUT_OPCODE_WIFI		0xF
-
-/* HDI IN OP CODE -- Non WiFi*/
-#define UMAC_HDI_IN_OPCODE_PING			0x0
-#define UMAC_HDI_IN_OPCODE_READ			0x1
-#define UMAC_HDI_IN_OPCODE_WRITE		0x2
-#define UMAC_HDI_IN_OPCODE_WRITE_PERSISTENT	0x5
-#define UMAC_HDI_IN_OPCODE_READ_PERSISTENT	0x6
-#define UMAC_HDI_IN_OPCODE_READ_MODIFY_WRITE	0x7
-#define UMAC_HDI_IN_OPCODE_EP_MGMT		0x8
-#define UMAC_HDI_IN_OPCODE_CREDIT_CHANGE	0x9
-#define UMAC_HDI_IN_OPCODE_CTRL_DATABASE	0xA
-#define UMAC_HDI_IN_OPCODE_WRITE_AUX_REG	0xB
-#define UMAC_HDI_IN_OPCODE_NONWIFI_MAX \
-		(UMAC_HDI_IN_OPCODE_WRITE_AUX_REG + 1)
-#define UMAC_HDI_IN_OPCODE_WIFI			0xF
-
-/* HDI IN SOURCE */
-#define UMAC_HDI_IN_SOURCE_FHRX			0x0
-#define UMAC_HDI_IN_SOURCE_UDMA			0x1
-#define UMAC_HDI_IN_SOURCE_FW			0x2
-#define UMAC_HDI_IN_SOURCE_RESERVED		0x3
-
-/* OUT CMD op-codes */
-#define UMAC_CMD_OPCODE_ECHO                    0x01
-#define UMAC_CMD_OPCODE_HALT                    0x02
-#define UMAC_CMD_OPCODE_RESET                   0x03
-#define UMAC_CMD_OPCODE_BULK_EP_INACT_TIMEOUT   0x09
-#define UMAC_CMD_OPCODE_URB_CANCEL_ACK          0x0A
-#define UMAC_CMD_OPCODE_DCACHE_FLUSH            0x0B
-#define UMAC_CMD_OPCODE_EEPROM_PROXY            0x0C
-#define UMAC_CMD_OPCODE_TX_ECHO                 0x0D
-#define UMAC_CMD_OPCODE_DBG_MON                 0x0E
-#define UMAC_CMD_OPCODE_INTERNAL_TX             0x0F
-#define UMAC_CMD_OPCODE_SET_PARAM_FIX           0x10
-#define UMAC_CMD_OPCODE_SET_PARAM_VAR           0x11
-#define UMAC_CMD_OPCODE_GET_PARAM               0x12
-#define UMAC_CMD_OPCODE_DBG_EVENT_WRAPPER       0x13
-#define UMAC_CMD_OPCODE_TARGET                  0x14
-#define UMAC_CMD_OPCODE_STATISTIC_REQUEST       0x15
-#define UMAC_CMD_OPCODE_GET_CHAN_INFO_LIST	0x16
-#define UMAC_CMD_OPCODE_SET_PARAM_LIST		0x17
-#define UMAC_CMD_OPCODE_GET_PARAM_LIST		0x18
-#define UMAC_CMD_OPCODE_STOP_RESUME_STA_TX      0x19
-#define UMAC_CMD_OPCODE_TEST_BLOCK_ACK          0x1A
-
-#define UMAC_CMD_OPCODE_BASE_WRAPPER            0xFA
-#define UMAC_CMD_OPCODE_LMAC_WRAPPER            0xFB
-#define UMAC_CMD_OPCODE_HW_TEST_WRAPPER         0xFC
-#define UMAC_CMD_OPCODE_WIFI_IF_WRAPPER         0xFD
-#define UMAC_CMD_OPCODE_WIFI_WRAPPER            0xFE
-#define UMAC_CMD_OPCODE_WIFI_PASS_THROUGH       0xFF
-
-/* UMAC WiFi interface op-codes */
-#define UMAC_WIFI_IF_CMD_SET_PROFILE                     0x11
-#define UMAC_WIFI_IF_CMD_INVALIDATE_PROFILE              0x12
-#define UMAC_WIFI_IF_CMD_SET_EXCLUDE_LIST                0x13
-#define UMAC_WIFI_IF_CMD_SCAN_REQUEST                    0x14
-#define UMAC_WIFI_IF_CMD_SCAN_CONFIG                     0x15
-#define UMAC_WIFI_IF_CMD_ADD_WEP40_KEY                   0x16
-#define UMAC_WIFI_IF_CMD_ADD_WEP104_KEY                  0x17
-#define UMAC_WIFI_IF_CMD_ADD_TKIP_KEY                    0x18
-#define UMAC_WIFI_IF_CMD_ADD_CCMP_KEY                    0x19
-#define UMAC_WIFI_IF_CMD_REMOVE_KEY                      0x1A
-#define UMAC_WIFI_IF_CMD_GLOBAL_TX_KEY_ID                0x1B
-#define UMAC_WIFI_IF_CMD_SET_HOST_EXTENDED_IE            0x1C
-#define UMAC_WIFI_IF_CMD_GET_SUPPORTED_CHANNELS          0x1E
-#define UMAC_WIFI_IF_CMD_PMKID_UPDATE                    0x1F
-#define UMAC_WIFI_IF_CMD_TX_PWR_TRIGGER                  0x20
-
-/* UMAC WiFi interface ports */
-#define UMAC_WIFI_IF_FLG_PORT_DEF                        0x00
-#define UMAC_WIFI_IF_FLG_PORT_PAN                        0x01
-#define UMAC_WIFI_IF_FLG_PORT_PAN_INVALID                WIFI_IF_FLG_PORT_DEF
-
-/* UMAC WiFi interface actions */
-#define UMAC_WIFI_IF_FLG_ACT_GET                         0x10
-#define UMAC_WIFI_IF_FLG_ACT_SET                         0x20
-
-/* iwm_umac_fw_cmd_hdr.meta_data byte count -- bits [11:0] */
-#define UMAC_FW_CMD_BYTE_COUNT_POS            0
-#define UMAC_FW_CMD_BYTE_COUNT_SEED           0xFFF
-
-/* iwm_umac_fw_cmd_hdr.meta_data status -- bits [15:12] */
-#define UMAC_FW_CMD_STATUS_POS                12
-#define UMAC_FW_CMD_STATUS_SEED               0xF
-
-/* iwm_umac_fw_cmd_hdr.meta_data full TX command by Driver -- bits [16:16] */
-#define UMAC_FW_CMD_TX_DRV_FULL_CMD_POS       16
-#define UMAC_FW_CMD_TX_DRV_FULL_CMD_SEED      0x1
-
-/* iwm_umac_fw_cmd_hdr.meta_data TX command by FW -- bits [17:17] */
-#define UMAC_FW_CMD_TX_FW_CMD_POS             17
-#define UMAC_FW_CMD_TX_FW_CMD_SEED            0x1
-
-/* iwm_umac_fw_cmd_hdr.meta_data TX plaintext mode -- bits [18:18] */
-#define UMAC_FW_CMD_TX_PLAINTEXT_POS          18
-#define UMAC_FW_CMD_TX_PLAINTEXT_SEED         0x1
-
-/* iwm_umac_fw_cmd_hdr.meta_data STA color -- bits [22:20] */
-#define UMAC_FW_CMD_TX_STA_COLOR_POS          20
-#define UMAC_FW_CMD_TX_STA_COLOR_SEED         0x7
-
-/* iwm_umac_fw_cmd_hdr.meta_data TX life time (TU) -- bits [31:24] */
-#define UMAC_FW_CMD_TX_LIFETIME_TU_POS        24
-#define UMAC_FW_CMD_TX_LIFETIME_TU_SEED       0xFF
-
-/* iwm_dev_cmd_hdr.flags Response required -- bits [5:5] */
-#define UMAC_DEV_CMD_FLAGS_RESP_REQ_POS		5
-#define UMAC_DEV_CMD_FLAGS_RESP_REQ_SEED	0x1
-
-/* iwm_dev_cmd_hdr.flags Aborted command -- bits [6:6] */
-#define UMAC_DEV_CMD_FLAGS_ABORT_POS		6
-#define UMAC_DEV_CMD_FLAGS_ABORT_SEED		0x1
-
-/* iwm_dev_cmd_hdr.flags Internal command -- bits [7:7] */
-#define DEV_CMD_FLAGS_FLD_INTERNAL_POS		7
-#define DEV_CMD_FLAGS_FLD_INTERNAL_SEED		0x1
-
-/* Rx */
-/* Rx actions */
-#define IWM_RX_TICKET_DROP           0x0
-#define IWM_RX_TICKET_RELEASE        0x1
-#define IWM_RX_TICKET_SNIFFER        0x2
-#define IWM_RX_TICKET_ENQUEUE        0x3
-
-/* Rx flags */
-#define IWM_RX_TICKET_PAD_SIZE_MSK        0x2
-#define IWM_RX_TICKET_SPECIAL_SNAP_MSK    0x4
-#define IWM_RX_TICKET_AMSDU_MSK           0x8
-#define IWM_RX_TICKET_DROP_REASON_POS       4
-#define IWM_RX_TICKET_DROP_REASON_MSK (0x1F << IWM_RX_TICKET_DROP_REASON_POS)
-
-#define IWM_RX_DROP_NO_DROP                          0x0
-#define IWM_RX_DROP_BAD_CRC                          0x1
-/* L2P no address match */
-#define IWM_RX_DROP_LMAC_ADDR_FILTER                 0x2
-/* Multicast address not in list */
-#define IWM_RX_DROP_MCAST_ADDR_FILTER                0x3
-/* Control frames are not sent to the driver */
-#define IWM_RX_DROP_CTL_FRAME                        0x4
-/* Our frame is back */
-#define IWM_RX_DROP_OUR_TX                           0x5
-/* Association class filtering */
-#define IWM_RX_DROP_CLASS_FILTER                     0x6
-/* Duplicated frame */
-#define IWM_RX_DROP_DUPLICATE_FILTER                 0x7
-/* Decryption error */
-#define IWM_RX_DROP_SEC_ERR                          0x8
-/* Unencrypted frame while encryption is on */
-#define IWM_RX_DROP_SEC_NO_ENCRYPTION                0x9
-/* Replay check failure */
-#define IWM_RX_DROP_SEC_REPLAY_ERR                   0xa
-/* uCode and FW key color mismatch, check before replay */
-#define IWM_RX_DROP_SEC_KEY_COLOR_MISMATCH           0xb
-#define IWM_RX_DROP_SEC_TKIP_COUNTER_MEASURE         0xc
-/* No fragmentations Db is found */
-#define IWM_RX_DROP_FRAG_NO_RESOURCE                 0xd
-/* Fragmention Db has seqCtl mismatch Vs. non-1st frag */
-#define IWM_RX_DROP_FRAG_ERR                         0xe
-#define IWM_RX_DROP_FRAG_LOST                        0xf
-#define IWM_RX_DROP_FRAG_COMPLETE                    0x10
-/* Should be handled by UMAC */
-#define IWM_RX_DROP_MANAGEMENT                       0x11
-/* STA not found by UMAC */
-#define IWM_RX_DROP_NO_STATION                       0x12
-/* NULL or QoS NULL */
-#define IWM_RX_DROP_NULL_DATA                        0x13
-#define IWM_RX_DROP_BA_REORDER_OLD_SEQCTL            0x14
-#define IWM_RX_DROP_BA_REORDER_DUPLICATE             0x15
-
-struct iwm_rx_ticket {
-	__le16 action;
-	__le16 id;
-	__le16 flags;
-	u8 payload_offset; /* includes: MAC header, pad, IV */
-	u8 tail_len; /* includes: MIC, ICV, CRC (w/o STATUS) */
-} __packed;
-
-struct iwm_rx_mpdu_hdr {
-	__le16 len;
-	__le16 reserved;
-} __packed;
-
-/* UMAC SW WIFI API */
-
-struct iwm_dev_cmd_hdr {
-	u8 cmd;
-	u8 flags;
-	__le16 seq_num;
-} __packed;
-
-struct iwm_umac_fw_cmd_hdr {
-	__le32 meta_data;
-	struct iwm_dev_cmd_hdr cmd;
-} __packed;
-
-struct iwm_umac_wifi_out_hdr {
-	struct iwm_udma_out_wifi_hdr hw_hdr;
-	struct iwm_umac_fw_cmd_hdr sw_hdr;
-} __packed;
-
-struct iwm_umac_nonwifi_out_hdr {
-	struct iwm_udma_out_nonwifi_hdr hw_hdr;
-} __packed;
-
-struct iwm_umac_wifi_in_hdr {
-	struct iwm_udma_in_hdr hw_hdr;
-	struct iwm_umac_fw_cmd_hdr sw_hdr;
-} __packed;
-
-struct iwm_umac_nonwifi_in_hdr {
-	struct iwm_udma_in_hdr hw_hdr;
-	__le32 time_stamp;
-} __packed;
-
-#define IWM_UMAC_PAGE_SIZE	0x200
-
-/* Notify structures */
-struct iwm_fw_version {
-	u8 minor;
-	u8 major;
-	__le16 id;
-};
-
-struct iwm_fw_build {
-	u8 type;
-	u8 subtype;
-	u8 platform;
-	u8 opt;
-};
-
-struct iwm_fw_alive_hdr {
-	struct iwm_fw_version ver;
-	struct iwm_fw_build build;
-	__le32 os_build;
-	__le32 log_hdr_addr;
-	__le32 log_buf_addr;
-	__le32 sys_timer_addr;
-};
-
-#define WAIT_NOTIF_TIMEOUT     (2 * HZ)
-#define SCAN_COMPLETE_TIMEOUT  (3 * HZ)
-
-#define UMAC_NTFY_ALIVE_STATUS_ERR		0xDEAD
-#define UMAC_NTFY_ALIVE_STATUS_OK		0xCAFE
-
-#define UMAC_NTFY_INIT_COMPLETE_STATUS_ERR	0xDEAD
-#define UMAC_NTFY_INIT_COMPLETE_STATUS_OK	0xCAFE
-
-#define UMAC_NTFY_WIFI_CORE_STATUS_LINK_EN      0x40
-#define UMAC_NTFY_WIFI_CORE_STATUS_MLME_EN      0x80
-
-#define IWM_MACS_OUT_GROUPS	6
-#define IWM_MACS_OUT_SGROUPS	1
-
-
-#define WIFI_IF_NTFY_ASSOC_START			0x80
-#define WIFI_IF_NTFY_ASSOC_COMPLETE			0x81
-#define WIFI_IF_NTFY_PROFILE_INVALIDATE_COMPLETE	0x82
-#define WIFI_IF_NTFY_CONNECTION_TERMINATED		0x83
-#define WIFI_IF_NTFY_SCAN_COMPLETE			0x84
-#define WIFI_IF_NTFY_STA_TABLE_CHANGE			0x85
-#define WIFI_IF_NTFY_EXTENDED_IE_REQUIRED		0x86
-#define WIFI_IF_NTFY_RADIO_PREEMPTION			0x87
-#define WIFI_IF_NTFY_BSS_TRK_TABLE_CHANGED		0x88
-#define WIFI_IF_NTFY_BSS_TRK_ENTRIES_REMOVED		0x89
-#define WIFI_IF_NTFY_LINK_QUALITY_STATISTICS		0x8A
-#define WIFI_IF_NTFY_MGMT_FRAME				0x8B
-
-/* DEBUG INDICATIONS */
-#define WIFI_DBG_IF_NTFY_SCAN_SUPER_JOB_START		0xE0
-#define WIFI_DBG_IF_NTFY_SCAN_SUPER_JOB_COMPLETE	0xE1
-#define WIFI_DBG_IF_NTFY_SCAN_CHANNEL_START		0xE2
-#define WIFI_DBG_IF_NTFY_SCAN_CHANNEL_RESULT		0xE3
-#define WIFI_DBG_IF_NTFY_SCAN_MINI_JOB_START		0xE4
-#define WIFI_DBG_IF_NTFY_SCAN_MINI_JOB_COMPLETE		0xE5
-#define WIFI_DBG_IF_NTFY_CNCT_ATC_START			0xE6
-#define WIFI_DBG_IF_NTFY_COEX_NOTIFICATION		0xE7
-#define WIFI_DBG_IF_NTFY_COEX_HANDLE_ENVELOP		0xE8
-#define WIFI_DBG_IF_NTFY_COEX_HANDLE_RELEASE_ENVELOP	0xE9
-
-#define WIFI_IF_NTFY_MAX 0xff
-
-/* Notification structures */
-struct iwm_umac_notif_wifi_if {
-	struct iwm_umac_wifi_in_hdr hdr;
-	u8 status;
-	u8 flags;
-	__le16 buf_size;
-} __packed;
-
-#define UMAC_ROAM_REASON_FIRST_SELECTION	0x1
-#define UMAC_ROAM_REASON_AP_DEAUTH		0x2
-#define UMAC_ROAM_REASON_AP_CONNECT_LOST	0x3
-#define UMAC_ROAM_REASON_RSSI			0x4
-#define UMAC_ROAM_REASON_AP_ASSISTED_ROAM	0x5
-#define UMAC_ROAM_REASON_IBSS_COALESCING	0x6
-
-struct iwm_umac_notif_assoc_start {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 roam_reason;
-	u8 bssid[ETH_ALEN];
-	u8 reserved[2];
-} __packed;
-
-#define UMAC_ASSOC_COMPLETE_SUCCESS		0x0
-#define UMAC_ASSOC_COMPLETE_FAILURE		0x1
-
-struct iwm_umac_notif_assoc_complete {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 status;
-	u8 bssid[ETH_ALEN];
-	u8 band;
-	u8 channel;
-} __packed;
-
-#define UMAC_PROFILE_INVALID_ASSOC_TIMEOUT	0x0
-#define UMAC_PROFILE_INVALID_ROAM_TIMEOUT	0x1
-#define UMAC_PROFILE_INVALID_REQUEST		0x2
-#define UMAC_PROFILE_INVALID_RF_PREEMPTED	0x3
-
-struct iwm_umac_notif_profile_invalidate {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 reason;
-} __packed;
-
-#define UMAC_SCAN_RESULT_SUCCESS  0x0
-#define UMAC_SCAN_RESULT_ABORTED  0x1
-#define UMAC_SCAN_RESULT_REJECTED 0x2
-#define UMAC_SCAN_RESULT_FAILED   0x3
-
-struct iwm_umac_notif_scan_complete {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 type;
-	__le32 result;
-	u8 seq_num;
-} __packed;
-
-#define UMAC_OPCODE_ADD_MODIFY	0x0
-#define UMAC_OPCODE_REMOVE	0x1
-#define UMAC_OPCODE_CLEAR_ALL	0x2
-
-#define UMAC_STA_FLAG_QOS	0x1
-
-struct iwm_umac_notif_sta_info {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 opcode;
-	u8 mac_addr[ETH_ALEN];
-	u8 sta_id; /* bits 0-3: station ID, bits 4-7: station color */
-	u8 flags;
-} __packed;
-
-#define UMAC_BAND_2GHZ 0
-#define UMAC_BAND_5GHZ 1
-
-#define UMAC_CHANNEL_WIDTH_20MHZ 0
-#define UMAC_CHANNEL_WIDTH_40MHZ 1
-
-struct iwm_umac_notif_bss_info {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 type;
-	__le32 timestamp;
-	__le16 table_idx;
-	__le16 frame_len;
-	u8 band;
-	u8 channel;
-	s8 rssi;
-	u8 reserved;
-	u8 frame_buf[1];
-} __packed;
-
-#define IWM_BSS_REMOVE_INDEX_MSK           0x0fff
-#define IWM_BSS_REMOVE_FLAGS_MSK           0xfc00
-
-#define IWM_BSS_REMOVE_FLG_AGE             0x1000
-#define IWM_BSS_REMOVE_FLG_TIMEOUT         0x2000
-#define IWM_BSS_REMOVE_FLG_TABLE_FULL      0x4000
-
-struct iwm_umac_notif_bss_removed {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le32 count;
-	__le16 entries[0];
-} __packed;
-
-struct iwm_umac_notif_mgt_frame {
-	struct iwm_umac_notif_wifi_if mlme_hdr;
-	__le16 len;
-	u8 frame[1];
-} __packed;
-
-struct iwm_umac_notif_alive {
-	struct iwm_umac_wifi_in_hdr hdr;
-	__le16 status;
-	__le16 reserved1;
-	struct iwm_fw_alive_hdr alive_data;
-	__le16 reserved2;
-	__le16 page_grp_count;
-	__le32 page_grp_state[IWM_MACS_OUT_GROUPS];
-} __packed;
-
-struct iwm_umac_notif_init_complete {
-	struct iwm_umac_wifi_in_hdr hdr;
-	__le16 status;
-	__le16 reserved;
-} __packed;
-
-/* error categories */
-enum {
-	UMAC_SYS_ERR_CAT_NONE = 0,
-	UMAC_SYS_ERR_CAT_BOOT,
-	UMAC_SYS_ERR_CAT_UMAC,
-	UMAC_SYS_ERR_CAT_UAXM,
-	UMAC_SYS_ERR_CAT_LMAC,
-	UMAC_SYS_ERR_CAT_MAX
-};
-
-struct iwm_fw_error_hdr {
-	__le32 category;
-	__le32 status;
-	__le32 pc;
-	__le32 blink1;
-	__le32 blink2;
-	__le32 ilink1;
-	__le32 ilink2;
-	__le32 data1;
-	__le32 data2;
-	__le32 line_num;
-	__le32 umac_status;
-	__le32 lmac_status;
-	__le32 sdio_status;
-	__le32 dbm_sample_ctrl;
-	__le32 dbm_buf_base;
-	__le32 dbm_buf_end;
-	__le32 dbm_buf_write_ptr;
-	__le32 dbm_buf_cycle_cnt;
-} __packed;
-
-struct iwm_umac_notif_error {
-	struct iwm_umac_wifi_in_hdr hdr;
-	struct iwm_fw_error_hdr err;
-} __packed;
-
-#define UMAC_DEALLOC_NTFY_CHANGES_CNT_POS	0
-#define UMAC_DEALLOC_NTFY_CHANGES_CNT_SEED	0xff
-#define UMAC_DEALLOC_NTFY_CHANGES_MSK_POS	8
-#define UMAC_DEALLOC_NTFY_CHANGES_MSK_SEED	0xffffff
-#define UMAC_DEALLOC_NTFY_PAGE_CNT_POS		0
-#define UMAC_DEALLOC_NTFY_PAGE_CNT_SEED		0xffffff
-#define UMAC_DEALLOC_NTFY_GROUP_NUM_POS		24
-#define UMAC_DEALLOC_NTFY_GROUP_NUM_SEED	0xf
-
-struct iwm_umac_notif_page_dealloc {
-	struct iwm_umac_wifi_in_hdr hdr;
-	__le32 changes;
-	__le32 grp_info[IWM_MACS_OUT_GROUPS];
-} __packed;
-
-struct iwm_umac_notif_wifi_status {
-	struct iwm_umac_wifi_in_hdr hdr;
-	__le16 status;
-	__le16 reserved;
-} __packed;
-
-struct iwm_umac_notif_rx_ticket {
-	struct iwm_umac_wifi_in_hdr hdr;
-	u8 num_tickets;
-	u8 reserved[3];
-	struct iwm_rx_ticket tickets[1];
-} __packed;
-
-/* Tx/Rx rates window (number of max of last update window per second) */
-#define UMAC_NTF_RATE_SAMPLE_NR	4
-
-/* Max numbers of bits required to go through all antennae in bitmasks */
-#define UMAC_PHY_NUM_CHAINS     3
-
-#define IWM_UMAC_MGMT_TID	8
-#define IWM_UMAC_TID_NR		9 /* 8 TIDs + MGMT */
-
-struct iwm_umac_notif_stats {
-	struct iwm_umac_wifi_in_hdr hdr;
-	__le32 flags;
-	__le32 timestamp;
-	__le16 tid_load[IWM_UMAC_TID_NR + 1]; /* 1 non-QoS + 1 dword align */
-	__le16 tx_rate[UMAC_NTF_RATE_SAMPLE_NR];
-	__le16 rx_rate[UMAC_NTF_RATE_SAMPLE_NR];
-	__le32 chain_energy[UMAC_PHY_NUM_CHAINS];
-	s32 rssi_dbm;
-	s32 noise_dbm;
-	__le32 supp_rates;
-	__le32 supp_ht_rates;
-	__le32 missed_beacons;
-	__le32 rx_beacons;
-	__le32 rx_dir_pkts;
-	__le32 rx_nondir_pkts;
-	__le32 rx_multicast;
-	__le32 rx_errors;
-	__le32 rx_drop_other_bssid;
-	__le32 rx_drop_decode;
-	__le32 rx_drop_reassembly;
-	__le32 rx_drop_bad_len;
-	__le32 rx_drop_overflow;
-	__le32 rx_drop_crc;
-	__le32 rx_drop_missed;
-	__le32 tx_dir_pkts;
-	__le32 tx_nondir_pkts;
-	__le32 tx_failure;
-	__le32 tx_errors;
-	__le32 tx_drop_max_retry;
-	__le32 tx_err_abort;
-	__le32 tx_err_carrier;
-	__le32 rx_bytes;
-	__le32 tx_bytes;
-	__le32 tx_power;
-	__le32 tx_max_power;
-	__le32 roam_threshold;
-	__le32 ap_assoc_nr;
-	__le32 scan_full;
-	__le32 scan_abort;
-	__le32 ap_nr;
-	__le32 roam_nr;
-	__le32 roam_missed_beacons;
-	__le32 roam_rssi;
-	__le32 roam_unassoc;
-	__le32 roam_deauth;
-	__le32 roam_ap_loadblance;
-} __packed;
-
-#define UMAC_STOP_TX_FLAG    0x1
-#define UMAC_RESUME_TX_FLAG  0x2
-
-#define LAST_SEQ_NUM_INVALID     0xFFFF
-
-struct iwm_umac_notif_stop_resume_tx {
-	struct iwm_umac_wifi_in_hdr hdr;
-	u8 flags; /* UMAC_*_TX_FLAG_* */
-	u8 sta_id;
-	__le16 stop_resume_tid_msk; /* tid bitmask */
-} __packed;
-
-#define UMAC_MAX_NUM_PMKIDS 4
-
-/* WiFi interface wrapper header */
-struct iwm_umac_wifi_if {
-	u8 oid;
-	u8 flags;
-	__le16 buf_size;
-} __packed;
-
-#define IWM_SEQ_NUM_HOST_MSK	0x0000
-#define IWM_SEQ_NUM_UMAC_MSK	0x4000
-#define IWM_SEQ_NUM_LMAC_MSK	0x8000
-#define IWM_SEQ_NUM_MSK		0xC000
-
-#endif
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index 2fa879b015b..eb5de800ed9 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -435,24 +435,40 @@ static int lbs_add_wpa_tlv(u8 *tlv, const u8 *ie, u8 ie_len)
  * Set Channel
  */
 
-static int lbs_cfg_set_channel(struct wiphy *wiphy,
-	struct net_device *netdev,
-	struct ieee80211_channel *channel,
-	enum nl80211_channel_type channel_type)
+static int lbs_cfg_set_monitor_channel(struct wiphy *wiphy,
+				       struct ieee80211_channel *channel,
+				       enum nl80211_channel_type channel_type)
 {
 	struct lbs_private *priv = wiphy_priv(wiphy);
 	int ret = -ENOTSUPP;
 
-	lbs_deb_enter_args(LBS_DEB_CFG80211, "iface %s freq %d, type %d",
-			   netdev_name(netdev), channel->center_freq, channel_type);
+	lbs_deb_enter_args(LBS_DEB_CFG80211, "freq %d, type %d",
+			   channel->center_freq, channel_type);
 
 	if (channel_type != NL80211_CHAN_NO_HT)
 		goto out;
 
-	if (netdev == priv->mesh_dev)
-		ret = lbs_mesh_set_channel(priv, channel->hw_value);
-	else
-		ret = lbs_set_channel(priv, channel->hw_value);
+	ret = lbs_set_channel(priv, channel->hw_value);
+
+ out:
+	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
+	return ret;
+}
+
+static int lbs_cfg_set_mesh_channel(struct wiphy *wiphy,
+				    struct net_device *netdev,
+				    struct ieee80211_channel *channel)
+{
+	struct lbs_private *priv = wiphy_priv(wiphy);
+	int ret = -ENOTSUPP;
+
+	lbs_deb_enter_args(LBS_DEB_CFG80211, "iface %s freq %d",
+			   netdev_name(netdev), channel->center_freq);
+
+	if (netdev != priv->mesh_dev)
+		goto out;
+
+	ret = lbs_mesh_set_channel(priv, channel->hw_value);
 
  out:
 	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
@@ -789,7 +805,6 @@ void lbs_scan_done(struct lbs_private *priv)
 }
 
 static int lbs_cfg_scan(struct wiphy *wiphy,
-	struct net_device *dev,
 	struct cfg80211_scan_request *request)
 {
 	struct lbs_private *priv = wiphy_priv(wiphy);
@@ -2029,7 +2044,8 @@ static int lbs_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
  */
 
 static struct cfg80211_ops lbs_cfg80211_ops = {
-	.set_channel = lbs_cfg_set_channel,
+	.set_monitor_channel = lbs_cfg_set_monitor_channel,
+	.libertas_set_mesh_channel = lbs_cfg_set_mesh_channel,
 	.scan = lbs_cfg_scan,
 	.connect = lbs_cfg_connect,
 	.disconnect = lbs_cfg_disconnect,
@@ -2164,13 +2180,15 @@ int lbs_reg_notifier(struct wiphy *wiphy,
 		struct regulatory_request *request)
 {
 	struct lbs_private *priv = wiphy_priv(wiphy);
-	int ret;
+	int ret = 0;
 
 	lbs_deb_enter_args(LBS_DEB_CFG80211, "cfg80211 regulatory domain "
 			"callback for domain %c%c\n", request->alpha2[0],
 			request->alpha2[1]);
 
-	ret = lbs_set_11d_domain_info(priv, request, wiphy->bands);
+	memcpy(priv->country_code, request->alpha2, sizeof(request->alpha2));
+	if (lbs_iface_active(priv))
+		ret = lbs_set_11d_domain_info(priv);
 
 	lbs_deb_leave(LBS_DEB_CFG80211);
 	return ret;
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index d798bcc0d83..26e68326710 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -733,15 +733,13 @@ int lbs_get_rssi(struct lbs_private *priv, s8 *rssi, s8 *nf)
  *  to the firmware
  *
  *  @priv:	pointer to &struct lbs_private
- *  @request:	cfg80211 regulatory request structure
- *  @bands:	the device's supported bands and channels
  *
  *  returns:	0 on success, error code on failure
 */
-int lbs_set_11d_domain_info(struct lbs_private *priv,
-			    struct regulatory_request *request,
-			    struct ieee80211_supported_band **bands)
+int lbs_set_11d_domain_info(struct lbs_private *priv)
 {
+	struct wiphy *wiphy = priv->wdev->wiphy;
+	struct ieee80211_supported_band **bands = wiphy->bands;
 	struct cmd_ds_802_11d_domain_info cmd;
 	struct mrvl_ie_domain_param_set *domain = &cmd.domain;
 	struct ieee80211_country_ie_triplet *t;
@@ -752,21 +750,23 @@ int lbs_set_11d_domain_info(struct lbs_private *priv,
 	u8 first_channel = 0, next_chan = 0, max_pwr = 0;
 	u8 i, flag = 0;
 	size_t triplet_size;
-	int ret;
+	int ret = 0;
 
 	lbs_deb_enter(LBS_DEB_11D);
+	if (!priv->country_code[0])
+		goto out;
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_SET);
 
 	lbs_deb_11d("Setting country code '%c%c'\n",
-		    request->alpha2[0], request->alpha2[1]);
+		    priv->country_code[0], priv->country_code[1]);
 
 	domain->header.type = cpu_to_le16(TLV_TYPE_DOMAIN);
 
 	/* Set country code */
-	domain->country_code[0] = request->alpha2[0];
-	domain->country_code[1] = request->alpha2[1];
+	domain->country_code[0] = priv->country_code[0];
+	domain->country_code[1] = priv->country_code[1];
 	domain->country_code[2] = ' ';
 
 	/* Now set up the channel triplets; firmware is somewhat picky here
@@ -848,6 +848,7 @@ int lbs_set_11d_domain_info(struct lbs_private *priv,
 
 	ret = lbs_cmd_with_response(priv, CMD_802_11D_DOMAIN_INFO, &cmd);
 
+out:
 	lbs_deb_leave_args(LBS_DEB_11D, "ret %d", ret);
 	return ret;
 }
@@ -1019,9 +1020,9 @@ static void lbs_submit_command(struct lbs_private *priv,
 	if (ret) {
 		netdev_info(priv->dev, "DNLD_CMD: hw_host_to_card failed: %d\n",
 			    ret);
-		/* Let the timer kick in and retry, and potentially reset
-		   the whole thing if the condition persists */
-		timeo = HZ/4;
+		/* Reset dnld state machine, report failure */
+		priv->dnld_sent = DNLD_RES_RECEIVED;
+		lbs_complete_command(priv, cmdnode, ret);
 	}
 
 	if (command == CMD_802_11_DEEP_SLEEP) {
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/libertas/cmd.h
index b280ef7a0ae..ab07608e13d 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/libertas/cmd.h
@@ -128,9 +128,7 @@ int lbs_set_monitor_mode(struct lbs_private *priv, int enable);
 
 int lbs_get_rssi(struct lbs_private *priv, s8 *snr, s8 *nf);
 
-int lbs_set_11d_domain_info(struct lbs_private *priv,
-			    struct regulatory_request *request,
-			    struct ieee80211_supported_band **bands);
+int lbs_set_11d_domain_info(struct lbs_private *priv);
 
 int lbs_get_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 *value);
 
diff --git a/drivers/net/wireless/libertas/debugfs.c b/drivers/net/wireless/libertas/debugfs.c
index a06cc283e23..668dd27616a 100644
--- a/drivers/net/wireless/libertas/debugfs.c
+++ b/drivers/net/wireless/libertas/debugfs.c
@@ -483,7 +483,7 @@ static ssize_t lbs_rdmac_write(struct file *file,
 		res = -EFAULT;
 		goto out_unlock;
 	}
-	priv->mac_offset = simple_strtoul((char *)buf, NULL, 16);
+	priv->mac_offset = simple_strtoul(buf, NULL, 16);
 	res = count;
 out_unlock:
 	free_page(addr);
@@ -565,7 +565,7 @@ static ssize_t lbs_rdbbp_write(struct file *file,
 		res = -EFAULT;
 		goto out_unlock;
 	}
-	priv->bbp_offset = simple_strtoul((char *)buf, NULL, 16);
+	priv->bbp_offset = simple_strtoul(buf, NULL, 16);
 	res = count;
 out_unlock:
 	free_page(addr);
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 672005430ac..6bd1608992b 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -49,6 +49,7 @@ struct lbs_private {
 	bool wiphy_registered;
 	struct cfg80211_scan_request *scan_req;
 	u8 assoc_bss[ETH_ALEN];
+	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
 	u8 disassoc_reason;
 
 	/* Mesh */
@@ -58,6 +59,7 @@ struct lbs_private {
 	uint16_t mesh_tlv;
 	u8 mesh_ssid[IEEE80211_MAX_SSID_LEN + 1];
 	u8 mesh_ssid_len;
+	u8 mesh_channel;
 #endif
 
 	/* Debugfs */
diff --git a/drivers/net/wireless/libertas/firmware.c b/drivers/net/wireless/libertas/firmware.c
index 601f2075355..c0f9e7e862f 100644
--- a/drivers/net/wireless/libertas/firmware.c
+++ b/drivers/net/wireless/libertas/firmware.c
@@ -4,9 +4,7 @@
 
 #include <linux/sched.h>
 #include <linux/firmware.h>
-#include <linux/firmware.h>
 #include <linux/module.h>
-#include <linux/sched.h>
 
 #include "dev.h"
 #include "decl.h"
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index 2e2dbfa2ee5..96726f79a1d 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -68,7 +68,6 @@
 #define CMD_802_11_BEACON_STOP                  0x0049
 #define CMD_802_11_MAC_ADDRESS                  0x004d
 #define CMD_802_11_LED_GPIO_CTRL                0x004e
-#define CMD_802_11_EEPROM_ACCESS                0x0059
 #define CMD_802_11_BAND_CONFIG                  0x0058
 #define CMD_GSPI_BUS_CONFIG                     0x005a
 #define CMD_802_11D_DOMAIN_INFO                 0x005b
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index cd3b0d40061..27980778d99 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -10,6 +10,7 @@
 #include <linux/netdevice.h>
 #include <linux/slab.h>
 #include <linux/usb.h>
+#include <linux/olpc-ec.h>
 
 #ifdef CONFIG_OLPC
 #include <asm/olpc.h>
@@ -302,14 +303,13 @@ error:
 static void if_usb_disconnect(struct usb_interface *intf)
 {
 	struct if_usb_card *cardp = usb_get_intfdata(intf);
-	struct lbs_private *priv = (struct lbs_private *) cardp->priv;
+	struct lbs_private *priv = cardp->priv;
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
 	cardp->surprise_removed = 1;
 
 	if (priv) {
-		priv->surpriseremoved = 1;
 		lbs_stop_card(priv);
 		lbs_remove_card(priv);
 	}
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index e96ee0aa843..58048189bd2 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -152,6 +152,12 @@ int lbs_start_iface(struct lbs_private *priv)
 		goto err;
 	}
 
+	ret = lbs_set_11d_domain_info(priv);
+	if (ret) {
+		lbs_deb_net("set 11d domain info failed\n");
+		goto err;
+	}
+
 	lbs_update_channel(priv);
 
 	priv->iface_running = true;
diff --git a/drivers/net/wireless/libertas/mesh.c b/drivers/net/wireless/libertas/mesh.c
index e87c031b298..97807751ebc 100644
--- a/drivers/net/wireless/libertas/mesh.c
+++ b/drivers/net/wireless/libertas/mesh.c
@@ -131,16 +131,13 @@ static int lbs_mesh_config(struct lbs_private *priv, uint16_t action,
 
 int lbs_mesh_set_channel(struct lbs_private *priv, u8 channel)
 {
+	priv->mesh_channel = channel;
 	return lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START, channel);
 }
 
 static uint16_t lbs_mesh_get_channel(struct lbs_private *priv)
 {
-	struct wireless_dev *mesh_wdev = priv->mesh_dev->ieee80211_ptr;
-	if (mesh_wdev->channel)
-		return mesh_wdev->channel->hw_value;
-	else
-		return 1;
+	return priv->mesh_channel ?: 1;
 }
 
 /***************************************************************************
diff --git a/drivers/net/wireless/libertas_tf/if_usb.c b/drivers/net/wireless/libertas_tf/if_usb.c
index 19a5a92dd77..d576dd6665d 100644
--- a/drivers/net/wireless/libertas_tf/if_usb.c
+++ b/drivers/net/wireless/libertas_tf/if_usb.c
@@ -253,7 +253,7 @@ lbtf_deb_leave(LBTF_DEB_MAIN);
 static void if_usb_disconnect(struct usb_interface *intf)
 {
 	struct if_usb_card *cardp = usb_get_intfdata(intf);
-	struct lbtf_private *priv = (struct lbtf_private *) cardp->priv;
+	struct lbtf_private *priv = cardp->priv;
 
 	lbtf_deb_enter(LBTF_DEB_MAIN);
 
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index a0b7cfd3468..00838395778 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -292,7 +292,7 @@ struct mac80211_hwsim_data {
 	struct list_head list;
 	struct ieee80211_hw *hw;
 	struct device *dev;
-	struct ieee80211_supported_band bands[2];
+	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
 	struct ieee80211_channel channels_2ghz[ARRAY_SIZE(hwsim_channels_2ghz)];
 	struct ieee80211_channel channels_5ghz[ARRAY_SIZE(hwsim_channels_5ghz)];
 	struct ieee80211_rate rates[ARRAY_SIZE(hwsim_rates)];
@@ -571,7 +571,7 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 			skb_dequeue(&data->pending);
 	}
 
-	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
+	skb = genlmsg_new(GENLMSG_DEFAULT_SIZE, GFP_ATOMIC);
 	if (skb == NULL)
 		goto nla_put_failure;
 
@@ -678,8 +678,7 @@ static bool mac80211_hwsim_tx_frame_no_nl(struct ieee80211_hw *hw,
 			continue;
 
 		if (data2->idle || !data2->started ||
-		    !hwsim_ps_rx_ok(data2, skb) ||
-		    !data->channel || !data2->channel ||
+		    !hwsim_ps_rx_ok(data2, skb) || !data2->channel ||
 		    data->channel->center_freq != data2->channel->center_freq ||
 		    !(data->group & data2->group))
 			continue;
@@ -740,11 +739,6 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	txi = IEEE80211_SKB_CB(skb);
 
-	if (txi->control.vif)
-		hwsim_check_magic(txi->control.vif);
-	if (txi->control.sta)
-		hwsim_check_sta_magic(txi->control.sta);
-
 	ieee80211_tx_info_clear_status(txi);
 
 	/* frame was transmitted at most favorable rate at first attempt */
@@ -1083,6 +1077,8 @@ enum hwsim_testmode_attr {
 enum hwsim_testmode_cmd {
 	HWSIM_TM_CMD_SET_PS		= 0,
 	HWSIM_TM_CMD_GET_PS		= 1,
+	HWSIM_TM_CMD_STOP_QUEUES	= 2,
+	HWSIM_TM_CMD_WAKE_QUEUES	= 3,
 };
 
 static const struct nla_policy hwsim_testmode_policy[HWSIM_TM_ATTR_MAX + 1] = {
@@ -1122,6 +1118,12 @@ static int mac80211_hwsim_testmode_cmd(struct ieee80211_hw *hw,
 		if (nla_put_u32(skb, HWSIM_TM_ATTR_PS, hwsim->ps))
 			goto nla_put_failure;
 		return cfg80211_testmode_reply(skb);
+	case HWSIM_TM_CMD_STOP_QUEUES:
+		ieee80211_stop_queues(hw);
+		return 0;
+	case HWSIM_TM_CMD_WAKE_QUEUES:
+		ieee80211_wake_queues(hw);
+		return 0;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -1486,7 +1488,7 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 	struct mac80211_hwsim_data *data2;
 	struct ieee80211_tx_info *txi;
 	struct hwsim_tx_rate *tx_attempts;
-	struct sk_buff __user *ret_skb;
+	unsigned long ret_skb_ptr;
 	struct sk_buff *skb, *tmp;
 	struct mac_address *src;
 	unsigned int hwsim_flags;
@@ -1504,8 +1506,7 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 				   info->attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
 	hwsim_flags = nla_get_u32(info->attrs[HWSIM_ATTR_FLAGS]);
 
-	ret_skb = (struct sk_buff __user *)
-		  (unsigned long) nla_get_u64(info->attrs[HWSIM_ATTR_COOKIE]);
+	ret_skb_ptr = nla_get_u64(info->attrs[HWSIM_ATTR_COOKIE]);
 
 	data2 = get_hwsim_data_ref_from_addr(src);
 
@@ -1514,7 +1515,7 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 
 	/* look for the skb matching the cookie passed back from user */
 	skb_queue_walk_safe(&data2->pending, skb, tmp) {
-		if (skb == ret_skb) {
+		if ((unsigned long)skb == ret_skb_ptr) {
 			skb_unlink(skb, &data2->pending);
 			found = true;
 			break;
@@ -1534,11 +1535,6 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 	/* now send back TX status */
 	txi = IEEE80211_SKB_CB(skb);
 
-	if (txi->control.vif)
-		hwsim_check_magic(txi->control.vif);
-	if (txi->control.sta)
-		hwsim_check_sta_magic(txi->control.sta);
-
 	ieee80211_tx_info_clear_status(txi);
 
 	for (i = 0; i < IEEE80211_TX_MAX_RATES; i++) {
@@ -1857,7 +1853,7 @@ static int __init init_mac80211_hwsim(void)
 				sband->n_bitrates = ARRAY_SIZE(hwsim_rates) - 4;
 				break;
 			default:
-				break;
+				continue;
 			}
 
 			sband->ht_cap.ht_supported = true;
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/mwifiex/11n.c
index fe8ebfebcc0..e535c937628 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/mwifiex/11n.c
@@ -101,8 +101,7 @@ int mwifiex_ret_11n_delba(struct mwifiex_private *priv,
 {
 	int tid;
 	struct mwifiex_tx_ba_stream_tbl *tx_ba_tbl;
-	struct host_cmd_ds_11n_delba *del_ba =
-		(struct host_cmd_ds_11n_delba *) &resp->params.del_ba;
+	struct host_cmd_ds_11n_delba *del_ba = &resp->params.del_ba;
 	uint16_t del_ba_param_set = le16_to_cpu(del_ba->del_ba_param_set);
 
 	tid = del_ba_param_set >> DELBA_TID_POS;
@@ -147,8 +146,7 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 			      struct host_cmd_ds_command *resp)
 {
 	int tid;
-	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp =
-		(struct host_cmd_ds_11n_addba_rsp *) &resp->params.add_ba_rsp;
+	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp = &resp->params.add_ba_rsp;
 	struct mwifiex_tx_ba_stream_tbl *tx_ba_tbl;
 
 	add_ba_rsp->ssn = cpu_to_le16((le16_to_cpu(add_ba_rsp->ssn))
@@ -412,7 +410,7 @@ mwifiex_cmd_append_11n_tlv(struct mwifiex_private *priv,
 
 		memcpy((u8 *) bss_co_2040 +
 		       sizeof(struct mwifiex_ie_types_header),
-		       (u8 *) bss_desc->bcn_bss_co_2040 +
+		       bss_desc->bcn_bss_co_2040 +
 		       sizeof(struct ieee_types_header),
 		       le16_to_cpu(bss_co_2040->header.len));
 
@@ -426,10 +424,8 @@ mwifiex_cmd_append_11n_tlv(struct mwifiex_private *priv,
 		ext_cap->header.type = cpu_to_le16(WLAN_EID_EXT_CAPABILITY);
 		ext_cap->header.len = cpu_to_le16(sizeof(ext_cap->ext_cap));
 
-		memcpy((u8 *) ext_cap +
-		       sizeof(struct mwifiex_ie_types_header),
-		       (u8 *) bss_desc->bcn_ext_cap +
-		       sizeof(struct ieee_types_header),
+		memcpy((u8 *)ext_cap + sizeof(struct mwifiex_ie_types_header),
+		       bss_desc->bcn_ext_cap + sizeof(struct ieee_types_header),
 		       le16_to_cpu(ext_cap->header.len));
 
 		*buffer += sizeof(struct mwifiex_ie_types_extcap);
diff --git a/drivers/net/wireless/mwifiex/11n.h b/drivers/net/wireless/mwifiex/11n.h
index 77646d777dc..28366e9211f 100644
--- a/drivers/net/wireless/mwifiex/11n.h
+++ b/drivers/net/wireless/mwifiex/11n.h
@@ -105,8 +105,7 @@ static inline u8 mwifiex_space_avail_for_new_ba_stream(
 		priv = adapter->priv[i];
 		if (priv)
 			ba_stream_num += mwifiex_wmm_list_len(
-						(struct list_head *)
-						&priv->tx_ba_stream_tbl_ptr);
+				&priv->tx_ba_stream_tbl_ptr);
 	}
 
 	return ((ba_stream_num <
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index 900ee129e82..591ccd33f83 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -297,9 +297,7 @@ mwifiex_11n_create_rx_reorder_tbl(struct mwifiex_private *priv, u8 *ta,
  */
 int mwifiex_cmd_11n_addba_req(struct host_cmd_ds_command *cmd, void *data_buf)
 {
-	struct host_cmd_ds_11n_addba_req *add_ba_req =
-		(struct host_cmd_ds_11n_addba_req *)
-		&cmd->params.add_ba_req;
+	struct host_cmd_ds_11n_addba_req *add_ba_req = &cmd->params.add_ba_req;
 
 	cmd->command = cpu_to_le16(HostCmd_CMD_11N_ADDBA_REQ);
 	cmd->size = cpu_to_le16(sizeof(*add_ba_req) + S_DS_GEN);
@@ -321,9 +319,7 @@ int mwifiex_cmd_11n_addba_rsp_gen(struct mwifiex_private *priv,
 				  struct host_cmd_ds_11n_addba_req
 				  *cmd_addba_req)
 {
-	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp =
-		(struct host_cmd_ds_11n_addba_rsp *)
-		&cmd->params.add_ba_rsp;
+	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp = &cmd->params.add_ba_rsp;
 	u8 tid;
 	int win_size;
 	uint16_t block_ack_param_set;
@@ -368,8 +364,7 @@ int mwifiex_cmd_11n_addba_rsp_gen(struct mwifiex_private *priv,
  */
 int mwifiex_cmd_11n_delba(struct host_cmd_ds_command *cmd, void *data_buf)
 {
-	struct host_cmd_ds_11n_delba *del_ba = (struct host_cmd_ds_11n_delba *)
-		&cmd->params.del_ba;
+	struct host_cmd_ds_11n_delba *del_ba = &cmd->params.del_ba;
 
 	cmd->command = cpu_to_le16(HostCmd_CMD_11N_DELBA);
 	cmd->size = cpu_to_le16(sizeof(*del_ba) + S_DS_GEN);
@@ -399,8 +394,7 @@ int mwifiex_11n_rx_reorder_pkt(struct mwifiex_private *priv,
 	int start_win, end_win, win_size;
 	u16 pkt_index;
 
-	tbl = mwifiex_11n_get_rx_reorder_tbl((struct mwifiex_private *) priv,
-					     tid, ta);
+	tbl = mwifiex_11n_get_rx_reorder_tbl(priv, tid, ta);
 	if (!tbl) {
 		if (pkt_type != PKT_TYPE_BAR)
 			mwifiex_process_rx_packet(priv->adapter, payload);
@@ -521,9 +515,7 @@ mwifiex_del_ba_tbl(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 int mwifiex_ret_11n_addba_resp(struct mwifiex_private *priv,
 			       struct host_cmd_ds_command *resp)
 {
-	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp =
-		(struct host_cmd_ds_11n_addba_rsp *)
-		&resp->params.add_ba_rsp;
+	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp = &resp->params.add_ba_rsp;
 	int tid, win_size;
 	struct mwifiex_rx_reorder_tbl *tbl;
 	uint16_t block_ack_param_set;
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index ce61b6fae1c..fe42137384d 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -48,10 +48,9 @@ static const struct ieee80211_iface_combination mwifiex_iface_comb_ap_sta = {
  *      Others                 -> IEEE80211_HT_PARAM_CHA_SEC_NONE
  */
 static u8
-mwifiex_cfg80211_channel_type_to_sec_chan_offset(enum nl80211_channel_type
-						 channel_type)
+mwifiex_chan_type_to_sec_chan_offset(enum nl80211_channel_type chan_type)
 {
-	switch (channel_type) {
+	switch (chan_type) {
 	case NL80211_CHAN_NO_HT:
 	case NL80211_CHAN_HT20:
 		return IEEE80211_HT_PARAM_CHA_SEC_NONE;
@@ -170,7 +169,9 @@ mwifiex_cfg80211_set_default_key(struct wiphy *wiphy, struct net_device *netdev,
 	if (!priv->sec_info.wep_enabled)
 		return 0;
 
-	if (mwifiex_set_encode(priv, NULL, 0, key_index, NULL, 0)) {
+	if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP) {
+		priv->wep_key_curr_index = key_index;
+	} else if (mwifiex_set_encode(priv, NULL, 0, key_index, NULL, 0)) {
 		wiphy_err(wiphy, "set default Tx key index\n");
 		return -EFAULT;
 	}
@@ -187,9 +188,25 @@ mwifiex_cfg80211_add_key(struct wiphy *wiphy, struct net_device *netdev,
 			 struct key_params *params)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(netdev);
+	struct mwifiex_wep_key *wep_key;
 	const u8 bc_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP &&
+	    (params->cipher == WLAN_CIPHER_SUITE_WEP40 ||
+	     params->cipher == WLAN_CIPHER_SUITE_WEP104)) {
+		if (params->key && params->key_len) {
+			wep_key = &priv->wep_key[key_index];
+			memset(wep_key, 0, sizeof(struct mwifiex_wep_key));
+			memcpy(wep_key->key_material, params->key,
+			       params->key_len);
+			wep_key->key_index = key_index;
+			wep_key->key_length = params->key_len;
+			priv->sec_info.wep_enabled = 1;
+		}
+		return 0;
+	}
+
 	if (mwifiex_set_encode(priv, params->key, params->key_len,
 			       key_index, peer_mac, 0)) {
 		wiphy_err(wiphy, "crypto keys added\n");
@@ -242,13 +259,13 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 			flag = 1;
 			first_chan = (u32) ch->hw_value;
 			next_chan = first_chan;
-			max_pwr = ch->max_power;
+			max_pwr = ch->max_reg_power;
 			no_of_parsed_chan = 1;
 			continue;
 		}
 
 		if (ch->hw_value == next_chan + 1 &&
-		    ch->max_power == max_pwr) {
+		    ch->max_reg_power == max_pwr) {
 			next_chan++;
 			no_of_parsed_chan++;
 		} else {
@@ -259,7 +276,7 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 			no_of_triplet++;
 			first_chan = (u32) ch->hw_value;
 			next_chan = first_chan;
-			max_pwr = ch->max_power;
+			max_pwr = ch->max_reg_power;
 			no_of_parsed_chan = 1;
 		}
 	}
@@ -321,79 +338,6 @@ static int mwifiex_reg_notifier(struct wiphy *wiphy,
 }
 
 /*
- * This function sets the RF channel.
- *
- * This function creates multiple IOCTL requests, populates them accordingly
- * and issues them to set the band/channel and frequency.
- */
-static int
-mwifiex_set_rf_channel(struct mwifiex_private *priv,
-		       struct ieee80211_channel *chan,
-		       enum nl80211_channel_type channel_type)
-{
-	struct mwifiex_chan_freq_power cfp;
-	u32 config_bands = 0;
-	struct wiphy *wiphy = priv->wdev->wiphy;
-	struct mwifiex_adapter *adapter = priv->adapter;
-
-	if (chan) {
-		/* Set appropriate bands */
-		if (chan->band == IEEE80211_BAND_2GHZ) {
-			if (channel_type == NL80211_CHAN_NO_HT)
-				if (priv->adapter->config_bands == BAND_B ||
-				    priv->adapter->config_bands == BAND_G)
-					config_bands =
-						priv->adapter->config_bands;
-				else
-					config_bands = BAND_B | BAND_G;
-			else
-				config_bands = BAND_B | BAND_G | BAND_GN;
-		} else {
-			if (channel_type == NL80211_CHAN_NO_HT)
-				config_bands = BAND_A;
-			else
-				config_bands = BAND_AN | BAND_A;
-		}
-
-		if (!((config_bands | adapter->fw_bands) &
-						~adapter->fw_bands)) {
-			adapter->config_bands = config_bands;
-			if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
-				adapter->adhoc_start_band = config_bands;
-				if ((config_bands & BAND_GN) ||
-				    (config_bands & BAND_AN))
-					adapter->adhoc_11n_enabled = true;
-				else
-					adapter->adhoc_11n_enabled = false;
-			}
-		}
-		adapter->sec_chan_offset =
-			mwifiex_cfg80211_channel_type_to_sec_chan_offset
-			(channel_type);
-		adapter->channel_type = channel_type;
-
-		mwifiex_send_domain_info_cmd_fw(wiphy);
-	}
-
-	wiphy_dbg(wiphy, "info: setting band %d, chan offset %d, mode %d\n",
-		  config_bands, adapter->sec_chan_offset, priv->bss_mode);
-	if (!chan)
-		return 0;
-
-	memset(&cfp, 0, sizeof(cfp));
-	cfp.freq = chan->center_freq;
-	cfp.channel = ieee80211_frequency_to_channel(chan->center_freq);
-
-	if (mwifiex_bss_set_channel(priv, &cfp))
-		return -EFAULT;
-
-	if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
-		return mwifiex_drv_change_adhoc_chan(priv, cfp.channel);
-	else
-		return mwifiex_uap_set_channel(priv, cfp.channel);
-}
-
-/*
  * This function sets the fragmentation threshold.
  *
  * The fragmentation threshold value must lie between MWIFIEX_FRAG_MIN_VALUE
@@ -608,7 +552,7 @@ static int
 mwifiex_dump_station_info(struct mwifiex_private *priv,
 			  struct station_info *sinfo)
 {
-	struct mwifiex_rate_cfg rate;
+	u32 rate;
 
 	sinfo->filled = STATION_INFO_RX_BYTES | STATION_INFO_TX_BYTES |
 			STATION_INFO_RX_PACKETS | STATION_INFO_TX_PACKETS |
@@ -634,9 +578,9 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 
 	/*
 	 * Bit 0 in tx_htinfo indicates that current Tx rate is 11n rate. Valid
-	 * MCS index values for us are 0 to 7.
+	 * MCS index values for us are 0 to 15.
 	 */
-	if ((priv->tx_htinfo & BIT(0)) && (priv->tx_rate < 8)) {
+	if ((priv->tx_htinfo & BIT(0)) && (priv->tx_rate < 16)) {
 		sinfo->txrate.mcs = priv->tx_rate;
 		sinfo->txrate.flags |= RATE_INFO_FLAGS_MCS;
 		/* 40MHz rate */
@@ -654,7 +598,7 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 	sinfo->tx_packets = priv->stats.tx_packets;
 	sinfo->signal = priv->bcn_rssi_avg;
 	/* bit rate is in 500 kb/s units. Convert it to 100kb/s units */
-	sinfo->txrate.legacy = rate.rate * 5;
+	sinfo->txrate.legacy = rate * 5;
 
 	if (priv->bss_mode == NL80211_IFTYPE_STATION) {
 		sinfo->filled |= STATION_INFO_BSS_PARAM;
@@ -809,8 +753,8 @@ static const u32 mwifiex_cipher_suites[] = {
 /*
  * CFG802.11 operation handler for setting bit rates.
  *
- * Function selects legacy bang B/G/BG from corresponding bitrates selection.
- * Currently only 2.4GHz band is supported.
+ * Function configures data rates to firmware using bitrate mask
+ * provided by cfg80211.
  */
 static int mwifiex_cfg80211_set_bitrate_mask(struct wiphy *wiphy,
 				struct net_device *dev,
@@ -818,43 +762,36 @@ static int mwifiex_cfg80211_set_bitrate_mask(struct wiphy *wiphy,
 				const struct cfg80211_bitrate_mask *mask)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-	int index = 0, mode = 0, i;
-	struct mwifiex_adapter *adapter = priv->adapter;
+	u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+	enum ieee80211_band band;
 
-	/* Currently only 2.4GHz is supported */
-	for (i = 0; i < mwifiex_band_2ghz.n_bitrates; i++) {
-		/*
-		 * Rates below 6 Mbps in the table are CCK rates; 802.11b
-		 * and from 6 they are OFDM; 802.11G
-		 */
-		if (mwifiex_rates[i].bitrate == 60) {
-			index = 1 << i;
-			break;
-		}
+	if (!priv->media_connected) {
+		dev_err(priv->adapter->dev,
+			"Can not set Tx data rate in disconnected state\n");
+		return -EINVAL;
 	}
 
-	if (mask->control[IEEE80211_BAND_2GHZ].legacy < index) {
-		mode = BAND_B;
-	} else {
-		mode = BAND_G;
-		if (mask->control[IEEE80211_BAND_2GHZ].legacy % index)
-			mode |=  BAND_B;
-	}
+	band = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
 
-	if (!((mode | adapter->fw_bands) & ~adapter->fw_bands)) {
-		adapter->config_bands = mode;
-		if (priv->bss_mode == NL80211_IFTYPE_ADHOC) {
-			adapter->adhoc_start_band = mode;
-			adapter->adhoc_11n_enabled = false;
-		}
-	}
-	adapter->sec_chan_offset = IEEE80211_HT_PARAM_CHA_SEC_NONE;
-	adapter->channel_type = NL80211_CHAN_NO_HT;
+	memset(bitmap_rates, 0, sizeof(bitmap_rates));
 
-	wiphy_debug(wiphy, "info: device configured in 802.11%s%s mode\n",
-		    (mode & BAND_B) ? "b" : "", (mode & BAND_G) ? "g" : "");
+	/* Fill HR/DSSS rates. */
+	if (band == IEEE80211_BAND_2GHZ)
+		bitmap_rates[0] = mask->control[band].legacy & 0x000f;
 
-	return 0;
+	/* Fill OFDM rates */
+	if (band == IEEE80211_BAND_2GHZ)
+		bitmap_rates[1] = (mask->control[band].legacy & 0x0ff0) >> 4;
+	else
+		bitmap_rates[1] = mask->control[band].legacy;
+
+	/* Fill MCS rates */
+	bitmap_rates[2] = mask->control[band].mcs[0];
+	if (priv->adapter->hw_dev_mcs_support == HT_STREAM_2X2)
+		bitmap_rates[2] |= mask->control[band].mcs[1] << 8;
+
+	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_TX_RATE_CFG,
+				     HostCmd_ACT_GEN_SET, 0, bitmap_rates);
 }
 
 /*
@@ -896,6 +833,69 @@ static int mwifiex_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy,
 	return 0;
 }
 
+/* cfg80211 operation handler for change_beacon.
+ * Function retrieves and sets modified management IEs to FW.
+ */
+static int mwifiex_cfg80211_change_beacon(struct wiphy *wiphy,
+					  struct net_device *dev,
+					  struct cfg80211_beacon_data *data)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP) {
+		wiphy_err(wiphy, "%s: bss_type mismatched\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!priv->bss_started) {
+		wiphy_err(wiphy, "%s: bss not started\n", __func__);
+		return -EINVAL;
+	}
+
+	if (mwifiex_set_mgmt_ies(priv, data)) {
+		wiphy_err(wiphy, "%s: setting mgmt ies failed\n", __func__);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int
+mwifiex_cfg80211_set_antenna(struct wiphy *wiphy, u32 tx_ant, u32 rx_ant)
+{
+	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
+	struct mwifiex_private *priv = mwifiex_get_priv(adapter,
+							MWIFIEX_BSS_ROLE_ANY);
+	struct mwifiex_ds_ant_cfg ant_cfg;
+
+	if (!tx_ant || !rx_ant)
+		return -EOPNOTSUPP;
+
+	if (adapter->hw_dev_mcs_support != HT_STREAM_2X2) {
+		/* Not a MIMO chip. User should provide specific antenna number
+		 * for Tx/Rx path or enable all antennas for diversity
+		 */
+		if (tx_ant != rx_ant)
+			return -EOPNOTSUPP;
+
+		if ((tx_ant & (tx_ant - 1)) &&
+		    (tx_ant != BIT(adapter->number_of_antenna) - 1))
+			return -EOPNOTSUPP;
+
+		if ((tx_ant == BIT(adapter->number_of_antenna) - 1) &&
+		    (priv->adapter->number_of_antenna > 1)) {
+			tx_ant = RF_ANTENNA_AUTO;
+			rx_ant = RF_ANTENNA_AUTO;
+		}
+	}
+
+	ant_cfg.tx_ant = tx_ant;
+	ant_cfg.rx_ant = rx_ant;
+
+	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_RF_ANTENNA,
+				     HostCmd_ACT_GEN_SET, 0, &ant_cfg);
+}
+
 /* cfg80211 operation handler for stop ap.
  * Function stops BSS running at uAP interface.
  */
@@ -926,10 +926,11 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 {
 	struct mwifiex_uap_bss_param *bss_cfg;
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	u8 config_bands = 0;
 
 	if (priv->bss_type != MWIFIEX_BSS_TYPE_UAP)
 		return -1;
-	if (mwifiex_set_mgmt_ies(priv, params))
+	if (mwifiex_set_mgmt_ies(priv, &params->beacon))
 		return -1;
 
 	bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
@@ -958,15 +959,41 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 	case NL80211_HIDDEN_SSID_ZERO_CONTENTS:
 		/* firmware doesn't support this type of hidden SSID */
 	default:
+		kfree(bss_cfg);
 		return -EINVAL;
 	}
 
+	bss_cfg->channel =
+	    (u8)ieee80211_frequency_to_channel(params->channel->center_freq);
+	bss_cfg->band_cfg = BAND_CONFIG_MANUAL;
+
+	/* Set appropriate bands */
+	if (params->channel->band == IEEE80211_BAND_2GHZ) {
+		if (params->channel_type == NL80211_CHAN_NO_HT)
+			config_bands = BAND_B | BAND_G;
+		else
+			config_bands = BAND_B | BAND_G | BAND_GN;
+	} else {
+		if (params->channel_type == NL80211_CHAN_NO_HT)
+			config_bands = BAND_A;
+		else
+			config_bands = BAND_AN | BAND_A;
+	}
+
+	if (!((config_bands | priv->adapter->fw_bands) &
+	      ~priv->adapter->fw_bands))
+		priv->adapter->config_bands = config_bands;
+
+	mwifiex_send_domain_info_cmd_fw(wiphy);
+
 	if (mwifiex_set_secure_params(priv, bss_cfg, params)) {
 		kfree(bss_cfg);
 		wiphy_err(wiphy, "Failed to parse secuirty parameters!\n");
 		return -1;
 	}
 
+	mwifiex_set_ht_params(priv, bss_cfg, params);
+
 	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
 				  HostCmd_ACT_GEN_SET, 0, NULL)) {
 		wiphy_err(wiphy, "Failed to stop the BSS\n");
@@ -990,6 +1017,16 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 		return -1;
 	}
 
+	if (priv->sec_info.wep_enabled)
+		priv->curr_pkt_filter |= HostCmd_ACT_MAC_WEP_ENABLE;
+	else
+		priv->curr_pkt_filter &= ~HostCmd_ACT_MAC_WEP_ENABLE;
+
+	if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_MAC_CONTROL,
+				  HostCmd_ACT_GEN_SET, 0,
+				  &priv->curr_pkt_filter))
+		return -1;
+
 	return 0;
 }
 
@@ -1082,7 +1119,7 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
 	struct cfg80211_ssid req_ssid;
 	int ret, auth_type = 0;
 	struct cfg80211_bss *bss = NULL;
-	u8 is_scanning_required = 0;
+	u8 is_scanning_required = 0, config_bands = 0;
 
 	memset(&req_ssid, 0, sizeof(struct cfg80211_ssid));
 
@@ -1101,9 +1138,19 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len, u8 *ssid,
 	/* disconnect before try to associate */
 	mwifiex_deauthenticate(priv, NULL);
 
-	if (channel)
-		ret = mwifiex_set_rf_channel(priv, channel,
-						priv->adapter->channel_type);
+	if (channel) {
+		if (mode == NL80211_IFTYPE_STATION) {
+			if (channel->band == IEEE80211_BAND_2GHZ)
+				config_bands = BAND_B | BAND_G | BAND_GN;
+			else
+				config_bands = BAND_A | BAND_AN;
+
+			if (!((config_bands | priv->adapter->fw_bands) &
+			      ~priv->adapter->fw_bands))
+				priv->adapter->config_bands = config_bands;
+		}
+		mwifiex_send_domain_info_cmd_fw(priv->wdev->wiphy);
+	}
 
 	/* As this is new association, clear locally stored
 	 * keys and security related flags */
@@ -1268,6 +1315,76 @@ done:
 }
 
 /*
+ * This function sets following parameters for ibss network.
+ *  -  channel
+ *  -  start band
+ *  -  11n flag
+ *  -  secondary channel offset
+ */
+static int mwifiex_set_ibss_params(struct mwifiex_private *priv,
+				   struct cfg80211_ibss_params *params)
+{
+	struct wiphy *wiphy = priv->wdev->wiphy;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	int index = 0, i;
+	u8 config_bands = 0;
+
+	if (params->channel->band == IEEE80211_BAND_2GHZ) {
+		if (!params->basic_rates) {
+			config_bands = BAND_B | BAND_G;
+		} else {
+			for (i = 0; i < mwifiex_band_2ghz.n_bitrates; i++) {
+				/*
+				 * Rates below 6 Mbps in the table are CCK
+				 * rates; 802.11b and from 6 they are OFDM;
+				 * 802.11G
+				 */
+				if (mwifiex_rates[i].bitrate == 60) {
+					index = 1 << i;
+					break;
+				}
+			}
+
+			if (params->basic_rates < index) {
+				config_bands = BAND_B;
+			} else {
+				config_bands = BAND_G;
+				if (params->basic_rates % index)
+					config_bands |= BAND_B;
+			}
+		}
+
+		if (params->channel_type != NL80211_CHAN_NO_HT)
+			config_bands |= BAND_GN;
+	} else {
+		if (params->channel_type == NL80211_CHAN_NO_HT)
+			config_bands = BAND_A;
+		else
+			config_bands = BAND_AN | BAND_A;
+	}
+
+	if (!((config_bands | adapter->fw_bands) & ~adapter->fw_bands)) {
+		adapter->config_bands = config_bands;
+		adapter->adhoc_start_band = config_bands;
+
+		if ((config_bands & BAND_GN) || (config_bands & BAND_AN))
+			adapter->adhoc_11n_enabled = true;
+		else
+			adapter->adhoc_11n_enabled = false;
+	}
+
+	adapter->sec_chan_offset =
+		mwifiex_chan_type_to_sec_chan_offset(params->channel_type);
+	priv->adhoc_channel =
+		ieee80211_frequency_to_channel(params->channel->center_freq);
+
+	wiphy_dbg(wiphy, "info: set ibss band %d, chan %d, chan offset %d\n",
+		  config_bands, priv->adhoc_channel, adapter->sec_chan_offset);
+
+	return 0;
+}
+
+/*
  * CFG802.11 operation handler to join an IBSS.
  *
  * This function does not work in any mode other than Ad-Hoc, or if
@@ -1289,6 +1406,8 @@ mwifiex_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	wiphy_dbg(wiphy, "info: trying to join to %s and bssid %pM\n",
 		  (char *) params->ssid, params->bssid);
 
+	mwifiex_set_ibss_params(priv, params);
+
 	ret = mwifiex_cfg80211_assoc(priv, params->ssid_len, params->ssid,
 				     params->bssid, priv->bss_mode,
 				     params->channel, NULL, params->privacy);
@@ -1335,9 +1454,10 @@ mwifiex_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
  * it also informs the results.
  */
 static int
-mwifiex_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
+mwifiex_cfg80211_scan(struct wiphy *wiphy,
 		      struct cfg80211_scan_request *request)
 {
+	struct net_device *dev = request->wdev->netdev;
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 	int i;
 	struct ieee80211_channel *chan;
@@ -1381,7 +1501,7 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
 
 		priv->user_scan_cfg->chan_list[i].scan_time = 0;
 	}
-	if (mwifiex_set_user_scan_ioctl(priv, priv->user_scan_cfg))
+	if (mwifiex_scan_networks(priv, priv->user_scan_cfg))
 		return -EFAULT;
 
 	if (request->ie && request->ie_len) {
@@ -1471,11 +1591,11 @@ mwifiex_setup_ht_caps(struct ieee80211_sta_ht_cap *ht_info,
 /*
  *  create a new virtual interface with the given name
  */
-struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
-					    char *name,
-					    enum nl80211_iftype type,
-					    u32 *flags,
-					    struct vif_params *params)
+struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
+					      char *name,
+					      enum nl80211_iftype type,
+					      u32 *flags,
+					      struct vif_params *params)
 {
 	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
 	struct mwifiex_private *priv;
@@ -1596,16 +1716,16 @@ struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 #ifdef CONFIG_DEBUG_FS
 	mwifiex_dev_debugfs_init(priv);
 #endif
-	return dev;
+	return wdev;
 }
 EXPORT_SYMBOL_GPL(mwifiex_add_virtual_intf);
 
 /*
  * del_virtual_intf: remove the virtual interface determined by dev
  */
-int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev)
+int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
 {
-	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(wdev->netdev);
 
 #ifdef CONFIG_DEBUG_FS
 	mwifiex_dev_debugfs_remove(priv);
@@ -1617,11 +1737,11 @@ int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev)
 	if (netif_carrier_ok(priv->netdev))
 		netif_carrier_off(priv->netdev);
 
-	if (dev->reg_state == NETREG_REGISTERED)
-		unregister_netdevice(dev);
+	if (wdev->netdev->reg_state == NETREG_REGISTERED)
+		unregister_netdevice(wdev->netdev);
 
-	if (dev->reg_state == NETREG_UNREGISTERED)
-		free_netdev(dev);
+	if (wdev->netdev->reg_state == NETREG_UNREGISTERED)
+		free_netdev(wdev->netdev);
 
 	/* Clear the priv in adapter */
 	priv->netdev = NULL;
@@ -1655,7 +1775,9 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.set_bitrate_mask = mwifiex_cfg80211_set_bitrate_mask,
 	.start_ap = mwifiex_cfg80211_start_ap,
 	.stop_ap = mwifiex_cfg80211_stop_ap,
+	.change_beacon = mwifiex_cfg80211_change_beacon,
 	.set_cqm_rssi_config = mwifiex_cfg80211_set_cqm_rssi_config,
+	.set_antenna = mwifiex_cfg80211_set_antenna,
 };
 
 /*
@@ -1702,7 +1824,16 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 
 	memcpy(wiphy->perm_addr, priv->curr_addr, ETH_ALEN);
 	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
-	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME | WIPHY_FLAG_CUSTOM_REGULATORY;
+	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME |
+			WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD;
+
+	wiphy->probe_resp_offload = NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS |
+				    NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2;
+
+	wiphy->available_antennas_tx = BIT(adapter->number_of_antenna) - 1;
+	wiphy->available_antennas_rx = BIT(adapter->number_of_antenna) - 1;
+
+	wiphy->features = NL80211_FEATURE_HT_IBSS;
 
 	/* Reserve space for mwifiex specific private data for BSS */
 	wiphy->bss_priv_size = sizeof(struct mwifiex_bss_priv);
@@ -1713,7 +1844,7 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 	wdev_priv = wiphy_priv(wiphy);
 	*(unsigned long *)wdev_priv = (unsigned long)adapter;
 
-	set_wiphy_dev(wiphy, (struct device *)priv->adapter->dev);
+	set_wiphy_dev(wiphy, priv->adapter->dev);
 
 	ret = wiphy_register(wiphy);
 	if (ret < 0) {
diff --git a/drivers/net/wireless/mwifiex/cfp.c b/drivers/net/wireless/mwifiex/cfp.c
index 560871b0e23..f69300f93f4 100644
--- a/drivers/net/wireless/mwifiex/cfp.c
+++ b/drivers/net/wireless/mwifiex/cfp.c
@@ -167,23 +167,6 @@ u32 mwifiex_index_to_data_rate(struct mwifiex_private *priv, u8 index,
 }
 
 /*
- * This function maps a data rate value into corresponding index in supported
- * rates table.
- */
-u8 mwifiex_data_rate_to_index(u32 rate)
-{
-	u16 *ptr;
-
-	if (rate) {
-		ptr = memchr(mwifiex_data_rates, rate,
-				sizeof(mwifiex_data_rates));
-		if (ptr)
-			return (u8) (ptr - mwifiex_data_rates);
-	}
-	return 0;
-}
-
-/*
  * This function returns the current active data rates.
  *
  * The result may vary depending upon connection status.
@@ -277,20 +260,6 @@ mwifiex_is_rate_auto(struct mwifiex_private *priv)
 }
 
 /*
- * This function converts rate bitmap into rate index.
- */
-int mwifiex_get_rate_index(u16 *rate_bitmap, int size)
-{
-	int i;
-
-	for (i = 0; i < size * 8; i++)
-		if (rate_bitmap[i / 16] & (1 << (i % 16)))
-			return i;
-
-	return 0;
-}
-
-/*
  * This function gets the supported data rates.
  *
  * The function works in both Ad-Hoc and infra mode by printing the
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 51e023ec1de..c68adec3cc8 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -578,6 +578,7 @@ int mwifiex_send_cmd_async(struct mwifiex_private *priv, uint16_t cmd_no,
 	} else {
 		adapter->cmd_queued = cmd_node;
 		mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
+		queue_work(adapter->workqueue, &adapter->main_work);
 	}
 
 	return ret;
@@ -1102,7 +1103,8 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
 		&resp->params.opt_hs_cfg;
 	uint32_t conditions = le32_to_cpu(phs_cfg->params.hs_config.conditions);
 
-	if (phs_cfg->action == cpu_to_le16(HS_ACTIVATE)) {
+	if (phs_cfg->action == cpu_to_le16(HS_ACTIVATE) &&
+	    adapter->iface_type == MWIFIEX_SDIO) {
 		mwifiex_hs_activated_event(priv, true);
 		return 0;
 	} else {
@@ -1114,6 +1116,9 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
 	}
 	if (conditions != HOST_SLEEP_CFG_CANCEL) {
 		adapter->is_hs_configured = true;
+		if (adapter->iface_type == MWIFIEX_USB ||
+		    adapter->iface_type == MWIFIEX_PCIE)
+			mwifiex_hs_activated_event(priv, true);
 	} else {
 		adapter->is_hs_configured = false;
 		if (adapter->hs_activated)
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index f918f66e5e2..070ef25f518 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -41,16 +41,7 @@
 #define MWIFIEX_AMPDU_DEF_RXWINSIZE        16
 #define MWIFIEX_DEFAULT_BLOCK_ACK_TIMEOUT  0xffff
 
-#define MWIFIEX_RATE_INDEX_HRDSSS0 0
-#define MWIFIEX_RATE_INDEX_HRDSSS3 3
-#define MWIFIEX_RATE_INDEX_OFDM0   4
-#define MWIFIEX_RATE_INDEX_OFDM7   11
-#define MWIFIEX_RATE_INDEX_MCS0    12
-
-#define MWIFIEX_RATE_BITMAP_OFDM0  16
-#define MWIFIEX_RATE_BITMAP_OFDM7  23
 #define MWIFIEX_RATE_BITMAP_MCS0   32
-#define MWIFIEX_RATE_BITMAP_MCS127 159
 
 #define MWIFIEX_RX_DATA_BUF_SIZE     (4 * 1024)
 #define MWIFIEX_RX_CMD_BUF_SIZE	     (2 * 1024)
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 561452a5c81..e831b440a24 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -124,6 +124,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_UAP_DTIM_PERIOD    (PROPRIETARY_TLV_BASE_ID + 45)
 #define TLV_TYPE_UAP_BCAST_SSID     (PROPRIETARY_TLV_BASE_ID + 48)
 #define TLV_TYPE_UAP_RTS_THRESHOLD  (PROPRIETARY_TLV_BASE_ID + 51)
+#define TLV_TYPE_UAP_WEP_KEY        (PROPRIETARY_TLV_BASE_ID + 59)
 #define TLV_TYPE_UAP_WPA_PASSPHRASE (PROPRIETARY_TLV_BASE_ID + 60)
 #define TLV_TYPE_UAP_ENCRY_PROTOCOL (PROPRIETARY_TLV_BASE_ID + 64)
 #define TLV_TYPE_UAP_AKMP           (PROPRIETARY_TLV_BASE_ID + 65)
@@ -162,6 +163,12 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define ISSUPP_11NENABLED(FwCapInfo) (FwCapInfo & BIT(11))
 
+#define MWIFIEX_DEF_HT_CAP	(IEEE80211_HT_CAP_DSSSCCK40 | \
+				 (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT) | \
+				 IEEE80211_HT_CAP_SM_PS)
+
+#define MWIFIEX_DEF_AMPDU	IEEE80211_HT_AMPDU_PARM_FACTOR
+
 /* dev_cap bitmap
  * BIT
  * 0-16		reserved
@@ -218,7 +225,8 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_BBP_REG_ACCESS                    0x001a
 #define HostCmd_CMD_RF_REG_ACCESS                     0x001b
 #define HostCmd_CMD_PMIC_REG_ACCESS                   0x00ad
-#define HostCmd_CMD_802_11_RF_CHANNEL                 0x001d
+#define HostCmd_CMD_RF_TX_PWR                         0x001e
+#define HostCmd_CMD_RF_ANTENNA                        0x0020
 #define HostCmd_CMD_802_11_DEAUTHENTICATE             0x0024
 #define HostCmd_CMD_MAC_CONTROL                       0x0028
 #define HostCmd_CMD_802_11_AD_HOC_START               0x002b
@@ -314,6 +322,12 @@ enum ENH_PS_MODES {
 
 #define HostCmd_BSS_TYPE_MASK           0xf000
 
+#define HostCmd_ACT_SET_RX              0x0001
+#define HostCmd_ACT_SET_TX              0x0002
+#define HostCmd_ACT_SET_BOTH            0x0003
+
+#define RF_ANTENNA_AUTO                 0xFFFF
+
 #define HostCmd_SET_SEQ_NO_BSS_INFO(seq, num, type) {   \
 	(((seq) & 0x00ff) |                             \
 	 (((num) & 0x000f) << 8)) |                     \
@@ -869,6 +883,25 @@ struct host_cmd_ds_txpwr_cfg {
 	__le32 mode;
 } __packed;
 
+struct host_cmd_ds_rf_tx_pwr {
+	__le16 action;
+	__le16 cur_level;
+	u8 max_power;
+	u8 min_power;
+} __packed;
+
+struct host_cmd_ds_rf_ant_mimo {
+	__le16 action_tx;
+	__le16 tx_ant_mode;
+	__le16 action_rx;
+	__le16 rx_ant_mode;
+};
+
+struct host_cmd_ds_rf_ant_siso {
+	__le16 action;
+	__le16 ant_mode;
+};
+
 struct mwifiex_bcn_param {
 	u8 bssid[ETH_ALEN];
 	u8 rssi;
@@ -1195,6 +1228,13 @@ struct host_cmd_tlv_passphrase {
 	u8 passphrase[0];
 } __packed;
 
+struct host_cmd_tlv_wep_key {
+	struct host_cmd_tlv tlv;
+	u8 key_index;
+	u8 is_default;
+	u8 key[1];
+};
+
 struct host_cmd_tlv_auth_type {
 	struct host_cmd_tlv tlv;
 	u8 auth_type;
@@ -1251,14 +1291,6 @@ struct host_cmd_tlv_channel_band {
 	u8 channel;
 } __packed;
 
-struct host_cmd_ds_802_11_rf_channel {
-	__le16 action;
-	__le16 current_channel;
-	__le16 rf_type;
-	__le16 reserved;
-	u8 reserved_1[32];
-} __packed;
-
 struct host_cmd_ds_version_ext {
 	u8 version_str_sel;
 	char version_str[128];
@@ -1343,10 +1375,12 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_802_11_rssi_info rssi_info;
 		struct host_cmd_ds_802_11_rssi_info_rsp rssi_info_rsp;
 		struct host_cmd_ds_802_11_snmp_mib smib;
-		struct host_cmd_ds_802_11_rf_channel rf_channel;
 		struct host_cmd_ds_tx_rate_query tx_rate;
 		struct host_cmd_ds_tx_rate_cfg tx_rate_cfg;
 		struct host_cmd_ds_txpwr_cfg txp_cfg;
+		struct host_cmd_ds_rf_tx_pwr txp;
+		struct host_cmd_ds_rf_ant_mimo ant_mimo;
+		struct host_cmd_ds_rf_ant_siso ant_siso;
 		struct host_cmd_ds_802_11_ps_mode_enh psmode_enh;
 		struct host_cmd_ds_802_11_hs_cfg_enh opt_hs_cfg;
 		struct host_cmd_ds_802_11_scan scan;
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/mwifiex/ie.c
index 383820a52be..1d8dd003e39 100644
--- a/drivers/net/wireless/mwifiex/ie.c
+++ b/drivers/net/wireless/mwifiex/ie.c
@@ -51,8 +51,7 @@ mwifiex_ie_get_autoidx(struct mwifiex_private *priv, u16 subtype_mask,
 
 	for (i = 0; i < priv->adapter->max_mgmt_ie_index; i++) {
 		mask = le16_to_cpu(priv->mgmt_ie[i].mgmt_subtype_mask);
-		len = le16_to_cpu(priv->mgmt_ie[i].ie_length) +
-		      le16_to_cpu(ie->ie_length);
+		len = le16_to_cpu(ie->ie_length);
 
 		if (mask == MWIFIEX_AUTO_IDX_MASK)
 			continue;
@@ -108,10 +107,8 @@ mwifiex_update_autoindex_ies(struct mwifiex_private *priv,
 				return -1;
 
 			tmp = (u8 *)&priv->mgmt_ie[index].ie_buffer;
-			tmp += le16_to_cpu(priv->mgmt_ie[index].ie_length);
 			memcpy(tmp, &ie->ie_buffer, le16_to_cpu(ie->ie_length));
-			le16_add_cpu(&priv->mgmt_ie[index].ie_length,
-				     le16_to_cpu(ie->ie_length));
+			priv->mgmt_ie[index].ie_length = ie->ie_length;
 			priv->mgmt_ie[index].ie_index = cpu_to_le16(index);
 			priv->mgmt_ie[index].mgmt_subtype_mask =
 							cpu_to_le16(mask);
@@ -217,92 +214,63 @@ mwifiex_update_uap_custom_ie(struct mwifiex_private *priv,
 	return ret;
 }
 
-/* This function parses different IEs- Tail IEs, beacon IEs, probe response IEs,
- * association response IEs from cfg80211_ap_settings function and sets these IE
- * to FW.
+/* This function checks if WPS IE is present in passed buffer and copies it to
+ * mwifiex_ie structure.
+ * Function takes pointer to struct mwifiex_ie pointer as argument.
+ * If WPS IE is present memory is allocated for mwifiex_ie pointer and filled
+ * in with WPS IE. Caller should take care of freeing this memory.
  */
-int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
-			 struct cfg80211_ap_settings *params)
+static int mwifiex_update_wps_ie(const u8 *ies, int ies_len,
+				 struct mwifiex_ie **ie_ptr, u16 mask)
 {
-	struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL;
-	struct mwifiex_ie *ar_ie = NULL, *rsn_ie = NULL;
-	struct ieee_types_header *ie = NULL;
-	u16 beacon_idx = MWIFIEX_AUTO_IDX_MASK, pr_idx = MWIFIEX_AUTO_IDX_MASK;
-	u16 ar_idx = MWIFIEX_AUTO_IDX_MASK, rsn_idx = MWIFIEX_AUTO_IDX_MASK;
-	u16 mask;
-	int ret = 0;
-
-	if (params->beacon.tail && params->beacon.tail_len) {
-		ie = (void *)cfg80211_find_ie(WLAN_EID_RSN, params->beacon.tail,
-					      params->beacon.tail_len);
-		if (ie) {
-			rsn_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
-			if (!rsn_ie)
-				return -ENOMEM;
-
-			rsn_ie->ie_index = cpu_to_le16(rsn_idx);
-			mask = MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
-			       MGMT_MASK_ASSOC_RESP;
-			rsn_ie->mgmt_subtype_mask = cpu_to_le16(mask);
-			rsn_ie->ie_length = cpu_to_le16(ie->len + 2);
-			memcpy(rsn_ie->ie_buffer, ie, ie->len + 2);
-
-			if (mwifiex_update_uap_custom_ie(priv, rsn_ie, &rsn_idx,
-							 NULL, NULL,
-							 NULL, NULL)) {
-				ret = -1;
-				goto done;
-			}
+	struct ieee_types_header *wps_ie;
+	struct mwifiex_ie *ie = NULL;
+	const u8 *vendor_ie;
+
+	vendor_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+					    WLAN_OUI_TYPE_MICROSOFT_WPS,
+					    ies, ies_len);
+	if (vendor_ie) {
+		ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!ie)
+			return -ENOMEM;
 
-			priv->rsn_idx = rsn_idx;
-		}
+		wps_ie = (struct ieee_types_header *)vendor_ie;
+		memcpy(ie->ie_buffer, wps_ie, wps_ie->len + 2);
+		ie->ie_length = cpu_to_le16(wps_ie->len + 2);
+		ie->mgmt_subtype_mask = cpu_to_le16(mask);
+		ie->ie_index = cpu_to_le16(MWIFIEX_AUTO_IDX_MASK);
 	}
 
-	if (params->beacon.beacon_ies && params->beacon.beacon_ies_len) {
-		beacon_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
-		if (!beacon_ie) {
-			ret = -ENOMEM;
-			goto done;
-		}
-
-		beacon_ie->ie_index = cpu_to_le16(beacon_idx);
-		beacon_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON);
-		beacon_ie->ie_length =
-				cpu_to_le16(params->beacon.beacon_ies_len);
-		memcpy(beacon_ie->ie_buffer, params->beacon.beacon_ies,
-		       params->beacon.beacon_ies_len);
-	}
+	*ie_ptr = ie;
+	return 0;
+}
 
-	if (params->beacon.proberesp_ies && params->beacon.proberesp_ies_len) {
-		pr_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
-		if (!pr_ie) {
-			ret = -ENOMEM;
-			goto done;
-		}
+/* This function parses beacon IEs, probe response IEs, association response IEs
+ * from cfg80211_ap_settings->beacon and sets these IE to FW.
+ */
+static int mwifiex_set_mgmt_beacon_data_ies(struct mwifiex_private *priv,
+					    struct cfg80211_beacon_data *data)
+{
+	struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL, *ar_ie = NULL;
+	u16 beacon_idx = MWIFIEX_AUTO_IDX_MASK, pr_idx = MWIFIEX_AUTO_IDX_MASK;
+	u16 ar_idx = MWIFIEX_AUTO_IDX_MASK;
+	int ret = 0;
 
-		pr_ie->ie_index = cpu_to_le16(pr_idx);
-		pr_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_PROBE_RESP);
-		pr_ie->ie_length =
-				cpu_to_le16(params->beacon.proberesp_ies_len);
-		memcpy(pr_ie->ie_buffer, params->beacon.proberesp_ies,
-		       params->beacon.proberesp_ies_len);
-	}
+	if (data->beacon_ies && data->beacon_ies_len)
+		mwifiex_update_wps_ie(data->beacon_ies, data->beacon_ies_len,
+				      &beacon_ie, MGMT_MASK_BEACON);
 
-	if (params->beacon.assocresp_ies && params->beacon.assocresp_ies_len) {
-		ar_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
-		if (!ar_ie) {
-			ret = -ENOMEM;
-			goto done;
-		}
+	if (data->proberesp_ies && data->proberesp_ies_len)
+		mwifiex_update_wps_ie(data->proberesp_ies,
+				      data->proberesp_ies_len, &pr_ie,
+				      MGMT_MASK_PROBE_RESP);
 
-		ar_ie->ie_index = cpu_to_le16(ar_idx);
-		mask = MGMT_MASK_ASSOC_RESP | MGMT_MASK_REASSOC_RESP;
-		ar_ie->mgmt_subtype_mask = cpu_to_le16(mask);
-		ar_ie->ie_length =
-				cpu_to_le16(params->beacon.assocresp_ies_len);
-		memcpy(ar_ie->ie_buffer, params->beacon.assocresp_ies,
-		       params->beacon.assocresp_ies_len);
-	}
+	if (data->assocresp_ies && data->assocresp_ies_len)
+		mwifiex_update_wps_ie(data->assocresp_ies,
+				      data->assocresp_ies_len, &ar_ie,
+				      MGMT_MASK_ASSOC_RESP |
+				      MGMT_MASK_REASSOC_RESP);
 
 	if (beacon_ie || pr_ie || ar_ie) {
 		ret = mwifiex_update_uap_custom_ie(priv, beacon_ie,
@@ -320,11 +288,67 @@ done:
 	kfree(beacon_ie);
 	kfree(pr_ie);
 	kfree(ar_ie);
-	kfree(rsn_ie);
 
 	return ret;
 }
 
+/* This function parses different IEs-tail IEs, beacon IEs, probe response IEs,
+ * association response IEs from cfg80211_ap_settings function and sets these IE
+ * to FW.
+ */
+int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
+			 struct cfg80211_beacon_data *info)
+{
+	struct mwifiex_ie *gen_ie;
+	struct ieee_types_header *rsn_ie, *wpa_ie = NULL;
+	u16 rsn_idx = MWIFIEX_AUTO_IDX_MASK, ie_len = 0;
+	const u8 *vendor_ie;
+
+	if (info->tail && info->tail_len) {
+		gen_ie = kzalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+		if (!gen_ie)
+			return -ENOMEM;
+		gen_ie->ie_index = cpu_to_le16(rsn_idx);
+		gen_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON |
+							MGMT_MASK_PROBE_RESP |
+							MGMT_MASK_ASSOC_RESP);
+
+		rsn_ie = (void *)cfg80211_find_ie(WLAN_EID_RSN,
+						  info->tail, info->tail_len);
+		if (rsn_ie) {
+			memcpy(gen_ie->ie_buffer, rsn_ie, rsn_ie->len + 2);
+			ie_len = rsn_ie->len + 2;
+			gen_ie->ie_length = cpu_to_le16(ie_len);
+		}
+
+		vendor_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+						    WLAN_OUI_TYPE_MICROSOFT_WPA,
+						    info->tail,
+						    info->tail_len);
+		if (vendor_ie) {
+			wpa_ie = (struct ieee_types_header *)vendor_ie;
+			memcpy(gen_ie->ie_buffer + ie_len,
+			       wpa_ie, wpa_ie->len + 2);
+			ie_len += wpa_ie->len + 2;
+			gen_ie->ie_length = cpu_to_le16(ie_len);
+		}
+
+		if (rsn_ie || wpa_ie) {
+			if (mwifiex_update_uap_custom_ie(priv, gen_ie, &rsn_idx,
+							 NULL, NULL,
+							 NULL, NULL)) {
+				kfree(gen_ie);
+				return -1;
+			}
+			priv->rsn_idx = rsn_idx;
+		}
+
+		kfree(gen_ie);
+	}
+
+	return mwifiex_set_mgmt_beacon_data_ies(priv, info);
+}
+
 /* This function removes management IE set */
 int mwifiex_del_mgmt_ies(struct mwifiex_private *priv)
 {
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index c1cb004db91..21fdc6c0277 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -57,6 +57,69 @@ static int mwifiex_add_bss_prio_tbl(struct mwifiex_private *priv)
 	return 0;
 }
 
+static void scan_delay_timer_fn(unsigned long data)
+{
+	struct mwifiex_private *priv = (struct mwifiex_private *)data;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct cmd_ctrl_node *cmd_node, *tmp_node;
+	unsigned long flags;
+
+	if (!mwifiex_wmm_lists_empty(adapter)) {
+		if (adapter->scan_delay_cnt == MWIFIEX_MAX_SCAN_DELAY_CNT) {
+			/*
+			 * Abort scan operation by cancelling all pending scan
+			 * command
+			 */
+			spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
+			list_for_each_entry_safe(cmd_node, tmp_node,
+						 &adapter->scan_pending_q,
+						 list) {
+				list_del(&cmd_node->list);
+				cmd_node->wait_q_enabled = false;
+				mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+			}
+			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+					       flags);
+
+			spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+			adapter->scan_processing = false;
+			spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock,
+					       flags);
+
+			if (priv->user_scan_cfg) {
+				dev_dbg(priv->adapter->dev,
+					"info: %s: scan aborted\n", __func__);
+				cfg80211_scan_done(priv->scan_request, 1);
+				priv->scan_request = NULL;
+				kfree(priv->user_scan_cfg);
+				priv->user_scan_cfg = NULL;
+			}
+		} else {
+			/*
+			 * Tx data queue is still not empty, delay scan
+			 * operation further by 20msec.
+			 */
+			mod_timer(&priv->scan_delay_timer, jiffies +
+				  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
+			adapter->scan_delay_cnt++;
+		}
+		queue_work(priv->adapter->workqueue, &priv->adapter->main_work);
+	} else {
+		/*
+		 * Tx data queue is empty. Get scan command from scan_pending_q
+		 * and put to cmd_pending_q to resume scan operation
+		 */
+		adapter->scan_delay_cnt = 0;
+		spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
+		cmd_node = list_first_entry(&adapter->scan_pending_q,
+					    struct cmd_ctrl_node, list);
+		list_del(&cmd_node->list);
+		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
+
+		mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
+	}
+}
+
 /*
  * This function initializes the private structure and sets default
  * values to the members.
@@ -136,6 +199,9 @@ static int mwifiex_init_priv(struct mwifiex_private *priv)
 
 	priv->scan_block = false;
 
+	setup_timer(&priv->scan_delay_timer, scan_delay_timer_fn,
+		    (unsigned long)priv);
+
 	return mwifiex_add_bss_prio_tbl(priv);
 }
 
@@ -278,7 +344,6 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	adapter->adhoc_awake_period = 0;
 	memset(&adapter->arp_filter, 0, sizeof(adapter->arp_filter));
 	adapter->arp_filter_size = 0;
-	adapter->channel_type = NL80211_CHAN_HT20;
 	adapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
 }
 
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index e6be6ee7595..50191539bb3 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -21,6 +21,7 @@
 #define _MWIFIEX_IOCTL_H_
 
 #include <net/mac80211.h>
+#include <net/lib80211.h>
 
 enum {
 	MWIFIEX_SCAN_TYPE_UNCHANGED = 0,
@@ -71,6 +72,13 @@ struct wpa_param {
 	u8 passphrase[MWIFIEX_WPA_PASSHPHRASE_LEN];
 };
 
+struct wep_key {
+	u8 key_index;
+	u8 is_default;
+	u16 length;
+	u8 key[WLAN_KEY_LEN_WEP104];
+};
+
 #define KEY_MGMT_ON_HOST        0x03
 #define MWIFIEX_AUTH_MODE_AUTO  0xFF
 #define BAND_CONFIG_MANUAL      0x00
@@ -90,6 +98,8 @@ struct mwifiex_uap_bss_param {
 	u16 key_mgmt;
 	u16 key_mgmt_operation;
 	struct wpa_param wpa_cfg;
+	struct wep_key wep_cfg[NUM_WEP_KEYS];
+	struct ieee80211_ht_cap ht_cap;
 };
 
 enum {
@@ -215,12 +225,6 @@ struct mwifiex_ds_encrypt_key {
 	u8 wapi_rxpn[WAPI_RXPN_LEN];
 };
 
-struct mwifiex_rate_cfg {
-	u32 action;
-	u32 is_rate_auto;
-	u32 rate;
-};
-
 struct mwifiex_power_cfg {
 	u32 is_power_auto;
 	u32 power_level;
@@ -267,6 +271,11 @@ struct mwifiex_ds_11n_amsdu_aggr_ctrl {
 	u16 curr_buf_size;
 };
 
+struct mwifiex_ds_ant_cfg {
+	u32 tx_ant;
+	u32 rx_ant;
+};
+
 #define MWIFIEX_NUM_OF_CMD_BUFFER	20
 #define MWIFIEX_SIZE_OF_CMD_BUFFER	2048
 
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index d6b4fb04011..82e63cee1e9 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -1349,22 +1349,16 @@ static int mwifiex_deauthenticate_infra(struct mwifiex_private *priv, u8 *mac)
 {
 	u8 mac_address[ETH_ALEN];
 	int ret;
-	u8 zero_mac[ETH_ALEN] = { 0, 0, 0, 0, 0, 0 };
 
-	if (mac) {
-		if (!memcmp(mac, zero_mac, sizeof(zero_mac)))
-			memcpy((u8 *) &mac_address,
-			       (u8 *) &priv->curr_bss_params.bss_descriptor.
-			       mac_address, ETH_ALEN);
-		else
-			memcpy((u8 *) &mac_address, (u8 *) mac, ETH_ALEN);
-	} else {
-		memcpy((u8 *) &mac_address, (u8 *) &priv->curr_bss_params.
-		       bss_descriptor.mac_address, ETH_ALEN);
-	}
+	if (!mac || is_zero_ether_addr(mac))
+		memcpy(mac_address,
+		       priv->curr_bss_params.bss_descriptor.mac_address,
+		       ETH_ALEN);
+	else
+		memcpy(mac_address, mac, ETH_ALEN);
 
 	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_DEAUTHENTICATE,
-				    HostCmd_ACT_GEN_SET, 0, &mac_address);
+				    HostCmd_ACT_GEN_SET, 0, mac_address);
 
 	return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 3192855c31c..46803621d01 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -190,7 +190,8 @@ process_start:
 			    adapter->tx_lock_flag)
 				break;
 
-			if (adapter->scan_processing || adapter->data_sent ||
+			if ((adapter->scan_processing &&
+			     !adapter->scan_delay_cnt) || adapter->data_sent ||
 			    mwifiex_wmm_lists_empty(adapter)) {
 				if (adapter->cmd_sent || adapter->curr_cmd ||
 				    (!is_command_pending(adapter)))
@@ -244,8 +245,8 @@ process_start:
 			}
 		}
 
-		if (!adapter->scan_processing && !adapter->data_sent &&
-		    !mwifiex_wmm_lists_empty(adapter)) {
+		if ((!adapter->scan_processing || adapter->scan_delay_cnt) &&
+		    !adapter->data_sent && !mwifiex_wmm_lists_empty(adapter)) {
 			mwifiex_wmm_process_tx(adapter);
 			if (adapter->hs_activated) {
 				adapter->is_hs_configured = false;
@@ -376,7 +377,7 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	goto done;
 
 err_add_intf:
-	mwifiex_del_virtual_intf(adapter->wiphy, priv->netdev);
+	mwifiex_del_virtual_intf(adapter->wiphy, priv->wdev);
 	rtnl_unlock();
 err_init_fw:
 	pr_debug("info: %s: unregister device\n", __func__);
@@ -843,7 +844,7 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 
 		rtnl_lock();
 		if (priv->wdev && priv->netdev)
-			mwifiex_del_virtual_intf(adapter->wiphy, priv->netdev);
+			mwifiex_del_virtual_intf(adapter->wiphy, priv->wdev);
 		rtnl_unlock();
 	}
 
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index bd3b0bf94b9..e7c2a82fd61 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -79,14 +79,17 @@ enum {
 
 #define SCAN_BEACON_ENTRY_PAD			6
 
-#define MWIFIEX_PASSIVE_SCAN_CHAN_TIME	200
-#define MWIFIEX_ACTIVE_SCAN_CHAN_TIME	200
-#define MWIFIEX_SPECIFIC_SCAN_CHAN_TIME	110
+#define MWIFIEX_PASSIVE_SCAN_CHAN_TIME	110
+#define MWIFIEX_ACTIVE_SCAN_CHAN_TIME	30
+#define MWIFIEX_SPECIFIC_SCAN_CHAN_TIME	30
 
 #define SCAN_RSSI(RSSI)					(0x100 - ((u8)(RSSI)))
 
 #define MWIFIEX_MAX_TOTAL_SCAN_TIME	(MWIFIEX_TIMER_10S - MWIFIEX_TIMER_1S)
 
+#define MWIFIEX_MAX_SCAN_DELAY_CNT			50
+#define MWIFIEX_SCAN_DELAY_MSEC				20
+
 #define RSN_GTK_OUI_OFFSET				2
 
 #define MWIFIEX_OUI_NOT_PRESENT			0
@@ -482,6 +485,7 @@ struct mwifiex_private {
 	u16 proberesp_idx;
 	u16 assocresp_idx;
 	u16 rsn_idx;
+	struct timer_list scan_delay_timer;
 };
 
 enum mwifiex_ba_status {
@@ -674,7 +678,6 @@ struct mwifiex_adapter {
 	u8 hw_dev_mcs_support;
 	u8 adhoc_11n_enabled;
 	u8 sec_chan_offset;
-	enum nl80211_channel_type channel_type;
 	struct mwifiex_dbg dbg;
 	u8 arp_filter[ARP_FILTER_MAX_BUF_SIZE];
 	u32 arp_filter_size;
@@ -686,6 +689,7 @@ struct mwifiex_adapter {
 	struct completion fw_load;
 	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
 	u16 max_mgmt_ie_index;
+	u8 scan_delay_cnt;
 };
 
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
@@ -819,9 +823,7 @@ int mwifiex_cmd_append_vsie_tlv(struct mwifiex_private *priv, u16 vsie_mask,
 u32 mwifiex_get_active_data_rates(struct mwifiex_private *priv,
 				    u8 *rates);
 u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates);
-u8 mwifiex_data_rate_to_index(u32 rate);
 u8 mwifiex_is_rate_auto(struct mwifiex_private *priv);
-int mwifiex_get_rate_index(u16 *rateBitmap, int size);
 extern u16 region_code_index[MWIFIEX_MAX_REGION_CODE];
 void mwifiex_save_curr_bcn(struct mwifiex_private *priv);
 void mwifiex_free_curr_bcn(struct mwifiex_private *priv);
@@ -835,6 +837,9 @@ void mwifiex_init_priv_params(struct mwifiex_private *priv,
 int mwifiex_set_secure_params(struct mwifiex_private *priv,
 			      struct mwifiex_uap_bss_param *bss_config,
 			      struct cfg80211_ap_settings *params);
+void mwifiex_set_ht_params(struct mwifiex_private *priv,
+			   struct mwifiex_uap_bss_param *bss_cfg,
+			   struct cfg80211_ap_settings *params);
 
 /*
  * This function checks if the queuing is RA based or not.
@@ -937,16 +942,13 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 int mwifiex_cancel_hs(struct mwifiex_private *priv, int cmd_type);
 int mwifiex_enable_hs(struct mwifiex_adapter *adapter);
 int mwifiex_disable_auto_ds(struct mwifiex_private *priv);
-int mwifiex_drv_get_data_rate(struct mwifiex_private *priv,
-			      struct mwifiex_rate_cfg *rate);
+int mwifiex_drv_get_data_rate(struct mwifiex_private *priv, u32 *rate);
 int mwifiex_request_scan(struct mwifiex_private *priv,
 			 struct cfg80211_ssid *req_ssid);
-int mwifiex_set_user_scan_ioctl(struct mwifiex_private *priv,
-				struct mwifiex_user_scan_cfg *scan_req);
+int mwifiex_scan_networks(struct mwifiex_private *priv,
+			  const struct mwifiex_user_scan_cfg *user_scan_in);
 int mwifiex_set_radio(struct mwifiex_private *priv, u8 option);
 
-int mwifiex_drv_change_adhoc_chan(struct mwifiex_private *priv, u16 channel);
-
 int mwifiex_set_encode(struct mwifiex_private *priv, const u8 *key,
 		       int key_len, u8 key_index, const u8 *mac_addr,
 		       int disable);
@@ -985,9 +987,6 @@ int mwifiex_set_tx_power(struct mwifiex_private *priv,
 
 int mwifiex_main_process(struct mwifiex_adapter *);
 
-int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel);
-int mwifiex_bss_set_channel(struct mwifiex_private *,
-			    struct mwifiex_chan_freq_power *cfp);
 int mwifiex_get_bss_info(struct mwifiex_private *,
 			 struct mwifiex_bss_info *);
 int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
@@ -998,15 +997,17 @@ int mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
 int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
 					struct mwifiex_bssdescriptor *bss_desc);
 
-struct net_device *mwifiex_add_virtual_intf(struct wiphy *wiphy,
-					char *name, enum nl80211_iftype type,
-					u32 *flags, struct vif_params *params);
-int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev);
+struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
+					      char *name,
+					      enum nl80211_iftype type,
+					      u32 *flags,
+					      struct vif_params *params);
+int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev);
 
 void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config);
 
 int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
-			 struct cfg80211_ap_settings *params);
+			 struct cfg80211_beacon_data *data);
 int mwifiex_del_mgmt_ies(struct mwifiex_private *priv);
 u8 *mwifiex_11d_code_2_region(u8 code);
 
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index 74f04571572..04dc7ca4ac2 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -28,7 +28,10 @@
 /* The maximum number of channels the firmware can scan per command */
 #define MWIFIEX_MAX_CHANNELS_PER_SPECIFIC_SCAN   14
 
-#define MWIFIEX_CHANNELS_PER_SCAN_CMD            4
+#define MWIFIEX_DEF_CHANNELS_PER_SCAN_CMD	4
+#define MWIFIEX_LIMIT_1_CHANNEL_PER_SCAN_CMD	15
+#define MWIFIEX_LIMIT_2_CHANNELS_PER_SCAN_CMD	27
+#define MWIFIEX_LIMIT_3_CHANNELS_PER_SCAN_CMD	35
 
 /* Memory needed to store a max sized Channel List TLV for a firmware scan */
 #define CHAN_TLV_MAX_SIZE  (sizeof(struct mwifiex_ie_types_header)         \
@@ -471,7 +474,7 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
  * This routine is used for any scan that is not provided with a
  * specific channel list to scan.
  */
-static void
+static int
 mwifiex_scan_create_channel_list(struct mwifiex_private *priv,
 				 const struct mwifiex_user_scan_cfg
 							*user_scan_in,
@@ -528,6 +531,7 @@ mwifiex_scan_create_channel_list(struct mwifiex_private *priv,
 		}
 
 	}
+	return chan_idx;
 }
 
 /*
@@ -727,6 +731,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 	u32 num_probes;
 	u32 ssid_len;
 	u32 chan_idx;
+	u32 chan_num;
 	u32 scan_type;
 	u16 scan_dur;
 	u8 channel;
@@ -850,7 +855,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 	if (*filtered_scan)
 		*max_chan_per_scan = MWIFIEX_MAX_CHANNELS_PER_SPECIFIC_SCAN;
 	else
-		*max_chan_per_scan = MWIFIEX_CHANNELS_PER_SCAN_CMD;
+		*max_chan_per_scan = MWIFIEX_DEF_CHANNELS_PER_SCAN_CMD;
 
 	/* If the input config or adapter has the number of Probes set,
 	   add tlv */
@@ -962,13 +967,28 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 			dev_dbg(adapter->dev,
 				"info: Scan: Scanning current channel only\n");
 		}
-
+		chan_num = chan_idx;
 	} else {
 		dev_dbg(adapter->dev,
 			"info: Scan: Creating full region channel list\n");
-		mwifiex_scan_create_channel_list(priv, user_scan_in,
-						 scan_chan_list,
-						 *filtered_scan);
+		chan_num = mwifiex_scan_create_channel_list(priv, user_scan_in,
+							    scan_chan_list,
+							    *filtered_scan);
+	}
+
+	/*
+	 * In associated state we will reduce the number of channels scanned per
+	 * scan command to avoid any traffic delay/loss. This number is decided
+	 * based on total number of channels to be scanned due to constraints
+	 * of command buffers.
+	 */
+	if (priv->media_connected) {
+		if (chan_num < MWIFIEX_LIMIT_1_CHANNEL_PER_SCAN_CMD)
+			*max_chan_per_scan = 1;
+		else if (chan_num < MWIFIEX_LIMIT_2_CHANNELS_PER_SCAN_CMD)
+			*max_chan_per_scan = 2;
+		else if (chan_num < MWIFIEX_LIMIT_3_CHANNELS_PER_SCAN_CMD)
+			*max_chan_per_scan = 3;
 	}
 }
 
@@ -1014,14 +1034,12 @@ mwifiex_ret_802_11_scan_get_tlv_ptrs(struct mwifiex_adapter *adapter,
 			case TLV_TYPE_TSFTIMESTAMP:
 				dev_dbg(adapter->dev, "info: SCAN_RESP: TSF "
 					"timestamp TLV, len = %d\n", tlv_len);
-				*tlv_data = (struct mwifiex_ie_types_data *)
-					current_tlv;
+				*tlv_data = current_tlv;
 				break;
 			case TLV_TYPE_CHANNELBANDLIST:
 				dev_dbg(adapter->dev, "info: SCAN_RESP: channel"
 					" band list TLV, len = %d\n", tlv_len);
-				*tlv_data = (struct mwifiex_ie_types_data *)
-					current_tlv;
+				*tlv_data = current_tlv;
 				break;
 			default:
 				dev_err(adapter->dev,
@@ -1226,15 +1244,15 @@ int mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
 					bss_entry->beacon_buf);
 			break;
 		case WLAN_EID_BSS_COEX_2040:
-			bss_entry->bcn_bss_co_2040 = (u8 *) (current_ptr +
-					sizeof(struct ieee_types_header));
+			bss_entry->bcn_bss_co_2040 = current_ptr +
+				sizeof(struct ieee_types_header);
 			bss_entry->bss_co_2040_offset = (u16) (current_ptr +
 					sizeof(struct ieee_types_header) -
 						bss_entry->beacon_buf);
 			break;
 		case WLAN_EID_EXT_CAPABILITY:
-			bss_entry->bcn_ext_cap = (u8 *) (current_ptr +
-					sizeof(struct ieee_types_header));
+			bss_entry->bcn_ext_cap = current_ptr +
+				sizeof(struct ieee_types_header);
 			bss_entry->ext_cap_offset = (u16) (current_ptr +
 					sizeof(struct ieee_types_header) -
 					bss_entry->beacon_buf);
@@ -1276,8 +1294,8 @@ mwifiex_radio_type_to_band(u8 radio_type)
  * order to send the appropriate scan commands to firmware to populate or
  * update the internal driver scan table.
  */
-static int mwifiex_scan_networks(struct mwifiex_private *priv,
-		const struct mwifiex_user_scan_cfg *user_scan_in)
+int mwifiex_scan_networks(struct mwifiex_private *priv,
+			  const struct mwifiex_user_scan_cfg *user_scan_in)
 {
 	int ret = 0;
 	struct mwifiex_adapter *adapter = priv->adapter;
@@ -1342,6 +1360,7 @@ static int mwifiex_scan_networks(struct mwifiex_private *priv,
 			adapter->cmd_queued = cmd_node;
 			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
 							true);
+			queue_work(adapter->workqueue, &adapter->main_work);
 		} else {
 			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
 					       flags);
@@ -1358,26 +1377,6 @@ static int mwifiex_scan_networks(struct mwifiex_private *priv,
 }
 
 /*
- * Sends IOCTL request to start a scan with user configurations.
- *
- * This function allocates the IOCTL request buffer, fills it
- * with requisite parameters and calls the IOCTL handler.
- *
- * Upon completion, it also generates a wireless event to notify
- * applications.
- */
-int mwifiex_set_user_scan_ioctl(struct mwifiex_private *priv,
-				struct mwifiex_user_scan_cfg *scan_req)
-{
-	int status;
-
-	status = mwifiex_scan_networks(priv, scan_req);
-	queue_work(priv->adapter->workqueue, &priv->adapter->main_work);
-
-	return status;
-}
-
-/*
  * This function prepares a scan command to be sent to the firmware.
  *
  * This uses the scan command configuration sent to the command processing
@@ -1683,8 +1682,7 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 				goto done;
 			}
 			if (element_id == WLAN_EID_DS_PARAMS) {
-				channel = *(u8 *) (current_ptr +
-					sizeof(struct ieee_types_header));
+				channel = *(current_ptr + sizeof(struct ieee_types_header));
 				break;
 			}
 
@@ -1772,14 +1770,23 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 			priv->user_scan_cfg = NULL;
 		}
 	} else {
-		/* Get scan command from scan_pending_q and put to
-		   cmd_pending_q */
-		cmd_node = list_first_entry(&adapter->scan_pending_q,
-					    struct cmd_ctrl_node, list);
-		list_del(&cmd_node->list);
-		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
-
-		mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
+		if (!mwifiex_wmm_lists_empty(adapter)) {
+			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+					       flags);
+			adapter->scan_delay_cnt = 1;
+			mod_timer(&priv->scan_delay_timer, jiffies +
+				  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
+		} else {
+			/* Get scan command from scan_pending_q and put to
+			   cmd_pending_q */
+			cmd_node = list_first_entry(&adapter->scan_pending_q,
+						    struct cmd_ctrl_node, list);
+			list_del(&cmd_node->list);
+			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+					       flags);
+			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
+							true);
+		}
 	}
 
 done:
@@ -2010,12 +2017,11 @@ mwifiex_save_curr_bcn(struct mwifiex_private *priv)
 
 	if (curr_bss->bcn_bss_co_2040)
 		curr_bss->bcn_bss_co_2040 =
-			(u8 *) (curr_bss->beacon_buf +
-					curr_bss->bss_co_2040_offset);
+			(curr_bss->beacon_buf + curr_bss->bss_co_2040_offset);
 
 	if (curr_bss->bcn_ext_cap)
-		curr_bss->bcn_ext_cap = (u8 *) (curr_bss->beacon_buf +
-				curr_bss->ext_cap_offset);
+		curr_bss->bcn_ext_cap = curr_bss->beacon_buf +
+			curr_bss->ext_cap_offset;
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 40e025da6bc..df3a33c530c 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -260,6 +260,56 @@ static int mwifiex_cmd_tx_power_cfg(struct host_cmd_ds_command *cmd,
 }
 
 /*
+ * This function prepares command to get RF Tx power.
+ */
+static int mwifiex_cmd_rf_tx_power(struct mwifiex_private *priv,
+				   struct host_cmd_ds_command *cmd,
+				   u16 cmd_action, void *data_buf)
+{
+	struct host_cmd_ds_rf_tx_pwr *txp = &cmd->params.txp;
+
+	cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_rf_tx_pwr)
+				+ S_DS_GEN);
+	cmd->command = cpu_to_le16(HostCmd_CMD_RF_TX_PWR);
+	txp->action = cpu_to_le16(cmd_action);
+
+	return 0;
+}
+
+/*
+ * This function prepares command to set rf antenna.
+ */
+static int mwifiex_cmd_rf_antenna(struct mwifiex_private *priv,
+				  struct host_cmd_ds_command *cmd,
+				  u16 cmd_action,
+				  struct mwifiex_ds_ant_cfg *ant_cfg)
+{
+	struct host_cmd_ds_rf_ant_mimo *ant_mimo = &cmd->params.ant_mimo;
+	struct host_cmd_ds_rf_ant_siso *ant_siso = &cmd->params.ant_siso;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_RF_ANTENNA);
+
+	if (cmd_action != HostCmd_ACT_GEN_SET)
+		return 0;
+
+	if (priv->adapter->hw_dev_mcs_support == HT_STREAM_2X2) {
+		cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_rf_ant_mimo) +
+					S_DS_GEN);
+		ant_mimo->action_tx = cpu_to_le16(HostCmd_ACT_SET_TX);
+		ant_mimo->tx_ant_mode = cpu_to_le16((u16)ant_cfg->tx_ant);
+		ant_mimo->action_rx = cpu_to_le16(HostCmd_ACT_SET_RX);
+		ant_mimo->rx_ant_mode = cpu_to_le16((u16)ant_cfg->rx_ant);
+	} else {
+		cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_rf_ant_siso) +
+					S_DS_GEN);
+		ant_siso->action = cpu_to_le16(HostCmd_ACT_SET_BOTH);
+		ant_siso->ant_mode = cpu_to_le16((u16)ant_cfg->tx_ant);
+	}
+
+	return 0;
+}
+
+/*
  * This function prepares command to set Host Sleep configuration.
  *
  * Preparation includes -
@@ -695,40 +745,6 @@ static int mwifiex_cmd_802_11d_domain_info(struct mwifiex_private *priv,
 }
 
 /*
- * This function prepares command to set/get RF channel.
- *
- * Preparation includes -
- *      - Setting command ID, action and proper size
- *      - Setting RF type and current RF channel (for SET only)
- *      - Ensuring correct endian-ness
- */
-static int mwifiex_cmd_802_11_rf_channel(struct mwifiex_private *priv,
-					 struct host_cmd_ds_command *cmd,
-					 u16 cmd_action, u16 *channel)
-{
-	struct host_cmd_ds_802_11_rf_channel *rf_chan =
-		&cmd->params.rf_channel;
-	uint16_t rf_type = le16_to_cpu(rf_chan->rf_type);
-
-	cmd->command = cpu_to_le16(HostCmd_CMD_802_11_RF_CHANNEL);
-	cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_802_11_rf_channel)
-				+ S_DS_GEN);
-
-	if (cmd_action == HostCmd_ACT_GEN_SET) {
-		if ((priv->adapter->adhoc_start_band & BAND_A) ||
-		    (priv->adapter->adhoc_start_band & BAND_AN))
-			rf_chan->rf_type =
-				cpu_to_le16(HostCmd_SCAN_RADIO_TYPE_A);
-
-		rf_type = le16_to_cpu(rf_chan->rf_type);
-		SET_SECONDARYCHAN(rf_type, priv->adapter->sec_chan_offset);
-		rf_chan->current_channel = cpu_to_le16(*channel);
-	}
-	rf_chan->action = cpu_to_le16(cmd_action);
-	return 0;
-}
-
-/*
  * This function prepares command to set/get IBSS coalescing status.
  *
  * Preparation includes -
@@ -793,8 +809,7 @@ static int mwifiex_cmd_reg_access(struct host_cmd_ds_command *cmd,
 		struct host_cmd_ds_mac_reg_access *mac_reg;
 
 		cmd->size = cpu_to_le16(sizeof(*mac_reg) + S_DS_GEN);
-		mac_reg = (struct host_cmd_ds_mac_reg_access *) &cmd->
-								params.mac_reg;
+		mac_reg = &cmd->params.mac_reg;
 		mac_reg->action = cpu_to_le16(cmd_action);
 		mac_reg->offset =
 			cpu_to_le16((u16) le32_to_cpu(reg_rw->offset));
@@ -806,8 +821,7 @@ static int mwifiex_cmd_reg_access(struct host_cmd_ds_command *cmd,
 		struct host_cmd_ds_bbp_reg_access *bbp_reg;
 
 		cmd->size = cpu_to_le16(sizeof(*bbp_reg) + S_DS_GEN);
-		bbp_reg = (struct host_cmd_ds_bbp_reg_access *)
-							&cmd->params.bbp_reg;
+		bbp_reg = &cmd->params.bbp_reg;
 		bbp_reg->action = cpu_to_le16(cmd_action);
 		bbp_reg->offset =
 			cpu_to_le16((u16) le32_to_cpu(reg_rw->offset));
@@ -819,8 +833,7 @@ static int mwifiex_cmd_reg_access(struct host_cmd_ds_command *cmd,
 		struct host_cmd_ds_rf_reg_access *rf_reg;
 
 		cmd->size = cpu_to_le16(sizeof(*rf_reg) + S_DS_GEN);
-		rf_reg = (struct host_cmd_ds_rf_reg_access *)
-							&cmd->params.rf_reg;
+		rf_reg = &cmd->params.rf_reg;
 		rf_reg->action = cpu_to_le16(cmd_action);
 		rf_reg->offset = cpu_to_le16((u16) le32_to_cpu(reg_rw->offset));
 		rf_reg->value = (u8) le32_to_cpu(reg_rw->value);
@@ -831,8 +844,7 @@ static int mwifiex_cmd_reg_access(struct host_cmd_ds_command *cmd,
 		struct host_cmd_ds_pmic_reg_access *pmic_reg;
 
 		cmd->size = cpu_to_le16(sizeof(*pmic_reg) + S_DS_GEN);
-		pmic_reg = (struct host_cmd_ds_pmic_reg_access *) &cmd->
-				params.pmic_reg;
+		pmic_reg = &cmd->params.pmic_reg;
 		pmic_reg->action = cpu_to_le16(cmd_action);
 		pmic_reg->offset =
 				cpu_to_le16((u16) le32_to_cpu(reg_rw->offset));
@@ -844,8 +856,7 @@ static int mwifiex_cmd_reg_access(struct host_cmd_ds_command *cmd,
 		struct host_cmd_ds_rf_reg_access *cau_reg;
 
 		cmd->size = cpu_to_le16(sizeof(*cau_reg) + S_DS_GEN);
-		cau_reg = (struct host_cmd_ds_rf_reg_access *)
-							&cmd->params.rf_reg;
+		cau_reg = &cmd->params.rf_reg;
 		cau_reg->action = cpu_to_le16(cmd_action);
 		cau_reg->offset =
 				cpu_to_le16((u16) le32_to_cpu(reg_rw->offset));
@@ -856,7 +867,6 @@ static int mwifiex_cmd_reg_access(struct host_cmd_ds_command *cmd,
 	{
 		struct mwifiex_ds_read_eeprom *rd_eeprom = data_buf;
 		struct host_cmd_ds_802_11_eeprom_access *cmd_eeprom =
-			(struct host_cmd_ds_802_11_eeprom_access *)
 			&cmd->params.eeprom;
 
 		cmd->size = cpu_to_le16(sizeof(*cmd_eeprom) + S_DS_GEN);
@@ -1055,6 +1065,14 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 		ret = mwifiex_cmd_tx_power_cfg(cmd_ptr, cmd_action,
 					       data_buf);
 		break;
+	case HostCmd_CMD_RF_TX_PWR:
+		ret = mwifiex_cmd_rf_tx_power(priv, cmd_ptr, cmd_action,
+					      data_buf);
+		break;
+	case HostCmd_CMD_RF_ANTENNA:
+		ret = mwifiex_cmd_rf_antenna(priv, cmd_ptr, cmd_action,
+					     data_buf);
+		break;
 	case HostCmd_CMD_802_11_PS_MODE_ENH:
 		ret = mwifiex_cmd_enh_power_mode(priv, cmd_ptr, cmd_action,
 						 (uint16_t)cmd_oid, data_buf);
@@ -1117,10 +1135,6 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 				    S_DS_GEN);
 		ret = 0;
 		break;
-	case HostCmd_CMD_802_11_RF_CHANNEL:
-		ret = mwifiex_cmd_802_11_rf_channel(priv, cmd_ptr, cmd_action,
-						    data_buf);
-		break;
 	case HostCmd_CMD_FUNC_INIT:
 		if (priv->adapter->hw_status == MWIFIEX_HW_STATUS_RESET)
 			priv->adapter->hw_status = MWIFIEX_HW_STATUS_READY;
@@ -1283,7 +1297,7 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta)
 	priv->data_rate = 0;
 
 	/* get tx power */
-	ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_TXPWR_CFG,
+	ret = mwifiex_send_cmd_async(priv, HostCmd_CMD_RF_TX_PWR,
 				     HostCmd_ACT_GEN_GET, 0, NULL);
 	if (ret)
 		return -1;
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index a79ed9bd969..0b09004ebb2 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -227,7 +227,7 @@ static int mwifiex_ret_get_log(struct mwifiex_private *priv,
 			       struct mwifiex_ds_get_stats *stats)
 {
 	struct host_cmd_ds_802_11_get_log *get_log =
-		(struct host_cmd_ds_802_11_get_log *) &resp->params.get_log;
+		&resp->params.get_log;
 
 	if (stats) {
 		stats->mcast_tx_frame = le32_to_cpu(get_log->mcast_tx_frame);
@@ -267,12 +267,10 @@ static int mwifiex_ret_get_log(struct mwifiex_private *priv,
  *
  * Based on the new rate bitmaps, the function re-evaluates if
  * auto data rate has been activated. If not, it sends another
- * query to the firmware to get the current Tx data rate and updates
- * the driver value.
+ * query to the firmware to get the current Tx data rate.
  */
 static int mwifiex_ret_tx_rate_cfg(struct mwifiex_private *priv,
-				   struct host_cmd_ds_command *resp,
-				   struct mwifiex_rate_cfg *ds_rate)
+				   struct host_cmd_ds_command *resp)
 {
 	struct host_cmd_ds_tx_rate_cfg *rate_cfg = &resp->params.tx_rate_cfg;
 	struct mwifiex_rate_scope *rate_scope;
@@ -280,9 +278,8 @@ static int mwifiex_ret_tx_rate_cfg(struct mwifiex_private *priv,
 	u16 tlv, tlv_buf_len;
 	u8 *tlv_buf;
 	u32 i;
-	int ret = 0;
 
-	tlv_buf = (u8 *) ((u8 *) rate_cfg) +
+	tlv_buf = ((u8 *)rate_cfg) +
 			sizeof(struct host_cmd_ds_tx_rate_cfg);
 	tlv_buf_len = *(u16 *) (tlv_buf + sizeof(u16));
 
@@ -318,33 +315,11 @@ static int mwifiex_ret_tx_rate_cfg(struct mwifiex_private *priv,
 	if (priv->is_data_rate_auto)
 		priv->data_rate = 0;
 	else
-		ret = mwifiex_send_cmd_async(priv,
-					  HostCmd_CMD_802_11_TX_RATE_QUERY,
-					  HostCmd_ACT_GEN_GET, 0, NULL);
-
-	if (!ds_rate)
-		return ret;
-
-	if (le16_to_cpu(rate_cfg->action) == HostCmd_ACT_GEN_GET) {
-		if (priv->is_data_rate_auto) {
-			ds_rate->is_rate_auto = 1;
-		return ret;
-	}
-	ds_rate->rate = mwifiex_get_rate_index(priv->bitmap_rates,
-					       sizeof(priv->bitmap_rates));
-
-	if (ds_rate->rate >= MWIFIEX_RATE_BITMAP_OFDM0 &&
-	    ds_rate->rate <= MWIFIEX_RATE_BITMAP_OFDM7)
-		ds_rate->rate -= (MWIFIEX_RATE_BITMAP_OFDM0 -
-				  MWIFIEX_RATE_INDEX_OFDM0);
-
-	if (ds_rate->rate >= MWIFIEX_RATE_BITMAP_MCS0 &&
-	    ds_rate->rate <= MWIFIEX_RATE_BITMAP_MCS127)
-		ds_rate->rate -= (MWIFIEX_RATE_BITMAP_MCS0 -
-				  MWIFIEX_RATE_INDEX_MCS0);
-	}
+		return mwifiex_send_cmd_async(priv,
+					      HostCmd_CMD_802_11_TX_RATE_QUERY,
+					      HostCmd_ACT_GEN_GET, 0, NULL);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -451,6 +426,57 @@ static int mwifiex_ret_tx_power_cfg(struct mwifiex_private *priv,
 }
 
 /*
+ * This function handles the command response of get RF Tx power.
+ */
+static int mwifiex_ret_rf_tx_power(struct mwifiex_private *priv,
+				   struct host_cmd_ds_command *resp)
+{
+	struct host_cmd_ds_rf_tx_pwr *txp = &resp->params.txp;
+	u16 action = le16_to_cpu(txp->action);
+
+	priv->tx_power_level = le16_to_cpu(txp->cur_level);
+
+	if (action == HostCmd_ACT_GEN_GET) {
+		priv->max_tx_power_level = txp->max_power;
+		priv->min_tx_power_level = txp->min_power;
+	}
+
+	dev_dbg(priv->adapter->dev,
+		"Current TxPower Level=%d, Max Power=%d, Min Power=%d\n",
+		priv->tx_power_level, priv->max_tx_power_level,
+		priv->min_tx_power_level);
+
+	return 0;
+}
+
+/*
+ * This function handles the command response of set rf antenna
+ */
+static int mwifiex_ret_rf_antenna(struct mwifiex_private *priv,
+				  struct host_cmd_ds_command *resp)
+{
+	struct host_cmd_ds_rf_ant_mimo *ant_mimo = &resp->params.ant_mimo;
+	struct host_cmd_ds_rf_ant_siso *ant_siso = &resp->params.ant_siso;
+	struct mwifiex_adapter *adapter = priv->adapter;
+
+	if (adapter->hw_dev_mcs_support == HT_STREAM_2X2)
+		dev_dbg(adapter->dev,
+			"RF_ANT_RESP: Tx action = 0x%x, Tx Mode = 0x%04x"
+			" Rx action = 0x%x, Rx Mode = 0x%04x\n",
+			le16_to_cpu(ant_mimo->action_tx),
+			le16_to_cpu(ant_mimo->tx_ant_mode),
+			le16_to_cpu(ant_mimo->action_rx),
+			le16_to_cpu(ant_mimo->rx_ant_mode));
+	else
+		dev_dbg(adapter->dev,
+			"RF_ANT_RESP: action = 0x%x, Mode = 0x%04x\n",
+			le16_to_cpu(ant_siso->action),
+			le16_to_cpu(ant_siso->ant_mode));
+
+	return 0;
+}
+
+/*
  * This function handles the command response of set/get MAC address.
  *
  * Handling includes saving the MAC address in driver.
@@ -605,34 +631,6 @@ static int mwifiex_ret_802_11d_domain_info(struct mwifiex_private *priv,
 }
 
 /*
- * This function handles the command response of get RF channel.
- *
- * Handling includes changing the header fields into CPU format
- * and saving the new channel in driver.
- */
-static int mwifiex_ret_802_11_rf_channel(struct mwifiex_private *priv,
-					 struct host_cmd_ds_command *resp,
-					 u16 *data_buf)
-{
-	struct host_cmd_ds_802_11_rf_channel *rf_channel =
-		&resp->params.rf_channel;
-	u16 new_channel = le16_to_cpu(rf_channel->current_channel);
-
-	if (priv->curr_bss_params.bss_descriptor.channel != new_channel) {
-		dev_dbg(priv->adapter->dev, "cmd: Channel Switch: %d to %d\n",
-			priv->curr_bss_params.bss_descriptor.channel,
-			new_channel);
-		/* Update the channel again */
-		priv->curr_bss_params.bss_descriptor.channel = new_channel;
-	}
-
-	if (data_buf)
-		*data_buf = new_channel;
-
-	return 0;
-}
-
-/*
  * This function handles the command response of get extended version.
  *
  * Handling includes forming the extended version string and sending it
@@ -679,39 +677,33 @@ static int mwifiex_ret_reg_access(u16 type, struct host_cmd_ds_command *resp,
 	eeprom = data_buf;
 	switch (type) {
 	case HostCmd_CMD_MAC_REG_ACCESS:
-		r.mac = (struct host_cmd_ds_mac_reg_access *)
-			&resp->params.mac_reg;
+		r.mac = &resp->params.mac_reg;
 		reg_rw->offset = cpu_to_le32((u32) le16_to_cpu(r.mac->offset));
 		reg_rw->value = r.mac->value;
 		break;
 	case HostCmd_CMD_BBP_REG_ACCESS:
-		r.bbp = (struct host_cmd_ds_bbp_reg_access *)
-			&resp->params.bbp_reg;
+		r.bbp = &resp->params.bbp_reg;
 		reg_rw->offset = cpu_to_le32((u32) le16_to_cpu(r.bbp->offset));
 		reg_rw->value = cpu_to_le32((u32) r.bbp->value);
 		break;
 
 	case HostCmd_CMD_RF_REG_ACCESS:
-		r.rf = (struct host_cmd_ds_rf_reg_access *)
-		       &resp->params.rf_reg;
+		r.rf = &resp->params.rf_reg;
 		reg_rw->offset = cpu_to_le32((u32) le16_to_cpu(r.rf->offset));
 		reg_rw->value = cpu_to_le32((u32) r.bbp->value);
 		break;
 	case HostCmd_CMD_PMIC_REG_ACCESS:
-		r.pmic = (struct host_cmd_ds_pmic_reg_access *)
-			 &resp->params.pmic_reg;
+		r.pmic = &resp->params.pmic_reg;
 		reg_rw->offset = cpu_to_le32((u32) le16_to_cpu(r.pmic->offset));
 		reg_rw->value = cpu_to_le32((u32) r.pmic->value);
 		break;
 	case HostCmd_CMD_CAU_REG_ACCESS:
-		r.rf = (struct host_cmd_ds_rf_reg_access *)
-		       &resp->params.rf_reg;
+		r.rf = &resp->params.rf_reg;
 		reg_rw->offset = cpu_to_le32((u32) le16_to_cpu(r.rf->offset));
 		reg_rw->value = cpu_to_le32((u32) r.rf->value);
 		break;
 	case HostCmd_CMD_802_11_EEPROM_ACCESS:
-		r.eeprom = (struct host_cmd_ds_802_11_eeprom_access *)
-			   &resp->params.eeprom;
+		r.eeprom = &resp->params.eeprom;
 		pr_debug("info: EEPROM read len=%x\n", r.eeprom->byte_count);
 		if (le16_to_cpu(eeprom->byte_count) <
 		    le16_to_cpu(r.eeprom->byte_count)) {
@@ -787,7 +779,7 @@ static int mwifiex_ret_subsc_evt(struct mwifiex_private *priv,
 				 struct mwifiex_ds_misc_subsc_evt *sub_event)
 {
 	struct host_cmd_ds_802_11_subsc_evt *cmd_sub_event =
-		(struct host_cmd_ds_802_11_subsc_evt *)&resp->params.subsc_evt;
+		&resp->params.subsc_evt;
 
 	/* For every subscribe event command (Get/Set/Clear), FW reports the
 	 * current set of subscribed events*/
@@ -833,7 +825,7 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		ret = mwifiex_ret_mac_multicast_adr(priv, resp);
 		break;
 	case HostCmd_CMD_TX_RATE_CFG:
-		ret = mwifiex_ret_tx_rate_cfg(priv, resp, data_buf);
+		ret = mwifiex_ret_tx_rate_cfg(priv, resp);
 		break;
 	case HostCmd_CMD_802_11_SCAN:
 		ret = mwifiex_ret_802_11_scan(priv, resp);
@@ -847,6 +839,12 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 	case HostCmd_CMD_TXPWR_CFG:
 		ret = mwifiex_ret_tx_power_cfg(priv, resp);
 		break;
+	case HostCmd_CMD_RF_TX_PWR:
+		ret = mwifiex_ret_rf_tx_power(priv, resp);
+		break;
+	case HostCmd_CMD_RF_ANTENNA:
+		ret = mwifiex_ret_rf_antenna(priv, resp);
+		break;
 	case HostCmd_CMD_802_11_PS_MODE_ENH:
 		ret = mwifiex_ret_enh_power_mode(priv, resp, data_buf);
 		break;
@@ -878,9 +876,6 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 	case HostCmd_CMD_802_11_TX_RATE_QUERY:
 		ret = mwifiex_ret_802_11_tx_rate_query(priv, resp);
 		break;
-	case HostCmd_CMD_802_11_RF_CHANNEL:
-		ret = mwifiex_ret_802_11_rf_channel(priv, resp, data_buf);
-		break;
 	case HostCmd_CMD_VERSION_EXT:
 		ret = mwifiex_ret_ver_ext(priv, resp, data_buf);
 		break;
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index 11e731f3581..b8614a82546 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -422,7 +422,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 
 			if (len != -1) {
 				sinfo.filled = STATION_INFO_ASSOC_REQ_IES;
-				sinfo.assoc_req_ies = (u8 *)&event->data[len];
+				sinfo.assoc_req_ies = &event->data[len];
 				len = (u8 *)sinfo.assoc_req_ies -
 				      (u8 *)&event->frame_control;
 				sinfo.assoc_req_ies_len =
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 106c449477b..fb2136089a2 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -66,9 +66,6 @@ int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter)
 	dev_dbg(adapter->dev, "cmd pending\n");
 	atomic_inc(&adapter->cmd_pending);
 
-	/* Status pending, wake up main process */
-	queue_work(adapter->workqueue, &adapter->main_work);
-
 	/* Wait for completion */
 	wait_event_interruptible(adapter->cmd_wait_q.wait,
 				 *(cmd_queued->condition));
@@ -500,297 +497,24 @@ int mwifiex_disable_auto_ds(struct mwifiex_private *priv)
 EXPORT_SYMBOL_GPL(mwifiex_disable_auto_ds);
 
 /*
- * IOCTL request handler to set/get active channel.
- *
- * This function performs validity checking on channel/frequency
- * compatibility and returns failure if not valid.
- */
-int mwifiex_bss_set_channel(struct mwifiex_private *priv,
-			    struct mwifiex_chan_freq_power *chan)
-{
-	struct mwifiex_adapter *adapter = priv->adapter;
-	struct mwifiex_chan_freq_power *cfp = NULL;
-
-	if (!chan)
-		return -1;
-
-	if (!chan->channel && !chan->freq)
-		return -1;
-	if (adapter->adhoc_start_band & BAND_AN)
-		adapter->adhoc_start_band = BAND_G | BAND_B | BAND_GN;
-	else if (adapter->adhoc_start_band & BAND_A)
-		adapter->adhoc_start_band = BAND_G | BAND_B;
-	if (chan->channel) {
-		if (chan->channel <= MAX_CHANNEL_BAND_BG)
-			cfp = mwifiex_get_cfp(priv, 0, (u16) chan->channel, 0);
-		if (!cfp) {
-			cfp = mwifiex_get_cfp(priv, BAND_A,
-					      (u16) chan->channel, 0);
-			if (cfp) {
-				if (adapter->adhoc_11n_enabled)
-					adapter->adhoc_start_band = BAND_A
-								    | BAND_AN;
-				else
-					adapter->adhoc_start_band = BAND_A;
-			}
-		}
-	} else {
-		if (chan->freq <= MAX_FREQUENCY_BAND_BG)
-			cfp = mwifiex_get_cfp(priv, 0, 0, chan->freq);
-		if (!cfp) {
-			cfp = mwifiex_get_cfp(priv, BAND_A, 0, chan->freq);
-			if (cfp) {
-				if (adapter->adhoc_11n_enabled)
-					adapter->adhoc_start_band = BAND_A
-								    | BAND_AN;
-				else
-					adapter->adhoc_start_band = BAND_A;
-			}
-		}
-	}
-	if (!cfp || !cfp->channel) {
-		dev_err(adapter->dev, "invalid channel/freq\n");
-		return -1;
-	}
-	priv->adhoc_channel = (u8) cfp->channel;
-	chan->channel = cfp->channel;
-	chan->freq = cfp->freq;
-
-	return 0;
-}
-
-/*
- * IOCTL request handler to set/get Ad-Hoc channel.
- *
- * This function prepares the correct firmware command and
- * issues it to set or get the ad-hoc channel.
- */
-static int mwifiex_bss_ioctl_ibss_channel(struct mwifiex_private *priv,
-					  u16 action, u16 *channel)
-{
-	if (action == HostCmd_ACT_GEN_GET) {
-		if (!priv->media_connected) {
-			*channel = priv->adhoc_channel;
-			return 0;
-		}
-	} else {
-		priv->adhoc_channel = (u8) *channel;
-	}
-
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_RF_CHANNEL,
-				     action, 0, channel);
-}
-
-/*
- * IOCTL request handler to change Ad-Hoc channel.
- *
- * This function allocates the IOCTL request buffer, fills it
- * with requisite parameters and calls the IOCTL handler.
- *
- * The function follows the following steps to perform the change -
- *      - Get current IBSS information
- *      - Get current channel
- *      - If no change is required, return
- *      - If not connected, change channel and return
- *      - If connected,
- *          - Disconnect
- *          - Change channel
- *          - Perform specific SSID scan with same SSID
- *          - Start/Join the IBSS
- */
-int
-mwifiex_drv_change_adhoc_chan(struct mwifiex_private *priv, u16 channel)
-{
-	int ret;
-	struct mwifiex_bss_info bss_info;
-	struct mwifiex_ssid_bssid ssid_bssid;
-	u16 curr_chan = 0;
-	struct cfg80211_bss *bss = NULL;
-	struct ieee80211_channel *chan;
-	enum ieee80211_band band;
-
-	memset(&bss_info, 0, sizeof(bss_info));
-
-	/* Get BSS information */
-	if (mwifiex_get_bss_info(priv, &bss_info))
-		return -1;
-
-	/* Get current channel */
-	ret = mwifiex_bss_ioctl_ibss_channel(priv, HostCmd_ACT_GEN_GET,
-					     &curr_chan);
-
-	if (curr_chan == channel) {
-		ret = 0;
-		goto done;
-	}
-	dev_dbg(priv->adapter->dev, "cmd: updating channel from %d to %d\n",
-		curr_chan, channel);
-
-	if (!bss_info.media_connected) {
-		ret = 0;
-		goto done;
-	}
-
-	/* Do disonnect */
-	memset(&ssid_bssid, 0, ETH_ALEN);
-	ret = mwifiex_deauthenticate(priv, ssid_bssid.bssid);
-
-	ret = mwifiex_bss_ioctl_ibss_channel(priv, HostCmd_ACT_GEN_SET,
-					     &channel);
-
-	/* Do specific SSID scanning */
-	if (mwifiex_request_scan(priv, &bss_info.ssid)) {
-		ret = -1;
-		goto done;
-	}
-
-	band = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
-	chan = __ieee80211_get_channel(priv->wdev->wiphy,
-				       ieee80211_channel_to_frequency(channel,
-								      band));
-
-	/* Find the BSS we want using available scan results */
-	bss = cfg80211_get_bss(priv->wdev->wiphy, chan, bss_info.bssid,
-			       bss_info.ssid.ssid, bss_info.ssid.ssid_len,
-			       WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
-	if (!bss)
-		wiphy_warn(priv->wdev->wiphy, "assoc: bss %pM not in scan results\n",
-			   bss_info.bssid);
-
-	ret = mwifiex_bss_start(priv, bss, &bss_info.ssid);
-done:
-	return ret;
-}
-
-/*
- * IOCTL request handler to get rate.
- *
- * This function prepares the correct firmware command and
- * issues it to get the current rate if it is connected,
- * otherwise, the function returns the lowest supported rate
- * for the band.
- */
-static int mwifiex_rate_ioctl_get_rate_value(struct mwifiex_private *priv,
-					     struct mwifiex_rate_cfg *rate_cfg)
-{
-	rate_cfg->is_rate_auto = priv->is_data_rate_auto;
-	return mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_TX_RATE_QUERY,
-				     HostCmd_ACT_GEN_GET, 0, NULL);
-}
-
-/*
- * IOCTL request handler to set rate.
- *
- * This function prepares the correct firmware command and
- * issues it to set the current rate.
- *
- * The function also performs validation checking on the supplied value.
- */
-static int mwifiex_rate_ioctl_set_rate_value(struct mwifiex_private *priv,
-					     struct mwifiex_rate_cfg *rate_cfg)
-{
-	u8 rates[MWIFIEX_SUPPORTED_RATES];
-	u8 *rate;
-	int rate_index, ret;
-	u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
-	u32 i;
-	struct mwifiex_adapter *adapter = priv->adapter;
-
-	if (rate_cfg->is_rate_auto) {
-		memset(bitmap_rates, 0, sizeof(bitmap_rates));
-		/* Support all HR/DSSS rates */
-		bitmap_rates[0] = 0x000F;
-		/* Support all OFDM rates */
-		bitmap_rates[1] = 0x00FF;
-		/* Support all HT-MCSs rate */
-		for (i = 0; i < ARRAY_SIZE(priv->bitmap_rates) - 3; i++)
-			bitmap_rates[i + 2] = 0xFFFF;
-		bitmap_rates[9] = 0x3FFF;
-	} else {
-		memset(rates, 0, sizeof(rates));
-		mwifiex_get_active_data_rates(priv, rates);
-		rate = rates;
-		for (i = 0; (rate[i] && i < MWIFIEX_SUPPORTED_RATES); i++) {
-			dev_dbg(adapter->dev, "info: rate=%#x wanted=%#x\n",
-				rate[i], rate_cfg->rate);
-			if ((rate[i] & 0x7f) == (rate_cfg->rate & 0x7f))
-				break;
-		}
-		if ((i == MWIFIEX_SUPPORTED_RATES) || !rate[i]) {
-			dev_err(adapter->dev, "fixed data rate %#x is out "
-			       "of range\n", rate_cfg->rate);
-			return -1;
-		}
-		memset(bitmap_rates, 0, sizeof(bitmap_rates));
-
-		rate_index = mwifiex_data_rate_to_index(rate_cfg->rate);
-
-		/* Only allow b/g rates to be set */
-		if (rate_index >= MWIFIEX_RATE_INDEX_HRDSSS0 &&
-		    rate_index <= MWIFIEX_RATE_INDEX_HRDSSS3) {
-			bitmap_rates[0] = 1 << rate_index;
-		} else {
-			rate_index -= 1; /* There is a 0x00 in the table */
-			if (rate_index >= MWIFIEX_RATE_INDEX_OFDM0 &&
-			    rate_index <= MWIFIEX_RATE_INDEX_OFDM7)
-				bitmap_rates[1] = 1 << (rate_index -
-						   MWIFIEX_RATE_INDEX_OFDM0);
-		}
-	}
-
-	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_TX_RATE_CFG,
-				    HostCmd_ACT_GEN_SET, 0, bitmap_rates);
-
-	return ret;
-}
-
-/*
- * IOCTL request handler to set/get rate.
- *
- * This function can be used to set/get either the rate value or the
- * rate index.
- */
-static int mwifiex_rate_ioctl_cfg(struct mwifiex_private *priv,
-				  struct mwifiex_rate_cfg *rate_cfg)
-{
-	int status;
-
-	if (!rate_cfg)
-		return -1;
-
-	if (rate_cfg->action == HostCmd_ACT_GEN_GET)
-		status = mwifiex_rate_ioctl_get_rate_value(priv, rate_cfg);
-	else
-		status = mwifiex_rate_ioctl_set_rate_value(priv, rate_cfg);
-
-	return status;
-}
-
-/*
  * Sends IOCTL request to get the data rate.
  *
  * This function allocates the IOCTL request buffer, fills it
  * with requisite parameters and calls the IOCTL handler.
  */
-int mwifiex_drv_get_data_rate(struct mwifiex_private *priv,
-			      struct mwifiex_rate_cfg *rate)
+int mwifiex_drv_get_data_rate(struct mwifiex_private *priv, u32 *rate)
 {
 	int ret;
 
-	memset(rate, 0, sizeof(struct mwifiex_rate_cfg));
-	rate->action = HostCmd_ACT_GEN_GET;
-	ret = mwifiex_rate_ioctl_cfg(priv, rate);
+	ret = mwifiex_send_cmd_sync(priv, HostCmd_CMD_802_11_TX_RATE_QUERY,
+				    HostCmd_ACT_GEN_GET, 0, NULL);
 
 	if (!ret) {
-		if (rate->is_rate_auto)
-			rate->rate = mwifiex_index_to_data_rate(priv,
-								priv->tx_rate,
-								priv->tx_htinfo
-								);
+		if (priv->is_data_rate_auto)
+			*rate = mwifiex_index_to_data_rate(priv, priv->tx_rate,
+							   priv->tx_htinfo);
 		else
-			rate->rate = priv->data_rate;
-	} else {
-		ret = -1;
+			*rate = priv->data_rate;
 	}
 
 	return ret;
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index 89f9a2a45de..f40e93fe894 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -26,6 +26,7 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 			      struct mwifiex_uap_bss_param *bss_config,
 			      struct cfg80211_ap_settings *params) {
 	int i;
+	struct mwifiex_wep_key wep_key;
 
 	if (!params->privacy) {
 		bss_config->protocol = PROTOCOL_NO_SECURITY;
@@ -65,7 +66,7 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 			}
 			if (params->crypto.wpa_versions &
 			    NL80211_WPA_VERSION_2) {
-				bss_config->protocol = PROTOCOL_WPA2;
+				bss_config->protocol |= PROTOCOL_WPA2;
 				bss_config->key_mgmt = KEY_MGMT_EAP;
 			}
 			break;
@@ -77,7 +78,7 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 			}
 			if (params->crypto.wpa_versions &
 			    NL80211_WPA_VERSION_2) {
-				bss_config->protocol = PROTOCOL_WPA2;
+				bss_config->protocol |= PROTOCOL_WPA2;
 				bss_config->key_mgmt = KEY_MGMT_PSK;
 			}
 			break;
@@ -91,10 +92,19 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 		case WLAN_CIPHER_SUITE_WEP104:
 			break;
 		case WLAN_CIPHER_SUITE_TKIP:
-			bss_config->wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+			if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+				bss_config->wpa_cfg.pairwise_cipher_wpa |=
+								CIPHER_TKIP;
+			if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+				bss_config->wpa_cfg.pairwise_cipher_wpa2 |=
+								CIPHER_TKIP;
 			break;
 		case WLAN_CIPHER_SUITE_CCMP:
-			bss_config->wpa_cfg.pairwise_cipher_wpa2 =
+			if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+				bss_config->wpa_cfg.pairwise_cipher_wpa |=
+								CIPHER_AES_CCMP;
+			if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+				bss_config->wpa_cfg.pairwise_cipher_wpa2 |=
 								CIPHER_AES_CCMP;
 		default:
 			break;
@@ -104,6 +114,27 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 	switch (params->crypto.cipher_group) {
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
+		if (priv->sec_info.wep_enabled) {
+			bss_config->protocol = PROTOCOL_STATIC_WEP;
+			bss_config->key_mgmt = KEY_MGMT_NONE;
+			bss_config->wpa_cfg.length = 0;
+
+			for (i = 0; i < NUM_WEP_KEYS; i++) {
+				wep_key = priv->wep_key[i];
+				bss_config->wep_cfg[i].key_index = i;
+
+				if (priv->wep_key_curr_index == i)
+					bss_config->wep_cfg[i].is_default = 1;
+				else
+					bss_config->wep_cfg[i].is_default = 0;
+
+				bss_config->wep_cfg[i].length =
+							     wep_key.key_length;
+				memcpy(&bss_config->wep_cfg[i].key,
+				       &wep_key.key_material,
+				       wep_key.key_length);
+			}
+		}
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
 		bss_config->wpa_cfg.group_cipher = CIPHER_TKIP;
@@ -118,6 +149,33 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
 	return 0;
 }
 
+/* This function updates 11n related parameters from IE and sets them into
+ * bss_config structure.
+ */
+void
+mwifiex_set_ht_params(struct mwifiex_private *priv,
+		      struct mwifiex_uap_bss_param *bss_cfg,
+		      struct cfg80211_ap_settings *params)
+{
+	const u8 *ht_ie;
+
+	if (!ISSUPP_11NENABLED(priv->adapter->fw_cap_info))
+		return;
+
+	ht_ie = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, params->beacon.tail,
+				 params->beacon.tail_len);
+	if (ht_ie) {
+		memcpy(&bss_cfg->ht_cap, ht_ie + 2,
+		       sizeof(struct ieee80211_ht_cap));
+	} else {
+		memset(&bss_cfg->ht_cap , 0, sizeof(struct ieee80211_ht_cap));
+		bss_cfg->ht_cap.cap_info = cpu_to_le16(MWIFIEX_DEF_HT_CAP);
+		bss_cfg->ht_cap.ampdu_params_info = MWIFIEX_DEF_AMPDU;
+	}
+
+	return;
+}
+
 /* This function initializes some of mwifiex_uap_bss_param variables.
  * This helps FW in ignoring invalid values. These values may or may not
  * be get updated to valid ones at later stage.
@@ -135,6 +193,120 @@ void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config)
 }
 
 /* This function parses BSS related parameters from structure
+ * and prepares TLVs specific to WPA/WPA2 security.
+ * These TLVs are appended to command buffer.
+ */
+static void
+mwifiex_uap_bss_wpa(u8 **tlv_buf, void *cmd_buf, u16 *param_size)
+{
+	struct host_cmd_tlv_pwk_cipher *pwk_cipher;
+	struct host_cmd_tlv_gwk_cipher *gwk_cipher;
+	struct host_cmd_tlv_passphrase *passphrase;
+	struct host_cmd_tlv_akmp *tlv_akmp;
+	struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
+	u16 cmd_size = *param_size;
+	u8 *tlv = *tlv_buf;
+
+	tlv_akmp = (struct host_cmd_tlv_akmp *)tlv;
+	tlv_akmp->tlv.type = cpu_to_le16(TLV_TYPE_UAP_AKMP);
+	tlv_akmp->tlv.len = cpu_to_le16(sizeof(struct host_cmd_tlv_akmp) -
+					sizeof(struct host_cmd_tlv));
+	tlv_akmp->key_mgmt_operation = cpu_to_le16(bss_cfg->key_mgmt_operation);
+	tlv_akmp->key_mgmt = cpu_to_le16(bss_cfg->key_mgmt);
+	cmd_size += sizeof(struct host_cmd_tlv_akmp);
+	tlv += sizeof(struct host_cmd_tlv_akmp);
+
+	if (bss_cfg->wpa_cfg.pairwise_cipher_wpa & VALID_CIPHER_BITMAP) {
+		pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+		pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+		pwk_cipher->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_pwk_cipher) -
+				    sizeof(struct host_cmd_tlv));
+		pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA);
+		pwk_cipher->cipher = bss_cfg->wpa_cfg.pairwise_cipher_wpa;
+		cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+		tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+	}
+
+	if (bss_cfg->wpa_cfg.pairwise_cipher_wpa2 & VALID_CIPHER_BITMAP) {
+		pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+		pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+		pwk_cipher->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_pwk_cipher) -
+				    sizeof(struct host_cmd_tlv));
+		pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA2);
+		pwk_cipher->cipher = bss_cfg->wpa_cfg.pairwise_cipher_wpa2;
+		cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+		tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+	}
+
+	if (bss_cfg->wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
+		gwk_cipher = (struct host_cmd_tlv_gwk_cipher *)tlv;
+		gwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_GWK_CIPHER);
+		gwk_cipher->tlv.len =
+			cpu_to_le16(sizeof(struct host_cmd_tlv_gwk_cipher) -
+				    sizeof(struct host_cmd_tlv));
+		gwk_cipher->cipher = bss_cfg->wpa_cfg.group_cipher;
+		cmd_size += sizeof(struct host_cmd_tlv_gwk_cipher);
+		tlv += sizeof(struct host_cmd_tlv_gwk_cipher);
+	}
+
+	if (bss_cfg->wpa_cfg.length) {
+		passphrase = (struct host_cmd_tlv_passphrase *)tlv;
+		passphrase->tlv.type = cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
+		passphrase->tlv.len = cpu_to_le16(bss_cfg->wpa_cfg.length);
+		memcpy(passphrase->passphrase, bss_cfg->wpa_cfg.passphrase,
+		       bss_cfg->wpa_cfg.length);
+		cmd_size += sizeof(struct host_cmd_tlv) +
+			    bss_cfg->wpa_cfg.length;
+		tlv += sizeof(struct host_cmd_tlv) + bss_cfg->wpa_cfg.length;
+	}
+
+	*param_size = cmd_size;
+	*tlv_buf = tlv;
+
+	return;
+}
+
+/* This function parses BSS related parameters from structure
+ * and prepares TLVs specific to WEP encryption.
+ * These TLVs are appended to command buffer.
+ */
+static void
+mwifiex_uap_bss_wep(u8 **tlv_buf, void *cmd_buf, u16 *param_size)
+{
+	struct host_cmd_tlv_wep_key *wep_key;
+	u16 cmd_size = *param_size;
+	int i;
+	u8 *tlv = *tlv_buf;
+	struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
+
+	for (i = 0; i < NUM_WEP_KEYS; i++) {
+		if (bss_cfg->wep_cfg[i].length &&
+		    (bss_cfg->wep_cfg[i].length == WLAN_KEY_LEN_WEP40 ||
+		     bss_cfg->wep_cfg[i].length == WLAN_KEY_LEN_WEP104)) {
+			wep_key = (struct host_cmd_tlv_wep_key *)tlv;
+			wep_key->tlv.type = cpu_to_le16(TLV_TYPE_UAP_WEP_KEY);
+			wep_key->tlv.len =
+				cpu_to_le16(bss_cfg->wep_cfg[i].length + 2);
+			wep_key->key_index = bss_cfg->wep_cfg[i].key_index;
+			wep_key->is_default = bss_cfg->wep_cfg[i].is_default;
+			memcpy(wep_key->key, bss_cfg->wep_cfg[i].key,
+			       bss_cfg->wep_cfg[i].length);
+			cmd_size += sizeof(struct host_cmd_tlv) + 2 +
+				    bss_cfg->wep_cfg[i].length;
+			tlv += sizeof(struct host_cmd_tlv) + 2 +
+				    bss_cfg->wep_cfg[i].length;
+		}
+	}
+
+	*param_size = cmd_size;
+	*tlv_buf = tlv;
+
+	return;
+}
+
+/* This function parses BSS related parameters from structure
  * and prepares TLVs. These TLVs are appended to command buffer.
 */
 static int
@@ -148,12 +320,9 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
 	struct host_cmd_tlv_frag_threshold *frag_threshold;
 	struct host_cmd_tlv_rts_threshold *rts_threshold;
 	struct host_cmd_tlv_retry_limit *retry_limit;
-	struct host_cmd_tlv_pwk_cipher *pwk_cipher;
-	struct host_cmd_tlv_gwk_cipher *gwk_cipher;
 	struct host_cmd_tlv_encrypt_protocol *encrypt_protocol;
 	struct host_cmd_tlv_auth_type *auth_type;
-	struct host_cmd_tlv_passphrase *passphrase;
-	struct host_cmd_tlv_akmp *tlv_akmp;
+	struct mwifiex_ie_types_htcap *htcap;
 	struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
 	u16 cmd_size = *param_size;
 
@@ -243,70 +412,11 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
 	}
 	if ((bss_cfg->protocol & PROTOCOL_WPA) ||
 	    (bss_cfg->protocol & PROTOCOL_WPA2) ||
-	    (bss_cfg->protocol & PROTOCOL_EAP)) {
-		tlv_akmp = (struct host_cmd_tlv_akmp *)tlv;
-		tlv_akmp->tlv.type = cpu_to_le16(TLV_TYPE_UAP_AKMP);
-		tlv_akmp->tlv.len =
-		    cpu_to_le16(sizeof(struct host_cmd_tlv_akmp) -
-				sizeof(struct host_cmd_tlv));
-		tlv_akmp->key_mgmt_operation =
-			cpu_to_le16(bss_cfg->key_mgmt_operation);
-		tlv_akmp->key_mgmt = cpu_to_le16(bss_cfg->key_mgmt);
-		cmd_size += sizeof(struct host_cmd_tlv_akmp);
-		tlv += sizeof(struct host_cmd_tlv_akmp);
-
-		if (bss_cfg->wpa_cfg.pairwise_cipher_wpa &
-				VALID_CIPHER_BITMAP) {
-			pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
-			pwk_cipher->tlv.type =
-				cpu_to_le16(TLV_TYPE_PWK_CIPHER);
-			pwk_cipher->tlv.len = cpu_to_le16(
-				sizeof(struct host_cmd_tlv_pwk_cipher) -
-				sizeof(struct host_cmd_tlv));
-			pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA);
-			pwk_cipher->cipher =
-				bss_cfg->wpa_cfg.pairwise_cipher_wpa;
-			cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
-			tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
-		}
-		if (bss_cfg->wpa_cfg.pairwise_cipher_wpa2 &
-				VALID_CIPHER_BITMAP) {
-			pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
-			pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
-			pwk_cipher->tlv.len = cpu_to_le16(
-				sizeof(struct host_cmd_tlv_pwk_cipher) -
-				sizeof(struct host_cmd_tlv));
-			pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA2);
-			pwk_cipher->cipher =
-				bss_cfg->wpa_cfg.pairwise_cipher_wpa2;
-			cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
-			tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
-		}
-		if (bss_cfg->wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
-			gwk_cipher = (struct host_cmd_tlv_gwk_cipher *)tlv;
-			gwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_GWK_CIPHER);
-			gwk_cipher->tlv.len = cpu_to_le16(
-				sizeof(struct host_cmd_tlv_gwk_cipher) -
-				sizeof(struct host_cmd_tlv));
-			gwk_cipher->cipher = bss_cfg->wpa_cfg.group_cipher;
-			cmd_size += sizeof(struct host_cmd_tlv_gwk_cipher);
-			tlv += sizeof(struct host_cmd_tlv_gwk_cipher);
-		}
-		if (bss_cfg->wpa_cfg.length) {
-			passphrase = (struct host_cmd_tlv_passphrase *)tlv;
-			passphrase->tlv.type =
-				cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
-			passphrase->tlv.len =
-				cpu_to_le16(bss_cfg->wpa_cfg.length);
-			memcpy(passphrase->passphrase,
-			       bss_cfg->wpa_cfg.passphrase,
-			       bss_cfg->wpa_cfg.length);
-			cmd_size += sizeof(struct host_cmd_tlv) +
-				    bss_cfg->wpa_cfg.length;
-			tlv += sizeof(struct host_cmd_tlv) +
-			       bss_cfg->wpa_cfg.length;
-		}
-	}
+	    (bss_cfg->protocol & PROTOCOL_EAP))
+		mwifiex_uap_bss_wpa(&tlv, cmd_buf, &cmd_size);
+	else
+		mwifiex_uap_bss_wep(&tlv, cmd_buf, &cmd_size);
+
 	if ((bss_cfg->auth_mode <= WLAN_AUTH_SHARED_KEY) ||
 	    (bss_cfg->auth_mode == MWIFIEX_AUTH_MODE_AUTO)) {
 		auth_type = (struct host_cmd_tlv_auth_type *)tlv;
@@ -330,6 +440,25 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
 		tlv += sizeof(struct host_cmd_tlv_encrypt_protocol);
 	}
 
+	if (bss_cfg->ht_cap.cap_info) {
+		htcap = (struct mwifiex_ie_types_htcap *)tlv;
+		htcap->header.type = cpu_to_le16(WLAN_EID_HT_CAPABILITY);
+		htcap->header.len =
+				cpu_to_le16(sizeof(struct ieee80211_ht_cap));
+		htcap->ht_cap.cap_info = bss_cfg->ht_cap.cap_info;
+		htcap->ht_cap.ampdu_params_info =
+					     bss_cfg->ht_cap.ampdu_params_info;
+		memcpy(&htcap->ht_cap.mcs, &bss_cfg->ht_cap.mcs,
+		       sizeof(struct ieee80211_mcs_info));
+		htcap->ht_cap.extended_ht_cap_info =
+					bss_cfg->ht_cap.extended_ht_cap_info;
+		htcap->ht_cap.tx_BF_cap_info = bss_cfg->ht_cap.tx_BF_cap_info;
+		htcap->ht_cap.antenna_selection_info =
+					bss_cfg->ht_cap.antenna_selection_info;
+		cmd_size += sizeof(struct mwifiex_ie_types_htcap);
+		tlv += sizeof(struct mwifiex_ie_types_htcap);
+	}
+
 	*param_size = cmd_size;
 
 	return 0;
@@ -421,33 +550,3 @@ int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, u16 cmd_no,
 
 	return 0;
 }
-
-/* This function sets the RF channel for AP.
- *
- * This function populates channel information in AP config structure
- * and sends command to configure channel information in AP.
- */
-int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel)
-{
-	struct mwifiex_uap_bss_param *bss_cfg;
-	struct wiphy *wiphy = priv->wdev->wiphy;
-
-	bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
-	if (!bss_cfg)
-		return -ENOMEM;
-
-	mwifiex_set_sys_config_invalid_data(bss_cfg);
-	bss_cfg->band_cfg = BAND_CONFIG_MANUAL;
-	bss_cfg->channel = channel;
-
-	if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
-				   HostCmd_ACT_GEN_SET,
-				   UAP_BSS_PARAMS_I, bss_cfg)) {
-		wiphy_err(wiphy, "Failed to set the uAP channel\n");
-		kfree(bss_cfg);
-		return -1;
-	}
-
-	kfree(bss_cfg);
-	return 0;
-}
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index cf7bdc66f82..224e03ade14 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -1665,7 +1665,9 @@ mwl8k_txq_reclaim(struct ieee80211_hw *hw, int index, int limit, int force)
 
 		info = IEEE80211_SKB_CB(skb);
 		if (ieee80211_is_data(wh->frame_control)) {
-			sta = info->control.sta;
+			rcu_read_lock();
+			sta = ieee80211_find_sta_by_ifaddr(hw, wh->addr1,
+							   wh->addr2);
 			if (sta) {
 				sta_info = MWL8K_STA(sta);
 				BUG_ON(sta_info == NULL);
@@ -1682,6 +1684,7 @@ mwl8k_txq_reclaim(struct ieee80211_hw *hw, int index, int limit, int force)
 					sta_info->is_ampdu_allowed = true;
 				}
 			}
+			rcu_read_unlock();
 		}
 
 		ieee80211_tx_info_clear_status(info);
diff --git a/drivers/net/wireless/orinoco/cfg.c b/drivers/net/wireless/orinoco/cfg.c
index f7b15b8934f..7b751fba7e1 100644
--- a/drivers/net/wireless/orinoco/cfg.c
+++ b/drivers/net/wireless/orinoco/cfg.c
@@ -138,7 +138,7 @@ static int orinoco_change_vif(struct wiphy *wiphy, struct net_device *dev,
 	return err;
 }
 
-static int orinoco_scan(struct wiphy *wiphy, struct net_device *dev,
+static int orinoco_scan(struct wiphy *wiphy,
 			struct cfg80211_scan_request *request)
 {
 	struct orinoco_private *priv = wiphy_priv(wiphy);
@@ -160,10 +160,9 @@ static int orinoco_scan(struct wiphy *wiphy, struct net_device *dev,
 	return err;
 }
 
-static int orinoco_set_channel(struct wiphy *wiphy,
-			struct net_device *netdev,
-			struct ieee80211_channel *chan,
-			enum nl80211_channel_type channel_type)
+static int orinoco_set_monitor_channel(struct wiphy *wiphy,
+				       struct ieee80211_channel *chan,
+				       enum nl80211_channel_type channel_type)
 {
 	struct orinoco_private *priv = wiphy_priv(wiphy);
 	int err = 0;
@@ -286,7 +285,7 @@ static int orinoco_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 
 const struct cfg80211_ops orinoco_cfg_ops = {
 	.change_virtual_intf = orinoco_change_vif,
-	.set_channel = orinoco_set_channel,
+	.set_monitor_channel = orinoco_set_monitor_channel,
 	.scan = orinoco_scan,
 	.set_wiphy_params = orinoco_set_wiphy_params,
 };
diff --git a/drivers/net/wireless/p54/eeprom.c b/drivers/net/wireless/p54/eeprom.c
index fa8ce510478..14037092ba8 100644
--- a/drivers/net/wireless/p54/eeprom.c
+++ b/drivers/net/wireless/p54/eeprom.c
@@ -857,7 +857,7 @@ good_eeprom:
 
 		wiphy_warn(dev->wiphy,
 			   "Invalid hwaddr! Using randomly generated MAC addr\n");
-		random_ether_addr(perm_addr);
+		eth_random_addr(perm_addr);
 		SET_IEEE80211_PERM_ADDR(dev, perm_addr);
 	}
 
@@ -905,7 +905,7 @@ int p54_read_eeprom(struct ieee80211_hw *dev)
 
 	while (eeprom_size) {
 		blocksize = min(eeprom_size, maxblocksize);
-		ret = p54_download_eeprom(priv, (void *) (eeprom + offset),
+		ret = p54_download_eeprom(priv, eeprom + offset,
 					  offset, blocksize);
 		if (unlikely(ret))
 			goto free;
diff --git a/drivers/net/wireless/p54/fwio.c b/drivers/net/wireless/p54/fwio.c
index 18e82b31afa..9ba85106eec 100644
--- a/drivers/net/wireless/p54/fwio.c
+++ b/drivers/net/wireless/p54/fwio.c
@@ -478,7 +478,7 @@ int p54_scan(struct p54_common *priv, u16 mode, u16 dwell)
 
 		if (priv->rxhw == PDR_SYNTH_FRONTEND_LONGBOW) {
 			memcpy(&body->longbow.curve_data,
-				(void *) entry + sizeof(__le16),
+				entry + sizeof(__le16),
 				priv->curve_data->entry_size);
 		} else {
 			struct p54_scan_body *chan = &body->normal;
diff --git a/drivers/net/wireless/p54/txrx.c b/drivers/net/wireless/p54/txrx.c
index 82a1cac920b..f38786e0262 100644
--- a/drivers/net/wireless/p54/txrx.c
+++ b/drivers/net/wireless/p54/txrx.c
@@ -422,11 +422,11 @@ static void p54_rx_frame_sent(struct p54_common *priv, struct sk_buff *skb)
 	 * Clear manually, ieee80211_tx_info_clear_status would
 	 * clear the counts too and we need them.
 	 */
-	memset(&info->status.ampdu_ack_len, 0,
+	memset(&info->status.ack_signal, 0,
 	       sizeof(struct ieee80211_tx_info) -
-	       offsetof(struct ieee80211_tx_info, status.ampdu_ack_len));
+	       offsetof(struct ieee80211_tx_info, status.ack_signal));
 	BUILD_BUG_ON(offsetof(struct ieee80211_tx_info,
-			      status.ampdu_ack_len) != 23);
+			      status.ack_signal) != 20);
 
 	if (entry_hdr->flags & cpu_to_le16(P54_HDR_FLAG_DATA_ALIGN))
 		pad = entry_data->align[0];
diff --git a/drivers/net/wireless/prism54/islpci_eth.c b/drivers/net/wireless/prism54/islpci_eth.c
index 266d45bf86f..799e148d037 100644
--- a/drivers/net/wireless/prism54/islpci_eth.c
+++ b/drivers/net/wireless/prism54/islpci_eth.c
@@ -455,7 +455,7 @@ islpci_eth_receive(islpci_private *priv)
 			      "Error mapping DMA address\n");
 
 			/* free the skbuf structure before aborting */
-			dev_kfree_skb_irq((struct sk_buff *) skb);
+			dev_kfree_skb_irq(skb);
 			skb = NULL;
 			break;
 		}
diff --git a/drivers/net/wireless/ray_cs.c b/drivers/net/wireless/ray_cs.c
index 86a738bf591..598ca1cafb9 100644
--- a/drivers/net/wireless/ray_cs.c
+++ b/drivers/net/wireless/ray_cs.c
@@ -1849,7 +1849,7 @@ static irqreturn_t ray_interrupt(int irq, void *dev_id)
 	pr_debug("ray_cs: interrupt for *dev=%p\n", dev);
 
 	local = netdev_priv(dev);
-	link = (struct pcmcia_device *)local->finder;
+	link = local->finder;
 	if (!pcmcia_dev_present(link)) {
 		pr_debug(
 			"ray_cs interrupt from device not present or suspended.\n");
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index dfcd02ab6ca..241162e8111 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -484,7 +484,7 @@ static int rndis_change_virtual_intf(struct wiphy *wiphy,
 					enum nl80211_iftype type, u32 *flags,
 					struct vif_params *params);
 
-static int rndis_scan(struct wiphy *wiphy, struct net_device *dev,
+static int rndis_scan(struct wiphy *wiphy,
 			struct cfg80211_scan_request *request);
 
 static int rndis_set_wiphy_params(struct wiphy *wiphy, u32 changed);
@@ -1941,9 +1941,10 @@ static int rndis_get_tx_power(struct wiphy *wiphy, int *dbm)
 }
 
 #define SCAN_DELAY_JIFFIES (6 * HZ)
-static int rndis_scan(struct wiphy *wiphy, struct net_device *dev,
+static int rndis_scan(struct wiphy *wiphy,
 			struct cfg80211_scan_request *request)
 {
+	struct net_device *dev = request->wdev->netdev;
 	struct usbnet *usbdev = netdev_priv(dev);
 	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
 	int ret;
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 299c3879582..c7548da6573 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -99,6 +99,14 @@ config RT2800PCI_RT53XX
          rt2800pci driver.
          Supported chips: RT5390
 
+config RT2800PCI_RT3290
+       bool "rt2800pci - Include support for rt3290 devices (EXPERIMENTAL)"
+       depends on EXPERIMENTAL
+       default y
+       ---help---
+         This adds support for rt3290 wireless chipset family to the
+         rt2800pci driver.
+         Supported chips: RT3290
 endif
 
 config RT2500USB
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 5e6b5014316..8b9dbd76a25 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -1455,7 +1455,7 @@ static int rt2400pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 */
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
-		random_ether_addr(mac);
+		eth_random_addr(mac);
 		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index 136b849f11b..d2cf8a4bc8b 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -1585,7 +1585,7 @@ static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 */
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
-		random_ether_addr(mac);
+		eth_random_addr(mac);
 		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 669aecdb411..3aae36bb0a9 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1352,7 +1352,7 @@ static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 */
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
-		random_ether_addr(mac);
+		eth_random_addr(mac);
 		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index 9348521e083..e252e9bafd0 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)
  * RF3053 2.4G/5G 3T3R(RT3883/RT3563/RT3573/RT3593/RT3662)
+ * RF5360 2.4G 1T1R
  * RF5370 2.4G 1T1R
  * RF5390 2.4G 1T1R
  */
@@ -67,9 +68,12 @@
 #define RF3320				0x000b
 #define RF3322				0x000c
 #define RF3053				0x000d
+#define RF3290				0x3290
+#define RF5360				0x5360
 #define RF5370				0x5370
 #define RF5372				0x5372
 #define RF5390				0x5390
+#define RF5392				0x5392
 
 /*
  * Chipset revisions.
@@ -114,6 +118,12 @@
  * Registers.
  */
 
+
+/*
+ * MAC_CSR0_3290: MAC_CSR0 for RT3290 to identity MAC version number.
+ */
+#define MAC_CSR0_3290				0x0000
+
 /*
  * E2PROM_CSR: PCI EEPROM control register.
  * RELOAD: Write 1 to reload eeprom content.
@@ -130,6 +140,150 @@
 #define E2PROM_CSR_RELOAD		FIELD32(0x00000080)
 
 /*
+ * CMB_CTRL_CFG
+ */
+#define CMB_CTRL		0x0020
+#define AUX_OPT_BIT0		FIELD32(0x00000001)
+#define AUX_OPT_BIT1		FIELD32(0x00000002)
+#define AUX_OPT_BIT2		FIELD32(0x00000004)
+#define AUX_OPT_BIT3		FIELD32(0x00000008)
+#define AUX_OPT_BIT4		FIELD32(0x00000010)
+#define AUX_OPT_BIT5		FIELD32(0x00000020)
+#define AUX_OPT_BIT6		FIELD32(0x00000040)
+#define AUX_OPT_BIT7		FIELD32(0x00000080)
+#define AUX_OPT_BIT8		FIELD32(0x00000100)
+#define AUX_OPT_BIT9		FIELD32(0x00000200)
+#define AUX_OPT_BIT10		FIELD32(0x00000400)
+#define AUX_OPT_BIT11		FIELD32(0x00000800)
+#define AUX_OPT_BIT12		FIELD32(0x00001000)
+#define AUX_OPT_BIT13		FIELD32(0x00002000)
+#define AUX_OPT_BIT14		FIELD32(0x00004000)
+#define AUX_OPT_BIT15		FIELD32(0x00008000)
+#define LDO25_LEVEL		FIELD32(0x00030000)
+#define LDO25_LARGEA		FIELD32(0x00040000)
+#define LDO25_FRC_ON		FIELD32(0x00080000)
+#define CMB_RSV			FIELD32(0x00300000)
+#define XTAL_RDY		FIELD32(0x00400000)
+#define PLL_LD			FIELD32(0x00800000)
+#define LDO_CORE_LEVEL		FIELD32(0x0F000000)
+#define LDO_BGSEL		FIELD32(0x30000000)
+#define LDO3_EN			FIELD32(0x40000000)
+#define LDO0_EN			FIELD32(0x80000000)
+
+/*
+ * EFUSE_CSR_3290: RT3290 EEPROM
+ */
+#define EFUSE_CTRL_3290			0x0024
+
+/*
+ * EFUSE_DATA3 of 3290
+ */
+#define EFUSE_DATA3_3290		0x0028
+
+/*
+ * EFUSE_DATA2 of 3290
+ */
+#define EFUSE_DATA2_3290		0x002c
+
+/*
+ * EFUSE_DATA1 of 3290
+ */
+#define EFUSE_DATA1_3290		0x0030
+
+/*
+ * EFUSE_DATA0 of 3290
+ */
+#define EFUSE_DATA0_3290		0x0034
+
+/*
+ * OSC_CTRL_CFG
+ * Ring oscillator configuration
+ */
+#define OSC_CTRL		0x0038
+#define OSC_REF_CYCLE		FIELD32(0x00001fff)
+#define OSC_RSV			FIELD32(0x0000e000)
+#define OSC_CAL_CNT		FIELD32(0x0fff0000)
+#define OSC_CAL_ACK		FIELD32(0x10000000)
+#define OSC_CLK_32K_VLD		FIELD32(0x20000000)
+#define OSC_CAL_REQ		FIELD32(0x40000000)
+#define OSC_ROSC_EN		FIELD32(0x80000000)
+
+/*
+ * COEX_CFG_0
+ */
+#define COEX_CFG0			0x0040
+#define COEX_CFG_ANT		FIELD32(0xff000000)
+/*
+ * COEX_CFG_1
+ */
+#define COEX_CFG1			0x0044
+
+/*
+ * COEX_CFG_2
+ */
+#define COEX_CFG2			0x0048
+#define BT_COEX_CFG1		FIELD32(0xff000000)
+#define BT_COEX_CFG0		FIELD32(0x00ff0000)
+#define WL_COEX_CFG1		FIELD32(0x0000ff00)
+#define WL_COEX_CFG0		FIELD32(0x000000ff)
+/*
+ * PLL_CTRL_CFG
+ * PLL configuration register
+ */
+#define PLL_CTRL		0x0050
+#define PLL_RESERVED_INPUT1	FIELD32(0x000000ff)
+#define PLL_RESERVED_INPUT2	FIELD32(0x0000ff00)
+#define PLL_CONTROL		FIELD32(0x00070000)
+#define PLL_LPF_R1		FIELD32(0x00080000)
+#define PLL_LPF_C1_CTRL	FIELD32(0x00300000)
+#define PLL_LPF_C2_CTRL	FIELD32(0x00c00000)
+#define PLL_CP_CURRENT_CTRL	FIELD32(0x03000000)
+#define PLL_PFD_DELAY_CTRL	FIELD32(0x0c000000)
+#define PLL_LOCK_CTRL		FIELD32(0x70000000)
+#define PLL_VBGBK_EN		FIELD32(0x80000000)
+
+
+/*
+ * WLAN_CTRL_CFG
+ * RT3290 wlan configuration
+ */
+#define WLAN_FUN_CTRL			0x0080
+#define WLAN_EN				FIELD32(0x00000001)
+#define WLAN_CLK_EN			FIELD32(0x00000002)
+#define WLAN_RSV1			FIELD32(0x00000004)
+#define WLAN_RESET			FIELD32(0x00000008)
+#define PCIE_APP0_CLK_REQ		FIELD32(0x00000010)
+#define FRC_WL_ANT_SET			FIELD32(0x00000020)
+#define INV_TR_SW0			FIELD32(0x00000040)
+#define WLAN_GPIO_IN_BIT0		FIELD32(0x00000100)
+#define WLAN_GPIO_IN_BIT1		FIELD32(0x00000200)
+#define WLAN_GPIO_IN_BIT2		FIELD32(0x00000400)
+#define WLAN_GPIO_IN_BIT3		FIELD32(0x00000800)
+#define WLAN_GPIO_IN_BIT4		FIELD32(0x00001000)
+#define WLAN_GPIO_IN_BIT5		FIELD32(0x00002000)
+#define WLAN_GPIO_IN_BIT6		FIELD32(0x00004000)
+#define WLAN_GPIO_IN_BIT7		FIELD32(0x00008000)
+#define WLAN_GPIO_IN_BIT_ALL		FIELD32(0x0000ff00)
+#define WLAN_GPIO_OUT_BIT0		FIELD32(0x00010000)
+#define WLAN_GPIO_OUT_BIT1		FIELD32(0x00020000)
+#define WLAN_GPIO_OUT_BIT2		FIELD32(0x00040000)
+#define WLAN_GPIO_OUT_BIT3		FIELD32(0x00050000)
+#define WLAN_GPIO_OUT_BIT4		FIELD32(0x00100000)
+#define WLAN_GPIO_OUT_BIT5		FIELD32(0x00200000)
+#define WLAN_GPIO_OUT_BIT6		FIELD32(0x00400000)
+#define WLAN_GPIO_OUT_BIT7		FIELD32(0x00800000)
+#define WLAN_GPIO_OUT_BIT_ALL		FIELD32(0x00ff0000)
+#define WLAN_GPIO_OUT_OE_BIT0		FIELD32(0x01000000)
+#define WLAN_GPIO_OUT_OE_BIT1		FIELD32(0x02000000)
+#define WLAN_GPIO_OUT_OE_BIT2		FIELD32(0x04000000)
+#define WLAN_GPIO_OUT_OE_BIT3		FIELD32(0x08000000)
+#define WLAN_GPIO_OUT_OE_BIT4		FIELD32(0x10000000)
+#define WLAN_GPIO_OUT_OE_BIT5		FIELD32(0x20000000)
+#define WLAN_GPIO_OUT_OE_BIT6		FIELD32(0x40000000)
+#define WLAN_GPIO_OUT_OE_BIT7		FIELD32(0x80000000)
+#define WLAN_GPIO_OUT_OE_BIT_ALL	FIELD32(0xff000000)
+
+/*
  * AUX_CTRL: Aux/PCI-E related configuration
  */
 #define AUX_CTRL			0x10c
@@ -1760,9 +1914,11 @@ struct mac_iveiv_entry {
 /*
  * BBP 3: RX Antenna
  */
-#define BBP3_RX_ADC				FIELD8(0x03)
+#define BBP3_RX_ADC			FIELD8(0x03)
 #define BBP3_RX_ANTENNA			FIELD8(0x18)
 #define BBP3_HT40_MINUS			FIELD8(0x20)
+#define BBP3_ADC_MODE_SWITCH		FIELD8(0x40)
+#define BBP3_ADC_INIT_MODE		FIELD8(0x80)
 
 /*
  * BBP 4: Bandwidth
@@ -1772,6 +1928,14 @@ struct mac_iveiv_entry {
 #define BBP4_MAC_IF_CTRL		FIELD8(0x40)
 
 /*
+ * BBP 47: Bandwidth
+ */
+#define BBP47_TSSI_REPORT_SEL		FIELD8(0x03)
+#define BBP47_TSSI_UPDATE_REQ		FIELD8(0x04)
+#define BBP47_TSSI_TSSI_MODE		FIELD8(0x18)
+#define BBP47_TSSI_ADC6			FIELD8(0x80)
+
+/*
  * BBP 109
  */
 #define BBP109_TX0_POWER		FIELD8(0x0f)
@@ -1914,6 +2078,16 @@ struct mac_iveiv_entry {
 #define RFCSR27_R4			FIELD8(0x40)
 
 /*
+ * RFCSR 29:
+ */
+#define RFCSR29_ADC6_TEST		FIELD8(0x01)
+#define RFCSR29_ADC6_INT_TEST		FIELD8(0x02)
+#define RFCSR29_RSSI_RESET		FIELD8(0x04)
+#define RFCSR29_RSSI_ON			FIELD8(0x08)
+#define RFCSR29_RSSI_RIP_CTRL		FIELD8(0x30)
+#define RFCSR29_RSSI_GAIN		FIELD8(0xc0)
+
+/*
  * RFCSR 30:
  */
 #define RFCSR30_TX_H20M			FIELD8(0x02)
@@ -1944,6 +2118,11 @@ struct mac_iveiv_entry {
 #define RFCSR49_TX			FIELD8(0x3f)
 
 /*
+ * RFCSR 50:
+ */
+#define RFCSR50_TX			FIELD8(0x3f)
+
+/*
  * RF registers
  */
 
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index dfc90d34be6..88455b1b9fe 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -354,16 +354,15 @@ int rt2800_check_firmware(struct rt2x00_dev *rt2x00dev,
 	 * of 4kb. Certain USB chipsets however require different firmware,
 	 * which Ralink only provides attached to the original firmware
 	 * file. Thus for USB devices, firmware files have a length
-	 * which is a multiple of 4kb.
+	 * which is a multiple of 4kb. The firmware for rt3290 chip also
+	 * have a length which is a multiple of 4kb.
 	 */
-	if (rt2x00_is_usb(rt2x00dev)) {
+	if (rt2x00_is_usb(rt2x00dev) || rt2x00_rt(rt2x00dev, RT3290))
 		fw_len = 4096;
-		multiple = true;
-	} else {
+	else
 		fw_len = 8192;
-		multiple = true;
-	}
 
+	multiple = true;
 	/*
 	 * Validate the firmware length
 	 */
@@ -415,7 +414,8 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
 		return -EBUSY;
 
 	if (rt2x00_is_pci(rt2x00dev)) {
-		if (rt2x00_rt(rt2x00dev, RT3572) ||
+		if (rt2x00_rt(rt2x00dev, RT3290) ||
+		    rt2x00_rt(rt2x00dev, RT3572) ||
 		    rt2x00_rt(rt2x00dev, RT5390) ||
 		    rt2x00_rt(rt2x00dev, RT5392)) {
 			rt2800_register_read(rt2x00dev, AUX_CTRL, &reg);
@@ -851,8 +851,13 @@ int rt2800_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	rt2800_register_read(rt2x00dev, GPIO_CTRL_CFG, &reg);
-	return rt2x00_get_field32(reg, GPIO_CTRL_CFG_BIT2);
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+		return rt2x00_get_field32(reg, WLAN_GPIO_IN_BIT0);
+	} else {
+		rt2800_register_read(rt2x00dev, GPIO_CTRL_CFG, &reg);
+		return rt2x00_get_field32(reg, GPIO_CTRL_CFG_BIT2);
+	}
 }
 EXPORT_SYMBOL_GPL(rt2800_rfkill_poll);
 
@@ -1935,8 +1940,50 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
 	rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 }
 
-#define RT5390_POWER_BOUND     0x27
-#define RT5390_FREQ_OFFSET_BOUND       0x5f
+#define POWER_BOUND		0x27
+#define FREQ_OFFSET_BOUND	0x5f
+
+static void rt2800_config_channel_rf3290(struct rt2x00_dev *rt2x00dev,
+					 struct ieee80211_conf *conf,
+					 struct rf_channel *rf,
+					 struct channel_info *info)
+{
+	u8 rfcsr;
+
+	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 > POWER_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, POWER_BOUND);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, info->default_power1);
+	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
+
+	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+	if (rt2x00dev->freq_offset > FREQ_OFFSET_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, FREQ_OFFSET_BOUND);
+	else
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
+	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+
+	if (rf->channel <= 14) {
+		if (rf->channel == 6)
+			rt2800_bbp_write(rt2x00dev, 68, 0x0c);
+		else
+			rt2800_bbp_write(rt2x00dev, 68, 0x0b);
+
+		if (rf->channel >= 1 && rf->channel <= 6)
+			rt2800_bbp_write(rt2x00dev, 59, 0x0f);
+		else if (rf->channel >= 7 && rf->channel <= 11)
+			rt2800_bbp_write(rt2x00dev, 59, 0x0e);
+		else if (rf->channel >= 12 && rf->channel <= 14)
+			rt2800_bbp_write(rt2x00dev, 59, 0x0d);
+	}
+}
 
 static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 					 struct ieee80211_conf *conf,
@@ -1952,13 +1999,27 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 	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);
+	if (info->default_power1 > POWER_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR49_TX, POWER_BOUND);
 	else
 		rt2x00_set_field8(&rfcsr, RFCSR49_TX, info->default_power1);
 	rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
 
+	if (rt2x00_rt(rt2x00dev, RT5392)) {
+		rt2800_rfcsr_read(rt2x00dev, 50, &rfcsr);
+		if (info->default_power1 > POWER_BOUND)
+			rt2x00_set_field8(&rfcsr, RFCSR50_TX, POWER_BOUND);
+		else
+			rt2x00_set_field8(&rfcsr, RFCSR50_TX,
+					  info->default_power2);
+		rt2800_rfcsr_write(rt2x00dev, 50, rfcsr);
+	}
+
 	rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+	if (rt2x00_rt(rt2x00dev, RT5392)) {
+		rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 1);
+		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
+	}
 	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);
@@ -1966,9 +2027,8 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 	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);
+	if (rt2x00dev->freq_offset > FREQ_OFFSET_BOUND)
+		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, FREQ_OFFSET_BOUND);
 	else
 		rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
 	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
@@ -2021,15 +2081,6 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
 			}
 		}
 	}
-
-	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,
@@ -2039,7 +2090,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 {
 	u32 reg;
 	unsigned int tx_pin;
-	u8 bbp;
+	u8 bbp, rfcsr;
 
 	if (rf->channel <= 14) {
 		info->default_power1 = TXPOWER_G_TO_DEV(info->default_power1);
@@ -2060,15 +2111,36 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	case RF3052:
 		rt2800_config_channel_rf3052(rt2x00dev, conf, rf, info);
 		break;
+	case RF3290:
+		rt2800_config_channel_rf3290(rt2x00dev, conf, rf, info);
+		break;
+	case RF5360:
 	case RF5370:
 	case RF5372:
 	case RF5390:
+	case RF5392:
 		rt2800_config_channel_rf53xx(rt2x00dev, conf, rf, info);
 		break;
 	default:
 		rt2800_config_channel_rf2xxx(rt2x00dev, conf, rf, info);
 	}
 
+	if (rt2x00_rf(rt2x00dev, RF3290) ||
+	    rt2x00_rf(rt2x00dev, RF5360) ||
+	    rt2x00_rf(rt2x00dev, RF5370) ||
+	    rt2x00_rf(rt2x00dev, RF5372) ||
+	    rt2x00_rf(rt2x00dev, RF5390) ||
+	    rt2x00_rf(rt2x00dev, RF5392)) {
+		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);
+	}
+
 	/*
 	 * Change BBP settings
 	 */
@@ -2549,9 +2621,12 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 		rt2x00_set_field8(&rfcsr, RFCSR7_RF_TUNING, 1);
 		rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 		break;
+	case RF3290:
+	case RF5360:
 	case RF5370:
 	case RF5372:
 	case RF5390:
+	case RF5392:
 		rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
 		rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
 		rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
@@ -2682,6 +2757,7 @@ 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, RT3290) ||
 		    rt2x00_rt(rt2x00dev, RT3390) ||
 		    rt2x00_rt(rt2x00dev, RT5390) ||
 		    rt2x00_rt(rt2x00dev, RT5392))
@@ -2778,10 +2854,54 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, BKOFF_SLOT_CFG_CC_DELAY_TIME, 2);
 	rt2800_register_write(rt2x00dev, BKOFF_SLOT_CFG, reg);
 
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+		if (rt2x00_get_field32(reg, WLAN_EN) == 1) {
+			rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 1);
+			rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+		}
+
+		rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
+		if (!(rt2x00_get_field32(reg, LDO0_EN) == 1)) {
+			rt2x00_set_field32(&reg, LDO0_EN, 1);
+			rt2x00_set_field32(&reg, LDO_BGSEL, 3);
+			rt2800_register_write(rt2x00dev, CMB_CTRL, reg);
+		}
+
+		rt2800_register_read(rt2x00dev, OSC_CTRL, &reg);
+		rt2x00_set_field32(&reg, OSC_ROSC_EN, 1);
+		rt2x00_set_field32(&reg, OSC_CAL_REQ, 1);
+		rt2x00_set_field32(&reg, OSC_REF_CYCLE, 0x27);
+		rt2800_register_write(rt2x00dev, OSC_CTRL, reg);
+
+		rt2800_register_read(rt2x00dev, COEX_CFG0, &reg);
+		rt2x00_set_field32(&reg, COEX_CFG_ANT, 0x5e);
+		rt2800_register_write(rt2x00dev, COEX_CFG0, reg);
+
+		rt2800_register_read(rt2x00dev, COEX_CFG2, &reg);
+		rt2x00_set_field32(&reg, BT_COEX_CFG1, 0x00);
+		rt2x00_set_field32(&reg, BT_COEX_CFG0, 0x17);
+		rt2x00_set_field32(&reg, WL_COEX_CFG1, 0x93);
+		rt2x00_set_field32(&reg, WL_COEX_CFG0, 0x7f);
+		rt2800_register_write(rt2x00dev, COEX_CFG2, reg);
+
+		rt2800_register_read(rt2x00dev, PLL_CTRL, &reg);
+		rt2x00_set_field32(&reg, PLL_CONTROL, 1);
+		rt2800_register_write(rt2x00dev, PLL_CTRL, reg);
+	}
+
 	if (rt2x00_rt(rt2x00dev, RT3071) ||
 	    rt2x00_rt(rt2x00dev, RT3090) ||
+	    rt2x00_rt(rt2x00dev, RT3290) ||
 	    rt2x00_rt(rt2x00dev, RT3390)) {
-		rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000400);
+
+		if (rt2x00_rt(rt2x00dev, RT3290))
+			rt2800_register_write(rt2x00dev, TX_SW_CFG0,
+					      0x00000404);
+		else
+			rt2800_register_write(rt2x00dev, TX_SW_CFG0,
+					      0x00000400);
+
 		rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00000000);
 		if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
 		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
@@ -3190,14 +3310,16 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 		     rt2800_wait_bbp_ready(rt2x00dev)))
 		return -EACCES;
 
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392)) {
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392)) {
 		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, RT3290) ||
 	    rt2x00_rt(rt2x00dev, RT3572) ||
 	    rt2x00_rt(rt2x00dev, RT5390) ||
 	    rt2x00_rt(rt2x00dev, RT5392))
@@ -3206,20 +3328,26 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	rt2800_bbp_write(rt2x00dev, 65, 0x2c);
 	rt2800_bbp_write(rt2x00dev, 66, 0x38);
 
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
 		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) ||
-			   rt2x00_rt(rt2x00dev, RT5392)) {
+	} else if (rt2x00_rt(rt2x00dev, RT3290) ||
+		   rt2x00_rt(rt2x00dev, RT5390) ||
+		   rt2x00_rt(rt2x00dev, RT5392)) {
 		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);
+
+		if (rt2x00_rt(rt2x00dev, RT3290))
+			rt2800_bbp_write(rt2x00dev, 77, 0x58);
+		else
+			rt2800_bbp_write(rt2x00dev, 77, 0x59);
 	} else {
 		rt2800_bbp_write(rt2x00dev, 69, 0x12);
 		rt2800_bbp_write(rt2x00dev, 73, 0x10);
@@ -3244,23 +3372,33 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 		rt2800_bbp_write(rt2x00dev, 81, 0x37);
 	}
 
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		rt2800_bbp_write(rt2x00dev, 74, 0x0b);
+		rt2800_bbp_write(rt2x00dev, 79, 0x18);
+		rt2800_bbp_write(rt2x00dev, 80, 0x09);
+		rt2800_bbp_write(rt2x00dev, 81, 0x33);
+	}
+
 	rt2800_bbp_write(rt2x00dev, 82, 0x62);
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
 		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) ||
-			 rt2x00_rt(rt2x00dev, RT5392))
+	else if (rt2x00_rt(rt2x00dev, RT3290) ||
+		     rt2x00_rt(rt2x00dev, RT5390) ||
+		     rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 84, 0x9a);
 	else
 		rt2800_bbp_write(rt2x00dev, 84, 0x99);
 
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 86, 0x38);
 	else
 		rt2800_bbp_write(rt2x00dev, 86, 0x00);
@@ -3270,8 +3408,9 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 
 	rt2800_bbp_write(rt2x00dev, 91, 0x04);
 
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 92, 0x02);
 	else
 		rt2800_bbp_write(rt2x00dev, 92, 0x00);
@@ -3285,6 +3424,7 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	    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, RT3290) ||
 	    rt2x00_rt(rt2x00dev, RT3572) ||
 	    rt2x00_rt(rt2x00dev, RT5390) ||
 	    rt2x00_rt(rt2x00dev, RT5392) ||
@@ -3293,27 +3433,32 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 	else
 		rt2800_bbp_write(rt2x00dev, 103, 0x00);
 
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 104, 0x92);
 
 	if (rt2800_is_305x_soc(rt2x00dev))
 		rt2800_bbp_write(rt2x00dev, 105, 0x01);
+	else if (rt2x00_rt(rt2x00dev, RT3290))
+		rt2800_bbp_write(rt2x00dev, 105, 0x1c);
 	else if (rt2x00_rt(rt2x00dev, RT5390) ||
 			 rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 105, 0x3c);
 	else
 		rt2800_bbp_write(rt2x00dev, 105, 0x05);
 
-	if (rt2x00_rt(rt2x00dev, RT5390))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390))
 		rt2800_bbp_write(rt2x00dev, 106, 0x03);
 	else if (rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 106, 0x12);
 	else
 		rt2800_bbp_write(rt2x00dev, 106, 0x35);
 
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392))
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392))
 		rt2800_bbp_write(rt2x00dev, 128, 0x12);
 
 	if (rt2x00_rt(rt2x00dev, RT5392)) {
@@ -3338,6 +3483,29 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 		rt2800_bbp_write(rt2x00dev, 138, value);
 	}
 
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		rt2800_bbp_write(rt2x00dev, 67, 0x24);
+		rt2800_bbp_write(rt2x00dev, 143, 0x04);
+		rt2800_bbp_write(rt2x00dev, 142, 0x99);
+		rt2800_bbp_write(rt2x00dev, 150, 0x30);
+		rt2800_bbp_write(rt2x00dev, 151, 0x2e);
+		rt2800_bbp_write(rt2x00dev, 152, 0x20);
+		rt2800_bbp_write(rt2x00dev, 153, 0x34);
+		rt2800_bbp_write(rt2x00dev, 154, 0x40);
+		rt2800_bbp_write(rt2x00dev, 155, 0x3b);
+		rt2800_bbp_write(rt2x00dev, 253, 0x04);
+
+		rt2800_bbp_read(rt2x00dev, 47, &value);
+		rt2x00_set_field8(&value, BBP47_TSSI_ADC6, 1);
+		rt2800_bbp_write(rt2x00dev, 47, value);
+
+		/* Use 5-bit ADC for Acquisition and 8-bit ADC for data */
+		rt2800_bbp_read(rt2x00dev, 3, &value);
+		rt2x00_set_field8(&value, BBP3_ADC_MODE_SWITCH, 1);
+		rt2x00_set_field8(&value, BBP3_ADC_INIT_MODE, 1);
+		rt2800_bbp_write(rt2x00dev, 3, value);
+	}
+
 	if (rt2x00_rt(rt2x00dev, RT5390) ||
 		rt2x00_rt(rt2x00dev, RT5392)) {
 		int ant, div_mode;
@@ -3470,6 +3638,7 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 	if (!rt2x00_rt(rt2x00dev, RT3070) &&
 	    !rt2x00_rt(rt2x00dev, RT3071) &&
 	    !rt2x00_rt(rt2x00dev, RT3090) &&
+	    !rt2x00_rt(rt2x00dev, RT3290) &&
 	    !rt2x00_rt(rt2x00dev, RT3390) &&
 	    !rt2x00_rt(rt2x00dev, RT3572) &&
 	    !rt2x00_rt(rt2x00dev, RT5390) &&
@@ -3480,8 +3649,9 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Init RF calibration.
 	 */
-	if (rt2x00_rt(rt2x00dev, RT5390) ||
-		rt2x00_rt(rt2x00dev, RT5392)) {
+	if (rt2x00_rt(rt2x00dev, RT3290) ||
+	    rt2x00_rt(rt2x00dev, RT5390) ||
+	    rt2x00_rt(rt2x00dev, RT5392)) {
 		rt2800_rfcsr_read(rt2x00dev, 2, &rfcsr);
 		rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 1);
 		rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
@@ -3519,6 +3689,53 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 24, 0x16);
 		rt2800_rfcsr_write(rt2x00dev, 25, 0x01);
 		rt2800_rfcsr_write(rt2x00dev, 29, 0x1f);
+	} else if (rt2x00_rt(rt2x00dev, RT3290)) {
+		rt2800_rfcsr_write(rt2x00dev, 1, 0x0f);
+		rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 3, 0x08);
+		rt2800_rfcsr_write(rt2x00dev, 4, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 6, 0xa0);
+		rt2800_rfcsr_write(rt2x00dev, 8, 0xf3);
+		rt2800_rfcsr_write(rt2x00dev, 9, 0x02);
+		rt2800_rfcsr_write(rt2x00dev, 10, 0x53);
+		rt2800_rfcsr_write(rt2x00dev, 11, 0x4a);
+		rt2800_rfcsr_write(rt2x00dev, 12, 0x46);
+		rt2800_rfcsr_write(rt2x00dev, 13, 0x9f);
+		rt2800_rfcsr_write(rt2x00dev, 18, 0x02);
+		rt2800_rfcsr_write(rt2x00dev, 22, 0x20);
+		rt2800_rfcsr_write(rt2x00dev, 25, 0x83);
+		rt2800_rfcsr_write(rt2x00dev, 26, 0x82);
+		rt2800_rfcsr_write(rt2x00dev, 27, 0x09);
+		rt2800_rfcsr_write(rt2x00dev, 29, 0x10);
+		rt2800_rfcsr_write(rt2x00dev, 30, 0x10);
+		rt2800_rfcsr_write(rt2x00dev, 31, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 32, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 33, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 34, 0x05);
+		rt2800_rfcsr_write(rt2x00dev, 35, 0x12);
+		rt2800_rfcsr_write(rt2x00dev, 36, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 38, 0x85);
+		rt2800_rfcsr_write(rt2x00dev, 39, 0x1b);
+		rt2800_rfcsr_write(rt2x00dev, 40, 0x0b);
+		rt2800_rfcsr_write(rt2x00dev, 41, 0xbb);
+		rt2800_rfcsr_write(rt2x00dev, 42, 0xd5);
+		rt2800_rfcsr_write(rt2x00dev, 43, 0x7b);
+		rt2800_rfcsr_write(rt2x00dev, 44, 0x0e);
+		rt2800_rfcsr_write(rt2x00dev, 45, 0xa2);
+		rt2800_rfcsr_write(rt2x00dev, 46, 0x73);
+		rt2800_rfcsr_write(rt2x00dev, 47, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 48, 0x10);
+		rt2800_rfcsr_write(rt2x00dev, 49, 0x98);
+		rt2800_rfcsr_write(rt2x00dev, 52, 0x38);
+		rt2800_rfcsr_write(rt2x00dev, 53, 0x00);
+		rt2800_rfcsr_write(rt2x00dev, 54, 0x78);
+		rt2800_rfcsr_write(rt2x00dev, 55, 0x43);
+		rt2800_rfcsr_write(rt2x00dev, 56, 0x02);
+		rt2800_rfcsr_write(rt2x00dev, 57, 0x80);
+		rt2800_rfcsr_write(rt2x00dev, 58, 0x7f);
+		rt2800_rfcsr_write(rt2x00dev, 59, 0x09);
+		rt2800_rfcsr_write(rt2x00dev, 60, 0x45);
+		rt2800_rfcsr_write(rt2x00dev, 61, 0xc1);
 	} else if (rt2x00_rt(rt2x00dev, RT3390)) {
 		rt2800_rfcsr_write(rt2x00dev, 0, 0xa0);
 		rt2800_rfcsr_write(rt2x00dev, 1, 0xe1);
@@ -3927,6 +4144,12 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
 	}
 
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		rt2800_rfcsr_read(rt2x00dev, 29, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR29_RSSI_GAIN, 3);
+		rt2800_rfcsr_write(rt2x00dev, 29, rfcsr);
+	}
+
 	if (rt2x00_rt(rt2x00dev, RT5390) ||
 		rt2x00_rt(rt2x00dev, RT5392)) {
 		rt2800_rfcsr_read(rt2x00dev, 38, &rfcsr);
@@ -4033,9 +4256,14 @@ EXPORT_SYMBOL_GPL(rt2800_disable_radio);
 int rt2800_efuse_detect(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
+	u16 efuse_ctrl_reg;
 
-	rt2800_register_read(rt2x00dev, EFUSE_CTRL, &reg);
+	if (rt2x00_rt(rt2x00dev, RT3290))
+		efuse_ctrl_reg = EFUSE_CTRL_3290;
+	else
+		efuse_ctrl_reg = EFUSE_CTRL;
 
+	rt2800_register_read(rt2x00dev, efuse_ctrl_reg, &reg);
 	return rt2x00_get_field32(reg, EFUSE_CTRL_PRESENT);
 }
 EXPORT_SYMBOL_GPL(rt2800_efuse_detect);
@@ -4043,27 +4271,44 @@ EXPORT_SYMBOL_GPL(rt2800_efuse_detect);
 static void rt2800_efuse_read(struct rt2x00_dev *rt2x00dev, unsigned int i)
 {
 	u32 reg;
-
+	u16 efuse_ctrl_reg;
+	u16 efuse_data0_reg;
+	u16 efuse_data1_reg;
+	u16 efuse_data2_reg;
+	u16 efuse_data3_reg;
+
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		efuse_ctrl_reg = EFUSE_CTRL_3290;
+		efuse_data0_reg = EFUSE_DATA0_3290;
+		efuse_data1_reg = EFUSE_DATA1_3290;
+		efuse_data2_reg = EFUSE_DATA2_3290;
+		efuse_data3_reg = EFUSE_DATA3_3290;
+	} else {
+		efuse_ctrl_reg = EFUSE_CTRL;
+		efuse_data0_reg = EFUSE_DATA0;
+		efuse_data1_reg = EFUSE_DATA1;
+		efuse_data2_reg = EFUSE_DATA2;
+		efuse_data3_reg = EFUSE_DATA3;
+	}
 	mutex_lock(&rt2x00dev->csr_mutex);
 
-	rt2800_register_read_lock(rt2x00dev, EFUSE_CTRL, &reg);
+	rt2800_register_read_lock(rt2x00dev, efuse_ctrl_reg, &reg);
 	rt2x00_set_field32(&reg, EFUSE_CTRL_ADDRESS_IN, i);
 	rt2x00_set_field32(&reg, EFUSE_CTRL_MODE, 0);
 	rt2x00_set_field32(&reg, EFUSE_CTRL_KICK, 1);
-	rt2800_register_write_lock(rt2x00dev, EFUSE_CTRL, reg);
+	rt2800_register_write_lock(rt2x00dev, efuse_ctrl_reg, reg);
 
 	/* Wait until the EEPROM has been loaded */
-	rt2800_regbusy_read(rt2x00dev, EFUSE_CTRL, EFUSE_CTRL_KICK, &reg);
-
+	rt2800_regbusy_read(rt2x00dev, efuse_ctrl_reg, EFUSE_CTRL_KICK, &reg);
 	/* Apparently the data is read from end to start */
-	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA3, &reg);
+	rt2800_register_read_lock(rt2x00dev, efuse_data3_reg, &reg);
 	/* The returned value is in CPU order, but eeprom is le */
 	*(u32 *)&rt2x00dev->eeprom[i] = cpu_to_le32(reg);
-	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA2, &reg);
+	rt2800_register_read_lock(rt2x00dev, efuse_data2_reg, &reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 2] = cpu_to_le32(reg);
-	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA1, &reg);
+	rt2800_register_read_lock(rt2x00dev, efuse_data1_reg, &reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 4] = cpu_to_le32(reg);
-	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA0, &reg);
+	rt2800_register_read_lock(rt2x00dev, efuse_data0_reg, &reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 6] = cpu_to_le32(reg);
 
 	mutex_unlock(&rt2x00dev->csr_mutex);
@@ -4090,7 +4335,7 @@ int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 */
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
-		random_ether_addr(mac);
+		eth_random_addr(mac);
 		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
@@ -4225,9 +4470,14 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * RT28xx/RT30xx: defined in "EEPROM_NIC_CONF0_RF_TYPE" field
 	 * RT53xx: defined in "EEPROM_CHIP_ID" field
 	 */
-	rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
-	if (rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5390 ||
-		rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5392)
+	if (rt2x00_rt(rt2x00dev, RT3290))
+		rt2800_register_read(rt2x00dev, MAC_CSR0_3290, &reg);
+	else
+		rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
+
+	if (rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT3290 ||
+	    rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5390 ||
+	    rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5392)
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_CHIP_ID, &value);
 	else
 		value = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RF_TYPE);
@@ -4242,6 +4492,7 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	case RT3070:
 	case RT3071:
 	case RT3090:
+	case RT3290:
 	case RT3390:
 	case RT3572:
 	case RT5390:
@@ -4262,10 +4513,13 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	case RF3021:
 	case RF3022:
 	case RF3052:
+	case RF3290:
 	case RF3320:
+	case RF5360:
 	case RF5370:
 	case RF5372:
 	case RF5390:
+	case RF5392:
 		break;
 	default:
 		ERROR(rt2x00dev, "Invalid RF chipset 0x%04x detected.\n",
@@ -4576,10 +4830,13 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 		   rt2x00_rf(rt2x00dev, RF2020) ||
 		   rt2x00_rf(rt2x00dev, RF3021) ||
 		   rt2x00_rf(rt2x00dev, RF3022) ||
+		   rt2x00_rf(rt2x00dev, RF3290) ||
 		   rt2x00_rf(rt2x00dev, RF3320) ||
+		   rt2x00_rf(rt2x00dev, RF5360) ||
 		   rt2x00_rf(rt2x00dev, RF5370) ||
 		   rt2x00_rf(rt2x00dev, RF5372) ||
-		   rt2x00_rf(rt2x00dev, RF5390)) {
+		   rt2x00_rf(rt2x00dev, RF5390) ||
+		   rt2x00_rf(rt2x00dev, RF5392)) {
 		spec->num_channels = 14;
 		spec->channels = rf_vals_3x;
 	} else if (rt2x00_rf(rt2x00dev, RF3052)) {
@@ -4662,9 +4919,12 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	case RF3022:
 	case RF3320:
 	case RF3052:
+	case RF3290:
+	case RF5360:
 	case RF5370:
 	case RF5372:
 	case RF5390:
+	case RF5392:
 		__set_bit(CAPABILITY_VCO_RECALIBRATION, &rt2x00dev->cap_flags);
 		break;
 	}
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index cad25bfebd7..235376e9cb0 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -280,7 +280,13 @@ static void rt2800pci_stop_queue(struct data_queue *queue)
  */
 static char *rt2800pci_get_firmware_name(struct rt2x00_dev *rt2x00dev)
 {
-	return FIRMWARE_RT2860;
+	/*
+	 * Chip rt3290 use specific 4KB firmware named rt3290.bin.
+	 */
+	if (rt2x00_rt(rt2x00dev, RT3290))
+		return FIRMWARE_RT3290;
+	else
+		return FIRMWARE_RT2860;
 }
 
 static int rt2800pci_write_firmware(struct rt2x00_dev *rt2x00dev,
@@ -974,6 +980,66 @@ static int rt2800pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	return rt2800_validate_eeprom(rt2x00dev);
 }
 
+static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	int i, count;
+
+	rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+	if (rt2x00_get_field32(reg, WLAN_EN))
+		return 0;
+
+	rt2x00_set_field32(&reg, WLAN_GPIO_OUT_OE_BIT_ALL, 0xff);
+	rt2x00_set_field32(&reg, FRC_WL_ANT_SET, 1);
+	rt2x00_set_field32(&reg, WLAN_CLK_EN, 0);
+	rt2x00_set_field32(&reg, WLAN_EN, 1);
+	rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+
+	udelay(REGISTER_BUSY_DELAY);
+
+	count = 0;
+	do {
+		/*
+		 * Check PLL_LD & XTAL_RDY.
+		 */
+		for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+			rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
+			if (rt2x00_get_field32(reg, PLL_LD) &&
+			    rt2x00_get_field32(reg, XTAL_RDY))
+				break;
+			udelay(REGISTER_BUSY_DELAY);
+		}
+
+		if (i >= REGISTER_BUSY_COUNT) {
+
+			if (count >= 10)
+				return -EIO;
+
+			rt2800_register_write(rt2x00dev, 0x58, 0x018);
+			udelay(REGISTER_BUSY_DELAY);
+			rt2800_register_write(rt2x00dev, 0x58, 0x418);
+			udelay(REGISTER_BUSY_DELAY);
+			rt2800_register_write(rt2x00dev, 0x58, 0x618);
+			udelay(REGISTER_BUSY_DELAY);
+			count++;
+		} else {
+			count = 0;
+		}
+
+		rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+		rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 0);
+		rt2x00_set_field32(&reg, WLAN_CLK_EN, 1);
+		rt2x00_set_field32(&reg, WLAN_RESET, 1);
+		rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+		udelay(10);
+		rt2x00_set_field32(&reg, WLAN_RESET, 0);
+		rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+		udelay(10);
+		rt2800_register_write(rt2x00dev, INT_SOURCE_CSR, 0x7fffffff);
+	} while (count != 0);
+
+	return 0;
+}
 static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 {
 	int retval;
@@ -997,6 +1063,17 @@ static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 		return retval;
 
 	/*
+	 * In probe phase call rt2800_enable_wlan_rt3290 to enable wlan
+	 * clk for rt3290. That avoid the MCU fail in start phase.
+	 */
+	if (rt2x00_rt(rt2x00dev, RT3290)) {
+		retval = rt2800_enable_wlan_rt3290(rt2x00dev);
+
+		if (retval)
+			return retval;
+	}
+
+	/*
 	 * This device has multiple filters for control frames
 	 * and has a separate filter for PS Poll frames.
 	 */
@@ -1175,6 +1252,9 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
 	{ PCI_DEVICE(0x1432, 0x7768) },
 	{ PCI_DEVICE(0x1462, 0x891a) },
 	{ PCI_DEVICE(0x1a3b, 0x1059) },
+#ifdef CONFIG_RT2800PCI_RT3290
+	{ PCI_DEVICE(0x1814, 0x3290) },
+#endif
 #ifdef CONFIG_RT2800PCI_RT33XX
 	{ PCI_DEVICE(0x1814, 0x3390) },
 #endif
@@ -1188,6 +1268,7 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
 	{ PCI_DEVICE(0x1814, 0x3593) },
 #endif
 #ifdef CONFIG_RT2800PCI_RT53XX
+	{ PCI_DEVICE(0x1814, 0x5360) },
 	{ PCI_DEVICE(0x1814, 0x5362) },
 	{ PCI_DEVICE(0x1814, 0x5390) },
 	{ PCI_DEVICE(0x1814, 0x5392) },
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.h b/drivers/net/wireless/rt2x00/rt2800pci.h
index 70e050d904c..ab22a087c50 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.h
+++ b/drivers/net/wireless/rt2x00/rt2800pci.h
@@ -47,6 +47,7 @@
  * 8051 firmware image.
  */
 #define FIRMWARE_RT2860			"rt2860.bin"
+#define FIRMWARE_RT3290			"rt3290.bin"
 #define FIRMWARE_IMAGE_BASE		0x2000
 
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index bf78317a6ad..6cf336595e2 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -971,6 +971,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x0411, 0x015d) },
 	{ USB_DEVICE(0x0411, 0x016f) },
 	{ USB_DEVICE(0x0411, 0x01a2) },
+	{ USB_DEVICE(0x0411, 0x01ee) },
 	/* Corega */
 	{ USB_DEVICE(0x07aa, 0x002f) },
 	{ USB_DEVICE(0x07aa, 0x003c) },
@@ -1137,6 +1138,8 @@ static struct usb_device_id rt2800usb_device_table[] = {
 #ifdef CONFIG_RT2800USB_RT33XX
 	/* Belkin */
 	{ USB_DEVICE(0x050d, 0x945b) },
+	/* D-Link */
+	{ USB_DEVICE(0x2001, 0x3c17) },
 	/* Panasonic */
 	{ USB_DEVICE(0x083a, 0xb511) },
 	/* Philips */
@@ -1237,7 +1240,6 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	/* D-Link */
 	{ USB_DEVICE(0x07d1, 0x3c0b) },
 	{ USB_DEVICE(0x07d1, 0x3c17) },
-	{ USB_DEVICE(0x2001, 0x3c17) },
 	/* Encore */
 	{ USB_DEVICE(0x203d, 0x14a1) },
 	/* Gemtek */
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 8f754025b06..8afb546c2b2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -187,6 +187,7 @@ struct rt2x00_chip {
 #define RT3070		0x3070
 #define RT3071		0x3071
 #define RT3090		0x3090	/* 2.4GHz PCIe */
+#define RT3290		0x3290
 #define RT3390		0x3390
 #define RT3572		0x3572
 #define RT3593		0x3593
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index e7361d913e8..49a63e97393 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -102,7 +102,7 @@ void rt2x00lib_config_erp(struct rt2x00_dev *rt2x00dev,
 
 	/* Update the AID, this is needed for dynamic PS support */
 	rt2x00dev->aid = bss_conf->assoc ? bss_conf->aid : 0;
-	rt2x00dev->last_beacon = bss_conf->last_tsf;
+	rt2x00dev->last_beacon = bss_conf->sync_tsf;
 
 	/* Update global beacon interval time, this is needed for PS support */
 	rt2x00dev->beacon_int = bss_conf->beacon_int;
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index e5404e57625..a6b88bd4a1a 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -1161,6 +1161,8 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 		    BIT(NL80211_IFTYPE_MESH_POINT) |
 		    BIT(NL80211_IFTYPE_WDS);
 
+	rt2x00dev->hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
+
 	/*
 	 * Initialize work.
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index dd24b2663b5..4ff26c2159b 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -506,9 +506,19 @@ int rt2x00mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
 		return 0;
-	else if (!test_bit(CAPABILITY_HW_CRYPTO, &rt2x00dev->cap_flags))
+
+	if (!test_bit(CAPABILITY_HW_CRYPTO, &rt2x00dev->cap_flags))
+		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;
-	else if (key->keylen > 32)
+
+	if (key->keylen > 32)
 		return -ENOSPC;
 
 	memset(&crypto, 0, sizeof(crypto));
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 0a4653a92ca..a0c8caef3b0 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -256,6 +256,7 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct rt2x00_ops *ops)
 	struct ieee80211_hw *hw;
 	struct rt2x00_dev *rt2x00dev;
 	int retval;
+	u16 chip;
 
 	retval = pci_enable_device(pci_dev);
 	if (retval) {
@@ -305,6 +306,14 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct rt2x00_ops *ops)
 	if (retval)
 		goto exit_free_device;
 
+	/*
+	 * Because rt3290 chip use different efuse offset to read efuse data.
+	 * So before read efuse it need to indicate it is the
+	 * rt3290 or not.
+	 */
+	pci_read_config_word(pci_dev, PCI_DEVICE_ID, &chip);
+	rt2x00dev->chip.rt = chip;
+
 	retval = rt2x00lib_probe_dev(rt2x00dev);
 	if (retval)
 		goto exit_free_reg;
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 2fd83010341..f7e74a0a775 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -774,9 +774,7 @@ int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
 bool rt2x00queue_for_each_entry(struct data_queue *queue,
 				enum queue_index start,
 				enum queue_index end,
-				void *data,
-				bool (*fn)(struct queue_entry *entry,
-					   void *data))
+				bool (*fn)(struct queue_entry *entry))
 {
 	unsigned long irqflags;
 	unsigned int index_start;
@@ -807,17 +805,17 @@ bool rt2x00queue_for_each_entry(struct data_queue *queue,
 	 */
 	if (index_start < index_end) {
 		for (i = index_start; i < index_end; i++) {
-			if (fn(&queue->entries[i], data))
+			if (fn(&queue->entries[i]))
 				return true;
 		}
 	} else {
 		for (i = index_start; i < queue->limit; i++) {
-			if (fn(&queue->entries[i], data))
+			if (fn(&queue->entries[i]))
 				return true;
 		}
 
 		for (i = 0; i < index_end; i++) {
-			if (fn(&queue->entries[i], data))
+			if (fn(&queue->entries[i]))
 				return true;
 		}
 	}
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index 5f1392c7267..9b8c10a86de 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -584,7 +584,6 @@ struct data_queue_desc {
  * @queue: Pointer to @data_queue
  * @start: &enum queue_index Pointer to start index
  * @end: &enum queue_index Pointer to end index
- * @data: Data to pass to the callback function
  * @fn: The function to call for each &struct queue_entry
  *
  * This will walk through all entries in the queue, in chronological
@@ -597,9 +596,7 @@ struct data_queue_desc {
 bool rt2x00queue_for_each_entry(struct data_queue *queue,
 				enum queue_index start,
 				enum queue_index end,
-				void *data,
-				bool (*fn)(struct queue_entry *entry,
-					   void *data));
+				bool (*fn)(struct queue_entry *entry));
 
 /**
  * rt2x00queue_empty - Check if the queue is empty.
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index d357d1ed92f..40ea80725a9 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -285,7 +285,7 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 		queue_work(rt2x00dev->workqueue, &rt2x00dev->txdone_work);
 }
 
-static bool rt2x00usb_kick_tx_entry(struct queue_entry *entry, void* data)
+static bool rt2x00usb_kick_tx_entry(struct queue_entry *entry)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
@@ -390,7 +390,7 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
 	queue_work(rt2x00dev->workqueue, &rt2x00dev->rxdone_work);
 }
 
-static bool rt2x00usb_kick_rx_entry(struct queue_entry *entry, void* data)
+static bool rt2x00usb_kick_rx_entry(struct queue_entry *entry)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
@@ -427,18 +427,12 @@ void rt2x00usb_kick_queue(struct data_queue *queue)
 	case QID_AC_BE:
 	case QID_AC_BK:
 		if (!rt2x00queue_empty(queue))
-			rt2x00queue_for_each_entry(queue,
-						   Q_INDEX_DONE,
-						   Q_INDEX,
-						   NULL,
+			rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX,
 						   rt2x00usb_kick_tx_entry);
 		break;
 	case QID_RX:
 		if (!rt2x00queue_full(queue))
-			rt2x00queue_for_each_entry(queue,
-						   Q_INDEX_DONE,
-						   Q_INDEX,
-						   NULL,
+			rt2x00queue_for_each_entry(queue, Q_INDEX, Q_INDEX_DONE,
 						   rt2x00usb_kick_rx_entry);
 		break;
 	default:
@@ -447,7 +441,7 @@ void rt2x00usb_kick_queue(struct data_queue *queue)
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_kick_queue);
 
-static bool rt2x00usb_flush_entry(struct queue_entry *entry, void* data)
+static bool rt2x00usb_flush_entry(struct queue_entry *entry)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
 	struct queue_entry_priv_usb *entry_priv = entry->priv_data;
@@ -474,7 +468,7 @@ void rt2x00usb_flush_queue(struct data_queue *queue, bool drop)
 	unsigned int i;
 
 	if (drop)
-		rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX, NULL,
+		rt2x00queue_for_each_entry(queue, Q_INDEX_DONE, Q_INDEX,
 					   rt2x00usb_flush_entry);
 
 	/*
@@ -565,7 +559,7 @@ void rt2x00usb_clear_entry(struct queue_entry *entry)
 	entry->flags = 0;
 
 	if (entry->queue->qid == QID_RX)
-		rt2x00usb_kick_rx_entry(entry, NULL);
+		rt2x00usb_kick_rx_entry(entry);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_clear_entry);
 
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index ee22bd74579..f32259686b4 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -2415,7 +2415,7 @@ static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 */
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
-		random_ether_addr(mac);
+		eth_random_addr(mac);
 		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 77ccbbc7da4..ba6e434b859 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -1770,7 +1770,7 @@ static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 	 */
 	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
 	if (!is_valid_ether_addr(mac)) {
-		random_ether_addr(mac);
+		eth_random_addr(mac);
 		EEPROM(rt2x00dev, "MAC: %pM\n", mac);
 	}
 
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 2bebcb71a1e..aceaf689f73 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -47,6 +47,8 @@ static DEFINE_PCI_DEVICE_TABLE(rtl8180_table) = {
 	{ PCI_DEVICE(0x1799, 0x6001) },
 	{ PCI_DEVICE(0x1799, 0x6020) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_DLINK, 0x3300) },
+	{ PCI_DEVICE(0x1186, 0x3301) },
+	{ PCI_DEVICE(0x1432, 0x7106) },
 	{ }
 };
 
@@ -1076,7 +1078,7 @@ static int __devinit rtl8180_probe(struct pci_dev *pdev,
 	if (!is_valid_ether_addr(mac_addr)) {
 		printk(KERN_WARNING "%s (rtl8180): Invalid hwaddr! Using"
 		       " randomly generated MAC addr\n", pci_name(pdev));
-		random_ether_addr(mac_addr);
+		eth_random_addr(mac_addr);
 	}
 	SET_IEEE80211_PERM_ADDR(dev, mac_addr);
 
diff --git a/drivers/net/wireless/rtl818x/rtl8187/dev.c b/drivers/net/wireless/rtl818x/rtl8187/dev.c
index 4fb1ca1b86b..71a30b02608 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/dev.c
@@ -1486,7 +1486,7 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	if (!is_valid_ether_addr(mac_addr)) {
 		printk(KERN_WARNING "rtl8187: Invalid hwaddr! Using randomly "
 		       "generated MAC address\n");
-		random_ether_addr(mac_addr);
+		eth_random_addr(mac_addr);
 	}
 	SET_IEEE80211_PERM_ADDR(dev, mac_addr);
 
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index f4c852c6749..942e56b77b6 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -167,7 +167,7 @@ static const u8 tid_to_ac[] = {
 	0, /* IEEE80211_AC_VO */
 };
 
-u8 rtl_tid_to_ac(struct ieee80211_hw *hw, u8 tid)
+u8 rtl_tid_to_ac(u8 tid)
 {
 	return tid_to_ac[tid];
 }
@@ -907,7 +907,7 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	__le16 fc = hdr->frame_control;
-	u8 *act = (u8 *) (((u8 *) skb->data + MAC80211_3ADDR_LEN));
+	u8 *act = (u8 *)skb->data + MAC80211_3ADDR_LEN;
 	u8 category;
 
 	if (!ieee80211_is_action(fc))
diff --git a/drivers/net/wireless/rtlwifi/base.h b/drivers/net/wireless/rtlwifi/base.h
index 5a23a6d0f49..f35af0fdaaf 100644
--- a/drivers/net/wireless/rtlwifi/base.h
+++ b/drivers/net/wireless/rtlwifi/base.h
@@ -138,7 +138,7 @@ int rtl_send_smps_action(struct ieee80211_hw *hw,
 		enum ieee80211_smps_mode smps);
 u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie);
 void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
-u8 rtl_tid_to_ac(struct ieee80211_hw *hw, u8 tid);
+u8 rtl_tid_to_ac(u8 tid);
 extern struct attribute_group rtl_attribute_group;
 int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 			 bool isht, u8 desc_rate, bool first_ampdu);
diff --git a/drivers/net/wireless/rtlwifi/cam.c b/drivers/net/wireless/rtlwifi/cam.c
index 3d8cc4a0c86..5b4b4d4eaf9 100644
--- a/drivers/net/wireless/rtlwifi/cam.c
+++ b/drivers/net/wireless/rtlwifi/cam.c
@@ -128,7 +128,7 @@ u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
 	u32 us_config;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
 		 "EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, ulUseDK=%x MacAddr %pM\n",
 		 ul_entry_idx, ul_key_id, ul_enc_alg,
 		 ul_default_key, mac_addr);
@@ -146,7 +146,7 @@ u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
 	}
 
 	rtl_cam_program_entry(hw, ul_entry_idx, mac_addr,
-			      (u8 *) key_content, us_config);
+			      key_content, us_config);
 
 	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "<===\n");
 
@@ -342,7 +342,8 @@ void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
 			/* Remove from HW Security CAM */
 			memset(rtlpriv->sec.hwsec_cam_sta_addr[i], 0, ETH_ALEN);
 			rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
-			pr_info("&&&&&&&&&del entry %d\n", i);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "del CAM entry %d\n", i);
 		}
 	}
 	return;
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index 278e9f957e0..a18ad2a9893 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -680,7 +680,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 
 		mac->short_preamble = bss_conf->use_short_preamble;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACK_PREAMBLE,
-					      (u8 *) (&mac->short_preamble));
+					      &mac->short_preamble);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -693,7 +693,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 			mac->slot_time = RTL_SLOT_TIME_20;
 
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-					      (u8 *) (&mac->slot_time));
+					      &mac->slot_time);
 	}
 
 	if (changed & BSS_CHANGED_HT) {
@@ -713,7 +713,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		rcu_read_unlock();
 
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
-					      (u8 *) (&mac->max_mss_density));
+					      &mac->max_mss_density);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_FACTOR,
 					      &mac->current_ampdu_factor);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_MIN_SPACE,
@@ -801,7 +801,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				u8 mstatus = RT_MEDIA_CONNECT;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_H2C_FW_JOINBSSRPT,
-						      (u8 *) (&mstatus));
+						      &mstatus);
 				ppsc->report_linked = true;
 			}
 		} else {
@@ -809,7 +809,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				u8 mstatus = RT_MEDIA_DISCONNECT;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_H2C_FW_JOINBSSRPT,
-						      (u8 *)(&mstatus));
+						      &mstatus);
 				ppsc->report_linked = false;
 			}
 		}
@@ -836,7 +836,7 @@ static void rtl_op_set_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
 
 	mac->tsf = tsf;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&bibss));
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, &bibss);
 }
 
 static void rtl_op_reset_tsf(struct ieee80211_hw *hw,
@@ -845,7 +845,7 @@ static void rtl_op_reset_tsf(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmp = 0;
 
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, (u8 *) (&tmp));
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, &tmp);
 }
 
 static void rtl_op_sta_notify(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rtlwifi/efuse.c b/drivers/net/wireless/rtlwifi/efuse.c
index 1f143800a8d..8e2f9afb125 100644
--- a/drivers/net/wireless/rtlwifi/efuse.c
+++ b/drivers/net/wireless/rtlwifi/efuse.c
@@ -352,7 +352,7 @@ void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_BYTES,
 				      (u8 *)&efuse_utilized);
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_USAGE,
-				      (u8 *)&efuse_usage);
+				      &efuse_usage);
 done:
 	for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++)
 		kfree(efuse_word[i]);
@@ -409,7 +409,7 @@ void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
 	else if (type == 2)
 		efuse_shadow_read_2byte(hw, offset, (u16 *) value);
 	else if (type == 4)
-		efuse_shadow_read_4byte(hw, offset, (u32 *) value);
+		efuse_shadow_read_4byte(hw, offset, value);
 
 }
 
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index 2062ea1d7c8..80f75d3ba84 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -480,7 +480,7 @@ static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
 
 	/* we juse use em for BE/BK/VI/VO */
 	for (tid = 7; tid >= 0; tid--) {
-		u8 hw_queue = ac_to_hwq[rtl_tid_to_ac(hw, tid)];
+		u8 hw_queue = ac_to_hwq[rtl_tid_to_ac(tid)];
 		struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
 		while (!mac->act_scanning &&
 		       rtlpriv->psc.rfpwr_state == ERFON) {
@@ -756,10 +756,10 @@ done:
 		if (index == rtlpci->rxringcount - 1)
 			rtlpriv->cfg->ops->set_desc((u8 *)pdesc, false,
 						    HW_DESC_RXERO,
-						    (u8 *)&tmp_one);
+						    &tmp_one);
 
 		rtlpriv->cfg->ops->set_desc((u8 *)pdesc, false, HW_DESC_RXOWN,
-					    (u8 *)&tmp_one);
+					    &tmp_one);
 
 		index = (index + 1) % rtlpci->rxringcount;
 	}
@@ -934,7 +934,7 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	__skb_queue_tail(&ring->queue, pskb);
 
 	rtlpriv->cfg->ops->set_desc((u8 *) pdesc, true, HW_DESC_OWN,
-				    (u8 *)&temp_one);
+				    &temp_one);
 
 	return;
 }
@@ -1126,11 +1126,11 @@ static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw)
 						    rxbuffersize);
 			rtlpriv->cfg->ops->set_desc((u8 *) entry, false,
 						    HW_DESC_RXOWN,
-						    (u8 *)&tmp_one);
+						    &tmp_one);
 		}
 
 		rtlpriv->cfg->ops->set_desc((u8 *) entry, false,
-					    HW_DESC_RXERO, (u8 *)&tmp_one);
+					    HW_DESC_RXERO, &tmp_one);
 	}
 	return 0;
 }
@@ -1263,7 +1263,7 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 				rtlpriv->cfg->ops->set_desc((u8 *) entry,
 							    false,
 							    HW_DESC_RXOWN,
-							    (u8 *)&tmp_one);
+							    &tmp_one);
 			}
 			rtlpci->rx_ring[rx_queue_idx].idx = 0;
 		}
@@ -1273,17 +1273,18 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 	 *after reset, release previous pending packet,
 	 *and force the  tx idx to the first one
 	 */
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
 		if (rtlpci->tx_ring[i].desc) {
 			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
 
 			while (skb_queue_len(&ring->queue)) {
-				struct rtl_tx_desc *entry =
-				    &ring->desc[ring->idx];
-				struct sk_buff *skb =
-				    __skb_dequeue(&ring->queue);
+				struct rtl_tx_desc *entry;
+				struct sk_buff *skb;
 
+				spin_lock_irqsave(&rtlpriv->locks.irq_th_lock,
+						  flags);
+				entry = &ring->desc[ring->idx];
+				skb = __skb_dequeue(&ring->queue);
 				pci_unmap_single(rtlpci->pdev,
 						 rtlpriv->cfg->ops->
 							 get_desc((u8 *)
@@ -1291,15 +1292,15 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 							 true,
 							 HW_DESC_TXBUFF_ADDR),
 						 skb->len, PCI_DMA_TODEVICE);
-				kfree_skb(skb);
 				ring->idx = (ring->idx + 1) % ring->entries;
+				spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
+						  flags);
+				kfree_skb(skb);
 			}
 			ring->idx = 0;
 		}
 	}
 
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
 	return 0;
 }
 
@@ -1422,7 +1423,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 	__skb_queue_tail(&ring->queue, skb);
 
 	rtlpriv->cfg->ops->set_desc((u8 *)pdesc, true,
-				    HW_DESC_OWN, (u8 *)&temp_one);
+				    HW_DESC_OWN, &temp_one);
 
 
 	if ((ring->entries - skb_queue_len(&ring->queue)) < 2 &&
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index 5ae26647f34..13ad33e8557 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -333,10 +333,10 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 			rpwm_val = 0x0C;	/* RF on */
 			fw_pwrmode = FW_PS_ACTIVE_MODE;
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-					(u8 *) (&rpwm_val));
+					&rpwm_val);
 			rtlpriv->cfg->ops->set_hw_reg(hw,
 					HW_VAR_H2C_FW_PWRMODE,
-					(u8 *) (&fw_pwrmode));
+					&fw_pwrmode);
 			fw_current_inps = false;
 
 			rtlpriv->cfg->ops->set_hw_reg(hw,
@@ -356,11 +356,11 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 						(u8 *) (&fw_current_inps));
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						HW_VAR_H2C_FW_PWRMODE,
-						(u8 *) (&ppsc->fwctrl_psmode));
+						&ppsc->fwctrl_psmode);
 
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						HW_VAR_SET_RPWM,
-						(u8 *) (&rpwm_val));
+						&rpwm_val);
 			} else {
 				/* Reset the power save related parameters. */
 				ppsc->dot11_psmode = EACTIVE;
@@ -446,7 +446,7 @@ void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *) data;
+	struct ieee80211_hdr *hdr = data;
 	struct ieee80211_tim_ie *tim_ie;
 	u8 *tim;
 	u8 tim_len;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
index f7f48c7ac85..a45afda8259 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -656,9 +656,8 @@ static void rtl92c_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	} 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->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      &tmp);
 			rtlpriv->dm.current_turbo_edca = false;
 		}
 	}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 692c8ef5ee8..44febfde949 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -168,7 +168,7 @@ 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));
-	u8 *bufferPtr = (u8 *) buffer;
+	u8 *bufferPtr = buffer;
 
 	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes\n", size);
 
@@ -262,7 +262,7 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 		return 1;
 
 	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
-	pfwdata = (u8 *) rtlhal->pfirmware;
+	pfwdata = rtlhal->pfirmware;
 	fwsize = rtlhal->fwsize;
 
 	if (IS_FW_HEADER_EXIST(pfwheader)) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index 5c4d9bc040f..bd0da7ef290 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -214,13 +214,13 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 							      HW_VAR_AC_PARAM,
-							      (u8 *) (&e_aci));
+							      &e_aci);
 			}
 			break;
 		}
 	case HW_VAR_ACK_PREAMBLE:{
 			u8 reg_tmp;
-			u8 short_preamble = (bool) (*(u8 *) val);
+			u8 short_preamble = (bool)*val;
 			reg_tmp = (mac->cur_40_prime_sc) << 5;
 			if (short_preamble)
 				reg_tmp |= 0x80;
@@ -232,7 +232,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			u8 min_spacing_to_set;
 			u8 sec_min_space;
 
-			min_spacing_to_set = *((u8 *) val);
+			min_spacing_to_set = *val;
 			if (min_spacing_to_set <= 7) {
 				sec_min_space = 0;
 
@@ -257,7 +257,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_SHORTGI_DENSITY:{
 			u8 density_to_set;
 
-			density_to_set = *((u8 *) val);
+			density_to_set = *val;
 			mac->min_space_cfg |= (density_to_set << 3);
 
 			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
@@ -284,7 +284,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			else
 				p_regtoset = regtoset_normal;
 
-			factor_toset = *((u8 *) val);
+			factor_toset = *(val);
 			if (factor_toset <= 3) {
 				factor_toset = (1 << (factor_toset + 2));
 				if (factor_toset > 0xf)
@@ -316,17 +316,17 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_AC_PARAM:{
-			u8 e_aci = *((u8 *) val);
+			u8 e_aci = *(val);
 			rtl92c_dm_init_edca_turbo(hw);
 
 			if (rtlpci->acm_method != eAcmWay2_SW)
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 							      HW_VAR_ACM_CTRL,
-							      (u8 *) (&e_aci));
+							      (&e_aci));
 			break;
 		}
 	case HW_VAR_ACM_CTRL:{
-			u8 e_aci = *((u8 *) val);
+			u8 e_aci = *(val);
 			union aci_aifsn *p_aci_aifsn =
 			    (union aci_aifsn *)(&(mac->ac[0].aifs));
 			u8 acm = p_aci_aifsn->f.acm;
@@ -382,7 +382,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_RETRY_LIMIT:{
-			u8 retry_limit = ((u8 *) (val))[0];
+			u8 retry_limit = val[0];
 
 			rtl_write_word(rtlpriv, REG_RL,
 				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
@@ -396,13 +396,13 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtlefuse->efuse_usedbytes = *((u16 *) val);
 		break;
 	case HW_VAR_EFUSE_USAGE:
-		rtlefuse->efuse_usedpercentage = *((u8 *) val);
+		rtlefuse->efuse_usedpercentage = *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));
+		rtl_write_byte(rtlpriv, REG_SECCFG, *val);
 		break;
 	case HW_VAR_SET_RPWM:{
 			u8 rpwm_val;
@@ -411,31 +411,30 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			udelay(1);
 
 			if (rpwm_val & BIT(7)) {
-				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
-					       (*(u8 *) val));
+				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, *val);
 			} else {
 				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
-					       ((*(u8 *) val) | BIT(7)));
+					       *val | BIT(7));
 			}
 
 			break;
 		}
 	case HW_VAR_H2C_FW_PWRMODE:{
-			u8 psmode = (*(u8 *) val);
+			u8 psmode = *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));
+			rtl92c_set_fw_pwrmode_cmd(hw, *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 mstatus = *val;
 			u8 tmp_regcr, tmp_reg422;
 			bool recover = false;
 
@@ -472,7 +471,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				rtl_write_byte(rtlpriv, REG_CR + 1,
 					       (tmp_regcr & ~(BIT(0))));
 			}
-			rtl92c_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
+			rtl92c_set_fw_joinbss_report_cmd(hw, *val);
 
 			break;
 		}
@@ -486,7 +485,7 @@ void rtl92ce_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_CORRECT_TSF:{
-			u8 btype_ibss = ((u8 *) (val))[0];
+			u8 btype_ibss = val[0];
 
 			if (btype_ibss)
 				_rtl92ce_stop_tx_beacon(hw);
@@ -1589,10 +1588,10 @@ static void _rtl92ce_read_adapter_info(struct ieee80211_hw *hw)
 						 rtlefuse->autoload_failflag,
 						 hwinfo);
 
-	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
+	rtlefuse->eeprom_channelplan = *&hwinfo[EEPROM_CHANNELPLAN];
 	rtlefuse->eeprom_version = *(u16 *)&hwinfo[EEPROM_VERSION];
 	rtlefuse->txpwr_fromeprom = true;
-	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+	rtlefuse->eeprom_oemid = *&hwinfo[EEPROM_CUSTOMER_ID];
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
@@ -1939,7 +1938,7 @@ void rtl92ce_update_channel_access_setting(struct ieee80211_hw *hw)
 	u16 sifs_timer;
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-				      (u8 *)&mac->slot_time);
+				      &mac->slot_time);
 	if (!mac->ht_enable)
 		sifs_timer = 0x0a0a;
 	else
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index 3af874e6959..52166640f16 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -605,7 +605,7 @@ void rtl92ce_tx_fill_desc(struct ieee80211_hw *hw,
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	bool defaultadapter = true;
 	struct ieee80211_sta *sta;
-	u8 *pdesc = (u8 *) pdesc_tx;
+	u8 *pdesc = pdesc_tx;
 	u16 seq_number;
 	__le16 fc = hdr->frame_control;
 	u8 fw_qsel = _rtl92ce_map_hwqueue_to_fwqueue(skb, hw_queue);
@@ -806,7 +806,7 @@ void rtl92ce_tx_fill_cmddesc(struct ieee80211_hw *hw,
 
 	SET_TX_DESC_OWN(pdesc, 1);
 
-	SET_TX_DESC_PKT_SIZE((u8 *) pdesc, (u16) (skb->len));
+	SET_TX_DESC_PKT_SIZE(pdesc, (u16) (skb->len));
 
 	SET_TX_DESC_FIRST_SEG(pdesc, 1);
 	SET_TX_DESC_LAST_SEG(pdesc, 1);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 0c74d4f2eeb..4bbb711a36c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -381,11 +381,11 @@ static void _rtl92cu_read_adapter_info(struct ieee80211_hw *hw)
 	rtlefuse->eeprom_did = le16_to_cpu(*(__le16 *)&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_channelplan = hwinfo[EEPROM_CHANNELPLAN];
 	rtlefuse->eeprom_version =
 			 le16_to_cpu(*(__le16 *)&hwinfo[EEPROM_VERSION]);
 	rtlefuse->txpwr_fromeprom = true;
-	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+	rtlefuse->eeprom_oemid = 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) {
@@ -1660,7 +1660,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				for (e_aci = 0; e_aci < AC_MAX; e_aci++)
 					rtlpriv->cfg->ops->set_hw_reg(hw,
 								HW_VAR_AC_PARAM,
-								(u8 *)(&e_aci));
+								&e_aci);
 			} else {
 				u8 sifstime = 0;
 				u8	u1bAIFS;
@@ -1685,7 +1685,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		}
 	case HW_VAR_ACK_PREAMBLE:{
 			u8 reg_tmp;
-			u8 short_preamble = (bool) (*(u8 *) val);
+			u8 short_preamble = (bool)*val;
 			reg_tmp = 0;
 			if (short_preamble)
 				reg_tmp |= 0x80;
@@ -1696,7 +1696,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			u8 min_spacing_to_set;
 			u8 sec_min_space;
 
-			min_spacing_to_set = *((u8 *) val);
+			min_spacing_to_set = *val;
 			if (min_spacing_to_set <= 7) {
 				switch (rtlpriv->sec.pairwise_enc_algorithm) {
 				case NO_ENCRYPTION:
@@ -1729,7 +1729,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_SHORTGI_DENSITY:{
 			u8 density_to_set;
 
-			density_to_set = *((u8 *) val);
+			density_to_set = *val;
 			density_to_set &= 0x1f;
 			mac->min_space_cfg &= 0x07;
 			mac->min_space_cfg |= (density_to_set << 3);
@@ -1747,7 +1747,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			u8 index = 0;
 
 			p_regtoset = regtoset_normal;
-			factor_toset = *((u8 *) val);
+			factor_toset = *val;
 			if (factor_toset <= 3) {
 				factor_toset = (1 << (factor_toset + 2));
 				if (factor_toset > 0xf)
@@ -1774,7 +1774,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_AC_PARAM:{
-			u8 e_aci = *((u8 *) val);
+			u8 e_aci = *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);
@@ -1814,11 +1814,11 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			}
 			if (rtlusb->acm_method != eAcmWay2_SW)
 				rtlpriv->cfg->ops->set_hw_reg(hw,
-					 HW_VAR_ACM_CTRL, (u8 *)(&e_aci));
+					 HW_VAR_ACM_CTRL, &e_aci);
 			break;
 		}
 	case HW_VAR_ACM_CTRL:{
-			u8 e_aci = *((u8 *) val);
+			u8 e_aci = *val;
 			union aci_aifsn *p_aci_aifsn = (union aci_aifsn *)
 							(&(mac->ac[0].aifs));
 			u8 acm = p_aci_aifsn->f.acm;
@@ -1874,7 +1874,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_RETRY_LIMIT:{
-			u8 retry_limit = ((u8 *) (val))[0];
+			u8 retry_limit = val[0];
 
 			rtl_write_word(rtlpriv, REG_RL,
 				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
@@ -1891,39 +1891,38 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtlefuse->efuse_usedbytes = *((u16 *) val);
 		break;
 	case HW_VAR_EFUSE_USAGE:
-		rtlefuse->efuse_usedpercentage = *((u8 *) val);
+		rtlefuse->efuse_usedpercentage = *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));
+		rtl_write_byte(rtlpriv, REG_SECCFG, *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));
+				rtl_write_byte(rtlpriv, REG_USB_HRPWM, *val);
 			else
 				rtl_write_byte(rtlpriv, REG_USB_HRPWM,
-					       ((*(u8 *)val) | BIT(7)));
+					       *val | BIT(7));
 			break;
 		}
 	case HW_VAR_H2C_FW_PWRMODE:{
-			u8 psmode = (*(u8 *) val);
+			u8 psmode = *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));
+			rtl92c_set_fw_pwrmode_cmd(hw, (*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 mstatus = *val;
 			u8 tmp_reg422;
 			bool recover = false;
 
@@ -1948,7 +1947,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 						tmp_reg422 | BIT(6));
 				rtl_write_byte(rtlpriv, REG_CR + 1, 0x02);
 			}
-			rtl92c_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
+			rtl92c_set_fw_joinbss_report_cmd(hw, (*val));
 			break;
 		}
 	case HW_VAR_AID:{
@@ -1961,7 +1960,7 @@ void rtl92cu_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_CORRECT_TSF:{
-			u8 btype_ibss = ((u8 *) (val))[0];
+			u8 btype_ibss = val[0];
 
 			if (btype_ibss)
 				_rtl92cu_stop_tx_beacon(hw);
@@ -2184,7 +2183,7 @@ void rtl92cu_update_channel_access_setting(struct ieee80211_hw *hw)
 	u16 sifs_timer;
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-				      (u8 *)&mac->slot_time);
+				      &mac->slot_time);
 	if (!mac->ht_enable)
 		sifs_timer = 0x0a0a;
 	else
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
index 21bc827c5fa..2e6eb356a93 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -668,7 +668,7 @@ void rtl92cu_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	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_PKT_SIZE(pdesc, (u16)skb->len);
 	SET_TX_DESC_FIRST_SEG(pdesc, 1);
 	SET_TX_DESC_LAST_SEG(pdesc, 1);
 	SET_TX_DESC_OFFSET(pdesc, 0x20);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
index a7d63a84551..c0201ed69dd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
@@ -696,7 +696,7 @@ static void rtl92d_dm_check_edca_turbo(struct ieee80211_hw *hw)
 		if (rtlpriv->dm.current_turbo_edca) {
 			u8 tmp = AC0_BE;
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      (u8 *) (&tmp));
+						      &tmp);
 			rtlpriv->dm.current_turbo_edca = false;
 		}
 	}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/fw.c b/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
index f548a8d0068..895ae6c1f35 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/fw.c
@@ -120,7 +120,7 @@ static void _rtl92d_write_fw(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 *bufferPtr = (u8 *) buffer;
+	u8 *bufferPtr = buffer;
 	u32 pagenums, remainSize;
 	u32 page, offset;
 
@@ -256,8 +256,8 @@ int rtl92d_download_fw(struct ieee80211_hw *hw)
 	if (rtlpriv->max_fw_size == 0 || !rtlhal->pfirmware)
 		return 1;
 	fwsize = rtlhal->fwsize;
-	pfwheader = (u8 *) rtlhal->pfirmware;
-	pfwdata = (u8 *) rtlhal->pfirmware;
+	pfwheader = rtlhal->pfirmware;
+	pfwdata = rtlhal->pfirmware;
 	rtlhal->fw_version = (u16) GET_FIRMWARE_HDR_VERSION(pfwheader);
 	rtlhal->fw_subversion = (u16) GET_FIRMWARE_HDR_SUB_VER(pfwheader);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
index b338d526c42..f4051f4f039 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/hw.c
@@ -235,12 +235,12 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		for (e_aci = 0; e_aci < AC_MAX; e_aci++)
 			rtlpriv->cfg->ops->set_hw_reg(hw,
 						      HW_VAR_AC_PARAM,
-						      (u8 *) (&e_aci));
+						      (&e_aci));
 		break;
 	}
 	case HW_VAR_ACK_PREAMBLE: {
 		u8 reg_tmp;
-		u8 short_preamble = (bool) (*(u8 *) val);
+		u8 short_preamble = (bool) (*val);
 
 		reg_tmp = (mac->cur_40_prime_sc) << 5;
 		if (short_preamble)
@@ -252,7 +252,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		u8 min_spacing_to_set;
 		u8 sec_min_space;
 
-		min_spacing_to_set = *((u8 *) val);
+		min_spacing_to_set = *val;
 		if (min_spacing_to_set <= 7) {
 			sec_min_space = 0;
 			if (min_spacing_to_set < sec_min_space)
@@ -271,7 +271,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_SHORTGI_DENSITY: {
 		u8 density_to_set;
 
-		density_to_set = *((u8 *) val);
+		density_to_set = *val;
 		mac->min_space_cfg = rtlpriv->rtlhal.minspace_cfg;
 		mac->min_space_cfg |= (density_to_set << 3);
 		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
@@ -293,7 +293,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			regtoSet = 0x66626641;
 		else
 			regtoSet = 0xb972a841;
-		factor_toset = *((u8 *) val);
+		factor_toset = *val;
 		if (factor_toset <= 3) {
 			factor_toset = (1 << (factor_toset + 2));
 			if (factor_toset > 0xf)
@@ -316,15 +316,15 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		break;
 	}
 	case HW_VAR_AC_PARAM: {
-		u8 e_aci = *((u8 *) val);
+		u8 e_aci = *val;
 		rtl92d_dm_init_edca_turbo(hw);
 		if (rtlpci->acm_method != eAcmWay2_SW)
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
-						      (u8 *) (&e_aci));
+						      &e_aci);
 		break;
 	}
 	case HW_VAR_ACM_CTRL: {
-		u8 e_aci = *((u8 *) val);
+		u8 e_aci = *val;
 		union aci_aifsn *p_aci_aifsn =
 		    (union aci_aifsn *)(&(mac->ac[0].aifs));
 		u8 acm = p_aci_aifsn->f.acm;
@@ -376,7 +376,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtlpci->receive_config = ((u32 *) (val))[0];
 		break;
 	case HW_VAR_RETRY_LIMIT: {
-		u8 retry_limit = ((u8 *) (val))[0];
+		u8 retry_limit = val[0];
 
 		rtl_write_word(rtlpriv, REG_RL,
 			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
@@ -390,16 +390,16 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtlefuse->efuse_usedbytes = *((u16 *) val);
 		break;
 	case HW_VAR_EFUSE_USAGE:
-		rtlefuse->efuse_usedpercentage = *((u8 *) val);
+		rtlefuse->efuse_usedpercentage = *val;
 		break;
 	case HW_VAR_IO_CMD:
 		rtl92d_phy_set_io_cmd(hw, (*(enum io_type *)val));
 		break;
 	case HW_VAR_WPA_CONFIG:
-		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
+		rtl_write_byte(rtlpriv, REG_SECCFG, *val);
 		break;
 	case HW_VAR_SET_RPWM:
-		rtl92d_fill_h2c_cmd(hw, H2C_PWRM, 1, (u8 *) (val));
+		rtl92d_fill_h2c_cmd(hw, H2C_PWRM, 1, (val));
 		break;
 	case HW_VAR_H2C_FW_PWRMODE:
 		break;
@@ -407,7 +407,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		ppsc->fw_current_inpsmode = *((bool *) val);
 		break;
 	case HW_VAR_H2C_FW_JOINBSSRPT: {
-		u8 mstatus = (*(u8 *) val);
+		u8 mstatus = (*val);
 		u8 tmp_regcr, tmp_reg422;
 		bool recover = false;
 
@@ -435,7 +435,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			rtl_write_byte(rtlpriv, REG_CR + 1,
 				       (tmp_regcr & ~(BIT(0))));
 		}
-		rtl92d_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
+		rtl92d_set_fw_joinbss_report_cmd(hw, (*val));
 		break;
 	}
 	case HW_VAR_AID: {
@@ -447,7 +447,7 @@ void rtl92de_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		break;
 	}
 	case HW_VAR_CORRECT_TSF: {
-		u8 btype_ibss = ((u8 *) (val))[0];
+		u8 btype_ibss = val[0];
 
 		if (btype_ibss)
 			_rtl92de_stop_tx_beacon(hw);
@@ -1794,7 +1794,7 @@ static void _rtl92de_read_adapter_info(struct ieee80211_hw *hw)
 			 "RTL819X Not boot from eeprom, check it !!\n");
 		return;
 	}
-	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+	rtlefuse->eeprom_oemid = hwinfo[EEPROM_CUSTOMER_ID];
 	_rtl92de_read_macphymode_and_bandtype(hw, hwinfo);
 
 	/* VID, DID  SE     0xA-D */
@@ -2115,7 +2115,7 @@ void rtl92de_update_channel_access_setting(struct ieee80211_hw *hw)
 	u16 sifs_timer;
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-				      (u8 *)&mac->slot_time);
+				      &mac->slot_time);
 	if (!mac->ht_enable)
 		sifs_timer = 0x0a0a;
 	else
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
index 18380a7829f..442031256bc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
@@ -3345,21 +3345,21 @@ void rtl92d_phy_config_macphymode_info(struct ieee80211_hw *hw)
 	switch (rtlhal->macphymode) {
 	case DUALMAC_SINGLEPHY:
 		rtlphy->rf_type = RF_2T2R;
-		rtlhal->version |= CHIP_92D_SINGLEPHY;
+		rtlhal->version |= RF_TYPE_2T2R;
 		rtlhal->bandset = BAND_ON_BOTH;
 		rtlhal->current_bandtype = BAND_ON_2_4G;
 		break;
 
 	case SINGLEMAC_SINGLEPHY:
 		rtlphy->rf_type = RF_2T2R;
-		rtlhal->version |= CHIP_92D_SINGLEPHY;
+		rtlhal->version |= RF_TYPE_2T2R;
 		rtlhal->bandset = BAND_ON_BOTH;
 		rtlhal->current_bandtype = BAND_ON_2_4G;
 		break;
 
 	case DUALMAC_DUALPHY:
 		rtlphy->rf_type = RF_1T1R;
-		rtlhal->version &= (~CHIP_92D_SINGLEPHY);
+		rtlhal->version &= RF_TYPE_1T1R;
 		/* Now we let MAC0 run on 5G band. */
 		if (rtlhal->interfaceindex == 0) {
 			rtlhal->bandset = BAND_ON_5G;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
index 1666ef7fd87..f80690d82c1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
@@ -560,7 +560,7 @@ void rtl92de_tx_fill_desc(struct ieee80211_hw *hw,
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct ieee80211_sta *sta = info->control.sta;
-	u8 *pdesc = (u8 *) pdesc_tx;
+	u8 *pdesc = pdesc_tx;
 	u16 seq_number;
 	__le16 fc = hdr->frame_control;
 	unsigned int buf_len = 0;
@@ -761,11 +761,11 @@ void rtl92de_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	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_TX_BUFFER_SIZE(pdesc, (u16) (skb->len));
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)skb->len);
 	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
 	SET_TX_DESC_RATE_ID(pdesc, 7);
 	SET_TX_DESC_MACID(pdesc, 0);
-	SET_TX_DESC_PKT_SIZE((u8 *) pdesc, (u16) (skb->len));
+	SET_TX_DESC_PKT_SIZE(pdesc, (u16) (skb->len));
 	SET_TX_DESC_FIRST_SEG(pdesc, 1);
 	SET_TX_DESC_LAST_SEG(pdesc, 1);
 	SET_TX_DESC_OFFSET(pdesc, 0x20);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
index 2e1158026fb..465f5815710 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
@@ -146,7 +146,7 @@ static void _rtl92s_dm_check_edca_turbo(struct ieee80211_hw *hw)
 		if (rtlpriv->dm.current_turbo_edca) {
 			u8 tmp = AC0_BE;
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      (u8 *)(&tmp));
+						      &tmp);
 			rtlpriv->dm.current_turbo_edca = false;
 		}
 	}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
index b141c35bf92..4542e6952b9 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/hw.c
@@ -145,13 +145,13 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						HW_VAR_AC_PARAM,
-						(u8 *)(&e_aci));
+						(&e_aci));
 			}
 			break;
 		}
 	case HW_VAR_ACK_PREAMBLE:{
 			u8 reg_tmp;
-			u8 short_preamble = (bool) (*(u8 *) val);
+			u8 short_preamble = (bool) (*val);
 			reg_tmp = (mac->cur_40_prime_sc) << 5;
 			if (short_preamble)
 				reg_tmp |= 0x80;
@@ -163,7 +163,7 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			u8 min_spacing_to_set;
 			u8 sec_min_space;
 
-			min_spacing_to_set = *((u8 *)val);
+			min_spacing_to_set = *val;
 			if (min_spacing_to_set <= 7) {
 				if (rtlpriv->sec.pairwise_enc_algorithm ==
 				    NO_ENCRYPTION)
@@ -194,7 +194,7 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_SHORTGI_DENSITY:{
 			u8 density_to_set;
 
-			density_to_set = *((u8 *) val);
+			density_to_set = *val;
 			mac->min_space_cfg = rtlpriv->rtlhal.minspace_cfg;
 			mac->min_space_cfg |= (density_to_set << 3);
 
@@ -216,7 +216,7 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				15, 15, 15, 15, 0};
 			u8 index = 0;
 
-			factor_toset = *((u8 *) val);
+			factor_toset = *val;
 			if (factor_toset <= 3) {
 				factor_toset = (1 << (factor_toset + 2));
 				if (factor_toset > 0xf)
@@ -248,17 +248,17 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_AC_PARAM:{
-			u8 e_aci = *((u8 *) val);
+			u8 e_aci = *val;
 			rtl92s_dm_init_edca_turbo(hw);
 
 			if (rtlpci->acm_method != eAcmWay2_SW)
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 						 HW_VAR_ACM_CTRL,
-						 (u8 *)(&e_aci));
+						 &e_aci);
 			break;
 		}
 	case HW_VAR_ACM_CTRL:{
-			u8 e_aci = *((u8 *) val);
+			u8 e_aci = *val;
 			union aci_aifsn *p_aci_aifsn = (union aci_aifsn *)(&(
 							mac->ac[0].aifs));
 			u8 acm = p_aci_aifsn->f.acm;
@@ -313,7 +313,7 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_RETRY_LIMIT:{
-			u8 retry_limit = ((u8 *) (val))[0];
+			u8 retry_limit = val[0];
 
 			rtl_write_word(rtlpriv, RETRY_LIMIT,
 				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
@@ -328,14 +328,14 @@ void rtl92se_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			break;
 		}
 	case HW_VAR_EFUSE_USAGE: {
-			rtlefuse->efuse_usedpercentage = *((u8 *) val);
+			rtlefuse->efuse_usedpercentage = *val;
 			break;
 		}
 	case HW_VAR_IO_CMD: {
 			break;
 		}
 	case HW_VAR_WPA_CONFIG: {
-			rtl_write_byte(rtlpriv, REG_SECR, *((u8 *) val));
+			rtl_write_byte(rtlpriv, REG_SECR, *val);
 			break;
 		}
 	case HW_VAR_SET_RPWM:{
@@ -1813,8 +1813,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 		else
 			index = 2;
 
-		tempval = (*(u8 *)&hwinfo[EEPROM_TX_PWR_HT20_DIFF +
-			   index]) & 0xff;
+		tempval = hwinfo[EEPROM_TX_PWR_HT20_DIFF + index] & 0xff;
 		rtlefuse->txpwr_ht20diff[RF90_PATH_A][i] = (tempval & 0xF);
 		rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] =
 						 ((tempval >> 4) & 0xF);
@@ -1830,14 +1829,13 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 		else
 			index = 1;
 
-		tempval = (*(u8 *)&hwinfo[EEPROM_TX_PWR_OFDM_DIFF + index])
-				  & 0xff;
+		tempval = hwinfo[EEPROM_TX_PWR_OFDM_DIFF + index] & 0xff;
 		rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i] =
 				 (tempval & 0xF);
 		rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i] =
 				 ((tempval >> 4) & 0xF);
 
-		tempval = (*(u8 *)&hwinfo[TX_PWR_SAFETY_CHK]);
+		tempval = hwinfo[TX_PWR_SAFETY_CHK];
 		rtlefuse->txpwr_safetyflag = (tempval & 0x01);
 	}
 
@@ -1876,7 +1874,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 
 	/* Read RF-indication and Tx Power gain
 	 * index diff of legacy to HT OFDM rate. */
-	tempval = (*(u8 *)&hwinfo[EEPROM_RFIND_POWERDIFF]) & 0xff;
+	tempval = hwinfo[EEPROM_RFIND_POWERDIFF] & 0xff;
 	rtlefuse->eeprom_txpowerdiff = tempval;
 	rtlefuse->legacy_httxpowerdiff =
 		rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][0];
@@ -1887,7 +1885,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 	/* Get TSSI value for each path. */
 	usvalue = *(u16 *)&hwinfo[EEPROM_TSSI_A];
 	rtlefuse->eeprom_tssi[RF90_PATH_A] = (u8)((usvalue & 0xff00) >> 8);
-	usvalue = *(u8 *)&hwinfo[EEPROM_TSSI_B];
+	usvalue = hwinfo[EEPROM_TSSI_B];
 	rtlefuse->eeprom_tssi[RF90_PATH_B] = (u8)(usvalue & 0xff);
 
 	RTPRINT(rtlpriv, FINIT, INIT_TxPower, "TSSI_A = 0x%x, TSSI_B = 0x%x\n",
@@ -1896,7 +1894,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 
 	/* Read antenna tx power offset of B/C/D to A  from EEPROM */
 	/* and read ThermalMeter from EEPROM */
-	tempval = *(u8 *)&hwinfo[EEPROM_THERMALMETER];
+	tempval = hwinfo[EEPROM_THERMALMETER];
 	rtlefuse->eeprom_thermalmeter = tempval;
 	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
@@ -1906,20 +1904,20 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 	rtlefuse->tssi_13dbm = rtlefuse->eeprom_thermalmeter * 100;
 
 	/* Read CrystalCap from EEPROM */
-	tempval = (*(u8 *)&hwinfo[EEPROM_CRYSTALCAP]) >> 4;
+	tempval = hwinfo[EEPROM_CRYSTALCAP] >> 4;
 	rtlefuse->eeprom_crystalcap = tempval;
 	/* CrystalCap, BIT(12)~15 */
 	rtlefuse->crystalcap = rtlefuse->eeprom_crystalcap;
 
 	/* Read IC Version && Channel Plan */
 	/* Version ID, Channel plan */
-	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
+	rtlefuse->eeprom_channelplan = hwinfo[EEPROM_CHANNELPLAN];
 	rtlefuse->txpwr_fromeprom = true;
 	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
 		"EEPROM ChannelPlan = 0x%4x\n", rtlefuse->eeprom_channelplan);
 
 	/* Read Customer ID or Board Type!!! */
-	tempval = *(u8 *)&hwinfo[EEPROM_BOARDTYPE];
+	tempval = hwinfo[EEPROM_BOARDTYPE];
 	/* Change RF type definition */
 	if (tempval == 0)
 		rtlphy->rf_type = RF_2T2R;
@@ -1941,7 +1939,7 @@ static void _rtl92se_read_adapter_info(struct ieee80211_hw *hw)
 		}
 	}
 	rtlefuse->b1ss_support = rtlefuse->b1x1_recvcombine;
-	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMID];
+	rtlefuse->eeprom_oemid = *&hwinfo[EEPROM_CUSTOMID];
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "EEPROM Customer ID: 0x%2x",
 		 rtlefuse->eeprom_oemid);
@@ -2251,7 +2249,7 @@ void rtl92se_update_channel_access_setting(struct ieee80211_hw *hw)
 	u16 sifs_timer;
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-				      (u8 *)&mac->slot_time);
+				      &mac->slot_time);
 	sifs_timer = 0x0e0e;
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *)&sifs_timer);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
index 8d7099bc472..b917a2a3caf 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
@@ -1247,6 +1247,9 @@ static void _rtl92s_phy_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
 		/* Read HT 40 OFDM TX power */
 		ofdmpowerLevel[0] = rtlefuse->txpwrlevel_ht40_2s[0][index];
 		ofdmpowerLevel[1] = rtlefuse->txpwrlevel_ht40_2s[1][index];
+	} else {
+		ofdmpowerLevel[0] = 0;
+		ofdmpowerLevel[1] = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
index 730bcc91952..ad4b4803482 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
@@ -29,7 +29,6 @@
 
 #include "../wifi.h"
 #include "../core.h"
-#include "../pci.h"
 #include "../base.h"
 #include "../pci.h"
 #include "reg.h"
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 812b5858f14..36d1cb3aef8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -599,7 +599,7 @@ void rtl92se_tx_fill_desc(struct ieee80211_hw *hw,
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct ieee80211_sta *sta = info->control.sta;
-	u8 *pdesc = (u8 *) pdesc_tx;
+	u8 *pdesc = pdesc_tx;
 	u16 seq_number;
 	__le16 fc = hdr->frame_control;
 	u8 reserved_macid = 0;
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index a6049d7d51b..aa970fc18a2 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -131,15 +131,19 @@ static u32 _usb_read_sync(struct rtl_priv *rtlpriv, u32 addr, u16 len)
 	u8 request;
 	u16 wvalue;
 	u16 index;
-	__le32 *data = &rtlpriv->usb_data[rtlpriv->usb_data_index];
+	__le32 *data;
+	unsigned long flags;
 
+	spin_lock_irqsave(&rtlpriv->locks.usb_lock, flags);
+	if (++rtlpriv->usb_data_index >= RTL_USB_MAX_RX_COUNT)
+		rtlpriv->usb_data_index = 0;
+	data = &rtlpriv->usb_data[rtlpriv->usb_data_index];
+	spin_unlock_irqrestore(&rtlpriv->locks.usb_lock, flags);
 	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);
-	if (++rtlpriv->usb_data_index >= RTL_USB_MAX_RX_COUNT)
-		rtlpriv->usb_data_index = 0;
 	return le32_to_cpu(*data);
 }
 
@@ -951,6 +955,10 @@ int __devinit rtl_usb_probe(struct usb_interface *intf,
 				    GFP_KERNEL);
 	if (!rtlpriv->usb_data)
 		return -ENOMEM;
+
+	/* this spin lock must be initialized early */
+	spin_lock_init(&rtlpriv->locks.usb_lock);
+
 	rtlpriv->usb_data_index = 0;
 	init_completion(&rtlpriv->firmware_loading_complete);
 	SET_IEEE80211_DEV(hw, &intf->dev);
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index bd816aef26d..cdaa21f2971 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -1555,6 +1555,7 @@ struct rtl_locks {
 	spinlock_t rf_ps_lock;
 	spinlock_t rf_lock;
 	spinlock_t waitq_lock;
+	spinlock_t usb_lock;
 
 	/*Dual mac*/
 	spinlock_t cck_and_rw_pagea_lock;
diff --git a/drivers/net/wireless/ti/Kconfig b/drivers/net/wireless/ti/Kconfig
index 1a72932e221..be800119d0a 100644
--- a/drivers/net/wireless/ti/Kconfig
+++ b/drivers/net/wireless/ti/Kconfig
@@ -8,6 +8,7 @@ menuconfig WL_TI
 if WL_TI
 source "drivers/net/wireless/ti/wl1251/Kconfig"
 source "drivers/net/wireless/ti/wl12xx/Kconfig"
+source "drivers/net/wireless/ti/wl18xx/Kconfig"
 
 # keep last for automatic dependencies
 source "drivers/net/wireless/ti/wlcore/Kconfig"
diff --git a/drivers/net/wireless/ti/Makefile b/drivers/net/wireless/ti/Makefile
index 0a565622d4a..4d6823983c0 100644
--- a/drivers/net/wireless/ti/Makefile
+++ b/drivers/net/wireless/ti/Makefile
@@ -2,3 +2,4 @@ obj-$(CONFIG_WLCORE)			+= wlcore/
 obj-$(CONFIG_WL12XX)			+= wl12xx/
 obj-$(CONFIG_WL12XX_PLATFORM_DATA)	+= wlcore/
 obj-$(CONFIG_WL1251)			+= wl1251/
+obj-$(CONFIG_WL18XX)			+= wl18xx/
diff --git a/drivers/net/wireless/ti/wl1251/cmd.c b/drivers/net/wireless/ti/wl1251/cmd.c
index d14d69d733a..6822b845efc 100644
--- a/drivers/net/wireless/ti/wl1251/cmd.c
+++ b/drivers/net/wireless/ti/wl1251/cmd.c
@@ -277,15 +277,6 @@ int wl1251_cmd_join(struct wl1251 *wl, u8 bss_type, u8 channel,
 	join->rx_config_options = wl->rx_config;
 	join->rx_filter_options = wl->rx_filter;
 
-	/*
-	 * FIXME: disable temporarily all filters because after commit
-	 * 9cef8737 "mac80211: fix managed mode BSSID handling" broke
-	 * association. The filter logic needs to be implemented properly
-	 * and once that is done, this hack can be removed.
-	 */
-	join->rx_config_options = 0;
-	join->rx_filter_options = WL1251_DEFAULT_RX_FILTER;
-
 	join->basic_rate_set = RATE_MASK_1MBPS | RATE_MASK_2MBPS |
 		RATE_MASK_5_5MBPS | RATE_MASK_11MBPS;
 
diff --git a/drivers/net/wireless/ti/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index d1afb8e3b2e..3118c425bcf 100644
--- a/drivers/net/wireless/ti/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
@@ -334,6 +334,12 @@ static int wl1251_join(struct wl1251 *wl, u8 bss_type, u8 channel,
 	if (ret < 0)
 		goto out;
 
+	/*
+	 * Join command applies filters, and if we are not associated,
+	 * BSSID filter must be disabled for association to work.
+	 */
+	if (is_zero_ether_addr(wl->bssid))
+		wl->rx_config &= ~CFG_BSSID_FILTER_EN;
 
 	ret = wl1251_cmd_join(wl, bss_type, channel, beacon_interval,
 			      dtim_period);
@@ -348,33 +354,6 @@ out:
 	return ret;
 }
 
-static void wl1251_filter_work(struct work_struct *work)
-{
-	struct wl1251 *wl =
-		container_of(work, struct wl1251, filter_work);
-	int ret;
-
-	mutex_lock(&wl->mutex);
-
-	if (wl->state == WL1251_STATE_OFF)
-		goto out;
-
-	ret = wl1251_ps_elp_wakeup(wl);
-	if (ret < 0)
-		goto out;
-
-	ret = wl1251_join(wl, wl->bss_type, wl->channel, wl->beacon_int,
-			  wl->dtim_period);
-	if (ret < 0)
-		goto out_sleep;
-
-out_sleep:
-	wl1251_ps_elp_sleep(wl);
-
-out:
-	mutex_unlock(&wl->mutex);
-}
-
 static void wl1251_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct wl1251 *wl = hw->priv;
@@ -478,7 +457,6 @@ static void wl1251_op_stop(struct ieee80211_hw *hw)
 
 	cancel_work_sync(&wl->irq_work);
 	cancel_work_sync(&wl->tx_work);
-	cancel_work_sync(&wl->filter_work);
 	cancel_delayed_work_sync(&wl->elp_work);
 
 	mutex_lock(&wl->mutex);
@@ -681,13 +659,15 @@ out:
 				  FIF_FCSFAIL | \
 				  FIF_BCN_PRBRESP_PROMISC | \
 				  FIF_CONTROL | \
-				  FIF_OTHER_BSS)
+				  FIF_OTHER_BSS | \
+				  FIF_PROBE_REQ)
 
 static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
 				       unsigned int changed,
 				       unsigned int *total,u64 multicast)
 {
 	struct wl1251 *wl = hw->priv;
+	int ret;
 
 	wl1251_debug(DEBUG_MAC80211, "mac80211 configure filter");
 
@@ -698,7 +678,7 @@ static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
 		/* no filters which we support changed */
 		return;
 
-	/* FIXME: wl->rx_config and wl->rx_filter are not protected */
+	mutex_lock(&wl->mutex);
 
 	wl->rx_config = WL1251_DEFAULT_RX_CONFIG;
 	wl->rx_filter = WL1251_DEFAULT_RX_FILTER;
@@ -721,15 +701,25 @@ static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
 	}
 	if (*total & FIF_CONTROL)
 		wl->rx_filter |= CFG_RX_CTL_EN;
-	if (*total & FIF_OTHER_BSS)
-		wl->rx_filter &= ~CFG_BSSID_FILTER_EN;
+	if (*total & FIF_OTHER_BSS || is_zero_ether_addr(wl->bssid))
+		wl->rx_config &= ~CFG_BSSID_FILTER_EN;
+	if (*total & FIF_PROBE_REQ)
+		wl->rx_filter |= CFG_RX_PREQ_EN;
 
-	/*
-	 * FIXME: workqueues need to be properly cancelled on stop(), for
-	 * now let's just disable changing the filter settings. They will
-	 * be updated any on config().
-	 */
-	/* schedule_work(&wl->filter_work); */
+	if (wl->state == WL1251_STATE_OFF)
+		goto out;
+
+	ret = wl1251_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	/* send filters to firmware */
+	wl1251_acx_rx_config(wl, wl->rx_config, wl->rx_filter);
+
+	wl1251_ps_elp_sleep(wl);
+
+out:
+	mutex_unlock(&wl->mutex);
 }
 
 /* HW encryption */
@@ -1390,7 +1380,6 @@ struct ieee80211_hw *wl1251_alloc_hw(void)
 
 	skb_queue_head_init(&wl->tx_queue);
 
-	INIT_WORK(&wl->filter_work, wl1251_filter_work);
 	INIT_DELAYED_WORK(&wl->elp_work, wl1251_elp_work);
 	wl->channel = WL1251_DEFAULT_CHANNEL;
 	wl->scanning = false;
diff --git a/drivers/net/wireless/ti/wl1251/wl1251.h b/drivers/net/wireless/ti/wl1251/wl1251.h
index 9d8f5816c6f..fd02060038d 100644
--- a/drivers/net/wireless/ti/wl1251/wl1251.h
+++ b/drivers/net/wireless/ti/wl1251/wl1251.h
@@ -315,7 +315,6 @@ struct wl1251 {
 	bool tx_queue_stopped;
 
 	struct work_struct tx_work;
-	struct work_struct filter_work;
 
 	/* Pending TX frames */
 	struct sk_buff *tx_frames[16];
diff --git a/drivers/net/wireless/ti/wl12xx/Makefile b/drivers/net/wireless/ti/wl12xx/Makefile
index 87f64b14db3..da509aa7d00 100644
--- a/drivers/net/wireless/ti/wl12xx/Makefile
+++ b/drivers/net/wireless/ti/wl12xx/Makefile
@@ -1,3 +1,3 @@
-wl12xx-objs	= main.o cmd.o acx.o
+wl12xx-objs	= main.o cmd.o acx.o debugfs.o
 
 obj-$(CONFIG_WL12XX)		+= wl12xx.o
diff --git a/drivers/net/wireless/ti/wl12xx/acx.h b/drivers/net/wireless/ti/wl12xx/acx.h
index d1f5aba0afc..2a26868b837 100644
--- a/drivers/net/wireless/ti/wl12xx/acx.h
+++ b/drivers/net/wireless/ti/wl12xx/acx.h
@@ -24,6 +24,21 @@
 #define __WL12XX_ACX_H__
 
 #include "../wlcore/wlcore.h"
+#include "../wlcore/acx.h"
+
+#define WL12XX_ACX_ALL_EVENTS_VECTOR	(WL1271_ACX_INTR_WATCHDOG      | \
+					WL1271_ACX_INTR_INIT_COMPLETE | \
+					WL1271_ACX_INTR_EVENT_A       | \
+					WL1271_ACX_INTR_EVENT_B       | \
+					WL1271_ACX_INTR_CMD_COMPLETE  | \
+					WL1271_ACX_INTR_HW_AVAILABLE  | \
+					WL1271_ACX_INTR_DATA)
+
+#define WL12XX_INTR_MASK		(WL1271_ACX_INTR_WATCHDOG      | \
+					WL1271_ACX_INTR_EVENT_A       | \
+					WL1271_ACX_INTR_EVENT_B       | \
+					WL1271_ACX_INTR_HW_AVAILABLE  | \
+					WL1271_ACX_INTR_DATA)
 
 struct wl1271_acx_host_config_bitmap {
 	struct acx_header header;
@@ -31,6 +46,228 @@ struct wl1271_acx_host_config_bitmap {
 	__le32 host_cfg_bitmap;
 } __packed;
 
+struct wl12xx_acx_tx_statistics {
+	__le32 internal_desc_overflow;
+}  __packed;
+
+struct wl12xx_acx_rx_statistics {
+	__le32 out_of_mem;
+	__le32 hdr_overflow;
+	__le32 hw_stuck;
+	__le32 dropped;
+	__le32 fcs_err;
+	__le32 xfr_hint_trig;
+	__le32 path_reset;
+	__le32 reset_counter;
+} __packed;
+
+struct wl12xx_acx_dma_statistics {
+	__le32 rx_requested;
+	__le32 rx_errors;
+	__le32 tx_requested;
+	__le32 tx_errors;
+}  __packed;
+
+struct wl12xx_acx_isr_statistics {
+	/* host command complete */
+	__le32 cmd_cmplt;
+
+	/* fiqisr() */
+	__le32 fiqs;
+
+	/* (INT_STS_ND & INT_TRIG_RX_HEADER) */
+	__le32 rx_headers;
+
+	/* (INT_STS_ND & INT_TRIG_RX_CMPLT) */
+	__le32 rx_completes;
+
+	/* (INT_STS_ND & INT_TRIG_NO_RX_BUF) */
+	__le32 rx_mem_overflow;
+
+	/* (INT_STS_ND & INT_TRIG_S_RX_RDY) */
+	__le32 rx_rdys;
+
+	/* irqisr() */
+	__le32 irqs;
+
+	/* (INT_STS_ND & INT_TRIG_TX_PROC) */
+	__le32 tx_procs;
+
+	/* (INT_STS_ND & INT_TRIG_DECRYPT_DONE) */
+	__le32 decrypt_done;
+
+	/* (INT_STS_ND & INT_TRIG_DMA0) */
+	__le32 dma0_done;
+
+	/* (INT_STS_ND & INT_TRIG_DMA1) */
+	__le32 dma1_done;
+
+	/* (INT_STS_ND & INT_TRIG_TX_EXC_CMPLT) */
+	__le32 tx_exch_complete;
+
+	/* (INT_STS_ND & INT_TRIG_COMMAND) */
+	__le32 commands;
+
+	/* (INT_STS_ND & INT_TRIG_RX_PROC) */
+	__le32 rx_procs;
+
+	/* (INT_STS_ND & INT_TRIG_PM_802) */
+	__le32 hw_pm_mode_changes;
+
+	/* (INT_STS_ND & INT_TRIG_ACKNOWLEDGE) */
+	__le32 host_acknowledges;
+
+	/* (INT_STS_ND & INT_TRIG_PM_PCI) */
+	__le32 pci_pm;
+
+	/* (INT_STS_ND & INT_TRIG_ACM_WAKEUP) */
+	__le32 wakeups;
+
+	/* (INT_STS_ND & INT_TRIG_LOW_RSSI) */
+	__le32 low_rssi;
+} __packed;
+
+struct wl12xx_acx_wep_statistics {
+	/* WEP address keys configured */
+	__le32 addr_key_count;
+
+	/* default keys configured */
+	__le32 default_key_count;
+
+	__le32 reserved;
+
+	/* number of times that WEP key not found on lookup */
+	__le32 key_not_found;
+
+	/* number of times that WEP key decryption failed */
+	__le32 decrypt_fail;
+
+	/* WEP packets decrypted */
+	__le32 packets;
+
+	/* WEP decrypt interrupts */
+	__le32 interrupt;
+} __packed;
+
+#define ACX_MISSED_BEACONS_SPREAD 10
+
+struct wl12xx_acx_pwr_statistics {
+	/* the amount of enters into power save mode (both PD & ELP) */
+	__le32 ps_enter;
+
+	/* the amount of enters into ELP mode */
+	__le32 elp_enter;
+
+	/* the amount of missing beacon interrupts to the host */
+	__le32 missing_bcns;
+
+	/* the amount of wake on host-access times */
+	__le32 wake_on_host;
+
+	/* the amount of wake on timer-expire */
+	__le32 wake_on_timer_exp;
+
+	/* the number of packets that were transmitted with PS bit set */
+	__le32 tx_with_ps;
+
+	/* the number of packets that were transmitted with PS bit clear */
+	__le32 tx_without_ps;
+
+	/* the number of received beacons */
+	__le32 rcvd_beacons;
+
+	/* the number of entering into PowerOn (power save off) */
+	__le32 power_save_off;
+
+	/* the number of entries into power save mode */
+	__le16 enable_ps;
+
+	/*
+	 * the number of exits from power save, not including failed PS
+	 * transitions
+	 */
+	__le16 disable_ps;
+
+	/*
+	 * the number of times the TSF counter was adjusted because
+	 * of drift
+	 */
+	__le32 fix_tsf_ps;
+
+	/* Gives statistics about the spread continuous missed beacons.
+	 * The 16 LSB are dedicated for the PS mode.
+	 * The 16 MSB are dedicated for the PS mode.
+	 * cont_miss_bcns_spread[0] - single missed beacon.
+	 * cont_miss_bcns_spread[1] - two continuous missed beacons.
+	 * cont_miss_bcns_spread[2] - three continuous missed beacons.
+	 * ...
+	 * cont_miss_bcns_spread[9] - ten and more continuous missed beacons.
+	*/
+	__le32 cont_miss_bcns_spread[ACX_MISSED_BEACONS_SPREAD];
+
+	/* the number of beacons in awake mode */
+	__le32 rcvd_awake_beacons;
+} __packed;
+
+struct wl12xx_acx_mic_statistics {
+	__le32 rx_pkts;
+	__le32 calc_failure;
+} __packed;
+
+struct wl12xx_acx_aes_statistics {
+	__le32 encrypt_fail;
+	__le32 decrypt_fail;
+	__le32 encrypt_packets;
+	__le32 decrypt_packets;
+	__le32 encrypt_interrupt;
+	__le32 decrypt_interrupt;
+} __packed;
+
+struct wl12xx_acx_event_statistics {
+	__le32 heart_beat;
+	__le32 calibration;
+	__le32 rx_mismatch;
+	__le32 rx_mem_empty;
+	__le32 rx_pool;
+	__le32 oom_late;
+	__le32 phy_transmit_error;
+	__le32 tx_stuck;
+} __packed;
+
+struct wl12xx_acx_ps_statistics {
+	__le32 pspoll_timeouts;
+	__le32 upsd_timeouts;
+	__le32 upsd_max_sptime;
+	__le32 upsd_max_apturn;
+	__le32 pspoll_max_apturn;
+	__le32 pspoll_utilization;
+	__le32 upsd_utilization;
+} __packed;
+
+struct wl12xx_acx_rxpipe_statistics {
+	__le32 rx_prep_beacon_drop;
+	__le32 descr_host_int_trig_rx_data;
+	__le32 beacon_buffer_thres_host_int_trig_rx_data;
+	__le32 missed_beacon_host_int_trig_rx_data;
+	__le32 tx_xfr_host_int_trig_rx_data;
+} __packed;
+
+struct wl12xx_acx_statistics {
+	struct acx_header header;
+
+	struct wl12xx_acx_tx_statistics tx;
+	struct wl12xx_acx_rx_statistics rx;
+	struct wl12xx_acx_dma_statistics dma;
+	struct wl12xx_acx_isr_statistics isr;
+	struct wl12xx_acx_wep_statistics wep;
+	struct wl12xx_acx_pwr_statistics pwr;
+	struct wl12xx_acx_aes_statistics aes;
+	struct wl12xx_acx_mic_statistics mic;
+	struct wl12xx_acx_event_statistics event;
+	struct wl12xx_acx_ps_statistics ps;
+	struct wl12xx_acx_rxpipe_statistics rxpipe;
+} __packed;
+
 int wl1271_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap);
 
 #endif /* __WL12XX_ACX_H__ */
diff --git a/drivers/net/wireless/ti/wl12xx/cmd.c b/drivers/net/wireless/ti/wl12xx/cmd.c
index 8ffaeb5f214..622206241e8 100644
--- a/drivers/net/wireless/ti/wl12xx/cmd.c
+++ b/drivers/net/wireless/ti/wl12xx/cmd.c
@@ -65,6 +65,7 @@ int wl1271_cmd_general_parms(struct wl1271 *wl)
 	struct wl1271_general_parms_cmd *gen_parms;
 	struct wl1271_ini_general_params *gp =
 		&((struct wl1271_nvs_file *)wl->nvs)->general_params;
+	struct wl12xx_priv *priv = wl->priv;
 	bool answer = false;
 	int ret;
 
@@ -84,11 +85,15 @@ int wl1271_cmd_general_parms(struct wl1271 *wl)
 
 	memcpy(&gen_parms->general_params, gp, sizeof(*gp));
 
-	if (gp->tx_bip_fem_auto_detect)
+	/* If we started in PLT FEM_DETECT mode, force auto detect */
+	if (wl->plt_mode == PLT_FEM_DETECT)
+		gen_parms->general_params.tx_bip_fem_auto_detect = true;
+
+	if (gen_parms->general_params.tx_bip_fem_auto_detect)
 		answer = true;
 
 	/* Override the REF CLK from the NVS with the one from platform data */
-	gen_parms->general_params.ref_clock = wl->ref_clock;
+	gen_parms->general_params.ref_clock = priv->ref_clock;
 
 	ret = wl1271_cmd_test(wl, gen_parms, sizeof(*gen_parms), answer);
 	if (ret < 0) {
@@ -105,8 +110,17 @@ int wl1271_cmd_general_parms(struct wl1271 *wl)
 		goto out;
 	}
 
+	/* If we are in calibrator based fem auto detect - save fem nr */
+	if (wl->plt_mode == PLT_FEM_DETECT)
+		wl->fem_manuf = gp->tx_bip_fem_manufacturer;
+
 	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
-		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
+		answer == false ?
+			"manual" :
+		wl->plt_mode == PLT_FEM_DETECT ?
+			"calibrator_fem_detect" :
+			"auto",
+		gp->tx_bip_fem_manufacturer);
 
 out:
 	kfree(gen_parms);
@@ -118,6 +132,7 @@ int wl128x_cmd_general_parms(struct wl1271 *wl)
 	struct wl128x_general_parms_cmd *gen_parms;
 	struct wl128x_ini_general_params *gp =
 		&((struct wl128x_nvs_file *)wl->nvs)->general_params;
+	struct wl12xx_priv *priv = wl->priv;
 	bool answer = false;
 	int ret;
 
@@ -137,12 +152,16 @@ int wl128x_cmd_general_parms(struct wl1271 *wl)
 
 	memcpy(&gen_parms->general_params, gp, sizeof(*gp));
 
-	if (gp->tx_bip_fem_auto_detect)
+	/* If we started in PLT FEM_DETECT mode, force auto detect */
+	if (wl->plt_mode == PLT_FEM_DETECT)
+		gen_parms->general_params.tx_bip_fem_auto_detect = true;
+
+	if (gen_parms->general_params.tx_bip_fem_auto_detect)
 		answer = true;
 
 	/* Replace REF and TCXO CLKs with the ones from platform data */
-	gen_parms->general_params.ref_clock = wl->ref_clock;
-	gen_parms->general_params.tcxo_ref_clock = wl->tcxo_clock;
+	gen_parms->general_params.ref_clock = priv->ref_clock;
+	gen_parms->general_params.tcxo_ref_clock = priv->tcxo_clock;
 
 	ret = wl1271_cmd_test(wl, gen_parms, sizeof(*gen_parms), answer);
 	if (ret < 0) {
@@ -159,8 +178,17 @@ int wl128x_cmd_general_parms(struct wl1271 *wl)
 		goto out;
 	}
 
+	/* If we are in calibrator based fem auto detect - save fem nr */
+	if (wl->plt_mode == PLT_FEM_DETECT)
+		wl->fem_manuf = gp->tx_bip_fem_manufacturer;
+
 	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
-		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
+		answer == false ?
+			"manual" :
+		wl->plt_mode == PLT_FEM_DETECT ?
+			"calibrator_fem_detect" :
+			"auto",
+		gp->tx_bip_fem_manufacturer);
 
 out:
 	kfree(gen_parms);
@@ -172,7 +200,7 @@ int wl1271_cmd_radio_parms(struct wl1271 *wl)
 	struct wl1271_nvs_file *nvs = (struct wl1271_nvs_file *)wl->nvs;
 	struct wl1271_radio_parms_cmd *radio_parms;
 	struct wl1271_ini_general_params *gp = &nvs->general_params;
-	int ret;
+	int ret, fem_idx;
 
 	if (!wl->nvs)
 		return -ENODEV;
@@ -183,11 +211,13 @@ int wl1271_cmd_radio_parms(struct wl1271 *wl)
 
 	radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
 
+	fem_idx = WL12XX_FEM_TO_NVS_ENTRY(gp->tx_bip_fem_manufacturer);
+
 	/* 2.4GHz parameters */
 	memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
 	       sizeof(struct wl1271_ini_band_params_2));
 	memcpy(&radio_parms->dyn_params_2,
-	       &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
+	       &nvs->dyn_radio_params_2[fem_idx].params,
 	       sizeof(struct wl1271_ini_fem_params_2));
 
 	/* 5GHz parameters */
@@ -195,7 +225,7 @@ int wl1271_cmd_radio_parms(struct wl1271 *wl)
 	       &nvs->stat_radio_params_5,
 	       sizeof(struct wl1271_ini_band_params_5));
 	memcpy(&radio_parms->dyn_params_5,
-	       &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
+	       &nvs->dyn_radio_params_5[fem_idx].params,
 	       sizeof(struct wl1271_ini_fem_params_5));
 
 	wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_RADIO_PARAM: ",
@@ -214,7 +244,7 @@ int wl128x_cmd_radio_parms(struct wl1271 *wl)
 	struct wl128x_nvs_file *nvs = (struct wl128x_nvs_file *)wl->nvs;
 	struct wl128x_radio_parms_cmd *radio_parms;
 	struct wl128x_ini_general_params *gp = &nvs->general_params;
-	int ret;
+	int ret, fem_idx;
 
 	if (!wl->nvs)
 		return -ENODEV;
@@ -225,11 +255,13 @@ int wl128x_cmd_radio_parms(struct wl1271 *wl)
 
 	radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
 
+	fem_idx = WL12XX_FEM_TO_NVS_ENTRY(gp->tx_bip_fem_manufacturer);
+
 	/* 2.4GHz parameters */
 	memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
 	       sizeof(struct wl128x_ini_band_params_2));
 	memcpy(&radio_parms->dyn_params_2,
-	       &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
+	       &nvs->dyn_radio_params_2[fem_idx].params,
 	       sizeof(struct wl128x_ini_fem_params_2));
 
 	/* 5GHz parameters */
@@ -237,7 +269,7 @@ int wl128x_cmd_radio_parms(struct wl1271 *wl)
 	       &nvs->stat_radio_params_5,
 	       sizeof(struct wl128x_ini_band_params_5));
 	memcpy(&radio_parms->dyn_params_5,
-	       &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
+	       &nvs->dyn_radio_params_5[fem_idx].params,
 	       sizeof(struct wl128x_ini_fem_params_5));
 
 	radio_parms->fem_vendor_and_options = nvs->fem_vendor_and_options;
diff --git a/drivers/net/wireless/ti/wl12xx/debugfs.c b/drivers/net/wireless/ti/wl12xx/debugfs.c
new file mode 100644
index 00000000000..0521cbf858c
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/debugfs.c
@@ -0,0 +1,243 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Copyright (C) 2011-2012 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include "../wlcore/debugfs.h"
+#include "../wlcore/wlcore.h"
+
+#include "wl12xx.h"
+#include "acx.h"
+#include "debugfs.h"
+
+#define WL12XX_DEBUGFS_FWSTATS_FILE(a, b, c) \
+	DEBUGFS_FWSTATS_FILE(a, b, c, wl12xx_acx_statistics)
+
+WL12XX_DEBUGFS_FWSTATS_FILE(tx, internal_desc_overflow, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, out_of_mem, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, hdr_overflow, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, hw_stuck, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, dropped, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, fcs_err, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, xfr_hint_trig, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, path_reset, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rx, reset_counter, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(dma, rx_requested, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(dma, rx_errors, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(dma, tx_requested, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(dma, tx_errors, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, cmd_cmplt, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, fiqs, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, rx_headers, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, rx_mem_overflow, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, rx_rdys, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, irqs, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, tx_procs, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, decrypt_done, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, dma0_done, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, dma1_done, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, tx_exch_complete, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, commands, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, rx_procs, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, hw_pm_mode_changes, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, host_acknowledges, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, pci_pm, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, wakeups, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(isr, low_rssi, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(wep, addr_key_count, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(wep, default_key_count, "%u");
+/* skipping wep.reserved */
+WL12XX_DEBUGFS_FWSTATS_FILE(wep, key_not_found, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(wep, decrypt_fail, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(wep, packets, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(wep, interrupt, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, ps_enter, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, elp_enter, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, missing_bcns, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, wake_on_host, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, wake_on_timer_exp, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, tx_with_ps, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, tx_without_ps, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, rcvd_beacons, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, power_save_off, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, enable_ps, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, disable_ps, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, fix_tsf_ps, "%u");
+/* skipping cont_miss_bcns_spread for now */
+WL12XX_DEBUGFS_FWSTATS_FILE(pwr, rcvd_awake_beacons, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(mic, rx_pkts, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(mic, calc_failure, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(aes, encrypt_fail, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(aes, decrypt_fail, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(aes, encrypt_packets, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(aes, decrypt_packets, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(aes, encrypt_interrupt, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(aes, decrypt_interrupt, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(event, heart_beat, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, calibration, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, rx_mismatch, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, rx_mem_empty, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, rx_pool, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, oom_late, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, phy_transmit_error, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(event, tx_stuck, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, pspoll_timeouts, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, upsd_timeouts, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, upsd_max_sptime, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, upsd_max_apturn, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, pspoll_max_apturn, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, pspoll_utilization, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(ps, upsd_utilization, "%u");
+
+WL12XX_DEBUGFS_FWSTATS_FILE(rxpipe, rx_prep_beacon_drop, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rxpipe, descr_host_int_trig_rx_data, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rxpipe, beacon_buffer_thres_host_int_trig_rx_data,
+			    "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rxpipe, missed_beacon_host_int_trig_rx_data, "%u");
+WL12XX_DEBUGFS_FWSTATS_FILE(rxpipe, tx_xfr_host_int_trig_rx_data, "%u");
+
+int wl12xx_debugfs_add_files(struct wl1271 *wl,
+			     struct dentry *rootdir)
+{
+	int ret = 0;
+	struct dentry *entry, *stats, *moddir;
+
+	moddir = debugfs_create_dir(KBUILD_MODNAME, rootdir);
+	if (!moddir || IS_ERR(moddir)) {
+		entry = moddir;
+		goto err;
+	}
+
+	stats = debugfs_create_dir("fw_stats", moddir);
+	if (!stats || IS_ERR(stats)) {
+		entry = stats;
+		goto err;
+	}
+
+	DEBUGFS_FWSTATS_ADD(tx, internal_desc_overflow);
+
+	DEBUGFS_FWSTATS_ADD(rx, out_of_mem);
+	DEBUGFS_FWSTATS_ADD(rx, hdr_overflow);
+	DEBUGFS_FWSTATS_ADD(rx, hw_stuck);
+	DEBUGFS_FWSTATS_ADD(rx, dropped);
+	DEBUGFS_FWSTATS_ADD(rx, fcs_err);
+	DEBUGFS_FWSTATS_ADD(rx, xfr_hint_trig);
+	DEBUGFS_FWSTATS_ADD(rx, path_reset);
+	DEBUGFS_FWSTATS_ADD(rx, reset_counter);
+
+	DEBUGFS_FWSTATS_ADD(dma, rx_requested);
+	DEBUGFS_FWSTATS_ADD(dma, rx_errors);
+	DEBUGFS_FWSTATS_ADD(dma, tx_requested);
+	DEBUGFS_FWSTATS_ADD(dma, tx_errors);
+
+	DEBUGFS_FWSTATS_ADD(isr, cmd_cmplt);
+	DEBUGFS_FWSTATS_ADD(isr, fiqs);
+	DEBUGFS_FWSTATS_ADD(isr, rx_headers);
+	DEBUGFS_FWSTATS_ADD(isr, rx_mem_overflow);
+	DEBUGFS_FWSTATS_ADD(isr, rx_rdys);
+	DEBUGFS_FWSTATS_ADD(isr, irqs);
+	DEBUGFS_FWSTATS_ADD(isr, tx_procs);
+	DEBUGFS_FWSTATS_ADD(isr, decrypt_done);
+	DEBUGFS_FWSTATS_ADD(isr, dma0_done);
+	DEBUGFS_FWSTATS_ADD(isr, dma1_done);
+	DEBUGFS_FWSTATS_ADD(isr, tx_exch_complete);
+	DEBUGFS_FWSTATS_ADD(isr, commands);
+	DEBUGFS_FWSTATS_ADD(isr, rx_procs);
+	DEBUGFS_FWSTATS_ADD(isr, hw_pm_mode_changes);
+	DEBUGFS_FWSTATS_ADD(isr, host_acknowledges);
+	DEBUGFS_FWSTATS_ADD(isr, pci_pm);
+	DEBUGFS_FWSTATS_ADD(isr, wakeups);
+	DEBUGFS_FWSTATS_ADD(isr, low_rssi);
+
+	DEBUGFS_FWSTATS_ADD(wep, addr_key_count);
+	DEBUGFS_FWSTATS_ADD(wep, default_key_count);
+	/* skipping wep.reserved */
+	DEBUGFS_FWSTATS_ADD(wep, key_not_found);
+	DEBUGFS_FWSTATS_ADD(wep, decrypt_fail);
+	DEBUGFS_FWSTATS_ADD(wep, packets);
+	DEBUGFS_FWSTATS_ADD(wep, interrupt);
+
+	DEBUGFS_FWSTATS_ADD(pwr, ps_enter);
+	DEBUGFS_FWSTATS_ADD(pwr, elp_enter);
+	DEBUGFS_FWSTATS_ADD(pwr, missing_bcns);
+	DEBUGFS_FWSTATS_ADD(pwr, wake_on_host);
+	DEBUGFS_FWSTATS_ADD(pwr, wake_on_timer_exp);
+	DEBUGFS_FWSTATS_ADD(pwr, tx_with_ps);
+	DEBUGFS_FWSTATS_ADD(pwr, tx_without_ps);
+	DEBUGFS_FWSTATS_ADD(pwr, rcvd_beacons);
+	DEBUGFS_FWSTATS_ADD(pwr, power_save_off);
+	DEBUGFS_FWSTATS_ADD(pwr, enable_ps);
+	DEBUGFS_FWSTATS_ADD(pwr, disable_ps);
+	DEBUGFS_FWSTATS_ADD(pwr, fix_tsf_ps);
+	/* skipping cont_miss_bcns_spread for now */
+	DEBUGFS_FWSTATS_ADD(pwr, rcvd_awake_beacons);
+
+	DEBUGFS_FWSTATS_ADD(mic, rx_pkts);
+	DEBUGFS_FWSTATS_ADD(mic, calc_failure);
+
+	DEBUGFS_FWSTATS_ADD(aes, encrypt_fail);
+	DEBUGFS_FWSTATS_ADD(aes, decrypt_fail);
+	DEBUGFS_FWSTATS_ADD(aes, encrypt_packets);
+	DEBUGFS_FWSTATS_ADD(aes, decrypt_packets);
+	DEBUGFS_FWSTATS_ADD(aes, encrypt_interrupt);
+	DEBUGFS_FWSTATS_ADD(aes, decrypt_interrupt);
+
+	DEBUGFS_FWSTATS_ADD(event, heart_beat);
+	DEBUGFS_FWSTATS_ADD(event, calibration);
+	DEBUGFS_FWSTATS_ADD(event, rx_mismatch);
+	DEBUGFS_FWSTATS_ADD(event, rx_mem_empty);
+	DEBUGFS_FWSTATS_ADD(event, rx_pool);
+	DEBUGFS_FWSTATS_ADD(event, oom_late);
+	DEBUGFS_FWSTATS_ADD(event, phy_transmit_error);
+	DEBUGFS_FWSTATS_ADD(event, tx_stuck);
+
+	DEBUGFS_FWSTATS_ADD(ps, pspoll_timeouts);
+	DEBUGFS_FWSTATS_ADD(ps, upsd_timeouts);
+	DEBUGFS_FWSTATS_ADD(ps, upsd_max_sptime);
+	DEBUGFS_FWSTATS_ADD(ps, upsd_max_apturn);
+	DEBUGFS_FWSTATS_ADD(ps, pspoll_max_apturn);
+	DEBUGFS_FWSTATS_ADD(ps, pspoll_utilization);
+	DEBUGFS_FWSTATS_ADD(ps, upsd_utilization);
+
+	DEBUGFS_FWSTATS_ADD(rxpipe, rx_prep_beacon_drop);
+	DEBUGFS_FWSTATS_ADD(rxpipe, descr_host_int_trig_rx_data);
+	DEBUGFS_FWSTATS_ADD(rxpipe, beacon_buffer_thres_host_int_trig_rx_data);
+	DEBUGFS_FWSTATS_ADD(rxpipe, missed_beacon_host_int_trig_rx_data);
+	DEBUGFS_FWSTATS_ADD(rxpipe, tx_xfr_host_int_trig_rx_data);
+
+	return 0;
+
+err:
+	if (IS_ERR(entry))
+		ret = PTR_ERR(entry);
+	else
+		ret = -ENOMEM;
+
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl12xx/debugfs.h b/drivers/net/wireless/ti/wl12xx/debugfs.h
new file mode 100644
index 00000000000..96898e291b7
--- /dev/null
+++ b/drivers/net/wireless/ti/wl12xx/debugfs.h
@@ -0,0 +1,28 @@
+/*
+ * This file is part of wl12xx
+ *
+ * Copyright (C) 2012 Texas Instruments. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL12XX_DEBUGFS_H__
+#define __WL12XX_DEBUGFS_H__
+
+int wl12xx_debugfs_add_files(struct wl1271 *wl,
+			     struct dentry *rootdir);
+
+#endif /* __WL12XX_DEBUGFS_H__ */
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
index d7dd3def07b..f429fc110cb 100644
--- a/drivers/net/wireless/ti/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -39,6 +39,10 @@
 #include "reg.h"
 #include "cmd.h"
 #include "acx.h"
+#include "debugfs.h"
+
+static char *fref_param;
+static char *tcxo_param;
 
 static struct wlcore_conf wl12xx_conf = {
 	.sg = {
@@ -212,7 +216,7 @@ static struct wlcore_conf wl12xx_conf = {
 		.suspend_wake_up_event       = CONF_WAKE_UP_EVENT_N_DTIM,
 		.suspend_listen_interval     = 3,
 		.bcn_filt_mode               = CONF_BCN_FILT_MODE_ENABLED,
-		.bcn_filt_ie_count           = 2,
+		.bcn_filt_ie_count           = 3,
 		.bcn_filt_ie = {
 			[0] = {
 				.ie          = WLAN_EID_CHANNEL_SWITCH,
@@ -222,9 +226,13 @@ static struct wlcore_conf wl12xx_conf = {
 				.ie          = WLAN_EID_HT_OPERATION,
 				.rule        = CONF_BCN_RULE_PASS_ON_CHANGE,
 			},
+			[2] = {
+				.ie	     = WLAN_EID_ERP_INFO,
+				.rule	     = CONF_BCN_RULE_PASS_ON_CHANGE,
+			},
 		},
-		.synch_fail_thold            = 10,
-		.bss_lose_timeout            = 100,
+		.synch_fail_thold            = 12,
+		.bss_lose_timeout            = 400,
 		.beacon_rx_timeout           = 10000,
 		.broadcast_timeout           = 20000,
 		.rx_broadcast_in_ps          = 1,
@@ -234,10 +242,11 @@ static struct wlcore_conf wl12xx_conf = {
 		.psm_entry_retries           = 8,
 		.psm_exit_retries            = 16,
 		.psm_entry_nullfunc_retries  = 3,
-		.dynamic_ps_timeout          = 40,
+		.dynamic_ps_timeout          = 1500,
 		.forced_ps                   = false,
 		.keep_alive_interval         = 55000,
 		.max_listen_interval         = 20,
+		.sta_sleep_auth              = WL1271_PSM_ILLEGAL,
 	},
 	.itrim = {
 		.enable = false,
@@ -245,7 +254,7 @@ static struct wlcore_conf wl12xx_conf = {
 	},
 	.pm_config = {
 		.host_clk_settling_time = 5000,
-		.host_fast_wakeup_support = false
+		.host_fast_wakeup_support = CONF_FAST_WAKEUP_DISABLE,
 	},
 	.roam_trigger = {
 		.trigger_pacing               = 1,
@@ -305,8 +314,8 @@ static struct wlcore_conf wl12xx_conf = {
 		.swallow_period               = 5,
 		.n_divider_fref_set_1         = 0xff,       /* default */
 		.n_divider_fref_set_2         = 12,
-		.m_divider_fref_set_1         = 148,
-		.m_divider_fref_set_2         = 0xffff,     /* default */
+		.m_divider_fref_set_1         = 0xffff,
+		.m_divider_fref_set_2         = 148,	    /* default */
 		.coex_pll_stabilization_time  = 0xffffffff, /* default */
 		.ldo_stabilization_time       = 0xffff,     /* default */
 		.fm_disturbed_band_margin     = 0xff,       /* default */
@@ -581,19 +590,21 @@ static const int wl12xx_rtable[REG_TABLE_LEN] = {
 };
 
 /* TODO: maybe move to a new header file? */
-#define WL127X_FW_NAME_MULTI	"ti-connectivity/wl127x-fw-4-mr.bin"
-#define WL127X_FW_NAME_SINGLE	"ti-connectivity/wl127x-fw-4-sr.bin"
-#define WL127X_PLT_FW_NAME	"ti-connectivity/wl127x-fw-4-plt.bin"
+#define WL127X_FW_NAME_MULTI	"ti-connectivity/wl127x-fw-5-mr.bin"
+#define WL127X_FW_NAME_SINGLE	"ti-connectivity/wl127x-fw-5-sr.bin"
+#define WL127X_PLT_FW_NAME	"ti-connectivity/wl127x-fw-5-plt.bin"
 
-#define WL128X_FW_NAME_MULTI	"ti-connectivity/wl128x-fw-4-mr.bin"
-#define WL128X_FW_NAME_SINGLE	"ti-connectivity/wl128x-fw-4-sr.bin"
-#define WL128X_PLT_FW_NAME	"ti-connectivity/wl128x-fw-4-plt.bin"
+#define WL128X_FW_NAME_MULTI	"ti-connectivity/wl128x-fw-5-mr.bin"
+#define WL128X_FW_NAME_SINGLE	"ti-connectivity/wl128x-fw-5-sr.bin"
+#define WL128X_PLT_FW_NAME	"ti-connectivity/wl128x-fw-5-plt.bin"
 
-static void wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
+static int wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
 {
+	int ret;
+
 	if (wl->chip.id != CHIP_ID_1283_PG20) {
 		struct wl1271_acx_mem_map *wl_mem_map = wl->target_mem_map;
-		struct wl1271_rx_mem_pool_addr rx_mem_addr;
+		struct wl127x_rx_mem_pool_addr rx_mem_addr;
 
 		/*
 		 * Choose the block we want to read
@@ -607,9 +618,13 @@ static void wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
 
 		rx_mem_addr.addr_extra = rx_mem_addr.addr + 4;
 
-		wl1271_write(wl, WL1271_SLV_REG_DATA,
-			     &rx_mem_addr, sizeof(rx_mem_addr), false);
+		ret = wlcore_write(wl, WL1271_SLV_REG_DATA, &rx_mem_addr,
+				   sizeof(rx_mem_addr), false);
+		if (ret < 0)
+			return ret;
 	}
+
+	return 0;
 }
 
 static int wl12xx_identify_chip(struct wl1271 *wl)
@@ -621,10 +636,9 @@ static int wl12xx_identify_chip(struct wl1271 *wl)
 		wl1271_warning("chip id 0x%x (1271 PG10) support is obsolete",
 			       wl->chip.id);
 
-		/* clear the alignment quirk, since we don't support it */
-		wl->quirks &= ~WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
-
-		wl->quirks |= WLCORE_QUIRK_LEGACY_NVS;
+		wl->quirks |= WLCORE_QUIRK_LEGACY_NVS |
+			      WLCORE_QUIRK_DUAL_PROBE_TMPL |
+			      WLCORE_QUIRK_TKIP_HEADER_SPACE;
 		wl->sr_fw_name = WL127X_FW_NAME_SINGLE;
 		wl->mr_fw_name = WL127X_FW_NAME_MULTI;
 		memcpy(&wl->conf.mem, &wl12xx_default_priv_conf.mem_wl127x,
@@ -633,16 +647,18 @@ static int wl12xx_identify_chip(struct wl1271 *wl)
 		/* read data preparation is only needed by wl127x */
 		wl->ops->prepare_read = wl127x_prepare_read;
 
+		wlcore_set_min_fw_ver(wl, WL127X_CHIP_VER, WL127X_IFTYPE_VER,
+				      WL127X_MAJOR_VER, WL127X_SUBTYPE_VER,
+				      WL127X_MINOR_VER);
 		break;
 
 	case CHIP_ID_1271_PG20:
 		wl1271_debug(DEBUG_BOOT, "chip id 0x%x (1271 PG20)",
 			     wl->chip.id);
 
-		/* clear the alignment quirk, since we don't support it */
-		wl->quirks &= ~WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
-
-		wl->quirks |= WLCORE_QUIRK_LEGACY_NVS;
+		wl->quirks |= WLCORE_QUIRK_LEGACY_NVS |
+			      WLCORE_QUIRK_DUAL_PROBE_TMPL |
+			      WLCORE_QUIRK_TKIP_HEADER_SPACE;
 		wl->plt_fw_name = WL127X_PLT_FW_NAME;
 		wl->sr_fw_name = WL127X_FW_NAME_SINGLE;
 		wl->mr_fw_name = WL127X_FW_NAME_MULTI;
@@ -652,6 +668,9 @@ static int wl12xx_identify_chip(struct wl1271 *wl)
 		/* read data preparation is only needed by wl127x */
 		wl->ops->prepare_read = wl127x_prepare_read;
 
+		wlcore_set_min_fw_ver(wl, WL127X_CHIP_VER, WL127X_IFTYPE_VER,
+				      WL127X_MAJOR_VER, WL127X_SUBTYPE_VER,
+				      WL127X_MINOR_VER);
 		break;
 
 	case CHIP_ID_1283_PG20:
@@ -660,6 +679,15 @@ static int wl12xx_identify_chip(struct wl1271 *wl)
 		wl->plt_fw_name = WL128X_PLT_FW_NAME;
 		wl->sr_fw_name = WL128X_FW_NAME_SINGLE;
 		wl->mr_fw_name = WL128X_FW_NAME_MULTI;
+
+		/* wl128x requires TX blocksize alignment */
+		wl->quirks |= WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN |
+			      WLCORE_QUIRK_DUAL_PROBE_TMPL |
+			      WLCORE_QUIRK_TKIP_HEADER_SPACE;
+
+		wlcore_set_min_fw_ver(wl, WL128X_CHIP_VER, WL128X_IFTYPE_VER,
+				      WL128X_MAJOR_VER, WL128X_SUBTYPE_VER,
+				      WL128X_MINOR_VER);
 		break;
 	case CHIP_ID_1283_PG10:
 	default:
@@ -672,64 +700,95 @@ out:
 	return ret;
 }
 
-static void wl12xx_top_reg_write(struct wl1271 *wl, int addr, u16 val)
+static int __must_check wl12xx_top_reg_write(struct wl1271 *wl, int addr,
+					     u16 val)
 {
+	int ret;
+
 	/* write address >> 1 + 0x30000 to OCP_POR_CTR */
 	addr = (addr >> 1) + 0x30000;
-	wl1271_write32(wl, WL12XX_OCP_POR_CTR, addr);
+	ret = wlcore_write32(wl, WL12XX_OCP_POR_CTR, addr);
+	if (ret < 0)
+		goto out;
 
 	/* write value to OCP_POR_WDATA */
-	wl1271_write32(wl, WL12XX_OCP_DATA_WRITE, val);
+	ret = wlcore_write32(wl, WL12XX_OCP_DATA_WRITE, val);
+	if (ret < 0)
+		goto out;
 
 	/* write 1 to OCP_CMD */
-	wl1271_write32(wl, WL12XX_OCP_CMD, OCP_CMD_WRITE);
+	ret = wlcore_write32(wl, WL12XX_OCP_CMD, OCP_CMD_WRITE);
+	if (ret < 0)
+		goto out;
+
+out:
+	return ret;
 }
 
-static u16 wl12xx_top_reg_read(struct wl1271 *wl, int addr)
+static int __must_check wl12xx_top_reg_read(struct wl1271 *wl, int addr,
+					    u16 *out)
 {
 	u32 val;
 	int timeout = OCP_CMD_LOOP;
+	int ret;
 
 	/* write address >> 1 + 0x30000 to OCP_POR_CTR */
 	addr = (addr >> 1) + 0x30000;
-	wl1271_write32(wl, WL12XX_OCP_POR_CTR, addr);
+	ret = wlcore_write32(wl, WL12XX_OCP_POR_CTR, addr);
+	if (ret < 0)
+		return ret;
 
 	/* write 2 to OCP_CMD */
-	wl1271_write32(wl, WL12XX_OCP_CMD, OCP_CMD_READ);
+	ret = wlcore_write32(wl, WL12XX_OCP_CMD, OCP_CMD_READ);
+	if (ret < 0)
+		return ret;
 
 	/* poll for data ready */
 	do {
-		val = wl1271_read32(wl, WL12XX_OCP_DATA_READ);
+		ret = wlcore_read32(wl, WL12XX_OCP_DATA_READ, &val);
+		if (ret < 0)
+			return ret;
 	} while (!(val & OCP_READY_MASK) && --timeout);
 
 	if (!timeout) {
 		wl1271_warning("Top register access timed out.");
-		return 0xffff;
+		return -ETIMEDOUT;
 	}
 
 	/* check data status and return if OK */
-	if ((val & OCP_STATUS_MASK) == OCP_STATUS_OK)
-		return val & 0xffff;
-	else {
+	if ((val & OCP_STATUS_MASK) != OCP_STATUS_OK) {
 		wl1271_warning("Top register access returned error.");
-		return 0xffff;
+		return -EIO;
 	}
+
+	if (out)
+		*out = val & 0xffff;
+
+	return 0;
 }
 
 static int wl128x_switch_tcxo_to_fref(struct wl1271 *wl)
 {
 	u16 spare_reg;
+	int ret;
 
 	/* Mask bits [2] & [8:4] in the sys_clk_cfg register */
-	spare_reg = wl12xx_top_reg_read(wl, WL_SPARE_REG);
+	ret = wl12xx_top_reg_read(wl, WL_SPARE_REG, &spare_reg);
+	if (ret < 0)
+		return ret;
+
 	if (spare_reg == 0xFFFF)
 		return -EFAULT;
 	spare_reg |= (BIT(3) | BIT(5) | BIT(6));
-	wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+	ret = wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+	if (ret < 0)
+		return ret;
 
 	/* Enable FREF_CLK_REQ & mux MCS and coex PLLs to FREF */
-	wl12xx_top_reg_write(wl, SYS_CLK_CFG_REG,
-			     WL_CLK_REQ_TYPE_PG2 | MCS_PLL_CLK_SEL_FREF);
+	ret = wl12xx_top_reg_write(wl, SYS_CLK_CFG_REG,
+				   WL_CLK_REQ_TYPE_PG2 | MCS_PLL_CLK_SEL_FREF);
+	if (ret < 0)
+		return ret;
 
 	/* Delay execution for 15msec, to let the HW settle */
 	mdelay(15);
@@ -740,8 +799,12 @@ static int wl128x_switch_tcxo_to_fref(struct wl1271 *wl)
 static bool wl128x_is_tcxo_valid(struct wl1271 *wl)
 {
 	u16 tcxo_detection;
+	int ret;
+
+	ret = wl12xx_top_reg_read(wl, TCXO_CLK_DETECT_REG, &tcxo_detection);
+	if (ret < 0)
+		return false;
 
-	tcxo_detection = wl12xx_top_reg_read(wl, TCXO_CLK_DETECT_REG);
 	if (tcxo_detection & TCXO_DET_FAILED)
 		return false;
 
@@ -751,8 +814,12 @@ static bool wl128x_is_tcxo_valid(struct wl1271 *wl)
 static bool wl128x_is_fref_valid(struct wl1271 *wl)
 {
 	u16 fref_detection;
+	int ret;
+
+	ret = wl12xx_top_reg_read(wl, FREF_CLK_DETECT_REG, &fref_detection);
+	if (ret < 0)
+		return false;
 
-	fref_detection = wl12xx_top_reg_read(wl, FREF_CLK_DETECT_REG);
 	if (fref_detection & FREF_CLK_DETECT_FAIL)
 		return false;
 
@@ -761,11 +828,21 @@ static bool wl128x_is_fref_valid(struct wl1271 *wl)
 
 static int wl128x_manually_configure_mcs_pll(struct wl1271 *wl)
 {
-	wl12xx_top_reg_write(wl, MCS_PLL_M_REG, MCS_PLL_M_REG_VAL);
-	wl12xx_top_reg_write(wl, MCS_PLL_N_REG, MCS_PLL_N_REG_VAL);
-	wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, MCS_PLL_CONFIG_REG_VAL);
+	int ret;
 
-	return 0;
+	ret = wl12xx_top_reg_write(wl, MCS_PLL_M_REG, MCS_PLL_M_REG_VAL);
+	if (ret < 0)
+		goto out;
+
+	ret = wl12xx_top_reg_write(wl, MCS_PLL_N_REG, MCS_PLL_N_REG_VAL);
+	if (ret < 0)
+		goto out;
+
+	ret = wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG,
+				   MCS_PLL_CONFIG_REG_VAL);
+
+out:
+	return ret;
 }
 
 static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
@@ -773,30 +850,40 @@ static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
 	u16 spare_reg;
 	u16 pll_config;
 	u8 input_freq;
+	struct wl12xx_priv *priv = wl->priv;
+	int ret;
 
 	/* Mask bits [3:1] in the sys_clk_cfg register */
-	spare_reg = wl12xx_top_reg_read(wl, WL_SPARE_REG);
+	ret = wl12xx_top_reg_read(wl, WL_SPARE_REG, &spare_reg);
+	if (ret < 0)
+		return ret;
+
 	if (spare_reg == 0xFFFF)
 		return -EFAULT;
 	spare_reg |= BIT(2);
-	wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+	ret = wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+	if (ret < 0)
+		return ret;
 
 	/* Handle special cases of the TCXO clock */
-	if (wl->tcxo_clock == WL12XX_TCXOCLOCK_16_8 ||
-	    wl->tcxo_clock == WL12XX_TCXOCLOCK_33_6)
+	if (priv->tcxo_clock == WL12XX_TCXOCLOCK_16_8 ||
+	    priv->tcxo_clock == WL12XX_TCXOCLOCK_33_6)
 		return wl128x_manually_configure_mcs_pll(wl);
 
 	/* Set the input frequency according to the selected clock source */
 	input_freq = (clk & 1) + 1;
 
-	pll_config = wl12xx_top_reg_read(wl, MCS_PLL_CONFIG_REG);
+	ret = wl12xx_top_reg_read(wl, MCS_PLL_CONFIG_REG, &pll_config);
+	if (ret < 0)
+		return ret;
+
 	if (pll_config == 0xFFFF)
 		return -EFAULT;
 	pll_config |= (input_freq << MCS_SEL_IN_FREQ_SHIFT);
 	pll_config |= MCS_PLL_ENABLE_HP;
-	wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, pll_config);
+	ret = wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, pll_config);
 
-	return 0;
+	return ret;
 }
 
 /*
@@ -808,26 +895,31 @@ static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
  */
 static int wl128x_boot_clk(struct wl1271 *wl, int *selected_clock)
 {
+	struct wl12xx_priv *priv = wl->priv;
 	u16 sys_clk_cfg;
+	int ret;
 
 	/* For XTAL-only modes, FREF will be used after switching from TCXO */
-	if (wl->ref_clock == WL12XX_REFCLOCK_26_XTAL ||
-	    wl->ref_clock == WL12XX_REFCLOCK_38_XTAL) {
+	if (priv->ref_clock == WL12XX_REFCLOCK_26_XTAL ||
+	    priv->ref_clock == WL12XX_REFCLOCK_38_XTAL) {
 		if (!wl128x_switch_tcxo_to_fref(wl))
 			return -EINVAL;
 		goto fref_clk;
 	}
 
 	/* Query the HW, to determine which clock source we should use */
-	sys_clk_cfg = wl12xx_top_reg_read(wl, SYS_CLK_CFG_REG);
+	ret = wl12xx_top_reg_read(wl, SYS_CLK_CFG_REG, &sys_clk_cfg);
+	if (ret < 0)
+		return ret;
+
 	if (sys_clk_cfg == 0xFFFF)
 		return -EINVAL;
 	if (sys_clk_cfg & PRCM_CM_EN_MUX_WLAN_FREF)
 		goto fref_clk;
 
 	/* If TCXO is either 32.736MHz or 16.368MHz, switch to FREF */
-	if (wl->tcxo_clock == WL12XX_TCXOCLOCK_16_368 ||
-	    wl->tcxo_clock == WL12XX_TCXOCLOCK_32_736) {
+	if (priv->tcxo_clock == WL12XX_TCXOCLOCK_16_368 ||
+	    priv->tcxo_clock == WL12XX_TCXOCLOCK_32_736) {
 		if (!wl128x_switch_tcxo_to_fref(wl))
 			return -EINVAL;
 		goto fref_clk;
@@ -836,14 +928,14 @@ static int wl128x_boot_clk(struct wl1271 *wl, int *selected_clock)
 	/* TCXO clock is selected */
 	if (!wl128x_is_tcxo_valid(wl))
 		return -EINVAL;
-	*selected_clock = wl->tcxo_clock;
+	*selected_clock = priv->tcxo_clock;
 	goto config_mcs_pll;
 
 fref_clk:
 	/* FREF clock is selected */
 	if (!wl128x_is_fref_valid(wl))
 		return -EINVAL;
-	*selected_clock = wl->ref_clock;
+	*selected_clock = priv->ref_clock;
 
 config_mcs_pll:
 	return wl128x_configure_mcs_pll(wl, *selected_clock);
@@ -851,69 +943,98 @@ config_mcs_pll:
 
 static int wl127x_boot_clk(struct wl1271 *wl)
 {
+	struct wl12xx_priv *priv = wl->priv;
 	u32 pause;
 	u32 clk;
+	int ret;
 
 	if (WL127X_PG_GET_MAJOR(wl->hw_pg_ver) < 3)
 		wl->quirks |= WLCORE_QUIRK_END_OF_TRANSACTION;
 
-	if (wl->ref_clock == CONF_REF_CLK_19_2_E ||
-	    wl->ref_clock == CONF_REF_CLK_38_4_E ||
-	    wl->ref_clock == CONF_REF_CLK_38_4_M_XTAL)
+	if (priv->ref_clock == CONF_REF_CLK_19_2_E ||
+	    priv->ref_clock == CONF_REF_CLK_38_4_E ||
+	    priv->ref_clock == CONF_REF_CLK_38_4_M_XTAL)
 		/* ref clk: 19.2/38.4/38.4-XTAL */
 		clk = 0x3;
-	else if (wl->ref_clock == CONF_REF_CLK_26_E ||
-		 wl->ref_clock == CONF_REF_CLK_52_E)
+	else if (priv->ref_clock == CONF_REF_CLK_26_E ||
+		 priv->ref_clock == CONF_REF_CLK_26_M_XTAL ||
+		 priv->ref_clock == CONF_REF_CLK_52_E)
 		/* ref clk: 26/52 */
 		clk = 0x5;
 	else
 		return -EINVAL;
 
-	if (wl->ref_clock != CONF_REF_CLK_19_2_E) {
+	if (priv->ref_clock != CONF_REF_CLK_19_2_E) {
 		u16 val;
 		/* Set clock type (open drain) */
-		val = wl12xx_top_reg_read(wl, OCP_REG_CLK_TYPE);
+		ret = wl12xx_top_reg_read(wl, OCP_REG_CLK_TYPE, &val);
+		if (ret < 0)
+			goto out;
+
 		val &= FREF_CLK_TYPE_BITS;
-		wl12xx_top_reg_write(wl, OCP_REG_CLK_TYPE, val);
+		ret = wl12xx_top_reg_write(wl, OCP_REG_CLK_TYPE, val);
+		if (ret < 0)
+			goto out;
 
 		/* Set clock pull mode (no pull) */
-		val = wl12xx_top_reg_read(wl, OCP_REG_CLK_PULL);
+		ret = wl12xx_top_reg_read(wl, OCP_REG_CLK_PULL, &val);
+		if (ret < 0)
+			goto out;
+
 		val |= NO_PULL;
-		wl12xx_top_reg_write(wl, OCP_REG_CLK_PULL, val);
+		ret = wl12xx_top_reg_write(wl, OCP_REG_CLK_PULL, val);
+		if (ret < 0)
+			goto out;
 	} else {
 		u16 val;
 		/* Set clock polarity */
-		val = wl12xx_top_reg_read(wl, OCP_REG_CLK_POLARITY);
+		ret = wl12xx_top_reg_read(wl, OCP_REG_CLK_POLARITY, &val);
+		if (ret < 0)
+			goto out;
+
 		val &= FREF_CLK_POLARITY_BITS;
 		val |= CLK_REQ_OUTN_SEL;
-		wl12xx_top_reg_write(wl, OCP_REG_CLK_POLARITY, val);
+		ret = wl12xx_top_reg_write(wl, OCP_REG_CLK_POLARITY, val);
+		if (ret < 0)
+			goto out;
 	}
 
-	wl1271_write32(wl, WL12XX_PLL_PARAMETERS, clk);
+	ret = wlcore_write32(wl, WL12XX_PLL_PARAMETERS, clk);
+	if (ret < 0)
+		goto out;
 
-	pause = wl1271_read32(wl, WL12XX_PLL_PARAMETERS);
+	ret = wlcore_read32(wl, WL12XX_PLL_PARAMETERS, &pause);
+	if (ret < 0)
+		goto out;
 
 	wl1271_debug(DEBUG_BOOT, "pause1 0x%x", pause);
 
 	pause &= ~(WU_COUNTER_PAUSE_VAL);
 	pause |= WU_COUNTER_PAUSE_VAL;
-	wl1271_write32(wl, WL12XX_WU_COUNTER_PAUSE, pause);
+	ret = wlcore_write32(wl, WL12XX_WU_COUNTER_PAUSE, pause);
 
-	return 0;
+out:
+	return ret;
 }
 
 static int wl1271_boot_soft_reset(struct wl1271 *wl)
 {
 	unsigned long timeout;
 	u32 boot_data;
+	int ret = 0;
 
 	/* perform soft reset */
-	wl1271_write32(wl, WL12XX_SLV_SOFT_RESET, ACX_SLV_SOFT_RESET_BIT);
+	ret = wlcore_write32(wl, WL12XX_SLV_SOFT_RESET, ACX_SLV_SOFT_RESET_BIT);
+	if (ret < 0)
+		goto out;
 
 	/* SOFT_RESET is self clearing */
 	timeout = jiffies + usecs_to_jiffies(SOFT_RESET_MAX_TIME);
 	while (1) {
-		boot_data = wl1271_read32(wl, WL12XX_SLV_SOFT_RESET);
+		ret = wlcore_read32(wl, WL12XX_SLV_SOFT_RESET, &boot_data);
+		if (ret < 0)
+			goto out;
+
 		wl1271_debug(DEBUG_BOOT, "soft reset bootdata 0x%x", boot_data);
 		if ((boot_data & ACX_SLV_SOFT_RESET_BIT) == 0)
 			break;
@@ -929,16 +1050,20 @@ static int wl1271_boot_soft_reset(struct wl1271 *wl)
 	}
 
 	/* disable Rx/Tx */
-	wl1271_write32(wl, WL12XX_ENABLE, 0x0);
+	ret = wlcore_write32(wl, WL12XX_ENABLE, 0x0);
+	if (ret < 0)
+		goto out;
 
 	/* disable auto calibration on start*/
-	wl1271_write32(wl, WL12XX_SPARE_A2, 0xffff);
+	ret = wlcore_write32(wl, WL12XX_SPARE_A2, 0xffff);
 
-	return 0;
+out:
+	return ret;
 }
 
 static int wl12xx_pre_boot(struct wl1271 *wl)
 {
+	struct wl12xx_priv *priv = wl->priv;
 	int ret = 0;
 	u32 clk;
 	int selected_clock = -1;
@@ -954,30 +1079,43 @@ static int wl12xx_pre_boot(struct wl1271 *wl)
 	}
 
 	/* Continue the ELP wake up sequence */
-	wl1271_write32(wl, WL12XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+	ret = wlcore_write32(wl, WL12XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+	if (ret < 0)
+		goto out;
+
 	udelay(500);
 
-	wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+	if (ret < 0)
+		goto out;
 
 	/* Read-modify-write DRPW_SCRATCH_START register (see next state)
 	   to be used by DRPw FW. The RTRIM value will be added by the FW
 	   before taking DRPw out of reset */
 
-	clk = wl1271_read32(wl, WL12XX_DRPW_SCRATCH_START);
+	ret = wlcore_read32(wl, WL12XX_DRPW_SCRATCH_START, &clk);
+	if (ret < 0)
+		goto out;
 
 	wl1271_debug(DEBUG_BOOT, "clk2 0x%x", clk);
 
 	if (wl->chip.id == CHIP_ID_1283_PG20)
 		clk |= ((selected_clock & 0x3) << 1) << 4;
 	else
-		clk |= (wl->ref_clock << 1) << 4;
+		clk |= (priv->ref_clock << 1) << 4;
 
-	wl1271_write32(wl, WL12XX_DRPW_SCRATCH_START, clk);
+	ret = wlcore_write32(wl, WL12XX_DRPW_SCRATCH_START, clk);
+	if (ret < 0)
+		goto out;
 
-	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+	if (ret < 0)
+		goto out;
 
 	/* Disable interrupts */
-	wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+	if (ret < 0)
+		goto out;
 
 	ret = wl1271_boot_soft_reset(wl);
 	if (ret < 0)
@@ -987,47 +1125,72 @@ out:
 	return ret;
 }
 
-static void wl12xx_pre_upload(struct wl1271 *wl)
+static int wl12xx_pre_upload(struct wl1271 *wl)
 {
 	u32 tmp;
+	u16 polarity;
+	int ret;
 
 	/* write firmware's last address (ie. it's length) to
 	 * ACX_EEPROMLESS_IND_REG */
 	wl1271_debug(DEBUG_BOOT, "ACX_EEPROMLESS_IND_REG");
 
-	wl1271_write32(wl, WL12XX_EEPROMLESS_IND, WL12XX_EEPROMLESS_IND);
+	ret = wlcore_write32(wl, WL12XX_EEPROMLESS_IND, WL12XX_EEPROMLESS_IND);
+	if (ret < 0)
+		goto out;
 
-	tmp = wlcore_read_reg(wl, REG_CHIP_ID_B);
+	ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &tmp);
+	if (ret < 0)
+		goto out;
 
 	wl1271_debug(DEBUG_BOOT, "chip id 0x%x", tmp);
 
 	/* 6. read the EEPROM parameters */
-	tmp = wl1271_read32(wl, WL12XX_SCR_PAD2);
+	ret = wlcore_read32(wl, WL12XX_SCR_PAD2, &tmp);
+	if (ret < 0)
+		goto out;
 
 	/* WL1271: The reference driver skips steps 7 to 10 (jumps directly
 	 * to upload_fw) */
 
-	if (wl->chip.id == CHIP_ID_1283_PG20)
-		wl12xx_top_reg_write(wl, SDIO_IO_DS, HCI_IO_DS_6MA);
-}
-
-static void wl12xx_enable_interrupts(struct wl1271 *wl)
-{
-	u32 polarity;
+	if (wl->chip.id == CHIP_ID_1283_PG20) {
+		ret = wl12xx_top_reg_write(wl, SDIO_IO_DS, HCI_IO_DS_6MA);
+		if (ret < 0)
+			goto out;
+	}
 
-	polarity = wl12xx_top_reg_read(wl, OCP_REG_POLARITY);
+	/* polarity must be set before the firmware is loaded */
+	ret = wl12xx_top_reg_read(wl, OCP_REG_POLARITY, &polarity);
+	if (ret < 0)
+		goto out;
 
 	/* We use HIGH polarity, so unset the LOW bit */
 	polarity &= ~POLARITY_LOW;
-	wl12xx_top_reg_write(wl, OCP_REG_POLARITY, polarity);
+	ret = wl12xx_top_reg_write(wl, OCP_REG_POLARITY, polarity);
+
+out:
+	return ret;
+}
 
-	wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_ALL_EVENTS_VECTOR);
+static int wl12xx_enable_interrupts(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+			       WL12XX_ACX_ALL_EVENTS_VECTOR);
+	if (ret < 0)
+		goto out;
 
 	wlcore_enable_interrupts(wl);
-	wlcore_write_reg(wl, REG_INTERRUPT_MASK,
-			 WL1271_ACX_INTR_ALL & ~(WL1271_INTR_MASK));
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+			       WL1271_ACX_INTR_ALL & ~(WL12XX_INTR_MASK));
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_write32(wl, WL12XX_HI_CFG, HI_CFG_DEF_VAL);
 
-	wl1271_write32(wl, WL12XX_HI_CFG, HI_CFG_DEF_VAL);
+out:
+	return ret;
 }
 
 static int wl12xx_boot(struct wl1271 *wl)
@@ -1042,7 +1205,9 @@ static int wl12xx_boot(struct wl1271 *wl)
 	if (ret < 0)
 		goto out;
 
-	wl12xx_pre_upload(wl);
+	ret = wl12xx_pre_upload(wl);
+	if (ret < 0)
+		goto out;
 
 	ret = wlcore_boot_upload_firmware(wl);
 	if (ret < 0)
@@ -1052,22 +1217,30 @@ static int wl12xx_boot(struct wl1271 *wl)
 	if (ret < 0)
 		goto out;
 
-	wl12xx_enable_interrupts(wl);
+	ret = wl12xx_enable_interrupts(wl);
 
 out:
 	return ret;
 }
 
-static void wl12xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
+static int wl12xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
 			       void *buf, size_t len)
 {
-	wl1271_write(wl, cmd_box_addr, buf, len, false);
-	wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_CMD);
+	int ret;
+
+	ret = wlcore_write(wl, cmd_box_addr, buf, len, false);
+	if (ret < 0)
+		return ret;
+
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_CMD);
+
+	return ret;
 }
 
-static void wl12xx_ack_event(struct wl1271 *wl)
+static int wl12xx_ack_event(struct wl1271 *wl)
 {
-	wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_EVENT_ACK);
+	return wlcore_write_reg(wl, REG_INTERRUPT_TRIG,
+				WL12XX_INTR_TRIG_EVENT_ACK);
 }
 
 static u32 wl12xx_calc_tx_blocks(struct wl1271 *wl, u32 len, u32 spare_blks)
@@ -1147,12 +1320,13 @@ static u32 wl12xx_get_rx_packet_len(struct wl1271 *wl, void *rx_data,
 	return data_len - sizeof(*desc) - desc->pad_len;
 }
 
-static void wl12xx_tx_delayed_compl(struct wl1271 *wl)
+static int wl12xx_tx_delayed_compl(struct wl1271 *wl)
 {
-	if (wl->fw_status->tx_results_counter == (wl->tx_results_count & 0xff))
-		return;
+	if (wl->fw_status_1->tx_results_counter ==
+	    (wl->tx_results_count & 0xff))
+		return 0;
 
-	wl1271_tx_complete(wl);
+	return wlcore_tx_complete(wl);
 }
 
 static int wl12xx_hw_init(struct wl1271 *wl)
@@ -1165,6 +1339,14 @@ static int wl12xx_hw_init(struct wl1271 *wl)
 		ret = wl128x_cmd_general_parms(wl);
 		if (ret < 0)
 			goto out;
+
+		/*
+		 * If we are in calibrator based auto detect then we got the FEM nr
+		 * in wl->fem_manuf. No need to continue further
+		 */
+		if (wl->plt_mode == PLT_FEM_DETECT)
+			goto out;
+
 		ret = wl128x_cmd_radio_parms(wl);
 		if (ret < 0)
 			goto out;
@@ -1181,6 +1363,14 @@ static int wl12xx_hw_init(struct wl1271 *wl)
 		ret = wl1271_cmd_general_parms(wl);
 		if (ret < 0)
 			goto out;
+
+		/*
+		 * If we are in calibrator based auto detect then we got the FEM nr
+		 * in wl->fem_manuf. No need to continue further
+		 */
+		if (wl->plt_mode == PLT_FEM_DETECT)
+			goto out;
+
 		ret = wl1271_cmd_radio_parms(wl);
 		if (ret < 0)
 			goto out;
@@ -1253,45 +1443,151 @@ static bool wl12xx_mac_in_fuse(struct wl1271 *wl)
 	return supported;
 }
 
-static void wl12xx_get_fuse_mac(struct wl1271 *wl)
+static int wl12xx_get_fuse_mac(struct wl1271 *wl)
 {
 	u32 mac1, mac2;
+	int ret;
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+	if (ret < 0)
+		goto out;
 
-	wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+	ret = wlcore_read32(wl, WL12XX_REG_FUSE_BD_ADDR_1, &mac1);
+	if (ret < 0)
+		goto out;
 
-	mac1 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_1);
-	mac2 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_2);
+	ret = wlcore_read32(wl, WL12XX_REG_FUSE_BD_ADDR_2, &mac2);
+	if (ret < 0)
+		goto out;
 
 	/* these are the two parts of the BD_ADDR */
 	wl->fuse_oui_addr = ((mac2 & 0xffff) << 8) +
 		((mac1 & 0xff000000) >> 24);
 	wl->fuse_nic_addr = mac1 & 0xffffff;
 
-	wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+
+out:
+	return ret;
 }
 
-static s8 wl12xx_get_pg_ver(struct wl1271 *wl)
+static int wl12xx_get_pg_ver(struct wl1271 *wl, s8 *ver)
 {
-	u32 die_info;
+	u16 die_info;
+	int ret;
 
 	if (wl->chip.id == CHIP_ID_1283_PG20)
-		die_info = wl12xx_top_reg_read(wl, WL128X_REG_FUSE_DATA_2_1);
+		ret = wl12xx_top_reg_read(wl, WL128X_REG_FUSE_DATA_2_1,
+					  &die_info);
 	else
-		die_info = wl12xx_top_reg_read(wl, WL127X_REG_FUSE_DATA_2_1);
+		ret = wl12xx_top_reg_read(wl, WL127X_REG_FUSE_DATA_2_1,
+					  &die_info);
 
-	return (s8) (die_info & PG_VER_MASK) >> PG_VER_OFFSET;
+	if (ret >= 0 && ver)
+		*ver = (s8)((die_info & PG_VER_MASK) >> PG_VER_OFFSET);
+
+	return ret;
 }
 
-static void wl12xx_get_mac(struct wl1271 *wl)
+static int wl12xx_get_mac(struct wl1271 *wl)
 {
 	if (wl12xx_mac_in_fuse(wl))
-		wl12xx_get_fuse_mac(wl);
+		return wl12xx_get_fuse_mac(wl);
+
+	return 0;
+}
+
+static void wl12xx_set_tx_desc_csum(struct wl1271 *wl,
+				    struct wl1271_tx_hw_descr *desc,
+				    struct sk_buff *skb)
+{
+	desc->wl12xx_reserved = 0;
+}
+
+static int wl12xx_plt_init(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wl->ops->boot(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl->ops->hw_init(wl);
+	if (ret < 0)
+		goto out_irq_disable;
+
+	/*
+	 * If we are in calibrator based auto detect then we got the FEM nr
+	 * in wl->fem_manuf. No need to continue further
+	 */
+	if (wl->plt_mode == PLT_FEM_DETECT)
+		goto out;
+
+	ret = wl1271_acx_init_mem_config(wl);
+	if (ret < 0)
+		goto out_irq_disable;
+
+	ret = wl12xx_acx_mem_cfg(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 CAM power saving (ie. always active) */
+	ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+	if (ret < 0)
+		goto out_free_memmap;
+
+	/* configure PM */
+	ret = wl1271_acx_pm_config(wl);
+	if (ret < 0)
+		goto out_free_memmap;
+
+	goto out;
+
+out_free_memmap:
+	kfree(wl->target_mem_map);
+	wl->target_mem_map = NULL;
+
+out_irq_disable:
+	mutex_unlock(&wl->mutex);
+	/* Unlocking the mutex in the middle of handling is
+	   inherently unsafe. In this case we deem it safe to do,
+	   because we need to let any possibly pending IRQ out of
+	   the system (and while we are WL1271_STATE_OFF the IRQ
+	   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. */
+	wlcore_disable_interrupts(wl);
+	mutex_lock(&wl->mutex);
+out:
+	return ret;
+}
+
+static int wl12xx_get_spare_blocks(struct wl1271 *wl, bool is_gem)
+{
+	if (is_gem)
+		return WL12XX_TX_HW_BLOCK_GEM_SPARE;
+
+	return WL12XX_TX_HW_BLOCK_SPARE_DEFAULT;
+}
+
+static int wl12xx_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
+			  struct ieee80211_vif *vif,
+			  struct ieee80211_sta *sta,
+			  struct ieee80211_key_conf *key_conf)
+{
+	return wlcore_set_key(wl, cmd, vif, sta, key_conf);
 }
 
 static struct wlcore_ops wl12xx_ops = {
 	.identify_chip		= wl12xx_identify_chip,
 	.identify_fw		= wl12xx_identify_fw,
 	.boot			= wl12xx_boot,
+	.plt_init		= wl12xx_plt_init,
 	.trigger_cmd		= wl12xx_trigger_cmd,
 	.ack_event		= wl12xx_ack_event,
 	.calc_tx_blocks		= wl12xx_calc_tx_blocks,
@@ -1306,6 +1602,13 @@ static struct wlcore_ops wl12xx_ops = {
 	.sta_get_ap_rate_mask	= wl12xx_sta_get_ap_rate_mask,
 	.get_pg_ver		= wl12xx_get_pg_ver,
 	.get_mac		= wl12xx_get_mac,
+	.set_tx_desc_csum	= wl12xx_set_tx_desc_csum,
+	.set_rx_csum		= NULL,
+	.ap_get_mimo_wide_rate_mask = NULL,
+	.debugfs_init		= wl12xx_debugfs_add_files,
+	.get_spare_blocks	= wl12xx_get_spare_blocks,
+	.set_key		= wl12xx_set_key,
+	.pre_pkt_send		= NULL,
 };
 
 static struct ieee80211_sta_ht_cap wl12xx_ht_cap = {
@@ -1323,6 +1626,7 @@ static struct ieee80211_sta_ht_cap wl12xx_ht_cap = {
 
 static int __devinit wl12xx_probe(struct platform_device *pdev)
 {
+	struct wl12xx_platform_data *pdata = pdev->dev.platform_data;
 	struct wl1271 *wl;
 	struct ieee80211_hw *hw;
 	struct wl12xx_priv *priv;
@@ -1334,19 +1638,63 @@ static int __devinit wl12xx_probe(struct platform_device *pdev)
 	}
 
 	wl = hw->priv;
+	priv = wl->priv;
 	wl->ops = &wl12xx_ops;
 	wl->ptable = wl12xx_ptable;
 	wl->rtable = wl12xx_rtable;
 	wl->num_tx_desc = 16;
-	wl->normal_tx_spare = WL12XX_TX_HW_BLOCK_SPARE_DEFAULT;
-	wl->gem_tx_spare = WL12XX_TX_HW_BLOCK_GEM_SPARE;
+	wl->num_rx_desc = 8;
 	wl->band_rate_to_idx = wl12xx_band_rate_to_idx;
 	wl->hw_tx_rate_tbl_size = WL12XX_CONF_HW_RXTX_RATE_MAX;
 	wl->hw_min_ht_rate = WL12XX_CONF_HW_RXTX_RATE_MCS0;
 	wl->fw_status_priv_len = 0;
-	memcpy(&wl->ht_cap, &wl12xx_ht_cap, sizeof(wl12xx_ht_cap));
+	wl->stats.fw_stats_len = sizeof(struct wl12xx_acx_statistics);
+	wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ, &wl12xx_ht_cap);
+	wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ, &wl12xx_ht_cap);
 	wl12xx_conf_init(wl);
 
+	if (!fref_param) {
+		priv->ref_clock = pdata->board_ref_clock;
+	} else {
+		if (!strcmp(fref_param, "19.2"))
+			priv->ref_clock = WL12XX_REFCLOCK_19;
+		else if (!strcmp(fref_param, "26"))
+			priv->ref_clock = WL12XX_REFCLOCK_26;
+		else if (!strcmp(fref_param, "26x"))
+			priv->ref_clock = WL12XX_REFCLOCK_26_XTAL;
+		else if (!strcmp(fref_param, "38.4"))
+			priv->ref_clock = WL12XX_REFCLOCK_38;
+		else if (!strcmp(fref_param, "38.4x"))
+			priv->ref_clock = WL12XX_REFCLOCK_38_XTAL;
+		else if (!strcmp(fref_param, "52"))
+			priv->ref_clock = WL12XX_REFCLOCK_52;
+		else
+			wl1271_error("Invalid fref parameter %s", fref_param);
+	}
+
+	if (!tcxo_param) {
+		priv->tcxo_clock = pdata->board_tcxo_clock;
+	} else {
+		if (!strcmp(tcxo_param, "19.2"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_19_2;
+		else if (!strcmp(tcxo_param, "26"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_26;
+		else if (!strcmp(tcxo_param, "38.4"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_38_4;
+		else if (!strcmp(tcxo_param, "52"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_52;
+		else if (!strcmp(tcxo_param, "16.368"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_16_368;
+		else if (!strcmp(tcxo_param, "32.736"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_32_736;
+		else if (!strcmp(tcxo_param, "16.8"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_16_8;
+		else if (!strcmp(tcxo_param, "33.6"))
+			priv->tcxo_clock = WL12XX_TCXOCLOCK_33_6;
+		else
+			wl1271_error("Invalid tcxo parameter %s", tcxo_param);
+	}
+
 	return wlcore_probe(wl, pdev);
 }
 
@@ -1378,6 +1726,13 @@ static void __exit wl12xx_exit(void)
 }
 module_exit(wl12xx_exit);
 
+module_param_named(fref, fref_param, charp, 0);
+MODULE_PARM_DESC(fref, "FREF clock: 19.2, 26, 26x, 38.4, 38.4x, 52");
+
+module_param_named(tcxo, tcxo_param, charp, 0);
+MODULE_PARM_DESC(tcxo,
+		 "TCXO clock: 19.2, 26, 38.4, 52, 16.368, 32.736, 16.8, 33.6");
+
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
 MODULE_FIRMWARE(WL127X_FW_NAME_SINGLE);
diff --git a/drivers/net/wireless/ti/wl12xx/wl12xx.h b/drivers/net/wireless/ti/wl12xx/wl12xx.h
index 74cd332e23e..26990fb4ede 100644
--- a/drivers/net/wireless/ti/wl12xx/wl12xx.h
+++ b/drivers/net/wireless/ti/wl12xx/wl12xx.h
@@ -24,8 +24,30 @@
 
 #include "conf.h"
 
+/* minimum FW required for driver for wl127x */
+#define WL127X_CHIP_VER		6
+#define WL127X_IFTYPE_VER	3
+#define WL127X_MAJOR_VER	10
+#define WL127X_SUBTYPE_VER	2
+#define WL127X_MINOR_VER	115
+
+/* minimum FW required for driver for wl128x */
+#define WL128X_CHIP_VER		7
+#define WL128X_IFTYPE_VER	3
+#define WL128X_MAJOR_VER	10
+#define WL128X_SUBTYPE_VER	2
+#define WL128X_MINOR_VER	115
+
+struct wl127x_rx_mem_pool_addr {
+	u32 addr;
+	u32 addr_extra;
+};
+
 struct wl12xx_priv {
 	struct wl12xx_priv_conf conf;
+
+	int ref_clock;
+	int tcxo_clock;
 };
 
 #endif /* __WL12XX_PRIV_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/Kconfig b/drivers/net/wireless/ti/wl18xx/Kconfig
new file mode 100644
index 00000000000..1cfdb254882
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/Kconfig
@@ -0,0 +1,7 @@
+config WL18XX
+	tristate "TI wl18xx support"
+	depends on MAC80211
+	select WLCORE
+	---help---
+	  This module adds support for wireless adapters based on TI
+	  WiLink 8 chipsets.
diff --git a/drivers/net/wireless/ti/wl18xx/Makefile b/drivers/net/wireless/ti/wl18xx/Makefile
new file mode 100644
index 00000000000..67c098734c7
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/Makefile
@@ -0,0 +1,3 @@
+wl18xx-objs	= main.o acx.o tx.o io.o debugfs.o
+
+obj-$(CONFIG_WL18XX)		+= wl18xx.o
diff --git a/drivers/net/wireless/ti/wl18xx/acx.c b/drivers/net/wireless/ti/wl18xx/acx.c
new file mode 100644
index 00000000000..72840e23bf5
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/acx.c
@@ -0,0 +1,111 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2011 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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 "../wlcore/cmd.h"
+#include "../wlcore/debug.h"
+#include "../wlcore/acx.h"
+
+#include "acx.h"
+
+int wl18xx_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap,
+				  u32 sdio_blk_size, u32 extra_mem_blks,
+				  u32 len_field_size)
+{
+	struct wl18xx_acx_host_config_bitmap *bitmap_conf;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx cfg bitmap %d blk %d spare %d field %d",
+		     host_cfg_bitmap, sdio_blk_size, extra_mem_blks,
+		     len_field_size);
+
+	bitmap_conf = kzalloc(sizeof(*bitmap_conf), GFP_KERNEL);
+	if (!bitmap_conf) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	bitmap_conf->host_cfg_bitmap = cpu_to_le32(host_cfg_bitmap);
+	bitmap_conf->host_sdio_block_size = cpu_to_le32(sdio_blk_size);
+	bitmap_conf->extra_mem_blocks = cpu_to_le32(extra_mem_blks);
+	bitmap_conf->length_field_size = cpu_to_le32(len_field_size);
+
+	ret = wl1271_cmd_configure(wl, ACX_HOST_IF_CFG_BITMAP,
+				   bitmap_conf, sizeof(*bitmap_conf));
+	if (ret < 0) {
+		wl1271_warning("wl1271 bitmap config opt failed: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(bitmap_conf);
+
+	return ret;
+}
+
+int wl18xx_acx_set_checksum_state(struct wl1271 *wl)
+{
+	struct wl18xx_acx_checksum_state *acx;
+	int ret;
+
+	wl1271_debug(DEBUG_ACX, "acx checksum state");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	acx->checksum_state = CHECKSUM_OFFLOAD_ENABLED;
+
+	ret = wl1271_cmd_configure(wl, ACX_CHECKSUM_CONFIG, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("failed to set Tx checksum state: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
+
+int wl18xx_acx_clear_statistics(struct wl1271 *wl)
+{
+	struct wl18xx_acx_clear_statistics *acx;
+	int ret = 0;
+
+	wl1271_debug(DEBUG_ACX, "acx clear statistics");
+
+	acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+	if (!acx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = wl1271_cmd_configure(wl, ACX_CLEAR_STATISTICS, acx, sizeof(*acx));
+	if (ret < 0) {
+		wl1271_warning("failed to clear firmware statistics: %d", ret);
+		goto out;
+	}
+
+out:
+	kfree(acx);
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl18xx/acx.h b/drivers/net/wireless/ti/wl18xx/acx.h
new file mode 100644
index 00000000000..e2609a6b734
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/acx.h
@@ -0,0 +1,287 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2011 Texas Instruments. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL18XX_ACX_H__
+#define __WL18XX_ACX_H__
+
+#include "../wlcore/wlcore.h"
+#include "../wlcore/acx.h"
+
+enum {
+	ACX_CLEAR_STATISTICS		 = 0x0047,
+};
+
+/* numbers of bits the length field takes (add 1 for the actual number) */
+#define WL18XX_HOST_IF_LEN_SIZE_FIELD 15
+
+#define WL18XX_ACX_EVENTS_VECTOR	(WL1271_ACX_INTR_WATCHDOG	| \
+					 WL1271_ACX_INTR_INIT_COMPLETE	| \
+					 WL1271_ACX_INTR_EVENT_A	| \
+					 WL1271_ACX_INTR_EVENT_B	| \
+					 WL1271_ACX_INTR_CMD_COMPLETE	| \
+					 WL1271_ACX_INTR_HW_AVAILABLE	| \
+					 WL1271_ACX_INTR_DATA		| \
+					 WL1271_ACX_SW_INTR_WATCHDOG)
+
+#define WL18XX_INTR_MASK		(WL1271_ACX_INTR_WATCHDOG	| \
+					 WL1271_ACX_INTR_EVENT_A	| \
+					 WL1271_ACX_INTR_EVENT_B	| \
+					 WL1271_ACX_INTR_HW_AVAILABLE	| \
+					 WL1271_ACX_INTR_DATA		| \
+					 WL1271_ACX_SW_INTR_WATCHDOG)
+
+struct wl18xx_acx_host_config_bitmap {
+	struct acx_header header;
+
+	__le32 host_cfg_bitmap;
+
+	__le32 host_sdio_block_size;
+
+	/* extra mem blocks per frame in TX. */
+	__le32 extra_mem_blocks;
+
+	/*
+	 * number of bits of the length field in the first TX word
+	 * (up to 15 - for using the entire 16 bits).
+	 */
+	__le32 length_field_size;
+
+} __packed;
+
+enum {
+	CHECKSUM_OFFLOAD_DISABLED = 0,
+	CHECKSUM_OFFLOAD_ENABLED  = 1,
+	CHECKSUM_OFFLOAD_FAKE_RX  = 2,
+	CHECKSUM_OFFLOAD_INVALID  = 0xFF
+};
+
+struct wl18xx_acx_checksum_state {
+	struct acx_header header;
+
+	 /* enum acx_checksum_state */
+	u8 checksum_state;
+	u8 pad[3];
+} __packed;
+
+
+struct wl18xx_acx_error_stats {
+	u32 error_frame;
+	u32 error_null_Frame_tx_start;
+	u32 error_numll_frame_cts_start;
+	u32 error_bar_retry;
+	u32 error_frame_cts_nul_flid;
+} __packed;
+
+struct wl18xx_acx_debug_stats {
+	u32 debug1;
+	u32 debug2;
+	u32 debug3;
+	u32 debug4;
+	u32 debug5;
+	u32 debug6;
+} __packed;
+
+struct wl18xx_acx_ring_stats {
+	u32 prepared_descs;
+	u32 tx_cmplt;
+} __packed;
+
+struct wl18xx_acx_tx_stats {
+	u32 tx_prepared_descs;
+	u32 tx_cmplt;
+	u32 tx_template_prepared;
+	u32 tx_data_prepared;
+	u32 tx_template_programmed;
+	u32 tx_data_programmed;
+	u32 tx_burst_programmed;
+	u32 tx_starts;
+	u32 tx_imm_resp;
+	u32 tx_start_templates;
+	u32 tx_start_int_templates;
+	u32 tx_start_fw_gen;
+	u32 tx_start_data;
+	u32 tx_start_null_frame;
+	u32 tx_exch;
+	u32 tx_retry_template;
+	u32 tx_retry_data;
+	u32 tx_exch_pending;
+	u32 tx_exch_expiry;
+	u32 tx_done_template;
+	u32 tx_done_data;
+	u32 tx_done_int_template;
+	u32 tx_frame_checksum;
+	u32 tx_checksum_result;
+	u32 frag_called;
+	u32 frag_mpdu_alloc_failed;
+	u32 frag_init_called;
+	u32 frag_in_process_called;
+	u32 frag_tkip_called;
+	u32 frag_key_not_found;
+	u32 frag_need_fragmentation;
+	u32 frag_bad_mblk_num;
+	u32 frag_failed;
+	u32 frag_cache_hit;
+	u32 frag_cache_miss;
+} __packed;
+
+struct wl18xx_acx_rx_stats {
+	u32 rx_beacon_early_term;
+	u32 rx_out_of_mpdu_nodes;
+	u32 rx_hdr_overflow;
+	u32 rx_dropped_frame;
+	u32 rx_done_stage;
+	u32 rx_done;
+	u32 rx_defrag;
+	u32 rx_defrag_end;
+	u32 rx_cmplt;
+	u32 rx_pre_complt;
+	u32 rx_cmplt_task;
+	u32 rx_phy_hdr;
+	u32 rx_timeout;
+	u32 rx_timeout_wa;
+	u32 rx_wa_density_dropped_frame;
+	u32 rx_wa_ba_not_expected;
+	u32 rx_frame_checksum;
+	u32 rx_checksum_result;
+	u32 defrag_called;
+	u32 defrag_init_called;
+	u32 defrag_in_process_called;
+	u32 defrag_tkip_called;
+	u32 defrag_need_defrag;
+	u32 defrag_decrypt_failed;
+	u32 decrypt_key_not_found;
+	u32 defrag_need_decrypt;
+	u32 rx_tkip_replays;
+} __packed;
+
+struct wl18xx_acx_isr_stats {
+	u32 irqs;
+} __packed;
+
+#define PWR_STAT_MAX_CONT_MISSED_BCNS_SPREAD 10
+
+struct wl18xx_acx_pwr_stats {
+	u32 missing_bcns_cnt;
+	u32 rcvd_bcns_cnt;
+	u32 connection_out_of_sync;
+	u32 cont_miss_bcns_spread[PWR_STAT_MAX_CONT_MISSED_BCNS_SPREAD];
+	u32 rcvd_awake_bcns_cnt;
+} __packed;
+
+struct wl18xx_acx_event_stats {
+	u32 calibration;
+	u32 rx_mismatch;
+	u32 rx_mem_empty;
+} __packed;
+
+struct wl18xx_acx_ps_poll_stats {
+	u32 ps_poll_timeouts;
+	u32 upsd_timeouts;
+	u32 upsd_max_ap_turn;
+	u32 ps_poll_max_ap_turn;
+	u32 ps_poll_utilization;
+	u32 upsd_utilization;
+} __packed;
+
+struct wl18xx_acx_rx_filter_stats {
+	u32 beacon_filter;
+	u32 arp_filter;
+	u32 mc_filter;
+	u32 dup_filter;
+	u32 data_filter;
+	u32 ibss_filter;
+	u32 protection_filter;
+	u32 accum_arp_pend_requests;
+	u32 max_arp_queue_dep;
+} __packed;
+
+struct wl18xx_acx_rx_rate_stats {
+	u32 rx_frames_per_rates[50];
+} __packed;
+
+#define AGGR_STATS_TX_AGG	16
+#define AGGR_STATS_TX_RATE	16
+#define AGGR_STATS_RX_SIZE_LEN	16
+
+struct wl18xx_acx_aggr_stats {
+	u32 tx_agg_vs_rate[AGGR_STATS_TX_AGG * AGGR_STATS_TX_RATE];
+	u32 rx_size[AGGR_STATS_RX_SIZE_LEN];
+} __packed;
+
+#define PIPE_STATS_HW_FIFO	11
+
+struct wl18xx_acx_pipeline_stats {
+	u32 hs_tx_stat_fifo_int;
+	u32 hs_rx_stat_fifo_int;
+	u32 tcp_tx_stat_fifo_int;
+	u32 tcp_rx_stat_fifo_int;
+	u32 enc_tx_stat_fifo_int;
+	u32 enc_rx_stat_fifo_int;
+	u32 rx_complete_stat_fifo_int;
+	u32 pre_proc_swi;
+	u32 post_proc_swi;
+	u32 sec_frag_swi;
+	u32 pre_to_defrag_swi;
+	u32 defrag_to_csum_swi;
+	u32 csum_to_rx_xfer_swi;
+	u32 dec_packet_in;
+	u32 dec_packet_in_fifo_full;
+	u32 dec_packet_out;
+	u32 cs_rx_packet_in;
+	u32 cs_rx_packet_out;
+	u16 pipeline_fifo_full[PIPE_STATS_HW_FIFO];
+} __packed;
+
+struct wl18xx_acx_mem_stats {
+	u32 rx_free_mem_blks;
+	u32 tx_free_mem_blks;
+	u32 fwlog_free_mem_blks;
+	u32 fw_gen_free_mem_blks;
+} __packed;
+
+struct wl18xx_acx_statistics {
+	struct acx_header header;
+
+	struct wl18xx_acx_error_stats		error;
+	struct wl18xx_acx_debug_stats		debug;
+	struct wl18xx_acx_tx_stats		tx;
+	struct wl18xx_acx_rx_stats		rx;
+	struct wl18xx_acx_isr_stats		isr;
+	struct wl18xx_acx_pwr_stats		pwr;
+	struct wl18xx_acx_ps_poll_stats		ps_poll;
+	struct wl18xx_acx_rx_filter_stats	rx_filter;
+	struct wl18xx_acx_rx_rate_stats		rx_rate;
+	struct wl18xx_acx_aggr_stats		aggr_size;
+	struct wl18xx_acx_pipeline_stats	pipeline;
+	struct wl18xx_acx_mem_stats		mem;
+} __packed;
+
+struct wl18xx_acx_clear_statistics {
+	struct acx_header header;
+};
+
+int wl18xx_acx_host_if_cfg_bitmap(struct wl1271 *wl, u32 host_cfg_bitmap,
+				  u32 sdio_blk_size, u32 extra_mem_blks,
+				  u32 len_field_size);
+int wl18xx_acx_set_checksum_state(struct wl1271 *wl);
+int wl18xx_acx_clear_statistics(struct wl1271 *wl);
+
+#endif /* __WL18XX_ACX_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/conf.h b/drivers/net/wireless/ti/wl18xx/conf.h
new file mode 100644
index 00000000000..4d426cc2027
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/conf.h
@@ -0,0 +1,111 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2011 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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
+ *
+ */
+
+#ifndef __WL18XX_CONF_H__
+#define __WL18XX_CONF_H__
+
+#define WL18XX_CONF_MAGIC	0x10e100ca
+#define WL18XX_CONF_VERSION	(WLCORE_CONF_VERSION | 0x0003)
+#define WL18XX_CONF_MASK	0x0000ffff
+#define WL18XX_CONF_SIZE	(WLCORE_CONF_SIZE + \
+				 sizeof(struct wl18xx_priv_conf))
+
+#define NUM_OF_CHANNELS_11_ABG 150
+#define NUM_OF_CHANNELS_11_P 7
+#define WL18XX_NUM_OF_SUB_BANDS 9
+#define SRF_TABLE_LEN 16
+#define PIN_MUXING_SIZE 2
+
+struct wl18xx_mac_and_phy_params {
+	u8 phy_standalone;
+	u8 rdl;
+	u8 enable_clpc;
+	u8 enable_tx_low_pwr_on_siso_rdl;
+	u8 auto_detect;
+	u8 dedicated_fem;
+
+	u8 low_band_component;
+
+	/* Bit 0: One Hot, Bit 1: Control Enable, Bit 2: 1.8V, Bit 3: 3V */
+	u8 low_band_component_type;
+
+	u8 high_band_component;
+
+	/* Bit 0: One Hot, Bit 1: Control Enable, Bit 2: 1.8V, Bit 3: 3V */
+	u8 high_band_component_type;
+	u8 number_of_assembled_ant2_4;
+	u8 number_of_assembled_ant5;
+	u8 pin_muxing_platform_options[PIN_MUXING_SIZE];
+	u8 external_pa_dc2dc;
+	u8 tcxo_ldo_voltage;
+	u8 xtal_itrim_val;
+	u8 srf_state;
+	u8 srf1[SRF_TABLE_LEN];
+	u8 srf2[SRF_TABLE_LEN];
+	u8 srf3[SRF_TABLE_LEN];
+	u8 io_configuration;
+	u8 sdio_configuration;
+	u8 settings;
+	u8 rx_profile;
+	u8 per_chan_pwr_limit_arr_11abg[NUM_OF_CHANNELS_11_ABG];
+	u8 pwr_limit_reference_11_abg;
+	u8 per_chan_pwr_limit_arr_11p[NUM_OF_CHANNELS_11_P];
+	u8 pwr_limit_reference_11p;
+	u8 per_sub_band_tx_trace_loss[WL18XX_NUM_OF_SUB_BANDS];
+	u8 per_sub_band_rx_trace_loss[WL18XX_NUM_OF_SUB_BANDS];
+	u8 primary_clock_setting_time;
+	u8 clock_valid_on_wake_up;
+	u8 secondary_clock_setting_time;
+	u8 board_type;
+	/* enable point saturation */
+	u8 psat;
+	/* low/medium/high Tx power in dBm */
+	s8 low_power_val;
+	s8 med_power_val;
+	s8 high_power_val;
+	u8 padding[1];
+} __packed;
+
+enum wl18xx_ht_mode {
+	/* Default - use MIMO, fallback to SISO20 */
+	HT_MODE_DEFAULT = 0,
+
+	/* Wide - use SISO40 */
+	HT_MODE_WIDE = 1,
+
+	/* Use SISO20 */
+	HT_MODE_SISO20 = 2,
+};
+
+struct wl18xx_ht_settings {
+	/* DEFAULT / WIDE / SISO20 */
+	u8 mode;
+} __packed;
+
+struct wl18xx_priv_conf {
+	/* Module params structures */
+	struct wl18xx_ht_settings ht;
+
+	/* this structure is copied wholesale to FW */
+	struct wl18xx_mac_and_phy_params phy;
+} __packed;
+
+#endif /* __WL18XX_CONF_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/debugfs.c b/drivers/net/wireless/ti/wl18xx/debugfs.c
new file mode 100644
index 00000000000..3ce6f1039af
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/debugfs.c
@@ -0,0 +1,403 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Copyright (C) 2011-2012 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include "../wlcore/debugfs.h"
+#include "../wlcore/wlcore.h"
+
+#include "wl18xx.h"
+#include "acx.h"
+#include "debugfs.h"
+
+#define WL18XX_DEBUGFS_FWSTATS_FILE(a, b, c) \
+	DEBUGFS_FWSTATS_FILE(a, b, c, wl18xx_acx_statistics)
+#define WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(a, b, c) \
+	DEBUGFS_FWSTATS_FILE_ARRAY(a, b, c, wl18xx_acx_statistics)
+
+
+WL18XX_DEBUGFS_FWSTATS_FILE(debug, debug1, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(debug, debug2, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(debug, debug3, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(debug, debug4, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(debug, debug5, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(debug, debug6, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(error, error_frame, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(error, error_null_Frame_tx_start, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(error, error_numll_frame_cts_start, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(error, error_bar_retry, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(error, error_frame_cts_nul_flid, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_prepared_descs, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_cmplt, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_template_prepared, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_data_prepared, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_template_programmed, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_data_programmed, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_burst_programmed, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_starts, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_imm_resp, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_start_templates, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_start_int_templates, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_start_fw_gen, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_start_data, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_start_null_frame, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_exch, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_retry_template, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_retry_data, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_exch_pending, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_exch_expiry, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_done_template, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_done_data, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_done_int_template, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_frame_checksum, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, tx_checksum_result, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_mpdu_alloc_failed, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_init_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_in_process_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_tkip_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_key_not_found, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_need_fragmentation, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_bad_mblk_num, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_failed, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_cache_hit, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(tx, frag_cache_miss, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_beacon_early_term, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_out_of_mpdu_nodes, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_hdr_overflow, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_dropped_frame, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_done, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_defrag, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_defrag_end, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_cmplt, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_pre_complt, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_cmplt_task, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_phy_hdr, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_timeout, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_timeout_wa, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_wa_density_dropped_frame, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_wa_ba_not_expected, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_frame_checksum, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_checksum_result, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_init_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_in_process_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_tkip_called, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_need_defrag, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_decrypt_failed, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, decrypt_key_not_found, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, defrag_need_decrypt, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx, rx_tkip_replays, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(isr, irqs, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(pwr, missing_bcns_cnt, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pwr, rcvd_bcns_cnt, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pwr, connection_out_of_sync, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(pwr, cont_miss_bcns_spread,
+				  PWR_STAT_MAX_CONT_MISSED_BCNS_SPREAD);
+WL18XX_DEBUGFS_FWSTATS_FILE(pwr, rcvd_awake_bcns_cnt, "%u");
+
+
+WL18XX_DEBUGFS_FWSTATS_FILE(ps_poll, ps_poll_timeouts, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(ps_poll, upsd_timeouts, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(ps_poll, upsd_max_ap_turn, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(ps_poll, ps_poll_max_ap_turn, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(ps_poll, ps_poll_utilization, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(ps_poll, upsd_utilization, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, beacon_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, arp_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, mc_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, dup_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, data_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, ibss_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, protection_filter, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, accum_arp_pend_requests, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_filter, max_arp_queue_dep, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE(rx_rate, rx_frames_per_rates, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(aggr_size, tx_agg_vs_rate,
+				  AGGR_STATS_TX_AGG*AGGR_STATS_TX_RATE);
+WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(aggr_size, rx_size,
+				  AGGR_STATS_RX_SIZE_LEN);
+
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, hs_tx_stat_fifo_int, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, tcp_tx_stat_fifo_int, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, tcp_rx_stat_fifo_int, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, enc_tx_stat_fifo_int, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, enc_rx_stat_fifo_int, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, rx_complete_stat_fifo_int, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, pre_proc_swi, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, post_proc_swi, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, sec_frag_swi, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, pre_to_defrag_swi, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, defrag_to_csum_swi, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, csum_to_rx_xfer_swi, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, dec_packet_in, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, dec_packet_in_fifo_full, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, dec_packet_out, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, cs_rx_packet_in, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(pipeline, cs_rx_packet_out, "%u");
+
+WL18XX_DEBUGFS_FWSTATS_FILE_ARRAY(pipeline, pipeline_fifo_full,
+				  PIPE_STATS_HW_FIFO);
+
+WL18XX_DEBUGFS_FWSTATS_FILE(mem, rx_free_mem_blks, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(mem, tx_free_mem_blks, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(mem, fwlog_free_mem_blks, "%u");
+WL18XX_DEBUGFS_FWSTATS_FILE(mem, fw_gen_free_mem_blks, "%u");
+
+static ssize_t conf_read(struct file *file, char __user *user_buf,
+			 size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	struct wl18xx_priv *priv = wl->priv;
+	struct wlcore_conf_header header;
+	char *buf, *pos;
+	size_t len;
+	int ret;
+
+	len = WL18XX_CONF_SIZE;
+	buf = kmalloc(len, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	header.magic	= cpu_to_le32(WL18XX_CONF_MAGIC);
+	header.version	= cpu_to_le32(WL18XX_CONF_VERSION);
+	header.checksum	= 0;
+
+	mutex_lock(&wl->mutex);
+
+	pos = buf;
+	memcpy(pos, &header, sizeof(header));
+	pos += sizeof(header);
+	memcpy(pos, &wl->conf, sizeof(wl->conf));
+	pos += sizeof(wl->conf);
+	memcpy(pos, &priv->conf, sizeof(priv->conf));
+
+	mutex_unlock(&wl->mutex);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations conf_ops = {
+	.read = conf_read,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
+static ssize_t clear_fw_stats_write(struct file *file,
+			      const char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	int ret;
+
+	mutex_lock(&wl->mutex);
+
+	if (wl->state == WL1271_STATE_OFF)
+		goto out;
+
+	ret = wl18xx_acx_clear_statistics(wl);
+	if (ret < 0) {
+		count = ret;
+		goto out;
+	}
+out:
+	mutex_unlock(&wl->mutex);
+	return count;
+}
+
+static const struct file_operations clear_fw_stats_ops = {
+	.write = clear_fw_stats_write,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
+int wl18xx_debugfs_add_files(struct wl1271 *wl,
+			     struct dentry *rootdir)
+{
+	int ret = 0;
+	struct dentry *entry, *stats, *moddir;
+
+	moddir = debugfs_create_dir(KBUILD_MODNAME, rootdir);
+	if (!moddir || IS_ERR(moddir)) {
+		entry = moddir;
+		goto err;
+	}
+
+	stats = debugfs_create_dir("fw_stats", moddir);
+	if (!stats || IS_ERR(stats)) {
+		entry = stats;
+		goto err;
+	}
+
+	DEBUGFS_ADD(clear_fw_stats, stats);
+
+	DEBUGFS_FWSTATS_ADD(debug, debug1);
+	DEBUGFS_FWSTATS_ADD(debug, debug2);
+	DEBUGFS_FWSTATS_ADD(debug, debug3);
+	DEBUGFS_FWSTATS_ADD(debug, debug4);
+	DEBUGFS_FWSTATS_ADD(debug, debug5);
+	DEBUGFS_FWSTATS_ADD(debug, debug6);
+
+	DEBUGFS_FWSTATS_ADD(error, error_frame);
+	DEBUGFS_FWSTATS_ADD(error, error_null_Frame_tx_start);
+	DEBUGFS_FWSTATS_ADD(error, error_numll_frame_cts_start);
+	DEBUGFS_FWSTATS_ADD(error, error_bar_retry);
+	DEBUGFS_FWSTATS_ADD(error, error_frame_cts_nul_flid);
+
+	DEBUGFS_FWSTATS_ADD(tx, tx_prepared_descs);
+	DEBUGFS_FWSTATS_ADD(tx, tx_cmplt);
+	DEBUGFS_FWSTATS_ADD(tx, tx_template_prepared);
+	DEBUGFS_FWSTATS_ADD(tx, tx_data_prepared);
+	DEBUGFS_FWSTATS_ADD(tx, tx_template_programmed);
+	DEBUGFS_FWSTATS_ADD(tx, tx_data_programmed);
+	DEBUGFS_FWSTATS_ADD(tx, tx_burst_programmed);
+	DEBUGFS_FWSTATS_ADD(tx, tx_starts);
+	DEBUGFS_FWSTATS_ADD(tx, tx_imm_resp);
+	DEBUGFS_FWSTATS_ADD(tx, tx_start_templates);
+	DEBUGFS_FWSTATS_ADD(tx, tx_start_int_templates);
+	DEBUGFS_FWSTATS_ADD(tx, tx_start_fw_gen);
+	DEBUGFS_FWSTATS_ADD(tx, tx_start_data);
+	DEBUGFS_FWSTATS_ADD(tx, tx_start_null_frame);
+	DEBUGFS_FWSTATS_ADD(tx, tx_exch);
+	DEBUGFS_FWSTATS_ADD(tx, tx_retry_template);
+	DEBUGFS_FWSTATS_ADD(tx, tx_retry_data);
+	DEBUGFS_FWSTATS_ADD(tx, tx_exch_pending);
+	DEBUGFS_FWSTATS_ADD(tx, tx_exch_expiry);
+	DEBUGFS_FWSTATS_ADD(tx, tx_done_template);
+	DEBUGFS_FWSTATS_ADD(tx, tx_done_data);
+	DEBUGFS_FWSTATS_ADD(tx, tx_done_int_template);
+	DEBUGFS_FWSTATS_ADD(tx, tx_frame_checksum);
+	DEBUGFS_FWSTATS_ADD(tx, tx_checksum_result);
+	DEBUGFS_FWSTATS_ADD(tx, frag_called);
+	DEBUGFS_FWSTATS_ADD(tx, frag_mpdu_alloc_failed);
+	DEBUGFS_FWSTATS_ADD(tx, frag_init_called);
+	DEBUGFS_FWSTATS_ADD(tx, frag_in_process_called);
+	DEBUGFS_FWSTATS_ADD(tx, frag_tkip_called);
+	DEBUGFS_FWSTATS_ADD(tx, frag_key_not_found);
+	DEBUGFS_FWSTATS_ADD(tx, frag_need_fragmentation);
+	DEBUGFS_FWSTATS_ADD(tx, frag_bad_mblk_num);
+	DEBUGFS_FWSTATS_ADD(tx, frag_failed);
+	DEBUGFS_FWSTATS_ADD(tx, frag_cache_hit);
+	DEBUGFS_FWSTATS_ADD(tx, frag_cache_miss);
+
+	DEBUGFS_FWSTATS_ADD(rx, rx_beacon_early_term);
+	DEBUGFS_FWSTATS_ADD(rx, rx_out_of_mpdu_nodes);
+	DEBUGFS_FWSTATS_ADD(rx, rx_hdr_overflow);
+	DEBUGFS_FWSTATS_ADD(rx, rx_dropped_frame);
+	DEBUGFS_FWSTATS_ADD(rx, rx_done);
+	DEBUGFS_FWSTATS_ADD(rx, rx_defrag);
+	DEBUGFS_FWSTATS_ADD(rx, rx_defrag_end);
+	DEBUGFS_FWSTATS_ADD(rx, rx_cmplt);
+	DEBUGFS_FWSTATS_ADD(rx, rx_pre_complt);
+	DEBUGFS_FWSTATS_ADD(rx, rx_cmplt_task);
+	DEBUGFS_FWSTATS_ADD(rx, rx_phy_hdr);
+	DEBUGFS_FWSTATS_ADD(rx, rx_timeout);
+	DEBUGFS_FWSTATS_ADD(rx, rx_timeout_wa);
+	DEBUGFS_FWSTATS_ADD(rx, rx_wa_density_dropped_frame);
+	DEBUGFS_FWSTATS_ADD(rx, rx_wa_ba_not_expected);
+	DEBUGFS_FWSTATS_ADD(rx, rx_frame_checksum);
+	DEBUGFS_FWSTATS_ADD(rx, rx_checksum_result);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_called);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_init_called);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_in_process_called);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_tkip_called);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_need_defrag);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_decrypt_failed);
+	DEBUGFS_FWSTATS_ADD(rx, decrypt_key_not_found);
+	DEBUGFS_FWSTATS_ADD(rx, defrag_need_decrypt);
+	DEBUGFS_FWSTATS_ADD(rx, rx_tkip_replays);
+
+	DEBUGFS_FWSTATS_ADD(isr, irqs);
+
+	DEBUGFS_FWSTATS_ADD(pwr, missing_bcns_cnt);
+	DEBUGFS_FWSTATS_ADD(pwr, rcvd_bcns_cnt);
+	DEBUGFS_FWSTATS_ADD(pwr, connection_out_of_sync);
+	DEBUGFS_FWSTATS_ADD(pwr, cont_miss_bcns_spread);
+	DEBUGFS_FWSTATS_ADD(pwr, rcvd_awake_bcns_cnt);
+
+	DEBUGFS_FWSTATS_ADD(ps_poll, ps_poll_timeouts);
+	DEBUGFS_FWSTATS_ADD(ps_poll, upsd_timeouts);
+	DEBUGFS_FWSTATS_ADD(ps_poll, upsd_max_ap_turn);
+	DEBUGFS_FWSTATS_ADD(ps_poll, ps_poll_max_ap_turn);
+	DEBUGFS_FWSTATS_ADD(ps_poll, ps_poll_utilization);
+	DEBUGFS_FWSTATS_ADD(ps_poll, upsd_utilization);
+
+	DEBUGFS_FWSTATS_ADD(rx_filter, beacon_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, arp_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, mc_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, dup_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, data_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, ibss_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, protection_filter);
+	DEBUGFS_FWSTATS_ADD(rx_filter, accum_arp_pend_requests);
+	DEBUGFS_FWSTATS_ADD(rx_filter, max_arp_queue_dep);
+
+	DEBUGFS_FWSTATS_ADD(rx_rate, rx_frames_per_rates);
+
+	DEBUGFS_FWSTATS_ADD(aggr_size, tx_agg_vs_rate);
+	DEBUGFS_FWSTATS_ADD(aggr_size, rx_size);
+
+	DEBUGFS_FWSTATS_ADD(pipeline, hs_tx_stat_fifo_int);
+	DEBUGFS_FWSTATS_ADD(pipeline, tcp_tx_stat_fifo_int);
+	DEBUGFS_FWSTATS_ADD(pipeline, tcp_rx_stat_fifo_int);
+	DEBUGFS_FWSTATS_ADD(pipeline, enc_tx_stat_fifo_int);
+	DEBUGFS_FWSTATS_ADD(pipeline, enc_rx_stat_fifo_int);
+	DEBUGFS_FWSTATS_ADD(pipeline, rx_complete_stat_fifo_int);
+	DEBUGFS_FWSTATS_ADD(pipeline, pre_proc_swi);
+	DEBUGFS_FWSTATS_ADD(pipeline, post_proc_swi);
+	DEBUGFS_FWSTATS_ADD(pipeline, sec_frag_swi);
+	DEBUGFS_FWSTATS_ADD(pipeline, pre_to_defrag_swi);
+	DEBUGFS_FWSTATS_ADD(pipeline, defrag_to_csum_swi);
+	DEBUGFS_FWSTATS_ADD(pipeline, csum_to_rx_xfer_swi);
+	DEBUGFS_FWSTATS_ADD(pipeline, dec_packet_in);
+	DEBUGFS_FWSTATS_ADD(pipeline, dec_packet_in_fifo_full);
+	DEBUGFS_FWSTATS_ADD(pipeline, dec_packet_out);
+	DEBUGFS_FWSTATS_ADD(pipeline, cs_rx_packet_in);
+	DEBUGFS_FWSTATS_ADD(pipeline, cs_rx_packet_out);
+	DEBUGFS_FWSTATS_ADD(pipeline, pipeline_fifo_full);
+
+	DEBUGFS_FWSTATS_ADD(mem, rx_free_mem_blks);
+	DEBUGFS_FWSTATS_ADD(mem, tx_free_mem_blks);
+	DEBUGFS_FWSTATS_ADD(mem, fwlog_free_mem_blks);
+	DEBUGFS_FWSTATS_ADD(mem, fw_gen_free_mem_blks);
+
+	DEBUGFS_ADD(conf, moddir);
+
+	return 0;
+
+err:
+	if (IS_ERR(entry))
+		ret = PTR_ERR(entry);
+	else
+		ret = -ENOMEM;
+
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl18xx/debugfs.h b/drivers/net/wireless/ti/wl18xx/debugfs.h
new file mode 100644
index 00000000000..ed679bebf62
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/debugfs.h
@@ -0,0 +1,28 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2012 Texas Instruments. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL18XX_DEBUGFS_H__
+#define __WL18XX_DEBUGFS_H__
+
+int wl18xx_debugfs_add_files(struct wl1271 *wl,
+			     struct dentry *rootdir);
+
+#endif /* __WL18XX_DEBUGFS_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/io.c b/drivers/net/wireless/ti/wl18xx/io.c
new file mode 100644
index 00000000000..f0abf3ef2c9
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/io.c
@@ -0,0 +1,75 @@
+/*
+ * This file is part of wl18xx
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include "../wlcore/wlcore.h"
+#include "../wlcore/io.h"
+
+#include "io.h"
+
+int wl18xx_top_reg_write(struct wl1271 *wl, int addr, u16 val)
+{
+	u32 tmp;
+	int ret;
+
+	if (WARN_ON(addr % 2))
+		return -EINVAL;
+
+	if ((addr % 4) == 0) {
+		ret = wlcore_read32(wl, addr, &tmp);
+		if (ret < 0)
+			goto out;
+
+		tmp = (tmp & 0xffff0000) | val;
+		ret = wlcore_write32(wl, addr, tmp);
+	} else {
+		ret = wlcore_read32(wl, addr - 2, &tmp);
+		if (ret < 0)
+			goto out;
+
+		tmp = (tmp & 0xffff) | (val << 16);
+		ret = wlcore_write32(wl, addr - 2, tmp);
+	}
+
+out:
+	return ret;
+}
+
+int wl18xx_top_reg_read(struct wl1271 *wl, int addr, u16 *out)
+{
+	u32 val = 0;
+	int ret;
+
+	if (WARN_ON(addr % 2))
+		return -EINVAL;
+
+	if ((addr % 4) == 0) {
+		/* address is 4-bytes aligned */
+		ret = wlcore_read32(wl, addr, &val);
+		if (ret >= 0 && out)
+			*out = val & 0xffff;
+	} else {
+		ret = wlcore_read32(wl, addr - 2, &val);
+		if (ret >= 0 && out)
+			*out = (val & 0xffff0000) >> 16;
+	}
+
+	return ret;
+}
diff --git a/drivers/net/wireless/ti/wl18xx/io.h b/drivers/net/wireless/ti/wl18xx/io.h
new file mode 100644
index 00000000000..c32ae30277d
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/io.h
@@ -0,0 +1,28 @@
+/*
+ * This file is part of wl18xx
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL18XX_IO_H__
+#define __WL18XX_IO_H__
+
+int __must_check wl18xx_top_reg_write(struct wl1271 *wl, int addr, u16 val);
+int __must_check wl18xx_top_reg_read(struct wl1271 *wl, int addr, u16 *out);
+
+#endif /* __WL18XX_IO_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
new file mode 100644
index 00000000000..69042bb9a09
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -0,0 +1,1610 @@
+/*
+ * This file is part of wl18xx
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/ip.h>
+#include <linux/firmware.h>
+
+#include "../wlcore/wlcore.h"
+#include "../wlcore/debug.h"
+#include "../wlcore/io.h"
+#include "../wlcore/acx.h"
+#include "../wlcore/tx.h"
+#include "../wlcore/rx.h"
+#include "../wlcore/io.h"
+#include "../wlcore/boot.h"
+
+#include "reg.h"
+#include "conf.h"
+#include "acx.h"
+#include "tx.h"
+#include "wl18xx.h"
+#include "io.h"
+#include "debugfs.h"
+
+#define WL18XX_RX_CHECKSUM_MASK      0x40
+
+static char *ht_mode_param = NULL;
+static char *board_type_param = NULL;
+static bool checksum_param = false;
+static bool enable_11a_param = true;
+static int num_rx_desc_param = -1;
+
+/* phy paramters */
+static int dc2dc_param = -1;
+static int n_antennas_2_param = -1;
+static int n_antennas_5_param = -1;
+static int low_band_component_param = -1;
+static int low_band_component_type_param = -1;
+static int high_band_component_param = -1;
+static int high_band_component_type_param = -1;
+static int pwr_limit_reference_11_abg_param = -1;
+
+static const u8 wl18xx_rate_to_idx_2ghz[] = {
+	/* MCS rates are used only with 11n */
+	15,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS15 */
+	14,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS14 */
+	13,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS13 */
+	12,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS12 */
+	11,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS11 */
+	10,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS10 */
+	9,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS9 */
+	8,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS8 */
+	7,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS7 */
+	6,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS6 */
+	5,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS5 */
+	4,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS4 */
+	3,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS3 */
+	2,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS2 */
+	1,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS1 */
+	0,                             /* WL18XX_CONF_HW_RXTX_RATE_MCS0 */
+
+	11,                            /* WL18XX_CONF_HW_RXTX_RATE_54   */
+	10,                            /* WL18XX_CONF_HW_RXTX_RATE_48   */
+	9,                             /* WL18XX_CONF_HW_RXTX_RATE_36   */
+	8,                             /* WL18XX_CONF_HW_RXTX_RATE_24   */
+
+	/* TI-specific rate */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL18XX_CONF_HW_RXTX_RATE_22   */
+
+	7,                             /* WL18XX_CONF_HW_RXTX_RATE_18   */
+	6,                             /* WL18XX_CONF_HW_RXTX_RATE_12   */
+	3,                             /* WL18XX_CONF_HW_RXTX_RATE_11   */
+	5,                             /* WL18XX_CONF_HW_RXTX_RATE_9    */
+	4,                             /* WL18XX_CONF_HW_RXTX_RATE_6    */
+	2,                             /* WL18XX_CONF_HW_RXTX_RATE_5_5  */
+	1,                             /* WL18XX_CONF_HW_RXTX_RATE_2    */
+	0                              /* WL18XX_CONF_HW_RXTX_RATE_1    */
+};
+
+static const u8 wl18xx_rate_to_idx_5ghz[] = {
+	/* MCS rates are used only with 11n */
+	15,                           /* WL18XX_CONF_HW_RXTX_RATE_MCS15 */
+	14,                           /* WL18XX_CONF_HW_RXTX_RATE_MCS14 */
+	13,                           /* WL18XX_CONF_HW_RXTX_RATE_MCS13 */
+	12,                           /* WL18XX_CONF_HW_RXTX_RATE_MCS12 */
+	11,                           /* WL18XX_CONF_HW_RXTX_RATE_MCS11 */
+	10,                           /* WL18XX_CONF_HW_RXTX_RATE_MCS10 */
+	9,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS9 */
+	8,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS8 */
+	7,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS7 */
+	6,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS6 */
+	5,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS5 */
+	4,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS4 */
+	3,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS3 */
+	2,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS2 */
+	1,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS1 */
+	0,                            /* WL18XX_CONF_HW_RXTX_RATE_MCS0 */
+
+	7,                             /* WL18XX_CONF_HW_RXTX_RATE_54   */
+	6,                             /* WL18XX_CONF_HW_RXTX_RATE_48   */
+	5,                             /* WL18XX_CONF_HW_RXTX_RATE_36   */
+	4,                             /* WL18XX_CONF_HW_RXTX_RATE_24   */
+
+	/* TI-specific rate */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL18XX_CONF_HW_RXTX_RATE_22   */
+
+	3,                             /* WL18XX_CONF_HW_RXTX_RATE_18   */
+	2,                             /* WL18XX_CONF_HW_RXTX_RATE_12   */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL18XX_CONF_HW_RXTX_RATE_11   */
+	1,                             /* WL18XX_CONF_HW_RXTX_RATE_9    */
+	0,                             /* WL18XX_CONF_HW_RXTX_RATE_6    */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL18XX_CONF_HW_RXTX_RATE_5_5  */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL18XX_CONF_HW_RXTX_RATE_2    */
+	CONF_HW_RXTX_RATE_UNSUPPORTED, /* WL18XX_CONF_HW_RXTX_RATE_1    */
+};
+
+static const u8 *wl18xx_band_rate_to_idx[] = {
+	[IEEE80211_BAND_2GHZ] = wl18xx_rate_to_idx_2ghz,
+	[IEEE80211_BAND_5GHZ] = wl18xx_rate_to_idx_5ghz
+};
+
+enum wl18xx_hw_rates {
+	WL18XX_CONF_HW_RXTX_RATE_MCS15 = 0,
+	WL18XX_CONF_HW_RXTX_RATE_MCS14,
+	WL18XX_CONF_HW_RXTX_RATE_MCS13,
+	WL18XX_CONF_HW_RXTX_RATE_MCS12,
+	WL18XX_CONF_HW_RXTX_RATE_MCS11,
+	WL18XX_CONF_HW_RXTX_RATE_MCS10,
+	WL18XX_CONF_HW_RXTX_RATE_MCS9,
+	WL18XX_CONF_HW_RXTX_RATE_MCS8,
+	WL18XX_CONF_HW_RXTX_RATE_MCS7,
+	WL18XX_CONF_HW_RXTX_RATE_MCS6,
+	WL18XX_CONF_HW_RXTX_RATE_MCS5,
+	WL18XX_CONF_HW_RXTX_RATE_MCS4,
+	WL18XX_CONF_HW_RXTX_RATE_MCS3,
+	WL18XX_CONF_HW_RXTX_RATE_MCS2,
+	WL18XX_CONF_HW_RXTX_RATE_MCS1,
+	WL18XX_CONF_HW_RXTX_RATE_MCS0,
+	WL18XX_CONF_HW_RXTX_RATE_54,
+	WL18XX_CONF_HW_RXTX_RATE_48,
+	WL18XX_CONF_HW_RXTX_RATE_36,
+	WL18XX_CONF_HW_RXTX_RATE_24,
+	WL18XX_CONF_HW_RXTX_RATE_22,
+	WL18XX_CONF_HW_RXTX_RATE_18,
+	WL18XX_CONF_HW_RXTX_RATE_12,
+	WL18XX_CONF_HW_RXTX_RATE_11,
+	WL18XX_CONF_HW_RXTX_RATE_9,
+	WL18XX_CONF_HW_RXTX_RATE_6,
+	WL18XX_CONF_HW_RXTX_RATE_5_5,
+	WL18XX_CONF_HW_RXTX_RATE_2,
+	WL18XX_CONF_HW_RXTX_RATE_1,
+	WL18XX_CONF_HW_RXTX_RATE_MAX,
+};
+
+static struct wlcore_conf wl18xx_conf = {
+	.sg = {
+		.params = {
+			[CONF_SG_ACL_BT_MASTER_MIN_BR] = 10,
+			[CONF_SG_ACL_BT_MASTER_MAX_BR] = 180,
+			[CONF_SG_ACL_BT_SLAVE_MIN_BR] = 10,
+			[CONF_SG_ACL_BT_SLAVE_MAX_BR] = 180,
+			[CONF_SG_ACL_BT_MASTER_MIN_EDR] = 10,
+			[CONF_SG_ACL_BT_MASTER_MAX_EDR] = 80,
+			[CONF_SG_ACL_BT_SLAVE_MIN_EDR] = 10,
+			[CONF_SG_ACL_BT_SLAVE_MAX_EDR] = 80,
+			[CONF_SG_ACL_WLAN_PS_MASTER_BR] = 8,
+			[CONF_SG_ACL_WLAN_PS_SLAVE_BR] = 8,
+			[CONF_SG_ACL_WLAN_PS_MASTER_EDR] = 20,
+			[CONF_SG_ACL_WLAN_PS_SLAVE_EDR] = 20,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR] = 20,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR] = 35,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR] = 16,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR] = 35,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR] = 32,
+			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR] = 50,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR] = 28,
+			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR] = 50,
+			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR] = 10,
+			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR] = 20,
+			[CONF_SG_ACL_PASSIVE_SCAN_BT_BR] = 75,
+			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR] = 15,
+			[CONF_SG_ACL_PASSIVE_SCAN_BT_EDR] = 27,
+			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR] = 17,
+			/* active scan params */
+			[CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
+			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
+			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP] = 100,
+			/* passive scan params */
+			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_BR] = 800,
+			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_EDR] = 200,
+			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3] = 200,
+			/* passive scan in dual antenna params */
+			[CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
+			[CONF_SG_BCN_HV3_COLLISION_THRESH_IN_PASSIVE_SCAN] = 0,
+			[CONF_SG_TX_RX_PROTECTION_BWIDTH_IN_PASSIVE_SCAN] = 0,
+			/* general params */
+			[CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
+			[CONF_SG_ANTENNA_CONFIGURATION] = 0,
+			[CONF_SG_BEACON_MISS_PERCENT] = 60,
+			[CONF_SG_DHCP_TIME] = 5000,
+			[CONF_SG_RXT] = 1200,
+			[CONF_SG_TXT] = 1000,
+			[CONF_SG_ADAPTIVE_RXT_TXT] = 1,
+			[CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
+			[CONF_SG_HV3_MAX_SERVED] = 6,
+			[CONF_SG_PS_POLL_TIMEOUT] = 10,
+			[CONF_SG_UPSD_TIMEOUT] = 10,
+			[CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
+			[CONF_SG_STA_RX_WINDOW_AFTER_DTIM] = 5,
+			[CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 30,
+			/* AP params */
+			[CONF_AP_BEACON_MISS_TX] = 3,
+			[CONF_AP_RX_WINDOW_AFTER_BEACON] = 10,
+			[CONF_AP_BEACON_WINDOW_INTERVAL] = 2,
+			[CONF_AP_CONNECTION_PROTECTION_TIME] = 0,
+			[CONF_AP_BT_ACL_VAL_BT_SERVE_TIME] = 25,
+			[CONF_AP_BT_ACL_VAL_WL_SERVE_TIME] = 25,
+			/* CTS Diluting params */
+			[CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
+			[CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
+		},
+		.state = CONF_SG_PROTECTIVE,
+	},
+	.rx = {
+		.rx_msdu_life_time           = 512000,
+		.packet_detection_threshold  = 0,
+		.ps_poll_timeout             = 15,
+		.upsd_timeout                = 15,
+		.rts_threshold               = IEEE80211_MAX_RTS_THRESHOLD,
+		.rx_cca_threshold            = 0,
+		.irq_blk_threshold           = 0xFFFF,
+		.irq_pkt_threshold           = 0,
+		.irq_timeout                 = 600,
+		.queue_type                  = CONF_RX_QUEUE_TYPE_LOW_PRIORITY,
+	},
+	.tx = {
+		.tx_energy_detection         = 0,
+		.sta_rc_conf                 = {
+			.enabled_rates       = 0,
+			.short_retry_limit   = 10,
+			.long_retry_limit    = 10,
+			.aflags              = 0,
+		},
+		.ac_conf_count               = 4,
+		.ac_conf                     = {
+			[CONF_TX_AC_BE] = {
+				.ac          = CONF_TX_AC_BE,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = 3,
+				.tx_op_limit = 0,
+			},
+			[CONF_TX_AC_BK] = {
+				.ac          = CONF_TX_AC_BK,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = 7,
+				.tx_op_limit = 0,
+			},
+			[CONF_TX_AC_VI] = {
+				.ac          = CONF_TX_AC_VI,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = CONF_TX_AIFS_PIFS,
+				.tx_op_limit = 3008,
+			},
+			[CONF_TX_AC_VO] = {
+				.ac          = CONF_TX_AC_VO,
+				.cw_min      = 15,
+				.cw_max      = 63,
+				.aifsn       = CONF_TX_AIFS_PIFS,
+				.tx_op_limit = 1504,
+			},
+		},
+		.max_tx_retries = 100,
+		.ap_aging_period = 300,
+		.tid_conf_count = 4,
+		.tid_conf = {
+			[CONF_TX_AC_BE] = {
+				.queue_id    = CONF_TX_AC_BE,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_BE,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+			[CONF_TX_AC_BK] = {
+				.queue_id    = CONF_TX_AC_BK,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_BK,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+			[CONF_TX_AC_VI] = {
+				.queue_id    = CONF_TX_AC_VI,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_VI,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+			[CONF_TX_AC_VO] = {
+				.queue_id    = CONF_TX_AC_VO,
+				.channel_type = CONF_CHANNEL_TYPE_EDCF,
+				.tsid        = CONF_TX_AC_VO,
+				.ps_scheme   = CONF_PS_SCHEME_LEGACY,
+				.ack_policy  = CONF_ACK_POLICY_LEGACY,
+				.apsd_conf   = {0, 0},
+			},
+		},
+		.frag_threshold              = IEEE80211_MAX_FRAG_THRESHOLD,
+		.tx_compl_timeout            = 350,
+		.tx_compl_threshold          = 10,
+		.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,
+		.tx_watchdog_timeout         = 5000,
+	},
+	.conn = {
+		.wake_up_event               = CONF_WAKE_UP_EVENT_DTIM,
+		.listen_interval             = 1,
+		.suspend_wake_up_event       = CONF_WAKE_UP_EVENT_N_DTIM,
+		.suspend_listen_interval     = 3,
+		.bcn_filt_mode               = CONF_BCN_FILT_MODE_ENABLED,
+		.bcn_filt_ie_count           = 3,
+		.bcn_filt_ie = {
+			[0] = {
+				.ie          = WLAN_EID_CHANNEL_SWITCH,
+				.rule        = CONF_BCN_RULE_PASS_ON_APPEARANCE,
+			},
+			[1] = {
+				.ie          = WLAN_EID_HT_OPERATION,
+				.rule        = CONF_BCN_RULE_PASS_ON_CHANGE,
+			},
+			[2] = {
+				.ie	     = WLAN_EID_ERP_INFO,
+				.rule	     = CONF_BCN_RULE_PASS_ON_CHANGE,
+			},
+		},
+		.synch_fail_thold            = 12,
+		.bss_lose_timeout            = 400,
+		.beacon_rx_timeout           = 10000,
+		.broadcast_timeout           = 20000,
+		.rx_broadcast_in_ps          = 1,
+		.ps_poll_threshold           = 10,
+		.bet_enable                  = CONF_BET_MODE_ENABLE,
+		.bet_max_consecutive         = 50,
+		.psm_entry_retries           = 8,
+		.psm_exit_retries            = 16,
+		.psm_entry_nullfunc_retries  = 3,
+		.dynamic_ps_timeout          = 1500,
+		.forced_ps                   = false,
+		.keep_alive_interval         = 55000,
+		.max_listen_interval         = 20,
+		.sta_sleep_auth              = WL1271_PSM_ILLEGAL,
+	},
+	.itrim = {
+		.enable = false,
+		.timeout = 50000,
+	},
+	.pm_config = {
+		.host_clk_settling_time = 5000,
+		.host_fast_wakeup_support = CONF_FAST_WAKEUP_DISABLE,
+	},
+	.roam_trigger = {
+		.trigger_pacing               = 1,
+		.avg_weight_rssi_beacon       = 20,
+		.avg_weight_rssi_data         = 10,
+		.avg_weight_snr_beacon        = 20,
+		.avg_weight_snr_data          = 10,
+	},
+	.scan = {
+		.min_dwell_time_active        = 7500,
+		.max_dwell_time_active        = 30000,
+		.min_dwell_time_passive       = 100000,
+		.max_dwell_time_passive       = 100000,
+		.num_probe_reqs               = 2,
+		.split_scan_timeout           = 50000,
+	},
+	.sched_scan = {
+		/*
+		 * Values are in TU/1000 but since sched scan FW command
+		 * params are in TUs rounding up may occur.
+		 */
+		.base_dwell_time		= 7500,
+		.max_dwell_time_delta		= 22500,
+		/* based on 250bits per probe @1Mbps */
+		.dwell_time_delta_per_probe	= 2000,
+		/* based on 250bits per probe @6Mbps (plus a bit more) */
+		.dwell_time_delta_per_probe_5	= 350,
+		.dwell_time_passive		= 100000,
+		.dwell_time_dfs			= 150000,
+		.num_probe_reqs			= 2,
+		.rssi_threshold			= -90,
+		.snr_threshold			= 0,
+	},
+	.ht = {
+		.rx_ba_win_size = 10,
+		.tx_ba_win_size = 64,
+		.inactivity_timeout = 10000,
+		.tx_ba_tid_bitmap = CONF_TX_BA_ENABLED_TID_BITMAP,
+	},
+	.mem = {
+		.num_stations                 = 1,
+		.ssid_profiles                = 1,
+		.rx_block_num                 = 40,
+		.tx_min_block_num             = 40,
+		.dynamic_memory               = 1,
+		.min_req_tx_blocks            = 45,
+		.min_req_rx_blocks            = 22,
+		.tx_min                       = 27,
+	},
+	.fm_coex = {
+		.enable                       = true,
+		.swallow_period               = 5,
+		.n_divider_fref_set_1         = 0xff,       /* default */
+		.n_divider_fref_set_2         = 12,
+		.m_divider_fref_set_1         = 0xffff,
+		.m_divider_fref_set_2         = 148,        /* default */
+		.coex_pll_stabilization_time  = 0xffffffff, /* default */
+		.ldo_stabilization_time       = 0xffff,     /* default */
+		.fm_disturbed_band_margin     = 0xff,       /* default */
+		.swallow_clk_diff             = 0xff,       /* default */
+	},
+	.rx_streaming = {
+		.duration                      = 150,
+		.queues                        = 0x1,
+		.interval                      = 20,
+		.always                        = 0,
+	},
+	.fwlog = {
+		.mode                         = WL12XX_FWLOG_ON_DEMAND,
+		.mem_blocks                   = 2,
+		.severity                     = 0,
+		.timestamp                    = WL12XX_FWLOG_TIMESTAMP_DISABLED,
+		.output                       = WL12XX_FWLOG_OUTPUT_HOST,
+		.threshold                    = 0,
+	},
+	.rate = {
+		.rate_retry_score = 32000,
+		.per_add = 8192,
+		.per_th1 = 2048,
+		.per_th2 = 4096,
+		.max_per = 8100,
+		.inverse_curiosity_factor = 5,
+		.tx_fail_low_th = 4,
+		.tx_fail_high_th = 10,
+		.per_alpha_shift = 4,
+		.per_add_shift = 13,
+		.per_beta1_shift = 10,
+		.per_beta2_shift = 8,
+		.rate_check_up = 2,
+		.rate_check_down = 12,
+		.rate_retry_policy = {
+			0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0x00,
+		},
+	},
+	.hangover = {
+		.recover_time               = 0,
+		.hangover_period            = 20,
+		.dynamic_mode               = 1,
+		.early_termination_mode     = 1,
+		.max_period                 = 20,
+		.min_period                 = 1,
+		.increase_delta             = 1,
+		.decrease_delta             = 2,
+		.quiet_time                 = 4,
+		.increase_time              = 1,
+		.window_size                = 16,
+	},
+};
+
+static struct wl18xx_priv_conf wl18xx_default_priv_conf = {
+	.ht = {
+		.mode				= HT_MODE_DEFAULT,
+	},
+	.phy = {
+		.phy_standalone			= 0x00,
+		.primary_clock_setting_time	= 0x05,
+		.clock_valid_on_wake_up		= 0x00,
+		.secondary_clock_setting_time	= 0x05,
+		.board_type 			= BOARD_TYPE_HDK_18XX,
+		.rdl				= 0x01,
+		.auto_detect			= 0x00,
+		.dedicated_fem			= FEM_NONE,
+		.low_band_component		= COMPONENT_2_WAY_SWITCH,
+		.low_band_component_type	= 0x06,
+		.high_band_component		= COMPONENT_2_WAY_SWITCH,
+		.high_band_component_type	= 0x09,
+		.tcxo_ldo_voltage		= 0x00,
+		.xtal_itrim_val			= 0x04,
+		.srf_state			= 0x00,
+		.io_configuration		= 0x01,
+		.sdio_configuration		= 0x00,
+		.settings			= 0x00,
+		.enable_clpc			= 0x00,
+		.enable_tx_low_pwr_on_siso_rdl	= 0x00,
+		.rx_profile			= 0x00,
+		.pwr_limit_reference_11_abg	= 0xc8,
+		.psat				= 0,
+		.low_power_val			= 0x00,
+		.med_power_val			= 0x0a,
+		.high_power_val			= 0x1e,
+		.external_pa_dc2dc		= 0,
+		.number_of_assembled_ant2_4	= 1,
+		.number_of_assembled_ant5	= 1,
+	},
+};
+
+static const struct wlcore_partition_set wl18xx_ptable[PART_TABLE_LEN] = {
+	[PART_TOP_PRCM_ELP_SOC] = {
+		.mem  = { .start = 0x00A02000, .size  = 0x00010000 },
+		.reg  = { .start = 0x00807000, .size  = 0x00005000 },
+		.mem2 = { .start = 0x00800000, .size  = 0x0000B000 },
+		.mem3 = { .start = 0x00000000, .size  = 0x00000000 },
+	},
+	[PART_DOWN] = {
+		.mem  = { .start = 0x00000000, .size  = 0x00014000 },
+		.reg  = { .start = 0x00810000, .size  = 0x0000BFFF },
+		.mem2 = { .start = 0x00000000, .size  = 0x00000000 },
+		.mem3 = { .start = 0x00000000, .size  = 0x00000000 },
+	},
+	[PART_BOOT] = {
+		.mem  = { .start = 0x00700000, .size = 0x0000030c },
+		.reg  = { .start = 0x00802000, .size = 0x00014578 },
+		.mem2 = { .start = 0x00B00404, .size = 0x00001000 },
+		.mem3 = { .start = 0x00C00000, .size = 0x00000400 },
+	},
+	[PART_WORK] = {
+		.mem  = { .start = 0x00800000, .size  = 0x000050FC },
+		.reg  = { .start = 0x00B00404, .size  = 0x00001000 },
+		.mem2 = { .start = 0x00C00000, .size  = 0x00000400 },
+		.mem3 = { .start = 0x00000000, .size  = 0x00000000 },
+	},
+	[PART_PHY_INIT] = {
+		.mem  = { .start = 0x80926000,
+			  .size = sizeof(struct wl18xx_mac_and_phy_params) },
+		.reg  = { .start = 0x00000000, .size = 0x00000000 },
+		.mem2 = { .start = 0x00000000, .size = 0x00000000 },
+		.mem3 = { .start = 0x00000000, .size = 0x00000000 },
+	},
+};
+
+static const int wl18xx_rtable[REG_TABLE_LEN] = {
+	[REG_ECPU_CONTROL]		= WL18XX_REG_ECPU_CONTROL,
+	[REG_INTERRUPT_NO_CLEAR]	= WL18XX_REG_INTERRUPT_NO_CLEAR,
+	[REG_INTERRUPT_ACK]		= WL18XX_REG_INTERRUPT_ACK,
+	[REG_COMMAND_MAILBOX_PTR]	= WL18XX_REG_COMMAND_MAILBOX_PTR,
+	[REG_EVENT_MAILBOX_PTR]		= WL18XX_REG_EVENT_MAILBOX_PTR,
+	[REG_INTERRUPT_TRIG]		= WL18XX_REG_INTERRUPT_TRIG_H,
+	[REG_INTERRUPT_MASK]		= WL18XX_REG_INTERRUPT_MASK,
+	[REG_PC_ON_RECOVERY]		= WL18XX_SCR_PAD4,
+	[REG_CHIP_ID_B]			= WL18XX_REG_CHIP_ID_B,
+	[REG_CMD_MBOX_ADDRESS]		= WL18XX_CMD_MBOX_ADDRESS,
+
+	/* data access memory addresses, used with partition translation */
+	[REG_SLV_MEM_DATA]		= WL18XX_SLV_MEM_DATA,
+	[REG_SLV_REG_DATA]		= WL18XX_SLV_REG_DATA,
+
+	/* raw data access memory addresses */
+	[REG_RAW_FW_STATUS_ADDR]	= WL18XX_FW_STATUS_ADDR,
+};
+
+static const struct wl18xx_clk_cfg wl18xx_clk_table[NUM_CLOCK_CONFIGS] = {
+	[CLOCK_CONFIG_16_2_M]	= { 7,  104,  801, 4,  true },
+	[CLOCK_CONFIG_16_368_M]	= { 9,  132, 3751, 4,  true },
+	[CLOCK_CONFIG_16_8_M]	= { 7,  100,    0, 0, false },
+	[CLOCK_CONFIG_19_2_M]	= { 8,  100,    0, 0, false },
+	[CLOCK_CONFIG_26_M]	= { 13, 120,    0, 0, false },
+	[CLOCK_CONFIG_32_736_M]	= { 9,  132, 3751, 4,  true },
+	[CLOCK_CONFIG_33_6_M]	= { 7,  100,    0, 0, false },
+	[CLOCK_CONFIG_38_468_M]	= { 8,  100,    0, 0, false },
+	[CLOCK_CONFIG_52_M]	= { 13, 120,    0, 0, false },
+};
+
+/* TODO: maybe move to a new header file? */
+#define WL18XX_FW_NAME "ti-connectivity/wl18xx-fw.bin"
+
+static int wl18xx_identify_chip(struct wl1271 *wl)
+{
+	int ret = 0;
+
+	switch (wl->chip.id) {
+	case CHIP_ID_185x_PG20:
+		wl1271_debug(DEBUG_BOOT, "chip id 0x%x (185x PG20)",
+				 wl->chip.id);
+		wl->sr_fw_name = WL18XX_FW_NAME;
+		/* wl18xx uses the same firmware for PLT */
+		wl->plt_fw_name = WL18XX_FW_NAME;
+		wl->quirks |= WLCORE_QUIRK_NO_ELP |
+			      WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN |
+			      WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN |
+			      WLCORE_QUIRK_NO_SCHED_SCAN_WHILE_CONN |
+			      WLCORE_QUIRK_TX_PAD_LAST_FRAME;
+
+		wlcore_set_min_fw_ver(wl, WL18XX_CHIP_VER, WL18XX_IFTYPE_VER,
+				      WL18XX_MAJOR_VER, WL18XX_SUBTYPE_VER,
+				      WL18XX_MINOR_VER);
+		break;
+	case CHIP_ID_185x_PG10:
+		wl1271_warning("chip id 0x%x (185x PG10) is deprecated",
+			       wl->chip.id);
+		ret = -ENODEV;
+		goto out;
+
+	default:
+		wl1271_warning("unsupported chip id: 0x%x", wl->chip.id);
+		ret = -ENODEV;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static int wl18xx_set_clk(struct wl1271 *wl)
+{
+	u16 clk_freq;
+	int ret;
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+	if (ret < 0)
+		goto out;
+
+	/* TODO: PG2: apparently we need to read the clk type */
+
+	ret = wl18xx_top_reg_read(wl, PRIMARY_CLK_DETECT, &clk_freq);
+	if (ret < 0)
+		goto out;
+
+	wl1271_debug(DEBUG_BOOT, "clock freq %d (%d, %d, %d, %d, %s)", clk_freq,
+		     wl18xx_clk_table[clk_freq].n, wl18xx_clk_table[clk_freq].m,
+		     wl18xx_clk_table[clk_freq].p, wl18xx_clk_table[clk_freq].q,
+		     wl18xx_clk_table[clk_freq].swallow ? "swallow" : "spit");
+
+	ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_N,
+				   wl18xx_clk_table[clk_freq].n);
+	if (ret < 0)
+		goto out;
+
+	ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_M,
+				   wl18xx_clk_table[clk_freq].m);
+	if (ret < 0)
+		goto out;
+
+	if (wl18xx_clk_table[clk_freq].swallow) {
+		/* first the 16 lower bits */
+		ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_Q_FACTOR_CFG_1,
+					   wl18xx_clk_table[clk_freq].q &
+					   PLLSH_WCS_PLL_Q_FACTOR_CFG_1_MASK);
+		if (ret < 0)
+			goto out;
+
+		/* then the 16 higher bits, masked out */
+		ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_Q_FACTOR_CFG_2,
+					(wl18xx_clk_table[clk_freq].q >> 16) &
+					PLLSH_WCS_PLL_Q_FACTOR_CFG_2_MASK);
+		if (ret < 0)
+			goto out;
+
+		/* first the 16 lower bits */
+		ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_P_FACTOR_CFG_1,
+					   wl18xx_clk_table[clk_freq].p &
+					   PLLSH_WCS_PLL_P_FACTOR_CFG_1_MASK);
+		if (ret < 0)
+			goto out;
+
+		/* then the 16 higher bits, masked out */
+		ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_P_FACTOR_CFG_2,
+					(wl18xx_clk_table[clk_freq].p >> 16) &
+					PLLSH_WCS_PLL_P_FACTOR_CFG_2_MASK);
+	} else {
+		ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_SWALLOW_EN,
+					   PLLSH_WCS_PLL_SWALLOW_EN_VAL2);
+	}
+
+out:
+	return ret;
+}
+
+static int wl18xx_boot_soft_reset(struct wl1271 *wl)
+{
+	int ret;
+
+	/* disable Rx/Tx */
+	ret = wlcore_write32(wl, WL18XX_ENABLE, 0x0);
+	if (ret < 0)
+		goto out;
+
+	/* disable auto calibration on start*/
+	ret = wlcore_write32(wl, WL18XX_SPARE_A2, 0xffff);
+
+out:
+	return ret;
+}
+
+static int wl18xx_pre_boot(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wl18xx_set_clk(wl);
+	if (ret < 0)
+		goto out;
+
+	/* Continue the ELP wake up sequence */
+	ret = wlcore_write32(wl, WL18XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+	if (ret < 0)
+		goto out;
+
+	udelay(500);
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	if (ret < 0)
+		goto out;
+
+	/* Disable interrupts */
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+	if (ret < 0)
+		goto out;
+
+	ret = wl18xx_boot_soft_reset(wl);
+
+out:
+	return ret;
+}
+
+static int wl18xx_pre_upload(struct wl1271 *wl)
+{
+	u32 tmp;
+	int ret;
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	if (ret < 0)
+		goto out;
+
+	/* TODO: check if this is all needed */
+	ret = wlcore_write32(wl, WL18XX_EEPROMLESS_IND, WL18XX_EEPROMLESS_IND);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &tmp);
+	if (ret < 0)
+		goto out;
+
+	wl1271_debug(DEBUG_BOOT, "chip id 0x%x", tmp);
+
+	ret = wlcore_read32(wl, WL18XX_SCR_PAD2, &tmp);
+
+out:
+	return ret;
+}
+
+static int wl18xx_set_mac_and_phy(struct wl1271 *wl)
+{
+	struct wl18xx_priv *priv = wl->priv;
+	struct wl18xx_mac_and_phy_params *params;
+	int ret;
+
+	params = kmemdup(&priv->conf.phy, sizeof(*params), GFP_KERNEL);
+	if (!params) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_PHY_INIT]);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_write(wl, WL18XX_PHY_INIT_MEM_ADDR, params,
+			   sizeof(*params), false);
+
+out:
+	kfree(params);
+	return ret;
+}
+
+static int wl18xx_enable_interrupts(struct wl1271 *wl)
+{
+	u32 event_mask, intr_mask;
+	int ret;
+
+	event_mask = WL18XX_ACX_EVENTS_VECTOR;
+	intr_mask = WL18XX_INTR_MASK;
+
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK, event_mask);
+	if (ret < 0)
+		goto out;
+
+	wlcore_enable_interrupts(wl);
+
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+			       WL1271_ACX_INTR_ALL & ~intr_mask);
+
+out:
+	return ret;
+}
+
+static int wl18xx_boot(struct wl1271 *wl)
+{
+	int ret;
+
+	ret = wl18xx_pre_boot(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl18xx_pre_upload(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_boot_upload_firmware(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl18xx_set_mac_and_phy(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_boot_run_firmware(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl18xx_enable_interrupts(wl);
+
+out:
+	return ret;
+}
+
+static int wl18xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
+			       void *buf, size_t len)
+{
+	struct wl18xx_priv *priv = wl->priv;
+
+	memcpy(priv->cmd_buf, buf, len);
+	memset(priv->cmd_buf + len, 0, WL18XX_CMD_MAX_SIZE - len);
+
+	return wlcore_write(wl, cmd_box_addr, priv->cmd_buf,
+			    WL18XX_CMD_MAX_SIZE, false);
+}
+
+static int wl18xx_ack_event(struct wl1271 *wl)
+{
+	return wlcore_write_reg(wl, REG_INTERRUPT_TRIG,
+				WL18XX_INTR_TRIG_EVENT_ACK);
+}
+
+static u32 wl18xx_calc_tx_blocks(struct wl1271 *wl, u32 len, u32 spare_blks)
+{
+	u32 blk_size = WL18XX_TX_HW_BLOCK_SIZE;
+	return (len + blk_size - 1) / blk_size + spare_blks;
+}
+
+static void
+wl18xx_set_tx_desc_blocks(struct wl1271 *wl, struct wl1271_tx_hw_descr *desc,
+			  u32 blks, u32 spare_blks)
+{
+	desc->wl18xx_mem.total_mem_blocks = blks;
+}
+
+static void
+wl18xx_set_tx_desc_data_len(struct wl1271 *wl, struct wl1271_tx_hw_descr *desc,
+			    struct sk_buff *skb)
+{
+	desc->length = cpu_to_le16(skb->len);
+
+	/* if only the last frame is to be padded, we unset this bit on Tx */
+	if (wl->quirks & WLCORE_QUIRK_TX_PAD_LAST_FRAME)
+		desc->wl18xx_mem.ctrl = WL18XX_TX_CTRL_NOT_PADDED;
+	else
+		desc->wl18xx_mem.ctrl = 0;
+
+	wl1271_debug(DEBUG_TX, "tx_fill_hdr: hlid: %d "
+		     "len: %d life: %d mem: %d", desc->hlid,
+		     le16_to_cpu(desc->length),
+		     le16_to_cpu(desc->life_time),
+		     desc->wl18xx_mem.total_mem_blocks);
+}
+
+static enum wl_rx_buf_align
+wl18xx_get_rx_buf_align(struct wl1271 *wl, u32 rx_desc)
+{
+	if (rx_desc & RX_BUF_PADDED_PAYLOAD)
+		return WLCORE_RX_BUF_PADDED;
+
+	return WLCORE_RX_BUF_ALIGNED;
+}
+
+static u32 wl18xx_get_rx_packet_len(struct wl1271 *wl, void *rx_data,
+				    u32 data_len)
+{
+	struct wl1271_rx_descriptor *desc = rx_data;
+
+	/* invalid packet */
+	if (data_len < sizeof(*desc))
+		return 0;
+
+	return data_len - sizeof(*desc);
+}
+
+static void wl18xx_tx_immediate_completion(struct wl1271 *wl)
+{
+	wl18xx_tx_immediate_complete(wl);
+}
+
+static int wl18xx_set_host_cfg_bitmap(struct wl1271 *wl, u32 extra_mem_blk)
+{
+	int ret;
+	u32 sdio_align_size = 0;
+	u32 host_cfg_bitmap = HOST_IF_CFG_RX_FIFO_ENABLE |
+			      HOST_IF_CFG_ADD_RX_ALIGNMENT;
+
+	/* Enable Tx SDIO padding */
+	if (wl->quirks & WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN) {
+		host_cfg_bitmap |= HOST_IF_CFG_TX_PAD_TO_SDIO_BLK;
+		sdio_align_size = WL12XX_BUS_BLOCK_SIZE;
+	}
+
+	/* Enable Rx SDIO padding */
+	if (wl->quirks & WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN) {
+		host_cfg_bitmap |= HOST_IF_CFG_RX_PAD_TO_SDIO_BLK;
+		sdio_align_size = WL12XX_BUS_BLOCK_SIZE;
+	}
+
+	ret = wl18xx_acx_host_if_cfg_bitmap(wl, host_cfg_bitmap,
+					    sdio_align_size, extra_mem_blk,
+					    WL18XX_HOST_IF_LEN_SIZE_FIELD);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wl18xx_hw_init(struct wl1271 *wl)
+{
+	int ret;
+	struct wl18xx_priv *priv = wl->priv;
+
+	/* (re)init private structures. Relevant on recovery as well. */
+	priv->last_fw_rls_idx = 0;
+	priv->extra_spare_vif_count = 0;
+
+	/* set the default amount of spare blocks in the bitmap */
+	ret = wl18xx_set_host_cfg_bitmap(wl, WL18XX_TX_HW_BLOCK_SPARE);
+	if (ret < 0)
+		return ret;
+
+	if (checksum_param) {
+		ret = wl18xx_acx_set_checksum_state(wl);
+		if (ret != 0)
+			return ret;
+	}
+
+	return ret;
+}
+
+static void wl18xx_set_tx_desc_csum(struct wl1271 *wl,
+				    struct wl1271_tx_hw_descr *desc,
+				    struct sk_buff *skb)
+{
+	u32 ip_hdr_offset;
+	struct iphdr *ip_hdr;
+
+	if (!checksum_param) {
+		desc->wl18xx_checksum_data = 0;
+		return;
+	}
+
+	if (skb->ip_summed != CHECKSUM_PARTIAL) {
+		desc->wl18xx_checksum_data = 0;
+		return;
+	}
+
+	ip_hdr_offset = skb_network_header(skb) - skb_mac_header(skb);
+	if (WARN_ON(ip_hdr_offset >= (1<<7))) {
+		desc->wl18xx_checksum_data = 0;
+		return;
+	}
+
+	desc->wl18xx_checksum_data = ip_hdr_offset << 1;
+
+	/* FW is interested only in the LSB of the protocol  TCP=0 UDP=1 */
+	ip_hdr = (void *)skb_network_header(skb);
+	desc->wl18xx_checksum_data |= (ip_hdr->protocol & 0x01);
+}
+
+static void wl18xx_set_rx_csum(struct wl1271 *wl,
+			       struct wl1271_rx_descriptor *desc,
+			       struct sk_buff *skb)
+{
+	if (desc->status & WL18XX_RX_CHECKSUM_MASK)
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+}
+
+static bool wl18xx_is_mimo_supported(struct wl1271 *wl)
+{
+	struct wl18xx_priv *priv = wl->priv;
+
+	return priv->conf.phy.number_of_assembled_ant2_4 >= 2;
+}
+
+/*
+ * TODO: instead of having these two functions to get the rate mask,
+ * we should modify the wlvif->rate_set instead
+ */
+static u32 wl18xx_sta_get_ap_rate_mask(struct wl1271 *wl,
+				       struct wl12xx_vif *wlvif)
+{
+	u32 hw_rate_set = wlvif->rate_set;
+
+	if (wlvif->channel_type == NL80211_CHAN_HT40MINUS ||
+	    wlvif->channel_type == NL80211_CHAN_HT40PLUS) {
+		wl1271_debug(DEBUG_ACX, "using wide channel rate mask");
+		hw_rate_set |= CONF_TX_RATE_USE_WIDE_CHAN;
+
+		/* we don't support MIMO in wide-channel mode */
+		hw_rate_set &= ~CONF_TX_MIMO_RATES;
+	} else if (wl18xx_is_mimo_supported(wl)) {
+		wl1271_debug(DEBUG_ACX, "using MIMO channel rate mask");
+		hw_rate_set |= CONF_TX_MIMO_RATES;
+	}
+
+	return hw_rate_set;
+}
+
+static u32 wl18xx_ap_get_mimo_wide_rate_mask(struct wl1271 *wl,
+					     struct wl12xx_vif *wlvif)
+{
+	if (wlvif->channel_type == NL80211_CHAN_HT40MINUS ||
+	    wlvif->channel_type == NL80211_CHAN_HT40PLUS) {
+		wl1271_debug(DEBUG_ACX, "using wide channel rate mask");
+
+		/* sanity check - we don't support this */
+		if (WARN_ON(wlvif->band != IEEE80211_BAND_5GHZ))
+			return 0;
+
+		return CONF_TX_RATE_USE_WIDE_CHAN;
+	} else if (wl18xx_is_mimo_supported(wl) &&
+		   wlvif->band == IEEE80211_BAND_2GHZ) {
+		wl1271_debug(DEBUG_ACX, "using MIMO rate mask");
+		/*
+		 * we don't care about HT channel here - if a peer doesn't
+		 * support MIMO, we won't enable it in its rates
+		 */
+		return CONF_TX_MIMO_RATES;
+	} else {
+		return 0;
+	}
+}
+
+static int wl18xx_get_pg_ver(struct wl1271 *wl, s8 *ver)
+{
+	u32 fuse;
+	int ret;
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_read32(wl, WL18XX_REG_FUSE_DATA_1_3, &fuse);
+	if (ret < 0)
+		goto out;
+
+	if (ver)
+		*ver = (fuse & WL18XX_PG_VER_MASK) >> WL18XX_PG_VER_OFFSET;
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+
+out:
+	return ret;
+}
+
+#define WL18XX_CONF_FILE_NAME "ti-connectivity/wl18xx-conf.bin"
+static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
+{
+	struct wl18xx_priv *priv = wl->priv;
+	struct wlcore_conf_file *conf_file;
+	const struct firmware *fw;
+	int ret;
+
+	ret = request_firmware(&fw, WL18XX_CONF_FILE_NAME, dev);
+	if (ret < 0) {
+		wl1271_error("could not get configuration binary %s: %d",
+			     WL18XX_CONF_FILE_NAME, ret);
+		goto out_fallback;
+	}
+
+	if (fw->size != WL18XX_CONF_SIZE) {
+		wl1271_error("configuration binary file size is wrong, expected %zu got %zu",
+			     WL18XX_CONF_SIZE, fw->size);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	conf_file = (struct wlcore_conf_file *) fw->data;
+
+	if (conf_file->header.magic != cpu_to_le32(WL18XX_CONF_MAGIC)) {
+		wl1271_error("configuration binary file magic number mismatch, "
+			     "expected 0x%0x got 0x%0x", WL18XX_CONF_MAGIC,
+			     conf_file->header.magic);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (conf_file->header.version != cpu_to_le32(WL18XX_CONF_VERSION)) {
+		wl1271_error("configuration binary file version not supported, "
+			     "expected 0x%08x got 0x%08x",
+			     WL18XX_CONF_VERSION, conf_file->header.version);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	memcpy(&wl->conf, &conf_file->core, sizeof(wl18xx_conf));
+	memcpy(&priv->conf, &conf_file->priv, sizeof(priv->conf));
+
+	goto out;
+
+out_fallback:
+	wl1271_warning("falling back to default config");
+
+	/* apply driver default configuration */
+	memcpy(&wl->conf, &wl18xx_conf, sizeof(wl18xx_conf));
+	/* apply default private configuration */
+	memcpy(&priv->conf, &wl18xx_default_priv_conf, sizeof(priv->conf));
+
+	/* For now we just fallback */
+	return 0;
+
+out:
+	release_firmware(fw);
+	return ret;
+}
+
+static int wl18xx_plt_init(struct wl1271 *wl)
+{
+	int ret;
+
+	/* calibrator based auto/fem detect not supported for 18xx */
+	if (wl->plt_mode == PLT_FEM_DETECT) {
+		wl1271_error("wl18xx_plt_init: PLT FEM_DETECT not supported");
+		return -EINVAL;
+	}
+
+	ret = wlcore_write32(wl, WL18XX_SCR_PAD8, WL18XX_SCR_PAD8_PLT);
+	if (ret < 0)
+		return ret;
+
+	return wl->ops->boot(wl);
+}
+
+static int wl18xx_get_mac(struct wl1271 *wl)
+{
+	u32 mac1, mac2;
+	int ret;
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_read32(wl, WL18XX_REG_FUSE_BD_ADDR_1, &mac1);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_read32(wl, WL18XX_REG_FUSE_BD_ADDR_2, &mac2);
+	if (ret < 0)
+		goto out;
+
+	/* these are the two parts of the BD_ADDR */
+	wl->fuse_oui_addr = ((mac2 & 0xffff) << 8) +
+		((mac1 & 0xff000000) >> 24);
+	wl->fuse_nic_addr = (mac1 & 0xffffff);
+
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+
+out:
+	return ret;
+}
+
+static int wl18xx_handle_static_data(struct wl1271 *wl,
+				     struct wl1271_static_data *static_data)
+{
+	struct wl18xx_static_data_priv *static_data_priv =
+		(struct wl18xx_static_data_priv *) static_data->priv;
+
+	wl1271_info("PHY firmware version: %s", static_data_priv->phy_version);
+
+	return 0;
+}
+
+static int wl18xx_get_spare_blocks(struct wl1271 *wl, bool is_gem)
+{
+	struct wl18xx_priv *priv = wl->priv;
+
+	/* If we have VIFs requiring extra spare, indulge them */
+	if (priv->extra_spare_vif_count)
+		return WL18XX_TX_HW_EXTRA_BLOCK_SPARE;
+
+	return WL18XX_TX_HW_BLOCK_SPARE;
+}
+
+static int wl18xx_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
+			  struct ieee80211_vif *vif,
+			  struct ieee80211_sta *sta,
+			  struct ieee80211_key_conf *key_conf)
+{
+	struct wl18xx_priv *priv = wl->priv;
+	bool change_spare = false;
+	int ret;
+
+	/*
+	 * when adding the first or removing the last GEM/TKIP interface,
+	 * we have to adjust the number of spare blocks.
+	 */
+	change_spare = (key_conf->cipher == WL1271_CIPHER_SUITE_GEM ||
+		key_conf->cipher == WLAN_CIPHER_SUITE_TKIP) &&
+		((priv->extra_spare_vif_count == 0 && cmd == SET_KEY) ||
+		 (priv->extra_spare_vif_count == 1 && cmd == DISABLE_KEY));
+
+	/* no need to change spare - just regular set_key */
+	if (!change_spare)
+		return wlcore_set_key(wl, cmd, vif, sta, key_conf);
+
+	/*
+	 * stop the queues and flush to ensure the next packets are
+	 * in sync with FW spare block accounting
+	 */
+	wlcore_stop_queues(wl, WLCORE_QUEUE_STOP_REASON_SPARE_BLK);
+	wl1271_tx_flush(wl);
+
+	ret = wlcore_set_key(wl, cmd, vif, sta, key_conf);
+	if (ret < 0)
+		goto out;
+
+	/* key is now set, change the spare blocks */
+	if (cmd == SET_KEY) {
+		ret = wl18xx_set_host_cfg_bitmap(wl,
+					WL18XX_TX_HW_EXTRA_BLOCK_SPARE);
+		if (ret < 0)
+			goto out;
+
+		priv->extra_spare_vif_count++;
+	} else {
+		ret = wl18xx_set_host_cfg_bitmap(wl,
+					WL18XX_TX_HW_BLOCK_SPARE);
+		if (ret < 0)
+			goto out;
+
+		priv->extra_spare_vif_count--;
+	}
+
+out:
+	wlcore_wake_queues(wl, WLCORE_QUEUE_STOP_REASON_SPARE_BLK);
+	return ret;
+}
+
+static u32 wl18xx_pre_pkt_send(struct wl1271 *wl,
+			       u32 buf_offset, u32 last_len)
+{
+	if (wl->quirks & WLCORE_QUIRK_TX_PAD_LAST_FRAME) {
+		struct wl1271_tx_hw_descr *last_desc;
+
+		/* get the last TX HW descriptor written to the aggr buf */
+		last_desc = (struct wl1271_tx_hw_descr *)(wl->aggr_buf +
+							buf_offset - last_len);
+
+		/* the last frame is padded up to an SDIO block */
+		last_desc->wl18xx_mem.ctrl &= ~WL18XX_TX_CTRL_NOT_PADDED;
+		return ALIGN(buf_offset, WL12XX_BUS_BLOCK_SIZE);
+	}
+
+	/* no modifications */
+	return buf_offset;
+}
+
+static struct wlcore_ops wl18xx_ops = {
+	.identify_chip	= wl18xx_identify_chip,
+	.boot		= wl18xx_boot,
+	.plt_init	= wl18xx_plt_init,
+	.trigger_cmd	= wl18xx_trigger_cmd,
+	.ack_event	= wl18xx_ack_event,
+	.calc_tx_blocks = wl18xx_calc_tx_blocks,
+	.set_tx_desc_blocks = wl18xx_set_tx_desc_blocks,
+	.set_tx_desc_data_len = wl18xx_set_tx_desc_data_len,
+	.get_rx_buf_align = wl18xx_get_rx_buf_align,
+	.get_rx_packet_len = wl18xx_get_rx_packet_len,
+	.tx_immediate_compl = wl18xx_tx_immediate_completion,
+	.tx_delayed_compl = NULL,
+	.hw_init	= wl18xx_hw_init,
+	.set_tx_desc_csum = wl18xx_set_tx_desc_csum,
+	.get_pg_ver	= wl18xx_get_pg_ver,
+	.set_rx_csum = wl18xx_set_rx_csum,
+	.sta_get_ap_rate_mask = wl18xx_sta_get_ap_rate_mask,
+	.ap_get_mimo_wide_rate_mask = wl18xx_ap_get_mimo_wide_rate_mask,
+	.get_mac	= wl18xx_get_mac,
+	.debugfs_init	= wl18xx_debugfs_add_files,
+	.handle_static_data	= wl18xx_handle_static_data,
+	.get_spare_blocks = wl18xx_get_spare_blocks,
+	.set_key	= wl18xx_set_key,
+	.pre_pkt_send	= wl18xx_pre_pkt_send,
+};
+
+/* HT cap appropriate for wide channels in 2Ghz */
+static struct ieee80211_sta_ht_cap wl18xx_siso40_ht_cap_2ghz = {
+	.cap = IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
+	       IEEE80211_HT_CAP_SUP_WIDTH_20_40 | IEEE80211_HT_CAP_DSSSCCK40,
+	.ht_supported = true,
+	.ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
+	.ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
+	.mcs = {
+		.rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
+		.rx_highest = cpu_to_le16(150),
+		.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+};
+
+/* HT cap appropriate for wide channels in 5Ghz */
+static struct ieee80211_sta_ht_cap wl18xx_siso40_ht_cap_5ghz = {
+	.cap = IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
+	       IEEE80211_HT_CAP_SUP_WIDTH_20_40,
+	.ht_supported = true,
+	.ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
+	.ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
+	.mcs = {
+		.rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
+		.rx_highest = cpu_to_le16(150),
+		.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+};
+
+/* HT cap appropriate for SISO 20 */
+static struct ieee80211_sta_ht_cap wl18xx_siso20_ht_cap = {
+	.cap = IEEE80211_HT_CAP_SGI_20,
+	.ht_supported = true,
+	.ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
+	.ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
+	.mcs = {
+		.rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
+		.rx_highest = cpu_to_le16(72),
+		.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+};
+
+/* HT cap appropriate for MIMO rates in 20mhz channel */
+static struct ieee80211_sta_ht_cap wl18xx_mimo_ht_cap_2ghz = {
+	.cap = IEEE80211_HT_CAP_SGI_20,
+	.ht_supported = true,
+	.ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
+	.ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
+	.mcs = {
+		.rx_mask = { 0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, },
+		.rx_highest = cpu_to_le16(144),
+		.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+};
+
+static int __devinit wl18xx_probe(struct platform_device *pdev)
+{
+	struct wl1271 *wl;
+	struct ieee80211_hw *hw;
+	struct wl18xx_priv *priv;
+	int ret;
+
+	hw = wlcore_alloc_hw(sizeof(*priv));
+	if (IS_ERR(hw)) {
+		wl1271_error("can't allocate hw");
+		ret = PTR_ERR(hw);
+		goto out;
+	}
+
+	wl = hw->priv;
+	priv = wl->priv;
+	wl->ops = &wl18xx_ops;
+	wl->ptable = wl18xx_ptable;
+	wl->rtable = wl18xx_rtable;
+	wl->num_tx_desc = 32;
+	wl->num_rx_desc = 32;
+	wl->band_rate_to_idx = wl18xx_band_rate_to_idx;
+	wl->hw_tx_rate_tbl_size = WL18XX_CONF_HW_RXTX_RATE_MAX;
+	wl->hw_min_ht_rate = WL18XX_CONF_HW_RXTX_RATE_MCS0;
+	wl->fw_status_priv_len = sizeof(struct wl18xx_fw_status_priv);
+	wl->stats.fw_stats_len = sizeof(struct wl18xx_acx_statistics);
+	wl->static_data_priv_len = sizeof(struct wl18xx_static_data_priv);
+
+	if (num_rx_desc_param != -1)
+		wl->num_rx_desc = num_rx_desc_param;
+
+	ret = wl18xx_conf_init(wl, &pdev->dev);
+	if (ret < 0)
+		goto out_free;
+
+	/* If the module param is set, update it in conf */
+	if (board_type_param) {
+		if (!strcmp(board_type_param, "fpga")) {
+			priv->conf.phy.board_type = BOARD_TYPE_FPGA_18XX;
+		} else if (!strcmp(board_type_param, "hdk")) {
+			priv->conf.phy.board_type = BOARD_TYPE_HDK_18XX;
+		} else if (!strcmp(board_type_param, "dvp")) {
+			priv->conf.phy.board_type = BOARD_TYPE_DVP_18XX;
+		} else if (!strcmp(board_type_param, "evb")) {
+			priv->conf.phy.board_type = BOARD_TYPE_EVB_18XX;
+		} else if (!strcmp(board_type_param, "com8")) {
+			priv->conf.phy.board_type = BOARD_TYPE_COM8_18XX;
+		} else {
+			wl1271_error("invalid board type '%s'",
+				board_type_param);
+			ret = -EINVAL;
+			goto out_free;
+		}
+	}
+
+	/* HACK! Just for now we hardcode COM8 and HDK to 0x06 */
+	switch (priv->conf.phy.board_type) {
+	case BOARD_TYPE_HDK_18XX:
+	case BOARD_TYPE_COM8_18XX:
+		priv->conf.phy.low_band_component_type = 0x06;
+		break;
+	case BOARD_TYPE_FPGA_18XX:
+	case BOARD_TYPE_DVP_18XX:
+	case BOARD_TYPE_EVB_18XX:
+		priv->conf.phy.low_band_component_type = 0x05;
+		break;
+	default:
+		wl1271_error("invalid board type '%d'",
+			priv->conf.phy.board_type);
+		ret = -EINVAL;
+		goto out_free;
+	}
+
+	if (low_band_component_param != -1)
+		priv->conf.phy.low_band_component = low_band_component_param;
+	if (low_band_component_type_param != -1)
+		priv->conf.phy.low_band_component_type =
+			low_band_component_type_param;
+	if (high_band_component_param != -1)
+		priv->conf.phy.high_band_component = high_band_component_param;
+	if (high_band_component_type_param != -1)
+		priv->conf.phy.high_band_component_type =
+			high_band_component_type_param;
+	if (pwr_limit_reference_11_abg_param != -1)
+		priv->conf.phy.pwr_limit_reference_11_abg =
+			pwr_limit_reference_11_abg_param;
+	if (n_antennas_2_param != -1)
+		priv->conf.phy.number_of_assembled_ant2_4 = n_antennas_2_param;
+	if (n_antennas_5_param != -1)
+		priv->conf.phy.number_of_assembled_ant5 = n_antennas_5_param;
+	if (dc2dc_param != -1)
+		priv->conf.phy.external_pa_dc2dc = dc2dc_param;
+
+	if (ht_mode_param) {
+		if (!strcmp(ht_mode_param, "default"))
+			priv->conf.ht.mode = HT_MODE_DEFAULT;
+		else if (!strcmp(ht_mode_param, "wide"))
+			priv->conf.ht.mode = HT_MODE_WIDE;
+		else if (!strcmp(ht_mode_param, "siso20"))
+			priv->conf.ht.mode = HT_MODE_SISO20;
+		else {
+			wl1271_error("invalid ht_mode '%s'", ht_mode_param);
+			ret = -EINVAL;
+			goto out_free;
+		}
+	}
+
+	if (priv->conf.ht.mode == HT_MODE_DEFAULT) {
+		/*
+		 * Only support mimo with multiple antennas. Fall back to
+		 * siso20.
+		 */
+		if (wl18xx_is_mimo_supported(wl))
+			wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+					  &wl18xx_mimo_ht_cap_2ghz);
+		else
+			wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+					  &wl18xx_siso20_ht_cap);
+
+		/* 5Ghz is always wide */
+		wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ,
+				  &wl18xx_siso40_ht_cap_5ghz);
+	} else if (priv->conf.ht.mode == HT_MODE_WIDE) {
+		wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+				  &wl18xx_siso40_ht_cap_2ghz);
+		wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ,
+				  &wl18xx_siso40_ht_cap_5ghz);
+	} else if (priv->conf.ht.mode == HT_MODE_SISO20) {
+		wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+				  &wl18xx_siso20_ht_cap);
+		wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ,
+				  &wl18xx_siso20_ht_cap);
+	}
+
+	if (!checksum_param) {
+		wl18xx_ops.set_rx_csum = NULL;
+		wl18xx_ops.init_vif = NULL;
+	}
+
+	wl->enable_11a = enable_11a_param;
+
+	return wlcore_probe(wl, pdev);
+
+out_free:
+	wlcore_free_hw(wl);
+out:
+	return ret;
+}
+
+static const struct platform_device_id wl18xx_id_table[] __devinitconst = {
+	{ "wl18xx", 0 },
+	{  } /* Terminating Entry */
+};
+MODULE_DEVICE_TABLE(platform, wl18xx_id_table);
+
+static struct platform_driver wl18xx_driver = {
+	.probe		= wl18xx_probe,
+	.remove		= __devexit_p(wlcore_remove),
+	.id_table	= wl18xx_id_table,
+	.driver = {
+		.name	= "wl18xx_driver",
+		.owner	= THIS_MODULE,
+	}
+};
+
+static int __init wl18xx_init(void)
+{
+	return platform_driver_register(&wl18xx_driver);
+}
+module_init(wl18xx_init);
+
+static void __exit wl18xx_exit(void)
+{
+	platform_driver_unregister(&wl18xx_driver);
+}
+module_exit(wl18xx_exit);
+
+module_param_named(ht_mode, ht_mode_param, charp, S_IRUSR);
+MODULE_PARM_DESC(ht_mode, "Force HT mode: wide or siso20");
+
+module_param_named(board_type, board_type_param, charp, S_IRUSR);
+MODULE_PARM_DESC(board_type, "Board type: fpga, hdk (default), evb, com8 or "
+		 "dvp");
+
+module_param_named(checksum, checksum_param, bool, S_IRUSR);
+MODULE_PARM_DESC(checksum, "Enable TCP checksum: boolean (defaults to false)");
+
+module_param_named(enable_11a, enable_11a_param, bool, S_IRUSR);
+MODULE_PARM_DESC(enable_11a, "Enable 11a (5GHz): boolean (defaults to true)");
+
+module_param_named(dc2dc, dc2dc_param, int, S_IRUSR);
+MODULE_PARM_DESC(dc2dc, "External DC2DC: u8 (defaults to 0)");
+
+module_param_named(n_antennas_2, n_antennas_2_param, int, S_IRUSR);
+MODULE_PARM_DESC(n_antennas_2,
+		 "Number of installed 2.4GHz antennas: 1 (default) or 2");
+
+module_param_named(n_antennas_5, n_antennas_5_param, int, S_IRUSR);
+MODULE_PARM_DESC(n_antennas_5,
+		 "Number of installed 5GHz antennas: 1 (default) or 2");
+
+module_param_named(low_band_component, low_band_component_param, int,
+		   S_IRUSR);
+MODULE_PARM_DESC(low_band_component, "Low band component: u8 "
+		 "(default is 0x01)");
+
+module_param_named(low_band_component_type, low_band_component_type_param,
+		   int, S_IRUSR);
+MODULE_PARM_DESC(low_band_component_type, "Low band component type: u8 "
+		 "(default is 0x05 or 0x06 depending on the board_type)");
+
+module_param_named(high_band_component, high_band_component_param, int,
+		   S_IRUSR);
+MODULE_PARM_DESC(high_band_component, "High band component: u8, "
+		 "(default is 0x01)");
+
+module_param_named(high_band_component_type, high_band_component_type_param,
+		   int, S_IRUSR);
+MODULE_PARM_DESC(high_band_component_type, "High band component type: u8 "
+		 "(default is 0x09)");
+
+module_param_named(pwr_limit_reference_11_abg,
+		   pwr_limit_reference_11_abg_param, int, S_IRUSR);
+MODULE_PARM_DESC(pwr_limit_reference_11_abg, "Power limit reference: u8 "
+		 "(default is 0xc8)");
+
+module_param_named(num_rx_desc,
+		   num_rx_desc_param, int, S_IRUSR);
+MODULE_PARM_DESC(num_rx_desc_param,
+		 "Number of Rx descriptors: u8 (default is 32)");
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
+MODULE_FIRMWARE(WL18XX_FW_NAME);
diff --git a/drivers/net/wireless/ti/wl18xx/reg.h b/drivers/net/wireless/ti/wl18xx/reg.h
new file mode 100644
index 00000000000..937b71d8783
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/reg.h
@@ -0,0 +1,191 @@
+/*
+ * This file is part of wlcore
+ *
+ * Copyright (C) 2011 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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
+ *
+ */
+
+#ifndef __REG_H__
+#define __REG_H__
+
+#define WL18XX_REGISTERS_BASE      0x00800000
+#define WL18XX_CODE_BASE           0x00000000
+#define WL18XX_DATA_BASE           0x00400000
+#define WL18XX_DOUBLE_BUFFER_BASE  0x00600000
+#define WL18XX_MCU_KEY_SEARCH_BASE 0x00700000
+#define WL18XX_PHY_BASE            0x00900000
+#define WL18XX_TOP_OCP_BASE        0x00A00000
+#define WL18XX_PACKET_RAM_BASE     0x00B00000
+#define WL18XX_HOST_BASE           0x00C00000
+
+#define WL18XX_REGISTERS_DOWN_SIZE 0x0000B000
+
+#define WL18XX_REG_BOOT_PART_START 0x00802000
+#define WL18XX_REG_BOOT_PART_SIZE  0x00014578
+
+#define WL18XX_PHY_INIT_MEM_ADDR   0x80926000
+
+#define WL18XX_SDIO_WSPI_BASE		(WL18XX_REGISTERS_BASE)
+#define WL18XX_REG_CONFIG_BASE		(WL18XX_REGISTERS_BASE + 0x02000)
+#define WL18XX_WGCM_REGS_BASE		(WL18XX_REGISTERS_BASE + 0x03000)
+#define WL18XX_ENC_BASE			(WL18XX_REGISTERS_BASE + 0x04000)
+#define WL18XX_INTERRUPT_BASE		(WL18XX_REGISTERS_BASE + 0x05000)
+#define WL18XX_UART_BASE		(WL18XX_REGISTERS_BASE + 0x06000)
+#define WL18XX_WELP_BASE		(WL18XX_REGISTERS_BASE + 0x07000)
+#define WL18XX_TCP_CKSM_BASE		(WL18XX_REGISTERS_BASE + 0x08000)
+#define WL18XX_FIFO_BASE		(WL18XX_REGISTERS_BASE + 0x09000)
+#define WL18XX_OCP_BRIDGE_BASE		(WL18XX_REGISTERS_BASE + 0x0A000)
+#define WL18XX_PMAC_RX_BASE		(WL18XX_REGISTERS_BASE + 0x14800)
+#define WL18XX_PMAC_ACM_BASE		(WL18XX_REGISTERS_BASE + 0x14C00)
+#define WL18XX_PMAC_TX_BASE		(WL18XX_REGISTERS_BASE + 0x15000)
+#define WL18XX_PMAC_CSR_BASE		(WL18XX_REGISTERS_BASE + 0x15400)
+
+#define WL18XX_REG_ECPU_CONTROL		(WL18XX_REGISTERS_BASE + 0x02004)
+#define WL18XX_REG_INTERRUPT_NO_CLEAR	(WL18XX_REGISTERS_BASE + 0x050E8)
+#define WL18XX_REG_INTERRUPT_ACK	(WL18XX_REGISTERS_BASE + 0x050F0)
+#define WL18XX_REG_INTERRUPT_TRIG	(WL18XX_REGISTERS_BASE + 0x5074)
+#define WL18XX_REG_INTERRUPT_TRIG_H	(WL18XX_REGISTERS_BASE + 0x5078)
+#define WL18XX_REG_INTERRUPT_MASK	(WL18XX_REGISTERS_BASE + 0x0050DC)
+
+#define WL18XX_REG_CHIP_ID_B		(WL18XX_REGISTERS_BASE + 0x01542C)
+
+#define WL18XX_SLV_MEM_DATA		(WL18XX_HOST_BASE + 0x0018)
+#define WL18XX_SLV_REG_DATA		(WL18XX_HOST_BASE + 0x0008)
+
+/* Scratch Pad registers*/
+#define WL18XX_SCR_PAD0			(WL18XX_REGISTERS_BASE + 0x0154EC)
+#define WL18XX_SCR_PAD1			(WL18XX_REGISTERS_BASE + 0x0154F0)
+#define WL18XX_SCR_PAD2			(WL18XX_REGISTERS_BASE + 0x0154F4)
+#define WL18XX_SCR_PAD3			(WL18XX_REGISTERS_BASE + 0x0154F8)
+#define WL18XX_SCR_PAD4			(WL18XX_REGISTERS_BASE + 0x0154FC)
+#define WL18XX_SCR_PAD4_SET		(WL18XX_REGISTERS_BASE + 0x015504)
+#define WL18XX_SCR_PAD4_CLR		(WL18XX_REGISTERS_BASE + 0x015500)
+#define WL18XX_SCR_PAD5			(WL18XX_REGISTERS_BASE + 0x015508)
+#define WL18XX_SCR_PAD5_SET		(WL18XX_REGISTERS_BASE + 0x015510)
+#define WL18XX_SCR_PAD5_CLR		(WL18XX_REGISTERS_BASE + 0x01550C)
+#define WL18XX_SCR_PAD6			(WL18XX_REGISTERS_BASE + 0x015514)
+#define WL18XX_SCR_PAD7			(WL18XX_REGISTERS_BASE + 0x015518)
+#define WL18XX_SCR_PAD8			(WL18XX_REGISTERS_BASE + 0x01551C)
+#define WL18XX_SCR_PAD9			(WL18XX_REGISTERS_BASE + 0x015520)
+
+/* Spare registers*/
+#define WL18XX_SPARE_A1			(WL18XX_REGISTERS_BASE + 0x002194)
+#define WL18XX_SPARE_A2			(WL18XX_REGISTERS_BASE + 0x002198)
+#define WL18XX_SPARE_A3			(WL18XX_REGISTERS_BASE + 0x00219C)
+#define WL18XX_SPARE_A4			(WL18XX_REGISTERS_BASE + 0x0021A0)
+#define WL18XX_SPARE_A5			(WL18XX_REGISTERS_BASE + 0x0021A4)
+#define WL18XX_SPARE_A6			(WL18XX_REGISTERS_BASE + 0x0021A8)
+#define WL18XX_SPARE_A7			(WL18XX_REGISTERS_BASE + 0x0021AC)
+#define WL18XX_SPARE_A8			(WL18XX_REGISTERS_BASE + 0x0021B0)
+#define WL18XX_SPARE_B1			(WL18XX_REGISTERS_BASE + 0x015524)
+#define WL18XX_SPARE_B2			(WL18XX_REGISTERS_BASE + 0x015528)
+#define WL18XX_SPARE_B3			(WL18XX_REGISTERS_BASE + 0x01552C)
+#define WL18XX_SPARE_B4			(WL18XX_REGISTERS_BASE + 0x015530)
+#define WL18XX_SPARE_B5			(WL18XX_REGISTERS_BASE + 0x015534)
+#define WL18XX_SPARE_B6			(WL18XX_REGISTERS_BASE + 0x015538)
+#define WL18XX_SPARE_B7			(WL18XX_REGISTERS_BASE + 0x01553C)
+#define WL18XX_SPARE_B8			(WL18XX_REGISTERS_BASE + 0x015540)
+
+#define WL18XX_REG_COMMAND_MAILBOX_PTR	(WL18XX_SCR_PAD0)
+#define WL18XX_REG_EVENT_MAILBOX_PTR	(WL18XX_SCR_PAD1)
+#define WL18XX_EEPROMLESS_IND		(WL18XX_SCR_PAD4)
+
+#define WL18XX_WELP_ARM_COMMAND		(WL18XX_REGISTERS_BASE + 0x7100)
+#define WL18XX_ENABLE			(WL18XX_REGISTERS_BASE + 0x01543C)
+
+/* PRCM registers */
+#define PLATFORM_DETECTION		0xA0E3E0
+#define OCS_EN				0xA02080
+#define PRIMARY_CLK_DETECT		0xA020A6
+#define PLLSH_WCS_PLL_N			0xA02362
+#define PLLSH_WCS_PLL_M			0xA02360
+#define PLLSH_WCS_PLL_Q_FACTOR_CFG_1	0xA02364
+#define PLLSH_WCS_PLL_Q_FACTOR_CFG_2	0xA02366
+#define PLLSH_WCS_PLL_P_FACTOR_CFG_1	0xA02368
+#define PLLSH_WCS_PLL_P_FACTOR_CFG_2	0xA0236A
+#define PLLSH_WCS_PLL_SWALLOW_EN	0xA0236C
+#define PLLSH_WL_PLL_EN			0xA02392
+
+#define PLLSH_WCS_PLL_Q_FACTOR_CFG_1_MASK	0xFFFF
+#define PLLSH_WCS_PLL_Q_FACTOR_CFG_2_MASK	0x007F
+#define PLLSH_WCS_PLL_P_FACTOR_CFG_1_MASK	0xFFFF
+#define PLLSH_WCS_PLL_P_FACTOR_CFG_2_MASK	0x000F
+
+#define PLLSH_WCS_PLL_SWALLOW_EN_VAL1	0x1
+#define PLLSH_WCS_PLL_SWALLOW_EN_VAL2	0x12
+
+#define WL18XX_REG_FUSE_DATA_1_3	0xA0260C
+#define WL18XX_PG_VER_MASK		0x70
+#define WL18XX_PG_VER_OFFSET		4
+
+#define WL18XX_REG_FUSE_BD_ADDR_1	0xA02602
+#define WL18XX_REG_FUSE_BD_ADDR_2	0xA02606
+
+#define WL18XX_CMD_MBOX_ADDRESS		0xB007B4
+
+#define WL18XX_FW_STATUS_ADDR		0x50F8
+
+#define CHIP_ID_185x_PG10              (0x06030101)
+#define CHIP_ID_185x_PG20              (0x06030111)
+
+/*
+ * Host Command Interrupt. Setting this bit masks
+ * the interrupt that the host issues to inform
+ * the FW that it has sent a command
+ * to the Wlan hardware Command Mailbox.
+ */
+#define WL18XX_INTR_TRIG_CMD       BIT(28)
+
+/*
+ * Host Event Acknowlegde Interrupt. The host
+ * sets this bit to acknowledge that it received
+ * the unsolicited information from the event
+ * mailbox.
+ */
+#define WL18XX_INTR_TRIG_EVENT_ACK BIT(29)
+
+/*
+ * To boot the firmware in PLT mode we need to write this value in
+ * SCR_PAD8 before starting.
+ */
+#define WL18XX_SCR_PAD8_PLT	0xBABABEBE
+
+enum {
+	COMPONENT_NO_SWITCH	= 0x0,
+	COMPONENT_2_WAY_SWITCH	= 0x1,
+	COMPONENT_3_WAY_SWITCH	= 0x2,
+	COMPONENT_MATCHING	= 0x3,
+};
+
+enum {
+	FEM_NONE	= 0x0,
+	FEM_VENDOR_1	= 0x1,
+	FEM_VENDOR_2	= 0x2,
+	FEM_VENDOR_3	= 0x3,
+};
+
+enum {
+	BOARD_TYPE_EVB_18XX     = 0,
+	BOARD_TYPE_DVP_18XX     = 1,
+	BOARD_TYPE_HDK_18XX     = 2,
+	BOARD_TYPE_FPGA_18XX    = 3,
+	BOARD_TYPE_COM8_18XX    = 4,
+
+	NUM_BOARD_TYPES,
+};
+
+#endif /* __REG_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/tx.c b/drivers/net/wireless/ti/wl18xx/tx.c
new file mode 100644
index 00000000000..5b1fb10d9fd
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/tx.c
@@ -0,0 +1,127 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2011 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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 "../wlcore/wlcore.h"
+#include "../wlcore/cmd.h"
+#include "../wlcore/debug.h"
+#include "../wlcore/acx.h"
+#include "../wlcore/tx.h"
+
+#include "wl18xx.h"
+#include "tx.h"
+
+static void wl18xx_tx_complete_packet(struct wl1271 *wl, u8 tx_stat_byte)
+{
+	struct ieee80211_tx_info *info;
+	struct sk_buff *skb;
+	int id = tx_stat_byte & WL18XX_TX_STATUS_DESC_ID_MASK;
+	bool tx_success;
+
+	/* check for id legality */
+	if (unlikely(id >= wl->num_tx_desc || wl->tx_frames[id] == NULL)) {
+		wl1271_warning("illegal id in tx completion: %d", id);
+		return;
+	}
+
+	/* a zero bit indicates Tx success */
+	tx_success = !(tx_stat_byte & BIT(WL18XX_TX_STATUS_STAT_BIT_IDX));
+
+
+	skb = wl->tx_frames[id];
+	info = IEEE80211_SKB_CB(skb);
+
+	if (wl12xx_is_dummy_packet(wl, skb)) {
+		wl1271_free_tx_id(wl, id);
+		return;
+	}
+
+	/* update the TX status info */
+	if (tx_success && !(info->flags & IEEE80211_TX_CTL_NO_ACK))
+		info->flags |= IEEE80211_TX_STAT_ACK;
+
+	/* no real data about Tx completion */
+	info->status.rates[0].idx = -1;
+	info->status.rates[0].count = 0;
+	info->status.rates[0].flags = 0;
+	info->status.ack_signal = -1;
+
+	if (!tx_success)
+		wl->stats.retry_count++;
+
+	/*
+	 * TODO: update sequence number for encryption? seems to be
+	 * unsupported for now. needed for recovery with encryption.
+	 */
+
+	/* remove private header from packet */
+	skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
+
+	/* remove TKIP header space if present */
+	if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
+	    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_EXTRA_SPACE_TKIP, skb->data, hdrlen);
+		skb_pull(skb, WL1271_EXTRA_SPACE_TKIP);
+	}
+
+	wl1271_debug(DEBUG_TX, "tx status id %u skb 0x%p success %d",
+		     id, skb, tx_success);
+
+	/* return the packet to the stack */
+	skb_queue_tail(&wl->deferred_tx_queue, skb);
+	queue_work(wl->freezable_wq, &wl->netstack_work);
+	wl1271_free_tx_id(wl, id);
+}
+
+void wl18xx_tx_immediate_complete(struct wl1271 *wl)
+{
+	struct wl18xx_fw_status_priv *status_priv =
+		(struct wl18xx_fw_status_priv *)wl->fw_status_2->priv;
+	struct wl18xx_priv *priv = wl->priv;
+	u8 i;
+
+	/* nothing to do here */
+	if (priv->last_fw_rls_idx == status_priv->fw_release_idx)
+		return;
+
+	/* freed Tx descriptors */
+	wl1271_debug(DEBUG_TX, "last released desc = %d, current idx = %d",
+		     priv->last_fw_rls_idx, status_priv->fw_release_idx);
+
+	if (status_priv->fw_release_idx >= WL18XX_FW_MAX_TX_STATUS_DESC) {
+		wl1271_error("invalid desc release index %d",
+			     status_priv->fw_release_idx);
+		WARN_ON(1);
+		return;
+	}
+
+	for (i = priv->last_fw_rls_idx;
+	     i != status_priv->fw_release_idx;
+	     i = (i + 1) % WL18XX_FW_MAX_TX_STATUS_DESC) {
+		wl18xx_tx_complete_packet(wl,
+			status_priv->released_tx_desc[i]);
+
+		wl->tx_results_count++;
+	}
+
+	priv->last_fw_rls_idx = status_priv->fw_release_idx;
+}
diff --git a/drivers/net/wireless/ti/wl18xx/tx.h b/drivers/net/wireless/ti/wl18xx/tx.h
new file mode 100644
index 00000000000..ccddc548e44
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/tx.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2011 Texas Instruments. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef __WL18XX_TX_H__
+#define __WL18XX_TX_H__
+
+#include "../wlcore/wlcore.h"
+
+#define WL18XX_TX_HW_BLOCK_SPARE        1
+/* for special cases - namely, TKIP and GEM */
+#define WL18XX_TX_HW_EXTRA_BLOCK_SPARE  2
+#define WL18XX_TX_HW_BLOCK_SIZE         268
+
+#define WL18XX_TX_STATUS_DESC_ID_MASK    0x7F
+#define WL18XX_TX_STATUS_STAT_BIT_IDX    7
+
+/* Indicates this TX HW frame is not padded to SDIO block size */
+#define WL18XX_TX_CTRL_NOT_PADDED	BIT(7)
+
+/*
+ * The FW uses a special bit to indicate a wide channel should be used in
+ * the rate policy.
+ */
+#define CONF_TX_RATE_USE_WIDE_CHAN BIT(31)
+
+void wl18xx_tx_immediate_complete(struct wl1271 *wl);
+
+#endif /* __WL12XX_TX_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/wl18xx.h b/drivers/net/wireless/ti/wl18xx/wl18xx.h
new file mode 100644
index 00000000000..6452396fa1d
--- /dev/null
+++ b/drivers/net/wireless/ti/wl18xx/wl18xx.h
@@ -0,0 +1,95 @@
+/*
+ * This file is part of wl18xx
+ *
+ * Copyright (C) 2011 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You 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
+ *
+ */
+
+#ifndef __WL18XX_PRIV_H__
+#define __WL18XX_PRIV_H__
+
+#include "conf.h"
+
+/* minimum FW required for driver */
+#define WL18XX_CHIP_VER		8
+#define WL18XX_IFTYPE_VER	2
+#define WL18XX_MAJOR_VER	0
+#define WL18XX_SUBTYPE_VER	0
+#define WL18XX_MINOR_VER	100
+
+#define WL18XX_CMD_MAX_SIZE          740
+
+struct wl18xx_priv {
+	/* buffer for sending commands to FW */
+	u8 cmd_buf[WL18XX_CMD_MAX_SIZE];
+
+	struct wl18xx_priv_conf conf;
+
+	/* Index of last released Tx desc in FW */
+	u8 last_fw_rls_idx;
+
+	/* number of VIFs requiring extra spare mem-blocks */
+	int extra_spare_vif_count;
+};
+
+#define WL18XX_FW_MAX_TX_STATUS_DESC 33
+
+struct wl18xx_fw_status_priv {
+	/*
+	 * Index in released_tx_desc for first byte that holds
+	 * released tx host desc
+	 */
+	u8 fw_release_idx;
+
+	/*
+	 * Array of host Tx descriptors, where fw_release_idx
+	 * indicated the first released idx.
+	 */
+	u8 released_tx_desc[WL18XX_FW_MAX_TX_STATUS_DESC];
+
+	u8 padding[2];
+};
+
+#define WL18XX_PHY_VERSION_MAX_LEN 20
+
+struct wl18xx_static_data_priv {
+	char phy_version[WL18XX_PHY_VERSION_MAX_LEN];
+};
+
+struct wl18xx_clk_cfg {
+	u32 n;
+	u32 m;
+	u32 p;
+	u32 q;
+	bool swallow;
+};
+
+enum {
+	CLOCK_CONFIG_16_2_M	= 1,
+	CLOCK_CONFIG_16_368_M,
+	CLOCK_CONFIG_16_8_M,
+	CLOCK_CONFIG_19_2_M,
+	CLOCK_CONFIG_26_M,
+	CLOCK_CONFIG_32_736_M,
+	CLOCK_CONFIG_33_6_M,
+	CLOCK_CONFIG_38_468_M,
+	CLOCK_CONFIG_52_M,
+
+	NUM_CLOCK_CONFIGS,
+};
+
+#endif /* __WL18XX_PRIV_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index f3d6fa50826..ce108a736bd 100644
--- a/drivers/net/wireless/ti/wlcore/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -70,7 +70,7 @@ int wl1271_acx_sleep_auth(struct wl1271 *wl, u8 sleep_auth)
 	struct acx_sleep_auth *auth;
 	int ret;
 
-	wl1271_debug(DEBUG_ACX, "acx sleep auth");
+	wl1271_debug(DEBUG_ACX, "acx sleep auth %d", sleep_auth);
 
 	auth = kzalloc(sizeof(*auth), GFP_KERNEL);
 	if (!auth) {
@@ -81,11 +81,18 @@ int wl1271_acx_sleep_auth(struct wl1271 *wl, u8 sleep_auth)
 	auth->sleep_auth = sleep_auth;
 
 	ret = wl1271_cmd_configure(wl, ACX_SLEEP_AUTH, auth, sizeof(*auth));
+	if (ret < 0) {
+		wl1271_error("could not configure sleep_auth to %d: %d",
+			     sleep_auth, ret);
+		goto out;
+	}
 
+	wl->sleep_auth = sleep_auth;
 out:
 	kfree(auth);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wl1271_acx_sleep_auth);
 
 int wl1271_acx_tx_power(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			int power)
@@ -708,14 +715,14 @@ out:
 	return ret;
 }
 
-int wl1271_acx_statistics(struct wl1271 *wl, struct acx_statistics *stats)
+int wl1271_acx_statistics(struct wl1271 *wl, void *stats)
 {
 	int ret;
 
 	wl1271_debug(DEBUG_ACX, "acx statistics");
 
 	ret = wl1271_cmd_interrogate(wl, ACX_STATISTICS, stats,
-				     sizeof(*stats));
+				     wl->stats.fw_stats_len);
 	if (ret < 0) {
 		wl1271_warning("acx statistics failed: %d", ret);
 		return -ENOMEM;
@@ -997,6 +1004,7 @@ out:
 	kfree(mem_conf);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wl12xx_acx_mem_cfg);
 
 int wl1271_acx_init_mem_config(struct wl1271 *wl)
 {
@@ -1027,6 +1035,7 @@ int wl1271_acx_init_mem_config(struct wl1271 *wl)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(wl1271_acx_init_mem_config);
 
 int wl1271_acx_init_rx_interrupt(struct wl1271 *wl)
 {
@@ -1150,6 +1159,7 @@ out:
 	kfree(acx);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wl1271_acx_pm_config);
 
 int wl1271_acx_keep_alive_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			       bool enable)
diff --git a/drivers/net/wireless/ti/wlcore/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index e6a74869a5f..d03215d6b3b 100644
--- a/drivers/net/wireless/ti/wlcore/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -51,21 +51,18 @@
 #define WL1271_ACX_INTR_TRACE_A            BIT(7)
 /* Trace message on MBOX #B */
 #define WL1271_ACX_INTR_TRACE_B            BIT(8)
+/* SW FW Initiated interrupt Watchdog timer expiration */
+#define WL1271_ACX_SW_INTR_WATCHDOG        BIT(9)
 
-#define WL1271_ACX_INTR_ALL		   0xFFFFFFFF
-#define WL1271_ACX_ALL_EVENTS_VECTOR       (WL1271_ACX_INTR_WATCHDOG      | \
-					    WL1271_ACX_INTR_INIT_COMPLETE | \
-					    WL1271_ACX_INTR_EVENT_A       | \
-					    WL1271_ACX_INTR_EVENT_B       | \
-					    WL1271_ACX_INTR_CMD_COMPLETE  | \
-					    WL1271_ACX_INTR_HW_AVAILABLE  | \
-					    WL1271_ACX_INTR_DATA)
-
-#define WL1271_INTR_MASK                   (WL1271_ACX_INTR_WATCHDOG     | \
-					    WL1271_ACX_INTR_EVENT_A      | \
-					    WL1271_ACX_INTR_EVENT_B      | \
-					    WL1271_ACX_INTR_HW_AVAILABLE | \
-					    WL1271_ACX_INTR_DATA)
+#define WL1271_ACX_INTR_ALL             0xFFFFFFFF
+
+/* all possible interrupts - only appropriate ones will be masked in */
+#define WLCORE_ALL_INTR_MASK		(WL1271_ACX_INTR_WATCHDOG     | \
+					WL1271_ACX_INTR_EVENT_A       | \
+					WL1271_ACX_INTR_EVENT_B       | \
+					WL1271_ACX_INTR_HW_AVAILABLE  | \
+					WL1271_ACX_INTR_DATA          | \
+					WL1271_ACX_SW_INTR_WATCHDOG)
 
 /* Target's information element */
 struct acx_header {
@@ -121,6 +118,11 @@ enum wl1271_psm_mode {
 
 	/* Extreme low power */
 	WL1271_PSM_ELP = 2,
+
+	WL1271_PSM_MAX = WL1271_PSM_ELP,
+
+	/* illegal out of band value of PSM mode */
+	WL1271_PSM_ILLEGAL = 0xff
 };
 
 struct acx_sleep_auth {
@@ -417,228 +419,6 @@ struct acx_ctsprotect {
 	u8 padding[2];
 } __packed;
 
-struct acx_tx_statistics {
-	__le32 internal_desc_overflow;
-}  __packed;
-
-struct acx_rx_statistics {
-	__le32 out_of_mem;
-	__le32 hdr_overflow;
-	__le32 hw_stuck;
-	__le32 dropped;
-	__le32 fcs_err;
-	__le32 xfr_hint_trig;
-	__le32 path_reset;
-	__le32 reset_counter;
-} __packed;
-
-struct acx_dma_statistics {
-	__le32 rx_requested;
-	__le32 rx_errors;
-	__le32 tx_requested;
-	__le32 tx_errors;
-}  __packed;
-
-struct acx_isr_statistics {
-	/* host command complete */
-	__le32 cmd_cmplt;
-
-	/* fiqisr() */
-	__le32 fiqs;
-
-	/* (INT_STS_ND & INT_TRIG_RX_HEADER) */
-	__le32 rx_headers;
-
-	/* (INT_STS_ND & INT_TRIG_RX_CMPLT) */
-	__le32 rx_completes;
-
-	/* (INT_STS_ND & INT_TRIG_NO_RX_BUF) */
-	__le32 rx_mem_overflow;
-
-	/* (INT_STS_ND & INT_TRIG_S_RX_RDY) */
-	__le32 rx_rdys;
-
-	/* irqisr() */
-	__le32 irqs;
-
-	/* (INT_STS_ND & INT_TRIG_TX_PROC) */
-	__le32 tx_procs;
-
-	/* (INT_STS_ND & INT_TRIG_DECRYPT_DONE) */
-	__le32 decrypt_done;
-
-	/* (INT_STS_ND & INT_TRIG_DMA0) */
-	__le32 dma0_done;
-
-	/* (INT_STS_ND & INT_TRIG_DMA1) */
-	__le32 dma1_done;
-
-	/* (INT_STS_ND & INT_TRIG_TX_EXC_CMPLT) */
-	__le32 tx_exch_complete;
-
-	/* (INT_STS_ND & INT_TRIG_COMMAND) */
-	__le32 commands;
-
-	/* (INT_STS_ND & INT_TRIG_RX_PROC) */
-	__le32 rx_procs;
-
-	/* (INT_STS_ND & INT_TRIG_PM_802) */
-	__le32 hw_pm_mode_changes;
-
-	/* (INT_STS_ND & INT_TRIG_ACKNOWLEDGE) */
-	__le32 host_acknowledges;
-
-	/* (INT_STS_ND & INT_TRIG_PM_PCI) */
-	__le32 pci_pm;
-
-	/* (INT_STS_ND & INT_TRIG_ACM_WAKEUP) */
-	__le32 wakeups;
-
-	/* (INT_STS_ND & INT_TRIG_LOW_RSSI) */
-	__le32 low_rssi;
-} __packed;
-
-struct acx_wep_statistics {
-	/* WEP address keys configured */
-	__le32 addr_key_count;
-
-	/* default keys configured */
-	__le32 default_key_count;
-
-	__le32 reserved;
-
-	/* number of times that WEP key not found on lookup */
-	__le32 key_not_found;
-
-	/* number of times that WEP key decryption failed */
-	__le32 decrypt_fail;
-
-	/* WEP packets decrypted */
-	__le32 packets;
-
-	/* WEP decrypt interrupts */
-	__le32 interrupt;
-} __packed;
-
-#define ACX_MISSED_BEACONS_SPREAD 10
-
-struct acx_pwr_statistics {
-	/* the amount of enters into power save mode (both PD & ELP) */
-	__le32 ps_enter;
-
-	/* the amount of enters into ELP mode */
-	__le32 elp_enter;
-
-	/* the amount of missing beacon interrupts to the host */
-	__le32 missing_bcns;
-
-	/* the amount of wake on host-access times */
-	__le32 wake_on_host;
-
-	/* the amount of wake on timer-expire */
-	__le32 wake_on_timer_exp;
-
-	/* the number of packets that were transmitted with PS bit set */
-	__le32 tx_with_ps;
-
-	/* the number of packets that were transmitted with PS bit clear */
-	__le32 tx_without_ps;
-
-	/* the number of received beacons */
-	__le32 rcvd_beacons;
-
-	/* the number of entering into PowerOn (power save off) */
-	__le32 power_save_off;
-
-	/* the number of entries into power save mode */
-	__le16 enable_ps;
-
-	/*
-	 * the number of exits from power save, not including failed PS
-	 * transitions
-	 */
-	__le16 disable_ps;
-
-	/*
-	 * the number of times the TSF counter was adjusted because
-	 * of drift
-	 */
-	__le32 fix_tsf_ps;
-
-	/* Gives statistics about the spread continuous missed beacons.
-	 * The 16 LSB are dedicated for the PS mode.
-	 * The 16 MSB are dedicated for the PS mode.
-	 * cont_miss_bcns_spread[0] - single missed beacon.
-	 * cont_miss_bcns_spread[1] - two continuous missed beacons.
-	 * cont_miss_bcns_spread[2] - three continuous missed beacons.
-	 * ...
-	 * cont_miss_bcns_spread[9] - ten and more continuous missed beacons.
-	*/
-	__le32 cont_miss_bcns_spread[ACX_MISSED_BEACONS_SPREAD];
-
-	/* the number of beacons in awake mode */
-	__le32 rcvd_awake_beacons;
-} __packed;
-
-struct acx_mic_statistics {
-	__le32 rx_pkts;
-	__le32 calc_failure;
-} __packed;
-
-struct acx_aes_statistics {
-	__le32 encrypt_fail;
-	__le32 decrypt_fail;
-	__le32 encrypt_packets;
-	__le32 decrypt_packets;
-	__le32 encrypt_interrupt;
-	__le32 decrypt_interrupt;
-} __packed;
-
-struct acx_event_statistics {
-	__le32 heart_beat;
-	__le32 calibration;
-	__le32 rx_mismatch;
-	__le32 rx_mem_empty;
-	__le32 rx_pool;
-	__le32 oom_late;
-	__le32 phy_transmit_error;
-	__le32 tx_stuck;
-} __packed;
-
-struct acx_ps_statistics {
-	__le32 pspoll_timeouts;
-	__le32 upsd_timeouts;
-	__le32 upsd_max_sptime;
-	__le32 upsd_max_apturn;
-	__le32 pspoll_max_apturn;
-	__le32 pspoll_utilization;
-	__le32 upsd_utilization;
-} __packed;
-
-struct acx_rxpipe_statistics {
-	__le32 rx_prep_beacon_drop;
-	__le32 descr_host_int_trig_rx_data;
-	__le32 beacon_buffer_thres_host_int_trig_rx_data;
-	__le32 missed_beacon_host_int_trig_rx_data;
-	__le32 tx_xfr_host_int_trig_rx_data;
-} __packed;
-
-struct acx_statistics {
-	struct acx_header header;
-
-	struct acx_tx_statistics tx;
-	struct acx_rx_statistics rx;
-	struct acx_dma_statistics dma;
-	struct acx_isr_statistics isr;
-	struct acx_wep_statistics wep;
-	struct acx_pwr_statistics pwr;
-	struct acx_aes_statistics aes;
-	struct acx_mic_statistics mic;
-	struct acx_event_statistics event;
-	struct acx_ps_statistics ps;
-	struct acx_rxpipe_statistics rxpipe;
-} __packed;
-
 struct acx_rate_class {
 	__le32 enabled_rates;
 	u8 short_retry_limit;
@@ -828,6 +608,8 @@ struct wl1271_acx_keep_alive_config {
 #define HOST_IF_CFG_RX_FIFO_ENABLE     BIT(0)
 #define HOST_IF_CFG_TX_EXTRA_BLKS_SWAP BIT(1)
 #define HOST_IF_CFG_TX_PAD_TO_SDIO_BLK BIT(3)
+#define HOST_IF_CFG_RX_PAD_TO_SDIO_BLK BIT(4)
+#define HOST_IF_CFG_ADD_RX_ALIGNMENT   BIT(6)
 
 enum {
 	WL1271_ACX_TRIG_TYPE_LEVEL = 0,
@@ -946,7 +728,7 @@ struct wl1271_acx_ht_information {
 	u8 padding[2];
 } __packed;
 
-#define RX_BA_MAX_SESSIONS 2
+#define RX_BA_MAX_SESSIONS 3
 
 struct wl1271_acx_ba_initiator_policy {
 	struct acx_header header;
@@ -1243,6 +1025,7 @@ enum {
 	ACX_CONFIG_HANGOVER              = 0x0042,
 	ACX_FEATURE_CFG                  = 0x0043,
 	ACX_PROTECTION_CFG               = 0x0044,
+	ACX_CHECKSUM_CONFIG              = 0x0045,
 };
 
 
@@ -1281,7 +1064,7 @@ int wl1271_acx_set_preamble(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			    enum acx_preamble_type preamble);
 int wl1271_acx_cts_protect(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			   enum acx_ctsprotect_type ctsprotect);
-int wl1271_acx_statistics(struct wl1271 *wl, struct acx_statistics *stats);
+int wl1271_acx_statistics(struct wl1271 *wl, void *stats);
 int wl1271_acx_sta_rate_policies(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 int wl1271_acx_ap_rate_policy(struct wl1271 *wl, struct conf_tx_rate_class *c,
 		      u8 idx);
diff --git a/drivers/net/wireless/ti/wlcore/boot.c b/drivers/net/wireless/ti/wlcore/boot.c
index 9b98230f84c..375ea574eaf 100644
--- a/drivers/net/wireless/ti/wlcore/boot.c
+++ b/drivers/net/wireless/ti/wlcore/boot.c
@@ -33,22 +33,35 @@
 #include "rx.h"
 #include "hw_ops.h"
 
-static void wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
+static int wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
 {
 	u32 cpu_ctrl;
+	int ret;
 
 	/* 10.5.0 run the firmware (I) */
-	cpu_ctrl = wlcore_read_reg(wl, REG_ECPU_CONTROL);
+	ret = wlcore_read_reg(wl, REG_ECPU_CONTROL, &cpu_ctrl);
+	if (ret < 0)
+		goto out;
 
 	/* 10.5.1 run the firmware (II) */
 	cpu_ctrl |= flag;
-	wlcore_write_reg(wl, REG_ECPU_CONTROL, cpu_ctrl);
+	ret = wlcore_write_reg(wl, REG_ECPU_CONTROL, cpu_ctrl);
+
+out:
+	return ret;
 }
 
-static int wlcore_parse_fw_ver(struct wl1271 *wl)
+static int wlcore_boot_parse_fw_ver(struct wl1271 *wl,
+				    struct wl1271_static_data *static_data)
 {
 	int ret;
 
+	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_str[sizeof(wl->chip.fw_ver_str) - 1] = '\0';
+
 	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],
@@ -57,43 +70,96 @@ static int wlcore_parse_fw_ver(struct wl1271 *wl)
 	if (ret != 5) {
 		wl1271_warning("fw version incorrect value");
 		memset(wl->chip.fw_ver, 0, sizeof(wl->chip.fw_ver));
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	ret = wlcore_identify_fw(wl);
 	if (ret < 0)
-		return ret;
+		goto out;
+out:
+	return ret;
+}
+
+static int wlcore_validate_fw_ver(struct wl1271 *wl)
+{
+	unsigned int *fw_ver = wl->chip.fw_ver;
+	unsigned int *min_ver = wl->min_fw_ver;
 
+	/* the chip must be exactly equal */
+	if (min_ver[FW_VER_CHIP] != fw_ver[FW_VER_CHIP])
+		goto fail;
+
+	/* always check the next digit if all previous ones are equal */
+
+	if (min_ver[FW_VER_IF_TYPE] < fw_ver[FW_VER_IF_TYPE])
+		goto out;
+	else if (min_ver[FW_VER_IF_TYPE] > fw_ver[FW_VER_IF_TYPE])
+		goto fail;
+
+	if (min_ver[FW_VER_MAJOR] < fw_ver[FW_VER_MAJOR])
+		goto out;
+	else if (min_ver[FW_VER_MAJOR] > fw_ver[FW_VER_MAJOR])
+		goto fail;
+
+	if (min_ver[FW_VER_SUBTYPE] < fw_ver[FW_VER_SUBTYPE])
+		goto out;
+	else if (min_ver[FW_VER_SUBTYPE] > fw_ver[FW_VER_SUBTYPE])
+		goto fail;
+
+	if (min_ver[FW_VER_MINOR] < fw_ver[FW_VER_MINOR])
+		goto out;
+	else if (min_ver[FW_VER_MINOR] > fw_ver[FW_VER_MINOR])
+		goto fail;
+
+out:
 	return 0;
+
+fail:
+	wl1271_error("Your WiFi FW version (%u.%u.%u.%u.%u) is outdated.\n"
+		     "Please use at least FW %u.%u.%u.%u.%u.\n"
+		     "You can get more information at:\n"
+		     "http://wireless.kernel.org/en/users/Drivers/wl12xx",
+		     fw_ver[FW_VER_CHIP], fw_ver[FW_VER_IF_TYPE],
+		     fw_ver[FW_VER_MAJOR], fw_ver[FW_VER_SUBTYPE],
+		     fw_ver[FW_VER_MINOR], min_ver[FW_VER_CHIP],
+		     min_ver[FW_VER_IF_TYPE], min_ver[FW_VER_MAJOR],
+		     min_ver[FW_VER_SUBTYPE], min_ver[FW_VER_MINOR]);
+	return -EINVAL;
 }
 
-static int wlcore_boot_fw_version(struct wl1271 *wl)
+static int wlcore_boot_static_data(struct wl1271 *wl)
 {
 	struct wl1271_static_data *static_data;
+	size_t len = sizeof(*static_data) + wl->static_data_priv_len;
 	int ret;
 
-	static_data = kmalloc(sizeof(*static_data), GFP_KERNEL | GFP_DMA);
+	static_data = kmalloc(len, GFP_KERNEL);
 	if (!static_data) {
-		wl1271_error("Couldn't allocate memory for static data!");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto out;
 	}
 
-	wl1271_read(wl, wl->cmd_box_addr, static_data, sizeof(*static_data),
-		    false);
-
-	strncpy(wl->chip.fw_ver_str, static_data->fw_version,
-		sizeof(wl->chip.fw_ver_str));
+	ret = wlcore_read(wl, wl->cmd_box_addr, static_data, len, false);
+	if (ret < 0)
+		goto out_free;
 
-	kfree(static_data);
+	ret = wlcore_boot_parse_fw_ver(wl, static_data);
+	if (ret < 0)
+		goto out_free;
 
-	/* make sure the string is NULL-terminated */
-	wl->chip.fw_ver_str[sizeof(wl->chip.fw_ver_str) - 1] = '\0';
+	ret = wlcore_validate_fw_ver(wl);
+	if (ret < 0)
+		goto out_free;
 
-	ret = wlcore_parse_fw_ver(wl);
+	ret = wlcore_handle_static_data(wl, static_data);
 	if (ret < 0)
-		return ret;
+		goto out_free;
 
-	return 0;
+out_free:
+	kfree(static_data);
+out:
+	return ret;
 }
 
 static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
@@ -102,6 +168,7 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
 	struct wlcore_partition_set partition;
 	int addr, chunk_num, partition_limit;
 	u8 *p, *chunk;
+	int ret;
 
 	/* whal_FwCtrl_LoadFwImageSm() */
 
@@ -123,7 +190,9 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
 
 	memcpy(&partition, &wl->ptable[PART_DOWN], sizeof(partition));
 	partition.mem.start = dest;
-	wlcore_set_partition(wl, &partition);
+	ret = wlcore_set_partition(wl, &partition);
+	if (ret < 0)
+		goto out;
 
 	/* 10.1 set partition limit and chunk num */
 	chunk_num = 0;
@@ -137,7 +206,9 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
 			partition_limit = chunk_num * CHUNK_SIZE +
 				wl->ptable[PART_DOWN].mem.size;
 			partition.mem.start = addr;
-			wlcore_set_partition(wl, &partition);
+			ret = wlcore_set_partition(wl, &partition);
+			if (ret < 0)
+				goto out;
 		}
 
 		/* 10.3 upload the chunk */
@@ -146,7 +217,9 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
 		memcpy(chunk, p, CHUNK_SIZE);
 		wl1271_debug(DEBUG_BOOT, "uploading fw chunk 0x%p to 0x%x",
 			     p, addr);
-		wl1271_write(wl, addr, chunk, CHUNK_SIZE, false);
+		ret = wlcore_write(wl, addr, chunk, CHUNK_SIZE, false);
+		if (ret < 0)
+			goto out;
 
 		chunk_num++;
 	}
@@ -157,10 +230,11 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
 	memcpy(chunk, p, fw_data_len % CHUNK_SIZE);
 	wl1271_debug(DEBUG_BOOT, "uploading fw last chunk (%zd B) 0x%p to 0x%x",
 		     fw_data_len % CHUNK_SIZE, p, addr);
-	wl1271_write(wl, addr, chunk, fw_data_len % CHUNK_SIZE, false);
+	ret = wlcore_write(wl, addr, chunk, fw_data_len % CHUNK_SIZE, false);
 
+out:
 	kfree(chunk);
-	return 0;
+	return ret;
 }
 
 int wlcore_boot_upload_firmware(struct wl1271 *wl)
@@ -203,9 +277,12 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
 	int i;
 	u32 dest_addr, val;
 	u8 *nvs_ptr, *nvs_aligned;
+	int ret;
 
-	if (wl->nvs == NULL)
+	if (wl->nvs == NULL) {
+		wl1271_error("NVS file is needed during boot");
 		return -ENODEV;
+	}
 
 	if (wl->quirks & WLCORE_QUIRK_LEGACY_NVS) {
 		struct wl1271_nvs_file *nvs =
@@ -298,7 +375,9 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
 			wl1271_debug(DEBUG_BOOT,
 				     "nvs burst write 0x%x: 0x%x",
 				     dest_addr, val);
-			wl1271_write32(wl, dest_addr, val);
+			ret = wlcore_write32(wl, dest_addr, val);
+			if (ret < 0)
+				return ret;
 
 			nvs_ptr += 4;
 			dest_addr += 4;
@@ -324,7 +403,9 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
 	nvs_len -= nvs_ptr - (u8 *)wl->nvs;
 
 	/* Now we must set the partition correctly */
-	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+	if (ret < 0)
+		return ret;
 
 	/* Copy the NVS tables to a new block to ensure alignment */
 	nvs_aligned = kmemdup(nvs_ptr, nvs_len, GFP_KERNEL);
@@ -332,11 +413,11 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
 		return -ENOMEM;
 
 	/* And finally we upload the NVS tables */
-	wlcore_write_data(wl, REG_CMD_MBOX_ADDRESS,
-			  nvs_aligned, nvs_len, false);
+	ret = wlcore_write_data(wl, REG_CMD_MBOX_ADDRESS, nvs_aligned, nvs_len,
+				false);
 
 	kfree(nvs_aligned);
-	return 0;
+	return ret;
 
 out_badnvs:
 	wl1271_error("nvs data is malformed");
@@ -350,11 +431,17 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
 	u32 chip_id, intr;
 
 	/* Make sure we have the boot partition */
-	wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	if (ret < 0)
+		return ret;
 
-	wl1271_boot_set_ecpu_ctrl(wl, ECPU_CONTROL_HALT);
+	ret = wl1271_boot_set_ecpu_ctrl(wl, ECPU_CONTROL_HALT);
+	if (ret < 0)
+		return ret;
 
-	chip_id = wlcore_read_reg(wl, REG_CHIP_ID_B);
+	ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &chip_id);
+	if (ret < 0)
+		return ret;
 
 	wl1271_debug(DEBUG_BOOT, "chip id after firmware boot: 0x%x", chip_id);
 
@@ -367,7 +454,9 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
 	loop = 0;
 	while (loop++ < INIT_LOOP) {
 		udelay(INIT_LOOP_DELAY);
-		intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+		ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &intr);
+		if (ret < 0)
+			return ret;
 
 		if (intr == 0xffffffff) {
 			wl1271_error("error reading hardware complete "
@@ -376,8 +465,10 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
 		}
 		/* check that ACX_INTR_INIT_COMPLETE is enabled */
 		else if (intr & WL1271_ACX_INTR_INIT_COMPLETE) {
-			wlcore_write_reg(wl, REG_INTERRUPT_ACK,
-					 WL1271_ACX_INTR_INIT_COMPLETE);
+			ret = wlcore_write_reg(wl, REG_INTERRUPT_ACK,
+					       WL1271_ACX_INTR_INIT_COMPLETE);
+			if (ret < 0)
+				return ret;
 			break;
 		}
 	}
@@ -389,20 +480,25 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
 	}
 
 	/* get hardware config command mail box */
-	wl->cmd_box_addr = wlcore_read_reg(wl, REG_COMMAND_MAILBOX_PTR);
+	ret = wlcore_read_reg(wl, REG_COMMAND_MAILBOX_PTR, &wl->cmd_box_addr);
+	if (ret < 0)
+		return ret;
 
 	wl1271_debug(DEBUG_MAILBOX, "cmd_box_addr 0x%x", wl->cmd_box_addr);
 
 	/* get hardware config event mail box */
-	wl->mbox_ptr[0] = wlcore_read_reg(wl, REG_EVENT_MAILBOX_PTR);
+	ret = wlcore_read_reg(wl, REG_EVENT_MAILBOX_PTR, &wl->mbox_ptr[0]);
+	if (ret < 0)
+		return ret;
+
 	wl->mbox_ptr[1] = wl->mbox_ptr[0] + sizeof(struct event_mailbox);
 
 	wl1271_debug(DEBUG_MAILBOX, "MBOX ptrs: 0x%x 0x%x",
 		     wl->mbox_ptr[0], wl->mbox_ptr[1]);
 
-	ret = wlcore_boot_fw_version(wl);
+	ret = wlcore_boot_static_data(wl);
 	if (ret < 0) {
-		wl1271_error("couldn't boot firmware");
+		wl1271_error("error getting static data");
 		return ret;
 	}
 
@@ -436,9 +532,9 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
 	}
 
 	/* set the working partition to its "running" mode offset */
-	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
 
 	/* firmware startup completed */
-	return 0;
+	return ret;
 }
 EXPORT_SYMBOL_GPL(wlcore_boot_run_firmware);
diff --git a/drivers/net/wireless/ti/wlcore/boot.h b/drivers/net/wireless/ti/wlcore/boot.h
index 094981dd222..a525225f990 100644
--- a/drivers/net/wireless/ti/wlcore/boot.h
+++ b/drivers/net/wireless/ti/wlcore/boot.h
@@ -40,6 +40,7 @@ struct wl1271_static_data {
 	u8 fw_version[WL1271_FW_VERSION_MAX_LEN];
 	u32 hw_version;
 	u8 tx_power_table[WL1271_NO_SUBBANDS][WL1271_NO_POWER_LEVELS];
+	u8 priv[0];
 };
 
 /* number of times we try to read the INIT interrupt */
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 5b128a97144..20e1bd92383 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -36,8 +36,10 @@
 #include "cmd.h"
 #include "event.h"
 #include "tx.h"
+#include "hw_ops.h"
 
 #define WL1271_CMD_FAST_POLL_COUNT       50
+#define WL1271_WAIT_EVENT_FAST_POLL_COUNT 20
 
 /*
  * send command to firmware
@@ -64,17 +66,24 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 	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);
+	ret = wlcore_write(wl, wl->cmd_box_addr, buf, len, false);
+	if (ret < 0)
+		goto fail;
 
 	/*
 	 * TODO: we just need this because one bit is in a different
 	 * place.  Is there any better way?
 	 */
-	wl->ops->trigger_cmd(wl, wl->cmd_box_addr, buf, len);
+	ret = wl->ops->trigger_cmd(wl, wl->cmd_box_addr, buf, len);
+	if (ret < 0)
+		goto fail;
 
 	timeout = jiffies + msecs_to_jiffies(WL1271_COMMAND_TIMEOUT);
 
-	intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+	ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &intr);
+	if (ret < 0)
+		goto fail;
+
 	while (!(intr & WL1271_ACX_INTR_CMD_COMPLETE)) {
 		if (time_after(jiffies, timeout)) {
 			wl1271_error("command complete timeout");
@@ -88,13 +97,18 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 		else
 			msleep(1);
 
-		intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+		ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &intr);
+		if (ret < 0)
+			goto fail;
 	}
 
 	/* read back the status code of the command */
 	if (res_len == 0)
 		res_len = sizeof(struct wl1271_cmd_header);
-	wl1271_read(wl, wl->cmd_box_addr, cmd, res_len, false);
+
+	ret = wlcore_read(wl, wl->cmd_box_addr, cmd, res_len, false);
+	if (ret < 0)
+		goto fail;
 
 	status = le16_to_cpu(cmd->status);
 	if (status != CMD_STATUS_SUCCESS) {
@@ -103,11 +117,14 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
 		goto fail;
 	}
 
-	wlcore_write_reg(wl, REG_INTERRUPT_ACK, WL1271_ACX_INTR_CMD_COMPLETE);
+	ret = wlcore_write_reg(wl, REG_INTERRUPT_ACK,
+			       WL1271_ACX_INTR_CMD_COMPLETE);
+	if (ret < 0)
+		goto fail;
+
 	return 0;
 
 fail:
-	WARN_ON(1);
 	wl12xx_queue_recovery_work(wl);
 	return ret;
 }
@@ -116,35 +133,50 @@ fail:
  * 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_or_timeout(struct wl1271 *wl, u32 mask)
+static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl,
+						u32 mask, bool *timeout)
 {
 	u32 *events_vector;
 	u32 event;
-	unsigned long timeout;
+	unsigned long timeout_time;
+	u16 poll_count = 0;
 	int ret = 0;
 
+	*timeout = false;
+
 	events_vector = kmalloc(sizeof(*events_vector), GFP_KERNEL | GFP_DMA);
 	if (!events_vector)
 		return -ENOMEM;
 
-	timeout = jiffies + msecs_to_jiffies(WL1271_EVENT_TIMEOUT);
+	timeout_time = jiffies + msecs_to_jiffies(WL1271_EVENT_TIMEOUT);
 
 	do {
-		if (time_after(jiffies, timeout)) {
+		if (time_after(jiffies, timeout_time)) {
 			wl1271_debug(DEBUG_CMD, "timeout waiting for event %d",
 				     (int)mask);
-			ret = -ETIMEDOUT;
+			*timeout = true;
 			goto out;
 		}
 
-		msleep(1);
+		poll_count++;
+		if (poll_count < WL1271_WAIT_EVENT_FAST_POLL_COUNT)
+			usleep_range(50, 51);
+		else
+			usleep_range(1000, 5000);
 
 		/* read from both event fields */
-		wl1271_read(wl, wl->mbox_ptr[0], events_vector,
-			    sizeof(*events_vector), false);
+		ret = wlcore_read(wl, wl->mbox_ptr[0], events_vector,
+				  sizeof(*events_vector), false);
+		if (ret < 0)
+			goto out;
+
 		event = *events_vector & mask;
-		wl1271_read(wl, wl->mbox_ptr[1], events_vector,
-			    sizeof(*events_vector), false);
+
+		ret = wlcore_read(wl, wl->mbox_ptr[1], events_vector,
+				  sizeof(*events_vector), false);
+		if (ret < 0)
+			goto out;
+
 		event |= *events_vector & mask;
 	} while (!event);
 
@@ -156,9 +188,10 @@ out:
 static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
 {
 	int ret;
+	bool timeout = false;
 
-	ret = wl1271_cmd_wait_for_event_or_timeout(wl, mask);
-	if (ret != 0) {
+	ret = wl1271_cmd_wait_for_event_or_timeout(wl, mask, &timeout);
+	if (ret != 0 || timeout) {
 		wl12xx_queue_recovery_work(wl);
 		return ret;
 	}
@@ -291,6 +324,23 @@ static int wl12xx_get_new_session_id(struct wl1271 *wl,
 	return wlvif->session_counter;
 }
 
+static u8 wlcore_get_native_channel_type(u8 nl_channel_type)
+{
+	switch (nl_channel_type) {
+	case NL80211_CHAN_NO_HT:
+		return WLCORE_CHAN_NO_HT;
+	case NL80211_CHAN_HT20:
+		return WLCORE_CHAN_HT20;
+	case NL80211_CHAN_HT40MINUS:
+		return WLCORE_CHAN_HT40MINUS;
+	case NL80211_CHAN_HT40PLUS:
+		return WLCORE_CHAN_HT40PLUS;
+	default:
+		WARN_ON(1);
+		return WLCORE_CHAN_NO_HT;
+	}
+}
+
 static int wl12xx_cmd_role_start_dev(struct wl1271 *wl,
 				     struct wl12xx_vif *wlvif)
 {
@@ -407,6 +457,7 @@ int wl12xx_cmd_role_start_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	memcpy(cmd->sta.ssid, wlvif->ssid, wlvif->ssid_len);
 	memcpy(cmd->sta.bssid, vif->bss_conf.bssid, ETH_ALEN);
 	cmd->sta.local_rates = cpu_to_le32(wlvif->rate_set);
+	cmd->channel_type = wlcore_get_native_channel_type(wlvif->channel_type);
 
 	if (wlvif->sta.hlid == WL12XX_INVALID_LINK_ID) {
 		ret = wl12xx_allocate_link(wl, wlvif, &wlvif->sta.hlid);
@@ -446,6 +497,7 @@ int wl12xx_cmd_role_stop_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 {
 	struct wl12xx_cmd_role_stop *cmd;
 	int ret;
+	bool timeout = false;
 
 	if (WARN_ON(wlvif->sta.hlid == WL12XX_INVALID_LINK_ID))
 		return -EINVAL;
@@ -468,6 +520,17 @@ int wl12xx_cmd_role_stop_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 		goto out_free;
 	}
 
+	/*
+	 * Sometimes the firmware doesn't send this event, so we just
+	 * time out without failing.  Queue recovery for other
+	 * failures.
+	 */
+	ret = wl1271_cmd_wait_for_event_or_timeout(wl,
+						   ROLE_STOP_COMPLETE_EVENT_ID,
+						   &timeout);
+	if (ret)
+		wl12xx_queue_recovery_work(wl);
+
 	wl12xx_free_link(wl, wlvif, &wlvif->sta.hlid);
 
 out_free:
@@ -482,6 +545,7 @@ int wl12xx_cmd_role_start_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	struct wl12xx_cmd_role_start *cmd;
 	struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
 	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	u32 supported_rates;
 	int ret;
 
 	wl1271_debug(DEBUG_CMD, "cmd role start ap %d", wlvif->role_id);
@@ -519,6 +583,7 @@ int wl12xx_cmd_role_start_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	/* FIXME: Change when adding DFS */
 	cmd->ap.reset_tsf = 1;  /* By default reset AP TSF */
 	cmd->channel = wlvif->channel;
+	cmd->channel_type = wlcore_get_native_channel_type(wlvif->channel_type);
 
 	if (!bss_conf->hidden_ssid) {
 		/* take the SSID from the beacon for backward compatibility */
@@ -531,7 +596,13 @@ int wl12xx_cmd_role_start_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 		memcpy(cmd->ap.ssid, bss_conf->ssid, bss_conf->ssid_len);
 	}
 
-	cmd->ap.local_rates = cpu_to_le32(0xffffffff);
+	supported_rates = CONF_TX_AP_ENABLED_RATES | CONF_TX_MCS_RATES |
+		wlcore_hw_ap_get_mimo_wide_rate_mask(wl, wlvif);
+
+	wl1271_debug(DEBUG_CMD, "cmd role start ap with supported_rates 0x%08x",
+		     supported_rates);
+
+	cmd->ap.local_rates = cpu_to_le32(supported_rates);
 
 	switch (wlvif->band) {
 	case IEEE80211_BAND_2GHZ:
@@ -797,6 +868,7 @@ out:
 	kfree(cmd);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wl1271_cmd_data_path);
 
 int wl1271_cmd_ps_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		       u8 ps_mode, u16 auto_ps_timeout)
@@ -953,12 +1025,14 @@ out:
 int wl12xx_cmd_build_probe_req(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			       u8 role_id, u8 band,
 			       const u8 *ssid, size_t ssid_len,
-			       const u8 *ie, size_t ie_len)
+			       const u8 *ie, size_t ie_len, bool sched_scan)
 {
 	struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
 	struct sk_buff *skb;
 	int ret;
 	u32 rate;
+	u16 template_id_2_4 = CMD_TEMPL_CFG_PROBE_REQ_2_4;
+	u16 template_id_5 = CMD_TEMPL_CFG_PROBE_REQ_5;
 
 	skb = ieee80211_probereq_get(wl->hw, vif, ssid, ssid_len,
 				     ie, ie_len);
@@ -969,14 +1043,20 @@ int wl12xx_cmd_build_probe_req(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 
 	wl1271_dump(DEBUG_SCAN, "PROBE REQ: ", skb->data, skb->len);
 
+	if (!sched_scan &&
+	    (wl->quirks & WLCORE_QUIRK_DUAL_PROBE_TMPL)) {
+		template_id_2_4 = CMD_TEMPL_APP_PROBE_REQ_2_4;
+		template_id_5 = CMD_TEMPL_APP_PROBE_REQ_5;
+	}
+
 	rate = wl1271_tx_min_rate_get(wl, wlvif->bitrate_masks[band]);
 	if (band == IEEE80211_BAND_2GHZ)
 		ret = wl1271_cmd_template_set(wl, role_id,
-					      CMD_TEMPL_CFG_PROBE_REQ_2_4,
+					      template_id_2_4,
 					      skb->data, skb->len, 0, rate);
 	else
 		ret = wl1271_cmd_template_set(wl, role_id,
-					      CMD_TEMPL_CFG_PROBE_REQ_5,
+					      template_id_5,
 					      skb->data, skb->len, 0, rate);
 
 out:
@@ -1018,7 +1098,7 @@ out:
 
 int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 {
-	int ret, extra;
+	int ret, extra = 0;
 	u16 fc;
 	struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
 	struct sk_buff *skb;
@@ -1057,7 +1137,8 @@ int wl1271_cmd_build_arp_rsp(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	/* encryption space */
 	switch (wlvif->encryption_type) {
 	case KEY_TKIP:
-		extra = WL1271_EXTRA_SPACE_TKIP;
+		if (wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE)
+			extra = WL1271_EXTRA_SPACE_TKIP;
 		break;
 	case KEY_AES:
 		extra = WL1271_EXTRA_SPACE_AES;
@@ -1346,13 +1427,18 @@ int wl12xx_cmd_add_peer(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 
 	for (i = 0; i < NUM_ACCESS_CATEGORIES_COPY; i++)
 		if (sta->wme && (sta->uapsd_queues & BIT(i)))
-			cmd->psd_type[i] = WL1271_PSD_UPSD_TRIGGER;
+			cmd->psd_type[NUM_ACCESS_CATEGORIES_COPY-1-i] =
+					WL1271_PSD_UPSD_TRIGGER;
 		else
-			cmd->psd_type[i] = WL1271_PSD_LEGACY;
+			cmd->psd_type[NUM_ACCESS_CATEGORIES_COPY-1-i] =
+					WL1271_PSD_LEGACY;
+
 
 	sta_rates = sta->supp_rates[wlvif->band];
 	if (sta->ht_cap.ht_supported)
-		sta_rates |= sta->ht_cap.mcs.rx_mask[0] << HW_HT_RATES_OFFSET;
+		sta_rates |=
+			(sta->ht_cap.mcs.rx_mask[0] << HW_HT_RATES_OFFSET) |
+			(sta->ht_cap.mcs.rx_mask[1] << HW_MIMO_RATES_OFFSET);
 
 	cmd->supported_rates =
 		cpu_to_le32(wl1271_tx_enabled_rates_get(wl, sta_rates,
@@ -1378,6 +1464,7 @@ int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid)
 {
 	struct wl12xx_cmd_remove_peer *cmd;
 	int ret;
+	bool timeout = false;
 
 	wl1271_debug(DEBUG_CMD, "cmd remove peer %d", (int)hlid);
 
@@ -1398,12 +1485,16 @@ int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid)
 		goto out_free;
 	}
 
+	ret = wl1271_cmd_wait_for_event_or_timeout(wl,
+					   PEER_REMOVE_COMPLETE_EVENT_ID,
+					   &timeout);
 	/*
 	 * We are ok with a timeout here. The event is sometimes not sent
-	 * due to a firmware bug.
+	 * due to a firmware bug. In case of another error (like SDIO timeout)
+	 * queue a recovery.
 	 */
-	wl1271_cmd_wait_for_event_or_timeout(wl,
-					     PEER_REMOVE_COMPLETE_EVENT_ID);
+	if (ret)
+		wl12xx_queue_recovery_work(wl);
 
 out_free:
 	kfree(cmd);
@@ -1573,19 +1664,25 @@ out:
 int wl12xx_roc(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 role_id)
 {
 	int ret = 0;
+	bool is_first_roc;
 
 	if (WARN_ON(test_bit(role_id, wl->roc_map)))
 		return 0;
 
+	is_first_roc = (find_first_bit(wl->roc_map, WL12XX_MAX_ROLES) >=
+			WL12XX_MAX_ROLES);
+
 	ret = wl12xx_cmd_roc(wl, wlvif, role_id);
 	if (ret < 0)
 		goto out;
 
-	ret = wl1271_cmd_wait_for_event(wl,
-					REMAIN_ON_CHANNEL_COMPLETE_EVENT_ID);
-	if (ret < 0) {
-		wl1271_error("cmd roc event completion error");
-		goto out;
+	if (is_first_roc) {
+		ret = wl1271_cmd_wait_for_event(wl,
+					   REMAIN_ON_CHANNEL_COMPLETE_EVENT_ID);
+		if (ret < 0) {
+			wl1271_error("cmd roc event completion error");
+			goto out;
+		}
 	}
 
 	__set_bit(role_id, wl->roc_map);
@@ -1714,7 +1811,9 @@ int wl12xx_stop_dev(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 		return -EINVAL;
 
 	/* flush all pending packets */
-	wl1271_tx_work_locked(wl);
+	ret = wlcore_tx_work_locked(wl);
+	if (ret < 0)
+		goto out;
 
 	if (test_bit(wlvif->dev_role_id, wl->roc_map)) {
 		ret = wl12xx_croc(wl, wlvif->dev_role_id);
diff --git a/drivers/net/wireless/ti/wlcore/cmd.h b/drivers/net/wireless/ti/wlcore/cmd.h
index a46ae07cb77..4ef0b095f0d 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.h
+++ b/drivers/net/wireless/ti/wlcore/cmd.h
@@ -58,7 +58,7 @@ int wl1271_cmd_build_ps_poll(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 int wl12xx_cmd_build_probe_req(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			       u8 role_id, u8 band,
 			       const u8 *ssid, size_t ssid_len,
-			       const u8 *ie, size_t ie_len);
+			       const u8 *ie, size_t ie_len, bool sched_scan);
 struct sk_buff *wl1271_cmd_build_ap_probe_req(struct wl1271 *wl,
 					      struct wl12xx_vif *wlvif,
 					      struct sk_buff *skb);
@@ -172,8 +172,8 @@ enum cmd_templ {
 	CMD_TEMPL_PS_POLL,
 	CMD_TEMPL_KLV,
 	CMD_TEMPL_DISCONNECT,
-	CMD_TEMPL_PROBE_REQ_2_4, /* for firmware internal use only */
-	CMD_TEMPL_PROBE_REQ_5,   /* for firmware internal use only */
+	CMD_TEMPL_APP_PROBE_REQ_2_4,
+	CMD_TEMPL_APP_PROBE_REQ_5,
 	CMD_TEMPL_BAR,           /* for firmware internal use only */
 	CMD_TEMPL_CTS,           /*
 				  * For CTS-to-self (FastCTS) mechanism
@@ -192,7 +192,7 @@ enum cmd_templ {
 #define WL1271_COMMAND_TIMEOUT     2000
 #define WL1271_CMD_TEMPL_DFLT_SIZE 252
 #define WL1271_CMD_TEMPL_MAX_SIZE  512
-#define WL1271_EVENT_TIMEOUT       750
+#define WL1271_EVENT_TIMEOUT       1500
 
 struct wl1271_cmd_header {
 	__le16 id;
@@ -266,13 +266,22 @@ enum wlcore_band {
 	WLCORE_BAND_MAX_RADIO		= 0x7F,
 };
 
+enum wlcore_channel_type {
+	WLCORE_CHAN_NO_HT,
+	WLCORE_CHAN_HT20,
+	WLCORE_CHAN_HT40MINUS,
+	WLCORE_CHAN_HT40PLUS
+};
+
 struct wl12xx_cmd_role_start {
 	struct wl1271_cmd_header header;
 
 	u8 role_id;
 	u8 band;
 	u8 channel;
-	u8 padding;
+
+	/* enum wlcore_channel_type */
+	u8 channel_type;
 
 	union {
 		struct {
@@ -643,4 +652,25 @@ struct wl12xx_cmd_stop_channel_switch {
 	struct wl1271_cmd_header header;
 } __packed;
 
+/* Used to check radio status after calibration */
+#define MAX_TLV_LENGTH		500
+#define TEST_CMD_P2G_CAL	2	/* TX BiP */
+
+struct wl1271_cmd_cal_p2g {
+	struct wl1271_cmd_header header;
+
+	struct wl1271_cmd_test_header test;
+
+	__le32 ver;
+	__le16 len;
+	u8 buf[MAX_TLV_LENGTH];
+	u8 type;
+	u8 padding;
+
+	__le16 radio_status;
+
+	u8 sub_band_mask;
+	u8 padding2;
+} __packed;
+
 #endif /* __WL1271_CMD_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/conf.h b/drivers/net/wireless/ti/wlcore/conf.h
index fef0db4213b..d77224f2ac6 100644
--- a/drivers/net/wireless/ti/wlcore/conf.h
+++ b/drivers/net/wireless/ti/wlcore/conf.h
@@ -45,7 +45,15 @@ enum {
 	CONF_HW_BIT_RATE_MCS_4   = BIT(17),
 	CONF_HW_BIT_RATE_MCS_5   = BIT(18),
 	CONF_HW_BIT_RATE_MCS_6   = BIT(19),
-	CONF_HW_BIT_RATE_MCS_7   = BIT(20)
+	CONF_HW_BIT_RATE_MCS_7   = BIT(20),
+	CONF_HW_BIT_RATE_MCS_8   = BIT(21),
+	CONF_HW_BIT_RATE_MCS_9   = BIT(22),
+	CONF_HW_BIT_RATE_MCS_10  = BIT(23),
+	CONF_HW_BIT_RATE_MCS_11  = BIT(24),
+	CONF_HW_BIT_RATE_MCS_12  = BIT(25),
+	CONF_HW_BIT_RATE_MCS_13  = BIT(26),
+	CONF_HW_BIT_RATE_MCS_14  = BIT(27),
+	CONF_HW_BIT_RATE_MCS_15  = BIT(28),
 };
 
 enum {
@@ -310,7 +318,7 @@ enum {
 struct conf_sg_settings {
 	u32 params[CONF_SG_PARAMS_MAX];
 	u8 state;
-};
+} __packed;
 
 enum conf_rx_queue_type {
 	CONF_RX_QUEUE_TYPE_LOW_PRIORITY,  /* All except the high priority */
@@ -394,7 +402,7 @@ struct conf_rx_settings {
 	 * Range: RX_QUEUE_TYPE_RX_LOW_PRIORITY, RX_QUEUE_TYPE_RX_HIGH_PRIORITY,
 	 */
 	u8 queue_type;
-};
+} __packed;
 
 #define CONF_TX_MAX_RATE_CLASSES       10
 
@@ -435,6 +443,12 @@ struct conf_rx_settings {
 	CONF_HW_BIT_RATE_MCS_5 | CONF_HW_BIT_RATE_MCS_6 |        \
 	CONF_HW_BIT_RATE_MCS_7)
 
+#define CONF_TX_MIMO_RATES (CONF_HW_BIT_RATE_MCS_8 |             \
+	CONF_HW_BIT_RATE_MCS_9 | CONF_HW_BIT_RATE_MCS_10 |       \
+	CONF_HW_BIT_RATE_MCS_11 | CONF_HW_BIT_RATE_MCS_12 |      \
+	CONF_HW_BIT_RATE_MCS_13 | CONF_HW_BIT_RATE_MCS_14 |      \
+	CONF_HW_BIT_RATE_MCS_15)
+
 /*
  * Default rates for management traffic when operating in AP mode. This
  * should be configured according to the basic rate set of the AP
@@ -487,7 +501,7 @@ struct conf_tx_rate_class {
 	 *               the policy (0 - long preamble, 1 - short preamble.
 	 */
 	u8 aflags;
-};
+} __packed;
 
 #define CONF_TX_MAX_AC_COUNT 4
 
@@ -504,7 +518,7 @@ enum conf_tx_ac {
 	CONF_TX_AC_VI = 2,         /* video */
 	CONF_TX_AC_VO = 3,         /* voice */
 	CONF_TX_AC_CTS2SELF = 4,   /* fictitious AC, follows AC_VO */
-	CONF_TX_AC_ANY_TID = 0x1f
+	CONF_TX_AC_ANY_TID = 0xff
 };
 
 struct conf_tx_ac_category {
@@ -544,7 +558,7 @@ struct conf_tx_ac_category {
 	 * Range: u16
 	 */
 	u16 tx_op_limit;
-};
+} __packed;
 
 #define CONF_TX_MAX_TID_COUNT 8
 
@@ -578,7 +592,7 @@ struct conf_tx_tid {
 	u8 ps_scheme;
 	u8 ack_policy;
 	u32 apsd_conf[2];
-};
+} __packed;
 
 struct conf_tx_settings {
 	/*
@@ -664,7 +678,7 @@ struct conf_tx_settings {
 
 	/* Time in ms for Tx watchdog timer to expire */
 	u32 tx_watchdog_timeout;
-};
+} __packed;
 
 enum {
 	CONF_WAKE_UP_EVENT_BEACON    = 0x01, /* Wake on every Beacon*/
@@ -711,7 +725,7 @@ struct conf_bcn_filt_rule {
 	 * Version for the vendor specifie IE (221)
 	 */
 	u8 version[CONF_BCN_IE_VER_LEN];
-};
+} __packed;
 
 #define CONF_MAX_RSSI_SNR_TRIGGERS 8
 
@@ -762,7 +776,7 @@ struct conf_sig_weights {
 	 * Range: u8
 	 */
 	u8 snr_pkt_avg_weight;
-};
+} __packed;
 
 enum conf_bcn_filt_mode {
 	CONF_BCN_FILT_MODE_DISABLED = 0,
@@ -810,7 +824,7 @@ struct conf_conn_settings {
 	 *
 	 * Range: CONF_BCN_FILT_MODE_*
 	 */
-	enum conf_bcn_filt_mode bcn_filt_mode;
+	u8 bcn_filt_mode;
 
 	/*
 	 * Configure Beacon filter pass-thru rules.
@@ -937,7 +951,13 @@ struct conf_conn_settings {
 	 * Range: u16
 	 */
 	u8 max_listen_interval;
-};
+
+	/*
+	 * Default sleep authorization for a new STA interface. This determines
+	 * whether we can go to ELP.
+	 */
+	u8 sta_sleep_auth;
+} __packed;
 
 enum {
 	CONF_REF_CLK_19_2_E,
@@ -965,6 +985,11 @@ struct conf_itrim_settings {
 
 	/* moderation timeout in microsecs from the last TX */
 	u32 timeout;
+} __packed;
+
+enum conf_fast_wakeup {
+	CONF_FAST_WAKEUP_ENABLE,
+	CONF_FAST_WAKEUP_DISABLE,
 };
 
 struct conf_pm_config_settings {
@@ -978,10 +1003,10 @@ struct conf_pm_config_settings {
 	/*
 	 * Host fast wakeup support
 	 *
-	 * Range: true, false
+	 * Range: enum conf_fast_wakeup
 	 */
-	bool host_fast_wakeup_support;
-};
+	u8 host_fast_wakeup_support;
+} __packed;
 
 struct conf_roam_trigger_settings {
 	/*
@@ -1018,7 +1043,7 @@ struct conf_roam_trigger_settings {
 	 * Range: 0 - 255
 	 */
 	u8 avg_weight_snr_data;
-};
+} __packed;
 
 struct conf_scan_settings {
 	/*
@@ -1064,7 +1089,7 @@ struct conf_scan_settings {
 	 * Range: u32 Microsecs
 	 */
 	u32 split_scan_timeout;
-};
+} __packed;
 
 struct conf_sched_scan_settings {
 	/*
@@ -1102,7 +1127,7 @@ struct conf_sched_scan_settings {
 
 	/* SNR threshold to be used for filtering */
 	s8 snr_threshold;
-};
+} __packed;
 
 struct conf_ht_setting {
 	u8 rx_ba_win_size;
@@ -1111,7 +1136,7 @@ struct conf_ht_setting {
 
 	/* bitmap of enabled TIDs for TX BA sessions */
 	u8 tx_ba_tid_bitmap;
-};
+} __packed;
 
 struct conf_memory_settings {
 	/* Number of stations supported in IBSS mode */
@@ -1151,7 +1176,7 @@ struct conf_memory_settings {
 	 * Range: 0-120
 	 */
 	u8 tx_min;
-};
+} __packed;
 
 struct conf_fm_coex {
 	u8 enable;
@@ -1164,7 +1189,7 @@ struct conf_fm_coex {
 	u16 ldo_stabilization_time;
 	u8 fm_disturbed_band_margin;
 	u8 swallow_clk_diff;
-};
+} __packed;
 
 struct conf_rx_streaming_settings {
 	/*
@@ -1193,7 +1218,7 @@ struct conf_rx_streaming_settings {
 	 * enable rx streaming also when there is no coex activity
 	 */
 	u8 always;
-};
+} __packed;
 
 struct conf_fwlog {
 	/* Continuous or on-demand */
@@ -1217,7 +1242,7 @@ struct conf_fwlog {
 
 	/* Regulates the frequency of log messages */
 	u8 threshold;
-};
+} __packed;
 
 #define ACX_RATE_MGMT_NUM_OF_RATES 13
 struct conf_rate_policy_settings {
@@ -1236,7 +1261,7 @@ struct conf_rate_policy_settings {
 	u8 rate_check_up;
 	u8 rate_check_down;
 	u8 rate_retry_policy[ACX_RATE_MGMT_NUM_OF_RATES];
-};
+} __packed;
 
 struct conf_hangover_settings {
 	u32 recover_time;
@@ -1250,7 +1275,23 @@ struct conf_hangover_settings {
 	u8 quiet_time;
 	u8 increase_time;
 	u8 window_size;
-};
+} __packed;
+
+/*
+ * The conf version consists of 4 bytes.  The two MSB are the wlcore
+ * version, the two LSB are the lower driver's private conf
+ * version.
+ */
+#define WLCORE_CONF_VERSION	(0x0002 << 16)
+#define WLCORE_CONF_MASK	0xffff0000
+#define WLCORE_CONF_SIZE	(sizeof(struct wlcore_conf_header) +	\
+				 sizeof(struct wlcore_conf))
+
+struct wlcore_conf_header {
+	__le32 magic;
+	__le32 version;
+	__le32 checksum;
+} __packed;
 
 struct wlcore_conf {
 	struct conf_sg_settings sg;
@@ -1269,6 +1310,12 @@ struct wlcore_conf {
 	struct conf_fwlog fwlog;
 	struct conf_rate_policy_settings rate;
 	struct conf_hangover_settings hangover;
-};
+} __packed;
+
+struct wlcore_conf_file {
+	struct wlcore_conf_header header;
+	struct wlcore_conf core;
+	u8 priv[0];
+} __packed;
 
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index d5aea1ff5ad..80dbc5304fa 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -25,6 +25,7 @@
 
 #include <linux/skbuff.h>
 #include <linux/slab.h>
+#include <linux/module.h>
 
 #include "wlcore.h"
 #include "debug.h"
@@ -32,14 +33,16 @@
 #include "ps.h"
 #include "io.h"
 #include "tx.h"
+#include "hw_ops.h"
 
 /* ms */
 #define WL1271_DEBUGFS_STATS_LIFETIME 1000
 
+#define WLCORE_MAX_BLOCK_SIZE ((size_t)(4*PAGE_SIZE))
+
 /* debugfs macros idea from mac80211 */
-#define DEBUGFS_FORMAT_BUFFER_SIZE 100
-static int wl1271_format_buffer(char __user *userbuf, size_t count,
-				    loff_t *ppos, char *fmt, ...)
+int wl1271_format_buffer(char __user *userbuf, size_t count,
+			 loff_t *ppos, char *fmt, ...)
 {
 	va_list args;
 	char buf[DEBUGFS_FORMAT_BUFFER_SIZE];
@@ -51,59 +54,9 @@ static int wl1271_format_buffer(char __user *userbuf, size_t count,
 
 	return simple_read_from_buffer(userbuf, count, ppos, buf, res);
 }
+EXPORT_SYMBOL_GPL(wl1271_format_buffer);
 
-#define DEBUGFS_READONLY_FILE(name, fmt, value...)			\
-static ssize_t name## _read(struct file *file, char __user *userbuf,	\
-			    size_t count, loff_t *ppos)			\
-{									\
-	struct wl1271 *wl = file->private_data;				\
-	return wl1271_format_buffer(userbuf, count, ppos,		\
-				    fmt "\n", ##value);			\
-}									\
-									\
-static const struct file_operations name## _ops = {			\
-	.read = name## _read,						\
-	.open = simple_open,						\
-	.llseek	= generic_file_llseek,					\
-};
-
-#define DEBUGFS_ADD(name, parent)					\
-	entry = debugfs_create_file(#name, 0400, parent,		\
-				    wl, &name## _ops);			\
-	if (!entry || IS_ERR(entry))					\
-		goto err;						\
-
-#define DEBUGFS_ADD_PREFIX(prefix, name, parent)			\
-	do {								\
-		entry = debugfs_create_file(#name, 0400, parent,	\
-				    wl, &prefix## _## name## _ops);	\
-		if (!entry || IS_ERR(entry))				\
-			goto err;					\
-	} while (0);
-
-#define DEBUGFS_FWSTATS_FILE(sub, name, fmt)				\
-static ssize_t sub## _ ##name## _read(struct file *file,		\
-				      char __user *userbuf,		\
-				      size_t count, loff_t *ppos)	\
-{									\
-	struct wl1271 *wl = file->private_data;				\
-									\
-	wl1271_debugfs_update_stats(wl);				\
-									\
-	return wl1271_format_buffer(userbuf, count, ppos, fmt "\n",	\
-				    wl->stats.fw_stats->sub.name);	\
-}									\
-									\
-static const struct file_operations sub## _ ##name## _ops = {		\
-	.read = sub## _ ##name## _read,					\
-	.open = simple_open,						\
-	.llseek	= generic_file_llseek,					\
-};
-
-#define DEBUGFS_FWSTATS_ADD(sub, name)				\
-	DEBUGFS_ADD(sub## _ ##name, stats)
-
-static void wl1271_debugfs_update_stats(struct wl1271 *wl)
+void wl1271_debugfs_update_stats(struct wl1271 *wl)
 {
 	int ret;
 
@@ -125,97 +78,7 @@ static void wl1271_debugfs_update_stats(struct wl1271 *wl)
 out:
 	mutex_unlock(&wl->mutex);
 }
-
-DEBUGFS_FWSTATS_FILE(tx, internal_desc_overflow, "%u");
-
-DEBUGFS_FWSTATS_FILE(rx, out_of_mem, "%u");
-DEBUGFS_FWSTATS_FILE(rx, hdr_overflow, "%u");
-DEBUGFS_FWSTATS_FILE(rx, hw_stuck, "%u");
-DEBUGFS_FWSTATS_FILE(rx, dropped, "%u");
-DEBUGFS_FWSTATS_FILE(rx, fcs_err, "%u");
-DEBUGFS_FWSTATS_FILE(rx, xfr_hint_trig, "%u");
-DEBUGFS_FWSTATS_FILE(rx, path_reset, "%u");
-DEBUGFS_FWSTATS_FILE(rx, reset_counter, "%u");
-
-DEBUGFS_FWSTATS_FILE(dma, rx_requested, "%u");
-DEBUGFS_FWSTATS_FILE(dma, rx_errors, "%u");
-DEBUGFS_FWSTATS_FILE(dma, tx_requested, "%u");
-DEBUGFS_FWSTATS_FILE(dma, tx_errors, "%u");
-
-DEBUGFS_FWSTATS_FILE(isr, cmd_cmplt, "%u");
-DEBUGFS_FWSTATS_FILE(isr, fiqs, "%u");
-DEBUGFS_FWSTATS_FILE(isr, rx_headers, "%u");
-DEBUGFS_FWSTATS_FILE(isr, rx_mem_overflow, "%u");
-DEBUGFS_FWSTATS_FILE(isr, rx_rdys, "%u");
-DEBUGFS_FWSTATS_FILE(isr, irqs, "%u");
-DEBUGFS_FWSTATS_FILE(isr, tx_procs, "%u");
-DEBUGFS_FWSTATS_FILE(isr, decrypt_done, "%u");
-DEBUGFS_FWSTATS_FILE(isr, dma0_done, "%u");
-DEBUGFS_FWSTATS_FILE(isr, dma1_done, "%u");
-DEBUGFS_FWSTATS_FILE(isr, tx_exch_complete, "%u");
-DEBUGFS_FWSTATS_FILE(isr, commands, "%u");
-DEBUGFS_FWSTATS_FILE(isr, rx_procs, "%u");
-DEBUGFS_FWSTATS_FILE(isr, hw_pm_mode_changes, "%u");
-DEBUGFS_FWSTATS_FILE(isr, host_acknowledges, "%u");
-DEBUGFS_FWSTATS_FILE(isr, pci_pm, "%u");
-DEBUGFS_FWSTATS_FILE(isr, wakeups, "%u");
-DEBUGFS_FWSTATS_FILE(isr, low_rssi, "%u");
-
-DEBUGFS_FWSTATS_FILE(wep, addr_key_count, "%u");
-DEBUGFS_FWSTATS_FILE(wep, default_key_count, "%u");
-/* skipping wep.reserved */
-DEBUGFS_FWSTATS_FILE(wep, key_not_found, "%u");
-DEBUGFS_FWSTATS_FILE(wep, decrypt_fail, "%u");
-DEBUGFS_FWSTATS_FILE(wep, packets, "%u");
-DEBUGFS_FWSTATS_FILE(wep, interrupt, "%u");
-
-DEBUGFS_FWSTATS_FILE(pwr, ps_enter, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, elp_enter, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, missing_bcns, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, wake_on_host, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, wake_on_timer_exp, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, tx_with_ps, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, tx_without_ps, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, rcvd_beacons, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, power_save_off, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, enable_ps, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, disable_ps, "%u");
-DEBUGFS_FWSTATS_FILE(pwr, fix_tsf_ps, "%u");
-/* skipping cont_miss_bcns_spread for now */
-DEBUGFS_FWSTATS_FILE(pwr, rcvd_awake_beacons, "%u");
-
-DEBUGFS_FWSTATS_FILE(mic, rx_pkts, "%u");
-DEBUGFS_FWSTATS_FILE(mic, calc_failure, "%u");
-
-DEBUGFS_FWSTATS_FILE(aes, encrypt_fail, "%u");
-DEBUGFS_FWSTATS_FILE(aes, decrypt_fail, "%u");
-DEBUGFS_FWSTATS_FILE(aes, encrypt_packets, "%u");
-DEBUGFS_FWSTATS_FILE(aes, decrypt_packets, "%u");
-DEBUGFS_FWSTATS_FILE(aes, encrypt_interrupt, "%u");
-DEBUGFS_FWSTATS_FILE(aes, decrypt_interrupt, "%u");
-
-DEBUGFS_FWSTATS_FILE(event, heart_beat, "%u");
-DEBUGFS_FWSTATS_FILE(event, calibration, "%u");
-DEBUGFS_FWSTATS_FILE(event, rx_mismatch, "%u");
-DEBUGFS_FWSTATS_FILE(event, rx_mem_empty, "%u");
-DEBUGFS_FWSTATS_FILE(event, rx_pool, "%u");
-DEBUGFS_FWSTATS_FILE(event, oom_late, "%u");
-DEBUGFS_FWSTATS_FILE(event, phy_transmit_error, "%u");
-DEBUGFS_FWSTATS_FILE(event, tx_stuck, "%u");
-
-DEBUGFS_FWSTATS_FILE(ps, pspoll_timeouts, "%u");
-DEBUGFS_FWSTATS_FILE(ps, upsd_timeouts, "%u");
-DEBUGFS_FWSTATS_FILE(ps, upsd_max_sptime, "%u");
-DEBUGFS_FWSTATS_FILE(ps, upsd_max_apturn, "%u");
-DEBUGFS_FWSTATS_FILE(ps, pspoll_max_apturn, "%u");
-DEBUGFS_FWSTATS_FILE(ps, pspoll_utilization, "%u");
-DEBUGFS_FWSTATS_FILE(ps, upsd_utilization, "%u");
-
-DEBUGFS_FWSTATS_FILE(rxpipe, rx_prep_beacon_drop, "%u");
-DEBUGFS_FWSTATS_FILE(rxpipe, descr_host_int_trig_rx_data, "%u");
-DEBUGFS_FWSTATS_FILE(rxpipe, beacon_buffer_thres_host_int_trig_rx_data, "%u");
-DEBUGFS_FWSTATS_FILE(rxpipe, missed_beacon_host_int_trig_rx_data, "%u");
-DEBUGFS_FWSTATS_FILE(rxpipe, tx_xfr_host_int_trig_rx_data, "%u");
+EXPORT_SYMBOL_GPL(wl1271_debugfs_update_stats);
 
 DEBUGFS_READONLY_FILE(retry_count, "%u", wl->stats.retry_count);
 DEBUGFS_READONLY_FILE(excessive_retries, "%u",
@@ -241,6 +104,89 @@ static const struct file_operations tx_queue_len_ops = {
 	.llseek = default_llseek,
 };
 
+static void chip_op_handler(struct wl1271 *wl, unsigned long value,
+			    void *arg)
+{
+	int ret;
+	int (*chip_op) (struct wl1271 *wl);
+
+	if (!arg) {
+		wl1271_warning("debugfs chip_op_handler with no callback");
+		return;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		return;
+
+	chip_op = arg;
+	chip_op(wl);
+
+	wl1271_ps_elp_sleep(wl);
+}
+
+
+static inline void no_write_handler(struct wl1271 *wl,
+				    unsigned long value,
+				    unsigned long param)
+{
+}
+
+#define WL12XX_CONF_DEBUGFS(param, conf_sub_struct,			\
+			    min_val, max_val, write_handler_locked,	\
+			    write_handler_arg)				\
+	static ssize_t param##_read(struct file *file,			\
+				      char __user *user_buf,		\
+				      size_t count, loff_t *ppos)	\
+	{								\
+	struct wl1271 *wl = file->private_data;				\
+	return wl1271_format_buffer(user_buf, count,			\
+				    ppos, "%d\n",			\
+				    wl->conf.conf_sub_struct.param);	\
+	}								\
+									\
+	static ssize_t param##_write(struct file *file,			\
+				     const char __user *user_buf,	\
+				     size_t count, loff_t *ppos)	\
+	{								\
+	struct wl1271 *wl = file->private_data;				\
+	unsigned long value;						\
+	int ret;							\
+									\
+	ret = kstrtoul_from_user(user_buf, count, 10, &value);		\
+	if (ret < 0) {							\
+		wl1271_warning("illegal value for " #param);		\
+		return -EINVAL;						\
+	}								\
+									\
+	if (value < min_val || value > max_val) {			\
+		wl1271_warning(#param " is not in valid range");	\
+		return -ERANGE;						\
+	}								\
+									\
+	mutex_lock(&wl->mutex);						\
+	wl->conf.conf_sub_struct.param = value;				\
+									\
+	write_handler_locked(wl, value, write_handler_arg);		\
+									\
+	mutex_unlock(&wl->mutex);					\
+	return count;							\
+	}								\
+									\
+	static const struct file_operations param##_ops = {		\
+		.read = param##_read,					\
+		.write = param##_write,					\
+		.open = simple_open,					\
+		.llseek = default_llseek,				\
+	};
+
+WL12XX_CONF_DEBUGFS(irq_pkt_threshold, rx, 0, 65535,
+		    chip_op_handler, wl1271_acx_init_rx_interrupt)
+WL12XX_CONF_DEBUGFS(irq_blk_threshold, rx, 0, 65535,
+		    chip_op_handler, wl1271_acx_init_rx_interrupt)
+WL12XX_CONF_DEBUGFS(irq_timeout, rx, 0, 100,
+		    chip_op_handler, wl1271_acx_init_rx_interrupt)
+
 static ssize_t gpio_power_read(struct file *file, char __user *user_buf,
 			  size_t count, loff_t *ppos)
 {
@@ -535,8 +481,7 @@ static ssize_t driver_state_read(struct file *file, char __user *user_buf,
 	DRIVER_STATE_PRINT_LHEX(ap_ps_map);
 	DRIVER_STATE_PRINT_HEX(quirks);
 	DRIVER_STATE_PRINT_HEX(irq);
-	DRIVER_STATE_PRINT_HEX(ref_clock);
-	DRIVER_STATE_PRINT_HEX(tcxo_clock);
+	/* TODO: ref_clock and tcxo_clock were moved to wl12xx priv */
 	DRIVER_STATE_PRINT_HEX(hw_pg_ver);
 	DRIVER_STATE_PRINT_HEX(platform_quirks);
 	DRIVER_STATE_PRINT_HEX(chip.id);
@@ -647,7 +592,6 @@ static ssize_t vifs_state_read(struct file *file, char __user *user_buf,
 		VIF_STATE_PRINT_INT(last_rssi_event);
 		VIF_STATE_PRINT_INT(ba_support);
 		VIF_STATE_PRINT_INT(ba_allowed);
-		VIF_STATE_PRINT_INT(is_gem);
 		VIF_STATE_PRINT_LLHEX(tx_security_seq);
 		VIF_STATE_PRINT_INT(tx_security_last_seq_lsb);
 	}
@@ -1002,108 +946,281 @@ static const struct file_operations beacon_filtering_ops = {
 	.llseek = default_llseek,
 };
 
-static int wl1271_debugfs_add_files(struct wl1271 *wl,
-				     struct dentry *rootdir)
+static ssize_t fw_stats_raw_read(struct file *file,
+				 char __user *userbuf,
+				 size_t count, loff_t *ppos)
 {
-	int ret = 0;
-	struct dentry *entry, *stats, *streaming;
+	struct wl1271 *wl = file->private_data;
 
-	stats = debugfs_create_dir("fw-statistics", rootdir);
-	if (!stats || IS_ERR(stats)) {
-		entry = stats;
-		goto err;
+	wl1271_debugfs_update_stats(wl);
+
+	return simple_read_from_buffer(userbuf, count, ppos,
+				       wl->stats.fw_stats,
+				       wl->stats.fw_stats_len);
+}
+
+static const struct file_operations fw_stats_raw_ops = {
+	.read = fw_stats_raw_read,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
+static ssize_t sleep_auth_read(struct file *file, char __user *user_buf,
+			       size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+
+	return wl1271_format_buffer(user_buf, count,
+				    ppos, "%d\n",
+				    wl->sleep_auth);
+}
+
+static ssize_t sleep_auth_write(struct file *file,
+				const char __user *user_buf,
+				size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	unsigned long value;
+	int ret;
+
+	ret = kstrtoul_from_user(user_buf, count, 0, &value);
+	if (ret < 0) {
+		wl1271_warning("illegal value in sleep_auth");
+		return -EINVAL;
+	}
+
+	if (value < 0 || value > WL1271_PSM_MAX) {
+		wl1271_warning("sleep_auth must be between 0 and %d",
+			       WL1271_PSM_MAX);
+		return -ERANGE;
+	}
+
+	mutex_lock(&wl->mutex);
+
+	wl->conf.conn.sta_sleep_auth = value;
+
+	if (wl->state == WL1271_STATE_OFF) {
+		/* this will show up on "read" in case we are off */
+		wl->sleep_auth = value;
+		goto out;
 	}
 
-	DEBUGFS_FWSTATS_ADD(tx, internal_desc_overflow);
-
-	DEBUGFS_FWSTATS_ADD(rx, out_of_mem);
-	DEBUGFS_FWSTATS_ADD(rx, hdr_overflow);
-	DEBUGFS_FWSTATS_ADD(rx, hw_stuck);
-	DEBUGFS_FWSTATS_ADD(rx, dropped);
-	DEBUGFS_FWSTATS_ADD(rx, fcs_err);
-	DEBUGFS_FWSTATS_ADD(rx, xfr_hint_trig);
-	DEBUGFS_FWSTATS_ADD(rx, path_reset);
-	DEBUGFS_FWSTATS_ADD(rx, reset_counter);
-
-	DEBUGFS_FWSTATS_ADD(dma, rx_requested);
-	DEBUGFS_FWSTATS_ADD(dma, rx_errors);
-	DEBUGFS_FWSTATS_ADD(dma, tx_requested);
-	DEBUGFS_FWSTATS_ADD(dma, tx_errors);
-
-	DEBUGFS_FWSTATS_ADD(isr, cmd_cmplt);
-	DEBUGFS_FWSTATS_ADD(isr, fiqs);
-	DEBUGFS_FWSTATS_ADD(isr, rx_headers);
-	DEBUGFS_FWSTATS_ADD(isr, rx_mem_overflow);
-	DEBUGFS_FWSTATS_ADD(isr, rx_rdys);
-	DEBUGFS_FWSTATS_ADD(isr, irqs);
-	DEBUGFS_FWSTATS_ADD(isr, tx_procs);
-	DEBUGFS_FWSTATS_ADD(isr, decrypt_done);
-	DEBUGFS_FWSTATS_ADD(isr, dma0_done);
-	DEBUGFS_FWSTATS_ADD(isr, dma1_done);
-	DEBUGFS_FWSTATS_ADD(isr, tx_exch_complete);
-	DEBUGFS_FWSTATS_ADD(isr, commands);
-	DEBUGFS_FWSTATS_ADD(isr, rx_procs);
-	DEBUGFS_FWSTATS_ADD(isr, hw_pm_mode_changes);
-	DEBUGFS_FWSTATS_ADD(isr, host_acknowledges);
-	DEBUGFS_FWSTATS_ADD(isr, pci_pm);
-	DEBUGFS_FWSTATS_ADD(isr, wakeups);
-	DEBUGFS_FWSTATS_ADD(isr, low_rssi);
-
-	DEBUGFS_FWSTATS_ADD(wep, addr_key_count);
-	DEBUGFS_FWSTATS_ADD(wep, default_key_count);
-	/* skipping wep.reserved */
-	DEBUGFS_FWSTATS_ADD(wep, key_not_found);
-	DEBUGFS_FWSTATS_ADD(wep, decrypt_fail);
-	DEBUGFS_FWSTATS_ADD(wep, packets);
-	DEBUGFS_FWSTATS_ADD(wep, interrupt);
-
-	DEBUGFS_FWSTATS_ADD(pwr, ps_enter);
-	DEBUGFS_FWSTATS_ADD(pwr, elp_enter);
-	DEBUGFS_FWSTATS_ADD(pwr, missing_bcns);
-	DEBUGFS_FWSTATS_ADD(pwr, wake_on_host);
-	DEBUGFS_FWSTATS_ADD(pwr, wake_on_timer_exp);
-	DEBUGFS_FWSTATS_ADD(pwr, tx_with_ps);
-	DEBUGFS_FWSTATS_ADD(pwr, tx_without_ps);
-	DEBUGFS_FWSTATS_ADD(pwr, rcvd_beacons);
-	DEBUGFS_FWSTATS_ADD(pwr, power_save_off);
-	DEBUGFS_FWSTATS_ADD(pwr, enable_ps);
-	DEBUGFS_FWSTATS_ADD(pwr, disable_ps);
-	DEBUGFS_FWSTATS_ADD(pwr, fix_tsf_ps);
-	/* skipping cont_miss_bcns_spread for now */
-	DEBUGFS_FWSTATS_ADD(pwr, rcvd_awake_beacons);
-
-	DEBUGFS_FWSTATS_ADD(mic, rx_pkts);
-	DEBUGFS_FWSTATS_ADD(mic, calc_failure);
-
-	DEBUGFS_FWSTATS_ADD(aes, encrypt_fail);
-	DEBUGFS_FWSTATS_ADD(aes, decrypt_fail);
-	DEBUGFS_FWSTATS_ADD(aes, encrypt_packets);
-	DEBUGFS_FWSTATS_ADD(aes, decrypt_packets);
-	DEBUGFS_FWSTATS_ADD(aes, encrypt_interrupt);
-	DEBUGFS_FWSTATS_ADD(aes, decrypt_interrupt);
-
-	DEBUGFS_FWSTATS_ADD(event, heart_beat);
-	DEBUGFS_FWSTATS_ADD(event, calibration);
-	DEBUGFS_FWSTATS_ADD(event, rx_mismatch);
-	DEBUGFS_FWSTATS_ADD(event, rx_mem_empty);
-	DEBUGFS_FWSTATS_ADD(event, rx_pool);
-	DEBUGFS_FWSTATS_ADD(event, oom_late);
-	DEBUGFS_FWSTATS_ADD(event, phy_transmit_error);
-	DEBUGFS_FWSTATS_ADD(event, tx_stuck);
-
-	DEBUGFS_FWSTATS_ADD(ps, pspoll_timeouts);
-	DEBUGFS_FWSTATS_ADD(ps, upsd_timeouts);
-	DEBUGFS_FWSTATS_ADD(ps, upsd_max_sptime);
-	DEBUGFS_FWSTATS_ADD(ps, upsd_max_apturn);
-	DEBUGFS_FWSTATS_ADD(ps, pspoll_max_apturn);
-	DEBUGFS_FWSTATS_ADD(ps, pspoll_utilization);
-	DEBUGFS_FWSTATS_ADD(ps, upsd_utilization);
-
-	DEBUGFS_FWSTATS_ADD(rxpipe, rx_prep_beacon_drop);
-	DEBUGFS_FWSTATS_ADD(rxpipe, descr_host_int_trig_rx_data);
-	DEBUGFS_FWSTATS_ADD(rxpipe, beacon_buffer_thres_host_int_trig_rx_data);
-	DEBUGFS_FWSTATS_ADD(rxpipe, missed_beacon_host_int_trig_rx_data);
-	DEBUGFS_FWSTATS_ADD(rxpipe, tx_xfr_host_int_trig_rx_data);
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl1271_acx_sleep_auth(wl, value);
+	if (ret < 0)
+		goto out_sleep;
+
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+	return count;
+}
+
+static const struct file_operations sleep_auth_ops = {
+	.read = sleep_auth_read,
+	.write = sleep_auth_write,
+	.open = simple_open,
+	.llseek = default_llseek,
+};
+
+static ssize_t dev_mem_read(struct file *file,
+	     char __user *user_buf, size_t count,
+	     loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	struct wlcore_partition_set part, old_part;
+	size_t bytes = count;
+	int ret;
+	char *buf;
+
+	/* only requests of dword-aligned size and offset are supported */
+	if (bytes % 4)
+		return -EINVAL;
+
+	if (*ppos % 4)
+		return -EINVAL;
+
+	/* function should return in reasonable time */
+	bytes = min(bytes, WLCORE_MAX_BLOCK_SIZE);
+
+	if (bytes == 0)
+		return -EINVAL;
+
+	memset(&part, 0, sizeof(part));
+	part.mem.start = file->f_pos;
+	part.mem.size = bytes;
+
+	buf = kmalloc(bytes, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	mutex_lock(&wl->mutex);
+
+	if (wl->state == WL1271_STATE_OFF) {
+		ret = -EFAULT;
+		goto skip_read;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto skip_read;
+
+	/* store current partition and switch partition */
+	memcpy(&old_part, &wl->curr_part, sizeof(old_part));
+	ret = wlcore_set_partition(wl, &part);
+	if (ret < 0)
+		goto part_err;
+
+	ret = wlcore_raw_read(wl, 0, buf, bytes, false);
+	if (ret < 0)
+		goto read_err;
+
+read_err:
+	/* recover partition */
+	ret = wlcore_set_partition(wl, &old_part);
+	if (ret < 0)
+		goto part_err;
+
+part_err:
+	wl1271_ps_elp_sleep(wl);
+
+skip_read:
+	mutex_unlock(&wl->mutex);
+
+	if (ret == 0) {
+		ret = copy_to_user(user_buf, buf, bytes);
+		if (ret < bytes) {
+			bytes -= ret;
+			*ppos += bytes;
+			ret = 0;
+		} else {
+			ret = -EFAULT;
+		}
+	}
+
+	kfree(buf);
+
+	return ((ret == 0) ? bytes : ret);
+}
+
+static ssize_t dev_mem_write(struct file *file, const char __user *user_buf,
+		size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	struct wlcore_partition_set part, old_part;
+	size_t bytes = count;
+	int ret;
+	char *buf;
+
+	/* only requests of dword-aligned size and offset are supported */
+	if (bytes % 4)
+		return -EINVAL;
+
+	if (*ppos % 4)
+		return -EINVAL;
+
+	/* function should return in reasonable time */
+	bytes = min(bytes, WLCORE_MAX_BLOCK_SIZE);
+
+	if (bytes == 0)
+		return -EINVAL;
+
+	memset(&part, 0, sizeof(part));
+	part.mem.start = file->f_pos;
+	part.mem.size = bytes;
+
+	buf = kmalloc(bytes, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	ret = copy_from_user(buf, user_buf, bytes);
+	if (ret) {
+		ret = -EFAULT;
+		goto err_out;
+	}
+
+	mutex_lock(&wl->mutex);
+
+	if (wl->state == WL1271_STATE_OFF) {
+		ret = -EFAULT;
+		goto skip_write;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto skip_write;
+
+	/* store current partition and switch partition */
+	memcpy(&old_part, &wl->curr_part, sizeof(old_part));
+	ret = wlcore_set_partition(wl, &part);
+	if (ret < 0)
+		goto part_err;
+
+	ret = wlcore_raw_write(wl, 0, buf, bytes, false);
+	if (ret < 0)
+		goto write_err;
+
+write_err:
+	/* recover partition */
+	ret = wlcore_set_partition(wl, &old_part);
+	if (ret < 0)
+		goto part_err;
+
+part_err:
+	wl1271_ps_elp_sleep(wl);
+
+skip_write:
+	mutex_unlock(&wl->mutex);
+
+	if (ret == 0)
+		*ppos += bytes;
+
+err_out:
+	kfree(buf);
+
+	return ((ret == 0) ? bytes : ret);
+}
+
+static loff_t dev_mem_seek(struct file *file, loff_t offset, int orig)
+{
+	loff_t ret;
+
+	/* only requests of dword-aligned size and offset are supported */
+	if (offset % 4)
+		return -EINVAL;
+
+	switch (orig) {
+	case SEEK_SET:
+		file->f_pos = offset;
+		ret = file->f_pos;
+		break;
+	case SEEK_CUR:
+		file->f_pos += offset;
+		ret = file->f_pos;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct file_operations dev_mem_ops = {
+	.open = simple_open,
+	.read = dev_mem_read,
+	.write = dev_mem_write,
+	.llseek = dev_mem_seek,
+};
+
+static int wl1271_debugfs_add_files(struct wl1271 *wl,
+				    struct dentry *rootdir)
+{
+	int ret = 0;
+	struct dentry *entry, *streaming;
 
 	DEBUGFS_ADD(tx_queue_len, rootdir);
 	DEBUGFS_ADD(retry_count, rootdir);
@@ -1120,6 +1237,11 @@ static int wl1271_debugfs_add_files(struct wl1271 *wl,
 	DEBUGFS_ADD(dynamic_ps_timeout, rootdir);
 	DEBUGFS_ADD(forced_ps, rootdir);
 	DEBUGFS_ADD(split_scan_timeout, rootdir);
+	DEBUGFS_ADD(irq_pkt_threshold, rootdir);
+	DEBUGFS_ADD(irq_blk_threshold, rootdir);
+	DEBUGFS_ADD(irq_timeout, rootdir);
+	DEBUGFS_ADD(fw_stats_raw, rootdir);
+	DEBUGFS_ADD(sleep_auth, rootdir);
 
 	streaming = debugfs_create_dir("rx_streaming", rootdir);
 	if (!streaming || IS_ERR(streaming))
@@ -1128,6 +1250,7 @@ static int wl1271_debugfs_add_files(struct wl1271 *wl,
 	DEBUGFS_ADD_PREFIX(rx_streaming, interval, streaming);
 	DEBUGFS_ADD_PREFIX(rx_streaming, always, streaming);
 
+	DEBUGFS_ADD_PREFIX(dev, mem, rootdir);
 
 	return 0;
 
@@ -1145,7 +1268,7 @@ void wl1271_debugfs_reset(struct wl1271 *wl)
 	if (!wl->stats.fw_stats)
 		return;
 
-	memset(wl->stats.fw_stats, 0, sizeof(*wl->stats.fw_stats));
+	memset(wl->stats.fw_stats, 0, wl->stats.fw_stats_len);
 	wl->stats.retry_count = 0;
 	wl->stats.excessive_retries = 0;
 }
@@ -1160,34 +1283,34 @@ int wl1271_debugfs_init(struct wl1271 *wl)
 
 	if (IS_ERR(rootdir)) {
 		ret = PTR_ERR(rootdir);
-		goto err;
+		goto out;
 	}
 
-	wl->stats.fw_stats = kzalloc(sizeof(*wl->stats.fw_stats),
-				      GFP_KERNEL);
-
+	wl->stats.fw_stats = kzalloc(wl->stats.fw_stats_len, GFP_KERNEL);
 	if (!wl->stats.fw_stats) {
 		ret = -ENOMEM;
-		goto err_fw;
+		goto out_remove;
 	}
 
 	wl->stats.fw_stats_update = jiffies;
 
 	ret = wl1271_debugfs_add_files(wl, rootdir);
+	if (ret < 0)
+		goto out_exit;
 
+	ret = wlcore_debugfs_init(wl, rootdir);
 	if (ret < 0)
-		goto err_file;
+		goto out_exit;
 
-	return 0;
+	goto out;
 
-err_file:
-	kfree(wl->stats.fw_stats);
-	wl->stats.fw_stats = NULL;
+out_exit:
+	wl1271_debugfs_exit(wl);
 
-err_fw:
+out_remove:
 	debugfs_remove_recursive(rootdir);
 
-err:
+out:
 	return ret;
 }
 
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.h b/drivers/net/wireless/ti/wlcore/debugfs.h
index a8d3aef011f..f7381dd6900 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.h
+++ b/drivers/net/wireless/ti/wlcore/debugfs.h
@@ -26,8 +26,95 @@
 
 #include "wlcore.h"
 
+int wl1271_format_buffer(char __user *userbuf, size_t count,
+			 loff_t *ppos, char *fmt, ...);
+
 int wl1271_debugfs_init(struct wl1271 *wl);
 void wl1271_debugfs_exit(struct wl1271 *wl);
 void wl1271_debugfs_reset(struct wl1271 *wl);
+void wl1271_debugfs_update_stats(struct wl1271 *wl);
+
+#define DEBUGFS_FORMAT_BUFFER_SIZE 256
+
+#define DEBUGFS_READONLY_FILE(name, fmt, value...)			\
+static ssize_t name## _read(struct file *file, char __user *userbuf,	\
+			    size_t count, loff_t *ppos)			\
+{									\
+	struct wl1271 *wl = file->private_data;				\
+	return wl1271_format_buffer(userbuf, count, ppos,		\
+				    fmt "\n", ##value);			\
+}									\
+									\
+static const struct file_operations name## _ops = {			\
+	.read = name## _read,						\
+	.open = simple_open,						\
+	.llseek	= generic_file_llseek,					\
+};
+
+#define DEBUGFS_ADD(name, parent)					\
+	do {								\
+		entry = debugfs_create_file(#name, 0400, parent,	\
+					    wl, &name## _ops);		\
+		if (!entry || IS_ERR(entry))				\
+			goto err;					\
+	} while (0);
+
+
+#define DEBUGFS_ADD_PREFIX(prefix, name, parent)			\
+	do {								\
+		entry = debugfs_create_file(#name, 0400, parent,	\
+				    wl, &prefix## _## name## _ops);	\
+		if (!entry || IS_ERR(entry))				\
+			goto err;					\
+	} while (0);
+
+#define DEBUGFS_FWSTATS_FILE(sub, name, fmt, struct_type)		\
+static ssize_t sub## _ ##name## _read(struct file *file,		\
+				      char __user *userbuf,		\
+				      size_t count, loff_t *ppos)	\
+{									\
+	struct wl1271 *wl = file->private_data;				\
+	struct struct_type *stats = wl->stats.fw_stats;			\
+									\
+	wl1271_debugfs_update_stats(wl);				\
+									\
+	return wl1271_format_buffer(userbuf, count, ppos, fmt "\n",	\
+				    stats->sub.name);			\
+}									\
+									\
+static const struct file_operations sub## _ ##name## _ops = {		\
+	.read = sub## _ ##name## _read,					\
+	.open = simple_open,						\
+	.llseek	= generic_file_llseek,					\
+};
+
+#define DEBUGFS_FWSTATS_FILE_ARRAY(sub, name, len, struct_type)		\
+static ssize_t sub## _ ##name## _read(struct file *file,		\
+				      char __user *userbuf,		\
+				      size_t count, loff_t *ppos)	\
+{									\
+	struct wl1271 *wl = file->private_data;				\
+	struct struct_type *stats = wl->stats.fw_stats;			\
+	char buf[DEBUGFS_FORMAT_BUFFER_SIZE] = "";			\
+	int res, i;							\
+									\
+	wl1271_debugfs_update_stats(wl);				\
+									\
+	for (i = 0; i < len; i++)					\
+		res = snprintf(buf, sizeof(buf), "%s[%d] = %d\n",	\
+			       buf, i, stats->sub.name[i]);		\
+									\
+	return wl1271_format_buffer(userbuf, count, ppos, "%s", buf);	\
+}									\
+									\
+static const struct file_operations sub## _ ##name## _ops = {		\
+	.read = sub## _ ##name## _read,					\
+	.open = simple_open,						\
+	.llseek	= generic_file_llseek,					\
+};
+
+#define DEBUGFS_FWSTATS_ADD(sub, name)					\
+	DEBUGFS_ADD(sub## _ ##name, stats)
+
 
 #endif /* WL1271_DEBUGFS_H */
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index 28e2a633c3b..48907054d49 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -105,6 +105,7 @@ static int wl1271_event_process(struct wl1271 *wl)
 	u32 vector;
 	bool disconnect_sta = false;
 	unsigned long sta_bitmap = 0;
+	int ret;
 
 	wl1271_event_mbox_dump(mbox);
 
@@ -148,15 +149,33 @@ static int wl1271_event_process(struct wl1271 *wl)
 		int delay = wl->conf.conn.synch_fail_thold *
 					wl->conf.conn.bss_lose_timeout;
 		wl1271_info("Beacon loss detected.");
-		cancel_delayed_work_sync(&wl->connection_loss_work);
+
+		/*
+		 * if the work is already queued, it should take place. We
+		 * don't want to delay the connection loss indication
+		 * any more.
+		 */
 		ieee80211_queue_delayed_work(wl->hw, &wl->connection_loss_work,
-		      msecs_to_jiffies(delay));
+					     msecs_to_jiffies(delay));
+
+		wl12xx_for_each_wlvif_sta(wl, wlvif) {
+			vif = wl12xx_wlvif_to_vif(wlvif);
+
+			ieee80211_cqm_rssi_notify(
+					vif,
+					NL80211_CQM_RSSI_BEACON_LOSS_EVENT,
+					GFP_KERNEL);
+		}
 	}
 
 	if (vector & REGAINED_BSS_EVENT_ID) {
 		/* TODO: check for multi-role */
 		wl1271_info("Beacon regained.");
-		cancel_delayed_work_sync(&wl->connection_loss_work);
+		cancel_delayed_work(&wl->connection_loss_work);
+
+		/* sanity check - we can't lose and gain the beacon together */
+		WARN(vector & BSS_LOSE_EVENT_ID,
+		     "Concurrent beacon loss and gain from FW");
 	}
 
 	if (vector & RSSI_SNR_TRIGGER_0_EVENT_ID) {
@@ -210,7 +229,9 @@ static int wl1271_event_process(struct wl1271 *wl)
 
 	if ((vector & DUMMY_PACKET_EVENT_ID)) {
 		wl1271_debug(DEBUG_EVENT, "DUMMY_PACKET_ID_EVENT_ID");
-		wl1271_tx_dummy_packet(wl);
+		ret = wl1271_tx_dummy_packet(wl);
+		if (ret < 0)
+			return ret;
 	}
 
 	/*
@@ -283,8 +304,10 @@ int wl1271_event_handle(struct wl1271 *wl, u8 mbox_num)
 		return -EINVAL;
 
 	/* first we read the mbox descriptor */
-	wl1271_read(wl, wl->mbox_ptr[mbox_num], wl->mbox,
-		    sizeof(*wl->mbox), false);
+	ret = wlcore_read(wl, wl->mbox_ptr[mbox_num], wl->mbox,
+			  sizeof(*wl->mbox), false);
+	if (ret < 0)
+		return ret;
 
 	/* process the descriptor */
 	ret = wl1271_event_process(wl);
@@ -295,7 +318,7 @@ int wl1271_event_handle(struct wl1271 *wl, u8 mbox_num)
 	 * TODO: we just need this because one bit is in a different
 	 * place.  Is there any better way?
 	 */
-	wl->ops->ack_event(wl);
+	ret = wl->ops->ack_event(wl);
 
-	return 0;
+	return ret;
 }
diff --git a/drivers/net/wireless/ti/wlcore/hw_ops.h b/drivers/net/wireless/ti/wlcore/hw_ops.h
index 9384b4d56c2..2673d783ec1 100644
--- a/drivers/net/wireless/ti/wlcore/hw_ops.h
+++ b/drivers/net/wireless/ti/wlcore/hw_ops.h
@@ -65,11 +65,13 @@ wlcore_hw_get_rx_buf_align(struct wl1271 *wl, u32 rx_desc)
 	return wl->ops->get_rx_buf_align(wl, rx_desc);
 }
 
-static inline void
+static inline int
 wlcore_hw_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
 {
 	if (wl->ops->prepare_read)
-		wl->ops->prepare_read(wl, rx_desc, len);
+		return wl->ops->prepare_read(wl, rx_desc, len);
+
+	return 0;
 }
 
 static inline u32
@@ -81,10 +83,12 @@ wlcore_hw_get_rx_packet_len(struct wl1271 *wl, void *rx_data, u32 data_len)
 	return wl->ops->get_rx_packet_len(wl, rx_data, data_len);
 }
 
-static inline void wlcore_hw_tx_delayed_compl(struct wl1271 *wl)
+static inline int wlcore_hw_tx_delayed_compl(struct wl1271 *wl)
 {
 	if (wl->ops->tx_delayed_compl)
-		wl->ops->tx_delayed_compl(wl);
+		return wl->ops->tx_delayed_compl(wl);
+
+	return 0;
 }
 
 static inline void wlcore_hw_tx_immediate_compl(struct wl1271 *wl)
@@ -119,4 +123,82 @@ static inline int wlcore_identify_fw(struct wl1271 *wl)
 	return 0;
 }
 
+static inline void
+wlcore_hw_set_tx_desc_csum(struct wl1271 *wl,
+			   struct wl1271_tx_hw_descr *desc,
+			   struct sk_buff *skb)
+{
+	if (!wl->ops->set_tx_desc_csum)
+		BUG_ON(1);
+
+	wl->ops->set_tx_desc_csum(wl, desc, skb);
+}
+
+static inline void
+wlcore_hw_set_rx_csum(struct wl1271 *wl,
+		      struct wl1271_rx_descriptor *desc,
+		      struct sk_buff *skb)
+{
+	if (wl->ops->set_rx_csum)
+		wl->ops->set_rx_csum(wl, desc, skb);
+}
+
+static inline u32
+wlcore_hw_ap_get_mimo_wide_rate_mask(struct wl1271 *wl,
+				     struct wl12xx_vif *wlvif)
+{
+	if (wl->ops->ap_get_mimo_wide_rate_mask)
+		return wl->ops->ap_get_mimo_wide_rate_mask(wl, wlvif);
+
+	return 0;
+}
+
+static inline int
+wlcore_debugfs_init(struct wl1271 *wl, struct dentry *rootdir)
+{
+	if (wl->ops->debugfs_init)
+		return wl->ops->debugfs_init(wl, rootdir);
+
+	return 0;
+}
+
+static inline int
+wlcore_handle_static_data(struct wl1271 *wl, void *static_data)
+{
+	if (wl->ops->handle_static_data)
+		return wl->ops->handle_static_data(wl, static_data);
+
+	return 0;
+}
+
+static inline int
+wlcore_hw_get_spare_blocks(struct wl1271 *wl, bool is_gem)
+{
+	if (!wl->ops->get_spare_blocks)
+		BUG_ON(1);
+
+	return wl->ops->get_spare_blocks(wl, is_gem);
+}
+
+static inline int
+wlcore_hw_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
+		  struct ieee80211_vif *vif,
+		  struct ieee80211_sta *sta,
+		  struct ieee80211_key_conf *key_conf)
+{
+	if (!wl->ops->set_key)
+		BUG_ON(1);
+
+	return wl->ops->set_key(wl, cmd, vif, sta, key_conf);
+}
+
+static inline u32
+wlcore_hw_pre_pkt_send(struct wl1271 *wl, u32 buf_offset, u32 last_len)
+{
+	if (wl->ops->pre_pkt_send)
+		return wl->ops->pre_pkt_send(wl, buf_offset, last_len);
+
+	return buf_offset;
+}
+
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/ini.h b/drivers/net/wireless/ti/wlcore/ini.h
index 4cf9ecc5621..d24fe3bbc67 100644
--- a/drivers/net/wireless/ti/wlcore/ini.h
+++ b/drivers/net/wireless/ti/wlcore/ini.h
@@ -172,7 +172,19 @@ struct wl128x_ini_fem_params_5 {
 
 /* NVS data structure */
 #define WL1271_INI_NVS_SECTION_SIZE		     468
-#define WL1271_INI_FEM_MODULE_COUNT                  2
+
+/* We have four FEM module types: 0-RFMD, 1-TQS, 2-SKW, 3-TQS_HP */
+#define WL1271_INI_FEM_MODULE_COUNT                  4
+
+/*
+ * In NVS we only store two FEM module entries -
+ *	  FEM modules 0,2,3 are stored in entry 0
+ *	  FEM module 1 is stored in entry 1
+ */
+#define WL12XX_NVS_FEM_MODULE_COUNT                  2
+
+#define WL12XX_FEM_TO_NVS_ENTRY(ini_fem_module)      \
+	((ini_fem_module) == 1 ? 1 : 0)
 
 #define WL1271_INI_LEGACY_NVS_FILE_SIZE              800
 
@@ -188,13 +200,13 @@ struct wl1271_nvs_file {
 	struct {
 		struct wl1271_ini_fem_params_2 params;
 		u8 padding;
-	} dyn_radio_params_2[WL1271_INI_FEM_MODULE_COUNT];
+	} dyn_radio_params_2[WL12XX_NVS_FEM_MODULE_COUNT];
 	struct wl1271_ini_band_params_5 stat_radio_params_5;
 	u8 padding3;
 	struct {
 		struct wl1271_ini_fem_params_5 params;
 		u8 padding;
-	} dyn_radio_params_5[WL1271_INI_FEM_MODULE_COUNT];
+	} dyn_radio_params_5[WL12XX_NVS_FEM_MODULE_COUNT];
 } __packed;
 
 struct wl128x_nvs_file {
@@ -209,12 +221,12 @@ struct wl128x_nvs_file {
 	struct {
 		struct wl128x_ini_fem_params_2 params;
 		u8 padding;
-	} dyn_radio_params_2[WL1271_INI_FEM_MODULE_COUNT];
+	} dyn_radio_params_2[WL12XX_NVS_FEM_MODULE_COUNT];
 	struct wl128x_ini_band_params_5 stat_radio_params_5;
 	u8 padding3;
 	struct {
 		struct wl128x_ini_fem_params_5 params;
 		u8 padding;
-	} dyn_radio_params_5[WL1271_INI_FEM_MODULE_COUNT];
+	} dyn_radio_params_5[WL12XX_NVS_FEM_MODULE_COUNT];
 } __packed;
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/init.c b/drivers/net/wireless/ti/wlcore/init.c
index 9f89255eb6e..a3c867786df 100644
--- a/drivers/net/wireless/ti/wlcore/init.c
+++ b/drivers/net/wireless/ti/wlcore/init.c
@@ -54,6 +54,22 @@ int wl1271_init_templates_config(struct wl1271 *wl)
 	if (ret < 0)
 		return ret;
 
+	if (wl->quirks & WLCORE_QUIRK_DUAL_PROBE_TMPL) {
+		ret = wl1271_cmd_template_set(wl, WL12XX_INVALID_ROLE_ID,
+					      CMD_TEMPL_APP_PROBE_REQ_2_4, NULL,
+					      WL1271_CMD_TEMPL_MAX_SIZE,
+					      0, WL1271_RATE_AUTOMATIC);
+		if (ret < 0)
+			return ret;
+
+		ret = wl1271_cmd_template_set(wl, WL12XX_INVALID_ROLE_ID,
+					      CMD_TEMPL_APP_PROBE_REQ_5, NULL,
+					      WL1271_CMD_TEMPL_MAX_SIZE,
+					      0, WL1271_RATE_AUTOMATIC);
+		if (ret < 0)
+			return ret;
+	}
+
 	ret = wl1271_cmd_template_set(wl, WL12XX_INVALID_ROLE_ID,
 				      CMD_TEMPL_NULL_DATA, NULL,
 				      sizeof(struct wl12xx_null_data_template),
@@ -460,6 +476,9 @@ int wl1271_init_ap_rates(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	/* unconditionally enable HT rates */
 	supported_rates |= CONF_TX_MCS_RATES;
 
+	/* get extra MIMO or wide-chan rates where the HW supports it */
+	supported_rates |= wlcore_hw_ap_get_mimo_wide_rate_mask(wl, wlvif);
+
 	/* configure unicast TX rate classes */
 	for (i = 0; i < wl->conf.tx.ac_conf_count; i++) {
 		rc.enabled_rates = supported_rates;
@@ -551,29 +570,28 @@ int wl1271_init_vif_specific(struct wl1271 *wl, struct ieee80211_vif *vif)
 	bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
 	int ret, i;
 
-	/*
-	 * consider all existing roles before configuring psm.
-	 * TODO: reconfigure on interface removal.
-	 */
-	if (!wl->ap_count) {
-		if (is_ap) {
-			/* Configure for power always on */
+	/* consider all existing roles before configuring psm. */
+
+	if (wl->ap_count == 0 && is_ap) { /* first AP */
+		/* Configure for power always on */
+		ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+		if (ret < 0)
+			return ret;
+	/* first STA, no APs */
+	} else if (wl->sta_count == 0 && wl->ap_count == 0 && !is_ap) {
+		u8 sta_auth = wl->conf.conn.sta_sleep_auth;
+		/* Configure for power according to debugfs */
+		if (sta_auth != WL1271_PSM_ILLEGAL)
+			ret = wl1271_acx_sleep_auth(wl, sta_auth);
+		/* Configure for power always on */
+		else if (wl->quirks & WLCORE_QUIRK_NO_ELP)
 			ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
-			if (ret < 0)
-				return ret;
-		} else if (!wl->sta_count) {
-			if (wl->quirks & WLCORE_QUIRK_NO_ELP) {
-				/* Configure for power always on */
-				ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
-				if (ret < 0)
-					return ret;
-			} else {
-				/* Configure for ELP power saving */
-				ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
-				if (ret < 0)
-					return ret;
-			}
-		}
+		/* Configure for ELP power saving */
+		else
+			ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
+
+		if (ret < 0)
+			return ret;
 	}
 
 	/* Mode specific init */
diff --git a/drivers/net/wireless/ti/wlcore/io.c b/drivers/net/wireless/ti/wlcore/io.c
index 7cd0081aede..68e74eefd29 100644
--- a/drivers/net/wireless/ti/wlcore/io.c
+++ b/drivers/net/wireless/ti/wlcore/io.c
@@ -48,12 +48,24 @@ void wlcore_disable_interrupts(struct wl1271 *wl)
 }
 EXPORT_SYMBOL_GPL(wlcore_disable_interrupts);
 
+void wlcore_disable_interrupts_nosync(struct wl1271 *wl)
+{
+	disable_irq_nosync(wl->irq);
+}
+EXPORT_SYMBOL_GPL(wlcore_disable_interrupts_nosync);
+
 void wlcore_enable_interrupts(struct wl1271 *wl)
 {
 	enable_irq(wl->irq);
 }
 EXPORT_SYMBOL_GPL(wlcore_enable_interrupts);
 
+void wlcore_synchronize_interrupts(struct wl1271 *wl)
+{
+	synchronize_irq(wl->irq);
+}
+EXPORT_SYMBOL_GPL(wlcore_synchronize_interrupts);
+
 int wlcore_translate_addr(struct wl1271 *wl, int addr)
 {
 	struct wlcore_partition_set *part = &wl->curr_part;
@@ -122,9 +134,11 @@ EXPORT_SYMBOL_GPL(wlcore_translate_addr);
  *                                    |    |
  *
  */
-void wlcore_set_partition(struct wl1271 *wl,
-			  const struct wlcore_partition_set *p)
+int wlcore_set_partition(struct wl1271 *wl,
+			 const struct wlcore_partition_set *p)
 {
+	int ret;
+
 	/* copy partition info */
 	memcpy(&wl->curr_part, p, sizeof(*p));
 
@@ -137,28 +151,41 @@ void wlcore_set_partition(struct wl1271 *wl,
 	wl1271_debug(DEBUG_IO, "mem3_start %08X mem3_size %08X",
 		     p->mem3.start, p->mem3.size);
 
-	wl1271_raw_write32(wl, HW_PART0_START_ADDR, p->mem.start);
-	wl1271_raw_write32(wl, HW_PART0_SIZE_ADDR, p->mem.size);
-	wl1271_raw_write32(wl, HW_PART1_START_ADDR, p->reg.start);
-	wl1271_raw_write32(wl, HW_PART1_SIZE_ADDR, p->reg.size);
-	wl1271_raw_write32(wl, HW_PART2_START_ADDR, p->mem2.start);
-	wl1271_raw_write32(wl, HW_PART2_SIZE_ADDR, p->mem2.size);
+	ret = wlcore_raw_write32(wl, HW_PART0_START_ADDR, p->mem.start);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_raw_write32(wl, HW_PART0_SIZE_ADDR, p->mem.size);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_raw_write32(wl, HW_PART1_START_ADDR, p->reg.start);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_raw_write32(wl, HW_PART1_SIZE_ADDR, p->reg.size);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_raw_write32(wl, HW_PART2_START_ADDR, p->mem2.start);
+	if (ret < 0)
+		goto out;
+
+	ret = wlcore_raw_write32(wl, HW_PART2_SIZE_ADDR, p->mem2.size);
+	if (ret < 0)
+		goto out;
+
 	/*
 	 * We don't need the size of the last partition, as it is
 	 * automatically calculated based on the total memory size and
 	 * the sizes of the previous partitions.
 	 */
-	wl1271_raw_write32(wl, HW_PART3_START_ADDR, p->mem3.start);
-}
-EXPORT_SYMBOL_GPL(wlcore_set_partition);
-
-void wlcore_select_partition(struct wl1271 *wl, u8 part)
-{
-	wl1271_debug(DEBUG_IO, "setting partition %d", part);
+	ret = wlcore_raw_write32(wl, HW_PART3_START_ADDR, p->mem3.start);
 
-	wlcore_set_partition(wl, &wl->ptable[part]);
+out:
+	return ret;
 }
-EXPORT_SYMBOL_GPL(wlcore_select_partition);
+EXPORT_SYMBOL_GPL(wlcore_set_partition);
 
 void wl1271_io_reset(struct wl1271 *wl)
 {
diff --git a/drivers/net/wireless/ti/wlcore/io.h b/drivers/net/wireless/ti/wlcore/io.h
index 8942954b56a..259149f36fa 100644
--- a/drivers/net/wireless/ti/wlcore/io.h
+++ b/drivers/net/wireless/ti/wlcore/io.h
@@ -45,86 +45,122 @@
 struct wl1271;
 
 void wlcore_disable_interrupts(struct wl1271 *wl);
+void wlcore_disable_interrupts_nosync(struct wl1271 *wl);
 void wlcore_enable_interrupts(struct wl1271 *wl);
+void wlcore_synchronize_interrupts(struct wl1271 *wl);
 
 void wl1271_io_reset(struct wl1271 *wl);
 void wl1271_io_init(struct wl1271 *wl);
 int wlcore_translate_addr(struct wl1271 *wl, int addr);
 
 /* Raw target IO, address is not translated */
-static inline void wl1271_raw_write(struct wl1271 *wl, int addr, void *buf,
-				    size_t len, bool fixed)
+static inline int __must_check wlcore_raw_write(struct wl1271 *wl, int addr,
+						void *buf, size_t len,
+						bool fixed)
 {
-	wl->if_ops->write(wl->dev, addr, buf, len, fixed);
+	int ret;
+
+	if (test_bit(WL1271_FLAG_IO_FAILED, &wl->flags))
+		return -EIO;
+
+	ret = wl->if_ops->write(wl->dev, addr, buf, len, fixed);
+	if (ret && wl->state != WL1271_STATE_OFF)
+		set_bit(WL1271_FLAG_IO_FAILED, &wl->flags);
+
+	return ret;
 }
 
-static inline void wl1271_raw_read(struct wl1271 *wl, int addr, void *buf,
-				   size_t len, bool fixed)
+static inline int __must_check wlcore_raw_read(struct wl1271 *wl, int addr,
+					       void *buf, size_t len,
+					       bool fixed)
 {
-	wl->if_ops->read(wl->dev, addr, buf, len, fixed);
+	int ret;
+
+	if (test_bit(WL1271_FLAG_IO_FAILED, &wl->flags))
+		return -EIO;
+
+	ret = wl->if_ops->read(wl->dev, addr, buf, len, fixed);
+	if (ret && wl->state != WL1271_STATE_OFF)
+		set_bit(WL1271_FLAG_IO_FAILED, &wl->flags);
+
+	return ret;
 }
 
-static inline void wlcore_raw_read_data(struct wl1271 *wl, int reg, void *buf,
-					size_t len, bool fixed)
+static inline int __must_check wlcore_raw_read_data(struct wl1271 *wl, int reg,
+						    void *buf, size_t len,
+						    bool fixed)
 {
-	wl1271_raw_read(wl, wl->rtable[reg], buf, len, fixed);
+	return wlcore_raw_read(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline void wlcore_raw_write_data(struct wl1271 *wl, int reg, void *buf,
-					 size_t len, bool fixed)
+static inline int __must_check wlcore_raw_write_data(struct wl1271 *wl, int reg,
+						     void *buf, size_t len,
+						     bool fixed)
 {
-	wl1271_raw_write(wl, wl->rtable[reg], buf, len, fixed);
+	return wlcore_raw_write(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline u32 wl1271_raw_read32(struct wl1271 *wl, int addr)
+static inline int __must_check wlcore_raw_read32(struct wl1271 *wl, int addr,
+						 u32 *val)
 {
-	wl1271_raw_read(wl, addr, &wl->buffer_32,
-			    sizeof(wl->buffer_32), false);
+	int ret;
+
+	ret = wlcore_raw_read(wl, addr, &wl->buffer_32,
+			      sizeof(wl->buffer_32), false);
+	if (ret < 0)
+		return ret;
+
+	if (val)
+		*val = le32_to_cpu(wl->buffer_32);
 
-	return le32_to_cpu(wl->buffer_32);
+	return 0;
 }
 
-static inline void wl1271_raw_write32(struct wl1271 *wl, int addr, u32 val)
+static inline int __must_check wlcore_raw_write32(struct wl1271 *wl, int addr,
+						  u32 val)
 {
 	wl->buffer_32 = cpu_to_le32(val);
-	wl1271_raw_write(wl, addr, &wl->buffer_32,
-			     sizeof(wl->buffer_32), false);
+	return wlcore_raw_write(wl, addr, &wl->buffer_32,
+				sizeof(wl->buffer_32), false);
 }
 
-static inline void wl1271_read(struct wl1271 *wl, int addr, void *buf,
-			       size_t len, bool fixed)
+static inline int __must_check wlcore_read(struct wl1271 *wl, int addr,
+					   void *buf, size_t len, bool fixed)
 {
 	int physical;
 
 	physical = wlcore_translate_addr(wl, addr);
 
-	wl1271_raw_read(wl, physical, buf, len, fixed);
+	return wlcore_raw_read(wl, physical, buf, len, fixed);
 }
 
-static inline void wl1271_write(struct wl1271 *wl, int addr, void *buf,
-				size_t len, bool fixed)
+static inline int __must_check wlcore_write(struct wl1271 *wl, int addr,
+					    void *buf, size_t len, bool fixed)
 {
 	int physical;
 
 	physical = wlcore_translate_addr(wl, addr);
 
-	wl1271_raw_write(wl, physical, buf, len, fixed);
+	return wlcore_raw_write(wl, physical, buf, len, fixed);
 }
 
-static inline void wlcore_write_data(struct wl1271 *wl, int reg, void *buf,
-				     size_t len, bool fixed)
+static inline int __must_check wlcore_write_data(struct wl1271 *wl, int reg,
+						 void *buf, size_t len,
+						 bool fixed)
 {
-	wl1271_write(wl, wl->rtable[reg], buf, len, fixed);
+	return wlcore_write(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline void wlcore_read_data(struct wl1271 *wl, int reg, void *buf,
-				    size_t len, bool fixed)
+static inline int __must_check wlcore_read_data(struct wl1271 *wl, int reg,
+						void *buf, size_t len,
+						bool fixed)
 {
-	wl1271_read(wl, wl->rtable[reg], buf, len, fixed);
+	return wlcore_read(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline void wl1271_read_hwaddr(struct wl1271 *wl, int hwaddr,
-				      void *buf, size_t len, bool fixed)
+static inline int __must_check wlcore_read_hwaddr(struct wl1271 *wl, int hwaddr,
+						  void *buf, size_t len,
+						  bool fixed)
 {
 	int physical;
 	int addr;
@@ -134,34 +170,47 @@ static inline void wl1271_read_hwaddr(struct wl1271 *wl, int hwaddr,
 
 	physical = wlcore_translate_addr(wl, addr);
 
-	wl1271_raw_read(wl, physical, buf, len, fixed);
+	return wlcore_raw_read(wl, physical, buf, len, fixed);
 }
 
-static inline u32 wl1271_read32(struct wl1271 *wl, int addr)
+static inline int __must_check wlcore_read32(struct wl1271 *wl, int addr,
+					     u32 *val)
 {
-	return wl1271_raw_read32(wl, wlcore_translate_addr(wl, addr));
+	return wlcore_raw_read32(wl, wlcore_translate_addr(wl, addr), val);
 }
 
-static inline void wl1271_write32(struct wl1271 *wl, int addr, u32 val)
+static inline int __must_check wlcore_write32(struct wl1271 *wl, int addr,
+					      u32 val)
 {
-	wl1271_raw_write32(wl, wlcore_translate_addr(wl, addr), val);
+	return wlcore_raw_write32(wl, wlcore_translate_addr(wl, addr), val);
 }
 
-static inline u32 wlcore_read_reg(struct wl1271 *wl, int reg)
+static inline int __must_check wlcore_read_reg(struct wl1271 *wl, int reg,
+					       u32 *val)
 {
-	return wl1271_raw_read32(wl,
-				 wlcore_translate_addr(wl, wl->rtable[reg]));
+	return wlcore_raw_read32(wl,
+				 wlcore_translate_addr(wl, wl->rtable[reg]),
+				 val);
 }
 
-static inline void wlcore_write_reg(struct wl1271 *wl, int reg, u32 val)
+static inline int __must_check wlcore_write_reg(struct wl1271 *wl, int reg,
+						u32 val)
 {
-	wl1271_raw_write32(wl, wlcore_translate_addr(wl, wl->rtable[reg]), val);
+	return wlcore_raw_write32(wl,
+				  wlcore_translate_addr(wl, wl->rtable[reg]),
+				  val);
 }
 
 static inline void wl1271_power_off(struct wl1271 *wl)
 {
-	wl->if_ops->power(wl->dev, false);
-	clear_bit(WL1271_FLAG_GPIO_POWER, &wl->flags);
+	int ret;
+
+	if (!test_bit(WL1271_FLAG_GPIO_POWER, &wl->flags))
+		return;
+
+	ret = wl->if_ops->power(wl->dev, false);
+	if (!ret)
+		clear_bit(WL1271_FLAG_GPIO_POWER, &wl->flags);
 }
 
 static inline int wl1271_power_on(struct wl1271 *wl)
@@ -173,8 +222,8 @@ static inline int wl1271_power_on(struct wl1271 *wl)
 	return ret;
 }
 
-void wlcore_set_partition(struct wl1271 *wl,
-			  const struct wlcore_partition_set *p);
+int wlcore_set_partition(struct wl1271 *wl,
+			 const struct wlcore_partition_set *p);
 
 bool wl1271_set_block_size(struct wl1271 *wl);
 
@@ -182,6 +231,4 @@ bool wl1271_set_block_size(struct wl1271 *wl);
 
 int wl1271_tx_dummy_packet(struct wl1271 *wl);
 
-void wlcore_select_partition(struct wl1271 *wl, u8 part);
-
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index acef93390d3..72548609f71 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -62,7 +62,7 @@ static bool no_recovery;
 static void __wl1271_op_remove_interface(struct wl1271 *wl,
 					 struct ieee80211_vif *vif,
 					 bool reset_tx_queues);
-static void wl1271_op_stop(struct ieee80211_hw *hw);
+static void wlcore_op_stop_locked(struct wl1271 *wl);
 static void wl1271_free_ap_keys(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 
 static int wl12xx_set_authorized(struct wl1271 *wl,
@@ -320,46 +320,6 @@ static void wlcore_adjust_conf(struct wl1271 *wl)
 	}
 }
 
-static int wl1271_plt_init(struct wl1271 *wl)
-{
-	int ret;
-
-	ret = wl->ops->hw_init(wl);
-	if (ret < 0)
-		return ret;
-
-	ret = wl1271_acx_init_mem_config(wl);
-	if (ret < 0)
-		return ret;
-
-	ret = wl12xx_acx_mem_cfg(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 CAM power saving (ie. always active) */
-	ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
-	if (ret < 0)
-		goto out_free_memmap;
-
-	/* configure PM */
-	ret = wl1271_acx_pm_config(wl);
-	if (ret < 0)
-		goto out_free_memmap;
-
-	return 0;
-
- out_free_memmap:
-	kfree(wl->target_mem_map);
-	wl->target_mem_map = NULL;
-
-	return ret;
-}
-
 static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
 					struct wl12xx_vif *wlvif,
 					u8 hlid, u8 tx_pkts)
@@ -387,7 +347,7 @@ static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
 
 static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 					   struct wl12xx_vif *wlvif,
-					   struct wl_fw_status *status)
+					   struct wl_fw_status_2 *status)
 {
 	struct wl1271_link *lnk;
 	u32 cur_fw_ps_map;
@@ -418,8 +378,9 @@ static void wl12xx_irq_update_links_status(struct wl1271 *wl,
 	}
 }
 
-static void wl12xx_fw_status(struct wl1271 *wl,
-			     struct wl_fw_status *status)
+static int wlcore_fw_status(struct wl1271 *wl,
+			    struct wl_fw_status_1 *status_1,
+			    struct wl_fw_status_2 *status_2)
 {
 	struct wl12xx_vif *wlvif;
 	struct timespec ts;
@@ -427,38 +388,42 @@ static void wl12xx_fw_status(struct wl1271 *wl,
 	int avail, freed_blocks;
 	int i;
 	size_t status_len;
+	int ret;
 
-	status_len = sizeof(*status) + wl->fw_status_priv_len;
+	status_len = WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc) +
+		sizeof(*status_2) + wl->fw_status_priv_len;
 
-	wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR, status,
-			     status_len, false);
+	ret = wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR, status_1,
+				   status_len, false);
+	if (ret < 0)
+		return ret;
 
 	wl1271_debug(DEBUG_IRQ, "intr: 0x%x (fw_rx_counter = %d, "
 		     "drv_rx_counter = %d, tx_results_counter = %d)",
-		     status->intr,
-		     status->fw_rx_counter,
-		     status->drv_rx_counter,
-		     status->tx_results_counter);
+		     status_1->intr,
+		     status_1->fw_rx_counter,
+		     status_1->drv_rx_counter,
+		     status_1->tx_results_counter);
 
 	for (i = 0; i < NUM_TX_QUEUES; i++) {
 		/* prevent wrap-around in freed-packets counter */
 		wl->tx_allocated_pkts[i] -=
-				(status->counters.tx_released_pkts[i] -
+				(status_2->counters.tx_released_pkts[i] -
 				wl->tx_pkts_freed[i]) & 0xff;
 
-		wl->tx_pkts_freed[i] = status->counters.tx_released_pkts[i];
+		wl->tx_pkts_freed[i] = status_2->counters.tx_released_pkts[i];
 	}
 
 	/* prevent wrap-around in total blocks counter */
 	if (likely(wl->tx_blocks_freed <=
-		   le32_to_cpu(status->total_released_blks)))
-		freed_blocks = le32_to_cpu(status->total_released_blks) -
+		   le32_to_cpu(status_2->total_released_blks)))
+		freed_blocks = le32_to_cpu(status_2->total_released_blks) -
 			       wl->tx_blocks_freed;
 	else
 		freed_blocks = 0x100000000LL - wl->tx_blocks_freed +
-			       le32_to_cpu(status->total_released_blks);
+			       le32_to_cpu(status_2->total_released_blks);
 
-	wl->tx_blocks_freed = le32_to_cpu(status->total_released_blks);
+	wl->tx_blocks_freed = le32_to_cpu(status_2->total_released_blks);
 
 	wl->tx_allocated_blocks -= freed_blocks;
 
@@ -474,7 +439,7 @@ static void wl12xx_fw_status(struct wl1271 *wl,
 			cancel_delayed_work(&wl->tx_watchdog_work);
 	}
 
-	avail = le32_to_cpu(status->tx_total) - wl->tx_allocated_blocks;
+	avail = le32_to_cpu(status_2->tx_total) - wl->tx_allocated_blocks;
 
 	/*
 	 * The FW might change the total number of TX memblocks before
@@ -493,13 +458,15 @@ static void wl12xx_fw_status(struct wl1271 *wl,
 
 	/* for AP update num of allocated TX blocks per link and ps status */
 	wl12xx_for_each_wlvif_ap(wl, wlvif) {
-		wl12xx_irq_update_links_status(wl, wlvif, status);
+		wl12xx_irq_update_links_status(wl, wlvif, status_2);
 	}
 
 	/* update the host-chipset time offset */
 	getnstimeofday(&ts);
 	wl->time_offset = (timespec_to_ns(&ts) >> 10) -
-		(s64)le32_to_cpu(status->fw_localtime);
+		(s64)le32_to_cpu(status_2->fw_localtime);
+
+	return 0;
 }
 
 static void wl1271_flush_deferred_work(struct wl1271 *wl)
@@ -527,20 +494,15 @@ static void wl1271_netstack_work(struct work_struct *work)
 
 #define WL1271_IRQ_MAX_LOOPS 256
 
-static irqreturn_t wl1271_irq(int irq, void *cookie)
+static int wlcore_irq_locked(struct wl1271 *wl)
 {
-	int ret;
+	int ret = 0;
 	u32 intr;
 	int loopcount = WL1271_IRQ_MAX_LOOPS;
-	struct wl1271 *wl = (struct wl1271 *)cookie;
 	bool done = false;
 	unsigned int defer_count;
 	unsigned long flags;
 
-	/* TX might be handled here, avoid redundant work */
-	set_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
-	cancel_work_sync(&wl->tx_work);
-
 	/*
 	 * In case edge triggered interrupt must be used, we cannot iterate
 	 * more than once without introducing race conditions with the hardirq.
@@ -548,8 +510,6 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 	if (wl->platform_quirks & WL12XX_PLATFORM_QUIRK_EDGE_IRQ)
 		loopcount = 1;
 
-	mutex_lock(&wl->mutex);
-
 	wl1271_debug(DEBUG_IRQ, "IRQ work");
 
 	if (unlikely(wl->state == WL1271_STATE_OFF))
@@ -568,21 +528,33 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 		clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
 		smp_mb__after_clear_bit();
 
-		wl12xx_fw_status(wl, wl->fw_status);
+		ret = wlcore_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+		if (ret < 0)
+			goto out;
 
 		wlcore_hw_tx_immediate_compl(wl);
 
-		intr = le32_to_cpu(wl->fw_status->intr);
-		intr &= WL1271_INTR_MASK;
+		intr = le32_to_cpu(wl->fw_status_1->intr);
+		intr &= WLCORE_ALL_INTR_MASK;
 		if (!intr) {
 			done = true;
 			continue;
 		}
 
 		if (unlikely(intr & WL1271_ACX_INTR_WATCHDOG)) {
-			wl1271_error("watchdog interrupt received! "
+			wl1271_error("HW watchdog interrupt received! starting recovery.");
+			wl->watchdog_recovery = true;
+			ret = -EIO;
+
+			/* restarting the chip. ignore any other interrupt. */
+			goto out;
+		}
+
+		if (unlikely(intr & WL1271_ACX_SW_INTR_WATCHDOG)) {
+			wl1271_error("SW watchdog interrupt received! "
 				     "starting recovery.");
-			wl12xx_queue_recovery_work(wl);
+			wl->watchdog_recovery = true;
+			ret = -EIO;
 
 			/* restarting the chip. ignore any other interrupt. */
 			goto out;
@@ -591,7 +563,9 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 		if (likely(intr & WL1271_ACX_INTR_DATA)) {
 			wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_DATA");
 
-			wl12xx_rx(wl, wl->fw_status);
+			ret = wlcore_rx(wl, wl->fw_status_1);
+			if (ret < 0)
+				goto out;
 
 			/* Check if any tx blocks were freed */
 			spin_lock_irqsave(&wl->wl_lock, flags);
@@ -602,13 +576,17 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 				 * In order to avoid starvation of the TX path,
 				 * call the work function directly.
 				 */
-				wl1271_tx_work_locked(wl);
+				ret = wlcore_tx_work_locked(wl);
+				if (ret < 0)
+					goto out;
 			} else {
 				spin_unlock_irqrestore(&wl->wl_lock, flags);
 			}
 
 			/* check for tx results */
-			wlcore_hw_tx_delayed_compl(wl);
+			ret = wlcore_hw_tx_delayed_compl(wl);
+			if (ret < 0)
+				goto out;
 
 			/* Make sure the deferred queues don't get too long */
 			defer_count = skb_queue_len(&wl->deferred_tx_queue) +
@@ -619,12 +597,16 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 
 		if (intr & WL1271_ACX_INTR_EVENT_A) {
 			wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_EVENT_A");
-			wl1271_event_handle(wl, 0);
+			ret = wl1271_event_handle(wl, 0);
+			if (ret < 0)
+				goto out;
 		}
 
 		if (intr & WL1271_ACX_INTR_EVENT_B) {
 			wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_EVENT_B");
-			wl1271_event_handle(wl, 1);
+			ret = wl1271_event_handle(wl, 1);
+			if (ret < 0)
+				goto out;
 		}
 
 		if (intr & WL1271_ACX_INTR_INIT_COMPLETE)
@@ -638,6 +620,25 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 	wl1271_ps_elp_sleep(wl);
 
 out:
+	return ret;
+}
+
+static irqreturn_t wlcore_irq(int irq, void *cookie)
+{
+	int ret;
+	unsigned long flags;
+	struct wl1271 *wl = cookie;
+
+	/* 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);
+
+	ret = wlcore_irq_locked(wl);
+	if (ret)
+		wl12xx_queue_recovery_work(wl);
+
 	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);
@@ -743,7 +744,7 @@ out:
 	return ret;
 }
 
-static int wl1271_fetch_nvs(struct wl1271 *wl)
+static void wl1271_fetch_nvs(struct wl1271 *wl)
 {
 	const struct firmware *fw;
 	int ret;
@@ -751,16 +752,15 @@ static int wl1271_fetch_nvs(struct wl1271 *wl)
 	ret = request_firmware(&fw, WL12XX_NVS_NAME, wl->dev);
 
 	if (ret < 0) {
-		wl1271_error("could not get nvs file %s: %d", WL12XX_NVS_NAME,
-			     ret);
-		return ret;
+		wl1271_debug(DEBUG_BOOT, "could not get nvs file %s: %d",
+			     WL12XX_NVS_NAME, ret);
+		return;
 	}
 
 	wl->nvs = kmemdup(fw->data, fw->size, GFP_KERNEL);
 
 	if (!wl->nvs) {
 		wl1271_error("could not allocate memory for the nvs file");
-		ret = -ENOMEM;
 		goto out;
 	}
 
@@ -768,14 +768,17 @@ static int wl1271_fetch_nvs(struct wl1271 *wl)
 
 out:
 	release_firmware(fw);
-
-	return ret;
 }
 
 void wl12xx_queue_recovery_work(struct wl1271 *wl)
 {
-	if (!test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
+	WARN_ON(!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));
+
+	/* Avoid a recursive recovery */
+	if (!test_and_set_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags)) {
+		wlcore_disable_interrupts_nosync(wl);
 		ieee80211_queue_work(wl->hw, &wl->recovery_work);
+	}
 }
 
 size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
@@ -801,14 +804,17 @@ size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
 	return len;
 }
 
+#define WLCORE_FW_LOG_END 0x2000000
+
 static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
 {
 	u32 addr;
-	u32 first_addr;
+	u32 offset;
+	u32 end_of_log;
 	u8 *block;
+	int ret;
 
 	if ((wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED) ||
-	    (wl->conf.fwlog.mode != WL12XX_FWLOG_ON_DEMAND) ||
 	    (wl->conf.fwlog.mem_blocks == 0))
 		return;
 
@@ -820,34 +826,49 @@ static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
 
 	/*
 	 * Make sure the chip is awake and the logger isn't active.
-	 * This might fail if the firmware hanged.
+	 * Do not send a stop fwlog command if the fw is hanged.
 	 */
-	if (!wl1271_ps_elp_wakeup(wl))
+	if (wl1271_ps_elp_wakeup(wl))
+		goto out;
+	if (!wl->watchdog_recovery)
 		wl12xx_cmd_stop_fwlog(wl);
 
 	/* Read the first memory block address */
-	wl12xx_fw_status(wl, wl->fw_status);
-	first_addr = le32_to_cpu(wl->fw_status->log_start_addr);
-	if (!first_addr)
+	ret = wlcore_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+	if (ret < 0)
 		goto out;
 
+	addr = le32_to_cpu(wl->fw_status_2->log_start_addr);
+	if (!addr)
+		goto out;
+
+	if (wl->conf.fwlog.mode == WL12XX_FWLOG_CONTINUOUS) {
+		offset = sizeof(addr) + sizeof(struct wl1271_rx_descriptor);
+		end_of_log = WLCORE_FW_LOG_END;
+	} else {
+		offset = sizeof(addr);
+		end_of_log = addr;
+	}
+
 	/* Traverse the memory blocks linked list */
-	addr = first_addr;
 	do {
 		memset(block, 0, WL12XX_HW_BLOCK_SIZE);
-		wl1271_read_hwaddr(wl, addr, block, WL12XX_HW_BLOCK_SIZE,
-				   false);
+		ret = wlcore_read_hwaddr(wl, addr, block, WL12XX_HW_BLOCK_SIZE,
+					 false);
+		if (ret < 0)
+			goto out;
 
 		/*
 		 * Memory blocks are linked to one another. The first 4 bytes
 		 * of each memory block hold the hardware address of the next
-		 * one. The last memory block points to the first one.
+		 * one. The last memory block points to the first one in
+		 * on demand mode and is equal to 0x2000000 in continuous mode.
 		 */
 		addr = le32_to_cpup((__le32 *)block);
-		if (!wl12xx_copy_fwlog(wl, block + sizeof(addr),
-				       WL12XX_HW_BLOCK_SIZE - sizeof(addr)))
+		if (!wl12xx_copy_fwlog(wl, block + offset,
+				       WL12XX_HW_BLOCK_SIZE - offset))
 			break;
-	} while (addr && (addr != first_addr));
+	} while (addr && (addr != end_of_log));
 
 	wake_up_interruptible(&wl->fwlog_waitq);
 
@@ -855,6 +876,34 @@ out:
 	kfree(block);
 }
 
+static void wlcore_print_recovery(struct wl1271 *wl)
+{
+	u32 pc = 0;
+	u32 hint_sts = 0;
+	int ret;
+
+	wl1271_info("Hardware recovery in progress. FW ver: %s",
+		    wl->chip.fw_ver_str);
+
+	/* change partitions momentarily so we can read the FW pc */
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	if (ret < 0)
+		return;
+
+	ret = wlcore_read_reg(wl, REG_PC_ON_RECOVERY, &pc);
+	if (ret < 0)
+		return;
+
+	ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &hint_sts);
+	if (ret < 0)
+		return;
+
+	wl1271_info("pc: 0x%x, hint_sts: 0x%08x", pc, hint_sts);
+
+	wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+}
+
+
 static void wl1271_recovery_work(struct work_struct *work)
 {
 	struct wl1271 *wl =
@@ -867,26 +916,19 @@ static void wl1271_recovery_work(struct work_struct *work)
 	if (wl->state != WL1271_STATE_ON || wl->plt)
 		goto out_unlock;
 
-	/* Avoid a recursive recovery */
-	set_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
-
-	wl12xx_read_fwlog_panic(wl);
-
-	wl1271_info("Hardware recovery in progress. FW ver: %s pc: 0x%x",
-		    wl->chip.fw_ver_str,
-		    wlcore_read_reg(wl, REG_PC_ON_RECOVERY));
+	if (!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags)) {
+		wl12xx_read_fwlog_panic(wl);
+		wlcore_print_recovery(wl);
+	}
 
 	BUG_ON(bug_on_recovery &&
 	       !test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));
 
 	if (no_recovery) {
 		wl1271_info("No recovery (chosen on module load). Fw will remain stuck.");
-		clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
 		goto out_unlock;
 	}
 
-	BUG_ON(bug_on_recovery);
-
 	/*
 	 * Advance security sequence number to overcome potential progress
 	 * in the firmware during recovery. This doens't hurt if the network is
@@ -900,7 +942,7 @@ static void wl1271_recovery_work(struct work_struct *work)
 	}
 
 	/* Prevent spurious TX during FW restart */
-	ieee80211_stop_queues(wl->hw);
+	wlcore_stop_queues(wl, WLCORE_QUEUE_STOP_REASON_FW_RESTART);
 
 	if (wl->sched_scanning) {
 		ieee80211_sched_scan_stopped(wl->hw);
@@ -914,10 +956,8 @@ static void wl1271_recovery_work(struct work_struct *work)
 		vif = wl12xx_wlvif_to_vif(wlvif);
 		__wl1271_op_remove_interface(wl, vif, false);
 	}
-	mutex_unlock(&wl->mutex);
-	wl1271_op_stop(wl->hw);
 
-	clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
+	wlcore_op_stop_locked(wl);
 
 	ieee80211_restart_hw(wl->hw);
 
@@ -925,26 +965,34 @@ static void wl1271_recovery_work(struct work_struct *work)
 	 * Its safe to enable TX now - the queues are stopped after a request
 	 * to restart the HW.
 	 */
-	ieee80211_wake_queues(wl->hw);
-	return;
+	wlcore_wake_queues(wl, WLCORE_QUEUE_STOP_REASON_FW_RESTART);
+
 out_unlock:
+	wl->watchdog_recovery = false;
+	clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
 	mutex_unlock(&wl->mutex);
 }
 
-static void wl1271_fw_wakeup(struct wl1271 *wl)
+static int wlcore_fw_wakeup(struct wl1271 *wl)
 {
-	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
+	return wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
 }
 
 static int wl1271_setup(struct wl1271 *wl)
 {
-	wl->fw_status = kmalloc(sizeof(*wl->fw_status), GFP_KERNEL);
-	if (!wl->fw_status)
+	wl->fw_status_1 = kmalloc(WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc) +
+				  sizeof(*wl->fw_status_2) +
+				  wl->fw_status_priv_len, GFP_KERNEL);
+	if (!wl->fw_status_1)
 		return -ENOMEM;
 
+	wl->fw_status_2 = (struct wl_fw_status_2 *)
+				(((u8 *) wl->fw_status_1) +
+				WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc));
+
 	wl->tx_res_if = kmalloc(sizeof(*wl->tx_res_if), GFP_KERNEL);
 	if (!wl->tx_res_if) {
-		kfree(wl->fw_status);
+		kfree(wl->fw_status_1);
 		return -ENOMEM;
 	}
 
@@ -963,13 +1011,21 @@ static int wl12xx_set_power_on(struct wl1271 *wl)
 	wl1271_io_reset(wl);
 	wl1271_io_init(wl);
 
-	wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+	if (ret < 0)
+		goto fail;
 
 	/* ELP module wake up */
-	wl1271_fw_wakeup(wl);
+	ret = wlcore_fw_wakeup(wl);
+	if (ret < 0)
+		goto fail;
 
 out:
 	return ret;
+
+fail:
+	wl1271_power_off(wl);
+	return ret;
 }
 
 static int wl12xx_chip_wakeup(struct wl1271 *wl, bool plt)
@@ -987,13 +1043,12 @@ static int wl12xx_chip_wakeup(struct wl1271 *wl, bool plt)
 	 * simplify the code and since the performance impact is
 	 * negligible, we use the same block size for all different
 	 * chip types.
+	 *
+	 * Check if the bus supports blocksize alignment and, if it
+	 * doesn't, make sure we don't have the quirk.
 	 */
-	if (wl1271_set_block_size(wl))
-		wl->quirks |= WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
-
-	ret = wl->ops->identify_chip(wl);
-	if (ret < 0)
-		goto out;
+	if (!wl1271_set_block_size(wl))
+		wl->quirks &= ~WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
 
 	/* TODO: make sure the lower driver has set things up correctly */
 
@@ -1005,21 +1060,21 @@ static int wl12xx_chip_wakeup(struct wl1271 *wl, bool plt)
 	if (ret < 0)
 		goto out;
 
-	/* No NVS from netlink, try to get it from the filesystem */
-	if (wl->nvs == NULL) {
-		ret = wl1271_fetch_nvs(wl);
-		if (ret < 0)
-			goto out;
-	}
-
 out:
 	return ret;
 }
 
-int wl1271_plt_start(struct wl1271 *wl)
+int wl1271_plt_start(struct wl1271 *wl, const enum plt_mode plt_mode)
 {
 	int retries = WL1271_BOOT_RETRIES;
 	struct wiphy *wiphy = wl->hw->wiphy;
+
+	static const char* const PLT_MODE[] = {
+		"PLT_OFF",
+		"PLT_ON",
+		"PLT_FEM_DETECT"
+	};
+
 	int ret;
 
 	mutex_lock(&wl->mutex);
@@ -1033,23 +1088,23 @@ int wl1271_plt_start(struct wl1271 *wl)
 		goto out;
 	}
 
+	/* Indicate to lower levels that we are now in PLT mode */
+	wl->plt = true;
+	wl->plt_mode = plt_mode;
+
 	while (retries) {
 		retries--;
 		ret = wl12xx_chip_wakeup(wl, true);
 		if (ret < 0)
 			goto power_off;
 
-		ret = wl->ops->boot(wl);
+		ret = wl->ops->plt_init(wl);
 		if (ret < 0)
 			goto power_off;
 
-		ret = wl1271_plt_init(wl);
-		if (ret < 0)
-			goto irq_disable;
-
-		wl->plt = true;
 		wl->state = WL1271_STATE_ON;
-		wl1271_notice("firmware booted in PLT mode (%s)",
+		wl1271_notice("firmware booted in PLT mode %s (%s)",
+			      PLT_MODE[plt_mode],
 			      wl->chip.fw_ver_str);
 
 		/* update hw/fw version info in wiphy struct */
@@ -1059,23 +1114,13 @@ int wl1271_plt_start(struct wl1271 *wl)
 
 		goto out;
 
-irq_disable:
-		mutex_unlock(&wl->mutex);
-		/* Unlocking the mutex in the middle of handling is
-		   inherently unsafe. In this case we deem it safe to do,
-		   because we need to let any possibly pending IRQ out of
-		   the system (and while we are WL1271_STATE_OFF the IRQ
-		   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. */
-		wlcore_disable_interrupts(wl);
-		wl1271_flush_deferred_work(wl);
-		cancel_work_sync(&wl->netstack_work);
-		mutex_lock(&wl->mutex);
 power_off:
 		wl1271_power_off(wl);
 	}
 
+	wl->plt = false;
+	wl->plt_mode = PLT_OFF;
+
 	wl1271_error("firmware boot in PLT mode failed despite %d retries",
 		     WL1271_BOOT_RETRIES);
 out:
@@ -1125,8 +1170,10 @@ int wl1271_plt_stop(struct wl1271 *wl)
 	mutex_lock(&wl->mutex);
 	wl1271_power_off(wl);
 	wl->flags = 0;
+	wl->sleep_auth = WL1271_PSM_ILLEGAL;
 	wl->state = WL1271_STATE_OFF;
 	wl->plt = false;
+	wl->plt_mode = PLT_OFF;
 	wl->rx_counter = 0;
 	mutex_unlock(&wl->mutex);
 
@@ -1154,9 +1201,16 @@ static void wl1271_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	spin_lock_irqsave(&wl->wl_lock, flags);
 
-	/* queue the packet */
+	/*
+	 * drop the packet if the link is invalid or the queue is stopped
+	 * for any reason but watermark. Watermark is a "soft"-stop so we
+	 * allow these packets through.
+	 */
 	if (hlid == WL12XX_INVALID_LINK_ID ||
-	    (wlvif && !test_bit(hlid, wlvif->links_map))) {
+	    (wlvif && !test_bit(hlid, wlvif->links_map)) ||
+	     (wlcore_is_queue_stopped(wl, q) &&
+	      !wlcore_is_queue_stopped_by_reason(wl, q,
+			WLCORE_QUEUE_STOP_REASON_WATERMARK))) {
 		wl1271_debug(DEBUG_TX, "DROP skb hlid %d q %d", hlid, q);
 		ieee80211_free_txskb(hw, skb);
 		goto out;
@@ -1172,10 +1226,12 @@ static void wl1271_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	 * The workqueue is slow to process the tx_queue and we need stop
 	 * the queue here, otherwise the queue will get too long.
 	 */
-	if (wl->tx_queue_count[q] >= WL1271_TX_QUEUE_HIGH_WATERMARK) {
+	if (wl->tx_queue_count[q] >= WL1271_TX_QUEUE_HIGH_WATERMARK &&
+	    !wlcore_is_queue_stopped_by_reason(wl, q,
+					WLCORE_QUEUE_STOP_REASON_WATERMARK)) {
 		wl1271_debug(DEBUG_TX, "op_tx: stopping queues for q %d", q);
-		ieee80211_stop_queue(wl->hw, mapping);
-		set_bit(q, &wl->stopped_queues_map);
+		wlcore_stop_queue_locked(wl, q,
+					 WLCORE_QUEUE_STOP_REASON_WATERMARK);
 	}
 
 	/*
@@ -1209,7 +1265,7 @@ int wl1271_tx_dummy_packet(struct wl1271 *wl)
 
 	/* The FW is low on RX memory blocks, so send the dummy packet asap */
 	if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags))
-		wl1271_tx_work_locked(wl);
+		return wlcore_tx_work_locked(wl);
 
 	/*
 	 * If the FW TX is busy, TX work will be scheduled by the threaded
@@ -1476,8 +1532,15 @@ static int wl1271_configure_wowlan(struct wl1271 *wl,
 	int i, ret;
 
 	if (!wow || wow->any || !wow->n_patterns) {
-		wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
-		wl1271_rx_filter_clear_all(wl);
+		ret = wl1271_acx_default_rx_filter_enable(wl, 0,
+							  FILTER_SIGNAL);
+		if (ret)
+			goto out;
+
+		ret = wl1271_rx_filter_clear_all(wl);
+		if (ret)
+			goto out;
+
 		return 0;
 	}
 
@@ -1493,8 +1556,13 @@ static int wl1271_configure_wowlan(struct wl1271 *wl,
 		}
 	}
 
-	wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
-	wl1271_rx_filter_clear_all(wl);
+	ret = wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
+	if (ret)
+		goto out;
+
+	ret = wl1271_rx_filter_clear_all(wl);
+	if (ret)
+		goto out;
 
 	/* Translate WoWLAN patterns into filters */
 	for (i = 0; i < wow->n_patterns; i++) {
@@ -1532,11 +1600,20 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
 	if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
 		goto out;
 
+	if ((wl->conf.conn.suspend_wake_up_event ==
+	     wl->conf.conn.wake_up_event) &&
+	    (wl->conf.conn.suspend_listen_interval ==
+	     wl->conf.conn.listen_interval))
+		goto out;
+
 	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		goto out;
 
-	wl1271_configure_wowlan(wl, wow);
+	ret = wl1271_configure_wowlan(wl, wow);
+	if (ret < 0)
+		goto out_sleep;
+
 	ret = wl1271_acx_wake_up_conditions(wl, wlvif,
 				    wl->conf.conn.suspend_wake_up_event,
 				    wl->conf.conn.suspend_listen_interval);
@@ -1544,8 +1621,8 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
 	if (ret < 0)
 		wl1271_error("suspend: set wake up conditions failed: %d", ret);
 
+out_sleep:
 	wl1271_ps_elp_sleep(wl);
-
 out:
 	return ret;
 
@@ -1592,6 +1669,13 @@ static void wl1271_configure_resume(struct wl1271 *wl,
 	if ((!is_ap) && (!is_sta))
 		return;
 
+	if (is_sta &&
+	    ((wl->conf.conn.suspend_wake_up_event ==
+	      wl->conf.conn.wake_up_event) &&
+	     (wl->conf.conn.suspend_listen_interval ==
+	      wl->conf.conn.listen_interval)))
+		return;
+
 	ret = wl1271_ps_elp_wakeup(wl);
 	if (ret < 0)
 		return;
@@ -1624,6 +1708,12 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
 	wl1271_debug(DEBUG_MAC80211, "mac80211 suspend wow=%d", !!wow);
 	WARN_ON(!wow);
 
+	/* we want to perform the recovery before suspending */
+	if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags)) {
+		wl1271_warning("postponing suspend to perform recovery");
+		return -EBUSY;
+	}
+
 	wl1271_tx_flush(wl);
 
 	mutex_lock(&wl->mutex);
@@ -1664,7 +1754,8 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
 	struct wl1271 *wl = hw->priv;
 	struct wl12xx_vif *wlvif;
 	unsigned long flags;
-	bool run_irq_work = false;
+	bool run_irq_work = false, pending_recovery;
+	int ret;
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 resume wow=%d",
 		     wl->wow_enabled);
@@ -1680,17 +1771,37 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
 		run_irq_work = true;
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
 
+	mutex_lock(&wl->mutex);
+
+	/* test the recovery flag before calling any SDIO functions */
+	pending_recovery = test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS,
+				    &wl->flags);
+
 	if (run_irq_work) {
 		wl1271_debug(DEBUG_MAC80211,
 			     "run postponed irq_work directly");
-		wl1271_irq(0, wl);
+
+		/* don't talk to the HW if recovery is pending */
+		if (!pending_recovery) {
+			ret = wlcore_irq_locked(wl);
+			if (ret)
+				wl12xx_queue_recovery_work(wl);
+		}
+
 		wlcore_enable_interrupts(wl);
 	}
 
-	mutex_lock(&wl->mutex);
+	if (pending_recovery) {
+		wl1271_warning("queuing forgotten recovery on resume");
+		ieee80211_queue_work(wl->hw, &wl->recovery_work);
+		goto out;
+	}
+
 	wl12xx_for_each_wlvif(wl, wlvif) {
 		wl1271_configure_resume(wl, wlvif);
 	}
+
+out:
 	wl->wow_enabled = false;
 	mutex_unlock(&wl->mutex);
 
@@ -1716,29 +1827,15 @@ static int wl1271_op_start(struct ieee80211_hw *hw)
 	return 0;
 }
 
-static void wl1271_op_stop(struct ieee80211_hw *hw)
+static void wlcore_op_stop_locked(struct wl1271 *wl)
 {
-	struct wl1271 *wl = hw->priv;
 	int i;
 
-	wl1271_debug(DEBUG_MAC80211, "mac80211 stop");
-
-	/*
-	 * Interrupts must be disabled before setting the state to OFF.
-	 * Otherwise, the interrupt handler might be called and exit without
-	 * reading the interrupt status.
-	 */
-	wlcore_disable_interrupts(wl);
-	mutex_lock(&wl->mutex);
 	if (wl->state == WL1271_STATE_OFF) {
-		mutex_unlock(&wl->mutex);
+		if (test_and_clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS,
+					&wl->flags))
+			wlcore_enable_interrupts(wl);
 
-		/*
-		 * This will not necessarily enable interrupts as interrupts
-		 * may have been disabled when op_stop was called. It will,
-		 * however, balance the above call to disable_interrupts().
-		 */
-		wlcore_enable_interrupts(wl);
 		return;
 	}
 
@@ -1747,8 +1844,16 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	 * functions don't perform further work.
 	 */
 	wl->state = WL1271_STATE_OFF;
+
+	/*
+	 * Use the nosync variant to disable interrupts, so the mutex could be
+	 * held while doing so without deadlocking.
+	 */
+	wlcore_disable_interrupts_nosync(wl);
+
 	mutex_unlock(&wl->mutex);
 
+	wlcore_synchronize_interrupts(wl);
 	wl1271_flush_deferred_work(wl);
 	cancel_delayed_work_sync(&wl->scan_complete_work);
 	cancel_work_sync(&wl->netstack_work);
@@ -1758,15 +1863,23 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	cancel_delayed_work_sync(&wl->connection_loss_work);
 
 	/* let's notify MAC80211 about the remaining pending TX frames */
-	wl12xx_tx_reset(wl, true);
+	wl12xx_tx_reset(wl);
 	mutex_lock(&wl->mutex);
 
 	wl1271_power_off(wl);
+	/*
+	 * In case a recovery was scheduled, interrupts were disabled to avoid
+	 * an interrupt storm. Now that the power is down, it is safe to
+	 * re-enable interrupts to balance the disable depth
+	 */
+	if (test_and_clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
+		wlcore_enable_interrupts(wl);
 
 	wl->band = IEEE80211_BAND_2GHZ;
 
 	wl->rx_counter = 0;
 	wl->power_level = WL1271_DEFAULT_POWER_LEVEL;
+	wl->channel_type = NL80211_CHAN_NO_HT;
 	wl->tx_blocks_available = 0;
 	wl->tx_allocated_blocks = 0;
 	wl->tx_results_count = 0;
@@ -1775,6 +1888,7 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 	wl->ap_fw_ps_map = 0;
 	wl->ap_ps_map = 0;
 	wl->sched_scanning = false;
+	wl->sleep_auth = WL1271_PSM_ILLEGAL;
 	memset(wl->roles_map, 0, sizeof(wl->roles_map));
 	memset(wl->links_map, 0, sizeof(wl->links_map));
 	memset(wl->roc_map, 0, sizeof(wl->roc_map));
@@ -1799,12 +1913,24 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
 
 	wl1271_debugfs_reset(wl);
 
-	kfree(wl->fw_status);
-	wl->fw_status = NULL;
+	kfree(wl->fw_status_1);
+	wl->fw_status_1 = NULL;
+	wl->fw_status_2 = NULL;
 	kfree(wl->tx_res_if);
 	wl->tx_res_if = NULL;
 	kfree(wl->target_mem_map);
 	wl->target_mem_map = NULL;
+}
+
+static void wlcore_op_stop(struct ieee80211_hw *hw)
+{
+	struct wl1271 *wl = hw->priv;
+
+	wl1271_debug(DEBUG_MAC80211, "mac80211 stop");
+
+	mutex_lock(&wl->mutex);
+
+	wlcore_op_stop_locked(wl);
 
 	mutex_unlock(&wl->mutex);
 }
@@ -1894,6 +2020,9 @@ static int wl12xx_init_vif_data(struct wl1271 *wl, struct ieee80211_vif *vif)
 		wl12xx_allocate_rate_policy(wl, &wlvif->sta.basic_rate_idx);
 		wl12xx_allocate_rate_policy(wl, &wlvif->sta.ap_rate_idx);
 		wl12xx_allocate_rate_policy(wl, &wlvif->sta.p2p_rate_idx);
+		wlvif->basic_rate_set = CONF_TX_RATE_MASK_BASIC;
+		wlvif->basic_rate = CONF_TX_RATE_MASK_BASIC;
+		wlvif->rate_set = CONF_TX_RATE_MASK_BASIC;
 	} else {
 		/* init ap data */
 		wlvif->ap.bcast_hlid = WL12XX_INVALID_LINK_ID;
@@ -1903,13 +2032,19 @@ static int wl12xx_init_vif_data(struct wl1271 *wl, struct ieee80211_vif *vif)
 		for (i = 0; i < CONF_TX_MAX_AC_COUNT; i++)
 			wl12xx_allocate_rate_policy(wl,
 						&wlvif->ap.ucast_rate_idx[i]);
+		wlvif->basic_rate_set = CONF_TX_AP_ENABLED_RATES;
+		/*
+		 * TODO: check if basic_rate shouldn't be
+		 * wl1271_tx_min_rate_get(wl, wlvif->basic_rate_set);
+		 * instead (the same thing for STA above).
+		*/
+		wlvif->basic_rate = CONF_TX_AP_ENABLED_RATES;
+		/* TODO: this seems to be used only for STA, check it */
+		wlvif->rate_set = CONF_TX_AP_ENABLED_RATES;
 	}
 
 	wlvif->bitrate_masks[IEEE80211_BAND_2GHZ] = wl->conf.tx.basic_rate;
 	wlvif->bitrate_masks[IEEE80211_BAND_5GHZ] = wl->conf.tx.basic_rate_5;
-	wlvif->basic_rate_set = CONF_TX_RATE_MASK_BASIC;
-	wlvif->basic_rate = CONF_TX_RATE_MASK_BASIC;
-	wlvif->rate_set = CONF_TX_RATE_MASK_BASIC;
 	wlvif->beacon_int = WL1271_DEFAULT_BEACON_INT;
 
 	/*
@@ -1919,6 +2054,7 @@ static int wl12xx_init_vif_data(struct wl1271 *wl, struct ieee80211_vif *vif)
 	wlvif->band = wl->band;
 	wlvif->channel = wl->channel;
 	wlvif->power_level = wl->power_level;
+	wlvif->channel_type = wl->channel_type;
 
 	INIT_WORK(&wlvif->rx_streaming_enable_work,
 		  wl1271_rx_streaming_enable_work);
@@ -2170,6 +2306,7 @@ static void __wl1271_op_remove_interface(struct wl1271 *wl,
 {
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
 	int i, ret;
+	bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
 
 	wl1271_debug(DEBUG_MAC80211, "mac80211 remove interface");
 
@@ -2250,11 +2387,33 @@ deinit:
 	wlvif->role_id = WL12XX_INVALID_ROLE_ID;
 	wlvif->dev_role_id = WL12XX_INVALID_ROLE_ID;
 
-	if (wlvif->bss_type == BSS_TYPE_AP_BSS)
+	if (is_ap)
 		wl->ap_count--;
 	else
 		wl->sta_count--;
 
+	/*
+	 * Last AP, have more stations. Configure sleep auth according to STA.
+	 * Don't do thin on unintended recovery.
+	 */
+	if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags) &&
+	    !test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags))
+		goto unlock;
+
+	if (wl->ap_count == 0 && is_ap && wl->sta_count) {
+		u8 sta_auth = wl->conf.conn.sta_sleep_auth;
+		/* Configure for power according to debugfs */
+		if (sta_auth != WL1271_PSM_ILLEGAL)
+			wl1271_acx_sleep_auth(wl, sta_auth);
+		/* Configure for power always on */
+		else if (wl->quirks & WLCORE_QUIRK_NO_ELP)
+			wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+		/* Configure for ELP power saving */
+		else
+			wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
+	}
+
+unlock:
 	mutex_unlock(&wl->mutex);
 
 	del_timer_sync(&wlvif->rx_streaming_timer);
@@ -2444,7 +2603,7 @@ static int wl1271_sta_handle_idle(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	} else {
 		/* The current firmware only supports sched_scan in idle */
 		if (wl->sched_scanning) {
-			wl1271_scan_sched_scan_stop(wl);
+			wl1271_scan_sched_scan_stop(wl, wlvif);
 			ieee80211_sched_scan_stopped(wl->hw);
 		}
 
@@ -2469,13 +2628,24 @@ static int wl12xx_config_vif(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	/* if the channel changes while joined, join again */
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL &&
 	    ((wlvif->band != conf->channel->band) ||
-	     (wlvif->channel != channel))) {
+	     (wlvif->channel != channel) ||
+	     (wlvif->channel_type != conf->channel_type))) {
 		/* send all pending packets */
-		wl1271_tx_work_locked(wl);
+		ret = wlcore_tx_work_locked(wl);
+		if (ret < 0)
+			return ret;
+
 		wlvif->band = conf->channel->band;
 		wlvif->channel = channel;
+		wlvif->channel_type = conf->channel_type;
 
-		if (!is_ap) {
+		if (is_ap) {
+			wl1271_set_band_rate(wl, wlvif);
+			ret = wl1271_init_ap_rates(wl, wlvif);
+			if (ret < 0)
+				wl1271_error("AP rate policy change failed %d",
+					     ret);
+		} else {
 			/*
 			 * FIXME: the mac80211 should really provide a fixed
 			 * rate to use here. for now, just use the smallest
@@ -2583,8 +2753,9 @@ static int wl1271_op_config(struct ieee80211_hw *hw, u32 changed)
 	 * frames, such as the deauth. To make sure those frames reach the air,
 	 * wait here until the TX queue is fully flushed.
 	 */
-	if ((changed & IEEE80211_CONF_CHANGE_IDLE) &&
-	    (conf->flags & IEEE80211_CONF_IDLE))
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) ||
+	    ((changed & IEEE80211_CONF_CHANGE_IDLE) &&
+	     (conf->flags & IEEE80211_CONF_IDLE)))
 		wl1271_tx_flush(wl);
 
 	mutex_lock(&wl->mutex);
@@ -2593,6 +2764,7 @@ static int wl1271_op_config(struct ieee80211_hw *hw, u32 changed)
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
 		wl->band = conf->channel->band;
 		wl->channel = channel;
+		wl->channel_type = conf->channel_type;
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER)
@@ -2825,17 +2997,6 @@ static int wl1271_set_key(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	int ret;
 	bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
 
-	/*
-	 * A role set to GEM cipher requires different Tx settings (namely
-	 * spare blocks). Note when we are in this mode so the HW can adjust.
-	 */
-	if (key_type == KEY_GEM) {
-		if (action == KEY_ADD_OR_REPLACE)
-			wlvif->is_gem = true;
-		else if (action == KEY_REMOVE)
-			wlvif->is_gem = false;
-	}
-
 	if (is_ap) {
 		struct wl1271_station *wl_sta;
 		u8 hlid;
@@ -2913,12 +3074,21 @@ static int wl1271_set_key(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	return 0;
 }
 
-static int wl1271_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+static int wlcore_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;
+
+	return wlcore_hw_set_key(wl, cmd, vif, sta, key_conf);
+}
+
+int wlcore_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
+		   struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta,
+		   struct ieee80211_key_conf *key_conf)
+{
 	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
 	int ret;
 	u32 tx_seq_32 = 0;
@@ -3029,6 +3199,7 @@ out_unlock:
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(wlcore_set_key);
 
 static int wl1271_op_hw_scan(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif,
@@ -3167,6 +3338,7 @@ static void wl1271_op_sched_scan_stop(struct ieee80211_hw *hw,
 				      struct ieee80211_vif *vif)
 {
 	struct wl1271 *wl = hw->priv;
+	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
 	int ret;
 
 	wl1271_debug(DEBUG_MAC80211, "wl1271_op_sched_scan_stop");
@@ -3180,7 +3352,7 @@ static void wl1271_op_sched_scan_stop(struct ieee80211_hw *hw,
 	if (ret < 0)
 		goto out;
 
-	wl1271_scan_sched_scan_stop(wl);
+	wl1271_scan_sched_scan_stop(wl, wlvif);
 
 	wl1271_ps_elp_sleep(wl);
 out:
@@ -3316,8 +3488,15 @@ static int wl1271_ap_set_probe_resp_tmpl(struct wl1271 *wl, u32 rates,
 				      skb->data,
 				      skb->len, 0,
 				      rates);
-
 	dev_kfree_skb(skb);
+
+	if (ret < 0)
+		goto out;
+
+	wl1271_debug(DEBUG_AP, "probe response updated");
+	set_bit(WLVIF_FLAG_AP_PROBE_RESP_SET, &wlvif->flags);
+
+out:
 	return ret;
 }
 
@@ -3422,6 +3601,87 @@ out:
 	return ret;
 }
 
+static int wlcore_set_beacon_template(struct wl1271 *wl,
+				      struct ieee80211_vif *vif,
+				      bool is_ap)
+{
+	struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
+	struct ieee80211_hdr *hdr;
+	u32 min_rate;
+	int ret;
+	int ieoffset = offsetof(struct ieee80211_mgmt,
+				u.beacon.variable);
+	struct sk_buff *beacon = ieee80211_beacon_get(wl->hw, vif);
+	u16 tmpl_id;
+
+	if (!beacon) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	wl1271_debug(DEBUG_MASTER, "beacon updated");
+
+	ret = wl1271_ssid_set(vif, beacon, ieoffset);
+	if (ret < 0) {
+		dev_kfree_skb(beacon);
+		goto out;
+	}
+	min_rate = wl1271_tx_min_rate_get(wl, wlvif->basic_rate_set);
+	tmpl_id = is_ap ? CMD_TEMPL_AP_BEACON :
+		CMD_TEMPL_BEACON;
+	ret = wl1271_cmd_template_set(wl, wlvif->role_id, tmpl_id,
+				      beacon->data,
+				      beacon->len, 0,
+				      min_rate);
+	if (ret < 0) {
+		dev_kfree_skb(beacon);
+		goto out;
+	}
+
+	/*
+	 * In case we already have a probe-resp beacon set explicitly
+	 * by usermode, don't use the beacon data.
+	 */
+	if (test_bit(WLVIF_FLAG_AP_PROBE_RESP_SET, &wlvif->flags))
+		goto end_bcn;
+
+	/* remove TIM ie from probe response */
+	wl12xx_remove_ie(beacon, WLAN_EID_TIM, ieoffset);
+
+	/*
+	 * remove p2p ie from probe response.
+	 * the fw reponds to probe requests that don't include
+	 * the p2p ie. probe requests with p2p ie will be passed,
+	 * and will be responded by the supplicant (the spec
+	 * forbids including the p2p ie when responding to probe
+	 * requests that didn't include it).
+	 */
+	wl12xx_remove_vendor_ie(beacon, WLAN_OUI_WFA,
+				WLAN_OUI_TYPE_WFA_P2P, ieoffset);
+
+	hdr = (struct ieee80211_hdr *) beacon->data;
+	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					 IEEE80211_STYPE_PROBE_RESP);
+	if (is_ap)
+		ret = wl1271_ap_set_probe_resp_tmpl_legacy(wl, vif,
+							   beacon->data,
+							   beacon->len,
+							   min_rate);
+	else
+		ret = wl1271_cmd_template_set(wl, wlvif->role_id,
+					      CMD_TEMPL_PROBE_RESPONSE,
+					      beacon->data,
+					      beacon->len, 0,
+					      min_rate);
+end_bcn:
+	dev_kfree_skb(beacon);
+	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,
@@ -3440,81 +3700,12 @@ static int wl1271_bss_beacon_info_changed(struct wl1271 *wl,
 
 	if ((changed & BSS_CHANGED_AP_PROBE_RESP) && is_ap) {
 		u32 rate = wl1271_tx_min_rate_get(wl, wlvif->basic_rate_set);
-		if (!wl1271_ap_set_probe_resp_tmpl(wl, rate, vif)) {
-			wl1271_debug(DEBUG_AP, "probe response updated");
-			set_bit(WLVIF_FLAG_AP_PROBE_RESP_SET, &wlvif->flags);
-		}
+
+		wl1271_ap_set_probe_resp_tmpl(wl, rate, vif);
 	}
 
 	if ((changed & BSS_CHANGED_BEACON)) {
-		struct ieee80211_hdr *hdr;
-		u32 min_rate;
-		int ieoffset = offsetof(struct ieee80211_mgmt,
-					u.beacon.variable);
-		struct sk_buff *beacon = ieee80211_beacon_get(wl->hw, vif);
-		u16 tmpl_id;
-
-		if (!beacon) {
-			ret = -EINVAL;
-			goto out;
-		}
-
-		wl1271_debug(DEBUG_MASTER, "beacon updated");
-
-		ret = wl1271_ssid_set(vif, beacon, ieoffset);
-		if (ret < 0) {
-			dev_kfree_skb(beacon);
-			goto out;
-		}
-		min_rate = wl1271_tx_min_rate_get(wl, wlvif->basic_rate_set);
-		tmpl_id = is_ap ? CMD_TEMPL_AP_BEACON :
-				  CMD_TEMPL_BEACON;
-		ret = wl1271_cmd_template_set(wl, wlvif->role_id, tmpl_id,
-					      beacon->data,
-					      beacon->len, 0,
-					      min_rate);
-		if (ret < 0) {
-			dev_kfree_skb(beacon);
-			goto out;
-		}
-
-		/*
-		 * In case we already have a probe-resp beacon set explicitly
-		 * by usermode, don't use the beacon data.
-		 */
-		if (test_bit(WLVIF_FLAG_AP_PROBE_RESP_SET, &wlvif->flags))
-			goto end_bcn;
-
-		/* remove TIM ie from probe response */
-		wl12xx_remove_ie(beacon, WLAN_EID_TIM, ieoffset);
-
-		/*
-		 * remove p2p ie from probe response.
-		 * the fw reponds to probe requests that don't include
-		 * the p2p ie. probe requests with p2p ie will be passed,
-		 * and will be responded by the supplicant (the spec
-		 * forbids including the p2p ie when responding to probe
-		 * requests that didn't include it).
-		 */
-		wl12xx_remove_vendor_ie(beacon, WLAN_OUI_WFA,
-					WLAN_OUI_TYPE_WFA_P2P, ieoffset);
-
-		hdr = (struct ieee80211_hdr *) beacon->data;
-		hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						 IEEE80211_STYPE_PROBE_RESP);
-		if (is_ap)
-			ret = wl1271_ap_set_probe_resp_tmpl_legacy(wl, vif,
-						beacon->data,
-						beacon->len,
-						min_rate);
-		else
-			ret = wl1271_cmd_template_set(wl, wlvif->role_id,
-						CMD_TEMPL_PROBE_RESPONSE,
-						beacon->data,
-						beacon->len, 0,
-						min_rate);
-end_bcn:
-		dev_kfree_skb(beacon);
+		ret = wlcore_set_beacon_template(wl, vif, is_ap);
 		if (ret < 0)
 			goto out;
 	}
@@ -3551,6 +3742,14 @@ static void wl1271_bss_info_changed_ap(struct wl1271 *wl,
 		ret = wl1271_ap_init_templates(wl, vif);
 		if (ret < 0)
 			goto out;
+
+		ret = wl1271_ap_set_probe_resp_tmpl(wl, wlvif->basic_rate, vif);
+		if (ret < 0)
+			goto out;
+
+		ret = wlcore_set_beacon_template(wl, vif, true);
+		if (ret < 0)
+			goto out;
 	}
 
 	ret = wl1271_bss_beacon_info_changed(wl, vif, bss_conf, changed);
@@ -3691,7 +3890,8 @@ static void wl1271_bss_info_changed_sta(struct wl1271 *wl,
 		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.mcs.rx_mask[0] << HW_HT_RATES_OFFSET) |
+			  (sta->ht_cap.mcs.rx_mask[1] << HW_MIMO_RATES_OFFSET);
 		sta_ht_cap = sta->ht_cap;
 		sta_exists = true;
 
@@ -3704,13 +3904,11 @@ sta_not_found:
 			u32 rates;
 			int ieoffset;
 			wlvif->aid = bss_conf->aid;
+			wlvif->channel_type = bss_conf->channel_type;
 			wlvif->beacon_int = bss_conf->beacon_int;
 			do_join = true;
 			set_assoc = true;
 
-			/* Cancel connection_loss_work */
-			cancel_delayed_work_sync(&wl->connection_loss_work);
-
 			/*
 			 * use basic rates from AP, and determine lowest rate
 			 * to use with control frames.
@@ -3960,6 +4158,17 @@ static void wl1271_op_bss_info_changed(struct ieee80211_hw *hw,
 	wl1271_debug(DEBUG_MAC80211, "mac80211 bss info changed 0x%x",
 		     (int)changed);
 
+	/*
+	 * make sure to cancel pending disconnections if our association
+	 * state changed
+	 */
+	if (!is_ap && (changed & BSS_CHANGED_ASSOC))
+		cancel_delayed_work_sync(&wl->connection_loss_work);
+
+	if (is_ap && (changed & BSS_CHANGED_BEACON_ENABLED) &&
+	    !bss_conf->enable_beacon)
+		wl1271_tx_flush(wl);
+
 	mutex_lock(&wl->mutex);
 
 	if (unlikely(wl->state == WL1271_STATE_OFF))
@@ -4068,16 +4277,13 @@ out:
 static int wl1271_op_get_survey(struct ieee80211_hw *hw, int idx,
 				struct survey_info *survey)
 {
-	struct wl1271 *wl = hw->priv;
 	struct ieee80211_conf *conf = &hw->conf;
 
 	if (idx != 0)
 		return -ENOENT;
 
 	survey->channel = conf->channel;
-	survey->filled = SURVEY_INFO_NOISE_DBM;
-	survey->noise = wl->noise;
-
+	survey->filled = 0;
 	return 0;
 }
 
@@ -4343,9 +4549,14 @@ static int wl1271_op_ampdu_action(struct ieee80211_hw *hw,
 
 	case IEEE80211_AMPDU_RX_STOP:
 		if (!(*ba_bitmap & BIT(tid))) {
-			ret = -EINVAL;
-			wl1271_error("no active RX BA session on tid: %d",
+			/*
+			 * this happens on reconfig - so only output a debug
+			 * message for now, and don't fail the function.
+			 */
+			wl1271_debug(DEBUG_MAC80211,
+				     "no active RX BA session on tid: %d",
 				     tid);
+			ret = 0;
 			break;
 		}
 
@@ -4394,7 +4605,7 @@ static int wl12xx_set_bitrate_mask(struct ieee80211_hw *hw,
 
 	mutex_lock(&wl->mutex);
 
-	for (i = 0; i < IEEE80211_NUM_BANDS; i++)
+	for (i = 0; i < WLCORE_NUM_BANDS; i++)
 		wlvif->bitrate_masks[i] =
 			wl1271_tx_enabled_rates_get(wl,
 						    mask->control[i].legacy,
@@ -4462,6 +4673,13 @@ out:
 	mutex_unlock(&wl->mutex);
 }
 
+static void wlcore_op_flush(struct ieee80211_hw *hw, bool drop)
+{
+	struct wl1271 *wl = hw->priv;
+
+	wl1271_tx_flush(wl);
+}
+
 static bool wl1271_tx_frames_pending(struct ieee80211_hw *hw)
 {
 	struct wl1271 *wl = hw->priv;
@@ -4624,7 +4842,7 @@ static struct ieee80211_supported_band wl1271_band_5ghz = {
 
 static const struct ieee80211_ops wl1271_ops = {
 	.start = wl1271_op_start,
-	.stop = wl1271_op_stop,
+	.stop = wlcore_op_stop,
 	.add_interface = wl1271_op_add_interface,
 	.remove_interface = wl1271_op_remove_interface,
 	.change_interface = wl12xx_op_change_interface,
@@ -4636,7 +4854,7 @@ static const struct ieee80211_ops wl1271_ops = {
 	.prepare_multicast = wl1271_op_prepare_multicast,
 	.configure_filter = wl1271_op_configure_filter,
 	.tx = wl1271_op_tx,
-	.set_key = wl1271_op_set_key,
+	.set_key = wlcore_op_set_key,
 	.hw_scan = wl1271_op_hw_scan,
 	.cancel_hw_scan = wl1271_op_cancel_hw_scan,
 	.sched_scan_start = wl1271_op_sched_scan_start,
@@ -4652,6 +4870,7 @@ static const struct ieee80211_ops wl1271_ops = {
 	.tx_frames_pending = wl1271_tx_frames_pending,
 	.set_bitrate_mask = wl12xx_set_bitrate_mask,
 	.channel_switch = wl12xx_op_channel_switch,
+	.flush = wlcore_op_flush,
 	CFG80211_TESTMODE_CMD(wl1271_tm_cmd)
 };
 
@@ -4882,18 +5101,22 @@ static int wl12xx_get_hw_info(struct wl1271 *wl)
 	if (ret < 0)
 		goto out;
 
-	wl->chip.id = wlcore_read_reg(wl, REG_CHIP_ID_B);
+	ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &wl->chip.id);
+	if (ret < 0)
+		goto out;
 
 	wl->fuse_oui_addr = 0;
 	wl->fuse_nic_addr = 0;
 
-	wl->hw_pg_ver = wl->ops->get_pg_ver(wl);
+	ret = wl->ops->get_pg_ver(wl, &wl->hw_pg_ver);
+	if (ret < 0)
+		goto out;
 
 	if (wl->ops->get_mac)
-		wl->ops->get_mac(wl);
+		ret = wl->ops->get_mac(wl);
 
-	wl1271_power_off(wl);
 out:
+	wl1271_power_off(wl);
 	return ret;
 }
 
@@ -4905,14 +5128,8 @@ static int wl1271_register_hw(struct wl1271 *wl)
 	if (wl->mac80211_registered)
 		return 0;
 
-	ret = wl12xx_get_hw_info(wl);
-	if (ret < 0) {
-		wl1271_error("couldn't get hw info");
-		goto out;
-	}
-
-	ret = wl1271_fetch_nvs(wl);
-	if (ret == 0) {
+	wl1271_fetch_nvs(wl);
+	if (wl->nvs != NULL) {
 		/* NOTE: The wl->nvs->nvs element must be first, in
 		 * order to simplify the casting, we assume it is at
 		 * the beginning of the wl->nvs structure.
@@ -4960,6 +5177,29 @@ static void wl1271_unregister_hw(struct wl1271 *wl)
 
 }
 
+static const struct ieee80211_iface_limit wlcore_iface_limits[] = {
+	{
+		.max = 2,
+		.types = BIT(NL80211_IFTYPE_STATION),
+	},
+	{
+		.max = 1,
+		.types = BIT(NL80211_IFTYPE_AP) |
+			 BIT(NL80211_IFTYPE_P2P_GO) |
+			 BIT(NL80211_IFTYPE_P2P_CLIENT),
+	},
+};
+
+static const struct ieee80211_iface_combination
+wlcore_iface_combinations[] = {
+	{
+	  .num_different_channels = 1,
+	  .max_interfaces = 2,
+	  .limits = wlcore_iface_limits,
+	  .n_limits = ARRAY_SIZE(wlcore_iface_limits),
+	},
+};
+
 static int wl1271_init_ieee80211(struct wl1271 *wl)
 {
 	static const u32 cipher_suites[] = {
@@ -4970,9 +5210,11 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 		WL1271_CIPHER_SUITE_GEM,
 	};
 
-	/* The tx descriptor buffer and the TKIP space. */
-	wl->hw->extra_tx_headroom = WL1271_EXTRA_SPACE_TKIP +
-		sizeof(struct wl1271_tx_hw_descr);
+	/* The tx descriptor buffer */
+	wl->hw->extra_tx_headroom = sizeof(struct wl1271_tx_hw_descr);
+
+	if (wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE)
+		wl->hw->extra_tx_headroom += WL1271_EXTRA_SPACE_TKIP;
 
 	/* unit us */
 	/* FIXME: find a proper value */
@@ -5025,12 +5267,14 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 	 */
 	memcpy(&wl->bands[IEEE80211_BAND_2GHZ], &wl1271_band_2ghz,
 	       sizeof(wl1271_band_2ghz));
-	memcpy(&wl->bands[IEEE80211_BAND_2GHZ].ht_cap, &wl->ht_cap,
-	       sizeof(wl->ht_cap));
+	memcpy(&wl->bands[IEEE80211_BAND_2GHZ].ht_cap,
+	       &wl->ht_cap[IEEE80211_BAND_2GHZ],
+	       sizeof(*wl->ht_cap));
 	memcpy(&wl->bands[IEEE80211_BAND_5GHZ], &wl1271_band_5ghz,
 	       sizeof(wl1271_band_5ghz));
-	memcpy(&wl->bands[IEEE80211_BAND_5GHZ].ht_cap, &wl->ht_cap,
-	       sizeof(wl->ht_cap));
+	memcpy(&wl->bands[IEEE80211_BAND_5GHZ].ht_cap,
+	       &wl->ht_cap[IEEE80211_BAND_5GHZ],
+	       sizeof(*wl->ht_cap));
 
 	wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
 		&wl->bands[IEEE80211_BAND_2GHZ];
@@ -5049,6 +5293,11 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2 |
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
 
+	/* allowed interface combinations */
+	wl->hw->wiphy->iface_combinations = wlcore_iface_combinations;
+	wl->hw->wiphy->n_iface_combinations =
+		ARRAY_SIZE(wlcore_iface_combinations);
+
 	SET_IEEE80211_DEV(wl->hw, wl->dev);
 
 	wl->hw->sta_data_size = sizeof(struct wl1271_station);
@@ -5117,8 +5366,10 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size)
 	wl->rx_counter = 0;
 	wl->power_level = WL1271_DEFAULT_POWER_LEVEL;
 	wl->band = IEEE80211_BAND_2GHZ;
+	wl->channel_type = NL80211_CHAN_NO_HT;
 	wl->flags = 0;
 	wl->sg_enabled = true;
+	wl->sleep_auth = WL1271_PSM_ILLEGAL;
 	wl->hw_pg_ver = -1;
 	wl->ap_ps_map = 0;
 	wl->ap_fw_ps_map = 0;
@@ -5142,6 +5393,7 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size)
 	wl->state = WL1271_STATE_OFF;
 	wl->fw_type = WL12XX_FW_TYPE_NONE;
 	mutex_init(&wl->mutex);
+	mutex_init(&wl->flush_mutex);
 
 	order = get_order(WL1271_AGGR_BUFFER_SIZE);
 	wl->aggr_buf = (u8 *)__get_free_pages(GFP_KERNEL, order);
@@ -5222,7 +5474,7 @@ int wlcore_free_hw(struct wl1271 *wl)
 	kfree(wl->nvs);
 	wl->nvs = NULL;
 
-	kfree(wl->fw_status);
+	kfree(wl->fw_status_1);
 	kfree(wl->tx_res_if);
 	destroy_workqueue(wl->freezable_wq);
 
@@ -5279,8 +5531,6 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 	wlcore_adjust_conf(wl);
 
 	wl->irq = platform_get_irq(pdev, 0);
-	wl->ref_clock = pdata->board_ref_clock;
-	wl->tcxo_clock = pdata->board_tcxo_clock;
 	wl->platform_quirks = pdata->platform_quirks;
 	wl->set_power = pdata->set_power;
 	wl->dev = &pdev->dev;
@@ -5293,7 +5543,7 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 	else
 		irqflags = IRQF_TRIGGER_HIGH | IRQF_ONESHOT;
 
-	ret = request_threaded_irq(wl->irq, wl12xx_hardirq, wl1271_irq,
+	ret = request_threaded_irq(wl->irq, wl12xx_hardirq, wlcore_irq,
 				   irqflags,
 				   pdev->name, wl);
 	if (ret < 0) {
@@ -5301,6 +5551,7 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 		goto out_free_hw;
 	}
 
+#ifdef CONFIG_PM
 	ret = enable_irq_wake(wl->irq);
 	if (!ret) {
 		wl->irq_wake_enabled = true;
@@ -5314,8 +5565,19 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 				WL1271_RX_FILTER_MAX_PATTERN_SIZE;
 		}
 	}
+#endif
 	disable_irq(wl->irq);
 
+	ret = wl12xx_get_hw_info(wl);
+	if (ret < 0) {
+		wl1271_error("couldn't get hw info");
+		goto out_irq;
+	}
+
+	ret = wl->ops->identify_chip(wl);
+	if (ret < 0)
+		goto out_irq;
+
 	ret = wl1271_init_ieee80211(wl);
 	if (ret)
 		goto out_irq;
@@ -5328,7 +5590,7 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
 	ret = device_create_file(wl->dev, &dev_attr_bt_coex_state);
 	if (ret < 0) {
 		wl1271_error("failed to create sysfs file bt_coex_state");
-		goto out_irq;
+		goto out_unreg;
 	}
 
 	/* Create sysfs file to get HW PG version */
@@ -5353,6 +5615,9 @@ out_hw_pg_ver:
 out_bt_coex_state:
 	device_remove_file(wl->dev, &dev_attr_bt_coex_state);
 
+out_unreg:
+	wl1271_unregister_hw(wl);
+
 out_irq:
 	free_irq(wl->irq, wl);
 
diff --git a/drivers/net/wireless/ti/wlcore/ps.c b/drivers/net/wireless/ti/wlcore/ps.c
index 756eee2257b..46d36fd30eb 100644
--- a/drivers/net/wireless/ti/wlcore/ps.c
+++ b/drivers/net/wireless/ti/wlcore/ps.c
@@ -28,11 +28,14 @@
 
 #define WL1271_WAKEUP_TIMEOUT 500
 
+#define ELP_ENTRY_DELAY  5
+
 void wl1271_elp_work(struct work_struct *work)
 {
 	struct delayed_work *dwork;
 	struct wl1271 *wl;
 	struct wl12xx_vif *wlvif;
+	int ret;
 
 	dwork = container_of(work, struct delayed_work, work);
 	wl = container_of(dwork, struct wl1271, elp_work);
@@ -61,7 +64,12 @@ void wl1271_elp_work(struct work_struct *work)
 	}
 
 	wl1271_debug(DEBUG_PSM, "chip to elp");
-	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_SLEEP);
+	ret = wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_SLEEP);
+	if (ret < 0) {
+		wl12xx_queue_recovery_work(wl);
+		goto out;
+	}
+
 	set_bit(WL1271_FLAG_IN_ELP, &wl->flags);
 
 out:
@@ -72,8 +80,9 @@ out:
 void wl1271_ps_elp_sleep(struct wl1271 *wl)
 {
 	struct wl12xx_vif *wlvif;
+	u32 timeout;
 
-	if (wl->quirks & WLCORE_QUIRK_NO_ELP)
+	if (wl->sleep_auth != WL1271_PSM_ELP)
 		return;
 
 	/* we shouldn't get consecutive sleep requests */
@@ -89,8 +98,13 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
 			return;
 	}
 
+	if (wl->conf.conn.forced_ps)
+		timeout = ELP_ENTRY_DELAY;
+	else
+		timeout = wl->conf.conn.dynamic_ps_timeout;
+
 	ieee80211_queue_delayed_work(wl->hw, &wl->elp_work,
-		msecs_to_jiffies(wl->conf.conn.dynamic_ps_timeout));
+				     msecs_to_jiffies(timeout));
 }
 
 int wl1271_ps_elp_wakeup(struct wl1271 *wl)
@@ -127,7 +141,11 @@ int wl1271_ps_elp_wakeup(struct wl1271 *wl)
 		wl->elp_compl = &compl;
 	spin_unlock_irqrestore(&wl->wl_lock, flags);
 
-	wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
+	ret = wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
+	if (ret < 0) {
+		wl12xx_queue_recovery_work(wl);
+		goto err;
+	}
 
 	if (!pending) {
 		ret = wait_for_completion_timeout(
@@ -185,8 +203,12 @@ int wl1271_ps_set_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 
 		set_bit(WLVIF_FLAG_IN_PS, &wlvif->flags);
 
-		/* enable beacon early termination. Not relevant for 5GHz */
-		if (wlvif->band == IEEE80211_BAND_2GHZ) {
+		/*
+		 * enable beacon early termination.
+		 * Not relevant for 5GHz and for high rates.
+		 */
+		if ((wlvif->band == IEEE80211_BAND_2GHZ) &&
+		    (wlvif->basic_rate < CONF_HW_BIT_RATE_9MBPS)) {
 			ret = wl1271_acx_bet_enable(wl, wlvif, true);
 			if (ret < 0)
 				return ret;
@@ -196,7 +218,8 @@ int wl1271_ps_set_mode(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		wl1271_debug(DEBUG_PSM, "leaving psm");
 
 		/* disable beacon early termination */
-		if (wlvif->band == IEEE80211_BAND_2GHZ) {
+		if ((wlvif->band == IEEE80211_BAND_2GHZ) &&
+		    (wlvif->basic_rate < CONF_HW_BIT_RATE_9MBPS)) {
 			ret = wl1271_acx_bet_enable(wl, wlvif, false);
 			if (ret < 0)
 				return ret;
diff --git a/drivers/net/wireless/ti/wlcore/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index d6a3c6b0782..f55e2f9e7ac 100644
--- a/drivers/net/wireless/ti/wlcore/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -127,7 +127,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	}
 
 	if (rx_align == WLCORE_RX_BUF_UNALIGNED)
-		reserved = NET_IP_ALIGN;
+		reserved = RX_BUF_ALIGN;
 
 	/* the data read starts with the descriptor */
 	desc = (struct wl1271_rx_descriptor *) data;
@@ -175,7 +175,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	 */
 	memcpy(buf, data + sizeof(*desc), pkt_data_len);
 	if (rx_align == WLCORE_RX_BUF_PADDED)
-		skb_pull(skb, NET_IP_ALIGN);
+		skb_pull(skb, RX_BUF_ALIGN);
 
 	*hlid = desc->hlid;
 
@@ -186,6 +186,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 		is_data = 1;
 
 	wl1271_rx_status(wl, desc, IEEE80211_SKB_RXCB(skb), beacon);
+	wlcore_hw_set_rx_csum(wl, desc, skb);
 
 	seq_num = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
 	wl1271_debug(DEBUG_RX, "rx skb 0x%p: %d B %s seq %d hlid %d", skb,
@@ -199,17 +200,18 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	return is_data;
 }
 
-void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
+int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
 {
 	unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
 	u32 buf_size;
-	u32 fw_rx_counter  = status->fw_rx_counter & NUM_RX_PKT_DESC_MOD_MASK;
-	u32 drv_rx_counter = wl->rx_counter & NUM_RX_PKT_DESC_MOD_MASK;
+	u32 fw_rx_counter = status->fw_rx_counter % wl->num_rx_desc;
+	u32 drv_rx_counter = wl->rx_counter % wl->num_rx_desc;
 	u32 rx_counter;
 	u32 pkt_len, align_pkt_len;
 	u32 pkt_offset, des;
 	u8 hlid;
 	enum wl_rx_buf_align rx_align;
+	int ret = 0;
 
 	while (drv_rx_counter != fw_rx_counter) {
 		buf_size = 0;
@@ -223,7 +225,7 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
 				break;
 			buf_size += align_pkt_len;
 			rx_counter++;
-			rx_counter &= NUM_RX_PKT_DESC_MOD_MASK;
+			rx_counter %= wl->num_rx_desc;
 		}
 
 		if (buf_size == 0) {
@@ -233,9 +235,14 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
 
 		/* Read all available packets at once */
 		des = le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]);
-		wlcore_hw_prepare_read(wl, des, buf_size);
-		wlcore_read_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
-				 buf_size, true);
+		ret = wlcore_hw_prepare_read(wl, des, buf_size);
+		if (ret < 0)
+			goto out;
+
+		ret = wlcore_read_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+				       buf_size, true);
+		if (ret < 0)
+			goto out;
 
 		/* Split data into separate packets */
 		pkt_offset = 0;
@@ -263,7 +270,7 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
 
 			wl->rx_counter++;
 			drv_rx_counter++;
-			drv_rx_counter &= NUM_RX_PKT_DESC_MOD_MASK;
+			drv_rx_counter %= wl->num_rx_desc;
 			pkt_offset += wlcore_rx_get_align_buf_size(wl, pkt_len);
 		}
 	}
@@ -272,11 +279,17 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status)
 	 * Write the driver's packet counter to the FW. This is only required
 	 * for older hardware revisions
 	 */
-	if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION)
-		wl1271_write32(wl, WL12XX_REG_RX_DRIVER_COUNTER,
-			       wl->rx_counter);
+	if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION) {
+		ret = wlcore_write32(wl, WL12XX_REG_RX_DRIVER_COUNTER,
+				     wl->rx_counter);
+		if (ret < 0)
+			goto out;
+	}
 
 	wl12xx_rearm_rx_streaming(wl, active_hlids);
+
+out:
+	return ret;
 }
 
 #ifdef CONFIG_PM
@@ -305,14 +318,19 @@ int wl1271_rx_filter_enable(struct wl1271 *wl,
 	return 0;
 }
 
-void wl1271_rx_filter_clear_all(struct wl1271 *wl)
+int wl1271_rx_filter_clear_all(struct wl1271 *wl)
 {
-	int i;
+	int i, ret = 0;
 
 	for (i = 0; i < WL1271_MAX_RX_FILTERS; i++) {
 		if (!wl->rx_filter_enabled[i])
 			continue;
-		wl1271_rx_filter_enable(wl, i, 0, NULL);
+		ret = wl1271_rx_filter_enable(wl, i, 0, NULL);
+		if (ret)
+			goto out;
 	}
+
+out:
+	return ret;
 }
 #endif /* CONFIG_PM */
diff --git a/drivers/net/wireless/ti/wlcore/rx.h b/drivers/net/wireless/ti/wlcore/rx.h
index e9a162a864c..71eba189991 100644
--- a/drivers/net/wireless/ti/wlcore/rx.h
+++ b/drivers/net/wireless/ti/wlcore/rx.h
@@ -38,8 +38,6 @@
 #define RX_DESC_PACKETID_SHIFT 11
 #define RX_MAX_PACKET_ID 3
 
-#define NUM_RX_PKT_DESC_MOD_MASK   7
-
 #define RX_DESC_VALID_FCS         0x0001
 #define RX_DESC_MATCH_RXADDR1     0x0002
 #define RX_DESC_MCAST             0x0004
@@ -102,6 +100,15 @@
 /* If set, the start of IP payload is not 4 bytes aligned */
 #define RX_BUF_UNALIGNED_PAYLOAD     BIT(20)
 
+/* If set, the buffer was padded by the FW to be 4 bytes aligned */
+#define RX_BUF_PADDED_PAYLOAD        BIT(30)
+
+/*
+ * Account for the padding inserted by the FW in case of RX_ALIGNMENT
+ * or for fixing alignment in case the packet wasn't aligned.
+ */
+#define RX_BUF_ALIGN                 2
+
 /* Describes the alignment state of a Rx buffer */
 enum wl_rx_buf_align {
 	WLCORE_RX_BUF_ALIGNED,
@@ -136,11 +143,11 @@ struct wl1271_rx_descriptor {
 	u8  reserved;
 } __packed;
 
-void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status *status);
+int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
 int wl1271_rx_filter_enable(struct wl1271 *wl,
 			    int index, bool enable,
 			    struct wl12xx_rx_filter *filter);
-void wl1271_rx_filter_clear_all(struct wl1271 *wl);
+int wl1271_rx_filter_clear_all(struct wl1271 *wl);
 
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/scan.c b/drivers/net/wireless/ti/wlcore/scan.c
index ade21a011c4..dbeca1bfbb2 100644
--- a/drivers/net/wireless/ti/wlcore/scan.c
+++ b/drivers/net/wireless/ti/wlcore/scan.c
@@ -226,7 +226,7 @@ static int wl1271_scan_send(struct wl1271 *wl, struct ieee80211_vif *vif,
 					 cmd->params.role_id, band,
 					 wl->scan.ssid, wl->scan.ssid_len,
 					 wl->scan.req->ie,
-					 wl->scan.req->ie_len);
+					 wl->scan.req->ie_len, false);
 	if (ret < 0) {
 		wl1271_error("PROBE request template failed");
 		goto out;
@@ -411,7 +411,8 @@ wl1271_scan_get_sched_scan_channels(struct wl1271 *wl,
 				    struct cfg80211_sched_scan_request *req,
 				    struct conn_scan_ch_params *channels,
 				    u32 band, bool radar, bool passive,
-				    int start, int max_channels)
+				    int start, int max_channels,
+				    u8 *n_pactive_ch)
 {
 	struct conf_sched_scan_settings *c = &wl->conf.sched_scan;
 	int i, j;
@@ -479,6 +480,23 @@ wl1271_scan_get_sched_scan_channels(struct wl1271 *wl,
 			channels[j].tx_power_att = req->channels[i]->max_power;
 			channels[j].channel = req->channels[i]->hw_value;
 
+			if ((band == IEEE80211_BAND_2GHZ) &&
+			    (channels[j].channel >= 12) &&
+			    (channels[j].channel <= 14) &&
+			    (flags & IEEE80211_CHAN_PASSIVE_SCAN) &&
+			    !force_passive) {
+				/* pactive channels treated as DFS */
+				channels[j].flags = SCAN_CHANNEL_FLAGS_DFS;
+
+				/*
+				 * n_pactive_ch is counted down from the end of
+				 * the passive channel list
+				 */
+				(*n_pactive_ch)++;
+				wl1271_debug(DEBUG_SCAN, "n_pactive_ch = %d",
+					     *n_pactive_ch);
+			}
+
 			j++;
 		}
 	}
@@ -491,38 +509,47 @@ wl1271_scan_sched_scan_channels(struct wl1271 *wl,
 				struct cfg80211_sched_scan_request *req,
 				struct wl1271_cmd_sched_scan_config *cfg)
 {
+	u8 n_pactive_ch = 0;
+
 	cfg->passive[0] =
 		wl1271_scan_get_sched_scan_channels(wl, req, cfg->channels_2,
 						    IEEE80211_BAND_2GHZ,
 						    false, true, 0,
-						    MAX_CHANNELS_2GHZ);
+						    MAX_CHANNELS_2GHZ,
+						    &n_pactive_ch);
 	cfg->active[0] =
 		wl1271_scan_get_sched_scan_channels(wl, req, cfg->channels_2,
 						    IEEE80211_BAND_2GHZ,
 						    false, false,
 						    cfg->passive[0],
-						    MAX_CHANNELS_2GHZ);
+						    MAX_CHANNELS_2GHZ,
+						    &n_pactive_ch);
 	cfg->passive[1] =
 		wl1271_scan_get_sched_scan_channels(wl, req, cfg->channels_5,
 						    IEEE80211_BAND_5GHZ,
 						    false, true, 0,
-						    MAX_CHANNELS_5GHZ);
+						    MAX_CHANNELS_5GHZ,
+						    &n_pactive_ch);
 	cfg->dfs =
 		wl1271_scan_get_sched_scan_channels(wl, req, cfg->channels_5,
 						    IEEE80211_BAND_5GHZ,
 						    true, true,
 						    cfg->passive[1],
-						    MAX_CHANNELS_5GHZ);
+						    MAX_CHANNELS_5GHZ,
+						    &n_pactive_ch);
 	cfg->active[1] =
 		wl1271_scan_get_sched_scan_channels(wl, req, cfg->channels_5,
 						    IEEE80211_BAND_5GHZ,
 						    false, false,
 						    cfg->passive[1] + cfg->dfs,
-						    MAX_CHANNELS_5GHZ);
+						    MAX_CHANNELS_5GHZ,
+						    &n_pactive_ch);
 	/* 802.11j channels are not supported yet */
 	cfg->passive[2] = 0;
 	cfg->active[2] = 0;
 
+	cfg->n_pactive_ch = n_pactive_ch;
+
 	wl1271_debug(DEBUG_SCAN, "    2.4GHz: active %d passive %d",
 		     cfg->active[0], cfg->passive[0]);
 	wl1271_debug(DEBUG_SCAN, "    5GHz: active %d passive %d",
@@ -537,6 +564,7 @@ wl1271_scan_sched_scan_channels(struct wl1271 *wl,
 /* Returns the scan type to be used or a negative value on error */
 static int
 wl12xx_scan_sched_scan_ssid_list(struct wl1271 *wl,
+				 struct wl12xx_vif *wlvif,
 				 struct cfg80211_sched_scan_request *req)
 {
 	struct wl1271_cmd_sched_scan_ssid_list *cmd = NULL;
@@ -565,6 +593,7 @@ wl12xx_scan_sched_scan_ssid_list(struct wl1271 *wl,
 		goto out;
 	}
 
+	cmd->role_id = wlvif->dev_role_id;
 	if (!n_match_ssids) {
 		/* No filter, with ssids */
 		type = SCAN_SSID_FILTER_DISABLED;
@@ -603,7 +632,9 @@ wl12xx_scan_sched_scan_ssid_list(struct wl1271 *wl,
 					continue;
 
 				for (j = 0; j < cmd->n_ssids; j++)
-					if (!memcmp(req->ssids[i].ssid,
+					if ((req->ssids[i].ssid_len ==
+					     cmd->ssids[j].len) &&
+					    !memcmp(req->ssids[i].ssid,
 						   cmd->ssids[j].ssid,
 						   req->ssids[i].ssid_len)) {
 						cmd->ssids[j].type =
@@ -652,6 +683,7 @@ int wl1271_scan_sched_scan_config(struct wl1271 *wl,
 	if (!cfg)
 		return -ENOMEM;
 
+	cfg->role_id = wlvif->dev_role_id;
 	cfg->rssi_threshold = c->rssi_threshold;
 	cfg->snr_threshold  = c->snr_threshold;
 	cfg->n_probe_reqs = c->num_probe_reqs;
@@ -669,7 +701,7 @@ int wl1271_scan_sched_scan_config(struct wl1271 *wl,
 		cfg->intervals[i] = cpu_to_le32(req->interval);
 
 	cfg->ssid_len = 0;
-	ret = wl12xx_scan_sched_scan_ssid_list(wl, req);
+	ret = wl12xx_scan_sched_scan_ssid_list(wl, wlvif, req);
 	if (ret < 0)
 		goto out;
 
@@ -690,7 +722,7 @@ int wl1271_scan_sched_scan_config(struct wl1271 *wl,
 						 req->ssids[0].ssid,
 						 req->ssids[0].ssid_len,
 						 ies->ie[band],
-						 ies->len[band]);
+						 ies->len[band], true);
 		if (ret < 0) {
 			wl1271_error("2.4GHz PROBE request template failed");
 			goto out;
@@ -704,7 +736,7 @@ int wl1271_scan_sched_scan_config(struct wl1271 *wl,
 						 req->ssids[0].ssid,
 						 req->ssids[0].ssid_len,
 						 ies->ie[band],
-						 ies->len[band]);
+						 ies->len[band], true);
 		if (ret < 0) {
 			wl1271_error("5GHz PROBE request template failed");
 			goto out;
@@ -734,13 +766,15 @@ int wl1271_scan_sched_scan_start(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 	if (wlvif->bss_type != BSS_TYPE_STA_BSS)
 		return -EOPNOTSUPP;
 
-	if (test_bit(WLVIF_FLAG_IN_USE, &wlvif->flags))
+	if ((wl->quirks & WLCORE_QUIRK_NO_SCHED_SCAN_WHILE_CONN) &&
+	    test_bit(WLVIF_FLAG_IN_USE, &wlvif->flags))
 		return -EBUSY;
 
 	start = kzalloc(sizeof(*start), GFP_KERNEL);
 	if (!start)
 		return -ENOMEM;
 
+	start->role_id = wlvif->dev_role_id;
 	start->tag = WL1271_SCAN_DEFAULT_TAG;
 
 	ret = wl1271_cmd_send(wl, CMD_START_PERIODIC_SCAN, start,
@@ -762,7 +796,7 @@ void wl1271_scan_sched_scan_results(struct wl1271 *wl)
 	ieee80211_sched_scan_results(wl->hw);
 }
 
-void wl1271_scan_sched_scan_stop(struct wl1271 *wl)
+void wl1271_scan_sched_scan_stop(struct wl1271 *wl,  struct wl12xx_vif *wlvif)
 {
 	struct wl1271_cmd_sched_scan_stop *stop;
 	int ret = 0;
@@ -776,6 +810,7 @@ void wl1271_scan_sched_scan_stop(struct wl1271 *wl)
 		return;
 	}
 
+	stop->role_id = wlvif->dev_role_id;
 	stop->tag = WL1271_SCAN_DEFAULT_TAG;
 
 	ret = wl1271_cmd_send(wl, CMD_STOP_PERIODIC_SCAN, stop,
diff --git a/drivers/net/wireless/ti/wlcore/scan.h b/drivers/net/wireless/ti/wlcore/scan.h
index 81ee36ac207..29f3c8d6b04 100644
--- a/drivers/net/wireless/ti/wlcore/scan.h
+++ b/drivers/net/wireless/ti/wlcore/scan.h
@@ -40,7 +40,7 @@ int wl1271_scan_sched_scan_config(struct wl1271 *wl,
 				     struct cfg80211_sched_scan_request *req,
 				     struct ieee80211_sched_scan_ies *ies);
 int wl1271_scan_sched_scan_start(struct wl1271 *wl, struct wl12xx_vif *wlvif);
-void wl1271_scan_sched_scan_stop(struct wl1271 *wl);
+void wl1271_scan_sched_scan_stop(struct wl1271 *wl,  struct wl12xx_vif *wlvif);
 void wl1271_scan_sched_scan_results(struct wl1271 *wl);
 
 #define WL1271_SCAN_MAX_CHANNELS       24
@@ -142,7 +142,8 @@ enum {
 	SCAN_BSS_TYPE_ANY,
 };
 
-#define SCAN_CHANNEL_FLAGS_DFS		BIT(0)
+#define SCAN_CHANNEL_FLAGS_DFS		BIT(0) /* channel is passive until an
+						  activity is detected on it */
 #define SCAN_CHANNEL_FLAGS_DFS_ENABLED	BIT(1)
 
 struct conn_scan_ch_params {
@@ -185,7 +186,10 @@ struct wl1271_cmd_sched_scan_config {
 
 	u8 dfs;
 
-	u8 padding[3];
+	u8 n_pactive_ch; /* number of pactive (passive until fw detects energy)
+			    channels in BG band */
+	u8 role_id;
+	u8 padding[1];
 
 	struct conn_scan_ch_params channels_2[MAX_CHANNELS_2GHZ];
 	struct conn_scan_ch_params channels_5[MAX_CHANNELS_5GHZ];
@@ -212,21 +216,24 @@ struct wl1271_cmd_sched_scan_ssid_list {
 
 	u8 n_ssids;
 	struct wl1271_ssid ssids[SCHED_SCAN_MAX_SSIDS];
-	u8 padding[3];
+	u8 role_id;
+	u8 padding[2];
 } __packed;
 
 struct wl1271_cmd_sched_scan_start {
 	struct wl1271_cmd_header header;
 
 	u8 tag;
-	u8 padding[3];
+	u8 role_id;
+	u8 padding[2];
 } __packed;
 
 struct wl1271_cmd_sched_scan_stop {
 	struct wl1271_cmd_header header;
 
 	u8 tag;
-	u8 padding[3];
+	u8 role_id;
+	u8 padding[2];
 } __packed;
 
 
diff --git a/drivers/net/wireless/ti/wlcore/sdio.c b/drivers/net/wireless/ti/wlcore/sdio.c
index 0a72347cfc4..73ace4b2604 100644
--- a/drivers/net/wireless/ti/wlcore/sdio.c
+++ b/drivers/net/wireless/ti/wlcore/sdio.c
@@ -25,6 +25,7 @@
 #include <linux/module.h>
 #include <linux/vmalloc.h>
 #include <linux/platform_device.h>
+#include <linux/mmc/sdio.h>
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/sdio_ids.h>
 #include <linux/mmc/card.h>
@@ -32,6 +33,7 @@
 #include <linux/gpio.h>
 #include <linux/wl12xx.h>
 #include <linux/pm_runtime.h>
+#include <linux/printk.h>
 
 #include "wlcore.h"
 #include "wl12xx_80211.h"
@@ -45,6 +47,8 @@
 #define SDIO_DEVICE_ID_TI_WL1271	0x4076
 #endif
 
+static bool dump = false;
+
 struct wl12xx_sdio_glue {
 	struct device *dev;
 	struct platform_device *core;
@@ -67,8 +71,8 @@ static void wl1271_sdio_set_block_size(struct device *child,
 	sdio_release_host(func);
 }
 
-static void wl12xx_sdio_raw_read(struct device *child, int addr, void *buf,
-				 size_t len, bool fixed)
+static int __must_check wl12xx_sdio_raw_read(struct device *child, int addr,
+					     void *buf, size_t len, bool fixed)
 {
 	int ret;
 	struct wl12xx_sdio_glue *glue = dev_get_drvdata(child->parent);
@@ -76,6 +80,13 @@ static void wl12xx_sdio_raw_read(struct device *child, int addr, void *buf,
 
 	sdio_claim_host(func);
 
+	if (unlikely(dump)) {
+		printk(KERN_DEBUG "wlcore_sdio: READ from 0x%04x\n", addr);
+		print_hex_dump(KERN_DEBUG, "wlcore_sdio: READ ",
+				DUMP_PREFIX_OFFSET, 16, 1,
+				buf, len, false);
+	}
+
 	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG)) {
 		((u8 *)buf)[0] = sdio_f0_readb(func, addr, &ret);
 		dev_dbg(child->parent, "sdio read 52 addr 0x%x, byte 0x%02x\n",
@@ -92,12 +103,14 @@ static void wl12xx_sdio_raw_read(struct device *child, int addr, void *buf,
 
 	sdio_release_host(func);
 
-	if (ret)
+	if (WARN_ON(ret))
 		dev_err(child->parent, "sdio read failed (%d)\n", ret);
+
+	return ret;
 }
 
-static void wl12xx_sdio_raw_write(struct device *child, int addr, void *buf,
-				  size_t len, bool fixed)
+static int __must_check wl12xx_sdio_raw_write(struct device *child, int addr,
+					      void *buf, size_t len, bool fixed)
 {
 	int ret;
 	struct wl12xx_sdio_glue *glue = dev_get_drvdata(child->parent);
@@ -105,6 +118,13 @@ static void wl12xx_sdio_raw_write(struct device *child, int addr, void *buf,
 
 	sdio_claim_host(func);
 
+	if (unlikely(dump)) {
+		printk(KERN_DEBUG "wlcore_sdio: WRITE to 0x%04x\n", addr);
+		print_hex_dump(KERN_DEBUG, "wlcore_sdio: WRITE ",
+				DUMP_PREFIX_OFFSET, 16, 1,
+				buf, len, false);
+	}
+
 	if (unlikely(addr == HW_ACCESS_ELP_CTRL_REG)) {
 		sdio_f0_writeb(func, ((u8 *)buf)[0], addr, &ret);
 		dev_dbg(child->parent, "sdio write 52 addr 0x%x, byte 0x%02x\n",
@@ -121,25 +141,30 @@ static void wl12xx_sdio_raw_write(struct device *child, int addr, void *buf,
 
 	sdio_release_host(func);
 
-	if (ret)
+	if (WARN_ON(ret))
 		dev_err(child->parent, "sdio write failed (%d)\n", ret);
+
+	return ret;
 }
 
 static int wl12xx_sdio_power_on(struct wl12xx_sdio_glue *glue)
 {
 	int ret;
 	struct sdio_func *func = dev_to_sdio_func(glue->dev);
+	struct mmc_card *card = func->card;
 
-	/* If enabled, tell runtime PM not to power off the card */
-	if (pm_runtime_enabled(&func->dev)) {
-		ret = pm_runtime_get_sync(&func->dev);
-		if (ret < 0)
-			goto out;
-	} else {
-		/* Runtime PM is disabled: power up the card manually */
-		ret = mmc_power_restore_host(func->card->host);
-		if (ret < 0)
+	ret = pm_runtime_get_sync(&card->dev);
+	if (ret) {
+		/*
+		 * Runtime PM might be temporarily disabled, or the device
+		 * might have a positive reference counter. Make sure it is
+		 * really powered on.
+		 */
+		ret = mmc_power_restore_host(card->host);
+		if (ret < 0) {
+			pm_runtime_put_sync(&card->dev);
 			goto out;
+		}
 	}
 
 	sdio_claim_host(func);
@@ -154,20 +179,21 @@ static int wl12xx_sdio_power_off(struct wl12xx_sdio_glue *glue)
 {
 	int ret;
 	struct sdio_func *func = dev_to_sdio_func(glue->dev);
+	struct mmc_card *card = func->card;
 
 	sdio_claim_host(func);
 	sdio_disable_func(func);
 	sdio_release_host(func);
 
-	/* Power off the card manually, even if runtime PM is enabled. */
-	ret = mmc_power_save_host(func->card->host);
+	/* Power off the card manually in case it wasn't powered off above */
+	ret = mmc_power_save_host(card->host);
 	if (ret < 0)
-		return ret;
+		goto out;
 
-	/* If enabled, let runtime PM know the card is powered off */
-	if (pm_runtime_enabled(&func->dev))
-		ret = pm_runtime_put_sync(&func->dev);
+	/* Let runtime PM know the card is powered off */
+	pm_runtime_put_sync(&card->dev);
 
+out:
 	return ret;
 }
 
@@ -196,6 +222,7 @@ static int __devinit wl1271_probe(struct sdio_func *func,
 	struct resource res[1];
 	mmc_pm_flag_t mmcflags;
 	int ret = -ENOMEM;
+	const char *chip_family;
 
 	/* We are only able to handle the wlan function */
 	if (func->num != 0x02)
@@ -236,7 +263,18 @@ static int __devinit wl1271_probe(struct sdio_func *func,
 	/* Tell PM core that we don't need the card to be powered now */
 	pm_runtime_put_noidle(&func->dev);
 
-	glue->core = platform_device_alloc("wl12xx", -1);
+	/*
+	 * Due to a hardware bug, we can't differentiate wl18xx from
+	 * wl12xx, because both report the same device ID.  The only
+	 * way to differentiate is by checking the SDIO revision,
+	 * which is 3.00 on the wl18xx chips.
+	 */
+	if (func->card->cccr.sdio_vsn == SDIO_SDIO_REV_3_00)
+		chip_family = "wl18xx";
+	else
+		chip_family = "wl12xx";
+
+	glue->core = platform_device_alloc(chip_family, -1);
 	if (!glue->core) {
 		dev_err(glue->dev, "can't allocate platform_device");
 		ret = -ENOMEM;
@@ -367,12 +405,9 @@ static void __exit wl1271_exit(void)
 module_init(wl1271_init);
 module_exit(wl1271_exit);
 
+module_param(dump, bool, S_IRUSR | S_IWUSR);
+MODULE_PARM_DESC(dump, "Enable sdio read/write dumps.");
+
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
 MODULE_AUTHOR("Juuso Oikarinen <juuso.oikarinen@nokia.com>");
-MODULE_FIRMWARE(WL127X_FW_NAME_SINGLE);
-MODULE_FIRMWARE(WL127X_FW_NAME_MULTI);
-MODULE_FIRMWARE(WL127X_PLT_FW_NAME);
-MODULE_FIRMWARE(WL128X_FW_NAME_SINGLE);
-MODULE_FIRMWARE(WL128X_FW_NAME_MULTI);
-MODULE_FIRMWARE(WL128X_PLT_FW_NAME);
diff --git a/drivers/net/wireless/ti/wlcore/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index 553cd3cbb98..8da4ed243eb 100644
--- a/drivers/net/wireless/ti/wlcore/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -193,8 +193,8 @@ static int wl12xx_spi_read_busy(struct device *child)
 	return -ETIMEDOUT;
 }
 
-static void wl12xx_spi_raw_read(struct device *child, int addr, void *buf,
-				size_t len, bool fixed)
+static int __must_check wl12xx_spi_raw_read(struct device *child, int addr,
+					    void *buf, size_t len, bool fixed)
 {
 	struct wl12xx_spi_glue *glue = dev_get_drvdata(child->parent);
 	struct wl1271 *wl = dev_get_drvdata(child);
@@ -238,7 +238,7 @@ static void wl12xx_spi_raw_read(struct device *child, int addr, void *buf,
 		if (!(busy_buf[WL1271_BUSY_WORD_CNT - 1] & 0x1) &&
 		    wl12xx_spi_read_busy(child)) {
 			memset(buf, 0, chunk_len);
-			return;
+			return 0;
 		}
 
 		spi_message_init(&m);
@@ -256,10 +256,12 @@ static void wl12xx_spi_raw_read(struct device *child, int addr, void *buf,
 		buf += chunk_len;
 		len -= chunk_len;
 	}
+
+	return 0;
 }
 
-static void wl12xx_spi_raw_write(struct device *child, int addr, void *buf,
-				 size_t len, bool fixed)
+static int __must_check wl12xx_spi_raw_write(struct device *child, int addr,
+					     void *buf, size_t len, bool fixed)
 {
 	struct wl12xx_spi_glue *glue = dev_get_drvdata(child->parent);
 	struct spi_transfer t[2 * WSPI_MAX_NUM_OF_CHUNKS];
@@ -304,6 +306,8 @@ static void wl12xx_spi_raw_write(struct device *child, int addr, void *buf,
 	}
 
 	spi_sync(to_spi_device(glue->dev), &m);
+
+	return 0;
 }
 
 static struct wl1271_if_operations spi_ops = {
@@ -431,10 +435,4 @@ module_exit(wl1271_exit);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
 MODULE_AUTHOR("Juuso Oikarinen <juuso.oikarinen@nokia.com>");
-MODULE_FIRMWARE(WL127X_FW_NAME_SINGLE);
-MODULE_FIRMWARE(WL127X_FW_NAME_MULTI);
-MODULE_FIRMWARE(WL127X_PLT_FW_NAME);
-MODULE_FIRMWARE(WL128X_FW_NAME_SINGLE);
-MODULE_FIRMWARE(WL128X_FW_NAME_MULTI);
-MODULE_FIRMWARE(WL128X_PLT_FW_NAME);
 MODULE_ALIAS("spi:wl1271");
diff --git a/drivers/net/wireless/ti/wlcore/testmode.c b/drivers/net/wireless/ti/wlcore/testmode.c
index 0e59ea2cdd3..49e5ee1525c 100644
--- a/drivers/net/wireless/ti/wlcore/testmode.c
+++ b/drivers/net/wireless/ti/wlcore/testmode.c
@@ -40,7 +40,7 @@ enum wl1271_tm_commands {
 	WL1271_TM_CMD_CONFIGURE,
 	WL1271_TM_CMD_NVS_PUSH,		/* Not in use. Keep to not break ABI */
 	WL1271_TM_CMD_SET_PLT_MODE,
-	WL1271_TM_CMD_RECOVER,
+	WL1271_TM_CMD_RECOVER,		/* Not in use. Keep to not break ABI */
 	WL1271_TM_CMD_GET_MAC,
 
 	__WL1271_TM_CMD_AFTER_LAST
@@ -108,6 +108,20 @@ static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
 	}
 
 	if (answer) {
+		/* If we got bip calibration answer print radio status */
+		struct wl1271_cmd_cal_p2g *params =
+			(struct wl1271_cmd_cal_p2g *) buf;
+
+		s16 radio_status = (s16) le16_to_cpu(params->radio_status);
+
+		if (params->test.id == TEST_CMD_P2G_CAL &&
+		    radio_status < 0)
+			wl1271_warning("testmode cmd: radio status=%d",
+					radio_status);
+		else
+			wl1271_info("testmode cmd: radio status=%d",
+					radio_status);
+
 		len = nla_total_size(buf_len);
 		skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
 		if (!skb) {
@@ -115,8 +129,12 @@ static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
 			goto out_sleep;
 		}
 
-		if (nla_put(skb, WL1271_TM_ATTR_DATA, buf_len, buf))
-			goto nla_put_failure;
+		if (nla_put(skb, WL1271_TM_ATTR_DATA, buf_len, buf)) {
+			kfree_skb(skb);
+			ret = -EMSGSIZE;
+			goto out_sleep;
+		}
+
 		ret = cfg80211_testmode_reply(skb);
 		if (ret < 0)
 			goto out_sleep;
@@ -128,11 +146,6 @@ out:
 	mutex_unlock(&wl->mutex);
 
 	return ret;
-
-nla_put_failure:
-	kfree_skb(skb);
-	ret = -EMSGSIZE;
-	goto out_sleep;
 }
 
 static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
@@ -178,8 +191,12 @@ static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
 		goto out_free;
 	}
 
-	if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd))
-		goto nla_put_failure;
+	if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd)) {
+		kfree_skb(skb);
+		ret = -EMSGSIZE;
+		goto out_free;
+	}
+
 	ret = cfg80211_testmode_reply(skb);
 	if (ret < 0)
 		goto out_free;
@@ -192,11 +209,6 @@ out:
 	mutex_unlock(&wl->mutex);
 
 	return ret;
-
-nla_put_failure:
-	kfree_skb(skb);
-	ret = -EMSGSIZE;
-	goto out_free;
 }
 
 static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[])
@@ -231,6 +243,43 @@ static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[])
 	return 0;
 }
 
+static int wl1271_tm_detect_fem(struct wl1271 *wl, struct nlattr *tb[])
+{
+	/* return FEM type */
+	int ret, len;
+	struct sk_buff *skb;
+
+	ret = wl1271_plt_start(wl, PLT_FEM_DETECT);
+	if (ret < 0)
+		goto out;
+
+	mutex_lock(&wl->mutex);
+
+	len = nla_total_size(sizeof(wl->fem_manuf));
+	skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
+	if (!skb) {
+		ret = -ENOMEM;
+		goto out_mutex;
+	}
+
+	if (nla_put(skb, WL1271_TM_ATTR_DATA, sizeof(wl->fem_manuf),
+					      &wl->fem_manuf)) {
+		kfree_skb(skb);
+		ret = -EMSGSIZE;
+		goto out_mutex;
+	}
+
+	ret = cfg80211_testmode_reply(skb);
+
+out_mutex:
+	mutex_unlock(&wl->mutex);
+
+	/* We always stop plt after DETECT mode */
+	wl1271_plt_stop(wl);
+out:
+	return ret;
+}
+
 static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[])
 {
 	u32 val;
@@ -244,11 +293,14 @@ static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[])
 	val = nla_get_u32(tb[WL1271_TM_ATTR_PLT_MODE]);
 
 	switch (val) {
-	case 0:
+	case PLT_OFF:
 		ret = wl1271_plt_stop(wl);
 		break;
-	case 1:
-		ret = wl1271_plt_start(wl);
+	case PLT_ON:
+		ret = wl1271_plt_start(wl, PLT_ON);
+		break;
+	case PLT_FEM_DETECT:
+		ret = wl1271_tm_detect_fem(wl, tb);
 		break;
 	default:
 		ret = -EINVAL;
@@ -258,15 +310,6 @@ static int wl1271_tm_cmd_set_plt_mode(struct wl1271 *wl, struct nlattr *tb[])
 	return ret;
 }
 
-static int wl1271_tm_cmd_recover(struct wl1271 *wl, struct nlattr *tb[])
-{
-	wl1271_debug(DEBUG_TESTMODE, "testmode cmd recover");
-
-	wl12xx_queue_recovery_work(wl);
-
-	return 0;
-}
-
 static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[])
 {
 	struct sk_buff *skb;
@@ -298,8 +341,12 @@ static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[])
 		goto out;
 	}
 
-	if (nla_put(skb, WL1271_TM_ATTR_DATA, ETH_ALEN, mac_addr))
-		goto nla_put_failure;
+	if (nla_put(skb, WL1271_TM_ATTR_DATA, ETH_ALEN, mac_addr)) {
+		kfree_skb(skb);
+		ret = -EMSGSIZE;
+		goto out;
+	}
+
 	ret = cfg80211_testmode_reply(skb);
 	if (ret < 0)
 		goto out;
@@ -307,11 +354,6 @@ static int wl12xx_tm_cmd_get_mac(struct wl1271 *wl, struct nlattr *tb[])
 out:
 	mutex_unlock(&wl->mutex);
 	return ret;
-
-nla_put_failure:
-	kfree_skb(skb);
-	ret = -EMSGSIZE;
-	goto out;
 }
 
 int wl1271_tm_cmd(struct ieee80211_hw *hw, void *data, int len)
@@ -336,8 +378,6 @@ int wl1271_tm_cmd(struct ieee80211_hw *hw, void *data, int len)
 		return wl1271_tm_cmd_configure(wl, tb);
 	case WL1271_TM_CMD_SET_PLT_MODE:
 		return wl1271_tm_cmd_set_plt_mode(wl, tb);
-	case WL1271_TM_CMD_RECOVER:
-		return wl1271_tm_cmd_recover(wl, tb);
 	case WL1271_TM_CMD_GET_MAC:
 		return wl12xx_tm_cmd_get_mac(wl, tb);
 	default:
diff --git a/drivers/net/wireless/ti/wlcore/tx.c b/drivers/net/wireless/ti/wlcore/tx.c
index 6893bc20799..f0081f74648 100644
--- a/drivers/net/wireless/ti/wlcore/tx.c
+++ b/drivers/net/wireless/ti/wlcore/tx.c
@@ -72,7 +72,7 @@ static int wl1271_alloc_tx_id(struct wl1271 *wl, struct sk_buff *skb)
 	return id;
 }
 
-static void wl1271_free_tx_id(struct wl1271 *wl, int id)
+void wl1271_free_tx_id(struct wl1271 *wl, int id)
 {
 	if (__test_and_clear_bit(id, wl->tx_frames_map)) {
 		if (unlikely(wl->tx_frames_cnt == wl->num_tx_desc))
@@ -82,6 +82,7 @@ static void wl1271_free_tx_id(struct wl1271 *wl, int id)
 		wl->tx_frames_cnt--;
 	}
 }
+EXPORT_SYMBOL(wl1271_free_tx_id);
 
 static void wl1271_tx_ap_update_inconnection_sta(struct wl1271 *wl,
 						 struct sk_buff *skb)
@@ -127,6 +128,7 @@ bool wl12xx_is_dummy_packet(struct wl1271 *wl, struct sk_buff *skb)
 {
 	return wl->dummy_packet == skb;
 }
+EXPORT_SYMBOL(wl12xx_is_dummy_packet);
 
 u8 wl12xx_tx_get_hlid_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			 struct sk_buff *skb)
@@ -146,10 +148,10 @@ u8 wl12xx_tx_get_hlid_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			return wl->system_hlid;
 
 		hdr = (struct ieee80211_hdr *)skb->data;
-		if (ieee80211_is_mgmt(hdr->frame_control))
-			return wlvif->ap.global_hlid;
-		else
+		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
 			return wlvif->ap.bcast_hlid;
+		else
+			return wlvif->ap.global_hlid;
 	}
 }
 
@@ -176,37 +178,34 @@ u8 wl12xx_tx_get_hlid(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 unsigned int wlcore_calc_packet_alignment(struct wl1271 *wl,
 					  unsigned int packet_length)
 {
-	if (wl->quirks & WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN)
-		return ALIGN(packet_length, WL12XX_BUS_BLOCK_SIZE);
-	else
+	if ((wl->quirks & WLCORE_QUIRK_TX_PAD_LAST_FRAME) ||
+	    !(wl->quirks & WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN))
 		return ALIGN(packet_length, WL1271_TX_ALIGN_TO);
+	else
+		return ALIGN(packet_length, WL12XX_BUS_BLOCK_SIZE);
 }
 EXPORT_SYMBOL(wlcore_calc_packet_alignment);
 
 static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 			      struct sk_buff *skb, u32 extra, u32 buf_offset,
-			      u8 hlid)
+			      u8 hlid, bool is_gem)
 {
 	struct wl1271_tx_hw_descr *desc;
 	u32 total_len = skb->len + sizeof(struct wl1271_tx_hw_descr) + extra;
 	u32 total_blocks;
 	int id, ret = -EBUSY, ac;
-	u32 spare_blocks = wl->normal_tx_spare;
-	bool is_dummy = false;
+	u32 spare_blocks;
 
 	if (buf_offset + total_len > WL1271_AGGR_BUFFER_SIZE)
 		return -EAGAIN;
 
+	spare_blocks = wlcore_hw_get_spare_blocks(wl, is_gem);
+
 	/* allocate free identifier for the packet */
 	id = wl1271_alloc_tx_id(wl, skb);
 	if (id < 0)
 		return id;
 
-	if (unlikely(wl12xx_is_dummy_packet(wl, skb)))
-		is_dummy = true;
-	else if (wlvif->is_gem)
-		spare_blocks = wl->gem_tx_spare;
-
 	total_blocks = wlcore_hw_calc_tx_blocks(wl, total_len, spare_blocks);
 
 	if (total_blocks <= wl->tx_blocks_available) {
@@ -228,7 +227,7 @@ static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		ac = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 		wl->tx_allocated_pkts[ac]++;
 
-		if (!is_dummy && wlvif &&
+		if (!wl12xx_is_dummy_packet(wl, skb) && wlvif &&
 		    wlvif->bss_type == BSS_TYPE_AP_BSS &&
 		    test_bit(hlid, wlvif->ap.sta_hlid_map))
 			wl->links[hlid].allocated_pkts++;
@@ -268,6 +267,7 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	if (extra) {
 		int hdrlen = ieee80211_hdrlen(frame_control);
 		memmove(frame_start, hdr, hdrlen);
+		skb_set_network_header(skb, skb_network_offset(skb) + extra);
 	}
 
 	/* configure packet life time */
@@ -305,19 +305,25 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	if (is_dummy || !wlvif)
 		rate_idx = 0;
 	else if (wlvif->bss_type != BSS_TYPE_AP_BSS) {
-		/* if the packets are destined for AP (have a STA entry)
-		   send them with AP rate policies, otherwise use default
-		   basic rates */
-		if (control->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
+		/*
+		 * if the packets are data packets
+		 * send them with AP rate policies (EAPOLs are an exception),
+		 * otherwise use default basic rates
+		 */
+		if (skb->protocol == cpu_to_be16(ETH_P_PAE))
+			rate_idx = wlvif->sta.basic_rate_idx;
+		else if (control->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
 			rate_idx = wlvif->sta.p2p_rate_idx;
-		else if (control->control.sta)
+		else if (ieee80211_is_data(frame_control))
 			rate_idx = wlvif->sta.ap_rate_idx;
 		else
 			rate_idx = wlvif->sta.basic_rate_idx;
 	} else {
 		if (hlid == wlvif->ap.global_hlid)
 			rate_idx = wlvif->ap.mgmt_rate_idx;
-		else if (hlid == wlvif->ap.bcast_hlid)
+		else if (hlid == wlvif->ap.bcast_hlid ||
+			 skb->protocol == cpu_to_be16(ETH_P_PAE))
+			/* send AP bcast and EAPOLs using the min basic rate */
 			rate_idx = wlvif->ap.bcast_rate_idx;
 		else
 			rate_idx = wlvif->ap.ucast_rate_idx[ac];
@@ -330,9 +336,9 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	    ieee80211_has_protected(frame_control))
 		tx_attr |= TX_HW_ATTR_HOST_ENCRYPT;
 
-	desc->reserved = 0;
 	desc->tx_attr = cpu_to_le16(tx_attr);
 
+	wlcore_hw_set_tx_desc_csum(wl, desc, skb);
 	wlcore_hw_set_tx_desc_data_len(wl, desc, skb);
 }
 
@@ -346,16 +352,20 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 	u32 total_len;
 	u8 hlid;
 	bool is_dummy;
+	bool is_gem = false;
 
-	if (!skb)
+	if (!skb) {
+		wl1271_error("discarding null skb");
 		return -EINVAL;
+	}
 
 	info = IEEE80211_SKB_CB(skb);
 
 	/* TODO: handle dummy packets on multi-vifs */
 	is_dummy = wl12xx_is_dummy_packet(wl, skb);
 
-	if (info->control.hw_key &&
+	if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
+	    info->control.hw_key &&
 	    info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
 		extra = WL1271_EXTRA_SPACE_TKIP;
 
@@ -373,6 +383,8 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 				return ret;
 			wlvif->default_key = idx;
 		}
+
+		is_gem = (cipher == WL1271_CIPHER_SUITE_GEM);
 	}
 	hlid = wl12xx_tx_get_hlid(wl, wlvif, skb);
 	if (hlid == WL12XX_INVALID_LINK_ID) {
@@ -380,7 +392,8 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 		return -EINVAL;
 	}
 
-	ret = wl1271_tx_allocate(wl, wlvif, skb, extra, buf_offset, hlid);
+	ret = wl1271_tx_allocate(wl, wlvif, skb, extra, buf_offset, hlid,
+				 is_gem);
 	if (ret < 0)
 		return ret;
 
@@ -425,10 +438,10 @@ u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set,
 		rate_set >>= 1;
 	}
 
-	/* MCS rates indication are on bits 16 - 23 */
+	/* MCS rates indication are on bits 16 - 31 */
 	rate_set >>= HW_HT_RATES_OFFSET - band->n_bitrates;
 
-	for (bit = 0; bit < 8; bit++) {
+	for (bit = 0; bit < 16; bit++) {
 		if (rate_set & 0x1)
 			enabled_rates |= (CONF_HW_BIT_RATE_MCS_0 << bit);
 		rate_set >>= 1;
@@ -439,18 +452,15 @@ u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set,
 
 void wl1271_handle_tx_low_watermark(struct wl1271 *wl)
 {
-	unsigned long flags;
 	int i;
 
 	for (i = 0; i < NUM_TX_QUEUES; i++) {
-		if (test_bit(i, &wl->stopped_queues_map) &&
+		if (wlcore_is_queue_stopped_by_reason(wl, i,
+			WLCORE_QUEUE_STOP_REASON_WATERMARK) &&
 		    wl->tx_queue_count[i] <= WL1271_TX_QUEUE_LOW_WATERMARK) {
 			/* firmware buffer has space, restart queues */
-			spin_lock_irqsave(&wl->wl_lock, flags);
-			ieee80211_wake_queue(wl->hw,
-					     wl1271_tx_get_mac80211_queue(i));
-			clear_bit(i, &wl->stopped_queues_map);
-			spin_unlock_irqrestore(&wl->wl_lock, flags);
+			wlcore_wake_queue(wl, i,
+					  WLCORE_QUEUE_STOP_REASON_WATERMARK);
 		}
 	}
 }
@@ -656,18 +666,29 @@ void wl12xx_rearm_rx_streaming(struct wl1271 *wl, unsigned long *active_hlids)
 	}
 }
 
-void wl1271_tx_work_locked(struct wl1271 *wl)
+/*
+ * Returns failure values only in case of failed bus ops within this function.
+ * wl1271_prepare_tx_frame retvals won't be returned in order to avoid
+ * triggering recovery by higher layers when not necessary.
+ * In case a FW command fails within wl1271_prepare_tx_frame fails a recovery
+ * will be queued in wl1271_cmd_send. -EAGAIN/-EBUSY from prepare_tx_frame
+ * can occur and are legitimate so don't propagate. -EINVAL will emit a WARNING
+ * within prepare_tx_frame code but there's nothing we should do about those
+ * as well.
+ */
+int wlcore_tx_work_locked(struct wl1271 *wl)
 {
 	struct wl12xx_vif *wlvif;
 	struct sk_buff *skb;
 	struct wl1271_tx_hw_descr *desc;
-	u32 buf_offset = 0;
+	u32 buf_offset = 0, last_len = 0;
 	bool sent_packets = false;
 	unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
-	int ret;
+	int ret = 0;
+	int bus_ret = 0;
 
 	if (unlikely(wl->state == WL1271_STATE_OFF))
-		return;
+		return 0;
 
 	while ((skb = wl1271_skb_dequeue(wl))) {
 		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -685,8 +706,14 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 			 * Flush buffer and try again.
 			 */
 			wl1271_skb_queue_head(wl, wlvif, skb);
-			wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
-					  buf_offset, true);
+
+			buf_offset = wlcore_hw_pre_pkt_send(wl, buf_offset,
+							    last_len);
+			bus_ret = wlcore_write_data(wl, REG_SLV_MEM_DATA,
+					     wl->aggr_buf, buf_offset, true);
+			if (bus_ret < 0)
+				goto out;
+
 			sent_packets = true;
 			buf_offset = 0;
 			continue;
@@ -710,7 +737,8 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 				ieee80211_free_txskb(wl->hw, skb);
 			goto out_ack;
 		}
-		buf_offset += ret;
+		last_len = ret;
+		buf_offset += last_len;
 		wl->tx_packets_count++;
 		if (has_data) {
 			desc = (struct wl1271_tx_hw_descr *) skb->data;
@@ -720,8 +748,12 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 
 out_ack:
 	if (buf_offset) {
-		wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
-				  buf_offset, true);
+		buf_offset = wlcore_hw_pre_pkt_send(wl, buf_offset, last_len);
+		bus_ret = wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+					     buf_offset, true);
+		if (bus_ret < 0)
+			goto out;
+
 		sent_packets = true;
 	}
 	if (sent_packets) {
@@ -729,13 +761,19 @@ out_ack:
 		 * Interrupt the firmware with the new packets. This is only
 		 * required for older hardware revisions
 		 */
-		if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION)
-			wl1271_write32(wl, WL12XX_HOST_WR_ACCESS,
-				       wl->tx_packets_count);
+		if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION) {
+			bus_ret = wlcore_write32(wl, WL12XX_HOST_WR_ACCESS,
+					     wl->tx_packets_count);
+			if (bus_ret < 0)
+				goto out;
+		}
 
 		wl1271_handle_tx_low_watermark(wl);
 	}
 	wl12xx_rearm_rx_streaming(wl, active_hlids);
+
+out:
+	return bus_ret;
 }
 
 void wl1271_tx_work(struct work_struct *work)
@@ -748,7 +786,11 @@ void wl1271_tx_work(struct work_struct *work)
 	if (ret < 0)
 		goto out;
 
-	wl1271_tx_work_locked(wl);
+	ret = wlcore_tx_work_locked(wl);
+	if (ret < 0) {
+		wl12xx_queue_recovery_work(wl);
+		goto out;
+	}
 
 	wl1271_ps_elp_sleep(wl);
 out:
@@ -849,7 +891,8 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 	skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
 
 	/* remove TKIP header space if present */
-	if (info->control.hw_key &&
+	if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
+	    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_EXTRA_SPACE_TKIP, skb->data,
@@ -869,22 +912,27 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 }
 
 /* Called upon reception of a TX complete interrupt */
-void wl1271_tx_complete(struct wl1271 *wl)
+int wlcore_tx_complete(struct wl1271 *wl)
 {
-	struct wl1271_acx_mem_map *memmap =
-		(struct wl1271_acx_mem_map *)wl->target_mem_map;
+	struct wl1271_acx_mem_map *memmap = wl->target_mem_map;
 	u32 count, fw_counter;
 	u32 i;
+	int ret;
 
 	/* read the tx results from the chipset */
-	wl1271_read(wl, le32_to_cpu(memmap->tx_result),
-		    wl->tx_res_if, sizeof(*wl->tx_res_if), false);
+	ret = wlcore_read(wl, le32_to_cpu(memmap->tx_result),
+			  wl->tx_res_if, sizeof(*wl->tx_res_if), false);
+	if (ret < 0)
+		goto out;
+
 	fw_counter = le32_to_cpu(wl->tx_res_if->tx_result_fw_counter);
 
 	/* write host counter to chipset (to ack) */
-	wl1271_write32(wl, le32_to_cpu(memmap->tx_result) +
-		       offsetof(struct wl1271_tx_hw_res_if,
-				tx_result_host_counter), fw_counter);
+	ret = wlcore_write32(wl, le32_to_cpu(memmap->tx_result) +
+			     offsetof(struct wl1271_tx_hw_res_if,
+				      tx_result_host_counter), fw_counter);
+	if (ret < 0)
+		goto out;
 
 	count = fw_counter - wl->tx_results_count;
 	wl1271_debug(DEBUG_TX, "tx_complete received, packets: %d", count);
@@ -904,8 +952,11 @@ void wl1271_tx_complete(struct wl1271 *wl)
 
 		wl->tx_results_count++;
 	}
+
+out:
+	return ret;
 }
-EXPORT_SYMBOL(wl1271_tx_complete);
+EXPORT_SYMBOL(wlcore_tx_complete);
 
 void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid)
 {
@@ -958,7 +1009,7 @@ void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif)
 
 }
 /* caller must hold wl->mutex and TX must be stopped */
-void wl12xx_tx_reset(struct wl1271 *wl, bool reset_tx_queues)
+void wl12xx_tx_reset(struct wl1271 *wl)
 {
 	int i;
 	struct sk_buff *skb;
@@ -973,15 +1024,12 @@ void wl12xx_tx_reset(struct wl1271 *wl, bool reset_tx_queues)
 			wl->tx_queue_count[i] = 0;
 	}
 
-	wl->stopped_queues_map = 0;
-
 	/*
 	 * Make sure the driver is at a consistent state, in case this
 	 * function is called from a context other than interface removal.
 	 * This call will always wake the TX queues.
 	 */
-	if (reset_tx_queues)
-		wl1271_handle_tx_low_watermark(wl);
+	wl1271_handle_tx_low_watermark(wl);
 
 	for (i = 0; i < wl->num_tx_desc; i++) {
 		if (wl->tx_frames[i] == NULL)
@@ -998,7 +1046,8 @@ void wl12xx_tx_reset(struct wl1271 *wl, bool reset_tx_queues)
 			 */
 			info = IEEE80211_SKB_CB(skb);
 			skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
-			if (info->control.hw_key &&
+			if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
+			    info->control.hw_key &&
 			    info->control.hw_key->cipher ==
 			    WLAN_CIPHER_SUITE_TKIP) {
 				int hdrlen = ieee80211_get_hdrlen_from_skb(skb);
@@ -1024,6 +1073,11 @@ void wl1271_tx_flush(struct wl1271 *wl)
 	int i;
 	timeout = jiffies + usecs_to_jiffies(WL1271_TX_FLUSH_TIMEOUT);
 
+	/* only one flush should be in progress, for consistent queue state */
+	mutex_lock(&wl->flush_mutex);
+
+	wlcore_stop_queues(wl, WLCORE_QUEUE_STOP_REASON_FLUSH);
+
 	while (!time_after(jiffies, timeout)) {
 		mutex_lock(&wl->mutex);
 		wl1271_debug(DEBUG_TX, "flushing tx buffer: %d %d",
@@ -1032,7 +1086,7 @@ void wl1271_tx_flush(struct wl1271 *wl)
 		if ((wl->tx_frames_cnt == 0) &&
 		    (wl1271_tx_total_queue_count(wl) == 0)) {
 			mutex_unlock(&wl->mutex);
-			return;
+			goto out;
 		}
 		mutex_unlock(&wl->mutex);
 		msleep(1);
@@ -1045,7 +1099,12 @@ void wl1271_tx_flush(struct wl1271 *wl)
 	for (i = 0; i < WL12XX_MAX_LINKS; i++)
 		wl1271_tx_reset_link_queues(wl, i);
 	mutex_unlock(&wl->mutex);
+
+out:
+	wlcore_wake_queues(wl, WLCORE_QUEUE_STOP_REASON_FLUSH);
+	mutex_unlock(&wl->flush_mutex);
 }
+EXPORT_SYMBOL_GPL(wl1271_tx_flush);
 
 u32 wl1271_tx_min_rate_get(struct wl1271 *wl, u32 rate_set)
 {
@@ -1054,3 +1113,96 @@ u32 wl1271_tx_min_rate_get(struct wl1271 *wl, u32 rate_set)
 
 	return BIT(__ffs(rate_set));
 }
+
+void wlcore_stop_queue_locked(struct wl1271 *wl, u8 queue,
+			      enum wlcore_queue_stop_reason reason)
+{
+	bool stopped = !!wl->queue_stop_reasons[queue];
+
+	/* queue should not be stopped for this reason */
+	WARN_ON(test_and_set_bit(reason, &wl->queue_stop_reasons[queue]));
+
+	if (stopped)
+		return;
+
+	ieee80211_stop_queue(wl->hw, wl1271_tx_get_mac80211_queue(queue));
+}
+
+void wlcore_stop_queue(struct wl1271 *wl, u8 queue,
+		       enum wlcore_queue_stop_reason reason)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->wl_lock, flags);
+	wlcore_stop_queue_locked(wl, queue, reason);
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+}
+
+void wlcore_wake_queue(struct wl1271 *wl, u8 queue,
+		       enum wlcore_queue_stop_reason reason)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->wl_lock, flags);
+
+	/* queue should not be clear for this reason */
+	WARN_ON(!test_and_clear_bit(reason, &wl->queue_stop_reasons[queue]));
+
+	if (wl->queue_stop_reasons[queue])
+		goto out;
+
+	ieee80211_wake_queue(wl->hw, wl1271_tx_get_mac80211_queue(queue));
+
+out:
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+}
+
+void wlcore_stop_queues(struct wl1271 *wl,
+			enum wlcore_queue_stop_reason reason)
+{
+	int i;
+
+	for (i = 0; i < NUM_TX_QUEUES; i++)
+		wlcore_stop_queue(wl, i, reason);
+}
+EXPORT_SYMBOL_GPL(wlcore_stop_queues);
+
+void wlcore_wake_queues(struct wl1271 *wl,
+			enum wlcore_queue_stop_reason reason)
+{
+	int i;
+
+	for (i = 0; i < NUM_TX_QUEUES; i++)
+		wlcore_wake_queue(wl, i, reason);
+}
+EXPORT_SYMBOL_GPL(wlcore_wake_queues);
+
+void wlcore_reset_stopped_queues(struct wl1271 *wl)
+{
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->wl_lock, flags);
+
+	for (i = 0; i < NUM_TX_QUEUES; i++) {
+		if (!wl->queue_stop_reasons[i])
+			continue;
+
+		wl->queue_stop_reasons[i] = 0;
+		ieee80211_wake_queue(wl->hw,
+				     wl1271_tx_get_mac80211_queue(i));
+	}
+
+	spin_unlock_irqrestore(&wl->wl_lock, flags);
+}
+
+bool wlcore_is_queue_stopped_by_reason(struct wl1271 *wl, u8 queue,
+			     enum wlcore_queue_stop_reason reason)
+{
+	return test_bit(reason, &wl->queue_stop_reasons[queue]);
+}
+
+bool wlcore_is_queue_stopped(struct wl1271 *wl, u8 queue)
+{
+	return !!wl->queue_stop_reasons[queue];
+}
diff --git a/drivers/net/wireless/ti/wlcore/tx.h b/drivers/net/wireless/ti/wlcore/tx.h
index 2fd6e5dc6f7..1e939b01615 100644
--- a/drivers/net/wireless/ti/wlcore/tx.h
+++ b/drivers/net/wireless/ti/wlcore/tx.h
@@ -85,6 +85,19 @@ struct wl128x_tx_mem {
 	u8 extra_bytes;
 } __packed;
 
+struct wl18xx_tx_mem {
+	/*
+	 * Total number of memory blocks allocated by the host for
+	 * this packet.
+	 */
+	u8 total_mem_blocks;
+
+	/*
+	 * control bits
+	 */
+	u8 ctrl;
+} __packed;
+
 /*
  * On wl128x based devices, when TX packets are aggregated, each packet
  * size must be aligned to the SDIO block size. The maximum block size
@@ -100,6 +113,7 @@ struct wl1271_tx_hw_descr {
 	union {
 		struct wl127x_tx_mem wl127x_mem;
 		struct wl128x_tx_mem wl128x_mem;
+		struct wl18xx_tx_mem wl18xx_mem;
 	} __packed;
 	/* Device time (in us) when the packet arrived to the driver */
 	__le32 start_time;
@@ -116,7 +130,16 @@ struct wl1271_tx_hw_descr {
 	u8 tid;
 	/* host link ID (HLID) */
 	u8 hlid;
-	u8 reserved;
+
+	union {
+		u8 wl12xx_reserved;
+
+		/*
+		 * bit 0   -> 0 = udp, 1 = tcp
+		 * bit 1:7 -> IP header offset
+		 */
+		u8 wl18xx_checksum_data;
+	} __packed;
 } __packed;
 
 enum wl1271_tx_hw_res_status {
@@ -161,6 +184,13 @@ struct wl1271_tx_hw_res_if {
 	struct wl1271_tx_hw_res_descr tx_results_queue[TX_HW_RESULT_QUEUE_LEN];
 } __packed;
 
+enum wlcore_queue_stop_reason {
+	WLCORE_QUEUE_STOP_REASON_WATERMARK,
+	WLCORE_QUEUE_STOP_REASON_FW_RESTART,
+	WLCORE_QUEUE_STOP_REASON_FLUSH,
+	WLCORE_QUEUE_STOP_REASON_SPARE_BLK, /* 18xx specific */
+};
+
 static inline int wl1271_tx_get_queue(int queue)
 {
 	switch (queue) {
@@ -204,10 +234,10 @@ static inline int wl1271_tx_total_queue_count(struct wl1271 *wl)
 }
 
 void wl1271_tx_work(struct work_struct *work);
-void wl1271_tx_work_locked(struct wl1271 *wl);
-void wl1271_tx_complete(struct wl1271 *wl);
+int wlcore_tx_work_locked(struct wl1271 *wl);
+int wlcore_tx_complete(struct wl1271 *wl);
 void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif);
-void wl12xx_tx_reset(struct wl1271 *wl, bool reset_tx_queues);
+void wl12xx_tx_reset(struct wl1271 *wl);
 void wl1271_tx_flush(struct wl1271 *wl);
 u8 wlcore_rate_to_idx(struct wl1271 *wl, u8 rate, enum ieee80211_band band);
 u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set,
@@ -223,6 +253,21 @@ bool wl12xx_is_dummy_packet(struct wl1271 *wl, struct sk_buff *skb);
 void wl12xx_rearm_rx_streaming(struct wl1271 *wl, unsigned long *active_hlids);
 unsigned int wlcore_calc_packet_alignment(struct wl1271 *wl,
 					  unsigned int packet_length);
+void wl1271_free_tx_id(struct wl1271 *wl, int id);
+void wlcore_stop_queue_locked(struct wl1271 *wl, u8 queue,
+			      enum wlcore_queue_stop_reason reason);
+void wlcore_stop_queue(struct wl1271 *wl, u8 queue,
+		       enum wlcore_queue_stop_reason reason);
+void wlcore_wake_queue(struct wl1271 *wl, u8 queue,
+		       enum wlcore_queue_stop_reason reason);
+void wlcore_stop_queues(struct wl1271 *wl,
+			enum wlcore_queue_stop_reason reason);
+void wlcore_wake_queues(struct wl1271 *wl,
+			enum wlcore_queue_stop_reason reason);
+void wlcore_reset_stopped_queues(struct wl1271 *wl);
+bool wlcore_is_queue_stopped_by_reason(struct wl1271 *wl, u8 queue,
+				       enum wlcore_queue_stop_reason reason);
+bool wlcore_is_queue_stopped(struct wl1271 *wl, u8 queue);
 
 /* from main.c */
 void wl1271_free_sta(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 hlid);
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index 0b3f0b586f4..0ce7a8ebbd4 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -24,8 +24,9 @@
 
 #include <linux/platform_device.h>
 
-#include "wl12xx.h"
+#include "wlcore_i.h"
 #include "event.h"
+#include "boot.h"
 
 /* The maximum number of Tx descriptors in all chip families */
 #define WLCORE_MAX_TX_DESCRIPTORS 32
@@ -33,14 +34,16 @@
 /* forward declaration */
 struct wl1271_tx_hw_descr;
 enum wl_rx_buf_align;
+struct wl1271_rx_descriptor;
 
 struct wlcore_ops {
 	int (*identify_chip)(struct wl1271 *wl);
 	int (*identify_fw)(struct wl1271 *wl);
 	int (*boot)(struct wl1271 *wl);
-	void (*trigger_cmd)(struct wl1271 *wl, int cmd_box_addr,
-			    void *buf, size_t len);
-	void (*ack_event)(struct wl1271 *wl);
+	int (*plt_init)(struct wl1271 *wl);
+	int (*trigger_cmd)(struct wl1271 *wl, int cmd_box_addr,
+			   void *buf, size_t len);
+	int (*ack_event)(struct wl1271 *wl);
 	u32 (*calc_tx_blocks)(struct wl1271 *wl, u32 len, u32 spare_blks);
 	void (*set_tx_desc_blocks)(struct wl1271 *wl,
 				   struct wl1271_tx_hw_descr *desc,
@@ -50,17 +53,34 @@ struct wlcore_ops {
 				     struct sk_buff *skb);
 	enum wl_rx_buf_align (*get_rx_buf_align)(struct wl1271 *wl,
 						 u32 rx_desc);
-	void (*prepare_read)(struct wl1271 *wl, u32 rx_desc, u32 len);
+	int (*prepare_read)(struct wl1271 *wl, u32 rx_desc, u32 len);
 	u32 (*get_rx_packet_len)(struct wl1271 *wl, void *rx_data,
 				 u32 data_len);
-	void (*tx_delayed_compl)(struct wl1271 *wl);
+	int (*tx_delayed_compl)(struct wl1271 *wl);
 	void (*tx_immediate_compl)(struct wl1271 *wl);
 	int (*hw_init)(struct wl1271 *wl);
 	int (*init_vif)(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 	u32 (*sta_get_ap_rate_mask)(struct wl1271 *wl,
 				    struct wl12xx_vif *wlvif);
-	s8 (*get_pg_ver)(struct wl1271 *wl);
-	void (*get_mac)(struct wl1271 *wl);
+	int (*get_pg_ver)(struct wl1271 *wl, s8 *ver);
+	int (*get_mac)(struct wl1271 *wl);
+	void (*set_tx_desc_csum)(struct wl1271 *wl,
+				 struct wl1271_tx_hw_descr *desc,
+				 struct sk_buff *skb);
+	void (*set_rx_csum)(struct wl1271 *wl,
+			    struct wl1271_rx_descriptor *desc,
+			    struct sk_buff *skb);
+	u32 (*ap_get_mimo_wide_rate_mask)(struct wl1271 *wl,
+					  struct wl12xx_vif *wlvif);
+	int (*debugfs_init)(struct wl1271 *wl, struct dentry *rootdir);
+	int (*handle_static_data)(struct wl1271 *wl,
+				  struct wl1271_static_data *static_data);
+	int (*get_spare_blocks)(struct wl1271 *wl, bool is_gem);
+	int (*set_key)(struct wl1271 *wl, enum set_key_cmd cmd,
+		       struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta,
+		       struct ieee80211_key_conf *key_conf);
+	u32 (*pre_pkt_send)(struct wl1271 *wl, u32 buf_offset, u32 last_len);
 };
 
 enum wlcore_partitions {
@@ -109,6 +129,15 @@ enum wlcore_registers {
 	REG_TABLE_LEN,
 };
 
+struct wl1271_stats {
+	void *fw_stats;
+	unsigned long fw_stats_update;
+	size_t fw_stats_len;
+
+	unsigned int retry_count;
+	unsigned int excessive_retries;
+};
+
 struct wl1271 {
 	struct ieee80211_hw *hw;
 	bool mac80211_registered;
@@ -121,13 +150,14 @@ struct wl1271 {
 
 	void (*set_power)(bool enable);
 	int irq;
-	int ref_clock;
 
 	spinlock_t wl_lock;
 
 	enum wl1271_state state;
 	enum wl12xx_fw_type fw_type;
 	bool plt;
+	enum plt_mode plt_mode;
+	u8 fem_manuf;
 	u8 last_vif_count;
 	struct mutex mutex;
 
@@ -186,7 +216,7 @@ struct wl1271 {
 
 	/* Frames scheduled for transmission, not handled yet */
 	int tx_queue_count[NUM_TX_QUEUES];
-	long stopped_queues_map;
+	unsigned long queue_stop_reasons[NUM_TX_QUEUES];
 
 	/* Frames received, not handled yet by mac80211 */
 	struct sk_buff_head deferred_rx_queue;
@@ -205,9 +235,6 @@ struct wl1271 {
 	/* FW Rx counter */
 	u32 rx_counter;
 
-	/* Rx memory pool address */
-	struct wl1271_rx_mem_pool_addr rx_mem_pool_addr;
-
 	/* Intermediate buffer, used for packet aggregation */
 	u8 *aggr_buf;
 
@@ -228,6 +255,7 @@ struct wl1271 {
 
 	/* Hardware recovery work */
 	struct work_struct recovery_work;
+	bool watchdog_recovery;
 
 	/* Pointer that holds DMA-friendly block for the mailbox */
 	struct event_mailbox *mbox;
@@ -263,7 +291,8 @@ struct wl1271 {
 	u32 buffer_cmd;
 	u32 buffer_busyword[WL1271_BUSY_WORD_CNT];
 
-	struct wl_fw_status *fw_status;
+	struct wl_fw_status_1 *fw_status_1;
+	struct wl_fw_status_2 *fw_status_2;
 	struct wl1271_tx_hw_res_if *tx_res_if;
 
 	/* Current chipset configuration */
@@ -277,9 +306,7 @@ struct wl1271 {
 	s8 noise;
 
 	/* bands supported by this instance of wl12xx */
-	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
-
-	int tcxo_clock;
+	struct ieee80211_supported_band bands[WLCORE_NUM_BANDS];
 
 	/*
 	 * wowlan trigger was configured during suspend.
@@ -333,10 +360,8 @@ struct wl1271 {
 
 	/* number of TX descriptors the HW supports. */
 	u32 num_tx_desc;
-
-	/* spare Tx blocks for normal/GEM operating modes */
-	u32 normal_tx_spare;
-	u32 gem_tx_spare;
+	/* number of RX descriptors the HW supports. */
+	u32 num_rx_desc;
 
 	/* translate HW Tx rates to standard rate-indices */
 	const u8 **band_rate_to_idx;
@@ -348,19 +373,57 @@ struct wl1271 {
 	u8 hw_min_ht_rate;
 
 	/* HW HT (11n) capabilities */
-	struct ieee80211_sta_ht_cap ht_cap;
+	struct ieee80211_sta_ht_cap ht_cap[WLCORE_NUM_BANDS];
 
 	/* size of the private FW status data */
 	size_t fw_status_priv_len;
 
 	/* RX Data filter rule state - enabled/disabled */
 	bool rx_filter_enabled[WL1271_MAX_RX_FILTERS];
+
+	/* size of the private static data */
+	size_t static_data_priv_len;
+
+	/* the current channel type */
+	enum nl80211_channel_type channel_type;
+
+	/* mutex for protecting the tx_flush function */
+	struct mutex flush_mutex;
+
+	/* sleep auth value currently configured to FW */
+	int sleep_auth;
+
+	/* the minimum FW version required for the driver to work */
+	unsigned int min_fw_ver[NUM_FW_VER];
 };
 
 int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev);
 int __devexit wlcore_remove(struct platform_device *pdev);
 struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size);
 int wlcore_free_hw(struct wl1271 *wl);
+int wlcore_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
+		   struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta,
+		   struct ieee80211_key_conf *key_conf);
+
+static inline void
+wlcore_set_ht_cap(struct wl1271 *wl, enum ieee80211_band band,
+		  struct ieee80211_sta_ht_cap *ht_cap)
+{
+	memcpy(&wl->ht_cap[band], ht_cap, sizeof(*ht_cap));
+}
+
+static inline void
+wlcore_set_min_fw_ver(struct wl1271 *wl, unsigned int chip,
+		      unsigned int iftype, unsigned int major,
+		      unsigned int subtype, unsigned int minor)
+{
+	wl->min_fw_ver[FW_VER_CHIP] = chip;
+	wl->min_fw_ver[FW_VER_IF_TYPE] = iftype;
+	wl->min_fw_ver[FW_VER_MAJOR] = major;
+	wl->min_fw_ver[FW_VER_SUBTYPE] = subtype;
+	wl->min_fw_ver[FW_VER_MINOR] = minor;
+}
 
 /* Firmware image load chunk size */
 #define CHUNK_SIZE	16384
@@ -385,6 +448,18 @@ int wlcore_free_hw(struct wl1271 *wl);
 /* Some firmwares may not support ELP */
 #define WLCORE_QUIRK_NO_ELP			BIT(6)
 
+/* pad only the last frame in the aggregate buffer */
+#define WLCORE_QUIRK_TX_PAD_LAST_FRAME		BIT(7)
+
+/* extra header space is required for TKIP */
+#define WLCORE_QUIRK_TKIP_HEADER_SPACE		BIT(8)
+
+/* Some firmwares not support sched scans while connected */
+#define WLCORE_QUIRK_NO_SCHED_SCAN_WHILE_CONN	BIT(9)
+
+/* separate probe response templates for one-shot and sched scans */
+#define WLCORE_QUIRK_DUAL_PROBE_TMPL		BIT(10)
+
 /* TODO: move to the lower drivers when all usages are abstracted */
 #define CHIP_ID_1271_PG10              (0x4030101)
 #define CHIP_ID_1271_PG20              (0x4030111)
diff --git a/drivers/net/wireless/ti/wlcore/wl12xx.h b/drivers/net/wireless/ti/wlcore/wlcore_i.h
index f12bdf74518..c0505635bb0 100644
--- a/drivers/net/wireless/ti/wlcore/wl12xx.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore_i.h
@@ -22,8 +22,8 @@
  *
  */
 
-#ifndef __WL12XX_H__
-#define __WL12XX_H__
+#ifndef __WLCORE_I_H__
+#define __WLCORE_I_H__
 
 #include <linux/mutex.h>
 #include <linux/completion.h>
@@ -35,15 +35,6 @@
 #include "conf.h"
 #include "ini.h"
 
-#define WL127X_FW_NAME_MULTI "ti-connectivity/wl127x-fw-4-mr.bin"
-#define WL127X_FW_NAME_SINGLE "ti-connectivity/wl127x-fw-4-sr.bin"
-
-#define WL128X_FW_NAME_MULTI "ti-connectivity/wl128x-fw-4-mr.bin"
-#define WL128X_FW_NAME_SINGLE "ti-connectivity/wl128x-fw-4-sr.bin"
-
-#define WL127X_PLT_FW_NAME "ti-connectivity/wl127x-fw-4-plt.bin"
-#define WL128X_PLT_FW_NAME "ti-connectivity/wl128x-fw-4-plt.bin"
-
 /*
  * wl127x and wl128x are using the same NVS file name. However, the
  * ini parameters between them are different.  The driver validates
@@ -71,6 +62,9 @@
 #define WL12XX_INVALID_ROLE_ID     0xff
 #define WL12XX_INVALID_LINK_ID     0xff
 
+/* the driver supports the 2.4Ghz and 5Ghz bands */
+#define WLCORE_NUM_BANDS           2
+
 #define WL12XX_MAX_RATE_POLICIES 16
 
 /* Defined by FW as 0. Will not be freed or allocated. */
@@ -89,7 +83,7 @@
 #define WL1271_AP_BSS_INDEX        0
 #define WL1271_AP_DEF_BEACON_EXP   20
 
-#define WL1271_AGGR_BUFFER_SIZE (4 * PAGE_SIZE)
+#define WL1271_AGGR_BUFFER_SIZE (5 * PAGE_SIZE)
 
 enum wl1271_state {
 	WL1271_STATE_OFF,
@@ -132,16 +126,7 @@ struct wl1271_chip {
 	unsigned int fw_ver[NUM_FW_VER];
 };
 
-struct wl1271_stats {
-	struct acx_statistics *fw_stats;
-	unsigned long fw_stats_update;
-
-	unsigned int retry_count;
-	unsigned int excessive_retries;
-};
-
 #define NUM_TX_QUEUES              4
-#define NUM_RX_PKT_DESC            8
 
 #define AP_MAX_STATIONS            8
 
@@ -159,13 +144,26 @@ struct wl_fw_packet_counters {
 } __packed;
 
 /* FW status registers */
-struct wl_fw_status {
+struct wl_fw_status_1 {
 	__le32 intr;
 	u8  fw_rx_counter;
 	u8  drv_rx_counter;
 	u8  reserved;
 	u8  tx_results_counter;
-	__le32 rx_pkt_descs[NUM_RX_PKT_DESC];
+	__le32 rx_pkt_descs[0];
+} __packed;
+
+/*
+ * Each HW arch has a different number of Rx descriptors.
+ * The length of the status depends on it, since it holds an array
+ * of descriptors.
+ */
+#define WLCORE_FW_STATUS_1_LEN(num_rx_desc) \
+		(sizeof(struct wl_fw_status_1) + \
+		(sizeof(((struct wl_fw_status_1 *)0)->rx_pkt_descs[0])) * \
+		num_rx_desc)
+
+struct wl_fw_status_2 {
 	__le32 fw_localtime;
 
 	/*
@@ -194,11 +192,6 @@ struct wl_fw_status {
 	u8 priv[0];
 } __packed;
 
-struct wl1271_rx_mem_pool_addr {
-	u32 addr;
-	u32 addr_extra;
-};
-
 #define WL1271_MAX_CHANNELS 64
 struct wl1271_scan {
 	struct cfg80211_scan_request *req;
@@ -210,10 +203,10 @@ struct wl1271_scan {
 };
 
 struct wl1271_if_operations {
-	void (*read)(struct device *child, int addr, void *buf, size_t len,
-		     bool fixed);
-	void (*write)(struct device *child, int addr, void *buf, size_t len,
-		     bool fixed);
+	int __must_check (*read)(struct device *child, int addr, void *buf,
+				 size_t len, bool fixed);
+	int __must_check (*write)(struct device *child, int addr, void *buf,
+				  size_t len, bool fixed);
 	void (*reset)(struct device *child);
 	void (*init)(struct device *child);
 	int (*power)(struct device *child, bool enable);
@@ -248,6 +241,7 @@ enum wl12xx_flags {
 	WL1271_FLAG_RECOVERY_IN_PROGRESS,
 	WL1271_FLAG_VIF_CHANGE_IN_PROGRESS,
 	WL1271_FLAG_INTENDED_FW_RECOVERY,
+	WL1271_FLAG_IO_FAILED,
 };
 
 enum wl12xx_vif_flags {
@@ -299,6 +293,12 @@ enum rx_filter_action {
 	FILTER_FW_HANDLE = 2
 };
 
+enum plt_mode {
+	PLT_OFF = 0,
+	PLT_ON = 1,
+	PLT_FEM_DETECT = 2,
+};
+
 struct wl12xx_rx_filter_field {
 	__le16 offset;
 	u8 len;
@@ -367,8 +367,9 @@ struct wl12xx_vif {
 	/* The current band */
 	enum ieee80211_band band;
 	int channel;
+	enum nl80211_channel_type channel_type;
 
-	u32 bitrate_masks[IEEE80211_NUM_BANDS];
+	u32 bitrate_masks[WLCORE_NUM_BANDS];
 	u32 basic_rate_set;
 
 	/*
@@ -417,9 +418,6 @@ struct wl12xx_vif {
 	struct work_struct rx_streaming_disable_work;
 	struct timer_list rx_streaming_timer;
 
-	/* does the current role use GEM for encryption (AP or STA) */
-	bool is_gem;
-
 	/*
 	 * This struct must be last!
 	 * data that has to be saved acrossed reconfigs (e.g. recovery)
@@ -467,7 +465,7 @@ struct ieee80211_vif *wl12xx_wlvif_to_vif(struct wl12xx_vif *wlvif)
 #define wl12xx_for_each_wlvif_ap(wl, wlvif)	\
 		wl12xx_for_each_wlvif_bss_type(wl, wlvif, BSS_TYPE_AP_BSS)
 
-int wl1271_plt_start(struct wl1271 *wl);
+int wl1271_plt_start(struct wl1271 *wl, const enum plt_mode plt_mode);
 int wl1271_plt_stop(struct wl1271 *wl);
 int wl1271_recalc_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 void wl12xx_queue_recovery_work(struct wl1271 *wl);
@@ -501,7 +499,8 @@ void wl1271_rx_filter_flatten_fields(struct wl12xx_rx_filter *filter,
 /* Macros to handle wl1271.sta_rate_set */
 #define HW_BG_RATES_MASK	0xffff
 #define HW_HT_RATES_OFFSET	16
+#define HW_MIMO_RATES_OFFSET	24
 
 #define WL12XX_HW_BLOCK_SIZE	256
 
-#endif
+#endif /* __WLCORE_I_H__ */
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.h b/drivers/net/wireless/zd1211rw/zd_chip.h
index 117c4123943..7ab922209b2 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.h
+++ b/drivers/net/wireless/zd1211rw/zd_chip.h
@@ -827,7 +827,7 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values,
 static inline int zd_ioread32_locked(struct zd_chip *chip, u32 *value,
 	                             const zd_addr_t addr)
 {
-	return zd_ioread32v_locked(chip, value, (const zd_addr_t *)&addr, 1);
+	return zd_ioread32v_locked(chip, value, &addr, 1);
 }
 
 static inline int zd_iowrite16_locked(struct zd_chip *chip, u16 value,
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.h b/drivers/net/wireless/zd1211rw/zd_usb.h
index 99193b456a7..45e3bb28a01 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.h
+++ b/drivers/net/wireless/zd1211rw/zd_usb.h
@@ -274,7 +274,7 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
 static inline int zd_usb_ioread16(struct zd_usb *usb, u16 *value,
 	                      const zd_addr_t addr)
 {
-	return zd_usb_ioread16v(usb, value, (const zd_addr_t *)&addr, 1);
+	return zd_usb_ioread16v(usb, value, &addr, 1);
 }
 
 void zd_usb_iowrite16v_async_start(struct zd_usb *usb);
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index f4a6fcaeffb..682633bfe00 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -1363,8 +1363,6 @@ static unsigned xen_netbk_tx_build_gops(struct xen_netbk *netbk)
 					     INVALID_PENDING_IDX);
 		}
 
-		__skb_queue_tail(&netbk->tx_queue, skb);
-
 		netbk->pending_cons++;
 
 		request_gop = xen_netbk_get_requests(netbk, vif,
@@ -1376,6 +1374,8 @@ static unsigned xen_netbk_tx_build_gops(struct xen_netbk *netbk)
 		}
 		gop = request_gop;
 
+		__skb_queue_tail(&netbk->tx_queue, skb);
+
 		vif->tx.req_cons = idx;
 		xen_netbk_check_rx_xenvif(vif);
 
diff --git a/drivers/nfc/nfcwilink.c b/drivers/nfc/nfcwilink.c
index 1f74a77d040..e7fd4938f9b 100644
--- a/drivers/nfc/nfcwilink.c
+++ b/drivers/nfc/nfcwilink.c
@@ -535,9 +535,10 @@ static int nfcwilink_probe(struct platform_device *pdev)
 	drv->pdev = pdev;
 
 	protocols = NFC_PROTO_JEWEL_MASK
-			| NFC_PROTO_MIFARE_MASK | NFC_PROTO_FELICA_MASK
-			| NFC_PROTO_ISO14443_MASK
-			| NFC_PROTO_NFC_DEP_MASK;
+		| NFC_PROTO_MIFARE_MASK | NFC_PROTO_FELICA_MASK
+		| NFC_PROTO_ISO14443_MASK
+		| NFC_PROTO_ISO14443_B_MASK
+		| NFC_PROTO_NFC_DEP_MASK;
 
 	drv->ndev = nci_allocate_device(&nfcwilink_ops,
 					protocols,
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index 19110f0eb15..d606f52fec8 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -38,13 +38,51 @@
 #define SCM_VENDOR_ID 0x4E6
 #define SCL3711_PRODUCT_ID 0x5591
 
+#define SONY_VENDOR_ID         0x054c
+#define PASORI_PRODUCT_ID      0x02e1
+
+#define PN533_QUIRKS_TYPE_A          BIT(0)
+#define PN533_QUIRKS_TYPE_F          BIT(1)
+#define PN533_QUIRKS_DEP             BIT(2)
+#define PN533_QUIRKS_RAW_EXCHANGE    BIT(3)
+
+#define PN533_DEVICE_STD    0x1
+#define PN533_DEVICE_PASORI 0x2
+
+#define PN533_ALL_PROTOCOLS (NFC_PROTO_JEWEL_MASK | NFC_PROTO_MIFARE_MASK |\
+			     NFC_PROTO_FELICA_MASK | NFC_PROTO_ISO14443_MASK |\
+			     NFC_PROTO_NFC_DEP_MASK |\
+			     NFC_PROTO_ISO14443_B_MASK)
+
+#define PN533_NO_TYPE_B_PROTOCOLS (NFC_PROTO_JEWEL_MASK | \
+				   NFC_PROTO_MIFARE_MASK | \
+				   NFC_PROTO_FELICA_MASK | \
+				   NFC_PROTO_ISO14443_MASK | \
+				   NFC_PROTO_NFC_DEP_MASK)
+
 static const struct usb_device_id pn533_table[] = {
-	{ USB_DEVICE(PN533_VENDOR_ID, PN533_PRODUCT_ID) },
-	{ USB_DEVICE(SCM_VENDOR_ID, SCL3711_PRODUCT_ID) },
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE,
+	  .idVendor		= PN533_VENDOR_ID,
+	  .idProduct		= PN533_PRODUCT_ID,
+	  .driver_info		= PN533_DEVICE_STD,
+	},
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE,
+	  .idVendor		= SCM_VENDOR_ID,
+	  .idProduct		= SCL3711_PRODUCT_ID,
+	  .driver_info		= PN533_DEVICE_STD,
+	},
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE,
+	  .idVendor		= SONY_VENDOR_ID,
+	  .idProduct		= PASORI_PRODUCT_ID,
+	  .driver_info		= PN533_DEVICE_PASORI,
+	},
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, pn533_table);
 
+/* How much time we spend listening for initiators */
+#define PN533_LISTEN_TIME 2
+
 /* frame definitions */
 #define PN533_FRAME_TAIL_SIZE 2
 #define PN533_FRAME_SIZE(f) (sizeof(struct pn533_frame) + f->datalen + \
@@ -69,11 +107,16 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 #define PN533_CMD_GET_FIRMWARE_VERSION 0x02
 #define PN533_CMD_RF_CONFIGURATION 0x32
 #define PN533_CMD_IN_DATA_EXCHANGE 0x40
+#define PN533_CMD_IN_COMM_THRU     0x42
 #define PN533_CMD_IN_LIST_PASSIVE_TARGET 0x4A
 #define PN533_CMD_IN_ATR 0x50
 #define PN533_CMD_IN_RELEASE 0x52
 #define PN533_CMD_IN_JUMP_FOR_DEP 0x56
 
+#define PN533_CMD_TG_INIT_AS_TARGET 0x8c
+#define PN533_CMD_TG_GET_DATA 0x86
+#define PN533_CMD_TG_SET_DATA 0x8e
+
 #define PN533_CMD_RESPONSE(cmd) (cmd + 1)
 
 /* PN533 Return codes */
@@ -81,6 +124,9 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 #define PN533_CMD_MI_MASK 0x40
 #define PN533_CMD_RET_SUCCESS 0x00
 
+/* PN533 status codes */
+#define PN533_STATUS_TARGET_RELEASED 0x29
+
 struct pn533;
 
 typedef int (*pn533_cmd_complete_t) (struct pn533 *dev, void *arg,
@@ -97,7 +143,14 @@ struct pn533_fw_version {
 };
 
 /* PN533_CMD_RF_CONFIGURATION */
+#define PN533_CFGITEM_TIMING 0x02
 #define PN533_CFGITEM_MAX_RETRIES 0x05
+#define PN533_CFGITEM_PASORI 0x82
+
+#define PN533_CONFIG_TIMING_102 0xb
+#define PN533_CONFIG_TIMING_204 0xc
+#define PN533_CONFIG_TIMING_409 0xd
+#define PN533_CONFIG_TIMING_819 0xe
 
 #define PN533_CONFIG_MAX_RETRIES_NO_RETRY 0x00
 #define PN533_CONFIG_MAX_RETRIES_ENDLESS 0xFF
@@ -108,6 +161,12 @@ struct pn533_config_max_retries {
 	u8 mx_rty_passive_act;
 } __packed;
 
+struct pn533_config_timing {
+	u8 rfu;
+	u8 atr_res_timeout;
+	u8 dep_timeout;
+} __packed;
+
 /* PN533_CMD_IN_LIST_PASSIVE_TARGET */
 
 /* felica commands opcode */
@@ -144,6 +203,7 @@ enum {
 	PN533_POLL_MOD_424KBPS_FELICA,
 	PN533_POLL_MOD_106KBPS_JEWEL,
 	PN533_POLL_MOD_847KBPS_B,
+	PN533_LISTEN_MOD,
 
 	__PN533_POLL_MOD_AFTER_LAST,
 };
@@ -211,6 +271,9 @@ const struct pn533_poll_modulations poll_mod[] = {
 		},
 		.len = 3,
 	},
+	[PN533_LISTEN_MOD] = {
+		.len = 0,
+	},
 };
 
 /* PN533_CMD_IN_ATR */
@@ -237,7 +300,7 @@ struct pn533_cmd_jump_dep {
 	u8 active;
 	u8 baud;
 	u8 next;
-	u8 gt[];
+	u8 data[];
 } __packed;
 
 struct pn533_cmd_jump_dep_response {
@@ -253,6 +316,29 @@ struct pn533_cmd_jump_dep_response {
 	u8 gt[];
 } __packed;
 
+
+/* PN533_TG_INIT_AS_TARGET */
+#define PN533_INIT_TARGET_PASSIVE 0x1
+#define PN533_INIT_TARGET_DEP 0x2
+
+#define PN533_INIT_TARGET_RESP_FRAME_MASK 0x3
+#define PN533_INIT_TARGET_RESP_ACTIVE     0x1
+#define PN533_INIT_TARGET_RESP_DEP        0x4
+
+struct pn533_cmd_init_target {
+	u8 mode;
+	u8 mifare[6];
+	u8 felica[18];
+	u8 nfcid3[10];
+	u8 gb_len;
+	u8 gb[];
+} __packed;
+
+struct pn533_cmd_init_target_response {
+	u8 mode;
+	u8 cmd[];
+} __packed;
+
 struct pn533 {
 	struct usb_device *udev;
 	struct usb_interface *interface;
@@ -270,22 +356,33 @@ struct pn533 {
 
 	struct workqueue_struct	*wq;
 	struct work_struct cmd_work;
+	struct work_struct poll_work;
 	struct work_struct mi_work;
+	struct work_struct tg_work;
+	struct timer_list listen_timer;
 	struct pn533_frame *wq_in_frame;
 	int wq_in_error;
+	int cancel_listen;
 
 	pn533_cmd_complete_t cmd_complete;
 	void *cmd_complete_arg;
-	struct semaphore cmd_lock;
+	struct mutex cmd_lock;
 	u8 cmd;
 
 	struct pn533_poll_modulations *poll_mod_active[PN533_POLL_MOD_MAX + 1];
 	u8 poll_mod_count;
 	u8 poll_mod_curr;
 	u32 poll_protocols;
+	u32 listen_protocols;
+
+	u8 *gb;
+	size_t gb_len;
 
 	u8 tgt_available_prots;
 	u8 tgt_active_prot;
+	u8 tgt_mode;
+
+	u32 device_type;
 };
 
 struct pn533_frame {
@@ -405,7 +502,7 @@ static void pn533_wq_cmd_complete(struct work_struct *work)
 					PN533_FRAME_CMD_PARAMS_LEN(in_frame));
 
 	if (rc != -EINPROGRESS)
-		up(&dev->cmd_lock);
+		mutex_unlock(&dev->cmd_lock);
 }
 
 static void pn533_recv_response(struct urb *urb)
@@ -583,7 +680,7 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
 
 	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-	if (down_trylock(&dev->cmd_lock))
+	if (!mutex_trylock(&dev->cmd_lock))
 		return -EBUSY;
 
 	rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
@@ -593,7 +690,7 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
 
 	return 0;
 error:
-	up(&dev->cmd_lock);
+	mutex_unlock(&dev->cmd_lock);
 	return rc;
 }
 
@@ -892,7 +989,7 @@ static int pn533_target_found_type_b(struct nfc_target *nfc_tgt, u8 *tgt_data,
 	if (!pn533_target_type_b_is_valid(tgt_type_b, tgt_data_len))
 		return -EPROTO;
 
-	nfc_tgt->supported_protocols = NFC_PROTO_ISO14443_MASK;
+	nfc_tgt->supported_protocols = NFC_PROTO_ISO14443_B_MASK;
 
 	return 0;
 }
@@ -963,6 +1060,11 @@ static int pn533_target_found(struct pn533 *dev,
 	return 0;
 }
 
+static inline void pn533_poll_next_mod(struct pn533 *dev)
+{
+	dev->poll_mod_curr = (dev->poll_mod_curr + 1) % dev->poll_mod_count;
+}
+
 static void pn533_poll_reset_mod_list(struct pn533 *dev)
 {
 	dev->poll_mod_count = 0;
@@ -975,102 +1077,283 @@ static void pn533_poll_add_mod(struct pn533 *dev, u8 mod_index)
 	dev->poll_mod_count++;
 }
 
-static void pn533_poll_create_mod_list(struct pn533 *dev, u32 protocols)
+static void pn533_poll_create_mod_list(struct pn533 *dev,
+				       u32 im_protocols, u32 tm_protocols)
 {
 	pn533_poll_reset_mod_list(dev);
 
-	if (protocols & NFC_PROTO_MIFARE_MASK
-					|| protocols & NFC_PROTO_ISO14443_MASK
-					|| protocols & NFC_PROTO_NFC_DEP_MASK)
+	if (im_protocols & NFC_PROTO_MIFARE_MASK
+	    || im_protocols & NFC_PROTO_ISO14443_MASK
+	    || im_protocols & NFC_PROTO_NFC_DEP_MASK)
 		pn533_poll_add_mod(dev, PN533_POLL_MOD_106KBPS_A);
 
-	if (protocols & NFC_PROTO_FELICA_MASK
-					|| protocols & NFC_PROTO_NFC_DEP_MASK) {
+	if (im_protocols & NFC_PROTO_FELICA_MASK
+	    || im_protocols & NFC_PROTO_NFC_DEP_MASK) {
 		pn533_poll_add_mod(dev, PN533_POLL_MOD_212KBPS_FELICA);
 		pn533_poll_add_mod(dev, PN533_POLL_MOD_424KBPS_FELICA);
 	}
 
-	if (protocols & NFC_PROTO_JEWEL_MASK)
+	if (im_protocols & NFC_PROTO_JEWEL_MASK)
 		pn533_poll_add_mod(dev, PN533_POLL_MOD_106KBPS_JEWEL);
 
-	if (protocols & NFC_PROTO_ISO14443_MASK)
+	if (im_protocols & NFC_PROTO_ISO14443_B_MASK)
 		pn533_poll_add_mod(dev, PN533_POLL_MOD_847KBPS_B);
+
+	if (tm_protocols)
+		pn533_poll_add_mod(dev, PN533_LISTEN_MOD);
 }
 
-static void pn533_start_poll_frame(struct pn533_frame *frame,
-					struct pn533_poll_modulations *mod)
+static int pn533_start_poll_complete(struct pn533 *dev, void *arg,
+				     u8 *params, int params_len)
 {
+	struct pn533_poll_response *resp;
+	int rc;
 
-	pn533_tx_frame_init(frame, PN533_CMD_IN_LIST_PASSIVE_TARGET);
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-	memcpy(PN533_FRAME_CMD_PARAMS_PTR(frame), &mod->data, mod->len);
-	frame->datalen += mod->len;
+	resp = (struct pn533_poll_response *) params;
+	if (resp->nbtg) {
+		rc = pn533_target_found(dev, resp, params_len);
+
+		/* We must stop the poll after a valid target found */
+		if (rc == 0) {
+			pn533_poll_reset_mod_list(dev);
+			return 0;
+		}
+	}
+
+	return -EAGAIN;
+}
+
+static int pn533_init_target_frame(struct pn533_frame *frame,
+				   u8 *gb, size_t gb_len)
+{
+	struct pn533_cmd_init_target *cmd;
+	size_t cmd_len;
+	u8 felica_params[18] = {0x1, 0xfe, /* DEP */
+				0x0, 0x0, 0x0, 0x0, 0x0, 0x0, /* random */
+				0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+				0xff, 0xff}; /* System code */
+	u8 mifare_params[6] = {0x1, 0x1, /* SENS_RES */
+			       0x0, 0x0, 0x0,
+			       0x40}; /* SEL_RES for DEP */
+
+	cmd_len = sizeof(struct pn533_cmd_init_target) + gb_len + 1;
+	cmd = kzalloc(cmd_len, GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	pn533_tx_frame_init(frame, PN533_CMD_TG_INIT_AS_TARGET);
+
+	/* DEP support only */
+	cmd->mode |= PN533_INIT_TARGET_DEP;
+
+	/* Felica params */
+	memcpy(cmd->felica, felica_params, 18);
+	get_random_bytes(cmd->felica + 2, 6);
+
+	/* NFCID3 */
+	memset(cmd->nfcid3, 0, 10);
+	memcpy(cmd->nfcid3, cmd->felica, 8);
+
+	/* MIFARE params */
+	memcpy(cmd->mifare, mifare_params, 6);
+
+	/* General bytes */
+	cmd->gb_len = gb_len;
+	memcpy(cmd->gb, gb, gb_len);
+
+	/* Len Tk */
+	cmd->gb[gb_len] = 0;
+
+	memcpy(PN533_FRAME_CMD_PARAMS_PTR(frame), cmd, cmd_len);
+
+	frame->datalen += cmd_len;
 
 	pn533_tx_frame_finish(frame);
+
+	kfree(cmd);
+
+	return 0;
 }
 
-static int pn533_start_poll_complete(struct pn533 *dev, void *arg,
-						u8 *params, int params_len)
+#define PN533_CMD_DATAEXCH_HEAD_LEN (sizeof(struct pn533_frame) + 3)
+#define PN533_CMD_DATAEXCH_DATA_MAXLEN 262
+static int pn533_tm_get_data_complete(struct pn533 *dev, void *arg,
+				      u8 *params, int params_len)
 {
-	struct pn533_poll_response *resp;
-	struct pn533_poll_modulations *next_mod;
-	int rc;
+	struct sk_buff *skb_resp = arg;
+	struct pn533_frame *in_frame = (struct pn533_frame *) skb_resp->data;
 
 	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-	if (params_len == -ENOENT) {
-		nfc_dev_dbg(&dev->interface->dev, "Polling operation has been"
-								" stopped");
-		goto stop_poll;
+	if (params_len < 0) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error %d when starting as a target",
+			    params_len);
+
+		return params_len;
+	}
+
+	if (params_len > 0 && params[0] != 0) {
+		nfc_tm_deactivated(dev->nfc_dev);
+
+		dev->tgt_mode = 0;
+
+		kfree_skb(skb_resp);
+		return 0;
 	}
 
+	skb_put(skb_resp, PN533_FRAME_SIZE(in_frame));
+	skb_pull(skb_resp, PN533_CMD_DATAEXCH_HEAD_LEN);
+	skb_trim(skb_resp, skb_resp->len - PN533_FRAME_TAIL_SIZE);
+
+	return nfc_tm_data_received(dev->nfc_dev, skb_resp);
+}
+
+static void pn533_wq_tg_get_data(struct work_struct *work)
+{
+	struct pn533 *dev = container_of(work, struct pn533, tg_work);
+	struct pn533_frame *in_frame;
+	struct sk_buff *skb_resp;
+	size_t skb_resp_len;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	skb_resp_len = PN533_CMD_DATAEXCH_HEAD_LEN +
+		PN533_CMD_DATAEXCH_DATA_MAXLEN +
+		PN533_FRAME_TAIL_SIZE;
+
+	skb_resp = nfc_alloc_recv_skb(skb_resp_len, GFP_KERNEL);
+	if (!skb_resp)
+		return;
+
+	in_frame = (struct pn533_frame *)skb_resp->data;
+
+	pn533_tx_frame_init(dev->out_frame, PN533_CMD_TG_GET_DATA);
+	pn533_tx_frame_finish(dev->out_frame);
+
+	pn533_send_cmd_frame_async(dev, dev->out_frame, in_frame,
+				   skb_resp_len,
+				   pn533_tm_get_data_complete,
+				   skb_resp, GFP_KERNEL);
+
+	return;
+}
+
+#define ATR_REQ_GB_OFFSET 17
+static int pn533_init_target_complete(struct pn533 *dev, void *arg,
+				      u8 *params, int params_len)
+{
+	struct pn533_cmd_init_target_response *resp;
+	u8 frame, comm_mode = NFC_COMM_PASSIVE, *gb;
+	size_t gb_len;
+	int rc;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
 	if (params_len < 0) {
-		nfc_dev_err(&dev->interface->dev, "Error %d when running poll",
-								params_len);
-		goto stop_poll;
+		nfc_dev_err(&dev->interface->dev,
+			    "Error %d when starting as a target",
+			    params_len);
+
+		return params_len;
 	}
 
-	resp = (struct pn533_poll_response *) params;
-	if (resp->nbtg) {
-		rc = pn533_target_found(dev, resp, params_len);
+	if (params_len < ATR_REQ_GB_OFFSET + 1)
+		return -EINVAL;
 
-		/* We must stop the poll after a valid target found */
-		if (rc == 0)
-			goto stop_poll;
+	resp = (struct pn533_cmd_init_target_response *) params;
+
+	nfc_dev_dbg(&dev->interface->dev, "Target mode 0x%x param len %d\n",
+		    resp->mode, params_len);
+
+	frame = resp->mode & PN533_INIT_TARGET_RESP_FRAME_MASK;
+	if (frame == PN533_INIT_TARGET_RESP_ACTIVE)
+		comm_mode = NFC_COMM_ACTIVE;
 
-		if (rc != -EAGAIN)
-			nfc_dev_err(&dev->interface->dev, "The target found is"
-					" not valid - continuing to poll");
+	/* Again, only DEP */
+	if ((resp->mode & PN533_INIT_TARGET_RESP_DEP) == 0)
+		return -EOPNOTSUPP;
+
+	gb = resp->cmd + ATR_REQ_GB_OFFSET;
+	gb_len = params_len - (ATR_REQ_GB_OFFSET + 1);
+
+	rc = nfc_tm_activated(dev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
+			      comm_mode, gb, gb_len);
+	if (rc < 0) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error when signaling target activation");
+		return rc;
 	}
 
-	dev->poll_mod_curr = (dev->poll_mod_curr + 1) % dev->poll_mod_count;
+	dev->tgt_mode = 1;
 
-	next_mod = dev->poll_mod_active[dev->poll_mod_curr];
+	queue_work(dev->wq, &dev->tg_work);
 
-	nfc_dev_dbg(&dev->interface->dev, "Polling next modulation (0x%x)",
-							dev->poll_mod_curr);
+	return 0;
+}
 
-	pn533_start_poll_frame(dev->out_frame, next_mod);
+static void pn533_listen_mode_timer(unsigned long data)
+{
+	struct pn533 *dev = (struct pn533 *) data;
 
-	/* Don't need to down the semaphore again */
-	rc = __pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame,
-				dev->in_maxlen, pn533_start_poll_complete,
-				NULL, GFP_ATOMIC);
+	nfc_dev_dbg(&dev->interface->dev, "Listen mode timeout");
+
+	/* An ack will cancel the last issued command (poll) */
+	pn533_send_ack(dev, GFP_ATOMIC);
+
+	dev->cancel_listen = 1;
+
+	mutex_unlock(&dev->cmd_lock);
+
+	pn533_poll_next_mod(dev);
+
+	queue_work(dev->wq, &dev->poll_work);
+}
+
+static int pn533_poll_complete(struct pn533 *dev, void *arg,
+			       u8 *params, int params_len)
+{
+	struct pn533_poll_modulations *cur_mod;
+	int rc;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	if (params_len == -ENOENT) {
+		if (dev->poll_mod_count != 0)
+			return 0;
+
+		nfc_dev_err(&dev->interface->dev,
+			    "Polling operation has been stopped");
 
-	if (rc == -EPERM) {
-		nfc_dev_dbg(&dev->interface->dev, "Cannot poll next modulation"
-					" because poll has been stopped");
 		goto stop_poll;
 	}
 
-	if (rc) {
-		nfc_dev_err(&dev->interface->dev, "Error %d when trying to poll"
-							" next modulation", rc);
+	if (params_len < 0) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error %d when running poll", params_len);
+
 		goto stop_poll;
 	}
 
-	/* Inform caller function to do not up the semaphore */
-	return -EINPROGRESS;
+	cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
+
+	if (cur_mod->len == 0) {
+		del_timer(&dev->listen_timer);
+
+		return pn533_init_target_complete(dev, arg, params, params_len);
+	} else {
+		rc = pn533_start_poll_complete(dev, arg, params, params_len);
+		if (!rc)
+			return rc;
+	}
+
+	pn533_poll_next_mod(dev);
+
+	queue_work(dev->wq, &dev->poll_work);
+
+	return 0;
 
 stop_poll:
 	pn533_poll_reset_mod_list(dev);
@@ -1078,61 +1361,104 @@ stop_poll:
 	return 0;
 }
 
-static int pn533_start_poll(struct nfc_dev *nfc_dev, u32 protocols)
+static void pn533_build_poll_frame(struct pn533 *dev,
+				   struct pn533_frame *frame,
+				   struct pn533_poll_modulations *mod)
 {
-	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
-	struct pn533_poll_modulations *start_mod;
-	int rc;
+	nfc_dev_dbg(&dev->interface->dev, "mod len %d\n", mod->len);
 
-	nfc_dev_dbg(&dev->interface->dev, "%s - protocols=0x%x", __func__,
-								protocols);
+	if (mod->len == 0) {
+		/* Listen mode */
+		pn533_init_target_frame(frame, dev->gb, dev->gb_len);
+	} else {
+		/* Polling mode */
+		pn533_tx_frame_init(frame, PN533_CMD_IN_LIST_PASSIVE_TARGET);
 
-	if (dev->poll_mod_count) {
-		nfc_dev_err(&dev->interface->dev, "Polling operation already"
-								" active");
-		return -EBUSY;
-	}
+		memcpy(PN533_FRAME_CMD_PARAMS_PTR(frame), &mod->data, mod->len);
+		frame->datalen += mod->len;
 
-	if (dev->tgt_active_prot) {
-		nfc_dev_err(&dev->interface->dev, "Cannot poll with a target"
-							" already activated");
-		return -EBUSY;
+		pn533_tx_frame_finish(frame);
 	}
+}
+
+static int pn533_send_poll_frame(struct pn533 *dev)
+{
+	struct pn533_poll_modulations *cur_mod;
+	int rc;
 
-	pn533_poll_create_mod_list(dev, protocols);
+	cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
 
-	if (!dev->poll_mod_count) {
-		nfc_dev_err(&dev->interface->dev, "No valid protocols"
-								" specified");
-		rc = -EINVAL;
-		goto error;
+	pn533_build_poll_frame(dev, dev->out_frame, cur_mod);
+
+	rc = pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame,
+				dev->in_maxlen,	pn533_poll_complete,
+				NULL, GFP_KERNEL);
+	if (rc)
+		nfc_dev_err(&dev->interface->dev, "Polling loop error %d", rc);
+
+	return rc;
+}
+
+static void pn533_wq_poll(struct work_struct *work)
+{
+	struct pn533 *dev = container_of(work, struct pn533, poll_work);
+	struct pn533_poll_modulations *cur_mod;
+	int rc;
+
+	cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
+
+	nfc_dev_dbg(&dev->interface->dev,
+		    "%s cancel_listen %d modulation len %d",
+		    __func__, dev->cancel_listen, cur_mod->len);
+
+	if (dev->cancel_listen == 1) {
+		dev->cancel_listen = 0;
+		usb_kill_urb(dev->in_urb);
 	}
 
-	nfc_dev_dbg(&dev->interface->dev, "It will poll %d modulations types",
-							dev->poll_mod_count);
+	rc = pn533_send_poll_frame(dev);
+	if (rc)
+		return;
 
-	dev->poll_mod_curr = 0;
-	start_mod = dev->poll_mod_active[dev->poll_mod_curr];
+	if (cur_mod->len == 0 && dev->poll_mod_count > 1)
+		mod_timer(&dev->listen_timer, jiffies + PN533_LISTEN_TIME * HZ);
 
-	pn533_start_poll_frame(dev->out_frame, start_mod);
+	return;
+}
 
-	rc = pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame,
-				dev->in_maxlen,	pn533_start_poll_complete,
-				NULL, GFP_KERNEL);
+static int pn533_start_poll(struct nfc_dev *nfc_dev,
+			    u32 im_protocols, u32 tm_protocols)
+{
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 
-	if (rc) {
-		nfc_dev_err(&dev->interface->dev, "Error %d when trying to"
-							" start poll", rc);
-		goto error;
+	nfc_dev_dbg(&dev->interface->dev,
+		    "%s: im protocols 0x%x tm protocols 0x%x",
+		    __func__, im_protocols, tm_protocols);
+
+	if (dev->tgt_active_prot) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Cannot poll with a target already activated");
+		return -EBUSY;
 	}
 
-	dev->poll_protocols = protocols;
+	if (dev->tgt_mode) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Cannot poll while already being activated");
+		return -EBUSY;
+	}
 
-	return 0;
+	if (tm_protocols) {
+		dev->gb = nfc_get_local_general_bytes(nfc_dev, &dev->gb_len);
+		if (dev->gb == NULL)
+			tm_protocols = 0;
+	}
 
-error:
-	pn533_poll_reset_mod_list(dev);
-	return rc;
+	dev->poll_mod_curr = 0;
+	pn533_poll_create_mod_list(dev, im_protocols, tm_protocols);
+	dev->poll_protocols = im_protocols;
+	dev->listen_protocols = tm_protocols;
+
+	return pn533_send_poll_frame(dev);
 }
 
 static void pn533_stop_poll(struct nfc_dev *nfc_dev)
@@ -1141,6 +1467,8 @@ static void pn533_stop_poll(struct nfc_dev *nfc_dev)
 
 	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
+	del_timer(&dev->listen_timer);
+
 	if (!dev->poll_mod_count) {
 		nfc_dev_dbg(&dev->interface->dev, "Polling operation was not"
 								" running");
@@ -1152,6 +1480,8 @@ static void pn533_stop_poll(struct nfc_dev *nfc_dev)
 
 	/* prevent pn533_start_poll_complete to issue a new poll meanwhile */
 	usb_kill_urb(dev->in_urb);
+
+	pn533_poll_reset_mod_list(dev);
 }
 
 static int pn533_activate_target_nfcdep(struct pn533 *dev)
@@ -1349,13 +1679,29 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
 	return 0;
 }
 
+static int pn533_mod_to_baud(struct pn533 *dev)
+{
+	switch (dev->poll_mod_curr) {
+	case PN533_POLL_MOD_106KBPS_A:
+		return 0;
+	case PN533_POLL_MOD_212KBPS_FELICA:
+		return 1;
+	case PN533_POLL_MOD_424KBPS_FELICA:
+		return 2;
+	default:
+		return -EINVAL;
+	}
+}
+
+#define PASSIVE_DATA_LEN 5
 static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 			     u8 comm_mode, u8* gb, size_t gb_len)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	struct pn533_cmd_jump_dep *cmd;
-	u8 cmd_len;
-	int rc;
+	u8 cmd_len, *data_ptr;
+	u8 passive_data[PASSIVE_DATA_LEN] = {0x00, 0xff, 0xff, 0x00, 0x3};
+	int rc, baud;
 
 	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
@@ -1371,7 +1717,17 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 		return -EBUSY;
 	}
 
+	baud = pn533_mod_to_baud(dev);
+	if (baud < 0) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Invalid curr modulation %d", dev->poll_mod_curr);
+		return baud;
+	}
+
 	cmd_len = sizeof(struct pn533_cmd_jump_dep) + gb_len;
+	if (comm_mode == NFC_COMM_PASSIVE)
+		cmd_len += PASSIVE_DATA_LEN;
+
 	cmd = kzalloc(cmd_len, GFP_KERNEL);
 	if (cmd == NULL)
 		return -ENOMEM;
@@ -1379,10 +1735,18 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
 	pn533_tx_frame_init(dev->out_frame, PN533_CMD_IN_JUMP_FOR_DEP);
 
 	cmd->active = !comm_mode;
-	cmd->baud = 0;
+	cmd->next = 0;
+	cmd->baud = baud;
+	data_ptr = cmd->data;
+	if (comm_mode == NFC_COMM_PASSIVE && cmd->baud > 0) {
+		memcpy(data_ptr, passive_data, PASSIVE_DATA_LEN);
+		cmd->next |= 1;
+		data_ptr += PASSIVE_DATA_LEN;
+	}
+
 	if (gb != NULL && gb_len > 0) {
-		cmd->next = 4; /* We have some Gi */
-		memcpy(cmd->gt, gb, gb_len);
+		cmd->next |= 4; /* We have some Gi */
+		memcpy(data_ptr, gb, gb_len);
 	} else {
 		cmd->next = 0;
 	}
@@ -1407,15 +1771,25 @@ out:
 
 static int pn533_dep_link_down(struct nfc_dev *nfc_dev)
 {
-	pn533_deactivate_target(nfc_dev, 0);
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+
+	pn533_poll_reset_mod_list(dev);
+
+	if (dev->tgt_mode || dev->tgt_active_prot) {
+		pn533_send_ack(dev, GFP_KERNEL);
+		usb_kill_urb(dev->in_urb);
+	}
+
+	dev->tgt_active_prot = 0;
+	dev->tgt_mode = 0;
+
+	skb_queue_purge(&dev->resp_q);
 
 	return 0;
 }
 
-#define PN533_CMD_DATAEXCH_HEAD_LEN (sizeof(struct pn533_frame) + 3)
-#define PN533_CMD_DATAEXCH_DATA_MAXLEN 262
-
-static int pn533_data_exchange_tx_frame(struct pn533 *dev, struct sk_buff *skb)
+static int pn533_build_tx_frame(struct pn533 *dev, struct sk_buff *skb,
+				bool target)
 {
 	int payload_len = skb->len;
 	struct pn533_frame *out_frame;
@@ -1432,14 +1806,37 @@ static int pn533_data_exchange_tx_frame(struct pn533 *dev, struct sk_buff *skb)
 		return -ENOSYS;
 	}
 
-	skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN);
-	out_frame = (struct pn533_frame *) skb->data;
+	if (target == true) {
+		switch (dev->device_type) {
+		case PN533_DEVICE_PASORI:
+			if (dev->tgt_active_prot == NFC_PROTO_FELICA) {
+				skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN - 1);
+				out_frame = (struct pn533_frame *) skb->data;
+				pn533_tx_frame_init(out_frame,
+						    PN533_CMD_IN_COMM_THRU);
+
+				break;
+			}
+
+		default:
+			skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN);
+			out_frame = (struct pn533_frame *) skb->data;
+			pn533_tx_frame_init(out_frame,
+					    PN533_CMD_IN_DATA_EXCHANGE);
+			tg = 1;
+			memcpy(PN533_FRAME_CMD_PARAMS_PTR(out_frame),
+			       &tg, sizeof(u8));
+			out_frame->datalen += sizeof(u8);
+
+			break;
+		}
 
-	pn533_tx_frame_init(out_frame, PN533_CMD_IN_DATA_EXCHANGE);
+	} else {
+		skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN - 1);
+		out_frame = (struct pn533_frame *) skb->data;
+		pn533_tx_frame_init(out_frame, PN533_CMD_TG_SET_DATA);
+	}
 
-	tg = 1;
-	memcpy(PN533_FRAME_CMD_PARAMS_PTR(out_frame), &tg, sizeof(u8));
-	out_frame->datalen += sizeof(u8);
 
 	/* The data is already in the out_frame, just update the datalen */
 	out_frame->datalen += payload_len;
@@ -1550,9 +1947,9 @@ error:
 	return 0;
 }
 
-static int pn533_data_exchange(struct nfc_dev *nfc_dev,
-			       struct nfc_target *target, struct sk_buff *skb,
-			       data_exchange_cb_t cb, void *cb_context)
+static int pn533_transceive(struct nfc_dev *nfc_dev,
+			    struct nfc_target *target, struct sk_buff *skb,
+			    data_exchange_cb_t cb, void *cb_context)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	struct pn533_frame *out_frame, *in_frame;
@@ -1570,7 +1967,7 @@ static int pn533_data_exchange(struct nfc_dev *nfc_dev,
 		goto error;
 	}
 
-	rc = pn533_data_exchange_tx_frame(dev, skb);
+	rc = pn533_build_tx_frame(dev, skb, true);
 	if (rc)
 		goto error;
 
@@ -1618,6 +2015,63 @@ error:
 	return rc;
 }
 
+static int pn533_tm_send_complete(struct pn533 *dev, void *arg,
+				  u8 *params, int params_len)
+{
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	if (params_len < 0) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error %d when sending data",
+			    params_len);
+
+		return params_len;
+	}
+
+	if (params_len > 0 && params[0] != 0) {
+		nfc_tm_deactivated(dev->nfc_dev);
+
+		dev->tgt_mode = 0;
+
+		return 0;
+	}
+
+	queue_work(dev->wq, &dev->tg_work);
+
+	return 0;
+}
+
+static int pn533_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+	struct pn533_frame *out_frame;
+	int rc;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	rc = pn533_build_tx_frame(dev, skb, false);
+	if (rc)
+		goto error;
+
+	out_frame = (struct pn533_frame *) skb->data;
+
+	rc = pn533_send_cmd_frame_async(dev, out_frame, dev->in_frame,
+					dev->in_maxlen, pn533_tm_send_complete,
+					NULL, GFP_KERNEL);
+	if (rc) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error %d when trying to send data", rc);
+		goto error;
+	}
+
+	return 0;
+
+error:
+	kfree_skb(skb);
+
+	return rc;
+}
+
 static void pn533_wq_mi_recv(struct work_struct *work)
 {
 	struct pn533 *dev = container_of(work, struct pn533, mi_work);
@@ -1638,7 +2092,7 @@ static void pn533_wq_mi_recv(struct work_struct *work)
 
 	skb_reserve(skb_cmd, PN533_CMD_DATAEXCH_HEAD_LEN);
 
-	rc = pn533_data_exchange_tx_frame(dev, skb_cmd);
+	rc = pn533_build_tx_frame(dev, skb_cmd, true);
 	if (rc)
 		goto error_frame;
 
@@ -1677,7 +2131,7 @@ error_cmd:
 
 	kfree(arg);
 
-	up(&dev->cmd_lock);
+	mutex_unlock(&dev->cmd_lock);
 }
 
 static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
@@ -1703,7 +2157,28 @@ static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
 	return rc;
 }
 
-struct nfc_ops pn533_nfc_ops = {
+static int pn533_fw_reset(struct pn533 *dev)
+{
+	int rc;
+	u8 *params;
+
+	nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+	pn533_tx_frame_init(dev->out_frame, 0x18);
+
+	params = PN533_FRAME_CMD_PARAMS_PTR(dev->out_frame);
+	params[0] = 0x1;
+	dev->out_frame->datalen += 1;
+
+	pn533_tx_frame_finish(dev->out_frame);
+
+	rc = pn533_send_cmd_frame_sync(dev, dev->out_frame, dev->in_frame,
+				       dev->in_maxlen);
+
+	return rc;
+}
+
+static struct nfc_ops pn533_nfc_ops = {
 	.dev_up = NULL,
 	.dev_down = NULL,
 	.dep_link_up = pn533_dep_link_up,
@@ -1712,9 +2187,88 @@ struct nfc_ops pn533_nfc_ops = {
 	.stop_poll = pn533_stop_poll,
 	.activate_target = pn533_activate_target,
 	.deactivate_target = pn533_deactivate_target,
-	.data_exchange = pn533_data_exchange,
+	.im_transceive = pn533_transceive,
+	.tm_send = pn533_tm_send,
 };
 
+static int pn533_setup(struct pn533 *dev)
+{
+	struct pn533_config_max_retries max_retries;
+	struct pn533_config_timing timing;
+	u8 pasori_cfg[3] = {0x08, 0x01, 0x08};
+	int rc;
+
+	switch (dev->device_type) {
+	case PN533_DEVICE_STD:
+		max_retries.mx_rty_atr = PN533_CONFIG_MAX_RETRIES_ENDLESS;
+		max_retries.mx_rty_psl = 2;
+		max_retries.mx_rty_passive_act =
+			PN533_CONFIG_MAX_RETRIES_NO_RETRY;
+
+		timing.rfu = PN533_CONFIG_TIMING_102;
+		timing.atr_res_timeout = PN533_CONFIG_TIMING_204;
+		timing.dep_timeout = PN533_CONFIG_TIMING_409;
+
+		break;
+
+	case PN533_DEVICE_PASORI:
+		max_retries.mx_rty_atr = 0x2;
+		max_retries.mx_rty_psl = 0x1;
+		max_retries.mx_rty_passive_act =
+			PN533_CONFIG_MAX_RETRIES_NO_RETRY;
+
+		timing.rfu = PN533_CONFIG_TIMING_102;
+		timing.atr_res_timeout = PN533_CONFIG_TIMING_102;
+		timing.dep_timeout = PN533_CONFIG_TIMING_204;
+
+		break;
+
+	default:
+		nfc_dev_err(&dev->interface->dev, "Unknown device type %d\n",
+			    dev->device_type);
+		return -EINVAL;
+	}
+
+	rc = pn533_set_configuration(dev, PN533_CFGITEM_MAX_RETRIES,
+				     (u8 *)&max_retries, sizeof(max_retries));
+	if (rc) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error on setting MAX_RETRIES config");
+		return rc;
+	}
+
+
+	rc = pn533_set_configuration(dev, PN533_CFGITEM_TIMING,
+				     (u8 *)&timing, sizeof(timing));
+	if (rc) {
+		nfc_dev_err(&dev->interface->dev,
+			    "Error on setting RF timings");
+		return rc;
+	}
+
+	switch (dev->device_type) {
+	case PN533_DEVICE_STD:
+		break;
+
+	case PN533_DEVICE_PASORI:
+		pn533_fw_reset(dev);
+
+		rc = pn533_set_configuration(dev, PN533_CFGITEM_PASORI,
+					     pasori_cfg, 3);
+		if (rc) {
+			nfc_dev_err(&dev->interface->dev,
+				    "Error while settings PASORI config");
+			return rc;
+		}
+
+		pn533_fw_reset(dev);
+
+		break;
+	}
+
+	return 0;
+}
+
 static int pn533_probe(struct usb_interface *interface,
 			const struct usb_device_id *id)
 {
@@ -1722,7 +2276,6 @@ static int pn533_probe(struct usb_interface *interface,
 	struct pn533 *dev;
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
-	struct pn533_config_max_retries max_retries;
 	int in_endpoint = 0;
 	int out_endpoint = 0;
 	int rc = -ENOMEM;
@@ -1735,7 +2288,7 @@ static int pn533_probe(struct usb_interface *interface,
 
 	dev->udev = usb_get_dev(interface_to_usbdev(interface));
 	dev->interface = interface;
-	sema_init(&dev->cmd_lock, 1);
+	mutex_init(&dev->cmd_lock);
 
 	iface_desc = interface->cur_altsetting;
 	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
@@ -1779,12 +2332,18 @@ static int pn533_probe(struct usb_interface *interface,
 
 	INIT_WORK(&dev->cmd_work, pn533_wq_cmd_complete);
 	INIT_WORK(&dev->mi_work, pn533_wq_mi_recv);
+	INIT_WORK(&dev->tg_work, pn533_wq_tg_get_data);
+	INIT_WORK(&dev->poll_work, pn533_wq_poll);
 	dev->wq = alloc_workqueue("pn533",
 				  WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
 				  1);
 	if (dev->wq == NULL)
 		goto error;
 
+	init_timer(&dev->listen_timer);
+	dev->listen_timer.data = (unsigned long) dev;
+	dev->listen_timer.function = pn533_listen_mode_timer;
+
 	skb_queue_head_init(&dev->resp_q);
 
 	usb_set_intfdata(interface, dev);
@@ -1802,10 +2361,22 @@ static int pn533_probe(struct usb_interface *interface,
 	nfc_dev_info(&dev->interface->dev, "NXP PN533 firmware ver %d.%d now"
 					" attached", fw_ver->ver, fw_ver->rev);
 
-	protocols = NFC_PROTO_JEWEL_MASK
-			| NFC_PROTO_MIFARE_MASK | NFC_PROTO_FELICA_MASK
-			| NFC_PROTO_ISO14443_MASK
-			| NFC_PROTO_NFC_DEP_MASK;
+	dev->device_type = id->driver_info;
+	switch (dev->device_type) {
+	case PN533_DEVICE_STD:
+		protocols = PN533_ALL_PROTOCOLS;
+		break;
+
+	case PN533_DEVICE_PASORI:
+		protocols = PN533_NO_TYPE_B_PROTOCOLS;
+		break;
+
+	default:
+		nfc_dev_err(&dev->interface->dev, "Unknown device type %d\n",
+			    dev->device_type);
+		rc = -EINVAL;
+		goto destroy_wq;
+	}
 
 	dev->nfc_dev = nfc_allocate_device(&pn533_nfc_ops, protocols,
 					   PN533_CMD_DATAEXCH_HEAD_LEN,
@@ -1820,23 +2391,18 @@ static int pn533_probe(struct usb_interface *interface,
 	if (rc)
 		goto free_nfc_dev;
 
-	max_retries.mx_rty_atr = PN533_CONFIG_MAX_RETRIES_ENDLESS;
-	max_retries.mx_rty_psl = 2;
-	max_retries.mx_rty_passive_act = PN533_CONFIG_MAX_RETRIES_NO_RETRY;
-
-	rc = pn533_set_configuration(dev, PN533_CFGITEM_MAX_RETRIES,
-				(u8 *) &max_retries, sizeof(max_retries));
-
-	if (rc) {
-		nfc_dev_err(&dev->interface->dev, "Error on setting MAX_RETRIES"
-								" config");
-		goto free_nfc_dev;
-	}
+	rc = pn533_setup(dev);
+	if (rc)
+		goto unregister_nfc_dev;
 
 	return 0;
 
+unregister_nfc_dev:
+	nfc_unregister_device(dev->nfc_dev);
+
 free_nfc_dev:
 	nfc_free_device(dev->nfc_dev);
+
 destroy_wq:
 	destroy_workqueue(dev->wq);
 error:
@@ -1865,6 +2431,8 @@ static void pn533_disconnect(struct usb_interface *interface)
 
 	skb_queue_purge(&dev->resp_q);
 
+	del_timer(&dev->listen_timer);
+
 	kfree(dev->in_frame);
 	usb_free_urb(dev->in_urb);
 	kfree(dev->out_frame);
diff --git a/drivers/nfc/pn544_hci.c b/drivers/nfc/pn544_hci.c
index 281f18c2fb8..aa71807189b 100644
--- a/drivers/nfc/pn544_hci.c
+++ b/drivers/nfc/pn544_hci.c
@@ -108,16 +108,22 @@ enum pn544_state {
 
 #define PN544_NFC_WI_MGMT_GATE			0xA1
 
-static u8 pn544_custom_gates[] = {
-	PN544_SYS_MGMT_GATE,
-	PN544_SWP_MGMT_GATE,
-	PN544_POLLING_LOOP_MGMT_GATE,
-	PN544_NFC_WI_MGMT_GATE,
-	PN544_RF_READER_F_GATE,
-	PN544_RF_READER_JEWEL_GATE,
-	PN544_RF_READER_ISO15693_GATE,
-	PN544_RF_READER_NFCIP1_INITIATOR_GATE,
-	PN544_RF_READER_NFCIP1_TARGET_GATE
+static struct nfc_hci_gate pn544_gates[] = {
+	{NFC_HCI_ADMIN_GATE, NFC_HCI_INVALID_PIPE},
+	{NFC_HCI_LOOPBACK_GATE, NFC_HCI_INVALID_PIPE},
+	{NFC_HCI_ID_MGMT_GATE, NFC_HCI_INVALID_PIPE},
+	{NFC_HCI_LINK_MGMT_GATE, NFC_HCI_INVALID_PIPE},
+	{NFC_HCI_RF_READER_B_GATE, NFC_HCI_INVALID_PIPE},
+	{NFC_HCI_RF_READER_A_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_SYS_MGMT_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_SWP_MGMT_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_POLLING_LOOP_MGMT_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_NFC_WI_MGMT_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_RF_READER_F_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_RF_READER_JEWEL_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_RF_READER_ISO15693_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_RF_READER_NFCIP1_INITIATOR_GATE, NFC_HCI_INVALID_PIPE},
+	{PN544_RF_READER_NFCIP1_TARGET_GATE, NFC_HCI_INVALID_PIPE}
 };
 
 /* Largest headroom needed for outgoing custom commands */
@@ -377,6 +383,9 @@ static int pn544_hci_open(struct nfc_shdlc *shdlc)
 
 	r = pn544_hci_enable(info, HCI_MODE);
 
+	if (r == 0)
+		info->state = PN544_ST_READY;
+
 out:
 	mutex_unlock(&info->info_lock);
 	return r;
@@ -393,6 +402,8 @@ static void pn544_hci_close(struct nfc_shdlc *shdlc)
 
 	pn544_hci_disable(info);
 
+	info->state = PN544_ST_COLD;
+
 out:
 	mutex_unlock(&info->info_lock);
 }
@@ -576,7 +587,8 @@ static int pn544_hci_xmit(struct nfc_shdlc *shdlc, struct sk_buff *skb)
 	return pn544_hci_i2c_write(client, skb->data, skb->len);
 }
 
-static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
+static int pn544_hci_start_poll(struct nfc_shdlc *shdlc,
+				u32 im_protocols, u32 tm_protocols)
 {
 	struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
 	u8 phases = 0;
@@ -584,7 +596,8 @@ static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
 	u8 duration[2];
 	u8 activated;
 
-	pr_info(DRIVER_DESC ": %s protocols = %d\n", __func__, protocols);
+	pr_info(DRIVER_DESC ": %s protocols 0x%x 0x%x\n",
+		__func__, im_protocols, tm_protocols);
 
 	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
 			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
@@ -604,10 +617,10 @@ static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
 	if (r < 0)
 		return r;
 
-	if (protocols & (NFC_PROTO_ISO14443_MASK | NFC_PROTO_MIFARE_MASK |
+	if (im_protocols & (NFC_PROTO_ISO14443_MASK | NFC_PROTO_MIFARE_MASK |
 			 NFC_PROTO_JEWEL_MASK))
 		phases |= 1;		/* Type A */
-	if (protocols & NFC_PROTO_FELICA_MASK) {
+	if (im_protocols & NFC_PROTO_FELICA_MASK) {
 		phases |= (1 << 2);	/* Type F 212 */
 		phases |= (1 << 3);	/* Type F 424 */
 	}
@@ -842,10 +855,9 @@ static int __devinit pn544_hci_probe(struct i2c_client *client,
 		goto err_rti;
 	}
 
-	init_data.gate_count = ARRAY_SIZE(pn544_custom_gates);
+	init_data.gate_count = ARRAY_SIZE(pn544_gates);
 
-	memcpy(init_data.gates, pn544_custom_gates,
-	       ARRAY_SIZE(pn544_custom_gates));
+	memcpy(init_data.gates, pn544_gates, sizeof(pn544_gates));
 
 	/*
 	 * TODO: Session id must include the driver name + some bus addr
@@ -857,6 +869,7 @@ static int __devinit pn544_hci_probe(struct i2c_client *client,
 		    NFC_PROTO_MIFARE_MASK |
 		    NFC_PROTO_FELICA_MASK |
 		    NFC_PROTO_ISO14443_MASK |
+		    NFC_PROTO_ISO14443_B_MASK |
 		    NFC_PROTO_NFC_DEP_MASK;
 
 	info->shdlc = nfc_shdlc_allocate(&pn544_shdlc_ops,
diff --git a/drivers/of/base.c b/drivers/of/base.c
index d9bfd49b193..c181b94abc3 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -173,9 +173,9 @@ struct property *of_find_property(const struct device_node *np,
 		return NULL;
 
 	read_lock(&devtree_lock);
-	for (pp = np->properties; pp != 0; pp = pp->next) {
+	for (pp = np->properties; pp; pp = pp->next) {
 		if (of_prop_cmp(pp->name, name) == 0) {
-			if (lenp != 0)
+			if (lenp)
 				*lenp = pp->length;
 			break;
 		}
@@ -497,7 +497,7 @@ struct device_node *of_find_node_with_property(struct device_node *from,
 	read_lock(&devtree_lock);
 	np = from ? from->allnext : allnodes;
 	for (; np; np = np->allnext) {
-		for (pp = np->properties; pp != 0; pp = pp->next) {
+		for (pp = np->properties; pp; pp = pp->next) {
 			if (of_prop_cmp(pp->name, prop_name) == 0) {
 				of_node_get(np);
 				goto out;
@@ -902,7 +902,7 @@ int of_parse_phandle_with_args(struct device_node *np, const char *list_name,
 	/* Retrieve the phandle list property */
 	list = of_get_property(np, list_name, &size);
 	if (!list)
-		return -EINVAL;
+		return -ENOENT;
 	list_end = list + size / sizeof(*list);
 
 	/* Loop over the phandles until all the requested entry is found */
@@ -1051,7 +1051,8 @@ int prom_remove_property(struct device_node *np, struct property *prop)
 }
 
 /*
- * prom_update_property - Update a property in a node.
+ * prom_update_property - Update a property in a node, if the property does
+ * not exist, add it.
  *
  * Note that we don't actually remove it, since we have given out
  * who-knows-how-many pointers to the data using get-property.
@@ -1059,13 +1060,19 @@ int prom_remove_property(struct device_node *np, struct property *prop)
  * and add the new property to the property list
  */
 int prom_update_property(struct device_node *np,
-			 struct property *newprop,
-			 struct property *oldprop)
+			 struct property *newprop)
 {
-	struct property **next;
+	struct property **next, *oldprop;
 	unsigned long flags;
 	int found = 0;
 
+	if (!newprop->name)
+		return -EINVAL;
+
+	oldprop = of_find_property(np, newprop->name, NULL);
+	if (!oldprop)
+		return prom_add_property(np, newprop);
+
 	write_lock_irqsave(&devtree_lock, flags);
 	next = &np->properties;
 	while (*next) {
@@ -1173,7 +1180,7 @@ static void of_alias_add(struct alias_prop *ap, struct device_node *np,
 	ap->stem[stem_len] = 0;
 	list_add_tail(&ap->link, &aliases_lookup);
 	pr_debug("adding DT alias:%s: stem=%s id=%i node=%s\n",
-		 ap->alias, ap->stem, ap->id, np ? np->full_name : NULL);
+		 ap->alias, ap->stem, ap->id, of_node_full_name(np));
 }
 
 /**
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 9cf00602f56..ff8ab7b2737 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -255,7 +255,7 @@ int of_irq_map_raw(struct device_node *parent, const __be32 *intspec,
 
 	skiplevel:
 		/* Iterate again with new parent */
-		pr_debug(" -> new parent: %s\n", newpar ? newpar->full_name : "<>");
+		pr_debug(" -> new parent: %s\n", of_node_full_name(newpar));
 		of_node_put(ipar);
 		ipar = newpar;
 		newpar = NULL;
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index 2574abde8d9..8e6c25f3504 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -57,6 +57,7 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 		const __be32 *paddr;
 		u32 addr;
 		int len;
+		bool is_c45;
 
 		/* A PHY must have a reg property in the range [0-31] */
 		paddr = of_get_property(child, "reg", &len);
@@ -79,11 +80,18 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 				mdio->irq[addr] = PHY_POLL;
 		}
 
-		phy = get_phy_device(mdio, addr);
+		is_c45 = of_device_is_compatible(child,
+						 "ethernet-phy-ieee802.3-c45");
+		phy = get_phy_device(mdio, addr, is_c45);
+
 		if (!phy || IS_ERR(phy)) {
-			dev_err(&mdio->dev, "error probing PHY at address %i\n",
-				addr);
-			continue;
+			phy = phy_device_create(mdio, addr, 0, false, NULL);
+			if (!phy || IS_ERR(phy)) {
+				dev_err(&mdio->dev,
+					"error creating PHY at address %i\n",
+					addr);
+				continue;
+			}
 		}
 
 		/* Associate the OF node with the device structure so it
diff --git a/drivers/of/of_mtd.c b/drivers/of/of_mtd.c
index e7cad627a5d..a27ec94877e 100644
--- a/drivers/of/of_mtd.c
+++ b/drivers/of/of_mtd.c
@@ -32,7 +32,7 @@ static const char *nand_ecc_modes[] = {
  * The function gets ecc mode string from property 'nand-ecc-mode',
  * and return its index in nand_ecc_modes table, or errno in error case.
  */
-const int of_get_nand_ecc_mode(struct device_node *np)
+int of_get_nand_ecc_mode(struct device_node *np)
 {
 	const char *pm;
 	int err, i;
diff --git a/drivers/oprofile/oprofile_perf.c b/drivers/oprofile/oprofile_perf.c
index efc4b7f308c..f3cfa0b9adf 100644
--- a/drivers/oprofile/oprofile_perf.c
+++ b/drivers/oprofile/oprofile_perf.c
@@ -1,5 +1,6 @@
 /*
  * Copyright 2010 ARM Ltd.
+ * Copyright 2012 Advanced Micro Devices, Inc., Robert Richter
  *
  * Perf-events backend for OProfile.
  */
@@ -25,7 +26,7 @@ static int oprofile_perf_enabled;
 static DEFINE_MUTEX(oprofile_perf_mutex);
 
 static struct op_counter_config *counter_config;
-static struct perf_event **perf_events[NR_CPUS];
+static DEFINE_PER_CPU(struct perf_event **, perf_events);
 static int num_counters;
 
 /*
@@ -38,7 +39,7 @@ static void op_overflow_handler(struct perf_event *event,
 	u32 cpu = smp_processor_id();
 
 	for (id = 0; id < num_counters; ++id)
-		if (perf_events[cpu][id] == event)
+		if (per_cpu(perf_events, cpu)[id] == event)
 			break;
 
 	if (id != num_counters)
@@ -74,7 +75,7 @@ static int op_create_counter(int cpu, int event)
 {
 	struct perf_event *pevent;
 
-	if (!counter_config[event].enabled || perf_events[cpu][event])
+	if (!counter_config[event].enabled || per_cpu(perf_events, cpu)[event])
 		return 0;
 
 	pevent = perf_event_create_kernel_counter(&counter_config[event].attr,
@@ -91,18 +92,18 @@ static int op_create_counter(int cpu, int event)
 		return -EBUSY;
 	}
 
-	perf_events[cpu][event] = pevent;
+	per_cpu(perf_events, cpu)[event] = pevent;
 
 	return 0;
 }
 
 static void op_destroy_counter(int cpu, int event)
 {
-	struct perf_event *pevent = perf_events[cpu][event];
+	struct perf_event *pevent = per_cpu(perf_events, cpu)[event];
 
 	if (pevent) {
 		perf_event_release_kernel(pevent);
-		perf_events[cpu][event] = NULL;
+		per_cpu(perf_events, cpu)[event] = NULL;
 	}
 }
 
@@ -257,12 +258,12 @@ void oprofile_perf_exit(void)
 
 	for_each_possible_cpu(cpu) {
 		for (id = 0; id < num_counters; ++id) {
-			event = perf_events[cpu][id];
+			event = per_cpu(perf_events, cpu)[id];
 			if (event)
 				perf_event_release_kernel(event);
 		}
 
-		kfree(perf_events[cpu]);
+		kfree(per_cpu(perf_events, cpu));
 	}
 
 	kfree(counter_config);
@@ -277,8 +278,6 @@ int __init oprofile_perf_init(struct oprofile_operations *ops)
 	if (ret)
 		return ret;
 
-	memset(&perf_events, 0, sizeof(perf_events));
-
 	num_counters = perf_num_counters();
 	if (num_counters <= 0) {
 		pr_info("oprofile: no performance counters\n");
@@ -298,9 +297,9 @@ int __init oprofile_perf_init(struct oprofile_operations *ops)
 	}
 
 	for_each_possible_cpu(cpu) {
-		perf_events[cpu] = kcalloc(num_counters,
+		per_cpu(perf_events, cpu) = kcalloc(num_counters,
 				sizeof(struct perf_event *), GFP_KERNEL);
-		if (!perf_events[cpu]) {
+		if (!per_cpu(perf_events, cpu)) {
 			pr_info("oprofile: failed to allocate %d perf events "
 					"for cpu %d\n", num_counters, cpu);
 			ret = -ENOMEM;
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index 432d4bbcc62..ffddc4f6426 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -174,7 +174,7 @@ static int dino_cfg_read(struct pci_bus *bus, unsigned int devfn, int where,
 		int size, u32 *val)
 {
 	struct dino_device *d = DINO_DEV(parisc_walk_tree(bus->bridge));
-	u32 local_bus = (bus->parent == NULL) ? 0 : bus->secondary;
+	u32 local_bus = (bus->parent == NULL) ? 0 : bus->busn_res.start;
 	u32 v = DINO_CFG_TOK(local_bus, devfn, where & ~3);
 	void __iomem *base_addr = d->hba.base_addr;
 	unsigned long flags;
@@ -209,7 +209,7 @@ static int dino_cfg_write(struct pci_bus *bus, unsigned int devfn, int where,
 	int size, u32 val)
 {
 	struct dino_device *d = DINO_DEV(parisc_walk_tree(bus->bridge));
-	u32 local_bus = (bus->parent == NULL) ? 0 : bus->secondary;
+	u32 local_bus = (bus->parent == NULL) ? 0 : bus->busn_res.start;
 	u32 v = DINO_CFG_TOK(local_bus, devfn, where & ~3);
 	void __iomem *base_addr = d->hba.base_addr;
 	unsigned long flags;
@@ -554,7 +554,7 @@ dino_fixup_bus(struct pci_bus *bus)
         struct dino_device *dino_dev = DINO_DEV(parisc_walk_tree(bus->bridge));
 
 	DBG(KERN_WARNING "%s(0x%p) bus %d platform_data 0x%p\n",
-	    __func__, bus, bus->secondary,
+	    __func__, bus, bus->busn_res.start,
 	    bus->bridge->platform_data);
 
 	/* Firmware doesn't set up card-mode dino, so we have to */
@@ -898,6 +898,7 @@ static int __init dino_probe(struct parisc_device *dev)
 	LIST_HEAD(resources);
 	struct pci_bus *bus;
 	unsigned long hpa = dev->hpa.start;
+	int max;
 
 	name = "Dino";
 	if (is_card_dino(&dev->id)) {
@@ -983,6 +984,10 @@ static int __init dino_probe(struct parisc_device *dev)
 	if (dino_dev->hba.gmmio_space.flags)
 		pci_add_resource(&resources, &dino_dev->hba.gmmio_space);
 
+	dino_dev->hba.bus_num.start = dino_current_bus;
+	dino_dev->hba.bus_num.end = 255;
+	dino_dev->hba.bus_num.flags = IORESOURCE_BUS;
+	pci_add_resource(&resources, &dino_dev->hba.bus_num);
 	/*
 	** It's not used to avoid chicken/egg problems
 	** with configuration accessor functions.
@@ -998,12 +1003,13 @@ static int __init dino_probe(struct parisc_device *dev)
 		return 0;
 	}
 
-	bus->subordinate = pci_scan_child_bus(bus);
+	max = pci_scan_child_bus(bus);
+	pci_bus_update_busn_res_end(bus, max);
 
 	/* This code *depends* on scanning being single threaded
 	 * if it isn't, this global bus number count will fail
 	 */
-	dino_current_bus = bus->subordinate + 1;
+	dino_current_bus = max + 1;
 	pci_bus_assign_resources(bus);
 	pci_bus_add_devices(bus);
 	return 0;
diff --git a/drivers/parisc/iosapic.c b/drivers/parisc/iosapic.c
index 1f9e9fefb8e..9544cdc0d1a 100644
--- a/drivers/parisc/iosapic.c
+++ b/drivers/parisc/iosapic.c
@@ -146,7 +146,7 @@
 #endif
 
 #include <asm/ropes.h>
-#include "./iosapic_private.h"
+#include "iosapic_private.h"
 
 #define MODULE_NAME "iosapic"
 
@@ -532,7 +532,7 @@ iosapic_xlate_pin(struct iosapic_info *isi, struct pci_dev *pcidev)
 		intr_slot = PCI_SLOT(pcidev->devfn);
 	}
 	DBG_IRT("iosapic_xlate_pin:  bus %d slot %d pin %d\n",
-				pcidev->bus->secondary, intr_slot, intr_pin);
+			pcidev->bus->busn_res.start, intr_slot, intr_pin);
 
 	return irt_find_irqline(isi, intr_slot, intr_pin);
 }
diff --git a/drivers/parisc/lba_pci.c b/drivers/parisc/lba_pci.c
index 052fa230bc7..4f9cf2456f4 100644
--- a/drivers/parisc/lba_pci.c
+++ b/drivers/parisc/lba_pci.c
@@ -189,8 +189,8 @@ lba_dump_res(struct resource *r, int d)
 
 static int lba_device_present(u8 bus, u8 dfn, struct lba_device *d)
 {
-	u8 first_bus = d->hba.hba_bus->secondary;
-	u8 last_sub_bus = d->hba.hba_bus->subordinate;
+	u8 first_bus = d->hba.hba_bus->busn_res.start;
+	u8 last_sub_bus = d->hba.hba_bus->busn_res.end;
 
 	if ((bus < first_bus) ||
 	    (bus > last_sub_bus) ||
@@ -364,7 +364,7 @@ lba_rd_cfg(struct lba_device *d, u32 tok, u8 reg, u32 size)
 static int elroy_cfg_read(struct pci_bus *bus, unsigned int devfn, int pos, int size, u32 *data)
 {
 	struct lba_device *d = LBA_DEV(parisc_walk_tree(bus->bridge));
-	u32 local_bus = (bus->parent == NULL) ? 0 : bus->secondary;
+	u32 local_bus = (bus->parent == NULL) ? 0 : bus->busn_res.start;
 	u32 tok = LBA_CFG_TOK(local_bus, devfn);
 	void __iomem *data_reg = d->hba.base_addr + LBA_PCI_CFG_DATA;
 
@@ -380,7 +380,7 @@ static int elroy_cfg_read(struct pci_bus *bus, unsigned int devfn, int pos, int
 		return 0;
 	}
 
-	if (LBA_SKIP_PROBE(d) && !lba_device_present(bus->secondary, devfn, d)) {
+	if (LBA_SKIP_PROBE(d) && !lba_device_present(bus->busn_res.start, devfn, d)) {
 		DBG_CFG("%s(%x+%2x) -> -1 (b)\n", __func__, tok, pos);
 		/* either don't want to look or know device isn't present. */
 		*data = ~0U;
@@ -431,7 +431,7 @@ lba_wr_cfg(struct lba_device *d, u32 tok, u8 reg, u32 data, u32 size)
 static int elroy_cfg_write(struct pci_bus *bus, unsigned int devfn, int pos, int size, u32 data)
 {
 	struct lba_device *d = LBA_DEV(parisc_walk_tree(bus->bridge));
-	u32 local_bus = (bus->parent == NULL) ? 0 : bus->secondary;
+	u32 local_bus = (bus->parent == NULL) ? 0 : bus->busn_res.start;
 	u32 tok = LBA_CFG_TOK(local_bus,devfn);
 
 	if ((pos > 255) || (devfn > 255))
@@ -444,7 +444,7 @@ static int elroy_cfg_write(struct pci_bus *bus, unsigned int devfn, int pos, int
 		return 0;
 	}
 
-	if (LBA_SKIP_PROBE(d) && (!lba_device_present(bus->secondary, devfn, d))) {
+	if (LBA_SKIP_PROBE(d) && (!lba_device_present(bus->busn_res.start, devfn, d))) {
 		DBG_CFG("%s(%x+%2x) = 0x%x (b)\n", __func__, tok, pos,data);
 		return 1; /* New Workaround */
 	}
@@ -481,7 +481,7 @@ static struct pci_ops elroy_cfg_ops = {
 static int mercury_cfg_read(struct pci_bus *bus, unsigned int devfn, int pos, int size, u32 *data)
 {
 	struct lba_device *d = LBA_DEV(parisc_walk_tree(bus->bridge));
-	u32 local_bus = (bus->parent == NULL) ? 0 : bus->secondary;
+	u32 local_bus = (bus->parent == NULL) ? 0 : bus->busn_res.start;
 	u32 tok = LBA_CFG_TOK(local_bus, devfn);
 	void __iomem *data_reg = d->hba.base_addr + LBA_PCI_CFG_DATA;
 
@@ -514,7 +514,7 @@ static int mercury_cfg_write(struct pci_bus *bus, unsigned int devfn, int pos, i
 {
 	struct lba_device *d = LBA_DEV(parisc_walk_tree(bus->bridge));
 	void __iomem *data_reg = d->hba.base_addr + LBA_PCI_CFG_DATA;
-	u32 local_bus = (bus->parent == NULL) ? 0 : bus->secondary;
+	u32 local_bus = (bus->parent == NULL) ? 0 : bus->busn_res.start;
 	u32 tok = LBA_CFG_TOK(local_bus,devfn);
 
 	if ((pos > 255) || (devfn > 255))
@@ -636,7 +636,7 @@ lba_fixup_bus(struct pci_bus *bus)
 	struct lba_device *ldev = LBA_DEV(parisc_walk_tree(bus->bridge));
 
 	DBG("lba_fixup_bus(0x%p) bus %d platform_data 0x%p\n",
-		bus, bus->secondary, bus->bridge->platform_data);
+		bus, (int)bus->busn_res.start, bus->bridge->platform_data);
 
 	/*
 	** Properly Setup MMIO resources for this bus.
@@ -989,6 +989,7 @@ lba_pat_resources(struct parisc_device *pa_dev, struct lba_device *lba_dev)
 		case PAT_PBNUM:
 			lba_dev->hba.bus_num.start = p->start;
 			lba_dev->hba.bus_num.end   = p->end;
+			lba_dev->hba.bus_num.flags = IORESOURCE_BUS;
 			break;
 
 		case PAT_LMMIO:
@@ -1366,6 +1367,7 @@ lba_driver_probe(struct parisc_device *dev)
 	void *tmp_obj;
 	char *version;
 	void __iomem *addr = ioremap_nocache(dev->hpa.start, 4096);
+	int max;
 
 	/* Read HW Rev First */
 	func_class = READ_REG32(addr + LBA_FCLASS);
@@ -1502,6 +1504,8 @@ lba_driver_probe(struct parisc_device *dev)
 	if (lba_dev->hba.gmmio_space.flags)
 		pci_add_resource(&resources, &lba_dev->hba.gmmio_space);
 
+	pci_add_resource(&resources, &lba_dev->hba.bus_num);
+
 	dev->dev.platform_data = lba_dev;
 	lba_bus = lba_dev->hba.hba_bus =
 		pci_create_root_bus(&dev->dev, lba_dev->hba.bus_num.start,
@@ -1511,7 +1515,7 @@ lba_driver_probe(struct parisc_device *dev)
 		return 0;
 	}
 
-	lba_bus->subordinate = pci_scan_child_bus(lba_bus);
+	max = pci_scan_child_bus(lba_bus);
 
 	/* This is in lieu of calling pci_assign_unassigned_resources() */
 	if (is_pdc_pat()) {
@@ -1541,7 +1545,7 @@ lba_driver_probe(struct parisc_device *dev)
 		lba_dev->flags |= LBA_FLAG_SKIP_PROBE;
 	}
 
-	lba_next_bus = lba_bus->subordinate + 1;
+	lba_next_bus = max + 1;
 	pci_bus_add_devices(lba_bus);
 
 	/* Whew! Finally done! Tell services we got this one covered. */
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index 01c001f3b76..8d688b260e2 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -49,6 +49,7 @@ obj-$(CONFIG_MN10300) += setup-bus.o
 obj-$(CONFIG_MICROBLAZE) += setup-bus.o
 obj-$(CONFIG_TILE) += setup-bus.o setup-irq.o
 obj-$(CONFIG_SPARC_LEON) += setup-bus.o setup-irq.o
+obj-$(CONFIG_M68K) += setup-bus.o setup-irq.o
 
 #
 # ACPI Related PCI FW Functions
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 2a581642c23..ba91a7e1751 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -162,7 +162,8 @@ int pci_user_read_config_##size						\
 	if (ret > 0)							\
 		ret = -EINVAL;						\
 	return ret;							\
-}
+}									\
+EXPORT_SYMBOL_GPL(pci_user_read_config_##size);
 
 /* Returns 0 on success, negative values indicate error. */
 #define PCI_USER_WRITE_CONFIG(size,type)				\
@@ -181,7 +182,8 @@ int pci_user_write_config_##size					\
 	if (ret > 0)							\
 		ret = -EINVAL;						\
 	return ret;							\
-}
+}									\
+EXPORT_SYMBOL_GPL(pci_user_write_config_##size);
 
 PCI_USER_READ_CONFIG(byte, u8)
 PCI_USER_READ_CONFIG(word, u16)
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 4ce5ef2f282..4b0970b46e0 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -164,6 +164,8 @@ pci_bus_alloc_resource(struct pci_bus *bus, struct resource *res,
 int pci_bus_add_device(struct pci_dev *dev)
 {
 	int retval;
+
+	pci_fixup_device(pci_fixup_final, dev);
 	retval = device_add(&dev->dev);
 	if (retval)
 		return retval;
diff --git a/drivers/pci/hotplug-pci.c b/drivers/pci/hotplug-pci.c
index d3509cdeb55..6258dc260d9 100644
--- a/drivers/pci/hotplug-pci.c
+++ b/drivers/pci/hotplug-pci.c
@@ -4,18 +4,26 @@
 #include <linux/export.h>
 #include "pci.h"
 
-
-unsigned int __devinit pci_do_scan_bus(struct pci_bus *bus)
+int __ref pci_hp_add_bridge(struct pci_dev *dev)
 {
-	unsigned int max;
-
-	max = pci_scan_child_bus(bus);
+	struct pci_bus *parent = dev->bus;
+	int pass, busnr, start = parent->busn_res.start;
+	int end = parent->busn_res.end;
 
-	/*
-	 * Make the discovered devices available.
-	 */
-	pci_bus_add_devices(bus);
+	for (busnr = start; busnr <= end; busnr++) {
+		if (!pci_find_bus(pci_domain_nr(parent), busnr))
+			break;
+	}
+	if (busnr-- > end) {
+		printk(KERN_ERR "No bus number available for hot-added bridge %s\n",
+				pci_name(dev));
+		return -1;
+	}
+	for (pass = 0; pass < 2; pass++)
+		busnr = pci_scan_bridge(parent, dev, busnr, pass);
+	if (!dev->subordinate)
+		return -1;
 
-	return max;
+	return 0;
 }
-EXPORT_SYMBOL(pci_do_scan_bus);
+EXPORT_SYMBOL_GPL(pci_hp_add_bridge);
diff --git a/drivers/pci/hotplug/acpiphp.h b/drivers/pci/hotplug/acpiphp.h
index 7722108e78d..a1afb5b39ad 100644
--- a/drivers/pci/hotplug/acpiphp.h
+++ b/drivers/pci/hotplug/acpiphp.h
@@ -89,8 +89,6 @@ struct acpiphp_bridge {
 
 	/* PCI-to-PCI bridge device */
 	struct pci_dev *pci_dev;
-
-	spinlock_t res_lock;
 };
 
 
@@ -207,6 +205,6 @@ extern u8 acpiphp_get_latch_status (struct acpiphp_slot *slot);
 extern u8 acpiphp_get_adapter_status (struct acpiphp_slot *slot);
 
 /* variables */
-extern int acpiphp_debug;
+extern bool acpiphp_debug;
 
 #endif /* _ACPIPHP_H */
diff --git a/drivers/pci/hotplug/acpiphp_core.c b/drivers/pci/hotplug/acpiphp_core.c
index aa41631e9e0..96316b74969 100644
--- a/drivers/pci/hotplug/acpiphp_core.c
+++ b/drivers/pci/hotplug/acpiphp_core.c
@@ -47,8 +47,7 @@
 /* name size which is used for entries in pcihpfs */
 #define SLOT_NAME_SIZE  21              /* {_SUN} */
 
-static bool debug;
-int acpiphp_debug;
+bool acpiphp_debug;
 
 /* local variables */
 static int num_slots;
@@ -62,7 +61,7 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 MODULE_PARM_DESC(debug, "Debugging mode enabled or not");
-module_param(debug, bool, 0644);
+module_param_named(debug, acpiphp_debug, bool, 0644);
 
 /* export the attention callback registration methods */
 EXPORT_SYMBOL_GPL(acpiphp_register_attention);
@@ -379,8 +378,6 @@ static int __init acpiphp_init(void)
 	if (acpi_pci_disabled)
 		return 0;
 
-	acpiphp_debug = debug;
-
 	/* read all the ACPI info from the system */
 	return init_acpi();
 }
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index 806c44fa645..ad6fd669549 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -100,11 +100,11 @@ static int post_dock_fixups(struct notifier_block *nb, unsigned long val,
 			PCI_PRIMARY_BUS,
 			&buses);
 
-	if (((buses >> 8) & 0xff) != bus->secondary) {
+	if (((buses >> 8) & 0xff) != bus->busn_res.start) {
 		buses = (buses & 0xff000000)
 			| ((unsigned int)(bus->primary)     <<  0)
-			| ((unsigned int)(bus->secondary)   <<  8)
-			| ((unsigned int)(bus->subordinate) << 16);
+			| ((unsigned int)(bus->busn_res.start)   <<  8)
+			| ((unsigned int)(bus->busn_res.end) << 16);
 		pci_write_config_dword(bus->self, PCI_PRIMARY_BUS, buses);
 	}
 	return NOTIFY_OK;
@@ -132,6 +132,15 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 	if (!acpi_pci_check_ejectable(pbus, handle) && !is_dock_device(handle))
 		return AE_OK;
 
+	status = acpi_evaluate_integer(handle, "_ADR", NULL, &adr);
+	if (ACPI_FAILURE(status)) {
+		warn("can't evaluate _ADR (%#x)\n", status);
+		return AE_OK;
+	}
+
+	device = (adr >> 16) & 0xffff;
+	function = adr & 0xffff;
+
 	pdev = pbus->self;
 	if (pdev && pci_is_pcie(pdev)) {
 		tmp = acpi_find_root_bridge_handle(pdev);
@@ -144,10 +153,6 @@ register_slot(acpi_handle handle, u32 lvl, void *context, void **rv)
 		}
 	}
 
-	acpi_evaluate_integer(handle, "_ADR", NULL, &adr);
-	device = (adr >> 16) & 0xffff;
-	function = adr & 0xffff;
-
 	newfunc = kzalloc(sizeof(struct acpiphp_func), GFP_KERNEL);
 	if (!newfunc)
 		return AE_NO_MEMORY;
@@ -391,8 +396,6 @@ static void add_host_bridge(acpi_handle *handle)
 
 	bridge->pci_bus = root->bus;
 
-	spin_lock_init(&bridge->res_lock);
-
 	init_bridge_misc(bridge);
 }
 
@@ -425,7 +428,6 @@ static void add_p2p_bridge(acpi_handle *handle)
 	 * (which we access during module unload).
 	 */
 	get_device(&bridge->pci_bus->dev);
-	spin_lock_init(&bridge->res_lock);
 
 	init_bridge_misc(bridge);
 	return;
@@ -692,7 +694,7 @@ static unsigned char acpiphp_max_busnr(struct pci_bus *bus)
 	 * bus->subordinate value because it could have
 	 * padding in it.
 	 */
-	max = bus->secondary;
+	max = bus->busn_res.start;
 
 	list_for_each(tmp, &bus->children) {
 		n = pci_bus_max_busnr(pci_bus_b(tmp));
@@ -878,6 +880,24 @@ static void disable_bridges(struct pci_bus *bus)
 	}
 }
 
+/* return first device in slot, acquiring a reference on it */
+static struct pci_dev *dev_in_slot(struct acpiphp_slot *slot)
+{
+	struct pci_bus *bus = slot->bridge->pci_bus;
+	struct pci_dev *dev;
+	struct pci_dev *ret = NULL;
+
+	down_read(&pci_bus_sem);
+	list_for_each_entry(dev, &bus->devices, bus_list)
+		if (PCI_SLOT(dev->devfn) == slot->device) {
+			ret = pci_dev_get(dev);
+			break;
+		}
+	up_read(&pci_bus_sem);
+
+	return ret;
+}
+
 /**
  * disable_device - disable a slot
  * @slot: ACPI PHP slot
@@ -893,6 +913,7 @@ static int disable_device(struct acpiphp_slot *slot)
 	pdev = pci_get_slot(bus, PCI_DEVFN(slot->device, 0));
 	if (!pdev)
 		goto err_exit;
+	pci_dev_put(pdev);
 
 	list_for_each_entry(func, &slot->funcs, sibling) {
 		if (func->bridge) {
@@ -901,18 +922,22 @@ static int disable_device(struct acpiphp_slot *slot)
 						(u32)1, NULL, NULL);
 			func->bridge = NULL;
 		}
+	}
 
-		pdev = pci_get_slot(slot->bridge->pci_bus,
-				    PCI_DEVFN(slot->device, func->function));
-		if (pdev) {
-			pci_stop_bus_device(pdev);
-			if (pdev->subordinate) {
-				disable_bridges(pdev->subordinate);
-				pci_disable_device(pdev);
-			}
-			__pci_remove_bus_device(pdev);
-			pci_dev_put(pdev);
+	/*
+	 * enable_device() enumerates all functions in this device via
+	 * pci_scan_slot(), whether they have associated ACPI hotplug
+	 * methods (_EJ0, etc.) or not.  Therefore, we remove all functions
+	 * here.
+	 */
+	while ((pdev = dev_in_slot(slot))) {
+		pci_stop_bus_device(pdev);
+		if (pdev->subordinate) {
+			disable_bridges(pdev->subordinate);
+			pci_disable_device(pdev);
 		}
+		__pci_remove_bus_device(pdev);
+		pci_dev_put(pdev);
 	}
 
 	list_for_each_entry(func, &slot->funcs, sibling) {
diff --git a/drivers/pci/hotplug/cpci_hotplug_core.c b/drivers/pci/hotplug/cpci_hotplug_core.c
index 3fadf2f135e..2b4c412f94c 100644
--- a/drivers/pci/hotplug/cpci_hotplug_core.c
+++ b/drivers/pci/hotplug/cpci_hotplug_core.c
@@ -225,7 +225,7 @@ cpci_hp_register_bus(struct pci_bus *bus, u8 first, u8 last)
 	struct hotplug_slot *hotplug_slot;
 	struct hotplug_slot_info *info;
 	char name[SLOT_NAME_SIZE];
-	int status = -ENOMEM;
+	int status;
 	int i;
 
 	if (!(controller && bus))
@@ -237,18 +237,24 @@ cpci_hp_register_bus(struct pci_bus *bus, u8 first, u8 last)
 	 */
 	for (i = first; i <= last; ++i) {
 		slot = kzalloc(sizeof (struct slot), GFP_KERNEL);
-		if (!slot)
+		if (!slot) {
+			status = -ENOMEM;
 			goto error;
+		}
 
 		hotplug_slot =
 			kzalloc(sizeof (struct hotplug_slot), GFP_KERNEL);
-		if (!hotplug_slot)
+		if (!hotplug_slot) {
+			status = -ENOMEM;
 			goto error_slot;
+		}
 		slot->hotplug_slot = hotplug_slot;
 
 		info = kzalloc(sizeof (struct hotplug_slot_info), GFP_KERNEL);
-		if (!info)
+		if (!info) {
+			status = -ENOMEM;
 			goto error_hpslot;
+		}
 		hotplug_slot->info = info;
 
 		slot->bus = bus;
diff --git a/drivers/pci/hotplug/cpci_hotplug_pci.c b/drivers/pci/hotplug/cpci_hotplug_pci.c
index ae853ccd0cd..dcc75c78544 100644
--- a/drivers/pci/hotplug/cpci_hotplug_pci.c
+++ b/drivers/pci/hotplug/cpci_hotplug_pci.c
@@ -285,42 +285,19 @@ int __ref cpci_configure_slot(struct slot *slot)
 	for (fn = 0; fn < 8; fn++) {
 		struct pci_dev *dev;
 
-		dev = pci_get_slot(parent, PCI_DEVFN(PCI_SLOT(slot->devfn), fn));
+		dev = pci_get_slot(parent,
+				   PCI_DEVFN(PCI_SLOT(slot->devfn), fn));
 		if (!dev)
 			continue;
 		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)) {
-			/* Find an unused bus number for the new bridge */
-			struct pci_bus *child;
-			unsigned char busnr, start = parent->secondary;
-			unsigned char end = parent->subordinate;
-
-			for (busnr = start; busnr <= end; busnr++) {
-				if (!pci_find_bus(pci_domain_nr(parent),
-						  busnr))
-					break;
-			}
-			if (busnr >= end) {
-				err("No free bus for hot-added bridge\n");
-				pci_dev_put(dev);
-				continue;
-			}
-			child = pci_add_new_bus(parent, dev, busnr);
-			if (!child) {
-				err("Cannot add new bus for %s\n",
-				    pci_name(dev));
-				pci_dev_put(dev);
-				continue;
-			}
-			child->subordinate = pci_do_scan_bus(child);
-			pci_bus_size_bridges(child);
-		}
+		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+			pci_hp_add_bridge(dev);
 		pci_dev_put(dev);
 	}
 
-	pci_bus_assign_resources(parent);
+	pci_assign_unassigned_bridge_resources(parent->self);
+
 	pci_bus_add_devices(parent);
-	pci_enable_bridges(parent);
 
 	dbg("%s - exit", __func__);
 	return 0;
diff --git a/drivers/pci/hotplug/cpqphp_core.c b/drivers/pci/hotplug/cpqphp_core.c
index 187a199da93..c8eaeb43fa5 100644
--- a/drivers/pci/hotplug/cpqphp_core.c
+++ b/drivers/pci/hotplug/cpqphp_core.c
@@ -611,7 +611,7 @@ static int ctrl_slot_setup(struct controller *ctrl,
 	u32 tempdword;
 	char name[SLOT_NAME_SIZE];
 	void __iomem *slot_entry= NULL;
-	int result = -ENOMEM;
+	int result;
 
 	dbg("%s\n", __func__);
 
@@ -623,19 +623,25 @@ static int ctrl_slot_setup(struct controller *ctrl,
 
 	while (number_of_slots) {
 		slot = kzalloc(sizeof(*slot), GFP_KERNEL);
-		if (!slot)
+		if (!slot) {
+			result = -ENOMEM;
 			goto error;
+		}
 
 		slot->hotplug_slot = kzalloc(sizeof(*(slot->hotplug_slot)),
 						GFP_KERNEL);
-		if (!slot->hotplug_slot)
+		if (!slot->hotplug_slot) {
+			result = -ENOMEM;
 			goto error_slot;
+		}
 		hotplug_slot = slot->hotplug_slot;
 
 		hotplug_slot->info = kzalloc(sizeof(*(hotplug_slot->info)),
 							GFP_KERNEL);
-		if (!hotplug_slot->info)
+		if (!hotplug_slot->info) {
+			result = -ENOMEM;
 			goto error_hpslot;
+		}
 		hotplug_slot_info = hotplug_slot->info;
 
 		slot->ctrl = ctrl;
diff --git a/drivers/pci/hotplug/cpqphp_pci.c b/drivers/pci/hotplug/cpqphp_pci.c
index 1c8494021a4..09801c6945c 100644
--- a/drivers/pci/hotplug/cpqphp_pci.c
+++ b/drivers/pci/hotplug/cpqphp_pci.c
@@ -83,7 +83,6 @@ static void __iomem *detect_HRT_floating_pointer(void __iomem *begin, void __iom
 
 int cpqhp_configure_device (struct controller* ctrl, struct pci_func* func)
 {
-	unsigned char bus;
 	struct pci_bus *child;
 	int num;
 
@@ -106,9 +105,10 @@ int cpqhp_configure_device (struct controller* ctrl, struct pci_func* func)
 	}
 
 	if (func->pci_dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) {
-		pci_read_config_byte(func->pci_dev, PCI_SECONDARY_BUS, &bus);
-		child = (struct pci_bus*) pci_add_new_bus(func->pci_dev->bus, (func->pci_dev), bus);
-		pci_do_scan_bus(child);
+		pci_hp_add_bridge(func->pci_dev);
+		child = func->pci_dev->subordinate;
+		if (child)
+			pci_bus_add_devices(child);
 	}
 
 	pci_dev_put(func->pci_dev);
diff --git a/drivers/pci/hotplug/ibmphp_core.c b/drivers/pci/hotplug/ibmphp_core.c
index 4fda7e6a86a..cbd72d81d25 100644
--- a/drivers/pci/hotplug/ibmphp_core.c
+++ b/drivers/pci/hotplug/ibmphp_core.c
@@ -760,7 +760,7 @@ static u8 bus_structure_fixup(u8 busno)
 	for (dev->devfn = 0; dev->devfn < 256; dev->devfn += 8) {
 		if (!pci_read_config_word(dev, PCI_VENDOR_ID, &l) &&
 					(l != 0x0000) && (l != 0xffff)) {
-			debug("%s - Inside bus_struture_fixup()\n",
+			debug("%s - Inside bus_structure_fixup()\n",
 							__func__);
 			pci_scan_bus(busno, ibmphp_pci_bus->ops, NULL);
 			break;
@@ -775,7 +775,6 @@ static u8 bus_structure_fixup(u8 busno)
 
 static int ibm_configure_device(struct pci_func *func)
 {
-	unsigned char bus;
 	struct pci_bus *child;
 	int num;
 	int flag = 0;	/* this is to make sure we don't double scan the bus,
@@ -805,9 +804,10 @@ static int ibm_configure_device(struct pci_func *func)
 		}
 	}
 	if (!(flag) && (func->dev->hdr_type == PCI_HEADER_TYPE_BRIDGE)) {
-		pci_read_config_byte(func->dev, PCI_SECONDARY_BUS, &bus);
-		child = pci_add_new_bus(func->dev->bus, func->dev, bus);
-		pci_do_scan_bus(child);
+		pci_hp_add_bridge(func->dev);
+		child = func->dev->subordinate;
+		if (child)
+			pci_bus_add_devices(child);
 	}
 
 	return 0;
diff --git a/drivers/pci/hotplug/ibmphp_ebda.c b/drivers/pci/hotplug/ibmphp_ebda.c
index 714ca5c4ed5..9df78bc1454 100644
--- a/drivers/pci/hotplug/ibmphp_ebda.c
+++ b/drivers/pci/hotplug/ibmphp_ebda.c
@@ -784,7 +784,7 @@ static int __init ebda_rsrc_controller (void)
 		hpc_ptr->ctlr_relative_id = ctlr;
 		hpc_ptr->slot_count = slot_num;
 		hpc_ptr->bus_count = bus_num;
-		debug ("now enter ctlr data struture ---\n");
+		debug ("now enter ctlr data structure ---\n");
 		debug ("ctlr id: %x\n", ctlr_id);
 		debug ("ctlr_relative_id: %x\n", hpc_ptr->ctlr_relative_id);
 		debug ("count of slots controlled by this ctlr: %x\n", slot_num);
diff --git a/drivers/pci/hotplug/ibmphp_pci.c b/drivers/pci/hotplug/ibmphp_pci.c
index 7b09e16173a..c60f5f3e838 100644
--- a/drivers/pci/hotplug/ibmphp_pci.c
+++ b/drivers/pci/hotplug/ibmphp_pci.c
@@ -109,7 +109,7 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 
 		cur_func->function = function;
 
-		debug ("inside the loop, cur_func->busno = %x, cur_func->device = %x, cur_func->funcion = %x\n",
+		debug ("inside the loop, cur_func->busno = %x, cur_func->device = %x, cur_func->function = %x\n",
 			cur_func->busno, cur_func->device, cur_func->function);
 
 		pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 4b7cce1de6e..26ffd3e3fb7 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -149,10 +149,6 @@ int pciehp_get_attention_status(struct slot *slot, u8 *status);
 int pciehp_set_attention_status(struct slot *slot, u8 status);
 int pciehp_get_latch_status(struct slot *slot, u8 *status);
 int pciehp_get_adapter_status(struct slot *slot, u8 *status);
-int pciehp_get_max_link_speed(struct slot *slot, enum pci_bus_speed *speed);
-int pciehp_get_max_link_width(struct slot *slot, enum pcie_link_width *val);
-int pciehp_get_cur_link_speed(struct slot *slot, enum pci_bus_speed *speed);
-int pciehp_get_cur_link_width(struct slot *slot, enum pcie_link_width *val);
 int pciehp_query_power_fault(struct slot *slot);
 void pciehp_green_led_on(struct slot *slot);
 void pciehp_green_led_off(struct slot *slot);
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index a960faec102..302451e8289 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -705,107 +705,6 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-int pciehp_get_max_lnk_width(struct slot *slot,
-				 enum pcie_link_width *value)
-{
-	struct controller *ctrl = slot->ctrl;
-	enum pcie_link_width lnk_wdth;
-	u32	lnk_cap;
-	int retval = 0;
-
-	retval = pciehp_readl(ctrl, PCI_EXP_LNKCAP, &lnk_cap);
-	if (retval) {
-		ctrl_err(ctrl, "%s: Cannot read LNKCAP register\n", __func__);
-		return retval;
-	}
-
-	switch ((lnk_cap & PCI_EXP_LNKSTA_NLW) >> 4){
-	case 0:
-		lnk_wdth = PCIE_LNK_WIDTH_RESRV;
-		break;
-	case 1:
-		lnk_wdth = PCIE_LNK_X1;
-		break;
-	case 2:
-		lnk_wdth = PCIE_LNK_X2;
-		break;
-	case 4:
-		lnk_wdth = PCIE_LNK_X4;
-		break;
-	case 8:
-		lnk_wdth = PCIE_LNK_X8;
-		break;
-	case 12:
-		lnk_wdth = PCIE_LNK_X12;
-		break;
-	case 16:
-		lnk_wdth = PCIE_LNK_X16;
-		break;
-	case 32:
-		lnk_wdth = PCIE_LNK_X32;
-		break;
-	default:
-		lnk_wdth = PCIE_LNK_WIDTH_UNKNOWN;
-		break;
-	}
-
-	*value = lnk_wdth;
-	ctrl_dbg(ctrl, "Max link width = %d\n", lnk_wdth);
-
-	return retval;
-}
-
-int pciehp_get_cur_lnk_width(struct slot *slot,
-				 enum pcie_link_width *value)
-{
-	struct controller *ctrl = slot->ctrl;
-	enum pcie_link_width lnk_wdth = PCIE_LNK_WIDTH_UNKNOWN;
-	int retval = 0;
-	u16 lnk_status;
-
-	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
-	if (retval) {
-		ctrl_err(ctrl, "%s: Cannot read LNKSTATUS register\n",
-			 __func__);
-		return retval;
-	}
-
-	switch ((lnk_status & PCI_EXP_LNKSTA_NLW) >> 4){
-	case 0:
-		lnk_wdth = PCIE_LNK_WIDTH_RESRV;
-		break;
-	case 1:
-		lnk_wdth = PCIE_LNK_X1;
-		break;
-	case 2:
-		lnk_wdth = PCIE_LNK_X2;
-		break;
-	case 4:
-		lnk_wdth = PCIE_LNK_X4;
-		break;
-	case 8:
-		lnk_wdth = PCIE_LNK_X8;
-		break;
-	case 12:
-		lnk_wdth = PCIE_LNK_X12;
-		break;
-	case 16:
-		lnk_wdth = PCIE_LNK_X16;
-		break;
-	case 32:
-		lnk_wdth = PCIE_LNK_X32;
-		break;
-	default:
-		lnk_wdth = PCIE_LNK_WIDTH_UNKNOWN;
-		break;
-	}
-
-	*value = lnk_wdth;
-	ctrl_dbg(ctrl, "Current link width = %d\n", lnk_wdth);
-
-	return retval;
-}
-
 int pcie_enable_notification(struct controller *ctrl)
 {
 	u16 cmd, mask;
diff --git a/drivers/pci/hotplug/pciehp_pci.c b/drivers/pci/hotplug/pciehp_pci.c
index 47d9dc06b10..09cecaf450c 100644
--- a/drivers/pci/hotplug/pciehp_pci.c
+++ b/drivers/pci/hotplug/pciehp_pci.c
@@ -34,29 +34,6 @@
 #include "../pci.h"
 #include "pciehp.h"
 
-static int __ref pciehp_add_bridge(struct pci_dev *dev)
-{
-	struct pci_bus *parent = dev->bus;
-	int pass, busnr, start = parent->secondary;
-	int end = parent->subordinate;
-
-	for (busnr = start; busnr <= end; busnr++) {
-		if (!pci_find_bus(pci_domain_nr(parent), busnr))
-			break;
-	}
-	if (busnr-- > end) {
-		err("No bus number available for hot-added bridge %s\n",
-				pci_name(dev));
-		return -1;
-	}
-	for (pass = 0; pass < 2; pass++)
-		busnr = pci_scan_bridge(parent, dev, busnr, pass);
-	if (!dev->subordinate)
-		return -1;
-
-	return 0;
-}
-
 int pciehp_configure_device(struct slot *p_slot)
 {
 	struct pci_dev *dev;
@@ -85,9 +62,8 @@ int pciehp_configure_device(struct slot *p_slot)
 		if (!dev)
 			continue;
 		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-				(dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)) {
-			pciehp_add_bridge(dev);
-		}
+				(dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+			pci_hp_add_bridge(dev);
 		pci_dev_put(dev);
 	}
 
diff --git a/drivers/pci/hotplug/pcihp_skeleton.c b/drivers/pci/hotplug/pcihp_skeleton.c
index b20ceaaa31f..1f00b937f72 100644
--- a/drivers/pci/hotplug/pcihp_skeleton.c
+++ b/drivers/pci/hotplug/pcihp_skeleton.c
@@ -252,7 +252,7 @@ static int __init init_slots(void)
 	struct slot *slot;
 	struct hotplug_slot *hotplug_slot;
 	struct hotplug_slot_info *info;
-	int retval = -ENOMEM;
+	int retval;
 	int i;
 
 	/*
@@ -261,17 +261,23 @@ static int __init init_slots(void)
 	 */
 	for (i = 0; i < num_slots; ++i) {
 		slot = kzalloc(sizeof(*slot), GFP_KERNEL);
-		if (!slot)
+		if (!slot) {
+			retval = -ENOMEM;
 			goto error;
+		}
 
 		hotplug_slot = kzalloc(sizeof(*hotplug_slot), GFP_KERNEL);
-		if (!hotplug_slot)
+		if (!hotplug_slot) {
+			retval = -ENOMEM;
 			goto error_slot;
+		}
 		slot->hotplug_slot = hotplug_slot;
 
 		info = kzalloc(sizeof(*info), GFP_KERNEL);
-		if (!info)
+		if (!info) {
+			retval = -ENOMEM;
 			goto error_hpslot;
+		}
 		hotplug_slot->info = info;
 
 		slot->number = i;
diff --git a/drivers/pci/hotplug/sgi_hotplug.c b/drivers/pci/hotplug/sgi_hotplug.c
index de573113c10..f64ca92253d 100644
--- a/drivers/pci/hotplug/sgi_hotplug.c
+++ b/drivers/pci/hotplug/sgi_hotplug.c
@@ -397,13 +397,11 @@ static int enable_slot(struct hotplug_slot *bss_hotplug_slot)
 			else
 				sn_io_slot_fixup(dev);
 			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) {
-				unsigned char sec_bus;
-				pci_read_config_byte(dev, PCI_SECONDARY_BUS,
-						     &sec_bus);
-				new_bus = pci_add_new_bus(dev->bus, dev,
-							  sec_bus);
-				pci_scan_child_bus(new_bus);
-				new_ppb = 1;
+				pci_hp_add_bridge(dev);
+				if (dev->subordinate) {
+					new_bus = dev->subordinate;
+					new_ppb = 1;
+				}
 			}
 			pci_dev_put(dev);
 		}
diff --git a/drivers/pci/hotplug/shpchp_core.c b/drivers/pci/hotplug/shpchp_core.c
index 7414fd9ad1d..b6de307248e 100644
--- a/drivers/pci/hotplug/shpchp_core.c
+++ b/drivers/pci/hotplug/shpchp_core.c
@@ -99,22 +99,28 @@ static int init_slots(struct controller *ctrl)
 	struct hotplug_slot *hotplug_slot;
 	struct hotplug_slot_info *info;
 	char name[SLOT_NAME_SIZE];
-	int retval = -ENOMEM;
+	int retval;
 	int i;
 
 	for (i = 0; i < ctrl->num_slots; i++) {
 		slot = kzalloc(sizeof(*slot), GFP_KERNEL);
-		if (!slot)
+		if (!slot) {
+			retval = -ENOMEM;
 			goto error;
+		}
 
 		hotplug_slot = kzalloc(sizeof(*hotplug_slot), GFP_KERNEL);
-		if (!hotplug_slot)
+		if (!hotplug_slot) {
+			retval = -ENOMEM;
 			goto error_slot;
+		}
 		slot->hotplug_slot = hotplug_slot;
 
 		info = kzalloc(sizeof(*info), GFP_KERNEL);
-		if (!info)
+		if (!info) {
+			retval = -ENOMEM;
 			goto error_hpslot;
+		}
 		hotplug_slot->info = info;
 
 		slot->hp_slot = i;
diff --git a/drivers/pci/hotplug/shpchp_ctrl.c b/drivers/pci/hotplug/shpchp_ctrl.c
index b00b09bdd38..f9b5a52e411 100644
--- a/drivers/pci/hotplug/shpchp_ctrl.c
+++ b/drivers/pci/hotplug/shpchp_ctrl.c
@@ -262,9 +262,6 @@ static int board_added(struct slot *p_slot)
 	}
 
 	if ((ctrl->pci_dev->vendor == 0x8086) && (ctrl->pci_dev->device == 0x0332)) {
-		if (slots_not_empty)
-			return WRONG_BUS_FREQUENCY;
-
 		if ((rc = p_slot->hpc_ops->set_bus_speed_mode(p_slot, PCI_SPEED_33MHz))) {
 			ctrl_err(ctrl, "%s: Issue of set bus speed mode command"
 				 " failed\n", __func__);
diff --git a/drivers/pci/hotplug/shpchp_pci.c b/drivers/pci/hotplug/shpchp_pci.c
index df7e4bfadae..c627ed9957d 100644
--- a/drivers/pci/hotplug/shpchp_pci.c
+++ b/drivers/pci/hotplug/shpchp_pci.c
@@ -37,9 +37,10 @@
 int __ref shpchp_configure_device(struct slot *p_slot)
 {
 	struct pci_dev *dev;
-	struct pci_bus *parent = p_slot->ctrl->pci_dev->subordinate;
-	int num, fn;
 	struct controller *ctrl = p_slot->ctrl;
+	struct pci_dev *bridge = ctrl->pci_dev;
+	struct pci_bus *parent = bridge->subordinate;
+	int num, fn;
 
 	dev = pci_get_slot(parent, PCI_DEVFN(p_slot->device, 0));
 	if (dev) {
@@ -61,39 +62,23 @@ int __ref shpchp_configure_device(struct slot *p_slot)
 		if (!dev)
 			continue;
 		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-				(dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)) {
-			/* Find an unused bus number for the new bridge */
-			struct pci_bus *child;
-			unsigned char busnr, start = parent->secondary;
-			unsigned char end = parent->subordinate;
-			for (busnr = start; busnr <= end; busnr++) {
-				if (!pci_find_bus(pci_domain_nr(parent),
-							busnr))
-					break;
-			}
-			if (busnr > end) {
-				ctrl_err(ctrl,
-					 "No free bus for hot-added bridge\n");
-				pci_dev_put(dev);
-				continue;
-			}
-			child = pci_add_new_bus(parent, dev, busnr);
-			if (!child) {
-				ctrl_err(ctrl, "Cannot add new bus for %s\n",
-					 pci_name(dev));
-				pci_dev_put(dev);
-				continue;
-			}
-			child->subordinate = pci_do_scan_bus(child);
-			pci_bus_size_bridges(child);
-		}
+		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+			pci_hp_add_bridge(dev);
+		pci_dev_put(dev);
+	}
+
+	pci_assign_unassigned_bridge_resources(bridge);
+
+	for (fn = 0; fn < 8; fn++) {
+		dev = pci_get_slot(parent, PCI_DEVFN(p_slot->device, fn));
+		if (!dev)
+			continue;
 		pci_configure_slot(dev);
 		pci_dev_put(dev);
 	}
 
-	pci_bus_assign_resources(parent);
 	pci_bus_add_devices(parent);
-	pci_enable_bridges(parent);
+
 	return 0;
 }
 
diff --git a/drivers/pci/hotplug/shpchp_sysfs.c b/drivers/pci/hotplug/shpchp_sysfs.c
index efa30da1ae8..eeb23ceae4a 100644
--- a/drivers/pci/hotplug/shpchp_sysfs.c
+++ b/drivers/pci/hotplug/shpchp_sysfs.c
@@ -73,13 +73,13 @@ static ssize_t show_ctrl (struct device *dev, struct device_attribute *attr, cha
 		}
 	}
 	out += sprintf(out, "Free resources: bus numbers\n");
-	for (busnr = bus->secondary; busnr <= bus->subordinate; busnr++) {
+	for (busnr = bus->busn_res.start; busnr <= bus->busn_res.end; busnr++) {
 		if (!pci_find_bus(pci_domain_nr(bus), busnr))
 			break;
 	}
-	if (busnr < bus->subordinate)
+	if (busnr < bus->busn_res.end)
 		out += sprintf(out, "start = %8.8x, length = %8.8x\n",
-				busnr, (bus->subordinate - busnr));
+				busnr, (int)(bus->busn_res.end - busnr));
 
 	return out - buf;
 }
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index 6554e1a0f63..74bbaf82638 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -47,7 +47,7 @@ static struct pci_bus *virtfn_add_bus(struct pci_bus *bus, int busnr)
 	if (!child)
 		return NULL;
 
-	child->subordinate = busnr;
+	pci_bus_insert_busn_res(child, busnr, busnr);
 	child->dev.parent = bus->bridge;
 	rc = pci_bus_add_child(child);
 	if (rc) {
@@ -327,7 +327,7 @@ static int sriov_enable(struct pci_dev *dev, int nr_virtfn)
 	iov->offset = offset;
 	iov->stride = stride;
 
-	if (virtfn_bus(dev, nr_virtfn - 1) > dev->bus->subordinate) {
+	if (virtfn_bus(dev, nr_virtfn - 1) > dev->bus->busn_res.end) {
 		dev_err(&dev->dev, "SR-IOV: bus number out of range\n");
 		return -ENOMEM;
 	}
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 61e2fefeeda..fbf7b26c7c8 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -48,6 +48,12 @@ static void pci_acpi_wake_dev(acpi_handle handle, u32 event, void *context)
 	if (event != ACPI_NOTIFY_DEVICE_WAKE || !pci_dev)
 		return;
 
+	if (pci_dev->current_state == PCI_D3cold) {
+		pci_wakeup_event(pci_dev);
+		pm_runtime_resume(&pci_dev->dev);
+		return;
+	}
+
 	if (!pci_dev->pm_cap || !pci_dev->pme_support
 	     || pci_check_pme_status(pci_dev)) {
 		if (pci_dev->pme_poll)
@@ -162,6 +168,20 @@ acpi_status pci_acpi_remove_pm_notifier(struct acpi_device *dev)
 	return remove_pm_notifier(dev, pci_acpi_wake_dev);
 }
 
+phys_addr_t acpi_pci_root_get_mcfg_addr(acpi_handle handle)
+{
+	acpi_status status = AE_NOT_EXIST;
+	unsigned long long mcfg_addr;
+
+	if (handle)
+		status = acpi_evaluate_integer(handle, METHOD_NAME__CBA,
+					       NULL, &mcfg_addr);
+	if (ACPI_FAILURE(status))
+		return 0;
+
+	return (phys_addr_t)mcfg_addr;
+}
+
 /*
  * _SxD returns the D-state with the highest power
  * (lowest D-state number) supported in the S-state "x".
@@ -187,9 +207,13 @@ acpi_status pci_acpi_remove_pm_notifier(struct acpi_device *dev)
 
 static pci_power_t acpi_pci_choose_state(struct pci_dev *pdev)
 {
-	int acpi_state;
+	int acpi_state, d_max;
 
-	acpi_state = acpi_pm_device_sleep_state(&pdev->dev, NULL);
+	if (pdev->no_d3cold)
+		d_max = ACPI_STATE_D3_HOT;
+	else
+		d_max = ACPI_STATE_D3_COLD;
+	acpi_state = acpi_pm_device_sleep_state(&pdev->dev, NULL, d_max);
 	if (acpi_state < 0)
 		return PCI_POWER_ERROR;
 
@@ -296,7 +320,13 @@ static void acpi_pci_propagate_run_wake(struct pci_bus *bus, bool enable)
 
 static int acpi_pci_run_wake(struct pci_dev *dev, bool enable)
 {
-	if (dev->pme_interrupt)
+	/*
+	 * Per PCI Express Base Specification Revision 2.0 section
+	 * 5.3.3.2 Link Wakeup, platform support is needed for D3cold
+	 * waking up to power on the main link even if there is PME
+	 * support for D3cold
+	 */
+	if (dev->pme_interrupt && !dev->runtime_d3cold)
 		return 0;
 
 	if (!acpi_pm_device_run_wake(&dev->dev, enable))
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index 099f46cd8e8..185be370334 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -459,16 +459,17 @@ static int pci_restore_standard_config(struct pci_dev *pci_dev)
 	return 0;
 }
 
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+
 static void pci_pm_default_resume_early(struct pci_dev *pci_dev)
 {
-	pci_restore_standard_config(pci_dev);
+	pci_power_up(pci_dev);
+	pci_restore_state(pci_dev);
 	pci_fixup_device(pci_fixup_resume_early, pci_dev);
 }
 
-#endif
-
-#ifdef CONFIG_PM_SLEEP
-
 /*
  * Default "suspend" method for devices that have no driver provided suspend,
  * or not even a driver at all (second part).
@@ -1031,10 +1032,13 @@ static int pci_pm_runtime_suspend(struct device *dev)
 	if (!pm || !pm->runtime_suspend)
 		return -ENOSYS;
 
+	pci_dev->no_d3cold = false;
 	error = pm->runtime_suspend(dev);
 	suspend_report_result(pm->runtime_suspend, error);
 	if (error)
 		return error;
+	if (!pci_dev->d3cold_allowed)
+		pci_dev->no_d3cold = true;
 
 	pci_fixup_device(pci_fixup_suspend, pci_dev);
 
@@ -1056,17 +1060,23 @@ static int pci_pm_runtime_suspend(struct device *dev)
 
 static int pci_pm_runtime_resume(struct device *dev)
 {
+	int rc;
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
 
 	if (!pm || !pm->runtime_resume)
 		return -ENOSYS;
 
-	pci_pm_default_resume_early(pci_dev);
+	pci_restore_standard_config(pci_dev);
+	pci_fixup_device(pci_fixup_resume_early, pci_dev);
 	__pci_enable_wake(pci_dev, PCI_D0, true, false);
 	pci_fixup_device(pci_fixup_resume, pci_dev);
 
-	return pm->runtime_resume(dev);
+	rc = pm->runtime_resume(dev);
+
+	pci_dev->runtime_d3cold = false;
+
+	return rc;
 }
 
 static int pci_pm_runtime_idle(struct device *dev)
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index 86c63fe45d1..6869009c739 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -28,6 +28,7 @@
 #include <linux/pci-aspm.h>
 #include <linux/slab.h>
 #include <linux/vgaarb.h>
+#include <linux/pm_runtime.h>
 #include "pci.h"
 
 static int sysfs_initialized;	/* = 0 */
@@ -378,6 +379,31 @@ dev_bus_rescan_store(struct device *dev, struct device_attribute *attr,
 
 #endif
 
+#if defined(CONFIG_PM_RUNTIME) && defined(CONFIG_ACPI)
+static ssize_t d3cold_allowed_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val) < 0)
+		return -EINVAL;
+
+	pdev->d3cold_allowed = !!val;
+	pm_runtime_resume(dev);
+
+	return count;
+}
+
+static ssize_t d3cold_allowed_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	return sprintf (buf, "%u\n", pdev->d3cold_allowed);
+}
+#endif
+
 struct device_attribute pci_dev_attrs[] = {
 	__ATTR_RO(resource),
 	__ATTR_RO(vendor),
@@ -402,6 +428,9 @@ struct device_attribute pci_dev_attrs[] = {
 	__ATTR(remove, (S_IWUSR|S_IWGRP), NULL, remove_store),
 	__ATTR(rescan, (S_IWUSR|S_IWGRP), NULL, dev_rescan_store),
 #endif
+#if defined(CONFIG_PM_RUNTIME) && defined(CONFIG_ACPI)
+	__ATTR(d3cold_allowed, 0644, d3cold_allowed_show, d3cold_allowed_store),
+#endif
 	__ATTR_NULL,
 };
 
@@ -1112,7 +1141,7 @@ static struct bin_attribute pcie_config_attr = {
 	.write = pci_write_config,
 };
 
-int __attribute__ ((weak)) pcibios_add_platform_entries(struct pci_dev *dev)
+int __weak pcibios_add_platform_entries(struct pci_dev *dev)
 {
 	return 0;
 }
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 447e83472c0..f3ea977a5b1 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -110,7 +110,7 @@ unsigned char pci_bus_max_busnr(struct pci_bus* bus)
 	struct list_head *tmp;
 	unsigned char max, n;
 
-	max = bus->subordinate;
+	max = bus->busn_res.end;
 	list_for_each(tmp, &bus->children) {
 		n = pci_bus_max_busnr(pci_bus_b(tmp));
 		if(n > max)
@@ -136,30 +136,6 @@ void __iomem *pci_ioremap_bar(struct pci_dev *pdev, int bar)
 EXPORT_SYMBOL_GPL(pci_ioremap_bar);
 #endif
 
-#if 0
-/**
- * pci_max_busnr - returns maximum PCI bus number
- *
- * Returns the highest PCI bus number present in the system global list of
- * PCI buses.
- */
-unsigned char __devinit
-pci_max_busnr(void)
-{
-	struct pci_bus *bus = NULL;
-	unsigned char max, n;
-
-	max = 0;
-	while ((bus = pci_find_next_bus(bus)) != NULL) {
-		n = pci_bus_max_busnr(bus);
-		if(n > max)
-			max = n;
-	}
-	return max;
-}
-
-#endif  /*  0  */
-
 #define PCI_FIND_CAP_TTL	48
 
 static int __pci_find_next_cap_ttl(struct pci_bus *bus, unsigned int devfn,
@@ -278,6 +254,38 @@ int pci_bus_find_capability(struct pci_bus *bus, unsigned int devfn, int cap)
 }
 
 /**
+ * pci_pcie_cap2 - query for devices' PCI_CAP_ID_EXP v2 capability structure
+ * @dev: PCI device to check
+ *
+ * Like pci_pcie_cap() but also checks that the PCIe capability version is
+ * >= 2.  Note that v1 capability structures could be sparse in that not
+ * all register fields were required.  v2 requires the entire structure to
+ * be present size wise, while still allowing for non-implemented registers
+ * to exist but they must be hardwired to 0.
+ *
+ * Due to the differences in the versions of capability structures, one
+ * must be careful not to try and access non-existant registers that may
+ * exist in early versions - v1 - of Express devices.
+ *
+ * Returns the offset of the PCIe capability structure as long as the
+ * capability version is >= 2; otherwise 0 is returned.
+ */
+static int pci_pcie_cap2(struct pci_dev *dev)
+{
+	u16 flags;
+	int pos;
+
+	pos = pci_pcie_cap(dev);
+	if (pos) {
+		pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &flags);
+		if ((flags & PCI_EXP_FLAGS_VERS) < 2)
+			pos = 0;
+	}
+
+	return pos;
+}
+
+/**
  * pci_find_ext_capability - Find an extended capability
  * @dev: PCI device to query
  * @cap: capability code
@@ -329,49 +337,6 @@ int pci_find_ext_capability(struct pci_dev *dev, int cap)
 }
 EXPORT_SYMBOL_GPL(pci_find_ext_capability);
 
-/**
- * pci_bus_find_ext_capability - find an extended capability
- * @bus:   the PCI bus to query
- * @devfn: PCI device to query
- * @cap:   capability code
- *
- * Like pci_find_ext_capability() but works for pci devices that do not have a
- * pci_dev structure set up yet.
- *
- * Returns the address of the requested capability structure within the
- * device's PCI configuration space or 0 in case the device does not
- * support it.
- */
-int pci_bus_find_ext_capability(struct pci_bus *bus, unsigned int devfn,
-				int cap)
-{
-	u32 header;
-	int ttl;
-	int pos = PCI_CFG_SPACE_SIZE;
-
-	/* minimum 8 bytes per capability */
-	ttl = (PCI_CFG_SPACE_EXP_SIZE - PCI_CFG_SPACE_SIZE) / 8;
-
-	if (!pci_bus_read_config_dword(bus, devfn, pos, &header))
-		return 0;
-	if (header == 0xffffffff || header == 0)
-		return 0;
-
-	while (ttl-- > 0) {
-		if (PCI_EXT_CAP_ID(header) == cap)
-			return pos;
-
-		pos = PCI_EXT_CAP_NEXT(header);
-		if (pos < PCI_CFG_SPACE_SIZE)
-			break;
-
-		if (!pci_bus_read_config_dword(bus, devfn, pos, &header))
-			break;
-	}
-
-	return 0;
-}
-
 static int __pci_find_next_ht_cap(struct pci_dev *dev, int pos, int ht_cap)
 {
 	int rc, ttl = PCI_FIND_CAP_TTL;
@@ -622,7 +587,8 @@ static int pci_raw_set_power_state(struct pci_dev *dev, pci_power_t state)
 		dev_info(&dev->dev, "Refused to change power state, "
 			"currently in D%d\n", dev->current_state);
 
-	/* According to section 5.4.1 of the "PCI BUS POWER MANAGEMENT
+	/*
+	 * According to section 5.4.1 of the "PCI BUS POWER MANAGEMENT
 	 * INTERFACE SPECIFICATION, REV. 1.2", a device transitioning
 	 * from D3hot to D0 _may_ perform an internal reset, thereby
 	 * going to "D0 Uninitialized" rather than "D0 Initialized".
@@ -654,6 +620,16 @@ void pci_update_current_state(struct pci_dev *dev, pci_power_t state)
 	if (dev->pm_cap) {
 		u16 pmcsr;
 
+		/*
+		 * Configuration space is not accessible for device in
+		 * D3cold, so just keep or set D3cold for safety
+		 */
+		if (dev->current_state == PCI_D3cold)
+			return;
+		if (state == PCI_D3cold) {
+			dev->current_state = PCI_D3cold;
+			return;
+		}
 		pci_read_config_word(dev, dev->pm_cap + PCI_PM_CTRL, &pmcsr);
 		dev->current_state = (pmcsr & PCI_PM_CTRL_STATE_MASK);
 	} else {
@@ -662,6 +638,19 @@ void pci_update_current_state(struct pci_dev *dev, pci_power_t state)
 }
 
 /**
+ * pci_power_up - Put the given device into D0 forcibly
+ * @dev: PCI device to power up
+ */
+void pci_power_up(struct pci_dev *dev)
+{
+	if (platform_pci_power_manageable(dev))
+		platform_pci_set_power_state(dev, PCI_D0);
+
+	pci_raw_set_power_state(dev, PCI_D0);
+	pci_update_current_state(dev, PCI_D0);
+}
+
+/**
  * pci_platform_power_transition - Use platform to change device power state
  * @dev: PCI device to handle.
  * @state: State to put the device into.
@@ -694,8 +683,50 @@ static int pci_platform_power_transition(struct pci_dev *dev, pci_power_t state)
  */
 static void __pci_start_power_transition(struct pci_dev *dev, pci_power_t state)
 {
-	if (state == PCI_D0)
+	if (state == PCI_D0) {
 		pci_platform_power_transition(dev, PCI_D0);
+		/*
+		 * Mandatory power management transition delays, see
+		 * PCI Express Base Specification Revision 2.0 Section
+		 * 6.6.1: Conventional Reset.  Do not delay for
+		 * devices powered on/off by corresponding bridge,
+		 * because have already delayed for the bridge.
+		 */
+		if (dev->runtime_d3cold) {
+			msleep(dev->d3cold_delay);
+			/*
+			 * When powering on a bridge from D3cold, the
+			 * whole hierarchy may be powered on into
+			 * D0uninitialized state, resume them to give
+			 * them a chance to suspend again
+			 */
+			pci_wakeup_bus(dev->subordinate);
+		}
+	}
+}
+
+/**
+ * __pci_dev_set_current_state - Set current state of a PCI device
+ * @dev: Device to handle
+ * @data: pointer to state to be set
+ */
+static int __pci_dev_set_current_state(struct pci_dev *dev, void *data)
+{
+	pci_power_t state = *(pci_power_t *)data;
+
+	dev->current_state = state;
+	return 0;
+}
+
+/**
+ * __pci_bus_set_current_state - Walk given bus and set current state of devices
+ * @bus: Top bus of the subtree to walk.
+ * @state: state to be set
+ */
+static void __pci_bus_set_current_state(struct pci_bus *bus, pci_power_t state)
+{
+	if (bus)
+		pci_walk_bus(bus, __pci_dev_set_current_state, &state);
 }
 
 /**
@@ -707,8 +738,15 @@ static void __pci_start_power_transition(struct pci_dev *dev, pci_power_t state)
  */
 int __pci_complete_power_transition(struct pci_dev *dev, pci_power_t state)
 {
-	return state >= PCI_D0 ?
-			pci_platform_power_transition(dev, state) : -EINVAL;
+	int ret;
+
+	if (state <= PCI_D0)
+		return -EINVAL;
+	ret = pci_platform_power_transition(dev, state);
+	/* Power off the bridge may power off the whole hierarchy */
+	if (!ret && state == PCI_D3cold)
+		__pci_bus_set_current_state(dev->subordinate, PCI_D3cold);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(__pci_complete_power_transition);
 
@@ -732,8 +770,8 @@ int pci_set_power_state(struct pci_dev *dev, pci_power_t state)
 	int error;
 
 	/* bound the state we're entering */
-	if (state > PCI_D3hot)
-		state = PCI_D3hot;
+	if (state > PCI_D3cold)
+		state = PCI_D3cold;
 	else if (state < PCI_D0)
 		state = PCI_D0;
 	else if ((state == PCI_D1 || state == PCI_D2) && pci_no_d1d2(dev))
@@ -744,14 +782,23 @@ int pci_set_power_state(struct pci_dev *dev, pci_power_t state)
 		 */
 		return 0;
 
+	/* Check if we're already there */
+	if (dev->current_state == state)
+		return 0;
+
 	__pci_start_power_transition(dev, state);
 
 	/* This device is quirked not to be put into D3, so
 	   don't put it in D3 */
-	if (state == PCI_D3hot && (dev->dev_flags & PCI_DEV_FLAGS_NO_D3))
+	if (state >= PCI_D3hot && (dev->dev_flags & PCI_DEV_FLAGS_NO_D3))
 		return 0;
 
-	error = pci_raw_set_power_state(dev, state);
+	/*
+	 * To put device in D3cold, we put device into D3hot in native
+	 * way, then put device into D3cold with platform ops
+	 */
+	error = pci_raw_set_power_state(dev, state > PCI_D3hot ?
+					PCI_D3hot : state);
 
 	if (!__pci_complete_power_transition(dev, state))
 		error = 0;
@@ -822,12 +869,6 @@ EXPORT_SYMBOL(pci_choose_state);
 		((flags & PCI_EXP_FLAGS_VERS) > 1 ||	\
 		 (type == PCI_EXP_TYPE_ROOT_PORT ||	\
 		  type == PCI_EXP_TYPE_RC_EC))
-#define pcie_cap_has_devctl2(type, flags)		\
-		((flags & PCI_EXP_FLAGS_VERS) > 1)
-#define pcie_cap_has_lnkctl2(type, flags)		\
-		((flags & PCI_EXP_FLAGS_VERS) > 1)
-#define pcie_cap_has_sltctl2(type, flags)		\
-		((flags & PCI_EXP_FLAGS_VERS) > 1)
 
 static struct pci_cap_saved_state *pci_find_saved_cap(
 	struct pci_dev *pci_dev, char cap)
@@ -870,13 +911,14 @@ static int pci_save_pcie_state(struct pci_dev *dev)
 		pci_read_config_word(dev, pos + PCI_EXP_SLTCTL, &cap[i++]);
 	if (pcie_cap_has_rtctl(dev->pcie_type, flags))
 		pci_read_config_word(dev, pos + PCI_EXP_RTCTL, &cap[i++]);
-	if (pcie_cap_has_devctl2(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &cap[i++]);
-	if (pcie_cap_has_lnkctl2(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_LNKCTL2, &cap[i++]);
-	if (pcie_cap_has_sltctl2(dev->pcie_type, flags))
-		pci_read_config_word(dev, pos + PCI_EXP_SLTCTL2, &cap[i++]);
 
+	pos = pci_pcie_cap2(dev);
+	if (!pos)
+		return 0;
+
+	pci_read_config_word(dev, pos + PCI_EXP_DEVCTL2, &cap[i++]);
+	pci_read_config_word(dev, pos + PCI_EXP_LNKCTL2, &cap[i++]);
+	pci_read_config_word(dev, pos + PCI_EXP_SLTCTL2, &cap[i++]);
 	return 0;
 }
 
@@ -903,12 +945,14 @@ static void pci_restore_pcie_state(struct pci_dev *dev)
 		pci_write_config_word(dev, pos + PCI_EXP_SLTCTL, cap[i++]);
 	if (pcie_cap_has_rtctl(dev->pcie_type, flags))
 		pci_write_config_word(dev, pos + PCI_EXP_RTCTL, cap[i++]);
-	if (pcie_cap_has_devctl2(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, cap[i++]);
-	if (pcie_cap_has_lnkctl2(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_LNKCTL2, cap[i++]);
-	if (pcie_cap_has_sltctl2(dev->pcie_type, flags))
-		pci_write_config_word(dev, pos + PCI_EXP_SLTCTL2, cap[i++]);
+
+	pos = pci_pcie_cap2(dev);
+	if (!pos)
+		return;
+
+	pci_write_config_word(dev, pos + PCI_EXP_DEVCTL2, cap[i++]);
+	pci_write_config_word(dev, pos + PCI_EXP_LNKCTL2, cap[i++]);
+	pci_write_config_word(dev, pos + PCI_EXP_SLTCTL2, cap[i++]);
 }
 
 
@@ -1349,7 +1393,7 @@ void pcim_pin_device(struct pci_dev *pdev)
  * is the default implementation. Architecture implementations can
  * override this.
  */
-void __attribute__ ((weak)) pcibios_disable_device (struct pci_dev *dev) {}
+void __weak pcibios_disable_device (struct pci_dev *dev) {}
 
 static void do_pci_disable_device(struct pci_dev *dev)
 {
@@ -1413,8 +1457,8 @@ pci_disable_device(struct pci_dev *dev)
  * Sets the PCIe reset state for the device. This is the default
  * implementation. Architecture implementations can override this.
  */
-int __attribute__ ((weak)) pcibios_set_pcie_reset_state(struct pci_dev *dev,
-							enum pcie_reset_state state)
+int __weak pcibios_set_pcie_reset_state(struct pci_dev *dev,
+					enum pcie_reset_state state)
 {
 	return -EINVAL;
 }
@@ -1498,6 +1542,28 @@ void pci_pme_wakeup_bus(struct pci_bus *bus)
 }
 
 /**
+ * pci_wakeup - Wake up a PCI device
+ * @dev: Device to handle.
+ * @ign: ignored parameter
+ */
+static int pci_wakeup(struct pci_dev *pci_dev, void *ign)
+{
+	pci_wakeup_event(pci_dev);
+	pm_request_resume(&pci_dev->dev);
+	return 0;
+}
+
+/**
+ * pci_wakeup_bus - Walk given bus and wake up devices on it
+ * @bus: Top bus of the subtree to walk.
+ */
+void pci_wakeup_bus(struct pci_bus *bus)
+{
+	if (bus)
+		pci_walk_bus(bus, pci_wakeup, NULL);
+}
+
+/**
  * pci_pme_capable - check the capability of PCI device to generate PME#
  * @dev: PCI device to handle.
  * @state: PCI state from which device will issue PME#.
@@ -1518,6 +1584,16 @@ static void pci_pme_list_scan(struct work_struct *work)
 	if (!list_empty(&pci_pme_list)) {
 		list_for_each_entry_safe(pme_dev, n, &pci_pme_list, list) {
 			if (pme_dev->dev->pme_poll) {
+				struct pci_dev *bridge;
+
+				bridge = pme_dev->dev->bus->self;
+				/*
+				 * If bridge is in low power state, the
+				 * configuration space of subordinate devices
+				 * may be not accessible
+				 */
+				if (bridge && bridge->current_state != PCI_D0)
+					continue;
 				pci_pme_wakeup(pme_dev->dev, NULL);
 			} else {
 				list_del(&pme_dev->list);
@@ -1744,6 +1820,10 @@ int pci_prepare_to_sleep(struct pci_dev *dev)
 	if (target_state == PCI_POWER_ERROR)
 		return -EIO;
 
+	/* D3cold during system suspend/hibernate is not supported */
+	if (target_state > PCI_D3hot)
+		target_state = PCI_D3hot;
+
 	pci_enable_wake(dev, target_state, device_may_wakeup(&dev->dev));
 
 	error = pci_set_power_state(dev, target_state);
@@ -1781,12 +1861,16 @@ int pci_finish_runtime_suspend(struct pci_dev *dev)
 	if (target_state == PCI_POWER_ERROR)
 		return -EIO;
 
+	dev->runtime_d3cold = target_state == PCI_D3cold;
+
 	__pci_enable_wake(dev, target_state, true, pci_dev_run_wake(dev));
 
 	error = pci_set_power_state(dev, target_state);
 
-	if (error)
+	if (error) {
 		__pci_enable_wake(dev, target_state, true, false);
+		dev->runtime_d3cold = false;
+	}
 
 	return error;
 }
@@ -1856,6 +1940,7 @@ void pci_pm_init(struct pci_dev *dev)
 
 	dev->pm_cap = pm;
 	dev->d3_delay = PCI_PM_D3_WAIT;
+	dev->d3cold_delay = PCI_PM_D3COLD_WAIT;
 
 	dev->d1_support = false;
 	dev->d2_support = false;
@@ -1983,7 +2068,7 @@ void pci_enable_ari(struct pci_dev *dev)
 {
 	int pos;
 	u32 cap;
-	u16 flags, ctrl;
+	u16 ctrl;
 	struct pci_dev *bridge;
 
 	if (pcie_ari_disabled || !pci_is_pcie(dev) || dev->devfn)
@@ -1994,18 +2079,14 @@ void pci_enable_ari(struct pci_dev *dev)
 		return;
 
 	bridge = dev->bus->self;
-	if (!bridge || !pci_is_pcie(bridge))
+	if (!bridge)
 		return;
 
-	pos = pci_pcie_cap(bridge);
+	/* ARI is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(bridge);
 	if (!pos)
 		return;
 
-	/* ARI is a PCIe v2 feature */
-	pci_read_config_word(bridge, pos + PCI_EXP_FLAGS, &flags);
-	if ((flags & PCI_EXP_FLAGS_VERS) < 2)
-		return;
-
 	pci_read_config_dword(bridge, pos + PCI_EXP_DEVCAP2, &cap);
 	if (!(cap & PCI_EXP_DEVCAP2_ARI))
 		return;
@@ -2018,7 +2099,7 @@ void pci_enable_ari(struct pci_dev *dev)
 }
 
 /**
- * pci_enable_ido - enable ID-based ordering on a device
+ * pci_enable_ido - enable ID-based Ordering on a device
  * @dev: the PCI device
  * @type: which types of IDO to enable
  *
@@ -2031,7 +2112,8 @@ void pci_enable_ido(struct pci_dev *dev, unsigned long type)
 	int pos;
 	u16 ctrl;
 
-	pos = pci_pcie_cap(dev);
+	/* ID-based Ordering is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return;
 
@@ -2054,10 +2136,8 @@ void pci_disable_ido(struct pci_dev *dev, unsigned long type)
 	int pos;
 	u16 ctrl;
 
-	if (!pci_is_pcie(dev))
-		return;
-
-	pos = pci_pcie_cap(dev);
+	/* ID-based Ordering is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return;
 
@@ -2096,10 +2176,8 @@ int pci_enable_obff(struct pci_dev *dev, enum pci_obff_signal_type type)
 	u16 ctrl;
 	int ret;
 
-	if (!pci_is_pcie(dev))
-		return -ENOTSUPP;
-
-	pos = pci_pcie_cap(dev);
+	/* OBFF is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return -ENOTSUPP;
 
@@ -2108,7 +2186,7 @@ int pci_enable_obff(struct pci_dev *dev, enum pci_obff_signal_type type)
 		return -ENOTSUPP; /* no OBFF support at all */
 
 	/* Make sure the topology supports OBFF as well */
-	if (dev->bus) {
+	if (dev->bus->self) {
 		ret = pci_enable_obff(dev->bus->self, type);
 		if (ret)
 			return ret;
@@ -2149,10 +2227,8 @@ void pci_disable_obff(struct pci_dev *dev)
 	int pos;
 	u16 ctrl;
 
-	if (!pci_is_pcie(dev))
-		return;
-
-	pos = pci_pcie_cap(dev);
+	/* OBFF is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return;
 
@@ -2169,15 +2245,13 @@ EXPORT_SYMBOL(pci_disable_obff);
  * RETURNS:
  * True if @dev supports latency tolerance reporting, false otherwise.
  */
-bool pci_ltr_supported(struct pci_dev *dev)
+static bool pci_ltr_supported(struct pci_dev *dev)
 {
 	int pos;
 	u32 cap;
 
-	if (!pci_is_pcie(dev))
-		return false;
-
-	pos = pci_pcie_cap(dev);
+	/* LTR is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return false;
 
@@ -2185,7 +2259,6 @@ bool pci_ltr_supported(struct pci_dev *dev)
 
 	return cap & PCI_EXP_DEVCAP2_LTR;
 }
-EXPORT_SYMBOL(pci_ltr_supported);
 
 /**
  * pci_enable_ltr - enable latency tolerance reporting
@@ -2206,7 +2279,8 @@ int pci_enable_ltr(struct pci_dev *dev)
 	if (!pci_ltr_supported(dev))
 		return -ENOTSUPP;
 
-	pos = pci_pcie_cap(dev);
+	/* LTR is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return -ENOTSUPP;
 
@@ -2215,7 +2289,7 @@ int pci_enable_ltr(struct pci_dev *dev)
 		return -EINVAL;
 
 	/* Enable upstream ports first */
-	if (dev->bus) {
+	if (dev->bus->self) {
 		ret = pci_enable_ltr(dev->bus->self);
 		if (ret)
 			return ret;
@@ -2241,7 +2315,8 @@ void pci_disable_ltr(struct pci_dev *dev)
 	if (!pci_ltr_supported(dev))
 		return;
 
-	pos = pci_pcie_cap(dev);
+	/* LTR is a PCIe cap v2 feature */
+	pos = pci_pcie_cap2(dev);
 	if (!pos)
 		return;
 
@@ -2360,6 +2435,75 @@ void pci_enable_acs(struct pci_dev *dev)
 }
 
 /**
+ * pci_acs_enabled - test ACS against required flags for a given device
+ * @pdev: device to test
+ * @acs_flags: required PCI ACS flags
+ *
+ * Return true if the device supports the provided flags.  Automatically
+ * filters out flags that are not implemented on multifunction devices.
+ */
+bool pci_acs_enabled(struct pci_dev *pdev, u16 acs_flags)
+{
+	int pos, ret;
+	u16 ctrl;
+
+	ret = pci_dev_specific_acs_enabled(pdev, acs_flags);
+	if (ret >= 0)
+		return ret > 0;
+
+	if (!pci_is_pcie(pdev))
+		return false;
+
+	/* Filter out flags not applicable to multifunction */
+	if (pdev->multifunction)
+		acs_flags &= (PCI_ACS_RR | PCI_ACS_CR |
+			      PCI_ACS_EC | PCI_ACS_DT);
+
+	if (pdev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM ||
+	    pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT ||
+	    pdev->multifunction) {
+		pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ACS);
+		if (!pos)
+			return false;
+
+		pci_read_config_word(pdev, pos + PCI_ACS_CTRL, &ctrl);
+		if ((ctrl & acs_flags) != acs_flags)
+			return false;
+	}
+
+	return true;
+}
+
+/**
+ * pci_acs_path_enable - test ACS flags from start to end in a hierarchy
+ * @start: starting downstream device
+ * @end: ending upstream device or NULL to search to the root bus
+ * @acs_flags: required flags
+ *
+ * Walk up a device tree from start to end testing PCI ACS support.  If
+ * any step along the way does not support the required flags, return false.
+ */
+bool pci_acs_path_enabled(struct pci_dev *start,
+			  struct pci_dev *end, u16 acs_flags)
+{
+	struct pci_dev *pdev, *parent = start;
+
+	do {
+		pdev = parent;
+
+		if (!pci_acs_enabled(pdev, acs_flags))
+			return false;
+
+		if (pci_is_root_bus(pdev->bus))
+			return (end == NULL);
+
+		parent = pdev->bus->self;
+	} while (pdev != end);
+
+	return true;
+}
+
+/**
  * pci_swizzle_interrupt_pin - swizzle INTx for device behind bridge
  * @dev: the PCI device
  * @pin: the INTx pin (1=INTA, 2=INTB, 3=INTD, 4=INTD)
@@ -2666,6 +2810,18 @@ static void __pci_set_master(struct pci_dev *dev, bool enable)
 }
 
 /**
+ * pcibios_setup - process "pci=" kernel boot arguments
+ * @str: string used to pass in "pci=" kernel boot arguments
+ *
+ * Process kernel boot arguments.  This is the default implementation.
+ * Architecture specific implementations can override this as necessary.
+ */
+char * __weak __init pcibios_setup(char *str)
+{
+	return str;
+}
+
+/**
  * pcibios_set_master - enable PCI bus-mastering for device dev
  * @dev: the PCI device to enable
  *
@@ -2876,6 +3032,9 @@ bool pci_intx_mask_supported(struct pci_dev *dev)
 	bool mask_supported = false;
 	u16 orig, new;
 
+	if (dev->broken_intx_masking)
+		return false;
+
 	pci_cfg_access_lock(dev);
 
 	pci_read_config_word(dev, PCI_COMMAND, &orig);
@@ -3395,8 +3554,7 @@ int pcix_set_mmrbc(struct pci_dev *dev, int mmrbc)
 
 	o = (cmd & PCI_X_CMD_MAX_READ) >> 2;
 	if (o != v) {
-		if (v > o && dev->bus &&
-		   (dev->bus->bus_flags & PCI_BUS_FLAGS_NO_MMRBC))
+		if (v > o && (dev->bus->bus_flags & PCI_BUS_FLAGS_NO_MMRBC))
 			return -EIO;
 
 		cmd &= ~PCI_X_CMD_MAX_READ;
@@ -3851,7 +4009,7 @@ static void __devinit pci_no_domains(void)
  * greater than 0xff). This is the default implementation. Architecture
  * implementations can override this.
  */
-int __attribute__ ((weak)) pci_ext_cfg_avail(struct pci_dev *dev)
+int __weak pci_ext_cfg_avail(struct pci_dev *dev)
 {
 	return 1;
 }
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index e4943479b23..bacbcba69cf 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -67,9 +67,11 @@ struct pci_platform_pm_ops {
 
 extern int pci_set_platform_pm(struct pci_platform_pm_ops *ops);
 extern void pci_update_current_state(struct pci_dev *dev, pci_power_t state);
+extern void pci_power_up(struct pci_dev *dev);
 extern void pci_disable_enabled_device(struct pci_dev *dev);
 extern int pci_finish_runtime_suspend(struct pci_dev *dev);
 extern int __pci_pme_wakeup(struct pci_dev *dev, void *ign);
+extern void pci_wakeup_bus(struct pci_bus *bus);
 extern void pci_pm_init(struct pci_dev *dev);
 extern void platform_pci_wakeup_init(struct pci_dev *dev);
 extern void pci_allocate_cap_save_buffers(struct pci_dev *dev);
@@ -86,13 +88,6 @@ static inline bool pci_is_bridge(struct pci_dev *pci_dev)
 	return !!(pci_dev->subordinate);
 }
 
-extern int pci_user_read_config_byte(struct pci_dev *dev, int where, u8 *val);
-extern int pci_user_read_config_word(struct pci_dev *dev, int where, u16 *val);
-extern int pci_user_read_config_dword(struct pci_dev *dev, int where, u32 *val);
-extern int pci_user_write_config_byte(struct pci_dev *dev, int where, u8 val);
-extern int pci_user_write_config_word(struct pci_dev *dev, int where, u16 val);
-extern int pci_user_write_config_dword(struct pci_dev *dev, int where, u32 val);
-
 struct pci_vpd_ops {
 	ssize_t (*read)(struct pci_dev *dev, loff_t pos, size_t count, void *buf);
 	ssize_t (*write)(struct pci_dev *dev, loff_t pos, size_t count, const void *buf);
@@ -124,7 +119,7 @@ static inline int pci_proc_detach_bus(struct pci_bus *bus) { return 0; }
 #endif
 
 /* Functions for PCI Hotplug drivers to use */
-extern unsigned int pci_do_scan_bus(struct pci_bus *bus);
+int pci_hp_add_bridge(struct pci_dev *dev);
 
 #ifdef HAVE_PCI_LEGACY
 extern void pci_create_legacy_files(struct pci_bus *bus);
diff --git a/drivers/pci/pcie/aer/aerdrv_acpi.c b/drivers/pci/pcie/aer/aerdrv_acpi.c
index 275bf158ffa..124f20ff11b 100644
--- a/drivers/pci/pcie/aer/aerdrv_acpi.c
+++ b/drivers/pci/pcie/aer/aerdrv_acpi.c
@@ -59,7 +59,7 @@ static int aer_hest_parse(struct acpi_hest_header *hest_hdr, void *data)
 
 	p = (struct acpi_hest_aer_common *)(hest_hdr + 1);
 	if (p->flags & ACPI_HEST_GLOBAL) {
-		if ((info->pci_dev->is_pcie &&
+		if ((pci_is_pcie(info->pci_dev) &&
 		     info->pci_dev->pcie_type == pcie_type) || bridge)
 			ff = !!(p->flags & ACPI_HEST_FIRMWARE_FIRST);
 	} else
diff --git a/drivers/pci/pcie/portdrv_pci.c b/drivers/pci/pcie/portdrv_pci.c
index e0610bda1de..3a7eefcb270 100644
--- a/drivers/pci/pcie/portdrv_pci.c
+++ b/drivers/pci/pcie/portdrv_pci.c
@@ -11,6 +11,7 @@
 #include <linux/kernel.h>
 #include <linux/errno.h>
 #include <linux/pm.h>
+#include <linux/pm_runtime.h>
 #include <linux/init.h>
 #include <linux/pcieport_if.h>
 #include <linux/aer.h>
@@ -99,6 +100,51 @@ static int pcie_port_resume_noirq(struct device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_RUNTIME
+struct d3cold_info {
+	bool no_d3cold;
+	unsigned int d3cold_delay;
+};
+
+static int pci_dev_d3cold_info(struct pci_dev *pdev, void *data)
+{
+	struct d3cold_info *info = data;
+
+	info->d3cold_delay = max_t(unsigned int, pdev->d3cold_delay,
+				   info->d3cold_delay);
+	if (pdev->no_d3cold)
+		info->no_d3cold = true;
+	return 0;
+}
+
+static int pcie_port_runtime_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct d3cold_info d3cold_info = {
+		.no_d3cold	= false,
+		.d3cold_delay	= PCI_PM_D3_WAIT,
+	};
+
+	/*
+	 * If any subordinate device disable D3cold, we should not put
+	 * the port into D3cold.  The D3cold delay of port should be
+	 * the max of that of all subordinate devices.
+	 */
+	pci_walk_bus(pdev->subordinate, pci_dev_d3cold_info, &d3cold_info);
+	pdev->no_d3cold = d3cold_info.no_d3cold;
+	pdev->d3cold_delay = d3cold_info.d3cold_delay;
+	return 0;
+}
+
+static int pcie_port_runtime_resume(struct device *dev)
+{
+	return 0;
+}
+#else
+#define pcie_port_runtime_suspend	NULL
+#define pcie_port_runtime_resume	NULL
+#endif
+
 static const struct dev_pm_ops pcie_portdrv_pm_ops = {
 	.suspend	= pcie_port_device_suspend,
 	.resume		= pcie_port_device_resume,
@@ -107,6 +153,8 @@ static const struct dev_pm_ops pcie_portdrv_pm_ops = {
 	.poweroff	= pcie_port_device_suspend,
 	.restore	= pcie_port_device_resume,
 	.resume_noirq	= pcie_port_resume_noirq,
+	.runtime_suspend = pcie_port_runtime_suspend,
+	.runtime_resume = pcie_port_runtime_resume,
 };
 
 #define PCIE_PORTDRV_PM_OPS	(&pcie_portdrv_pm_ops)
@@ -117,6 +165,14 @@ static const struct dev_pm_ops pcie_portdrv_pm_ops = {
 #endif /* !PM */
 
 /*
+ * PCIe port runtime suspend is broken for some chipsets, so use a
+ * black list to disable runtime PM for these chipsets.
+ */
+static const struct pci_device_id port_runtime_pm_black_list[] = {
+	{ /* end: all zeroes */ }
+};
+
+/*
  * pcie_portdrv_probe - Probe PCI-Express port devices
  * @dev: PCI-Express port device being probed
  *
@@ -144,12 +200,16 @@ static int __devinit pcie_portdrv_probe(struct pci_dev *dev,
 		return status;
 
 	pci_save_state(dev);
+	if (!pci_match_id(port_runtime_pm_black_list, dev))
+		pm_runtime_put_noidle(&dev->dev);
 
 	return 0;
 }
 
 static void pcie_portdrv_remove(struct pci_dev *dev)
 {
+	if (!pci_match_id(port_runtime_pm_black_list, dev))
+		pm_runtime_get_noresume(&dev->dev);
 	pcie_port_device_remove(dev);
 	pci_disable_device(dev);
 }
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 658ac977cb5..6c143b4497c 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -16,10 +16,47 @@
 #define CARDBUS_LATENCY_TIMER	176	/* secondary latency timer */
 #define CARDBUS_RESERVE_BUSNR	3
 
+struct resource busn_resource = {
+	.name	= "PCI busn",
+	.start	= 0,
+	.end	= 255,
+	.flags	= IORESOURCE_BUS,
+};
+
 /* Ugh.  Need to stop exporting this to modules. */
 LIST_HEAD(pci_root_buses);
 EXPORT_SYMBOL(pci_root_buses);
 
+static LIST_HEAD(pci_domain_busn_res_list);
+
+struct pci_domain_busn_res {
+	struct list_head list;
+	struct resource res;
+	int domain_nr;
+};
+
+static struct resource *get_pci_domain_busn_res(int domain_nr)
+{
+	struct pci_domain_busn_res *r;
+
+	list_for_each_entry(r, &pci_domain_busn_res_list, list)
+		if (r->domain_nr == domain_nr)
+			return &r->res;
+
+	r = kzalloc(sizeof(*r), GFP_KERNEL);
+	if (!r)
+		return NULL;
+
+	r->domain_nr = domain_nr;
+	r->res.start = 0;
+	r->res.end = 0xff;
+	r->res.flags = IORESOURCE_BUS | IORESOURCE_PCI_FIXED;
+
+	list_add_tail(&r->list, &pci_domain_busn_res_list);
+
+	return &r->res;
+}
+
 static int find_anything(struct device *dev, void *data)
 {
 	return 1;
@@ -152,9 +189,6 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	pci_read_config_dword(dev, pos, &sz);
 	pci_write_config_dword(dev, pos, l);
 
-	if (!dev->mmio_always_on)
-		pci_write_config_word(dev, PCI_COMMAND, orig_cmd);
-
 	/*
 	 * All bits set in sz means the device isn't working properly.
 	 * If the BAR isn't implemented, all bits must be 0.  If it's a
@@ -239,6 +273,9 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	}
 
  out:
+	if (!dev->mmio_always_on)
+		pci_write_config_word(dev, PCI_COMMAND, orig_cmd);
+
 	return (res->flags & IORESOURCE_MEM_64) ? 1 : 0;
  fail:
 	res->flags = 0;
@@ -269,34 +306,38 @@ static void __devinit pci_read_bridge_io(struct pci_bus *child)
 {
 	struct pci_dev *dev = child->self;
 	u8 io_base_lo, io_limit_lo;
-	unsigned long base, limit;
+	unsigned long io_mask, io_granularity, base, limit;
 	struct pci_bus_region region;
-	struct resource *res, res2;
+	struct resource *res;
+
+	io_mask = PCI_IO_RANGE_MASK;
+	io_granularity = 0x1000;
+	if (dev->io_window_1k) {
+		/* Support 1K I/O space granularity */
+		io_mask = PCI_IO_1K_RANGE_MASK;
+		io_granularity = 0x400;
+	}
 
 	res = child->resource[0];
 	pci_read_config_byte(dev, PCI_IO_BASE, &io_base_lo);
 	pci_read_config_byte(dev, PCI_IO_LIMIT, &io_limit_lo);
-	base = (io_base_lo & PCI_IO_RANGE_MASK) << 8;
-	limit = (io_limit_lo & PCI_IO_RANGE_MASK) << 8;
+	base = (io_base_lo & io_mask) << 8;
+	limit = (io_limit_lo & io_mask) << 8;
 
 	if ((io_base_lo & PCI_IO_RANGE_TYPE_MASK) == PCI_IO_RANGE_TYPE_32) {
 		u16 io_base_hi, io_limit_hi;
+
 		pci_read_config_word(dev, PCI_IO_BASE_UPPER16, &io_base_hi);
 		pci_read_config_word(dev, PCI_IO_LIMIT_UPPER16, &io_limit_hi);
-		base |= (io_base_hi << 16);
-		limit |= (io_limit_hi << 16);
+		base |= ((unsigned long) io_base_hi << 16);
+		limit |= ((unsigned long) io_limit_hi << 16);
 	}
 
-	if (base && base <= limit) {
+	if (base <= limit) {
 		res->flags = (io_base_lo & PCI_IO_RANGE_TYPE_MASK) | IORESOURCE_IO;
-		res2.flags = res->flags;
 		region.start = base;
-		region.end = limit + 0xfff;
-		pcibios_bus_to_resource(dev, &res2, &region);
-		if (!res->start)
-			res->start = res2.start;
-		if (!res->end)
-			res->end = res2.end;
+		region.end = limit + io_granularity - 1;
+		pcibios_bus_to_resource(dev, res, &region);
 		dev_printk(KERN_DEBUG, &dev->dev, "  bridge window %pR\n", res);
 	}
 }
@@ -312,9 +353,9 @@ static void __devinit pci_read_bridge_mmio(struct pci_bus *child)
 	res = child->resource[1];
 	pci_read_config_word(dev, PCI_MEMORY_BASE, &mem_base_lo);
 	pci_read_config_word(dev, PCI_MEMORY_LIMIT, &mem_limit_lo);
-	base = (mem_base_lo & PCI_MEMORY_RANGE_MASK) << 16;
-	limit = (mem_limit_lo & PCI_MEMORY_RANGE_MASK) << 16;
-	if (base && base <= limit) {
+	base = ((unsigned long) mem_base_lo & PCI_MEMORY_RANGE_MASK) << 16;
+	limit = ((unsigned long) mem_limit_lo & PCI_MEMORY_RANGE_MASK) << 16;
+	if (base <= limit) {
 		res->flags = (mem_base_lo & PCI_MEMORY_RANGE_TYPE_MASK) | IORESOURCE_MEM;
 		region.start = base;
 		region.end = limit + 0xfffff;
@@ -334,11 +375,12 @@ static void __devinit pci_read_bridge_mmio_pref(struct pci_bus *child)
 	res = child->resource[2];
 	pci_read_config_word(dev, PCI_PREF_MEMORY_BASE, &mem_base_lo);
 	pci_read_config_word(dev, PCI_PREF_MEMORY_LIMIT, &mem_limit_lo);
-	base = (mem_base_lo & PCI_PREF_RANGE_MASK) << 16;
-	limit = (mem_limit_lo & PCI_PREF_RANGE_MASK) << 16;
+	base = ((unsigned long) mem_base_lo & PCI_PREF_RANGE_MASK) << 16;
+	limit = ((unsigned long) mem_limit_lo & PCI_PREF_RANGE_MASK) << 16;
 
 	if ((mem_base_lo & PCI_PREF_RANGE_TYPE_MASK) == PCI_PREF_RANGE_TYPE_64) {
 		u32 mem_base_hi, mem_limit_hi;
+
 		pci_read_config_dword(dev, PCI_PREF_BASE_UPPER32, &mem_base_hi);
 		pci_read_config_dword(dev, PCI_PREF_LIMIT_UPPER32, &mem_limit_hi);
 
@@ -349,8 +391,8 @@ static void __devinit pci_read_bridge_mmio_pref(struct pci_bus *child)
 		 */
 		if (mem_base_hi <= mem_limit_hi) {
 #if BITS_PER_LONG == 64
-			base |= ((long) mem_base_hi) << 32;
-			limit |= ((long) mem_limit_hi) << 32;
+			base |= ((unsigned long) mem_base_hi) << 32;
+			limit |= ((unsigned long) mem_limit_hi) << 32;
 #else
 			if (mem_base_hi || mem_limit_hi) {
 				dev_err(&dev->dev, "can't handle 64-bit "
@@ -360,7 +402,7 @@ static void __devinit pci_read_bridge_mmio_pref(struct pci_bus *child)
 #endif
 		}
 	}
-	if (base && base <= limit) {
+	if (base <= limit) {
 		res->flags = (mem_base_lo & PCI_PREF_RANGE_TYPE_MASK) |
 					 IORESOURCE_MEM | IORESOURCE_PREFETCH;
 		if (res->flags & PCI_PREF_RANGE_TYPE_64)
@@ -381,8 +423,8 @@ void __devinit pci_read_bridge_bases(struct pci_bus *child)
 	if (pci_is_root_bus(child))	/* It's a host bus, nothing to read */
 		return;
 
-	dev_info(&dev->dev, "PCI bridge to [bus %02x-%02x]%s\n",
-		 child->secondary, child->subordinate,
+	dev_info(&dev->dev, "PCI bridge to %pR%s\n",
+		 &child->busn_res,
 		 dev->transparent ? " (subtractive decode)" : "");
 
 	pci_bus_remove_resources(child);
@@ -599,9 +641,9 @@ static struct pci_bus *pci_alloc_child_bus(struct pci_bus *parent,
 	 * Set up the primary, secondary and subordinate
 	 * bus numbers.
 	 */
-	child->number = child->secondary = busnr;
-	child->primary = parent->secondary;
-	child->subordinate = 0xff;
+	child->number = child->busn_res.start = busnr;
+	child->primary = parent->busn_res.start;
+	child->busn_res.end = 0xff;
 
 	if (!bridge)
 		return child;
@@ -643,8 +685,8 @@ static void pci_fixup_parent_subordinate_busnr(struct pci_bus *child, int max)
 	if (!pcibios_assign_all_busses())
 		return;
 
-	while (parent->parent && parent->subordinate < max) {
-		parent->subordinate = max;
+	while (parent->parent && parent->busn_res.end < max) {
+		parent->busn_res.end = max;
 		pci_write_config_byte(parent->self, PCI_SUBORDINATE_BUS, max);
 		parent = parent->parent;
 	}
@@ -718,15 +760,15 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 			if (!child)
 				goto out;
 			child->primary = primary;
-			child->subordinate = subordinate;
+			pci_bus_insert_busn_res(child, secondary, subordinate);
 			child->bridge_ctl = bctl;
 		}
 
 		cmax = pci_scan_child_bus(child);
 		if (cmax > max)
 			max = cmax;
-		if (child->subordinate > max)
-			max = child->subordinate;
+		if (child->busn_res.end > max)
+			max = child->busn_res.end;
 	} else {
 		/*
 		 * We need to assign a number to this bus which we always
@@ -756,11 +798,12 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 			child = pci_add_new_bus(bus, dev, ++max);
 			if (!child)
 				goto out;
+			pci_bus_insert_busn_res(child, max, 0xff);
 		}
 		buses = (buses & 0xff000000)
 		      | ((unsigned int)(child->primary)     <<  0)
-		      | ((unsigned int)(child->secondary)   <<  8)
-		      | ((unsigned int)(child->subordinate) << 16);
+		      | ((unsigned int)(child->busn_res.start)   <<  8)
+		      | ((unsigned int)(child->busn_res.end) << 16);
 
 		/*
 		 * yenta.c forces a secondary latency timer of 176.
@@ -805,8 +848,8 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 					break;
 				while (parent->parent) {
 					if ((!pcibios_assign_all_busses()) &&
-					    (parent->subordinate > max) &&
-					    (parent->subordinate <= max+i)) {
+					    (parent->busn_res.end > max) &&
+					    (parent->busn_res.end <= max+i)) {
 						j = 1;
 					}
 					parent = parent->parent;
@@ -827,7 +870,7 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 		/*
 		 * Set the subordinate bus number to its real value.
 		 */
-		child->subordinate = max;
+		pci_bus_update_busn_res_end(child, max);
 		pci_write_config_byte(dev, PCI_SUBORDINATE_BUS, max);
 	}
 
@@ -837,19 +880,19 @@ int __devinit pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max,
 
 	/* Has only triggered on CardBus, fixup is in yenta_socket */
 	while (bus->parent) {
-		if ((child->subordinate > bus->subordinate) ||
-		    (child->number > bus->subordinate) ||
+		if ((child->busn_res.end > bus->busn_res.end) ||
+		    (child->number > bus->busn_res.end) ||
 		    (child->number < bus->number) ||
-		    (child->subordinate < bus->number)) {
-			dev_info(&child->dev, "[bus %02x-%02x] %s "
-				"hidden behind%s bridge %s [bus %02x-%02x]\n",
-				child->number, child->subordinate,
-				(bus->number > child->subordinate &&
-				 bus->subordinate < child->number) ?
+		    (child->busn_res.end < bus->number)) {
+			dev_info(&child->dev, "%pR %s "
+				"hidden behind%s bridge %s %pR\n",
+				&child->busn_res,
+				(bus->number > child->busn_res.end &&
+				 bus->busn_res.end < child->number) ?
 					"wholly" : "partially",
 				bus->self->transparent ? " transparent" : "",
 				dev_name(&bus->dev),
-				bus->number, bus->subordinate);
+				&bus->busn_res);
 		}
 		bus = bus->parent;
 	}
@@ -1548,7 +1591,7 @@ EXPORT_SYMBOL_GPL(pcie_bus_configure_settings);
 
 unsigned int __devinit pci_scan_child_bus(struct pci_bus *bus)
 {
-	unsigned int devfn, pass, max = bus->secondary;
+	unsigned int devfn, pass, max = bus->busn_res.start;
 	struct pci_dev *dev;
 
 	dev_dbg(&bus->dev, "scanning bus\n");
@@ -1642,7 +1685,7 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 	/* Create legacy_io and legacy_mem files for this bus */
 	pci_create_legacy_files(b);
 
-	b->number = b->secondary = bus;
+	b->number = b->busn_res.start = bus;
 
 	if (parent)
 		dev_info(parent, "PCI host bridge to bus %s\n", dev_name(&b->dev));
@@ -1654,7 +1697,10 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 		list_move_tail(&window->list, &bridge->windows);
 		res = window->res;
 		offset = window->offset;
-		pci_bus_add_resource(b, res, 0);
+		if (res->flags & IORESOURCE_BUS)
+			pci_bus_insert_busn_res(b, bus, res->end);
+		else
+			pci_bus_add_resource(b, res, 0);
 		if (offset) {
 			if (resource_type(res) == IORESOURCE_IO)
 				fmt = " (bus address [%#06llx-%#06llx])";
@@ -1684,16 +1730,104 @@ err_out:
 	return NULL;
 }
 
+int pci_bus_insert_busn_res(struct pci_bus *b, int bus, int bus_max)
+{
+	struct resource *res = &b->busn_res;
+	struct resource *parent_res, *conflict;
+
+	res->start = bus;
+	res->end = bus_max;
+	res->flags = IORESOURCE_BUS;
+
+	if (!pci_is_root_bus(b))
+		parent_res = &b->parent->busn_res;
+	else {
+		parent_res = get_pci_domain_busn_res(pci_domain_nr(b));
+		res->flags |= IORESOURCE_PCI_FIXED;
+	}
+
+	conflict = insert_resource_conflict(parent_res, res);
+
+	if (conflict)
+		dev_printk(KERN_DEBUG, &b->dev,
+			   "busn_res: can not insert %pR under %s%pR (conflicts with %s %pR)\n",
+			    res, pci_is_root_bus(b) ? "domain " : "",
+			    parent_res, conflict->name, conflict);
+	else
+		dev_printk(KERN_DEBUG, &b->dev,
+			   "busn_res: %pR is inserted under %s%pR\n",
+			   res, pci_is_root_bus(b) ? "domain " : "",
+			   parent_res);
+
+	return conflict == NULL;
+}
+
+int pci_bus_update_busn_res_end(struct pci_bus *b, int bus_max)
+{
+	struct resource *res = &b->busn_res;
+	struct resource old_res = *res;
+	resource_size_t size;
+	int ret;
+
+	if (res->start > bus_max)
+		return -EINVAL;
+
+	size = bus_max - res->start + 1;
+	ret = adjust_resource(res, res->start, size);
+	dev_printk(KERN_DEBUG, &b->dev,
+			"busn_res: %pR end %s updated to %02x\n",
+			&old_res, ret ? "can not be" : "is", bus_max);
+
+	if (!ret && !res->parent)
+		pci_bus_insert_busn_res(b, res->start, res->end);
+
+	return ret;
+}
+
+void pci_bus_release_busn_res(struct pci_bus *b)
+{
+	struct resource *res = &b->busn_res;
+	int ret;
+
+	if (!res->flags || !res->parent)
+		return;
+
+	ret = release_resource(res);
+	dev_printk(KERN_DEBUG, &b->dev,
+			"busn_res: %pR %s released\n",
+			res, ret ? "can not be" : "is");
+}
+
 struct pci_bus * __devinit pci_scan_root_bus(struct device *parent, int bus,
 		struct pci_ops *ops, void *sysdata, struct list_head *resources)
 {
+	struct pci_host_bridge_window *window;
+	bool found = false;
 	struct pci_bus *b;
+	int max;
+
+	list_for_each_entry(window, resources, list)
+		if (window->res->flags & IORESOURCE_BUS) {
+			found = true;
+			break;
+		}
 
 	b = pci_create_root_bus(parent, bus, ops, sysdata, resources);
 	if (!b)
 		return NULL;
 
-	b->subordinate = pci_scan_child_bus(b);
+	if (!found) {
+		dev_info(&b->dev,
+		 "No busn resource found for root bus, will use [bus %02x-ff]\n",
+			bus);
+		pci_bus_insert_busn_res(b, bus, 255);
+	}
+
+	max = pci_scan_child_bus(b);
+
+	if (!found)
+		pci_bus_update_busn_res_end(b, max);
+
 	pci_bus_add_devices(b);
 	return b;
 }
@@ -1708,9 +1842,10 @@ struct pci_bus * __devinit pci_scan_bus_parented(struct device *parent,
 
 	pci_add_resource(&resources, &ioport_resource);
 	pci_add_resource(&resources, &iomem_resource);
+	pci_add_resource(&resources, &busn_resource);
 	b = pci_create_root_bus(parent, bus, ops, sysdata, &resources);
 	if (b)
-		b->subordinate = pci_scan_child_bus(b);
+		pci_scan_child_bus(b);
 	else
 		pci_free_resource_list(&resources);
 	return b;
@@ -1725,9 +1860,10 @@ struct pci_bus * __devinit pci_scan_bus(int bus, struct pci_ops *ops,
 
 	pci_add_resource(&resources, &ioport_resource);
 	pci_add_resource(&resources, &iomem_resource);
+	pci_add_resource(&resources, &busn_resource);
 	b = pci_create_root_bus(NULL, bus, ops, sysdata, &resources);
 	if (b) {
-		b->subordinate = pci_scan_child_bus(b);
+		pci_scan_child_bus(b);
 		pci_bus_add_devices(b);
 	} else {
 		pci_free_resource_list(&resources);
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 2a752167754..51553179e96 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -253,7 +253,7 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_VIA,	PCI_DEVICE_ID_VIA_82C576,	quirk_vsfx)
  *	workaround applied too
  *	[Info kindly provided by ALi]
  */	
-static void __init quirk_alimagik(struct pci_dev *dev)
+static void __devinit quirk_alimagik(struct pci_dev *dev)
 {
 	if ((pci_pci_problems&PCIPCI_ALIMAGIK)==0) {
 		dev_info(&dev->dev, "Limiting direct PCI/PCI transfers\n");
@@ -789,7 +789,7 @@ static void __devinit quirk_amd_ioapic(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_AMD,	PCI_DEVICE_ID_AMD_VIPER_7410,	quirk_amd_ioapic);
 
-static void __init quirk_ioapic_rmw(struct pci_dev *dev)
+static void __devinit quirk_ioapic_rmw(struct pci_dev *dev)
 {
 	if (dev->devfn == 0 && dev->bus->number == 0)
 		sis_apic_bug = 1;
@@ -801,7 +801,7 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_SI,	PCI_ANY_ID,			quirk_ioapic_rmw);
  * Some settings of MMRBC can lead to data corruption so block changes.
  * See AMD 8131 HyperTransport PCI-X Tunnel Revision Guide
  */
-static void __init quirk_amd_8131_mmrbc(struct pci_dev *dev)
+static void __devinit quirk_amd_8131_mmrbc(struct pci_dev *dev)
 {
 	if (dev->subordinate && dev->revision <= 0x12) {
 		dev_info(&dev->dev, "AMD8131 rev %x detected; "
@@ -1039,7 +1039,7 @@ static void quirk_disable_pxb(struct pci_dev *pdev)
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82454NX,	quirk_disable_pxb);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82454NX,	quirk_disable_pxb);
 
-static void __devinit quirk_amd_ide_mode(struct pci_dev *pdev)
+static void quirk_amd_ide_mode(struct pci_dev *pdev)
 {
 	/* set SBX00/Hudson-2 SATA in IDE mode to AHCI mode */
 	u8 tmp;
@@ -1082,7 +1082,7 @@ DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_SERVERWORKS, PCI_DEVICE_ID_SERVERWORKS_CSB
 /*
  *	Intel 82801CAM ICH3-M datasheet says IDE modes must be the same
  */
-static void __init quirk_ide_samemode(struct pci_dev *pdev)
+static void __devinit quirk_ide_samemode(struct pci_dev *pdev)
 {
 	u8 prog;
 
@@ -1121,7 +1121,7 @@ DECLARE_PCI_FIXUP_CLASS_EARLY(PCI_VENDOR_ID_VIA, PCI_ANY_ID,
 /* This was originally an Alpha specific thing, but it really fits here.
  * The i82375 PCI/EISA bridge appears as non-classified. Fix that.
  */
-static void __init quirk_eisa_bridge(struct pci_dev *dev)
+static void __devinit quirk_eisa_bridge(struct pci_dev *dev)
 {
 	dev->class = PCI_CLASS_BRIDGE_EISA << 8;
 }
@@ -1155,7 +1155,7 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_82375,	quirk_e
  */
 static int asus_hides_smbus;
 
-static void __init asus_hides_smbus_hostbridge(struct pci_dev *dev)
+static void __devinit asus_hides_smbus_hostbridge(struct pci_dev *dev)
 {
 	if (unlikely(dev->subsystem_vendor == PCI_VENDOR_ID_ASUSTEK)) {
 		if (dev->device == PCI_DEVICE_ID_INTEL_82845_HB)
@@ -1538,7 +1538,7 @@ DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB3
 #endif
 
 #ifdef CONFIG_X86_IO_APIC
-static void __init quirk_alder_ioapic(struct pci_dev *pdev)
+static void __devinit quirk_alder_ioapic(struct pci_dev *pdev)
 {
 	int i;
 
@@ -1777,7 +1777,7 @@ DECLARE_PCI_FIXUP_RESUME(PCI_VENDOR_ID_AMD,   PCI_DEVICE_ID_AMD_8111_SMBUS, 	qui
  * but the PIO transfers won't work if BAR0 falls at the odd 8 bytes.
  * Re-allocate the region if needed...
  */
-static void __init quirk_tc86c001_ide(struct pci_dev *dev)
+static void __devinit quirk_tc86c001_ide(struct pci_dev *dev)
 {
 	struct resource *r = &dev->resource[0];
 
@@ -1938,53 +1938,16 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_NCR, PCI_DEVICE_ID_NCR_53C810, fixup_rev1
 static void __devinit quirk_p64h2_1k_io(struct pci_dev *dev)
 {
 	u16 en1k;
-	u8 io_base_lo, io_limit_lo;
-	unsigned long base, limit;
-	struct resource *res = dev->resource + PCI_BRIDGE_RESOURCES;
 
 	pci_read_config_word(dev, 0x40, &en1k);
 
 	if (en1k & 0x200) {
 		dev_info(&dev->dev, "Enable I/O Space to 1KB granularity\n");
-
-		pci_read_config_byte(dev, PCI_IO_BASE, &io_base_lo);
-		pci_read_config_byte(dev, PCI_IO_LIMIT, &io_limit_lo);
-		base = (io_base_lo & (PCI_IO_RANGE_MASK | 0x0c)) << 8;
-		limit = (io_limit_lo & (PCI_IO_RANGE_MASK | 0x0c)) << 8;
-
-		if (base <= limit) {
-			res->start = base;
-			res->end = limit + 0x3ff;
-		}
+		dev->io_window_1k = 1;
 	}
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	0x1460,		quirk_p64h2_1k_io);
 
-/* Fix the IOBL_ADR for 1k I/O space granularity on the Intel P64H2
- * The IOBL_ADR gets re-written to 4k boundaries in pci_setup_bridge()
- * in drivers/pci/setup-bus.c
- */
-static void __devinit quirk_p64h2_1k_io_fix_iobl(struct pci_dev *dev)
-{
-	u16 en1k, iobl_adr, iobl_adr_1k;
-	struct resource *res = dev->resource + PCI_BRIDGE_RESOURCES;
-
-	pci_read_config_word(dev, 0x40, &en1k);
-
-	if (en1k & 0x200) {
-		pci_read_config_word(dev, PCI_IO_BASE, &iobl_adr);
-
-		iobl_adr_1k = iobl_adr | (res->start >> 8) | (res->end & 0xfc00);
-
-		if (iobl_adr != iobl_adr_1k) {
-			dev_info(&dev->dev, "Fixing P64H2 IOBL_ADR from 0x%x to 0x%x for 1KB granularity\n",
-				iobl_adr,iobl_adr_1k);
-			pci_write_config_word(dev, PCI_IO_BASE, iobl_adr_1k);
-		}
-	}
-}
-DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL,	0x1460,		quirk_p64h2_1k_io_fix_iobl);
-
 /* Under some circumstances, AER is not linked with extended capabilities.
  * Force it to be linked by setting the corresponding control bit in the
  * config space.
@@ -2104,7 +2067,7 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_BROADCOM,
 			PCI_DEVICE_ID_NX2_5709S,
 			quirk_brcm_570x_limit_vpd);
 
-static void __devinit quirk_brcm_5719_limit_mrrs(struct pci_dev *dev)
+static void quirk_brcm_5719_limit_mrrs(struct pci_dev *dev)
 {
 	u32 rev;
 
@@ -2143,9 +2106,9 @@ DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82865_HB,
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82875_HB,
 			quirk_unhide_mch_dev6);
 
-#ifdef CONFIG_TILE
+#ifdef CONFIG_TILEPRO
 /*
- * The Tilera TILEmpower platform needs to set the link speed
+ * The Tilera TILEmpower tilepro platform needs to set the link speed
  * to 2.5GT(Giga-Transfers)/s (Gen 1). The default link speed
  * setting is 5GT/s (Gen 2). 0x98 is the Link Control2 PCIe
  * capability register of the PEX8624 PCIe switch. The switch
@@ -2160,7 +2123,7 @@ static void __devinit quirk_tile_plx_gen1(struct pci_dev *dev)
 	}
 }
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_PLX, 0x8624, quirk_tile_plx_gen1);
-#endif /* CONFIG_TILE */
+#endif /* CONFIG_TILEPRO */
 
 #ifdef CONFIG_PCI_MSI
 /* Some chipsets do not support MSI. We cannot easily rely on setting
@@ -2169,7 +2132,7 @@ DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_PLX, 0x8624, quirk_tile_plx_gen1);
  * aware of it.  Instead of setting the flag on all busses in the
  * machine, simply disable MSI globally.
  */
-static void __init quirk_disable_all_msi(struct pci_dev *dev)
+static void __devinit quirk_disable_all_msi(struct pci_dev *dev)
 {
 	pci_no_msi();
 	dev_warn(&dev->dev, "MSI quirk detected; MSI disabled\n");
@@ -2217,7 +2180,7 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_AMD, 0x9601, quirk_amd_780_apc_msi);
 
 /* Go through the list of Hypertransport capabilities and
  * return 1 if a HT MSI capability is found and enabled */
-static int __devinit msi_ht_cap_enabled(struct pci_dev *dev)
+static int msi_ht_cap_enabled(struct pci_dev *dev)
 {
 	int pos, ttl = 48;
 
@@ -2241,7 +2204,7 @@ static int __devinit msi_ht_cap_enabled(struct pci_dev *dev)
 }
 
 /* Check the hypertransport MSI mapping to know whether MSI is enabled or not */
-static void __devinit quirk_msi_ht_cap(struct pci_dev *dev)
+static void quirk_msi_ht_cap(struct pci_dev *dev)
 {
 	if (dev->subordinate && !msi_ht_cap_enabled(dev)) {
 		dev_warn(&dev->dev, "MSI quirk detected; "
@@ -2255,7 +2218,7 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_SERVERWORKS, PCI_DEVICE_ID_SERVERWORKS_HT2
 /* The nVidia CK804 chipset may have 2 HT MSI mappings.
  * MSI are supported if the MSI capability set in any of these mappings.
  */
-static void __devinit quirk_nvidia_ck804_msi_ht_cap(struct pci_dev *dev)
+static void quirk_nvidia_ck804_msi_ht_cap(struct pci_dev *dev)
 {
 	struct pci_dev *pdev;
 
@@ -2279,7 +2242,7 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_NVIDIA, PCI_DEVICE_ID_NVIDIA_CK804_PCIE,
 			quirk_nvidia_ck804_msi_ht_cap);
 
 /* Force enable MSI mapping capability on HT bridges */
-static void __devinit ht_enable_msi_mapping(struct pci_dev *dev)
+static void ht_enable_msi_mapping(struct pci_dev *dev)
 {
 	int pos, ttl = 48;
 
@@ -2359,7 +2322,7 @@ DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_NVIDIA,
 			PCI_DEVICE_ID_NVIDIA_MCP55_BRIDGE_V4,
 			nvbridge_check_legacy_irq_routing);
 
-static int __devinit ht_check_msi_mapping(struct pci_dev *dev)
+static int ht_check_msi_mapping(struct pci_dev *dev)
 {
 	int pos, ttl = 48;
 	int found = 0;
@@ -2387,7 +2350,7 @@ static int __devinit ht_check_msi_mapping(struct pci_dev *dev)
 	return found;
 }
 
-static int __devinit host_bridge_with_leaf(struct pci_dev *host_bridge)
+static int host_bridge_with_leaf(struct pci_dev *host_bridge)
 {
 	struct pci_dev *dev;
 	int pos;
@@ -2421,7 +2384,7 @@ static int __devinit host_bridge_with_leaf(struct pci_dev *host_bridge)
 #define PCI_HT_CAP_SLAVE_CTRL0     4    /* link control */
 #define PCI_HT_CAP_SLAVE_CTRL1     8    /* link control to */
 
-static int __devinit is_end_of_ht_chain(struct pci_dev *dev)
+static int is_end_of_ht_chain(struct pci_dev *dev)
 {
 	int pos, ctrl_off;
 	int end = 0;
@@ -2445,7 +2408,7 @@ out:
 	return end;
 }
 
-static void __devinit nv_ht_enable_msi_mapping(struct pci_dev *dev)
+static void nv_ht_enable_msi_mapping(struct pci_dev *dev)
 {
 	struct pci_dev *host_bridge;
 	int pos;
@@ -2484,7 +2447,7 @@ out:
 	pci_dev_put(host_bridge);
 }
 
-static void __devinit ht_disable_msi_mapping(struct pci_dev *dev)
+static void ht_disable_msi_mapping(struct pci_dev *dev)
 {
 	int pos, ttl = 48;
 
@@ -2504,7 +2467,7 @@ static void __devinit ht_disable_msi_mapping(struct pci_dev *dev)
 	}
 }
 
-static void __devinit __nv_msi_ht_cap_quirk(struct pci_dev *dev, int all)
+static void __nv_msi_ht_cap_quirk(struct pci_dev *dev, int all)
 {
 	struct pci_dev *host_bridge;
 	int pos;
@@ -2541,23 +2504,26 @@ static void __devinit __nv_msi_ht_cap_quirk(struct pci_dev *dev, int all)
 			else
 				nv_ht_enable_msi_mapping(dev);
 		}
-		return;
+		goto out;
 	}
 
 	/* HT MSI is not enabled */
 	if (found == 1)
-		return;
+		goto out;
 
 	/* Host bridge is not to HT, disable HT MSI mapping on this device */
 	ht_disable_msi_mapping(dev);
+
+out:
+	pci_dev_put(host_bridge);
 }
 
-static void __devinit nv_msi_ht_cap_quirk_all(struct pci_dev *dev)
+static void nv_msi_ht_cap_quirk_all(struct pci_dev *dev)
 {
 	return __nv_msi_ht_cap_quirk(dev, 1);
 }
 
-static void __devinit nv_msi_ht_cap_quirk_leaf(struct pci_dev *dev)
+static void nv_msi_ht_cap_quirk_leaf(struct pci_dev *dev)
 {
 	return __nv_msi_ht_cap_quirk(dev, 0);
 }
@@ -2879,20 +2845,34 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x65f9, quirk_intel_mc_errata);
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x65fa, quirk_intel_mc_errata);
 
 
-static void do_one_fixup_debug(void (*fn)(struct pci_dev *dev), struct pci_dev *dev)
+static ktime_t fixup_debug_start(struct pci_dev *dev,
+				 void (*fn)(struct pci_dev *dev))
+{
+	ktime_t calltime = ktime_set(0, 0);
+
+	dev_dbg(&dev->dev, "calling %pF\n", fn);
+	if (initcall_debug) {
+		pr_debug("calling  %pF @ %i for %s\n",
+			 fn, task_pid_nr(current), dev_name(&dev->dev));
+		calltime = ktime_get();
+	}
+
+	return calltime;
+}
+
+static void fixup_debug_report(struct pci_dev *dev, ktime_t calltime,
+			       void (*fn)(struct pci_dev *dev))
 {
-	ktime_t calltime, delta, rettime;
+	ktime_t delta, rettime;
 	unsigned long long duration;
 
-	printk(KERN_DEBUG "calling  %pF @ %i for %s\n",
-			fn, task_pid_nr(current), dev_name(&dev->dev));
-	calltime = ktime_get();
-	fn(dev);
-	rettime = ktime_get();
-	delta = ktime_sub(rettime, calltime);
-	duration = (unsigned long long) ktime_to_ns(delta) >> 10;
-	printk(KERN_DEBUG "pci fixup %pF returned after %lld usecs for %s\n",
-			fn, duration, dev_name(&dev->dev));
+	if (initcall_debug) {
+		rettime = ktime_get();
+		delta = ktime_sub(rettime, calltime);
+		duration = (unsigned long long) ktime_to_ns(delta) >> 10;
+		pr_debug("pci fixup %pF returned after %lld usecs for %s\n",
+			 fn, duration, dev_name(&dev->dev));
+	}
 }
 
 /*
@@ -2929,9 +2909,25 @@ static void __devinit disable_igfx_irq(struct pci_dev *dev)
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0102, disable_igfx_irq);
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x010a, disable_igfx_irq);
 
+/*
+ * Some devices may pass our check in pci_intx_mask_supported if
+ * PCI_COMMAND_INTX_DISABLE works though they actually do not properly
+ * support this feature.
+ */
+static void __devinit quirk_broken_intx_masking(struct pci_dev *dev)
+{
+	dev->broken_intx_masking = 1;
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_CHELSIO, 0x0030,
+			 quirk_broken_intx_masking);
+DECLARE_PCI_FIXUP_HEADER(0x1814, 0x0601, /* Ralink RT2800 802.11n PCI */
+			 quirk_broken_intx_masking);
+
 static void pci_do_fixups(struct pci_dev *dev, struct pci_fixup *f,
 			  struct pci_fixup *end)
 {
+	ktime_t calltime;
+
 	for (; f < end; f++)
 		if ((f->class == (u32) (dev->class >> f->class_shift) ||
 		     f->class == (u32) PCI_ANY_ID) &&
@@ -2939,11 +2935,9 @@ static void pci_do_fixups(struct pci_dev *dev, struct pci_fixup *f,
 		     f->vendor == (u16) PCI_ANY_ID) &&
 		    (f->device == dev->device ||
 		     f->device == (u16) PCI_ANY_ID)) {
-			dev_dbg(&dev->dev, "calling %pF\n", f->hook);
-			if (initcall_debug)
-				do_one_fixup_debug(f->hook, dev);
-			else
-				f->hook(dev);
+			calltime = fixup_debug_start(dev, f->hook);
+			f->hook(dev);
+			fixup_debug_report(dev, calltime, f->hook);
 		}
 }
 
@@ -2962,6 +2956,7 @@ extern struct pci_fixup __end_pci_fixups_resume_early[];
 extern struct pci_fixup __start_pci_fixups_suspend[];
 extern struct pci_fixup __end_pci_fixups_suspend[];
 
+static bool pci_apply_fixup_final_quirks;
 
 void pci_fixup_device(enum pci_fixup_pass pass, struct pci_dev *dev)
 {
@@ -2979,6 +2974,8 @@ void pci_fixup_device(enum pci_fixup_pass pass, struct pci_dev *dev)
 		break;
 
 	case pci_fixup_final:
+		if (!pci_apply_fixup_final_quirks)
+			return;
 		start = __start_pci_fixups_final;
 		end = __end_pci_fixups_final;
 		break;
@@ -3011,6 +3008,7 @@ void pci_fixup_device(enum pci_fixup_pass pass, struct pci_dev *dev)
 }
 EXPORT_SYMBOL(pci_fixup_device);
 
+
 static int __init pci_apply_final_quirks(void)
 {
 	struct pci_dev *dev = NULL;
@@ -3021,6 +3019,7 @@ static int __init pci_apply_final_quirks(void)
 		printk(KERN_DEBUG "PCI: CLS %u bytes\n",
 		       pci_cache_line_size << 2);
 
+	pci_apply_fixup_final_quirks = true;
 	for_each_pci_dev(dev) {
 		pci_fixup_device(pci_fixup_final, dev);
 		/*
@@ -3041,6 +3040,7 @@ static int __init pci_apply_final_quirks(void)
 			pci_cache_line_size = pci_dfl_cache_line_size;
 		}
 	}
+
 	if (!pci_cache_line_size) {
 		printk(KERN_DEBUG "PCI: CLS %u bytes, default %u\n",
 		       cls << 2, pci_dfl_cache_line_size << 2);
@@ -3179,3 +3179,87 @@ int pci_dev_specific_reset(struct pci_dev *dev, int probe)
 
 	return -ENOTTY;
 }
+
+static struct pci_dev *pci_func_0_dma_source(struct pci_dev *dev)
+{
+	if (!PCI_FUNC(dev->devfn))
+		return pci_dev_get(dev);
+
+	return pci_get_slot(dev->bus, PCI_DEVFN(PCI_SLOT(dev->devfn), 0));
+}
+
+static const struct pci_dev_dma_source {
+	u16 vendor;
+	u16 device;
+	struct pci_dev *(*dma_source)(struct pci_dev *dev);
+} pci_dev_dma_source[] = {
+	/*
+	 * https://bugzilla.redhat.com/show_bug.cgi?id=605888
+	 *
+	 * Some Ricoh devices use the function 0 source ID for DMA on
+	 * other functions of a multifunction device.  The DMA devices
+	 * is therefore function 0, which will have implications of the
+	 * iommu grouping of these devices.
+	 */
+	{ PCI_VENDOR_ID_RICOH, 0xe822, pci_func_0_dma_source },
+	{ PCI_VENDOR_ID_RICOH, 0xe230, pci_func_0_dma_source },
+	{ PCI_VENDOR_ID_RICOH, 0xe832, pci_func_0_dma_source },
+	{ PCI_VENDOR_ID_RICOH, 0xe476, pci_func_0_dma_source },
+	{ 0 }
+};
+
+/*
+ * IOMMUs with isolation capabilities need to be programmed with the
+ * correct source ID of a device.  In most cases, the source ID matches
+ * the device doing the DMA, but sometimes hardware is broken and will
+ * tag the DMA as being sourced from a different device.  This function
+ * allows that translation.  Note that the reference count of the
+ * returned device is incremented on all paths.
+ */
+struct pci_dev *pci_get_dma_source(struct pci_dev *dev)
+{
+	const struct pci_dev_dma_source *i;
+
+	for (i = pci_dev_dma_source; i->dma_source; i++) {
+		if ((i->vendor == dev->vendor ||
+		     i->vendor == (u16)PCI_ANY_ID) &&
+		    (i->device == dev->device ||
+		     i->device == (u16)PCI_ANY_ID))
+			return i->dma_source(dev);
+	}
+
+	return pci_dev_get(dev);
+}
+
+static const struct pci_dev_acs_enabled {
+	u16 vendor;
+	u16 device;
+	int (*acs_enabled)(struct pci_dev *dev, u16 acs_flags);
+} pci_dev_acs_enabled[] = {
+	{ 0 }
+};
+
+int pci_dev_specific_acs_enabled(struct pci_dev *dev, u16 acs_flags)
+{
+	const struct pci_dev_acs_enabled *i;
+	int ret;
+
+	/*
+	 * Allow devices that do not expose standard PCIe ACS capabilities
+	 * or control to indicate their support here.  Multi-function express
+	 * devices which do not allow internal peer-to-peer between functions,
+	 * but do not implement PCIe ACS may wish to return true here.
+	 */
+	for (i = pci_dev_acs_enabled; i->acs_enabled; i++) {
+		if ((i->vendor == dev->vendor ||
+		     i->vendor == (u16)PCI_ANY_ID) &&
+		    (i->device == dev->device ||
+		     i->device == (u16)PCI_ANY_ID)) {
+			ret = i->acs_enabled(dev, acs_flags);
+			if (ret >= 0)
+				return ret;
+		}
+	}
+
+	return -ENOTTY;
+}
diff --git a/drivers/pci/remove.c b/drivers/pci/remove.c
index fd77e2bde2e..04a4861b474 100644
--- a/drivers/pci/remove.c
+++ b/drivers/pci/remove.c
@@ -68,6 +68,7 @@ void pci_remove_bus(struct pci_bus *pci_bus)
 
 	down_write(&pci_bus_sem);
 	list_del(&pci_bus->node);
+	pci_bus_release_busn_res(pci_bus);
 	up_write(&pci_bus_sem);
 	if (!pci_bus->is_added)
 		return;
diff --git a/drivers/pci/search.c b/drivers/pci/search.c
index 9d75dc8ca60..993d4a0a246 100644
--- a/drivers/pci/search.c
+++ b/drivers/pci/search.c
@@ -15,6 +15,8 @@
 #include "pci.h"
 
 DECLARE_RWSEM(pci_bus_sem);
+EXPORT_SYMBOL_GPL(pci_bus_sem);
+
 /*
  * find the upstream PCIe-to-PCI bridge of a PCI device
  * if the device is PCIE, return NULL
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 8fa2d4be88d..fb506137aae 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -265,7 +265,7 @@ out:
  * 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
+ * @fail_head : head of the list tracking requests that could
  *		not be allocated
  *
  * Satisfy resource requests of each element in the list. Add
@@ -308,7 +308,7 @@ static void __assign_resources_sorted(struct list_head *head,
 	 * Should not assign requested resources at first.
 	 *   they could be adjacent, so later reassign can not reallocate
 	 *   them one by one in parent resource window.
-	 * Try to assign requested + add_size at begining
+	 * Try to assign requested + add_size at beginning
 	 *  if could do that, could get out early.
 	 *  if could not do that, we still try to assign requested at first,
 	 *    then try to reassign add_size for some resources.
@@ -404,8 +404,8 @@ void pci_setup_cardbus(struct pci_bus *bus)
 	struct resource *res;
 	struct pci_bus_region region;
 
-	dev_info(&bridge->dev, "CardBus bridge to [bus %02x-%02x]\n",
-		 bus->secondary, bus->subordinate);
+	dev_info(&bridge->dev, "CardBus bridge to %pR\n",
+		 &bus->busn_res);
 
 	res = bus->resource[0];
 	pcibios_resource_to_bus(bridge, &region, res);
@@ -469,16 +469,23 @@ static void pci_setup_bridge_io(struct pci_bus *bus)
 	struct pci_dev *bridge = bus->self;
 	struct resource *res;
 	struct pci_bus_region region;
+	unsigned long io_mask;
+	u8 io_base_lo, io_limit_lo;
 	u32 l, io_upper16;
 
+	io_mask = PCI_IO_RANGE_MASK;
+	if (bridge->io_window_1k)
+		io_mask = PCI_IO_1K_RANGE_MASK;
+
 	/* Set up the top and bottom of the PCI I/O segment for this bus. */
 	res = bus->resource[0];
 	pcibios_resource_to_bus(bridge, &region, res);
 	if (res->flags & IORESOURCE_IO) {
 		pci_read_config_dword(bridge, PCI_IO_BASE, &l);
 		l &= 0xffff0000;
-		l |= (region.start >> 8) & 0x00f0;
-		l |= region.end & 0xf000;
+		io_base_lo = (region.start >> 8) & io_mask;
+		io_limit_lo = (region.end >> 8) & io_mask;
+		l |= ((u32) io_limit_lo << 8) | io_base_lo;
 		/* Set up upper 16 bits of I/O base/limit. */
 		io_upper16 = (region.end & 0xffff0000) | (region.start >> 16);
 		dev_info(&bridge->dev, "  bridge window %pR\n", res);
@@ -553,8 +560,8 @@ static void __pci_setup_bridge(struct pci_bus *bus, unsigned long type)
 {
 	struct pci_dev *bridge = bus->self;
 
-	dev_info(&bridge->dev, "PCI bridge to [bus %02x-%02x]\n",
-		 bus->secondary, bus->subordinate);
+	dev_info(&bridge->dev, "PCI bridge to %pR\n",
+		 &bus->busn_res);
 
 	if (type & IORESOURCE_IO)
 		pci_setup_bridge_io(bus);
@@ -699,7 +706,7 @@ static resource_size_t calculate_memsize(resource_size_t size,
  * @realloc_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
+ * since these windows have 1K or 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.
  */
@@ -710,10 +717,17 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO);
 	unsigned long size = 0, size0 = 0, size1 = 0;
 	resource_size_t children_add_size = 0;
+	resource_size_t min_align = 4096, align;
 
 	if (!b_res)
  		return;
 
+	/*
+	 * Per spec, I/O windows are 4K-aligned, but some bridges have an
+	 * extension to support 1K alignment.
+	 */
+	if (bus->self->io_window_1k)
+		min_align = 1024;
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		int i;
 
@@ -731,34 +745,43 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 			else
 				size1 += r_size;
 
+			align = pci_resource_alignment(dev, r);
+			if (align > min_align)
+				min_align = align;
+
 			if (realloc_head)
 				children_add_size += get_res_add_size(realloc_head, r);
 		}
 	}
+
+	if (min_align > 4096)
+		min_align = 4096;
+
 	size0 = calculate_iosize(size, min_size, size1,
-			resource_size(b_res), 4096);
+			resource_size(b_res), min_align);
 	if (children_add_size > add_size)
 		add_size = children_add_size;
 	size1 = (!realloc_head || (realloc_head && !add_size)) ? size0 :
 		calculate_iosize(size, min_size, add_size + size1,
-			resource_size(b_res), 4096);
+			resource_size(b_res), min_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,
-				 bus->secondary, bus->subordinate);
+				 "%pR to %pR (unused)\n", b_res,
+				 &bus->busn_res);
 		b_res->flags = 0;
 		return;
 	}
-	/* Alignment of the IO window is always 4K */
-	b_res->start = 4096;
+
+	b_res->start = min_align;
 	b_res->end = b_res->start + size0 - 1;
 	b_res->flags |= IORESOURCE_STARTALIGN;
 	if (size1 > size0 && realloc_head) {
-		add_to_list(realloc_head, bus->self, b_res, size1-size0, 4096);
+		add_to_list(realloc_head, bus->self, b_res, size1-size0,
+			    min_align);
 		dev_printk(KERN_DEBUG, &bus->self->dev, "bridge window "
-				 "%pR to [bus %02x-%02x] add_size %lx\n", b_res,
-				 bus->secondary, bus->subordinate, size1-size0);
+				 "%pR to %pR add_size %lx\n", b_res,
+				 &bus->busn_res, size1-size0);
 	}
 }
 
@@ -863,8 +886,8 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 	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,
-				 bus->secondary, bus->subordinate);
+				 "%pR to %pR (unused)\n", b_res,
+				 &bus->busn_res);
 		b_res->flags = 0;
 		return 1;
 	}
@@ -874,8 +897,8 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 	if (size1 > size0 && realloc_head) {
 		add_to_list(realloc_head, bus->self, b_res, size1-size0, min_align);
 		dev_printk(KERN_DEBUG, &bus->self->dev, "bridge window "
-				 "%pR to [bus %02x-%02x] add_size %llx\n", b_res,
-				 bus->secondary, bus->subordinate, (unsigned long long)size1-size0);
+				 "%pR to %pR add_size %llx\n", b_res,
+				 &bus->busn_res, (unsigned long long)size1-size0);
 	}
 	return 1;
 }
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index eea85dafc76..81b88bda793 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -30,6 +30,8 @@
 void pci_update_resource(struct pci_dev *dev, int resno)
 {
 	struct pci_bus_region region;
+	bool disable;
+	u16 cmd;
 	u32 new, check, mask;
 	int reg;
 	enum pci_bar_type type;
@@ -67,6 +69,18 @@ void pci_update_resource(struct pci_dev *dev, int resno)
 		new |= PCI_ROM_ADDRESS_ENABLE;
 	}
 
+	/*
+	 * We can't update a 64-bit BAR atomically, so when possible,
+	 * disable decoding so that a half-updated BAR won't conflict
+	 * with another device.
+	 */
+	disable = (res->flags & IORESOURCE_MEM_64) && !dev->mmio_always_on;
+	if (disable) {
+		pci_read_config_word(dev, PCI_COMMAND, &cmd);
+		pci_write_config_word(dev, PCI_COMMAND,
+				      cmd & ~PCI_COMMAND_MEMORY);
+	}
+
 	pci_write_config_dword(dev, reg, new);
 	pci_read_config_dword(dev, reg, &check);
 
@@ -84,6 +98,10 @@ void pci_update_resource(struct pci_dev *dev, int resno)
 			       "(high %#08x != %#08x)\n", resno, new, check);
 		}
 	}
+
+	if (disable)
+		pci_write_config_word(dev, PCI_COMMAND, cmd);
+
 	res->flags &= ~IORESOURCE_UNSET;
 	dev_dbg(&dev->dev, "BAR %d: set to %pR (PCI address [%#llx-%#llx])\n",
 		resno, res, (unsigned long long)region.start,
@@ -127,33 +145,6 @@ void pci_disable_bridge_window(struct pci_dev *dev)
 	pci_write_config_dword(dev, PCI_PREF_BASE_UPPER32, 0xffffffff);
 }
 
-static int __pci_assign_resource(struct pci_bus *bus, struct pci_dev *dev,
-		int resno, resource_size_t size, resource_size_t align)
-{
-	struct resource *res = dev->resource + resno;
-	resource_size_t min;
-	int ret;
-
-	min = (res->flags & IORESOURCE_IO) ? PCIBIOS_MIN_IO : PCIBIOS_MIN_MEM;
-
-	/* First, try exact prefetching match.. */
-	ret = pci_bus_alloc_resource(bus, res, size, align, min,
-				     IORESOURCE_PREFETCH,
-				     pcibios_align_resource, dev);
-
-	if (ret < 0 && (res->flags & IORESOURCE_PREFETCH)) {
-		/*
-		 * That failed.
-		 *
-		 * But a prefetching area can handle a non-prefetching
-		 * window (it will just not perform as well).
-		 */
-		ret = pci_bus_alloc_resource(bus, res, size, align, min, 0,
-					     pcibios_align_resource, dev);
-	}
-	return ret;
-}
-
 /*
  * Generic function that returns a value indicating that the device's
  * original BIOS BAR address was not saved and so is not available for
@@ -206,7 +197,35 @@ static int pci_revert_fw_address(struct resource *res, struct pci_dev *dev,
 	return ret;
 }
 
-static int _pci_assign_resource(struct pci_dev *dev, int resno, int size, resource_size_t min_align)
+static int __pci_assign_resource(struct pci_bus *bus, struct pci_dev *dev,
+		int resno, resource_size_t size, resource_size_t align)
+{
+	struct resource *res = dev->resource + resno;
+	resource_size_t min;
+	int ret;
+
+	min = (res->flags & IORESOURCE_IO) ? PCIBIOS_MIN_IO : PCIBIOS_MIN_MEM;
+
+	/* First, try exact prefetching match.. */
+	ret = pci_bus_alloc_resource(bus, res, size, align, min,
+				     IORESOURCE_PREFETCH,
+				     pcibios_align_resource, dev);
+
+	if (ret < 0 && (res->flags & IORESOURCE_PREFETCH)) {
+		/*
+		 * That failed.
+		 *
+		 * But a prefetching area can handle a non-prefetching
+		 * window (it will just not perform as well).
+		 */
+		ret = pci_bus_alloc_resource(bus, res, size, align, min, 0,
+					     pcibios_align_resource, dev);
+	}
+	return ret;
+}
+
+static int _pci_assign_resource(struct pci_dev *dev, int resno,
+				resource_size_t size, resource_size_t min_align)
 {
 	struct resource *res = dev->resource + resno;
 	struct pci_bus *bus;
@@ -238,31 +257,6 @@ static int _pci_assign_resource(struct pci_dev *dev, int resno, int size, resour
 	return ret;
 }
 
-int pci_reassign_resource(struct pci_dev *dev, int resno, resource_size_t addsize,
-			resource_size_t min_align)
-{
-	struct resource *res = dev->resource + resno;
-	resource_size_t new_size;
-	int ret;
-
-	if (!res->parent) {
-		dev_info(&dev->dev, "BAR %d: can't reassign an unassigned resource %pR "
-			 "\n", resno, res);
-		return -EINVAL;
-	}
-
-	/* already aligned with min_align */
-	new_size = resource_size(res) + addsize;
-	ret = _pci_assign_resource(dev, resno, new_size, min_align);
-	if (!ret) {
-		res->flags &= ~IORESOURCE_STARTALIGN;
-		dev_info(&dev->dev, "BAR %d: reassigned %pR\n", resno, res);
-		if (resno < PCI_BRIDGE_RESOURCES)
-			pci_update_resource(dev, resno);
-	}
-	return ret;
-}
-
 int pci_assign_resource(struct pci_dev *dev, int resno)
 {
 	struct resource *res = dev->resource + resno;
@@ -298,6 +292,31 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
 	return ret;
 }
 
+int pci_reassign_resource(struct pci_dev *dev, int resno, resource_size_t addsize,
+			resource_size_t min_align)
+{
+	struct resource *res = dev->resource + resno;
+	resource_size_t new_size;
+	int ret;
+
+	if (!res->parent) {
+		dev_info(&dev->dev, "BAR %d: can't reassign an unassigned resource %pR "
+			 "\n", resno, res);
+		return -EINVAL;
+	}
+
+	/* already aligned with min_align */
+	new_size = resource_size(res) + addsize;
+	ret = _pci_assign_resource(dev, resno, new_size, min_align);
+	if (!ret) {
+		res->flags &= ~IORESOURCE_STARTALIGN;
+		dev_info(&dev->dev, "BAR %d: reassigned %pR\n", resno, res);
+		if (resno < PCI_BRIDGE_RESOURCES)
+			pci_update_resource(dev, resno);
+	}
+	return ret;
+}
+
 int pci_enable_resources(struct pci_dev *dev, int mask)
 {
 	u16 cmd, old_cmd;
diff --git a/drivers/pcmcia/cardbus.c b/drivers/pcmcia/cardbus.c
index 6e75153c5b4..24caeaf5052 100644
--- a/drivers/pcmcia/cardbus.c
+++ b/drivers/pcmcia/cardbus.c
@@ -73,7 +73,7 @@ int __ref cb_alloc(struct pcmcia_socket *s)
 	s->functions = pci_scan_slot(bus, PCI_DEVFN(0, 0));
 	pci_fixup_cardbus(bus);
 
-	max = bus->secondary;
+	max = bus->busn_res.start;
 	for (pass = 0; pass < 2; pass++)
 		list_for_each_entry(dev, &bus->devices, bus_list)
 			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
diff --git a/drivers/pcmcia/sa1100_shannon.c b/drivers/pcmcia/sa1100_shannon.c
index decb34730bc..56ab7391560 100644
--- a/drivers/pcmcia/sa1100_shannon.c
+++ b/drivers/pcmcia/sa1100_shannon.c
@@ -8,6 +8,7 @@
 #include <linux/kernel.h>
 #include <linux/device.h>
 #include <linux/init.h>
+#include <linux/io.h>
 
 #include <mach/hardware.h>
 #include <asm/mach-types.h>
diff --git a/drivers/pcmcia/yenta_socket.c b/drivers/pcmcia/yenta_socket.c
index d07f9ac8c41..667678db115 100644
--- a/drivers/pcmcia/yenta_socket.c
+++ b/drivers/pcmcia/yenta_socket.c
@@ -1048,8 +1048,8 @@ static void yenta_config_init(struct yenta_socket *socket)
 	config_writeb(socket, PCI_LATENCY_TIMER, 168);
 	config_writel(socket, PCI_PRIMARY_BUS,
 		(176 << 24) |			   /* sec. latency timer */
-		(dev->subordinate->subordinate << 16) | /* subordinate bus */
-		(dev->subordinate->secondary << 8) |  /* secondary bus */
+		((unsigned int)dev->subordinate->busn_res.end << 16) | /* subordinate bus */
+		((unsigned int)dev->subordinate->busn_res.start << 8) |  /* secondary bus */
 		dev->subordinate->primary);		   /* primary bus */
 
 	/*
@@ -1086,14 +1086,14 @@ static void yenta_fixup_parent_bridge(struct pci_bus *cardbus_bridge)
 	struct pci_bus *bridge_to_fix = cardbus_bridge->parent;
 
 	/* Check bus numbers are already set up correctly: */
-	if (bridge_to_fix->subordinate >= cardbus_bridge->subordinate)
+	if (bridge_to_fix->busn_res.end >= cardbus_bridge->busn_res.end)
 		return; /* The subordinate number is ok, nothing to do */
 
 	if (!bridge_to_fix->parent)
 		return; /* Root bridges are ok */
 
 	/* stay within the limits of the bus range of the parent: */
-	upper_limit = bridge_to_fix->parent->subordinate;
+	upper_limit = bridge_to_fix->parent->busn_res.end;
 
 	/* check the bus ranges of all silbling bridges to prevent overlap */
 	list_for_each(tmp, &bridge_to_fix->parent->children) {
@@ -1104,36 +1104,36 @@ static void yenta_fixup_parent_bridge(struct pci_bus *cardbus_bridge)
 		 * current upper limit, set the new upper limit to
 		 * the bus number below the silbling's range:
 		 */
-		if (silbling->secondary > bridge_to_fix->subordinate
-		    && silbling->secondary <= upper_limit)
-			upper_limit = silbling->secondary - 1;
+		if (silbling->busn_res.start > bridge_to_fix->busn_res.end
+		    && silbling->busn_res.start <= upper_limit)
+			upper_limit = silbling->busn_res.start - 1;
 	}
 
 	/* Show that the wanted subordinate number is not possible: */
-	if (cardbus_bridge->subordinate > upper_limit)
+	if (cardbus_bridge->busn_res.end > upper_limit)
 		dev_printk(KERN_WARNING, &cardbus_bridge->dev,
 			   "Upper limit for fixing this "
 			   "bridge's parent bridge: #%02x\n", upper_limit);
 
 	/* If we have room to increase the bridge's subordinate number, */
-	if (bridge_to_fix->subordinate < upper_limit) {
+	if (bridge_to_fix->busn_res.end < upper_limit) {
 
 		/* use the highest number of the hidden bus, within limits */
 		unsigned char subordinate_to_assign =
-			min(cardbus_bridge->subordinate, upper_limit);
+			min_t(int, cardbus_bridge->busn_res.end, upper_limit);
 
 		dev_printk(KERN_INFO, &bridge_to_fix->dev,
 			   "Raising subordinate bus# of parent "
 			   "bus (#%02x) from #%02x to #%02x\n",
 			   bridge_to_fix->number,
-			   bridge_to_fix->subordinate, subordinate_to_assign);
+			   (int)bridge_to_fix->busn_res.end, subordinate_to_assign);
 
 		/* Save the new subordinate in the bus struct of the bridge */
-		bridge_to_fix->subordinate = subordinate_to_assign;
+		bridge_to_fix->busn_res.end = subordinate_to_assign;
 
 		/* and update the PCI config space with the new subordinate */
 		pci_write_config_byte(bridge_to_fix->self,
-			PCI_SUBORDINATE_BUS, bridge_to_fix->subordinate);
+			PCI_SUBORDINATE_BUS, bridge_to_fix->busn_res.end);
 	}
 }
 
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index c6e6ae0aa3b..54e3588bef6 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -102,6 +102,14 @@ config PINCTRL_PXA910
 	select PINCTRL_PXA3xx
 	select PINCONF
 
+config PINCTRL_SINGLE
+	tristate "One-register-per-pin type device tree based pinctrl driver"
+	depends on OF
+	select PINMUX
+	select PINCONF
+	help
+	  This selects the device tree based generic pinctrl driver.
+
 config PINCTRL_SIRF
 	bool "CSR SiRFprimaII pin controller driver"
 	depends on ARCH_PRIMA2
@@ -130,7 +138,7 @@ config PINCTRL_U300
 
 config PINCTRL_COH901
 	bool "ST-Ericsson U300 COH 901 335/571 GPIO"
-	depends on GPIOLIB && ARCH_U300 && PINMUX_U300
+	depends on GPIOLIB && ARCH_U300 && PINCTRL_U300
 	help
 	  Say yes here to support GPIO interface on ST-Ericsson U300.
 	  The names of the two IP block variants supported are
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 8c074376cde..f40b1f81ff2 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -22,6 +22,7 @@ obj-$(CONFIG_PINCTRL_NOMADIK)	+= pinctrl-nomadik.o
 obj-$(CONFIG_PINCTRL_DB8500)	+= pinctrl-nomadik-db8500.o
 obj-$(CONFIG_PINCTRL_PXA168)	+= pinctrl-pxa168.o
 obj-$(CONFIG_PINCTRL_PXA910)	+= pinctrl-pxa910.o
+obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= pinctrl-sirf.o
 obj-$(CONFIG_PINCTRL_TEGRA)	+= pinctrl-tegra.o
 obj-$(CONFIG_PINCTRL_TEGRA20)	+= pinctrl-tegra20.o
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index 0cc053af70b..fb7f3bebdc6 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -332,19 +332,16 @@ void pinctrl_add_gpio_range(struct pinctrl_dev *pctldev,
 }
 EXPORT_SYMBOL_GPL(pinctrl_add_gpio_range);
 
-/**
- * pinctrl_remove_gpio_range() - remove a range of GPIOs fro a pin controller
- * @pctldev: pin controller device to remove the range from
- * @range: the GPIO range to remove
- */
-void pinctrl_remove_gpio_range(struct pinctrl_dev *pctldev,
-			       struct pinctrl_gpio_range *range)
+void pinctrl_add_gpio_ranges(struct pinctrl_dev *pctldev,
+			     struct pinctrl_gpio_range *ranges,
+			     unsigned nranges)
 {
-	mutex_lock(&pinctrl_mutex);
-	list_del(&range->node);
-	mutex_unlock(&pinctrl_mutex);
+	int i;
+
+	for (i = 0; i < nranges; i++)
+		pinctrl_add_gpio_range(pctldev, &ranges[i]);
 }
-EXPORT_SYMBOL_GPL(pinctrl_remove_gpio_range);
+EXPORT_SYMBOL_GPL(pinctrl_add_gpio_ranges);
 
 /**
  * pinctrl_get_group_selector() - returns the group selector for a group
@@ -1395,9 +1392,9 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	struct pinctrl_dev *pctldev;
 	int ret;
 
-	if (pctldesc == NULL)
+	if (!pctldesc)
 		return NULL;
-	if (pctldesc->name == NULL)
+	if (!pctldesc->name)
 		return NULL;
 
 	pctldev = kzalloc(sizeof(*pctldev), GFP_KERNEL);
@@ -1415,23 +1412,20 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	pctldev->dev = dev;
 
 	/* check core ops for sanity */
-	ret = pinctrl_check_ops(pctldev);
-	if (ret) {
+	if (pinctrl_check_ops(pctldev)) {
 		dev_err(dev, "pinctrl ops lacks necessary functions\n");
 		goto out_err;
 	}
 
 	/* If we're implementing pinmuxing, check the ops for sanity */
 	if (pctldesc->pmxops) {
-		ret = pinmux_check_ops(pctldev);
-		if (ret)
+		if (pinmux_check_ops(pctldev))
 			goto out_err;
 	}
 
 	/* If we're implementing pinconfig, check the ops for sanity */
 	if (pctldesc->confops) {
-		ret = pinconf_check_ops(pctldev);
-		if (ret)
+		if (pinconf_check_ops(pctldev))
 			goto out_err;
 	}
 
@@ -1457,11 +1451,9 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 		if (IS_ERR(s)) {
 			dev_dbg(dev, "failed to lookup the default state\n");
 		} else {
-			ret = pinctrl_select_state_locked(pctldev->p, s);
-			if (ret) {
+			if (pinctrl_select_state_locked(pctldev->p, s))
 				dev_err(dev,
 					"failed to select default state\n");
-			}
 		}
 	}
 
@@ -1485,6 +1477,7 @@ EXPORT_SYMBOL_GPL(pinctrl_register);
  */
 void pinctrl_unregister(struct pinctrl_dev *pctldev)
 {
+	struct pinctrl_gpio_range *range, *n;
 	if (pctldev == NULL)
 		return;
 
@@ -1500,6 +1493,10 @@ void pinctrl_unregister(struct pinctrl_dev *pctldev)
 	/* Destroy descriptor tree */
 	pinctrl_free_pindescs(pctldev, pctldev->desc->pins,
 			      pctldev->desc->npins);
+	/* remove gpio ranges map */
+	list_for_each_entry_safe(range, n, &pctldev->gpio_ranges, node)
+		list_del(&range->node);
+
 	kfree(pctldev);
 
 	mutex_unlock(&pinctrl_mutex);
diff --git a/drivers/pinctrl/pinctrl-coh901.c b/drivers/pinctrl/pinctrl-coh901.c
index 55697a5d748..cc0f00d73d1 100644
--- a/drivers/pinctrl/pinctrl-coh901.c
+++ b/drivers/pinctrl/pinctrl-coh901.c
@@ -770,7 +770,7 @@ static int __init u300_gpio_probe(struct platform_device *pdev)
 		dev_err(gpio->dev, "could not get GPIO clock\n");
 		goto err_no_clk;
 	}
-	err = clk_enable(gpio->clk);
+	err = clk_prepare_enable(gpio->clk);
 	if (err) {
 		dev_err(gpio->dev, "could not enable GPIO clock\n");
 		goto err_no_clk_enable;
@@ -912,7 +912,7 @@ err_no_ioremap:
 	release_mem_region(gpio->memres->start, resource_size(gpio->memres));
 err_no_ioregion:
 err_no_resource:
-	clk_disable(gpio->clk);
+	clk_disable_unprepare(gpio->clk);
 err_no_clk_enable:
 	clk_put(gpio->clk);
 err_no_clk:
@@ -943,7 +943,7 @@ static int __exit u300_gpio_remove(struct platform_device *pdev)
 	iounmap(gpio->base);
 	release_mem_region(gpio->memres->start,
 			   resource_size(gpio->memres));
-	clk_disable(gpio->clk);
+	clk_disable_unprepare(gpio->clk);
 	clk_put(gpio->clk);
 	platform_set_drvdata(pdev, NULL);
 	kfree(gpio);
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
index dd6d93aa533..44e97265cd7 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -146,7 +146,7 @@ static int imx_dt_node_to_map(struct pinctrl_dev *pctldev,
 	struct pinctrl_map *new_map;
 	struct device_node *parent;
 	int map_num = 1;
-	int i;
+	int i, j;
 
 	/*
 	 * first find the group of this node and check if we need create
@@ -184,13 +184,14 @@ static int imx_dt_node_to_map(struct pinctrl_dev *pctldev,
 
 	/* create config map */
 	new_map++;
-	for (i = 0; i < grp->npins; i++) {
+	for (i = j = 0; i < grp->npins; i++) {
 		if (!(grp->configs[i] & IMX_NO_PAD_CTL)) {
-			new_map[i].type = PIN_MAP_TYPE_CONFIGS_PIN;
-			new_map[i].data.configs.group_or_pin =
+			new_map[j].type = PIN_MAP_TYPE_CONFIGS_PIN;
+			new_map[j].data.configs.group_or_pin =
 					pin_get_name(pctldev, grp->pins[i]);
-			new_map[i].data.configs.configs = &grp->configs[i];
-			new_map[i].data.configs.num_configs = 1;
+			new_map[j].data.configs.configs = &grp->configs[i];
+			new_map[j].data.configs.num_configs = 1;
+			j++;
 		}
 	}
 
@@ -474,7 +475,9 @@ static int __devinit imx_pinctrl_parse_groups(struct device_node *np,
 		grp->configs[j] = config & ~IMX_PAD_SION;
 	}
 
+#ifdef DEBUG
 	IMX_PMX_DUMP(info, grp->pins, grp->mux_mode, grp->configs, grp->npins);
+#endif
 
 	return 0;
 }
diff --git a/drivers/pinctrl/pinctrl-imx6q.c b/drivers/pinctrl/pinctrl-imx6q.c
index 7737d4d71a3..e9bf71fbedc 100644
--- a/drivers/pinctrl/pinctrl-imx6q.c
+++ b/drivers/pinctrl/pinctrl-imx6q.c
@@ -1950,6 +1950,8 @@ static struct imx_pin_reg imx6q_pin_regs[] = {
 	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 5, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__GPIO_1_12 */
 	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 6, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__SJC_DONE */
 	IMX_PIN_REG(MX6Q_PAD_SD2_DAT3, 0x0744, 0x035C, 7, 0x0000, 0), /* MX6Q_PAD_SD2_DAT3__ANATOP_TESTO_3 */
+	IMX_PIN_REG(MX6Q_PAD_ENET_RX_ER, 0x04EC, 0x01D8, 0, 0x0000, 0), /* MX6Q_PAD_ENET_RX_ER__ANATOP_USBOTG_ID */
+	IMX_PIN_REG(MX6Q_PAD_GPIO_1, 0x05F4, 0x0224, 3, 0x0000, 0), /* MX6Q_PAD_GPIO_1__ANATOP_USBOTG_ID */
 };
 
 /* Pad names for the pinmux subsystem */
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8500.c b/drivers/pinctrl/pinctrl-nomadik-db8500.c
index 8b2022276f7..6f99769c673 100644
--- a/drivers/pinctrl/pinctrl-nomadik-db8500.c
+++ b/drivers/pinctrl/pinctrl-nomadik-db8500.c
@@ -467,9 +467,12 @@ static const unsigned mc1_a_1_pins[] = { DB8500_PIN_AH16, DB8500_PIN_AG15,
 	DB8500_PIN_AH15 };
 static const unsigned mc1dir_a_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AG12,
 	DB8500_PIN_AH12, DB8500_PIN_AH11 };
-static const unsigned hsir_a_1_pins[] = { DB8500_PIN_AG10, DB8500_PIN_AH10 };
-static const unsigned hsit_a_1_pins[] = { DB8500_PIN_AJ11, DB8500_PIN_AJ9,
-	DB8500_PIN_AH9, DB8500_PIN_AG9, DB8500_PIN_AG8, DB8500_PIN_AF8 };
+static const unsigned hsir_a_1_pins[] = { DB8500_PIN_AG10, DB8500_PIN_AH10,
+	DB8500_PIN_AJ11 };
+static const unsigned hsit_a_1_pins[] = { DB8500_PIN_AJ9, DB8500_PIN_AH9,
+	DB8500_PIN_AG9, DB8500_PIN_AG8, DB8500_PIN_AF8 };
+static const unsigned hsit_a_2_pins[] = { DB8500_PIN_AJ9, DB8500_PIN_AH9,
+	DB8500_PIN_AG9, DB8500_PIN_AG8 };
 static const unsigned clkout_a_1_pins[] = { DB8500_PIN_AH7, DB8500_PIN_AJ6 };
 static const unsigned clkout_a_2_pins[] = { DB8500_PIN_AG7, DB8500_PIN_AF7 };
 static const unsigned usb_a_1_pins[] = { DB8500_PIN_AF28, DB8500_PIN_AE29,
@@ -508,9 +511,11 @@ static const unsigned sm_b_1_pins[] = { DB8500_PIN_C6, DB8500_PIN_B3,
 	DB8500_PIN_D9, DB8500_PIN_A5, DB8500_PIN_B4, DB8500_PIN_C8,
 	DB8500_PIN_A12, DB8500_PIN_C10, DB8500_PIN_B10, DB8500_PIN_B9,
 	DB8500_PIN_A9, DB8500_PIN_C7, DB8500_PIN_A7, DB8500_PIN_C5,
-	DB8500_PIN_C9, DB8500_PIN_B14 };
-/* This chip select pin can be "ps0" in alt B so have it separately */
+	DB8500_PIN_C9 };
+/* This chip select pin can be "ps0" in alt C so have it separately */
 static const unsigned smcs0_b_1_pins[] = { DB8500_PIN_E8 };
+/* This chip select pin can be "ps1" in alt C so have it separately */
+static const unsigned smcs1_b_1_pins[] = { DB8500_PIN_B14 };
 static const unsigned ipgpio7_b_1_pins[] = { DB8500_PIN_B11 };
 static const unsigned ipgpio2_b_1_pins[] = { DB8500_PIN_C12 };
 static const unsigned ipgpio3_b_1_pins[] = { DB8500_PIN_C11 };
@@ -572,6 +577,7 @@ static const unsigned mc2rstn_c_1_pins[] = { DB8500_PIN_C8 };
 static const unsigned kp_c_1_pins[] = { DB8500_PIN_C9, DB8500_PIN_B11,
 	DB8500_PIN_C12, DB8500_PIN_C11, DB8500_PIN_D17, DB8500_PIN_D16,
 	DB8500_PIN_C23, DB8500_PIN_D23 };
+static const unsigned smps0_c_1_pins[] = { DB8500_PIN_E8 };
 static const unsigned smps1_c_1_pins[] = { DB8500_PIN_B14 };
 static const unsigned u2rxtx_c_3_pins[] = { DB8500_PIN_B17, DB8500_PIN_C16 };
 static const unsigned stmape_c_2_pins[] = { DB8500_PIN_C19, DB8500_PIN_C17,
@@ -595,6 +601,8 @@ static const unsigned kp_oc1_1_pins[] = { DB8500_PIN_C6, DB8500_PIN_B3,
 	DB8500_PIN_D6, DB8500_PIN_B7 };
 static const unsigned spi2_oc1_1_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AG12,
 	DB8500_PIN_AH12, DB8500_PIN_AH11 };
+static const unsigned spi2_oc1_2_pins[] = { DB8500_PIN_AH13, DB8500_PIN_AH12,
+	DB8500_PIN_AH11 };
 
 #define DB8500_PIN_GROUP(a,b) { .name = #a, .pins = a##_pins,		\
 			.npins = ARRAY_SIZE(a##_pins), .altsetting = b }
@@ -610,6 +618,8 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
 	DB8500_PIN_GROUP(msp0tfstck_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(msp0rfsrck_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(mc0_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(mc0_dat47_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(mc0dat31dir_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(msp1txrx_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(msp1_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(lcdb_a_1, NMK_GPIO_ALT_A),
@@ -631,6 +641,7 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
 	DB8500_PIN_GROUP(mc1_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(hsir_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(hsit_a_1, NMK_GPIO_ALT_A),
+	DB8500_PIN_GROUP(hsit_a_2, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(clkout_a_1, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(clkout_a_2, NMK_GPIO_ALT_A),
 	DB8500_PIN_GROUP(usb_a_1, NMK_GPIO_ALT_A),
@@ -653,6 +664,7 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
 	DB8500_PIN_GROUP(kp_b_1, NMK_GPIO_ALT_B),
 	DB8500_PIN_GROUP(sm_b_1, NMK_GPIO_ALT_B),
 	DB8500_PIN_GROUP(smcs0_b_1, NMK_GPIO_ALT_B),
+	DB8500_PIN_GROUP(smcs1_b_1, NMK_GPIO_ALT_B),
 	DB8500_PIN_GROUP(ipgpio7_b_1, NMK_GPIO_ALT_B),
 	DB8500_PIN_GROUP(ipgpio2_b_1, NMK_GPIO_ALT_B),
 	DB8500_PIN_GROUP(ipgpio3_b_1, NMK_GPIO_ALT_B),
@@ -693,6 +705,7 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
 	DB8500_PIN_GROUP(mc5_c_1, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(mc2rstn_c_1, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(kp_c_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(smps0_c_1, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(smps1_c_1, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(u2rxtx_c_3, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(stmape_c_2, NMK_GPIO_ALT_C),
@@ -709,6 +722,7 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
 	/* Other alt C1 column, these are still configured as alt C */
 	DB8500_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(spi2_oc1_1, NMK_GPIO_ALT_C),
+	DB8500_PIN_GROUP(spi2_oc1_2, NMK_GPIO_ALT_C),
 };
 
 /* We use this macro to define the groups applicable to a function */
@@ -731,7 +745,7 @@ DB8500_FUNC_GROUPS(ipi2c, "ipi2c_a_1", "ipi2c_a_2");
  */
 DB8500_FUNC_GROUPS(msp0, "msp0txrx_a_1", "msp0tfstck_a_1", "msp0rfstck_a_1",
 		   "msp0txrx_b_1", "msp0sck_b_1");
-DB8500_FUNC_GROUPS(mc0, "mc0_a_1");
+DB8500_FUNC_GROUPS(mc0, "mc0_a_1", "mc0_dat47_a_1", "mc0dat31dir_a_1");
 /* MSP0 can swap RX/TX like MSP0 but has no SCK pin available */
 DB8500_FUNC_GROUPS(msp1, "msp1txrx_a_1", "msp1_a_1", "msp1txrx_b_1");
 DB8500_FUNC_GROUPS(lcdb, "lcdb_a_1");
@@ -752,7 +766,7 @@ DB8500_FUNC_GROUPS(ipgpio, "ipgpio0_a_1", "ipgpio1_a_1", "ipgpio7_b_1",
 DB8500_FUNC_GROUPS(msp2, "msp2sck_a_1", "msp2_a_1");
 DB8500_FUNC_GROUPS(mc4, "mc4_a_1", "mc4rstn_c_1");
 DB8500_FUNC_GROUPS(mc1, "mc1_a_1", "mc1dir_a_1");
-DB8500_FUNC_GROUPS(hsi, "hsir1_a_1", "hsit1_a_1");
+DB8500_FUNC_GROUPS(hsi, "hsir1_a_1", "hsit1_a_1", "hsit_a_2");
 DB8500_FUNC_GROUPS(clkout, "clkout_a_1", "clkout_a_2", "clkout_c_1");
 DB8500_FUNC_GROUPS(usb, "usb_a_1");
 DB8500_FUNC_GROUPS(trig, "trig_b_1");
@@ -768,7 +782,8 @@ DB8500_FUNC_GROUPS(uartmod, "uartmodtx_b_1", "uartmodrx_b_1", "uartmodrx_b_2",
 DB8500_FUNC_GROUPS(stmmod, "stmmod_b_1", "stmmod_c_1");
 DB8500_FUNC_GROUPS(spi3, "spi3_b_1");
 /* Select between CS0 on alt B or PS1 on alt C */
-DB8500_FUNC_GROUPS(sm, "sm_b_1", "smcs0_b_1", "smcleale_c_1", "smps1_c_1");
+DB8500_FUNC_GROUPS(sm, "sm_b_1", "smcs0_b_1", "smcs1_b_1", "smcleale_c_1",
+		   "smps0_c_1", "smps1_c_1");
 DB8500_FUNC_GROUPS(lcda, "lcdaclk_b_1", "lcda_b_1");
 DB8500_FUNC_GROUPS(ddrtrig, "ddrtrig_b_1");
 DB8500_FUNC_GROUPS(pwl, "pwl_b_1", "pwl_b_2", "pwl_b_3", "pwl_b_4");
@@ -783,7 +798,7 @@ DB8500_FUNC_GROUPS(mc5, "mc5_c_1");
 DB8500_FUNC_GROUPS(usbsim, "usbsim_c_1", "usbsim_c_2");
 DB8500_FUNC_GROUPS(i2c3, "i2c3_c_1", "i2c3_c_2");
 DB8500_FUNC_GROUPS(spi0, "spi0_c_1");
-DB8500_FUNC_GROUPS(spi2, "spi2_oc1_1");
+DB8500_FUNC_GROUPS(spi2, "spi2_oc1_1", "spi2_oc1_2");
 
 #define FUNCTION(fname)					\
 	{						\
diff --git a/drivers/pinctrl/pinctrl-nomadik.c b/drivers/pinctrl/pinctrl-nomadik.c
index 3e7e47d6b38..53b0d49a7a1 100644
--- a/drivers/pinctrl/pinctrl-nomadik.c
+++ b/drivers/pinctrl/pinctrl-nomadik.c
@@ -434,7 +434,7 @@ static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
 /**
  * nmk_config_pin - configure a pin's mux attributes
  * @cfg: pin confguration
- *
+ * @sleep: Non-zero to apply the sleep mode configuration
  * Configures a pin's mode (alternate function or GPIO), its pull up status,
  * and its sleep mode based on the specified configuration.  The @cfg is
  * usually one of the SoC specific macros defined in mach/<soc>-pins.h.  These
@@ -1194,11 +1194,11 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	}
 
 	if (np) {
-		pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+		pdata = devm_kzalloc(&dev->dev, sizeof(*pdata), GFP_KERNEL);
 		if (!pdata)
 			return -ENOMEM;
 
-		if (of_get_property(np, "supports-sleepmode", NULL))
+		if (of_get_property(np, "st,supports-sleepmode", NULL))
 			pdata->supports_sleepmode = true;
 
 		if (of_property_read_u32(np, "gpio-bank", &dev->id)) {
@@ -1229,29 +1229,23 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 		goto out;
 	}
 
-	if (request_mem_region(res->start, resource_size(res),
-			       dev_name(&dev->dev)) == NULL) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	base = ioremap(res->start, resource_size(res));
+	base = devm_request_and_ioremap(&dev->dev, res);
 	if (!base) {
 		ret = -ENOMEM;
-		goto out_release;
+		goto out;
 	}
 
-	clk = clk_get(&dev->dev, NULL);
+	clk = devm_clk_get(&dev->dev, NULL);
 	if (IS_ERR(clk)) {
 		ret = PTR_ERR(clk);
-		goto out_unmap;
+		goto out;
 	}
 	clk_prepare(clk);
 
-	nmk_chip = kzalloc(sizeof(*nmk_chip), GFP_KERNEL);
+	nmk_chip = devm_kzalloc(&dev->dev, sizeof(*nmk_chip), GFP_KERNEL);
 	if (!nmk_chip) {
 		ret = -ENOMEM;
-		goto out_clk;
+		goto out;
 	}
 
 	/*
@@ -1286,7 +1280,7 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 
 	ret = gpiochip_add(&nmk_chip->chip);
 	if (ret)
-		goto out_free;
+		goto out;
 
 	BUG_ON(nmk_chip->bank >= ARRAY_SIZE(nmk_gpio_chips));
 
@@ -1300,7 +1294,7 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	if (!nmk_chip->domain) {
 		pr_err("%s: Failed to create irqdomain\n", np->full_name);
 		ret = -ENOSYS;
-		goto out_free;
+		goto out;
 	}
 
 	nmk_gpio_init_irq(nmk_chip);
@@ -1309,20 +1303,9 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 
 	return 0;
 
-out_free:
-	kfree(nmk_chip);
-out_clk:
-	clk_disable(clk);
-	clk_put(clk);
-out_unmap:
-	iounmap(base);
-out_release:
-	release_mem_region(res->start, resource_size(res));
 out:
 	dev_err(&dev->dev, "Failure %i for GPIO %i-%i\n", ret,
 		  pdata->first_gpio, pdata->first_gpio+31);
-	if (np)
-		kfree(pdata);
 
 	return ret;
 }
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
new file mode 100644
index 00000000000..76a4260f20f
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -0,0 +1,987 @@
+/*
+ * Generic device tree based pinctrl driver for one register per pin
+ * type pinmux controllers
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/list.h>
+
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+
+#include "core.h"
+
+#define DRIVER_NAME			"pinctrl-single"
+#define PCS_MUX_NAME			"pinctrl-single,pins"
+#define PCS_REG_NAME_LEN		((sizeof(unsigned long) * 2) + 1)
+#define PCS_OFF_DISABLED		~0U
+
+/**
+ * struct pcs_pingroup - pingroups for a function
+ * @np:		pingroup device node pointer
+ * @name:	pingroup name
+ * @gpins:	array of the pins in the group
+ * @ngpins:	number of pins in the group
+ * @node:	list node
+ */
+struct pcs_pingroup {
+	struct device_node *np;
+	const char *name;
+	int *gpins;
+	int ngpins;
+	struct list_head node;
+};
+
+/**
+ * struct pcs_func_vals - mux function register offset and value pair
+ * @reg:	register virtual address
+ * @val:	register value
+ */
+struct pcs_func_vals {
+	void __iomem *reg;
+	unsigned val;
+};
+
+/**
+ * struct pcs_function - pinctrl function
+ * @name:	pinctrl function name
+ * @vals:	register and vals array
+ * @nvals:	number of entries in vals array
+ * @pgnames:	array of pingroup names the function uses
+ * @npgnames:	number of pingroup names the function uses
+ * @node:	list node
+ */
+struct pcs_function {
+	const char *name;
+	struct pcs_func_vals *vals;
+	unsigned nvals;
+	const char **pgnames;
+	int npgnames;
+	struct list_head node;
+};
+
+/**
+ * struct pcs_data - wrapper for data needed by pinctrl framework
+ * @pa:		pindesc array
+ * @cur:	index to current element
+ *
+ * REVISIT: We should be able to drop this eventually by adding
+ * support for registering pins individually in the pinctrl
+ * framework for those drivers that don't need a static array.
+ */
+struct pcs_data {
+	struct pinctrl_pin_desc *pa;
+	int cur;
+};
+
+/**
+ * struct pcs_name - register name for a pin
+ * @name:	name of the pinctrl register
+ *
+ * REVISIT: We may want to make names optional in the pinctrl
+ * framework as some drivers may not care about pin names to
+ * avoid kernel bloat. The pin names can be deciphered by user
+ * space tools using debugfs based on the register address and
+ * SoC packaging information.
+ */
+struct pcs_name {
+	char name[PCS_REG_NAME_LEN];
+};
+
+/**
+ * struct pcs_device - pinctrl device instance
+ * @res:	resources
+ * @base:	virtual address of the controller
+ * @size:	size of the ioremapped area
+ * @dev:	device entry
+ * @pctl:	pin controller device
+ * @mutex:	mutex protecting the lists
+ * @width:	bits per mux register
+ * @fmask:	function register mask
+ * @fshift:	function register shift
+ * @foff:	value to turn mux off
+ * @fmax:	max number of functions in fmask
+ * @names:	array of register names for pins
+ * @pins:	physical pins on the SoC
+ * @pgtree:	pingroup index radix tree
+ * @ftree:	function index radix tree
+ * @pingroups:	list of pingroups
+ * @functions:	list of functions
+ * @ngroups:	number of pingroups
+ * @nfuncs:	number of functions
+ * @desc:	pin controller descriptor
+ * @read:	register read function to use
+ * @write:	register write function to use
+ */
+struct pcs_device {
+	struct resource *res;
+	void __iomem *base;
+	unsigned size;
+	struct device *dev;
+	struct pinctrl_dev *pctl;
+	struct mutex mutex;
+	unsigned width;
+	unsigned fmask;
+	unsigned fshift;
+	unsigned foff;
+	unsigned fmax;
+	struct pcs_name *names;
+	struct pcs_data pins;
+	struct radix_tree_root pgtree;
+	struct radix_tree_root ftree;
+	struct list_head pingroups;
+	struct list_head functions;
+	unsigned ngroups;
+	unsigned nfuncs;
+	struct pinctrl_desc desc;
+	unsigned (*read)(void __iomem *reg);
+	void (*write)(unsigned val, void __iomem *reg);
+};
+
+/*
+ * REVISIT: Reads and writes could eventually use regmap or something
+ * generic. But at least on omaps, some mux registers are performance
+ * critical as they may need to be remuxed every time before and after
+ * idle. Adding tests for register access width for every read and
+ * write like regmap is doing is not desired, and caching the registers
+ * does not help in this case.
+ */
+
+static unsigned __maybe_unused pcs_readb(void __iomem *reg)
+{
+	return readb(reg);
+}
+
+static unsigned __maybe_unused pcs_readw(void __iomem *reg)
+{
+	return readw(reg);
+}
+
+static unsigned __maybe_unused pcs_readl(void __iomem *reg)
+{
+	return readl(reg);
+}
+
+static void __maybe_unused pcs_writeb(unsigned val, void __iomem *reg)
+{
+	writeb(val, reg);
+}
+
+static void __maybe_unused pcs_writew(unsigned val, void __iomem *reg)
+{
+	writew(val, reg);
+}
+
+static void __maybe_unused pcs_writel(unsigned val, void __iomem *reg)
+{
+	writel(val, reg);
+}
+
+static int pcs_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct pcs_device *pcs;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+
+	return pcs->ngroups;
+}
+
+static const char *pcs_get_group_name(struct pinctrl_dev *pctldev,
+					unsigned gselector)
+{
+	struct pcs_device *pcs;
+	struct pcs_pingroup *group;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	group = radix_tree_lookup(&pcs->pgtree, gselector);
+	if (!group) {
+		dev_err(pcs->dev, "%s could not find pingroup%i\n",
+			__func__, gselector);
+		return NULL;
+	}
+
+	return group->name;
+}
+
+static int pcs_get_group_pins(struct pinctrl_dev *pctldev,
+					unsigned gselector,
+					const unsigned **pins,
+					unsigned *npins)
+{
+	struct pcs_device *pcs;
+	struct pcs_pingroup *group;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	group = radix_tree_lookup(&pcs->pgtree, gselector);
+	if (!group) {
+		dev_err(pcs->dev, "%s could not find pingroup%i\n",
+			__func__, gselector);
+		return -EINVAL;
+	}
+
+	*pins = group->gpins;
+	*npins = group->ngpins;
+
+	return 0;
+}
+
+static void pcs_pin_dbg_show(struct pinctrl_dev *pctldev,
+					struct seq_file *s,
+					unsigned offset)
+{
+	seq_printf(s, " " DRIVER_NAME);
+}
+
+static void pcs_dt_free_map(struct pinctrl_dev *pctldev,
+				struct pinctrl_map *map, unsigned num_maps)
+{
+	struct pcs_device *pcs;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	devm_kfree(pcs->dev, map);
+}
+
+static int pcs_dt_node_to_map(struct pinctrl_dev *pctldev,
+				struct device_node *np_config,
+				struct pinctrl_map **map, unsigned *num_maps);
+
+static struct pinctrl_ops pcs_pinctrl_ops = {
+	.get_groups_count = pcs_get_groups_count,
+	.get_group_name = pcs_get_group_name,
+	.get_group_pins = pcs_get_group_pins,
+	.pin_dbg_show = pcs_pin_dbg_show,
+	.dt_node_to_map = pcs_dt_node_to_map,
+	.dt_free_map = pcs_dt_free_map,
+};
+
+static int pcs_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	struct pcs_device *pcs;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+
+	return pcs->nfuncs;
+}
+
+static const char *pcs_get_function_name(struct pinctrl_dev *pctldev,
+						unsigned fselector)
+{
+	struct pcs_device *pcs;
+	struct pcs_function *func;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	func = radix_tree_lookup(&pcs->ftree, fselector);
+	if (!func) {
+		dev_err(pcs->dev, "%s could not find function%i\n",
+			__func__, fselector);
+		return NULL;
+	}
+
+	return func->name;
+}
+
+static int pcs_get_function_groups(struct pinctrl_dev *pctldev,
+					unsigned fselector,
+					const char * const **groups,
+					unsigned * const ngroups)
+{
+	struct pcs_device *pcs;
+	struct pcs_function *func;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	func = radix_tree_lookup(&pcs->ftree, fselector);
+	if (!func) {
+		dev_err(pcs->dev, "%s could not find function%i\n",
+			__func__, fselector);
+		return -EINVAL;
+	}
+	*groups = func->pgnames;
+	*ngroups = func->npgnames;
+
+	return 0;
+}
+
+static int pcs_enable(struct pinctrl_dev *pctldev, unsigned fselector,
+	unsigned group)
+{
+	struct pcs_device *pcs;
+	struct pcs_function *func;
+	int i;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	func = radix_tree_lookup(&pcs->ftree, fselector);
+	if (!func)
+		return -EINVAL;
+
+	dev_dbg(pcs->dev, "enabling %s function%i\n",
+		func->name, fselector);
+
+	for (i = 0; i < func->nvals; i++) {
+		struct pcs_func_vals *vals;
+		unsigned val;
+
+		vals = &func->vals[i];
+		val = pcs->read(vals->reg);
+		val &= ~pcs->fmask;
+		val |= vals->val;
+		pcs->write(val, vals->reg);
+	}
+
+	return 0;
+}
+
+static void pcs_disable(struct pinctrl_dev *pctldev, unsigned fselector,
+					unsigned group)
+{
+	struct pcs_device *pcs;
+	struct pcs_function *func;
+	int i;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+	func = radix_tree_lookup(&pcs->ftree, fselector);
+	if (!func) {
+		dev_err(pcs->dev, "%s could not find function%i\n",
+			__func__, fselector);
+		return;
+	}
+
+	/*
+	 * Ignore disable if function-off is not specified. Some hardware
+	 * does not have clearly defined disable function. For pin specific
+	 * off modes, you can use alternate named states as described in
+	 * pinctrl-bindings.txt.
+	 */
+	if (pcs->foff == PCS_OFF_DISABLED) {
+		dev_dbg(pcs->dev, "ignoring disable for %s function%i\n",
+			func->name, fselector);
+		return;
+	}
+
+	dev_dbg(pcs->dev, "disabling function%i %s\n",
+		fselector, func->name);
+
+	for (i = 0; i < func->nvals; i++) {
+		struct pcs_func_vals *vals;
+		unsigned val;
+
+		vals = &func->vals[i];
+		val = pcs->read(vals->reg);
+		val &= ~pcs->fmask;
+		val |= pcs->foff << pcs->fshift;
+		pcs->write(val, vals->reg);
+	}
+}
+
+static int pcs_request_gpio(struct pinctrl_dev *pctldev,
+			struct pinctrl_gpio_range *range, unsigned offset)
+{
+	return -ENOTSUPP;
+}
+
+static struct pinmux_ops pcs_pinmux_ops = {
+	.get_functions_count = pcs_get_functions_count,
+	.get_function_name = pcs_get_function_name,
+	.get_function_groups = pcs_get_function_groups,
+	.enable = pcs_enable,
+	.disable = pcs_disable,
+	.gpio_request_enable = pcs_request_gpio,
+};
+
+static int pcs_pinconf_get(struct pinctrl_dev *pctldev,
+				unsigned pin, unsigned long *config)
+{
+	return -ENOTSUPP;
+}
+
+static int pcs_pinconf_set(struct pinctrl_dev *pctldev,
+				unsigned pin, unsigned long config)
+{
+	return -ENOTSUPP;
+}
+
+static int pcs_pinconf_group_get(struct pinctrl_dev *pctldev,
+				unsigned group, unsigned long *config)
+{
+	return -ENOTSUPP;
+}
+
+static int pcs_pinconf_group_set(struct pinctrl_dev *pctldev,
+				unsigned group, unsigned long config)
+{
+	return -ENOTSUPP;
+}
+
+static void pcs_pinconf_dbg_show(struct pinctrl_dev *pctldev,
+				struct seq_file *s, unsigned offset)
+{
+}
+
+static void pcs_pinconf_group_dbg_show(struct pinctrl_dev *pctldev,
+				struct seq_file *s, unsigned selector)
+{
+}
+
+static struct pinconf_ops pcs_pinconf_ops = {
+	.pin_config_get = pcs_pinconf_get,
+	.pin_config_set = pcs_pinconf_set,
+	.pin_config_group_get = pcs_pinconf_group_get,
+	.pin_config_group_set = pcs_pinconf_group_set,
+	.pin_config_dbg_show = pcs_pinconf_dbg_show,
+	.pin_config_group_dbg_show = pcs_pinconf_group_dbg_show,
+};
+
+/**
+ * pcs_add_pin() - add a pin to the static per controller pin array
+ * @pcs: pcs driver instance
+ * @offset: register offset from base
+ */
+static int __devinit pcs_add_pin(struct pcs_device *pcs, unsigned offset)
+{
+	struct pinctrl_pin_desc *pin;
+	struct pcs_name *pn;
+	int i;
+
+	i = pcs->pins.cur;
+	if (i >= pcs->desc.npins) {
+		dev_err(pcs->dev, "too many pins, max %i\n",
+			pcs->desc.npins);
+		return -ENOMEM;
+	}
+
+	pin = &pcs->pins.pa[i];
+	pn = &pcs->names[i];
+	sprintf(pn->name, "%lx",
+		(unsigned long)pcs->res->start + offset);
+	pin->name = pn->name;
+	pin->number = i;
+	pcs->pins.cur++;
+
+	return i;
+}
+
+/**
+ * pcs_allocate_pin_table() - adds all the pins for the pinctrl driver
+ * @pcs: pcs driver instance
+ *
+ * In case of errors, resources are freed in pcs_free_resources.
+ *
+ * If your hardware needs holes in the address space, then just set
+ * up multiple driver instances.
+ */
+static int __devinit pcs_allocate_pin_table(struct pcs_device *pcs)
+{
+	int mux_bytes, nr_pins, i;
+
+	mux_bytes = pcs->width / BITS_PER_BYTE;
+	nr_pins = pcs->size / mux_bytes;
+
+	dev_dbg(pcs->dev, "allocating %i pins\n", nr_pins);
+	pcs->pins.pa = devm_kzalloc(pcs->dev,
+				sizeof(*pcs->pins.pa) * nr_pins,
+				GFP_KERNEL);
+	if (!pcs->pins.pa)
+		return -ENOMEM;
+
+	pcs->names = devm_kzalloc(pcs->dev,
+				sizeof(struct pcs_name) * nr_pins,
+				GFP_KERNEL);
+	if (!pcs->names)
+		return -ENOMEM;
+
+	pcs->desc.pins = pcs->pins.pa;
+	pcs->desc.npins = nr_pins;
+
+	for (i = 0; i < pcs->desc.npins; i++) {
+		unsigned offset;
+		int res;
+
+		offset = i * mux_bytes;
+		res = pcs_add_pin(pcs, offset);
+		if (res < 0) {
+			dev_err(pcs->dev, "error adding pins: %i\n", res);
+			return res;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * pcs_add_function() - adds a new function to the function list
+ * @pcs: pcs driver instance
+ * @np: device node of the mux entry
+ * @name: name of the function
+ * @vals: array of mux register value pairs used by the function
+ * @nvals: number of mux register value pairs
+ * @pgnames: array of pingroup names for the function
+ * @npgnames: number of pingroup names
+ */
+static struct pcs_function *pcs_add_function(struct pcs_device *pcs,
+					struct device_node *np,
+					const char *name,
+					struct pcs_func_vals *vals,
+					unsigned nvals,
+					const char **pgnames,
+					unsigned npgnames)
+{
+	struct pcs_function *function;
+
+	function = devm_kzalloc(pcs->dev, sizeof(*function), GFP_KERNEL);
+	if (!function)
+		return NULL;
+
+	function->name = name;
+	function->vals = vals;
+	function->nvals = nvals;
+	function->pgnames = pgnames;
+	function->npgnames = npgnames;
+
+	mutex_lock(&pcs->mutex);
+	list_add_tail(&function->node, &pcs->functions);
+	radix_tree_insert(&pcs->ftree, pcs->nfuncs, function);
+	pcs->nfuncs++;
+	mutex_unlock(&pcs->mutex);
+
+	return function;
+}
+
+static void pcs_remove_function(struct pcs_device *pcs,
+				struct pcs_function *function)
+{
+	int i;
+
+	mutex_lock(&pcs->mutex);
+	for (i = 0; i < pcs->nfuncs; i++) {
+		struct pcs_function *found;
+
+		found = radix_tree_lookup(&pcs->ftree, i);
+		if (found == function)
+			radix_tree_delete(&pcs->ftree, i);
+	}
+	list_del(&function->node);
+	mutex_unlock(&pcs->mutex);
+}
+
+/**
+ * pcs_add_pingroup() - add a pingroup to the pingroup list
+ * @pcs: pcs driver instance
+ * @np: device node of the mux entry
+ * @name: name of the pingroup
+ * @gpins: array of the pins that belong to the group
+ * @ngpins: number of pins in the group
+ */
+static int pcs_add_pingroup(struct pcs_device *pcs,
+					struct device_node *np,
+					const char *name,
+					int *gpins,
+					int ngpins)
+{
+	struct pcs_pingroup *pingroup;
+
+	pingroup = devm_kzalloc(pcs->dev, sizeof(*pingroup), GFP_KERNEL);
+	if (!pingroup)
+		return -ENOMEM;
+
+	pingroup->name = name;
+	pingroup->np = np;
+	pingroup->gpins = gpins;
+	pingroup->ngpins = ngpins;
+
+	mutex_lock(&pcs->mutex);
+	list_add_tail(&pingroup->node, &pcs->pingroups);
+	radix_tree_insert(&pcs->pgtree, pcs->ngroups, pingroup);
+	pcs->ngroups++;
+	mutex_unlock(&pcs->mutex);
+
+	return 0;
+}
+
+/**
+ * pcs_get_pin_by_offset() - get a pin index based on the register offset
+ * @pcs: pcs driver instance
+ * @offset: register offset from the base
+ *
+ * Note that this is OK as long as the pins are in a static array.
+ */
+static int pcs_get_pin_by_offset(struct pcs_device *pcs, unsigned offset)
+{
+	unsigned index;
+
+	if (offset >= pcs->size) {
+		dev_err(pcs->dev, "mux offset out of range: 0x%x (0x%x)\n",
+			offset, pcs->size);
+		return -EINVAL;
+	}
+
+	index = offset / (pcs->width / BITS_PER_BYTE);
+
+	return index;
+}
+
+/**
+ * smux_parse_one_pinctrl_entry() - parses a device tree mux entry
+ * @pcs: pinctrl driver instance
+ * @np: device node of the mux entry
+ * @map: map entry
+ * @pgnames: pingroup names
+ *
+ * Note that this binding currently supports only sets of one register + value.
+ *
+ * Also note that this driver tries to avoid understanding pin and function
+ * names because of the extra bloat they would cause especially in the case of
+ * a large number of pins. This driver just sets what is specified for the board
+ * in the .dts file. Further user space debugging tools can be developed to
+ * decipher the pin and function names using debugfs.
+ *
+ * If you are concerned about the boot time, set up the static pins in
+ * the bootloader, and only set up selected pins as device tree entries.
+ */
+static int pcs_parse_one_pinctrl_entry(struct pcs_device *pcs,
+						struct device_node *np,
+						struct pinctrl_map **map,
+						const char **pgnames)
+{
+	struct pcs_func_vals *vals;
+	const __be32 *mux;
+	int size, rows, *pins, index = 0, found = 0, res = -ENOMEM;
+	struct pcs_function *function;
+
+	mux = of_get_property(np, PCS_MUX_NAME, &size);
+	if ((!mux) || (size < sizeof(*mux) * 2)) {
+		dev_err(pcs->dev, "bad data for mux %s\n",
+			np->name);
+		return -EINVAL;
+	}
+
+	size /= sizeof(*mux);	/* Number of elements in array */
+	rows = size / 2;	/* Each row is a key value pair */
+
+	vals = devm_kzalloc(pcs->dev, sizeof(*vals) * rows, GFP_KERNEL);
+	if (!vals)
+		return -ENOMEM;
+
+	pins = devm_kzalloc(pcs->dev, sizeof(*pins) * rows, GFP_KERNEL);
+	if (!pins)
+		goto free_vals;
+
+	while (index < size) {
+		unsigned offset, val;
+		int pin;
+
+		offset = be32_to_cpup(mux + index++);
+		val = be32_to_cpup(mux + index++);
+		vals[found].reg = pcs->base + offset;
+		vals[found].val = val;
+
+		pin = pcs_get_pin_by_offset(pcs, offset);
+		if (pin < 0) {
+			dev_err(pcs->dev,
+				"could not add functions for %s %ux\n",
+				np->name, offset);
+			break;
+		}
+		pins[found++] = pin;
+	}
+
+	pgnames[0] = np->name;
+	function = pcs_add_function(pcs, np, np->name, vals, found, pgnames, 1);
+	if (!function)
+		goto free_pins;
+
+	res = pcs_add_pingroup(pcs, np, np->name, pins, found);
+	if (res < 0)
+		goto free_function;
+
+	(*map)->type = PIN_MAP_TYPE_MUX_GROUP;
+	(*map)->data.mux.group = np->name;
+	(*map)->data.mux.function = np->name;
+
+	return 0;
+
+free_function:
+	pcs_remove_function(pcs, function);
+
+free_pins:
+	devm_kfree(pcs->dev, pins);
+
+free_vals:
+	devm_kfree(pcs->dev, vals);
+
+	return res;
+}
+/**
+ * pcs_dt_node_to_map() - allocates and parses pinctrl maps
+ * @pctldev: pinctrl instance
+ * @np_config: device tree pinmux entry
+ * @map: array of map entries
+ * @num_maps: number of maps
+ */
+static int pcs_dt_node_to_map(struct pinctrl_dev *pctldev,
+				struct device_node *np_config,
+				struct pinctrl_map **map, unsigned *num_maps)
+{
+	struct pcs_device *pcs;
+	const char **pgnames;
+	int ret;
+
+	pcs = pinctrl_dev_get_drvdata(pctldev);
+
+	*map = devm_kzalloc(pcs->dev, sizeof(**map), GFP_KERNEL);
+	if (!map)
+		return -ENOMEM;
+
+	*num_maps = 0;
+
+	pgnames = devm_kzalloc(pcs->dev, sizeof(*pgnames), GFP_KERNEL);
+	if (!pgnames) {
+		ret = -ENOMEM;
+		goto free_map;
+	}
+
+	ret = pcs_parse_one_pinctrl_entry(pcs, np_config, map, pgnames);
+	if (ret < 0) {
+		dev_err(pcs->dev, "no pins entries for %s\n",
+			np_config->name);
+		goto free_pgnames;
+	}
+	*num_maps = 1;
+
+	return 0;
+
+free_pgnames:
+	devm_kfree(pcs->dev, pgnames);
+free_map:
+	devm_kfree(pcs->dev, *map);
+
+	return ret;
+}
+
+/**
+ * pcs_free_funcs() - free memory used by functions
+ * @pcs: pcs driver instance
+ */
+static void pcs_free_funcs(struct pcs_device *pcs)
+{
+	struct list_head *pos, *tmp;
+	int i;
+
+	mutex_lock(&pcs->mutex);
+	for (i = 0; i < pcs->nfuncs; i++) {
+		struct pcs_function *func;
+
+		func = radix_tree_lookup(&pcs->ftree, i);
+		if (!func)
+			continue;
+		radix_tree_delete(&pcs->ftree, i);
+	}
+	list_for_each_safe(pos, tmp, &pcs->functions) {
+		struct pcs_function *function;
+
+		function = list_entry(pos, struct pcs_function, node);
+		list_del(&function->node);
+	}
+	mutex_unlock(&pcs->mutex);
+}
+
+/**
+ * pcs_free_pingroups() - free memory used by pingroups
+ * @pcs: pcs driver instance
+ */
+static void pcs_free_pingroups(struct pcs_device *pcs)
+{
+	struct list_head *pos, *tmp;
+	int i;
+
+	mutex_lock(&pcs->mutex);
+	for (i = 0; i < pcs->ngroups; i++) {
+		struct pcs_pingroup *pingroup;
+
+		pingroup = radix_tree_lookup(&pcs->pgtree, i);
+		if (!pingroup)
+			continue;
+		radix_tree_delete(&pcs->pgtree, i);
+	}
+	list_for_each_safe(pos, tmp, &pcs->pingroups) {
+		struct pcs_pingroup *pingroup;
+
+		pingroup = list_entry(pos, struct pcs_pingroup, node);
+		list_del(&pingroup->node);
+	}
+	mutex_unlock(&pcs->mutex);
+}
+
+/**
+ * pcs_free_resources() - free memory used by this driver
+ * @pcs: pcs driver instance
+ */
+static void pcs_free_resources(struct pcs_device *pcs)
+{
+	if (pcs->pctl)
+		pinctrl_unregister(pcs->pctl);
+
+	pcs_free_funcs(pcs);
+	pcs_free_pingroups(pcs);
+}
+
+#define PCS_GET_PROP_U32(name, reg, err)				\
+	do {								\
+		ret = of_property_read_u32(np, name, reg);		\
+		if (ret) {						\
+			dev_err(pcs->dev, err);				\
+			return ret;					\
+		}							\
+	} while (0);
+
+static struct of_device_id pcs_of_match[];
+
+static int __devinit pcs_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	struct resource *res;
+	struct pcs_device *pcs;
+	int ret;
+
+	match = of_match_device(pcs_of_match, &pdev->dev);
+	if (!match)
+		return -EINVAL;
+
+	pcs = devm_kzalloc(&pdev->dev, sizeof(*pcs), GFP_KERNEL);
+	if (!pcs) {
+		dev_err(&pdev->dev, "could not allocate\n");
+		return -ENOMEM;
+	}
+	pcs->dev = &pdev->dev;
+	mutex_init(&pcs->mutex);
+	INIT_LIST_HEAD(&pcs->pingroups);
+	INIT_LIST_HEAD(&pcs->functions);
+
+	PCS_GET_PROP_U32("pinctrl-single,register-width", &pcs->width,
+			 "register width not specified\n");
+
+	PCS_GET_PROP_U32("pinctrl-single,function-mask", &pcs->fmask,
+			 "function register mask not specified\n");
+	pcs->fshift = ffs(pcs->fmask) - 1;
+	pcs->fmax = pcs->fmask >> pcs->fshift;
+
+	ret = of_property_read_u32(np, "pinctrl-single,function-off",
+					&pcs->foff);
+	if (ret)
+		pcs->foff = PCS_OFF_DISABLED;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(pcs->dev, "could not get resource\n");
+		return -ENODEV;
+	}
+
+	pcs->res = devm_request_mem_region(pcs->dev, res->start,
+			resource_size(res), DRIVER_NAME);
+	if (!pcs->res) {
+		dev_err(pcs->dev, "could not get mem_region\n");
+		return -EBUSY;
+	}
+
+	pcs->size = resource_size(pcs->res);
+	pcs->base = devm_ioremap(pcs->dev, pcs->res->start, pcs->size);
+	if (!pcs->base) {
+		dev_err(pcs->dev, "could not ioremap\n");
+		return -ENODEV;
+	}
+
+	INIT_RADIX_TREE(&pcs->pgtree, GFP_KERNEL);
+	INIT_RADIX_TREE(&pcs->ftree, GFP_KERNEL);
+	platform_set_drvdata(pdev, pcs);
+
+	switch (pcs->width) {
+	case 8:
+		pcs->read = pcs_readb;
+		pcs->write = pcs_writeb;
+		break;
+	case 16:
+		pcs->read = pcs_readw;
+		pcs->write = pcs_writew;
+		break;
+	case 32:
+		pcs->read = pcs_readl;
+		pcs->write = pcs_writel;
+		break;
+	default:
+		break;
+	}
+
+	pcs->desc.name = DRIVER_NAME;
+	pcs->desc.pctlops = &pcs_pinctrl_ops;
+	pcs->desc.pmxops = &pcs_pinmux_ops;
+	pcs->desc.confops = &pcs_pinconf_ops;
+	pcs->desc.owner = THIS_MODULE;
+
+	ret = pcs_allocate_pin_table(pcs);
+	if (ret < 0)
+		goto free;
+
+	pcs->pctl = pinctrl_register(&pcs->desc, pcs->dev, pcs);
+	if (!pcs->pctl) {
+		dev_err(pcs->dev, "could not register single pinctrl driver\n");
+		ret = -EINVAL;
+		goto free;
+	}
+
+	dev_info(pcs->dev, "%i pins at pa %p size %u\n",
+		 pcs->desc.npins, pcs->base, pcs->size);
+
+	return 0;
+
+free:
+	pcs_free_resources(pcs);
+
+	return ret;
+}
+
+static int __devexit pcs_remove(struct platform_device *pdev)
+{
+	struct pcs_device *pcs = platform_get_drvdata(pdev);
+
+	if (!pcs)
+		return 0;
+
+	pcs_free_resources(pcs);
+
+	return 0;
+}
+
+static struct of_device_id pcs_of_match[] __devinitdata = {
+	{ .compatible = DRIVER_NAME, },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, pcs_of_match);
+
+static struct platform_driver pcs_driver = {
+	.probe		= pcs_probe,
+	.remove		= __devexit_p(pcs_remove),
+	.driver = {
+		.owner		= THIS_MODULE,
+		.name		= DRIVER_NAME,
+		.of_match_table	= pcs_of_match,
+	},
+};
+
+module_platform_driver(pcs_driver);
+
+MODULE_AUTHOR("Tony Lindgren <tony@atomide.com>");
+MODULE_DESCRIPTION("One-register-per-pin type device tree based pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-sirf.c b/drivers/pinctrl/pinctrl-sirf.c
index e9f8e7d1100..2aae8a8978e 100644
--- a/drivers/pinctrl/pinctrl-sirf.c
+++ b/drivers/pinctrl/pinctrl-sirf.c
@@ -8,24 +8,61 @@
 
 #include <linux/init.h>
 #include <linux/module.h>
+#include <linux/irq.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/irqdomain.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
 #include <linux/bitops.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
 
 #define DRIVER_NAME "pinmux-sirf"
 
 #define SIRFSOC_NUM_PADS    622
-#define SIRFSOC_GPIO_PAD_EN(g) ((g)*0x100 + 0x84)
 #define SIRFSOC_RSC_PIN_MUX 0x4
 
+#define SIRFSOC_GPIO_PAD_EN(g) ((g)*0x100 + 0x84)
+#define SIRFSOC_GPIO_CTRL(g, i)			((g)*0x100 + (i)*4)
+#define SIRFSOC_GPIO_DSP_EN0			(0x80)
+#define SIRFSOC_GPIO_PAD_EN(g)			((g)*0x100 + 0x84)
+#define SIRFSOC_GPIO_INT_STATUS(g)		((g)*0x100 + 0x8C)
+
+#define SIRFSOC_GPIO_CTL_INTR_LOW_MASK		0x1
+#define SIRFSOC_GPIO_CTL_INTR_HIGH_MASK		0x2
+#define SIRFSOC_GPIO_CTL_INTR_TYPE_MASK		0x4
+#define SIRFSOC_GPIO_CTL_INTR_EN_MASK		0x8
+#define SIRFSOC_GPIO_CTL_INTR_STS_MASK		0x10
+#define SIRFSOC_GPIO_CTL_OUT_EN_MASK		0x20
+#define SIRFSOC_GPIO_CTL_DATAOUT_MASK		0x40
+#define SIRFSOC_GPIO_CTL_DATAIN_MASK		0x80
+#define SIRFSOC_GPIO_CTL_PULL_MASK		0x100
+#define SIRFSOC_GPIO_CTL_PULL_HIGH		0x200
+#define SIRFSOC_GPIO_CTL_DSP_INT		0x400
+
+#define SIRFSOC_GPIO_NO_OF_BANKS        5
+#define SIRFSOC_GPIO_BANK_SIZE          32
+#define SIRFSOC_GPIO_NUM(bank, index)	(((bank)*(32)) + (index))
+
+struct sirfsoc_gpio_bank {
+	struct of_mm_gpio_chip chip;
+	struct irq_domain *domain;
+	int id;
+	int parent_irq;
+	spinlock_t lock;
+};
+
+static struct sirfsoc_gpio_bank sgpio_bank[SIRFSOC_GPIO_NO_OF_BANKS];
+static DEFINE_SPINLOCK(sgpio_lock);
+
 /*
  * pad list for the pinmux subsystem
  * refer to CS-131858-DC-6A.xls
@@ -1204,7 +1241,457 @@ static int __init sirfsoc_pinmux_init(void)
 }
 arch_initcall(sirfsoc_pinmux_init);
 
+static inline int sirfsoc_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	struct sirfsoc_gpio_bank *bank = container_of(to_of_mm_gpio_chip(chip),
+		struct sirfsoc_gpio_bank, chip);
+
+	return irq_find_mapping(bank->domain, offset);
+}
+
+static inline int sirfsoc_gpio_to_offset(unsigned int gpio)
+{
+	return gpio % SIRFSOC_GPIO_BANK_SIZE;
+}
+
+static inline struct sirfsoc_gpio_bank *sirfsoc_gpio_to_bank(unsigned int gpio)
+{
+	return &sgpio_bank[gpio / SIRFSOC_GPIO_BANK_SIZE];
+}
+
+void sirfsoc_gpio_set_pull(unsigned gpio, unsigned mode)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(gpio);
+	int idx = sirfsoc_gpio_to_offset(gpio);
+	u32 val, offset;
+	unsigned long flags;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&sgpio_lock, flags);
+
+	val = readl(bank->chip.regs + offset);
+
+	switch (mode) {
+	case SIRFSOC_GPIO_PULL_NONE:
+		val &= ~SIRFSOC_GPIO_CTL_PULL_MASK;
+		break;
+	case SIRFSOC_GPIO_PULL_UP:
+		val |= SIRFSOC_GPIO_CTL_PULL_MASK;
+		val |= SIRFSOC_GPIO_CTL_PULL_HIGH;
+		break;
+	case SIRFSOC_GPIO_PULL_DOWN:
+		val |= SIRFSOC_GPIO_CTL_PULL_MASK;
+		val &= ~SIRFSOC_GPIO_CTL_PULL_HIGH;
+		break;
+	default:
+		break;
+	}
+
+	writel(val, bank->chip.regs + offset);
+
+	spin_unlock_irqrestore(&sgpio_lock, flags);
+}
+EXPORT_SYMBOL(sirfsoc_gpio_set_pull);
+
+static inline struct sirfsoc_gpio_bank *sirfsoc_irqchip_to_bank(struct gpio_chip *chip)
+{
+	return container_of(to_of_mm_gpio_chip(chip), struct sirfsoc_gpio_bank, chip);
+}
+
+static void sirfsoc_gpio_irq_ack(struct irq_data *d)
+{
+	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	u32 val, offset;
+	unsigned long flags;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&sgpio_lock, flags);
+
+	val = readl(bank->chip.regs + offset);
+
+	writel(val, bank->chip.regs + offset);
+
+	spin_unlock_irqrestore(&sgpio_lock, flags);
+}
+
+static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_bank *bank, int idx)
+{
+	u32 val, offset;
+	unsigned long flags;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&sgpio_lock, flags);
+
+	val = readl(bank->chip.regs + offset);
+	val &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
+	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
+	writel(val, bank->chip.regs + offset);
+
+	spin_unlock_irqrestore(&sgpio_lock, flags);
+}
+
+static void sirfsoc_gpio_irq_mask(struct irq_data *d)
+{
+	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+
+	__sirfsoc_gpio_irq_mask(bank, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
+}
+
+static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
+{
+	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	u32 val, offset;
+	unsigned long flags;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&sgpio_lock, flags);
+
+	val = readl(bank->chip.regs + offset);
+	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
+	val |= SIRFSOC_GPIO_CTL_INTR_EN_MASK;
+	writel(val, bank->chip.regs + offset);
+
+	spin_unlock_irqrestore(&sgpio_lock, flags);
+}
+
+static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
+{
+	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	u32 val, offset;
+	unsigned long flags;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&sgpio_lock, flags);
+
+	val = readl(bank->chip.regs + offset);
+	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
+
+	switch (type) {
+	case IRQ_TYPE_NONE:
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
+		val &= ~SIRFSOC_GPIO_CTL_INTR_LOW_MASK;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		val &= ~SIRFSOC_GPIO_CTL_INTR_HIGH_MASK;
+		val |= SIRFSOC_GPIO_CTL_INTR_LOW_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK | SIRFSOC_GPIO_CTL_INTR_LOW_MASK |
+			 SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		val &= ~(SIRFSOC_GPIO_CTL_INTR_HIGH_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK);
+		val |= SIRFSOC_GPIO_CTL_INTR_LOW_MASK;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK;
+		val &= ~(SIRFSOC_GPIO_CTL_INTR_LOW_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK);
+		break;
+	}
+
+	writel(val, bank->chip.regs + offset);
+
+	spin_unlock_irqrestore(&sgpio_lock, flags);
+
+	return 0;
+}
+
+static struct irq_chip sirfsoc_irq_chip = {
+	.name = "sirf-gpio-irq",
+	.irq_ack = sirfsoc_gpio_irq_ack,
+	.irq_mask = sirfsoc_gpio_irq_mask,
+	.irq_unmask = sirfsoc_gpio_irq_unmask,
+	.irq_set_type = sirfsoc_gpio_irq_type,
+};
+
+static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
+{
+	struct sirfsoc_gpio_bank *bank = irq_get_handler_data(irq);
+	u32 status, ctrl;
+	int idx = 0;
+	unsigned int first_irq;
+
+	status = readl(bank->chip.regs + SIRFSOC_GPIO_INT_STATUS(bank->id));
+	if (!status) {
+		printk(KERN_WARNING
+			"%s: gpio id %d status %#x no interrupt is flaged\n",
+			__func__, bank->id, status);
+		handle_bad_irq(irq, desc);
+		return;
+	}
+
+	first_irq = bank->domain->revmap_data.legacy.first_irq;
+
+	while (status) {
+		ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, idx));
+
+		/*
+		 * Here we must check whether the corresponding GPIO's interrupt
+		 * has been enabled, otherwise just skip it
+		 */
+		if ((status & 0x1) && (ctrl & SIRFSOC_GPIO_CTL_INTR_EN_MASK)) {
+			pr_debug("%s: gpio id %d idx %d happens\n",
+				__func__, bank->id, idx);
+			generic_handle_irq(first_irq + idx);
+		}
+
+		idx++;
+		status = status >> 1;
+	}
+}
+
+static inline void sirfsoc_gpio_set_input(struct sirfsoc_gpio_bank *bank, unsigned ctrl_offset)
+{
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	val = readl(bank->chip.regs + ctrl_offset);
+	val &= ~SIRFSOC_GPIO_CTL_OUT_EN_MASK;
+	writel(val, bank->chip.regs + ctrl_offset);
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+}
+
+static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	unsigned long flags;
+
+	if (pinctrl_request_gpio(chip->base + offset))
+		return -ENODEV;
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	/*
+	 * default status:
+	 * set direction as input and mask irq
+	 */
+	sirfsoc_gpio_set_input(bank, SIRFSOC_GPIO_CTRL(bank->id, offset));
+	__sirfsoc_gpio_irq_mask(bank, offset);
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+
+	return 0;
+}
+
+static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	unsigned long flags;
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	__sirfsoc_gpio_irq_mask(bank, offset);
+	sirfsoc_gpio_set_input(bank, SIRFSOC_GPIO_CTRL(bank->id, offset));
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int sirfsoc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	int idx = sirfsoc_gpio_to_offset(gpio);
+	unsigned long flags;
+	unsigned offset;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	sirfsoc_gpio_set_input(bank, offset);
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+
+	return 0;
+}
+
+static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_bank *bank, unsigned offset,
+	int value)
+{
+	u32 out_ctrl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	out_ctrl = readl(bank->chip.regs + offset);
+	if (value)
+		out_ctrl |= SIRFSOC_GPIO_CTL_DATAOUT_MASK;
+	else
+		out_ctrl &= ~SIRFSOC_GPIO_CTL_DATAOUT_MASK;
+
+	out_ctrl &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
+	out_ctrl |= SIRFSOC_GPIO_CTL_OUT_EN_MASK;
+	writel(out_ctrl, bank->chip.regs + offset);
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+}
+
+static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio, int value)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	int idx = sirfsoc_gpio_to_offset(gpio);
+	u32 offset;
+	unsigned long flags;
+
+	offset = SIRFSOC_GPIO_CTRL(bank->id, idx);
+
+	spin_lock_irqsave(&sgpio_lock, flags);
+
+	sirfsoc_gpio_set_output(bank, offset, value);
+
+	spin_unlock_irqrestore(&sgpio_lock, flags);
+
+	return 0;
+}
+
+static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	val = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+
+	return !!(val & SIRFSOC_GPIO_CTL_DATAIN_MASK);
+}
+
+static void sirfsoc_gpio_set_value(struct gpio_chip *chip, unsigned offset,
+	int value)
+{
+	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	u32 ctrl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bank->lock, flags);
+
+	ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	if (value)
+		ctrl |= SIRFSOC_GPIO_CTL_DATAOUT_MASK;
+	else
+		ctrl &= ~SIRFSOC_GPIO_CTL_DATAOUT_MASK;
+	writel(ctrl, bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+
+	spin_unlock_irqrestore(&bank->lock, flags);
+}
+
+int sirfsoc_gpio_irq_map(struct irq_domain *d, unsigned int irq,
+	irq_hw_number_t hwirq)
+{
+	struct sirfsoc_gpio_bank *bank = d->host_data;
+
+	if (!bank)
+		return -EINVAL;
+
+	irq_set_chip(irq, &sirfsoc_irq_chip);
+	irq_set_handler(irq, handle_level_irq);
+	irq_set_chip_data(irq, bank);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+const struct irq_domain_ops sirfsoc_gpio_irq_simple_ops = {
+	.map = sirfsoc_gpio_irq_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
+static int __devinit sirfsoc_gpio_probe(struct device_node *np)
+{
+	int i, err = 0;
+	struct sirfsoc_gpio_bank *bank;
+	void *regs;
+	struct platform_device *pdev;
+
+	pdev = of_find_device_by_node(np);
+	if (!pdev)
+		return -ENODEV;
+
+	regs = of_iomap(np, 0);
+	if (!regs)
+		return -ENOMEM;
+
+	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
+		bank = &sgpio_bank[i];
+		spin_lock_init(&bank->lock);
+		bank->chip.gc.request = sirfsoc_gpio_request;
+		bank->chip.gc.free = sirfsoc_gpio_free;
+		bank->chip.gc.direction_input = sirfsoc_gpio_direction_input;
+		bank->chip.gc.get = sirfsoc_gpio_get_value;
+		bank->chip.gc.direction_output = sirfsoc_gpio_direction_output;
+		bank->chip.gc.set = sirfsoc_gpio_set_value;
+		bank->chip.gc.to_irq = sirfsoc_gpio_to_irq;
+		bank->chip.gc.base = i * SIRFSOC_GPIO_BANK_SIZE;
+		bank->chip.gc.ngpio = SIRFSOC_GPIO_BANK_SIZE;
+		bank->chip.gc.label = kstrdup(np->full_name, GFP_KERNEL);
+		bank->chip.gc.of_node = np;
+		bank->chip.regs = regs;
+		bank->id = i;
+		bank->parent_irq = platform_get_irq(pdev, i);
+		if (bank->parent_irq < 0) {
+			err = bank->parent_irq;
+			goto out;
+		}
+
+		err = gpiochip_add(&bank->chip.gc);
+		if (err) {
+			pr_err("%s: error in probe function with status %d\n",
+				np->full_name, err);
+			goto out;
+		}
+
+		bank->domain = irq_domain_add_legacy(np, SIRFSOC_GPIO_BANK_SIZE,
+			SIRFSOC_GPIO_IRQ_START + i * SIRFSOC_GPIO_BANK_SIZE, 0,
+			&sirfsoc_gpio_irq_simple_ops, bank);
+
+		if (!bank->domain) {
+			pr_err("%s: Failed to create irqdomain\n", np->full_name);
+			err = -ENOSYS;
+			goto out;
+		}
+
+		irq_set_chained_handler(bank->parent_irq, sirfsoc_gpio_handle_irq);
+		irq_set_handler_data(bank->parent_irq, bank);
+	}
+
+out:
+	iounmap(regs);
+	return err;
+}
+
+static int __init sirfsoc_gpio_init(void)
+{
+
+	struct device_node *np;
+
+	np = of_find_matching_node(NULL, pinmux_ids);
+
+	if (!np)
+		return -ENODEV;
+
+	return sirfsoc_gpio_probe(np);
+}
+subsys_initcall(sirfsoc_gpio_init);
+
 MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>, "
+	"Yuping Luo <yuping.luo@csr.com>, "
 	"Barry Song <baohua.song@csr.com>");
 MODULE_DESCRIPTION("SIRFSOC pin control driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index b6934867d8d..ae52e4e5d09 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -745,9 +745,9 @@ int __devinit tegra_pinctrl_probe(struct platform_device *pdev,
 	}
 
 	pmx->pctl = pinctrl_register(&tegra_pinctrl_desc, &pdev->dev, pmx);
-	if (IS_ERR(pmx->pctl)) {
+	if (!pmx->pctl) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return PTR_ERR(pmx->pctl);
+		return -ENODEV;
 	}
 
 	pinctrl_add_gpio_range(pmx->pctl, &tegra_pinctrl_gpio_range);
@@ -764,7 +764,6 @@ int __devexit tegra_pinctrl_remove(struct platform_device *pdev)
 {
 	struct tegra_pmx *pmx = platform_get_drvdata(pdev);
 
-	pinctrl_remove_gpio_range(pmx->pctl, &tegra_pinctrl_gpio_range);
 	pinctrl_unregister(pmx->pctl);
 
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-u300.c b/drivers/pinctrl/pinctrl-u300.c
index 05d029911be..a7ad8c112d9 100644
--- a/drivers/pinctrl/pinctrl-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -1113,8 +1113,6 @@ static int __devinit u300_pmx_probe(struct platform_device *pdev)
 	int ret;
 	int i;
 
-	pr_err("U300 PMX PROBE\n");
-
 	/* Create state holders etc for this driver */
 	upmx = devm_kzalloc(&pdev->dev, sizeof(*upmx), GFP_KERNEL);
 	if (!upmx)
@@ -1175,15 +1173,11 @@ out_no_resource:
 static int __devexit u300_pmx_remove(struct platform_device *pdev)
 {
 	struct u300_pmx *upmx = platform_get_drvdata(pdev);
-	int i;
 
-	for (i = 0; i < ARRAY_SIZE(u300_gpio_ranges); i++)
-		pinctrl_remove_gpio_range(upmx->pctl, &u300_gpio_ranges[i]);
 	pinctrl_unregister(upmx->pctl);
 	iounmap(upmx->virtbase);
 	release_mem_region(upmx->phybase, upmx->physize);
 	platform_set_drvdata(pdev, NULL);
-	devm_kfree(&pdev->dev, upmx);
 
 	return 0;
 }
diff --git a/drivers/pinctrl/spear/pinctrl-spear.c b/drivers/pinctrl/spear/pinctrl-spear.c
index b3f6b2873fd..5d4f44f462f 100644
--- a/drivers/pinctrl/spear/pinctrl-spear.c
+++ b/drivers/pinctrl/spear/pinctrl-spear.c
@@ -336,9 +336,9 @@ int __devinit spear_pinctrl_probe(struct platform_device *pdev,
 	spear_pinctrl_desc.npins = machdata->npins;
 
 	pmx->pctl = pinctrl_register(&spear_pinctrl_desc, &pdev->dev, pmx);
-	if (IS_ERR(pmx->pctl)) {
+	if (!pmx->pctl) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return PTR_ERR(pmx->pctl);
+		return -ENODEV;
 	}
 
 	return 0;
diff --git a/drivers/platform/Makefile b/drivers/platform/Makefile
index 782953ae4c0..b17c16ce54a 100644
--- a/drivers/platform/Makefile
+++ b/drivers/platform/Makefile
@@ -3,3 +3,4 @@
 #
 
 obj-$(CONFIG_X86)		+= x86/
+obj-$(CONFIG_OLPC)		+= olpc/
diff --git a/drivers/platform/olpc/Makefile b/drivers/platform/olpc/Makefile
new file mode 100644
index 00000000000..dc8b26bc720
--- /dev/null
+++ b/drivers/platform/olpc/Makefile
@@ -0,0 +1,4 @@
+#
+# OLPC XO platform-specific drivers
+#
+obj-$(CONFIG_OLPC)		+= olpc-ec.o
diff --git a/drivers/platform/olpc/olpc-ec.c b/drivers/platform/olpc/olpc-ec.c
new file mode 100644
index 00000000000..0f9f8596b30
--- /dev/null
+++ b/drivers/platform/olpc/olpc-ec.c
@@ -0,0 +1,336 @@
+/*
+ * Generic driver for the OLPC Embedded Controller.
+ *
+ * Copyright (C) 2011-2012 One Laptop per Child Foundation.
+ *
+ * Licensed under the GPL v2 or later.
+ */
+#include <linux/completion.h>
+#include <linux/debugfs.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/olpc-ec.h>
+#include <asm/olpc.h>
+
+struct ec_cmd_desc {
+	u8 cmd;
+	u8 *inbuf, *outbuf;
+	size_t inlen, outlen;
+
+	int err;
+	struct completion finished;
+	struct list_head node;
+
+	void *priv;
+};
+
+struct olpc_ec_priv {
+	struct olpc_ec_driver *drv;
+	struct work_struct worker;
+	struct mutex cmd_lock;
+
+	/* Pending EC commands */
+	struct list_head cmd_q;
+	spinlock_t cmd_q_lock;
+
+	struct dentry *dbgfs_dir;
+
+	/*
+	 * Running an EC command while suspending means we don't always finish
+	 * the command before the machine suspends.  This means that the EC
+	 * is expecting the command protocol to finish, but we after a period
+	 * of time (while the OS is asleep) the EC times out and restarts its
+	 * idle loop.  Meanwhile, the OS wakes up, thinks it's still in the
+	 * middle of the command protocol, starts throwing random things at
+	 * the EC... and everyone's uphappy.
+	 */
+	bool suspended;
+};
+
+static struct olpc_ec_driver *ec_driver;
+static struct olpc_ec_priv *ec_priv;
+static void *ec_cb_arg;
+
+void olpc_ec_driver_register(struct olpc_ec_driver *drv, void *arg)
+{
+	ec_driver = drv;
+	ec_cb_arg = arg;
+}
+EXPORT_SYMBOL_GPL(olpc_ec_driver_register);
+
+static void olpc_ec_worker(struct work_struct *w)
+{
+	struct olpc_ec_priv *ec = container_of(w, struct olpc_ec_priv, worker);
+	struct ec_cmd_desc *desc = NULL;
+	unsigned long flags;
+
+	/* Grab the first pending command from the queue */
+	spin_lock_irqsave(&ec->cmd_q_lock, flags);
+	if (!list_empty(&ec->cmd_q)) {
+		desc = list_first_entry(&ec->cmd_q, struct ec_cmd_desc, node);
+		list_del(&desc->node);
+	}
+	spin_unlock_irqrestore(&ec->cmd_q_lock, flags);
+
+	/* Do we actually have anything to do? */
+	if (!desc)
+		return;
+
+	/* Protect the EC hw with a mutex; only run one cmd at a time */
+	mutex_lock(&ec->cmd_lock);
+	desc->err = ec_driver->ec_cmd(desc->cmd, desc->inbuf, desc->inlen,
+			desc->outbuf, desc->outlen, ec_cb_arg);
+	mutex_unlock(&ec->cmd_lock);
+
+	/* Finished, wake up olpc_ec_cmd() */
+	complete(&desc->finished);
+
+	/* Run the worker thread again in case there are more cmds pending */
+	schedule_work(&ec->worker);
+}
+
+/*
+ * Throw a cmd descripter onto the list.  We now have SMP OLPC machines, so
+ * locking is pretty critical.
+ */
+static void queue_ec_descriptor(struct ec_cmd_desc *desc,
+		struct olpc_ec_priv *ec)
+{
+	unsigned long flags;
+
+	INIT_LIST_HEAD(&desc->node);
+
+	spin_lock_irqsave(&ec->cmd_q_lock, flags);
+	list_add_tail(&desc->node, &ec->cmd_q);
+	spin_unlock_irqrestore(&ec->cmd_q_lock, flags);
+
+	schedule_work(&ec->worker);
+}
+
+int olpc_ec_cmd(u8 cmd, u8 *inbuf, size_t inlen, u8 *outbuf, size_t outlen)
+{
+	struct olpc_ec_priv *ec = ec_priv;
+	struct ec_cmd_desc desc;
+
+	/* Ensure a driver and ec hook have been registered */
+	if (WARN_ON(!ec_driver || !ec_driver->ec_cmd))
+		return -ENODEV;
+
+	if (!ec)
+		return -ENOMEM;
+
+	/* Suspending in the middle of a command hoses things really badly */
+	if (WARN_ON(ec->suspended))
+		return -EBUSY;
+
+	might_sleep();
+
+	desc.cmd = cmd;
+	desc.inbuf = inbuf;
+	desc.outbuf = outbuf;
+	desc.inlen = inlen;
+	desc.outlen = outlen;
+	desc.err = 0;
+	init_completion(&desc.finished);
+
+	queue_ec_descriptor(&desc, ec);
+
+	/* Timeouts must be handled in the platform-specific EC hook */
+	wait_for_completion(&desc.finished);
+
+	/* The worker thread dequeues the cmd; no need to do anything here */
+	return desc.err;
+}
+EXPORT_SYMBOL_GPL(olpc_ec_cmd);
+
+#ifdef CONFIG_DEBUG_FS
+
+/*
+ * debugfs support for "generic commands", to allow sending
+ * arbitrary EC commands from userspace.
+ */
+
+#define EC_MAX_CMD_ARGS (5 + 1)		/* cmd byte + 5 args */
+#define EC_MAX_CMD_REPLY (8)
+
+static DEFINE_MUTEX(ec_dbgfs_lock);
+static unsigned char ec_dbgfs_resp[EC_MAX_CMD_REPLY];
+static unsigned int ec_dbgfs_resp_bytes;
+
+static ssize_t ec_dbgfs_cmd_write(struct file *file, const char __user *buf,
+		size_t size, loff_t *ppos)
+{
+	int i, m;
+	unsigned char ec_cmd[EC_MAX_CMD_ARGS];
+	unsigned int ec_cmd_int[EC_MAX_CMD_ARGS];
+	char cmdbuf[64];
+	int ec_cmd_bytes;
+
+	mutex_lock(&ec_dbgfs_lock);
+
+	size = simple_write_to_buffer(cmdbuf, sizeof(cmdbuf), ppos, buf, size);
+
+	m = sscanf(cmdbuf, "%x:%u %x %x %x %x %x", &ec_cmd_int[0],
+			&ec_dbgfs_resp_bytes, &ec_cmd_int[1], &ec_cmd_int[2],
+			&ec_cmd_int[3], &ec_cmd_int[4], &ec_cmd_int[5]);
+	if (m < 2 || ec_dbgfs_resp_bytes > EC_MAX_CMD_REPLY) {
+		/* reset to prevent overflow on read */
+		ec_dbgfs_resp_bytes = 0;
+
+		pr_debug("olpc-ec: bad ec cmd:  cmd:response-count [arg1 [arg2 ...]]\n");
+		size = -EINVAL;
+		goto out;
+	}
+
+	/* convert scanf'd ints to char */
+	ec_cmd_bytes = m - 2;
+	for (i = 0; i <= ec_cmd_bytes; i++)
+		ec_cmd[i] = ec_cmd_int[i];
+
+	pr_debug("olpc-ec: debugfs cmd 0x%02x with %d args %02x %02x %02x %02x %02x, want %d returns\n",
+			ec_cmd[0], ec_cmd_bytes, ec_cmd[1], ec_cmd[2],
+			ec_cmd[3], ec_cmd[4], ec_cmd[5], ec_dbgfs_resp_bytes);
+
+	olpc_ec_cmd(ec_cmd[0], (ec_cmd_bytes == 0) ? NULL : &ec_cmd[1],
+			ec_cmd_bytes, ec_dbgfs_resp, ec_dbgfs_resp_bytes);
+
+	pr_debug("olpc-ec: response %02x %02x %02x %02x %02x %02x %02x %02x (%d bytes expected)\n",
+			ec_dbgfs_resp[0], ec_dbgfs_resp[1], ec_dbgfs_resp[2],
+			ec_dbgfs_resp[3], ec_dbgfs_resp[4], ec_dbgfs_resp[5],
+			ec_dbgfs_resp[6], ec_dbgfs_resp[7],
+			ec_dbgfs_resp_bytes);
+
+out:
+	mutex_unlock(&ec_dbgfs_lock);
+	return size;
+}
+
+static ssize_t ec_dbgfs_cmd_read(struct file *file, char __user *buf,
+		size_t size, loff_t *ppos)
+{
+	unsigned int i, r;
+	char *rp;
+	char respbuf[64];
+
+	mutex_lock(&ec_dbgfs_lock);
+	rp = respbuf;
+	rp += sprintf(rp, "%02x", ec_dbgfs_resp[0]);
+	for (i = 1; i < ec_dbgfs_resp_bytes; i++)
+		rp += sprintf(rp, ", %02x", ec_dbgfs_resp[i]);
+	mutex_unlock(&ec_dbgfs_lock);
+	rp += sprintf(rp, "\n");
+
+	r = rp - respbuf;
+	return simple_read_from_buffer(buf, size, ppos, respbuf, r);
+}
+
+static const struct file_operations ec_dbgfs_ops = {
+	.write = ec_dbgfs_cmd_write,
+	.read = ec_dbgfs_cmd_read,
+};
+
+static struct dentry *olpc_ec_setup_debugfs(void)
+{
+	struct dentry *dbgfs_dir;
+
+	dbgfs_dir = debugfs_create_dir("olpc-ec", NULL);
+	if (IS_ERR_OR_NULL(dbgfs_dir))
+		return NULL;
+
+	debugfs_create_file("cmd", 0600, dbgfs_dir, NULL, &ec_dbgfs_ops);
+
+	return dbgfs_dir;
+}
+
+#else
+
+static struct dentry *olpc_ec_setup_debugfs(void)
+{
+	return NULL;
+}
+
+#endif /* CONFIG_DEBUG_FS */
+
+static int olpc_ec_probe(struct platform_device *pdev)
+{
+	struct olpc_ec_priv *ec;
+	int err;
+
+	if (!ec_driver)
+		return -ENODEV;
+
+	ec = kzalloc(sizeof(*ec), GFP_KERNEL);
+	if (!ec)
+		return -ENOMEM;
+
+	ec->drv = ec_driver;
+	INIT_WORK(&ec->worker, olpc_ec_worker);
+	mutex_init(&ec->cmd_lock);
+
+	INIT_LIST_HEAD(&ec->cmd_q);
+	spin_lock_init(&ec->cmd_q_lock);
+
+	ec_priv = ec;
+	platform_set_drvdata(pdev, ec);
+
+	err = ec_driver->probe ? ec_driver->probe(pdev) : 0;
+	if (err) {
+		ec_priv = NULL;
+		kfree(ec);
+	} else {
+		ec->dbgfs_dir = olpc_ec_setup_debugfs();
+	}
+
+	return err;
+}
+
+static int olpc_ec_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct olpc_ec_priv *ec = platform_get_drvdata(pdev);
+	int err = 0;
+
+	if (ec_driver->suspend)
+		err = ec_driver->suspend(pdev);
+	if (!err)
+		ec->suspended = true;
+
+	return err;
+}
+
+static int olpc_ec_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct olpc_ec_priv *ec = platform_get_drvdata(pdev);
+
+	ec->suspended = false;
+	return ec_driver->resume ? ec_driver->resume(pdev) : 0;
+}
+
+static const struct dev_pm_ops olpc_ec_pm_ops = {
+	.suspend_late = olpc_ec_suspend,
+	.resume_early = olpc_ec_resume,
+};
+
+static struct platform_driver olpc_ec_plat_driver = {
+	.probe = olpc_ec_probe,
+	.driver = {
+		.name = "olpc-ec",
+		.pm = &olpc_ec_pm_ops,
+	},
+};
+
+static int __init olpc_ec_init_module(void)
+{
+	return platform_driver_register(&olpc_ec_plat_driver);
+}
+
+module_init(olpc_ec_init_module);
+
+MODULE_AUTHOR("Andres Salomon <dilinger@queued.net>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/acer-wmi.c b/drivers/platform/x86/acer-wmi.c
index ce875dc365e..3782e1cd369 100644
--- a/drivers/platform/x86/acer-wmi.c
+++ b/drivers/platform/x86/acer-wmi.c
@@ -95,6 +95,7 @@ MODULE_ALIAS("wmi:676AA15E-6A47-4D9F-A2CC-1E6D18D14026");
 
 enum acer_wmi_event_ids {
 	WMID_HOTKEY_EVENT = 0x1,
+	WMID_ACCEL_EVENT = 0x5,
 };
 
 static const struct key_entry acer_wmi_keymap[] = {
@@ -130,6 +131,7 @@ static const struct key_entry acer_wmi_keymap[] = {
 };
 
 static struct input_dev *acer_wmi_input_dev;
+static struct input_dev *acer_wmi_accel_dev;
 
 struct event_return_value {
 	u8 function;
@@ -200,6 +202,7 @@ struct hotkey_function_type_aa {
 #define ACER_CAP_BLUETOOTH		(1<<2)
 #define ACER_CAP_BRIGHTNESS		(1<<3)
 #define ACER_CAP_THREEG			(1<<4)
+#define ACER_CAP_ACCEL			(1<<5)
 #define ACER_CAP_ANY			(0xFFFFFFFF)
 
 /*
@@ -1399,6 +1402,60 @@ static void acer_backlight_exit(void)
 }
 
 /*
+ * Accelerometer device
+ */
+static acpi_handle gsensor_handle;
+
+static int acer_gsensor_init(void)
+{
+	acpi_status status;
+	struct acpi_buffer output;
+	union acpi_object out_obj;
+
+	output.length = sizeof(out_obj);
+	output.pointer = &out_obj;
+	status = acpi_evaluate_object(gsensor_handle, "_INI", NULL, &output);
+	if (ACPI_FAILURE(status))
+		return -1;
+
+	return 0;
+}
+
+static int acer_gsensor_open(struct input_dev *input)
+{
+	return acer_gsensor_init();
+}
+
+static int acer_gsensor_event(void)
+{
+	acpi_status status;
+	struct acpi_buffer output;
+	union acpi_object out_obj[5];
+
+	if (!has_cap(ACER_CAP_ACCEL))
+		return -1;
+
+	output.length = sizeof(out_obj);
+	output.pointer = out_obj;
+
+	status = acpi_evaluate_object(gsensor_handle, "RDVL", NULL, &output);
+	if (ACPI_FAILURE(status))
+		return -1;
+
+	if (out_obj->package.count != 4)
+		return -1;
+
+	input_report_abs(acer_wmi_accel_dev, ABS_X,
+		(s16)out_obj->package.elements[0].integer.value);
+	input_report_abs(acer_wmi_accel_dev, ABS_Y,
+		(s16)out_obj->package.elements[1].integer.value);
+	input_report_abs(acer_wmi_accel_dev, ABS_Z,
+		(s16)out_obj->package.elements[2].integer.value);
+	input_sync(acer_wmi_accel_dev);
+	return 0;
+}
+
+/*
  * Rfkill devices
  */
 static void acer_rfkill_update(struct work_struct *ignored);
@@ -1673,6 +1730,9 @@ static void acer_wmi_notify(u32 value, void *context)
 						   1, true);
 		}
 		break;
+	case WMID_ACCEL_EVENT:
+		acer_gsensor_event();
+		break;
 	default:
 		pr_warn("Unknown function number - %d - %d\n",
 			return_value.function, return_value.key_num);
@@ -1758,6 +1818,73 @@ static int acer_wmi_enable_lm(void)
 	return status;
 }
 
+static acpi_status __init acer_wmi_get_handle_cb(acpi_handle ah, u32 level,
+						void *ctx, void **retval)
+{
+	*(acpi_handle *)retval = ah;
+	return AE_OK;
+}
+
+static int __init acer_wmi_get_handle(const char *name, const char *prop,
+					acpi_handle *ah)
+{
+	acpi_status status;
+	acpi_handle handle;
+
+	BUG_ON(!name || !ah);
+
+	handle = NULL;
+	status = acpi_get_devices(prop, acer_wmi_get_handle_cb,
+					(void *)name, &handle);
+
+	if (ACPI_SUCCESS(status)) {
+		*ah = handle;
+		return 0;
+	} else {
+		return -ENODEV;
+	}
+}
+
+static int __init acer_wmi_accel_setup(void)
+{
+	int err;
+
+	err = acer_wmi_get_handle("SENR", "BST0001", &gsensor_handle);
+	if (err)
+		return err;
+
+	interface->capability |= ACER_CAP_ACCEL;
+
+	acer_wmi_accel_dev = input_allocate_device();
+	if (!acer_wmi_accel_dev)
+		return -ENOMEM;
+
+	acer_wmi_accel_dev->open = acer_gsensor_open;
+
+	acer_wmi_accel_dev->name = "Acer BMA150 accelerometer";
+	acer_wmi_accel_dev->phys = "wmi/input1";
+	acer_wmi_accel_dev->id.bustype = BUS_HOST;
+	acer_wmi_accel_dev->evbit[0] = BIT_MASK(EV_ABS);
+	input_set_abs_params(acer_wmi_accel_dev, ABS_X, -16384, 16384, 0, 0);
+	input_set_abs_params(acer_wmi_accel_dev, ABS_Y, -16384, 16384, 0, 0);
+	input_set_abs_params(acer_wmi_accel_dev, ABS_Z, -16384, 16384, 0, 0);
+
+	err = input_register_device(acer_wmi_accel_dev);
+	if (err)
+		goto err_free_dev;
+
+	return 0;
+
+err_free_dev:
+	input_free_device(acer_wmi_accel_dev);
+	return err;
+}
+
+static void acer_wmi_accel_destroy(void)
+{
+	input_unregister_device(acer_wmi_accel_dev);
+}
+
 static int __init acer_wmi_input_setup(void)
 {
 	acpi_status status;
@@ -1877,8 +2004,7 @@ static int acer_platform_remove(struct platform_device *device)
 	return 0;
 }
 
-static int acer_platform_suspend(struct platform_device *dev,
-pm_message_t state)
+static int acer_suspend(struct device *dev)
 {
 	u32 value;
 	struct acer_data *data = &interface->data;
@@ -1900,7 +2026,7 @@ pm_message_t state)
 	return 0;
 }
 
-static int acer_platform_resume(struct platform_device *device)
+static int acer_resume(struct device *dev)
 {
 	struct acer_data *data = &interface->data;
 
@@ -1913,9 +2039,14 @@ static int acer_platform_resume(struct platform_device *device)
 	if (has_cap(ACER_CAP_BRIGHTNESS))
 		set_u32(data->brightness, ACER_CAP_BRIGHTNESS);
 
+	if (has_cap(ACER_CAP_ACCEL))
+		acer_gsensor_init();
+
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(acer_pm, acer_suspend, acer_resume);
+
 static void acer_platform_shutdown(struct platform_device *device)
 {
 	struct acer_data *data = &interface->data;
@@ -1931,11 +2062,10 @@ static struct platform_driver acer_platform_driver = {
 	.driver = {
 		.name = "acer-wmi",
 		.owner = THIS_MODULE,
+		.pm = &acer_pm,
 	},
 	.probe = acer_platform_probe,
 	.remove = acer_platform_remove,
-	.suspend = acer_platform_suspend,
-	.resume = acer_platform_resume,
 	.shutdown = acer_platform_shutdown,
 };
 
@@ -2060,14 +2190,16 @@ static int __init acer_wmi_init(void)
 
 	set_quirks();
 
+	if (dmi_check_system(video_vendor_dmi_table))
+		acpi_video_dmi_promote_vendor();
 	if (acpi_video_backlight_support()) {
-		if (dmi_check_system(video_vendor_dmi_table)) {
-			acpi_video_unregister();
-		} else {
-			interface->capability &= ~ACER_CAP_BRIGHTNESS;
-			pr_info("Brightness must be controlled by "
-				"acpi video driver\n");
-		}
+		interface->capability &= ~ACER_CAP_BRIGHTNESS;
+		pr_info("Brightness must be controlled by acpi video driver\n");
+	} else {
+#ifdef CONFIG_ACPI_VIDEO
+		pr_info("Disabling ACPI video driver\n");
+		acpi_video_unregister();
+#endif
 	}
 
 	if (wmi_has_guid(WMID_GUID3)) {
@@ -2090,6 +2222,8 @@ static int __init acer_wmi_init(void)
 			return err;
 	}
 
+	acer_wmi_accel_setup();
+
 	err = platform_driver_register(&acer_platform_driver);
 	if (err) {
 		pr_err("Unable to register platform driver\n");
@@ -2133,6 +2267,8 @@ error_device_alloc:
 error_platform_register:
 	if (wmi_has_guid(ACERWMID_EVENT_GUID))
 		acer_wmi_input_destroy();
+	if (has_cap(ACER_CAP_ACCEL))
+		acer_wmi_accel_destroy();
 
 	return err;
 }
@@ -2142,6 +2278,9 @@ static void __exit acer_wmi_exit(void)
 	if (wmi_has_guid(ACERWMID_EVENT_GUID))
 		acer_wmi_input_destroy();
 
+	if (has_cap(ACER_CAP_ACCEL))
+		acer_wmi_accel_destroy();
+
 	remove_sysfs(acer_platform_device);
 	remove_debugfs();
 	platform_device_unregister(acer_platform_device);
diff --git a/drivers/platform/x86/acerhdf.c b/drivers/platform/x86/acerhdf.c
index 2fd9d36acd1..39abb150bdd 100644
--- a/drivers/platform/x86/acerhdf.c
+++ b/drivers/platform/x86/acerhdf.c
@@ -660,7 +660,7 @@ static int acerhdf_register_thermal(void)
 	if (IS_ERR(cl_dev))
 		return -EINVAL;
 
-	thz_dev = thermal_zone_device_register("acerhdf", 1, NULL,
+	thz_dev = thermal_zone_device_register("acerhdf", 1, 0, NULL,
 					      &acerhdf_dev_ops, 0, 0, 0,
 					      (kernelmode) ? interval*1000 : 0);
 	if (IS_ERR(thz_dev))
diff --git a/drivers/platform/x86/apple-gmux.c b/drivers/platform/x86/apple-gmux.c
index 694a15a5623..905fa01ac8d 100644
--- a/drivers/platform/x86/apple-gmux.c
+++ b/drivers/platform/x86/apple-gmux.c
@@ -193,7 +193,10 @@ static int __devinit gmux_probe(struct pnp_dev *pnp,
 	 * backlight control and supports more levels than other options.
 	 * Disable the other backlight choices.
 	 */
+	acpi_video_dmi_promote_vendor();
+#ifdef CONFIG_ACPI_VIDEO
 	acpi_video_unregister();
+#endif
 	apple_bl_unregister();
 
 	return 0;
@@ -213,7 +216,10 @@ static void __devexit gmux_remove(struct pnp_dev *pnp)
 	release_region(gmux_data->iostart, gmux_data->iolen);
 	kfree(gmux_data);
 
+	acpi_video_dmi_demote_vendor();
+#ifdef CONFIG_ACPI_VIDEO
 	acpi_video_register();
+#endif
 	apple_bl_register();
 }
 
diff --git a/drivers/platform/x86/asus-nb-wmi.c b/drivers/platform/x86/asus-nb-wmi.c
index 99a30b51313..6b0ebdeae91 100644
--- a/drivers/platform/x86/asus-nb-wmi.c
+++ b/drivers/platform/x86/asus-nb-wmi.c
@@ -26,6 +26,7 @@
 #include <linux/input.h>
 #include <linux/input/sparse-keymap.h>
 #include <linux/fb.h>
+#include <linux/dmi.h>
 
 #include "asus-wmi.h"
 
@@ -48,18 +49,115 @@ MODULE_ALIAS("wmi:"ASUS_NB_WMI_EVENT_GUID);
  *  1  | Hardware  | Software
  *  4  | Software  | Software
  */
-static uint wapf;
+static int wapf = -1;
 module_param(wapf, uint, 0444);
 MODULE_PARM_DESC(wapf, "WAPF value");
 
+static struct quirk_entry *quirks;
+
 static struct quirk_entry quirk_asus_unknown = {
+	.wapf = 0,
+};
+
+static struct quirk_entry quirk_asus_x401u = {
+	.wapf = 4,
+};
+
+static int dmi_matched(const struct dmi_system_id *dmi)
+{
+	quirks = dmi->driver_data;
+	return 1;
+}
+
+static struct dmi_system_id asus_quirks[] = {
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X401U",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X401U"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X401A1",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X401A1"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X501U",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X501U"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X501A1",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X501A1"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X55A",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X55A"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X55C",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X55C"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X55U",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X55U"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK COMPUTER INC. X55VD",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X55VD"),
+		},
+		.driver_data = &quirk_asus_x401u,
+	},
+	{},
 };
 
 static void asus_nb_wmi_quirks(struct asus_wmi_driver *driver)
 {
-	driver->quirks = &quirk_asus_unknown;
-	driver->quirks->wapf = wapf;
+	quirks = &quirk_asus_unknown;
+	dmi_check_system(asus_quirks);
+
+	driver->quirks = quirks;
 	driver->panel_power = FB_BLANK_UNBLANK;
+
+	/* overwrite the wapf setting if the wapf paramater is specified */
+	if (wapf != -1)
+		quirks->wapf = wapf;
+	else
+		wapf = quirks->wapf;
 }
 
 static const struct key_entry asus_nb_wmi_keymap[] = {
@@ -94,6 +192,10 @@ static const struct key_entry asus_nb_wmi_keymap[] = {
 	{ KE_KEY, 0x8A, { KEY_PROG1 } },
 	{ KE_KEY, 0x95, { KEY_MEDIA } },
 	{ KE_KEY, 0x99, { KEY_PHONE } },
+	{ KE_KEY, 0xA0, { KEY_SWITCHVIDEOMODE } }, /* SDSP HDMI only */
+	{ KE_KEY, 0xA1, { KEY_SWITCHVIDEOMODE } }, /* SDSP LCD + HDMI */
+	{ KE_KEY, 0xA2, { KEY_SWITCHVIDEOMODE } }, /* SDSP CRT + HDMI */
+	{ KE_KEY, 0xA3, { KEY_SWITCHVIDEOMODE } }, /* SDSP TV + HDMI */
 	{ KE_KEY, 0xb5, { KEY_CALC } },
 	{ KE_KEY, 0xc4, { KEY_KBDILLUMUP } },
 	{ KE_KEY, 0xc5, { KEY_KBDILLUMDOWN } },
diff --git a/drivers/platform/x86/asus-wmi.c b/drivers/platform/x86/asus-wmi.c
index 77aadde5281..c7a36f6b058 100644
--- a/drivers/platform/x86/asus-wmi.c
+++ b/drivers/platform/x86/asus-wmi.c
@@ -47,6 +47,9 @@
 #include <linux/thermal.h>
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
+#ifdef CONFIG_ACPI_VIDEO
+#include <acpi/video.h>
+#endif
 
 #include "asus-wmi.h"
 
@@ -136,6 +139,9 @@ MODULE_LICENSE("GPL");
 /* Power */
 #define ASUS_WMI_DEVID_PROCESSOR_STATE	0x00120012
 
+/* Deep S3 / Resume on LID open */
+#define ASUS_WMI_DEVID_LID_RESUME	0x00120031
+
 /* DSTS masks */
 #define ASUS_WMI_DSTS_STATUS_BIT	0x00000001
 #define ASUS_WMI_DSTS_UNKNOWN_BIT	0x00000002
@@ -1365,6 +1371,7 @@ static ssize_t show_sys_wmi(struct asus_wmi *asus, int devid, char *buf)
 ASUS_WMI_CREATE_DEVICE_ATTR(touchpad, 0644, ASUS_WMI_DEVID_TOUCHPAD);
 ASUS_WMI_CREATE_DEVICE_ATTR(camera, 0644, ASUS_WMI_DEVID_CAMERA);
 ASUS_WMI_CREATE_DEVICE_ATTR(cardr, 0644, ASUS_WMI_DEVID_CARDREADER);
+ASUS_WMI_CREATE_DEVICE_ATTR(lid_resume, 0644, ASUS_WMI_DEVID_LID_RESUME);
 
 static ssize_t store_cpufv(struct device *dev, struct device_attribute *attr,
 			   const char *buf, size_t count)
@@ -1390,6 +1397,7 @@ static struct attribute *platform_attributes[] = {
 	&dev_attr_camera.attr,
 	&dev_attr_cardr.attr,
 	&dev_attr_touchpad.attr,
+	&dev_attr_lid_resume.attr,
 	NULL
 };
 
@@ -1408,6 +1416,8 @@ static umode_t asus_sysfs_is_visible(struct kobject *kobj,
 		devid = ASUS_WMI_DEVID_CARDREADER;
 	else if (attr == &dev_attr_touchpad.attr)
 		devid = ASUS_WMI_DEVID_TOUCHPAD;
+	else if (attr == &dev_attr_lid_resume.attr)
+		devid = ASUS_WMI_DEVID_LID_RESUME;
 
 	if (devid != -1)
 		ok = !(asus_wmi_get_devstate_simple(asus, devid) < 0);
@@ -1467,14 +1477,9 @@ static int asus_wmi_platform_init(struct asus_wmi *asus)
 	 */
 	if (!asus_wmi_evaluate_method(ASUS_WMI_METHODID_DSTS, 0, 0, NULL))
 		asus->dsts_id = ASUS_WMI_METHODID_DSTS;
-	else if (!asus_wmi_evaluate_method(ASUS_WMI_METHODID_DSTS2, 0, 0, NULL))
+	else
 		asus->dsts_id = ASUS_WMI_METHODID_DSTS2;
 
-	if (!asus->dsts_id) {
-		pr_err("Can't find DSTS");
-		return -ENODEV;
-	}
-
 	/* CWAP allow to define the behavior of the Fn+F2 key,
 	 * this method doesn't seems to be present on Eee PCs */
 	if (asus->driver->quirks->wapf >= 0)
@@ -1681,7 +1686,13 @@ static int asus_wmi_add(struct platform_device *pdev)
 	if (err)
 		goto fail_rfkill;
 
+	if (asus->driver->quirks->wmi_backlight_power)
+		acpi_video_dmi_promote_vendor();
 	if (!acpi_video_backlight_support()) {
+#ifdef CONFIG_ACPI_VIDEO
+		pr_info("Disabling ACPI video driver\n");
+		acpi_video_unregister();
+#endif
 		err = asus_wmi_backlight_init(asus);
 		if (err && err != -ENODEV)
 			goto fail_backlight;
diff --git a/drivers/platform/x86/asus-wmi.h b/drivers/platform/x86/asus-wmi.h
index d43b6674200..9c1da8b81be 100644
--- a/drivers/platform/x86/asus-wmi.h
+++ b/drivers/platform/x86/asus-wmi.h
@@ -39,6 +39,7 @@ struct quirk_entry {
 	bool hotplug_wireless;
 	bool scalar_panel_brightness;
 	bool store_backlight_power;
+	bool wmi_backlight_power;
 	int wapf;
 };
 
diff --git a/drivers/platform/x86/classmate-laptop.c b/drivers/platform/x86/classmate-laptop.c
index 94f93b621d7..2ca7dd1ab3e 100644
--- a/drivers/platform/x86/classmate-laptop.c
+++ b/drivers/platform/x86/classmate-laptop.c
@@ -31,15 +31,21 @@ MODULE_LICENSE("GPL");
 
 struct cmpc_accel {
 	int sensitivity;
+	int g_select;
+	int inputdev_state;
 };
 
-#define CMPC_ACCEL_SENSITIVITY_DEFAULT		5
+#define CMPC_ACCEL_DEV_STATE_CLOSED	0
+#define CMPC_ACCEL_DEV_STATE_OPEN	1
 
+#define CMPC_ACCEL_SENSITIVITY_DEFAULT		5
+#define CMPC_ACCEL_G_SELECT_DEFAULT		0
 
 #define CMPC_ACCEL_HID		"ACCE0000"
+#define CMPC_ACCEL_HID_V4	"ACCE0001"
 #define CMPC_TABLET_HID		"TBLT0000"
 #define CMPC_IPML_HID	"IPML200"
-#define CMPC_KEYS_HID		"FnBT0000"
+#define CMPC_KEYS_HID		"FNBT0000"
 
 /*
  * Generic input device code.
@@ -76,7 +82,391 @@ static int cmpc_remove_acpi_notify_device(struct acpi_device *acpi)
 }
 
 /*
- * Accelerometer code.
+ * Accelerometer code for Classmate V4
+ */
+static acpi_status cmpc_start_accel_v4(acpi_handle handle)
+{
+	union acpi_object param[4];
+	struct acpi_object_list input;
+	acpi_status status;
+
+	param[0].type = ACPI_TYPE_INTEGER;
+	param[0].integer.value = 0x3;
+	param[1].type = ACPI_TYPE_INTEGER;
+	param[1].integer.value = 0;
+	param[2].type = ACPI_TYPE_INTEGER;
+	param[2].integer.value = 0;
+	param[3].type = ACPI_TYPE_INTEGER;
+	param[3].integer.value = 0;
+	input.count = 4;
+	input.pointer = param;
+	status = acpi_evaluate_object(handle, "ACMD", &input, NULL);
+	return status;
+}
+
+static acpi_status cmpc_stop_accel_v4(acpi_handle handle)
+{
+	union acpi_object param[4];
+	struct acpi_object_list input;
+	acpi_status status;
+
+	param[0].type = ACPI_TYPE_INTEGER;
+	param[0].integer.value = 0x4;
+	param[1].type = ACPI_TYPE_INTEGER;
+	param[1].integer.value = 0;
+	param[2].type = ACPI_TYPE_INTEGER;
+	param[2].integer.value = 0;
+	param[3].type = ACPI_TYPE_INTEGER;
+	param[3].integer.value = 0;
+	input.count = 4;
+	input.pointer = param;
+	status = acpi_evaluate_object(handle, "ACMD", &input, NULL);
+	return status;
+}
+
+static acpi_status cmpc_accel_set_sensitivity_v4(acpi_handle handle, int val)
+{
+	union acpi_object param[4];
+	struct acpi_object_list input;
+
+	param[0].type = ACPI_TYPE_INTEGER;
+	param[0].integer.value = 0x02;
+	param[1].type = ACPI_TYPE_INTEGER;
+	param[1].integer.value = val;
+	param[2].type = ACPI_TYPE_INTEGER;
+	param[2].integer.value = 0;
+	param[3].type = ACPI_TYPE_INTEGER;
+	param[3].integer.value = 0;
+	input.count = 4;
+	input.pointer = param;
+	return acpi_evaluate_object(handle, "ACMD", &input, NULL);
+}
+
+static acpi_status cmpc_accel_set_g_select_v4(acpi_handle handle, int val)
+{
+	union acpi_object param[4];
+	struct acpi_object_list input;
+
+	param[0].type = ACPI_TYPE_INTEGER;
+	param[0].integer.value = 0x05;
+	param[1].type = ACPI_TYPE_INTEGER;
+	param[1].integer.value = val;
+	param[2].type = ACPI_TYPE_INTEGER;
+	param[2].integer.value = 0;
+	param[3].type = ACPI_TYPE_INTEGER;
+	param[3].integer.value = 0;
+	input.count = 4;
+	input.pointer = param;
+	return acpi_evaluate_object(handle, "ACMD", &input, NULL);
+}
+
+static acpi_status cmpc_get_accel_v4(acpi_handle handle,
+				     int16_t *x,
+				     int16_t *y,
+				     int16_t *z)
+{
+	union acpi_object param[4];
+	struct acpi_object_list input;
+	struct acpi_buffer output = { ACPI_ALLOCATE_BUFFER, NULL };
+	int16_t *locs;
+	acpi_status status;
+
+	param[0].type = ACPI_TYPE_INTEGER;
+	param[0].integer.value = 0x01;
+	param[1].type = ACPI_TYPE_INTEGER;
+	param[1].integer.value = 0;
+	param[2].type = ACPI_TYPE_INTEGER;
+	param[2].integer.value = 0;
+	param[3].type = ACPI_TYPE_INTEGER;
+	param[3].integer.value = 0;
+	input.count = 4;
+	input.pointer = param;
+	status = acpi_evaluate_object(handle, "ACMD", &input, &output);
+	if (ACPI_SUCCESS(status)) {
+		union acpi_object *obj;
+		obj = output.pointer;
+		locs = (int16_t *) obj->buffer.pointer;
+		*x = locs[0];
+		*y = locs[1];
+		*z = locs[2];
+		kfree(output.pointer);
+	}
+	return status;
+}
+
+static void cmpc_accel_handler_v4(struct acpi_device *dev, u32 event)
+{
+	if (event == 0x81) {
+		int16_t x, y, z;
+		acpi_status status;
+
+		status = cmpc_get_accel_v4(dev->handle, &x, &y, &z);
+		if (ACPI_SUCCESS(status)) {
+			struct input_dev *inputdev = dev_get_drvdata(&dev->dev);
+
+			input_report_abs(inputdev, ABS_X, x);
+			input_report_abs(inputdev, ABS_Y, y);
+			input_report_abs(inputdev, ABS_Z, z);
+			input_sync(inputdev);
+		}
+	}
+}
+
+static ssize_t cmpc_accel_sensitivity_show_v4(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct acpi_device *acpi;
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+
+	acpi = to_acpi_device(dev);
+	inputdev = dev_get_drvdata(&acpi->dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	return sprintf(buf, "%d\n", accel->sensitivity);
+}
+
+static ssize_t cmpc_accel_sensitivity_store_v4(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf, size_t count)
+{
+	struct acpi_device *acpi;
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+	unsigned long sensitivity;
+	int r;
+
+	acpi = to_acpi_device(dev);
+	inputdev = dev_get_drvdata(&acpi->dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	r = kstrtoul(buf, 0, &sensitivity);
+	if (r)
+		return r;
+
+	/* sensitivity must be between 1 and 127 */
+	if (sensitivity < 1 || sensitivity > 127)
+		return -EINVAL;
+
+	accel->sensitivity = sensitivity;
+	cmpc_accel_set_sensitivity_v4(acpi->handle, sensitivity);
+
+	return strnlen(buf, count);
+}
+
+static struct device_attribute cmpc_accel_sensitivity_attr_v4 = {
+	.attr = { .name = "sensitivity", .mode = 0660 },
+	.show = cmpc_accel_sensitivity_show_v4,
+	.store = cmpc_accel_sensitivity_store_v4
+};
+
+static ssize_t cmpc_accel_g_select_show_v4(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct acpi_device *acpi;
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+
+	acpi = to_acpi_device(dev);
+	inputdev = dev_get_drvdata(&acpi->dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	return sprintf(buf, "%d\n", accel->g_select);
+}
+
+static ssize_t cmpc_accel_g_select_store_v4(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf, size_t count)
+{
+	struct acpi_device *acpi;
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+	unsigned long g_select;
+	int r;
+
+	acpi = to_acpi_device(dev);
+	inputdev = dev_get_drvdata(&acpi->dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	r = kstrtoul(buf, 0, &g_select);
+	if (r)
+		return r;
+
+	/* 0 means 1.5g, 1 means 6g, everything else is wrong */
+	if (g_select != 0 && g_select != 1)
+		return -EINVAL;
+
+	accel->g_select = g_select;
+	cmpc_accel_set_g_select_v4(acpi->handle, g_select);
+
+	return strnlen(buf, count);
+}
+
+static struct device_attribute cmpc_accel_g_select_attr_v4 = {
+	.attr = { .name = "g_select", .mode = 0660 },
+	.show = cmpc_accel_g_select_show_v4,
+	.store = cmpc_accel_g_select_store_v4
+};
+
+static int cmpc_accel_open_v4(struct input_dev *input)
+{
+	struct acpi_device *acpi;
+	struct cmpc_accel *accel;
+
+	acpi = to_acpi_device(input->dev.parent);
+	accel = dev_get_drvdata(&input->dev);
+
+	cmpc_accel_set_sensitivity_v4(acpi->handle, accel->sensitivity);
+	cmpc_accel_set_g_select_v4(acpi->handle, accel->g_select);
+
+	if (ACPI_SUCCESS(cmpc_start_accel_v4(acpi->handle))) {
+		accel->inputdev_state = CMPC_ACCEL_DEV_STATE_OPEN;
+		return 0;
+	}
+	return -EIO;
+}
+
+static void cmpc_accel_close_v4(struct input_dev *input)
+{
+	struct acpi_device *acpi;
+	struct cmpc_accel *accel;
+
+	acpi = to_acpi_device(input->dev.parent);
+	accel = dev_get_drvdata(&input->dev);
+
+	cmpc_stop_accel_v4(acpi->handle);
+	accel->inputdev_state = CMPC_ACCEL_DEV_STATE_CLOSED;
+}
+
+static void cmpc_accel_idev_init_v4(struct input_dev *inputdev)
+{
+	set_bit(EV_ABS, inputdev->evbit);
+	input_set_abs_params(inputdev, ABS_X, -255, 255, 16, 0);
+	input_set_abs_params(inputdev, ABS_Y, -255, 255, 16, 0);
+	input_set_abs_params(inputdev, ABS_Z, -255, 255, 16, 0);
+	inputdev->open = cmpc_accel_open_v4;
+	inputdev->close = cmpc_accel_close_v4;
+}
+
+static int cmpc_accel_suspend_v4(struct device *dev)
+{
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+
+	inputdev = dev_get_drvdata(dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	if (accel->inputdev_state == CMPC_ACCEL_DEV_STATE_OPEN)
+		return cmpc_stop_accel_v4(to_acpi_device(dev)->handle);
+
+	return 0;
+}
+
+static int cmpc_accel_resume_v4(struct device *dev)
+{
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+
+	inputdev = dev_get_drvdata(dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	if (accel->inputdev_state == CMPC_ACCEL_DEV_STATE_OPEN) {
+		cmpc_accel_set_sensitivity_v4(to_acpi_device(dev)->handle,
+					      accel->sensitivity);
+		cmpc_accel_set_g_select_v4(to_acpi_device(dev)->handle,
+					   accel->g_select);
+
+		if (ACPI_FAILURE(cmpc_start_accel_v4(to_acpi_device(dev)->handle)))
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static int cmpc_accel_add_v4(struct acpi_device *acpi)
+{
+	int error;
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+
+	accel = kmalloc(sizeof(*accel), GFP_KERNEL);
+	if (!accel)
+		return -ENOMEM;
+
+	accel->inputdev_state = CMPC_ACCEL_DEV_STATE_CLOSED;
+
+	accel->sensitivity = CMPC_ACCEL_SENSITIVITY_DEFAULT;
+	cmpc_accel_set_sensitivity_v4(acpi->handle, accel->sensitivity);
+
+	error = device_create_file(&acpi->dev, &cmpc_accel_sensitivity_attr_v4);
+	if (error)
+		goto failed_sensitivity;
+
+	accel->g_select = CMPC_ACCEL_G_SELECT_DEFAULT;
+	cmpc_accel_set_g_select_v4(acpi->handle, accel->g_select);
+
+	error = device_create_file(&acpi->dev, &cmpc_accel_g_select_attr_v4);
+	if (error)
+		goto failed_g_select;
+
+	error = cmpc_add_acpi_notify_device(acpi, "cmpc_accel_v4",
+					    cmpc_accel_idev_init_v4);
+	if (error)
+		goto failed_input;
+
+	inputdev = dev_get_drvdata(&acpi->dev);
+	dev_set_drvdata(&inputdev->dev, accel);
+
+	return 0;
+
+failed_input:
+	device_remove_file(&acpi->dev, &cmpc_accel_g_select_attr_v4);
+failed_g_select:
+	device_remove_file(&acpi->dev, &cmpc_accel_sensitivity_attr_v4);
+failed_sensitivity:
+	kfree(accel);
+	return error;
+}
+
+static int cmpc_accel_remove_v4(struct acpi_device *acpi, int type)
+{
+	struct input_dev *inputdev;
+	struct cmpc_accel *accel;
+
+	inputdev = dev_get_drvdata(&acpi->dev);
+	accel = dev_get_drvdata(&inputdev->dev);
+
+	device_remove_file(&acpi->dev, &cmpc_accel_sensitivity_attr_v4);
+	device_remove_file(&acpi->dev, &cmpc_accel_g_select_attr_v4);
+	return cmpc_remove_acpi_notify_device(acpi);
+}
+
+static SIMPLE_DEV_PM_OPS(cmpc_accel_pm, cmpc_accel_suspend_v4,
+			 cmpc_accel_resume_v4);
+
+static const struct acpi_device_id cmpc_accel_device_ids_v4[] = {
+	{CMPC_ACCEL_HID_V4, 0},
+	{"", 0}
+};
+
+static struct acpi_driver cmpc_accel_acpi_driver_v4 = {
+	.owner = THIS_MODULE,
+	.name = "cmpc_accel_v4",
+	.class = "cmpc_accel_v4",
+	.ids = cmpc_accel_device_ids_v4,
+	.ops = {
+		.add = cmpc_accel_add_v4,
+		.remove = cmpc_accel_remove_v4,
+		.notify = cmpc_accel_handler_v4,
+	},
+	.drv.pm = &cmpc_accel_pm,
+};
+
+
+/*
+ * Accelerometer code for Classmate versions prior to V4
  */
 static acpi_status cmpc_start_accel(acpi_handle handle)
 {
@@ -362,15 +752,18 @@ static int cmpc_tablet_remove(struct acpi_device *acpi, int type)
 	return cmpc_remove_acpi_notify_device(acpi);
 }
 
-static int cmpc_tablet_resume(struct acpi_device *acpi)
+static int cmpc_tablet_resume(struct device *dev)
 {
-	struct input_dev *inputdev = dev_get_drvdata(&acpi->dev);
+	struct input_dev *inputdev = dev_get_drvdata(dev);
+
 	unsigned long long val = 0;
-	if (ACPI_SUCCESS(cmpc_get_tablet(acpi->handle, &val)))
+	if (ACPI_SUCCESS(cmpc_get_tablet(to_acpi_device(dev)->handle, &val)))
 		input_report_switch(inputdev, SW_TABLET_MODE, !val);
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(cmpc_tablet_pm, NULL, cmpc_tablet_resume);
+
 static const struct acpi_device_id cmpc_tablet_device_ids[] = {
 	{CMPC_TABLET_HID, 0},
 	{"", 0}
@@ -384,9 +777,9 @@ static struct acpi_driver cmpc_tablet_acpi_driver = {
 	.ops = {
 		.add = cmpc_tablet_add,
 		.remove = cmpc_tablet_remove,
-		.resume = cmpc_tablet_resume,
 		.notify = cmpc_tablet_handler,
-	}
+	},
+	.drv.pm = &cmpc_tablet_pm,
 };
 
 
@@ -723,8 +1116,15 @@ static int cmpc_init(void)
 	if (r)
 		goto failed_accel;
 
+	r = acpi_bus_register_driver(&cmpc_accel_acpi_driver_v4);
+	if (r)
+		goto failed_accel_v4;
+
 	return r;
 
+failed_accel_v4:
+	acpi_bus_unregister_driver(&cmpc_accel_acpi_driver);
+
 failed_accel:
 	acpi_bus_unregister_driver(&cmpc_tablet_acpi_driver);
 
@@ -740,6 +1140,7 @@ failed_keys:
 
 static void cmpc_exit(void)
 {
+	acpi_bus_unregister_driver(&cmpc_accel_acpi_driver_v4);
 	acpi_bus_unregister_driver(&cmpc_accel_acpi_driver);
 	acpi_bus_unregister_driver(&cmpc_tablet_acpi_driver);
 	acpi_bus_unregister_driver(&cmpc_ipml_acpi_driver);
@@ -751,6 +1152,7 @@ module_exit(cmpc_exit);
 
 static const struct acpi_device_id cmpc_device_ids[] = {
 	{CMPC_ACCEL_HID, 0},
+	{CMPC_ACCEL_HID_V4, 0},
 	{CMPC_TABLET_HID, 0},
 	{CMPC_IPML_HID, 0},
 	{CMPC_KEYS_HID, 0},
diff --git a/drivers/platform/x86/dell-laptop.c b/drivers/platform/x86/dell-laptop.c
index 5f78aac9b16..4e96e8c0b60 100644
--- a/drivers/platform/x86/dell-laptop.c
+++ b/drivers/platform/x86/dell-laptop.c
@@ -206,6 +206,60 @@ static struct dmi_system_id __devinitdata dell_quirks[] = {
 		},
 		.driver_data = &quirk_dell_vostro_v130,
 	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell Inspiron 5420",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Isnpiron 5420"),
+		},
+		.driver_data = &quirk_dell_vostro_v130,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell Inspiron 5520",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Isnpiron 5520"),
+		},
+		.driver_data = &quirk_dell_vostro_v130,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell Inspiron 5720",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Isnpiron 5720"),
+		},
+		.driver_data = &quirk_dell_vostro_v130,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell Inspiron 7420",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Isnpiron 7420"),
+		},
+		.driver_data = &quirk_dell_vostro_v130,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell Inspiron 7520",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Isnpiron 7520"),
+		},
+		.driver_data = &quirk_dell_vostro_v130,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "Dell Inspiron 7720",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Isnpiron 7720"),
+		},
+		.driver_data = &quirk_dell_vostro_v130,
+	},
 	{ }
 };
 
diff --git a/drivers/platform/x86/eeepc-wmi.c b/drivers/platform/x86/eeepc-wmi.c
index 65676138034..5838332ea5b 100644
--- a/drivers/platform/x86/eeepc-wmi.c
+++ b/drivers/platform/x86/eeepc-wmi.c
@@ -79,7 +79,7 @@ static const struct key_entry eeepc_wmi_keymap[] = {
 	{ KE_KEY, 0xe1, { KEY_F14 } }, /* Change Resolution */
 	{ KE_KEY, HOME_PRESS, { KEY_CONFIG } }, /* Home/Express gate key */
 	{ KE_KEY, 0xe8, { KEY_SCREENLOCK } },
-	{ KE_KEY, 0xe9, { KEY_BRIGHTNESS_ZERO } },
+	{ KE_KEY, 0xe9, { KEY_DISPLAYTOGGLE } },
 	{ KE_KEY, 0xeb, { KEY_CAMERA_ZOOMOUT } },
 	{ KE_KEY, 0xec, { KEY_CAMERA_UP } },
 	{ KE_KEY, 0xed, { KEY_CAMERA_DOWN } },
@@ -107,6 +107,11 @@ static struct quirk_entry quirk_asus_et2012_type3 = {
 	.store_backlight_power = true,
 };
 
+static struct quirk_entry quirk_asus_x101ch = {
+	/* We need this when ACPI function doesn't do this well */
+	.wmi_backlight_power = true,
+};
+
 static struct quirk_entry *quirks;
 
 static void et2012_quirks(void)
@@ -157,6 +162,24 @@ static struct dmi_system_id asus_quirks[] = {
 		},
 		.driver_data = &quirk_asus_unknown,
 	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK Computer INC. X101CH",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X101CH"),
+		},
+		.driver_data = &quirk_asus_x101ch,
+	},
+	{
+		.callback = dmi_matched,
+		.ident = "ASUSTeK Computer INC. 1015CX",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "1015CX"),
+		},
+		.driver_data = &quirk_asus_x101ch,
+	},
 	{},
 };
 
diff --git a/drivers/platform/x86/fujitsu-tablet.c b/drivers/platform/x86/fujitsu-tablet.c
index da267eae8ba..d2e41735a47 100644
--- a/drivers/platform/x86/fujitsu-tablet.c
+++ b/drivers/platform/x86/fujitsu-tablet.c
@@ -440,12 +440,14 @@ static int __devexit acpi_fujitsu_remove(struct acpi_device *adev, int type)
 	return 0;
 }
 
-static int acpi_fujitsu_resume(struct acpi_device *adev)
+static int acpi_fujitsu_resume(struct device *dev)
 {
 	fujitsu_reset();
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(acpi_fujitsu_pm, NULL, acpi_fujitsu_resume);
+
 static struct acpi_driver acpi_fujitsu_driver = {
 	.name  = MODULENAME,
 	.class = "hotkey",
@@ -453,8 +455,8 @@ static struct acpi_driver acpi_fujitsu_driver = {
 	.ops   = {
 		.add    = acpi_fujitsu_add,
 		.remove	= acpi_fujitsu_remove,
-		.resume = acpi_fujitsu_resume,
-	}
+	},
+	.drv.pm = &acpi_fujitsu_pm,
 };
 
 static int __init fujitsu_module_init(void)
diff --git a/drivers/platform/x86/hdaps.c b/drivers/platform/x86/hdaps.c
index 24a3ae065f1..d9ab6f64dce 100644
--- a/drivers/platform/x86/hdaps.c
+++ b/drivers/platform/x86/hdaps.c
@@ -305,17 +305,19 @@ static int hdaps_probe(struct platform_device *dev)
 	return 0;
 }
 
-static int hdaps_resume(struct platform_device *dev)
+static int hdaps_resume(struct device *dev)
 {
 	return hdaps_device_init();
 }
 
+static SIMPLE_DEV_PM_OPS(hdaps_pm, NULL, hdaps_resume);
+
 static struct platform_driver hdaps_driver = {
 	.probe = hdaps_probe,
-	.resume = hdaps_resume,
 	.driver	= {
 		.name = "hdaps",
 		.owner = THIS_MODULE,
+		.pm = &hdaps_pm,
 	},
 };
 
diff --git a/drivers/platform/x86/hp_accel.c b/drivers/platform/x86/hp_accel.c
index 22b2dfa7314..f4d91154ad6 100644
--- a/drivers/platform/x86/hp_accel.c
+++ b/drivers/platform/x86/hp_accel.c
@@ -353,20 +353,22 @@ static int lis3lv02d_remove(struct acpi_device *device, int type)
 
 
 #ifdef CONFIG_PM
-static int lis3lv02d_suspend(struct acpi_device *device, pm_message_t state)
+static int lis3lv02d_suspend(struct device *dev)
 {
 	/* make sure the device is off when we suspend */
 	lis3lv02d_poweroff(&lis3_dev);
 	return 0;
 }
 
-static int lis3lv02d_resume(struct acpi_device *device)
+static int lis3lv02d_resume(struct device *dev)
 {
 	return lis3lv02d_poweron(&lis3_dev);
 }
+
+static SIMPLE_DEV_PM_OPS(hp_accel_pm, lis3lv02d_suspend, lis3lv02d_resume);
+#define HP_ACCEL_PM (&hp_accel_pm)
 #else
-#define lis3lv02d_suspend NULL
-#define lis3lv02d_resume NULL
+#define HP_ACCEL_PM NULL
 #endif
 
 /* For the HP MDPS aka 3D Driveguard */
@@ -377,9 +379,8 @@ static struct acpi_driver lis3lv02d_driver = {
 	.ops = {
 		.add     = lis3lv02d_add,
 		.remove  = lis3lv02d_remove,
-		.suspend = lis3lv02d_suspend,
-		.resume  = lis3lv02d_resume,
-	}
+	},
+	.drv.pm = HP_ACCEL_PM,
 };
 
 static int __init lis3lv02d_init_module(void)
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index 4f20f8dd3d7..17f6dfd8dbf 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -694,10 +694,10 @@ MODULE_DEVICE_TABLE(acpi, ideapad_device_ids);
 static int __devinit ideapad_acpi_add(struct acpi_device *adevice)
 {
 	int ret, i;
-	unsigned long cfg;
+	int cfg;
 	struct ideapad_private *priv;
 
-	if (read_method_int(adevice->handle, "_CFG", (int *)&cfg))
+	if (read_method_int(adevice->handle, "_CFG", &cfg))
 		return -ENODEV;
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
@@ -721,7 +721,7 @@ static int __devinit ideapad_acpi_add(struct acpi_device *adevice)
 		goto input_failed;
 
 	for (i = 0; i < IDEAPAD_RFKILL_DEV_NUM; i++) {
-		if (test_bit(ideapad_rfk_data[i].cfgbit, &cfg))
+		if (test_bit(ideapad_rfk_data[i].cfgbit, &priv->cfg))
 			ideapad_register_rfkill(adevice, i);
 		else
 			priv->rfk[i] = NULL;
diff --git a/drivers/platform/x86/intel_ips.c b/drivers/platform/x86/intel_ips.c
index 0ffdb3cde2b..5051aa970e0 100644
--- a/drivers/platform/x86/intel_ips.c
+++ b/drivers/platform/x86/intel_ips.c
@@ -72,6 +72,7 @@
 #include <linux/string.h>
 #include <linux/tick.h>
 #include <linux/timer.h>
+#include <linux/dmi.h>
 #include <drm/i915_drm.h>
 #include <asm/msr.h>
 #include <asm/processor.h>
@@ -1485,6 +1486,24 @@ static DEFINE_PCI_DEVICE_TABLE(ips_id_table) = {
 
 MODULE_DEVICE_TABLE(pci, ips_id_table);
 
+static int ips_blacklist_callback(const struct dmi_system_id *id)
+{
+	pr_info("Blacklisted intel_ips for %s\n", id->ident);
+	return 1;
+}
+
+static const struct dmi_system_id ips_blacklist[] = {
+	{
+		.callback = ips_blacklist_callback,
+		.ident = "HP ProBook",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "HP ProBook"),
+		},
+	},
+	{ }	/* terminating entry */
+};
+
 static int ips_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
 	u64 platform_info;
@@ -1494,6 +1513,9 @@ static int ips_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	u16 htshi, trc, trc_required_mask;
 	u8 tse;
 
+	if (dmi_check_system(ips_blacklist))
+		return -ENODEV;
+
 	ips = kzalloc(sizeof(struct ips_driver), GFP_KERNEL);
 	if (!ips)
 		return -ENOMEM;
@@ -1697,21 +1719,6 @@ static void ips_remove(struct pci_dev *dev)
 	dev_dbg(&dev->dev, "IPS driver removed\n");
 }
 
-#ifdef CONFIG_PM
-static int ips_suspend(struct pci_dev *dev, pm_message_t state)
-{
-	return 0;
-}
-
-static int ips_resume(struct pci_dev *dev)
-{
-	return 0;
-}
-#else
-#define ips_suspend NULL
-#define ips_resume NULL
-#endif /* CONFIG_PM */
-
 static void ips_shutdown(struct pci_dev *dev)
 {
 }
@@ -1721,8 +1728,6 @@ static struct pci_driver ips_pci_driver = {
 	.id_table = ips_id_table,
 	.probe = ips_probe,
 	.remove = ips_remove,
-	.suspend = ips_suspend,
-	.resume = ips_resume,
 	.shutdown = ips_shutdown,
 };
 
diff --git a/drivers/platform/x86/intel_mid_thermal.c b/drivers/platform/x86/intel_mid_thermal.c
index 5ae9cd9c7e6..3a27113deda 100644
--- a/drivers/platform/x86/intel_mid_thermal.c
+++ b/drivers/platform/x86/intel_mid_thermal.c
@@ -418,23 +418,23 @@ static struct thermal_device_info *initialize_sensor(int index)
 
 /**
  * mid_thermal_resume - resume routine
- * @pdev: platform device structure
+ * @dev: device structure
  *
  * mid thermal resume: re-initializes the adc. Can sleep.
  */
-static int mid_thermal_resume(struct platform_device *pdev)
+static int mid_thermal_resume(struct device *dev)
 {
-	return mid_initialize_adc(&pdev->dev);
+	return mid_initialize_adc(dev);
 }
 
 /**
  * mid_thermal_suspend - suspend routine
- * @pdev: platform device structure
+ * @dev: 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)
+static int mid_thermal_suspend(struct device *dev)
 {
 	/*
 	 * This just stops the ADC and does not disable it.
@@ -444,6 +444,9 @@ static int mid_thermal_suspend(struct platform_device *pdev, pm_message_t mesg)
 	return configure_adc(0);
 }
 
+static SIMPLE_DEV_PM_OPS(mid_thermal_pm,
+			 mid_thermal_suspend, mid_thermal_resume);
+
 /**
  * read_curr_temp - reads the current temperature and stores in temp
  * @temp: holds the current temperature value after reading
@@ -499,7 +502,7 @@ static int mid_thermal_probe(struct platform_device *pdev)
 			goto err;
 		}
 		pinfo->tzd[i] = thermal_zone_device_register(name[i],
-				0, td_info, &tzd_ops, 0, 0, 0, 0);
+				0, 0, td_info, &tzd_ops, 0, 0, 0, 0);
 		if (IS_ERR(pinfo->tzd[i])) {
 			kfree(td_info);
 			ret = PTR_ERR(pinfo->tzd[i]);
@@ -557,10 +560,9 @@ static struct platform_driver mid_thermal_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
+		.pm = &mid_thermal_pm,
 	},
 	.probe = mid_thermal_probe,
-	.suspend = mid_thermal_suspend,
-	.resume = mid_thermal_resume,
 	.remove = __devexit_p(mid_thermal_remove),
 	.id_table = therm_id_table,
 };
diff --git a/drivers/platform/x86/msi-laptop.c b/drivers/platform/x86/msi-laptop.c
index bb5132128b3..f6444184431 100644
--- a/drivers/platform/x86/msi-laptop.c
+++ b/drivers/platform/x86/msi-laptop.c
@@ -85,7 +85,8 @@
 #define MSI_STANDARD_EC_TOUCHPAD_ADDRESS	0xe4
 #define MSI_STANDARD_EC_TOUCHPAD_MASK		(1 << 4)
 
-static int msi_laptop_resume(struct platform_device *device);
+static int msi_laptop_resume(struct device *device);
+static SIMPLE_DEV_PM_OPS(msi_laptop_pm, NULL, msi_laptop_resume);
 
 #define MSI_STANDARD_EC_DEVICES_EXISTS_ADDRESS	0x2f
 
@@ -437,8 +438,8 @@ static struct platform_driver msipf_driver = {
 	.driver = {
 		.name = "msi-laptop-pf",
 		.owner = THIS_MODULE,
+		.pm = &msi_laptop_pm,
 	},
-	.resume = msi_laptop_resume,
 };
 
 static struct platform_device *msipf_device;
@@ -752,7 +753,7 @@ err_bluetooth:
 	return retval;
 }
 
-static int msi_laptop_resume(struct platform_device *device)
+static int msi_laptop_resume(struct device *device)
 {
 	u8 data;
 	int result;
diff --git a/drivers/platform/x86/panasonic-laptop.c b/drivers/platform/x86/panasonic-laptop.c
index ffff8b4b494..24480074bcf 100644
--- a/drivers/platform/x86/panasonic-laptop.c
+++ b/drivers/platform/x86/panasonic-laptop.c
@@ -177,7 +177,6 @@ enum SINF_BITS { SINF_NUM_BATTERIES = 0,
 
 static int acpi_pcc_hotkey_add(struct acpi_device *device);
 static int acpi_pcc_hotkey_remove(struct acpi_device *device, int type);
-static int acpi_pcc_hotkey_resume(struct acpi_device *device);
 static void acpi_pcc_hotkey_notify(struct acpi_device *device, u32 event);
 
 static const struct acpi_device_id pcc_device_ids[] = {
@@ -189,6 +188,9 @@ static const struct acpi_device_id pcc_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, pcc_device_ids);
 
+static int acpi_pcc_hotkey_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(acpi_pcc_hotkey_pm, NULL, acpi_pcc_hotkey_resume);
+
 static struct acpi_driver acpi_pcc_driver = {
 	.name =		ACPI_PCC_DRIVER_NAME,
 	.class =	ACPI_PCC_CLASS,
@@ -196,9 +198,9 @@ static struct acpi_driver acpi_pcc_driver = {
 	.ops =		{
 				.add =		acpi_pcc_hotkey_add,
 				.remove =	acpi_pcc_hotkey_remove,
-				.resume =       acpi_pcc_hotkey_resume,
 				.notify =	acpi_pcc_hotkey_notify,
 			},
+	.drv.pm =	&acpi_pcc_hotkey_pm,
 };
 
 static const struct key_entry panasonic_keymap[] = {
@@ -538,11 +540,15 @@ static void acpi_pcc_destroy_input(struct pcc_acpi *pcc)
 
 /* kernel module interface */
 
-static int acpi_pcc_hotkey_resume(struct acpi_device *device)
+static int acpi_pcc_hotkey_resume(struct device *dev)
 {
-	struct pcc_acpi *pcc = acpi_driver_data(device);
+	struct pcc_acpi *pcc;
+
+	if (!dev)
+		return -EINVAL;
 
-	if (device == NULL || pcc == NULL)
+	pcc = acpi_driver_data(to_acpi_device(dev));
+	if (!pcc)
 		return -EINVAL;
 
 	ACPI_DEBUG_PRINT((ACPI_DB_ERROR, "Sticky mode restore: %d\n",
diff --git a/drivers/platform/x86/samsung-laptop.c b/drivers/platform/x86/samsung-laptop.c
index e2a34b42ddc..c1ca7bcebb6 100644
--- a/drivers/platform/x86/samsung-laptop.c
+++ b/drivers/platform/x86/samsung-laptop.c
@@ -26,7 +26,7 @@
 #include <linux/seq_file.h>
 #include <linux/debugfs.h>
 #include <linux/ctype.h>
-#if (defined CONFIG_ACPI_VIDEO || defined CONFIG_ACPI_VIDEO_MODULE)
+#ifdef CONFIG_ACPI_VIDEO
 #include <acpi/video.h>
 #endif
 
@@ -1465,6 +1465,15 @@ static struct dmi_system_id __initdata samsung_dmi_table[] = {
 			DMI_MATCH(DMI_CHASSIS_TYPE, "14"), /* Sub-Notebook */
 		},
 	},
+	/* DMI ids for laptops with bad Chassis Type */
+	{
+	  .ident = "R40/R41",
+	  .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "R40/R41"),
+		DMI_MATCH(DMI_BOARD_NAME, "R40/R41"),
+		},
+	},
 	/* Specific DMI ids for laptop with quirks */
 	{
 	 .callback = samsung_dmi_matched,
@@ -1506,6 +1515,16 @@ static struct dmi_system_id __initdata samsung_dmi_table[] = {
 		},
 	 .driver_data = &samsung_broken_acpi_video,
 	},
+	{
+	 .callback = samsung_dmi_matched,
+	 .ident = "X360",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "X360"),
+		DMI_MATCH(DMI_BOARD_NAME, "X360"),
+		},
+	 .driver_data = &samsung_broken_acpi_video,
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(dmi, samsung_dmi_table);
@@ -1530,15 +1549,18 @@ static int __init samsung_init(void)
 	samsung->quirks = quirks;
 
 
-#if (defined CONFIG_ACPI_VIDEO || defined CONFIG_ACPI_VIDEO_MODULE)
+#ifdef CONFIG_ACPI
+	if (samsung->quirks->broken_acpi_video)
+		acpi_video_dmi_promote_vendor();
+
 	/* Don't handle backlight here if the acpi video already handle it */
 	if (acpi_video_backlight_support()) {
-		if (samsung->quirks->broken_acpi_video) {
-			pr_info("Disabling ACPI video driver\n");
-			acpi_video_unregister();
-		} else {
-			samsung->handle_backlight = false;
-		}
+		samsung->handle_backlight = false;
+	} else if (samsung->quirks->broken_acpi_video) {
+		pr_info("Disabling ACPI video driver\n");
+#ifdef CONFIG_ACPI_VIDEO
+		acpi_video_unregister();
+#endif
 	}
 #endif
 
@@ -1552,8 +1574,7 @@ static int __init samsung_init(void)
 
 #ifdef CONFIG_ACPI
 	/* Only log that if we are really on a sabi platform */
-	if (acpi_video_backlight_support() &&
-	    !samsung->quirks->broken_acpi_video)
+	if (acpi_video_backlight_support())
 		pr_info("Backlight controlled by ACPI video driver\n");
 #endif
 
diff --git a/drivers/platform/x86/sony-laptop.c b/drivers/platform/x86/sony-laptop.c
index 210d4ae547c..9363969ad07 100644
--- a/drivers/platform/x86/sony-laptop.c
+++ b/drivers/platform/x86/sony-laptop.c
@@ -973,7 +973,7 @@ static ssize_t sony_nc_sysfs_store(struct device *dev,
 			       struct device_attribute *attr,
 			       const char *buffer, size_t count)
 {
-	unsigned long value = 0;
+	int value;
 	int ret = 0;
 	struct sony_nc_value *item =
 	    container_of(attr, struct sony_nc_value, devattr);
@@ -984,7 +984,7 @@ static ssize_t sony_nc_sysfs_store(struct device *dev,
 	if (count > 31)
 		return -EINVAL;
 
-	if (kstrtoul(buffer, 10, &value))
+	if (kstrtoint(buffer, 10, &value))
 		return -EINVAL;
 
 	if (item->validate)
@@ -994,7 +994,7 @@ static ssize_t sony_nc_sysfs_store(struct device *dev,
 		return value;
 
 	ret = sony_nc_int_call(sony_nc_acpi_handle, *item->acpiset,
-			(int *)&value, NULL);
+			       &value, NULL);
 	if (ret < 0)
 		return -EIO;
 
@@ -1010,6 +1010,7 @@ static ssize_t sony_nc_sysfs_store(struct device *dev,
 struct sony_backlight_props {
 	struct backlight_device *dev;
 	int			handle;
+	int			cmd_base;
 	u8			offset;
 	u8			maxlvl;
 };
@@ -1037,7 +1038,7 @@ static int sony_nc_get_brightness_ng(struct backlight_device *bd)
 	struct sony_backlight_props *sdev =
 		(struct sony_backlight_props *)bl_get_data(bd);
 
-	sony_call_snc_handle(sdev->handle, 0x0200, &result);
+	sony_call_snc_handle(sdev->handle, sdev->cmd_base + 0x100, &result);
 
 	return (result & 0xff) - sdev->offset;
 }
@@ -1049,7 +1050,8 @@ static int sony_nc_update_status_ng(struct backlight_device *bd)
 		(struct sony_backlight_props *)bl_get_data(bd);
 
 	value = bd->props.brightness + sdev->offset;
-	if (sony_call_snc_handle(sdev->handle, 0x0100 | (value << 16), &result))
+	if (sony_call_snc_handle(sdev->handle, sdev->cmd_base | (value << 0x10),
+				&result))
 		return -EIO;
 
 	return value;
@@ -1172,6 +1174,11 @@ static int sony_nc_hotkeys_decode(u32 event, unsigned int handle)
 /*
  * ACPI callbacks
  */
+enum event_types {
+	HOTKEY = 1,
+	KILLSWITCH,
+	GFX_SWITCH
+};
 static void sony_nc_notify(struct acpi_device *device, u32 event)
 {
 	u32 real_ev = event;
@@ -1196,7 +1203,7 @@ static void sony_nc_notify(struct acpi_device *device, u32 event)
 		/* hotkey event */
 		case 0x0100:
 		case 0x0127:
-			ev_type = 1;
+			ev_type = HOTKEY;
 			real_ev = sony_nc_hotkeys_decode(event, handle);
 
 			if (real_ev > 0)
@@ -1216,7 +1223,7 @@ static void sony_nc_notify(struct acpi_device *device, u32 event)
 			 * update the rfkill device status when the
 			 * switch is moved.
 			 */
-			ev_type = 2;
+			ev_type = KILLSWITCH;
 			sony_call_snc_handle(handle, 0x0100, &result);
 			real_ev = result & 0x03;
 
@@ -1226,6 +1233,24 @@ static void sony_nc_notify(struct acpi_device *device, u32 event)
 
 			break;
 
+		case 0x0128:
+		case 0x0146:
+			/* Hybrid GFX switching */
+			sony_call_snc_handle(handle, 0x0000, &result);
+			dprintk("GFX switch event received (reason: %s)\n",
+					(result & 0x01) ?
+					"switch change" : "unknown");
+
+			/* verify the switch state
+			 * 1: discrete GFX
+			 * 0: integrated GFX
+			 */
+			sony_call_snc_handle(handle, 0x0100, &result);
+
+			ev_type = GFX_SWITCH;
+			real_ev = result & 0xff;
+			break;
+
 		default:
 			dprintk("Unknown event 0x%x for handle 0x%x\n",
 					event, handle);
@@ -1238,7 +1263,7 @@ static void sony_nc_notify(struct acpi_device *device, u32 event)
 
 	} else {
 		/* old style event */
-		ev_type = 1;
+		ev_type = HOTKEY;
 		sony_laptop_report_input_event(real_ev);
 	}
 
@@ -1452,7 +1477,7 @@ static void sony_nc_function_resume(void)
 				&result);
 }
 
-static int sony_nc_resume(struct acpi_device *device)
+static int sony_nc_resume(struct device *dev)
 {
 	struct sony_nc_value *item;
 	acpi_handle handle;
@@ -1484,6 +1509,8 @@ static int sony_nc_resume(struct acpi_device *device)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(sony_nc_pm, NULL, sony_nc_resume);
+
 static void sony_nc_rfkill_cleanup(void)
 {
 	int i;
@@ -1893,32 +1920,33 @@ static ssize_t sony_nc_battery_care_limit_store(struct device *dev,
 	 *  bits 4,5: store the limit into the EC
 	 *  bits 6,7: store the limit into the battery
 	 */
+	cmd = 0;
 
-	/*
-	 * handle 0x0115 should allow storing on battery too;
-	 * handle 0x0136 same as 0x0115 + health status;
-	 * handle 0x013f, same as 0x0136 but no storing on the battery
-	 *
-	 * Store only inside the EC for now, regardless the handle number
-	 */
-	if (value == 0)
-		/* disable limits */
-		cmd = 0x0;
+	if (value > 0) {
+		if (value <= 50)
+			cmd = 0x20;
 
-	else if (value <= 50)
-		cmd = 0x21;
+		else if (value <= 80)
+			cmd = 0x10;
 
-	else if (value <= 80)
-		cmd = 0x11;
+		else if (value <= 100)
+			cmd = 0x30;
 
-	else if (value <= 100)
-		cmd = 0x31;
+		else
+			return -EINVAL;
 
-	else
-		return -EINVAL;
+		/*
+		 * handle 0x0115 should allow storing on battery too;
+		 * handle 0x0136 same as 0x0115 + health status;
+		 * handle 0x013f, same as 0x0136 but no storing on the battery
+		 */
+		if (bcare_ctl->handle != 0x013f)
+			cmd = cmd | (cmd << 2);
 
-	if (sony_call_snc_handle(bcare_ctl->handle, (cmd << 0x10) | 0x0100,
-				&result))
+		cmd = (cmd | 0x1) << 0x10;
+	}
+
+	if (sony_call_snc_handle(bcare_ctl->handle, cmd | 0x0100, &result))
 		return -EIO;
 
 	return count;
@@ -2113,7 +2141,7 @@ static ssize_t sony_nc_thermal_mode_show(struct device *dev,
 		struct device_attribute *attr, char *buffer)
 {
 	ssize_t count = 0;
-	unsigned int mode = sony_nc_thermal_mode_get();
+	int mode = sony_nc_thermal_mode_get();
 
 	if (mode < 0)
 		return mode;
@@ -2472,6 +2500,7 @@ static void sony_nc_backlight_ng_read_limits(int handle,
 {
 	u64 offset;
 	int i;
+	int lvl_table_len = 0;
 	u8 min = 0xff, max = 0x00;
 	unsigned char buffer[32] = { 0 };
 
@@ -2480,8 +2509,6 @@ static void sony_nc_backlight_ng_read_limits(int handle,
 	props->maxlvl = 0xff;
 
 	offset = sony_find_snc_handle(handle);
-	if (offset < 0)
-		return;
 
 	/* try to read the boundaries from ACPI tables, if we fail the above
 	 * defaults should be reasonable
@@ -2491,11 +2518,21 @@ static void sony_nc_backlight_ng_read_limits(int handle,
 	if (i < 0)
 		return;
 
+	switch (handle) {
+	case 0x012f:
+	case 0x0137:
+		lvl_table_len = 9;
+		break;
+	case 0x143:
+		lvl_table_len = 16;
+		break;
+	}
+
 	/* the buffer lists brightness levels available, brightness levels are
 	 * from position 0 to 8 in the array, other values are used by ALS
 	 * control.
 	 */
-	for (i = 0; i < 9 && i < ARRAY_SIZE(buffer); i++) {
+	for (i = 0; i < lvl_table_len && i < ARRAY_SIZE(buffer); i++) {
 
 		dprintk("Brightness level: %d\n", buffer[i]);
 
@@ -2520,16 +2557,24 @@ static void sony_nc_backlight_setup(void)
 	const struct backlight_ops *ops = NULL;
 	struct backlight_properties props;
 
-	if (sony_find_snc_handle(0x12f) != -1) {
+	if (sony_find_snc_handle(0x12f) >= 0) {
 		ops = &sony_backlight_ng_ops;
+		sony_bl_props.cmd_base = 0x0100;
 		sony_nc_backlight_ng_read_limits(0x12f, &sony_bl_props);
 		max_brightness = sony_bl_props.maxlvl - sony_bl_props.offset;
 
-	} else if (sony_find_snc_handle(0x137) != -1) {
+	} else if (sony_find_snc_handle(0x137) >= 0) {
 		ops = &sony_backlight_ng_ops;
+		sony_bl_props.cmd_base = 0x0100;
 		sony_nc_backlight_ng_read_limits(0x137, &sony_bl_props);
 		max_brightness = sony_bl_props.maxlvl - sony_bl_props.offset;
 
+	} else if (sony_find_snc_handle(0x143) >= 0) {
+		ops = &sony_backlight_ng_ops;
+		sony_bl_props.cmd_base = 0x3000;
+		sony_nc_backlight_ng_read_limits(0x143, &sony_bl_props);
+		max_brightness = sony_bl_props.maxlvl - sony_bl_props.offset;
+
 	} else if (ACPI_SUCCESS(acpi_get_handle(sony_nc_acpi_handle, "GBRT",
 						&unused))) {
 		ops = &sony_backlight_ops;
@@ -2597,6 +2642,12 @@ static int sony_nc_add(struct acpi_device *device)
 		}
 	}
 
+	result = sony_laptop_setup_input(device);
+	if (result) {
+		pr_err("Unable to create input devices\n");
+		goto outplatform;
+	}
+
 	if (ACPI_SUCCESS(acpi_get_handle(sony_nc_acpi_handle, "ECON",
 					 &handle))) {
 		int arg = 1;
@@ -2614,12 +2665,6 @@ static int sony_nc_add(struct acpi_device *device)
 	}
 
 	/* setup input devices and helper fifo */
-	result = sony_laptop_setup_input(device);
-	if (result) {
-		pr_err("Unable to create input devices\n");
-		goto outsnc;
-	}
-
 	if (acpi_video_backlight_support()) {
 		pr_info("brightness ignored, must be controlled by ACPI video driver\n");
 	} else {
@@ -2667,22 +2712,21 @@ static int sony_nc_add(struct acpi_device *device)
 
 	return 0;
 
-      out_sysfs:
+out_sysfs:
 	for (item = sony_nc_values; item->name; ++item) {
 		device_remove_file(&sony_pf_device->dev, &item->devattr);
 	}
 	sony_nc_backlight_cleanup();
-
-	sony_laptop_remove_input();
-
-      outsnc:
 	sony_nc_function_cleanup(sony_pf_device);
 	sony_nc_handles_cleanup(sony_pf_device);
 
-      outpresent:
+outplatform:
+	sony_laptop_remove_input();
+
+outpresent:
 	sony_pf_remove();
 
-      outwalk:
+outwalk:
 	sony_nc_rfkill_cleanup();
 	return result;
 }
@@ -2728,9 +2772,9 @@ static struct acpi_driver sony_nc_driver = {
 	.ops = {
 		.add = sony_nc_add,
 		.remove = sony_nc_remove,
-		.resume = sony_nc_resume,
 		.notify = sony_nc_notify,
 		},
+	.drv.pm = &sony_nc_pm,
 };
 
 /*********** SPIC (SNY6001) Device ***********/
@@ -4243,19 +4287,22 @@ err_free_resources:
 	return result;
 }
 
-static int sony_pic_suspend(struct acpi_device *device, pm_message_t state)
+static int sony_pic_suspend(struct device *dev)
 {
-	if (sony_pic_disable(device))
+	if (sony_pic_disable(to_acpi_device(dev)))
 		return -ENXIO;
 	return 0;
 }
 
-static int sony_pic_resume(struct acpi_device *device)
+static int sony_pic_resume(struct device *dev)
 {
-	sony_pic_enable(device, spic_dev.cur_ioport, spic_dev.cur_irq);
+	sony_pic_enable(to_acpi_device(dev),
+			spic_dev.cur_ioport, spic_dev.cur_irq);
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(sony_pic_pm, sony_pic_suspend, sony_pic_resume);
+
 static const struct acpi_device_id sony_pic_device_ids[] = {
 	{SONY_PIC_HID, 0},
 	{"", 0},
@@ -4269,9 +4316,8 @@ static struct acpi_driver sony_pic_driver = {
 	.ops = {
 		.add = sony_pic_add,
 		.remove = sony_pic_remove,
-		.suspend = sony_pic_suspend,
-		.resume = sony_pic_resume,
 		},
+	.drv.pm = &sony_pic_pm,
 };
 
 static struct dmi_system_id __initdata sonypi_dmi_table[] = {
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 8b5610d8841..e7f73287636 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -277,7 +277,7 @@ struct ibm_struct {
 	int (*write) (char *);
 	void (*exit) (void);
 	void (*resume) (void);
-	void (*suspend) (pm_message_t state);
+	void (*suspend) (void);
 	void (*shutdown) (void);
 
 	struct list_head all_drivers;
@@ -922,8 +922,7 @@ static struct input_dev *tpacpi_inputdev;
 static struct mutex tpacpi_inputdev_send_mutex;
 static LIST_HEAD(tpacpi_all_drivers);
 
-static int tpacpi_suspend_handler(struct platform_device *pdev,
-				  pm_message_t state)
+static int tpacpi_suspend_handler(struct device *dev)
 {
 	struct ibm_struct *ibm, *itmp;
 
@@ -931,13 +930,13 @@ static int tpacpi_suspend_handler(struct platform_device *pdev,
 				 &tpacpi_all_drivers,
 				 all_drivers) {
 		if (ibm->suspend)
-			(ibm->suspend)(state);
+			(ibm->suspend)();
 	}
 
 	return 0;
 }
 
-static int tpacpi_resume_handler(struct platform_device *pdev)
+static int tpacpi_resume_handler(struct device *dev)
 {
 	struct ibm_struct *ibm, *itmp;
 
@@ -951,6 +950,9 @@ static int tpacpi_resume_handler(struct platform_device *pdev)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(tpacpi_pm,
+			 tpacpi_suspend_handler, tpacpi_resume_handler);
+
 static void tpacpi_shutdown_handler(struct platform_device *pdev)
 {
 	struct ibm_struct *ibm, *itmp;
@@ -967,9 +969,8 @@ static struct platform_driver tpacpi_pdriver = {
 	.driver = {
 		.name = TPACPI_DRVR_NAME,
 		.owner = THIS_MODULE,
+		.pm = &tpacpi_pm,
 	},
-	.suspend = tpacpi_suspend_handler,
-	.resume = tpacpi_resume_handler,
 	.shutdown = tpacpi_shutdown_handler,
 };
 
@@ -3014,8 +3015,6 @@ static void hotkey_exit(void)
 	if (hotkey_dev_attributes)
 		delete_attr_set(hotkey_dev_attributes, &tpacpi_pdev->dev.kobj);
 
-	kfree(hotkey_keycode_map);
-
 	dbg_printk(TPACPI_DBG_EXIT | TPACPI_DBG_HKEY,
 		   "restoring original HKEY status and mask\n");
 	/* yes, there is a bitwise or below, we want the
@@ -3758,7 +3757,7 @@ static void hotkey_notify(struct ibm_struct *ibm, u32 event)
 	}
 }
 
-static void hotkey_suspend(pm_message_t state)
+static void hotkey_suspend(void)
 {
 	/* Do these on suspend, we get the events on early resume! */
 	hotkey_wakeup_reason = TP_ACPI_WAKEUP_NONE;
@@ -5216,6 +5215,7 @@ static void led_exit(void)
 			led_classdev_unregister(&tpacpi_leds[i].led_classdev);
 	}
 
+	flush_workqueue(tpacpi_wq);
 	kfree(tpacpi_leds);
 }
 
@@ -6329,7 +6329,7 @@ static int __init brightness_init(struct ibm_init_struct *iibm)
 	return 0;
 }
 
-static void brightness_suspend(pm_message_t state)
+static void brightness_suspend(void)
 {
 	tpacpi_brightness_checkpoint_nvram();
 }
@@ -6748,7 +6748,7 @@ static struct snd_kcontrol_new volume_alsa_control_mute __devinitdata = {
 	.get = volume_alsa_mute_get,
 };
 
-static void volume_suspend(pm_message_t state)
+static void volume_suspend(void)
 {
 	tpacpi_volume_checkpoint_nvram();
 }
@@ -8107,7 +8107,7 @@ static void fan_exit(void)
 	flush_workqueue(tpacpi_wq);
 }
 
-static void fan_suspend(pm_message_t state)
+static void fan_suspend(void)
 {
 	int rc;
 
@@ -8935,6 +8935,7 @@ static void thinkpad_acpi_module_exit(void)
 			input_unregister_device(tpacpi_inputdev);
 		else
 			input_free_device(tpacpi_inputdev);
+		kfree(hotkey_keycode_map);
 	}
 
 	if (tpacpi_hwmon)
@@ -8968,6 +8969,7 @@ static void thinkpad_acpi_module_exit(void)
 	kfree(thinkpad_id.bios_version_str);
 	kfree(thinkpad_id.ec_version_str);
 	kfree(thinkpad_id.model_str);
+	kfree(thinkpad_id.nummodel_str);
 }
 
 
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index dab10f6edcd..c13ba5bac93 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -1296,10 +1296,9 @@ static void toshiba_acpi_notify(struct acpi_device *acpi_dev, u32 event)
 	}
 }
 
-static int toshiba_acpi_suspend(struct acpi_device *acpi_dev,
-				pm_message_t state)
+static int toshiba_acpi_suspend(struct device *device)
 {
-	struct toshiba_acpi_dev *dev = acpi_driver_data(acpi_dev);
+	struct toshiba_acpi_dev *dev = acpi_driver_data(to_acpi_device(device));
 	u32 result;
 
 	if (dev->hotkey_dev)
@@ -1308,9 +1307,9 @@ static int toshiba_acpi_suspend(struct acpi_device *acpi_dev,
 	return 0;
 }
 
-static int toshiba_acpi_resume(struct acpi_device *acpi_dev)
+static int toshiba_acpi_resume(struct device *device)
 {
-	struct toshiba_acpi_dev *dev = acpi_driver_data(acpi_dev);
+	struct toshiba_acpi_dev *dev = acpi_driver_data(to_acpi_device(device));
 	u32 result;
 
 	if (dev->hotkey_dev)
@@ -1319,6 +1318,9 @@ static int toshiba_acpi_resume(struct acpi_device *acpi_dev)
 	return 0;
 }
 
+static SIMPLE_DEV_PM_OPS(toshiba_acpi_pm,
+			 toshiba_acpi_suspend, toshiba_acpi_resume);
+
 static struct acpi_driver toshiba_acpi_driver = {
 	.name	= "Toshiba ACPI driver",
 	.owner	= THIS_MODULE,
@@ -1328,9 +1330,8 @@ static struct acpi_driver toshiba_acpi_driver = {
 		.add		= toshiba_acpi_add,
 		.remove		= toshiba_acpi_remove,
 		.notify		= toshiba_acpi_notify,
-		.suspend	= toshiba_acpi_suspend,
-		.resume		= toshiba_acpi_resume,
 	},
+	.drv.pm	= &toshiba_acpi_pm,
 };
 
 static int __init toshiba_acpi_init(void)
diff --git a/drivers/platform/x86/toshiba_bluetooth.c b/drivers/platform/x86/toshiba_bluetooth.c
index 5fb7186694d..715a43cb5e3 100644
--- a/drivers/platform/x86/toshiba_bluetooth.c
+++ b/drivers/platform/x86/toshiba_bluetooth.c
@@ -34,7 +34,6 @@ MODULE_LICENSE("GPL");
 static int toshiba_bt_rfkill_add(struct acpi_device *device);
 static int toshiba_bt_rfkill_remove(struct acpi_device *device, int type);
 static void toshiba_bt_rfkill_notify(struct acpi_device *device, u32 event);
-static int toshiba_bt_resume(struct acpi_device *device);
 
 static const struct acpi_device_id bt_device_ids[] = {
 	{ "TOS6205", 0},
@@ -42,6 +41,9 @@ static const struct acpi_device_id bt_device_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, bt_device_ids);
 
+static int toshiba_bt_resume(struct device *dev);
+static SIMPLE_DEV_PM_OPS(toshiba_bt_pm, NULL, toshiba_bt_resume);
+
 static struct acpi_driver toshiba_bt_rfkill_driver = {
 	.name =		"Toshiba BT",
 	.class =	"Toshiba",
@@ -50,9 +52,9 @@ static struct acpi_driver toshiba_bt_rfkill_driver = {
 				.add =		toshiba_bt_rfkill_add,
 				.remove =	toshiba_bt_rfkill_remove,
 				.notify =	toshiba_bt_rfkill_notify,
-				.resume =	toshiba_bt_resume,
 			},
 	.owner = 	THIS_MODULE,
+	.drv.pm =	&toshiba_bt_pm,
 };
 
 
@@ -88,9 +90,9 @@ static void toshiba_bt_rfkill_notify(struct acpi_device *device, u32 event)
 	toshiba_bluetooth_enable(device->handle);
 }
 
-static int toshiba_bt_resume(struct acpi_device *device)
+static int toshiba_bt_resume(struct device *dev)
 {
-	return toshiba_bluetooth_enable(device->handle);
+	return toshiba_bluetooth_enable(to_acpi_device(dev)->handle);
 }
 
 static int toshiba_bt_rfkill_add(struct acpi_device *device)
diff --git a/drivers/platform/x86/xo1-rfkill.c b/drivers/platform/x86/xo1-rfkill.c
index b57ad864148..1da13ed34b0 100644
--- a/drivers/platform/x86/xo1-rfkill.c
+++ b/drivers/platform/x86/xo1-rfkill.c
@@ -12,8 +12,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/rfkill.h>
-
-#include <asm/olpc.h>
+#include <linux/olpc-ec.h>
 
 static bool card_blocked;
 
diff --git a/drivers/platform/x86/xo15-ebook.c b/drivers/platform/x86/xo15-ebook.c
index fad153dc035..849c07c13bf 100644
--- a/drivers/platform/x86/xo15-ebook.c
+++ b/drivers/platform/x86/xo15-ebook.c
@@ -77,11 +77,13 @@ static void ebook_switch_notify(struct acpi_device *device, u32 event)
 	}
 }
 
-static int ebook_switch_resume(struct acpi_device *device)
+static int ebook_switch_resume(struct device *dev)
 {
-	return ebook_send_state(device);
+	return ebook_send_state(to_acpi_device(dev));
 }
 
+static SIMPLE_DEV_PM_OPS(ebook_switch_pm, NULL, ebook_switch_resume);
+
 static int ebook_switch_add(struct acpi_device *device)
 {
 	struct ebook_switch *button;
@@ -161,10 +163,10 @@ static struct acpi_driver xo15_ebook_driver = {
 	.ids = ebook_device_ids,
 	.ops = {
 		.add = ebook_switch_add,
-		.resume = ebook_switch_resume,
 		.remove = ebook_switch_remove,
 		.notify = ebook_switch_notify,
 	},
+	.drv.pm = &ebook_switch_pm,
 };
 
 static int __init xo15_ebook_init(void)
diff --git a/drivers/pnp/pnpacpi/core.c b/drivers/pnp/pnpacpi/core.c
index d21e8f59c84..507a8e2b9a4 100644
--- a/drivers/pnp/pnpacpi/core.c
+++ b/drivers/pnp/pnpacpi/core.c
@@ -170,8 +170,8 @@ static int pnpacpi_suspend(struct pnp_dev *dev, pm_message_t state)
 	}
 
 	if (acpi_bus_power_manageable(handle)) {
-		int power_state = acpi_pm_device_sleep_state(&dev->dev, NULL);
-
+		int power_state = acpi_pm_device_sleep_state(&dev->dev, NULL,
+							     ACPI_STATE_D3);
 		if (power_state < 0)
 			power_state = (state.event == PM_EVENT_ON) ?
 					ACPI_STATE_D0 : ACPI_STATE_D3;
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index e3a3b4956f0..c1892f321c4 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -268,6 +268,7 @@ config CHARGER_GPIO
 config CHARGER_MANAGER
 	bool "Battery charger manager for multiple chargers"
 	depends on REGULATOR && RTC_CLASS
+	select EXTCON
 	help
           Say Y to enable charger-manager support, which allows multiple
           chargers attached to a battery and multiple batteries attached to a
@@ -301,7 +302,7 @@ config AB8500_BM
 	bool "AB8500 Battery Management Driver"
 	depends on AB8500_CORE && AB8500_GPADC
 	help
-	  Say Y to include support for AB5500 battery management.
+	  Say Y to include support for AB8500 battery management.
 
 config AB8500_BATTERY_THERM_ON_BATCTRL
 	bool "Thermistor connected on BATCTRL ADC"
@@ -310,3 +311,5 @@ config AB8500_BATTERY_THERM_ON_BATCTRL
 	  Say Y to enable battery temperature measurements using
 	  thermistor connected on BATCTRL ADC.
 endif # POWER_SUPPLY
+
+source "drivers/power/avs/Kconfig"
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index b6b243416c0..ee58afb1e71 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -43,4 +43,5 @@ obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
 obj-$(CONFIG_CHARGER_MANAGER)	+= charger-manager.o
 obj-$(CONFIG_CHARGER_MAX8997)	+= max8997_charger.o
 obj-$(CONFIG_CHARGER_MAX8998)	+= max8998_charger.o
+obj-$(CONFIG_POWER_AVS)		+= avs/
 obj-$(CONFIG_CHARGER_SMB347)	+= smb347-charger.o
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
index d2303d0b7c7..d4f0c98428c 100644
--- a/drivers/power/ab8500_charger.c
+++ b/drivers/power/ab8500_charger.c
@@ -2517,7 +2517,7 @@ static int __devexit ab8500_charger_remove(struct platform_device *pdev)
 		dev_err(di->dev, "%s mask and set failed\n", __func__);
 
 	usb_unregister_notifier(di->usb_phy, &di->nb);
-	usb_put_transceiver(di->usb_phy);
+	usb_put_phy(di->usb_phy);
 
 	/* Delete the work queue */
 	destroy_workqueue(di->charger_wq);
@@ -2688,8 +2688,8 @@ static int __devinit ab8500_charger_probe(struct platform_device *pdev)
 		goto free_ac;
 	}
 
-	di->usb_phy = usb_get_transceiver();
-	if (!di->usb_phy) {
+	di->usb_phy = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(di->usb_phy)) {
 		dev_err(di->dev, "failed to get usb transceiver\n");
 		ret = -EINVAL;
 		goto free_usb;
@@ -2747,7 +2747,7 @@ free_irq:
 		free_irq(irq, di);
 	}
 put_usb_phy:
-	usb_put_transceiver(di->usb_phy);
+	usb_put_phy(di->usb_phy);
 free_usb:
 	power_supply_unregister(&di->usb_chg.psy);
 free_ac:
diff --git a/drivers/power/avs/Kconfig b/drivers/power/avs/Kconfig
new file mode 100644
index 00000000000..2a1008b6112
--- /dev/null
+++ b/drivers/power/avs/Kconfig
@@ -0,0 +1,12 @@
+menuconfig POWER_AVS
+	bool "Adaptive Voltage Scaling class support"
+	help
+	  AVS is a power management technique which finely controls the
+	  operating voltage of a device in order to optimize (i.e. reduce)
+	  its power consumption.
+	  At a given operating point the voltage is adapted depending on
+	  static factors (chip manufacturing process) and dynamic factors
+	  (temperature depending performance).
+	  AVS is also called SmartReflex on OMAP devices.
+
+	  Say Y here to enable Adaptive Voltage Scaling class support.
diff --git a/drivers/power/avs/Makefile b/drivers/power/avs/Makefile
new file mode 100644
index 00000000000..0843386a6c1
--- /dev/null
+++ b/drivers/power/avs/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_POWER_AVS_OMAP)		+= smartreflex.o
diff --git a/drivers/power/avs/smartreflex.c b/drivers/power/avs/smartreflex.c
new file mode 100644
index 00000000000..44efc6e202a
--- /dev/null
+++ b/drivers/power/avs/smartreflex.c
@@ -0,0 +1,1126 @@
+/*
+ * OMAP SmartReflex Voltage Control
+ *
+ * Author: Thara Gopinath	<thara@ti.com>
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc.
+ * Thara Gopinath <thara@ti.com>
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Kalle Jokiniemi
+ *
+ * Copyright (C) 2007 Texas Instruments, Inc.
+ * Lesly A M <x0080970@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/power/smartreflex.h>
+
+#define SMARTREFLEX_NAME_LEN	16
+#define NVALUE_NAME_LEN		40
+#define SR_DISABLE_TIMEOUT	200
+
+/* sr_list contains all the instances of smartreflex module */
+static LIST_HEAD(sr_list);
+
+static struct omap_sr_class_data *sr_class;
+static struct omap_sr_pmic_data *sr_pmic_data;
+static struct dentry		*sr_dbg_dir;
+
+static inline void sr_write_reg(struct omap_sr *sr, unsigned offset, u32 value)
+{
+	__raw_writel(value, (sr->base + offset));
+}
+
+static inline void sr_modify_reg(struct omap_sr *sr, unsigned offset, u32 mask,
+					u32 value)
+{
+	u32 reg_val;
+
+	/*
+	 * Smartreflex error config register is special as it contains
+	 * certain status bits which if written a 1 into means a clear
+	 * of those bits. So in order to make sure no accidental write of
+	 * 1 happens to those status bits, do a clear of them in the read
+	 * value. This mean this API doesn't rewrite values in these bits
+	 * if they are currently set, but does allow the caller to write
+	 * those bits.
+	 */
+	if (sr->ip_type == SR_TYPE_V1 && offset == ERRCONFIG_V1)
+		mask |= ERRCONFIG_STATUS_V1_MASK;
+	else if (sr->ip_type == SR_TYPE_V2 && offset == ERRCONFIG_V2)
+		mask |= ERRCONFIG_VPBOUNDINTST_V2;
+
+	reg_val = __raw_readl(sr->base + offset);
+	reg_val &= ~mask;
+
+	value &= mask;
+
+	reg_val |= value;
+
+	__raw_writel(reg_val, (sr->base + offset));
+}
+
+static inline u32 sr_read_reg(struct omap_sr *sr, unsigned offset)
+{
+	return __raw_readl(sr->base + offset);
+}
+
+static struct omap_sr *_sr_lookup(struct voltagedomain *voltdm)
+{
+	struct omap_sr *sr_info;
+
+	if (!voltdm) {
+		pr_err("%s: Null voltage domain passed!\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	list_for_each_entry(sr_info, &sr_list, node) {
+		if (voltdm == sr_info->voltdm)
+			return sr_info;
+	}
+
+	return ERR_PTR(-ENODATA);
+}
+
+static irqreturn_t sr_interrupt(int irq, void *data)
+{
+	struct omap_sr *sr_info = data;
+	u32 status = 0;
+
+	switch (sr_info->ip_type) {
+	case SR_TYPE_V1:
+		/* Read the status bits */
+		status = sr_read_reg(sr_info, ERRCONFIG_V1);
+
+		/* Clear them by writing back */
+		sr_write_reg(sr_info, ERRCONFIG_V1, status);
+		break;
+	case SR_TYPE_V2:
+		/* Read the status bits */
+		status = sr_read_reg(sr_info, IRQSTATUS);
+
+		/* Clear them by writing back */
+		sr_write_reg(sr_info, IRQSTATUS, status);
+		break;
+	default:
+		dev_err(&sr_info->pdev->dev, "UNKNOWN IP type %d\n",
+			sr_info->ip_type);
+		return IRQ_NONE;
+	}
+
+	if (sr_class->notify)
+		sr_class->notify(sr_info, status);
+
+	return IRQ_HANDLED;
+}
+
+static void sr_set_clk_length(struct omap_sr *sr)
+{
+	struct clk *sys_ck;
+	u32 sys_clk_speed;
+
+	if (cpu_is_omap34xx())
+		sys_ck = clk_get(NULL, "sys_ck");
+	else
+		sys_ck = clk_get(NULL, "sys_clkin_ck");
+
+	if (IS_ERR(sys_ck)) {
+		dev_err(&sr->pdev->dev, "%s: unable to get sys clk\n",
+			__func__);
+		return;
+	}
+
+	sys_clk_speed = clk_get_rate(sys_ck);
+	clk_put(sys_ck);
+
+	switch (sys_clk_speed) {
+	case 12000000:
+		sr->clk_length = SRCLKLENGTH_12MHZ_SYSCLK;
+		break;
+	case 13000000:
+		sr->clk_length = SRCLKLENGTH_13MHZ_SYSCLK;
+		break;
+	case 19200000:
+		sr->clk_length = SRCLKLENGTH_19MHZ_SYSCLK;
+		break;
+	case 26000000:
+		sr->clk_length = SRCLKLENGTH_26MHZ_SYSCLK;
+		break;
+	case 38400000:
+		sr->clk_length = SRCLKLENGTH_38MHZ_SYSCLK;
+		break;
+	default:
+		dev_err(&sr->pdev->dev, "%s: Invalid sysclk value: %d\n",
+			__func__, sys_clk_speed);
+		break;
+	}
+}
+
+static void sr_set_regfields(struct omap_sr *sr)
+{
+	/*
+	 * For time being these values are defined in smartreflex.h
+	 * and populated during init. May be they can be moved to board
+	 * file or pmic specific data structure. In that case these structure
+	 * fields will have to be populated using the pdata or pmic structure.
+	 */
+	if (cpu_is_omap34xx() || cpu_is_omap44xx()) {
+		sr->err_weight = OMAP3430_SR_ERRWEIGHT;
+		sr->err_maxlimit = OMAP3430_SR_ERRMAXLIMIT;
+		sr->accum_data = OMAP3430_SR_ACCUMDATA;
+		if (!(strcmp(sr->name, "smartreflex_mpu_iva"))) {
+			sr->senn_avgweight = OMAP3430_SR1_SENNAVGWEIGHT;
+			sr->senp_avgweight = OMAP3430_SR1_SENPAVGWEIGHT;
+		} else {
+			sr->senn_avgweight = OMAP3430_SR2_SENNAVGWEIGHT;
+			sr->senp_avgweight = OMAP3430_SR2_SENPAVGWEIGHT;
+		}
+	}
+}
+
+static void sr_start_vddautocomp(struct omap_sr *sr)
+{
+	if (!sr_class || !(sr_class->enable) || !(sr_class->configure)) {
+		dev_warn(&sr->pdev->dev,
+			"%s: smartreflex class driver not registered\n",
+			__func__);
+		return;
+	}
+
+	if (!sr_class->enable(sr))
+		sr->autocomp_active = true;
+}
+
+static void sr_stop_vddautocomp(struct omap_sr *sr)
+{
+	if (!sr_class || !(sr_class->disable)) {
+		dev_warn(&sr->pdev->dev,
+			"%s: smartreflex class driver not registered\n",
+			__func__);
+		return;
+	}
+
+	if (sr->autocomp_active) {
+		sr_class->disable(sr, 1);
+		sr->autocomp_active = false;
+	}
+}
+
+/*
+ * This function handles the intializations which have to be done
+ * only when both sr device and class driver regiter has
+ * completed. This will be attempted to be called from both sr class
+ * driver register and sr device intializtion API's. Only one call
+ * will ultimately succeed.
+ *
+ * Currently this function registers interrupt handler for a particular SR
+ * if smartreflex class driver is already registered and has
+ * requested for interrupts and the SR interrupt line in present.
+ */
+static int sr_late_init(struct omap_sr *sr_info)
+{
+	struct omap_sr_data *pdata = sr_info->pdev->dev.platform_data;
+	struct resource *mem;
+	int ret = 0;
+
+	if (sr_class->notify && sr_class->notify_flags && sr_info->irq) {
+		ret = request_irq(sr_info->irq, sr_interrupt,
+				  0, sr_info->name, sr_info);
+		if (ret)
+			goto error;
+		disable_irq(sr_info->irq);
+	}
+
+	if (pdata && pdata->enable_on_init)
+		sr_start_vddautocomp(sr_info);
+
+	return ret;
+
+error:
+	iounmap(sr_info->base);
+	mem = platform_get_resource(sr_info->pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, resource_size(mem));
+	list_del(&sr_info->node);
+	dev_err(&sr_info->pdev->dev, "%s: ERROR in registering"
+		"interrupt handler. Smartreflex will"
+		"not function as desired\n", __func__);
+	kfree(sr_info);
+
+	return ret;
+}
+
+static void sr_v1_disable(struct omap_sr *sr)
+{
+	int timeout = 0;
+	int errconf_val = ERRCONFIG_MCUACCUMINTST | ERRCONFIG_MCUVALIDINTST |
+			ERRCONFIG_MCUBOUNDINTST;
+
+	/* Enable MCUDisableAcknowledge interrupt */
+	sr_modify_reg(sr, ERRCONFIG_V1,
+			ERRCONFIG_MCUDISACKINTEN, ERRCONFIG_MCUDISACKINTEN);
+
+	/* SRCONFIG - disable SR */
+	sr_modify_reg(sr, SRCONFIG, SRCONFIG_SRENABLE, 0x0);
+
+	/* Disable all other SR interrupts and clear the status as needed */
+	if (sr_read_reg(sr, ERRCONFIG_V1) & ERRCONFIG_VPBOUNDINTST_V1)
+		errconf_val |= ERRCONFIG_VPBOUNDINTST_V1;
+	sr_modify_reg(sr, ERRCONFIG_V1,
+			(ERRCONFIG_MCUACCUMINTEN | ERRCONFIG_MCUVALIDINTEN |
+			ERRCONFIG_MCUBOUNDINTEN | ERRCONFIG_VPBOUNDINTEN_V1),
+			errconf_val);
+
+	/*
+	 * Wait for SR to be disabled.
+	 * wait until ERRCONFIG.MCUDISACKINTST = 1. Typical latency is 1us.
+	 */
+	sr_test_cond_timeout((sr_read_reg(sr, ERRCONFIG_V1) &
+			     ERRCONFIG_MCUDISACKINTST), SR_DISABLE_TIMEOUT,
+			     timeout);
+
+	if (timeout >= SR_DISABLE_TIMEOUT)
+		dev_warn(&sr->pdev->dev, "%s: Smartreflex disable timedout\n",
+			__func__);
+
+	/* Disable MCUDisableAcknowledge interrupt & clear pending interrupt */
+	sr_modify_reg(sr, ERRCONFIG_V1, ERRCONFIG_MCUDISACKINTEN,
+			ERRCONFIG_MCUDISACKINTST);
+}
+
+static void sr_v2_disable(struct omap_sr *sr)
+{
+	int timeout = 0;
+
+	/* Enable MCUDisableAcknowledge interrupt */
+	sr_write_reg(sr, IRQENABLE_SET, IRQENABLE_MCUDISABLEACKINT);
+
+	/* SRCONFIG - disable SR */
+	sr_modify_reg(sr, SRCONFIG, SRCONFIG_SRENABLE, 0x0);
+
+	/*
+	 * Disable all other SR interrupts and clear the status
+	 * write to status register ONLY on need basis - only if status
+	 * is set.
+	 */
+	if (sr_read_reg(sr, ERRCONFIG_V2) & ERRCONFIG_VPBOUNDINTST_V2)
+		sr_modify_reg(sr, ERRCONFIG_V2, ERRCONFIG_VPBOUNDINTEN_V2,
+			ERRCONFIG_VPBOUNDINTST_V2);
+	else
+		sr_modify_reg(sr, ERRCONFIG_V2, ERRCONFIG_VPBOUNDINTEN_V2,
+				0x0);
+	sr_write_reg(sr, IRQENABLE_CLR, (IRQENABLE_MCUACCUMINT |
+			IRQENABLE_MCUVALIDINT |
+			IRQENABLE_MCUBOUNDSINT));
+	sr_write_reg(sr, IRQSTATUS, (IRQSTATUS_MCUACCUMINT |
+			IRQSTATUS_MCVALIDINT |
+			IRQSTATUS_MCBOUNDSINT));
+
+	/*
+	 * Wait for SR to be disabled.
+	 * wait until IRQSTATUS.MCUDISACKINTST = 1. Typical latency is 1us.
+	 */
+	sr_test_cond_timeout((sr_read_reg(sr, IRQSTATUS) &
+			     IRQSTATUS_MCUDISABLEACKINT), SR_DISABLE_TIMEOUT,
+			     timeout);
+
+	if (timeout >= SR_DISABLE_TIMEOUT)
+		dev_warn(&sr->pdev->dev, "%s: Smartreflex disable timedout\n",
+			__func__);
+
+	/* Disable MCUDisableAcknowledge interrupt & clear pending interrupt */
+	sr_write_reg(sr, IRQENABLE_CLR, IRQENABLE_MCUDISABLEACKINT);
+	sr_write_reg(sr, IRQSTATUS, IRQSTATUS_MCUDISABLEACKINT);
+}
+
+static struct omap_sr_nvalue_table *sr_retrieve_nvalue_row(
+				struct omap_sr *sr, u32 efuse_offs)
+{
+	int i;
+
+	if (!sr->nvalue_table) {
+		dev_warn(&sr->pdev->dev, "%s: Missing ntarget value table\n",
+			__func__);
+		return NULL;
+	}
+
+	for (i = 0; i < sr->nvalue_count; i++) {
+		if (sr->nvalue_table[i].efuse_offs == efuse_offs)
+			return &sr->nvalue_table[i];
+	}
+
+	return NULL;
+}
+
+/* Public Functions */
+
+/**
+ * sr_configure_errgen() - Configures the smrtreflex to perform AVS using the
+ *			 error generator module.
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the smartreflex class driver to
+ * configure the error generator module inside the smartreflex module.
+ * SR settings if using the ERROR module inside Smartreflex.
+ * SR CLASS 3 by default uses only the ERROR module where as
+ * SR CLASS 2 can choose between ERROR module and MINMAXAVG
+ * module. Returns 0 on success and error value in case of failure.
+ */
+int sr_configure_errgen(struct voltagedomain *voltdm)
+{
+	u32 sr_config, sr_errconfig, errconfig_offs;
+	u32 vpboundint_en, vpboundint_st;
+	u32 senp_en = 0, senn_en = 0;
+	u8 senp_shift, senn_shift;
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return PTR_ERR(sr);
+	}
+
+	if (!sr->clk_length)
+		sr_set_clk_length(sr);
+
+	senp_en = sr->senp_mod;
+	senn_en = sr->senn_mod;
+
+	sr_config = (sr->clk_length << SRCONFIG_SRCLKLENGTH_SHIFT) |
+		SRCONFIG_SENENABLE | SRCONFIG_ERRGEN_EN;
+
+	switch (sr->ip_type) {
+	case SR_TYPE_V1:
+		sr_config |= SRCONFIG_DELAYCTRL;
+		senn_shift = SRCONFIG_SENNENABLE_V1_SHIFT;
+		senp_shift = SRCONFIG_SENPENABLE_V1_SHIFT;
+		errconfig_offs = ERRCONFIG_V1;
+		vpboundint_en = ERRCONFIG_VPBOUNDINTEN_V1;
+		vpboundint_st = ERRCONFIG_VPBOUNDINTST_V1;
+		break;
+	case SR_TYPE_V2:
+		senn_shift = SRCONFIG_SENNENABLE_V2_SHIFT;
+		senp_shift = SRCONFIG_SENPENABLE_V2_SHIFT;
+		errconfig_offs = ERRCONFIG_V2;
+		vpboundint_en = ERRCONFIG_VPBOUNDINTEN_V2;
+		vpboundint_st = ERRCONFIG_VPBOUNDINTST_V2;
+		break;
+	default:
+		dev_err(&sr->pdev->dev, "%s: Trying to Configure smartreflex"
+			"module without specifying the ip\n", __func__);
+		return -EINVAL;
+	}
+
+	sr_config |= ((senn_en << senn_shift) | (senp_en << senp_shift));
+	sr_write_reg(sr, SRCONFIG, sr_config);
+	sr_errconfig = (sr->err_weight << ERRCONFIG_ERRWEIGHT_SHIFT) |
+		(sr->err_maxlimit << ERRCONFIG_ERRMAXLIMIT_SHIFT) |
+		(sr->err_minlimit <<  ERRCONFIG_ERRMINLIMIT_SHIFT);
+	sr_modify_reg(sr, errconfig_offs, (SR_ERRWEIGHT_MASK |
+		SR_ERRMAXLIMIT_MASK | SR_ERRMINLIMIT_MASK),
+		sr_errconfig);
+
+	/* Enabling the interrupts if the ERROR module is used */
+	sr_modify_reg(sr, errconfig_offs, (vpboundint_en | vpboundint_st),
+		      vpboundint_en);
+
+	return 0;
+}
+
+/**
+ * sr_disable_errgen() - Disables SmartReflex AVS module's errgen component
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the smartreflex class driver to
+ * disable the error generator module inside the smartreflex module.
+ *
+ * Returns 0 on success and error value in case of failure.
+ */
+int sr_disable_errgen(struct voltagedomain *voltdm)
+{
+	u32 errconfig_offs;
+	u32 vpboundint_en, vpboundint_st;
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return PTR_ERR(sr);
+	}
+
+	switch (sr->ip_type) {
+	case SR_TYPE_V1:
+		errconfig_offs = ERRCONFIG_V1;
+		vpboundint_en = ERRCONFIG_VPBOUNDINTEN_V1;
+		vpboundint_st = ERRCONFIG_VPBOUNDINTST_V1;
+		break;
+	case SR_TYPE_V2:
+		errconfig_offs = ERRCONFIG_V2;
+		vpboundint_en = ERRCONFIG_VPBOUNDINTEN_V2;
+		vpboundint_st = ERRCONFIG_VPBOUNDINTST_V2;
+		break;
+	default:
+		dev_err(&sr->pdev->dev, "%s: Trying to Configure smartreflex"
+			"module without specifying the ip\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Disable the interrupts of ERROR module */
+	sr_modify_reg(sr, errconfig_offs, vpboundint_en | vpboundint_st, 0);
+
+	/* Disable the Sensor and errorgen */
+	sr_modify_reg(sr, SRCONFIG, SRCONFIG_SENENABLE | SRCONFIG_ERRGEN_EN, 0);
+
+	return 0;
+}
+
+/**
+ * sr_configure_minmax() - Configures the smrtreflex to perform AVS using the
+ *			 minmaxavg module.
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the smartreflex class driver to
+ * configure the minmaxavg module inside the smartreflex module.
+ * SR settings if using the ERROR module inside Smartreflex.
+ * SR CLASS 3 by default uses only the ERROR module where as
+ * SR CLASS 2 can choose between ERROR module and MINMAXAVG
+ * module. Returns 0 on success and error value in case of failure.
+ */
+int sr_configure_minmax(struct voltagedomain *voltdm)
+{
+	u32 sr_config, sr_avgwt;
+	u32 senp_en = 0, senn_en = 0;
+	u8 senp_shift, senn_shift;
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return PTR_ERR(sr);
+	}
+
+	if (!sr->clk_length)
+		sr_set_clk_length(sr);
+
+	senp_en = sr->senp_mod;
+	senn_en = sr->senn_mod;
+
+	sr_config = (sr->clk_length << SRCONFIG_SRCLKLENGTH_SHIFT) |
+		SRCONFIG_SENENABLE |
+		(sr->accum_data << SRCONFIG_ACCUMDATA_SHIFT);
+
+	switch (sr->ip_type) {
+	case SR_TYPE_V1:
+		sr_config |= SRCONFIG_DELAYCTRL;
+		senn_shift = SRCONFIG_SENNENABLE_V1_SHIFT;
+		senp_shift = SRCONFIG_SENPENABLE_V1_SHIFT;
+		break;
+	case SR_TYPE_V2:
+		senn_shift = SRCONFIG_SENNENABLE_V2_SHIFT;
+		senp_shift = SRCONFIG_SENPENABLE_V2_SHIFT;
+		break;
+	default:
+		dev_err(&sr->pdev->dev, "%s: Trying to Configure smartreflex"
+			"module without specifying the ip\n", __func__);
+		return -EINVAL;
+	}
+
+	sr_config |= ((senn_en << senn_shift) | (senp_en << senp_shift));
+	sr_write_reg(sr, SRCONFIG, sr_config);
+	sr_avgwt = (sr->senp_avgweight << AVGWEIGHT_SENPAVGWEIGHT_SHIFT) |
+		(sr->senn_avgweight << AVGWEIGHT_SENNAVGWEIGHT_SHIFT);
+	sr_write_reg(sr, AVGWEIGHT, sr_avgwt);
+
+	/*
+	 * Enabling the interrupts if MINMAXAVG module is used.
+	 * TODO: check if all the interrupts are mandatory
+	 */
+	switch (sr->ip_type) {
+	case SR_TYPE_V1:
+		sr_modify_reg(sr, ERRCONFIG_V1,
+			(ERRCONFIG_MCUACCUMINTEN | ERRCONFIG_MCUVALIDINTEN |
+			ERRCONFIG_MCUBOUNDINTEN),
+			(ERRCONFIG_MCUACCUMINTEN | ERRCONFIG_MCUACCUMINTST |
+			 ERRCONFIG_MCUVALIDINTEN | ERRCONFIG_MCUVALIDINTST |
+			 ERRCONFIG_MCUBOUNDINTEN | ERRCONFIG_MCUBOUNDINTST));
+		break;
+	case SR_TYPE_V2:
+		sr_write_reg(sr, IRQSTATUS,
+			IRQSTATUS_MCUACCUMINT | IRQSTATUS_MCVALIDINT |
+			IRQSTATUS_MCBOUNDSINT | IRQSTATUS_MCUDISABLEACKINT);
+		sr_write_reg(sr, IRQENABLE_SET,
+			IRQENABLE_MCUACCUMINT | IRQENABLE_MCUVALIDINT |
+			IRQENABLE_MCUBOUNDSINT | IRQENABLE_MCUDISABLEACKINT);
+		break;
+	default:
+		dev_err(&sr->pdev->dev, "%s: Trying to Configure smartreflex"
+			"module without specifying the ip\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * sr_enable() - Enables the smartreflex module.
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ * @volt:	The voltage at which the Voltage domain associated with
+ *		the smartreflex module is operating at.
+ *		This is required only to program the correct Ntarget value.
+ *
+ * This API is to be called from the smartreflex class driver to
+ * enable a smartreflex module. Returns 0 on success. Returns error
+ * value if the voltage passed is wrong or if ntarget value is wrong.
+ */
+int sr_enable(struct voltagedomain *voltdm, unsigned long volt)
+{
+	struct omap_volt_data *volt_data;
+	struct omap_sr *sr = _sr_lookup(voltdm);
+	struct omap_sr_nvalue_table *nvalue_row;
+	int ret;
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return PTR_ERR(sr);
+	}
+
+	volt_data = omap_voltage_get_voltdata(sr->voltdm, volt);
+
+	if (IS_ERR(volt_data)) {
+		dev_warn(&sr->pdev->dev, "%s: Unable to get voltage table"
+			"for nominal voltage %ld\n", __func__, volt);
+		return PTR_ERR(volt_data);
+	}
+
+	nvalue_row = sr_retrieve_nvalue_row(sr, volt_data->sr_efuse_offs);
+
+	if (!nvalue_row) {
+		dev_warn(&sr->pdev->dev, "%s: failure getting SR data for this voltage %ld\n",
+			 __func__, volt);
+		return -ENODATA;
+	}
+
+	/* errminlimit is opp dependent and hence linked to voltage */
+	sr->err_minlimit = nvalue_row->errminlimit;
+
+	pm_runtime_get_sync(&sr->pdev->dev);
+
+	/* Check if SR is already enabled. If yes do nothing */
+	if (sr_read_reg(sr, SRCONFIG) & SRCONFIG_SRENABLE)
+		return 0;
+
+	/* Configure SR */
+	ret = sr_class->configure(sr);
+	if (ret)
+		return ret;
+
+	sr_write_reg(sr, NVALUERECIPROCAL, nvalue_row->nvalue);
+
+	/* SRCONFIG - enable SR */
+	sr_modify_reg(sr, SRCONFIG, SRCONFIG_SRENABLE, SRCONFIG_SRENABLE);
+	return 0;
+}
+
+/**
+ * sr_disable() - Disables the smartreflex module.
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the smartreflex class driver to
+ * disable a smartreflex module.
+ */
+void sr_disable(struct voltagedomain *voltdm)
+{
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return;
+	}
+
+	/* Check if SR clocks are already disabled. If yes do nothing */
+	if (pm_runtime_suspended(&sr->pdev->dev))
+		return;
+
+	/*
+	 * Disable SR if only it is indeed enabled. Else just
+	 * disable the clocks.
+	 */
+	if (sr_read_reg(sr, SRCONFIG) & SRCONFIG_SRENABLE) {
+		switch (sr->ip_type) {
+		case SR_TYPE_V1:
+			sr_v1_disable(sr);
+			break;
+		case SR_TYPE_V2:
+			sr_v2_disable(sr);
+			break;
+		default:
+			dev_err(&sr->pdev->dev, "UNKNOWN IP type %d\n",
+				sr->ip_type);
+		}
+	}
+
+	pm_runtime_put_sync_suspend(&sr->pdev->dev);
+}
+
+/**
+ * sr_register_class() - API to register a smartreflex class parameters.
+ * @class_data:	The structure containing various sr class specific data.
+ *
+ * This API is to be called by the smartreflex class driver to register itself
+ * with the smartreflex driver during init. Returns 0 on success else the
+ * error value.
+ */
+int sr_register_class(struct omap_sr_class_data *class_data)
+{
+	struct omap_sr *sr_info;
+
+	if (!class_data) {
+		pr_warning("%s:, Smartreflex class data passed is NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (sr_class) {
+		pr_warning("%s: Smartreflex class driver already registered\n",
+			__func__);
+		return -EBUSY;
+	}
+
+	sr_class = class_data;
+
+	/*
+	 * Call into late init to do intializations that require
+	 * both sr driver and sr class driver to be initiallized.
+	 */
+	list_for_each_entry(sr_info, &sr_list, node)
+		sr_late_init(sr_info);
+
+	return 0;
+}
+
+/**
+ * omap_sr_enable() -  API to enable SR clocks and to call into the
+ *			registered smartreflex class enable API.
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the kernel in order to enable
+ * a particular smartreflex module. This API will do the initial
+ * configurations to turn on the smartreflex module and in turn call
+ * into the registered smartreflex class enable API.
+ */
+void omap_sr_enable(struct voltagedomain *voltdm)
+{
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return;
+	}
+
+	if (!sr->autocomp_active)
+		return;
+
+	if (!sr_class || !(sr_class->enable) || !(sr_class->configure)) {
+		dev_warn(&sr->pdev->dev, "%s: smartreflex class driver not"
+			"registered\n", __func__);
+		return;
+	}
+
+	sr_class->enable(sr);
+}
+
+/**
+ * omap_sr_disable() - API to disable SR without resetting the voltage
+ *			processor voltage
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the kernel in order to disable
+ * a particular smartreflex module. This API will in turn call
+ * into the registered smartreflex class disable API. This API will tell
+ * the smartreflex class disable not to reset the VP voltage after
+ * disabling smartreflex.
+ */
+void omap_sr_disable(struct voltagedomain *voltdm)
+{
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return;
+	}
+
+	if (!sr->autocomp_active)
+		return;
+
+	if (!sr_class || !(sr_class->disable)) {
+		dev_warn(&sr->pdev->dev, "%s: smartreflex class driver not"
+			"registered\n", __func__);
+		return;
+	}
+
+	sr_class->disable(sr, 0);
+}
+
+/**
+ * omap_sr_disable_reset_volt() - API to disable SR and reset the
+ *				voltage processor voltage
+ * @voltdm:	VDD pointer to which the SR module to be configured belongs to.
+ *
+ * This API is to be called from the kernel in order to disable
+ * a particular smartreflex module. This API will in turn call
+ * into the registered smartreflex class disable API. This API will tell
+ * the smartreflex class disable to reset the VP voltage after
+ * disabling smartreflex.
+ */
+void omap_sr_disable_reset_volt(struct voltagedomain *voltdm)
+{
+	struct omap_sr *sr = _sr_lookup(voltdm);
+
+	if (IS_ERR(sr)) {
+		pr_warning("%s: omap_sr struct for voltdm not found\n",	__func__);
+		return;
+	}
+
+	if (!sr->autocomp_active)
+		return;
+
+	if (!sr_class || !(sr_class->disable)) {
+		dev_warn(&sr->pdev->dev, "%s: smartreflex class driver not"
+			"registered\n", __func__);
+		return;
+	}
+
+	sr_class->disable(sr, 1);
+}
+
+/**
+ * omap_sr_register_pmic() - API to register pmic specific info.
+ * @pmic_data:	The structure containing pmic specific data.
+ *
+ * This API is to be called from the PMIC specific code to register with
+ * smartreflex driver pmic specific info. Currently the only info required
+ * is the smartreflex init on the PMIC side.
+ */
+void omap_sr_register_pmic(struct omap_sr_pmic_data *pmic_data)
+{
+	if (!pmic_data) {
+		pr_warning("%s: Trying to register NULL PMIC data structure"
+			"with smartreflex\n", __func__);
+		return;
+	}
+
+	sr_pmic_data = pmic_data;
+}
+
+/* PM Debug FS entries to enable and disable smartreflex. */
+static int omap_sr_autocomp_show(void *data, u64 *val)
+{
+	struct omap_sr *sr_info = data;
+
+	if (!sr_info) {
+		pr_warning("%s: omap_sr struct not found\n", __func__);
+		return -EINVAL;
+	}
+
+	*val = sr_info->autocomp_active;
+
+	return 0;
+}
+
+static int omap_sr_autocomp_store(void *data, u64 val)
+{
+	struct omap_sr *sr_info = data;
+
+	if (!sr_info) {
+		pr_warning("%s: omap_sr struct not found\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Sanity check */
+	if (val > 1) {
+		pr_warning("%s: Invalid argument %lld\n", __func__, val);
+		return -EINVAL;
+	}
+
+	/* control enable/disable only if there is a delta in value */
+	if (sr_info->autocomp_active != val) {
+		if (!val)
+			sr_stop_vddautocomp(sr_info);
+		else
+			sr_start_vddautocomp(sr_info);
+	}
+
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(pm_sr_fops, omap_sr_autocomp_show,
+			omap_sr_autocomp_store, "%llu\n");
+
+static int __init omap_sr_probe(struct platform_device *pdev)
+{
+	struct omap_sr *sr_info;
+	struct omap_sr_data *pdata = pdev->dev.platform_data;
+	struct resource *mem, *irq;
+	struct dentry *nvalue_dir;
+	int i, ret = 0;
+
+	sr_info = kzalloc(sizeof(struct omap_sr), GFP_KERNEL);
+	if (!sr_info) {
+		dev_err(&pdev->dev, "%s: unable to allocate sr_info\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, sr_info);
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "%s: platform data missing\n", __func__);
+		ret = -EINVAL;
+		goto err_free_devinfo;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		dev_err(&pdev->dev, "%s: no mem resource\n", __func__);
+		ret = -ENODEV;
+		goto err_free_devinfo;
+	}
+
+	mem = request_mem_region(mem->start, resource_size(mem),
+					dev_name(&pdev->dev));
+	if (!mem) {
+		dev_err(&pdev->dev, "%s: no mem region\n", __func__);
+		ret = -EBUSY;
+		goto err_free_devinfo;
+	}
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_irq_safe(&pdev->dev);
+
+	sr_info->name = kasprintf(GFP_KERNEL, "%s", pdata->name);
+	if (!sr_info->name) {
+		dev_err(&pdev->dev, "%s: Unable to alloc SR instance name\n",
+			__func__);
+		ret = -ENOMEM;
+		goto err_release_region;
+	}
+
+	sr_info->pdev = pdev;
+	sr_info->srid = pdev->id;
+	sr_info->voltdm = pdata->voltdm;
+	sr_info->nvalue_table = pdata->nvalue_table;
+	sr_info->nvalue_count = pdata->nvalue_count;
+	sr_info->senn_mod = pdata->senn_mod;
+	sr_info->senp_mod = pdata->senp_mod;
+	sr_info->autocomp_active = false;
+	sr_info->ip_type = pdata->ip_type;
+	sr_info->base = ioremap(mem->start, resource_size(mem));
+	if (!sr_info->base) {
+		dev_err(&pdev->dev, "%s: ioremap fail\n", __func__);
+		ret = -ENOMEM;
+		goto err_release_region;
+	}
+
+	if (irq)
+		sr_info->irq = irq->start;
+
+	sr_set_clk_length(sr_info);
+	sr_set_regfields(sr_info);
+
+	list_add(&sr_info->node, &sr_list);
+
+	/*
+	 * Call into late init to do intializations that require
+	 * both sr driver and sr class driver to be initiallized.
+	 */
+	if (sr_class) {
+		ret = sr_late_init(sr_info);
+		if (ret) {
+			pr_warning("%s: Error in SR late init\n", __func__);
+			goto err_iounmap;
+		}
+	}
+
+	dev_info(&pdev->dev, "%s: SmartReflex driver initialized\n", __func__);
+	if (!sr_dbg_dir) {
+		sr_dbg_dir = debugfs_create_dir("smartreflex", NULL);
+		if (IS_ERR_OR_NULL(sr_dbg_dir)) {
+			ret = PTR_ERR(sr_dbg_dir);
+			pr_err("%s:sr debugfs dir creation failed(%d)\n",
+				__func__, ret);
+			goto err_iounmap;
+		}
+	}
+
+	sr_info->dbg_dir = debugfs_create_dir(sr_info->name, sr_dbg_dir);
+	if (IS_ERR_OR_NULL(sr_info->dbg_dir)) {
+		dev_err(&pdev->dev, "%s: Unable to create debugfs directory\n",
+			__func__);
+		ret = PTR_ERR(sr_info->dbg_dir);
+		goto err_free_name;
+	}
+
+	(void) debugfs_create_file("autocomp", S_IRUGO | S_IWUSR,
+			sr_info->dbg_dir, (void *)sr_info, &pm_sr_fops);
+	(void) debugfs_create_x32("errweight", S_IRUGO, sr_info->dbg_dir,
+			&sr_info->err_weight);
+	(void) debugfs_create_x32("errmaxlimit", S_IRUGO, sr_info->dbg_dir,
+			&sr_info->err_maxlimit);
+
+	nvalue_dir = debugfs_create_dir("nvalue", sr_info->dbg_dir);
+	if (IS_ERR_OR_NULL(nvalue_dir)) {
+		dev_err(&pdev->dev, "%s: Unable to create debugfs directory"
+			"for n-values\n", __func__);
+		ret = PTR_ERR(nvalue_dir);
+		goto err_debugfs;
+	}
+
+	if (sr_info->nvalue_count == 0 || !sr_info->nvalue_table) {
+		dev_warn(&pdev->dev, "%s: %s: No Voltage table for the corresponding vdd. Cannot create debugfs entries for n-values\n",
+			 __func__, sr_info->name);
+
+		ret = -ENODATA;
+		goto err_debugfs;
+	}
+
+	for (i = 0; i < sr_info->nvalue_count; i++) {
+		char name[NVALUE_NAME_LEN + 1];
+
+		snprintf(name, sizeof(name), "volt_%lu",
+				sr_info->nvalue_table[i].volt_nominal);
+		(void) debugfs_create_x32(name, S_IRUGO | S_IWUSR, nvalue_dir,
+				&(sr_info->nvalue_table[i].nvalue));
+		snprintf(name, sizeof(name), "errminlimit_%lu",
+			 sr_info->nvalue_table[i].volt_nominal);
+		(void) debugfs_create_x32(name, S_IRUGO | S_IWUSR, nvalue_dir,
+				&(sr_info->nvalue_table[i].errminlimit));
+
+	}
+
+	return ret;
+
+err_debugfs:
+	debugfs_remove_recursive(sr_info->dbg_dir);
+err_free_name:
+	kfree(sr_info->name);
+err_iounmap:
+	list_del(&sr_info->node);
+	iounmap(sr_info->base);
+err_release_region:
+	release_mem_region(mem->start, resource_size(mem));
+err_free_devinfo:
+	kfree(sr_info);
+
+	return ret;
+}
+
+static int __devexit omap_sr_remove(struct platform_device *pdev)
+{
+	struct omap_sr_data *pdata = pdev->dev.platform_data;
+	struct omap_sr *sr_info;
+	struct resource *mem;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "%s: platform data missing\n", __func__);
+		return -EINVAL;
+	}
+
+	sr_info = _sr_lookup(pdata->voltdm);
+	if (IS_ERR(sr_info)) {
+		dev_warn(&pdev->dev, "%s: omap_sr struct not found\n",
+			__func__);
+		return PTR_ERR(sr_info);
+	}
+
+	if (sr_info->autocomp_active)
+		sr_stop_vddautocomp(sr_info);
+	if (sr_info->dbg_dir)
+		debugfs_remove_recursive(sr_info->dbg_dir);
+
+	list_del(&sr_info->node);
+	iounmap(sr_info->base);
+	kfree(sr_info->name);
+	kfree(sr_info);
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(mem->start, resource_size(mem));
+
+	return 0;
+}
+
+static void __devexit omap_sr_shutdown(struct platform_device *pdev)
+{
+	struct omap_sr_data *pdata = pdev->dev.platform_data;
+	struct omap_sr *sr_info;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "%s: platform data missing\n", __func__);
+		return;
+	}
+
+	sr_info = _sr_lookup(pdata->voltdm);
+	if (IS_ERR(sr_info)) {
+		dev_warn(&pdev->dev, "%s: omap_sr struct not found\n",
+			__func__);
+		return;
+	}
+
+	if (sr_info->autocomp_active)
+		sr_stop_vddautocomp(sr_info);
+
+	return;
+}
+
+static struct platform_driver smartreflex_driver = {
+	.remove         = __devexit_p(omap_sr_remove),
+	.shutdown	= __devexit_p(omap_sr_shutdown),
+	.driver		= {
+		.name	= "smartreflex",
+	},
+};
+
+static int __init sr_init(void)
+{
+	int ret = 0;
+
+	/*
+	 * sr_init is a late init. If by then a pmic specific API is not
+	 * registered either there is no need for anything to be done on
+	 * the PMIC side or somebody has forgotten to register a PMIC
+	 * handler. Warn for the second condition.
+	 */
+	if (sr_pmic_data && sr_pmic_data->sr_pmic_init)
+		sr_pmic_data->sr_pmic_init();
+	else
+		pr_warning("%s: No PMIC hook to init smartreflex\n", __func__);
+
+	ret = platform_driver_probe(&smartreflex_driver, omap_sr_probe);
+	if (ret) {
+		pr_err("%s: platform driver register failed for SR\n",
+			__func__);
+		return ret;
+	}
+
+	return 0;
+}
+late_initcall(sr_init);
+
+static void __exit sr_exit(void)
+{
+	platform_driver_unregister(&smartreflex_driver);
+}
+module_exit(sr_exit);
+
+MODULE_DESCRIPTION("OMAP Smartreflex Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRIVER_NAME);
+MODULE_AUTHOR("Texas Instruments Inc");
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index f5d6d379f2f..181ddece518 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -22,6 +22,7 @@
  * Datasheets:
  * http://focus.ti.com/docs/prod/folders/print/bq27000.html
  * http://focus.ti.com/docs/prod/folders/print/bq27500.html
+ * http://www.ti.com/product/bq27425-g1
  */
 
 #include <linux/module.h>
@@ -51,6 +52,7 @@
 #define BQ27x00_REG_LMD			0x12 /* Last measured discharge */
 #define BQ27x00_REG_CYCT		0x2A /* Cycle count total */
 #define BQ27x00_REG_AE			0x22 /* Available energy */
+#define BQ27x00_POWER_AVG		0x24
 
 #define BQ27000_REG_RSOC		0x0B /* Relative State-of-Charge */
 #define BQ27000_REG_ILMD		0x76 /* Initial last measured discharge */
@@ -66,15 +68,21 @@
 #define BQ27500_FLAG_SOCF		BIT(1) /* State-of-Charge threshold final */
 #define BQ27500_FLAG_SOC1		BIT(2) /* State-of-Charge threshold 1 */
 #define BQ27500_FLAG_FC			BIT(9)
+#define BQ27500_FLAG_OTC		BIT(15)
+
+/* bq27425 register addresses are same as bq27x00 addresses minus 4 */
+#define BQ27425_REG_OFFSET		0x04
+#define BQ27425_REG_SOC			0x18 /* Register address plus offset */
 
 #define BQ27000_RS			20 /* Resistor sense */
+#define BQ27x00_POWER_CONSTANT		(256 * 29200 / 1000)
 
 struct bq27x00_device_info;
 struct bq27x00_access_methods {
 	int (*read)(struct bq27x00_device_info *di, u8 reg, bool single);
 };
 
-enum bq27x00_chip { BQ27000, BQ27500 };
+enum bq27x00_chip { BQ27000, BQ27500, BQ27425};
 
 struct bq27x00_reg_cache {
 	int temperature;
@@ -86,6 +94,8 @@ struct bq27x00_reg_cache {
 	int capacity;
 	int energy;
 	int flags;
+	int power_avg;
+	int health;
 };
 
 struct bq27x00_device_info {
@@ -123,6 +133,22 @@ static enum power_supply_property bq27x00_battery_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
 	POWER_SUPPLY_PROP_CYCLE_COUNT,
 	POWER_SUPPLY_PROP_ENERGY_NOW,
+	POWER_SUPPLY_PROP_POWER_AVG,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
+static enum power_supply_property bq27425_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
 };
 
 static unsigned int poll_interval = 360;
@@ -137,10 +163,24 @@ MODULE_PARM_DESC(poll_interval, "battery poll interval in seconds - " \
 static inline int bq27x00_read(struct bq27x00_device_info *di, u8 reg,
 		bool single)
 {
+	if (di->chip == BQ27425)
+		return di->bus.read(di, reg - BQ27425_REG_OFFSET, single);
 	return di->bus.read(di, reg, single);
 }
 
 /*
+ * Higher versions of the chip like BQ27425 and BQ27500
+ * differ from BQ27000 and BQ27200 in calculation of certain
+ * parameters. Hence we need to check for the chip type.
+ */
+static bool bq27xxx_is_chip_version_higher(struct bq27x00_device_info *di)
+{
+	if (di->chip == BQ27425 || di->chip == BQ27500)
+		return true;
+	return false;
+}
+
+/*
  * Return the battery Relative State-of-Charge
  * Or < 0 if something fails.
  */
@@ -150,6 +190,8 @@ static int bq27x00_battery_read_rsoc(struct bq27x00_device_info *di)
 
 	if (di->chip == BQ27500)
 		rsoc = bq27x00_read(di, BQ27500_REG_SOC, false);
+	else if (di->chip == BQ27425)
+		rsoc = bq27x00_read(di, BQ27425_REG_SOC, false);
 	else
 		rsoc = bq27x00_read(di, BQ27000_REG_RSOC, true);
 
@@ -174,7 +216,7 @@ static int bq27x00_battery_read_charge(struct bq27x00_device_info *di, u8 reg)
 		return charge;
 	}
 
-	if (di->chip == BQ27500)
+	if (bq27xxx_is_chip_version_higher(di))
 		charge *= 1000;
 	else
 		charge = charge * 3570 / BQ27000_RS;
@@ -208,7 +250,7 @@ static int bq27x00_battery_read_ilmd(struct bq27x00_device_info *di)
 {
 	int ilmd;
 
-	if (di->chip == BQ27500)
+	if (bq27xxx_is_chip_version_higher(di))
 		ilmd = bq27x00_read(di, BQ27500_REG_DCAP, false);
 	else
 		ilmd = bq27x00_read(di, BQ27000_REG_ILMD, true);
@@ -218,7 +260,7 @@ static int bq27x00_battery_read_ilmd(struct bq27x00_device_info *di)
 		return ilmd;
 	}
 
-	if (di->chip == BQ27500)
+	if (bq27xxx_is_chip_version_higher(di))
 		ilmd *= 1000;
 	else
 		ilmd = ilmd * 256 * 3570 / BQ27000_RS;
@@ -262,7 +304,7 @@ static int bq27x00_battery_read_temperature(struct bq27x00_device_info *di)
 		return temp;
 	}
 
-	if (di->chip == BQ27500)
+	if (bq27xxx_is_chip_version_higher(di))
 		temp -= 2731;
 	else
 		temp = ((temp * 5) - 5463) / 2;
@@ -306,14 +348,70 @@ static int bq27x00_battery_read_time(struct bq27x00_device_info *di, u8 reg)
 	return tval * 60;
 }
 
+/*
+ * Read a power avg register.
+ * Return < 0 if something fails.
+ */
+static int bq27x00_battery_read_pwr_avg(struct bq27x00_device_info *di, u8 reg)
+{
+	int tval;
+
+	tval = bq27x00_read(di, reg, false);
+	if (tval < 0) {
+		dev_err(di->dev, "error reading power avg rgister  %02x: %d\n",
+			reg, tval);
+		return tval;
+	}
+
+	if (di->chip == BQ27500)
+		return tval;
+	else
+		return (tval * BQ27x00_POWER_CONSTANT) / BQ27000_RS;
+}
+
+/*
+ * Read flag register.
+ * Return < 0 if something fails.
+ */
+static int bq27x00_battery_read_health(struct bq27x00_device_info *di)
+{
+	int tval;
+
+	tval = bq27x00_read(di, BQ27x00_REG_FLAGS, false);
+	if (tval < 0) {
+		dev_err(di->dev, "error reading flag register:%d\n", tval);
+		return tval;
+	}
+
+	if ((di->chip == BQ27500)) {
+		if (tval & BQ27500_FLAG_SOCF)
+			tval = POWER_SUPPLY_HEALTH_DEAD;
+		else if (tval & BQ27500_FLAG_OTC)
+			tval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else
+			tval = POWER_SUPPLY_HEALTH_GOOD;
+		return tval;
+	} else {
+		if (tval & BQ27000_FLAG_EDV1)
+			tval = POWER_SUPPLY_HEALTH_DEAD;
+		else
+			tval = POWER_SUPPLY_HEALTH_GOOD;
+		return tval;
+	}
+
+	return -1;
+}
+
 static void bq27x00_update(struct bq27x00_device_info *di)
 {
 	struct bq27x00_reg_cache cache = {0, };
 	bool is_bq27500 = di->chip == BQ27500;
+	bool is_bq27425 = di->chip == BQ27425;
 
 	cache.flags = bq27x00_read(di, BQ27x00_REG_FLAGS, !is_bq27500);
 	if (cache.flags >= 0) {
-		if (!is_bq27500 && (cache.flags & BQ27000_FLAG_CI)) {
+		if (!is_bq27500 && !is_bq27425
+				&& (cache.flags & BQ27000_FLAG_CI)) {
 			dev_info(di->dev, "battery is not calibrated! ignoring capacity values\n");
 			cache.capacity = -ENODATA;
 			cache.energy = -ENODATA;
@@ -321,16 +419,30 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 			cache.time_to_empty_avg = -ENODATA;
 			cache.time_to_full = -ENODATA;
 			cache.charge_full = -ENODATA;
+			cache.health = -ENODATA;
 		} else {
 			cache.capacity = bq27x00_battery_read_rsoc(di);
-			cache.energy = bq27x00_battery_read_energy(di);
-			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);
+			if (!is_bq27425) {
+				cache.energy = bq27x00_battery_read_energy(di);
+				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.health = bq27x00_battery_read_health(di);
 		}
 		cache.temperature = bq27x00_battery_read_temperature(di);
+		if (!is_bq27425)
+			cache.cycle_count = bq27x00_battery_read_cyct(di);
 		cache.cycle_count = bq27x00_battery_read_cyct(di);
+		cache.power_avg =
+			bq27x00_battery_read_pwr_avg(di, BQ27x00_POWER_AVG);
 
 		/* We only have to read charge design full once */
 		if (di->charge_design_full <= 0)
@@ -376,7 +488,7 @@ static int bq27x00_battery_current(struct bq27x00_device_info *di,
 		return curr;
 	}
 
-	if (di->chip == BQ27500) {
+	if (bq27xxx_is_chip_version_higher(di)) {
 		/* bq27500 returns signed value */
 		val->intval = (int)((s16)curr) * 1000;
 	} else {
@@ -397,7 +509,7 @@ static int bq27x00_battery_status(struct bq27x00_device_info *di,
 {
 	int status;
 
-	if (di->chip == BQ27500) {
+	if (bq27xxx_is_chip_version_higher(di)) {
 		if (di->cache.flags & BQ27500_FLAG_FC)
 			status = POWER_SUPPLY_STATUS_FULL;
 		else if (di->cache.flags & BQ27500_FLAG_DSC)
@@ -425,7 +537,7 @@ static int bq27x00_battery_capacity_level(struct bq27x00_device_info *di,
 {
 	int level;
 
-	if (di->chip == BQ27500) {
+	if (bq27xxx_is_chip_version_higher(di)) {
 		if (di->cache.flags & BQ27500_FLAG_FC)
 			level = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
 		else if (di->cache.flags & BQ27500_FLAG_SOC1)
@@ -550,6 +662,12 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_ENERGY_NOW:
 		ret = bq27x00_simple_value(di->cache.energy, val);
 		break;
+	case POWER_SUPPLY_PROP_POWER_AVG:
+		ret = bq27x00_simple_value(di->cache.power_avg, val);
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		ret = bq27x00_simple_value(di->cache.health, val);
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -570,8 +688,14 @@ 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->chip = BQ27425;
+	if (di->chip == BQ27425) {
+		di->bat.properties = bq27425_battery_props;
+		di->bat.num_properties = ARRAY_SIZE(bq27425_battery_props);
+	} else {
+		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 = bq27x00_external_power_changed;
 
@@ -729,6 +853,7 @@ static int bq27x00_battery_remove(struct i2c_client *client)
 static const struct i2c_device_id bq27x00_id[] = {
 	{ "bq27200", BQ27000 },	/* bq27200 is same as bq27000, but with i2c */
 	{ "bq27500", BQ27500 },
+	{ "bq27425", BQ27425 },
 	{},
 };
 MODULE_DEVICE_TABLE(i2c, bq27x00_id);
diff --git a/drivers/power/charger-manager.c b/drivers/power/charger-manager.c
index 86935ec1895..526e5c93129 100644
--- a/drivers/power/charger-manager.c
+++ b/drivers/power/charger-manager.c
@@ -271,16 +271,13 @@ static int try_charger_enable(struct charger_manager *cm, bool enable)
 	if (enable) {
 		if (cm->emergency_stop)
 			return -EAGAIN;
-		err = regulator_bulk_enable(desc->num_charger_regulators,
-					desc->charger_regulators);
+		for (i = 0 ; i < desc->num_charger_regulators ; i++)
+			regulator_enable(desc->charger_regulators[i].consumer);
 	} else {
 		/*
 		 * Abnormal battery state - Stop charging forcibly,
 		 * even if charger was enabled at the other places
 		 */
-		err = regulator_bulk_disable(desc->num_charger_regulators,
-					desc->charger_regulators);
-
 		for (i = 0; i < desc->num_charger_regulators; i++) {
 			if (regulator_is_enabled(
 				    desc->charger_regulators[i].consumer)) {
@@ -288,7 +285,7 @@ static int try_charger_enable(struct charger_manager *cm, bool enable)
 					desc->charger_regulators[i].consumer);
 				dev_warn(cm->dev,
 					"Disable regulator(%s) forcibly.\n",
-					desc->charger_regulators[i].supply);
+					desc->charger_regulators[i].regulator_name);
 			}
 		}
 	}
@@ -994,11 +991,92 @@ int setup_charger_manager(struct charger_global_desc *gd)
 }
 EXPORT_SYMBOL_GPL(setup_charger_manager);
 
+/**
+ * charger_extcon_work - enable/diable charger according to the state
+ *			of charger cable
+ *
+ * @work: work_struct of the function charger_extcon_work.
+ */
+static void charger_extcon_work(struct work_struct *work)
+{
+	struct charger_cable *cable =
+			container_of(work, struct charger_cable, wq);
+	int ret;
+
+	if (cable->attached && cable->min_uA != 0 && cable->max_uA != 0) {
+		ret = regulator_set_current_limit(cable->charger->consumer,
+					cable->min_uA, cable->max_uA);
+		if (ret < 0) {
+			pr_err("Cannot set current limit of %s (%s)\n",
+				cable->charger->regulator_name, cable->name);
+			return;
+		}
+
+		pr_info("Set current limit of %s : %duA ~ %duA\n",
+					cable->charger->regulator_name,
+					cable->min_uA, cable->max_uA);
+	}
+
+	try_charger_enable(cable->cm, cable->attached);
+}
+
+/**
+ * charger_extcon_notifier - receive the state of charger cable
+ *			when registered cable is attached or detached.
+ *
+ * @self: the notifier block of the charger_extcon_notifier.
+ * @event: the cable state.
+ * @ptr: the data pointer of notifier block.
+ */
+static int charger_extcon_notifier(struct notifier_block *self,
+			unsigned long event, void *ptr)
+{
+	struct charger_cable *cable =
+		container_of(self, struct charger_cable, nb);
+
+	cable->attached = event;
+	schedule_work(&cable->wq);
+
+	return NOTIFY_DONE;
+}
+
+/**
+ * charger_extcon_init - register external connector to use it
+ *			as the charger cable
+ *
+ * @cm: the Charger Manager representing the battery.
+ * @cable: the Charger cable representing the external connector.
+ */
+static int charger_extcon_init(struct charger_manager *cm,
+		struct charger_cable *cable)
+{
+	int ret = 0;
+
+	/*
+	 * Charger manager use Extcon framework to identify
+	 * the charger cable among various external connector
+	 * cable (e.g., TA, USB, MHL, Dock).
+	 */
+	INIT_WORK(&cable->wq, charger_extcon_work);
+	cable->nb.notifier_call = charger_extcon_notifier;
+	ret = extcon_register_interest(&cable->extcon_dev,
+			cable->extcon_name, cable->name, &cable->nb);
+	if (ret < 0) {
+		pr_info("Cannot register extcon_dev for %s(cable: %s).\n",
+				cable->extcon_name,
+				cable->name);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
 static int charger_manager_probe(struct platform_device *pdev)
 {
 	struct charger_desc *desc = dev_get_platdata(&pdev->dev);
 	struct charger_manager *cm;
 	int ret = 0, i = 0;
+	int j = 0;
 	union power_supply_propval val;
 
 	if (g_desc && !rtc_dev && g_desc->rtc_name) {
@@ -1167,11 +1245,31 @@ static int charger_manager_probe(struct platform_device *pdev)
 		goto err_register;
 	}
 
-	ret = regulator_bulk_get(&pdev->dev, desc->num_charger_regulators,
-				 desc->charger_regulators);
-	if (ret) {
-		dev_err(&pdev->dev, "Cannot get charger regulators.\n");
-		goto err_bulk_get;
+	for (i = 0 ; i < desc->num_charger_regulators ; i++) {
+		struct charger_regulator *charger
+					= &desc->charger_regulators[i];
+
+		charger->consumer = regulator_get(&pdev->dev,
+					charger->regulator_name);
+		if (charger->consumer == NULL) {
+			dev_err(&pdev->dev, "Cannot find charger(%s)n",
+						charger->regulator_name);
+			ret = -EINVAL;
+			goto err_chg_get;
+		}
+
+		for (j = 0 ; j < charger->num_cables ; j++) {
+			struct charger_cable *cable = &charger->cables[j];
+
+			ret = charger_extcon_init(cm, cable);
+			if (ret < 0) {
+				dev_err(&pdev->dev, "Cannot find charger(%s)n",
+						charger->regulator_name);
+				goto err_extcon;
+			}
+			cable->charger = charger;
+			cable->cm = cm;
+		}
 	}
 
 	ret = try_charger_enable(cm, true);
@@ -1197,9 +1295,19 @@ static int charger_manager_probe(struct platform_device *pdev)
 	return 0;
 
 err_chg_enable:
-	regulator_bulk_free(desc->num_charger_regulators,
-			    desc->charger_regulators);
-err_bulk_get:
+err_extcon:
+	for (i = 0 ; i < desc->num_charger_regulators ; i++) {
+		struct charger_regulator *charger
+				= &desc->charger_regulators[i];
+		for (j = 0 ; j < charger->num_cables ; j++) {
+			struct charger_cable *cable = &charger->cables[j];
+			extcon_unregister_interest(&cable->extcon_dev);
+		}
+	}
+err_chg_get:
+	for (i = 0 ; i < desc->num_charger_regulators ; i++)
+		regulator_put(desc->charger_regulators[i].consumer);
+
 	power_supply_unregister(&cm->charger_psy);
 err_register:
 	kfree(cm->charger_psy.properties);
@@ -1218,6 +1326,8 @@ static int __devexit charger_manager_remove(struct platform_device *pdev)
 {
 	struct charger_manager *cm = platform_get_drvdata(pdev);
 	struct charger_desc *desc = cm->desc;
+	int i = 0;
+	int j = 0;
 
 	/* Remove from the list */
 	mutex_lock(&cm_list_mtx);
@@ -1229,8 +1339,18 @@ static int __devexit charger_manager_remove(struct platform_device *pdev)
 	if (delayed_work_pending(&cm_monitor_work))
 		cancel_delayed_work_sync(&cm_monitor_work);
 
-	regulator_bulk_free(desc->num_charger_regulators,
-			    desc->charger_regulators);
+	for (i = 0 ; i < desc->num_charger_regulators ; i++) {
+		struct charger_regulator *charger
+				= &desc->charger_regulators[i];
+		for (j = 0 ; j < charger->num_cables ; j++) {
+			struct charger_cable *cable = &charger->cables[j];
+			extcon_unregister_interest(&cable->extcon_dev);
+		}
+	}
+
+	for (i = 0 ; i < desc->num_charger_regulators ; i++)
+		regulator_put(desc->charger_regulators[i].consumer);
+
 	power_supply_unregister(&cm->charger_psy);
 
 	try_charger_enable(cm, false);
diff --git a/drivers/power/ds2780_battery.c b/drivers/power/ds2780_battery.c
index de31cae1ba5..74fad941c56 100644
--- a/drivers/power/ds2780_battery.c
+++ b/drivers/power/ds2780_battery.c
@@ -39,7 +39,6 @@ struct ds2780_device_info {
 	struct device *dev;
 	struct power_supply bat;
 	struct device *w1_dev;
-	struct task_struct *mutex_holder;
 };
 
 enum current_types {
@@ -64,10 +63,7 @@ static inline struct power_supply *to_power_supply(struct device *dev)
 static inline int ds2780_battery_io(struct ds2780_device_info *dev_info,
 	char *buf, int addr, size_t count, int io)
 {
-	if (dev_info->mutex_holder == current)
-		return w1_ds2780_io_nolock(dev_info->w1_dev, buf, addr, count, io);
-	else
-		return w1_ds2780_io(dev_info->w1_dev, buf, addr, count, io);
+	return w1_ds2780_io(dev_info->w1_dev, buf, addr, count, io);
 }
 
 static inline int ds2780_read8(struct ds2780_device_info *dev_info, u8 *val,
@@ -779,7 +775,6 @@ static int __devinit ds2780_battery_probe(struct platform_device *pdev)
 	dev_info->bat.properties	= ds2780_battery_props;
 	dev_info->bat.num_properties	= ARRAY_SIZE(ds2780_battery_props);
 	dev_info->bat.get_property	= ds2780_battery_get_property;
-	dev_info->mutex_holder		= current;
 
 	ret = power_supply_register(&pdev->dev, &dev_info->bat);
 	if (ret) {
@@ -809,8 +804,6 @@ static int __devinit ds2780_battery_probe(struct platform_device *pdev)
 		goto fail_remove_bin_file;
 	}
 
-	dev_info->mutex_holder = NULL;
-
 	return 0;
 
 fail_remove_bin_file:
@@ -830,8 +823,6 @@ static int __devexit ds2780_battery_remove(struct platform_device *pdev)
 {
 	struct ds2780_device_info *dev_info = platform_get_drvdata(pdev);
 
-	dev_info->mutex_holder = current;
-
 	/* remove attributes */
 	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
 
diff --git a/drivers/power/ds2781_battery.c b/drivers/power/ds2781_battery.c
index 975684a40f1..7a1ff4e4cf9 100644
--- a/drivers/power/ds2781_battery.c
+++ b/drivers/power/ds2781_battery.c
@@ -37,7 +37,6 @@ struct ds2781_device_info {
 	struct device *dev;
 	struct power_supply bat;
 	struct device *w1_dev;
-	struct task_struct *mutex_holder;
 };
 
 enum current_types {
@@ -62,14 +61,10 @@ static inline struct power_supply *to_power_supply(struct device *dev)
 static inline int ds2781_battery_io(struct ds2781_device_info *dev_info,
 	char *buf, int addr, size_t count, int io)
 {
-	if (dev_info->mutex_holder == current)
-		return w1_ds2781_io_nolock(dev_info->w1_dev, buf, addr,
-				count, io);
-	else
-		return w1_ds2781_io(dev_info->w1_dev, buf, addr, count, io);
+	return w1_ds2781_io(dev_info->w1_dev, buf, addr, count, io);
 }
 
-int w1_ds2781_read(struct ds2781_device_info *dev_info, char *buf,
+static int w1_ds2781_read(struct ds2781_device_info *dev_info, char *buf,
 		int addr, size_t count)
 {
 	return ds2781_battery_io(dev_info, buf, addr, count, 0);
@@ -775,7 +770,6 @@ static int __devinit ds2781_battery_probe(struct platform_device *pdev)
 	dev_info->bat.properties	= ds2781_battery_props;
 	dev_info->bat.num_properties	= ARRAY_SIZE(ds2781_battery_props);
 	dev_info->bat.get_property	= ds2781_battery_get_property;
-	dev_info->mutex_holder		= current;
 
 	ret = power_supply_register(&pdev->dev, &dev_info->bat);
 	if (ret) {
@@ -805,8 +799,6 @@ static int __devinit ds2781_battery_probe(struct platform_device *pdev)
 		goto fail_remove_bin_file;
 	}
 
-	dev_info->mutex_holder = NULL;
-
 	return 0;
 
 fail_remove_bin_file:
@@ -826,8 +818,6 @@ static int __devexit ds2781_battery_remove(struct platform_device *pdev)
 {
 	struct ds2781_device_info *dev_info = platform_get_drvdata(pdev);
 
-	dev_info->mutex_holder = current;
-
 	/* remove attributes */
 	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2781_attr_group);
 
diff --git a/drivers/power/gpio-charger.c b/drivers/power/gpio-charger.c
index 8672c9177dd..cb2aa319568 100644
--- a/drivers/power/gpio-charger.c
+++ b/drivers/power/gpio-charger.c
@@ -54,7 +54,7 @@ static int gpio_charger_get_property(struct power_supply *psy,
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
-		val->intval = gpio_get_value(pdata->gpio);
+		val->intval = gpio_get_value_cansleep(pdata->gpio);
 		val->intval ^= pdata->gpio_active_low;
 		break;
 	default:
diff --git a/drivers/power/isp1704_charger.c b/drivers/power/isp1704_charger.c
index e5ccd297977..122911978da 100644
--- a/drivers/power/isp1704_charger.c
+++ b/drivers/power/isp1704_charger.c
@@ -415,8 +415,8 @@ static int __devinit isp1704_charger_probe(struct platform_device *pdev)
 	if (!isp)
 		return -ENOMEM;
 
-	isp->phy = usb_get_transceiver();
-	if (!isp->phy)
+	isp->phy = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(isp->phy))
 		goto fail0;
 
 	isp->dev = &pdev->dev;
@@ -475,7 +475,7 @@ fail2:
 	power_supply_unregister(&isp->psy);
 fail1:
 	isp1704_charger_set_power(isp, 0);
-	usb_put_transceiver(isp->phy);
+	usb_put_phy(isp->phy);
 fail0:
 	kfree(isp);
 
@@ -490,7 +490,7 @@ static int __devexit isp1704_charger_remove(struct platform_device *pdev)
 
 	usb_unregister_notifier(isp->phy, &isp->nb);
 	power_supply_unregister(&isp->psy);
-	usb_put_transceiver(isp->phy);
+	usb_put_phy(isp->phy);
 	isp1704_charger_set_power(isp, 0);
 	kfree(isp);
 
diff --git a/drivers/power/lp8727_charger.c b/drivers/power/lp8727_charger.c
index d8b75780bfe..6a364f4798f 100644
--- a/drivers/power/lp8727_charger.c
+++ b/drivers/power/lp8727_charger.c
@@ -15,7 +15,7 @@
 #include <linux/interrupt.h>
 #include <linux/i2c.h>
 #include <linux/power_supply.h>
-#include <linux/lp8727.h>
+#include <linux/platform_data/lp8727.h>
 
 #define DEBOUNCE_MSEC	270
 
diff --git a/drivers/power/max17042_battery.c b/drivers/power/max17042_battery.c
index 140788b309f..74abc6c755b 100644
--- a/drivers/power/max17042_battery.c
+++ b/drivers/power/max17042_battery.c
@@ -113,6 +113,7 @@ static enum power_supply_property max17042_battery_props[] = {
 	POWER_SUPPLY_PROP_VOLTAGE_OCV,
 	POWER_SUPPLY_PROP_CAPACITY,
 	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_COUNTER,
 	POWER_SUPPLY_PROP_TEMP,
 	POWER_SUPPLY_PROP_CURRENT_NOW,
 	POWER_SUPPLY_PROP_CURRENT_AVG,
@@ -201,6 +202,13 @@ static int max17042_get_property(struct power_supply *psy,
 
 		val->intval = ret * 1000 / 2;
 		break;
+	case POWER_SUPPLY_PROP_CHARGE_COUNTER:
+		ret = max17042_read_reg(chip->client, MAX17042_QH);
+		if (ret < 0)
+			return ret;
+
+		val->intval = ret * 1000 / 2;
+		break;
 	case POWER_SUPPLY_PROP_TEMP:
 		ret = max17042_read_reg(chip->client, MAX17042_TEMP);
 		if (ret < 0)
diff --git a/drivers/power/olpc_battery.c b/drivers/power/olpc_battery.c
index 7385092f9bc..a89a41acf9c 100644
--- a/drivers/power/olpc_battery.c
+++ b/drivers/power/olpc_battery.c
@@ -17,6 +17,7 @@
 #include <linux/power_supply.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
+#include <linux/olpc-ec.h>
 #include <asm/olpc.h>
 
 
@@ -231,11 +232,9 @@ static int olpc_bat_get_charge_full_design(union power_supply_propval *val)
 
 	case POWER_SUPPLY_TECHNOLOGY_LiFe:
 		switch (mfr) {
-		case 1: /* Gold Peak */
-			val->intval = 2800000;
-			break;
+		case 1: /* Gold Peak, fall through */
 		case 2: /* BYD */
-			val->intval = 3100000;
+			val->intval = 2800000;
 			break;
 		default:
 			return -EIO;
@@ -267,6 +266,55 @@ static int olpc_bat_get_charge_now(union power_supply_propval *val)
 	return 0;
 }
 
+static int olpc_bat_get_voltage_max_design(union power_supply_propval *val)
+{
+	uint8_t ec_byte;
+	union power_supply_propval tech;
+	int mfr;
+	int ret;
+
+	ret = olpc_bat_get_tech(&tech);
+	if (ret)
+		return ret;
+
+	ec_byte = BAT_ADDR_MFR_TYPE;
+	ret = olpc_ec_cmd(EC_BAT_EEPROM, &ec_byte, 1, &ec_byte, 1);
+	if (ret)
+		return ret;
+
+	mfr = ec_byte >> 4;
+
+	switch (tech.intval) {
+	case POWER_SUPPLY_TECHNOLOGY_NiMH:
+		switch (mfr) {
+		case 1: /* Gold Peak */
+			val->intval = 6000000;
+			break;
+		default:
+			return -EIO;
+		}
+		break;
+
+	case POWER_SUPPLY_TECHNOLOGY_LiFe:
+		switch (mfr) {
+		case 1: /* Gold Peak */
+			val->intval = 6400000;
+			break;
+		case 2: /* BYD */
+			val->intval = 6500000;
+			break;
+		default:
+			return -EIO;
+		}
+		break;
+
+	default:
+		return -EIO;
+	}
+
+	return ret;
+}
+
 /*********************************************************************
  *		Battery properties
  *********************************************************************/
@@ -401,6 +449,11 @@ static int olpc_bat_get_property(struct power_supply *psy,
 		sprintf(bat_serial, "%016llx", (long long)be64_to_cpu(ser_buf));
 		val->strval = bat_serial;
 		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		ret = olpc_bat_get_voltage_max_design(val);
+		if (ret)
+			return ret;
+		break;
 	default:
 		ret = -EINVAL;
 		break;
@@ -428,6 +481,7 @@ static enum power_supply_property olpc_xo1_bat_props[] = {
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
 	POWER_SUPPLY_PROP_CHARGE_COUNTER,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
 };
 
 /* XO-1.5 does not have ambient temperature property */
@@ -449,6 +503,7 @@ static enum power_supply_property olpc_xo15_bat_props[] = {
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
 	POWER_SUPPLY_PROP_CHARGE_COUNTER,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
 };
 
 /* EEPROM reading goes completely around the power_supply API, sadly */
diff --git a/drivers/power/pda_power.c b/drivers/power/pda_power.c
index 214468f4444..7312f265164 100644
--- a/drivers/power/pda_power.c
+++ b/drivers/power/pda_power.c
@@ -24,11 +24,7 @@
 
 static inline unsigned int get_irq_flags(struct resource *res)
 {
-	unsigned int flags = IRQF_SAMPLE_RANDOM | IRQF_SHARED;
-
-	flags |= res->flags & IRQF_TRIGGER_MASK;
-
-	return flags;
+	return IRQF_SHARED | (res->flags & IRQF_TRIGGER_MASK);
 }
 
 static struct device *dev;
@@ -134,13 +130,13 @@ static void update_charger(void)
 			regulator_set_current_limit(ac_draw, max_uA, max_uA);
 			if (!regulator_enabled) {
 				dev_dbg(dev, "charger on (AC)\n");
-				regulator_enable(ac_draw);
+				WARN_ON(regulator_enable(ac_draw));
 				regulator_enabled = 1;
 			}
 		} else {
 			if (regulator_enabled) {
 				dev_dbg(dev, "charger off\n");
-				regulator_disable(ac_draw);
+				WARN_ON(regulator_disable(ac_draw));
 				regulator_enabled = 0;
 			}
 		}
@@ -321,12 +317,12 @@ static int pda_power_probe(struct platform_device *pdev)
 	}
 
 #ifdef CONFIG_USB_OTG_UTILS
-	transceiver = usb_get_transceiver();
-	if (transceiver && !pdata->is_usb_online) {
-		pdata->is_usb_online = otg_is_usb_online;
-	}
-	if (transceiver && !pdata->is_ac_online) {
-		pdata->is_ac_online = otg_is_ac_online;
+	transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (!IS_ERR_OR_NULL(transceiver)) {
+		if (!pdata->is_usb_online)
+			pdata->is_usb_online = otg_is_usb_online;
+		if (!pdata->is_ac_online)
+			pdata->is_ac_online = otg_is_ac_online;
 	}
 #endif
 
@@ -373,7 +369,7 @@ static int pda_power_probe(struct platform_device *pdev)
 	}
 
 #ifdef CONFIG_USB_OTG_UTILS
-	if (transceiver && pdata->use_otg_notifier) {
+	if (!IS_ERR_OR_NULL(transceiver) && pdata->use_otg_notifier) {
 		otg_nb.notifier_call = otg_handle_notification;
 		ret = usb_register_notifier(transceiver, &otg_nb);
 		if (ret) {
@@ -408,8 +404,8 @@ usb_supply_failed:
 	if (pdata->is_ac_online && ac_irq)
 		free_irq(ac_irq->start, &pda_psy_ac);
 #ifdef CONFIG_USB_OTG_UTILS
-	if (transceiver)
-		usb_put_transceiver(transceiver);
+	if (!IS_ERR_OR_NULL(transceiver))
+		usb_put_phy(transceiver);
 #endif
 ac_irq_failed:
 	if (pdata->is_ac_online)
@@ -443,8 +439,8 @@ static int pda_power_remove(struct platform_device *pdev)
 	if (pdata->is_ac_online)
 		power_supply_unregister(&pda_psy_ac);
 #ifdef CONFIG_USB_OTG_UTILS
-	if (transceiver)
-		usb_put_transceiver(transceiver);
+	if (!IS_ERR_OR_NULL(transceiver))
+		usb_put_phy(transceiver);
 #endif
 	if (ac_draw) {
 		regulator_put(ac_draw);
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 6ad61272678..08cc8a3c15a 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -17,6 +17,7 @@
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/power_supply.h>
+#include <linux/thermal.h>
 #include "power_supply.h"
 
 /* exported for the APM Power driver, APM emulation */
@@ -169,6 +170,63 @@ static void power_supply_dev_release(struct device *dev)
 	kfree(dev);
 }
 
+#ifdef CONFIG_THERMAL
+static int power_supply_read_temp(struct thermal_zone_device *tzd,
+		unsigned long *temp)
+{
+	struct power_supply *psy;
+	union power_supply_propval val;
+	int ret;
+
+	WARN_ON(tzd == NULL);
+	psy = tzd->devdata;
+	ret = psy->get_property(psy, POWER_SUPPLY_PROP_TEMP, &val);
+
+	/* Convert tenths of degree Celsius to milli degree Celsius. */
+	if (!ret)
+		*temp = val.intval * 100;
+
+	return ret;
+}
+
+static struct thermal_zone_device_ops psy_tzd_ops = {
+	.get_temp = power_supply_read_temp,
+};
+
+static int psy_register_thermal(struct power_supply *psy)
+{
+	int i;
+
+	/* Register battery zone device psy reports temperature */
+	for (i = 0; i < psy->num_properties; i++) {
+		if (psy->properties[i] == POWER_SUPPLY_PROP_TEMP) {
+			psy->tzd = thermal_zone_device_register(psy->name, 0, 0,
+					psy, &psy_tzd_ops, 0, 0, 0, 0);
+			if (IS_ERR(psy->tzd))
+				return PTR_ERR(psy->tzd);
+			break;
+		}
+	}
+	return 0;
+}
+
+static void psy_unregister_thermal(struct power_supply *psy)
+{
+	if (IS_ERR_OR_NULL(psy->tzd))
+		return;
+	thermal_zone_device_unregister(psy->tzd);
+}
+#else
+static int psy_register_thermal(struct power_supply *psy)
+{
+	return 0;
+}
+
+static void psy_unregister_thermal(struct power_supply *psy)
+{
+}
+#endif
+
 int power_supply_register(struct device *parent, struct power_supply *psy)
 {
 	struct device *dev;
@@ -197,6 +255,10 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	if (rc)
 		goto device_add_failed;
 
+	rc = psy_register_thermal(psy);
+	if (rc)
+		goto register_thermal_failed;
+
 	rc = power_supply_create_triggers(psy);
 	if (rc)
 		goto create_triggers_failed;
@@ -206,6 +268,8 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	goto success;
 
 create_triggers_failed:
+	psy_unregister_thermal(psy);
+register_thermal_failed:
 	device_del(dev);
 kobject_set_name_failed:
 device_add_failed:
@@ -220,6 +284,7 @@ void power_supply_unregister(struct power_supply *psy)
 	cancel_work_sync(&psy->changed_work);
 	sysfs_remove_link(&psy->dev->kobj, "powers");
 	power_supply_remove_triggers(psy);
+	psy_unregister_thermal(psy);
 	device_unregister(psy->dev);
 }
 EXPORT_SYMBOL_GPL(power_supply_unregister);
diff --git a/drivers/power/power_supply_sysfs.c b/drivers/power/power_supply_sysfs.c
index 4150747f918..1d96614a17a 100644
--- a/drivers/power/power_supply_sysfs.c
+++ b/drivers/power/power_supply_sysfs.c
@@ -159,6 +159,8 @@ static struct device_attribute power_supply_attrs[] = {
 	POWER_SUPPLY_ATTR(charge_now),
 	POWER_SUPPLY_ATTR(charge_avg),
 	POWER_SUPPLY_ATTR(charge_counter),
+	POWER_SUPPLY_ATTR(constant_charge_current),
+	POWER_SUPPLY_ATTR(constant_charge_voltage),
 	POWER_SUPPLY_ATTR(energy_full_design),
 	POWER_SUPPLY_ATTR(energy_empty_design),
 	POWER_SUPPLY_ATTR(energy_full),
@@ -166,9 +168,15 @@ static struct device_attribute power_supply_attrs[] = {
 	POWER_SUPPLY_ATTR(energy_now),
 	POWER_SUPPLY_ATTR(energy_avg),
 	POWER_SUPPLY_ATTR(capacity),
+	POWER_SUPPLY_ATTR(capacity_alert_min),
+	POWER_SUPPLY_ATTR(capacity_alert_max),
 	POWER_SUPPLY_ATTR(capacity_level),
 	POWER_SUPPLY_ATTR(temp),
+	POWER_SUPPLY_ATTR(temp_alert_min),
+	POWER_SUPPLY_ATTR(temp_alert_max),
 	POWER_SUPPLY_ATTR(temp_ambient),
+	POWER_SUPPLY_ATTR(temp_ambient_alert_min),
+	POWER_SUPPLY_ATTR(temp_ambient_alert_max),
 	POWER_SUPPLY_ATTR(time_to_empty_now),
 	POWER_SUPPLY_ATTR(time_to_empty_avg),
 	POWER_SUPPLY_ATTR(time_to_full_now),
diff --git a/drivers/power/sbs-battery.c b/drivers/power/sbs-battery.c
index a5b6849d412..a65e8f54157 100644
--- a/drivers/power/sbs-battery.c
+++ b/drivers/power/sbs-battery.c
@@ -469,7 +469,7 @@ static int sbs_get_property(struct power_supply *psy,
 
 	case POWER_SUPPLY_PROP_TECHNOLOGY:
 		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
-		break;
+		goto done; /* don't trigger power_supply_changed()! */
 
 	case POWER_SUPPLY_PROP_ENERGY_NOW:
 	case POWER_SUPPLY_PROP_ENERGY_FULL:
diff --git a/drivers/power/smb347-charger.c b/drivers/power/smb347-charger.c
index f8eedd8a676..332dd0110bd 100644
--- a/drivers/power/smb347-charger.c
+++ b/drivers/power/smb347-charger.c
@@ -196,6 +196,14 @@ static const unsigned int ccc_tbl[] = {
 	1200000,
 };
 
+/* Convert register value to current using lookup table */
+static int hw_to_current(const unsigned int *tbl, size_t size, unsigned int val)
+{
+	if (val >= size)
+		return -EINVAL;
+	return tbl[val];
+}
+
 /* Convert current to register value using lookup table */
 static int current_to_hw(const unsigned int *tbl, size_t size, unsigned int val)
 {
@@ -841,22 +849,101 @@ fail:
 	return ret;
 }
 
+/*
+ * Returns the constant charge current programmed
+ * into the charger in uA.
+ */
+static int get_const_charge_current(struct smb347_charger *smb)
+{
+	int ret, intval;
+	unsigned int v;
+
+	if (!smb347_is_ps_online(smb))
+		return -ENODATA;
+
+	ret = regmap_read(smb->regmap, STAT_B, &v);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * The current value is composition of FCC and PCC values
+	 * and we can detect which table to use from bit 5.
+	 */
+	if (v & 0x20) {
+		intval = hw_to_current(fcc_tbl, ARRAY_SIZE(fcc_tbl), v & 7);
+	} else {
+		v >>= 3;
+		intval = hw_to_current(pcc_tbl, ARRAY_SIZE(pcc_tbl), v & 7);
+	}
+
+	return intval;
+}
+
+/*
+ * Returns the constant charge voltage programmed
+ * into the charger in uV.
+ */
+static int get_const_charge_voltage(struct smb347_charger *smb)
+{
+	int ret, intval;
+	unsigned int v;
+
+	if (!smb347_is_ps_online(smb))
+		return -ENODATA;
+
+	ret = regmap_read(smb->regmap, STAT_A, &v);
+	if (ret < 0)
+		return ret;
+
+	v &= STAT_A_FLOAT_VOLTAGE_MASK;
+	if (v > 0x3d)
+		v = 0x3d;
+
+	intval = 3500000 + v * 20000;
+
+	return intval;
+}
+
 static int smb347_mains_get_property(struct power_supply *psy,
 				     enum power_supply_property prop,
 				     union power_supply_propval *val)
 {
 	struct smb347_charger *smb =
 		container_of(psy, struct smb347_charger, mains);
+	int ret;
 
-	if (prop == POWER_SUPPLY_PROP_ONLINE) {
+	switch (prop) {
+	case POWER_SUPPLY_PROP_ONLINE:
 		val->intval = smb->mains_online;
-		return 0;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		ret = get_const_charge_voltage(smb);
+		if (ret < 0)
+			return ret;
+		else
+			val->intval = ret;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		ret = get_const_charge_current(smb);
+		if (ret < 0)
+			return ret;
+		else
+			val->intval = ret;
+		break;
+
+	default:
+		return -EINVAL;
 	}
-	return -EINVAL;
+
+	return 0;
 }
 
 static enum power_supply_property smb347_mains_properties[] = {
 	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE,
 };
 
 static int smb347_usb_get_property(struct power_supply *psy,
@@ -865,16 +952,40 @@ static int smb347_usb_get_property(struct power_supply *psy,
 {
 	struct smb347_charger *smb =
 		container_of(psy, struct smb347_charger, usb);
+	int ret;
 
-	if (prop == POWER_SUPPLY_PROP_ONLINE) {
+	switch (prop) {
+	case POWER_SUPPLY_PROP_ONLINE:
 		val->intval = smb->usb_online;
-		return 0;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		ret = get_const_charge_voltage(smb);
+		if (ret < 0)
+			return ret;
+		else
+			val->intval = ret;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		ret = get_const_charge_current(smb);
+		if (ret < 0)
+			return ret;
+		else
+			val->intval = ret;
+		break;
+
+	default:
+		return -EINVAL;
 	}
-	return -EINVAL;
+
+	return 0;
 }
 
 static enum power_supply_property smb347_usb_properties[] = {
 	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE,
 };
 
 static int smb347_battery_get_property(struct power_supply *psy,
diff --git a/drivers/power/test_power.c b/drivers/power/test_power.c
index b527c93bf2f..b99a452a4fd 100644
--- a/drivers/power/test_power.c
+++ b/drivers/power/test_power.c
@@ -22,11 +22,13 @@
 #include <linux/vermagic.h>
 
 static int ac_online			= 1;
+static int usb_online			= 1;
 static int battery_status		= POWER_SUPPLY_STATUS_DISCHARGING;
 static int battery_health		= POWER_SUPPLY_HEALTH_GOOD;
 static int battery_present		= 1; /* true */
 static int battery_technology		= POWER_SUPPLY_TECHNOLOGY_LION;
 static int battery_capacity		= 50;
+static int battery_voltage		= 3300;
 
 static int test_power_get_ac_property(struct power_supply *psy,
 				      enum power_supply_property psp,
@@ -42,6 +44,20 @@ static int test_power_get_ac_property(struct power_supply *psy,
 	return 0;
 }
 
+static int test_power_get_usb_property(struct power_supply *psy,
+				      enum power_supply_property psp,
+				      union power_supply_propval *val)
+{
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = usb_online;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static int test_power_get_battery_property(struct power_supply *psy,
 					   enum power_supply_property psp,
 					   union power_supply_propval *val)
@@ -86,6 +102,12 @@ static int test_power_get_battery_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_TIME_TO_FULL_NOW:
 		val->intval = 3600;
 		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		val->intval = 26;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = battery_voltage;
+		break;
 	default:
 		pr_info("%s: some properties deliberately report errors.\n",
 			__func__);
@@ -114,6 +136,8 @@ static enum power_supply_property test_power_battery_props[] = {
 	POWER_SUPPLY_PROP_MODEL_NAME,
 	POWER_SUPPLY_PROP_MANUFACTURER,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
 };
 
 static char *test_power_ac_supplied_to[] = {
@@ -135,6 +159,14 @@ static struct power_supply test_power_supplies[] = {
 		.properties = test_power_battery_props,
 		.num_properties = ARRAY_SIZE(test_power_battery_props),
 		.get_property = test_power_get_battery_property,
+	}, {
+		.name = "test_usb",
+		.type = POWER_SUPPLY_TYPE_USB,
+		.supplied_to = test_power_ac_supplied_to,
+		.num_supplicants = ARRAY_SIZE(test_power_ac_supplied_to),
+		.properties = test_power_ac_props,
+		.num_properties = ARRAY_SIZE(test_power_ac_props),
+		.get_property = test_power_get_usb_property,
 	},
 };
 
@@ -167,6 +199,7 @@ static void __exit test_power_exit(void)
 
 	/* Let's see how we handle changes... */
 	ac_online = 0;
+	usb_online = 0;
 	battery_status = POWER_SUPPLY_STATUS_DISCHARGING;
 	for (i = 0; i < ARRAY_SIZE(test_power_supplies); i++)
 		power_supply_changed(&test_power_supplies[i]);
@@ -275,6 +308,19 @@ static int param_get_ac_online(char *buffer, const struct kernel_param *kp)
 	return strlen(buffer);
 }
 
+static int param_set_usb_online(const char *key, const struct kernel_param *kp)
+{
+	usb_online = map_get_value(map_ac_online, key, usb_online);
+	power_supply_changed(&test_power_supplies[2]);
+	return 0;
+}
+
+static int param_get_usb_online(char *buffer, const struct kernel_param *kp)
+{
+	strcpy(buffer, map_get_key(map_ac_online, usb_online, "unknown"));
+	return strlen(buffer);
+}
+
 static int param_set_battery_status(const char *key,
 					const struct kernel_param *kp)
 {
@@ -350,13 +396,31 @@ static int param_set_battery_capacity(const char *key,
 
 #define param_get_battery_capacity param_get_int
 
+static int param_set_battery_voltage(const char *key,
+					const struct kernel_param *kp)
+{
+	int tmp;
+
+	if (1 != sscanf(key, "%d", &tmp))
+		return -EINVAL;
+
+	battery_voltage = tmp;
+	power_supply_changed(&test_power_supplies[1]);
+	return 0;
+}
 
+#define param_get_battery_voltage param_get_int
 
 static struct kernel_param_ops param_ops_ac_online = {
 	.set = param_set_ac_online,
 	.get = param_get_ac_online,
 };
 
+static struct kernel_param_ops param_ops_usb_online = {
+	.set = param_set_usb_online,
+	.get = param_get_usb_online,
+};
+
 static struct kernel_param_ops param_ops_battery_status = {
 	.set = param_set_battery_status,
 	.get = param_get_battery_status,
@@ -382,18 +446,27 @@ static struct kernel_param_ops param_ops_battery_capacity = {
 	.get = param_get_battery_capacity,
 };
 
+static struct kernel_param_ops param_ops_battery_voltage = {
+	.set = param_set_battery_voltage,
+	.get = param_get_battery_voltage,
+};
 
 #define param_check_ac_online(name, p) __param_check(name, p, void);
+#define param_check_usb_online(name, p) __param_check(name, p, void);
 #define param_check_battery_status(name, p) __param_check(name, p, void);
 #define param_check_battery_present(name, p) __param_check(name, p, void);
 #define param_check_battery_technology(name, p) __param_check(name, p, void);
 #define param_check_battery_health(name, p) __param_check(name, p, void);
 #define param_check_battery_capacity(name, p) __param_check(name, p, void);
+#define param_check_battery_voltage(name, p) __param_check(name, p, void);
 
 
 module_param(ac_online, ac_online, 0644);
 MODULE_PARM_DESC(ac_online, "AC charging state <on|off>");
 
+module_param(usb_online, usb_online, 0644);
+MODULE_PARM_DESC(usb_online, "USB charging state <on|off>");
+
 module_param(battery_status, battery_status, 0644);
 MODULE_PARM_DESC(battery_status,
 	"battery status <charging|discharging|not-charging|full>");
@@ -413,6 +486,8 @@ MODULE_PARM_DESC(battery_health,
 module_param(battery_capacity, battery_capacity, 0644);
 MODULE_PARM_DESC(battery_capacity, "battery capacity (percentage)");
 
+module_param(battery_voltage, battery_voltage, 0644);
+MODULE_PARM_DESC(battery_voltage, "battery voltage (millivolts)");
 
 MODULE_DESCRIPTION("Power supply driver for testing");
 MODULE_AUTHOR("Anton Vorontsov <cbouatmailru@gmail.com>");
diff --git a/drivers/power/twl4030_charger.c b/drivers/power/twl4030_charger.c
index fdad850c77d..15f4d5d8611 100644
--- a/drivers/power/twl4030_charger.c
+++ b/drivers/power/twl4030_charger.c
@@ -15,12 +15,14 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
 #include <linux/i2c/twl.h>
 #include <linux/power_supply.h>
 #include <linux/notifier.h>
 #include <linux/usb/otg.h>
+#include <linux/regulator/machine.h>
 
 #define TWL4030_BCIMSTATEC	0x02
 #define TWL4030_BCIICHG		0x08
@@ -28,6 +30,7 @@
 #define TWL4030_BCIVBUS		0x0c
 #define TWL4030_BCIMFSTS4	0x10
 #define TWL4030_BCICTL1		0x23
+#define TWL4030_BB_CFG		0x12
 
 #define TWL4030_BCIAUTOWEN	BIT(5)
 #define TWL4030_CONFIG_DONE	BIT(4)
@@ -37,6 +40,17 @@
 #define TWL4030_USBFASTMCHG	BIT(2)
 #define TWL4030_STS_VBUS	BIT(7)
 #define TWL4030_STS_USB_ID	BIT(2)
+#define TWL4030_BBCHEN		BIT(4)
+#define TWL4030_BBSEL_MASK	0b1100
+#define TWL4030_BBSEL_2V5	0b0000
+#define TWL4030_BBSEL_3V0	0b0100
+#define TWL4030_BBSEL_3V1	0b1000
+#define TWL4030_BBSEL_3V2	0b1100
+#define TWL4030_BBISEL_MASK	0b11
+#define TWL4030_BBISEL_25uA	0b00
+#define TWL4030_BBISEL_150uA	0b01
+#define TWL4030_BBISEL_500uA	0b10
+#define TWL4030_BBISEL_1000uA	0b11
 
 /* BCI interrupts */
 #define TWL4030_WOVF		BIT(0) /* Watchdog overflow */
@@ -74,6 +88,8 @@ struct twl4030_bci {
 	struct work_struct	work;
 	int			irq_chg;
 	int			irq_bci;
+	struct regulator	*usb_reg;
+	int			usb_enabled;
 
 	unsigned long		event;
 };
@@ -103,7 +119,7 @@ static int twl4030_bci_read(u8 reg, u8 *val)
 
 static int twl4030_clear_set_boot_bci(u8 clear, u8 set)
 {
-	return twl4030_clear_set(TWL4030_MODULE_PM_MASTER, 0,
+	return twl4030_clear_set(TWL4030_MODULE_PM_MASTER, clear,
 			TWL4030_CONFIG_DONE | TWL4030_BCIAUTOWEN | set,
 			TWL4030_PM_MASTER_BOOT_BCI);
 }
@@ -151,14 +167,14 @@ static int twl4030_bci_have_vbus(struct twl4030_bci *bci)
 }
 
 /*
- * Enable/Disable USB Charge funtionality.
+ * Enable/Disable USB Charge functionality.
  */
 static int twl4030_charger_enable_usb(struct twl4030_bci *bci, bool enable)
 {
 	int ret;
 
 	if (enable) {
-		/* Check for USB charger conneted */
+		/* Check for USB charger connected */
 		if (!twl4030_bci_have_vbus(bci))
 			return -ENODEV;
 
@@ -171,6 +187,12 @@ static int twl4030_charger_enable_usb(struct twl4030_bci *bci, bool enable)
 			return -EACCES;
 		}
 
+		/* Need to keep regulator on */
+		if (!bci->usb_enabled) {
+			regulator_enable(bci->usb_reg);
+			bci->usb_enabled = 1;
+		}
+
 		/* forcing the field BCIAUTOUSB (BOOT_BCI[1]) to 1 */
 		ret = twl4030_clear_set_boot_bci(0, TWL4030_BCIAUTOUSB);
 		if (ret < 0)
@@ -181,6 +203,10 @@ static int twl4030_charger_enable_usb(struct twl4030_bci *bci, bool enable)
 			TWL4030_USBFASTMCHG, TWL4030_BCIMFSTS4);
 	} else {
 		ret = twl4030_clear_set_boot_bci(TWL4030_BCIAUTOUSB, 0);
+		if (bci->usb_enabled) {
+			regulator_disable(bci->usb_reg);
+			bci->usb_enabled = 0;
+		}
 	}
 
 	return ret;
@@ -202,6 +228,49 @@ static int twl4030_charger_enable_ac(bool enable)
 }
 
 /*
+ * Enable/Disable charging of Backup Battery.
+ */
+static int twl4030_charger_enable_backup(int uvolt, int uamp)
+{
+	int ret;
+	u8 flags;
+
+	if (uvolt < 2500000 ||
+	    uamp < 25) {
+		/* disable charging of backup battery */
+		ret = twl4030_clear_set(TWL4030_MODULE_PM_RECEIVER,
+					TWL4030_BBCHEN, 0, TWL4030_BB_CFG);
+		return ret;
+	}
+
+	flags = TWL4030_BBCHEN;
+	if (uvolt >= 3200000)
+		flags |= TWL4030_BBSEL_3V2;
+	else if (uvolt >= 3100000)
+		flags |= TWL4030_BBSEL_3V1;
+	else if (uvolt >= 3000000)
+		flags |= TWL4030_BBSEL_3V0;
+	else
+		flags |= TWL4030_BBSEL_2V5;
+
+	if (uamp >= 1000)
+		flags |= TWL4030_BBISEL_1000uA;
+	else if (uamp >= 500)
+		flags |= TWL4030_BBISEL_500uA;
+	else if (uamp >= 150)
+		flags |= TWL4030_BBISEL_150uA;
+	else
+		flags |= TWL4030_BBISEL_25uA;
+
+	ret = twl4030_clear_set(TWL4030_MODULE_PM_RECEIVER,
+				TWL4030_BBSEL_MASK | TWL4030_BBISEL_MASK,
+				flags,
+				TWL4030_BB_CFG);
+
+	return ret;
+}
+
+/*
  * TWL4030 CHG_PRES (AC charger presence) events
  */
 static irqreturn_t twl4030_charger_interrupt(int irq, void *arg)
@@ -424,6 +493,7 @@ static enum power_supply_property twl4030_charger_props[] = {
 static int __init twl4030_bci_probe(struct platform_device *pdev)
 {
 	struct twl4030_bci *bci;
+	struct twl4030_bci_platform_data *pdata = pdev->dev.platform_data;
 	int ret;
 	u32 reg;
 
@@ -455,6 +525,8 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 	bci->usb.num_properties = ARRAY_SIZE(twl4030_charger_props);
 	bci->usb.get_property = twl4030_bci_get_property;
 
+	bci->usb_reg = regulator_get(bci->dev, "bci3v1");
+
 	ret = power_supply_register(&pdev->dev, &bci->usb);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register usb: %d\n", ret);
@@ -479,8 +551,8 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 
 	INIT_WORK(&bci->work, twl4030_bci_usb_work);
 
-	bci->transceiver = usb_get_transceiver();
-	if (bci->transceiver != NULL) {
+	bci->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (!IS_ERR_OR_NULL(bci->transceiver)) {
 		bci->usb_nb.notifier_call = twl4030_bci_usb_ncb;
 		usb_register_notifier(bci->transceiver, &bci->usb_nb);
 	}
@@ -503,13 +575,15 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 
 	twl4030_charger_enable_ac(true);
 	twl4030_charger_enable_usb(bci, true);
+	twl4030_charger_enable_backup(pdata->bb_uvolt,
+				      pdata->bb_uamp);
 
 	return 0;
 
 fail_unmask_interrupts:
-	if (bci->transceiver != NULL) {
+	if (!IS_ERR_OR_NULL(bci->transceiver)) {
 		usb_unregister_notifier(bci->transceiver, &bci->usb_nb);
-		usb_put_transceiver(bci->transceiver);
+		usb_put_phy(bci->transceiver);
 	}
 	free_irq(bci->irq_bci, bci);
 fail_bci_irq:
@@ -531,6 +605,7 @@ static int __exit twl4030_bci_remove(struct platform_device *pdev)
 
 	twl4030_charger_enable_ac(false);
 	twl4030_charger_enable_usb(bci, false);
+	twl4030_charger_enable_backup(0, 0);
 
 	/* mask interrupts */
 	twl_i2c_write_u8(TWL4030_MODULE_INTERRUPTS, 0xff,
@@ -538,9 +613,9 @@ static int __exit twl4030_bci_remove(struct platform_device *pdev)
 	twl_i2c_write_u8(TWL4030_MODULE_INTERRUPTS, 0xff,
 			 TWL4030_INTERRUPTS_BCIIMR2A);
 
-	if (bci->transceiver != NULL) {
+	if (!IS_ERR_OR_NULL(bci->transceiver)) {
 		usb_unregister_notifier(bci->transceiver, &bci->usb_nb);
-		usb_put_transceiver(bci->transceiver);
+		usb_put_phy(bci->transceiver);
 	}
 	free_irq(bci->irq_bci, bci);
 	free_irq(bci->irq_chg, bci);
diff --git a/drivers/pps/pps.c b/drivers/pps/pps.c
index 98fbe62694d..e771487132f 100644
--- a/drivers/pps/pps.c
+++ b/drivers/pps/pps.c
@@ -327,8 +327,10 @@ int pps_register_cdev(struct pps_device *pps)
 	}
 	pps->dev = device_create(pps_class, pps->info.dev, devt, pps,
 							"pps%d", pps->id);
-	if (IS_ERR(pps->dev))
+	if (IS_ERR(pps->dev)) {
+		err = PTR_ERR(pps->dev);
 		goto del_cdev;
+	}
 
 	pps->dev->release = pps_device_destruct;
 
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
new file mode 100644
index 00000000000..8fc3808d7a3
--- /dev/null
+++ b/drivers/pwm/Kconfig
@@ -0,0 +1,108 @@
+menuconfig PWM
+	bool "PWM Support"
+	depends on !MACH_JZ4740 && !PUV3_PWM
+	help
+	  This enables PWM support through the generic PWM framework.
+	  You only need to enable this, if you also want to enable
+	  one or more of the PWM drivers below.
+
+	  If unsure, say N.
+
+if PWM
+
+config PWM_BFIN
+	tristate "Blackfin PWM support"
+	depends on BFIN_GPTIMERS
+	help
+	  Generic PWM framework driver for Blackfin.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-bfin.
+
+config PWM_IMX
+	tristate "i.MX pwm support"
+	depends on ARCH_MXC
+	help
+	  Generic PWM framework driver for i.MX.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-imx.
+
+config PWM_LPC32XX
+	tristate "LPC32XX PWM support"
+	depends on ARCH_LPC32XX
+	help
+	  Generic PWM framework driver for LPC32XX. The LPC32XX SOC has two
+	  PWM controllers.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-lpc32xx.
+
+config PWM_MXS
+	tristate "Freescale MXS PWM support"
+	depends on ARCH_MXS && OF
+	select STMP_DEVICE
+	help
+	  Generic PWM framework driver for Freescale MXS.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-mxs.
+
+config PWM_PXA
+	tristate "PXA PWM support"
+	depends on ARCH_PXA
+	help
+	  Generic PWM framework driver for PXA.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-pxa.
+
+config PWM_SAMSUNG
+	tristate "Samsung pwm support"
+	depends on PLAT_SAMSUNG
+	help
+	  Generic PWM framework driver for Samsung.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-samsung.
+
+config PWM_TEGRA
+	tristate "NVIDIA Tegra PWM support"
+	depends on ARCH_TEGRA
+	help
+	  Generic PWM framework driver for the PWFM controller found on NVIDIA
+	  Tegra SoCs.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-tegra.
+
+config  PWM_TIECAP
+	tristate "ECAP PWM support"
+	depends on SOC_AM33XX
+	help
+	  PWM driver support for the ECAP APWM controller found on AM33XX
+	  TI SOC
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-tiecap.
+
+config  PWM_TIEHRPWM
+	tristate "EHRPWM PWM support"
+	depends on SOC_AM33XX
+	help
+	  PWM driver support for the EHRPWM controller found on AM33XX
+	  TI SOC
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-tiehrpwm.
+
+config PWM_VT8500
+	tristate "vt8500 pwm support"
+	depends on ARCH_VT8500
+	help
+	  Generic PWM framework driver for vt8500.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-vt8500.
+
+endif
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
new file mode 100644
index 00000000000..e4b2c898964
--- /dev/null
+++ b/drivers/pwm/Makefile
@@ -0,0 +1,11 @@
+obj-$(CONFIG_PWM)		+= core.o
+obj-$(CONFIG_PWM_BFIN)		+= pwm-bfin.o
+obj-$(CONFIG_PWM_IMX)		+= pwm-imx.o
+obj-$(CONFIG_PWM_LPC32XX)	+= pwm-lpc32xx.o
+obj-$(CONFIG_PWM_MXS)		+= pwm-mxs.o
+obj-$(CONFIG_PWM_PXA)		+= pwm-pxa.o
+obj-$(CONFIG_PWM_SAMSUNG)	+= pwm-samsung.o
+obj-$(CONFIG_PWM_TEGRA)		+= pwm-tegra.o
+obj-$(CONFIG_PWM_TIECAP)	+= pwm-tiecap.o
+obj-$(CONFIG_PWM_TIEHRPWM)	+= pwm-tiehrpwm.o
+obj-$(CONFIG_PWM_VT8500)	+= pwm-vt8500.o
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
new file mode 100644
index 00000000000..ecb76909e94
--- /dev/null
+++ b/drivers/pwm/core.c
@@ -0,0 +1,713 @@
+/*
+ * Generic pwmlib implementation
+ *
+ * Copyright (C) 2011 Sascha Hauer <s.hauer@pengutronix.de>
+ * Copyright (C) 2011-2012 Avionic Design GmbH
+ *
+ *  This program is free software; you can redistribute 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; see the file COPYING.  If not, write to
+ *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/pwm.h>
+#include <linux/radix-tree.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#define MAX_PWMS 1024
+
+static DEFINE_MUTEX(pwm_lookup_lock);
+static LIST_HEAD(pwm_lookup_list);
+static DEFINE_MUTEX(pwm_lock);
+static LIST_HEAD(pwm_chips);
+static DECLARE_BITMAP(allocated_pwms, MAX_PWMS);
+static RADIX_TREE(pwm_tree, GFP_KERNEL);
+
+static struct pwm_device *pwm_to_device(unsigned int pwm)
+{
+	return radix_tree_lookup(&pwm_tree, pwm);
+}
+
+static int alloc_pwms(int pwm, unsigned int count)
+{
+	unsigned int from = 0;
+	unsigned int start;
+
+	if (pwm >= MAX_PWMS)
+		return -EINVAL;
+
+	if (pwm >= 0)
+		from = pwm;
+
+	start = bitmap_find_next_zero_area(allocated_pwms, MAX_PWMS, from,
+					   count, 0);
+
+	if (pwm >= 0 && start != pwm)
+		return -EEXIST;
+
+	if (start + count > MAX_PWMS)
+		return -ENOSPC;
+
+	return start;
+}
+
+static void free_pwms(struct pwm_chip *chip)
+{
+	unsigned int i;
+
+	for (i = 0; i < chip->npwm; i++) {
+		struct pwm_device *pwm = &chip->pwms[i];
+		radix_tree_delete(&pwm_tree, pwm->pwm);
+	}
+
+	bitmap_clear(allocated_pwms, chip->base, chip->npwm);
+
+	kfree(chip->pwms);
+	chip->pwms = NULL;
+}
+
+static struct pwm_chip *pwmchip_find_by_name(const char *name)
+{
+	struct pwm_chip *chip;
+
+	if (!name)
+		return NULL;
+
+	mutex_lock(&pwm_lock);
+
+	list_for_each_entry(chip, &pwm_chips, list) {
+		const char *chip_name = dev_name(chip->dev);
+
+		if (chip_name && strcmp(chip_name, name) == 0) {
+			mutex_unlock(&pwm_lock);
+			return chip;
+		}
+	}
+
+	mutex_unlock(&pwm_lock);
+
+	return NULL;
+}
+
+static int pwm_device_request(struct pwm_device *pwm, const char *label)
+{
+	int err;
+
+	if (test_bit(PWMF_REQUESTED, &pwm->flags))
+		return -EBUSY;
+
+	if (!try_module_get(pwm->chip->ops->owner))
+		return -ENODEV;
+
+	if (pwm->chip->ops->request) {
+		err = pwm->chip->ops->request(pwm->chip, pwm);
+		if (err) {
+			module_put(pwm->chip->ops->owner);
+			return err;
+		}
+	}
+
+	set_bit(PWMF_REQUESTED, &pwm->flags);
+	pwm->label = label;
+
+	return 0;
+}
+
+static struct pwm_device *of_pwm_simple_xlate(struct pwm_chip *pc,
+					      const struct of_phandle_args *args)
+{
+	struct pwm_device *pwm;
+
+	if (pc->of_pwm_n_cells < 2)
+		return ERR_PTR(-EINVAL);
+
+	if (args->args[0] >= pc->npwm)
+		return ERR_PTR(-EINVAL);
+
+	pwm = pwm_request_from_chip(pc, args->args[0], NULL);
+	if (IS_ERR(pwm))
+		return pwm;
+
+	pwm_set_period(pwm, args->args[1]);
+
+	return pwm;
+}
+
+void of_pwmchip_add(struct pwm_chip *chip)
+{
+	if (!chip->dev || !chip->dev->of_node)
+		return;
+
+	if (!chip->of_xlate) {
+		chip->of_xlate = of_pwm_simple_xlate;
+		chip->of_pwm_n_cells = 2;
+	}
+
+	of_node_get(chip->dev->of_node);
+}
+
+void of_pwmchip_remove(struct pwm_chip *chip)
+{
+	if (chip->dev && chip->dev->of_node)
+		of_node_put(chip->dev->of_node);
+}
+
+/**
+ * pwm_set_chip_data() - set private chip data for a PWM
+ * @pwm: PWM device
+ * @data: pointer to chip-specific data
+ */
+int pwm_set_chip_data(struct pwm_device *pwm, void *data)
+{
+	if (!pwm)
+		return -EINVAL;
+
+	pwm->chip_data = data;
+
+	return 0;
+}
+
+/**
+ * pwm_get_chip_data() - get private chip data for a PWM
+ * @pwm: PWM device
+ */
+void *pwm_get_chip_data(struct pwm_device *pwm)
+{
+	return pwm ? pwm->chip_data : NULL;
+}
+
+/**
+ * pwmchip_add() - register a new PWM chip
+ * @chip: the PWM chip to add
+ *
+ * Register a new PWM chip. If chip->base < 0 then a dynamically assigned base
+ * will be used.
+ */
+int pwmchip_add(struct pwm_chip *chip)
+{
+	struct pwm_device *pwm;
+	unsigned int i;
+	int ret;
+
+	if (!chip || !chip->dev || !chip->ops || !chip->ops->config ||
+	    !chip->ops->enable || !chip->ops->disable)
+		return -EINVAL;
+
+	mutex_lock(&pwm_lock);
+
+	ret = alloc_pwms(chip->base, chip->npwm);
+	if (ret < 0)
+		goto out;
+
+	chip->pwms = kzalloc(chip->npwm * sizeof(*pwm), GFP_KERNEL);
+	if (!chip->pwms) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	chip->base = ret;
+
+	for (i = 0; i < chip->npwm; i++) {
+		pwm = &chip->pwms[i];
+
+		pwm->chip = chip;
+		pwm->pwm = chip->base + i;
+		pwm->hwpwm = i;
+
+		radix_tree_insert(&pwm_tree, pwm->pwm, pwm);
+	}
+
+	bitmap_set(allocated_pwms, chip->base, chip->npwm);
+
+	INIT_LIST_HEAD(&chip->list);
+	list_add(&chip->list, &pwm_chips);
+
+	ret = 0;
+
+	if (IS_ENABLED(CONFIG_OF))
+		of_pwmchip_add(chip);
+
+out:
+	mutex_unlock(&pwm_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(pwmchip_add);
+
+/**
+ * pwmchip_remove() - remove a PWM chip
+ * @chip: the PWM chip to remove
+ *
+ * Removes a PWM chip. This function may return busy if the PWM chip provides
+ * a PWM device that is still requested.
+ */
+int pwmchip_remove(struct pwm_chip *chip)
+{
+	unsigned int i;
+	int ret = 0;
+
+	mutex_lock(&pwm_lock);
+
+	for (i = 0; i < chip->npwm; i++) {
+		struct pwm_device *pwm = &chip->pwms[i];
+
+		if (test_bit(PWMF_REQUESTED, &pwm->flags)) {
+			ret = -EBUSY;
+			goto out;
+		}
+	}
+
+	list_del_init(&chip->list);
+
+	if (IS_ENABLED(CONFIG_OF))
+		of_pwmchip_remove(chip);
+
+	free_pwms(chip);
+
+out:
+	mutex_unlock(&pwm_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(pwmchip_remove);
+
+/**
+ * pwm_request() - request a PWM device
+ * @pwm_id: global PWM device index
+ * @label: PWM device label
+ *
+ * This function is deprecated, use pwm_get() instead.
+ */
+struct pwm_device *pwm_request(int pwm, const char *label)
+{
+	struct pwm_device *dev;
+	int err;
+
+	if (pwm < 0 || pwm >= MAX_PWMS)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&pwm_lock);
+
+	dev = pwm_to_device(pwm);
+	if (!dev) {
+		dev = ERR_PTR(-EPROBE_DEFER);
+		goto out;
+	}
+
+	err = pwm_device_request(dev, label);
+	if (err < 0)
+		dev = ERR_PTR(err);
+
+out:
+	mutex_unlock(&pwm_lock);
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(pwm_request);
+
+/**
+ * pwm_request_from_chip() - request a PWM device relative to a PWM chip
+ * @chip: PWM chip
+ * @index: per-chip index of the PWM to request
+ * @label: a literal description string of this PWM
+ *
+ * Returns the PWM at the given index of the given PWM chip. A negative error
+ * code is returned if the index is not valid for the specified PWM chip or
+ * if the PWM device cannot be requested.
+ */
+struct pwm_device *pwm_request_from_chip(struct pwm_chip *chip,
+					 unsigned int index,
+					 const char *label)
+{
+	struct pwm_device *pwm;
+	int err;
+
+	if (!chip || index >= chip->npwm)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&pwm_lock);
+	pwm = &chip->pwms[index];
+
+	err = pwm_device_request(pwm, label);
+	if (err < 0)
+		pwm = ERR_PTR(err);
+
+	mutex_unlock(&pwm_lock);
+	return pwm;
+}
+EXPORT_SYMBOL_GPL(pwm_request_from_chip);
+
+/**
+ * pwm_free() - free a PWM device
+ * @pwm: PWM device
+ *
+ * This function is deprecated, use pwm_put() instead.
+ */
+void pwm_free(struct pwm_device *pwm)
+{
+	pwm_put(pwm);
+}
+EXPORT_SYMBOL_GPL(pwm_free);
+
+/**
+ * pwm_config() - change a PWM device configuration
+ * @pwm: PWM device
+ * @duty_ns: "on" time (in nanoseconds)
+ * @period_ns: duration (in nanoseconds) of one cycle
+ */
+int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+	if (!pwm || period_ns == 0 || duty_ns > period_ns)
+		return -EINVAL;
+
+	return pwm->chip->ops->config(pwm->chip, pwm, duty_ns, period_ns);
+}
+EXPORT_SYMBOL_GPL(pwm_config);
+
+/**
+ * pwm_enable() - start a PWM output toggling
+ * @pwm: PWM device
+ */
+int pwm_enable(struct pwm_device *pwm)
+{
+	if (pwm && !test_and_set_bit(PWMF_ENABLED, &pwm->flags))
+		return pwm->chip->ops->enable(pwm->chip, pwm);
+
+	return pwm ? 0 : -EINVAL;
+}
+EXPORT_SYMBOL_GPL(pwm_enable);
+
+/**
+ * pwm_disable() - stop a PWM output toggling
+ * @pwm: PWM device
+ */
+void pwm_disable(struct pwm_device *pwm)
+{
+	if (pwm && test_and_clear_bit(PWMF_ENABLED, &pwm->flags))
+		pwm->chip->ops->disable(pwm->chip, pwm);
+}
+EXPORT_SYMBOL_GPL(pwm_disable);
+
+static struct pwm_chip *of_node_to_pwmchip(struct device_node *np)
+{
+	struct pwm_chip *chip;
+
+	mutex_lock(&pwm_lock);
+
+	list_for_each_entry(chip, &pwm_chips, list)
+		if (chip->dev && chip->dev->of_node == np) {
+			mutex_unlock(&pwm_lock);
+			return chip;
+		}
+
+	mutex_unlock(&pwm_lock);
+
+	return ERR_PTR(-EPROBE_DEFER);
+}
+
+/**
+ * of_pwm_request() - request a PWM via the PWM framework
+ * @np: device node to get the PWM from
+ * @con_id: consumer name
+ *
+ * Returns the PWM device parsed from the phandle and index specified in the
+ * "pwms" property of a device tree node or a negative error-code on failure.
+ * Values parsed from the device tree are stored in the returned PWM device
+ * object.
+ *
+ * If con_id is NULL, the first PWM device listed in the "pwms" property will
+ * be requested. Otherwise the "pwm-names" property is used to do a reverse
+ * lookup of the PWM index. This also means that the "pwm-names" property
+ * becomes mandatory for devices that look up the PWM device via the con_id
+ * parameter.
+ */
+static struct pwm_device *of_pwm_request(struct device_node *np,
+					 const char *con_id)
+{
+	struct pwm_device *pwm = NULL;
+	struct of_phandle_args args;
+	struct pwm_chip *pc;
+	int index = 0;
+	int err;
+
+	if (con_id) {
+		index = of_property_match_string(np, "pwm-names", con_id);
+		if (index < 0)
+			return ERR_PTR(index);
+	}
+
+	err = of_parse_phandle_with_args(np, "pwms", "#pwm-cells", index,
+					 &args);
+	if (err) {
+		pr_debug("%s(): can't parse \"pwms\" property\n", __func__);
+		return ERR_PTR(err);
+	}
+
+	pc = of_node_to_pwmchip(args.np);
+	if (IS_ERR(pc)) {
+		pr_debug("%s(): PWM chip not found\n", __func__);
+		pwm = ERR_CAST(pc);
+		goto put;
+	}
+
+	if (args.args_count != pc->of_pwm_n_cells) {
+		pr_debug("%s: wrong #pwm-cells for %s\n", np->full_name,
+			 args.np->full_name);
+		pwm = ERR_PTR(-EINVAL);
+		goto put;
+	}
+
+	pwm = pc->of_xlate(pc, &args);
+	if (IS_ERR(pwm))
+		goto put;
+
+	/*
+	 * If a consumer name was not given, try to look it up from the
+	 * "pwm-names" property if it exists. Otherwise use the name of
+	 * the user device node.
+	 */
+	if (!con_id) {
+		err = of_property_read_string_index(np, "pwm-names", index,
+						    &con_id);
+		if (err < 0)
+			con_id = np->name;
+	}
+
+	pwm->label = con_id;
+
+put:
+	of_node_put(args.np);
+
+	return pwm;
+}
+
+/**
+ * pwm_add_table() - register PWM device consumers
+ * @table: array of consumers to register
+ * @num: number of consumers in table
+ */
+void __init pwm_add_table(struct pwm_lookup *table, size_t num)
+{
+	mutex_lock(&pwm_lookup_lock);
+
+	while (num--) {
+		list_add_tail(&table->list, &pwm_lookup_list);
+		table++;
+	}
+
+	mutex_unlock(&pwm_lookup_lock);
+}
+
+/**
+ * pwm_get() - look up and request a PWM device
+ * @dev: device for PWM consumer
+ * @con_id: consumer name
+ *
+ * Lookup is first attempted using DT. If the device was not instantiated from
+ * a device tree, a PWM chip and a relative index is looked up via a table
+ * supplied by board setup code (see pwm_add_table()).
+ *
+ * Once a PWM chip has been found the specified PWM device will be requested
+ * and is ready to be used.
+ */
+struct pwm_device *pwm_get(struct device *dev, const char *con_id)
+{
+	struct pwm_device *pwm = ERR_PTR(-EPROBE_DEFER);
+	const char *dev_id = dev ? dev_name(dev): NULL;
+	struct pwm_chip *chip = NULL;
+	unsigned int index = 0;
+	unsigned int best = 0;
+	struct pwm_lookup *p;
+	unsigned int match;
+
+	/* look up via DT first */
+	if (IS_ENABLED(CONFIG_OF) && dev && dev->of_node)
+		return of_pwm_request(dev->of_node, con_id);
+
+	/*
+	 * We look up the provider in the static table typically provided by
+	 * board setup code. We first try to lookup the consumer device by
+	 * name. If the consumer device was passed in as NULL or if no match
+	 * was found, we try to find the consumer by directly looking it up
+	 * by name.
+	 *
+	 * If a match is found, the provider PWM chip is looked up by name
+	 * and a PWM device is requested using the PWM device per-chip index.
+	 *
+	 * The lookup algorithm was shamelessly taken from the clock
+	 * framework:
+	 *
+	 * We do slightly fuzzy matching here:
+	 *  An entry with a NULL ID is assumed to be a wildcard.
+	 *  If an entry has a device ID, it must match
+	 *  If an entry has a connection ID, it must match
+	 * Then we take the most specific entry - with the following order
+	 * of precedence: dev+con > dev only > con only.
+	 */
+	mutex_lock(&pwm_lookup_lock);
+
+	list_for_each_entry(p, &pwm_lookup_list, list) {
+		match = 0;
+
+		if (p->dev_id) {
+			if (!dev_id || strcmp(p->dev_id, dev_id))
+				continue;
+
+			match += 2;
+		}
+
+		if (p->con_id) {
+			if (!con_id || strcmp(p->con_id, con_id))
+				continue;
+
+			match += 1;
+		}
+
+		if (match > best) {
+			chip = pwmchip_find_by_name(p->provider);
+			index = p->index;
+
+			if (match != 3)
+				best = match;
+			else
+				break;
+		}
+	}
+
+	if (chip)
+		pwm = pwm_request_from_chip(chip, index, con_id ?: dev_id);
+
+	mutex_unlock(&pwm_lookup_lock);
+
+	return pwm;
+}
+EXPORT_SYMBOL_GPL(pwm_get);
+
+/**
+ * pwm_put() - release a PWM device
+ * @pwm: PWM device
+ */
+void pwm_put(struct pwm_device *pwm)
+{
+	if (!pwm)
+		return;
+
+	mutex_lock(&pwm_lock);
+
+	if (!test_and_clear_bit(PWMF_REQUESTED, &pwm->flags)) {
+		pr_warning("PWM device already freed\n");
+		goto out;
+	}
+
+	if (pwm->chip->ops->free)
+		pwm->chip->ops->free(pwm->chip, pwm);
+
+	pwm->label = NULL;
+
+	module_put(pwm->chip->ops->owner);
+out:
+	mutex_unlock(&pwm_lock);
+}
+EXPORT_SYMBOL_GPL(pwm_put);
+
+#ifdef CONFIG_DEBUG_FS
+static void pwm_dbg_show(struct pwm_chip *chip, struct seq_file *s)
+{
+	unsigned int i;
+
+	for (i = 0; i < chip->npwm; i++) {
+		struct pwm_device *pwm = &chip->pwms[i];
+
+		seq_printf(s, " pwm-%-3d (%-20.20s):", i, pwm->label);
+
+		if (test_bit(PWMF_REQUESTED, &pwm->flags))
+			seq_printf(s, " requested");
+
+		if (test_bit(PWMF_ENABLED, &pwm->flags))
+			seq_printf(s, " enabled");
+
+		seq_printf(s, "\n");
+	}
+}
+
+static void *pwm_seq_start(struct seq_file *s, loff_t *pos)
+{
+	mutex_lock(&pwm_lock);
+	s->private = "";
+
+	return seq_list_start(&pwm_chips, *pos);
+}
+
+static void *pwm_seq_next(struct seq_file *s, void *v, loff_t *pos)
+{
+	s->private = "\n";
+
+	return seq_list_next(v, &pwm_chips, pos);
+}
+
+static void pwm_seq_stop(struct seq_file *s, void *v)
+{
+	mutex_unlock(&pwm_lock);
+}
+
+static int pwm_seq_show(struct seq_file *s, void *v)
+{
+	struct pwm_chip *chip = list_entry(v, struct pwm_chip, list);
+
+	seq_printf(s, "%s%s/%s, %d PWM device%s\n", (char *)s->private,
+		   chip->dev->bus ? chip->dev->bus->name : "no-bus",
+		   dev_name(chip->dev), chip->npwm,
+		   (chip->npwm != 1) ? "s" : "");
+
+	if (chip->ops->dbg_show)
+		chip->ops->dbg_show(chip, s);
+	else
+		pwm_dbg_show(chip, s);
+
+	return 0;
+}
+
+static const struct seq_operations pwm_seq_ops = {
+	.start = pwm_seq_start,
+	.next = pwm_seq_next,
+	.stop = pwm_seq_stop,
+	.show = pwm_seq_show,
+};
+
+static int pwm_seq_open(struct inode *inode, struct file *file)
+{
+	return seq_open(file, &pwm_seq_ops);
+}
+
+static const struct file_operations pwm_debugfs_ops = {
+	.owner = THIS_MODULE,
+	.open = pwm_seq_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int __init pwm_debugfs_init(void)
+{
+	debugfs_create_file("pwm", S_IFREG | S_IRUGO, NULL, NULL,
+			    &pwm_debugfs_ops);
+
+	return 0;
+}
+
+subsys_initcall(pwm_debugfs_init);
+#endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/pwm/pwm-bfin.c b/drivers/pwm/pwm-bfin.c
new file mode 100644
index 00000000000..d53c4e7941e
--- /dev/null
+++ b/drivers/pwm/pwm-bfin.c
@@ -0,0 +1,162 @@
+/*
+ * Blackfin Pulse Width Modulation (PWM) core
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/slab.h>
+
+#include <asm/gptimers.h>
+#include <asm/portmux.h>
+
+struct bfin_pwm_chip {
+	struct pwm_chip chip;
+};
+
+struct bfin_pwm {
+	unsigned short pin;
+};
+
+static const unsigned short pwm_to_gptimer_per[] = {
+	P_TMR0, P_TMR1, P_TMR2, P_TMR3, P_TMR4, P_TMR5,
+	P_TMR6, P_TMR7, P_TMR8, P_TMR9, P_TMR10, P_TMR11,
+};
+
+static int bfin_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bfin_pwm *priv;
+	int ret;
+
+	if (pwm->hwpwm >= ARRAY_SIZE(pwm_to_gptimer_per))
+		return -EINVAL;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->pin = pwm_to_gptimer_per[pwm->hwpwm];
+
+	ret = peripheral_request(priv->pin, NULL);
+	if (ret) {
+		kfree(priv);
+		return ret;
+	}
+
+	pwm_set_chip_data(pwm, priv);
+
+	return 0;
+}
+
+static void bfin_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bfin_pwm *priv = pwm_get_chip_data(pwm);
+
+	if (priv) {
+		peripheral_free(priv->pin);
+		kfree(priv);
+	}
+}
+
+static int bfin_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+		int duty_ns, int period_ns)
+{
+	struct bfin_pwm *priv = pwm_get_chip_data(pwm);
+	unsigned long period, duty;
+	unsigned long long val;
+
+	if (duty_ns < 0 || duty_ns > period_ns)
+		return -EINVAL;
+
+	val = (unsigned long long)get_sclk() * period_ns;
+	do_div(val, NSEC_PER_SEC);
+	period = val;
+
+	val = (unsigned long long)period * duty_ns;
+	do_div(val, period_ns);
+	duty = period - val;
+
+	if (duty >= period)
+		duty = period - 1;
+
+	set_gptimer_config(priv->pin, TIMER_MODE_PWM | TIMER_PERIOD_CNT);
+	set_gptimer_pwidth(priv->pin, duty);
+	set_gptimer_period(priv->pin, period);
+
+	return 0;
+}
+
+static int bfin_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bfin_pwm *priv = pwm_get_chip_data(pwm);
+
+	enable_gptimer(priv->pin);
+
+	return 0;
+}
+
+static void bfin_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct bfin_pwm *priv = pwm_get_chip_data(pwm);
+
+	disable_gptimer(priv->pin);
+}
+
+static struct pwm_ops bfin_pwm_ops = {
+	.request = bfin_pwm_request,
+	.free = bfin_pwm_free,
+	.config = bfin_pwm_config,
+	.enable = bfin_pwm_enable,
+	.disable = bfin_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int bfin_pwm_probe(struct platform_device *pdev)
+{
+	struct bfin_pwm_chip *pwm;
+	int ret;
+
+	pwm = devm_kzalloc(&pdev->dev, sizeof(*pwm), GFP_KERNEL);
+	if (!pwm) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, pwm);
+
+	pwm->chip.dev = &pdev->dev;
+	pwm->chip.ops = &bfin_pwm_ops;
+	pwm->chip.base = -1;
+	pwm->chip.npwm = 12;
+
+	ret = pwmchip_add(&pwm->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit bfin_pwm_remove(struct platform_device *pdev)
+{
+	struct bfin_pwm_chip *pwm = platform_get_drvdata(pdev);
+
+	return pwmchip_remove(&pwm->chip);
+}
+
+static struct platform_driver bfin_pwm_driver = {
+	.driver = {
+		.name = "bfin-pwm",
+	},
+	.probe = bfin_pwm_probe,
+	.remove = __devexit_p(bfin_pwm_remove),
+};
+
+module_platform_driver(bfin_pwm_driver);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
new file mode 100644
index 00000000000..2a0b3533397
--- /dev/null
+++ b/drivers/pwm/pwm-imx.c
@@ -0,0 +1,230 @@
+/*
+ * simple driver for PWM (Pulse Width Modulator) controller
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from pxa PWM driver by eric miao <eric.miao@marvell.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+#include <mach/hardware.h>
+
+
+/* i.MX1 and i.MX21 share the same PWM function block: */
+
+#define MX1_PWMC    0x00   /* PWM Control Register */
+#define MX1_PWMS    0x04   /* PWM Sample Register */
+#define MX1_PWMP    0x08   /* PWM Period Register */
+
+
+/* i.MX27, i.MX31, i.MX35 share the same PWM function block: */
+
+#define MX3_PWMCR                 0x00    /* PWM Control Register */
+#define MX3_PWMSAR                0x0C    /* PWM Sample Register */
+#define MX3_PWMPR                 0x10    /* PWM Period Register */
+#define MX3_PWMCR_PRESCALER(x)    (((x - 1) & 0xFFF) << 4)
+#define MX3_PWMCR_DOZEEN                (1 << 24)
+#define MX3_PWMCR_WAITEN                (1 << 23)
+#define MX3_PWMCR_DBGEN			(1 << 22)
+#define MX3_PWMCR_CLKSRC_IPG_HIGH (2 << 16)
+#define MX3_PWMCR_CLKSRC_IPG      (1 << 16)
+#define MX3_PWMCR_EN              (1 << 0)
+
+struct imx_chip {
+	struct clk	*clk;
+
+	int		clk_enabled;
+	void __iomem	*mmio_base;
+
+	struct pwm_chip	chip;
+};
+
+#define to_imx_chip(chip)	container_of(chip, struct imx_chip, chip)
+
+static int imx_pwm_config(struct pwm_chip *chip,
+		struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+
+	if (!(cpu_is_mx1() || cpu_is_mx21())) {
+		unsigned long long c;
+		unsigned long period_cycles, duty_cycles, prescale;
+		u32 cr;
+
+		c = clk_get_rate(imx->clk);
+		c = c * period_ns;
+		do_div(c, 1000000000);
+		period_cycles = c;
+
+		prescale = period_cycles / 0x10000 + 1;
+
+		period_cycles /= prescale;
+		c = (unsigned long long)period_cycles * duty_ns;
+		do_div(c, period_ns);
+		duty_cycles = c;
+
+		/*
+		 * according to imx pwm RM, the real period value should be
+		 * PERIOD value in PWMPR plus 2.
+		 */
+		if (period_cycles > 2)
+			period_cycles -= 2;
+		else
+			period_cycles = 0;
+
+		writel(duty_cycles, imx->mmio_base + MX3_PWMSAR);
+		writel(period_cycles, imx->mmio_base + MX3_PWMPR);
+
+		cr = MX3_PWMCR_PRESCALER(prescale) |
+			MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
+			MX3_PWMCR_DBGEN | MX3_PWMCR_EN;
+
+		if (cpu_is_mx25())
+			cr |= MX3_PWMCR_CLKSRC_IPG;
+		else
+			cr |= MX3_PWMCR_CLKSRC_IPG_HIGH;
+
+		writel(cr, imx->mmio_base + MX3_PWMCR);
+	} else if (cpu_is_mx1() || cpu_is_mx21()) {
+		/* The PWM subsystem allows for exact frequencies. However,
+		 * I cannot connect a scope on my device to the PWM line and
+		 * thus cannot provide the program the PWM controller
+		 * exactly. Instead, I'm relying on the fact that the
+		 * Bootloader (u-boot or WinCE+haret) has programmed the PWM
+		 * function group already. So I'll just modify the PWM sample
+		 * register to follow the ratio of duty_ns vs. period_ns
+		 * accordingly.
+		 *
+		 * This is good enough for programming the brightness of
+		 * the LCD backlight.
+		 *
+		 * The real implementation would divide PERCLK[0] first by
+		 * both the prescaler (/1 .. /128) and then by CLKSEL
+		 * (/2 .. /16).
+		 */
+		u32 max = readl(imx->mmio_base + MX1_PWMP);
+		u32 p = max * duty_ns / period_ns;
+		writel(max - p, imx->mmio_base + MX1_PWMS);
+	} else {
+		BUG();
+	}
+
+	return 0;
+}
+
+static int imx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+	int rc = 0;
+
+	if (!imx->clk_enabled) {
+		rc = clk_prepare_enable(imx->clk);
+		if (!rc)
+			imx->clk_enabled = 1;
+	}
+	return rc;
+}
+
+static void imx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+
+	writel(0, imx->mmio_base + MX3_PWMCR);
+
+	if (imx->clk_enabled) {
+		clk_disable_unprepare(imx->clk);
+		imx->clk_enabled = 0;
+	}
+}
+
+static struct pwm_ops imx_pwm_ops = {
+	.enable = imx_pwm_enable,
+	.disable = imx_pwm_disable,
+	.config = imx_pwm_config,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit imx_pwm_probe(struct platform_device *pdev)
+{
+	struct imx_chip *imx;
+	struct resource *r;
+	int ret = 0;
+
+	imx = devm_kzalloc(&pdev->dev, sizeof(*imx), GFP_KERNEL);
+	if (imx == NULL) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	imx->clk = devm_clk_get(&pdev->dev, "pwm");
+
+	if (IS_ERR(imx->clk))
+		return PTR_ERR(imx->clk);
+
+	imx->chip.ops = &imx_pwm_ops;
+	imx->chip.dev = &pdev->dev;
+	imx->chip.base = -1;
+	imx->chip.npwm = 1;
+
+	imx->clk_enabled = 0;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no memory resource defined\n");
+		return -ENODEV;
+	}
+
+	imx->mmio_base = devm_request_and_ioremap(&pdev->dev, r);
+	if (imx->mmio_base == NULL)
+		return -EADDRNOTAVAIL;
+
+	ret = pwmchip_add(&imx->chip);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, imx);
+	return 0;
+}
+
+static int __devexit imx_pwm_remove(struct platform_device *pdev)
+{
+	struct imx_chip *imx;
+
+	imx = platform_get_drvdata(pdev);
+	if (imx == NULL)
+		return -ENODEV;
+
+	return pwmchip_remove(&imx->chip);
+}
+
+static struct platform_driver imx_pwm_driver = {
+	.driver		= {
+		.name	= "mxc_pwm",
+	},
+	.probe		= imx_pwm_probe,
+	.remove		= __devexit_p(imx_pwm_remove),
+};
+
+static int __init imx_pwm_init(void)
+{
+	return platform_driver_register(&imx_pwm_driver);
+}
+arch_initcall(imx_pwm_init);
+
+static void __exit imx_pwm_exit(void)
+{
+	platform_driver_unregister(&imx_pwm_driver);
+}
+module_exit(imx_pwm_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
diff --git a/drivers/pwm/pwm-lpc32xx.c b/drivers/pwm/pwm-lpc32xx.c
new file mode 100644
index 00000000000..adb87f0c163
--- /dev/null
+++ b/drivers/pwm/pwm-lpc32xx.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright 2012 Alexandre Pereira da Silva <aletes.xgr@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/slab.h>
+
+struct lpc32xx_pwm_chip {
+	struct pwm_chip chip;
+	struct clk *clk;
+	void __iomem *base;
+};
+
+#define PWM_ENABLE	(1 << 31)
+#define PWM_RELOADV(x)	(((x) & 0xFF) << 8)
+#define PWM_DUTY(x)	((x) & 0xFF)
+
+#define to_lpc32xx_pwm_chip(_chip) \
+	container_of(_chip, struct lpc32xx_pwm_chip, chip)
+
+static int lpc32xx_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			      int duty_ns, int period_ns)
+{
+	struct lpc32xx_pwm_chip *lpc32xx = to_lpc32xx_pwm_chip(chip);
+	unsigned long long c;
+	int period_cycles, duty_cycles;
+
+	c = clk_get_rate(lpc32xx->clk) / 256;
+	c = c * period_ns;
+	do_div(c, NSEC_PER_SEC);
+
+	/* Handle high and low extremes */
+	if (c == 0)
+		c = 1;
+	if (c > 255)
+		c = 0; /* 0 set division by 256 */
+	period_cycles = c;
+
+	c = 256 * duty_ns;
+	do_div(c, period_ns);
+	duty_cycles = c;
+
+	writel(PWM_ENABLE | PWM_RELOADV(period_cycles) | PWM_DUTY(duty_cycles),
+		lpc32xx->base + (pwm->hwpwm << 2));
+
+	return 0;
+}
+
+static int lpc32xx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct lpc32xx_pwm_chip *lpc32xx = to_lpc32xx_pwm_chip(chip);
+
+	return clk_enable(lpc32xx->clk);
+}
+
+static void lpc32xx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct lpc32xx_pwm_chip *lpc32xx = to_lpc32xx_pwm_chip(chip);
+
+	writel(0, lpc32xx->base + (pwm->hwpwm << 2));
+	clk_disable(lpc32xx->clk);
+}
+
+static const struct pwm_ops lpc32xx_pwm_ops = {
+	.config = lpc32xx_pwm_config,
+	.enable = lpc32xx_pwm_enable,
+	.disable = lpc32xx_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int lpc32xx_pwm_probe(struct platform_device *pdev)
+{
+	struct lpc32xx_pwm_chip *lpc32xx;
+	struct resource *res;
+	int ret;
+
+	lpc32xx = devm_kzalloc(&pdev->dev, sizeof(*lpc32xx), GFP_KERNEL);
+	if (!lpc32xx)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+
+	lpc32xx->base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!lpc32xx->base)
+		return -EADDRNOTAVAIL;
+
+	lpc32xx->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(lpc32xx->clk))
+		return PTR_ERR(lpc32xx->clk);
+
+	lpc32xx->chip.dev = &pdev->dev;
+	lpc32xx->chip.ops = &lpc32xx_pwm_ops;
+	lpc32xx->chip.npwm = 2;
+
+	ret = pwmchip_add(&lpc32xx->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add PWM chip, error %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, lpc32xx);
+
+	return 0;
+}
+
+static int __devexit lpc32xx_pwm_remove(struct platform_device *pdev)
+{
+	struct lpc32xx_pwm_chip *lpc32xx = platform_get_drvdata(pdev);
+
+	clk_disable(lpc32xx->clk);
+	return pwmchip_remove(&lpc32xx->chip);
+}
+
+static struct of_device_id lpc32xx_pwm_dt_ids[] = {
+	{ .compatible = "nxp,lpc3220-pwm", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_pwm_dt_ids);
+
+static struct platform_driver lpc32xx_pwm_driver = {
+	.driver = {
+		.name = "lpc32xx-pwm",
+		.of_match_table = of_match_ptr(lpc32xx_pwm_dt_ids),
+	},
+	.probe = lpc32xx_pwm_probe,
+	.remove = __devexit_p(lpc32xx_pwm_remove),
+};
+module_platform_driver(lpc32xx_pwm_driver);
+
+MODULE_ALIAS("platform:lpc32xx-pwm");
+MODULE_AUTHOR("Alexandre Pereira da Silva <aletes.xgr@gmail.com>");
+MODULE_DESCRIPTION("LPC32XX PWM Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-mxs.c b/drivers/pwm/pwm-mxs.c
new file mode 100644
index 00000000000..e5852646f08
--- /dev/null
+++ b/drivers/pwm/pwm-mxs.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/slab.h>
+#include <linux/stmp_device.h>
+
+#define SET	0x4
+#define CLR	0x8
+#define TOG	0xc
+
+#define PWM_CTRL		0x0
+#define PWM_ACTIVE0		0x10
+#define PWM_PERIOD0		0x20
+#define  PERIOD_PERIOD(p)	((p) & 0xffff)
+#define  PERIOD_PERIOD_MAX	0x10000
+#define  PERIOD_ACTIVE_HIGH	(3 << 16)
+#define  PERIOD_INACTIVE_LOW	(2 << 18)
+#define  PERIOD_CDIV(div)	(((div) & 0x7) << 20)
+#define  PERIOD_CDIV_MAX	8
+
+struct mxs_pwm_chip {
+	struct pwm_chip chip;
+	struct device *dev;
+	struct clk *clk;
+	void __iomem *base;
+};
+
+#define to_mxs_pwm_chip(_chip) container_of(_chip, struct mxs_pwm_chip, chip)
+
+static int mxs_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			  int duty_ns, int period_ns)
+{
+	struct mxs_pwm_chip *mxs = to_mxs_pwm_chip(chip);
+	int ret, div = 0;
+	unsigned int period_cycles, duty_cycles;
+	unsigned long rate;
+	unsigned long long c;
+
+	rate = clk_get_rate(mxs->clk);
+	while (1) {
+		c = rate / (1 << div);
+		c = c * period_ns;
+		do_div(c, 1000000000);
+		if (c < PERIOD_PERIOD_MAX)
+			break;
+		div++;
+		if (div > PERIOD_CDIV_MAX)
+			return -EINVAL;
+	}
+
+	period_cycles = c;
+	c *= duty_ns;
+	do_div(c, period_ns);
+	duty_cycles = c;
+
+	/*
+	 * If the PWM channel is disabled, make sure to turn on the clock
+	 * before writing the register. Otherwise, keep it enabled.
+	 */
+	if (!test_bit(PWMF_ENABLED, &pwm->flags)) {
+		ret = clk_prepare_enable(mxs->clk);
+		if (ret)
+			return ret;
+	}
+
+	writel(duty_cycles << 16,
+			mxs->base + PWM_ACTIVE0 + pwm->hwpwm * 0x20);
+	writel(PERIOD_PERIOD(period_cycles) | PERIOD_ACTIVE_HIGH |
+	       PERIOD_INACTIVE_LOW | PERIOD_CDIV(div),
+			mxs->base + PWM_PERIOD0 + pwm->hwpwm * 0x20);
+
+	/*
+	 * If the PWM is not enabled, turn the clock off again to save power.
+	 */
+	if (!test_bit(PWMF_ENABLED, &pwm->flags))
+		clk_disable_unprepare(mxs->clk);
+
+	return 0;
+}
+
+static int mxs_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct mxs_pwm_chip *mxs = to_mxs_pwm_chip(chip);
+	int ret;
+
+	ret = clk_prepare_enable(mxs->clk);
+	if (ret)
+		return ret;
+
+	writel(1 << pwm->hwpwm, mxs->base + PWM_CTRL + SET);
+
+	return 0;
+}
+
+static void mxs_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct mxs_pwm_chip *mxs = to_mxs_pwm_chip(chip);
+
+	writel(1 << pwm->hwpwm, mxs->base + PWM_CTRL + CLR);
+
+	clk_disable_unprepare(mxs->clk);
+}
+
+static const struct pwm_ops mxs_pwm_ops = {
+	.config = mxs_pwm_config,
+	.enable = mxs_pwm_enable,
+	.disable = mxs_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int mxs_pwm_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct mxs_pwm_chip *mxs;
+	struct resource *res;
+	struct pinctrl *pinctrl;
+	int ret;
+
+	mxs = devm_kzalloc(&pdev->dev, sizeof(*mxs), GFP_KERNEL);
+	if (!mxs)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mxs->base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!mxs->base)
+		return -EADDRNOTAVAIL;
+
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl))
+		return PTR_ERR(pinctrl);
+
+	mxs->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(mxs->clk))
+		return PTR_ERR(mxs->clk);
+
+	mxs->chip.dev = &pdev->dev;
+	mxs->chip.ops = &mxs_pwm_ops;
+	mxs->chip.base = -1;
+	ret = of_property_read_u32(np, "fsl,pwm-number", &mxs->chip.npwm);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to get pwm number: %d\n", ret);
+		return ret;
+	}
+
+	ret = pwmchip_add(&mxs->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add pwm chip %d\n", ret);
+		return ret;
+	}
+
+	mxs->dev = &pdev->dev;
+	platform_set_drvdata(pdev, mxs);
+
+	stmp_reset_block(mxs->base);
+
+	return 0;
+}
+
+static int __devexit mxs_pwm_remove(struct platform_device *pdev)
+{
+	struct mxs_pwm_chip *mxs = platform_get_drvdata(pdev);
+
+	return pwmchip_remove(&mxs->chip);
+}
+
+static struct of_device_id mxs_pwm_dt_ids[] = {
+	{ .compatible = "fsl,imx23-pwm", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_pwm_dt_ids);
+
+static struct platform_driver mxs_pwm_driver = {
+	.driver = {
+		.name = "mxs-pwm",
+		.of_match_table = of_match_ptr(mxs_pwm_dt_ids),
+	},
+	.probe = mxs_pwm_probe,
+	.remove = __devexit_p(mxs_pwm_remove),
+};
+module_platform_driver(mxs_pwm_driver);
+
+MODULE_ALIAS("platform:mxs-pwm");
+MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
+MODULE_DESCRIPTION("Freescale MXS PWM Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-pxa.c b/drivers/pwm/pwm-pxa.c
new file mode 100644
index 00000000000..bd5867a1c70
--- /dev/null
+++ b/drivers/pwm/pwm-pxa.c
@@ -0,0 +1,218 @@
+/*
+ * drivers/pwm/pwm-pxa.c
+ *
+ * simple driver for PWM (Pulse Width Modulator) controller
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 2008-02-13	initial version
+ * 		eric miao <eric.miao@marvell.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+
+#include <asm/div64.h>
+
+#define HAS_SECONDARY_PWM	0x10
+#define PWM_ID_BASE(d)		((d) & 0xf)
+
+static const struct platform_device_id pwm_id_table[] = {
+	/*   PWM    has_secondary_pwm? */
+	{ "pxa25x-pwm", 0 },
+	{ "pxa27x-pwm", 0 | HAS_SECONDARY_PWM },
+	{ "pxa168-pwm", 1 },
+	{ "pxa910-pwm", 1 },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, pwm_id_table);
+
+/* PWM registers and bits definitions */
+#define PWMCR		(0x00)
+#define PWMDCR		(0x04)
+#define PWMPCR		(0x08)
+
+#define PWMCR_SD	(1 << 6)
+#define PWMDCR_FD	(1 << 10)
+
+struct pxa_pwm_chip {
+	struct pwm_chip	chip;
+	struct device	*dev;
+
+	struct clk	*clk;
+	int		clk_enabled;
+	void __iomem	*mmio_base;
+};
+
+static inline struct pxa_pwm_chip *to_pxa_pwm_chip(struct pwm_chip *chip)
+{
+	return container_of(chip, struct pxa_pwm_chip, chip);
+}
+
+/*
+ * period_ns = 10^9 * (PRESCALE + 1) * (PV + 1) / PWM_CLK_RATE
+ * duty_ns   = 10^9 * (PRESCALE + 1) * DC / PWM_CLK_RATE
+ */
+static int pxa_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			  int duty_ns, int period_ns)
+{
+	struct pxa_pwm_chip *pc = to_pxa_pwm_chip(chip);
+	unsigned long long c;
+	unsigned long period_cycles, prescale, pv, dc;
+	unsigned long offset;
+	int rc;
+
+	if (period_ns == 0 || duty_ns > period_ns)
+		return -EINVAL;
+
+	offset = pwm->hwpwm ? 0x10 : 0;
+
+	c = clk_get_rate(pc->clk);
+	c = c * period_ns;
+	do_div(c, 1000000000);
+	period_cycles = c;
+
+	if (period_cycles < 1)
+		period_cycles = 1;
+	prescale = (period_cycles - 1) / 1024;
+	pv = period_cycles / (prescale + 1) - 1;
+
+	if (prescale > 63)
+		return -EINVAL;
+
+	if (duty_ns == period_ns)
+		dc = PWMDCR_FD;
+	else
+		dc = (pv + 1) * duty_ns / period_ns;
+
+	/* NOTE: the clock to PWM has to be enabled first
+	 * before writing to the registers
+	 */
+	rc = clk_prepare_enable(pc->clk);
+	if (rc < 0)
+		return rc;
+
+	writel(prescale, pc->mmio_base + offset + PWMCR);
+	writel(dc, pc->mmio_base + offset + PWMDCR);
+	writel(pv, pc->mmio_base + offset + PWMPCR);
+
+	clk_disable_unprepare(pc->clk);
+	return 0;
+}
+
+static int pxa_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct pxa_pwm_chip *pc = to_pxa_pwm_chip(chip);
+	int rc = 0;
+
+	if (!pc->clk_enabled) {
+		rc = clk_prepare_enable(pc->clk);
+		if (!rc)
+			pc->clk_enabled++;
+	}
+	return rc;
+}
+
+static void pxa_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct pxa_pwm_chip *pc = to_pxa_pwm_chip(chip);
+
+	if (pc->clk_enabled) {
+		clk_disable_unprepare(pc->clk);
+		pc->clk_enabled--;
+	}
+}
+
+static struct pwm_ops pxa_pwm_ops = {
+	.config = pxa_pwm_config,
+	.enable = pxa_pwm_enable,
+	.disable = pxa_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit pwm_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *id = platform_get_device_id(pdev);
+	struct pxa_pwm_chip *pwm;
+	struct resource *r;
+	int ret = 0;
+
+	pwm = devm_kzalloc(&pdev->dev, sizeof(*pwm), GFP_KERNEL);
+	if (pwm == NULL) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	pwm->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pwm->clk))
+		return PTR_ERR(pwm->clk);
+
+	pwm->clk_enabled = 0;
+
+	pwm->chip.dev = &pdev->dev;
+	pwm->chip.ops = &pxa_pwm_ops;
+	pwm->chip.base = -1;
+	pwm->chip.npwm = (id->driver_data & HAS_SECONDARY_PWM) ? 2 : 1;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no memory resource defined\n");
+		return -ENODEV;
+	}
+
+	pwm->mmio_base = devm_request_and_ioremap(&pdev->dev, r);
+	if (pwm->mmio_base == NULL)
+		return -EADDRNOTAVAIL;
+
+	ret = pwmchip_add(&pwm->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, pwm);
+	return 0;
+}
+
+static int __devexit pwm_remove(struct platform_device *pdev)
+{
+	struct pxa_pwm_chip *chip;
+
+	chip = platform_get_drvdata(pdev);
+	if (chip == NULL)
+		return -ENODEV;
+
+	return pwmchip_remove(&chip->chip);
+}
+
+static struct platform_driver pwm_driver = {
+	.driver		= {
+		.name	= "pxa25x-pwm",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= pwm_probe,
+	.remove		= __devexit_p(pwm_remove),
+	.id_table	= pwm_id_table,
+};
+
+static int __init pwm_init(void)
+{
+	return platform_driver_register(&pwm_driver);
+}
+arch_initcall(pwm_init);
+
+static void __exit pwm_exit(void)
+{
+	platform_driver_unregister(&pwm_driver);
+}
+module_exit(pwm_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
new file mode 100644
index 00000000000..d10386528c9
--- /dev/null
+++ b/drivers/pwm/pwm-samsung.c
@@ -0,0 +1,356 @@
+/* drivers/pwm/pwm-samsung.c
+ *
+ * Copyright (c) 2007 Ben Dooks
+ * Copyright (c) 2008 Simtec Electronics
+ *	Ben Dooks <ben@simtec.co.uk>, <ben-linux@fluff.org>
+ *
+ * S3C series PWM device core
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License.
+*/
+
+#define pr_fmt(fmt) "pwm-samsung: " fmt
+
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+
+#include <mach/map.h>
+
+#include <plat/regs-timer.h>
+
+struct s3c_chip {
+	struct platform_device	*pdev;
+
+	struct clk		*clk_div;
+	struct clk		*clk;
+	const char		*label;
+
+	unsigned int		 period_ns;
+	unsigned int		 duty_ns;
+
+	unsigned char		 tcon_base;
+	unsigned char		 pwm_id;
+	struct pwm_chip		 chip;
+};
+
+#define to_s3c_chip(chip)	container_of(chip, struct s3c_chip, chip)
+
+#define pwm_dbg(_pwm, msg...) dev_dbg(&(_pwm)->pdev->dev, msg)
+
+static struct clk *clk_scaler[2];
+
+static inline int pwm_is_tdiv(struct s3c_chip *chip)
+{
+	return clk_get_parent(chip->clk) == chip->clk_div;
+}
+
+#define pwm_tcon_start(pwm) (1 << (pwm->tcon_base + 0))
+#define pwm_tcon_invert(pwm) (1 << (pwm->tcon_base + 2))
+#define pwm_tcon_autoreload(pwm) (1 << (pwm->tcon_base + 3))
+#define pwm_tcon_manulupdate(pwm) (1 << (pwm->tcon_base + 1))
+
+static int s3c_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct s3c_chip *s3c = to_s3c_chip(chip);
+	unsigned long flags;
+	unsigned long tcon;
+
+	local_irq_save(flags);
+
+	tcon = __raw_readl(S3C2410_TCON);
+	tcon |= pwm_tcon_start(s3c);
+	__raw_writel(tcon, S3C2410_TCON);
+
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static void s3c_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct s3c_chip *s3c = to_s3c_chip(chip);
+	unsigned long flags;
+	unsigned long tcon;
+
+	local_irq_save(flags);
+
+	tcon = __raw_readl(S3C2410_TCON);
+	tcon &= ~pwm_tcon_start(s3c);
+	__raw_writel(tcon, S3C2410_TCON);
+
+	local_irq_restore(flags);
+}
+
+static unsigned long pwm_calc_tin(struct s3c_chip *s3c, unsigned long freq)
+{
+	unsigned long tin_parent_rate;
+	unsigned int div;
+
+	tin_parent_rate = clk_get_rate(clk_get_parent(s3c->clk_div));
+	pwm_dbg(s3c, "tin parent at %lu\n", tin_parent_rate);
+
+	for (div = 2; div <= 16; div *= 2) {
+		if ((tin_parent_rate / (div << 16)) < freq)
+			return tin_parent_rate / div;
+	}
+
+	return tin_parent_rate / 16;
+}
+
+#define NS_IN_HZ (1000000000UL)
+
+static int s3c_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+		int duty_ns, int period_ns)
+{
+	struct s3c_chip *s3c = to_s3c_chip(chip);
+	unsigned long tin_rate;
+	unsigned long tin_ns;
+	unsigned long period;
+	unsigned long flags;
+	unsigned long tcon;
+	unsigned long tcnt;
+	long tcmp;
+
+	/* We currently avoid using 64bit arithmetic by using the
+	 * fact that anything faster than 1Hz is easily representable
+	 * by 32bits. */
+
+	if (period_ns > NS_IN_HZ || duty_ns > NS_IN_HZ)
+		return -ERANGE;
+
+	if (duty_ns > period_ns)
+		return -EINVAL;
+
+	if (period_ns == s3c->period_ns &&
+	    duty_ns == s3c->duty_ns)
+		return 0;
+
+	/* The TCMP and TCNT can be read without a lock, they're not
+	 * shared between the timers. */
+
+	tcmp = __raw_readl(S3C2410_TCMPB(s3c->pwm_id));
+	tcnt = __raw_readl(S3C2410_TCNTB(s3c->pwm_id));
+
+	period = NS_IN_HZ / period_ns;
+
+	pwm_dbg(s3c, "duty_ns=%d, period_ns=%d (%lu)\n",
+		duty_ns, period_ns, period);
+
+	/* Check to see if we are changing the clock rate of the PWM */
+
+	if (s3c->period_ns != period_ns) {
+		if (pwm_is_tdiv(s3c)) {
+			tin_rate = pwm_calc_tin(s3c, period);
+			clk_set_rate(s3c->clk_div, tin_rate);
+		} else
+			tin_rate = clk_get_rate(s3c->clk);
+
+		s3c->period_ns = period_ns;
+
+		pwm_dbg(s3c, "tin_rate=%lu\n", tin_rate);
+
+		tin_ns = NS_IN_HZ / tin_rate;
+		tcnt = period_ns / tin_ns;
+	} else
+		tin_ns = NS_IN_HZ / clk_get_rate(s3c->clk);
+
+	/* Note, counters count down */
+
+	tcmp = duty_ns / tin_ns;
+	tcmp = tcnt - tcmp;
+	/* the pwm hw only checks the compare register after a decrement,
+	   so the pin never toggles if tcmp = tcnt */
+	if (tcmp == tcnt)
+		tcmp--;
+
+	pwm_dbg(s3c, "tin_ns=%lu, tcmp=%ld/%lu\n", tin_ns, tcmp, tcnt);
+
+	if (tcmp < 0)
+		tcmp = 0;
+
+	/* Update the PWM register block. */
+
+	local_irq_save(flags);
+
+	__raw_writel(tcmp, S3C2410_TCMPB(s3c->pwm_id));
+	__raw_writel(tcnt, S3C2410_TCNTB(s3c->pwm_id));
+
+	tcon = __raw_readl(S3C2410_TCON);
+	tcon |= pwm_tcon_manulupdate(s3c);
+	tcon |= pwm_tcon_autoreload(s3c);
+	__raw_writel(tcon, S3C2410_TCON);
+
+	tcon &= ~pwm_tcon_manulupdate(s3c);
+	__raw_writel(tcon, S3C2410_TCON);
+
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static struct pwm_ops s3c_pwm_ops = {
+	.enable = s3c_pwm_enable,
+	.disable = s3c_pwm_disable,
+	.config = s3c_pwm_config,
+	.owner = THIS_MODULE,
+};
+
+static int s3c_pwm_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct s3c_chip *s3c;
+	unsigned long flags;
+	unsigned long tcon;
+	unsigned int id = pdev->id;
+	int ret;
+
+	if (id == 4) {
+		dev_err(dev, "TIMER4 is currently not supported\n");
+		return -ENXIO;
+	}
+
+	s3c = devm_kzalloc(&pdev->dev, sizeof(*s3c), GFP_KERNEL);
+	if (s3c == NULL) {
+		dev_err(dev, "failed to allocate pwm_device\n");
+		return -ENOMEM;
+	}
+
+	/* calculate base of control bits in TCON */
+	s3c->tcon_base = id == 0 ? 0 : (id * 4) + 4;
+	s3c->chip.ops = &s3c_pwm_ops;
+	s3c->chip.base = -1;
+	s3c->chip.npwm = 1;
+
+	s3c->clk = devm_clk_get(dev, "pwm-tin");
+	if (IS_ERR(s3c->clk)) {
+		dev_err(dev, "failed to get pwm tin clk\n");
+		return PTR_ERR(s3c->clk);
+	}
+
+	s3c->clk_div = devm_clk_get(dev, "pwm-tdiv");
+	if (IS_ERR(s3c->clk_div)) {
+		dev_err(dev, "failed to get pwm tdiv clk\n");
+		return PTR_ERR(s3c->clk_div);
+	}
+
+	clk_enable(s3c->clk);
+	clk_enable(s3c->clk_div);
+
+	local_irq_save(flags);
+
+	tcon = __raw_readl(S3C2410_TCON);
+	tcon |= pwm_tcon_invert(s3c);
+	__raw_writel(tcon, S3C2410_TCON);
+
+	local_irq_restore(flags);
+
+	ret = pwmchip_add(&s3c->chip);
+	if (ret < 0) {
+		dev_err(dev, "failed to register pwm\n");
+		goto err_clk_tdiv;
+	}
+
+	pwm_dbg(s3c, "config bits %02x\n",
+		(__raw_readl(S3C2410_TCON) >> s3c->tcon_base) & 0x0f);
+
+	dev_info(dev, "tin at %lu, tdiv at %lu, tin=%sclk, base %d\n",
+		 clk_get_rate(s3c->clk),
+		 clk_get_rate(s3c->clk_div),
+		 pwm_is_tdiv(s3c) ? "div" : "ext", s3c->tcon_base);
+
+	platform_set_drvdata(pdev, s3c);
+	return 0;
+
+ err_clk_tdiv:
+	clk_disable(s3c->clk_div);
+	clk_disable(s3c->clk);
+	return ret;
+}
+
+static int __devexit s3c_pwm_remove(struct platform_device *pdev)
+{
+	struct s3c_chip *s3c = platform_get_drvdata(pdev);
+	int err;
+
+	err = pwmchip_remove(&s3c->chip);
+	if (err < 0)
+		return err;
+
+	clk_disable(s3c->clk_div);
+	clk_disable(s3c->clk);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int s3c_pwm_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct s3c_chip *s3c = platform_get_drvdata(pdev);
+
+	/* No one preserve these values during suspend so reset them
+	 * Otherwise driver leaves PWM unconfigured if same values
+	 * passed to pwm_config
+	 */
+	s3c->period_ns = 0;
+	s3c->duty_ns = 0;
+
+	return 0;
+}
+
+static int s3c_pwm_resume(struct platform_device *pdev)
+{
+	struct s3c_chip *s3c = platform_get_drvdata(pdev);
+	unsigned long tcon;
+
+	/* Restore invertion */
+	tcon = __raw_readl(S3C2410_TCON);
+	tcon |= pwm_tcon_invert(s3c);
+	__raw_writel(tcon, S3C2410_TCON);
+
+	return 0;
+}
+
+#else
+#define s3c_pwm_suspend NULL
+#define s3c_pwm_resume NULL
+#endif
+
+static struct platform_driver s3c_pwm_driver = {
+	.driver		= {
+		.name	= "s3c24xx-pwm",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= s3c_pwm_probe,
+	.remove		= __devexit_p(s3c_pwm_remove),
+	.suspend	= s3c_pwm_suspend,
+	.resume		= s3c_pwm_resume,
+};
+
+static int __init pwm_init(void)
+{
+	int ret;
+
+	clk_scaler[0] = clk_get(NULL, "pwm-scaler0");
+	clk_scaler[1] = clk_get(NULL, "pwm-scaler1");
+
+	if (IS_ERR(clk_scaler[0]) || IS_ERR(clk_scaler[1])) {
+		pr_err("failed to get scaler clocks\n");
+		return -EINVAL;
+	}
+
+	ret = platform_driver_register(&s3c_pwm_driver);
+	if (ret)
+		pr_err("failed to add pwm driver\n");
+
+	return ret;
+}
+
+arch_initcall(pwm_init);
diff --git a/drivers/pwm/pwm-tegra.c b/drivers/pwm/pwm-tegra.c
new file mode 100644
index 00000000000..02ce18d5e49
--- /dev/null
+++ b/drivers/pwm/pwm-tegra.c
@@ -0,0 +1,261 @@
+/*
+ * drivers/pwm/pwm-tegra.c
+ *
+ * Tegra pulse-width-modulation controller driver
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ * Based on arch/arm/plat-mxc/pwm.c by Sascha Hauer <s.hauer@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pwm.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define PWM_ENABLE	(1 << 31)
+#define PWM_DUTY_WIDTH	8
+#define PWM_DUTY_SHIFT	16
+#define PWM_SCALE_WIDTH	13
+#define PWM_SCALE_SHIFT	0
+
+#define NUM_PWM 4
+
+struct tegra_pwm_chip {
+	struct pwm_chip		chip;
+	struct device		*dev;
+
+	struct clk		*clk;
+
+	void __iomem		*mmio_base;
+};
+
+static inline struct tegra_pwm_chip *to_tegra_pwm_chip(struct pwm_chip *chip)
+{
+	return container_of(chip, struct tegra_pwm_chip, chip);
+}
+
+static inline u32 pwm_readl(struct tegra_pwm_chip *chip, unsigned int num)
+{
+	return readl(chip->mmio_base + (num << 4));
+}
+
+static inline void pwm_writel(struct tegra_pwm_chip *chip, unsigned int num,
+			     unsigned long val)
+{
+	writel(val, chip->mmio_base + (num << 4));
+}
+
+static int tegra_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			    int duty_ns, int period_ns)
+{
+	struct tegra_pwm_chip *pc = to_tegra_pwm_chip(chip);
+	unsigned long long c;
+	unsigned long rate, hz;
+	u32 val = 0;
+	int err;
+
+	/*
+	 * Convert from duty_ns / period_ns to a fixed number of duty ticks
+	 * per (1 << PWM_DUTY_WIDTH) cycles and make sure to round to the
+	 * nearest integer during division.
+	 */
+	c = duty_ns * ((1 << PWM_DUTY_WIDTH) - 1) + period_ns / 2;
+	do_div(c, period_ns);
+
+	val = (u32)c << PWM_DUTY_SHIFT;
+
+	/*
+	 * Compute the prescaler value for which (1 << PWM_DUTY_WIDTH)
+	 * cycles at the PWM clock rate will take period_ns nanoseconds.
+	 */
+	rate = clk_get_rate(pc->clk) >> PWM_DUTY_WIDTH;
+	hz = 1000000000ul / period_ns;
+
+	rate = (rate + (hz / 2)) / hz;
+
+	/*
+	 * Since the actual PWM divider is the register's frequency divider
+	 * field minus 1, we need to decrement to get the correct value to
+	 * write to the register.
+	 */
+	if (rate > 0)
+		rate--;
+
+	/*
+	 * Make sure that the rate will fit in the register's frequency
+	 * divider field.
+	 */
+	if (rate >> PWM_SCALE_WIDTH)
+		return -EINVAL;
+
+	val |= rate << PWM_SCALE_SHIFT;
+
+	/*
+	 * If the PWM channel is disabled, make sure to turn on the clock
+	 * before writing the register. Otherwise, keep it enabled.
+	 */
+	if (!test_bit(PWMF_ENABLED, &pwm->flags)) {
+		err = clk_prepare_enable(pc->clk);
+		if (err < 0)
+			return err;
+	} else
+		val |= PWM_ENABLE;
+
+	pwm_writel(pc, pwm->hwpwm, val);
+
+	/*
+	 * If the PWM is not enabled, turn the clock off again to save power.
+	 */
+	if (!test_bit(PWMF_ENABLED, &pwm->flags))
+		clk_disable_unprepare(pc->clk);
+
+	return 0;
+}
+
+static int tegra_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct tegra_pwm_chip *pc = to_tegra_pwm_chip(chip);
+	int rc = 0;
+	u32 val;
+
+	rc = clk_prepare_enable(pc->clk);
+	if (rc < 0)
+		return rc;
+
+	val = pwm_readl(pc, pwm->hwpwm);
+	val |= PWM_ENABLE;
+	pwm_writel(pc, pwm->hwpwm, val);
+
+	return 0;
+}
+
+static void tegra_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct tegra_pwm_chip *pc = to_tegra_pwm_chip(chip);
+	u32 val;
+
+	val = pwm_readl(pc, pwm->hwpwm);
+	val &= ~PWM_ENABLE;
+	pwm_writel(pc, pwm->hwpwm, val);
+
+	clk_disable_unprepare(pc->clk);
+}
+
+static const struct pwm_ops tegra_pwm_ops = {
+	.config = tegra_pwm_config,
+	.enable = tegra_pwm_enable,
+	.disable = tegra_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int tegra_pwm_probe(struct platform_device *pdev)
+{
+	struct tegra_pwm_chip *pwm;
+	struct resource *r;
+	int ret;
+
+	pwm = devm_kzalloc(&pdev->dev, sizeof(*pwm), GFP_KERNEL);
+	if (!pwm) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	pwm->dev = &pdev->dev;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "no memory resources defined\n");
+		return -ENODEV;
+	}
+
+	pwm->mmio_base = devm_request_and_ioremap(&pdev->dev, r);
+	if (!pwm->mmio_base) {
+		dev_err(&pdev->dev, "failed to ioremap() region\n");
+		return -EADDRNOTAVAIL;
+	}
+
+	platform_set_drvdata(pdev, pwm);
+
+	pwm->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pwm->clk))
+		return PTR_ERR(pwm->clk);
+
+	pwm->chip.dev = &pdev->dev;
+	pwm->chip.ops = &tegra_pwm_ops;
+	pwm->chip.base = -1;
+	pwm->chip.npwm = NUM_PWM;
+
+	ret = pwmchip_add(&pwm->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __devexit tegra_pwm_remove(struct platform_device *pdev)
+{
+	struct tegra_pwm_chip *pc = platform_get_drvdata(pdev);
+	int i;
+
+	if (WARN_ON(!pc))
+		return -ENODEV;
+
+	for (i = 0; i < NUM_PWM; i++) {
+		struct pwm_device *pwm = &pc->chip.pwms[i];
+
+		if (!test_bit(PWMF_ENABLED, &pwm->flags))
+			if (clk_prepare_enable(pc->clk) < 0)
+				continue;
+
+		pwm_writel(pc, i, 0);
+
+		clk_disable_unprepare(pc->clk);
+	}
+
+	return pwmchip_remove(&pc->chip);
+}
+
+#ifdef CONFIG_OF
+static struct of_device_id tegra_pwm_of_match[] = {
+	{ .compatible = "nvidia,tegra20-pwm" },
+	{ .compatible = "nvidia,tegra30-pwm" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, tegra_pwm_of_match);
+#endif
+
+static struct platform_driver tegra_pwm_driver = {
+	.driver = {
+		.name = "tegra-pwm",
+		.of_match_table = of_match_ptr(tegra_pwm_of_match),
+	},
+	.probe = tegra_pwm_probe,
+	.remove = __devexit_p(tegra_pwm_remove),
+};
+
+module_platform_driver(tegra_pwm_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_ALIAS("platform:tegra-pwm");
diff --git a/drivers/pwm/pwm-tiecap.c b/drivers/pwm/pwm-tiecap.c
new file mode 100644
index 00000000000..3c2ad284ee3
--- /dev/null
+++ b/drivers/pwm/pwm-tiecap.c
@@ -0,0 +1,232 @@
+/*
+ * ECAP PWM driver
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc. - 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You 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/platform_device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/pwm.h>
+
+/* ECAP registers and bits definitions */
+#define CAP1			0x08
+#define CAP2			0x0C
+#define CAP3			0x10
+#define CAP4			0x14
+#define ECCTL2			0x2A
+#define ECCTL2_APWM_MODE	BIT(9)
+#define ECCTL2_SYNC_SEL_DISA	(BIT(7) | BIT(6))
+#define ECCTL2_TSCTR_FREERUN	BIT(4)
+
+struct ecap_pwm_chip {
+	struct pwm_chip	chip;
+	unsigned int	clk_rate;
+	void __iomem	*mmio_base;
+};
+
+static inline struct ecap_pwm_chip *to_ecap_pwm_chip(struct pwm_chip *chip)
+{
+	return container_of(chip, struct ecap_pwm_chip, chip);
+}
+
+/*
+ * period_ns = 10^9 * period_cycles / PWM_CLK_RATE
+ * duty_ns   = 10^9 * duty_cycles / PWM_CLK_RATE
+ */
+static int ecap_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+		int duty_ns, int period_ns)
+{
+	struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
+	unsigned long long c;
+	unsigned long period_cycles, duty_cycles;
+	unsigned int reg_val;
+
+	if (period_ns < 0 || duty_ns < 0 || period_ns > NSEC_PER_SEC)
+		return -ERANGE;
+
+	c = pc->clk_rate;
+	c = c * period_ns;
+	do_div(c, NSEC_PER_SEC);
+	period_cycles = (unsigned long)c;
+
+	if (period_cycles < 1) {
+		period_cycles = 1;
+		duty_cycles = 1;
+	} else {
+		c = pc->clk_rate;
+		c = c * duty_ns;
+		do_div(c, NSEC_PER_SEC);
+		duty_cycles = (unsigned long)c;
+	}
+
+	pm_runtime_get_sync(pc->chip.dev);
+
+	reg_val = readw(pc->mmio_base + ECCTL2);
+
+	/* Configure APWM mode & disable sync option */
+	reg_val |= ECCTL2_APWM_MODE | ECCTL2_SYNC_SEL_DISA;
+
+	writew(reg_val, pc->mmio_base + ECCTL2);
+
+	if (!test_bit(PWMF_ENABLED, &pwm->flags)) {
+		/* Update active registers if not running */
+		writel(duty_cycles, pc->mmio_base + CAP2);
+		writel(period_cycles, pc->mmio_base + CAP1);
+	} else {
+		/*
+		 * Update shadow registers to configure period and
+		 * compare values. This helps current PWM period to
+		 * complete on reconfiguring
+		 */
+		writel(duty_cycles, pc->mmio_base + CAP4);
+		writel(period_cycles, pc->mmio_base + CAP3);
+	}
+
+	pm_runtime_put_sync(pc->chip.dev);
+	return 0;
+}
+
+static int ecap_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
+	unsigned int reg_val;
+
+	/* Leave clock enabled on enabling PWM */
+	pm_runtime_get_sync(pc->chip.dev);
+
+	/*
+	 * Enable 'Free run Time stamp counter mode' to start counter
+	 * and  'APWM mode' to enable APWM output
+	 */
+	reg_val = readw(pc->mmio_base + ECCTL2);
+	reg_val |= ECCTL2_TSCTR_FREERUN | ECCTL2_APWM_MODE;
+	writew(reg_val, pc->mmio_base + ECCTL2);
+	return 0;
+}
+
+static void ecap_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
+	unsigned int reg_val;
+
+	/*
+	 * Disable 'Free run Time stamp counter mode' to stop counter
+	 * and 'APWM mode' to put APWM output to low
+	 */
+	reg_val = readw(pc->mmio_base + ECCTL2);
+	reg_val &= ~(ECCTL2_TSCTR_FREERUN | ECCTL2_APWM_MODE);
+	writew(reg_val, pc->mmio_base + ECCTL2);
+
+	/* Disable clock on PWM disable */
+	pm_runtime_put_sync(pc->chip.dev);
+}
+
+static void ecap_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	if (test_bit(PWMF_ENABLED, &pwm->flags)) {
+		dev_warn(chip->dev, "Removing PWM device without disabling\n");
+		pm_runtime_put_sync(chip->dev);
+	}
+}
+
+static const struct pwm_ops ecap_pwm_ops = {
+	.free		= ecap_pwm_free,
+	.config		= ecap_pwm_config,
+	.enable		= ecap_pwm_enable,
+	.disable	= ecap_pwm_disable,
+	.owner		= THIS_MODULE,
+};
+
+static int __devinit ecap_pwm_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct resource *r;
+	struct clk *clk;
+	struct ecap_pwm_chip *pc;
+
+	pc = devm_kzalloc(&pdev->dev, sizeof(*pc), GFP_KERNEL);
+	if (!pc) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	clk = devm_clk_get(&pdev->dev, "fck");
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "failed to get clock\n");
+		return PTR_ERR(clk);
+	}
+
+	pc->clk_rate = clk_get_rate(clk);
+	if (!pc->clk_rate) {
+		dev_err(&pdev->dev, "failed to get clock rate\n");
+		return -EINVAL;
+	}
+
+	pc->chip.dev = &pdev->dev;
+	pc->chip.ops = &ecap_pwm_ops;
+	pc->chip.base = -1;
+	pc->chip.npwm = 1;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "no memory resource defined\n");
+		return -ENODEV;
+	}
+
+	pc->mmio_base = devm_request_and_ioremap(&pdev->dev, r);
+	if (!pc->mmio_base) {
+		dev_err(&pdev->dev, "failed to ioremap() registers\n");
+		return -EADDRNOTAVAIL;
+	}
+
+	ret = pwmchip_add(&pc->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+		return ret;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	platform_set_drvdata(pdev, pc);
+	return 0;
+}
+
+static int __devexit ecap_pwm_remove(struct platform_device *pdev)
+{
+	struct ecap_pwm_chip *pc = platform_get_drvdata(pdev);
+
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return pwmchip_remove(&pc->chip);
+}
+
+static struct platform_driver ecap_pwm_driver = {
+	.driver = {
+		.name = "ecap",
+	},
+	.probe = ecap_pwm_probe,
+	.remove = __devexit_p(ecap_pwm_remove),
+};
+
+module_platform_driver(ecap_pwm_driver);
+
+MODULE_DESCRIPTION("ECAP PWM driver");
+MODULE_AUTHOR("Texas Instruments");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pwm/pwm-tiehrpwm.c b/drivers/pwm/pwm-tiehrpwm.c
new file mode 100644
index 00000000000..010d232cb0c
--- /dev/null
+++ b/drivers/pwm/pwm-tiehrpwm.c
@@ -0,0 +1,411 @@
+/*
+ * EHRPWM PWM driver
+ *
+ * Copyright (C) 2012 Texas Instruments, Inc. - 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You 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/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+
+/* EHRPWM registers and bits definitions */
+
+/* Time base module registers */
+#define TBCTL			0x00
+#define TBPRD			0x0A
+
+#define TBCTL_RUN_MASK		(BIT(15) | BIT(14))
+#define TBCTL_STOP_NEXT		0
+#define TBCTL_STOP_ON_CYCLE	BIT(14)
+#define TBCTL_FREE_RUN		(BIT(15) | BIT(14))
+#define TBCTL_PRDLD_MASK	BIT(3)
+#define TBCTL_PRDLD_SHDW	0
+#define TBCTL_PRDLD_IMDT	BIT(3)
+#define TBCTL_CLKDIV_MASK	(BIT(12) | BIT(11) | BIT(10) | BIT(9) | \
+				BIT(8) | BIT(7))
+#define TBCTL_CTRMODE_MASK	(BIT(1) | BIT(0))
+#define TBCTL_CTRMODE_UP	0
+#define TBCTL_CTRMODE_DOWN	BIT(0)
+#define TBCTL_CTRMODE_UPDOWN	BIT(1)
+#define TBCTL_CTRMODE_FREEZE	(BIT(1) | BIT(0))
+
+#define TBCTL_HSPCLKDIV_SHIFT	7
+#define TBCTL_CLKDIV_SHIFT	10
+
+#define CLKDIV_MAX		7
+#define HSPCLKDIV_MAX		7
+#define PERIOD_MAX		0xFFFF
+
+/* compare module registers */
+#define CMPA			0x12
+#define CMPB			0x14
+
+/* Action qualifier module registers */
+#define AQCTLA			0x16
+#define AQCTLB			0x18
+#define AQSFRC			0x1A
+#define AQCSFRC			0x1C
+
+#define AQCTL_CBU_MASK		(BIT(9) | BIT(8))
+#define AQCTL_CBU_FRCLOW	BIT(8)
+#define AQCTL_CBU_FRCHIGH	BIT(9)
+#define AQCTL_CBU_FRCTOGGLE	(BIT(9) | BIT(8))
+#define AQCTL_CAU_MASK		(BIT(5) | BIT(4))
+#define AQCTL_CAU_FRCLOW	BIT(4)
+#define AQCTL_CAU_FRCHIGH	BIT(5)
+#define AQCTL_CAU_FRCTOGGLE	(BIT(5) | BIT(4))
+#define AQCTL_PRD_MASK		(BIT(3) | BIT(2))
+#define AQCTL_PRD_FRCLOW	BIT(2)
+#define AQCTL_PRD_FRCHIGH	BIT(3)
+#define AQCTL_PRD_FRCTOGGLE	(BIT(3) | BIT(2))
+#define AQCTL_ZRO_MASK		(BIT(1) | BIT(0))
+#define AQCTL_ZRO_FRCLOW	BIT(0)
+#define AQCTL_ZRO_FRCHIGH	BIT(1)
+#define AQCTL_ZRO_FRCTOGGLE	(BIT(1) | BIT(0))
+
+#define AQSFRC_RLDCSF_MASK	(BIT(7) | BIT(6))
+#define AQSFRC_RLDCSF_ZRO	0
+#define AQSFRC_RLDCSF_PRD	BIT(6)
+#define AQSFRC_RLDCSF_ZROPRD	BIT(7)
+#define AQSFRC_RLDCSF_IMDT	(BIT(7) | BIT(6))
+
+#define AQCSFRC_CSFB_MASK	(BIT(3) | BIT(2))
+#define AQCSFRC_CSFB_FRCDIS	0
+#define AQCSFRC_CSFB_FRCLOW	BIT(2)
+#define AQCSFRC_CSFB_FRCHIGH	BIT(3)
+#define AQCSFRC_CSFB_DISSWFRC	(BIT(3) | BIT(2))
+#define AQCSFRC_CSFA_MASK	(BIT(1) | BIT(0))
+#define AQCSFRC_CSFA_FRCDIS	0
+#define AQCSFRC_CSFA_FRCLOW	BIT(0)
+#define AQCSFRC_CSFA_FRCHIGH	BIT(1)
+#define AQCSFRC_CSFA_DISSWFRC	(BIT(1) | BIT(0))
+
+#define NUM_PWM_CHANNEL		2	/* EHRPWM channels */
+
+struct ehrpwm_pwm_chip {
+	struct pwm_chip	chip;
+	unsigned int	clk_rate;
+	void __iomem	*mmio_base;
+};
+
+static inline struct ehrpwm_pwm_chip *to_ehrpwm_pwm_chip(struct pwm_chip *chip)
+{
+	return container_of(chip, struct ehrpwm_pwm_chip, chip);
+}
+
+static void ehrpwm_write(void *base, int offset, unsigned int val)
+{
+	writew(val & 0xFFFF, base + offset);
+}
+
+static void ehrpwm_modify(void *base, int offset,
+		unsigned short mask, unsigned short val)
+{
+	unsigned short regval;
+
+	regval = readw(base + offset);
+	regval &= ~mask;
+	regval |= val & mask;
+	writew(regval, base + offset);
+}
+
+/**
+ * set_prescale_div -	Set up the prescaler divider function
+ * @rqst_prescaler:	prescaler value min
+ * @prescale_div:	prescaler value set
+ * @tb_clk_div:		Time Base Control prescaler bits
+ */
+static int set_prescale_div(unsigned long rqst_prescaler,
+		unsigned short *prescale_div, unsigned short *tb_clk_div)
+{
+	unsigned int clkdiv, hspclkdiv;
+
+	for (clkdiv = 0; clkdiv <= CLKDIV_MAX; clkdiv++) {
+		for (hspclkdiv = 0; hspclkdiv <= HSPCLKDIV_MAX; hspclkdiv++) {
+
+			/*
+			 * calculations for prescaler value :
+			 * prescale_div = HSPCLKDIVIDER * CLKDIVIDER.
+			 * HSPCLKDIVIDER =  2 ** hspclkdiv
+			 * CLKDIVIDER = (1),		if clkdiv == 0 *OR*
+			 *		(2 * clkdiv),	if clkdiv != 0
+			 *
+			 * Configure prescale_div value such that period
+			 * register value is less than 65535.
+			 */
+
+			*prescale_div = (1 << clkdiv) *
+					(hspclkdiv ? (hspclkdiv * 2) : 1);
+			if (*prescale_div > rqst_prescaler) {
+				*tb_clk_div = (clkdiv << TBCTL_CLKDIV_SHIFT) |
+					(hspclkdiv << TBCTL_HSPCLKDIV_SHIFT);
+				return 0;
+			}
+		}
+	}
+	return 1;
+}
+
+static void configure_chans(struct ehrpwm_pwm_chip *pc, int chan,
+		unsigned long duty_cycles)
+{
+	int cmp_reg, aqctl_reg;
+	unsigned short aqctl_val, aqctl_mask;
+
+	/*
+	 * Channels can be configured from action qualifier module.
+	 * Channel 0 configured with compare A register and for
+	 * up-counter mode.
+	 * Channel 1 configured with compare B register and for
+	 * up-counter mode.
+	 */
+	if (chan == 1) {
+		aqctl_reg = AQCTLB;
+		cmp_reg = CMPB;
+		/* Configure PWM Low from compare B value */
+		aqctl_val = AQCTL_CBU_FRCLOW;
+		aqctl_mask = AQCTL_CBU_MASK;
+	} else {
+		cmp_reg = CMPA;
+		aqctl_reg = AQCTLA;
+		/* Configure PWM Low from compare A value*/
+		aqctl_val = AQCTL_CAU_FRCLOW;
+		aqctl_mask = AQCTL_CAU_MASK;
+	}
+
+	/* Configure PWM High from period value and zero value */
+	aqctl_val |= AQCTL_PRD_FRCHIGH | AQCTL_ZRO_FRCHIGH;
+	aqctl_mask |= AQCTL_PRD_MASK | AQCTL_ZRO_MASK;
+	ehrpwm_modify(pc->mmio_base,  aqctl_reg, aqctl_mask, aqctl_val);
+
+	ehrpwm_write(pc->mmio_base,  cmp_reg, duty_cycles);
+}
+
+/*
+ * period_ns = 10^9 * (ps_divval * period_cycles) / PWM_CLK_RATE
+ * duty_ns   = 10^9 * (ps_divval * duty_cycles) / PWM_CLK_RATE
+ */
+static int ehrpwm_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+		int duty_ns, int period_ns)
+{
+	struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
+	unsigned long long c;
+	unsigned long period_cycles, duty_cycles;
+	unsigned short ps_divval, tb_divval;
+
+	if (period_ns < 0 || duty_ns < 0 || period_ns > NSEC_PER_SEC)
+		return -ERANGE;
+
+	c = pc->clk_rate;
+	c = c * period_ns;
+	do_div(c, NSEC_PER_SEC);
+	period_cycles = (unsigned long)c;
+
+	if (period_cycles < 1) {
+		period_cycles = 1;
+		duty_cycles = 1;
+	} else {
+		c = pc->clk_rate;
+		c = c * duty_ns;
+		do_div(c, NSEC_PER_SEC);
+		duty_cycles = (unsigned long)c;
+	}
+
+	/* Configure clock prescaler to support Low frequency PWM wave */
+	if (set_prescale_div(period_cycles/PERIOD_MAX, &ps_divval,
+				&tb_divval)) {
+		dev_err(chip->dev, "Unsupported values\n");
+		return -EINVAL;
+	}
+
+	pm_runtime_get_sync(chip->dev);
+
+	/* Update clock prescaler values */
+	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_CLKDIV_MASK, tb_divval);
+
+	/* Update period & duty cycle with presacler division */
+	period_cycles = period_cycles / ps_divval;
+	duty_cycles = duty_cycles / ps_divval;
+
+	/* Configure shadow loading on Period register */
+	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_PRDLD_MASK, TBCTL_PRDLD_SHDW);
+
+	ehrpwm_write(pc->mmio_base, TBPRD, period_cycles);
+
+	/* Configure ehrpwm counter for up-count mode */
+	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_CTRMODE_MASK,
+			TBCTL_CTRMODE_UP);
+
+	/* Configure the channel for duty cycle */
+	configure_chans(pc, pwm->hwpwm, duty_cycles);
+	pm_runtime_put_sync(chip->dev);
+	return 0;
+}
+
+static int ehrpwm_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
+	unsigned short aqcsfrc_val, aqcsfrc_mask;
+
+	/* Leave clock enabled on enabling PWM */
+	pm_runtime_get_sync(chip->dev);
+
+	/* Disabling Action Qualifier on PWM output */
+	if (pwm->hwpwm) {
+		aqcsfrc_val = AQCSFRC_CSFB_FRCDIS;
+		aqcsfrc_mask = AQCSFRC_CSFB_MASK;
+	} else {
+		aqcsfrc_val = AQCSFRC_CSFA_FRCDIS;
+		aqcsfrc_mask = AQCSFRC_CSFA_MASK;
+	}
+
+	/* Changes to shadow mode */
+	ehrpwm_modify(pc->mmio_base, AQSFRC, AQSFRC_RLDCSF_MASK,
+			AQSFRC_RLDCSF_ZRO);
+
+	ehrpwm_modify(pc->mmio_base, AQCSFRC, aqcsfrc_mask, aqcsfrc_val);
+
+	/* Enable time counter for free_run */
+	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_RUN_MASK, TBCTL_FREE_RUN);
+	return 0;
+}
+
+static void ehrpwm_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
+	unsigned short aqcsfrc_val, aqcsfrc_mask;
+
+	/* Action Qualifier puts PWM output low forcefully */
+	if (pwm->hwpwm) {
+		aqcsfrc_val = AQCSFRC_CSFB_FRCLOW;
+		aqcsfrc_mask = AQCSFRC_CSFB_MASK;
+	} else {
+		aqcsfrc_val = AQCSFRC_CSFA_FRCLOW;
+		aqcsfrc_mask = AQCSFRC_CSFA_MASK;
+	}
+
+	/*
+	 * Changes to immediate action on Action Qualifier. This puts
+	 * Action Qualifier control on PWM output from next TBCLK
+	 */
+	ehrpwm_modify(pc->mmio_base, AQSFRC, AQSFRC_RLDCSF_MASK,
+			AQSFRC_RLDCSF_IMDT);
+
+	ehrpwm_modify(pc->mmio_base, AQCSFRC, aqcsfrc_mask, aqcsfrc_val);
+
+	/* Stop Time base counter */
+	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_RUN_MASK, TBCTL_STOP_NEXT);
+
+	/* Disable clock on PWM disable */
+	pm_runtime_put_sync(chip->dev);
+}
+
+static void ehrpwm_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	if (test_bit(PWMF_ENABLED, &pwm->flags)) {
+		dev_warn(chip->dev, "Removing PWM device without disabling\n");
+		pm_runtime_put_sync(chip->dev);
+	}
+}
+
+static const struct pwm_ops ehrpwm_pwm_ops = {
+	.free		= ehrpwm_pwm_free,
+	.config		= ehrpwm_pwm_config,
+	.enable		= ehrpwm_pwm_enable,
+	.disable	= ehrpwm_pwm_disable,
+	.owner		= THIS_MODULE,
+};
+
+static int __devinit ehrpwm_pwm_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct resource *r;
+	struct clk *clk;
+	struct ehrpwm_pwm_chip *pc;
+
+	pc = devm_kzalloc(&pdev->dev, sizeof(*pc), GFP_KERNEL);
+	if (!pc) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	clk = devm_clk_get(&pdev->dev, "fck");
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "failed to get clock\n");
+		return PTR_ERR(clk);
+	}
+
+	pc->clk_rate = clk_get_rate(clk);
+	if (!pc->clk_rate) {
+		dev_err(&pdev->dev, "failed to get clock rate\n");
+		return -EINVAL;
+	}
+
+	pc->chip.dev = &pdev->dev;
+	pc->chip.ops = &ehrpwm_pwm_ops;
+	pc->chip.base = -1;
+	pc->chip.npwm = NUM_PWM_CHANNEL;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "no memory resource defined\n");
+		return -ENODEV;
+	}
+
+	pc->mmio_base = devm_request_and_ioremap(&pdev->dev, r);
+	if (!pc->mmio_base) {
+		dev_err(&pdev->dev, "failed to ioremap() registers\n");
+		return  -EADDRNOTAVAIL;
+	}
+
+	ret = pwmchip_add(&pc->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+		return ret;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	platform_set_drvdata(pdev, pc);
+	return 0;
+}
+
+static int __devexit ehrpwm_pwm_remove(struct platform_device *pdev)
+{
+	struct ehrpwm_pwm_chip *pc = platform_get_drvdata(pdev);
+
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return pwmchip_remove(&pc->chip);
+}
+
+static struct platform_driver ehrpwm_pwm_driver = {
+	.driver = {
+		.name = "ehrpwm",
+	},
+	.probe = ehrpwm_pwm_probe,
+	.remove = __devexit_p(ehrpwm_pwm_remove),
+};
+
+module_platform_driver(ehrpwm_pwm_driver);
+
+MODULE_DESCRIPTION("EHRPWM PWM driver");
+MODULE_AUTHOR("Texas Instruments");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pwm/pwm-vt8500.c b/drivers/pwm/pwm-vt8500.c
new file mode 100644
index 00000000000..548021439f0
--- /dev/null
+++ b/drivers/pwm/pwm-vt8500.c
@@ -0,0 +1,177 @@
+/*
+ * drivers/pwm/pwm-vt8500.c
+ *
+ *  Copyright (C) 2010 Alexey Charkov <alchark@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+#include <linux/delay.h>
+
+#include <asm/div64.h>
+
+#define VT8500_NR_PWMS 4
+
+struct vt8500_chip {
+	struct pwm_chip chip;
+	void __iomem *base;
+};
+
+#define to_vt8500_chip(chip)	container_of(chip, struct vt8500_chip, chip)
+
+#define msecs_to_loops(t) (loops_per_jiffy / 1000 * HZ * t)
+static inline void pwm_busy_wait(void __iomem *reg, u8 bitmask)
+{
+	int loops = msecs_to_loops(10);
+	while ((readb(reg) & bitmask) && --loops)
+		cpu_relax();
+
+	if (unlikely(!loops))
+		pr_warning("Waiting for status bits 0x%x to clear timed out\n",
+			   bitmask);
+}
+
+static int vt8500_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+		int duty_ns, int period_ns)
+{
+	struct vt8500_chip *vt8500 = to_vt8500_chip(chip);
+	unsigned long long c;
+	unsigned long period_cycles, prescale, pv, dc;
+
+	c = 25000000/2; /* wild guess --- need to implement clocks */
+	c = c * period_ns;
+	do_div(c, 1000000000);
+	period_cycles = c;
+
+	if (period_cycles < 1)
+		period_cycles = 1;
+	prescale = (period_cycles - 1) / 4096;
+	pv = period_cycles / (prescale + 1) - 1;
+	if (pv > 4095)
+		pv = 4095;
+
+	if (prescale > 1023)
+		return -EINVAL;
+
+	c = (unsigned long long)pv * duty_ns;
+	do_div(c, period_ns);
+	dc = c;
+
+	pwm_busy_wait(vt8500->base + 0x40 + pwm->hwpwm, (1 << 1));
+	writel(prescale, vt8500->base + 0x4 + (pwm->hwpwm << 4));
+
+	pwm_busy_wait(vt8500->base + 0x40 + pwm->hwpwm, (1 << 2));
+	writel(pv, vt8500->base + 0x8 + (pwm->hwpwm << 4));
+
+	pwm_busy_wait(vt8500->base + 0x40 + pwm->hwpwm, (1 << 3));
+	writel(dc, vt8500->base + 0xc + (pwm->hwpwm << 4));
+
+	return 0;
+}
+
+static int vt8500_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct vt8500_chip *vt8500 = to_vt8500_chip(chip);
+
+	pwm_busy_wait(vt8500->base + 0x40 + pwm->hwpwm, (1 << 0));
+	writel(5, vt8500->base + (pwm->hwpwm << 4));
+	return 0;
+}
+
+static void vt8500_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct vt8500_chip *vt8500 = to_vt8500_chip(chip);
+
+	pwm_busy_wait(vt8500->base + 0x40 + pwm->hwpwm, (1 << 0));
+	writel(0, vt8500->base + (pwm->hwpwm << 4));
+}
+
+static struct pwm_ops vt8500_pwm_ops = {
+	.enable = vt8500_pwm_enable,
+	.disable = vt8500_pwm_disable,
+	.config = vt8500_pwm_config,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit pwm_probe(struct platform_device *pdev)
+{
+	struct vt8500_chip *chip;
+	struct resource *r;
+	int ret;
+
+	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
+	if (chip == NULL) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	chip->chip.dev = &pdev->dev;
+	chip->chip.ops = &vt8500_pwm_ops;
+	chip->chip.base = -1;
+	chip->chip.npwm = VT8500_NR_PWMS;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no memory resource defined\n");
+		return -ENODEV;
+	}
+
+	chip->base = devm_request_and_ioremap(&pdev->dev, r);
+	if (chip->base == NULL)
+		return -EADDRNOTAVAIL;
+
+	ret = pwmchip_add(&chip->chip);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, chip);
+	return ret;
+}
+
+static int __devexit pwm_remove(struct platform_device *pdev)
+{
+	struct vt8500_chip *chip;
+
+	chip = platform_get_drvdata(pdev);
+	if (chip == NULL)
+		return -ENODEV;
+
+	return pwmchip_remove(&chip->chip);
+}
+
+static struct platform_driver pwm_driver = {
+	.driver		= {
+		.name	= "vt8500-pwm",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= pwm_probe,
+	.remove		= __devexit_p(pwm_remove),
+};
+
+static int __init pwm_init(void)
+{
+	return platform_driver_register(&pwm_driver);
+}
+arch_initcall(pwm_init);
+
+static void __exit pwm_exit(void)
+{
+	platform_driver_unregister(&pwm_driver);
+}
+module_exit(pwm_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index c86b8864e41..4e932cc695e 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -20,6 +20,7 @@ menuconfig REGULATOR
 
 	  If unsure, say no.
 
+
 if REGULATOR
 
 config REGULATOR_DEBUG
@@ -88,6 +89,13 @@ config REGULATOR_AAT2870
 	  If you have a AnalogicTech AAT2870 say Y to enable the
 	  regulator driver.
 
+config REGULATOR_ARIZONA
+	tristate "Wolfson Arizona class devices"
+	depends on MFD_ARIZONA
+	help
+	  Support for the regulators found on Wolfson Arizona class
+	  devices.
+
 config REGULATOR_DA903X
 	tristate "Dialog Semiconductor DA9030/DA9034 regulators"
 	depends on PMIC_DA903X
@@ -195,6 +203,14 @@ config REGULATOR_MAX8998
 	  via I2C bus. The provided regulator is suitable for S3C6410
 	  and S5PC1XX chips to control VCC_CORE and VCC_USIM voltages.
 
+config REGULATOR_MAX77686
+	tristate "Maxim 77686 regulator"
+	depends on MFD_MAX77686
+	help
+	  This driver controls a Maxim 77686 regulator
+	  via I2C bus. The provided regulator is suitable for
+	  Exynos-4 chips to control VARM and VINT voltages.
+
 config REGULATOR_PCAP
 	tristate "Motorola PCAP2 regulator driver"
 	depends on EZX_PCAP
@@ -216,6 +232,19 @@ config REGULATOR_LP3972
 	 Say Y here to support the voltage regulators and convertors
 	 on National Semiconductors LP3972 PMIC
 
+config REGULATOR_LP872X
+	bool "TI/National Semiconductor LP8720/LP8725 voltage regulators"
+	depends on I2C=y
+	select REGMAP_I2C
+	help
+	  This driver supports LP8720/LP8725 PMIC
+
+config REGULATOR_LP8788
+	bool "TI LP8788 Power Regulators"
+	depends on MFD_LP8788
+	help
+	  This driver supports LP8788 voltage regulator chip.
+
 config REGULATOR_PCF50633
 	tristate "NXP PCF50633 regulator driver"
         depends on MFD_PCF50633
@@ -233,9 +262,17 @@ config REGULATOR_RC5T583
 	  through regulator interface. The device supports multiple DCDC/LDO
 	  outputs which can be controlled by i2c communication.
 
+config REGULATOR_S2MPS11
+	tristate "Samsung S2MPS11 voltage regulator"
+	depends on MFD_SEC_CORE
+	help
+	 This driver supports a Samsung S2MPS11 voltage output regulator
+	 via I2C bus. S2MPS11 is comprised of high efficient Buck converters
+	 including Dual-Phase Buck converter, Buck-Boost converter, various LDOs.
+
 config REGULATOR_S5M8767
 	tristate "Samsung S5M8767A voltage regulator"
-	depends on MFD_S5M_CORE
+	depends on MFD_SEC_CORE
 	help
 	 This driver supports a Samsung S5M8767A voltage output regulator
 	 via I2C bus. S5M8767A have 9 Bucks and 28 LDOs output and
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 977fd46909a..3342615cf25 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_REGULATOR_AB3100) += ab3100.o
 obj-$(CONFIG_REGULATOR_AB8500)	+= ab8500.o
 obj-$(CONFIG_REGULATOR_AD5398) += ad5398.o
 obj-$(CONFIG_REGULATOR_ANATOP) += anatop-regulator.o
+obj-$(CONFIG_REGULATOR_ARIZONA) += arizona-micsupp.o arizona-ldo1.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
 obj-$(CONFIG_REGULATOR_DBX500_PRCMU) += dbx500-prcmu.o
@@ -23,6 +24,9 @@ obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
 obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
 obj-$(CONFIG_REGULATOR_LP3972) += lp3972.o
+obj-$(CONFIG_REGULATOR_LP872X) += lp872x.o
+obj-$(CONFIG_REGULATOR_LP8788) += lp8788-buck.o
+obj-$(CONFIG_REGULATOR_LP8788) += lp8788-ldo.o
 obj-$(CONFIG_REGULATOR_MAX1586) += max1586.o
 obj-$(CONFIG_REGULATOR_MAX8649)	+= max8649.o
 obj-$(CONFIG_REGULATOR_MAX8660) += max8660.o
@@ -30,6 +34,7 @@ obj-$(CONFIG_REGULATOR_MAX8925) += max8925-regulator.o
 obj-$(CONFIG_REGULATOR_MAX8952) += max8952.o
 obj-$(CONFIG_REGULATOR_MAX8997) += max8997.o
 obj-$(CONFIG_REGULATOR_MAX8998) += max8998.o
+obj-$(CONFIG_REGULATOR_MAX77686) += max77686.o
 obj-$(CONFIG_REGULATOR_MC13783) += mc13783-regulator.o
 obj-$(CONFIG_REGULATOR_MC13892) += mc13892-regulator.o
 obj-$(CONFIG_REGULATOR_MC13XXX_CORE) +=  mc13xxx-regulator-core.o
@@ -37,6 +42,7 @@ obj-$(CONFIG_REGULATOR_PALMAS) += palmas-regulator.o
 obj-$(CONFIG_REGULATOR_PCAP) += pcap-regulator.o
 obj-$(CONFIG_REGULATOR_PCF50633) += pcf50633-regulator.o
 obj-$(CONFIG_REGULATOR_RC5T583)  += rc5t583-regulator.o
+obj-$(CONFIG_REGULATOR_S2MPS11) += s2mps11.o
 obj-$(CONFIG_REGULATOR_S5M8767) += s5m8767.o
 obj-$(CONFIG_REGULATOR_TPS6105X) += tps6105x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS62360) += tps62360-regulator.o
diff --git a/drivers/regulator/aat2870-regulator.c b/drivers/regulator/aat2870-regulator.c
index 06776ca945f..6f45bfd22e8 100644
--- a/drivers/regulator/aat2870-regulator.c
+++ b/drivers/regulator/aat2870-regulator.c
@@ -33,11 +33,6 @@ struct aat2870_regulator {
 	struct aat2870_data *aat2870;
 	struct regulator_desc desc;
 
-	const int *voltages; /* uV */
-
-	int min_uV;
-	int max_uV;
-
 	u8 enable_addr;
 	u8 enable_shift;
 	u8 enable_mask;
@@ -47,14 +42,6 @@ struct aat2870_regulator {
 	u8 voltage_mask;
 };
 
-static int aat2870_ldo_list_voltage(struct regulator_dev *rdev,
-				    unsigned selector)
-{
-	struct aat2870_regulator *ri = rdev_get_drvdata(rdev);
-
-	return ri->voltages[selector];
-}
-
 static int aat2870_ldo_set_voltage_sel(struct regulator_dev *rdev,
 				       unsigned selector)
 {
@@ -111,7 +98,7 @@ static int aat2870_ldo_is_enabled(struct regulator_dev *rdev)
 }
 
 static struct regulator_ops aat2870_ldo_ops = {
-	.list_voltage = aat2870_ldo_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.set_voltage_sel = aat2870_ldo_set_voltage_sel,
 	.get_voltage_sel = aat2870_ldo_get_voltage_sel,
 	.enable = aat2870_ldo_enable,
@@ -119,7 +106,7 @@ static struct regulator_ops aat2870_ldo_ops = {
 	.is_enabled = aat2870_ldo_is_enabled,
 };
 
-static const int aat2870_ldo_voltages[] = {
+static const unsigned int aat2870_ldo_voltages[] = {
 	1200000, 1300000, 1500000, 1600000,
 	1800000, 2000000, 2200000, 2500000,
 	2600000, 2700000, 2800000, 2900000,
@@ -132,13 +119,11 @@ static const int aat2870_ldo_voltages[] = {
 			.name = #ids,			\
 			.id = AAT2870_ID_##ids,		\
 			.n_voltages = ARRAY_SIZE(aat2870_ldo_voltages),	\
+			.volt_table = aat2870_ldo_voltages, \
 			.ops = &aat2870_ldo_ops,	\
 			.type = REGULATOR_VOLTAGE,	\
 			.owner = THIS_MODULE,		\
 		},					\
-		.voltages = aat2870_ldo_voltages,	\
-		.min_uV = 1200000,			\
-		.max_uV = 3300000,			\
 	}
 
 static struct aat2870_regulator aat2870_regulators[] = {
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index 03f4d9c604e..182b553059c 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -43,20 +43,12 @@
  * @dev: handle to the device
  * @plfdata: AB3100 platform data passed in at probe time
  * @regreg: regulator register number in the AB3100
- * @fixed_voltage: a fixed voltage for this regulator, if this
- *          0 the voltages array is used instead.
- * @typ_voltages: an array of available typical voltages for
- *          this regulator
- * @voltages_len: length of the array of available voltages
  */
 struct ab3100_regulator {
 	struct regulator_dev *rdev;
 	struct device *dev;
 	struct ab3100_platform_data *plfdata;
 	u8 regreg;
-	int fixed_voltage;
-	int const *typ_voltages;
-	u8 voltages_len;
 };
 
 /* The order in which registers are initialized */
@@ -80,7 +72,7 @@ static const u8 ab3100_reg_init_order[AB3100_NUM_REGULATORS+2] = {
 #define LDO_C_VOLTAGE 2650000
 #define LDO_D_VOLTAGE 2650000
 
-static const int ldo_e_buck_typ_voltages[] = {
+static const unsigned int ldo_e_buck_typ_voltages[] = {
 	1800000,
 	1400000,
 	1300000,
@@ -90,7 +82,7 @@ static const int ldo_e_buck_typ_voltages[] = {
 	900000,
 };
 
-static const int ldo_f_typ_voltages[] = {
+static const unsigned int ldo_f_typ_voltages[] = {
 	1800000,
 	1400000,
 	1300000,
@@ -101,21 +93,21 @@ static const int ldo_f_typ_voltages[] = {
 	2650000,
 };
 
-static const int ldo_g_typ_voltages[] = {
+static const unsigned int ldo_g_typ_voltages[] = {
 	2850000,
 	2750000,
 	1800000,
 	1500000,
 };
 
-static const int ldo_h_typ_voltages[] = {
+static const unsigned int ldo_h_typ_voltages[] = {
 	2750000,
 	1800000,
 	1500000,
 	1200000,
 };
 
-static const int ldo_k_typ_voltages[] = {
+static const unsigned int ldo_k_typ_voltages[] = {
 	2750000,
 	1800000,
 };
@@ -126,40 +118,27 @@ static struct ab3100_regulator
 ab3100_regulators[AB3100_NUM_REGULATORS] = {
 	{
 		.regreg = AB3100_LDO_A,
-		.fixed_voltage = LDO_A_VOLTAGE,
 	},
 	{
 		.regreg = AB3100_LDO_C,
-		.fixed_voltage = LDO_C_VOLTAGE,
 	},
 	{
 		.regreg = AB3100_LDO_D,
-		.fixed_voltage = LDO_D_VOLTAGE,
 	},
 	{
 		.regreg = AB3100_LDO_E,
-		.typ_voltages = ldo_e_buck_typ_voltages,
-		.voltages_len = ARRAY_SIZE(ldo_e_buck_typ_voltages),
 	},
 	{
 		.regreg = AB3100_LDO_F,
-		.typ_voltages = ldo_f_typ_voltages,
-		.voltages_len = ARRAY_SIZE(ldo_f_typ_voltages),
 	},
 	{
 		.regreg = AB3100_LDO_G,
-		.typ_voltages = ldo_g_typ_voltages,
-		.voltages_len = ARRAY_SIZE(ldo_g_typ_voltages),
 	},
 	{
 		.regreg = AB3100_LDO_H,
-		.typ_voltages = ldo_h_typ_voltages,
-		.voltages_len = ARRAY_SIZE(ldo_h_typ_voltages),
 	},
 	{
 		.regreg = AB3100_LDO_K,
-		.typ_voltages = ldo_k_typ_voltages,
-		.voltages_len = ARRAY_SIZE(ldo_k_typ_voltages),
 	},
 	{
 		.regreg = AB3100_LDO_EXT,
@@ -167,8 +146,6 @@ ab3100_regulators[AB3100_NUM_REGULATORS] = {
 	},
 	{
 		.regreg = AB3100_BUCK,
-		.typ_voltages = ldo_e_buck_typ_voltages,
-		.voltages_len = ARRAY_SIZE(ldo_e_buck_typ_voltages),
 	},
 };
 
@@ -178,7 +155,7 @@ ab3100_regulators[AB3100_NUM_REGULATORS] = {
  */
 static int ab3100_enable_regulator(struct regulator_dev *reg)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 	int err;
 	u8 regval;
 
@@ -209,7 +186,7 @@ static int ab3100_enable_regulator(struct regulator_dev *reg)
 
 static int ab3100_disable_regulator(struct regulator_dev *reg)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 	int err;
 	u8 regval;
 
@@ -242,7 +219,7 @@ static int ab3100_disable_regulator(struct regulator_dev *reg)
 
 static int ab3100_is_enabled_regulator(struct regulator_dev *reg)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 	u8 regval;
 	int err;
 
@@ -257,26 +234,12 @@ static int ab3100_is_enabled_regulator(struct regulator_dev *reg)
 	return regval & AB3100_REG_ON_MASK;
 }
 
-static int ab3100_list_voltage_regulator(struct regulator_dev *reg,
-					 unsigned selector)
-{
-	struct ab3100_regulator *abreg = reg->reg_data;
-
-	if (selector >= abreg->voltages_len)
-		return -EINVAL;
-	return abreg->typ_voltages[selector];
-}
-
 static int ab3100_get_voltage_regulator(struct regulator_dev *reg)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 	u8 regval;
 	int err;
 
-	/* Return the voltage for fixed regulators immediately */
-	if (abreg->fixed_voltage)
-		return abreg->fixed_voltage;
-
 	/*
 	 * For variable types, read out setting and index into
 	 * supplied voltage list.
@@ -294,20 +257,20 @@ static int ab3100_get_voltage_regulator(struct regulator_dev *reg)
 	regval &= 0xE0;
 	regval >>= 5;
 
-	if (regval >= abreg->voltages_len) {
+	if (regval >= reg->desc->n_voltages) {
 		dev_err(&reg->dev,
 			"regulator register %02x contains an illegal voltage setting\n",
 			abreg->regreg);
 		return -EINVAL;
 	}
 
-	return abreg->typ_voltages[regval];
+	return reg->desc->volt_table[regval];
 }
 
 static int ab3100_set_voltage_regulator_sel(struct regulator_dev *reg,
 					    unsigned selector)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 	u8 regval;
 	int err;
 
@@ -336,7 +299,7 @@ static int ab3100_set_voltage_regulator_sel(struct regulator_dev *reg,
 static int ab3100_set_suspend_voltage_regulator(struct regulator_dev *reg,
 						int uV)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 	u8 regval;
 	int err;
 	int bestindex;
@@ -379,42 +342,22 @@ static int ab3100_set_suspend_voltage_regulator(struct regulator_dev *reg,
  */
 static int ab3100_get_voltage_regulator_external(struct regulator_dev *reg)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
+	struct ab3100_regulator *abreg = rdev_get_drvdata(reg);
 
 	return abreg->plfdata->external_voltage;
 }
 
-static int ab3100_enable_time_regulator(struct regulator_dev *reg)
+static int ab3100_get_fixed_voltage_regulator(struct regulator_dev *reg)
 {
-	struct ab3100_regulator *abreg = reg->reg_data;
-
-	/* Per-regulator power on delay from spec */
-	switch (abreg->regreg) {
-	case AB3100_LDO_A: /* Fallthrough */
-	case AB3100_LDO_C: /* Fallthrough */
-	case AB3100_LDO_D: /* Fallthrough */
-	case AB3100_LDO_E: /* Fallthrough */
-	case AB3100_LDO_H: /* Fallthrough */
-	case AB3100_LDO_K:
-		return 200;
-	case AB3100_LDO_F:
-		return 600;
-	case AB3100_LDO_G:
-		return 400;
-	case AB3100_BUCK:
-		return 1000;
-	default:
-		break;
-	}
-	return 0;
+	return reg->desc->min_uV;
 }
 
 static struct regulator_ops regulator_ops_fixed = {
+	.list_voltage = regulator_list_voltage_linear,
 	.enable      = ab3100_enable_regulator,
 	.disable     = ab3100_disable_regulator,
 	.is_enabled  = ab3100_is_enabled_regulator,
-	.get_voltage = ab3100_get_voltage_regulator,
-	.enable_time = ab3100_enable_time_regulator,
+	.get_voltage = ab3100_get_fixed_voltage_regulator,
 };
 
 static struct regulator_ops regulator_ops_variable = {
@@ -423,8 +366,7 @@ static struct regulator_ops regulator_ops_variable = {
 	.is_enabled  = ab3100_is_enabled_regulator,
 	.get_voltage = ab3100_get_voltage_regulator,
 	.set_voltage_sel = ab3100_set_voltage_regulator_sel,
-	.list_voltage = ab3100_list_voltage_regulator,
-	.enable_time = ab3100_enable_time_regulator,
+	.list_voltage = regulator_list_voltage_table,
 };
 
 static struct regulator_ops regulator_ops_variable_sleepable = {
@@ -434,8 +376,7 @@ static struct regulator_ops regulator_ops_variable_sleepable = {
 	.get_voltage = ab3100_get_voltage_regulator,
 	.set_voltage_sel = ab3100_set_voltage_regulator_sel,
 	.set_suspend_voltage = ab3100_set_suspend_voltage_regulator,
-	.list_voltage = ab3100_list_voltage_regulator,
-	.enable_time = ab3100_enable_time_regulator,
+	.list_voltage = regulator_list_voltage_table,
 };
 
 /*
@@ -457,62 +398,81 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 		.name = "LDO_A",
 		.id   = AB3100_LDO_A,
 		.ops  = &regulator_ops_fixed,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.min_uV = LDO_A_VOLTAGE,
+		.enable_time = 200,
 	},
 	{
 		.name = "LDO_C",
 		.id   = AB3100_LDO_C,
 		.ops  = &regulator_ops_fixed,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.min_uV = LDO_C_VOLTAGE,
+		.enable_time = 200,
 	},
 	{
 		.name = "LDO_D",
 		.id   = AB3100_LDO_D,
 		.ops  = &regulator_ops_fixed,
+		.n_voltages = 1,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.min_uV = LDO_D_VOLTAGE,
+		.enable_time = 200,
 	},
 	{
 		.name = "LDO_E",
 		.id   = AB3100_LDO_E,
 		.ops  = &regulator_ops_variable_sleepable,
 		.n_voltages = ARRAY_SIZE(ldo_e_buck_typ_voltages),
+		.volt_table = ldo_e_buck_typ_voltages,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.enable_time = 200,
 	},
 	{
 		.name = "LDO_F",
 		.id   = AB3100_LDO_F,
 		.ops  = &regulator_ops_variable,
 		.n_voltages = ARRAY_SIZE(ldo_f_typ_voltages),
+		.volt_table = ldo_f_typ_voltages,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.enable_time = 600,
 	},
 	{
 		.name = "LDO_G",
 		.id   = AB3100_LDO_G,
 		.ops  = &regulator_ops_variable,
 		.n_voltages = ARRAY_SIZE(ldo_g_typ_voltages),
+		.volt_table = ldo_g_typ_voltages,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.enable_time = 400,
 	},
 	{
 		.name = "LDO_H",
 		.id   = AB3100_LDO_H,
 		.ops  = &regulator_ops_variable,
 		.n_voltages = ARRAY_SIZE(ldo_h_typ_voltages),
+		.volt_table = ldo_h_typ_voltages,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.enable_time = 200,
 	},
 	{
 		.name = "LDO_K",
 		.id   = AB3100_LDO_K,
 		.ops  = &regulator_ops_variable,
 		.n_voltages = ARRAY_SIZE(ldo_k_typ_voltages),
+		.volt_table = ldo_k_typ_voltages,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.enable_time = 200,
 	},
 	{
 		.name = "LDO_EXT",
@@ -528,6 +488,7 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 		.n_voltages = ARRAY_SIZE(ldo_e_buck_typ_voltages),
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
+		.enable_time = 1000,
 	},
 };
 
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index a739f5ca936..10f2f4d4d19 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -30,9 +30,6 @@
  * @dev: device pointer
  * @desc: regulator description
  * @regulator_dev: regulator device
- * @max_uV: maximum voltage (for variable voltage supplies)
- * @min_uV: minimum voltage (for variable voltage supplies)
- * @fixed_uV: typical voltage (for fixed voltage supplies)
  * @update_bank: bank to control on/off
  * @update_reg: register to control on/off
  * @update_mask: mask to enable/disable regulator
@@ -40,17 +37,12 @@
  * @voltage_bank: bank to control regulator voltage
  * @voltage_reg: register to control regulator voltage
  * @voltage_mask: mask to control regulator voltage
- * @voltages: supported voltage table
- * @voltages_len: number of supported voltages for the regulator
  * @delay: startup/set voltage delay in us
  */
 struct ab8500_regulator_info {
 	struct device		*dev;
 	struct regulator_desc	desc;
 	struct regulator_dev	*regulator;
-	int max_uV;
-	int min_uV;
-	int fixed_uV;
 	u8 update_bank;
 	u8 update_reg;
 	u8 update_mask;
@@ -58,13 +50,11 @@ struct ab8500_regulator_info {
 	u8 voltage_bank;
 	u8 voltage_reg;
 	u8 voltage_mask;
-	int const *voltages;
-	int voltages_len;
 	unsigned int delay;
 };
 
 /* voltage tables for the vauxn/vintcore supplies */
-static const int ldo_vauxn_voltages[] = {
+static const unsigned int ldo_vauxn_voltages[] = {
 	1100000,
 	1200000,
 	1300000,
@@ -83,7 +73,7 @@ static const int ldo_vauxn_voltages[] = {
 	3300000,
 };
 
-static const int ldo_vaux3_voltages[] = {
+static const unsigned int ldo_vaux3_voltages[] = {
 	1200000,
 	1500000,
 	1800000,
@@ -94,7 +84,7 @@ static const int ldo_vaux3_voltages[] = {
 	2910000,
 };
 
-static const int ldo_vintcore_voltages[] = {
+static const unsigned int ldo_vintcore_voltages[] = {
 	1200000,
 	1225000,
 	1250000,
@@ -185,25 +175,6 @@ static int ab8500_regulator_is_enabled(struct regulator_dev *rdev)
 		return false;
 }
 
-static int ab8500_list_voltage(struct regulator_dev *rdev, unsigned selector)
-{
-	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
-
-	if (info == NULL) {
-		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
-		return -EINVAL;
-	}
-
-	/* return the uV for the fixed regulators */
-	if (info->fixed_uV)
-		return info->fixed_uV;
-
-	if (selector >= info->voltages_len)
-		return -EINVAL;
-
-	return info->voltages[selector];
-}
-
 static int ab8500_regulator_get_voltage_sel(struct regulator_dev *rdev)
 {
 	int ret, val;
@@ -279,14 +250,7 @@ static int ab8500_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 					     unsigned int new_sel)
 {
 	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
-	int ret;
 
-	/* If the regulator isn't on, it won't take time here */
-	ret = ab8500_regulator_is_enabled(rdev);
-	if (ret < 0)
-		return ret;
-	if (!ret)
-		return 0;
 	return info->delay;
 }
 
@@ -296,21 +260,14 @@ static struct regulator_ops ab8500_regulator_ops = {
 	.is_enabled	= ab8500_regulator_is_enabled,
 	.get_voltage_sel = ab8500_regulator_get_voltage_sel,
 	.set_voltage_sel = ab8500_regulator_set_voltage_sel,
-	.list_voltage	= ab8500_list_voltage,
+	.list_voltage	= regulator_list_voltage_table,
 	.enable_time	= ab8500_regulator_enable_time,
 	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
 };
 
 static int ab8500_fixed_get_voltage(struct regulator_dev *rdev)
 {
-	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
-
-	if (info == NULL) {
-		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
-		return -EINVAL;
-	}
-
-	return info->fixed_uV;
+	return rdev->desc->min_uV;
 }
 
 static struct regulator_ops ab8500_regulator_fixed_ops = {
@@ -318,9 +275,8 @@ static struct regulator_ops ab8500_regulator_fixed_ops = {
 	.disable	= ab8500_regulator_disable,
 	.is_enabled	= ab8500_regulator_is_enabled,
 	.get_voltage	= ab8500_fixed_get_voltage,
-	.list_voltage	= ab8500_list_voltage,
+	.list_voltage	= regulator_list_voltage_linear,
 	.enable_time	= ab8500_regulator_enable_time,
-	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
 };
 
 static struct ab8500_regulator_info
@@ -329,7 +285,7 @@ static struct ab8500_regulator_info
 	 * Variable Voltage Regulators
 	 *   name, min mV, max mV,
 	 *   update bank, reg, mask, enable val
-	 *   volt bank, reg, mask, table, table length
+	 *   volt bank, reg, mask
 	 */
 	[AB8500_LDO_AUX1] = {
 		.desc = {
@@ -339,9 +295,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_AUX1,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
 		},
-		.min_uV			= 1100000,
-		.max_uV			= 3300000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x03,
@@ -349,8 +304,6 @@ static struct ab8500_regulator_info
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x1f,
 		.voltage_mask		= 0x0f,
-		.voltages		= ldo_vauxn_voltages,
-		.voltages_len		= ARRAY_SIZE(ldo_vauxn_voltages),
 	},
 	[AB8500_LDO_AUX2] = {
 		.desc = {
@@ -360,9 +313,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_AUX2,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+			.volt_table	= ldo_vauxn_voltages,
 		},
-		.min_uV			= 1100000,
-		.max_uV			= 3300000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x0c,
@@ -370,8 +322,6 @@ static struct ab8500_regulator_info
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x20,
 		.voltage_mask		= 0x0f,
-		.voltages		= ldo_vauxn_voltages,
-		.voltages_len		= ARRAY_SIZE(ldo_vauxn_voltages),
 	},
 	[AB8500_LDO_AUX3] = {
 		.desc = {
@@ -381,9 +331,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_AUX3,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vaux3_voltages),
+			.volt_table	= ldo_vaux3_voltages,
 		},
-		.min_uV			= 1100000,
-		.max_uV			= 3300000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x0a,
 		.update_mask		= 0x03,
@@ -391,8 +340,6 @@ static struct ab8500_regulator_info
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x21,
 		.voltage_mask		= 0x07,
-		.voltages		= ldo_vaux3_voltages,
-		.voltages_len		= ARRAY_SIZE(ldo_vaux3_voltages),
 	},
 	[AB8500_LDO_INTCORE] = {
 		.desc = {
@@ -402,9 +349,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_INTCORE,
 			.owner		= THIS_MODULE,
 			.n_voltages	= ARRAY_SIZE(ldo_vintcore_voltages),
+			.volt_table	= ldo_vintcore_voltages,
 		},
-		.min_uV			= 1100000,
-		.max_uV			= 3300000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x44,
@@ -412,8 +358,6 @@ static struct ab8500_regulator_info
 		.voltage_bank		= 0x03,
 		.voltage_reg		= 0x80,
 		.voltage_mask		= 0x38,
-		.voltages		= ldo_vintcore_voltages,
-		.voltages_len		= ARRAY_SIZE(ldo_vintcore_voltages),
 	},
 
 	/*
@@ -429,9 +373,9 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_TVOUT,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
+			.min_uV		= 2000000,
 		},
 		.delay			= 10000,
-		.fixed_uV		= 2000000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x82,
@@ -445,8 +389,8 @@ static struct ab8500_regulator_info
 			.id             = AB8500_LDO_USB,
 			.owner          = THIS_MODULE,
 			.n_voltages     = 1,
+			.min_uV		= 3300000,
 		},
-		.fixed_uV               = 3300000,
 		.update_bank            = 0x03,
 		.update_reg             = 0x82,
 		.update_mask            = 0x03,
@@ -460,8 +404,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_AUDIO,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
+			.min_uV		= 2000000,
 		},
-		.fixed_uV		= 2000000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x02,
@@ -475,8 +419,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_ANAMIC1,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
+			.min_uV		= 2050000,
 		},
-		.fixed_uV		= 2050000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x08,
@@ -490,8 +434,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_ANAMIC2,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
+			.min_uV		= 2050000,
 		},
-		.fixed_uV		= 2050000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x10,
@@ -505,8 +449,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_DMIC,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
+			.min_uV		= 1800000,
 		},
-		.fixed_uV		= 1800000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x04,
@@ -520,8 +464,8 @@ static struct ab8500_regulator_info
 			.id		= AB8500_LDO_ANA,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
+			.min_uV		= 1200000,
 		},
-		.fixed_uV		= 1200000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x06,
 		.update_mask		= 0x0c,
@@ -769,9 +713,7 @@ static __devinit int ab8500_regulator_register(struct platform_device *pdev,
 		if (info->desc.id == AB8500_LDO_AUX3) {
 			info->desc.n_voltages =
 				ARRAY_SIZE(ldo_vauxn_voltages);
-			info->voltages = ldo_vauxn_voltages;
-			info->voltages_len =
-				ARRAY_SIZE(ldo_vauxn_voltages);
+			info->desc.volt_table = ldo_vauxn_voltages;
 			info->voltage_mask = 0xf;
 		}
 	}
@@ -906,18 +848,12 @@ static __devexit int ab8500_regulator_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id ab8500_regulator_match[] = {
-        { .compatible = "stericsson,ab8500-regulator", },
-        {}
-};
-
 static struct platform_driver ab8500_regulator_driver = {
 	.probe = ab8500_regulator_probe,
 	.remove = __devexit_p(ab8500_regulator_remove),
 	.driver         = {
 		.name   = "ab8500-regulator",
 		.owner  = THIS_MODULE,
-		.of_match_table = ab8500_regulator_match,
 	},
 };
 
diff --git a/drivers/regulator/ad5398.c b/drivers/regulator/ad5398.c
index 46d05f38baf..f123f7e3b75 100644
--- a/drivers/regulator/ad5398.c
+++ b/drivers/regulator/ad5398.c
@@ -89,9 +89,12 @@ static int ad5398_set_current_limit(struct regulator_dev *rdev, int min_uA, int
 	unsigned short data;
 	int ret;
 
-	if (min_uA > chip->max_uA || min_uA < chip->min_uA)
-		return -EINVAL;
-	if (max_uA > chip->max_uA || max_uA < chip->min_uA)
+	if (min_uA < chip->min_uA)
+		min_uA = chip->min_uA;
+	if (max_uA > chip->max_uA)
+		max_uA = chip->max_uA;
+
+	if (min_uA > chip->max_uA || max_uA < chip->min_uA)
 		return -EINVAL;
 
 	selector = DIV_ROUND_UP((min_uA - chip->min_uA) * chip->current_level,
diff --git a/drivers/regulator/anatop-regulator.c b/drivers/regulator/anatop-regulator.c
index e82e7eaac0f..e9c2085f9df 100644
--- a/drivers/regulator/anatop-regulator.c
+++ b/drivers/regulator/anatop-regulator.c
@@ -43,33 +43,15 @@ struct anatop_regulator {
 	struct regulator_init_data *initdata;
 };
 
-static int anatop_set_voltage(struct regulator_dev *reg, int min_uV,
-				  int max_uV, unsigned *selector)
+static int anatop_set_voltage_sel(struct regulator_dev *reg, unsigned selector)
 {
 	struct anatop_regulator *anatop_reg = rdev_get_drvdata(reg);
-	u32 val, sel, mask;
-	int uv;
-
-	uv = min_uV;
-	dev_dbg(&reg->dev, "%s: uv %d, min %d, max %d\n", __func__,
-		uv, anatop_reg->min_voltage,
-		anatop_reg->max_voltage);
-
-	if (uv < anatop_reg->min_voltage) {
-		if (max_uV > anatop_reg->min_voltage)
-			uv = anatop_reg->min_voltage;
-		else
-			return -EINVAL;
-	}
+	u32 val, mask;
 
 	if (!anatop_reg->control_reg)
 		return -ENOTSUPP;
 
-	sel = DIV_ROUND_UP(uv - anatop_reg->min_voltage, 25000);
-	if (sel * 25000 + anatop_reg->min_voltage > anatop_reg->max_voltage)
-		return -EINVAL;
-	val = anatop_reg->min_bit_val + sel;
-	*selector = sel;
+	val = anatop_reg->min_bit_val + selector;
 	dev_dbg(&reg->dev, "%s: calculated val %d\n", __func__, val);
 	mask = ((1 << anatop_reg->vol_bit_width) - 1) <<
 		anatop_reg->vol_bit_shift;
@@ -94,21 +76,11 @@ static int anatop_get_voltage_sel(struct regulator_dev *reg)
 	return val - anatop_reg->min_bit_val;
 }
 
-static int anatop_list_voltage(struct regulator_dev *reg, unsigned selector)
-{
-	struct anatop_regulator *anatop_reg = rdev_get_drvdata(reg);
-	int uv;
-
-	uv = anatop_reg->min_voltage + selector * 25000;
-	dev_dbg(&reg->dev, "vddio = %d, selector = %u\n", uv, selector);
-
-	return uv;
-}
-
 static struct regulator_ops anatop_rops = {
-	.set_voltage     = anatop_set_voltage,
+	.set_voltage_sel = anatop_set_voltage_sel,
 	.get_voltage_sel = anatop_get_voltage_sel,
-	.list_voltage    = anatop_list_voltage,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
 };
 
 static int __devinit anatop_regulator_probe(struct platform_device *pdev)
@@ -176,6 +148,8 @@ static int __devinit anatop_regulator_probe(struct platform_device *pdev)
 
 	rdesc->n_voltages = (sreg->max_voltage - sreg->min_voltage)
 		/ 25000 + 1;
+	rdesc->min_uV = sreg->min_voltage;
+	rdesc->uV_step = 25000;
 
 	config.dev = &pdev->dev;
 	config.init_data = initdata;
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
new file mode 100644
index 00000000000..c8f95c07adb
--- /dev/null
+++ b/drivers/regulator/arizona-ldo1.c
@@ -0,0 +1,138 @@
+/*
+ * arizona-ldo1.c  --  LDO1 supply for Arizona devices
+ *
+ * Copyright 2012 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/init.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/pdata.h>
+#include <linux/mfd/arizona/registers.h>
+
+struct arizona_ldo1 {
+	struct regulator_dev *regulator;
+	struct arizona *arizona;
+
+	struct regulator_consumer_supply supply;
+	struct regulator_init_data init_data;
+};
+
+static struct regulator_ops arizona_ldo1_ops = {
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+};
+
+static const struct regulator_desc arizona_ldo1 = {
+	.name = "LDO1",
+	.supply_name = "LDOVDD",
+	.type = REGULATOR_VOLTAGE,
+	.ops = &arizona_ldo1_ops,
+
+	.vsel_reg = ARIZONA_LDO1_CONTROL_1,
+	.vsel_mask = ARIZONA_LDO1_VSEL_MASK,
+	.min_uV = 900000,
+	.uV_step = 50000,
+	.n_voltages = 7,
+
+	.owner = THIS_MODULE,
+};
+
+static const struct regulator_init_data arizona_ldo1_default = {
+	.constraints = {
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+	},
+	.num_consumer_supplies = 1,
+};
+
+static __devinit int arizona_ldo1_probe(struct platform_device *pdev)
+{
+	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct arizona_ldo1 *ldo1;
+	int ret;
+
+	ldo1 = devm_kzalloc(&pdev->dev, sizeof(*ldo1), GFP_KERNEL);
+	if (ldo1 == NULL) {
+		dev_err(&pdev->dev, "Unable to allocate private data\n");
+		return -ENOMEM;
+	}
+
+	ldo1->arizona = arizona;
+
+	/*
+	 * Since the chip usually supplies itself we provide some
+	 * default init_data for it.  This will be overridden with
+	 * platform data if provided.
+	 */
+	ldo1->init_data = arizona_ldo1_default;
+	ldo1->init_data.consumer_supplies = &ldo1->supply;
+	ldo1->supply.supply = "DCVDD";
+	ldo1->supply.dev_name = dev_name(arizona->dev);
+
+	config.dev = arizona->dev;
+	config.driver_data = ldo1;
+	config.regmap = arizona->regmap;
+	config.ena_gpio = arizona->pdata.ldoena;
+
+	if (arizona->pdata.ldo1)
+		config.init_data = arizona->pdata.ldo1;
+	else
+		config.init_data = &ldo1->init_data;
+
+	ldo1->regulator = regulator_register(&arizona_ldo1, &config);
+	if (IS_ERR(ldo1->regulator)) {
+		ret = PTR_ERR(ldo1->regulator);
+		dev_err(arizona->dev, "Failed to register LDO1 supply: %d\n",
+			ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, ldo1);
+
+	return 0;
+}
+
+static __devexit int arizona_ldo1_remove(struct platform_device *pdev)
+{
+	struct arizona_ldo1 *ldo1 = platform_get_drvdata(pdev);
+
+	regulator_unregister(ldo1->regulator);
+
+	return 0;
+}
+
+static struct platform_driver arizona_ldo1_driver = {
+	.probe = arizona_ldo1_probe,
+	.remove = __devexit_p(arizona_ldo1_remove),
+	.driver		= {
+		.name	= "arizona-ldo1",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(arizona_ldo1_driver);
+
+/* Module information */
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_DESCRIPTION("Arizona LDO1 driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:arizona-ldo1");
diff --git a/drivers/regulator/arizona-micsupp.c b/drivers/regulator/arizona-micsupp.c
new file mode 100644
index 00000000000..450a069aa9b
--- /dev/null
+++ b/drivers/regulator/arizona-micsupp.c
@@ -0,0 +1,188 @@
+/*
+ * arizona-micsupp.c  --  Microphone supply for Arizona devices
+ *
+ * Copyright 2012 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/init.h>
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/arizona/core.h>
+#include <linux/mfd/arizona/pdata.h>
+#include <linux/mfd/arizona/registers.h>
+
+#define ARIZONA_MICSUPP_MAX_SELECTOR 0x1f
+
+struct arizona_micsupp {
+	struct regulator_dev *regulator;
+	struct arizona *arizona;
+
+	struct regulator_consumer_supply supply;
+	struct regulator_init_data init_data;
+};
+
+static int arizona_micsupp_list_voltage(struct regulator_dev *rdev,
+					unsigned int selector)
+{
+	if (selector > ARIZONA_MICSUPP_MAX_SELECTOR)
+		return -EINVAL;
+
+	if (selector == ARIZONA_MICSUPP_MAX_SELECTOR)
+		return 3300000;
+	else
+		return (selector * 50000) + 1700000;
+}
+
+static int arizona_micsupp_map_voltage(struct regulator_dev *rdev,
+				       int min_uV, int max_uV)
+{
+	unsigned int voltage;
+	int selector;
+
+	if (min_uV < 1700000)
+		min_uV = 1700000;
+
+	if (min_uV > 3200000)
+		selector = ARIZONA_MICSUPP_MAX_SELECTOR;
+	else
+		selector = DIV_ROUND_UP(min_uV - 1700000, 50000);
+
+	if (selector < 0)
+		return -EINVAL;
+
+	voltage = arizona_micsupp_list_voltage(rdev, selector);
+	if (voltage < min_uV || voltage > max_uV)
+		return -EINVAL;
+
+	return selector;
+}
+
+static struct regulator_ops arizona_micsupp_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+
+	.list_voltage = arizona_micsupp_list_voltage,
+	.map_voltage = arizona_micsupp_map_voltage,
+
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+};
+
+static const struct regulator_desc arizona_micsupp = {
+	.name = "MICVDD",
+	.supply_name = "CPVDD",
+	.type = REGULATOR_VOLTAGE,
+	.n_voltages = ARIZONA_MICSUPP_MAX_SELECTOR + 1,
+	.ops = &arizona_micsupp_ops,
+
+	.vsel_reg = ARIZONA_LDO2_CONTROL_1,
+	.vsel_mask = ARIZONA_LDO2_VSEL_MASK,
+	.enable_reg = ARIZONA_MIC_CHARGE_PUMP_1,
+	.enable_mask = ARIZONA_CPMIC_ENA,
+
+	.owner = THIS_MODULE,
+};
+
+static const struct regulator_init_data arizona_micsupp_default = {
+	.constraints = {
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS |
+				REGULATOR_CHANGE_VOLTAGE,
+		.min_uV = 1700000,
+		.max_uV = 3300000,
+	},
+
+	.num_consumer_supplies = 1,
+};
+
+static __devinit int arizona_micsupp_probe(struct platform_device *pdev)
+{
+	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct arizona_micsupp *micsupp;
+	int ret;
+
+	micsupp = devm_kzalloc(&pdev->dev, sizeof(*micsupp), GFP_KERNEL);
+	if (micsupp == NULL) {
+		dev_err(&pdev->dev, "Unable to allocate private data\n");
+		return -ENOMEM;
+	}
+
+	micsupp->arizona = arizona;
+
+	/*
+	 * Since the chip usually supplies itself we provide some
+	 * default init_data for it.  This will be overridden with
+	 * platform data if provided.
+	 */
+	micsupp->init_data = arizona_micsupp_default;
+	micsupp->init_data.consumer_supplies = &micsupp->supply;
+	micsupp->supply.supply = "MICVDD";
+	micsupp->supply.dev_name = dev_name(arizona->dev);
+
+	config.dev = arizona->dev;
+	config.driver_data = micsupp;
+	config.regmap = arizona->regmap;
+
+	if (arizona->pdata.micvdd)
+		config.init_data = arizona->pdata.micvdd;
+	else
+		config.init_data = &micsupp->init_data;
+
+	/* Default to regulated mode until the API supports bypass */
+	regmap_update_bits(arizona->regmap, ARIZONA_MIC_CHARGE_PUMP_1,
+			   ARIZONA_CPMIC_BYPASS, 0);
+
+	micsupp->regulator = regulator_register(&arizona_micsupp, &config);
+	if (IS_ERR(micsupp->regulator)) {
+		ret = PTR_ERR(micsupp->regulator);
+		dev_err(arizona->dev, "Failed to register mic supply: %d\n",
+			ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, micsupp);
+
+	return 0;
+}
+
+static __devexit int arizona_micsupp_remove(struct platform_device *pdev)
+{
+	struct arizona_micsupp *micsupp = platform_get_drvdata(pdev);
+
+	regulator_unregister(micsupp->regulator);
+
+	return 0;
+}
+
+static struct platform_driver arizona_micsupp_driver = {
+	.probe = arizona_micsupp_probe,
+	.remove = __devexit_p(arizona_micsupp_remove),
+	.driver		= {
+		.name	= "arizona-micsupp",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(arizona_micsupp_driver);
+
+/* Module information */
+MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
+MODULE_DESCRIPTION("Arizona microphone supply driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:arizona-micsupp");
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 8b4b3829d9e..f092588a078 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -23,6 +23,7 @@
 #include <linux/mutex.h>
 #include <linux/suspend.h>
 #include <linux/delay.h>
+#include <linux/gpio.h>
 #include <linux/of.h>
 #include <linux/regmap.h>
 #include <linux/regulator/of_regulator.h>
@@ -108,28 +109,6 @@ static const char *rdev_get_name(struct regulator_dev *rdev)
 		return "";
 }
 
-/* gets the regulator for a given consumer device */
-static struct regulator *get_device_regulator(struct device *dev)
-{
-	struct regulator *regulator = NULL;
-	struct regulator_dev *rdev;
-
-	mutex_lock(&regulator_list_mutex);
-	list_for_each_entry(rdev, &regulator_list, list) {
-		mutex_lock(&rdev->mutex);
-		list_for_each_entry(regulator, &rdev->consumer_list, list) {
-			if (regulator->dev == dev) {
-				mutex_unlock(&rdev->mutex);
-				mutex_unlock(&regulator_list_mutex);
-				return regulator;
-			}
-		}
-		mutex_unlock(&rdev->mutex);
-	}
-	mutex_unlock(&regulator_list_mutex);
-	return NULL;
-}
-
 /**
  * of_get_regulator - get a regulator device node based on supply name
  * @dev: Device pointer for the consumer (of regulator) device
@@ -303,18 +282,6 @@ static int regulator_check_drms(struct regulator_dev *rdev)
 	return 0;
 }
 
-static ssize_t device_requested_uA_show(struct device *dev,
-			     struct device_attribute *attr, char *buf)
-{
-	struct regulator *regulator;
-
-	regulator = get_device_regulator(dev);
-	if (regulator == NULL)
-		return 0;
-
-	return sprintf(buf, "%d\n", regulator->uA_load);
-}
-
 static ssize_t regulator_uV_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -427,6 +394,9 @@ static ssize_t regulator_status_show(struct device *dev,
 	case REGULATOR_STATUS_STANDBY:
 		label = "standby";
 		break;
+	case REGULATOR_STATUS_UNDEFINED:
+		label = "undefined";
+		break;
 	default:
 		return -ERANGE;
 	}
@@ -967,6 +937,14 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 		}
 	}
 
+	if (rdev->constraints->ramp_delay && ops->set_ramp_delay) {
+		ret = ops->set_ramp_delay(rdev, rdev->constraints->ramp_delay);
+		if (ret < 0) {
+			rdev_err(rdev, "failed to set ramp_delay\n");
+			goto out;
+		}
+	}
+
 	print_constraints(rdev);
 	return 0;
 out:
@@ -1097,48 +1075,29 @@ static struct regulator *create_regulator(struct regulator_dev *rdev,
 	list_add(&regulator->list, &rdev->consumer_list);
 
 	if (dev) {
-		/* create a 'requested_microamps_name' sysfs entry */
-		size = scnprintf(buf, REG_STR_SIZE,
-				 "microamps_requested_%s-%s",
-				 dev_name(dev), supply_name);
-		if (size >= REG_STR_SIZE)
-			goto overflow_err;
-
 		regulator->dev = dev;
-		sysfs_attr_init(&regulator->dev_attr.attr);
-		regulator->dev_attr.attr.name = kstrdup(buf, GFP_KERNEL);
-		if (regulator->dev_attr.attr.name == NULL)
-			goto attr_name_err;
-
-		regulator->dev_attr.attr.mode = 0444;
-		regulator->dev_attr.show = device_requested_uA_show;
-		err = device_create_file(dev, &regulator->dev_attr);
-		if (err < 0) {
-			rdev_warn(rdev, "could not add regulator_dev requested microamps sysfs entry\n");
-			goto attr_name_err;
-		}
 
-		/* also add a link to the device sysfs entry */
+		/* Add a link to the device sysfs entry */
 		size = scnprintf(buf, REG_STR_SIZE, "%s-%s",
 				 dev->kobj.name, supply_name);
 		if (size >= REG_STR_SIZE)
-			goto attr_err;
+			goto overflow_err;
 
 		regulator->supply_name = kstrdup(buf, GFP_KERNEL);
 		if (regulator->supply_name == NULL)
-			goto attr_err;
+			goto overflow_err;
 
 		err = sysfs_create_link(&rdev->dev.kobj, &dev->kobj,
 					buf);
 		if (err) {
 			rdev_warn(rdev, "could not add device link %s err %d\n",
 				  dev->kobj.name, err);
-			goto link_name_err;
+			/* non-fatal */
 		}
 	} else {
 		regulator->supply_name = kstrdup(supply_name, GFP_KERNEL);
 		if (regulator->supply_name == NULL)
-			goto attr_err;
+			goto overflow_err;
 	}
 
 	regulator->debugfs = debugfs_create_dir(regulator->supply_name,
@@ -1165,12 +1124,6 @@ static struct regulator *create_regulator(struct regulator_dev *rdev,
 
 	mutex_unlock(&rdev->mutex);
 	return regulator;
-link_name_err:
-	kfree(regulator->supply_name);
-attr_err:
-	device_remove_file(regulator->dev, &regulator->dev_attr);
-attr_name_err:
-	kfree(regulator->dev_attr.attr.name);
 overflow_err:
 	list_del(&regulator->list);
 	kfree(regulator);
@@ -1181,7 +1134,7 @@ overflow_err:
 static int _regulator_get_enable_time(struct regulator_dev *rdev)
 {
 	if (!rdev->desc->ops->enable_time)
-		return 0;
+		return rdev->desc->enable_time;
 	return rdev->desc->ops->enable_time(rdev);
 }
 
@@ -1420,11 +1373,8 @@ void regulator_put(struct regulator *regulator)
 	debugfs_remove_recursive(regulator->debugfs);
 
 	/* remove any sysfs entries */
-	if (regulator->dev) {
+	if (regulator->dev)
 		sysfs_remove_link(&rdev->dev.kobj, regulator->supply_name);
-		device_remove_file(regulator->dev, &regulator->dev_attr);
-		kfree(regulator->dev_attr.attr.name);
-	}
 	kfree(regulator->supply_name);
 	list_del(&regulator->list);
 	kfree(regulator);
@@ -1459,19 +1409,61 @@ void devm_regulator_put(struct regulator *regulator)
 {
 	int rc;
 
-	rc = devres_destroy(regulator->dev, devm_regulator_release,
+	rc = devres_release(regulator->dev, devm_regulator_release,
 			    devm_regulator_match, regulator);
-	if (rc == 0)
-		regulator_put(regulator);
-	else
+	if (rc != 0)
 		WARN_ON(rc);
 }
 EXPORT_SYMBOL_GPL(devm_regulator_put);
 
+static int _regulator_do_enable(struct regulator_dev *rdev)
+{
+	int ret, delay;
+
+	/* Query before enabling in case configuration dependent.  */
+	ret = _regulator_get_enable_time(rdev);
+	if (ret >= 0) {
+		delay = ret;
+	} else {
+		rdev_warn(rdev, "enable_time() failed: %d\n", ret);
+		delay = 0;
+	}
+
+	trace_regulator_enable(rdev_get_name(rdev));
+
+	if (rdev->ena_gpio) {
+		gpio_set_value_cansleep(rdev->ena_gpio,
+					!rdev->ena_gpio_invert);
+		rdev->ena_gpio_state = 1;
+	} else if (rdev->desc->ops->enable) {
+		ret = rdev->desc->ops->enable(rdev);
+		if (ret < 0)
+			return ret;
+	} else {
+		return -EINVAL;
+	}
+
+	/* Allow the regulator to ramp; it would be useful to extend
+	 * this for bulk operations so that the regulators can ramp
+	 * together.  */
+	trace_regulator_enable_delay(rdev_get_name(rdev));
+
+	if (delay >= 1000) {
+		mdelay(delay / 1000);
+		udelay(delay % 1000);
+	} else if (delay) {
+		udelay(delay);
+	}
+
+	trace_regulator_enable_complete(rdev_get_name(rdev));
+
+	return 0;
+}
+
 /* locks held by regulator_enable() */
 static int _regulator_enable(struct regulator_dev *rdev)
 {
-	int ret, delay;
+	int ret;
 
 	/* check voltage and requested load before enabling */
 	if (rdev->constraints &&
@@ -1485,40 +1477,10 @@ static int _regulator_enable(struct regulator_dev *rdev)
 			if (!_regulator_can_change_status(rdev))
 				return -EPERM;
 
-			if (!rdev->desc->ops->enable)
-				return -EINVAL;
-
-			/* Query before enabling in case configuration
-			 * dependent.  */
-			ret = _regulator_get_enable_time(rdev);
-			if (ret >= 0) {
-				delay = ret;
-			} else {
-				rdev_warn(rdev, "enable_time() failed: %d\n",
-					   ret);
-				delay = 0;
-			}
-
-			trace_regulator_enable(rdev_get_name(rdev));
-
-			/* Allow the regulator to ramp; it would be useful
-			 * to extend this for bulk operations so that the
-			 * regulators can ramp together.  */
-			ret = rdev->desc->ops->enable(rdev);
+			ret = _regulator_do_enable(rdev);
 			if (ret < 0)
 				return ret;
 
-			trace_regulator_enable_delay(rdev_get_name(rdev));
-
-			if (delay >= 1000) {
-				mdelay(delay / 1000);
-				udelay(delay % 1000);
-			} else if (delay) {
-				udelay(delay);
-			}
-
-			trace_regulator_enable_complete(rdev_get_name(rdev));
-
 		} else if (ret < 0) {
 			rdev_err(rdev, "is_enabled() failed: %d\n", ret);
 			return ret;
@@ -1567,6 +1529,30 @@ int regulator_enable(struct regulator *regulator)
 }
 EXPORT_SYMBOL_GPL(regulator_enable);
 
+static int _regulator_do_disable(struct regulator_dev *rdev)
+{
+	int ret;
+
+	trace_regulator_disable(rdev_get_name(rdev));
+
+	if (rdev->ena_gpio) {
+		gpio_set_value_cansleep(rdev->ena_gpio,
+					rdev->ena_gpio_invert);
+		rdev->ena_gpio_state = 0;
+
+	} else if (rdev->desc->ops->disable) {
+		ret = rdev->desc->ops->disable(rdev);
+		if (ret != 0)
+			return ret;
+	}
+
+	trace_regulator_disable_complete(rdev_get_name(rdev));
+
+	_notifier_call_chain(rdev, REGULATOR_EVENT_DISABLE,
+			     NULL);
+	return 0;
+}
+
 /* locks held by regulator_disable() */
 static int _regulator_disable(struct regulator_dev *rdev)
 {
@@ -1581,20 +1567,12 @@ static int _regulator_disable(struct regulator_dev *rdev)
 	    (rdev->constraints && !rdev->constraints->always_on)) {
 
 		/* we are last user */
-		if (_regulator_can_change_status(rdev) &&
-		    rdev->desc->ops->disable) {
-			trace_regulator_disable(rdev_get_name(rdev));
-
-			ret = rdev->desc->ops->disable(rdev);
+		if (_regulator_can_change_status(rdev)) {
+			ret = _regulator_do_disable(rdev);
 			if (ret < 0) {
 				rdev_err(rdev, "failed to disable\n");
 				return ret;
 			}
-
-			trace_regulator_disable_complete(rdev_get_name(rdev));
-
-			_notifier_call_chain(rdev, REGULATOR_EVENT_DISABLE,
-					     NULL);
 		}
 
 		rdev->use_count = 0;
@@ -1812,6 +1790,10 @@ EXPORT_SYMBOL_GPL(regulator_disable_regmap);
 
 static int _regulator_is_enabled(struct regulator_dev *rdev)
 {
+	/* A GPIO control always takes precedence */
+	if (rdev->ena_gpio)
+		return rdev->ena_gpio_state;
+
 	/* If we don't know then assume that the regulator is always on */
 	if (!rdev->desc->ops->is_enabled)
 		return 1;
@@ -1883,6 +1865,31 @@ int regulator_list_voltage_linear(struct regulator_dev *rdev,
 EXPORT_SYMBOL_GPL(regulator_list_voltage_linear);
 
 /**
+ * regulator_list_voltage_table - List voltages with table based mapping
+ *
+ * @rdev: Regulator device
+ * @selector: Selector to convert into a voltage
+ *
+ * Regulators with table based mapping between voltages and
+ * selectors can set volt_table in the regulator descriptor
+ * and then use this function as their list_voltage() operation.
+ */
+int regulator_list_voltage_table(struct regulator_dev *rdev,
+				 unsigned int selector)
+{
+	if (!rdev->desc->volt_table) {
+		BUG_ON(!rdev->desc->volt_table);
+		return -EINVAL;
+	}
+
+	if (selector >= rdev->desc->n_voltages)
+		return -EINVAL;
+
+	return rdev->desc->volt_table[selector];
+}
+EXPORT_SYMBOL_GPL(regulator_list_voltage_table);
+
+/**
  * regulator_list_voltage - enumerate supported voltages
  * @regulator: regulator source
  * @selector: identify voltage to list
@@ -1928,8 +1935,18 @@ EXPORT_SYMBOL_GPL(regulator_list_voltage);
 int regulator_is_supported_voltage(struct regulator *regulator,
 				   int min_uV, int max_uV)
 {
+	struct regulator_dev *rdev = regulator->rdev;
 	int i, voltages, ret;
 
+	/* If we can't change voltage check the current voltage */
+	if (!(rdev->constraints->valid_ops_mask & REGULATOR_CHANGE_VOLTAGE)) {
+		ret = regulator_get_voltage(regulator);
+		if (ret >= 0)
+			return (min_uV >= ret && ret <= max_uV);
+		else
+			return ret;
+	}
+
 	ret = regulator_count_voltages(regulator);
 	if (ret < 0)
 		return ret;
@@ -2045,6 +2062,14 @@ int regulator_map_voltage_linear(struct regulator_dev *rdev,
 {
 	int ret, voltage;
 
+	/* Allow uV_step to be 0 for fixed voltage */
+	if (rdev->desc->n_voltages == 1 && rdev->desc->uV_step == 0) {
+		if (min_uV <= rdev->desc->min_uV && rdev->desc->min_uV <= max_uV)
+			return 0;
+		else
+			return -EINVAL;
+	}
+
 	if (!rdev->desc->uV_step) {
 		BUG_ON(!rdev->desc->uV_step);
 		return -EINVAL;
@@ -2071,7 +2096,7 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 {
 	int ret;
 	int delay = 0;
-	int best_val;
+	int best_val = 0;
 	unsigned int selector;
 	int old_selector = -1;
 
@@ -2084,7 +2109,8 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 	 * If we can't obtain the old selector there is not enough
 	 * info to call set_voltage_time_sel().
 	 */
-	if (rdev->desc->ops->set_voltage_time_sel &&
+	if (_regulator_is_enabled(rdev) &&
+	    rdev->desc->ops->set_voltage_time_sel &&
 	    rdev->desc->ops->get_voltage_sel) {
 		old_selector = rdev->desc->ops->get_voltage_sel(rdev);
 		if (old_selector < 0)
@@ -2094,29 +2120,45 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 	if (rdev->desc->ops->set_voltage) {
 		ret = rdev->desc->ops->set_voltage(rdev, min_uV, max_uV,
 						   &selector);
+
+		if (ret >= 0) {
+			if (rdev->desc->ops->list_voltage)
+				best_val = rdev->desc->ops->list_voltage(rdev,
+									 selector);
+			else
+				best_val = _regulator_get_voltage(rdev);
+		}
+
 	} else if (rdev->desc->ops->set_voltage_sel) {
-		if (rdev->desc->ops->map_voltage)
+		if (rdev->desc->ops->map_voltage) {
 			ret = rdev->desc->ops->map_voltage(rdev, min_uV,
 							   max_uV);
-		else
-			ret = regulator_map_voltage_iterate(rdev, min_uV,
-							    max_uV);
+		} else {
+			if (rdev->desc->ops->list_voltage ==
+			    regulator_list_voltage_linear)
+				ret = regulator_map_voltage_linear(rdev,
+								min_uV, max_uV);
+			else
+				ret = regulator_map_voltage_iterate(rdev,
+								min_uV, max_uV);
+		}
 
 		if (ret >= 0) {
-			selector = ret;
-			ret = rdev->desc->ops->set_voltage_sel(rdev, ret);
+			best_val = rdev->desc->ops->list_voltage(rdev, ret);
+			if (min_uV <= best_val && max_uV >= best_val) {
+				selector = ret;
+				ret = rdev->desc->ops->set_voltage_sel(rdev,
+								       ret);
+			} else {
+				ret = -EINVAL;
+			}
 		}
 	} else {
 		ret = -EINVAL;
 	}
 
-	if (rdev->desc->ops->list_voltage)
-		best_val = rdev->desc->ops->list_voltage(rdev, selector);
-	else
-		best_val = -1;
-
 	/* Call set_voltage_time_sel if successfully obtained old_selector */
-	if (ret == 0 && old_selector >= 0 &&
+	if (ret == 0 && _regulator_is_enabled(rdev) && old_selector >= 0 &&
 	    rdev->desc->ops->set_voltage_time_sel) {
 
 		delay = rdev->desc->ops->set_voltage_time_sel(rdev,
@@ -2126,19 +2168,19 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				  delay);
 			delay = 0;
 		}
-	}
 
-	/* Insert any necessary delays */
-	if (delay >= 1000) {
-		mdelay(delay / 1000);
-		udelay(delay % 1000);
-	} else if (delay) {
-		udelay(delay);
+		/* Insert any necessary delays */
+		if (delay >= 1000) {
+			mdelay(delay / 1000);
+			udelay(delay % 1000);
+		} else if (delay) {
+			udelay(delay);
+		}
 	}
 
-	if (ret == 0)
+	if (ret == 0 && best_val >= 0)
 		_notifier_call_chain(rdev, REGULATOR_EVENT_VOLTAGE_CHANGE,
-				     NULL);
+				     (void *)best_val);
 
 	trace_regulator_set_voltage_complete(rdev_get_name(rdev), best_val);
 
@@ -2249,6 +2291,46 @@ int regulator_set_voltage_time(struct regulator *regulator,
 EXPORT_SYMBOL_GPL(regulator_set_voltage_time);
 
 /**
+ *regulator_set_voltage_time_sel - get raise/fall time
+ * @regulator: regulator source
+ * @old_selector: selector for starting voltage
+ * @new_selector: selector for target voltage
+ *
+ * Provided with the starting and target voltage selectors, this function
+ * returns time in microseconds required to rise or fall to this new voltage
+ *
+ * Drivers providing ramp_delay in regulation_constraints can use this as their
+ * set_voltage_time_sel() operation.
+ */
+int regulator_set_voltage_time_sel(struct regulator_dev *rdev,
+				   unsigned int old_selector,
+				   unsigned int new_selector)
+{
+	unsigned int ramp_delay = 0;
+	int old_volt, new_volt;
+
+	if (rdev->constraints->ramp_delay)
+		ramp_delay = rdev->constraints->ramp_delay;
+	else if (rdev->desc->ramp_delay)
+		ramp_delay = rdev->desc->ramp_delay;
+
+	if (ramp_delay == 0) {
+		rdev_warn(rdev, "ramp_delay not set\n");
+		return 0;
+	}
+
+	/* sanity check */
+	if (!rdev->desc->ops->list_voltage)
+		return -EINVAL;
+
+	old_volt = rdev->desc->ops->list_voltage(rdev, old_selector);
+	new_volt = rdev->desc->ops->list_voltage(rdev, new_selector);
+
+	return DIV_ROUND_UP(abs(new_volt - old_volt), ramp_delay);
+}
+EXPORT_SYMBOL_GPL(regulator_set_voltage_time_sel);
+
+/**
  * regulator_sync_voltage - re-apply last regulator output voltage
  * @regulator: regulator source
  *
@@ -2628,7 +2710,7 @@ static void _notifier_call_chain(struct regulator_dev *rdev,
 				  unsigned long event, void *data)
 {
 	/* call rdev chain first */
-	blocking_notifier_call_chain(&rdev->notifier, event, NULL);
+	blocking_notifier_call_chain(&rdev->notifier, event, data);
 }
 
 /**
@@ -2744,7 +2826,7 @@ static void regulator_bulk_enable_async(void *data, async_cookie_t cookie)
 int regulator_bulk_enable(int num_consumers,
 			  struct regulator_bulk_data *consumers)
 {
-	LIST_HEAD(async_domain);
+	ASYNC_DOMAIN_EXCLUSIVE(async_domain);
 	int i;
 	int ret = 0;
 
@@ -2909,10 +2991,10 @@ int regulator_mode_to_status(unsigned int mode)
 		return REGULATOR_STATUS_NORMAL;
 	case REGULATOR_MODE_IDLE:
 		return REGULATOR_STATUS_IDLE;
-	case REGULATOR_STATUS_STANDBY:
+	case REGULATOR_MODE_STANDBY:
 		return REGULATOR_STATUS_STANDBY;
 	default:
-		return 0;
+		return REGULATOR_STATUS_UNDEFINED;
 	}
 }
 EXPORT_SYMBOL_GPL(regulator_mode_to_status);
@@ -3105,7 +3187,10 @@ regulator_register(const struct regulator_desc *regulator_desc,
 	rdev->reg_data = config->driver_data;
 	rdev->owner = regulator_desc->owner;
 	rdev->desc = regulator_desc;
-	rdev->regmap = config->regmap;
+	if (config->regmap)
+		rdev->regmap = config->regmap;
+	else
+		rdev->regmap = dev_get_regmap(dev, NULL);
 	INIT_LIST_HEAD(&rdev->consumer_list);
 	INIT_LIST_HEAD(&rdev->list);
 	BLOCKING_INIT_NOTIFIER_HEAD(&rdev->notifier);
@@ -3132,6 +3217,26 @@ regulator_register(const struct regulator_desc *regulator_desc,
 
 	dev_set_drvdata(&rdev->dev, rdev);
 
+	if (config->ena_gpio) {
+		ret = gpio_request_one(config->ena_gpio,
+				       GPIOF_DIR_OUT | config->ena_gpio_flags,
+				       rdev_get_name(rdev));
+		if (ret != 0) {
+			rdev_err(rdev, "Failed to request enable GPIO%d: %d\n",
+				 config->ena_gpio, ret);
+			goto clean;
+		}
+
+		rdev->ena_gpio = config->ena_gpio;
+		rdev->ena_gpio_invert = config->ena_gpio_invert;
+
+		if (config->ena_gpio_flags & GPIOF_OUT_INIT_HIGH)
+			rdev->ena_gpio_state = 1;
+
+		if (rdev->ena_gpio_invert)
+			rdev->ena_gpio_state = !rdev->ena_gpio_state;
+	}
+
 	/* set regulator constraints */
 	if (init_data)
 		constraints = &init_data->constraints;
@@ -3200,6 +3305,8 @@ unset_supplies:
 scrub:
 	if (rdev->supply)
 		regulator_put(rdev->supply);
+	if (rdev->ena_gpio)
+		gpio_free(rdev->ena_gpio);
 	kfree(rdev->constraints);
 	device_unregister(&rdev->dev);
 	/* device core frees rdev */
@@ -3233,6 +3340,8 @@ void regulator_unregister(struct regulator_dev *rdev)
 	unset_regulator_supplies(rdev);
 	list_del(&rdev->list);
 	kfree(rdev->constraints);
+	if (rdev->ena_gpio)
+		gpio_free(rdev->ena_gpio);
 	device_unregister(&rdev->dev);
 	mutex_unlock(&regulator_list_mutex);
 }
@@ -3472,6 +3581,15 @@ static int __init regulator_init_complete(void)
 	struct regulation_constraints *c;
 	int enabled, ret;
 
+	/*
+	 * Since DT doesn't provide an idiomatic mechanism for
+	 * enabling full constraints and since it's much more natural
+	 * with DT to provide them just assume that a DT enabled
+	 * system has full constraints.
+	 */
+	if (of_have_populated_dt())
+		has_full_constraints = true;
+
 	mutex_lock(&regulator_list_mutex);
 
 	/* If we have a full configuration then disable any regulators
diff --git a/drivers/regulator/da903x.c b/drivers/regulator/da903x.c
index 1005f5f7e60..36c5b92fe0a 100644
--- a/drivers/regulator/da903x.c
+++ b/drivers/regulator/da903x.c
@@ -107,6 +107,9 @@ static int da903x_set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 	struct device *da9034_dev = to_da903x_dev(rdev);
 	uint8_t val, mask;
 
+	if (rdev->desc->n_voltages == 1)
+		return -EINVAL;
+
 	val = selector << info->vol_shift;
 	mask = ((1 << info->vol_nbits) - 1)  << info->vol_shift;
 
@@ -120,6 +123,9 @@ static int da903x_get_voltage_sel(struct regulator_dev *rdev)
 	uint8_t val, mask;
 	int ret;
 
+	if (rdev->desc->n_voltages == 1)
+		return 0;
+
 	ret = da903x_read(da9034_dev, info->vol_reg, &val);
 	if (ret)
 		return ret;
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
index 88976d8d44e..903299cf15c 100644
--- a/drivers/regulator/da9052-regulator.c
+++ b/drivers/regulator/da9052-regulator.c
@@ -405,12 +405,12 @@ static int __devinit da9052_regulator_probe(struct platform_device *pdev)
 		if (!nproot)
 			return -ENODEV;
 
-		for (np = of_get_next_child(nproot, NULL); np;
-		     np = of_get_next_child(nproot, np)) {
+		for_each_child_of_node(nproot, np) {
 			if (!of_node_cmp(np->name,
 					 regulator->info->reg_desc.name)) {
 				config.init_data = of_get_regulator_init_data(
 					&pdev->dev, np);
+				config.of_node = np;
 				break;
 			}
 		}
diff --git a/drivers/regulator/db8500-prcmu.c b/drivers/regulator/db8500-prcmu.c
index 9dbb491b6ef..359f8d18fc3 100644
--- a/drivers/regulator/db8500-prcmu.c
+++ b/drivers/regulator/db8500-prcmu.c
@@ -547,16 +547,10 @@ static int __exit db8500_regulator_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id db8500_prcmu_regulator_match[] = {
-        { .compatible = "stericsson,db8500-prcmu-regulator", },
-        {}
-};
-
 static struct platform_driver db8500_regulator_driver = {
 	.driver = {
 		.name = "db8500-prcmu-regulators",
 		.owner = THIS_MODULE,
-		.of_match_table = db8500_prcmu_regulator_match,
 	},
 	.probe = db8500_regulator_probe,
 	.remove = __exit_p(db8500_regulator_remove),
diff --git a/drivers/regulator/fixed-helper.c b/drivers/regulator/fixed-helper.c
index cacd33c9d04..f9d027992aa 100644
--- a/drivers/regulator/fixed-helper.c
+++ b/drivers/regulator/fixed-helper.c
@@ -1,4 +1,5 @@
 #include <linux/slab.h>
+#include <linux/string.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/fixed.h>
@@ -13,17 +14,20 @@ static void regulator_fixed_release(struct device *dev)
 {
 	struct fixed_regulator_data *data = container_of(dev,
 			struct fixed_regulator_data, pdev.dev);
+	kfree(data->cfg.supply_name);
 	kfree(data);
 }
 
 /**
- * regulator_register_fixed - register a no-op fixed regulator
+ * regulator_register_fixed_name - register a no-op fixed regulator
  * @id: platform device id
+ * @name: name to be used for the regulator
  * @supplies: consumers for this regulator
  * @num_supplies: number of consumers
+ * @uv: voltage in microvolts
  */
-struct platform_device *regulator_register_fixed(int id,
-		struct regulator_consumer_supply *supplies, int num_supplies)
+struct platform_device *regulator_register_always_on(int id, const char *name,
+	struct regulator_consumer_supply *supplies, int num_supplies, int uv)
 {
 	struct fixed_regulator_data *data;
 
@@ -31,8 +35,13 @@ struct platform_device *regulator_register_fixed(int id,
 	if (!data)
 		return NULL;
 
-	data->cfg.supply_name = "fixed-dummy";
-	data->cfg.microvolts = 0;
+	data->cfg.supply_name = kstrdup(name, GFP_KERNEL);
+	if (!data->cfg.supply_name) {
+		kfree(data);
+		return NULL;
+	}
+
+	data->cfg.microvolts = uv;
 	data->cfg.gpio = -EINVAL;
 	data->cfg.enabled_at_boot = 1;
 	data->cfg.init_data = &data->init_data;
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index f09fe7b20e8..185468c4d38 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -35,10 +35,6 @@ struct fixed_voltage_data {
 	struct regulator_desc desc;
 	struct regulator_dev *dev;
 	int microvolts;
-	int gpio;
-	unsigned startup_delay;
-	bool enable_high;
-	bool is_enabled;
 };
 
 
@@ -61,11 +57,11 @@ of_get_fixed_voltage_config(struct device *dev)
 	config = devm_kzalloc(dev, sizeof(struct fixed_voltage_config),
 								 GFP_KERNEL);
 	if (!config)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	config->init_data = of_get_regulator_init_data(dev, dev->of_node);
 	if (!config->init_data)
-		return NULL;
+		return ERR_PTR(-EINVAL);
 
 	init_data = config->init_data;
 	init_data->constraints.apply_uV = 0;
@@ -76,13 +72,26 @@ of_get_fixed_voltage_config(struct device *dev)
 	} else {
 		dev_err(dev,
 			 "Fixed regulator specified with variable voltages\n");
-		return NULL;
+		return ERR_PTR(-EINVAL);
 	}
 
 	if (init_data->constraints.boot_on)
 		config->enabled_at_boot = true;
 
 	config->gpio = of_get_named_gpio(np, "gpio", 0);
+	/*
+	 * of_get_named_gpio() currently returns ENODEV rather than
+	 * EPROBE_DEFER. This code attempts to be compatible with both
+	 * for now; the ENODEV check can be removed once the API is fixed.
+	 * of_get_named_gpio() doesn't differentiate between a missing
+	 * property (which would be fine here, since the GPIO is optional)
+	 * and some other error. Patches have been posted for both issues.
+	 * Once they are check in, we should replace this with:
+	 * if (config->gpio < 0 && config->gpio != -ENOENT)
+	 */
+	if ((config->gpio == -ENODEV) || (config->gpio == -EPROBE_DEFER))
+		return ERR_PTR(-EPROBE_DEFER);
+
 	delay = of_get_property(np, "startup-delay-us", NULL);
 	if (delay)
 		config->startup_delay = be32_to_cpu(*delay);
@@ -93,41 +102,10 @@ of_get_fixed_voltage_config(struct device *dev)
 	if (of_find_property(np, "gpio-open-drain", NULL))
 		config->gpio_is_open_drain = true;
 
-	return config;
-}
-
-static int fixed_voltage_is_enabled(struct regulator_dev *dev)
-{
-	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
-
-	return data->is_enabled;
-}
-
-static int fixed_voltage_enable(struct regulator_dev *dev)
-{
-	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
-
-	gpio_set_value_cansleep(data->gpio, data->enable_high);
-	data->is_enabled = true;
-
-	return 0;
-}
-
-static int fixed_voltage_disable(struct regulator_dev *dev)
-{
-	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
-
-	gpio_set_value_cansleep(data->gpio, !data->enable_high);
-	data->is_enabled = false;
-
-	return 0;
-}
+	if (of_find_property(np, "vin-supply", NULL))
+		config->input_supply = "vin";
 
-static int fixed_voltage_enable_time(struct regulator_dev *dev)
-{
-	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
-
-	return data->startup_delay;
+	return config;
 }
 
 static int fixed_voltage_get_voltage(struct regulator_dev *dev)
@@ -151,15 +129,6 @@ static int fixed_voltage_list_voltage(struct regulator_dev *dev,
 	return data->microvolts;
 }
 
-static struct regulator_ops fixed_voltage_gpio_ops = {
-	.is_enabled = fixed_voltage_is_enabled,
-	.enable = fixed_voltage_enable,
-	.disable = fixed_voltage_disable,
-	.enable_time = fixed_voltage_enable_time,
-	.get_voltage = fixed_voltage_get_voltage,
-	.list_voltage = fixed_voltage_list_voltage,
-};
-
 static struct regulator_ops fixed_voltage_ops = {
 	.get_voltage = fixed_voltage_get_voltage,
 	.list_voltage = fixed_voltage_list_voltage,
@@ -172,10 +141,13 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	struct regulator_config cfg = { };
 	int ret;
 
-	if (pdev->dev.of_node)
+	if (pdev->dev.of_node) {
 		config = of_get_fixed_voltage_config(&pdev->dev);
-	else
+		if (IS_ERR(config))
+			return PTR_ERR(config);
+	} else {
 		config = pdev->dev.platform_data;
+	}
 
 	if (!config)
 		return -ENOMEM;
@@ -196,59 +168,44 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	}
 	drvdata->desc.type = REGULATOR_VOLTAGE;
 	drvdata->desc.owner = THIS_MODULE;
+	drvdata->desc.ops = &fixed_voltage_ops;
 
-	if (config->microvolts)
-		drvdata->desc.n_voltages = 1;
+	drvdata->desc.enable_time = config->startup_delay;
 
-	drvdata->microvolts = config->microvolts;
-	drvdata->gpio = config->gpio;
-	drvdata->startup_delay = config->startup_delay;
-
-	if (gpio_is_valid(config->gpio)) {
-		int gpio_flag;
-		drvdata->enable_high = config->enable_high;
-
-		/* FIXME: Remove below print warning
-		 *
-		 * config->gpio must be set to -EINVAL by platform code if
-		 * GPIO control is not required. However, early adopters
-		 * not requiring GPIO control may forget to initialize
-		 * config->gpio to -EINVAL. This will cause GPIO 0 to be used
-		 * for GPIO control.
-		 *
-		 * This warning will be removed once there are a couple of users
-		 * for this driver.
-		 */
-		if (!config->gpio)
-			dev_warn(&pdev->dev,
-				"using GPIO 0 for regulator enable control\n");
-
-		/*
-		 * set output direction without changing state
-		 * to prevent glitch
-		 */
-		drvdata->is_enabled = config->enabled_at_boot;
-		ret = drvdata->is_enabled ?
-				config->enable_high : !config->enable_high;
-		gpio_flag = ret ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
-
-		if (config->gpio_is_open_drain)
-			gpio_flag |= GPIOF_OPEN_DRAIN;
-
-		ret = gpio_request_one(config->gpio, gpio_flag,
-						config->supply_name);
-		if (ret) {
+	if (config->input_supply) {
+		drvdata->desc.supply_name = kstrdup(config->input_supply,
+							GFP_KERNEL);
+		if (!drvdata->desc.supply_name) {
 			dev_err(&pdev->dev,
-			   "Could not obtain regulator enable GPIO %d: %d\n",
-							config->gpio, ret);
+				"Failed to allocate input supply\n");
+			ret = -ENOMEM;
 			goto err_name;
 		}
+	}
+
+	if (config->microvolts)
+		drvdata->desc.n_voltages = 1;
 
-		drvdata->desc.ops = &fixed_voltage_gpio_ops;
+	drvdata->microvolts = config->microvolts;
 
+	if (config->gpio >= 0)
+		cfg.ena_gpio = config->gpio;
+	cfg.ena_gpio_invert = !config->enable_high;
+	if (config->enabled_at_boot) {
+		if (config->enable_high) {
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_HIGH;
+		} else {
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_LOW;
+		}
 	} else {
-		drvdata->desc.ops = &fixed_voltage_ops;
+		if (config->enable_high) {
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_LOW;
+		} else {
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_HIGH;
+		}
 	}
+	if (config->gpio_is_open_drain)
+		cfg.ena_gpio_flags |= GPIOF_OPEN_DRAIN;
 
 	cfg.dev = &pdev->dev;
 	cfg.init_data = config->init_data;
@@ -259,7 +216,7 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	if (IS_ERR(drvdata->dev)) {
 		ret = PTR_ERR(drvdata->dev);
 		dev_err(&pdev->dev, "Failed to register regulator: %d\n", ret);
-		goto err_gpio;
+		goto err_input;
 	}
 
 	platform_set_drvdata(pdev, drvdata);
@@ -269,9 +226,8 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_gpio:
-	if (gpio_is_valid(config->gpio))
-		gpio_free(config->gpio);
+err_input:
+	kfree(drvdata->desc.supply_name);
 err_name:
 	kfree(drvdata->desc.name);
 err:
@@ -283,8 +239,7 @@ static int __devexit reg_fixed_voltage_remove(struct platform_device *pdev)
 	struct fixed_voltage_data *drvdata = platform_get_drvdata(pdev);
 
 	regulator_unregister(drvdata->dev);
-	if (gpio_is_valid(drvdata->gpio))
-		gpio_free(drvdata->gpio);
+	kfree(drvdata->desc.supply_name);
 	kfree(drvdata->desc.name);
 
 	return 0;
@@ -296,8 +251,6 @@ static const struct of_device_id fixed_of_match[] __devinitconst = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, fixed_of_match);
-#else
-#define fixed_of_match NULL
 #endif
 
 static struct platform_driver regulator_fixed_voltage_driver = {
@@ -306,7 +259,7 @@ static struct platform_driver regulator_fixed_voltage_driver = {
 	.driver		= {
 		.name		= "reg-fixed-voltage",
 		.owner		= THIS_MODULE,
-		.of_match_table = fixed_of_match,
+		.of_match_table = of_match_ptr(fixed_of_match),
 	},
 };
 
diff --git a/drivers/regulator/gpio-regulator.c b/drivers/regulator/gpio-regulator.c
index 242851a4c1a..34b67bee932 100644
--- a/drivers/regulator/gpio-regulator.c
+++ b/drivers/regulator/gpio-regulator.c
@@ -36,11 +36,6 @@ struct gpio_regulator_data {
 	struct regulator_desc desc;
 	struct regulator_dev *dev;
 
-	int enable_gpio;
-	bool enable_high;
-	bool is_enabled;
-	unsigned startup_delay;
-
 	struct gpio *gpios;
 	int nr_gpios;
 
@@ -50,44 +45,6 @@ struct gpio_regulator_data {
 	int state;
 };
 
-static int gpio_regulator_is_enabled(struct regulator_dev *dev)
-{
-	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
-
-	return data->is_enabled;
-}
-
-static int gpio_regulator_enable(struct regulator_dev *dev)
-{
-	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
-
-	if (gpio_is_valid(data->enable_gpio)) {
-		gpio_set_value_cansleep(data->enable_gpio, data->enable_high);
-		data->is_enabled = true;
-	}
-
-	return 0;
-}
-
-static int gpio_regulator_disable(struct regulator_dev *dev)
-{
-	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
-
-	if (gpio_is_valid(data->enable_gpio)) {
-		gpio_set_value_cansleep(data->enable_gpio, !data->enable_high);
-		data->is_enabled = false;
-	}
-
-	return 0;
-}
-
-static int gpio_regulator_enable_time(struct regulator_dev *dev)
-{
-	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
-
-	return data->startup_delay;
-}
-
 static int gpio_regulator_get_value(struct regulator_dev *dev)
 {
 	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
@@ -153,20 +110,12 @@ static int gpio_regulator_set_current_limit(struct regulator_dev *dev,
 }
 
 static struct regulator_ops gpio_regulator_voltage_ops = {
-	.is_enabled = gpio_regulator_is_enabled,
-	.enable = gpio_regulator_enable,
-	.disable = gpio_regulator_disable,
-	.enable_time = gpio_regulator_enable_time,
 	.get_voltage = gpio_regulator_get_value,
 	.set_voltage = gpio_regulator_set_voltage,
 	.list_voltage = gpio_regulator_list_voltage,
 };
 
 static struct regulator_ops gpio_regulator_current_ops = {
-	.is_enabled = gpio_regulator_is_enabled,
-	.enable = gpio_regulator_enable,
-	.disable = gpio_regulator_disable,
-	.enable_time = gpio_regulator_enable_time,
 	.get_current_limit = gpio_regulator_get_value,
 	.set_current_limit = gpio_regulator_set_current_limit,
 };
@@ -213,6 +162,7 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 	drvdata->nr_states = config->nr_states;
 
 	drvdata->desc.owner = THIS_MODULE;
+	drvdata->desc.enable_time = config->startup_delay;
 
 	/* handle regulator type*/
 	switch (config->type) {
@@ -232,52 +182,12 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 		break;
 	}
 
-	drvdata->enable_gpio = config->enable_gpio;
-	drvdata->startup_delay = config->startup_delay;
-
-	if (gpio_is_valid(config->enable_gpio)) {
-		drvdata->enable_high = config->enable_high;
-
-		ret = gpio_request(config->enable_gpio, config->supply_name);
-		if (ret) {
-			dev_err(&pdev->dev,
-			   "Could not obtain regulator enable GPIO %d: %d\n",
-						config->enable_gpio, ret);
-			goto err_memstate;
-		}
-
-		/* set output direction without changing state
-		 * to prevent glitch
-		 */
-		if (config->enabled_at_boot) {
-			drvdata->is_enabled = true;
-			ret = gpio_direction_output(config->enable_gpio,
-						    config->enable_high);
-		} else {
-			drvdata->is_enabled = false;
-			ret = gpio_direction_output(config->enable_gpio,
-						    !config->enable_high);
-		}
-
-		if (ret) {
-			dev_err(&pdev->dev,
-			   "Could not configure regulator enable GPIO %d direction: %d\n",
-						config->enable_gpio, ret);
-			goto err_enablegpio;
-		}
-	} else {
-		/* Regulator without GPIO control is considered
-		 * always enabled
-		 */
-		drvdata->is_enabled = true;
-	}
-
 	drvdata->nr_gpios = config->nr_gpios;
 	ret = gpio_request_array(drvdata->gpios, drvdata->nr_gpios);
 	if (ret) {
 		dev_err(&pdev->dev,
 		   "Could not obtain regulator setting GPIOs: %d\n", ret);
-		goto err_enablegpio;
+		goto err_memstate;
 	}
 
 	/* build initial state from gpio init data. */
@@ -292,6 +202,21 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 	cfg.init_data = config->init_data;
 	cfg.driver_data = drvdata;
 
+	if (config->enable_gpio >= 0)
+		cfg.ena_gpio = config->enable_gpio;
+	cfg.ena_gpio_invert = !config->enable_high;
+	if (config->enabled_at_boot) {
+		if (config->enable_high)
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_HIGH;
+		else
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_LOW;
+	} else {
+		if (config->enable_high)
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_LOW;
+		else
+			cfg.ena_gpio_flags |= GPIOF_OUT_INIT_HIGH;
+	}
+
 	drvdata->dev = regulator_register(&drvdata->desc, &cfg);
 	if (IS_ERR(drvdata->dev)) {
 		ret = PTR_ERR(drvdata->dev);
@@ -305,9 +230,6 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 
 err_stategpio:
 	gpio_free_array(drvdata->gpios, drvdata->nr_gpios);
-err_enablegpio:
-	if (gpio_is_valid(config->enable_gpio))
-		gpio_free(config->enable_gpio);
 err_memstate:
 	kfree(drvdata->states);
 err_memgpio:
@@ -329,9 +251,6 @@ static int __devexit gpio_regulator_remove(struct platform_device *pdev)
 	kfree(drvdata->states);
 	kfree(drvdata->gpios);
 
-	if (gpio_is_valid(drvdata->enable_gpio))
-		gpio_free(drvdata->enable_gpio);
-
 	kfree(drvdata->desc.name);
 
 	return 0;
diff --git a/drivers/regulator/isl6271a-regulator.c b/drivers/regulator/isl6271a-regulator.c
index 56d273f2560..1d145a07ada 100644
--- a/drivers/regulator/isl6271a-regulator.c
+++ b/drivers/regulator/isl6271a-regulator.c
@@ -75,19 +75,12 @@ static struct regulator_ops isl_core_ops = {
 
 static int isl6271a_get_fixed_voltage(struct regulator_dev *dev)
 {
-	int id = rdev_get_id(dev);
-	return (id == 1) ? 1100000 : 1300000;
-}
-
-static int isl6271a_list_fixed_voltage(struct regulator_dev *dev, unsigned selector)
-{
-	int id = rdev_get_id(dev);
-	return (id == 1) ? 1100000 : 1300000;
+	return dev->desc->min_uV;
 }
 
 static struct regulator_ops isl_fixed_ops = {
 	.get_voltage	= isl6271a_get_fixed_voltage,
-	.list_voltage	= isl6271a_list_fixed_voltage,
+	.list_voltage	= regulator_list_voltage_linear,
 };
 
 static const struct regulator_desc isl_rd[] = {
@@ -107,6 +100,7 @@ static const struct regulator_desc isl_rd[] = {
 		.ops		= &isl_fixed_ops,
 		.type		= REGULATOR_VOLTAGE,
 		.owner		= THIS_MODULE,
+		.min_uV		= 1100000,
 	}, {
 		.name		= "LDO2",
 		.id		= 2,
@@ -114,6 +108,7 @@ static const struct regulator_desc isl_rd[] = {
 		.ops		= &isl_fixed_ops,
 		.type		= REGULATOR_VOLTAGE,
 		.owner		= THIS_MODULE,
+		.min_uV		= 1300000,
 	},
 };
 
diff --git a/drivers/regulator/lp3971.c b/drivers/regulator/lp3971.c
index 981bea9cb9d..7c6e3b8ff48 100644
--- a/drivers/regulator/lp3971.c
+++ b/drivers/regulator/lp3971.c
@@ -65,11 +65,11 @@ static const int buck_base_addr[] = {
 #define LP3971_BUCK_TARGET_VOL1_REG(x) (buck_base_addr[x])
 #define LP3971_BUCK_TARGET_VOL2_REG(x) (buck_base_addr[x]+1)
 
-static const int buck_voltage_map[] = {
-	   0,  800,  850,  900,  950, 1000, 1050, 1100,
-	1150, 1200, 1250, 1300, 1350, 1400, 1450, 1500,
-	1550, 1600, 1650, 1700, 1800, 1900, 2500, 2800,
-	3000, 3300,
+static const unsigned int buck_voltage_map[] = {
+	      0,  800000,  850000,  900000,  950000, 1000000, 1050000, 1100000,
+	1150000, 1200000, 1250000, 1300000, 1350000, 1400000, 1450000, 1500000,
+	1550000, 1600000, 1650000, 1700000, 1800000, 1900000, 2500000, 2800000,
+	3000000, 3300000,
 };
 
 #define BUCK_TARGET_VOL_MASK 0x3f
@@ -98,39 +98,19 @@ static const int buck_voltage_map[] = {
 #define LDO_VOL_CONTR_SHIFT(x) ((x & 1) << 2)
 #define LDO_VOL_CONTR_MASK 0x0f
 
-static const int ldo45_voltage_map[] = {
-	1000, 1050, 1100, 1150, 1200, 1250, 1300, 1350,
-	1400, 1500, 1800, 1900, 2500, 2800, 3000, 3300,
+static const unsigned int ldo45_voltage_map[] = {
+	1000000, 1050000, 1100000, 1150000, 1200000, 1250000, 1300000, 1350000,
+	1400000, 1500000, 1800000, 1900000, 2500000, 2800000, 3000000, 3300000,
 };
 
-static const int ldo123_voltage_map[] = {
-	1800, 1900, 2000, 2100, 2200, 2300, 2400, 2500,
-	2600, 2700, 2800, 2900, 3000, 3100, 3200, 3300,
+static const unsigned int ldo123_voltage_map[] = {
+	1800000, 1900000, 2000000, 2100000, 2200000, 2300000, 2400000, 2500000,
+	2600000, 2700000, 2800000, 2900000, 3000000, 3100000, 3200000, 3300000,
 };
 
-static const int *ldo_voltage_map[] = {
-	ldo123_voltage_map, /* LDO1 */
-	ldo123_voltage_map, /* LDO2 */
-	ldo123_voltage_map, /* LDO3 */
-	ldo45_voltage_map, /* LDO4 */
-	ldo45_voltage_map, /* LDO5 */
-};
-
-#define LDO_VOL_VALUE_MAP(x) (ldo_voltage_map[(x - LP3971_LDO1)])
-
 #define LDO_VOL_MIN_IDX 0x00
 #define LDO_VOL_MAX_IDX 0x0f
 
-static int lp3971_ldo_list_voltage(struct regulator_dev *dev, unsigned index)
-{
-	int ldo = rdev_get_id(dev) - LP3971_LDO1;
-
-	if (index > LDO_VOL_MAX_IDX)
-		return -EINVAL;
-
-	return 1000 * LDO_VOL_VALUE_MAP(ldo)[index];
-}
-
 static int lp3971_ldo_is_enabled(struct regulator_dev *dev)
 {
 	struct lp3971 *lp3971 = rdev_get_drvdata(dev);
@@ -169,7 +149,7 @@ static int lp3971_ldo_get_voltage(struct regulator_dev *dev)
 	reg = lp3971_reg_read(lp3971, LP3971_LDO_VOL_CONTR_REG(ldo));
 	val = (reg >> LDO_VOL_CONTR_SHIFT(ldo)) & LDO_VOL_CONTR_MASK;
 
-	return 1000 * LDO_VOL_VALUE_MAP(ldo)[val];
+	return dev->desc->volt_table[val];
 }
 
 static int lp3971_ldo_set_voltage_sel(struct regulator_dev *dev,
@@ -184,7 +164,7 @@ static int lp3971_ldo_set_voltage_sel(struct regulator_dev *dev,
 }
 
 static struct regulator_ops lp3971_ldo_ops = {
-	.list_voltage = lp3971_ldo_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.is_enabled = lp3971_ldo_is_enabled,
 	.enable = lp3971_ldo_enable,
 	.disable = lp3971_ldo_disable,
@@ -192,14 +172,6 @@ static struct regulator_ops lp3971_ldo_ops = {
 	.set_voltage_sel = lp3971_ldo_set_voltage_sel,
 };
 
-static int lp3971_dcdc_list_voltage(struct regulator_dev *dev, unsigned index)
-{
-	if (index < BUCK_TARGET_VOL_MIN_IDX || index > BUCK_TARGET_VOL_MAX_IDX)
-		return -EINVAL;
-
-	return 1000 * buck_voltage_map[index];
-}
-
 static int lp3971_dcdc_is_enabled(struct regulator_dev *dev)
 {
 	struct lp3971 *lp3971 = rdev_get_drvdata(dev);
@@ -240,7 +212,7 @@ static int lp3971_dcdc_get_voltage(struct regulator_dev *dev)
 	reg &= BUCK_TARGET_VOL_MASK;
 
 	if (reg <= BUCK_TARGET_VOL_MAX_IDX)
-		val = 1000 * buck_voltage_map[reg];
+		val = buck_voltage_map[reg];
 	else {
 		val = 0;
 		dev_warn(&dev->dev, "chip reported incorrect voltage value.\n");
@@ -273,7 +245,7 @@ static int lp3971_dcdc_set_voltage_sel(struct regulator_dev *dev,
 }
 
 static struct regulator_ops lp3971_dcdc_ops = {
-	.list_voltage = lp3971_dcdc_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.is_enabled = lp3971_dcdc_is_enabled,
 	.enable = lp3971_dcdc_enable,
 	.disable = lp3971_dcdc_disable,
@@ -287,6 +259,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_LDO1,
 		.ops = &lp3971_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo123_voltage_map),
+		.volt_table = ldo123_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -295,6 +268,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_LDO2,
 		.ops = &lp3971_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo123_voltage_map),
+		.volt_table = ldo123_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -303,6 +277,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_LDO3,
 		.ops = &lp3971_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo123_voltage_map),
+		.volt_table = ldo123_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -311,6 +286,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_LDO4,
 		.ops = &lp3971_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo45_voltage_map),
+		.volt_table = ldo45_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -319,6 +295,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_LDO5,
 		.ops = &lp3971_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo45_voltage_map),
+		.volt_table = ldo45_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -327,6 +304,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_DCDC1,
 		.ops = &lp3971_dcdc_ops,
 		.n_voltages = ARRAY_SIZE(buck_voltage_map),
+		.volt_table = buck_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -335,6 +313,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_DCDC2,
 		.ops = &lp3971_dcdc_ops,
 		.n_voltages = ARRAY_SIZE(buck_voltage_map),
+		.volt_table = buck_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -343,6 +322,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3971_DCDC3,
 		.ops = &lp3971_dcdc_ops,
 		.n_voltages = ARRAY_SIZE(buck_voltage_map),
+		.volt_table = buck_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
diff --git a/drivers/regulator/lp3972.c b/drivers/regulator/lp3972.c
index de073df7d34..3cdc755d9b2 100644
--- a/drivers/regulator/lp3972.c
+++ b/drivers/regulator/lp3972.c
@@ -74,54 +74,40 @@ struct lp3972 {
 #define LP3972_OVER2_LDO4_EN	BIT(4)
 #define LP3972_OVER1_S_EN	BIT(2)
 
-static const int ldo1_voltage_map[] = {
-	1700, 1725, 1750, 1775, 1800, 1825, 1850, 1875,
-	1900, 1925, 1950, 1975, 2000,
+static const unsigned int ldo1_voltage_map[] = {
+	1700000, 1725000, 1750000, 1775000, 1800000, 1825000, 1850000, 1875000,
+	1900000, 1925000, 1950000, 1975000, 2000000,
 };
 
-static const int ldo23_voltage_map[] = {
-	1800, 1900, 2000, 2100, 2200, 2300, 2400, 2500,
-	2600, 2700, 2800, 2900, 3000, 3100, 3200, 3300,
+static const unsigned int ldo23_voltage_map[] = {
+	1800000, 1900000, 2000000, 2100000, 2200000, 2300000, 2400000, 2500000,
+	2600000, 2700000, 2800000, 2900000, 3000000, 3100000, 3200000, 3300000,
 };
 
-static const int ldo4_voltage_map[] = {
-	1000, 1050, 1100, 1150, 1200, 1250, 1300, 1350,
-	1400, 1500, 1800, 1900, 2500, 2800, 3000, 3300,
+static const unsigned int ldo4_voltage_map[] = {
+	1000000, 1050000, 1100000, 1150000, 1200000, 1250000, 1300000, 1350000,
+	1400000, 1500000, 1800000, 1900000, 2500000, 2800000, 3000000, 3300000,
 };
 
-static const int ldo5_voltage_map[] = {
-	   0,    0,    0,    0,    0,  850,  875,  900,
-	 925,  950,  975, 1000, 1025, 1050, 1075, 1100,
-	1125, 1150, 1175, 1200, 1225, 1250, 1275, 1300,
-	1325, 1350, 1375, 1400, 1425, 1450, 1475, 1500,
+static const unsigned int ldo5_voltage_map[] = {
+	      0,       0,       0,       0,       0,  850000,  875000,  900000,
+	 925000,  950000,  975000, 1000000, 1025000, 1050000, 1075000, 1100000,
+	1125000, 1150000, 1175000, 1200000, 1225000, 1250000, 1275000, 1300000,
+	1325000, 1350000, 1375000, 1400000, 1425000, 1450000, 1475000, 1500000,
 };
 
-static const int buck1_voltage_map[] = {
-	 725,  750,  775,  800,  825,  850,  875,  900,
-	 925,  950,  975, 1000, 1025, 1050, 1075, 1100,
-	1125, 1150, 1175, 1200, 1225, 1250, 1275, 1300,
-	1325, 1350, 1375, 1400, 1425, 1450, 1475, 1500,
+static const unsigned int buck1_voltage_map[] = {
+	 725000,  750000,  775000,  800000,  825000,  850000,  875000,  900000,
+	 925000,  950000,  975000, 1000000, 1025000, 1050000, 1075000, 1100000,
+	1125000, 1150000, 1175000, 1200000, 1225000, 1250000, 1275000, 1300000,
+	1325000, 1350000, 1375000, 1400000, 1425000, 1450000, 1475000, 1500000,
 };
 
-static const int buck23_voltage_map[] = {
-	   0,  800,  850,  900,  950, 1000, 1050, 1100,
-	1150, 1200, 1250, 1300, 1350, 1400, 1450, 1500,
-	1550, 1600, 1650, 1700, 1800, 1900, 2500, 2800,
-	3000, 3300,
-};
-
-static const int *ldo_voltage_map[] = {
-	ldo1_voltage_map,
-	ldo23_voltage_map,
-	ldo23_voltage_map,
-	ldo4_voltage_map,
-	ldo5_voltage_map,
-};
-
-static const int *buck_voltage_map[] = {
-	buck1_voltage_map,
-	buck23_voltage_map,
-	buck23_voltage_map,
+static const unsigned int buck23_voltage_map[] = {
+	      0,  800000,  850000,  900000,  950000, 1000000, 1050000, 1100000,
+	1150000, 1200000, 1250000, 1300000, 1350000, 1400000, 1450000, 1500000,
+	1550000, 1600000, 1650000, 1700000, 1800000, 1900000, 2500000, 2800000,
+	3000000, 3300000,
 };
 
 static const int ldo_output_enable_mask[] = {
@@ -160,7 +146,6 @@ static const int buck_base_addr[] = {
 	LP3972_B3TV_REG,
 };
 
-#define LP3972_LDO_VOL_VALUE_MAP(x) (ldo_voltage_map[x])
 #define LP3972_LDO_OUTPUT_ENABLE_MASK(x) (ldo_output_enable_mask[x])
 #define LP3972_LDO_OUTPUT_ENABLE_REG(x) (ldo_output_enable_addr[x])
 
@@ -177,7 +162,6 @@ static const int buck_base_addr[] = {
 #define LP3972_LDO_VOL_MIN_IDX(x) (((x) == 4) ? 0x05 : 0x00)
 #define LP3972_LDO_VOL_MAX_IDX(x) ((x) ? (((x) == 4) ? 0x1f : 0x0f) : 0x0c)
 
-#define LP3972_BUCK_VOL_VALUE_MAP(x) (buck_voltage_map[x])
 #define LP3972_BUCK_VOL_ENABLE_REG(x) (buck_vol_enable_addr[x])
 #define LP3972_BUCK_VOL1_REG(x) (buck_base_addr[x])
 #define LP3972_BUCK_VOL_MASK 0x1f
@@ -242,17 +226,6 @@ static int lp3972_set_bits(struct lp3972 *lp3972, u8 reg, u16 mask, u16 val)
 	return ret;
 }
 
-static int lp3972_ldo_list_voltage(struct regulator_dev *dev, unsigned index)
-{
-	int ldo = rdev_get_id(dev) - LP3972_LDO1;
-
-	if (index < LP3972_LDO_VOL_MIN_IDX(ldo) ||
-	    index > LP3972_LDO_VOL_MAX_IDX(ldo))
-		return -EINVAL;
-
-	return 1000 * LP3972_LDO_VOL_VALUE_MAP(ldo)[index];
-}
-
 static int lp3972_ldo_is_enabled(struct regulator_dev *dev)
 {
 	struct lp3972 *lp3972 = rdev_get_drvdata(dev);
@@ -294,7 +267,7 @@ static int lp3972_ldo_get_voltage(struct regulator_dev *dev)
 	reg = lp3972_reg_read(lp3972, LP3972_LDO_VOL_CONTR_REG(ldo));
 	val = (reg >> LP3972_LDO_VOL_CONTR_SHIFT(ldo)) & mask;
 
-	return 1000 * LP3972_LDO_VOL_VALUE_MAP(ldo)[val];
+	return dev->desc->volt_table[val];
 }
 
 static int lp3972_ldo_set_voltage_sel(struct regulator_dev *dev,
@@ -337,7 +310,7 @@ static int lp3972_ldo_set_voltage_sel(struct regulator_dev *dev,
 }
 
 static struct regulator_ops lp3972_ldo_ops = {
-	.list_voltage = lp3972_ldo_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.is_enabled = lp3972_ldo_is_enabled,
 	.enable = lp3972_ldo_enable,
 	.disable = lp3972_ldo_disable,
@@ -345,17 +318,6 @@ static struct regulator_ops lp3972_ldo_ops = {
 	.set_voltage_sel = lp3972_ldo_set_voltage_sel,
 };
 
-static int lp3972_dcdc_list_voltage(struct regulator_dev *dev, unsigned index)
-{
-	int buck = rdev_get_id(dev) - LP3972_DCDC1;
-
-	if (index < LP3972_BUCK_VOL_MIN_IDX(buck) ||
-	    index > LP3972_BUCK_VOL_MAX_IDX(buck))
-		return -EINVAL;
-
-	return 1000 * buck_voltage_map[buck][index];
-}
-
 static int lp3972_dcdc_is_enabled(struct regulator_dev *dev)
 {
 	struct lp3972 *lp3972 = rdev_get_drvdata(dev);
@@ -401,7 +363,7 @@ static int lp3972_dcdc_get_voltage(struct regulator_dev *dev)
 	reg = lp3972_reg_read(lp3972, LP3972_BUCK_VOL1_REG(buck));
 	reg &= LP3972_BUCK_VOL_MASK;
 	if (reg <= LP3972_BUCK_VOL_MAX_IDX(buck))
-		val = 1000 * buck_voltage_map[buck][reg];
+		val = dev->desc->volt_table[reg];
 	else {
 		val = 0;
 		dev_warn(&dev->dev, "chip reported incorrect voltage value."
@@ -436,7 +398,7 @@ static int lp3972_dcdc_set_voltage_sel(struct regulator_dev *dev,
 }
 
 static struct regulator_ops lp3972_dcdc_ops = {
-	.list_voltage = lp3972_dcdc_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.is_enabled = lp3972_dcdc_is_enabled,
 	.enable = lp3972_dcdc_enable,
 	.disable = lp3972_dcdc_disable,
@@ -450,6 +412,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_LDO1,
 		.ops = &lp3972_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo1_voltage_map),
+		.volt_table = ldo1_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -458,6 +421,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_LDO2,
 		.ops = &lp3972_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo23_voltage_map),
+		.volt_table = ldo23_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -466,6 +430,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_LDO3,
 		.ops = &lp3972_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo23_voltage_map),
+		.volt_table = ldo23_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -474,6 +439,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_LDO4,
 		.ops = &lp3972_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo4_voltage_map),
+		.volt_table = ldo4_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -482,6 +448,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_LDO5,
 		.ops = &lp3972_ldo_ops,
 		.n_voltages = ARRAY_SIZE(ldo5_voltage_map),
+		.volt_table = ldo5_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -490,6 +457,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_DCDC1,
 		.ops = &lp3972_dcdc_ops,
 		.n_voltages = ARRAY_SIZE(buck1_voltage_map),
+		.volt_table = buck1_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -498,6 +466,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_DCDC2,
 		.ops = &lp3972_dcdc_ops,
 		.n_voltages = ARRAY_SIZE(buck23_voltage_map),
+		.volt_table = buck23_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
@@ -506,6 +475,7 @@ static const struct regulator_desc regulators[] = {
 		.id = LP3972_DCDC3,
 		.ops = &lp3972_dcdc_ops,
 		.n_voltages = ARRAY_SIZE(buck23_voltage_map),
+		.volt_table = buck23_voltage_map,
 		.type = REGULATOR_VOLTAGE,
 		.owner = THIS_MODULE,
 	},
diff --git a/drivers/regulator/lp872x.c b/drivers/regulator/lp872x.c
new file mode 100644
index 00000000000..212c38eaba7
--- /dev/null
+++ b/drivers/regulator/lp872x.c
@@ -0,0 +1,943 @@
+/*
+ * Copyright 2012 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/regulator/lp872x.h>
+#include <linux/regulator/driver.h>
+#include <linux/platform_device.h>
+
+/* Registers : LP8720/8725 shared */
+#define LP872X_GENERAL_CFG		0x00
+#define LP872X_LDO1_VOUT		0x01
+#define LP872X_LDO2_VOUT		0x02
+#define LP872X_LDO3_VOUT		0x03
+#define LP872X_LDO4_VOUT		0x04
+#define LP872X_LDO5_VOUT		0x05
+
+/* Registers : LP8720 */
+#define LP8720_BUCK_VOUT1		0x06
+#define LP8720_BUCK_VOUT2		0x07
+#define LP8720_ENABLE			0x08
+
+/* Registers : LP8725 */
+#define LP8725_LILO1_VOUT		0x06
+#define LP8725_LILO2_VOUT		0x07
+#define LP8725_BUCK1_VOUT1		0x08
+#define LP8725_BUCK1_VOUT2		0x09
+#define LP8725_BUCK2_VOUT1		0x0A
+#define LP8725_BUCK2_VOUT2		0x0B
+#define LP8725_BUCK_CTRL		0x0C
+#define LP8725_LDO_CTRL			0x0D
+
+/* Mask/shift : LP8720/LP8725 shared */
+#define LP872X_VOUT_M			0x1F
+#define LP872X_START_DELAY_M		0xE0
+#define LP872X_START_DELAY_S		5
+#define LP872X_EN_LDO1_M		BIT(0)
+#define LP872X_EN_LDO2_M		BIT(1)
+#define LP872X_EN_LDO3_M		BIT(2)
+#define LP872X_EN_LDO4_M		BIT(3)
+#define LP872X_EN_LDO5_M		BIT(4)
+
+/* Mask/shift : LP8720 */
+#define LP8720_TIMESTEP_S		0		/* Addr 00h */
+#define LP8720_TIMESTEP_M		BIT(0)
+#define LP8720_EXT_DVS_M		BIT(2)
+#define LP8720_BUCK_FPWM_S		5		/* Addr 07h */
+#define LP8720_BUCK_FPWM_M		BIT(5)
+#define LP8720_EN_BUCK_M		BIT(5)		/* Addr 08h */
+#define LP8720_DVS_SEL_M		BIT(7)
+
+/* Mask/shift : LP8725 */
+#define LP8725_TIMESTEP_M		0xC0		/* Addr 00h */
+#define LP8725_TIMESTEP_S		6
+#define LP8725_BUCK1_EN_M		BIT(0)
+#define LP8725_DVS1_M			BIT(2)
+#define LP8725_DVS2_M			BIT(3)
+#define LP8725_BUCK2_EN_M		BIT(4)
+#define LP8725_BUCK_CL_M		0xC0		/* Addr 09h, 0Bh */
+#define LP8725_BUCK_CL_S		6
+#define LP8725_BUCK1_FPWM_S		1		/* Addr 0Ch */
+#define LP8725_BUCK1_FPWM_M		BIT(1)
+#define LP8725_BUCK2_FPWM_S		5
+#define LP8725_BUCK2_FPWM_M		BIT(5)
+#define LP8725_EN_LILO1_M		BIT(5)		/* Addr 0Dh */
+#define LP8725_EN_LILO2_M		BIT(6)
+
+/* PWM mode */
+#define LP872X_FORCE_PWM		1
+#define LP872X_AUTO_PWM			0
+
+#define LP8720_NUM_REGULATORS		6
+#define LP8725_NUM_REGULATORS		9
+#define EXTERN_DVS_USED			0
+#define MAX_DELAY			6
+
+/* dump registers in regmap-debugfs */
+#define MAX_REGISTERS			0x0F
+
+enum lp872x_id {
+	LP8720,
+	LP8725,
+};
+
+struct lp872x {
+	struct regmap *regmap;
+	struct device *dev;
+	enum lp872x_id chipid;
+	struct lp872x_platform_data *pdata;
+	struct regulator_dev **regulators;
+	int num_regulators;
+	enum lp872x_dvs_state dvs_pin;
+	int dvs_gpio;
+};
+
+/* LP8720/LP8725 shared voltage table for LDOs */
+static const unsigned int lp872x_ldo_vtbl[] = {
+	1200000, 1250000, 1300000, 1350000, 1400000, 1450000, 1500000, 1550000,
+	1600000, 1650000, 1700000, 1750000, 1800000, 1850000, 1900000, 2000000,
+	2100000, 2200000, 2300000, 2400000, 2500000, 2600000, 2650000, 2700000,
+	2750000, 2800000, 2850000, 2900000, 2950000, 3000000, 3100000, 3300000,
+};
+
+/* LP8720 LDO4 voltage table */
+static const unsigned int lp8720_ldo4_vtbl[] = {
+	 800000,  850000,  900000, 1000000, 1100000, 1200000, 1250000, 1300000,
+	1350000, 1400000, 1450000, 1500000, 1550000, 1600000, 1650000, 1700000,
+	1750000, 1800000, 1850000, 1900000, 2000000, 2100000, 2200000, 2300000,
+	2400000, 2500000, 2600000, 2650000, 2700000, 2750000, 2800000, 2850000,
+};
+
+/* LP8725 LILO(Low Input Low Output) voltage table */
+static const unsigned int lp8725_lilo_vtbl[] = {
+	 800000,  850000,  900000,  950000, 1000000, 1050000, 1100000, 1150000,
+	1200000, 1250000, 1300000, 1350000, 1400000, 1500000, 1600000, 1700000,
+	1800000, 1900000, 2000000, 2100000, 2200000, 2300000, 2400000, 2500000,
+	2600000, 2700000, 2800000, 2850000, 2900000, 3000000, 3100000, 3300000,
+};
+
+/* LP8720 BUCK voltage table */
+#define EXT_R		0	/* external resistor divider */
+static const unsigned int lp8720_buck_vtbl[] = {
+	  EXT_R,  800000,  850000,  900000,  950000, 1000000, 1050000, 1100000,
+	1150000, 1200000, 1250000, 1300000, 1350000, 1400000, 1450000, 1500000,
+	1550000, 1600000, 1650000, 1700000, 1750000, 1800000, 1850000, 1900000,
+	1950000, 2000000, 2050000, 2100000, 2150000, 2200000, 2250000, 2300000,
+};
+
+/* LP8725 BUCK voltage table */
+static const unsigned int lp8725_buck_vtbl[] = {
+	 800000,  850000,  900000,  950000, 1000000, 1050000, 1100000, 1150000,
+	1200000, 1250000, 1300000, 1350000, 1400000, 1500000, 1600000, 1700000,
+	1750000, 1800000, 1850000, 1900000, 2000000, 2100000, 2200000, 2300000,
+	2400000, 2500000, 2600000, 2700000, 2800000, 2850000, 2900000, 3000000,
+};
+
+/* LP8725 BUCK current limit */
+static const unsigned int lp8725_buck_uA[] = {
+	460000, 780000, 1050000, 1370000,
+};
+
+static int lp872x_read_byte(struct lp872x *lp, u8 addr, u8 *data)
+{
+	int ret;
+	unsigned int val;
+
+	ret = regmap_read(lp->regmap, addr, &val);
+	if (ret < 0) {
+		dev_err(lp->dev, "failed to read 0x%.2x\n", addr);
+		return ret;
+	}
+
+	*data = (u8)val;
+	return 0;
+}
+
+static inline int lp872x_write_byte(struct lp872x *lp, u8 addr, u8 data)
+{
+	return regmap_write(lp->regmap, addr, data);
+}
+
+static inline int lp872x_update_bits(struct lp872x *lp, u8 addr,
+				unsigned int mask, u8 data)
+{
+	return regmap_update_bits(lp->regmap, addr, mask, data);
+}
+
+static int _rdev_to_offset(struct regulator_dev *rdev)
+{
+	enum lp872x_regulator_id id = rdev_get_id(rdev);
+
+	switch (id) {
+	case LP8720_ID_LDO1 ... LP8720_ID_BUCK:
+		return id;
+	case LP8725_ID_LDO1 ... LP8725_ID_BUCK2:
+		return id - LP8725_ID_BASE;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int lp872x_get_timestep_usec(struct lp872x *lp)
+{
+	enum lp872x_id chip = lp->chipid;
+	u8 val, mask, shift;
+	int *time_usec, size, ret;
+	int lp8720_time_usec[] = { 25, 50 };
+	int lp8725_time_usec[] = { 32, 64, 128, 256 };
+
+	switch (chip) {
+	case LP8720:
+		mask = LP8720_TIMESTEP_M;
+		shift = LP8720_TIMESTEP_S;
+		time_usec = &lp8720_time_usec[0];
+		size = ARRAY_SIZE(lp8720_time_usec);
+		break;
+	case LP8725:
+		mask = LP8725_TIMESTEP_M;
+		shift = LP8725_TIMESTEP_S;
+		time_usec = &lp8725_time_usec[0];
+		size = ARRAY_SIZE(lp8725_time_usec);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = lp872x_read_byte(lp, LP872X_GENERAL_CFG, &val);
+	if (ret)
+		return -EINVAL;
+
+	val = (val & mask) >> shift;
+	if (val >= size)
+		return -EINVAL;
+
+	return *(time_usec + val);
+}
+
+static int lp872x_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id regulator = rdev_get_id(rdev);
+	int time_step_us = lp872x_get_timestep_usec(lp);
+	int ret, offset;
+	u8 addr, val;
+
+	if (time_step_us < 0)
+		return -EINVAL;
+
+	switch (regulator) {
+	case LP8720_ID_LDO1 ... LP8720_ID_LDO5:
+	case LP8725_ID_LDO1 ... LP8725_ID_LILO2:
+		offset = _rdev_to_offset(rdev);
+		if (offset < 0)
+			return -EINVAL;
+
+		addr = LP872X_LDO1_VOUT + offset;
+		break;
+	case LP8720_ID_BUCK:
+		addr = LP8720_BUCK_VOUT1;
+		break;
+	case LP8725_ID_BUCK1:
+		addr = LP8725_BUCK1_VOUT1;
+		break;
+	case LP8725_ID_BUCK2:
+		addr = LP8725_BUCK2_VOUT1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = lp872x_read_byte(lp, addr, &val);
+	if (ret)
+		return ret;
+
+	val = (val & LP872X_START_DELAY_M) >> LP872X_START_DELAY_S;
+
+	return val > MAX_DELAY ? 0 : val * time_step_us;
+}
+
+static void lp872x_set_dvs(struct lp872x *lp, int gpio)
+{
+	enum lp872x_dvs_sel dvs_sel = lp->pdata->dvs->vsel;
+	enum lp872x_dvs_state state;
+
+	state = dvs_sel == SEL_V1 ? DVS_HIGH : DVS_LOW;
+	gpio_set_value(gpio, state);
+	lp->dvs_pin = state;
+}
+
+static u8 lp872x_select_buck_vout_addr(struct lp872x *lp,
+				enum lp872x_regulator_id buck)
+{
+	u8 val, addr;
+
+	if (lp872x_read_byte(lp, LP872X_GENERAL_CFG, &val))
+		return 0;
+
+	switch (buck) {
+	case LP8720_ID_BUCK:
+		if (val & LP8720_EXT_DVS_M) {
+			addr = (lp->dvs_pin == DVS_HIGH) ?
+				LP8720_BUCK_VOUT1 : LP8720_BUCK_VOUT2;
+		} else {
+			if (lp872x_read_byte(lp, LP8720_ENABLE, &val))
+				return 0;
+
+			addr = val & LP8720_DVS_SEL_M ?
+				LP8720_BUCK_VOUT1 : LP8720_BUCK_VOUT2;
+		}
+		break;
+	case LP8725_ID_BUCK1:
+		if (val & LP8725_DVS1_M)
+			addr = LP8725_BUCK1_VOUT1;
+		else
+			addr = (lp->dvs_pin == DVS_HIGH) ?
+				LP8725_BUCK1_VOUT1 : LP8725_BUCK1_VOUT2;
+		break;
+	case LP8725_ID_BUCK2:
+		addr =  val & LP8725_DVS2_M ?
+			LP8725_BUCK2_VOUT1 : LP8725_BUCK2_VOUT2;
+		break;
+	default:
+		return 0;
+	}
+
+	return addr;
+}
+
+static bool lp872x_is_valid_buck_addr(u8 addr)
+{
+	switch (addr) {
+	case LP8720_BUCK_VOUT1:
+	case LP8720_BUCK_VOUT2:
+	case LP8725_BUCK1_VOUT1:
+	case LP8725_BUCK1_VOUT2:
+	case LP8725_BUCK2_VOUT1:
+	case LP8725_BUCK2_VOUT2:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static int lp872x_buck_set_voltage_sel(struct regulator_dev *rdev,
+					unsigned selector)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id buck = rdev_get_id(rdev);
+	u8 addr, mask = LP872X_VOUT_M;
+	struct lp872x_dvs *dvs = lp->pdata->dvs;
+
+	if (dvs && gpio_is_valid(dvs->gpio))
+		lp872x_set_dvs(lp, dvs->gpio);
+
+	addr = lp872x_select_buck_vout_addr(lp, buck);
+	if (!lp872x_is_valid_buck_addr(addr))
+		return -EINVAL;
+
+	return lp872x_update_bits(lp, addr, mask, selector);
+}
+
+static int lp872x_buck_get_voltage_sel(struct regulator_dev *rdev)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id buck = rdev_get_id(rdev);
+	u8 addr, val;
+	int ret;
+
+	addr = lp872x_select_buck_vout_addr(lp, buck);
+	if (!lp872x_is_valid_buck_addr(addr))
+		return -EINVAL;
+
+	ret = lp872x_read_byte(lp, addr, &val);
+	if (ret)
+		return ret;
+
+	return val & LP872X_VOUT_M;
+}
+
+static int lp8725_buck_set_current_limit(struct regulator_dev *rdev,
+					int min_uA, int max_uA)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id buck = rdev_get_id(rdev);
+	int i, max = ARRAY_SIZE(lp8725_buck_uA);
+	u8 addr, val;
+
+	switch (buck) {
+	case LP8725_ID_BUCK1:
+		addr = LP8725_BUCK1_VOUT2;
+		break;
+	case LP8725_ID_BUCK2:
+		addr = LP8725_BUCK2_VOUT2;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	for (i = 0 ; i < max ; i++)
+		if (lp8725_buck_uA[i] >= min_uA &&
+			lp8725_buck_uA[i] <= max_uA)
+			break;
+
+	if (i == max)
+		return -EINVAL;
+
+	val = i << LP8725_BUCK_CL_S;
+
+	return lp872x_update_bits(lp, addr, LP8725_BUCK_CL_M, val);
+}
+
+static int lp8725_buck_get_current_limit(struct regulator_dev *rdev)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id buck = rdev_get_id(rdev);
+	u8 addr, val;
+	int ret;
+
+	switch (buck) {
+	case LP8725_ID_BUCK1:
+		addr = LP8725_BUCK1_VOUT2;
+		break;
+	case LP8725_ID_BUCK2:
+		addr = LP8725_BUCK2_VOUT2;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = lp872x_read_byte(lp, addr, &val);
+	if (ret)
+		return ret;
+
+	val = (val & LP8725_BUCK_CL_M) >> LP8725_BUCK_CL_S;
+
+	return (val < ARRAY_SIZE(lp8725_buck_uA)) ?
+			lp8725_buck_uA[val] : -EINVAL;
+}
+
+static int lp872x_buck_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id buck = rdev_get_id(rdev);
+	u8 addr, mask, shift, val;
+
+	switch (buck) {
+	case LP8720_ID_BUCK:
+		addr = LP8720_BUCK_VOUT2;
+		mask = LP8720_BUCK_FPWM_M;
+		shift = LP8720_BUCK_FPWM_S;
+		break;
+	case LP8725_ID_BUCK1:
+		addr = LP8725_BUCK_CTRL;
+		mask = LP8725_BUCK1_FPWM_M;
+		shift = LP8725_BUCK1_FPWM_S;
+		break;
+	case LP8725_ID_BUCK2:
+		addr = LP8725_BUCK_CTRL;
+		mask = LP8725_BUCK2_FPWM_M;
+		shift = LP8725_BUCK2_FPWM_S;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (mode == REGULATOR_MODE_FAST)
+		val = LP872X_FORCE_PWM << shift;
+	else if (mode == REGULATOR_MODE_NORMAL)
+		val = LP872X_AUTO_PWM << shift;
+	else
+		return -EINVAL;
+
+	return lp872x_update_bits(lp, addr, mask, val);
+}
+
+static unsigned int lp872x_buck_get_mode(struct regulator_dev *rdev)
+{
+	struct lp872x *lp = rdev_get_drvdata(rdev);
+	enum lp872x_regulator_id buck = rdev_get_id(rdev);
+	u8 addr, mask, val;
+	int ret;
+
+	switch (buck) {
+	case LP8720_ID_BUCK:
+		addr = LP8720_BUCK_VOUT2;
+		mask = LP8720_BUCK_FPWM_M;
+		break;
+	case LP8725_ID_BUCK1:
+		addr = LP8725_BUCK_CTRL;
+		mask = LP8725_BUCK1_FPWM_M;
+		break;
+	case LP8725_ID_BUCK2:
+		addr = LP8725_BUCK_CTRL;
+		mask = LP8725_BUCK2_FPWM_M;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = lp872x_read_byte(lp, addr, &val);
+	if (ret)
+		return ret;
+
+	return val & mask ? REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
+}
+
+static struct regulator_ops lp872x_ldo_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable_time = lp872x_regulator_enable_time,
+};
+
+static struct regulator_ops lp8720_buck_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = lp872x_buck_set_voltage_sel,
+	.get_voltage_sel = lp872x_buck_get_voltage_sel,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable_time = lp872x_regulator_enable_time,
+	.set_mode = lp872x_buck_set_mode,
+	.get_mode = lp872x_buck_get_mode,
+};
+
+static struct regulator_ops lp8725_buck_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = lp872x_buck_set_voltage_sel,
+	.get_voltage_sel = lp872x_buck_get_voltage_sel,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable_time = lp872x_regulator_enable_time,
+	.set_mode = lp872x_buck_set_mode,
+	.get_mode = lp872x_buck_get_mode,
+	.set_current_limit = lp8725_buck_set_current_limit,
+	.get_current_limit = lp8725_buck_get_current_limit,
+};
+
+static struct regulator_desc lp8720_regulator_desc[] = {
+	{
+		.name = "ldo1",
+		.id = LP8720_ID_LDO1,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO1_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8720_ENABLE,
+		.enable_mask = LP872X_EN_LDO1_M,
+	},
+	{
+		.name = "ldo2",
+		.id = LP8720_ID_LDO2,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO2_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8720_ENABLE,
+		.enable_mask = LP872X_EN_LDO2_M,
+	},
+	{
+		.name = "ldo3",
+		.id = LP8720_ID_LDO3,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO3_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8720_ENABLE,
+		.enable_mask = LP872X_EN_LDO3_M,
+	},
+	{
+		.name = "ldo4",
+		.id = LP8720_ID_LDO4,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp8720_ldo4_vtbl),
+		.volt_table = lp8720_ldo4_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO4_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8720_ENABLE,
+		.enable_mask = LP872X_EN_LDO4_M,
+	},
+	{
+		.name = "ldo5",
+		.id = LP8720_ID_LDO5,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO5_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8720_ENABLE,
+		.enable_mask = LP872X_EN_LDO5_M,
+	},
+	{
+		.name = "buck",
+		.id = LP8720_ID_BUCK,
+		.ops = &lp8720_buck_ops,
+		.n_voltages = ARRAY_SIZE(lp8720_buck_vtbl),
+		.volt_table = lp8720_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8720_ENABLE,
+		.enable_mask = LP8720_EN_BUCK_M,
+	},
+};
+
+static struct regulator_desc lp8725_regulator_desc[] = {
+	{
+		.name = "ldo1",
+		.id = LP8725_ID_LDO1,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO1_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP872X_EN_LDO1_M,
+	},
+	{
+		.name = "ldo2",
+		.id = LP8725_ID_LDO2,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO2_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP872X_EN_LDO2_M,
+	},
+	{
+		.name = "ldo3",
+		.id = LP8725_ID_LDO3,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO3_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP872X_EN_LDO3_M,
+	},
+	{
+		.name = "ldo4",
+		.id = LP8725_ID_LDO4,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO4_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP872X_EN_LDO4_M,
+	},
+	{
+		.name = "ldo5",
+		.id = LP8725_ID_LDO5,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
+		.volt_table = lp872x_ldo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP872X_LDO5_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP872X_EN_LDO5_M,
+	},
+	{
+		.name = "lilo1",
+		.id = LP8725_ID_LILO1,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp8725_lilo_vtbl),
+		.volt_table = lp8725_lilo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8725_LILO1_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP8725_EN_LILO1_M,
+	},
+	{
+		.name = "lilo2",
+		.id = LP8725_ID_LILO2,
+		.ops = &lp872x_ldo_ops,
+		.n_voltages = ARRAY_SIZE(lp8725_lilo_vtbl),
+		.volt_table = lp8725_lilo_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8725_LILO2_VOUT,
+		.vsel_mask = LP872X_VOUT_M,
+		.enable_reg = LP8725_LDO_CTRL,
+		.enable_mask = LP8725_EN_LILO2_M,
+	},
+	{
+		.name = "buck1",
+		.id = LP8725_ID_BUCK1,
+		.ops = &lp8725_buck_ops,
+		.n_voltages = ARRAY_SIZE(lp8725_buck_vtbl),
+		.volt_table = lp8725_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP872X_GENERAL_CFG,
+		.enable_mask = LP8725_BUCK1_EN_M,
+	},
+	{
+		.name = "buck2",
+		.id = LP8725_ID_BUCK2,
+		.ops = &lp8725_buck_ops,
+		.n_voltages = ARRAY_SIZE(lp8725_buck_vtbl),
+		.volt_table = lp8725_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP872X_GENERAL_CFG,
+		.enable_mask = LP8725_BUCK2_EN_M,
+	},
+};
+
+static int lp872x_check_dvs_validity(struct lp872x *lp)
+{
+	struct lp872x_dvs *dvs = lp->pdata->dvs;
+	u8 val = 0;
+	int ret;
+
+	ret = lp872x_read_byte(lp, LP872X_GENERAL_CFG, &val);
+	if (ret)
+		return ret;
+
+	ret = 0;
+	if (lp->chipid == LP8720) {
+		if (val & LP8720_EXT_DVS_M)
+			ret = dvs ? 0 : -EINVAL;
+	} else {
+		if ((val & LP8725_DVS1_M) == EXTERN_DVS_USED)
+			ret = dvs ? 0 : -EINVAL;
+	}
+
+	return ret;
+}
+
+static int lp872x_init_dvs(struct lp872x *lp)
+{
+	int ret, gpio;
+	struct lp872x_dvs *dvs = lp->pdata->dvs;
+	enum lp872x_dvs_state pinstate;
+
+	ret = lp872x_check_dvs_validity(lp);
+	if (ret) {
+		dev_warn(lp->dev, "invalid dvs data: %d\n", ret);
+		return ret;
+	}
+
+	gpio = dvs->gpio;
+	if (!gpio_is_valid(gpio)) {
+		dev_err(lp->dev, "invalid gpio: %d\n", gpio);
+		return -EINVAL;
+	}
+
+	pinstate = dvs->init_state;
+	ret = devm_gpio_request_one(lp->dev, gpio, pinstate, "LP872X DVS");
+	if (ret) {
+		dev_err(lp->dev, "gpio request err: %d\n", ret);
+		return ret;
+	}
+
+	lp->dvs_pin = pinstate;
+	lp->dvs_gpio = gpio;
+
+	return 0;
+}
+
+static int lp872x_config(struct lp872x *lp)
+{
+	struct lp872x_platform_data *pdata = lp->pdata;
+	int ret;
+
+	if (!pdata->update_config)
+		return 0;
+
+	ret = lp872x_write_byte(lp, LP872X_GENERAL_CFG, pdata->general_config);
+	if (ret)
+		return ret;
+
+	return lp872x_init_dvs(lp);
+}
+
+static struct regulator_init_data
+*lp872x_find_regulator_init_data(int id, struct lp872x *lp)
+{
+	int i;
+
+	for (i = 0; i < lp->num_regulators; i++) {
+		if (lp->pdata->regulator_data[i].id == id)
+			return lp->pdata->regulator_data[i].init_data;
+	}
+
+	return NULL;
+}
+
+static int lp872x_regulator_register(struct lp872x *lp)
+{
+	struct regulator_desc *desc;
+	struct regulator_config cfg = { };
+	struct regulator_dev *rdev;
+	int i, ret;
+
+	for (i = 0 ; i < lp->num_regulators ; i++) {
+		desc = (lp->chipid == LP8720) ? &lp8720_regulator_desc[i] :
+						&lp8725_regulator_desc[i];
+
+		cfg.dev = lp->dev;
+		cfg.init_data = lp872x_find_regulator_init_data(desc->id, lp);
+		cfg.driver_data = lp;
+		cfg.regmap = lp->regmap;
+
+		rdev = regulator_register(desc, &cfg);
+		if (IS_ERR(rdev)) {
+			dev_err(lp->dev, "regulator register err");
+			ret =  PTR_ERR(rdev);
+			goto err;
+		}
+
+		*(lp->regulators + i) = rdev;
+	}
+
+	return 0;
+err:
+	while (--i >= 0) {
+		rdev = *(lp->regulators + i);
+		regulator_unregister(rdev);
+	}
+	return ret;
+}
+
+static void lp872x_regulator_unregister(struct lp872x *lp)
+{
+	struct regulator_dev *rdev;
+	int i;
+
+	for (i = 0 ; i < lp->num_regulators ; i++) {
+		rdev = *(lp->regulators + i);
+		regulator_unregister(rdev);
+	}
+}
+
+static const struct regmap_config lp872x_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = MAX_REGISTERS,
+};
+
+static int lp872x_probe(struct i2c_client *cl, const struct i2c_device_id *id)
+{
+	struct lp872x *lp;
+	struct lp872x_platform_data *pdata = cl->dev.platform_data;
+	int ret, size, num_regulators;
+	const int lp872x_num_regulators[] = {
+		[LP8720] = LP8720_NUM_REGULATORS,
+		[LP8725] = LP8725_NUM_REGULATORS,
+	};
+
+	if (!pdata) {
+		dev_err(&cl->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	lp = devm_kzalloc(&cl->dev, sizeof(struct lp872x), GFP_KERNEL);
+	if (!lp)
+		goto err_mem;
+
+	num_regulators = lp872x_num_regulators[id->driver_data];
+	size = sizeof(struct regulator_dev *) * num_regulators;
+
+	lp->regulators = devm_kzalloc(&cl->dev, size, GFP_KERNEL);
+	if (!lp->regulators)
+		goto err_mem;
+
+	lp->regmap = devm_regmap_init_i2c(cl, &lp872x_regmap_config);
+	if (IS_ERR(lp->regmap)) {
+		ret = PTR_ERR(lp->regmap);
+		dev_err(&cl->dev, "regmap init i2c err: %d\n", ret);
+		goto err_dev;
+	}
+
+	lp->dev = &cl->dev;
+	lp->pdata = pdata;
+	lp->chipid = id->driver_data;
+	lp->num_regulators = num_regulators;
+	i2c_set_clientdata(cl, lp);
+
+	ret = lp872x_config(lp);
+	if (ret)
+		goto err_dev;
+
+	return lp872x_regulator_register(lp);
+
+err_mem:
+	return -ENOMEM;
+err_dev:
+	return ret;
+}
+
+static int __devexit lp872x_remove(struct i2c_client *cl)
+{
+	struct lp872x *lp = i2c_get_clientdata(cl);
+
+	lp872x_regulator_unregister(lp);
+	return 0;
+}
+
+static const struct i2c_device_id lp872x_ids[] = {
+	{"lp8720", LP8720},
+	{"lp8725", LP8725},
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lp872x_ids);
+
+static struct i2c_driver lp872x_driver = {
+	.driver = {
+		.name = "lp872x",
+		.owner = THIS_MODULE,
+	},
+	.probe = lp872x_probe,
+	.remove = __devexit_p(lp872x_remove),
+	.id_table = lp872x_ids,
+};
+
+module_i2c_driver(lp872x_driver);
+
+MODULE_DESCRIPTION("TI/National Semiconductor LP872x PMU Regulator Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/lp8788-buck.c b/drivers/regulator/lp8788-buck.c
new file mode 100644
index 00000000000..6356e821400
--- /dev/null
+++ b/drivers/regulator/lp8788-buck.c
@@ -0,0 +1,629 @@
+/*
+ * TI LP8788 MFD - buck regulator driver
+ *
+ * Copyright 2012 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/lp8788.h>
+#include <linux/gpio.h>
+
+/* register address */
+#define LP8788_EN_BUCK			0x0C
+#define LP8788_BUCK_DVS_SEL		0x1D
+#define LP8788_BUCK1_VOUT0		0x1E
+#define LP8788_BUCK1_VOUT1		0x1F
+#define LP8788_BUCK1_VOUT2		0x20
+#define LP8788_BUCK1_VOUT3		0x21
+#define LP8788_BUCK2_VOUT0		0x22
+#define LP8788_BUCK2_VOUT1		0x23
+#define LP8788_BUCK2_VOUT2		0x24
+#define LP8788_BUCK2_VOUT3		0x25
+#define LP8788_BUCK3_VOUT		0x26
+#define LP8788_BUCK4_VOUT		0x27
+#define LP8788_BUCK1_TIMESTEP		0x28
+#define LP8788_BUCK_PWM			0x2D
+
+/* mask/shift bits */
+#define LP8788_EN_BUCK1_M		BIT(0)	/* Addr 0Ch */
+#define LP8788_EN_BUCK2_M		BIT(1)
+#define LP8788_EN_BUCK3_M		BIT(2)
+#define LP8788_EN_BUCK4_M		BIT(3)
+#define LP8788_BUCK1_DVS_SEL_M		0x04	/* Addr 1Dh */
+#define LP8788_BUCK1_DVS_M		0x03
+#define LP8788_BUCK1_DVS_S		0
+#define LP8788_BUCK2_DVS_SEL_M		0x40
+#define LP8788_BUCK2_DVS_M		0x30
+#define LP8788_BUCK2_DVS_S		4
+#define LP8788_BUCK1_DVS_I2C		BIT(2)
+#define LP8788_BUCK2_DVS_I2C		BIT(6)
+#define LP8788_BUCK1_DVS_PIN		(0 << 2)
+#define LP8788_BUCK2_DVS_PIN		(0 << 6)
+#define LP8788_VOUT_M			0x1F	/* Addr 1Eh ~ 27h */
+#define LP8788_STARTUP_TIME_M		0xF8	/* Addr 28h ~ 2Bh */
+#define LP8788_STARTUP_TIME_S		3
+#define LP8788_FPWM_BUCK1_M		BIT(0)	/* Addr 2Dh */
+#define LP8788_FPWM_BUCK1_S		0
+#define LP8788_FPWM_BUCK2_M		BIT(1)
+#define LP8788_FPWM_BUCK2_S		1
+#define LP8788_FPWM_BUCK3_M		BIT(2)
+#define LP8788_FPWM_BUCK3_S		2
+#define LP8788_FPWM_BUCK4_M		BIT(3)
+#define LP8788_FPWM_BUCK4_S		3
+
+#define INVALID_ADDR			0xFF
+#define LP8788_FORCE_PWM		1
+#define LP8788_AUTO_PWM			0
+#define PIN_LOW				0
+#define PIN_HIGH			1
+#define ENABLE_TIME_USEC		32
+
+enum lp8788_dvs_state {
+	DVS_LOW  = GPIOF_OUT_INIT_LOW,
+	DVS_HIGH = GPIOF_OUT_INIT_HIGH,
+};
+
+enum lp8788_dvs_mode {
+	REGISTER,
+	EXTPIN,
+};
+
+enum lp8788_buck_id {
+	BUCK1,
+	BUCK2,
+	BUCK3,
+	BUCK4,
+};
+
+struct lp8788_pwm_map {
+	u8 mask;
+	u8 shift;
+};
+
+struct lp8788_buck {
+	struct lp8788 *lp;
+	struct regulator_dev *regulator;
+	struct lp8788_pwm_map *pmap;
+	void *dvs;
+};
+
+/* BUCK 1 ~ 4 voltage table */
+static const int lp8788_buck_vtbl[] = {
+	 500000,  800000,  850000,  900000,  950000, 1000000, 1050000, 1100000,
+	1150000, 1200000, 1250000, 1300000, 1350000, 1400000, 1450000, 1500000,
+	1550000, 1600000, 1650000, 1700000, 1750000, 1800000, 1850000, 1900000,
+	1950000, 2000000,
+};
+
+/* buck pwm mode selection : used for set/get_mode in regulator ops
+ * @forced pwm : fast mode
+ * @auto pwm   : normal mode
+ */
+static struct lp8788_pwm_map buck_pmap[] = {
+	[BUCK1] = {
+		.mask = LP8788_FPWM_BUCK1_M,
+		.shift = LP8788_FPWM_BUCK1_S,
+	},
+	[BUCK2] = {
+		.mask = LP8788_FPWM_BUCK2_M,
+		.shift = LP8788_FPWM_BUCK2_S,
+	},
+	[BUCK3] = {
+		.mask = LP8788_FPWM_BUCK3_M,
+		.shift = LP8788_FPWM_BUCK3_S,
+	},
+	[BUCK4] = {
+		.mask = LP8788_FPWM_BUCK4_M,
+		.shift = LP8788_FPWM_BUCK4_S,
+	},
+};
+
+static const u8 buck1_vout_addr[] = {
+	LP8788_BUCK1_VOUT0, LP8788_BUCK1_VOUT1,
+	LP8788_BUCK1_VOUT2, LP8788_BUCK1_VOUT3,
+};
+
+static const u8 buck2_vout_addr[] = {
+	LP8788_BUCK2_VOUT0, LP8788_BUCK2_VOUT1,
+	LP8788_BUCK2_VOUT2, LP8788_BUCK2_VOUT3,
+};
+
+static void lp8788_buck1_set_dvs(struct lp8788_buck *buck)
+{
+	struct lp8788_buck1_dvs *dvs = (struct lp8788_buck1_dvs *)buck->dvs;
+	enum lp8788_dvs_state pinstate;
+
+	if (!dvs)
+		return;
+
+	pinstate = dvs->vsel == DVS_SEL_V0 ? DVS_LOW : DVS_HIGH;
+	if (gpio_is_valid(dvs->gpio))
+		gpio_set_value(dvs->gpio, pinstate);
+}
+
+static void lp8788_buck2_set_dvs(struct lp8788_buck *buck)
+{
+	struct lp8788_buck2_dvs *dvs = (struct lp8788_buck2_dvs *)buck->dvs;
+	enum lp8788_dvs_state pin1, pin2;
+
+	if (!dvs)
+		return;
+
+	switch (dvs->vsel) {
+	case DVS_SEL_V0:
+		pin1 = DVS_LOW;
+		pin2 = DVS_LOW;
+		break;
+	case DVS_SEL_V1:
+		pin1 = DVS_HIGH;
+		pin2 = DVS_LOW;
+		break;
+	case DVS_SEL_V2:
+		pin1 = DVS_LOW;
+		pin2 = DVS_HIGH;
+		break;
+	case DVS_SEL_V3:
+		pin1 = DVS_HIGH;
+		pin2 = DVS_HIGH;
+		break;
+	default:
+		return;
+	}
+
+	if (gpio_is_valid(dvs->gpio[0]))
+		gpio_set_value(dvs->gpio[0], pin1);
+
+	if (gpio_is_valid(dvs->gpio[1]))
+		gpio_set_value(dvs->gpio[1], pin2);
+}
+
+static void lp8788_set_dvs(struct lp8788_buck *buck, enum lp8788_buck_id id)
+{
+	switch (id) {
+	case BUCK1:
+		lp8788_buck1_set_dvs(buck);
+		break;
+	case BUCK2:
+		lp8788_buck2_set_dvs(buck);
+		break;
+	default:
+		break;
+	}
+}
+
+static enum lp8788_dvs_mode
+lp8788_get_buck_dvs_ctrl_mode(struct lp8788_buck *buck, enum lp8788_buck_id id)
+{
+	u8 val, mask;
+
+	switch (id) {
+	case BUCK1:
+		mask = LP8788_BUCK1_DVS_SEL_M;
+		break;
+	case BUCK2:
+		mask = LP8788_BUCK2_DVS_SEL_M;
+		break;
+	default:
+		return REGISTER;
+	}
+
+	lp8788_read_byte(buck->lp, LP8788_BUCK_DVS_SEL, &val);
+
+	return val & mask ? REGISTER : EXTPIN;
+}
+
+static bool lp8788_is_valid_buck_addr(u8 addr)
+{
+	switch (addr) {
+	case LP8788_BUCK1_VOUT0:
+	case LP8788_BUCK1_VOUT1:
+	case LP8788_BUCK1_VOUT2:
+	case LP8788_BUCK1_VOUT3:
+	case LP8788_BUCK2_VOUT0:
+	case LP8788_BUCK2_VOUT1:
+	case LP8788_BUCK2_VOUT2:
+	case LP8788_BUCK2_VOUT3:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static u8 lp8788_select_buck_vout_addr(struct lp8788_buck *buck,
+					enum lp8788_buck_id id)
+{
+	enum lp8788_dvs_mode mode = lp8788_get_buck_dvs_ctrl_mode(buck, id);
+	struct lp8788_buck1_dvs *b1_dvs;
+	struct lp8788_buck2_dvs *b2_dvs;
+	u8 val, idx, addr;
+	int pin1, pin2;
+
+	switch (id) {
+	case BUCK1:
+		if (mode == EXTPIN) {
+			b1_dvs = (struct lp8788_buck1_dvs *)buck->dvs;
+			if (!b1_dvs)
+				goto err;
+
+			idx = gpio_get_value(b1_dvs->gpio) ? 1 : 0;
+		} else {
+			lp8788_read_byte(buck->lp, LP8788_BUCK_DVS_SEL, &val);
+			idx = (val & LP8788_BUCK1_DVS_M) >> LP8788_BUCK1_DVS_S;
+		}
+		addr = buck1_vout_addr[idx];
+		break;
+	case BUCK2:
+		if (mode == EXTPIN) {
+			b2_dvs = (struct lp8788_buck2_dvs *)buck->dvs;
+			if (!b2_dvs)
+				goto err;
+
+			pin1 = gpio_get_value(b2_dvs->gpio[0]);
+			pin2 = gpio_get_value(b2_dvs->gpio[1]);
+
+			if (pin1 == PIN_LOW && pin2 == PIN_LOW)
+				idx = 0;
+			else if (pin1 == PIN_LOW && pin2 == PIN_HIGH)
+				idx = 2;
+			else if (pin1 == PIN_HIGH && pin2 == PIN_LOW)
+				idx = 1;
+			else
+				idx = 3;
+		} else {
+			lp8788_read_byte(buck->lp, LP8788_BUCK_DVS_SEL, &val);
+			idx = (val & LP8788_BUCK2_DVS_M) >> LP8788_BUCK2_DVS_S;
+		}
+		addr = buck2_vout_addr[idx];
+		break;
+	default:
+		goto err;
+	}
+
+	return addr;
+err:
+	return INVALID_ADDR;
+}
+
+static int lp8788_buck12_set_voltage_sel(struct regulator_dev *rdev,
+					unsigned selector)
+{
+	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
+	enum lp8788_buck_id id = rdev_get_id(rdev);
+	u8 addr;
+
+	if (buck->dvs)
+		lp8788_set_dvs(buck, id);
+
+	addr = lp8788_select_buck_vout_addr(buck, id);
+	if (!lp8788_is_valid_buck_addr(addr))
+		return -EINVAL;
+
+	return lp8788_update_bits(buck->lp, addr, LP8788_VOUT_M, selector);
+}
+
+static int lp8788_buck12_get_voltage_sel(struct regulator_dev *rdev)
+{
+	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
+	enum lp8788_buck_id id = rdev_get_id(rdev);
+	int ret;
+	u8 val, addr;
+
+	addr = lp8788_select_buck_vout_addr(buck, id);
+	if (!lp8788_is_valid_buck_addr(addr))
+		return -EINVAL;
+
+	ret = lp8788_read_byte(buck->lp, addr, &val);
+	if (ret)
+		return ret;
+
+	return val & LP8788_VOUT_M;
+}
+
+static int lp8788_buck_enable_time(struct regulator_dev *rdev)
+{
+	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
+	enum lp8788_buck_id id = rdev_get_id(rdev);
+	u8 val, addr = LP8788_BUCK1_TIMESTEP + id;
+
+	if (lp8788_read_byte(buck->lp, addr, &val))
+		return -EINVAL;
+
+	val = (val & LP8788_STARTUP_TIME_M) >> LP8788_STARTUP_TIME_S;
+
+	return ENABLE_TIME_USEC * val;
+}
+
+static int lp8788_buck_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
+	struct lp8788_pwm_map *pmap = buck->pmap;
+	u8 val;
+
+	if (!pmap)
+		return -EINVAL;
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = LP8788_FORCE_PWM << pmap->shift;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		val = LP8788_AUTO_PWM << pmap->shift;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return lp8788_update_bits(buck->lp, LP8788_BUCK_PWM, pmap->mask, val);
+}
+
+static unsigned int lp8788_buck_get_mode(struct regulator_dev *rdev)
+{
+	struct lp8788_buck *buck = rdev_get_drvdata(rdev);
+	struct lp8788_pwm_map *pmap = buck->pmap;
+	u8 val;
+	int ret;
+
+	if (!pmap)
+		return -EINVAL;
+
+	ret = lp8788_read_byte(buck->lp, LP8788_BUCK_PWM, &val);
+	if (ret)
+		return ret;
+
+	return val & pmap->mask ? REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
+}
+
+static struct regulator_ops lp8788_buck12_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = lp8788_buck12_set_voltage_sel,
+	.get_voltage_sel = lp8788_buck12_get_voltage_sel,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable_time = lp8788_buck_enable_time,
+	.set_mode = lp8788_buck_set_mode,
+	.get_mode = lp8788_buck_get_mode,
+};
+
+static struct regulator_ops lp8788_buck34_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable_time = lp8788_buck_enable_time,
+	.set_mode = lp8788_buck_set_mode,
+	.get_mode = lp8788_buck_get_mode,
+};
+
+static struct regulator_desc lp8788_buck_desc[] = {
+	{
+		.name = "buck1",
+		.id = BUCK1,
+		.ops = &lp8788_buck12_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_buck_vtbl),
+		.volt_table = lp8788_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_BUCK,
+		.enable_mask = LP8788_EN_BUCK1_M,
+	},
+	{
+		.name = "buck2",
+		.id = BUCK2,
+		.ops = &lp8788_buck12_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_buck_vtbl),
+		.volt_table = lp8788_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_BUCK,
+		.enable_mask = LP8788_EN_BUCK2_M,
+	},
+	{
+		.name = "buck3",
+		.id = BUCK3,
+		.ops = &lp8788_buck34_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_buck_vtbl),
+		.volt_table = lp8788_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_BUCK3_VOUT,
+		.vsel_mask = LP8788_VOUT_M,
+		.enable_reg = LP8788_EN_BUCK,
+		.enable_mask = LP8788_EN_BUCK3_M,
+	},
+	{
+		.name = "buck4",
+		.id = BUCK4,
+		.ops = &lp8788_buck34_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_buck_vtbl),
+		.volt_table = lp8788_buck_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_BUCK4_VOUT,
+		.vsel_mask = LP8788_VOUT_M,
+		.enable_reg = LP8788_EN_BUCK,
+		.enable_mask = LP8788_EN_BUCK4_M,
+	},
+};
+
+static int lp8788_set_default_dvs_ctrl_mode(struct lp8788 *lp,
+					enum lp8788_buck_id id)
+{
+	u8 mask, val;
+
+	switch (id) {
+	case BUCK1:
+		mask = LP8788_BUCK1_DVS_SEL_M;
+		val  = LP8788_BUCK1_DVS_I2C;
+		break;
+	case BUCK2:
+		mask = LP8788_BUCK2_DVS_SEL_M;
+		val  = LP8788_BUCK2_DVS_I2C;
+		break;
+	default:
+		return 0;
+	}
+
+	return lp8788_update_bits(lp, LP8788_BUCK_DVS_SEL, mask, val);
+}
+
+static int _gpio_request(struct lp8788_buck *buck, int gpio, char *name)
+{
+	struct device *dev = buck->lp->dev;
+
+	if (!gpio_is_valid(gpio)) {
+		dev_err(dev, "invalid gpio: %d\n", gpio);
+		return -EINVAL;
+	}
+
+	return devm_gpio_request_one(dev, gpio, DVS_LOW, name);
+}
+
+static int lp8788_dvs_gpio_request(struct lp8788_buck *buck,
+				enum lp8788_buck_id id)
+{
+	struct lp8788_platform_data *pdata = buck->lp->pdata;
+	char *b1_name = "LP8788_B1_DVS";
+	char *b2_name[] = { "LP8788_B2_DVS1", "LP8788_B2_DVS2" };
+	int i, gpio, ret;
+
+	switch (id) {
+	case BUCK1:
+		gpio = pdata->buck1_dvs->gpio;
+		ret = _gpio_request(buck, gpio, b1_name);
+		if (ret)
+			return ret;
+
+		buck->dvs = pdata->buck1_dvs;
+		break;
+	case BUCK2:
+		for (i = 0 ; i < LP8788_NUM_BUCK2_DVS ; i++) {
+			gpio = pdata->buck2_dvs->gpio[i];
+			ret = _gpio_request(buck, gpio, b2_name[i]);
+			if (ret)
+				return ret;
+		}
+		buck->dvs = pdata->buck2_dvs;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int lp8788_init_dvs(struct lp8788_buck *buck, enum lp8788_buck_id id)
+{
+	struct lp8788_platform_data *pdata = buck->lp->pdata;
+	u8 mask[] = { LP8788_BUCK1_DVS_SEL_M, LP8788_BUCK2_DVS_SEL_M };
+	u8 val[]  = { LP8788_BUCK1_DVS_PIN, LP8788_BUCK2_DVS_PIN };
+
+	/* no dvs for buck3, 4 */
+	if (id == BUCK3 || id == BUCK4)
+		return 0;
+
+	/* no dvs platform data, then dvs will be selected by I2C registers */
+	if (!pdata)
+		goto set_default_dvs_mode;
+
+	if ((id == BUCK1 && !pdata->buck1_dvs) ||
+		(id == BUCK2 && !pdata->buck2_dvs))
+		goto set_default_dvs_mode;
+
+	if (lp8788_dvs_gpio_request(buck, id))
+		goto set_default_dvs_mode;
+
+	return lp8788_update_bits(buck->lp, LP8788_BUCK_DVS_SEL, mask[id],
+				val[id]);
+
+set_default_dvs_mode:
+	return lp8788_set_default_dvs_ctrl_mode(buck->lp, id);
+}
+
+static __devinit int lp8788_buck_probe(struct platform_device *pdev)
+{
+	struct lp8788 *lp = dev_get_drvdata(pdev->dev.parent);
+	int id = pdev->id;
+	struct lp8788_buck *buck;
+	struct regulator_config cfg = { };
+	struct regulator_dev *rdev;
+	int ret;
+
+	buck = devm_kzalloc(lp->dev, sizeof(struct lp8788_buck), GFP_KERNEL);
+	if (!buck)
+		return -ENOMEM;
+
+	buck->lp = lp;
+	buck->pmap = &buck_pmap[id];
+
+	ret = lp8788_init_dvs(buck, id);
+	if (ret)
+		return ret;
+
+	cfg.dev = lp->dev;
+	cfg.init_data = lp->pdata ? lp->pdata->buck_data[id] : NULL;
+	cfg.driver_data = buck;
+	cfg.regmap = lp->regmap;
+
+	rdev = regulator_register(&lp8788_buck_desc[id], &cfg);
+	if (IS_ERR(rdev)) {
+		ret = PTR_ERR(rdev);
+		dev_err(lp->dev, "BUCK%d regulator register err = %d\n",
+				id + 1, ret);
+		return ret;
+	}
+
+	buck->regulator = rdev;
+	platform_set_drvdata(pdev, buck);
+
+	return 0;
+}
+
+static int __devexit lp8788_buck_remove(struct platform_device *pdev)
+{
+	struct lp8788_buck *buck = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+	regulator_unregister(buck->regulator);
+
+	return 0;
+}
+
+static struct platform_driver lp8788_buck_driver = {
+	.probe = lp8788_buck_probe,
+	.remove = __devexit_p(lp8788_buck_remove),
+	.driver = {
+		.name = LP8788_DEV_BUCK,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init lp8788_buck_init(void)
+{
+	return platform_driver_register(&lp8788_buck_driver);
+}
+subsys_initcall(lp8788_buck_init);
+
+static void __exit lp8788_buck_exit(void)
+{
+	platform_driver_unregister(&lp8788_buck_driver);
+}
+module_exit(lp8788_buck_exit);
+
+MODULE_DESCRIPTION("TI LP8788 BUCK Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lp8788-buck");
diff --git a/drivers/regulator/lp8788-ldo.c b/drivers/regulator/lp8788-ldo.c
new file mode 100644
index 00000000000..d2122e41a96
--- /dev/null
+++ b/drivers/regulator/lp8788-ldo.c
@@ -0,0 +1,842 @@
+/*
+ * TI LP8788 MFD - ldo regulator driver
+ *
+ * Copyright 2012 Texas Instruments
+ *
+ * Author: Milo(Woogyom) Kim <milo.kim@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/gpio.h>
+#include <linux/mfd/lp8788.h>
+
+/* register address */
+#define LP8788_EN_LDO_A			0x0D	/* DLDO 1 ~ 8 */
+#define LP8788_EN_LDO_B			0x0E	/* DLDO 9 ~ 12, ALDO 1 ~ 4 */
+#define LP8788_EN_LDO_C			0x0F	/* ALDO 5 ~ 10 */
+#define LP8788_EN_SEL			0x10
+#define LP8788_DLDO1_VOUT		0x2E
+#define LP8788_DLDO2_VOUT		0x2F
+#define LP8788_DLDO3_VOUT		0x30
+#define LP8788_DLDO4_VOUT		0x31
+#define LP8788_DLDO5_VOUT		0x32
+#define LP8788_DLDO6_VOUT		0x33
+#define LP8788_DLDO7_VOUT		0x34
+#define LP8788_DLDO8_VOUT		0x35
+#define LP8788_DLDO9_VOUT		0x36
+#define LP8788_DLDO10_VOUT		0x37
+#define LP8788_DLDO11_VOUT		0x38
+#define LP8788_DLDO12_VOUT		0x39
+#define LP8788_ALDO1_VOUT		0x3A
+#define LP8788_ALDO2_VOUT		0x3B
+#define LP8788_ALDO3_VOUT		0x3C
+#define LP8788_ALDO4_VOUT		0x3D
+#define LP8788_ALDO5_VOUT		0x3E
+#define LP8788_ALDO6_VOUT		0x3F
+#define LP8788_ALDO7_VOUT		0x40
+#define LP8788_ALDO8_VOUT		0x41
+#define LP8788_ALDO9_VOUT		0x42
+#define LP8788_ALDO10_VOUT		0x43
+#define LP8788_DLDO1_TIMESTEP		0x44
+
+/* mask/shift bits */
+#define LP8788_EN_DLDO1_M		BIT(0)	/* Addr 0Dh ~ 0Fh */
+#define LP8788_EN_DLDO2_M		BIT(1)
+#define LP8788_EN_DLDO3_M		BIT(2)
+#define LP8788_EN_DLDO4_M		BIT(3)
+#define LP8788_EN_DLDO5_M		BIT(4)
+#define LP8788_EN_DLDO6_M		BIT(5)
+#define LP8788_EN_DLDO7_M		BIT(6)
+#define LP8788_EN_DLDO8_M		BIT(7)
+#define LP8788_EN_DLDO9_M		BIT(0)
+#define LP8788_EN_DLDO10_M		BIT(1)
+#define LP8788_EN_DLDO11_M		BIT(2)
+#define LP8788_EN_DLDO12_M		BIT(3)
+#define LP8788_EN_ALDO1_M		BIT(4)
+#define LP8788_EN_ALDO2_M		BIT(5)
+#define LP8788_EN_ALDO3_M		BIT(6)
+#define LP8788_EN_ALDO4_M		BIT(7)
+#define LP8788_EN_ALDO5_M		BIT(0)
+#define LP8788_EN_ALDO6_M		BIT(1)
+#define LP8788_EN_ALDO7_M		BIT(2)
+#define LP8788_EN_ALDO8_M		BIT(3)
+#define LP8788_EN_ALDO9_M		BIT(4)
+#define LP8788_EN_ALDO10_M		BIT(5)
+#define LP8788_EN_SEL_DLDO911_M		BIT(0)	/* Addr 10h */
+#define LP8788_EN_SEL_DLDO7_M		BIT(1)
+#define LP8788_EN_SEL_ALDO7_M		BIT(2)
+#define LP8788_EN_SEL_ALDO5_M		BIT(3)
+#define LP8788_EN_SEL_ALDO234_M		BIT(4)
+#define LP8788_EN_SEL_ALDO1_M		BIT(5)
+#define LP8788_VOUT_5BIT_M		0x1F	/* Addr 2Eh ~ 43h */
+#define LP8788_VOUT_4BIT_M		0x0F
+#define LP8788_VOUT_3BIT_M		0x07
+#define LP8788_VOUT_1BIT_M		0x01
+#define LP8788_STARTUP_TIME_M		0xF8	/* Addr 44h ~ 59h */
+#define LP8788_STARTUP_TIME_S		3
+
+#define ENABLE_TIME_USEC		32
+#define ENABLE				GPIOF_OUT_INIT_HIGH
+#define DISABLE				GPIOF_OUT_INIT_LOW
+
+enum lp8788_enable_mode {
+	REGISTER,
+	EXTPIN,
+};
+
+enum lp8788_ldo_id {
+	DLDO1,
+	DLDO2,
+	DLDO3,
+	DLDO4,
+	DLDO5,
+	DLDO6,
+	DLDO7,
+	DLDO8,
+	DLDO9,
+	DLDO10,
+	DLDO11,
+	DLDO12,
+	ALDO1,
+	ALDO2,
+	ALDO3,
+	ALDO4,
+	ALDO5,
+	ALDO6,
+	ALDO7,
+	ALDO8,
+	ALDO9,
+	ALDO10,
+};
+
+struct lp8788_ldo {
+	struct lp8788 *lp;
+	struct regulator_desc *desc;
+	struct regulator_dev *regulator;
+	struct lp8788_ldo_enable_pin *en_pin;
+};
+
+/* DLDO 1, 2, 3, 9 voltage table */
+const int lp8788_dldo1239_vtbl[] = {
+	1800000, 1900000, 2000000, 2100000, 2200000, 2300000, 2400000, 2500000,
+	2600000, 2700000, 2800000, 2900000, 3000000, 2850000, 2850000, 2850000,
+	2850000, 2850000, 2850000, 2850000, 2850000, 2850000, 2850000, 2850000,
+	2850000, 2850000, 2850000, 2850000, 2850000, 2850000, 2850000, 2850000,
+};
+
+/* DLDO 4 voltage table */
+static const int lp8788_dldo4_vtbl[] = { 1800000, 3000000 };
+
+/* DLDO 5, 7, 8 and ALDO 6 voltage table */
+static const int lp8788_dldo578_aldo6_vtbl[] = {
+	1800000, 1900000, 2000000, 2100000, 2200000, 2300000, 2400000, 2500000,
+	2600000, 2700000, 2800000, 2900000, 3000000, 3000000, 3000000, 3000000,
+};
+
+/* DLDO 6 voltage table */
+static const int lp8788_dldo6_vtbl[] = {
+	3000000, 3100000, 3200000, 3300000, 3400000, 3500000, 3600000, 3600000,
+};
+
+/* DLDO 10, 11 voltage table */
+static const int lp8788_dldo1011_vtbl[] = {
+	1100000, 1150000, 1200000, 1250000, 1300000, 1350000, 1400000, 1450000,
+	1500000, 1500000, 1500000, 1500000, 1500000, 1500000, 1500000, 1500000,
+};
+
+/* ALDO 1 voltage table */
+static const int lp8788_aldo1_vtbl[] = { 1800000, 2850000 };
+
+/* ALDO 7 voltage table */
+static const int lp8788_aldo7_vtbl[] = {
+	1200000, 1300000, 1400000, 1500000, 1600000, 1700000, 1800000, 1800000,
+};
+
+static enum lp8788_ldo_id lp8788_dldo_id[] = {
+	DLDO1,
+	DLDO2,
+	DLDO3,
+	DLDO4,
+	DLDO5,
+	DLDO6,
+	DLDO7,
+	DLDO8,
+	DLDO9,
+	DLDO10,
+	DLDO11,
+	DLDO12,
+};
+
+static enum lp8788_ldo_id lp8788_aldo_id[] = {
+	ALDO1,
+	ALDO2,
+	ALDO3,
+	ALDO4,
+	ALDO5,
+	ALDO6,
+	ALDO7,
+	ALDO8,
+	ALDO9,
+	ALDO10,
+};
+
+/* DLDO 7, 9 and 11, ALDO 1 ~ 5 and 7
+   : can be enabled either by external pin or by i2c register */
+static enum lp8788_enable_mode
+lp8788_get_ldo_enable_mode(struct lp8788_ldo *ldo, enum lp8788_ldo_id id)
+{
+	int ret;
+	u8 val, mask;
+
+	ret = lp8788_read_byte(ldo->lp, LP8788_EN_SEL, &val);
+	if (ret)
+		return ret;
+
+	switch (id) {
+	case DLDO7:
+		mask =  LP8788_EN_SEL_DLDO7_M;
+		break;
+	case DLDO9:
+	case DLDO11:
+		mask =  LP8788_EN_SEL_DLDO911_M;
+		break;
+	case ALDO1:
+		mask =  LP8788_EN_SEL_ALDO1_M;
+		break;
+	case ALDO2 ... ALDO4:
+		mask =  LP8788_EN_SEL_ALDO234_M;
+		break;
+	case ALDO5:
+		mask =  LP8788_EN_SEL_ALDO5_M;
+		break;
+	case ALDO7:
+		mask =  LP8788_EN_SEL_ALDO7_M;
+		break;
+	default:
+		return REGISTER;
+	}
+
+	return val & mask ? EXTPIN : REGISTER;
+}
+
+static int lp8788_ldo_ctrl_by_extern_pin(struct lp8788_ldo *ldo, int pinstate)
+{
+	struct lp8788_ldo_enable_pin *pin = ldo->en_pin;
+
+	if (!pin)
+		return -EINVAL;
+
+	if (gpio_is_valid(pin->gpio))
+		gpio_set_value(pin->gpio, pinstate);
+
+	return 0;
+}
+
+static int lp8788_ldo_is_enabled_by_extern_pin(struct lp8788_ldo *ldo)
+{
+	struct lp8788_ldo_enable_pin *pin = ldo->en_pin;
+
+	if (!pin)
+		return -EINVAL;
+
+	return gpio_get_value(pin->gpio) ? 1 : 0;
+}
+
+static int lp8788_ldo_enable(struct regulator_dev *rdev)
+{
+	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
+	enum lp8788_ldo_id id = rdev_get_id(rdev);
+	enum lp8788_enable_mode mode = lp8788_get_ldo_enable_mode(ldo, id);
+
+	switch (mode) {
+	case EXTPIN:
+		return lp8788_ldo_ctrl_by_extern_pin(ldo, ENABLE);
+	case REGISTER:
+		return regulator_enable_regmap(rdev);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int lp8788_ldo_disable(struct regulator_dev *rdev)
+{
+	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
+	enum lp8788_ldo_id id = rdev_get_id(rdev);
+	enum lp8788_enable_mode mode = lp8788_get_ldo_enable_mode(ldo, id);
+
+	switch (mode) {
+	case EXTPIN:
+		return lp8788_ldo_ctrl_by_extern_pin(ldo, DISABLE);
+	case REGISTER:
+		return regulator_disable_regmap(rdev);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int lp8788_ldo_is_enabled(struct regulator_dev *rdev)
+{
+	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
+	enum lp8788_ldo_id id = rdev_get_id(rdev);
+	enum lp8788_enable_mode mode = lp8788_get_ldo_enable_mode(ldo, id);
+
+	switch (mode) {
+	case EXTPIN:
+		return lp8788_ldo_is_enabled_by_extern_pin(ldo);
+	case REGISTER:
+		return regulator_is_enabled_regmap(rdev);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int lp8788_ldo_enable_time(struct regulator_dev *rdev)
+{
+	struct lp8788_ldo *ldo = rdev_get_drvdata(rdev);
+	enum lp8788_ldo_id id = rdev_get_id(rdev);
+	u8 val, addr = LP8788_DLDO1_TIMESTEP + id;
+
+	if (lp8788_read_byte(ldo->lp, addr, &val))
+		return -EINVAL;
+
+	val = (val & LP8788_STARTUP_TIME_M) >> LP8788_STARTUP_TIME_S;
+
+	return ENABLE_TIME_USEC * val;
+}
+
+static int lp8788_ldo_fixed_get_voltage(struct regulator_dev *rdev)
+{
+	enum lp8788_ldo_id id = rdev_get_id(rdev);
+
+	switch (id) {
+	case ALDO2 ... ALDO5:
+		return 2850000;
+	case DLDO12:
+	case ALDO8 ... ALDO9:
+		return 2500000;
+	case ALDO10:
+		return 1100000;
+	default:
+		return -EINVAL;
+	}
+}
+
+static struct regulator_ops lp8788_ldo_voltage_table_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.enable = lp8788_ldo_enable,
+	.disable = lp8788_ldo_disable,
+	.is_enabled = lp8788_ldo_is_enabled,
+	.enable_time = lp8788_ldo_enable_time,
+};
+
+static struct regulator_ops lp8788_ldo_voltage_fixed_ops = {
+	.get_voltage = lp8788_ldo_fixed_get_voltage,
+	.enable = lp8788_ldo_enable,
+	.disable = lp8788_ldo_disable,
+	.is_enabled = lp8788_ldo_is_enabled,
+	.enable_time = lp8788_ldo_enable_time,
+};
+
+static struct regulator_desc lp8788_dldo_desc[] = {
+	{
+		.name = "dldo1",
+		.id = DLDO1,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo1239_vtbl),
+		.volt_table = lp8788_dldo1239_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO1_VOUT,
+		.vsel_mask = LP8788_VOUT_5BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO1_M,
+	},
+	{
+		.name = "dldo2",
+		.id = DLDO2,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo1239_vtbl),
+		.volt_table = lp8788_dldo1239_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO2_VOUT,
+		.vsel_mask = LP8788_VOUT_5BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO2_M,
+	},
+	{
+		.name = "dldo3",
+		.id = DLDO3,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo1239_vtbl),
+		.volt_table = lp8788_dldo1239_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO3_VOUT,
+		.vsel_mask = LP8788_VOUT_5BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO3_M,
+	},
+	{
+		.name = "dldo4",
+		.id = DLDO4,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo4_vtbl),
+		.volt_table = lp8788_dldo4_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO4_VOUT,
+		.vsel_mask = LP8788_VOUT_1BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO4_M,
+	},
+	{
+		.name = "dldo5",
+		.id = DLDO5,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo578_aldo6_vtbl),
+		.volt_table = lp8788_dldo578_aldo6_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO5_VOUT,
+		.vsel_mask = LP8788_VOUT_4BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO5_M,
+	},
+	{
+		.name = "dldo6",
+		.id = DLDO6,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo6_vtbl),
+		.volt_table = lp8788_dldo6_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO6_VOUT,
+		.vsel_mask = LP8788_VOUT_3BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO6_M,
+	},
+	{
+		.name = "dldo7",
+		.id = DLDO7,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo578_aldo6_vtbl),
+		.volt_table = lp8788_dldo578_aldo6_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO7_VOUT,
+		.vsel_mask = LP8788_VOUT_4BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO7_M,
+	},
+	{
+		.name = "dldo8",
+		.id = DLDO8,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo578_aldo6_vtbl),
+		.volt_table = lp8788_dldo578_aldo6_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO8_VOUT,
+		.vsel_mask = LP8788_VOUT_4BIT_M,
+		.enable_reg = LP8788_EN_LDO_A,
+		.enable_mask = LP8788_EN_DLDO8_M,
+	},
+	{
+		.name = "dldo9",
+		.id = DLDO9,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo1239_vtbl),
+		.volt_table = lp8788_dldo1239_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO9_VOUT,
+		.vsel_mask = LP8788_VOUT_5BIT_M,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_DLDO9_M,
+	},
+	{
+		.name = "dldo10",
+		.id = DLDO10,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo1011_vtbl),
+		.volt_table = lp8788_dldo1011_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO10_VOUT,
+		.vsel_mask = LP8788_VOUT_4BIT_M,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_DLDO10_M,
+	},
+	{
+		.name = "dldo11",
+		.id = DLDO11,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo1011_vtbl),
+		.volt_table = lp8788_dldo1011_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_DLDO11_VOUT,
+		.vsel_mask = LP8788_VOUT_4BIT_M,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_DLDO11_M,
+	},
+	{
+		.name = "dldo12",
+		.id = DLDO12,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_DLDO12_M,
+	},
+};
+
+static struct regulator_desc lp8788_aldo_desc[] = {
+	{
+		.name = "aldo1",
+		.id = ALDO1,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_aldo1_vtbl),
+		.volt_table = lp8788_aldo1_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_ALDO1_VOUT,
+		.vsel_mask = LP8788_VOUT_1BIT_M,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_ALDO1_M,
+	},
+	{
+		.name = "aldo2",
+		.id = ALDO2,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_ALDO2_M,
+	},
+	{
+		.name = "aldo3",
+		.id = ALDO3,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_ALDO3_M,
+	},
+	{
+		.name = "aldo4",
+		.id = ALDO4,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_B,
+		.enable_mask = LP8788_EN_ALDO4_M,
+	},
+	{
+		.name = "aldo5",
+		.id = ALDO5,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_C,
+		.enable_mask = LP8788_EN_ALDO5_M,
+	},
+	{
+		.name = "aldo6",
+		.id = ALDO6,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_dldo578_aldo6_vtbl),
+		.volt_table = lp8788_dldo578_aldo6_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_ALDO6_VOUT,
+		.vsel_mask = LP8788_VOUT_4BIT_M,
+		.enable_reg = LP8788_EN_LDO_C,
+		.enable_mask = LP8788_EN_ALDO6_M,
+	},
+	{
+		.name = "aldo7",
+		.id = ALDO7,
+		.ops = &lp8788_ldo_voltage_table_ops,
+		.n_voltages = ARRAY_SIZE(lp8788_aldo7_vtbl),
+		.volt_table = lp8788_aldo7_vtbl,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.vsel_reg = LP8788_ALDO7_VOUT,
+		.vsel_mask = LP8788_VOUT_3BIT_M,
+		.enable_reg = LP8788_EN_LDO_C,
+		.enable_mask = LP8788_EN_ALDO7_M,
+	},
+	{
+		.name = "aldo8",
+		.id = ALDO8,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_C,
+		.enable_mask = LP8788_EN_ALDO8_M,
+	},
+	{
+		.name = "aldo9",
+		.id = ALDO9,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_C,
+		.enable_mask = LP8788_EN_ALDO9_M,
+	},
+	{
+		.name = "aldo10",
+		.id = ALDO10,
+		.ops = &lp8788_ldo_voltage_fixed_ops,
+		.type = REGULATOR_VOLTAGE,
+		.owner = THIS_MODULE,
+		.enable_reg = LP8788_EN_LDO_C,
+		.enable_mask = LP8788_EN_ALDO10_M,
+	},
+};
+
+static int lp8788_gpio_request_ldo_en(struct lp8788_ldo *ldo,
+				enum lp8788_ext_ldo_en_id id)
+{
+	struct device *dev = ldo->lp->dev;
+	struct lp8788_ldo_enable_pin *pin = ldo->en_pin;
+	int ret, gpio, pinstate;
+	char *name[] = {
+		[EN_ALDO1]   = "LP8788_EN_ALDO1",
+		[EN_ALDO234] = "LP8788_EN_ALDO234",
+		[EN_ALDO5]   = "LP8788_EN_ALDO5",
+		[EN_ALDO7]   = "LP8788_EN_ALDO7",
+		[EN_DLDO7]   = "LP8788_EN_DLDO7",
+		[EN_DLDO911] = "LP8788_EN_DLDO911",
+	};
+
+	gpio = pin->gpio;
+	if (!gpio_is_valid(gpio)) {
+		dev_err(dev, "invalid gpio: %d\n", gpio);
+		return -EINVAL;
+	}
+
+	pinstate = pin->init_state;
+	ret = devm_gpio_request_one(dev, gpio, pinstate, name[id]);
+	if (ret == -EBUSY) {
+		dev_warn(dev, "gpio%d already used\n", gpio);
+		return 0;
+	}
+
+	return ret;
+}
+
+static int lp8788_config_ldo_enable_mode(struct lp8788_ldo *ldo,
+					enum lp8788_ldo_id id)
+{
+	int ret;
+	struct lp8788 *lp = ldo->lp;
+	struct lp8788_platform_data *pdata = lp->pdata;
+	enum lp8788_ext_ldo_en_id enable_id;
+	u8 en_mask[] = {
+		[EN_ALDO1]   = LP8788_EN_SEL_ALDO1_M,
+		[EN_ALDO234] = LP8788_EN_SEL_ALDO234_M,
+		[EN_ALDO5]   = LP8788_EN_SEL_ALDO5_M,
+		[EN_ALDO7]   = LP8788_EN_SEL_ALDO7_M,
+		[EN_DLDO7]   = LP8788_EN_SEL_DLDO7_M,
+		[EN_DLDO911] = LP8788_EN_SEL_DLDO911_M,
+	};
+	u8 val[] = {
+		[EN_ALDO1]   = 0 << 5,
+		[EN_ALDO234] = 0 << 4,
+		[EN_ALDO5]   = 0 << 3,
+		[EN_ALDO7]   = 0 << 2,
+		[EN_DLDO7]   = 0 << 1,
+		[EN_DLDO911] = 0 << 0,
+	};
+
+	switch (id) {
+	case DLDO7:
+		enable_id = EN_DLDO7;
+		break;
+	case DLDO9:
+	case DLDO11:
+		enable_id = EN_DLDO911;
+		break;
+	case ALDO1:
+		enable_id = EN_ALDO1;
+		break;
+	case ALDO2 ... ALDO4:
+		enable_id = EN_ALDO234;
+		break;
+	case ALDO5:
+		enable_id = EN_ALDO5;
+		break;
+	case ALDO7:
+		enable_id = EN_ALDO7;
+		break;
+	default:
+		return 0;
+	}
+
+	/* if no platform data for ldo pin, then set default enable mode */
+	if (!pdata || !pdata->ldo_pin || !pdata->ldo_pin[enable_id])
+		goto set_default_ldo_enable_mode;
+
+	ldo->en_pin = pdata->ldo_pin[enable_id];
+
+	ret = lp8788_gpio_request_ldo_en(ldo, enable_id);
+	if (ret)
+		goto set_default_ldo_enable_mode;
+
+	return ret;
+
+set_default_ldo_enable_mode:
+	return lp8788_update_bits(lp, LP8788_EN_SEL, en_mask[enable_id],
+				val[enable_id]);
+}
+
+static __devinit int lp8788_dldo_probe(struct platform_device *pdev)
+{
+	struct lp8788 *lp = dev_get_drvdata(pdev->dev.parent);
+	int id = pdev->id;
+	struct lp8788_ldo *ldo;
+	struct regulator_config cfg = { };
+	struct regulator_dev *rdev;
+	int ret;
+
+	ldo = devm_kzalloc(lp->dev, sizeof(struct lp8788_ldo), GFP_KERNEL);
+	if (!ldo)
+		return -ENOMEM;
+
+	ldo->lp = lp;
+	ret = lp8788_config_ldo_enable_mode(ldo, lp8788_dldo_id[id]);
+	if (ret)
+		return ret;
+
+	cfg.dev = lp->dev;
+	cfg.init_data = lp->pdata ? lp->pdata->dldo_data[id] : NULL;
+	cfg.driver_data = ldo;
+	cfg.regmap = lp->regmap;
+
+	rdev = regulator_register(&lp8788_dldo_desc[id], &cfg);
+	if (IS_ERR(rdev)) {
+		ret = PTR_ERR(rdev);
+		dev_err(lp->dev, "DLDO%d regulator register err = %d\n",
+				id + 1, ret);
+		return ret;
+	}
+
+	ldo->regulator = rdev;
+	platform_set_drvdata(pdev, ldo);
+
+	return 0;
+}
+
+static int __devexit lp8788_dldo_remove(struct platform_device *pdev)
+{
+	struct lp8788_ldo *ldo = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+	regulator_unregister(ldo->regulator);
+
+	return 0;
+}
+
+static struct platform_driver lp8788_dldo_driver = {
+	.probe = lp8788_dldo_probe,
+	.remove = __devexit_p(lp8788_dldo_remove),
+	.driver = {
+		.name = LP8788_DEV_DLDO,
+		.owner = THIS_MODULE,
+	},
+};
+
+static __devinit int lp8788_aldo_probe(struct platform_device *pdev)
+{
+	struct lp8788 *lp = dev_get_drvdata(pdev->dev.parent);
+	int id = pdev->id;
+	struct lp8788_ldo *ldo;
+	struct regulator_config cfg = { };
+	struct regulator_dev *rdev;
+	int ret;
+
+	ldo = devm_kzalloc(lp->dev, sizeof(struct lp8788_ldo), GFP_KERNEL);
+	if (!ldo)
+		return -ENOMEM;
+
+	ldo->lp = lp;
+	ret = lp8788_config_ldo_enable_mode(ldo, lp8788_aldo_id[id]);
+	if (ret)
+		return ret;
+
+	cfg.dev = lp->dev;
+	cfg.init_data = lp->pdata ? lp->pdata->aldo_data[id] : NULL;
+	cfg.driver_data = ldo;
+	cfg.regmap = lp->regmap;
+
+	rdev = regulator_register(&lp8788_aldo_desc[id], &cfg);
+	if (IS_ERR(rdev)) {
+		ret = PTR_ERR(rdev);
+		dev_err(lp->dev, "ALDO%d regulator register err = %d\n",
+				id + 1, ret);
+		return ret;
+	}
+
+	ldo->regulator = rdev;
+	platform_set_drvdata(pdev, ldo);
+
+	return 0;
+}
+
+static int __devexit lp8788_aldo_remove(struct platform_device *pdev)
+{
+	struct lp8788_ldo *ldo = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+	regulator_unregister(ldo->regulator);
+
+	return 0;
+}
+
+static struct platform_driver lp8788_aldo_driver = {
+	.probe = lp8788_aldo_probe,
+	.remove = __devexit_p(lp8788_aldo_remove),
+	.driver = {
+		.name = LP8788_DEV_ALDO,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init lp8788_ldo_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&lp8788_dldo_driver);
+	if (ret)
+		return ret;
+
+	return platform_driver_register(&lp8788_aldo_driver);
+}
+subsys_initcall(lp8788_ldo_init);
+
+static void __exit lp8788_ldo_exit(void)
+{
+	platform_driver_unregister(&lp8788_aldo_driver);
+	platform_driver_unregister(&lp8788_dldo_driver);
+}
+module_exit(lp8788_ldo_exit);
+
+MODULE_DESCRIPTION("TI LP8788 LDO Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:lp8788-dldo");
+MODULE_ALIAS("platform:lp8788-aldo");
diff --git a/drivers/regulator/max1586.c b/drivers/regulator/max1586.c
index b9444ee08da..f67af3c1b96 100644
--- a/drivers/regulator/max1586.c
+++ b/drivers/regulator/max1586.c
@@ -48,6 +48,14 @@ struct max1586_data {
 };
 
 /*
+ * V6 voltage
+ * On I2C bus, sending a "x" byte to the max1586 means :
+ *   set V6 to either 0V, 1.8V, 2.5V, 3V depending on (x & 0x3)
+ * As regulator framework doesn't accept voltages to be 0V, we use 1uV.
+ */
+static int v6_voltages_uv[] = { 1, 1800000, 2500000, 3000000 };
+
+/*
  * V3 voltage
  * On I2C bus, sending a "x" byte to the max1586 means :
  *   set V3 to 0.700V + (x & 0x1f) * 0.025V
@@ -55,113 +63,49 @@ struct max1586_data {
  * R24 and R25=100kOhm as described in the data sheet.
  * The gain is approximately: 1 + R24/R25 + R24/185.5kOhm
  */
-static int max1586_v3_calc_voltage(struct max1586_data *max1586,
-	unsigned selector)
-{
-	unsigned range_uV = max1586->max_uV - max1586->min_uV;
-
-	return max1586->min_uV + (selector * range_uV / MAX1586_V3_MAX_VSEL);
-}
-
-static int max1586_v3_set(struct regulator_dev *rdev, int min_uV, int max_uV,
-			  unsigned *selector)
+static int max1586_v3_set_voltage_sel(struct regulator_dev *rdev,
+				      unsigned selector)
 {
 	struct max1586_data *max1586 = rdev_get_drvdata(rdev);
 	struct i2c_client *client = max1586->client;
-	unsigned range_uV = max1586->max_uV - max1586->min_uV;
 	u8 v3_prog;
 
-	if (min_uV > max1586->max_uV || max_uV < max1586->min_uV)
-		return -EINVAL;
-	if (min_uV < max1586->min_uV)
-		min_uV = max1586->min_uV;
-
-	*selector = DIV_ROUND_UP((min_uV - max1586->min_uV) *
-				 MAX1586_V3_MAX_VSEL, range_uV);
-	if (max1586_v3_calc_voltage(max1586, *selector) > max_uV)
-		return -EINVAL;
-
 	dev_dbg(&client->dev, "changing voltage v3 to %dmv\n",
-		max1586_v3_calc_voltage(max1586, *selector) / 1000);
+		regulator_list_voltage_linear(rdev, selector) / 1000);
 
-	v3_prog = I2C_V3_SELECT | (u8) *selector;
+	v3_prog = I2C_V3_SELECT | (u8) selector;
 	return i2c_smbus_write_byte(client, v3_prog);
 }
 
-static int max1586_v3_list(struct regulator_dev *rdev, unsigned selector)
-{
-	struct max1586_data *max1586 = rdev_get_drvdata(rdev);
-
-	if (selector > MAX1586_V3_MAX_VSEL)
-		return -EINVAL;
-	return max1586_v3_calc_voltage(max1586, selector);
-}
-
-/*
- * V6 voltage
- * On I2C bus, sending a "x" byte to the max1586 means :
- *   set V6 to either 0V, 1.8V, 2.5V, 3V depending on (x & 0x3)
- * As regulator framework doesn't accept voltages to be 0V, we use 1uV.
- */
-static int max1586_v6_calc_voltage(unsigned selector)
-{
-	static int voltages_uv[] = { 1, 1800000, 2500000, 3000000 };
-
-	return voltages_uv[selector];
-}
-
-static int max1586_v6_set(struct regulator_dev *rdev, int min_uV, int max_uV,
-			  unsigned int *selector)
+static int max1586_v6_set_voltage_sel(struct regulator_dev *rdev,
+				      unsigned int selector)
 {
 	struct i2c_client *client = rdev_get_drvdata(rdev);
 	u8 v6_prog;
 
-	if (min_uV < MAX1586_V6_MIN_UV || min_uV > MAX1586_V6_MAX_UV)
-		return -EINVAL;
-	if (max_uV < MAX1586_V6_MIN_UV || max_uV > MAX1586_V6_MAX_UV)
-		return -EINVAL;
-
-	if (min_uV < 1800000)
-		*selector = 0;
-	else if (min_uV < 2500000)
-		*selector = 1;
-	else if (min_uV < 3000000)
-		*selector = 2;
-	else if (min_uV >= 3000000)
-		*selector = 3;
-
-	if (max1586_v6_calc_voltage(*selector) > max_uV)
-		return -EINVAL;
-
 	dev_dbg(&client->dev, "changing voltage v6 to %dmv\n",
-		max1586_v6_calc_voltage(*selector) / 1000);
+		rdev->desc->volt_table[selector] / 1000);
 
-	v6_prog = I2C_V6_SELECT | (u8) *selector;
+	v6_prog = I2C_V6_SELECT | (u8) selector;
 	return i2c_smbus_write_byte(client, v6_prog);
 }
 
-static int max1586_v6_list(struct regulator_dev *rdev, unsigned selector)
-{
-	if (selector > MAX1586_V6_MAX_VSEL)
-		return -EINVAL;
-	return max1586_v6_calc_voltage(selector);
-}
-
 /*
  * The Maxim 1586 controls V3 and V6 voltages, but offers no way of reading back
  * the set up value.
  */
 static struct regulator_ops max1586_v3_ops = {
-	.set_voltage = max1586_v3_set,
-	.list_voltage = max1586_v3_list,
+	.set_voltage_sel = max1586_v3_set_voltage_sel,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
 };
 
 static struct regulator_ops max1586_v6_ops = {
-	.set_voltage = max1586_v6_set,
-	.list_voltage = max1586_v6_list,
+	.set_voltage_sel = max1586_v6_set_voltage_sel,
+	.list_voltage = regulator_list_voltage_table,
 };
 
-static const struct regulator_desc max1586_reg[] = {
+static struct regulator_desc max1586_reg[] = {
 	{
 		.name = "Output_V3",
 		.id = MAX1586_V3,
@@ -176,6 +120,7 @@ static const struct regulator_desc max1586_reg[] = {
 		.ops = &max1586_v6_ops,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = MAX1586_V6_MAX_VSEL + 1,
+		.volt_table = v6_voltages_uv,
 		.owner = THIS_MODULE,
 	},
 };
@@ -213,6 +158,13 @@ static int __devinit max1586_pmic_probe(struct i2c_client *client,
 			goto err;
 		}
 
+		if (id == MAX1586_V3) {
+			max1586_reg[id].min_uV = max1586->min_uV;
+			max1586_reg[id].uV_step =
+					(max1586->max_uV - max1586->min_uV) /
+					MAX1586_V3_MAX_VSEL;
+		}
+
 		config.dev = &client->dev;
 		config.init_data = pdata->subdevs[i].platform_data;
 		config.driver_data = max1586;
diff --git a/drivers/regulator/max77686.c b/drivers/regulator/max77686.c
new file mode 100644
index 00000000000..c564af6f05a
--- /dev/null
+++ b/drivers/regulator/max77686.c
@@ -0,0 +1,389 @@
+/*
+ * max77686.c - Regulator driver for the Maxim 77686
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * Chiwoong Byun <woong.byun@smasung.com>
+ * Jonghwa Lee <jonghwa3.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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 is based on max8997.c
+ */
+
+#include <linux/kernel.h>
+#include <linux/bug.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/mfd/max77686.h>
+#include <linux/mfd/max77686-private.h>
+
+#define MAX77686_LDO_MINUV	800000
+#define MAX77686_LDO_UVSTEP	50000
+#define MAX77686_LDO_LOW_MINUV	800000
+#define MAX77686_LDO_LOW_UVSTEP	25000
+#define MAX77686_BUCK_MINUV	750000
+#define MAX77686_BUCK_UVSTEP	50000
+#define MAX77686_RAMP_DELAY	100000			/* uV/us */
+#define MAX77686_DVS_RAMP_DELAY	27500			/* uV/us */
+#define MAX77686_DVS_MINUV	600000
+#define MAX77686_DVS_UVSTEP	12500
+
+#define MAX77686_OPMODE_SHIFT	6
+#define MAX77686_OPMODE_BUCK234_SHIFT	4
+#define MAX77686_OPMODE_MASK	0x3
+
+#define MAX77686_VSEL_MASK	0x3F
+#define MAX77686_DVS_VSEL_MASK	0xFF
+
+#define MAX77686_RAMP_RATE_MASK	0xC0
+
+#define MAX77686_REGULATORS	MAX77686_REG_MAX
+#define MAX77686_LDOS		26
+
+enum max77686_ramp_rate {
+	RAMP_RATE_13P75MV,
+	RAMP_RATE_27P5MV,
+	RAMP_RATE_55MV,
+	RAMP_RATE_NO_CTRL,	/* 100mV/us */
+};
+
+struct max77686_data {
+	struct regulator_dev **rdev;
+};
+
+static int max77686_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
+{
+	unsigned int ramp_value = RAMP_RATE_NO_CTRL;
+
+	switch (ramp_delay) {
+	case 1 ... 13750:
+		ramp_value = RAMP_RATE_13P75MV;
+		break;
+	case 13751 ... 27500:
+		ramp_value = RAMP_RATE_27P5MV;
+		break;
+	case 27501 ... 55000:
+		ramp_value = RAMP_RATE_55MV;
+		break;
+	case 55001 ... 100000:
+		break;
+	default:
+		pr_warn("%s: ramp_delay: %d not supported, setting 100000\n",
+			rdev->desc->name, ramp_delay);
+	}
+
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  MAX77686_RAMP_RATE_MASK, ramp_value << 6);
+}
+
+static struct regulator_ops max77686_ops = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+};
+
+static struct regulator_ops max77686_buck_dvs_ops = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_ramp_delay		= max77686_set_ramp_delay,
+};
+
+#define regulator_desc_ldo(num)		{				\
+	.name		= "LDO"#num,					\
+	.id		= MAX77686_LDO##num,				\
+	.ops		= &max77686_ops,				\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= MAX77686_LDO_MINUV,				\
+	.uV_step	= MAX77686_LDO_UVSTEP,				\
+	.ramp_delay	= MAX77686_RAMP_DELAY,				\
+	.n_voltages	= MAX77686_VSEL_MASK + 1,			\
+	.vsel_reg	= MAX77686_REG_LDO1CTRL1 + num - 1,		\
+	.vsel_mask	= MAX77686_VSEL_MASK,				\
+	.enable_reg	= MAX77686_REG_LDO1CTRL1 + num - 1,		\
+	.enable_mask	= MAX77686_OPMODE_MASK				\
+			<< MAX77686_OPMODE_SHIFT,			\
+}
+#define regulator_desc_ldo_low(num)		{			\
+	.name		= "LDO"#num,					\
+	.id		= MAX77686_LDO##num,				\
+	.ops		= &max77686_ops,				\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= MAX77686_LDO_LOW_MINUV,			\
+	.uV_step	= MAX77686_LDO_LOW_UVSTEP,			\
+	.ramp_delay	= MAX77686_RAMP_DELAY,				\
+	.n_voltages	= MAX77686_VSEL_MASK + 1,			\
+	.vsel_reg	= MAX77686_REG_LDO1CTRL1 + num - 1,		\
+	.vsel_mask	= MAX77686_VSEL_MASK,				\
+	.enable_reg	= MAX77686_REG_LDO1CTRL1 + num - 1,		\
+	.enable_mask	= MAX77686_OPMODE_MASK				\
+			<< MAX77686_OPMODE_SHIFT,			\
+}
+#define regulator_desc_buck(num)		{			\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77686_BUCK##num,				\
+	.ops		= &max77686_ops,				\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= MAX77686_BUCK_MINUV,				\
+	.uV_step	= MAX77686_BUCK_UVSTEP,				\
+	.ramp_delay	= MAX77686_RAMP_DELAY,				\
+	.n_voltages	= MAX77686_VSEL_MASK + 1,			\
+	.vsel_reg	= MAX77686_REG_BUCK5OUT + (num - 5) * 2,	\
+	.vsel_mask	= MAX77686_VSEL_MASK,				\
+	.enable_reg	= MAX77686_REG_BUCK5CTRL + (num - 5) * 2,	\
+	.enable_mask	= MAX77686_OPMODE_MASK,				\
+}
+#define regulator_desc_buck1(num)		{			\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77686_BUCK##num,				\
+	.ops		= &max77686_ops,				\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= MAX77686_BUCK_MINUV,				\
+	.uV_step	= MAX77686_BUCK_UVSTEP,				\
+	.ramp_delay	= MAX77686_RAMP_DELAY,				\
+	.n_voltages	= MAX77686_VSEL_MASK + 1,			\
+	.vsel_reg	= MAX77686_REG_BUCK1OUT,			\
+	.vsel_mask	= MAX77686_VSEL_MASK,				\
+	.enable_reg	= MAX77686_REG_BUCK1CTRL,			\
+	.enable_mask	= MAX77686_OPMODE_MASK,				\
+}
+#define regulator_desc_buck_dvs(num)		{			\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77686_BUCK##num,				\
+	.ops		= &max77686_buck_dvs_ops,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= MAX77686_DVS_MINUV,				\
+	.uV_step	= MAX77686_DVS_UVSTEP,				\
+	.ramp_delay	= MAX77686_DVS_RAMP_DELAY,			\
+	.n_voltages	= MAX77686_DVS_VSEL_MASK + 1,			\
+	.vsel_reg	= MAX77686_REG_BUCK2DVS1 + (num - 2) * 10,	\
+	.vsel_mask	= MAX77686_DVS_VSEL_MASK,			\
+	.enable_reg	= MAX77686_REG_BUCK2CTRL1 + (num - 2) * 10,	\
+	.enable_mask	= MAX77686_OPMODE_MASK				\
+			<< MAX77686_OPMODE_BUCK234_SHIFT,		\
+}
+
+static struct regulator_desc regulators[] = {
+	regulator_desc_ldo_low(1),
+	regulator_desc_ldo_low(2),
+	regulator_desc_ldo(3),
+	regulator_desc_ldo(4),
+	regulator_desc_ldo(5),
+	regulator_desc_ldo_low(6),
+	regulator_desc_ldo_low(7),
+	regulator_desc_ldo_low(8),
+	regulator_desc_ldo(9),
+	regulator_desc_ldo(10),
+	regulator_desc_ldo(11),
+	regulator_desc_ldo(12),
+	regulator_desc_ldo(13),
+	regulator_desc_ldo(14),
+	regulator_desc_ldo_low(15),
+	regulator_desc_ldo(16),
+	regulator_desc_ldo(17),
+	regulator_desc_ldo(18),
+	regulator_desc_ldo(19),
+	regulator_desc_ldo(20),
+	regulator_desc_ldo(21),
+	regulator_desc_ldo(22),
+	regulator_desc_ldo(23),
+	regulator_desc_ldo(24),
+	regulator_desc_ldo(25),
+	regulator_desc_ldo(26),
+	regulator_desc_buck1(1),
+	regulator_desc_buck_dvs(2),
+	regulator_desc_buck_dvs(3),
+	regulator_desc_buck_dvs(4),
+	regulator_desc_buck(5),
+	regulator_desc_buck(6),
+	regulator_desc_buck(7),
+	regulator_desc_buck(8),
+	regulator_desc_buck(9),
+};
+
+#ifdef CONFIG_OF
+static int max77686_pmic_dt_parse_pdata(struct max77686_dev *iodev,
+					struct max77686_platform_data *pdata)
+{
+	struct device_node *pmic_np, *regulators_np;
+	struct max77686_regulator_data *rdata;
+	struct of_regulator_match rmatch;
+	unsigned int i;
+
+	pmic_np = iodev->dev->of_node;
+	regulators_np = of_find_node_by_name(pmic_np, "voltage-regulators");
+	if (!regulators_np) {
+		dev_err(iodev->dev, "could not find regulators sub-node\n");
+		return -EINVAL;
+	}
+
+	pdata->num_regulators = ARRAY_SIZE(regulators);
+	rdata = devm_kzalloc(iodev->dev, sizeof(*rdata) *
+			     pdata->num_regulators, GFP_KERNEL);
+	if (!rdata) {
+		dev_err(iodev->dev,
+			"could not allocate memory for regulator data\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < pdata->num_regulators; i++) {
+		rmatch.name = regulators[i].name;
+		rmatch.init_data = NULL;
+		rmatch.of_node = NULL;
+		of_regulator_match(iodev->dev, regulators_np, &rmatch, 1);
+		rdata[i].initdata = rmatch.init_data;
+	}
+
+	pdata->regulators = rdata;
+
+	return 0;
+}
+#else
+static int max77686_pmic_dt_parse_pdata(struct max77686_dev *iodev,
+					struct max77686_platform_data *pdata)
+{
+	return 0;
+}
+#endif /* CONFIG_OF */
+
+static __devinit int max77686_pmic_probe(struct platform_device *pdev)
+{
+	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct max77686_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct regulator_dev **rdev;
+	struct max77686_data *max77686;
+	int i,  size;
+	int ret = 0;
+	struct regulator_config config = { };
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data found for regulator\n");
+		return -ENODEV;
+	}
+
+	if (iodev->dev->of_node) {
+		ret = max77686_pmic_dt_parse_pdata(iodev, pdata);
+		if (ret)
+			return ret;
+	}
+
+	if (pdata->num_regulators != MAX77686_REGULATORS) {
+		dev_err(&pdev->dev,
+			"Invalid initial data for regulator's initialiation\n");
+		return -EINVAL;
+	}
+
+	max77686 = devm_kzalloc(&pdev->dev, sizeof(struct max77686_data),
+				GFP_KERNEL);
+	if (!max77686)
+		return -ENOMEM;
+
+	size = sizeof(struct regulator_dev *) * MAX77686_REGULATORS;
+	max77686->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	if (!max77686->rdev)
+		return -ENOMEM;
+
+	rdev = max77686->rdev;
+	config.dev = &pdev->dev;
+	config.regmap = iodev->regmap;
+	platform_set_drvdata(pdev, max77686);
+
+	for (i = 0; i < MAX77686_REGULATORS; i++) {
+		config.init_data = pdata->regulators[i].initdata;
+
+		rdev[i] = regulator_register(&regulators[i], &config);
+		if (IS_ERR(rdev[i])) {
+			ret = PTR_ERR(rdev[i]);
+			dev_err(&pdev->dev,
+				"regulator init failed for %d\n", i);
+				rdev[i] = NULL;
+				goto err;
+		}
+	}
+
+	return 0;
+err:
+	while (--i >= 0)
+		regulator_unregister(rdev[i]);
+	return ret;
+}
+
+static int __devexit max77686_pmic_remove(struct platform_device *pdev)
+{
+	struct max77686_data *max77686 = platform_get_drvdata(pdev);
+	struct regulator_dev **rdev = max77686->rdev;
+	int i;
+
+	for (i = 0; i < MAX77686_REGULATORS; i++)
+		if (rdev[i])
+			regulator_unregister(rdev[i]);
+
+	return 0;
+}
+
+static const struct platform_device_id max77686_pmic_id[] = {
+	{"max77686-pmic", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, max77686_pmic_id);
+
+static struct platform_driver max77686_pmic_driver = {
+	.driver = {
+		.name = "max77686-pmic",
+		.owner = THIS_MODULE,
+	},
+	.probe = max77686_pmic_probe,
+	.remove = __devexit_p(max77686_pmic_remove),
+	.id_table = max77686_pmic_id,
+};
+
+static int __init max77686_pmic_init(void)
+{
+	return platform_driver_register(&max77686_pmic_driver);
+}
+subsys_initcall(max77686_pmic_init);
+
+static void __exit max77686_pmic_cleanup(void)
+{
+	platform_driver_unregister(&max77686_pmic_driver);
+}
+module_exit(max77686_pmic_cleanup);
+
+MODULE_DESCRIPTION("MAXIM 77686 Regulator Driver");
+MODULE_AUTHOR("Chiwoong Byun <woong.byun@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/max8952.c b/drivers/regulator/max8952.c
index 910c9b26d49..355ca7bad9d 100644
--- a/drivers/regulator/max8952.c
+++ b/drivers/regulator/max8952.c
@@ -51,7 +51,6 @@ struct max8952_data {
 
 	bool vid0;
 	bool vid1;
-	bool en;
 };
 
 static int max8952_read_reg(struct max8952_data *max8952, u8 reg)
@@ -80,38 +79,6 @@ static int max8952_list_voltage(struct regulator_dev *rdev,
 	return (max8952->pdata->dvs_mode[selector] * 10 + 770) * 1000;
 }
 
-static int max8952_is_enabled(struct regulator_dev *rdev)
-{
-	struct max8952_data *max8952 = rdev_get_drvdata(rdev);
-	return max8952->en;
-}
-
-static int max8952_enable(struct regulator_dev *rdev)
-{
-	struct max8952_data *max8952 = rdev_get_drvdata(rdev);
-
-	/* If not valid, assume "ALWAYS_HIGH" */
-	if (gpio_is_valid(max8952->pdata->gpio_en))
-		gpio_set_value(max8952->pdata->gpio_en, 1);
-
-	max8952->en = true;
-	return 0;
-}
-
-static int max8952_disable(struct regulator_dev *rdev)
-{
-	struct max8952_data *max8952 = rdev_get_drvdata(rdev);
-
-	/* If not valid, assume "ALWAYS_HIGH" -> not permitted */
-	if (gpio_is_valid(max8952->pdata->gpio_en))
-		gpio_set_value(max8952->pdata->gpio_en, 0);
-	else
-		return -EPERM;
-
-	max8952->en = false;
-	return 0;
-}
-
 static int max8952_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct max8952_data *max8952 = rdev_get_drvdata(rdev);
@@ -146,12 +113,8 @@ static int max8952_set_voltage_sel(struct regulator_dev *rdev,
 
 static struct regulator_ops max8952_ops = {
 	.list_voltage		= max8952_list_voltage,
-	.is_enabled		= max8952_is_enabled,
-	.enable			= max8952_enable,
-	.disable		= max8952_disable,
 	.get_voltage_sel	= max8952_get_voltage_sel,
 	.set_voltage_sel	= max8952_set_voltage_sel,
-	.set_suspend_disable	= max8952_disable,
 };
 
 static const struct regulator_desc regulator = {
@@ -194,6 +157,10 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 	config.init_data = &pdata->reg_data;
 	config.driver_data = max8952;
 
+	config.ena_gpio = pdata->gpio_en;
+	if (pdata->reg_data.constraints.boot_on)
+		config.ena_gpio_flags |= GPIOF_OUT_INIT_HIGH;
+
 	max8952->rdev = regulator_register(&regulator, &config);
 
 	if (IS_ERR(max8952->rdev)) {
@@ -202,27 +169,9 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 		return ret;
 	}
 
-	max8952->en = !!(pdata->reg_data.constraints.boot_on);
 	max8952->vid0 = pdata->default_mode & 0x1;
 	max8952->vid1 = (pdata->default_mode >> 1) & 0x1;
 
-	if (gpio_is_valid(pdata->gpio_en)) {
-		if (!gpio_request(pdata->gpio_en, "MAX8952 EN"))
-			gpio_direction_output(pdata->gpio_en, max8952->en);
-		else
-			err = 1;
-	} else
-		err = 2;
-
-	if (err) {
-		dev_info(max8952->dev, "EN gpio invalid: assume that EN"
-				"is always High\n");
-		max8952->en = 1;
-		pdata->gpio_en = -1; /* Mark invalid */
-	}
-
-	err = 0;
-
 	if (gpio_is_valid(pdata->gpio_vid0) &&
 			gpio_is_valid(pdata->gpio_vid1)) {
 		if (!gpio_request(pdata->gpio_vid0, "MAX8952 VID0"))
@@ -308,7 +257,6 @@ static int __devexit max8952_pmic_remove(struct i2c_client *client)
 
 	gpio_free(pdata->gpio_vid0);
 	gpio_free(pdata->gpio_vid1);
-	gpio_free(pdata->gpio_en);
 	return 0;
 }
 
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
index 704cd49ef37..e39a0c7260d 100644
--- a/drivers/regulator/max8997.c
+++ b/drivers/regulator/max8997.c
@@ -1025,7 +1025,6 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 	 */
 	if (pdata->buck1_gpiodvs || pdata->buck2_gpiodvs ||
 			pdata->buck5_gpiodvs) {
-		bool gpio1set = false, gpio2set = false;
 
 		if (!gpio_is_valid(pdata->buck125_gpios[0]) ||
 				!gpio_is_valid(pdata->buck125_gpios[1]) ||
@@ -1035,40 +1034,20 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 			goto err_out;
 		}
 
-		ret = gpio_request(pdata->buck125_gpios[0],
-				"MAX8997 SET1");
-		if (ret == -EBUSY)
-			dev_warn(&pdev->dev, "Duplicated gpio request"
-					" on SET1\n");
-		else if (ret)
+		ret = devm_gpio_request(&pdev->dev, pdata->buck125_gpios[0],
+					"MAX8997 SET1");
+		if (ret)
 			goto err_out;
-		else
-			gpio1set = true;
-
-		ret = gpio_request(pdata->buck125_gpios[1],
-				"MAX8997 SET2");
-		if (ret == -EBUSY)
-			dev_warn(&pdev->dev, "Duplicated gpio request"
-					" on SET2\n");
-		else if (ret) {
-			if (gpio1set)
-				gpio_free(pdata->buck125_gpios[0]);
+
+		ret = devm_gpio_request(&pdev->dev, pdata->buck125_gpios[1],
+					"MAX8997 SET2");
+		if (ret)
 			goto err_out;
-		} else
-			gpio2set = true;
 
-		ret = gpio_request(pdata->buck125_gpios[2],
+		ret = devm_gpio_request(&pdev->dev, pdata->buck125_gpios[2],
 				"MAX8997 SET3");
-		if (ret == -EBUSY)
-			dev_warn(&pdev->dev, "Duplicated gpio request"
-					" on SET3\n");
-		else if (ret) {
-			if (gpio1set)
-				gpio_free(pdata->buck125_gpios[0]);
-			if (gpio2set)
-				gpio_free(pdata->buck125_gpios[1]);
+		if (ret)
 			goto err_out;
-		}
 
 		gpio_direction_output(pdata->buck125_gpios[0],
 				(max8997->buck125_gpioindex >> 2)
@@ -1079,7 +1058,6 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 		gpio_direction_output(pdata->buck125_gpios[2],
 				(max8997->buck125_gpioindex >> 0)
 				& 0x1); /* SET3 */
-		ret = 0;
 	}
 
 	/* DVS-GPIO disabled */
diff --git a/drivers/regulator/max8998.c b/drivers/regulator/max8998.c
index 18bb58b9b96..5dfa920ff0c 100644
--- a/drivers/regulator/max8998.c
+++ b/drivers/regulator/max8998.c
@@ -111,27 +111,6 @@ static const struct voltage_map_desc *ldo_voltage_map[] = {
 	&buck4_voltage_map_desc,	/* BUCK4 */
 };
 
-static int max8998_list_voltage(struct regulator_dev *rdev,
-				unsigned int selector)
-{
-	const struct voltage_map_desc *desc;
-	int ldo = rdev_get_id(rdev);
-	int val;
-
-	if (ldo >= ARRAY_SIZE(ldo_voltage_map))
-		return -EINVAL;
-
-	desc = ldo_voltage_map[ldo];
-	if (desc == NULL)
-		return -EINVAL;
-
-	val = desc->min + desc->step * selector;
-	if (val > desc->max)
-		return -EINVAL;
-
-	return val * 1000;
-}
-
 static int max8998_get_enable_register(struct regulator_dev *rdev,
 					int *reg, int *shift)
 {
@@ -297,41 +276,18 @@ static int max8998_get_voltage_sel(struct regulator_dev *rdev)
 	return val;
 }
 
-static int max8998_set_voltage_ldo(struct regulator_dev *rdev,
-				   int min_uV, int max_uV, unsigned *selector)
+static int max8998_set_voltage_ldo_sel(struct regulator_dev *rdev,
+				       unsigned selector)
 {
 	struct max8998_data *max8998 = rdev_get_drvdata(rdev);
 	struct i2c_client *i2c = max8998->iodev->i2c;
-	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
-	const struct voltage_map_desc *desc;
-	int ldo = rdev_get_id(rdev);
-	int reg, shift = 0, mask, ret, i;
-
-	if (ldo >= ARRAY_SIZE(ldo_voltage_map))
-		return -EINVAL;
-
-	desc = ldo_voltage_map[ldo];
-	if (desc == NULL)
-		return -EINVAL;
-
-	if (max_vol < desc->min || min_vol > desc->max)
-		return -EINVAL;
-
-	if (min_vol < desc->min)
-		min_vol = desc->min;
-
-	i = DIV_ROUND_UP(min_vol - desc->min, desc->step);
-
-	if (desc->min + desc->step*i > max_vol)
-		return -EINVAL;
-
-	*selector = i;
+	int reg, shift = 0, mask, ret;
 
 	ret = max8998_get_voltage_register(rdev, &reg, &shift, &mask);
 	if (ret)
 		return ret;
 
-	ret = max8998_update_reg(i2c, reg, i<<shift, mask<<shift);
+	ret = max8998_update_reg(i2c, reg, selector<<shift, mask<<shift);
 
 	return ret;
 }
@@ -347,41 +303,18 @@ static inline void buck2_gpio_set(int gpio, int v)
 	gpio_set_value(gpio, v & 0x1);
 }
 
-static int max8998_set_voltage_buck(struct regulator_dev *rdev,
-				    int min_uV, int max_uV, unsigned *selector)
+static int max8998_set_voltage_buck_sel(struct regulator_dev *rdev,
+					unsigned selector)
 {
 	struct max8998_data *max8998 = rdev_get_drvdata(rdev);
 	struct max8998_platform_data *pdata =
 		dev_get_platdata(max8998->iodev->dev);
 	struct i2c_client *i2c = max8998->iodev->i2c;
-	int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
-	const struct voltage_map_desc *desc;
 	int buck = rdev_get_id(rdev);
 	int reg, shift = 0, mask, ret;
-	int i, j, previous_sel;
+	int j, previous_sel;
 	static u8 buck1_last_val;
 
-	if (buck >= ARRAY_SIZE(ldo_voltage_map))
-		return -EINVAL;
-
-	desc = ldo_voltage_map[buck];
-
-	if (desc == NULL)
-		return -EINVAL;
-
-	if (max_vol < desc->min || min_vol > desc->max)
-		return -EINVAL;
-
-	if (min_vol < desc->min)
-		min_vol = desc->min;
-
-	i = DIV_ROUND_UP(min_vol - desc->min, desc->step);
-
-	if (desc->min + desc->step*i > max_vol)
-		return -EINVAL;
-
-	*selector = i;
-
 	ret = max8998_get_voltage_register(rdev, &reg, &shift, &mask);
 	if (ret)
 		return ret;
@@ -390,19 +323,19 @@ static int max8998_set_voltage_buck(struct regulator_dev *rdev,
 
 	/* Check if voltage needs to be changed */
 	/* if previous_voltage equal new voltage, return */
-	if (previous_sel == i) {
+	if (previous_sel == selector) {
 		dev_dbg(max8998->dev, "No voltage change, old:%d, new:%d\n",
-			max8998_list_voltage(rdev, previous_sel),
-			max8998_list_voltage(rdev, i));
+			regulator_list_voltage_linear(rdev, previous_sel),
+			regulator_list_voltage_linear(rdev, selector));
 		return ret;
 	}
 
 	switch (buck) {
 	case MAX8998_BUCK1:
 		dev_dbg(max8998->dev,
-			"BUCK1, i:%d, buck1_vol1:%d, buck1_vol2:%d\n"
+			"BUCK1, selector:%d, buck1_vol1:%d, buck1_vol2:%d\n"
 			"buck1_vol3:%d, buck1_vol4:%d\n",
-			i, max8998->buck1_vol[0], max8998->buck1_vol[1],
+			selector, max8998->buck1_vol[0], max8998->buck1_vol[1],
 			max8998->buck1_vol[2], max8998->buck1_vol[3]);
 
 		if (gpio_is_valid(pdata->buck1_set1) &&
@@ -411,7 +344,7 @@ static int max8998_set_voltage_buck(struct regulator_dev *rdev,
 			/* check if requested voltage */
 			/* value is already defined */
 			for (j = 0; j < ARRAY_SIZE(max8998->buck1_vol); j++) {
-				if (max8998->buck1_vol[j] == i) {
+				if (max8998->buck1_vol[j] == selector) {
 					max8998->buck1_idx = j;
 					buck1_gpio_set(pdata->buck1_set1,
 						       pdata->buck1_set2, j);
@@ -426,11 +359,11 @@ static int max8998_set_voltage_buck(struct regulator_dev *rdev,
 			max8998->buck1_idx = (buck1_last_val % 2) + 2;
 			dev_dbg(max8998->dev, "max8998->buck1_idx:%d\n",
 				max8998->buck1_idx);
-			max8998->buck1_vol[max8998->buck1_idx] = i;
+			max8998->buck1_vol[max8998->buck1_idx] = selector;
 			ret = max8998_get_voltage_register(rdev, &reg,
 							   &shift,
 							   &mask);
-			ret = max8998_write_reg(i2c, reg, i);
+			ret = max8998_write_reg(i2c, reg, selector);
 			buck1_gpio_set(pdata->buck1_set1,
 				       pdata->buck1_set2, max8998->buck1_idx);
 			buck1_last_val++;
@@ -440,20 +373,20 @@ buck1_exit:
 				gpio_get_value(pdata->buck1_set2));
 			break;
 		} else {
-			ret = max8998_write_reg(i2c, reg, i);
+			ret = max8998_write_reg(i2c, reg, selector);
 		}
 		break;
 
 	case MAX8998_BUCK2:
 		dev_dbg(max8998->dev,
-			"BUCK2, i:%d buck2_vol1:%d, buck2_vol2:%d\n"
-			, i, max8998->buck2_vol[0], max8998->buck2_vol[1]);
+			"BUCK2, selector:%d buck2_vol1:%d, buck2_vol2:%d\n",
+			selector, max8998->buck2_vol[0], max8998->buck2_vol[1]);
 		if (gpio_is_valid(pdata->buck2_set3)) {
 
 			/* check if requested voltage */
 			/* value is already defined */
 			for (j = 0; j < ARRAY_SIZE(max8998->buck2_vol); j++) {
-				if (max8998->buck2_vol[j] == i) {
+				if (max8998->buck2_vol[j] == selector) {
 					max8998->buck2_idx = j;
 					buck2_gpio_set(pdata->buck2_set3, j);
 					goto buck2_exit;
@@ -465,20 +398,21 @@ buck1_exit:
 
 			max8998_get_voltage_register(rdev,
 					&reg, &shift, &mask);
-			ret = max8998_write_reg(i2c, reg, i);
-			max8998->buck2_vol[max8998->buck2_idx] = i;
+			ret = max8998_write_reg(i2c, reg, selector);
+			max8998->buck2_vol[max8998->buck2_idx] = selector;
 			buck2_gpio_set(pdata->buck2_set3, max8998->buck2_idx);
 buck2_exit:
 			dev_dbg(max8998->dev, "%s: SET3:%d\n", i2c->name,
 				gpio_get_value(pdata->buck2_set3));
 		} else {
-			ret = max8998_write_reg(i2c, reg, i);
+			ret = max8998_write_reg(i2c, reg, selector);
 		}
 		break;
 
 	case MAX8998_BUCK3:
 	case MAX8998_BUCK4:
-		ret = max8998_update_reg(i2c, reg, i<<shift, mask<<shift);
+		ret = max8998_update_reg(i2c, reg, selector<<shift,
+					 mask<<shift);
 		break;
 	}
 
@@ -519,34 +453,30 @@ static int max8998_set_voltage_buck_time_sel(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops max8998_ldo_ops = {
-	.list_voltage		= max8998_list_voltage,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
 	.is_enabled		= max8998_ldo_is_enabled,
 	.enable			= max8998_ldo_enable,
 	.disable		= max8998_ldo_disable,
 	.get_voltage_sel	= max8998_get_voltage_sel,
-	.set_voltage		= max8998_set_voltage_ldo,
-	.set_suspend_enable	= max8998_ldo_enable,
-	.set_suspend_disable	= max8998_ldo_disable,
+	.set_voltage_sel	= max8998_set_voltage_ldo_sel,
 };
 
 static struct regulator_ops max8998_buck_ops = {
-	.list_voltage		= max8998_list_voltage,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
 	.is_enabled		= max8998_ldo_is_enabled,
 	.enable			= max8998_ldo_enable,
 	.disable		= max8998_ldo_disable,
 	.get_voltage_sel	= max8998_get_voltage_sel,
-	.set_voltage		= max8998_set_voltage_buck,
+	.set_voltage_sel	= max8998_set_voltage_buck_sel,
 	.set_voltage_time_sel	= max8998_set_voltage_buck_time_sel,
-	.set_suspend_enable	= max8998_ldo_enable,
-	.set_suspend_disable	= max8998_ldo_disable,
 };
 
 static struct regulator_ops max8998_others_ops = {
 	.is_enabled		= max8998_ldo_is_enabled,
 	.enable			= max8998_ldo_enable,
 	.disable		= max8998_ldo_disable,
-	.set_suspend_enable	= max8998_ldo_enable,
-	.set_suspend_disable	= max8998_ldo_disable,
 };
 
 static struct regulator_desc regulators[] = {
@@ -860,7 +790,10 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 		desc = ldo_voltage_map[id];
 		if (desc && regulators[index].ops != &max8998_others_ops) {
 			int count = (desc->max - desc->min) / desc->step + 1;
+
 			regulators[index].n_voltages = count;
+			regulators[index].min_uV = desc->min * 1000;
+			regulators[index].uV_step = desc->step * 1000;
 		}
 
 		config.dev = max8998->dev;
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 7dcdfa283e9..4932e3449fe 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -93,78 +93,78 @@
 
 
 /* Voltage Values */
-static const int mc13783_sw3_val[] = {
+static const unsigned int mc13783_sw3_val[] = {
 	5000000, 5000000, 5000000, 5500000,
 };
 
-static const int mc13783_vaudio_val[] = {
+static const unsigned int mc13783_vaudio_val[] = {
 	2775000,
 };
 
-static const int mc13783_viohi_val[] = {
+static const unsigned int mc13783_viohi_val[] = {
 	2775000,
 };
 
-static const int mc13783_violo_val[] = {
+static const unsigned int mc13783_violo_val[] = {
 	1200000, 1300000, 1500000, 1800000,
 };
 
-static const int mc13783_vdig_val[] = {
+static const unsigned int mc13783_vdig_val[] = {
 	1200000, 1300000, 1500000, 1800000,
 };
 
-static const int mc13783_vgen_val[] = {
+static const unsigned int mc13783_vgen_val[] = {
 	1200000, 1300000, 1500000, 1800000,
 	1100000, 2000000, 2775000, 2400000,
 };
 
-static const int mc13783_vrfdig_val[] = {
+static const unsigned int mc13783_vrfdig_val[] = {
 	1200000, 1500000, 1800000, 1875000,
 };
 
-static const int mc13783_vrfref_val[] = {
+static const unsigned int mc13783_vrfref_val[] = {
 	2475000, 2600000, 2700000, 2775000,
 };
 
-static const int mc13783_vrfcp_val[] = {
+static const unsigned int mc13783_vrfcp_val[] = {
 	2700000, 2775000,
 };
 
-static const int mc13783_vsim_val[] = {
+static const unsigned int mc13783_vsim_val[] = {
 	1800000, 2900000, 3000000,
 };
 
-static const int mc13783_vesim_val[] = {
+static const unsigned int mc13783_vesim_val[] = {
 	1800000, 2900000,
 };
 
-static const int mc13783_vcam_val[] = {
+static const unsigned int mc13783_vcam_val[] = {
 	1500000, 1800000, 2500000, 2550000,
 	2600000, 2750000, 2800000, 3000000,
 };
 
-static const int mc13783_vrfbg_val[] = {
+static const unsigned int mc13783_vrfbg_val[] = {
 	1250000,
 };
 
-static const int mc13783_vvib_val[] = {
+static const unsigned int mc13783_vvib_val[] = {
 	1300000, 1800000, 2000000, 3000000,
 };
 
-static const int mc13783_vmmc_val[] = {
+static const unsigned int mc13783_vmmc_val[] = {
 	1600000, 1800000, 2000000, 2600000,
 	2700000, 2800000, 2900000, 3000000,
 };
 
-static const int mc13783_vrf_val[] = {
+static const unsigned int mc13783_vrf_val[] = {
 	1500000, 1875000, 2700000, 2775000,
 };
 
-static const int mc13783_gpo_val[] = {
+static const unsigned int mc13783_gpo_val[] = {
 	3100000,
 };
 
-static const int mc13783_pwgtdrv_val[] = {
+static const unsigned int mc13783_pwgtdrv_val[] = {
 	5500000,
 };
 
@@ -328,7 +328,7 @@ static struct regulator_ops mc13783_gpo_regulator_ops = {
 	.enable = mc13783_gpo_regulator_enable,
 	.disable = mc13783_gpo_regulator_disable,
 	.is_enabled = mc13783_gpo_regulator_is_enabled,
-	.list_voltage = mc13xxx_regulator_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.set_voltage = mc13xxx_fixed_regulator_set_voltage,
 	.get_voltage = mc13xxx_fixed_regulator_get_voltage,
 };
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 970a233dbe4..b388b746452 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -150,12 +150,12 @@
 #define MC13892_USB1				50
 #define MC13892_USB1_VUSBEN			(1<<3)
 
-static const int mc13892_vcoincell[] = {
+static const unsigned int mc13892_vcoincell[] = {
 	2500000, 2700000, 2800000, 2900000, 3000000, 3100000,
 	3200000, 3300000,
 };
 
-static const int mc13892_sw1[] = {
+static const unsigned int mc13892_sw1[] = {
 	600000,   625000,  650000,  675000,  700000,  725000,
 	750000,   775000,  800000,  825000,  850000,  875000,
 	900000,   925000,  950000,  975000, 1000000, 1025000,
@@ -164,7 +164,7 @@ static const int mc13892_sw1[] = {
 	1350000, 1375000
 };
 
-static const int mc13892_sw[] = {
+static const unsigned int mc13892_sw[] = {
 	600000,   625000,  650000,  675000,  700000,  725000,
 	750000,   775000,  800000,  825000,  850000,  875000,
 	900000,   925000,  950000,  975000, 1000000, 1025000,
@@ -176,65 +176,65 @@ static const int mc13892_sw[] = {
 	1800000, 1825000, 1850000, 1875000
 };
 
-static const int mc13892_swbst[] = {
+static const unsigned int mc13892_swbst[] = {
 	5000000,
 };
 
-static const int mc13892_viohi[] = {
+static const unsigned int mc13892_viohi[] = {
 	2775000,
 };
 
-static const int mc13892_vpll[] = {
+static const unsigned int mc13892_vpll[] = {
 	1050000, 1250000, 1650000, 1800000,
 };
 
-static const int mc13892_vdig[] = {
+static const unsigned int mc13892_vdig[] = {
 	1050000, 1250000, 1650000, 1800000,
 };
 
-static const int mc13892_vsd[] = {
+static const unsigned int mc13892_vsd[] = {
 	1800000, 2000000, 2600000, 2700000,
 	2800000, 2900000, 3000000, 3150000,
 };
 
-static const int mc13892_vusb2[] = {
+static const unsigned int mc13892_vusb2[] = {
 	2400000, 2600000, 2700000, 2775000,
 };
 
-static const int mc13892_vvideo[] = {
+static const unsigned int mc13892_vvideo[] = {
 	2700000, 2775000, 2500000, 2600000,
 };
 
-static const int mc13892_vaudio[] = {
+static const unsigned int mc13892_vaudio[] = {
 	2300000, 2500000, 2775000, 3000000,
 };
 
-static const int mc13892_vcam[] = {
+static const unsigned int mc13892_vcam[] = {
 	2500000, 2600000, 2750000, 3000000,
 };
 
-static const int mc13892_vgen1[] = {
+static const unsigned int mc13892_vgen1[] = {
 	1200000, 1500000, 2775000, 3150000,
 };
 
-static const int mc13892_vgen2[] = {
+static const unsigned int mc13892_vgen2[] = {
 	1200000, 1500000, 1600000, 1800000,
 	2700000, 2800000, 3000000, 3150000,
 };
 
-static const int mc13892_vgen3[] = {
+static const unsigned int mc13892_vgen3[] = {
 	1800000, 2900000,
 };
 
-static const int mc13892_vusb[] = {
+static const unsigned int mc13892_vusb[] = {
 	3300000,
 };
 
-static const int mc13892_gpo[] = {
+static const unsigned int mc13892_gpo[] = {
 	2750000,
 };
 
-static const int mc13892_pwgtdrv[] = {
+static const unsigned int mc13892_pwgtdrv[] = {
 	5000000,
 };
 
@@ -394,7 +394,7 @@ static struct regulator_ops mc13892_gpo_regulator_ops = {
 	.enable = mc13892_gpo_regulator_enable,
 	.disable = mc13892_gpo_regulator_disable,
 	.is_enabled = mc13892_gpo_regulator_is_enabled,
-	.list_voltage = mc13xxx_regulator_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.set_voltage = mc13xxx_fixed_regulator_set_voltage,
 	.get_voltage = mc13xxx_fixed_regulator_get_voltage,
 };
@@ -436,7 +436,7 @@ static int mc13892_sw_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	u32 valread;
 	int ret;
 
-	value = mc13892_regulators[id].voltages[selector];
+	value = rdev->desc->volt_table[selector];
 
 	mc13xxx_lock(priv->mc13xxx);
 	ret = mc13xxx_reg_read(priv->mc13xxx,
@@ -469,8 +469,7 @@ err:
 }
 
 static struct regulator_ops mc13892_sw_regulator_ops = {
-	.is_enabled = mc13xxx_sw_regulator_is_enabled,
-	.list_voltage = mc13xxx_regulator_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.set_voltage_sel = mc13892_sw_regulator_set_voltage_sel,
 	.get_voltage = mc13892_sw_regulator_get_voltage,
 };
diff --git a/drivers/regulator/mc13xxx-regulator-core.c b/drivers/regulator/mc13xxx-regulator-core.c
index 4fa9704739b..d6eda28ca5d 100644
--- a/drivers/regulator/mc13xxx-regulator-core.c
+++ b/drivers/regulator/mc13xxx-regulator-core.c
@@ -80,20 +80,6 @@ static int mc13xxx_regulator_is_enabled(struct regulator_dev *rdev)
 	return (val & mc13xxx_regulators[id].enable_bit) != 0;
 }
 
-int mc13xxx_regulator_list_voltage(struct regulator_dev *rdev,
-						unsigned selector)
-{
-	int id = rdev_get_id(rdev);
-	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
-	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
-
-	if (selector >= mc13xxx_regulators[id].desc.n_voltages)
-		return -EINVAL;
-
-	return mc13xxx_regulators[id].voltages[selector];
-}
-EXPORT_SYMBOL_GPL(mc13xxx_regulator_list_voltage);
-
 static int mc13xxx_regulator_set_voltage_sel(struct regulator_dev *rdev,
 					     unsigned selector)
 {
@@ -135,14 +121,14 @@ static int mc13xxx_regulator_get_voltage(struct regulator_dev *rdev)
 
 	BUG_ON(val >= mc13xxx_regulators[id].desc.n_voltages);
 
-	return mc13xxx_regulators[id].voltages[val];
+	return rdev->desc->volt_table[val];
 }
 
 struct regulator_ops mc13xxx_regulator_ops = {
 	.enable = mc13xxx_regulator_enable,
 	.disable = mc13xxx_regulator_disable,
 	.is_enabled = mc13xxx_regulator_is_enabled,
-	.list_voltage = mc13xxx_regulator_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.set_voltage_sel = mc13xxx_regulator_set_voltage_sel,
 	.get_voltage = mc13xxx_regulator_get_voltage,
 };
@@ -151,15 +137,13 @@ EXPORT_SYMBOL_GPL(mc13xxx_regulator_ops);
 int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev, int min_uV,
 	       int max_uV, unsigned *selector)
 {
-	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
-	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
 	int id = rdev_get_id(rdev);
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d min_uV: %d max_uV: %d\n",
 		__func__, id, min_uV, max_uV);
 
-	if (min_uV >= mc13xxx_regulators[id].voltages[0] &&
-	    max_uV <= mc13xxx_regulators[id].voltages[0])
+	if (min_uV <= rdev->desc->volt_table[0] &&
+	    rdev->desc->volt_table[0] <= max_uV)
 		return 0;
 	else
 		return -EINVAL;
@@ -168,13 +152,11 @@ EXPORT_SYMBOL_GPL(mc13xxx_fixed_regulator_set_voltage);
 
 int mc13xxx_fixed_regulator_get_voltage(struct regulator_dev *rdev)
 {
-	struct mc13xxx_regulator_priv *priv = rdev_get_drvdata(rdev);
-	struct mc13xxx_regulator *mc13xxx_regulators = priv->mc13xxx_regulators;
 	int id = rdev_get_id(rdev);
 
 	dev_dbg(rdev_get_dev(rdev), "%s id: %d\n", __func__, id);
 
-	return mc13xxx_regulators[id].voltages[0];
+	return rdev->desc->volt_table[0];
 }
 EXPORT_SYMBOL_GPL(mc13xxx_fixed_regulator_get_voltage);
 
@@ -182,18 +164,12 @@ struct regulator_ops mc13xxx_fixed_regulator_ops = {
 	.enable = mc13xxx_regulator_enable,
 	.disable = mc13xxx_regulator_disable,
 	.is_enabled = mc13xxx_regulator_is_enabled,
-	.list_voltage = mc13xxx_regulator_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.set_voltage = mc13xxx_fixed_regulator_set_voltage,
 	.get_voltage = mc13xxx_fixed_regulator_get_voltage,
 };
 EXPORT_SYMBOL_GPL(mc13xxx_fixed_regulator_ops);
 
-int mc13xxx_sw_regulator_is_enabled(struct regulator_dev *rdev)
-{
-	return 1;
-}
-EXPORT_SYMBOL_GPL(mc13xxx_sw_regulator_is_enabled);
-
 #ifdef CONFIG_OF
 int __devinit mc13xxx_get_num_regulators_dt(struct platform_device *pdev)
 {
diff --git a/drivers/regulator/mc13xxx.h b/drivers/regulator/mc13xxx.h
index 044aba4d28e..eaff5510b6d 100644
--- a/drivers/regulator/mc13xxx.h
+++ b/drivers/regulator/mc13xxx.h
@@ -22,7 +22,6 @@ struct mc13xxx_regulator {
 	int vsel_shift;
 	int vsel_mask;
 	int hi_bit;
-	int const *voltages;
 };
 
 struct mc13xxx_regulator_priv {
@@ -33,10 +32,6 @@ struct mc13xxx_regulator_priv {
 	struct regulator_dev *regulators[];
 };
 
-extern int mc13xxx_sw_regulator(struct regulator_dev *rdev);
-extern int mc13xxx_sw_regulator_is_enabled(struct regulator_dev *rdev);
-extern int mc13xxx_regulator_list_voltage(struct regulator_dev *rdev,
-						unsigned selector);
 extern int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev,
 		int min_uV, int max_uV, unsigned *selector);
 extern int mc13xxx_fixed_regulator_get_voltage(struct regulator_dev *rdev);
@@ -68,6 +63,7 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 		.desc = {						\
 			.name = #_name,					\
 			.n_voltages = ARRAY_SIZE(_voltages),		\
+			.volt_table =  _voltages,			\
 			.ops = &_ops,			\
 			.type = REGULATOR_VOLTAGE,			\
 			.id = prefix ## _name,		\
@@ -78,7 +74,6 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 		.vsel_reg = prefix ## _vsel_reg,			\
 		.vsel_shift = prefix ## _vsel_reg ## _ ## _name ## VSEL,\
 		.vsel_mask = prefix ## _vsel_reg ## _ ## _name ## VSEL_M,\
-		.voltages =  _voltages,					\
 	}
 
 #define MC13xxx_FIXED_DEFINE(prefix, _name, _reg, _voltages, _ops)	\
@@ -86,6 +81,7 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 		.desc = {						\
 			.name = #_name,					\
 			.n_voltages = ARRAY_SIZE(_voltages),		\
+			.volt_table =  _voltages,			\
 			.ops = &_ops,		\
 			.type = REGULATOR_VOLTAGE,			\
 			.id = prefix ## _name,		\
@@ -93,7 +89,6 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 		},							\
 		.reg = prefix ## _reg,				\
 		.enable_bit = prefix ## _reg ## _ ## _name ## EN,	\
-		.voltages =  _voltages,					\
 	}
 
 #define MC13xxx_GPO_DEFINE(prefix, _name, _reg,  _voltages, _ops)	\
@@ -101,6 +96,7 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 		.desc = {						\
 			.name = #_name,					\
 			.n_voltages = ARRAY_SIZE(_voltages),		\
+			.volt_table =  _voltages,			\
 			.ops = &_ops,		\
 			.type = REGULATOR_VOLTAGE,			\
 			.id = prefix ## _name,		\
@@ -108,7 +104,6 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 		},							\
 		.reg = prefix ## _reg,				\
 		.enable_bit = prefix ## _reg ## _ ## _name ## EN,	\
-		.voltages =  _voltages,					\
 	}
 
 #define MC13xxx_DEFINE_SW(_name, _reg, _vsel_reg, _voltages, ops)	\
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index 56593b75168..3e4106f2bda 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -20,7 +20,7 @@ static void of_get_regulation_constraints(struct device_node *np,
 					struct regulator_init_data **init_data)
 {
 	const __be32 *min_uV, *max_uV, *uV_offset;
-	const __be32 *min_uA, *max_uA;
+	const __be32 *min_uA, *max_uA, *ramp_delay;
 	struct regulation_constraints *constraints = &(*init_data)->constraints;
 
 	constraints->name = of_get_property(np, "regulator-name", NULL);
@@ -60,6 +60,10 @@ static void of_get_regulation_constraints(struct device_node *np,
 		constraints->always_on = true;
 	else /* status change should be possible if not always on. */
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_STATUS;
+
+	ramp_delay = of_get_property(np, "regulator-ramp-delay", NULL);
+	if (ramp_delay)
+		constraints->ramp_delay = be32_to_cpu(*ramp_delay);
 }
 
 /**
@@ -88,15 +92,17 @@ struct regulator_init_data *of_get_regulator_init_data(struct device *dev,
 EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
 
 /**
- * of_regulator_match - extract regulator init data
+ * of_regulator_match - extract regulator init data when node
+ * property "regulator-compatible" matches with the regulator name.
  * @dev: device requesting the data
  * @node: parent device node of the regulators
  * @matches: match table for the regulators
  * @num_matches: number of entries in match table
  *
  * This function uses a match table specified by the regulator driver and
- * looks up the corresponding init data in the device tree. Note that the
- * match table is modified in place.
+ * looks up the corresponding init data in the device tree  if
+ * regulator-compatible matches. Note that the match table is modified
+ * in place.
  *
  * Returns the number of matches found or a negative error code on failure.
  */
@@ -106,27 +112,40 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 {
 	unsigned int count = 0;
 	unsigned int i;
+	const char *regulator_comp;
+	struct device_node *child;
 
 	if (!dev || !node)
 		return -EINVAL;
 
-	for (i = 0; i < num_matches; i++) {
-		struct of_regulator_match *match = &matches[i];
-		struct device_node *child;
-
-		child = of_find_node_by_name(node, match->name);
-		if (!child)
-			continue;
-
-		match->init_data = of_get_regulator_init_data(dev, child);
-		if (!match->init_data) {
-			dev_err(dev, "failed to parse DT for regulator %s\n",
+	for_each_child_of_node(node, child) {
+		regulator_comp = of_get_property(child,
+					"regulator-compatible", NULL);
+		if (!regulator_comp) {
+			dev_err(dev, "regulator-compatible is missing for node %s\n",
 				child->name);
-			return -EINVAL;
+			continue;
+		}
+		for (i = 0; i < num_matches; i++) {
+			struct of_regulator_match *match = &matches[i];
+			if (match->of_node)
+				continue;
+
+			if (strcmp(match->name, regulator_comp))
+				continue;
+
+			match->init_data =
+				of_get_regulator_init_data(dev, child);
+			if (!match->init_data) {
+				dev_err(dev,
+					"failed to parse DT for regulator %s\n",
+					child->name);
+				return -EINVAL;
+			}
+			match->of_node = child;
+			count++;
+			break;
 		}
-
-		match->of_node = child;
-		count++;
 	}
 
 	return count;
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 795f75a6ac3..17d19fbbc49 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -257,8 +257,7 @@ static int palmas_set_mode_smps(struct regulator_dev *dev, unsigned int mode)
 	unsigned int reg;
 
 	palmas_smps_read(pmic->palmas, palmas_regs_info[id].ctrl_addr, &reg);
-	reg &= ~PALMAS_SMPS12_CTRL_STATUS_MASK;
-	reg >>= PALMAS_SMPS12_CTRL_STATUS_SHIFT;
+	reg &= ~PALMAS_SMPS12_CTRL_MODE_ACTIVE_MASK;
 
 	switch (mode) {
 	case REGULATOR_MODE_NORMAL:
@@ -374,11 +373,22 @@ static int palmas_set_voltage_smps_sel(struct regulator_dev *dev,
 static int palmas_map_voltage_smps(struct regulator_dev *rdev,
 		int min_uV, int max_uV)
 {
+	struct palmas_pmic *pmic = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
 	int ret, voltage;
 
-	ret = ((min_uV - 500000) / 10000) + 1;
-	if (ret < 0)
-		return ret;
+	if (min_uV == 0)
+		return 0;
+
+	if (pmic->range[id]) { /* RANGE is x2 */
+		if (min_uV < 1000000)
+			min_uV = 1000000;
+		ret = DIV_ROUND_UP(min_uV - 1000000, 20000) + 1;
+	} else {		/* RANGE is x1 */
+		if (min_uV < 500000)
+			min_uV = 500000;
+		ret = DIV_ROUND_UP(min_uV - 500000, 10000) + 1;
+	}
 
 	/* Map back into a voltage to verify we're still in bounds */
 	voltage = palmas_list_voltage_smps(rdev, ret);
@@ -400,19 +410,14 @@ static struct regulator_ops palmas_ops_smps = {
 	.map_voltage		= palmas_map_voltage_smps,
 };
 
-static int palmas_list_voltage_smps10(struct regulator_dev *dev,
-					unsigned selector)
-{
-	return 3750000 + (selector * 1250000);
-}
-
 static struct regulator_ops palmas_ops_smps10 = {
 	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
-	.list_voltage		= palmas_list_voltage_smps10,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
 };
 
 static int palmas_is_enabled_ldo(struct regulator_dev *dev)
@@ -522,7 +527,15 @@ static int palmas_smps_init(struct palmas *palmas, int id,
 	if (ret)
 		return ret;
 
-	if (id != PALMAS_REG_SMPS10) {
+	switch (id) {
+	case PALMAS_REG_SMPS10:
+		if (reg_init->mode_sleep) {
+			reg &= ~PALMAS_SMPS10_CTRL_MODE_SLEEP_MASK;
+			reg |= reg_init->mode_sleep <<
+					PALMAS_SMPS10_CTRL_MODE_SLEEP_SHIFT;
+		}
+		break;
+	default:
 		if (reg_init->warm_reset)
 			reg |= PALMAS_SMPS12_CTRL_WR_S;
 
@@ -534,14 +547,8 @@ static int palmas_smps_init(struct palmas *palmas, int id,
 			reg |= reg_init->mode_sleep <<
 					PALMAS_SMPS12_CTRL_MODE_SLEEP_SHIFT;
 		}
-	} else {
-		if (reg_init->mode_sleep) {
-			reg &= ~PALMAS_SMPS10_CTRL_MODE_SLEEP_MASK;
-			reg |= reg_init->mode_sleep <<
-					PALMAS_SMPS10_CTRL_MODE_SLEEP_SHIFT;
-		}
-
 	}
+
 	ret = palmas_smps_write(palmas, addr, reg);
 	if (ret)
 		return ret;
@@ -665,10 +672,8 @@ static __devinit int palmas_probe(struct platform_device *pdev)
 		pmic->desc[id].name = palmas_regs_info[id].name;
 		pmic->desc[id].id = id;
 
-		if (id != PALMAS_REG_SMPS10) {
-			pmic->desc[id].ops = &palmas_ops_smps;
-			pmic->desc[id].n_voltages = PALMAS_SMPS_NUM_VOLTAGES;
-		} else {
+		switch (id) {
+		case PALMAS_REG_SMPS10:
 			pmic->desc[id].n_voltages = PALMAS_SMPS10_NUM_VOLTAGES;
 			pmic->desc[id].ops = &palmas_ops_smps10;
 			pmic->desc[id].vsel_reg = PALMAS_SMPS10_CTRL;
@@ -677,6 +682,12 @@ static __devinit int palmas_probe(struct platform_device *pdev)
 					PALMAS_BASE_TO_REG(PALMAS_SMPS_BASE,
 							PALMAS_SMPS10_STATUS);
 			pmic->desc[id].enable_mask = SMPS10_BOOST_EN;
+			pmic->desc[id].min_uV = 3750000;
+			pmic->desc[id].uV_step = 1250000;
+			break;
+		default:
+			pmic->desc[id].ops = &palmas_ops_smps;
+			pmic->desc[id].n_voltages = PALMAS_SMPS_NUM_VOLTAGES;
 		}
 
 		pmic->desc[id].type = REGULATOR_VOLTAGE;
diff --git a/drivers/regulator/pcap-regulator.c b/drivers/regulator/pcap-regulator.c
index 8211101121f..68777acc099 100644
--- a/drivers/regulator/pcap-regulator.c
+++ b/drivers/regulator/pcap-regulator.c
@@ -18,80 +18,80 @@
 #include <linux/regulator/machine.h>
 #include <linux/mfd/ezx-pcap.h>
 
-static const u16 V1_table[] = {
-	2775, 1275, 1600, 1725, 1825, 1925, 2075, 2275,
+static const unsigned int V1_table[] = {
+	2775000, 1275000, 1600000, 1725000, 1825000, 1925000, 2075000, 2275000,
 };
 
-static const u16 V2_table[] = {
-	2500, 2775,
+static const unsigned int V2_table[] = {
+	2500000, 2775000,
 };
 
-static const u16 V3_table[] = {
-	1075, 1275, 1550, 1725, 1876, 1950, 2075, 2275,
+static const unsigned int V3_table[] = {
+	1075000, 1275000, 1550000, 1725000, 1876000, 1950000, 2075000, 2275000,
 };
 
-static const u16 V4_table[] = {
-	1275, 1550, 1725, 1875, 1950, 2075, 2275, 2775,
+static const unsigned int V4_table[] = {
+	1275000, 1550000, 1725000, 1875000, 1950000, 2075000, 2275000, 2775000,
 };
 
-static const u16 V5_table[] = {
-	1875, 2275, 2475, 2775,
+static const unsigned int V5_table[] = {
+	1875000, 2275000, 2475000, 2775000,
 };
 
-static const u16 V6_table[] = {
-	2475, 2775,
+static const unsigned int V6_table[] = {
+	2475000, 2775000,
 };
 
-static const u16 V7_table[] = {
-	1875, 2775,
+static const unsigned int V7_table[] = {
+	1875000, 2775000,
 };
 
 #define V8_table V4_table
 
-static const u16 V9_table[] = {
-	1575, 1875, 2475, 2775,
+static const unsigned int V9_table[] = {
+	1575000, 1875000, 2475000, 2775000,
 };
 
-static const u16 V10_table[] = {
-	5000,
+static const unsigned int V10_table[] = {
+	5000000,
 };
 
-static const u16 VAUX1_table[] = {
-	1875, 2475, 2775, 3000,
+static const unsigned int VAUX1_table[] = {
+	1875000, 2475000, 2775000, 3000000,
 };
 
 #define VAUX2_table VAUX1_table
 
-static const u16 VAUX3_table[] = {
-	1200, 1200, 1200, 1200, 1400, 1600, 1800, 2000,
-	2200, 2400, 2600, 2800, 3000, 3200, 3400, 3600,
+static const unsigned int VAUX3_table[] = {
+	1200000, 1200000, 1200000, 1200000, 1400000, 1600000, 1800000, 2000000,
+	2200000, 2400000, 2600000, 2800000, 3000000, 3200000, 3400000, 3600000,
 };
 
-static const u16 VAUX4_table[] = {
-	1800, 1800, 3000, 5000,
+static const unsigned int VAUX4_table[] = {
+	1800000, 1800000, 3000000, 5000000,
 };
 
-static const u16 VSIM_table[] = {
-	1875, 3000,
+static const unsigned int VSIM_table[] = {
+	1875000, 3000000,
 };
 
-static const u16 VSIM2_table[] = {
-	1875,
+static const unsigned int VSIM2_table[] = {
+	1875000,
 };
 
-static const u16 VVIB_table[] = {
-	1300, 1800, 2000, 3000,
+static const unsigned int VVIB_table[] = {
+	1300000, 1800000, 2000000, 3000000,
 };
 
-static const u16 SW1_table[] = {
-	900, 950, 1000, 1050, 1100, 1150, 1200, 1250,
-	1300, 1350, 1400, 1450, 1500, 1600, 1875, 2250,
+static const unsigned int SW1_table[] = {
+	 900000,  950000, 1000000, 1050000, 1100000, 1150000, 1200000, 1250000,
+	1300000, 1350000, 1400000, 1450000, 1500000, 1600000, 1875000, 2250000,
 };
 
 #define SW2_table SW1_table
 
-static const u16 SW3_table[] = {
-	4000, 4500, 5000, 5500,
+static const unsigned int SW3_table[] = {
+	4000000, 4500000, 5000000, 5500000,
 };
 
 struct pcap_regulator {
@@ -100,8 +100,6 @@ struct pcap_regulator {
 	const u8 index;
 	const u8 stby;
 	const u8 lowpwr;
-	const u8 n_voltages;
-	const u16 *voltage_table;
 };
 
 #define NA 0xff
@@ -113,8 +111,6 @@ struct pcap_regulator {
 		.index		= _index,				\
 		.stby		= _stby,				\
 		.lowpwr		= _lowpwr,				\
-		.n_voltages	= ARRAY_SIZE(_vreg##_table),		\
-		.voltage_table	= _vreg##_table,			\
 	}
 
 static struct pcap_regulator vreg_table[] = {
@@ -157,11 +153,11 @@ static int pcap_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	void *pcap = rdev_get_drvdata(rdev);
 
 	/* the regulator doesn't support voltage switching */
-	if (vreg->n_voltages == 1)
+	if (rdev->desc->n_voltages == 1)
 		return -EINVAL;
 
 	return ezx_pcap_set_bits(pcap, vreg->reg,
-				 (vreg->n_voltages - 1) << vreg->index,
+				 (rdev->desc->n_voltages - 1) << vreg->index,
 				 selector << vreg->index);
 }
 
@@ -171,11 +167,11 @@ static int pcap_regulator_get_voltage_sel(struct regulator_dev *rdev)
 	void *pcap = rdev_get_drvdata(rdev);
 	u32 tmp;
 
-	if (vreg->n_voltages == 1)
+	if (rdev->desc->n_voltages == 1)
 		return 0;
 
 	ezx_pcap_read(pcap, vreg->reg, &tmp);
-	tmp = ((tmp >> vreg->index) & (vreg->n_voltages - 1));
+	tmp = ((tmp >> vreg->index) & (rdev->desc->n_voltages - 1));
 	return tmp;
 }
 
@@ -214,16 +210,8 @@ static int pcap_regulator_is_enabled(struct regulator_dev *rdev)
 	return (tmp >> vreg->en) & 1;
 }
 
-static int pcap_regulator_list_voltage(struct regulator_dev *rdev,
-							unsigned int index)
-{
-	struct pcap_regulator *vreg = &vreg_table[rdev_get_id(rdev)];
-
-	return vreg->voltage_table[index] * 1000;
-}
-
 static struct regulator_ops pcap_regulator_ops = {
-	.list_voltage	= pcap_regulator_list_voltage,
+	.list_voltage	= regulator_list_voltage_table,
 	.set_voltage_sel = pcap_regulator_set_voltage_sel,
 	.get_voltage_sel = pcap_regulator_get_voltage_sel,
 	.enable		= pcap_regulator_enable,
@@ -236,6 +224,7 @@ static struct regulator_ops pcap_regulator_ops = {
 		.name		= #_vreg,			\
 		.id		= _vreg,			\
 		.n_voltages	= ARRAY_SIZE(_vreg##_table),	\
+		.volt_table	= _vreg##_table,		\
 		.ops		= &pcap_regulator_ops,		\
 		.type		= REGULATOR_VOLTAGE,		\
 		.owner		= THIS_MODULE,			\
diff --git a/drivers/regulator/pcf50633-regulator.c b/drivers/regulator/pcf50633-regulator.c
index 3c9d14c0017..092e5cb848a 100644
--- a/drivers/regulator/pcf50633-regulator.c
+++ b/drivers/regulator/pcf50633-regulator.c
@@ -100,13 +100,12 @@ static unsigned int ldo_voltage_value(u8 bits)
 	return 900 + (bits * 100);
 }
 
-static int pcf50633_regulator_set_voltage(struct regulator_dev *rdev,
-					  int min_uV, int max_uV,
-					  unsigned *selector)
+static int pcf50633_regulator_map_voltage(struct regulator_dev *rdev,
+					  int min_uV, int max_uV)
 {
 	struct pcf50633 *pcf;
 	int regulator_id, millivolts;
-	u8 volt_bits, regnr;
+	u8 volt_bits;
 
 	pcf = rdev_get_drvdata(rdev);
 
@@ -116,15 +115,11 @@ static int pcf50633_regulator_set_voltage(struct regulator_dev *rdev,
 
 	millivolts = min_uV / 1000;
 
-	regnr = rdev->desc->vsel_reg;
-
 	switch (regulator_id) {
 	case PCF50633_REGULATOR_AUTO:
 		volt_bits = auto_voltage_bits(millivolts);
 		break;
 	case PCF50633_REGULATOR_DOWN1:
-		volt_bits = down_voltage_bits(millivolts);
-		break;
 	case PCF50633_REGULATOR_DOWN2:
 		volt_bits = down_voltage_bits(millivolts);
 		break;
@@ -142,9 +137,7 @@ static int pcf50633_regulator_set_voltage(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
-	*selector = volt_bits;
-
-	return pcf50633_reg_write(pcf, regnr, volt_bits);
+	return volt_bits;
 }
 
 static int pcf50633_regulator_list_voltage(struct regulator_dev *rdev,
@@ -159,8 +152,6 @@ static int pcf50633_regulator_list_voltage(struct regulator_dev *rdev,
 		millivolts = auto_voltage_value(index);
 		break;
 	case PCF50633_REGULATOR_DOWN1:
-		millivolts = down_voltage_value(index);
-		break;
 	case PCF50633_REGULATOR_DOWN2:
 		millivolts = down_voltage_value(index);
 		break;
@@ -182,9 +173,10 @@ static int pcf50633_regulator_list_voltage(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops pcf50633_regulator_ops = {
-	.set_voltage = pcf50633_regulator_set_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage = pcf50633_regulator_list_voltage,
+	.map_voltage = pcf50633_regulator_map_voltage,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
 	.is_enabled = regulator_is_enabled_regmap,
diff --git a/drivers/regulator/rc5t583-regulator.c b/drivers/regulator/rc5t583-regulator.c
index 1d34e64a130..8bf4e8c9de9 100644
--- a/drivers/regulator/rc5t583-regulator.c
+++ b/drivers/regulator/rc5t583-regulator.c
@@ -42,7 +42,6 @@ struct rc5t583_regulator_info {
 
 	/* Regulator specific turn-on delay  and voltage settling time*/
 	int			enable_uv_per_us;
-	int			change_uv_per_us;
 
 	/* Used by regulator core */
 	struct regulator_desc	desc;
@@ -66,25 +65,6 @@ static int rc5t583_regulator_enable_time(struct regulator_dev *rdev)
 	return DIV_ROUND_UP(curr_uV, reg->reg_info->enable_uv_per_us);
 }
 
-static int rc5t583_set_voltage_time_sel(struct regulator_dev *rdev,
-		unsigned int old_selector, unsigned int new_selector)
-{
-	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
-	int old_uV, new_uV;
-	old_uV = regulator_list_voltage_linear(rdev, old_selector);
-
-	if (old_uV < 0)
-		return old_uV;
-
-	new_uV = regulator_list_voltage_linear(rdev, new_selector);
-	if (new_uV < 0)
-		return new_uV;
-
-	return DIV_ROUND_UP(abs(old_uV - new_uV),
-				reg->reg_info->change_uv_per_us);
-}
-
-
 static struct regulator_ops rc5t583_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
@@ -94,7 +74,7 @@ static struct regulator_ops rc5t583_ops = {
 	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
 	.list_voltage		= regulator_list_voltage_linear,
 	.map_voltage		= regulator_map_voltage_linear,
-	.set_voltage_time_sel	= rc5t583_set_voltage_time_sel,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 };
 
 #define RC5T583_REG(_id, _en_reg, _en_bit, _disc_reg, _disc_bit, \
@@ -104,7 +84,6 @@ static struct regulator_ops rc5t583_ops = {
 	.disc_bit	= _disc_bit,				\
 	.deepsleep_reg	= RC5T583_REG_##_id##DAC_DS,		\
 	.enable_uv_per_us = _enable_mv * 1000,			\
-	.change_uv_per_us = 40 * 1000,				\
 	.deepsleep_id	= RC5T583_DS_##_id,			\
 	.desc = {						\
 		.name = "rc5t583-regulator-"#_id,		\
@@ -119,6 +98,7 @@ static struct regulator_ops rc5t583_ops = {
 		.enable_mask = BIT(_en_bit),			\
 		.min_uV	= _min_mv * 1000,			\
 		.uV_step = _step_uV,				\
+		.ramp_delay = 40 * 1000,			\
 	},							\
 }
 
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
new file mode 100644
index 00000000000..4669dc9ac74
--- /dev/null
+++ b/drivers/regulator/s2mps11.c
@@ -0,0 +1,363 @@
+/*
+ * s2mps11.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd
+ *              http://www.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/bug.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/samsung/core.h>
+#include <linux/mfd/samsung/s2mps11.h>
+
+struct s2mps11_info {
+	struct regulator_dev **rdev;
+
+	int ramp_delay2;
+	int ramp_delay34;
+	int ramp_delay5;
+	int ramp_delay16;
+	int ramp_delay7810;
+	int ramp_delay9;
+
+	bool buck6_ramp;
+	bool buck2_ramp;
+	bool buck3_ramp;
+	bool buck4_ramp;
+};
+
+static int get_ramp_delay(int ramp_delay)
+{
+	unsigned char cnt = 0;
+
+	ramp_delay /= 6;
+
+	while (true) {
+		ramp_delay = ramp_delay >> 1;
+		if (ramp_delay == 0)
+			break;
+		cnt++;
+	}
+	return cnt;
+}
+
+static struct regulator_ops s2mps11_ldo_ops = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+};
+
+static struct regulator_ops s2mps11_buck_ops = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+};
+
+#define regulator_desc_ldo1(num)	{		\
+	.name		= "LDO"#num,			\
+	.id		= S2MPS11_LDO##num,		\
+	.ops		= &s2mps11_ldo_ops,		\
+	.type		= REGULATOR_VOLTAGE,		\
+	.owner		= THIS_MODULE,			\
+	.min_uV		= S2MPS11_LDO_MIN,		\
+	.uV_step	= S2MPS11_LDO_STEP1,		\
+	.n_voltages	= S2MPS11_LDO_N_VOLTAGES,	\
+	.vsel_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
+	.vsel_mask	= S2MPS11_LDO_VSEL_MASK,	\
+	.enable_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
+	.enable_mask	= S2MPS11_ENABLE_MASK		\
+}
+#define regulator_desc_ldo2(num)	{		\
+	.name		= "LDO"#num,			\
+	.id		= S2MPS11_LDO##num,		\
+	.ops		= &s2mps11_ldo_ops,		\
+	.type		= REGULATOR_VOLTAGE,		\
+	.owner		= THIS_MODULE,			\
+	.min_uV		= S2MPS11_LDO_MIN,		\
+	.uV_step	= S2MPS11_LDO_STEP2,		\
+	.n_voltages	= S2MPS11_LDO_N_VOLTAGES,	\
+	.vsel_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
+	.vsel_mask	= S2MPS11_LDO_VSEL_MASK,	\
+	.enable_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
+	.enable_mask	= S2MPS11_ENABLE_MASK		\
+}
+
+#define regulator_desc_buck1_4(num)	{			\
+	.name		= "BUCK"#num,				\
+	.id		= S2MPS11_BUCK##num,			\
+	.ops		= &s2mps11_buck_ops,			\
+	.type		= REGULATOR_VOLTAGE,			\
+	.owner		= THIS_MODULE,				\
+	.min_uV		= S2MPS11_BUCK_MIN1,			\
+	.uV_step	= S2MPS11_BUCK_STEP1,			\
+	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
+	.vsel_reg	= S2MPS11_REG_B1CTRL2 + (num - 1) * 2,	\
+	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
+	.enable_reg	= S2MPS11_REG_B1CTRL1 + (num - 1) * 2,	\
+	.enable_mask	= S2MPS11_ENABLE_MASK			\
+}
+
+#define regulator_desc_buck5	{				\
+	.name		= "BUCK5",				\
+	.id		= S2MPS11_BUCK5,			\
+	.ops		= &s2mps11_buck_ops,			\
+	.type		= REGULATOR_VOLTAGE,			\
+	.owner		= THIS_MODULE,				\
+	.min_uV		= S2MPS11_BUCK_MIN1,			\
+	.uV_step	= S2MPS11_BUCK_STEP1,			\
+	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
+	.vsel_reg	= S2MPS11_REG_B5CTRL2,			\
+	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
+	.enable_reg	= S2MPS11_REG_B5CTRL1,			\
+	.enable_mask	= S2MPS11_ENABLE_MASK			\
+}
+
+#define regulator_desc_buck6_8(num)	{			\
+	.name		= "BUCK"#num,				\
+	.id		= S2MPS11_BUCK##num,			\
+	.ops		= &s2mps11_buck_ops,			\
+	.type		= REGULATOR_VOLTAGE,			\
+	.owner		= THIS_MODULE,				\
+	.min_uV		= S2MPS11_BUCK_MIN1,			\
+	.uV_step	= S2MPS11_BUCK_STEP1,			\
+	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
+	.vsel_reg	= S2MPS11_REG_B6CTRL2 + (num - 6) * 2,	\
+	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
+	.enable_reg	= S2MPS11_REG_B6CTRL1 + (num - 6) * 2,	\
+	.enable_mask	= S2MPS11_ENABLE_MASK			\
+}
+
+#define regulator_desc_buck9	{				\
+	.name		= "BUCK9",				\
+	.id		= S2MPS11_BUCK9,			\
+	.ops		= &s2mps11_buck_ops,			\
+	.type		= REGULATOR_VOLTAGE,			\
+	.owner		= THIS_MODULE,				\
+	.min_uV		= S2MPS11_BUCK_MIN3,			\
+	.uV_step	= S2MPS11_BUCK_STEP3,			\
+	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
+	.vsel_reg	= S2MPS11_REG_B9CTRL2,			\
+	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
+	.enable_reg	= S2MPS11_REG_B9CTRL1,			\
+	.enable_mask	= S2MPS11_ENABLE_MASK			\
+}
+
+#define regulator_desc_buck10	{				\
+	.name		= "BUCK10",				\
+	.id		= S2MPS11_BUCK10,			\
+	.ops		= &s2mps11_buck_ops,			\
+	.type		= REGULATOR_VOLTAGE,			\
+	.owner		= THIS_MODULE,				\
+	.min_uV		= S2MPS11_BUCK_MIN2,			\
+	.uV_step	= S2MPS11_BUCK_STEP2,			\
+	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
+	.vsel_reg	= S2MPS11_REG_B9CTRL2,			\
+	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
+	.enable_reg	= S2MPS11_REG_B9CTRL1,			\
+	.enable_mask	= S2MPS11_ENABLE_MASK			\
+}
+
+static struct regulator_desc regulators[] = {
+	regulator_desc_ldo2(1),
+	regulator_desc_ldo1(2),
+	regulator_desc_ldo1(3),
+	regulator_desc_ldo1(4),
+	regulator_desc_ldo1(5),
+	regulator_desc_ldo2(6),
+	regulator_desc_ldo1(7),
+	regulator_desc_ldo1(8),
+	regulator_desc_ldo1(9),
+	regulator_desc_ldo1(10),
+	regulator_desc_ldo2(11),
+	regulator_desc_ldo1(12),
+	regulator_desc_ldo1(13),
+	regulator_desc_ldo1(14),
+	regulator_desc_ldo1(15),
+	regulator_desc_ldo1(16),
+	regulator_desc_ldo1(17),
+	regulator_desc_ldo1(18),
+	regulator_desc_ldo1(19),
+	regulator_desc_ldo1(20),
+	regulator_desc_ldo1(21),
+	regulator_desc_ldo2(22),
+	regulator_desc_ldo2(23),
+	regulator_desc_ldo1(24),
+	regulator_desc_ldo1(25),
+	regulator_desc_ldo1(26),
+	regulator_desc_ldo2(27),
+	regulator_desc_ldo1(28),
+	regulator_desc_ldo1(29),
+	regulator_desc_ldo1(30),
+	regulator_desc_ldo1(31),
+	regulator_desc_ldo1(32),
+	regulator_desc_ldo1(33),
+	regulator_desc_ldo1(34),
+	regulator_desc_ldo1(35),
+	regulator_desc_ldo1(36),
+	regulator_desc_ldo1(37),
+	regulator_desc_ldo1(38),
+	regulator_desc_buck1_4(1),
+	regulator_desc_buck1_4(2),
+	regulator_desc_buck1_4(3),
+	regulator_desc_buck1_4(4),
+	regulator_desc_buck5,
+	regulator_desc_buck6_8(6),
+	regulator_desc_buck6_8(7),
+	regulator_desc_buck6_8(8),
+	regulator_desc_buck9,
+	regulator_desc_buck10,
+};
+
+static __devinit int s2mps11_pmic_probe(struct platform_device *pdev)
+{
+	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct sec_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct regulator_config config = { };
+	struct regulator_dev **rdev;
+	struct s2mps11_info *s2mps11;
+	int i, ret, size;
+	unsigned char ramp_enable, ramp_reg = 0;
+
+	if (!pdata) {
+		dev_err(pdev->dev.parent, "Platform data not supplied\n");
+		return -ENODEV;
+	}
+
+	s2mps11 = devm_kzalloc(&pdev->dev, sizeof(struct s2mps11_info),
+				GFP_KERNEL);
+	if (!s2mps11)
+		return -ENOMEM;
+
+	size = sizeof(struct regulator_dev *) * S2MPS11_REGULATOR_MAX;
+	s2mps11->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	if (!s2mps11->rdev) {
+		return -ENOMEM;
+	}
+
+	rdev = s2mps11->rdev;
+	platform_set_drvdata(pdev, s2mps11);
+
+	s2mps11->ramp_delay2 = pdata->buck2_ramp_delay;
+	s2mps11->ramp_delay34 = pdata->buck34_ramp_delay;
+	s2mps11->ramp_delay5 = pdata->buck5_ramp_delay;
+	s2mps11->ramp_delay16 = pdata->buck16_ramp_delay;
+	s2mps11->ramp_delay7810 = pdata->buck7810_ramp_delay;
+	s2mps11->ramp_delay9 = pdata->buck9_ramp_delay;
+
+	s2mps11->buck6_ramp = pdata->buck6_ramp_enable;
+	s2mps11->buck2_ramp = pdata->buck2_ramp_enable;
+	s2mps11->buck3_ramp = pdata->buck3_ramp_enable;
+	s2mps11->buck4_ramp = pdata->buck4_ramp_enable;
+
+	ramp_enable = (s2mps11->buck2_ramp << 3) | (s2mps11->buck3_ramp << 2) |
+		(s2mps11->buck4_ramp << 1) | s2mps11->buck6_ramp ;
+
+	if (ramp_enable) {
+		if (s2mps11->buck2_ramp)
+			ramp_reg |= get_ramp_delay(s2mps11->ramp_delay2) >> 6;
+		if (s2mps11->buck3_ramp || s2mps11->buck4_ramp)
+			ramp_reg |= get_ramp_delay(s2mps11->ramp_delay34) >> 4;
+		sec_reg_write(iodev, S2MPS11_REG_RAMP, ramp_reg | ramp_enable);
+	}
+
+	ramp_reg &= 0x00;
+	ramp_reg |= get_ramp_delay(s2mps11->ramp_delay5) >> 6;
+	ramp_reg |= get_ramp_delay(s2mps11->ramp_delay16) >> 4;
+	ramp_reg |= get_ramp_delay(s2mps11->ramp_delay7810) >> 2;
+	ramp_reg |= get_ramp_delay(s2mps11->ramp_delay9);
+	sec_reg_write(iodev, S2MPS11_REG_RAMP_BUCK, ramp_reg);
+
+	for (i = 0; i < S2MPS11_REGULATOR_MAX; i++) {
+
+		config.dev = &pdev->dev;
+		config.regmap = iodev->regmap;
+		config.init_data = pdata->regulators[i].initdata;
+		config.driver_data = s2mps11;
+
+		rdev[i] = regulator_register(&regulators[i], &config);
+		if (IS_ERR(rdev[i])) {
+			ret = PTR_ERR(rdev[i]);
+			dev_err(&pdev->dev, "regulator init failed for %d\n",
+				i);
+			rdev[i] = NULL;
+			goto err;
+		}
+	}
+
+	return 0;
+err:
+	for (i = 0; i < S2MPS11_REGULATOR_MAX; i++)
+		if (rdev[i])
+			regulator_unregister(rdev[i]);
+
+	return ret;
+}
+
+static int __devexit s2mps11_pmic_remove(struct platform_device *pdev)
+{
+	struct s2mps11_info *s2mps11 = platform_get_drvdata(pdev);
+	struct regulator_dev **rdev = s2mps11->rdev;
+	int i;
+
+	for (i = 0; i < S2MPS11_REGULATOR_MAX; i++)
+		if (rdev[i])
+			regulator_unregister(rdev[i]);
+
+	return 0;
+}
+
+static const struct platform_device_id s2mps11_pmic_id[] = {
+	{ "s2mps11-pmic", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, s2mps11_pmic_id);
+
+static struct platform_driver s2mps11_pmic_driver = {
+	.driver = {
+		.name = "s2mps11-pmic",
+		.owner = THIS_MODULE,
+	},
+	.probe = s2mps11_pmic_probe,
+	.remove = __devexit_p(s2mps11_pmic_remove),
+	.id_table = s2mps11_pmic_id,
+};
+
+static int __init s2mps11_pmic_init(void)
+{
+	return platform_driver_register(&s2mps11_pmic_driver);
+}
+subsys_initcall(s2mps11_pmic_init);
+
+static void __exit s2mps11_pmic_exit(void)
+{
+	platform_driver_unregister(&s2mps11_pmic_driver);
+}
+module_exit(s2mps11_pmic_exit);
+
+/* Module information */
+MODULE_AUTHOR("Sangbeom Kim <sbkim73@samsung.com>");
+MODULE_DESCRIPTION("SAMSUNG S2MPS11 Regulator Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/s5m8767.c b/drivers/regulator/s5m8767.c
index 9caadb48217..abe64a32aed 100644
--- a/drivers/regulator/s5m8767.c
+++ b/drivers/regulator/s5m8767.c
@@ -19,15 +19,15 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
-#include <linux/mfd/s5m87xx/s5m-core.h>
-#include <linux/mfd/s5m87xx/s5m-pmic.h>
+#include <linux/mfd/samsung/core.h>
+#include <linux/mfd/samsung/s5m8767.h>
 
 struct s5m8767_info {
 	struct device *dev;
-	struct s5m87xx_dev *iodev;
+	struct sec_pmic_dev *iodev;
 	int num_regulators;
 	struct regulator_dev **rdev;
-	struct s5m_opmode_data *opmode;
+	struct sec_opmode_data *opmode;
 
 	int ramp_delay;
 	bool buck2_ramp;
@@ -41,46 +41,47 @@ struct s5m8767_info {
 	u8 buck3_vol[8];
 	u8 buck4_vol[8];
 	int buck_gpios[3];
+	int buck_ds[3];
 	int buck_gpioindex;
 };
 
-struct s5m_voltage_desc {
+struct sec_voltage_desc {
 	int max;
 	int min;
 	int step;
 };
 
-static const struct s5m_voltage_desc buck_voltage_val1 = {
+static const struct sec_voltage_desc buck_voltage_val1 = {
 	.max = 2225000,
 	.min =  650000,
 	.step =   6250,
 };
 
-static const struct s5m_voltage_desc buck_voltage_val2 = {
+static const struct sec_voltage_desc buck_voltage_val2 = {
 	.max = 1600000,
 	.min =  600000,
 	.step =   6250,
 };
 
-static const struct s5m_voltage_desc buck_voltage_val3 = {
+static const struct sec_voltage_desc buck_voltage_val3 = {
 	.max = 3000000,
 	.min =  750000,
 	.step =  12500,
 };
 
-static const struct s5m_voltage_desc ldo_voltage_val1 = {
+static const struct sec_voltage_desc ldo_voltage_val1 = {
 	.max = 3950000,
 	.min =  800000,
 	.step =  50000,
 };
 
-static const struct s5m_voltage_desc ldo_voltage_val2 = {
+static const struct sec_voltage_desc ldo_voltage_val2 = {
 	.max = 2375000,
 	.min =  800000,
 	.step =  25000,
 };
 
-static const struct s5m_voltage_desc *reg_voltage_map[] = {
+static const struct sec_voltage_desc *reg_voltage_map[] = {
 	[S5M8767_LDO1] = &ldo_voltage_val2,
 	[S5M8767_LDO2] = &ldo_voltage_val2,
 	[S5M8767_LDO3] = &ldo_voltage_val1,
@@ -120,27 +121,6 @@ static const struct s5m_voltage_desc *reg_voltage_map[] = {
 	[S5M8767_BUCK9] = &buck_voltage_val3,
 };
 
-static int s5m8767_list_voltage(struct regulator_dev *rdev,
-				unsigned int selector)
-{
-	const struct s5m_voltage_desc *desc;
-	int reg_id = rdev_get_id(rdev);
-	int val;
-
-	if (reg_id >= ARRAY_SIZE(reg_voltage_map) || reg_id < 0)
-		return -EINVAL;
-
-	desc = reg_voltage_map[reg_id];
-	if (desc == NULL)
-		return -EINVAL;
-
-	val = desc->min + desc->step * selector;
-	if (val > desc->max)
-		return -EINVAL;
-
-	return val;
-}
-
 static unsigned int s5m8767_opmode_reg[][4] = {
 	/* {OFF, ON, LOWPOWER, SUSPEND} */
 	/* LDO1 ... LDO28 */
@@ -233,7 +213,7 @@ static int s5m8767_reg_is_enabled(struct regulator_dev *rdev)
 	else if (ret)
 		return ret;
 
-	ret = s5m_reg_read(s5m8767->iodev, reg, &val);
+	ret = sec_reg_read(s5m8767->iodev, reg, &val);
 	if (ret)
 		return ret;
 
@@ -250,7 +230,7 @@ static int s5m8767_reg_enable(struct regulator_dev *rdev)
 	if (ret)
 		return ret;
 
-	return s5m_reg_update(s5m8767->iodev, reg, enable_ctrl, mask);
+	return sec_reg_update(s5m8767->iodev, reg, enable_ctrl, mask);
 }
 
 static int s5m8767_reg_disable(struct regulator_dev *rdev)
@@ -263,7 +243,7 @@ static int s5m8767_reg_disable(struct regulator_dev *rdev)
 	if (ret)
 		return ret;
 
-	return s5m_reg_update(s5m8767->iodev, reg, ~mask, mask);
+	return sec_reg_update(s5m8767->iodev, reg, ~mask, mask);
 }
 
 static int s5m8767_get_voltage_register(struct regulator_dev *rdev, int *_reg)
@@ -283,17 +263,17 @@ static int s5m8767_get_voltage_register(struct regulator_dev *rdev, int *_reg)
 		reg = S5M8767_REG_BUCK1CTRL2;
 		break;
 	case S5M8767_BUCK2:
-		reg = S5M8767_REG_BUCK2DVS1;
+		reg = S5M8767_REG_BUCK2DVS2;
 		if (s5m8767->buck2_gpiodvs)
 			reg += s5m8767->buck_gpioindex;
 		break;
 	case S5M8767_BUCK3:
-		reg = S5M8767_REG_BUCK3DVS1;
+		reg = S5M8767_REG_BUCK3DVS2;
 		if (s5m8767->buck3_gpiodvs)
 			reg += s5m8767->buck_gpioindex;
 		break;
 	case S5M8767_BUCK4:
-		reg = S5M8767_REG_BUCK4DVS1;
+		reg = S5M8767_REG_BUCK4DVS2;
 		if (s5m8767->buck4_gpiodvs)
 			reg += s5m8767->buck_gpioindex;
 		break;
@@ -325,7 +305,7 @@ static int s5m8767_get_voltage_sel(struct regulator_dev *rdev)
 
 	mask = (reg_id < S5M8767_BUCK1) ? 0x3f : 0xff;
 
-	ret = s5m_reg_read(s5m8767->iodev, reg, &val);
+	ret = sec_reg_read(s5m8767->iodev, reg, &val);
 	if (ret)
 		return ret;
 
@@ -335,7 +315,7 @@ static int s5m8767_get_voltage_sel(struct regulator_dev *rdev)
 }
 
 static int s5m8767_convert_voltage_to_sel(
-		const struct s5m_voltage_desc *desc,
+		const struct sec_voltage_desc *desc,
 		int min_vol, int max_vol)
 {
 	int selector = 0;
@@ -357,32 +337,34 @@ static int s5m8767_convert_voltage_to_sel(
 	return selector;
 }
 
-static inline void s5m8767_set_high(struct s5m8767_info *s5m8767)
+static inline int s5m8767_set_high(struct s5m8767_info *s5m8767)
 {
 	int temp_index = s5m8767->buck_gpioindex;
 
 	gpio_set_value(s5m8767->buck_gpios[0], (temp_index >> 2) & 0x1);
 	gpio_set_value(s5m8767->buck_gpios[1], (temp_index >> 1) & 0x1);
 	gpio_set_value(s5m8767->buck_gpios[2], temp_index & 0x1);
+
+	return 0;
 }
 
-static inline void s5m8767_set_low(struct s5m8767_info *s5m8767)
+static inline int s5m8767_set_low(struct s5m8767_info *s5m8767)
 {
 	int temp_index = s5m8767->buck_gpioindex;
 
 	gpio_set_value(s5m8767->buck_gpios[2], temp_index & 0x1);
 	gpio_set_value(s5m8767->buck_gpios[1], (temp_index >> 1) & 0x1);
 	gpio_set_value(s5m8767->buck_gpios[0], (temp_index >> 2) & 0x1);
+
+	return 0;
 }
 
-static int s5m8767_set_voltage(struct regulator_dev *rdev,
-				int min_uV, int max_uV, unsigned *selector)
+static int s5m8767_set_voltage_sel(struct regulator_dev *rdev,
+				   unsigned selector)
 {
 	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
-	const struct s5m_voltage_desc *desc;
 	int reg_id = rdev_get_id(rdev);
-	int sel, reg, mask, ret = 0, old_index, index = 0;
-	u8 val;
+	int reg, mask, ret = 0, old_index, index = 0;
 	u8 *buck234_vol = NULL;
 
 	switch (reg_id) {
@@ -407,15 +389,9 @@ static int s5m8767_set_voltage(struct regulator_dev *rdev,
 		return -EINVAL;
 	}
 
-	desc = reg_voltage_map[reg_id];
-
-	sel = s5m8767_convert_voltage_to_sel(desc, min_uV, max_uV);
-	if (sel < 0)
-		return sel;
-
 	/* buck234_vol != NULL means to control buck234 voltage via DVS GPIO */
 	if (buck234_vol) {
-		while (*buck234_vol != sel) {
+		while (*buck234_vol != selector) {
 			buck234_vol++;
 			index++;
 		}
@@ -423,22 +399,16 @@ static int s5m8767_set_voltage(struct regulator_dev *rdev,
 		s5m8767->buck_gpioindex = index;
 
 		if (index > old_index)
-			s5m8767_set_high(s5m8767);
+			return s5m8767_set_high(s5m8767);
 		else
-			s5m8767_set_low(s5m8767);
+			return s5m8767_set_low(s5m8767);
 	} else {
 		ret = s5m8767_get_voltage_register(rdev, &reg);
 		if (ret)
 			return ret;
 
-		s5m_reg_read(s5m8767->iodev, reg, &val);
-		val = (val & ~mask) | sel;
-
-		ret = s5m_reg_write(s5m8767->iodev, reg, val);
+		return sec_reg_update(s5m8767->iodev, reg, selector, mask);
 	}
-
-	*selector = sel;
-	return ret;
 }
 
 static int s5m8767_set_voltage_time_sel(struct regulator_dev *rdev,
@@ -446,7 +416,7 @@ static int s5m8767_set_voltage_time_sel(struct regulator_dev *rdev,
 					     unsigned int new_sel)
 {
 	struct s5m8767_info *s5m8767 = rdev_get_drvdata(rdev);
-	const struct s5m_voltage_desc *desc;
+	const struct sec_voltage_desc *desc;
 	int reg_id = rdev_get_id(rdev);
 
 	desc = reg_voltage_map[reg_id];
@@ -458,15 +428,21 @@ static int s5m8767_set_voltage_time_sel(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops s5m8767_ops = {
-	.list_voltage		= s5m8767_list_voltage,
+	.list_voltage		= regulator_list_voltage_linear,
 	.is_enabled		= s5m8767_reg_is_enabled,
 	.enable			= s5m8767_reg_enable,
 	.disable		= s5m8767_reg_disable,
 	.get_voltage_sel	= s5m8767_get_voltage_sel,
-	.set_voltage		= s5m8767_set_voltage,
+	.set_voltage_sel	= s5m8767_set_voltage_sel,
 	.set_voltage_time_sel	= s5m8767_set_voltage_time_sel,
 };
 
+static struct regulator_ops s5m8767_buck78_ops = {
+	.is_enabled		= s5m8767_reg_is_enabled,
+	.enable			= s5m8767_reg_enable,
+	.disable		= s5m8767_reg_disable,
+};
+
 #define s5m8767_regulator_desc(_name) {		\
 	.name		= #_name,		\
 	.id		= S5M8767_##_name,	\
@@ -475,6 +451,14 @@ static struct regulator_ops s5m8767_ops = {
 	.owner		= THIS_MODULE,		\
 }
 
+#define s5m8767_regulator_buck78_desc(_name) {	\
+	.name		= #_name,		\
+	.id		= S5M8767_##_name,	\
+	.ops		= &s5m8767_buck78_ops,	\
+	.type		= REGULATOR_VOLTAGE,	\
+	.owner		= THIS_MODULE,		\
+}
+
 static struct regulator_desc regulators[] = {
 	s5m8767_regulator_desc(LDO1),
 	s5m8767_regulator_desc(LDO2),
@@ -510,19 +494,19 @@ static struct regulator_desc regulators[] = {
 	s5m8767_regulator_desc(BUCK4),
 	s5m8767_regulator_desc(BUCK5),
 	s5m8767_regulator_desc(BUCK6),
-	s5m8767_regulator_desc(BUCK7),
-	s5m8767_regulator_desc(BUCK8),
+	s5m8767_regulator_buck78_desc(BUCK7),
+	s5m8767_regulator_buck78_desc(BUCK8),
 	s5m8767_regulator_desc(BUCK9),
 };
 
 static __devinit int s5m8767_pmic_probe(struct platform_device *pdev)
 {
-	struct s5m87xx_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct s5m_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct sec_platform_data *pdata = dev_get_platdata(iodev->dev);
 	struct regulator_config config = { };
 	struct regulator_dev **rdev;
 	struct s5m8767_info *s5m8767;
-	int i, ret, size;
+	int i, ret, size, buck_init;
 
 	if (!pdata) {
 		dev_err(pdev->dev.parent, "Platform data not supplied\n");
@@ -573,12 +557,37 @@ static __devinit int s5m8767_pmic_probe(struct platform_device *pdev)
 	s5m8767->buck_gpios[0] = pdata->buck_gpios[0];
 	s5m8767->buck_gpios[1] = pdata->buck_gpios[1];
 	s5m8767->buck_gpios[2] = pdata->buck_gpios[2];
+	s5m8767->buck_ds[0] = pdata->buck_ds[0];
+	s5m8767->buck_ds[1] = pdata->buck_ds[1];
+	s5m8767->buck_ds[2] = pdata->buck_ds[2];
+
 	s5m8767->ramp_delay = pdata->buck_ramp_delay;
 	s5m8767->buck2_ramp = pdata->buck2_ramp_enable;
 	s5m8767->buck3_ramp = pdata->buck3_ramp_enable;
 	s5m8767->buck4_ramp = pdata->buck4_ramp_enable;
 	s5m8767->opmode = pdata->opmode;
 
+	buck_init = s5m8767_convert_voltage_to_sel(&buck_voltage_val2,
+						pdata->buck2_init,
+						pdata->buck2_init +
+						buck_voltage_val2.step);
+
+	sec_reg_write(s5m8767->iodev, S5M8767_REG_BUCK2DVS2, buck_init);
+
+	buck_init = s5m8767_convert_voltage_to_sel(&buck_voltage_val2,
+						pdata->buck3_init,
+						pdata->buck3_init +
+						buck_voltage_val2.step);
+
+	sec_reg_write(s5m8767->iodev, S5M8767_REG_BUCK3DVS2, buck_init);
+
+	buck_init = s5m8767_convert_voltage_to_sel(&buck_voltage_val2,
+						pdata->buck4_init,
+						pdata->buck4_init +
+						buck_voltage_val2.step);
+
+	sec_reg_write(s5m8767->iodev, S5M8767_REG_BUCK4DVS2, buck_init);
+
 	for (i = 0; i < 8; i++) {
 		if (s5m8767->buck2_gpiodvs) {
 			s5m8767->buck2_vol[i] =
@@ -608,112 +617,138 @@ static __devinit int s5m8767_pmic_probe(struct platform_device *pdev)
 		}
 	}
 
-	if (pdata->buck2_gpiodvs || pdata->buck3_gpiodvs ||
-		pdata->buck4_gpiodvs) {
-		if (gpio_is_valid(pdata->buck_gpios[0]) &&
-			gpio_is_valid(pdata->buck_gpios[1]) &&
-			gpio_is_valid(pdata->buck_gpios[2])) {
-			ret = gpio_request(pdata->buck_gpios[0],
-						"S5M8767 SET1");
-			if (ret == -EBUSY)
-				dev_warn(&pdev->dev, "Duplicated gpio request for SET1\n");
-
-			ret = gpio_request(pdata->buck_gpios[1],
-					   "S5M8767 SET2");
-			if (ret == -EBUSY)
-				dev_warn(&pdev->dev, "Duplicated gpio request for SET2\n");
-
-			ret = gpio_request(pdata->buck_gpios[2],
-					   "S5M8767 SET3");
-			if (ret == -EBUSY)
-				dev_warn(&pdev->dev, "Duplicated gpio request for SET3\n");
-			/* SET1 GPIO */
-			gpio_direction_output(pdata->buck_gpios[0],
-					(s5m8767->buck_gpioindex >> 2) & 0x1);
-			/* SET2 GPIO */
-			gpio_direction_output(pdata->buck_gpios[1],
-					(s5m8767->buck_gpioindex >> 1) & 0x1);
-			/* SET3 GPIO */
-			gpio_direction_output(pdata->buck_gpios[2],
-					(s5m8767->buck_gpioindex >> 0) & 0x1);
-			ret = 0;
-		} else {
-			dev_err(&pdev->dev, "GPIO NOT VALID\n");
-			ret = -EINVAL;
+	if (gpio_is_valid(pdata->buck_gpios[0]) &&
+		gpio_is_valid(pdata->buck_gpios[1]) &&
+		gpio_is_valid(pdata->buck_gpios[2])) {
+		ret = devm_gpio_request(&pdev->dev, pdata->buck_gpios[0],
+					"S5M8767 SET1");
+		if (ret)
 			return ret;
-		}
+
+		ret = devm_gpio_request(&pdev->dev, pdata->buck_gpios[1],
+					"S5M8767 SET2");
+		if (ret)
+			return ret;
+
+		ret = devm_gpio_request(&pdev->dev, pdata->buck_gpios[2],
+					"S5M8767 SET3");
+		if (ret)
+			return ret;
+
+		/* SET1 GPIO */
+		gpio_direction_output(pdata->buck_gpios[0],
+				(s5m8767->buck_gpioindex >> 2) & 0x1);
+		/* SET2 GPIO */
+		gpio_direction_output(pdata->buck_gpios[1],
+				(s5m8767->buck_gpioindex >> 1) & 0x1);
+		/* SET3 GPIO */
+		gpio_direction_output(pdata->buck_gpios[2],
+				(s5m8767->buck_gpioindex >> 0) & 0x1);
+	} else {
+		dev_err(&pdev->dev, "GPIO NOT VALID\n");
+		ret = -EINVAL;
+		return ret;
 	}
 
-	s5m_reg_update(s5m8767->iodev, S5M8767_REG_BUCK2CTRL,
-			(pdata->buck2_gpiodvs) ? (1 << 1) : (0 << 1), 1 << 1);
-	s5m_reg_update(s5m8767->iodev, S5M8767_REG_BUCK3CTRL,
-			(pdata->buck3_gpiodvs) ? (1 << 1) : (0 << 1), 1 << 1);
-	s5m_reg_update(s5m8767->iodev, S5M8767_REG_BUCK4CTRL,
-			(pdata->buck4_gpiodvs) ? (1 << 1) : (0 << 1), 1 << 1);
+	ret = devm_gpio_request(&pdev->dev, pdata->buck_ds[0], "S5M8767 DS2");
+	if (ret)
+		return ret;
+
+	ret = devm_gpio_request(&pdev->dev, pdata->buck_ds[1], "S5M8767 DS3");
+	if (ret)
+		return ret;
+
+	ret = devm_gpio_request(&pdev->dev, pdata->buck_ds[2], "S5M8767 DS4");
+	if (ret)
+		return ret;
+
+	/* DS2 GPIO */
+	gpio_direction_output(pdata->buck_ds[0], 0x0);
+	/* DS3 GPIO */
+	gpio_direction_output(pdata->buck_ds[1], 0x0);
+	/* DS4 GPIO */
+	gpio_direction_output(pdata->buck_ds[2], 0x0);
+
+	if (pdata->buck2_gpiodvs || pdata->buck3_gpiodvs ||
+	   pdata->buck4_gpiodvs) {
+		sec_reg_update(s5m8767->iodev, S5M8767_REG_BUCK2CTRL,
+				(pdata->buck2_gpiodvs) ? (1 << 1) : (0 << 1),
+				1 << 1);
+		sec_reg_update(s5m8767->iodev, S5M8767_REG_BUCK3CTRL,
+				(pdata->buck3_gpiodvs) ? (1 << 1) : (0 << 1),
+				1 << 1);
+		sec_reg_update(s5m8767->iodev, S5M8767_REG_BUCK4CTRL,
+				(pdata->buck4_gpiodvs) ? (1 << 1) : (0 << 1),
+				1 << 1);
+	}
 
 	/* Initialize GPIO DVS registers */
 	for (i = 0; i < 8; i++) {
 		if (s5m8767->buck2_gpiodvs) {
-			s5m_reg_write(s5m8767->iodev, S5M8767_REG_BUCK2DVS1 + i,
+			sec_reg_write(s5m8767->iodev, S5M8767_REG_BUCK2DVS1 + i,
 					   s5m8767->buck2_vol[i]);
 		}
 
 		if (s5m8767->buck3_gpiodvs) {
-			s5m_reg_write(s5m8767->iodev, S5M8767_REG_BUCK3DVS1 + i,
+			sec_reg_write(s5m8767->iodev, S5M8767_REG_BUCK3DVS1 + i,
 					   s5m8767->buck3_vol[i]);
 		}
 
 		if (s5m8767->buck4_gpiodvs) {
-			s5m_reg_write(s5m8767->iodev, S5M8767_REG_BUCK4DVS1 + i,
+			sec_reg_write(s5m8767->iodev, S5M8767_REG_BUCK4DVS1 + i,
 					   s5m8767->buck4_vol[i]);
 		}
 	}
-	s5m_reg_update(s5m8767->iodev, S5M8767_REG_BUCK2CTRL, 0x78, 0xff);
-	s5m_reg_update(s5m8767->iodev, S5M8767_REG_BUCK3CTRL, 0x58, 0xff);
-	s5m_reg_update(s5m8767->iodev, S5M8767_REG_BUCK4CTRL, 0x78, 0xff);
 
 	if (s5m8767->buck2_ramp)
-		s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP, 0x08, 0x08);
+		sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP, 0x08, 0x08);
 
 	if (s5m8767->buck3_ramp)
-		s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP, 0x04, 0x04);
+		sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP, 0x04, 0x04);
 
 	if (s5m8767->buck4_ramp)
-		s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP, 0x02, 0x02);
+		sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP, 0x02, 0x02);
 
 	if (s5m8767->buck2_ramp || s5m8767->buck3_ramp
 		|| s5m8767->buck4_ramp) {
 		switch (s5m8767->ramp_delay) {
-		case 15:
-			s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
-					0xc0, 0xf0);
+		case 5:
+			sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
+					0x40, 0xf0);
+			break;
+		case 10:
+			sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
+					0x90, 0xf0);
 			break;
 		case 25:
-			s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
+			sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
 					0xd0, 0xf0);
 			break;
 		case 50:
-			s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
+			sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
 					0xe0, 0xf0);
 			break;
 		case 100:
-			s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
+			sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
 					0xf0, 0xf0);
 			break;
 		default:
-			s5m_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
+			sec_reg_update(s5m8767->iodev, S5M8767_REG_DVSRAMP,
 					0x90, 0xf0);
 		}
 	}
 
 	for (i = 0; i < pdata->num_regulators; i++) {
-		const struct s5m_voltage_desc *desc;
+		const struct sec_voltage_desc *desc;
 		int id = pdata->regulators[i].id;
 
 		desc = reg_voltage_map[id];
-		if (desc)
+		if (desc) {
 			regulators[id].n_voltages =
 				(desc->max - desc->min) / desc->step + 1;
+			regulators[id].min_uV = desc->min;
+			regulators[id].uV_step = desc->step;
+		}
 
 		config.dev = s5m8767->dev;
 		config.init_data = pdata->regulators[i].initdata;
diff --git a/drivers/regulator/tps6105x-regulator.c b/drivers/regulator/tps6105x-regulator.c
index d840d8440a9..1378409efae 100644
--- a/drivers/regulator/tps6105x-regulator.c
+++ b/drivers/regulator/tps6105x-regulator.c
@@ -20,7 +20,7 @@
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps6105x.h>
 
-static const int tps6105x_voltages[] = {
+static const unsigned int tps6105x_voltages[] = {
 	4500000,
 	5000000,
 	5250000,
@@ -105,22 +105,13 @@ static int tps6105x_regulator_set_voltage_sel(struct regulator_dev *rdev,
 	return 0;
 }
 
-static int tps6105x_regulator_list_voltage(struct regulator_dev *rdev,
-					   unsigned selector)
-{
-	if (selector >= ARRAY_SIZE(tps6105x_voltages))
-		return -EINVAL;
-
-	return tps6105x_voltages[selector];
-}
-
 static struct regulator_ops tps6105x_regulator_ops = {
 	.enable		= tps6105x_regulator_enable,
 	.disable	= tps6105x_regulator_disable,
 	.is_enabled	= tps6105x_regulator_is_enabled,
 	.get_voltage_sel = tps6105x_regulator_get_voltage_sel,
 	.set_voltage_sel = tps6105x_regulator_set_voltage_sel,
-	.list_voltage	= tps6105x_regulator_list_voltage,
+	.list_voltage	= regulator_list_voltage_table,
 };
 
 static const struct regulator_desc tps6105x_regulator_desc = {
@@ -130,6 +121,7 @@ static const struct regulator_desc tps6105x_regulator_desc = {
 	.id		= 0,
 	.owner		= THIS_MODULE,
 	.n_voltages	= ARRAY_SIZE(tps6105x_voltages),
+	.volt_table	= tps6105x_voltages,
 };
 
 /*
diff --git a/drivers/regulator/tps62360-regulator.c b/drivers/regulator/tps62360-regulator.c
index e534269ed44..68729a7c870 100644
--- a/drivers/regulator/tps62360-regulator.c
+++ b/drivers/regulator/tps62360-regulator.c
@@ -65,10 +65,8 @@ struct tps62360_chip {
 	struct regulator_desc desc;
 	struct regulator_dev *rdev;
 	struct regmap *regmap;
-	int chip_id;
 	int vsel0_gpio;
 	int vsel1_gpio;
-	int voltage_base;
 	u8 voltage_reg_mask;
 	bool en_internal_pulldn;
 	bool en_discharge;
@@ -76,7 +74,6 @@ struct tps62360_chip {
 	int lru_index[4];
 	int curr_vset_vsel[4];
 	int curr_vset_id;
-	int change_uv_per_us;
 };
 
 /*
@@ -175,23 +172,6 @@ static int tps62360_dcdc_set_voltage_sel(struct regulator_dev *dev,
 	return 0;
 }
 
-static int tps62360_set_voltage_time_sel(struct regulator_dev *rdev,
-		unsigned int old_selector, unsigned int new_selector)
-{
-	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
-	int old_uV, new_uV;
-
-	old_uV = regulator_list_voltage_linear(rdev, old_selector);
-	if (old_uV < 0)
-		return old_uV;
-
-	new_uV = regulator_list_voltage_linear(rdev, new_selector);
-	if (new_uV < 0)
-		return new_uV;
-
-	return DIV_ROUND_UP(abs(old_uV - new_uV), tps->change_uv_per_us);
-}
-
 static int tps62360_set_mode(struct regulator_dev *rdev, unsigned int mode)
 {
 	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
@@ -258,7 +238,7 @@ static struct regulator_ops tps62360_dcdc_ops = {
 	.set_voltage_sel	= tps62360_dcdc_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_linear,
 	.map_voltage		= regulator_map_voltage_linear,
-	.set_voltage_time_sel	= tps62360_set_voltage_time_sel,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.set_mode		= tps62360_set_mode,
 	.get_mode		= tps62360_get_mode,
 };
@@ -301,7 +281,7 @@ static int __devinit tps62360_init_dcdc(struct tps62360_chip *tps,
 	ramp_ctrl = (ramp_ctrl >> 4) & 0x7;
 
 	/* ramp mV/us = 32/(2^ramp_ctrl) */
-	tps->change_uv_per_us = DIV_ROUND_UP(32000, BIT(ramp_ctrl));
+	tps->desc.ramp_delay = DIV_ROUND_UP(32000, BIT(ramp_ctrl));
 	return ret;
 }
 
@@ -408,13 +388,13 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 	switch (chip_id) {
 	case TPS62360:
 	case TPS62362:
-		tps->voltage_base = TPS62360_BASE_VOLTAGE;
+		tps->desc.min_uV = TPS62360_BASE_VOLTAGE;
 		tps->voltage_reg_mask = 0x3F;
 		tps->desc.n_voltages = TPS62360_N_VOLTAGES;
 		break;
 	case TPS62361:
 	case TPS62363:
-		tps->voltage_base = TPS62361_BASE_VOLTAGE;
+		tps->desc.min_uV = TPS62361_BASE_VOLTAGE;
 		tps->voltage_reg_mask = 0x7F;
 		tps->desc.n_voltages = TPS62361_N_VOLTAGES;
 		break;
@@ -427,7 +407,6 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 	tps->desc.ops = &tps62360_dcdc_ops;
 	tps->desc.type = REGULATOR_VOLTAGE;
 	tps->desc.owner = THIS_MODULE;
-	tps->desc.min_uV = tps->voltage_base;
 	tps->desc.uV_step = 10000;
 
 	tps->regmap = devm_regmap_init_i2c(client, &tps62360_regmap_config);
@@ -449,24 +428,24 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 		int gpio_flags;
 		gpio_flags = (pdata->vsel0_def_state) ?
 				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
-		ret = gpio_request_one(tps->vsel0_gpio,
+		ret = devm_gpio_request_one(&client->dev, tps->vsel0_gpio,
 				gpio_flags, "tps62360-vsel0");
 		if (ret) {
 			dev_err(&client->dev,
 				"%s(): Could not obtain vsel0 GPIO %d: %d\n",
 				__func__, tps->vsel0_gpio, ret);
-			goto err_gpio0;
+			return ret;
 		}
 
 		gpio_flags = (pdata->vsel1_def_state) ?
 				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
-		ret = gpio_request_one(tps->vsel1_gpio,
+		ret = devm_gpio_request_one(&client->dev, tps->vsel1_gpio,
 				gpio_flags, "tps62360-vsel1");
 		if (ret) {
 			dev_err(&client->dev,
 				"%s(): Could not obtain vsel1 GPIO %d: %d\n",
 				__func__, tps->vsel1_gpio, ret);
-			goto err_gpio1;
+			return ret;
 		}
 		tps->valid_gpios = true;
 
@@ -484,7 +463,7 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 	if (ret < 0) {
 		dev_err(tps->dev, "%s(): Init failed with err = %d\n",
 				__func__, ret);
-		goto err_init;
+		return ret;
 	}
 
 	config.dev = &client->dev;
@@ -498,21 +477,11 @@ static int __devinit tps62360_probe(struct i2c_client *client,
 		dev_err(tps->dev,
 			"%s(): regulator register failed with err %s\n",
 			__func__, id->name);
-		ret = PTR_ERR(rdev);
-		goto err_init;
+		return PTR_ERR(rdev);
 	}
 
 	tps->rdev = rdev;
 	return 0;
-
-err_init:
-	if (gpio_is_valid(tps->vsel1_gpio))
-		gpio_free(tps->vsel1_gpio);
-err_gpio1:
-	if (gpio_is_valid(tps->vsel0_gpio))
-		gpio_free(tps->vsel0_gpio);
-err_gpio0:
-	return ret;
 }
 
 /**
@@ -525,12 +494,6 @@ static int __devexit tps62360_remove(struct i2c_client *client)
 {
 	struct tps62360_chip *tps = i2c_get_clientdata(client);
 
-	if (gpio_is_valid(tps->vsel1_gpio))
-		gpio_free(tps->vsel1_gpio);
-
-	if (gpio_is_valid(tps->vsel0_gpio))
-		gpio_free(tps->vsel0_gpio);
-
 	regulator_unregister(tps->rdev);
 	return 0;
 }
diff --git a/drivers/regulator/tps65023-regulator.c b/drivers/regulator/tps65023-regulator.c
index 8f1be8586c7..6998d579d07 100644
--- a/drivers/regulator/tps65023-regulator.c
+++ b/drivers/regulator/tps65023-regulator.c
@@ -69,10 +69,6 @@
 #define TPS65023_REG_CTRL2_DCDC1	BIT(1)
 #define TPS65023_REG_CTRL2_DCDC3	BIT(0)
 
-/* LDO_CTRL bitfields */
-#define TPS65023_LDO_CTRL_LDOx_SHIFT(ldo_id)	((ldo_id)*4)
-#define TPS65023_LDO_CTRL_LDOx_MASK(ldo_id)	(0x07 << ((ldo_id)*4))
-
 /* Number of step-down converters available */
 #define TPS65023_NUM_DCDC		3
 /* Number of LDO voltage regulators  available */
@@ -91,48 +87,53 @@
 #define TPS65023_MAX_REG_ID		TPS65023_LDO_2
 
 /* Supported voltage values for regulators */
-static const u16 VCORE_VSEL_table[] = {
-	800, 825, 850, 875,
-	900, 925, 950, 975,
-	1000, 1025, 1050, 1075,
-	1100, 1125, 1150, 1175,
-	1200, 1225, 1250, 1275,
-	1300, 1325, 1350, 1375,
-	1400, 1425, 1450, 1475,
-	1500, 1525, 1550, 1600,
+static const unsigned int VCORE_VSEL_table[] = {
+	800000, 825000, 850000, 875000,
+	900000, 925000, 950000, 975000,
+	1000000, 1025000, 1050000, 1075000,
+	1100000, 1125000, 1150000, 1175000,
+	1200000, 1225000, 1250000, 1275000,
+	1300000, 1325000, 1350000, 1375000,
+	1400000, 1425000, 1450000, 1475000,
+	1500000, 1525000, 1550000, 1600000,
+};
+
+static const unsigned int DCDC_FIXED_3300000_VSEL_table[] = {
+	3300000,
+};
+
+static const unsigned int DCDC_FIXED_1800000_VSEL_table[] = {
+	1800000,
 };
 
 /* Supported voltage values for LDO regulators for tps65020 */
-static const u16 TPS65020_LDO1_VSEL_table[] = {
-	1000, 1050, 1100, 1300,
-	1800, 2500, 3000, 3300,
+static const unsigned int TPS65020_LDO1_VSEL_table[] = {
+	1000000, 1050000, 1100000, 1300000,
+	1800000, 2500000, 3000000, 3300000,
 };
 
-static const u16 TPS65020_LDO2_VSEL_table[] = {
-	1000, 1050, 1100, 1300,
-	1800, 2500, 3000, 3300,
+static const unsigned int TPS65020_LDO2_VSEL_table[] = {
+	1000000, 1050000, 1100000, 1300000,
+	1800000, 2500000, 3000000, 3300000,
 };
 
 /* Supported voltage values for LDO regulators
  * for tps65021 and tps65023 */
-static const u16 TPS65023_LDO1_VSEL_table[] = {
-	1000, 1100, 1300, 1800,
-	2200, 2600, 2800, 3150,
+static const unsigned int TPS65023_LDO1_VSEL_table[] = {
+	1000000, 1100000, 1300000, 1800000,
+	2200000, 2600000, 2800000, 3150000,
 };
 
-static const u16 TPS65023_LDO2_VSEL_table[] = {
-	1050, 1200, 1300, 1800,
-	2500, 2800, 3000, 3300,
+static const unsigned int TPS65023_LDO2_VSEL_table[] = {
+	1050000, 1200000, 1300000, 1800000,
+	2500000, 2800000, 3000000, 3300000,
 };
 
 /* Regulator specific details */
 struct tps_info {
 	const char *name;
-	unsigned min_uV;
-	unsigned max_uV;
-	bool fixed;
 	u8 table_len;
-	const u16 *table;
+	const unsigned int *table;
 };
 
 /* PMIC details */
@@ -150,7 +151,7 @@ struct tps_driver_data {
 	u8 core_regulator;
 };
 
-static int tps65023_dcdc_get_voltage(struct regulator_dev *dev)
+static int tps65023_dcdc_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int ret;
@@ -164,9 +165,9 @@ static int tps65023_dcdc_get_voltage(struct regulator_dev *dev)
 		if (ret != 0)
 			return ret;
 		data &= (tps->info[dcdc]->table_len - 1);
-		return tps->info[dcdc]->table[data] * 1000;
+		return data;
 	} else
-		return tps->info[dcdc]->min_uV;
+		return 0;
 }
 
 static int tps65023_dcdc_set_voltage_sel(struct regulator_dev *dev,
@@ -193,76 +194,14 @@ out:
 	return ret;
 }
 
-static int tps65023_ldo_get_voltage(struct regulator_dev *dev)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int data, ldo = rdev_get_id(dev);
-	int ret;
-
-	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
-		return -EINVAL;
-
-	ret = regmap_read(tps->regmap, TPS65023_REG_LDO_CTRL, &data);
-	if (ret != 0)
-		return ret;
-
-	data >>= (TPS65023_LDO_CTRL_LDOx_SHIFT(ldo - TPS65023_LDO_1));
-	data &= (tps->info[ldo]->table_len - 1);
-	return tps->info[ldo]->table[data] * 1000;
-}
-
-static int tps65023_ldo_set_voltage_sel(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int ldo_index = rdev_get_id(dev) - TPS65023_LDO_1;
-
-	return regmap_update_bits(tps->regmap, TPS65023_REG_LDO_CTRL,
-			TPS65023_LDO_CTRL_LDOx_MASK(ldo_index),
-			selector << TPS65023_LDO_CTRL_LDOx_SHIFT(ldo_index));
-}
-
-static int tps65023_dcdc_list_voltage(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int dcdc = rdev_get_id(dev);
-
-	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
-		return -EINVAL;
-
-	if (dcdc == tps->core_regulator) {
-		if (selector >= tps->info[dcdc]->table_len)
-			return -EINVAL;
-		else
-			return tps->info[dcdc]->table[selector] * 1000;
-	} else
-		return tps->info[dcdc]->min_uV;
-}
-
-static int tps65023_ldo_list_voltage(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct tps_pmic *tps = rdev_get_drvdata(dev);
-	int ldo = rdev_get_id(dev);
-
-	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
-		return -EINVAL;
-
-	if (selector >= tps->info[ldo]->table_len)
-		return -EINVAL;
-	else
-		return tps->info[ldo]->table[selector] * 1000;
-}
-
 /* Operations permitted on VDCDCx */
 static struct regulator_ops tps65023_dcdc_ops = {
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
-	.get_voltage = tps65023_dcdc_get_voltage,
+	.get_voltage_sel = tps65023_dcdc_get_voltage_sel,
 	.set_voltage_sel = tps65023_dcdc_set_voltage_sel,
-	.list_voltage = tps65023_dcdc_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 };
 
 /* Operations permitted on LDOx */
@@ -270,9 +209,9 @@ static struct regulator_ops tps65023_ldo_ops = {
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
-	.get_voltage = tps65023_ldo_get_voltage,
-	.set_voltage_sel = tps65023_ldo_set_voltage_sel,
-	.list_voltage = tps65023_ldo_list_voltage,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.list_voltage = regulator_list_voltage_table,
 };
 
 static struct regmap_config tps65023_regmap_config = {
@@ -325,19 +264,28 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
 		tps->desc[i].name = info->name;
 		tps->desc[i].id = i;
 		tps->desc[i].n_voltages = info->table_len;
+		tps->desc[i].volt_table = info->table;
 		tps->desc[i].ops = (i > TPS65023_DCDC_3 ?
 					&tps65023_ldo_ops : &tps65023_dcdc_ops);
 		tps->desc[i].type = REGULATOR_VOLTAGE;
 		tps->desc[i].owner = THIS_MODULE;
 
 		tps->desc[i].enable_reg = TPS65023_REG_REG_CTRL;
-		if (i == TPS65023_LDO_1)
+		switch (i) {
+		case TPS65023_LDO_1:
+			tps->desc[i].vsel_reg = TPS65023_REG_LDO_CTRL;
+			tps->desc[i].vsel_mask = 0x07;
 			tps->desc[i].enable_mask = 1 << 1;
-		else if (i == TPS65023_LDO_2)
+			break;
+		case TPS65023_LDO_2:
+			tps->desc[i].vsel_reg = TPS65023_REG_LDO_CTRL;
+			tps->desc[i].vsel_mask = 0x70;
 			tps->desc[i].enable_mask = 1 << 2;
-		else /* DCDCx */
+			break;
+		default: /* DCDCx */
 			tps->desc[i].enable_mask =
 					1 << (TPS65023_NUM_REGULATOR - i);
+		}
 
 		config.dev = &client->dev;
 		config.init_data = init_data;
@@ -384,35 +332,26 @@ static int __devexit tps_65023_remove(struct i2c_client *client)
 static const struct tps_info tps65020_regs[] = {
 	{
 		.name = "VDCDC1",
-		.min_uV = 3300000,
-		.max_uV = 3300000,
-		.fixed	= 1,
+		.table_len = ARRAY_SIZE(DCDC_FIXED_3300000_VSEL_table),
+		.table = DCDC_FIXED_3300000_VSEL_table,
 	},
 	{
 		.name = "VDCDC2",
-		.min_uV =  1800000,
-		.max_uV = 1800000,
-		.fixed = 1,
+		.table_len = ARRAY_SIZE(DCDC_FIXED_1800000_VSEL_table),
+		.table = DCDC_FIXED_1800000_VSEL_table,
 	},
 	{
 		.name = "VDCDC3",
-		.min_uV =  800000,
-		.max_uV = 1600000,
 		.table_len = ARRAY_SIZE(VCORE_VSEL_table),
 		.table = VCORE_VSEL_table,
 	},
-
 	{
 		.name = "LDO1",
-		.min_uV = 1000000,
-		.max_uV = 3150000,
 		.table_len = ARRAY_SIZE(TPS65020_LDO1_VSEL_table),
 		.table = TPS65020_LDO1_VSEL_table,
 	},
 	{
 		.name = "LDO2",
-		.min_uV = 1050000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(TPS65020_LDO2_VSEL_table),
 		.table = TPS65020_LDO2_VSEL_table,
 	},
@@ -421,34 +360,26 @@ static const struct tps_info tps65020_regs[] = {
 static const struct tps_info tps65021_regs[] = {
 	{
 		.name = "VDCDC1",
-		.min_uV =  3300000,
-		.max_uV = 3300000,
-		.fixed = 1,
+		.table_len = ARRAY_SIZE(DCDC_FIXED_3300000_VSEL_table),
+		.table = DCDC_FIXED_3300000_VSEL_table,
 	},
 	{
 		.name = "VDCDC2",
-		.min_uV =  1800000,
-		.max_uV = 1800000,
-		.fixed = 1,
+		.table_len = ARRAY_SIZE(DCDC_FIXED_1800000_VSEL_table),
+		.table = DCDC_FIXED_1800000_VSEL_table,
 	},
 	{
 		.name = "VDCDC3",
-		.min_uV =  800000,
-		.max_uV = 1600000,
 		.table_len = ARRAY_SIZE(VCORE_VSEL_table),
 		.table = VCORE_VSEL_table,
 	},
 	{
 		.name = "LDO1",
-		.min_uV = 1000000,
-		.max_uV = 3150000,
 		.table_len = ARRAY_SIZE(TPS65023_LDO1_VSEL_table),
 		.table = TPS65023_LDO1_VSEL_table,
 	},
 	{
 		.name = "LDO2",
-		.min_uV = 1050000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(TPS65023_LDO2_VSEL_table),
 		.table = TPS65023_LDO2_VSEL_table,
 	},
@@ -457,34 +388,26 @@ static const struct tps_info tps65021_regs[] = {
 static const struct tps_info tps65023_regs[] = {
 	{
 		.name = "VDCDC1",
-		.min_uV =  800000,
-		.max_uV = 1600000,
 		.table_len = ARRAY_SIZE(VCORE_VSEL_table),
 		.table = VCORE_VSEL_table,
 	},
 	{
 		.name = "VDCDC2",
-		.min_uV =  3300000,
-		.max_uV = 3300000,
-		.fixed = 1,
+		.table_len = ARRAY_SIZE(DCDC_FIXED_3300000_VSEL_table),
+		.table = DCDC_FIXED_3300000_VSEL_table,
 	},
 	{
 		.name = "VDCDC3",
-		.min_uV =  1800000,
-		.max_uV = 1800000,
-		.fixed = 1,
+		.table_len = ARRAY_SIZE(DCDC_FIXED_1800000_VSEL_table),
+		.table = DCDC_FIXED_1800000_VSEL_table,
 	},
 	{
 		.name = "LDO1",
-		.min_uV = 1000000,
-		.max_uV = 3150000,
 		.table_len = ARRAY_SIZE(TPS65023_LDO1_VSEL_table),
 		.table = TPS65023_LDO1_VSEL_table,
 	},
 	{
 		.name = "LDO2",
-		.min_uV = 1050000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(TPS65023_LDO2_VSEL_table),
 		.table = TPS65023_LDO2_VSEL_table,
 	},
diff --git a/drivers/regulator/tps6507x-regulator.c b/drivers/regulator/tps6507x-regulator.c
index da38be1016a..07d01ccdf30 100644
--- a/drivers/regulator/tps6507x-regulator.c
+++ b/drivers/regulator/tps6507x-regulator.c
@@ -43,58 +43,40 @@
 /* Number of total regulators available */
 #define TPS6507X_NUM_REGULATOR		(TPS6507X_NUM_DCDC + TPS6507X_NUM_LDO)
 
-/* Supported voltage values for regulators (in milliVolts) */
-static const u16 VDCDCx_VSEL_table[] = {
-	725, 750, 775, 800,
-	825, 850, 875, 900,
-	925, 950, 975, 1000,
-	1025, 1050, 1075, 1100,
-	1125, 1150, 1175, 1200,
-	1225, 1250, 1275, 1300,
-	1325, 1350, 1375, 1400,
-	1425, 1450, 1475, 1500,
-	1550, 1600, 1650, 1700,
-	1750, 1800, 1850, 1900,
-	1950, 2000, 2050, 2100,
-	2150, 2200, 2250, 2300,
-	2350, 2400, 2450, 2500,
-	2550, 2600, 2650, 2700,
-	2750, 2800, 2850, 2900,
-	3000, 3100, 3200, 3300,
+/* Supported voltage values for regulators (in microVolts) */
+static const unsigned int VDCDCx_VSEL_table[] = {
+	725000, 750000, 775000, 800000,
+	825000, 850000, 875000, 900000,
+	925000, 950000, 975000, 1000000,
+	1025000, 1050000, 1075000, 1100000,
+	1125000, 1150000, 1175000, 1200000,
+	1225000, 1250000, 1275000, 1300000,
+	1325000, 1350000, 1375000, 1400000,
+	1425000, 1450000, 1475000, 1500000,
+	1550000, 1600000, 1650000, 1700000,
+	1750000, 1800000, 1850000, 1900000,
+	1950000, 2000000, 2050000, 2100000,
+	2150000, 2200000, 2250000, 2300000,
+	2350000, 2400000, 2450000, 2500000,
+	2550000, 2600000, 2650000, 2700000,
+	2750000, 2800000, 2850000, 2900000,
+	3000000, 3100000, 3200000, 3300000,
 };
 
-static const u16 LDO1_VSEL_table[] = {
-	1000, 1100, 1200, 1250,
-	1300, 1350, 1400, 1500,
-	1600, 1800, 2500, 2750,
-	2800, 3000, 3100, 3300,
+static const unsigned int LDO1_VSEL_table[] = {
+	1000000, 1100000, 1200000, 1250000,
+	1300000, 1350000, 1400000, 1500000,
+	1600000, 1800000, 2500000, 2750000,
+	2800000, 3000000, 3100000, 3300000,
 };
 
-static const u16 LDO2_VSEL_table[] = {
-	725, 750, 775, 800,
-	825, 850, 875, 900,
-	925, 950, 975, 1000,
-	1025, 1050, 1075, 1100,
-	1125, 1150, 1175, 1200,
-	1225, 1250, 1275, 1300,
-	1325, 1350, 1375, 1400,
-	1425, 1450, 1475, 1500,
-	1550, 1600, 1650, 1700,
-	1750, 1800, 1850, 1900,
-	1950, 2000, 2050, 2100,
-	2150, 2200, 2250, 2300,
-	2350, 2400, 2450, 2500,
-	2550, 2600, 2650, 2700,
-	2750, 2800, 2850, 2900,
-	3000, 3100, 3200, 3300,
-};
+/* The voltage mapping table for LDO2 is the same as VDCDCx */
+#define LDO2_VSEL_table VDCDCx_VSEL_table
 
 struct tps_info {
 	const char *name;
-	unsigned min_uV;
-	unsigned max_uV;
 	u8 table_len;
-	const u16 *table;
+	const unsigned int *table;
 
 	/* Does DCDC high or the low register defines output voltage? */
 	bool defdcdc_default;
@@ -103,36 +85,26 @@ struct tps_info {
 static struct tps_info tps6507x_pmic_regs[] = {
 	{
 		.name = "VDCDC1",
-		.min_uV = 725000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
 		.table = VDCDCx_VSEL_table,
 	},
 	{
 		.name = "VDCDC2",
-		.min_uV = 725000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
 		.table = VDCDCx_VSEL_table,
 	},
 	{
 		.name = "VDCDC3",
-		.min_uV = 725000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
 		.table = VDCDCx_VSEL_table,
 	},
 	{
 		.name = "LDO1",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(LDO1_VSEL_table),
 		.table = LDO1_VSEL_table,
 	},
 	{
 		.name = "LDO2",
-		.min_uV = 725000,
-		.max_uV = 3300000,
 		.table_len = ARRAY_SIZE(LDO2_VSEL_table),
 		.table = LDO2_VSEL_table,
 	},
@@ -375,28 +347,13 @@ static int tps6507x_pmic_set_voltage_sel(struct regulator_dev *dev,
 	return tps6507x_pmic_reg_write(tps, reg, data);
 }
 
-static int tps6507x_pmic_list_voltage(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
-	int rid = rdev_get_id(dev);
-
-	if (rid < TPS6507X_DCDC_1 || rid > TPS6507X_LDO_2)
-		return -EINVAL;
-
-	if (selector >= tps->info[rid]->table_len)
-		return -EINVAL;
-	else
-		return tps->info[rid]->table[selector] * 1000;
-}
-
 static struct regulator_ops tps6507x_pmic_ops = {
 	.is_enabled = tps6507x_pmic_is_enabled,
 	.enable = tps6507x_pmic_enable,
 	.disable = tps6507x_pmic_disable,
 	.get_voltage_sel = tps6507x_pmic_get_voltage_sel,
 	.set_voltage_sel = tps6507x_pmic_set_voltage_sel,
-	.list_voltage = tps6507x_pmic_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 };
 
 static __devinit int tps6507x_pmic_probe(struct platform_device *pdev)
@@ -449,6 +406,7 @@ static __devinit int tps6507x_pmic_probe(struct platform_device *pdev)
 		tps->desc[i].name = info->name;
 		tps->desc[i].id = i;
 		tps->desc[i].n_voltages = info->table_len;
+		tps->desc[i].volt_table = info->table;
 		tps->desc[i].ops = &tps6507x_pmic_ops;
 		tps->desc[i].type = REGULATOR_VOLTAGE;
 		tps->desc[i].owner = THIS_MODULE;
diff --git a/drivers/regulator/tps65217-regulator.c b/drivers/regulator/tps65217-regulator.c
index 9d371d2cbca..6caa222af77 100644
--- a/drivers/regulator/tps65217-regulator.c
+++ b/drivers/regulator/tps65217-regulator.c
@@ -26,7 +26,7 @@
 #include <linux/regulator/machine.h>
 #include <linux/mfd/tps65217.h>
 
-#define TPS65217_REGULATOR(_name, _id, _ops, _n)	\
+#define TPS65217_REGULATOR(_name, _id, _ops, _n, _vr, _vm, _em, _t) \
 	{						\
 		.name		= _name,		\
 		.id		= _id,			\
@@ -34,23 +34,23 @@
 		.n_voltages	= _n,			\
 		.type		= REGULATOR_VOLTAGE,	\
 		.owner		= THIS_MODULE,		\
+		.vsel_reg	= _vr,			\
+		.vsel_mask	= _vm,			\
+		.enable_reg	= TPS65217_REG_ENABLE,	\
+		.enable_mask	= _em,			\
+		.volt_table	= _t,			\
 	}						\
 
-#define TPS65217_INFO(_nm, _min, _max, _f1, _f2, _t, _n, _em, _vr, _vm)	\
+#define TPS65217_INFO(_nm, _min, _max, _f1, _f2)	\
 	{						\
 		.name		= _nm,			\
 		.min_uV		= _min,			\
 		.max_uV		= _max,			\
 		.vsel_to_uv	= _f1,			\
 		.uv_to_vsel	= _f2,			\
-		.table		= _t,			\
-		.table_len	= _n,			\
-		.enable_mask	= _em,			\
-		.set_vout_reg	= _vr,			\
-		.set_vout_mask	= _vm,			\
 	}
 
-static const int LDO1_VSEL_table[] = {
+static const unsigned int LDO1_VSEL_table[] = {
 	1000000, 1100000, 1200000, 1250000,
 	1300000, 1350000, 1400000, 1500000,
 	1600000, 1800000, 2500000, 2750000,
@@ -78,7 +78,7 @@ static int tps65217_vsel_to_uv1(unsigned int vsel)
 
 static int tps65217_uv_to_vsel1(int uV, unsigned int *vsel)
 {
-	if ((uV < 0) && (uV > 3300000))
+	if (uV < 0 || uV > 3300000)
 		return -EINVAL;
 
 	if (uV <= 1500000)
@@ -112,7 +112,7 @@ static int tps65217_vsel_to_uv2(unsigned int vsel)
 
 static int tps65217_uv_to_vsel2(int uV, unsigned int *vsel)
 {
-	if ((uV < 0) && (uV > 3300000))
+	if (uV < 0 || uV > 3300000)
 		return -EINVAL;
 
 	if (uV <= 1900000)
@@ -127,46 +127,20 @@ static int tps65217_uv_to_vsel2(int uV, unsigned int *vsel)
 
 static struct tps_info tps65217_pmic_regs[] = {
 	TPS65217_INFO("DCDC1", 900000, 1800000, tps65217_vsel_to_uv1,
-			tps65217_uv_to_vsel1, NULL, 64, TPS65217_ENABLE_DC1_EN,
-			TPS65217_REG_DEFDCDC1, TPS65217_DEFDCDCX_DCDC_MASK),
+			tps65217_uv_to_vsel1),
 	TPS65217_INFO("DCDC2", 900000, 3300000, tps65217_vsel_to_uv1,
-			tps65217_uv_to_vsel1, NULL, 64, TPS65217_ENABLE_DC2_EN,
-			TPS65217_REG_DEFDCDC2, TPS65217_DEFDCDCX_DCDC_MASK),
+			tps65217_uv_to_vsel1),
 	TPS65217_INFO("DCDC3", 900000, 1500000, tps65217_vsel_to_uv1,
-			tps65217_uv_to_vsel1, NULL, 64, TPS65217_ENABLE_DC3_EN,
-			TPS65217_REG_DEFDCDC3, TPS65217_DEFDCDCX_DCDC_MASK),
-	TPS65217_INFO("LDO1", 1000000, 3300000, NULL, NULL, LDO1_VSEL_table,
-			16, TPS65217_ENABLE_LDO1_EN, TPS65217_REG_DEFLDO1,
-			TPS65217_DEFLDO1_LDO1_MASK),
+			tps65217_uv_to_vsel1),
+	TPS65217_INFO("LDO1", 1000000, 3300000, NULL, NULL),
 	TPS65217_INFO("LDO2", 900000, 3300000, tps65217_vsel_to_uv1,
-			tps65217_uv_to_vsel1, NULL, 64, TPS65217_ENABLE_LDO2_EN,
-			TPS65217_REG_DEFLDO2, TPS65217_DEFLDO2_LDO2_MASK),
+			tps65217_uv_to_vsel1),
 	TPS65217_INFO("LDO3", 1800000, 3300000, tps65217_vsel_to_uv2,
-			tps65217_uv_to_vsel2, NULL, 32,
-			TPS65217_ENABLE_LS1_EN | TPS65217_DEFLDO3_LDO3_EN,
-			TPS65217_REG_DEFLS1, TPS65217_DEFLDO3_LDO3_MASK),
+			tps65217_uv_to_vsel2),
 	TPS65217_INFO("LDO4", 1800000, 3300000, tps65217_vsel_to_uv2,
-			tps65217_uv_to_vsel2, NULL, 32,
-			TPS65217_ENABLE_LS2_EN | TPS65217_DEFLDO4_LDO4_EN,
-			TPS65217_REG_DEFLS2, TPS65217_DEFLDO4_LDO4_MASK),
+			tps65217_uv_to_vsel2),
 };
 
-static int tps65217_pmic_is_enabled(struct regulator_dev *dev)
-{
-	int ret;
-	struct tps65217 *tps = rdev_get_drvdata(dev);
-	unsigned int data, rid = rdev_get_id(dev);
-
-	if (rid < TPS65217_DCDC_1 || rid > TPS65217_LDO_4)
-		return -EINVAL;
-
-	ret = tps65217_reg_read(tps, TPS65217_REG_ENABLE, &data);
-	if (ret)
-		return ret;
-
-	return (data & tps->info[rid]->enable_mask) ? 1 : 0;
-}
-
 static int tps65217_pmic_enable(struct regulator_dev *dev)
 {
 	struct tps65217 *tps = rdev_get_drvdata(dev);
@@ -177,9 +151,8 @@ static int tps65217_pmic_enable(struct regulator_dev *dev)
 
 	/* Enable the regulator and password protection is level 1 */
 	return tps65217_set_bits(tps, TPS65217_REG_ENABLE,
-				tps->info[rid]->enable_mask,
-				tps->info[rid]->enable_mask,
-				TPS65217_PROTECT_L1);
+				 dev->desc->enable_mask, dev->desc->enable_mask,
+				 TPS65217_PROTECT_L1);
 }
 
 static int tps65217_pmic_disable(struct regulator_dev *dev)
@@ -192,25 +165,7 @@ static int tps65217_pmic_disable(struct regulator_dev *dev)
 
 	/* Disable the regulator and password protection is level 1 */
 	return tps65217_clear_bits(tps, TPS65217_REG_ENABLE,
-			tps->info[rid]->enable_mask, TPS65217_PROTECT_L1);
-}
-
-static int tps65217_pmic_get_voltage_sel(struct regulator_dev *dev)
-{
-	int ret;
-	struct tps65217 *tps = rdev_get_drvdata(dev);
-	unsigned int selector, rid = rdev_get_id(dev);
-
-	if (rid < TPS65217_DCDC_1 || rid > TPS65217_LDO_4)
-		return -EINVAL;
-
-	ret = tps65217_reg_read(tps, tps->info[rid]->set_vout_reg, &selector);
-	if (ret)
-		return ret;
-
-	selector &= tps->info[rid]->set_vout_mask;
-
-	return selector;
+				   dev->desc->enable_mask, TPS65217_PROTECT_L1);
 }
 
 static int tps65217_pmic_set_voltage_sel(struct regulator_dev *dev,
@@ -221,8 +176,7 @@ static int tps65217_pmic_set_voltage_sel(struct regulator_dev *dev,
 	unsigned int rid = rdev_get_id(dev);
 
 	/* Set the voltage based on vsel value and write protect level is 2 */
-	ret = tps65217_set_bits(tps, tps->info[rid]->set_vout_reg,
-				tps->info[rid]->set_vout_mask,
+	ret = tps65217_set_bits(tps, dev->desc->vsel_reg, dev->desc->vsel_mask,
 				selector, TPS65217_PROTECT_L2);
 
 	/* Set GO bit for DCDCx to initiate voltage transistion */
@@ -252,10 +206,10 @@ static int tps65217_pmic_map_voltage(struct regulator_dev *dev,
 	if (rid < TPS65217_DCDC_1 || rid > TPS65217_LDO_4)
 		return -EINVAL;
 
-	if (min_uV < tps->info[rid]->min_uV || min_uV > tps->info[rid]->max_uV)
-		return -EINVAL;
+	if (min_uV < tps->info[rid]->min_uV)
+		min_uV = tps->info[rid]->min_uV;
 
-	if (max_uV < tps->info[rid]->min_uV || max_uV > tps->info[rid]->max_uV)
+	if (max_uV < tps->info[rid]->min_uV || min_uV > tps->info[rid]->max_uV)
 		return -EINVAL;
 
 	ret = tps->info[rid]->uv_to_vsel(min_uV, &sel);
@@ -274,21 +228,18 @@ static int tps65217_pmic_list_voltage(struct regulator_dev *dev,
 	if (rid < TPS65217_DCDC_1 || rid > TPS65217_LDO_4)
 		return -EINVAL;
 
-	if (selector >= tps->info[rid]->table_len)
+	if (selector >= dev->desc->n_voltages)
 		return -EINVAL;
 
-	if (tps->info[rid]->table)
-		return tps->info[rid]->table[selector];
-
 	return tps->info[rid]->vsel_to_uv(selector);
 }
 
 /* Operations permitted on DCDCx, LDO2, LDO3 and LDO4 */
 static struct regulator_ops tps65217_pmic_ops = {
-	.is_enabled		= tps65217_pmic_is_enabled,
+	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= tps65217_pmic_enable,
 	.disable		= tps65217_pmic_disable,
-	.get_voltage_sel	= tps65217_pmic_get_voltage_sel,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= tps65217_pmic_set_voltage_sel,
 	.list_voltage		= tps65217_pmic_list_voltage,
 	.map_voltage		= tps65217_pmic_map_voltage,
@@ -296,22 +247,38 @@ static struct regulator_ops tps65217_pmic_ops = {
 
 /* Operations permitted on LDO1 */
 static struct regulator_ops tps65217_pmic_ldo1_ops = {
-	.is_enabled		= tps65217_pmic_is_enabled,
+	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= tps65217_pmic_enable,
 	.disable		= tps65217_pmic_disable,
-	.get_voltage_sel	= tps65217_pmic_get_voltage_sel,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= tps65217_pmic_set_voltage_sel,
-	.list_voltage		= tps65217_pmic_list_voltage,
+	.list_voltage		= regulator_list_voltage_table,
 };
 
 static const struct regulator_desc regulators[] = {
-	TPS65217_REGULATOR("DCDC1", TPS65217_DCDC_1, tps65217_pmic_ops, 64),
-	TPS65217_REGULATOR("DCDC2", TPS65217_DCDC_2, tps65217_pmic_ops, 64),
-	TPS65217_REGULATOR("DCDC3", TPS65217_DCDC_3, tps65217_pmic_ops, 64),
-	TPS65217_REGULATOR("LDO1", TPS65217_LDO_1, tps65217_pmic_ldo1_ops, 16),
-	TPS65217_REGULATOR("LDO2", TPS65217_LDO_2, tps65217_pmic_ops, 64),
-	TPS65217_REGULATOR("LDO3", TPS65217_LDO_3, tps65217_pmic_ops, 32),
-	TPS65217_REGULATOR("LDO4", TPS65217_LDO_4, tps65217_pmic_ops, 32),
+	TPS65217_REGULATOR("DCDC1", TPS65217_DCDC_1, tps65217_pmic_ops, 64,
+			   TPS65217_REG_DEFDCDC1, TPS65217_DEFDCDCX_DCDC_MASK,
+			   TPS65217_ENABLE_DC1_EN, NULL),
+	TPS65217_REGULATOR("DCDC2", TPS65217_DCDC_2, tps65217_pmic_ops, 64,
+			   TPS65217_REG_DEFDCDC2, TPS65217_DEFDCDCX_DCDC_MASK,
+			   TPS65217_ENABLE_DC2_EN, NULL),
+	TPS65217_REGULATOR("DCDC3", TPS65217_DCDC_3, tps65217_pmic_ops, 64,
+			   TPS65217_REG_DEFDCDC3, TPS65217_DEFDCDCX_DCDC_MASK,
+			   TPS65217_ENABLE_DC3_EN, NULL),
+	TPS65217_REGULATOR("LDO1", TPS65217_LDO_1, tps65217_pmic_ldo1_ops, 16,
+			   TPS65217_REG_DEFLDO1, TPS65217_DEFLDO1_LDO1_MASK,
+			   TPS65217_ENABLE_LDO1_EN, LDO1_VSEL_table),
+	TPS65217_REGULATOR("LDO2", TPS65217_LDO_2, tps65217_pmic_ops, 64,
+			   TPS65217_REG_DEFLDO2, TPS65217_DEFLDO2_LDO2_MASK,
+			   TPS65217_ENABLE_LDO2_EN, NULL),
+	TPS65217_REGULATOR("LDO3", TPS65217_LDO_3, tps65217_pmic_ops, 32,
+			   TPS65217_REG_DEFLS1, TPS65217_DEFLDO3_LDO3_MASK,
+			   TPS65217_ENABLE_LS1_EN | TPS65217_DEFLDO3_LDO3_EN,
+			   NULL),
+	TPS65217_REGULATOR("LDO4", TPS65217_LDO_4, tps65217_pmic_ops, 32,
+			   TPS65217_REG_DEFLS2, TPS65217_DEFLDO4_LDO4_MASK,
+			   TPS65217_ENABLE_LS2_EN | TPS65217_DEFLDO4_LDO4_EN,
+			   NULL),
 };
 
 static int __devinit tps65217_regulator_probe(struct platform_device *pdev)
@@ -326,6 +293,7 @@ static int __devinit tps65217_regulator_probe(struct platform_device *pdev)
 	tps->info[pdev->id] = info;
 
 	config.dev = &pdev->dev;
+	config.of_node = pdev->dev.of_node;
 	config.init_data = pdev->dev.platform_data;
 	config.driver_data = tps;
 
diff --git a/drivers/regulator/tps6524x-regulator.c b/drivers/regulator/tps6524x-regulator.c
index 1b299aacf22..947ece933d9 100644
--- a/drivers/regulator/tps6524x-regulator.c
+++ b/drivers/regulator/tps6524x-regulator.c
@@ -110,9 +110,6 @@
 #define N_SWITCH		2
 #define N_REGULATORS		(N_DCDC + N_LDO + N_SWITCH)
 
-#define FIXED_ILIMSEL		BIT(0)
-#define FIXED_VOLTAGE		BIT(1)
-
 #define CMD_READ(reg)		((reg) << 6)
 #define CMD_WRITE(reg)		(BIT(5) | (reg) << 6)
 #define STAT_CLK		BIT(3)
@@ -129,12 +126,9 @@ struct field {
 struct supply_info {
 	const char	*name;
 	int		n_voltages;
-	const int	*voltages;
-	int		fixed_voltage;
+	const unsigned int *voltages;
 	int		n_ilimsels;
-	const int	*ilimsels;
-	int		fixed_ilimsel;
-	int		flags;
+	const unsigned int *ilimsels;
 	struct field	enable, voltage, ilimsel;
 };
 
@@ -307,7 +301,7 @@ static int write_field(struct tps6524x *hw, const struct field *field,
 				    val << field->shift);
 }
 
-static const int dcdc1_voltages[] = {
+static const unsigned int dcdc1_voltages[] = {
 	 800000,  825000,  850000,  875000,
 	 900000,  925000,  950000,  975000,
 	1000000, 1025000, 1050000, 1075000,
@@ -318,7 +312,7 @@ static const int dcdc1_voltages[] = {
 	1500000, 1525000, 1550000, 1575000,
 };
 
-static const int dcdc2_voltages[] = {
+static const unsigned int dcdc2_voltages[] = {
 	1400000, 1450000, 1500000, 1550000,
 	1600000, 1650000, 1700000, 1750000,
 	1800000, 1850000, 1900000, 1950000,
@@ -329,7 +323,7 @@ static const int dcdc2_voltages[] = {
 	2800000, 2850000, 2900000, 2950000,
 };
 
-static const int dcdc3_voltages[] = {
+static const unsigned int dcdc3_voltages[] = {
 	2400000, 2450000, 2500000, 2550000, 2600000,
 	2650000, 2700000, 2750000, 2800000, 2850000,
 	2900000, 2950000, 3000000, 3050000, 3100000,
@@ -337,38 +331,54 @@ static const int dcdc3_voltages[] = {
 	3400000, 3450000, 3500000, 3550000, 3600000,
 };
 
-static const int ldo1_voltages[] = {
+static const unsigned int ldo1_voltages[] = {
 	4300000, 4350000, 4400000, 4450000,
 	4500000, 4550000, 4600000, 4650000,
 	4700000, 4750000, 4800000, 4850000,
 	4900000, 4950000, 5000000, 5050000,
 };
 
-static const int ldo2_voltages[] = {
+static const unsigned int ldo2_voltages[] = {
 	1100000, 1150000, 1200000, 1250000,
 	1300000, 1700000, 1750000, 1800000,
 	1850000, 1900000, 3150000, 3200000,
 	3250000, 3300000, 3350000, 3400000,
 };
 
-static const int ldo_ilimsel[] = {
+static const unsigned int fixed_5000000_voltage[] = {
+	5000000
+};
+
+static const unsigned int ldo_ilimsel[] = {
 	400000, 1500000
 };
 
-static const int usb_ilimsel[] = {
+static const unsigned int usb_ilimsel[] = {
 	200000, 400000, 800000, 1000000
 };
 
+static const unsigned int fixed_2400000_ilimsel[] = {
+	2400000
+};
+
+static const unsigned int fixed_1200000_ilimsel[] = {
+	1200000
+};
+
+static const unsigned int fixed_400000_ilimsel[] = {
+	400000
+};
+
 #define __MK_FIELD(_reg, _mask, _shift) \
 	{ .reg = (_reg), .mask = (_mask), .shift = (_shift), }
 
 static const struct supply_info supply_info[N_REGULATORS] = {
 	{
 		.name		= "DCDC1",
-		.flags		= FIXED_ILIMSEL,
 		.n_voltages	= ARRAY_SIZE(dcdc1_voltages),
 		.voltages	= dcdc1_voltages,
-		.fixed_ilimsel	= 2400000,
+		.n_ilimsels	= ARRAY_SIZE(fixed_2400000_ilimsel),
+		.ilimsels	= fixed_2400000_ilimsel,
 		.enable		= __MK_FIELD(REG_DCDC_EN, DCDCDCDC_EN_MASK,
 					     DCDCDCDC1_EN_SHIFT),
 		.voltage	= __MK_FIELD(REG_DCDC_SET, DCDC_VDCDC_MASK,
@@ -376,10 +386,10 @@ static const struct supply_info supply_info[N_REGULATORS] = {
 	},
 	{
 		.name		= "DCDC2",
-		.flags		= FIXED_ILIMSEL,
 		.n_voltages	= ARRAY_SIZE(dcdc2_voltages),
 		.voltages	= dcdc2_voltages,
-		.fixed_ilimsel	= 1200000,
+		.n_ilimsels	= ARRAY_SIZE(fixed_1200000_ilimsel),
+		.ilimsels	= fixed_1200000_ilimsel,
 		.enable		= __MK_FIELD(REG_DCDC_EN, DCDCDCDC_EN_MASK,
 					     DCDCDCDC2_EN_SHIFT),
 		.voltage	= __MK_FIELD(REG_DCDC_SET, DCDC_VDCDC_MASK,
@@ -387,10 +397,10 @@ static const struct supply_info supply_info[N_REGULATORS] = {
 	},
 	{
 		.name		= "DCDC3",
-		.flags		= FIXED_ILIMSEL,
 		.n_voltages	= ARRAY_SIZE(dcdc3_voltages),
 		.voltages	= dcdc3_voltages,
-		.fixed_ilimsel	= 1200000,
+		.n_ilimsels	= ARRAY_SIZE(fixed_1200000_ilimsel),
+		.ilimsels	= fixed_1200000_ilimsel,
 		.enable		= __MK_FIELD(REG_DCDC_EN, DCDCDCDC_EN_MASK,
 					DCDCDCDC3_EN_SHIFT),
 		.voltage	= __MK_FIELD(REG_DCDC_SET, DCDC_VDCDC_MASK,
@@ -424,8 +434,8 @@ static const struct supply_info supply_info[N_REGULATORS] = {
 	},
 	{
 		.name		= "USB",
-		.flags		= FIXED_VOLTAGE,
-		.fixed_voltage	= 5000000,
+		.n_voltages	= ARRAY_SIZE(fixed_5000000_voltage),
+		.voltages	= fixed_5000000_voltage,
 		.n_ilimsels	= ARRAY_SIZE(usb_ilimsel),
 		.ilimsels	= usb_ilimsel,
 		.enable		= __MK_FIELD(REG_BLOCK_EN, BLOCK_MASK,
@@ -435,29 +445,15 @@ static const struct supply_info supply_info[N_REGULATORS] = {
 	},
 	{
 		.name		= "LCD",
-		.flags		= FIXED_VOLTAGE | FIXED_ILIMSEL,
-		.fixed_voltage	= 5000000,
-		.fixed_ilimsel	=  400000,
+		.n_voltages	= ARRAY_SIZE(fixed_5000000_voltage),
+		.voltages	= fixed_5000000_voltage,
+		.n_ilimsels	= ARRAY_SIZE(fixed_400000_ilimsel),
+		.ilimsels	= fixed_400000_ilimsel,
 		.enable		= __MK_FIELD(REG_BLOCK_EN, BLOCK_MASK,
 					     BLOCK_LCD_SHIFT),
 	},
 };
 
-static int list_voltage(struct regulator_dev *rdev, unsigned selector)
-{
-	const struct supply_info *info;
-	struct tps6524x *hw;
-
-	hw	= rdev_get_drvdata(rdev);
-	info	= &supply_info[rdev_get_id(rdev)];
-
-	if (info->flags & FIXED_VOLTAGE)
-		return selector ? -EINVAL : info->fixed_voltage;
-
-	return ((selector < info->n_voltages) ?
-		info->voltages[selector] : -EINVAL);
-}
-
 static int set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 {
 	const struct supply_info *info;
@@ -466,7 +462,7 @@ static int set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 	hw	= rdev_get_drvdata(rdev);
 	info	= &supply_info[rdev_get_id(rdev)];
 
-	if (info->flags & FIXED_VOLTAGE)
+	if (rdev->desc->n_voltages == 1)
 		return -EINVAL;
 
 	return write_field(hw, &info->voltage, selector);
@@ -481,7 +477,7 @@ static int get_voltage_sel(struct regulator_dev *rdev)
 	hw	= rdev_get_drvdata(rdev);
 	info	= &supply_info[rdev_get_id(rdev)];
 
-	if (info->flags & FIXED_VOLTAGE)
+	if (rdev->desc->n_voltages == 1)
 		return 0;
 
 	ret = read_field(hw, &info->voltage);
@@ -503,7 +499,7 @@ static int set_current_limit(struct regulator_dev *rdev, int min_uA,
 	hw	= rdev_get_drvdata(rdev);
 	info	= &supply_info[rdev_get_id(rdev)];
 
-	if (info->flags & FIXED_ILIMSEL)
+	if (info->n_ilimsels == 1)
 		return -EINVAL;
 
 	for (i = 0; i < info->n_ilimsels; i++)
@@ -526,8 +522,8 @@ static int get_current_limit(struct regulator_dev *rdev)
 	hw	= rdev_get_drvdata(rdev);
 	info	= &supply_info[rdev_get_id(rdev)];
 
-	if (info->flags & FIXED_ILIMSEL)
-		return info->fixed_ilimsel;
+	if (info->n_ilimsels == 1)
+		return info->ilimsels[0];
 
 	ret = read_field(hw, &info->ilimsel);
 	if (ret < 0)
@@ -577,7 +573,7 @@ static struct regulator_ops regulator_ops = {
 	.disable		= disable_supply,
 	.get_voltage_sel	= get_voltage_sel,
 	.set_voltage_sel	= set_voltage_sel,
-	.list_voltage		= list_voltage,
+	.list_voltage		= regulator_list_voltage_table,
 	.set_current_limit	= set_current_limit,
 	.get_current_limit	= get_current_limit,
 };
@@ -629,13 +625,11 @@ static int __devinit pmic_probe(struct spi_device *spi)
 		hw->desc[i].name	= info->name;
 		hw->desc[i].id		= i;
 		hw->desc[i].n_voltages	= info->n_voltages;
+		hw->desc[i].volt_table	= info->voltages;
 		hw->desc[i].ops		= &regulator_ops;
 		hw->desc[i].type	= REGULATOR_VOLTAGE;
 		hw->desc[i].owner	= THIS_MODULE;
 
-		if (info->flags & FIXED_VOLTAGE)
-			hw->desc[i].n_voltages = 1;
-
 		config.dev = dev;
 		config.init_data = init_data;
 		config.driver_data = hw;
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index c0a21457538..e6da90ab515 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -63,8 +63,6 @@ struct tps6586x_regulator {
 	int enable_bit[2];
 	int enable_reg[2];
 
-	int *voltages;
-
 	/* for DVM regulators */
 	int go_reg;
 	int go_bit;
@@ -72,22 +70,9 @@ struct tps6586x_regulator {
 
 static inline struct device *to_tps6586x_dev(struct regulator_dev *rdev)
 {
-	return rdev_get_dev(rdev)->parent->parent;
+	return rdev_get_dev(rdev)->parent;
 }
 
-static int tps6586x_list_voltage(struct regulator_dev *rdev, unsigned selector)
-{
-	struct tps6586x_regulator *info = rdev_get_drvdata(rdev);
-	int rid = rdev_get_id(rdev);
-
-	/* LDO0 has minimal voltage 1.2V rather than 1.25V */
-	if ((rid == TPS6586X_ID_LDO_0) && (selector == 0))
-		return (info->voltages[0] - 50) * 1000;
-
-	return info->voltages[selector] * 1000;
-}
-
-
 static int tps6586x_set_voltage_sel(struct regulator_dev *rdev,
 				    unsigned selector)
 {
@@ -168,7 +153,7 @@ static int tps6586x_regulator_is_enabled(struct regulator_dev *rdev)
 }
 
 static struct regulator_ops tps6586x_regulator_ops = {
-	.list_voltage = tps6586x_list_voltage,
+	.list_voltage = regulator_list_voltage_table,
 	.get_voltage_sel = tps6586x_get_voltage_sel,
 	.set_voltage_sel = tps6586x_set_voltage_sel,
 
@@ -177,39 +162,45 @@ static struct regulator_ops tps6586x_regulator_ops = {
 	.disable = tps6586x_regulator_disable,
 };
 
-static int tps6586x_ldo_voltages[] = {
-	1250, 1500, 1800, 2500, 2700, 2850, 3100, 3300,
+static const unsigned int tps6586x_ldo0_voltages[] = {
+	1200000, 1500000, 1800000, 2500000, 2700000, 2850000, 3100000, 3300000,
+};
+
+static const unsigned int tps6586x_ldo4_voltages[] = {
+	1700000, 1725000, 1750000, 1775000, 1800000, 1825000, 1850000, 1875000,
+	1900000, 1925000, 1950000, 1975000, 2000000, 2025000, 2050000, 2075000,
+	2100000, 2125000, 2150000, 2175000, 2200000, 2225000, 2250000, 2275000,
+	2300000, 2325000, 2350000, 2375000, 2400000, 2425000, 2450000, 2475000,
 };
 
-static int tps6586x_ldo4_voltages[] = {
-	1700, 1725, 1750, 1775, 1800, 1825, 1850, 1875,
-	1900, 1925, 1950, 1975, 2000, 2025, 2050, 2075,
-	2100, 2125, 2150, 2175, 2200, 2225, 2250, 2275,
-	2300, 2325, 2350, 2375, 2400, 2425, 2450, 2475,
+static const unsigned int tps6586x_ldo_voltages[] = {
+	1250000, 1500000, 1800000, 2500000, 2700000, 2850000, 3100000, 3300000,
 };
 
-static int tps6586x_sm2_voltages[] = {
-	3000, 3050, 3100, 3150, 3200, 3250, 3300, 3350,
-	3400, 3450, 3500, 3550, 3600, 3650, 3700, 3750,
-	3800, 3850, 3900, 3950, 4000, 4050, 4100, 4150,
-	4200, 4250, 4300, 4350, 4400, 4450, 4500, 4550,
+static const unsigned int tps6586x_sm2_voltages[] = {
+	3000000, 3050000, 3100000, 3150000, 3200000, 3250000, 3300000, 3350000,
+	3400000, 3450000, 3500000, 3550000, 3600000, 3650000, 3700000, 3750000,
+	3800000, 3850000, 3900000, 3950000, 4000000, 4050000, 4100000, 4150000,
+	4200000, 4250000, 4300000, 4350000, 4400000, 4450000, 4500000, 4550000,
 };
 
-static int tps6586x_dvm_voltages[] = {
-	 725,  750,  775,  800,  825,  850,  875,  900,
-	 925,  950,  975, 1000, 1025, 1050, 1075, 1100,
-	1125, 1150, 1175, 1200, 1225, 1250, 1275, 1300,
-	1325, 1350, 1375, 1400, 1425, 1450, 1475, 1500,
+static const unsigned int tps6586x_dvm_voltages[] = {
+	 725000,  750000,  775000,  800000,  825000,  850000,  875000,  900000,
+	 925000,  950000,  975000, 1000000, 1025000, 1050000, 1075000, 1100000,
+	1125000, 1150000, 1175000, 1200000, 1225000, 1250000, 1275000, 1300000,
+	1325000, 1350000, 1375000, 1400000, 1425000, 1450000, 1475000, 1500000,
 };
 
-#define TPS6586X_REGULATOR(_id, vdata, vreg, shift, nbits,		\
+#define TPS6586X_REGULATOR(_id, _pin_name, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1)			\
 	.desc	= {							\
+		.supply_name = _pin_name,				\
 		.name	= "REG-" #_id,					\
 		.ops	= &tps6586x_regulator_ops,			\
 		.type	= REGULATOR_VOLTAGE,				\
 		.id	= TPS6586X_ID_##_id,				\
 		.n_voltages = ARRAY_SIZE(tps6586x_##vdata##_voltages),	\
+		.volt_table = tps6586x_##vdata##_voltages,		\
 		.owner	= THIS_MODULE,					\
 	},								\
 	.volt_reg	= TPS6586X_##vreg,				\
@@ -218,44 +209,45 @@ static int tps6586x_dvm_voltages[] = {
 	.enable_reg[0]	= TPS6586X_SUPPLY##ereg0,			\
 	.enable_bit[0]	= (ebit0),					\
 	.enable_reg[1]	= TPS6586X_SUPPLY##ereg1,			\
-	.enable_bit[1]	= (ebit1),					\
-	.voltages	= tps6586x_##vdata##_voltages,
+	.enable_bit[1]	= (ebit1),
 
 #define TPS6586X_REGULATOR_DVM_GOREG(goreg, gobit)			\
 	.go_reg = TPS6586X_##goreg,					\
 	.go_bit = (gobit),
 
-#define TPS6586X_LDO(_id, vdata, vreg, shift, nbits,			\
+#define TPS6586X_LDO(_id, _pname, vdata, vreg, shift, nbits,		\
 		     ereg0, ebit0, ereg1, ebit1)			\
 {									\
-	TPS6586X_REGULATOR(_id, vdata, vreg, shift, nbits,		\
+	TPS6586X_REGULATOR(_id, _pname, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1)			\
 }
 
-#define TPS6586X_DVM(_id, vdata, vreg, shift, nbits,			\
+#define TPS6586X_DVM(_id, _pname, vdata, vreg, shift, nbits,		\
 		     ereg0, ebit0, ereg1, ebit1, goreg, gobit)		\
 {									\
-	TPS6586X_REGULATOR(_id, vdata, vreg, shift, nbits,		\
+	TPS6586X_REGULATOR(_id, _pname, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1)			\
 	TPS6586X_REGULATOR_DVM_GOREG(goreg, gobit)			\
 }
 
 static struct tps6586x_regulator tps6586x_regulator[] = {
-	TPS6586X_LDO(LDO_0, ldo, SUPPLYV1, 5, 3, ENC, 0, END, 0),
-	TPS6586X_LDO(LDO_3, ldo, SUPPLYV4, 0, 3, ENC, 2, END, 2),
-	TPS6586X_LDO(LDO_5, ldo, SUPPLYV6, 0, 3, ENE, 6, ENE, 6),
-	TPS6586X_LDO(LDO_6, ldo, SUPPLYV3, 0, 3, ENC, 4, END, 4),
-	TPS6586X_LDO(LDO_7, ldo, SUPPLYV3, 3, 3, ENC, 5, END, 5),
-	TPS6586X_LDO(LDO_8, ldo, SUPPLYV2, 5, 3, ENC, 6, END, 6),
-	TPS6586X_LDO(LDO_9, ldo, SUPPLYV6, 3, 3, ENE, 7, ENE, 7),
-	TPS6586X_LDO(LDO_RTC, ldo, SUPPLYV4, 3, 3, V4, 7, V4, 7),
-	TPS6586X_LDO(LDO_1, dvm, SUPPLYV1, 0, 5, ENC, 1, END, 1),
-	TPS6586X_LDO(SM_2, sm2, SUPPLYV2, 0, 5, ENC, 7, END, 7),
-
-	TPS6586X_DVM(LDO_2, dvm, LDO2BV1, 0, 5, ENA, 3, ENB, 3, VCC2, 6),
-	TPS6586X_DVM(LDO_4, ldo4, LDO4V1, 0, 5, ENC, 3, END, 3, VCC1, 6),
-	TPS6586X_DVM(SM_0, dvm, SM0V1, 0, 5, ENA, 1, ENB, 1, VCC1, 2),
-	TPS6586X_DVM(SM_1, dvm, SM1V1, 0, 5, ENA, 0, ENB, 0, VCC1, 0),
+	TPS6586X_LDO(LDO_0, "vinldo01", ldo0, SUPPLYV1, 5, 3, ENC, 0, END, 0),
+	TPS6586X_LDO(LDO_3, "vinldo23", ldo, SUPPLYV4, 0, 3, ENC, 2, END, 2),
+	TPS6586X_LDO(LDO_5, NULL, ldo, SUPPLYV6, 0, 3, ENE, 6, ENE, 6),
+	TPS6586X_LDO(LDO_6, "vinldo678", ldo, SUPPLYV3, 0, 3, ENC, 4, END, 4),
+	TPS6586X_LDO(LDO_7, "vinldo678", ldo, SUPPLYV3, 3, 3, ENC, 5, END, 5),
+	TPS6586X_LDO(LDO_8, "vinldo678", ldo, SUPPLYV2, 5, 3, ENC, 6, END, 6),
+	TPS6586X_LDO(LDO_9, "vinldo9", ldo, SUPPLYV6, 3, 3, ENE, 7, ENE, 7),
+	TPS6586X_LDO(LDO_RTC, NULL, ldo, SUPPLYV4, 3, 3, V4, 7, V4, 7),
+	TPS6586X_LDO(LDO_1, "vinldo01", dvm, SUPPLYV1, 0, 5, ENC, 1, END, 1),
+	TPS6586X_LDO(SM_2, "sm2", sm2, SUPPLYV2, 0, 5, ENC, 7, END, 7),
+
+	TPS6586X_DVM(LDO_2, "vinldo23", dvm, LDO2BV1, 0, 5, ENA, 3,
+					ENB, 3, VCC2, 6),
+	TPS6586X_DVM(LDO_4, "vinldo4", ldo4, LDO4V1, 0, 5, ENC, 3,
+					END, 3, VCC1, 6),
+	TPS6586X_DVM(SM_0, "sm0", dvm, SM0V1, 0, 5, ENA, 1, ENB, 1, VCC1, 2),
+	TPS6586X_DVM(SM_1, "sm1", dvm, SM1V1, 0, 5, ENA, 0, ENB, 0, VCC1, 0),
 };
 
 /*
@@ -362,7 +354,7 @@ static int __devinit tps6586x_regulator_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	config.dev = &pdev->dev;
+	config.dev = pdev->dev.parent;
 	config.of_node = pdev->dev.of_node;
 	config.init_data = pdev->dev.platform_data;
 	config.driver_data = ri;
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index 6bf864b4bdf..793adda560c 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -31,160 +31,147 @@
 			TPS65910_SLEEP_CONTROL_EXT_INPUT_EN3 |		\
 			TPS65911_SLEEP_CONTROL_EXT_INPUT_SLEEP)
 
-/* supported VIO voltages in millivolts */
-static const u16 VIO_VSEL_table[] = {
-	1500, 1800, 2500, 3300,
+/* supported VIO voltages in microvolts */
+static const unsigned int VIO_VSEL_table[] = {
+	1500000, 1800000, 2500000, 3300000,
 };
 
 /* VSEL tables for TPS65910 specific LDOs and dcdc's */
 
-/* supported VDD3 voltages in millivolts */
-static const u16 VDD3_VSEL_table[] = {
-	5000,
+/* supported VDD3 voltages in microvolts */
+static const unsigned int VDD3_VSEL_table[] = {
+	5000000,
 };
 
-/* supported VDIG1 voltages in millivolts */
-static const u16 VDIG1_VSEL_table[] = {
-	1200, 1500, 1800, 2700,
+/* supported VDIG1 voltages in microvolts */
+static const unsigned int VDIG1_VSEL_table[] = {
+	1200000, 1500000, 1800000, 2700000,
 };
 
-/* supported VDIG2 voltages in millivolts */
-static const u16 VDIG2_VSEL_table[] = {
-	1000, 1100, 1200, 1800,
+/* supported VDIG2 voltages in microvolts */
+static const unsigned int VDIG2_VSEL_table[] = {
+	1000000, 1100000, 1200000, 1800000,
 };
 
-/* supported VPLL voltages in millivolts */
-static const u16 VPLL_VSEL_table[] = {
-	1000, 1100, 1800, 2500,
+/* supported VPLL voltages in microvolts */
+static const unsigned int VPLL_VSEL_table[] = {
+	1000000, 1100000, 1800000, 2500000,
 };
 
-/* supported VDAC voltages in millivolts */
-static const u16 VDAC_VSEL_table[] = {
-	1800, 2600, 2800, 2850,
+/* supported VDAC voltages in microvolts */
+static const unsigned int VDAC_VSEL_table[] = {
+	1800000, 2600000, 2800000, 2850000,
 };
 
-/* supported VAUX1 voltages in millivolts */
-static const u16 VAUX1_VSEL_table[] = {
-	1800, 2500, 2800, 2850,
+/* supported VAUX1 voltages in microvolts */
+static const unsigned int VAUX1_VSEL_table[] = {
+	1800000, 2500000, 2800000, 2850000,
 };
 
-/* supported VAUX2 voltages in millivolts */
-static const u16 VAUX2_VSEL_table[] = {
-	1800, 2800, 2900, 3300,
+/* supported VAUX2 voltages in microvolts */
+static const unsigned int VAUX2_VSEL_table[] = {
+	1800000, 2800000, 2900000, 3300000,
 };
 
-/* supported VAUX33 voltages in millivolts */
-static const u16 VAUX33_VSEL_table[] = {
-	1800, 2000, 2800, 3300,
+/* supported VAUX33 voltages in microvolts */
+static const unsigned int VAUX33_VSEL_table[] = {
+	1800000, 2000000, 2800000, 3300000,
 };
 
-/* supported VMMC voltages in millivolts */
-static const u16 VMMC_VSEL_table[] = {
-	1800, 2800, 3000, 3300,
+/* supported VMMC voltages in microvolts */
+static const unsigned int VMMC_VSEL_table[] = {
+	1800000, 2800000, 3000000, 3300000,
 };
 
 struct tps_info {
 	const char *name;
-	unsigned min_uV;
-	unsigned max_uV;
+	const char *vin_name;
 	u8 n_voltages;
-	const u16 *voltage_table;
+	const unsigned int *voltage_table;
 	int enable_time_us;
 };
 
 static struct tps_info tps65910_regs[] = {
 	{
 		.name = "vrtc",
+		.vin_name = "vcc7",
 		.enable_time_us = 2200,
 	},
 	{
 		.name = "vio",
-		.min_uV = 1500000,
-		.max_uV = 3300000,
+		.vin_name = "vccio",
 		.n_voltages = ARRAY_SIZE(VIO_VSEL_table),
 		.voltage_table = VIO_VSEL_table,
 		.enable_time_us = 350,
 	},
 	{
 		.name = "vdd1",
-		.min_uV = 600000,
-		.max_uV = 4500000,
+		.vin_name = "vcc1",
 		.enable_time_us = 350,
 	},
 	{
 		.name = "vdd2",
-		.min_uV = 600000,
-		.max_uV = 4500000,
+		.vin_name = "vcc2",
 		.enable_time_us = 350,
 	},
 	{
 		.name = "vdd3",
-		.min_uV = 5000000,
-		.max_uV = 5000000,
 		.n_voltages = ARRAY_SIZE(VDD3_VSEL_table),
 		.voltage_table = VDD3_VSEL_table,
 		.enable_time_us = 200,
 	},
 	{
 		.name = "vdig1",
-		.min_uV = 1200000,
-		.max_uV = 2700000,
+		.vin_name = "vcc6",
 		.n_voltages = ARRAY_SIZE(VDIG1_VSEL_table),
 		.voltage_table = VDIG1_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vdig2",
-		.min_uV = 1000000,
-		.max_uV = 1800000,
+		.vin_name = "vcc6",
 		.n_voltages = ARRAY_SIZE(VDIG2_VSEL_table),
 		.voltage_table = VDIG2_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vpll",
-		.min_uV = 1000000,
-		.max_uV = 2500000,
+		.vin_name = "vcc5",
 		.n_voltages = ARRAY_SIZE(VPLL_VSEL_table),
 		.voltage_table = VPLL_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vdac",
-		.min_uV = 1800000,
-		.max_uV = 2850000,
+		.vin_name = "vcc5",
 		.n_voltages = ARRAY_SIZE(VDAC_VSEL_table),
 		.voltage_table = VDAC_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vaux1",
-		.min_uV = 1800000,
-		.max_uV = 2850000,
+		.vin_name = "vcc4",
 		.n_voltages = ARRAY_SIZE(VAUX1_VSEL_table),
 		.voltage_table = VAUX1_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vaux2",
-		.min_uV = 1800000,
-		.max_uV = 3300000,
+		.vin_name = "vcc4",
 		.n_voltages = ARRAY_SIZE(VAUX2_VSEL_table),
 		.voltage_table = VAUX2_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vaux33",
-		.min_uV = 1800000,
-		.max_uV = 3300000,
+		.vin_name = "vcc3",
 		.n_voltages = ARRAY_SIZE(VAUX33_VSEL_table),
 		.voltage_table = VAUX33_VSEL_table,
 		.enable_time_us = 100,
 	},
 	{
 		.name = "vmmc",
-		.min_uV = 1800000,
-		.max_uV = 3300000,
+		.vin_name = "vcc3",
 		.n_voltages = ARRAY_SIZE(VMMC_VSEL_table),
 		.voltage_table = VMMC_VSEL_table,
 		.enable_time_us = 100,
@@ -194,91 +181,79 @@ static struct tps_info tps65910_regs[] = {
 static struct tps_info tps65911_regs[] = {
 	{
 		.name = "vrtc",
+		.vin_name = "vcc7",
 		.enable_time_us = 2200,
 	},
 	{
 		.name = "vio",
-		.min_uV = 1500000,
-		.max_uV = 3300000,
+		.vin_name = "vccio",
 		.n_voltages = ARRAY_SIZE(VIO_VSEL_table),
 		.voltage_table = VIO_VSEL_table,
 		.enable_time_us = 350,
 	},
 	{
 		.name = "vdd1",
-		.min_uV = 600000,
-		.max_uV = 4500000,
-		.n_voltages = 73,
+		.vin_name = "vcc1",
+		.n_voltages = 0x4C,
 		.enable_time_us = 350,
 	},
 	{
 		.name = "vdd2",
-		.min_uV = 600000,
-		.max_uV = 4500000,
-		.n_voltages = 73,
+		.vin_name = "vcc2",
+		.n_voltages = 0x4C,
 		.enable_time_us = 350,
 	},
 	{
 		.name = "vddctrl",
-		.min_uV = 600000,
-		.max_uV = 1400000,
-		.n_voltages = 65,
+		.n_voltages = 0x44,
 		.enable_time_us = 900,
 	},
 	{
 		.name = "ldo1",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 47,
+		.vin_name = "vcc6",
+		.n_voltages = 0x33,
 		.enable_time_us = 420,
 	},
 	{
 		.name = "ldo2",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 47,
+		.vin_name = "vcc6",
+		.n_voltages = 0x33,
 		.enable_time_us = 420,
 	},
 	{
 		.name = "ldo3",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 24,
+		.vin_name = "vcc5",
+		.n_voltages = 0x1A,
 		.enable_time_us = 230,
 	},
 	{
 		.name = "ldo4",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 47,
+		.vin_name = "vcc5",
+		.n_voltages = 0x33,
 		.enable_time_us = 230,
 	},
 	{
 		.name = "ldo5",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 24,
+		.vin_name = "vcc4",
+		.n_voltages = 0x1A,
 		.enable_time_us = 230,
 	},
 	{
 		.name = "ldo6",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 24,
+		.vin_name = "vcc3",
+		.n_voltages = 0x1A,
 		.enable_time_us = 230,
 	},
 	{
 		.name = "ldo7",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 24,
+		.vin_name = "vcc3",
+		.n_voltages = 0x1A,
 		.enable_time_us = 230,
 	},
 	{
 		.name = "ldo8",
-		.min_uV = 1000000,
-		.max_uV = 3300000,
-		.n_voltages = 24,
+		.vin_name = "vcc3",
+		.n_voltages = 0x1A,
 		.enable_time_us = 230,
 	},
 };
@@ -321,7 +296,6 @@ struct tps65910_reg {
 	struct tps65910 *mfd;
 	struct regulator_dev **rdev;
 	struct tps_info **info;
-	struct mutex mutex;
 	int num_regulators;
 	int mode;
 	int  (*get_ctrl_reg)(int);
@@ -329,71 +303,6 @@ struct tps65910_reg {
 	unsigned int board_ext_control[TPS65910_NUM_REGS];
 };
 
-static inline int tps65910_read(struct tps65910_reg *pmic, u8 reg)
-{
-	unsigned int val;
-	int err;
-
-	err = tps65910_reg_read(pmic->mfd, reg, &val);
-	if (err)
-		return err;
-
-	return val;
-}
-
-static int tps65910_modify_bits(struct tps65910_reg *pmic, u8 reg,
-					u8 set_mask, u8 clear_mask)
-{
-	int err, data;
-
-	mutex_lock(&pmic->mutex);
-
-	data = tps65910_read(pmic, reg);
-	if (data < 0) {
-		dev_err(pmic->mfd->dev, "Read from reg 0x%x failed\n", reg);
-		err = data;
-		goto out;
-	}
-
-	data &= ~clear_mask;
-	data |= set_mask;
-	err = tps65910_reg_write(pmic->mfd, reg, data);
-	if (err)
-		dev_err(pmic->mfd->dev, "Write for reg 0x%x failed\n", reg);
-
-out:
-	mutex_unlock(&pmic->mutex);
-	return err;
-}
-
-static int tps65910_reg_read_locked(struct tps65910_reg *pmic, u8 reg)
-{
-	int data;
-
-	mutex_lock(&pmic->mutex);
-
-	data = tps65910_read(pmic, reg);
-	if (data < 0)
-		dev_err(pmic->mfd->dev, "Read from reg 0x%x failed\n", reg);
-
-	mutex_unlock(&pmic->mutex);
-	return data;
-}
-
-static int tps65910_reg_write_locked(struct tps65910_reg *pmic, u8 reg, u8 val)
-{
-	int err;
-
-	mutex_lock(&pmic->mutex);
-
-	err = tps65910_reg_write(pmic->mfd, reg, val);
-	if (err < 0)
-		dev_err(pmic->mfd->dev, "Write for reg 0x%x failed\n", reg);
-
-	mutex_unlock(&pmic->mutex);
-	return err;
-}
-
 static int tps65910_get_ctrl_register(int id)
 {
 	switch (id) {
@@ -462,13 +371,6 @@ static int tps65911_get_ctrl_register(int id)
 	}
 }
 
-static int tps65910_enable_time(struct regulator_dev *dev)
-{
-	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	return pmic->info[id]->enable_time_us;
-}
-
 static int tps65910_set_mode(struct regulator_dev *dev, unsigned int mode)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
@@ -481,8 +383,9 @@ static int tps65910_set_mode(struct regulator_dev *dev, unsigned int mode)
 
 	switch (mode) {
 	case REGULATOR_MODE_NORMAL:
-		return tps65910_modify_bits(pmic, reg, LDO_ST_ON_BIT,
-							LDO_ST_MODE_BIT);
+		return tps65910_reg_update_bits(pmic->mfd, reg,
+						LDO_ST_MODE_BIT | LDO_ST_ON_BIT,
+						LDO_ST_ON_BIT);
 	case REGULATOR_MODE_IDLE:
 		value = LDO_ST_ON_BIT | LDO_ST_MODE_BIT;
 		return tps65910_reg_set_bits(mfd, reg, value);
@@ -496,15 +399,15 @@ static int tps65910_set_mode(struct regulator_dev *dev, unsigned int mode)
 static unsigned int tps65910_get_mode(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int reg, value, id = rdev_get_id(dev);
+	int ret, reg, value, id = rdev_get_id(dev);
 
 	reg = pmic->get_ctrl_reg(id);
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read_locked(pmic, reg);
-	if (value < 0)
-		return value;
+	ret = tps65910_reg_read(pmic->mfd, reg, &value);
+	if (ret < 0)
+		return ret;
 
 	if (!(value & LDO_ST_ON_BIT))
 		return REGULATOR_MODE_STANDBY;
@@ -517,33 +420,51 @@ static unsigned int tps65910_get_mode(struct regulator_dev *dev)
 static int tps65910_get_voltage_dcdc_sel(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
+	int ret, id = rdev_get_id(dev);
 	int opvsel = 0, srvsel = 0, vselmax = 0, mult = 0, sr = 0;
 
 	switch (id) {
 	case TPS65910_REG_VDD1:
-		opvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD1_OP);
-		mult = tps65910_reg_read_locked(pmic, TPS65910_VDD1);
+		ret = tps65910_reg_read(pmic->mfd, TPS65910_VDD1_OP, &opvsel);
+		if (ret < 0)
+			return ret;
+		ret = tps65910_reg_read(pmic->mfd, TPS65910_VDD1, &mult);
+		if (ret < 0)
+			return ret;
 		mult = (mult & VDD1_VGAIN_SEL_MASK) >> VDD1_VGAIN_SEL_SHIFT;
-		srvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD1_SR);
+		ret = tps65910_reg_read(pmic->mfd, TPS65910_VDD1_SR, &srvsel);
+		if (ret < 0)
+			return ret;
 		sr = opvsel & VDD1_OP_CMD_MASK;
 		opvsel &= VDD1_OP_SEL_MASK;
 		srvsel &= VDD1_SR_SEL_MASK;
 		vselmax = 75;
 		break;
 	case TPS65910_REG_VDD2:
-		opvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD2_OP);
-		mult = tps65910_reg_read_locked(pmic, TPS65910_VDD2);
+		ret = tps65910_reg_read(pmic->mfd, TPS65910_VDD2_OP, &opvsel);
+		if (ret < 0)
+			return ret;
+		ret = tps65910_reg_read(pmic->mfd, TPS65910_VDD2, &mult);
+		if (ret < 0)
+			return ret;
 		mult = (mult & VDD2_VGAIN_SEL_MASK) >> VDD2_VGAIN_SEL_SHIFT;
-		srvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD2_SR);
+		ret = tps65910_reg_read(pmic->mfd, TPS65910_VDD2_SR, &srvsel);
+		if (ret < 0)
+			return ret;
 		sr = opvsel & VDD2_OP_CMD_MASK;
 		opvsel &= VDD2_OP_SEL_MASK;
 		srvsel &= VDD2_SR_SEL_MASK;
 		vselmax = 75;
 		break;
 	case TPS65911_REG_VDDCTRL:
-		opvsel = tps65910_reg_read_locked(pmic, TPS65911_VDDCTRL_OP);
-		srvsel = tps65910_reg_read_locked(pmic, TPS65911_VDDCTRL_SR);
+		ret = tps65910_reg_read(pmic->mfd, TPS65911_VDDCTRL_OP,
+					&opvsel);
+		if (ret < 0)
+			return ret;
+		ret = tps65910_reg_read(pmic->mfd, TPS65911_VDDCTRL_SR,
+					&srvsel);
+		if (ret < 0)
+			return ret;
 		sr = opvsel & VDDCTRL_OP_CMD_MASK;
 		opvsel &= VDDCTRL_OP_SEL_MASK;
 		srvsel &= VDDCTRL_SR_SEL_MASK;
@@ -577,15 +498,15 @@ static int tps65910_get_voltage_dcdc_sel(struct regulator_dev *dev)
 static int tps65910_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int reg, value, id = rdev_get_id(dev);
+	int ret, reg, value, id = rdev_get_id(dev);
 
 	reg = pmic->get_ctrl_reg(id);
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read_locked(pmic, reg);
-	if (value < 0)
-		return value;
+	ret = tps65910_reg_read(pmic->mfd, reg, &value);
+	if (ret < 0)
+		return ret;
 
 	switch (id) {
 	case TPS65910_REG_VIO:
@@ -609,18 +530,20 @@ static int tps65910_get_voltage_sel(struct regulator_dev *dev)
 
 static int tps65910_get_voltage_vdd3(struct regulator_dev *dev)
 {
-	return 5 * 1000 * 1000;
+	return dev->desc->volt_table[0];
 }
 
 static int tps65911_get_voltage_sel(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	u8 value, reg;
+	int ret, id = rdev_get_id(dev);
+	unsigned int value, reg;
 
 	reg = pmic->get_ctrl_reg(id);
 
-	value = tps65910_reg_read_locked(pmic, reg);
+	ret = tps65910_reg_read(pmic->mfd, reg, &value);
+	if (ret < 0)
+		return ret;
 
 	switch (id) {
 	case TPS65911_REG_LDO1:
@@ -662,10 +585,10 @@ static int tps65910_set_voltage_dcdc_sel(struct regulator_dev *dev,
 			dcdc_mult--;
 		vsel = (selector % VDD1_2_NUM_VOLT_FINE) + 3;
 
-		tps65910_modify_bits(pmic, TPS65910_VDD1,
-				(dcdc_mult << VDD1_VGAIN_SEL_SHIFT),
-						VDD1_VGAIN_SEL_MASK);
-		tps65910_reg_write_locked(pmic, TPS65910_VDD1_OP, vsel);
+		tps65910_reg_update_bits(pmic->mfd, TPS65910_VDD1,
+					 VDD1_VGAIN_SEL_MASK,
+					 dcdc_mult << VDD1_VGAIN_SEL_SHIFT);
+		tps65910_reg_write(pmic->mfd, TPS65910_VDD1_OP, vsel);
 		break;
 	case TPS65910_REG_VDD2:
 		dcdc_mult = (selector / VDD1_2_NUM_VOLT_FINE) + 1;
@@ -673,14 +596,14 @@ static int tps65910_set_voltage_dcdc_sel(struct regulator_dev *dev,
 			dcdc_mult--;
 		vsel = (selector % VDD1_2_NUM_VOLT_FINE) + 3;
 
-		tps65910_modify_bits(pmic, TPS65910_VDD2,
-				(dcdc_mult << VDD2_VGAIN_SEL_SHIFT),
-						VDD1_VGAIN_SEL_MASK);
-		tps65910_reg_write_locked(pmic, TPS65910_VDD2_OP, vsel);
+		tps65910_reg_update_bits(pmic->mfd, TPS65910_VDD2,
+					 VDD1_VGAIN_SEL_MASK,
+					 dcdc_mult << VDD2_VGAIN_SEL_SHIFT);
+		tps65910_reg_write(pmic->mfd, TPS65910_VDD2_OP, vsel);
 		break;
 	case TPS65911_REG_VDDCTRL:
 		vsel = selector + 3;
-		tps65910_reg_write_locked(pmic, TPS65911_VDDCTRL_OP, vsel);
+		tps65910_reg_write(pmic->mfd, TPS65911_VDDCTRL_OP, vsel);
 	}
 
 	return 0;
@@ -706,8 +629,8 @@ static int tps65910_set_voltage_sel(struct regulator_dev *dev,
 	case TPS65910_REG_VAUX2:
 	case TPS65910_REG_VAUX33:
 	case TPS65910_REG_VMMC:
-		return tps65910_modify_bits(pmic, reg,
-				(selector << LDO_SEL_SHIFT), LDO_SEL_MASK);
+		return tps65910_reg_update_bits(pmic->mfd, reg, LDO_SEL_MASK,
+						selector << LDO_SEL_SHIFT);
 	}
 
 	return -EINVAL;
@@ -727,18 +650,18 @@ static int tps65911_set_voltage_sel(struct regulator_dev *dev,
 	case TPS65911_REG_LDO1:
 	case TPS65911_REG_LDO2:
 	case TPS65911_REG_LDO4:
-		return tps65910_modify_bits(pmic, reg,
-				(selector << LDO_SEL_SHIFT), LDO1_SEL_MASK);
+		return tps65910_reg_update_bits(pmic->mfd, reg, LDO1_SEL_MASK,
+						selector << LDO_SEL_SHIFT);
 	case TPS65911_REG_LDO3:
 	case TPS65911_REG_LDO5:
 	case TPS65911_REG_LDO6:
 	case TPS65911_REG_LDO7:
 	case TPS65911_REG_LDO8:
-		return tps65910_modify_bits(pmic, reg,
-				(selector << LDO_SEL_SHIFT), LDO3_SEL_MASK);
+		return tps65910_reg_update_bits(pmic->mfd, reg, LDO3_SEL_MASK,
+						selector << LDO_SEL_SHIFT);
 	case TPS65910_REG_VIO:
-		return tps65910_modify_bits(pmic, reg,
-				(selector << LDO_SEL_SHIFT), LDO_SEL_MASK);
+		return tps65910_reg_update_bits(pmic->mfd, reg, LDO_SEL_MASK,
+						selector << LDO_SEL_SHIFT);
 	}
 
 	return -EINVAL;
@@ -768,23 +691,6 @@ static int tps65910_list_voltage_dcdc(struct regulator_dev *dev,
 	return  volt * 100 * mult;
 }
 
-static int tps65910_list_voltage(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev), voltage;
-
-	if (id < TPS65910_REG_VIO || id > TPS65910_REG_VMMC)
-		return -EINVAL;
-
-	if (selector >= pmic->info[id]->n_voltages)
-		return -EINVAL;
-	else
-		voltage = pmic->info[id]->voltage_table[selector] * 1000;
-
-	return voltage;
-}
-
 static int tps65911_list_voltage(struct regulator_dev *dev, unsigned selector)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
@@ -816,7 +722,7 @@ static int tps65911_list_voltage(struct regulator_dev *dev, unsigned selector)
 		step_mv = 100;
 		break;
 	case TPS65910_REG_VIO:
-		return pmic->info[id]->voltage_table[selector] * 1000;
+		return pmic->info[id]->voltage_table[selector];
 	default:
 		return -EINVAL;
 	}
@@ -824,42 +730,16 @@ static int tps65911_list_voltage(struct regulator_dev *dev, unsigned selector)
 	return (LDO_MIN_VOLT + selector * step_mv) * 1000;
 }
 
-static int tps65910_set_voltage_dcdc_time_sel(struct regulator_dev *dev,
-		unsigned int old_selector, unsigned int new_selector)
-{
-	int id = rdev_get_id(dev);
-	int old_volt, new_volt;
-
-	old_volt = tps65910_list_voltage_dcdc(dev, old_selector);
-	if (old_volt < 0)
-		return old_volt;
-
-	new_volt = tps65910_list_voltage_dcdc(dev, new_selector);
-	if (new_volt < 0)
-		return new_volt;
-
-	/* VDD1 and VDD2 are 12.5mV/us, VDDCTRL is 100mV/20us */
-	switch (id) {
-	case TPS65910_REG_VDD1:
-	case TPS65910_REG_VDD2:
-		return DIV_ROUND_UP(abs(old_volt - new_volt), 12500);
-	case TPS65911_REG_VDDCTRL:
-		return DIV_ROUND_UP(abs(old_volt - new_volt), 5000);
-	}
-	return -EINVAL;
-}
-
 /* Regulator ops (except VRTC) */
 static struct regulator_ops tps65910_ops_dcdc = {
 	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
-	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage_sel	= tps65910_get_voltage_dcdc_sel,
 	.set_voltage_sel	= tps65910_set_voltage_dcdc_sel,
-	.set_voltage_time_sel	= tps65910_set_voltage_dcdc_time_sel,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.list_voltage		= tps65910_list_voltage_dcdc,
 };
 
@@ -867,30 +747,27 @@ static struct regulator_ops tps65910_ops_vdd3 = {
 	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
-	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage		= tps65910_get_voltage_vdd3,
-	.list_voltage		= tps65910_list_voltage,
+	.list_voltage		= regulator_list_voltage_table,
 };
 
 static struct regulator_ops tps65910_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
-	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage_sel	= tps65910_get_voltage_sel,
 	.set_voltage_sel	= tps65910_set_voltage_sel,
-	.list_voltage		= tps65910_list_voltage,
+	.list_voltage		= regulator_list_voltage_table,
 };
 
 static struct regulator_ops tps65911_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
-	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage_sel	= tps65911_get_voltage_sel,
@@ -996,19 +873,27 @@ static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
 				(tps65910_chip_id(mfd) == TPS65911))) {
 		int op_reg_add = pmic->get_ctrl_reg(id) + 1;
 		int sr_reg_add = pmic->get_ctrl_reg(id) + 2;
-		int opvsel = tps65910_reg_read_locked(pmic, op_reg_add);
-		int srvsel = tps65910_reg_read_locked(pmic, sr_reg_add);
+		int opvsel, srvsel;
+
+		ret = tps65910_reg_read(pmic->mfd, op_reg_add, &opvsel);
+		if (ret < 0)
+			return ret;
+		ret = tps65910_reg_read(pmic->mfd, sr_reg_add, &srvsel);
+		if (ret < 0)
+			return ret;
+
 		if (opvsel & VDD1_OP_CMD_MASK) {
 			u8 reg_val = srvsel & VDD1_OP_SEL_MASK;
-			ret = tps65910_reg_write_locked(pmic, op_reg_add,
-							reg_val);
+
+			ret = tps65910_reg_write(pmic->mfd, op_reg_add,
+						 reg_val);
 			if (ret < 0) {
 				dev_err(mfd->dev,
 					"Error in configuring op register\n");
 				return ret;
 			}
 		}
-		ret = tps65910_reg_write_locked(pmic, sr_reg_add, 0);
+		ret = tps65910_reg_write(pmic->mfd, sr_reg_add, 0);
 		if (ret < 0) {
 			dev_err(mfd->dev, "Error in settting sr register\n");
 			return ret;
@@ -1126,6 +1011,7 @@ static struct tps65910_board *tps65910_parse_dt_reg_data(
 				"ti,regulator-ext-sleep-control", &prop);
 		if (!ret)
 			pmic_plat_data->regulator_ext_sleep_control[idx] = prop;
+
 	}
 
 	return pmic_plat_data;
@@ -1136,7 +1022,7 @@ static inline struct tps65910_board *tps65910_parse_dt_reg_data(
 			struct of_regulator_match **tps65910_reg_matches)
 {
 	*tps65910_reg_matches = NULL;
-	return 0;
+	return NULL;
 }
 #endif
 
@@ -1168,7 +1054,6 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	mutex_init(&pmic->mutex);
 	pmic->mfd = tps65910;
 	platform_set_drvdata(pdev, pmic);
 
@@ -1229,23 +1114,31 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 		pmic->info[i] = info;
 
 		pmic->desc[i].name = info->name;
+		pmic->desc[i].supply_name = info->vin_name;
 		pmic->desc[i].id = i;
 		pmic->desc[i].n_voltages = info->n_voltages;
+		pmic->desc[i].enable_time = info->enable_time_us;
 
 		if (i == TPS65910_REG_VDD1 || i == TPS65910_REG_VDD2) {
 			pmic->desc[i].ops = &tps65910_ops_dcdc;
 			pmic->desc[i].n_voltages = VDD1_2_NUM_VOLT_FINE *
 							VDD1_2_NUM_VOLT_COARSE;
+			pmic->desc[i].ramp_delay = 12500;
 		} else if (i == TPS65910_REG_VDD3) {
-			if (tps65910_chip_id(tps65910) == TPS65910)
+			if (tps65910_chip_id(tps65910) == TPS65910) {
 				pmic->desc[i].ops = &tps65910_ops_vdd3;
-			else
+				pmic->desc[i].volt_table = info->voltage_table;
+			} else {
 				pmic->desc[i].ops = &tps65910_ops_dcdc;
+				pmic->desc[i].ramp_delay = 5000;
+			}
 		} else {
-			if (tps65910_chip_id(tps65910) == TPS65910)
+			if (tps65910_chip_id(tps65910) == TPS65910) {
 				pmic->desc[i].ops = &tps65910_ops;
-			else
+				pmic->desc[i].volt_table = info->voltage_table;
+			} else {
 				pmic->desc[i].ops = &tps65911_ops;
+			}
 		}
 
 		err = tps65910_set_ext_sleep_config(pmic, i,
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index c7390711d95..242fe90dc56 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -43,9 +43,6 @@ struct twlreg_info {
 	u8			table_len;
 	const u16		*table;
 
-	/* regulator specific turn-on delay */
-	u16			delay;
-
 	/* State REMAP default configuration */
 	u8			remap;
 
@@ -223,20 +220,6 @@ static int twl6030reg_enable(struct regulator_dev *rdev)
 	return ret;
 }
 
-static int twl4030reg_enable_time(struct regulator_dev *rdev)
-{
-	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-
-	return info->delay;
-}
-
-static int twl6030reg_enable_time(struct regulator_dev *rdev)
-{
-	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-
-	return info->delay;
-}
-
 static int twl4030reg_disable(struct regulator_dev *rdev)
 {
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
@@ -508,7 +491,6 @@ static struct regulator_ops twl4030ldo_ops = {
 	.enable		= twl4030reg_enable,
 	.disable	= twl4030reg_disable,
 	.is_enabled	= twl4030reg_is_enabled,
-	.enable_time	= twl4030reg_enable_time,
 
 	.set_mode	= twl4030reg_set_mode,
 
@@ -577,59 +559,53 @@ static struct regulator_ops twl6030coresmps_ops = {
 	.get_voltage	= twl6030coresmps_get_voltage,
 };
 
-static int twl6030ldo_list_voltage(struct regulator_dev *rdev, unsigned index)
+static int twl6030ldo_list_voltage(struct regulator_dev *rdev, unsigned sel)
 {
-	struct twlreg_info	*info = rdev_get_drvdata(rdev);
+	struct twlreg_info *info = rdev_get_drvdata(rdev);
 
-	return ((info->min_mV + (index * 100)) * 1000);
+	switch (sel) {
+	case 0:
+		return 0;
+	case 1 ... 24:
+		/* Linear mapping from 00000001 to 00011000:
+		 * Absolute voltage value = 1.0 V + 0.1 V × (sel – 00000001)
+		 */
+		return (info->min_mV + 100 * (sel - 1)) * 1000;
+	case 25 ... 30:
+		return -EINVAL;
+	case 31:
+		return 2750000;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int
-twl6030ldo_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
-		       unsigned *selector)
+twl6030ldo_set_voltage_sel(struct regulator_dev *rdev, unsigned selector)
 {
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-	int			vsel;
-
-	if ((min_uV/1000 < info->min_mV) || (max_uV/1000 > info->max_mV))
-		return -EDOM;
-
-	/*
-	 * Use the below formula to calculate vsel
-	 * mV = 1000mv + 100mv * (vsel - 1)
-	 */
-	vsel = (min_uV/1000 - 1000)/100 + 1;
-	*selector = vsel;
-	return twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_VOLTAGE, vsel);
 
+	return twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_VOLTAGE,
+			    selector);
 }
 
-static int twl6030ldo_get_voltage(struct regulator_dev *rdev)
+static int twl6030ldo_get_voltage_sel(struct regulator_dev *rdev)
 {
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
-	int		vsel = twlreg_read(info, TWL_MODULE_PM_RECEIVER,
-								VREG_VOLTAGE);
-
-	if (vsel < 0)
-		return vsel;
+	int vsel = twlreg_read(info, TWL_MODULE_PM_RECEIVER, VREG_VOLTAGE);
 
-	/*
-	 * Use the below formula to calculate vsel
-	 * mV = 1000mv + 100mv * (vsel - 1)
-	 */
-	return (1000 + (100 * (vsel - 1))) * 1000;
+	return vsel;
 }
 
 static struct regulator_ops twl6030ldo_ops = {
 	.list_voltage	= twl6030ldo_list_voltage,
 
-	.set_voltage	= twl6030ldo_set_voltage,
-	.get_voltage	= twl6030ldo_get_voltage,
+	.set_voltage_sel = twl6030ldo_set_voltage_sel,
+	.get_voltage_sel = twl6030ldo_get_voltage_sel,
 
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
-	.enable_time	= twl6030reg_enable_time,
 
 	.set_mode	= twl6030reg_set_mode,
 
@@ -663,7 +639,6 @@ static struct regulator_ops twl4030fixed_ops = {
 	.enable		= twl4030reg_enable,
 	.disable	= twl4030reg_disable,
 	.is_enabled	= twl4030reg_is_enabled,
-	.enable_time	= twl4030reg_enable_time,
 
 	.set_mode	= twl4030reg_set_mode,
 
@@ -678,7 +653,6 @@ static struct regulator_ops twl6030fixed_ops = {
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
-	.enable_time	= twl6030reg_enable_time,
 
 	.set_mode	= twl6030reg_set_mode,
 
@@ -689,7 +663,6 @@ static struct regulator_ops twl6030_fixed_resource = {
 	.enable		= twl6030reg_enable,
 	.disable	= twl6030reg_disable,
 	.is_enabled	= twl6030reg_is_enabled,
-	.enable_time	= twl6030reg_enable_time,
 	.get_status	= twl6030reg_get_status,
 };
 
@@ -886,7 +859,6 @@ static struct regulator_ops twlsmps_ops = {
 	.enable			= twl6030reg_enable,
 	.disable		= twl6030reg_disable,
 	.is_enabled		= twl6030reg_is_enabled,
-	.enable_time		= twl6030reg_enable_time,
 
 	.set_mode		= twl6030reg_set_mode,
 
@@ -909,7 +881,6 @@ static struct twlreg_info TWL4030_INFO_##label = { \
 	.id = num, \
 	.table_len = ARRAY_SIZE(label##_VSEL_table), \
 	.table = label##_VSEL_table, \
-	.delay = turnon_delay, \
 	.remap = remap_conf, \
 	.desc = { \
 		.name = #label, \
@@ -918,6 +889,7 @@ static struct twlreg_info TWL4030_INFO_##label = { \
 		.ops = &twl4030ldo_ops, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
+		.enable_time = turnon_delay, \
 		}, \
 	}
 
@@ -925,7 +897,6 @@ static struct twlreg_info TWL4030_INFO_##label = { \
 static struct twlreg_info TWL4030_INFO_##label = { \
 	.base = offset, \
 	.id = num, \
-	.delay = turnon_delay, \
 	.remap = remap_conf, \
 	.desc = { \
 		.name = #label, \
@@ -933,6 +904,7 @@ static struct twlreg_info TWL4030_INFO_##label = { \
 		.ops = &twl4030smps_ops, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
+		.enable_time = turnon_delay, \
 		}, \
 	}
 
@@ -955,7 +927,7 @@ static struct twlreg_info TWL6030_INFO_##label = { \
 	.desc = { \
 		.name = #label, \
 		.id = TWL6030_REG_##label, \
-		.n_voltages = (max_mVolts - min_mVolts)/100 + 1, \
+		.n_voltages = 32, \
 		.ops = &twl6030ldo_ops, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
@@ -970,7 +942,7 @@ static struct twlreg_info TWL6025_INFO_##label = { \
 	.desc = { \
 		.name = #label, \
 		.id = TWL6025_REG_##label, \
-		.n_voltages = ((max_mVolts - min_mVolts)/100) + 1, \
+		.n_voltages = 32, \
 		.ops = &twl6030ldo_ops, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
@@ -983,7 +955,6 @@ static struct twlreg_info TWLFIXED_INFO_##label = { \
 	.base = offset, \
 	.id = num, \
 	.min_mV = mVolts, \
-	.delay = turnon_delay, \
 	.remap = remap_conf, \
 	.desc = { \
 		.name = #label, \
@@ -992,19 +963,20 @@ static struct twlreg_info TWLFIXED_INFO_##label = { \
 		.ops = &operations, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
+		.enable_time = turnon_delay, \
 		}, \
 	}
 
 #define TWL6030_FIXED_RESOURCE(label, offset, turnon_delay) \
 static struct twlreg_info TWLRES_INFO_##label = { \
 	.base = offset, \
-	.delay = turnon_delay, \
 	.desc = { \
 		.name = #label, \
 		.id = TWL6030_REG_##label, \
 		.ops = &twl6030_fixed_resource, \
 		.type = REGULATOR_VOLTAGE, \
 		.owner = THIS_MODULE, \
+		.enable_time = turnon_delay, \
 		}, \
 	}
 
@@ -1109,7 +1081,6 @@ static u8 twl_get_smps_mult(void)
 #define TWL6030_OF_MATCH(comp, label) TWL_OF_MATCH(comp, TWL6030, label)
 #define TWL6025_OF_MATCH(comp, label) TWL_OF_MATCH(comp, TWL6025, label)
 #define TWLFIXED_OF_MATCH(comp, label) TWL_OF_MATCH(comp, TWLFIXED, label)
-#define TWLRES_OF_MATCH(comp, label) TWL_OF_MATCH(comp, TWLRES, label)
 #define TWLSMPS_OF_MATCH(comp, label) TWL_OF_MATCH(comp, TWLSMPS, label)
 
 static const struct of_device_id twl_of_match[] __devinitconst = {
@@ -1157,7 +1128,6 @@ static const struct of_device_id twl_of_match[] __devinitconst = {
 	TWLFIXED_OF_MATCH("ti,twl6030-vusb", VUSB),
 	TWLFIXED_OF_MATCH("ti,twl6030-v1v8", V1V8),
 	TWLFIXED_OF_MATCH("ti,twl6030-v2v1", V2V1),
-	TWLRES_OF_MATCH("ti,twl6030-clk32kg", CLK32KG),
 	TWLSMPS_OF_MATCH("ti,twl6025-smps3", SMPS3),
 	TWLSMPS_OF_MATCH("ti,twl6025-smps4", SMPS4),
 	TWLSMPS_OF_MATCH("ti,twl6025-vio", VIO),
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index 099da11e989..7413885be01 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -215,8 +215,8 @@ static int wm831x_buckv_list_voltage(struct regulator_dev *rdev,
 	return -EINVAL;
 }
 
-static int wm831x_buckv_select_min_voltage(struct regulator_dev *rdev,
-					   int min_uV, int max_uV)
+static int wm831x_buckv_map_voltage(struct regulator_dev *rdev,
+				   int min_uV, int max_uV)
 {
 	u16 vsel;
 
@@ -251,20 +251,14 @@ static int wm831x_buckv_set_dvs(struct regulator_dev *rdev, int state)
 	return 0;
 }
 
-static int wm831x_buckv_set_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV, unsigned *selector)
+static int wm831x_buckv_set_voltage_sel(struct regulator_dev *rdev,
+					unsigned vsel)
 {
 	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
 	struct wm831x *wm831x = dcdc->wm831x;
 	int on_reg = dcdc->base + WM831X_DCDC_ON_CONFIG;
 	int dvs_reg = dcdc->base + WM831X_DCDC_DVS_CONTROL;
-	int vsel, ret;
-
-	vsel = wm831x_buckv_select_min_voltage(rdev, min_uV, max_uV);
-	if (vsel < 0)
-		return vsel;
-
-	*selector = vsel;
+	int ret;
 
 	/* If this value is already set then do a GPIO update if we can */
 	if (dcdc->dvs_gpio && dcdc->on_vsel == vsel)
@@ -315,7 +309,7 @@ static int wm831x_buckv_set_suspend_voltage(struct regulator_dev *rdev,
 	u16 reg = dcdc->base + WM831X_DCDC_SLEEP_CONTROL;
 	int vsel;
 
-	vsel = wm831x_buckv_select_min_voltage(rdev, uV, uV);
+	vsel = wm831x_buckv_map_voltage(rdev, uV, uV);
 	if (vsel < 0)
 		return vsel;
 
@@ -373,9 +367,10 @@ static int wm831x_buckv_get_current_limit(struct regulator_dev *rdev)
 }
 
 static struct regulator_ops wm831x_buckv_ops = {
-	.set_voltage = wm831x_buckv_set_voltage,
+	.set_voltage_sel = wm831x_buckv_set_voltage_sel,
 	.get_voltage_sel = wm831x_buckv_get_voltage_sel,
 	.list_voltage = wm831x_buckv_list_voltage,
+	.map_voltage = wm831x_buckv_map_voltage,
 	.set_suspend_voltage = wm831x_buckv_set_suspend_voltage,
 	.set_current_limit = wm831x_buckv_set_current_limit,
 	.get_current_limit = wm831x_buckv_get_current_limit,
@@ -599,60 +594,25 @@ static struct platform_driver wm831x_buckv_driver = {
  * BUCKP specifics
  */
 
-static int wm831x_buckp_list_voltage(struct regulator_dev *rdev,
-				      unsigned selector)
-{
-	if (selector <= WM831X_BUCKP_MAX_SELECTOR)
-		return 850000 + (selector * 25000);
-	else
-		return -EINVAL;
-}
-
-static int wm831x_buckp_set_voltage_int(struct regulator_dev *rdev, int reg,
-					int min_uV, int max_uV, int *selector)
+static int wm831x_buckp_set_suspend_voltage(struct regulator_dev *rdev, int uV)
 {
 	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
 	struct wm831x *wm831x = dcdc->wm831x;
-	u16 vsel;
-
-	if (min_uV <= 34000000)
-		vsel = (min_uV - 850000) / 25000;
-	else
-		return -EINVAL;
-
-	if (wm831x_buckp_list_voltage(rdev, vsel) > max_uV)
-		return -EINVAL;
-
-	*selector = vsel;
-
-	return wm831x_set_bits(wm831x, reg, WM831X_DC3_ON_VSEL_MASK, vsel);
-}
-
-static int wm831x_buckp_set_voltage(struct regulator_dev *rdev,
-				    int min_uV, int max_uV,
-				    unsigned *selector)
-{
-	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
-	u16 reg = dcdc->base + WM831X_DCDC_ON_CONFIG;
-
-	return wm831x_buckp_set_voltage_int(rdev, reg, min_uV, max_uV,
-					    selector);
-}
-
-static int wm831x_buckp_set_suspend_voltage(struct regulator_dev *rdev,
-					    int uV)
-{
-	struct wm831x_dcdc *dcdc = rdev_get_drvdata(rdev);
 	u16 reg = dcdc->base + WM831X_DCDC_SLEEP_CONTROL;
-	unsigned selector;
+	int sel;
+
+	sel = regulator_map_voltage_linear(rdev, uV, uV);
+	if (sel < 0)
+		return sel;
 
-	return wm831x_buckp_set_voltage_int(rdev, reg, uV, uV, &selector);
+	return wm831x_set_bits(wm831x, reg, WM831X_DC3_ON_VSEL_MASK, sel);
 }
 
 static struct regulator_ops wm831x_buckp_ops = {
-	.set_voltage = wm831x_buckp_set_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
-	.list_voltage = wm831x_buckp_list_voltage,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
 	.set_suspend_voltage = wm831x_buckp_set_suspend_voltage,
 
 	.is_enabled = regulator_is_enabled_regmap,
@@ -715,6 +675,8 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 	dcdc->desc.vsel_mask = WM831X_DC3_ON_VSEL_MASK;
 	dcdc->desc.enable_reg = WM831X_DCDC_ENABLE;
 	dcdc->desc.enable_mask = 1 << id;
+	dcdc->desc.min_uV = 850000;
+	dcdc->desc.uV_step = 25000;
 
 	config.dev = pdev->dev.parent;
 	if (pdata)
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index a9a28d8ac18..5cb70ca1e98 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -78,13 +78,10 @@ static int wm831x_gp_ldo_list_voltage(struct regulator_dev *rdev,
 	return -EINVAL;
 }
 
-static int wm831x_gp_ldo_set_voltage_int(struct regulator_dev *rdev, int reg,
-					 int min_uV, int max_uV,
-					 unsigned *selector)
+static int wm831x_gp_ldo_map_voltage(struct regulator_dev *rdev,
+				     int min_uV, int max_uV)
 {
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int vsel, ret;
+	int volt, vsel;
 
 	if (min_uV < 900000)
 		vsel = 0;
@@ -94,36 +91,25 @@ static int wm831x_gp_ldo_set_voltage_int(struct regulator_dev *rdev, int reg,
 		vsel = ((min_uV - 1700000) / 100000)
 			+ WM831X_GP_LDO_SELECTOR_LOW + 1;
 
-	ret = wm831x_gp_ldo_list_voltage(rdev, vsel);
-	if (ret < 0)
-		return ret;
-	if (ret < min_uV || ret > max_uV)
+	volt = wm831x_gp_ldo_list_voltage(rdev, vsel);
+	if (volt < min_uV || volt > max_uV)
 		return -EINVAL;
 
-	*selector = vsel;
-
-	return wm831x_set_bits(wm831x, reg, WM831X_LDO1_ON_VSEL_MASK, vsel);
-}
-
-static int wm831x_gp_ldo_set_voltage(struct regulator_dev *rdev,
-				     int min_uV, int max_uV,
-				     unsigned *selector)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	int reg = ldo->base + WM831X_LDO_ON_CONTROL;
-
-	return wm831x_gp_ldo_set_voltage_int(rdev, reg, min_uV, max_uV,
-					     selector);
+	return vsel;
 }
 
 static int wm831x_gp_ldo_set_suspend_voltage(struct regulator_dev *rdev,
 					     int uV)
 {
 	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	int reg = ldo->base + WM831X_LDO_SLEEP_CONTROL;
-	unsigned int selector;
+	struct wm831x *wm831x = ldo->wm831x;
+	int sel, reg = ldo->base + WM831X_LDO_SLEEP_CONTROL;
 
-	return wm831x_gp_ldo_set_voltage_int(rdev, reg, uV, uV, &selector);
+	sel = wm831x_gp_ldo_map_voltage(rdev, uV, uV);
+	if (sel < 0)
+		return sel;
+
+	return wm831x_set_bits(wm831x, reg, WM831X_LDO1_ON_VSEL_MASK, sel);
 }
 
 static unsigned int wm831x_gp_ldo_get_mode(struct regulator_dev *rdev)
@@ -243,8 +229,9 @@ static unsigned int wm831x_gp_ldo_get_optimum_mode(struct regulator_dev *rdev,
 
 static struct regulator_ops wm831x_gp_ldo_ops = {
 	.list_voltage = wm831x_gp_ldo_list_voltage,
+	.map_voltage = wm831x_gp_ldo_map_voltage,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
-	.set_voltage = wm831x_gp_ldo_set_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.set_suspend_voltage = wm831x_gp_ldo_set_suspend_voltage,
 	.get_mode = wm831x_gp_ldo_get_mode,
 	.set_mode = wm831x_gp_ldo_set_mode,
@@ -384,13 +371,10 @@ static int wm831x_aldo_list_voltage(struct regulator_dev *rdev,
 	return -EINVAL;
 }
 
-static int wm831x_aldo_set_voltage_int(struct regulator_dev *rdev, int reg,
-				       int min_uV, int max_uV,
-				       unsigned *selector)
+static int wm831x_aldo_map_voltage(struct regulator_dev *rdev,
+				   int min_uV, int max_uV)
 {
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int vsel, ret;
+	int volt, vsel;
 
 	if (min_uV < 1000000)
 		vsel = 0;
@@ -400,35 +384,26 @@ static int wm831x_aldo_set_voltage_int(struct regulator_dev *rdev, int reg,
 		vsel = ((min_uV - 1700000) / 100000)
 			+ WM831X_ALDO_SELECTOR_LOW + 1;
 
-	ret = wm831x_aldo_list_voltage(rdev, vsel);
-	if (ret < 0)
-		return ret;
-	if (ret < min_uV || ret > max_uV)
+	volt = wm831x_aldo_list_voltage(rdev, vsel);
+	if (volt < min_uV || volt > max_uV)
 		return -EINVAL;
 
-	*selector = vsel;
-
-	return wm831x_set_bits(wm831x, reg, WM831X_LDO7_ON_VSEL_MASK, vsel);
-}
-
-static int wm831x_aldo_set_voltage(struct regulator_dev *rdev,
-				   int min_uV, int max_uV, unsigned *selector)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	int reg = ldo->base + WM831X_LDO_ON_CONTROL;
+	return vsel;
 
-	return wm831x_aldo_set_voltage_int(rdev, reg, min_uV, max_uV,
-					   selector);
 }
 
 static int wm831x_aldo_set_suspend_voltage(struct regulator_dev *rdev,
 					     int uV)
 {
 	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	int reg = ldo->base + WM831X_LDO_SLEEP_CONTROL;
-	unsigned int selector;
+	struct wm831x *wm831x = ldo->wm831x;
+	int sel, reg = ldo->base + WM831X_LDO_SLEEP_CONTROL;
+
+	sel = wm831x_aldo_map_voltage(rdev, uV, uV);
+	if (sel < 0)
+		return sel;
 
-	return wm831x_aldo_set_voltage_int(rdev, reg, uV, uV, &selector);
+	return wm831x_set_bits(wm831x, reg, WM831X_LDO7_ON_VSEL_MASK, sel);
 }
 
 static unsigned int wm831x_aldo_get_mode(struct regulator_dev *rdev)
@@ -506,8 +481,9 @@ static int wm831x_aldo_get_status(struct regulator_dev *rdev)
 
 static struct regulator_ops wm831x_aldo_ops = {
 	.list_voltage = wm831x_aldo_list_voltage,
+	.map_voltage = wm831x_aldo_map_voltage,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
-	.set_voltage = wm831x_aldo_set_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.set_suspend_voltage = wm831x_aldo_set_suspend_voltage,
 	.get_mode = wm831x_aldo_get_mode,
 	.set_mode = wm831x_aldo_set_mode,
@@ -628,47 +604,18 @@ static struct platform_driver wm831x_aldo_driver = {
 
 #define WM831X_ALIVE_LDO_MAX_SELECTOR 0xf
 
-static int wm831x_alive_ldo_set_voltage_int(struct regulator_dev *rdev,
-					    int reg,
-					    int min_uV, int max_uV,
-					    unsigned *selector)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	struct wm831x *wm831x = ldo->wm831x;
-	int vsel, ret;
-
-	vsel = (min_uV - 800000) / 50000;
-
-	ret = regulator_list_voltage_linear(rdev, vsel);
-	if (ret < 0)
-		return ret;
-	if (ret < min_uV || ret > max_uV)
-		return -EINVAL;
-
-	*selector = vsel;
-
-	return wm831x_set_bits(wm831x, reg, WM831X_LDO11_ON_VSEL_MASK, vsel);
-}
-
-static int wm831x_alive_ldo_set_voltage(struct regulator_dev *rdev,
-					int min_uV, int max_uV,
-					unsigned *selector)
-{
-	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	int reg = ldo->base + WM831X_ALIVE_LDO_ON_CONTROL;
-
-	return wm831x_alive_ldo_set_voltage_int(rdev, reg, min_uV, max_uV,
-						selector);
-}
-
 static int wm831x_alive_ldo_set_suspend_voltage(struct regulator_dev *rdev,
 					     int uV)
 {
 	struct wm831x_ldo *ldo = rdev_get_drvdata(rdev);
-	int reg = ldo->base + WM831X_ALIVE_LDO_SLEEP_CONTROL;
-	unsigned selector;
+	struct wm831x *wm831x = ldo->wm831x;
+	int sel, reg = ldo->base + WM831X_ALIVE_LDO_SLEEP_CONTROL;
+
+	sel = regulator_map_voltage_linear(rdev, uV, uV);
+	if (sel < 0)
+		return sel;
 
-	return wm831x_alive_ldo_set_voltage_int(rdev, reg, uV, uV, &selector);
+	return wm831x_set_bits(wm831x, reg, WM831X_LDO11_ON_VSEL_MASK, sel);
 }
 
 static int wm831x_alive_ldo_get_status(struct regulator_dev *rdev)
@@ -690,8 +637,9 @@ static int wm831x_alive_ldo_get_status(struct regulator_dev *rdev)
 
 static struct regulator_ops wm831x_alive_ldo_ops = {
 	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
-	.set_voltage = wm831x_alive_ldo_set_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.set_suspend_voltage = wm831x_alive_ldo_set_suspend_voltage,
 	.get_status = wm831x_alive_ldo_get_status,
 
@@ -753,6 +701,7 @@ static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 	ldo->desc.enable_mask = 1 << id;
 	ldo->desc.min_uV = 800000;
 	ldo->desc.uV_step = 50000;
+	ldo->desc.enable_time = 1000;
 
 	config.dev = pdev->dev.parent;
 	if (pdata)
diff --git a/drivers/regulator/wm8350-regulator.c b/drivers/regulator/wm8350-regulator.c
index 94e550dc70b..7f0fa22ef2a 100644
--- a/drivers/regulator/wm8350-regulator.c
+++ b/drivers/regulator/wm8350-regulator.c
@@ -108,33 +108,6 @@ static int get_isink_val(int min_uA, int max_uA, u16 *setting)
 	return -EINVAL;
 }
 
-static inline int wm8350_ldo_val_to_mvolts(unsigned int val)
-{
-	if (val < 16)
-		return (val * 50) + 900;
-	else
-		return ((val - 16) * 100) + 1800;
-
-}
-
-static inline unsigned int wm8350_ldo_mvolts_to_val(int mV)
-{
-	if (mV < 1800)
-		return (mV - 900) / 50;
-	else
-		return ((mV - 1800) / 100) + 16;
-}
-
-static inline int wm8350_dcdc_val_to_mvolts(unsigned int val)
-{
-	return (val * 25) + 850;
-}
-
-static inline unsigned int wm8350_dcdc_mvolts_to_val(int mV)
-{
-	return (mV - 850) / 25;
-}
-
 static int wm8350_isink_set_current(struct regulator_dev *rdev, int min_uA,
 	int max_uA)
 {
@@ -359,104 +332,13 @@ int wm8350_isink_set_flash(struct wm8350 *wm8350, int isink, u16 mode,
 }
 EXPORT_SYMBOL_GPL(wm8350_isink_set_flash);
 
-static int wm8350_dcdc_set_voltage(struct regulator_dev *rdev, int min_uV,
-				   int max_uV, unsigned *selector)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int volt_reg, dcdc = rdev_get_id(rdev), mV,
-		min_mV = min_uV / 1000, max_mV = max_uV / 1000;
-	u16 val;
-
-	if (min_mV < 850 || min_mV > 4025)
-		return -EINVAL;
-	if (max_mV < 850 || max_mV > 4025)
-		return -EINVAL;
-
-	/* step size is 25mV */
-	mV = (min_mV - 826) / 25;
-	if (wm8350_dcdc_val_to_mvolts(mV) > max_mV)
-		return -EINVAL;
-	BUG_ON(wm8350_dcdc_val_to_mvolts(mV) < min_mV);
-
-	switch (dcdc) {
-	case WM8350_DCDC_1:
-		volt_reg = WM8350_DCDC1_CONTROL;
-		break;
-	case WM8350_DCDC_3:
-		volt_reg = WM8350_DCDC3_CONTROL;
-		break;
-	case WM8350_DCDC_4:
-		volt_reg = WM8350_DCDC4_CONTROL;
-		break;
-	case WM8350_DCDC_6:
-		volt_reg = WM8350_DCDC6_CONTROL;
-		break;
-	case WM8350_DCDC_2:
-	case WM8350_DCDC_5:
-	default:
-		return -EINVAL;
-	}
-
-	*selector = mV;
-
-	/* all DCDCs have same mV bits */
-	val = wm8350_reg_read(wm8350, volt_reg) & ~WM8350_DC1_VSEL_MASK;
-	wm8350_reg_write(wm8350, volt_reg, val | mV);
-	return 0;
-}
-
-static int wm8350_dcdc_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int volt_reg, dcdc = rdev_get_id(rdev);
-
-	switch (dcdc) {
-	case WM8350_DCDC_1:
-		volt_reg = WM8350_DCDC1_CONTROL;
-		break;
-	case WM8350_DCDC_3:
-		volt_reg = WM8350_DCDC3_CONTROL;
-		break;
-	case WM8350_DCDC_4:
-		volt_reg = WM8350_DCDC4_CONTROL;
-		break;
-	case WM8350_DCDC_6:
-		volt_reg = WM8350_DCDC6_CONTROL;
-		break;
-	case WM8350_DCDC_2:
-	case WM8350_DCDC_5:
-	default:
-		return -EINVAL;
-	}
-
-	/* all DCDCs have same mV bits */
-	return wm8350_reg_read(wm8350, volt_reg) & WM8350_DC1_VSEL_MASK;
-}
-
-static int wm8350_dcdc_list_voltage(struct regulator_dev *rdev,
-				    unsigned selector)
-{
-	if (selector > WM8350_DCDC_MAX_VSEL)
-		return -EINVAL;
-	return wm8350_dcdc_val_to_mvolts(selector) * 1000;
-}
-
 static int wm8350_dcdc_set_suspend_voltage(struct regulator_dev *rdev, int uV)
 {
 	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int volt_reg, mV = uV / 1000, dcdc = rdev_get_id(rdev);
+	int sel, volt_reg, dcdc = rdev_get_id(rdev);
 	u16 val;
 
-	dev_dbg(wm8350->dev, "%s %d mV %d\n", __func__, dcdc, mV);
-
-	if (mV && (mV < 850 || mV > 4025)) {
-		dev_err(wm8350->dev,
-			"DCDC%d suspend voltage %d mV out of range\n",
-			dcdc, mV);
-		return -EINVAL;
-	}
-	if (mV == 0)
-		mV = 850;
+	dev_dbg(wm8350->dev, "%s %d mV %d\n", __func__, dcdc, uV / 1000);
 
 	switch (dcdc) {
 	case WM8350_DCDC_1:
@@ -477,10 +359,13 @@ static int wm8350_dcdc_set_suspend_voltage(struct regulator_dev *rdev, int uV)
 		return -EINVAL;
 	}
 
+	sel = regulator_map_voltage_linear(rdev, uV, uV);
+	if (sel < 0)
+		return -EINVAL;
+
 	/* all DCDCs have same mV bits */
 	val = wm8350_reg_read(wm8350, volt_reg) & ~WM8350_DC1_VSEL_MASK;
-	wm8350_reg_write(wm8350, volt_reg,
-			 val | wm8350_dcdc_mvolts_to_val(mV));
+	wm8350_reg_write(wm8350, volt_reg, val | sel);
 	return 0;
 }
 
@@ -657,19 +542,49 @@ static int wm8350_dcdc_set_suspend_mode(struct regulator_dev *rdev,
 	return 0;
 }
 
+static int wm8350_ldo_list_voltage(struct regulator_dev *rdev,
+				    unsigned selector)
+{
+	if (selector > WM8350_LDO1_VSEL_MASK)
+		return -EINVAL;
+
+	if (selector < 16)
+		return (selector * 50000) + 900000;
+	else
+		return ((selector - 16) * 100000) + 1800000;
+}
+
+static int wm8350_ldo_map_voltage(struct regulator_dev *rdev, int min_uV,
+				  int max_uV)
+{
+	int volt, sel;
+	int min_mV = min_uV / 1000;
+	int max_mV = max_uV / 1000;
+
+	if (min_mV < 900 || min_mV > 3300)
+		return -EINVAL;
+	if (max_mV < 900 || max_mV > 3300)
+		return -EINVAL;
+
+	if (min_mV < 1800) /* step size is 50mV < 1800mV */
+		sel = DIV_ROUND_UP(min_uV - 900, 50);
+	else /* step size is 100mV > 1800mV */
+		sel = DIV_ROUND_UP(min_uV - 1800, 100) + 16;
+
+	volt = wm8350_ldo_list_voltage(rdev, sel);
+	if (volt < min_uV || volt > max_uV)
+		return -EINVAL;
+
+	return sel;
+}
+
 static int wm8350_ldo_set_suspend_voltage(struct regulator_dev *rdev, int uV)
 {
 	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int volt_reg, mV = uV / 1000, ldo = rdev_get_id(rdev);
+	int sel, volt_reg, ldo = rdev_get_id(rdev);
 	u16 val;
 
-	dev_dbg(wm8350->dev, "%s %d mV %d\n", __func__, ldo, mV);
-
-	if (mV < 900 || mV > 3300) {
-		dev_err(wm8350->dev, "LDO%d voltage %d mV out of range\n",
-			ldo, mV);
-		return -EINVAL;
-	}
+	dev_dbg(wm8350->dev, "%s %d mV %d\n", __func__, ldo, uV / 1000);
 
 	switch (ldo) {
 	case WM8350_LDO_1:
@@ -688,10 +603,13 @@ static int wm8350_ldo_set_suspend_voltage(struct regulator_dev *rdev, int uV)
 		return -EINVAL;
 	}
 
+	sel = wm8350_ldo_map_voltage(rdev, uV, uV);
+	if (sel < 0)
+		return -EINVAL;
+
 	/* all LDOs have same mV bits */
 	val = wm8350_reg_read(wm8350, volt_reg) & ~WM8350_LDO1_VSEL_MASK;
-	wm8350_reg_write(wm8350, volt_reg,
-			 val | wm8350_ldo_mvolts_to_val(mV));
+	wm8350_reg_write(wm8350, volt_reg, val | sel);
 	return 0;
 }
 
@@ -753,92 +671,6 @@ static int wm8350_ldo_set_suspend_disable(struct regulator_dev *rdev)
 	return 0;
 }
 
-static int wm8350_ldo_set_voltage(struct regulator_dev *rdev, int min_uV,
-				  int max_uV, unsigned *selector)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int volt_reg, ldo = rdev_get_id(rdev), mV, min_mV = min_uV / 1000,
-		max_mV = max_uV / 1000;
-	u16 val;
-
-	if (min_mV < 900 || min_mV > 3300)
-		return -EINVAL;
-	if (max_mV < 900 || max_mV > 3300)
-		return -EINVAL;
-
-	if (min_mV < 1800) {
-		/* step size is 50mV < 1800mV */
-		mV = (min_mV - 851) / 50;
-		if (wm8350_ldo_val_to_mvolts(mV) > max_mV)
-			return -EINVAL;
-		BUG_ON(wm8350_ldo_val_to_mvolts(mV) < min_mV);
-	} else {
-		/* step size is 100mV > 1800mV */
-		mV = ((min_mV - 1701) / 100) + 16;
-		if (wm8350_ldo_val_to_mvolts(mV) > max_mV)
-			return -EINVAL;
-		BUG_ON(wm8350_ldo_val_to_mvolts(mV) < min_mV);
-	}
-
-	switch (ldo) {
-	case WM8350_LDO_1:
-		volt_reg = WM8350_LDO1_CONTROL;
-		break;
-	case WM8350_LDO_2:
-		volt_reg = WM8350_LDO2_CONTROL;
-		break;
-	case WM8350_LDO_3:
-		volt_reg = WM8350_LDO3_CONTROL;
-		break;
-	case WM8350_LDO_4:
-		volt_reg = WM8350_LDO4_CONTROL;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	*selector = mV;
-
-	/* all LDOs have same mV bits */
-	val = wm8350_reg_read(wm8350, volt_reg) & ~WM8350_LDO1_VSEL_MASK;
-	wm8350_reg_write(wm8350, volt_reg, val | mV);
-	return 0;
-}
-
-static int wm8350_ldo_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int volt_reg, ldo = rdev_get_id(rdev);
-
-	switch (ldo) {
-	case WM8350_LDO_1:
-		volt_reg = WM8350_LDO1_CONTROL;
-		break;
-	case WM8350_LDO_2:
-		volt_reg = WM8350_LDO2_CONTROL;
-		break;
-	case WM8350_LDO_3:
-		volt_reg = WM8350_LDO3_CONTROL;
-		break;
-	case WM8350_LDO_4:
-		volt_reg = WM8350_LDO4_CONTROL;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	/* all LDOs have same mV bits */
-	return wm8350_reg_read(wm8350, volt_reg) & WM8350_LDO1_VSEL_MASK;
-}
-
-static int wm8350_ldo_list_voltage(struct regulator_dev *rdev,
-				    unsigned selector)
-{
-	if (selector > WM8350_LDO1_VSEL_MASK)
-		return -EINVAL;
-	return wm8350_ldo_val_to_mvolts(selector) * 1000;
-}
-
 int wm8350_dcdc_set_slot(struct wm8350 *wm8350, int dcdc, u16 start,
 			 u16 stop, u16 fault)
 {
@@ -959,63 +791,6 @@ int wm8350_dcdc25_set_mode(struct wm8350 *wm8350, int dcdc, u16 mode,
 }
 EXPORT_SYMBOL_GPL(wm8350_dcdc25_set_mode);
 
-static int wm8350_dcdc_enable(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int dcdc = rdev_get_id(rdev);
-	u16 shift;
-
-	if (dcdc < WM8350_DCDC_1 || dcdc > WM8350_DCDC_6)
-		return -EINVAL;
-
-	shift = dcdc - WM8350_DCDC_1;
-	wm8350_set_bits(wm8350, WM8350_DCDC_LDO_REQUESTED, 1 << shift);
-	return 0;
-}
-
-static int wm8350_dcdc_disable(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int dcdc = rdev_get_id(rdev);
-	u16 shift;
-
-	if (dcdc < WM8350_DCDC_1 || dcdc > WM8350_DCDC_6)
-		return -EINVAL;
-
-	shift = dcdc - WM8350_DCDC_1;
-	wm8350_clear_bits(wm8350, WM8350_DCDC_LDO_REQUESTED, 1 << shift);
-
-	return 0;
-}
-
-static int wm8350_ldo_enable(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int ldo = rdev_get_id(rdev);
-	u16 shift;
-
-	if (ldo < WM8350_LDO_1 || ldo > WM8350_LDO_4)
-		return -EINVAL;
-
-	shift = (ldo - WM8350_LDO_1) + 8;
-	wm8350_set_bits(wm8350, WM8350_DCDC_LDO_REQUESTED, 1 << shift);
-	return 0;
-}
-
-static int wm8350_ldo_disable(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int ldo = rdev_get_id(rdev);
-	u16 shift;
-
-	if (ldo < WM8350_LDO_1 || ldo > WM8350_LDO_4)
-		return -EINVAL;
-
-	shift = (ldo - WM8350_LDO_1) + 8;
-	wm8350_clear_bits(wm8350, WM8350_DCDC_LDO_REQUESTED, 1 << shift);
-	return 0;
-}
-
 static int force_continuous_enable(struct wm8350 *wm8350, int dcdc, int enable)
 {
 	int reg = 0, ret;
@@ -1197,42 +972,17 @@ static unsigned int wm8350_dcdc_get_optimum_mode(struct regulator_dev *rdev,
 	return mode;
 }
 
-static int wm8350_dcdc_is_enabled(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int dcdc = rdev_get_id(rdev), shift;
-
-	if (dcdc < WM8350_DCDC_1 || dcdc > WM8350_DCDC_6)
-		return -EINVAL;
-
-	shift = dcdc - WM8350_DCDC_1;
-	return wm8350_reg_read(wm8350, WM8350_DCDC_LDO_REQUESTED)
-	    & (1 << shift);
-}
-
-static int wm8350_ldo_is_enabled(struct regulator_dev *rdev)
-{
-	struct wm8350 *wm8350 = rdev_get_drvdata(rdev);
-	int ldo = rdev_get_id(rdev), shift;
-
-	if (ldo < WM8350_LDO_1 || ldo > WM8350_LDO_4)
-		return -EINVAL;
-
-	shift = (ldo - WM8350_LDO_1) + 8;
-	return wm8350_reg_read(wm8350, WM8350_DCDC_LDO_REQUESTED)
-	    & (1 << shift);
-}
-
 static struct regulator_ops wm8350_dcdc_ops = {
-	.set_voltage = wm8350_dcdc_set_voltage,
-	.get_voltage_sel = wm8350_dcdc_get_voltage_sel,
-	.list_voltage = wm8350_dcdc_list_voltage,
-	.enable = wm8350_dcdc_enable,
-	.disable = wm8350_dcdc_disable,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.get_mode = wm8350_dcdc_get_mode,
 	.set_mode = wm8350_dcdc_set_mode,
 	.get_optimum_mode = wm8350_dcdc_get_optimum_mode,
-	.is_enabled = wm8350_dcdc_is_enabled,
 	.set_suspend_voltage = wm8350_dcdc_set_suspend_voltage,
 	.set_suspend_enable = wm8350_dcdc_set_suspend_enable,
 	.set_suspend_disable = wm8350_dcdc_set_suspend_disable,
@@ -1240,20 +990,21 @@ static struct regulator_ops wm8350_dcdc_ops = {
 };
 
 static struct regulator_ops wm8350_dcdc2_5_ops = {
-	.enable = wm8350_dcdc_enable,
-	.disable = wm8350_dcdc_disable,
-	.is_enabled = wm8350_dcdc_is_enabled,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.set_suspend_enable = wm8350_dcdc25_set_suspend_enable,
 	.set_suspend_disable = wm8350_dcdc25_set_suspend_disable,
 };
 
 static struct regulator_ops wm8350_ldo_ops = {
-	.set_voltage = wm8350_ldo_set_voltage,
-	.get_voltage_sel = wm8350_ldo_get_voltage_sel,
+	.map_voltage = wm8350_ldo_map_voltage,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage = wm8350_ldo_list_voltage,
-	.enable = wm8350_ldo_enable,
-	.disable = wm8350_ldo_disable,
-	.is_enabled = wm8350_ldo_is_enabled,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.get_mode = wm8350_ldo_get_mode,
 	.set_suspend_voltage = wm8350_ldo_set_suspend_voltage,
 	.set_suspend_enable = wm8350_ldo_set_suspend_enable,
@@ -1277,6 +1028,12 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_DC1,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_DCDC_MAX_VSEL + 1,
+		.min_uV = 850000,
+		.uV_step = 25000,
+		.vsel_reg = WM8350_DCDC1_CONTROL,
+		.vsel_mask = WM8350_DC1_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_DC1_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1285,6 +1042,8 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.ops = &wm8350_dcdc2_5_ops,
 		.irq = WM8350_IRQ_UV_DC2,
 		.type = REGULATOR_VOLTAGE,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_DC2_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1294,6 +1053,12 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_DC3,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_DCDC_MAX_VSEL + 1,
+		.min_uV = 850000,
+		.uV_step = 25000,
+		.vsel_reg = WM8350_DCDC3_CONTROL,
+		.vsel_mask = WM8350_DC3_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_DC3_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1303,6 +1068,12 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_DC4,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_DCDC_MAX_VSEL + 1,
+		.min_uV = 850000,
+		.uV_step = 25000,
+		.vsel_reg = WM8350_DCDC4_CONTROL,
+		.vsel_mask = WM8350_DC4_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_DC4_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1311,6 +1082,8 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.ops = &wm8350_dcdc2_5_ops,
 		.irq = WM8350_IRQ_UV_DC5,
 		.type = REGULATOR_VOLTAGE,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_DC5_ENA,
 		.owner = THIS_MODULE,
 	 },
 	{
@@ -1320,6 +1093,12 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_DC6,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_DCDC_MAX_VSEL + 1,
+		.min_uV = 850000,
+		.uV_step = 25000,
+		.vsel_reg = WM8350_DCDC6_CONTROL,
+		.vsel_mask = WM8350_DC6_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_DC6_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1329,6 +1108,10 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_LDO1,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_LDO1_VSEL_MASK + 1,
+		.vsel_reg = WM8350_LDO1_CONTROL,
+		.vsel_mask = WM8350_LDO1_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_LDO1_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1338,6 +1121,10 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_LDO2,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_LDO2_VSEL_MASK + 1,
+		.vsel_reg = WM8350_LDO2_CONTROL,
+		.vsel_mask = WM8350_LDO2_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_LDO2_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1347,6 +1134,10 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_LDO3,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_LDO3_VSEL_MASK + 1,
+		.vsel_reg = WM8350_LDO3_CONTROL,
+		.vsel_mask = WM8350_LDO3_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_LDO3_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1356,6 +1147,10 @@ static const struct regulator_desc wm8350_reg[NUM_WM8350_REGULATORS] = {
 		.irq = WM8350_IRQ_UV_LDO4,
 		.type = REGULATOR_VOLTAGE,
 		.n_voltages = WM8350_LDO4_VSEL_MASK + 1,
+		.vsel_reg = WM8350_LDO4_CONTROL,
+		.vsel_mask = WM8350_LDO4_VSEL_MASK,
+		.enable_reg = WM8350_DCDC_LDO_REQUESTED,
+		.enable_mask = WM8350_LDO4_ENA,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -1429,6 +1224,7 @@ static int wm8350_regulator_probe(struct platform_device *pdev)
 	config.dev = &pdev->dev;
 	config.init_data = pdev->dev.platform_data;
 	config.driver_data = dev_get_drvdata(&pdev->dev);
+	config.regmap = wm8350->regmap;
 
 	/* register regulator */
 	rdev = regulator_register(&wm8350_reg[pdev->id], &config);
diff --git a/drivers/regulator/wm8400-regulator.c b/drivers/regulator/wm8400-regulator.c
index 69a2b7ce5e4..9035dd05361 100644
--- a/drivers/regulator/wm8400-regulator.c
+++ b/drivers/regulator/wm8400-regulator.c
@@ -28,34 +28,26 @@ static int wm8400_ldo_list_voltage(struct regulator_dev *dev,
 	if (selector < 15)
 		return 900000 + (selector * 50000);
 	else
-		return 1600000 + ((selector - 14) * 100000);
+		return 1700000 + ((selector - 15) * 100000);
 }
 
 static int wm8400_ldo_map_voltage(struct regulator_dev *dev,
 				  int min_uV, int max_uV)
 {
 	u16 val;
+	int volt;
 
 	if (min_uV < 900000 || min_uV > 3300000)
 		return -EINVAL;
 
-	if (min_uV < 1700000) {
-		/* Steps of 50mV from 900mV;  */
+	if (min_uV < 1700000) /* Steps of 50mV from 900mV;  */
 		val = DIV_ROUND_UP(min_uV - 900000, 50000);
+	else /* Steps of 100mV from 1700mV */
+		val = DIV_ROUND_UP(min_uV - 1700000, 100000) + 15;
 
-		if ((val * 50000) + 900000 > max_uV)
-			return -EINVAL;
-		BUG_ON((val * 50000) + 900000 < min_uV);
-	} else {
-		/* Steps of 100mV from 1700mV */
-		val = DIV_ROUND_UP(min_uV - 1700000, 100000);
-
-		if ((val * 100000) + 1700000 > max_uV)
-			return -EINVAL;
-		BUG_ON((val * 100000) + 1700000 < min_uV);
-
-		val += 0xf;
-	}
+	volt = wm8400_ldo_list_voltage(dev, val);
+	if (volt < min_uV || volt > max_uV)
+		return -EINVAL;
 
 	return val;
 }
@@ -152,6 +144,7 @@ static struct regulator_ops wm8400_dcdc_ops = {
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
 	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_mode = wm8400_dcdc_get_mode,
diff --git a/drivers/regulator/wm8994-regulator.c b/drivers/regulator/wm8994-regulator.c
index 9a994316e63..86bb48db149 100644
--- a/drivers/regulator/wm8994-regulator.c
+++ b/drivers/regulator/wm8994-regulator.c
@@ -26,8 +26,6 @@
 #include <linux/mfd/wm8994/pdata.h>
 
 struct wm8994_ldo {
-	int enable;
-	bool is_enabled;
 	struct regulator_dev *regulator;
 	struct wm8994 *wm8994;
 };
@@ -35,64 +33,9 @@ struct wm8994_ldo {
 #define WM8994_LDO1_MAX_SELECTOR 0x7
 #define WM8994_LDO2_MAX_SELECTOR 0x3
 
-static int wm8994_ldo_enable(struct regulator_dev *rdev)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-
-	/* If we have no soft control assume that the LDO is always enabled. */
-	if (!ldo->enable)
-		return 0;
-
-	gpio_set_value_cansleep(ldo->enable, 1);
-	ldo->is_enabled = true;
-
-	return 0;
-}
-
-static int wm8994_ldo_disable(struct regulator_dev *rdev)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-
-	/* If we have no soft control assume that the LDO is always enabled. */
-	if (!ldo->enable)
-		return -EINVAL;
-
-	gpio_set_value_cansleep(ldo->enable, 0);
-	ldo->is_enabled = false;
-
-	return 0;
-}
-
-static int wm8994_ldo_is_enabled(struct regulator_dev *rdev)
-{
-	struct wm8994_ldo *ldo = rdev_get_drvdata(rdev);
-
-	return ldo->is_enabled;
-}
-
-static int wm8994_ldo_enable_time(struct regulator_dev *rdev)
-{
-	/* 3ms is fairly conservative but this shouldn't be too performance
-	 * critical; can be tweaked per-system if required. */
-	return 3000;
-}
-
-static int wm8994_ldo1_list_voltage(struct regulator_dev *rdev,
-				    unsigned int selector)
-{
-	if (selector > WM8994_LDO1_MAX_SELECTOR)
-		return -EINVAL;
-
-	return (selector * 100000) + 2400000;
-}
-
 static struct regulator_ops wm8994_ldo1_ops = {
-	.enable = wm8994_ldo_enable,
-	.disable = wm8994_ldo_disable,
-	.is_enabled = wm8994_ldo_is_enabled,
-	.enable_time = wm8994_ldo_enable_time,
-
-	.list_voltage = wm8994_ldo1_list_voltage,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 };
@@ -124,11 +67,6 @@ static int wm8994_ldo2_list_voltage(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops wm8994_ldo2_ops = {
-	.enable = wm8994_ldo_enable,
-	.disable = wm8994_ldo_disable,
-	.is_enabled = wm8994_ldo_is_enabled,
-	.enable_time = wm8994_ldo_enable_time,
-
 	.list_voltage = wm8994_ldo2_list_voltage,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
@@ -143,6 +81,9 @@ static const struct regulator_desc wm8994_ldo_desc[] = {
 		.vsel_reg = WM8994_LDO_1,
 		.vsel_mask = WM8994_LDO1_VSEL_MASK,
 		.ops = &wm8994_ldo1_ops,
+		.min_uV = 2400000,
+		.uV_step = 100000,
+		.enable_time = 3000,
 		.owner = THIS_MODULE,
 	},
 	{
@@ -153,6 +94,7 @@ static const struct regulator_desc wm8994_ldo_desc[] = {
 		.vsel_reg = WM8994_LDO_2,
 		.vsel_mask = WM8994_LDO2_VSEL_MASK,
 		.ops = &wm8994_ldo2_ops,
+		.enable_time = 3000,
 		.owner = THIS_MODULE,
 	},
 };
@@ -176,39 +118,26 @@ static __devinit int wm8994_ldo_probe(struct platform_device *pdev)
 
 	ldo->wm8994 = wm8994;
 
-	if (pdata->ldo[id].enable && gpio_is_valid(pdata->ldo[id].enable)) {
-		ldo->enable = pdata->ldo[id].enable;
-
-		ret = gpio_request_one(ldo->enable, 0, "WM8994 LDO enable");
-		if (ret < 0) {
-			dev_err(&pdev->dev, "Failed to get enable GPIO: %d\n",
-				ret);
-			goto err;
-		}
-	} else
-		ldo->is_enabled = true;
-
 	config.dev = wm8994->dev;
 	config.driver_data = ldo;
 	config.regmap = wm8994->regmap;
-	if (pdata)
+	if (pdata) {
 		config.init_data = pdata->ldo[id].init_data;
+		config.ena_gpio = pdata->ldo[id].enable;
+	}
 
 	ldo->regulator = regulator_register(&wm8994_ldo_desc[id], &config);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm8994->dev, "Failed to register LDO%d: %d\n",
 			id + 1, ret);
-		goto err_gpio;
+		goto err;
 	}
 
 	platform_set_drvdata(pdev, ldo);
 
 	return 0;
 
-err_gpio:
-	if (gpio_is_valid(ldo->enable))
-		gpio_free(ldo->enable);
 err:
 	return ret;
 }
@@ -220,8 +149,6 @@ static __devexit int wm8994_ldo_remove(struct platform_device *pdev)
 	platform_set_drvdata(pdev, NULL);
 
 	regulator_unregister(ldo->regulator);
-	if (gpio_is_valid(ldo->enable))
-		gpio_free(ldo->enable);
 
 	return 0;
 }
diff --git a/drivers/remoteproc/Makefile b/drivers/remoteproc/Makefile
index 5445d9b2329..934ce6e2c66 100644
--- a/drivers/remoteproc/Makefile
+++ b/drivers/remoteproc/Makefile
@@ -6,4 +6,5 @@ obj-$(CONFIG_REMOTEPROC)		+= remoteproc.o
 remoteproc-y				:= remoteproc_core.o
 remoteproc-y				+= remoteproc_debugfs.o
 remoteproc-y				+= remoteproc_virtio.o
+remoteproc-y				+= remoteproc_elf_loader.o
 obj-$(CONFIG_OMAP_REMOTEPROC)		+= omap_remoteproc.o
diff --git a/drivers/remoteproc/omap_remoteproc.c b/drivers/remoteproc/omap_remoteproc.c
index de138e30d3e..a1f7ac1f8cf 100644
--- a/drivers/remoteproc/omap_remoteproc.c
+++ b/drivers/remoteproc/omap_remoteproc.c
@@ -66,7 +66,7 @@ static int omap_rproc_mbox_callback(struct notifier_block *this,
 {
 	mbox_msg_t msg = (mbox_msg_t) data;
 	struct omap_rproc *oproc = container_of(this, struct omap_rproc, nb);
-	struct device *dev = oproc->rproc->dev;
+	struct device *dev = oproc->rproc->dev.parent;
 	const char *name = oproc->rproc->name;
 
 	dev_dbg(dev, "mbox msg: 0x%x\n", msg);
@@ -92,12 +92,13 @@ static int omap_rproc_mbox_callback(struct notifier_block *this,
 static void omap_rproc_kick(struct rproc *rproc, int vqid)
 {
 	struct omap_rproc *oproc = rproc->priv;
+	struct device *dev = rproc->dev.parent;
 	int ret;
 
 	/* send the index of the triggered virtqueue in the mailbox payload */
 	ret = omap_mbox_msg_send(oproc->mbox, vqid);
 	if (ret)
-		dev_err(rproc->dev, "omap_mbox_msg_send failed: %d\n", ret);
+		dev_err(dev, "omap_mbox_msg_send failed: %d\n", ret);
 }
 
 /*
@@ -110,7 +111,8 @@ static void omap_rproc_kick(struct rproc *rproc, int vqid)
 static int omap_rproc_start(struct rproc *rproc)
 {
 	struct omap_rproc *oproc = rproc->priv;
-	struct platform_device *pdev = to_platform_device(rproc->dev);
+	struct device *dev = rproc->dev.parent;
+	struct platform_device *pdev = to_platform_device(dev);
 	struct omap_rproc_pdata *pdata = pdev->dev.platform_data;
 	int ret;
 
@@ -120,7 +122,7 @@ static int omap_rproc_start(struct rproc *rproc)
 	oproc->mbox = omap_mbox_get(pdata->mbox_name, &oproc->nb);
 	if (IS_ERR(oproc->mbox)) {
 		ret = PTR_ERR(oproc->mbox);
-		dev_err(rproc->dev, "omap_mbox_get failed: %d\n", ret);
+		dev_err(dev, "omap_mbox_get failed: %d\n", ret);
 		return ret;
 	}
 
@@ -133,13 +135,13 @@ static int omap_rproc_start(struct rproc *rproc)
 	 */
 	ret = omap_mbox_msg_send(oproc->mbox, RP_MBOX_ECHO_REQUEST);
 	if (ret) {
-		dev_err(rproc->dev, "omap_mbox_get failed: %d\n", ret);
+		dev_err(dev, "omap_mbox_get failed: %d\n", ret);
 		goto put_mbox;
 	}
 
 	ret = pdata->device_enable(pdev);
 	if (ret) {
-		dev_err(rproc->dev, "omap_device_enable failed: %d\n", ret);
+		dev_err(dev, "omap_device_enable failed: %d\n", ret);
 		goto put_mbox;
 	}
 
@@ -153,7 +155,8 @@ put_mbox:
 /* power off the remote processor */
 static int omap_rproc_stop(struct rproc *rproc)
 {
-	struct platform_device *pdev = to_platform_device(rproc->dev);
+	struct device *dev = rproc->dev.parent;
+	struct platform_device *pdev = to_platform_device(dev);
 	struct omap_rproc_pdata *pdata = pdev->dev.platform_data;
 	struct omap_rproc *oproc = rproc->priv;
 	int ret;
@@ -196,14 +199,14 @@ static int __devinit omap_rproc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rproc);
 
-	ret = rproc_register(rproc);
+	ret = rproc_add(rproc);
 	if (ret)
 		goto free_rproc;
 
 	return 0;
 
 free_rproc:
-	rproc_free(rproc);
+	rproc_put(rproc);
 	return ret;
 }
 
@@ -211,7 +214,10 @@ static int __devexit omap_rproc_remove(struct platform_device *pdev)
 {
 	struct rproc *rproc = platform_get_drvdata(pdev);
 
-	return rproc_unregister(rproc);
+	rproc_del(rproc);
+	rproc_put(rproc);
+
+	return 0;
 }
 
 static struct platform_driver omap_rproc_driver = {
diff --git a/drivers/remoteproc/remoteproc_core.c b/drivers/remoteproc/remoteproc_core.c
index 66324ee4678..d5c2dbfc744 100644
--- a/drivers/remoteproc/remoteproc_core.c
+++ b/drivers/remoteproc/remoteproc_core.c
@@ -35,7 +35,7 @@
 #include <linux/debugfs.h>
 #include <linux/remoteproc.h>
 #include <linux/iommu.h>
-#include <linux/klist.h>
+#include <linux/idr.h>
 #include <linux/elf.h>
 #include <linux/virtio_ids.h>
 #include <linux/virtio_ring.h>
@@ -43,29 +43,13 @@
 
 #include "remoteproc_internal.h"
 
-static void klist_rproc_get(struct klist_node *n);
-static void klist_rproc_put(struct klist_node *n);
-
-/*
- * klist of the available remote processors.
- *
- * We need this in order to support name-based lookups (needed by the
- * rproc_get_by_name()).
- *
- * That said, we don't use rproc_get_by_name() at this point.
- * The use cases that do require its existence should be
- * scrutinized, and hopefully migrated to rproc_boot() using device-based
- * binding.
- *
- * If/when this materializes, we could drop the klist (and the by_name
- * API).
- */
-static DEFINE_KLIST(rprocs, klist_rproc_get, klist_rproc_put);
-
 typedef int (*rproc_handle_resources_t)(struct rproc *rproc,
 				struct resource_table *table, int len);
 typedef int (*rproc_handle_resource_t)(struct rproc *rproc, void *, int avail);
 
+/* Unique indices for remoteproc devices */
+static DEFINE_IDA(rproc_dev_index);
+
 /*
  * This is the IOMMU fault handler we register with the IOMMU API
  * (when relevant; not all remote processors access memory through
@@ -92,7 +76,7 @@ static int rproc_iommu_fault(struct iommu_domain *domain, struct device *dev,
 static int rproc_enable_iommu(struct rproc *rproc)
 {
 	struct iommu_domain *domain;
-	struct device *dev = rproc->dev;
+	struct device *dev = rproc->dev.parent;
 	int ret;
 
 	/*
@@ -137,7 +121,7 @@ free_domain:
 static void rproc_disable_iommu(struct rproc *rproc)
 {
 	struct iommu_domain *domain = rproc->domain;
-	struct device *dev = rproc->dev;
+	struct device *dev = rproc->dev.parent;
 
 	if (!domain)
 		return;
@@ -165,7 +149,7 @@ static void rproc_disable_iommu(struct rproc *rproc)
  * but only on kernel direct mapped RAM memory. Instead, we're just using
  * here the output of the DMA API, which should be more correct.
  */
-static void *rproc_da_to_va(struct rproc *rproc, u64 da, int len)
+void *rproc_da_to_va(struct rproc *rproc, u64 da, int len)
 {
 	struct rproc_mem_entry *carveout;
 	void *ptr = NULL;
@@ -188,125 +172,19 @@ static void *rproc_da_to_va(struct rproc *rproc, u64 da, int len)
 
 	return ptr;
 }
+EXPORT_SYMBOL(rproc_da_to_va);
 
-/**
- * rproc_load_segments() - load firmware segments to memory
- * @rproc: remote processor which will be booted using these fw segments
- * @elf_data: the content of the ELF firmware image
- * @len: firmware size (in bytes)
- *
- * This function loads the firmware segments to memory, where the remote
- * processor expects them.
- *
- * Some remote processors will expect their code and data to be placed
- * in specific device addresses, and can't have them dynamically assigned.
- *
- * We currently support only those kind of remote processors, and expect
- * the program header's paddr member to contain those addresses. We then go
- * through the physically contiguous "carveout" memory regions which we
- * allocated (and mapped) earlier on behalf of the remote processor,
- * and "translate" device address to kernel addresses, so we can copy the
- * segments where they are expected.
- *
- * Currently we only support remote processors that required carveout
- * allocations and got them mapped onto their iommus. Some processors
- * might be different: they might not have iommus, and would prefer to
- * directly allocate memory for every segment/resource. This is not yet
- * supported, though.
- */
-static int
-rproc_load_segments(struct rproc *rproc, const u8 *elf_data, size_t len)
-{
-	struct device *dev = rproc->dev;
-	struct elf32_hdr *ehdr;
-	struct elf32_phdr *phdr;
-	int i, ret = 0;
-
-	ehdr = (struct elf32_hdr *)elf_data;
-	phdr = (struct elf32_phdr *)(elf_data + ehdr->e_phoff);
-
-	/* go through the available ELF segments */
-	for (i = 0; i < ehdr->e_phnum; i++, phdr++) {
-		u32 da = phdr->p_paddr;
-		u32 memsz = phdr->p_memsz;
-		u32 filesz = phdr->p_filesz;
-		u32 offset = phdr->p_offset;
-		void *ptr;
-
-		if (phdr->p_type != PT_LOAD)
-			continue;
-
-		dev_dbg(dev, "phdr: type %d da 0x%x memsz 0x%x filesz 0x%x\n",
-					phdr->p_type, da, memsz, filesz);
-
-		if (filesz > memsz) {
-			dev_err(dev, "bad phdr filesz 0x%x memsz 0x%x\n",
-							filesz, memsz);
-			ret = -EINVAL;
-			break;
-		}
-
-		if (offset + filesz > len) {
-			dev_err(dev, "truncated fw: need 0x%x avail 0x%zx\n",
-					offset + filesz, len);
-			ret = -EINVAL;
-			break;
-		}
-
-		/* grab the kernel address for this device address */
-		ptr = rproc_da_to_va(rproc, da, memsz);
-		if (!ptr) {
-			dev_err(dev, "bad phdr da 0x%x mem 0x%x\n", da, memsz);
-			ret = -EINVAL;
-			break;
-		}
-
-		/* put the segment where the remote processor expects it */
-		if (phdr->p_filesz)
-			memcpy(ptr, elf_data + phdr->p_offset, filesz);
-
-		/*
-		 * Zero out remaining memory for this segment.
-		 *
-		 * This isn't strictly required since dma_alloc_coherent already
-		 * did this for us. albeit harmless, we may consider removing
-		 * this.
-		 */
-		if (memsz > filesz)
-			memset(ptr + filesz, 0, memsz - filesz);
-	}
-
-	return ret;
-}
-
-static int
-__rproc_handle_vring(struct rproc_vdev *rvdev, struct fw_rsc_vdev *rsc, int i)
+int rproc_alloc_vring(struct rproc_vdev *rvdev, int i)
 {
 	struct rproc *rproc = rvdev->rproc;
-	struct device *dev = rproc->dev;
-	struct fw_rsc_vdev_vring *vring = &rsc->vring[i];
+	struct device *dev = &rproc->dev;
+	struct rproc_vring *rvring = &rvdev->vring[i];
 	dma_addr_t dma;
 	void *va;
 	int ret, size, notifyid;
 
-	dev_dbg(dev, "vdev rsc: vring%d: da %x, qsz %d, align %d\n",
-				i, vring->da, vring->num, vring->align);
-
-	/* make sure reserved bytes are zeroes */
-	if (vring->reserved) {
-		dev_err(dev, "vring rsc has non zero reserved bytes\n");
-		return -EINVAL;
-	}
-
-	/* verify queue size and vring alignment are sane */
-	if (!vring->num || !vring->align) {
-		dev_err(dev, "invalid qsz (%d) or alignment (%d)\n",
-						vring->num, vring->align);
-		return -EINVAL;
-	}
-
 	/* actual size of vring (in bytes) */
-	size = PAGE_ALIGN(vring_size(vring->num, vring->align));
+	size = PAGE_ALIGN(vring_size(rvring->len, rvring->align));
 
 	if (!idr_pre_get(&rproc->notifyids, GFP_KERNEL)) {
 		dev_err(dev, "idr_pre_get failed\n");
@@ -316,51 +194,75 @@ __rproc_handle_vring(struct rproc_vdev *rvdev, struct fw_rsc_vdev *rsc, int i)
 	/*
 	 * Allocate non-cacheable memory for the vring. In the future
 	 * this call will also configure the IOMMU for us
+	 * TODO: let the rproc know the da of this vring
 	 */
-	va = dma_alloc_coherent(dev, size, &dma, GFP_KERNEL);
+	va = dma_alloc_coherent(dev->parent, size, &dma, GFP_KERNEL);
 	if (!va) {
-		dev_err(dev, "dma_alloc_coherent failed\n");
+		dev_err(dev->parent, "dma_alloc_coherent failed\n");
 		return -EINVAL;
 	}
 
-	/* assign an rproc-wide unique index for this vring */
-	/* TODO: assign a notifyid for rvdev updates as well */
-	ret = idr_get_new(&rproc->notifyids, &rvdev->vring[i], &notifyid);
+	/*
+	 * Assign an rproc-wide unique index for this vring
+	 * TODO: assign a notifyid for rvdev updates as well
+	 * TODO: let the rproc know the notifyid of this vring
+	 * TODO: support predefined notifyids (via resource table)
+	 */
+	ret = idr_get_new(&rproc->notifyids, rvring, &notifyid);
 	if (ret) {
 		dev_err(dev, "idr_get_new failed: %d\n", ret);
-		dma_free_coherent(dev, size, va, dma);
+		dma_free_coherent(dev->parent, size, va, dma);
 		return ret;
 	}
 
-	/* let the rproc know the da and notifyid of this vring */
-	/* TODO: expose this to remote processor */
-	vring->da = dma;
-	vring->notifyid = notifyid;
-
 	dev_dbg(dev, "vring%d: va %p dma %x size %x idr %d\n", i, va,
 					dma, size, notifyid);
 
-	rvdev->vring[i].len = vring->num;
-	rvdev->vring[i].align = vring->align;
-	rvdev->vring[i].va = va;
-	rvdev->vring[i].dma = dma;
-	rvdev->vring[i].notifyid = notifyid;
-	rvdev->vring[i].rvdev = rvdev;
+	rvring->va = va;
+	rvring->dma = dma;
+	rvring->notifyid = notifyid;
 
 	return 0;
 }
 
-static void __rproc_free_vrings(struct rproc_vdev *rvdev, int i)
+static int
+rproc_parse_vring(struct rproc_vdev *rvdev, struct fw_rsc_vdev *rsc, int i)
 {
 	struct rproc *rproc = rvdev->rproc;
+	struct device *dev = &rproc->dev;
+	struct fw_rsc_vdev_vring *vring = &rsc->vring[i];
+	struct rproc_vring *rvring = &rvdev->vring[i];
+
+	dev_dbg(dev, "vdev rsc: vring%d: da %x, qsz %d, align %d\n",
+				i, vring->da, vring->num, vring->align);
 
-	for (i--; i >= 0; i--) {
-		struct rproc_vring *rvring = &rvdev->vring[i];
-		int size = PAGE_ALIGN(vring_size(rvring->len, rvring->align));
+	/* make sure reserved bytes are zeroes */
+	if (vring->reserved) {
+		dev_err(dev, "vring rsc has non zero reserved bytes\n");
+		return -EINVAL;
+	}
 
-		dma_free_coherent(rproc->dev, size, rvring->va, rvring->dma);
-		idr_remove(&rproc->notifyids, rvring->notifyid);
+	/* verify queue size and vring alignment are sane */
+	if (!vring->num || !vring->align) {
+		dev_err(dev, "invalid qsz (%d) or alignment (%d)\n",
+						vring->num, vring->align);
+		return -EINVAL;
 	}
+
+	rvring->len = vring->num;
+	rvring->align = vring->align;
+	rvring->rvdev = rvdev;
+
+	return 0;
+}
+
+void rproc_free_vring(struct rproc_vring *rvring)
+{
+	int size = PAGE_ALIGN(vring_size(rvring->len, rvring->align));
+	struct rproc *rproc = rvring->rvdev->rproc;
+
+	dma_free_coherent(rproc->dev.parent, size, rvring->va, rvring->dma);
+	idr_remove(&rproc->notifyids, rvring->notifyid);
 }
 
 /**
@@ -393,14 +295,14 @@ static void __rproc_free_vrings(struct rproc_vdev *rvdev, int i)
 static int rproc_handle_vdev(struct rproc *rproc, struct fw_rsc_vdev *rsc,
 								int avail)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	struct rproc_vdev *rvdev;
 	int i, ret;
 
 	/* make sure resource isn't truncated */
 	if (sizeof(*rsc) + rsc->num_of_vrings * sizeof(struct fw_rsc_vdev_vring)
 			+ rsc->config_len > avail) {
-		dev_err(rproc->dev, "vdev rsc is truncated\n");
+		dev_err(dev, "vdev rsc is truncated\n");
 		return -EINVAL;
 	}
 
@@ -425,11 +327,11 @@ static int rproc_handle_vdev(struct rproc *rproc, struct fw_rsc_vdev *rsc,
 
 	rvdev->rproc = rproc;
 
-	/* allocate the vrings */
+	/* parse the vrings */
 	for (i = 0; i < rsc->num_of_vrings; i++) {
-		ret = __rproc_handle_vring(rvdev, rsc, i);
+		ret = rproc_parse_vring(rvdev, rsc, i);
 		if (ret)
-			goto free_vrings;
+			goto free_rvdev;
 	}
 
 	/* remember the device features */
@@ -440,12 +342,11 @@ static int rproc_handle_vdev(struct rproc *rproc, struct fw_rsc_vdev *rsc,
 	/* it is now safe to add the virtio device */
 	ret = rproc_add_virtio_dev(rvdev, rsc->id);
 	if (ret)
-		goto free_vrings;
+		goto free_rvdev;
 
 	return 0;
 
-free_vrings:
-	__rproc_free_vrings(rvdev, i);
+free_rvdev:
 	kfree(rvdev);
 	return ret;
 }
@@ -470,12 +371,12 @@ static int rproc_handle_trace(struct rproc *rproc, struct fw_rsc_trace *rsc,
 								int avail)
 {
 	struct rproc_mem_entry *trace;
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	void *ptr;
 	char name[15];
 
 	if (sizeof(*rsc) > avail) {
-		dev_err(rproc->dev, "trace rsc is truncated\n");
+		dev_err(dev, "trace rsc is truncated\n");
 		return -EINVAL;
 	}
 
@@ -552,6 +453,7 @@ static int rproc_handle_devmem(struct rproc *rproc, struct fw_rsc_devmem *rsc,
 								int avail)
 {
 	struct rproc_mem_entry *mapping;
+	struct device *dev = &rproc->dev;
 	int ret;
 
 	/* no point in handling this resource without a valid iommu domain */
@@ -559,25 +461,25 @@ static int rproc_handle_devmem(struct rproc *rproc, struct fw_rsc_devmem *rsc,
 		return -EINVAL;
 
 	if (sizeof(*rsc) > avail) {
-		dev_err(rproc->dev, "devmem rsc is truncated\n");
+		dev_err(dev, "devmem rsc is truncated\n");
 		return -EINVAL;
 	}
 
 	/* make sure reserved bytes are zeroes */
 	if (rsc->reserved) {
-		dev_err(rproc->dev, "devmem rsc has non zero reserved bytes\n");
+		dev_err(dev, "devmem rsc has non zero reserved bytes\n");
 		return -EINVAL;
 	}
 
 	mapping = kzalloc(sizeof(*mapping), GFP_KERNEL);
 	if (!mapping) {
-		dev_err(rproc->dev, "kzalloc mapping failed\n");
+		dev_err(dev, "kzalloc mapping failed\n");
 		return -ENOMEM;
 	}
 
 	ret = iommu_map(rproc->domain, rsc->da, rsc->pa, rsc->len, rsc->flags);
 	if (ret) {
-		dev_err(rproc->dev, "failed to map devmem: %d\n", ret);
+		dev_err(dev, "failed to map devmem: %d\n", ret);
 		goto out;
 	}
 
@@ -592,7 +494,7 @@ static int rproc_handle_devmem(struct rproc *rproc, struct fw_rsc_devmem *rsc,
 	mapping->len = rsc->len;
 	list_add_tail(&mapping->node, &rproc->mappings);
 
-	dev_dbg(rproc->dev, "mapped devmem pa 0x%x, da 0x%x, len 0x%x\n",
+	dev_dbg(dev, "mapped devmem pa 0x%x, da 0x%x, len 0x%x\n",
 					rsc->pa, rsc->da, rsc->len);
 
 	return 0;
@@ -624,13 +526,13 @@ static int rproc_handle_carveout(struct rproc *rproc,
 				struct fw_rsc_carveout *rsc, int avail)
 {
 	struct rproc_mem_entry *carveout, *mapping;
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	dma_addr_t dma;
 	void *va;
 	int ret;
 
 	if (sizeof(*rsc) > avail) {
-		dev_err(rproc->dev, "carveout rsc is truncated\n");
+		dev_err(dev, "carveout rsc is truncated\n");
 		return -EINVAL;
 	}
 
@@ -656,9 +558,9 @@ static int rproc_handle_carveout(struct rproc *rproc,
 		goto free_mapping;
 	}
 
-	va = dma_alloc_coherent(dev, rsc->len, &dma, GFP_KERNEL);
+	va = dma_alloc_coherent(dev->parent, rsc->len, &dma, GFP_KERNEL);
 	if (!va) {
-		dev_err(dev, "failed to dma alloc carveout: %d\n", rsc->len);
+		dev_err(dev->parent, "dma_alloc_coherent err: %d\n", rsc->len);
 		ret = -ENOMEM;
 		goto free_carv;
 	}
@@ -702,23 +604,27 @@ static int rproc_handle_carveout(struct rproc *rproc,
 		list_add_tail(&mapping->node, &rproc->mappings);
 
 		dev_dbg(dev, "carveout mapped 0x%x to 0x%x\n", rsc->da, dma);
-
-		/*
-		 * Some remote processors might need to know the pa
-		 * even though they are behind an IOMMU. E.g., OMAP4's
-		 * remote M3 processor needs this so it can control
-		 * on-chip hardware accelerators that are not behind
-		 * the IOMMU, and therefor must know the pa.
-		 *
-		 * Generally we don't want to expose physical addresses
-		 * if we don't have to (remote processors are generally
-		 * _not_ trusted), so we might want to do this only for
-		 * remote processor that _must_ have this (e.g. OMAP4's
-		 * dual M3 subsystem).
-		 */
-		rsc->pa = dma;
 	}
 
+	/*
+	 * Some remote processors might need to know the pa
+	 * even though they are behind an IOMMU. E.g., OMAP4's
+	 * remote M3 processor needs this so it can control
+	 * on-chip hardware accelerators that are not behind
+	 * the IOMMU, and therefor must know the pa.
+	 *
+	 * Generally we don't want to expose physical addresses
+	 * if we don't have to (remote processors are generally
+	 * _not_ trusted), so we might want to do this only for
+	 * remote processor that _must_ have this (e.g. OMAP4's
+	 * dual M3 subsystem).
+	 *
+	 * Non-IOMMU processors might also want to have this info.
+	 * In this case, the device address and the physical address
+	 * are the same.
+	 */
+	rsc->pa = dma;
+
 	carveout->va = va;
 	carveout->len = rsc->len;
 	carveout->dma = dma;
@@ -729,7 +635,7 @@ static int rproc_handle_carveout(struct rproc *rproc,
 	return 0;
 
 dma_free:
-	dma_free_coherent(dev, rsc->len, va, dma);
+	dma_free_coherent(dev->parent, rsc->len, va, dma);
 free_carv:
 	kfree(carveout);
 free_mapping:
@@ -752,7 +658,7 @@ static rproc_handle_resource_t rproc_handle_rsc[] = {
 static int
 rproc_handle_boot_rsc(struct rproc *rproc, struct resource_table *table, int len)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	rproc_handle_resource_t handler;
 	int ret = 0, i;
 
@@ -791,7 +697,7 @@ rproc_handle_boot_rsc(struct rproc *rproc, struct resource_table *table, int len
 static int
 rproc_handle_virtio_rsc(struct rproc *rproc, struct resource_table *table, int len)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	int ret = 0, i;
 
 	for (i = 0; i < table->num; i++) {
@@ -822,85 +728,6 @@ rproc_handle_virtio_rsc(struct rproc *rproc, struct resource_table *table, int l
 }
 
 /**
- * rproc_find_rsc_table() - find the resource table
- * @rproc: the rproc handle
- * @elf_data: the content of the ELF firmware image
- * @len: firmware size (in bytes)
- * @tablesz: place holder for providing back the table size
- *
- * This function finds the resource table inside the remote processor's
- * firmware. It is used both upon the registration of @rproc (in order
- * to look for and register the supported virito devices), and when the
- * @rproc is booted.
- *
- * Returns the pointer to the resource table if it is found, and write its
- * size into @tablesz. If a valid table isn't found, NULL is returned
- * (and @tablesz isn't set).
- */
-static struct resource_table *
-rproc_find_rsc_table(struct rproc *rproc, const u8 *elf_data, size_t len,
-							int *tablesz)
-{
-	struct elf32_hdr *ehdr;
-	struct elf32_shdr *shdr;
-	const char *name_table;
-	struct device *dev = rproc->dev;
-	struct resource_table *table = NULL;
-	int i;
-
-	ehdr = (struct elf32_hdr *)elf_data;
-	shdr = (struct elf32_shdr *)(elf_data + ehdr->e_shoff);
-	name_table = elf_data + shdr[ehdr->e_shstrndx].sh_offset;
-
-	/* look for the resource table and handle it */
-	for (i = 0; i < ehdr->e_shnum; i++, shdr++) {
-		int size = shdr->sh_size;
-		int offset = shdr->sh_offset;
-
-		if (strcmp(name_table + shdr->sh_name, ".resource_table"))
-			continue;
-
-		table = (struct resource_table *)(elf_data + offset);
-
-		/* make sure we have the entire table */
-		if (offset + size > len) {
-			dev_err(dev, "resource table truncated\n");
-			return NULL;
-		}
-
-		/* make sure table has at least the header */
-		if (sizeof(struct resource_table) > size) {
-			dev_err(dev, "header-less resource table\n");
-			return NULL;
-		}
-
-		/* we don't support any version beyond the first */
-		if (table->ver != 1) {
-			dev_err(dev, "unsupported fw ver: %d\n", table->ver);
-			return NULL;
-		}
-
-		/* make sure reserved bytes are zeroes */
-		if (table->reserved[0] || table->reserved[1]) {
-			dev_err(dev, "non zero reserved bytes\n");
-			return NULL;
-		}
-
-		/* make sure the offsets array isn't truncated */
-		if (table->num * sizeof(table->offset[0]) +
-				sizeof(struct resource_table) > size) {
-			dev_err(dev, "resource table incomplete\n");
-			return NULL;
-		}
-
-		*tablesz = shdr->sh_size;
-		break;
-	}
-
-	return table;
-}
-
-/**
  * rproc_resource_cleanup() - clean up and free all acquired resources
  * @rproc: rproc handle
  *
@@ -910,7 +737,7 @@ rproc_find_rsc_table(struct rproc *rproc, const u8 *elf_data, size_t len,
 static void rproc_resource_cleanup(struct rproc *rproc)
 {
 	struct rproc_mem_entry *entry, *tmp;
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 
 	/* clean up debugfs trace entries */
 	list_for_each_entry_safe(entry, tmp, &rproc->traces, node) {
@@ -922,7 +749,7 @@ static void rproc_resource_cleanup(struct rproc *rproc)
 
 	/* clean up carveout allocations */
 	list_for_each_entry_safe(entry, tmp, &rproc->carveouts, node) {
-		dma_free_coherent(dev, entry->len, entry->va, entry->dma);
+		dma_free_coherent(dev->parent, entry->len, entry->va, entry->dma);
 		list_del(&entry->node);
 		kfree(entry);
 	}
@@ -943,74 +770,13 @@ static void rproc_resource_cleanup(struct rproc *rproc)
 	}
 }
 
-/* make sure this fw image is sane */
-static int rproc_fw_sanity_check(struct rproc *rproc, const struct firmware *fw)
-{
-	const char *name = rproc->firmware;
-	struct device *dev = rproc->dev;
-	struct elf32_hdr *ehdr;
-	char class;
-
-	if (!fw) {
-		dev_err(dev, "failed to load %s\n", name);
-		return -EINVAL;
-	}
-
-	if (fw->size < sizeof(struct elf32_hdr)) {
-		dev_err(dev, "Image is too small\n");
-		return -EINVAL;
-	}
-
-	ehdr = (struct elf32_hdr *)fw->data;
-
-	/* We only support ELF32 at this point */
-	class = ehdr->e_ident[EI_CLASS];
-	if (class != ELFCLASS32) {
-		dev_err(dev, "Unsupported class: %d\n", class);
-		return -EINVAL;
-	}
-
-	/* We assume the firmware has the same endianess as the host */
-# ifdef __LITTLE_ENDIAN
-	if (ehdr->e_ident[EI_DATA] != ELFDATA2LSB) {
-# else /* BIG ENDIAN */
-	if (ehdr->e_ident[EI_DATA] != ELFDATA2MSB) {
-# endif
-		dev_err(dev, "Unsupported firmware endianess\n");
-		return -EINVAL;
-	}
-
-	if (fw->size < ehdr->e_shoff + sizeof(struct elf32_shdr)) {
-		dev_err(dev, "Image is too small\n");
-		return -EINVAL;
-	}
-
-	if (memcmp(ehdr->e_ident, ELFMAG, SELFMAG)) {
-		dev_err(dev, "Image is corrupted (bad magic)\n");
-		return -EINVAL;
-	}
-
-	if (ehdr->e_phnum == 0) {
-		dev_err(dev, "No loadable segments\n");
-		return -EINVAL;
-	}
-
-	if (ehdr->e_phoff > fw->size) {
-		dev_err(dev, "Firmware size is too small\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
 /*
  * take a firmware and boot a remote processor with it.
  */
 static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	const char *name = rproc->firmware;
-	struct elf32_hdr *ehdr;
 	struct resource_table *table;
 	int ret, tablesz;
 
@@ -1018,8 +784,6 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 	if (ret)
 		return ret;
 
-	ehdr = (struct elf32_hdr *)fw->data;
-
 	dev_info(dev, "Booting fw image %s, size %zd\n", name, fw->size);
 
 	/*
@@ -1032,15 +796,10 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 		return ret;
 	}
 
-	/*
-	 * The ELF entry point is the rproc's boot addr (though this is not
-	 * a configurable property of all remote processors: some will always
-	 * boot at a specific hardcoded address).
-	 */
-	rproc->bootaddr = ehdr->e_entry;
+	rproc->bootaddr = rproc_get_boot_addr(rproc, fw);
 
 	/* look for the resource table */
-	table = rproc_find_rsc_table(rproc, fw->data, fw->size, &tablesz);
+	table = rproc_find_rsc_table(rproc, fw, &tablesz);
 	if (!table) {
 		ret = -EINVAL;
 		goto clean_up;
@@ -1054,7 +813,7 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 	}
 
 	/* load the ELF segments to memory */
-	ret = rproc_load_segments(rproc, fw->data, fw->size);
+	ret = rproc_load_segments(rproc, fw);
 	if (ret) {
 		dev_err(dev, "Failed to load program segments: %d\n", ret);
 		goto clean_up;
@@ -1097,7 +856,7 @@ static void rproc_fw_config_virtio(const struct firmware *fw, void *context)
 		goto out;
 
 	/* look for the resource table */
-	table = rproc_find_rsc_table(rproc, fw->data, fw->size, &tablesz);
+	table = rproc_find_rsc_table(rproc, fw,  &tablesz);
 	if (!table)
 		goto out;
 
@@ -1108,7 +867,7 @@ static void rproc_fw_config_virtio(const struct firmware *fw, void *context)
 
 out:
 	release_firmware(fw);
-	/* allow rproc_unregister() contexts, if any, to proceed */
+	/* allow rproc_del() contexts, if any, to proceed */
 	complete_all(&rproc->firmware_loading_complete);
 }
 
@@ -1134,7 +893,7 @@ int rproc_boot(struct rproc *rproc)
 		return -EINVAL;
 	}
 
-	dev = rproc->dev;
+	dev = &rproc->dev;
 
 	ret = mutex_lock_interruptible(&rproc->lock);
 	if (ret) {
@@ -1150,7 +909,7 @@ int rproc_boot(struct rproc *rproc)
 	}
 
 	/* prevent underlying implementation from being removed */
-	if (!try_module_get(dev->driver->owner)) {
+	if (!try_module_get(dev->parent->driver->owner)) {
 		dev_err(dev, "%s: can't get owner\n", __func__);
 		ret = -EINVAL;
 		goto unlock_mutex;
@@ -1177,7 +936,7 @@ int rproc_boot(struct rproc *rproc)
 
 downref_rproc:
 	if (ret) {
-		module_put(dev->driver->owner);
+		module_put(dev->parent->driver->owner);
 		atomic_dec(&rproc->power);
 	}
 unlock_mutex:
@@ -1204,14 +963,10 @@ EXPORT_SYMBOL(rproc_boot);
  *   which means that the @rproc handle stays valid even after rproc_shutdown()
  *   returns, and users can still use it with a subsequent rproc_boot(), if
  *   needed.
- * - don't call rproc_shutdown() to unroll rproc_get_by_name(), exactly
- *   because rproc_shutdown() _does not_ decrement the refcount of @rproc.
- *   To decrement the refcount of @rproc, use rproc_put() (but _only_ if
- *   you acquired @rproc using rproc_get_by_name()).
  */
 void rproc_shutdown(struct rproc *rproc)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	int ret;
 
 	ret = mutex_lock_interruptible(&rproc->lock);
@@ -1244,148 +999,12 @@ void rproc_shutdown(struct rproc *rproc)
 out:
 	mutex_unlock(&rproc->lock);
 	if (!ret)
-		module_put(dev->driver->owner);
+		module_put(dev->parent->driver->owner);
 }
 EXPORT_SYMBOL(rproc_shutdown);
 
 /**
- * rproc_release() - completely deletes the existence of a remote processor
- * @kref: the rproc's kref
- *
- * This function should _never_ be called directly.
- *
- * The only reasonable location to use it is as an argument when kref_put'ing
- * @rproc's refcount.
- *
- * This way it will be called when no one holds a valid pointer to this @rproc
- * anymore (and obviously after it is removed from the rprocs klist).
- *
- * Note: this function is not static because rproc_vdev_release() needs it when
- * it decrements @rproc's refcount.
- */
-void rproc_release(struct kref *kref)
-{
-	struct rproc *rproc = container_of(kref, struct rproc, refcount);
-	struct rproc_vdev *rvdev, *rvtmp;
-
-	dev_info(rproc->dev, "removing %s\n", rproc->name);
-
-	rproc_delete_debug_dir(rproc);
-
-	/* clean up remote vdev entries */
-	list_for_each_entry_safe(rvdev, rvtmp, &rproc->rvdevs, node) {
-		__rproc_free_vrings(rvdev, RVDEV_NUM_VRINGS);
-		list_del(&rvdev->node);
-	}
-
-	/*
-	 * At this point no one holds a reference to rproc anymore,
-	 * so we can directly unroll rproc_alloc()
-	 */
-	rproc_free(rproc);
-}
-
-/* will be called when an rproc is added to the rprocs klist */
-static void klist_rproc_get(struct klist_node *n)
-{
-	struct rproc *rproc = container_of(n, struct rproc, node);
-
-	kref_get(&rproc->refcount);
-}
-
-/* will be called when an rproc is removed from the rprocs klist */
-static void klist_rproc_put(struct klist_node *n)
-{
-	struct rproc *rproc = container_of(n, struct rproc, node);
-
-	kref_put(&rproc->refcount, rproc_release);
-}
-
-static struct rproc *next_rproc(struct klist_iter *i)
-{
-	struct klist_node *n;
-
-	n = klist_next(i);
-	if (!n)
-		return NULL;
-
-	return container_of(n, struct rproc, node);
-}
-
-/**
- * rproc_get_by_name() - find a remote processor by name and boot it
- * @name: name of the remote processor
- *
- * Finds an rproc handle using the remote processor's name, and then
- * boot it. If it's already powered on, then just immediately return
- * (successfully).
- *
- * Returns the rproc handle on success, and NULL on failure.
- *
- * This function increments the remote processor's refcount, so always
- * use rproc_put() to decrement it back once rproc isn't needed anymore.
- *
- * Note: currently this function (and its counterpart rproc_put()) are not
- * being used. We need to scrutinize the use cases
- * that still need them, and see if we can migrate them to use the non
- * name-based boot/shutdown interface.
- */
-struct rproc *rproc_get_by_name(const char *name)
-{
-	struct rproc *rproc;
-	struct klist_iter i;
-	int ret;
-
-	/* find the remote processor, and upref its refcount */
-	klist_iter_init(&rprocs, &i);
-	while ((rproc = next_rproc(&i)) != NULL)
-		if (!strcmp(rproc->name, name)) {
-			kref_get(&rproc->refcount);
-			break;
-		}
-	klist_iter_exit(&i);
-
-	/* can't find this rproc ? */
-	if (!rproc) {
-		pr_err("can't find remote processor %s\n", name);
-		return NULL;
-	}
-
-	ret = rproc_boot(rproc);
-	if (ret < 0) {
-		kref_put(&rproc->refcount, rproc_release);
-		return NULL;
-	}
-
-	return rproc;
-}
-EXPORT_SYMBOL(rproc_get_by_name);
-
-/**
- * rproc_put() - decrement the refcount of a remote processor, and shut it down
- * @rproc: the remote processor
- *
- * This function tries to shutdown @rproc, and it then decrements its
- * refcount.
- *
- * After this function returns, @rproc may _not_ be used anymore, and its
- * handle should be considered invalid.
- *
- * This function should be called _iff_ the @rproc handle was grabbed by
- * calling rproc_get_by_name().
- */
-void rproc_put(struct rproc *rproc)
-{
-	/* try to power off the remote processor */
-	rproc_shutdown(rproc);
-
-	/* downref rproc's refcount */
-	kref_put(&rproc->refcount, rproc_release);
-}
-EXPORT_SYMBOL(rproc_put);
-
-/**
- * rproc_register() - register a remote processor
+ * rproc_add() - register a remote processor
  * @rproc: the remote processor handle to register
  *
  * Registers @rproc with the remoteproc framework, after it has been
@@ -1404,15 +1023,16 @@ EXPORT_SYMBOL(rproc_put);
  * of registering this remote processor, additional virtio drivers might be
  * probed.
  */
-int rproc_register(struct rproc *rproc)
+int rproc_add(struct rproc *rproc)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	int ret = 0;
 
-	/* expose to rproc_get_by_name users */
-	klist_add_tail(&rproc->node, &rprocs);
+	ret = device_add(dev);
+	if (ret < 0)
+		return ret;
 
-	dev_info(rproc->dev, "%s is available\n", rproc->name);
+	dev_info(dev, "%s is available\n", rproc->name);
 
 	dev_info(dev, "Note: remoteproc is still under development and considered experimental.\n");
 	dev_info(dev, "THE BINARY FORMAT IS NOT YET FINALIZED, and backward compatibility isn't yet guaranteed.\n");
@@ -1420,7 +1040,7 @@ int rproc_register(struct rproc *rproc)
 	/* create debugfs entries */
 	rproc_create_debug_dir(rproc);
 
-	/* rproc_unregister() calls must wait until async loader completes */
+	/* rproc_del() calls must wait until async loader completes */
 	init_completion(&rproc->firmware_loading_complete);
 
 	/*
@@ -1437,12 +1057,42 @@ int rproc_register(struct rproc *rproc)
 	if (ret < 0) {
 		dev_err(dev, "request_firmware_nowait failed: %d\n", ret);
 		complete_all(&rproc->firmware_loading_complete);
-		klist_remove(&rproc->node);
 	}
 
 	return ret;
 }
-EXPORT_SYMBOL(rproc_register);
+EXPORT_SYMBOL(rproc_add);
+
+/**
+ * rproc_type_release() - release a remote processor instance
+ * @dev: the rproc's device
+ *
+ * This function should _never_ be called directly.
+ *
+ * It will be called by the driver core when no one holds a valid pointer
+ * to @dev anymore.
+ */
+static void rproc_type_release(struct device *dev)
+{
+	struct rproc *rproc = container_of(dev, struct rproc, dev);
+
+	dev_info(&rproc->dev, "releasing %s\n", rproc->name);
+
+	rproc_delete_debug_dir(rproc);
+
+	idr_remove_all(&rproc->notifyids);
+	idr_destroy(&rproc->notifyids);
+
+	if (rproc->index >= 0)
+		ida_simple_remove(&rproc_dev_index, rproc->index);
+
+	kfree(rproc);
+}
+
+static struct device_type rproc_type = {
+	.name		= "remoteproc",
+	.release	= rproc_type_release,
+};
 
 /**
  * rproc_alloc() - allocate a remote processor handle
@@ -1459,13 +1109,13 @@ EXPORT_SYMBOL(rproc_register);
  * of the remote processor.
  *
  * After creating an rproc handle using this function, and when ready,
- * implementations should then call rproc_register() to complete
+ * implementations should then call rproc_add() to complete
  * the registration of the remote processor.
  *
  * On success the new rproc is returned, and on failure, NULL.
  *
  * Note: _never_ directly deallocate @rproc, even if it was not registered
- * yet. Instead, if you just need to unroll rproc_alloc(), use rproc_free().
+ * yet. Instead, when you need to unroll rproc_alloc(), use rproc_put().
  */
 struct rproc *rproc_alloc(struct device *dev, const char *name,
 				const struct rproc_ops *ops,
@@ -1482,15 +1132,29 @@ struct rproc *rproc_alloc(struct device *dev, const char *name,
 		return NULL;
 	}
 
-	rproc->dev = dev;
 	rproc->name = name;
 	rproc->ops = ops;
 	rproc->firmware = firmware;
 	rproc->priv = &rproc[1];
 
+	device_initialize(&rproc->dev);
+	rproc->dev.parent = dev;
+	rproc->dev.type = &rproc_type;
+
+	/* Assign a unique device index and name */
+	rproc->index = ida_simple_get(&rproc_dev_index, 0, 0, GFP_KERNEL);
+	if (rproc->index < 0) {
+		dev_err(dev, "ida_simple_get failed: %d\n", rproc->index);
+		put_device(&rproc->dev);
+		return NULL;
+	}
+
+	dev_set_name(&rproc->dev, "remoteproc%d", rproc->index);
+
 	atomic_set(&rproc->power, 0);
 
-	kref_init(&rproc->refcount);
+	/* Set ELF as the default fw_ops handler */
+	rproc->fw_ops = &rproc_elf_fw_ops;
 
 	mutex_init(&rproc->lock);
 
@@ -1508,47 +1172,38 @@ struct rproc *rproc_alloc(struct device *dev, const char *name,
 EXPORT_SYMBOL(rproc_alloc);
 
 /**
- * rproc_free() - free an rproc handle that was allocated by rproc_alloc
+ * rproc_put() - unroll rproc_alloc()
  * @rproc: the remote processor handle
  *
- * This function should _only_ be used if @rproc was only allocated,
- * but not registered yet.
+ * This function decrements the rproc dev refcount.
  *
- * If @rproc was already successfully registered (by calling rproc_register()),
- * then use rproc_unregister() instead.
+ * If no one holds any reference to rproc anymore, then its refcount would
+ * now drop to zero, and it would be freed.
  */
-void rproc_free(struct rproc *rproc)
+void rproc_put(struct rproc *rproc)
 {
-	idr_remove_all(&rproc->notifyids);
-	idr_destroy(&rproc->notifyids);
-
-	kfree(rproc);
+	put_device(&rproc->dev);
 }
-EXPORT_SYMBOL(rproc_free);
+EXPORT_SYMBOL(rproc_put);
 
 /**
- * rproc_unregister() - unregister a remote processor
+ * rproc_del() - unregister a remote processor
  * @rproc: rproc handle to unregister
  *
- * Unregisters a remote processor, and decrements its refcount.
- * If its refcount drops to zero, then @rproc will be freed. If not,
- * it will be freed later once the last reference is dropped.
- *
  * This function should be called when the platform specific rproc
  * implementation decides to remove the rproc device. it should
- * _only_ be called if a previous invocation of rproc_register()
+ * _only_ be called if a previous invocation of rproc_add()
  * has completed successfully.
  *
- * After rproc_unregister() returns, @rproc is _not_ valid anymore and
- * it shouldn't be used. More specifically, don't call rproc_free()
- * or try to directly free @rproc after rproc_unregister() returns;
- * none of these are needed, and calling them is a bug.
+ * After rproc_del() returns, @rproc isn't freed yet, because
+ * of the outstanding reference created by rproc_alloc. To decrement that
+ * one last refcount, one still needs to call rproc_put().
  *
  * Returns 0 on success and -EINVAL if @rproc isn't valid.
  */
-int rproc_unregister(struct rproc *rproc)
+int rproc_del(struct rproc *rproc)
 {
-	struct rproc_vdev *rvdev;
+	struct rproc_vdev *rvdev, *tmp;
 
 	if (!rproc)
 		return -EINVAL;
@@ -1557,22 +1212,19 @@ int rproc_unregister(struct rproc *rproc)
 	wait_for_completion(&rproc->firmware_loading_complete);
 
 	/* clean up remote vdev entries */
-	list_for_each_entry(rvdev, &rproc->rvdevs, node)
+	list_for_each_entry_safe(rvdev, tmp, &rproc->rvdevs, node)
 		rproc_remove_virtio_dev(rvdev);
 
-	/* the rproc is downref'ed as soon as it's removed from the klist */
-	klist_del(&rproc->node);
-
-	/* the rproc will only be released after its refcount drops to zero */
-	kref_put(&rproc->refcount, rproc_release);
+	device_del(&rproc->dev);
 
 	return 0;
 }
-EXPORT_SYMBOL(rproc_unregister);
+EXPORT_SYMBOL(rproc_del);
 
 static int __init remoteproc_init(void)
 {
 	rproc_init_debugfs();
+
 	return 0;
 }
 module_init(remoteproc_init);
diff --git a/drivers/remoteproc/remoteproc_debugfs.c b/drivers/remoteproc/remoteproc_debugfs.c
index 85d31a69e11..03833850f21 100644
--- a/drivers/remoteproc/remoteproc_debugfs.c
+++ b/drivers/remoteproc/remoteproc_debugfs.c
@@ -124,7 +124,7 @@ struct dentry *rproc_create_trace_file(const char *name, struct rproc *rproc,
 	tfile = debugfs_create_file(name, 0400, rproc->dbg_dir,
 						trace, &trace_rproc_ops);
 	if (!tfile) {
-		dev_err(rproc->dev, "failed to create debugfs trace entry\n");
+		dev_err(&rproc->dev, "failed to create debugfs trace entry\n");
 		return NULL;
 	}
 
@@ -141,7 +141,7 @@ void rproc_delete_debug_dir(struct rproc *rproc)
 
 void rproc_create_debug_dir(struct rproc *rproc)
 {
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 
 	if (!rproc_dbg)
 		return;
diff --git a/drivers/remoteproc/remoteproc_elf_loader.c b/drivers/remoteproc/remoteproc_elf_loader.c
new file mode 100644
index 00000000000..e1f89d64973
--- /dev/null
+++ b/drivers/remoteproc/remoteproc_elf_loader.c
@@ -0,0 +1,295 @@
+/*
+ * Remote Processor Framework Elf loader
+ *
+ * Copyright (C) 2011 Texas Instruments, Inc.
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * Ohad Ben-Cohen <ohad@wizery.com>
+ * Brian Swetland <swetland@google.com>
+ * Mark Grosen <mgrosen@ti.com>
+ * Fernando Guzman Lugo <fernando.lugo@ti.com>
+ * Suman Anna <s-anna@ti.com>
+ * Robert Tivy <rtivy@ti.com>
+ * Armando Uribe De Leon <x0095078@ti.com>
+ * Sjur Brændeland <sjur.brandeland@stericsson.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/module.h>
+#include <linux/firmware.h>
+#include <linux/remoteproc.h>
+#include <linux/elf.h>
+
+#include "remoteproc_internal.h"
+
+/**
+ * rproc_elf_sanity_check() - Sanity Check ELF firmware image
+ * @rproc: the remote processor handle
+ * @fw: the ELF firmware image
+ *
+ * Make sure this fw image is sane.
+ */
+static int
+rproc_elf_sanity_check(struct rproc *rproc, const struct firmware *fw)
+{
+	const char *name = rproc->firmware;
+	struct device *dev = &rproc->dev;
+	struct elf32_hdr *ehdr;
+	char class;
+
+	if (!fw) {
+		dev_err(dev, "failed to load %s\n", name);
+		return -EINVAL;
+	}
+
+	if (fw->size < sizeof(struct elf32_hdr)) {
+		dev_err(dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	ehdr = (struct elf32_hdr *)fw->data;
+
+	/* We only support ELF32 at this point */
+	class = ehdr->e_ident[EI_CLASS];
+	if (class != ELFCLASS32) {
+		dev_err(dev, "Unsupported class: %d\n", class);
+		return -EINVAL;
+	}
+
+	/* We assume the firmware has the same endianess as the host */
+# ifdef __LITTLE_ENDIAN
+	if (ehdr->e_ident[EI_DATA] != ELFDATA2LSB) {
+# else /* BIG ENDIAN */
+	if (ehdr->e_ident[EI_DATA] != ELFDATA2MSB) {
+# endif
+		dev_err(dev, "Unsupported firmware endianess\n");
+		return -EINVAL;
+	}
+
+	if (fw->size < ehdr->e_shoff + sizeof(struct elf32_shdr)) {
+		dev_err(dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	if (memcmp(ehdr->e_ident, ELFMAG, SELFMAG)) {
+		dev_err(dev, "Image is corrupted (bad magic)\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_phnum == 0) {
+		dev_err(dev, "No loadable segments\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_phoff > fw->size) {
+		dev_err(dev, "Firmware size is too small\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * rproc_elf_get_boot_addr() - Get rproc's boot address.
+ * @rproc: the remote processor handle
+ * @fw: the ELF firmware image
+ *
+ * This function returns the entry point address of the ELF
+ * image.
+ *
+ * Note that the boot address is not a configurable property of all remote
+ * processors. Some will always boot at a specific hard-coded address.
+ */
+static
+u32 rproc_elf_get_boot_addr(struct rproc *rproc, const struct firmware *fw)
+{
+	struct elf32_hdr *ehdr  = (struct elf32_hdr *)fw->data;
+
+	return ehdr->e_entry;
+}
+
+/**
+ * rproc_elf_load_segments() - load firmware segments to memory
+ * @rproc: remote processor which will be booted using these fw segments
+ * @fw: the ELF firmware image
+ *
+ * This function loads the firmware segments to memory, where the remote
+ * processor expects them.
+ *
+ * Some remote processors will expect their code and data to be placed
+ * in specific device addresses, and can't have them dynamically assigned.
+ *
+ * We currently support only those kind of remote processors, and expect
+ * the program header's paddr member to contain those addresses. We then go
+ * through the physically contiguous "carveout" memory regions which we
+ * allocated (and mapped) earlier on behalf of the remote processor,
+ * and "translate" device address to kernel addresses, so we can copy the
+ * segments where they are expected.
+ *
+ * Currently we only support remote processors that required carveout
+ * allocations and got them mapped onto their iommus. Some processors
+ * might be different: they might not have iommus, and would prefer to
+ * directly allocate memory for every segment/resource. This is not yet
+ * supported, though.
+ */
+static int
+rproc_elf_load_segments(struct rproc *rproc, const struct firmware *fw)
+{
+	struct device *dev = &rproc->dev;
+	struct elf32_hdr *ehdr;
+	struct elf32_phdr *phdr;
+	int i, ret = 0;
+	const u8 *elf_data = fw->data;
+
+	ehdr = (struct elf32_hdr *)elf_data;
+	phdr = (struct elf32_phdr *)(elf_data + ehdr->e_phoff);
+
+	/* go through the available ELF segments */
+	for (i = 0; i < ehdr->e_phnum; i++, phdr++) {
+		u32 da = phdr->p_paddr;
+		u32 memsz = phdr->p_memsz;
+		u32 filesz = phdr->p_filesz;
+		u32 offset = phdr->p_offset;
+		void *ptr;
+
+		if (phdr->p_type != PT_LOAD)
+			continue;
+
+		dev_dbg(dev, "phdr: type %d da 0x%x memsz 0x%x filesz 0x%x\n",
+					phdr->p_type, da, memsz, filesz);
+
+		if (filesz > memsz) {
+			dev_err(dev, "bad phdr filesz 0x%x memsz 0x%x\n",
+							filesz, memsz);
+			ret = -EINVAL;
+			break;
+		}
+
+		if (offset + filesz > fw->size) {
+			dev_err(dev, "truncated fw: need 0x%x avail 0x%zx\n",
+					offset + filesz, fw->size);
+			ret = -EINVAL;
+			break;
+		}
+
+		/* grab the kernel address for this device address */
+		ptr = rproc_da_to_va(rproc, da, memsz);
+		if (!ptr) {
+			dev_err(dev, "bad phdr da 0x%x mem 0x%x\n", da, memsz);
+			ret = -EINVAL;
+			break;
+		}
+
+		/* put the segment where the remote processor expects it */
+		if (phdr->p_filesz)
+			memcpy(ptr, elf_data + phdr->p_offset, filesz);
+
+		/*
+		 * Zero out remaining memory for this segment.
+		 *
+		 * This isn't strictly required since dma_alloc_coherent already
+		 * did this for us. albeit harmless, we may consider removing
+		 * this.
+		 */
+		if (memsz > filesz)
+			memset(ptr + filesz, 0, memsz - filesz);
+	}
+
+	return ret;
+}
+
+/**
+ * rproc_elf_find_rsc_table() - find the resource table
+ * @rproc: the rproc handle
+ * @fw: the ELF firmware image
+ * @tablesz: place holder for providing back the table size
+ *
+ * This function finds the resource table inside the remote processor's
+ * firmware. It is used both upon the registration of @rproc (in order
+ * to look for and register the supported virito devices), and when the
+ * @rproc is booted.
+ *
+ * Returns the pointer to the resource table if it is found, and write its
+ * size into @tablesz. If a valid table isn't found, NULL is returned
+ * (and @tablesz isn't set).
+ */
+static struct resource_table *
+rproc_elf_find_rsc_table(struct rproc *rproc, const struct firmware *fw,
+							int *tablesz)
+{
+	struct elf32_hdr *ehdr;
+	struct elf32_shdr *shdr;
+	const char *name_table;
+	struct device *dev = &rproc->dev;
+	struct resource_table *table = NULL;
+	int i;
+	const u8 *elf_data = fw->data;
+
+	ehdr = (struct elf32_hdr *)elf_data;
+	shdr = (struct elf32_shdr *)(elf_data + ehdr->e_shoff);
+	name_table = elf_data + shdr[ehdr->e_shstrndx].sh_offset;
+
+	/* look for the resource table and handle it */
+	for (i = 0; i < ehdr->e_shnum; i++, shdr++) {
+		int size = shdr->sh_size;
+		int offset = shdr->sh_offset;
+
+		if (strcmp(name_table + shdr->sh_name, ".resource_table"))
+			continue;
+
+		table = (struct resource_table *)(elf_data + offset);
+
+		/* make sure we have the entire table */
+		if (offset + size > fw->size) {
+			dev_err(dev, "resource table truncated\n");
+			return NULL;
+		}
+
+		/* make sure table has at least the header */
+		if (sizeof(struct resource_table) > size) {
+			dev_err(dev, "header-less resource table\n");
+			return NULL;
+		}
+
+		/* we don't support any version beyond the first */
+		if (table->ver != 1) {
+			dev_err(dev, "unsupported fw ver: %d\n", table->ver);
+			return NULL;
+		}
+
+		/* make sure reserved bytes are zeroes */
+		if (table->reserved[0] || table->reserved[1]) {
+			dev_err(dev, "non zero reserved bytes\n");
+			return NULL;
+		}
+
+		/* make sure the offsets array isn't truncated */
+		if (table->num * sizeof(table->offset[0]) +
+				sizeof(struct resource_table) > size) {
+			dev_err(dev, "resource table incomplete\n");
+			return NULL;
+		}
+
+		*tablesz = shdr->sh_size;
+		break;
+	}
+
+	return table;
+}
+
+const struct rproc_fw_ops rproc_elf_fw_ops = {
+	.load = rproc_elf_load_segments,
+	.find_rsc_table = rproc_elf_find_rsc_table,
+	.sanity_check = rproc_elf_sanity_check,
+	.get_boot_addr = rproc_elf_get_boot_addr
+};
diff --git a/drivers/remoteproc/remoteproc_internal.h b/drivers/remoteproc/remoteproc_internal.h
index 9f336d6bdef..a690ebe7aa5 100644
--- a/drivers/remoteproc/remoteproc_internal.h
+++ b/drivers/remoteproc/remoteproc_internal.h
@@ -21,9 +21,27 @@
 #define REMOTEPROC_INTERNAL_H
 
 #include <linux/irqreturn.h>
+#include <linux/firmware.h>
 
 struct rproc;
 
+/**
+ * struct rproc_fw_ops - firmware format specific operations.
+ * @find_rsc_table:	finds the resource table inside the firmware image
+ * @load:		load firmeware to memory, where the remote processor
+ *			expects to find it
+ * @sanity_check:	sanity check the fw image
+ * @get_boot_addr:	get boot address to entry point specified in firmware
+ */
+struct rproc_fw_ops {
+	struct resource_table *(*find_rsc_table) (struct rproc *rproc,
+						const struct firmware *fw,
+						int *tablesz);
+	int (*load)(struct rproc *rproc, const struct firmware *fw);
+	int (*sanity_check)(struct rproc *rproc, const struct firmware *fw);
+	u32 (*get_boot_addr)(struct rproc *rproc, const struct firmware *fw);
+};
+
 /* from remoteproc_core.c */
 void rproc_release(struct kref *kref);
 irqreturn_t rproc_vq_interrupt(struct rproc *rproc, int vq_id);
@@ -41,4 +59,48 @@ void rproc_create_debug_dir(struct rproc *rproc);
 void rproc_init_debugfs(void);
 void rproc_exit_debugfs(void);
 
+void rproc_free_vring(struct rproc_vring *rvring);
+int rproc_alloc_vring(struct rproc_vdev *rvdev, int i);
+
+void *rproc_da_to_va(struct rproc *rproc, u64 da, int len);
+
+static inline
+int rproc_fw_sanity_check(struct rproc *rproc, const struct firmware *fw)
+{
+	if (rproc->fw_ops->sanity_check)
+		return rproc->fw_ops->sanity_check(rproc, fw);
+
+	return 0;
+}
+
+static inline
+u32 rproc_get_boot_addr(struct rproc *rproc, const struct firmware *fw)
+{
+	if (rproc->fw_ops->get_boot_addr)
+		return rproc->fw_ops->get_boot_addr(rproc, fw);
+
+	return 0;
+}
+
+static inline
+int rproc_load_segments(struct rproc *rproc, const struct firmware *fw)
+{
+	if (rproc->fw_ops->load)
+		return rproc->fw_ops->load(rproc, fw);
+
+	return -EINVAL;
+}
+
+static inline
+struct resource_table *rproc_find_rsc_table(struct rproc *rproc,
+				const struct firmware *fw, int *tablesz)
+{
+	if (rproc->fw_ops->find_rsc_table)
+		return rproc->fw_ops->find_rsc_table(rproc, fw, tablesz);
+
+	return NULL;
+}
+
+extern const struct rproc_fw_ops rproc_elf_fw_ops;
+
 #endif /* REMOTEPROC_INTERNAL_H */
diff --git a/drivers/remoteproc/remoteproc_virtio.c b/drivers/remoteproc/remoteproc_virtio.c
index ecf61213075..3541b4492f6 100644
--- a/drivers/remoteproc/remoteproc_virtio.c
+++ b/drivers/remoteproc/remoteproc_virtio.c
@@ -36,7 +36,7 @@ static void rproc_virtio_notify(struct virtqueue *vq)
 	struct rproc *rproc = rvring->rvdev->rproc;
 	int notifyid = rvring->notifyid;
 
-	dev_dbg(rproc->dev, "kicking vq index: %d\n", notifyid);
+	dev_dbg(&rproc->dev, "kicking vq index: %d\n", notifyid);
 
 	rproc->ops->kick(rproc, notifyid);
 }
@@ -57,7 +57,7 @@ irqreturn_t rproc_vq_interrupt(struct rproc *rproc, int notifyid)
 {
 	struct rproc_vring *rvring;
 
-	dev_dbg(rproc->dev, "vq index %d is interrupted\n", notifyid);
+	dev_dbg(&rproc->dev, "vq index %d is interrupted\n", notifyid);
 
 	rvring = idr_find(&rproc->notifyids, notifyid);
 	if (!rvring || !rvring->vq)
@@ -74,17 +74,21 @@ static struct virtqueue *rp_find_vq(struct virtio_device *vdev,
 {
 	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
 	struct rproc *rproc = vdev_to_rproc(vdev);
+	struct device *dev = &rproc->dev;
 	struct rproc_vring *rvring;
 	struct virtqueue *vq;
 	void *addr;
-	int len, size;
+	int len, size, ret;
 
 	/* we're temporarily limited to two virtqueues per rvdev */
 	if (id >= ARRAY_SIZE(rvdev->vring))
 		return ERR_PTR(-EINVAL);
 
-	rvring = &rvdev->vring[id];
+	ret = rproc_alloc_vring(rvdev, id);
+	if (ret)
+		return ERR_PTR(ret);
 
+	rvring = &rvdev->vring[id];
 	addr = rvring->va;
 	len = rvring->len;
 
@@ -92,7 +96,7 @@ static struct virtqueue *rp_find_vq(struct virtio_device *vdev,
 	size = vring_size(len, rvring->align);
 	memset(addr, 0, size);
 
-	dev_dbg(rproc->dev, "vring%d: va %p qsz %d notifyid %d\n",
+	dev_dbg(dev, "vring%d: va %p qsz %d notifyid %d\n",
 					id, addr, len, rvring->notifyid);
 
 	/*
@@ -102,7 +106,8 @@ static struct virtqueue *rp_find_vq(struct virtio_device *vdev,
 	vq = vring_new_virtqueue(len, rvring->align, vdev, false, addr,
 					rproc_virtio_notify, callback, name);
 	if (!vq) {
-		dev_err(rproc->dev, "vring_new_virtqueue %s failed\n", name);
+		dev_err(dev, "vring_new_virtqueue %s failed\n", name);
+		rproc_free_vring(rvring);
 		return ERR_PTR(-ENOMEM);
 	}
 
@@ -125,6 +130,7 @@ static void rproc_virtio_del_vqs(struct virtio_device *vdev)
 		rvring = vq->priv;
 		rvring->vq = NULL;
 		vring_del_virtqueue(vq);
+		rproc_free_vring(rvring);
 	}
 }
 
@@ -147,7 +153,7 @@ static int rproc_virtio_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 	/* now that the vqs are all set, boot the remote processor */
 	ret = rproc_boot(rproc);
 	if (ret) {
-		dev_err(rproc->dev, "rproc_boot() failed %d\n", ret);
+		dev_err(&rproc->dev, "rproc_boot() failed %d\n", ret);
 		goto error;
 	}
 
@@ -219,7 +225,7 @@ static struct virtio_config_ops rproc_virtio_config_ops = {
 
 /*
  * This function is called whenever vdev is released, and is responsible
- * to decrement the remote processor's refcount taken when vdev was
+ * to decrement the remote processor's refcount which was taken when vdev was
  * added.
  *
  * Never call this function directly; it will be called by the driver
@@ -228,9 +234,13 @@ static struct virtio_config_ops rproc_virtio_config_ops = {
 static void rproc_vdev_release(struct device *dev)
 {
 	struct virtio_device *vdev = dev_to_virtio(dev);
+	struct rproc_vdev *rvdev = vdev_to_rvdev(vdev);
 	struct rproc *rproc = vdev_to_rproc(vdev);
 
-	kref_put(&rproc->refcount, rproc_release);
+	list_del(&rvdev->node);
+	kfree(rvdev);
+
+	put_device(&rproc->dev);
 }
 
 /**
@@ -245,7 +255,7 @@ static void rproc_vdev_release(struct device *dev)
 int rproc_add_virtio_dev(struct rproc_vdev *rvdev, int id)
 {
 	struct rproc *rproc = rvdev->rproc;
-	struct device *dev = rproc->dev;
+	struct device *dev = &rproc->dev;
 	struct virtio_device *vdev = &rvdev->vdev;
 	int ret;
 
@@ -262,11 +272,11 @@ int rproc_add_virtio_dev(struct rproc_vdev *rvdev, int id)
 	 * Therefore we must increment the rproc refcount here, and decrement
 	 * it _only_ when the vdev is released.
 	 */
-	kref_get(&rproc->refcount);
+	get_device(&rproc->dev);
 
 	ret = register_virtio_device(vdev);
 	if (ret) {
-		kref_put(&rproc->refcount, rproc_release);
+		put_device(&rproc->dev);
 		dev_err(dev, "failed to register vdev: %d\n", ret);
 		goto out;
 	}
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 39d3aa41add..590cfafc7c1 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -956,7 +956,8 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	vrp->svq = vqs[1];
 
 	/* allocate coherent memory for the buffers */
-	bufs_va = dma_alloc_coherent(vdev->dev.parent, RPMSG_TOTAL_BUF_SPACE,
+	bufs_va = dma_alloc_coherent(vdev->dev.parent->parent,
+				RPMSG_TOTAL_BUF_SPACE,
 				&vrp->bufs_dma, GFP_KERNEL);
 	if (!bufs_va)
 		goto vqs_del;
@@ -1085,7 +1086,7 @@ static int __init rpmsg_init(void)
 
 	return ret;
 }
-module_init(rpmsg_init);
+subsys_initcall(rpmsg_init);
 
 static void __exit rpmsg_fini(void)
 {
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 08cbdb900a1..fabc99a75c6 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -135,6 +135,16 @@ config RTC_DRV_88PM860X
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-88pm860x.
 
+config RTC_DRV_88PM80X
+	tristate "Marvell 88PM80x"
+	depends on RTC_CLASS && I2C && MFD_88PM800
+	help
+	  If you say yes here you get support for RTC function in Marvell
+	  88PM80x chips.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-88pm80x.
+
 config RTC_DRV_DS1307
 	tristate "Dallas/Maxim DS1307/37/38/39/40, ST M41T00, EPSON RX-8025"
 	help
@@ -694,6 +704,7 @@ config RTC_DRV_AB3100
 config RTC_DRV_AB8500
 	tristate "ST-Ericsson AB8500 RTC"
 	depends on AB8500_CORE
+	select RTC_INTF_DEV_UIE_EMUL
 	help
 	  Select this to enable the ST-Ericsson AB8500 power management IC RTC
 	  support. This chip contains a battery- and capacitor-backed RTC.
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 2973921c30d..0d5b2b66f90 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -16,6 +16,7 @@ rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
 # Keep the list ordered.
 
 obj-$(CONFIG_RTC_DRV_88PM860X)  += rtc-88pm860x.o
+obj-$(CONFIG_RTC_DRV_88PM80X)	+= rtc-88pm80x.o
 obj-$(CONFIG_RTC_DRV_AB3100)	+= rtc-ab3100.o
 obj-$(CONFIG_RTC_DRV_AB8500)	+= rtc-ab8500.o
 obj-$(CONFIG_RTC_DRV_AT32AP700X)+= rtc-at32ap700x.o
diff --git a/drivers/rtc/rtc-88pm80x.c b/drivers/rtc/rtc-88pm80x.c
new file mode 100644
index 00000000000..6367984e056
--- /dev/null
+++ b/drivers/rtc/rtc-88pm80x.c
@@ -0,0 +1,369 @@
+/*
+ * Real Time Clock driver for Marvell 88PM80x PMIC
+ *
+ * Copyright (c) 2012 Marvell International Ltd.
+ *  Wenzeng Chen<wzch@marvell.com>
+ *  Qiao Zhou <zhouqiao@marvell.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License. See the file "COPYING" in the main directory of this
+ * archive for more details.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/88pm80x.h>
+#include <linux/rtc.h>
+
+#define PM800_RTC_COUNTER1		(0xD1)
+#define PM800_RTC_COUNTER2		(0xD2)
+#define PM800_RTC_COUNTER3		(0xD3)
+#define PM800_RTC_COUNTER4		(0xD4)
+#define PM800_RTC_EXPIRE1_1		(0xD5)
+#define PM800_RTC_EXPIRE1_2		(0xD6)
+#define PM800_RTC_EXPIRE1_3		(0xD7)
+#define PM800_RTC_EXPIRE1_4		(0xD8)
+#define PM800_RTC_TRIM1			(0xD9)
+#define PM800_RTC_TRIM2			(0xDA)
+#define PM800_RTC_TRIM3			(0xDB)
+#define PM800_RTC_TRIM4			(0xDC)
+#define PM800_RTC_EXPIRE2_1		(0xDD)
+#define PM800_RTC_EXPIRE2_2		(0xDE)
+#define PM800_RTC_EXPIRE2_3		(0xDF)
+#define PM800_RTC_EXPIRE2_4		(0xE0)
+
+#define PM800_POWER_DOWN_LOG1	(0xE5)
+#define PM800_POWER_DOWN_LOG2	(0xE6)
+
+struct pm80x_rtc_info {
+	struct pm80x_chip *chip;
+	struct regmap *map;
+	struct rtc_device *rtc_dev;
+	struct device *dev;
+	struct delayed_work calib_work;
+
+	int irq;
+	int vrtc;
+};
+
+static irqreturn_t rtc_update_handler(int irq, void *data)
+{
+	struct pm80x_rtc_info *info = (struct pm80x_rtc_info *)data;
+	int mask;
+
+	mask = PM800_ALARM | PM800_ALARM_WAKEUP;
+	regmap_update_bits(info->map, PM800_RTC_CONTROL, mask | PM800_ALARM1_EN,
+			   mask);
+	rtc_update_irq(info->rtc_dev, 1, RTC_AF);
+	return IRQ_HANDLED;
+}
+
+static int pm80x_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct pm80x_rtc_info *info = dev_get_drvdata(dev);
+
+	if (enabled)
+		regmap_update_bits(info->map, PM800_RTC_CONTROL,
+				   PM800_ALARM1_EN, PM800_ALARM1_EN);
+	else
+		regmap_update_bits(info->map, PM800_RTC_CONTROL,
+				   PM800_ALARM1_EN, 0);
+	return 0;
+}
+
+/*
+ * Calculate the next alarm time given the requested alarm time mask
+ * and the current time.
+ */
+static void rtc_next_alarm_time(struct rtc_time *next, struct rtc_time *now,
+				struct rtc_time *alrm)
+{
+	unsigned long next_time;
+	unsigned long now_time;
+
+	next->tm_year = now->tm_year;
+	next->tm_mon = now->tm_mon;
+	next->tm_mday = now->tm_mday;
+	next->tm_hour = alrm->tm_hour;
+	next->tm_min = alrm->tm_min;
+	next->tm_sec = alrm->tm_sec;
+
+	rtc_tm_to_time(now, &now_time);
+	rtc_tm_to_time(next, &next_time);
+
+	if (next_time < now_time) {
+		/* Advance one day */
+		next_time += 60 * 60 * 24;
+		rtc_time_to_tm(next_time, next);
+	}
+}
+
+static int pm80x_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct pm80x_rtc_info *info = dev_get_drvdata(dev);
+	unsigned char buf[4];
+	unsigned long ticks, base, data;
+	regmap_raw_read(info->map, PM800_RTC_EXPIRE2_1, buf, 4);
+	base = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	dev_dbg(info->dev, "%x-%x-%x-%x\n", buf[0], buf[1], buf[2], buf[3]);
+
+	/* load 32-bit read-only counter */
+	regmap_raw_read(info->map, PM800_RTC_COUNTER1, buf, 4);
+	data = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	ticks = base + data;
+	dev_dbg(info->dev, "get base:0x%lx, RO count:0x%lx, ticks:0x%lx\n",
+		base, data, ticks);
+	rtc_time_to_tm(ticks, tm);
+	return 0;
+}
+
+static int pm80x_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct pm80x_rtc_info *info = dev_get_drvdata(dev);
+	unsigned char buf[4];
+	unsigned long ticks, base, data;
+	if ((tm->tm_year < 70) || (tm->tm_year > 138)) {
+		dev_dbg(info->dev,
+			"Set time %d out of range. Please set time between 1970 to 2038.\n",
+			1900 + tm->tm_year);
+		return -EINVAL;
+	}
+	rtc_tm_to_time(tm, &ticks);
+
+	/* load 32-bit read-only counter */
+	regmap_raw_read(info->map, PM800_RTC_COUNTER1, buf, 4);
+	data = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	base = ticks - data;
+	dev_dbg(info->dev, "set base:0x%lx, RO count:0x%lx, ticks:0x%lx\n",
+		base, data, ticks);
+	buf[0] = base & 0xFF;
+	buf[1] = (base >> 8) & 0xFF;
+	buf[2] = (base >> 16) & 0xFF;
+	buf[3] = (base >> 24) & 0xFF;
+	regmap_raw_write(info->map, PM800_RTC_EXPIRE2_1, buf, 4);
+
+	return 0;
+}
+
+static int pm80x_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct pm80x_rtc_info *info = dev_get_drvdata(dev);
+	unsigned char buf[4];
+	unsigned long ticks, base, data;
+	int ret;
+
+	regmap_raw_read(info->map, PM800_RTC_EXPIRE2_1, buf, 4);
+	base = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	dev_dbg(info->dev, "%x-%x-%x-%x\n", buf[0], buf[1], buf[2], buf[3]);
+
+	regmap_raw_read(info->map, PM800_RTC_EXPIRE1_1, buf, 4);
+	data = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	ticks = base + data;
+	dev_dbg(info->dev, "get base:0x%lx, RO count:0x%lx, ticks:0x%lx\n",
+		base, data, ticks);
+
+	rtc_time_to_tm(ticks, &alrm->time);
+	regmap_read(info->map, PM800_RTC_CONTROL, &ret);
+	alrm->enabled = (ret & PM800_ALARM1_EN) ? 1 : 0;
+	alrm->pending = (ret & (PM800_ALARM | PM800_ALARM_WAKEUP)) ? 1 : 0;
+	return 0;
+}
+
+static int pm80x_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct pm80x_rtc_info *info = dev_get_drvdata(dev);
+	struct rtc_time now_tm, alarm_tm;
+	unsigned long ticks, base, data;
+	unsigned char buf[4];
+	int mask;
+
+	regmap_update_bits(info->map, PM800_RTC_CONTROL, PM800_ALARM1_EN, 0);
+
+	regmap_raw_read(info->map, PM800_RTC_EXPIRE2_1, buf, 4);
+	base = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	dev_dbg(info->dev, "%x-%x-%x-%x\n", buf[0], buf[1], buf[2], buf[3]);
+
+	/* load 32-bit read-only counter */
+	regmap_raw_read(info->map, PM800_RTC_COUNTER1, buf, 4);
+	data = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	ticks = base + data;
+	dev_dbg(info->dev, "get base:0x%lx, RO count:0x%lx, ticks:0x%lx\n",
+		base, data, ticks);
+
+	rtc_time_to_tm(ticks, &now_tm);
+	dev_dbg(info->dev, "%s, now time : %lu\n", __func__, ticks);
+	rtc_next_alarm_time(&alarm_tm, &now_tm, &alrm->time);
+	/* get new ticks for alarm in 24 hours */
+	rtc_tm_to_time(&alarm_tm, &ticks);
+	dev_dbg(info->dev, "%s, alarm time: %lu\n", __func__, ticks);
+	data = ticks - base;
+
+	buf[0] = data & 0xff;
+	buf[1] = (data >> 8) & 0xff;
+	buf[2] = (data >> 16) & 0xff;
+	buf[3] = (data >> 24) & 0xff;
+	regmap_raw_write(info->map, PM800_RTC_EXPIRE1_1, buf, 4);
+	if (alrm->enabled) {
+		mask = PM800_ALARM | PM800_ALARM_WAKEUP | PM800_ALARM1_EN;
+		regmap_update_bits(info->map, PM800_RTC_CONTROL, mask, mask);
+	} else {
+		mask = PM800_ALARM | PM800_ALARM_WAKEUP | PM800_ALARM1_EN;
+		regmap_update_bits(info->map, PM800_RTC_CONTROL, mask,
+				   PM800_ALARM | PM800_ALARM_WAKEUP);
+	}
+	return 0;
+}
+
+static const struct rtc_class_ops pm80x_rtc_ops = {
+	.read_time = pm80x_rtc_read_time,
+	.set_time = pm80x_rtc_set_time,
+	.read_alarm = pm80x_rtc_read_alarm,
+	.set_alarm = pm80x_rtc_set_alarm,
+	.alarm_irq_enable = pm80x_rtc_alarm_irq_enable,
+};
+
+#ifdef CONFIG_PM
+static int pm80x_rtc_suspend(struct device *dev)
+{
+	return pm80x_dev_suspend(dev);
+}
+
+static int pm80x_rtc_resume(struct device *dev)
+{
+	return pm80x_dev_resume(dev);
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(pm80x_rtc_pm_ops, pm80x_rtc_suspend, pm80x_rtc_resume);
+
+static int __devinit pm80x_rtc_probe(struct platform_device *pdev)
+{
+	struct pm80x_chip *chip = dev_get_drvdata(pdev->dev.parent);
+	struct pm80x_platform_data *pm80x_pdata;
+	struct pm80x_rtc_pdata *pdata = NULL;
+	struct pm80x_rtc_info *info;
+	struct rtc_time tm;
+	unsigned long ticks = 0;
+	int ret;
+
+	pdata = pdev->dev.platform_data;
+	if (pdata == NULL)
+		dev_warn(&pdev->dev, "No platform data!\n");
+
+	info =
+	    devm_kzalloc(&pdev->dev, sizeof(struct pm80x_rtc_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+	info->irq = platform_get_irq(pdev, 0);
+	if (info->irq < 0) {
+		dev_err(&pdev->dev, "No IRQ resource!\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	info->chip = chip;
+	info->map = chip->regmap;
+	if (!info->map) {
+		dev_err(&pdev->dev, "no regmap!\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	info->dev = &pdev->dev;
+	dev_set_drvdata(&pdev->dev, info);
+
+	ret = pm80x_request_irq(chip, info->irq, rtc_update_handler,
+				IRQF_ONESHOT, "rtc", info);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
+			info->irq, ret);
+		goto out;
+	}
+
+	ret = pm80x_rtc_read_time(&pdev->dev, &tm);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to read initial time.\n");
+		goto out_rtc;
+	}
+	if ((tm.tm_year < 70) || (tm.tm_year > 138)) {
+		tm.tm_year = 70;
+		tm.tm_mon = 0;
+		tm.tm_mday = 1;
+		tm.tm_hour = 0;
+		tm.tm_min = 0;
+		tm.tm_sec = 0;
+		ret = pm80x_rtc_set_time(&pdev->dev, &tm);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Failed to set initial time.\n");
+			goto out_rtc;
+		}
+	}
+	rtc_tm_to_time(&tm, &ticks);
+
+	info->rtc_dev = rtc_device_register("88pm80x-rtc", &pdev->dev,
+					    &pm80x_rtc_ops, THIS_MODULE);
+	if (IS_ERR(info->rtc_dev)) {
+		ret = PTR_ERR(info->rtc_dev);
+		dev_err(&pdev->dev, "Failed to register RTC device: %d\n", ret);
+		goto out_rtc;
+	}
+	/*
+	 * enable internal XO instead of internal 3.25MHz clock since it can
+	 * free running in PMIC power-down state.
+	 */
+	regmap_update_bits(info->map, PM800_RTC_CONTROL, PM800_RTC1_USE_XO,
+			   PM800_RTC1_USE_XO);
+
+	if (pdev->dev.parent->platform_data) {
+		pm80x_pdata = pdev->dev.parent->platform_data;
+		pdata = pm80x_pdata->rtc;
+		if (pdata)
+			info->rtc_dev->dev.platform_data = &pdata->rtc_wakeup;
+	}
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	return 0;
+out_rtc:
+	pm80x_free_irq(chip, info->irq, info);
+out:
+	return ret;
+}
+
+static int __devexit pm80x_rtc_remove(struct platform_device *pdev)
+{
+	struct pm80x_rtc_info *info = platform_get_drvdata(pdev);
+	platform_set_drvdata(pdev, NULL);
+	rtc_device_unregister(info->rtc_dev);
+	pm80x_free_irq(info->chip, info->irq, info);
+	return 0;
+}
+
+static struct platform_driver pm80x_rtc_driver = {
+	.driver = {
+		   .name = "88pm80x-rtc",
+		   .owner = THIS_MODULE,
+		   .pm = &pm80x_rtc_pm_ops,
+		   },
+	.probe = pm80x_rtc_probe,
+	.remove = __devexit_p(pm80x_rtc_remove),
+};
+
+module_platform_driver(pm80x_rtc_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Marvell 88PM80x RTC driver");
+MODULE_AUTHOR("Qiao Zhou <zhouqiao@marvell.com>");
+MODULE_ALIAS("platform:88pm80x-rtc");
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index 370889d0489..bf3c2f669c3 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -89,22 +89,17 @@ static int ab8500_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	if (retval < 0)
 		return retval;
 
-	/* Early AB8500 chips will not clear the rtc read request bit */
-	if (abx500_get_chip_id(dev) == 0) {
-		usleep_range(1000, 1000);
-	} else {
-		/* Wait for some cycles after enabling the rtc read in ab8500 */
-		while (time_before(jiffies, timeout)) {
-			retval = abx500_get_register_interruptible(dev,
-				AB8500_RTC, AB8500_RTC_READ_REQ_REG, &value);
-			if (retval < 0)
-				return retval;
-
-			if (!(value & RTC_READ_REQUEST))
-				break;
-
-			usleep_range(1000, 5000);
-		}
+	/* Wait for some cycles after enabling the rtc read in ab8500 */
+	while (time_before(jiffies, timeout)) {
+		retval = abx500_get_register_interruptible(dev,
+			AB8500_RTC, AB8500_RTC_READ_REQ_REG, &value);
+		if (retval < 0)
+			return retval;
+
+		if (!(value & RTC_READ_REQUEST))
+			break;
+
+		usleep_range(1000, 5000);
 	}
 
 	/* Read the Watchtime registers */
@@ -225,7 +220,8 @@ static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
 	int retval, i;
 	unsigned char buf[ARRAY_SIZE(ab8500_rtc_alarm_regs)];
-	unsigned long mins, secs = 0;
+	unsigned long mins, secs = 0, cursec = 0;
+	struct rtc_time curtm;
 
 	if (alarm->time.tm_year < (AB8500_RTC_EPOCH - 1900)) {
 		dev_dbg(dev, "year should be equal to or greater than %d\n",
@@ -237,6 +233,18 @@ static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 	rtc_tm_to_time(&alarm->time, &secs);
 
 	/*
+	 * Check whether alarm is set less than 1min.
+	 * Since our RTC doesn't support alarm resolution less than 1min,
+	 * return -EINVAL, so UIE EMUL can take it up, incase of UIE_ON
+	 */
+	ab8500_rtc_read_time(dev, &curtm); /* Read current time */
+	rtc_tm_to_time(&curtm, &cursec);
+	if ((secs - cursec) < 59) {
+		dev_dbg(dev, "Alarm less than 1 minute not supported\r\n");
+		return -EINVAL;
+	}
+
+	/*
 	 * Convert it to the number of seconds since 01-01-2000 00:00:00, since
 	 * we only have a small counter in the RTC.
 	 */
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index dc474bc6522..fca9790c7de 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -27,6 +27,7 @@
 #include <linux/interrupt.h>
 #include <linux/ioctl.h>
 #include <linux/completion.h>
+#include <linux/io.h>
 
 #include <asm/uaccess.h>
 
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 4267789ca99..132333d7540 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -568,6 +568,7 @@ static irqreturn_t cmos_interrupt(int irq, void *p)
 		hpet_mask_rtc_irq_bit(RTC_AIE);
 
 		CMOS_READ(RTC_INTR_FLAGS);
+		pm_wakeup_event(cmos_rtc.dev, 0);
 	}
 	spin_unlock(&rtc_lock);
 
diff --git a/drivers/rtc/rtc-coh901331.c b/drivers/rtc/rtc-coh901331.c
index a5b8a0c4ea8..76b2156d3c6 100644
--- a/drivers/rtc/rtc-coh901331.c
+++ b/drivers/rtc/rtc-coh901331.c
@@ -155,13 +155,10 @@ static int __exit coh901331_remove(struct platform_device *pdev)
 	struct coh901331_port *rtap = dev_get_drvdata(&pdev->dev);
 
 	if (rtap) {
-		free_irq(rtap->irq, rtap);
 		rtc_device_unregister(rtap->rtc);
+		clk_unprepare(rtap->clk);
 		clk_put(rtap->clk);
-		iounmap(rtap->virtbase);
-		release_mem_region(rtap->phybase, rtap->physize);
 		platform_set_drvdata(pdev, NULL);
-		kfree(rtap);
 	}
 
 	return 0;
@@ -174,49 +171,43 @@ static int __init coh901331_probe(struct platform_device *pdev)
 	struct coh901331_port *rtap;
 	struct resource *res;
 
-	rtap = kzalloc(sizeof(struct coh901331_port), GFP_KERNEL);
+	rtap = devm_kzalloc(&pdev->dev,
+			    sizeof(struct coh901331_port), GFP_KERNEL);
 	if (!rtap)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENOENT;
-		goto out_no_resource;
-	}
+	if (!res)
+		return -ENOENT;
+
 	rtap->phybase = res->start;
 	rtap->physize = resource_size(res);
 
-	if (request_mem_region(rtap->phybase, rtap->physize,
-			       "rtc-coh901331") == NULL) {
-		ret = -EBUSY;
-		goto out_no_memregion;
-	}
+	if (devm_request_mem_region(&pdev->dev, rtap->phybase, rtap->physize,
+				    "rtc-coh901331") == NULL)
+		return -EBUSY;
 
-	rtap->virtbase = ioremap(rtap->phybase, rtap->physize);
-	if (!rtap->virtbase) {
-		ret = -ENOMEM;
-		goto out_no_remap;
-	}
+	rtap->virtbase = devm_ioremap(&pdev->dev, rtap->phybase, rtap->physize);
+	if (!rtap->virtbase)
+		return -ENOMEM;
 
 	rtap->irq = platform_get_irq(pdev, 0);
-	if (request_irq(rtap->irq, coh901331_interrupt, 0,
-			"RTC COH 901 331 Alarm", rtap)) {
-		ret = -EIO;
-		goto out_no_irq;
-	}
+	if (devm_request_irq(&pdev->dev, rtap->irq, coh901331_interrupt, 0,
+			     "RTC COH 901 331 Alarm", rtap))
+		return -EIO;
 
 	rtap->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(rtap->clk)) {
 		ret = PTR_ERR(rtap->clk);
 		dev_err(&pdev->dev, "could not get clock\n");
-		goto out_no_clk;
+		return ret;
 	}
 
 	/* We enable/disable the clock only to assure it works */
-	ret = clk_enable(rtap->clk);
+	ret = clk_prepare_enable(rtap->clk);
 	if (ret) {
 		dev_err(&pdev->dev, "could not enable clock\n");
-		goto out_no_clk_enable;
+		goto out_no_clk_prepenable;
 	}
 	clk_disable(rtap->clk);
 
@@ -232,18 +223,9 @@ static int __init coh901331_probe(struct platform_device *pdev)
 
  out_no_rtc:
 	platform_set_drvdata(pdev, NULL);
- out_no_clk_enable:
+	clk_unprepare(rtap->clk);
+ out_no_clk_prepenable:
 	clk_put(rtap->clk);
- out_no_clk:
-	free_irq(rtap->irq, rtap);
- out_no_irq:
-	iounmap(rtap->virtbase);
- out_no_remap:
-	platform_set_drvdata(pdev, NULL);
- out_no_memregion:
-	release_mem_region(rtap->phybase, SZ_4K);
- out_no_resource:
-	kfree(rtap);
 	return ret;
 }
 
@@ -265,6 +247,7 @@ static int coh901331_suspend(struct platform_device *pdev, pm_message_t state)
 		writel(0, rtap->virtbase + COH901331_IRQ_MASK);
 		clk_disable(rtap->clk);
 	}
+	clk_unprepare(rtap->clk);
 	return 0;
 }
 
@@ -272,6 +255,7 @@ static int coh901331_resume(struct platform_device *pdev)
 {
 	struct coh901331_port *rtap = dev_get_drvdata(&pdev->dev);
 
+	clk_prepare(rtap->clk);
 	if (device_may_wakeup(&pdev->dev)) {
 		disable_irq_wake(rtap->irq);
 	} else {
@@ -293,6 +277,7 @@ static void coh901331_shutdown(struct platform_device *pdev)
 	clk_enable(rtap->clk);
 	writel(0, rtap->virtbase + COH901331_IRQ_MASK);
 	clk_disable(rtap->clk);
+	clk_unprepare(rtap->clk);
 }
 
 static struct platform_driver coh901331_driver = {
diff --git a/drivers/rtc/rtc-da9052.c b/drivers/rtc/rtc-da9052.c
index da6ab5291a4..78070255bd3 100644
--- a/drivers/rtc/rtc-da9052.c
+++ b/drivers/rtc/rtc-da9052.c
@@ -245,7 +245,7 @@ static int __devinit da9052_rtc_probe(struct platform_device *pdev)
 				   "ALM", rtc);
 	if (ret != 0) {
 		rtc_err(rtc->da9052, "irq registration failed: %d\n", ret);
-		goto err_mem;
+		return ret;
 	}
 
 	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
@@ -259,8 +259,6 @@ static int __devinit da9052_rtc_probe(struct platform_device *pdev)
 
 err_free_irq:
 	free_irq(rtc->irq, rtc);
-err_mem:
-	devm_kfree(&pdev->dev, rtc);
 	return ret;
 }
 
@@ -271,7 +269,6 @@ static int __devexit da9052_rtc_remove(struct platform_device *pdev)
 	rtc_device_unregister(rtc->rtc);
 	free_irq(rtc->irq, rtc);
 	platform_set_drvdata(pdev, NULL);
-	devm_kfree(&pdev->dev, rtc);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-max8925.c b/drivers/rtc/rtc-max8925.c
index 1459055a83a..34e4349611d 100644
--- a/drivers/rtc/rtc-max8925.c
+++ b/drivers/rtc/rtc-max8925.c
@@ -69,6 +69,7 @@ struct max8925_rtc_info {
 	struct max8925_chip	*chip;
 	struct i2c_client	*rtc;
 	struct device		*dev;
+	int			irq;
 };
 
 static irqreturn_t rtc_update_handler(int irq, void *data)
@@ -250,7 +251,7 @@ static int __devinit max8925_rtc_probe(struct platform_device *pdev)
 {
 	struct max8925_chip *chip = dev_get_drvdata(pdev->dev.parent);
 	struct max8925_rtc_info *info;
-	int irq, ret;
+	int ret;
 
 	info = kzalloc(sizeof(struct max8925_rtc_info), GFP_KERNEL);
 	if (!info)
@@ -258,13 +259,13 @@ static int __devinit max8925_rtc_probe(struct platform_device *pdev)
 	info->chip = chip;
 	info->rtc = chip->rtc;
 	info->dev = &pdev->dev;
-	irq = chip->irq_base + MAX8925_IRQ_RTC_ALARM0;
+	info->irq = platform_get_irq(pdev, 0);
 
-	ret = request_threaded_irq(irq, NULL, rtc_update_handler,
+	ret = request_threaded_irq(info->irq, NULL, rtc_update_handler,
 				   IRQF_ONESHOT, "rtc-alarm0", info);
 	if (ret < 0) {
 		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
-			irq, ret);
+			info->irq, ret);
 		goto out_irq;
 	}
 
@@ -285,7 +286,7 @@ static int __devinit max8925_rtc_probe(struct platform_device *pdev)
 	return 0;
 out_rtc:
 	platform_set_drvdata(pdev, NULL);
-	free_irq(chip->irq_base + MAX8925_IRQ_RTC_ALARM0, info);
+	free_irq(info->irq, info);
 out_irq:
 	kfree(info);
 	return ret;
@@ -296,7 +297,7 @@ static int __devexit max8925_rtc_remove(struct platform_device *pdev)
 	struct max8925_rtc_info *info = platform_get_drvdata(pdev);
 
 	if (info) {
-		free_irq(info->chip->irq_base + MAX8925_IRQ_RTC_ALARM0, info);
+		free_irq(info->irq, info);
 		rtc_device_unregister(info->rtc_dev);
 		kfree(info);
 	}
diff --git a/drivers/rtc/rtc-mc13xxx.c b/drivers/rtc/rtc-mc13xxx.c
index 546f6850bff..2643d887492 100644
--- a/drivers/rtc/rtc-mc13xxx.c
+++ b/drivers/rtc/rtc-mc13xxx.c
@@ -404,9 +404,12 @@ static const struct platform_device_id mc13xxx_rtc_idtable[] = {
 		.name = "mc13783-rtc",
 	}, {
 		.name = "mc13892-rtc",
+	}, {
+		.name = "mc34708-rtc",
 	},
-	{ }
+	{ /* sentinel */ }
 };
+MODULE_DEVICE_TABLE(platform, mc13xxx_rtc_idtable);
 
 static struct platform_driver mc13xxx_rtc_driver = {
 	.id_table = mc13xxx_rtc_idtable,
@@ -432,4 +435,3 @@ module_exit(mc13xxx_rtc_exit);
 MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
 MODULE_DESCRIPTION("RTC driver for Freescale MC13XXX PMIC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
index b2185f4255a..ebc1649d45d 100644
--- a/drivers/rtc/rtc-mv.c
+++ b/drivers/rtc/rtc-mv.c
@@ -297,7 +297,7 @@ static int __exit mv_rtc_remove(struct platform_device *pdev)
 
 #ifdef CONFIG_OF
 static struct of_device_id rtc_mv_of_match_table[] = {
-	{ .compatible = "mrvl,orion-rtc", },
+	{ .compatible = "marvell,orion-rtc", },
 	{}
 };
 #endif
diff --git a/drivers/rtc/rtc-pcf8563.c b/drivers/rtc/rtc-pcf8563.c
index 97a3284bb7c..c2fe426a6ef 100644
--- a/drivers/rtc/rtc-pcf8563.c
+++ b/drivers/rtc/rtc-pcf8563.c
@@ -19,6 +19,7 @@
 #include <linux/rtc.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/of.h>
 
 #define DRV_VERSION "0.4.3"
 
@@ -285,9 +286,19 @@ static const struct i2c_device_id pcf8563_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, pcf8563_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id pcf8563_of_match[] __devinitconst = {
+	{ .compatible = "nxp,pcf8563" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, pcf8563_of_match);
+#endif
+
 static struct i2c_driver pcf8563_driver = {
 	.driver		= {
 		.name	= "rtc-pcf8563",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(pcf8563_of_match),
 	},
 	.probe		= pcf8563_probe,
 	.remove		= pcf8563_remove,
diff --git a/drivers/rtc/rtc-pl031.c b/drivers/rtc/rtc-pl031.c
index cc0533994f6..08378e3cc21 100644
--- a/drivers/rtc/rtc-pl031.c
+++ b/drivers/rtc/rtc-pl031.c
@@ -68,11 +68,26 @@
 
 #define RTC_TIMER_FREQ 32768
 
+/**
+ * struct pl031_vendor_data - per-vendor variations
+ * @ops: the vendor-specific operations used on this silicon version
+ * @clockwatch: if this is an ST Microelectronics silicon version with a
+ *	clockwatch function
+ * @st_weekday: if this is an ST Microelectronics silicon version that need
+ *	the weekday fix
+ * @irqflags: special IRQ flags per variant
+ */
+struct pl031_vendor_data {
+	struct rtc_class_ops ops;
+	bool clockwatch;
+	bool st_weekday;
+	unsigned long irqflags;
+};
+
 struct pl031_local {
+	struct pl031_vendor_data *vendor;
 	struct rtc_device *rtc;
 	void __iomem *base;
-	u8 hw_designer;
-	u8 hw_revision:4;
 };
 
 static int pl031_alarm_irq_enable(struct device *dev,
@@ -303,7 +318,8 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	int ret;
 	struct pl031_local *ldata;
-	struct rtc_class_ops *ops = id->data;
+	struct pl031_vendor_data *vendor = id->data;
+	struct rtc_class_ops *ops = &vendor->ops;
 	unsigned long time;
 
 	ret = amba_request_regions(adev, NULL);
@@ -315,6 +331,7 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 		ret = -ENOMEM;
 		goto out;
 	}
+	ldata->vendor = vendor;
 
 	ldata->base = ioremap(adev->res.start, resource_size(&adev->res));
 
@@ -325,14 +342,11 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 
 	amba_set_drvdata(adev, ldata);
 
-	ldata->hw_designer = amba_manf(adev);
-	ldata->hw_revision = amba_rev(adev);
-
-	dev_dbg(&adev->dev, "designer ID = 0x%02x\n", ldata->hw_designer);
-	dev_dbg(&adev->dev, "revision = 0x%01x\n", ldata->hw_revision);
+	dev_dbg(&adev->dev, "designer ID = 0x%02x\n", amba_manf(adev));
+	dev_dbg(&adev->dev, "revision = 0x%01x\n", amba_rev(adev));
 
 	/* Enable the clockwatch on ST Variants */
-	if (ldata->hw_designer == AMBA_VENDOR_ST)
+	if (vendor->clockwatch)
 		writel(readl(ldata->base + RTC_CR) | RTC_CR_CWEN,
 		       ldata->base + RTC_CR);
 
@@ -340,7 +354,7 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 	 * On ST PL031 variants, the RTC reset value does not provide correct
 	 * weekday for 2000-01-01. Correct the erroneous sunday to saturday.
 	 */
-	if (ldata->hw_designer == AMBA_VENDOR_ST) {
+	if (vendor->st_weekday) {
 		if (readl(ldata->base + RTC_YDR) == 0x2000) {
 			time = readl(ldata->base + RTC_DR);
 			if ((time &
@@ -361,7 +375,7 @@ static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
 	}
 
 	if (request_irq(adev->irq[0], pl031_interrupt,
-			0, "rtc-pl031", ldata)) {
+			vendor->irqflags, "rtc-pl031", ldata)) {
 		ret = -EIO;
 		goto out_no_irq;
 	}
@@ -383,48 +397,65 @@ err_req:
 }
 
 /* Operations for the original ARM version */
-static struct rtc_class_ops arm_pl031_ops = {
-	.read_time = pl031_read_time,
-	.set_time = pl031_set_time,
-	.read_alarm = pl031_read_alarm,
-	.set_alarm = pl031_set_alarm,
-	.alarm_irq_enable = pl031_alarm_irq_enable,
+static struct pl031_vendor_data arm_pl031 = {
+	.ops = {
+		.read_time = pl031_read_time,
+		.set_time = pl031_set_time,
+		.read_alarm = pl031_read_alarm,
+		.set_alarm = pl031_set_alarm,
+		.alarm_irq_enable = pl031_alarm_irq_enable,
+	},
+	.irqflags = IRQF_NO_SUSPEND,
 };
 
 /* The First ST derivative */
-static struct rtc_class_ops stv1_pl031_ops = {
-	.read_time = pl031_read_time,
-	.set_time = pl031_set_time,
-	.read_alarm = pl031_read_alarm,
-	.set_alarm = pl031_set_alarm,
-	.alarm_irq_enable = pl031_alarm_irq_enable,
+static struct pl031_vendor_data stv1_pl031 = {
+	.ops = {
+		.read_time = pl031_read_time,
+		.set_time = pl031_set_time,
+		.read_alarm = pl031_read_alarm,
+		.set_alarm = pl031_set_alarm,
+		.alarm_irq_enable = pl031_alarm_irq_enable,
+	},
+	.clockwatch = true,
+	.st_weekday = true,
+	.irqflags = IRQF_NO_SUSPEND,
 };
 
 /* And the second ST derivative */
-static struct rtc_class_ops stv2_pl031_ops = {
-	.read_time = pl031_stv2_read_time,
-	.set_time = pl031_stv2_set_time,
-	.read_alarm = pl031_stv2_read_alarm,
-	.set_alarm = pl031_stv2_set_alarm,
-	.alarm_irq_enable = pl031_alarm_irq_enable,
+static struct pl031_vendor_data stv2_pl031 = {
+	.ops = {
+		.read_time = pl031_stv2_read_time,
+		.set_time = pl031_stv2_set_time,
+		.read_alarm = pl031_stv2_read_alarm,
+		.set_alarm = pl031_stv2_set_alarm,
+		.alarm_irq_enable = pl031_alarm_irq_enable,
+	},
+	.clockwatch = true,
+	.st_weekday = true,
+	/*
+	 * This variant shares the IRQ with another block and must not
+	 * suspend that IRQ line.
+	 */
+	.irqflags = IRQF_SHARED | IRQF_NO_SUSPEND,
 };
 
 static struct amba_id pl031_ids[] = {
 	{
 		.id = 0x00041031,
 		.mask = 0x000fffff,
-		.data = &arm_pl031_ops,
+		.data = &arm_pl031,
 	},
 	/* ST Micro variants */
 	{
 		.id = 0x00180031,
 		.mask = 0x00ffffff,
-		.data = &stv1_pl031_ops,
+		.data = &stv1_pl031,
 	},
 	{
 		.id = 0x00280031,
 		.mask = 0x00ffffff,
-		.data = &stv2_pl031_ops,
+		.data = &stv2_pl031,
 	},
 	{0, 0},
 };
diff --git a/drivers/rtc/rtc-r9701.c b/drivers/rtc/rtc-r9701.c
index 33b6ba0afa0..2c183ebff71 100644
--- a/drivers/rtc/rtc-r9701.c
+++ b/drivers/rtc/rtc-r9701.c
@@ -138,8 +138,7 @@ static int __devinit r9701_probe(struct spi_device *spi)
 	 * contain invalid values. If so, try to write a default date:
 	 * 2000/1/1 00:00:00
 	 */
-	r9701_get_datetime(&spi->dev, &dt);
-	if (rtc_valid_tm(&dt)) {
+	if (r9701_get_datetime(&spi->dev, &dt)) {
 		dev_info(&spi->dev, "trying to repair invalid date/time\n");
 		dt.tm_sec  = 0;
 		dt.tm_min  = 0;
@@ -148,7 +147,8 @@ static int __devinit r9701_probe(struct spi_device *spi)
 		dt.tm_mon  = 0;
 		dt.tm_year = 100;
 
-		if (r9701_set_datetime(&spi->dev, &dt)) {
+		if (r9701_set_datetime(&spi->dev, &dt) ||
+				r9701_get_datetime(&spi->dev, &dt)) {
 			dev_err(&spi->dev, "cannot repair RTC register\n");
 			return -ENODEV;
 		}
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 7e6af0b22f1..bfbd92c8d1c 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -26,10 +26,10 @@
 #include <linux/log2.h>
 #include <linux/slab.h>
 #include <linux/of.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
 
 #include <mach/hardware.h>
-#include <asm/uaccess.h>
-#include <asm/io.h>
 #include <asm/irq.h>
 #include <plat/regs-rtc.h>
 
diff --git a/drivers/rtc/rtc-stmp3xxx.c b/drivers/rtc/rtc-stmp3xxx.c
index 10287865e33..739ef55694f 100644
--- a/drivers/rtc/rtc-stmp3xxx.c
+++ b/drivers/rtc/rtc-stmp3xxx.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/rtc.h>
 #include <linux/slab.h>
+#include <linux/of_device.h>
 
 #include <mach/common.h>
 
@@ -265,6 +266,12 @@ static int stmp3xxx_rtc_resume(struct platform_device *dev)
 #define stmp3xxx_rtc_resume	NULL
 #endif
 
+static const struct of_device_id rtc_dt_ids[] = {
+	{ .compatible = "fsl,stmp3xxx-rtc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, rtc_dt_ids);
+
 static struct platform_driver stmp3xxx_rtcdrv = {
 	.probe		= stmp3xxx_rtc_probe,
 	.remove		= stmp3xxx_rtc_remove,
@@ -273,6 +280,7 @@ static struct platform_driver stmp3xxx_rtcdrv = {
 	.driver		= {
 		.name	= "stmp3xxx-rtc",
 		.owner	= THIS_MODULE,
+		.of_match_table = rtc_dt_ids,
 	},
 };
 
diff --git a/drivers/rtc/rtc-wm831x.c b/drivers/rtc/rtc-wm831x.c
index 59c6245e042..ea5c6f857ca 100644
--- a/drivers/rtc/rtc-wm831x.c
+++ b/drivers/rtc/rtc-wm831x.c
@@ -24,7 +24,7 @@
 #include <linux/mfd/wm831x/core.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
-
+#include <linux/random.h>
 
 /*
  * R16416 (0x4020) - RTC Write Counter
@@ -96,6 +96,26 @@ struct wm831x_rtc {
 	unsigned int alarm_enabled:1;
 };
 
+static void wm831x_rtc_add_randomness(struct wm831x *wm831x)
+{
+	int ret;
+	u16 reg;
+
+	/*
+	 * The write counter contains a pseudo-random number which is
+	 * regenerated every time we set the RTC so it should be a
+	 * useful per-system source of entropy.
+	 */
+	ret = wm831x_reg_read(wm831x, WM831X_RTC_WRITE_COUNTER);
+	if (ret >= 0) {
+		reg = ret;
+		add_device_randomness(&reg, sizeof(reg));
+	} else {
+		dev_warn(wm831x->dev, "Failed to read RTC write counter: %d\n",
+			 ret);
+	}
+}
+
 /*
  * Read current time and date in RTC
  */
@@ -431,6 +451,8 @@ static int wm831x_rtc_probe(struct platform_device *pdev)
 			alm_irq, ret);
 	}
 
+	wm831x_rtc_add_randomness(wm831x);
+
 	return 0;
 
 err:
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index f3509120a50..15370a2c5ff 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -1,5 +1,4 @@
 /*
- * File...........: linux/drivers/s390/block/dasd.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
@@ -52,7 +51,7 @@ void dasd_int_handler(struct ccw_device *, unsigned long, struct irb *);
 
 MODULE_AUTHOR("Holger Smolinski <Holger.Smolinski@de.ibm.com>");
 MODULE_DESCRIPTION("Linux on S/390 DASD device driver,"
-		   " Copyright 2000 IBM Corporation");
+		   " Copyright IBM Corp. 2000");
 MODULE_SUPPORTED_DEVICE("dasd");
 MODULE_LICENSE("GPL");
 
@@ -82,6 +81,7 @@ static void dasd_profile_exit(struct dasd_profile *);
 static wait_queue_head_t dasd_init_waitq;
 static wait_queue_head_t dasd_flush_wq;
 static wait_queue_head_t generic_waitq;
+static wait_queue_head_t shutdown_waitq;
 
 /*
  * Allocate memory for a new device structure.
@@ -1994,6 +1994,8 @@ static void dasd_device_tasklet(struct dasd_device *device)
 	/* Now check if the head of the ccw queue needs to be started. */
 	__dasd_device_start_head(device);
 	spin_unlock_irq(get_ccwdev_lock(device->cdev));
+	if (waitqueue_active(&shutdown_waitq))
+		wake_up(&shutdown_waitq);
 	dasd_put_device(device);
 }
 
@@ -2632,6 +2634,8 @@ static void dasd_block_tasklet(struct dasd_block *block)
 	__dasd_block_start_head(block);
 	spin_unlock(&block->queue_lock);
 	spin_unlock_irq(&block->request_queue_lock);
+	if (waitqueue_active(&shutdown_waitq))
+		wake_up(&shutdown_waitq);
 	dasd_put_device(block->base);
 }
 
@@ -3474,6 +3478,32 @@ char *dasd_get_sense(struct irb *irb)
 }
 EXPORT_SYMBOL_GPL(dasd_get_sense);
 
+static inline int _wait_for_empty_queues(struct dasd_device *device)
+{
+	if (device->block)
+		return list_empty(&device->ccw_queue) &&
+			list_empty(&device->block->ccw_queue);
+	else
+		return list_empty(&device->ccw_queue);
+}
+
+void dasd_generic_shutdown(struct ccw_device *cdev)
+{
+	struct dasd_device *device;
+
+	device = dasd_device_from_cdev(cdev);
+	if (IS_ERR(device))
+		return;
+
+	if (device->block)
+		dasd_schedule_block_bh(device->block);
+
+	dasd_schedule_device_bh(device);
+
+	wait_event(shutdown_waitq, _wait_for_empty_queues(device));
+}
+EXPORT_SYMBOL_GPL(dasd_generic_shutdown);
+
 static int __init dasd_init(void)
 {
 	int rc;
@@ -3481,6 +3511,7 @@ static int __init dasd_init(void)
 	init_waitqueue_head(&dasd_init_waitq);
 	init_waitqueue_head(&dasd_flush_wq);
 	init_waitqueue_head(&generic_waitq);
+	init_waitqueue_head(&shutdown_waitq);
 
 	/* register 'common' DASD debug area, used for all DBF_XXX calls */
 	dasd_debug_area = debug_register("dasd", 1, 1, 8 * sizeof(long));
diff --git a/drivers/s390/block/dasd_3990_erp.c b/drivers/s390/block/dasd_3990_erp.c
index 0326571e7ff..f8212d54013 100644
--- a/drivers/s390/block/dasd_3990_erp.c
+++ b/drivers/s390/block/dasd_3990_erp.c
@@ -1,9 +1,8 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_3990_erp.c
  * Author(s)......: Horst  Hummel    <Horst.Hummel@de.ibm.com>
  *		    Holger Smolinski <Holger.Smolinski@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 2000, 2001
+ * Copyright IBM Corp. 2000, 2001
  *
  */
 
diff --git a/drivers/s390/block/dasd_alias.c b/drivers/s390/block/dasd_alias.c
index b3beed5434e..157defe5e06 100644
--- a/drivers/s390/block/dasd_alias.c
+++ b/drivers/s390/block/dasd_alias.c
@@ -1,7 +1,7 @@
 /*
  * PAV alias management for the DASD ECKD discipline
  *
- * Copyright IBM Corporation, 2007
+ * Copyright IBM Corp. 2007
  * Author(s): Stefan Weinhuber <wein@de.ibm.com>
  */
 
diff --git a/drivers/s390/block/dasd_devmap.c b/drivers/s390/block/dasd_devmap.c
index d71511c7850..b2b8c18eece 100644
--- a/drivers/s390/block/dasd_devmap.c
+++ b/drivers/s390/block/dasd_devmap.c
@@ -1,11 +1,10 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_devmap.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
  *		    Martin Schwidefsky <schwidefsky@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999-2001
+ * Copyright IBM Corp. 1999,2001
  *
  * Device mapping and dasd= parameter parsing functions. All devmap
  * functions may not be called from interrupt context. In particular
diff --git a/drivers/s390/block/dasd_diag.c b/drivers/s390/block/dasd_diag.c
index 0cea7e98f46..9bd5da36f99 100644
--- a/drivers/s390/block/dasd_diag.c
+++ b/drivers/s390/block/dasd_diag.c
@@ -1,10 +1,9 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_diag.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  * Based on.......: linux/drivers/s390/block/mdisk.c
  * ...............: by Hartmunt Penner <hpenner@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999,2000
+ * Copyright IBM Corp. 1999, 2000
  *
  */
 
diff --git a/drivers/s390/block/dasd_diag.h b/drivers/s390/block/dasd_diag.h
index 4f71fbe60c8..a803cc73158 100644
--- a/drivers/s390/block/dasd_diag.h
+++ b/drivers/s390/block/dasd_diag.h
@@ -1,10 +1,9 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_diag.h
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  * Based on.......: linux/drivers/s390/block/mdisk.h
  * ...............: by Hartmunt Penner <hpenner@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999,2000
+ * Copyright IBM Corp. 1999, 2000
  *
  */
 
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index bc2e8a7c265..40a826a7295 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -1,5 +1,4 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_eckd.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
@@ -4247,6 +4246,7 @@ static struct ccw_driver dasd_eckd_driver = {
 	.set_online  = dasd_eckd_set_online,
 	.notify      = dasd_generic_notify,
 	.path_event  = dasd_generic_path_event,
+	.shutdown    = dasd_generic_shutdown,
 	.freeze      = dasd_generic_pm_freeze,
 	.thaw	     = dasd_generic_restore_device,
 	.restore     = dasd_generic_restore_device,
diff --git a/drivers/s390/block/dasd_eckd.h b/drivers/s390/block/dasd_eckd.h
index 4a688a873a7..2555e494591 100644
--- a/drivers/s390/block/dasd_eckd.h
+++ b/drivers/s390/block/dasd_eckd.h
@@ -1,9 +1,8 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_eckd.h
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999,2000
+ * Copyright IBM Corp. 1999, 2000
  *
  */
 
diff --git a/drivers/s390/block/dasd_eer.c b/drivers/s390/block/dasd_eer.c
index 16c5208c3dc..ff901b5509c 100644
--- a/drivers/s390/block/dasd_eer.c
+++ b/drivers/s390/block/dasd_eer.c
@@ -1,7 +1,7 @@
 /*
  *  Character device driver for extended error reporting.
  *
- *  Copyright (C) 2005 IBM Corporation
+ *  Copyright IBM Corp. 2005
  *  extended error reporting for DASD ECKD devices
  *  Author(s): Stefan Weinhuber <wein@de.ibm.com>
  */
diff --git a/drivers/s390/block/dasd_erp.c b/drivers/s390/block/dasd_erp.c
index 0eafe2e421e..d01ef82f875 100644
--- a/drivers/s390/block/dasd_erp.c
+++ b/drivers/s390/block/dasd_erp.c
@@ -1,11 +1,10 @@
 /*
- * File...........: linux/drivers/s390/block/dasd.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
  *		    Martin Schwidefsky <schwidefsky@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999-2001
+ * Copyright IBM Corp. 1999, 2001
  *
  */
 
diff --git a/drivers/s390/block/dasd_fba.c b/drivers/s390/block/dasd_fba.c
index a62a75358eb..fb7f3bdc660 100644
--- a/drivers/s390/block/dasd_fba.c
+++ b/drivers/s390/block/dasd_fba.c
@@ -1,5 +1,4 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_fba.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
  * Copyright IBM Corp. 1999, 2009
diff --git a/drivers/s390/block/dasd_fba.h b/drivers/s390/block/dasd_fba.h
index 14c910baa5f..b5d3db0e5ef 100644
--- a/drivers/s390/block/dasd_fba.h
+++ b/drivers/s390/block/dasd_fba.h
@@ -1,8 +1,7 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_fba.h
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999,2000
+ * Coypright IBM Corp. 1999, 2000
  *
  */
 
diff --git a/drivers/s390/block/dasd_genhd.c b/drivers/s390/block/dasd_genhd.c
index 19a1ff03d65..f64921756ad 100644
--- a/drivers/s390/block/dasd_genhd.c
+++ b/drivers/s390/block/dasd_genhd.c
@@ -1,11 +1,10 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_genhd.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
  *		    Martin Schwidefsky <schwidefsky@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999-2001
+ * Copyright IBM Corp. 1999, 2001
  *
  * gendisk related functions for the dasd driver.
  *
diff --git a/drivers/s390/block/dasd_int.h b/drivers/s390/block/dasd_int.h
index c05da00583f..7ff93eea673 100644
--- a/drivers/s390/block/dasd_int.h
+++ b/drivers/s390/block/dasd_int.h
@@ -1,5 +1,4 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_int.h
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Martin Schwidefsky <schwidefsky@de.ibm.com>
@@ -686,6 +685,7 @@ int dasd_generic_set_offline (struct ccw_device *cdev);
 int dasd_generic_notify(struct ccw_device *, int);
 int dasd_generic_last_path_gone(struct dasd_device *);
 int dasd_generic_path_operational(struct dasd_device *);
+void dasd_generic_shutdown(struct ccw_device *);
 
 void dasd_generic_handle_state_change(struct dasd_device *);
 int dasd_generic_pm_freeze(struct ccw_device *);
diff --git a/drivers/s390/block/dasd_ioctl.c b/drivers/s390/block/dasd_ioctl.c
index 792c69e78fe..cceae70279f 100644
--- a/drivers/s390/block/dasd_ioctl.c
+++ b/drivers/s390/block/dasd_ioctl.c
@@ -1,11 +1,10 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_ioctl.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
  *		    Martin Schwidefsky <schwidefsky@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999-2001
+ * Copyright IBM Corp. 1999, 2001
  *
  * i/o controls for the dasd driver.
  */
diff --git a/drivers/s390/block/dasd_proc.c b/drivers/s390/block/dasd_proc.c
index e12989fff4f..78ac905a5b7 100644
--- a/drivers/s390/block/dasd_proc.c
+++ b/drivers/s390/block/dasd_proc.c
@@ -1,11 +1,10 @@
 /*
- * File...........: linux/drivers/s390/block/dasd_proc.c
  * Author(s)......: Holger Smolinski <Holger.Smolinski@de.ibm.com>
  *		    Horst Hummel <Horst.Hummel@de.ibm.com>
  *		    Carsten Otte <Cotte@de.ibm.com>
  *		    Martin Schwidefsky <schwidefsky@de.ibm.com>
  * Bugreports.to..: <Linux390@de.ibm.com>
- * (C) IBM Corporation, IBM Deutschland Entwicklung GmbH, 1999-2002
+ * Coypright IBM Corp. 1999, 2002
  *
  * /proc interface for the dasd driver.
  *
diff --git a/drivers/s390/char/ctrlchar.c b/drivers/s390/char/ctrlchar.c
index 0e9a309b966..8de2deb176d 100644
--- a/drivers/s390/char/ctrlchar.c
+++ b/drivers/s390/char/ctrlchar.c
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/char/ctrlchar.c
  *  Unified handling of special chars.
  *
- *    Copyright (C) 2001 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 2001
  *    Author(s): Fritz Elfert <felfert@millenux.com> <elfert@de.ibm.com>
  *
  */
diff --git a/drivers/s390/char/ctrlchar.h b/drivers/s390/char/ctrlchar.h
index 935ffa0ea7c..1a53552f498 100644
--- a/drivers/s390/char/ctrlchar.h
+++ b/drivers/s390/char/ctrlchar.h
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/char/ctrlchar.c
  *  Unified handling of special chars.
  *
- *    Copyright (C) 2001 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 2001
  *    Author(s): Fritz Elfert <felfert@millenux.com> <elfert@de.ibm.com>
  *
  */
diff --git a/drivers/s390/char/keyboard.c b/drivers/s390/char/keyboard.c
index 7ef9cfdc17d..01463b052ae 100644
--- a/drivers/s390/char/keyboard.c
+++ b/drivers/s390/char/keyboard.c
@@ -1,9 +1,8 @@
 /*
- *  drivers/s390/char/keyboard.c
  *    ebcdic keycode functions for s390 console drivers
  *
  *  S390 version
- *    Copyright (C) 2003 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 2003
  *    Author(s): Martin Schwidefsky (schwidefsky@de.ibm.com),
  */
 
diff --git a/drivers/s390/char/keyboard.h b/drivers/s390/char/keyboard.h
index f682f4e4968..d0ae2be5819 100644
--- a/drivers/s390/char/keyboard.h
+++ b/drivers/s390/char/keyboard.h
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/char/keyboard.h
  *    ebcdic keycode functions for s390 console drivers
  *
- *    Copyright (C) 2003 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 2003
  *    Author(s): Martin Schwidefsky (schwidefsky@de.ibm.com),
  */
 
diff --git a/drivers/s390/char/sclp.c b/drivers/s390/char/sclp.c
index 30f29a0020a..3fcc000efc5 100644
--- a/drivers/s390/char/sclp.c
+++ b/drivers/s390/char/sclp.c
@@ -654,16 +654,6 @@ sclp_remove_processed(struct sccb_header *sccb)
 
 EXPORT_SYMBOL(sclp_remove_processed);
 
-struct init_sccb {
-	struct sccb_header header;
-	u16 _reserved;
-	u16 mask_length;
-	sccb_mask_t receive_mask;
-	sccb_mask_t send_mask;
-	sccb_mask_t sclp_receive_mask;
-	sccb_mask_t sclp_send_mask;
-} __attribute__((packed));
-
 /* Prepare init mask request. Called while sclp_lock is locked. */
 static inline void
 __sclp_make_init_req(u32 receive_mask, u32 send_mask)
diff --git a/drivers/s390/char/sclp.h b/drivers/s390/char/sclp.h
index 49a1bb52bc8..d7e97ae9ef6 100644
--- a/drivers/s390/char/sclp.h
+++ b/drivers/s390/char/sclp.h
@@ -88,6 +88,16 @@ struct sccb_header {
 	u16	response_code;
 } __attribute__((packed));
 
+struct init_sccb {
+	struct sccb_header header;
+	u16 _reserved;
+	u16 mask_length;
+	sccb_mask_t receive_mask;
+	sccb_mask_t send_mask;
+	sccb_mask_t sclp_receive_mask;
+	sccb_mask_t sclp_send_mask;
+} __attribute__((packed));
+
 extern u64 sclp_facilities;
 #define SCLP_HAS_CHP_INFO	(sclp_facilities & 0x8000000000000000ULL)
 #define SCLP_HAS_CHP_RECONFIG	(sclp_facilities & 0x2000000000000000ULL)
diff --git a/drivers/s390/char/sclp_cmd.c b/drivers/s390/char/sclp_cmd.c
index 766cb7b19b4..71ea923c322 100644
--- a/drivers/s390/char/sclp_cmd.c
+++ b/drivers/s390/char/sclp_cmd.c
@@ -48,6 +48,7 @@ struct read_info_sccb {
 	u8	_reserved5[4096 - 112];	/* 112-4095 */
 } __attribute__((packed, aligned(PAGE_SIZE)));
 
+static struct init_sccb __initdata early_event_mask_sccb __aligned(PAGE_SIZE);
 static struct read_info_sccb __initdata early_read_info_sccb;
 static int __initdata early_read_info_sccb_valid;
 
@@ -104,6 +105,19 @@ static void __init sclp_read_info_early(void)
 	}
 }
 
+static void __init sclp_event_mask_early(void)
+{
+	struct init_sccb *sccb = &early_event_mask_sccb;
+	int rc;
+
+	do {
+		memset(sccb, 0, sizeof(*sccb));
+		sccb->header.length = sizeof(*sccb);
+		sccb->mask_length = sizeof(sccb_mask_t);
+		rc = sclp_cmd_sync_early(SCLP_CMDW_WRITE_EVENT_MASK, sccb);
+	} while (rc == -EBUSY);
+}
+
 void __init sclp_facilities_detect(void)
 {
 	struct read_info_sccb *sccb;
@@ -119,6 +133,30 @@ void __init sclp_facilities_detect(void)
 	rnmax = sccb->rnmax ? sccb->rnmax : sccb->rnmax2;
 	rzm = sccb->rnsize ? sccb->rnsize : sccb->rnsize2;
 	rzm <<= 20;
+
+	sclp_event_mask_early();
+}
+
+bool __init sclp_has_linemode(void)
+{
+	struct init_sccb *sccb = &early_event_mask_sccb;
+
+	if (sccb->header.response_code != 0x20)
+		return 0;
+	if (sccb->sclp_send_mask & (EVTYP_MSG_MASK | EVTYP_PMSGCMD_MASK))
+		return 1;
+	return 0;
+}
+
+bool __init sclp_has_vt220(void)
+{
+	struct init_sccb *sccb = &early_event_mask_sccb;
+
+	if (sccb->header.response_code != 0x20)
+		return 0;
+	if (sccb->sclp_send_mask & EVTYP_VT220MSG_MASK)
+		return 1;
+	return 0;
 }
 
 unsigned long long sclp_get_rnmax(void)
diff --git a/drivers/s390/char/sclp_config.c b/drivers/s390/char/sclp_config.c
index 3c03c1060be..444d36183a2 100644
--- a/drivers/s390/char/sclp_config.c
+++ b/drivers/s390/char/sclp_config.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/char/sclp_config.c
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Heiko Carstens <heiko.carstens@de.ibm.com>
  */
diff --git a/drivers/s390/char/sclp_cpi.c b/drivers/s390/char/sclp_cpi.c
index 5716487b8c9..d70d8c20229 100644
--- a/drivers/s390/char/sclp_cpi.c
+++ b/drivers/s390/char/sclp_cpi.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/sclp_cpi.c
  *    SCLP control programm identification
  *
  *    Copyright IBM Corp. 2001, 2007
diff --git a/drivers/s390/char/sclp_cpi_sys.c b/drivers/s390/char/sclp_cpi_sys.c
index bd1b9c91905..2acea809e2a 100644
--- a/drivers/s390/char/sclp_cpi_sys.c
+++ b/drivers/s390/char/sclp_cpi_sys.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/sclp_cpi_sys.c
  *    SCLP control program identification sysfs interface
  *
  *    Copyright IBM Corp. 2001, 2007
diff --git a/drivers/s390/char/sclp_cpi_sys.h b/drivers/s390/char/sclp_cpi_sys.h
index deef3e6ff49..65bb6a99c97 100644
--- a/drivers/s390/char/sclp_cpi_sys.h
+++ b/drivers/s390/char/sclp_cpi_sys.h
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/sclp_cpi_sys.h
  *    SCLP control program identification sysfs interface
  *
  *    Copyright IBM Corp. 2007
diff --git a/drivers/s390/char/sclp_ocf.c b/drivers/s390/char/sclp_ocf.c
index ab294d5a534..2553db0fdb5 100644
--- a/drivers/s390/char/sclp_ocf.c
+++ b/drivers/s390/char/sclp_ocf.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/sclp_ocf.c
  *    SCLP OCF communication parameters sysfs interface
  *
  *    Copyright IBM Corp. 2011
diff --git a/drivers/s390/char/sclp_quiesce.c b/drivers/s390/char/sclp_quiesce.c
index 69df137310b..475e470d976 100644
--- a/drivers/s390/char/sclp_quiesce.c
+++ b/drivers/s390/char/sclp_quiesce.c
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/char/sclp_quiesce.c
  *     signal quiesce handler
  *
- *  (C) Copyright IBM Corp. 1999,2004
+ *  Copyright IBM Corp. 1999, 2004
  *  Author(s): Martin Schwidefsky <schwidefsky@de.ibm.com>
  *             Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
  */
diff --git a/drivers/s390/char/sclp_sdias.c b/drivers/s390/char/sclp_sdias.c
index 50f7115990f..6a6f76bf6e3 100644
--- a/drivers/s390/char/sclp_sdias.c
+++ b/drivers/s390/char/sclp_sdias.c
@@ -1,7 +1,7 @@
 /*
  * Sclp "store data in absolut storage"
  *
- * Copyright IBM Corp. 2003,2007
+ * Copyright IBM Corp. 2003, 2007
  * Author(s): Michael Holzheu
  */
 
diff --git a/drivers/s390/char/sclp_tty.c b/drivers/s390/char/sclp_tty.c
index e66a75b3822..0792c85baaf 100644
--- a/drivers/s390/char/sclp_tty.c
+++ b/drivers/s390/char/sclp_tty.c
@@ -1,9 +1,8 @@
 /*
- *  drivers/s390/char/sclp_tty.c
  *    SCLP line mode terminal driver.
  *
  *  S390 version
- *    Copyright (C) 1999 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 1999
  *    Author(s): Martin Peschke <mpeschke@de.ibm.com>
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>
  */
diff --git a/drivers/s390/char/sclp_tty.h b/drivers/s390/char/sclp_tty.h
index 4b965b22fec..c8773421c31 100644
--- a/drivers/s390/char/sclp_tty.h
+++ b/drivers/s390/char/sclp_tty.h
@@ -1,9 +1,8 @@
 /*
- *  drivers/s390/char/sclp_tty.h
  *    interface to the SCLP-read/write driver
  *
  *  S390 version
- *    Copyright (C) 1999 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 1999
  *    Author(s): Martin Peschke <mpeschke@de.ibm.com>
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>
  */
diff --git a/drivers/s390/char/tape.h b/drivers/s390/char/tape.h
index bc6c7cfd36b..c06be6cc2fc 100644
--- a/drivers/s390/char/tape.h
+++ b/drivers/s390/char/tape.h
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/tape.h
  *    tape device driver for 3480/3490E/3590 tapes.
  *
  *  S390 and zSeries version
diff --git a/drivers/s390/char/tape_34xx.c b/drivers/s390/char/tape_34xx.c
index b28de80b7ca..6ae929c024a 100644
--- a/drivers/s390/char/tape_34xx.c
+++ b/drivers/s390/char/tape_34xx.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/tape_34xx.c
  *    tape device discipline for 3480/3490 tapes.
  *
  *    Copyright IBM Corp. 2001, 2009
diff --git a/drivers/s390/char/tape_3590.c b/drivers/s390/char/tape_3590.c
index a5c6614b0db..1b0eb49f739 100644
--- a/drivers/s390/char/tape_3590.c
+++ b/drivers/s390/char/tape_3590.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/tape_3590.c
  *    tape device discipline for 3590 tapes.
  *
  *    Copyright IBM Corp. 2001, 2009
diff --git a/drivers/s390/char/tape_3590.h b/drivers/s390/char/tape_3590.h
index 4534055f137..36b759e89d2 100644
--- a/drivers/s390/char/tape_3590.h
+++ b/drivers/s390/char/tape_3590.h
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/char/tape_3590.h
  *    tape device discipline for 3590 tapes.
  *
- *    Copyright IBM Corp. 2001,2006
+ *    Copyright IBM Corp. 2001, 2006
  *    Author(s): Stefan Bader <shbader@de.ibm.com>
  *		 Michael Holzheu <holzheu@de.ibm.com>
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>
diff --git a/drivers/s390/char/tape_char.c b/drivers/s390/char/tape_char.c
index 46886a7578c..2d61db3fc62 100644
--- a/drivers/s390/char/tape_char.c
+++ b/drivers/s390/char/tape_char.c
@@ -1,9 +1,8 @@
 /*
- *  drivers/s390/char/tape_char.c
  *    character device frontend for tape device driver
  *
  *  S390 and zSeries version
- *    Copyright IBM Corp. 2001,2006
+ *    Copyright IBM Corp. 2001, 2006
  *    Author(s): Carsten Otte <cotte@de.ibm.com>
  *		 Michael Holzheu <holzheu@de.ibm.com>
  *		 Tuan Ngo-Anh <ngoanh@de.ibm.com>
diff --git a/drivers/s390/char/tape_class.c b/drivers/s390/char/tape_class.c
index 55343df61ed..54b3c79203f 100644
--- a/drivers/s390/char/tape_class.c
+++ b/drivers/s390/char/tape_class.c
@@ -1,6 +1,5 @@
 /*
- * (C) Copyright IBM Corp. 2004
- * tape_class.c
+ * Copyright IBM Corp. 2004
  *
  * Tape class device support
  *
@@ -17,7 +16,7 @@
 
 MODULE_AUTHOR("Stefan Bader <shbader@de.ibm.com>");
 MODULE_DESCRIPTION(
-	"(C) Copyright IBM Corp. 2004   All Rights Reserved.\n"
+	"Copyright IBM Corp. 2004   All Rights Reserved.\n"
 	"tape_class.c"
 );
 MODULE_LICENSE("GPL");
diff --git a/drivers/s390/char/tape_class.h b/drivers/s390/char/tape_class.h
index ba2092f741d..a332c10d50a 100644
--- a/drivers/s390/char/tape_class.h
+++ b/drivers/s390/char/tape_class.h
@@ -1,6 +1,5 @@
 /*
- * (C) Copyright IBM Corp. 2004   All Rights Reserved.
- * tape_class.h
+ * Copyright IBM Corp. 2004   All Rights Reserved.
  *
  * Tape class device support
  *
diff --git a/drivers/s390/char/tape_core.c b/drivers/s390/char/tape_core.c
index 585618663ba..f3b5123faf0 100644
--- a/drivers/s390/char/tape_core.c
+++ b/drivers/s390/char/tape_core.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/char/tape_core.c
  *    basic function of the tape device driver
  *
  *  S390 and zSeries version
diff --git a/drivers/s390/char/tape_proc.c b/drivers/s390/char/tape_proc.c
index 0ceb37984f7..8733b232a11 100644
--- a/drivers/s390/char/tape_proc.c
+++ b/drivers/s390/char/tape_proc.c
@@ -1,9 +1,8 @@
 /*
- *  drivers/s390/char/tape.c
  *    tape device driver for S/390 and zSeries tapes.
  *
  *  S390 and zSeries version
- *    Copyright (C) 2001 IBM Corporation
+ *    Copyright IBM Corp. 2001
  *    Author(s): Carsten Otte <cotte@de.ibm.com>
  *		 Michael Holzheu <holzheu@de.ibm.com>
  *		 Tuan Ngo-Anh <ngoanh@de.ibm.com>
diff --git a/drivers/s390/char/tape_std.c b/drivers/s390/char/tape_std.c
index e7650170274..981a99fd8d4 100644
--- a/drivers/s390/char/tape_std.c
+++ b/drivers/s390/char/tape_std.c
@@ -1,9 +1,8 @@
 /*
- *  drivers/s390/char/tape_std.c
  *    standard tape device functions for ibm tapes.
  *
  *  S390 and zSeries version
- *    Copyright (C) 2001,2002 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *    Copyright IBM Corp. 2001, 2002
  *    Author(s): Carsten Otte <cotte@de.ibm.com>
  *		 Michael Holzheu <holzheu@de.ibm.com>
  *		 Tuan Ngo-Anh <ngoanh@de.ibm.com>
diff --git a/drivers/s390/char/tape_std.h b/drivers/s390/char/tape_std.h
index 1fc95235934..c5816ad9ed7 100644
--- a/drivers/s390/char/tape_std.h
+++ b/drivers/s390/char/tape_std.h
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/char/tape_std.h
  *    standard tape device functions for ibm tapes.
  *
- *    Copyright (C) IBM Corp. 2001,2006
+ *    Copyright IBM Corp. 2001, 2006
  *    Author(s): Carsten Otte <cotte@de.ibm.com>
  *		 Tuan Ngo-Anh <ngoanh@de.ibm.com>
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>
diff --git a/drivers/s390/char/tty3270.c b/drivers/s390/char/tty3270.c
index 10ec690197c..1928f3458d1 100644
--- a/drivers/s390/char/tty3270.c
+++ b/drivers/s390/char/tty3270.c
@@ -1,11 +1,10 @@
 /*
- *  drivers/s390/char/tty3270.c
  *    IBM/3270 Driver - tty functions.
  *
  *  Author(s):
  *    Original 3270 Code for 2.4 written by Richard Hitt (UTS Global)
  *    Rewritten for 2.5 by Martin Schwidefsky <schwidefsky@de.ibm.com>
- *	-- Copyright (C) 2003 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ *	-- Copyright IBM Corp. 2003
  */
 
 #include <linux/module.h>
diff --git a/drivers/s390/char/tty3270.h b/drivers/s390/char/tty3270.h
index 799da57f039..11141a8f897 100644
--- a/drivers/s390/char/tty3270.h
+++ b/drivers/s390/char/tty3270.h
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/char/tty3270.h
- *
  *    Copyright IBM Corp. 2007
  *
  */
diff --git a/drivers/s390/char/vmcp.c b/drivers/s390/char/vmcp.c
index 89c03e6b1c0..0fdedadff7b 100644
--- a/drivers/s390/char/vmcp.c
+++ b/drivers/s390/char/vmcp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright IBM Corp. 2004,2010
+ * Copyright IBM Corp. 2004, 2010
  * Interface implementation for communication with the z/VM control program
  *
  * Author(s): Christian Borntraeger <borntraeger@de.ibm.com>
diff --git a/drivers/s390/char/vmcp.h b/drivers/s390/char/vmcp.h
index 6a993948e18..1e29b041838 100644
--- a/drivers/s390/char/vmcp.h
+++ b/drivers/s390/char/vmcp.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2004, 2005 IBM Corporation
+ * Copyright IBM Corp. 2004, 2005
  * Interface implementation for communication with the z/VM control program
  * Version 1.0
  * Author(s): Christian Borntraeger <cborntra@de.ibm.com>
diff --git a/drivers/s390/char/vmlogrdr.c b/drivers/s390/char/vmlogrdr.c
index 524d988d89d..c131bc40f96 100644
--- a/drivers/s390/char/vmlogrdr.c
+++ b/drivers/s390/char/vmlogrdr.c
@@ -1,5 +1,4 @@
 /*
- * drivers/s390/char/vmlogrdr.c
  *	character device driver for reading z/VM system service records
  *
  *
@@ -656,10 +655,19 @@ static ssize_t vmlogrdr_recording_status_show(struct device_driver *driver,
 	len = strlen(buf);
 	return len;
 }
-
-
 static DRIVER_ATTR(recording_status, 0444, vmlogrdr_recording_status_show,
 		   NULL);
+static struct attribute *vmlogrdr_drv_attrs[] = {
+	&driver_attr_recording_status.attr,
+	NULL,
+};
+static struct attribute_group vmlogrdr_drv_attr_group = {
+	.attrs = vmlogrdr_drv_attrs,
+};
+static const struct attribute_group *vmlogrdr_drv_attr_groups[] = {
+	&vmlogrdr_drv_attr_group,
+	NULL,
+};
 
 static struct attribute *vmlogrdr_attrs[] = {
 	&dev_attr_autopurge.attr,
@@ -668,6 +676,13 @@ static struct attribute *vmlogrdr_attrs[] = {
 	&dev_attr_recording.attr,
 	NULL,
 };
+static struct attribute_group vmlogrdr_attr_group = {
+	.attrs = vmlogrdr_attrs,
+};
+static const struct attribute_group *vmlogrdr_attr_groups[] = {
+	&vmlogrdr_attr_group,
+	NULL,
+};
 
 static int vmlogrdr_pm_prepare(struct device *dev)
 {
@@ -692,18 +707,14 @@ static const struct dev_pm_ops vmlogrdr_pm_ops = {
 	.prepare = vmlogrdr_pm_prepare,
 };
 
-static struct attribute_group vmlogrdr_attr_group = {
-	.attrs = vmlogrdr_attrs,
-};
-
 static struct class *vmlogrdr_class;
 static struct device_driver vmlogrdr_driver = {
 	.name = "vmlogrdr",
 	.bus  = &iucv_bus,
 	.pm = &vmlogrdr_pm_ops,
+	.groups = vmlogrdr_drv_attr_groups,
 };
 
-
 static int vmlogrdr_register_driver(void)
 {
 	int ret;
@@ -717,21 +728,14 @@ static int vmlogrdr_register_driver(void)
 	if (ret)
 		goto out_iucv;
 
-	ret = driver_create_file(&vmlogrdr_driver,
-				 &driver_attr_recording_status);
-	if (ret)
-		goto out_driver;
-
 	vmlogrdr_class = class_create(THIS_MODULE, "vmlogrdr");
 	if (IS_ERR(vmlogrdr_class)) {
 		ret = PTR_ERR(vmlogrdr_class);
 		vmlogrdr_class = NULL;
-		goto out_attr;
+		goto out_driver;
 	}
 	return 0;
 
-out_attr:
-	driver_remove_file(&vmlogrdr_driver, &driver_attr_recording_status);
 out_driver:
 	driver_unregister(&vmlogrdr_driver);
 out_iucv:
@@ -745,7 +749,6 @@ static void vmlogrdr_unregister_driver(void)
 {
 	class_destroy(vmlogrdr_class);
 	vmlogrdr_class = NULL;
-	driver_remove_file(&vmlogrdr_driver, &driver_attr_recording_status);
 	driver_unregister(&vmlogrdr_driver);
 	iucv_unregister(&vmlogrdr_iucv_handler, 1);
 }
@@ -762,6 +765,7 @@ static int vmlogrdr_register_device(struct vmlogrdr_priv_t *priv)
 		dev->bus = &iucv_bus;
 		dev->parent = iucv_root;
 		dev->driver = &vmlogrdr_driver;
+		dev->groups = vmlogrdr_attr_groups;
 		dev_set_drvdata(dev, priv);
 		/*
 		 * The release function could be called after the
@@ -779,11 +783,6 @@ static int vmlogrdr_register_device(struct vmlogrdr_priv_t *priv)
 		return ret;
 	}
 
-	ret = sysfs_create_group(&dev->kobj, &vmlogrdr_attr_group);
-	if (ret) {
-		device_unregister(dev);
-		return ret;
-	}
 	priv->class_device = device_create(vmlogrdr_class, dev,
 					   MKDEV(vmlogrdr_major,
 						 priv->minor_num),
@@ -791,7 +790,6 @@ static int vmlogrdr_register_device(struct vmlogrdr_priv_t *priv)
 	if (IS_ERR(priv->class_device)) {
 		ret = PTR_ERR(priv->class_device);
 		priv->class_device=NULL;
-		sysfs_remove_group(&dev->kobj, &vmlogrdr_attr_group);
 		device_unregister(dev);
 		return ret;
 	}
@@ -804,7 +802,6 @@ static int vmlogrdr_unregister_device(struct vmlogrdr_priv_t *priv)
 {
 	device_destroy(vmlogrdr_class, MKDEV(vmlogrdr_major, priv->minor_num));
 	if (priv->device != NULL) {
-		sysfs_remove_group(&priv->device->kobj, &vmlogrdr_attr_group);
 		device_unregister(priv->device);
 		priv->device=NULL;
 	}
diff --git a/drivers/s390/char/vmwatchdog.c b/drivers/s390/char/vmwatchdog.c
index 2211277a107..e9b72311e25 100644
--- a/drivers/s390/char/vmwatchdog.c
+++ b/drivers/s390/char/vmwatchdog.c
@@ -1,7 +1,7 @@
 /*
  * Watchdog implementation based on z/VM Watchdog Timer API
  *
- * Copyright IBM Corp. 2004,2009
+ * Copyright IBM Corp. 2004, 2009
  *
  * The user space watchdog daemon can use this driver as
  * /dev/vmwatchdog to have z/VM execute the specified CP
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 3303d66b279..e3b9308b0fe 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -5,7 +5,7 @@
  *
  * For more information please refer to Documentation/s390/zfcpdump.txt
  *
- * Copyright IBM Corp. 2003,2008
+ * Copyright IBM Corp. 2003, 2008
  * Author(s): Michael Holzheu
  */
 
diff --git a/drivers/s390/cio/airq.c b/drivers/s390/cio/airq.c
index 65d2e769dfa..bc10220f684 100644
--- a/drivers/s390/cio/airq.c
+++ b/drivers/s390/cio/airq.c
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/cio/airq.c
  *    Support for adapter interruptions
  *
- *    Copyright IBM Corp. 1999,2007
+ *    Copyright IBM Corp. 1999, 2007
  *    Author(s): Ingo Adlung <adlung@de.ibm.com>
  *		 Cornelia Huck <cornelia.huck@de.ibm.com>
  *		 Arnd Bergmann <arndb@de.ibm.com>
diff --git a/drivers/s390/cio/blacklist.c b/drivers/s390/cio/blacklist.c
index 08c66035dd1..2d2a966a3b3 100644
--- a/drivers/s390/cio/blacklist.c
+++ b/drivers/s390/cio/blacklist.c
@@ -1,9 +1,7 @@
 /*
- *  drivers/s390/cio/blacklist.c
  *   S/390 common I/O routines -- blacklisting of specific devices
  *
- *    Copyright (C) 1999-2002 IBM Deutschland Entwicklung GmbH,
- *			      IBM Corporation
+ *    Copyright IBM Corp. 1999, 2002
  *    Author(s): Ingo Adlung (adlung@de.ibm.com)
  *		 Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Arnd Bergmann (arndb@de.ibm.com)
diff --git a/drivers/s390/cio/chp.c b/drivers/s390/cio/chp.c
index e792436c927..50ad5fdd815 100644
--- a/drivers/s390/cio/chp.c
+++ b/drivers/s390/cio/chp.c
@@ -1,7 +1,5 @@
 /*
- *  drivers/s390/cio/chp.c
- *
- *    Copyright IBM Corp. 1999,2010
+ *    Copyright IBM Corp. 1999, 2010
  *    Author(s): Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Arnd Bergmann (arndb@de.ibm.com)
  *		 Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
@@ -362,10 +360,13 @@ static struct attribute *chp_attrs[] = {
 	&dev_attr_shared.attr,
 	NULL,
 };
-
 static struct attribute_group chp_attr_group = {
 	.attrs = chp_attrs,
 };
+static const struct attribute_group *chp_attr_groups[] = {
+	&chp_attr_group,
+	NULL,
+};
 
 static void chp_release(struct device *dev)
 {
@@ -397,6 +398,7 @@ int chp_new(struct chp_id chpid)
 	chp->chpid = chpid;
 	chp->state = 1;
 	chp->dev.parent = &channel_subsystems[chpid.cssid]->device;
+	chp->dev.groups = chp_attr_groups;
 	chp->dev.release = chp_release;
 	mutex_init(&chp->lock);
 
@@ -426,16 +428,10 @@ int chp_new(struct chp_id chpid)
 		put_device(&chp->dev);
 		goto out;
 	}
-	ret = sysfs_create_group(&chp->dev.kobj, &chp_attr_group);
-	if (ret) {
-		device_unregister(&chp->dev);
-		goto out;
-	}
 	mutex_lock(&channel_subsystems[chpid.cssid]->mutex);
 	if (channel_subsystems[chpid.cssid]->cm_enabled) {
 		ret = chp_add_cmg_attr(chp);
 		if (ret) {
-			sysfs_remove_group(&chp->dev.kobj, &chp_attr_group);
 			device_unregister(&chp->dev);
 			mutex_unlock(&channel_subsystems[chpid.cssid]->mutex);
 			goto out;
diff --git a/drivers/s390/cio/chp.h b/drivers/s390/cio/chp.h
index 12b4903d6fe..e1399dbee83 100644
--- a/drivers/s390/cio/chp.h
+++ b/drivers/s390/cio/chp.h
@@ -1,7 +1,5 @@
 /*
- *  drivers/s390/cio/chp.h
- *
- *    Copyright IBM Corp. 2007,2010
+ *    Copyright IBM Corp. 2007, 2010
  *    Author(s): Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
  */
 
diff --git a/drivers/s390/cio/chsc.c b/drivers/s390/cio/chsc.c
index a84631a7391..cfe0c087fe5 100644
--- a/drivers/s390/cio/chsc.c
+++ b/drivers/s390/cio/chsc.c
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/cio/chsc.c
  *   S/390 common I/O routines -- channel subsystem call
  *
- *    Copyright IBM Corp. 1999,2010
+ *    Copyright IBM Corp. 1999, 2010
  *    Author(s): Ingo Adlung (adlung@de.ibm.com)
  *		 Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Arnd Bergmann (arndb@de.ibm.com)
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index a6ddaed8793..33d1ef70359 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/cio/cio.c
  *   S/390 common I/O routines -- low level i/o calls
  *
- *    Copyright IBM Corp. 1999,2008
+ *    Copyright IBM Corp. 1999, 2008
  *    Author(s): Ingo Adlung (adlung@de.ibm.com)
  *		 Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Arnd Bergmann (arndb@de.ibm.com)
diff --git a/drivers/s390/cio/cmf.c b/drivers/s390/cio/cmf.c
index 204ca728e7f..c9fc61c0a86 100644
--- a/drivers/s390/cio/cmf.c
+++ b/drivers/s390/cio/cmf.c
@@ -1,9 +1,7 @@
 /*
- * linux/drivers/s390/cio/cmf.c
- *
  * Linux on zSeries Channel Measurement Facility support
  *
- * Copyright 2000,2006 IBM Corporation
+ * Copyright IBM Corp. 2000, 2006
  *
  * Authors: Arnd Bergmann <arndb@de.ibm.com>
  *	    Cornelia Huck <cornelia.huck@de.ibm.com>
@@ -1341,7 +1339,7 @@ module_init(init_cmf);
 MODULE_AUTHOR("Arnd Bergmann <arndb@de.ibm.com>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("channel measurement facility base driver\n"
-		   "Copyright 2003 IBM Corporation\n");
+		   "Copyright IBM Corp. 2003\n");
 
 EXPORT_SYMBOL_GPL(enable_cmf);
 EXPORT_SYMBOL_GPL(disable_cmf);
diff --git a/drivers/s390/cio/crw.c b/drivers/s390/cio/crw.c
index d0a2dff43fb..0f8a25f98b1 100644
--- a/drivers/s390/cio/crw.c
+++ b/drivers/s390/cio/crw.c
@@ -1,7 +1,7 @@
 /*
  *   Channel report handling code
  *
- *    Copyright IBM Corp. 2000,2009
+ *    Copyright IBM Corp. 2000, 2009
  *    Author(s): Ingo Adlung <adlung@de.ibm.com>,
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>,
  *		 Cornelia Huck <cornelia.huck@de.ibm.com>,
diff --git a/drivers/s390/cio/device.c b/drivers/s390/cio/device.c
index f8f952d5204..ed25c8740a9 100644
--- a/drivers/s390/cio/device.c
+++ b/drivers/s390/cio/device.c
@@ -1,8 +1,7 @@
 /*
- *  drivers/s390/cio/device.c
  *  bus driver for ccw devices
  *
- *    Copyright IBM Corp. 2002,2008
+ *    Copyright IBM Corp. 2002, 2008
  *    Author(s): Arnd Bergmann (arndb@de.ibm.com)
  *		 Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Martin Schwidefsky (schwidefsky@de.ibm.com)
diff --git a/drivers/s390/cio/device_fsm.c b/drivers/s390/cio/device_fsm.c
index 1b853513c89..1bb1d00095a 100644
--- a/drivers/s390/cio/device_fsm.c
+++ b/drivers/s390/cio/device_fsm.c
@@ -1,8 +1,7 @@
 /*
- * drivers/s390/cio/device_fsm.c
  * finite state machine for device handling
  *
- *    Copyright IBM Corp. 2002,2008
+ *    Copyright IBM Corp. 2002, 2008
  *    Author(s): Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Martin Schwidefsky (schwidefsky@de.ibm.com)
  */
diff --git a/drivers/s390/cio/device_id.c b/drivers/s390/cio/device_id.c
index 78a0b43862c..d4fa30541a3 100644
--- a/drivers/s390/cio/device_id.c
+++ b/drivers/s390/cio/device_id.c
@@ -1,7 +1,7 @@
 /*
  *  CCW device SENSE ID I/O handling.
  *
- *    Copyright IBM Corp. 2002,2009
+ *    Copyright IBM Corp. 2002, 2009
  *    Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>
  *		 Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
diff --git a/drivers/s390/cio/device_pgid.c b/drivers/s390/cio/device_pgid.c
index 07a4fd29f09..368368fe04b 100644
--- a/drivers/s390/cio/device_pgid.c
+++ b/drivers/s390/cio/device_pgid.c
@@ -1,7 +1,7 @@
 /*
  *  CCW device PGID and path verification I/O handling.
  *
- *    Copyright IBM Corp. 2002,2009
+ *    Copyright IBM Corp. 2002, 2009
  *    Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
  *		 Martin Schwidefsky <schwidefsky@de.ibm.com>
  *		 Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
diff --git a/drivers/s390/cio/device_status.c b/drivers/s390/cio/device_status.c
index 66d8066ef22..15b56a15db1 100644
--- a/drivers/s390/cio/device_status.c
+++ b/drivers/s390/cio/device_status.c
@@ -1,8 +1,5 @@
 /*
- * drivers/s390/cio/device_status.c
- *
- *    Copyright (C) 2002 IBM Deutschland Entwicklung GmbH,
- *			 IBM Corporation
+ *    Copyright IBM Corp. 2002
  *    Author(s): Cornelia Huck (cornelia.huck@de.ibm.com)
  *		 Martin Schwidefsky (schwidefsky@de.ibm.com)
  *
diff --git a/drivers/s390/cio/idset.c b/drivers/s390/cio/idset.c
index 4d10981c7cc..e6d5f8c4952 100644
--- a/drivers/s390/cio/idset.c
+++ b/drivers/s390/cio/idset.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/cio/idset.c
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
  */
diff --git a/drivers/s390/cio/idset.h b/drivers/s390/cio/idset.h
index 7543da4529f..3d943f03591 100644
--- a/drivers/s390/cio/idset.h
+++ b/drivers/s390/cio/idset.h
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/cio/idset.h
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Peter Oberparleiter <peter.oberparleiter@de.ibm.com>
  */
diff --git a/drivers/s390/cio/qdio.h b/drivers/s390/cio/qdio.h
index b962ffbc080..5132554d791 100644
--- a/drivers/s390/cio/qdio.h
+++ b/drivers/s390/cio/qdio.h
@@ -1,7 +1,5 @@
 /*
- * linux/drivers/s390/cio/qdio.h
- *
- * Copyright 2000,2009 IBM Corp.
+ * Copyright IBM Corp. 2000, 2009
  * Author(s): Utz Bacher <utz.bacher@de.ibm.com>
  *	      Jan Glauber <jang@linux.vnet.ibm.com>
  */
diff --git a/drivers/s390/cio/qdio_debug.c b/drivers/s390/cio/qdio_debug.c
index 29021f4e96b..e6e0d31c02a 100644
--- a/drivers/s390/cio/qdio_debug.c
+++ b/drivers/s390/cio/qdio_debug.c
@@ -1,7 +1,5 @@
 /*
- *  drivers/s390/cio/qdio_debug.c
- *
- *  Copyright IBM Corp. 2008,2009
+ *  Copyright IBM Corp. 2008, 2009
  *
  *  Author: Jan Glauber (jang@linux.vnet.ibm.com)
  */
diff --git a/drivers/s390/cio/qdio_debug.h b/drivers/s390/cio/qdio_debug.h
index 5d70bd162ae..e1f646800dd 100644
--- a/drivers/s390/cio/qdio_debug.h
+++ b/drivers/s390/cio/qdio_debug.h
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/cio/qdio_debug.h
- *
  *  Copyright IBM Corp. 2008
  *
  *  Author: Jan Glauber (jang@linux.vnet.ibm.com)
diff --git a/drivers/s390/cio/qdio_main.c b/drivers/s390/cio/qdio_main.c
index 7493efafa0d..e06fa03ea1e 100644
--- a/drivers/s390/cio/qdio_main.c
+++ b/drivers/s390/cio/qdio_main.c
@@ -1,9 +1,7 @@
 /*
- * linux/drivers/s390/cio/qdio_main.c
- *
  * Linux for s390 qdio support, buffer handling, qdio API and module support.
  *
- * Copyright 2000,2008 IBM Corp.
+ * Copyright IBM Corp. 2000, 2008
  * Author(s): Utz Bacher <utz.bacher@de.ibm.com>
  *	      Jan Glauber <jang@linux.vnet.ibm.com>
  * 2.6 cio integration by Cornelia Huck <cornelia.huck@de.ibm.com>
diff --git a/drivers/s390/cio/qdio_setup.c b/drivers/s390/cio/qdio_setup.c
index ecf12f0aca7..6c973db1498 100644
--- a/drivers/s390/cio/qdio_setup.c
+++ b/drivers/s390/cio/qdio_setup.c
@@ -1,9 +1,7 @@
 /*
- * driver/s390/cio/qdio_setup.c
- *
  * qdio queue initialization
  *
- * Copyright (C) IBM Corp. 2008
+ * Copyright IBM Corp. 2008
  * Author(s): Jan Glauber <jang@linux.vnet.ibm.com>
  */
 #include <linux/kernel.h>
diff --git a/drivers/s390/cio/qdio_thinint.c b/drivers/s390/cio/qdio_thinint.c
index 011eadea3ee..2e060088fa8 100644
--- a/drivers/s390/cio/qdio_thinint.c
+++ b/drivers/s390/cio/qdio_thinint.c
@@ -1,7 +1,5 @@
 /*
- * linux/drivers/s390/cio/thinint_qdio.c
- *
- * Copyright 2000,2009 IBM Corp.
+ * Copyright IBM Corp. 2000, 2009
  * Author(s): Utz Bacher <utz.bacher@de.ibm.com>
  *	      Cornelia Huck <cornelia.huck@de.ibm.com>
  *	      Jan Glauber <jang@linux.vnet.ibm.com>
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index b987d461958..ae258a4b4e5 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -1,7 +1,5 @@
 /*
- * linux/drivers/s390/crypto/ap_bus.c
- *
- * Copyright (C) 2006 IBM Corporation
+ * Copyright IBM Corp. 2006
  * Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
  *	      Martin Schwidefsky <schwidefsky@de.ibm.com>
  *	      Ralph Wuerthner <rwuerthn@de.ibm.com>
@@ -70,7 +68,7 @@ static int ap_select_domain(void);
  */
 MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("Adjunct Processor Bus driver, "
-		   "Copyright 2006 IBM Corporation");
+		   "Copyright IBM Corp. 2006");
 MODULE_LICENSE("GPL");
 
 /*
@@ -338,6 +336,12 @@ static int ap_queue_enable_interruption(ap_qid_t qid, void *ind)
 			break;
 		case AP_RESPONSE_RESET_IN_PROGRESS:
 		case AP_RESPONSE_BUSY:
+			if (i < AP_MAX_RESET - 1) {
+				udelay(5);
+				status = ap_queue_interruption_control(qid,
+								       ind);
+				continue;
+			}
 			break;
 		case AP_RESPONSE_Q_NOT_AVAIL:
 		case AP_RESPONSE_DECONFIGURED:
diff --git a/drivers/s390/crypto/ap_bus.h b/drivers/s390/crypto/ap_bus.h
index 726fc65809d..52d61995af8 100644
--- a/drivers/s390/crypto/ap_bus.h
+++ b/drivers/s390/crypto/ap_bus.h
@@ -1,7 +1,5 @@
 /*
- * linux/drivers/s390/crypto/ap_bus.h
- *
- * Copyright (C) 2006 IBM Corporation
+ * Copyright IBM Corp. 2006
  * Author(s): Cornelia Huck <cornelia.huck@de.ibm.com>
  *	      Martin Schwidefsky <schwidefsky@de.ibm.com>
  *	      Ralph Wuerthner <rwuerthn@de.ibm.com>
diff --git a/drivers/s390/crypto/zcrypt_api.c b/drivers/s390/crypto/zcrypt_api.c
index 88523208d47..2f94132246a 100644
--- a/drivers/s390/crypto/zcrypt_api.c
+++ b/drivers/s390/crypto/zcrypt_api.c
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_api.c
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *	       Cornelia Huck <cornelia.huck@de.ibm.com>
@@ -47,7 +45,7 @@
  */
 MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("Cryptographic Coprocessor interface, "
-		   "Copyright 2001, 2006 IBM Corporation");
+		   "Copyright IBM Corp. 2001, 2006");
 MODULE_LICENSE("GPL");
 
 static DEFINE_SPINLOCK(zcrypt_device_lock);
diff --git a/drivers/s390/crypto/zcrypt_api.h b/drivers/s390/crypto/zcrypt_api.h
index 9688f3985b0..7a32c4bc8ef 100644
--- a/drivers/s390/crypto/zcrypt_api.h
+++ b/drivers/s390/crypto/zcrypt_api.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_api.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *	       Cornelia Huck <cornelia.huck@de.ibm.com>
diff --git a/drivers/s390/crypto/zcrypt_cca_key.h b/drivers/s390/crypto/zcrypt_cca_key.h
index ed82f2f59b1..1f42f103c76 100644
--- a/drivers/s390/crypto/zcrypt_cca_key.h
+++ b/drivers/s390/crypto/zcrypt_cca_key.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_cca_key.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
diff --git a/drivers/s390/crypto/zcrypt_cex2a.c b/drivers/s390/crypto/zcrypt_cex2a.c
index 46812440425..744c668f586 100644
--- a/drivers/s390/crypto/zcrypt_cex2a.c
+++ b/drivers/s390/crypto/zcrypt_cex2a.c
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_cex2a.c
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
@@ -66,7 +64,7 @@ static struct ap_device_id zcrypt_cex2a_ids[] = {
 MODULE_DEVICE_TABLE(ap, zcrypt_cex2a_ids);
 MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("CEX2A Cryptographic Coprocessor device driver, "
-		   "Copyright 2001, 2006 IBM Corporation");
+		   "Copyright IBM Corp. 2001, 2006");
 MODULE_LICENSE("GPL");
 
 static int zcrypt_cex2a_probe(struct ap_device *ap_dev);
diff --git a/drivers/s390/crypto/zcrypt_cex2a.h b/drivers/s390/crypto/zcrypt_cex2a.h
index 0350665810c..0dce4b9af18 100644
--- a/drivers/s390/crypto/zcrypt_cex2a.h
+++ b/drivers/s390/crypto/zcrypt_cex2a.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_cex2a.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
diff --git a/drivers/s390/crypto/zcrypt_error.h b/drivers/s390/crypto/zcrypt_error.h
index 03ba27f05f9..0965e2626d1 100644
--- a/drivers/s390/crypto/zcrypt_error.h
+++ b/drivers/s390/crypto/zcrypt_error.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_error.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
diff --git a/drivers/s390/crypto/zcrypt_pcica.c b/drivers/s390/crypto/zcrypt_pcica.c
index ad7951c21b7..f2b71d8df01 100644
--- a/drivers/s390/crypto/zcrypt_pcica.c
+++ b/drivers/s390/crypto/zcrypt_pcica.c
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_pcica.c
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
@@ -56,7 +54,7 @@ static struct ap_device_id zcrypt_pcica_ids[] = {
 MODULE_DEVICE_TABLE(ap, zcrypt_pcica_ids);
 MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("PCICA Cryptographic Coprocessor device driver, "
-		   "Copyright 2001, 2006 IBM Corporation");
+		   "Copyright IBM Corp. 2001, 2006");
 MODULE_LICENSE("GPL");
 
 static int zcrypt_pcica_probe(struct ap_device *ap_dev);
diff --git a/drivers/s390/crypto/zcrypt_pcica.h b/drivers/s390/crypto/zcrypt_pcica.h
index 3be11187f6d..9a59155cad5 100644
--- a/drivers/s390/crypto/zcrypt_pcica.h
+++ b/drivers/s390/crypto/zcrypt_pcica.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_pcica.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
diff --git a/drivers/s390/crypto/zcrypt_pcicc.c b/drivers/s390/crypto/zcrypt_pcicc.c
index e5dd335fda5..0d90a433405 100644
--- a/drivers/s390/crypto/zcrypt_pcicc.c
+++ b/drivers/s390/crypto/zcrypt_pcicc.c
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_pcicc.c
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
@@ -68,7 +66,7 @@ static struct ap_device_id zcrypt_pcicc_ids[] = {
 MODULE_DEVICE_TABLE(ap, zcrypt_pcicc_ids);
 MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("PCICC Cryptographic Coprocessor device driver, "
-		   "Copyright 2001, 2006 IBM Corporation");
+		   "Copyright IBM Corp. 2001, 2006");
 MODULE_LICENSE("GPL");
 
 static int zcrypt_pcicc_probe(struct ap_device *ap_dev);
diff --git a/drivers/s390/crypto/zcrypt_pcicc.h b/drivers/s390/crypto/zcrypt_pcicc.h
index 6d4454846c8..7fe27e15075 100644
--- a/drivers/s390/crypto/zcrypt_pcicc.h
+++ b/drivers/s390/crypto/zcrypt_pcicc.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_pcicc.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.c b/drivers/s390/crypto/zcrypt_pcixcc.c
index f7cc4340181..ccb4f8b60c7 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.c
+++ b/drivers/s390/crypto/zcrypt_pcixcc.c
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_pcixcc.c
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
@@ -78,7 +76,7 @@ static struct ap_device_id zcrypt_pcixcc_ids[] = {
 MODULE_DEVICE_TABLE(ap, zcrypt_pcixcc_ids);
 MODULE_AUTHOR("IBM Corporation");
 MODULE_DESCRIPTION("PCIXCC Cryptographic Coprocessor device driver, "
-		   "Copyright 2001, 2006 IBM Corporation");
+		   "Copyright IBM Corp. 2001, 2006");
 MODULE_LICENSE("GPL");
 
 static int zcrypt_pcixcc_probe(struct ap_device *ap_dev);
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.h b/drivers/s390/crypto/zcrypt_pcixcc.h
index 8cb7d7a6973..c7cdf599e46 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.h
+++ b/drivers/s390/crypto/zcrypt_pcixcc.h
@@ -1,9 +1,7 @@
 /*
- *  linux/drivers/s390/crypto/zcrypt_pcixcc.h
- *
  *  zcrypt 2.1.0
  *
- *  Copyright (C)  2001, 2006 IBM Corporation
+ *  Copyright IBM Corp. 2001, 2006
  *  Author(s): Robert Burroughs
  *	       Eric Rossman (edrossma@us.ibm.com)
  *
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index d74e9ae6dfb..47cccd52aae 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -1,5 +1,5 @@
 /*
- * kvm_virtio.c - virtio for kvm on s390
+ * virtio for kvm on s390
  *
  * Copyright IBM Corp. 2008
  *
@@ -25,6 +25,7 @@
 #include <asm/io.h>
 #include <asm/kvm_para.h>
 #include <asm/kvm_virtio.h>
+#include <asm/sclp.h>
 #include <asm/setup.h>
 #include <asm/irq.h>
 
@@ -468,7 +469,7 @@ static __init int early_put_chars(u32 vtermno, const char *buf, int count)
 
 static int __init s390_virtio_console_init(void)
 {
-	if (!MACHINE_IS_KVM)
+	if (sclp_has_vt220() || sclp_has_linemode())
 		return -ENODEV;
 	return virtio_cons_early_init(early_put_chars);
 }
diff --git a/drivers/s390/net/claw.c b/drivers/s390/net/claw.c
index 6b1ff90d2f0..a0a4afe537d 100644
--- a/drivers/s390/net/claw.c
+++ b/drivers/s390/net/claw.c
@@ -1,5 +1,4 @@
 /*
- *  drivers/s390/net/claw.c
  *    ESCON CLAW network driver
  *
  *  Linux for zSeries version
@@ -3380,5 +3379,5 @@ module_exit(claw_cleanup);
 
 MODULE_AUTHOR("Andy Richter <richtera@us.ibm.com>");
 MODULE_DESCRIPTION("Linux for System z CLAW Driver\n" \
-			"Copyright 2000,2008 IBM Corporation\n");
+			"Copyright IBM Corp. 2000, 2008\n");
 MODULE_LICENSE("GPL");
diff --git a/drivers/s390/net/ctcm_dbug.c b/drivers/s390/net/ctcm_dbug.c
index d962fd741a2..6514e1cb3f1 100644
--- a/drivers/s390/net/ctcm_dbug.c
+++ b/drivers/s390/net/ctcm_dbug.c
@@ -1,6 +1,4 @@
 /*
- *	drivers/s390/net/ctcm_dbug.c
- *
  *	Copyright IBM Corp. 2001, 2007
  *	Authors:	Peter Tiedemann (ptiedem@de.ibm.com)
  *
diff --git a/drivers/s390/net/ctcm_dbug.h b/drivers/s390/net/ctcm_dbug.h
index 26966d0b9ab..47bf0501995 100644
--- a/drivers/s390/net/ctcm_dbug.h
+++ b/drivers/s390/net/ctcm_dbug.h
@@ -1,6 +1,4 @@
 /*
- *	drivers/s390/net/ctcm_dbug.h
- *
  *	Copyright IBM Corp. 2001, 2007
  *	Authors:	Peter Tiedemann (ptiedem@de.ibm.com)
  *
diff --git a/drivers/s390/net/ctcm_fsms.c b/drivers/s390/net/ctcm_fsms.c
index a69766900a1..d4ade9e92fb 100644
--- a/drivers/s390/net/ctcm_fsms.c
+++ b/drivers/s390/net/ctcm_fsms.c
@@ -1,6 +1,4 @@
 /*
- * drivers/s390/net/ctcm_fsms.c
- *
  * Copyright IBM Corp. 2001, 2007
  * Authors:	Fritz Elfert (felfert@millenux.com)
  * 		Peter Tiedemann (ptiedem@de.ibm.com)
diff --git a/drivers/s390/net/ctcm_fsms.h b/drivers/s390/net/ctcm_fsms.h
index 046d077fabb..c963d04799c 100644
--- a/drivers/s390/net/ctcm_fsms.h
+++ b/drivers/s390/net/ctcm_fsms.h
@@ -1,6 +1,4 @@
 /*
- * drivers/s390/net/ctcm_fsms.h
- *
  * Copyright IBM Corp. 2001, 2007
  * Authors: 	Fritz Elfert (felfert@millenux.com)
  * 		Peter Tiedemann (ptiedem@de.ibm.com)
diff --git a/drivers/s390/net/ctcm_main.c b/drivers/s390/net/ctcm_main.c
index 3cd25544a27..5227e5734a9 100644
--- a/drivers/s390/net/ctcm_main.c
+++ b/drivers/s390/net/ctcm_main.c
@@ -1,6 +1,4 @@
 /*
- * drivers/s390/net/ctcm_main.c
- *
  * Copyright IBM Corp. 2001, 2009
  * Author(s):
  *	Original CTC driver(s):
diff --git a/drivers/s390/net/ctcm_main.h b/drivers/s390/net/ctcm_main.h
index b9056a55d99..477c933685f 100644
--- a/drivers/s390/net/ctcm_main.h
+++ b/drivers/s390/net/ctcm_main.h
@@ -1,6 +1,4 @@
 /*
- *	drivers/s390/net/ctcm_main.h
- *
  *	Copyright IBM Corp. 2001, 2007
  *	Authors:	Fritz Elfert (felfert@millenux.com)
  *			Peter Tiedemann (ptiedem@de.ibm.com)
diff --git a/drivers/s390/net/ctcm_mpc.c b/drivers/s390/net/ctcm_mpc.c
index ac7975b7a83..05b734a2b5b 100644
--- a/drivers/s390/net/ctcm_mpc.c
+++ b/drivers/s390/net/ctcm_mpc.c
@@ -1,6 +1,4 @@
 /*
- *	drivers/s390/net/ctcm_mpc.c
- *
  *	Copyright IBM Corp. 2004, 2007
  *	Authors:	Belinda Thompson (belindat@us.ibm.com)
  *			Andy Richter (richtera@us.ibm.com)
diff --git a/drivers/s390/net/ctcm_mpc.h b/drivers/s390/net/ctcm_mpc.h
index 1fa07b0c11c..bd1b1cc54ff 100644
--- a/drivers/s390/net/ctcm_mpc.h
+++ b/drivers/s390/net/ctcm_mpc.h
@@ -1,6 +1,4 @@
 /*
- * drivers/s390/net/ctcm_mpc.h
- *
  * Copyright IBM Corp. 2007
  * Authors:	Peter Tiedemann (ptiedem@de.ibm.com)
  *
diff --git a/drivers/s390/net/ctcm_sysfs.c b/drivers/s390/net/ctcm_sysfs.c
index 0c27ae72647..985b5dcbdac 100644
--- a/drivers/s390/net/ctcm_sysfs.c
+++ b/drivers/s390/net/ctcm_sysfs.c
@@ -1,6 +1,4 @@
 /*
- * drivers/s390/net/ctcm_sysfs.c
- *
  * Copyright IBM Corp. 2007, 2007
  * Authors:	Peter Tiedemann (ptiedem@de.ibm.com)
  *
diff --git a/drivers/s390/net/netiucv.c b/drivers/s390/net/netiucv.c
index 8160591913f..4ffa66c87ea 100644
--- a/drivers/s390/net/netiucv.c
+++ b/drivers/s390/net/netiucv.c
@@ -1854,26 +1854,11 @@ static struct attribute_group netiucv_stat_attr_group = {
 	.attrs = netiucv_stat_attrs,
 };
 
-static int netiucv_add_files(struct device *dev)
-{
-	int ret;
-
-	IUCV_DBF_TEXT(trace, 3, __func__);
-	ret = sysfs_create_group(&dev->kobj, &netiucv_attr_group);
-	if (ret)
-		return ret;
-	ret = sysfs_create_group(&dev->kobj, &netiucv_stat_attr_group);
-	if (ret)
-		sysfs_remove_group(&dev->kobj, &netiucv_attr_group);
-	return ret;
-}
-
-static void netiucv_remove_files(struct device *dev)
-{
-	IUCV_DBF_TEXT(trace, 3, __func__);
-	sysfs_remove_group(&dev->kobj, &netiucv_stat_attr_group);
-	sysfs_remove_group(&dev->kobj, &netiucv_attr_group);
-}
+static const struct attribute_group *netiucv_attr_groups[] = {
+	&netiucv_stat_attr_group,
+	&netiucv_attr_group,
+	NULL,
+};
 
 static int netiucv_register_device(struct net_device *ndev)
 {
@@ -1887,6 +1872,7 @@ static int netiucv_register_device(struct net_device *ndev)
 		dev_set_name(dev, "net%s", ndev->name);
 		dev->bus = &iucv_bus;
 		dev->parent = iucv_root;
+		dev->groups = netiucv_attr_groups;
 		/*
 		 * The release function could be called after the
 		 * module has been unloaded. It's _only_ task is to
@@ -1904,22 +1890,14 @@ static int netiucv_register_device(struct net_device *ndev)
 		put_device(dev);
 		return ret;
 	}
-	ret = netiucv_add_files(dev);
-	if (ret)
-		goto out_unreg;
 	priv->dev = dev;
 	dev_set_drvdata(dev, priv);
 	return 0;
-
-out_unreg:
-	device_unregister(dev);
-	return ret;
 }
 
 static void netiucv_unregister_device(struct device *dev)
 {
 	IUCV_DBF_TEXT(trace, 3, __func__);
-	netiucv_remove_files(dev);
 	device_unregister(dev);
 }
 
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 06e8f31ff3d..fa7adad6f9b 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_core.h
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index e118e1e1e1c..7a8b09612c4 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_core_main.c
- *
  *    Copyright IBM Corp. 2007, 2009
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
diff --git a/drivers/s390/net/qeth_core_mpc.c b/drivers/s390/net/qeth_core_mpc.c
index 7fab6544def..5cebfddb86b 100644
--- a/drivers/s390/net/qeth_core_mpc.c
+++ b/drivers/s390/net/qeth_core_mpc.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_core_mpc.c
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Frank Pavlic <fpavlic@de.ibm.com>,
  *		 Thomas Spatzier <tspat@de.ibm.com>,
diff --git a/drivers/s390/net/qeth_core_mpc.h b/drivers/s390/net/qeth_core_mpc.h
index a11b30c3842..3690bbf2cb3 100644
--- a/drivers/s390/net/qeth_core_mpc.h
+++ b/drivers/s390/net/qeth_core_mpc.h
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_core_mpc.h
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Frank Pavlic <fpavlic@de.ibm.com>,
  *		 Thomas Spatzier <tspat@de.ibm.com>,
diff --git a/drivers/s390/net/qeth_core_sys.c b/drivers/s390/net/qeth_core_sys.c
index f163af575c4..9655dc0ea0e 100644
--- a/drivers/s390/net/qeth_core_sys.c
+++ b/drivers/s390/net/qeth_core_sys.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_core_sys.c
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 426986518e9..2db409330c2 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_l2_main.c
- *
  *    Copyright IBM Corp. 2007, 2009
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
@@ -647,7 +645,7 @@ static int qeth_l2_request_initial_mac(struct qeth_card *card)
 		}
 		QETH_DBF_HEX(SETUP, 2, card->dev->dev_addr, OSA_ADDR_LEN);
 	} else {
-		random_ether_addr(card->dev->dev_addr);
+		eth_random_addr(card->dev->dev_addr);
 		memcpy(card->dev->dev_addr, vendor_pre, 3);
 	}
 	return 0;
diff --git a/drivers/s390/net/qeth_l3.h b/drivers/s390/net/qeth_l3.h
index e367315a63f..29c1c00e3a0 100644
--- a/drivers/s390/net/qeth_l3.h
+++ b/drivers/s390/net/qeth_l3.h
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_l3.h
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 7be5e977569..c5f03fa70fb 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_l3_main.c
- *
  *    Copyright IBM Corp. 2007, 2009
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
@@ -1473,7 +1471,7 @@ static int qeth_l3_iqd_read_initial_mac_cb(struct qeth_card *card,
 		memcpy(card->dev->dev_addr,
 			cmd->data.create_destroy_addr.unique_id, ETH_ALEN);
 	else
-		random_ether_addr(card->dev->dev_addr);
+		eth_random_addr(card->dev->dev_addr);
 
 	return 0;
 }
@@ -1760,6 +1758,8 @@ static void qeth_l3_free_vlan_addresses4(struct qeth_card *card,
 	QETH_CARD_TEXT(card, 4, "frvaddr4");
 
 	netdev = __vlan_find_dev_deep(card->dev, vid);
+	if (!netdev)
+		return;
 	in_dev = in_dev_get(netdev);
 	if (!in_dev)
 		return;
@@ -1788,6 +1788,8 @@ static void qeth_l3_free_vlan_addresses6(struct qeth_card *card,
 	QETH_CARD_TEXT(card, 4, "frvaddr6");
 
 	netdev = __vlan_find_dev_deep(card->dev, vid);
+	if (!netdev)
+		return;
 	in6_dev = in6_dev_get(netdev);
 	if (!in6_dev)
 		return;
@@ -2700,10 +2702,11 @@ int inline qeth_l3_get_cast_type(struct qeth_card *card, struct sk_buff *skb)
 	rcu_read_lock();
 	dst = skb_dst(skb);
 	if (dst)
-		n = dst_get_neighbour_noref(dst);
+		n = dst_neigh_lookup_skb(dst, skb);
 	if (n) {
 		cast_type = n->type;
 		rcu_read_unlock();
+		neigh_release(n);
 		if ((cast_type == RTN_BROADCAST) ||
 		    (cast_type == RTN_MULTICAST) ||
 		    (cast_type == RTN_ANYCAST))
diff --git a/drivers/s390/net/qeth_l3_sys.c b/drivers/s390/net/qeth_l3_sys.c
index 4cafedf950a..ebc37948626 100644
--- a/drivers/s390/net/qeth_l3_sys.c
+++ b/drivers/s390/net/qeth_l3_sys.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/s390/net/qeth_l3_sys.c
- *
  *    Copyright IBM Corp. 2007
  *    Author(s): Utz Bacher <utz.bacher@de.ibm.com>,
  *		 Frank Pavlic <fpavlic@de.ibm.com>,
diff --git a/drivers/s390/net/smsgiucv.h b/drivers/s390/net/smsgiucv.h
index 149a1151608..45bc925928c 100644
--- a/drivers/s390/net/smsgiucv.h
+++ b/drivers/s390/net/smsgiucv.h
@@ -1,7 +1,7 @@
 /*
  * IUCV special message driver
  *
- * Copyright (C) 2003 IBM Deutschland Entwicklung GmbH, IBM Corporation
+ * Copyright IBM Corp. 2003
  * Author(s): Martin Schwidefsky (schwidefsky@de.ibm.com)
  */
 
diff --git a/drivers/s390/scsi/zfcp_aux.c b/drivers/s390/scsi/zfcp_aux.c
index 08601810966..aff8621de80 100644
--- a/drivers/s390/scsi/zfcp_aux.c
+++ b/drivers/s390/scsi/zfcp_aux.c
@@ -3,7 +3,7 @@
  *
  * Module interface and handling of zfcp data structures.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 /*
diff --git a/drivers/s390/scsi/zfcp_ccw.c b/drivers/s390/scsi/zfcp_ccw.c
index 96f13ad8812..e37f0455194 100644
--- a/drivers/s390/scsi/zfcp_ccw.c
+++ b/drivers/s390/scsi/zfcp_ccw.c
@@ -3,7 +3,7 @@
  *
  * Registration and callback for the s390 common I/O layer.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_cfdc.c b/drivers/s390/scsi/zfcp_cfdc.c
index fab2c2592a9..fbd8b4db602 100644
--- a/drivers/s390/scsi/zfcp_cfdc.c
+++ b/drivers/s390/scsi/zfcp_cfdc.c
@@ -5,7 +5,7 @@
  * Access Control Lists / Control File Data Channel;
  * handling of response code and states for ports and LUNs.
  *
- * Copyright IBM Corporation 2008, 2010
+ * Copyright IBM Corp. 2008, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_dbf.c b/drivers/s390/scsi/zfcp_dbf.c
index a9a816e4aa5..3c1d22097ad 100644
--- a/drivers/s390/scsi/zfcp_dbf.c
+++ b/drivers/s390/scsi/zfcp_dbf.c
@@ -3,7 +3,7 @@
  *
  * Debug traces for zfcp.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_def.h b/drivers/s390/scsi/zfcp_def.h
index ed5d921e82c..2955e1a3dea 100644
--- a/drivers/s390/scsi/zfcp_def.h
+++ b/drivers/s390/scsi/zfcp_def.h
@@ -3,7 +3,7 @@
  *
  * Global definitions for the zfcp device driver.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #ifndef ZFCP_DEF_H
diff --git a/drivers/s390/scsi/zfcp_erp.c b/drivers/s390/scsi/zfcp_erp.c
index e1b4f800e22..92d3df6ac8b 100644
--- a/drivers/s390/scsi/zfcp_erp.c
+++ b/drivers/s390/scsi/zfcp_erp.c
@@ -3,7 +3,7 @@
  *
  * Error Recovery Procedures (ERP).
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_ext.h b/drivers/s390/scsi/zfcp_ext.h
index 2302e1cfb76..36f422770ff 100644
--- a/drivers/s390/scsi/zfcp_ext.h
+++ b/drivers/s390/scsi/zfcp_ext.h
@@ -3,7 +3,7 @@
  *
  * External function declarations.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #ifndef ZFCP_EXT_H
diff --git a/drivers/s390/scsi/zfcp_fc.c b/drivers/s390/scsi/zfcp_fc.c
index 297e6b71ce9..88688a80b2c 100644
--- a/drivers/s390/scsi/zfcp_fc.c
+++ b/drivers/s390/scsi/zfcp_fc.c
@@ -3,7 +3,7 @@
  *
  * Fibre Channel related functions for the zfcp device driver.
  *
- * Copyright IBM Corporation 2008, 2010
+ * Copyright IBM Corp. 2008, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_fc.h b/drivers/s390/scsi/zfcp_fc.h
index 4561f3bf730..b1d2024ed51 100644
--- a/drivers/s390/scsi/zfcp_fc.h
+++ b/drivers/s390/scsi/zfcp_fc.h
@@ -4,7 +4,7 @@
  * Fibre Channel related definitions and inline functions for the zfcp
  * device driver
  *
- * Copyright IBM Corporation 2009
+ * Copyright IBM Corp. 2009
  */
 
 #ifndef ZFCP_FC_H
diff --git a/drivers/s390/scsi/zfcp_fsf.c b/drivers/s390/scsi/zfcp_fsf.c
index e9a787e2e6a..e1c1efc2c5a 100644
--- a/drivers/s390/scsi/zfcp_fsf.c
+++ b/drivers/s390/scsi/zfcp_fsf.c
@@ -3,7 +3,7 @@
  *
  * Implementation of FSF commands.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_fsf.h b/drivers/s390/scsi/zfcp_fsf.h
index db8c85382dc..5e795b86931 100644
--- a/drivers/s390/scsi/zfcp_fsf.h
+++ b/drivers/s390/scsi/zfcp_fsf.h
@@ -3,7 +3,7 @@
  *
  * Interface to the FSF support functions.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #ifndef FSF_H
diff --git a/drivers/s390/scsi/zfcp_qdio.c b/drivers/s390/scsi/zfcp_qdio.c
index e14da5751d3..b9fffc8d94a 100644
--- a/drivers/s390/scsi/zfcp_qdio.c
+++ b/drivers/s390/scsi/zfcp_qdio.c
@@ -3,7 +3,7 @@
  *
  * Setup and helper functions to access QDIO.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_qdio.h b/drivers/s390/scsi/zfcp_qdio.h
index 8ac7f5342d2..497cd379b0d 100644
--- a/drivers/s390/scsi/zfcp_qdio.h
+++ b/drivers/s390/scsi/zfcp_qdio.h
@@ -3,7 +3,7 @@
  *
  * Header file for zfcp qdio interface
  *
- * Copyright IBM Corporation 2010
+ * Copyright IBM Corp. 2010
  */
 
 #ifndef ZFCP_QDIO_H
diff --git a/drivers/s390/scsi/zfcp_reqlist.h b/drivers/s390/scsi/zfcp_reqlist.h
index a72d1b730ab..7c2c6194dfc 100644
--- a/drivers/s390/scsi/zfcp_reqlist.h
+++ b/drivers/s390/scsi/zfcp_reqlist.h
@@ -4,7 +4,7 @@
  * Data structure and helper functions for tracking pending FSF
  * requests.
  *
- * Copyright IBM Corporation 2009
+ * Copyright IBM Corp. 2009
  */
 
 #ifndef ZFCP_REQLIST_H
diff --git a/drivers/s390/scsi/zfcp_scsi.c b/drivers/s390/scsi/zfcp_scsi.c
index b79576b64f4..7b31e3f403f 100644
--- a/drivers/s390/scsi/zfcp_scsi.c
+++ b/drivers/s390/scsi/zfcp_scsi.c
@@ -3,7 +3,7 @@
  *
  * Interface to Linux SCSI midlayer.
  *
- * Copyright IBM Corporation 2002, 2010
+ * Copyright IBM Corp. 2002, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_sysfs.c b/drivers/s390/scsi/zfcp_sysfs.c
index cdc4ff78a7b..c66af27b230 100644
--- a/drivers/s390/scsi/zfcp_sysfs.c
+++ b/drivers/s390/scsi/zfcp_sysfs.c
@@ -3,7 +3,7 @@
  *
  * sysfs attributes.
  *
- * Copyright IBM Corporation 2008, 2010
+ * Copyright IBM Corp. 2008, 2010
  */
 
 #define KMSG_COMPONENT "zfcp"
diff --git a/drivers/s390/scsi/zfcp_unit.c b/drivers/s390/scsi/zfcp_unit.c
index 20796ebc33c..3f2bff0d3aa 100644
--- a/drivers/s390/scsi/zfcp_unit.c
+++ b/drivers/s390/scsi/zfcp_unit.c
@@ -4,7 +4,7 @@
  * Tracking of manually configured LUNs and helper functions to
  * register the LUNs with the SCSI midlayer.
  *
- * Copyright IBM Corporation 2010
+ * Copyright IBM Corp. 2010
  */
 
 #include "zfcp_def.h"
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index e9559782d3e..74bf1aa7af4 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -263,23 +263,6 @@ config SCSI_SCAN_ASYNC
 	  You can override this choice by specifying "scsi_mod.scan=sync"
 	  or async on the kernel's command line.
 
-config SCSI_WAIT_SCAN
-	tristate  # No prompt here, this is an invisible symbol.
-	default m
-	depends on SCSI
-	depends on MODULES
-# scsi_wait_scan is a loadable module which waits until all the async scans are
-# complete.  The idea is to use it in initrd/ initramfs scripts.  You modprobe
-# it after all the modprobes of the root SCSI drivers and it will wait until
-# they have all finished scanning their buses before allowing the boot to
-# proceed.  (This method is not applicable if targets boot independently in
-# parallel with the initiator, or with transports with non-deterministic target
-# discovery schemes, or if a transport driver does not support scsi_wait_scan.)
-#
-# This symbol is not exposed as a prompt because little is to be gained by
-# disabling it, whereas people who accidentally switch it off may wonder why
-# their mkinitrd gets into trouble.
-
 menu "SCSI Transports"
 	depends on SCSI
 
@@ -461,7 +444,7 @@ config SCSI_ACARD
 
 config SCSI_AHA152X
 	tristate "Adaptec AHA152X/2825 support"
-	depends on ISA && SCSI && !64BIT
+	depends on ISA && SCSI
 	select SCSI_SPI_ATTRS
 	select CHECK_SIGNATURE
 	---help---
diff --git a/drivers/scsi/Makefile b/drivers/scsi/Makefile
index 1a3368b0861..888f73a4aae 100644
--- a/drivers/scsi/Makefile
+++ b/drivers/scsi/Makefile
@@ -159,8 +159,6 @@ obj-$(CONFIG_SCSI_OSD_INITIATOR) += osd/
 # This goes last, so that "real" scsi devices probe earlier
 obj-$(CONFIG_SCSI_DEBUG)	+= scsi_debug.o
 
-obj-$(CONFIG_SCSI_WAIT_SCAN)	+= scsi_wait_scan.o
-
 scsi_mod-y			+= scsi.o hosts.o scsi_ioctl.o constants.o \
 				   scsicam.o scsi_error.o scsi_lib.o
 scsi_mod-$(CONFIG_SCSI_DMA)	+= scsi_lib_dma.o
diff --git a/drivers/scsi/aacraid/aachba.c b/drivers/scsi/aacraid/aachba.c
index 52551662d10..d79457ac8be 100644
--- a/drivers/scsi/aacraid/aachba.c
+++ b/drivers/scsi/aacraid/aachba.c
@@ -135,6 +135,8 @@ struct inquiry_data {
 static unsigned long aac_build_sg(struct scsi_cmnd* scsicmd, struct sgmap* sgmap);
 static unsigned long aac_build_sg64(struct scsi_cmnd* scsicmd, struct sgmap64* psg);
 static unsigned long aac_build_sgraw(struct scsi_cmnd* scsicmd, struct sgmapraw* psg);
+static unsigned long aac_build_sgraw2(struct scsi_cmnd *scsicmd, struct aac_raw_io2 *rio2, int sg_max);
+static int aac_convert_sgraw2(struct aac_raw_io2 *rio2, int pages, int nseg, int nseg_new);
 static int aac_send_srb_fib(struct scsi_cmnd* scsicmd);
 #ifdef AAC_DETAILED_STATUS_INFO
 static char *aac_get_status_string(u32 status);
@@ -152,10 +154,14 @@ int aac_commit = -1;
 int startup_timeout = 180;
 int aif_timeout = 120;
 int aac_sync_mode;  /* Only Sync. transfer - disabled */
+int aac_convert_sgl = 1;	/* convert non-conformable s/g list - enabled */
 
 module_param(aac_sync_mode, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(aac_sync_mode, "Force sync. transfer mode"
 	" 0=off, 1=on");
+module_param(aac_convert_sgl, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(aac_convert_sgl, "Convert non-conformable s/g list"
+	" 0=off, 1=on");
 module_param(nondasd, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(nondasd, "Control scanning of hba for nondasd devices."
 	" 0=off, 1=on");
@@ -963,25 +969,44 @@ static void io_callback(void *context, struct fib * fibptr);
 
 static int aac_read_raw_io(struct fib * fib, struct scsi_cmnd * cmd, u64 lba, u32 count)
 {
-	u16 fibsize;
-	struct aac_raw_io *readcmd;
+	struct aac_dev *dev = fib->dev;
+	u16 fibsize, command;
+
 	aac_fib_init(fib);
-	readcmd = (struct aac_raw_io *) fib_data(fib);
-	readcmd->block[0] = cpu_to_le32((u32)(lba&0xffffffff));
-	readcmd->block[1] = cpu_to_le32((u32)((lba&0xffffffff00000000LL)>>32));
-	readcmd->count = cpu_to_le32(count<<9);
-	readcmd->cid = cpu_to_le16(scmd_id(cmd));
-	readcmd->flags = cpu_to_le16(IO_TYPE_READ);
-	readcmd->bpTotal = 0;
-	readcmd->bpComplete = 0;
+	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE2 && !dev->sync_mode) {
+		struct aac_raw_io2 *readcmd2;
+		readcmd2 = (struct aac_raw_io2 *) fib_data(fib);
+		memset(readcmd2, 0, sizeof(struct aac_raw_io2));
+		readcmd2->blockLow = cpu_to_le32((u32)(lba&0xffffffff));
+		readcmd2->blockHigh = cpu_to_le32((u32)((lba&0xffffffff00000000LL)>>32));
+		readcmd2->byteCount = cpu_to_le32(count<<9);
+		readcmd2->cid = cpu_to_le16(scmd_id(cmd));
+		readcmd2->flags = cpu_to_le16(RIO2_IO_TYPE_READ);
+		aac_build_sgraw2(cmd, readcmd2, dev->scsi_host_ptr->sg_tablesize);
+		command = ContainerRawIo2;
+		fibsize = sizeof(struct aac_raw_io2) +
+			((le32_to_cpu(readcmd2->sgeCnt)-1) * sizeof(struct sge_ieee1212));
+	} else {
+		struct aac_raw_io *readcmd;
+		readcmd = (struct aac_raw_io *) fib_data(fib);
+		readcmd->block[0] = cpu_to_le32((u32)(lba&0xffffffff));
+		readcmd->block[1] = cpu_to_le32((u32)((lba&0xffffffff00000000LL)>>32));
+		readcmd->count = cpu_to_le32(count<<9);
+		readcmd->cid = cpu_to_le16(scmd_id(cmd));
+		readcmd->flags = cpu_to_le16(RIO_TYPE_READ);
+		readcmd->bpTotal = 0;
+		readcmd->bpComplete = 0;
+		aac_build_sgraw(cmd, &readcmd->sg);
+		command = ContainerRawIo;
+		fibsize = sizeof(struct aac_raw_io) +
+			((le32_to_cpu(readcmd->sg.count)-1) * sizeof(struct sgentryraw));
+	}
 
-	aac_build_sgraw(cmd, &readcmd->sg);
-	fibsize = sizeof(struct aac_raw_io) + ((le32_to_cpu(readcmd->sg.count) - 1) * sizeof (struct sgentryraw));
 	BUG_ON(fibsize > (fib->dev->max_fib_size - sizeof(struct aac_fibhdr)));
 	/*
 	 *	Now send the Fib to the adapter
 	 */
-	return aac_fib_send(ContainerRawIo,
+	return aac_fib_send(command,
 			  fib,
 			  fibsize,
 			  FsaNormal,
@@ -1052,28 +1077,50 @@ static int aac_read_block(struct fib * fib, struct scsi_cmnd * cmd, u64 lba, u32
 
 static int aac_write_raw_io(struct fib * fib, struct scsi_cmnd * cmd, u64 lba, u32 count, int fua)
 {
-	u16 fibsize;
-	struct aac_raw_io *writecmd;
+	struct aac_dev *dev = fib->dev;
+	u16 fibsize, command;
+
 	aac_fib_init(fib);
-	writecmd = (struct aac_raw_io *) fib_data(fib);
-	writecmd->block[0] = cpu_to_le32((u32)(lba&0xffffffff));
-	writecmd->block[1] = cpu_to_le32((u32)((lba&0xffffffff00000000LL)>>32));
-	writecmd->count = cpu_to_le32(count<<9);
-	writecmd->cid = cpu_to_le16(scmd_id(cmd));
-	writecmd->flags = (fua && ((aac_cache & 5) != 1) &&
-	  (((aac_cache & 5) != 5) || !fib->dev->cache_protected)) ?
-		cpu_to_le16(IO_TYPE_WRITE|IO_SUREWRITE) :
-		cpu_to_le16(IO_TYPE_WRITE);
-	writecmd->bpTotal = 0;
-	writecmd->bpComplete = 0;
-
-	aac_build_sgraw(cmd, &writecmd->sg);
-	fibsize = sizeof(struct aac_raw_io) + ((le32_to_cpu(writecmd->sg.count) - 1) * sizeof (struct sgentryraw));
+	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE2 && !dev->sync_mode) {
+		struct aac_raw_io2 *writecmd2;
+		writecmd2 = (struct aac_raw_io2 *) fib_data(fib);
+		memset(writecmd2, 0, sizeof(struct aac_raw_io2));
+		writecmd2->blockLow = cpu_to_le32((u32)(lba&0xffffffff));
+		writecmd2->blockHigh = cpu_to_le32((u32)((lba&0xffffffff00000000LL)>>32));
+		writecmd2->byteCount = cpu_to_le32(count<<9);
+		writecmd2->cid = cpu_to_le16(scmd_id(cmd));
+		writecmd2->flags = (fua && ((aac_cache & 5) != 1) &&
+						   (((aac_cache & 5) != 5) || !fib->dev->cache_protected)) ?
+			cpu_to_le16(RIO2_IO_TYPE_WRITE|RIO2_IO_SUREWRITE) :
+			cpu_to_le16(RIO2_IO_TYPE_WRITE);
+		aac_build_sgraw2(cmd, writecmd2, dev->scsi_host_ptr->sg_tablesize);
+		command = ContainerRawIo2;
+		fibsize = sizeof(struct aac_raw_io2) +
+			((le32_to_cpu(writecmd2->sgeCnt)-1) * sizeof(struct sge_ieee1212));
+	} else {
+		struct aac_raw_io *writecmd;
+		writecmd = (struct aac_raw_io *) fib_data(fib);
+		writecmd->block[0] = cpu_to_le32((u32)(lba&0xffffffff));
+		writecmd->block[1] = cpu_to_le32((u32)((lba&0xffffffff00000000LL)>>32));
+		writecmd->count = cpu_to_le32(count<<9);
+		writecmd->cid = cpu_to_le16(scmd_id(cmd));
+		writecmd->flags = (fua && ((aac_cache & 5) != 1) &&
+						   (((aac_cache & 5) != 5) || !fib->dev->cache_protected)) ?
+			cpu_to_le16(RIO_TYPE_WRITE|RIO_SUREWRITE) :
+			cpu_to_le16(RIO_TYPE_WRITE);
+		writecmd->bpTotal = 0;
+		writecmd->bpComplete = 0;
+		aac_build_sgraw(cmd, &writecmd->sg);
+		command = ContainerRawIo;
+		fibsize = sizeof(struct aac_raw_io) +
+			((le32_to_cpu(writecmd->sg.count)-1) * sizeof (struct sgentryraw));
+	}
+
 	BUG_ON(fibsize > (fib->dev->max_fib_size - sizeof(struct aac_fibhdr)));
 	/*
 	 *	Now send the Fib to the adapter
 	 */
-	return aac_fib_send(ContainerRawIo,
+	return aac_fib_send(command,
 			  fib,
 			  fibsize,
 			  FsaNormal,
@@ -1492,8 +1539,6 @@ int aac_get_adapter_info(struct aac_dev* dev)
 			dev->a_ops.adapter_write = aac_write_block;
 		}
 		dev->scsi_host_ptr->max_sectors = AAC_MAX_32BIT_SGBCOUNT;
-		if (dev->adapter_info.options & AAC_OPT_NEW_COMM_TYPE1)
-			dev->adapter_info.options |= AAC_OPT_NEW_COMM;
 		if (!(dev->adapter_info.options & AAC_OPT_NEW_COMM)) {
 			/*
 			 * Worst case size that could cause sg overflow when
@@ -2616,12 +2661,18 @@ static void aac_srb_callback(void *context, struct fib * fibptr)
 	srbreply = (struct aac_srb_reply *) fib_data(fibptr);
 
 	scsicmd->sense_buffer[0] = '\0';  /* Initialize sense valid flag to false */
-	/*
-	 *	Calculate resid for sg
-	 */
 
-	scsi_set_resid(scsicmd, scsi_bufflen(scsicmd)
-		       - le32_to_cpu(srbreply->data_xfer_length));
+	if (fibptr->flags & FIB_CONTEXT_FLAG_FASTRESP) {
+		/* fast response */
+		srbreply->srb_status = cpu_to_le32(SRB_STATUS_SUCCESS);
+		srbreply->scsi_status = cpu_to_le32(SAM_STAT_GOOD);
+	} else {
+		/*
+		 *	Calculate resid for sg
+		 */
+		scsi_set_resid(scsicmd, scsi_bufflen(scsicmd)
+				   - le32_to_cpu(srbreply->data_xfer_length));
+	}
 
 	scsi_dma_unmap(scsicmd);
 
@@ -2954,6 +3005,118 @@ static unsigned long aac_build_sgraw(struct scsi_cmnd* scsicmd, struct sgmapraw*
 	return byte_count;
 }
 
+static unsigned long aac_build_sgraw2(struct scsi_cmnd *scsicmd, struct aac_raw_io2 *rio2, int sg_max)
+{
+	unsigned long byte_count = 0;
+	int nseg;
+
+	nseg = scsi_dma_map(scsicmd);
+	BUG_ON(nseg < 0);
+	if (nseg) {
+		struct scatterlist *sg;
+		int i, conformable = 0;
+		u32 min_size = PAGE_SIZE, cur_size;
+
+		scsi_for_each_sg(scsicmd, sg, nseg, i) {
+			int count = sg_dma_len(sg);
+			u64 addr = sg_dma_address(sg);
+
+			BUG_ON(i >= sg_max);
+			rio2->sge[i].addrHigh = cpu_to_le32((u32)(addr>>32));
+			rio2->sge[i].addrLow = cpu_to_le32((u32)(addr & 0xffffffff));
+			cur_size = cpu_to_le32(count);
+			rio2->sge[i].length = cur_size;
+			rio2->sge[i].flags = 0;
+			if (i == 0) {
+				conformable = 1;
+				rio2->sgeFirstSize = cur_size;
+			} else if (i == 1) {
+				rio2->sgeNominalSize = cur_size;
+				min_size = cur_size;
+			} else if ((i+1) < nseg && cur_size != rio2->sgeNominalSize) {
+				conformable = 0;
+				if (cur_size < min_size)
+					min_size = cur_size;
+			}
+			byte_count += count;
+		}
+
+		/* hba wants the size to be exact */
+		if (byte_count > scsi_bufflen(scsicmd)) {
+			u32 temp = le32_to_cpu(rio2->sge[i-1].length) -
+				(byte_count - scsi_bufflen(scsicmd));
+			rio2->sge[i-1].length = cpu_to_le32(temp);
+			byte_count = scsi_bufflen(scsicmd);
+		}
+
+		rio2->sgeCnt = cpu_to_le32(nseg);
+		rio2->flags |= cpu_to_le16(RIO2_SG_FORMAT_IEEE1212);
+		/* not conformable: evaluate required sg elements */
+		if (!conformable) {
+			int j, nseg_new = nseg, err_found;
+			for (i = min_size / PAGE_SIZE; i >= 1; --i) {
+				err_found = 0;
+				nseg_new = 2;
+				for (j = 1; j < nseg - 1; ++j) {
+					if (rio2->sge[j].length % (i*PAGE_SIZE)) {
+						err_found = 1;
+						break;
+					}
+					nseg_new += (rio2->sge[j].length / (i*PAGE_SIZE));
+				}
+				if (!err_found)
+					break;
+			}
+			if (i > 0 && nseg_new <= sg_max)
+				aac_convert_sgraw2(rio2, i, nseg, nseg_new);
+		} else
+			rio2->flags |= cpu_to_le16(RIO2_SGL_CONFORMANT);
+
+		/* Check for command underflow */
+		if (scsicmd->underflow && (byte_count < scsicmd->underflow)) {
+			printk(KERN_WARNING"aacraid: cmd len %08lX cmd underflow %08X\n",
+					byte_count, scsicmd->underflow);
+		}
+	}
+
+	return byte_count;
+}
+
+static int aac_convert_sgraw2(struct aac_raw_io2 *rio2, int pages, int nseg, int nseg_new)
+{
+	struct sge_ieee1212 *sge;
+	int i, j, pos;
+	u32 addr_low;
+
+	if (aac_convert_sgl == 0)
+		return 0;
+
+	sge = kmalloc(nseg_new * sizeof(struct sge_ieee1212), GFP_ATOMIC);
+	if (sge == NULL)
+		return -1;
+
+	for (i = 1, pos = 1; i < nseg-1; ++i) {
+		for (j = 0; j < rio2->sge[i].length / (pages * PAGE_SIZE); ++j) {
+			addr_low = rio2->sge[i].addrLow + j * pages * PAGE_SIZE;
+			sge[pos].addrLow = addr_low;
+			sge[pos].addrHigh = rio2->sge[i].addrHigh;
+			if (addr_low < rio2->sge[i].addrLow)
+				sge[pos].addrHigh++;
+			sge[pos].length = pages * PAGE_SIZE;
+			sge[pos].flags = 0;
+			pos++;
+		}
+	}
+	sge[pos] = rio2->sge[nseg-1];
+	memcpy(&rio2->sge[1], &sge[1], (nseg_new-1)*sizeof(struct sge_ieee1212));
+
+	kfree(sge);
+	rio2->sgeCnt = cpu_to_le32(nseg_new);
+	rio2->flags |= cpu_to_le16(RIO2_SGL_CONFORMANT);
+	rio2->sgeNominalSize = pages * PAGE_SIZE;
+	return 0;
+}
+
 #ifdef AAC_DETAILED_STATUS_INFO
 
 struct aac_srb_status_info {
diff --git a/drivers/scsi/aacraid/aacraid.h b/drivers/scsi/aacraid/aacraid.h
index 3fcf62724fa..9e933a88a8b 100644
--- a/drivers/scsi/aacraid/aacraid.h
+++ b/drivers/scsi/aacraid/aacraid.h
@@ -12,7 +12,7 @@
  *----------------------------------------------------------------------------*/
 
 #ifndef AAC_DRIVER_BUILD
-# define AAC_DRIVER_BUILD 28900
+# define AAC_DRIVER_BUILD 29800
 # define AAC_DRIVER_BRANCH "-ms"
 #endif
 #define MAXIMUM_NUM_CONTAINERS	32
@@ -100,6 +100,13 @@ struct user_sgentryraw {
 	u32		flags;	/* reserved for F/W use */
 };
 
+struct sge_ieee1212 {
+	u32	addrLow;
+	u32	addrHigh;
+	u32	length;
+	u32	flags;
+};
+
 /*
  *	SGMAP
  *
@@ -270,6 +277,8 @@ enum aac_queue_types {
  */
 
 #define		FIB_MAGIC	0x0001
+#define		FIB_MAGIC2	0x0004
+#define		FIB_MAGIC2_64	0x0005
 
 /*
  *	Define the priority levels the FSA communication routines support.
@@ -296,22 +305,20 @@ struct aac_fibhdr {
 	__le32 XferState;	/* Current transfer state for this CCB */
 	__le16 Command;		/* Routing information for the destination */
 	u8 StructType;		/* Type FIB */
-	u8 Flags;		/* Flags for FIB */
+	u8 Unused;		/* Unused */
 	__le16 Size;		/* Size of this FIB in bytes */
 	__le16 SenderSize;	/* Size of the FIB in the sender
 				   (for response sizing) */
 	__le32 SenderFibAddress;  /* Host defined data in the FIB */
-	__le32 ReceiverFibAddress;/* Logical address of this FIB for
-				     the adapter */
-	u32 SenderData;		/* Place holder for the sender to store data */
 	union {
-		struct {
-		    __le32 _ReceiverTimeStart;	/* Timestamp for
-						   receipt of fib */
-		    __le32 _ReceiverTimeDone;	/* Timestamp for
-						   completion of fib */
-		} _s;
-	} _u;
+		__le32 ReceiverFibAddress;/* Logical address of this FIB for
+				     the adapter (old) */
+		__le32 SenderFibAddressHigh;/* upper 32bit of phys. FIB address */
+		__le32 TimeStamp;	/* otherwise timestamp for FW internal use */
+	} u;
+	u32 Handle;		/* FIB handle used for MSGU commnunication */
+	u32 Previous;		/* FW internal use */
+	u32 Next;		/* FW internal use */
 };
 
 struct hw_fib {
@@ -361,6 +368,7 @@ struct hw_fib {
 #define		ContainerCommand		500
 #define		ContainerCommand64		501
 #define		ContainerRawIo			502
+#define		ContainerRawIo2			503
 /*
  *	Scsi Port commands (scsi passthrough)
  */
@@ -417,6 +425,7 @@ enum fib_xfer_state {
 #define ADAPTER_INIT_STRUCT_REVISION		3
 #define ADAPTER_INIT_STRUCT_REVISION_4		4 // rocket science
 #define ADAPTER_INIT_STRUCT_REVISION_6		6 /* PMC src */
+#define ADAPTER_INIT_STRUCT_REVISION_7		7 /* Denali */
 
 struct aac_init
 {
@@ -441,7 +450,9 @@ struct aac_init
 #define INITFLAGS_NEW_COMM_SUPPORTED	0x00000001
 #define INITFLAGS_DRIVER_USES_UTC_TIME	0x00000010
 #define INITFLAGS_DRIVER_SUPPORTS_PM	0x00000020
-#define INITFLAGS_NEW_COMM_TYPE1_SUPPORTED	0x00000041
+#define INITFLAGS_NEW_COMM_TYPE1_SUPPORTED	0x00000040
+#define INITFLAGS_FAST_JBOD_SUPPORTED	0x00000080
+#define INITFLAGS_NEW_COMM_TYPE2_SUPPORTED	0x00000100
 	__le32	MaxIoCommands;	/* max outstanding commands */
 	__le32	MaxIoSize;	/* largest I/O command */
 	__le32	MaxFibSize;	/* largest FIB to adapter */
@@ -1052,10 +1063,11 @@ struct aac_dev
 	struct adapter_ops	a_ops;
 	unsigned long		fsrev;		/* Main driver's revision number */
 
-	unsigned long		dbg_base;	/* address of UART
+	resource_size_t		base_start;	/* main IO base */
+	resource_size_t		dbg_base;	/* address of UART
 						 * debug buffer */
 
-	unsigned		base_size, dbg_size;	/* Size of
+	resource_size_t		base_size, dbg_size;	/* Size of
 							 *  mapped in region */
 
 	struct aac_init		*init;		/* Holds initialization info to communicate with adapter */
@@ -1123,6 +1135,7 @@ struct aac_dev
 #	define AAC_COMM_PRODUCER 0
 #	define AAC_COMM_MESSAGE  1
 #	define AAC_COMM_MESSAGE_TYPE1	3
+#	define AAC_COMM_MESSAGE_TYPE2	4
 	u8			raw_io_interface;
 	u8			raw_io_64;
 	u8			printf_enabled;
@@ -1181,6 +1194,7 @@ struct aac_dev
 #define FIB_CONTEXT_FLAG_TIMED_OUT		(0x00000001)
 #define FIB_CONTEXT_FLAG			(0x00000002)
 #define FIB_CONTEXT_FLAG_WAIT			(0x00000004)
+#define FIB_CONTEXT_FLAG_FASTRESP		(0x00000008)
 
 /*
  *	Define the command values
@@ -1287,6 +1301,22 @@ struct aac_dev
 #define CMDATA_SYNCH		4
 #define CMUNSTABLE		5
 
+#define	RIO_TYPE_WRITE 			0x0000
+#define	RIO_TYPE_READ			0x0001
+#define	RIO_SUREWRITE			0x0008
+
+#define RIO2_IO_TYPE			0x0003
+#define RIO2_IO_TYPE_WRITE		0x0000
+#define RIO2_IO_TYPE_READ		0x0001
+#define RIO2_IO_TYPE_VERIFY		0x0002
+#define RIO2_IO_ERROR			0x0004
+#define RIO2_IO_SUREWRITE		0x0008
+#define RIO2_SGL_CONFORMANT		0x0010
+#define RIO2_SG_FORMAT			0xF000
+#define RIO2_SG_FORMAT_ARC		0x0000
+#define RIO2_SG_FORMAT_SRL		0x1000
+#define RIO2_SG_FORMAT_IEEE1212		0x2000
+
 struct aac_read
 {
 	__le32		command;
@@ -1331,9 +1361,6 @@ struct aac_write64
 	__le32		block;
 	__le16		pad;
 	__le16		flags;
-#define	IO_TYPE_WRITE 0x00000000
-#define	IO_TYPE_READ  0x00000001
-#define	IO_SUREWRITE  0x00000008
 	struct sgmap64	sg;	// Must be last in struct because it is variable
 };
 struct aac_write_reply
@@ -1354,6 +1381,22 @@ struct aac_raw_io
 	struct sgmapraw	sg;
 };
 
+struct aac_raw_io2 {
+	__le32		blockLow;
+	__le32		blockHigh;
+	__le32		byteCount;
+	__le16		cid;
+	__le16		flags;		/* RIO2 flags */
+	__le32		sgeFirstSize;	/* size of first sge el. */
+	__le32		sgeNominalSize;	/* size of 2nd sge el. (if conformant) */
+	u8		sgeCnt;		/* only 8 bits required */
+	u8		bpTotal;	/* reserved for F/W use */
+	u8		bpComplete;	/* reserved for F/W use */
+	u8		sgeFirstIndex;	/* reserved for F/W use */
+	u8		unused[4];
+	struct sge_ieee1212	sge[1];
+};
+
 #define CT_FLUSH_CACHE 129
 struct aac_synchronize {
 	__le32		command;	/* VM_ContainerConfig */
diff --git a/drivers/scsi/aacraid/commctrl.c b/drivers/scsi/aacraid/commctrl.c
index 0bd38da4ada..1ef041bc60c 100644
--- a/drivers/scsi/aacraid/commctrl.c
+++ b/drivers/scsi/aacraid/commctrl.c
@@ -498,6 +498,8 @@ static int aac_send_raw_srb(struct aac_dev* dev, void __user * arg)
 		return -ENOMEM;
 	}
 	aac_fib_init(srbfib);
+	/* raw_srb FIB is not FastResponseCapable */
+	srbfib->hw_fib_va->header.XferState &= ~cpu_to_le32(FastResponseCapable);
 
 	srbcmd = (struct aac_srb*) fib_data(srbfib);
 
diff --git a/drivers/scsi/aacraid/comminit.c b/drivers/scsi/aacraid/comminit.c
index a35f54ebdce..8e5d3be1612 100644
--- a/drivers/scsi/aacraid/comminit.c
+++ b/drivers/scsi/aacraid/comminit.c
@@ -58,7 +58,8 @@ static int aac_alloc_comm(struct aac_dev *dev, void **commaddr, unsigned long co
 	dma_addr_t phys;
 	unsigned long aac_max_hostphysmempages;
 
-	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1)
+	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1 ||
+	    dev->comm_interface == AAC_COMM_MESSAGE_TYPE2)
 		host_rrq_size = (dev->scsi_host_ptr->can_queue
 			+ AAC_NUM_MGT_FIB) * sizeof(u32);
 	size = fibsize + sizeof(struct aac_init) + commsize +
@@ -75,7 +76,8 @@ static int aac_alloc_comm(struct aac_dev *dev, void **commaddr, unsigned long co
 	dev->comm_phys = phys;
 	dev->comm_size = size;
 	
-	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1) {
+	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1 ||
+	    dev->comm_interface == AAC_COMM_MESSAGE_TYPE2) {
 		dev->host_rrq = (u32 *)(base + fibsize);
 		dev->host_rrq_pa = phys + fibsize;
 		memset(dev->host_rrq, 0, host_rrq_size);
@@ -115,26 +117,32 @@ static int aac_alloc_comm(struct aac_dev *dev, void **commaddr, unsigned long co
 	else
 		init->HostPhysMemPages = cpu_to_le32(AAC_MAX_HOSTPHYSMEMPAGES);
 
-	init->InitFlags = 0;
+	init->InitFlags = cpu_to_le32(INITFLAGS_DRIVER_USES_UTC_TIME |
+		INITFLAGS_DRIVER_SUPPORTS_PM);
+	init->MaxIoCommands = cpu_to_le32(dev->scsi_host_ptr->can_queue + AAC_NUM_MGT_FIB);
+	init->MaxIoSize = cpu_to_le32(dev->scsi_host_ptr->max_sectors << 9);
+	init->MaxFibSize = cpu_to_le32(dev->max_fib_size);
+	init->MaxNumAif = cpu_to_le32(dev->max_num_aif);
+
 	if (dev->comm_interface == AAC_COMM_MESSAGE) {
 		init->InitFlags |= cpu_to_le32(INITFLAGS_NEW_COMM_SUPPORTED);
 		dprintk((KERN_WARNING"aacraid: New Comm Interface enabled\n"));
 	} else if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1) {
 		init->InitStructRevision = cpu_to_le32(ADAPTER_INIT_STRUCT_REVISION_6);
-		init->InitFlags |= cpu_to_le32(INITFLAGS_NEW_COMM_TYPE1_SUPPORTED);
-		dprintk((KERN_WARNING
-			"aacraid: New Comm Interface type1 enabled\n"));
+		init->InitFlags |= cpu_to_le32(INITFLAGS_NEW_COMM_SUPPORTED |
+			INITFLAGS_NEW_COMM_TYPE1_SUPPORTED | INITFLAGS_FAST_JBOD_SUPPORTED);
+		init->HostRRQ_AddrHigh = cpu_to_le32((u32)((u64)dev->host_rrq_pa >> 32));
+		init->HostRRQ_AddrLow = cpu_to_le32((u32)(dev->host_rrq_pa & 0xffffffff));
+		dprintk((KERN_WARNING"aacraid: New Comm Interface type1 enabled\n"));
+	} else if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE2) {
+		init->InitStructRevision = cpu_to_le32(ADAPTER_INIT_STRUCT_REVISION_7);
+		init->InitFlags |= cpu_to_le32(INITFLAGS_NEW_COMM_SUPPORTED |
+			INITFLAGS_NEW_COMM_TYPE2_SUPPORTED | INITFLAGS_FAST_JBOD_SUPPORTED);
+		init->HostRRQ_AddrHigh = cpu_to_le32((u32)((u64)dev->host_rrq_pa >> 32));
+		init->HostRRQ_AddrLow = cpu_to_le32((u32)(dev->host_rrq_pa & 0xffffffff));
+		init->MiniPortRevision = cpu_to_le32(0L);		/* number of MSI-X */
+		dprintk((KERN_WARNING"aacraid: New Comm Interface type2 enabled\n"));
 	}
-	init->InitFlags |= cpu_to_le32(INITFLAGS_DRIVER_USES_UTC_TIME |
-				       INITFLAGS_DRIVER_SUPPORTS_PM);
-	init->MaxIoCommands = cpu_to_le32(dev->scsi_host_ptr->can_queue + AAC_NUM_MGT_FIB);
-	init->MaxIoSize = cpu_to_le32(dev->scsi_host_ptr->max_sectors << 9);
-	init->MaxFibSize = cpu_to_le32(dev->max_fib_size);
-
-	init->MaxNumAif = cpu_to_le32(dev->max_num_aif);
-	init->HostRRQ_AddrHigh = (u32)((u64)dev->host_rrq_pa >> 32);
-	init->HostRRQ_AddrLow = (u32)(dev->host_rrq_pa & 0xffffffff);
-
 
 	/*
 	 * Increment the base address by the amount already used
@@ -354,13 +362,15 @@ struct aac_dev *aac_init_adapter(struct aac_dev *dev)
 			if ((status[1] & le32_to_cpu(AAC_OPT_NEW_COMM_TYPE1))) {
 				/* driver supports TYPE1 (Tupelo) */
 				dev->comm_interface = AAC_COMM_MESSAGE_TYPE1;
+			} else if ((status[1] & le32_to_cpu(AAC_OPT_NEW_COMM_TYPE2))) {
+				/* driver supports TYPE2 (Denali) */
+				dev->comm_interface = AAC_COMM_MESSAGE_TYPE2;
 			} else if ((status[1] & le32_to_cpu(AAC_OPT_NEW_COMM_TYPE4)) ||
-				  (status[1] & le32_to_cpu(AAC_OPT_NEW_COMM_TYPE3)) ||
-				  (status[1] & le32_to_cpu(AAC_OPT_NEW_COMM_TYPE2))) {
-					/* driver doesn't support TYPE2 (Series7), TYPE3 and TYPE4 */
-					/* switch to sync. mode */
-					dev->comm_interface = AAC_COMM_MESSAGE_TYPE1;
-					dev->sync_mode = 1;
+				  (status[1] & le32_to_cpu(AAC_OPT_NEW_COMM_TYPE3))) {
+				/* driver doesn't TYPE3 and TYPE4 */
+				/* switch to sync. mode */
+				dev->comm_interface = AAC_COMM_MESSAGE_TYPE2;
+				dev->sync_mode = 1;
 			}
 		}
 		if ((dev->comm_interface == AAC_COMM_MESSAGE) &&
diff --git a/drivers/scsi/aacraid/commsup.c b/drivers/scsi/aacraid/commsup.c
index 4b32ca44243..1be0776a80c 100644
--- a/drivers/scsi/aacraid/commsup.c
+++ b/drivers/scsi/aacraid/commsup.c
@@ -136,6 +136,7 @@ int aac_fib_setup(struct aac_dev * dev)
 		i < (dev->scsi_host_ptr->can_queue + AAC_NUM_MGT_FIB);
 		i++, fibptr++)
 	{
+		fibptr->flags = 0;
 		fibptr->dev = dev;
 		fibptr->hw_fib_va = hw_fib;
 		fibptr->data = (void *) fibptr->hw_fib_va->data;
@@ -240,11 +241,11 @@ void aac_fib_init(struct fib *fibptr)
 {
 	struct hw_fib *hw_fib = fibptr->hw_fib_va;
 
+	memset(&hw_fib->header, 0, sizeof(struct aac_fibhdr));
 	hw_fib->header.StructType = FIB_MAGIC;
 	hw_fib->header.Size = cpu_to_le16(fibptr->dev->max_fib_size);
 	hw_fib->header.XferState = cpu_to_le32(HostOwned | FibInitialized | FibEmpty | FastResponseCapable);
-	hw_fib->header.SenderFibAddress = 0; /* Filled in later if needed */
-	hw_fib->header.ReceiverFibAddress = cpu_to_le32(fibptr->hw_fib_pa);
+	hw_fib->header.u.ReceiverFibAddress = cpu_to_le32(fibptr->hw_fib_pa);
 	hw_fib->header.SenderSize = cpu_to_le16(fibptr->dev->max_fib_size);
 }
 
@@ -259,7 +260,6 @@ void aac_fib_init(struct fib *fibptr)
 static void fib_dealloc(struct fib * fibptr)
 {
 	struct hw_fib *hw_fib = fibptr->hw_fib_va;
-	BUG_ON(hw_fib->header.StructType != FIB_MAGIC);
 	hw_fib->header.XferState = 0;
 }
 
@@ -370,7 +370,7 @@ int aac_queue_get(struct aac_dev * dev, u32 * index, u32 qid, struct hw_fib * hw
 		entry->size = cpu_to_le32(le16_to_cpu(hw_fib->header.Size));
 		entry->addr = hw_fib->header.SenderFibAddress;
 			/* Restore adapters pointer to the FIB */
-		hw_fib->header.ReceiverFibAddress = hw_fib->header.SenderFibAddress;	/* Let the adapter now where to find its data */
+		hw_fib->header.u.ReceiverFibAddress = hw_fib->header.SenderFibAddress;  /* Let the adapter now where to find its data */
 		map = 0;
 	}
 	/*
@@ -450,7 +450,7 @@ int aac_fib_send(u16 command, struct fib *fibptr, unsigned long size,
 	 */
 
 	hw_fib->header.SenderFibAddress = cpu_to_le32(((u32)(fibptr - dev->fibs)) << 2);
-	hw_fib->header.SenderData = (u32)(fibptr - dev->fibs);
+	hw_fib->header.Handle = (u32)(fibptr - dev->fibs) + 1;
 	/*
 	 *	Set FIB state to indicate where it came from and if we want a
 	 *	response from the adapter. Also load the command from the
@@ -460,7 +460,6 @@ int aac_fib_send(u16 command, struct fib *fibptr, unsigned long size,
 	 */
 	hw_fib->header.Command = cpu_to_le16(command);
 	hw_fib->header.XferState |= cpu_to_le32(SentFromHost);
-	fibptr->hw_fib_va->header.Flags = 0;	/* 0 the flags field - internal only*/
 	/*
 	 *	Set the size of the Fib we want to send to the adapter
 	 */
@@ -564,10 +563,10 @@ int aac_fib_send(u16 command, struct fib *fibptr, unsigned long size,
 			 * functioning because an interrupt routing or other
 			 * hardware failure has occurred.
 			 */
-			unsigned long count = 36000000L; /* 3 minutes */
+			unsigned long timeout = jiffies + (180 * HZ); /* 3 minutes */
 			while (down_trylock(&fibptr->event_wait)) {
 				int blink;
-				if (--count == 0) {
+				if (time_is_before_eq_jiffies(timeout)) {
 					struct aac_queue * q = &dev->queues->queue[AdapNormCmdQueue];
 					spin_lock_irqsave(q->lock, qflags);
 					q->numpending--;
@@ -588,7 +587,10 @@ int aac_fib_send(u16 command, struct fib *fibptr, unsigned long size,
 					}
 					return -EFAULT;
 				}
-				udelay(5);
+				/* We used to udelay() here but that absorbed
+				 * a CPU when a timeout occured. Not very
+				 * useful. */
+				cpu_relax();
 			}
 		} else if (down_interruptible(&fibptr->event_wait)) {
 			/* Do nothing ... satisfy
@@ -708,7 +710,8 @@ int aac_fib_adapter_complete(struct fib *fibptr, unsigned short size)
 	unsigned long nointr = 0;
 	unsigned long qflags;
 
-	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1) {
+	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE1 ||
+	    dev->comm_interface == AAC_COMM_MESSAGE_TYPE2) {
 		kfree(hw_fib);
 		return 0;
 	}
@@ -721,7 +724,9 @@ int aac_fib_adapter_complete(struct fib *fibptr, unsigned short size)
 	/*
 	 *	If we plan to do anything check the structure type first.
 	 */
-	if (hw_fib->header.StructType != FIB_MAGIC) {
+	if (hw_fib->header.StructType != FIB_MAGIC &&
+	    hw_fib->header.StructType != FIB_MAGIC2 &&
+	    hw_fib->header.StructType != FIB_MAGIC2_64) {
 		if (dev->comm_interface == AAC_COMM_MESSAGE)
 			kfree(hw_fib);
 		return -EINVAL;
@@ -783,7 +788,9 @@ int aac_fib_complete(struct fib *fibptr)
 	 *	If we plan to do anything check the structure type first.
 	 */
 
-	if (hw_fib->header.StructType != FIB_MAGIC)
+	if (hw_fib->header.StructType != FIB_MAGIC &&
+	    hw_fib->header.StructType != FIB_MAGIC2 &&
+	    hw_fib->header.StructType != FIB_MAGIC2_64)
 		return -EINVAL;
 	/*
 	 *	This block completes a cdb which orginated on the host and we
diff --git a/drivers/scsi/aacraid/dpcsup.c b/drivers/scsi/aacraid/dpcsup.c
index f0c66a80ad1..d81b2810f0f 100644
--- a/drivers/scsi/aacraid/dpcsup.c
+++ b/drivers/scsi/aacraid/dpcsup.c
@@ -101,6 +101,7 @@ unsigned int aac_response_normal(struct aac_queue * q)
 			 */
 			*(__le32 *)hwfib->data = cpu_to_le32(ST_OK);
 			hwfib->header.XferState |= cpu_to_le32(AdapterProcessed);
+			fib->flags |= FIB_CONTEXT_FLAG_FASTRESP;
 		}
 
 		FIB_COUNTER_INCREMENT(aac_config.FibRecved);
@@ -121,7 +122,7 @@ unsigned int aac_response_normal(struct aac_queue * q)
 			 *	NOTE:  we cannot touch the fib after this
 			 *	    call, because it may have been deallocated.
 			 */
-			fib->flags = 0;
+			fib->flags &= FIB_CONTEXT_FLAG_FASTRESP;
 			fib->callback(fib->callback_data, fib);
 		} else {
 			unsigned long flagv;
@@ -367,6 +368,7 @@ unsigned int aac_intr_normal(struct aac_dev *dev, u32 index,
 			 */
 			*(__le32 *)hwfib->data = cpu_to_le32(ST_OK);
 			hwfib->header.XferState |= cpu_to_le32(AdapterProcessed);
+			fib->flags |= FIB_CONTEXT_FLAG_FASTRESP;
 		}
 
 		FIB_COUNTER_INCREMENT(aac_config.FibRecved);
@@ -387,7 +389,7 @@ unsigned int aac_intr_normal(struct aac_dev *dev, u32 index,
 			 *	NOTE:  we cannot touch the fib after this
 			 *	    call, because it may have been deallocated.
 			 */
-			fib->flags = 0;
+			fib->flags &= FIB_CONTEXT_FLAG_FASTRESP;
 			fib->callback(fib->callback_data, fib);
 		} else {
 			unsigned long flagv;
diff --git a/drivers/scsi/aacraid/linit.c b/drivers/scsi/aacraid/linit.c
index 0d279c445a3..7199534cd07 100644
--- a/drivers/scsi/aacraid/linit.c
+++ b/drivers/scsi/aacraid/linit.c
@@ -1089,8 +1089,17 @@ static struct scsi_host_template aac_driver_template = {
 
 static void __aac_shutdown(struct aac_dev * aac)
 {
-	if (aac->aif_thread)
+	if (aac->aif_thread) {
+		int i;
+		/* Clear out events first */
+		for (i = 0; i < (aac->scsi_host_ptr->can_queue + AAC_NUM_MGT_FIB); i++) {
+			struct fib *fib = &aac->fibs[i];
+			if (!(fib->hw_fib_va->header.XferState & cpu_to_le32(NoResponseExpected | Async)) &&
+			    (fib->hw_fib_va->header.XferState & cpu_to_le32(ResponseExpected)))
+				up(&fib->event_wait);
+		}
 		kthread_stop(aac->thread);
+	}
 	aac_send_shutdown(aac);
 	aac_adapter_disable_int(aac);
 	free_irq(aac->pdev->irq, aac);
@@ -1145,11 +1154,11 @@ static int __devinit aac_probe_one(struct pci_dev *pdev,
 		goto out_disable_pdev;
 
 	shost->irq = pdev->irq;
-	shost->base = pci_resource_start(pdev, 0);
 	shost->unique_id = unique_id;
 	shost->max_cmd_len = 16;
 
 	aac = (struct aac_dev *)shost->hostdata;
+	aac->base_start = pci_resource_start(pdev, 0);
 	aac->scsi_host_ptr = shost;
 	aac->pdev = pdev;
 	aac->name = aac_driver_template.name;
@@ -1157,7 +1166,7 @@ static int __devinit aac_probe_one(struct pci_dev *pdev,
 	aac->cardtype = index;
 	INIT_LIST_HEAD(&aac->entry);
 
-	aac->fibs = kmalloc(sizeof(struct fib) * (shost->can_queue + AAC_NUM_MGT_FIB), GFP_KERNEL);
+	aac->fibs = kzalloc(sizeof(struct fib) * (shost->can_queue + AAC_NUM_MGT_FIB), GFP_KERNEL);
 	if (!aac->fibs)
 		goto out_free_host;
 	spin_lock_init(&aac->fib_lock);
@@ -1191,6 +1200,7 @@ static int __devinit aac_probe_one(struct pci_dev *pdev,
 	if (IS_ERR(aac->thread)) {
 		printk(KERN_ERR "aacraid: Unable to create command thread.\n");
 		error = PTR_ERR(aac->thread);
+		aac->thread = NULL;
 		goto out_deinit;
 	}
 
diff --git a/drivers/scsi/aacraid/nark.c b/drivers/scsi/aacraid/nark.c
index f397d21a0c0..6c53b1d8b2b 100644
--- a/drivers/scsi/aacraid/nark.c
+++ b/drivers/scsi/aacraid/nark.c
@@ -49,14 +49,14 @@ static int aac_nark_ioremap(struct aac_dev * dev, u32 size)
 		dev->base = NULL;
 		return 0;
 	}
-	dev->scsi_host_ptr->base = pci_resource_start(dev->pdev, 2);
+	dev->base_start = pci_resource_start(dev->pdev, 2);
 	dev->regs.rx = ioremap((u64)pci_resource_start(dev->pdev, 0) |
 	  ((u64)pci_resource_start(dev->pdev, 1) << 32),
 	  sizeof(struct rx_registers) - sizeof(struct rx_inbound));
 	dev->base = NULL;
 	if (dev->regs.rx == NULL)
 		return -1;
-	dev->base = ioremap(dev->scsi_host_ptr->base, size);
+	dev->base = ioremap(dev->base_start, size);
 	if (dev->base == NULL) {
 		iounmap(dev->regs.rx);
 		dev->regs.rx = NULL;
diff --git a/drivers/scsi/aacraid/rkt.c b/drivers/scsi/aacraid/rkt.c
index be44de92429..7d8013feedd 100644
--- a/drivers/scsi/aacraid/rkt.c
+++ b/drivers/scsi/aacraid/rkt.c
@@ -79,7 +79,7 @@ static int aac_rkt_ioremap(struct aac_dev * dev, u32 size)
 		iounmap(dev->regs.rkt);
 		return 0;
 	}
-	dev->base = dev->regs.rkt = ioremap(dev->scsi_host_ptr->base, size);
+	dev->base = dev->regs.rkt = ioremap(dev->base_start, size);
 	if (dev->base == NULL)
 		return -1;
 	dev->IndexRegs = &dev->regs.rkt->IndexRegs;
diff --git a/drivers/scsi/aacraid/rx.c b/drivers/scsi/aacraid/rx.c
index b029c7cc785..dada38aeacc 100644
--- a/drivers/scsi/aacraid/rx.c
+++ b/drivers/scsi/aacraid/rx.c
@@ -471,7 +471,7 @@ static int aac_rx_ioremap(struct aac_dev * dev, u32 size)
 		iounmap(dev->regs.rx);
 		return 0;
 	}
-	dev->base = dev->regs.rx = ioremap(dev->scsi_host_ptr->base, size);
+	dev->base = dev->regs.rx = ioremap(dev->base_start, size);
 	if (dev->base == NULL)
 		return -1;
 	dev->IndexRegs = &dev->regs.rx->IndexRegs;
@@ -653,7 +653,7 @@ int _aac_rx_init(struct aac_dev *dev)
 			name, instance);
 		goto error_iounmap;
 	}
-	dev->dbg_base = dev->scsi_host_ptr->base;
+	dev->dbg_base = dev->base_start;
 	dev->dbg_base_mapped = dev->base;
 	dev->dbg_size = dev->base_size;
 
diff --git a/drivers/scsi/aacraid/sa.c b/drivers/scsi/aacraid/sa.c
index beb533630d4..2244f315f33 100644
--- a/drivers/scsi/aacraid/sa.c
+++ b/drivers/scsi/aacraid/sa.c
@@ -305,7 +305,7 @@ static int aac_sa_ioremap(struct aac_dev * dev, u32 size)
 		iounmap(dev->regs.sa);
 		return 0;
 	}
-	dev->base = dev->regs.sa = ioremap(dev->scsi_host_ptr->base, size);
+	dev->base = dev->regs.sa = ioremap(dev->base_start, size);
 	return (dev->base == NULL) ? -1 : 0;
 }
 
@@ -393,7 +393,7 @@ int aac_sa_init(struct aac_dev *dev)
 			name, instance);
 		goto error_iounmap;
 	}
-	dev->dbg_base = dev->scsi_host_ptr->base;
+	dev->dbg_base = dev->base_start;
 	dev->dbg_base_mapped = dev->base;
 	dev->dbg_size = dev->base_size;
 
diff --git a/drivers/scsi/aacraid/src.c b/drivers/scsi/aacraid/src.c
index 76282063630..3b021ec6325 100644
--- a/drivers/scsi/aacraid/src.c
+++ b/drivers/scsi/aacraid/src.c
@@ -56,25 +56,14 @@ static irqreturn_t aac_src_intr_message(int irq, void *dev_id)
 	if (bellbits & PmDoorBellResponseSent) {
 		bellbits = PmDoorBellResponseSent;
 		/* handle async. status */
+		src_writel(dev, MUnit.ODR_C, bellbits);
+		src_readl(dev, MUnit.ODR_C);
 		our_interrupt = 1;
 		index = dev->host_rrq_idx;
-		if (dev->host_rrq[index] == 0) {
-			u32 old_index = index;
-			/* adjust index */
-			do {
-				index++;
-				if (index == dev->scsi_host_ptr->can_queue +
-							AAC_NUM_MGT_FIB)
-					index = 0;
-				if (dev->host_rrq[index] != 0)
-					break;
-			} while (index != old_index);
-			dev->host_rrq_idx = index;
-		}
 		for (;;) {
 			isFastResponse = 0;
 			/* remove toggle bit (31) */
-			handle = (dev->host_rrq[index] & 0x7fffffff);
+			handle = le32_to_cpu(dev->host_rrq[index]) & 0x7fffffff;
 			/* check fast response bit (30) */
 			if (handle & 0x40000000)
 				isFastResponse = 1;
@@ -93,6 +82,8 @@ static irqreturn_t aac_src_intr_message(int irq, void *dev_id)
 	} else {
 		bellbits_shifted = (bellbits >> SRC_ODR_SHIFT);
 		if (bellbits_shifted & DoorBellAifPending) {
+			src_writel(dev, MUnit.ODR_C, bellbits);
+			src_readl(dev, MUnit.ODR_C);
 			our_interrupt = 1;
 			/* handle AIF */
 			aac_intr_normal(dev, 0, 2, 0, NULL);
@@ -100,6 +91,13 @@ static irqreturn_t aac_src_intr_message(int irq, void *dev_id)
 			unsigned long sflags;
 			struct list_head *entry;
 			int send_it = 0;
+			extern int aac_sync_mode;
+
+			if (!aac_sync_mode) {
+				src_writel(dev, MUnit.ODR_C, bellbits);
+				src_readl(dev, MUnit.ODR_C);
+				our_interrupt = 1;
+			}
 
 			if (dev->sync_fib) {
 				our_interrupt = 1;
@@ -132,7 +130,6 @@ static irqreturn_t aac_src_intr_message(int irq, void *dev_id)
 	}
 
 	if (our_interrupt) {
-		src_writel(dev, MUnit.ODR_C, bellbits);
 		return IRQ_HANDLED;
 	}
 	return IRQ_NONE;
@@ -336,6 +333,9 @@ static void aac_src_start_adapter(struct aac_dev *dev)
 {
 	struct aac_init *init;
 
+	 /* reset host_rrq_idx first */
+	dev->host_rrq_idx = 0;
+
 	init = dev->init;
 	init->HostElapsedSeconds = cpu_to_le32(get_seconds());
 
@@ -389,30 +389,51 @@ static int aac_src_deliver_message(struct fib *fib)
 	struct aac_queue *q = &dev->queues->queue[AdapNormCmdQueue];
 	unsigned long qflags;
 	u32 fibsize;
-	u64 address;
+	dma_addr_t address;
 	struct aac_fib_xporthdr *pFibX;
+	u16 hdr_size = le16_to_cpu(fib->hw_fib_va->header.Size);
 
 	spin_lock_irqsave(q->lock, qflags);
 	q->numpending++;
 	spin_unlock_irqrestore(q->lock, qflags);
 
-	/* Calculate the amount to the fibsize bits */
-	fibsize = (sizeof(struct aac_fib_xporthdr) +
-		fib->hw_fib_va->header.Size + 127) / 128 - 1;
-	if (fibsize > (ALIGN32 - 1))
-		fibsize = ALIGN32 - 1;
-
-    /* Fill XPORT header */
-	pFibX = (struct aac_fib_xporthdr *)
-		((unsigned char *)fib->hw_fib_va -
-		sizeof(struct aac_fib_xporthdr));
-	pFibX->Handle = fib->hw_fib_va->header.SenderData + 1;
-	pFibX->HostAddress = fib->hw_fib_pa;
-	pFibX->Size = fib->hw_fib_va->header.Size;
-	address = fib->hw_fib_pa - (u64)sizeof(struct aac_fib_xporthdr);
-
-	src_writel(dev, MUnit.IQ_H, (u32)(address >> 32));
-	src_writel(dev, MUnit.IQ_L, (u32)(address & 0xffffffff) + fibsize);
+	if (dev->comm_interface == AAC_COMM_MESSAGE_TYPE2) {
+		/* Calculate the amount to the fibsize bits */
+		fibsize = (hdr_size + 127) / 128 - 1;
+		if (fibsize > (ALIGN32 - 1))
+			return -EMSGSIZE;
+		/* New FIB header, 32-bit */
+		address = fib->hw_fib_pa;
+		fib->hw_fib_va->header.StructType = FIB_MAGIC2;
+		fib->hw_fib_va->header.SenderFibAddress = (u32)address;
+		fib->hw_fib_va->header.u.TimeStamp = 0;
+		BUG_ON((u32)(address >> 32) != 0L);
+		address |= fibsize;
+	} else {
+		/* Calculate the amount to the fibsize bits */
+		fibsize = (sizeof(struct aac_fib_xporthdr) + hdr_size + 127) / 128 - 1;
+		if (fibsize > (ALIGN32 - 1))
+			return -EMSGSIZE;
+
+		/* Fill XPORT header */
+		pFibX = (void *)fib->hw_fib_va - sizeof(struct aac_fib_xporthdr);
+		pFibX->Handle = cpu_to_le32(fib->hw_fib_va->header.Handle);
+		pFibX->HostAddress = cpu_to_le64(fib->hw_fib_pa);
+		pFibX->Size = cpu_to_le32(hdr_size);
+
+		/*
+		 * The xport header has been 32-byte aligned for us so that fibsize
+		 * can be masked out of this address by hardware. -- BenC
+		 */
+		address = fib->hw_fib_pa - sizeof(struct aac_fib_xporthdr);
+		if (address & (ALIGN32 - 1))
+			return -EINVAL;
+		address |= fibsize;
+	}
+
+	src_writel(dev, MUnit.IQ_H, (address >> 32) & 0xffffffff);
+	src_writel(dev, MUnit.IQ_L, address & 0xffffffff);
+
 	return 0;
 }
 
@@ -435,8 +456,7 @@ static int aac_src_ioremap(struct aac_dev *dev, u32 size)
 	dev->base = NULL;
 	if (dev->regs.src.bar1 == NULL)
 		return -1;
-	dev->base = dev->regs.src.bar0 = ioremap(dev->scsi_host_ptr->base,
-				size);
+	dev->base = dev->regs.src.bar0 = ioremap(dev->base_start, size);
 	if (dev->base == NULL) {
 		iounmap(dev->regs.src.bar1);
 		dev->regs.src.bar1 = NULL;
@@ -459,7 +479,7 @@ static int aac_srcv_ioremap(struct aac_dev *dev, u32 size)
 		dev->base = dev->regs.src.bar0 = NULL;
 		return 0;
 	}
-	dev->base = dev->regs.src.bar0 = ioremap(dev->scsi_host_ptr->base, size);
+	dev->base = dev->regs.src.bar0 = ioremap(dev->base_start, size);
 	if (dev->base == NULL)
 		return -1;
 	dev->IndexRegs = &((struct src_registers __iomem *)
@@ -753,7 +773,7 @@ int aac_srcv_init(struct aac_dev *dev)
 
 	if (aac_init_adapter(dev) == NULL)
 		goto error_iounmap;
-	if (dev->comm_interface != AAC_COMM_MESSAGE_TYPE1)
+	if (dev->comm_interface != AAC_COMM_MESSAGE_TYPE2)
 		goto error_iounmap;
 	dev->msi = aac_msi && !pci_enable_msi(dev->pdev);
 	if (request_irq(dev->pdev->irq, dev->a_ops.adapter_intr,
@@ -764,7 +784,7 @@ int aac_srcv_init(struct aac_dev *dev)
 			name, instance);
 		goto error_iounmap;
 	}
-	dev->dbg_base = dev->scsi_host_ptr->base;
+	dev->dbg_base = dev->base_start;
 	dev->dbg_base_mapped = dev->base;
 	dev->dbg_size = dev->base_size;
 
diff --git a/drivers/scsi/aha152x.c b/drivers/scsi/aha152x.c
index 19a36945e6f..dd4547bf688 100644
--- a/drivers/scsi/aha152x.c
+++ b/drivers/scsi/aha152x.c
@@ -2984,8 +2984,8 @@ static int get_command(char *pos, Scsi_Cmnd * ptr)
 	char *start = pos;
 	int i;
 
-	SPRINTF("0x%08x: target=%d; lun=%d; cmnd=( ",
-		(unsigned int) ptr, ptr->device->id, ptr->device->lun);
+	SPRINTF("%p: target=%d; lun=%d; cmnd=( ",
+		ptr, ptr->device->id, ptr->device->lun);
 
 	for (i = 0; i < COMMAND_SIZE(ptr->cmnd[0]); i++)
 		SPRINTF("0x%02x ", ptr->cmnd[i]);
diff --git a/drivers/scsi/aha1542.c b/drivers/scsi/aha1542.c
index f79c8f9e33a..770c48ddbe5 100644
--- a/drivers/scsi/aha1542.c
+++ b/drivers/scsi/aha1542.c
@@ -49,7 +49,7 @@
 #define SCSI_BUF_PA(address)	isa_virt_to_bus(address)
 #define SCSI_SG_PA(sgent)	(isa_page_to_bus(sg_page((sgent))) + (sgent)->offset)
 
-#include<linux/stat.h>
+#include <linux/stat.h>
 
 #ifdef DEBUG
 #define DEB(x) x
diff --git a/drivers/scsi/arcmsr/arcmsr_hba.c b/drivers/scsi/arcmsr/arcmsr_hba.c
index cbde1dca45a..def24a1079a 100644
--- a/drivers/scsi/arcmsr/arcmsr_hba.c
+++ b/drivers/scsi/arcmsr/arcmsr_hba.c
@@ -2821,7 +2821,7 @@ static void arcmsr_hardware_reset(struct AdapterControlBlock *acb)
 	int i, count = 0;
 	struct MessageUnit_A __iomem *pmuA = acb->pmuA;
 	struct MessageUnit_C __iomem *pmuC = acb->pmuC;
-	u32 temp = 0;
+
 	/* backup pci config data */
 	printk(KERN_NOTICE "arcmsr%d: executing hw bus reset .....\n", acb->host->host_no);
 	for (i = 0; i < 64; i++) {
@@ -2839,7 +2839,7 @@ static void arcmsr_hardware_reset(struct AdapterControlBlock *acb)
 			writel(0x2, &pmuC->write_sequence);
 			writel(0x7, &pmuC->write_sequence);
 			writel(0xD, &pmuC->write_sequence);
-		} while ((((temp = readl(&pmuC->host_diagnostic)) | ARCMSR_ARC1880_DiagWrite_ENABLE) == 0) && (count < 5));
+		} while (((readl(&pmuC->host_diagnostic) & ARCMSR_ARC1880_DiagWrite_ENABLE) == 0) && (count < 5));
 		writel(ARCMSR_ARC1880_RESET_ADAPTER, &pmuC->host_diagnostic);
 	} else {
 		pci_write_config_byte(acb->pdev, 0x84, 0x20);
diff --git a/drivers/scsi/bfa/bfa_fcs_lport.c b/drivers/scsi/bfa/bfa_fcs_lport.c
index 937000db62a..bcc4966e8ba 100644
--- a/drivers/scsi/bfa/bfa_fcs_lport.c
+++ b/drivers/scsi/bfa/bfa_fcs_lport.c
@@ -5722,9 +5722,7 @@ bfa_fcs_vport_free(struct bfa_fcs_vport_s *vport)
 	 * The memory for the bfad_vport_s is freed from the FC function
 	 * template vport_delete entry point.
 	 */
-	if (vport_drv)
-		bfad_im_port_delete(vport_drv->drv_port.bfad,
-				&vport_drv->drv_port);
+	bfad_im_port_delete(vport_drv->drv_port.bfad, &vport_drv->drv_port);
 }
 
 /*
diff --git a/drivers/scsi/bfa/bfa_ioc.c b/drivers/scsi/bfa/bfa_ioc.c
index 14e6284e48e..8cdb79c2fcd 100644
--- a/drivers/scsi/bfa/bfa_ioc.c
+++ b/drivers/scsi/bfa/bfa_ioc.c
@@ -2357,7 +2357,7 @@ bfa_ioc_mbox_isr(struct bfa_ioc_s *ioc)
 			return;
 		}
 
-		if ((mc > BFI_MC_MAX) || (mod->mbhdlr[mc].cbfn == NULL))
+		if ((mc >= BFI_MC_MAX) || (mod->mbhdlr[mc].cbfn == NULL))
 			return;
 
 		mod->mbhdlr[mc].cbfn(mod->mbhdlr[mc].cbarg, &m);
diff --git a/drivers/scsi/bfa/bfad.c b/drivers/scsi/bfa/bfad.c
index 2e4b0be14a2..2c8f0c71307 100644
--- a/drivers/scsi/bfa/bfad.c
+++ b/drivers/scsi/bfa/bfad.c
@@ -1383,6 +1383,8 @@ bfad_pci_probe(struct pci_dev *pdev, const struct pci_device_id *pid)
 	bfa_sm_set_state(bfad, bfad_sm_uninit);
 
 	spin_lock_init(&bfad->bfad_lock);
+	spin_lock_init(&bfad->bfad_aen_spinlock);
+
 	pci_set_drvdata(pdev, bfad);
 
 	bfad->ref_count = 0;
diff --git a/drivers/scsi/bfa/bfad_bsg.c b/drivers/scsi/bfa/bfad_bsg.c
index e1f4b10df42..9c1495b321d 100644
--- a/drivers/scsi/bfa/bfad_bsg.c
+++ b/drivers/scsi/bfa/bfad_bsg.c
@@ -3008,12 +3008,15 @@ bfad_im_bsg_els_ct_request(struct fc_bsg_job *job)
 	 * buffer of size bsg_data->payload_len
 	 */
 	bsg_fcpt = kzalloc(bsg_data->payload_len, GFP_KERNEL);
-	if (!bsg_fcpt)
+	if (!bsg_fcpt) {
+		rc = -ENOMEM;
 		goto out;
+	}
 
 	if (copy_from_user((uint8_t *)bsg_fcpt, bsg_data->payload,
 				bsg_data->payload_len)) {
 		kfree(bsg_fcpt);
+		rc = -EIO;
 		goto out;
 	}
 
diff --git a/drivers/scsi/bfa/bfad_im.c b/drivers/scsi/bfa/bfad_im.c
index 1ac09afe35e..2eebf8d4d58 100644
--- a/drivers/scsi/bfa/bfad_im.c
+++ b/drivers/scsi/bfa/bfad_im.c
@@ -687,25 +687,21 @@ bfa_status_t
 bfad_im_probe(struct bfad_s *bfad)
 {
 	struct bfad_im_s      *im;
-	bfa_status_t    rc = BFA_STATUS_OK;
 
 	im = kzalloc(sizeof(struct bfad_im_s), GFP_KERNEL);
-	if (im == NULL) {
-		rc = BFA_STATUS_ENOMEM;
-		goto ext;
-	}
+	if (im == NULL)
+		return BFA_STATUS_ENOMEM;
 
 	bfad->im = im;
 	im->bfad = bfad;
 
 	if (bfad_thread_workq(bfad) != BFA_STATUS_OK) {
 		kfree(im);
-		rc = BFA_STATUS_FAILED;
+		return BFA_STATUS_FAILED;
 	}
 
 	INIT_WORK(&im->aen_im_notify_work, bfad_aen_im_notify_handler);
-ext:
-	return rc;
+	return BFA_STATUS_OK;
 }
 
 void
diff --git a/drivers/scsi/bnx2fc/Makefile b/drivers/scsi/bnx2fc/Makefile
index a92695a2517..141149e8cda 100644
--- a/drivers/scsi/bnx2fc/Makefile
+++ b/drivers/scsi/bnx2fc/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_SCSI_BNX2X_FCOE) += bnx2fc.o
 
-bnx2fc-y := bnx2fc_els.o bnx2fc_fcoe.o bnx2fc_hwi.o bnx2fc_io.o bnx2fc_tgt.o
+bnx2fc-y := bnx2fc_els.o bnx2fc_fcoe.o bnx2fc_hwi.o bnx2fc_io.o bnx2fc_tgt.o \
+	    bnx2fc_debug.o
diff --git a/drivers/scsi/bnx2fc/bnx2fc.h b/drivers/scsi/bnx2fc/bnx2fc.h
index 0578fa0dc14..3486845ba30 100644
--- a/drivers/scsi/bnx2fc/bnx2fc.h
+++ b/drivers/scsi/bnx2fc/bnx2fc.h
@@ -11,6 +11,8 @@
  * Written by: Bhanu Prakash Gollapudi (bprakash@broadcom.com)
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/kernel.h>
@@ -57,12 +59,12 @@
 #include <scsi/fc/fc_fcp.h>
 
 #include "57xx_hsi_bnx2fc.h"
-#include "bnx2fc_debug.h"
 #include "../../net/ethernet/broadcom/cnic_if.h"
+#include  "../../net/ethernet/broadcom/bnx2x/bnx2x_mfw_req.h"
 #include "bnx2fc_constants.h"
 
 #define BNX2FC_NAME		"bnx2fc"
-#define BNX2FC_VERSION		"1.0.11"
+#define BNX2FC_VERSION		"1.0.12"
 
 #define PFX			"bnx2fc: "
 
@@ -84,6 +86,8 @@
 #define BNX2FC_NUM_MAX_SESS	1024
 #define BNX2FC_NUM_MAX_SESS_LOG	(ilog2(BNX2FC_NUM_MAX_SESS))
 
+#define BNX2FC_MAX_NPIV		256
+
 #define BNX2FC_MAX_OUTSTANDING_CMNDS	2048
 #define BNX2FC_CAN_QUEUE		BNX2FC_MAX_OUTSTANDING_CMNDS
 #define BNX2FC_ELSTM_XIDS		BNX2FC_CAN_QUEUE
@@ -206,6 +210,7 @@ struct bnx2fc_hba {
 	struct fcoe_statistics_params *stats_buffer;
 	dma_addr_t stats_buf_dma;
 	struct completion stat_req_done;
+	struct fcoe_capabilities fcoe_cap;
 
 	/*destroy handling */
 	struct timer_list destroy_timer;
@@ -274,6 +279,7 @@ struct bnx2fc_rport {
 #define BNX2FC_FLAG_CTX_ALLOC_FAILURE	0x6
 #define BNX2FC_FLAG_UPLD_REQ_COMPL	0x7
 #define BNX2FC_FLAG_EXPL_LOGO		0x8
+#define BNX2FC_FLAG_DISABLE_FAILED	0x9
 
 	u8 src_addr[ETH_ALEN];
 	u32 max_sqes;
@@ -554,4 +560,7 @@ void bnx2fc_process_seq_cleanup_compl(struct bnx2fc_cmd *seq_clnup_req,
 int bnx2fc_initiate_seq_cleanup(struct bnx2fc_cmd *orig_io_req, u32 offset,
 				enum fc_rctl r_ctl);
 
+
+#include "bnx2fc_debug.h"
+
 #endif
diff --git a/drivers/scsi/bnx2fc/bnx2fc_debug.c b/drivers/scsi/bnx2fc/bnx2fc_debug.c
new file mode 100644
index 00000000000..0cbee1b23ee
--- /dev/null
+++ b/drivers/scsi/bnx2fc/bnx2fc_debug.c
@@ -0,0 +1,70 @@
+#include "bnx2fc.h"
+
+void BNX2FC_IO_DBG(const struct bnx2fc_cmd *io_req, const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	if (likely(!(bnx2fc_debug_level & LOG_IO)))
+		return;
+
+	va_start(args, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	if (io_req && io_req->port && io_req->port->lport &&
+	    io_req->port->lport->host)
+		shost_printk(KERN_INFO, io_req->port->lport->host,
+			     PFX "xid:0x%x %pV",
+			     io_req->xid, &vaf);
+	else
+		pr_info("NULL %pV", &vaf);
+
+	va_end(args);
+}
+
+void BNX2FC_TGT_DBG(const struct bnx2fc_rport *tgt, const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	if (likely(!(bnx2fc_debug_level & LOG_TGT)))
+		return;
+
+	va_start(args, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	if (tgt && tgt->port && tgt->port->lport && tgt->port->lport->host &&
+	    tgt->rport)
+		shost_printk(KERN_INFO, tgt->port->lport->host,
+			     PFX "port:%x %pV",
+			     tgt->rport->port_id, &vaf);
+	else
+		pr_info("NULL %pV", &vaf);
+
+	va_end(args);
+}
+
+void BNX2FC_HBA_DBG(const struct fc_lport *lport, const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	if (likely(!(bnx2fc_debug_level & LOG_HBA)))
+		return;
+
+	va_start(args, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	if (lport && lport->host)
+		shost_printk(KERN_INFO, lport->host, PFX "%pV", &vaf);
+	else
+		pr_info("NULL %pV", &vaf);
+
+	va_end(args);
+}
diff --git a/drivers/scsi/bnx2fc/bnx2fc_debug.h b/drivers/scsi/bnx2fc/bnx2fc_debug.h
index 3416d9a746c..4808ff99621 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_debug.h
+++ b/drivers/scsi/bnx2fc/bnx2fc_debug.h
@@ -11,60 +11,23 @@
 
 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_INFO PFX fmt, ##arg))
-
-#define BNX2FC_MISC_DBG(fmt, arg...)					\
-	BNX2FC_CHK_LOGGING(LOG_MISC,					\
-			   printk(KERN_INFO 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_INFO PFX "NULL " fmt, ##arg));	\
-		else							\
-			BNX2FC_CHK_LOGGING(LOG_IO,			\
-			   shost_printk(KERN_INFO,			\
-				   (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_INFO PFX "NULL " fmt, ##arg));	\
-		else							\
-			BNX2FC_CHK_LOGGING(LOG_TGT,			\
-			   shost_printk(KERN_INFO,			\
-				   (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_INFO PFX "NULL " fmt, ##arg));	\
-		else							\
-			BNX2FC_CHK_LOGGING(LOG_HBA,			\
-			   shost_printk(KERN_INFO, lport->host,	\
-				   PFX fmt, ##arg));			\
-	} while (0)
+#define BNX2FC_ELS_DBG(fmt, ...)				\
+do {								\
+	if (unlikely(bnx2fc_debug_level & LOG_ELS))		\
+		pr_info(fmt, ##__VA_ARGS__);			\
+} while (0)
+
+#define BNX2FC_MISC_DBG(fmt, ...)				\
+do {								\
+	if (unlikely(bnx2fc_debug_level & LOG_MISC))		\
+		pr_info(fmt, ##__VA_ARGS__);			\
+} while (0)
+
+__printf(2, 3)
+void BNX2FC_IO_DBG(const struct bnx2fc_cmd *io_req, const char *fmt, ...);
+__printf(2, 3)
+void BNX2FC_TGT_DBG(const struct bnx2fc_rport *tgt, const char *fmt, ...);
+__printf(2, 3)
+void BNX2FC_HBA_DBG(const struct fc_lport *lport, const char *fmt, ...);
 
 #endif
diff --git a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
index f52f668fd24..ae1cb7639d9 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_fcoe.c
@@ -22,7 +22,7 @@ DEFINE_PER_CPU(struct bnx2fc_percpu_s, bnx2fc_percpu);
 
 #define DRV_MODULE_NAME		"bnx2fc"
 #define DRV_MODULE_VERSION	BNX2FC_VERSION
-#define DRV_MODULE_RELDATE	"Apr 24, 2012"
+#define DRV_MODULE_RELDATE	"Jun 04, 2012"
 
 
 static char version[] __devinitdata =
@@ -286,7 +286,7 @@ static int bnx2fc_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	struct fcoe_port	*port;
 	struct fcoe_hdr		*hp;
 	struct bnx2fc_rport	*tgt;
-	struct fcoe_dev_stats	*stats;
+	struct fc_stats		*stats;
 	u8			sof, eof;
 	u32			crc;
 	unsigned int		hlen, tlen, elen;
@@ -412,7 +412,7 @@ static int bnx2fc_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	}
 
 	/*update tx stats */
-	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	stats->TxFrames++;
 	stats->TxWords += wlen;
 	put_cpu();
@@ -522,7 +522,7 @@ 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_stats *stats;
 	struct fc_frame_header *fh;
 	struct fcoe_crc_eof crc_eof;
 	struct fc_frame *fp;
@@ -551,7 +551,7 @@ static void bnx2fc_recv_frame(struct sk_buff *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 = per_cpu_ptr(lport->stats, get_cpu());
 	stats->RxFrames++;
 	stats->RxWords += fr_len / FCOE_WORD_TO_BYTE;
 
@@ -942,7 +942,7 @@ static void bnx2fc_indicate_netevent(void *context, unsigned long event,
 							FC_PORTTYPE_UNKNOWN;
 			mutex_unlock(&lport->lp_mutex);
 			fc_host_port_type(lport->host) = FC_PORTTYPE_UNKNOWN;
-			per_cpu_ptr(lport->dev_stats,
+			per_cpu_ptr(lport->stats,
 				    get_cpu())->LinkFailureCount++;
 			put_cpu();
 			fcoe_clean_pending_queue(lport);
@@ -1326,6 +1326,7 @@ static void bnx2fc_hba_destroy(struct bnx2fc_hba *hba)
 static struct bnx2fc_hba *bnx2fc_hba_create(struct cnic_dev *cnic)
 {
 	struct bnx2fc_hba *hba;
+	struct fcoe_capabilities *fcoe_cap;
 	int rc;
 
 	hba = kzalloc(sizeof(*hba), GFP_KERNEL);
@@ -1361,6 +1362,21 @@ static struct bnx2fc_hba *bnx2fc_hba_create(struct cnic_dev *cnic)
 		printk(KERN_ERR PFX "em_config:bnx2fc_cmd_mgr_alloc failed\n");
 		goto cmgr_err;
 	}
+	fcoe_cap = &hba->fcoe_cap;
+
+	fcoe_cap->capability1 = BNX2FC_TM_MAX_SQES <<
+					FCOE_IOS_PER_CONNECTION_SHIFT;
+	fcoe_cap->capability1 |= BNX2FC_NUM_MAX_SESS <<
+					FCOE_LOGINS_PER_PORT_SHIFT;
+	fcoe_cap->capability2 = BNX2FC_MAX_OUTSTANDING_CMNDS <<
+					FCOE_NUMBER_OF_EXCHANGES_SHIFT;
+	fcoe_cap->capability2 |= BNX2FC_MAX_NPIV <<
+					FCOE_NPIV_WWN_PER_PORT_SHIFT;
+	fcoe_cap->capability3 = BNX2FC_NUM_MAX_SESS <<
+					FCOE_TARGETS_SUPPORTED_SHIFT;
+	fcoe_cap->capability3 |= BNX2FC_MAX_OUTSTANDING_CMNDS <<
+					FCOE_OUTSTANDING_COMMANDS_SHIFT;
+	fcoe_cap->capability4 = FCOE_CAPABILITY4_STATEFUL;
 
 	init_waitqueue_head(&hba->shutdown_wait);
 	init_waitqueue_head(&hba->destroy_wait);
@@ -1691,6 +1707,32 @@ static void bnx2fc_unbind_pcidev(struct bnx2fc_hba *hba)
 	hba->pcidev = NULL;
 }
 
+/**
+ * bnx2fc_ulp_get_stats - cnic callback to populate FCoE stats
+ *
+ * @handle:    transport handle pointing to adapter struture
+ */
+static int bnx2fc_ulp_get_stats(void *handle)
+{
+	struct bnx2fc_hba *hba = handle;
+	struct cnic_dev *cnic;
+	struct fcoe_stats_info *stats_addr;
+
+	if (!hba)
+		return -EINVAL;
+
+	cnic = hba->cnic;
+	stats_addr = &cnic->stats_addr->fcoe_stat;
+	if (!stats_addr)
+		return -EINVAL;
+
+	strncpy(stats_addr->version, BNX2FC_VERSION,
+		sizeof(stats_addr->version));
+	stats_addr->txq_size = BNX2FC_SQ_WQES_MAX;
+	stats_addr->rxq_size = BNX2FC_CQ_WQES_MAX;
+
+	return 0;
+}
 
 
 /**
@@ -1944,6 +1986,7 @@ static void bnx2fc_ulp_init(struct cnic_dev *dev)
 	adapter_count++;
 	mutex_unlock(&bnx2fc_dev_lock);
 
+	dev->fcoe_cap = &hba->fcoe_cap;
 	clear_bit(BNX2FC_CNIC_REGISTERED, &hba->reg_with_cnic);
 	rc = dev->register_device(dev, CNIC_ULP_FCOE,
 						(void *) hba);
@@ -2019,11 +2062,11 @@ static int bnx2fc_create(struct net_device *netdev, enum fip_state fip_mode)
 	struct fcoe_ctlr *ctlr;
 	struct bnx2fc_interface *interface;
 	struct bnx2fc_hba *hba;
-	struct net_device *phys_dev;
+	struct net_device *phys_dev = netdev;
 	struct fc_lport *lport;
 	struct ethtool_drvinfo drvinfo;
 	int rc = 0;
-	int vlan_id;
+	int vlan_id = 0;
 
 	BNX2FC_MISC_DBG("Entered bnx2fc_create\n");
 	if (fip_mode != FIP_MODE_FABRIC) {
@@ -2041,14 +2084,9 @@ static int bnx2fc_create(struct net_device *netdev, enum fip_state fip_mode)
 	}
 
 	/* obtain physical netdev */
-	if (netdev->priv_flags & IFF_802_1Q_VLAN) {
+	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));
@@ -2083,9 +2121,13 @@ static int bnx2fc_create(struct net_device *netdev, enum fip_state fip_mode)
 		goto ifput_err;
 	}
 
+	if (netdev->priv_flags & IFF_802_1Q_VLAN) {
+		vlan_id = vlan_dev_vlan_id(netdev);
+		interface->vlan_enabled = 1;
+	}
+
 	ctlr = bnx2fc_to_ctlr(interface);
 	interface->vlan_id = vlan_id;
-	interface->vlan_enabled = 1;
 
 	interface->timer_work_queue =
 			create_singlethread_workqueue("bnx2fc_timer_wq");
@@ -2152,13 +2194,10 @@ mod_err:
  **/
 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;
+	list_for_each_entry(hba, &adapter_list, list) {
 		if (hba->cnic == cnic)
 			return hba;
 	}
@@ -2252,15 +2291,17 @@ static int bnx2fc_fcoe_reset(struct Scsi_Host *shost)
 
 static bool bnx2fc_match(struct net_device *netdev)
 {
+	struct net_device *phys_dev = 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 (netdev->priv_flags & IFF_802_1Q_VLAN)
+		phys_dev = vlan_dev_real_dev(netdev);
 
-		if (bnx2fc_hba_lookup(phys_dev)) {
-			mutex_unlock(&bnx2fc_dev_lock);
-			return true;
-		}
+	if (bnx2fc_hba_lookup(phys_dev)) {
+		mutex_unlock(&bnx2fc_dev_lock);
+		return true;
 	}
+
 	mutex_unlock(&bnx2fc_dev_lock);
 	return false;
 }
@@ -2290,9 +2331,9 @@ static void bnx2fc_percpu_thread_create(unsigned int cpu)
 
 	p = &per_cpu(bnx2fc_percpu, cpu);
 
-	thread = kthread_create(bnx2fc_percpu_io_thread,
-				(void *)p,
-				"bnx2fc_thread/%d", cpu);
+	thread = kthread_create_on_node(bnx2fc_percpu_io_thread,
+					(void *)p, cpu_to_node(cpu),
+					"bnx2fc_thread/%d", cpu);
 	/* bind thread to the cpu */
 	if (likely(!IS_ERR(thread))) {
 		kthread_bind(thread, cpu);
@@ -2643,4 +2684,5 @@ static struct cnic_ulp_ops bnx2fc_cnic_cb = {
 	.cnic_stop		= bnx2fc_ulp_stop,
 	.indicate_kcqes		= bnx2fc_indicate_kcqe,
 	.indicate_netevent	= bnx2fc_indicate_netevent,
+	.cnic_get_stats		= bnx2fc_ulp_get_stats,
 };
diff --git a/drivers/scsi/bnx2fc/bnx2fc_hwi.c b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
index 2ca6bfe4ce5..6d6eee42ac7 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_hwi.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_hwi.c
@@ -1244,7 +1244,9 @@ static void bnx2fc_process_conn_disable_cmpl(struct bnx2fc_hba *hba,
 	if (disable_kcqe->completion_status) {
 		printk(KERN_ERR PFX "Disable failed with cmpl status %d\n",
 			disable_kcqe->completion_status);
-		return;
+		set_bit(BNX2FC_FLAG_DISABLE_FAILED, &tgt->flags);
+		set_bit(BNX2FC_FLAG_UPLD_REQ_COMPL, &tgt->flags);
+		wake_up_interruptible(&tgt->upld_wait);
 	} else {
 		/* disable successful */
 		BNX2FC_TGT_DBG(tgt, "disable successful\n");
diff --git a/drivers/scsi/bnx2fc/bnx2fc_io.c b/drivers/scsi/bnx2fc/bnx2fc_io.c
index 4f7453b9e41..73f231ccd45 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_io.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_io.c
@@ -405,11 +405,10 @@ free_cmd_pool:
 		goto free_cmgr;
 
 	for (i = 0; i < num_possible_cpus() + 1; i++)  {
-		struct list_head *list;
-		struct list_head *tmp;
+		struct bnx2fc_cmd *tmp, *io_req;
 
-		list_for_each_safe(list, tmp, &cmgr->free_list[i]) {
-			struct bnx2fc_cmd *io_req = (struct bnx2fc_cmd *)list;
+		list_for_each_entry_safe(io_req, tmp,
+					 &cmgr->free_list[i], link) {
 			list_del(&io_req->link);
 			kfree(io_req);
 		}
@@ -1436,9 +1435,7 @@ 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;
+	struct bnx2fc_cmd *cmd, *tmp;
 	int tm_lun = sc_cmd->device->lun;
 	int rc = 0;
 	int lun;
@@ -1449,9 +1446,8 @@ static void bnx2fc_lun_reset_cmpl(struct bnx2fc_cmd *io_req)
 	 * 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) {
+	list_for_each_entry_safe(cmd, tmp, &tgt->active_cmd_queue, link) {
 		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 */
@@ -1476,9 +1472,7 @@ static void bnx2fc_lun_reset_cmpl(struct bnx2fc_cmd *io_req)
 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;
+	struct bnx2fc_cmd *cmd, *tmp;
 	int rc = 0;
 
 	/* called with tgt_lock held */
@@ -1487,9 +1481,8 @@ static void bnx2fc_tgt_reset_cmpl(struct bnx2fc_cmd *io_req)
 	 * 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) {
+	list_for_each_entry_safe(cmd, tmp, &tgt->active_cmd_queue, link) {
 		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)) {
@@ -1980,7 +1973,7 @@ int bnx2fc_post_io_req(struct bnx2fc_rport *tgt,
 	struct bnx2fc_interface *interface = port->priv;
 	struct bnx2fc_hba *hba = interface->hba;
 	struct fc_lport *lport = port->lport;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 	int task_idx, index;
 	u16 xid;
 
@@ -1991,7 +1984,7 @@ int bnx2fc_post_io_req(struct bnx2fc_rport *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());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	if (sc_cmd->sc_data_direction == DMA_FROM_DEVICE) {
 		io_req->io_req_flags = BNX2FC_READ;
 		stats->InputRequests++;
diff --git a/drivers/scsi/bnx2fc/bnx2fc_tgt.c b/drivers/scsi/bnx2fc/bnx2fc_tgt.c
index 082a25c3117..b9d0d9cb17f 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_tgt.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_tgt.c
@@ -150,8 +150,7 @@ tgt_init_err:
 void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt)
 {
 	struct bnx2fc_cmd *io_req;
-	struct list_head *list;
-	struct list_head *tmp;
+	struct bnx2fc_cmd *tmp;
 	int rc;
 	int i = 0;
 	BNX2FC_TGT_DBG(tgt, "Entered flush_active_ios - %d\n",
@@ -160,9 +159,8 @@ void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt)
 	spin_lock_bh(&tgt->tgt_lock);
 	tgt->flush_in_prog = 1;
 
-	list_for_each_safe(list, tmp, &tgt->active_cmd_queue) {
+	list_for_each_entry_safe(io_req, tmp, &tgt->active_cmd_queue, link) {
 		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");
@@ -181,13 +179,18 @@ void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt)
 
 		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);
+
+		/* Do not issue cleanup when disable request failed */
+		if (test_bit(BNX2FC_FLAG_DISABLE_FAILED, &tgt->flags))
+			bnx2fc_process_cleanup_compl(io_req, io_req->task, 0);
+		else {
+			rc = bnx2fc_initiate_cleanup(io_req);
+			BUG_ON(rc);
+		}
 	}
 
-	list_for_each_safe(list, tmp, &tgt->active_tm_queue) {
+	list_for_each_entry_safe(io_req, tmp, &tgt->active_tm_queue, link) {
 		i++;
-		io_req = (struct bnx2fc_cmd *)list;
 		list_del_init(&io_req->link);
 		io_req->on_tmf_queue = 0;
 		BNX2FC_IO_DBG(io_req, "tm_queue cleanup\n");
@@ -195,9 +198,8 @@ void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt)
 			complete(&io_req->tm_done);
 	}
 
-	list_for_each_safe(list, tmp, &tgt->els_queue) {
+	list_for_each_entry_safe(io_req, tmp, &tgt->els_queue, link) {
 		i++;
-		io_req = (struct bnx2fc_cmd *)list;
 		list_del_init(&io_req->link);
 		io_req->on_active_queue = 0;
 
@@ -212,13 +214,17 @@ void bnx2fc_flush_active_ios(struct bnx2fc_rport *tgt)
 			io_req->cb_arg = NULL;
 		}
 
-		rc = bnx2fc_initiate_cleanup(io_req);
-		BUG_ON(rc);
+		/* Do not issue cleanup when disable request failed */
+		if (test_bit(BNX2FC_FLAG_DISABLE_FAILED, &tgt->flags))
+			bnx2fc_process_cleanup_compl(io_req, io_req->task, 0);
+		else {
+			rc = bnx2fc_initiate_cleanup(io_req);
+			BUG_ON(rc);
+		}
 	}
 
-	list_for_each_safe(list, tmp, &tgt->io_retire_queue) {
+	list_for_each_entry_safe(io_req, tmp, &tgt->io_retire_queue, link) {
 		i++;
-		io_req = (struct bnx2fc_cmd *)list;
 		list_del_init(&io_req->link);
 
 		BNX2FC_IO_DBG(io_req, "retire_queue flush\n");
@@ -321,9 +327,13 @@ static void bnx2fc_upload_session(struct fcoe_port *port,
 
 		del_timer_sync(&tgt->upld_timer);
 
-	} else
+	} else if (test_bit(BNX2FC_FLAG_DISABLE_FAILED, &tgt->flags)) {
+		printk(KERN_ERR PFX "ERROR!! DISABLE req failed, destroy"
+				" not sent to FW\n");
+	} else {
 		printk(KERN_ERR PFX "ERROR!! DISABLE req timed out, destroy"
 				" not sent to FW\n");
+	}
 
 	/* Free session resources */
 	bnx2fc_free_session_resc(hba, tgt);
diff --git a/drivers/scsi/bnx2i/57xx_iscsi_hsi.h b/drivers/scsi/bnx2i/57xx_iscsi_hsi.h
index dc0a08e69c8..f2db5fe7bdc 100644
--- a/drivers/scsi/bnx2i/57xx_iscsi_hsi.h
+++ b/drivers/scsi/bnx2i/57xx_iscsi_hsi.h
@@ -267,7 +267,13 @@ struct bnx2i_cmd_request {
  * task statistics for write response
  */
 struct bnx2i_write_resp_task_stat {
-	u32 num_data_ins;
+#if defined(__BIG_ENDIAN)
+	u16 num_r2ts;
+	u16 num_data_outs;
+#elif defined(__LITTLE_ENDIAN)
+	u16 num_data_outs;
+	u16 num_r2ts;
+#endif
 };
 
 /*
@@ -275,11 +281,11 @@ struct bnx2i_write_resp_task_stat {
  */
 struct bnx2i_read_resp_task_stat {
 #if defined(__BIG_ENDIAN)
-	u16 num_data_outs;
-	u16 num_r2ts;
+	u16 reserved;
+	u16 num_data_ins;
 #elif defined(__LITTLE_ENDIAN)
-	u16 num_r2ts;
-	u16 num_data_outs;
+	u16 num_data_ins;
+	u16 reserved;
 #endif
 };
 
diff --git a/drivers/scsi/bnx2i/bnx2i.h b/drivers/scsi/bnx2i/bnx2i.h
index 7e77cf62029..3f9e7061258 100644
--- a/drivers/scsi/bnx2i/bnx2i.h
+++ b/drivers/scsi/bnx2i/bnx2i.h
@@ -44,6 +44,8 @@
 #include "57xx_iscsi_hsi.h"
 #include "57xx_iscsi_constants.h"
 
+#include "../../net/ethernet/broadcom/bnx2x/bnx2x_mfw_req.h"
+
 #define BNX2_ISCSI_DRIVER_NAME		"bnx2i"
 
 #define BNX2I_MAX_ADAPTERS		8
@@ -126,6 +128,43 @@
 #define REG_WR(__hba, offset, val)			\
 		writel(val, __hba->regview + offset)
 
+#ifdef CONFIG_32BIT
+#define GET_STATS_64(__hba, dst, field)				\
+	do {							\
+		spin_lock_bh(&__hba->stat_lock);		\
+		dst->field##_lo = __hba->stats.field##_lo;	\
+		dst->field##_hi = __hba->stats.field##_hi;	\
+		spin_unlock_bh(&__hba->stat_lock);		\
+	} while (0)
+
+#define ADD_STATS_64(__hba, field, len)				\
+	do {							\
+		if (spin_trylock(&__hba->stat_lock)) {		\
+			if (__hba->stats.field##_lo + len <	\
+			    __hba->stats.field##_lo)		\
+				__hba->stats.field##_hi++;	\
+			__hba->stats.field##_lo += len;		\
+			spin_unlock(&__hba->stat_lock);		\
+		}						\
+	} while (0)
+
+#else
+#define GET_STATS_64(__hba, dst, field)				\
+	do {							\
+		u64 val, *out;					\
+								\
+		val = __hba->bnx2i_stats.field;			\
+		out = (u64 *)&__hba->stats.field##_lo;		\
+		*out = cpu_to_le64(val);			\
+		out = (u64 *)&dst->field##_lo;			\
+		*out = cpu_to_le64(val);			\
+	} while (0)
+
+#define ADD_STATS_64(__hba, field, len)				\
+	do {							\
+		__hba->bnx2i_stats.field += len;		\
+	} while (0)
+#endif
 
 /**
  * struct generic_pdu_resc - login pdu resource structure
@@ -288,6 +327,15 @@ struct iscsi_cid_queue {
 	struct bnx2i_conn **conn_cid_tbl;
 };
 
+
+struct bnx2i_stats_info {
+	u64 rx_pdus;
+	u64 rx_bytes;
+	u64 tx_pdus;
+	u64 tx_bytes;
+};
+
+
 /**
  * struct bnx2i_hba - bnx2i adapter structure
  *
@@ -341,6 +389,8 @@ struct iscsi_cid_queue {
  * @ctx_ccell_tasks:       captures number of ccells and tasks supported by
  *                         currently offloaded connection, used to decode
  *                         context memory
+ * @stat_lock:		   spin lock used by the statistic collector (32 bit)
+ * @stats:		   local iSCSI statistic collection place holder
  *
  * Adapter Data Structure
  */
@@ -427,6 +477,12 @@ struct bnx2i_hba {
 	u32 num_sess_opened;
 	u32 num_conn_opened;
 	unsigned int ctx_ccell_tasks;
+
+#ifdef CONFIG_32BIT
+	spinlock_t stat_lock;
+#endif
+	struct bnx2i_stats_info bnx2i_stats;
+	struct iscsi_stats_info stats;
 };
 
 
@@ -750,6 +806,8 @@ extern void bnx2i_ulp_init(struct cnic_dev *dev);
 extern void bnx2i_ulp_exit(struct cnic_dev *dev);
 extern void bnx2i_start(void *handle);
 extern void bnx2i_stop(void *handle);
+extern int bnx2i_get_stats(void *handle);
+
 extern struct bnx2i_hba *get_adapter_list_head(void);
 
 struct bnx2i_conn *bnx2i_get_conn_from_id(struct bnx2i_hba *hba,
diff --git a/drivers/scsi/bnx2i/bnx2i_hwi.c b/drivers/scsi/bnx2i/bnx2i_hwi.c
index 86a12b48e47..33d6630529d 100644
--- a/drivers/scsi/bnx2i/bnx2i_hwi.c
+++ b/drivers/scsi/bnx2i/bnx2i_hwi.c
@@ -1350,6 +1350,7 @@ int bnx2i_process_scsi_cmd_resp(struct iscsi_session *session,
 				struct cqe *cqe)
 {
 	struct iscsi_conn *conn = bnx2i_conn->cls_conn->dd_data;
+	struct bnx2i_hba *hba = bnx2i_conn->hba;
 	struct bnx2i_cmd_response *resp_cqe;
 	struct bnx2i_cmd *bnx2i_cmd;
 	struct iscsi_task *task;
@@ -1367,16 +1368,26 @@ int bnx2i_process_scsi_cmd_resp(struct iscsi_session *session,
 
 	if (bnx2i_cmd->req.op_attr & ISCSI_CMD_REQUEST_READ) {
 		conn->datain_pdus_cnt +=
-			resp_cqe->task_stat.read_stat.num_data_outs;
+			resp_cqe->task_stat.read_stat.num_data_ins;
 		conn->rxdata_octets +=
 			bnx2i_cmd->req.total_data_transfer_length;
+		ADD_STATS_64(hba, rx_pdus,
+			     resp_cqe->task_stat.read_stat.num_data_ins);
+		ADD_STATS_64(hba, rx_bytes,
+			     bnx2i_cmd->req.total_data_transfer_length);
 	} else {
 		conn->dataout_pdus_cnt +=
-			resp_cqe->task_stat.read_stat.num_data_outs;
+			resp_cqe->task_stat.write_stat.num_data_outs;
 		conn->r2t_pdus_cnt +=
-			resp_cqe->task_stat.read_stat.num_r2ts;
+			resp_cqe->task_stat.write_stat.num_r2ts;
 		conn->txdata_octets +=
 			bnx2i_cmd->req.total_data_transfer_length;
+		ADD_STATS_64(hba, tx_pdus,
+			     resp_cqe->task_stat.write_stat.num_data_outs);
+		ADD_STATS_64(hba, tx_bytes,
+			     bnx2i_cmd->req.total_data_transfer_length);
+		ADD_STATS_64(hba, rx_pdus,
+			     resp_cqe->task_stat.write_stat.num_r2ts);
 	}
 	bnx2i_iscsi_unmap_sg_list(bnx2i_cmd);
 
@@ -1961,6 +1972,7 @@ static int bnx2i_process_new_cqes(struct bnx2i_conn *bnx2i_conn)
 {
 	struct iscsi_conn *conn = bnx2i_conn->cls_conn->dd_data;
 	struct iscsi_session *session = conn->session;
+	struct bnx2i_hba *hba = bnx2i_conn->hba;
 	struct qp_info *qp;
 	struct bnx2i_nop_in_msg *nopin;
 	int tgt_async_msg;
@@ -1973,7 +1985,7 @@ static int bnx2i_process_new_cqes(struct bnx2i_conn *bnx2i_conn)
 
 	if (!qp->cq_virt) {
 		printk(KERN_ALERT "bnx2i (%s): cq resr freed in bh execution!",
-			bnx2i_conn->hba->netdev->name);
+		       hba->netdev->name);
 		goto out;
 	}
 	while (1) {
@@ -1985,9 +1997,9 @@ static int bnx2i_process_new_cqes(struct bnx2i_conn *bnx2i_conn)
 			if (nopin->op_code == ISCSI_OP_NOOP_IN &&
 			    nopin->itt == (u16) RESERVED_ITT) {
 				printk(KERN_ALERT "bnx2i: Unsolicited "
-					"NOP-In detected for suspended "
-					"connection dev=%s!\n",
-					bnx2i_conn->hba->netdev->name);
+				       "NOP-In detected for suspended "
+				       "connection dev=%s!\n",
+				       hba->netdev->name);
 				bnx2i_unsol_pdu_adjust_rq(bnx2i_conn);
 				goto cqe_out;
 			}
@@ -2001,7 +2013,7 @@ static int bnx2i_process_new_cqes(struct bnx2i_conn *bnx2i_conn)
 			/* Run the kthread engine only for data cmds
 			   All other cmds will be completed in this bh! */
 			bnx2i_queue_scsi_cmd_resp(session, bnx2i_conn, nopin);
-			break;
+			goto done;
 		case ISCSI_OP_LOGIN_RSP:
 			bnx2i_process_login_resp(session, bnx2i_conn,
 						 qp->cq_cons_qe);
@@ -2044,11 +2056,15 @@ static int bnx2i_process_new_cqes(struct bnx2i_conn *bnx2i_conn)
 			printk(KERN_ALERT "bnx2i: unknown opcode 0x%x\n",
 					  nopin->op_code);
 		}
+
+		ADD_STATS_64(hba, rx_pdus, 1);
+		ADD_STATS_64(hba, rx_bytes, nopin->data_length);
+done:
 		if (!tgt_async_msg) {
 			if (!atomic_read(&bnx2i_conn->ep->num_active_cmds))
 				printk(KERN_ALERT "bnx2i (%s): no active cmd! "
 				       "op 0x%x\n",
-				       bnx2i_conn->hba->netdev->name,
+				       hba->netdev->name,
 				       nopin->op_code);
 			else
 				atomic_dec(&bnx2i_conn->ep->num_active_cmds);
@@ -2692,6 +2708,7 @@ struct cnic_ulp_ops bnx2i_cnic_cb = {
 	.cm_remote_close = bnx2i_cm_remote_close,
 	.cm_remote_abort = bnx2i_cm_remote_abort,
 	.iscsi_nl_send_msg = bnx2i_send_nl_mesg,
+	.cnic_get_stats = bnx2i_get_stats,
 	.owner = THIS_MODULE
 };
 
diff --git a/drivers/scsi/bnx2i/bnx2i_init.c b/drivers/scsi/bnx2i/bnx2i_init.c
index 8b6816706ee..b17637aab9a 100644
--- a/drivers/scsi/bnx2i/bnx2i_init.c
+++ b/drivers/scsi/bnx2i/bnx2i_init.c
@@ -381,6 +381,46 @@ void bnx2i_ulp_exit(struct cnic_dev *dev)
 
 
 /**
+ * bnx2i_get_stats - Retrieve various statistic from iSCSI offload
+ * @handle:	bnx2i_hba
+ *
+ * function callback exported via bnx2i - cnic driver interface to
+ *      retrieve various iSCSI offload related statistics.
+ */
+int bnx2i_get_stats(void *handle)
+{
+	struct bnx2i_hba *hba = handle;
+	struct iscsi_stats_info *stats;
+
+	if (!hba)
+		return -EINVAL;
+
+	stats = (struct iscsi_stats_info *)hba->cnic->stats_addr;
+
+	if (!stats)
+		return -ENOMEM;
+
+	strlcpy(stats->version, DRV_MODULE_VERSION, sizeof(stats->version));
+	memcpy(stats->mac_add1 + 2, hba->cnic->mac_addr, ETH_ALEN);
+
+	stats->max_frame_size = hba->netdev->mtu;
+	stats->txq_size = hba->max_sqes;
+	stats->rxq_size = hba->max_cqes;
+
+	stats->txq_avg_depth = 0;
+	stats->rxq_avg_depth = 0;
+
+	GET_STATS_64(hba, stats, rx_pdus);
+	GET_STATS_64(hba, stats, rx_bytes);
+
+	GET_STATS_64(hba, stats, tx_pdus);
+	GET_STATS_64(hba, stats, tx_bytes);
+
+	return 0;
+}
+
+
+/**
  * bnx2i_percpu_thread_create - Create a receive thread for an
  *				online CPU
  *
diff --git a/drivers/scsi/bnx2i/bnx2i_iscsi.c b/drivers/scsi/bnx2i/bnx2i_iscsi.c
index 621538b8b54..3b34c13e2f0 100644
--- a/drivers/scsi/bnx2i/bnx2i_iscsi.c
+++ b/drivers/scsi/bnx2i/bnx2i_iscsi.c
@@ -874,6 +874,11 @@ struct bnx2i_hba *bnx2i_alloc_hba(struct cnic_dev *cnic)
 		hba->conn_ctx_destroy_tmo = 2 * HZ;
 	}
 
+#ifdef CONFIG_32BIT
+	spin_lock_init(&hba->stat_lock);
+#endif
+	memset(&hba->stats, 0, sizeof(struct iscsi_stats_info));
+
 	if (iscsi_host_add(shost, &hba->pcidev->dev))
 		goto free_dump_mem;
 	return hba;
@@ -1181,12 +1186,18 @@ static int
 bnx2i_mtask_xmit(struct iscsi_conn *conn, struct iscsi_task *task)
 {
 	struct bnx2i_conn *bnx2i_conn = conn->dd_data;
+	struct bnx2i_hba *hba = bnx2i_conn->hba;
 	struct bnx2i_cmd *cmd = task->dd_data;
 
 	memset(bnx2i_conn->gen_pdu.req_buf, 0, ISCSI_DEF_MAX_RECV_SEG_LEN);
 
 	bnx2i_setup_cmd_wqe_template(cmd);
 	bnx2i_conn->gen_pdu.req_buf_size = task->data_count;
+
+	/* Tx PDU/data length count */
+	ADD_STATS_64(hba, tx_pdus, 1);
+	ADD_STATS_64(hba, tx_bytes, task->data_count);
+
 	if (task->data_count) {
 		memcpy(bnx2i_conn->gen_pdu.req_buf, task->data,
 		       task->data_count);
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
index 36739da8bc1..49692a1ac44 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
@@ -966,7 +966,8 @@ static int init_act_open(struct cxgbi_sock *csk)
 		csk->saddr.sin_addr.s_addr = chba->ipv4addr;
 
 	csk->rss_qid = 0;
-	csk->l2t = t3_l2t_get(t3dev, dst, ndev);
+	csk->l2t = t3_l2t_get(t3dev, dst, ndev,
+			      &csk->daddr.sin_addr.s_addr);
 	if (!csk->l2t) {
 		pr_err("NO l2t available.\n");
 		return -EINVAL;
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
index 5a4a3bfc60c..f924b3c3720 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
@@ -438,8 +438,8 @@ static inline void make_tx_data_wr(struct cxgbi_sock *csk, struct sk_buff *skb,
 	if (submode)
 		wr_ulp_mode = FW_OFLD_TX_DATA_WR_ULPMODE(ULP2_MODE_ISCSI) |
 				FW_OFLD_TX_DATA_WR_ULPSUBMODE(submode);
-	req->tunnel_to_proxy = htonl(wr_ulp_mode) |
-		 FW_OFLD_TX_DATA_WR_SHOVE(skb_peek(&csk->write_queue) ? 0 : 1);
+	req->tunnel_to_proxy = htonl(wr_ulp_mode |
+		 FW_OFLD_TX_DATA_WR_SHOVE(skb_peek(&csk->write_queue) ? 0 : 1));
 	req->plen = htonl(len);
 	if (!cxgbi_sock_flag(csk, CTPF_TX_DATA_SENT))
 		cxgbi_sock_set_flag(csk, CTPF_TX_DATA_SENT);
@@ -1142,7 +1142,7 @@ static int init_act_open(struct cxgbi_sock *csk)
 	cxgbi_sock_set_flag(csk, CTPF_HAS_ATID);
 	cxgbi_sock_get(csk);
 
-	n = dst_get_neighbour_noref(csk->dst);
+	n = dst_neigh_lookup(csk->dst, &csk->daddr.sin_addr.s_addr);
 	if (!n) {
 		pr_err("%s, can't get neighbour of csk->dst.\n", ndev->name);
 		goto rel_resource;
@@ -1182,9 +1182,12 @@ static int init_act_open(struct cxgbi_sock *csk)
 
 	cxgbi_sock_set_state(csk, CTP_ACTIVE_OPEN);
 	send_act_open_req(csk, skb, csk->l2t);
+	neigh_release(n);
 	return 0;
 
 rel_resource:
+	if (n)
+		neigh_release(n);
 	if (skb)
 		__kfree_skb(skb);
 	return -EINVAL;
diff --git a/drivers/scsi/cxgbi/libcxgbi.c b/drivers/scsi/cxgbi/libcxgbi.c
index d9253db1d0e..b44c1cff311 100644
--- a/drivers/scsi/cxgbi/libcxgbi.c
+++ b/drivers/scsi/cxgbi/libcxgbi.c
@@ -494,7 +494,7 @@ static struct cxgbi_sock *cxgbi_check_route(struct sockaddr *dst_addr)
 		goto err_out;
 	}
 	dst = &rt->dst;
-	n = dst_get_neighbour_noref(dst);
+	n = dst_neigh_lookup(dst, &daddr->sin_addr.s_addr);
 	if (!n) {
 		err = -ENODEV;
 		goto rel_rt;
@@ -506,7 +506,7 @@ static struct cxgbi_sock *cxgbi_check_route(struct sockaddr *dst_addr)
 			&daddr->sin_addr.s_addr, ntohs(daddr->sin_port),
 			ndev->name);
 		err = -ENETUNREACH;
-		goto rel_rt;
+		goto rel_neigh;
 	}
 
 	if (ndev->flags & IFF_LOOPBACK) {
@@ -521,7 +521,7 @@ static struct cxgbi_sock *cxgbi_check_route(struct sockaddr *dst_addr)
 		pr_info("dst %pI4, %s, NOT cxgbi device.\n",
 			&daddr->sin_addr.s_addr, ndev->name);
 		err = -ENETUNREACH;
-		goto rel_rt;
+		goto rel_neigh;
 	}
 	log_debug(1 << CXGBI_DBG_SOCK,
 		"route to %pI4 :%u, ndev p#%d,%s, cdev 0x%p.\n",
@@ -531,7 +531,7 @@ static struct cxgbi_sock *cxgbi_check_route(struct sockaddr *dst_addr)
 	csk = cxgbi_sock_create(cdev);
 	if (!csk) {
 		err = -ENOMEM;
-		goto rel_rt;
+		goto rel_neigh;
 	}
 	csk->cdev = cdev;
 	csk->port_id = port;
@@ -541,9 +541,13 @@ static struct cxgbi_sock *cxgbi_check_route(struct sockaddr *dst_addr)
 	csk->daddr.sin_port = daddr->sin_port;
 	csk->daddr.sin_family = daddr->sin_family;
 	csk->saddr.sin_addr.s_addr = fl4.saddr;
+	neigh_release(n);
 
 	return csk;
 
+rel_neigh:
+	neigh_release(n);
+
 rel_rt:
 	ip_rt_put(rt);
 	if (csk)
diff --git a/drivers/scsi/device_handler/scsi_dh.c b/drivers/scsi/device_handler/scsi_dh.c
index 48e46f5b77c..33e422e7583 100644
--- a/drivers/scsi/device_handler/scsi_dh.c
+++ b/drivers/scsi/device_handler/scsi_dh.c
@@ -468,7 +468,8 @@ EXPORT_SYMBOL_GPL(scsi_dh_handler_exist);
 
 /*
  * scsi_dh_attach - Attach device handler
- * @sdev - sdev the handler should be attached to
+ * @q - Request queue that is associated with the scsi_device
+ *      the handler should be attached to
  * @name - name of the handler to attach
  */
 int scsi_dh_attach(struct request_queue *q, const char *name)
@@ -498,7 +499,8 @@ EXPORT_SYMBOL_GPL(scsi_dh_attach);
 
 /*
  * scsi_dh_detach - Detach device handler
- * @sdev - sdev the handler should be detached from
+ * @q - Request queue that is associated with the scsi_device
+ *      the handler should be detached from
  *
  * This function will detach the device handler only
  * if the sdev is not part of the internal list, ie
@@ -527,6 +529,38 @@ void scsi_dh_detach(struct request_queue *q)
 }
 EXPORT_SYMBOL_GPL(scsi_dh_detach);
 
+/*
+ * scsi_dh_attached_handler_name - Get attached device handler's name
+ * @q - Request queue that is associated with the scsi_device
+ *      that may have a device handler attached
+ * @gfp - the GFP mask used in the kmalloc() call when allocating memory
+ *
+ * Returns name of attached handler, NULL if no handler is attached.
+ * Caller must take care to free the returned string.
+ */
+const char *scsi_dh_attached_handler_name(struct request_queue *q, gfp_t gfp)
+{
+	unsigned long flags;
+	struct scsi_device *sdev;
+	const char *handler_name = NULL;
+
+	spin_lock_irqsave(q->queue_lock, flags);
+	sdev = q->queuedata;
+	if (!sdev || !get_device(&sdev->sdev_gendev))
+		sdev = NULL;
+	spin_unlock_irqrestore(q->queue_lock, flags);
+
+	if (!sdev)
+		return NULL;
+
+	if (sdev->scsi_dh_data)
+		handler_name = kstrdup(sdev->scsi_dh_data->scsi_dh->name, gfp);
+
+	put_device(&sdev->sdev_gendev);
+	return handler_name;
+}
+EXPORT_SYMBOL_GPL(scsi_dh_attached_handler_name);
+
 static struct notifier_block scsi_dh_nb = {
 	.notifier_call = scsi_dh_notifier
 };
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index fda9cdea0e6..08d80a6d272 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -46,13 +46,16 @@
 #define TPGS_SUPPORT_OFFLINE		0x40
 #define TPGS_SUPPORT_TRANSITION		0x80
 
+#define RTPG_FMT_MASK			0x70
+#define RTPG_FMT_EXT_HDR		0x10
+
 #define TPGS_MODE_UNINITIALIZED		 -1
 #define TPGS_MODE_NONE			0x0
 #define TPGS_MODE_IMPLICIT		0x1
 #define TPGS_MODE_EXPLICIT		0x2
 
 #define ALUA_INQUIRY_SIZE		36
-#define ALUA_FAILOVER_TIMEOUT		(60 * HZ)
+#define ALUA_FAILOVER_TIMEOUT		60
 #define ALUA_FAILOVER_RETRIES		5
 
 /* flags passed from user level */
@@ -68,6 +71,7 @@ struct alua_dh_data {
 	unsigned char		inq[ALUA_INQUIRY_SIZE];
 	unsigned char		*buff;
 	int			bufflen;
+	unsigned char		transition_tmo;
 	unsigned char		sense[SCSI_SENSE_BUFFERSIZE];
 	int			senselen;
 	struct scsi_device	*sdev;
@@ -128,7 +132,7 @@ static struct request *get_alua_req(struct scsi_device *sdev,
 	rq->cmd_flags |= REQ_FAILFAST_DEV | REQ_FAILFAST_TRANSPORT |
 			 REQ_FAILFAST_DRIVER;
 	rq->retries = ALUA_FAILOVER_RETRIES;
-	rq->timeout = ALUA_FAILOVER_TIMEOUT;
+	rq->timeout = ALUA_FAILOVER_TIMEOUT * HZ;
 
 	return rq;
 }
@@ -174,7 +178,8 @@ done:
  * submit_rtpg - Issue a REPORT TARGET GROUP STATES command
  * @sdev: sdev the command should be sent to
  */
-static unsigned submit_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
+static unsigned submit_rtpg(struct scsi_device *sdev, struct alua_dh_data *h,
+			    bool rtpg_ext_hdr_req)
 {
 	struct request *rq;
 	int err = SCSI_DH_RES_TEMP_UNAVAIL;
@@ -185,7 +190,10 @@ static unsigned submit_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 
 	/* Prepare the command. */
 	rq->cmd[0] = MAINTENANCE_IN;
-	rq->cmd[1] = MI_REPORT_TARGET_PGS;
+	if (rtpg_ext_hdr_req)
+		rq->cmd[1] = MI_REPORT_TARGET_PGS | MI_EXT_HDR_PARAM_FMT;
+	else
+		rq->cmd[1] = MI_REPORT_TARGET_PGS;
 	rq->cmd[6] = (h->bufflen >> 24) & 0xff;
 	rq->cmd[7] = (h->bufflen >> 16) & 0xff;
 	rq->cmd[8] = (h->bufflen >>  8) & 0xff;
@@ -518,11 +526,18 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 	int len, k, off, valid_states = 0;
 	unsigned char *ucp;
 	unsigned err;
-	unsigned long expiry, interval = 1000;
+	bool rtpg_ext_hdr_req = 1;
+	unsigned long expiry, interval = 0;
+	unsigned int tpg_desc_tbl_off;
+	unsigned char orig_transition_tmo;
+
+	if (!h->transition_tmo)
+		expiry = round_jiffies_up(jiffies + ALUA_FAILOVER_TIMEOUT * HZ);
+	else
+		expiry = round_jiffies_up(jiffies + h->transition_tmo * HZ);
 
-	expiry = round_jiffies_up(jiffies + ALUA_FAILOVER_TIMEOUT);
  retry:
-	err = submit_rtpg(sdev, h);
+	err = submit_rtpg(sdev, h, rtpg_ext_hdr_req);
 
 	if (err == SCSI_DH_IO && h->senselen > 0) {
 		err = scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
@@ -530,6 +545,21 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 		if (!err)
 			return SCSI_DH_IO;
 
+		/*
+		 * submit_rtpg() has failed on existing arrays
+		 * when requesting extended header info, and
+		 * the array doesn't support extended headers,
+		 * even though it shouldn't according to T10.
+		 * The retry without rtpg_ext_hdr_req set
+		 * handles this.
+		 */
+		if (rtpg_ext_hdr_req == 1 &&
+		    sense_hdr.sense_key == ILLEGAL_REQUEST &&
+		    sense_hdr.asc == 0x24 && sense_hdr.ascq == 0) {
+			rtpg_ext_hdr_req = 0;
+			goto retry;
+		}
+
 		err = alua_check_sense(sdev, &sense_hdr);
 		if (err == ADD_TO_MLQUEUE && time_before(jiffies, expiry))
 			goto retry;
@@ -556,7 +586,28 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 		goto retry;
 	}
 
-	for (k = 4, ucp = h->buff + 4; k < len; k += off, ucp += off) {
+	orig_transition_tmo = h->transition_tmo;
+	if ((h->buff[4] & RTPG_FMT_MASK) == RTPG_FMT_EXT_HDR && h->buff[5] != 0)
+		h->transition_tmo = h->buff[5];
+	else
+		h->transition_tmo = ALUA_FAILOVER_TIMEOUT;
+
+	if (orig_transition_tmo != h->transition_tmo) {
+		sdev_printk(KERN_INFO, sdev,
+			    "%s: transition timeout set to %d seconds\n",
+			    ALUA_DH_NAME, h->transition_tmo);
+		expiry = jiffies + h->transition_tmo * HZ;
+	}
+
+	if ((h->buff[4] & RTPG_FMT_MASK) == RTPG_FMT_EXT_HDR)
+		tpg_desc_tbl_off = 8;
+	else
+		tpg_desc_tbl_off = 4;
+
+	for (k = tpg_desc_tbl_off, ucp = h->buff + tpg_desc_tbl_off;
+	     k < len;
+	     k += off, ucp += off) {
+
 		if (h->group_id == (ucp[2] << 8) + ucp[3]) {
 			h->state = ucp[0] & 0x0f;
 			h->pref = ucp[0] >> 7;
@@ -581,7 +632,7 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 	case TPGS_STATE_TRANSITIONING:
 		if (time_before(jiffies, expiry)) {
 			/* State transition, retry */
-			interval *= 2;
+			interval += 2000;
 			msleep(interval);
 			goto retry;
 		}
@@ -691,9 +742,9 @@ static int alua_activate(struct scsi_device *sdev,
 				stpg = 0;
 			break;
 		case TPGS_STATE_STANDBY:
+		case TPGS_STATE_UNAVAILABLE:
 			stpg = 1;
 			break;
-		case TPGS_STATE_UNAVAILABLE:
 		case TPGS_STATE_OFFLINE:
 			err = SCSI_DH_IO;
 			break;
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index fe30b1b65e1..078d262ac7c 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1529,7 +1529,7 @@ static int fcoe_rcv(struct sk_buff *skb, struct net_device *netdev,
 
 	return 0;
 err:
-	per_cpu_ptr(lport->dev_stats, get_cpu())->ErrorFrames++;
+	per_cpu_ptr(lport->stats, get_cpu())->ErrorFrames++;
 	put_cpu();
 err2:
 	kfree_skb(skb);
@@ -1569,7 +1569,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	struct ethhdr *eh;
 	struct fcoe_crc_eof *cp;
 	struct sk_buff *skb;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 	struct fc_frame_header *fh;
 	unsigned int hlen;		/* header length implies the version */
 	unsigned int tlen;		/* trailer length */
@@ -1680,7 +1680,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 		skb_shinfo(skb)->gso_size = 0;
 	}
 	/* update tx stats: regardless if LLD fails */
-	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	stats->TxFrames++;
 	stats->TxWords += wlen;
 	put_cpu();
@@ -1714,7 +1714,7 @@ static inline int fcoe_filter_frames(struct fc_lport *lport,
 	struct fcoe_interface *fcoe;
 	struct fc_frame_header *fh;
 	struct sk_buff *skb = (struct sk_buff *)fp;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 
 	/*
 	 * We only check CRC if no offload is available and if it is
@@ -1745,7 +1745,7 @@ static inline int fcoe_filter_frames(struct fc_lport *lport,
 		return 0;
 	}
 
-	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	stats->InvalidCRCCount++;
 	if (stats->InvalidCRCCount < 5)
 		printk(KERN_WARNING "fcoe: dropping frame with CRC error\n");
@@ -1762,7 +1762,7 @@ static void fcoe_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_stats *stats;
 	struct fcoe_crc_eof crc_eof;
 	struct fc_frame *fp;
 	struct fcoe_port *port;
@@ -1793,7 +1793,7 @@ static void fcoe_recv_frame(struct sk_buff *skb)
 	 */
 	hp = (struct fcoe_hdr *) skb_network_header(skb);
 
-	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	if (unlikely(FC_FCOE_DECAPS_VER(hp) != FC_FCOE_VER)) {
 		if (stats->ErrorFrames < 5)
 			printk(KERN_WARNING "fcoe: FCoE version "
@@ -1851,23 +1851,25 @@ static int fcoe_percpu_receive_thread(void *arg)
 
 	set_user_nice(current, -20);
 
+retry:
 	while (!kthread_should_stop()) {
 
 		spin_lock_bh(&p->fcoe_rx_list.lock);
 		skb_queue_splice_init(&p->fcoe_rx_list, &tmp);
-		spin_unlock_bh(&p->fcoe_rx_list.lock);
-
-		while ((skb = __skb_dequeue(&tmp)) != NULL)
-			fcoe_recv_frame(skb);
 
-		spin_lock_bh(&p->fcoe_rx_list.lock);
-		if (!skb_queue_len(&p->fcoe_rx_list)) {
+		if (!skb_queue_len(&tmp)) {
 			set_current_state(TASK_INTERRUPTIBLE);
 			spin_unlock_bh(&p->fcoe_rx_list.lock);
 			schedule();
 			set_current_state(TASK_RUNNING);
-		} else
-			spin_unlock_bh(&p->fcoe_rx_list.lock);
+			goto retry;
+		}
+
+		spin_unlock_bh(&p->fcoe_rx_list.lock);
+
+		while ((skb = __skb_dequeue(&tmp)) != NULL)
+			fcoe_recv_frame(skb);
+
 	}
 	return 0;
 }
@@ -1970,7 +1972,7 @@ static int fcoe_device_notification(struct notifier_block *notifier,
 	struct fcoe_ctlr *ctlr;
 	struct fcoe_interface *fcoe;
 	struct fcoe_port *port;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 	u32 link_possible = 1;
 	u32 mfs;
 	int rc = NOTIFY_OK;
@@ -2024,7 +2026,7 @@ static int fcoe_device_notification(struct notifier_block *notifier,
 	if (link_possible && !fcoe_link_ok(lport))
 		fcoe_ctlr_link_up(ctlr);
 	else if (fcoe_ctlr_link_down(ctlr)) {
-		stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+		stats = per_cpu_ptr(lport->stats, get_cpu());
 		stats->LinkFailureCount++;
 		put_cpu();
 		fcoe_clean_pending_queue(lport);
diff --git a/drivers/scsi/fcoe/fcoe_ctlr.c b/drivers/scsi/fcoe/fcoe_ctlr.c
index d68d57241ee..2ebe03a4b51 100644
--- a/drivers/scsi/fcoe/fcoe_ctlr.c
+++ b/drivers/scsi/fcoe/fcoe_ctlr.c
@@ -788,11 +788,11 @@ static unsigned long fcoe_ctlr_age_fcfs(struct fcoe_ctlr *fip)
 	unsigned long deadline;
 	unsigned long sel_time = 0;
 	struct list_head del_list;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 
 	INIT_LIST_HEAD(&del_list);
 
-	stats = per_cpu_ptr(fip->lp->dev_stats, get_cpu());
+	stats = per_cpu_ptr(fip->lp->stats, get_cpu());
 
 	list_for_each_entry_safe(fcf, next, &fip->fcfs, list) {
 		deadline = fcf->time + fcf->fka_period + fcf->fka_period / 2;
@@ -1104,8 +1104,8 @@ static void fcoe_ctlr_recv_els(struct fcoe_ctlr *fip, struct sk_buff *skb)
 	struct fc_frame_header *fh = NULL;
 	struct fip_desc *desc;
 	struct fip_encaps *els;
-	struct fcoe_dev_stats *stats;
 	struct fcoe_fcf *sel;
+	struct fc_stats *stats;
 	enum fip_desc_type els_dtype = 0;
 	u8 els_op;
 	u8 sub;
@@ -1249,7 +1249,7 @@ static void fcoe_ctlr_recv_els(struct fcoe_ctlr *fip, struct sk_buff *skb)
 	fr_dev(fp) = lport;
 	fr_encaps(fp) = els_dtype;
 
-	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	stats->RxFrames++;
 	stats->RxWords += skb->len / FIP_BPW;
 	put_cpu();
@@ -1353,7 +1353,7 @@ static void fcoe_ctlr_recv_clr_vlink(struct fcoe_ctlr *fip,
 						      ntoh24(vp->fd_fc_id));
 			if (vn_port && (vn_port == lport)) {
 				mutex_lock(&fip->ctlr_mutex);
-				per_cpu_ptr(lport->dev_stats,
+				per_cpu_ptr(lport->stats,
 					    get_cpu())->VLinkFailureCount++;
 				put_cpu();
 				fcoe_ctlr_reset(fip);
@@ -1383,8 +1383,7 @@ static void fcoe_ctlr_recv_clr_vlink(struct fcoe_ctlr *fip,
 		 * followed by physical port
 		 */
 		mutex_lock(&fip->ctlr_mutex);
-		per_cpu_ptr(lport->dev_stats,
-			    get_cpu())->VLinkFailureCount++;
+		per_cpu_ptr(lport->stats, get_cpu())->VLinkFailureCount++;
 		put_cpu();
 		fcoe_ctlr_reset(fip);
 		mutex_unlock(&fip->ctlr_mutex);
diff --git a/drivers/scsi/fcoe/fcoe_sysfs.c b/drivers/scsi/fcoe/fcoe_sysfs.c
index 2bc163198d3..5e751689a08 100644
--- a/drivers/scsi/fcoe/fcoe_sysfs.c
+++ b/drivers/scsi/fcoe/fcoe_sysfs.c
@@ -102,7 +102,7 @@ static int fcoe_str_to_dev_loss(const char *buf, unsigned long *val)
 	int ret;
 
 	ret = kstrtoul(buf, 0, val);
-	if (ret || *val < 0)
+	if (ret)
 		return -EINVAL;
 	/*
 	 * Check for overflow; dev_loss_tmo is u32
diff --git a/drivers/scsi/fcoe/fcoe_transport.c b/drivers/scsi/fcoe/fcoe_transport.c
index b46f43dced7..ac76d8a042d 100644
--- a/drivers/scsi/fcoe/fcoe_transport.c
+++ b/drivers/scsi/fcoe/fcoe_transport.c
@@ -89,7 +89,7 @@ void __fcoe_get_lesb(struct fc_lport *lport,
 {
 	unsigned int cpu;
 	u32 lfc, vlfc, mdac;
-	struct fcoe_dev_stats *devst;
+	struct fc_stats *stats;
 	struct fcoe_fc_els_lesb *lesb;
 	struct rtnl_link_stats64 temp;
 
@@ -99,10 +99,10 @@ void __fcoe_get_lesb(struct fc_lport *lport,
 	lesb = (struct fcoe_fc_els_lesb *)fc_lesb;
 	memset(lesb, 0, sizeof(*lesb));
 	for_each_possible_cpu(cpu) {
-		devst = per_cpu_ptr(lport->dev_stats, cpu);
-		lfc += devst->LinkFailureCount;
-		vlfc += devst->VLinkFailureCount;
-		mdac += devst->MissDiscAdvCount;
+		stats = per_cpu_ptr(lport->stats, cpu);
+		lfc += stats->LinkFailureCount;
+		vlfc += stats->VLinkFailureCount;
+		mdac += stats->MissDiscAdvCount;
 	}
 	lesb->lesb_link_fail = htonl(lfc);
 	lesb->lesb_vlink_fail = htonl(vlfc);
@@ -502,7 +502,7 @@ static int __init fcoe_transport_init(void)
 	return 0;
 }
 
-static int __exit fcoe_transport_exit(void)
+static int fcoe_transport_exit(void)
 {
 	struct fcoe_transport *ft;
 
diff --git a/drivers/scsi/hosts.c b/drivers/scsi/hosts.c
index a3a056a9db6..593085a5227 100644
--- a/drivers/scsi/hosts.c
+++ b/drivers/scsi/hosts.c
@@ -42,7 +42,7 @@
 #include "scsi_logging.h"
 
 
-static atomic_t scsi_host_next_hn;	/* host_no for next new host */
+static atomic_t scsi_host_next_hn = ATOMIC_INIT(0);	/* host_no for next new host */
 
 
 static void scsi_host_cls_release(struct device *dev)
@@ -290,6 +290,7 @@ static void scsi_host_dev_release(struct device *dev)
 	struct Scsi_Host *shost = dev_to_shost(dev);
 	struct device *parent = dev->parent;
 	struct request_queue *q;
+	void *queuedata;
 
 	scsi_proc_hostdir_rm(shost->hostt);
 
@@ -299,9 +300,9 @@ static void scsi_host_dev_release(struct device *dev)
 		destroy_workqueue(shost->work_q);
 	q = shost->uspace_req_q;
 	if (q) {
-		kfree(q->queuedata);
-		q->queuedata = NULL;
-		scsi_free_queue(q);
+		queuedata = q->queuedata;
+		blk_cleanup_queue(q);
+		kfree(queuedata);
 	}
 
 	scsi_destroy_command_freelist(shost);
diff --git a/drivers/scsi/hptiop.c b/drivers/scsi/hptiop.c
index 10b65556937..192724ed7a3 100644
--- a/drivers/scsi/hptiop.c
+++ b/drivers/scsi/hptiop.c
@@ -42,7 +42,7 @@ MODULE_DESCRIPTION("HighPoint RocketRAID 3xxx/4xxx Controller Driver");
 
 static char driver_name[] = "hptiop";
 static const char driver_name_long[] = "RocketRAID 3xxx/4xxx Controller driver";
-static const char driver_ver[] = "v1.6 (090910)";
+static const char driver_ver[] = "v1.6 (091225)";
 
 static int iop_send_sync_msg(struct hptiop_hba *hba, u32 msg, u32 millisec);
 static void hptiop_finish_scsi_req(struct hptiop_hba *hba, u32 tag,
@@ -958,6 +958,7 @@ static int __devinit hptiop_probe(struct pci_dev *pcidev,
 {
 	struct Scsi_Host *host = NULL;
 	struct hptiop_hba *hba;
+	struct hptiop_adapter_ops *iop_ops;
 	struct hpt_iop_request_get_config iop_config;
 	struct hpt_iop_request_set_config set_config;
 	dma_addr_t start_phy;
@@ -978,7 +979,8 @@ static int __devinit hptiop_probe(struct pci_dev *pcidev,
 	pci_set_master(pcidev);
 
 	/* Enable 64bit DMA if possible */
-	if (pci_set_dma_mask(pcidev, DMA_BIT_MASK(64))) {
+	iop_ops = (struct hptiop_adapter_ops *)id->driver_data;
+	if (pci_set_dma_mask(pcidev, DMA_BIT_MASK(iop_ops->hw_dma_bit_mask))) {
 		if (pci_set_dma_mask(pcidev, DMA_BIT_MASK(32))) {
 			printk(KERN_ERR "hptiop: fail to set dma_mask\n");
 			goto disable_pci_device;
@@ -998,7 +1000,7 @@ static int __devinit hptiop_probe(struct pci_dev *pcidev,
 
 	hba = (struct hptiop_hba *)host->hostdata;
 
-	hba->ops = (struct hptiop_adapter_ops *)id->driver_data;
+	hba->ops = iop_ops;
 	hba->pcidev = pcidev;
 	hba->host = host;
 	hba->initialized = 0;
@@ -1239,6 +1241,7 @@ static struct hptiop_adapter_ops hptiop_itl_ops = {
 	.iop_intr          = iop_intr_itl,
 	.post_msg          = hptiop_post_msg_itl,
 	.post_req          = hptiop_post_req_itl,
+	.hw_dma_bit_mask   = 64,
 };
 
 static struct hptiop_adapter_ops hptiop_mv_ops = {
@@ -1254,6 +1257,7 @@ static struct hptiop_adapter_ops hptiop_mv_ops = {
 	.iop_intr          = iop_intr_mv,
 	.post_msg          = hptiop_post_msg_mv,
 	.post_req          = hptiop_post_req_mv,
+	.hw_dma_bit_mask   = 33,
 };
 
 static struct pci_device_id hptiop_id_table[] = {
diff --git a/drivers/scsi/hptiop.h b/drivers/scsi/hptiop.h
index 0b871c0ae56..baa648d87fd 100644
--- a/drivers/scsi/hptiop.h
+++ b/drivers/scsi/hptiop.h
@@ -297,6 +297,7 @@ struct hptiop_adapter_ops {
 	int  (*iop_intr)(struct hptiop_hba *hba);
 	void (*post_msg)(struct hptiop_hba *hba, u32 msg);
 	void (*post_req)(struct hptiop_hba *hba, struct hptiop_request *_req);
+	int  hw_dma_bit_mask;
 };
 
 #define HPT_IOCTL_RESULT_OK         0
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index 47e28b55502..92c1d86d1fc 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -166,6 +166,9 @@ static struct scsi_host_template isci_sht = {
 	.sg_tablesize			= SG_ALL,
 	.max_sectors			= SCSI_DEFAULT_MAX_SECTORS,
 	.use_clustering			= ENABLE_CLUSTERING,
+	.eh_abort_handler		= sas_eh_abort_handler,
+	.eh_device_reset_handler        = sas_eh_device_reset_handler,
+	.eh_bus_reset_handler           = sas_eh_bus_reset_handler,
 	.target_destroy			= sas_target_destroy,
 	.ioctl				= sas_ioctl,
 	.shost_attrs			= isci_host_attrs,
diff --git a/drivers/scsi/libfc/fc_exch.c b/drivers/scsi/libfc/fc_exch.c
index aceffadb21c..c772d8d2715 100644
--- a/drivers/scsi/libfc/fc_exch.c
+++ b/drivers/scsi/libfc/fc_exch.c
@@ -99,11 +99,6 @@ struct fc_exch_mgr {
 	u16		max_xid;
 	u16		pool_max_index;
 
-	/*
-	 * currently exchange mgr stats are updated but not used.
-	 * either stats can be expose via sysfs or remove them
-	 * all together if not used XXX
-	 */
 	struct {
 		atomic_t no_free_exch;
 		atomic_t no_free_exch_xid;
@@ -124,7 +119,7 @@ struct fc_exch_mgr {
  * for each anchor to determine if that EM should be used. The last
  * anchor in the list will always match to handle any exchanges not
  * handled by other EMs. The non-default EMs would be added to the
- * anchor list by HW that provides FCoE offloads.
+ * anchor list by HW that provides offloads.
  */
 struct fc_exch_mgr_anchor {
 	struct list_head ema_list;
@@ -339,6 +334,52 @@ static void fc_exch_release(struct fc_exch *ep)
 }
 
 /**
+ * fc_exch_timer_cancel() - cancel exch timer
+ * @ep:		The exchange whose timer to be canceled
+ */
+static inline  void fc_exch_timer_cancel(struct fc_exch *ep)
+{
+	if (cancel_delayed_work(&ep->timeout_work)) {
+		FC_EXCH_DBG(ep, "Exchange timer canceled\n");
+		atomic_dec(&ep->ex_refcnt); /* drop hold for timer */
+	}
+}
+
+/**
+ * fc_exch_timer_set_locked() - Start a timer for an exchange w/ the
+ *				the exchange lock held
+ * @ep:		The exchange whose timer will start
+ * @timer_msec: The timeout period
+ *
+ * Used for upper level protocols to time out the exchange.
+ * The timer is cancelled when it fires or when the exchange completes.
+ */
+static inline void fc_exch_timer_set_locked(struct fc_exch *ep,
+					    unsigned int timer_msec)
+{
+	if (ep->state & (FC_EX_RST_CLEANUP | FC_EX_DONE))
+		return;
+
+	FC_EXCH_DBG(ep, "Exchange timer armed : %d msecs\n", timer_msec);
+
+	if (queue_delayed_work(fc_exch_workqueue, &ep->timeout_work,
+			       msecs_to_jiffies(timer_msec)))
+		fc_exch_hold(ep);		/* hold for timer */
+}
+
+/**
+ * fc_exch_timer_set() - Lock the exchange and set the timer
+ * @ep:		The exchange whose timer will start
+ * @timer_msec: The timeout period
+ */
+static void fc_exch_timer_set(struct fc_exch *ep, unsigned int timer_msec)
+{
+	spin_lock_bh(&ep->ex_lock);
+	fc_exch_timer_set_locked(ep, timer_msec);
+	spin_unlock_bh(&ep->ex_lock);
+}
+
+/**
  * fc_exch_done_locked() - Complete an exchange with the exchange lock held
  * @ep: The exchange that is complete
  */
@@ -359,8 +400,7 @@ static int fc_exch_done_locked(struct fc_exch *ep)
 
 	if (!(ep->esb_stat & ESB_ST_REC_QUAL)) {
 		ep->state |= FC_EX_DONE;
-		if (cancel_delayed_work(&ep->timeout_work))
-			atomic_dec(&ep->ex_refcnt); /* drop hold for timer */
+		fc_exch_timer_cancel(ep);
 		rc = 0;
 	}
 	return rc;
@@ -424,40 +464,6 @@ static void fc_exch_delete(struct fc_exch *ep)
 }
 
 /**
- * fc_exch_timer_set_locked() - Start a timer for an exchange w/ the
- *				the exchange lock held
- * @ep:		The exchange whose timer will start
- * @timer_msec: The timeout period
- *
- * Used for upper level protocols to time out the exchange.
- * The timer is cancelled when it fires or when the exchange completes.
- */
-static inline void fc_exch_timer_set_locked(struct fc_exch *ep,
-					    unsigned int timer_msec)
-{
-	if (ep->state & (FC_EX_RST_CLEANUP | FC_EX_DONE))
-		return;
-
-	FC_EXCH_DBG(ep, "Exchange timer armed\n");
-
-	if (queue_delayed_work(fc_exch_workqueue, &ep->timeout_work,
-			       msecs_to_jiffies(timer_msec)))
-		fc_exch_hold(ep);		/* hold for timer */
-}
-
-/**
- * fc_exch_timer_set() - Lock the exchange and set the timer
- * @ep:		The exchange whose timer will start
- * @timer_msec: The timeout period
- */
-static void fc_exch_timer_set(struct fc_exch *ep, unsigned int timer_msec)
-{
-	spin_lock_bh(&ep->ex_lock);
-	fc_exch_timer_set_locked(ep, timer_msec);
-	spin_unlock_bh(&ep->ex_lock);
-}
-
-/**
  * fc_seq_send() - Send a frame using existing sequence/exchange pair
  * @lport: The local port that the exchange will be sent on
  * @sp:	   The sequence to be sent
@@ -986,7 +992,7 @@ static enum fc_pf_rjt_reason fc_seq_lookup_recip(struct fc_lport *lport,
 				/*
 				 * Update sequence_id based on incoming last
 				 * frame of sequence exchange. This is needed
-				 * for FCoE target where DDP has been used
+				 * for FC target where DDP has been used
 				 * on target where, stack is indicated only
 				 * about last frame's (payload _header) header.
 				 * Whereas "seq_id" which is part of
@@ -1549,8 +1555,10 @@ static void fc_exch_abts_resp(struct fc_exch *ep, struct fc_frame *fp)
 	FC_EXCH_DBG(ep, "exch: BLS rctl %x - %s\n", fh->fh_r_ctl,
 		    fc_exch_rctl_name(fh->fh_r_ctl));
 
-	if (cancel_delayed_work_sync(&ep->timeout_work))
+	if (cancel_delayed_work_sync(&ep->timeout_work)) {
+		FC_EXCH_DBG(ep, "Exchange timer canceled\n");
 		fc_exch_release(ep);	/* release from pending timer hold */
+	}
 
 	spin_lock_bh(&ep->ex_lock);
 	switch (fh->fh_r_ctl) {
@@ -1737,8 +1745,7 @@ static void fc_exch_reset(struct fc_exch *ep)
 	spin_lock_bh(&ep->ex_lock);
 	fc_exch_abort_locked(ep, 0);
 	ep->state |= FC_EX_RST_CLEANUP;
-	if (cancel_delayed_work(&ep->timeout_work))
-		atomic_dec(&ep->ex_refcnt);	/* drop hold for timer */
+	fc_exch_timer_cancel(ep);
 	resp = ep->resp;
 	ep->resp = NULL;
 	if (ep->esb_stat & ESB_ST_REC_QUAL)
@@ -2133,10 +2140,8 @@ static void fc_exch_els_rrq(struct fc_frame *fp)
 		ep->esb_stat &= ~ESB_ST_REC_QUAL;
 		atomic_dec(&ep->ex_refcnt);	/* drop hold for rec qual */
 	}
-	if (ep->esb_stat & ESB_ST_COMPLETE) {
-		if (cancel_delayed_work(&ep->timeout_work))
-			atomic_dec(&ep->ex_refcnt);	/* drop timer hold */
-	}
+	if (ep->esb_stat & ESB_ST_COMPLETE)
+		fc_exch_timer_cancel(ep);
 
 	spin_unlock_bh(&ep->ex_lock);
 
@@ -2156,6 +2161,31 @@ out:
 }
 
 /**
+ * fc_exch_update_stats() - update exches stats to lport
+ * @lport: The local port to update exchange manager stats
+ */
+void fc_exch_update_stats(struct fc_lport *lport)
+{
+	struct fc_host_statistics *st;
+	struct fc_exch_mgr_anchor *ema;
+	struct fc_exch_mgr *mp;
+
+	st = &lport->host_stats;
+
+	list_for_each_entry(ema, &lport->ema_list, ema_list) {
+		mp = ema->mp;
+		st->fc_no_free_exch += atomic_read(&mp->stats.no_free_exch);
+		st->fc_no_free_exch_xid +=
+				atomic_read(&mp->stats.no_free_exch_xid);
+		st->fc_xid_not_found += atomic_read(&mp->stats.xid_not_found);
+		st->fc_xid_busy += atomic_read(&mp->stats.xid_busy);
+		st->fc_seq_not_found += atomic_read(&mp->stats.seq_not_found);
+		st->fc_non_bls_resp += atomic_read(&mp->stats.non_bls_resp);
+	}
+}
+EXPORT_SYMBOL(fc_exch_update_stats);
+
+/**
  * fc_exch_mgr_add() - Add an exchange manager to a local port's list of EMs
  * @lport: The local port to add the exchange manager to
  * @mp:	   The exchange manager to be added to the local port
diff --git a/drivers/scsi/libfc/fc_fcp.c b/drivers/scsi/libfc/fc_fcp.c
index f7357308655..14243fa5f8e 100644
--- a/drivers/scsi/libfc/fc_fcp.c
+++ b/drivers/scsi/libfc/fc_fcp.c
@@ -158,6 +158,9 @@ static struct fc_fcp_pkt *fc_fcp_pkt_alloc(struct fc_lport *lport, gfp_t gfp)
 		fsp->timer.data = (unsigned long)fsp;
 		INIT_LIST_HEAD(&fsp->list);
 		spin_lock_init(&fsp->scsi_pkt_lock);
+	} else {
+		per_cpu_ptr(lport->stats, get_cpu())->FcpPktAllocFails++;
+		put_cpu();
 	}
 	return fsp;
 }
@@ -264,6 +267,9 @@ static int fc_fcp_send_abort(struct fc_fcp_pkt *fsp)
 	if (!fsp->seq_ptr)
 		return -EINVAL;
 
+	per_cpu_ptr(fsp->lp->stats, get_cpu())->FcpPktAborts++;
+	put_cpu();
+
 	fsp->state |= FC_SRB_ABORT_PENDING;
 	return fsp->lp->tt.seq_exch_abort(fsp->seq_ptr, 0);
 }
@@ -420,6 +426,8 @@ static inline struct fc_frame *fc_fcp_frame_alloc(struct fc_lport *lport,
 	if (likely(fp))
 		return fp;
 
+	per_cpu_ptr(lport->stats, get_cpu())->FcpFrameAllocFails++;
+	put_cpu();
 	/* error case */
 	fc_fcp_can_queue_ramp_down(lport);
 	return NULL;
@@ -434,7 +442,7 @@ static void fc_fcp_recv_data(struct fc_fcp_pkt *fsp, struct fc_frame *fp)
 {
 	struct scsi_cmnd *sc = fsp->cmd;
 	struct fc_lport *lport = fsp->lp;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 	struct fc_frame_header *fh;
 	size_t start_offset;
 	size_t offset;
@@ -496,7 +504,7 @@ static void fc_fcp_recv_data(struct fc_fcp_pkt *fsp, struct fc_frame *fp)
 
 		if (~crc != le32_to_cpu(fr_crc(fp))) {
 crc_err:
-			stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+			stats = per_cpu_ptr(lport->stats, get_cpu());
 			stats->ErrorFrames++;
 			/* per cpu count, not total count, but OK for limit */
 			if (stats->InvalidCRCCount++ < FC_MAX_ERROR_CNT)
@@ -1372,10 +1380,10 @@ static void fc_fcp_timeout(unsigned long data)
 
 	fsp->state |= FC_SRB_FCP_PROCESSING_TMO;
 
-	if (rpriv->flags & FC_RP_FLAGS_REC_SUPPORTED)
-		fc_fcp_rec(fsp);
-	else if (fsp->state & FC_SRB_RCV_STATUS)
+	if (fsp->state & FC_SRB_RCV_STATUS)
 		fc_fcp_complete_locked(fsp);
+	else if (rpriv->flags & FC_RP_FLAGS_REC_SUPPORTED)
+		fc_fcp_rec(fsp);
 	else
 		fc_fcp_recovery(fsp, FC_TIMED_OUT);
 	fsp->state &= ~FC_SRB_FCP_PROCESSING_TMO;
@@ -1786,7 +1794,7 @@ int fc_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *sc_cmd)
 	struct fc_rport_libfc_priv *rpriv;
 	int rval;
 	int rc = 0;
-	struct fcoe_dev_stats *stats;
+	struct fc_stats *stats;
 
 	rval = fc_remote_port_chkready(rport);
 	if (rval) {
@@ -1835,7 +1843,7 @@ int fc_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *sc_cmd)
 	/*
 	 * setup the data direction
 	 */
-	stats = per_cpu_ptr(lport->dev_stats, get_cpu());
+	stats = per_cpu_ptr(lport->stats, get_cpu());
 	if (sc_cmd->sc_data_direction == DMA_FROM_DEVICE) {
 		fsp->req_flags = FC_SRB_READ;
 		stats->InputRequests++;
diff --git a/drivers/scsi/libfc/fc_frame.c b/drivers/scsi/libfc/fc_frame.c
index 981329a17c4..0382ac06906 100644
--- a/drivers/scsi/libfc/fc_frame.c
+++ b/drivers/scsi/libfc/fc_frame.c
@@ -49,7 +49,7 @@ u32 fc_frame_crc_check(struct fc_frame *fp)
 EXPORT_SYMBOL(fc_frame_crc_check);
 
 /*
- * Allocate a frame intended to be sent via fcoe_xmit.
+ * Allocate a frame intended to be sent.
  * Get an sk_buff for the frame and set the length.
  */
 struct fc_frame *_fc_frame_alloc(size_t len)
diff --git a/drivers/scsi/libfc/fc_lport.c b/drivers/scsi/libfc/fc_lport.c
index c1402fb499a..f04d15c67df 100644
--- a/drivers/scsi/libfc/fc_lport.c
+++ b/drivers/scsi/libfc/fc_lport.c
@@ -299,47 +299,54 @@ EXPORT_SYMBOL(fc_get_host_speed);
  */
 struct fc_host_statistics *fc_get_host_stats(struct Scsi_Host *shost)
 {
-	struct fc_host_statistics *fcoe_stats;
+	struct fc_host_statistics *fc_stats;
 	struct fc_lport *lport = shost_priv(shost);
 	struct timespec v0, v1;
 	unsigned int cpu;
 	u64 fcp_in_bytes = 0;
 	u64 fcp_out_bytes = 0;
 
-	fcoe_stats = &lport->host_stats;
-	memset(fcoe_stats, 0, sizeof(struct fc_host_statistics));
+	fc_stats = &lport->host_stats;
+	memset(fc_stats, 0, sizeof(struct fc_host_statistics));
 
 	jiffies_to_timespec(jiffies, &v0);
 	jiffies_to_timespec(lport->boot_time, &v1);
-	fcoe_stats->seconds_since_last_reset = (v0.tv_sec - v1.tv_sec);
+	fc_stats->seconds_since_last_reset = (v0.tv_sec - v1.tv_sec);
 
 	for_each_possible_cpu(cpu) {
-		struct fcoe_dev_stats *stats;
-
-		stats = per_cpu_ptr(lport->dev_stats, cpu);
-
-		fcoe_stats->tx_frames += stats->TxFrames;
-		fcoe_stats->tx_words += stats->TxWords;
-		fcoe_stats->rx_frames += stats->RxFrames;
-		fcoe_stats->rx_words += stats->RxWords;
-		fcoe_stats->error_frames += stats->ErrorFrames;
-		fcoe_stats->invalid_crc_count += stats->InvalidCRCCount;
-		fcoe_stats->fcp_input_requests += stats->InputRequests;
-		fcoe_stats->fcp_output_requests += stats->OutputRequests;
-		fcoe_stats->fcp_control_requests += stats->ControlRequests;
+		struct fc_stats *stats;
+
+		stats = per_cpu_ptr(lport->stats, cpu);
+
+		fc_stats->tx_frames += stats->TxFrames;
+		fc_stats->tx_words += stats->TxWords;
+		fc_stats->rx_frames += stats->RxFrames;
+		fc_stats->rx_words += stats->RxWords;
+		fc_stats->error_frames += stats->ErrorFrames;
+		fc_stats->invalid_crc_count += stats->InvalidCRCCount;
+		fc_stats->fcp_input_requests += stats->InputRequests;
+		fc_stats->fcp_output_requests += stats->OutputRequests;
+		fc_stats->fcp_control_requests += stats->ControlRequests;
 		fcp_in_bytes += stats->InputBytes;
 		fcp_out_bytes += stats->OutputBytes;
-		fcoe_stats->link_failure_count += stats->LinkFailureCount;
+		fc_stats->fcp_packet_alloc_failures += stats->FcpPktAllocFails;
+		fc_stats->fcp_packet_aborts += stats->FcpPktAborts;
+		fc_stats->fcp_frame_alloc_failures += stats->FcpFrameAllocFails;
+		fc_stats->link_failure_count += stats->LinkFailureCount;
 	}
-	fcoe_stats->fcp_input_megabytes = div_u64(fcp_in_bytes, 1000000);
-	fcoe_stats->fcp_output_megabytes = div_u64(fcp_out_bytes, 1000000);
-	fcoe_stats->lip_count = -1;
-	fcoe_stats->nos_count = -1;
-	fcoe_stats->loss_of_sync_count = -1;
-	fcoe_stats->loss_of_signal_count = -1;
-	fcoe_stats->prim_seq_protocol_err_count = -1;
-	fcoe_stats->dumped_frames = -1;
-	return fcoe_stats;
+	fc_stats->fcp_input_megabytes = div_u64(fcp_in_bytes, 1000000);
+	fc_stats->fcp_output_megabytes = div_u64(fcp_out_bytes, 1000000);
+	fc_stats->lip_count = -1;
+	fc_stats->nos_count = -1;
+	fc_stats->loss_of_sync_count = -1;
+	fc_stats->loss_of_signal_count = -1;
+	fc_stats->prim_seq_protocol_err_count = -1;
+	fc_stats->dumped_frames = -1;
+
+	/* update exches stats */
+	fc_exch_update_stats(lport);
+
+	return fc_stats;
 }
 EXPORT_SYMBOL(fc_get_host_stats);
 
@@ -973,7 +980,8 @@ 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);
+	if (sp)
+		lport->tt.exch_done(sp);
 }
 
 /**
@@ -1590,8 +1598,9 @@ static void fc_lport_timeout(struct work_struct *work)
 	case LPORT_ST_RPA:
 	case LPORT_ST_DHBA:
 	case LPORT_ST_DPRT:
-		fc_lport_enter_ms(lport, lport->state);
-		break;
+		FC_LPORT_DBG(lport, "Skipping lport state %s to SCR\n",
+			     fc_lport_state(lport));
+		/* fall thru */
 	case LPORT_ST_SCR:
 		fc_lport_enter_scr(lport);
 		break;
diff --git a/drivers/scsi/libsas/sas_ata.c b/drivers/scsi/libsas/sas_ata.c
index d109cc3a17b..a59fcdc8fd6 100644
--- a/drivers/scsi/libsas/sas_ata.c
+++ b/drivers/scsi/libsas/sas_ata.c
@@ -523,6 +523,31 @@ static void sas_ata_set_dmamode(struct ata_port *ap, struct ata_device *ata_dev)
 		i->dft->lldd_ata_set_dmamode(dev);
 }
 
+static void sas_ata_sched_eh(struct ata_port *ap)
+{
+	struct domain_device *dev = ap->private_data;
+	struct sas_ha_struct *ha = dev->port->ha;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ha->lock, flags);
+	if (!test_and_set_bit(SAS_DEV_EH_PENDING, &dev->state))
+		ha->eh_active++;
+	ata_std_sched_eh(ap);
+	spin_unlock_irqrestore(&ha->lock, flags);
+}
+
+void sas_ata_end_eh(struct ata_port *ap)
+{
+	struct domain_device *dev = ap->private_data;
+	struct sas_ha_struct *ha = dev->port->ha;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ha->lock, flags);
+	if (test_and_clear_bit(SAS_DEV_EH_PENDING, &dev->state))
+		ha->eh_active--;
+	spin_unlock_irqrestore(&ha->lock, flags);
+}
+
 static struct ata_port_operations sas_sata_ops = {
 	.prereset		= ata_std_prereset,
 	.hardreset		= sas_ata_hard_reset,
@@ -536,6 +561,8 @@ static struct ata_port_operations sas_sata_ops = {
 	.port_start		= ata_sas_port_start,
 	.port_stop		= ata_sas_port_stop,
 	.set_dmamode		= sas_ata_set_dmamode,
+	.sched_eh		= sas_ata_sched_eh,
+	.end_eh			= sas_ata_end_eh,
 };
 
 static struct ata_port_info sata_port_info = {
@@ -591,7 +618,6 @@ void sas_ata_task_abort(struct sas_task *task)
 		spin_lock_irqsave(q->queue_lock, flags);
 		blk_abort_request(qc->scsicmd->request);
 		spin_unlock_irqrestore(q->queue_lock, flags);
-		scsi_schedule_eh(qc->scsicmd->device->host);
 		return;
 	}
 
@@ -708,10 +734,6 @@ static void async_sas_ata_eh(void *data, async_cookie_t cookie)
 	struct ata_port *ap = dev->sata_dev.ap;
 	struct sas_ha_struct *ha = dev->port->ha;
 
-	/* hold a reference over eh since we may be racing with final
-	 * remove once all commands are completed
-	 */
-	kref_get(&dev->kref);
 	sas_ata_printk(KERN_DEBUG, dev, "dev error handler\n");
 	ata_scsi_port_error_handler(ha->core.shost, ap);
 	sas_put_device(dev);
@@ -720,7 +742,7 @@ static void async_sas_ata_eh(void *data, async_cookie_t cookie)
 void sas_ata_strategy_handler(struct Scsi_Host *shost)
 {
 	struct sas_ha_struct *sas_ha = SHOST_TO_SAS_HA(shost);
-	LIST_HEAD(async);
+	ASYNC_DOMAIN_EXCLUSIVE(async);
 	int i;
 
 	/* it's ok to defer revalidation events during ata eh, these
@@ -742,6 +764,13 @@ void sas_ata_strategy_handler(struct Scsi_Host *shost)
 		list_for_each_entry(dev, &port->dev_list, dev_list_node) {
 			if (!dev_is_sata(dev))
 				continue;
+
+			/* hold a reference over eh since we may be
+			 * racing with final remove once all commands
+			 * are completed
+			 */
+			kref_get(&dev->kref);
+
 			async_schedule_domain(async_sas_ata_eh, dev, &async);
 		}
 		spin_unlock(&port->dev_list_lock);
diff --git a/drivers/scsi/libsas/sas_discover.c b/drivers/scsi/libsas/sas_discover.c
index 629a0865b13..3e9dc1a8435 100644
--- a/drivers/scsi/libsas/sas_discover.c
+++ b/drivers/scsi/libsas/sas_discover.c
@@ -39,18 +39,13 @@ void sas_init_dev(struct domain_device *dev)
 {
 	switch (dev->dev_type) {
 	case SAS_END_DEV:
+		INIT_LIST_HEAD(&dev->ssp_dev.eh_list_node);
 		break;
 	case EDGE_DEV:
 	case FANOUT_DEV:
 		INIT_LIST_HEAD(&dev->ex_dev.children);
 		mutex_init(&dev->ex_dev.cmd_mutex);
 		break;
-	case SATA_DEV:
-	case SATA_PM:
-	case SATA_PM_PORT:
-	case SATA_PENDING:
-		INIT_LIST_HEAD(&dev->sata_dev.children);
-		break;
 	default:
 		break;
 	}
@@ -286,6 +281,8 @@ void sas_free_device(struct kref *kref)
 
 static void sas_unregister_common_dev(struct asd_sas_port *port, struct domain_device *dev)
 {
+	struct sas_ha_struct *ha = port->ha;
+
 	sas_notify_lldd_dev_gone(dev);
 	if (!dev->parent)
 		dev->port->port_dev = NULL;
@@ -294,8 +291,18 @@ static void sas_unregister_common_dev(struct asd_sas_port *port, struct domain_d
 
 	spin_lock_irq(&port->dev_list_lock);
 	list_del_init(&dev->dev_list_node);
+	if (dev_is_sata(dev))
+		sas_ata_end_eh(dev->sata_dev.ap);
 	spin_unlock_irq(&port->dev_list_lock);
 
+	spin_lock_irq(&ha->lock);
+	if (dev->dev_type == SAS_END_DEV &&
+	    !list_empty(&dev->ssp_dev.eh_list_node)) {
+		list_del_init(&dev->ssp_dev.eh_list_node);
+		ha->eh_active--;
+	}
+	spin_unlock_irq(&ha->lock);
+
 	sas_put_device(dev);
 }
 
@@ -488,9 +495,9 @@ static void sas_chain_event(int event, unsigned long *pending,
 	if (!test_and_set_bit(event, pending)) {
 		unsigned long flags;
 
-		spin_lock_irqsave(&ha->state_lock, flags);
+		spin_lock_irqsave(&ha->lock, flags);
 		sas_chain_work(ha, sw);
-		spin_unlock_irqrestore(&ha->state_lock, flags);
+		spin_unlock_irqrestore(&ha->lock, flags);
 	}
 }
 
diff --git a/drivers/scsi/libsas/sas_event.c b/drivers/scsi/libsas/sas_event.c
index 4e4292d210c..789c4d8bb7a 100644
--- a/drivers/scsi/libsas/sas_event.c
+++ b/drivers/scsi/libsas/sas_event.c
@@ -47,9 +47,9 @@ static void sas_queue_event(int event, unsigned long *pending,
 	if (!test_and_set_bit(event, pending)) {
 		unsigned long flags;
 
-		spin_lock_irqsave(&ha->state_lock, flags);
+		spin_lock_irqsave(&ha->lock, flags);
 		sas_queue_work(ha, work);
-		spin_unlock_irqrestore(&ha->state_lock, flags);
+		spin_unlock_irqrestore(&ha->lock, flags);
 	}
 }
 
@@ -61,18 +61,18 @@ void __sas_drain_work(struct sas_ha_struct *ha)
 
 	set_bit(SAS_HA_DRAINING, &ha->state);
 	/* flush submitters */
-	spin_lock_irq(&ha->state_lock);
-	spin_unlock_irq(&ha->state_lock);
+	spin_lock_irq(&ha->lock);
+	spin_unlock_irq(&ha->lock);
 
 	drain_workqueue(wq);
 
-	spin_lock_irq(&ha->state_lock);
+	spin_lock_irq(&ha->lock);
 	clear_bit(SAS_HA_DRAINING, &ha->state);
 	list_for_each_entry_safe(sw, _sw, &ha->defer_q, drain_node) {
 		list_del_init(&sw->drain_node);
 		sas_queue_work(ha, sw);
 	}
-	spin_unlock_irq(&ha->state_lock);
+	spin_unlock_irq(&ha->lock);
 }
 
 int sas_drain_work(struct sas_ha_struct *ha)
diff --git a/drivers/scsi/libsas/sas_expander.c b/drivers/scsi/libsas/sas_expander.c
index caa0525d252..efc6e72f09f 100644
--- a/drivers/scsi/libsas/sas_expander.c
+++ b/drivers/scsi/libsas/sas_expander.c
@@ -51,14 +51,14 @@ static void smp_task_timedout(unsigned long _task)
 		task->task_state_flags |= SAS_TASK_STATE_ABORTED;
 	spin_unlock_irqrestore(&task->task_state_lock, flags);
 
-	complete(&task->completion);
+	complete(&task->slow_task->completion);
 }
 
 static void smp_task_done(struct sas_task *task)
 {
-	if (!del_timer(&task->timer))
+	if (!del_timer(&task->slow_task->timer))
 		return;
-	complete(&task->completion);
+	complete(&task->slow_task->completion);
 }
 
 /* Give it some long enough timeout. In seconds. */
@@ -79,7 +79,7 @@ static int smp_execute_task(struct domain_device *dev, void *req, int req_size,
 			break;
 		}
 
-		task = sas_alloc_task(GFP_KERNEL);
+		task = sas_alloc_slow_task(GFP_KERNEL);
 		if (!task) {
 			res = -ENOMEM;
 			break;
@@ -91,20 +91,20 @@ static int smp_execute_task(struct domain_device *dev, void *req, int req_size,
 
 		task->task_done = smp_task_done;
 
-		task->timer.data = (unsigned long) task;
-		task->timer.function = smp_task_timedout;
-		task->timer.expires = jiffies + SMP_TIMEOUT*HZ;
-		add_timer(&task->timer);
+		task->slow_task->timer.data = (unsigned long) task;
+		task->slow_task->timer.function = smp_task_timedout;
+		task->slow_task->timer.expires = jiffies + SMP_TIMEOUT*HZ;
+		add_timer(&task->slow_task->timer);
 
 		res = i->dft->lldd_execute_task(task, 1, GFP_KERNEL);
 
 		if (res) {
-			del_timer(&task->timer);
+			del_timer(&task->slow_task->timer);
 			SAS_DPRINTK("executing SMP task failed:%d\n", res);
 			break;
 		}
 
-		wait_for_completion(&task->completion);
+		wait_for_completion(&task->slow_task->completion);
 		res = -ECOMM;
 		if ((task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
 			SAS_DPRINTK("smp task timed out or aborted\n");
@@ -868,7 +868,7 @@ static struct domain_device *sas_ex_discover_end_dev(
 }
 
 /* See if this phy is part of a wide port */
-static int sas_ex_join_wide_port(struct domain_device *parent, int phy_id)
+static bool sas_ex_join_wide_port(struct domain_device *parent, int phy_id)
 {
 	struct ex_phy *phy = &parent->ex_dev.ex_phy[phy_id];
 	int i;
@@ -884,11 +884,11 @@ static int sas_ex_join_wide_port(struct domain_device *parent, int phy_id)
 			sas_port_add_phy(ephy->port, phy->phy);
 			phy->port = ephy->port;
 			phy->phy_state = PHY_DEVICE_DISCOVERED;
-			return 0;
+			return true;
 		}
 	}
 
-	return -ENODEV;
+	return false;
 }
 
 static struct domain_device *sas_ex_discover_expander(
@@ -1030,8 +1030,7 @@ static int sas_ex_discover_dev(struct domain_device *dev, int phy_id)
 		return res;
 	}
 
-	res = sas_ex_join_wide_port(dev, phy_id);
-	if (!res) {
+	if (sas_ex_join_wide_port(dev, phy_id)) {
 		SAS_DPRINTK("Attaching ex phy%d to wide port %016llx\n",
 			    phy_id, SAS_ADDR(ex_phy->attached_sas_addr));
 		return res;
@@ -1077,8 +1076,7 @@ static int sas_ex_discover_dev(struct domain_device *dev, int phy_id)
 			if (SAS_ADDR(ex->ex_phy[i].attached_sas_addr) ==
 			    SAS_ADDR(child->sas_addr)) {
 				ex->ex_phy[i].phy_state= PHY_DEVICE_DISCOVERED;
-				res = sas_ex_join_wide_port(dev, i);
-				if (!res)
+				if (sas_ex_join_wide_port(dev, i))
 					SAS_DPRINTK("Attaching ex phy%d to wide port %016llx\n",
 						    i, SAS_ADDR(ex->ex_phy[i].attached_sas_addr));
 
@@ -1943,32 +1941,20 @@ static int sas_discover_new(struct domain_device *dev, int phy_id)
 {
 	struct ex_phy *ex_phy = &dev->ex_dev.ex_phy[phy_id];
 	struct domain_device *child;
-	bool found = false;
-	int res, i;
+	int res;
 
 	SAS_DPRINTK("ex %016llx phy%d new device attached\n",
 		    SAS_ADDR(dev->sas_addr), phy_id);
 	res = sas_ex_phy_discover(dev, phy_id);
 	if (res)
-		goto out;
-	/* to support the wide port inserted */
-	for (i = 0; i < dev->ex_dev.num_phys; i++) {
-		struct ex_phy *ex_phy_temp = &dev->ex_dev.ex_phy[i];
-		if (i == phy_id)
-			continue;
-		if (SAS_ADDR(ex_phy_temp->attached_sas_addr) ==
-		    SAS_ADDR(ex_phy->attached_sas_addr)) {
-			found = true;
-			break;
-		}
-	}
-	if (found) {
-		sas_ex_join_wide_port(dev, phy_id);
+		return res;
+
+	if (sas_ex_join_wide_port(dev, phy_id))
 		return 0;
-	}
+
 	res = sas_ex_discover_devices(dev, phy_id);
-	if (!res)
-		goto out;
+	if (res)
+		return res;
 	list_for_each_entry(child, &dev->ex_dev.children, siblings) {
 		if (SAS_ADDR(child->sas_addr) ==
 		    SAS_ADDR(ex_phy->attached_sas_addr)) {
@@ -1978,7 +1964,6 @@ static int sas_discover_new(struct domain_device *dev, int phy_id)
 			break;
 		}
 	}
-out:
 	return res;
 }
 
@@ -2005,6 +1990,7 @@ static int sas_rediscover_dev(struct domain_device *dev, int phy_id, bool last)
 	u8 sas_addr[8];
 	int res;
 
+	memset(sas_addr, 0, 8);
 	res = sas_get_phy_attached_dev(dev, phy_id, sas_addr, &type);
 	switch (res) {
 	case SMP_RESP_NO_PHY:
@@ -2017,9 +2003,13 @@ static int sas_rediscover_dev(struct domain_device *dev, int phy_id, bool last)
 		return res;
 	case SMP_RESP_FUNC_ACC:
 		break;
+	case -ECOMM:
+		break;
+	default:
+		return res;
 	}
 
-	if (SAS_ADDR(sas_addr) == 0) {
+	if ((SAS_ADDR(sas_addr) == 0) || (res == -ECOMM)) {
 		phy->phy_state = PHY_EMPTY;
 		sas_unregister_devs_sas_addr(dev, phy_id, last);
 		return res;
@@ -2109,9 +2099,7 @@ int sas_ex_revalidate_domain(struct domain_device *port_dev)
 	struct domain_device *dev = NULL;
 
 	res = sas_find_bcast_dev(port_dev, &dev);
-	if (res)
-		goto out;
-	if (dev) {
+	while (res == 0 && dev) {
 		struct expander_device *ex = &dev->ex_dev;
 		int i = 0, phy_id;
 
@@ -2123,8 +2111,10 @@ int sas_ex_revalidate_domain(struct domain_device *port_dev)
 			res = sas_rediscover(dev, phy_id);
 			i = phy_id + 1;
 		} while (i < ex->num_phys);
+
+		dev = NULL;
+		res = sas_find_bcast_dev(port_dev, &dev);
 	}
-out:
 	return res;
 }
 
diff --git a/drivers/scsi/libsas/sas_init.c b/drivers/scsi/libsas/sas_init.c
index 10cb5ae3097..014297c0588 100644
--- a/drivers/scsi/libsas/sas_init.c
+++ b/drivers/scsi/libsas/sas_init.c
@@ -48,18 +48,37 @@ struct sas_task *sas_alloc_task(gfp_t flags)
 		INIT_LIST_HEAD(&task->list);
 		spin_lock_init(&task->task_state_lock);
 		task->task_state_flags = SAS_TASK_STATE_PENDING;
-		init_timer(&task->timer);
-		init_completion(&task->completion);
 	}
 
 	return task;
 }
 EXPORT_SYMBOL_GPL(sas_alloc_task);
 
+struct sas_task *sas_alloc_slow_task(gfp_t flags)
+{
+	struct sas_task *task = sas_alloc_task(flags);
+	struct sas_task_slow *slow = kmalloc(sizeof(*slow), flags);
+
+	if (!task || !slow) {
+		if (task)
+			kmem_cache_free(sas_task_cache, task);
+		kfree(slow);
+		return NULL;
+	}
+
+	task->slow_task = slow;
+	init_timer(&slow->timer);
+	init_completion(&slow->completion);
+
+	return task;
+}
+EXPORT_SYMBOL_GPL(sas_alloc_slow_task);
+
 void sas_free_task(struct sas_task *task)
 {
 	if (task) {
 		BUG_ON(!list_empty(&task->list));
+		kfree(task->slow_task);
 		kmem_cache_free(sas_task_cache, task);
 	}
 }
@@ -114,9 +133,11 @@ int sas_register_ha(struct sas_ha_struct *sas_ha)
 		sas_ha->lldd_queue_size = 128; /* Sanity */
 
 	set_bit(SAS_HA_REGISTERED, &sas_ha->state);
-	spin_lock_init(&sas_ha->state_lock);
+	spin_lock_init(&sas_ha->lock);
 	mutex_init(&sas_ha->drain_mutex);
+	init_waitqueue_head(&sas_ha->eh_wait_q);
 	INIT_LIST_HEAD(&sas_ha->defer_q);
+	INIT_LIST_HEAD(&sas_ha->eh_dev_q);
 
 	error = sas_register_phys(sas_ha);
 	if (error) {
@@ -163,9 +184,9 @@ int sas_unregister_ha(struct sas_ha_struct *sas_ha)
 	 * events to be queued, and flush any in-progress drainers
 	 */
 	mutex_lock(&sas_ha->drain_mutex);
-	spin_lock_irq(&sas_ha->state_lock);
+	spin_lock_irq(&sas_ha->lock);
 	clear_bit(SAS_HA_REGISTERED, &sas_ha->state);
-	spin_unlock_irq(&sas_ha->state_lock);
+	spin_unlock_irq(&sas_ha->lock);
 	__sas_drain_work(sas_ha);
 	mutex_unlock(&sas_ha->drain_mutex);
 
@@ -411,9 +432,9 @@ static int queue_phy_reset(struct sas_phy *phy, int hard_reset)
 	d->reset_result = 0;
 	d->hard_reset = hard_reset;
 
-	spin_lock_irq(&ha->state_lock);
+	spin_lock_irq(&ha->lock);
 	sas_queue_work(ha, &d->reset_work);
-	spin_unlock_irq(&ha->state_lock);
+	spin_unlock_irq(&ha->lock);
 
 	rc = sas_drain_work(ha);
 	if (rc == 0)
@@ -438,9 +459,9 @@ static int queue_phy_enable(struct sas_phy *phy, int enable)
 	d->enable_result = 0;
 	d->enable = enable;
 
-	spin_lock_irq(&ha->state_lock);
+	spin_lock_irq(&ha->lock);
 	sas_queue_work(ha, &d->enable_work);
-	spin_unlock_irq(&ha->state_lock);
+	spin_unlock_irq(&ha->lock);
 
 	rc = sas_drain_work(ha);
 	if (rc == 0)
diff --git a/drivers/scsi/libsas/sas_scsi_host.c b/drivers/scsi/libsas/sas_scsi_host.c
index f0b9b7bf188..6e795a174a1 100644
--- a/drivers/scsi/libsas/sas_scsi_host.c
+++ b/drivers/scsi/libsas/sas_scsi_host.c
@@ -460,14 +460,109 @@ struct sas_phy *sas_get_local_phy(struct domain_device *dev)
 }
 EXPORT_SYMBOL_GPL(sas_get_local_phy);
 
+static void sas_wait_eh(struct domain_device *dev)
+{
+	struct sas_ha_struct *ha = dev->port->ha;
+	DEFINE_WAIT(wait);
+
+	if (dev_is_sata(dev)) {
+		ata_port_wait_eh(dev->sata_dev.ap);
+		return;
+	}
+ retry:
+	spin_lock_irq(&ha->lock);
+
+	while (test_bit(SAS_DEV_EH_PENDING, &dev->state)) {
+		prepare_to_wait(&ha->eh_wait_q, &wait, TASK_UNINTERRUPTIBLE);
+		spin_unlock_irq(&ha->lock);
+		schedule();
+		spin_lock_irq(&ha->lock);
+	}
+	finish_wait(&ha->eh_wait_q, &wait);
+
+	spin_unlock_irq(&ha->lock);
+
+	/* make sure SCSI EH is complete */
+	if (scsi_host_in_recovery(ha->core.shost)) {
+		msleep(10);
+		goto retry;
+	}
+}
+EXPORT_SYMBOL(sas_wait_eh);
+
+static int sas_queue_reset(struct domain_device *dev, int reset_type, int lun, int wait)
+{
+	struct sas_ha_struct *ha = dev->port->ha;
+	int scheduled = 0, tries = 100;
+
+	/* ata: promote lun reset to bus reset */
+	if (dev_is_sata(dev)) {
+		sas_ata_schedule_reset(dev);
+		if (wait)
+			sas_ata_wait_eh(dev);
+		return SUCCESS;
+	}
+
+	while (!scheduled && tries--) {
+		spin_lock_irq(&ha->lock);
+		if (!test_bit(SAS_DEV_EH_PENDING, &dev->state) &&
+		    !test_bit(reset_type, &dev->state)) {
+			scheduled = 1;
+			ha->eh_active++;
+			list_add_tail(&dev->ssp_dev.eh_list_node, &ha->eh_dev_q);
+			set_bit(SAS_DEV_EH_PENDING, &dev->state);
+			set_bit(reset_type, &dev->state);
+			int_to_scsilun(lun, &dev->ssp_dev.reset_lun);
+			scsi_schedule_eh(ha->core.shost);
+		}
+		spin_unlock_irq(&ha->lock);
+
+		if (wait)
+			sas_wait_eh(dev);
+
+		if (scheduled)
+			return SUCCESS;
+	}
+
+	SAS_DPRINTK("%s reset of %s failed\n",
+		    reset_type == SAS_DEV_LU_RESET ? "LUN" : "Bus",
+		    dev_name(&dev->rphy->dev));
+
+	return FAILED;
+}
+
+int sas_eh_abort_handler(struct scsi_cmnd *cmd)
+{
+	int res;
+	struct sas_task *task = TO_SAS_TASK(cmd);
+	struct Scsi_Host *host = cmd->device->host;
+	struct sas_internal *i = to_sas_internal(host->transportt);
+
+	if (current != host->ehandler)
+		return FAILED;
+
+	if (!i->dft->lldd_abort_task)
+		return FAILED;
+
+	res = i->dft->lldd_abort_task(task);
+	if (res == TMF_RESP_FUNC_SUCC || res == TMF_RESP_FUNC_COMPLETE)
+		return SUCCESS;
+
+	return FAILED;
+}
+EXPORT_SYMBOL_GPL(sas_eh_abort_handler);
+
 /* Attempt to send a LUN reset message to a device */
 int sas_eh_device_reset_handler(struct scsi_cmnd *cmd)
 {
-	struct domain_device *dev = cmd_to_domain_dev(cmd);
-	struct sas_internal *i =
-		to_sas_internal(dev->port->ha->core.shost->transportt);
-	struct scsi_lun lun;
 	int res;
+	struct scsi_lun lun;
+	struct Scsi_Host *host = cmd->device->host;
+	struct domain_device *dev = cmd_to_domain_dev(cmd);
+	struct sas_internal *i = to_sas_internal(host->transportt);
+
+	if (current != host->ehandler)
+		return sas_queue_reset(dev, SAS_DEV_LU_RESET, cmd->device->lun, 0);
 
 	int_to_scsilun(cmd->device->lun, &lun);
 
@@ -481,21 +576,22 @@ int sas_eh_device_reset_handler(struct scsi_cmnd *cmd)
 	return FAILED;
 }
 
-/* Attempt to send a phy (bus) reset */
 int sas_eh_bus_reset_handler(struct scsi_cmnd *cmd)
 {
-	struct domain_device *dev = cmd_to_domain_dev(cmd);
-	struct sas_phy *phy = sas_get_local_phy(dev);
 	int res;
+	struct Scsi_Host *host = cmd->device->host;
+	struct domain_device *dev = cmd_to_domain_dev(cmd);
+	struct sas_internal *i = to_sas_internal(host->transportt);
 
-	res = sas_phy_reset(phy, 1);
-	if (res)
-		SAS_DPRINTK("Bus reset of %s failed 0x%x\n",
-			    kobject_name(&phy->dev.kobj),
-			    res);
-	sas_put_local_phy(phy);
+	if (current != host->ehandler)
+		return sas_queue_reset(dev, SAS_DEV_RESET, 0, 0);
 
-	if (res == TMF_RESP_FUNC_SUCC || res == TMF_RESP_FUNC_COMPLETE)
+	if (!i->dft->lldd_I_T_nexus_reset)
+		return FAILED;
+
+	res = i->dft->lldd_I_T_nexus_reset(dev);
+	if (res == TMF_RESP_FUNC_SUCC || res == TMF_RESP_FUNC_COMPLETE ||
+	    res == -ENODEV)
 		return SUCCESS;
 
 	return FAILED;
@@ -667,16 +763,53 @@ static void sas_eh_handle_sas_errors(struct Scsi_Host *shost, struct list_head *
 	goto out;
 }
 
+static void sas_eh_handle_resets(struct Scsi_Host *shost)
+{
+	struct sas_ha_struct *ha = SHOST_TO_SAS_HA(shost);
+	struct sas_internal *i = to_sas_internal(shost->transportt);
+
+	/* handle directed resets to sas devices */
+	spin_lock_irq(&ha->lock);
+	while (!list_empty(&ha->eh_dev_q)) {
+		struct domain_device *dev;
+		struct ssp_device *ssp;
+
+		ssp = list_entry(ha->eh_dev_q.next, typeof(*ssp), eh_list_node);
+		list_del_init(&ssp->eh_list_node);
+		dev = container_of(ssp, typeof(*dev), ssp_dev);
+		kref_get(&dev->kref);
+		WARN_ONCE(dev_is_sata(dev), "ssp reset to ata device?\n");
+
+		spin_unlock_irq(&ha->lock);
+
+		if (test_and_clear_bit(SAS_DEV_LU_RESET, &dev->state))
+			i->dft->lldd_lu_reset(dev, ssp->reset_lun.scsi_lun);
+
+		if (test_and_clear_bit(SAS_DEV_RESET, &dev->state))
+			i->dft->lldd_I_T_nexus_reset(dev);
+
+		sas_put_device(dev);
+		spin_lock_irq(&ha->lock);
+		clear_bit(SAS_DEV_EH_PENDING, &dev->state);
+		ha->eh_active--;
+	}
+	spin_unlock_irq(&ha->lock);
+}
+
+
 void sas_scsi_recover_host(struct Scsi_Host *shost)
 {
 	struct sas_ha_struct *ha = SHOST_TO_SAS_HA(shost);
-	unsigned long flags;
 	LIST_HEAD(eh_work_q);
+	int tries = 0;
+	bool retry;
 
-	spin_lock_irqsave(shost->host_lock, flags);
+retry:
+	tries++;
+	retry = true;
+	spin_lock_irq(shost->host_lock);
 	list_splice_init(&shost->eh_cmd_q, &eh_work_q);
-	shost->host_eh_scheduled = 0;
-	spin_unlock_irqrestore(shost->host_lock, flags);
+	spin_unlock_irq(shost->host_lock);
 
 	SAS_DPRINTK("Enter %s busy: %d failed: %d\n",
 		    __func__, shost->host_busy, shost->host_failed);
@@ -705,13 +838,26 @@ out:
 	if (ha->lldd_max_execute_num > 1)
 		wake_up_process(ha->core.queue_thread);
 
+	sas_eh_handle_resets(shost);
+
 	/* 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: busy: %d failed: %d\n",
-		    __func__, shost->host_busy, shost->host_failed);
+	/* check if any new eh work was scheduled during the last run */
+	spin_lock_irq(&ha->lock);
+	if (ha->eh_active == 0) {
+		shost->host_eh_scheduled = 0;
+		retry = false;
+	}
+	spin_unlock_irq(&ha->lock);
+
+	if (retry)
+		goto retry;
+
+	SAS_DPRINTK("--- Exit %s: busy: %d failed: %d tries: %d\n",
+		    __func__, shost->host_busy, shost->host_failed, tries);
 }
 
 enum blk_eh_timer_return sas_scsi_timed_out(struct scsi_cmnd *cmd)
@@ -988,9 +1134,13 @@ void sas_task_abort(struct sas_task *task)
 
 	/* Escape for libsas internal commands */
 	if (!sc) {
-		if (!del_timer(&task->timer))
+		struct sas_task_slow *slow = task->slow_task;
+
+		if (!slow)
+			return;
+		if (!del_timer(&slow->timer))
 			return;
-		task->timer.function(task->timer.data);
+		slow->timer.function(slow->timer.data);
 		return;
 	}
 
@@ -1003,7 +1153,6 @@ void sas_task_abort(struct sas_task *task)
 		spin_lock_irqsave(q->queue_lock, flags);
 		blk_abort_request(sc->request);
 		spin_unlock_irqrestore(q->queue_lock, flags);
-		scsi_schedule_eh(sc->device->host);
 	}
 }
 
diff --git a/drivers/scsi/lpfc/Makefile b/drivers/scsi/lpfc/Makefile
index fe5d396aca7..e2516ba8ebf 100644
--- a/drivers/scsi/lpfc/Makefile
+++ b/drivers/scsi/lpfc/Makefile
@@ -22,7 +22,9 @@
 ccflags-$(GCOV) := -fprofile-arcs -ftest-coverage
 ccflags-$(GCOV) += -O0
 
+ifdef WARNINGS_BECOME_ERRORS
 ccflags-y += -Werror
+endif
 
 obj-$(CONFIG_SCSI_LPFC) := lpfc.o
 
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index e5da6da20f8..a65c05a8d48 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -96,6 +96,10 @@ struct lpfc_sli2_slim;
 /* queue dump line buffer size */
 #define LPFC_LBUF_SZ		128
 
+/* mailbox system shutdown options */
+#define LPFC_MBX_NO_WAIT	0
+#define LPFC_MBX_WAIT		1
+
 enum lpfc_polling_flags {
 	ENABLE_FCP_RING_POLLING = 0x1,
 	DISABLE_FCP_RING_INT    = 0x2
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index 5eb2bc11618..adef5bb2100 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -3617,6 +3617,91 @@ lpfc_sriov_nr_virtfn_init(struct lpfc_hba *phba, int val)
 static DEVICE_ATTR(lpfc_sriov_nr_virtfn, S_IRUGO | S_IWUSR,
 		   lpfc_sriov_nr_virtfn_show, lpfc_sriov_nr_virtfn_store);
 
+/**
+ * lpfc_fcp_imax_store
+ *
+ * @dev: class device that is converted into a Scsi_host.
+ * @attr: device attribute, not used.
+ * @buf: string with the number of fast-path FCP interrupts per second.
+ * @count: unused variable.
+ *
+ * Description:
+ * If val is in a valid range [636,651042], then set the adapter's
+ * maximum number of fast-path FCP interrupts per second.
+ *
+ * Returns:
+ * length of the buf on success if val is in range the intended mode
+ * is supported.
+ * -EINVAL if val out of range or intended mode is not supported.
+ **/
+static ssize_t
+lpfc_fcp_imax_store(struct device *dev, struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	struct Scsi_Host *shost = class_to_shost(dev);
+	struct lpfc_vport *vport = (struct lpfc_vport *)shost->hostdata;
+	struct lpfc_hba *phba = vport->phba;
+	int val = 0, i;
+
+	/* Sanity check on user data */
+	if (!isdigit(buf[0]))
+		return -EINVAL;
+	if (sscanf(buf, "%i", &val) != 1)
+		return -EINVAL;
+
+	/* Value range is [636,651042] */
+	if (val < LPFC_MIM_IMAX || val > LPFC_DMULT_CONST)
+		return -EINVAL;
+
+	phba->cfg_fcp_imax = (uint32_t)val;
+	for (i = 0; i < phba->cfg_fcp_eq_count; i += LPFC_MAX_EQ_DELAY)
+		lpfc_modify_fcp_eq_delay(phba, i);
+
+	return strlen(buf);
+}
+
+/*
+# lpfc_fcp_imax: The maximum number of fast-path FCP interrupts per second
+#
+# Value range is [636,651042]. Default value is 10000.
+*/
+static int lpfc_fcp_imax = LPFC_FP_DEF_IMAX;
+module_param(lpfc_fcp_imax, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(lpfc_fcp_imax,
+	    "Set the maximum number of fast-path FCP interrupts per second");
+lpfc_param_show(fcp_imax)
+
+/**
+ * lpfc_fcp_imax_init - Set the initial sr-iov virtual function enable
+ * @phba: lpfc_hba pointer.
+ * @val: link speed value.
+ *
+ * Description:
+ * If val is in a valid range [636,651042], then initialize the adapter's
+ * maximum number of fast-path FCP interrupts per second.
+ *
+ * Returns:
+ * zero if val saved.
+ * -EINVAL val out of range
+ **/
+static int
+lpfc_fcp_imax_init(struct lpfc_hba *phba, int val)
+{
+	if (val >= LPFC_MIM_IMAX && val <= LPFC_DMULT_CONST) {
+		phba->cfg_fcp_imax = val;
+		return 0;
+	}
+
+	lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+			"3016 fcp_imax: %d out of range, using default\n", val);
+	phba->cfg_fcp_imax = LPFC_FP_DEF_IMAX;
+
+	return 0;
+}
+
+static DEVICE_ATTR(lpfc_fcp_imax, S_IRUGO | S_IWUSR,
+		   lpfc_fcp_imax_show, lpfc_fcp_imax_store);
+
 /*
 # lpfc_fcp_class:  Determines FC class to use for the FCP protocol.
 # Value range is [2,3]. Default value is 3.
@@ -3758,14 +3843,6 @@ LPFC_ATTR_R(use_msi, 2, 0, 2, "Use Message Signaled Interrupts (1) or "
 	    "MSI-X (2), if possible");
 
 /*
-# lpfc_fcp_imax: Set the maximum number of fast-path FCP interrupts per second
-#
-# Value range is [636,651042]. Default value is 10000.
-*/
-LPFC_ATTR_R(fcp_imax, LPFC_FP_DEF_IMAX, LPFC_MIM_IMAX, LPFC_DMULT_CONST,
-	    "Set the maximum number of fast-path FCP interrupts per second");
-
-/*
 # lpfc_fcp_wq_count: Set the number of fast-path FCP work queues
 #
 # Value range is [1,31]. Default value is 4.
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 9b2a16f3bc7..8a2a514a255 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -183,7 +183,7 @@ int lpfc_post_buffer(struct lpfc_hba *, struct lpfc_sli_ring *, int);
 void lpfc_decode_firmware_rev(struct lpfc_hba *, char *, int);
 int lpfc_online(struct lpfc_hba *);
 void lpfc_unblock_mgmt_io(struct lpfc_hba *);
-void lpfc_offline_prep(struct lpfc_hba *);
+void lpfc_offline_prep(struct lpfc_hba *, int);
 void lpfc_offline(struct lpfc_hba *);
 void lpfc_reset_hba(struct lpfc_hba *);
 
@@ -273,7 +273,7 @@ int lpfc_sli_host_down(struct lpfc_vport *);
 int lpfc_sli_hba_down(struct lpfc_hba *);
 int lpfc_sli_issue_mbox(struct lpfc_hba *, LPFC_MBOXQ_t *, uint32_t);
 int lpfc_sli_handle_mb_event(struct lpfc_hba *);
-void lpfc_sli_mbox_sys_shutdown(struct lpfc_hba *);
+void lpfc_sli_mbox_sys_shutdown(struct lpfc_hba *, int);
 int lpfc_sli_check_eratt(struct lpfc_hba *);
 void lpfc_sli_handle_slow_ring_event(struct lpfc_hba *,
 				    struct lpfc_sli_ring *, uint32_t);
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.h b/drivers/scsi/lpfc/lpfc_debugfs.h
index 616c400dae1..afe368fd1b9 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.h
+++ b/drivers/scsi/lpfc/lpfc_debugfs.h
@@ -395,8 +395,13 @@ lpfc_debug_dump_fcp_cq(struct lpfc_hba *phba, int fcp_wqidx)
 	for (fcp_cqidx = 0; fcp_cqidx < phba->cfg_fcp_eq_count; fcp_cqidx++)
 		if (phba->sli4_hba.fcp_cq[fcp_cqidx]->queue_id == fcp_cqid)
 			break;
-	if (fcp_cqidx >= phba->cfg_fcp_eq_count)
-		return;
+	if (phba->intr_type == MSIX) {
+		if (fcp_cqidx >= phba->cfg_fcp_eq_count)
+			return;
+	} else {
+		if (fcp_cqidx > 0)
+			return;
+	}
 
 	printk(KERN_ERR "FCP CQ: WQ[Idx:%d|Qid%d]->CQ[Idx%d|Qid%d]:\n",
 		fcp_wqidx, phba->sli4_hba.fcp_wq[fcp_wqidx]->queue_id,
@@ -426,8 +431,13 @@ lpfc_debug_dump_fcp_eq(struct lpfc_hba *phba, int fcp_wqidx)
 	for (fcp_cqidx = 0; fcp_cqidx < phba->cfg_fcp_eq_count; fcp_cqidx++)
 		if (phba->sli4_hba.fcp_cq[fcp_cqidx]->queue_id == fcp_cqid)
 			break;
-	if (fcp_cqidx >= phba->cfg_fcp_eq_count)
-		return;
+	if (phba->intr_type == MSIX) {
+		if (fcp_cqidx >= phba->cfg_fcp_eq_count)
+			return;
+	} else {
+		if (fcp_cqidx > 0)
+			return;
+	}
 
 	if (phba->cfg_fcp_eq_count == 0) {
 		fcp_eqidx = -1;
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 5bb269e224f..9b4f92941dc 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -530,7 +530,7 @@ lpfc_work_list_done(struct lpfc_hba *phba)
 			break;
 		case LPFC_EVT_OFFLINE_PREP:
 			if (phba->link_state >= LPFC_LINK_DOWN)
-				lpfc_offline_prep(phba);
+				lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 			*(int *)(evtp->evt_arg1) = 0;
 			complete((struct completion *)(evtp->evt_arg2));
 			break;
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index f1946dfda5b..953603a7a43 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -874,6 +874,7 @@ struct mbox_header {
 #define LPFC_MBOX_OPCODE_MQ_CREATE			0x15
 #define LPFC_MBOX_OPCODE_GET_CNTL_ATTRIBUTES		0x20
 #define LPFC_MBOX_OPCODE_NOP				0x21
+#define LPFC_MBOX_OPCODE_MODIFY_EQ_DELAY		0x29
 #define LPFC_MBOX_OPCODE_MQ_DESTROY			0x35
 #define LPFC_MBOX_OPCODE_CQ_DESTROY			0x36
 #define LPFC_MBOX_OPCODE_EQ_DESTROY			0x37
@@ -940,6 +941,13 @@ struct eq_context {
 	uint32_t reserved3;
 };
 
+struct eq_delay_info {
+	uint32_t eq_id;
+	uint32_t phase;
+	uint32_t delay_multi;
+};
+#define	LPFC_MAX_EQ_DELAY	8
+
 struct sgl_page_pairs {
 	uint32_t sgl_pg0_addr_lo;
 	uint32_t sgl_pg0_addr_hi;
@@ -1002,6 +1010,19 @@ struct lpfc_mbx_eq_create {
 	} u;
 };
 
+struct lpfc_mbx_modify_eq_delay {
+	struct mbox_header header;
+	union {
+		struct {
+			uint32_t num_eq;
+			struct eq_delay_info eq[LPFC_MAX_EQ_DELAY];
+		} request;
+		struct {
+			uint32_t word0;
+		} response;
+	} u;
+};
+
 struct lpfc_mbx_eq_destroy {
 	struct mbox_header header;
 	union {
@@ -2875,6 +2896,7 @@ struct lpfc_mqe {
 		struct lpfc_mbx_mq_create mq_create;
 		struct lpfc_mbx_mq_create_ext mq_create_ext;
 		struct lpfc_mbx_eq_create eq_create;
+		struct lpfc_mbx_modify_eq_delay eq_delay;
 		struct lpfc_mbx_cq_create cq_create;
 		struct lpfc_mbx_wq_create wq_create;
 		struct lpfc_mbx_rq_create rq_create;
@@ -3084,6 +3106,28 @@ struct lpfc_acqe_fc_la {
 #define LPFC_FC_LA_EVENT_TYPE_SHARED_LINK	0x2
 };
 
+struct lpfc_acqe_misconfigured_event {
+	struct {
+	uint32_t word0;
+#define lpfc_sli_misconfigured_port0_SHIFT	0
+#define lpfc_sli_misconfigured_port0_MASK	0x000000FF
+#define lpfc_sli_misconfigured_port0_WORD	word0
+#define lpfc_sli_misconfigured_port1_SHIFT	8
+#define lpfc_sli_misconfigured_port1_MASK	0x000000FF
+#define lpfc_sli_misconfigured_port1_WORD	word0
+#define lpfc_sli_misconfigured_port2_SHIFT	16
+#define lpfc_sli_misconfigured_port2_MASK	0x000000FF
+#define lpfc_sli_misconfigured_port2_WORD	word0
+#define lpfc_sli_misconfigured_port3_SHIFT	24
+#define lpfc_sli_misconfigured_port3_MASK	0x000000FF
+#define lpfc_sli_misconfigured_port3_WORD	word0
+	} theEvent;
+#define LPFC_SLI_EVENT_STATUS_VALID			0x00
+#define LPFC_SLI_EVENT_STATUS_NOT_PRESENT	0x01
+#define LPFC_SLI_EVENT_STATUS_WRONG_TYPE	0x02
+#define LPFC_SLI_EVENT_STATUS_UNSUPPORTED	0x03
+};
+
 struct lpfc_acqe_sli {
 	uint32_t event_data1;
 	uint32_t event_data2;
@@ -3094,6 +3138,7 @@ struct lpfc_acqe_sli {
 #define LPFC_SLI_EVENT_TYPE_NORM_TEMP		0x3
 #define LPFC_SLI_EVENT_TYPE_NVLOG_POST		0x4
 #define LPFC_SLI_EVENT_TYPE_DIAG_DUMP		0x5
+#define LPFC_SLI_EVENT_TYPE_MISCONFIGURED	0x9
 };
 
 /*
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 411ed48d79d..45c15208be9 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -73,6 +73,8 @@ static int lpfc_hba_down_post_s4(struct lpfc_hba *phba);
 static int lpfc_sli4_cq_event_pool_create(struct lpfc_hba *);
 static void lpfc_sli4_cq_event_pool_destroy(struct lpfc_hba *);
 static void lpfc_sli4_cq_event_release_all(struct lpfc_hba *);
+static void lpfc_sli4_disable_intr(struct lpfc_hba *);
+static uint32_t lpfc_sli4_enable_intr(struct lpfc_hba *, uint32_t);
 
 static struct scsi_transport_template *lpfc_transport_template = NULL;
 static struct scsi_transport_template *lpfc_vport_transport_template = NULL;
@@ -1169,7 +1171,7 @@ lpfc_offline_eratt(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	psli->sli_flag &= ~LPFC_SLI_ACTIVE;
 	spin_unlock_irq(&phba->hbalock);
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_NO_WAIT);
 
 	lpfc_offline(phba);
 	lpfc_reset_barrier(phba);
@@ -1193,7 +1195,7 @@ lpfc_offline_eratt(struct lpfc_hba *phba)
 static void
 lpfc_sli4_offline_eratt(struct lpfc_hba *phba)
 {
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_NO_WAIT);
 	lpfc_offline(phba);
 	lpfc_sli4_brdreset(phba);
 	lpfc_hba_down_post(phba);
@@ -1251,7 +1253,7 @@ lpfc_handle_deferred_eratt(struct lpfc_hba *phba)
 	 * There was a firmware error. Take the hba offline and then
 	 * attempt to restart it.
 	 */
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 	lpfc_offline(phba);
 
 	/* Wait for the ER1 bit to clear.*/
@@ -1372,7 +1374,7 @@ lpfc_handle_eratt_s3(struct lpfc_hba *phba)
 		 * There was a firmware error.  Take the hba offline and then
 		 * attempt to restart it.
 		 */
-		lpfc_offline_prep(phba);
+		lpfc_offline_prep(phba, LPFC_MBX_NO_WAIT);
 		lpfc_offline(phba);
 		lpfc_sli_brdrestart(phba);
 		if (lpfc_online(phba) == 0) {	/* Initialize the HBA */
@@ -1428,6 +1430,54 @@ lpfc_handle_eratt_s3(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_sli4_port_sta_fn_reset - The SLI4 function reset due to port status reg
+ * @phba: pointer to lpfc hba data structure.
+ * @mbx_action: flag for mailbox shutdown action.
+ *
+ * This routine is invoked to perform an SLI4 port PCI function reset in
+ * response to port status register polling attention. It waits for port
+ * status register (ERR, RDY, RN) bits before proceeding with function reset.
+ * During this process, interrupt vectors are freed and later requested
+ * for handling possible port resource change.
+ **/
+static int
+lpfc_sli4_port_sta_fn_reset(struct lpfc_hba *phba, int mbx_action)
+{
+	int rc;
+	uint32_t intr_mode;
+
+	/*
+	 * On error status condition, driver need to wait for port
+	 * ready before performing reset.
+	 */
+	rc = lpfc_sli4_pdev_status_reg_wait(phba);
+	if (!rc) {
+		/* need reset: attempt for port recovery */
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"2887 Reset Needed: Attempting Port "
+				"Recovery...\n");
+		lpfc_offline_prep(phba, mbx_action);
+		lpfc_offline(phba);
+		/* release interrupt for possible resource change */
+		lpfc_sli4_disable_intr(phba);
+		lpfc_sli_brdrestart(phba);
+		/* request and enable interrupt */
+		intr_mode = lpfc_sli4_enable_intr(phba, phba->intr_mode);
+		if (intr_mode == LPFC_INTR_ERROR) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"3175 Failed to enable interrupt\n");
+			return -EIO;
+		} else {
+			phba->intr_mode = intr_mode;
+		}
+		rc = lpfc_online(phba);
+		if (rc == 0)
+			lpfc_unblock_mgmt_io(phba);
+	}
+	return rc;
+}
+
+/**
  * lpfc_handle_eratt_s4 - The SLI4 HBA hardware error handler
  * @phba: pointer to lpfc hba data structure.
  *
@@ -1506,30 +1556,18 @@ lpfc_handle_eratt_s4(struct lpfc_hba *phba)
 			 reg_err2 == SLIPORT_ERR2_REG_FUNC_PROVISON)
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"3145 Port Down: Provisioning\n");
-		/*
-		 * On error status condition, driver need to wait for port
-		 * ready before performing reset.
-		 */
-		rc = lpfc_sli4_pdev_status_reg_wait(phba);
-		if (!rc) {
-			/* need reset: attempt for port recovery */
-			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2887 Reset Needed: Attempting Port "
-					"Recovery...\n");
-			lpfc_offline_prep(phba);
-			lpfc_offline(phba);
-			lpfc_sli_brdrestart(phba);
-			if (lpfc_online(phba) == 0) {
-				lpfc_unblock_mgmt_io(phba);
-				/* don't report event on forced debug dump */
-				if (reg_err1 == SLIPORT_ERR1_REG_ERR_CODE_2 &&
-				    reg_err2 == SLIPORT_ERR2_REG_FORCED_DUMP)
-					return;
-				else
-					break;
-			}
-			/* fall through for not able to recover */
+
+		/* Check port status register for function reset */
+		rc = lpfc_sli4_port_sta_fn_reset(phba, LPFC_MBX_NO_WAIT);
+		if (rc == 0) {
+			/* don't report event on forced debug dump */
+			if (reg_err1 == SLIPORT_ERR1_REG_ERR_CODE_2 &&
+			    reg_err2 == SLIPORT_ERR2_REG_FORCED_DUMP)
+				return;
+			else
+				break;
 		}
+		/* fall through for not able to recover */
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"3152 Unrecoverable error, bring the port "
 				"offline\n");
@@ -2494,15 +2532,19 @@ lpfc_stop_hba_timers(struct lpfc_hba *phba)
  * driver prepares the HBA interface for online or offline.
  **/
 static void
-lpfc_block_mgmt_io(struct lpfc_hba * phba)
+lpfc_block_mgmt_io(struct lpfc_hba *phba, int mbx_action)
 {
 	unsigned long iflag;
 	uint8_t actcmd = MBX_HEARTBEAT;
 	unsigned long timeout;
 
-	timeout = msecs_to_jiffies(LPFC_MBOX_TMO * 1000) + jiffies;
 	spin_lock_irqsave(&phba->hbalock, iflag);
 	phba->sli.sli_flag |= LPFC_BLOCK_MGMT_IO;
+	spin_unlock_irqrestore(&phba->hbalock, iflag);
+	if (mbx_action == LPFC_MBX_NO_WAIT)
+		return;
+	timeout = msecs_to_jiffies(LPFC_MBOX_TMO * 1000) + jiffies;
+	spin_lock_irqsave(&phba->hbalock, iflag);
 	if (phba->sli.mbox_active) {
 		actcmd = phba->sli.mbox_active->u.mb.mbxCommand;
 		/* Determine how long we might wait for the active mailbox
@@ -2592,7 +2634,7 @@ lpfc_online(struct lpfc_hba *phba)
 	lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
 			"0458 Bring Adapter online\n");
 
-	lpfc_block_mgmt_io(phba);
+	lpfc_block_mgmt_io(phba, LPFC_MBX_WAIT);
 
 	if (!lpfc_sli_queue_setup(phba)) {
 		lpfc_unblock_mgmt_io(phba);
@@ -2660,7 +2702,7 @@ lpfc_unblock_mgmt_io(struct lpfc_hba * phba)
  * queue to make it ready to be brought offline.
  **/
 void
-lpfc_offline_prep(struct lpfc_hba * phba)
+lpfc_offline_prep(struct lpfc_hba *phba, int mbx_action)
 {
 	struct lpfc_vport *vport = phba->pport;
 	struct lpfc_nodelist  *ndlp, *next_ndlp;
@@ -2671,7 +2713,7 @@ lpfc_offline_prep(struct lpfc_hba * phba)
 	if (vport->fc_flag & FC_OFFLINE_MODE)
 		return;
 
-	lpfc_block_mgmt_io(phba);
+	lpfc_block_mgmt_io(phba, mbx_action);
 
 	lpfc_linkdown(phba);
 
@@ -2718,7 +2760,7 @@ lpfc_offline_prep(struct lpfc_hba * phba)
 	}
 	lpfc_destroy_vport_work_array(phba, vports);
 
-	lpfc_sli_mbox_sys_shutdown(phba);
+	lpfc_sli_mbox_sys_shutdown(phba, mbx_action);
 }
 
 /**
@@ -3684,12 +3726,76 @@ out_free_pmb:
 static void
 lpfc_sli4_async_sli_evt(struct lpfc_hba *phba, struct lpfc_acqe_sli *acqe_sli)
 {
-	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-			"2901 Async SLI event - Event Data1:x%08x Event Data2:"
-			"x%08x SLI Event Type:%d",
-			acqe_sli->event_data1, acqe_sli->event_data2,
-			bf_get(lpfc_trailer_type, acqe_sli));
-	return;
+	char port_name;
+	char message[80];
+	uint8_t status;
+	struct lpfc_acqe_misconfigured_event *misconfigured;
+
+	/* special case misconfigured event as it contains data for all ports */
+	if ((bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) !=
+		 LPFC_SLI_INTF_IF_TYPE_2) ||
+		(bf_get(lpfc_trailer_type, acqe_sli) !=
+			LPFC_SLI_EVENT_TYPE_MISCONFIGURED)) {
+		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"2901 Async SLI event - Event Data1:x%08x Event Data2:"
+				"x%08x SLI Event Type:%d\n",
+				acqe_sli->event_data1, acqe_sli->event_data2,
+				bf_get(lpfc_trailer_type, acqe_sli));
+		return;
+	}
+
+	port_name = phba->Port[0];
+	if (port_name == 0x00)
+		port_name = '?'; /* get port name is empty */
+
+	misconfigured = (struct lpfc_acqe_misconfigured_event *)
+					&acqe_sli->event_data1;
+
+	/* fetch the status for this port */
+	switch (phba->sli4_hba.lnk_info.lnk_no) {
+	case LPFC_LINK_NUMBER_0:
+		status = bf_get(lpfc_sli_misconfigured_port0,
+					&misconfigured->theEvent);
+		break;
+	case LPFC_LINK_NUMBER_1:
+		status = bf_get(lpfc_sli_misconfigured_port1,
+					&misconfigured->theEvent);
+		break;
+	case LPFC_LINK_NUMBER_2:
+		status = bf_get(lpfc_sli_misconfigured_port2,
+					&misconfigured->theEvent);
+		break;
+	case LPFC_LINK_NUMBER_3:
+		status = bf_get(lpfc_sli_misconfigured_port3,
+					&misconfigured->theEvent);
+		break;
+	default:
+		status = ~LPFC_SLI_EVENT_STATUS_VALID;
+		break;
+	}
+
+	switch (status) {
+	case LPFC_SLI_EVENT_STATUS_VALID:
+		return; /* no message if the sfp is okay */
+	case LPFC_SLI_EVENT_STATUS_NOT_PRESENT:
+		sprintf(message, "Not installed");
+		break;
+	case LPFC_SLI_EVENT_STATUS_WRONG_TYPE:
+		sprintf(message,
+			"Optics of two types installed");
+		break;
+	case LPFC_SLI_EVENT_STATUS_UNSUPPORTED:
+		sprintf(message, "Incompatible optics");
+		break;
+	default:
+		/* firmware is reporting a status we don't know about */
+		sprintf(message, "Unknown event status x%02x", status);
+		break;
+	}
+
+	lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+			"3176 Misconfigured Physical Port - "
+			"Port Name %c %s\n", port_name, message);
 }
 
 /**
@@ -4312,7 +4418,7 @@ lpfc_reset_hba(struct lpfc_hba *phba)
 		phba->link_state = LPFC_HBA_ERROR;
 		return;
 	}
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 	lpfc_offline(phba);
 	lpfc_sli_brdrestart(phba);
 	lpfc_online(phba);
@@ -5514,14 +5620,45 @@ lpfc_destroy_shost(struct lpfc_hba *phba)
 static void
 lpfc_setup_bg(struct lpfc_hba *phba, struct Scsi_Host *shost)
 {
+	uint32_t old_mask;
+	uint32_t old_guard;
+
 	int pagecnt = 10;
 	if (lpfc_prot_mask && lpfc_prot_guard) {
 		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
 				"1478 Registering BlockGuard with the "
 				"SCSI layer\n");
-		scsi_host_set_prot(shost, lpfc_prot_mask);
-		scsi_host_set_guard(shost, lpfc_prot_guard);
+
+		old_mask = lpfc_prot_mask;
+		old_guard = lpfc_prot_guard;
+
+		/* Only allow supported values */
+		lpfc_prot_mask &= (SHOST_DIF_TYPE1_PROTECTION |
+			SHOST_DIX_TYPE0_PROTECTION |
+			SHOST_DIX_TYPE1_PROTECTION);
+		lpfc_prot_guard &= (SHOST_DIX_GUARD_IP | SHOST_DIX_GUARD_CRC);
+
+		/* DIF Type 1 protection for profiles AST1/C1 is end to end */
+		if (lpfc_prot_mask == SHOST_DIX_TYPE1_PROTECTION)
+			lpfc_prot_mask |= SHOST_DIF_TYPE1_PROTECTION;
+
+		if (lpfc_prot_mask && lpfc_prot_guard) {
+			if ((old_mask != lpfc_prot_mask) ||
+				(old_guard != lpfc_prot_guard))
+				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"1475 Registering BlockGuard with the "
+					"SCSI layer: mask %d  guard %d\n",
+					lpfc_prot_mask, lpfc_prot_guard);
+
+			scsi_host_set_prot(shost, lpfc_prot_mask);
+			scsi_host_set_guard(shost, lpfc_prot_guard);
+		} else
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"1479 Not Registering BlockGuard with the SCSI "
+				"layer, Bad protection parameters: %d %d\n",
+				old_mask, old_guard);
 	}
+
 	if (!_dump_buf_data) {
 		while (pagecnt) {
 			spin_lock_init(&_dump_buf_lock);
@@ -8859,7 +8996,7 @@ lpfc_pci_suspend_one_s3(struct pci_dev *pdev, pm_message_t msg)
 			"0473 PCI device Power Management suspend.\n");
 
 	/* Bring down the device */
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 	lpfc_offline(phba);
 	kthread_stop(phba->worker_thread);
 
@@ -8985,7 +9122,7 @@ lpfc_sli_prep_dev_for_reset(struct lpfc_hba *phba)
 			"2710 PCI channel disable preparing for reset\n");
 
 	/* Block any management I/Os to the device */
-	lpfc_block_mgmt_io(phba);
+	lpfc_block_mgmt_io(phba, LPFC_MBX_WAIT);
 
 	/* Block all SCSI devices' I/Os on the host */
 	lpfc_scsi_dev_block(phba);
@@ -9129,7 +9266,7 @@ lpfc_io_slot_reset_s3(struct pci_dev *pdev)
 		phba->intr_mode = intr_mode;
 
 	/* Take device offline, it will perform cleanup */
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 	lpfc_offline(phba);
 	lpfc_sli_brdrestart(phba);
 
@@ -9603,7 +9740,7 @@ lpfc_pci_suspend_one_s4(struct pci_dev *pdev, pm_message_t msg)
 			"2843 PCI device Power Management suspend.\n");
 
 	/* Bring down the device */
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 	lpfc_offline(phba);
 	kthread_stop(phba->worker_thread);
 
@@ -9729,7 +9866,7 @@ lpfc_sli4_prep_dev_for_reset(struct lpfc_hba *phba)
 			"2826 PCI channel disable preparing for reset\n");
 
 	/* Block any management I/Os to the device */
-	lpfc_block_mgmt_io(phba);
+	lpfc_block_mgmt_io(phba, LPFC_MBX_NO_WAIT);
 
 	/* Block all SCSI devices' I/Os on the host */
 	lpfc_scsi_dev_block(phba);
@@ -9902,7 +10039,7 @@ lpfc_io_resume_s4(struct pci_dev *pdev)
 	 */
 	if (!(phba->sli.sli_flag & LPFC_SLI_ACTIVE)) {
 		/* Perform device reset */
-		lpfc_offline_prep(phba);
+		lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 		lpfc_offline(phba);
 		lpfc_sli_brdrestart(phba);
 		/* Bring the device back online */
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 66e09069f28..925975d2d76 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -4275,10 +4275,8 @@ lpfc_queuecommand_lck(struct scsi_cmnd *cmnd, void (*done) (struct scsi_cmnd *))
 	 * Catch race where our node has transitioned, but the
 	 * transport is still transitioning.
 	 */
-	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
-		cmnd->result = ScsiResult(DID_IMM_RETRY, 0);
-		goto out_fail_command;
-	}
+	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
+		goto out_tgt_busy;
 	if (atomic_read(&ndlp->cmd_pending) >= ndlp->cmd_qdepth)
 		goto out_tgt_busy;
 
@@ -4412,12 +4410,12 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	struct lpfc_iocbq *abtsiocb;
 	struct lpfc_scsi_buf *lpfc_cmd;
 	IOCB_t *cmd, *icmd;
-	int ret = SUCCESS;
+	int ret = SUCCESS, status = 0;
 	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(waitq);
 
-	ret = fc_block_scsi_eh(cmnd);
-	if (ret)
-		return ret;
+	status = fc_block_scsi_eh(cmnd);
+	if (status)
+		return status;
 
 	spin_lock_irq(&phba->hbalock);
 	/* driver queued commands are in process of being flushed */
@@ -4435,7 +4433,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			 "2873 SCSI Layer I/O Abort Request IO CMPL Status "
 			 "x%x ID %d LUN %d\n",
-			 ret, cmnd->device->id, cmnd->device->lun);
+			 SUCCESS, cmnd->device->id, cmnd->device->lun);
 		return SUCCESS;
 	}
 
@@ -4762,7 +4760,7 @@ lpfc_device_reset_handler(struct scsi_cmnd *cmnd)
 	unsigned tgt_id = cmnd->device->id;
 	unsigned int lun_id = cmnd->device->lun;
 	struct lpfc_scsi_event_header scsi_event;
-	int status;
+	int status, ret = SUCCESS;
 
 	if (!rdata) {
 		lpfc_printf_vlog(vport, KERN_ERR, LOG_FCP,
@@ -4803,9 +4801,9 @@ lpfc_device_reset_handler(struct scsi_cmnd *cmnd)
 	 * So, continue on.
 	 * We will report success if all the i/o aborts successfully.
 	 */
-	status = lpfc_reset_flush_io_context(vport, tgt_id, lun_id,
+	ret = lpfc_reset_flush_io_context(vport, tgt_id, lun_id,
 						LPFC_CTX_LUN);
-	return status;
+	return ret;
 }
 
 /**
@@ -4829,7 +4827,7 @@ lpfc_target_reset_handler(struct scsi_cmnd *cmnd)
 	unsigned tgt_id = cmnd->device->id;
 	unsigned int lun_id = cmnd->device->lun;
 	struct lpfc_scsi_event_header scsi_event;
-	int status;
+	int status, ret = SUCCESS;
 
 	if (!rdata) {
 		lpfc_printf_vlog(vport, KERN_ERR, LOG_FCP,
@@ -4870,9 +4868,9 @@ lpfc_target_reset_handler(struct scsi_cmnd *cmnd)
 	 * So, continue on.
 	 * We will report success if all the i/o aborts successfully.
 	 */
-	status = lpfc_reset_flush_io_context(vport, tgt_id, lun_id,
-					LPFC_CTX_TGT);
-	return status;
+	ret = lpfc_reset_flush_io_context(vport, tgt_id, lun_id,
+					  LPFC_CTX_TGT);
+	return ret;
 }
 
 /**
@@ -4982,7 +4980,7 @@ lpfc_host_reset_handler(struct scsi_cmnd *cmnd)
 	struct lpfc_hba *phba = vport->phba;
 	int rc, ret = SUCCESS;
 
-	lpfc_offline_prep(phba);
+	lpfc_offline_prep(phba, LPFC_MBX_WAIT);
 	lpfc_offline(phba);
 	rc = lpfc_sli_brdrestart(phba);
 	if (rc)
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index b4720a10981..9cbd20b1328 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -8984,7 +8984,7 @@ lpfc_sli_hba_down(struct lpfc_hba *phba)
 	int i;
 
 	/* Shutdown the mailbox command sub-system */
-	lpfc_sli_mbox_sys_shutdown(phba);
+	lpfc_sli_mbox_sys_shutdown(phba, LPFC_MBX_WAIT);
 
 	lpfc_hba_down_prep(phba);
 
@@ -9996,11 +9996,17 @@ lpfc_sli_issue_mbox_wait(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq,
  * sub-system flush routine to gracefully bring down mailbox sub-system.
  **/
 void
-lpfc_sli_mbox_sys_shutdown(struct lpfc_hba *phba)
+lpfc_sli_mbox_sys_shutdown(struct lpfc_hba *phba, int mbx_action)
 {
 	struct lpfc_sli *psli = &phba->sli;
 	unsigned long timeout;
 
+	if (mbx_action == LPFC_MBX_NO_WAIT) {
+		/* delay 100ms for port state */
+		msleep(100);
+		lpfc_sli_mbox_sys_flush(phba);
+		return;
+	}
 	timeout = msecs_to_jiffies(LPFC_MBOX_TMO * 1000) + jiffies;
 
 	spin_lock_irq(&phba->hbalock);
@@ -12042,6 +12048,83 @@ out_fail:
 }
 
 /**
+ * lpfc_modify_fcp_eq_delay - Modify Delay Multiplier on FCP EQs
+ * @phba: HBA structure that indicates port to create a queue on.
+ * @startq: The starting FCP EQ to modify
+ *
+ * This function sends an MODIFY_EQ_DELAY mailbox command to the HBA.
+ *
+ * The @phba struct is used to send mailbox command to HBA. The @startq
+ * is used to get the starting FCP EQ to change.
+ * This function is asynchronous and will wait for the mailbox
+ * command to finish before continuing.
+ *
+ * On success this function will return a zero. If unable to allocate enough
+ * memory this function will return -ENOMEM. If the queue create mailbox command
+ * fails this function will return -ENXIO.
+ **/
+uint32_t
+lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
+{
+	struct lpfc_mbx_modify_eq_delay *eq_delay;
+	LPFC_MBOXQ_t *mbox;
+	struct lpfc_queue *eq;
+	int cnt, rc, length, status = 0;
+	uint32_t shdr_status, shdr_add_status;
+	int fcp_eqidx;
+	union lpfc_sli4_cfg_shdr *shdr;
+	uint16_t dmult;
+
+	if (startq >= phba->cfg_fcp_eq_count)
+		return 0;
+
+	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mbox)
+		return -ENOMEM;
+	length = (sizeof(struct lpfc_mbx_modify_eq_delay) -
+		  sizeof(struct lpfc_sli4_cfg_mhdr));
+	lpfc_sli4_config(phba, mbox, LPFC_MBOX_SUBSYSTEM_COMMON,
+			 LPFC_MBOX_OPCODE_MODIFY_EQ_DELAY,
+			 length, LPFC_SLI4_MBX_EMBED);
+	eq_delay = &mbox->u.mqe.un.eq_delay;
+
+	/* Calculate delay multiper from maximum interrupt per second */
+	dmult = LPFC_DMULT_CONST/phba->cfg_fcp_imax - 1;
+
+	cnt = 0;
+	for (fcp_eqidx = startq; fcp_eqidx < phba->cfg_fcp_eq_count;
+	    fcp_eqidx++) {
+		eq = phba->sli4_hba.fp_eq[fcp_eqidx];
+		if (!eq)
+			continue;
+		eq_delay->u.request.eq[cnt].eq_id = eq->queue_id;
+		eq_delay->u.request.eq[cnt].phase = 0;
+		eq_delay->u.request.eq[cnt].delay_multi = dmult;
+		cnt++;
+		if (cnt >= LPFC_MAX_EQ_DELAY)
+			break;
+	}
+	eq_delay->u.request.num_eq = cnt;
+
+	mbox->vport = phba->pport;
+	mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+	mbox->context1 = NULL;
+	rc = lpfc_sli_issue_mbox(phba, mbox, MBX_POLL);
+	shdr = (union lpfc_sli4_cfg_shdr *) &eq_delay->header.cfg_shdr;
+	shdr_status = bf_get(lpfc_mbox_hdr_status, &shdr->response);
+	shdr_add_status = bf_get(lpfc_mbox_hdr_add_status, &shdr->response);
+	if (shdr_status || shdr_add_status || rc) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"2512 MODIFY_EQ_DELAY mailbox failed with "
+				"status x%x add_status x%x, mbx status x%x\n",
+				shdr_status, shdr_add_status, rc);
+		status = -ENXIO;
+	}
+	mempool_free(mbox, phba->mbox_mem_pool);
+	return status;
+}
+
+/**
  * lpfc_eq_create - Create an Event Queue on the HBA
  * @phba: HBA structure that indicates port to create a queue on.
  * @eq: The queue structure to use to create the event queue.
@@ -12228,8 +12311,10 @@ lpfc_cq_create(struct lpfc_hba *phba, struct lpfc_queue *cq,
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"0361 Unsupported CQ count. (%d)\n",
 				cq->entry_count);
-		if (cq->entry_count < 256)
-			return -EINVAL;
+		if (cq->entry_count < 256) {
+			status = -EINVAL;
+			goto out;
+		}
 		/* otherwise default to smallest count (drop through) */
 	case 256:
 		bf_set(lpfc_cq_context_count, &cq_create->u.request.context,
@@ -12420,8 +12505,10 @@ lpfc_mq_create(struct lpfc_hba *phba, struct lpfc_queue *mq,
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"0362 Unsupported MQ count. (%d)\n",
 				mq->entry_count);
-		if (mq->entry_count < 16)
-			return -EINVAL;
+		if (mq->entry_count < 16) {
+			status = -EINVAL;
+			goto out;
+		}
 		/* otherwise default to smallest count (drop through) */
 	case 16:
 		bf_set(lpfc_mq_context_ring_size,
@@ -12710,8 +12797,10 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 					"2535 Unsupported RQ count. (%d)\n",
 					hrq->entry_count);
-			if (hrq->entry_count < 512)
-				return -EINVAL;
+			if (hrq->entry_count < 512) {
+				status = -EINVAL;
+				goto out;
+			}
 			/* otherwise default to smallest count (drop through) */
 		case 512:
 			bf_set(lpfc_rq_context_rqe_count,
@@ -12791,8 +12880,10 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 					"2536 Unsupported RQ count. (%d)\n",
 					drq->entry_count);
-			if (drq->entry_count < 512)
-				return -EINVAL;
+			if (drq->entry_count < 512) {
+				status = -EINVAL;
+				goto out;
+			}
 			/* otherwise default to smallest count (drop through) */
 		case 512:
 			bf_set(lpfc_rq_context_rqe_count,
@@ -15855,24 +15946,18 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 		spin_lock_irqsave(&phba->hbalock, iflags);
 
 		piocbq = lpfc_sli_ringtx_get(phba, pring);
+		if (!piocbq) {
+			spin_unlock_irqrestore(&phba->hbalock, iflags);
+			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+				"2823 txq empty and txq_cnt is %d\n ",
+				pring->txq_cnt);
+			break;
+		}
 		sglq = __lpfc_sli_get_sglq(phba, piocbq);
 		if (!sglq) {
 			__lpfc_sli_ringtx_put(phba, pring, piocbq);
 			spin_unlock_irqrestore(&phba->hbalock, iflags);
 			break;
-		} else {
-			if (!piocbq) {
-				/* The txq_cnt out of sync. This should
-				 * never happen
-				 */
-				sglq = __lpfc_clear_active_sglq(phba,
-						 sglq->sli4_lxritag);
-				spin_unlock_irqrestore(&phba->hbalock, iflags);
-				lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-					"2823 txq empty and txq_cnt is %d\n ",
-					pring->txq_cnt);
-				break;
-			}
 		}
 
 		/* The xri and iocb resources secured,
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index a4a77080091..ec756118c5c 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -598,6 +598,7 @@ struct lpfc_queue *lpfc_sli4_queue_alloc(struct lpfc_hba *, uint32_t,
 			uint32_t);
 void lpfc_sli4_queue_free(struct lpfc_queue *);
 uint32_t lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint16_t);
+uint32_t lpfc_modify_fcp_eq_delay(struct lpfc_hba *, uint16_t);
 uint32_t lpfc_cq_create(struct lpfc_hba *, struct lpfc_queue *,
 			struct lpfc_queue *, uint32_t, uint32_t);
 int32_t lpfc_mq_create(struct lpfc_hba *, struct lpfc_queue *,
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 59c57a40998..4704e5b5088 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "8.3.31"
+#define LPFC_DRIVER_VERSION "8.3.32"
 #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/megaraid.c b/drivers/scsi/megaraid.c
index 4d39a9ffc08..97825f11695 100644
--- a/drivers/scsi/megaraid.c
+++ b/drivers/scsi/megaraid.c
@@ -524,7 +524,7 @@ mega_build_cmd(adapter_t *adapter, Scsi_Cmnd *cmd, int *busy)
 	mega_passthru	*pthru;
 	scb_t	*scb;
 	mbox_t	*mbox;
-	long	seg;
+	u32	seg;
 	char	islogical;
 	int	max_ldrv_num;
 	int	channel = 0;
@@ -858,7 +858,7 @@ mega_build_cmd(adapter_t *adapter, Scsi_Cmnd *cmd, int *busy)
 
 			/* Calculate Scatter-Gather info */
 			mbox->m_out.numsgelements = mega_build_sglist(adapter, scb,
-					(u32 *)&mbox->m_out.xferaddr, (u32 *)&seg);
+					(u32 *)&mbox->m_out.xferaddr, &seg);
 
 			return scb;
 
diff --git a/drivers/scsi/megaraid/megaraid_mbox.c b/drivers/scsi/megaraid/megaraid_mbox.c
index 35bd13879fe..54b1c5bb310 100644
--- a/drivers/scsi/megaraid/megaraid_mbox.c
+++ b/drivers/scsi/megaraid/megaraid_mbox.c
@@ -2731,7 +2731,7 @@ megaraid_reset_handler(struct scsi_cmnd *scp)
 	}
 
  out:
-	spin_unlock_irq(&adapter->lock);
+	spin_unlock(&adapter->lock);
 	return rval;
 }
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index b6dd3a5de7f..b3a1a30055d 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -1158,6 +1158,7 @@ extern struct scsi_transport_template *mpt2sas_transport_template;
 extern int scsi_internal_device_block(struct scsi_device *sdev);
 extern u8 mpt2sas_stm_zero_smid_handler(struct MPT2SAS_ADAPTER *ioc,
     u8 msix_index, u32 reply);
-extern int scsi_internal_device_unblock(struct scsi_device *sdev);
+extern int scsi_internal_device_unblock(struct scsi_device *sdev,
+					enum scsi_device_state new_state);
 
 #endif /* MPT2SAS_BASE_H_INCLUDED */
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index 76973e8ca4b..b1ebd6f8dab 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -2904,7 +2904,7 @@ _scsih_ublock_io_all_device(struct MPT2SAS_ADAPTER *ioc)
 		dewtprintk(ioc, sdev_printk(KERN_INFO, sdev, "device_running, "
 		    "handle(0x%04x)\n",
 		    sas_device_priv_data->sas_target->handle));
-		scsi_internal_device_unblock(sdev);
+		scsi_internal_device_unblock(sdev, SDEV_RUNNING);
 	}
 }
 /**
@@ -2933,7 +2933,7 @@ _scsih_ublock_io_device(struct MPT2SAS_ADAPTER *ioc, u64 sas_address)
 			    "sas address(0x%016llx)\n", ioc->name,
 				(unsigned long long)sas_address));
 			sas_device_priv_data->block = 0;
-			scsi_internal_device_unblock(sdev);
+			scsi_internal_device_unblock(sdev, SDEV_RUNNING);
 		}
 	}
 }
diff --git a/drivers/scsi/mvsas/mv_sas.c b/drivers/scsi/mvsas/mv_sas.c
index fd3b2839843..4539d59a085 100644
--- a/drivers/scsi/mvsas/mv_sas.c
+++ b/drivers/scsi/mvsas/mv_sas.c
@@ -885,7 +885,6 @@ static int mvs_task_exec(struct sas_task *task, const int num, gfp_t gfp_flags,
 				struct completion *completion, int is_tmf,
 				struct mvs_tmf_task *tmf)
 {
-	struct domain_device *dev = task->dev;
 	struct mvs_info *mvi = NULL;
 	u32 rc = 0;
 	u32 pass = 0;
@@ -1365,9 +1364,9 @@ void mvs_dev_gone(struct domain_device *dev)
 
 static void mvs_task_done(struct sas_task *task)
 {
-	if (!del_timer(&task->timer))
+	if (!del_timer(&task->slow_task->timer))
 		return;
-	complete(&task->completion);
+	complete(&task->slow_task->completion);
 }
 
 static void mvs_tmf_timedout(unsigned long data)
@@ -1375,7 +1374,7 @@ static void mvs_tmf_timedout(unsigned long data)
 	struct sas_task *task = (struct sas_task *)data;
 
 	task->task_state_flags |= SAS_TASK_STATE_ABORTED;
-	complete(&task->completion);
+	complete(&task->slow_task->completion);
 }
 
 #define MVS_TASK_TIMEOUT 20
@@ -1386,7 +1385,7 @@ static int mvs_exec_internal_tmf_task(struct domain_device *dev,
 	struct sas_task *task = NULL;
 
 	for (retry = 0; retry < 3; retry++) {
-		task = sas_alloc_task(GFP_KERNEL);
+		task = sas_alloc_slow_task(GFP_KERNEL);
 		if (!task)
 			return -ENOMEM;
 
@@ -1396,20 +1395,20 @@ static int mvs_exec_internal_tmf_task(struct domain_device *dev,
 		memcpy(&task->ssp_task, parameter, para_len);
 		task->task_done = mvs_task_done;
 
-		task->timer.data = (unsigned long) task;
-		task->timer.function = mvs_tmf_timedout;
-		task->timer.expires = jiffies + MVS_TASK_TIMEOUT*HZ;
-		add_timer(&task->timer);
+		task->slow_task->timer.data = (unsigned long) task;
+		task->slow_task->timer.function = mvs_tmf_timedout;
+		task->slow_task->timer.expires = jiffies + MVS_TASK_TIMEOUT*HZ;
+		add_timer(&task->slow_task->timer);
 
 		res = mvs_task_exec(task, 1, GFP_KERNEL, NULL, 1, tmf);
 
 		if (res) {
-			del_timer(&task->timer);
+			del_timer(&task->slow_task->timer);
 			mv_printk("executing internel task failed:%d\n", res);
 			goto ex_err;
 		}
 
-		wait_for_completion(&task->completion);
+		wait_for_completion(&task->slow_task->completion);
 		res = TMF_RESP_FUNC_FAILED;
 		/* Even TMF timed out, return direct. */
 		if ((task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
diff --git a/drivers/scsi/pm8001/pm8001_sas.c b/drivers/scsi/pm8001/pm8001_sas.c
index 3b11edd4a50..b961112395d 100644
--- a/drivers/scsi/pm8001/pm8001_sas.c
+++ b/drivers/scsi/pm8001/pm8001_sas.c
@@ -650,9 +650,9 @@ int pm8001_dev_found(struct domain_device *dev)
 
 static void pm8001_task_done(struct sas_task *task)
 {
-	if (!del_timer(&task->timer))
+	if (!del_timer(&task->slow_task->timer))
 		return;
-	complete(&task->completion);
+	complete(&task->slow_task->completion);
 }
 
 static void pm8001_tmf_timedout(unsigned long data)
@@ -660,7 +660,7 @@ static void pm8001_tmf_timedout(unsigned long data)
 	struct sas_task *task = (struct sas_task *)data;
 
 	task->task_state_flags |= SAS_TASK_STATE_ABORTED;
-	complete(&task->completion);
+	complete(&task->slow_task->completion);
 }
 
 #define PM8001_TASK_TIMEOUT 20
@@ -683,7 +683,7 @@ static int pm8001_exec_internal_tmf_task(struct domain_device *dev,
 	struct pm8001_hba_info *pm8001_ha = pm8001_find_ha_by_dev(dev);
 
 	for (retry = 0; retry < 3; retry++) {
-		task = sas_alloc_task(GFP_KERNEL);
+		task = sas_alloc_slow_task(GFP_KERNEL);
 		if (!task)
 			return -ENOMEM;
 
@@ -691,21 +691,21 @@ static int pm8001_exec_internal_tmf_task(struct domain_device *dev,
 		task->task_proto = dev->tproto;
 		memcpy(&task->ssp_task, parameter, para_len);
 		task->task_done = pm8001_task_done;
-		task->timer.data = (unsigned long)task;
-		task->timer.function = pm8001_tmf_timedout;
-		task->timer.expires = jiffies + PM8001_TASK_TIMEOUT*HZ;
-		add_timer(&task->timer);
+		task->slow_task->timer.data = (unsigned long)task;
+		task->slow_task->timer.function = pm8001_tmf_timedout;
+		task->slow_task->timer.expires = jiffies + PM8001_TASK_TIMEOUT*HZ;
+		add_timer(&task->slow_task->timer);
 
 		res = pm8001_task_exec(task, 1, GFP_KERNEL, 1, tmf);
 
 		if (res) {
-			del_timer(&task->timer);
+			del_timer(&task->slow_task->timer);
 			PM8001_FAIL_DBG(pm8001_ha,
 				pm8001_printk("Executing internal task "
 				"failed\n"));
 			goto ex_err;
 		}
-		wait_for_completion(&task->completion);
+		wait_for_completion(&task->slow_task->completion);
 		res = -TMF_RESP_FUNC_FAILED;
 		/* Even TMF timed out, return direct. */
 		if ((task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
@@ -765,17 +765,17 @@ pm8001_exec_internal_task_abort(struct pm8001_hba_info *pm8001_ha,
 	struct sas_task *task = NULL;
 
 	for (retry = 0; retry < 3; retry++) {
-		task = sas_alloc_task(GFP_KERNEL);
+		task = sas_alloc_slow_task(GFP_KERNEL);
 		if (!task)
 			return -ENOMEM;
 
 		task->dev = dev;
 		task->task_proto = dev->tproto;
 		task->task_done = pm8001_task_done;
-		task->timer.data = (unsigned long)task;
-		task->timer.function = pm8001_tmf_timedout;
-		task->timer.expires = jiffies + PM8001_TASK_TIMEOUT * HZ;
-		add_timer(&task->timer);
+		task->slow_task->timer.data = (unsigned long)task;
+		task->slow_task->timer.function = pm8001_tmf_timedout;
+		task->slow_task->timer.expires = jiffies + PM8001_TASK_TIMEOUT * HZ;
+		add_timer(&task->slow_task->timer);
 
 		res = pm8001_tag_alloc(pm8001_ha, &ccb_tag);
 		if (res)
@@ -789,13 +789,13 @@ pm8001_exec_internal_task_abort(struct pm8001_hba_info *pm8001_ha,
 			pm8001_dev, flag, task_tag, ccb_tag);
 
 		if (res) {
-			del_timer(&task->timer);
+			del_timer(&task->slow_task->timer);
 			PM8001_FAIL_DBG(pm8001_ha,
 				pm8001_printk("Executing internal task "
 				"failed\n"));
 			goto ex_err;
 		}
-		wait_for_completion(&task->completion);
+		wait_for_completion(&task->slow_task->completion);
 		res = TMF_RESP_FUNC_FAILED;
 		/* Even TMF timed out, return direct. */
 		if ((task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
@@ -962,8 +962,9 @@ int pm8001_I_T_nexus_reset(struct domain_device *dev)
 	struct pm8001_device *pm8001_dev;
 	struct pm8001_hba_info *pm8001_ha;
 	struct sas_phy *phy;
+
 	if (!dev || !dev->lldd_dev)
-		return -1;
+		return -ENODEV;
 
 	pm8001_dev = dev->lldd_dev;
 	pm8001_ha = pm8001_find_ha_by_dev(dev);
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index ca508474313..a44653b4216 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -685,7 +685,7 @@ qla24xx_pci_config(scsi_qla_host_t *vha)
 		pcix_set_mmrbc(ha->pdev, 2048);
 
 	/* PCIe -- adjust Maximum Read Request Size (2048). */
-	if (pci_find_capability(ha->pdev, PCI_CAP_ID_EXP))
+	if (pci_is_pcie(ha->pdev))
 		pcie_set_readrq(ha->pdev, 2048);
 
 	pci_disable_rom(ha->pdev);
@@ -721,7 +721,7 @@ qla25xx_pci_config(scsi_qla_host_t *vha)
 	pci_write_config_word(ha->pdev, PCI_COMMAND, w);
 
 	/* PCIe -- adjust Maximum Read Request Size (2048). */
-	if (pci_find_capability(ha->pdev, PCI_CAP_ID_EXP))
+	if (pci_is_pcie(ha->pdev))
 		pcie_set_readrq(ha->pdev, 2048);
 
 	pci_disable_rom(ha->pdev);
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index caf627ba7fa..9ce3a8f8754 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -1620,7 +1620,7 @@ qla82xx_pci_info_str(struct scsi_qla_host *vha, char *str)
 	char lwstr[6];
 	uint16_t lnk;
 
-	pcie_reg = pci_find_capability(ha->pdev, PCI_CAP_ID_EXP);
+	pcie_reg = pci_pcie_cap(ha->pdev);
 	pci_read_config_word(ha->pdev, pcie_reg + PCI_EXP_LNKSTA, &lnk);
 	ha->link_width = (lnk >> 4) & 0x3f;
 
@@ -2528,7 +2528,7 @@ qla82xx_start_firmware(scsi_qla_host_t *vha)
 	}
 
 	/* Negotiated Link width */
-	pcie_cap = pci_find_capability(ha->pdev, PCI_CAP_ID_EXP);
+	pcie_cap = pci_pcie_cap(ha->pdev);
 	pci_read_config_word(ha->pdev, pcie_cap + PCI_EXP_LNKSTA, &lnk);
 	ha->link_width = (lnk >> 4) & 0x3f;
 
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index 6d1d873a20e..fb8cd3847d4 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -482,12 +482,12 @@ qla24xx_pci_info_str(struct scsi_qla_host *vha, char *str)
 	uint32_t pci_bus;
 	int pcie_reg;
 
-	pcie_reg = pci_find_capability(ha->pdev, PCI_CAP_ID_EXP);
+	pcie_reg = pci_pcie_cap(ha->pdev);
 	if (pcie_reg) {
 		char lwstr[6];
 		uint16_t pcie_lstat, lspeed, lwidth;
 
-		pcie_reg += 0x12;
+		pcie_reg += PCI_EXP_LNKCAP;
 		pci_read_config_word(ha->pdev, pcie_reg, &pcie_lstat);
 		lspeed = pcie_lstat & (BIT_0 | BIT_1 | BIT_2 | BIT_3);
 		lwidth = (pcie_lstat &
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index 77759c78cc2..5b30132960c 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -2643,19 +2643,9 @@ static void qlt_do_work(struct work_struct *work)
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	sess = ha->tgt.tgt_ops->find_sess_by_s_id(vha,
 	    atio->u.isp24.fcp_hdr.s_id);
-	if (sess) {
-		if (unlikely(sess->tearing_down)) {
-			sess = NULL;
-			spin_unlock_irqrestore(&ha->hardware_lock, flags);
-			goto out_term;
-		} else {
-			/*
-			 * Do the extra kref_get() before dropping
-			 * qla_hw_data->hardware_lock.
-			 */
-			kref_get(&sess->se_sess->sess_kref);
-		}
-	}
+	/* Do kref_get() before dropping qla_hw_data->hardware_lock. */
+	if (sess)
+		kref_get(&sess->se_sess->sess_kref);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	if (unlikely(!sess)) {
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index 9f9ef1644fd..170af157121 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -639,7 +639,7 @@ struct qla_tgt_func_tmpl {
 
 	int (*handle_cmd)(struct scsi_qla_host *, struct qla_tgt_cmd *,
 			unsigned char *, uint32_t, int, int, int);
-	int (*handle_data)(struct qla_tgt_cmd *);
+	void (*handle_data)(struct qla_tgt_cmd *);
 	int (*handle_tmr)(struct qla_tgt_mgmt_cmd *, uint32_t, uint8_t,
 			uint32_t);
 	void (*free_cmd)(struct qla_tgt_cmd *);
@@ -813,7 +813,6 @@ struct qla_tgt_sess {
 	unsigned int conf_compl_supported:1;
 	unsigned int deleted:1;
 	unsigned int local:1;
-	unsigned int tearing_down:1;
 
 	struct se_session *se_sess;
 	struct scsi_qla_host *vha;
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 6e64314dbbb..4752f65a927 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -38,8 +38,6 @@
 #include <linux/string.h>
 #include <linux/configfs.h>
 #include <linux/ctype.h>
-#include <linux/string.h>
-#include <linux/ctype.h>
 #include <asm/unaligned.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
@@ -466,8 +464,7 @@ static int tcm_qla2xxx_shutdown_session(struct se_session *se_sess)
 	vha = sess->vha;
 
 	spin_lock_irqsave(&vha->hw->hardware_lock, flags);
-	sess->tearing_down = 1;
-	target_splice_sess_cmd_list(se_sess);
+	target_sess_cmd_list_set_waiting(se_sess);
 	spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
 
 	return 1;
@@ -600,28 +597,15 @@ static int tcm_qla2xxx_handle_cmd(scsi_qla_host_t *vha, struct qla_tgt_cmd *cmd,
 		return -EINVAL;
 	}
 
-	target_submit_cmd(se_cmd, se_sess, cdb, &cmd->sense_buffer[0],
+	return target_submit_cmd(se_cmd, se_sess, cdb, &cmd->sense_buffer[0],
 				cmd->unpacked_lun, data_length, fcp_task_attr,
 				data_dir, flags);
-	return 0;
 }
 
-static void tcm_qla2xxx_do_rsp(struct work_struct *work)
+static void tcm_qla2xxx_handle_data_work(struct work_struct *work)
 {
 	struct qla_tgt_cmd *cmd = container_of(work, struct qla_tgt_cmd, work);
-	/*
-	 * Dispatch ->queue_status from workqueue process context
-	 */
-	transport_generic_request_failure(&cmd->se_cmd);
-}
 
-/*
- * Called from qla_target.c:qlt_do_ctio_completion()
- */
-static int tcm_qla2xxx_handle_data(struct qla_tgt_cmd *cmd)
-{
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	unsigned long flags;
 	/*
 	 * Ensure that the complete FCP WRITE payload has been received.
 	 * Otherwise return an exception via CHECK_CONDITION status.
@@ -631,24 +615,26 @@ static int tcm_qla2xxx_handle_data(struct qla_tgt_cmd *cmd)
 		 * Check if se_cmd has already been aborted via LUN_RESET, and
 		 * waiting upon completion in tcm_qla2xxx_write_pending_status()
 		 */
-		spin_lock_irqsave(&se_cmd->t_state_lock, flags);
-		if (se_cmd->transport_state & CMD_T_ABORTED) {
-			spin_unlock_irqrestore(&se_cmd->t_state_lock, flags);
-			complete(&se_cmd->t_transport_stop_comp);
-			return 0;
+		if (cmd->se_cmd.transport_state & CMD_T_ABORTED) {
+			complete(&cmd->se_cmd.t_transport_stop_comp);
+			return;
 		}
-		spin_unlock_irqrestore(&se_cmd->t_state_lock, flags);
 
-		se_cmd->scsi_sense_reason = TCM_CHECK_CONDITION_ABORT_CMD;
-		INIT_WORK(&cmd->work, tcm_qla2xxx_do_rsp);
-		queue_work(tcm_qla2xxx_free_wq, &cmd->work);
-		return 0;
+		cmd->se_cmd.scsi_sense_reason = TCM_CHECK_CONDITION_ABORT_CMD;
+		transport_generic_request_failure(&cmd->se_cmd);
+		return;
 	}
-	/*
-	 * We now tell TCM to queue this WRITE CDB with TRANSPORT_PROCESS_WRITE
-	 * status to the backstore processing thread.
-	 */
-	return transport_generic_handle_data(&cmd->se_cmd);
+
+	return target_execute_cmd(&cmd->se_cmd);
+}
+
+/*
+ * Called from qla_target.c:qlt_do_ctio_completion()
+ */
+static void tcm_qla2xxx_handle_data(struct qla_tgt_cmd *cmd)
+{
+	INIT_WORK(&cmd->work, tcm_qla2xxx_handle_data_work);
+	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
 }
 
 /*
@@ -1690,7 +1676,6 @@ static struct target_core_fabric_ops tcm_qla2xxx_ops = {
 	.tpg_alloc_fabric_acl		= tcm_qla2xxx_alloc_fabric_acl,
 	.tpg_release_fabric_acl		= tcm_qla2xxx_release_fabric_acl,
 	.tpg_get_inst_index		= tcm_qla2xxx_tpg_get_inst_index,
-	.new_cmd_map			= NULL,
 	.check_stop_free		= tcm_qla2xxx_check_stop_free,
 	.release_cmd			= tcm_qla2xxx_release_cmd,
 	.put_session			= tcm_qla2xxx_put_session,
diff --git a/drivers/scsi/qla4xxx/ql4_def.h b/drivers/scsi/qla4xxx/ql4_def.h
index 96a5616a8fd..7fdba7f1ffb 100644
--- a/drivers/scsi/qla4xxx/ql4_def.h
+++ b/drivers/scsi/qla4xxx/ql4_def.h
@@ -279,6 +279,7 @@ struct qla_ddb_index {
 	struct list_head list;
 	uint16_t fw_ddb_idx;
 	struct dev_db_entry fw_ddb;
+	uint8_t flash_isid[6];
 };
 
 #define DDB_IPADDR_LEN 64
diff --git a/drivers/scsi/qla4xxx/ql4_glbl.h b/drivers/scsi/qla4xxx/ql4_glbl.h
index 20b49d01904..5b2525c4139 100644
--- a/drivers/scsi/qla4xxx/ql4_glbl.h
+++ b/drivers/scsi/qla4xxx/ql4_glbl.h
@@ -183,7 +183,8 @@ int qla4xxx_flash_ddb_change(struct scsi_qla_host *ha, uint32_t fw_ddb_index,
 int qla4xxx_ddb_change(struct scsi_qla_host *ha, uint32_t fw_ddb_index,
 		       struct ddb_entry *ddb_entry, uint32_t state);
 void qla4xxx_build_ddb_list(struct scsi_qla_host *ha, int is_reset);
-int qla4xxx_post_aen_work(struct scsi_qla_host *ha, uint32_t aen_code,
+int qla4xxx_post_aen_work(struct scsi_qla_host *ha,
+			  enum iscsi_host_event_code aen_code,
 			  uint32_t data_size, uint8_t *data);
 int qla4xxx_ping_iocb(struct scsi_qla_host *ha, uint32_t options,
 		      uint32_t payload_size, uint32_t pid, uint8_t *ipaddr);
diff --git a/drivers/scsi/qla4xxx/ql4_init.c b/drivers/scsi/qla4xxx/ql4_init.c
index bf36723b84e..ddd9472066c 100644
--- a/drivers/scsi/qla4xxx/ql4_init.c
+++ b/drivers/scsi/qla4xxx/ql4_init.c
@@ -126,7 +126,7 @@ int qla4xxx_init_rings(struct scsi_qla_host *ha)
 
 	qla4xxx_init_response_q_entries(ha);
 
-	/* Initialize mabilbox active array */
+	/* Initialize mailbox active array */
 	for (i = 0; i < MAX_MRB; i++)
 		ha->active_mrb_array[i] = NULL;
 
diff --git a/drivers/scsi/qla4xxx/ql4_nx.c b/drivers/scsi/qla4xxx/ql4_nx.c
index 228b67020d2..939d7261c37 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.c
+++ b/drivers/scsi/qla4xxx/ql4_nx.c
@@ -1590,7 +1590,7 @@ qla4_8xxx_start_firmware(struct scsi_qla_host *ha, uint32_t image_start)
 	}
 
 	/* Negotiated Link width */
-	pcie_cap = pci_find_capability(ha->pdev, PCI_CAP_ID_EXP);
+	pcie_cap = pci_pcie_cap(ha->pdev);
 	pci_read_config_word(ha->pdev, pcie_cap + PCI_EXP_LNKSTA, &lnk);
 	ha->link_width = (lnk >> 4) & 0x3f;
 
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index cd15678f9ad..9da426628b9 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -4299,7 +4299,8 @@ static void qla4xxx_get_param_ddb(struct ddb_entry *ddb_entry,
 }
 
 static void qla4xxx_convert_param_ddb(struct dev_db_entry *fw_ddb_entry,
-				      struct ql4_tuple_ddb *tddb)
+				      struct ql4_tuple_ddb *tddb,
+				      uint8_t *flash_isid)
 {
 	uint16_t options = 0;
 
@@ -4314,7 +4315,12 @@ static void qla4xxx_convert_param_ddb(struct dev_db_entry *fw_ddb_entry,
 		sprintf(tddb->ip_addr, "%pI4", fw_ddb_entry->ip_addr);
 
 	tddb->port = le16_to_cpu(fw_ddb_entry->port);
-	memcpy(&tddb->isid[0], &fw_ddb_entry->isid[0], sizeof(tddb->isid));
+
+	if (flash_isid == NULL)
+		memcpy(&tddb->isid[0], &fw_ddb_entry->isid[0],
+		       sizeof(tddb->isid));
+	else
+		memcpy(&tddb->isid[0], &flash_isid[0], sizeof(tddb->isid));
 }
 
 static int qla4xxx_compare_tuple_ddb(struct scsi_qla_host *ha,
@@ -4385,7 +4391,7 @@ static int qla4xxx_is_session_exists(struct scsi_qla_host *ha,
 		goto exit_check;
 	}
 
-	qla4xxx_convert_param_ddb(fw_ddb_entry, fw_tddb);
+	qla4xxx_convert_param_ddb(fw_ddb_entry, fw_tddb, NULL);
 
 	for (idx = 0; idx < MAX_DDB_ENTRIES; idx++) {
 		ddb_entry = qla4xxx_lookup_ddb_by_fw_index(ha, idx);
@@ -4407,6 +4413,102 @@ exit_check:
 	return ret;
 }
 
+/**
+ * qla4xxx_check_existing_isid - check if target with same isid exist
+ *				 in target list
+ * @list_nt: list of target
+ * @isid: isid to check
+ *
+ * This routine return QLA_SUCCESS if target with same isid exist
+ **/
+static int qla4xxx_check_existing_isid(struct list_head *list_nt, uint8_t *isid)
+{
+	struct qla_ddb_index *nt_ddb_idx, *nt_ddb_idx_tmp;
+	struct dev_db_entry *fw_ddb_entry;
+
+	list_for_each_entry_safe(nt_ddb_idx, nt_ddb_idx_tmp, list_nt, list) {
+		fw_ddb_entry = &nt_ddb_idx->fw_ddb;
+
+		if (memcmp(&fw_ddb_entry->isid[0], &isid[0],
+			   sizeof(nt_ddb_idx->fw_ddb.isid)) == 0) {
+			return QLA_SUCCESS;
+		}
+	}
+	return QLA_ERROR;
+}
+
+/**
+ * qla4xxx_update_isid - compare ddbs and updated isid
+ * @ha: Pointer to host adapter structure.
+ * @list_nt: list of nt target
+ * @fw_ddb_entry: firmware ddb entry
+ *
+ * This routine update isid if ddbs have same iqn, same isid and
+ * different IP addr.
+ * Return QLA_SUCCESS if isid is updated.
+ **/
+static int qla4xxx_update_isid(struct scsi_qla_host *ha,
+			       struct list_head *list_nt,
+			       struct dev_db_entry *fw_ddb_entry)
+{
+	uint8_t base_value, i;
+
+	base_value = fw_ddb_entry->isid[1] & 0x1f;
+	for (i = 0; i < 8; i++) {
+		fw_ddb_entry->isid[1] = (base_value | (i << 5));
+		if (qla4xxx_check_existing_isid(list_nt, fw_ddb_entry->isid))
+			break;
+	}
+
+	if (!qla4xxx_check_existing_isid(list_nt, fw_ddb_entry->isid))
+		return QLA_ERROR;
+
+	return QLA_SUCCESS;
+}
+
+/**
+ * qla4xxx_should_update_isid - check if isid need to update
+ * @ha: Pointer to host adapter structure.
+ * @old_tddb: ddb tuple
+ * @new_tddb: ddb tuple
+ *
+ * Return QLA_SUCCESS if different IP, different PORT, same iqn,
+ * same isid
+ **/
+static int qla4xxx_should_update_isid(struct scsi_qla_host *ha,
+				      struct ql4_tuple_ddb *old_tddb,
+				      struct ql4_tuple_ddb *new_tddb)
+{
+	if (strcmp(old_tddb->ip_addr, new_tddb->ip_addr) == 0) {
+		/* Same ip */
+		if (old_tddb->port == new_tddb->port)
+			return QLA_ERROR;
+	}
+
+	if (strcmp(old_tddb->iscsi_name, new_tddb->iscsi_name))
+		/* different iqn */
+		return QLA_ERROR;
+
+	if (memcmp(&old_tddb->isid[0], &new_tddb->isid[0],
+		   sizeof(old_tddb->isid)))
+		/* different isid */
+		return QLA_ERROR;
+
+	return QLA_SUCCESS;
+}
+
+/**
+ * qla4xxx_is_flash_ddb_exists - check if fw_ddb_entry already exists in list_nt
+ * @ha: Pointer to host adapter structure.
+ * @list_nt: list of nt target.
+ * @fw_ddb_entry: firmware ddb entry.
+ *
+ * This routine check if fw_ddb_entry already exists in list_nt to avoid
+ * duplicate ddb in list_nt.
+ * Return QLA_SUCCESS if duplicate ddb exit in list_nl.
+ * Note: This function also update isid of DDB if required.
+ **/
+
 static int qla4xxx_is_flash_ddb_exists(struct scsi_qla_host *ha,
 				       struct list_head *list_nt,
 				       struct dev_db_entry *fw_ddb_entry)
@@ -4414,7 +4516,7 @@ static int qla4xxx_is_flash_ddb_exists(struct scsi_qla_host *ha,
 	struct qla_ddb_index  *nt_ddb_idx, *nt_ddb_idx_tmp;
 	struct ql4_tuple_ddb *fw_tddb = NULL;
 	struct ql4_tuple_ddb *tmp_tddb = NULL;
-	int ret = QLA_ERROR;
+	int rval, ret = QLA_ERROR;
 
 	fw_tddb = vzalloc(sizeof(*fw_tddb));
 	if (!fw_tddb) {
@@ -4432,12 +4534,28 @@ static int qla4xxx_is_flash_ddb_exists(struct scsi_qla_host *ha,
 		goto exit_check;
 	}
 
-	qla4xxx_convert_param_ddb(fw_ddb_entry, fw_tddb);
+	qla4xxx_convert_param_ddb(fw_ddb_entry, fw_tddb, NULL);
 
 	list_for_each_entry_safe(nt_ddb_idx, nt_ddb_idx_tmp, list_nt, list) {
-		qla4xxx_convert_param_ddb(&nt_ddb_idx->fw_ddb, tmp_tddb);
-		if (!qla4xxx_compare_tuple_ddb(ha, fw_tddb, tmp_tddb, true)) {
-			ret = QLA_SUCCESS; /* found */
+		qla4xxx_convert_param_ddb(&nt_ddb_idx->fw_ddb, tmp_tddb,
+					  nt_ddb_idx->flash_isid);
+		ret = qla4xxx_compare_tuple_ddb(ha, fw_tddb, tmp_tddb, true);
+		/* found duplicate ddb */
+		if (ret == QLA_SUCCESS)
+			goto exit_check;
+	}
+
+	list_for_each_entry_safe(nt_ddb_idx, nt_ddb_idx_tmp, list_nt, list) {
+		qla4xxx_convert_param_ddb(&nt_ddb_idx->fw_ddb, tmp_tddb, NULL);
+
+		ret = qla4xxx_should_update_isid(ha, tmp_tddb, fw_tddb);
+		if (ret == QLA_SUCCESS) {
+			rval = qla4xxx_update_isid(ha, list_nt, fw_ddb_entry);
+			if (rval == QLA_SUCCESS)
+				ret = QLA_ERROR;
+			else
+				ret = QLA_SUCCESS;
+
 			goto exit_check;
 		}
 	}
@@ -4788,14 +4906,26 @@ static void qla4xxx_build_nt_list(struct scsi_qla_host *ha,
 
 			nt_ddb_idx->fw_ddb_idx = idx;
 
-			memcpy(&nt_ddb_idx->fw_ddb, fw_ddb_entry,
-			       sizeof(struct dev_db_entry));
-
-			if (qla4xxx_is_flash_ddb_exists(ha, list_nt,
-					fw_ddb_entry) == QLA_SUCCESS) {
+			/* Copy original isid as it may get updated in function
+			 * qla4xxx_update_isid(). We need original isid in
+			 * function qla4xxx_compare_tuple_ddb to find duplicate
+			 * target */
+			memcpy(&nt_ddb_idx->flash_isid[0],
+			       &fw_ddb_entry->isid[0],
+			       sizeof(nt_ddb_idx->flash_isid));
+
+			ret = qla4xxx_is_flash_ddb_exists(ha, list_nt,
+							  fw_ddb_entry);
+			if (ret == QLA_SUCCESS) {
+				/* free nt_ddb_idx and do not add to list_nt */
 				vfree(nt_ddb_idx);
 				goto continue_next_nt;
 			}
+
+			/* Copy updated isid */
+			memcpy(&nt_ddb_idx->fw_ddb, fw_ddb_entry,
+			       sizeof(struct dev_db_entry));
+
 			list_add_tail(&nt_ddb_idx->list, list_nt);
 		} else if (is_reset == RESET_ADAPTER) {
 			if (qla4xxx_is_session_exists(ha, fw_ddb_entry) ==
diff --git a/drivers/scsi/qla4xxx/ql4_version.h b/drivers/scsi/qla4xxx/ql4_version.h
index cc1cc3518b8..725034f4252 100644
--- a/drivers/scsi/qla4xxx/ql4_version.h
+++ b/drivers/scsi/qla4xxx/ql4_version.h
@@ -5,4 +5,4 @@
  * See LICENSE.qla4xxx for copyright and licensing details.
  */
 
-#define QLA4XXX_DRIVER_VERSION	"5.02.00-k17"
+#define QLA4XXX_DRIVER_VERSION	"5.02.00-k18"
diff --git a/drivers/scsi/scsi.c b/drivers/scsi/scsi.c
index bbbc9c918d4..2936b447cae 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -54,6 +54,7 @@
 #include <linux/notifier.h>
 #include <linux/cpu.h>
 #include <linux/mutex.h>
+#include <linux/async.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -91,7 +92,7 @@ EXPORT_SYMBOL(scsi_logging_level);
 #endif
 
 /* sd, scsi core and power management need to coordinate flushing async actions */
-LIST_HEAD(scsi_sd_probe_domain);
+ASYNC_DOMAIN(scsi_sd_probe_domain);
 EXPORT_SYMBOL(scsi_sd_probe_domain);
 
 /* NB: These are exposed through /proc/scsi/scsi and form part of the ABI.
@@ -1354,6 +1355,7 @@ static void __exit exit_scsi(void)
 	scsi_exit_devinfo();
 	scsi_exit_procfs();
 	scsi_exit_queue();
+	async_unregister_domain(&scsi_sd_probe_domain);
 }
 
 subsys_initcall(init_scsi);
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index d0f71e5d065..4a6381c8725 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -1687,6 +1687,20 @@ static void scsi_restart_operations(struct Scsi_Host *shost)
 	 * requests are started.
 	 */
 	scsi_run_host_queues(shost);
+
+	/*
+	 * if eh is active and host_eh_scheduled is pending we need to re-run
+	 * recovery.  we do this check after scsi_run_host_queues() to allow
+	 * everything pent up since the last eh run a chance to make forward
+	 * progress before we sync again.  Either we'll immediately re-run
+	 * recovery or scsi_device_unbusy() will wake us again when these
+	 * pending commands complete.
+	 */
+	spin_lock_irqsave(shost->host_lock, flags);
+	if (shost->host_eh_scheduled)
+		if (scsi_host_set_state(shost, SHOST_RECOVERY))
+			WARN_ON(scsi_host_set_state(shost, SHOST_CANCEL_RECOVERY));
+	spin_unlock_irqrestore(shost->host_lock, flags);
 }
 
 /**
@@ -1804,15 +1818,14 @@ int scsi_error_handler(void *data)
 	 * We never actually get interrupted because kthread_run
 	 * disables signal delivery for the created thread.
 	 */
-	set_current_state(TASK_INTERRUPTIBLE);
 	while (!kthread_should_stop()) {
+		set_current_state(TASK_INTERRUPTIBLE);
 		if ((shost->host_failed == 0 && shost->host_eh_scheduled == 0) ||
 		    shost->host_failed != shost->host_busy) {
 			SCSI_LOG_ERROR_RECOVERY(1,
 				printk("Error handler scsi_eh_%d sleeping\n",
 					shost->host_no));
 			schedule();
-			set_current_state(TASK_INTERRUPTIBLE);
 			continue;
 		}
 
@@ -1849,7 +1862,6 @@ int scsi_error_handler(void *data)
 		scsi_restart_operations(shost);
 		if (!shost->eh_noresume)
 			scsi_autopm_put_host(shost);
-		set_current_state(TASK_INTERRUPTIBLE);
 	}
 	__set_current_state(TASK_RUNNING);
 
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 6dfb9785d34..ffd77739ae3 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -68,6 +68,23 @@ static struct scsi_host_sg_pool scsi_sg_pools[] = {
 
 struct kmem_cache *scsi_sdb_cache;
 
+#ifdef CONFIG_ACPI
+#include <acpi/acpi_bus.h>
+
+int scsi_register_acpi_bus_type(struct acpi_bus_type *bus)
+{
+        bus->bus = &scsi_bus_type;
+        return register_acpi_bus_type(bus);
+}
+EXPORT_SYMBOL_GPL(scsi_register_acpi_bus_type);
+
+void scsi_unregister_acpi_bus_type(struct acpi_bus_type *bus)
+{
+	unregister_acpi_bus_type(bus);
+}
+EXPORT_SYMBOL_GPL(scsi_unregister_acpi_bus_type);
+#endif
+
 /*
  * When to reinvoke queueing after a resource shortage. It's 3 msecs to
  * not change behaviour from the previous unplug mechanism, experimentation
@@ -109,7 +126,7 @@ static void scsi_unprep_request(struct request *req)
  * for a requeue after completion, which should only occur in this
  * file.
  */
-static int __scsi_queue_insert(struct scsi_cmnd *cmd, int reason, int unbusy)
+static void __scsi_queue_insert(struct scsi_cmnd *cmd, int reason, int unbusy)
 {
 	struct Scsi_Host *host = cmd->device->host;
 	struct scsi_device *device = cmd->device;
@@ -155,15 +172,14 @@ static int __scsi_queue_insert(struct scsi_cmnd *cmd, int reason, int unbusy)
 
 	/*
 	 * Requeue this command.  It will go before all other commands
-	 * that are already in the queue.
+	 * that are already in the queue. Schedule requeue work under
+	 * lock such that the kblockd_schedule_work() call happens
+	 * before blk_cleanup_queue() finishes.
 	 */
 	spin_lock_irqsave(q->queue_lock, flags);
 	blk_requeue_request(q, cmd->request);
-	spin_unlock_irqrestore(q->queue_lock, flags);
-
 	kblockd_schedule_work(q, &device->requeue_work);
-
-	return 0;
+	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
 /*
@@ -185,9 +201,9 @@ static int __scsi_queue_insert(struct scsi_cmnd *cmd, int reason, int unbusy)
  * Notes:       This could be called either from an interrupt context or a
  *              normal process context.
  */
-int scsi_queue_insert(struct scsi_cmnd *cmd, int reason)
+void scsi_queue_insert(struct scsi_cmnd *cmd, int reason)
 {
-	return __scsi_queue_insert(cmd, reason, 1);
+	__scsi_queue_insert(cmd, reason, 1);
 }
 /**
  * scsi_execute - insert request and wait for the result
@@ -406,10 +422,6 @@ static void scsi_run_queue(struct request_queue *q)
 	LIST_HEAD(starved_list);
 	unsigned long flags;
 
-	/* if the device is dead, sdev will be NULL, so no queue to run */
-	if (!sdev)
-		return;
-
 	shost = sdev->host;
 	if (scsi_target(sdev)->single_lun)
 		scsi_single_lun_run(sdev);
@@ -483,15 +495,26 @@ void scsi_requeue_run_queue(struct work_struct *work)
  */
 static void scsi_requeue_command(struct request_queue *q, struct scsi_cmnd *cmd)
 {
+	struct scsi_device *sdev = cmd->device;
 	struct request *req = cmd->request;
 	unsigned long flags;
 
+	/*
+	 * We need to hold a reference on the device to avoid the queue being
+	 * killed after the unlock and before scsi_run_queue is invoked which
+	 * may happen because scsi_unprep_request() puts the command which
+	 * releases its reference on the device.
+	 */
+	get_device(&sdev->sdev_gendev);
+
 	spin_lock_irqsave(q->queue_lock, flags);
 	scsi_unprep_request(req);
 	blk_requeue_request(q, req);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 
 	scsi_run_queue(q);
+
+	put_device(&sdev->sdev_gendev);
 }
 
 void scsi_next_command(struct scsi_cmnd *cmd)
@@ -1173,6 +1196,7 @@ int scsi_prep_state_check(struct scsi_device *sdev, struct request *req)
 	if (unlikely(sdev->sdev_state != SDEV_RUNNING)) {
 		switch (sdev->sdev_state) {
 		case SDEV_OFFLINE:
+		case SDEV_TRANSPORT_OFFLINE:
 			/*
 			 * If the device is offline we refuse to process any
 			 * commands.  The device must be brought online
@@ -1370,16 +1394,16 @@ static inline int scsi_host_queue_ready(struct request_queue *q,
  * may be changed after request stacking drivers call the function,
  * regardless of taking lock or not.
  *
- * When scsi can't dispatch I/Os anymore and needs to kill I/Os
- * (e.g. !sdev), scsi needs to return 'not busy'.
- * Otherwise, request stacking drivers may hold requests forever.
+ * When scsi can't dispatch I/Os anymore and needs to kill I/Os scsi
+ * needs to return 'not busy'. Otherwise, request stacking drivers
+ * may hold requests forever.
  */
 static int scsi_lld_busy(struct request_queue *q)
 {
 	struct scsi_device *sdev = q->queuedata;
 	struct Scsi_Host *shost;
 
-	if (!sdev)
+	if (blk_queue_dead(q))
 		return 0;
 
 	shost = sdev->host;
@@ -1490,12 +1514,6 @@ static void scsi_request_fn(struct request_queue *q)
 	struct scsi_cmnd *cmd;
 	struct request *req;
 
-	if (!sdev) {
-		while ((req = blk_peek_request(q)) != NULL)
-			scsi_kill_request(req, q);
-		return;
-	}
-
 	if(!get_device(&sdev->sdev_gendev))
 		/* We must be tearing the block queue down already */
 		return;
@@ -1697,20 +1715,6 @@ struct request_queue *scsi_alloc_queue(struct scsi_device *sdev)
 	return q;
 }
 
-void scsi_free_queue(struct request_queue *q)
-{
-	unsigned long flags;
-
-	WARN_ON(q->queuedata);
-
-	/* cause scsi_request_fn() to kill all non-finished requests */
-	spin_lock_irqsave(q->queue_lock, flags);
-	q->request_fn(q);
-	spin_unlock_irqrestore(q->queue_lock, flags);
-
-	blk_cleanup_queue(q);
-}
-
 /*
  * Function:    scsi_block_requests()
  *
@@ -2081,6 +2085,7 @@ scsi_device_set_state(struct scsi_device *sdev, enum scsi_device_state state)
 		switch (oldstate) {
 		case SDEV_CREATED:
 		case SDEV_OFFLINE:
+		case SDEV_TRANSPORT_OFFLINE:
 		case SDEV_QUIESCE:
 		case SDEV_BLOCK:
 			break;
@@ -2093,6 +2098,7 @@ scsi_device_set_state(struct scsi_device *sdev, enum scsi_device_state state)
 		switch (oldstate) {
 		case SDEV_RUNNING:
 		case SDEV_OFFLINE:
+		case SDEV_TRANSPORT_OFFLINE:
 			break;
 		default:
 			goto illegal;
@@ -2100,6 +2106,7 @@ scsi_device_set_state(struct scsi_device *sdev, enum scsi_device_state state)
 		break;
 
 	case SDEV_OFFLINE:
+	case SDEV_TRANSPORT_OFFLINE:
 		switch (oldstate) {
 		case SDEV_CREATED:
 		case SDEV_RUNNING:
@@ -2136,6 +2143,7 @@ scsi_device_set_state(struct scsi_device *sdev, enum scsi_device_state state)
 		case SDEV_RUNNING:
 		case SDEV_QUIESCE:
 		case SDEV_OFFLINE:
+		case SDEV_TRANSPORT_OFFLINE:
 		case SDEV_BLOCK:
 			break;
 		default:
@@ -2148,6 +2156,7 @@ scsi_device_set_state(struct scsi_device *sdev, enum scsi_device_state state)
 		case SDEV_CREATED:
 		case SDEV_RUNNING:
 		case SDEV_OFFLINE:
+		case SDEV_TRANSPORT_OFFLINE:
 		case SDEV_CANCEL:
 			break;
 		default:
@@ -2405,7 +2414,6 @@ EXPORT_SYMBOL(scsi_target_resume);
  *	(which must be a legal transition).  When the device is in this
  *	state, all commands are deferred until the scsi lld reenables
  *	the device with scsi_device_unblock or device_block_tmo fires.
- *	This routine assumes the host_lock is held on entry.
  */
 int
 scsi_internal_device_block(struct scsi_device *sdev)
@@ -2438,6 +2446,7 @@ EXPORT_SYMBOL_GPL(scsi_internal_device_block);
 /**
  * scsi_internal_device_unblock - resume a device after a block request
  * @sdev:	device to resume
+ * @new_state:	state to set devices to after unblocking
  *
  * Called by scsi lld's or the midlayer to restart the device queue
  * for the previously suspended scsi device.  Called from interrupt or
@@ -2447,25 +2456,29 @@ EXPORT_SYMBOL_GPL(scsi_internal_device_block);
  *
  * Notes:       
  *	This routine transitions the device to the SDEV_RUNNING state
- *	(which must be a legal transition) allowing the midlayer to
- *	goose the queue for this device.  This routine assumes the 
- *	host_lock is held upon entry.
+ *	or to one of the offline states (which must be a legal transition)
+ *	allowing the midlayer to goose the queue for this device.
  */
 int
-scsi_internal_device_unblock(struct scsi_device *sdev)
+scsi_internal_device_unblock(struct scsi_device *sdev,
+			     enum scsi_device_state new_state)
 {
 	struct request_queue *q = sdev->request_queue; 
 	unsigned long flags;
-	
-	/* 
-	 * Try to transition the scsi device to SDEV_RUNNING
-	 * and goose the device queue if successful.  
+
+	/*
+	 * Try to transition the scsi device to SDEV_RUNNING or one of the
+	 * offlined states and goose the device queue if successful.
 	 */
 	if (sdev->sdev_state == SDEV_BLOCK)
-		sdev->sdev_state = SDEV_RUNNING;
-	else if (sdev->sdev_state == SDEV_CREATED_BLOCK)
-		sdev->sdev_state = SDEV_CREATED;
-	else if (sdev->sdev_state != SDEV_CANCEL &&
+		sdev->sdev_state = new_state;
+	else if (sdev->sdev_state == SDEV_CREATED_BLOCK) {
+		if (new_state == SDEV_TRANSPORT_OFFLINE ||
+		    new_state == SDEV_OFFLINE)
+			sdev->sdev_state = new_state;
+		else
+			sdev->sdev_state = SDEV_CREATED;
+	} else if (sdev->sdev_state != SDEV_CANCEL &&
 		 sdev->sdev_state != SDEV_OFFLINE)
 		return -EINVAL;
 
@@ -2506,26 +2519,26 @@ EXPORT_SYMBOL_GPL(scsi_target_block);
 static void
 device_unblock(struct scsi_device *sdev, void *data)
 {
-	scsi_internal_device_unblock(sdev);
+	scsi_internal_device_unblock(sdev, *(enum scsi_device_state *)data);
 }
 
 static int
 target_unblock(struct device *dev, void *data)
 {
 	if (scsi_is_target_device(dev))
-		starget_for_each_device(to_scsi_target(dev), NULL,
+		starget_for_each_device(to_scsi_target(dev), data,
 					device_unblock);
 	return 0;
 }
 
 void
-scsi_target_unblock(struct device *dev)
+scsi_target_unblock(struct device *dev, enum scsi_device_state new_state)
 {
 	if (scsi_is_target_device(dev))
-		starget_for_each_device(to_scsi_target(dev), NULL,
+		starget_for_each_device(to_scsi_target(dev), &new_state,
 					device_unblock);
 	else
-		device_for_each_child(dev, NULL, target_unblock);
+		device_for_each_child(dev, &new_state, target_unblock);
 }
 EXPORT_SYMBOL_GPL(scsi_target_unblock);
 
diff --git a/drivers/scsi/scsi_netlink.c b/drivers/scsi/scsi_netlink.c
index c77628afbf9..8818dd681c1 100644
--- a/drivers/scsi/scsi_netlink.c
+++ b/drivers/scsi/scsi_netlink.c
@@ -486,6 +486,10 @@ void
 scsi_netlink_init(void)
 {
 	int error;
+	struct netlink_kernel_cfg cfg = {
+		.input	= scsi_nl_rcv_msg,
+		.groups	= SCSI_NL_GRP_CNT,
+	};
 
 	INIT_LIST_HEAD(&scsi_nl_drivers);
 
@@ -497,8 +501,7 @@ scsi_netlink_init(void)
 	}
 
 	scsi_nl_sock = netlink_kernel_create(&init_net, NETLINK_SCSITRANSPORT,
-				SCSI_NL_GRP_CNT, scsi_nl_rcv_msg, NULL,
-				THIS_MODULE);
+					     THIS_MODULE, &cfg);
 	if (!scsi_nl_sock) {
 		printk(KERN_ERR "%s: register of receive handler failed\n",
 				__func__);
diff --git a/drivers/scsi/scsi_pm.c b/drivers/scsi/scsi_pm.c
index d4201ded3b2..dc0ad85853e 100644
--- a/drivers/scsi/scsi_pm.c
+++ b/drivers/scsi/scsi_pm.c
@@ -76,23 +76,24 @@ static int scsi_bus_resume_common(struct device *dev)
 {
 	int err = 0;
 
-	if (scsi_is_sdev_device(dev)) {
-		/*
-		 * Parent device may have runtime suspended as soon as
-		 * it is woken up during the system resume.
-		 *
-		 * Resume it on behalf of child.
-		 */
-		pm_runtime_get_sync(dev->parent);
-		err = scsi_dev_type_resume(dev);
-		pm_runtime_put_sync(dev->parent);
-	}
+	/*
+	 * Parent device may have runtime suspended as soon as
+	 * it is woken up during the system resume.
+	 *
+	 * Resume it on behalf of child.
+	 */
+	pm_runtime_get_sync(dev->parent);
 
+	if (scsi_is_sdev_device(dev))
+		err = scsi_dev_type_resume(dev);
 	if (err == 0) {
 		pm_runtime_disable(dev);
 		pm_runtime_set_active(dev);
 		pm_runtime_enable(dev);
 	}
+
+	pm_runtime_put_sync(dev->parent);
+
 	return err;
 }
 
diff --git a/drivers/scsi/scsi_priv.h b/drivers/scsi/scsi_priv.h
index 07ce3f51701..8f9a0cadc29 100644
--- a/drivers/scsi/scsi_priv.h
+++ b/drivers/scsi/scsi_priv.h
@@ -2,6 +2,8 @@
 #define _SCSI_PRIV_H
 
 #include <linux/device.h>
+#include <linux/async.h>
+#include <scsi/scsi_device.h>
 
 struct request_queue;
 struct request;
@@ -79,12 +81,11 @@ int scsi_noretry_cmd(struct scsi_cmnd *scmd);
 /* scsi_lib.c */
 extern int scsi_maybe_unblock_host(struct scsi_device *sdev);
 extern void scsi_device_unbusy(struct scsi_device *sdev);
-extern int scsi_queue_insert(struct scsi_cmnd *cmd, int reason);
+extern void scsi_queue_insert(struct scsi_cmnd *cmd, int reason);
 extern void scsi_next_command(struct scsi_cmnd *cmd);
 extern void scsi_io_completion(struct scsi_cmnd *, unsigned int);
 extern void scsi_run_host_queues(struct Scsi_Host *shost);
 extern struct request_queue *scsi_alloc_queue(struct scsi_device *sdev);
-extern void scsi_free_queue(struct request_queue *q);
 extern int scsi_init_queue(void);
 extern void scsi_exit_queue(void);
 struct request_queue;
@@ -163,7 +164,7 @@ static inline int scsi_autopm_get_host(struct Scsi_Host *h) { return 0; }
 static inline void scsi_autopm_put_host(struct Scsi_Host *h) {}
 #endif /* CONFIG_PM_RUNTIME */
 
-extern struct list_head scsi_sd_probe_domain;
+extern struct async_domain scsi_sd_probe_domain;
 
 /* 
  * internal scsi timeout functions: for use by mid-layer and transport
@@ -172,6 +173,7 @@ extern struct list_head scsi_sd_probe_domain;
 
 #define SCSI_DEVICE_BLOCK_MAX_TIMEOUT	600	/* units in seconds */
 extern int scsi_internal_device_block(struct scsi_device *sdev);
-extern int scsi_internal_device_unblock(struct scsi_device *sdev);
+extern int scsi_internal_device_unblock(struct scsi_device *sdev,
+					enum scsi_device_state new_state);
 
 #endif /* _SCSI_PRIV_H */
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 2e5fe584aad..56a93794c47 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -147,7 +147,7 @@ int scsi_complete_async_scans(void)
 
 	do {
 		if (list_empty(&scanning_hosts))
-			goto out;
+			return 0;
 		/* If we can't get memory immediately, that's OK.  Just
 		 * sleep a little.  Even if we never get memory, the async
 		 * scans will finish eventually.
@@ -179,26 +179,11 @@ int scsi_complete_async_scans(void)
 	}
  done:
 	spin_unlock(&async_scan_lock);
-	kfree(data);
-
- out:
-	async_synchronize_full_domain(&scsi_sd_probe_domain);
 
+	kfree(data);
 	return 0;
 }
 
-/* Only exported for the benefit of scsi_wait_scan */
-EXPORT_SYMBOL_GPL(scsi_complete_async_scans);
-
-#ifndef MODULE
-/*
- * For async scanning we need to wait for all the scans to complete before
- * trying to mount the root fs.  Otherwise non-modular drivers may not be ready
- * yet.
- */
-late_initcall(scsi_complete_async_scans);
-#endif
-
 /**
  * scsi_unlock_floptical - unlock device via a special MODE SENSE command
  * @sdev:	scsi device to send command to
@@ -1717,6 +1702,9 @@ static void scsi_sysfs_add_devices(struct Scsi_Host *shost)
 {
 	struct scsi_device *sdev;
 	shost_for_each_device(sdev, shost) {
+		/* target removed before the device could be added */
+		if (sdev->sdev_state == SDEV_DEL)
+			continue;
 		if (!scsi_host_scan_allowed(shost) ||
 		    scsi_sysfs_add_sdev(sdev) != 0)
 			__scsi_remove_device(sdev);
@@ -1842,14 +1830,13 @@ static void do_scsi_scan_host(struct Scsi_Host *shost)
 	}
 }
 
-static int do_scan_async(void *_data)
+static void do_scan_async(void *_data, async_cookie_t c)
 {
 	struct async_scan_data *data = _data;
 	struct Scsi_Host *shost = data->shost;
 
 	do_scsi_scan_host(shost);
 	scsi_finish_async_scan(data);
-	return 0;
 }
 
 /**
@@ -1858,7 +1845,6 @@ static int do_scan_async(void *_data)
  **/
 void scsi_scan_host(struct Scsi_Host *shost)
 {
-	struct task_struct *p;
 	struct async_scan_data *data;
 
 	if (strncmp(scsi_scan_type, "none", 4) == 0)
@@ -1873,9 +1859,11 @@ void scsi_scan_host(struct Scsi_Host *shost)
 		return;
 	}
 
-	p = kthread_run(do_scan_async, data, "scsi_scan_%d", shost->host_no);
-	if (IS_ERR(p))
-		do_scan_async(data);
+	/* register with the async subsystem so wait_for_device_probe()
+	 * will flush this work
+	 */
+	async_schedule(do_scan_async, data);
+
 	/* scsi_autopm_put_host(shost) is called in scsi_finish_async_scan() */
 }
 EXPORT_SYMBOL(scsi_scan_host);
diff --git a/drivers/scsi/scsi_sysfs.c b/drivers/scsi/scsi_sysfs.c
index 04c2a278076..093d4f6a54d 100644
--- a/drivers/scsi/scsi_sysfs.c
+++ b/drivers/scsi/scsi_sysfs.c
@@ -35,6 +35,7 @@ static const struct {
 	{ SDEV_DEL, "deleted" },
 	{ SDEV_QUIESCE, "quiesce" },
 	{ SDEV_OFFLINE,	"offline" },
+	{ SDEV_TRANSPORT_OFFLINE, "transport-offline" },
 	{ SDEV_BLOCK,	"blocked" },
 	{ SDEV_CREATED_BLOCK, "created-blocked" },
 };
@@ -966,16 +967,20 @@ void __scsi_remove_device(struct scsi_device *sdev)
 		device_del(dev);
 	} else
 		put_device(&sdev->sdev_dev);
+
+	/*
+	 * Stop accepting new requests and wait until all queuecommand() and
+	 * scsi_run_queue() invocations have finished before tearing down the
+	 * device.
+	 */
 	scsi_device_set_state(sdev, SDEV_DEL);
+	blk_cleanup_queue(sdev->request_queue);
+	cancel_work_sync(&sdev->requeue_work);
+
 	if (sdev->host->hostt->slave_destroy)
 		sdev->host->hostt->slave_destroy(sdev);
 	transport_destroy_device(dev);
 
-	/* cause the request function to reject all I/O requests */
-	sdev->request_queue->queuedata = NULL;
-
-	/* Freeing the queue signals to block that we're done */
-	scsi_free_queue(sdev->request_queue);
 	put_device(dev);
 }
 
@@ -1000,7 +1005,6 @@ static void __scsi_remove_target(struct scsi_target *starget)
 	struct scsi_device *sdev;
 
 	spin_lock_irqsave(shost->host_lock, flags);
-	starget->reap_ref++;
  restart:
 	list_for_each_entry(sdev, &shost->__devices, siblings) {
 		if (sdev->channel != starget->channel ||
@@ -1014,14 +1018,6 @@ static void __scsi_remove_target(struct scsi_target *starget)
 		goto restart;
 	}
 	spin_unlock_irqrestore(shost->host_lock, flags);
-	scsi_target_reap(starget);
-}
-
-static int __remove_child (struct device * dev, void * data)
-{
-	if (scsi_is_target_device(dev))
-		__scsi_remove_target(to_scsi_target(dev));
-	return 0;
 }
 
 /**
@@ -1034,14 +1030,34 @@ static int __remove_child (struct device * dev, void * data)
  */
 void scsi_remove_target(struct device *dev)
 {
-	if (scsi_is_target_device(dev)) {
-		__scsi_remove_target(to_scsi_target(dev));
-		return;
+	struct Scsi_Host *shost = dev_to_shost(dev->parent);
+	struct scsi_target *starget, *found;
+	unsigned long flags;
+
+ restart:
+	found = NULL;
+	spin_lock_irqsave(shost->host_lock, flags);
+	list_for_each_entry(starget, &shost->__targets, siblings) {
+		if (starget->state == STARGET_DEL)
+			continue;
+		if (starget->dev.parent == dev || &starget->dev == dev) {
+			found = starget;
+			found->reap_ref++;
+			break;
+		}
 	}
+	spin_unlock_irqrestore(shost->host_lock, flags);
 
-	get_device(dev);
-	device_for_each_child(dev, NULL, __remove_child);
-	put_device(dev);
+	if (found) {
+		__scsi_remove_target(found);
+		scsi_target_reap(found);
+		/* in the case where @dev has multiple starget children,
+		 * continue removing.
+		 *
+		 * FIXME: does such a case exist?
+		 */
+		goto restart;
+	}
 }
 EXPORT_SYMBOL(scsi_remove_target);
 
diff --git a/drivers/scsi/scsi_transport_fc.c b/drivers/scsi/scsi_transport_fc.c
index 579760420d5..e894ca7b54c 100644
--- a/drivers/scsi/scsi_transport_fc.c
+++ b/drivers/scsi/scsi_transport_fc.c
@@ -1744,6 +1744,15 @@ fc_host_statistic(fcp_output_requests);
 fc_host_statistic(fcp_control_requests);
 fc_host_statistic(fcp_input_megabytes);
 fc_host_statistic(fcp_output_megabytes);
+fc_host_statistic(fcp_packet_alloc_failures);
+fc_host_statistic(fcp_packet_aborts);
+fc_host_statistic(fcp_frame_alloc_failures);
+fc_host_statistic(fc_no_free_exch);
+fc_host_statistic(fc_no_free_exch_xid);
+fc_host_statistic(fc_xid_not_found);
+fc_host_statistic(fc_xid_busy);
+fc_host_statistic(fc_seq_not_found);
+fc_host_statistic(fc_non_bls_resp);
 
 static ssize_t
 fc_reset_statistics(struct device *dev, struct device_attribute *attr,
@@ -1784,6 +1793,15 @@ static struct attribute *fc_statistics_attrs[] = {
 	&device_attr_host_fcp_control_requests.attr,
 	&device_attr_host_fcp_input_megabytes.attr,
 	&device_attr_host_fcp_output_megabytes.attr,
+	&device_attr_host_fcp_packet_alloc_failures.attr,
+	&device_attr_host_fcp_packet_aborts.attr,
+	&device_attr_host_fcp_frame_alloc_failures.attr,
+	&device_attr_host_fc_no_free_exch.attr,
+	&device_attr_host_fc_no_free_exch_xid.attr,
+	&device_attr_host_fc_xid_not_found.attr,
+	&device_attr_host_fc_xid_busy.attr,
+	&device_attr_host_fc_seq_not_found.attr,
+	&device_attr_host_fc_non_bls_resp.attr,
 	&device_attr_host_reset_statistics.attr,
 	NULL
 };
@@ -2477,11 +2495,9 @@ static void fc_terminate_rport_io(struct fc_rport *rport)
 		i->f->terminate_rport_io(rport);
 
 	/*
-	 * must unblock to flush queued IO. The caller will have set
-	 * the port_state or flags, so that fc_remote_port_chkready will
-	 * fail IO.
+	 * Must unblock to flush queued IO. scsi-ml will fail incoming reqs.
 	 */
-	scsi_target_unblock(&rport->dev);
+	scsi_target_unblock(&rport->dev, SDEV_TRANSPORT_OFFLINE);
 }
 
 /**
@@ -2812,8 +2828,8 @@ fc_remote_port_add(struct Scsi_Host *shost, int channel,
 
 				/* if target, initiate a scan */
 				if (rport->scsi_target_id != -1) {
-					scsi_target_unblock(&rport->dev);
-
+					scsi_target_unblock(&rport->dev,
+							    SDEV_RUNNING);
 					spin_lock_irqsave(shost->host_lock,
 							  flags);
 					rport->flags |= FC_RPORT_SCAN_PENDING;
@@ -2882,7 +2898,7 @@ fc_remote_port_add(struct Scsi_Host *shost, int channel,
 			spin_unlock_irqrestore(shost->host_lock, flags);
 
 			if (ids->roles & FC_PORT_ROLE_FCP_TARGET) {
-				scsi_target_unblock(&rport->dev);
+				scsi_target_unblock(&rport->dev, SDEV_RUNNING);
 
 				/* initiate a scan of the target */
 				spin_lock_irqsave(shost->host_lock, flags);
@@ -3087,7 +3103,7 @@ fc_remote_port_rolechg(struct fc_rport  *rport, u32 roles)
 		/* ensure any stgt delete functions are done */
 		fc_flush_work(shost);
 
-		scsi_target_unblock(&rport->dev);
+		scsi_target_unblock(&rport->dev, SDEV_RUNNING);
 		/* initiate a scan of the target */
 		spin_lock_irqsave(shost->host_lock, flags);
 		rport->flags |= FC_RPORT_SCAN_PENDING;
@@ -3131,7 +3147,7 @@ fc_timeout_deleted_rport(struct work_struct *work)
 			"blocked FC remote port time out: no longer"
 			" a FCP target, removing starget\n");
 		spin_unlock_irqrestore(shost->host_lock, flags);
-		scsi_target_unblock(&rport->dev);
+		scsi_target_unblock(&rport->dev, SDEV_TRANSPORT_OFFLINE);
 		fc_queue_work(shost, &rport->stgt_delete_work);
 		return;
 	}
@@ -4130,45 +4146,7 @@ fc_bsg_rportadd(struct Scsi_Host *shost, struct fc_rport *rport)
 static void
 fc_bsg_remove(struct request_queue *q)
 {
-	struct request *req; /* block request */
-	int counts; /* totals for request_list count and starved */
-
 	if (q) {
-		/* Stop taking in new requests */
-		spin_lock_irq(q->queue_lock);
-		blk_stop_queue(q);
-
-		/* drain all requests in the queue */
-		while (1) {
-			/* need the lock to fetch a request
-			 * this may fetch the same reqeust as the previous pass
-			 */
-			req = blk_fetch_request(q);
-			/* save requests in use and starved */
-			counts = q->rq.count[0] + q->rq.count[1] +
-				q->rq.starved[0] + q->rq.starved[1];
-			spin_unlock_irq(q->queue_lock);
-			/* any requests still outstanding? */
-			if (counts == 0)
-				break;
-
-			/* This may be the same req as the previous iteration,
-			 * always send the blk_end_request_all after a prefetch.
-			 * It is not okay to not end the request because the
-			 * prefetch started the request.
-			 */
-			if (req) {
-				/* return -ENXIO to indicate that this queue is
-				 * going away
-				 */
-				req->errors = -ENXIO;
-				blk_end_request_all(req, -ENXIO);
-			}
-
-			msleep(200); /* allow bsg to possibly finish */
-			spin_lock_irq(q->queue_lock);
-		}
-
 		bsg_unregister_queue(q);
 		blk_cleanup_queue(q);
 	}
diff --git a/drivers/scsi/scsi_transport_iscsi.c b/drivers/scsi/scsi_transport_iscsi.c
index 1cf640e575d..fa1dfaa83e3 100644
--- a/drivers/scsi/scsi_transport_iscsi.c
+++ b/drivers/scsi/scsi_transport_iscsi.c
@@ -575,7 +575,7 @@ static int iscsi_remove_host(struct transport_container *tc,
 	struct iscsi_cls_host *ihost = shost->shost_data;
 
 	if (ihost->bsg_q) {
-		bsg_remove_queue(ihost->bsg_q);
+		bsg_unregister_queue(ihost->bsg_q);
 		blk_cleanup_queue(ihost->bsg_q);
 	}
 	return 0;
@@ -907,7 +907,7 @@ static void session_recovery_timedout(struct work_struct *work)
 		session->transport->session_recovery_timedout(session);
 
 	ISCSI_DBG_TRANS_SESSION(session, "Unblocking SCSI target\n");
-	scsi_target_unblock(&session->dev);
+	scsi_target_unblock(&session->dev, SDEV_TRANSPORT_OFFLINE);
 	ISCSI_DBG_TRANS_SESSION(session, "Completed unblocking SCSI target\n");
 }
 
@@ -930,7 +930,7 @@ static void __iscsi_unblock_session(struct work_struct *work)
 	session->state = ISCSI_SESSION_LOGGED_IN;
 	spin_unlock_irqrestore(&session->lock, flags);
 	/* start IO */
-	scsi_target_unblock(&session->dev);
+	scsi_target_unblock(&session->dev, SDEV_RUNNING);
 	/*
 	 * Only do kernel scanning if the driver is properly hooked into
 	 * the async scanning code (drivers like iscsi_tcp do login and
@@ -1180,7 +1180,7 @@ void iscsi_remove_session(struct iscsi_cls_session *session)
 	session->state = ISCSI_SESSION_FREE;
 	spin_unlock_irqrestore(&session->lock, flags);
 
-	scsi_target_unblock(&session->dev);
+	scsi_target_unblock(&session->dev, SDEV_TRANSPORT_OFFLINE);
 	/* flush running scans then delete devices */
 	scsi_flush_work(shost);
 	__iscsi_unbind_session(&session->unbind_work);
@@ -2936,7 +2936,10 @@ EXPORT_SYMBOL_GPL(iscsi_unregister_transport);
 static __init int iscsi_transport_init(void)
 {
 	int err;
-
+	struct netlink_kernel_cfg cfg = {
+		.groups	= 1,
+		.input	= iscsi_if_rx,
+	};
 	printk(KERN_INFO "Loading iSCSI transport class v%s.\n",
 		ISCSI_TRANSPORT_VERSION);
 
@@ -2966,8 +2969,8 @@ static __init int iscsi_transport_init(void)
 	if (err)
 		goto unregister_conn_class;
 
-	nls = netlink_kernel_create(&init_net, NETLINK_ISCSI, 1, iscsi_if_rx,
-				    NULL, THIS_MODULE);
+	nls = netlink_kernel_create(&init_net, NETLINK_ISCSI,
+				    THIS_MODULE, &cfg);
 	if (!nls) {
 		err = -ENOBUFS;
 		goto unregister_session_class;
diff --git a/drivers/scsi/scsi_wait_scan.c b/drivers/scsi/scsi_wait_scan.c
deleted file mode 100644
index ae781487461..00000000000
--- a/drivers/scsi/scsi_wait_scan.c
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * scsi_wait_scan.c
- *
- * Copyright (C) 2006 James Bottomley <James.Bottomley@SteelEye.com>
- *
- * This is a simple module to wait until all the async scans are
- * complete.  The idea is to use it in initrd/initramfs scripts.  You
- * modprobe it after all the modprobes of the root SCSI drivers and it
- * will wait until they have all finished scanning their busses before
- * allowing the boot to proceed
- */
-
-#include <linux/module.h>
-#include <linux/device.h>
-#include "scsi_priv.h"
-
-static int __init wait_scan_init(void)
-{
-	/*
-	 * First we need to wait for device probing to finish;
-	 * the drivers we just loaded might just still be probing
-	 * and might not yet have reached the scsi async scanning
-	 */
-	wait_for_device_probe();
-	/*
-	 * and then we wait for the actual asynchronous scsi scan
-	 * to finish.
-	 */
-	scsi_complete_async_scans();
-	return 0;
-}
-
-static void __exit wait_scan_exit(void)
-{
-}
-
-MODULE_DESCRIPTION("SCSI wait for scans");
-MODULE_AUTHOR("James Bottomley");
-MODULE_LICENSE("GPL");
-
-late_initcall(wait_scan_init);
-module_exit(wait_scan_exit);
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 6f72b80121a..4df73e52a4f 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -2261,8 +2261,13 @@ bad_sense:
 		sd_printk(KERN_ERR, sdkp, "Asking for cache data failed\n");
 
 defaults:
-	sd_printk(KERN_ERR, sdkp, "Assuming drive cache: write through\n");
-	sdkp->WCE = 0;
+	if (sdp->wce_default_on) {
+		sd_printk(KERN_NOTICE, sdkp, "Assuming drive cache: write back\n");
+		sdkp->WCE = 1;
+	} else {
+		sd_printk(KERN_ERR, sdkp, "Assuming drive cache: write through\n");
+		sdkp->WCE = 0;
+	}
 	sdkp->RCD = 0;
 	sdkp->DPOFUA = 0;
 }
@@ -2704,6 +2709,7 @@ static int sd_probe(struct device *dev)
 	sdkp->disk = gd;
 	sdkp->index = index;
 	atomic_set(&sdkp->openers, 0);
+	atomic_set(&sdkp->device->ioerr_cnt, 0);
 
 	if (!sdp->request_queue->rq_timeout) {
 		if (sdp->type != TYPE_MOD)
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index 6a4fd00117c..58f4ba6fe41 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -232,11 +232,11 @@ static inline u32 ufshcd_get_ufs_version(struct ufs_hba *hba)
  *			      the host controller
  * @reg_hcs - host controller status register value
  *
- * Returns 0 if device present, non-zero if no device detected
+ * Returns 1 if device present, 0 if no device detected
  */
 static inline int ufshcd_is_device_present(u32 reg_hcs)
 {
-	return (DEVICE_PRESENT & reg_hcs) ? 0 : -1;
+	return (DEVICE_PRESENT & reg_hcs) ? 1 : 0;
 }
 
 /**
@@ -911,7 +911,7 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
 
 	/* check if device present */
 	reg = readl((hba->mmio_base + REG_CONTROLLER_STATUS));
-	if (ufshcd_is_device_present(reg)) {
+	if (!ufshcd_is_device_present(reg)) {
 		dev_err(&hba->pdev->dev, "cc: Device not present\n");
 		err = -ENXIO;
 		goto out;
@@ -1163,6 +1163,8 @@ static int ufshcd_task_req_compl(struct ufs_hba *hba, u32 index)
 		if (task_result != UPIU_TASK_MANAGEMENT_FUNC_COMPL &&
 		    task_result != UPIU_TASK_MANAGEMENT_FUNC_SUCCEEDED)
 			task_result = FAILED;
+		else
+			task_result = SUCCESS;
 	} else {
 		task_result = FAILED;
 		dev_err(&hba->pdev->dev,
@@ -1556,7 +1558,7 @@ ufshcd_issue_tm_cmd(struct ufs_hba *hba,
 		goto out;
 	}
 	clear_bit(free_slot, &hba->tm_condition);
-	return ufshcd_task_req_compl(hba, free_slot);
+	err = ufshcd_task_req_compl(hba, free_slot);
 out:
 	return err;
 }
@@ -1580,7 +1582,7 @@ static int ufshcd_device_reset(struct scsi_cmnd *cmd)
 	tag = cmd->request->tag;
 
 	err = ufshcd_issue_tm_cmd(hba, &hba->lrb[tag], UFS_LOGICAL_RESET);
-	if (err)
+	if (err == FAILED)
 		goto out;
 
 	for (pos = 0; pos < hba->nutrs; pos++) {
@@ -1620,7 +1622,7 @@ static int ufshcd_host_reset(struct scsi_cmnd *cmd)
 	if (hba->ufshcd_state == UFSHCD_STATE_RESET)
 		return SUCCESS;
 
-	return (ufshcd_do_reset(hba) == SUCCESS) ? SUCCESS : FAILED;
+	return ufshcd_do_reset(hba);
 }
 
 /**
@@ -1652,7 +1654,7 @@ static int ufshcd_abort(struct scsi_cmnd *cmd)
 	spin_unlock_irqrestore(host->host_lock, flags);
 
 	err = ufshcd_issue_tm_cmd(hba, &hba->lrb[tag], UFS_ABORT_TASK);
-	if (err)
+	if (err == FAILED)
 		goto out;
 
 	scsi_dma_unmap(cmd);
@@ -1953,24 +1955,7 @@ static struct pci_driver ufshcd_pci_driver = {
 #endif
 };
 
-/**
- * ufshcd_init - Driver registration routine
- */
-static int __init ufshcd_init(void)
-{
-	return pci_register_driver(&ufshcd_pci_driver);
-}
-module_init(ufshcd_init);
-
-/**
- * ufshcd_exit - Driver exit clean-up routine
- */
-static void __exit ufshcd_exit(void)
-{
-	pci_unregister_driver(&ufshcd_pci_driver);
-}
-module_exit(ufshcd_exit);
-
+module_pci_driver(ufshcd_pci_driver);
 
 MODULE_AUTHOR("Santosh Yaragnavi <santosh.sy@samsung.com>, "
 	      "Vinayak Holikatti <h.vinayak@samsung.com>");
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index 1b384311726..c7030fbee79 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -25,6 +25,7 @@
 #include <scsi/scsi_cmnd.h>
 
 #define VIRTIO_SCSI_MEMPOOL_SZ 64
+#define VIRTIO_SCSI_EVENT_LEN 8
 
 /* Command queue element */
 struct virtio_scsi_cmd {
@@ -43,20 +44,42 @@ struct virtio_scsi_cmd {
 	} resp;
 } ____cacheline_aligned_in_smp;
 
-/* Driver instance state */
-struct virtio_scsi {
-	/* Protects ctrl_vq, req_vq and sg[] */
+struct virtio_scsi_event_node {
+	struct virtio_scsi *vscsi;
+	struct virtio_scsi_event event;
+	struct work_struct work;
+};
+
+struct virtio_scsi_vq {
+	/* Protects vq */
 	spinlock_t vq_lock;
 
-	struct virtio_device *vdev;
-	struct virtqueue *ctrl_vq;
-	struct virtqueue *event_vq;
-	struct virtqueue *req_vq;
+	struct virtqueue *vq;
+};
+
+/* Per-target queue state */
+struct virtio_scsi_target_state {
+	/* Protects sg.  Lock hierarchy is tgt_lock -> vq_lock.  */
+	spinlock_t tgt_lock;
 
 	/* For sglist construction when adding commands to the virtqueue.  */
 	struct scatterlist sg[];
 };
 
+/* Driver instance state */
+struct virtio_scsi {
+	struct virtio_device *vdev;
+
+	struct virtio_scsi_vq ctrl_vq;
+	struct virtio_scsi_vq event_vq;
+	struct virtio_scsi_vq req_vq;
+
+	/* Get some buffers ready for event vq */
+	struct virtio_scsi_event_node event_list[VIRTIO_SCSI_EVENT_LEN];
+
+	struct virtio_scsi_target_state *tgt[];
+};
+
 static struct kmem_cache *virtscsi_cmd_cache;
 static mempool_t *virtscsi_cmd_pool;
 
@@ -147,26 +170,25 @@ static void virtscsi_complete_cmd(void *buf)
 
 static void virtscsi_vq_done(struct virtqueue *vq, void (*fn)(void *buf))
 {
-	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
-	struct virtio_scsi *vscsi = shost_priv(sh);
 	void *buf;
-	unsigned long flags;
 	unsigned int len;
 
-	spin_lock_irqsave(&vscsi->vq_lock, flags);
-
 	do {
 		virtqueue_disable_cb(vq);
 		while ((buf = virtqueue_get_buf(vq, &len)) != NULL)
 			fn(buf);
 	} while (!virtqueue_enable_cb(vq));
-
-	spin_unlock_irqrestore(&vscsi->vq_lock, flags);
 }
 
 static void virtscsi_req_done(struct virtqueue *vq)
 {
+	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
+	struct virtio_scsi *vscsi = shost_priv(sh);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vscsi->req_vq.vq_lock, flags);
 	virtscsi_vq_done(vq, virtscsi_complete_cmd);
+	spin_unlock_irqrestore(&vscsi->req_vq.vq_lock, flags);
 };
 
 static void virtscsi_complete_free(void *buf)
@@ -181,12 +203,123 @@ static void virtscsi_complete_free(void *buf)
 
 static void virtscsi_ctrl_done(struct virtqueue *vq)
 {
+	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
+	struct virtio_scsi *vscsi = shost_priv(sh);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vscsi->ctrl_vq.vq_lock, flags);
 	virtscsi_vq_done(vq, virtscsi_complete_free);
+	spin_unlock_irqrestore(&vscsi->ctrl_vq.vq_lock, flags);
 };
 
+static int virtscsi_kick_event(struct virtio_scsi *vscsi,
+			       struct virtio_scsi_event_node *event_node)
+{
+	int ret;
+	struct scatterlist sg;
+	unsigned long flags;
+
+	sg_set_buf(&sg, &event_node->event, sizeof(struct virtio_scsi_event));
+
+	spin_lock_irqsave(&vscsi->event_vq.vq_lock, flags);
+
+	ret = virtqueue_add_buf(vscsi->event_vq.vq, &sg, 0, 1, event_node, GFP_ATOMIC);
+	if (ret >= 0)
+		virtqueue_kick(vscsi->event_vq.vq);
+
+	spin_unlock_irqrestore(&vscsi->event_vq.vq_lock, flags);
+
+	return ret;
+}
+
+static int virtscsi_kick_event_all(struct virtio_scsi *vscsi)
+{
+	int i;
+
+	for (i = 0; i < VIRTIO_SCSI_EVENT_LEN; i++) {
+		vscsi->event_list[i].vscsi = vscsi;
+		virtscsi_kick_event(vscsi, &vscsi->event_list[i]);
+	}
+
+	return 0;
+}
+
+static void virtscsi_cancel_event_work(struct virtio_scsi *vscsi)
+{
+	int i;
+
+	for (i = 0; i < VIRTIO_SCSI_EVENT_LEN; i++)
+		cancel_work_sync(&vscsi->event_list[i].work);
+}
+
+static void virtscsi_handle_transport_reset(struct virtio_scsi *vscsi,
+						struct virtio_scsi_event *event)
+{
+	struct scsi_device *sdev;
+	struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
+	unsigned int target = event->lun[1];
+	unsigned int lun = (event->lun[2] << 8) | event->lun[3];
+
+	switch (event->reason) {
+	case VIRTIO_SCSI_EVT_RESET_RESCAN:
+		scsi_add_device(shost, 0, target, lun);
+		break;
+	case VIRTIO_SCSI_EVT_RESET_REMOVED:
+		sdev = scsi_device_lookup(shost, 0, target, lun);
+		if (sdev) {
+			scsi_remove_device(sdev);
+			scsi_device_put(sdev);
+		} else {
+			pr_err("SCSI device %d 0 %d %d not found\n",
+				shost->host_no, target, lun);
+		}
+		break;
+	default:
+		pr_info("Unsupport virtio scsi event reason %x\n", event->reason);
+	}
+}
+
+static void virtscsi_handle_event(struct work_struct *work)
+{
+	struct virtio_scsi_event_node *event_node =
+		container_of(work, struct virtio_scsi_event_node, work);
+	struct virtio_scsi *vscsi = event_node->vscsi;
+	struct virtio_scsi_event *event = &event_node->event;
+
+	if (event->event & VIRTIO_SCSI_T_EVENTS_MISSED) {
+		event->event &= ~VIRTIO_SCSI_T_EVENTS_MISSED;
+		scsi_scan_host(virtio_scsi_host(vscsi->vdev));
+	}
+
+	switch (event->event) {
+	case VIRTIO_SCSI_T_NO_EVENT:
+		break;
+	case VIRTIO_SCSI_T_TRANSPORT_RESET:
+		virtscsi_handle_transport_reset(vscsi, event);
+		break;
+	default:
+		pr_err("Unsupport virtio scsi event %x\n", event->event);
+	}
+	virtscsi_kick_event(vscsi, event_node);
+}
+
+static void virtscsi_complete_event(void *buf)
+{
+	struct virtio_scsi_event_node *event_node = buf;
+
+	INIT_WORK(&event_node->work, virtscsi_handle_event);
+	schedule_work(&event_node->work);
+}
+
 static void virtscsi_event_done(struct virtqueue *vq)
 {
-	virtscsi_vq_done(vq, virtscsi_complete_free);
+	struct Scsi_Host *sh = virtio_scsi_host(vq->vdev);
+	struct virtio_scsi *vscsi = shost_priv(sh);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vscsi->event_vq.vq_lock, flags);
+	virtscsi_vq_done(vq, virtscsi_complete_event);
+	spin_unlock_irqrestore(&vscsi->event_vq.vq_lock, flags);
 };
 
 static void virtscsi_map_sgl(struct scatterlist *sg, unsigned int *p_idx,
@@ -212,25 +345,17 @@ static void virtscsi_map_sgl(struct scatterlist *sg, unsigned int *p_idx,
  * @req_size	: size of the request buffer
  * @resp_size	: size of the response buffer
  *
- * Called with vq_lock held.
+ * Called with tgt_lock held.
  */
-static void virtscsi_map_cmd(struct virtio_scsi *vscsi,
+static void virtscsi_map_cmd(struct virtio_scsi_target_state *tgt,
 			     struct virtio_scsi_cmd *cmd,
 			     unsigned *out_num, unsigned *in_num,
 			     size_t req_size, size_t resp_size)
 {
 	struct scsi_cmnd *sc = cmd->sc;
-	struct scatterlist *sg = vscsi->sg;
+	struct scatterlist *sg = tgt->sg;
 	unsigned int idx = 0;
 
-	if (sc) {
-		struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
-		BUG_ON(scsi_sg_count(sc) > shost->sg_tablesize);
-
-		/* TODO: check feature bit and fail if unsupported?  */
-		BUG_ON(sc->sc_data_direction == DMA_BIDIRECTIONAL);
-	}
-
 	/* Request header.  */
 	sg_set_buf(&sg[idx++], &cmd->req, req_size);
 
@@ -250,7 +375,8 @@ static void virtscsi_map_cmd(struct virtio_scsi *vscsi,
 	*in_num = idx - *out_num;
 }
 
-static int virtscsi_kick_cmd(struct virtio_scsi *vscsi, struct virtqueue *vq,
+static int virtscsi_kick_cmd(struct virtio_scsi_target_state *tgt,
+			     struct virtio_scsi_vq *vq,
 			     struct virtio_scsi_cmd *cmd,
 			     size_t req_size, size_t resp_size, gfp_t gfp)
 {
@@ -258,24 +384,35 @@ static int virtscsi_kick_cmd(struct virtio_scsi *vscsi, struct virtqueue *vq,
 	unsigned long flags;
 	int ret;
 
-	spin_lock_irqsave(&vscsi->vq_lock, flags);
-
-	virtscsi_map_cmd(vscsi, cmd, &out_num, &in_num, req_size, resp_size);
+	spin_lock_irqsave(&tgt->tgt_lock, flags);
+	virtscsi_map_cmd(tgt, cmd, &out_num, &in_num, req_size, resp_size);
 
-	ret = virtqueue_add_buf(vq, vscsi->sg, out_num, in_num, cmd, gfp);
+	spin_lock(&vq->vq_lock);
+	ret = virtqueue_add_buf(vq->vq, tgt->sg, out_num, in_num, cmd, gfp);
+	spin_unlock(&tgt->tgt_lock);
 	if (ret >= 0)
-		virtqueue_kick(vq);
+		ret = virtqueue_kick_prepare(vq->vq);
+
+	spin_unlock_irqrestore(&vq->vq_lock, flags);
 
-	spin_unlock_irqrestore(&vscsi->vq_lock, flags);
+	if (ret > 0)
+		virtqueue_notify(vq->vq);
 	return ret;
 }
 
 static int virtscsi_queuecommand(struct Scsi_Host *sh, struct scsi_cmnd *sc)
 {
 	struct virtio_scsi *vscsi = shost_priv(sh);
+	struct virtio_scsi_target_state *tgt = vscsi->tgt[sc->device->id];
 	struct virtio_scsi_cmd *cmd;
 	int ret;
 
+	struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
+	BUG_ON(scsi_sg_count(sc) > shost->sg_tablesize);
+
+	/* TODO: check feature bit and fail if unsupported?  */
+	BUG_ON(sc->sc_data_direction == DMA_BIDIRECTIONAL);
+
 	dev_dbg(&sc->device->sdev_gendev,
 		"cmd %p CDB: %#02x\n", sc, sc->cmnd[0]);
 
@@ -300,7 +437,7 @@ static int virtscsi_queuecommand(struct Scsi_Host *sh, struct scsi_cmnd *sc)
 	BUG_ON(sc->cmd_len > VIRTIO_SCSI_CDB_SIZE);
 	memcpy(cmd->req.cmd.cdb, sc->cmnd, sc->cmd_len);
 
-	if (virtscsi_kick_cmd(vscsi, vscsi->req_vq, cmd,
+	if (virtscsi_kick_cmd(tgt, &vscsi->req_vq, cmd,
 			      sizeof cmd->req.cmd, sizeof cmd->resp.cmd,
 			      GFP_ATOMIC) >= 0)
 		ret = 0;
@@ -312,10 +449,11 @@ out:
 static int virtscsi_tmf(struct virtio_scsi *vscsi, struct virtio_scsi_cmd *cmd)
 {
 	DECLARE_COMPLETION_ONSTACK(comp);
+	struct virtio_scsi_target_state *tgt = vscsi->tgt[cmd->sc->device->id];
 	int ret = FAILED;
 
 	cmd->comp = &comp;
-	if (virtscsi_kick_cmd(vscsi, vscsi->ctrl_vq, cmd,
+	if (virtscsi_kick_cmd(tgt, &vscsi->ctrl_vq, cmd,
 			      sizeof cmd->req.tmf, sizeof cmd->resp.tmf,
 			      GFP_NOIO) < 0)
 		goto out;
@@ -408,11 +546,63 @@ static struct scsi_host_template virtscsi_host_template = {
 				  &__val, sizeof(__val)); \
 	})
 
+static void virtscsi_init_vq(struct virtio_scsi_vq *virtscsi_vq,
+			     struct virtqueue *vq)
+{
+	spin_lock_init(&virtscsi_vq->vq_lock);
+	virtscsi_vq->vq = vq;
+}
+
+static struct virtio_scsi_target_state *virtscsi_alloc_tgt(
+	struct virtio_device *vdev, int sg_elems)
+{
+	struct virtio_scsi_target_state *tgt;
+	gfp_t gfp_mask = GFP_KERNEL;
+
+	/* We need extra sg elements at head and tail.  */
+	tgt = kmalloc(sizeof(*tgt) + sizeof(tgt->sg[0]) * (sg_elems + 2),
+		      gfp_mask);
+
+	if (!tgt)
+		return NULL;
+
+	spin_lock_init(&tgt->tgt_lock);
+	sg_init_table(tgt->sg, sg_elems + 2);
+	return tgt;
+}
+
+static void virtscsi_scan(struct virtio_device *vdev)
+{
+	struct Scsi_Host *shost = (struct Scsi_Host *)vdev->priv;
+
+	scsi_scan_host(shost);
+}
+
+static void virtscsi_remove_vqs(struct virtio_device *vdev)
+{
+	struct Scsi_Host *sh = virtio_scsi_host(vdev);
+	struct virtio_scsi *vscsi = shost_priv(sh);
+	u32 i, num_targets;
+
+	/* Stop all the virtqueues. */
+	vdev->config->reset(vdev);
+
+	num_targets = sh->max_id;
+	for (i = 0; i < num_targets; i++) {
+		kfree(vscsi->tgt[i]);
+		vscsi->tgt[i] = NULL;
+	}
+
+	vdev->config->del_vqs(vdev);
+}
+
 static int virtscsi_init(struct virtio_device *vdev,
-			 struct virtio_scsi *vscsi)
+			 struct virtio_scsi *vscsi, int num_targets)
 {
 	int err;
 	struct virtqueue *vqs[3];
+	u32 i, sg_elems;
+
 	vq_callback_t *callbacks[] = {
 		virtscsi_ctrl_done,
 		virtscsi_event_done,
@@ -429,13 +619,32 @@ static int virtscsi_init(struct virtio_device *vdev,
 	if (err)
 		return err;
 
-	vscsi->ctrl_vq = vqs[0];
-	vscsi->event_vq = vqs[1];
-	vscsi->req_vq = vqs[2];
+	virtscsi_init_vq(&vscsi->ctrl_vq, vqs[0]);
+	virtscsi_init_vq(&vscsi->event_vq, vqs[1]);
+	virtscsi_init_vq(&vscsi->req_vq, vqs[2]);
 
 	virtscsi_config_set(vdev, cdb_size, VIRTIO_SCSI_CDB_SIZE);
 	virtscsi_config_set(vdev, sense_size, VIRTIO_SCSI_SENSE_SIZE);
-	return 0;
+
+	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_HOTPLUG))
+		virtscsi_kick_event_all(vscsi);
+
+	/* We need to know how many segments before we allocate.  */
+	sg_elems = virtscsi_config_get(vdev, seg_max) ?: 1;
+
+	for (i = 0; i < num_targets; i++) {
+		vscsi->tgt[i] = virtscsi_alloc_tgt(vdev, sg_elems);
+		if (!vscsi->tgt[i]) {
+			err = -ENOMEM;
+			goto out;
+		}
+	}
+	err = 0;
+
+out:
+	if (err)
+		virtscsi_remove_vqs(vdev);
+	return err;
 }
 
 static int __devinit virtscsi_probe(struct virtio_device *vdev)
@@ -443,31 +652,25 @@ static int __devinit virtscsi_probe(struct virtio_device *vdev)
 	struct Scsi_Host *shost;
 	struct virtio_scsi *vscsi;
 	int err;
-	u32 sg_elems;
+	u32 sg_elems, num_targets;
 	u32 cmd_per_lun;
 
-	/* We need to know how many segments before we allocate.
-	 * We need an extra sg elements at head and tail.
-	 */
-	sg_elems = virtscsi_config_get(vdev, seg_max) ?: 1;
-
 	/* Allocate memory and link the structs together.  */
+	num_targets = virtscsi_config_get(vdev, max_target) + 1;
 	shost = scsi_host_alloc(&virtscsi_host_template,
-		sizeof(*vscsi) + sizeof(vscsi->sg[0]) * (sg_elems + 2));
+		sizeof(*vscsi)
+		+ num_targets * sizeof(struct virtio_scsi_target_state));
 
 	if (!shost)
 		return -ENOMEM;
 
+	sg_elems = virtscsi_config_get(vdev, seg_max) ?: 1;
 	shost->sg_tablesize = sg_elems;
 	vscsi = shost_priv(shost);
 	vscsi->vdev = vdev;
 	vdev->priv = shost;
 
-	/* Random initializations.  */
-	spin_lock_init(&vscsi->vq_lock);
-	sg_init_table(vscsi->sg, sg_elems + 2);
-
-	err = virtscsi_init(vdev, vscsi);
+	err = virtscsi_init(vdev, vscsi, num_targets);
 	if (err)
 		goto virtscsi_init_failed;
 
@@ -475,15 +678,16 @@ static int __devinit virtscsi_probe(struct virtio_device *vdev)
 	shost->cmd_per_lun = min_t(u32, cmd_per_lun, shost->can_queue);
 	shost->max_sectors = virtscsi_config_get(vdev, max_sectors) ?: 0xFFFF;
 	shost->max_lun = virtscsi_config_get(vdev, max_lun) + 1;
-	shost->max_id = virtscsi_config_get(vdev, max_target) + 1;
+	shost->max_id = num_targets;
 	shost->max_channel = 0;
 	shost->max_cmd_len = VIRTIO_SCSI_CDB_SIZE;
 	err = scsi_add_host(shost, &vdev->dev);
 	if (err)
 		goto scsi_add_host_failed;
-
-	scsi_scan_host(shost);
-
+	/*
+	 * scsi_scan_host() happens in virtscsi_scan() via virtio_driver->scan()
+	 * after VIRTIO_CONFIG_S_DRIVER_OK has been set..
+	 */
 	return 0;
 
 scsi_add_host_failed:
@@ -493,17 +697,13 @@ virtscsi_init_failed:
 	return err;
 }
 
-static void virtscsi_remove_vqs(struct virtio_device *vdev)
-{
-	/* Stop all the virtqueues. */
-	vdev->config->reset(vdev);
-
-	vdev->config->del_vqs(vdev);
-}
-
 static void __devexit virtscsi_remove(struct virtio_device *vdev)
 {
 	struct Scsi_Host *shost = virtio_scsi_host(vdev);
+	struct virtio_scsi *vscsi = shost_priv(shost);
+
+	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_HOTPLUG))
+		virtscsi_cancel_event_work(vscsi);
 
 	scsi_remove_host(shost);
 
@@ -523,7 +723,7 @@ static int virtscsi_restore(struct virtio_device *vdev)
 	struct Scsi_Host *sh = virtio_scsi_host(vdev);
 	struct virtio_scsi *vscsi = shost_priv(sh);
 
-	return virtscsi_init(vdev, vscsi);
+	return virtscsi_init(vdev, vscsi, sh->max_id);
 }
 #endif
 
@@ -532,11 +732,18 @@ static struct virtio_device_id id_table[] = {
 	{ 0 },
 };
 
+static unsigned int features[] = {
+	VIRTIO_SCSI_F_HOTPLUG
+};
+
 static struct virtio_driver virtio_scsi_driver = {
+	.feature_table = features,
+	.feature_table_size = ARRAY_SIZE(features),
 	.driver.name = KBUILD_MODNAME,
 	.driver.owner = THIS_MODULE,
 	.id_table = id_table,
 	.probe = virtscsi_probe,
+	.scan = virtscsi_scan,
 #ifdef CONFIG_PM
 	.freeze = virtscsi_freeze,
 	.restore = virtscsi_restore,
diff --git a/drivers/sh/Kconfig b/drivers/sh/Kconfig
index f168a615996..d860ef74356 100644
--- a/drivers/sh/Kconfig
+++ b/drivers/sh/Kconfig
@@ -1,5 +1,6 @@
 menu "SuperH / SH-Mobile Driver Options"
 
 source "drivers/sh/intc/Kconfig"
+source "drivers/sh/pfc/Kconfig"
 
 endmenu
diff --git a/drivers/sh/Makefile b/drivers/sh/Makefile
index 7139ad2f208..e57895b1a42 100644
--- a/drivers/sh/Makefile
+++ b/drivers/sh/Makefile
@@ -5,6 +5,7 @@ obj-y	:= intc/
 
 obj-$(CONFIG_HAVE_CLK)		+= clk/
 obj-$(CONFIG_MAPLE)		+= maple/
+obj-$(CONFIG_SH_PFC)		+= pfc/
 obj-$(CONFIG_SUPERHYWAY)	+= superhyway/
-obj-$(CONFIG_GENERIC_GPIO)	+= pfc.o
+
 obj-y				+= pm_runtime.o
diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c
index f0d015dd0fe..07e9fb4f804 100644
--- a/drivers/sh/clk/cpg.c
+++ b/drivers/sh/clk/cpg.c
@@ -14,6 +14,8 @@
 #include <linux/io.h>
 #include <linux/sh_clk.h>
 
+#define CPG_CKSTP_BIT	BIT(8)
+
 static unsigned int sh_clk_read(struct clk *clk)
 {
 	if (clk->flags & CLK_ENABLE_REG_8BIT)
@@ -66,71 +68,43 @@ int __init sh_clk_mstp_register(struct clk *clks, int nr)
 	return ret;
 }
 
-static long sh_clk_div_round_rate(struct clk *clk, unsigned long rate)
+/*
+ * Div/mult table lookup helpers
+ */
+static inline struct clk_div_table *clk_to_div_table(struct clk *clk)
 {
-	return clk_rate_table_round(clk, clk->freq_table, rate);
+	return clk->priv;
 }
 
-static int sh_clk_div6_divisors[64] = {
-	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
-	17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
-	33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
-	49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64
-};
+static inline struct clk_div_mult_table *clk_to_div_mult_table(struct clk *clk)
+{
+	return clk_to_div_table(clk)->div_mult_table;
+}
 
-static struct clk_div_mult_table sh_clk_div6_table = {
-	.divisors = sh_clk_div6_divisors,
-	.nr_divisors = ARRAY_SIZE(sh_clk_div6_divisors),
-};
+/*
+ * Common div ops
+ */
+static long sh_clk_div_round_rate(struct clk *clk, unsigned long rate)
+{
+	return clk_rate_table_round(clk, clk->freq_table, rate);
+}
 
-static unsigned long sh_clk_div6_recalc(struct clk *clk)
+static unsigned long sh_clk_div_recalc(struct clk *clk)
 {
-	struct clk_div_mult_table *table = &sh_clk_div6_table;
+	struct clk_div_mult_table *table = clk_to_div_mult_table(clk);
 	unsigned int idx;
 
 	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
-			     table, NULL);
+			     table, clk->arch_flags ? &clk->arch_flags : NULL);
 
-	idx = sh_clk_read(clk) & 0x003f;
+	idx = (sh_clk_read(clk) >> clk->enable_bit) & clk->div_mask;
 
 	return clk->freq_table[idx].frequency;
 }
 
-static int sh_clk_div6_set_parent(struct clk *clk, struct clk *parent)
-{
-	struct clk_div_mult_table *table = &sh_clk_div6_table;
-	u32 value;
-	int ret, i;
-
-	if (!clk->parent_table || !clk->parent_num)
-		return -EINVAL;
-
-	/* Search the parent */
-	for (i = 0; i < clk->parent_num; i++)
-		if (clk->parent_table[i] == parent)
-			break;
-
-	if (i == clk->parent_num)
-		return -ENODEV;
-
-	ret = clk_reparent(clk, parent);
-	if (ret < 0)
-		return ret;
-
-	value = sh_clk_read(clk) &
-		~(((1 << clk->src_width) - 1) << clk->src_shift);
-
-	sh_clk_write(value | (i << clk->src_shift), clk);
-
-	/* Rebuild the frequency table */
-	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
-			     table, NULL);
-
-	return 0;
-}
-
-static int sh_clk_div6_set_rate(struct clk *clk, unsigned long rate)
+static int sh_clk_div_set_rate(struct clk *clk, unsigned long rate)
 {
+	struct clk_div_table *dt = clk_to_div_table(clk);
 	unsigned long value;
 	int idx;
 
@@ -139,51 +113,53 @@ static int sh_clk_div6_set_rate(struct clk *clk, unsigned long rate)
 		return idx;
 
 	value = sh_clk_read(clk);
-	value &= ~0x3f;
-	value |= idx;
+	value &= ~(clk->div_mask << clk->enable_bit);
+	value |= (idx << clk->enable_bit);
 	sh_clk_write(value, clk);
+
+	/* XXX: Should use a post-change notifier */
+	if (dt->kick)
+		dt->kick(clk);
+
 	return 0;
 }
 
-static int sh_clk_div6_enable(struct clk *clk)
+static int sh_clk_div_enable(struct clk *clk)
 {
-	unsigned long value;
-	int ret;
-
-	ret = sh_clk_div6_set_rate(clk, clk->rate);
-	if (ret == 0) {
-		value = sh_clk_read(clk);
-		value &= ~0x100; /* clear stop bit to enable clock */
-		sh_clk_write(value, clk);
-	}
-	return ret;
+	sh_clk_write(sh_clk_read(clk) & ~CPG_CKSTP_BIT, clk);
+	return 0;
 }
 
-static void sh_clk_div6_disable(struct clk *clk)
+static void sh_clk_div_disable(struct clk *clk)
 {
-	unsigned long value;
+	unsigned int val;
 
-	value = sh_clk_read(clk);
-	value |= 0x100; /* stop clock */
-	value |= 0x3f; /* VDIV bits must be non-zero, overwrite divider */
-	sh_clk_write(value, clk);
+	val = sh_clk_read(clk);
+	val |= CPG_CKSTP_BIT;
+
+	/*
+	 * div6 clocks require the divisor field to be non-zero or the
+	 * above CKSTP toggle silently fails. Ensure that the divisor
+	 * array is reset to its initial state on disable.
+	 */
+	if (clk->flags & CLK_MASK_DIV_ON_DISABLE)
+		val |= clk->div_mask;
+
+	sh_clk_write(val, clk);
 }
 
-static struct sh_clk_ops sh_clk_div6_clk_ops = {
-	.recalc		= sh_clk_div6_recalc,
+static struct sh_clk_ops sh_clk_div_clk_ops = {
+	.recalc		= sh_clk_div_recalc,
+	.set_rate	= sh_clk_div_set_rate,
 	.round_rate	= sh_clk_div_round_rate,
-	.set_rate	= sh_clk_div6_set_rate,
-	.enable		= sh_clk_div6_enable,
-	.disable	= sh_clk_div6_disable,
 };
 
-static struct sh_clk_ops sh_clk_div6_reparent_clk_ops = {
-	.recalc		= sh_clk_div6_recalc,
+static struct sh_clk_ops sh_clk_div_enable_clk_ops = {
+	.recalc		= sh_clk_div_recalc,
+	.set_rate	= sh_clk_div_set_rate,
 	.round_rate	= sh_clk_div_round_rate,
-	.set_rate	= sh_clk_div6_set_rate,
-	.enable		= sh_clk_div6_enable,
-	.disable	= sh_clk_div6_disable,
-	.set_parent	= sh_clk_div6_set_parent,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
 };
 
 static int __init sh_clk_init_parent(struct clk *clk)
@@ -218,12 +194,12 @@ static int __init sh_clk_init_parent(struct clk *clk)
 	return 0;
 }
 
-static int __init sh_clk_div6_register_ops(struct clk *clks, int nr,
-					   struct sh_clk_ops *ops)
+static int __init sh_clk_div_register_ops(struct clk *clks, int nr,
+			struct clk_div_table *table, struct sh_clk_ops *ops)
 {
 	struct clk *clkp;
 	void *freq_table;
-	int nr_divs = sh_clk_div6_table.nr_divisors;
+	int nr_divs = table->div_mult_table->nr_divisors;
 	int freq_table_size = sizeof(struct cpufreq_frequency_table);
 	int ret = 0;
 	int k;
@@ -231,7 +207,7 @@ static int __init sh_clk_div6_register_ops(struct clk *clks, int nr,
 	freq_table_size *= (nr_divs + 1);
 	freq_table = kzalloc(freq_table_size * nr, GFP_KERNEL);
 	if (!freq_table) {
-		pr_err("sh_clk_div6_register: unable to alloc memory\n");
+		pr_err("%s: unable to alloc memory\n", __func__);
 		return -ENOMEM;
 	}
 
@@ -239,47 +215,98 @@ static int __init sh_clk_div6_register_ops(struct clk *clks, int nr,
 		clkp = clks + k;
 
 		clkp->ops = ops;
+		clkp->priv = table;
+
 		clkp->freq_table = freq_table + (k * freq_table_size);
 		clkp->freq_table[nr_divs].frequency = CPUFREQ_TABLE_END;
-		ret = clk_register(clkp);
-		if (ret < 0)
-			break;
 
-		ret = sh_clk_init_parent(clkp);
+		ret = clk_register(clkp);
+		if (ret == 0)
+			ret = sh_clk_init_parent(clkp);
 	}
 
 	return ret;
 }
 
-int __init sh_clk_div6_register(struct clk *clks, int nr)
-{
-	return sh_clk_div6_register_ops(clks, nr, &sh_clk_div6_clk_ops);
-}
+/*
+ * div6 support
+ */
+static int sh_clk_div6_divisors[64] = {
+	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+	17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+	33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+	49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64
+};
 
-int __init sh_clk_div6_reparent_register(struct clk *clks, int nr)
-{
-	return sh_clk_div6_register_ops(clks, nr,
-					&sh_clk_div6_reparent_clk_ops);
-}
+static struct clk_div_mult_table div6_div_mult_table = {
+	.divisors = sh_clk_div6_divisors,
+	.nr_divisors = ARRAY_SIZE(sh_clk_div6_divisors),
+};
 
-static unsigned long sh_clk_div4_recalc(struct clk *clk)
+static struct clk_div_table sh_clk_div6_table = {
+	.div_mult_table	= &div6_div_mult_table,
+};
+
+static int sh_clk_div6_set_parent(struct clk *clk, struct clk *parent)
 {
-	struct clk_div4_table *d4t = clk->priv;
-	struct clk_div_mult_table *table = d4t->div_mult_table;
-	unsigned int idx;
+	struct clk_div_mult_table *table = clk_to_div_mult_table(clk);
+	u32 value;
+	int ret, i;
 
+	if (!clk->parent_table || !clk->parent_num)
+		return -EINVAL;
+
+	/* Search the parent */
+	for (i = 0; i < clk->parent_num; i++)
+		if (clk->parent_table[i] == parent)
+			break;
+
+	if (i == clk->parent_num)
+		return -ENODEV;
+
+	ret = clk_reparent(clk, parent);
+	if (ret < 0)
+		return ret;
+
+	value = sh_clk_read(clk) &
+		~(((1 << clk->src_width) - 1) << clk->src_shift);
+
+	sh_clk_write(value | (i << clk->src_shift), clk);
+
+	/* Rebuild the frequency table */
 	clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
-			     table, &clk->arch_flags);
+			     table, NULL);
 
-	idx = (sh_clk_read(clk) >> clk->enable_bit) & 0x000f;
+	return 0;
+}
 
-	return clk->freq_table[idx].frequency;
+static struct sh_clk_ops sh_clk_div6_reparent_clk_ops = {
+	.recalc		= sh_clk_div_recalc,
+	.round_rate	= sh_clk_div_round_rate,
+	.set_rate	= sh_clk_div_set_rate,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
+	.set_parent	= sh_clk_div6_set_parent,
+};
+
+int __init sh_clk_div6_register(struct clk *clks, int nr)
+{
+	return sh_clk_div_register_ops(clks, nr, &sh_clk_div6_table,
+				       &sh_clk_div_enable_clk_ops);
+}
+
+int __init sh_clk_div6_reparent_register(struct clk *clks, int nr)
+{
+	return sh_clk_div_register_ops(clks, nr, &sh_clk_div6_table,
+				       &sh_clk_div6_reparent_clk_ops);
 }
 
+/*
+ * div4 support
+ */
 static int sh_clk_div4_set_parent(struct clk *clk, struct clk *parent)
 {
-	struct clk_div4_table *d4t = clk->priv;
-	struct clk_div_mult_table *table = d4t->div_mult_table;
+	struct clk_div_mult_table *table = clk_to_div_mult_table(clk);
 	u32 value;
 	int ret;
 
@@ -306,107 +333,31 @@ static int sh_clk_div4_set_parent(struct clk *clk, struct clk *parent)
 	return 0;
 }
 
-static int sh_clk_div4_set_rate(struct clk *clk, unsigned long rate)
-{
-	struct clk_div4_table *d4t = clk->priv;
-	unsigned long value;
-	int idx = clk_rate_table_find(clk, clk->freq_table, rate);
-	if (idx < 0)
-		return idx;
-
-	value = sh_clk_read(clk);
-	value &= ~(0xf << clk->enable_bit);
-	value |= (idx << clk->enable_bit);
-	sh_clk_write(value, clk);
-
-	if (d4t->kick)
-		d4t->kick(clk);
-
-	return 0;
-}
-
-static int sh_clk_div4_enable(struct clk *clk)
-{
-	sh_clk_write(sh_clk_read(clk) & ~(1 << 8), clk);
-	return 0;
-}
-
-static void sh_clk_div4_disable(struct clk *clk)
-{
-	sh_clk_write(sh_clk_read(clk) | (1 << 8), clk);
-}
-
-static struct sh_clk_ops sh_clk_div4_clk_ops = {
-	.recalc		= sh_clk_div4_recalc,
-	.set_rate	= sh_clk_div4_set_rate,
-	.round_rate	= sh_clk_div_round_rate,
-};
-
-static struct sh_clk_ops sh_clk_div4_enable_clk_ops = {
-	.recalc		= sh_clk_div4_recalc,
-	.set_rate	= sh_clk_div4_set_rate,
-	.round_rate	= sh_clk_div_round_rate,
-	.enable		= sh_clk_div4_enable,
-	.disable	= sh_clk_div4_disable,
-};
-
 static struct sh_clk_ops sh_clk_div4_reparent_clk_ops = {
-	.recalc		= sh_clk_div4_recalc,
-	.set_rate	= sh_clk_div4_set_rate,
+	.recalc		= sh_clk_div_recalc,
+	.set_rate	= sh_clk_div_set_rate,
 	.round_rate	= sh_clk_div_round_rate,
-	.enable		= sh_clk_div4_enable,
-	.disable	= sh_clk_div4_disable,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
 	.set_parent	= sh_clk_div4_set_parent,
 };
 
-static int __init sh_clk_div4_register_ops(struct clk *clks, int nr,
-			struct clk_div4_table *table, struct sh_clk_ops *ops)
-{
-	struct clk *clkp;
-	void *freq_table;
-	int nr_divs = table->div_mult_table->nr_divisors;
-	int freq_table_size = sizeof(struct cpufreq_frequency_table);
-	int ret = 0;
-	int k;
-
-	freq_table_size *= (nr_divs + 1);
-	freq_table = kzalloc(freq_table_size * nr, GFP_KERNEL);
-	if (!freq_table) {
-		pr_err("sh_clk_div4_register: unable to alloc memory\n");
-		return -ENOMEM;
-	}
-
-	for (k = 0; !ret && (k < nr); k++) {
-		clkp = clks + k;
-
-		clkp->ops = ops;
-		clkp->priv = table;
-
-		clkp->freq_table = freq_table + (k * freq_table_size);
-		clkp->freq_table[nr_divs].frequency = CPUFREQ_TABLE_END;
-
-		ret = clk_register(clkp);
-	}
-
-	return ret;
-}
-
 int __init sh_clk_div4_register(struct clk *clks, int nr,
 				struct clk_div4_table *table)
 {
-	return sh_clk_div4_register_ops(clks, nr, table, &sh_clk_div4_clk_ops);
+	return sh_clk_div_register_ops(clks, nr, table, &sh_clk_div_clk_ops);
 }
 
 int __init sh_clk_div4_enable_register(struct clk *clks, int nr,
 				struct clk_div4_table *table)
 {
-	return sh_clk_div4_register_ops(clks, nr, table,
-					&sh_clk_div4_enable_clk_ops);
+	return sh_clk_div_register_ops(clks, nr, table,
+				       &sh_clk_div_enable_clk_ops);
 }
 
 int __init sh_clk_div4_reparent_register(struct clk *clks, int nr,
 				struct clk_div4_table *table)
 {
-	return sh_clk_div4_register_ops(clks, nr, table,
-					&sh_clk_div4_reparent_clk_ops);
+	return sh_clk_div_register_ops(clks, nr, table,
+				       &sh_clk_div4_reparent_clk_ops);
 }
diff --git a/drivers/sh/intc/Kconfig b/drivers/sh/intc/Kconfig
index c88cbccc62b..a305731742a 100644
--- a/drivers/sh/intc/Kconfig
+++ b/drivers/sh/intc/Kconfig
@@ -1,3 +1,7 @@
+config SH_INTC
+	def_bool y
+	select IRQ_DOMAIN
+
 comment "Interrupt controller options"
 
 config INTC_USERIMASK
diff --git a/drivers/sh/intc/Makefile b/drivers/sh/intc/Makefile
index bb5df868d77..54ec2a0643d 100644
--- a/drivers/sh/intc/Makefile
+++ b/drivers/sh/intc/Makefile
@@ -1,4 +1,4 @@
-obj-y 	:= access.o chip.o core.o dynamic.o handle.o virq.o
+obj-y 	:= access.o chip.o core.o handle.o irqdomain.o virq.o
 
 obj-$(CONFIG_INTC_BALANCING)		+= balancing.o
 obj-$(CONFIG_INTC_USERIMASK)		+= userimask.o
diff --git a/drivers/sh/intc/core.c b/drivers/sh/intc/core.c
index 7e562ccb699..2374468615e 100644
--- a/drivers/sh/intc/core.c
+++ b/drivers/sh/intc/core.c
@@ -25,6 +25,7 @@
 #include <linux/stat.h>
 #include <linux/interrupt.h>
 #include <linux/sh_intc.h>
+#include <linux/irqdomain.h>
 #include <linux/device.h>
 #include <linux/syscore_ops.h>
 #include <linux/list.h>
@@ -310,6 +311,8 @@ int __init register_intc_controller(struct intc_desc *desc)
 
 	BUG_ON(k > 256); /* _INTC_ADDR_E() and _INTC_ADDR_D() are 8 bits */
 
+	intc_irq_domain_init(d, hw);
+
 	/* register the vectors one by one */
 	for (i = 0; i < hw->nr_vectors; i++) {
 		struct intc_vect *vect = hw->vectors + i;
@@ -319,8 +322,8 @@ int __init register_intc_controller(struct intc_desc *desc)
 		if (!vect->enum_id)
 			continue;
 
-		res = irq_alloc_desc_at(irq, numa_node_id());
-		if (res != irq && res != -EEXIST) {
+		res = irq_create_identity_mapping(d->domain, irq);
+		if (unlikely(res)) {
 			pr_err("can't get irq_desc for %d\n", irq);
 			continue;
 		}
@@ -340,8 +343,8 @@ int __init register_intc_controller(struct intc_desc *desc)
 			 * IRQ support, each vector still needs to have
 			 * its own backing irq_desc.
 			 */
-			res = irq_alloc_desc_at(irq2, numa_node_id());
-			if (res != irq2 && res != -EEXIST) {
+			res = irq_create_identity_mapping(d->domain, irq2);
+			if (unlikely(res)) {
 				pr_err("can't get irq_desc for %d\n", irq2);
 				continue;
 			}
diff --git a/drivers/sh/intc/dynamic.c b/drivers/sh/intc/dynamic.c
deleted file mode 100644
index 14eb01ef5d7..00000000000
--- a/drivers/sh/intc/dynamic.c
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Dynamic IRQ management
- *
- * Copyright (C) 2010  Paul Mundt
- *
- * Modelled after arch/x86/kernel/apic/io_apic.c
- *
- * This file is subject to the terms and conditions of the GNU General Public
- * License.  See the file "COPYING" in the main directory of this archive
- * for more details.
- */
-#define pr_fmt(fmt) "intc: " fmt
-
-#include <linux/irq.h>
-#include <linux/bitmap.h>
-#include <linux/spinlock.h>
-#include <linux/module.h>
-#include "internals.h" /* only for activate_irq() damage.. */
-
-/*
- * The IRQ bitmap provides a global map of bound IRQ vectors for a
- * given platform. Allocation of IRQs are either static through the CPU
- * vector map, or dynamic in the case of board mux vectors or MSI.
- *
- * As this is a central point for all IRQ controllers on the system,
- * each of the available sources are mapped out here. This combined with
- * sparseirq makes it quite trivial to keep the vector map tightly packed
- * when dynamically creating IRQs, as well as tying in to otherwise
- * unused irq_desc positions in the sparse array.
- */
-
-/*
- * Dynamic IRQ allocation and deallocation
- */
-unsigned int create_irq_nr(unsigned int irq_want, int node)
-{
-	int irq = irq_alloc_desc_at(irq_want, node);
-	if (irq < 0)
-		return 0;
-
-	activate_irq(irq);
-	return irq;
-}
-
-int create_irq(void)
-{
-	int irq = irq_alloc_desc(numa_node_id());
-	if (irq >= 0)
-		activate_irq(irq);
-
-	return irq;
-}
-
-void destroy_irq(unsigned int irq)
-{
-	irq_free_desc(irq);
-}
diff --git a/drivers/sh/intc/internals.h b/drivers/sh/intc/internals.h
index f034a979a16..7dff08e2a07 100644
--- a/drivers/sh/intc/internals.h
+++ b/drivers/sh/intc/internals.h
@@ -1,5 +1,6 @@
 #include <linux/sh_intc.h>
 #include <linux/irq.h>
+#include <linux/irqdomain.h>
 #include <linux/list.h>
 #include <linux/kernel.h>
 #include <linux/types.h>
@@ -66,6 +67,7 @@ struct intc_desc_int {
 	unsigned int nr_sense;
 	struct intc_window *window;
 	unsigned int nr_windows;
+	struct irq_domain *domain;
 	struct irq_chip chip;
 	bool skip_suspend;
 };
@@ -187,6 +189,9 @@ unsigned long intc_get_ack_handle(unsigned int irq);
 void intc_enable_disable_enum(struct intc_desc *desc, struct intc_desc_int *d,
 			      intc_enum enum_id, int enable);
 
+/* irqdomain.c */
+void intc_irq_domain_init(struct intc_desc_int *d, struct intc_hw_desc *hw);
+
 /* virq.c */
 void intc_subgroup_init(struct intc_desc *desc, struct intc_desc_int *d);
 void intc_irq_xlate_set(unsigned int irq, intc_enum id, struct intc_desc_int *d);
diff --git a/drivers/sh/intc/irqdomain.c b/drivers/sh/intc/irqdomain.c
new file mode 100644
index 00000000000..3968f1c3c5c
--- /dev/null
+++ b/drivers/sh/intc/irqdomain.c
@@ -0,0 +1,68 @@
+/*
+ * IRQ domain support for SH INTC subsystem
+ *
+ * Copyright (C) 2012  Paul Mundt
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#define pr_fmt(fmt) "intc: " fmt
+
+#include <linux/irqdomain.h>
+#include <linux/sh_intc.h>
+#include <linux/export.h>
+#include "internals.h"
+
+/**
+ * intc_irq_domain_evt_xlate() - Generic xlate for vectored IRQs.
+ *
+ * This takes care of exception vector to hwirq translation through
+ * by way of evt2irq() translation.
+ *
+ * Note: For platforms that use a flat vector space without INTEVT this
+ * basically just mimics irq_domain_xlate_onecell() by way of a nopped
+ * out evt2irq() implementation.
+ */
+static int intc_evt_xlate(struct irq_domain *d, struct device_node *ctrlr,
+			  const u32 *intspec, unsigned int intsize,
+			  unsigned long *out_hwirq, unsigned int *out_type)
+{
+	if (WARN_ON(intsize < 1))
+		return -EINVAL;
+
+	*out_hwirq = evt2irq(intspec[0]);
+	*out_type = IRQ_TYPE_NONE;
+
+	return 0;
+}
+
+static const struct irq_domain_ops intc_evt_ops = {
+	.xlate		= intc_evt_xlate,
+};
+
+void __init intc_irq_domain_init(struct intc_desc_int *d,
+				 struct intc_hw_desc *hw)
+{
+	unsigned int irq_base, irq_end;
+
+	/*
+	 * Quick linear revmap check
+	 */
+	irq_base = evt2irq(hw->vectors[0].vect);
+	irq_end = evt2irq(hw->vectors[hw->nr_vectors - 1].vect);
+
+	/*
+	 * Linear domains have a hard-wired assertion that IRQs start at
+	 * 0 in order to make some performance optimizations. Lamely
+	 * restrict the linear case to these conditions here, taking the
+	 * tree penalty for linear cases with non-zero hwirq bases.
+	 */
+	if (irq_base == 0 && irq_end == (irq_base + hw->nr_vectors - 1))
+		d->domain = irq_domain_add_linear(NULL, hw->nr_vectors,
+						  &intc_evt_ops, NULL);
+	else
+		d->domain = irq_domain_add_tree(NULL, &intc_evt_ops, NULL);
+
+	BUG_ON(!d->domain);
+}
diff --git a/drivers/sh/intc/virq.c b/drivers/sh/intc/virq.c
index 93cec21e788..f30ac9354ff 100644
--- a/drivers/sh/intc/virq.c
+++ b/drivers/sh/intc/virq.c
@@ -219,12 +219,14 @@ restart:
 		if (radix_tree_deref_retry(entry))
 			goto restart;
 
-		irq = create_irq();
+		irq = irq_alloc_desc(numa_node_id());
 		if (unlikely(irq < 0)) {
 			pr_err("no more free IRQs, bailing..\n");
 			break;
 		}
 
+		activate_irq(irq);
+
 		pr_info("Setting up a chained VIRQ from %d -> %d\n",
 			irq, entry->pirq);
 
diff --git a/drivers/sh/pfc.c b/drivers/sh/pfc.c
deleted file mode 100644
index 522c6c46d1b..00000000000
--- a/drivers/sh/pfc.c
+++ /dev/null
@@ -1,739 +0,0 @@
-/*
- * Pinmuxed GPIO support for SuperH.
- *
- * Copyright (C) 2008 Magnus Damm
- *
- * This file is subject to the terms and conditions of the GNU General Public
- * License.  See the file "COPYING" in the main directory of this archive
- * for more details.
- */
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/module.h>
-#include <linux/clk.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/bitops.h>
-#include <linux/gpio.h>
-#include <linux/slab.h>
-#include <linux/ioport.h>
-
-static void pfc_iounmap(struct pinmux_info *pip)
-{
-	int k;
-
-	for (k = 0; k < pip->num_resources; k++)
-		if (pip->window[k].virt)
-			iounmap(pip->window[k].virt);
-
-	kfree(pip->window);
-	pip->window = NULL;
-}
-
-static int pfc_ioremap(struct pinmux_info *pip)
-{
-	struct resource *res;
-	int k;
-
-	if (!pip->num_resources)
-		return 0;
-
-	pip->window = kzalloc(pip->num_resources * sizeof(*pip->window),
-			      GFP_NOWAIT);
-	if (!pip->window)
-		goto err1;
-
-	for (k = 0; k < pip->num_resources; k++) {
-		res = pip->resource + k;
-		WARN_ON(resource_type(res) != IORESOURCE_MEM);
-		pip->window[k].phys = res->start;
-		pip->window[k].size = resource_size(res);
-		pip->window[k].virt = ioremap_nocache(res->start,
-							 resource_size(res));
-		if (!pip->window[k].virt)
-			goto err2;
-	}
-
-	return 0;
-
-err2:
-	pfc_iounmap(pip);
-err1:
-	return -1;
-}
-
-static void __iomem *pfc_phys_to_virt(struct pinmux_info *pip,
-				      unsigned long address)
-{
-	struct pfc_window *window;
-	int k;
-
-	/* scan through physical windows and convert address */
-	for (k = 0; k < pip->num_resources; k++) {
-		window = pip->window + k;
-
-		if (address < window->phys)
-			continue;
-
-		if (address >= (window->phys + window->size))
-			continue;
-
-		return window->virt + (address - window->phys);
-	}
-
-	/* no windows defined, register must be 1:1 mapped virt:phys */
-	return (void __iomem *)address;
-}
-
-static int enum_in_range(pinmux_enum_t enum_id, struct pinmux_range *r)
-{
-	if (enum_id < r->begin)
-		return 0;
-
-	if (enum_id > r->end)
-		return 0;
-
-	return 1;
-}
-
-static unsigned long gpio_read_raw_reg(void __iomem *mapped_reg,
-				       unsigned long reg_width)
-{
-	switch (reg_width) {
-	case 8:
-		return ioread8(mapped_reg);
-	case 16:
-		return ioread16(mapped_reg);
-	case 32:
-		return ioread32(mapped_reg);
-	}
-
-	BUG();
-	return 0;
-}
-
-static void gpio_write_raw_reg(void __iomem *mapped_reg,
-			       unsigned long reg_width,
-			       unsigned long data)
-{
-	switch (reg_width) {
-	case 8:
-		iowrite8(data, mapped_reg);
-		return;
-	case 16:
-		iowrite16(data, mapped_reg);
-		return;
-	case 32:
-		iowrite32(data, mapped_reg);
-		return;
-	}
-
-	BUG();
-}
-
-static int gpio_read_bit(struct pinmux_data_reg *dr,
-			 unsigned long in_pos)
-{
-	unsigned long pos;
-
-	pos = dr->reg_width - (in_pos + 1);
-
-	pr_debug("read_bit: addr = %lx, pos = %ld, "
-		 "r_width = %ld\n", dr->reg, pos, dr->reg_width);
-
-	return (gpio_read_raw_reg(dr->mapped_reg, dr->reg_width) >> pos) & 1;
-}
-
-static void gpio_write_bit(struct pinmux_data_reg *dr,
-			   unsigned long in_pos, unsigned long value)
-{
-	unsigned long pos;
-
-	pos = dr->reg_width - (in_pos + 1);
-
-	pr_debug("write_bit addr = %lx, value = %d, pos = %ld, "
-		 "r_width = %ld\n",
-		 dr->reg, !!value, pos, dr->reg_width);
-
-	if (value)
-		set_bit(pos, &dr->reg_shadow);
-	else
-		clear_bit(pos, &dr->reg_shadow);
-
-	gpio_write_raw_reg(dr->mapped_reg, dr->reg_width, dr->reg_shadow);
-}
-
-static void config_reg_helper(struct pinmux_info *gpioc,
-			      struct pinmux_cfg_reg *crp,
-			      unsigned long in_pos,
-			      void __iomem **mapped_regp,
-			      unsigned long *maskp,
-			      unsigned long *posp)
-{
-	int k;
-
-	*mapped_regp = pfc_phys_to_virt(gpioc, crp->reg);
-
-	if (crp->field_width) {
-		*maskp = (1 << crp->field_width) - 1;
-		*posp = crp->reg_width - ((in_pos + 1) * crp->field_width);
-	} else {
-		*maskp = (1 << crp->var_field_width[in_pos]) - 1;
-		*posp = crp->reg_width;
-		for (k = 0; k <= in_pos; k++)
-			*posp -= crp->var_field_width[k];
-	}
-}
-
-static int read_config_reg(struct pinmux_info *gpioc,
-			   struct pinmux_cfg_reg *crp,
-			   unsigned long field)
-{
-	void __iomem *mapped_reg;
-	unsigned long mask, pos;
-
-	config_reg_helper(gpioc, crp, field, &mapped_reg, &mask, &pos);
-
-	pr_debug("read_reg: addr = %lx, field = %ld, "
-		 "r_width = %ld, f_width = %ld\n",
-		 crp->reg, field, crp->reg_width, crp->field_width);
-
-	return (gpio_read_raw_reg(mapped_reg, crp->reg_width) >> pos) & mask;
-}
-
-static void write_config_reg(struct pinmux_info *gpioc,
-			     struct pinmux_cfg_reg *crp,
-			     unsigned long field, unsigned long value)
-{
-	void __iomem *mapped_reg;
-	unsigned long mask, pos, data;
-
-	config_reg_helper(gpioc, crp, field, &mapped_reg, &mask, &pos);
-
-	pr_debug("write_reg addr = %lx, value = %ld, field = %ld, "
-		 "r_width = %ld, f_width = %ld\n",
-		 crp->reg, value, field, crp->reg_width, crp->field_width);
-
-	mask = ~(mask << pos);
-	value = value << pos;
-
-	data = gpio_read_raw_reg(mapped_reg, crp->reg_width);
-	data &= mask;
-	data |= value;
-
-	if (gpioc->unlock_reg)
-		gpio_write_raw_reg(pfc_phys_to_virt(gpioc, gpioc->unlock_reg),
-				   32, ~data);
-
-	gpio_write_raw_reg(mapped_reg, crp->reg_width, data);
-}
-
-static int setup_data_reg(struct pinmux_info *gpioc, unsigned gpio)
-{
-	struct pinmux_gpio *gpiop = &gpioc->gpios[gpio];
-	struct pinmux_data_reg *data_reg;
-	int k, n;
-
-	if (!enum_in_range(gpiop->enum_id, &gpioc->data))
-		return -1;
-
-	k = 0;
-	while (1) {
-		data_reg = gpioc->data_regs + k;
-
-		if (!data_reg->reg_width)
-			break;
-
-		data_reg->mapped_reg = pfc_phys_to_virt(gpioc, data_reg->reg);
-
-		for (n = 0; n < data_reg->reg_width; n++) {
-			if (data_reg->enum_ids[n] == gpiop->enum_id) {
-				gpiop->flags &= ~PINMUX_FLAG_DREG;
-				gpiop->flags |= (k << PINMUX_FLAG_DREG_SHIFT);
-				gpiop->flags &= ~PINMUX_FLAG_DBIT;
-				gpiop->flags |= (n << PINMUX_FLAG_DBIT_SHIFT);
-				return 0;
-			}
-		}
-		k++;
-	}
-
-	BUG();
-
-	return -1;
-}
-
-static void setup_data_regs(struct pinmux_info *gpioc)
-{
-	struct pinmux_data_reg *drp;
-	int k;
-
-	for (k = gpioc->first_gpio; k <= gpioc->last_gpio; k++)
-		setup_data_reg(gpioc, k);
-
-	k = 0;
-	while (1) {
-		drp = gpioc->data_regs + k;
-
-		if (!drp->reg_width)
-			break;
-
-		drp->reg_shadow = gpio_read_raw_reg(drp->mapped_reg,
-						    drp->reg_width);
-		k++;
-	}
-}
-
-static int get_data_reg(struct pinmux_info *gpioc, unsigned gpio,
-			struct pinmux_data_reg **drp, int *bitp)
-{
-	struct pinmux_gpio *gpiop = &gpioc->gpios[gpio];
-	int k, n;
-
-	if (!enum_in_range(gpiop->enum_id, &gpioc->data))
-		return -1;
-
-	k = (gpiop->flags & PINMUX_FLAG_DREG) >> PINMUX_FLAG_DREG_SHIFT;
-	n = (gpiop->flags & PINMUX_FLAG_DBIT) >> PINMUX_FLAG_DBIT_SHIFT;
-	*drp = gpioc->data_regs + k;
-	*bitp = n;
-	return 0;
-}
-
-static int get_config_reg(struct pinmux_info *gpioc, pinmux_enum_t enum_id,
-			  struct pinmux_cfg_reg **crp,
-			  int *fieldp, int *valuep,
-			  unsigned long **cntp)
-{
-	struct pinmux_cfg_reg *config_reg;
-	unsigned long r_width, f_width, curr_width, ncomb;
-	int k, m, n, pos, bit_pos;
-
-	k = 0;
-	while (1) {
-		config_reg = gpioc->cfg_regs + k;
-
-		r_width = config_reg->reg_width;
-		f_width = config_reg->field_width;
-
-		if (!r_width)
-			break;
-
-		pos = 0;
-		m = 0;
-		for (bit_pos = 0; bit_pos < r_width; bit_pos += curr_width) {
-			if (f_width)
-				curr_width = f_width;
-			else
-				curr_width = config_reg->var_field_width[m];
-
-			ncomb = 1 << curr_width;
-			for (n = 0; n < ncomb; n++) {
-				if (config_reg->enum_ids[pos + n] == enum_id) {
-					*crp = config_reg;
-					*fieldp = m;
-					*valuep = n;
-					*cntp = &config_reg->cnt[m];
-					return 0;
-				}
-			}
-			pos += ncomb;
-			m++;
-		}
-		k++;
-	}
-
-	return -1;
-}
-
-static int get_gpio_enum_id(struct pinmux_info *gpioc, unsigned gpio,
-			    int pos, pinmux_enum_t *enum_idp)
-{
-	pinmux_enum_t enum_id = gpioc->gpios[gpio].enum_id;
-	pinmux_enum_t *data = gpioc->gpio_data;
-	int k;
-
-	if (!enum_in_range(enum_id, &gpioc->data)) {
-		if (!enum_in_range(enum_id, &gpioc->mark)) {
-			pr_err("non data/mark enum_id for gpio %d\n", gpio);
-			return -1;
-		}
-	}
-
-	if (pos) {
-		*enum_idp = data[pos + 1];
-		return pos + 1;
-	}
-
-	for (k = 0; k < gpioc->gpio_data_size; k++) {
-		if (data[k] == enum_id) {
-			*enum_idp = data[k + 1];
-			return k + 1;
-		}
-	}
-
-	pr_err("cannot locate data/mark enum_id for gpio %d\n", gpio);
-	return -1;
-}
-
-enum { GPIO_CFG_DRYRUN, GPIO_CFG_REQ, GPIO_CFG_FREE };
-
-static int pinmux_config_gpio(struct pinmux_info *gpioc, unsigned gpio,
-			      int pinmux_type, int cfg_mode)
-{
-	struct pinmux_cfg_reg *cr = NULL;
-	pinmux_enum_t enum_id;
-	struct pinmux_range *range;
-	int in_range, pos, field, value;
-	unsigned long *cntp;
-
-	switch (pinmux_type) {
-
-	case PINMUX_TYPE_FUNCTION:
-		range = NULL;
-		break;
-
-	case PINMUX_TYPE_OUTPUT:
-		range = &gpioc->output;
-		break;
-
-	case PINMUX_TYPE_INPUT:
-		range = &gpioc->input;
-		break;
-
-	case PINMUX_TYPE_INPUT_PULLUP:
-		range = &gpioc->input_pu;
-		break;
-
-	case PINMUX_TYPE_INPUT_PULLDOWN:
-		range = &gpioc->input_pd;
-		break;
-
-	default:
-		goto out_err;
-	}
-
-	pos = 0;
-	enum_id = 0;
-	field = 0;
-	value = 0;
-	while (1) {
-		pos = get_gpio_enum_id(gpioc, gpio, pos, &enum_id);
-		if (pos <= 0)
-			goto out_err;
-
-		if (!enum_id)
-			break;
-
-		/* first check if this is a function enum */
-		in_range = enum_in_range(enum_id, &gpioc->function);
-		if (!in_range) {
-			/* not a function enum */
-			if (range) {
-				/*
-				 * other range exists, so this pin is
-				 * a regular GPIO pin that now is being
-				 * bound to a specific direction.
-				 *
-				 * for this case we only allow function enums
-				 * and the enums that match the other range.
-				 */
-				in_range = enum_in_range(enum_id, range);
-
-				/*
-				 * special case pass through for fixed
-				 * input-only or output-only pins without
-				 * function enum register association.
-				 */
-				if (in_range && enum_id == range->force)
-					continue;
-			} else {
-				/*
-				 * no other range exists, so this pin
-				 * must then be of the function type.
-				 *
-				 * allow function type pins to select
-				 * any combination of function/in/out
-				 * in their MARK lists.
-				 */
-				in_range = 1;
-			}
-		}
-
-		if (!in_range)
-			continue;
-
-		if (get_config_reg(gpioc, enum_id, &cr,
-				   &field, &value, &cntp) != 0)
-			goto out_err;
-
-		switch (cfg_mode) {
-		case GPIO_CFG_DRYRUN:
-			if (!*cntp ||
-			    (read_config_reg(gpioc, cr, field) != value))
-				continue;
-			break;
-
-		case GPIO_CFG_REQ:
-			write_config_reg(gpioc, cr, field, value);
-			*cntp = *cntp + 1;
-			break;
-
-		case GPIO_CFG_FREE:
-			*cntp = *cntp - 1;
-			break;
-		}
-	}
-
-	return 0;
- out_err:
-	return -1;
-}
-
-static DEFINE_SPINLOCK(gpio_lock);
-
-static struct pinmux_info *chip_to_pinmux(struct gpio_chip *chip)
-{
-	return container_of(chip, struct pinmux_info, chip);
-}
-
-static int sh_gpio_request(struct gpio_chip *chip, unsigned offset)
-{
-	struct pinmux_info *gpioc = chip_to_pinmux(chip);
-	struct pinmux_data_reg *dummy;
-	unsigned long flags;
-	int i, ret, pinmux_type;
-
-	ret = -EINVAL;
-
-	if (!gpioc)
-		goto err_out;
-
-	spin_lock_irqsave(&gpio_lock, flags);
-
-	if ((gpioc->gpios[offset].flags & PINMUX_FLAG_TYPE) != PINMUX_TYPE_NONE)
-		goto err_unlock;
-
-	/* setup pin function here if no data is associated with pin */
-
-	if (get_data_reg(gpioc, offset, &dummy, &i) != 0)
-		pinmux_type = PINMUX_TYPE_FUNCTION;
-	else
-		pinmux_type = PINMUX_TYPE_GPIO;
-
-	if (pinmux_type == PINMUX_TYPE_FUNCTION) {
-		if (pinmux_config_gpio(gpioc, offset,
-				       pinmux_type,
-				       GPIO_CFG_DRYRUN) != 0)
-			goto err_unlock;
-
-		if (pinmux_config_gpio(gpioc, offset,
-				       pinmux_type,
-				       GPIO_CFG_REQ) != 0)
-			BUG();
-	}
-
-	gpioc->gpios[offset].flags &= ~PINMUX_FLAG_TYPE;
-	gpioc->gpios[offset].flags |= pinmux_type;
-
-	ret = 0;
- err_unlock:
-	spin_unlock_irqrestore(&gpio_lock, flags);
- err_out:
-	return ret;
-}
-
-static void sh_gpio_free(struct gpio_chip *chip, unsigned offset)
-{
-	struct pinmux_info *gpioc = chip_to_pinmux(chip);
-	unsigned long flags;
-	int pinmux_type;
-
-	if (!gpioc)
-		return;
-
-	spin_lock_irqsave(&gpio_lock, flags);
-
-	pinmux_type = gpioc->gpios[offset].flags & PINMUX_FLAG_TYPE;
-	pinmux_config_gpio(gpioc, offset, pinmux_type, GPIO_CFG_FREE);
-	gpioc->gpios[offset].flags &= ~PINMUX_FLAG_TYPE;
-	gpioc->gpios[offset].flags |= PINMUX_TYPE_NONE;
-
-	spin_unlock_irqrestore(&gpio_lock, flags);
-}
-
-static int pinmux_direction(struct pinmux_info *gpioc,
-			    unsigned gpio, int new_pinmux_type)
-{
-	int pinmux_type;
-	int ret = -EINVAL;
-
-	if (!gpioc)
-		goto err_out;
-
-	pinmux_type = gpioc->gpios[gpio].flags & PINMUX_FLAG_TYPE;
-
-	switch (pinmux_type) {
-	case PINMUX_TYPE_GPIO:
-		break;
-	case PINMUX_TYPE_OUTPUT:
-	case PINMUX_TYPE_INPUT:
-	case PINMUX_TYPE_INPUT_PULLUP:
-	case PINMUX_TYPE_INPUT_PULLDOWN:
-		pinmux_config_gpio(gpioc, gpio, pinmux_type, GPIO_CFG_FREE);
-		break;
-	default:
-		goto err_out;
-	}
-
-	if (pinmux_config_gpio(gpioc, gpio,
-			       new_pinmux_type,
-			       GPIO_CFG_DRYRUN) != 0)
-		goto err_out;
-
-	if (pinmux_config_gpio(gpioc, gpio,
-			       new_pinmux_type,
-			       GPIO_CFG_REQ) != 0)
-		BUG();
-
-	gpioc->gpios[gpio].flags &= ~PINMUX_FLAG_TYPE;
-	gpioc->gpios[gpio].flags |= new_pinmux_type;
-
-	ret = 0;
- err_out:
-	return ret;
-}
-
-static int sh_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
-{
-	struct pinmux_info *gpioc = chip_to_pinmux(chip);
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&gpio_lock, flags);
-	ret = pinmux_direction(gpioc, offset, PINMUX_TYPE_INPUT);
-	spin_unlock_irqrestore(&gpio_lock, flags);
-
-	return ret;
-}
-
-static void sh_gpio_set_value(struct pinmux_info *gpioc,
-			     unsigned gpio, int value)
-{
-	struct pinmux_data_reg *dr = NULL;
-	int bit = 0;
-
-	if (!gpioc || get_data_reg(gpioc, gpio, &dr, &bit) != 0)
-		BUG();
-	else
-		gpio_write_bit(dr, bit, value);
-}
-
-static int sh_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
-				    int value)
-{
-	struct pinmux_info *gpioc = chip_to_pinmux(chip);
-	unsigned long flags;
-	int ret;
-
-	sh_gpio_set_value(gpioc, offset, value);
-	spin_lock_irqsave(&gpio_lock, flags);
-	ret = pinmux_direction(gpioc, offset, PINMUX_TYPE_OUTPUT);
-	spin_unlock_irqrestore(&gpio_lock, flags);
-
-	return ret;
-}
-
-static int sh_gpio_get_value(struct pinmux_info *gpioc, unsigned gpio)
-{
-	struct pinmux_data_reg *dr = NULL;
-	int bit = 0;
-
-	if (!gpioc || get_data_reg(gpioc, gpio, &dr, &bit) != 0)
-		return -EINVAL;
-
-	return gpio_read_bit(dr, bit);
-}
-
-static int sh_gpio_get(struct gpio_chip *chip, unsigned offset)
-{
-	return sh_gpio_get_value(chip_to_pinmux(chip), offset);
-}
-
-static void sh_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
-{
-	sh_gpio_set_value(chip_to_pinmux(chip), offset, value);
-}
-
-static int sh_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct pinmux_info *gpioc = chip_to_pinmux(chip);
-	pinmux_enum_t enum_id;
-	pinmux_enum_t *enum_ids;
-	int i, k, pos;
-
-	pos = 0;
-	enum_id = 0;
-	while (1) {
-		pos = get_gpio_enum_id(gpioc, offset, pos, &enum_id);
-		if (pos <= 0 || !enum_id)
-			break;
-
-		for (i = 0; i < gpioc->gpio_irq_size; i++) {
-			enum_ids = gpioc->gpio_irq[i].enum_ids;
-			for (k = 0; enum_ids[k]; k++) {
-				if (enum_ids[k] == enum_id)
-					return gpioc->gpio_irq[i].irq;
-			}
-		}
-	}
-
-	return -ENOSYS;
-}
-
-int register_pinmux(struct pinmux_info *pip)
-{
-	struct gpio_chip *chip = &pip->chip;
-	int ret;
-
-	pr_info("%s handling gpio %d -> %d\n",
-		pip->name, pip->first_gpio, pip->last_gpio);
-
-	ret = pfc_ioremap(pip);
-	if (ret < 0)
-		return ret;
-
-	setup_data_regs(pip);
-
-	chip->request = sh_gpio_request;
-	chip->free = sh_gpio_free;
-	chip->direction_input = sh_gpio_direction_input;
-	chip->get = sh_gpio_get;
-	chip->direction_output = sh_gpio_direction_output;
-	chip->set = sh_gpio_set;
-	chip->to_irq = sh_gpio_to_irq;
-
-	WARN_ON(pip->first_gpio != 0); /* needs testing */
-
-	chip->label = pip->name;
-	chip->owner = THIS_MODULE;
-	chip->base = pip->first_gpio;
-	chip->ngpio = (pip->last_gpio - pip->first_gpio) + 1;
-
-	ret = gpiochip_add(chip);
-	if (ret < 0)
-		pfc_iounmap(pip);
-
-	return ret;
-}
-
-int unregister_pinmux(struct pinmux_info *pip)
-{
-	pr_info("%s deregistering\n", pip->name);
-	pfc_iounmap(pip);
-	return gpiochip_remove(&pip->chip);
-}
diff --git a/drivers/sh/pfc/Kconfig b/drivers/sh/pfc/Kconfig
new file mode 100644
index 00000000000..804f9ad1bf4
--- /dev/null
+++ b/drivers/sh/pfc/Kconfig
@@ -0,0 +1,26 @@
+comment "Pin function controller options"
+
+config SH_PFC
+	# XXX move off the gpio dependency
+	depends on GENERIC_GPIO
+	select GPIO_SH_PFC if ARCH_REQUIRE_GPIOLIB
+	select PINCTRL_SH_PFC
+	def_bool y
+
+#
+# Placeholder for now, rehome to drivers/pinctrl once the PFC APIs
+# have settled.
+#
+config PINCTRL_SH_PFC
+	tristate "SuperH PFC pin controller driver"
+	depends on SH_PFC
+	select PINCTRL
+	select PINMUX
+	select PINCONF
+
+config GPIO_SH_PFC
+	tristate "SuperH PFC GPIO support"
+	depends on SH_PFC && GPIOLIB
+	help
+	  This enables support for GPIOs within the SoC's pin function
+	  controller.
diff --git a/drivers/sh/pfc/Makefile b/drivers/sh/pfc/Makefile
new file mode 100644
index 00000000000..7916027cce3
--- /dev/null
+++ b/drivers/sh/pfc/Makefile
@@ -0,0 +1,3 @@
+obj-y				+= core.o
+obj-$(CONFIG_PINCTRL_SH_PFC)	+= pinctrl.o
+obj-$(CONFIG_GPIO_SH_PFC)	+= gpio.o
diff --git a/drivers/sh/pfc/core.c b/drivers/sh/pfc/core.c
new file mode 100644
index 00000000000..68169373c98
--- /dev/null
+++ b/drivers/sh/pfc/core.c
@@ -0,0 +1,572 @@
+/*
+ * SuperH Pin Function Controller support.
+ *
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2009 - 2012 Paul Mundt
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#define pr_fmt(fmt) "sh_pfc " KBUILD_MODNAME ": " fmt
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sh_pfc.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/ioport.h>
+#include <linux/pinctrl/machine.h>
+
+static struct sh_pfc *sh_pfc __read_mostly;
+
+static inline bool sh_pfc_initialized(void)
+{
+	return !!sh_pfc;
+}
+
+static void pfc_iounmap(struct sh_pfc *pfc)
+{
+	int k;
+
+	for (k = 0; k < pfc->num_resources; k++)
+		if (pfc->window[k].virt)
+			iounmap(pfc->window[k].virt);
+
+	kfree(pfc->window);
+	pfc->window = NULL;
+}
+
+static int pfc_ioremap(struct sh_pfc *pfc)
+{
+	struct resource *res;
+	int k;
+
+	if (!pfc->num_resources)
+		return 0;
+
+	pfc->window = kzalloc(pfc->num_resources * sizeof(*pfc->window),
+			      GFP_NOWAIT);
+	if (!pfc->window)
+		goto err1;
+
+	for (k = 0; k < pfc->num_resources; k++) {
+		res = pfc->resource + k;
+		WARN_ON(resource_type(res) != IORESOURCE_MEM);
+		pfc->window[k].phys = res->start;
+		pfc->window[k].size = resource_size(res);
+		pfc->window[k].virt = ioremap_nocache(res->start,
+							 resource_size(res));
+		if (!pfc->window[k].virt)
+			goto err2;
+	}
+
+	return 0;
+
+err2:
+	pfc_iounmap(pfc);
+err1:
+	return -1;
+}
+
+static void __iomem *pfc_phys_to_virt(struct sh_pfc *pfc,
+				      unsigned long address)
+{
+	struct pfc_window *window;
+	int k;
+
+	/* scan through physical windows and convert address */
+	for (k = 0; k < pfc->num_resources; k++) {
+		window = pfc->window + k;
+
+		if (address < window->phys)
+			continue;
+
+		if (address >= (window->phys + window->size))
+			continue;
+
+		return window->virt + (address - window->phys);
+	}
+
+	/* no windows defined, register must be 1:1 mapped virt:phys */
+	return (void __iomem *)address;
+}
+
+static int enum_in_range(pinmux_enum_t enum_id, struct pinmux_range *r)
+{
+	if (enum_id < r->begin)
+		return 0;
+
+	if (enum_id > r->end)
+		return 0;
+
+	return 1;
+}
+
+static unsigned long gpio_read_raw_reg(void __iomem *mapped_reg,
+				       unsigned long reg_width)
+{
+	switch (reg_width) {
+	case 8:
+		return ioread8(mapped_reg);
+	case 16:
+		return ioread16(mapped_reg);
+	case 32:
+		return ioread32(mapped_reg);
+	}
+
+	BUG();
+	return 0;
+}
+
+static void gpio_write_raw_reg(void __iomem *mapped_reg,
+			       unsigned long reg_width,
+			       unsigned long data)
+{
+	switch (reg_width) {
+	case 8:
+		iowrite8(data, mapped_reg);
+		return;
+	case 16:
+		iowrite16(data, mapped_reg);
+		return;
+	case 32:
+		iowrite32(data, mapped_reg);
+		return;
+	}
+
+	BUG();
+}
+
+int sh_pfc_read_bit(struct pinmux_data_reg *dr, unsigned long in_pos)
+{
+	unsigned long pos;
+
+	pos = dr->reg_width - (in_pos + 1);
+
+	pr_debug("read_bit: addr = %lx, pos = %ld, "
+		 "r_width = %ld\n", dr->reg, pos, dr->reg_width);
+
+	return (gpio_read_raw_reg(dr->mapped_reg, dr->reg_width) >> pos) & 1;
+}
+EXPORT_SYMBOL_GPL(sh_pfc_read_bit);
+
+void sh_pfc_write_bit(struct pinmux_data_reg *dr, unsigned long in_pos,
+		      unsigned long value)
+{
+	unsigned long pos;
+
+	pos = dr->reg_width - (in_pos + 1);
+
+	pr_debug("write_bit addr = %lx, value = %d, pos = %ld, "
+		 "r_width = %ld\n",
+		 dr->reg, !!value, pos, dr->reg_width);
+
+	if (value)
+		set_bit(pos, &dr->reg_shadow);
+	else
+		clear_bit(pos, &dr->reg_shadow);
+
+	gpio_write_raw_reg(dr->mapped_reg, dr->reg_width, dr->reg_shadow);
+}
+EXPORT_SYMBOL_GPL(sh_pfc_write_bit);
+
+static void config_reg_helper(struct sh_pfc *pfc,
+			      struct pinmux_cfg_reg *crp,
+			      unsigned long in_pos,
+			      void __iomem **mapped_regp,
+			      unsigned long *maskp,
+			      unsigned long *posp)
+{
+	int k;
+
+	*mapped_regp = pfc_phys_to_virt(pfc, crp->reg);
+
+	if (crp->field_width) {
+		*maskp = (1 << crp->field_width) - 1;
+		*posp = crp->reg_width - ((in_pos + 1) * crp->field_width);
+	} else {
+		*maskp = (1 << crp->var_field_width[in_pos]) - 1;
+		*posp = crp->reg_width;
+		for (k = 0; k <= in_pos; k++)
+			*posp -= crp->var_field_width[k];
+	}
+}
+
+static int read_config_reg(struct sh_pfc *pfc,
+			   struct pinmux_cfg_reg *crp,
+			   unsigned long field)
+{
+	void __iomem *mapped_reg;
+	unsigned long mask, pos;
+
+	config_reg_helper(pfc, crp, field, &mapped_reg, &mask, &pos);
+
+	pr_debug("read_reg: addr = %lx, field = %ld, "
+		 "r_width = %ld, f_width = %ld\n",
+		 crp->reg, field, crp->reg_width, crp->field_width);
+
+	return (gpio_read_raw_reg(mapped_reg, crp->reg_width) >> pos) & mask;
+}
+
+static void write_config_reg(struct sh_pfc *pfc,
+			     struct pinmux_cfg_reg *crp,
+			     unsigned long field, unsigned long value)
+{
+	void __iomem *mapped_reg;
+	unsigned long mask, pos, data;
+
+	config_reg_helper(pfc, crp, field, &mapped_reg, &mask, &pos);
+
+	pr_debug("write_reg addr = %lx, value = %ld, field = %ld, "
+		 "r_width = %ld, f_width = %ld\n",
+		 crp->reg, value, field, crp->reg_width, crp->field_width);
+
+	mask = ~(mask << pos);
+	value = value << pos;
+
+	data = gpio_read_raw_reg(mapped_reg, crp->reg_width);
+	data &= mask;
+	data |= value;
+
+	if (pfc->unlock_reg)
+		gpio_write_raw_reg(pfc_phys_to_virt(pfc, pfc->unlock_reg),
+				   32, ~data);
+
+	gpio_write_raw_reg(mapped_reg, crp->reg_width, data);
+}
+
+static int setup_data_reg(struct sh_pfc *pfc, unsigned gpio)
+{
+	struct pinmux_gpio *gpiop = &pfc->gpios[gpio];
+	struct pinmux_data_reg *data_reg;
+	int k, n;
+
+	if (!enum_in_range(gpiop->enum_id, &pfc->data))
+		return -1;
+
+	k = 0;
+	while (1) {
+		data_reg = pfc->data_regs + k;
+
+		if (!data_reg->reg_width)
+			break;
+
+		data_reg->mapped_reg = pfc_phys_to_virt(pfc, data_reg->reg);
+
+		for (n = 0; n < data_reg->reg_width; n++) {
+			if (data_reg->enum_ids[n] == gpiop->enum_id) {
+				gpiop->flags &= ~PINMUX_FLAG_DREG;
+				gpiop->flags |= (k << PINMUX_FLAG_DREG_SHIFT);
+				gpiop->flags &= ~PINMUX_FLAG_DBIT;
+				gpiop->flags |= (n << PINMUX_FLAG_DBIT_SHIFT);
+				return 0;
+			}
+		}
+		k++;
+	}
+
+	BUG();
+
+	return -1;
+}
+
+static void setup_data_regs(struct sh_pfc *pfc)
+{
+	struct pinmux_data_reg *drp;
+	int k;
+
+	for (k = pfc->first_gpio; k <= pfc->last_gpio; k++)
+		setup_data_reg(pfc, k);
+
+	k = 0;
+	while (1) {
+		drp = pfc->data_regs + k;
+
+		if (!drp->reg_width)
+			break;
+
+		drp->reg_shadow = gpio_read_raw_reg(drp->mapped_reg,
+						    drp->reg_width);
+		k++;
+	}
+}
+
+int sh_pfc_get_data_reg(struct sh_pfc *pfc, unsigned gpio,
+			struct pinmux_data_reg **drp, int *bitp)
+{
+	struct pinmux_gpio *gpiop = &pfc->gpios[gpio];
+	int k, n;
+
+	if (!enum_in_range(gpiop->enum_id, &pfc->data))
+		return -1;
+
+	k = (gpiop->flags & PINMUX_FLAG_DREG) >> PINMUX_FLAG_DREG_SHIFT;
+	n = (gpiop->flags & PINMUX_FLAG_DBIT) >> PINMUX_FLAG_DBIT_SHIFT;
+	*drp = pfc->data_regs + k;
+	*bitp = n;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(sh_pfc_get_data_reg);
+
+static int get_config_reg(struct sh_pfc *pfc, pinmux_enum_t enum_id,
+			  struct pinmux_cfg_reg **crp,
+			  int *fieldp, int *valuep,
+			  unsigned long **cntp)
+{
+	struct pinmux_cfg_reg *config_reg;
+	unsigned long r_width, f_width, curr_width, ncomb;
+	int k, m, n, pos, bit_pos;
+
+	k = 0;
+	while (1) {
+		config_reg = pfc->cfg_regs + k;
+
+		r_width = config_reg->reg_width;
+		f_width = config_reg->field_width;
+
+		if (!r_width)
+			break;
+
+		pos = 0;
+		m = 0;
+		for (bit_pos = 0; bit_pos < r_width; bit_pos += curr_width) {
+			if (f_width)
+				curr_width = f_width;
+			else
+				curr_width = config_reg->var_field_width[m];
+
+			ncomb = 1 << curr_width;
+			for (n = 0; n < ncomb; n++) {
+				if (config_reg->enum_ids[pos + n] == enum_id) {
+					*crp = config_reg;
+					*fieldp = m;
+					*valuep = n;
+					*cntp = &config_reg->cnt[m];
+					return 0;
+				}
+			}
+			pos += ncomb;
+			m++;
+		}
+		k++;
+	}
+
+	return -1;
+}
+
+int sh_pfc_gpio_to_enum(struct sh_pfc *pfc, unsigned gpio, int pos,
+			pinmux_enum_t *enum_idp)
+{
+	pinmux_enum_t enum_id = pfc->gpios[gpio].enum_id;
+	pinmux_enum_t *data = pfc->gpio_data;
+	int k;
+
+	if (!enum_in_range(enum_id, &pfc->data)) {
+		if (!enum_in_range(enum_id, &pfc->mark)) {
+			pr_err("non data/mark enum_id for gpio %d\n", gpio);
+			return -1;
+		}
+	}
+
+	if (pos) {
+		*enum_idp = data[pos + 1];
+		return pos + 1;
+	}
+
+	for (k = 0; k < pfc->gpio_data_size; k++) {
+		if (data[k] == enum_id) {
+			*enum_idp = data[k + 1];
+			return k + 1;
+		}
+	}
+
+	pr_err("cannot locate data/mark enum_id for gpio %d\n", gpio);
+	return -1;
+}
+EXPORT_SYMBOL_GPL(sh_pfc_gpio_to_enum);
+
+int sh_pfc_config_gpio(struct sh_pfc *pfc, unsigned gpio, int pinmux_type,
+		       int cfg_mode)
+{
+	struct pinmux_cfg_reg *cr = NULL;
+	pinmux_enum_t enum_id;
+	struct pinmux_range *range;
+	int in_range, pos, field, value;
+	unsigned long *cntp;
+
+	switch (pinmux_type) {
+
+	case PINMUX_TYPE_FUNCTION:
+		range = NULL;
+		break;
+
+	case PINMUX_TYPE_OUTPUT:
+		range = &pfc->output;
+		break;
+
+	case PINMUX_TYPE_INPUT:
+		range = &pfc->input;
+		break;
+
+	case PINMUX_TYPE_INPUT_PULLUP:
+		range = &pfc->input_pu;
+		break;
+
+	case PINMUX_TYPE_INPUT_PULLDOWN:
+		range = &pfc->input_pd;
+		break;
+
+	default:
+		goto out_err;
+	}
+
+	pos = 0;
+	enum_id = 0;
+	field = 0;
+	value = 0;
+	while (1) {
+		pos = sh_pfc_gpio_to_enum(pfc, gpio, pos, &enum_id);
+		if (pos <= 0)
+			goto out_err;
+
+		if (!enum_id)
+			break;
+
+		/* first check if this is a function enum */
+		in_range = enum_in_range(enum_id, &pfc->function);
+		if (!in_range) {
+			/* not a function enum */
+			if (range) {
+				/*
+				 * other range exists, so this pin is
+				 * a regular GPIO pin that now is being
+				 * bound to a specific direction.
+				 *
+				 * for this case we only allow function enums
+				 * and the enums that match the other range.
+				 */
+				in_range = enum_in_range(enum_id, range);
+
+				/*
+				 * special case pass through for fixed
+				 * input-only or output-only pins without
+				 * function enum register association.
+				 */
+				if (in_range && enum_id == range->force)
+					continue;
+			} else {
+				/*
+				 * no other range exists, so this pin
+				 * must then be of the function type.
+				 *
+				 * allow function type pins to select
+				 * any combination of function/in/out
+				 * in their MARK lists.
+				 */
+				in_range = 1;
+			}
+		}
+
+		if (!in_range)
+			continue;
+
+		if (get_config_reg(pfc, enum_id, &cr,
+				   &field, &value, &cntp) != 0)
+			goto out_err;
+
+		switch (cfg_mode) {
+		case GPIO_CFG_DRYRUN:
+			if (!*cntp ||
+			    (read_config_reg(pfc, cr, field) != value))
+				continue;
+			break;
+
+		case GPIO_CFG_REQ:
+			write_config_reg(pfc, cr, field, value);
+			*cntp = *cntp + 1;
+			break;
+
+		case GPIO_CFG_FREE:
+			*cntp = *cntp - 1;
+			break;
+		}
+	}
+
+	return 0;
+ out_err:
+	return -1;
+}
+EXPORT_SYMBOL_GPL(sh_pfc_config_gpio);
+
+int register_sh_pfc(struct sh_pfc *pfc)
+{
+	int (*initroutine)(struct sh_pfc *) = NULL;
+	int ret;
+
+	/*
+	 * Ensure that the type encoding fits
+	 */
+	BUILD_BUG_ON(PINMUX_FLAG_TYPE > ((1 << PINMUX_FLAG_DBIT_SHIFT) - 1));
+
+	if (sh_pfc)
+		return -EBUSY;
+
+	ret = pfc_ioremap(pfc);
+	if (unlikely(ret < 0))
+		return ret;
+
+	spin_lock_init(&pfc->lock);
+
+	pinctrl_provide_dummies();
+	setup_data_regs(pfc);
+
+	sh_pfc = pfc;
+
+	/*
+	 * Initialize pinctrl bindings first
+	 */
+	initroutine = symbol_request(sh_pfc_register_pinctrl);
+	if (initroutine) {
+		ret = (*initroutine)(pfc);
+		symbol_put_addr(initroutine);
+
+		if (unlikely(ret != 0))
+			goto err;
+	} else {
+		pr_err("failed to initialize pinctrl bindings\n");
+		goto err;
+	}
+
+	/*
+	 * Then the GPIO chip
+	 */
+	initroutine = symbol_request(sh_pfc_register_gpiochip);
+	if (initroutine) {
+		ret = (*initroutine)(pfc);
+		symbol_put_addr(initroutine);
+
+		/*
+		 * If the GPIO chip fails to come up we still leave the
+		 * PFC state as it is, given that there are already
+		 * extant users of it that have succeeded by this point.
+		 */
+		if (unlikely(ret != 0)) {
+			pr_notice("failed to init GPIO chip, ignoring...\n");
+			ret = 0;
+		}
+	}
+
+	pr_info("%s support registered\n", pfc->name);
+
+	return 0;
+
+err:
+	pfc_iounmap(pfc);
+	sh_pfc = NULL;
+
+	return ret;
+}
diff --git a/drivers/sh/pfc/gpio.c b/drivers/sh/pfc/gpio.c
new file mode 100644
index 00000000000..62bca98474a
--- /dev/null
+++ b/drivers/sh/pfc/gpio.c
@@ -0,0 +1,239 @@
+/*
+ * SuperH Pin Function Controller GPIO driver.
+ *
+ * Copyright (C) 2008 Magnus Damm
+ * Copyright (C) 2009 - 2012 Paul Mundt
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#define pr_fmt(fmt) "sh_pfc " KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/consumer.h>
+
+struct sh_pfc_chip {
+	struct sh_pfc		*pfc;
+	struct gpio_chip	gpio_chip;
+};
+
+static struct sh_pfc_chip *gpio_to_pfc_chip(struct gpio_chip *gc)
+{
+	return container_of(gc, struct sh_pfc_chip, gpio_chip);
+}
+
+static struct sh_pfc *gpio_to_pfc(struct gpio_chip *gc)
+{
+	return gpio_to_pfc_chip(gc)->pfc;
+}
+
+static int sh_gpio_request(struct gpio_chip *gc, unsigned offset)
+{
+	return pinctrl_request_gpio(offset);
+}
+
+static void sh_gpio_free(struct gpio_chip *gc, unsigned offset)
+{
+	pinctrl_free_gpio(offset);
+}
+
+static void sh_gpio_set_value(struct sh_pfc *pfc, unsigned gpio, int value)
+{
+	struct pinmux_data_reg *dr = NULL;
+	int bit = 0;
+
+	if (!pfc || sh_pfc_get_data_reg(pfc, gpio, &dr, &bit) != 0)
+		BUG();
+	else
+		sh_pfc_write_bit(dr, bit, value);
+}
+
+static int sh_gpio_get_value(struct sh_pfc *pfc, unsigned gpio)
+{
+	struct pinmux_data_reg *dr = NULL;
+	int bit = 0;
+
+	if (!pfc || sh_pfc_get_data_reg(pfc, gpio, &dr, &bit) != 0)
+		return -EINVAL;
+
+	return sh_pfc_read_bit(dr, bit);
+}
+
+static int sh_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
+{
+	return pinctrl_gpio_direction_input(offset);
+}
+
+static int sh_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
+				    int value)
+{
+	sh_gpio_set_value(gpio_to_pfc(gc), offset, value);
+
+	return pinctrl_gpio_direction_output(offset);
+}
+
+static int sh_gpio_get(struct gpio_chip *gc, unsigned offset)
+{
+	return sh_gpio_get_value(gpio_to_pfc(gc), offset);
+}
+
+static void sh_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
+{
+	sh_gpio_set_value(gpio_to_pfc(gc), offset, value);
+}
+
+static int sh_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct sh_pfc *pfc = gpio_to_pfc(gc);
+	pinmux_enum_t enum_id;
+	pinmux_enum_t *enum_ids;
+	int i, k, pos;
+
+	pos = 0;
+	enum_id = 0;
+	while (1) {
+		pos = sh_pfc_gpio_to_enum(pfc, offset, pos, &enum_id);
+		if (pos <= 0 || !enum_id)
+			break;
+
+		for (i = 0; i < pfc->gpio_irq_size; i++) {
+			enum_ids = pfc->gpio_irq[i].enum_ids;
+			for (k = 0; enum_ids[k]; k++) {
+				if (enum_ids[k] == enum_id)
+					return pfc->gpio_irq[i].irq;
+			}
+		}
+	}
+
+	return -ENOSYS;
+}
+
+static void sh_pfc_gpio_setup(struct sh_pfc_chip *chip)
+{
+	struct sh_pfc *pfc = chip->pfc;
+	struct gpio_chip *gc = &chip->gpio_chip;
+
+	gc->request = sh_gpio_request;
+	gc->free = sh_gpio_free;
+	gc->direction_input = sh_gpio_direction_input;
+	gc->get = sh_gpio_get;
+	gc->direction_output = sh_gpio_direction_output;
+	gc->set = sh_gpio_set;
+	gc->to_irq = sh_gpio_to_irq;
+
+	WARN_ON(pfc->first_gpio != 0); /* needs testing */
+
+	gc->label = pfc->name;
+	gc->owner = THIS_MODULE;
+	gc->base = pfc->first_gpio;
+	gc->ngpio = (pfc->last_gpio - pfc->first_gpio) + 1;
+}
+
+int sh_pfc_register_gpiochip(struct sh_pfc *pfc)
+{
+	struct sh_pfc_chip *chip;
+	int ret;
+
+	chip = kzalloc(sizeof(struct sh_pfc_chip), GFP_KERNEL);
+	if (unlikely(!chip))
+		return -ENOMEM;
+
+	chip->pfc = pfc;
+
+	sh_pfc_gpio_setup(chip);
+
+	ret = gpiochip_add(&chip->gpio_chip);
+	if (unlikely(ret < 0))
+		kfree(chip);
+
+	pr_info("%s handling gpio %d -> %d\n",
+		pfc->name, pfc->first_gpio, pfc->last_gpio);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(sh_pfc_register_gpiochip);
+
+static int sh_pfc_gpio_match(struct gpio_chip *gc, void *data)
+{
+	return !!strstr(gc->label, data);
+}
+
+static int __devinit sh_pfc_gpio_probe(struct platform_device *pdev)
+{
+	struct sh_pfc_chip *chip;
+	struct gpio_chip *gc;
+
+	gc = gpiochip_find("_pfc", sh_pfc_gpio_match);
+	if (unlikely(!gc)) {
+		pr_err("Cant find gpio chip\n");
+		return -ENODEV;
+	}
+
+	chip = gpio_to_pfc_chip(gc);
+	platform_set_drvdata(pdev, chip);
+
+	pr_info("attaching to GPIO chip %s\n", chip->pfc->name);
+
+	return 0;
+}
+
+static int __devexit sh_pfc_gpio_remove(struct platform_device *pdev)
+{
+	struct sh_pfc_chip *chip = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = gpiochip_remove(&chip->gpio_chip);
+	if (unlikely(ret < 0))
+		return ret;
+
+	kfree(chip);
+	return 0;
+}
+
+static struct platform_driver sh_pfc_gpio_driver = {
+	.probe		= sh_pfc_gpio_probe,
+	.remove		= __devexit_p(sh_pfc_gpio_remove),
+	.driver		= {
+		.name	= KBUILD_MODNAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static struct platform_device sh_pfc_gpio_device = {
+	.name		= KBUILD_MODNAME,
+	.id		= -1,
+};
+
+static int __init sh_pfc_gpio_init(void)
+{
+	int rc;
+
+	rc = platform_driver_register(&sh_pfc_gpio_driver);
+	if (likely(!rc)) {
+		rc = platform_device_register(&sh_pfc_gpio_device);
+		if (unlikely(rc))
+			platform_driver_unregister(&sh_pfc_gpio_driver);
+	}
+
+	return rc;
+}
+
+static void __exit sh_pfc_gpio_exit(void)
+{
+	platform_device_unregister(&sh_pfc_gpio_device);
+	platform_driver_unregister(&sh_pfc_gpio_driver);
+}
+
+module_init(sh_pfc_gpio_init);
+module_exit(sh_pfc_gpio_exit);
+
+MODULE_AUTHOR("Magnus Damm, Paul Mundt");
+MODULE_DESCRIPTION("GPIO driver for SuperH pin function controller");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:pfc-gpio");
diff --git a/drivers/sh/pfc/pinctrl.c b/drivers/sh/pfc/pinctrl.c
new file mode 100644
index 00000000000..2804eaae804
--- /dev/null
+++ b/drivers/sh/pfc/pinctrl.c
@@ -0,0 +1,526 @@
+/*
+ * SuperH Pin Function Controller pinmux support.
+ *
+ * Copyright (C) 2012  Paul Mundt
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#define DRV_NAME "pinctrl-sh_pfc"
+
+#define pr_fmt(fmt) DRV_NAME " " KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/sh_pfc.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf-generic.h>
+
+struct sh_pfc_pinctrl {
+	struct pinctrl_dev *pctl;
+	struct sh_pfc *pfc;
+
+	struct pinmux_gpio **functions;
+	unsigned int nr_functions;
+
+	struct pinctrl_pin_desc *pads;
+	unsigned int nr_pads;
+
+	spinlock_t lock;
+};
+
+static struct sh_pfc_pinctrl *sh_pfc_pmx;
+
+static int sh_pfc_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->nr_pads;
+}
+
+static const char *sh_pfc_get_group_name(struct pinctrl_dev *pctldev,
+					 unsigned selector)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->pads[selector].name;
+}
+
+static int sh_pfc_get_group_pins(struct pinctrl_dev *pctldev, unsigned group,
+				 const unsigned **pins, unsigned *num_pins)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = &pmx->pads[group].number;
+	*num_pins = 1;
+
+	return 0;
+}
+
+static void sh_pfc_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
+				unsigned offset)
+{
+	seq_printf(s, "%s", DRV_NAME);
+}
+
+static struct pinctrl_ops sh_pfc_pinctrl_ops = {
+	.get_groups_count	= sh_pfc_get_groups_count,
+	.get_group_name		= sh_pfc_get_group_name,
+	.get_group_pins		= sh_pfc_get_group_pins,
+	.pin_dbg_show		= sh_pfc_pin_dbg_show,
+};
+
+static int sh_pfc_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->nr_functions;
+}
+
+static const char *sh_pfc_get_function_name(struct pinctrl_dev *pctldev,
+					    unsigned selector)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->functions[selector]->name;
+}
+
+static int sh_pfc_get_function_groups(struct pinctrl_dev *pctldev, unsigned func,
+				      const char * const **groups,
+				      unsigned * const num_groups)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = &pmx->functions[func]->name;
+	*num_groups = 1;
+
+	return 0;
+}
+
+static int sh_pfc_noop_enable(struct pinctrl_dev *pctldev, unsigned func,
+			      unsigned group)
+{
+	return 0;
+}
+
+static void sh_pfc_noop_disable(struct pinctrl_dev *pctldev, unsigned func,
+				unsigned group)
+{
+}
+
+static inline int sh_pfc_config_function(struct sh_pfc *pfc, unsigned offset)
+{
+	if (sh_pfc_config_gpio(pfc, offset,
+			       PINMUX_TYPE_FUNCTION,
+			       GPIO_CFG_DRYRUN) != 0)
+		return -EINVAL;
+
+	if (sh_pfc_config_gpio(pfc, offset,
+			       PINMUX_TYPE_FUNCTION,
+			       GPIO_CFG_REQ) != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int sh_pfc_reconfig_pin(struct sh_pfc *pfc, unsigned offset,
+			       int new_type)
+{
+	unsigned long flags;
+	int pinmux_type;
+	int ret = -EINVAL;
+
+	spin_lock_irqsave(&pfc->lock, flags);
+
+	pinmux_type = pfc->gpios[offset].flags & PINMUX_FLAG_TYPE;
+
+	/*
+	 * See if the present config needs to first be de-configured.
+	 */
+	switch (pinmux_type) {
+	case PINMUX_TYPE_GPIO:
+		break;
+	case PINMUX_TYPE_OUTPUT:
+	case PINMUX_TYPE_INPUT:
+	case PINMUX_TYPE_INPUT_PULLUP:
+	case PINMUX_TYPE_INPUT_PULLDOWN:
+		sh_pfc_config_gpio(pfc, offset, pinmux_type, GPIO_CFG_FREE);
+		break;
+	default:
+		goto err;
+	}
+
+	/*
+	 * Dry run
+	 */
+	if (sh_pfc_config_gpio(pfc, offset, new_type,
+			       GPIO_CFG_DRYRUN) != 0)
+		goto err;
+
+	/*
+	 * Request
+	 */
+	if (sh_pfc_config_gpio(pfc, offset, new_type,
+			       GPIO_CFG_REQ) != 0)
+		goto err;
+
+	pfc->gpios[offset].flags &= ~PINMUX_FLAG_TYPE;
+	pfc->gpios[offset].flags |= new_type;
+
+	ret = 0;
+
+err:
+	spin_unlock_irqrestore(&pfc->lock, flags);
+
+	return ret;
+}
+
+
+static int sh_pfc_gpio_request_enable(struct pinctrl_dev *pctldev,
+				      struct pinctrl_gpio_range *range,
+				      unsigned offset)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+	unsigned long flags;
+	int ret, pinmux_type;
+
+	spin_lock_irqsave(&pfc->lock, flags);
+
+	pinmux_type = pfc->gpios[offset].flags & PINMUX_FLAG_TYPE;
+
+	switch (pinmux_type) {
+	case PINMUX_TYPE_FUNCTION:
+		pr_notice_once("Use of GPIO API for function requests is "
+			       "deprecated, convert to pinctrl\n");
+		/* handle for now */
+		ret = sh_pfc_config_function(pfc, offset);
+		if (unlikely(ret < 0))
+			goto err;
+
+		break;
+	case PINMUX_TYPE_GPIO:
+		break;
+	default:
+		pr_err("Unsupported mux type (%d), bailing...\n", pinmux_type);
+		return -ENOTSUPP;
+	}
+
+	ret = 0;
+
+err:
+	spin_unlock_irqrestore(&pfc->lock, flags);
+
+	return ret;
+}
+
+static void sh_pfc_gpio_disable_free(struct pinctrl_dev *pctldev,
+				     struct pinctrl_gpio_range *range,
+				     unsigned offset)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+	unsigned long flags;
+	int pinmux_type;
+
+	spin_lock_irqsave(&pfc->lock, flags);
+
+	pinmux_type = pfc->gpios[offset].flags & PINMUX_FLAG_TYPE;
+
+	sh_pfc_config_gpio(pfc, offset, pinmux_type, GPIO_CFG_FREE);
+
+	spin_unlock_irqrestore(&pfc->lock, flags);
+}
+
+static int sh_pfc_gpio_set_direction(struct pinctrl_dev *pctldev,
+				     struct pinctrl_gpio_range *range,
+				     unsigned offset, bool input)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	int type = input ? PINMUX_TYPE_INPUT : PINMUX_TYPE_OUTPUT;
+
+	return sh_pfc_reconfig_pin(pmx->pfc, offset, type);
+}
+
+static struct pinmux_ops sh_pfc_pinmux_ops = {
+	.get_functions_count	= sh_pfc_get_functions_count,
+	.get_function_name	= sh_pfc_get_function_name,
+	.get_function_groups	= sh_pfc_get_function_groups,
+	.enable			= sh_pfc_noop_enable,
+	.disable		= sh_pfc_noop_disable,
+	.gpio_request_enable	= sh_pfc_gpio_request_enable,
+	.gpio_disable_free	= sh_pfc_gpio_disable_free,
+	.gpio_set_direction	= sh_pfc_gpio_set_direction,
+};
+
+static int sh_pfc_pinconf_get(struct pinctrl_dev *pctldev, unsigned pin,
+			      unsigned long *config)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct sh_pfc *pfc = pmx->pfc;
+
+	*config = pfc->gpios[pin].flags & PINMUX_FLAG_TYPE;
+
+	return 0;
+}
+
+static int sh_pfc_pinconf_set(struct pinctrl_dev *pctldev, unsigned pin,
+			      unsigned long config)
+{
+	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	/* Validate the new type */
+	if (config >= PINMUX_FLAG_TYPE)
+		return -EINVAL;
+
+	return sh_pfc_reconfig_pin(pmx->pfc, pin, config);
+}
+
+static void sh_pfc_pinconf_dbg_show(struct pinctrl_dev *pctldev,
+				    struct seq_file *s, unsigned pin)
+{
+	const char *pinmux_type_str[] = {
+		[PINMUX_TYPE_NONE]		= "none",
+		[PINMUX_TYPE_FUNCTION]		= "function",
+		[PINMUX_TYPE_GPIO]		= "gpio",
+		[PINMUX_TYPE_OUTPUT]		= "output",
+		[PINMUX_TYPE_INPUT]		= "input",
+		[PINMUX_TYPE_INPUT_PULLUP]	= "input bias pull up",
+		[PINMUX_TYPE_INPUT_PULLDOWN]	= "input bias pull down",
+	};
+	unsigned long config;
+	int rc;
+
+	rc = sh_pfc_pinconf_get(pctldev, pin, &config);
+	if (unlikely(rc != 0))
+		return;
+
+	seq_printf(s, " %s", pinmux_type_str[config]);
+}
+
+static struct pinconf_ops sh_pfc_pinconf_ops = {
+	.pin_config_get		= sh_pfc_pinconf_get,
+	.pin_config_set		= sh_pfc_pinconf_set,
+	.pin_config_dbg_show	= sh_pfc_pinconf_dbg_show,
+};
+
+static struct pinctrl_gpio_range sh_pfc_gpio_range = {
+	.name		= DRV_NAME,
+	.id		= 0,
+};
+
+static struct pinctrl_desc sh_pfc_pinctrl_desc = {
+	.name		= DRV_NAME,
+	.owner		= THIS_MODULE,
+	.pctlops	= &sh_pfc_pinctrl_ops,
+	.pmxops		= &sh_pfc_pinmux_ops,
+	.confops	= &sh_pfc_pinconf_ops,
+};
+
+static inline void __devinit sh_pfc_map_one_gpio(struct sh_pfc *pfc,
+						 struct sh_pfc_pinctrl *pmx,
+						 struct pinmux_gpio *gpio,
+						 unsigned offset)
+{
+	struct pinmux_data_reg *dummy;
+	unsigned long flags;
+	int bit;
+
+	gpio->flags &= ~PINMUX_FLAG_TYPE;
+
+	if (sh_pfc_get_data_reg(pfc, offset, &dummy, &bit) == 0)
+		gpio->flags |= PINMUX_TYPE_GPIO;
+	else {
+		gpio->flags |= PINMUX_TYPE_FUNCTION;
+
+		spin_lock_irqsave(&pmx->lock, flags);
+		pmx->nr_functions++;
+		spin_unlock_irqrestore(&pmx->lock, flags);
+	}
+}
+
+/* pinmux ranges -> pinctrl pin descs */
+static int __devinit sh_pfc_map_gpios(struct sh_pfc *pfc,
+				      struct sh_pfc_pinctrl *pmx)
+{
+	unsigned long flags;
+	int i;
+
+	pmx->nr_pads = pfc->last_gpio - pfc->first_gpio + 1;
+
+	pmx->pads = kmalloc(sizeof(struct pinctrl_pin_desc) * pmx->nr_pads,
+			    GFP_KERNEL);
+	if (unlikely(!pmx->pads)) {
+		pmx->nr_pads = 0;
+		return -ENOMEM;
+	}
+
+	spin_lock_irqsave(&pfc->lock, flags);
+
+	/*
+	 * We don't necessarily have a 1:1 mapping between pin and linux
+	 * GPIO number, as the latter maps to the associated enum_id.
+	 * Care needs to be taken to translate back to pin space when
+	 * dealing with any pin configurations.
+	 */
+	for (i = 0; i < pmx->nr_pads; i++) {
+		struct pinctrl_pin_desc *pin = pmx->pads + i;
+		struct pinmux_gpio *gpio = pfc->gpios + i;
+
+		pin->number = pfc->first_gpio + i;
+		pin->name = gpio->name;
+
+		/* XXX */
+		if (unlikely(!gpio->enum_id))
+			continue;
+
+		sh_pfc_map_one_gpio(pfc, pmx, gpio, i);
+	}
+
+	spin_unlock_irqrestore(&pfc->lock, flags);
+
+	sh_pfc_pinctrl_desc.pins = pmx->pads;
+	sh_pfc_pinctrl_desc.npins = pmx->nr_pads;
+
+	return 0;
+}
+
+static int __devinit sh_pfc_map_functions(struct sh_pfc *pfc,
+					  struct sh_pfc_pinctrl *pmx)
+{
+	unsigned long flags;
+	int i, fn;
+
+	pmx->functions = kzalloc(pmx->nr_functions * sizeof(void *),
+				 GFP_KERNEL);
+	if (unlikely(!pmx->functions))
+		return -ENOMEM;
+
+	spin_lock_irqsave(&pmx->lock, flags);
+
+	for (i = fn = 0; i < pmx->nr_pads; i++) {
+		struct pinmux_gpio *gpio = pfc->gpios + i;
+
+		if ((gpio->flags & PINMUX_FLAG_TYPE) == PINMUX_TYPE_FUNCTION)
+			pmx->functions[fn++] = gpio;
+	}
+
+	spin_unlock_irqrestore(&pmx->lock, flags);
+
+	return 0;
+}
+
+static int __devinit sh_pfc_pinctrl_probe(struct platform_device *pdev)
+{
+	struct sh_pfc *pfc;
+	int ret;
+
+	if (unlikely(!sh_pfc_pmx))
+		return -ENODEV;
+
+	pfc = sh_pfc_pmx->pfc;
+
+	ret = sh_pfc_map_gpios(pfc, sh_pfc_pmx);
+	if (unlikely(ret != 0))
+		return ret;
+
+	ret = sh_pfc_map_functions(pfc, sh_pfc_pmx);
+	if (unlikely(ret != 0))
+		goto free_pads;
+
+	sh_pfc_pmx->pctl = pinctrl_register(&sh_pfc_pinctrl_desc, &pdev->dev,
+					    sh_pfc_pmx);
+	if (IS_ERR(sh_pfc_pmx->pctl)) {
+		ret = PTR_ERR(sh_pfc_pmx->pctl);
+		goto free_functions;
+	}
+
+	sh_pfc_gpio_range.npins = pfc->last_gpio - pfc->first_gpio + 1;
+	sh_pfc_gpio_range.base = pfc->first_gpio;
+	sh_pfc_gpio_range.pin_base = pfc->first_gpio;
+
+	pinctrl_add_gpio_range(sh_pfc_pmx->pctl, &sh_pfc_gpio_range);
+
+	platform_set_drvdata(pdev, sh_pfc_pmx);
+
+	return 0;
+
+free_functions:
+	kfree(sh_pfc_pmx->functions);
+free_pads:
+	kfree(sh_pfc_pmx->pads);
+	kfree(sh_pfc_pmx);
+
+	return ret;
+}
+
+static int __devexit sh_pfc_pinctrl_remove(struct platform_device *pdev)
+{
+	struct sh_pfc_pinctrl *pmx = platform_get_drvdata(pdev);
+
+	pinctrl_unregister(pmx->pctl);
+
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(sh_pfc_pmx->functions);
+	kfree(sh_pfc_pmx->pads);
+	kfree(sh_pfc_pmx);
+
+	return 0;
+}
+
+static struct platform_driver sh_pfc_pinctrl_driver = {
+	.probe		= sh_pfc_pinctrl_probe,
+	.remove		= __devexit_p(sh_pfc_pinctrl_remove),
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static struct platform_device sh_pfc_pinctrl_device = {
+	.name		= DRV_NAME,
+	.id		= -1,
+};
+
+static int sh_pfc_pinctrl_init(void)
+{
+	int rc;
+
+	rc = platform_driver_register(&sh_pfc_pinctrl_driver);
+	if (likely(!rc)) {
+		rc = platform_device_register(&sh_pfc_pinctrl_device);
+		if (unlikely(rc))
+			platform_driver_unregister(&sh_pfc_pinctrl_driver);
+	}
+
+	return rc;
+}
+
+int sh_pfc_register_pinctrl(struct sh_pfc *pfc)
+{
+	sh_pfc_pmx = kzalloc(sizeof(struct sh_pfc_pinctrl), GFP_KERNEL);
+	if (unlikely(!sh_pfc_pmx))
+		return -ENOMEM;
+
+	spin_lock_init(&sh_pfc_pmx->lock);
+
+	sh_pfc_pmx->pfc = pfc;
+
+	return sh_pfc_pinctrl_init();
+}
+EXPORT_SYMBOL_GPL(sh_pfc_register_pinctrl);
+
+static void __exit sh_pfc_pinctrl_exit(void)
+{
+	platform_driver_unregister(&sh_pfc_pinctrl_driver);
+}
+module_exit(sh_pfc_pinctrl_exit);
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index cd2fe350e72..5f84b5563c2 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -125,7 +125,7 @@ config SPI_BUTTERFLY
 
 config SPI_COLDFIRE_QSPI
 	tristate "Freescale Coldfire QSPI controller"
-	depends on (M520x || M523x || M5249 || M527x || M528x || M532x)
+	depends on (M520x || M523x || M5249 || M525x || M527x || M528x || M532x)
 	help
 	  This enables support for the Coldfire QSPI controller in master
 	  mode.
@@ -144,6 +144,15 @@ config SPI_EP93XX
 	  This enables using the Cirrus EP93xx SPI controller in master
 	  mode.
 
+config SPI_FALCON
+	tristate "Falcon SPI controller support"
+	depends on SOC_FALCON
+	help
+	  The external bus unit (EBU) found on the FALC-ON SoC has SPI
+	  emulation that is designed for serial flash access. This driver
+	  has only been tested with m25p80 type chips. The hardware has no
+	  support for other types of SPI peripherals.
+
 config SPI_GPIO
 	tristate "GPIO-based bitbanging SPI Master"
 	depends on GENERIC_GPIO
@@ -357,7 +366,7 @@ config SPI_STMP3XXX
 
 config SPI_TEGRA
 	tristate "Nvidia Tegra SPI controller"
-	depends on ARCH_TEGRA && TEGRA_SYSTEM_DMA
+	depends on ARCH_TEGRA && (TEGRA_SYSTEM_DMA || TEGRA20_APB_DMA)
 	help
 	  SPI driver for NVidia Tegra SoCs
 
@@ -384,6 +393,13 @@ config SPI_TXX9
 	help
 	  SPI driver for Toshiba TXx9 MIPS SoCs
 
+config SPI_XCOMM
+	tristate "Analog Devices AD-FMCOMMS1-EBZ SPI-I2C-bridge driver"
+	depends on I2C
+	help
+	  Support for the SPI-I2C bridge found on the Analog Devices
+	  AD-FMCOMMS1-EBZ board.
+
 config SPI_XILINX
 	tristate "Xilinx SPI controller common module"
 	depends on HAS_IOMEM && EXPERIMENTAL
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 9d75d2198ff..3920dcf4c74 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -26,6 +26,7 @@ obj-$(CONFIG_SPI_DW_MMIO)		+= spi-dw-mmio.o
 obj-$(CONFIG_SPI_DW_PCI)		+= spi-dw-midpci.o
 spi-dw-midpci-objs			:= spi-dw-pci.o spi-dw-mid.o
 obj-$(CONFIG_SPI_EP93XX)		+= spi-ep93xx.o
+obj-$(CONFIG_SPI_FALCON)		+= spi-falcon.o
 obj-$(CONFIG_SPI_FSL_LIB)		+= spi-fsl-lib.o
 obj-$(CONFIG_SPI_FSL_ESPI)		+= spi-fsl-espi.o
 obj-$(CONFIG_SPI_FSL_SPI)		+= spi-fsl-spi.o
@@ -61,5 +62,6 @@ obj-$(CONFIG_SPI_TI_SSP)		+= spi-ti-ssp.o
 obj-$(CONFIG_SPI_TLE62X0)		+= spi-tle62x0.o
 obj-$(CONFIG_SPI_TOPCLIFF_PCH)		+= spi-topcliff-pch.o
 obj-$(CONFIG_SPI_TXX9)			+= spi-txx9.o
+obj-$(CONFIG_SPI_XCOMM)		+= spi-xcomm.o
 obj-$(CONFIG_SPI_XILINX)		+= spi-xilinx.o
 
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index 7491971139a..6e25ef1bce9 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -129,7 +129,7 @@ static void bcm63xx_spi_setup_transfer(struct spi_device *spi,
 
 	/* Find the closest clock configuration */
 	for (i = 0; i < SPI_CLK_MASK; i++) {
-		if (hz <= bcm63xx_spi_freq_table[i][0]) {
+		if (hz >= bcm63xx_spi_freq_table[i][0]) {
 			clk_cfg = bcm63xx_spi_freq_table[i][1];
 			break;
 		}
diff --git a/drivers/spi/spi-falcon.c b/drivers/spi/spi-falcon.c
new file mode 100644
index 00000000000..8f6aa735a24
--- /dev/null
+++ b/drivers/spi/spi-falcon.c
@@ -0,0 +1,469 @@
+/*
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under the terms of the GNU General Public License version 2 as published
+ *  by the Free Software Foundation.
+ *
+ *  Copyright (C) 2012 Thomas Langer <thomas.langer@lantiq.com>
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+
+#include <lantiq_soc.h>
+
+#define DRV_NAME		"sflash-falcon"
+
+#define FALCON_SPI_XFER_BEGIN	(1 << 0)
+#define FALCON_SPI_XFER_END	(1 << 1)
+
+/* Bus Read Configuration Register0 */
+#define BUSRCON0		0x00000010
+/* Bus Write Configuration Register0 */
+#define BUSWCON0		0x00000018
+/* Serial Flash Configuration Register */
+#define SFCON			0x00000080
+/* Serial Flash Time Register */
+#define SFTIME			0x00000084
+/* Serial Flash Status Register */
+#define SFSTAT			0x00000088
+/* Serial Flash Command Register */
+#define SFCMD			0x0000008C
+/* Serial Flash Address Register */
+#define SFADDR			0x00000090
+/* Serial Flash Data Register */
+#define SFDATA			0x00000094
+/* Serial Flash I/O Control Register */
+#define SFIO			0x00000098
+/* EBU Clock Control Register */
+#define EBUCC			0x000000C4
+
+/* Dummy Phase Length */
+#define SFCMD_DUMLEN_OFFSET	16
+#define SFCMD_DUMLEN_MASK	0x000F0000
+/* Chip Select */
+#define SFCMD_CS_OFFSET		24
+#define SFCMD_CS_MASK		0x07000000
+/* field offset */
+#define SFCMD_ALEN_OFFSET	20
+#define SFCMD_ALEN_MASK		0x00700000
+/* SCK Rise-edge Position */
+#define SFTIME_SCKR_POS_OFFSET	8
+#define SFTIME_SCKR_POS_MASK	0x00000F00
+/* SCK Period */
+#define SFTIME_SCK_PER_OFFSET	0
+#define SFTIME_SCK_PER_MASK	0x0000000F
+/* SCK Fall-edge Position */
+#define SFTIME_SCKF_POS_OFFSET	12
+#define SFTIME_SCKF_POS_MASK	0x0000F000
+/* Device Size */
+#define SFCON_DEV_SIZE_A23_0	0x03000000
+#define SFCON_DEV_SIZE_MASK	0x0F000000
+/* Read Data Position */
+#define SFTIME_RD_POS_MASK	0x000F0000
+/* Data Output */
+#define SFIO_UNUSED_WD_MASK	0x0000000F
+/* Command Opcode mask */
+#define SFCMD_OPC_MASK		0x000000FF
+/* dlen bytes of data to write */
+#define SFCMD_DIR_WRITE		0x00000100
+/* Data Length offset */
+#define SFCMD_DLEN_OFFSET	9
+/* Command Error */
+#define SFSTAT_CMD_ERR		0x20000000
+/* Access Command Pending */
+#define SFSTAT_CMD_PEND		0x00400000
+/* Frequency set to 100MHz. */
+#define EBUCC_EBUDIV_SELF100	0x00000001
+/* Serial Flash */
+#define BUSRCON0_AGEN_SERIAL_FLASH	0xF0000000
+/* 8-bit multiplexed */
+#define BUSRCON0_PORTW_8_BIT_MUX	0x00000000
+/* Serial Flash */
+#define BUSWCON0_AGEN_SERIAL_FLASH	0xF0000000
+/* Chip Select after opcode */
+#define SFCMD_KEEP_CS_KEEP_SELECTED	0x00008000
+
+#define CLOCK_100M	100000000
+#define CLOCK_50M	50000000
+
+struct falcon_sflash {
+	u32 sfcmd; /* for caching of opcode, direction, ... */
+	struct spi_master *master;
+};
+
+int falcon_sflash_xfer(struct spi_device *spi, struct spi_transfer *t,
+		unsigned long flags)
+{
+	struct device *dev = &spi->dev;
+	struct falcon_sflash *priv = spi_master_get_devdata(spi->master);
+	const u8 *txp = t->tx_buf;
+	u8 *rxp = t->rx_buf;
+	unsigned int bytelen = ((8 * t->len + 7) / 8);
+	unsigned int len, alen, dumlen;
+	u32 val;
+	enum {
+		state_init,
+		state_command_prepare,
+		state_write,
+		state_read,
+		state_disable_cs,
+		state_end
+	} state = state_init;
+
+	do {
+		switch (state) {
+		case state_init: /* detect phase of upper layer sequence */
+		{
+			/* initial write ? */
+			if (flags & FALCON_SPI_XFER_BEGIN) {
+				if (!txp) {
+					dev_err(dev,
+						"BEGIN without tx data!\n");
+					return -ENODATA;
+				}
+				/*
+				 * Prepare the parts of the sfcmd register,
+				 * which should not change during a sequence!
+				 * Only exception are the length fields,
+				 * especially alen and dumlen.
+				 */
+
+				priv->sfcmd = ((spi->chip_select
+						<< SFCMD_CS_OFFSET)
+					       & SFCMD_CS_MASK);
+				priv->sfcmd |= SFCMD_KEEP_CS_KEEP_SELECTED;
+				priv->sfcmd |= *txp;
+				txp++;
+				bytelen--;
+				if (bytelen) {
+					/*
+					 * more data:
+					 * maybe address and/or dummy
+					 */
+					state = state_command_prepare;
+					break;
+				} else {
+					dev_dbg(dev, "write cmd %02X\n",
+						priv->sfcmd & SFCMD_OPC_MASK);
+				}
+			}
+			/* continued write ? */
+			if (txp && bytelen) {
+				state = state_write;
+				break;
+			}
+			/* read data? */
+			if (rxp && bytelen) {
+				state = state_read;
+				break;
+			}
+			/* end of sequence? */
+			if (flags & FALCON_SPI_XFER_END)
+				state = state_disable_cs;
+			else
+				state = state_end;
+			break;
+		}
+		/* collect tx data for address and dummy phase */
+		case state_command_prepare:
+		{
+			/* txp is valid, already checked */
+			val = 0;
+			alen = 0;
+			dumlen = 0;
+			while (bytelen > 0) {
+				if (alen < 3) {
+					val = (val << 8) | (*txp++);
+					alen++;
+				} else if ((dumlen < 15) && (*txp == 0)) {
+					/*
+					 * assume dummy bytes are set to 0
+					 * from upper layer
+					 */
+					dumlen++;
+					txp++;
+				} else {
+					break;
+				}
+				bytelen--;
+			}
+			priv->sfcmd &= ~(SFCMD_ALEN_MASK | SFCMD_DUMLEN_MASK);
+			priv->sfcmd |= (alen << SFCMD_ALEN_OFFSET) |
+					 (dumlen << SFCMD_DUMLEN_OFFSET);
+			if (alen > 0)
+				ltq_ebu_w32(val, SFADDR);
+
+			dev_dbg(dev, "wr %02X, alen=%d (addr=%06X) dlen=%d\n",
+				priv->sfcmd & SFCMD_OPC_MASK,
+				alen, val, dumlen);
+
+			if (bytelen > 0) {
+				/* continue with write */
+				state = state_write;
+			} else if (flags & FALCON_SPI_XFER_END) {
+				/* end of sequence? */
+				state = state_disable_cs;
+			} else {
+				/*
+				 * go to end and expect another
+				 * call (read or write)
+				 */
+				state = state_end;
+			}
+			break;
+		}
+		case state_write:
+		{
+			/* txp still valid */
+			priv->sfcmd |= SFCMD_DIR_WRITE;
+			len = 0;
+			val = 0;
+			do {
+				if (bytelen--)
+					val |= (*txp++) << (8 * len++);
+				if ((flags & FALCON_SPI_XFER_END)
+				    && (bytelen == 0)) {
+					priv->sfcmd &=
+						~SFCMD_KEEP_CS_KEEP_SELECTED;
+				}
+				if ((len == 4) || (bytelen == 0)) {
+					ltq_ebu_w32(val, SFDATA);
+					ltq_ebu_w32(priv->sfcmd
+						| (len<<SFCMD_DLEN_OFFSET),
+						SFCMD);
+					len = 0;
+					val = 0;
+					priv->sfcmd &= ~(SFCMD_ALEN_MASK
+							 | SFCMD_DUMLEN_MASK);
+				}
+			} while (bytelen);
+			state = state_end;
+			break;
+		}
+		case state_read:
+		{
+			/* read data */
+			priv->sfcmd &= ~SFCMD_DIR_WRITE;
+			do {
+				if ((flags & FALCON_SPI_XFER_END)
+				    && (bytelen <= 4)) {
+					priv->sfcmd &=
+						~SFCMD_KEEP_CS_KEEP_SELECTED;
+				}
+				len = (bytelen > 4) ? 4 : bytelen;
+				bytelen -= len;
+				ltq_ebu_w32(priv->sfcmd
+					| (len << SFCMD_DLEN_OFFSET), SFCMD);
+				priv->sfcmd &= ~(SFCMD_ALEN_MASK
+						 | SFCMD_DUMLEN_MASK);
+				do {
+					val = ltq_ebu_r32(SFSTAT);
+					if (val & SFSTAT_CMD_ERR) {
+						/* reset error status */
+						dev_err(dev, "SFSTAT: CMD_ERR");
+						dev_err(dev, " (%x)\n", val);
+						ltq_ebu_w32(SFSTAT_CMD_ERR,
+							SFSTAT);
+						return -EBADE;
+					}
+				} while (val & SFSTAT_CMD_PEND);
+				val = ltq_ebu_r32(SFDATA);
+				do {
+					*rxp = (val & 0xFF);
+					rxp++;
+					val >>= 8;
+					len--;
+				} while (len);
+			} while (bytelen);
+			state = state_end;
+			break;
+		}
+		case state_disable_cs:
+		{
+			priv->sfcmd &= ~SFCMD_KEEP_CS_KEEP_SELECTED;
+			ltq_ebu_w32(priv->sfcmd | (0 << SFCMD_DLEN_OFFSET),
+				SFCMD);
+			val = ltq_ebu_r32(SFSTAT);
+			if (val & SFSTAT_CMD_ERR) {
+				/* reset error status */
+				dev_err(dev, "SFSTAT: CMD_ERR (%x)\n", val);
+				ltq_ebu_w32(SFSTAT_CMD_ERR, SFSTAT);
+				return -EBADE;
+			}
+			state = state_end;
+			break;
+		}
+		case state_end:
+			break;
+		}
+	} while (state != state_end);
+
+	return 0;
+}
+
+static int falcon_sflash_setup(struct spi_device *spi)
+{
+	unsigned int i;
+	unsigned long flags;
+
+	if (spi->chip_select > 0)
+		return -ENODEV;
+
+	spin_lock_irqsave(&ebu_lock, flags);
+
+	if (spi->max_speed_hz >= CLOCK_100M) {
+		/* set EBU clock to 100 MHz */
+		ltq_sys1_w32_mask(0, EBUCC_EBUDIV_SELF100, EBUCC);
+		i = 1; /* divider */
+	} else {
+		/* set EBU clock to 50 MHz */
+		ltq_sys1_w32_mask(EBUCC_EBUDIV_SELF100, 0, EBUCC);
+
+		/* search for suitable divider */
+		for (i = 1; i < 7; i++) {
+			if (CLOCK_50M / i <= spi->max_speed_hz)
+				break;
+		}
+	}
+
+	/* setup period of serial clock */
+	ltq_ebu_w32_mask(SFTIME_SCKF_POS_MASK
+		     | SFTIME_SCKR_POS_MASK
+		     | SFTIME_SCK_PER_MASK,
+		     (i << SFTIME_SCKR_POS_OFFSET)
+		     | (i << (SFTIME_SCK_PER_OFFSET + 1)),
+		     SFTIME);
+
+	/*
+	 * set some bits of unused_wd, to not trigger HOLD/WP
+	 * signals on non QUAD flashes
+	 */
+	ltq_ebu_w32((SFIO_UNUSED_WD_MASK & (0x8 | 0x4)), SFIO);
+
+	ltq_ebu_w32(BUSRCON0_AGEN_SERIAL_FLASH | BUSRCON0_PORTW_8_BIT_MUX,
+			BUSRCON0);
+	ltq_ebu_w32(BUSWCON0_AGEN_SERIAL_FLASH, BUSWCON0);
+	/* set address wrap around to maximum for 24-bit addresses */
+	ltq_ebu_w32_mask(SFCON_DEV_SIZE_MASK, SFCON_DEV_SIZE_A23_0, SFCON);
+
+	spin_unlock_irqrestore(&ebu_lock, flags);
+
+	return 0;
+}
+
+static int falcon_sflash_prepare_xfer(struct spi_master *master)
+{
+	return 0;
+}
+
+static int falcon_sflash_unprepare_xfer(struct spi_master *master)
+{
+	return 0;
+}
+
+static int falcon_sflash_xfer_one(struct spi_master *master,
+					struct spi_message *m)
+{
+	struct falcon_sflash *priv = spi_master_get_devdata(master);
+	struct spi_transfer *t;
+	unsigned long spi_flags;
+	unsigned long flags;
+	int ret = 0;
+
+	priv->sfcmd = 0;
+	m->actual_length = 0;
+
+	spi_flags = FALCON_SPI_XFER_BEGIN;
+	list_for_each_entry(t, &m->transfers, transfer_list) {
+		if (list_is_last(&t->transfer_list, &m->transfers))
+			spi_flags |= FALCON_SPI_XFER_END;
+
+		spin_lock_irqsave(&ebu_lock, flags);
+		ret = falcon_sflash_xfer(m->spi, t, spi_flags);
+		spin_unlock_irqrestore(&ebu_lock, flags);
+
+		if (ret)
+			break;
+
+		m->actual_length += t->len;
+
+		WARN_ON(t->delay_usecs || t->cs_change);
+		spi_flags = 0;
+	}
+
+	m->status = ret;
+	m->complete(m->context);
+
+	return 0;
+}
+
+static int __devinit falcon_sflash_probe(struct platform_device *pdev)
+{
+	struct falcon_sflash *priv;
+	struct spi_master *master;
+	int ret;
+
+	if (ltq_boot_select() != BS_SPI) {
+		dev_err(&pdev->dev, "invalid bootstrap options\n");
+		return -ENODEV;
+	}
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*priv));
+	if (!master)
+		return -ENOMEM;
+
+	priv = spi_master_get_devdata(master);
+	priv->master = master;
+
+	master->mode_bits = SPI_MODE_3;
+	master->num_chipselect = 1;
+	master->bus_num = -1;
+	master->setup = falcon_sflash_setup;
+	master->prepare_transfer_hardware = falcon_sflash_prepare_xfer;
+	master->transfer_one_message = falcon_sflash_xfer_one;
+	master->unprepare_transfer_hardware = falcon_sflash_unprepare_xfer;
+	master->dev.of_node = pdev->dev.of_node;
+
+	platform_set_drvdata(pdev, priv);
+
+	ret = spi_register_master(master);
+	if (ret)
+		spi_master_put(master);
+	return ret;
+}
+
+static int __devexit falcon_sflash_remove(struct platform_device *pdev)
+{
+	struct falcon_sflash *priv = platform_get_drvdata(pdev);
+
+	spi_unregister_master(priv->master);
+
+	return 0;
+}
+
+static const struct of_device_id falcon_sflash_match[] = {
+	{ .compatible = "lantiq,sflash-falcon" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, falcon_sflash_match);
+
+static struct platform_driver falcon_sflash_driver = {
+	.probe	= falcon_sflash_probe,
+	.remove	= __devexit_p(falcon_sflash_remove),
+	.driver = {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = falcon_sflash_match,
+	}
+};
+
+module_platform_driver(falcon_sflash_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Lantiq Falcon SPI/SFLASH controller driver");
diff --git a/drivers/spi/spi-gpio.c b/drivers/spi/spi-gpio.c
index 0094c645ff0..0b56cfc71fa 100644
--- a/drivers/spi/spi-gpio.c
+++ b/drivers/spi/spi-gpio.c
@@ -235,7 +235,8 @@ static int spi_gpio_setup(struct spi_device *spi)
 			status = gpio_request(cs, dev_name(&spi->dev));
 			if (status)
 				return status;
-			status = gpio_direction_output(cs, spi->mode & SPI_CS_HIGH);
+			status = gpio_direction_output(cs,
+					!(spi->mode & SPI_CS_HIGH));
 		}
 	}
 	if (!status)
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index 47877d68761..e834ff8c018 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -626,7 +626,7 @@ static void spi_imx_chipselect(struct spi_device *spi, int is_active)
 	int active = is_active != BITBANG_CS_INACTIVE;
 	int dev_is_lowactive = !(spi->mode & SPI_CS_HIGH);
 
-	if (gpio < 0)
+	if (!gpio_is_valid(gpio))
 		return;
 
 	gpio_set_value(gpio, dev_is_lowactive ^ active);
@@ -688,8 +688,6 @@ static int spi_imx_setupxfer(struct spi_device *spi,
 		config.speed_hz = spi->max_speed_hz;
 	if (!config.bpw)
 		config.bpw = spi->bits_per_word;
-	if (!config.speed_hz)
-		config.speed_hz = spi->max_speed_hz;
 
 	/* Initialize the functions for transfer */
 	if (config.bpw <= 8) {
@@ -738,7 +736,7 @@ static int spi_imx_setup(struct spi_device *spi)
 	dev_dbg(&spi->dev, "%s: mode %d, %u bpw, %d hz\n", __func__,
 		 spi->mode, spi->bits_per_word, spi->max_speed_hz);
 
-	if (gpio >= 0)
+	if (gpio_is_valid(gpio))
 		gpio_direction_output(gpio, spi->mode & SPI_CS_HIGH ? 0 : 1);
 
 	spi_imx_chipselect(spi, BITBANG_CS_INACTIVE);
@@ -791,11 +789,11 @@ static int __devinit spi_imx_probe(struct platform_device *pdev)
 
 	for (i = 0; i < master->num_chipselect; i++) {
 		int cs_gpio = of_get_named_gpio(np, "cs-gpios", i);
-		if (cs_gpio < 0 && mxc_platform_info)
+		if (!gpio_is_valid(cs_gpio) && mxc_platform_info)
 			cs_gpio = mxc_platform_info->chipselect[i];
 
 		spi_imx->chipselect[i] = cs_gpio;
-		if (cs_gpio < 0)
+		if (!gpio_is_valid(cs_gpio))
 			continue;
 
 		ret = gpio_request(spi_imx->chipselect[i], DRIVER_NAME);
@@ -897,7 +895,7 @@ out_release_mem:
 	release_mem_region(res->start, resource_size(res));
 out_gpio_free:
 	while (--i >= 0) {
-		if (spi_imx->chipselect[i] >= 0)
+		if (gpio_is_valid(spi_imx->chipselect[i]))
 			gpio_free(spi_imx->chipselect[i]);
 	}
 	spi_master_put(master);
@@ -922,7 +920,7 @@ static int __devexit spi_imx_remove(struct platform_device *pdev)
 	iounmap(spi_imx->base);
 
 	for (i = 0; i < master->num_chipselect; i++)
-		if (spi_imx->chipselect[i] >= 0)
+		if (gpio_is_valid(spi_imx->chipselect[i]))
 			gpio_free(spi_imx->chipselect[i]);
 
 	spi_master_put(master);
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index 0c73dd4f43a..bc4778175e3 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -28,6 +28,8 @@
 #include <linux/device.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/omap-dma.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/clk.h>
@@ -39,7 +41,6 @@
 
 #include <linux/spi/spi.h>
 
-#include <plat/dma.h>
 #include <plat/clock.h>
 #include <plat/mcspi.h>
 
@@ -93,8 +94,8 @@
 
 /* We have 2 DMA channels per CS, one for RX and one for TX */
 struct omap2_mcspi_dma {
-	int dma_tx_channel;
-	int dma_rx_channel;
+	struct dma_chan *dma_tx;
+	struct dma_chan *dma_rx;
 
 	int dma_tx_sync_dev;
 	int dma_rx_sync_dev;
@@ -300,20 +301,46 @@ static int mcspi_wait_for_reg_bit(void __iomem *reg, unsigned long bit)
 	return 0;
 }
 
+static void omap2_mcspi_rx_callback(void *data)
+{
+	struct spi_device *spi = data;
+	struct omap2_mcspi *mcspi = spi_master_get_devdata(spi->master);
+	struct omap2_mcspi_dma *mcspi_dma = &mcspi->dma_channels[spi->chip_select];
+
+	complete(&mcspi_dma->dma_rx_completion);
+
+	/* We must disable the DMA RX request */
+	omap2_mcspi_set_dma_req(spi, 1, 0);
+}
+
+static void omap2_mcspi_tx_callback(void *data)
+{
+	struct spi_device *spi = data;
+	struct omap2_mcspi *mcspi = spi_master_get_devdata(spi->master);
+	struct omap2_mcspi_dma *mcspi_dma = &mcspi->dma_channels[spi->chip_select];
+
+	complete(&mcspi_dma->dma_tx_completion);
+
+	/* We must disable the DMA TX request */
+	omap2_mcspi_set_dma_req(spi, 0, 0);
+}
+
 static unsigned
 omap2_mcspi_txrx_dma(struct spi_device *spi, struct spi_transfer *xfer)
 {
 	struct omap2_mcspi	*mcspi;
 	struct omap2_mcspi_cs	*cs = spi->controller_state;
 	struct omap2_mcspi_dma  *mcspi_dma;
-	unsigned int		count, c;
-	unsigned long		base, tx_reg, rx_reg;
-	int			word_len, data_type, element_count;
+	unsigned int		count;
+	int			word_len, element_count;
 	int			elements = 0;
 	u32			l;
 	u8			* rx;
 	const u8		* tx;
 	void __iomem		*chstat_reg;
+	struct dma_slave_config	cfg;
+	enum dma_slave_buswidth width;
+	unsigned es;
 
 	mcspi = spi_master_get_devdata(spi->master);
 	mcspi_dma = &mcspi->dma_channels[spi->chip_select];
@@ -321,74 +348,99 @@ omap2_mcspi_txrx_dma(struct spi_device *spi, struct spi_transfer *xfer)
 
 	chstat_reg = cs->base + OMAP2_MCSPI_CHSTAT0;
 
+	if (cs->word_len <= 8) {
+		width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+		es = 1;
+	} else if (cs->word_len <= 16) {
+		width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+		es = 2;
+	} else {
+		width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		es = 4;
+	}
+
+	memset(&cfg, 0, sizeof(cfg));
+	cfg.src_addr = cs->phys + OMAP2_MCSPI_RX0;
+	cfg.dst_addr = cs->phys + OMAP2_MCSPI_TX0;
+	cfg.src_addr_width = width;
+	cfg.dst_addr_width = width;
+	cfg.src_maxburst = 1;
+	cfg.dst_maxburst = 1;
+
+	if (xfer->tx_buf && mcspi_dma->dma_tx) {
+		struct dma_async_tx_descriptor *tx;
+		struct scatterlist sg;
+
+		dmaengine_slave_config(mcspi_dma->dma_tx, &cfg);
+
+		sg_init_table(&sg, 1);
+		sg_dma_address(&sg) = xfer->tx_dma;
+		sg_dma_len(&sg) = xfer->len;
+
+		tx = dmaengine_prep_slave_sg(mcspi_dma->dma_tx, &sg, 1,
+			DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (tx) {
+			tx->callback = omap2_mcspi_tx_callback;
+			tx->callback_param = spi;
+			dmaengine_submit(tx);
+		} else {
+			/* FIXME: fall back to PIO? */
+		}
+	}
+
+	if (xfer->rx_buf && mcspi_dma->dma_rx) {
+		struct dma_async_tx_descriptor *tx;
+		struct scatterlist sg;
+		size_t len = xfer->len - es;
+
+		dmaengine_slave_config(mcspi_dma->dma_rx, &cfg);
+
+		if (l & OMAP2_MCSPI_CHCONF_TURBO)
+			len -= es;
+
+		sg_init_table(&sg, 1);
+		sg_dma_address(&sg) = xfer->rx_dma;
+		sg_dma_len(&sg) = len;
+
+		tx = dmaengine_prep_slave_sg(mcspi_dma->dma_rx, &sg, 1,
+			DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (tx) {
+			tx->callback = omap2_mcspi_rx_callback;
+			tx->callback_param = spi;
+			dmaengine_submit(tx);
+		} else {
+			/* FIXME: fall back to PIO? */
+		}
+	}
+
 	count = xfer->len;
-	c = count;
 	word_len = cs->word_len;
 
-	base = cs->phys;
-	tx_reg = base + OMAP2_MCSPI_TX0;
-	rx_reg = base + OMAP2_MCSPI_RX0;
 	rx = xfer->rx_buf;
 	tx = xfer->tx_buf;
 
 	if (word_len <= 8) {
-		data_type = OMAP_DMA_DATA_TYPE_S8;
 		element_count = count;
 	} else if (word_len <= 16) {
-		data_type = OMAP_DMA_DATA_TYPE_S16;
 		element_count = count >> 1;
 	} else /* word_len <= 32 */ {
-		data_type = OMAP_DMA_DATA_TYPE_S32;
 		element_count = count >> 2;
 	}
 
 	if (tx != NULL) {
-		omap_set_dma_transfer_params(mcspi_dma->dma_tx_channel,
-				data_type, element_count, 1,
-				OMAP_DMA_SYNC_ELEMENT,
-				mcspi_dma->dma_tx_sync_dev, 0);
-
-		omap_set_dma_dest_params(mcspi_dma->dma_tx_channel, 0,
-				OMAP_DMA_AMODE_CONSTANT,
-				tx_reg, 0, 0);
-
-		omap_set_dma_src_params(mcspi_dma->dma_tx_channel, 0,
-				OMAP_DMA_AMODE_POST_INC,
-				xfer->tx_dma, 0, 0);
-	}
-
-	if (rx != NULL) {
-		elements = element_count - 1;
-		if (l & OMAP2_MCSPI_CHCONF_TURBO)
-			elements--;
-
-		omap_set_dma_transfer_params(mcspi_dma->dma_rx_channel,
-				data_type, elements, 1,
-				OMAP_DMA_SYNC_ELEMENT,
-				mcspi_dma->dma_rx_sync_dev, 1);
-
-		omap_set_dma_src_params(mcspi_dma->dma_rx_channel, 0,
-				OMAP_DMA_AMODE_CONSTANT,
-				rx_reg, 0, 0);
-
-		omap_set_dma_dest_params(mcspi_dma->dma_rx_channel, 0,
-				OMAP_DMA_AMODE_POST_INC,
-				xfer->rx_dma, 0, 0);
-	}
-
-	if (tx != NULL) {
-		omap_start_dma(mcspi_dma->dma_tx_channel);
+		dma_async_issue_pending(mcspi_dma->dma_tx);
 		omap2_mcspi_set_dma_req(spi, 0, 1);
 	}
 
 	if (rx != NULL) {
-		omap_start_dma(mcspi_dma->dma_rx_channel);
+		dma_async_issue_pending(mcspi_dma->dma_rx);
 		omap2_mcspi_set_dma_req(spi, 1, 1);
 	}
 
 	if (tx != NULL) {
 		wait_for_completion(&mcspi_dma->dma_tx_completion);
-		dma_unmap_single(&spi->dev, xfer->tx_dma, count, DMA_TO_DEVICE);
+		dma_unmap_single(mcspi->dev, xfer->tx_dma, count,
+				 DMA_TO_DEVICE);
 
 		/* for TX_ONLY mode, be sure all words have shifted out */
 		if (rx == NULL) {
@@ -403,10 +455,14 @@ omap2_mcspi_txrx_dma(struct spi_device *spi, struct spi_transfer *xfer)
 
 	if (rx != NULL) {
 		wait_for_completion(&mcspi_dma->dma_rx_completion);
-		dma_unmap_single(&spi->dev, xfer->rx_dma, count, DMA_FROM_DEVICE);
+		dma_unmap_single(mcspi->dev, xfer->rx_dma, count,
+				 DMA_FROM_DEVICE);
 		omap2_mcspi_set_enable(spi, 0);
 
+		elements = element_count - 1;
+
 		if (l & OMAP2_MCSPI_CHCONF_TURBO) {
+			elements--;
 
 			if (likely(mcspi_read_cs_reg(spi, OMAP2_MCSPI_CHSTAT0)
 				   & OMAP2_MCSPI_CHSTAT_RXS)) {
@@ -723,64 +779,38 @@ static int omap2_mcspi_setup_transfer(struct spi_device *spi,
 	return 0;
 }
 
-static void omap2_mcspi_dma_rx_callback(int lch, u16 ch_status, void *data)
-{
-	struct spi_device	*spi = data;
-	struct omap2_mcspi	*mcspi;
-	struct omap2_mcspi_dma	*mcspi_dma;
-
-	mcspi = spi_master_get_devdata(spi->master);
-	mcspi_dma = &(mcspi->dma_channels[spi->chip_select]);
-
-	complete(&mcspi_dma->dma_rx_completion);
-
-	/* We must disable the DMA RX request */
-	omap2_mcspi_set_dma_req(spi, 1, 0);
-}
-
-static void omap2_mcspi_dma_tx_callback(int lch, u16 ch_status, void *data)
-{
-	struct spi_device	*spi = data;
-	struct omap2_mcspi	*mcspi;
-	struct omap2_mcspi_dma	*mcspi_dma;
-
-	mcspi = spi_master_get_devdata(spi->master);
-	mcspi_dma = &(mcspi->dma_channels[spi->chip_select]);
-
-	complete(&mcspi_dma->dma_tx_completion);
-
-	/* We must disable the DMA TX request */
-	omap2_mcspi_set_dma_req(spi, 0, 0);
-}
-
 static int omap2_mcspi_request_dma(struct spi_device *spi)
 {
 	struct spi_master	*master = spi->master;
 	struct omap2_mcspi	*mcspi;
 	struct omap2_mcspi_dma	*mcspi_dma;
+	dma_cap_mask_t mask;
+	unsigned sig;
 
 	mcspi = spi_master_get_devdata(master);
 	mcspi_dma = mcspi->dma_channels + spi->chip_select;
 
-	if (omap_request_dma(mcspi_dma->dma_rx_sync_dev, "McSPI RX",
-			omap2_mcspi_dma_rx_callback, spi,
-			&mcspi_dma->dma_rx_channel)) {
-		dev_err(&spi->dev, "no RX DMA channel for McSPI\n");
+	init_completion(&mcspi_dma->dma_rx_completion);
+	init_completion(&mcspi_dma->dma_tx_completion);
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	sig = mcspi_dma->dma_rx_sync_dev;
+	mcspi_dma->dma_rx = dma_request_channel(mask, omap_dma_filter_fn, &sig);
+	if (!mcspi_dma->dma_rx) {
+		dev_err(&spi->dev, "no RX DMA engine channel for McSPI\n");
 		return -EAGAIN;
 	}
 
-	if (omap_request_dma(mcspi_dma->dma_tx_sync_dev, "McSPI TX",
-			omap2_mcspi_dma_tx_callback, spi,
-			&mcspi_dma->dma_tx_channel)) {
-		omap_free_dma(mcspi_dma->dma_rx_channel);
-		mcspi_dma->dma_rx_channel = -1;
-		dev_err(&spi->dev, "no TX DMA channel for McSPI\n");
+	sig = mcspi_dma->dma_tx_sync_dev;
+	mcspi_dma->dma_tx = dma_request_channel(mask, omap_dma_filter_fn, &sig);
+	if (!mcspi_dma->dma_tx) {
+		dev_err(&spi->dev, "no TX DMA engine channel for McSPI\n");
+		dma_release_channel(mcspi_dma->dma_rx);
+		mcspi_dma->dma_rx = NULL;
 		return -EAGAIN;
 	}
 
-	init_completion(&mcspi_dma->dma_rx_completion);
-	init_completion(&mcspi_dma->dma_tx_completion);
-
 	return 0;
 }
 
@@ -812,8 +842,7 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 		list_add_tail(&cs->node, &ctx->cs);
 	}
 
-	if (mcspi_dma->dma_rx_channel == -1
-			|| mcspi_dma->dma_tx_channel == -1) {
+	if (!mcspi_dma->dma_rx || !mcspi_dma->dma_tx) {
 		ret = omap2_mcspi_request_dma(spi);
 		if (ret < 0)
 			return ret;
@@ -848,13 +877,13 @@ static void omap2_mcspi_cleanup(struct spi_device *spi)
 	if (spi->chip_select < spi->master->num_chipselect) {
 		mcspi_dma = &mcspi->dma_channels[spi->chip_select];
 
-		if (mcspi_dma->dma_rx_channel != -1) {
-			omap_free_dma(mcspi_dma->dma_rx_channel);
-			mcspi_dma->dma_rx_channel = -1;
+		if (mcspi_dma->dma_rx) {
+			dma_release_channel(mcspi_dma->dma_rx);
+			mcspi_dma->dma_rx = NULL;
 		}
-		if (mcspi_dma->dma_tx_channel != -1) {
-			omap_free_dma(mcspi_dma->dma_tx_channel);
-			mcspi_dma->dma_tx_channel = -1;
+		if (mcspi_dma->dma_tx) {
+			dma_release_channel(mcspi_dma->dma_tx);
+			mcspi_dma->dma_tx = NULL;
 		}
 	}
 }
@@ -1032,7 +1061,7 @@ static int omap2_mcspi_transfer_one_message(struct spi_master *master,
 	return 0;
 }
 
-static int __init omap2_mcspi_master_setup(struct omap2_mcspi *mcspi)
+static int __devinit omap2_mcspi_master_setup(struct omap2_mcspi *mcspi)
 {
 	struct spi_master	*master = mcspi->master;
 	struct omap2_mcspi_regs	*ctx = &mcspi->ctx;
@@ -1174,7 +1203,6 @@ static int __devinit omap2_mcspi_probe(struct platform_device *pdev)
 			break;
 		}
 
-		mcspi->dma_channels[i].dma_rx_channel = -1;
 		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,
@@ -1185,7 +1213,6 @@ static int __devinit omap2_mcspi_probe(struct platform_device *pdev)
 			break;
 		}
 
-		mcspi->dma_channels[i].dma_tx_channel = -1;
 		mcspi->dma_channels[i].dma_tx_sync_dev = dma_res->start;
 	}
 
diff --git a/drivers/spi/spi-orion.c b/drivers/spi/spi-orion.c
index dfd04e91fa6..9b0caddce50 100644
--- a/drivers/spi/spi-orion.c
+++ b/drivers/spi/spi-orion.c
@@ -17,6 +17,7 @@
 #include <linux/io.h>
 #include <linux/spi/spi.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/clk.h>
 #include <asm/unaligned.h>
 
@@ -45,7 +46,6 @@ struct orion_spi {
 	void __iomem		*base;
 	unsigned int		max_speed;
 	unsigned int		min_speed;
-	struct orion_spi_info	*spi_info;
 	struct clk              *clk;
 };
 
@@ -450,11 +450,10 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 	struct spi_master *master;
 	struct orion_spi *spi;
 	struct resource *r;
-	struct orion_spi_info *spi_info;
 	unsigned long tclk_hz;
 	int status = 0;
-
-	spi_info = pdev->dev.platform_data;
+	const u32 *iprop;
+	int size;
 
 	master = spi_alloc_master(&pdev->dev, sizeof *spi);
 	if (master == NULL) {
@@ -464,6 +463,12 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 
 	if (pdev->id != -1)
 		master->bus_num = pdev->id;
+	if (pdev->dev.of_node) {
+		iprop = of_get_property(pdev->dev.of_node, "cell-index",
+					&size);
+		if (iprop && size == sizeof(*iprop))
+			master->bus_num = *iprop;
+	}
 
 	/* we support only mode 0, and no options */
 	master->mode_bits = 0;
@@ -476,7 +481,6 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 
 	spi = spi_master_get_devdata(master);
 	spi->master = master;
-	spi->spi_info = spi_info;
 
 	spi->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(spi->clk)) {
@@ -511,6 +515,7 @@ static int __init orion_spi_probe(struct platform_device *pdev)
 	if (orion_spi_reset(spi) < 0)
 		goto out_rel_mem;
 
+	master->dev.of_node = pdev->dev.of_node;
 	status = spi_register_master(master);
 	if (status < 0)
 		goto out_rel_mem;
@@ -552,10 +557,17 @@ static int __exit orion_spi_remove(struct platform_device *pdev)
 
 MODULE_ALIAS("platform:" DRIVER_NAME);
 
+static const struct of_device_id orion_spi_of_match_table[] __devinitdata = {
+	{ .compatible = "marvell,orion-spi", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, orion_spi_of_match_table);
+
 static struct platform_driver orion_spi_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(orion_spi_of_match_table),
 	},
 	.remove		= __exit_p(orion_spi_remove),
 };
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 400ae2121a2..aab518ec2bb 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -489,6 +489,11 @@ static void giveback(struct pl022 *pl022)
 	pl022->cur_transfer = NULL;
 	pl022->cur_chip = NULL;
 	spi_finalize_current_message(pl022->master);
+
+	/* disable the SPI/SSP operation */
+	writew((readw(SSP_CR1(pl022->virtbase)) &
+		(~SSP_CR1_MASK_SSE)), SSP_CR1(pl022->virtbase));
+
 }
 
 /**
@@ -2048,6 +2053,9 @@ pl022_probe(struct amba_device *adev, const struct amba_id *id)
 	printk(KERN_INFO "pl022: mapped registers from 0x%08x to %p\n",
 	       adev->res.start, pl022->virtbase);
 
+	pm_runtime_enable(dev);
+	pm_runtime_resume(dev);
+
 	pl022->clk = clk_get(&adev->dev, NULL);
 	if (IS_ERR(pl022->clk)) {
 		status = PTR_ERR(pl022->clk);
@@ -2158,6 +2166,7 @@ pl022_remove(struct amba_device *adev)
 	clk_disable(pl022->clk);
 	clk_unprepare(pl022->clk);
 	clk_put(pl022->clk);
+	pm_runtime_disable(&adev->dev);
 	iounmap(pl022->virtbase);
 	amba_release_regions(adev);
 	tasklet_disable(&pl022->pump_transfers);
@@ -2251,15 +2260,6 @@ static struct vendor_data vendor_st_pl023 = {
 	.loopback = false,
 };
 
-static struct vendor_data vendor_db5500_pl023 = {
-	.fifodepth = 32,
-	.max_bpw = 32,
-	.unidir = false,
-	.extended_cr = true,
-	.pl023 = true,
-	.loopback = true,
-};
-
 static struct amba_id pl022_ids[] = {
 	{
 		/*
@@ -2291,11 +2291,6 @@ static struct amba_id pl022_ids[] = {
 		.mask	= 0xffffffff,
 		.data	= &vendor_st_pl023,
 	},
-	{
-		.id	= 0x10080023,
-		.mask	= 0xffffffff,
-		.data	= &vendor_db5500_pl023,
-	},
 	{ 0, 0 },
 };
 
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index 972a94c58be..646a7657fe6 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -27,10 +27,15 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/spi/spi.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 
 #include <mach/dma.h>
 #include <plat/s3c64xx-spi.h>
 
+#define MAX_SPI_PORTS		3
+
 /* Registers and bit-fields */
 
 #define S3C64XX_SPI_CH_CFG		0x00
@@ -74,11 +79,6 @@
 #define S3C64XX_SPI_SLAVE_AUTO			(1<<1)
 #define S3C64XX_SPI_SLAVE_SIG_INACT		(1<<0)
 
-#define S3C64XX_SPI_ACT(c) writel(0, (c)->regs + S3C64XX_SPI_SLAVE_SEL)
-
-#define S3C64XX_SPI_DEACT(c) writel(S3C64XX_SPI_SLAVE_SIG_INACT, \
-					(c)->regs + S3C64XX_SPI_SLAVE_SEL)
-
 #define S3C64XX_SPI_INT_TRAILING_EN		(1<<6)
 #define S3C64XX_SPI_INT_RX_OVERRUN_EN		(1<<5)
 #define S3C64XX_SPI_INT_RX_UNDERRUN_EN		(1<<4)
@@ -113,13 +113,12 @@
 
 #define S3C64XX_SPI_FBCLK_MSK		(3<<0)
 
-#define S3C64XX_SPI_ST_TRLCNTZ(v, i) ((((v) >> (i)->rx_lvl_offset) & \
-					(((i)->fifo_lvl_mask + 1))) \
-					? 1 : 0)
-
-#define S3C64XX_SPI_ST_TX_DONE(v, i) (((v) & (1 << (i)->tx_st_done)) ? 1 : 0)
-#define TX_FIFO_LVL(v, i) (((v) >> 6) & (i)->fifo_lvl_mask)
-#define RX_FIFO_LVL(v, i) (((v) >> (i)->rx_lvl_offset) & (i)->fifo_lvl_mask)
+#define FIFO_LVL_MASK(i) ((i)->port_conf->fifo_lvl_mask[i->port_id])
+#define S3C64XX_SPI_ST_TX_DONE(v, i) (((v) & \
+				(1 << (i)->port_conf->tx_st_done)) ? 1 : 0)
+#define TX_FIFO_LVL(v, i) (((v) >> 6) & FIFO_LVL_MASK(i))
+#define RX_FIFO_LVL(v, i) (((v) >> (i)->port_conf->rx_lvl_offset) & \
+					FIFO_LVL_MASK(i))
 
 #define S3C64XX_SPI_MAX_TRAILCNT	0x3ff
 #define S3C64XX_SPI_TRAILCNT_OFF	19
@@ -135,6 +134,29 @@ struct s3c64xx_spi_dma_data {
 	unsigned		ch;
 	enum dma_data_direction direction;
 	enum dma_ch	dmach;
+	struct property		*dma_prop;
+};
+
+/**
+ * struct s3c64xx_spi_info - SPI Controller hardware info
+ * @fifo_lvl_mask: Bit-mask for {TX|RX}_FIFO_LVL bits in SPI_STATUS register.
+ * @rx_lvl_offset: Bit offset of RX_FIFO_LVL bits in SPI_STATUS regiter.
+ * @tx_st_done: Bit offset of TX_DONE bit in SPI_STATUS regiter.
+ * @high_speed: True, if the controller supports HIGH_SPEED_EN bit.
+ * @clk_from_cmu: True, if the controller does not include a clock mux and
+ *	prescaler unit.
+ *
+ * The Samsung s3c64xx SPI controller are used on various Samsung SoC's but
+ * differ in some aspects such as the size of the fifo and spi bus clock
+ * setup. Such differences are specified to the driver using this structure
+ * which is provided as driver data to the driver.
+ */
+struct s3c64xx_spi_port_config {
+	int	fifo_lvl_mask[MAX_SPI_PORTS];
+	int	rx_lvl_offset;
+	int	tx_st_done;
+	bool	high_speed;
+	bool	clk_from_cmu;
 };
 
 /**
@@ -175,6 +197,9 @@ struct s3c64xx_spi_driver_data {
 	struct s3c64xx_spi_dma_data	rx_dma;
 	struct s3c64xx_spi_dma_data	tx_dma;
 	struct samsung_dma_ops		*ops;
+	struct s3c64xx_spi_port_config	*port_conf;
+	unsigned int			port_id;
+	unsigned long			gpios[4];
 };
 
 static struct s3c2410_dma_client s3c64xx_spi_dma_client = {
@@ -183,7 +208,6 @@ static struct s3c2410_dma_client s3c64xx_spi_dma_client = {
 
 static void flush_fifo(struct s3c64xx_spi_driver_data *sdd)
 {
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	void __iomem *regs = sdd->regs;
 	unsigned long loops;
 	u32 val;
@@ -199,7 +223,7 @@ static void flush_fifo(struct s3c64xx_spi_driver_data *sdd)
 	loops = msecs_to_loops(1);
 	do {
 		val = readl(regs + S3C64XX_SPI_STATUS);
-	} while (TX_FIFO_LVL(val, sci) && loops--);
+	} while (TX_FIFO_LVL(val, sdd) && loops--);
 
 	if (loops == 0)
 		dev_warn(&sdd->pdev->dev, "Timed out flushing TX FIFO\n");
@@ -208,7 +232,7 @@ static void flush_fifo(struct s3c64xx_spi_driver_data *sdd)
 	loops = msecs_to_loops(1);
 	do {
 		val = readl(regs + S3C64XX_SPI_STATUS);
-		if (RX_FIFO_LVL(val, sci))
+		if (RX_FIFO_LVL(val, sdd))
 			readl(regs + S3C64XX_SPI_RX_DATA);
 		else
 			break;
@@ -262,14 +286,24 @@ static void prepare_dma(struct s3c64xx_spi_dma_data *dma,
 					unsigned len, dma_addr_t buf)
 {
 	struct s3c64xx_spi_driver_data *sdd;
-	struct samsung_dma_prep_info info;
+	struct samsung_dma_prep info;
+	struct samsung_dma_config config;
 
-	if (dma->direction == DMA_DEV_TO_MEM)
+	if (dma->direction == DMA_DEV_TO_MEM) {
 		sdd = container_of((void *)dma,
 			struct s3c64xx_spi_driver_data, rx_dma);
-	else
+		config.direction = sdd->rx_dma.direction;
+		config.fifo = sdd->sfr_start + S3C64XX_SPI_RX_DATA;
+		config.width = sdd->cur_bpw / 8;
+		sdd->ops->config(sdd->rx_dma.ch, &config);
+	} else {
 		sdd = container_of((void *)dma,
 			struct s3c64xx_spi_driver_data, tx_dma);
+		config.direction =  sdd->tx_dma.direction;
+		config.fifo = sdd->sfr_start + S3C64XX_SPI_TX_DATA;
+		config.width = sdd->cur_bpw / 8;
+		sdd->ops->config(sdd->tx_dma.ch, &config);
+	}
 
 	info.cap = DMA_SLAVE;
 	info.len = len;
@@ -284,20 +318,17 @@ static void prepare_dma(struct s3c64xx_spi_dma_data *dma,
 
 static int acquire_dma(struct s3c64xx_spi_driver_data *sdd)
 {
-	struct samsung_dma_info info;
+	struct samsung_dma_req req;
 
 	sdd->ops = samsung_dma_get_ops();
 
-	info.cap = DMA_SLAVE;
-	info.client = &s3c64xx_spi_dma_client;
-	info.width = sdd->cur_bpw / 8;
+	req.cap = DMA_SLAVE;
+	req.client = &s3c64xx_spi_dma_client;
 
-	info.direction = sdd->rx_dma.direction;
-	info.fifo = sdd->sfr_start + S3C64XX_SPI_RX_DATA;
-	sdd->rx_dma.ch = sdd->ops->request(sdd->rx_dma.dmach, &info);
-	info.direction =  sdd->tx_dma.direction;
-	info.fifo = sdd->sfr_start + S3C64XX_SPI_TX_DATA;
-	sdd->tx_dma.ch = sdd->ops->request(sdd->tx_dma.dmach, &info);
+	req.dt_dmach_prop = sdd->rx_dma.dma_prop;
+	sdd->rx_dma.ch = sdd->ops->request(sdd->rx_dma.dmach, &req);
+	req.dt_dmach_prop = sdd->tx_dma.dma_prop;
+	sdd->tx_dma.ch = sdd->ops->request(sdd->tx_dma.dmach, &req);
 
 	return 1;
 }
@@ -306,7 +337,6 @@ static void enable_datapath(struct s3c64xx_spi_driver_data *sdd,
 				struct spi_device *spi,
 				struct spi_transfer *xfer, int dma_mode)
 {
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	void __iomem *regs = sdd->regs;
 	u32 modecfg, chcfg;
 
@@ -356,7 +386,7 @@ static void enable_datapath(struct s3c64xx_spi_driver_data *sdd,
 	if (xfer->rx_buf != NULL) {
 		sdd->state |= RXBUSY;
 
-		if (sci->high_speed && sdd->cur_speed >= 30000000UL
+		if (sdd->port_conf->high_speed && sdd->cur_speed >= 30000000UL
 					&& !(sdd->cur_mode & SPI_CPHA))
 			chcfg |= S3C64XX_SPI_CH_HS_EN;
 
@@ -383,20 +413,19 @@ static inline void enable_cs(struct s3c64xx_spi_driver_data *sdd,
 		if (sdd->tgl_spi != spi) { /* if last mssg on diff device */
 			/* Deselect the last toggled device */
 			cs = sdd->tgl_spi->controller_data;
-			cs->set_level(cs->line,
-					spi->mode & SPI_CS_HIGH ? 0 : 1);
+			gpio_set_value(cs->line,
+				spi->mode & SPI_CS_HIGH ? 0 : 1);
 		}
 		sdd->tgl_spi = NULL;
 	}
 
 	cs = spi->controller_data;
-	cs->set_level(cs->line, spi->mode & SPI_CS_HIGH ? 1 : 0);
+	gpio_set_value(cs->line, spi->mode & SPI_CS_HIGH ? 1 : 0);
 }
 
 static int wait_for_xfer(struct s3c64xx_spi_driver_data *sdd,
 				struct spi_transfer *xfer, int dma_mode)
 {
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	void __iomem *regs = sdd->regs;
 	unsigned long val;
 	int ms;
@@ -413,7 +442,7 @@ static int wait_for_xfer(struct s3c64xx_spi_driver_data *sdd,
 		val = msecs_to_loops(ms);
 		do {
 			status = readl(regs + S3C64XX_SPI_STATUS);
-		} while (RX_FIFO_LVL(status, sci) < xfer->len && --val);
+		} while (RX_FIFO_LVL(status, sdd) < xfer->len && --val);
 	}
 
 	if (!val)
@@ -432,8 +461,8 @@ static int wait_for_xfer(struct s3c64xx_spi_driver_data *sdd,
 		if (xfer->rx_buf == NULL) {
 			val = msecs_to_loops(10);
 			status = readl(regs + S3C64XX_SPI_STATUS);
-			while ((TX_FIFO_LVL(status, sci)
-				|| !S3C64XX_SPI_ST_TX_DONE(status, sci))
+			while ((TX_FIFO_LVL(status, sdd)
+				|| !S3C64XX_SPI_ST_TX_DONE(status, sdd))
 					&& --val) {
 				cpu_relax();
 				status = readl(regs + S3C64XX_SPI_STATUS);
@@ -477,17 +506,16 @@ static inline void disable_cs(struct s3c64xx_spi_driver_data *sdd,
 	if (sdd->tgl_spi == spi)
 		sdd->tgl_spi = NULL;
 
-	cs->set_level(cs->line, spi->mode & SPI_CS_HIGH ? 0 : 1);
+	gpio_set_value(cs->line, spi->mode & SPI_CS_HIGH ? 0 : 1);
 }
 
 static void s3c64xx_spi_config(struct s3c64xx_spi_driver_data *sdd)
 {
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	void __iomem *regs = sdd->regs;
 	u32 val;
 
 	/* Disable Clock */
-	if (sci->clk_from_cmu) {
+	if (sdd->port_conf->clk_from_cmu) {
 		clk_disable(sdd->src_clk);
 	} else {
 		val = readl(regs + S3C64XX_SPI_CLK_CFG);
@@ -531,7 +559,7 @@ static void s3c64xx_spi_config(struct s3c64xx_spi_driver_data *sdd)
 
 	writel(val, regs + S3C64XX_SPI_MODE_CFG);
 
-	if (sci->clk_from_cmu) {
+	if (sdd->port_conf->clk_from_cmu) {
 		/* Configure Clock */
 		/* There is half-multiplier before the SPI */
 		clk_set_rate(sdd->src_clk, sdd->cur_speed * 2);
@@ -557,7 +585,6 @@ static void s3c64xx_spi_config(struct s3c64xx_spi_driver_data *sdd)
 static int s3c64xx_spi_map_mssg(struct s3c64xx_spi_driver_data *sdd,
 						struct spi_message *msg)
 {
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	struct device *dev = &sdd->pdev->dev;
 	struct spi_transfer *xfer;
 
@@ -573,7 +600,7 @@ static int s3c64xx_spi_map_mssg(struct s3c64xx_spi_driver_data *sdd,
 	/* Map until end or first fail */
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
 
-		if (xfer->len <= ((sci->fifo_lvl_mask >> 1) + 1))
+		if (xfer->len <= ((FIFO_LVL_MASK(sdd) >> 1) + 1))
 			continue;
 
 		if (xfer->tx_buf != NULL) {
@@ -607,7 +634,6 @@ static int s3c64xx_spi_map_mssg(struct s3c64xx_spi_driver_data *sdd,
 static void s3c64xx_spi_unmap_mssg(struct s3c64xx_spi_driver_data *sdd,
 						struct spi_message *msg)
 {
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	struct device *dev = &sdd->pdev->dev;
 	struct spi_transfer *xfer;
 
@@ -616,7 +642,7 @@ static void s3c64xx_spi_unmap_mssg(struct s3c64xx_spi_driver_data *sdd,
 
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
 
-		if (xfer->len <= ((sci->fifo_lvl_mask >> 1) + 1))
+		if (xfer->len <= ((FIFO_LVL_MASK(sdd) >> 1) + 1))
 			continue;
 
 		if (xfer->rx_buf != NULL
@@ -635,7 +661,6 @@ static int s3c64xx_spi_transfer_one_message(struct spi_master *master,
 					    struct spi_message *msg)
 {
 	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(master);
-	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 	struct spi_device *spi = msg->spi;
 	struct s3c64xx_spi_csinfo *cs = spi->controller_data;
 	struct spi_transfer *xfer;
@@ -690,7 +715,7 @@ static int s3c64xx_spi_transfer_one_message(struct spi_master *master,
 		}
 
 		/* Polling method for xfers not bigger than FIFO capacity */
-		if (xfer->len <= ((sci->fifo_lvl_mask >> 1) + 1))
+		if (xfer->len <= ((FIFO_LVL_MASK(sdd) >> 1) + 1))
 			use_dma = 0;
 		else
 			use_dma = 1;
@@ -707,14 +732,15 @@ static int s3c64xx_spi_transfer_one_message(struct spi_master *master,
 		enable_cs(sdd, spi);
 
 		/* Start the signals */
-		S3C64XX_SPI_ACT(sdd);
+		writel(0, sdd->regs + S3C64XX_SPI_SLAVE_SEL);
 
 		spin_unlock_irqrestore(&sdd->lock, flags);
 
 		status = wait_for_xfer(sdd, xfer, use_dma);
 
 		/* Quiese the signals */
-		S3C64XX_SPI_DEACT(sdd);
+		writel(S3C64XX_SPI_SLAVE_SIG_INACT,
+		       sdd->regs + S3C64XX_SPI_SLAVE_SEL);
 
 		if (status) {
 			dev_err(&spi->dev, "I/O Error: "
@@ -795,6 +821,48 @@ static int s3c64xx_spi_unprepare_transfer(struct spi_master *spi)
 	return 0;
 }
 
+static struct s3c64xx_spi_csinfo *s3c64xx_get_slave_ctrldata(
+				struct s3c64xx_spi_driver_data *sdd,
+				struct spi_device *spi)
+{
+	struct s3c64xx_spi_csinfo *cs;
+	struct device_node *slave_np, *data_np;
+	u32 fb_delay = 0;
+
+	slave_np = spi->dev.of_node;
+	if (!slave_np) {
+		dev_err(&spi->dev, "device node not found\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	for_each_child_of_node(slave_np, data_np)
+		if (!strcmp(data_np->name, "controller-data"))
+			break;
+	if (!data_np) {
+		dev_err(&spi->dev, "child node 'controller-data' not found\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	cs = kzalloc(sizeof(*cs), GFP_KERNEL);
+	if (!cs) {
+		dev_err(&spi->dev, "could not allocate memory for controller"
+					" data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	cs->line = of_get_named_gpio(data_np, "cs-gpio", 0);
+	if (!gpio_is_valid(cs->line)) {
+		dev_err(&spi->dev, "chip select gpio is not specified or "
+					"invalid\n");
+		kfree(cs);
+		return ERR_PTR(-EINVAL);
+	}
+
+	of_property_read_u32(data_np, "samsung,spi-feedback-delay", &fb_delay);
+	cs->fb_delay = fb_delay;
+	return cs;
+}
+
 /*
  * Here we only check the validity of requested configuration
  * and save the configuration in a local data-structure.
@@ -808,14 +876,31 @@ static int s3c64xx_spi_setup(struct spi_device *spi)
 	struct s3c64xx_spi_info *sci;
 	struct spi_message *msg;
 	unsigned long flags;
-	int err = 0;
+	int err;
 
-	if (cs == NULL || cs->set_level == NULL) {
+	sdd = spi_master_get_devdata(spi->master);
+	if (!cs && spi->dev.of_node) {
+		cs = s3c64xx_get_slave_ctrldata(sdd, spi);
+		spi->controller_data = cs;
+	}
+
+	if (IS_ERR_OR_NULL(cs)) {
 		dev_err(&spi->dev, "No CS for SPI(%d)\n", spi->chip_select);
 		return -ENODEV;
 	}
 
-	sdd = spi_master_get_devdata(spi->master);
+	if (!spi_get_ctldata(spi)) {
+		err = gpio_request_one(cs->line, GPIOF_OUT_INIT_HIGH,
+				       dev_name(&spi->dev));
+		if (err) {
+			dev_err(&spi->dev,
+				"Failed to get /CS gpio [%d]: %d\n",
+				cs->line, err);
+			goto err_gpio_req;
+		}
+		spi_set_ctldata(spi, cs);
+	}
+
 	sci = sdd->cntrlr_info;
 
 	spin_lock_irqsave(&sdd->lock, flags);
@@ -826,7 +911,8 @@ static int s3c64xx_spi_setup(struct spi_device *spi)
 			dev_err(&spi->dev,
 				"setup: attempt while mssg in queue!\n");
 			spin_unlock_irqrestore(&sdd->lock, flags);
-			return -EBUSY;
+			err = -EBUSY;
+			goto err_msgq;
 		}
 	}
 
@@ -844,7 +930,7 @@ static int s3c64xx_spi_setup(struct spi_device *spi)
 	pm_runtime_get_sync(&sdd->pdev->dev);
 
 	/* Check if we can provide the requested rate */
-	if (!sci->clk_from_cmu) {
+	if (!sdd->port_conf->clk_from_cmu) {
 		u32 psr, speed;
 
 		/* Max possible */
@@ -869,22 +955,44 @@ static int s3c64xx_spi_setup(struct spi_device *spi)
 		}
 
 		speed = clk_get_rate(sdd->src_clk) / 2 / (psr + 1);
-		if (spi->max_speed_hz >= speed)
+		if (spi->max_speed_hz >= speed) {
 			spi->max_speed_hz = speed;
-		else
+		} else {
 			err = -EINVAL;
+			goto setup_exit;
+		}
 	}
 
 	pm_runtime_put(&sdd->pdev->dev);
+	disable_cs(sdd, spi);
+	return 0;
 
 setup_exit:
-
 	/* setup() returns with device de-selected */
 	disable_cs(sdd, spi);
 
+err_msgq:
+	gpio_free(cs->line);
+	spi_set_ctldata(spi, NULL);
+
+err_gpio_req:
+	kfree(cs);
+
 	return err;
 }
 
+static void s3c64xx_spi_cleanup(struct spi_device *spi)
+{
+	struct s3c64xx_spi_csinfo *cs = spi_get_ctldata(spi);
+
+	if (cs) {
+		gpio_free(cs->line);
+		if (spi->dev.of_node)
+			kfree(cs);
+	}
+	spi_set_ctldata(spi, NULL);
+}
+
 static irqreturn_t s3c64xx_spi_irq(int irq, void *data)
 {
 	struct s3c64xx_spi_driver_data *sdd = data;
@@ -920,12 +1028,12 @@ static void s3c64xx_spi_hwinit(struct s3c64xx_spi_driver_data *sdd, int channel)
 
 	sdd->cur_speed = 0;
 
-	S3C64XX_SPI_DEACT(sdd);
+	writel(S3C64XX_SPI_SLAVE_SIG_INACT, sdd->regs + S3C64XX_SPI_SLAVE_SEL);
 
 	/* Disable Interrupts - we use Polling if not DMA mode */
 	writel(0, regs + S3C64XX_SPI_INT_EN);
 
-	if (!sci->clk_from_cmu)
+	if (!sdd->port_conf->clk_from_cmu)
 		writel(sci->src_clk_nr << S3C64XX_SPI_CLKSEL_SRCSHFT,
 				regs + S3C64XX_SPI_CLK_CFG);
 	writel(0, regs + S3C64XX_SPI_MODE_CFG);
@@ -946,40 +1054,165 @@ static void s3c64xx_spi_hwinit(struct s3c64xx_spi_driver_data *sdd, int channel)
 	flush_fifo(sdd);
 }
 
-static int __init s3c64xx_spi_probe(struct platform_device *pdev)
+static int __devinit s3c64xx_spi_get_dmares(
+			struct s3c64xx_spi_driver_data *sdd, bool tx)
+{
+	struct platform_device *pdev = sdd->pdev;
+	struct s3c64xx_spi_dma_data *dma_data;
+	struct property *prop;
+	struct resource *res;
+	char prop_name[15], *chan_str;
+
+	if (tx) {
+		dma_data = &sdd->tx_dma;
+		dma_data->direction = DMA_TO_DEVICE;
+		chan_str = "tx";
+	} else {
+		dma_data = &sdd->rx_dma;
+		dma_data->direction = DMA_FROM_DEVICE;
+		chan_str = "rx";
+	}
+
+	if (!sdd->pdev->dev.of_node) {
+		res = platform_get_resource(pdev, IORESOURCE_DMA, tx ? 0 : 1);
+		if (!res) {
+			dev_err(&pdev->dev, "Unable to get SPI-%s dma "
+					"resource\n", chan_str);
+			return -ENXIO;
+		}
+		dma_data->dmach = res->start;
+		return 0;
+	}
+
+	sprintf(prop_name, "%s-dma-channel", chan_str);
+	prop = of_find_property(pdev->dev.of_node, prop_name, NULL);
+	if (!prop) {
+		dev_err(&pdev->dev, "%s dma channel property not specified\n",
+					chan_str);
+		return -ENXIO;
+	}
+
+	dma_data->dmach = DMACH_DT_PROP;
+	dma_data->dma_prop = prop;
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static int s3c64xx_spi_parse_dt_gpio(struct s3c64xx_spi_driver_data *sdd)
+{
+	struct device *dev = &sdd->pdev->dev;
+	int idx, gpio, ret;
+
+	/* find gpios for mosi, miso and clock lines */
+	for (idx = 0; idx < 3; idx++) {
+		gpio = of_get_gpio(dev->of_node, idx);
+		if (!gpio_is_valid(gpio)) {
+			dev_err(dev, "invalid gpio[%d]: %d\n", idx, gpio);
+			goto free_gpio;
+		}
+
+		ret = gpio_request(gpio, "spi-bus");
+		if (ret) {
+			dev_err(dev, "gpio [%d] request failed: %d\n",
+				gpio, ret);
+			goto free_gpio;
+		}
+	}
+	return 0;
+
+free_gpio:
+	while (--idx >= 0)
+		gpio_free(sdd->gpios[idx]);
+	return -EINVAL;
+}
+
+static void s3c64xx_spi_dt_gpio_free(struct s3c64xx_spi_driver_data *sdd)
+{
+	unsigned int idx;
+	for (idx = 0; idx < 3; idx++)
+		gpio_free(sdd->gpios[idx]);
+}
+
+static struct __devinit s3c64xx_spi_info * s3c64xx_spi_parse_dt(
+						struct device *dev)
 {
-	struct resource	*mem_res, *dmatx_res, *dmarx_res;
-	struct s3c64xx_spi_driver_data *sdd;
 	struct s3c64xx_spi_info *sci;
-	struct spi_master *master;
-	int ret, irq;
-	char clk_name[16];
+	u32 temp;
 
-	if (pdev->id < 0) {
-		dev_err(&pdev->dev,
-				"Invalid platform device id-%d\n", pdev->id);
-		return -ENODEV;
+	sci = devm_kzalloc(dev, sizeof(*sci), GFP_KERNEL);
+	if (!sci) {
+		dev_err(dev, "memory allocation for spi_info failed\n");
+		return ERR_PTR(-ENOMEM);
 	}
 
-	if (pdev->dev.platform_data == NULL) {
-		dev_err(&pdev->dev, "platform_data missing!\n");
-		return -ENODEV;
+	if (of_property_read_u32(dev->of_node, "samsung,spi-src-clk", &temp)) {
+		dev_warn(dev, "spi bus clock parent not specified, using "
+				"clock at index 0 as parent\n");
+		sci->src_clk_nr = 0;
+	} else {
+		sci->src_clk_nr = temp;
 	}
 
-	sci = pdev->dev.platform_data;
+	if (of_property_read_u32(dev->of_node, "num-cs", &temp)) {
+		dev_warn(dev, "number of chip select lines not specified, "
+				"assuming 1 chip select line\n");
+		sci->num_cs = 1;
+	} else {
+		sci->num_cs = temp;
+	}
+
+	return sci;
+}
+#else
+static struct s3c64xx_spi_info *s3c64xx_spi_parse_dt(struct device *dev)
+{
+	return dev->platform_data;
+}
+
+static int s3c64xx_spi_parse_dt_gpio(struct s3c64xx_spi_driver_data *sdd)
+{
+	return -EINVAL;
+}
+
+static void s3c64xx_spi_dt_gpio_free(struct s3c64xx_spi_driver_data *sdd)
+{
+}
+#endif
 
-	/* Check for availability of necessary resource */
+static const struct of_device_id s3c64xx_spi_dt_match[];
 
-	dmatx_res = platform_get_resource(pdev, IORESOURCE_DMA, 0);
-	if (dmatx_res == NULL) {
-		dev_err(&pdev->dev, "Unable to get SPI-Tx dma resource\n");
-		return -ENXIO;
+static inline struct s3c64xx_spi_port_config *s3c64xx_spi_get_port_config(
+						struct platform_device *pdev)
+{
+#ifdef CONFIG_OF
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(s3c64xx_spi_dt_match, pdev->dev.of_node);
+		return (struct s3c64xx_spi_port_config *)match->data;
 	}
+#endif
+	return (struct s3c64xx_spi_port_config *)
+			 platform_get_device_id(pdev)->driver_data;
+}
 
-	dmarx_res = platform_get_resource(pdev, IORESOURCE_DMA, 1);
-	if (dmarx_res == NULL) {
-		dev_err(&pdev->dev, "Unable to get SPI-Rx dma resource\n");
-		return -ENXIO;
+static int __init s3c64xx_spi_probe(struct platform_device *pdev)
+{
+	struct resource	*mem_res;
+	struct s3c64xx_spi_driver_data *sdd;
+	struct s3c64xx_spi_info *sci = pdev->dev.platform_data;
+	struct spi_master *master;
+	int ret, irq;
+	char clk_name[16];
+
+	if (!sci && pdev->dev.of_node) {
+		sci = s3c64xx_spi_parse_dt(&pdev->dev);
+		if (IS_ERR(sci))
+			return PTR_ERR(sci);
+	}
+
+	if (!sci) {
+		dev_err(&pdev->dev, "platform_data missing!\n");
+		return -ENODEV;
 	}
 
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1004,19 +1237,37 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, master);
 
 	sdd = spi_master_get_devdata(master);
+	sdd->port_conf = s3c64xx_spi_get_port_config(pdev);
 	sdd->master = master;
 	sdd->cntrlr_info = sci;
 	sdd->pdev = pdev;
 	sdd->sfr_start = mem_res->start;
-	sdd->tx_dma.dmach = dmatx_res->start;
-	sdd->tx_dma.direction = DMA_MEM_TO_DEV;
-	sdd->rx_dma.dmach = dmarx_res->start;
-	sdd->rx_dma.direction = DMA_DEV_TO_MEM;
+	if (pdev->dev.of_node) {
+		ret = of_alias_get_id(pdev->dev.of_node, "spi");
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to get alias id, "
+						"errno %d\n", ret);
+			goto err0;
+		}
+		sdd->port_id = ret;
+	} else {
+		sdd->port_id = pdev->id;
+	}
 
 	sdd->cur_bpw = 8;
 
-	master->bus_num = pdev->id;
+	ret = s3c64xx_spi_get_dmares(sdd, true);
+	if (ret)
+		goto err0;
+
+	ret = s3c64xx_spi_get_dmares(sdd, false);
+	if (ret)
+		goto err0;
+
+	master->dev.of_node = pdev->dev.of_node;
+	master->bus_num = sdd->port_id;
 	master->setup = s3c64xx_spi_setup;
+	master->cleanup = s3c64xx_spi_cleanup;
 	master->prepare_transfer_hardware = s3c64xx_spi_prepare_transfer;
 	master->transfer_one_message = s3c64xx_spi_transfer_one_message;
 	master->unprepare_transfer_hardware = s3c64xx_spi_unprepare_transfer;
@@ -1025,21 +1276,17 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 	/* the spi->mode bits understood by this driver: */
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
 
-	if (request_mem_region(mem_res->start,
-			resource_size(mem_res), pdev->name) == NULL) {
-		dev_err(&pdev->dev, "Req mem region failed\n");
-		ret = -ENXIO;
-		goto err0;
-	}
-
-	sdd->regs = ioremap(mem_res->start, resource_size(mem_res));
+	sdd->regs = devm_request_and_ioremap(&pdev->dev, mem_res);
 	if (sdd->regs == NULL) {
 		dev_err(&pdev->dev, "Unable to remap IO\n");
 		ret = -ENXIO;
 		goto err1;
 	}
 
-	if (sci->cfg_gpio == NULL || sci->cfg_gpio(pdev)) {
+	if (!sci->cfg_gpio && pdev->dev.of_node) {
+		if (s3c64xx_spi_parse_dt_gpio(sdd))
+			return -EBUSY;
+	} else if (sci->cfg_gpio == NULL || sci->cfg_gpio()) {
 		dev_err(&pdev->dev, "Unable to config gpio\n");
 		ret = -EBUSY;
 		goto err2;
@@ -1075,7 +1322,7 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 	}
 
 	/* Setup Deufult Mode */
-	s3c64xx_spi_hwinit(sdd, pdev->id);
+	s3c64xx_spi_hwinit(sdd, sdd->port_id);
 
 	spin_lock_init(&sdd->lock);
 	init_completion(&sdd->xfer_completion);
@@ -1100,7 +1347,7 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Samsung SoC SPI Driver loaded for Bus SPI-%d "
 					"with %d Slaves attached\n",
-					pdev->id, master->num_chipselect);
+					sdd->port_id, master->num_chipselect);
 	dev_dbg(&pdev->dev, "\tIOmem=[0x%x-0x%x]\tDMA=[Rx-%d, Tx-%d]\n",
 					mem_res->end, mem_res->start,
 					sdd->rx_dma.dmach, sdd->tx_dma.dmach);
@@ -1120,10 +1367,10 @@ err5:
 err4:
 	clk_put(sdd->clk);
 err3:
+	if (!sdd->cntrlr_info->cfg_gpio && pdev->dev.of_node)
+		s3c64xx_spi_dt_gpio_free(sdd);
 err2:
-	iounmap((void *) sdd->regs);
 err1:
-	release_mem_region(mem_res->start, resource_size(mem_res));
 err0:
 	platform_set_drvdata(pdev, NULL);
 	spi_master_put(master);
@@ -1135,7 +1382,6 @@ static int s3c64xx_spi_remove(struct platform_device *pdev)
 {
 	struct spi_master *master = spi_master_get(platform_get_drvdata(pdev));
 	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(master);
-	struct resource	*mem_res;
 
 	pm_runtime_disable(&pdev->dev);
 
@@ -1151,11 +1397,8 @@ static int s3c64xx_spi_remove(struct platform_device *pdev)
 	clk_disable(sdd->clk);
 	clk_put(sdd->clk);
 
-	iounmap((void *) sdd->regs);
-
-	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (mem_res != NULL)
-		release_mem_region(mem_res->start, resource_size(mem_res));
+	if (!sdd->cntrlr_info->cfg_gpio && pdev->dev.of_node)
+		s3c64xx_spi_dt_gpio_free(sdd);
 
 	platform_set_drvdata(pdev, NULL);
 	spi_master_put(master);
@@ -1175,6 +1418,9 @@ static int s3c64xx_spi_suspend(struct device *dev)
 	clk_disable(sdd->src_clk);
 	clk_disable(sdd->clk);
 
+	if (!sdd->cntrlr_info->cfg_gpio && dev->of_node)
+		s3c64xx_spi_dt_gpio_free(sdd);
+
 	sdd->cur_speed = 0; /* Output Clock is stopped */
 
 	return 0;
@@ -1182,18 +1428,20 @@ static int s3c64xx_spi_suspend(struct device *dev)
 
 static int s3c64xx_spi_resume(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
 	struct spi_master *master = spi_master_get(dev_get_drvdata(dev));
 	struct s3c64xx_spi_driver_data *sdd = spi_master_get_devdata(master);
 	struct s3c64xx_spi_info *sci = sdd->cntrlr_info;
 
-	sci->cfg_gpio(pdev);
+	if (!sci->cfg_gpio && dev->of_node)
+		s3c64xx_spi_parse_dt_gpio(sdd);
+	else
+		sci->cfg_gpio();
 
 	/* Enable the clock */
 	clk_enable(sdd->src_clk);
 	clk_enable(sdd->clk);
 
-	s3c64xx_spi_hwinit(sdd, pdev->id);
+	s3c64xx_spi_hwinit(sdd, sdd->port_id);
 
 	spi_master_resume(master);
 
@@ -1231,13 +1479,89 @@ static const struct dev_pm_ops s3c64xx_spi_pm = {
 			   s3c64xx_spi_runtime_resume, NULL)
 };
 
+struct s3c64xx_spi_port_config s3c2443_spi_port_config = {
+	.fifo_lvl_mask	= { 0x7f },
+	.rx_lvl_offset	= 13,
+	.tx_st_done	= 21,
+	.high_speed	= true,
+};
+
+struct s3c64xx_spi_port_config s3c6410_spi_port_config = {
+	.fifo_lvl_mask	= { 0x7f, 0x7F },
+	.rx_lvl_offset	= 13,
+	.tx_st_done	= 21,
+};
+
+struct s3c64xx_spi_port_config s5p64x0_spi_port_config = {
+	.fifo_lvl_mask	= { 0x1ff, 0x7F },
+	.rx_lvl_offset	= 15,
+	.tx_st_done	= 25,
+};
+
+struct s3c64xx_spi_port_config s5pc100_spi_port_config = {
+	.fifo_lvl_mask	= { 0x7f, 0x7F },
+	.rx_lvl_offset	= 13,
+	.tx_st_done	= 21,
+	.high_speed	= true,
+};
+
+struct s3c64xx_spi_port_config s5pv210_spi_port_config = {
+	.fifo_lvl_mask	= { 0x1ff, 0x7F },
+	.rx_lvl_offset	= 15,
+	.tx_st_done	= 25,
+	.high_speed	= true,
+};
+
+struct s3c64xx_spi_port_config exynos4_spi_port_config = {
+	.fifo_lvl_mask	= { 0x1ff, 0x7F, 0x7F },
+	.rx_lvl_offset	= 15,
+	.tx_st_done	= 25,
+	.high_speed	= true,
+	.clk_from_cmu	= true,
+};
+
+static struct platform_device_id s3c64xx_spi_driver_ids[] = {
+	{
+		.name		= "s3c2443-spi",
+		.driver_data	= (kernel_ulong_t)&s3c2443_spi_port_config,
+	}, {
+		.name		= "s3c6410-spi",
+		.driver_data	= (kernel_ulong_t)&s3c6410_spi_port_config,
+	}, {
+		.name		= "s5p64x0-spi",
+		.driver_data	= (kernel_ulong_t)&s5p64x0_spi_port_config,
+	}, {
+		.name		= "s5pc100-spi",
+		.driver_data	= (kernel_ulong_t)&s5pc100_spi_port_config,
+	}, {
+		.name		= "s5pv210-spi",
+		.driver_data	= (kernel_ulong_t)&s5pv210_spi_port_config,
+	}, {
+		.name		= "exynos4210-spi",
+		.driver_data	= (kernel_ulong_t)&exynos4_spi_port_config,
+	},
+	{ },
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id s3c64xx_spi_dt_match[] = {
+	{ .compatible = "samsung,exynos4210-spi",
+			.data = (void *)&exynos4_spi_port_config,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, s3c64xx_spi_dt_match);
+#endif /* CONFIG_OF */
+
 static struct platform_driver s3c64xx_spi_driver = {
 	.driver = {
 		.name	= "s3c64xx-spi",
 		.owner = THIS_MODULE,
 		.pm = &s3c64xx_spi_pm,
+		.of_match_table = of_match_ptr(s3c64xx_spi_dt_match),
 	},
 	.remove = s3c64xx_spi_remove,
+	.id_table = s3c64xx_spi_driver_ids,
 };
 MODULE_ALIAS("platform:s3c64xx-spi");
 
diff --git a/drivers/spi/spi-tegra.c b/drivers/spi/spi-tegra.c
index ae6d78a3e91..ef52c1c6f5c 100644
--- a/drivers/spi/spi-tegra.c
+++ b/drivers/spi/spi-tegra.c
@@ -30,6 +30,7 @@
 #include <linux/delay.h>
 
 #include <linux/spi/spi.h>
+#include <linux/dmaengine.h>
 
 #include <mach/dma.h>
 
@@ -162,12 +163,23 @@ struct spi_tegra_data {
 	 * require transfers to be 4 byte aligned we need a bounce buffer
 	 * for the generic case.
 	 */
+	int			dma_req_len;
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
 	struct tegra_dma_req	rx_dma_req;
 	struct tegra_dma_channel *rx_dma;
+#else
+	struct dma_chan		*rx_dma;
+	struct dma_slave_config	sconfig;
+	struct dma_async_tx_descriptor	*rx_dma_desc;
+	dma_cookie_t		rx_cookie;
+#endif
 	u32			*rx_bb;
 	dma_addr_t		rx_bb_phys;
 };
 
+#if !defined(CONFIG_TEGRA_SYSTEM_DMA)
+static void tegra_spi_rx_dma_complete(void *args);
+#endif
 
 static inline unsigned long spi_tegra_readl(struct spi_tegra_data *tspi,
 					    unsigned long reg)
@@ -190,10 +202,24 @@ static void spi_tegra_go(struct spi_tegra_data *tspi)
 
 	val = spi_tegra_readl(tspi, SLINK_DMA_CTL);
 	val &= ~SLINK_DMA_BLOCK_SIZE(~0) & ~SLINK_DMA_EN;
-	val |= SLINK_DMA_BLOCK_SIZE(tspi->rx_dma_req.size / 4 - 1);
+	val |= SLINK_DMA_BLOCK_SIZE(tspi->dma_req_len / 4 - 1);
 	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
-
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
+	tspi->rx_dma_req.size = tspi->dma_req_len;
 	tegra_dma_enqueue_req(tspi->rx_dma, &tspi->rx_dma_req);
+#else
+	tspi->rx_dma_desc = dmaengine_prep_slave_single(tspi->rx_dma,
+				tspi->rx_bb_phys, tspi->dma_req_len,
+				DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT);
+	if (!tspi->rx_dma_desc) {
+		dev_err(&tspi->pdev->dev, "dmaengine slave prep failed\n");
+		return;
+	}
+	tspi->rx_dma_desc->callback = tegra_spi_rx_dma_complete;
+	tspi->rx_dma_desc->callback_param = tspi;
+	tspi->rx_cookie = dmaengine_submit(tspi->rx_dma_desc);
+	dma_async_issue_pending(tspi->rx_dma);
+#endif
 
 	val |= SLINK_DMA_EN;
 	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
@@ -221,7 +247,7 @@ static unsigned spi_tegra_fill_tx_fifo(struct spi_tegra_data *tspi,
 		spi_tegra_writel(tspi, val, SLINK_TX_FIFO);
 	}
 
-	tspi->rx_dma_req.size = len / tspi->cur_bytes_per_word * 4;
+	tspi->dma_req_len = len / tspi->cur_bytes_per_word * 4;
 
 	return len;
 }
@@ -261,7 +287,7 @@ static void spi_tegra_start_transfer(struct spi_device *spi,
 		clk_set_rate(tspi->clk, speed);
 
 	if (tspi->cur_speed == 0)
-		clk_enable(tspi->clk);
+		clk_prepare_enable(tspi->clk);
 
 	tspi->cur_speed = speed;
 
@@ -318,9 +344,8 @@ static void spi_tegra_start_message(struct spi_device *spi,
 	spi_tegra_start_transfer(spi, t);
 }
 
-static void tegra_spi_rx_dma_complete(struct tegra_dma_req *req)
+static void handle_spi_rx_dma_complete(struct spi_tegra_data *tspi)
 {
-	struct spi_tegra_data *tspi = req->dev;
 	unsigned long flags;
 	struct spi_message *m;
 	struct spi_device *spi;
@@ -373,13 +398,26 @@ static void tegra_spi_rx_dma_complete(struct tegra_dma_req *req)
 			spi = m->state;
 			spi_tegra_start_message(spi, m);
 		} else {
-			clk_disable(tspi->clk);
+			clk_disable_unprepare(tspi->clk);
 			tspi->cur_speed = 0;
 		}
 	}
 
 	spin_unlock_irqrestore(&tspi->lock, flags);
 }
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
+static void tegra_spi_rx_dma_complete(struct tegra_dma_req *req)
+{
+	struct spi_tegra_data *tspi = req->dev;
+	handle_spi_rx_dma_complete(tspi);
+}
+#else
+static void tegra_spi_rx_dma_complete(void *args)
+{
+	struct spi_tegra_data *tspi = args;
+	handle_spi_rx_dma_complete(tspi);
+}
+#endif
 
 static int spi_tegra_setup(struct spi_device *spi)
 {
@@ -471,6 +509,9 @@ static int __devinit spi_tegra_probe(struct platform_device *pdev)
 	struct spi_tegra_data	*tspi;
 	struct resource		*r;
 	int ret;
+#if !defined(CONFIG_TEGRA_SYSTEM_DMA)
+	dma_cap_mask_t mask;
+#endif
 
 	master = spi_alloc_master(&pdev->dev, sizeof *tspi);
 	if (master == NULL) {
@@ -522,12 +563,24 @@ static int __devinit spi_tegra_probe(struct platform_device *pdev)
 
 	INIT_LIST_HEAD(&tspi->queue);
 
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
 	tspi->rx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_ONESHOT);
 	if (!tspi->rx_dma) {
 		dev_err(&pdev->dev, "can not allocate rx dma channel\n");
 		ret = -ENODEV;
 		goto err3;
 	}
+#else
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	tspi->rx_dma = dma_request_channel(mask, NULL, NULL);
+	if (!tspi->rx_dma) {
+		dev_err(&pdev->dev, "can not allocate rx dma channel\n");
+		ret = -ENODEV;
+		goto err3;
+	}
+
+#endif
 
 	tspi->rx_bb = dma_alloc_coherent(&pdev->dev, sizeof(u32) * BB_LEN,
 					 &tspi->rx_bb_phys, GFP_KERNEL);
@@ -537,6 +590,7 @@ static int __devinit spi_tegra_probe(struct platform_device *pdev)
 		goto err4;
 	}
 
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
 	tspi->rx_dma_req.complete = tegra_spi_rx_dma_complete;
 	tspi->rx_dma_req.to_memory = 1;
 	tspi->rx_dma_req.dest_addr = tspi->rx_bb_phys;
@@ -546,6 +600,23 @@ static int __devinit spi_tegra_probe(struct platform_device *pdev)
 	tspi->rx_dma_req.source_wrap = 4;
 	tspi->rx_dma_req.req_sel = spi_tegra_req_sels[pdev->id];
 	tspi->rx_dma_req.dev = tspi;
+#else
+	/* Dmaengine Dma slave config */
+	tspi->sconfig.src_addr = tspi->phys + SLINK_RX_FIFO;
+	tspi->sconfig.dst_addr = tspi->phys + SLINK_RX_FIFO;
+	tspi->sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	tspi->sconfig.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	tspi->sconfig.slave_id = spi_tegra_req_sels[pdev->id];
+	tspi->sconfig.src_maxburst = 1;
+	tspi->sconfig.dst_maxburst = 1;
+	ret = dmaengine_device_control(tspi->rx_dma,
+			DMA_SLAVE_CONFIG, (unsigned long) &tspi->sconfig);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can not do slave configure for dma %d\n",
+			ret);
+		goto err4;
+	}
+#endif
 
 	master->dev.of_node = pdev->dev.of_node;
 	ret = spi_register_master(master);
@@ -559,7 +630,11 @@ err5:
 	dma_free_coherent(&pdev->dev, sizeof(u32) * BB_LEN,
 			  tspi->rx_bb, tspi->rx_bb_phys);
 err4:
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
 	tegra_dma_free_channel(tspi->rx_dma);
+#else
+	dma_release_channel(tspi->rx_dma);
+#endif
 err3:
 	clk_put(tspi->clk);
 err2:
@@ -581,7 +656,11 @@ static int __devexit spi_tegra_remove(struct platform_device *pdev)
 	tspi = spi_master_get_devdata(master);
 
 	spi_unregister_master(master);
+#if defined(CONFIG_TEGRA_SYSTEM_DMA)
 	tegra_dma_free_channel(tspi->rx_dma);
+#else
+	dma_release_channel(tspi->rx_dma);
+#endif
 
 	dma_free_coherent(&pdev->dev, sizeof(u32) * BB_LEN,
 			  tspi->rx_bb, tspi->rx_bb_phys);
diff --git a/drivers/spi/spi-xcomm.c b/drivers/spi/spi-xcomm.c
new file mode 100644
index 00000000000..266a847e299
--- /dev/null
+++ b/drivers/spi/spi-xcomm.c
@@ -0,0 +1,276 @@
+/*
+ * Analog Devices AD-FMCOMMS1-EBZ board I2C-SPI bridge driver
+ *
+ * Copyright 2012 Analog Devices Inc.
+ * Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/spi/spi.h>
+#include <asm/unaligned.h>
+
+#define SPI_XCOMM_SETTINGS_LEN_OFFSET		10
+#define SPI_XCOMM_SETTINGS_3WIRE		BIT(6)
+#define SPI_XCOMM_SETTINGS_CS_HIGH		BIT(5)
+#define SPI_XCOMM_SETTINGS_SAMPLE_END		BIT(4)
+#define SPI_XCOMM_SETTINGS_CPHA			BIT(3)
+#define SPI_XCOMM_SETTINGS_CPOL			BIT(2)
+#define SPI_XCOMM_SETTINGS_CLOCK_DIV_MASK	0x3
+#define SPI_XCOMM_SETTINGS_CLOCK_DIV_64		0x2
+#define SPI_XCOMM_SETTINGS_CLOCK_DIV_16		0x1
+#define SPI_XCOMM_SETTINGS_CLOCK_DIV_4		0x0
+
+#define SPI_XCOMM_CMD_UPDATE_CONFIG	0x03
+#define SPI_XCOMM_CMD_WRITE		0x04
+
+#define SPI_XCOMM_CLOCK 48000000
+
+struct spi_xcomm {
+	struct i2c_client *i2c;
+
+	uint16_t settings;
+	uint16_t chipselect;
+
+	unsigned int current_speed;
+
+	uint8_t buf[63];
+};
+
+static int spi_xcomm_sync_config(struct spi_xcomm *spi_xcomm, unsigned int len)
+{
+	uint16_t settings;
+	uint8_t *buf = spi_xcomm->buf;
+
+	settings = spi_xcomm->settings;
+	settings |= len << SPI_XCOMM_SETTINGS_LEN_OFFSET;
+
+	buf[0] = SPI_XCOMM_CMD_UPDATE_CONFIG;
+	put_unaligned_be16(settings, &buf[1]);
+	put_unaligned_be16(spi_xcomm->chipselect, &buf[3]);
+
+	return i2c_master_send(spi_xcomm->i2c, buf, 5);
+}
+
+static void spi_xcomm_chipselect(struct spi_xcomm *spi_xcomm,
+	struct spi_device *spi, int is_active)
+{
+	unsigned long cs = spi->chip_select;
+	uint16_t chipselect = spi_xcomm->chipselect;
+
+	if (is_active)
+		chipselect |= BIT(cs);
+	else
+		chipselect &= ~BIT(cs);
+
+	spi_xcomm->chipselect = chipselect;
+}
+
+static int spi_xcomm_setup_transfer(struct spi_xcomm *spi_xcomm,
+	struct spi_device *spi, struct spi_transfer *t, unsigned int *settings)
+{
+	unsigned int speed;
+
+	if ((t->bits_per_word && t->bits_per_word != 8) || t->len > 62)
+		return -EINVAL;
+
+	speed = t->speed_hz ? t->speed_hz : spi->max_speed_hz;
+
+	if (speed != spi_xcomm->current_speed) {
+		unsigned int divider = DIV_ROUND_UP(SPI_XCOMM_CLOCK, speed);
+		if (divider >= 64)
+			*settings |= SPI_XCOMM_SETTINGS_CLOCK_DIV_64;
+		else if (divider >= 16)
+			*settings |= SPI_XCOMM_SETTINGS_CLOCK_DIV_16;
+		else
+			*settings |= SPI_XCOMM_SETTINGS_CLOCK_DIV_4;
+
+		spi_xcomm->current_speed = speed;
+	}
+
+	if (spi->mode & SPI_CPOL)
+		*settings |= SPI_XCOMM_SETTINGS_CPOL;
+	else
+		*settings &= ~SPI_XCOMM_SETTINGS_CPOL;
+
+	if (spi->mode & SPI_CPHA)
+		*settings &= ~SPI_XCOMM_SETTINGS_CPHA;
+	else
+		*settings |= SPI_XCOMM_SETTINGS_CPHA;
+
+	if (spi->mode & SPI_3WIRE)
+		*settings |= SPI_XCOMM_SETTINGS_3WIRE;
+	else
+		*settings &= ~SPI_XCOMM_SETTINGS_3WIRE;
+
+	return 0;
+}
+
+static int spi_xcomm_txrx_bufs(struct spi_xcomm *spi_xcomm,
+	struct spi_device *spi, struct spi_transfer *t)
+{
+	int ret;
+
+	if (t->tx_buf) {
+		spi_xcomm->buf[0] = SPI_XCOMM_CMD_WRITE;
+		memcpy(spi_xcomm->buf + 1, t->tx_buf, t->len);
+
+		ret = i2c_master_send(spi_xcomm->i2c, spi_xcomm->buf, t->len + 1);
+		if (ret < 0)
+			return ret;
+		else if (ret != t->len + 1)
+			return -EIO;
+	} else if (t->rx_buf) {
+		ret = i2c_master_recv(spi_xcomm->i2c, t->rx_buf, t->len);
+		if (ret < 0)
+			return ret;
+		else if (ret != t->len)
+			return -EIO;
+	}
+
+	return t->len;
+}
+
+static int spi_xcomm_transfer_one(struct spi_master *master,
+	struct spi_message *msg)
+{
+	struct spi_xcomm *spi_xcomm = spi_master_get_devdata(master);
+	unsigned int settings = spi_xcomm->settings;
+	struct spi_device *spi = msg->spi;
+	unsigned cs_change = 0;
+	struct spi_transfer *t;
+	bool is_first = true;
+	int status = 0;
+	bool is_last;
+
+	is_first = true;
+
+	spi_xcomm_chipselect(spi_xcomm, spi, true);
+
+	list_for_each_entry(t, &msg->transfers, transfer_list) {
+
+		if (!t->tx_buf && !t->rx_buf && t->len) {
+			status = -EINVAL;
+			break;
+		}
+
+		status = spi_xcomm_setup_transfer(spi_xcomm, spi, t, &settings);
+		if (status < 0)
+			break;
+
+		is_last = list_is_last(&t->transfer_list, &msg->transfers);
+		cs_change = t->cs_change;
+
+		if (cs_change ^ is_last)
+			settings |= BIT(5);
+		else
+			settings &= ~BIT(5);
+
+		if (t->rx_buf) {
+			spi_xcomm->settings = settings;
+			status = spi_xcomm_sync_config(spi_xcomm, t->len);
+			if (status < 0)
+				break;
+		} else if (settings != spi_xcomm->settings || is_first) {
+			spi_xcomm->settings = settings;
+			status = spi_xcomm_sync_config(spi_xcomm, 0);
+			if (status < 0)
+				break;
+		}
+
+		if (t->len) {
+			status = spi_xcomm_txrx_bufs(spi_xcomm, spi, t);
+
+			if (status < 0)
+				break;
+
+			if (status > 0)
+				msg->actual_length += status;
+		}
+		status = 0;
+
+		if (t->delay_usecs)
+			udelay(t->delay_usecs);
+
+		is_first = false;
+	}
+
+	if (status != 0 || !cs_change)
+		spi_xcomm_chipselect(spi_xcomm, spi, false);
+
+	msg->status = status;
+	spi_finalize_current_message(master);
+
+	return status;
+}
+
+static int spi_xcomm_setup(struct spi_device *spi)
+{
+	if (spi->bits_per_word != 8)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int __devinit spi_xcomm_probe(struct i2c_client *i2c,
+	const struct i2c_device_id *id)
+{
+	struct spi_xcomm *spi_xcomm;
+	struct spi_master *master;
+	int ret;
+
+	master = spi_alloc_master(&i2c->dev, sizeof(*spi_xcomm));
+	if (!master)
+		return -ENOMEM;
+
+	spi_xcomm = spi_master_get_devdata(master);
+	spi_xcomm->i2c = i2c;
+
+	master->num_chipselect = 16;
+	master->mode_bits = SPI_CPHA | SPI_CPOL | SPI_3WIRE;
+	master->flags = SPI_MASTER_HALF_DUPLEX;
+	master->setup = spi_xcomm_setup;
+	master->transfer_one_message = spi_xcomm_transfer_one;
+	master->dev.of_node = i2c->dev.of_node;
+	i2c_set_clientdata(i2c, master);
+
+	ret = spi_register_master(master);
+	if (ret < 0)
+		spi_master_put(master);
+
+	return ret;
+}
+
+static int __devexit spi_xcomm_remove(struct i2c_client *i2c)
+{
+	struct spi_master *master = i2c_get_clientdata(i2c);
+
+	spi_unregister_master(master);
+
+	return 0;
+}
+
+static const struct i2c_device_id spi_xcomm_ids[] = {
+	{ "spi-xcomm" },
+	{ },
+};
+
+static struct i2c_driver spi_xcomm_driver = {
+	.driver = {
+		.name	= "spi-xcomm",
+		.owner	= THIS_MODULE,
+	},
+	.id_table	= spi_xcomm_ids,
+	.probe		= spi_xcomm_probe,
+	.remove		= __devexit_p(spi_xcomm_remove),
+};
+module_i2c_driver(spi_xcomm_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices AD-FMCOMMS1-EBZ board I2C-SPI bridge driver");
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 1041cb83d67..84c2861d6f4 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -53,7 +53,7 @@ modalias_show(struct device *dev, struct device_attribute *a, char *buf)
 {
 	const struct spi_device	*spi = to_spi_device(dev);
 
-	return sprintf(buf, "%s\n", spi->modalias);
+	return sprintf(buf, "%s%s\n", SPI_MODULE_PREFIX, spi->modalias);
 }
 
 static struct device_attribute spi_dev_attrs[] = {
diff --git a/drivers/ssb/b43_pci_bridge.c b/drivers/ssb/b43_pci_bridge.c
index f551e537614..266aa1648a0 100644
--- a/drivers/ssb/b43_pci_bridge.c
+++ b/drivers/ssb/b43_pci_bridge.c
@@ -36,6 +36,7 @@ static const struct pci_device_id b43_pci_bridge_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4328) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4329) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432b) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432c) },
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, b43_pci_bridge_tbl);
diff --git a/drivers/ssb/scan.c b/drivers/ssb/scan.c
index 266c7c5c86d..ab4627cf111 100644
--- a/drivers/ssb/scan.c
+++ b/drivers/ssb/scan.c
@@ -90,6 +90,8 @@ const char *ssb_core_name(u16 coreid)
 		return "ARM 1176";
 	case SSB_DEV_ARM_7TDMI:
 		return "ARM 7TDMI";
+	case SSB_DEV_ARM_CM3:
+		return "ARM Cortex M3";
 	}
 	return "UNKNOWN";
 }
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 05e33c70075..e3402d5644d 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -84,7 +84,7 @@ source "drivers/staging/wlags49_h2/Kconfig"
 
 source "drivers/staging/wlags49_h25/Kconfig"
 
-source "drivers/staging/sm7xx/Kconfig"
+source "drivers/staging/sm7xxfb/Kconfig"
 
 source "drivers/staging/crystalhd/Kconfig"
 
@@ -132,4 +132,8 @@ source "drivers/staging/ipack/Kconfig"
 
 source "drivers/staging/gdm72xx/Kconfig"
 
+source "drivers/staging/csr/Kconfig"
+
+source "drivers/staging/omap-thermal/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index a987b3ad380..3be59d02cae 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -37,7 +37,7 @@ obj-$(CONFIG_ZCACHE)		+= zcache/
 obj-$(CONFIG_ZSMALLOC)		+= zsmalloc/
 obj-$(CONFIG_WLAGS49_H2)	+= wlags49_h2/
 obj-$(CONFIG_WLAGS49_H25)	+= wlags49_h25/
-obj-$(CONFIG_FB_SM7XX)		+= sm7xx/
+obj-$(CONFIG_FB_SM7XX)		+= sm7xxfb/
 obj-$(CONFIG_CRYSTALHD)		+= crystalhd/
 obj-$(CONFIG_CXT1E1)		+= cxt1e1/
 obj-$(CONFIG_FB_XGI)		+= xgifb/
@@ -58,3 +58,5 @@ obj-$(CONFIG_RAMSTER)		+= ramster/
 obj-$(CONFIG_USB_WPAN_HCD)	+= ozwpan/
 obj-$(CONFIG_USB_G_CCG)		+= ccg/
 obj-$(CONFIG_WIMAX_GDM72XX)	+= gdm72xx/
+obj-$(CONFIG_CSR_WIFI)		+= csr/
+obj-$(CONFIG_OMAP_BANDGAP)	+= omap-thermal/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 0e16b594460..0ce50d12c30 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -25,11 +25,6 @@ config ANDROID_LOGGER
 	tristate "Android log driver"
 	default n
 
-config ANDROID_RAM_CONSOLE
-	bool "Android RAM buffer console"
-	depends on !S390 && !UML && HAVE_MEMBLOCK && PSTORE_RAM=y
-	default n
-
 config ANDROID_TIMED_OUTPUT
 	bool "Timed output class driver"
 	default y
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index 98711e2b2af..e16fcd51716 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -1,7 +1,6 @@
 obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
 obj-$(CONFIG_ASHMEM)			+= ashmem.o
 obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
-obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
 obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
 obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
 obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
diff --git a/drivers/staging/android/alarm-dev.c b/drivers/staging/android/alarm-dev.c
index 53ce6ecf390..5b706400518 100644
--- a/drivers/staging/android/alarm-dev.c
+++ b/drivers/staging/android/alarm-dev.c
@@ -29,16 +29,14 @@
 #define ANDROID_ALARM_PRINT_IO (1U << 1)
 #define ANDROID_ALARM_PRINT_INT (1U << 2)
 
-
 static int debug_mask = ANDROID_ALARM_PRINT_INFO;
 module_param_named(debug_mask, debug_mask, int, S_IRUGO | S_IWUSR | S_IWGRP);
 
-#define pr_alarm(debug_level_mask, args...) \
-	do { \
-		if (debug_mask & ANDROID_ALARM_PRINT_##debug_level_mask) { \
-			pr_info(args); \
-		} \
-	} while (0)
+#define alarm_dbg(debug_level_mask, fmt, ...)				\
+do {									\
+	if (debug_mask & ANDROID_ALARM_PRINT_##debug_level_mask)	\
+		pr_info(fmt, ##__VA_ARGS__);				\
+} while (0)
 
 #define ANDROID_ALARM_WAKEUP_MASK ( \
 	ANDROID_ALARM_RTC_WAKEUP_MASK | \
@@ -138,7 +136,7 @@ static long alarm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	switch (ANDROID_ALARM_BASE_CMD(cmd)) {
 	case ANDROID_ALARM_CLEAR(0):
 		spin_lock_irqsave(&alarm_slock, flags);
-		pr_alarm(IO, "alarm %d clear\n", alarm_type);
+		alarm_dbg(IO, "alarm %d clear\n", alarm_type);
 		devalarm_try_to_cancel(&alarms[alarm_type]);
 		if (alarm_pending) {
 			alarm_pending &= ~alarm_type_mask;
@@ -167,8 +165,9 @@ static long alarm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		}
 from_old_alarm_set:
 		spin_lock_irqsave(&alarm_slock, flags);
-		pr_alarm(IO, "alarm %d set %ld.%09ld\n", alarm_type,
-			new_alarm_time.tv_sec, new_alarm_time.tv_nsec);
+		alarm_dbg(IO, "alarm %d set %ld.%09ld\n",
+			  alarm_type,
+			  new_alarm_time.tv_sec, new_alarm_time.tv_nsec);
 		alarm_enabled |= alarm_type_mask;
 		devalarm_start(&alarms[alarm_type],
 			timespec_to_ktime(new_alarm_time));
@@ -179,7 +178,7 @@ from_old_alarm_set:
 		/* fall though */
 	case ANDROID_ALARM_WAIT:
 		spin_lock_irqsave(&alarm_slock, flags);
-		pr_alarm(IO, "alarm wait\n");
+		alarm_dbg(IO, "alarm wait\n");
 		if (!alarm_pending && wait_pending) {
 			__pm_relax(&alarm_wake_lock);
 			wait_pending = 0;
@@ -238,7 +237,6 @@ from_old_alarm_set:
 
 	default:
 		rv = -EINVAL;
-		goto err1;
 	}
 err1:
 	return rv;
@@ -256,13 +254,14 @@ static int alarm_release(struct inode *inode, struct file *file)
 	unsigned long flags;
 
 	spin_lock_irqsave(&alarm_slock, flags);
-	if (file->private_data != 0) {
+	if (file->private_data) {
 		for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++) {
 			uint32_t alarm_type_mask = 1U << i;
 			if (alarm_enabled & alarm_type_mask) {
-				pr_alarm(INFO, "alarm_release: clear alarm, "
-					"pending %d\n",
-					!!(alarm_pending & alarm_type_mask));
+				alarm_dbg(INFO,
+					  "%s: clear alarm, pending %d\n",
+					  __func__,
+					  !!(alarm_pending & alarm_type_mask));
 				alarm_enabled &= ~alarm_type_mask;
 			}
 			spin_unlock_irqrestore(&alarm_slock, flags);
@@ -271,8 +270,8 @@ static int alarm_release(struct inode *inode, struct file *file)
 		}
 		if (alarm_pending | wait_pending) {
 			if (alarm_pending)
-				pr_alarm(INFO, "alarm_release: clear "
-					"pending alarms %x\n", alarm_pending);
+				alarm_dbg(INFO, "%s: clear pending alarms %x\n",
+					  __func__, alarm_pending);
 			__pm_relax(&alarm_wake_lock);
 			wait_pending = 0;
 			alarm_pending = 0;
@@ -288,7 +287,7 @@ static void devalarm_triggered(struct devalarm *alarm)
 	unsigned long flags;
 	uint32_t alarm_type_mask = 1U << alarm->type;
 
-	pr_alarm(INT, "devalarm_triggered type %d\n", alarm->type);
+	alarm_dbg(INT, "%s: type %d\n", __func__, alarm->type);
 	spin_lock_irqsave(&alarm_slock, flags);
 	if (alarm_enabled & alarm_type_mask) {
 		__pm_wakeup_event(&alarm_wake_lock, 5000); /* 5secs */
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index e84dbecd099..69cf2db1d69 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -16,6 +16,8 @@
 ** GNU General Public License for more details.
 */
 
+#define pr_fmt(fmt) "ashmem: " fmt
+
 #include <linux/module.h>
 #include <linux/file.h>
 #include <linux/fs.h>
@@ -707,7 +709,7 @@ static int __init ashmem_init(void)
 					  sizeof(struct ashmem_area),
 					  0, 0, NULL);
 	if (unlikely(!ashmem_area_cachep)) {
-		printk(KERN_ERR "ashmem: failed to create slab cache\n");
+		pr_err("failed to create slab cache\n");
 		return -ENOMEM;
 	}
 
@@ -715,19 +717,19 @@ static int __init ashmem_init(void)
 					  sizeof(struct ashmem_range),
 					  0, 0, NULL);
 	if (unlikely(!ashmem_range_cachep)) {
-		printk(KERN_ERR "ashmem: failed to create slab cache\n");
+		pr_err("failed to create slab cache\n");
 		return -ENOMEM;
 	}
 
 	ret = misc_register(&ashmem_misc);
 	if (unlikely(ret)) {
-		printk(KERN_ERR "ashmem: failed to register misc device!\n");
+		pr_err("failed to register misc device!\n");
 		return ret;
 	}
 
 	register_shrinker(&ashmem_shrinker);
 
-	printk(KERN_INFO "ashmem: initialized\n");
+	pr_info("initialized\n");
 
 	return 0;
 }
@@ -740,12 +742,12 @@ static void __exit ashmem_exit(void)
 
 	ret = misc_deregister(&ashmem_misc);
 	if (unlikely(ret))
-		printk(KERN_ERR "ashmem: failed to unregister misc device!\n");
+		pr_err("failed to unregister misc device!\n");
 
 	kmem_cache_destroy(ashmem_range_cachep);
 	kmem_cache_destroy(ashmem_area_cachep);
 
-	printk(KERN_INFO "ashmem: unloaded\n");
+	pr_info("unloaded\n");
 }
 
 module_init(ashmem_init);
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index c2832124bb3..574e99210c3 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -124,13 +124,13 @@ module_param_call(stop_on_user_error, binder_set_stop_on_user_error,
 #define binder_debug(mask, x...) \
 	do { \
 		if (binder_debug_mask & mask) \
-			printk(KERN_INFO x); \
+			pr_info(x); \
 	} while (0)
 
 #define binder_user_error(x...) \
 	do { \
 		if (binder_debug_mask & BINDER_DEBUG_USER_ERROR) \
-			printk(KERN_INFO x); \
+			pr_info(x); \
 		if (binder_stop_on_user_error) \
 			binder_stop_on_user_error = 2; \
 	} while (0)
@@ -418,7 +418,7 @@ repeat:
 #if 1
 	/* Sanity check */
 	if (fdt->fd[fd] != NULL) {
-		printk(KERN_WARNING "get_unused_fd: slot %d not NULL!\n", fd);
+		pr_warn("get_unused_fd: slot %d not NULL!\n", fd);
 		fdt->fd[fd] = NULL;
 	}
 #endif
@@ -644,7 +644,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 		goto free_range;
 
 	if (vma == NULL) {
-		printk(KERN_ERR "binder: %d: binder_alloc_buf failed to "
+		pr_err("binder: %d: binder_alloc_buf failed to "
 		       "map pages in userspace, no vma\n", proc->pid);
 		goto err_no_vma;
 	}
@@ -657,7 +657,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 		BUG_ON(*page);
 		*page = alloc_page(GFP_KERNEL | __GFP_ZERO);
 		if (*page == NULL) {
-			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			pr_err("binder: %d: binder_alloc_buf failed "
 			       "for page at %p\n", proc->pid, page_addr);
 			goto err_alloc_page_failed;
 		}
@@ -666,7 +666,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 		page_array_ptr = page;
 		ret = map_vm_area(&tmp_area, PAGE_KERNEL, &page_array_ptr);
 		if (ret) {
-			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			pr_err("binder: %d: binder_alloc_buf failed "
 			       "to map page at %p in kernel\n",
 			       proc->pid, page_addr);
 			goto err_map_kernel_failed;
@@ -675,7 +675,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 			(uintptr_t)page_addr + proc->user_buffer_offset;
 		ret = vm_insert_page(vma, user_page_addr, page[0]);
 		if (ret) {
-			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			pr_err("binder: %d: binder_alloc_buf failed "
 			       "to map page at %lx in userspace\n",
 			       proc->pid, user_page_addr);
 			goto err_vm_insert_page_failed;
@@ -724,7 +724,7 @@ static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
 	size_t size;
 
 	if (proc->vma == NULL) {
-		printk(KERN_ERR "binder: %d: binder_alloc_buf, no vma\n",
+		pr_err("binder: %d: binder_alloc_buf, no vma\n",
 		       proc->pid);
 		return NULL;
 	}
@@ -762,7 +762,7 @@ static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
 		}
 	}
 	if (best_fit == NULL) {
-		printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd failed, "
+		pr_err("binder: %d: binder_alloc_buf size %zd failed, "
 		       "no address space\n", proc->pid, size);
 		return NULL;
 	}
@@ -997,7 +997,7 @@ static int binder_inc_node(struct binder_node *node, int strong, int internal,
 			    node->internal_strong_refs == 0 &&
 			    !(node == binder_context_mgr_node &&
 			    node->has_strong_ref)) {
-				printk(KERN_ERR "binder: invalid inc strong "
+				pr_err("binder: invalid inc strong "
 					"node for %d\n", node->debug_id);
 				return -EINVAL;
 			}
@@ -1013,7 +1013,7 @@ static int binder_inc_node(struct binder_node *node, int strong, int internal,
 			node->local_weak_refs++;
 		if (!node->has_weak_ref && list_empty(&node->work.entry)) {
 			if (target_list == NULL) {
-				printk(KERN_ERR "binder: invalid inc weak node "
+				pr_err("binder: invalid inc weak node "
 					"for %d\n", node->debug_id);
 				return -EINVAL;
 			}
@@ -1276,7 +1276,7 @@ static void binder_send_failed_reply(struct binder_transaction *t,
 				target_thread->return_error = error_code;
 				wake_up_interruptible(&target_thread->wait);
 			} else {
-				printk(KERN_ERR "binder: reply failed, target "
+				pr_err("binder: reply failed, target "
 					"thread, %d:%d, has error code %d "
 					"already\n", target_thread->proc->pid,
 					target_thread->pid,
@@ -1331,7 +1331,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		if (*offp > buffer->data_size - sizeof(*fp) ||
 		    buffer->data_size < sizeof(*fp) ||
 		    !IS_ALIGNED(*offp, sizeof(void *))) {
-			printk(KERN_ERR "binder: transaction release %d bad"
+			pr_err("binder: transaction release %d bad"
 					"offset %zd, size %zd\n", debug_id,
 					*offp, buffer->data_size);
 			continue;
@@ -1342,7 +1342,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		case BINDER_TYPE_WEAK_BINDER: {
 			struct binder_node *node = binder_get_node(proc, fp->binder);
 			if (node == NULL) {
-				printk(KERN_ERR "binder: transaction release %d"
+				pr_err("binder: transaction release %d"
 				       " bad node %p\n", debug_id, fp->binder);
 				break;
 			}
@@ -1355,7 +1355,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		case BINDER_TYPE_WEAK_HANDLE: {
 			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
 			if (ref == NULL) {
-				printk(KERN_ERR "binder: transaction release %d"
+				pr_err("binder: transaction release %d"
 				       " bad handle %ld\n", debug_id,
 				       fp->handle);
 				break;
@@ -1374,7 +1374,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 			break;
 
 		default:
-			printk(KERN_ERR "binder: transaction release %d bad "
+			pr_err("binder: transaction release %d bad "
 			       "object type %lx\n", debug_id, fp->type);
 			break;
 		}
@@ -1925,10 +1925,10 @@ int binder_thread_write(struct binder_proc *proc, struct binder_thread *thread,
 			break;
 		}
 		case BC_ATTEMPT_ACQUIRE:
-			printk(KERN_ERR "binder: BC_ATTEMPT_ACQUIRE not supported\n");
+			pr_err("binder: BC_ATTEMPT_ACQUIRE not supported\n");
 			return -EINVAL;
 		case BC_ACQUIRE_RESULT:
-			printk(KERN_ERR "binder: BC_ACQUIRE_RESULT not supported\n");
+			pr_err("binder: BC_ACQUIRE_RESULT not supported\n");
 			return -EINVAL;
 
 		case BC_FREE_BUFFER: {
@@ -2165,7 +2165,7 @@ int binder_thread_write(struct binder_proc *proc, struct binder_thread *thread,
 		} break;
 
 		default:
-			printk(KERN_ERR "binder: %d:%d unknown command %d\n",
+			pr_err("binder: %d:%d unknown command %d\n",
 			       proc->pid, thread->pid, cmd);
 			return -EINVAL;
 		}
@@ -2635,7 +2635,7 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	unsigned int size = _IOC_SIZE(cmd);
 	void __user *ubuf = (void __user *)arg;
 
-	/*printk(KERN_INFO "binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
+	/*pr_info("binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
 
 	ret = wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
 	if (ret)
@@ -2701,13 +2701,13 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 		break;
 	case BINDER_SET_CONTEXT_MGR:
 		if (binder_context_mgr_node != NULL) {
-			printk(KERN_ERR "binder: BINDER_SET_CONTEXT_MGR already set\n");
+			pr_err("binder: BINDER_SET_CONTEXT_MGR already set\n");
 			ret = -EBUSY;
 			goto err;
 		}
 		if (binder_context_mgr_uid != -1) {
 			if (binder_context_mgr_uid != current->cred->euid) {
-				printk(KERN_ERR "binder: BINDER_SET_"
+				pr_err("binder: BINDER_SET_"
 				       "CONTEXT_MGR bad uid %d != %d\n",
 				       current->cred->euid,
 				       binder_context_mgr_uid);
@@ -2753,7 +2753,7 @@ err:
 	mutex_unlock(&binder_lock);
 	wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
 	if (ret && ret != -ERESTARTSYS)
-		printk(KERN_INFO "binder: %d:%d ioctl %x %lx returned %d\n", proc->pid, current->pid, cmd, arg, ret);
+		pr_info("binder: %d:%d ioctl %x %lx returned %d\n", proc->pid, current->pid, cmd, arg, ret);
 	return ret;
 }
 
@@ -2829,7 +2829,7 @@ static int binder_mmap(struct file *filp, struct vm_area_struct *vma)
 #ifdef CONFIG_CPU_CACHE_VIPT
 	if (cache_is_vipt_aliasing()) {
 		while (CACHE_COLOUR((vma->vm_start ^ (uint32_t)proc->buffer))) {
-			printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p bad alignment\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);
+			pr_info("binder_mmap: %d %lx-%lx maps %p bad alignment\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);
 			vma->vm_start += PAGE_SIZE;
 		}
 	}
@@ -2861,7 +2861,7 @@ static int binder_mmap(struct file *filp, struct vm_area_struct *vma)
 	proc->vma = vma;
 	proc->vma_vm_mm = vma->vm_mm;
 
-	/*printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p\n",
+	/*pr_info("binder_mmap: %d %lx-%lx maps %p\n",
 		 proc->pid, vma->vm_start, vma->vm_end, proc->buffer);*/
 	return 0;
 
@@ -2876,7 +2876,7 @@ err_get_vm_area_failed:
 err_already_mapped:
 	mutex_unlock(&binder_mmap_lock);
 err_bad_arg:
-	printk(KERN_ERR "binder_mmap: %d %lx-%lx %s failed %d\n",
+	pr_err("binder_mmap: %d %lx-%lx %s failed %d\n",
 	       proc->pid, vma->vm_start, vma->vm_end, failure_string, ret);
 	return ret;
 }
@@ -3031,7 +3031,7 @@ static void binder_deferred_release(struct binder_proc *proc)
 		if (t) {
 			t->buffer = NULL;
 			buffer->transaction = NULL;
-			printk(KERN_ERR "binder: release proc %d, "
+			pr_err("binder: release proc %d, "
 			       "transaction %d, not freed\n",
 			       proc->pid, t->debug_id);
 			/*BUG();*/
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index b2e71c6fd17..f7b8237d5be 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -17,6 +17,8 @@
  * GNU General Public License for more details.
  */
 
+#define pr_fmt(fmt) "logger: " fmt
+
 #include <linux/sched.h>
 #include <linux/module.h>
 #include <linux/fs.h>
@@ -621,13 +623,13 @@ static int __init create_log(char *log_name, int size)
 	/* finally, initialize the misc device for this log */
 	ret = misc_register(&log->misc);
 	if (unlikely(ret)) {
-		printk(KERN_ERR "logger: failed to register misc "
-		       "device for log '%s'!\n", log->misc.name);
+		pr_err("failed to register misc device for log '%s'!\n",
+				log->misc.name);
 		goto out_free_log;
 	}
 
-	printk(KERN_INFO "logger: created %luK log '%s'\n",
-	       (unsigned long) log->size >> 10, log->misc.name);
+	pr_info("created %luK log '%s'\n",
+		(unsigned long) log->size >> 10, log->misc.name);
 
 	return 0;
 
diff --git a/drivers/staging/android/ram_console.c b/drivers/staging/android/ram_console.c
deleted file mode 100644
index 82323bb1d1a..00000000000
--- a/drivers/staging/android/ram_console.c
+++ /dev/null
@@ -1,179 +0,0 @@
-/* drivers/android/ram_console.c
- *
- * Copyright (C) 2007-2008 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#include <linux/console.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/proc_fs.h>
-#include <linux/string.h>
-#include <linux/uaccess.h>
-#include <linux/io.h>
-#include <linux/pstore_ram.h>
-#include "ram_console.h"
-
-static struct persistent_ram_zone *ram_console_zone;
-static const char *bootinfo;
-static size_t bootinfo_size;
-
-static void
-ram_console_write(struct console *console, const char *s, unsigned int count)
-{
-	struct persistent_ram_zone *prz = console->data;
-	persistent_ram_write(prz, s, count);
-}
-
-static struct console ram_console = {
-	.name	= "ram",
-	.write	= ram_console_write,
-	.flags	= CON_PRINTBUFFER | CON_ENABLED | CON_ANYTIME,
-	.index	= -1,
-};
-
-void ram_console_enable_console(int enabled)
-{
-	if (enabled)
-		ram_console.flags |= CON_ENABLED;
-	else
-		ram_console.flags &= ~CON_ENABLED;
-}
-
-static int __init ram_console_probe(struct platform_device *pdev)
-{
-	struct ram_console_platform_data *pdata = pdev->dev.platform_data;
-	struct persistent_ram_zone *prz;
-
-	prz = persistent_ram_init_ringbuffer(&pdev->dev, true);
-	if (IS_ERR(prz))
-		return PTR_ERR(prz);
-
-
-	if (pdata) {
-		bootinfo = kstrdup(pdata->bootinfo, GFP_KERNEL);
-		if (bootinfo)
-			bootinfo_size = strlen(bootinfo);
-	}
-
-	ram_console_zone = prz;
-	ram_console.data = prz;
-
-	register_console(&ram_console);
-
-	return 0;
-}
-
-static struct platform_driver ram_console_driver = {
-	.driver		= {
-		.name	= "ram_console",
-	},
-};
-
-static int __init ram_console_module_init(void)
-{
-	return platform_driver_probe(&ram_console_driver, ram_console_probe);
-}
-
-#ifndef CONFIG_PRINTK
-#define dmesg_restrict	0
-#endif
-
-static ssize_t ram_console_read_old(struct file *file, char __user *buf,
-				    size_t len, loff_t *offset)
-{
-	loff_t pos = *offset;
-	ssize_t count;
-	struct persistent_ram_zone *prz = ram_console_zone;
-	size_t old_log_size = persistent_ram_old_size(prz);
-	const char *old_log = persistent_ram_old(prz);
-	char *str;
-	int ret;
-
-	if (dmesg_restrict && !capable(CAP_SYSLOG))
-		return -EPERM;
-
-	/* Main last_kmsg log */
-	if (pos < old_log_size) {
-		count = min(len, (size_t)(old_log_size - pos));
-		if (copy_to_user(buf, old_log + pos, count))
-			return -EFAULT;
-		goto out;
-	}
-
-	/* ECC correction notice */
-	pos -= old_log_size;
-	count = persistent_ram_ecc_string(prz, NULL, 0);
-	if (pos < count) {
-		str = kmalloc(count, GFP_KERNEL);
-		if (!str)
-			return -ENOMEM;
-		persistent_ram_ecc_string(prz, str, count + 1);
-		count = min(len, (size_t)(count - pos));
-		ret = copy_to_user(buf, str + pos, count);
-		kfree(str);
-		if (ret)
-			return -EFAULT;
-		goto out;
-	}
-
-	/* Boot info passed through pdata */
-	pos -= count;
-	if (pos < bootinfo_size) {
-		count = min(len, (size_t)(bootinfo_size - pos));
-		if (copy_to_user(buf, bootinfo + pos, count))
-			return -EFAULT;
-		goto out;
-	}
-
-	/* EOF */
-	return 0;
-
-out:
-	*offset += count;
-	return count;
-}
-
-static const struct file_operations ram_console_file_ops = {
-	.owner = THIS_MODULE,
-	.read = ram_console_read_old,
-};
-
-static int __init ram_console_late_init(void)
-{
-	struct proc_dir_entry *entry;
-	struct persistent_ram_zone *prz = ram_console_zone;
-
-	if (!prz)
-		return 0;
-
-	if (persistent_ram_old_size(prz) == 0)
-		return 0;
-
-	entry = create_proc_entry("last_kmsg", S_IFREG | S_IRUGO, NULL);
-	if (!entry) {
-		printk(KERN_ERR "ram_console: failed to create proc entry\n");
-		persistent_ram_free_old(prz);
-		return 0;
-	}
-
-	entry->proc_fops = &ram_console_file_ops;
-	entry->size = persistent_ram_old_size(prz) +
-		persistent_ram_ecc_string(prz, NULL, 0) +
-		bootinfo_size;
-
-	return 0;
-}
-
-late_initcall(ram_console_late_init);
-postcore_initcall(ram_console_module_init);
diff --git a/drivers/staging/android/timed_output.c b/drivers/staging/android/timed_output.c
index 38d930cadad..ec9e2ae2de0 100644
--- a/drivers/staging/android/timed_output.c
+++ b/drivers/staging/android/timed_output.c
@@ -14,6 +14,8 @@
  *
  */
 
+#define pr_fmt(fmt) "timed_output: " fmt
+
 #include <linux/module.h>
 #include <linux/types.h>
 #include <linux/device.h>
@@ -90,7 +92,7 @@ int timed_output_dev_register(struct timed_output_dev *tdev)
 
 err_create_file:
 	device_destroy(timed_output_class, MKDEV(0, tdev->index));
-	printk(KERN_ERR "timed_output: Failed to register driver %s\n",
+	pr_err("failed to register driver %s\n",
 			tdev->name);
 
 	return ret;
diff --git a/drivers/staging/asus_oled/asus_oled.c b/drivers/staging/asus_oled/asus_oled.c
index 510d7963921..f63c1d3aeb6 100644
--- a/drivers/staging/asus_oled/asus_oled.c
+++ b/drivers/staging/asus_oled/asus_oled.c
@@ -782,20 +782,20 @@ static int __init asus_oled_init(void)
 	oled_class = class_create(THIS_MODULE, ASUS_OLED_UNDERSCORE_NAME);
 
 	if (IS_ERR(oled_class)) {
-		printk(KERN_ERR "Error creating " ASUS_OLED_UNDERSCORE_NAME " class\n");
+		pr_err("Error creating " ASUS_OLED_UNDERSCORE_NAME " class\n");
 		return PTR_ERR(oled_class);
 	}
 
 	retval = class_create_file(oled_class, &class_attr_version.attr);
 	if (retval) {
-		printk(KERN_ERR "Error creating class version file\n");
+		pr_err("Error creating class version file\n");
 		goto error;
 	}
 
 	retval = usb_register(&oled_driver);
 
 	if (retval) {
-		printk(KERN_ERR "usb_register failed. Error number %d\n", retval);
+		pr_err("usb_register failed. Error number %d\n", retval);
 		goto error;
 	}
 
diff --git a/drivers/staging/bcm/Adapter.h b/drivers/staging/bcm/Adapter.h
index aa51d17be5a..4d490a99110 100644
--- a/drivers/staging/bcm/Adapter.h
+++ b/drivers/staging/bcm/Adapter.h
@@ -7,74 +7,28 @@
 #define MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES 256
 #include "Debug.h"
 
-struct _LEADER {
+struct bcm_leader {
 	USHORT	Vcid;
 	USHORT	PLength;
 	UCHAR	Status;
 	UCHAR	Unused[3];
 } __packed;
-typedef struct _LEADER LEADER, *PLEADER;
 
-struct _PACKETTOSEND {
-	LEADER	Leader;
+struct bcm_packettosend {
+	struct bcm_leader Leader;
 	UCHAR	ucPayload;
 } __packed;
-typedef struct _PACKETTOSEND PACKETTOSEND, *PPACKETTOSEND;
 
-struct _CONTROL_PACKET {
+struct bcm_control_packet {
 	PVOID	ControlBuff;
 	UINT	ControlBuffLen;
-	struct _CONTROL_PACKET *next;
+	struct bcm_control_packet *next;
 } __packed;
-typedef struct _CONTROL_PACKET CONTROL_PACKET, *PCONTROL_PACKET;
 
-struct link_request {
-	LEADER	Leader;
+struct bcm_link_request {
+	struct bcm_leader Leader;
 	UCHAR	szData[4];
 } __packed;
-typedef struct link_request LINK_REQUEST, *PLINK_REQUEST;
-
-/* classification extension is added */
-typedef struct _ADD_CONNECTION {
-	ULONG	SrcIpAddressCount;
-	ULONG	SrcIpAddress[MAX_CONNECTIONS];
-	ULONG	SrcIpMask[MAX_CONNECTIONS];
-
-	ULONG	DestIpAddressCount;
-	ULONG	DestIpAddress[MAX_CONNECTIONS];
-	ULONG	DestIpMask[MAX_CONNECTIONS];
-
-	USHORT	SrcPortBegin;
-	USHORT	SrcPortEnd;
-
-	USHORT	DestPortBegin;
-	USHORT	DestPortEnd;
-
-	UCHAR	SrcTOS;
-	UCHAR	SrcProtocol;
-} ADD_CONNECTION, *PADD_CONNECTION;
-
-typedef struct _CLASSIFICATION_RULE {
-	UCHAR	ucIPSrcAddrLen;
-	UCHAR	ucIPSrcAddr[32];
-	UCHAR	ucIPDestAddrLen;
-	UCHAR	ucIPDestAddr[32];
-	UCHAR	ucSrcPortRangeLen;
-	UCHAR	ucSrcPortRange[4];
-	UCHAR	ucDestPortRangeLen;
-	UCHAR	ucDestPortRange[4];
-	USHORT	usVcid;
-} CLASSIFICATION_RULE, *PCLASSIFICATION_RULE;
-
-typedef struct _CLASSIFICATION_ONLY {
-	USHORT	usVcid;
-	ULONG	DestIpAddress;
-	ULONG	DestIpMask;
-	USHORT	usPortLo;
-	USHORT	usPortHi;
-	BOOLEAN	bIpVersion;
-	UCHAR	ucDestinationAddress[16];
-} CLASSIFICATION_ONLY, *PCLASSIFICATION_ONLY;
 
 #define MAX_IP_RANGE_LENGTH 4
 #define MAX_PORT_RANGE 4
@@ -99,14 +53,13 @@ typedef union _U_IP_ADDRESS {
 		UCHAR ucIpv6Mask[MAX_IP_RANGE_LENGTH * IPV6_ADDRESS_SIZEINBYTES];
 	};
 } U_IP_ADDRESS;
-struct _packet_info;
 
-typedef struct _S_HDR_SUPRESSION_CONTEXTINFO {
-	UCHAR ucaHdrSupressionInBuf[MAX_PHS_LENGTHS]; /* Intermediate buffer to accumulate pkt Header for PHS */
-	UCHAR ucaHdrSupressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN]; /* Intermediate buffer containing pkt Header after PHS */
-} S_HDR_SUPRESSION_CONTEXTINFO;
+struct bcm_hdr_suppression_contextinfo {
+	UCHAR ucaHdrSuppressionInBuf[MAX_PHS_LENGTHS]; /* Intermediate buffer to accumulate pkt Header for PHS */
+	UCHAR ucaHdrSuppressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN]; /* Intermediate buffer containing pkt Header after PHS */
+};
 
-typedef struct _S_CLASSIFIER_RULE {
+struct bcm_classifier_rule {
 	ULONG		ulSFID;
 	UCHAR		ucReserved[2];
 	B_UINT16	uiClassifierRuleIndex;
@@ -157,18 +110,17 @@ typedef struct _S_CLASSIFIER_RULE {
 	UCHAR		usUserPriority[2];
 	USHORT		usVLANID;
 	USHORT		usValidityBitMap;
-} S_CLASSIFIER_RULE;
-/* typedef struct _S_CLASSIFIER_RULE S_CLASSIFIER_RULE; */
+};
 
-typedef struct _S_FRAGMENTED_PACKET_INFO {
+struct bcm_fragmented_packet_info {
 	BOOLEAN			bUsed;
 	ULONG			ulSrcIpAddress;
 	USHORT			usIpIdentification;
-	S_CLASSIFIER_RULE	*pstMatchedClassifierEntry;
+	struct bcm_classifier_rule *pstMatchedClassifierEntry;
 	BOOLEAN			bOutOfOrderFragment;
-} S_FRAGMENTED_PACKET_INFO, *PS_FRAGMENTED_PACKET_INFO;
+};
 
-struct _packet_info {
+struct bcm_packet_info {
 	/* classification extension Rule */
 	ULONG		ulSFID;
 	USHORT		usVCID_Value;
@@ -237,11 +189,10 @@ struct _packet_info {
 	UCHAR		bIPCSSupport;
 	UCHAR		bEthCSSupport;
 };
-typedef struct _packet_info PacketInfo;
 
-typedef struct _PER_TARANG_DATA {
-	struct _PER_TARANG_DATA	*next;
-	struct _MINI_ADAPTER	*Adapter;
+struct bcm_tarang_data {
+	struct bcm_tarang_data	*next;
+	struct bcm_mini_adapter	*Adapter;
 	struct sk_buff		*RxAppControlHead;
 	struct sk_buff		*RxAppControlTail;
 	int			AppCtrlQueueLen;
@@ -249,104 +200,23 @@ typedef struct _PER_TARANG_DATA {
 	BOOLEAN			bApplicationToExit;
 	S_MIBS_DROPPED_APP_CNTRL_MESSAGES	stDroppedAppCntrlMsgs;
 	ULONG			RxCntrlMsgBitMask;
-} PER_TARANG_DATA, *PPER_TARANG_DATA;
-
-#ifdef REL_4_1
-typedef struct _TARGET_PARAMS {
-	B_UINT32 m_u32CfgVersion;
-
-	/* Scanning Related Params */
-	B_UINT32 m_u32CenterFrequency;
-	B_UINT32 m_u32BandAScan;
-	B_UINT32 m_u32BandBScan;
-	B_UINT32 m_u32BandCScan;
-
-	/* QoS Params */
-	B_UINT32 m_u32minGrantsize;	/* size of minimum grant is 0 or 6 */
-	B_UINT32 m_u32PHSEnable;
-
-	/* HO Params */
-	B_UINT32 m_u32HoEnable;
-	B_UINT32 m_u32HoReserved1;
-	B_UINT32 m_u32HoReserved2;
-
-	/* Power Control Params */
-	B_UINT32 m_u32MimoEnable;
-	B_UINT32 m_u32SecurityEnable;
-	/*
-	 * bit 1: 1 Idlemode enable;
-	 * bit 2: 1 Sleepmode Enable
-	 */
-	B_UINT32 m_u32PowerSavingModesEnable;
-	/* PowerSaving Mode Options:
-	 * bit 0 = 1: CPE mode - to keep pcmcia if alive;
-	 * bit 1 = 1: CINR reporing in Idlemode Msg
-	 * bit 2 = 1: Default PSC Enable in sleepmode
-	 */
-	B_UINT32 m_u32PowerSavingModeOptions;
-
-	B_UINT32 m_u32ArqEnable;
-
-	/* From Version #3, the HARQ section renamed as general */
-	B_UINT32 m_u32HarqEnable;
-	/* EEPROM Param Location */
-	B_UINT32 m_u32EEPROMFlag;
-	/* BINARY TYPE - 4th MSByte:
-	 * Interface Type -  3rd MSByte:
-	 * Vendor Type - 2nd MSByte
-	 */
-	/* Unused - LSByte */
-	B_UINT32 m_u32Customize;
-	B_UINT32 m_u32ConfigBW;  /* In Hz */
-	B_UINT32 m_u32ShutDownTimer;
-	B_UINT32 m_u32RadioParameter;
-	B_UINT32 m_u32PhyParameter1;
-	B_UINT32 m_u32PhyParameter2;
-	B_UINT32 m_u32PhyParameter3;
-
-	/* in eval mode only;
-	 * lower 16bits = basic cid for testing;
-	 * then bit 16 is test cqich,
-	 * bit 17  test init rang;
-	 * bit 18 test periodic rang
-	 * bit 19 is test harq ack/nack
-	 */
-	B_UINT32 m_u32TestOptions;
-	B_UINT32 m_u32MaxMACDataperDLFrame;
-	B_UINT32 m_u32MaxMACDataperULFrame;
-	B_UINT32 m_u32Corr2MacFlags;
-
-	/* adding driver params. */
-	B_UINT32 HostDrvrConfig1;
-	B_UINT32 HostDrvrConfig2;
-	B_UINT32 HostDrvrConfig3;
-	B_UINT32 HostDrvrConfig4;
-	B_UINT32 HostDrvrConfig5;
-	B_UINT32 HostDrvrConfig6;
-	B_UINT32 m_u32SegmentedPUSCenable;
-
-	/* BAMC enable - but 4.x does not support this feature
-	 * This is added just to sync 4.x and 5.x CFGs
-	 */
-	B_UINT32 m_u32BandAMCEnable;
-} STARGETPARAMS, *PSTARGETPARAMS;
-#endif
+};
 
-typedef struct _STTARGETDSXBUFFER {
+struct bcm_targetdsx_buffer {
 	ULONG		ulTargetDsxBuffer;
 	B_UINT16	tid;
 	BOOLEAN		valid;
-} STTARGETDSXBUFFER, *PSTTARGETDSXBUFFER;
+};
 
-typedef int (*FP_FLASH_WRITE)(struct _MINI_ADAPTER *, UINT, PVOID);
+typedef int (*FP_FLASH_WRITE)(struct bcm_mini_adapter *, UINT, PVOID);
 
-typedef int (*FP_FLASH_WRITE_STATUS)(struct _MINI_ADAPTER *, UINT, PVOID);
+typedef int (*FP_FLASH_WRITE_STATUS)(struct bcm_mini_adapter *, UINT, PVOID);
 
 /*
  * Driver adapter data structure
  */
-struct _MINI_ADAPTER {
-	struct _MINI_ADAPTER	*next;
+struct bcm_mini_adapter {
+	struct bcm_mini_adapter	*next;
 	struct net_device	*dev;
 	u32			msg_enable;
 	CHAR			*caDsxReqResp;
@@ -361,7 +231,7 @@ struct _MINI_ADAPTER {
 	struct sk_buff		*RxControlTail;
 	struct semaphore	RxAppControlQueuelock;
 	struct semaphore	fw_download_sema;
-	PPER_TARANG_DATA	pTarangs;
+	struct bcm_tarang_data	*pTarangs;
 	spinlock_t		control_queue_lock;
 	wait_queue_head_t	process_read_wait_queue;
 
@@ -377,8 +247,8 @@ struct _MINI_ADAPTER {
 	USHORT			PrevNumRecvDescs;
 	USHORT			CurrNumRecvDescs;
 	UINT			u32TotalDSD;
-	PacketInfo		PackInfo[NO_OF_QUEUES];
-	S_CLASSIFIER_RULE	astClassifierTable[MAX_CLASSIFIERS];
+	struct bcm_packet_info	PackInfo[NO_OF_QUEUES];
+	struct bcm_classifier_rule astClassifierTable[MAX_CLASSIFIERS];
 	BOOLEAN			TransferMode;
 
 	/*************** qos ******************/
@@ -404,7 +274,7 @@ struct _MINI_ADAPTER {
 	UINT			index_datpkt;
 	struct semaphore	rdmwrmsync;
 
-	STTARGETDSXBUFFER	astTargetDsxBuffer[MAX_TARGET_DSX_BUFFERS];
+	struct bcm_targetdsx_buffer	astTargetDsxBuffer[MAX_TARGET_DSX_BUFFERS];
 	ULONG			ulFreeTargetBufferCnt;
 	ULONG			ulCurrentTargetBuffer;
 	ULONG			ulTotalTargetBuffersAvailable;
@@ -464,7 +334,7 @@ struct _MINI_ADAPTER {
 	BOOLEAN			bLinkDownRequested;
 	int			downloadDDR;
 	PHS_DEVICE_EXTENSION	stBCMPhsContext;
-	S_HDR_SUPRESSION_CONTEXTINFO stPhsTxContextInfo;
+	struct bcm_hdr_suppression_contextinfo stPhsTxContextInfo;
 	uint8_t			ucaPHSPktRestoreBuf[2048];
 	uint8_t			bPHSEnabled;
 	BOOLEAN			AutoFirmDld;
@@ -472,7 +342,7 @@ struct _MINI_ADAPTER {
 	BOOLEAN			bDPLLConfig;
 	UINT32			aTxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
 	UINT32			aRxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
-	S_FRAGMENTED_PACKET_INFO astFragmentedPktClassifierTable[MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES];
+	struct bcm_fragmented_packet_info astFragmentedPktClassifierTable[MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES];
 	atomic_t		uiMBupdate;
 	UINT32			PmuMode;
 	NVM_TYPE		eNVMType;
@@ -524,37 +394,29 @@ struct _MINI_ADAPTER {
 	UINT			gpioBitMap;
 	S_BCM_DEBUG_STATE	stDebugState;
 };
-typedef struct _MINI_ADAPTER MINI_ADAPTER, *PMINI_ADAPTER;
 
 #define GET_BCM_ADAPTER(net_dev) netdev_priv(net_dev)
 
-struct _ETH_HEADER_STRUC {
+struct bcm_eth_header {
 	UCHAR	au8DestinationAddress[6];
 	UCHAR	au8SourceAddress[6];
 	USHORT	u16Etype;
 } __packed;
-typedef struct _ETH_HEADER_STRUC ETH_HEADER_STRUC, *PETH_HEADER_STRUC;
 
-typedef struct FirmwareInfo {
+struct bcm_firmware_info {
 	void	__user *pvMappedFirmwareAddress;
 	ULONG	u32FirmwareLength;
 	ULONG	u32StartingAddress;
-} __packed FIRMWARE_INFO, *PFIRMWARE_INFO;
+} __packed;
 
 /* holds the value of net_device structure.. */
 extern struct net_device *gblpnetdev;
-typedef struct _cntl_pkt {
-	PMINI_ADAPTER	Adapter;
-	PLEADER		PLeader;
-} cntl_pkt;
-typedef LINK_REQUEST CONTROL_MESSAGE;
 
-typedef struct _DDR_SETTING {
+struct bcm_ddr_setting {
 	UINT ulRegAddress;
 	UINT ulRegValue;
-} DDR_SETTING, *PDDR_SETTING;
-typedef DDR_SETTING DDR_SET_NODE, *PDDR_SET_NODE;
-int InitAdapter(PMINI_ADAPTER psAdapter);
+};
+int InitAdapter(struct bcm_mini_adapter *psAdapter);
 
 /* =====================================================================
  * Beceem vendor request codes for EP0
@@ -585,9 +447,9 @@ int InitAdapter(PMINI_ADAPTER psAdapter);
 #define EP5 4
 #define EP6 5
 
-typedef enum eInterface_setting {
+enum bcm_einterface_setting {
 	DEFAULT_SETTING_0  = 0,
 	ALTERNATE_SETTING_1 = 1,
-} INTERFACE_SETTING;
+};
 
 #endif	/* __ADAPTER_H__ */
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index cf305921695..cf411d1706b 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -15,11 +15,11 @@
 
 static int bcm_char_open(struct inode *inode, struct file * filp)
 {
-	PMINI_ADAPTER       Adapter = NULL;
-	PPER_TARANG_DATA    pTarang = NULL;
+	struct bcm_mini_adapter *Adapter = NULL;
+	struct bcm_tarang_data *pTarang = NULL;
 
 	Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	pTarang = kzalloc(sizeof(PER_TARANG_DATA), GFP_KERNEL);
+	pTarang = kzalloc(sizeof(struct bcm_tarang_data), GFP_KERNEL);
 	if (!pTarang)
 		return -ENOMEM;
 
@@ -43,11 +43,11 @@ static int bcm_char_open(struct inode *inode, struct file * filp)
 
 static int bcm_char_release(struct inode *inode, struct file *filp)
 {
-	PPER_TARANG_DATA pTarang, tmp, ptmp;
-	PMINI_ADAPTER Adapter = NULL;
+	struct bcm_tarang_data *pTarang, *tmp, *ptmp;
+	struct bcm_mini_adapter *Adapter = NULL;
 	struct sk_buff *pkt, *npkt;
 
-	pTarang = (PPER_TARANG_DATA)filp->private_data;
+	pTarang = (struct bcm_tarang_data *)filp->private_data;
 
 	if (pTarang == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
@@ -97,8 +97,8 @@ static int bcm_char_release(struct inode *inode, struct file *filp)
 static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
 			     loff_t *f_pos)
 {
-	PPER_TARANG_DATA pTarang = filp->private_data;
-	PMINI_ADAPTER	Adapter = pTarang->Adapter;
+	struct bcm_tarang_data *pTarang = filp->private_data;
+	struct bcm_mini_adapter *Adapter = pTarang->Adapter;
 	struct sk_buff *Packet = NULL;
 	ssize_t PktLen = 0;
 	int wait_ret_val = 0;
@@ -155,9 +155,9 @@ static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
 
 static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 {
-	PPER_TARANG_DATA  pTarang = filp->private_data;
+	struct bcm_tarang_data *pTarang = filp->private_data;
 	void __user *argp = (void __user *)arg;
-	PMINI_ADAPTER Adapter = pTarang->Adapter;
+	struct bcm_mini_adapter *Adapter = pTarang->Adapter;
 	INT Status = STATUS_FAILURE;
 	int timeout = 0;
 	IOCTL_BUFFER IoBuffer;
@@ -722,7 +722,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER)))
 			return -EFAULT;
 
-		if (IoBuffer.InputLength < sizeof(struct link_request))
+		if (IoBuffer.InputLength < sizeof(struct bcm_link_request))
 			return -EINVAL;
 
 		if (IoBuffer.InputLength > MAX_CNTL_PKT_SIZE)
@@ -787,7 +787,7 @@ cntrlEnd:
 	}
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD: {
-		FIRMWARE_INFO *psFwInfo = NULL;
+		struct bcm_firmware_info *psFwInfo = NULL;
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Starting the firmware download PID =0x%x!!!!\n", current->pid);
 
 		if (!down_trylock(&Adapter->fw_download_sema)) {
@@ -807,7 +807,7 @@ cntrlEnd:
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 				"Length for FW DLD is : %lx\n", IoBuffer.InputLength);
 
-		if (IoBuffer.InputLength > sizeof(FIRMWARE_INFO)) {
+		if (IoBuffer.InputLength > sizeof(struct bcm_firmware_info)) {
 			up(&Adapter->fw_download_sema);
 			return -EINVAL;
 		}
@@ -971,7 +971,7 @@ cntrlEnd:
 		break;
 
 	case IOCTL_GET_PACK_INFO:
-		if (copy_to_user(argp, &Adapter->PackInfo, sizeof(PacketInfo)*NO_OF_QUEUES))
+		if (copy_to_user(argp, &Adapter->PackInfo, sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
 			return -EFAULT;
 		Status = STATUS_SUCCESS;
 		break;
@@ -2014,7 +2014,7 @@ static const struct file_operations bcm_fops = {
 	.llseek = no_llseek,
 };
 
-int register_control_device_interface(PMINI_ADAPTER Adapter)
+int register_control_device_interface(struct bcm_mini_adapter *Adapter)
 {
 
 	if (Adapter->major > 0)
@@ -2039,7 +2039,7 @@ int register_control_device_interface(PMINI_ADAPTER Adapter)
 	return 0;
 }
 
-void unregister_control_device_interface(PMINI_ADAPTER Adapter)
+void unregister_control_device_interface(struct bcm_mini_adapter *Adapter)
 {
 	if (Adapter->major > 0) {
 		device_destroy(bcm_class, MKDEV(Adapter->major, 0));
diff --git a/drivers/staging/bcm/Bcmnet.c b/drivers/staging/bcm/Bcmnet.c
index 133e146a3dd..6e8c7f52321 100644
--- a/drivers/staging/bcm/Bcmnet.c
+++ b/drivers/staging/bcm/Bcmnet.c
@@ -4,7 +4,7 @@ struct net_device *gblpnetdev;
 
 static INT bcm_open(struct net_device *dev)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 
 	if (Adapter->fw_download_done == FALSE) {
 		pr_notice(PFX "%s: link up failed (download in progress)\n",
@@ -28,7 +28,7 @@ static INT bcm_open(struct net_device *dev)
 
 static INT bcm_close(struct net_device *dev)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 
 	if (netif_msg_ifdown(Adapter))
 		pr_info(PFX "%s: disabling interface\n", dev->name);
@@ -59,7 +59,7 @@ static u16 bcm_select_queue(struct net_device *dev, struct sk_buff *skb)
 
 static netdev_tx_t bcm_transmit(struct sk_buff *skb, struct net_device *dev)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 	u16 qindex = skb_get_queue_mapping(skb);
 
 
@@ -141,7 +141,7 @@ static int bcm_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 static void bcm_get_drvinfo(struct net_device *dev,
 			    struct ethtool_drvinfo *info)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 	PS_INTERFACE_ADAPTER psIntfAdapter = Adapter->pvInterfaceAdapter;
 	struct usb_device *udev = interface_to_usbdev(psIntfAdapter->interface);
 
@@ -156,21 +156,21 @@ static void bcm_get_drvinfo(struct net_device *dev,
 
 static u32 bcm_get_link(struct net_device *dev)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 
 	return Adapter->LinkUpStatus;
 }
 
 static u32 bcm_get_msglevel(struct net_device *dev)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 
 	return Adapter->msg_enable;
 }
 
 static void bcm_set_msglevel(struct net_device *dev, u32 level)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
 
 	Adapter->msg_enable = level;
 }
@@ -183,7 +183,7 @@ static const struct ethtool_ops bcm_ethtool_ops = {
 	.set_msglevel	= bcm_set_msglevel,
 };
 
-int register_networkdev(PMINI_ADAPTER Adapter)
+int register_networkdev(struct bcm_mini_adapter *Adapter)
 {
 	struct net_device *net = Adapter->dev;
 	PS_INTERFACE_ADAPTER IntfAdapter = Adapter->pvInterfaceAdapter;
@@ -224,7 +224,7 @@ int register_networkdev(PMINI_ADAPTER Adapter)
 	return 0;
 }
 
-void unregister_networkdev(PMINI_ADAPTER Adapter)
+void unregister_networkdev(struct bcm_mini_adapter *Adapter)
 {
 	struct net_device *net = Adapter->dev;
 	PS_INTERFACE_ADAPTER IntfAdapter = Adapter->pvInterfaceAdapter;
diff --git a/drivers/staging/bcm/CmHost.c b/drivers/staging/bcm/CmHost.c
index 7e38af5e176..b54ec974477 100644
--- a/drivers/staging/bcm/CmHost.c
+++ b/drivers/staging/bcm/CmHost.c
@@ -4,7 +4,6 @@
  * Management.
  ************************************************************/
 
-/* #define CONN_MSG */
 #include "headers.h"
 
 enum E_CLASSIFIER_ACTION {
@@ -14,7 +13,7 @@ enum E_CLASSIFIER_ACTION {
 	eDeleteClassifier
 };
 
-static ULONG GetNextTargetBufferLocation(PMINI_ADAPTER Adapter, B_UINT16 tid);
+static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UINT16 tid);
 
 /************************************************************
  * Function - SearchSfid
@@ -28,7 +27,7 @@ static ULONG GetNextTargetBufferLocation(PMINI_ADAPTER Adapter, B_UINT16 tid);
  * Returns - Queue index for this SFID(If matched)
  *  Else Invalid Queue Index(If Not matched)
  ************************************************************/
-int SearchSfid(PMINI_ADAPTER Adapter, UINT uiSfid)
+int SearchSfid(struct bcm_mini_adapter *Adapter, UINT uiSfid)
 {
 	int i;
 
@@ -49,7 +48,7 @@ int SearchSfid(PMINI_ADAPTER Adapter, UINT uiSfid)
  * Returns - Queue index for the free SFID
  *  Else returns Invalid Index.
  ****************************************************************/
-static int SearchFreeSfid(PMINI_ADAPTER Adapter)
+static int SearchFreeSfid(struct bcm_mini_adapter *Adapter)
 {
 	int i;
 
@@ -63,12 +62,12 @@ static int SearchFreeSfid(PMINI_ADAPTER Adapter)
 /*
  * Function: SearchClsid
  * Description:	This routinue would search Classifier  having specified ClassifierID as input parameter
- * Input parameters: PMINI_ADAPTER Adapter - Adapter Context
+ * Input parameters: struct bcm_mini_adapter *Adapter - Adapter Context
  *  unsigned int uiSfid   - The SF in which the classifier is to searched
  *  B_UINT16  uiClassifierID - The classifier ID to be searched
  * Return: int :Classifier table index of matching entry
  */
-static int SearchClsid(PMINI_ADAPTER Adapter, ULONG ulSFID, B_UINT16  uiClassifierID)
+static int SearchClsid(struct bcm_mini_adapter *Adapter, ULONG ulSFID, B_UINT16  uiClassifierID)
 {
 	int i;
 
@@ -87,7 +86,7 @@ static int SearchClsid(PMINI_ADAPTER Adapter, ULONG ulSFID, B_UINT16  uiClassifi
  * This routinue would search Free available Classifier entry in classifier table.
  * @return free Classifier Entry index in classifier table for specified SF
  */
-static int SearchFreeClsid(PMINI_ADAPTER Adapter /**Adapter Context*/)
+static int SearchFreeClsid(struct bcm_mini_adapter *Adapter /**Adapter Context*/)
 {
 	int i;
 
@@ -99,7 +98,7 @@ static int SearchFreeClsid(PMINI_ADAPTER Adapter /**Adapter Context*/)
 	return MAX_CLASSIFIERS+1;
 }
 
-static VOID deleteSFBySfid(PMINI_ADAPTER Adapter, UINT uiSearchRuleIndex)
+static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex)
 {
 	/* deleting all the packet held in the SF */
 	flush_queue(Adapter, uiSearchRuleIndex);
@@ -112,7 +111,7 @@ static VOID deleteSFBySfid(PMINI_ADAPTER Adapter, UINT uiSearchRuleIndex)
 }
 
 static inline VOID
-CopyIpAddrToClassifier(S_CLASSIFIER_RULE *pstClassifierEntry,
+CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 		B_UINT8 u8IpAddressLen, B_UINT8 *pu8IpAddressMaskSrc,
 		BOOLEAN bIpVersion6, E_IPADDR_CONTEXT eIpAddrContext)
 {
@@ -120,7 +119,7 @@ CopyIpAddrToClassifier(S_CLASSIFIER_RULE *pstClassifierEntry,
 	UINT nSizeOfIPAddressInBytes = IP_LENGTH_OF_ADDRESS;
 	UCHAR *ptrClassifierIpAddress = NULL;
 	UCHAR *ptrClassifierIpMask = NULL;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	if (bIpVersion6)
 		nSizeOfIPAddressInBytes = IPV6_ADDRESS_SIZEINBYTES;
@@ -214,7 +213,7 @@ CopyIpAddrToClassifier(S_CLASSIFIER_RULE *pstClassifierEntry,
 	}
 }
 
-void ClearTargetDSXBuffer(PMINI_ADAPTER Adapter, B_UINT16 TID, BOOLEAN bFreeAll)
+void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, BOOLEAN bFreeAll)
 {
 	int i;
 
@@ -236,9 +235,9 @@ void ClearTargetDSXBuffer(PMINI_ADAPTER Adapter, B_UINT16 TID, BOOLEAN bFreeAll)
  * @ingroup ctrl_pkt_functions
  * copy classifier rule into the specified SF index
  */
-static inline VOID CopyClassifierRuleToSF(PMINI_ADAPTER Adapter, stConvergenceSLTypes  *psfCSType, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stConvergenceSLTypes  *psfCSType, UINT uiSearchRuleIndex, UINT nClassifierIndex)
 {
-	S_CLASSIFIER_RULE *pstClassifierEntry = NULL;
+	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	/* VOID *pvPhsContext = NULL; */
 	int i;
 	/* UCHAR ucProtocolLength=0; */
@@ -365,9 +364,9 @@ static inline VOID CopyClassifierRuleToSF(PMINI_ADAPTER Adapter, stConvergenceSL
 /*
  * @ingroup ctrl_pkt_functions
  */
-static inline VOID DeleteClassifierRuleFromSF(PMINI_ADAPTER Adapter, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex, UINT nClassifierIndex)
 {
-	S_CLASSIFIER_RULE *pstClassifierEntry = NULL;
+	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	B_UINT16 u16PacketClassificationRuleIndex;
 	USHORT usVCID;
 	/* VOID *pvPhsContext = NULL; */
@@ -386,7 +385,7 @@ static inline VOID DeleteClassifierRuleFromSF(PMINI_ADAPTER Adapter, UINT uiSear
 	if (pstClassifierEntry) {
 		pstClassifierEntry->bUsed = FALSE;
 		pstClassifierEntry->uiClassifierRuleIndex = 0;
-		memset(pstClassifierEntry, 0, sizeof(S_CLASSIFIER_RULE));
+		memset(pstClassifierEntry, 0, sizeof(struct bcm_classifier_rule));
 
 		/* Delete the PHS Rule for this classifier */
 		PhsDeleteClassifierRule(&Adapter->stBCMPhsContext, usVCID, u16PacketClassificationRuleIndex);
@@ -396,9 +395,9 @@ static inline VOID DeleteClassifierRuleFromSF(PMINI_ADAPTER Adapter, UINT uiSear
 /*
  * @ingroup ctrl_pkt_functions
  */
-VOID DeleteAllClassifiersForSF(PMINI_ADAPTER Adapter, UINT uiSearchRuleIndex)
+VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex)
 {
-	S_CLASSIFIER_RULE *pstClassifierEntry = NULL;
+	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	int i;
 	/* B_UINT16  u16PacketClassificationRuleIndex; */
 	USHORT ulVCID;
@@ -428,7 +427,7 @@ VOID DeleteAllClassifiersForSF(PMINI_ADAPTER Adapter, UINT uiSearchRuleIndex)
  * related data into the Adapter structure.
  * @ingroup ctrl_pkt_functions
  */
-static VOID CopyToAdapter(register PMINI_ADAPTER Adapter, /* <Pointer to the Adapter structure */
+static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer to the Adapter structure */
 			register pstServiceFlowParamSI psfLocalSet, /* <Pointer to the ServiceFlowParamSI structure */
 			register UINT uiSearchRuleIndex, /* <Index of Queue, to which this data belongs */
 			register UCHAR ucDsxType,
@@ -836,7 +835,7 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 	int nIndex;
 	stLocalSFAddIndicationAlt *pstAddIndication;
 	UINT nCurClassifierCnt;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	pstAddIndication = (stLocalSFAddIndicationAlt *)pvBuffer;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "======>");
@@ -967,24 +966,18 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddressLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddress[6]: 0x %02X %02X %02X %02X %02X %02X",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8EthernetDestMacAddress[6]: %pM",
+				psfCSType->cCPacketClassificationRule.
+						u8EthernetDestMacAddress);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddressLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: 0x %02X %02X %02X %02X %02X %02X",
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: "
+				"%pM", psfCSType->cCPacketClassificationRule.
+						u8EthernetSourceMACAddress);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthertypeLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8EthertypeLength);
@@ -1123,24 +1116,18 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddressLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetDestMacAddress[6]: 0x %02X %02X %02X %02X %02X %02X",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8EthernetDestMacAddress[6]: %pM",
+				psfCSType->cCPacketClassificationRule.
+						u8EthernetDestMacAddress);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddressLength: 0x%02X ",
 				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: 0x %02X %02X %02X %02X %02X %02X",
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: "
+				"%pM", psfCSType->cCPacketClassificationRule.
+						u8EthernetSourceMACAddress);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthertypeLength: 0x%02X ", psfCSType->cCPacketClassificationRule.u8EthertypeLength);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8Ethertype[3]: 0x%02X %02X %02X",
@@ -1325,7 +1312,7 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " bValid: 0x%X", pstAddIndication->sfActiveSet.bValid);
 }
 
-static inline ULONG RestoreSFParam(PMINI_ADAPTER Adapter, ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
+static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter, ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
 {
 	UINT  nBytesToRead = sizeof(stServiceFlowParamSI);
 
@@ -1342,7 +1329,7 @@ static inline ULONG RestoreSFParam(PMINI_ADAPTER Adapter, ULONG ulAddrSFParamSet
 	return 1;
 }
 
-static ULONG StoreSFParam(PMINI_ADAPTER Adapter, PUCHAR pucSrcBuffer, ULONG ulAddrSFParamSet)
+static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer, ULONG ulAddrSFParamSet)
 {
 	UINT nBytesToWrite = sizeof(stServiceFlowParamSI);
 	int ret = 0;
@@ -1358,7 +1345,7 @@ static ULONG StoreSFParam(PMINI_ADAPTER Adapter, PUCHAR pucSrcBuffer, ULONG ulAd
 	return 1;
 }
 
-ULONG StoreCmControlResponseMessage(PMINI_ADAPTER Adapter, PVOID pvBuffer, UINT *puBufferLength)
+ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBuffer, UINT *puBufferLength)
 {
 	stLocalSFAddIndicationAlt *pstAddIndicationAlt = NULL;
 	stLocalSFAddIndication *pstAddIndication = NULL;
@@ -1473,7 +1460,7 @@ ULONG StoreCmControlResponseMessage(PMINI_ADAPTER Adapter, PVOID pvBuffer, UINT
 }
 
 static inline stLocalSFAddIndicationAlt
-*RestoreCmControlResponseMessage(register PMINI_ADAPTER Adapter, register PVOID pvBuffer)
+*RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter, register PVOID pvBuffer)
 {
 	ULONG ulStatus = 0;
 	stLocalSFAddIndication *pstAddIndication = NULL;
@@ -1551,7 +1538,7 @@ failed_restore_sf_param:
 	return NULL;
 }
 
-ULONG SetUpTargetDsxBuffers(PMINI_ADAPTER Adapter)
+ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 {
 	ULONG ulTargetDsxBuffersBase = 0;
 	ULONG ulCntTargetBuffers;
@@ -1598,7 +1585,7 @@ ULONG SetUpTargetDsxBuffers(PMINI_ADAPTER Adapter)
 	return 1;
 }
 
-static ULONG GetNextTargetBufferLocation(PMINI_ADAPTER Adapter, B_UINT16 tid)
+static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UINT16 tid)
 {
 	ULONG ulTargetDSXBufferAddress;
 	ULONG ulTargetDsxBufferIndexToUse, ulMaxTry;
@@ -1632,7 +1619,7 @@ static ULONG GetNextTargetBufferLocation(PMINI_ADAPTER Adapter, B_UINT16 tid)
 	return ulTargetDSXBufferAddress;
 }
 
-int AllocAdapterDsxBuffer(PMINI_ADAPTER Adapter)
+int AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
 {
 	/*
 	 * Need to Allocate memory to contain the SUPER Large structures
@@ -1645,7 +1632,7 @@ int AllocAdapterDsxBuffer(PMINI_ADAPTER Adapter)
 	return 0;
 }
 
-int FreeAdapterDsxBuffer(PMINI_ADAPTER Adapter)
+int FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
 {
 	kfree(Adapter->caDsxReqResp);
 	return 0;
@@ -1657,13 +1644,13 @@ int FreeAdapterDsxBuffer(PMINI_ADAPTER Adapter)
  * for the Connection Management.
  * @return - Queue index for the free SFID else returns Invalid Index.
  */
-BOOLEAN CmControlResponseMessage(PMINI_ADAPTER Adapter,  /* <Pointer to the Adapter structure */
+BOOLEAN CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer to the Adapter structure */
 				PVOID pvBuffer /* Starting Address of the Buffer, that contains the AddIndication Data */)
 {
 	stServiceFlowParamSI *psfLocalSet = NULL;
 	stLocalSFAddIndicationAlt *pstAddIndication = NULL;
 	stLocalSFChangeIndicationAlt *pstChangeIndication = NULL;
-	PLEADER pLeader = NULL;
+	struct bcm_leader *pLeader = NULL;
 
 	/*
 	 * Otherwise the message contains a target address from where we need to
@@ -1678,7 +1665,7 @@ BOOLEAN CmControlResponseMessage(PMINI_ADAPTER Adapter,  /* <Pointer to the Adap
 
 	DumpCmControlPacket(pstAddIndication);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "====>");
-	pLeader = (PLEADER)Adapter->caDsxReqResp;
+	pLeader = (struct bcm_leader *)Adapter->caDsxReqResp;
 
 	pLeader->Status = CM_CONTROL_NEWDSX_MULTICLASSIFIER_REQ;
 	pLeader->Vcid = 0;
@@ -1915,10 +1902,10 @@ BOOLEAN CmControlResponseMessage(PMINI_ADAPTER Adapter,  /* <Pointer to the Adap
 	return TRUE;
 }
 
-int get_dsx_sf_data_to_application(PMINI_ADAPTER Adapter, UINT uiSFId, void __user *user_buffer)
+int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter, UINT uiSFId, void __user *user_buffer)
 {
 	int status = 0;
-	struct _packet_info *psSfInfo = NULL;
+	struct bcm_packet_info *psSfInfo = NULL;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "status =%d", status);
 	status = SearchSfid(Adapter, uiSFId);
@@ -1937,7 +1924,7 @@ int get_dsx_sf_data_to_application(PMINI_ADAPTER Adapter, UINT uiSFId, void __us
 	return STATUS_SUCCESS;
 }
 
-VOID OverrideServiceFlowParams(PMINI_ADAPTER Adapter, PUINT puiBuffer)
+VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer)
 {
 	B_UINT32 u32NumofSFsinMsg = ntohl(*(puiBuffer + 1));
 	stIM_SFHostNotify *pHostInfo = NULL;
diff --git a/drivers/staging/bcm/CmHost.h b/drivers/staging/bcm/CmHost.h
index 8f689769b4b..4cc6f93f232 100644
--- a/drivers/staging/bcm/CmHost.h
+++ b/drivers/staging/bcm/CmHost.h
@@ -148,14 +148,14 @@ typedef struct stLocalSFChangeIndicationAlt{
 
 }stLocalSFChangeIndicationAlt;
 
-ULONG StoreCmControlResponseMessage(PMINI_ADAPTER Adapter,PVOID pvBuffer,UINT *puBufferLength);
+ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBuffer,UINT *puBufferLength);
 
-INT AllocAdapterDsxBuffer(PMINI_ADAPTER Adapter);
+INT AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
 
-INT FreeAdapterDsxBuffer(PMINI_ADAPTER Adapter);
-ULONG SetUpTargetDsxBuffers(PMINI_ADAPTER Adapter);
+INT FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
+ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter);
 
-BOOLEAN CmControlResponseMessage(PMINI_ADAPTER Adapter,PVOID pvBuffer);
+BOOLEAN CmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBuffer);
 
 
 #pragma pack (pop)
diff --git a/drivers/staging/bcm/DDRInit.c b/drivers/staging/bcm/DDRInit.c
index 2b46f4d4ef0..8c696b64ab2 100644
--- a/drivers/staging/bcm/DDRInit.c
+++ b/drivers/staging/bcm/DDRInit.c
@@ -7,7 +7,7 @@
 
     //DDR INIT-133Mhz
 #define T3_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 12  //index for 0x0F007000
-static DDR_SET_NODE asT3_DDRSetting133MHz[]= {//      # DPLL Clock Setting
+static struct bcm_ddr_setting asT3_DDRSetting133MHz[]= {//      # DPLL Clock Setting
                                         {0x0F000800,0x00007212},
                                         {0x0f000820,0x07F13FFF},
                                         {0x0f000810,0x00000F95},
@@ -65,7 +65,7 @@ static DDR_SET_NODE asT3_DDRSetting133MHz[]= {//      # DPLL Clock Setting
                                         };
 //80Mhz
 #define T3_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 10  //index for 0x0F007000
-static DDR_SET_NODE asT3_DDRSetting80MHz[]= {//   # DPLL Clock Setting
+static struct bcm_ddr_setting asT3_DDRSetting80MHz[]= {//   # DPLL Clock Setting
                                         {0x0f000810,0x00000F95},
                                         {0x0f000820,0x07f1ffff},
                                         {0x0f000860,0x00000000},
@@ -117,7 +117,7 @@ static DDR_SET_NODE asT3_DDRSetting80MHz[]= {//   # DPLL Clock Setting
                                 };
 //100Mhz
 #define T3_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 13  //index for 0x0F007000
-static DDR_SET_NODE asT3_DDRSetting100MHz[]= {//  # DPLL Clock Setting
+static struct bcm_ddr_setting asT3_DDRSetting100MHz[]= {//  # DPLL Clock Setting
                                         {0x0F000800,0x00007008},
                                         {0x0f000810,0x00000F95},
                                         {0x0f000820,0x07F13E3F},
@@ -177,7 +177,7 @@ static DDR_SET_NODE asT3_DDRSetting100MHz[]= {//  # DPLL Clock Setting
 
 //Net T3B DDR Settings
 //DDR INIT-133Mhz
-static DDR_SET_NODE asDPLL_266MHZ[] = {
+static struct bcm_ddr_setting asDPLL_266MHZ[] = {
                                         {0x0F000800,0x00007212},
                                         {0x0f000820,0x07F13FFF},
                                         {0x0f000810,0x00000F95},
@@ -189,7 +189,7 @@ static DDR_SET_NODE asDPLL_266MHZ[] = {
 									  };
 
 #define T3B_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 11  //index for 0x0F007000
-static DDR_SET_NODE asT3B_DDRSetting133MHz[] = {//      # DPLL Clock Setting
+static struct bcm_ddr_setting asT3B_DDRSetting133MHz[] = {//      # DPLL Clock Setting
                                         {0x0f000810,0x00000F95},
                                         {0x0f000810,0x00000F95},
                                         {0x0f000810,0x00000F95},
@@ -247,7 +247,7 @@ static DDR_SET_NODE asT3B_DDRSetting133MHz[] = {//      # DPLL Clock Setting
                                         };
 
 #define T3B_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 9  //index for 0x0F007000
-static DDR_SET_NODE asT3B_DDRSetting80MHz[] = {//       # DPLL Clock Setting
+static struct bcm_ddr_setting asT3B_DDRSetting80MHz[] = {//       # DPLL Clock Setting
 										{0x0f000810,0x00000F95},
 										{0x0f000820,0x07F13FFF},
 										{0x0f000840,0x0FFF1F00},
@@ -301,7 +301,7 @@ static DDR_SET_NODE asT3B_DDRSetting80MHz[] = {//       # DPLL Clock Setting
 
 //100Mhz
 #define T3B_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 9  //index for 0x0F007000
-static DDR_SET_NODE asT3B_DDRSetting100MHz[] = {//      # DPLL Clock Setting
+static struct bcm_ddr_setting asT3B_DDRSetting100MHz[] = {//      # DPLL Clock Setting
 										{0x0f000810,0x00000F95},
 										{0x0f000820,0x07F1369B},
 										{0x0f000840,0x0FFF0800},
@@ -356,7 +356,7 @@ static DDR_SET_NODE asT3B_DDRSetting100MHz[] = {//      # DPLL Clock Setting
 
 
 #define T3LP_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 9  //index for 0x0F007000
-static DDR_SET_NODE asT3LP_DDRSetting133MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LP_DDRSetting133MHz[]= {//	# DPLL Clock Setting
 								{0x0f000820,0x03F1365B},
 								{0x0f000810,0x00002F95},
 								{0x0f000880,0x000003DD},
@@ -416,7 +416,7 @@ static DDR_SET_NODE asT3LP_DDRSetting133MHz[]= {//	# DPLL Clock Setting
 };
 
 #define T3LP_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 11  //index for 0x0F007000
-static DDR_SET_NODE asT3LP_DDRSetting100MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LP_DDRSetting100MHz[]= {//	# DPLL Clock Setting
 								{0x0f000810,0x00002F95},
 								{0x0f000820,0x03F1369B},
 								{0x0f000840,0x0fff0000},
@@ -476,7 +476,7 @@ static DDR_SET_NODE asT3LP_DDRSetting100MHz[]= {//	# DPLL Clock Setting
 };
 
 #define T3LP_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 9  //index for 0x0F007000
-static DDR_SET_NODE asT3LP_DDRSetting80MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LP_DDRSetting80MHz[]= {//	# DPLL Clock Setting
 								{0x0f000820,0x07F13FFF},
 								{0x0f000810,0x00002F95},
 								{0x0f000860,0x00000000},
@@ -536,7 +536,7 @@ static DDR_SET_NODE asT3LP_DDRSetting80MHz[]= {//	# DPLL Clock Setting
 ///T3 LP-B (UMA-B)
 
 #define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_160MHZ 7  //index for 0x0F007000
-static DDR_SET_NODE asT3LPB_DDRSetting160MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LPB_DDRSetting160MHz[]= {//	# DPLL Clock Setting
 
 								{0x0f000820,0x03F137DB},
 								{0x0f000810,0x01842795},
@@ -594,7 +594,7 @@ static DDR_SET_NODE asT3LPB_DDRSetting160MHz[]= {//	# DPLL Clock Setting
 
 
 #define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_133MHZ 7  //index for 0x0F007000
-static DDR_SET_NODE asT3LPB_DDRSetting133MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LPB_DDRSetting133MHz[]= {//	# DPLL Clock Setting
 								{0x0f000820,0x03F1365B},
 								{0x0f000810,0x00002F95},
 								{0x0f000880,0x000003DD},
@@ -655,7 +655,7 @@ static DDR_SET_NODE asT3LPB_DDRSetting133MHz[]= {//	# DPLL Clock Setting
 };
 
 #define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_100MHZ 8  //index for 0x0F007000
-static DDR_SET_NODE asT3LPB_DDRSetting100MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LPB_DDRSetting100MHz[]= {//	# DPLL Clock Setting
 								{0x0f000810,0x00002F95},
 								{0x0f000820,0x03F1369B},
 								{0x0f000840,0x0fff0000},
@@ -716,7 +716,7 @@ static DDR_SET_NODE asT3LPB_DDRSetting100MHz[]= {//	# DPLL Clock Setting
 };
 
 #define T3LPB_SKIP_CLOCK_PROGRAM_DUMP_80MHZ 7  //index for 0x0F007000
-static DDR_SET_NODE asT3LPB_DDRSetting80MHz[]= {//	# DPLL Clock Setting
+static struct bcm_ddr_setting asT3LPB_DDRSetting80MHz[]= {//	# DPLL Clock Setting
 								{0x0f000820,0x07F13FFF},
 								{0x0f000810,0x00002F95},
 								{0x0f000860,0x00000000},
@@ -772,9 +772,9 @@ static DDR_SET_NODE asT3LPB_DDRSetting80MHz[]= {//	# DPLL Clock Setting
 };
 
 
-int ddr_init(MINI_ADAPTER *Adapter)
+int ddr_init(struct bcm_mini_adapter *Adapter)
 {
-	PDDR_SETTING psDDRSetting=NULL;
+	struct bcm_ddr_setting *psDDRSetting=NULL;
 	ULONG RegCount=0;
 	UINT value = 0;
 	UINT  uiResetValue = 0;
@@ -789,17 +789,17 @@ int ddr_init(MINI_ADAPTER *Adapter)
 	        case DDR_80_MHZ:
 				psDDRSetting=asT3LP_DDRSetting80MHz;
 			    RegCount=(sizeof(asT3LP_DDRSetting80MHz)/
-			  	sizeof(DDR_SETTING));
+			  	sizeof(struct bcm_ddr_setting));
 			    break;
 		    case DDR_100_MHZ:
 				psDDRSetting=asT3LP_DDRSetting100MHz;
 			    RegCount=(sizeof(asT3LP_DDRSetting100MHz)/
-			  	sizeof(DDR_SETTING));
+			  	sizeof(struct bcm_ddr_setting));
 			    break;
 		    case DDR_133_MHZ:
 				psDDRSetting=asT3LP_DDRSetting133MHz;
 			    RegCount=(sizeof(asT3LP_DDRSetting133MHz)/
-		 	  		sizeof(DDR_SETTING));
+		 	  		sizeof(struct bcm_ddr_setting));
 				if(Adapter->bMipsConfig == MIPS_200_MHZ)
 				{
 					uiClockSetting = 0x03F13652;
@@ -846,17 +846,17 @@ int ddr_init(MINI_ADAPTER *Adapter)
 			case DDR_80_MHZ:
 				psDDRSetting = asT3LPB_DDRSetting80MHz;
 		        RegCount=(sizeof(asT3B_DDRSetting80MHz)/
-		                  sizeof(DDR_SETTING));
+		                  sizeof(struct bcm_ddr_setting));
 			break;
             case DDR_100_MHZ:
 				psDDRSetting=asT3LPB_DDRSetting100MHz;
 		        RegCount=(sizeof(asT3B_DDRSetting100MHz)/
-		                 sizeof(DDR_SETTING));
+		                 sizeof(struct bcm_ddr_setting));
 			break;
             case DDR_133_MHZ:
 				psDDRSetting = asT3LPB_DDRSetting133MHz;
 				RegCount=(sizeof(asT3B_DDRSetting133MHz)/
-						 sizeof(DDR_SETTING));
+						 sizeof(struct bcm_ddr_setting));
 
 				if(Adapter->bMipsConfig == MIPS_200_MHZ)
 				{
@@ -870,7 +870,7 @@ int ddr_init(MINI_ADAPTER *Adapter)
 
 			case DDR_160_MHZ:
 				psDDRSetting = asT3LPB_DDRSetting160MHz;
-				RegCount = sizeof(asT3LPB_DDRSetting160MHz)/sizeof(DDR_SETTING);
+				RegCount = sizeof(asT3LPB_DDRSetting160MHz)/sizeof(struct bcm_ddr_setting);
 
 				if(Adapter->bMipsConfig == MIPS_200_MHZ)
 				{
@@ -894,17 +894,17 @@ int ddr_init(MINI_ADAPTER *Adapter)
 	        case DDR_80_MHZ:
 				psDDRSetting = asT3_DDRSetting80MHz;
 			    RegCount = (sizeof(asT3_DDRSetting80MHz)/
-			  	sizeof(DDR_SETTING));
+			  	sizeof(struct bcm_ddr_setting));
 			    break;
 		    case DDR_100_MHZ:
 				psDDRSetting = asT3_DDRSetting100MHz;
 			    RegCount = (sizeof(asT3_DDRSetting100MHz)/
-			  	sizeof(DDR_SETTING));
+			  	sizeof(struct bcm_ddr_setting));
 			    break;
 		    case DDR_133_MHZ:
 				psDDRSetting = asT3_DDRSetting133MHz;
 			    RegCount = (sizeof(asT3_DDRSetting133MHz)/
-		 	  	sizeof(DDR_SETTING));
+		 	  	sizeof(struct bcm_ddr_setting));
 				break;
 		    default:
 			    return -EINVAL;
@@ -916,12 +916,12 @@ int ddr_init(MINI_ADAPTER *Adapter)
 	        case DDR_80_MHZ:
 				psDDRSetting = asT3B_DDRSetting80MHz;
 		        RegCount=(sizeof(asT3B_DDRSetting80MHz)/
-		                  sizeof(DDR_SETTING));
+		                  sizeof(struct bcm_ddr_setting));
 		    break;
             case DDR_100_MHZ:
 				psDDRSetting=asT3B_DDRSetting100MHz;
 		        RegCount=(sizeof(asT3B_DDRSetting100MHz)/
-		                 sizeof(DDR_SETTING));
+		                 sizeof(struct bcm_ddr_setting));
 			break;
             case DDR_133_MHZ:
 
@@ -931,13 +931,13 @@ int ddr_init(MINI_ADAPTER *Adapter)
 									 sizeof(asDPLL_266MHZ));
 					psDDRSetting = asT3B_DDRSetting133MHz;
 					RegCount=(sizeof(asT3B_DDRSetting133MHz)/
-									sizeof(DDR_SETTING));
+									sizeof(struct bcm_ddr_setting));
 				}
 				else
 				{
 					psDDRSetting = asT3B_DDRSetting133MHz;
 					RegCount=(sizeof(asT3B_DDRSetting133MHz)/
-									sizeof(DDR_SETTING));
+									sizeof(struct bcm_ddr_setting));
 					if(Adapter->bMipsConfig == MIPS_200_MHZ)
 					{
 						uiClockSetting = 0x07F13652;
@@ -1099,9 +1099,9 @@ int ddr_init(MINI_ADAPTER *Adapter)
 	return retval;
 }
 
-int download_ddr_settings(PMINI_ADAPTER Adapter)
+int download_ddr_settings(struct bcm_mini_adapter *Adapter)
 {
-	PDDR_SET_NODE psDDRSetting=NULL;
+	struct bcm_ddr_setting *psDDRSetting=NULL;
 	ULONG RegCount=0;
 	unsigned long ul_ddr_setting_load_addr = DDR_DUMP_INTERNAL_DEVICE_MEMORY;
 	UINT  value = 0;
@@ -1250,7 +1250,7 @@ int download_ddr_settings(PMINI_ADAPTER Adapter)
 	}
 
 	ul_ddr_setting_load_addr+=sizeof(ULONG);
-	RegCount*=(sizeof(DDR_SETTING)/sizeof(ULONG));
+	RegCount*=(sizeof(struct bcm_ddr_setting)/sizeof(ULONG));
 
 	while(RegCount && !retval)
 	{
diff --git a/drivers/staging/bcm/DDRInit.h b/drivers/staging/bcm/DDRInit.h
index 550e260df53..b0196fce925 100644
--- a/drivers/staging/bcm/DDRInit.h
+++ b/drivers/staging/bcm/DDRInit.h
@@ -3,7 +3,7 @@
 
 
 
-int ddr_init(PMINI_ADAPTER	psAdapter);
-int download_ddr_settings(PMINI_ADAPTER	psAdapter);
+int ddr_init(struct bcm_mini_adapter *psAdapter);
+int download_ddr_settings(struct bcm_mini_adapter *psAdapter);
 
 #endif
diff --git a/drivers/staging/bcm/Debug.h b/drivers/staging/bcm/Debug.h
index 420382d1cac..8018a189f81 100644
--- a/drivers/staging/bcm/Debug.h
+++ b/drivers/staging/bcm/Debug.h
@@ -42,10 +42,6 @@
 #define ARP_REQ  	(TX<<5)
 #define ARP_RESP 	(TX<<6)
 
-// dhcp.c
-//#define DHCP TX
-//#define DHCP_REQ (DHCP<<7)
-
 // Leakybucket.c
 #define TOKEN_COUNTS (TX<<8)
 #define CHECK_TOKENS (TX<<9)
@@ -147,7 +143,6 @@ DriverEntry.c, bcmfwup.c, ChipDetectTask.c, HaltnReset.c, InterfaceDDR.c */
 
 #define HOST_MIBS   	(OTHERS << 28)
 #define CONN_MSG    	(CMHOST << 29)
-//#define OTHERS_MISC		(OTHERS << 29)	// ProcSupport.c
 /*-----------------END SUBTYPEs------------------------------------------*/
 
 
diff --git a/drivers/staging/bcm/HandleControlPacket.c b/drivers/staging/bcm/HandleControlPacket.c
index b058e30b2ca..25e5c68bfe8 100644
--- a/drivers/staging/bcm/HandleControlPacket.c
+++ b/drivers/staging/bcm/HandleControlPacket.c
@@ -11,9 +11,9 @@
  * Enqueue the control packet for Application.
  * @return None
  */
-static VOID handle_rx_control_packet(PMINI_ADAPTER Adapter, struct sk_buff *skb)
+static VOID handle_rx_control_packet(struct bcm_mini_adapter *Adapter, struct sk_buff *skb)
 {
-	PPER_TARANG_DATA pTarang = NULL;
+	struct bcm_tarang_data *pTarang = NULL;
 	BOOLEAN HighPriorityMessage = FALSE;
 	struct sk_buff *newPacket = NULL;
 	CHAR cntrl_msg_mask_bit = 0;
@@ -154,7 +154,7 @@ static VOID handle_rx_control_packet(PMINI_ADAPTER Adapter, struct sk_buff *skb)
  * @ingroup ctrl_pkt_functions
  * Thread to handle control pkt reception
  */
-int control_packet_handler(PMINI_ADAPTER Adapter /* pointer to adapter object*/)
+int control_packet_handler(struct bcm_mini_adapter *Adapter /* pointer to adapter object*/)
 {
 	struct sk_buff *ctrl_packet = NULL;
 	unsigned long flags = 0;
@@ -213,8 +213,8 @@ int control_packet_handler(PMINI_ADAPTER Adapter /* pointer to adapter object*/)
 
 INT flushAllAppQ(void)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	PPER_TARANG_DATA pTarang = NULL;
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_tarang_data *pTarang = NULL;
 	struct sk_buff *PacketToDrop = NULL;
 	for (pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next) {
 		while (pTarang->RxAppControlHead != NULL) {
diff --git a/drivers/staging/bcm/IPv6Protocol.c b/drivers/staging/bcm/IPv6Protocol.c
index 1da21642c18..4745ddd62f5 100644
--- a/drivers/staging/bcm/IPv6Protocol.c
+++ b/drivers/staging/bcm/IPv6Protocol.c
@@ -1,8 +1,8 @@
 #include "headers.h"
 
-static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+static BOOLEAN MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
 	IPV6Header *pstIpv6Header);
-static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+static BOOLEAN MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
 	IPV6Header *pstIpv6Header);
 static VOID DumpIpv6Header(IPV6Header *pstIpv6Header);
 
@@ -12,7 +12,7 @@ static UCHAR *GetNextIPV6ChainedHeader(UCHAR **ppucPayload,
 	UCHAR *pucRetHeaderPtr = NULL;
 	UCHAR *pucPayloadPtr = NULL;
 	USHORT  usNextHeaderOffset = 0 ;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	if ((ppucPayload == NULL) || (*pusPayloadLength == 0) ||
 		(*bParseDone)) {
@@ -147,7 +147,7 @@ static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload, USHORT *pusSrcPort,
 	BOOLEAN bDone = FALSE;
 	UCHAR ucHeaderType = 0;
 	UCHAR *pucNextHeader = NULL;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	if (!pucPayload || (usPayloadLength == 0))
 		return 0;
@@ -177,11 +177,11 @@ static UCHAR GetIpv6ProtocolPorts(UCHAR *pucPayload, USHORT *pusSrcPort,
 
 
 /*
- * Arg 1 PMINI_ADAPTER Adapter is a pointer ot the driver contorl structure
+ * Arg 1 struct bcm_mini_adapter *Adapter is a pointer ot the driver contorl structure
  * Arg 2 PVOID pcIpHeader is a pointer to the IP header of the packet
  */
-USHORT	IpVersion6(PMINI_ADAPTER Adapter, PVOID pcIpHeader,
-					S_CLASSIFIER_RULE *pstClassifierRule)
+USHORT	IpVersion6(struct bcm_mini_adapter *Adapter, PVOID pcIpHeader,
+					struct bcm_classifier_rule *pstClassifierRule)
 {
 	USHORT	ushDestPort = 0;
 	USHORT	ushSrcPort = 0;
@@ -288,14 +288,14 @@ USHORT	IpVersion6(PMINI_ADAPTER Adapter, PVOID pcIpHeader,
 }
 
 
-static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+static BOOLEAN MatchSrcIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
 	IPV6Header *pstIpv6Header)
 {
 	UINT uiLoopIndex = 0;
 	UINT uiIpv6AddIndex = 0;
 	UINT uiIpv6AddrNoLongWords = 4;
 	ULONG aulSrcIP[4];
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	/*
 	 * This is the no. of Src Addresses ie Range of IP Addresses contained
 	 * in the classifier rule for which we need to match
@@ -344,14 +344,14 @@ static BOOLEAN MatchSrcIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
 	return FALSE;
 }
 
-static BOOLEAN MatchDestIpv6Address(S_CLASSIFIER_RULE *pstClassifierRule,
+static BOOLEAN MatchDestIpv6Address(struct bcm_classifier_rule *pstClassifierRule,
 	IPV6Header *pstIpv6Header)
 {
 	UINT uiLoopIndex = 0;
 	UINT uiIpv6AddIndex = 0;
 	UINT uiIpv6AddrNoLongWords = 4;
 	ULONG aulDestIP[4];
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	/*
 	 * This is the no. of Destination Addresses
 	 * ie Range of IP Addresses contained in the classifier rule
@@ -406,7 +406,7 @@ VOID DumpIpv6Address(ULONG *puIpv6Address)
 {
 	UINT uiIpv6AddrNoLongWords = 4;
 	UINT uiIpv6AddIndex = 0;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	for (uiIpv6AddIndex = 0; uiIpv6AddIndex < uiIpv6AddrNoLongWords; uiIpv6AddIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 				":%lx", puIpv6Address[uiIpv6AddIndex]);
@@ -418,7 +418,7 @@ static VOID DumpIpv6Header(IPV6Header *pstIpv6Header)
 {
 	UCHAR ucVersion;
 	UCHAR ucPrio;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV6_DBG, DBG_LVL_ALL,
 			"----Ipv6 Header---");
 	ucVersion = pstIpv6Header->ucVersionPrio & 0xf0;
diff --git a/drivers/staging/bcm/IPv6ProtocolHdr.h b/drivers/staging/bcm/IPv6ProtocolHdr.h
index a0db5a1de76..8ba88a5b081 100644
--- a/drivers/staging/bcm/IPv6ProtocolHdr.h
+++ b/drivers/staging/bcm/IPv6ProtocolHdr.h
@@ -102,15 +102,15 @@ typedef enum _E_IPADDR_CONTEXT
 
 //Function Prototypes
 
-USHORT	IpVersion6(PMINI_ADAPTER Adapter, /**< Pointer to the driver control structure */
+USHORT	IpVersion6(struct bcm_mini_adapter *Adapter, /**< Pointer to the driver control structure */
 					PVOID pcIpHeader, /**<Pointer to the IP Hdr of the packet*/
-					S_CLASSIFIER_RULE *pstClassifierRule );
+					struct bcm_classifier_rule *pstClassifierRule );
 
 VOID DumpIpv6Address(ULONG *puIpv6Address);
 
-extern BOOLEAN MatchSrcPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushSrcPort);
-extern BOOLEAN MatchDestPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushSrcPort);
-extern BOOLEAN MatchProtocol(S_CLASSIFIER_RULE *pstClassifierRule,UCHAR ucProtocol);
+extern BOOLEAN MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule,USHORT ushSrcPort);
+extern BOOLEAN MatchDestPort(struct bcm_classifier_rule *pstClassifierRule,USHORT ushSrcPort);
+extern BOOLEAN MatchProtocol(struct bcm_classifier_rule *pstClassifierRule,UCHAR ucProtocol);
 
 
 #endif
diff --git a/drivers/staging/bcm/InterfaceAdapter.h b/drivers/staging/bcm/InterfaceAdapter.h
index 6397c20f4f6..4607c265d98 100644
--- a/drivers/staging/bcm/InterfaceAdapter.h
+++ b/drivers/staging/bcm/InterfaceAdapter.h
@@ -85,7 +85,7 @@ typedef struct _S_INTERFACE_ADAPTER
 	atomic_t		uNumRcbUsed;
 	atomic_t		uCurrRcb;
 
-	PMINI_ADAPTER	psAdapter;
+	struct bcm_mini_adapter *psAdapter;
 	BOOLEAN                 bFlashBoot;
 	BOOLEAN 		bHighSpeedDevice ;
 
diff --git a/drivers/staging/bcm/InterfaceDld.c b/drivers/staging/bcm/InterfaceDld.c
index 65c352f3568..3a89e33733e 100644
--- a/drivers/staging/bcm/InterfaceDld.c
+++ b/drivers/staging/bcm/InterfaceDld.c
@@ -7,7 +7,7 @@ int InterfaceFileDownload(PVOID arg, struct file *flp, unsigned int on_chip_loc)
 	int errno = 0, len = 0; /* ,is_config_file = 0 */
 	loff_t pos = 0;
 	PS_INTERFACE_ADAPTER psIntfAdapter = (PS_INTERFACE_ADAPTER)arg;
-	/* PMINI_ADAPTER Adapter = psIntfAdapter->psAdapter; */
+	/* struct bcm_mini_adapter *Adapter = psIntfAdapter->psAdapter; */
 	char *buff = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_KERNEL);
 
 	if (!buff)
@@ -132,7 +132,7 @@ exit:
 	return Status;
 }
 
-static int bcm_download_config_file(PMINI_ADAPTER Adapter, FIRMWARE_INFO *psFwInfo)
+static int bcm_download_config_file(struct bcm_mini_adapter *Adapter, struct bcm_firmware_info *psFwInfo)
 {
 	int retval = STATUS_SUCCESS;
 	B_UINT32 value = 0;
@@ -208,7 +208,7 @@ static int bcm_download_config_file(PMINI_ADAPTER Adapter, FIRMWARE_INFO *psFwIn
 static int bcm_compare_buff_contents(unsigned char *readbackbuff, unsigned char *buff, unsigned int len)
 {
 	int retval = STATUS_SUCCESS;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if ((len-sizeof(unsigned int)) < 4) {
 		if (memcmp(readbackbuff , buff, len))
 			retval = -EINVAL;
@@ -229,7 +229,7 @@ static int bcm_compare_buff_contents(unsigned char *readbackbuff, unsigned char
 	return retval;
 }
 
-int bcm_ioctl_fw_download(PMINI_ADAPTER Adapter, FIRMWARE_INFO *psFwInfo)
+int bcm_ioctl_fw_download(struct bcm_mini_adapter *Adapter, struct bcm_firmware_info *psFwInfo)
 {
 	int retval = STATUS_SUCCESS;
 	PUCHAR buff = NULL;
@@ -278,7 +278,7 @@ error:
 	return retval;
 }
 
-static INT buffDnld(PMINI_ADAPTER Adapter, PUCHAR mappedbuffer, UINT u32FirmwareLength, ULONG u32StartingAddress)
+static INT buffDnld(struct bcm_mini_adapter *Adapter, PUCHAR mappedbuffer, UINT u32FirmwareLength, ULONG u32StartingAddress)
 {
 	unsigned int len = 0;
 	int retval = STATUS_SUCCESS;
@@ -299,7 +299,7 @@ static INT buffDnld(PMINI_ADAPTER Adapter, PUCHAR mappedbuffer, UINT u32Firmware
 	return retval;
 }
 
-static INT buffRdbkVerify(PMINI_ADAPTER Adapter, PUCHAR mappedbuffer, UINT u32FirmwareLength, ULONG u32StartingAddress)
+static INT buffRdbkVerify(struct bcm_mini_adapter *Adapter, PUCHAR mappedbuffer, UINT u32FirmwareLength, ULONG u32StartingAddress)
 {
 	UINT len = u32FirmwareLength;
 	INT retval = STATUS_SUCCESS;
@@ -334,7 +334,7 @@ static INT buffRdbkVerify(PMINI_ADAPTER Adapter, PUCHAR mappedbuffer, UINT u32Fi
 	return retval;
 }
 
-INT buffDnldVerify(PMINI_ADAPTER Adapter, unsigned char *mappedbuffer, unsigned int u32FirmwareLength, unsigned long u32StartingAddress)
+INT buffDnldVerify(struct bcm_mini_adapter *Adapter, unsigned char *mappedbuffer, unsigned int u32FirmwareLength, unsigned long u32StartingAddress)
 {
 	INT status = STATUS_SUCCESS;
 
diff --git a/drivers/staging/bcm/InterfaceIdleMode.c b/drivers/staging/bcm/InterfaceIdleMode.c
index faeb03e62c0..4f2f490921e 100644
--- a/drivers/staging/bcm/InterfaceIdleMode.c
+++ b/drivers/staging/bcm/InterfaceIdleMode.c
@@ -7,7 +7,7 @@ Description:			This is the hardware specific Function for waking up HW device fr
 						A software abort pattern is written to the device to wake it and necessary power state
 						transitions from host are performed here.
 
-Input parameters:		IN PMINI_ADAPTER Adapter   - Miniport Adapter Context
+Input parameters:		IN struct bcm_mini_adapter *Adapter   - Miniport Adapter Context
 
 
 Return:				BCM_STATUS_SUCCESS - If Wakeup of the HW Interface was successful.
@@ -22,7 +22,7 @@ Description:			This is the hardware specific Function for responding to Idle mod
 						Necessary power state transitions from host for idle mode or other device specific
 						initializations are performed here.
 
-Input parameters:		IN PMINI_ADAPTER Adapter   - Miniport Adapter Context
+Input parameters:		IN struct bcm_mini_adapter * Adapter   - Miniport Adapter Context
 
 
 Return:				BCM_STATUS_SUCCESS - If Idle mode response related HW configuration was successful.
@@ -42,7 +42,7 @@ send to f/w with in 200 ms after the Idle/Shutdown req issued
 */
 
 
-int InterfaceIdleModeRespond(PMINI_ADAPTER Adapter, unsigned int* puiBuffer)
+int InterfaceIdleModeRespond(struct bcm_mini_adapter *Adapter, unsigned int* puiBuffer)
 {
 	int	status = STATUS_SUCCESS;
 	unsigned int	uiRegRead = 0;
@@ -147,7 +147,7 @@ int InterfaceIdleModeRespond(PMINI_ADAPTER Adapter, unsigned int* puiBuffer)
 	return status;
 }
 
-static int InterfaceAbortIdlemode(PMINI_ADAPTER Adapter, unsigned int Pattern)
+static int InterfaceAbortIdlemode(struct bcm_mini_adapter *Adapter, unsigned int Pattern)
 {
 	int 	status = STATUS_SUCCESS;
 	unsigned int value;
@@ -246,7 +246,7 @@ static int InterfaceAbortIdlemode(PMINI_ADAPTER Adapter, unsigned int Pattern)
 	}
 	return status;
 }
-int InterfaceIdleModeWakeup(PMINI_ADAPTER Adapter)
+int InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter)
 {
 	ULONG	Status = 0;
 	if(Adapter->bTriedToWakeUpFromlowPowerMode)
@@ -263,7 +263,7 @@ int InterfaceIdleModeWakeup(PMINI_ADAPTER Adapter)
 	return Status;
 }
 
-void InterfaceHandleShutdownModeWakeup(PMINI_ADAPTER Adapter)
+void InterfaceHandleShutdownModeWakeup(struct bcm_mini_adapter *Adapter)
 {
 	unsigned int uiRegVal = 0;
 	INT Status = 0;
diff --git a/drivers/staging/bcm/InterfaceIdleMode.h b/drivers/staging/bcm/InterfaceIdleMode.h
index 859a2ffba6b..c3338c8a1dc 100644
--- a/drivers/staging/bcm/InterfaceIdleMode.h
+++ b/drivers/staging/bcm/InterfaceIdleMode.h
@@ -1,14 +1,14 @@
 #ifndef _INTERFACE_IDLEMODE_H
 #define _INTERFACE_IDLEMODE_H
 
-INT InterfaceIdleModeWakeup(PMINI_ADAPTER Adapter);
+INT InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter);
 
-INT InterfaceIdleModeRespond(PMINI_ADAPTER Adapter, unsigned int *puiBuffer);
+INT InterfaceIdleModeRespond(struct bcm_mini_adapter *Adapter, unsigned int *puiBuffer);
 
-VOID InterfaceWriteIdleModeWakePattern(PMINI_ADAPTER Adapter);
+VOID InterfaceWriteIdleModeWakePattern(struct bcm_mini_adapter *Adapter);
 
-INT InterfaceWakeUp(PMINI_ADAPTER Adapter);
+INT InterfaceWakeUp(struct bcm_mini_adapter * Adapter);
 
-VOID InterfaceHandleShutdownModeWakeup(PMINI_ADAPTER Adapter);
+VOID InterfaceHandleShutdownModeWakeup(struct bcm_mini_adapter *Adapter);
 #endif
 
diff --git a/drivers/staging/bcm/InterfaceInit.c b/drivers/staging/bcm/InterfaceInit.c
index 8e3c586a699..8f85de6a57b 100644
--- a/drivers/staging/bcm/InterfaceInit.c
+++ b/drivers/staging/bcm/InterfaceInit.c
@@ -65,7 +65,7 @@ static void InterfaceAdapterFree(PS_INTERFACE_ADAPTER psIntfAdapter)
 	AdapterFree(psIntfAdapter->psAdapter);
 }
 
-static void ConfigureEndPointTypesThroughEEPROM(PMINI_ADAPTER Adapter)
+static void ConfigureEndPointTypesThroughEEPROM(struct bcm_mini_adapter *Adapter)
 {
 	unsigned long ulReg = 0;
 	int bytes;
@@ -143,12 +143,12 @@ static int usbbcm_device_probe(struct usb_interface *intf, const struct usb_devi
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
 	int retval;
-	PMINI_ADAPTER psAdapter;
+	struct bcm_mini_adapter *psAdapter;
 	PS_INTERFACE_ADAPTER psIntfAdapter;
 	struct net_device *ndev;
 
 	/* Reserve one extra queue for the bit-bucket */
-	ndev = alloc_etherdev_mq(sizeof(MINI_ADAPTER), NO_OF_QUEUES+1);
+	ndev = alloc_etherdev_mq(sizeof(struct bcm_mini_adapter), NO_OF_QUEUES+1);
 	if (ndev == NULL) {
 		dev_err(&udev->dev, DRV_NAME ": no memory for device\n");
 		return -ENOMEM;
@@ -257,7 +257,7 @@ static int usbbcm_device_probe(struct usb_interface *intf, const struct usb_devi
 static void usbbcm_disconnect(struct usb_interface *intf)
 {
 	PS_INTERFACE_ADAPTER psIntfAdapter = usb_get_intfdata(intf);
-	PMINI_ADAPTER psAdapter;
+	struct bcm_mini_adapter *psAdapter;
 	struct usb_device  *udev = interface_to_usbdev(intf);
 
 	if (psIntfAdapter == NULL)
diff --git a/drivers/staging/bcm/InterfaceIsr.c b/drivers/staging/bcm/InterfaceIsr.c
index 67719d57256..6ee3428daa5 100644
--- a/drivers/staging/bcm/InterfaceIsr.c
+++ b/drivers/staging/bcm/InterfaceIsr.c
@@ -5,7 +5,7 @@ static void read_int_callback(struct urb *urb/*, struct pt_regs *regs*/)
 {
 	int		status = urb->status;
 	PS_INTERFACE_ADAPTER psIntfAdapter = (PS_INTERFACE_ADAPTER)urb->context;
-	PMINI_ADAPTER Adapter = psIntfAdapter->psAdapter ;
+	struct bcm_mini_adapter *Adapter = psIntfAdapter->psAdapter ;
 
 	if (netif_msg_intr(Adapter))
 		pr_info(PFX "%s: interrupt status %d\n",
diff --git a/drivers/staging/bcm/InterfaceIsr.h b/drivers/staging/bcm/InterfaceIsr.h
index 6065a7141bc..40399788c41 100644
--- a/drivers/staging/bcm/InterfaceIsr.h
+++ b/drivers/staging/bcm/InterfaceIsr.h
@@ -7,9 +7,9 @@ int CreateInterruptUrb(PS_INTERFACE_ADAPTER psIntfAdapter);
 INT StartInterruptUrb(PS_INTERFACE_ADAPTER psIntfAdapter);
 
 
-VOID InterfaceEnableInterrupt(PMINI_ADAPTER Adapter);
+VOID InterfaceEnableInterrupt(struct bcm_mini_adapter *Adapter);
 
-VOID InterfaceDisableInterrupt(PMINI_ADAPTER Adapter);
+VOID InterfaceDisableInterrupt(struct bcm_mini_adapter *Adapter);
 
 #endif
 
diff --git a/drivers/staging/bcm/InterfaceMisc.c b/drivers/staging/bcm/InterfaceMisc.c
index 2218faeaf8a..bbe90994609 100644
--- a/drivers/staging/bcm/InterfaceMisc.c
+++ b/drivers/staging/bcm/InterfaceMisc.c
@@ -133,7 +133,7 @@ INT BcmWRM(PVOID arg,
 	return InterfaceWRM((PS_INTERFACE_ADAPTER)arg, addr, buff, len);
 }
 
-INT Bcm_clear_halt_of_endpoints(PMINI_ADAPTER Adapter)
+INT Bcm_clear_halt_of_endpoints(struct bcm_mini_adapter *Adapter)
 {
 	PS_INTERFACE_ADAPTER psIntfAdapter = (PS_INTERFACE_ADAPTER)(Adapter->pvInterfaceAdapter);
 	INT status = STATUS_SUCCESS;
diff --git a/drivers/staging/bcm/InterfaceMisc.h b/drivers/staging/bcm/InterfaceMisc.h
index 6c9e39bf988..1dfabdc3aad 100644
--- a/drivers/staging/bcm/InterfaceMisc.h
+++ b/drivers/staging/bcm/InterfaceMisc.h
@@ -33,7 +33,7 @@ int BcmWRM(PVOID arg,
 			PVOID buff,
 			INT len);
 
-INT Bcm_clear_halt_of_endpoints(PMINI_ADAPTER Adapter);
+INT Bcm_clear_halt_of_endpoints(struct bcm_mini_adapter *Adapter);
 
 VOID Bcm_kill_all_URBs(PS_INTERFACE_ADAPTER psIntfAdapter);
 
diff --git a/drivers/staging/bcm/InterfaceRx.c b/drivers/staging/bcm/InterfaceRx.c
index d495828a731..8a9f90fbdf1 100644
--- a/drivers/staging/bcm/InterfaceRx.c
+++ b/drivers/staging/bcm/InterfaceRx.c
@@ -1,6 +1,6 @@
 #include "headers.h"
 
-static int SearchVcid(PMINI_ADAPTER Adapter,unsigned short usVcid)
+static int SearchVcid(struct bcm_mini_adapter *Adapter,unsigned short usVcid)
 {
 	int iIndex=0;
 
@@ -45,8 +45,8 @@ static void read_bulk_callback(struct urb *urb)
 	//int idleflag = 0 ;
 	PUSB_RCB pRcb = (PUSB_RCB)urb->context;
 	PS_INTERFACE_ADAPTER psIntfAdapter = pRcb->psIntfAdapter;
-	PMINI_ADAPTER Adapter = psIntfAdapter->psAdapter;
-	PLEADER pLeader = urb->transfer_buffer;
+	struct bcm_mini_adapter *Adapter = psIntfAdapter->psAdapter;
+	struct bcm_leader *pLeader = urb->transfer_buffer;
 
 	if (unlikely(netif_msg_rx_status(Adapter)))
 		pr_info(PFX "%s: rx urb status %d length %d\n",
@@ -126,7 +126,7 @@ static void read_bulk_callback(struct urb *urb)
 	    BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,DBG_TYPE_RX, RX_CTRL, DBG_LVL_ALL, "Received control pkt...");
 		*(PUSHORT)skb->data = pLeader->Status;
        	memcpy(skb->data+sizeof(USHORT), urb->transfer_buffer +
-			(sizeof(LEADER)), pLeader->PLength);
+			(sizeof(struct bcm_leader)), pLeader->PLength);
 		skb->len = pLeader->PLength + sizeof(USHORT);
 
 		spin_lock(&Adapter->control_queue_lock);
@@ -144,7 +144,7 @@ static void read_bulk_callback(struct urb *urb)
 		  */
         BCM_DEBUG_PRINT(psIntfAdapter->psAdapter,DBG_TYPE_RX, RX_DATA, DBG_LVL_ALL, "Received Data pkt...");
 		skb_reserve(skb, 2 + SKB_RESERVE_PHS_BYTES);
-		memcpy(skb->data+ETH_HLEN, (PUCHAR)urb->transfer_buffer + sizeof(LEADER), pLeader->PLength);
+		memcpy(skb->data+ETH_HLEN, (PUCHAR)urb->transfer_buffer + sizeof(struct bcm_leader), pLeader->PLength);
 		skb->dev = Adapter->dev;
 
 		/* currently skb->len has extra ETH_HLEN bytes in the beginning */
@@ -232,7 +232,7 @@ Function:				InterfaceRx
 Description:			This is the hardware specific Function for Receiving
 						data packet/control packets from the device.
 
-Input parameters:		IN PMINI_ADAPTER Adapter   - Miniport Adapter Context
+Input parameters:		IN struct bcm_mini_adapter *Adapter   - Miniport Adapter Context
 
 
 
diff --git a/drivers/staging/bcm/InterfaceTx.c b/drivers/staging/bcm/InterfaceTx.c
index a842de9de6b..7e2b53be4d9 100644
--- a/drivers/staging/bcm/InterfaceTx.c
+++ b/drivers/staging/bcm/InterfaceTx.c
@@ -5,10 +5,10 @@ static void write_bulk_callback(struct urb *urb/*, struct pt_regs *regs*/)
 {
 	PUSB_TCB pTcb= (PUSB_TCB)urb->context;
 	PS_INTERFACE_ADAPTER psIntfAdapter = pTcb->psIntfAdapter;
-	CONTROL_MESSAGE *pControlMsg = (CONTROL_MESSAGE *)urb->transfer_buffer;
-	PMINI_ADAPTER psAdapter = psIntfAdapter->psAdapter ;
+	struct bcm_link_request *pControlMsg = (struct bcm_link_request *)urb->transfer_buffer;
+	struct bcm_mini_adapter *psAdapter = psIntfAdapter->psAdapter ;
 	BOOLEAN bpowerDownMsg = FALSE ;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
     if (unlikely(netif_msg_tx_done(Adapter)))
 	    pr_info(PFX "%s: transmit status %d\n", Adapter->dev->name, urb->status);
diff --git a/drivers/staging/bcm/LeakyBucket.c b/drivers/staging/bcm/LeakyBucket.c
index a55d4228e8e..6e8a3279698 100644
--- a/drivers/staging/bcm/LeakyBucket.c
+++ b/drivers/staging/bcm/LeakyBucket.c
@@ -15,7 +15,7 @@
 * Returns     - None
 **********************************************************************/
 
-static VOID UpdateTokenCount(register PMINI_ADAPTER Adapter)
+static VOID UpdateTokenCount(register struct bcm_mini_adapter *Adapter)
 {
 	ULONG 	liCurrentTime;
 	INT 	i = 0;
@@ -75,7 +75,7 @@ static VOID UpdateTokenCount(register PMINI_ADAPTER Adapter)
 * Returns     - The number of bytes allowed for transmission.
 *
 ***********************************************************************/
-static ULONG GetSFTokenCount(PMINI_ADAPTER Adapter, PacketInfo *psSF)
+static ULONG GetSFTokenCount(struct bcm_mini_adapter *Adapter, struct bcm_packet_info *psSF)
 {
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL, "IsPacketAllowedForFlow ===>");
 	/* Validate the parameters */
@@ -112,8 +112,8 @@ static ULONG GetSFTokenCount(PMINI_ADAPTER Adapter, PacketInfo *psSF)
 This function despatches packet from the specified queue.
 @return Zero(success) or Negative value(failure)
 */
-static INT SendPacketFromQueue(PMINI_ADAPTER Adapter,/**<Logical Adapter*/
-			       PacketInfo *psSF,		/**<Queue identifier*/
+static INT SendPacketFromQueue(struct bcm_mini_adapter *Adapter,/**<Logical Adapter*/
+			struct bcm_packet_info *psSF, /**<Queue identifier*/
 			       struct sk_buff*  Packet)	/**<Pointer to the packet to be sent*/
 {
 	INT  	Status=STATUS_FAILURE;
@@ -156,7 +156,7 @@ static INT SendPacketFromQueue(PMINI_ADAPTER Adapter,/**<Logical Adapter*/
 * Returns     - None.
 *
 ****************************************************************************/
-static VOID CheckAndSendPacketFromIndex(PMINI_ADAPTER Adapter, PacketInfo *psSF)
+static VOID CheckAndSendPacketFromIndex(struct bcm_mini_adapter *Adapter, struct bcm_packet_info *psSF)
 {
 	struct sk_buff	*QueuePacket=NULL;
 	char 			*pControlPacket = NULL;
@@ -243,10 +243,10 @@ static VOID CheckAndSendPacketFromIndex(PMINI_ADAPTER Adapter, PacketInfo *psSF)
 				{
 					spin_lock_bh(&psSF->SFQueueLock);
 					psSF->NumOfPacketsSent++;
-					psSF->uiSentBytes+=((PLEADER)pControlPacket)->PLength;
+					psSF->uiSentBytes+=((struct bcm_leader *)pControlPacket)->PLength;
 					psSF->uiSentPackets++;
 					atomic_dec(&Adapter->TotalPacketCount);
-					psSF->uiCurrentBytesOnHost -= ((PLEADER)pControlPacket)->PLength;
+					psSF->uiCurrentBytesOnHost -= ((struct bcm_leader *)pControlPacket)->PLength;
 					psSF->uiCurrentPacketsOnHost--;
 					atomic_inc(&Adapter->index_rd_txcntrlpkt);
 					spin_unlock_bh(&psSF->SFQueueLock);
@@ -273,7 +273,7 @@ static VOID CheckAndSendPacketFromIndex(PMINI_ADAPTER Adapter, PacketInfo *psSF)
 *
 * Returns     - None.
 ********************************************************************/
-VOID transmit_packets(PMINI_ADAPTER Adapter)
+VOID transmit_packets(struct bcm_mini_adapter *Adapter)
 {
 	UINT 	uiPrevTotalCount = 0;
 	int iIndex = 0;
diff --git a/drivers/staging/bcm/Macros.h b/drivers/staging/bcm/Macros.h
index 46ed99c5376..46f5f0feea8 100644
--- a/drivers/staging/bcm/Macros.h
+++ b/drivers/staging/bcm/Macros.h
@@ -6,7 +6,6 @@
 
 #define TX_TIMER_PERIOD 10 /*10 msec*/
 #define MAX_CLASSIFIERS 100
-/* #define MAX_CLASSIFIERS_PER_SF  20 */
 #define MAX_TARGET_DSX_BUFFERS 24
 
 #define MAX_CNTRL_PKTS    100
@@ -108,11 +107,11 @@
 /*Leader related terms */
 #define LEADER_STATUS         0x00
 #define LEADER_STATUS_TCP_ACK 0x1
-#define LEADER_SIZE           sizeof(LEADER)
-#define MAC_ADDR_REQ_SIZE     sizeof(PACKETTOSEND)
-#define SS_INFO_REQ_SIZE      sizeof(PACKETTOSEND)
+#define LEADER_SIZE           sizeof(struct bcm_leader)
+#define MAC_ADDR_REQ_SIZE     sizeof(struct bcm_packettosend)
+#define SS_INFO_REQ_SIZE      sizeof(struct bcm_packettosend)
 #define CM_REQUEST_SIZE       (LEADER_SIZE + sizeof(stLocalSFChangeRequest))
-#define IDLE_REQ_SIZE         sizeof(PACKETTOSEND)
+#define IDLE_REQ_SIZE         sizeof(struct bcm_packettosend)
 
 
 #define MAX_TRANSFER_CTRL_BYTE_USB (2*1024)
@@ -252,11 +251,7 @@ typedef enum _E_PHS_DSC_ACTION {
 #define IDLE_MODE_WAKEUP_NOTIFIER_ADDRESS 0x1FC02FA8
 #define IDLE_MODE_MAX_RETRY_COUNT 1000
 
-#ifdef REL_4_1
-#define CONFIG_BEGIN_ADDR 0xBF60B004
-#else
 #define CONFIG_BEGIN_ADDR 0xBF60B000
-#endif
 
 #define FIRMWARE_BEGIN_ADDR 0xBFC00000
 
diff --git a/drivers/staging/bcm/Misc.c b/drivers/staging/bcm/Misc.c
index 8223a6913fc..f545716c666 100644
--- a/drivers/staging/bcm/Misc.c
+++ b/drivers/staging/bcm/Misc.c
@@ -1,12 +1,12 @@
 #include "headers.h"
 
-static int BcmFileDownload(PMINI_ADAPTER Adapter, const char *path, unsigned int loc);
-static VOID doPowerAutoCorrection(PMINI_ADAPTER psAdapter);
-static void HandleShutDownModeRequest(PMINI_ADAPTER Adapter, PUCHAR pucBuffer);
-static int bcm_parse_target_params(PMINI_ADAPTER Adapter);
-static void beceem_protocol_reset(PMINI_ADAPTER Adapter);
+static int BcmFileDownload(struct bcm_mini_adapter *Adapter, const char *path, unsigned int loc);
+static VOID doPowerAutoCorrection(struct bcm_mini_adapter *psAdapter);
+static void HandleShutDownModeRequest(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer);
+static int bcm_parse_target_params(struct bcm_mini_adapter *Adapter);
+static void beceem_protocol_reset(struct bcm_mini_adapter *Adapter);
 
-static VOID default_wimax_protocol_initialize(PMINI_ADAPTER Adapter)
+static VOID default_wimax_protocol_initialize(struct bcm_mini_adapter *Adapter)
 {
 	UINT uiLoopIndex;
 
@@ -24,7 +24,7 @@ static VOID default_wimax_protocol_initialize(PMINI_ADAPTER Adapter)
 	return;
 }
 
-INT InitAdapter(PMINI_ADAPTER psAdapter)
+INT InitAdapter(struct bcm_mini_adapter *psAdapter)
 {
 	int i = 0;
 	INT Status = STATUS_SUCCESS;
@@ -93,7 +93,7 @@ INT InitAdapter(PMINI_ADAPTER psAdapter)
 	return STATUS_SUCCESS;
 }
 
-VOID AdapterFree(PMINI_ADAPTER Adapter)
+VOID AdapterFree(struct bcm_mini_adapter *Adapter)
 {
 	int count;
 	beceem_protocol_reset(Adapter);
@@ -134,7 +134,7 @@ VOID AdapterFree(PMINI_ADAPTER Adapter)
 	free_netdev(Adapter->dev);
 }
 
-static int create_worker_threads(PMINI_ADAPTER psAdapter)
+static int create_worker_threads(struct bcm_mini_adapter *psAdapter)
 {
 	/* Rx Control Packets Processing */
 	psAdapter->control_packet_handler = kthread_run((int (*)(void *))
@@ -155,14 +155,9 @@ static int create_worker_threads(PMINI_ADAPTER psAdapter)
 	return 0;
 }
 
-static struct file *open_firmware_file(PMINI_ADAPTER Adapter, const char *path)
+static struct file *open_firmware_file(struct bcm_mini_adapter *Adapter, const char *path)
 {
-	struct file *flp = NULL;
-	mm_segment_t oldfs;
-	oldfs = get_fs();
-	set_fs(get_ds());
-	flp = filp_open(path, O_RDONLY, S_IRWXU);
-	set_fs(oldfs);
+	struct file *flp = filp_open(path, O_RDONLY, S_IRWXU);
 	if (IS_ERR(flp)) {
 		pr_err(DRV_NAME "Unable To Open File %s, err %ld", path, PTR_ERR(flp));
 		flp = NULL;
@@ -179,18 +174,16 @@ static struct file *open_firmware_file(PMINI_ADAPTER Adapter, const char *path)
  * Path to image file
  * Download Address on the chip
  */
-static int BcmFileDownload(PMINI_ADAPTER Adapter, const char *path, unsigned int loc)
+static int BcmFileDownload(struct bcm_mini_adapter *Adapter, const char *path, unsigned int loc)
 {
 	int errorno = 0;
 	struct file *flp = NULL;
-	mm_segment_t oldfs;
 	struct timeval tv = {0};
 
 	flp = open_firmware_file(Adapter, path);
 	if (!flp) {
-		errorno = -ENOENT;
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Unable to Open %s\n", path);
-		goto exit_download;
+		return -ENOENT;
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Opened file is = %s and length =0x%lx to be downloaded at =0x%x", path, (unsigned long)flp->f_dentry->d_inode->i_size, loc);
 	do_gettimeofday(&tv);
@@ -201,10 +194,7 @@ static int BcmFileDownload(PMINI_ADAPTER Adapter, const char *path, unsigned int
 		errorno = -EIO;
 		goto exit_download;
 	}
-	oldfs = get_fs();
-	set_fs(get_ds());
 	vfs_llseek(flp, 0, 0);
-	set_fs(oldfs);
 	if (Adapter->bcm_file_readback_from_chip(Adapter->pvInterfaceAdapter, flp, loc)) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Failed to read back firmware!");
 		errorno = -EIO;
@@ -212,12 +202,7 @@ static int BcmFileDownload(PMINI_ADAPTER Adapter, const char *path, unsigned int
 	}
 
 exit_download:
-	oldfs = get_fs();
-	set_fs(get_ds());
-	if (flp && !(IS_ERR(flp)))
-		filp_close(flp, current->files);
-	set_fs(oldfs);
-
+	filp_close(flp, NULL);
 	return errorno;
 }
 
@@ -231,13 +216,13 @@ exit_download:
  * Logical Adapter
  * Control Packet Buffer
  */
-INT CopyBufferToControlPacket(PMINI_ADAPTER Adapter, PVOID ioBuffer)
+INT CopyBufferToControlPacket(struct bcm_mini_adapter *Adapter, PVOID ioBuffer)
 {
-	PLEADER	pLeader = NULL;
+	struct bcm_leader *pLeader = NULL;
 	INT Status = 0;
 	unsigned char *ctrl_buff = NULL;
 	UINT pktlen = 0;
-	PLINK_REQUEST pLinkReq = NULL;
+	struct bcm_link_request *pLinkReq = NULL;
 	PUCHAR pucAddIndication = NULL;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "======>");
@@ -246,8 +231,8 @@ INT CopyBufferToControlPacket(PMINI_ADAPTER Adapter, PVOID ioBuffer)
 		return -EINVAL;
 	}
 
-	pLinkReq = (PLINK_REQUEST)ioBuffer;
-	pLeader = (PLEADER)ioBuffer; /* ioBuffer Contains sw_Status and Payload */
+	pLinkReq = (struct bcm_link_request *)ioBuffer;
+	pLeader = (struct bcm_leader *)ioBuffer; /* ioBuffer Contains sw_Status and Payload */
 
 	if (Adapter->bShutStatus == TRUE &&
 		pLinkReq->szData[0] == LINK_DOWN_REQ_PAYLOAD &&
@@ -373,7 +358,7 @@ INT CopyBufferToControlPacket(PMINI_ADAPTER Adapter, PVOID ioBuffer)
 
 		memset(ctrl_buff, 0, pktlen+LEADER_SIZE);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Copying the Control Packet Buffer with length=%d\n", pLeader->PLength);
-		*(PLEADER)ctrl_buff = *pLeader;
+		*(struct bcm_leader *)ctrl_buff = *pLeader;
 		memcpy(ctrl_buff + LEADER_SIZE, ((PUCHAR)ioBuffer + LEADER_SIZE), pLeader->PLength);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Enqueuing the Control Packet");
 
@@ -402,30 +387,6 @@ INT CopyBufferToControlPacket(PMINI_ADAPTER Adapter, PVOID ioBuffer)
 	return Status;
 }
 
-#if 0
-/*****************************************************************
-* Function    - SendStatisticsPointerRequest()
-*
-* Description - This function builds and forwards the Statistics
-* Pointer Request control Packet.
-*
-* Parameters  - Adapter					: Pointer to Adapter structure.
-* - pstStatisticsPtrRequest : Pointer to link request.
-*
-* Returns     - None.
-*****************************************************************/
-static VOID SendStatisticsPointerRequest(PMINI_ADAPTER Adapter, PLINK_REQUEST pstStatisticsPtrRequest)
-{
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "======>");
-	pstStatisticsPtrRequest->Leader.Status = STATS_POINTER_REQ_STATUS;
-	pstStatisticsPtrRequest->Leader.PLength = sizeof(ULONG); /* minimum 4 bytes */
-	pstStatisticsPtrRequest->szData[0] = STATISTICS_POINTER_REQ;
-	CopyBufferToControlPacket(Adapter, pstStatisticsPtrRequest);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "<=====");
-	return;
-}
-#endif
-
 /******************************************************************
 * Function    - LinkMessage()
 *
@@ -436,12 +397,12 @@ static VOID SendStatisticsPointerRequest(PMINI_ADAPTER Adapter, PLINK_REQUEST ps
 *
 * Returns     - None.
 *******************************************************************/
-VOID LinkMessage(PMINI_ADAPTER Adapter)
+VOID LinkMessage(struct bcm_mini_adapter *Adapter)
 {
-	PLINK_REQUEST pstLinkRequest = NULL;
+	struct bcm_link_request *pstLinkRequest = NULL;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "=====>");
 	if (Adapter->LinkStatus == SYNC_UP_REQUEST && Adapter->AutoSyncup) {
-		pstLinkRequest = kzalloc(sizeof(LINK_REQUEST), GFP_ATOMIC);
+		pstLinkRequest = kzalloc(sizeof(struct bcm_link_request), GFP_ATOMIC);
 		if (!pstLinkRequest) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Can not allocate memory for Link request!");
 			return;
@@ -456,7 +417,7 @@ VOID LinkMessage(PMINI_ADAPTER Adapter)
 		Adapter->bSyncUpRequestSent = TRUE;
 
 	} else if (Adapter->LinkStatus == PHY_SYNC_ACHIVED && Adapter->AutoLinkUp) {
-		pstLinkRequest = kzalloc(sizeof(LINK_REQUEST), GFP_ATOMIC);
+		pstLinkRequest = kzalloc(sizeof(struct bcm_link_request), GFP_ATOMIC);
 		if (!pstLinkRequest) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LINK_UP_MSG, DBG_LVL_ALL, "Can not allocate memory for Link request!");
 			return;
@@ -487,7 +448,7 @@ VOID LinkMessage(PMINI_ADAPTER Adapter)
 *
 * Returns     - None.
 ************************************************************************/
-VOID StatisticsResponse(PMINI_ADAPTER Adapter, PVOID pvBuffer)
+VOID StatisticsResponse(struct bcm_mini_adapter *Adapter, PVOID pvBuffer)
 {
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "%s====>", __func__);
 	Adapter->StatisticsPointer = ntohl(*(__be32 *)pvBuffer);
@@ -506,7 +467,7 @@ VOID StatisticsResponse(PMINI_ADAPTER Adapter, PVOID pvBuffer)
 *
 * Returns     - None.
 ***********************************************************************/
-VOID LinkControlResponseMessage(PMINI_ADAPTER Adapter, PUCHAR pucBuffer)
+VOID LinkControlResponseMessage(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer)
 {
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_RX, RX_DPC, DBG_LVL_ALL, "=====>");
 
@@ -580,11 +541,11 @@ VOID LinkControlResponseMessage(PMINI_ADAPTER Adapter, PUCHAR pucBuffer)
 	return;
 }
 
-void SendIdleModeResponse(PMINI_ADAPTER Adapter)
+void SendIdleModeResponse(struct bcm_mini_adapter *Adapter)
 {
 	INT status = 0, NVMAccess = 0, lowPwrAbortMsg = 0;
 	struct timeval tv;
-	CONTROL_MESSAGE stIdleResponse = {{0} };
+	struct bcm_link_request stIdleResponse = {{0} };
 	memset(&tv, 0, sizeof(tv));
 	stIdleResponse.Leader.Status = IDLE_MESSAGE;
 	stIdleResponse.Leader.PLength = IDLE_MODE_PAYLOAD_LENGTH;
@@ -679,12 +640,12 @@ void SendIdleModeResponse(PMINI_ADAPTER Adapter)
 *
 * Returns     - None.
 *******************************************************************/
-VOID DumpPackInfo(PMINI_ADAPTER Adapter)
+VOID DumpPackInfo(struct bcm_mini_adapter *Adapter)
 {
 	UINT uiLoopIndex = 0;
 	UINT uiIndex = 0;
 	UINT uiClsfrIndex = 0;
-	S_CLASSIFIER_RULE *pstClassifierEntry = NULL;
+	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 
 	for (uiLoopIndex = 0; uiLoopIndex < NO_OF_QUEUES; uiLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "*********** Showing Details Of Queue %d***** ******", uiLoopIndex);
@@ -808,10 +769,10 @@ VOID DumpPackInfo(PMINI_ADAPTER Adapter)
 	return;
 }
 
-int reset_card_proc(PMINI_ADAPTER ps_adapter)
+int reset_card_proc(struct bcm_mini_adapter *ps_adapter)
 {
 	int retval = STATUS_SUCCESS;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	PS_INTERFACE_ADAPTER psIntfAdapter = NULL;
 	unsigned int value = 0, uiResetValue = 0;
 	int bytes;
@@ -926,7 +887,7 @@ err_exit:
 	return retval;
 }
 
-int run_card_proc(PMINI_ADAPTER ps_adapter)
+int run_card_proc(struct bcm_mini_adapter *ps_adapter)
 {
 	int status = STATUS_SUCCESS;
 	int bytes;
@@ -953,7 +914,7 @@ int run_card_proc(PMINI_ADAPTER ps_adapter)
 	return status;
 }
 
-int InitCardAndDownloadFirmware(PMINI_ADAPTER ps_adapter)
+int InitCardAndDownloadFirmware(struct bcm_mini_adapter *ps_adapter)
 {
 	int status;
 	UINT value = 0;
@@ -1077,13 +1038,11 @@ OUT:
 	return status;
 }
 
-static int bcm_parse_target_params(PMINI_ADAPTER Adapter)
+static int bcm_parse_target_params(struct bcm_mini_adapter *Adapter)
 {
 	struct file *flp = NULL;
-	mm_segment_t oldfs = {0};
 	char *buff;
 	int len = 0;
-	loff_t pos = 0;
 
 	buff = kmalloc(BUFFER_1K, GFP_KERNEL);
 	if (!buff)
@@ -1103,20 +1062,16 @@ static int bcm_parse_target_params(PMINI_ADAPTER Adapter)
 		Adapter->pstargetparams = NULL;
 		return -ENOENT;
 	}
-	oldfs = get_fs();
-	set_fs(get_ds());
-	len = vfs_read(flp, (void __user __force *)buff, BUFFER_1K, &pos);
-	set_fs(oldfs);
+	len = kernel_read(flp, 0, buff, BUFFER_1K);
+	filp_close(flp, NULL);
 
 	if (len != sizeof(STARGETPARAMS)) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "Mismatch in Target Param Structure!\n");
 		kfree(buff);
 		kfree(Adapter->pstargetparams);
 		Adapter->pstargetparams = NULL;
-		filp_close(flp, current->files);
 		return -ENOENT;
 	}
-	filp_close(flp, current->files);
 
 	/* Check for autolink in config params */
 	/*
@@ -1128,7 +1083,7 @@ static int bcm_parse_target_params(PMINI_ADAPTER Adapter)
 	return STATUS_SUCCESS;
 }
 
-void beceem_parse_target_struct(PMINI_ADAPTER Adapter)
+void beceem_parse_target_struct(struct bcm_mini_adapter *Adapter)
 {
 	UINT uiHostDrvrCfg6 = 0, uiEEPROMFlag = 0;
 
@@ -1186,7 +1141,7 @@ void beceem_parse_target_struct(PMINI_ADAPTER Adapter)
 		doPowerAutoCorrection(Adapter);
 }
 
-static VOID doPowerAutoCorrection(PMINI_ADAPTER psAdapter)
+static VOID doPowerAutoCorrection(struct bcm_mini_adapter *psAdapter)
 {
 	UINT reporting_mode;
 
@@ -1217,45 +1172,6 @@ static VOID doPowerAutoCorrection(PMINI_ADAPTER psAdapter)
 	}
 }
 
-#if 0
-static unsigned char *ReadMacAddrEEPROM(PMINI_ADAPTER Adapter, ulong dwAddress)
-{
-	int status = 0, i = 0;
-	unsigned int temp = 0;
-	unsigned char *pucmacaddr = kmalloc(MAC_ADDRESS_SIZE, GFP_KERNEL);
-	int bytes;
-
-	if (!pucmacaddr) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "No Buffers to Read the EEPROM Address\n");
-		return NULL;
-	}
-
-	dwAddress |= 0x5b000000;
-	status = wrmalt(Adapter, EEPROM_COMMAND_Q_REG, (PUINT)&dwAddress, sizeof(UINT));
-	if (status != STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "wrm Failed..\n");
-		kfree(pucmacaddr);
-		pucmacaddr = NULL;
-		goto OUT;
-	}
-
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
-		bytes = rdmalt(Adapter, EEPROM_READ_DATA_Q_REG, &temp, sizeof(temp));
-		if (bytes < 0) {
-			status = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "rdm Failed..\n");
-			kfree(pucmacaddr);
-			pucmacaddr = NULL;
-			goto OUT;
-		}
-		pucmacaddr[i] = temp & 0xff;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL, "%x\n", pucmacaddr[i]);
-	}
-OUT:
-	return pucmacaddr;
-}
-#endif
-
 static void convertEndian(B_UINT8 rwFlag, PUINT puiBuffer, UINT uiByteCount)
 {
 	UINT uiIndex = 0;
@@ -1269,16 +1185,13 @@ static void convertEndian(B_UINT8 rwFlag, PUINT puiBuffer, UINT uiByteCount)
 	}
 }
 
-#define CACHE_ADDRESS_MASK 0x80000000
-#define UNCACHE_ADDRESS_MASK 0xa0000000
-
-int rdm(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
+int rdm(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
 {
 	return Adapter->interface_rdm(Adapter->pvInterfaceAdapter,
 				uiAddress, pucBuff, sSize);
 }
 
-int wrm(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
+int wrm(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
 {
 	int iRetVal;
 
@@ -1287,13 +1200,13 @@ int wrm(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
 	return iRetVal;
 }
 
-int wrmalt(PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
+int wrmalt(struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
 {
 	convertEndian(RWM_WRITE, pucBuff, size);
 	return wrm(Adapter, uiAddress, (PUCHAR)pucBuff, size);
 }
 
-int rdmalt(PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
+int rdmalt(struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
 {
 	INT uiRetVal = 0;
 
@@ -1303,7 +1216,7 @@ int rdmalt(PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
 	return uiRetVal;
 }
 
-int wrmWithLock(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
+int wrmWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t sSize)
 {
 	INT status = STATUS_SUCCESS;
 	down(&Adapter->rdmwrmsync);
@@ -1322,7 +1235,7 @@ exit:
 	return status;
 }
 
-int wrmaltWithLock(PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
+int wrmaltWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
 {
 	int iRetVal = STATUS_SUCCESS;
 
@@ -1342,7 +1255,7 @@ exit:
 	return iRetVal;
 }
 
-int rdmaltWithLock(PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
+int rdmaltWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t size)
 {
 	INT uiRetVal = STATUS_SUCCESS;
 
@@ -1361,7 +1274,7 @@ exit:
 	return uiRetVal;
 }
 
-static VOID HandleShutDownModeWakeup(PMINI_ADAPTER Adapter)
+static VOID HandleShutDownModeWakeup(struct bcm_mini_adapter *Adapter)
 {
 	int clear_abort_pattern = 0, Status = 0;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "====>\n");
@@ -1390,13 +1303,13 @@ static VOID HandleShutDownModeWakeup(PMINI_ADAPTER Adapter)
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, MP_SHUTDOWN, DBG_LVL_ALL, "<====\n");
 }
 
-static VOID SendShutModeResponse(PMINI_ADAPTER Adapter)
+static VOID SendShutModeResponse(struct bcm_mini_adapter *Adapter)
 {
-	CONTROL_MESSAGE stShutdownResponse;
+	struct bcm_link_request stShutdownResponse;
 	UINT NVMAccess = 0, lowPwrAbortMsg = 0;
 	UINT Status = 0;
 
-	memset(&stShutdownResponse, 0, sizeof(CONTROL_MESSAGE));
+	memset(&stShutdownResponse, 0, sizeof(struct bcm_link_request));
 	stShutdownResponse.Leader.Status  = LINK_UP_CONTROL_REQ;
 	stShutdownResponse.Leader.PLength = 8; /* 8 bytes; */
 	stShutdownResponse.szData[0] = LINK_UP_ACK;
@@ -1474,7 +1387,7 @@ static VOID SendShutModeResponse(PMINI_ADAPTER Adapter)
 	}
 }
 
-static void HandleShutDownModeRequest(PMINI_ADAPTER Adapter, PUCHAR pucBuffer)
+static void HandleShutDownModeRequest(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer)
 {
 	B_UINT32 uiResetValue = 0;
 
@@ -1503,7 +1416,7 @@ static void HandleShutDownModeRequest(PMINI_ADAPTER Adapter, PUCHAR pucBuffer)
 	return;
 }
 
-VOID ResetCounters(PMINI_ADAPTER Adapter)
+VOID ResetCounters(struct bcm_mini_adapter *Adapter)
 {
 	beceem_protocol_reset(Adapter);
 	Adapter->CurrNumRecvDescs = 0;
@@ -1519,7 +1432,7 @@ VOID ResetCounters(PMINI_ADAPTER Adapter)
 	Adapter->bShutStatus = FALSE;
 }
 
-S_CLASSIFIER_RULE *GetFragIPClsEntry(PMINI_ADAPTER Adapter, USHORT usIpIdentification, ULONG SrcIP)
+struct bcm_classifier_rule *GetFragIPClsEntry(struct bcm_mini_adapter *Adapter, USHORT usIpIdentification, ULONG SrcIP)
 {
 	UINT uiIndex = 0;
 	for (uiIndex = 0; uiIndex < MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES; uiIndex++) {
@@ -1533,18 +1446,18 @@ S_CLASSIFIER_RULE *GetFragIPClsEntry(PMINI_ADAPTER Adapter, USHORT usIpIdentific
 	return NULL;
 }
 
-void AddFragIPClsEntry(PMINI_ADAPTER Adapter, PS_FRAGMENTED_PACKET_INFO psFragPktInfo)
+void AddFragIPClsEntry(struct bcm_mini_adapter *Adapter, struct bcm_fragmented_packet_info *psFragPktInfo)
 {
 	UINT uiIndex = 0;
 	for (uiIndex = 0; uiIndex < MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES; uiIndex++) {
 		if (!Adapter->astFragmentedPktClassifierTable[uiIndex].bUsed) {
-			memcpy(&Adapter->astFragmentedPktClassifierTable[uiIndex], psFragPktInfo, sizeof(S_FRAGMENTED_PACKET_INFO));
+			memcpy(&Adapter->astFragmentedPktClassifierTable[uiIndex], psFragPktInfo, sizeof(struct bcm_fragmented_packet_info));
 			break;
 		}
 	}
 }
 
-void DelFragIPClsEntry(PMINI_ADAPTER Adapter, USHORT usIpIdentification, ULONG SrcIp)
+void DelFragIPClsEntry(struct bcm_mini_adapter *Adapter, USHORT usIpIdentification, ULONG SrcIp)
 {
 	UINT uiIndex = 0;
 	for (uiIndex = 0; uiIndex < MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES; uiIndex++) {
@@ -1552,11 +1465,11 @@ void DelFragIPClsEntry(PMINI_ADAPTER Adapter, USHORT usIpIdentification, ULONG S
 			(Adapter->astFragmentedPktClassifierTable[uiIndex].usIpIdentification == usIpIdentification) &&
 			(Adapter->astFragmentedPktClassifierTable[uiIndex].ulSrcIpAddress == SrcIp))
 
-			memset(&Adapter->astFragmentedPktClassifierTable[uiIndex], 0, sizeof(S_FRAGMENTED_PACKET_INFO));
+			memset(&Adapter->astFragmentedPktClassifierTable[uiIndex], 0, sizeof(struct bcm_fragmented_packet_info));
 	}
 }
 
-void update_per_cid_rx(PMINI_ADAPTER Adapter)
+void update_per_cid_rx(struct bcm_mini_adapter *Adapter)
 {
 	UINT qindex = 0;
 
@@ -1580,7 +1493,7 @@ void update_per_cid_rx(PMINI_ADAPTER Adapter)
 	Adapter->liDrainCalculated = jiffies;
 }
 
-void update_per_sf_desc_cnts(PMINI_ADAPTER Adapter)
+void update_per_sf_desc_cnts(struct bcm_mini_adapter *Adapter)
 {
 	INT iIndex = 0;
 	u32 uibuff[MAX_TARGET_DSX_BUFFERS];
@@ -1606,7 +1519,7 @@ void update_per_sf_desc_cnts(PMINI_ADAPTER Adapter)
 	atomic_set(&Adapter->uiMBupdate, FALSE);
 }
 
-void flush_queue(PMINI_ADAPTER Adapter, UINT iQIndex)
+void flush_queue(struct bcm_mini_adapter *Adapter, UINT iQIndex)
 {
 	struct sk_buff *PacketToDrop = NULL;
 	struct net_device_stats *netstats = &Adapter->dev->stats;
@@ -1630,7 +1543,7 @@ void flush_queue(PMINI_ADAPTER Adapter, UINT iQIndex)
 	spin_unlock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
 }
 
-static void beceem_protocol_reset(PMINI_ADAPTER Adapter)
+static void beceem_protocol_reset(struct bcm_mini_adapter *Adapter)
 {
 	int i;
 	if (netif_msg_link(Adapter))
@@ -1652,7 +1565,7 @@ static void beceem_protocol_reset(PMINI_ADAPTER Adapter)
 	if (Adapter->TimerActive == TRUE)
 		Adapter->TimerActive = FALSE;
 
-	memset(Adapter->astFragmentedPktClassifierTable, 0, sizeof(S_FRAGMENTED_PACKET_INFO) * MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES);
+	memset(Adapter->astFragmentedPktClassifierTable, 0, sizeof(struct bcm_fragmented_packet_info) * MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES);
 
 	for (i = 0; i < HiPriority; i++) {
 		/* resetting only the first size (S_MIBS_SERVICEFLOW_TABLE) for the SF. */
diff --git a/drivers/staging/bcm/PHSDefines.h b/drivers/staging/bcm/PHSDefines.h
index eed4cfc6e53..6016fc502d2 100644
--- a/drivers/staging/bcm/PHSDefines.h
+++ b/drivers/staging/bcm/PHSDefines.h
@@ -27,7 +27,6 @@
 #define PHS_BUFFER_SIZE				 1532
 
 
-//#define MAX_PHS_LENGTHS 100
 #define MAX_PHSRULE_PER_SF       20
 #define MAX_SERVICEFLOWS			 17
 
diff --git a/drivers/staging/bcm/PHSModule.c b/drivers/staging/bcm/PHSModule.c
index 4aa2b71a40e..479574234e4 100644
--- a/drivers/staging/bcm/PHSModule.c
+++ b/drivers/staging/bcm/PHSModule.c
@@ -54,11 +54,11 @@ Function:				PHSTransmit
 
 Description:			This routine handle PHS(Payload Header Suppression for Tx path.
 					It extracts a fragment of the NDIS_PACKET containing the header
-					to be suppressed.It then supresses the header by invoking PHS exported compress routine.
-					The header data after supression is copied back to the NDIS_PACKET.
+					to be suppressed. It then suppresses the header by invoking PHS exported compress routine.
+					The header data after suppression is copied back to the NDIS_PACKET.
 
 
-Input parameters:		IN PMINI_ADAPTER Adapter         - Miniport Adapter Context
+Input parameters:		IN struct bcm_mini_adapter *Adapter         - Miniport Adapter Context
 						IN Packet 				- NDIS packet containing data to be transmitted
 						IN USHORT Vcid        - vcid pertaining to connection on which the packet is being sent.Used to
 										        identify PHS rule to be applied.
@@ -69,7 +69,7 @@ Return:					STATUS_SUCCESS - If the send was successful.
 						Other          - If an error occured.
 */
 
-int PHSTransmit(PMINI_ADAPTER Adapter,
+int PHSTransmit(struct bcm_mini_adapter *Adapter,
 					 struct sk_buff	**pPacket,
 					 USHORT Vcid,
 					 B_UINT16 uiClassifierRuleID,
@@ -84,10 +84,10 @@ int PHSTransmit(PMINI_ADAPTER Adapter,
 	UINT	unPHSNewPktHeaderLen = 0;
 	/* Pointer to PHS IN Hdr Buffer */
 	PUCHAR pucPHSPktHdrInBuf =
-				Adapter->stPhsTxContextInfo.ucaHdrSupressionInBuf;
+				Adapter->stPhsTxContextInfo.ucaHdrSuppressionInBuf;
 	/* Pointer to PHS OUT Hdr Buffer */
 	PUCHAR  pucPHSPktHdrOutBuf =
-					Adapter->stPhsTxContextInfo.ucaHdrSupressionOutBuf;
+					Adapter->stPhsTxContextInfo.ucaHdrSuppressionOutBuf;
 	UINT       usPacketType;
 	UINT       BytesToRemove=0;
 	BOOLEAN  bPHSI = 0;
@@ -101,7 +101,7 @@ int PHSTransmit(PMINI_ADAPTER Adapter,
 	if(!bEthCSSupport)
 		BytesToRemove=ETH_HLEN;
 	/*
-		Accumulate the header upto the size we support supression
+		Accumulate the header upto the size we support suppression
 		from NDIS packet
 	*/
 
@@ -125,7 +125,7 @@ int PHSTransmit(PMINI_ADAPTER Adapter,
 	{
 
 
-		// Step 2 Supress Header using PHS and fill into intermediate ucaPHSPktHdrOutBuf.
+		// Step 2 Suppress Header using PHS and fill into intermediate ucaPHSPktHdrOutBuf.
 	// Suppress only if IP Header and PHS Enabled For the Service Flow
 		if(((usPacketType == ETHERNET_FRAMETYPE_IPV4) ||
 			(usPacketType == ETHERNET_FRAMETYPE_IPV6)) &&
@@ -209,7 +209,7 @@ int PHSTransmit(PMINI_ADAPTER Adapter,
 	return STATUS_SUCCESS;
 }
 
-int PHSReceive(PMINI_ADAPTER Adapter,
+int PHSReceive(struct bcm_mini_adapter *Adapter,
 					USHORT usVcid,
 					struct sk_buff *packet,
 					UINT *punPacketLen,
@@ -217,7 +217,7 @@ int PHSReceive(PMINI_ADAPTER Adapter,
 					UINT	bHeaderSuppressionEnabled)
 {
 	u32   nStandardPktHdrLen            		= 0;
-	u32   nTotalsupressedPktHdrBytes  = 0;
+	u32   nTotalsuppressedPktHdrBytes  = 0;
 	int     ulPhsStatus 		= 0;
 	PUCHAR pucInBuff = NULL ;
 	UINT TotalBytesAdded = 0;
@@ -235,11 +235,11 @@ int PHSReceive(PMINI_ADAPTER Adapter,
 		usVcid,
 		pucInBuff,
 		Adapter->ucaPHSPktRestoreBuf,
-		&nTotalsupressedPktHdrBytes,
+		&nTotalsuppressedPktHdrBytes,
 		&nStandardPktHdrLen);
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nSupressed PktHdrLen : 0x%x Restored PktHdrLen : 0x%x",
-					nTotalsupressedPktHdrBytes,nStandardPktHdrLen);
+	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"\nSuppressed PktHdrLen : 0x%x Restored PktHdrLen : 0x%x",
+					nTotalsuppressedPktHdrBytes,nStandardPktHdrLen);
 
 	if(ulPhsStatus != STATUS_PHS_COMPRESSED)
 	{
@@ -248,7 +248,7 @@ int PHSReceive(PMINI_ADAPTER Adapter,
 	}
 	else
 	{
-		TotalBytesAdded = nStandardPktHdrLen - nTotalsupressedPktHdrBytes - PHSI_LEN;
+		TotalBytesAdded = nStandardPktHdrLen - nTotalsuppressedPktHdrBytes - PHSI_LEN;
 		if(TotalBytesAdded)
 		{
 			if(skb_headroom(packet) >= (SKB_RESERVE_ETHERNET_HEADER + TotalBytesAdded))
@@ -273,7 +273,7 @@ int PHSReceive(PMINI_ADAPTER Adapter,
 
 void DumpFullPacket(UCHAR *pBuf,UINT nPktLen)
 {
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
     BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,"Dumping Data Packet");
     BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,pBuf,nPktLen);
 }
@@ -291,7 +291,7 @@ void DumpFullPacket(UCHAR *pBuf,UINT nPktLen)
 // TRUE(1)	-If allocation of memory was success full.
 // FALSE	-If allocation of memory fails.
 //-----------------------------------------------------------------------------
-int phs_init(PPHS_DEVICE_EXTENSION pPhsdeviceExtension,PMINI_ADAPTER Adapter)
+int phs_init(PPHS_DEVICE_EXTENSION pPhsdeviceExtension, struct bcm_mini_adapter *Adapter)
 {
 	int i;
 	S_SERVICEFLOW_TABLE *pstServiceFlowTable;
@@ -398,7 +398,7 @@ ULONG PhsUpdateClassifierRule(IN void* pvContext,
 	ULONG lStatus =0;
 	UINT nSFIndex =0 ;
 	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 
 
@@ -462,7 +462,7 @@ ULONG PhsDeletePHSRule(IN void* pvContext,IN B_UINT16 uiVcid,IN B_UINT8 u8PHSI)
 	UINT nSFIndex =0, nClsidIndex =0 ;
 	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
 	S_CLASSIFIER_TABLE *pstClassifierRulesTable = NULL;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 
 	PPHS_DEVICE_EXTENSION pDeviceExtension= (PPHS_DEVICE_EXTENSION)pvContext;
@@ -528,7 +528,7 @@ ULONG PhsDeleteClassifierRule(IN void* pvContext,IN B_UINT16 uiVcid ,IN B_UINT16
 	UINT nSFIndex =0, nClsidIndex =0 ;
 	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
 	S_CLASSIFIER_ENTRY *pstClassifierEntry = NULL;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	PPHS_DEVICE_EXTENSION pDeviceExtension= (PPHS_DEVICE_EXTENSION)pvContext;
 
 	if(pDeviceExtension)
@@ -592,7 +592,7 @@ ULONG PhsDeleteSFRules(IN void* pvContext,IN B_UINT16 uiVcid)
 	UINT nSFIndex =0, nClsidIndex =0  ;
 	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
 	S_CLASSIFIER_TABLE *pstClassifierRulesTable = NULL;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	PPHS_DEVICE_EXTENSION pDeviceExtension= (PPHS_DEVICE_EXTENSION)pvContext;
     BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"====> \n");
 
@@ -684,7 +684,7 @@ ULONG PhsCompress(IN void* pvContext,
 	S_CLASSIFIER_ENTRY *pstClassifierEntry = NULL;
 	S_PHS_RULE *pstPhsRule = NULL;
 	ULONG lStatus =0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 
 
@@ -778,7 +778,7 @@ ULONG PhsDeCompress(IN void* pvContext,
 	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
 	S_PHS_RULE *pstPhsRule = NULL;
 	UINT phsi;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	PPHS_DEVICE_EXTENSION pDeviceExtension=
         (PPHS_DEVICE_EXTENSION)pvContext;
 
@@ -847,7 +847,7 @@ ULONG PhsDeCompress(IN void* pvContext,
 static void free_phs_serviceflow_rules(S_SERVICEFLOW_TABLE *psServiceFlowRulesTable)
 {
 	int i,j;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "=======>\n");
     if(psServiceFlowRulesTable)
@@ -1057,7 +1057,7 @@ UINT CreateClassiferToPHSRuleMapping(IN B_UINT16 uiVcid,
 	UINT uiStatus =PHS_SUCCESS;
 	UINT nClassifierIndex = 0;
 	S_CLASSIFIER_TABLE *psaClassifiertable = NULL;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
     psaClassifiertable = pstServiceFlowEntry->pstClassifierTable;
 
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL, "==>");
@@ -1148,7 +1148,7 @@ static UINT CreateClassifierPHSRule(IN B_UINT16  uiClsId,
 	BOOLEAN bFreeEntryFound = FALSE;
 	S_CLASSIFIER_ENTRY *psClassifierRules = NULL;
 	UINT nStatus = PHS_SUCCESS;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_DISPATCH, DBG_LVL_ALL,"Inside CreateClassifierPHSRule");
     if(psaClassifiertable == NULL)
 	{
@@ -1259,7 +1259,7 @@ static UINT UpdateClassifierPHSRule(IN B_UINT16  uiClsId,
 	S_PHS_RULE *pstAddPhsRule = NULL;
 	UINT              nPhsRuleIndex = 0;
 	BOOLEAN       bPHSRuleOrphaned = FALSE;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	psPhsRule->u8RefCnt =0;
 
 	/* Step 1 Deref Any Exisiting PHS Rule in this classifier Entry*/
@@ -1334,7 +1334,7 @@ static BOOLEAN DerefPhsRule(IN B_UINT16  uiClsId,S_CLASSIFIER_TABLE *psaClassifi
 void DumpPhsRules(PPHS_DEVICE_EXTENSION pDeviceExtension)
 {
 	int i,j,k,l;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
     BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "\n Dumping PHS Rules : \n");
 	for(i=0;i<MAX_SERVICEFLOWS;i++)
 	{
@@ -1415,7 +1415,7 @@ int phs_decompress(unsigned char *in_buf,unsigned char *out_buf,
 	int bit,i=0;
 	unsigned char *phsf,*phsm;
 	int in_buf_len = *header_size-1;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	in_buf++;
     BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_RECEIVE,DBG_LVL_ALL,"====>\n");
 	*header_size = 0;
@@ -1494,8 +1494,8 @@ static int phs_compress(S_PHS_RULE  *phs_rule,unsigned char *in_buf
 			,unsigned char *out_buf,UINT *header_size,UINT *new_header_size)
 {
 	unsigned char *old_addr = out_buf;
-	int supress = 0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	int suppress = 0;
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
     if(phs_rule == NULL)
 	{
 		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nphs_compress(): phs_rule null!");
@@ -1514,10 +1514,10 @@ static int phs_compress(S_PHS_RULE  *phs_rule,unsigned char *in_buf
 	}
 	//To copy PHSI
 	out_buf++;
-	supress = verify_suppress_phsf(in_buf,out_buf,phs_rule->u8PHSF,
+	suppress = verify_suppress_phsf(in_buf,out_buf,phs_rule->u8PHSF,
         phs_rule->u8PHSM, phs_rule->u8PHSS, phs_rule->u8PHSV,new_header_size);
 
-	if(supress == STATUS_PHS_COMPRESSED)
+	if(suppress == STATUS_PHS_COMPRESSED)
 	{
 		*old_addr = (unsigned char)phs_rule->u8PHSI;
 		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In phs_compress phsi %d",phs_rule->u8PHSI);
@@ -1527,7 +1527,7 @@ static int phs_compress(S_PHS_RULE  *phs_rule,unsigned char *in_buf
 		*old_addr = ZERO_PHSI;
 		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In phs_compress PHSV Verification failed");
 	}
-	return supress;
+	return suppress;
 }
 
 
@@ -1557,7 +1557,7 @@ static int verify_suppress_phsf(unsigned char *in_buffer,unsigned char *out_buff
 {
 	unsigned int size=0;
 	int bit,i=0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
     BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, PHS_SEND, DBG_LVL_ALL,"\nCOMP:In verify_phsf PHSM - 0x%X",*phsm);
 
 
diff --git a/drivers/staging/bcm/PHSModule.h b/drivers/staging/bcm/PHSModule.h
index c629585d0a8..b5f21157ac4 100644
--- a/drivers/staging/bcm/PHSModule.h
+++ b/drivers/staging/bcm/PHSModule.h
@@ -1,7 +1,7 @@
 #ifndef BCM_MINIPORT_PHSMODULE_H
 #define BCM_MINIPORT_PHSMODULE_H
 
-int PHSTransmit(PMINI_ADAPTER Adapter,
+int PHSTransmit(struct bcm_mini_adapter *Adapter,
 					struct sk_buff **pPacket,
 					 USHORT Vcid,
 					 B_UINT16 uiClassifierRuleID,
@@ -9,7 +9,7 @@ int PHSTransmit(PMINI_ADAPTER Adapter,
 					 PUINT PacketLen,
 					 UCHAR bEthCSSupport);
 
-int PHSReceive(PMINI_ADAPTER Adapter,
+int PHSReceive(struct bcm_mini_adapter *Adapter,
 					USHORT usVcid,
 					struct sk_buff *packet,
 					UINT *punPacketLen,
@@ -25,7 +25,7 @@ void DumpFullPacket(UCHAR *pBuf,UINT nPktLen);
 void DumpPhsRules(PPHS_DEVICE_EXTENSION pDeviceExtension);
 
 
-int phs_init(PPHS_DEVICE_EXTENSION pPhsdeviceExtension,PMINI_ADAPTER Adapter);
+int phs_init(PPHS_DEVICE_EXTENSION pPhsdeviceExtension,struct bcm_mini_adapter *Adapter);
 
 int PhsCleanup(PPHS_DEVICE_EXTENSION pPHSDeviceExt);
 
diff --git a/drivers/staging/bcm/Protocol.h b/drivers/staging/bcm/Protocol.h
index b8a4009bdf0..562d4dd81a7 100644
--- a/drivers/staging/bcm/Protocol.h
+++ b/drivers/staging/bcm/Protocol.h
@@ -55,7 +55,7 @@ typedef struct _S_ETHCS_PKT_INFO
 
 typedef struct _ETH_CS_802_Q_FRAME
 {
-	ETH_HEADER_STRUC EThHdr;
+	struct bcm_eth_header EThHdr;
 	USHORT UserPriority:3;
 	USHORT CFI:1;
 	USHORT VLANID:12;
@@ -64,7 +64,7 @@ typedef struct _ETH_CS_802_Q_FRAME
 
 typedef struct _ETH_CS_802_LLC_FRAME
 {
-	ETH_HEADER_STRUC EThHdr;
+	struct bcm_eth_header EThHdr;
 	unsigned char DSAP;
 	unsigned char SSAP;
 	unsigned char Control;
@@ -72,7 +72,7 @@ typedef struct _ETH_CS_802_LLC_FRAME
 
 typedef struct _ETH_CS_802_LLC_SNAP_FRAME
 {
-	ETH_HEADER_STRUC EThHdr;
+	struct bcm_eth_header EThHdr;
 	unsigned char DSAP;
 	unsigned char SSAP;
 	unsigned char Control;
@@ -82,7 +82,7 @@ typedef struct _ETH_CS_802_LLC_SNAP_FRAME
 
 typedef struct _ETH_CS_ETH2_FRAME
 {
-	ETH_HEADER_STRUC EThHdr;
+	struct bcm_eth_header EThHdr;
 } __attribute__((packed)) ETH_CS_ETH2_FRAME;
 
 #define ETHERNET_FRAMETYPE_IPV4		ntohs(0x0800)
diff --git a/drivers/staging/bcm/Prototypes.h b/drivers/staging/bcm/Prototypes.h
index b80b806c90a..3c8cc5ba2e2 100644
--- a/drivers/staging/bcm/Prototypes.h
+++ b/drivers/staging/bcm/Prototypes.h
@@ -1,172 +1,172 @@
 #ifndef _PROTOTYPES_H_
 #define _PROTOTYPES_H_
 
-VOID LinkControlResponseMessage(PMINI_ADAPTER Adapter, PUCHAR pucBuffer);
+VOID LinkControlResponseMessage(struct bcm_mini_adapter *Adapter, PUCHAR pucBuffer);
 
-VOID StatisticsResponse(PMINI_ADAPTER Adapter,PVOID pvBuffer);
+VOID StatisticsResponse(struct bcm_mini_adapter *Adapter,PVOID pvBuffer);
 
-VOID IdleModeResponse(PMINI_ADAPTER Adapter,PUINT puiBuffer);
+VOID IdleModeResponse(struct bcm_mini_adapter *Adapter,PUINT puiBuffer);
 
-int control_packet_handler	(PMINI_ADAPTER Adapter);
+int control_packet_handler	(struct bcm_mini_adapter *Adapter);
 
-VOID DeleteAllClassifiersForSF(PMINI_ADAPTER Adapter,UINT uiSearchRuleIndex);
+VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter,UINT uiSearchRuleIndex);
 
-VOID flush_all_queues(PMINI_ADAPTER Adapter);
+VOID flush_all_queues(struct bcm_mini_adapter *Adapter);
 
-int register_control_device_interface(PMINI_ADAPTER ps_adapter);
+int register_control_device_interface(struct bcm_mini_adapter *ps_adapter);
 
-void unregister_control_device_interface(PMINI_ADAPTER Adapter);
+void unregister_control_device_interface(struct bcm_mini_adapter *Adapter);
 
-INT CopyBufferToControlPacket(PMINI_ADAPTER Adapter,/**<Logical Adapter*/
+INT CopyBufferToControlPacket(struct bcm_mini_adapter *Adapter,/**<Logical Adapter*/
 									  PVOID ioBuffer/**<Control Packet Buffer*/
 									  );
 
-VOID SortPackInfo(PMINI_ADAPTER Adapter);
+VOID SortPackInfo(struct bcm_mini_adapter *Adapter);
 
-VOID SortClassifiers(PMINI_ADAPTER Adapter);
+VOID SortClassifiers(struct bcm_mini_adapter *Adapter);
 
-VOID flush_all_queues(PMINI_ADAPTER Adapter);
+VOID flush_all_queues(struct bcm_mini_adapter *Adapter);
 
-VOID PruneQueueAllSF(PMINI_ADAPTER Adapter);
+VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter);
 
-INT SearchSfid(PMINI_ADAPTER Adapter,UINT uiSfid);
+INT SearchSfid(struct bcm_mini_adapter *Adapter,UINT uiSfid);
 
-USHORT ClassifyPacket(PMINI_ADAPTER Adapter,struct sk_buff* skb);
+USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter,struct sk_buff* skb);
 
-BOOLEAN MatchSrcPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushSrcPort);
-BOOLEAN MatchDestPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushSrcPort);
-BOOLEAN MatchProtocol(S_CLASSIFIER_RULE *pstClassifierRule,UCHAR ucProtocol);
+BOOLEAN MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule,USHORT ushSrcPort);
+BOOLEAN MatchDestPort(struct bcm_classifier_rule *pstClassifierRule,USHORT ushSrcPort);
+BOOLEAN MatchProtocol(struct bcm_classifier_rule *pstClassifierRule,UCHAR ucProtocol);
 
 
-INT SetupNextSend(PMINI_ADAPTER Adapter, /**<Logical Adapter*/
+INT SetupNextSend(struct bcm_mini_adapter *Adapter, /**<Logical Adapter*/
 					struct sk_buff *Packet, /**<data buffer*/
 					USHORT Vcid)	;
 
-VOID LinkMessage(PMINI_ADAPTER Adapter);
+VOID LinkMessage(struct bcm_mini_adapter *Adapter);
 
-VOID transmit_packets(PMINI_ADAPTER Adapter);
+VOID transmit_packets(struct bcm_mini_adapter *Adapter);
 
-INT SendControlPacket(PMINI_ADAPTER Adapter, /**<Logical Adapter*/
+INT SendControlPacket(struct bcm_mini_adapter *Adapter, /**<Logical Adapter*/
 							char *pControlPacket/**<Control Packet*/
 							);
 
 
-int register_networkdev(PMINI_ADAPTER Adapter);
-void unregister_networkdev(PMINI_ADAPTER Adapter);
+int register_networkdev(struct bcm_mini_adapter *Adapter);
+void unregister_networkdev(struct bcm_mini_adapter *Adapter);
 
-INT AllocAdapterDsxBuffer(PMINI_ADAPTER Adapter);
+INT AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
 
-VOID AdapterFree(PMINI_ADAPTER Adapter);
+VOID AdapterFree(struct bcm_mini_adapter *Adapter);
 
-INT FreeAdapterDsxBuffer(PMINI_ADAPTER Adapter);
+INT FreeAdapterDsxBuffer(struct bcm_mini_adapter *Adapter);
 
-int tx_pkt_handler(PMINI_ADAPTER Adapter);
+int tx_pkt_handler(struct bcm_mini_adapter *Adapter);
 
-int  reset_card_proc(PMINI_ADAPTER Adapter );
+int  reset_card_proc(struct bcm_mini_adapter *Adapter );
 
-int run_card_proc(PMINI_ADAPTER Adapter );
+int run_card_proc(struct bcm_mini_adapter *Adapter );
 
-int InitCardAndDownloadFirmware(PMINI_ADAPTER ps_adapter);
+int InitCardAndDownloadFirmware(struct bcm_mini_adapter *ps_adapter);
 
 
-INT ReadMacAddressFromNVM(PMINI_ADAPTER Adapter);
+INT ReadMacAddressFromNVM(struct bcm_mini_adapter *Adapter);
 
-int register_control_device_interface(PMINI_ADAPTER ps_adapter);
+int register_control_device_interface(struct bcm_mini_adapter *ps_adapter);
 
-void DumpPackInfo(PMINI_ADAPTER Adapter);
+void DumpPackInfo(struct bcm_mini_adapter *Adapter);
 
-int rdm(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
+int rdm(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
 
-int wrm(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
+int wrm(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
 
-int wrmalt (PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
+int wrmalt (struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
 
-int rdmalt (PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
+int rdmalt (struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
 
-int get_dsx_sf_data_to_application(PMINI_ADAPTER Adapter, UINT uiSFId, void __user * user_buffer);
+int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter, UINT uiSFId, void __user * user_buffer);
 
-void SendIdleModeResponse(PMINI_ADAPTER Adapter);
+void SendIdleModeResponse(struct bcm_mini_adapter *Adapter);
 
 
-int  ProcessGetHostMibs(PMINI_ADAPTER Adapter, S_MIBS_HOST_STATS_MIBS *buf);
-void GetDroppedAppCntrlPktMibs(S_MIBS_HOST_STATS_MIBS *ioBuffer, PPER_TARANG_DATA pTarang);
-void beceem_parse_target_struct(PMINI_ADAPTER Adapter);
+int  ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, S_MIBS_HOST_STATS_MIBS *buf);
+void GetDroppedAppCntrlPktMibs(S_MIBS_HOST_STATS_MIBS *ioBuffer, struct bcm_tarang_data *pTarang);
+void beceem_parse_target_struct(struct bcm_mini_adapter *Adapter);
 
-int bcm_ioctl_fw_download(PMINI_ADAPTER Adapter, FIRMWARE_INFO *psFwInfo);
+int bcm_ioctl_fw_download(struct bcm_mini_adapter *Adapter, struct bcm_firmware_info *psFwInfo);
 
-void CopyMIBSExtendedSFParameters(PMINI_ADAPTER Adapter,
+void CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter,
 		CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex);
 
-VOID ResetCounters(PMINI_ADAPTER Adapter);
+VOID ResetCounters(struct bcm_mini_adapter *Adapter);
 
-int InitLedSettings(PMINI_ADAPTER Adapter);
+int InitLedSettings(struct bcm_mini_adapter *Adapter);
 
-S_CLASSIFIER_RULE *GetFragIPClsEntry(PMINI_ADAPTER Adapter,USHORT usIpIdentification,ULONG SrcIP);
+struct bcm_classifier_rule *GetFragIPClsEntry(struct bcm_mini_adapter *Adapter,USHORT usIpIdentification,ULONG SrcIP);
 
-void AddFragIPClsEntry(PMINI_ADAPTER Adapter,PS_FRAGMENTED_PACKET_INFO psFragPktInfo);
+void AddFragIPClsEntry(struct bcm_mini_adapter *Adapter, struct bcm_fragmented_packet_info *psFragPktInfo);
 
-void DelFragIPClsEntry(PMINI_ADAPTER Adapter,USHORT usIpIdentification,ULONG SrcIp);
+void DelFragIPClsEntry(struct bcm_mini_adapter *Adapter,USHORT usIpIdentification,ULONG SrcIp);
 
-void update_per_cid_rx (PMINI_ADAPTER Adapter);
+void update_per_cid_rx (struct bcm_mini_adapter *Adapter);
 
-void update_per_sf_desc_cnts( PMINI_ADAPTER Adapter);
+void update_per_sf_desc_cnts( struct bcm_mini_adapter *Adapter);
 
-void ClearTargetDSXBuffer(PMINI_ADAPTER Adapter,B_UINT16 TID,BOOLEAN bFreeAll);
+void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter,B_UINT16 TID,BOOLEAN bFreeAll);
 
 
-void flush_queue(PMINI_ADAPTER Adapter, UINT iQIndex);
+void flush_queue(struct bcm_mini_adapter *Adapter, UINT iQIndex);
 
 
 INT flushAllAppQ(VOID);
 
 
 INT BeceemEEPROMBulkRead(
-	PMINI_ADAPTER Adapter,
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
 	UINT uiOffset,
 	UINT uiNumBytes);
 
 
 
-INT WriteBeceemEEPROM(PMINI_ADAPTER Adapter,UINT uiEEPROMOffset, UINT uiData);
+INT WriteBeceemEEPROM(struct bcm_mini_adapter *Adapter,UINT uiEEPROMOffset, UINT uiData);
 
-INT PropagateCalParamsFromFlashToMemory(PMINI_ADAPTER Adapter);
+INT PropagateCalParamsFromFlashToMemory(struct bcm_mini_adapter *Adapter);
 
 
 INT BeceemEEPROMBulkWrite(
-	PMINI_ADAPTER Adapter,
+	struct bcm_mini_adapter *Adapter,
 	PUCHAR pBuffer,
 	UINT uiOffset,
 	UINT uiNumBytes,
 	BOOLEAN bVerify);
 
 
-INT ReadBeceemEEPROM(PMINI_ADAPTER Adapter,UINT dwAddress, UINT *pdwData);
+INT ReadBeceemEEPROM(struct bcm_mini_adapter *Adapter,UINT dwAddress, UINT *pdwData);
 
 
 INT BeceemNVMRead(
-	PMINI_ADAPTER Adapter,
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
 	UINT uiOffset,
 	UINT uiNumBytes);
 
 INT BeceemNVMWrite(
-	PMINI_ADAPTER Adapter,
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
 	UINT uiOffset,
 	UINT uiNumBytes,
 	BOOLEAN bVerify);
 
 
-INT BcmInitNVM(PMINI_ADAPTER Adapter);
+INT BcmInitNVM(struct bcm_mini_adapter *Adapter);
 
-INT BcmUpdateSectorSize(PMINI_ADAPTER Adapter,UINT uiSectorSize);
-BOOLEAN IsSectionExistInFlash(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL section);
+INT BcmUpdateSectorSize(struct bcm_mini_adapter *Adapter,UINT uiSectorSize);
+BOOLEAN IsSectionExistInFlash(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL section);
 
-INT BcmGetFlash2xSectionalBitMap(PMINI_ADAPTER Adapter, PFLASH2X_BITMAP psFlash2xBitMap);
+INT BcmGetFlash2xSectionalBitMap(struct bcm_mini_adapter *Adapter, PFLASH2X_BITMAP psFlash2xBitMap);
 
 INT BcmFlash2xBulkWrite(
-	PMINI_ADAPTER Adapter,
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
 	FLASH2X_SECTION_VAL eFlashSectionVal,
 	UINT uiOffset,
@@ -174,45 +174,45 @@ INT BcmFlash2xBulkWrite(
 	UINT bVerify);
 
 INT BcmFlash2xBulkRead(
-	PMINI_ADAPTER Adapter,
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
 	FLASH2X_SECTION_VAL eFlashSectionVal,
 	UINT uiOffsetWithinSectionVal,
 	UINT uiNumBytes);
 
-INT BcmGetSectionValStartOffset(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlashSectionVal);
+INT BcmGetSectionValStartOffset(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlashSectionVal);
 
-INT BcmSetActiveSection(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectVal);
-INT BcmAllocFlashCSStructure(PMINI_ADAPTER psAdapter);
-INT BcmDeAllocFlashCSStructure(PMINI_ADAPTER psAdapter);
+INT BcmSetActiveSection(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectVal);
+INT BcmAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter);
+INT BcmDeAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter);
 
-INT BcmCopyISO(PMINI_ADAPTER Adapter, FLASH2X_COPY_SECTION sCopySectStrut);
-INT BcmFlash2xCorruptSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
-INT BcmFlash2xWriteSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlashSectionVal);
-INT	validateFlash2xReadWrite(PMINI_ADAPTER Adapter, PFLASH2X_READWRITE psFlash2xReadWrite);
-INT IsFlash2x(PMINI_ADAPTER Adapter);
-INT	BcmCopySection(PMINI_ADAPTER Adapter,
+INT BcmCopyISO(struct bcm_mini_adapter *Adapter, FLASH2X_COPY_SECTION sCopySectStrut);
+INT BcmFlash2xCorruptSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
+INT BcmFlash2xWriteSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlashSectionVal);
+INT	validateFlash2xReadWrite(struct bcm_mini_adapter *Adapter, PFLASH2X_READWRITE psFlash2xReadWrite);
+INT IsFlash2x(struct bcm_mini_adapter *Adapter);
+INT	BcmCopySection(struct bcm_mini_adapter *Adapter,
 						FLASH2X_SECTION_VAL SrcSection,
 						FLASH2X_SECTION_VAL DstSection,
 						UINT offset,
 						UINT numOfBytes);
 
 
-BOOLEAN IsNonCDLessDevice(PMINI_ADAPTER Adapter);
+BOOLEAN IsNonCDLessDevice(struct bcm_mini_adapter *Adapter);
 
 
-VOID OverrideServiceFlowParams(PMINI_ADAPTER Adapter,PUINT puiBuffer);
+VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter,PUINT puiBuffer);
 
-int wrmaltWithLock (PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
-int rdmaltWithLock (PMINI_ADAPTER Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
+int wrmaltWithLock (struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
+int rdmaltWithLock (struct bcm_mini_adapter *Adapter, UINT uiAddress, PUINT pucBuff, size_t sSize);
 
-int wrmWithLock(PMINI_ADAPTER Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
-INT buffDnldVerify(PMINI_ADAPTER Adapter, unsigned char *mappedbuffer, unsigned int u32FirmwareLength,
+int wrmWithLock(struct bcm_mini_adapter *Adapter, UINT uiAddress, PCHAR pucBuff, size_t size);
+INT buffDnldVerify(struct bcm_mini_adapter *Adapter, unsigned char *mappedbuffer, unsigned int u32FirmwareLength,
 		unsigned long u32StartingAddress);
 
 
 VOID putUsbSuspend(struct work_struct *work);
-BOOLEAN IsReqGpioIsLedInNVM(PMINI_ADAPTER Adapter, UINT gpios);
+BOOLEAN IsReqGpioIsLedInNVM(struct bcm_mini_adapter *Adapter, UINT gpios);
 
 
 #endif
diff --git a/drivers/staging/bcm/Qos.c b/drivers/staging/bcm/Qos.c
index c97020f0fb6..1b857bd887f 100644
--- a/drivers/staging/bcm/Qos.c
+++ b/drivers/staging/bcm/Qos.c
@@ -4,13 +4,13 @@ This file contains the routines related to Quality of Service.
 */
 #include "headers.h"
 
-static void EThCSGetPktInfo(PMINI_ADAPTER Adapter,PVOID pvEthPayload,PS_ETHCS_PKT_INFO pstEthCsPktInfo);
-static BOOLEAN EThCSClassifyPkt(PMINI_ADAPTER Adapter,struct sk_buff* skb,PS_ETHCS_PKT_INFO pstEthCsPktInfo,S_CLASSIFIER_RULE *pstClassifierRule, B_UINT8 EthCSCupport);
+static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,PVOID pvEthPayload,PS_ETHCS_PKT_INFO pstEthCsPktInfo);
+static BOOLEAN EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,struct sk_buff* skb,PS_ETHCS_PKT_INFO pstEthCsPktInfo,struct bcm_classifier_rule *pstClassifierRule, B_UINT8 EthCSCupport);
 
-static USHORT	IpVersion4(PMINI_ADAPTER Adapter, struct iphdr *iphd,
-			   S_CLASSIFIER_RULE *pstClassifierRule );
+static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
+			   struct bcm_classifier_rule *pstClassifierRule );
 
-static VOID PruneQueue(PMINI_ADAPTER Adapter, INT iIndex);
+static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex);
 
 
 /*******************************************************************
@@ -24,11 +24,11 @@ static VOID PruneQueue(PMINI_ADAPTER Adapter, INT iIndex);
 *
 * Returns     - TRUE(If address matches) else FAIL .
 *********************************************************************/
-BOOLEAN MatchSrcIpAddress(S_CLASSIFIER_RULE *pstClassifierRule,ULONG ulSrcIP)
+BOOLEAN MatchSrcIpAddress(struct bcm_classifier_rule *pstClassifierRule,ULONG ulSrcIP)
 {
     UCHAR 	ucLoopIndex=0;
 
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+    struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
     ulSrcIP=ntohl(ulSrcIP);
     if(0 == pstClassifierRule->ucIPSourceAddressLength)
@@ -58,10 +58,10 @@ BOOLEAN MatchSrcIpAddress(S_CLASSIFIER_RULE *pstClassifierRule,ULONG ulSrcIP)
 *
 * Returns     - TRUE(If address matches) else FAIL .
 *********************************************************************/
-BOOLEAN MatchDestIpAddress(S_CLASSIFIER_RULE *pstClassifierRule,ULONG ulDestIP)
+BOOLEAN MatchDestIpAddress(struct bcm_classifier_rule *pstClassifierRule,ULONG ulDestIP)
 {
 	UCHAR 	ucLoopIndex=0;
-    PMINI_ADAPTER	Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	ulDestIP=ntohl(ulDestIP);
     if(0 == pstClassifierRule->ucIPDestinationAddressLength)
@@ -91,10 +91,10 @@ BOOLEAN MatchDestIpAddress(S_CLASSIFIER_RULE *pstClassifierRule,ULONG ulDestIP)
 *
 * Returns     - TRUE(If address matches) else FAIL.
 **************************************************************************/
-BOOLEAN MatchTos(S_CLASSIFIER_RULE *pstClassifierRule,UCHAR ucTypeOfService)
+BOOLEAN MatchTos(struct bcm_classifier_rule *pstClassifierRule,UCHAR ucTypeOfService)
 {
 
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
     if( 3 != pstClassifierRule->ucIPTypeOfServiceLength )
        	return TRUE;
 
@@ -117,10 +117,10 @@ BOOLEAN MatchTos(S_CLASSIFIER_RULE *pstClassifierRule,UCHAR ucTypeOfService)
 *
 * Returns     - TRUE(If address matches) else FAIL.
 ****************************************************************************/
-BOOLEAN MatchProtocol(S_CLASSIFIER_RULE *pstClassifierRule,UCHAR ucProtocol)
+BOOLEAN MatchProtocol(struct bcm_classifier_rule *pstClassifierRule,UCHAR ucProtocol)
 {
    	UCHAR 	ucLoopIndex=0;
-	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if(0 == pstClassifierRule->ucProtocolLength)
       	return TRUE;
 	for(ucLoopIndex=0;ucLoopIndex<pstClassifierRule->ucProtocolLength;ucLoopIndex++)
@@ -146,11 +146,11 @@ BOOLEAN MatchProtocol(S_CLASSIFIER_RULE *pstClassifierRule,UCHAR ucProtocol)
 *
 * Returns     - TRUE(If address matches) else FAIL.
 ***************************************************************************/
-BOOLEAN MatchSrcPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushSrcPort)
+BOOLEAN MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule,USHORT ushSrcPort)
 {
     	UCHAR 	ucLoopIndex=0;
 
-		PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 
     	if(0 == pstClassifierRule->ucSrcPortRangeLength)
@@ -178,10 +178,10 @@ BOOLEAN MatchSrcPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushSrcPort)
 *
 * Returns     - TRUE(If address matches) else FAIL.
 ***************************************************************************/
-BOOLEAN MatchDestPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushDestPort)
+BOOLEAN MatchDestPort(struct bcm_classifier_rule *pstClassifierRule,USHORT ushDestPort)
 {
     	UCHAR 	ucLoopIndex=0;
-		PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
     	if(0 == pstClassifierRule->ucDestPortRangeLength)
         	return TRUE;
@@ -204,9 +204,9 @@ BOOLEAN MatchDestPort(S_CLASSIFIER_RULE *pstClassifierRule,USHORT ushDestPort)
 Compares IPV4 Ip address and port number
 @return Queue Index.
 */
-static USHORT	IpVersion4(PMINI_ADAPTER Adapter,
+static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter,
 			   struct iphdr *iphd,
-			   S_CLASSIFIER_RULE *pstClassifierRule )
+			   struct bcm_classifier_rule *pstClassifierRule)
 {
 	xporthdr     		*xprt_hdr=NULL;
 	BOOLEAN	bClassificationSucceed=FALSE;
@@ -302,7 +302,7 @@ static USHORT	IpVersion4(PMINI_ADAPTER Adapter,
 	return bClassificationSucceed;
 }
 
-VOID PruneQueueAllSF(PMINI_ADAPTER Adapter)
+VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter)
 {
 	UINT iIndex = 0;
 
@@ -323,7 +323,7 @@ is less than number of bytes in the queue. If so -
 drops packets from the Head till the number of bytes is
 less than or equal to max queue size for the queue.
 */
-static VOID PruneQueue(PMINI_ADAPTER Adapter, INT iIndex)
+static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
 {
 	struct sk_buff* PacketToDrop=NULL;
 	struct net_device_stats *netstats;
@@ -392,7 +392,7 @@ static VOID PruneQueue(PMINI_ADAPTER Adapter, INT iIndex)
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "<=====");
 }
 
-VOID flush_all_queues(PMINI_ADAPTER Adapter)
+VOID flush_all_queues(struct bcm_mini_adapter *Adapter)
 {
 	INT		iQIndex;
 	UINT	uiTotalPacketLength;
@@ -442,10 +442,10 @@ VOID flush_all_queues(PMINI_ADAPTER Adapter)
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "<=====");
 }
 
-USHORT ClassifyPacket(PMINI_ADAPTER Adapter,struct sk_buff* skb)
+USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter,struct sk_buff* skb)
 {
 	INT			uiLoopIndex=0;
-	S_CLASSIFIER_RULE *pstClassifierRule = NULL;
+	struct bcm_classifier_rule *pstClassifierRule = NULL;
 	S_ETHCS_PKT_INFO stEthCsPktInfo;
 	PVOID pvEThPayload = NULL;
 	struct iphdr 		*pIpHeader = NULL;
@@ -631,7 +631,7 @@ USHORT ClassifyPacket(PMINI_ADAPTER Adapter,struct sk_buff* skb)
 		if(bFragmentedPkt && (usCurrFragment == 0))
 		{
 			//First Fragment of Fragmented Packet. Create Frag CLS Entry
-			S_FRAGMENTED_PACKET_INFO stFragPktInfo;
+			struct bcm_fragmented_packet_info stFragPktInfo;
 			stFragPktInfo.bUsed = TRUE;
 			stFragPktInfo.ulSrcIpAddress = pIpHeader->saddr;
 			stFragPktInfo.usIpIdentification = pIpHeader->id;
@@ -649,10 +649,10 @@ USHORT ClassifyPacket(PMINI_ADAPTER Adapter,struct sk_buff* skb)
 		return INVALID_QUEUE_INDEX;
 }
 
-static BOOLEAN EthCSMatchSrcMACAddress(S_CLASSIFIER_RULE *pstClassifierRule,PUCHAR Mac)
+static BOOLEAN EthCSMatchSrcMACAddress(struct bcm_classifier_rule *pstClassifierRule,PUCHAR Mac)
 {
 	UINT i=0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if(pstClassifierRule->ucEthCSSrcMACLen==0)
 		return TRUE;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s \n",__FUNCTION__);
@@ -666,10 +666,10 @@ static BOOLEAN EthCSMatchSrcMACAddress(S_CLASSIFIER_RULE *pstClassifierRule,PUCH
 	return TRUE;
 }
 
-static BOOLEAN EthCSMatchDestMACAddress(S_CLASSIFIER_RULE *pstClassifierRule,PUCHAR Mac)
+static BOOLEAN EthCSMatchDestMACAddress(struct bcm_classifier_rule *pstClassifierRule,PUCHAR Mac)
 {
 	UINT i=0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if(pstClassifierRule->ucEthCSDestMACLen==0)
 		return TRUE;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s \n",__FUNCTION__);
@@ -683,9 +683,9 @@ static BOOLEAN EthCSMatchDestMACAddress(S_CLASSIFIER_RULE *pstClassifierRule,PUC
 	return TRUE;
 }
 
-static BOOLEAN EthCSMatchEThTypeSAP(S_CLASSIFIER_RULE *pstClassifierRule,struct sk_buff* skb,PS_ETHCS_PKT_INFO pstEthCsPktInfo)
+static BOOLEAN EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,struct sk_buff* skb,PS_ETHCS_PKT_INFO pstEthCsPktInfo)
 {
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if((pstClassifierRule->ucEtherTypeLen==0)||
 		(pstClassifierRule->au8EthCSEtherType[0] == 0))
 		return TRUE;
@@ -718,12 +718,12 @@ static BOOLEAN EthCSMatchEThTypeSAP(S_CLASSIFIER_RULE *pstClassifierRule,struct
 
 }
 
-static BOOLEAN EthCSMatchVLANRules(S_CLASSIFIER_RULE *pstClassifierRule,struct sk_buff* skb,PS_ETHCS_PKT_INFO pstEthCsPktInfo)
+static BOOLEAN EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule,struct sk_buff* skb,PS_ETHCS_PKT_INFO pstEthCsPktInfo)
 {
 	BOOLEAN bClassificationSucceed = FALSE;
 	USHORT usVLANID;
 	B_UINT8 uPriority = 0;
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS UserPrio:%x CLS VLANID:%x\n",__FUNCTION__,ntohs(*((USHORT *)pstClassifierRule->usUserPriority)),pstClassifierRule->usVLANID);
 
@@ -733,7 +733,7 @@ static BOOLEAN EthCSMatchVLANRules(S_CLASSIFIER_RULE *pstClassifierRule,struct s
 		if(pstEthCsPktInfo->eNwpktEthFrameType!=eEth802QVLANFrame)
 				return FALSE;
 
-		uPriority = (ntohs(*(USHORT *)(skb->data + sizeof(ETH_HEADER_STRUC))) & 0xF000) >> 13;
+		uPriority = (ntohs(*(USHORT *)(skb->data + sizeof(struct bcm_eth_header))) & 0xF000) >> 13;
 
 		if((uPriority >= pstClassifierRule->usUserPriority[0]) && (uPriority <= pstClassifierRule->usUserPriority[1]))
 				bClassificationSucceed = TRUE;
@@ -751,7 +751,7 @@ static BOOLEAN EthCSMatchVLANRules(S_CLASSIFIER_RULE *pstClassifierRule,struct s
 		if(pstEthCsPktInfo->eNwpktEthFrameType!=eEth802QVLANFrame)
 				return FALSE;
 
-		usVLANID = ntohs(*(USHORT *)(skb->data + sizeof(ETH_HEADER_STRUC))) & 0xFFF;
+		usVLANID = ntohs(*(USHORT *)(skb->data + sizeof(struct bcm_eth_header))) & 0xFFF;
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  Pkt VLANID %x Priority: %d\n",__FUNCTION__,usVLANID, uPriority);
 
@@ -768,18 +768,18 @@ static BOOLEAN EthCSMatchVLANRules(S_CLASSIFIER_RULE *pstClassifierRule,struct s
 }
 
 
-static BOOLEAN EThCSClassifyPkt(PMINI_ADAPTER Adapter,struct sk_buff* skb,
+static BOOLEAN EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,struct sk_buff* skb,
 				PS_ETHCS_PKT_INFO pstEthCsPktInfo,
-				S_CLASSIFIER_RULE *pstClassifierRule,
+				struct bcm_classifier_rule *pstClassifierRule,
 				B_UINT8 EthCSCupport)
 {
 	BOOLEAN bClassificationSucceed = FALSE;
-	bClassificationSucceed = EthCSMatchSrcMACAddress(pstClassifierRule,((ETH_HEADER_STRUC *)(skb->data))->au8SourceAddress);
+	bClassificationSucceed = EthCSMatchSrcMACAddress(pstClassifierRule,((struct bcm_eth_header *)(skb->data))->au8SourceAddress);
 	if(!bClassificationSucceed)
 		return FALSE;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS SrcMAC Matched\n");
 
-	bClassificationSucceed = EthCSMatchDestMACAddress(pstClassifierRule,((ETH_HEADER_STRUC*)(skb->data))->au8DestinationAddress);
+	bClassificationSucceed = EthCSMatchDestMACAddress(pstClassifierRule,((struct bcm_eth_header *)(skb->data))->au8DestinationAddress);
 	if(!bClassificationSucceed)
 		return FALSE;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ETH CS DestMAC Matched\n");
@@ -801,10 +801,10 @@ static BOOLEAN EThCSClassifyPkt(PMINI_ADAPTER Adapter,struct sk_buff* skb,
 	return bClassificationSucceed;
 }
 
-static void EThCSGetPktInfo(PMINI_ADAPTER Adapter,PVOID pvEthPayload,
+static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,PVOID pvEthPayload,
 			    PS_ETHCS_PKT_INFO pstEthCsPktInfo)
 {
-	USHORT u16Etype = ntohs(((ETH_HEADER_STRUC*)pvEthPayload)->u16Etype);
+	USHORT u16Etype = ntohs(((struct bcm_eth_header *)pvEthPayload)->u16Etype);
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCSGetPktInfo : Eth Hdr Type : %X\n",u16Etype);
 	if(u16Etype > 0x5dc)
@@ -845,7 +845,7 @@ static void EThCSGetPktInfo(PMINI_ADAPTER Adapter,PVOID pvEthPayload,
 	else
 		pstEthCsPktInfo->eNwpktIPFrameType = eNonIPPacket;
 
-	pstEthCsPktInfo->usEtherType = ((ETH_HEADER_STRUC*)pvEthPayload)->u16Etype;
+	pstEthCsPktInfo->usEtherType = ((struct bcm_eth_header *)pvEthPayload)->u16Etype;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCsPktInfo->eNwpktIPFrameType : %x\n",pstEthCsPktInfo->eNwpktIPFrameType);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCsPktInfo->eNwpktEthFrameType : %x\n",pstEthCsPktInfo->eNwpktEthFrameType);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCsPktInfo->usEtherType : %x\n",pstEthCsPktInfo->usEtherType);
diff --git a/drivers/staging/bcm/Transmit.c b/drivers/staging/bcm/Transmit.c
index d5e4a7404f7..5e603ce76fe 100644
--- a/drivers/staging/bcm/Transmit.c
+++ b/drivers/staging/bcm/Transmit.c
@@ -41,9 +41,9 @@ SendPacketFromQueue->SetupNextSend->bcm_cmd53
 This function dispatches control packet to the h/w interface
 @return zero(success) or -ve value(failure)
 */
-INT SendControlPacket(PMINI_ADAPTER Adapter, char *pControlPacket)
+INT SendControlPacket(struct bcm_mini_adapter *Adapter, char *pControlPacket)
 {
-	PLEADER PLeader = (PLEADER)pControlPacket;
+	struct bcm_leader *PLeader = (struct bcm_leader *)pControlPacket;
 
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Tx");
 	if(!pControlPacket || !Adapter)
@@ -84,13 +84,13 @@ This function despatches the IP packets with the given vcid
 to the target via the host h/w interface.
 @return  zero(success) or -ve value(failure)
 */
-INT SetupNextSend(PMINI_ADAPTER Adapter,  struct sk_buff *Packet, USHORT Vcid)
+INT SetupNextSend(struct bcm_mini_adapter *Adapter,  struct sk_buff *Packet, USHORT Vcid)
 {
 	int		status=0;
 	BOOLEAN bHeaderSupressionEnabled = FALSE;
 	B_UINT16            uiClassifierRuleID;
 	u16	QueueIndex = skb_get_queue_mapping(Packet);
-	LEADER Leader={0};
+	struct bcm_leader Leader={0};
 
 	if(Packet->len > MAX_DEVICE_DESC_SIZE)
 	{
@@ -143,7 +143,7 @@ INT SetupNextSend(PMINI_ADAPTER Adapter,  struct sk_buff *Packet, USHORT Vcid)
 	else
 	{
 		Leader.PLength = Packet->len - ETH_HLEN;
-		memcpy((LEADER*)skb_pull(Packet, (ETH_HLEN - LEADER_SIZE)), &Leader, LEADER_SIZE);
+		memcpy((struct bcm_leader *)skb_pull(Packet, (ETH_HLEN - LEADER_SIZE)), &Leader, LEADER_SIZE);
 	}
 
 	status = Adapter->interface_transmit(Adapter->pvInterfaceAdapter,
@@ -180,7 +180,7 @@ errExit:
 	return status;
 }
 
-static int tx_pending(PMINI_ADAPTER Adapter)
+static int tx_pending(struct bcm_mini_adapter *Adapter)
 {
 	return (atomic_read(&Adapter->TxPktAvail)
 		&& MINIMUM_PENDING_DESCRIPTORS < atomic_read(&Adapter->CurrNumFreeTxDesc))
@@ -191,7 +191,7 @@ static int tx_pending(PMINI_ADAPTER Adapter)
 @ingroup tx_functions
 Transmit thread
 */
-int tx_pkt_handler(PMINI_ADAPTER Adapter  /**< pointer to adapter object*/
+int tx_pkt_handler(struct bcm_mini_adapter *Adapter  /**< pointer to adapter object*/
 				)
 {
 	int status = 0;
diff --git a/drivers/staging/bcm/cntrl_SignalingInterface.h b/drivers/staging/bcm/cntrl_SignalingInterface.h
index ab131806e2c..7619e4b819b 100644
--- a/drivers/staging/bcm/cntrl_SignalingInterface.h
+++ b/drivers/staging/bcm/cntrl_SignalingInterface.h
@@ -117,7 +117,7 @@ typedef struct _stPhsRuleSI {
     B_UINT8                         u8PHSM[MAX_PHS_LENGTHS];
 	/**  8bit Total number of bytes to be suppressed for the Service Flow*/
     B_UINT8                         u8PHSS;
-	/**  8bit Indicates whether or not Packet Header contents need to be verified prior to supression */
+	/**  8bit Indicates whether or not Packet Header contents need to be verified prior to suppression */
     B_UINT8                         u8PHSV;
 	/**  Vendor Specific PHS param Length Of The Service Flow*/
     B_UINT8                         u8VendorSpecificPHSParamsLength;
diff --git a/drivers/staging/bcm/hostmibs.c b/drivers/staging/bcm/hostmibs.c
index 101c4e31249..08d13a4dfd7 100644
--- a/drivers/staging/bcm/hostmibs.c
+++ b/drivers/staging/bcm/hostmibs.c
@@ -9,7 +9,7 @@
 
 #include "headers.h"
 
-INT ProcessGetHostMibs(PMINI_ADAPTER Adapter, S_MIBS_HOST_STATS_MIBS *pstHostMibs)
+INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, S_MIBS_HOST_STATS_MIBS *pstHostMibs)
 {
 	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
 	S_PHS_RULE *pstPhsRule = NULL;
@@ -94,14 +94,14 @@ INT ProcessGetHostMibs(PMINI_ADAPTER Adapter, S_MIBS_HOST_STATS_MIBS *pstHostMib
 	return STATUS_SUCCESS;
 }
 
-VOID GetDroppedAppCntrlPktMibs(S_MIBS_HOST_STATS_MIBS *pstHostMibs, const PPER_TARANG_DATA pTarang)
+VOID GetDroppedAppCntrlPktMibs(S_MIBS_HOST_STATS_MIBS *pstHostMibs, struct bcm_tarang_data *pTarang)
 {
 	memcpy(&(pstHostMibs->stDroppedAppCntrlMsgs),
 	       &(pTarang->stDroppedAppCntrlMsgs),
 	       sizeof(S_MIBS_DROPPED_APP_CNTRL_MESSAGES));
 }
 
-VOID CopyMIBSExtendedSFParameters(PMINI_ADAPTER Adapter, CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex)
+VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter, CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex)
 {
 	S_MIBS_EXTSERVICEFLOW_PARAMETERS *t = &Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable;
 
diff --git a/drivers/staging/bcm/led_control.c b/drivers/staging/bcm/led_control.c
index c7f48862972..252a1b31d61 100644
--- a/drivers/staging/bcm/led_control.c
+++ b/drivers/staging/bcm/led_control.c
@@ -13,7 +13,7 @@ static B_UINT16 CFG_CalculateChecksum(B_UINT8 *pu8Buffer, B_UINT32 u32Size)
 	return u16CheckSum;
 }
 
-BOOLEAN IsReqGpioIsLedInNVM(PMINI_ADAPTER Adapter, UINT gpios)
+BOOLEAN IsReqGpioIsLedInNVM(struct bcm_mini_adapter *Adapter, UINT gpios)
 {
 	INT Status;
 	Status = (Adapter->gpioBitMap & gpios) ^ gpios;
@@ -23,7 +23,7 @@ BOOLEAN IsReqGpioIsLedInNVM(PMINI_ADAPTER Adapter, UINT gpios)
 		return TRUE;
 }
 
-static INT LED_Blink(PMINI_ADAPTER Adapter, UINT GPIO_Num, UCHAR uiLedIndex,
+static INT LED_Blink(struct bcm_mini_adapter *Adapter, UINT GPIO_Num, UCHAR uiLedIndex,
 		ULONG timeout, INT num_of_time, LedEventInfo_t currdriverstate)
 {
 	int Status = STATUS_SUCCESS;
@@ -95,7 +95,7 @@ static INT ScaleRateofTransfer(ULONG rate)
 
 
 
-static INT LED_Proportional_Blink(PMINI_ADAPTER Adapter, UCHAR GPIO_Num_tx,
+static INT LED_Proportional_Blink(struct bcm_mini_adapter *Adapter, UCHAR GPIO_Num_tx,
 		UCHAR uiTxLedIndex, UCHAR GPIO_Num_rx, UCHAR uiRxLedIndex,
 		LedEventInfo_t currdriverstate)
 {
@@ -261,7 +261,7 @@ static INT LED_Proportional_Blink(PMINI_ADAPTER Adapter, UCHAR GPIO_Num_tx,
  *  <OSAL_STATUS_CODE>
  * -----------------------------------------------------------------------------
  */
-static INT ValidateDSDParamsChecksum(PMINI_ADAPTER Adapter, ULONG ulParamOffset,
+static INT ValidateDSDParamsChecksum(struct bcm_mini_adapter *Adapter, ULONG ulParamOffset,
 					USHORT usParamLen)
 {
 	INT Status = STATUS_SUCCESS;
@@ -347,7 +347,7 @@ exit:
  *  <OSAL_STATUS_CODE>
  * -----------------------------------------------------------------------------
  */
-static INT ValidateHWParmStructure(PMINI_ADAPTER Adapter, ULONG ulHwParamOffset)
+static INT ValidateHWParmStructure(struct bcm_mini_adapter *Adapter, ULONG ulHwParamOffset)
 {
 
 	INT Status = STATUS_SUCCESS;
@@ -371,7 +371,7 @@ static INT ValidateHWParmStructure(PMINI_ADAPTER Adapter, ULONG ulHwParamOffset)
 	return Status;
 } /* ValidateHWParmStructure() */
 
-static int ReadLEDInformationFromEEPROM(PMINI_ADAPTER Adapter,
+static int ReadLEDInformationFromEEPROM(struct bcm_mini_adapter *Adapter,
 					UCHAR GPIO_Array[])
 {
 	int Status = STATUS_SUCCESS;
@@ -477,7 +477,7 @@ static int ReadLEDInformationFromEEPROM(PMINI_ADAPTER Adapter,
 }
 
 
-static int ReadConfigFileStructure(PMINI_ADAPTER Adapter,
+static int ReadConfigFileStructure(struct bcm_mini_adapter *Adapter,
 					BOOLEAN *bEnableThread)
 {
 	int Status = STATUS_SUCCESS;
@@ -580,7 +580,7 @@ static int ReadConfigFileStructure(PMINI_ADAPTER Adapter,
  *
  * -----------------------------------------------------------------------------
  */
-static VOID LedGpioInit(PMINI_ADAPTER Adapter)
+static VOID LedGpioInit(struct bcm_mini_adapter *Adapter)
 {
 	UINT uiResetValue = 0;
 	UINT uiIndex      = 0;
@@ -605,7 +605,7 @@ static VOID LedGpioInit(PMINI_ADAPTER Adapter)
 	Adapter->LEDInfo.bIdle_led_off = FALSE;
 }
 
-static INT BcmGetGPIOPinInfo(PMINI_ADAPTER Adapter, UCHAR *GPIO_num_tx,
+static INT BcmGetGPIOPinInfo(struct bcm_mini_adapter *Adapter, UCHAR *GPIO_num_tx,
 		UCHAR *GPIO_num_rx, UCHAR *uiLedTxIndex, UCHAR *uiLedRxIndex,
 		LedEventInfo_t currdriverstate)
 {
@@ -645,7 +645,7 @@ static INT BcmGetGPIOPinInfo(PMINI_ADAPTER Adapter, UCHAR *GPIO_num_tx,
 	}
 	return STATUS_SUCCESS;
 }
-static VOID LEDControlThread(PMINI_ADAPTER Adapter)
+static VOID LEDControlThread(struct bcm_mini_adapter *Adapter)
 {
 	UINT uiIndex = 0;
 	UCHAR GPIO_num = 0;
@@ -857,7 +857,7 @@ static VOID LEDControlThread(PMINI_ADAPTER Adapter)
 	Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_DISABLED;
 }
 
-int InitLedSettings(PMINI_ADAPTER Adapter)
+int InitLedSettings(struct bcm_mini_adapter *Adapter)
 {
 	int Status = STATUS_SUCCESS;
 	BOOLEAN bEnableThread = TRUE;
diff --git a/drivers/staging/bcm/nvm.c b/drivers/staging/bcm/nvm.c
index 7d703cb3c5e..b179dbab93b 100644
--- a/drivers/staging/bcm/nvm.c
+++ b/drivers/staging/bcm/nvm.c
@@ -2,195 +2,181 @@
 
 #define DWORD unsigned int
 
-static INT BcmDoChipSelect(PMINI_ADAPTER Adapter, UINT offset);
-static INT BcmGetActiveDSD(PMINI_ADAPTER Adapter);
-static INT BcmGetActiveISO(PMINI_ADAPTER Adapter);
-static UINT BcmGetEEPROMSize(PMINI_ADAPTER Adapter);
-static INT BcmGetFlashCSInfo(PMINI_ADAPTER Adapter);
-static UINT BcmGetFlashSectorSize(PMINI_ADAPTER Adapter, UINT FlashSectorSizeSig, UINT FlashSectorSize);
-
-static VOID BcmValidateNvmType(PMINI_ADAPTER Adapter);
-static INT BcmGetNvmSize(PMINI_ADAPTER Adapter);
-static UINT BcmGetFlashSize(PMINI_ADAPTER Adapter);
-static NVM_TYPE BcmGetNvmType(PMINI_ADAPTER Adapter);
-
-static INT BcmGetSectionValEndOffset(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
-
-static B_UINT8 IsOffsetWritable(PMINI_ADAPTER Adapter, UINT uiOffset);
-static INT IsSectionWritable(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL Section);
-static INT IsSectionExistInVendorInfo(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL section);
-
-static INT ReadDSDPriority(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL dsd);
-static INT ReadDSDSignature(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL dsd);
-static INT ReadISOPriority(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL iso);
-static INT ReadISOSignature(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL iso);
-
-static INT CorruptDSDSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
-static INT CorruptISOSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
-static INT SaveHeaderIfPresent(PMINI_ADAPTER Adapter, PUCHAR pBuff, UINT uiSectAlignAddr);
-static INT WriteToFlashWithoutSectorErase(PMINI_ADAPTER Adapter, PUINT pBuff,
-					  FLASH2X_SECTION_VAL eFlash2xSectionVal,
-					  UINT uiOffset, UINT uiNumBytes);
-static FLASH2X_SECTION_VAL getHighestPriDSD(PMINI_ADAPTER Adapter);
-static FLASH2X_SECTION_VAL getHighestPriISO(PMINI_ADAPTER Adapter);
-
-static INT BeceemFlashBulkRead(
-	PMINI_ADAPTER Adapter,
+static int BcmDoChipSelect(struct bcm_mini_adapter *Adapter, unsigned int offset);
+static int BcmGetActiveDSD(struct bcm_mini_adapter *Adapter);
+static int BcmGetActiveISO(struct bcm_mini_adapter *Adapter);
+static unsigned int BcmGetEEPROMSize(struct bcm_mini_adapter *Adapter);
+static int BcmGetFlashCSInfo(struct bcm_mini_adapter *Adapter);
+static unsigned int BcmGetFlashSectorSize(struct bcm_mini_adapter *Adapter, unsigned int FlashSectorSizeSig, unsigned int FlashSectorSize);
+
+static VOID BcmValidateNvmType(struct bcm_mini_adapter *Adapter);
+static int BcmGetNvmSize(struct bcm_mini_adapter *Adapter);
+static unsigned int BcmGetFlashSize(struct bcm_mini_adapter *Adapter);
+static NVM_TYPE BcmGetNvmType(struct bcm_mini_adapter *Adapter);
+
+static int BcmGetSectionValEndOffset(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
+
+static B_UINT8 IsOffsetWritable(struct bcm_mini_adapter *Adapter, unsigned int uiOffset);
+static int IsSectionWritable(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL Section);
+static int IsSectionExistInVendorInfo(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL section);
+
+static int ReadDSDPriority(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL dsd);
+static int ReadDSDSignature(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL dsd);
+static int ReadISOPriority(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL iso);
+static int ReadISOSignature(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL iso);
+
+static int CorruptDSDSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
+static int CorruptISOSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal);
+static int SaveHeaderIfPresent(struct bcm_mini_adapter *Adapter, PUCHAR pBuff, unsigned int uiSectAlignAddr);
+static int WriteToFlashWithoutSectorErase(struct bcm_mini_adapter *Adapter, PUINT pBuff,
+					FLASH2X_SECTION_VAL eFlash2xSectionVal,
+					unsigned int uiOffset, unsigned int uiNumBytes);
+static FLASH2X_SECTION_VAL getHighestPriDSD(struct bcm_mini_adapter *Adapter);
+static FLASH2X_SECTION_VAL getHighestPriISO(struct bcm_mini_adapter *Adapter);
+
+static int BeceemFlashBulkRead(
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes);
+	unsigned int uiOffset,
+	unsigned int uiNumBytes);
 
-static INT BeceemFlashBulkWrite(
-	PMINI_ADAPTER Adapter,
+static int BeceemFlashBulkWrite(
+	struct bcm_mini_adapter *Adapter,
 	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
+	unsigned int uiOffset,
+	unsigned int uiNumBytes,
 	BOOLEAN bVerify);
 
-static INT GetFlashBaseAddr(PMINI_ADAPTER Adapter);
+static int GetFlashBaseAddr(struct bcm_mini_adapter *Adapter);
 
-static INT ReadBeceemEEPROMBulk(PMINI_ADAPTER Adapter,UINT dwAddress, UINT *pdwData, UINT dwNumData);
+static int ReadBeceemEEPROMBulk(struct bcm_mini_adapter *Adapter, unsigned int dwAddress, unsigned int *pdwData, unsigned int dwNumData);
 
-// Procedure:	ReadEEPROMStatusRegister
-//
-// Description: Reads the standard EEPROM Status Register.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static UCHAR ReadEEPROMStatusRegister( PMINI_ADAPTER Adapter )
+/* Procedure:	ReadEEPROMStatusRegister
+ *
+ * Description: Reads the standard EEPROM Status Register.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ * Returns:
+ *		OSAL_STATUS_CODE
+ */
+static UCHAR ReadEEPROMStatusRegister(struct bcm_mini_adapter *Adapter)
 {
 	UCHAR uiData = 0;
-	DWORD dwRetries = MAX_EEPROM_RETRIES*RETRIES_PER_DELAY;
-	UINT uiStatus = 0;
-	UINT value = 0;
-	UINT value1 = 0;
+	DWORD dwRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
+	unsigned int uiStatus = 0;
+	unsigned int value = 0;
+	unsigned int value1 = 0;
 
 	/* Read the EEPROM status register */
-	value = EEPROM_READ_STATUS_REGISTER ;
-	wrmalt( Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
+	value = EEPROM_READ_STATUS_REGISTER;
+	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
 
-	while ( dwRetries != 0 )
-	{
-		value=0;
-		uiStatus = 0 ;
+	while (dwRetries != 0) {
+		value = 0;
+		uiStatus = 0;
 		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-		if(Adapter->device_removed == TRUE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Modem has got removed hence exiting....");
+		if (Adapter->device_removed == TRUE) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem has got removed hence exiting....");
 			break;
 		}
 
 		/* Wait for Avail bit to be set. */
-		if ( ( uiStatus & EEPROM_READ_DATA_AVAIL) != 0 )
-		{
+		if ((uiStatus & EEPROM_READ_DATA_AVAIL) != 0) {
 			/* Clear the Avail/Full bits - which ever is set. */
 			value = uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL);
-			wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
+			wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 
-			value =0;
+			value = 0;
 			rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 			uiData = (UCHAR)value;
 
 			break;
 		}
 
-		dwRetries-- ;
-		if ( dwRetries == 0 )
-		{
-			 rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
-			 rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
-			 BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"0x3004 = %x 0x3008 = %x, retries = %d failed.\n",value,value1,  MAX_EEPROM_RETRIES*RETRIES_PER_DELAY);
+		dwRetries--;
+		if (dwRetries == 0) {
+			rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
+			rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "0x3004 = %x 0x3008 = %x, retries = %d failed.\n", value, value1, MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
 			return uiData;
 		}
-		if( !(dwRetries%RETRIES_PER_DELAY) )
-			msleep(1);
+		if (!(dwRetries%RETRIES_PER_DELAY))
+			udelay(1000);
 		uiStatus = 0 ;
 	}
 	return uiData;
 } /* ReadEEPROMStatusRegister */
 
-//-----------------------------------------------------------------------------
-// Procedure:	ReadBeceemEEPROMBulk
-//
-// Description: This routine reads 16Byte data from EEPROM
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//      dwAddress   - EEPROM Offset to read the data from.
-//      pdwData     - Pointer to double word where data needs to be stored in.  //		dwNumWords  - Number of words.  Valid values are 4 ONLY.
-//
-// Returns:
-//		OSAL_STATUS_CODE:
-//-----------------------------------------------------------------------------
-
-INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
-									   DWORD dwAddress,
-									   DWORD *pdwData,
-									   DWORD dwNumWords
-									 )
+/*
+ * Procedure:	ReadBeceemEEPROMBulk
+ *
+ * Description: This routine reads 16Byte data from EEPROM
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *      dwAddress   - EEPROM Offset to read the data from.
+ *      pdwData     - Pointer to double word where data needs to be stored in.  //		dwNumWords  - Number of words.  Valid values are 4 ONLY.
+ *
+ * Returns:
+ *		OSAL_STATUS_CODE:
+ */
+
+int ReadBeceemEEPROMBulk(struct bcm_mini_adapter *Adapter,
+			DWORD dwAddress,
+			DWORD *pdwData,
+			DWORD dwNumWords)
 {
 	DWORD dwIndex = 0;
-	DWORD dwRetries = MAX_EEPROM_RETRIES*RETRIES_PER_DELAY;
-	UINT uiStatus  = 0;
-	UINT value= 0;
-	UINT value1 = 0;
+	DWORD dwRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
+	unsigned int uiStatus  = 0;
+	unsigned int value = 0;
+	unsigned int value1 = 0;
 	UCHAR *pvalue;
 
 	/* Flush the read and cmd queue. */
-	value=( EEPROM_READ_QUEUE_FLUSH | EEPROM_CMD_QUEUE_FLUSH );
-	wrmalt( Adapter, SPI_FLUSH_REG, &value, sizeof(value) );
-	value=0;
-	wrmalt( Adapter, SPI_FLUSH_REG, &value, sizeof(value));
+	value = (EEPROM_READ_QUEUE_FLUSH | EEPROM_CMD_QUEUE_FLUSH);
+	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
+	value = 0;
+	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
 
 	/* Clear the Avail/Full bits. */
-	value=( EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL );
-	wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&value, sizeof(value));
+	value = (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL);
+	wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 
-	value= dwAddress | ( (dwNumWords == 4) ? EEPROM_16_BYTE_PAGE_READ : EEPROM_4_BYTE_PAGE_READ );
-	wrmalt( Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
-
-	while ( dwRetries != 0 )
-		{
+	value = dwAddress | ((dwNumWords == 4) ? EEPROM_16_BYTE_PAGE_READ : EEPROM_4_BYTE_PAGE_READ);
+	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
 
+	while (dwRetries != 0) {
 		uiStatus = 0;
 		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-		if(Adapter->device_removed == TRUE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Modem has got Removed.hence exiting from loop...");
+		if (Adapter->device_removed == TRUE) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem has got Removed.hence exiting from loop...");
 			return -ENODEV;
 		}
 
 		/* If we are reading 16 bytes we want to be sure that the queue
 		 * is full before we read.  In the other cases we are ok if the
-		 * queue has data available */
-		if ( dwNumWords == 4 )
-		{
-			if ( ( uiStatus & EEPROM_READ_DATA_FULL ) != 0 )
-			{
+		 * queue has data available
+		 */
+		if (dwNumWords == 4) {
+			if ((uiStatus & EEPROM_READ_DATA_FULL) != 0) {
 				/* Clear the Avail/Full bits - which ever is set. */
-				value = ( uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL) ) ;
-				wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&value, sizeof(value));
+				value = (uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL));
+				wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 				break;
 			}
-		}
-		else if ( dwNumWords == 1 )
-		{
-
-			if ( ( uiStatus & EEPROM_READ_DATA_AVAIL ) != 0 )
-			{
+		} else if (dwNumWords == 1) {
+			if ((uiStatus & EEPROM_READ_DATA_AVAIL) != 0) {
 				/* We just got Avail and we have to read 32bits so we
-				 * need this sleep for Cardbus kind of devices. */
-				if (Adapter->chip_id == 0xBECE0210 )
-	  					udelay(800);
+				 * need this sleep for Cardbus kind of devices.
+				 */
+				if (Adapter->chip_id == 0xBECE0210)
+					udelay(800);
 
 				/* Clear the Avail/Full bits - which ever is set. */
-				value=( uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL) );
-				wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&value, sizeof(value));
+				value = (uiStatus & (EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL));
+				wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 				break;
 			}
 		}
@@ -198,25 +184,25 @@ INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
 		uiStatus = 0;
 
 		dwRetries--;
-		if(dwRetries == 0)
-		{
-			value=0;
-			value1=0;
+		if (dwRetries == 0) {
+			value = 0;
+			value1 = 0;
 			rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 			rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "dwNumWords %d 0x3004 = %x 0x3008 = %x  retries = %d failed.\n", dwNumWords, value,  value1,  MAX_EEPROM_RETRIES*RETRIES_PER_DELAY);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "dwNumWords %d 0x3004 = %x 0x3008 = %x  retries = %d failed.\n",
+					dwNumWords, value,  value1,  MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
 			return STATUS_FAILURE;
 		}
-		if( !(dwRetries%RETRIES_PER_DELAY) )
-			msleep(1);
+
+		if (!(dwRetries%RETRIES_PER_DELAY))
+			udelay(1000);
 	}
 
-	for ( dwIndex = 0; dwIndex < dwNumWords ; dwIndex++ )
-	{
+	for (dwIndex = 0; dwIndex < dwNumWords; dwIndex++) {
 		/* We get only a byte at a time - from LSB to MSB. We shift it into an integer. */
 		pvalue = (PUCHAR)(pdwData + dwIndex);
 
-		value =0;
+		value = 0;
 		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 
 		pvalue[0] = value;
@@ -226,7 +212,7 @@ INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
 
 		pvalue[1] = value;
 
-		value =0;
+		value = 0;
 		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 
 		pvalue[2] = value;
@@ -240,31 +226,30 @@ INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
 	return STATUS_SUCCESS;
 } /* ReadBeceemEEPROMBulk() */
 
-//-----------------------------------------------------------------------------
-// Procedure:	ReadBeceemEEPROM
-//
-// Description: This routine reads 4 data from EEPROM.  It uses 1 or 2 page
-//				reads to do this operation.
-//
-// Arguments:
-//		Adapter     - ptr to Adapter object instance
-//      uiOffset	- EEPROM Offset to read the data from.
-//      pBuffer		- Pointer to word where data needs to be stored in.
-//
-// Returns:
-//		OSAL_STATUS_CODE:
-//-----------------------------------------------------------------------------
-
-INT ReadBeceemEEPROM( PMINI_ADAPTER Adapter,
-								   DWORD uiOffset,
-								   DWORD *pBuffer
-								 )
+/*
+ * Procedure:	ReadBeceemEEPROM
+ *
+ * Description: This routine reads 4 data from EEPROM.  It uses 1 or 2 page
+ *				reads to do this operation.
+ *
+ * Arguments:
+ *		Adapter     - ptr to Adapter object instance
+ *      uiOffset	- EEPROM Offset to read the data from.
+ *      pBuffer		- Pointer to word where data needs to be stored in.
+ *
+ * Returns:
+ *		OSAL_STATUS_CODE:
+ */
+
+int ReadBeceemEEPROM(struct bcm_mini_adapter *Adapter,
+		DWORD uiOffset,
+		DWORD *pBuffer)
 {
-	UINT uiData[8]	 	= {0};
-	UINT uiByteOffset	= 0;
-	UINT uiTempOffset	= 0;
+	unsigned int uiData[8]		= {0};
+	unsigned int uiByteOffset	= 0;
+	unsigned int uiTempOffset	= 0;
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL," ====> ");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " ====> ");
 
 	uiTempOffset = uiOffset - (uiOffset % MAX_RW_SIZE);
 	uiByteOffset = uiOffset - uiTempOffset;
@@ -272,22 +257,19 @@ INT ReadBeceemEEPROM( PMINI_ADAPTER Adapter,
 	ReadBeceemEEPROMBulk(Adapter, uiTempOffset, (PUINT)&uiData[0], 4);
 
 	/* A word can overlap at most over 2 pages. In that case we read the
-	 * next page too. */
-	if ( uiByteOffset > 12 )
-	{
+	 * next page too.
+	 */
+	if (uiByteOffset > 12)
 		ReadBeceemEEPROMBulk(Adapter, uiTempOffset + MAX_RW_SIZE, (PUINT)&uiData[4], 4);
-	}
 
-	memcpy( (PUCHAR) pBuffer, ( ((PUCHAR)&uiData[0]) + uiByteOffset ), 4);
+	memcpy((PUCHAR)pBuffer, (((PUCHAR)&uiData[0]) + uiByteOffset), 4);
 
 	return STATUS_SUCCESS;
 } /* ReadBeceemEEPROM() */
 
-
-
-INT ReadMacAddressFromNVM(PMINI_ADAPTER Adapter)
+int ReadMacAddressFromNVM(struct bcm_mini_adapter *Adapter)
 {
-	INT Status;
+	int Status;
 	unsigned char puMacAddr[6];
 
 	Status = BeceemNVMRead(Adapter,
@@ -295,182 +277,154 @@ INT ReadMacAddressFromNVM(PMINI_ADAPTER Adapter)
 			INIT_PARAMS_1_MACADDRESS_ADDRESS,
 			MAC_ADDRESS_SIZE);
 
-	if(Status == STATUS_SUCCESS)
+	if (Status == STATUS_SUCCESS)
 		memcpy(Adapter->dev->dev_addr, puMacAddr, MAC_ADDRESS_SIZE);
 
 	return Status;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemEEPROMBulkRead
-//
-// Description: Reads the EEPROM and returns the Data.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		pBuffer    - Buffer to store the data read from EEPROM
-//		uiOffset   - Offset of EEPROM from where data should be read
-//		uiNumBytes - Number of bytes to be read from the EEPROM.
-//
-// Returns:
-//		OSAL_STATUS_SUCCESS - if EEPROM read is successful.
-//		<FAILURE>			- if failed.
-//-----------------------------------------------------------------------------
-
-INT BeceemEEPROMBulkRead(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes)
+/*
+ * Procedure:	BeceemEEPROMBulkRead
+ *
+ * Description: Reads the EEPROM and returns the Data.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		pBuffer    - Buffer to store the data read from EEPROM
+ *		uiOffset   - Offset of EEPROM from where data should be read
+ *		uiNumBytes - Number of bytes to be read from the EEPROM.
+ *
+ * Returns:
+ *		OSAL_STATUS_SUCCESS - if EEPROM read is successful.
+ *		<FAILURE>			- if failed.
+ */
+
+int BeceemEEPROMBulkRead(struct bcm_mini_adapter *Adapter,
+			PUINT pBuffer,
+			unsigned int uiOffset,
+			unsigned int uiNumBytes)
 {
-	UINT uiData[4]		  = {0};
-	//UINT uiAddress 		  = 0;
-	UINT uiBytesRemaining = uiNumBytes;
-	UINT uiIndex 		  = 0;
-	UINT uiTempOffset  	  = 0;
-	UINT uiExtraBytes     = 0;
-	UINT uiFailureRetries = 0;
+	unsigned int uiData[4]		= {0};
+	/* unsigned int uiAddress	= 0; */
+	unsigned int uiBytesRemaining	= uiNumBytes;
+	unsigned int uiIndex		= 0;
+	unsigned int uiTempOffset	= 0;
+	unsigned int uiExtraBytes	= 0;
+	unsigned int uiFailureRetries	= 0;
 	PUCHAR pcBuff = (PUCHAR)pBuffer;
 
-
-	if(uiOffset%MAX_RW_SIZE&& uiBytesRemaining)
-	{
-		uiTempOffset = uiOffset - (uiOffset%MAX_RW_SIZE);
-		uiExtraBytes = uiOffset-uiTempOffset;
-		ReadBeceemEEPROMBulk(Adapter,uiTempOffset,(PUINT)&uiData[0],4);
-		if(uiBytesRemaining >= (MAX_RW_SIZE - uiExtraBytes))
-		{
-			memcpy(pBuffer,(((PUCHAR)&uiData[0])+uiExtraBytes),MAX_RW_SIZE - uiExtraBytes);
-
+	if (uiOffset % MAX_RW_SIZE && uiBytesRemaining) {
+		uiTempOffset = uiOffset - (uiOffset % MAX_RW_SIZE);
+		uiExtraBytes = uiOffset - uiTempOffset;
+		ReadBeceemEEPROMBulk(Adapter, uiTempOffset, (PUINT)&uiData[0], 4);
+		if (uiBytesRemaining >= (MAX_RW_SIZE - uiExtraBytes)) {
+			memcpy(pBuffer, (((PUCHAR)&uiData[0]) + uiExtraBytes), MAX_RW_SIZE - uiExtraBytes);
 			uiBytesRemaining -= (MAX_RW_SIZE - uiExtraBytes);
 			uiIndex += (MAX_RW_SIZE - uiExtraBytes);
 			uiOffset += (MAX_RW_SIZE - uiExtraBytes);
-		}
-		else
-		{
-			memcpy(pBuffer,(((PUCHAR)&uiData[0])+uiExtraBytes),uiBytesRemaining);
+		} else {
+			memcpy(pBuffer, (((PUCHAR)&uiData[0]) + uiExtraBytes), uiBytesRemaining);
 			uiIndex += uiBytesRemaining;
 			uiOffset += uiBytesRemaining;
 			uiBytesRemaining = 0;
 		}
-
-
 	}
 
-
-	while(uiBytesRemaining && uiFailureRetries != 128)
-	{
-		if(Adapter->device_removed )
-		{
+	while (uiBytesRemaining && uiFailureRetries != 128) {
+		if (Adapter->device_removed)
 			return -1;
-		}
 
-		if(uiBytesRemaining >= MAX_RW_SIZE)
-		{
+		if (uiBytesRemaining >= MAX_RW_SIZE) {
 			/* For the requests more than or equal to 16 bytes, use bulk
 			 * read function to make the access faster.
-			 * We read 4 Dwords of data */
-			if(0 == ReadBeceemEEPROMBulk(Adapter,uiOffset,&uiData[0],4))
-			{
-				memcpy(pcBuff+uiIndex,&uiData[0],MAX_RW_SIZE);
+			 * We read 4 Dwords of data
+			 */
+			if (ReadBeceemEEPROMBulk(Adapter, uiOffset, &uiData[0], 4) == 0) {
+				memcpy(pcBuff + uiIndex, &uiData[0], MAX_RW_SIZE);
 				uiOffset += MAX_RW_SIZE;
 				uiBytesRemaining -= MAX_RW_SIZE;
 				uiIndex += MAX_RW_SIZE;
-			}
-			else
-			{
+			} else {
 				uiFailureRetries++;
-				mdelay(3);//sleep for a while before retry...
+				mdelay(3); /* sleep for a while before retry... */
 			}
-		}
-		else if(uiBytesRemaining >= 4)
-		{
-			if(0 == ReadBeceemEEPROM(Adapter,uiOffset,&uiData[0]))
-			{
-				memcpy(pcBuff+uiIndex,&uiData[0],4);
+		} else if (uiBytesRemaining >= 4) {
+			if (ReadBeceemEEPROM(Adapter, uiOffset, &uiData[0]) == 0) {
+				memcpy(pcBuff + uiIndex, &uiData[0], 4);
 				uiOffset += 4;
 				uiBytesRemaining -= 4;
-				uiIndex +=4;
-			}
-			else
-			{
+				uiIndex += 4;
+			} else {
 				uiFailureRetries++;
-				mdelay(3);//sleep for a while before retry...
+				mdelay(3); /* sleep for a while before retry... */
 			}
-		}
-		else
-		{ // Handle the reads less than 4 bytes...
+		} else {
+			/* Handle the reads less than 4 bytes... */
 			PUCHAR pCharBuff = (PUCHAR)pBuffer;
 			pCharBuff += uiIndex;
-			if(0 == ReadBeceemEEPROM(Adapter,uiOffset,&uiData[0]))
-			{
-				memcpy(pCharBuff,&uiData[0],uiBytesRemaining);//copy only bytes requested.
+			if (ReadBeceemEEPROM(Adapter, uiOffset, &uiData[0]) == 0) {
+				memcpy(pCharBuff, &uiData[0], uiBytesRemaining); /* copy only bytes requested. */
 				uiBytesRemaining = 0;
-			}
-			else
-			{
+			} else {
 				uiFailureRetries++;
-				mdelay(3);//sleep for a while before retry...
+				mdelay(3); /* sleep for a while before retry... */
 			}
 		}
-
 	}
 
 	return 0;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemFlashBulkRead
-//
-// Description: Reads the FLASH and returns the Data.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		pBuffer    - Buffer to store the data read from FLASH
-//		uiOffset   - Offset of FLASH from where data should be read
-//		uiNumBytes - Number of bytes to be read from the FLASH.
-//
-// Returns:
-//		OSAL_STATUS_SUCCESS - if FLASH read is successful.
-//		<FAILURE>			- if failed.
-//-----------------------------------------------------------------------------
-
-static INT BeceemFlashBulkRead(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes)
+/*
+ * Procedure:	BeceemFlashBulkRead
+ *
+ * Description: Reads the FLASH and returns the Data.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		pBuffer    - Buffer to store the data read from FLASH
+ *		uiOffset   - Offset of FLASH from where data should be read
+ *		uiNumBytes - Number of bytes to be read from the FLASH.
+ *
+ * Returns:
+ *		OSAL_STATUS_SUCCESS - if FLASH read is successful.
+ *		<FAILURE>			- if failed.
+ */
+
+static int BeceemFlashBulkRead(struct bcm_mini_adapter *Adapter,
+			PUINT pBuffer,
+			unsigned int uiOffset,
+			unsigned int uiNumBytes)
 {
-	UINT uiIndex = 0;
-	UINT uiBytesToRead = uiNumBytes;
-	INT Status = 0;
-	UINT uiPartOffset = 0;
+	unsigned int uiIndex = 0;
+	unsigned int uiBytesToRead = uiNumBytes;
+	int Status = 0;
+	unsigned int uiPartOffset = 0;
 	int bytes;
 
-	if(Adapter->device_removed )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Device Got Removed ");
+	if (Adapter->device_removed) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Device Got Removed");
 		return -ENODEV;
 	}
 
-	//Adding flash Base address
-//	uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
-#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-  Status = bcmflash_raw_read((uiOffset/FLASH_PART_SIZE),(uiOffset % FLASH_PART_SIZE),( unsigned char *)pBuffer,uiNumBytes);
-  return Status;
-#endif
+	/* Adding flash Base address
+	 * uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
+	 */
+	#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
+		Status = bcmflash_raw_read((uiOffset/FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
+		return Status;
+	#endif
 
 	Adapter->SelectedChip = RESET_CHIP_SELECT;
 
-	if(uiOffset % MAX_RW_SIZE)
-	{
-		BcmDoChipSelect(Adapter,uiOffset);
+	if (uiOffset % MAX_RW_SIZE) {
+		BcmDoChipSelect(Adapter, uiOffset);
 		uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
 
-		uiBytesToRead = MAX_RW_SIZE - (uiOffset%MAX_RW_SIZE);
-		uiBytesToRead = MIN(uiNumBytes,uiBytesToRead);
+		uiBytesToRead = MAX_RW_SIZE - (uiOffset % MAX_RW_SIZE);
+		uiBytesToRead = MIN(uiNumBytes, uiBytesToRead);
 
-		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer+uiIndex, uiBytesToRead);
+		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer + uiIndex, uiBytesToRead);
 		if (bytes < 0) {
 			Status = bytes;
 			Adapter->SelectedChip = RESET_CHIP_SELECT;
@@ -482,142 +436,122 @@ static INT BeceemFlashBulkRead(
 		uiNumBytes -= uiBytesToRead;
 	}
 
-	while(uiNumBytes)
-	{
-		BcmDoChipSelect(Adapter,uiOffset);
+	while (uiNumBytes) {
+		BcmDoChipSelect(Adapter, uiOffset);
 		uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
 
-		uiBytesToRead = MIN(uiNumBytes,MAX_RW_SIZE);
+		uiBytesToRead = MIN(uiNumBytes, MAX_RW_SIZE);
 
-		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer+uiIndex, uiBytesToRead);
+		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer + uiIndex, uiBytesToRead);
 		if (bytes < 0) {
 			Status = bytes;
 			break;
 		}
 
-
 		uiIndex += uiBytesToRead;
 		uiOffset += uiBytesToRead;
 		uiNumBytes -= uiBytesToRead;
-
 	}
 	Adapter->SelectedChip = RESET_CHIP_SELECT;
 	return Status;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmGetFlashSize
-//
-// Description: Finds the size of FLASH.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		UINT - size of the FLASH Storage.
-//
-//-----------------------------------------------------------------------------
-
-static UINT BcmGetFlashSize(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	BcmGetFlashSize
+ *
+ * Description: Finds the size of FLASH.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		unsigned int - size of the FLASH Storage.
+ *
+ */
+
+static unsigned int BcmGetFlashSize(struct bcm_mini_adapter *Adapter)
 {
-	if(IsFlash2x(Adapter))
-		return 	(Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER));
+	if (IsFlash2x(Adapter))
+		return Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER);
 	else
-		return 32*1024;
-
-
+		return 32 * 1024;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmGetEEPROMSize
-//
-// Description: Finds the size of EEPROM.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		UINT - size of the EEPROM Storage.
-//
-//-----------------------------------------------------------------------------
-
-static UINT BcmGetEEPROMSize(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	BcmGetEEPROMSize
+ *
+ * Description: Finds the size of EEPROM.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		unsigned int - size of the EEPROM Storage.
+ *
+ */
+
+static unsigned int BcmGetEEPROMSize(struct bcm_mini_adapter *Adapter)
 {
-	UINT uiData = 0;
-	UINT uiIndex = 0;
-
-//
-// if EEPROM is present and already Calibrated,it will have
-// 'BECM' string at 0th offset.
-//	To find the EEPROM size read the possible boundaries of the
-// EEPROM like 4K,8K etc..accessing the EEPROM beyond its size will
-// result in wrap around. So when we get the End of the EEPROM we will
-// get 'BECM' string which is indeed at offset 0.
-//
-	BeceemEEPROMBulkRead(Adapter,&uiData,0x0,4);
-	if(uiData == BECM)
-	{
-		for(uiIndex = 2;uiIndex <=256; uiIndex*=2)
-		{
-			BeceemEEPROMBulkRead(Adapter,&uiData,uiIndex*1024,4);
-			if(uiData == BECM)
-			{
-				return uiIndex*1024;
-			}
-		}
-	}
-	else
-	{
-//
-// EEPROM may not be present or not programmed
-//
+	unsigned int uiData = 0;
+	unsigned int uiIndex = 0;
 
-        uiData = 0xBABEFACE;
-		if(0 == BeceemEEPROMBulkWrite(Adapter,(PUCHAR)&uiData,0,4,TRUE))
-		{
+	/*
+	 * if EEPROM is present and already Calibrated,it will have
+	 * 'BECM' string at 0th offset.
+	 * To find the EEPROM size read the possible boundaries of the
+	 * EEPROM like 4K,8K etc..accessing the EEPROM beyond its size will
+	 * result in wrap around. So when we get the End of the EEPROM we will
+	 * get 'BECM' string which is indeed at offset 0.
+	 */
+	BeceemEEPROMBulkRead(Adapter, &uiData, 0x0, 4);
+	if (uiData == BECM) {
+		for (uiIndex = 2; uiIndex <= 256; uiIndex *= 2)	{
+			BeceemEEPROMBulkRead(Adapter, &uiData, uiIndex * 1024, 4);
+			if (uiData == BECM)
+				return uiIndex * 1024;
+		}
+	} else {
+		/*
+		 * EEPROM may not be present or not programmed
+		 */
+		uiData = 0xBABEFACE;
+		if (BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&uiData, 0, 4, TRUE) == 0) {
 			uiData = 0;
-			for(uiIndex = 2;uiIndex <=256; uiIndex*=2)
-			{
-				BeceemEEPROMBulkRead(Adapter,&uiData,uiIndex*1024,4);
-				if(uiData == 0xBABEFACE)
-				{
-					return uiIndex*1024;
-				}
+			for (uiIndex = 2; uiIndex <= 256; uiIndex *= 2) {
+				BeceemEEPROMBulkRead(Adapter, &uiData, uiIndex * 1024, 4);
+				if (uiData == 0xBABEFACE)
+					return uiIndex * 1024;
 			}
 		}
-
 	}
 	return 0;
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:	FlashSectorErase
-//
-// Description: Finds the sector size of the FLASH.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		addr	   - sector start address
-//		numOfSectors - number of sectors to  be erased.
-//
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-
-static INT FlashSectorErase(PMINI_ADAPTER Adapter,
-	UINT addr,
-	UINT numOfSectors)
+/*
+ * Procedure:	FlashSectorErase
+ *
+ * Description: Finds the sector size of the FLASH.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		addr	   - sector start address
+ *		numOfSectors - number of sectors to  be erased.
+ *
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int FlashSectorErase(struct bcm_mini_adapter *Adapter,
+			unsigned int addr,
+			unsigned int numOfSectors)
 {
-	UINT iIndex = 0, iRetries = 0;
-	UINT uiStatus = 0;
-	UINT value;
+	unsigned int iIndex = 0, iRetries = 0;
+	unsigned int uiStatus = 0;
+	unsigned int value;
 	int bytes;
 
-	for(iIndex=0;iIndex<numOfSectors;iIndex++)
-	{
+	for (iIndex = 0; iIndex < numOfSectors; iIndex++) {
 		value = 0x06000000;
 		wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 
@@ -625,12 +559,10 @@ static INT FlashSectorErase(PMINI_ADAPTER Adapter,
 		wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 		iRetries = 0;
 
-		do
-		{
+		do {
 			value = (FLASH_CMD_STATUS_REG_READ << 24);
-			if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programing of FLASH_SPI_CMDQ_REG fails");
+			if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
 				return STATUS_FAILURE;
 			}
 
@@ -641,15 +573,15 @@ static INT FlashSectorErase(PMINI_ADAPTER Adapter,
 				return uiStatus;
 			}
 			iRetries++;
-			//After every try lets make the CPU free for 10 ms. generally time taken by the
-			//the sector erase cycle is 500 ms to 40000 msec. hence sleeping 10 ms
-			//won't hamper performance in any case.
-			msleep(10);
-		}while((uiStatus & 0x1) && (iRetries < 400));
-
-		if(uiStatus & 0x1)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"iRetries crossing the limit of 80000\n");
+			/* After every try lets make the CPU free for 10 ms. generally time taken by the
+			 * the sector erase cycle is 500 ms to 40000 msec. hence sleeping 10 ms
+			 * won't hamper performance in any case.
+			 */
+			udelay(10000);
+		} while ((uiStatus & 0x1) && (iRetries < 400));
+
+		if (uiStatus & 0x1) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "iRetries crossing the limit of 80000\n");
 			return STATUS_FAILURE;
 		}
 
@@ -657,158 +589,137 @@ static INT FlashSectorErase(PMINI_ADAPTER Adapter,
 	}
 	return 0;
 }
-//-----------------------------------------------------------------------------
-// Procedure:	flashByteWrite
-//
-// Description: Performs Byte by Byte write to flash
-//
-// Arguments:
-//		Adapter   - ptr to Adapter object instance
-//		uiOffset   - Offset of the flash where data needs to be written to.
-//		pData	- Address of Data to be written.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static INT flashByteWrite(
-	PMINI_ADAPTER Adapter,
-	UINT uiOffset,
-	PVOID pData)
+/*
+ * Procedure:	flashByteWrite
+ *
+ * Description: Performs Byte by Byte write to flash
+ *
+ * Arguments:
+ *		Adapter   - ptr to Adapter object instance
+ *		uiOffset   - Offset of the flash where data needs to be written to.
+ *		pData	- Address of Data to be written.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int flashByteWrite(struct bcm_mini_adapter *Adapter,
+			unsigned int uiOffset,
+			PVOID pData)
 {
-
-	UINT uiStatus = 0;
-	INT  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; //3
-
-	UINT value;
+	unsigned int uiStatus = 0;
+	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
+	unsigned int value;
 	ULONG ulData = *(PUCHAR)pData;
 	int bytes;
+	/*
+	 * need not write 0xFF because write requires an erase and erase will
+	 * make whole sector 0xFF.
+	 */
 
-//
-// need not write 0xFF because write requires an erase and erase will
-// make whole sector 0xFF.
-//
-
-	if(0xFF == ulData)
-	{
+	if (0xFF == ulData)
 		return STATUS_SUCCESS;
-	}
 
-//	DumpDebug(NVM_RW,("flashWrite ====>\n"));
+	/* DumpDebug(NVM_RW,("flashWrite ====>\n")); */
 	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value)) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Write enable in FLASH_SPI_CMDQ_REG register fails");
+	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write enable in FLASH_SPI_CMDQ_REG register fails");
 		return STATUS_FAILURE;
 	}
-	if(wrm(Adapter,FLASH_SPI_WRITEQ_REG, (PCHAR)&ulData, 4) < 0 )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"DATA Write on FLASH_SPI_WRITEQ_REG fails");
+
+	if (wrm(Adapter, FLASH_SPI_WRITEQ_REG, (PCHAR)&ulData, 4) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "DATA Write on FLASH_SPI_WRITEQ_REG fails");
 		return STATUS_FAILURE;
 	}
 	value = (0x02000000 | (uiOffset & 0xFFFFFF));
-	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value)) < 0 )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programming of FLASH_SPI_CMDQ_REG fails");
+	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programming of FLASH_SPI_CMDQ_REG fails");
 		return STATUS_FAILURE;
 	}
 
-	//__udelay(950);
+	/* __udelay(950); */
 
-	do
-	{
+	do {
 		value = (FLASH_CMD_STATUS_REG_READ << 24);
-	  	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0)
-	  	{
-	  		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programing of FLASH_SPI_CMDQ_REG fails");
+		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
 			return STATUS_FAILURE;
-	  	}
-	  	//__udelay(1);
+		}
+		/* __udelay(1); */
 		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
 		if (bytes < 0) {
 			uiStatus = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
 			return uiStatus;
 		}
-	  	iRetries--;
-		if( iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-			 msleep(1);
+		iRetries--;
+		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
+			udelay(1000);
 
-	}while((uiStatus & 0x1) && (iRetries  >0) );
+	} while ((uiStatus & 0x1) && (iRetries  > 0));
 
-	if(uiStatus & 0x1)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE ;
+	if (uiStatus & 0x1) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
+		return STATUS_FAILURE;
 	}
 
 	return STATUS_SUCCESS;
 }
 
-
-
-//-----------------------------------------------------------------------------
-// Procedure:	flashWrite
-//
-// Description: Performs write to flash
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		uiOffset   - Offset of the flash where data needs to be written to.
-//		pData	- Address of Data to be written.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static INT flashWrite(
-	PMINI_ADAPTER Adapter,
-	UINT uiOffset,
-	PVOID pData)
-
+/*
+ * Procedure:	flashWrite
+ *
+ * Description: Performs write to flash
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		uiOffset   - Offset of the flash where data needs to be written to.
+ *		pData	- Address of Data to be written.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int flashWrite(struct bcm_mini_adapter *Adapter,
+		unsigned int uiOffset,
+		PVOID pData)
 {
-	//UINT uiStatus = 0;
-	//INT  iRetries = 0;
-	//UINT uiReadBack = 0;
-
-	UINT uiStatus = 0;
-	INT  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; //3
-
-	UINT value;
-	UINT uiErasePattern[4] = {0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF};
+	/* unsigned int uiStatus = 0;
+	 * int  iRetries = 0;
+	 * unsigned int uiReadBack = 0;
+	 */
+	unsigned int uiStatus = 0;
+	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
+	unsigned int value;
+	unsigned int uiErasePattern[4] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF};
 	int bytes;
-//
-// need not write 0xFFFFFFFF because write requires an erase and erase will
-// make whole sector 0xFFFFFFFF.
-//
+	/*
+	 * need not write 0xFFFFFFFF because write requires an erase and erase will
+	 * make whole sector 0xFFFFFFFF.
+	 */
 	if (!memcmp(pData, uiErasePattern, MAX_RW_SIZE))
-	{
 		return 0;
-	}
 
 	value = (FLASH_CMD_WRITE_ENABLE << 24);
 
-	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value)) < 0 )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Write Enable of FLASH_SPI_CMDQ_REG fails");
+	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write Enable of FLASH_SPI_CMDQ_REG fails");
 		return STATUS_FAILURE;
 	}
-	if(wrm(Adapter, uiOffset, (PCHAR)pData, MAX_RW_SIZE) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Data write fails...");
+
+	if (wrm(Adapter, uiOffset, (PCHAR)pData, MAX_RW_SIZE) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Data write fails...");
 		return STATUS_FAILURE;
 	}
 
-	//__udelay(950);
-	do
-	{
+	/* __udelay(950); */
+	do {
 		value = (FLASH_CMD_STATUS_REG_READ << 24);
-	  	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0)
-	  	{
-	  		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programing of FLASH_SPI_CMDQ_REG fails");
+		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
 			return STATUS_FAILURE;
-	  	}
-	  	//__udelay(1);
+		}
+		/* __udelay(1); */
 		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
 		if (bytes < 0) {
 			uiStatus = bytes;
@@ -817,88 +728,80 @@ static INT flashWrite(
 		}
 
 		iRetries--;
-		//this will ensure that in there will be no changes in the current path.
-		//currently one rdm/wrm takes 125 us.
-		//Hence  125 *2 * FLASH_PER_RETRIES_DELAY > 3 ms(worst case delay)
-		//Hence current implementation cycle will intoduce no delay in current path
-		if(iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-				msleep(1);
-	}while((uiStatus & 0x1) && (iRetries > 0));
-
-	if(uiStatus & 0x1)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE ;
+		/* this will ensure that in there will be no changes in the current path.
+		 * currently one rdm/wrm takes 125 us.
+		 * Hence  125 *2 * FLASH_PER_RETRIES_DELAY > 3 ms(worst case delay)
+		 * Hence current implementation cycle will intoduce no delay in current path
+		 */
+		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
+			udelay(1000);
+	} while ((uiStatus & 0x1) && (iRetries > 0));
+
+	if (uiStatus & 0x1) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
+		return STATUS_FAILURE;
 	}
 
 	return STATUS_SUCCESS;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	flashByteWriteStatus
-//
-// Description: Performs byte by byte write to flash with write done status check
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		uiOffset    - Offset of the flash where data needs to be written to.
-//		pData	 - Address of the Data to be written.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-static INT flashByteWriteStatus(
-	PMINI_ADAPTER Adapter,
-	UINT uiOffset,
-	PVOID pData)
+/*-----------------------------------------------------------------------------
+ * Procedure:	flashByteWriteStatus
+ *
+ * Description: Performs byte by byte write to flash with write done status check
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		uiOffset    - Offset of the flash where data needs to be written to.
+ *		pData	 - Address of the Data to be written.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+static int flashByteWriteStatus(struct bcm_mini_adapter *Adapter,
+				unsigned int uiOffset,
+				PVOID pData)
 {
-	UINT uiStatus = 0;
-	INT  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; //3
+	unsigned int uiStatus = 0;
+	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
 	ULONG ulData  = *(PUCHAR)pData;
-	UINT value;
+	unsigned int value;
 	int bytes;
 
-//
-// need not write 0xFFFFFFFF because write requires an erase and erase will
-// make whole sector 0xFFFFFFFF.
-//
+	/*
+	 * need not write 0xFFFFFFFF because write requires an erase and erase will
+	 * make whole sector 0xFFFFFFFF.
+	 */
 
-	if(0xFF == ulData)
-	{
+	if (0xFF == ulData)
 		return STATUS_SUCCESS;
-	}
 
-	//	DumpDebug(NVM_RW,("flashWrite ====>\n"));
+	/* DumpDebug(NVM_RW,("flashWrite ====>\n")); */
 
 	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value)) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Write enable in FLASH_SPI_CMDQ_REG register fails");
+	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write enable in FLASH_SPI_CMDQ_REG register fails");
 		return STATUS_SUCCESS;
 	}
-	if(wrm(Adapter,FLASH_SPI_WRITEQ_REG, (PCHAR)&ulData, 4) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"DATA Write on FLASH_SPI_WRITEQ_REG fails");
+	if (wrm(Adapter, FLASH_SPI_WRITEQ_REG, (PCHAR)&ulData, 4) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "DATA Write on FLASH_SPI_WRITEQ_REG fails");
 		return STATUS_FAILURE;
 	}
 	value = (0x02000000 | (uiOffset & 0xFFFFFF));
-	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value)) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programming of FLASH_SPI_CMDQ_REG fails");
+	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programming of FLASH_SPI_CMDQ_REG fails");
 		return STATUS_FAILURE;
 	}
 
-    //msleep(1);
+	/* msleep(1); */
 
-	do
-	{
+	do {
 		value = (FLASH_CMD_STATUS_REG_READ << 24);
-		if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programing of FLASH_SPI_CMDQ_REG fails");
+		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
 			return STATUS_FAILURE;
 		}
-		//__udelay(1);
+		/* __udelay(1); */
 		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
 		if (bytes < 0) {
 			uiStatus = bytes;
@@ -907,405 +810,361 @@ static INT flashByteWriteStatus(
 		}
 
 		iRetries--;
-		if( iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-				msleep(1);
-	}while((uiStatus & 0x1) && (iRetries > 0));
+		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
+			udelay(1000);
 
-	if(uiStatus & 0x1)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE ;
+	} while ((uiStatus & 0x1) && (iRetries > 0));
+
+	if (uiStatus & 0x1) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
+		return STATUS_FAILURE;
 	}
 
 	return STATUS_SUCCESS;
-
 }
-//-----------------------------------------------------------------------------
-// Procedure:	flashWriteStatus
-//
-// Description: Performs write to flash with write done status check
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		uiOffset    - Offset of the flash where data needs to be written to.
-//		pData	 - Address of the Data to be written.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static INT flashWriteStatus(
-	PMINI_ADAPTER Adapter,
-	UINT uiOffset,
-	PVOID pData)
+/*
+ * Procedure:	flashWriteStatus
+ *
+ * Description: Performs write to flash with write done status check
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		uiOffset    - Offset of the flash where data needs to be written to.
+ *		pData	 - Address of the Data to be written.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int flashWriteStatus(struct bcm_mini_adapter *Adapter,
+			unsigned int uiOffset,
+			PVOID pData)
 {
-	UINT uiStatus = 0;
-	INT  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; //3
-	//UINT uiReadBack = 0;
-	UINT value;
-	UINT uiErasePattern[4] = {0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF};
+	unsigned int uiStatus = 0;
+	int  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; /* 3 */
+	/* unsigned int uiReadBack = 0; */
+	unsigned int value;
+	unsigned int uiErasePattern[4] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF};
 	int bytes;
 
-//
-// need not write 0xFFFFFFFF because write requires an erase and erase will
-// make whole sector 0xFFFFFFFF.
-//
-	if (!memcmp(pData,uiErasePattern,MAX_RW_SIZE))
-	{
+	/*
+	 * need not write 0xFFFFFFFF because write requires an erase and erase will
+	 * make whole sector 0xFFFFFFFF.
+	 */
+	if (!memcmp(pData, uiErasePattern, MAX_RW_SIZE))
 		return 0;
-	}
 
 	value = (FLASH_CMD_WRITE_ENABLE << 24);
-	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value)) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Write Enable of FLASH_SPI_CMDQ_REG fails");
+	if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write Enable of FLASH_SPI_CMDQ_REG fails");
 		return STATUS_FAILURE;
 	}
-	if(wrm(Adapter, uiOffset, (PCHAR)pData, MAX_RW_SIZE) < 0)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Data write fails...");
+
+	if (wrm(Adapter, uiOffset, (PCHAR)pData, MAX_RW_SIZE) < 0) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Data write fails...");
 		return STATUS_FAILURE;
 	}
-   // __udelay(1);
+	/* __udelay(1); */
 
-	do
-	{
+	do {
 		value = (FLASH_CMD_STATUS_REG_READ << 24);
-	  	if(wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0)
-	  	{
-	  		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Programing of FLASH_SPI_CMDQ_REG fails");
+		if (wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value)) < 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Programing of FLASH_SPI_CMDQ_REG fails");
 			return STATUS_FAILURE;
-	  	}
-	  	//__udelay(1);
+		}
+		/* __udelay(1); */
 		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
 		if (bytes < 0) {
 			uiStatus = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
 			return uiStatus;
 		}
-	  	iRetries--;
-		//this will ensure that in there will be no changes in the current path.
-		//currently one rdm/wrm takes 125 us.
-		//Hence  125 *2  * FLASH_PER_RETRIES_DELAY  >3 ms(worst case delay)
-		//Hence current implementation cycle will intoduce no delay in current path
-		if(iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
-				msleep(1);
-	}while((uiStatus & 0x1) && (iRetries >0));
-
-	if(uiStatus & 0x1)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Flash Write fails even after checking status for 200 times.");
-		return STATUS_FAILURE ;
+		iRetries--;
+		/* this will ensure that in there will be no changes in the current path.
+		 * currently one rdm/wrm takes 125 us.
+		 * Hence  125 *2  * FLASH_PER_RETRIES_DELAY  >3 ms(worst case delay)
+		 * Hence current implementation cycle will intoduce no delay in current path
+		 */
+		if (iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
+			udelay(1000);
+
+	} while ((uiStatus & 0x1) && (iRetries > 0));
+
+	if (uiStatus & 0x1) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write fails even after checking status for 200 times.");
+		return STATUS_FAILURE;
 	}
 
 	return STATUS_SUCCESS;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmRestoreBlockProtectStatus
-//
-// Description: Restores the original block protection status.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		ulWriteStatus   -Original status
-// Returns:
-//		<VOID>
-//
-//-----------------------------------------------------------------------------
-
-static VOID BcmRestoreBlockProtectStatus(PMINI_ADAPTER Adapter,ULONG ulWriteStatus)
+/*
+ * Procedure:	BcmRestoreBlockProtectStatus
+ *
+ * Description: Restores the original block protection status.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		ulWriteStatus   -Original status
+ * Returns:
+ *		<VOID>
+ *
+ */
+
+static VOID BcmRestoreBlockProtectStatus(struct bcm_mini_adapter *Adapter, ULONG ulWriteStatus)
 {
-	UINT value;
-	value = (FLASH_CMD_WRITE_ENABLE<< 24);
+	unsigned int value;
+	value = (FLASH_CMD_WRITE_ENABLE << 24);
 	wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 
 	udelay(20);
-	value = (FLASH_CMD_STATUS_REG_WRITE<<24)|(ulWriteStatus << 16);
+	value = (FLASH_CMD_STATUS_REG_WRITE << 24) | (ulWriteStatus << 16);
 	wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 	udelay(20);
 }
-//-----------------------------------------------------------------------------
-// Procedure:	BcmFlashUnProtectBlock
-//
-// Description: UnProtects appropriate blocks for writing.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		uiOffset   - Offset of the flash where data needs to be written to. This should be Sector aligned.
-// Returns:
-//		ULONG   - Status value before UnProtect.
-//
-//-----------------------------------------------------------------------------
-static ULONG BcmFlashUnProtectBlock(PMINI_ADAPTER Adapter,UINT uiOffset, UINT uiLength)
+
+/*
+ * Procedure:	BcmFlashUnProtectBlock
+ *
+ * Description: UnProtects appropriate blocks for writing.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *		uiOffset   - Offset of the flash where data needs to be written to. This should be Sector aligned.
+ * Returns:
+ *		ULONG   - Status value before UnProtect.
+ *
+ */
+
+static ULONG BcmFlashUnProtectBlock(struct bcm_mini_adapter *Adapter, unsigned int uiOffset, unsigned int uiLength)
 {
-	ULONG ulStatus      = 0;
-	ULONG ulWriteStatus = 0;
-	UINT value;
-	uiOffset = uiOffset&0x000FFFFF;
+	ULONG ulStatus		= 0;
+	ULONG ulWriteStatus	= 0;
+	unsigned int value;
 
-//
-// Implemented only for 1MB Flash parts.
-//
-	if(FLASH_PART_SST25VF080B == Adapter->ulFlashID)
-	{
-	//
-	// Get Current BP status.
-	//
+	uiOffset = uiOffset&0x000FFFFF;
+	/*
+	 * Implemented only for 1MB Flash parts.
+	 */
+	if (FLASH_PART_SST25VF080B == Adapter->ulFlashID) {
+		/*
+		 * Get Current BP status.
+		 */
 		value = (FLASH_CMD_STATUS_REG_READ << 24);
 		wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 		udelay(10);
-	//
-	// Read status will be WWXXYYZZ. We have to take only WW.
-	//
+		/*
+		 * Read status will be WWXXYYZZ. We have to take only WW.
+		 */
 		rdmalt(Adapter, FLASH_SPI_READQ_REG, (PUINT)&ulStatus, sizeof(ulStatus));
 		ulStatus >>= 24;
 		ulWriteStatus = ulStatus;
-
-	//
-	// Bits [5-2] give current block level protection status.
-	// Bit5: BP3 - DONT CARE
-	// BP2-BP0: 0 - NO PROTECTION, 1 - UPPER 1/16, 2 - UPPER 1/8, 3 - UPPER 1/4
-	//                4 - UPPER 1/2. 5 to 7 - ALL BLOCKS
-	//
-
-		if(ulStatus)
-		{
-			if((uiOffset+uiLength) <= 0x80000)
-			{
-			//
-			// Offset comes in lower half of 1MB. Protect the upper half.
-			// Clear BP1 and BP0 and set BP2.
-			//
+		/*
+		 * Bits [5-2] give current block level protection status.
+		 * Bit5: BP3 - DONT CARE
+		 * BP2-BP0: 0 - NO PROTECTION, 1 - UPPER 1/16, 2 - UPPER 1/8, 3 - UPPER 1/4
+		 *                4 - UPPER 1/2. 5 to 7 - ALL BLOCKS
+		 */
+
+		if (ulStatus) {
+			if ((uiOffset+uiLength) <= 0x80000) {
+				/*
+				 * Offset comes in lower half of 1MB. Protect the upper half.
+				 * Clear BP1 and BP0 and set BP2.
+				 */
 				ulWriteStatus |= (0x4<<2);
 				ulWriteStatus &= ~(0x3<<2);
-			}
-			else if((uiOffset+uiLength) <= 0xC0000)
-			{
-			//
-			// Offset comes below Upper 1/4. Upper 1/4 can be protected.
-			//  Clear BP2 and set BP1 and BP0.
-			//
+			} else if ((uiOffset + uiLength) <= 0xC0000) {
+				/*
+				 * Offset comes below Upper 1/4. Upper 1/4 can be protected.
+				 *  Clear BP2 and set BP1 and BP0.
+				 */
 				ulWriteStatus |= (0x3<<2);
 				ulWriteStatus &= ~(0x1<<4);
+			} else if ((uiOffset + uiLength) <= 0xE0000) {
+				/*
+				 * Offset comes below Upper 1/8. Upper 1/8 can be protected.
+				 * Clear BP2 and BP0  and set BP1
+				 */
+				ulWriteStatus |= (0x1<<3);
+				ulWriteStatus &= ~(0x5<<2);
+			} else if ((uiOffset + uiLength) <= 0xF0000) {
+				/*
+				 * Offset comes below Upper 1/16. Only upper 1/16 can be protected.
+				 * Set BP0 and Clear BP2,BP1.
+				 */
+				ulWriteStatus |= (0x1<<2);
+				ulWriteStatus &= ~(0x3<<3);
+			} else {
+				/*
+				 * Unblock all.
+				 * Clear BP2,BP1 and BP0.
+				 */
+				ulWriteStatus &= ~(0x7<<2);
 			}
-			else if((uiOffset+uiLength) <= 0xE0000)
-		    {
-		    //
-		    // Offset comes below Upper 1/8. Upper 1/8 can be protected.
-		    // Clear BP2 and BP0  and set BP1
-		    //
-		    	ulWriteStatus |= (0x1<<3);
-		    	ulWriteStatus &= ~(0x5<<2);
-
-		    }
-		    else if((uiOffset+uiLength) <= 0xF0000)
-		    {
-		    //
-		    // Offset comes below Upper 1/16. Only upper 1/16 can be protected.
-		    // Set BP0 and Clear BP2,BP1.
-		    //
-		    	ulWriteStatus |= (0x1<<2);
-		    	ulWriteStatus &= ~(0x3<<3);
-		    }
-		    else
-		    {
-		    //
-		    // Unblock all.
-		    // Clear BP2,BP1 and BP0.
-		    //
-		    	ulWriteStatus &= ~(0x7<<2);
-		    }
-
-			value = (FLASH_CMD_WRITE_ENABLE<< 24);
+
+			value = (FLASH_CMD_WRITE_ENABLE << 24);
 			wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 			udelay(20);
-			value = (FLASH_CMD_STATUS_REG_WRITE<<24)|(ulWriteStatus << 16);
+			value = (FLASH_CMD_STATUS_REG_WRITE << 24) | (ulWriteStatus << 16);
 			wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 			udelay(20);
-
 		}
-
 	}
 	return ulStatus;
 }
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemFlashBulkWrite
-//
-// Description: Performs write to the flash
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		pBuffer 	- Data to be written.
-//		uiOffset   - Offset of the flash where data needs to be written to.
-//		uiNumBytes - Number of bytes to be written.
-//		bVerify    - read verify flag.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static INT BeceemFlashBulkWrite(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	BOOLEAN bVerify)
+
+/*
+ * Procedure:	BeceemFlashBulkWrite
+ *
+ * Description: Performs write to the flash
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ * pBuffer - Data to be written.
+ *		uiOffset   - Offset of the flash where data needs to be written to.
+ *		uiNumBytes - Number of bytes to be written.
+ *		bVerify    - read verify flag.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int BeceemFlashBulkWrite(struct bcm_mini_adapter *Adapter,
+				PUINT pBuffer,
+				unsigned int uiOffset,
+				unsigned int uiNumBytes,
+				BOOLEAN bVerify)
 {
-	PCHAR  pTempBuff 			= NULL;
-	PUCHAR pcBuffer             = (PUCHAR)pBuffer;
-	UINT  uiIndex				= 0;
-	UINT  uiOffsetFromSectStart = 0;
-	UINT  uiSectAlignAddr		= 0;
-	UINT  uiCurrSectOffsetAddr	= 0;
-	UINT  uiSectBoundary		= 0;
-	UINT  uiNumSectTobeRead 	= 0;
-	UCHAR ucReadBk[16]       	= {0};
-	ULONG ulStatus              = 0;
-	INT Status 					= STATUS_SUCCESS;
-	UINT uiTemp 				= 0;
-	UINT index 					= 0;
-	UINT uiPartOffset 			= 0;
-
-#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-  Status = bcmflash_raw_write((uiOffset/FLASH_PART_SIZE),(uiOffset % FLASH_PART_SIZE),( unsigned char *)pBuffer,uiNumBytes);
-  return Status;
-#endif
-
-	uiOffsetFromSectStart 	= uiOffset & ~(Adapter->uiSectorSize - 1);
-
-	//Adding flash Base address
-//	uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
-
-	uiSectAlignAddr   		= uiOffset & ~(Adapter->uiSectorSize - 1);
-	uiCurrSectOffsetAddr	= uiOffset & (Adapter->uiSectorSize - 1);
-	uiSectBoundary	  		= uiSectAlignAddr + Adapter->uiSectorSize;
+	PCHAR pTempBuff			= NULL;
+	PUCHAR pcBuffer			= (PUCHAR)pBuffer;
+	unsigned int uiIndex			= 0;
+	unsigned int uiOffsetFromSectStart	= 0;
+	unsigned int uiSectAlignAddr		= 0;
+	unsigned int uiCurrSectOffsetAddr	= 0;
+	unsigned int uiSectBoundary		= 0;
+	unsigned int uiNumSectTobeRead		= 0;
+	UCHAR ucReadBk[16]		= {0};
+	ULONG ulStatus			= 0;
+	int Status			= STATUS_SUCCESS;
+	unsigned int uiTemp			= 0;
+	unsigned int index			= 0;
+	unsigned int uiPartOffset		= 0;
+
+	#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
+		Status = bcmflash_raw_write((uiOffset / FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
+		return Status;
+	#endif
+
+	uiOffsetFromSectStart = uiOffset & ~(Adapter->uiSectorSize - 1);
+
+	/* Adding flash Base address
+	 * uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
+	 */
+
+	uiSectAlignAddr	= uiOffset & ~(Adapter->uiSectorSize - 1);
+	uiCurrSectOffsetAddr = uiOffset & (Adapter->uiSectorSize - 1);
+	uiSectBoundary = uiSectAlignAddr + Adapter->uiSectorSize;
 
 	pTempBuff = kmalloc(Adapter->uiSectorSize, GFP_KERNEL);
-	if(NULL == pTempBuff)
+	if (!pTempBuff)
 		goto BeceemFlashBulkWrite_EXIT;
-//
-// check if the data to be written is overlapped across sectors
-//
-	if(uiOffset+uiNumBytes < uiSectBoundary)
-	{
+	/*
+	 * check if the data to be written is overlapped across sectors
+	 */
+	if (uiOffset+uiNumBytes < uiSectBoundary) {
 		uiNumSectTobeRead = 1;
-	}
-	else
-	{
-		//      Number of sectors  = Last sector start address/First sector start address
-		uiNumSectTobeRead =  (uiCurrSectOffsetAddr+uiNumBytes)/Adapter->uiSectorSize;
-		if((uiCurrSectOffsetAddr+uiNumBytes)%Adapter->uiSectorSize)
-		{
+	} else {
+		/* Number of sectors  = Last sector start address/First sector start address */
+		uiNumSectTobeRead =  (uiCurrSectOffsetAddr + uiNumBytes) / Adapter->uiSectorSize;
+		if ((uiCurrSectOffsetAddr + uiNumBytes)%Adapter->uiSectorSize)
 			uiNumSectTobeRead++;
-		}
 	}
-	//Check whether Requested sector is writable or not in case of flash2x write. But if  write call is
-	// for DSD calibration, allow it without checking of sector permission
+	/* Check whether Requested sector is writable or not in case of flash2x write. But if  write call is
+	 * for DSD calibration, allow it without checking of sector permission
+	 */
 
-	if(IsFlash2x(Adapter) && (Adapter->bAllDSDWriteAllow == FALSE))
-	{
+	if (IsFlash2x(Adapter) && (Adapter->bAllDSDWriteAllow == FALSE)) {
 		index = 0;
-		uiTemp = uiNumSectTobeRead ;
-		while(uiTemp)
-		{
-			 if(IsOffsetWritable(Adapter, uiOffsetFromSectStart + index * Adapter->uiSectorSize ) == FALSE)
-			 {
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Sector Starting at offset <0X%X> is not writable",
-											(uiOffsetFromSectStart + index * Adapter->uiSectorSize));
+		uiTemp = uiNumSectTobeRead;
+		while (uiTemp) {
+			if (IsOffsetWritable(Adapter, uiOffsetFromSectStart + index * Adapter->uiSectorSize) == FALSE) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Sector Starting at offset <0X%X> is not writable",
+						(uiOffsetFromSectStart + index * Adapter->uiSectorSize));
 				Status = SECTOR_IS_NOT_WRITABLE;
 				goto BeceemFlashBulkWrite_EXIT;
-			 }
-			 uiTemp = uiTemp - 1;
-			 index = index + 1 ;
+			}
+			uiTemp = uiTemp - 1;
+			index = index + 1 ;
 		}
 	}
 	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	while(uiNumSectTobeRead)
-	{
-		//do_gettimeofday(&tv1);
-		//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "\nTime In start of write :%ld ms\n",(tv1.tv_sec *1000 + tv1.tv_usec /1000));
+	while (uiNumSectTobeRead) {
+		/* do_gettimeofday(&tv1);
+		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "\nTime In start of write :%ld ms\n",(tv1.tv_sec *1000 + tv1.tv_usec /1000));
+		 */
 		uiPartOffset = (uiSectAlignAddr & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
 
-		BcmDoChipSelect(Adapter,uiSectAlignAddr);
+		BcmDoChipSelect(Adapter, uiSectAlignAddr);
 
-		if(0 != BeceemFlashBulkRead(Adapter,
+		if (0 != BeceemFlashBulkRead(Adapter,
 						(PUINT)pTempBuff,
 						uiOffsetFromSectStart,
-						Adapter->uiSectorSize))
-		{
+						Adapter->uiSectorSize)) {
 			Status = -1;
 			goto BeceemFlashBulkWrite_EXIT;
 		}
 
-		//do_gettimeofday(&tr);
-		//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by Read :%ld ms\n", (tr.tv_sec *1000 + tr.tv_usec/1000) - (tv1.tv_sec *1000 + tv1.tv_usec/1000));
-
-		ulStatus = BcmFlashUnProtectBlock(Adapter,uiSectAlignAddr,Adapter->uiSectorSize);
-
-
-		if(uiNumSectTobeRead > 1)
-		{
-
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr],pcBuffer,uiSectBoundary-(uiSectAlignAddr+uiCurrSectOffsetAddr));
-			pcBuffer += ((uiSectBoundary-(uiSectAlignAddr+uiCurrSectOffsetAddr)));
-			uiNumBytes -= (uiSectBoundary-(uiSectAlignAddr+uiCurrSectOffsetAddr));
-		}
-		else
-		{
-				memcpy(&pTempBuff[uiCurrSectOffsetAddr],pcBuffer,uiNumBytes);
-		}
-
-		if(IsFlash2x(Adapter))
-		{
-			SaveHeaderIfPresent(Adapter,(PUCHAR)pTempBuff,uiOffsetFromSectStart);
+		/* do_gettimeofday(&tr);
+		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by Read :%ld ms\n", (tr.tv_sec *1000 + tr.tv_usec/1000) - (tv1.tv_sec *1000 + tv1.tv_usec/1000));
+		 */
+		ulStatus = BcmFlashUnProtectBlock(Adapter, uiSectAlignAddr, Adapter->uiSectorSize);
+
+		if (uiNumSectTobeRead > 1) {
+			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
+			pcBuffer += ((uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr)));
+			uiNumBytes -= (uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
+		} else {
+			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiNumBytes);
 		}
 
-		FlashSectorErase(Adapter,uiPartOffset,1);
-		//do_gettimeofday(&te);
-		//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by Erase :%ld ms\n", (te.tv_sec *1000 + te.tv_usec/1000) - (tr.tv_sec *1000 + tr.tv_usec/1000));
+		if (IsFlash2x(Adapter))
+			SaveHeaderIfPresent(Adapter, (PUCHAR)pTempBuff, uiOffsetFromSectStart);
 
-		for(uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex +=Adapter->ulFlashWriteSize)
-		{
-			if(Adapter->device_removed)
-			{
+		FlashSectorErase(Adapter, uiPartOffset, 1);
+		/* do_gettimeofday(&te);
+		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by Erase :%ld ms\n", (te.tv_sec *1000 + te.tv_usec/1000) - (tr.tv_sec *1000 + tr.tv_usec/1000));
+		 */
+		for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += Adapter->ulFlashWriteSize) {
+			if (Adapter->device_removed) {
 				Status = -1;
 				goto BeceemFlashBulkWrite_EXIT;
 			}
-			if(STATUS_SUCCESS != (*Adapter->fpFlashWrite)(Adapter,uiPartOffset+uiIndex,(&pTempBuff[uiIndex])))
-			{
+
+			if (STATUS_SUCCESS != (*Adapter->fpFlashWrite)(Adapter, uiPartOffset + uiIndex, (&pTempBuff[uiIndex]))) {
 				Status = -1;
 				goto BeceemFlashBulkWrite_EXIT;
 			}
 		}
 
-		//do_gettimeofday(&tw);
-		//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken in Write  to Flash :%ld ms\n", (tw.tv_sec *1000 + tw.tv_usec/1000) - (te.tv_sec *1000 + te.tv_usec/1000));
-		for(uiIndex = 0;uiIndex < Adapter->uiSectorSize;uiIndex += MAX_RW_SIZE)
-		{
-			if(STATUS_SUCCESS == BeceemFlashBulkRead(Adapter,(PUINT)ucReadBk,uiOffsetFromSectStart+uiIndex,MAX_RW_SIZE))
-			{
-				if(Adapter->ulFlashWriteSize == 1)
-				{
-					UINT uiReadIndex = 0;
-					for(uiReadIndex = 0; uiReadIndex < 16; uiReadIndex++)
-					{
-						if(ucReadBk[uiReadIndex] != pTempBuff[uiIndex+uiReadIndex])
-						{
-							if(STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter,uiPartOffset+uiIndex+uiReadIndex,&pTempBuff[uiIndex+uiReadIndex]))
-							{
+		/* do_gettimeofday(&tw);
+		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken in Write  to Flash :%ld ms\n", (tw.tv_sec *1000 + tw.tv_usec/1000) - (te.tv_sec *1000 + te.tv_usec/1000));
+		 */
+		for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += MAX_RW_SIZE) {
+			if (STATUS_SUCCESS == BeceemFlashBulkRead(Adapter, (PUINT)ucReadBk, uiOffsetFromSectStart + uiIndex, MAX_RW_SIZE)) {
+				if (Adapter->ulFlashWriteSize == 1) {
+					unsigned int uiReadIndex = 0;
+					for (uiReadIndex = 0; uiReadIndex < 16; uiReadIndex++) {
+						if (ucReadBk[uiReadIndex] != pTempBuff[uiIndex + uiReadIndex]) {
+							if (STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter, uiPartOffset + uiIndex + uiReadIndex, &pTempBuff[uiIndex+uiReadIndex])) {
 								Status = STATUS_FAILURE;
 								goto BeceemFlashBulkWrite_EXIT;
 							}
 						}
 					}
-				}
-				else
-				{
-					if(memcmp(ucReadBk,&pTempBuff[uiIndex],MAX_RW_SIZE))
-					{
-						if(STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter,uiPartOffset+uiIndex,&pTempBuff[uiIndex]))
-						{
+				} else {
+					if (memcmp(ucReadBk, &pTempBuff[uiIndex], MAX_RW_SIZE)) {
+						if (STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter, uiPartOffset + uiIndex, &pTempBuff[uiIndex])) {
 							Status = STATUS_FAILURE;
 							goto BeceemFlashBulkWrite_EXIT;
 						}
@@ -1313,13 +1172,11 @@ static INT BeceemFlashBulkWrite(
 				}
 			}
 		}
-		//do_gettimeofday(&twv);
-		//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken in Write  to Flash verification :%ld ms\n", (twv.tv_sec *1000 + twv.tv_usec/1000) - (tw.tv_sec *1000 + tw.tv_usec/1000));
-
-
-		if(ulStatus)
-		{
-			BcmRestoreBlockProtectStatus(Adapter,ulStatus);
+		/* do_gettimeofday(&twv);
+		 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken in Write  to Flash verification :%ld ms\n", (twv.tv_sec *1000 + twv.tv_usec/1000) - (tw.tv_sec *1000 + tw.tv_usec/1000));
+		 */
+		if (ulStatus) {
+			BcmRestoreBlockProtectStatus(Adapter, ulStatus);
 			ulStatus = 0;
 		}
 
@@ -1329,185 +1186,153 @@ static INT BeceemFlashBulkWrite(
 		uiOffsetFromSectStart += Adapter->uiSectorSize;
 		uiNumSectTobeRead--;
 	}
-	//do_gettimeofday(&tv2);
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Time after Write :%ld ms\n",(tv2.tv_sec *1000 + tv2.tv_usec/1000));
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by in Write is :%ld ms\n", (tv2.tv_sec *1000 + tv2.tv_usec/1000) - (tv1.tv_sec *1000 + tv1.tv_usec/1000));
-//
-// Cleanup.
-//
+	/* do_gettimeofday(&tv2);
+	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Time after Write :%ld ms\n",(tv2.tv_sec *1000 + tv2.tv_usec/1000));
+	 * BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Total time taken by in Write is :%ld ms\n", (tv2.tv_sec *1000 + tv2.tv_usec/1000) - (tv1.tv_sec *1000 + tv1.tv_usec/1000));
+	 *
+	 * Cleanup.
+	 */
 BeceemFlashBulkWrite_EXIT:
-	if(ulStatus)
-	{
-		BcmRestoreBlockProtectStatus(Adapter,ulStatus);
-	}
-	
+	if (ulStatus)
+		BcmRestoreBlockProtectStatus(Adapter, ulStatus);
+
 	kfree(pTempBuff);
 
 	Adapter->SelectedChip = RESET_CHIP_SELECT;
 	return Status;
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemFlashBulkWriteStatus
-//
-// Description: Writes to Flash. Checks the SPI status after each write.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//		pBuffer 	- Data to be written.
-//		uiOffset   - Offset of the flash where data needs to be written to.
-//		uiNumBytes - Number of bytes to be written.
-//		bVerify    - read verify flag.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static INT BeceemFlashBulkWriteStatus(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	BOOLEAN bVerify)
+/*
+ * Procedure:	BeceemFlashBulkWriteStatus
+ *
+ * Description: Writes to Flash. Checks the SPI status after each write.
+ *
+ * Arguments:
+ *		Adapter		- ptr to Adapter object instance
+ *		pBuffer		- Data to be written.
+ *		uiOffset	- Offset of the flash where data needs to be written to.
+ *		uiNumBytes	- Number of bytes to be written.
+ *		bVerify		- read verify flag.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int BeceemFlashBulkWriteStatus(struct bcm_mini_adapter *Adapter,
+				PUINT pBuffer,
+				unsigned int uiOffset,
+				unsigned int uiNumBytes,
+				BOOLEAN bVerify)
 {
-	PCHAR  pTempBuff 			= NULL;
-	PUCHAR pcBuffer             = (PUCHAR)pBuffer;
-	UINT  uiIndex				= 0;
-	UINT  uiOffsetFromSectStart = 0;
-	UINT  uiSectAlignAddr		= 0;
-	UINT  uiCurrSectOffsetAddr	= 0;
-	UINT  uiSectBoundary		= 0;
-	UINT  uiNumSectTobeRead 	= 0;
-	UCHAR ucReadBk[16]			= {0};
-	ULONG ulStatus              = 0;
-	UINT  Status				= STATUS_SUCCESS;
-	UINT uiTemp 				= 0;
-	UINT index 					= 0;
-	UINT uiPartOffset			= 0;
-
-	uiOffsetFromSectStart 	= uiOffset & ~(Adapter->uiSectorSize - 1);
-
-	//uiOffset += Adapter->ulFlashCalStart;
-	//Adding flash Base address
-//	uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
-
-	uiSectAlignAddr 		= uiOffset & ~(Adapter->uiSectorSize - 1);
-	uiCurrSectOffsetAddr	= uiOffset & (Adapter->uiSectorSize - 1);
-	uiSectBoundary			= uiSectAlignAddr + Adapter->uiSectorSize;
+	PCHAR pTempBuff			= NULL;
+	PUCHAR pcBuffer			= (PUCHAR)pBuffer;
+	unsigned int uiIndex			= 0;
+	unsigned int uiOffsetFromSectStart	= 0;
+	unsigned int uiSectAlignAddr		= 0;
+	unsigned int uiCurrSectOffsetAddr	= 0;
+	unsigned int uiSectBoundary		= 0;
+	unsigned int uiNumSectTobeRead		= 0;
+	UCHAR ucReadBk[16]		= {0};
+	ULONG ulStatus			= 0;
+	unsigned int Status			= STATUS_SUCCESS;
+	unsigned int uiTemp			= 0;
+	unsigned int index			= 0;
+	unsigned int uiPartOffset		= 0;
+
+	uiOffsetFromSectStart = uiOffset & ~(Adapter->uiSectorSize - 1);
+
+	/* uiOffset += Adapter->ulFlashCalStart;
+	 * Adding flash Base address
+	 * uiOffset = uiOffset + GetFlashBaseAddr(Adapter);
+	 */
+	uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize - 1);
+	uiCurrSectOffsetAddr = uiOffset & (Adapter->uiSectorSize - 1);
+	uiSectBoundary = uiSectAlignAddr + Adapter->uiSectorSize;
 
 	pTempBuff = kmalloc(Adapter->uiSectorSize, GFP_KERNEL);
-	if(NULL == pTempBuff)
+	if (!pTempBuff)
 		goto BeceemFlashBulkWriteStatus_EXIT;
 
-//
-// check if the data to be written is overlapped across sectors
-//
-	if(uiOffset+uiNumBytes < uiSectBoundary)
-	{
+	/*
+	 * check if the data to be written is overlapped across sectors
+	 */
+	if (uiOffset+uiNumBytes < uiSectBoundary) {
 		uiNumSectTobeRead = 1;
-	}
-	else
-	{
-//      Number of sectors  = Last sector start address/First sector start address
-		uiNumSectTobeRead =  (uiCurrSectOffsetAddr+uiNumBytes)/Adapter->uiSectorSize;
-		if((uiCurrSectOffsetAddr+uiNumBytes)%Adapter->uiSectorSize)
-		{
+	} else {
+		/* Number of sectors  = Last sector start address/First sector start address */
+		uiNumSectTobeRead =  (uiCurrSectOffsetAddr + uiNumBytes) / Adapter->uiSectorSize;
+		if ((uiCurrSectOffsetAddr + uiNumBytes)%Adapter->uiSectorSize)
 			uiNumSectTobeRead++;
-		}
 	}
 
-	if(IsFlash2x(Adapter) && (Adapter->bAllDSDWriteAllow == FALSE))
-	{
+	if (IsFlash2x(Adapter) && (Adapter->bAllDSDWriteAllow == FALSE)) {
 		index = 0;
-		uiTemp = uiNumSectTobeRead ;
-		while(uiTemp)
-		{
-			 if(IsOffsetWritable(Adapter,uiOffsetFromSectStart + index * Adapter->uiSectorSize ) == FALSE)
-			 {
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Sector Starting at offset <0X%x> is not writable",
-											(uiOffsetFromSectStart + index * Adapter->uiSectorSize));
+		uiTemp = uiNumSectTobeRead;
+		while (uiTemp) {
+			if (IsOffsetWritable(Adapter, uiOffsetFromSectStart + index * Adapter->uiSectorSize) == FALSE) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Sector Starting at offset <0X%x> is not writable",
+						(uiOffsetFromSectStart + index * Adapter->uiSectorSize));
 				Status = SECTOR_IS_NOT_WRITABLE;
 				goto BeceemFlashBulkWriteStatus_EXIT;
-			 }
-			 uiTemp = uiTemp - 1;
-			 index = index + 1 ;
+			}
+			uiTemp = uiTemp - 1;
+			index = index + 1 ;
 		}
 	}
 
 	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	while(uiNumSectTobeRead)
-	{
+	while (uiNumSectTobeRead) {
 		uiPartOffset = (uiSectAlignAddr & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
 
-		BcmDoChipSelect(Adapter,uiSectAlignAddr);
-		if(0 != BeceemFlashBulkRead(Adapter,
+		BcmDoChipSelect(Adapter, uiSectAlignAddr);
+		if (0 != BeceemFlashBulkRead(Adapter,
 						(PUINT)pTempBuff,
 						uiOffsetFromSectStart,
-						Adapter->uiSectorSize))
-		{
+						Adapter->uiSectorSize))	{
 			Status = -1;
 			goto BeceemFlashBulkWriteStatus_EXIT;
 		}
 
-		ulStatus = BcmFlashUnProtectBlock(Adapter,uiOffsetFromSectStart,Adapter->uiSectorSize);
-
-		if(uiNumSectTobeRead > 1)
-		{
+		ulStatus = BcmFlashUnProtectBlock(Adapter, uiOffsetFromSectStart, Adapter->uiSectorSize);
 
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr],pcBuffer,uiSectBoundary-(uiSectAlignAddr+uiCurrSectOffsetAddr));
-			pcBuffer += ((uiSectBoundary-(uiSectAlignAddr+uiCurrSectOffsetAddr)));
-			uiNumBytes -= (uiSectBoundary-(uiSectAlignAddr+uiCurrSectOffsetAddr));
-		}
-		else
-		{
-			memcpy(&pTempBuff[uiCurrSectOffsetAddr],pcBuffer,uiNumBytes);
+		if (uiNumSectTobeRead > 1) {
+			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
+			pcBuffer += ((uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr)));
+			uiNumBytes -= (uiSectBoundary - (uiSectAlignAddr + uiCurrSectOffsetAddr));
+		} else {
+			memcpy(&pTempBuff[uiCurrSectOffsetAddr], pcBuffer, uiNumBytes);
 		}
 
-		if(IsFlash2x(Adapter))
-		{
-			SaveHeaderIfPresent(Adapter,(PUCHAR)pTempBuff,uiOffsetFromSectStart);
-		}
+		if (IsFlash2x(Adapter))
+			SaveHeaderIfPresent(Adapter, (PUCHAR)pTempBuff, uiOffsetFromSectStart);
 
-		FlashSectorErase(Adapter,uiPartOffset,1);
+		FlashSectorErase(Adapter, uiPartOffset, 1);
 
-		for(uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex +=Adapter->ulFlashWriteSize)
-
-		{
-			if(Adapter->device_removed)
-			{
+		for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += Adapter->ulFlashWriteSize) {
+			if (Adapter->device_removed) {
 				Status = -1;
 				goto BeceemFlashBulkWriteStatus_EXIT;
 			}
 
-			if(STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter,uiPartOffset+uiIndex,&pTempBuff[uiIndex]))
-			{
+			if (STATUS_SUCCESS != (*Adapter->fpFlashWriteWithStatusCheck)(Adapter, uiPartOffset+uiIndex, &pTempBuff[uiIndex])) {
 				Status = -1;
 				goto BeceemFlashBulkWriteStatus_EXIT;
 			}
 		}
 
-		if(bVerify)
-		{
-			for(uiIndex = 0;uiIndex < Adapter->uiSectorSize;uiIndex += MAX_RW_SIZE)
-			{
-
-				if(STATUS_SUCCESS == BeceemFlashBulkRead(Adapter,(PUINT)ucReadBk,uiOffsetFromSectStart+uiIndex,MAX_RW_SIZE))
-				{
-					if(memcmp(ucReadBk,&pTempBuff[uiIndex],MAX_RW_SIZE))
-					{
+		if (bVerify) {
+			for (uiIndex = 0; uiIndex < Adapter->uiSectorSize; uiIndex += MAX_RW_SIZE) {
+				if (STATUS_SUCCESS == BeceemFlashBulkRead(Adapter, (PUINT)ucReadBk, uiOffsetFromSectStart + uiIndex, MAX_RW_SIZE)) {
+					if (memcmp(ucReadBk, &pTempBuff[uiIndex], MAX_RW_SIZE)) {
 						Status = STATUS_FAILURE;
 						goto BeceemFlashBulkWriteStatus_EXIT;
 					}
-
 				}
-
 			}
 		}
 
-		if(ulStatus)
-		{
-			BcmRestoreBlockProtectStatus(Adapter,ulStatus);
+		if (ulStatus) {
+			BcmRestoreBlockProtectStatus(Adapter, ulStatus);
 			ulStatus = 0;
 		}
 
@@ -1517,265 +1342,231 @@ static INT BeceemFlashBulkWriteStatus(
 		uiOffsetFromSectStart += Adapter->uiSectorSize;
 		uiNumSectTobeRead--;
 	}
-//
-// Cleanup.
-//
+/*
+ * Cleanup.
+ */
 BeceemFlashBulkWriteStatus_EXIT:
-	if(ulStatus)
-	{
-		BcmRestoreBlockProtectStatus(Adapter,ulStatus);
-	}
+	if (ulStatus)
+		BcmRestoreBlockProtectStatus(Adapter, ulStatus);
 
 	kfree(pTempBuff);
 	Adapter->SelectedChip = RESET_CHIP_SELECT;
 	return Status;
-
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	PropagateCalParamsFromEEPROMToMemory
-//
-// Description: Dumps the calibration section of EEPROM to DDR.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-
-INT PropagateCalParamsFromEEPROMToMemory(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	PropagateCalParamsFromEEPROMToMemory
+ *
+ * Description: Dumps the calibration section of EEPROM to DDR.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+int PropagateCalParamsFromEEPROMToMemory(struct bcm_mini_adapter *Adapter)
 {
 	PCHAR pBuff = kmalloc(BUFFER_4K, GFP_KERNEL);
-	UINT uiEepromSize = 0;
-	UINT uiIndex = 0;
-	UINT uiBytesToCopy = 0;
-	UINT uiCalStartAddr = EEPROM_CALPARAM_START;
-	UINT uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
-	UINT value;
-	INT Status = 0;
-	if(pBuff == NULL)
-	{
-		return -1;
-	}
-
-	if(0 != BeceemEEPROMBulkRead(Adapter,&uiEepromSize,EEPROM_SIZE_OFFSET,4))
-	{
+	unsigned int uiEepromSize = 0;
+	unsigned int uiIndex = 0;
+	unsigned int uiBytesToCopy = 0;
+	unsigned int uiCalStartAddr = EEPROM_CALPARAM_START;
+	unsigned int uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
+	unsigned int value;
+	int Status = 0;
+
+	if (!pBuff)
+		return -ENOMEM;
 
+	if (0 != BeceemEEPROMBulkRead(Adapter, &uiEepromSize, EEPROM_SIZE_OFFSET, 4)) {
 		kfree(pBuff);
 		return -1;
 	}
 
 	uiEepromSize >>= 16;
-	if(uiEepromSize > 1024*1024)
-	{
+	if (uiEepromSize > 1024 * 1024) {
 		kfree(pBuff);
 		return -1;
 	}
 
+	uiBytesToCopy = MIN(BUFFER_4K, uiEepromSize);
 
-	uiBytesToCopy = MIN(BUFFER_4K,uiEepromSize);
-
-	while(uiBytesToCopy)
-	{
-		if(0 != BeceemEEPROMBulkRead(Adapter,(PUINT)pBuff,uiCalStartAddr,uiBytesToCopy))
-		{
+	while (uiBytesToCopy) {
+		if (0 != BeceemEEPROMBulkRead(Adapter, (PUINT)pBuff, uiCalStartAddr, uiBytesToCopy)) {
 			Status = -1;
 			break;
 		}
-		wrm(Adapter,uiMemoryLoc,(PCHAR)(((PULONG)pBuff)+uiIndex),uiBytesToCopy);
+		wrm(Adapter, uiMemoryLoc, (PCHAR)(((PULONG)pBuff) + uiIndex), uiBytesToCopy);
 		uiMemoryLoc += uiBytesToCopy;
 		uiEepromSize -= uiBytesToCopy;
 		uiCalStartAddr += uiBytesToCopy;
-		uiIndex += uiBytesToCopy/4;
-		uiBytesToCopy = MIN(BUFFER_4K,uiEepromSize);
+		uiIndex += uiBytesToCopy / 4;
+		uiBytesToCopy = MIN(BUFFER_4K, uiEepromSize);
 
 	}
 	value = 0xbeadbead;
-	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC-4,&value, sizeof(value));
+	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 4, &value, sizeof(value));
 	value = 0xbeadbead;
-	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC-8,&value, sizeof(value));
+	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 8, &value, sizeof(value));
 	kfree(pBuff);
 
 	return Status;
-
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	PropagateCalParamsFromFlashToMemory
-//
-// Description: Dumps the calibration section of EEPROM to DDR.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-INT PropagateCalParamsFromFlashToMemory(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	PropagateCalParamsFromFlashToMemory
+ *
+ * Description: Dumps the calibration section of EEPROM to DDR.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+int PropagateCalParamsFromFlashToMemory(struct bcm_mini_adapter *Adapter)
 {
 	PCHAR pBuff, pPtr;
-	UINT uiEepromSize = 0;
-	UINT uiBytesToCopy = 0;
-	//UINT uiIndex = 0;
-	UINT uiCalStartAddr = EEPROM_CALPARAM_START;
-	UINT uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
-	UINT value;
-	INT Status = 0;
-//
-// Write the signature first. This will ensure firmware does not access EEPROM.
-//
+	unsigned int uiEepromSize = 0;
+	unsigned int uiBytesToCopy = 0;
+	/* unsigned int uiIndex = 0; */
+	unsigned int uiCalStartAddr = EEPROM_CALPARAM_START;
+	unsigned int uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
+	unsigned int value;
+	int Status = 0;
+
+	/*
+	 * Write the signature first. This will ensure firmware does not access EEPROM.
+	 */
 	value = 0xbeadbead;
 	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 4, &value, sizeof(value));
 	value = 0xbeadbead;
 	wrmalt(Adapter, EEPROM_CAL_DATA_INTERNAL_LOC - 8, &value, sizeof(value));
 
-	if(0 != BeceemNVMRead(Adapter,&uiEepromSize,EEPROM_SIZE_OFFSET, 4))
-	{
+	if (0 != BeceemNVMRead(Adapter, &uiEepromSize, EEPROM_SIZE_OFFSET, 4))
 		return -1;
-	}
+
 	uiEepromSize = ntohl(uiEepromSize);
 	uiEepromSize >>= 16;
 
-//
-//	subtract the auto init section size
-//
+	/*
+	 * subtract the auto init section size
+	 */
 	uiEepromSize -= EEPROM_CALPARAM_START;
 
-	if(uiEepromSize > 1024*1024)
-	{
+	if (uiEepromSize > 1024 * 1024)
 		return -1;
-	}
 
 	pBuff = kmalloc(uiEepromSize, GFP_KERNEL);
-	if ( pBuff == NULL )
-		return -1;
+	if (pBuff == NULL)
+		return -ENOMEM;
 
-	if(0 != BeceemNVMRead(Adapter,(PUINT)pBuff,uiCalStartAddr, uiEepromSize))
-	{
+	if (0 != BeceemNVMRead(Adapter, (PUINT)pBuff, uiCalStartAddr, uiEepromSize)) {
 		kfree(pBuff);
 		return -1;
 	}
 
 	pPtr = pBuff;
 
-	uiBytesToCopy = MIN(BUFFER_4K,uiEepromSize);
+	uiBytesToCopy = MIN(BUFFER_4K, uiEepromSize);
 
-	while(uiBytesToCopy)
-	{
-		Status = wrm(Adapter,uiMemoryLoc,(PCHAR)pPtr,uiBytesToCopy);
-		if(Status)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"wrm failed with status :%d",Status);
+	while (uiBytesToCopy) {
+		Status = wrm(Adapter, uiMemoryLoc, (PCHAR)pPtr, uiBytesToCopy);
+		if (Status) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "wrm failed with status :%d", Status);
 			break;
 		}
 
 		pPtr += uiBytesToCopy;
 		uiEepromSize -= uiBytesToCopy;
 		uiMemoryLoc += uiBytesToCopy;
-		uiBytesToCopy = MIN(BUFFER_4K,uiEepromSize);
+		uiBytesToCopy = MIN(BUFFER_4K, uiEepromSize);
 	}
 
 	kfree(pBuff);
 	return Status;
-
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemEEPROMReadBackandVerify
-//
-// Description: Read back the data written and verifies.
-//
-// Arguments:
-//		Adapter       - ptr to Adapter object instance
-//		pBuffer 	    - Data to be written.
-//		uiOffset       - Offset of the flash where data needs to be written to.
-//		uiNumBytes - Number of bytes to be written.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-static INT BeceemEEPROMReadBackandVerify(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes)
+/*
+ * Procedure:	BeceemEEPROMReadBackandVerify
+ *
+ * Description: Read back the data written and verifies.
+ *
+ * Arguments:
+ *		Adapter		- ptr to Adapter object instance
+ *		pBuffer		- Data to be written.
+ *		uiOffset	- Offset of the flash where data needs to be written to.
+ *		uiNumBytes	- Number of bytes to be written.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int BeceemEEPROMReadBackandVerify(struct bcm_mini_adapter *Adapter,
+					PUINT pBuffer,
+					unsigned int uiOffset,
+					unsigned int uiNumBytes)
 {
-	UINT uiRdbk  	= 0;
-	UINT uiIndex 	= 0;
-	UINT uiData  	= 0;
-	UINT auiData[4] = {0};
+	unsigned int uiRdbk	= 0;
+	unsigned int uiIndex	= 0;
+	unsigned int uiData	= 0;
+	unsigned int auiData[4]	= {0};
 
-	while(uiNumBytes)
-	{
-		if(Adapter->device_removed )
-		{
+	while (uiNumBytes) {
+		if (Adapter->device_removed)
 			return -1;
-		}
 
-		if(uiNumBytes >= MAX_RW_SIZE)
-		{// for the requests more than or equal to MAX_RW_SIZE bytes, use bulk read function to make the access faster.
-			BeceemEEPROMBulkRead(Adapter,&auiData[0],uiOffset,MAX_RW_SIZE);
+		if (uiNumBytes >= MAX_RW_SIZE) {
+			/* for the requests more than or equal to MAX_RW_SIZE bytes, use bulk read function to make the access faster. */
+			BeceemEEPROMBulkRead(Adapter, &auiData[0], uiOffset, MAX_RW_SIZE);
 
-			if(memcmp(&pBuffer[uiIndex],&auiData[0],MAX_RW_SIZE))
-			{
-				// re-write
-				BeceemEEPROMBulkWrite(Adapter,(PUCHAR)(pBuffer+uiIndex),uiOffset,MAX_RW_SIZE,FALSE);
+			if (memcmp(&pBuffer[uiIndex], &auiData[0], MAX_RW_SIZE)) {
+				/* re-write */
+				BeceemEEPROMBulkWrite(Adapter, (PUCHAR)(pBuffer + uiIndex), uiOffset, MAX_RW_SIZE, FALSE);
 				mdelay(3);
-				BeceemEEPROMBulkRead(Adapter,&auiData[0],uiOffset,MAX_RW_SIZE);
+				BeceemEEPROMBulkRead(Adapter, &auiData[0], uiOffset, MAX_RW_SIZE);
 
-				if(memcmp(&pBuffer[uiIndex],&auiData[0],MAX_RW_SIZE))
-				{
+				if (memcmp(&pBuffer[uiIndex], &auiData[0], MAX_RW_SIZE))
 					return -1;
-				}
 			}
 			uiOffset += MAX_RW_SIZE;
 			uiNumBytes -= MAX_RW_SIZE;
 			uiIndex += 4;
-
-		}
-		else if(uiNumBytes >= 4)
-		{
-			BeceemEEPROMBulkRead(Adapter,&uiData,uiOffset,4);
-			if(uiData != pBuffer[uiIndex])
-			{
-				//re-write
-				BeceemEEPROMBulkWrite(Adapter,(PUCHAR)(pBuffer+uiIndex),uiOffset,4,FALSE);
+		} else if (uiNumBytes >= 4) {
+			BeceemEEPROMBulkRead(Adapter, &uiData, uiOffset, 4);
+			if (uiData != pBuffer[uiIndex]) {
+				/* re-write */
+				BeceemEEPROMBulkWrite(Adapter, (PUCHAR)(pBuffer + uiIndex), uiOffset, 4, FALSE);
 				mdelay(3);
-				BeceemEEPROMBulkRead(Adapter,&uiData,uiOffset,4);
-				if(uiData != pBuffer[uiIndex])
-				{
+				BeceemEEPROMBulkRead(Adapter, &uiData, uiOffset, 4);
+				if (uiData != pBuffer[uiIndex])
 					return -1;
-				}
 			}
 			uiOffset += 4;
 			uiNumBytes -= 4;
 			uiIndex++;
-
-		}
-		else
-		{ // Handle the reads less than 4 bytes...
+		} else {
+			/* Handle the reads less than 4 bytes... */
 			uiData = 0;
-			memcpy(&uiData,((PUCHAR)pBuffer)+(uiIndex*sizeof(UINT)),uiNumBytes);
-			BeceemEEPROMBulkRead(Adapter,&uiRdbk,uiOffset,4);
+			memcpy(&uiData, ((PUCHAR)pBuffer) + (uiIndex * sizeof(unsigned int)), uiNumBytes);
+			BeceemEEPROMBulkRead(Adapter, &uiRdbk, uiOffset, 4);
 
-			if(memcmp(&uiData, &uiRdbk, uiNumBytes))
+			if (memcmp(&uiData, &uiRdbk, uiNumBytes))
 				return -1;
 
 			uiNumBytes = 0;
 		}
-
 	}
 
 	return 0;
 }
 
-static VOID BcmSwapWord(UINT *ptr1) {
-
-	UINT  tempval = (UINT)*ptr1;
+static VOID BcmSwapWord(unsigned int *ptr1)
+{
+	unsigned int tempval = (unsigned int)*ptr1;
 	char *ptr2 = (char *)&tempval;
 	char *ptr = (char *)ptr1;
 
@@ -1785,495 +1576,436 @@ static VOID BcmSwapWord(UINT *ptr1) {
 	ptr[3] = ptr2[0];
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemEEPROMWritePage
-//
-// Description: Performs page write (16bytes) to the EEPROM
-//
-// Arguments:
-//		Adapter       - ptr to Adapter object instance
-//		uiData 	  	  - Data to be written.
-//		uiOffset      - Offset of the EEPROM where data needs to be written to.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-static INT BeceemEEPROMWritePage( PMINI_ADAPTER Adapter, UINT uiData[], UINT uiOffset )
+/*
+ * Procedure:	BeceemEEPROMWritePage
+ *
+ * Description: Performs page write (16bytes) to the EEPROM
+ *
+ * Arguments:
+ *		Adapter		- ptr to Adapter object instance
+ *		uiData		- Data to be written.
+ *		uiOffset	- Offset of the EEPROM where data needs to be written to.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+static int BeceemEEPROMWritePage(struct bcm_mini_adapter *Adapter, unsigned int uiData[], unsigned int uiOffset)
 {
-	UINT uiRetries = MAX_EEPROM_RETRIES*RETRIES_PER_DELAY;
-	UINT uiStatus = 0;
+	unsigned int uiRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
+	unsigned int uiStatus = 0;
 	UCHAR uiEpromStatus = 0;
-	UINT value =0 ;
+	unsigned int value = 0;
 
 	/* Flush the Write/Read/Cmd queues. */
-	value = ( EEPROM_WRITE_QUEUE_FLUSH | EEPROM_CMD_QUEUE_FLUSH | EEPROM_READ_QUEUE_FLUSH );
-	wrmalt( Adapter, SPI_FLUSH_REG, &value, sizeof(value));
-	value = 0 ;
-	wrmalt( Adapter, SPI_FLUSH_REG, &value, sizeof(value) );
+	value = (EEPROM_WRITE_QUEUE_FLUSH | EEPROM_CMD_QUEUE_FLUSH | EEPROM_READ_QUEUE_FLUSH);
+	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
+	value = 0;
+	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
 
 	/* Clear the Empty/Avail/Full bits.  After this it has been confirmed
 	 * that the bit was cleared by reading back the register. See NOTE below.
 	 * We also clear the Read queues as we do a EEPROM status register read
-	 * later. */
-	value = ( EEPROM_WRITE_QUEUE_EMPTY | EEPROM_WRITE_QUEUE_AVAIL | EEPROM_WRITE_QUEUE_FULL | EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL ) ;
-	wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&value, sizeof(value));
+	 * later.
+	 */
+	value = (EEPROM_WRITE_QUEUE_EMPTY | EEPROM_WRITE_QUEUE_AVAIL | EEPROM_WRITE_QUEUE_FULL | EEPROM_READ_DATA_AVAIL | EEPROM_READ_DATA_FULL);
+	wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 
 	/* Enable write */
-	value = EEPROM_WRITE_ENABLE ;
-	wrmalt( Adapter, EEPROM_CMDQ_SPI_REG,&value, sizeof(value) );
+	value = EEPROM_WRITE_ENABLE;
+	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
 
 	/* We can write back to back 8bits * 16 into the queue and as we have
-	 * checked for the queue to be empty we can write in a burst. */
+	 * checked for the queue to be empty we can write in a burst.
+	 */
 
 	value = uiData[0];
 	BcmSwapWord(&value);
-	wrm( Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
+	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
 
 	value = uiData[1];
 	BcmSwapWord(&value);
-	wrm( Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
+	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
 
 	value = uiData[2];
 	BcmSwapWord(&value);
-	wrm( Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
+	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
 
 	value = uiData[3];
 	BcmSwapWord(&value);
-	wrm( Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
+	wrm(Adapter, EEPROM_WRITE_DATAQ_REG, (PUCHAR)&value, 4);
 
 	/* NOTE : After this write, on readback of EEPROM_SPI_Q_STATUS1_REG
 	 * shows that we see 7 for the EEPROM data write.  Which means that
 	 * queue got full, also space is available as well as the queue is empty.
-	 * This may happen in sequence. */
-	value =  EEPROM_16_BYTE_PAGE_WRITE | uiOffset ;
-	wrmalt( Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value) );
+	 * This may happen in sequence.
+	 */
+	value =  EEPROM_16_BYTE_PAGE_WRITE | uiOffset;
+	wrmalt(Adapter, EEPROM_CMDQ_SPI_REG, &value, sizeof(value));
 
 	/* Ideally we should loop here without tries and eventually succeed.
 	 * What we are checking if the previous write has completed, and this
-	 * may take time. We should wait till the Empty bit is set. */
+	 * may take time. We should wait till the Empty bit is set.
+	 */
 	uiStatus = 0;
 	rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-	while ( ( uiStatus & EEPROM_WRITE_QUEUE_EMPTY ) == 0 )
-	{
+	while ((uiStatus & EEPROM_WRITE_QUEUE_EMPTY) == 0) {
 		uiRetries--;
-		if ( uiRetries == 0 )
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "0x0f003004 = %x, %d retries failed.\n", uiStatus, MAX_EEPROM_RETRIES *RETRIES_PER_DELAY);
-			return STATUS_FAILURE ;
+		if (uiRetries == 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "0x0f003004 = %x, %d retries failed.\n", uiStatus, MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
+			return STATUS_FAILURE;
 		}
 
-		if( !(uiRetries%RETRIES_PER_DELAY) )
-					msleep(1);
+		if (!(uiRetries%RETRIES_PER_DELAY))
+			udelay(1000);
 
 		uiStatus = 0;
 		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
-		if(Adapter->device_removed == TRUE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Modem got removed hence exiting from loop....");
+		if (Adapter->device_removed == TRUE) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem got removed hence exiting from loop....");
 			return -ENODEV;
 		}
-
 	}
 
-	if ( uiRetries != 0 )
-	{
+	if (uiRetries != 0) {
 		/* Clear the ones that are set - either, Empty/Full/Avail bits */
-		value = ( uiStatus & ( EEPROM_WRITE_QUEUE_EMPTY | EEPROM_WRITE_QUEUE_AVAIL | EEPROM_WRITE_QUEUE_FULL ) );
-		wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
+		value = (uiStatus & (EEPROM_WRITE_QUEUE_EMPTY | EEPROM_WRITE_QUEUE_AVAIL | EEPROM_WRITE_QUEUE_FULL));
+		wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 	}
 
 	/* Here we should check if the EEPROM status register is correct before
 	 * proceeding. Bit 0 in the EEPROM Status register should be 0 before
 	 * we proceed further.  A 1 at Bit 0 indicates that the EEPROM is busy
 	 * with the previous write. Note also that issuing this read finally
-	 * means the previous write to the EEPROM has completed. */
-	uiRetries = MAX_EEPROM_RETRIES*RETRIES_PER_DELAY;
+	 * means the previous write to the EEPROM has completed.
+	 */
+	uiRetries = MAX_EEPROM_RETRIES * RETRIES_PER_DELAY;
 	uiEpromStatus = 0;
-	while ( uiRetries != 0 )
-	{
-		uiEpromStatus = ReadEEPROMStatusRegister( Adapter) ;
-		if(Adapter->device_removed == TRUE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Modem has got removed hence exiting from loop...");
+	while (uiRetries != 0) {
+		uiEpromStatus = ReadEEPROMStatusRegister(Adapter);
+		if (Adapter->device_removed == TRUE) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Modem has got removed hence exiting from loop...");
 			return -ENODEV;
 		}
-		if ( ( EEPROM_STATUS_REG_WRITE_BUSY & uiEpromStatus ) == 0 )
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "EEPROM status register = %x tries = %d\n", uiEpromStatus, (MAX_EEPROM_RETRIES * RETRIES_PER_DELAY- uiRetries) );
-			return STATUS_SUCCESS ;
+		if ((EEPROM_STATUS_REG_WRITE_BUSY & uiEpromStatus) == 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "EEPROM status register = %x tries = %d\n", uiEpromStatus, (MAX_EEPROM_RETRIES * RETRIES_PER_DELAY - uiRetries));
+			return STATUS_SUCCESS;
 		}
 		uiRetries--;
-		if ( uiRetries == 0 )
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "0x0f003004 = %x, for EEPROM status read %d retries failed.\n", uiEpromStatus, MAX_EEPROM_RETRIES *RETRIES_PER_DELAY);
-			return STATUS_FAILURE ;
+		if (uiRetries == 0) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "0x0f003004 = %x, for EEPROM status read %d retries failed.\n", uiEpromStatus, MAX_EEPROM_RETRIES * RETRIES_PER_DELAY);
+			return STATUS_FAILURE;
 		}
 		uiEpromStatus = 0;
-		if( !(uiRetries%RETRIES_PER_DELAY) )
-				msleep(1);
+		if (!(uiRetries%RETRIES_PER_DELAY))
+			udelay(1000);
 	}
 
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 } /* BeceemEEPROMWritePage */
 
-
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemEEPROMBulkWrite
-//
-// Description: Performs write to the EEPROM
-//
-// Arguments:
-//		Adapter       - ptr to Adapter object instance
-//		pBuffer 	    - Data to be written.
-//		uiOffset       - Offset of the EEPROM where data needs to be written to.
-//		uiNumBytes - Number of bytes to be written.
-//		bVerify        - read verify flag.
-// Returns:
-//		OSAL_STATUS_CODE
-//
-//-----------------------------------------------------------------------------
-
-INT BeceemEEPROMBulkWrite(
-	PMINI_ADAPTER Adapter,
-	PUCHAR pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	BOOLEAN bVerify)
+/*
+ * Procedure:	BeceemEEPROMBulkWrite
+ *
+ * Description: Performs write to the EEPROM
+ *
+ * Arguments:
+ *		Adapter		- ptr to Adapter object instance
+ *		pBuffer		- Data to be written.
+ *		uiOffset	- Offset of the EEPROM where data needs to be written to.
+ *		uiNumBytes	- Number of bytes to be written.
+ *		bVerify		- read verify flag.
+ * Returns:
+ *		OSAL_STATUS_CODE
+ *
+ */
+
+int BeceemEEPROMBulkWrite(struct bcm_mini_adapter *Adapter,
+			PUCHAR pBuffer,
+			unsigned int uiOffset,
+			unsigned int uiNumBytes,
+			BOOLEAN bVerify)
 {
-	UINT  uiBytesToCopy = uiNumBytes;
-	//UINT  uiRdbk 		= 0;
-	UINT  uiData[4] 	= {0};
-	UINT  uiIndex 		= 0;
-	UINT  uiTempOffset  = 0;
-	UINT  uiExtraBytes  = 0;
-	//PUINT puiBuffer 	= (PUINT)pBuffer;
-	//INT value;
-
-	if(uiOffset%MAX_RW_SIZE && uiBytesToCopy)
-	{
-		uiTempOffset = uiOffset - (uiOffset%MAX_RW_SIZE);
-		uiExtraBytes = uiOffset-uiTempOffset;
-
-
-		BeceemEEPROMBulkRead(Adapter,&uiData[0],uiTempOffset,MAX_RW_SIZE);
-
-		if(uiBytesToCopy >= (16 -uiExtraBytes))
-		{
-			memcpy((((PUCHAR)&uiData[0])+uiExtraBytes),pBuffer,MAX_RW_SIZE- uiExtraBytes);
-
-			if ( STATUS_FAILURE == BeceemEEPROMWritePage( Adapter, uiData, uiTempOffset ) )
-					return STATUS_FAILURE;
+	unsigned int uiBytesToCopy	= uiNumBytes;
+	/* unsigned int uiRdbk		= 0; */
+	unsigned int uiData[4]		= {0};
+	unsigned int uiIndex		= 0;
+	unsigned int uiTempOffset	= 0;
+	unsigned int uiExtraBytes	= 0;
+	/* PUINT puiBuffer	= (PUINT)pBuffer;
+	 * int value;
+	 */
+
+	if (uiOffset % MAX_RW_SIZE && uiBytesToCopy) {
+		uiTempOffset = uiOffset - (uiOffset % MAX_RW_SIZE);
+		uiExtraBytes = uiOffset - uiTempOffset;
+
+		BeceemEEPROMBulkRead(Adapter, &uiData[0], uiTempOffset, MAX_RW_SIZE);
+
+		if (uiBytesToCopy >= (16 - uiExtraBytes)) {
+			memcpy((((PUCHAR)&uiData[0]) + uiExtraBytes), pBuffer, MAX_RW_SIZE - uiExtraBytes);
+
+			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, uiData, uiTempOffset))
+				return STATUS_FAILURE;
 
 			uiBytesToCopy -= (MAX_RW_SIZE - uiExtraBytes);
 			uiIndex += (MAX_RW_SIZE - uiExtraBytes);
 			uiOffset += (MAX_RW_SIZE - uiExtraBytes);
-		}
-		else
-		{
-			memcpy((((PUCHAR)&uiData[0])+uiExtraBytes),pBuffer,uiBytesToCopy);
+		} else {
+			memcpy((((PUCHAR)&uiData[0]) + uiExtraBytes), pBuffer, uiBytesToCopy);
 
-			if ( STATUS_FAILURE == BeceemEEPROMWritePage( Adapter, uiData, uiTempOffset ) )
-					return STATUS_FAILURE;
+			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, uiData, uiTempOffset))
+				return STATUS_FAILURE;
 
 			uiIndex += uiBytesToCopy;
 			uiOffset += uiBytesToCopy;
 			uiBytesToCopy = 0;
 		}
-
-
 	}
 
-	while(uiBytesToCopy)
-	{
-		if(Adapter->device_removed)
-		{
+	while (uiBytesToCopy) {
+		if (Adapter->device_removed)
 			return -1;
-		}
 
-		if(uiBytesToCopy >= MAX_RW_SIZE)
-		{
-
-			if (STATUS_FAILURE == BeceemEEPROMWritePage( Adapter, (PUINT) &pBuffer[uiIndex], uiOffset ) )
-						return STATUS_FAILURE;
+		if (uiBytesToCopy >= MAX_RW_SIZE) {
+			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, (PUINT) &pBuffer[uiIndex], uiOffset))
+				return STATUS_FAILURE;
 
 			uiIndex += MAX_RW_SIZE;
 			uiOffset += MAX_RW_SIZE;
-			uiBytesToCopy	-= MAX_RW_SIZE;
-		}
-		else
-		{
-	//
-	// To program non 16byte aligned data, read 16byte and then update.
-	//
-			BeceemEEPROMBulkRead(Adapter,&uiData[0],uiOffset,16);
-			memcpy(&uiData[0],pBuffer+uiIndex,uiBytesToCopy);
-
+			uiBytesToCopy -= MAX_RW_SIZE;
+		} else {
+			/*
+			 * To program non 16byte aligned data, read 16byte and then update.
+			 */
+			BeceemEEPROMBulkRead(Adapter, &uiData[0], uiOffset, 16);
+			memcpy(&uiData[0], pBuffer + uiIndex, uiBytesToCopy);
+
+			if (STATUS_FAILURE == BeceemEEPROMWritePage(Adapter, uiData, uiOffset))
+				return STATUS_FAILURE;
 
-			if ( STATUS_FAILURE == BeceemEEPROMWritePage( Adapter, uiData, uiOffset ) )
-					return STATUS_FAILURE;
 			uiBytesToCopy = 0;
 		}
-
 	}
 
 	return 0;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemNVMRead
-//
-// Description: Reads n number of bytes from NVM.
-//
-// Arguments:
-//		Adapter      - ptr to Adapter object instance
-//		pBuffer       - Buffer to store the data read from NVM
-//		uiOffset       - Offset of NVM from where data should be read
-//		uiNumBytes - Number of bytes to be read from the NVM.
-//
-// Returns:
-//		OSAL_STATUS_SUCCESS - if NVM read is successful.
-//		<FAILURE>			- if failed.
-//-----------------------------------------------------------------------------
-
-INT BeceemNVMRead(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes)
+/*
+ * Procedure:	BeceemNVMRead
+ *
+ * Description: Reads n number of bytes from NVM.
+ *
+ * Arguments:
+ *		Adapter      - ptr to Adapter object instance
+ *		pBuffer       - Buffer to store the data read from NVM
+ *		uiOffset       - Offset of NVM from where data should be read
+ *		uiNumBytes - Number of bytes to be read from the NVM.
+ *
+ * Returns:
+ *		OSAL_STATUS_SUCCESS - if NVM read is successful.
+ *		<FAILURE>			- if failed.
+ */
+
+int BeceemNVMRead(struct bcm_mini_adapter *Adapter,
+		PUINT pBuffer,
+		unsigned int uiOffset,
+		unsigned int uiNumBytes)
 {
-	INT Status = 0;
-#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-	UINT uiTemp = 0, value;
-#endif
+	int Status = 0;
 
-	if(Adapter->eNVMType == NVM_FLASH)
-	{
-		if(Adapter->bFlashRawRead == FALSE)
-		{
-			if (IsSectionExistInVendorInfo(Adapter,Adapter->eActiveDSD))
-				return vendorextnReadSection(Adapter,(PUCHAR)pBuffer,Adapter->eActiveDSD,uiOffset,uiNumBytes);
-			uiOffset = uiOffset+ Adapter->ulFlashCalStart ;
+	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
+		unsigned int uiTemp = 0, value;
+	#endif
+
+	if (Adapter->eNVMType == NVM_FLASH) {
+		if (Adapter->bFlashRawRead == FALSE) {
+			if (IsSectionExistInVendorInfo(Adapter, Adapter->eActiveDSD))
+				return vendorextnReadSection(Adapter, (PUCHAR)pBuffer, Adapter->eActiveDSD, uiOffset, uiNumBytes);
+
+			uiOffset = uiOffset + Adapter->ulFlashCalStart;
 		}
-#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-		Status = bcmflash_raw_read((uiOffset/FLASH_PART_SIZE),(uiOffset % FLASH_PART_SIZE),( unsigned char *)pBuffer,uiNumBytes);
-#else
 
-		rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-		value = 0;
-		wrmalt(Adapter, 0x0f000C80,&value, sizeof(value));
-		Status = BeceemFlashBulkRead(Adapter,
+		#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
+			Status = bcmflash_raw_read((uiOffset / FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
+		#else
+			rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
+			value = 0;
+			wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
+			Status = BeceemFlashBulkRead(Adapter,
 						pBuffer,
 						uiOffset,
 						uiNumBytes);
-		wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-#endif
-	}
-	else if(Adapter->eNVMType == NVM_EEPROM)
-	{
+			wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
+		#endif
+	} else if (Adapter->eNVMType == NVM_EEPROM) {
 		Status = BeceemEEPROMBulkRead(Adapter,
 					pBuffer,
 					uiOffset,
 					uiNumBytes);
-	}
-	else
-	{
+	} else {
 		Status = -1;
 	}
+
 	return Status;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BeceemNVMWrite
-//
-// Description: Writes n number of bytes to NVM.
-//
-// Arguments:
-//		Adapter      - ptr to Adapter object instance
-//		pBuffer       - Buffer contains the data to be written.
-//		uiOffset       - Offset of NVM where data to be written to.
-//		uiNumBytes - Number of bytes to be written..
-//
-// Returns:
-//		OSAL_STATUS_SUCCESS - if NVM write is successful.
-//		<FAILURE>			- if failed.
-//-----------------------------------------------------------------------------
-
-INT BeceemNVMWrite(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	BOOLEAN bVerify)
+/*
+ * Procedure:	BeceemNVMWrite
+ *
+ * Description: Writes n number of bytes to NVM.
+ *
+ * Arguments:
+ *		Adapter      - ptr to Adapter object instance
+ *		pBuffer       - Buffer contains the data to be written.
+ *		uiOffset       - Offset of NVM where data to be written to.
+ *		uiNumBytes - Number of bytes to be written..
+ *
+ * Returns:
+ *		OSAL_STATUS_SUCCESS - if NVM write is successful.
+ *		<FAILURE>			- if failed.
+ */
+
+int BeceemNVMWrite(struct bcm_mini_adapter *Adapter,
+		PUINT pBuffer,
+		unsigned int uiOffset,
+		unsigned int uiNumBytes,
+		BOOLEAN bVerify)
 {
-	INT Status = 0;
-	UINT uiTemp = 0;
-	UINT uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
-	UINT uiIndex = 0;
-#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-	UINT value;
-#endif
-	UINT uiFlashOffset = 0;
-
-	if(Adapter->eNVMType == NVM_FLASH)
-	{
-		if (IsSectionExistInVendorInfo(Adapter,Adapter->eActiveDSD))
-			Status = vendorextnWriteSection(Adapter,(PUCHAR)pBuffer,Adapter->eActiveDSD,uiOffset,uiNumBytes,bVerify);
-		else
-		{
-			uiFlashOffset = uiOffset + Adapter->ulFlashCalStart;
+	int Status = 0;
+	unsigned int uiTemp = 0;
+	unsigned int uiMemoryLoc = EEPROM_CAL_DATA_INTERNAL_LOC;
+	unsigned int uiIndex = 0;
 
-#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-			Status = bcmflash_raw_write((uiFlashOffset/FLASH_PART_SIZE), (uiFlashOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer,uiNumBytes);
-#else
-			rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-			value = 0;
-			wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
+	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
+		unsigned int value;
+	#endif
 
-			if(Adapter->bStatusWrite == TRUE)
-			{
-				Status = BeceemFlashBulkWriteStatus(Adapter,
-							pBuffer,
-							uiFlashOffset,
-							uiNumBytes ,
-							bVerify);
-			}
-			else
-			{
+	unsigned int uiFlashOffset = 0;
 
-				Status = BeceemFlashBulkWrite(Adapter,
-							pBuffer,
-							uiFlashOffset,
-							uiNumBytes,
-							bVerify);
-			}
-#endif
-		}
+	if (Adapter->eNVMType == NVM_FLASH) {
+		if (IsSectionExistInVendorInfo(Adapter, Adapter->eActiveDSD))
+			Status = vendorextnWriteSection(Adapter, (PUCHAR)pBuffer, Adapter->eActiveDSD, uiOffset, uiNumBytes, bVerify);
+		else {
+			uiFlashOffset = uiOffset + Adapter->ulFlashCalStart;
+
+			#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
+				Status = bcmflash_raw_write((uiFlashOffset / FLASH_PART_SIZE), (uiFlashOffset % FLASH_PART_SIZE), (unsigned char *)pBuffer, uiNumBytes);
+			#else
+				rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
+				value = 0;
+				wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
+
+				if (Adapter->bStatusWrite == TRUE)
+					Status = BeceemFlashBulkWriteStatus(Adapter,
+									pBuffer,
+									uiFlashOffset,
+									uiNumBytes ,
+									bVerify);
+				else
 
+					Status = BeceemFlashBulkWrite(Adapter,
+								pBuffer,
+								uiFlashOffset,
+								uiNumBytes,
+								bVerify);
+			#endif
+		}
 
-		if(uiOffset >= EEPROM_CALPARAM_START)
-		{
+		if (uiOffset >= EEPROM_CALPARAM_START) {
 			uiMemoryLoc += (uiOffset - EEPROM_CALPARAM_START);
-			while(uiNumBytes)
-			{
-				if(uiNumBytes > BUFFER_4K)
-				{
-					wrm(Adapter,(uiMemoryLoc+uiIndex),(PCHAR)(pBuffer+(uiIndex/4)),BUFFER_4K);
+			while (uiNumBytes) {
+				if (uiNumBytes > BUFFER_4K) {
+					wrm(Adapter, (uiMemoryLoc+uiIndex), (PCHAR)(pBuffer + (uiIndex / 4)), BUFFER_4K);
 					uiNumBytes -= BUFFER_4K;
 					uiIndex += BUFFER_4K;
-				}
-				else
-				{
-					wrm(Adapter,uiMemoryLoc+uiIndex,(PCHAR)(pBuffer+(uiIndex/4)),uiNumBytes);
+				} else {
+					wrm(Adapter, uiMemoryLoc+uiIndex, (PCHAR)(pBuffer + (uiIndex / 4)), uiNumBytes);
 					uiNumBytes = 0;
 					break;
 				}
 			}
-		}
-		else
-		{
-			if((uiOffset+uiNumBytes) > EEPROM_CALPARAM_START)
-			{
+		} else {
+			if ((uiOffset + uiNumBytes) > EEPROM_CALPARAM_START) {
 				ULONG ulBytesTobeSkipped = 0;
-				PUCHAR pcBuffer = (PUCHAR)pBuffer;// char pointer to take care of odd byte cases.
+				PUCHAR pcBuffer = (PUCHAR)pBuffer; /* char pointer to take care of odd byte cases. */
 				uiNumBytes -= (EEPROM_CALPARAM_START - uiOffset);
 				ulBytesTobeSkipped += (EEPROM_CALPARAM_START - uiOffset);
 				uiOffset += (EEPROM_CALPARAM_START - uiOffset);
-				while(uiNumBytes)
-				{
-					if(uiNumBytes > BUFFER_4K)
-					{
-						wrm(Adapter,uiMemoryLoc+uiIndex,(PCHAR )&pcBuffer[ulBytesTobeSkipped+uiIndex],BUFFER_4K);
+				while (uiNumBytes) {
+					if (uiNumBytes > BUFFER_4K) {
+						wrm(Adapter, uiMemoryLoc + uiIndex, (PCHAR)&pcBuffer[ulBytesTobeSkipped + uiIndex], BUFFER_4K);
 						uiNumBytes -= BUFFER_4K;
 						uiIndex += BUFFER_4K;
-					}
-					else
-					{
-						wrm(Adapter,uiMemoryLoc+uiIndex,(PCHAR)&pcBuffer[ulBytesTobeSkipped+uiIndex],uiNumBytes);
+					} else {
+						wrm(Adapter, uiMemoryLoc + uiIndex, (PCHAR)&pcBuffer[ulBytesTobeSkipped + uiIndex], uiNumBytes);
 						uiNumBytes = 0;
 						break;
 					}
 				}
-
 			}
 		}
-
-	// restore the values.
-		wrmalt(Adapter,0x0f000C80,&uiTemp, sizeof(uiTemp));
-	}
-	else if(Adapter->eNVMType == NVM_EEPROM)
-	{
+		/* restore the values. */
+		wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
+	} else if (Adapter->eNVMType == NVM_EEPROM) {
 		Status = BeceemEEPROMBulkWrite(Adapter,
 					(PUCHAR)pBuffer,
 					uiOffset,
 					uiNumBytes,
 					bVerify);
-		if(bVerify)
-		{
-			Status = BeceemEEPROMReadBackandVerify(Adapter,(PUINT)pBuffer,uiOffset,uiNumBytes);
-		}
-	}
-	else
-	{
+		if (bVerify)
+			Status = BeceemEEPROMReadBackandVerify(Adapter, (PUINT)pBuffer, uiOffset, uiNumBytes);
+	} else {
 		Status = -1;
 	}
 	return Status;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmUpdateSectorSize
-//
-// Description: Updates the sector size to FLASH.
-//
-// Arguments:
-//		Adapter       - ptr to Adapter object instance
-//          uiSectorSize - sector size
-//
-// Returns:
-//		OSAL_STATUS_SUCCESS - if NVM write is successful.
-//		<FAILURE>			- if failed.
-//-----------------------------------------------------------------------------
-
-INT BcmUpdateSectorSize(PMINI_ADAPTER Adapter,UINT uiSectorSize)
+/*
+ * Procedure:	BcmUpdateSectorSize
+ *
+ * Description: Updates the sector size to FLASH.
+ *
+ * Arguments:
+ *		Adapter       - ptr to Adapter object instance
+ *          uiSectorSize - sector size
+ *
+ * Returns:
+ *		OSAL_STATUS_SUCCESS - if NVM write is successful.
+ *		<FAILURE>			- if failed.
+ */
+
+int BcmUpdateSectorSize(struct bcm_mini_adapter *Adapter, unsigned int uiSectorSize)
 {
-	INT Status = -1;
+	int Status = -1;
 	FLASH_CS_INFO sFlashCsInfo = {0};
-	UINT uiTemp = 0;
-
-	UINT uiSectorSig = 0;
-	UINT uiCurrentSectorSize = 0;
-
-	UINT value;
-
-
+	unsigned int uiTemp = 0;
+	unsigned int uiSectorSig = 0;
+	unsigned int uiCurrentSectorSize = 0;
+	unsigned int value;
 
 	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
 	value = 0;
-	wrmalt(Adapter, 0x0f000C80,&value, sizeof(value));
+	wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
 
-//
-// Before updating the sector size in the reserved area, check if already present.
-//
-	BeceemFlashBulkRead(Adapter,(PUINT)&sFlashCsInfo,Adapter->ulFlashControlSectionStart,sizeof(sFlashCsInfo));
+	/*
+	 * Before updating the sector size in the reserved area, check if already present.
+	 */
+	BeceemFlashBulkRead(Adapter, (PUINT)&sFlashCsInfo, Adapter->ulFlashControlSectionStart, sizeof(sFlashCsInfo));
 	uiSectorSig = ntohl(sFlashCsInfo.FlashSectorSizeSig);
 	uiCurrentSectorSize = ntohl(sFlashCsInfo.FlashSectorSize);
 
-	if(uiSectorSig == FLASH_SECTOR_SIZE_SIG)
-	{
-
-		if((uiCurrentSectorSize <= MAX_SECTOR_SIZE) && (uiCurrentSectorSize >= MIN_SECTOR_SIZE))
-		{
-			if(uiSectorSize == uiCurrentSectorSize)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Provided sector size is same as programmed in Flash");
+	if (uiSectorSig == FLASH_SECTOR_SIZE_SIG) {
+		if ((uiCurrentSectorSize <= MAX_SECTOR_SIZE) && (uiCurrentSectorSize >= MIN_SECTOR_SIZE)) {
+			if (uiSectorSize == uiCurrentSectorSize) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Provided sector size is same as programmed in Flash");
 				Status = STATUS_SUCCESS;
-				goto Restore ;
+				goto Restore;
 			}
 		}
 	}
 
-	if((uiSectorSize <= MAX_SECTOR_SIZE) && (uiSectorSize >= MIN_SECTOR_SIZE))
-	{
-
+	if ((uiSectorSize <= MAX_SECTOR_SIZE) && (uiSectorSize >= MIN_SECTOR_SIZE)) {
 		sFlashCsInfo.FlashSectorSize = htonl(uiSectorSize);
 		sFlashCsInfo.FlashSectorSizeSig = htonl(FLASH_SECTOR_SIZE_SIG);
 
@@ -2282,152 +2014,128 @@ INT BcmUpdateSectorSize(PMINI_ADAPTER Adapter,UINT uiSectorSize)
 					Adapter->ulFlashControlSectionStart,
 					sizeof(sFlashCsInfo),
 					TRUE);
-
-
 	}
 
-	Restore :
-	// restore the values.
-	wrmalt(Adapter, 0x0f000C80,&uiTemp, sizeof(uiTemp));
-
+Restore:
+	/* restore the values. */
+	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
 
 	return Status;
-
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmGetFlashSectorSize
-//
-// Description: Finds the sector size of the FLASH.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		UINT - sector size.
-//
-//-----------------------------------------------------------------------------
-
-static UINT BcmGetFlashSectorSize(PMINI_ADAPTER Adapter, UINT FlashSectorSizeSig, UINT FlashSectorSize)
+/*
+ * Procedure:	BcmGetFlashSectorSize
+ *
+ * Description: Finds the sector size of the FLASH.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		unsigned int - sector size.
+ *
+ */
+
+static unsigned int BcmGetFlashSectorSize(struct bcm_mini_adapter *Adapter, unsigned int FlashSectorSizeSig, unsigned int FlashSectorSize)
 {
-	UINT uiSectorSize = 0;
-	UINT uiSectorSig = 0;
+	unsigned int uiSectorSize = 0;
+	unsigned int uiSectorSig = 0;
 
-	if(Adapter->bSectorSizeOverride &&
+	if (Adapter->bSectorSizeOverride &&
 		(Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
-		Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE))
-	{
+			Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE)) {
 		Adapter->uiSectorSize = Adapter->uiSectorSizeInCFG;
-	}
-	else
-	{
-
+	} else {
 		uiSectorSig = FlashSectorSizeSig;
 
-		if(uiSectorSig == FLASH_SECTOR_SIZE_SIG)
-		{
+		if (uiSectorSig == FLASH_SECTOR_SIZE_SIG) {
 			uiSectorSize = FlashSectorSize;
-	//
-	// If the sector size stored in the FLASH makes sense then use it.
-	//
-			if(uiSectorSize <= MAX_SECTOR_SIZE && uiSectorSize >= MIN_SECTOR_SIZE)
-			{
+			/*
+			 * If the sector size stored in the FLASH makes sense then use it.
+			 */
+			if (uiSectorSize <= MAX_SECTOR_SIZE && uiSectorSize >= MIN_SECTOR_SIZE) {
 				Adapter->uiSectorSize = uiSectorSize;
-			}
-	//No valid size in FLASH, check if Config file has it.
-			else if(Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
-					Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE)
-			{
+			} else if (Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
+				Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE) {
+				/* No valid size in FLASH, check if Config file has it. */
 				Adapter->uiSectorSize = Adapter->uiSectorSizeInCFG;
-			}
-	// Init to Default, if none of the above works.
-			else
-			{
+			} else {
+				/* Init to Default, if none of the above works. */
 				Adapter->uiSectorSize = DEFAULT_SECTOR_SIZE;
 			}
-
-		}
-		else
-		{
-			if(Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
-					Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE)
-			{
+		} else {
+			if (Adapter->uiSectorSizeInCFG <= MAX_SECTOR_SIZE &&
+				Adapter->uiSectorSizeInCFG >= MIN_SECTOR_SIZE)
 				Adapter->uiSectorSize = Adapter->uiSectorSizeInCFG;
-			}
 			else
-			{
 				Adapter->uiSectorSize = DEFAULT_SECTOR_SIZE;
-			}
 		}
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Sector size  :%x \n", Adapter->uiSectorSize);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Sector size  :%x\n", Adapter->uiSectorSize);
+
 	return Adapter->uiSectorSize;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmInitEEPROMQueues
-//
-// Description: Initialization of EEPROM queues.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		<OSAL_STATUS_CODE>
-//-----------------------------------------------------------------------------
-
-static INT BcmInitEEPROMQueues(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	BcmInitEEPROMQueues
+ *
+ * Description: Initialization of EEPROM queues.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		<OSAL_STATUS_CODE>
+ */
+
+static int BcmInitEEPROMQueues(struct bcm_mini_adapter *Adapter)
 {
-	UINT value = 0;
+	unsigned int value = 0;
 	/* CHIP Bug : Clear the Avail bits on the Read queue. The default
 	 * value on this register is supposed to be 0x00001102.
-	 * But we get 0x00001122. */
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Fixing reset value on 0x0f003004 register\n" );
+	 * But we get 0x00001122.
+	 */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Fixing reset value on 0x0f003004 register\n");
 	value = EEPROM_READ_DATA_AVAIL;
-	wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
+	wrmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 
 	/* Flush the all the EEPROM queues. */
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " Flushing the queues\n");
-	value =EEPROM_ALL_QUEUE_FLUSH ;
-	wrmalt( Adapter, SPI_FLUSH_REG, &value, sizeof(value));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " Flushing the queues\n");
+	value = EEPROM_ALL_QUEUE_FLUSH;
+	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
 
 	value = 0;
-	wrmalt( Adapter, SPI_FLUSH_REG, &value, sizeof(value) );
+	wrmalt(Adapter, SPI_FLUSH_REG, &value, sizeof(value));
 
 	/* Read the EEPROM Status Register. Just to see, no real purpose. */
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "EEPROM Status register value = %x\n", ReadEEPROMStatusRegister(Adapter) );
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "EEPROM Status register value = %x\n", ReadEEPROMStatusRegister(Adapter));
 
 	return STATUS_SUCCESS;
 } /* BcmInitEEPROMQueues() */
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmInitNVM
-//
-// Description: Initialization of NVM, EEPROM size,FLASH size, sector size etc.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		<OSAL_STATUS_CODE>
-//-----------------------------------------------------------------------------
-
-INT BcmInitNVM(PMINI_ADAPTER ps_adapter)
+/*
+ * Procedure:	BcmInitNVM
+ *
+ * Description: Initialization of NVM, EEPROM size,FLASH size, sector size etc.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		<OSAL_STATUS_CODE>
+ */
+
+int BcmInitNVM(struct bcm_mini_adapter *ps_adapter)
 {
 	BcmValidateNvmType(ps_adapter);
 	BcmInitEEPROMQueues(ps_adapter);
 
-	if(ps_adapter->eNVMType == NVM_AUTODETECT)
-	{
+	if (ps_adapter->eNVMType == NVM_AUTODETECT) {
 		ps_adapter->eNVMType = BcmGetNvmType(ps_adapter);
-		if(ps_adapter->eNVMType == NVM_UNKNOWN)
-		{
-			BCM_DEBUG_PRINT(ps_adapter,DBG_TYPE_PRINTK, 0, 0, "NVM Type is unknown!!\n");
-		}
-	}
-	else if(ps_adapter->eNVMType == NVM_FLASH)
-	{
+		if (ps_adapter->eNVMType == NVM_UNKNOWN)
+			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_PRINTK, 0, 0, "NVM Type is unknown!!\n");
+	} else if (ps_adapter->eNVMType == NVM_FLASH) {
 		BcmGetFlashCSInfo(ps_adapter);
 	}
 
@@ -2435,115 +2143,107 @@ INT BcmInitNVM(PMINI_ADAPTER ps_adapter)
 
 	return STATUS_SUCCESS;
 }
-/***************************************************************************/
-/*BcmGetNvmSize : set the EEPROM or flash size in Adapter.
-*
-*Input Parameter:
-*		Adapter data structure
-*Return Value :
-*		0. means success;
-*/
-/***************************************************************************/
-
-static INT BcmGetNvmSize(PMINI_ADAPTER Adapter)
+
+/* BcmGetNvmSize : set the EEPROM or flash size in Adapter.
+ *
+ * Input Parameter:
+ *		Adapter data structure
+ * Return Value :
+ *		0. means success;
+ */
+
+static int BcmGetNvmSize(struct bcm_mini_adapter *Adapter)
 {
-	if(Adapter->eNVMType == NVM_EEPROM)
-	{
+	if (Adapter->eNVMType == NVM_EEPROM)
 		Adapter->uiNVMDSDSize = BcmGetEEPROMSize(Adapter);
-	}
-	else if(Adapter->eNVMType == NVM_FLASH)
-	{
+	else if (Adapter->eNVMType == NVM_FLASH)
 		Adapter->uiNVMDSDSize = BcmGetFlashSize(Adapter);
-	}
+
 	return 0;
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmValidateNvm
-//
-// Description: Validates the NVM Type option selected against the device
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		<VOID>
-//-----------------------------------------------------------------------------
-static VOID BcmValidateNvmType(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	BcmValidateNvm
+ *
+ * Description: Validates the NVM Type option selected against the device
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		<VOID>
+ */
+
+static VOID BcmValidateNvmType(struct bcm_mini_adapter *Adapter)
 {
+	/*
+	 * if forcing the FLASH through CFG file, we should ensure device really has a FLASH.
+	 * Accessing the FLASH address without the FLASH being present can cause hang/freeze etc.
+	 * So if NVM_FLASH is selected for older chipsets, change it to AUTODETECT where EEPROM is 1st choice.
+	 */
 
-	//
-	// if forcing the FLASH through CFG file, we should ensure device really has a FLASH.
-	// Accessing the FLASH address without the FLASH being present can cause hang/freeze etc.
-	// So if NVM_FLASH is selected for older chipsets, change it to AUTODETECT where EEPROM is 1st choice.
-	//
-
-	if(Adapter->eNVMType == NVM_FLASH &&
+	if (Adapter->eNVMType == NVM_FLASH &&
 		Adapter->chip_id < 0xBECE3300)
-	{
 		Adapter->eNVMType = NVM_AUTODETECT;
-	}
 }
-//-----------------------------------------------------------------------------
-// Procedure:	BcmReadFlashRDID
-//
-// Description: Reads ID from Serial Flash
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		Flash ID
-//-----------------------------------------------------------------------------
-static ULONG BcmReadFlashRDID(PMINI_ADAPTER Adapter)
+
+/*
+ * Procedure:	BcmReadFlashRDID
+ *
+ * Description: Reads ID from Serial Flash
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		Flash ID
+ */
+
+static ULONG BcmReadFlashRDID(struct bcm_mini_adapter *Adapter)
 {
 	ULONG ulRDID = 0;
-	UINT value;
-//
-// Read ID Instruction.
-//
-	value = (FLASH_CMD_READ_ID<<24);
-	wrmalt(Adapter, FLASH_SPI_CMDQ_REG,&value, sizeof(value));
-
-//Delay
-	udelay(10);
-//
-// Read SPI READQ REG. The output will be WWXXYYZZ.
-// The ID is 3Bytes long and is WWXXYY. ZZ needs to be Ignored.
-//
-	rdmalt(Adapter, FLASH_SPI_READQ_REG, (PUINT)&ulRDID, sizeof(ulRDID));
+	unsigned int value;
+
+	/*
+	 * Read ID Instruction.
+	 */
+	value = (FLASH_CMD_READ_ID << 24);
+	wrmalt(Adapter, FLASH_SPI_CMDQ_REG, &value, sizeof(value));
 
-	return (ulRDID >>8);
+	/* Delay */
+	udelay(10);
 
+	/*
+	 * Read SPI READQ REG. The output will be WWXXYYZZ.
+	 * The ID is 3Bytes long and is WWXXYY. ZZ needs to be Ignored.
+	 */
+	rdmalt(Adapter, FLASH_SPI_READQ_REG, (PUINT)&ulRDID, sizeof(ulRDID));
 
+	return ulRDID >> 8;
 }
 
-INT BcmAllocFlashCSStructure(PMINI_ADAPTER psAdapter)
+int BcmAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter)
 {
-	if(psAdapter == NULL)
-	{
-		BCM_DEBUG_PRINT(psAdapter,DBG_TYPE_PRINTK, 0, 0, "Adapter structure point is NULL");
+	if (!psAdapter) {
+		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure point is NULL");
 		return -EINVAL;
 	}
 	psAdapter->psFlashCSInfo = (PFLASH_CS_INFO)kzalloc(sizeof(FLASH_CS_INFO), GFP_KERNEL);
-	if(psAdapter->psFlashCSInfo == NULL)
-	{
-		BCM_DEBUG_PRINT(psAdapter,DBG_TYPE_PRINTK, 0, 0,"Can't Allocate memory for Flash 1.x");
+	if (psAdapter->psFlashCSInfo == NULL) {
+		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate memory for Flash 1.x");
 		return -ENOMEM;
 	}
 
 	psAdapter->psFlash2xCSInfo = (PFLASH2X_CS_INFO)kzalloc(sizeof(FLASH2X_CS_INFO), GFP_KERNEL);
-	if(psAdapter->psFlash2xCSInfo == NULL)
-	{
-		BCM_DEBUG_PRINT(psAdapter,DBG_TYPE_PRINTK, 0, 0,"Can't Allocate memory for Flash 2.x");
+	if (!psAdapter->psFlash2xCSInfo) {
+		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate memory for Flash 2.x");
 		kfree(psAdapter->psFlashCSInfo);
 		return -ENOMEM;
 	}
 
 	psAdapter->psFlash2xVendorInfo = (PFLASH2X_VENDORSPECIFIC_INFO)kzalloc(sizeof(FLASH2X_VENDORSPECIFIC_INFO), GFP_KERNEL);
-	if(psAdapter->psFlash2xVendorInfo == NULL)
-	{
-		BCM_DEBUG_PRINT(psAdapter,DBG_TYPE_PRINTK, 0, 0,"Can't Allocate Vendor Info Memory for Flash 2.x");
+	if (!psAdapter->psFlash2xVendorInfo) {
+		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Can't Allocate Vendor Info Memory for Flash 2.x");
 		kfree(psAdapter->psFlashCSInfo);
 		kfree(psAdapter->psFlash2xCSInfo);
 		return -ENOMEM;
@@ -2552,91 +2252,90 @@ INT BcmAllocFlashCSStructure(PMINI_ADAPTER psAdapter)
 	return STATUS_SUCCESS;
 }
 
-INT BcmDeAllocFlashCSStructure(PMINI_ADAPTER psAdapter)
+int BcmDeAllocFlashCSStructure(struct bcm_mini_adapter *psAdapter)
 {
-	if(psAdapter == NULL)
-	{
-		BCM_DEBUG_PRINT(psAdapter,DBG_TYPE_PRINTK, 0, 0," Adapter structure point is NULL");
+	if (!psAdapter) {
+		BCM_DEBUG_PRINT(psAdapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure point is NULL");
 		return -EINVAL;
 	}
 	kfree(psAdapter->psFlashCSInfo);
 	kfree(psAdapter->psFlash2xCSInfo);
 	kfree(psAdapter->psFlash2xVendorInfo);
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 }
 
-static INT	BcmDumpFlash2XCSStructure(PFLASH2X_CS_INFO psFlash2xCSInfo,PMINI_ADAPTER Adapter)
+static int BcmDumpFlash2XCSStructure(PFLASH2X_CS_INFO psFlash2xCSInfo, struct bcm_mini_adapter *Adapter)
 {
-	UINT Index = 0;
-    BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "**********************FLASH2X CS Structure *******************");
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is  :%x", (psFlash2xCSInfo->MagicNumber));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Major Version :%d", MAJOR_VERSION(psFlash2xCSInfo->FlashLayoutVersion));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Minor Version :%d", MINOR_VERSION(psFlash2xCSInfo->FlashLayoutVersion));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " ISOImageMajorVersion:0x%x", (psFlash2xCSInfo->ISOImageVersion));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SCSIFirmwareMajorVersion :0x%x", (psFlash2xCSInfo->SCSIFirmwareVersion));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForPart1ISOImage :0x%x", (psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForScsiFirmware :0x%x", (psFlash2xCSInfo->OffsetFromZeroForScsiFirmware));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SizeOfScsiFirmware  :0x%x", (psFlash2xCSInfo->SizeOfScsiFirmware ));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForPart2ISOImage :0x%x", (psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSDStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSDStart));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSDEnd :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSDEnd));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSAStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSAStart));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSAEnd :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSAEnd));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForControlSectionStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForControlSectionStart));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForControlSectionData :0x%x", (psFlash2xCSInfo->OffsetFromZeroForControlSectionData));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "CDLessInactivityTimeout :0x%x", (psFlash2xCSInfo->CDLessInactivityTimeout));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "NewImageSignature :0x%x", (psFlash2xCSInfo->NewImageSignature));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashSectorSizeSig :0x%x", (psFlash2xCSInfo->FlashSectorSizeSig));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashSectorSize :0x%x", (psFlash2xCSInfo->FlashSectorSize));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashWriteSupportSize :0x%x", (psFlash2xCSInfo->FlashWriteSupportSize));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "TotalFlashSize :0x%X", (psFlash2xCSInfo->TotalFlashSize));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashBaseAddr :0x%x", (psFlash2xCSInfo->FlashBaseAddr));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashPartMaxSize :0x%x", (psFlash2xCSInfo->FlashPartMaxSize));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "IsCDLessDeviceBootSig :0x%x", (psFlash2xCSInfo->IsCDLessDeviceBootSig));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "MassStorageTimeout :0x%x", (psFlash2xCSInfo->MassStorageTimeout));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part1Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part1Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part1End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part1End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part2Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part2Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part2End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part2End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part3Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part3Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part3End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part3End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part1Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part1Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part1End	:0x%x", (psFlash2xCSInfo->OffsetISOImage2Part1End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part2Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part2Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part2End :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part2End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part3Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part3Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part3End :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part3End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromDSDStartForDSDHeader :0x%x", (psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD1Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD1Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD1End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD1End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD2Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD2Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD2End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD2End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA1Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA1Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA1End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA1End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA2Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA2Start));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA2End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA2End));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Sector Access Bit Map is Defined as :");
-	for(Index =0; Index <(FLASH2X_TOTAL_SIZE/(DEFAULT_SECTOR_SIZE *16)); Index++)
-	{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SectorAccessBitMap[%d] :0x%x", Index,
+	unsigned int Index = 0;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "**********************FLASH2X CS Structure *******************");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is  :%x", (psFlash2xCSInfo->MagicNumber));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Major Version :%d", MAJOR_VERSION(psFlash2xCSInfo->FlashLayoutVersion));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Minor Version :%d", MINOR_VERSION(psFlash2xCSInfo->FlashLayoutVersion));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " ISOImageMajorVersion:0x%x", (psFlash2xCSInfo->ISOImageVersion));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SCSIFirmwareMajorVersion :0x%x", (psFlash2xCSInfo->SCSIFirmwareVersion));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForPart1ISOImage :0x%x", (psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForScsiFirmware :0x%x", (psFlash2xCSInfo->OffsetFromZeroForScsiFirmware));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SizeOfScsiFirmware  :0x%x", (psFlash2xCSInfo->SizeOfScsiFirmware));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForPart2ISOImage :0x%x", (psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSDStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSDStart));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSDEnd :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSDEnd));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSAStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSAStart));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSAEnd :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSAEnd));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForControlSectionStart :0x%x", (psFlash2xCSInfo->OffsetFromZeroForControlSectionStart));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForControlSectionData :0x%x", (psFlash2xCSInfo->OffsetFromZeroForControlSectionData));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "CDLessInactivityTimeout :0x%x", (psFlash2xCSInfo->CDLessInactivityTimeout));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "NewImageSignature :0x%x", (psFlash2xCSInfo->NewImageSignature));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashSectorSizeSig :0x%x", (psFlash2xCSInfo->FlashSectorSizeSig));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashSectorSize :0x%x", (psFlash2xCSInfo->FlashSectorSize));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashWriteSupportSize :0x%x", (psFlash2xCSInfo->FlashWriteSupportSize));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "TotalFlashSize :0x%X", (psFlash2xCSInfo->TotalFlashSize));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashBaseAddr :0x%x", (psFlash2xCSInfo->FlashBaseAddr));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FlashPartMaxSize :0x%x", (psFlash2xCSInfo->FlashPartMaxSize));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "IsCDLessDeviceBootSig :0x%x", (psFlash2xCSInfo->IsCDLessDeviceBootSig));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "MassStorageTimeout :0x%x", (psFlash2xCSInfo->MassStorageTimeout));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part1Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part1Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part1End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part1End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part2Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part2Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part2End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part2End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part3Start :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part3Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage1Part3End :0x%x", (psFlash2xCSInfo->OffsetISOImage1Part3End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part1Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part1Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part1End	:0x%x", (psFlash2xCSInfo->OffsetISOImage2Part1End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part2Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part2Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part2End :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part2End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part3Start :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part3Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetISOImage2Part3End :0x%x", (psFlash2xCSInfo->OffsetISOImage2Part3End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromDSDStartForDSDHeader :0x%x", (psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD1Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD1Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD1End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD1End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD2Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD2Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForDSD2End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForDSD2End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA1Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA1Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA1End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA1End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA2Start :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA2Start));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "OffsetFromZeroForVSA2End :0x%x", (psFlash2xCSInfo->OffsetFromZeroForVSA2End));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Sector Access Bit Map is Defined as :");
+
+	for (Index = 0; Index < (FLASH2X_TOTAL_SIZE / (DEFAULT_SECTOR_SIZE * 16)); Index++)
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SectorAccessBitMap[%d] :0x%x", Index,
 				(psFlash2xCSInfo->SectorAccessBitMap[Index]));
-	}
 
 	return STATUS_SUCCESS;
 }
 
-
-static INT	ConvertEndianOf2XCSStructure(PFLASH2X_CS_INFO psFlash2xCSInfo)
+static int ConvertEndianOf2XCSStructure(PFLASH2X_CS_INFO psFlash2xCSInfo)
 {
-	UINT Index = 0;
+	unsigned int Index = 0;
+
 	psFlash2xCSInfo->MagicNumber = ntohl(psFlash2xCSInfo->MagicNumber);
-	psFlash2xCSInfo->FlashLayoutVersion= ntohl(psFlash2xCSInfo->FlashLayoutVersion);
-	//psFlash2xCSInfo->FlashLayoutMinorVersion = ntohs(psFlash2xCSInfo->FlashLayoutMinorVersion);
+	psFlash2xCSInfo->FlashLayoutVersion = ntohl(psFlash2xCSInfo->FlashLayoutVersion);
+	/* psFlash2xCSInfo->FlashLayoutMinorVersion = ntohs(psFlash2xCSInfo->FlashLayoutMinorVersion); */
 	psFlash2xCSInfo->ISOImageVersion = ntohl(psFlash2xCSInfo->ISOImageVersion);
-	psFlash2xCSInfo->SCSIFirmwareVersion =ntohl(psFlash2xCSInfo->SCSIFirmwareVersion);
+	psFlash2xCSInfo->SCSIFirmwareVersion = ntohl(psFlash2xCSInfo->SCSIFirmwareVersion);
 	psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage = ntohl(psFlash2xCSInfo->OffsetFromZeroForPart1ISOImage);
 	psFlash2xCSInfo->OffsetFromZeroForScsiFirmware = ntohl(psFlash2xCSInfo->OffsetFromZeroForScsiFirmware);
-	psFlash2xCSInfo->SizeOfScsiFirmware = ntohl(psFlash2xCSInfo->SizeOfScsiFirmware );
+	psFlash2xCSInfo->SizeOfScsiFirmware = ntohl(psFlash2xCSInfo->SizeOfScsiFirmware);
 	psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage = ntohl(psFlash2xCSInfo->OffsetFromZeroForPart2ISOImage);
 	psFlash2xCSInfo->OffsetFromZeroForDSDStart = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSDStart);
 	psFlash2xCSInfo->OffsetFromZeroForDSDEnd = ntohl(psFlash2xCSInfo->OffsetFromZeroForDSDEnd);
@@ -2675,119 +2374,115 @@ static INT	ConvertEndianOf2XCSStructure(PFLASH2X_CS_INFO psFlash2xCSInfo)
 	psFlash2xCSInfo->OffsetFromZeroForVSA1End = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA1End);
 	psFlash2xCSInfo->OffsetFromZeroForVSA2Start = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA2Start);
 	psFlash2xCSInfo->OffsetFromZeroForVSA2End = ntohl(psFlash2xCSInfo->OffsetFromZeroForVSA2End);
-	for(Index =0; Index <(FLASH2X_TOTAL_SIZE/(DEFAULT_SECTOR_SIZE *16)); Index++)
-	{
-			psFlash2xCSInfo->SectorAccessBitMap[Index] = ntohl(psFlash2xCSInfo->SectorAccessBitMap[Index]);
-	}
+
+	for (Index = 0; Index < (FLASH2X_TOTAL_SIZE / (DEFAULT_SECTOR_SIZE * 16)); Index++)
+		psFlash2xCSInfo->SectorAccessBitMap[Index] = ntohl(psFlash2xCSInfo->SectorAccessBitMap[Index]);
+
 	return STATUS_SUCCESS;
 }
 
-static INT	ConvertEndianOfCSStructure(PFLASH_CS_INFO psFlashCSInfo)
+static int ConvertEndianOfCSStructure(PFLASH_CS_INFO psFlashCSInfo)
 {
-	//UINT Index = 0;
-	psFlashCSInfo->MagicNumber					 		=ntohl(psFlashCSInfo->MagicNumber);
-	psFlashCSInfo->FlashLayoutVersion					=ntohl(psFlashCSInfo->FlashLayoutVersion);
-	psFlashCSInfo->ISOImageVersion 						= ntohl(psFlashCSInfo->ISOImageVersion);
-	//won't convert according to old assumption
-	psFlashCSInfo->SCSIFirmwareVersion =(psFlashCSInfo->SCSIFirmwareVersion);
-
-	psFlashCSInfo->OffsetFromZeroForPart1ISOImage  		= ntohl(psFlashCSInfo->OffsetFromZeroForPart1ISOImage);
-	psFlashCSInfo->OffsetFromZeroForScsiFirmware        = ntohl(psFlashCSInfo->OffsetFromZeroForScsiFirmware);
-	psFlashCSInfo->SizeOfScsiFirmware                   = ntohl(psFlashCSInfo->SizeOfScsiFirmware );
-	psFlashCSInfo->OffsetFromZeroForPart2ISOImage       = ntohl(psFlashCSInfo->OffsetFromZeroForPart2ISOImage);
-	psFlashCSInfo->OffsetFromZeroForCalibrationStart    = ntohl(psFlashCSInfo->OffsetFromZeroForCalibrationStart);
-	psFlashCSInfo->OffsetFromZeroForCalibrationEnd      = ntohl(psFlashCSInfo->OffsetFromZeroForCalibrationEnd);
-	psFlashCSInfo->OffsetFromZeroForVSAStart            = ntohl(psFlashCSInfo->OffsetFromZeroForVSAStart);
-	psFlashCSInfo->OffsetFromZeroForVSAEnd              = ntohl(psFlashCSInfo->OffsetFromZeroForVSAEnd);
-	psFlashCSInfo->OffsetFromZeroForControlSectionStart = ntohl(psFlashCSInfo->OffsetFromZeroForControlSectionStart);
-	psFlashCSInfo->OffsetFromZeroForControlSectionData  = ntohl(psFlashCSInfo->OffsetFromZeroForControlSectionData);
-	psFlashCSInfo->CDLessInactivityTimeout 				= ntohl(psFlashCSInfo->CDLessInactivityTimeout);
-	psFlashCSInfo->NewImageSignature                    = ntohl(psFlashCSInfo->NewImageSignature);
-	psFlashCSInfo->FlashSectorSizeSig                   = ntohl(psFlashCSInfo->FlashSectorSizeSig);
-	psFlashCSInfo->FlashSectorSize                      = ntohl(psFlashCSInfo->FlashSectorSize);
-	psFlashCSInfo->FlashWriteSupportSize                = ntohl(psFlashCSInfo->FlashWriteSupportSize);
-	psFlashCSInfo->TotalFlashSize        				= ntohl(psFlashCSInfo->TotalFlashSize);
- 	psFlashCSInfo->FlashBaseAddr         				= ntohl(psFlashCSInfo->FlashBaseAddr);
-	psFlashCSInfo->FlashPartMaxSize      				= ntohl(psFlashCSInfo->FlashPartMaxSize);
- 	psFlashCSInfo->IsCDLessDeviceBootSig 				= ntohl(psFlashCSInfo->IsCDLessDeviceBootSig);
-	psFlashCSInfo->MassStorageTimeout    				= ntohl(psFlashCSInfo->MassStorageTimeout);
+	/* unsigned int Index = 0; */
+	psFlashCSInfo->MagicNumber				= ntohl(psFlashCSInfo->MagicNumber);
+	psFlashCSInfo->FlashLayoutVersion			= ntohl(psFlashCSInfo->FlashLayoutVersion);
+	psFlashCSInfo->ISOImageVersion				= ntohl(psFlashCSInfo->ISOImageVersion);
+	/* won't convert according to old assumption */
+	psFlashCSInfo->SCSIFirmwareVersion			= (psFlashCSInfo->SCSIFirmwareVersion);
+	psFlashCSInfo->OffsetFromZeroForPart1ISOImage		= ntohl(psFlashCSInfo->OffsetFromZeroForPart1ISOImage);
+	psFlashCSInfo->OffsetFromZeroForScsiFirmware		= ntohl(psFlashCSInfo->OffsetFromZeroForScsiFirmware);
+	psFlashCSInfo->SizeOfScsiFirmware			= ntohl(psFlashCSInfo->SizeOfScsiFirmware);
+	psFlashCSInfo->OffsetFromZeroForPart2ISOImage		= ntohl(psFlashCSInfo->OffsetFromZeroForPart2ISOImage);
+	psFlashCSInfo->OffsetFromZeroForCalibrationStart	= ntohl(psFlashCSInfo->OffsetFromZeroForCalibrationStart);
+	psFlashCSInfo->OffsetFromZeroForCalibrationEnd		= ntohl(psFlashCSInfo->OffsetFromZeroForCalibrationEnd);
+	psFlashCSInfo->OffsetFromZeroForVSAStart		= ntohl(psFlashCSInfo->OffsetFromZeroForVSAStart);
+	psFlashCSInfo->OffsetFromZeroForVSAEnd			= ntohl(psFlashCSInfo->OffsetFromZeroForVSAEnd);
+	psFlashCSInfo->OffsetFromZeroForControlSectionStart	= ntohl(psFlashCSInfo->OffsetFromZeroForControlSectionStart);
+	psFlashCSInfo->OffsetFromZeroForControlSectionData	= ntohl(psFlashCSInfo->OffsetFromZeroForControlSectionData);
+	psFlashCSInfo->CDLessInactivityTimeout			= ntohl(psFlashCSInfo->CDLessInactivityTimeout);
+	psFlashCSInfo->NewImageSignature			= ntohl(psFlashCSInfo->NewImageSignature);
+	psFlashCSInfo->FlashSectorSizeSig			= ntohl(psFlashCSInfo->FlashSectorSizeSig);
+	psFlashCSInfo->FlashSectorSize				= ntohl(psFlashCSInfo->FlashSectorSize);
+	psFlashCSInfo->FlashWriteSupportSize			= ntohl(psFlashCSInfo->FlashWriteSupportSize);
+	psFlashCSInfo->TotalFlashSize				= ntohl(psFlashCSInfo->TotalFlashSize);
+	psFlashCSInfo->FlashBaseAddr				= ntohl(psFlashCSInfo->FlashBaseAddr);
+	psFlashCSInfo->FlashPartMaxSize				= ntohl(psFlashCSInfo->FlashPartMaxSize);
+	psFlashCSInfo->IsCDLessDeviceBootSig			= ntohl(psFlashCSInfo->IsCDLessDeviceBootSig);
+	psFlashCSInfo->MassStorageTimeout			= ntohl(psFlashCSInfo->MassStorageTimeout);
 
 	return STATUS_SUCCESS;
 }
 
-static INT IsSectionExistInVendorInfo(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL section)
+static int IsSectionExistInVendorInfo(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL section)
 {
- 	return ( Adapter->uiVendorExtnFlag &&
- 		(Adapter->psFlash2xVendorInfo->VendorSection[section].AccessFlags & FLASH2X_SECTION_PRESENT) &&
- 		(Adapter->psFlash2xVendorInfo->VendorSection[section].OffsetFromZeroForSectionStart != UNINIT_PTR_IN_CS) );
+	return (Adapter->uiVendorExtnFlag &&
+		(Adapter->psFlash2xVendorInfo->VendorSection[section].AccessFlags & FLASH2X_SECTION_PRESENT) &&
+		(Adapter->psFlash2xVendorInfo->VendorSection[section].OffsetFromZeroForSectionStart != UNINIT_PTR_IN_CS));
 }
 
-static VOID UpdateVendorInfo(PMINI_ADAPTER Adapter)
+static VOID UpdateVendorInfo(struct bcm_mini_adapter *Adapter)
 {
 	B_UINT32 i = 0;
-	UINT uiSizeSection = 0;
+	unsigned int uiSizeSection = 0;
 
 	Adapter->uiVendorExtnFlag = FALSE;
 
-	for(i = 0;i < TOTAL_SECTIONS;i++)
+	for (i = 0; i < TOTAL_SECTIONS; i++)
 		Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart = UNINIT_PTR_IN_CS;
 
-	if(STATUS_SUCCESS != vendorextnGetSectionInfo(Adapter, Adapter->psFlash2xVendorInfo))
+	if (STATUS_SUCCESS != vendorextnGetSectionInfo(Adapter, Adapter->psFlash2xVendorInfo))
 		return;
 
 	i = 0;
-	while(i < TOTAL_SECTIONS)
-	{
-		if(!(Adapter->psFlash2xVendorInfo->VendorSection[i].AccessFlags & FLASH2X_SECTION_PRESENT))
-		{
+	while (i < TOTAL_SECTIONS) {
+		if (!(Adapter->psFlash2xVendorInfo->VendorSection[i].AccessFlags & FLASH2X_SECTION_PRESENT)) {
 			i++;
 			continue;
 		}
 
 		Adapter->uiVendorExtnFlag = TRUE;
 		uiSizeSection = (Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionEnd -
-						Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart);
+				Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart);
 
-		switch(i)
-		{
+		switch (i) {
 		case DSD0:
-			if(( uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER))) &&
-			(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
+			if ((uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER))) &&
+				(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd = VENDOR_PTR_IN_CS;
 			else
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd = UNINIT_PTR_IN_CS;
 			break;
 
 		case DSD1:
-			if(( uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER))) &&
-			(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
+			if ((uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER))) &&
+				(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End = VENDOR_PTR_IN_CS;
 			else
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End = UNINIT_PTR_IN_CS;
 			break;
 
 		case DSD2:
-			if(( uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER))) &&
-			(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
+			if ((uiSizeSection >= (Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER))) &&
+				(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart))
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End = VENDOR_PTR_IN_CS;
 			else
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End = UNINIT_PTR_IN_CS;
 			break;
 		case VSA0:
-			if(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
+			if (UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd = VENDOR_PTR_IN_CS;
 			else
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd = UNINIT_PTR_IN_CS;
 			break;
 
 		case VSA1:
-			if(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
+			if (UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End = VENDOR_PTR_IN_CS;
 			else
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End = UNINIT_PTR_IN_CS;
 			break;
 		case VSA2:
-			if(UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
+			if (UNINIT_PTR_IN_CS != Adapter->psFlash2xVendorInfo->VendorSection[i].OffsetFromZeroForSectionStart)
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End = VENDOR_PTR_IN_CS;
 			else
 				Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start = Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End = UNINIT_PTR_IN_CS;
@@ -2798,133 +2493,111 @@ static VOID UpdateVendorInfo(PMINI_ADAPTER Adapter)
 		}
 		i++;
 	}
-
 }
 
-//-----------------------------------------------------------------------------
-// Procedure:	BcmGetFlashCSInfo
-//
-// Description: Reads control structure and gets Cal section addresses.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		<VOID>
-//-----------------------------------------------------------------------------
-
-static INT BcmGetFlashCSInfo(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	BcmGetFlashCSInfo
+ *
+ * Description: Reads control structure and gets Cal section addresses.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		<VOID>
+ */
+
+static int BcmGetFlashCSInfo(struct bcm_mini_adapter *Adapter)
 {
-	//FLASH_CS_INFO sFlashCsInfo = {0};
+	/* FLASH_CS_INFO sFlashCsInfo = {0}; */
 
-#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-	UINT value;
-#endif
-	UINT uiFlashLayoutMajorVersion;
+	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
+		unsigned int value;
+	#endif
+
+	unsigned int uiFlashLayoutMajorVersion;
 	Adapter->uiFlashLayoutMinorVersion = 0;
 	Adapter->uiFlashLayoutMajorVersion = 0;
 	Adapter->ulFlashControlSectionStart = FLASH_CS_INFO_START_ADDR;
 
-
 	Adapter->uiFlashBaseAdd = 0;
 	Adapter->ulFlashCalStart = 0;
-	memset(Adapter->psFlashCSInfo, 0 ,sizeof(FLASH_CS_INFO));
-	memset(Adapter->psFlash2xCSInfo, 0 ,sizeof(FLASH2X_CS_INFO));
+	memset(Adapter->psFlashCSInfo, 0 , sizeof(FLASH_CS_INFO));
+	memset(Adapter->psFlash2xCSInfo, 0 , sizeof(FLASH2X_CS_INFO));
 
-	if(!Adapter->bDDRInitDone)
-	{
-		{
-			value = FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
-			wrmalt(Adapter, 0xAF00A080, &value, sizeof(value));
-		}
+	if (!Adapter->bDDRInitDone) {
+		value = FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
+		wrmalt(Adapter, 0xAF00A080, &value, sizeof(value));
 	}
 
-
-	// Reading first 8 Bytes to get the Flash Layout
-	// MagicNumber(4 bytes) +FlashLayoutMinorVersion(2 Bytes) +FlashLayoutMajorVersion(2 Bytes)
-	BeceemFlashBulkRead(Adapter,(PUINT)Adapter->psFlashCSInfo,Adapter->ulFlashControlSectionStart,8);
+	/* Reading first 8 Bytes to get the Flash Layout
+	 * MagicNumber(4 bytes) +FlashLayoutMinorVersion(2 Bytes) +FlashLayoutMajorVersion(2 Bytes)
+	 */
+	BeceemFlashBulkRead(Adapter, (PUINT)Adapter->psFlashCSInfo, Adapter->ulFlashControlSectionStart, 8);
 
 	Adapter->psFlashCSInfo->FlashLayoutVersion =  ntohl(Adapter->psFlashCSInfo->FlashLayoutVersion);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Layout Version :%X", (Adapter->psFlashCSInfo->FlashLayoutVersion));
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Layout Minor Version :%d\n", ntohs(sFlashCsInfo.FlashLayoutMinorVersion));
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is  :%x\n", ntohl(Adapter->psFlashCSInfo->MagicNumber));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Layout Version :%X", (Adapter->psFlashCSInfo->FlashLayoutVersion));
+	/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Flash Layout Minor Version :%d\n", ntohs(sFlashCsInfo.FlashLayoutMinorVersion)); */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is  :%x\n", ntohl(Adapter->psFlashCSInfo->MagicNumber));
 
-	if(FLASH_CONTROL_STRUCT_SIGNATURE == ntohl(Adapter->psFlashCSInfo->MagicNumber))
-	{
+	if (FLASH_CONTROL_STRUCT_SIGNATURE == ntohl(Adapter->psFlashCSInfo->MagicNumber)) {
 		uiFlashLayoutMajorVersion = MAJOR_VERSION((Adapter->psFlashCSInfo->FlashLayoutVersion));
 		Adapter->uiFlashLayoutMinorVersion = MINOR_VERSION((Adapter->psFlashCSInfo->FlashLayoutVersion));
-	}
-	else
-	{
+	} else {
 		Adapter->uiFlashLayoutMinorVersion = 0;
 		uiFlashLayoutMajorVersion = 0;
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"FLASH LAYOUT MAJOR VERSION :%X", uiFlashLayoutMajorVersion);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "FLASH LAYOUT MAJOR VERSION :%X", uiFlashLayoutMajorVersion);
 
-	if(uiFlashLayoutMajorVersion < FLASH_2X_MAJOR_NUMBER)
-	{
-		BeceemFlashBulkRead(Adapter,(PUINT)Adapter->psFlashCSInfo,Adapter->ulFlashControlSectionStart,sizeof(FLASH_CS_INFO));
+	if (uiFlashLayoutMajorVersion < FLASH_2X_MAJOR_NUMBER) {
+		BeceemFlashBulkRead(Adapter, (PUINT)Adapter->psFlashCSInfo, Adapter->ulFlashControlSectionStart, sizeof(FLASH_CS_INFO));
 		ConvertEndianOfCSStructure(Adapter->psFlashCSInfo);
 		Adapter->ulFlashCalStart = (Adapter->psFlashCSInfo->OffsetFromZeroForCalibrationStart);
 
-		if(!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
-		{
+		if (!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
 			Adapter->ulFlashControlSectionStart = Adapter->psFlashCSInfo->OffsetFromZeroForControlSectionStart;
-		}
 
-		if((FLASH_CONTROL_STRUCT_SIGNATURE == (Adapter->psFlashCSInfo->MagicNumber)) &&
-		   (SCSI_FIRMWARE_MINOR_VERSION <= MINOR_VERSION(Adapter->psFlashCSInfo->SCSIFirmwareVersion)) &&
-		   (FLASH_SECTOR_SIZE_SIG == (Adapter->psFlashCSInfo->FlashSectorSizeSig)) &&
-		   (BYTE_WRITE_SUPPORT == (Adapter->psFlashCSInfo->FlashWriteSupportSize)))
-		{
+		if ((FLASH_CONTROL_STRUCT_SIGNATURE == (Adapter->psFlashCSInfo->MagicNumber)) &&
+			(SCSI_FIRMWARE_MINOR_VERSION <= MINOR_VERSION(Adapter->psFlashCSInfo->SCSIFirmwareVersion)) &&
+			(FLASH_SECTOR_SIZE_SIG == (Adapter->psFlashCSInfo->FlashSectorSizeSig)) &&
+			(BYTE_WRITE_SUPPORT == (Adapter->psFlashCSInfo->FlashWriteSupportSize))) {
 			Adapter->ulFlashWriteSize = (Adapter->psFlashCSInfo->FlashWriteSupportSize);
-		   	Adapter->fpFlashWrite = flashByteWrite;
-		   	Adapter->fpFlashWriteWithStatusCheck = flashByteWriteStatus;
-		}
-		else
-		{
+			Adapter->fpFlashWrite = flashByteWrite;
+			Adapter->fpFlashWriteWithStatusCheck = flashByteWriteStatus;
+		} else {
 			Adapter->ulFlashWriteSize = MAX_RW_SIZE;
 			Adapter->fpFlashWrite = flashWrite;
-		   	Adapter->fpFlashWriteWithStatusCheck = flashWriteStatus;
+			Adapter->fpFlashWriteWithStatusCheck = flashWriteStatus;
 		}
 
 		BcmGetFlashSectorSize(Adapter, (Adapter->psFlashCSInfo->FlashSectorSizeSig),
-					 (Adapter->psFlashCSInfo->FlashSectorSize));
-
-
+				(Adapter->psFlashCSInfo->FlashSectorSize));
 		Adapter->uiFlashBaseAdd = Adapter->psFlashCSInfo->FlashBaseAddr & 0xFCFFFFFF;
-
-
-	}
-	else
-	{
-		if(BcmFlash2xBulkRead(Adapter,(PUINT)Adapter->psFlash2xCSInfo,NO_SECTION_VAL,
-				Adapter->ulFlashControlSectionStart,sizeof(FLASH2X_CS_INFO)))
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Unable to read CS structure \n");
+	} else {
+		if (BcmFlash2xBulkRead(Adapter, (PUINT)Adapter->psFlash2xCSInfo, NO_SECTION_VAL,
+					Adapter->ulFlashControlSectionStart, sizeof(FLASH2X_CS_INFO))) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Unable to read CS structure\n");
 			return STATUS_FAILURE;
 		}
+
 		ConvertEndianOf2XCSStructure(Adapter->psFlash2xCSInfo);
-		BcmDumpFlash2XCSStructure(Adapter->psFlash2xCSInfo,Adapter);
-		if((FLASH_CONTROL_STRUCT_SIGNATURE == Adapter->psFlash2xCSInfo->MagicNumber) &&
-		   (SCSI_FIRMWARE_MINOR_VERSION <= MINOR_VERSION(Adapter->psFlash2xCSInfo->SCSIFirmwareVersion)) &&
-		   (FLASH_SECTOR_SIZE_SIG == Adapter->psFlash2xCSInfo->FlashSectorSizeSig) &&
-		   (BYTE_WRITE_SUPPORT == Adapter->psFlash2xCSInfo->FlashWriteSupportSize))
-		{
+		BcmDumpFlash2XCSStructure(Adapter->psFlash2xCSInfo, Adapter);
+		if ((FLASH_CONTROL_STRUCT_SIGNATURE == Adapter->psFlash2xCSInfo->MagicNumber) &&
+			(SCSI_FIRMWARE_MINOR_VERSION <= MINOR_VERSION(Adapter->psFlash2xCSInfo->SCSIFirmwareVersion)) &&
+			(FLASH_SECTOR_SIZE_SIG == Adapter->psFlash2xCSInfo->FlashSectorSizeSig) &&
+			(BYTE_WRITE_SUPPORT == Adapter->psFlash2xCSInfo->FlashWriteSupportSize)) {
 			Adapter->ulFlashWriteSize = Adapter->psFlash2xCSInfo->FlashWriteSupportSize;
-		   	Adapter->fpFlashWrite = flashByteWrite;
-		   	Adapter->fpFlashWriteWithStatusCheck = flashByteWriteStatus;
-		}
-		else
-		{
+			Adapter->fpFlashWrite = flashByteWrite;
+			Adapter->fpFlashWriteWithStatusCheck = flashByteWriteStatus;
+		} else {
 			Adapter->ulFlashWriteSize = MAX_RW_SIZE;
 			Adapter->fpFlashWrite = flashWrite;
-		   	Adapter->fpFlashWriteWithStatusCheck = flashWriteStatus;
+			Adapter->fpFlashWriteWithStatusCheck = flashWriteStatus;
 		}
 
 		BcmGetFlashSectorSize(Adapter, Adapter->psFlash2xCSInfo->FlashSectorSizeSig,
-					Adapter->psFlash2xCSInfo->FlashSectorSize);
+				Adapter->psFlash2xCSInfo->FlashSectorSize);
 
 		UpdateVendorInfo(Adapter);
 
@@ -2932,426 +2605,394 @@ static INT BcmGetFlashCSInfo(PMINI_ADAPTER Adapter)
 		BcmGetActiveISO(Adapter);
 		Adapter->uiFlashBaseAdd = Adapter->psFlash2xCSInfo->FlashBaseAddr & 0xFCFFFFFF;
 		Adapter->ulFlashControlSectionStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart;
-
 	}
 	/*
-	Concerns: what if CS sector size does not match with this sector size ???
-	what is the indication of AccessBitMap  in CS in flash 2.x ????
-	*/
+	 * Concerns: what if CS sector size does not match with this sector size ???
+	 * what is the indication of AccessBitMap  in CS in flash 2.x ????
+	 */
 	Adapter->ulFlashID = BcmReadFlashRDID(Adapter);
-
 	Adapter->uiFlashLayoutMajorVersion = uiFlashLayoutMajorVersion;
 
-
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:	BcmGetNvmType
-//
-// Description: Finds the type of NVM used.
-//
-// Arguments:
-//		Adapter    - ptr to Adapter object instance
-//
-// Returns:
-//		NVM_TYPE
-//
-//-----------------------------------------------------------------------------
-
-static NVM_TYPE BcmGetNvmType(PMINI_ADAPTER Adapter)
+/*
+ * Procedure:	BcmGetNvmType
+ *
+ * Description: Finds the type of NVM used.
+ *
+ * Arguments:
+ *		Adapter    - ptr to Adapter object instance
+ *
+ * Returns:
+ *		NVM_TYPE
+ *
+ */
+
+static NVM_TYPE BcmGetNvmType(struct bcm_mini_adapter *Adapter)
 {
-	UINT uiData = 0;
+	unsigned int uiData = 0;
 
-	BeceemEEPROMBulkRead(Adapter,&uiData,0x0,4);
-	if(uiData == BECM)
-	{
+	BeceemEEPROMBulkRead(Adapter, &uiData, 0x0, 4);
+	if (uiData == BECM)
 		return NVM_EEPROM;
-	}
-	//
-	// Read control struct and get cal addresses before accessing the flash
-	//
+
+	/*
+	 * Read control struct and get cal addresses before accessing the flash
+	 */
 	BcmGetFlashCSInfo(Adapter);
 
-	BeceemFlashBulkRead(Adapter,&uiData,0x0 + Adapter->ulFlashCalStart,4);
-	if(uiData == BECM)
-	{
+	BeceemFlashBulkRead(Adapter, &uiData, 0x0 + Adapter->ulFlashCalStart, 4);
+	if (uiData == BECM)
 		return NVM_FLASH;
-	}
-//
-// even if there is no valid signature on EEPROM/FLASH find out if they really exist.
-// if exist select it.
-//
-	if(BcmGetEEPROMSize(Adapter))
-	{
-		return NVM_EEPROM;
-	}
-
-//TBD for Flash.
 
+	/*
+	 * even if there is no valid signature on EEPROM/FLASH find out if they really exist.
+	 * if exist select it.
+	 */
+	if (BcmGetEEPROMSize(Adapter))
+		return NVM_EEPROM;
 
+	/* TBD for Flash. */
 	return NVM_UNKNOWN;
 }
 
-/**
-*	BcmGetSectionValStartOffset - this will calculate the section's starting offset if section val is given
-*	@Adapter : Drivers Private Data structure
-*	@eFlashSectionVal : Flash secion value defined in enum FLASH2X_SECTION_VAL
-*
-*	Return value:-
-*	On success it return the start offset of the provided section val
-*	On Failure -returns STATUS_FAILURE
-**/
-
-INT BcmGetSectionValStartOffset(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlashSectionVal)
+/*
+ * BcmGetSectionValStartOffset - this will calculate the section's starting offset if section val is given
+ * @Adapter : Drivers Private Data structure
+ * @eFlashSectionVal : Flash secion value defined in enum FLASH2X_SECTION_VAL
+ *
+ * Return value:-
+ * On success it return the start offset of the provided section val
+ * On Failure -returns STATUS_FAILURE
+ */
+
+int BcmGetSectionValStartOffset(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlashSectionVal)
 {
 	/*
-	*	Considering all the section for which end offset can be calculated or directly given
-	*	in CS Structure. if matching case does not exist, return STATUS_FAILURE indicating section
-	*	endoffset can't be calculated or given in CS Structure.
-	*/
+	 * Considering all the section for which end offset can be calculated or directly given
+	 * in CS Structure. if matching case does not exist, return STATUS_FAILURE indicating section
+	 * endoffset can't be calculated or given in CS Structure.
+	 */
 
-	INT SectStartOffset = 0 ;
+	int SectStartOffset = 0;
 
-	SectStartOffset = INVALID_OFFSET ;
+	SectStartOffset = INVALID_OFFSET;
 
-	if(IsSectionExistInVendorInfo(Adapter,eFlashSectionVal))
-	{
+	if (IsSectionExistInVendorInfo(Adapter, eFlashSectionVal))
 		return Adapter->psFlash2xVendorInfo->VendorSection[eFlashSectionVal].OffsetFromZeroForSectionStart;
-	}
 
-	switch(eFlashSectionVal)
-	{
-		case ISO_IMAGE1 :
-			  if((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start != UNINIT_PTR_IN_CS) &&
-			  	(IsNonCDLessDevice(Adapter) == FALSE))
-				  SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start);
-			   break;
-		case ISO_IMAGE2 :
-				if((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start != UNINIT_PTR_IN_CS) &&
-					(IsNonCDLessDevice(Adapter) == FALSE))
-			  		SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start);
-			  break;
-		case DSD0 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart != UNINIT_PTR_IN_CS)
-					SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart);
-				break;
-		case DSD1 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start);
-				break;
-		case DSD2 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start);
-				break;
-		case VSA0 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart);
-				break;
-		case VSA1 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start);
-				break;
-		case VSA2 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start);
-				break;
-		case SCSI :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware);
-				break;
-		case CONTROL_SECTION :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart != UNINIT_PTR_IN_CS)
-					SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart);
-				break;
-		case ISO_IMAGE1_PART2 :
-				if(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start != UNINIT_PTR_IN_CS)
-				 	 SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start);
-				 break;
-		case ISO_IMAGE1_PART3 :
-				if(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start != UNINIT_PTR_IN_CS)
-				  SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
-			  	break;
-		case ISO_IMAGE2_PART2 :
-				if(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start != UNINIT_PTR_IN_CS)
-			 		 SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start);
-			    break;
-		case ISO_IMAGE2_PART3 :
-  			  if(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start != UNINIT_PTR_IN_CS)
-  				  SectStartOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
-  			  break;
-		default :
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section Does not exist in Flash 2.x");
-			SectStartOffset =  INVALID_OFFSET;
+	switch (eFlashSectionVal) {
+	case ISO_IMAGE1:
+		if ((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start != UNINIT_PTR_IN_CS) &&
+			(IsNonCDLessDevice(Adapter) == FALSE))
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start);
+		break;
+	case ISO_IMAGE2:
+		if ((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start != UNINIT_PTR_IN_CS) &&
+			(IsNonCDLessDevice(Adapter) == FALSE))
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start);
+		break;
+	case DSD0:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart);
+		break;
+	case DSD1:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start);
+		break;
+	case DSD2:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start);
+		break;
+	case VSA0:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart);
+		break;
+	case VSA1:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start);
+		break;
+	case VSA2:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start);
+		break;
+	case SCSI:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware);
+		break;
+	case CONTROL_SECTION:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart);
+		break;
+	case ISO_IMAGE1_PART2:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start);
+		break;
+	case ISO_IMAGE1_PART3:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
+		break;
+	case ISO_IMAGE2_PART2:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start);
+		break;
+	case ISO_IMAGE2_PART3:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start != UNINIT_PTR_IN_CS)
+			SectStartOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
+		break;
+	default:
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section Does not exist in Flash 2.x");
+		SectStartOffset = INVALID_OFFSET;
 	}
+
 	return SectStartOffset;
 }
 
-/**
-*	BcmGetSectionValEndOffset - this will calculate the section's Ending offset if section val is given
-*	@Adapter : Drivers Private Data structure
-*	@eFlashSectionVal : Flash secion value defined in enum FLASH2X_SECTION_VAL
-*
-*	Return value:-
-*	On success it return the end offset of the provided section val
-*	On Failure -returns STATUS_FAILURE
-**/
-
-INT BcmGetSectionValEndOffset(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
+/*
+ * BcmGetSectionValEndOffset - this will calculate the section's Ending offset if section val is given
+ * @Adapter : Drivers Private Data structure
+ * @eFlashSectionVal : Flash secion value defined in enum FLASH2X_SECTION_VAL
+ *
+ * Return value:-
+ * On success it return the end offset of the provided section val
+ * On Failure -returns STATUS_FAILURE
+ */
+
+int BcmGetSectionValEndOffset(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
 {
-	INT SectEndOffset = 0 ;
-	SectEndOffset = INVALID_OFFSET;
+	int SectEndOffset = 0;
 
-	if(IsSectionExistInVendorInfo(Adapter,eFlash2xSectionVal))
-	{
+	SectEndOffset = INVALID_OFFSET;
+	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectionVal))
 		return Adapter->psFlash2xVendorInfo->VendorSection[eFlash2xSectionVal].OffsetFromZeroForSectionEnd;
-	}
 
-	switch(eFlash2xSectionVal)
-	{
-		case ISO_IMAGE1 :
-			 if((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End!= UNINIT_PTR_IN_CS) &&
-			 	 (IsNonCDLessDevice(Adapter) == FALSE))
-				  SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End);
-			   break;
-		case ISO_IMAGE2 :
-			if((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End!= UNINIT_PTR_IN_CS) &&
-				(IsNonCDLessDevice(Adapter) == FALSE))
-					SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End);
-			 break;
-		case DSD0 :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd != UNINIT_PTR_IN_CS)
-				SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd);
-			break;
-		case DSD1 :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End != UNINIT_PTR_IN_CS)
-				SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End);
-			break;
-		case DSD2 :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End != UNINIT_PTR_IN_CS)
-				SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End);
-			break;
-		case VSA0 :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd != UNINIT_PTR_IN_CS)
-				SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd);
-			break;
-		case VSA1 :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End != UNINIT_PTR_IN_CS)
-				SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End);
-			break;
-		case VSA2 :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End != UNINIT_PTR_IN_CS)
-				SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End);
-			break;
-		case SCSI :
-			if(Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
-				SectEndOffset = ((Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware) +
+	switch (eFlash2xSectionVal) {
+	case ISO_IMAGE1:
+		if ((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End != UNINIT_PTR_IN_CS) &&
+			(IsNonCDLessDevice(Adapter) == FALSE))
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End);
+		break;
+	case ISO_IMAGE2:
+		if ((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End != UNINIT_PTR_IN_CS) &&
+			(IsNonCDLessDevice(Adapter) == FALSE))
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End);
+		break;
+	case DSD0:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDEnd);
+		break;
+	case DSD1:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1End);
+		break;
+	case DSD2:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2End);
+		break;
+	case VSA0:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAEnd);
+		break;
+	case VSA1:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1End);
+		break;
+	case VSA2:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2End);
+		break;
+	case SCSI:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
+			SectEndOffset = ((Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware) +
 					(Adapter->psFlash2xCSInfo->SizeOfScsiFirmware));
-			break;
-		case CONTROL_SECTION :
-				//Not Clear So Putting failure. confirm and fix it.
-				SectEndOffset = STATUS_FAILURE;
-		case ISO_IMAGE1_PART2 :
-				if(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End!= UNINIT_PTR_IN_CS)
-				 	 SectEndOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End);
-				 break;
-		case ISO_IMAGE1_PART3 :
-				if(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End!= UNINIT_PTR_IN_CS)
-				  SectEndOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End);
-			  	break;
-		case ISO_IMAGE2_PART2 :
-				if(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End != UNINIT_PTR_IN_CS)
-			 		 SectEndOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End);
-			    break;
-		case ISO_IMAGE2_PART3 :
-  			  if(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End!= UNINIT_PTR_IN_CS)
-  				  SectEndOffset =  (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End);
-  			  break;
-
-		default :
-			SectEndOffset = INVALID_OFFSET;
+		break;
+	case CONTROL_SECTION:
+		/* Not Clear So Putting failure. confirm and fix it. */
+		SectEndOffset = STATUS_FAILURE;
+	case ISO_IMAGE1_PART2:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End);
+		break;
+	case ISO_IMAGE1_PART3:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End);
+		break;
+	case ISO_IMAGE2_PART2:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End);
+		break;
+	case ISO_IMAGE2_PART3:
+		if (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End != UNINIT_PTR_IN_CS)
+			SectEndOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End);
+		break;
+	default:
+		SectEndOffset = INVALID_OFFSET;
 	}
+
 	return SectEndOffset ;
 }
 
 /*
-*	BcmFlash2xBulkRead:- Read API for Flash Map 2.x .
-*	@Adapter :Driver Private Data Structure
-*	@pBuffer : Buffer where data has to be put after reading
-*	@eFlashSectionVal :Flash Section Val defined in FLASH2X_SECTION_VAL
-*	@uiOffsetWithinSectionVal :- Offset with in provided section
-*	@uiNumBytes : Number of Bytes for Read
-*
-*	Return value:-
-*		return true on success and STATUS_FAILURE on fail.
-*/
-
-INT BcmFlash2xBulkRead(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	FLASH2X_SECTION_VAL eFlash2xSectionVal,
-	UINT uiOffsetWithinSectionVal,
-	UINT uiNumBytes)
+ * BcmFlash2xBulkRead:- Read API for Flash Map 2.x .
+ * @Adapter :Driver Private Data Structure
+ * @pBuffer : Buffer where data has to be put after reading
+ * @eFlashSectionVal :Flash Section Val defined in FLASH2X_SECTION_VAL
+ * @uiOffsetWithinSectionVal :- Offset with in provided section
+ * @uiNumBytes : Number of Bytes for Read
+ *
+ * Return value:-
+ * return true on success and STATUS_FAILURE on fail.
+ */
+
+int BcmFlash2xBulkRead(struct bcm_mini_adapter *Adapter,
+		PUINT pBuffer,
+		FLASH2X_SECTION_VAL eFlash2xSectionVal,
+		unsigned int uiOffsetWithinSectionVal,
+		unsigned int uiNumBytes)
 {
+	int Status = STATUS_SUCCESS;
+	int SectionStartOffset = 0;
+	unsigned int uiAbsoluteOffset = 0;
+	unsigned int uiTemp = 0, value = 0;
 
-	INT Status = STATUS_SUCCESS;
-	INT SectionStartOffset = 0;
-	UINT uiAbsoluteOffset = 0 ;
-	UINT uiTemp =0, value =0 ;
-	if(Adapter == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Adapter structure is NULL");
+	if (!Adapter) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure is NULL");
 		return -EINVAL;
 	}
-	if(Adapter->device_removed )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Device has been removed");
+	if (Adapter->device_removed) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Device has been removed");
 		return -ENODEV;
 	}
 
-	//NO_SECTION_VAL means absolute offset is given.
-	if(eFlash2xSectionVal == NO_SECTION_VAL)
+	/* NO_SECTION_VAL means absolute offset is given. */
+	if (eFlash2xSectionVal == NO_SECTION_VAL)
 		SectionStartOffset = 0;
 	else
-		SectionStartOffset = BcmGetSectionValStartOffset(Adapter,eFlash2xSectionVal);
+		SectionStartOffset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
 
-	if(SectionStartOffset == STATUS_FAILURE )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"This Section<%d> does not exixt in Flash 2.x Map ",eFlash2xSectionVal);
+	if (SectionStartOffset == STATUS_FAILURE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "This Section<%d> does not exixt in Flash 2.x Map ", eFlash2xSectionVal);
 		return -EINVAL;
 	}
 
-	if(IsSectionExistInVendorInfo(Adapter,eFlash2xSectionVal))
-		return vendorextnReadSection(Adapter,(PUCHAR)pBuffer, eFlash2xSectionVal, uiOffsetWithinSectionVal, uiNumBytes);
+	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectionVal))
+		return vendorextnReadSection(Adapter, (PUCHAR)pBuffer, eFlash2xSectionVal, uiOffsetWithinSectionVal, uiNumBytes);
 
-	//calculating  the absolute offset from FLASH;
+	/* calculating  the absolute offset from FLASH; */
 	uiAbsoluteOffset = uiOffsetWithinSectionVal + SectionStartOffset;
 	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
 	value = 0;
-	wrmalt(Adapter, 0x0f000C80,&value, sizeof(value));
-
-	Status= BeceemFlashBulkRead(Adapter, pBuffer,uiAbsoluteOffset,uiNumBytes) ;
-
+	wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
+	Status = BeceemFlashBulkRead(Adapter, pBuffer, uiAbsoluteOffset, uiNumBytes);
 	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	if(Status)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Flash Read Failed with Status :%d", Status);
-		return Status ;
+	if (Status) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Read Failed with Status :%d", Status);
+		return Status;
 	}
 
 	return Status;
 }
 
 /*
-*	BcmFlash2xBulkWrite :-API for Writing on the Flash Map 2.x.
-*	@Adapter :Driver Private Data Structure
-*	@pBuffer : Buffer From where data has to taken for writing
-*	@eFlashSectionVal :Flash Section Val defined in FLASH2X_SECTION_VAL
-*	@uiOffsetWithinSectionVal :- Offset with in provided section
-*	@uiNumBytes : Number of Bytes for Write
-*
-*	Return value:-
-*		return true on success and STATUS_FAILURE on fail.
-*
-*/
-
-INT BcmFlash2xBulkWrite(
-	PMINI_ADAPTER Adapter,
-	PUINT pBuffer,
-	FLASH2X_SECTION_VAL eFlash2xSectVal,
-	UINT uiOffset,
-	UINT uiNumBytes,
-	UINT bVerify)
+ * BcmFlash2xBulkWrite :-API for Writing on the Flash Map 2.x.
+ * @Adapter :Driver Private Data Structure
+ * @pBuffer : Buffer From where data has to taken for writing
+ * @eFlashSectionVal :Flash Section Val defined in FLASH2X_SECTION_VAL
+ * @uiOffsetWithinSectionVal :- Offset with in provided section
+ * @uiNumBytes : Number of Bytes for Write
+ *
+ * Return value:-
+ * return true on success and STATUS_FAILURE on fail.
+ *
+ */
+
+int BcmFlash2xBulkWrite(struct bcm_mini_adapter *Adapter,
+			PUINT pBuffer,
+			FLASH2X_SECTION_VAL eFlash2xSectVal,
+			unsigned int uiOffset,
+			unsigned int uiNumBytes,
+			unsigned int bVerify)
 {
+	int Status = STATUS_SUCCESS;
+	unsigned int FlashSectValStartOffset = 0;
+	unsigned int uiTemp = 0, value = 0;
 
-	INT Status 	= STATUS_SUCCESS;
-	UINT FlashSectValStartOffset = 0;
-	UINT uiTemp = 0, value = 0;
-	if(Adapter == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Adapter structure is NULL");
+	if (!Adapter) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Adapter structure is NULL");
 		return -EINVAL;
 	}
-	if(Adapter->device_removed )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Device has been removed");
+
+	if (Adapter->device_removed) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Device has been removed");
 		return -ENODEV;
 	}
 
-	//NO_SECTION_VAL means absolute offset is given.
-	if(eFlash2xSectVal == NO_SECTION_VAL)
+	/* NO_SECTION_VAL means absolute offset is given. */
+	if (eFlash2xSectVal == NO_SECTION_VAL)
 		FlashSectValStartOffset = 0;
 	else
-		FlashSectValStartOffset = BcmGetSectionValStartOffset(Adapter,eFlash2xSectVal);
+		FlashSectValStartOffset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectVal);
 
-	if(FlashSectValStartOffset == STATUS_FAILURE )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"This Section<%d> does not exixt in Flash Map 2.x",eFlash2xSectVal);
+	if (FlashSectValStartOffset == STATUS_FAILURE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "This Section<%d> does not exixt in Flash Map 2.x", eFlash2xSectVal);
 		return -EINVAL;
 	}
 
-	if(IsSectionExistInVendorInfo(Adapter,eFlash2xSectVal))
+	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectVal))
 		return vendorextnWriteSection(Adapter, (PUCHAR)pBuffer, eFlash2xSectVal, uiOffset, uiNumBytes, bVerify);
 
-	//calculating  the absolute offset from FLASH;
+	/* calculating  the absolute offset from FLASH; */
 	uiOffset = uiOffset + FlashSectValStartOffset;
 
 	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
 	value = 0;
-	wrmalt(Adapter, 0x0f000C80,&value, sizeof(value));
+	wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
 
-	Status = BeceemFlashBulkWrite(Adapter, pBuffer,uiOffset,uiNumBytes,bVerify);
+	Status = BeceemFlashBulkWrite(Adapter, pBuffer, uiOffset, uiNumBytes, bVerify);
 
 	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	if(Status)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Flash Write failed with Status :%d", Status);
-		return Status ;
+	if (Status) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Flash Write failed with Status :%d", Status);
+		return Status;
 	}
 
 	return Status;
-
 }
 
-/**
-*	BcmGetActiveDSD : Set the Active DSD in Adapter Structure which has to be dumped in DDR
-*	@Adapter :-Drivers private Data Structure
-*
-*	Return Value:-
-*		Return STATUS_SUCESS if get success in setting the right DSD else negaive error code
-*
-**/
-static INT BcmGetActiveDSD(PMINI_ADAPTER Adapter)
+/*
+ * BcmGetActiveDSD : Set the Active DSD in Adapter Structure which has to be dumped in DDR
+ * @Adapter :-Drivers private Data Structure
+ *
+ * Return Value:-
+ * Return STATUS_SUCESS if get success in setting the right DSD else negaive error code
+ *
+ */
+
+static int BcmGetActiveDSD(struct bcm_mini_adapter *Adapter)
 {
-	FLASH2X_SECTION_VAL uiHighestPriDSD = 0 ;
+	FLASH2X_SECTION_VAL uiHighestPriDSD = 0;
 
 	uiHighestPriDSD = getHighestPriDSD(Adapter);
 	Adapter->eActiveDSD = uiHighestPriDSD;
 
-	if(DSD0  == uiHighestPriDSD)
+	if (DSD0  == uiHighestPriDSD)
 		Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart;
-	if(DSD1 == uiHighestPriDSD)
+	if (DSD1 == uiHighestPriDSD)
 		Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start;
-	if(DSD2 == uiHighestPriDSD)
+	if (DSD2 == uiHighestPriDSD)
 		Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start;
-	if(Adapter->eActiveDSD)
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Active DSD :%d", Adapter->eActiveDSD);
-	if(Adapter->eActiveDSD == 0)
-	{
-		//if No DSD gets Active, Make Active the DSD with WR  permission
-		if(IsSectionWritable(Adapter,DSD2))
-		{
+	if (Adapter->eActiveDSD)
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Active DSD :%d", Adapter->eActiveDSD);
+	if (Adapter->eActiveDSD == 0) {
+		/* if No DSD gets Active, Make Active the DSD with WR  permission */
+		if (IsSectionWritable(Adapter, DSD2)) {
 			Adapter->eActiveDSD = DSD2;
 			Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start;
-		}
-		else if(IsSectionWritable(Adapter,DSD1))
-		{
+		} else if (IsSectionWritable(Adapter, DSD1)) {
 			Adapter->eActiveDSD = DSD1;
 			Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start;
-		}
-		else if(IsSectionWritable(Adapter,DSD0))
-		{
+		} else if (IsSectionWritable(Adapter, DSD0)) {
 			Adapter->eActiveDSD = DSD0;
 			Adapter->ulFlashCalStart = Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart;
 		}
@@ -3360,1398 +3001,1230 @@ static INT BcmGetActiveDSD(PMINI_ADAPTER Adapter)
 	return STATUS_SUCCESS;
 }
 
-
-/**
-*	BcmGetActiveISO :- Set the Active ISO in Adapter Data Structue
-*	@Adapter : Driver private Data Structure
-*
-*	Return Value:-
-*		Sucsess:- STATUS_SUCESS
-*		Failure- : negative erro code
-*
-**/
-
-static INT BcmGetActiveISO(PMINI_ADAPTER Adapter)
+/*
+ * BcmGetActiveISO :- Set the Active ISO in Adapter Data Structue
+ * @Adapter : Driver private Data Structure
+ *
+ * Return Value:-
+ * Sucsess:- STATUS_SUCESS
+ * Failure- : negative erro code
+ *
+ */
+
+static int BcmGetActiveISO(struct bcm_mini_adapter *Adapter)
 {
+	int HighestPriISO = 0;
 
-	INT HighestPriISO = 0 ;
 	HighestPriISO = getHighestPriISO(Adapter);
 
-	Adapter->eActiveISO = HighestPriISO ;
-	if(Adapter->eActiveISO == ISO_IMAGE2)
+	Adapter->eActiveISO = HighestPriISO;
+	if (Adapter->eActiveISO == ISO_IMAGE2)
 		Adapter->uiActiveISOOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start);
-	else if(Adapter->eActiveISO == ISO_IMAGE1)
+	else if (Adapter->eActiveISO == ISO_IMAGE1)
 		Adapter->uiActiveISOOffset = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start);
 
-	if(Adapter->eActiveISO)
-	 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Active ISO :%x", Adapter->eActiveISO);
+	if (Adapter->eActiveISO)
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Active ISO :%x", Adapter->eActiveISO);
 
 	return STATUS_SUCCESS;
 }
 
-/**
-*	IsOffsetWritable :- it will tell the access permission of the sector having passed offset
-*	@Adapter : Drivers Private Data Structure
-*	@uiOffset : Offset provided in the Flash
-*
-*	Return Value:-
-*	Success:-TRUE ,  offset is writable
-*	Failure:-FALSE, offset is RO
-*
-**/
-B_UINT8 IsOffsetWritable(PMINI_ADAPTER Adapter, UINT uiOffset)
+/*
+ * IsOffsetWritable :- it will tell the access permission of the sector having passed offset
+ * @Adapter : Drivers Private Data Structure
+ * @uiOffset : Offset provided in the Flash
+ *
+ * Return Value:-
+ * Success:-TRUE ,  offset is writable
+ * Failure:-FALSE, offset is RO
+ *
+ */
+
+B_UINT8 IsOffsetWritable(struct bcm_mini_adapter *Adapter, unsigned int uiOffset)
 {
-	UINT uiSectorNum = 0;
-	UINT uiWordOfSectorPermission =0;
-	UINT uiBitofSectorePermission = 0;
+	unsigned int uiSectorNum = 0;
+	unsigned int uiWordOfSectorPermission = 0;
+	unsigned int uiBitofSectorePermission = 0;
 	B_UINT32 permissionBits = 0;
+
 	uiSectorNum = uiOffset/Adapter->uiSectorSize;
 
-	//calculating the word having this Sector Access permission from SectorAccessBitMap Array
-	uiWordOfSectorPermission = Adapter->psFlash2xCSInfo->SectorAccessBitMap[uiSectorNum /16];
+	/* calculating the word having this Sector Access permission from SectorAccessBitMap Array */
+	uiWordOfSectorPermission = Adapter->psFlash2xCSInfo->SectorAccessBitMap[uiSectorNum / 16];
 
-	//calculating the bit index inside the word for  this sector
-	uiBitofSectorePermission = 2*(15 - uiSectorNum %16);
+	/* calculating the bit index inside the word for  this sector */
+	uiBitofSectorePermission = 2 * (15 - uiSectorNum % 16);
 
-	//Setting Access permission
-	permissionBits = uiWordOfSectorPermission & (0x3 << uiBitofSectorePermission) ;
+	/* Setting Access permission */
+	permissionBits = uiWordOfSectorPermission & (0x3 << uiBitofSectorePermission);
 	permissionBits = (permissionBits >> uiBitofSectorePermission) & 0x3;
-	if(permissionBits == SECTOR_READWRITE_PERMISSION)
-		return 	TRUE;
+	if (permissionBits == SECTOR_READWRITE_PERMISSION)
+		return TRUE;
 	else
 		return FALSE;
 }
 
-static INT BcmDumpFlash2xSectionBitMap(PFLASH2X_BITMAP psFlash2xBitMap)
+static int BcmDumpFlash2xSectionBitMap(PFLASH2X_BITMAP psFlash2xBitMap)
 {
-    PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "***************Flash 2.x Section Bitmap***************");
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"ISO_IMAGE1  :0X%x", psFlash2xBitMap->ISO_IMAGE1);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"ISO_IMAGE2  :0X%x", psFlash2xBitMap->ISO_IMAGE2);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"DSD0  :0X%x", psFlash2xBitMap->DSD0);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"DSD1  :0X%x", psFlash2xBitMap->DSD1);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"DSD2  :0X%x", psFlash2xBitMap->DSD2);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"VSA0  :0X%x", psFlash2xBitMap->VSA0);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"VSA1  :0X%x", psFlash2xBitMap->VSA1);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"VSA2  :0X%x", psFlash2xBitMap->VSA2);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"SCSI  :0X%x", psFlash2xBitMap->SCSI);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"CONTROL_SECTION  :0X%x", psFlash2xBitMap->CONTROL_SECTION);
+	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "***************Flash 2.x Section Bitmap***************");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO_IMAGE1  :0X%x", psFlash2xBitMap->ISO_IMAGE1);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO_IMAGE2  :0X%x", psFlash2xBitMap->ISO_IMAGE2);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD0  :0X%x", psFlash2xBitMap->DSD0);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD1  :0X%x", psFlash2xBitMap->DSD1);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD2  :0X%x", psFlash2xBitMap->DSD2);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "VSA0  :0X%x", psFlash2xBitMap->VSA0);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "VSA1  :0X%x", psFlash2xBitMap->VSA1);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "VSA2  :0X%x", psFlash2xBitMap->VSA2);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SCSI  :0X%x", psFlash2xBitMap->SCSI);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "CONTROL_SECTION  :0X%x", psFlash2xBitMap->CONTROL_SECTION);
 
 	return STATUS_SUCCESS;
 }
 
-/**
-*	BcmGetFlash2xSectionalBitMap :- It will provide the bit map of all the section present in Flash
-*	8bit has been assigned to every section.
-	bit[0] :Section present or not
-	bit[1] :section is valid or not
-	bit[2] : Secton is read only or has write permission too.
-	bit[3] : Active Section -
-	bit[7...4] = Reserved .
-
-	@Adapter:-Driver private Data Structure
-*
-*	Return value:-
-*	Success:- STATUS_SUCESS
-*	Failure:- negative error code
-**/
-
-INT BcmGetFlash2xSectionalBitMap(PMINI_ADAPTER Adapter, PFLASH2X_BITMAP psFlash2xBitMap)
+/*
+ * BcmGetFlash2xSectionalBitMap :- It will provide the bit map of all the section present in Flash
+ * 8bit has been assigned to every section.
+ * bit[0] :Section present or not
+ * bit[1] :section is valid or not
+ * bit[2] : Secton is read only or has write permission too.
+ * bit[3] : Active Section -
+ * bit[7...4] = Reserved .
+ *
+ * @Adapter:-Driver private Data Structure
+ *
+ * Return value:-
+ * Success:- STATUS_SUCESS
+ * Failure:- negative error code
+ */
+
+int BcmGetFlash2xSectionalBitMap(struct bcm_mini_adapter *Adapter, PFLASH2X_BITMAP psFlash2xBitMap)
 {
-
-
 	PFLASH2X_CS_INFO psFlash2xCSInfo = Adapter->psFlash2xCSInfo;
-	FLASH2X_SECTION_VAL uiHighestPriDSD = 0 ;
-	FLASH2X_SECTION_VAL uiHighestPriISO= 0 ;
-	BOOLEAN SetActiveDSDDone = FALSE ;
-	BOOLEAN SetActiveISODone = FALSE ;
-
-	//For 1.x map all the section except DSD0 will be shown as not present
-	//This part will be used by calibration tool to detect the number of DSD present in Flash.
-	if(IsFlash2x(Adapter) == FALSE)
-	{
+	FLASH2X_SECTION_VAL uiHighestPriDSD = 0;
+	FLASH2X_SECTION_VAL uiHighestPriISO = 0;
+	BOOLEAN SetActiveDSDDone = FALSE;
+	BOOLEAN SetActiveISODone = FALSE;
+
+	/* For 1.x map all the section except DSD0 will be shown as not present
+	 * This part will be used by calibration tool to detect the number of DSD present in Flash.
+	 */
+	if (IsFlash2x(Adapter) == FALSE) {
 		psFlash2xBitMap->ISO_IMAGE2 = 0;
 		psFlash2xBitMap->ISO_IMAGE1 = 0;
-		psFlash2xBitMap->DSD0 = FLASH2X_SECTION_VALID | FLASH2X_SECTION_ACT | FLASH2X_SECTION_PRESENT; //0xF;   //0000(Reseved)1(Active)0(RW)1(valid)1(present)
-		psFlash2xBitMap->DSD1  = 0 ;
-		psFlash2xBitMap->DSD2 = 0 ;
-		psFlash2xBitMap->VSA0 = 0 ;
-		psFlash2xBitMap->VSA1 = 0 ;
-		psFlash2xBitMap->VSA2 = 0 ;
-		psFlash2xBitMap->CONTROL_SECTION = 0 ;
-		psFlash2xBitMap->SCSI= 0 ;
-		psFlash2xBitMap->Reserved0 = 0 ;
-		psFlash2xBitMap->Reserved1 = 0 ;
-		psFlash2xBitMap->Reserved2 = 0 ;
-		return STATUS_SUCCESS ;
+		psFlash2xBitMap->DSD0 = FLASH2X_SECTION_VALID | FLASH2X_SECTION_ACT | FLASH2X_SECTION_PRESENT; /* 0xF; 0000(Reseved)1(Active)0(RW)1(valid)1(present) */
+		psFlash2xBitMap->DSD1  = 0;
+		psFlash2xBitMap->DSD2 = 0;
+		psFlash2xBitMap->VSA0 = 0;
+		psFlash2xBitMap->VSA1 = 0;
+		psFlash2xBitMap->VSA2 = 0;
+		psFlash2xBitMap->CONTROL_SECTION = 0;
+		psFlash2xBitMap->SCSI = 0;
+		psFlash2xBitMap->Reserved0 = 0;
+		psFlash2xBitMap->Reserved1 = 0;
+		psFlash2xBitMap->Reserved2 = 0;
 
+		return STATUS_SUCCESS;
 	}
 
 	uiHighestPriDSD = getHighestPriDSD(Adapter);
 	uiHighestPriISO = getHighestPriISO(Adapter);
 
-	///
-	//	IS0 IMAGE 2
-	///
-	if((psFlash2xCSInfo->OffsetISOImage2Part1Start) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->ISO_IMAGE2= psFlash2xBitMap->ISO_IMAGE2 | FLASH2X_SECTION_PRESENT;
-
+	/*
+	 * IS0 IMAGE 2
+	 */
+	if ((psFlash2xCSInfo->OffsetISOImage2Part1Start) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->ISO_IMAGE2 = psFlash2xBitMap->ISO_IMAGE2 | FLASH2X_SECTION_PRESENT;
 
-		if(ReadISOSignature(Adapter,ISO_IMAGE2)== ISO_IMAGE_MAGIC_NUMBER)
+		if (ReadISOSignature(Adapter, ISO_IMAGE2) == ISO_IMAGE_MAGIC_NUMBER)
 			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_VALID;
 
-
-		//Calculation for extrating the Access permission
-		if(IsSectionWritable(Adapter, ISO_IMAGE2) == FALSE)
+		/* Calculation for extrating the Access permission */
+		if (IsSectionWritable(Adapter, ISO_IMAGE2) == FALSE)
 			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_RO;
 
-		if(SetActiveISODone == FALSE && uiHighestPriISO == ISO_IMAGE2)
-		{
-			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_ACT ;
+		if (SetActiveISODone == FALSE && uiHighestPriISO == ISO_IMAGE2) {
+			psFlash2xBitMap->ISO_IMAGE2 |= FLASH2X_SECTION_ACT;
 			SetActiveISODone = TRUE;
 		}
-
 	}
 
-	///
-	//	IS0 IMAGE 1
-	///
-	if((psFlash2xCSInfo->OffsetISOImage1Part1Start) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
+	/*
+	 * IS0 IMAGE 1
+	 */
+	if ((psFlash2xCSInfo->OffsetISOImage1Part1Start) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
 		psFlash2xBitMap->ISO_IMAGE1 = psFlash2xBitMap->ISO_IMAGE1 | FLASH2X_SECTION_PRESENT;
 
-		if(ReadISOSignature(Adapter,ISO_IMAGE1) == ISO_IMAGE_MAGIC_NUMBER)
+		if (ReadISOSignature(Adapter, ISO_IMAGE1) == ISO_IMAGE_MAGIC_NUMBER)
 			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_VALID;
 
-		//	Calculation for extrating the Access permission
-		if(IsSectionWritable(Adapter, ISO_IMAGE1) == FALSE)
+		/* Calculation for extrating the Access permission */
+		if (IsSectionWritable(Adapter, ISO_IMAGE1) == FALSE)
 			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_RO;
 
-		if(SetActiveISODone == FALSE && uiHighestPriISO == ISO_IMAGE1)
-		{
-			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_ACT ;
+		if (SetActiveISODone == FALSE && uiHighestPriISO == ISO_IMAGE1) {
+			psFlash2xBitMap->ISO_IMAGE1 |= FLASH2X_SECTION_ACT;
 			SetActiveISODone = TRUE;
 		}
 	}
 
+	/*
+	 * DSD2
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForDSD2Start) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->DSD2 = psFlash2xBitMap->DSD2 | FLASH2X_SECTION_PRESENT;
 
-
-	///
-	// DSD2
-	///
-	if((psFlash2xCSInfo->OffsetFromZeroForDSD2Start) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->DSD2= psFlash2xBitMap->DSD2 | FLASH2X_SECTION_PRESENT;
-
-		if(ReadDSDSignature(Adapter,DSD2)== DSD_IMAGE_MAGIC_NUMBER)
+		if (ReadDSDSignature(Adapter, DSD2) == DSD_IMAGE_MAGIC_NUMBER)
 			psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_VALID;
 
-		//Calculation for extrating the Access permission
-		if(IsSectionWritable(Adapter, DSD2) == FALSE)
-		{
+		/* Calculation for extrating the Access permission */
+		if (IsSectionWritable(Adapter, DSD2) == FALSE) {
 			psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_RO;
-
-		}
-		else
-		{
-			//Means section is writable
-			if((SetActiveDSDDone == FALSE) && (uiHighestPriDSD == DSD2))
-			{
-				psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_ACT ;
-				SetActiveDSDDone =TRUE ;
+		} else {
+			/* Means section is writable */
+			if ((SetActiveDSDDone == FALSE) && (uiHighestPriDSD == DSD2)) {
+				psFlash2xBitMap->DSD2 |= FLASH2X_SECTION_ACT;
+				SetActiveDSDDone = TRUE;
 			}
 		}
 	}
 
-	///
-	//	DSD 1
-	///
-	if((psFlash2xCSInfo->OffsetFromZeroForDSD1Start) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->DSD1= psFlash2xBitMap->DSD1 | FLASH2X_SECTION_PRESENT;
-
+	/*
+	 * DSD 1
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForDSD1Start) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->DSD1 = psFlash2xBitMap->DSD1 | FLASH2X_SECTION_PRESENT;
 
-		if(ReadDSDSignature(Adapter,DSD1)== DSD_IMAGE_MAGIC_NUMBER)
+		if (ReadDSDSignature(Adapter, DSD1) == DSD_IMAGE_MAGIC_NUMBER)
 			psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_VALID;
 
-		//Calculation for extrating the Access permission
-		if(IsSectionWritable(Adapter, DSD1) == FALSE)
-		{
+		/* Calculation for extrating the Access permission */
+		if (IsSectionWritable(Adapter, DSD1) == FALSE) {
 			psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_RO;
-		}
-		else
-		{
-			//Means section is writable
-			if((SetActiveDSDDone == FALSE) && (uiHighestPriDSD == DSD1))
-			{
-					psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_ACT ;
-					SetActiveDSDDone =TRUE ;
+		} else {
+			/* Means section is writable */
+			if ((SetActiveDSDDone == FALSE) && (uiHighestPriDSD == DSD1)) {
+				psFlash2xBitMap->DSD1 |= FLASH2X_SECTION_ACT;
+				SetActiveDSDDone = TRUE;
 			}
 		}
-
 	}
 
-	///
-	//For DSD 0
-	//
-	if((psFlash2xCSInfo->OffsetFromZeroForDSDStart) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
+	/*
+	 * For DSD 0
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForDSDStart) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
 		psFlash2xBitMap->DSD0 = psFlash2xBitMap->DSD0 | FLASH2X_SECTION_PRESENT;
 
-		if(ReadDSDSignature(Adapter,DSD0) == DSD_IMAGE_MAGIC_NUMBER)
+		if (ReadDSDSignature(Adapter, DSD0) == DSD_IMAGE_MAGIC_NUMBER)
 			psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_VALID;
 
-		//Setting Access permission
-		if(IsSectionWritable(Adapter, DSD0) == FALSE)
-		{
+		/* Setting Access permission */
+		if (IsSectionWritable(Adapter, DSD0) == FALSE) {
 			psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_RO;
-		}
-		else
-		{
-			//Means section is writable
-			if((SetActiveDSDDone == FALSE) &&(uiHighestPriDSD == DSD0))
-			{
-					psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_ACT ;
-					SetActiveDSDDone =TRUE ;
+		} else {
+			/* Means section is writable */
+			if ((SetActiveDSDDone == FALSE) && (uiHighestPriDSD == DSD0)) {
+				psFlash2xBitMap->DSD0 |= FLASH2X_SECTION_ACT;
+				SetActiveDSDDone = TRUE;
 			}
 		}
 	}
 
-	///
-	// 	VSA 0
-	///
-	if((psFlash2xCSInfo->OffsetFromZeroForVSAStart) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->VSA0= psFlash2xBitMap->VSA0 | FLASH2X_SECTION_PRESENT;
+	/*
+	 * VSA 0
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForVSAStart) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->VSA0 = psFlash2xBitMap->VSA0 | FLASH2X_SECTION_PRESENT;
 
-		//Setting the Access Bit. Map is not defined hece setting it always valid
+		/* Setting the Access Bit. Map is not defined hece setting it always valid */
 		psFlash2xBitMap->VSA0 |= FLASH2X_SECTION_VALID;
 
-		//Calculation for extrating the Access permission
-		if(IsSectionWritable(Adapter, VSA0) == FALSE)
+		/* Calculation for extrating the Access permission */
+		if (IsSectionWritable(Adapter, VSA0) == FALSE)
 			psFlash2xBitMap->VSA0 |=  FLASH2X_SECTION_RO;
 
-		//By Default section is Active
-		psFlash2xBitMap->VSA0 |= FLASH2X_SECTION_ACT ;
-
+		/* By Default section is Active */
+		psFlash2xBitMap->VSA0 |= FLASH2X_SECTION_ACT;
 	}
 
+	/*
+	 * VSA 1
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForVSA1Start) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->VSA1 = psFlash2xBitMap->VSA1 | FLASH2X_SECTION_PRESENT;
 
-	///
-	//	 VSA 1
-	///
-
-	if((psFlash2xCSInfo->OffsetFromZeroForVSA1Start) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->VSA1= psFlash2xBitMap->VSA1 | FLASH2X_SECTION_PRESENT;
-
-		//Setting the Access Bit. Map is not defined hece setting it always valid
-		psFlash2xBitMap->VSA1|= FLASH2X_SECTION_VALID;
+		/* Setting the Access Bit. Map is not defined hece setting it always valid */
+		psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_VALID;
 
-		//Checking For Access permission
-		if(IsSectionWritable(Adapter, VSA1) == FALSE)
+		/* Checking For Access permission */
+		if (IsSectionWritable(Adapter, VSA1) == FALSE)
 			psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_RO;
 
-		//By Default section is Active
-		psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_ACT ;
-
+		/* By Default section is Active */
+		psFlash2xBitMap->VSA1 |= FLASH2X_SECTION_ACT;
 	}
 
+	/*
+	 * VSA 2
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForVSA2Start) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->VSA2 = psFlash2xBitMap->VSA2 | FLASH2X_SECTION_PRESENT;
 
-	///
-	//	VSA 2
-	///
-
-	if((psFlash2xCSInfo->OffsetFromZeroForVSA2Start) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->VSA2= psFlash2xBitMap->VSA2 | FLASH2X_SECTION_PRESENT;
-
-
-		//Setting the Access Bit. Map is not defined hece setting it always valid
+		/* Setting the Access Bit. Map is not defined hece setting it always valid */
 		psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_VALID;
 
-		//Checking For Access permission
-		if(IsSectionWritable(Adapter, VSA2) == FALSE)
+		/* Checking For Access permission */
+		if (IsSectionWritable(Adapter, VSA2) == FALSE)
 			psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_RO;
 
-		//By Default section is Active
-		psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_ACT ;
+		/* By Default section is Active */
+		psFlash2xBitMap->VSA2 |= FLASH2X_SECTION_ACT;
 	}
 
-	///
-	// SCSI Section
-	///
-	if((psFlash2xCSInfo->OffsetFromZeroForScsiFirmware) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
-		psFlash2xBitMap->SCSI= psFlash2xBitMap->SCSI | FLASH2X_SECTION_PRESENT;
-
+	/*
+	 * SCSI Section
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForScsiFirmware) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
+		psFlash2xBitMap->SCSI = psFlash2xBitMap->SCSI | FLASH2X_SECTION_PRESENT;
 
-		//Setting the Access Bit. Map is not defined hece setting it always valid
-		psFlash2xBitMap->SCSI|= FLASH2X_SECTION_VALID;
+		/* Setting the Access Bit. Map is not defined hece setting it always valid */
+		psFlash2xBitMap->SCSI |= FLASH2X_SECTION_VALID;
 
-		//Checking For Access permission
-		if(IsSectionWritable(Adapter, SCSI) == FALSE)
+		/* Checking For Access permission */
+		if (IsSectionWritable(Adapter, SCSI) == FALSE)
 			psFlash2xBitMap->SCSI |= FLASH2X_SECTION_RO;
 
-		//By Default section is Active
-		psFlash2xBitMap->SCSI |= FLASH2X_SECTION_ACT ;
-
+		/* By Default section is Active */
+		psFlash2xBitMap->SCSI |= FLASH2X_SECTION_ACT;
 	}
 
-
-	///
-	//	Control Section
-	///
-	if((psFlash2xCSInfo->OffsetFromZeroForControlSectionStart) != UNINIT_PTR_IN_CS)
-	{
-		//Setting the 0th Bit representing the Section is present or not.
+	/*
+	 * Control Section
+	 */
+	if ((psFlash2xCSInfo->OffsetFromZeroForControlSectionStart) != UNINIT_PTR_IN_CS) {
+		/* Setting the 0th Bit representing the Section is present or not. */
 		psFlash2xBitMap->CONTROL_SECTION = psFlash2xBitMap->CONTROL_SECTION | (FLASH2X_SECTION_PRESENT);
 
-
-		//Setting the Access Bit. Map is not defined hece setting it always valid
+		/* Setting the Access Bit. Map is not defined hece setting it always valid */
 		psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_VALID;
 
-		//Checking For Access permission
-		if(IsSectionWritable(Adapter, CONTROL_SECTION) == FALSE)
+		/* Checking For Access permission */
+		if (IsSectionWritable(Adapter, CONTROL_SECTION) == FALSE)
 			psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_RO;
 
-		//By Default section is Active
-		psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_ACT ;
-
+		/* By Default section is Active */
+		psFlash2xBitMap->CONTROL_SECTION |= FLASH2X_SECTION_ACT;
 	}
 
-	///
-	//	For Reserved Sections
-	///
+	/*
+	 * For Reserved Sections
+	 */
 	psFlash2xBitMap->Reserved0 = 0;
 	psFlash2xBitMap->Reserved0 = 0;
 	psFlash2xBitMap->Reserved0 = 0;
-
 	BcmDumpFlash2xSectionBitMap(psFlash2xBitMap);
 
-	return STATUS_SUCCESS ;
-
+	return STATUS_SUCCESS;
 }
-/**
-BcmSetActiveSection :- Set Active section is used to make priority field highest over other
-					section of same type.
-
-@Adapater :- Bcm Driver Private Data Structure
-@eFlash2xSectionVal :- Flash section val whose priority has to be made highest.
-
-Return Value:- Make the priorit highest else return erorr code
 
-**/
-INT BcmSetActiveSection(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectVal)
+/*
+ * BcmSetActiveSection :- Set Active section is used to make priority field highest over other
+ * section of same type.
+ *
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @eFlash2xSectionVal :- Flash section val whose priority has to be made highest.
+ *
+ * Return Value:- Make the priorit highest else return erorr code
+ *
+ */
+
+int BcmSetActiveSection(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectVal)
 {
 	unsigned int SectImagePriority = 0;
-	INT Status =STATUS_SUCCESS;
-
-	//DSD_HEADER sDSD = {0};
-	//ISO_HEADER sISO = {0};
-	INT HighestPriDSD = 0 ;
-	INT HighestPriISO = 0;
-
+	int Status = STATUS_SUCCESS;
 
+	/* DSD_HEADER sDSD = {0};
+	 * ISO_HEADER sISO = {0};
+	 */
+	int HighestPriDSD = 0 ;
+	int HighestPriISO = 0;
 
-	Status = IsSectionWritable(Adapter,eFlash2xSectVal) ;
-	if(Status != TRUE )
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Provided Section <%d> is not writable",eFlash2xSectVal);
+	Status = IsSectionWritable(Adapter, eFlash2xSectVal);
+	if (Status != TRUE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Provided Section <%d> is not writable", eFlash2xSectVal);
 		return STATUS_FAILURE;
 	}
 
-	Adapter->bHeaderChangeAllowed = TRUE ;
-	switch(eFlash2xSectVal)
-	{
-		case ISO_IMAGE1 :
-		case ISO_IMAGE2	:
-			if(ReadISOSignature(Adapter,eFlash2xSectVal)== ISO_IMAGE_MAGIC_NUMBER )
-			{
-				HighestPriISO = getHighestPriISO(Adapter);
+	Adapter->bHeaderChangeAllowed = TRUE;
+	switch (eFlash2xSectVal) {
+	case ISO_IMAGE1:
+	case ISO_IMAGE2:
+		if (ReadISOSignature(Adapter, eFlash2xSectVal) == ISO_IMAGE_MAGIC_NUMBER) {
+			HighestPriISO = getHighestPriISO(Adapter);
+
+			if (HighestPriISO == eFlash2xSectVal) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given ISO<%x> already has highest priority", eFlash2xSectVal);
+				Status = STATUS_SUCCESS;
+				break;
+			}
+
+			SectImagePriority = ReadISOPriority(Adapter, HighestPriISO) + 1;
 
-				if(HighestPriISO == eFlash2xSectVal	)
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Given ISO<%x> already  has highest priority",eFlash2xSectVal );
-					Status = STATUS_SUCCESS ;
+			if ((SectImagePriority <= 0) && IsSectionWritable(Adapter, HighestPriISO)) {
+				/* This is a SPECIAL Case which will only happen if the current highest priority ISO has priority value = 0x7FFFFFFF.
+				 * We will write 1 to the current Highest priority ISO And then shall increase the priority of the requested ISO
+				 * by user
+				 */
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SectImagePriority wraparound happened, eFlash2xSectVal: 0x%x\n", eFlash2xSectVal);
+				SectImagePriority = htonl(0x1);
+				Status = BcmFlash2xBulkWrite(Adapter,
+							&SectImagePriority,
+							HighestPriISO,
+							0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImagePriority),
+							SIGNATURE_SIZE,
+							TRUE);
+				if (Status) {
+					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
+					Status = STATUS_FAILURE;
 					break;
 				}
 
-				SectImagePriority = ReadISOPriority(Adapter, HighestPriISO) + 1;
-
-				if((SectImagePriority <= 0) && IsSectionWritable(Adapter,HighestPriISO))
-				{
-					// This is a SPECIAL Case which will only happen if the current highest priority ISO has priority value = 0x7FFFFFFF.
-					// We will write 1 to the current Highest priority ISO And then shall increase the priority of the requested ISO
-					// by user
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "SectImagePriority wraparound happened, eFlash2xSectVal: 0x%x\n",eFlash2xSectVal);
-					SectImagePriority = htonl(0x1);
-					Status = BcmFlash2xBulkWrite(Adapter,
-								&SectImagePriority,
-								HighestPriISO,
-								0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImagePriority),
-								SIGNATURE_SIZE,
-								TRUE);
-
-					if(Status)
-					{
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Priority has not been written properly");
-						Status = STATUS_FAILURE;
-						break ;
-					}
-
-					HighestPriISO = getHighestPriISO(Adapter);
-
-					if(HighestPriISO == eFlash2xSectVal	)
-					{
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Given ISO<%x> already  has highest priority",eFlash2xSectVal );
-						Status = STATUS_SUCCESS ;
-						break;
-					}
+				HighestPriISO = getHighestPriISO(Adapter);
 
-					SectImagePriority = 2;
-				 }
+				if (HighestPriISO == eFlash2xSectVal) {
+					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given ISO<%x> already has highest priority", eFlash2xSectVal);
+					Status = STATUS_SUCCESS;
+					break;
+				}
 
+				SectImagePriority = 2;
+			}
 
-				SectImagePriority = htonl(SectImagePriority);
+			SectImagePriority = htonl(SectImagePriority);
 
-				Status = BcmFlash2xBulkWrite(Adapter,
-								&SectImagePriority,
-								eFlash2xSectVal,
-								0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImagePriority),
-								SIGNATURE_SIZE,
-								TRUE);
-				if(Status)
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Priority has not been written properly");
-					break ;
-				}
-			}
-			else
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Signature is currupted. Hence can't increase the priority");
-				Status = STATUS_FAILURE ;
+			Status = BcmFlash2xBulkWrite(Adapter,
+						&SectImagePriority,
+						eFlash2xSectVal,
+						0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImagePriority),
+						SIGNATURE_SIZE,
+						TRUE);
+			if (Status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
 				break;
 			}
+		} else {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Signature is currupted. Hence can't increase the priority");
+			Status = STATUS_FAILURE;
 			break;
-		case DSD0 :
-		case DSD1 :
-		case DSD2 :
-			if(ReadDSDSignature(Adapter,eFlash2xSectVal)== DSD_IMAGE_MAGIC_NUMBER)
-			{
-				HighestPriDSD = getHighestPriDSD(Adapter);
+		}
+		break;
+	case DSD0:
+	case DSD1:
+	case DSD2:
+		if (ReadDSDSignature(Adapter, eFlash2xSectVal) == DSD_IMAGE_MAGIC_NUMBER) {
+			HighestPriDSD = getHighestPriDSD(Adapter);
+			if ((HighestPriDSD == eFlash2xSectVal)) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given DSD<%x> already has highest priority", eFlash2xSectVal);
+				Status = STATUS_SUCCESS;
+				break;
+			}
+
+			SectImagePriority = ReadDSDPriority(Adapter, HighestPriDSD) + 1;
+			if (SectImagePriority <= 0) {
+				/* This is a SPECIAL Case which will only happen if the current highest priority DSD has priority value = 0x7FFFFFFF.
+				 * We will write 1 to the current Highest priority DSD And then shall increase the priority of the requested DSD
+				 * by user
+				 */
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, NVM_RW, DBG_LVL_ALL, "SectImagePriority wraparound happened, eFlash2xSectVal: 0x%x\n", eFlash2xSectVal);
+				SectImagePriority = htonl(0x1);
 
-				if((HighestPriDSD == eFlash2xSectVal))
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Given DSD<%x> already  has highest priority", eFlash2xSectVal);
-					Status = STATUS_SUCCESS ;
+				Status = BcmFlash2xBulkWrite(Adapter,
+							&SectImagePriority,
+							HighestPriDSD,
+							Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
+							SIGNATURE_SIZE,
+							TRUE);
+				if (Status) {
+					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
 					break;
 				}
 
-				SectImagePriority = ReadDSDPriority(Adapter, HighestPriDSD) + 1 ;
-				if(SectImagePriority <= 0)
-				{
-					// This is a SPECIAL Case which will only happen if the current highest priority DSD has priority value = 0x7FFFFFFF.
-					// We will write 1 to the current Highest priority DSD And then shall increase the priority of the requested DSD
-					// by user
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, NVM_RW, DBG_LVL_ALL, "SectImagePriority wraparound happened, eFlash2xSectVal: 0x%x\n",eFlash2xSectVal);
-					SectImagePriority = htonl(0x1);
-
-					Status = BcmFlash2xBulkWrite(Adapter,
-									&SectImagePriority,
-									HighestPriDSD,
-									Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
-									SIGNATURE_SIZE,
-									TRUE);
-
-					if(Status)
-					{
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-						break ;
-					}
-
-					HighestPriDSD = getHighestPriDSD(Adapter);
-
-					if((HighestPriDSD == eFlash2xSectVal))
-					{
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Made the DSD: %x highest by reducing priority of other\n", eFlash2xSectVal);
-						Status = STATUS_SUCCESS ;
-						break;
-					}
-
-					SectImagePriority = htonl(0x2);
-					Status = BcmFlash2xBulkWrite(Adapter,
-									&SectImagePriority,
-									HighestPriDSD,
-									Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
-									SIGNATURE_SIZE,
-									TRUE);
-
-					if(Status)
-					{
-						BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
-						break ;
-					}
-
-					HighestPriDSD = getHighestPriDSD(Adapter);
-
-					if((HighestPriDSD == eFlash2xSectVal))
-					{
-						Status = STATUS_SUCCESS ;
-						break;
-					}
-					SectImagePriority = 3 ;
+				HighestPriDSD = getHighestPriDSD(Adapter);
 
+				if ((HighestPriDSD == eFlash2xSectVal)) {
+					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Made the DSD: %x highest by reducing priority of other\n", eFlash2xSectVal);
+					Status = STATUS_SUCCESS;
+					break;
 				}
-				SectImagePriority = htonl(SectImagePriority);
+
+				SectImagePriority = htonl(0x2);
 				Status = BcmFlash2xBulkWrite(Adapter,
-								&SectImagePriority,
-								eFlash2xSectVal,
-								Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
-								SIGNATURE_SIZE ,
-								TRUE);
-				if(Status)
-				{
-					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Priority has not been written properly");
-					Status = STATUS_FAILURE ;
-					break ;
+							&SectImagePriority,
+							HighestPriDSD,
+							Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
+							SIGNATURE_SIZE,
+							TRUE);
+				if (Status) {
+					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
+					break;
 				}
+
+				HighestPriDSD = getHighestPriDSD(Adapter);
+				if ((HighestPriDSD == eFlash2xSectVal)) {
+					Status = STATUS_SUCCESS;
+					break;
+				}
+
+				SectImagePriority = 3;
 			}
-			else
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Signature is currupted. Hence can't increase the priority");
-				Status = STATUS_FAILURE ;
+			SectImagePriority = htonl(SectImagePriority);
+			Status = BcmFlash2xBulkWrite(Adapter,
+						&SectImagePriority,
+						eFlash2xSectVal,
+						Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
+						SIGNATURE_SIZE,
+						TRUE);
+			if (Status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Priority has not been written properly");
+				Status = STATUS_FAILURE;
 				break;
 			}
+		} else {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Signature is currupted. Hence can't increase the priority");
+			Status = STATUS_FAILURE;
 			break;
-		case VSA0 :
-		case VSA1 :
-		case VSA2 :
-			//Has to be decided
-			break ;
-		default :
-				Status = STATUS_FAILURE ;
-				break;
-
+		}
+		break;
+	case VSA0:
+	case VSA1:
+	case VSA2:
+		/* Has to be decided */
+		break;
+	default:
+		Status = STATUS_FAILURE;
+		break;
 	}
 
-	Adapter->bHeaderChangeAllowed = FALSE ;
+	Adapter->bHeaderChangeAllowed = FALSE;
 	return Status;
-
 }
 
-/**
-BcmCopyISO - Used only for copying the ISO section
-@Adapater :- Bcm Driver Private Data Structure
-@sCopySectStrut :- Section copy structure
-
-Return value:- SUCCESS if copies successfully else negative error code
-
-**/
-INT BcmCopyISO(PMINI_ADAPTER Adapter, FLASH2X_COPY_SECTION sCopySectStrut)
+/*
+ * BcmCopyISO - Used only for copying the ISO section
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @sCopySectStrut :- Section copy structure
+ *
+ * Return value:- SUCCESS if copies successfully else negative error code
+ *
+ */
+
+int BcmCopyISO(struct bcm_mini_adapter *Adapter, FLASH2X_COPY_SECTION sCopySectStrut)
 {
-
 	PCHAR Buff = NULL;
-	FLASH2X_SECTION_VAL eISOReadPart = 0,eISOWritePart = 0;
-	UINT uiReadOffsetWithinPart = 0, uiWriteOffsetWithinPart = 0;
-	UINT uiTotalDataToCopy = 0;
-	BOOLEAN IsThisHeaderSector = FALSE ;
-	UINT sigOffset = 0;
-	UINT ISOLength = 0;
-	UINT Status = STATUS_SUCCESS;
-	UINT SigBuff[MAX_RW_SIZE];
-	UINT i = 0;
-
-	if(ReadISOSignature(Adapter,sCopySectStrut.SrcSection) != ISO_IMAGE_MAGIC_NUMBER)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
+	FLASH2X_SECTION_VAL eISOReadPart = 0, eISOWritePart = 0;
+	unsigned int uiReadOffsetWithinPart = 0, uiWriteOffsetWithinPart = 0;
+	unsigned int uiTotalDataToCopy = 0;
+	BOOLEAN IsThisHeaderSector = FALSE;
+	unsigned int sigOffset = 0;
+	unsigned int ISOLength = 0;
+	unsigned int Status = STATUS_SUCCESS;
+	unsigned int SigBuff[MAX_RW_SIZE];
+	unsigned int i = 0;
+
+	if (ReadISOSignature(Adapter, sCopySectStrut.SrcSection) != ISO_IMAGE_MAGIC_NUMBER) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
 		return STATUS_FAILURE;
 	}
 
 	Status = BcmFlash2xBulkRead(Adapter,
-					   &ISOLength,
-					   sCopySectStrut.SrcSection,
-					   0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER,ISOImageSize),
-					   4);
-
-	if(Status)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO\n");
+				&ISOLength,
+				sCopySectStrut.SrcSection,
+				0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImageSize),
+				4);
+	if (Status) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO\n");
 		return Status;
 	}
 
 	ISOLength = htonl(ISOLength);
-
-	if(ISOLength % Adapter->uiSectorSize)
-	{
-		ISOLength = Adapter->uiSectorSize*(1 + ISOLength/Adapter->uiSectorSize);
-	}
+	if (ISOLength % Adapter->uiSectorSize)
+		ISOLength = Adapter->uiSectorSize * (1 + ISOLength/Adapter->uiSectorSize);
 
 	sigOffset = FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImageMagicNumber);
 
 	Buff = kzalloc(Adapter->uiSectorSize, GFP_KERNEL);
 
-	if(Buff == NULL)
-	{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Memory allocation failed for section size");
-			return -ENOMEM;
+	if (!Buff) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed for section size");
+		return -ENOMEM;
 	}
 
-	if(sCopySectStrut.SrcSection ==ISO_IMAGE1 && sCopySectStrut.DstSection ==ISO_IMAGE2)
-	{
-		eISOReadPart = ISO_IMAGE1 ;
-		eISOWritePart = ISO_IMAGE2 ;
+	if (sCopySectStrut.SrcSection == ISO_IMAGE1 && sCopySectStrut.DstSection == ISO_IMAGE2) {
+		eISOReadPart = ISO_IMAGE1;
+		eISOWritePart = ISO_IMAGE2;
 		uiReadOffsetWithinPart =  0;
-		uiWriteOffsetWithinPart = 0 ;
-
-		uiTotalDataToCopy =(Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
-
-		if(uiTotalDataToCopy < ISOLength)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"error as Source ISO Section does not have valid signature");
+		uiWriteOffsetWithinPart = 0;
+
+		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
+
+		if (uiTotalDataToCopy < ISOLength) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
 			Status = STATUS_FAILURE;
 			goto out;
 		}
 
-		uiTotalDataToCopy =(Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
+		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
 
-		if(uiTotalDataToCopy < ISOLength)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"error as Dest ISO Section does not have enough section size");
+		if (uiTotalDataToCopy < ISOLength) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Dest ISO Section does not have enough section size");
 			Status = STATUS_FAILURE;
 			goto out;
 		}
 
 		uiTotalDataToCopy = ISOLength;
 
-		CorruptISOSig(Adapter,ISO_IMAGE2);
-
-		while(uiTotalDataToCopy)
-		{
-			if(uiTotalDataToCopy == Adapter->uiSectorSize)
-			{
-				//Setting for write of first sector. First sector is assumed to be written in last
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Writing the signature sector");
-				eISOReadPart = ISO_IMAGE1 ;
+		CorruptISOSig(Adapter, ISO_IMAGE2);
+		while (uiTotalDataToCopy) {
+			if (uiTotalDataToCopy == Adapter->uiSectorSize) {
+				/* Setting for write of first sector. First sector is assumed to be written in last */
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Writing the signature sector");
+				eISOReadPart = ISO_IMAGE1;
 				uiReadOffsetWithinPart = 0;
 				eISOWritePart = ISO_IMAGE2;
-				uiWriteOffsetWithinPart = 0 ;
-				IsThisHeaderSector = TRUE ;
-
-			}
-			else
-			{
-				uiReadOffsetWithinPart = uiReadOffsetWithinPart + Adapter->uiSectorSize ;
-				uiWriteOffsetWithinPart = uiWriteOffsetWithinPart + Adapter->uiSectorSize ;
+				uiWriteOffsetWithinPart = 0;
+				IsThisHeaderSector = TRUE;
+			} else {
+				uiReadOffsetWithinPart = uiReadOffsetWithinPart + Adapter->uiSectorSize;
+				uiWriteOffsetWithinPart = uiWriteOffsetWithinPart + Adapter->uiSectorSize;
 
-				if((eISOReadPart == ISO_IMAGE1) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start) ))
-				{
-					eISOReadPart = ISO_IMAGE1_PART2 ;
+				if ((eISOReadPart == ISO_IMAGE1) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start))) {
+					eISOReadPart = ISO_IMAGE1_PART2;
 					uiReadOffsetWithinPart = 0;
 				}
-				if((eISOReadPart == ISO_IMAGE1_PART2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start)))
-				{
-					eISOReadPart = ISO_IMAGE1_PART3 ;
+
+				if ((eISOReadPart == ISO_IMAGE1_PART2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start))) {
+					eISOReadPart = ISO_IMAGE1_PART3;
 					uiReadOffsetWithinPart = 0;
 				}
-				if((eISOWritePart == ISO_IMAGE2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start)))
-				{
-					eISOWritePart = ISO_IMAGE2_PART2 ;
+
+				if ((eISOWritePart == ISO_IMAGE2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start))) {
+					eISOWritePart = ISO_IMAGE2_PART2;
 					uiWriteOffsetWithinPart = 0;
 				}
-				if((eISOWritePart == ISO_IMAGE2_PART2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start)))
-				{
-					eISOWritePart = ISO_IMAGE2_PART3 ;
+
+				if ((eISOWritePart == ISO_IMAGE2_PART2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start))) {
+					eISOWritePart = ISO_IMAGE2_PART3;
 					uiWriteOffsetWithinPart = 0;
 				}
 			}
 
 			Status = BcmFlash2xBulkRead(Adapter,
-								   (PUINT)Buff,
-								   eISOReadPart,
-								   uiReadOffsetWithinPart,
-								   Adapter->uiSectorSize
-								);
-
-			if(Status)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOReadPart, uiReadOffsetWithinPart);
+						(PUINT)Buff,
+						eISOReadPart,
+						uiReadOffsetWithinPart,
+						Adapter->uiSectorSize);
+			if (Status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOReadPart, uiReadOffsetWithinPart);
 				break;
 			}
 
-			if(IsThisHeaderSector == TRUE)
-			{
-				//If this is header sector write 0xFFFFFFFF at the sig time and in last write sig
+			if (IsThisHeaderSector == TRUE) {
+				/* If this is header sector write 0xFFFFFFFF at the sig time and in last write sig */
 				memcpy(SigBuff, Buff + sigOffset, MAX_RW_SIZE);
 
-				for(i = 0; i < MAX_RW_SIZE;i++)
+				for (i = 0; i < MAX_RW_SIZE; i++)
 					*(Buff + sigOffset + i) = 0xFF;
 			}
-			Adapter->bHeaderChangeAllowed = TRUE ;
-
+			Adapter->bHeaderChangeAllowed = TRUE;
 			Status = BcmFlash2xBulkWrite(Adapter,
-								 (PUINT)Buff,
-								 eISOWritePart,
-								 uiWriteOffsetWithinPart,
-								 Adapter->uiSectorSize,
-								 TRUE);
-			if(Status)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Write failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOWritePart, uiWriteOffsetWithinPart);
+						(PUINT)Buff,
+						eISOWritePart,
+						uiWriteOffsetWithinPart,
+						Adapter->uiSectorSize,
+						TRUE);
+			if (Status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOWritePart, uiWriteOffsetWithinPart);
 				break;
 			}
 
 			Adapter->bHeaderChangeAllowed = FALSE;
-
-			if(IsThisHeaderSector == TRUE)
-			{
+			if (IsThisHeaderSector == TRUE) {
 				WriteToFlashWithoutSectorErase(Adapter,
-												SigBuff,
-												eISOWritePart,
-												sigOffset,
-												MAX_RW_SIZE);
-				IsThisHeaderSector = FALSE ;
+							SigBuff,
+							eISOWritePart,
+							sigOffset,
+							MAX_RW_SIZE);
+				IsThisHeaderSector = FALSE;
 			}
-			//subtracting the written Data
-			uiTotalDataToCopy = uiTotalDataToCopy - Adapter->uiSectorSize ;
+			/* subtracting the written Data */
+			uiTotalDataToCopy = uiTotalDataToCopy - Adapter->uiSectorSize;
 		}
+	}
 
+	if (sCopySectStrut.SrcSection == ISO_IMAGE2 && sCopySectStrut.DstSection == ISO_IMAGE1) {
+		eISOReadPart = ISO_IMAGE2;
+		eISOWritePart = ISO_IMAGE1;
+		uiReadOffsetWithinPart = 0;
+		uiWriteOffsetWithinPart = 0;
 
-	}
+		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
 
-	if(sCopySectStrut.SrcSection ==ISO_IMAGE2 && sCopySectStrut.DstSection ==ISO_IMAGE1)
-	{
-		eISOReadPart = ISO_IMAGE2 ;
-		eISOWritePart = ISO_IMAGE1 ;
-		uiReadOffsetWithinPart =	0;
-		uiWriteOffsetWithinPart = 0 ;
-
-		uiTotalDataToCopy =(Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage2Part3Start);
-
-		if(uiTotalDataToCopy < ISOLength)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"error as Source ISO Section does not have valid signature");
+		if (uiTotalDataToCopy < ISOLength) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Source ISO Section does not have valid signature");
 			Status = STATUS_FAILURE;
 			goto out;
 		}
 
-		uiTotalDataToCopy =(Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start)+
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End) -
-						   (Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
+		uiTotalDataToCopy = (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start) +
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3End) -
+			(Adapter->psFlash2xCSInfo->OffsetISOImage1Part3Start);
 
-		if(uiTotalDataToCopy < ISOLength)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"error as Dest ISO Section does not have enough section size");
+		if (uiTotalDataToCopy < ISOLength) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "error as Dest ISO Section does not have enough section size");
 			Status = STATUS_FAILURE;
 			goto out;
 		}
 
 		uiTotalDataToCopy = ISOLength;
 
-		CorruptISOSig(Adapter,ISO_IMAGE1);
+		CorruptISOSig(Adapter, ISO_IMAGE1);
 
-		while(uiTotalDataToCopy)
-		{
-			if(uiTotalDataToCopy == Adapter->uiSectorSize)
-			{
-				//Setting for write of first sector. First sector is assumed to be written in last
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Writing the signature sector");
-				eISOReadPart = ISO_IMAGE2 ;
+		while (uiTotalDataToCopy) {
+			if (uiTotalDataToCopy == Adapter->uiSectorSize) {
+				/* Setting for write of first sector. First sector is assumed to be written in last */
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Writing the signature sector");
+				eISOReadPart = ISO_IMAGE2;
 				uiReadOffsetWithinPart = 0;
 				eISOWritePart = ISO_IMAGE1;
-				uiWriteOffsetWithinPart = 0 ;
+				uiWriteOffsetWithinPart = 0;
 				IsThisHeaderSector = TRUE;
+			} else {
+				uiReadOffsetWithinPart = uiReadOffsetWithinPart + Adapter->uiSectorSize;
+				uiWriteOffsetWithinPart = uiWriteOffsetWithinPart + Adapter->uiSectorSize;
 
-			}
-			else
-			{
-				uiReadOffsetWithinPart = uiReadOffsetWithinPart + Adapter->uiSectorSize ;
-				uiWriteOffsetWithinPart = uiWriteOffsetWithinPart + Adapter->uiSectorSize ;
-
-				if((eISOReadPart == ISO_IMAGE2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start) ))
-				{
-					eISOReadPart = ISO_IMAGE2_PART2 ;
+				if ((eISOReadPart == ISO_IMAGE2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start))) {
+					eISOReadPart = ISO_IMAGE2_PART2;
 					uiReadOffsetWithinPart = 0;
 				}
-				if((eISOReadPart == ISO_IMAGE2_PART2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start)))
-				{
-					eISOReadPart = ISO_IMAGE2_PART3 ;
+
+				if ((eISOReadPart == ISO_IMAGE2_PART2) && (uiReadOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage2Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage2Part2Start))) {
+					eISOReadPart = ISO_IMAGE2_PART3;
 					uiReadOffsetWithinPart = 0;
 				}
-				if((eISOWritePart == ISO_IMAGE1) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start)))
-				{
-					eISOWritePart = ISO_IMAGE1_PART2 ;
+
+				if ((eISOWritePart == ISO_IMAGE1) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part1End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start))) {
+					eISOWritePart = ISO_IMAGE1_PART2;
 					uiWriteOffsetWithinPart = 0;
 				}
-				if((eISOWritePart == ISO_IMAGE1_PART2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start)))
-				{
-					eISOWritePart = ISO_IMAGE1_PART3 ;
+
+				if ((eISOWritePart == ISO_IMAGE1_PART2) && (uiWriteOffsetWithinPart == (Adapter->psFlash2xCSInfo->OffsetISOImage1Part2End - Adapter->psFlash2xCSInfo->OffsetISOImage1Part2Start))) {
+					eISOWritePart = ISO_IMAGE1_PART3;
 					uiWriteOffsetWithinPart = 0;
 				}
 			}
 
 			Status = BcmFlash2xBulkRead(Adapter,
-								   (PUINT)Buff,
-								   eISOReadPart,
-								   uiReadOffsetWithinPart,
-								   Adapter->uiSectorSize
-								);
-			if(Status)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOReadPart, uiReadOffsetWithinPart);
+						(PUINT)Buff,
+						eISOReadPart,
+						uiReadOffsetWithinPart,
+						Adapter->uiSectorSize);
+			if (Status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOReadPart, uiReadOffsetWithinPart);
 				break;
 			}
 
-			if(IsThisHeaderSector == TRUE)
-			{
-				//If this is header sector write 0xFFFFFFFF at the sig time and in last write sig
+			if (IsThisHeaderSector == TRUE) {
+				/* If this is header sector write 0xFFFFFFFF at the sig time and in last write sig */
 				memcpy(SigBuff, Buff + sigOffset, MAX_RW_SIZE);
 
-				for(i = 0; i < MAX_RW_SIZE;i++)
+				for (i = 0; i < MAX_RW_SIZE; i++)
 					*(Buff + sigOffset + i) = 0xFF;
-
 			}
-			Adapter->bHeaderChangeAllowed = TRUE ;
+			Adapter->bHeaderChangeAllowed = TRUE;
 			Status = BcmFlash2xBulkWrite(Adapter,
-								 (PUINT)Buff,
-								 eISOWritePart,
-								 uiWriteOffsetWithinPart,
-								 Adapter->uiSectorSize,
-								 TRUE);
-
-			if(Status)
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Write failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOWritePart, uiWriteOffsetWithinPart);
+						(PUINT)Buff,
+						eISOWritePart,
+						uiWriteOffsetWithinPart,
+						Adapter->uiSectorSize,
+						TRUE);
+			if (Status) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write failed while copying ISO: Part: %x, OffsetWithinPart: %x\n", eISOWritePart, uiWriteOffsetWithinPart);
 				break;
 			}
 
-			Adapter->bHeaderChangeAllowed = FALSE ;
-
-			if(IsThisHeaderSector == TRUE)
-			{
+			Adapter->bHeaderChangeAllowed = FALSE;
+			if (IsThisHeaderSector == TRUE) {
 				WriteToFlashWithoutSectorErase(Adapter,
-												SigBuff,
-												eISOWritePart,
-												sigOffset,
-												MAX_RW_SIZE);
-				IsThisHeaderSector = FALSE ;
+							SigBuff,
+							eISOWritePart,
+							sigOffset,
+							MAX_RW_SIZE);
+
+				IsThisHeaderSector = FALSE;
 			}
 
-			//subtracting the written Data
-			uiTotalDataToCopy = uiTotalDataToCopy - Adapter->uiSectorSize ;
+			/* subtracting the written Data */
+			uiTotalDataToCopy = uiTotalDataToCopy - Adapter->uiSectorSize;
 		}
-
-
 	}
-
 out:
 	kfree(Buff);
 
 	return Status;
 }
-/**
-BcmFlash2xCorruptSig : this API is used to corrupt the written sig in Bcm Header present in flash section.
-					     It will corrupt the sig, if Section is writable, by making first bytes as zero.
-@Adapater :- Bcm Driver Private Data Structure
-@eFlash2xSectionVal :- Flash section val which has header
-
-Return Value :-
-	Success :- If Section is present and writable, corrupt the sig and return STATUS_SUCCESS
-	Failure :-Return negative error code
 
-
-**/
-INT BcmFlash2xCorruptSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
+/*
+ * BcmFlash2xCorruptSig : this API is used to corrupt the written sig in Bcm Header present in flash section.
+ * It will corrupt the sig, if Section is writable, by making first bytes as zero.
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @eFlash2xSectionVal :- Flash section val which has header
+ *
+ * Return Value :-
+ *	Success :- If Section is present and writable, corrupt the sig and return STATUS_SUCCESS
+ *	Failure :-Return negative error code
+ */
+
+int BcmFlash2xCorruptSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
 {
+	int Status = STATUS_SUCCESS;
 
-	INT Status = STATUS_SUCCESS ;
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Section Value :%x \n", eFlash2xSectionVal);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section Value :%x\n", eFlash2xSectionVal);
 
-	if((eFlash2xSectionVal == DSD0) || (eFlash2xSectionVal == DSD1) || (eFlash2xSectionVal == DSD2))
-	{
+	if ((eFlash2xSectionVal == DSD0) || (eFlash2xSectionVal == DSD1) || (eFlash2xSectionVal == DSD2)) {
 		Status = CorruptDSDSig(Adapter, eFlash2xSectionVal);
-	}
-	else if(eFlash2xSectionVal == ISO_IMAGE1 || eFlash2xSectionVal == ISO_IMAGE2)
-	{
+	} else if (eFlash2xSectionVal == ISO_IMAGE1 || eFlash2xSectionVal == ISO_IMAGE2) {
 		Status = CorruptISOSig(Adapter, eFlash2xSectionVal);
-	}
-	else
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Given Section <%d>does not have Header",eFlash2xSectionVal);
+	} else {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Given Section <%d>does not have Header", eFlash2xSectionVal);
 		return STATUS_SUCCESS;
 	}
 	return Status;
 }
-/**
-BcmFlash2xWriteSig :-this API is used to Write the sig if requested Section has
-					  header and  Write Permission.
-@Adapater :- Bcm Driver Private Data Structure
-@eFlashSectionVal :- Flash section val which has header
-
-Return Value :-
-	Success :- If Section is present and writable write the sig and return STATUS_SUCCESS
-	Failure :-Return negative error code
-
-**/
-INT BcmFlash2xWriteSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlashSectionVal)
-{
 
-	UINT uiSignature = 0 ;
-	UINT uiOffset = 0;
-	//DSD_HEADER dsdHeader = {0};
+/*
+ *BcmFlash2xWriteSig :-this API is used to Write the sig if requested Section has
+ *					  header and  Write Permission.
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @eFlashSectionVal :- Flash section val which has header
+ *
+ * Return Value :-
+ *	Success :- If Section is present and writable write the sig and return STATUS_SUCCESS
+ *	Failure :-Return negative error code
+ */
+
+int BcmFlash2xWriteSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlashSectionVal)
+{
+	unsigned int uiSignature = 0;
+	unsigned int uiOffset = 0;
 
-	if(Adapter->bSigCorrupted == FALSE)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Signature is not corrupted by driver, hence not restoring\n");
+	/* DSD_HEADER dsdHeader = {0}; */
+	if (Adapter->bSigCorrupted == FALSE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Signature is not corrupted by driver, hence not restoring\n");
 		return STATUS_SUCCESS;
 	}
-	if(Adapter->bAllDSDWriteAllow == FALSE)
-	{
-		if(IsSectionWritable(Adapter,eFlashSectionVal) == FALSE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section is not Writable...Hence can't Write signature");
+
+	if (Adapter->bAllDSDWriteAllow == FALSE) {
+		if (IsSectionWritable(Adapter, eFlashSectionVal) == FALSE) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section is not Writable...Hence can't Write signature");
 			return SECTOR_IS_NOT_WRITABLE;
 		}
 	}
-	if((eFlashSectionVal == DSD0) ||(eFlashSectionVal == DSD1) || (eFlashSectionVal == DSD2))
-	{
-		uiSignature = htonl(DSD_IMAGE_MAGIC_NUMBER) ;
-		uiOffset = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader ;
 
-		uiOffset += FIELD_OFFSET_IN_HEADER(PDSD_HEADER,DSDImageMagicNumber);
+	if ((eFlashSectionVal == DSD0) || (eFlashSectionVal == DSD1) || (eFlashSectionVal == DSD2)) {
+		uiSignature = htonl(DSD_IMAGE_MAGIC_NUMBER);
+		uiOffset = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader;
+
+		uiOffset += FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImageMagicNumber);
 
-		if((ReadDSDSignature(Adapter,eFlashSectionVal) & 0xFF000000) != CORRUPTED_PATTERN)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Corrupted Pattern is not there. Hence won't write sig");
+		if ((ReadDSDSignature(Adapter, eFlashSectionVal) & 0xFF000000) != CORRUPTED_PATTERN) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Corrupted Pattern is not there. Hence won't write sig");
 			return STATUS_FAILURE;
 		}
-
-	}
-	else if((eFlashSectionVal == ISO_IMAGE1) || (eFlashSectionVal == ISO_IMAGE2))
-	{
+	} else if ((eFlashSectionVal == ISO_IMAGE1) || (eFlashSectionVal == ISO_IMAGE2)) {
 		uiSignature = htonl(ISO_IMAGE_MAGIC_NUMBER);
-		//uiOffset = 0;
-		uiOffset = FIELD_OFFSET_IN_HEADER(PISO_HEADER,ISOImageMagicNumber);
-		if((ReadISOSignature(Adapter,eFlashSectionVal) & 0xFF000000) != CORRUPTED_PATTERN)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Currupted Pattern is not there. Hence won't write sig");
+		/* uiOffset = 0; */
+		uiOffset = FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImageMagicNumber);
+		if ((ReadISOSignature(Adapter, eFlashSectionVal) & 0xFF000000) != CORRUPTED_PATTERN) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Currupted Pattern is not there. Hence won't write sig");
 			return STATUS_FAILURE;
 		}
-	}
-	else
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"GIVEN SECTION< %d > IS NOT VALID FOR SIG WRITE...", eFlashSectionVal);
+	} else {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "GIVEN SECTION< %d > IS NOT VALID FOR SIG WRITE...", eFlashSectionVal);
 		return STATUS_FAILURE;
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Restoring the signature");
-
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Restoring the signature");
 
 	Adapter->bHeaderChangeAllowed = TRUE;
 	Adapter->bSigCorrupted = FALSE;
-	BcmFlash2xBulkWrite(Adapter, &uiSignature,eFlashSectionVal,uiOffset,SIGNATURE_SIZE,TRUE);
+	BcmFlash2xBulkWrite(Adapter, &uiSignature, eFlashSectionVal, uiOffset, SIGNATURE_SIZE, TRUE);
 	Adapter->bHeaderChangeAllowed = FALSE;
 
-
-
 	return STATUS_SUCCESS;
 }
-/**
-validateFlash2xReadWrite :- This API is used to validate the user request for Read/Write.
-						      if requested Bytes goes beyond the Requested section, it reports error.
-@Adapater :- Bcm Driver Private Data Structure
-@psFlash2xReadWrite :-Flash2x Read/write structure pointer
-
-Return values:-Return TRUE is request is valid else FALSE.
 
-
-**/
-INT	validateFlash2xReadWrite(PMINI_ADAPTER Adapter, PFLASH2X_READWRITE psFlash2xReadWrite)
+/*
+ * validateFlash2xReadWrite :- This API is used to validate the user request for Read/Write.
+ *						      if requested Bytes goes beyond the Requested section, it reports error.
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @psFlash2xReadWrite :-Flash2x Read/write structure pointer
+ *
+ * Return values:-Return TRUE is request is valid else FALSE.
+ */
+
+int validateFlash2xReadWrite(struct bcm_mini_adapter *Adapter, PFLASH2X_READWRITE psFlash2xReadWrite)
 {
-	UINT uiNumOfBytes = 0 ;
-	UINT uiSectStartOffset = 0 ;
-	UINT uiSectEndOffset = 0;
+	unsigned int uiNumOfBytes = 0;
+	unsigned int uiSectStartOffset = 0;
+	unsigned int uiSectEndOffset = 0;
+
 	uiNumOfBytes = psFlash2xReadWrite->numOfBytes;
 
-	if(IsSectionExistInFlash(Adapter,psFlash2xReadWrite->Section) != TRUE)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section<%x> does not exixt in Flash",psFlash2xReadWrite->Section);
+	if (IsSectionExistInFlash(Adapter, psFlash2xReadWrite->Section) != TRUE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section<%x> does not exixt in Flash", psFlash2xReadWrite->Section);
 		return FALSE;
 	}
-	uiSectStartOffset = BcmGetSectionValStartOffset(Adapter,psFlash2xReadWrite->Section);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Start offset :%x ,section :%d\n",uiSectStartOffset,psFlash2xReadWrite->Section);
-	if((psFlash2xReadWrite->Section == ISO_IMAGE1) ||(psFlash2xReadWrite->Section == ISO_IMAGE2))
-	{
-		if(psFlash2xReadWrite->Section == ISO_IMAGE1)
-		{
-			uiSectEndOffset = BcmGetSectionValEndOffset(Adapter,ISO_IMAGE1) -
-							  BcmGetSectionValStartOffset(Adapter,ISO_IMAGE1)+
-							  BcmGetSectionValEndOffset(Adapter,ISO_IMAGE1_PART2) -
-							  BcmGetSectionValStartOffset(Adapter,ISO_IMAGE1_PART2)+
-							  BcmGetSectionValEndOffset(Adapter,ISO_IMAGE1_PART3) -
-							  BcmGetSectionValStartOffset(Adapter,ISO_IMAGE1_PART3);
+	uiSectStartOffset = BcmGetSectionValStartOffset(Adapter, psFlash2xReadWrite->Section);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Start offset :%x ,section :%d\n", uiSectStartOffset, psFlash2xReadWrite->Section);
+	if ((psFlash2xReadWrite->Section == ISO_IMAGE1) || (psFlash2xReadWrite->Section == ISO_IMAGE2)) {
+		if (psFlash2xReadWrite->Section == ISO_IMAGE1) {
+			uiSectEndOffset = BcmGetSectionValEndOffset(Adapter, ISO_IMAGE1) -
+				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1) +
+				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE1_PART2) -
+				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1_PART2) +
+				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE1_PART3) -
+				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1_PART3);
+		} else if (psFlash2xReadWrite->Section == ISO_IMAGE2) {
+			uiSectEndOffset = BcmGetSectionValEndOffset(Adapter, ISO_IMAGE2) -
+				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2) +
+				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE2_PART2) -
+				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2_PART2) +
+				BcmGetSectionValEndOffset(Adapter, ISO_IMAGE2_PART3) -
+				BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2_PART3);
 		}
-		else if(psFlash2xReadWrite->Section == ISO_IMAGE2)
-		{
-			uiSectEndOffset = BcmGetSectionValEndOffset(Adapter,ISO_IMAGE2) -
-							  BcmGetSectionValStartOffset(Adapter,ISO_IMAGE2)+
-							  BcmGetSectionValEndOffset(Adapter,ISO_IMAGE2_PART2) -
-							  BcmGetSectionValStartOffset(Adapter,ISO_IMAGE2_PART2)+
-							  BcmGetSectionValEndOffset(Adapter,ISO_IMAGE2_PART3) -
-							  BcmGetSectionValStartOffset(Adapter,ISO_IMAGE2_PART3);
 
-		}
+		/* since this uiSectEndoffset is the size of iso Image. hence for calculating the vitual endoffset
+		 * it should be added in startoffset. so that check done in last of this function can be valued.
+		 */
+		uiSectEndOffset = uiSectStartOffset + uiSectEndOffset;
 
-		//since this uiSectEndoffset is the size of iso Image. hence for calculating the vitual endoffset
-		//it should be added in startoffset. so that check done in last of this function can be valued.
-		uiSectEndOffset = uiSectStartOffset + uiSectEndOffset ;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Total size of the ISO Image :%x", uiSectEndOffset);
+	} else
+		uiSectEndOffset = BcmGetSectionValEndOffset(Adapter, psFlash2xReadWrite->Section);
 
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Total size of the ISO Image :%x",uiSectEndOffset);
-	}
-	else
-		uiSectEndOffset   = BcmGetSectionValEndOffset(Adapter,psFlash2xReadWrite->Section);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "End offset :%x \n",uiSectEndOffset);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "End offset :%x\n", uiSectEndOffset);
 
-	//Checking the boundary condition
-	if((uiSectStartOffset + psFlash2xReadWrite->offset + uiNumOfBytes) <= uiSectEndOffset)
+	/* Checking the boundary condition */
+	if ((uiSectStartOffset + psFlash2xReadWrite->offset + uiNumOfBytes) <= uiSectEndOffset)
 		return TRUE;
-	else
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Invalid Request....");
+	else {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Invalid Request....");
 		return FALSE;
 	}
-
 }
 
-/**
-IsFlash2x :- check for Flash 2.x
-@Adapater :- Bcm Driver Private Data Structure
-
-Return value:-
-	return TRUE if flah2.x of hgher version else return false.
-**/
-
-INT IsFlash2x(PMINI_ADAPTER Adapter)
+/*
+ * IsFlash2x :- check for Flash 2.x
+ * Adapater :- Bcm Driver Private Data Structure
+ *
+ * Return value:-
+ *	return TRUE if flah2.x of hgher version else return false.
+ */
+
+int IsFlash2x(struct bcm_mini_adapter *Adapter)
 {
-	if(Adapter->uiFlashLayoutMajorVersion >= FLASH_2X_MAJOR_NUMBER)
-		return TRUE ;
+	if (Adapter->uiFlashLayoutMajorVersion >= FLASH_2X_MAJOR_NUMBER)
+		return TRUE;
 	else
 		return FALSE;
 }
-/**
-GetFlashBaseAddr :- Calculate the Flash Base address
-@Adapater :- Bcm Driver Private Data Structure
-
-Return Value:-
-	Success :- Base Address of the Flash
-**/
 
-static INT GetFlashBaseAddr(PMINI_ADAPTER Adapter)
+/*
+ * GetFlashBaseAddr :- Calculate the Flash Base address
+ * @Adapater :- Bcm Driver Private Data Structure
+ *
+ * Return Value:-
+ *	Success :- Base Address of the Flash
+ */
+
+static int GetFlashBaseAddr(struct bcm_mini_adapter *Adapter)
 {
+	unsigned int uiBaseAddr = 0;
 
-	UINT uiBaseAddr = 0;
-
-	if(Adapter->bDDRInitDone)
-	{
+	if (Adapter->bDDRInitDone) {
 		/*
-		For All Valid Flash Versions... except 1.1, take the value from FlashBaseAddr
-		In case of Raw Read... use the default value
-		*/
-		if(Adapter->uiFlashLayoutMajorVersion && (Adapter->bFlashRawRead == FALSE) &&
-			!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1))
-			)
-			uiBaseAddr = Adapter->uiFlashBaseAdd ;
+		 * For All Valid Flash Versions... except 1.1, take the value from FlashBaseAddr
+		 * In case of Raw Read... use the default value
+		 */
+		if (Adapter->uiFlashLayoutMajorVersion && (Adapter->bFlashRawRead == FALSE) &&
+			!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
+			uiBaseAddr = Adapter->uiFlashBaseAdd;
 		else
 			uiBaseAddr = FLASH_CONTIGIOUS_START_ADDR_AFTER_INIT;
-	}
-	else
-	{
+	} else {
 		/*
-		For All Valid Flash Versions... except 1.1, take the value from FlashBaseAddr
-		In case of Raw Read... use the default value
-		*/
-		if(Adapter->uiFlashLayoutMajorVersion && (Adapter->bFlashRawRead == FALSE) &&
-			!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1))
-			)
+		 * For All Valid Flash Versions... except 1.1, take the value from FlashBaseAddr
+		 * In case of Raw Read... use the default value
+		 */
+		if (Adapter->uiFlashLayoutMajorVersion && (Adapter->bFlashRawRead == FALSE) &&
+			!((Adapter->uiFlashLayoutMajorVersion == 1) && (Adapter->uiFlashLayoutMinorVersion == 1)))
 			uiBaseAddr = Adapter->uiFlashBaseAdd | FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
 		else
 			uiBaseAddr = FLASH_CONTIGIOUS_START_ADDR_BEFORE_INIT;
 	}
 
-	return uiBaseAddr ;
+	return uiBaseAddr;
 }
-/**
-BcmCopySection :- This API is used to copy the One section in another. Both section should
-				    be contiuous and of same size. Hence this Will not be applicabe to copy ISO.
-
-@Adapater :- Bcm Driver Private Data Structure
-@SrcSection :- Source section From where data has to be copied
-@DstSection :- Destination section to which data has to be copied
-@offset :- Offset from/to  where data has to be copied from one section to another.
-@numOfBytes :- number of byes that has to be copyed from one section to another at given offset.
-			     in case of numofBytes  equal zero complete section will be copied.
-
-Return Values-
-	Success : Return STATUS_SUCCESS
-	Faillure :- return negative error code
-
-**/
-
-INT	BcmCopySection(PMINI_ADAPTER Adapter,
-						FLASH2X_SECTION_VAL SrcSection,
-						FLASH2X_SECTION_VAL DstSection,
-						UINT offset,
-						UINT numOfBytes)
+
+/*
+ * BcmCopySection :- This API is used to copy the One section in another. Both section should
+ *				    be contiuous and of same size. Hence this Will not be applicabe to copy ISO.
+ *
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @SrcSection :- Source section From where data has to be copied
+ * @DstSection :- Destination section to which data has to be copied
+ * @offset :- Offset from/to  where data has to be copied from one section to another.
+ * @numOfBytes :- number of byes that has to be copyed from one section to another at given offset.
+ *			     in case of numofBytes  equal zero complete section will be copied.
+ * Return Values-
+ *	Success : Return STATUS_SUCCESS
+ *	Faillure :- return negative error code
+ */
+
+int BcmCopySection(struct bcm_mini_adapter *Adapter,
+		FLASH2X_SECTION_VAL SrcSection,
+		FLASH2X_SECTION_VAL DstSection,
+		unsigned int offset,
+		unsigned int numOfBytes)
 {
-	UINT BuffSize = 0 ;
-	UINT BytesToBeCopied = 0;
-	PUCHAR pBuff = NULL ;
-	INT Status = STATUS_SUCCESS ;
-	if(SrcSection == DstSection)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Source and Destination should be different ...try again");
+	unsigned int BuffSize = 0;
+	unsigned int BytesToBeCopied = 0;
+	PUCHAR pBuff = NULL;
+	int Status = STATUS_SUCCESS;
+
+	if (SrcSection == DstSection) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Source and Destination should be different ...try again");
 		return -EINVAL;
 	}
-	if((SrcSection != DSD0) && (SrcSection != DSD1) && (SrcSection != DSD2))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Source should be DSD subsection");
-		return  -EINVAL;
-	}
-	if((DstSection != DSD0) && (DstSection != DSD1) && (DstSection != DSD2))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Destination should be DSD subsection");
-		return  -EINVAL;
+
+	if ((SrcSection != DSD0) && (SrcSection != DSD1) && (SrcSection != DSD2)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Source should be DSD subsection");
+		return -EINVAL;
 	}
 
-	//if offset zero means have to copy complete secton
+	if ((DstSection != DSD0) && (DstSection != DSD1) && (DstSection != DSD2)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Destination should be DSD subsection");
+		return -EINVAL;
+	}
 
-	if(numOfBytes == 0)
-	{
-		numOfBytes = BcmGetSectionValEndOffset(Adapter,SrcSection)
-				  - BcmGetSectionValStartOffset(Adapter,SrcSection);
+	/* if offset zero means have to copy complete secton */
+	if (numOfBytes == 0) {
+		numOfBytes = BcmGetSectionValEndOffset(Adapter, SrcSection)
+			- BcmGetSectionValStartOffset(Adapter, SrcSection);
 
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL," Section Size :0x%x",numOfBytes);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Section Size :0x%x", numOfBytes);
 	}
 
-	if((offset + numOfBytes) > BcmGetSectionValEndOffset(Adapter,SrcSection)
-				  - BcmGetSectionValStartOffset(Adapter,SrcSection))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0," Input parameters going beyond the section offS: %x numB: %x of Source Section\n",
-						offset, numOfBytes);
+	if ((offset + numOfBytes) > BcmGetSectionValEndOffset(Adapter, SrcSection)
+		- BcmGetSectionValStartOffset(Adapter, SrcSection)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, " Input parameters going beyond the section offS: %x numB: %x of Source Section\n",
+				offset, numOfBytes);
 		return -EINVAL;
 	}
 
-	if((offset + numOfBytes) > BcmGetSectionValEndOffset(Adapter,DstSection)
-				  - BcmGetSectionValStartOffset(Adapter,DstSection))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0," Input parameters going beyond the section offS: %x numB: %x of Destination Section\n",
-						offset, numOfBytes);
+	if ((offset + numOfBytes) > BcmGetSectionValEndOffset(Adapter, DstSection)
+		- BcmGetSectionValStartOffset(Adapter, DstSection)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Input parameters going beyond the section offS: %x numB: %x of Destination Section\n",
+				offset, numOfBytes);
 		return -EINVAL;
 	}
 
-
-	if(numOfBytes > Adapter->uiSectorSize )
+	if (numOfBytes > Adapter->uiSectorSize)
 		BuffSize = Adapter->uiSectorSize;
 	else
-		BuffSize = numOfBytes ;
+		BuffSize = numOfBytes;
 
 	pBuff = (PCHAR)kzalloc(BuffSize, GFP_KERNEL);
-	if(pBuff == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Memory allocation failed.. ");
+	if (!pBuff) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed.. ");
 		return -ENOMEM;
 	}
 
-
-	BytesToBeCopied = Adapter->uiSectorSize ;
-	if(offset % Adapter->uiSectorSize)
+	BytesToBeCopied = Adapter->uiSectorSize;
+	if (offset % Adapter->uiSectorSize)
 		BytesToBeCopied = Adapter->uiSectorSize - (offset % Adapter->uiSectorSize);
-	if(BytesToBeCopied > numOfBytes)
-		BytesToBeCopied = numOfBytes ;
-
-
+	if (BytesToBeCopied > numOfBytes)
+		BytesToBeCopied = numOfBytes;
 
 	Adapter->bHeaderChangeAllowed = TRUE;
 
-	do
-	{
-		Status = BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, SrcSection , offset,BytesToBeCopied);
-		if(Status)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Read failed at offset :%d for NOB :%d", SrcSection,BytesToBeCopied);
+	do {
+		Status = BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, SrcSection , offset, BytesToBeCopied);
+		if (Status) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read failed at offset :%d for NOB :%d", SrcSection, BytesToBeCopied);
 			break;
 		}
-		Status = BcmFlash2xBulkWrite(Adapter,(PUINT)pBuff,DstSection,offset,BytesToBeCopied,FALSE);
-		if(Status)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Write failed at offset :%d for NOB :%d", DstSection,BytesToBeCopied);
+		Status = BcmFlash2xBulkWrite(Adapter, (PUINT)pBuff, DstSection, offset, BytesToBeCopied, FALSE);
+		if (Status) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Write failed at offset :%d for NOB :%d", DstSection, BytesToBeCopied);
 			break;
 		}
 		offset = offset + BytesToBeCopied;
-		numOfBytes = numOfBytes - BytesToBeCopied ;
-		if(numOfBytes)
-		{
-			if(numOfBytes > Adapter->uiSectorSize )
+		numOfBytes = numOfBytes - BytesToBeCopied;
+		if (numOfBytes) {
+			if (numOfBytes > Adapter->uiSectorSize)
 				BytesToBeCopied = Adapter->uiSectorSize;
 			else
 				BytesToBeCopied = numOfBytes;
 		}
-	}while(numOfBytes > 0) ;
+	} while (numOfBytes > 0);
+
 	kfree(pBuff);
-	Adapter->bHeaderChangeAllowed = FALSE ;
+	Adapter->bHeaderChangeAllowed = FALSE;
+
 	return Status;
 }
 
-/**
-SaveHeaderIfPresent :- This API is use to Protect the Header in case of Header Sector write
-@Adapater :- Bcm Driver Private Data Structure
-@pBuff :- Data buffer that has to be written in sector having the header map.
-@uiOffset :- Flash offset that has to be written.
-
-Return value :-
-	Success :- On success return STATUS_SUCCESS
-	Faillure :- Return negative error code
-
-**/
-
-INT SaveHeaderIfPresent(PMINI_ADAPTER Adapter, PUCHAR pBuff, UINT uiOffset)
+/*
+ * SaveHeaderIfPresent :- This API is use to Protect the Header in case of Header Sector write
+ * @Adapater :- Bcm Driver Private Data Structure
+ * @pBuff :- Data buffer that has to be written in sector having the header map.
+ * @uiOffset :- Flash offset that has to be written.
+ *
+ * Return value :-
+ *	Success :- On success return STATUS_SUCCESS
+ *	Faillure :- Return negative error code
+ */
+
+int SaveHeaderIfPresent(struct bcm_mini_adapter *Adapter, PUCHAR pBuff, unsigned int uiOffset)
 {
-	UINT offsetToProtect = 0,HeaderSizeToProtect =0;
-	BOOLEAN bHasHeader = FALSE ;
-	PUCHAR pTempBuff =NULL;
-	UINT uiSectAlignAddr = 0;
-	UINT sig = 0;
+	unsigned int offsetToProtect = 0, HeaderSizeToProtect = 0;
+	BOOLEAN bHasHeader = FALSE;
+	PUCHAR pTempBuff = NULL;
+	unsigned int uiSectAlignAddr = 0;
+	unsigned int sig = 0;
 
-	//making the offset sector aligned
+	/* making the offset sector aligned */
 	uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize - 1);
 
-
-	if((uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter,DSD2)- Adapter->uiSectorSize)||
-	(uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter,DSD1)- Adapter->uiSectorSize)||
-	(uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter,DSD0)- Adapter->uiSectorSize))
-	{
-
-		//offset from the sector boundary having the header map
+	if ((uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter, DSD2) - Adapter->uiSectorSize) ||
+		(uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter, DSD1) - Adapter->uiSectorSize) ||
+		(uiSectAlignAddr == BcmGetSectionValEndOffset(Adapter, DSD0) - Adapter->uiSectorSize)) {
+		/* offset from the sector boundary having the header map */
 		offsetToProtect = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader % Adapter->uiSectorSize;
 		HeaderSizeToProtect = sizeof(DSD_HEADER);
-		bHasHeader = TRUE ;
+		bHasHeader = TRUE;
 	}
 
-	if(uiSectAlignAddr == BcmGetSectionValStartOffset(Adapter,ISO_IMAGE1) ||
-		uiSectAlignAddr == BcmGetSectionValStartOffset(Adapter,ISO_IMAGE2))
-	{
+	if (uiSectAlignAddr == BcmGetSectionValStartOffset(Adapter, ISO_IMAGE1) ||
+		uiSectAlignAddr == BcmGetSectionValStartOffset(Adapter, ISO_IMAGE2)) {
 		offsetToProtect = 0;
 		HeaderSizeToProtect = sizeof(ISO_HEADER);
 		bHasHeader = TRUE;
 	}
-	//If Header is present overwrite passed buffer with this
-	if(bHasHeader && (Adapter->bHeaderChangeAllowed == FALSE))
-	{
+	/* If Header is present overwrite passed buffer with this */
+	if (bHasHeader && (Adapter->bHeaderChangeAllowed == FALSE)) {
 		pTempBuff = (PUCHAR)kzalloc(HeaderSizeToProtect, GFP_KERNEL);
-		if(pTempBuff == NULL)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Memory allocation failed ");
+		if (!pTempBuff) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Memory allocation failed");
 			return -ENOMEM;
 		}
-		//Read header
-		BeceemFlashBulkRead(Adapter,(PUINT)pTempBuff,(uiSectAlignAddr + offsetToProtect),HeaderSizeToProtect);
-		BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,pTempBuff ,HeaderSizeToProtect);
-		//Replace Buffer content with Header
-		memcpy(pBuff +offsetToProtect,pTempBuff,HeaderSizeToProtect);
+		/* Read header */
+		BeceemFlashBulkRead(Adapter, (PUINT)pTempBuff, (uiSectAlignAddr + offsetToProtect), HeaderSizeToProtect);
+		BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, pTempBuff, HeaderSizeToProtect);
+		/* Replace Buffer content with Header */
+		memcpy(pBuff + offsetToProtect, pTempBuff, HeaderSizeToProtect);
 
 		kfree(pTempBuff);
 	}
-	if(bHasHeader && Adapter->bSigCorrupted)
-	{
-		sig = *((PUINT)(pBuff + offsetToProtect + FIELD_OFFSET_IN_HEADER(PDSD_HEADER,DSDImageMagicNumber)));
+	if (bHasHeader && Adapter->bSigCorrupted) {
+		sig = *((PUINT)(pBuff + offsetToProtect + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImageMagicNumber)));
 		sig = ntohl(sig);
-		if((sig & 0xFF000000) != CORRUPTED_PATTERN)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Desired pattern is not at sig offset. Hence won't restore");
+		if ((sig & 0xFF000000) != CORRUPTED_PATTERN) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Desired pattern is not at sig offset. Hence won't restore");
 			Adapter->bSigCorrupted = FALSE;
 			return STATUS_SUCCESS;
 		}
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL," Corrupted sig is :%X", sig);
-		*((PUINT)(pBuff + offsetToProtect + FIELD_OFFSET_IN_HEADER(PDSD_HEADER,DSDImageMagicNumber)))= htonl(DSD_IMAGE_MAGIC_NUMBER);
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Restoring the signature in Header Write only");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, " Corrupted sig is :%X", sig);
+		*((PUINT)(pBuff + offsetToProtect + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImageMagicNumber))) = htonl(DSD_IMAGE_MAGIC_NUMBER);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Restoring the signature in Header Write only");
 		Adapter->bSigCorrupted = FALSE;
 	}
 
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 }
 
-/**
-BcmDoChipSelect : This will selcet the appropriate chip for writing.
-@Adapater :- Bcm Driver Private Data Structure
-
-OutPut:-
-	Select the Appropriate chip and retrn status Success
-**/
-static INT BcmDoChipSelect(PMINI_ADAPTER Adapter, UINT offset)
+/*
+ * BcmDoChipSelect : This will selcet the appropriate chip for writing.
+ * @Adapater :- Bcm Driver Private Data Structure
+ *
+ * OutPut:-
+ *	Select the Appropriate chip and retrn status Success
+ */
+static int BcmDoChipSelect(struct bcm_mini_adapter *Adapter, unsigned int offset)
 {
-	UINT FlashConfig = 0;
-	INT ChipNum = 0;
-	UINT GPIOConfig = 0;
-	UINT PartNum = 0;
+	unsigned int FlashConfig = 0;
+	int ChipNum = 0;
+	unsigned int GPIOConfig = 0;
+	unsigned int PartNum = 0;
 
-	ChipNum = offset / FLASH_PART_SIZE ;
+	ChipNum = offset / FLASH_PART_SIZE;
 
-	//
-	// Chip Select mapping to enable flash0.
-	// To select flash 0, we have to OR with (0<<12).
-	// ORing 0 will have no impact so not doing that part.
-	// In future if Chip select value changes from 0 to non zero,
-	// That needs be taken care with backward comaptibility. No worries for now.
-	//
+	/*
+	 * Chip Select mapping to enable flash0.
+	 * To select flash 0, we have to OR with (0<<12).
+	 * ORing 0 will have no impact so not doing that part.
+	 * In future if Chip select value changes from 0 to non zero,
+	 * That needs be taken care with backward comaptibility. No worries for now.
+	 */
 
 	/*
-	SelectedChip Variable is the selection that the host is 100% Sure the same as what the register will hold. This can be ONLY ensured
-	if the Chip doesn't goes to low power mode while the flash operation is in progress (NVMRdmWrmLock is taken)
-	Before every new Flash Write operation, we reset the variable. This is to ensure that after any wake-up from
-	power down modes (Idle mode/shutdown mode), the values in the register will be different.
-	*/
+	 * SelectedChip Variable is the selection that the host is 100% Sure the same as what the register will hold. This can be ONLY ensured
+	 * if the Chip doesn't goes to low power mode while the flash operation is in progress (NVMRdmWrmLock is taken)
+	 * Before every new Flash Write operation, we reset the variable. This is to ensure that after any wake-up from
+	 * power down modes (Idle mode/shutdown mode), the values in the register will be different.
+	 */
 
-	if(Adapter->SelectedChip == ChipNum)
-    		return STATUS_SUCCESS;
+	if (Adapter->SelectedChip == ChipNum)
+		return STATUS_SUCCESS;
 
-	//BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Selected Chip :%x", ChipNum);
-	Adapter->SelectedChip = ChipNum ;
+	/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Selected Chip :%x", ChipNum); */
+	Adapter->SelectedChip = ChipNum;
 
-	//bit[13..12]  will select the appropriate chip
+	/* bit[13..12]  will select the appropriate chip */
 	rdmalt(Adapter, FLASH_CONFIG_REG, &FlashConfig, 4);
 	rdmalt(Adapter, FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
-
 	{
-		switch(ChipNum)
-		{
+		switch (ChipNum) {
 		case 0:
 			PartNum = 0;
 			break;
@@ -4770,453 +4243,422 @@ static INT BcmDoChipSelect(PMINI_ADAPTER Adapter, UINT offset)
 		}
 	}
 	/* In case the bits already written in the FLASH_CONFIG_REG is same as what the user desired,
-	    nothing to do... can return immediately.
-	    ASSUMPTION: FLASH_GPIO_CONFIG_REG will be in sync with FLASH_CONFIG_REG.
-	    Even if the chip goes to low power mode, it should wake with values in each register in sync with each other.
-	    These values are not written by host other than during CHIP_SELECT.
-	*/
-	if(PartNum == ((FlashConfig >> CHIP_SELECT_BIT12) & 0x3))
+	 * nothing to do... can return immediately.
+	 * ASSUMPTION: FLASH_GPIO_CONFIG_REG will be in sync with FLASH_CONFIG_REG.
+	 * Even if the chip goes to low power mode, it should wake with values in each register in sync with each other.
+	 * These values are not written by host other than during CHIP_SELECT.
+	 */
+	if (PartNum == ((FlashConfig >> CHIP_SELECT_BIT12) & 0x3))
 		return STATUS_SUCCESS;
 
-	//clearing the bit[13..12]
+	/* clearing the bit[13..12] */
 	FlashConfig &= 0xFFFFCFFF;
-	FlashConfig = (FlashConfig | (PartNum<<CHIP_SELECT_BIT12)); //00
+	FlashConfig = (FlashConfig | (PartNum<<CHIP_SELECT_BIT12)); /* 00 */
 
-	wrmalt(Adapter,FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
+	wrmalt(Adapter, FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
 	udelay(100);
 
-	wrmalt(Adapter,FLASH_CONFIG_REG, &FlashConfig, 4);
+	wrmalt(Adapter, FLASH_CONFIG_REG, &FlashConfig, 4);
 	udelay(100);
 
 	return STATUS_SUCCESS;
-
 }
-INT ReadDSDSignature(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL dsd)
-{
-		UINT uiDSDsig = 0;
-		//UINT sigoffsetInMap = 0;
-		//DSD_HEADER dsdHeader = {0};
 
+int ReadDSDSignature(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL dsd)
+{
+	unsigned int uiDSDsig = 0;
+	/* unsigned int sigoffsetInMap = 0;
+	 * DSD_HEADER dsdHeader = {0};
+	 */
 
-		//sigoffsetInMap =(PUCHAR)&(dsdHeader.DSDImageMagicNumber) -(PUCHAR)&dsdHeader;
+	/* sigoffsetInMap =(PUCHAR)&(dsdHeader.DSDImageMagicNumber) -(PUCHAR)&dsdHeader; */
 
-		if(dsd != DSD0 && dsd != DSD1 && dsd != DSD2)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"passed section value is not for DSDs");
-			return STATUS_FAILURE;
-		}
-		BcmFlash2xBulkRead(Adapter,
-						   &uiDSDsig,
-						   dsd,
-						   Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER,DSDImageMagicNumber),
-						   SIGNATURE_SIZE);
+	if (dsd != DSD0 && dsd != DSD1 && dsd != DSD2) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "passed section value is not for DSDs");
+		return STATUS_FAILURE;
+	}
+	BcmFlash2xBulkRead(Adapter,
+			&uiDSDsig,
+			dsd,
+			Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImageMagicNumber),
+			SIGNATURE_SIZE);
 
-		uiDSDsig = ntohl(uiDSDsig);
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"DSD SIG :%x", uiDSDsig);
+	uiDSDsig = ntohl(uiDSDsig);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD SIG :%x", uiDSDsig);
 
-		return uiDSDsig ;
+	return uiDSDsig;
 }
-INT ReadDSDPriority(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL dsd)
+
+int ReadDSDPriority(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL dsd)
 {
-	//UINT priOffsetInMap = 0 ;
+	/* unsigned int priOffsetInMap = 0 ; */
 	unsigned int uiDSDPri = STATUS_FAILURE;
-	//DSD_HEADER dsdHeader = {0};
-	//priOffsetInMap = (PUCHAR)&(dsdHeader.DSDImagePriority) -(PUCHAR)&dsdHeader;
-	if(IsSectionWritable(Adapter,dsd))
-	{
-		if(ReadDSDSignature(Adapter,dsd)== DSD_IMAGE_MAGIC_NUMBER)
-		{
+	/* DSD_HEADER dsdHeader = {0};
+	 * priOffsetInMap = (PUCHAR)&(dsdHeader.DSDImagePriority) -(PUCHAR)&dsdHeader;
+	 */
+	if (IsSectionWritable(Adapter, dsd)) {
+		if (ReadDSDSignature(Adapter, dsd) == DSD_IMAGE_MAGIC_NUMBER) {
 			BcmFlash2xBulkRead(Adapter,
-							   &uiDSDPri,
-							   dsd,
-							   Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader +FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
-							   4);
+					&uiDSDPri,
+					dsd,
+					Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + FIELD_OFFSET_IN_HEADER(PDSD_HEADER, DSDImagePriority),
+					4);
 
 			uiDSDPri = ntohl(uiDSDPri);
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"DSD<%x> Priority :%x", dsd, uiDSDPri);
-
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "DSD<%x> Priority :%x", dsd, uiDSDPri);
 		}
 	}
+
 	return uiDSDPri;
 }
-FLASH2X_SECTION_VAL getHighestPriDSD(PMINI_ADAPTER Adapter)
+
+FLASH2X_SECTION_VAL getHighestPriDSD(struct bcm_mini_adapter *Adapter)
 {
-	INT DSDHighestPri = STATUS_FAILURE;
-	INT  DsdPri= 0 ;
-	FLASH2X_SECTION_VAL HighestPriDSD = 0 ;
+	int DSDHighestPri = STATUS_FAILURE;
+	int DsdPri = 0;
+	FLASH2X_SECTION_VAL HighestPriDSD = 0;
 
-	if(IsSectionWritable(Adapter,DSD2))
-	{
-		DSDHighestPri = ReadDSDPriority(Adapter,DSD2);
-		HighestPriDSD = DSD2 ;
+	if (IsSectionWritable(Adapter, DSD2)) {
+		DSDHighestPri = ReadDSDPriority(Adapter, DSD2);
+		HighestPriDSD = DSD2;
 	}
-	if(IsSectionWritable(Adapter,DSD1))
-	{
-		 DsdPri = ReadDSDPriority(Adapter,DSD1);
-		 if(DSDHighestPri  < DsdPri)
-		 {
-		 	DSDHighestPri = DsdPri ;
+
+	if (IsSectionWritable(Adapter, DSD1)) {
+		DsdPri = ReadDSDPriority(Adapter, DSD1);
+		if (DSDHighestPri  < DsdPri) {
+			DSDHighestPri = DsdPri;
 			HighestPriDSD = DSD1;
-		 }
+		}
 	}
-	if(IsSectionWritable(Adapter,DSD0))
-	{
-		 DsdPri = ReadDSDPriority(Adapter,DSD0);
-		 if(DSDHighestPri  < DsdPri)
-		 {
-		 	DSDHighestPri = DsdPri ;
+
+	if (IsSectionWritable(Adapter, DSD0)) {
+		DsdPri = ReadDSDPriority(Adapter, DSD0);
+		if (DSDHighestPri  < DsdPri) {
+			DSDHighestPri = DsdPri;
 			HighestPriDSD = DSD0;
-		 }
+		}
 	}
-	if(HighestPriDSD)
-	 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Highest DSD :%x , and its  Pri :%x", HighestPriDSD, DSDHighestPri);
-	return  HighestPriDSD ;
+	if (HighestPriDSD)
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Highest DSD :%x , and its  Pri :%x", HighestPriDSD, DSDHighestPri);
+
+	return  HighestPriDSD;
 }
 
-INT ReadISOSignature(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL iso)
+int ReadISOSignature(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL iso)
 {
-		UINT uiISOsig = 0;
-		//UINT sigoffsetInMap = 0;
-		//ISO_HEADER ISOHeader = {0};
-
-
-		//sigoffsetInMap =(PUCHAR)&(ISOHeader.ISOImageMagicNumber) -(PUCHAR)&ISOHeader;
-
-		if(iso != ISO_IMAGE1 && iso != ISO_IMAGE2)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"passed section value is not for ISOs");
-			return STATUS_FAILURE;
-		}
-		BcmFlash2xBulkRead(Adapter,
-						   &uiISOsig,
-						   iso,
-						   0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER,ISOImageMagicNumber),
-						   SIGNATURE_SIZE);
+	unsigned int uiISOsig = 0;
+	/* unsigned int sigoffsetInMap = 0;
+	 * ISO_HEADER ISOHeader = {0};
+	 * sigoffsetInMap =(PUCHAR)&(ISOHeader.ISOImageMagicNumber) -(PUCHAR)&ISOHeader;
+	 */
+	if (iso != ISO_IMAGE1 && iso != ISO_IMAGE2) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "passed section value is not for ISOs");
+		return STATUS_FAILURE;
+	}
+	BcmFlash2xBulkRead(Adapter,
+			&uiISOsig,
+			iso,
+			0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImageMagicNumber),
+			SIGNATURE_SIZE);
 
-		uiISOsig = ntohl(uiISOsig);
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"ISO SIG :%x", uiISOsig);
+	uiISOsig = ntohl(uiISOsig);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO SIG :%x", uiISOsig);
 
-		return uiISOsig ;
+	return uiISOsig;
 }
-INT ReadISOPriority(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL iso)
-{
 
+int ReadISOPriority(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL iso)
+{
 	unsigned int ISOPri = STATUS_FAILURE;
-	if(IsSectionWritable(Adapter,iso))
-	{
-		if(ReadISOSignature(Adapter,iso)== ISO_IMAGE_MAGIC_NUMBER)
-		{
+	if (IsSectionWritable(Adapter, iso)) {
+		if (ReadISOSignature(Adapter, iso) == ISO_IMAGE_MAGIC_NUMBER) {
 			BcmFlash2xBulkRead(Adapter,
-							   &ISOPri,
-							   iso,
-							   0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImagePriority),
-							   4);
+					&ISOPri,
+					iso,
+					0 + FIELD_OFFSET_IN_HEADER(PISO_HEADER, ISOImagePriority),
+					4);
 
 			ISOPri = ntohl(ISOPri);
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"ISO<%x> Priority :%x", iso, ISOPri);
-
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "ISO<%x> Priority :%x", iso, ISOPri);
 		}
 	}
+
 	return ISOPri;
 }
-FLASH2X_SECTION_VAL getHighestPriISO(PMINI_ADAPTER Adapter)
+
+FLASH2X_SECTION_VAL getHighestPriISO(struct bcm_mini_adapter *Adapter)
 {
-	INT ISOHighestPri = STATUS_FAILURE;
-	INT  ISOPri= 0 ;
-	FLASH2X_SECTION_VAL HighestPriISO = NO_SECTION_VAL ;
+	int ISOHighestPri = STATUS_FAILURE;
+	int ISOPri = 0;
+	FLASH2X_SECTION_VAL HighestPriISO = NO_SECTION_VAL;
 
-	if(IsSectionWritable(Adapter,ISO_IMAGE2))
-	{
-		ISOHighestPri = ReadISOPriority(Adapter,ISO_IMAGE2);
-		HighestPriISO = ISO_IMAGE2 ;
+	if (IsSectionWritable(Adapter, ISO_IMAGE2)) {
+		ISOHighestPri = ReadISOPriority(Adapter, ISO_IMAGE2);
+		HighestPriISO = ISO_IMAGE2;
 	}
-	if(IsSectionWritable(Adapter,ISO_IMAGE1))
-	{
-		 ISOPri = ReadISOPriority(Adapter,ISO_IMAGE1);
-		 if(ISOHighestPri  < ISOPri)
-		 {
-			ISOHighestPri = ISOPri ;
+
+	if (IsSectionWritable(Adapter, ISO_IMAGE1)) {
+		ISOPri = ReadISOPriority(Adapter, ISO_IMAGE1);
+		if (ISOHighestPri  < ISOPri) {
+			ISOHighestPri = ISOPri;
 			HighestPriISO = ISO_IMAGE1;
-		 }
+		}
 	}
-	if(HighestPriISO)
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Highest ISO :%x and its Pri :%x",HighestPriISO,ISOHighestPri);
-	return	HighestPriISO ;
+	if (HighestPriISO)
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Highest ISO :%x and its Pri :%x", HighestPriISO, ISOHighestPri);
+
+	return HighestPriISO;
 }
-INT WriteToFlashWithoutSectorErase(PMINI_ADAPTER Adapter,
-										PUINT pBuff,
-										FLASH2X_SECTION_VAL eFlash2xSectionVal,
-										UINT uiOffset,
-										UINT uiNumBytes
-										)
+
+int WriteToFlashWithoutSectorErase(struct bcm_mini_adapter *Adapter,
+				PUINT pBuff,
+				FLASH2X_SECTION_VAL eFlash2xSectionVal,
+				unsigned int uiOffset,
+				unsigned int uiNumBytes)
 {
-#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
-	UINT uiTemp = 0, value = 0 ;
-	UINT i = 0;
-	UINT uiPartOffset = 0;
-#endif
-	UINT uiStartOffset = 0;
-	//Adding section start address
-	INT Status = STATUS_SUCCESS;
+	#if !defined(BCM_SHM_INTERFACE) || defined(FLASH_DIRECT_ACCESS)
+		unsigned int uiTemp = 0, value = 0;
+		unsigned int i = 0;
+		unsigned int uiPartOffset = 0;
+	#endif
+	unsigned int uiStartOffset = 0;
+	/* Adding section start address */
+	int Status = STATUS_SUCCESS;
 	PUCHAR pcBuff = (PUCHAR)pBuff;
 
-	if(uiNumBytes % Adapter->ulFlashWriteSize)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Writing without Sector Erase for non-FlashWriteSize number of bytes 0x%x\n", uiNumBytes);
+	if (uiNumBytes % Adapter->ulFlashWriteSize) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Writing without Sector Erase for non-FlashWriteSize number of bytes 0x%x\n", uiNumBytes);
 		return STATUS_FAILURE;
 	}
 
-	uiStartOffset = BcmGetSectionValStartOffset(Adapter,eFlash2xSectionVal);
+	uiStartOffset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
 
-	if(IsSectionExistInVendorInfo(Adapter,eFlash2xSectionVal))
-	{
+	if (IsSectionExistInVendorInfo(Adapter, eFlash2xSectionVal))
 		return vendorextnWriteSectionWithoutErase(Adapter, pcBuff, eFlash2xSectionVal, uiOffset, uiNumBytes);
-	}
 
 	uiOffset = uiOffset + uiStartOffset;
 
-#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
-  Status = bcmflash_raw_writenoerase((uiOffset/FLASH_PART_SIZE),(uiOffset % FLASH_PART_SIZE), pcBuff,uiNumBytes);
-#else
-	rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	value = 0;
-	wrmalt(Adapter, 0x0f000C80,&value, sizeof(value));
+	#if defined(BCM_SHM_INTERFACE) && !defined(FLASH_DIRECT_ACCESS)
+		Status = bcmflash_raw_writenoerase((uiOffset / FLASH_PART_SIZE), (uiOffset % FLASH_PART_SIZE), pcBuff, uiNumBytes);
+	#else
+		rdmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
+		value = 0;
+		wrmalt(Adapter, 0x0f000C80, &value, sizeof(value));
 
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-	BcmDoChipSelect(Adapter,uiOffset);
-	uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
+		Adapter->SelectedChip = RESET_CHIP_SELECT;
+		BcmDoChipSelect(Adapter, uiOffset);
+		uiPartOffset = (uiOffset & (FLASH_PART_SIZE - 1)) + GetFlashBaseAddr(Adapter);
 
-	for(i = 0 ; i< uiNumBytes; i += Adapter->ulFlashWriteSize)
-	{
-		if(Adapter->ulFlashWriteSize == BYTE_WRITE_SUPPORT)
-			Status = flashByteWrite(Adapter,uiPartOffset, pcBuff);
-		else
-			Status = flashWrite(Adapter,uiPartOffset, pcBuff);
+		for (i = 0 ; i < uiNumBytes; i += Adapter->ulFlashWriteSize) {
+			if (Adapter->ulFlashWriteSize == BYTE_WRITE_SUPPORT)
+				Status = flashByteWrite(Adapter, uiPartOffset, pcBuff);
+			else
+				Status = flashWrite(Adapter, uiPartOffset, pcBuff);
 
-		if(Status != STATUS_SUCCESS)
-			break;
+			if (Status != STATUS_SUCCESS)
+				break;
 
-		pcBuff = pcBuff + Adapter->ulFlashWriteSize;
-		uiPartOffset = uiPartOffset +  Adapter->ulFlashWriteSize;
-	}
-	wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
-	Adapter->SelectedChip = RESET_CHIP_SELECT;
-#endif
+			pcBuff = pcBuff + Adapter->ulFlashWriteSize;
+			uiPartOffset = uiPartOffset +  Adapter->ulFlashWriteSize;
+		}
+		wrmalt(Adapter, 0x0f000C80, &uiTemp, sizeof(uiTemp));
+		Adapter->SelectedChip = RESET_CHIP_SELECT;
+	#endif
 
 	return Status;
 }
 
-BOOLEAN IsSectionExistInFlash(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL section)
+BOOLEAN IsSectionExistInFlash(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL section)
 {
+	BOOLEAN SectionPresent = FALSE;
+
+	switch (section) {
+	case ISO_IMAGE1:
+		if ((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start != UNINIT_PTR_IN_CS) &&
+			(IsNonCDLessDevice(Adapter) == FALSE))
+			SectionPresent = TRUE;
+		break;
+	case ISO_IMAGE2:
+		if ((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start != UNINIT_PTR_IN_CS) &&
+			(IsNonCDLessDevice(Adapter) == FALSE))
+			SectionPresent = TRUE;
+		break;
+	case DSD0:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case DSD1:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case DSD2:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case VSA0:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case VSA1:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case VSA2:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case SCSI:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	case CONTROL_SECTION:
+		if (Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart != UNINIT_PTR_IN_CS)
+			SectionPresent = TRUE;
+		break;
+	default:
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section Does not exist in Flash 2.x");
+		SectionPresent =  FALSE;
+	}
+
+	return SectionPresent;
+}
 
-	BOOLEAN SectionPresent = FALSE ;
+int IsSectionWritable(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL Section)
+{
+	int offset = STATUS_FAILURE;
+	int Status = FALSE;
 
-	switch(section)
-	{
+	if (IsSectionExistInFlash(Adapter, Section) == FALSE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section <%d> does not exixt", Section);
+		return FALSE;
+	}
 
-		case ISO_IMAGE1 :
-			  if((Adapter->psFlash2xCSInfo->OffsetISOImage1Part1Start != UNINIT_PTR_IN_CS) &&
-			  		(IsNonCDLessDevice(Adapter) == FALSE))
-				  SectionPresent = TRUE ;
-			   break;
-		case ISO_IMAGE2 :
-				if((Adapter->psFlash2xCSInfo->OffsetISOImage2Part1Start != UNINIT_PTR_IN_CS) &&
-					(IsNonCDLessDevice(Adapter) == FALSE))
-					 SectionPresent = TRUE ;
-			  break;
-		case DSD0 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSDStart != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case DSD1 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD1Start != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case DSD2 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForDSD2Start != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case VSA0 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSAStart != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case VSA1 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA1Start != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case VSA2 :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForVSA2Start != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case SCSI :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForScsiFirmware != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		case CONTROL_SECTION :
-				if(Adapter->psFlash2xCSInfo->OffsetFromZeroForControlSectionStart != UNINIT_PTR_IN_CS)
-					 SectionPresent = TRUE ;
-				break;
-		default :
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section Does not exist in Flash 2.x");
-			SectionPresent =  FALSE;
+	offset = BcmGetSectionValStartOffset(Adapter, Section);
+	if (offset == INVALID_OFFSET) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section<%d> does not exixt", Section);
+		return FALSE;
 	}
-	return SectionPresent ;
-}
-INT IsSectionWritable(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL Section)
-{
-		INT offset = STATUS_FAILURE;
-		INT Status = FALSE;
-		if(IsSectionExistInFlash(Adapter,Section) == FALSE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section <%d> does not exixt", Section);
-			return FALSE;
-		}
-		offset = BcmGetSectionValStartOffset(Adapter,Section);
-		if(offset == INVALID_OFFSET)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section<%d> does not exixt", Section);
-			return FALSE;
-		}
 
-		if(IsSectionExistInVendorInfo(Adapter,Section))
-		{
-			return !(Adapter->psFlash2xVendorInfo->VendorSection[Section].AccessFlags & FLASH2X_SECTION_RO);
-		}
+	if (IsSectionExistInVendorInfo(Adapter, Section))
+		return !(Adapter->psFlash2xVendorInfo->VendorSection[Section].AccessFlags & FLASH2X_SECTION_RO);
 
-		Status = IsOffsetWritable(Adapter,offset);
-		return Status ;
+	Status = IsOffsetWritable(Adapter, offset);
+	return Status;
 }
 
-static INT CorruptDSDSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
+static int CorruptDSDSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
 {
-
 	PUCHAR pBuff = NULL;
-	UINT sig = 0;
-	UINT uiOffset = 0;
-	UINT BlockStatus = 0;
-	UINT uiSectAlignAddr = 0;
+	unsigned int sig = 0;
+	unsigned int uiOffset = 0;
+	unsigned int BlockStatus = 0;
+	unsigned int uiSectAlignAddr = 0;
 
 	Adapter->bSigCorrupted = FALSE;
-
-	if(Adapter->bAllDSDWriteAllow == FALSE)
-	{
-		if(IsSectionWritable(Adapter,eFlash2xSectionVal) != TRUE)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section is not Writable...Hence can't Corrupt signature");
+	if (Adapter->bAllDSDWriteAllow == FALSE) {
+		if (IsSectionWritable(Adapter, eFlash2xSectionVal) != TRUE) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section is not Writable...Hence can't Corrupt signature");
 			return SECTOR_IS_NOT_WRITABLE;
 		}
 	}
 
 	pBuff = (PUCHAR)kzalloc(MAX_RW_SIZE, GFP_KERNEL);
-	if(pBuff == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
-		return -ENOMEM ;
+	if (!pBuff) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
+		return -ENOMEM;
 	}
 
 	uiOffset = Adapter->psFlash2xCSInfo->OffsetFromDSDStartForDSDHeader + sizeof(DSD_HEADER);
-	uiOffset -= MAX_RW_SIZE ;
+	uiOffset -= MAX_RW_SIZE;
 
-	BcmFlash2xBulkRead(Adapter, (PUINT)pBuff,eFlash2xSectionVal,uiOffset,MAX_RW_SIZE);
+	BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, eFlash2xSectionVal, uiOffset, MAX_RW_SIZE);
 
-
-	sig = *((PUINT)(pBuff +12));
-	sig =ntohl(sig);
-	BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,pBuff,MAX_RW_SIZE);
-	//Now corrupting the sig by corrupting 4th last Byte.
+	sig = *((PUINT)(pBuff + 12));
+	sig = ntohl(sig);
+	BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, pBuff, MAX_RW_SIZE);
+	/* Now corrupting the sig by corrupting 4th last Byte. */
 	*(pBuff + 12) = 0;
 
-	if(sig == DSD_IMAGE_MAGIC_NUMBER)
-	{
+	if (sig == DSD_IMAGE_MAGIC_NUMBER) {
 		Adapter->bSigCorrupted = TRUE;
-		if(Adapter->ulFlashWriteSize == BYTE_WRITE_SUPPORT)
-		{
-			uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize -1);
-			BlockStatus = BcmFlashUnProtectBlock(Adapter,uiSectAlignAddr,Adapter->uiSectorSize);
-
-			WriteToFlashWithoutSectorErase(Adapter,(PUINT)(pBuff + 12),eFlash2xSectionVal,
-												(uiOffset + 12),BYTE_WRITE_SUPPORT);
-			if(BlockStatus)
-			{
-				BcmRestoreBlockProtectStatus(Adapter,BlockStatus);
+		if (Adapter->ulFlashWriteSize == BYTE_WRITE_SUPPORT) {
+			uiSectAlignAddr = uiOffset & ~(Adapter->uiSectorSize - 1);
+			BlockStatus = BcmFlashUnProtectBlock(Adapter, uiSectAlignAddr, Adapter->uiSectorSize);
+
+			WriteToFlashWithoutSectorErase(Adapter, (PUINT)(pBuff + 12), eFlash2xSectionVal,
+						(uiOffset + 12), BYTE_WRITE_SUPPORT);
+			if (BlockStatus) {
+				BcmRestoreBlockProtectStatus(Adapter, BlockStatus);
 				BlockStatus = 0;
 			}
+		} else {
+			WriteToFlashWithoutSectorErase(Adapter, (PUINT)pBuff, eFlash2xSectionVal,
+						uiOffset, MAX_RW_SIZE);
 		}
-		else
-		{
-			WriteToFlashWithoutSectorErase(Adapter,(PUINT)pBuff,eFlash2xSectionVal,
-												uiOffset ,MAX_RW_SIZE);
-		}
-	}
-	else
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"BCM Signature is not present in header");
+	} else {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "BCM Signature is not present in header");
 		kfree(pBuff);
+
 		return STATUS_FAILURE;
 	}
 
 	kfree(pBuff);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Corrupted the signature");
-	return STATUS_SUCCESS ;
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Corrupted the signature");
+
+	return STATUS_SUCCESS;
 }
 
-static INT CorruptISOSig(PMINI_ADAPTER Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
+static int CorruptISOSig(struct bcm_mini_adapter *Adapter, FLASH2X_SECTION_VAL eFlash2xSectionVal)
 {
-
 	PUCHAR pBuff = NULL;
-	UINT sig = 0;
-	UINT uiOffset = 0;
+	unsigned int sig = 0;
+	unsigned int uiOffset = 0;
 
 	Adapter->bSigCorrupted = FALSE;
 
-	if(IsSectionWritable(Adapter,eFlash2xSectionVal) != TRUE)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Section is not Writable...Hence can't Corrupt signature");
+	if (IsSectionWritable(Adapter, eFlash2xSectionVal) != TRUE) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Section is not Writable...Hence can't Corrupt signature");
 		return SECTOR_IS_NOT_WRITABLE;
 	}
 
 	pBuff = (PUCHAR)kzalloc(MAX_RW_SIZE, GFP_KERNEL);
-	if(pBuff == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Can't allocate memorey");
-		return -ENOMEM ;
+	if (!pBuff) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Can't allocate memorey");
+		return -ENOMEM;
 	}
 
 	uiOffset = 0;
 
-	BcmFlash2xBulkRead(Adapter, (PUINT)pBuff,eFlash2xSectionVal,uiOffset, MAX_RW_SIZE);
+	BcmFlash2xBulkRead(Adapter, (PUINT)pBuff, eFlash2xSectionVal, uiOffset, MAX_RW_SIZE);
 
 	sig = *((PUINT)pBuff);
-	sig =ntohl(sig);
+	sig = ntohl(sig);
 
-	//corrupt signature
+	/* corrupt signature */
 	*pBuff = 0;
 
-	if(sig == ISO_IMAGE_MAGIC_NUMBER)
-	{
+	if (sig == ISO_IMAGE_MAGIC_NUMBER) {
 		Adapter->bSigCorrupted = TRUE;
-		WriteToFlashWithoutSectorErase(Adapter,(PUINT)pBuff,eFlash2xSectionVal,
-											uiOffset ,Adapter->ulFlashWriteSize);
-	}
-	else
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"BCM Signature is not present in header");
+		WriteToFlashWithoutSectorErase(Adapter, (PUINT)pBuff, eFlash2xSectionVal,
+					uiOffset, Adapter->ulFlashWriteSize);
+	} else {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "BCM Signature is not present in header");
 		kfree(pBuff);
+
 		return STATUS_FAILURE;
 	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,"Corrupted the signature");
-	BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL,pBuff,MAX_RW_SIZE);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, "Corrupted the signature");
+	BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, NVM_RW, DBG_LVL_ALL, pBuff, MAX_RW_SIZE);
 
 	kfree(pBuff);
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 }
 
-BOOLEAN IsNonCDLessDevice(PMINI_ADAPTER Adapter)
+BOOLEAN IsNonCDLessDevice(struct bcm_mini_adapter *Adapter)
 {
-	if(Adapter->psFlash2xCSInfo->IsCDLessDeviceBootSig == NON_CDLESS_DEVICE_BOOT_SIG)
+	if (Adapter->psFlash2xCSInfo->IsCDLessDeviceBootSig == NON_CDLESS_DEVICE_BOOT_SIG)
 		return TRUE;
 	else
-		return FALSE ;
+		return FALSE;
 }
-
diff --git a/drivers/staging/bcm/sort.c b/drivers/staging/bcm/sort.c
index 63c966a0254..d518c4217f1 100644
--- a/drivers/staging/bcm/sort.c
+++ b/drivers/staging/bcm/sort.c
@@ -13,8 +13,8 @@
 
 static int compare_packet_info(void const *a, void const *b)
 {
-	PacketInfo const *pa = a;
-	PacketInfo const *pb = b;
+	struct bcm_packet_info const *pa = a;
+	struct bcm_packet_info const *pb = b;
 
 	if (!pa->bValid || !pb->bValid)
 		return 0;
@@ -22,19 +22,19 @@ static int compare_packet_info(void const *a, void const *b)
 	return pa->u8TrafficPriority - pb->u8TrafficPriority;
 }
 
-VOID SortPackInfo(PMINI_ADAPTER Adapter)
+VOID SortPackInfo(struct bcm_mini_adapter *Adapter)
 {
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
 			DBG_LVL_ALL, "<=======");
 
-	sort(Adapter->PackInfo, NO_OF_QUEUES, sizeof(PacketInfo),
+	sort(Adapter->PackInfo, NO_OF_QUEUES, sizeof(struct bcm_packet_info),
 		compare_packet_info, NULL);
 }
 
 static int compare_classifiers(void const *a, void const *b)
 {
-	S_CLASSIFIER_RULE const *pa = a;
-	S_CLASSIFIER_RULE const *pb = b;
+	struct bcm_classifier_rule const *pa = a;
+	struct bcm_classifier_rule const *pb = b;
 
 	if (!pa->bUsed || !pb->bUsed)
 		return 0;
@@ -42,11 +42,11 @@ static int compare_classifiers(void const *a, void const *b)
 	return pa->u8ClassifierRulePriority - pb->u8ClassifierRulePriority;
 }
 
-VOID SortClassifiers(PMINI_ADAPTER Adapter)
+VOID SortClassifiers(struct bcm_mini_adapter *Adapter)
 {
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
 			DBG_LVL_ALL, "<=======");
 
 	sort(Adapter->astClassifierTable, MAX_CLASSIFIERS,
-		sizeof(S_CLASSIFIER_RULE), compare_classifiers, NULL);
+		sizeof(struct bcm_classifier_rule), compare_classifiers, NULL);
 }
diff --git a/drivers/staging/bcm/vendorspecificextn.c b/drivers/staging/bcm/vendorspecificextn.c
index 4178cd161da..833883c21a2 100644
--- a/drivers/staging/bcm/vendorspecificextn.c
+++ b/drivers/staging/bcm/vendorspecificextn.c
@@ -28,7 +28,7 @@ INT vendorextnGetSectionInfo(PVOID  pContext,PFLASH2X_VENDORSPECIFIC_INFO pVendo
 //              STATUS_SUCCESS/STATUS_FAILURE
 //
 //-----------------------------------------------------------------------------
-INT vendorextnInit(PMINI_ADAPTER Adapter)
+INT vendorextnInit(struct bcm_mini_adapter *Adapter)
 {
 	return STATUS_SUCCESS;
 }
@@ -45,7 +45,7 @@ INT vendorextnInit(PMINI_ADAPTER Adapter)
 //              STATUS_SUCCESS/STATUS_FAILURE
 //
 //-----------------------------------------------------------------------------
-INT vendorextnExit(PMINI_ADAPTER Adapter)
+INT vendorextnExit(struct bcm_mini_adapter *Adapter)
 {
 	return STATUS_SUCCESS;
 }
@@ -65,7 +65,7 @@ INT vendorextnExit(PMINI_ADAPTER Adapter)
 //		STATUS_SUCCESS/STATUS_FAILURE as per the IOCTL return value
 //
 //--------------------------------------------------------------------------
-INT vendorextnIoctl(PMINI_ADAPTER Adapter, UINT cmd, ULONG arg)
+INT vendorextnIoctl(struct bcm_mini_adapter *Adapter, UINT cmd, ULONG arg)
 {
 	return CONTINUE_COMMON_PATH;
 }
diff --git a/drivers/staging/bcm/vendorspecificextn.h b/drivers/staging/bcm/vendorspecificextn.h
index 7ff14951f0c..f237891b9f2 100644
--- a/drivers/staging/bcm/vendorspecificextn.h
+++ b/drivers/staging/bcm/vendorspecificextn.h
@@ -5,9 +5,9 @@
 #define CONTINUE_COMMON_PATH 0xFFFF
 
 INT vendorextnGetSectionInfo(PVOID  pContext,PFLASH2X_VENDORSPECIFIC_INFO pVendorInfo);
-INT vendorextnExit(PMINI_ADAPTER Adapter);
-INT vendorextnInit(PMINI_ADAPTER Adapter);
-INT vendorextnIoctl(PMINI_ADAPTER Adapter, UINT cmd, ULONG arg);
+INT vendorextnExit(struct bcm_mini_adapter *Adapter);
+INT vendorextnInit(struct bcm_mini_adapter *Adapter);
+INT vendorextnIoctl(struct bcm_mini_adapter *Adapter, UINT cmd, ULONG arg);
 INT vendorextnReadSection(PVOID  pContext, PUCHAR pBuffer, FLASH2X_SECTION_VAL SectionVal,
 			UINT offset, UINT numOfBytes);
 INT vendorextnWriteSection(PVOID  pContext, PUCHAR pBuffer, FLASH2X_SECTION_VAL SectionVal,
diff --git a/drivers/staging/ccg/Kconfig b/drivers/staging/ccg/Kconfig
index ff05e52392b..1f00d701da2 100644
--- a/drivers/staging/ccg/Kconfig
+++ b/drivers/staging/ccg/Kconfig
@@ -2,7 +2,7 @@ if USB_GADGET
 
 config USB_G_CCG
 	tristate "Configurable Composite Gadget (STAGING)"
-	depends on STAGING && !USB_ZERO && !USB_ZERO_HNPTEST && !USB_AUDIO && !GADGET_UAC1 && !USB_ETH && !USB_ETH_RNDIS && !USB_ETH_EEM && !USB_G_NCM && !USB_GADGETFS && !USB_FUNCTIONFS && !USB_FUNCTIONFS_ETH && !USB_FUNCTIONFS_RNDIS && !USB_FUNCTIONFS_GENERIC && !USB_FILE_STORAGE && !USB_FILE_STORAGE_TEST && !USB_MASS_STORAGE && !USB_G_SERIAL && !USB_MIDI_GADGET && !USB_G_PRINTER && !USB_CDC_COMPOSITE && !USB_G_NOKIA && !USB_G_ACM_MS && !USB_G_MULTI && !USB_G_MULTI_RNDIS && !USB_G_MULTI_CDC && !USB_G_HID && !USB_G_DBGP && !USB_G_WEBCAM
+	depends on STAGING && BLOCK && !USB_ZERO && !USB_ZERO_HNPTEST && !USB_AUDIO && !GADGET_UAC1 && !USB_ETH && !USB_ETH_RNDIS && !USB_ETH_EEM && !USB_G_NCM && !USB_GADGETFS && !USB_FUNCTIONFS && !USB_FUNCTIONFS_ETH && !USB_FUNCTIONFS_RNDIS && !USB_FUNCTIONFS_GENERIC && !USB_FILE_STORAGE && !USB_FILE_STORAGE_TEST && !USB_MASS_STORAGE && !USB_G_SERIAL && !USB_MIDI_GADGET && !USB_G_PRINTER && !USB_CDC_COMPOSITE && !USB_G_NOKIA && !USB_G_ACM_MS && !USB_G_MULTI && !USB_G_MULTI_RNDIS && !USB_G_MULTI_CDC && !USB_G_HID && !USB_G_DBGP && !USB_G_WEBCAM
 	help
 	  The Configurable Composite Gadget supports multiple USB
 	  functions: acm, mass storage, rndis and FunctionFS.
diff --git a/drivers/staging/ccg/ccg.c b/drivers/staging/ccg/ccg.c
index a5b36a97598..6a7aab8d9bf 100644
--- a/drivers/staging/ccg/ccg.c
+++ b/drivers/staging/ccg/ccg.c
@@ -564,9 +564,7 @@ static int rndis_function_bind_config(struct ccg_usb_function *f,
 		return -1;
 	}
 
-	pr_info("%s MAC: %02X:%02X:%02X:%02X:%02X:%02X\n", __func__,
-		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
-		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
+	pr_info("%s MAC: %pM\n", __func__, rndis->ethaddr);
 
 	ret = gether_setup_name(c->cdev->gadget, rndis->ethaddr, "rndis");
 	if (ret) {
@@ -654,9 +652,7 @@ static ssize_t rndis_ethaddr_show(struct device *dev,
 {
 	struct ccg_usb_function *f = dev_get_drvdata(dev);
 	struct rndis_function_config *rndis = f->config;
-	return sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x\n",
-		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
-		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
+	return sprintf(buf, "%pM\n", rndis->ethaddr);
 }
 
 static ssize_t rndis_ethaddr_store(struct device *dev,
@@ -1143,7 +1139,7 @@ static int ccg_bind(struct usb_composite_dev *cdev)
 	if (gcnum >= 0)
 		device_desc.bcdDevice = cpu_to_le16(0x0200 + gcnum);
 	else {
-		pr_warning("%s: controller '%s' not recognized\n",
+		pr_warn("%s: controller '%s' not recognized\n",
 			longname, gadget->name);
 		device_desc.bcdDevice = __constant_cpu_to_le16(0x9999);
 	}
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 3bbe3fd103f..6cee7855b01 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -6,9 +6,10 @@ config COMEDI
 	  Enable support a wide range of data acquisition devices
 	  for Linux.
 
+if COMEDI
+
 config COMEDI_DEBUG
 	bool "Comedi debugging"
-	depends on COMEDI != n
 	---help---
 	  This is an option for use by developers; most people should
 	  say N here. This enables comedi core and driver debugging.
@@ -16,7 +17,6 @@ config COMEDI_DEBUG
 config COMEDI_DEFAULT_BUF_SIZE_KB
 	int "Comedi default initial asynchronous buffer size in KiB"
 	default "2048"
-	depends on COMEDI != n
 	---help---
 	  This is the default asynchronous buffer size which is used for
 	  commands running in the background in kernel space.  This
@@ -26,7 +26,6 @@ config COMEDI_DEFAULT_BUF_SIZE_KB
 config COMEDI_DEFAULT_BUF_MAXSIZE_KB
 	int "Comedi default maximum asynchronous buffer size in KiB"
 	default "20480"
-	depends on COMEDI != n
 	---help---
 	  This is the default maximum asynchronous buffer size which can
 	  be requested by a userspace program without root privileges.
@@ -34,8 +33,7 @@ config COMEDI_DEFAULT_BUF_MAXSIZE_KB
 	  channels running at 100 kHz has 2-4 seconds of buffer.
 
 menuconfig COMEDI_MISC_DRIVERS
-	tristate "Comedi misc drivers"
-	depends on COMEDI
+	bool "Comedi misc drivers"
 	---help---
 	  Enable comedi misc drivers to be built
 
@@ -102,8 +100,8 @@ config COMEDI_SKEL
 endif # COMEDI_MISC_DRIVERS
 
 menuconfig COMEDI_ISA_DRIVERS
-	tristate "Comedi ISA and PC/104 drivers"
-	depends on COMEDI && ISA
+	bool "Comedi ISA and PC/104 drivers"
+	depends on ISA
 	---help---
 	  Enable comedi ISA and PC/104 drivers to be built
 
@@ -111,7 +109,7 @@ menuconfig COMEDI_ISA_DRIVERS
 	  kernel: saying N will just cause the configurator to skip all
 	  the questions about ISA and PC/104 comedi drivers.
 
-if COMEDI_ISA_DRIVERS && ISA
+if COMEDI_ISA_DRIVERS
 
 config COMEDI_ACL7225B
 	tristate "ADlink NuDAQ ACL-7225b and compatibles support"
@@ -215,7 +213,6 @@ config COMEDI_PCM3730
 config COMEDI_AMPLC_DIO200_ISA
 	tristate "Amplicon PC212E/PC214E/PC215E/PC218E/PC272E"
 	select COMEDI_AMPLC_DIO200
-	depends on COMEDI_ISA_DRIVERS
 	---help---
 	  Enable support for Amplicon PC212E, PC214E, PC215E, PC218E and
 	  PC272E ISA DIO boards
@@ -447,7 +444,6 @@ config COMEDI_ADQ12B
 
 config COMEDI_NI_AT_A2150
 	tristate "NI AT-A2150 ISA card support"
-	depends on COMEDI_NI_COMMON
 	depends on VIRT_TO_BUS
 	---help---
 	  Enable support for National Instruments AT-A2150 cards
@@ -457,7 +453,6 @@ config COMEDI_NI_AT_A2150
 
 config COMEDI_NI_AT_AO
 	tristate "NI AT-AO-6/10 EISA card support"
-	depends on COMEDI_NI_COMMON
 	---help---
 	  Enable support for National Instruments AT-AO-6/10 cards
 
@@ -466,8 +461,9 @@ config COMEDI_NI_AT_AO
 
 config COMEDI_NI_ATMIO
 	tristate "NI AT-MIO E series ISA-PNP card support"
-	depends on ISAPNP && COMEDI_NI_TIO && COMEDI_NI_COMMON
+	depends on ISAPNP
 	select COMEDI_8255
+	select COMEDI_NI_TIO
 	---help---
 	  Enable support for National Instruments AT-MIO E series cards
 	  National Instruments AT-MIO-16E-1 (ni_atmio),
@@ -479,7 +475,7 @@ config COMEDI_NI_ATMIO
 
 config COMEDI_NI_ATMIO16D
 	tristate "NI AT-MIO16/AT-MIO16D series ISA-PNP card support"
-	depends on ISAPNP && COMEDI_NI_COMMON
+	depends on ISAPNP
 	select COMEDI_8255
 	---help---
 	  Enable support for National Instruments AT-MIO16/AT-MIO16D cards.
@@ -542,8 +538,8 @@ config COMEDI_POC
 endif # COMEDI_ISA_DRIVERS
 
 menuconfig COMEDI_PCI_DRIVERS
-	tristate "Comedi PCI drivers"
-	depends on COMEDI && PCI
+	bool "Comedi PCI drivers"
+	depends on PCI
 	---help---
 	  Enable comedi PCI drivers to be built
 
@@ -551,7 +547,7 @@ menuconfig COMEDI_PCI_DRIVERS
 	  kernel: saying N will just cause the configurator to skip all
 	  the questions about PCI comedi drivers.
 
-if COMEDI_PCI_DRIVERS && PCI
+if COMEDI_PCI_DRIVERS
 
 config COMEDI_ADDI_APCI_035
 	tristate "ADDI-DATA APCI_035 support"
@@ -674,7 +670,6 @@ config COMEDI_ADDI_APCI_3XXX
 
 config COMEDI_ADL_PCI6208
 	tristate "ADLink PCI-6208A support"
-	select COMEDI_8255
 	---help---
 	  Enable support for ADLink PCI-6208A cards
 
@@ -691,6 +686,7 @@ config COMEDI_ADL_PCI7230
 
 config COMEDI_ADL_PCI7296
 	tristate "ADLink PCI-7296 96 ch. digital io board support"
+	select COMEDI_8255
 	---help---
 	  Enable support for ADlink PCI-7296 96 ch. digital io board support
 
@@ -988,7 +984,7 @@ config COMEDI_ME_DAQ
 
 config COMEDI_NI_6527
 	tristate "NI 6527 support"
-	depends on COMEDI_MITE
+	select COMEDI_MITE
 	---help---
 	  Enable support for the National Instruments 6527 PCI card
 
@@ -997,7 +993,7 @@ config COMEDI_NI_6527
 
 config COMEDI_NI_65XX
 	tristate "NI 65xx static dio PCI card support"
-	depends on COMEDI_MITE
+	select COMEDI_MITE
 	---help---
 	  Enable support for National Instruments 65xx static dio boards.
 	  Supported devices: National Instruments PCI-6509 (ni_65xx),
@@ -1010,7 +1006,7 @@ config COMEDI_NI_65XX
 
 config COMEDI_NI_660X
 	tristate "NI 660x counter/timer PCI card support"
-	depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
+	select COMEDI_NI_TIOCMD
 	---help---
 	  Enable support for National Instruments PCI-6601 (ni_660x), PCI-6602,
 	  PXI-6602 and PXI-6608.
@@ -1020,16 +1016,31 @@ config COMEDI_NI_660X
 
 config COMEDI_NI_670X
 	tristate "NI 670x PCI card support"
-	depends on COMEDI_MITE
+	select COMEDI_MITE
 	---help---
 	  Enable support for National Instruments PCI-6703 and PCI-6704
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called ni_670x.
 
+config COMEDI_NI_LABPC
+	tristate "NI Lab-PC and compatibles ISA and PCI support"
+	select COMEDI_MITE
+	select COMEDI_8255
+	select COMEDI_FC
+	depends on VIRT_TO_BUS
+	---help---
+	  Enable support for National Instruments Lab-PC and compatibles
+	  Lab-PC-1200, Lab-PC-1200AI, Lab-PC+ and PCI-1200.
+	  Kernel-level ISA plug-and-play support for the lab-pc-1200 boards has
+	  not yet been added to the driver.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ni_labpc.
+
 config COMEDI_NI_PCIDIO
 	tristate "NI PCI-DIO32HS, PCI-DIO96, PCI-6533, PCI-6503 support"
-	depends on COMEDI_MITE
+	select COMEDI_MITE
 	select COMEDI_8255
 	---help---
 	  Enable support for National Instruments PCI-DIO-32HS, PXI-6533,
@@ -1043,7 +1054,7 @@ config COMEDI_NI_PCIDIO
 
 config COMEDI_NI_PCIMIO
 	tristate "NI PCI-MIO-E series and M series support"
-	depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
+	select COMEDI_NI_TIOCMD
 	select COMEDI_8255
 	select COMEDI_FC
 	---help---
@@ -1063,7 +1074,6 @@ config COMEDI_NI_PCIMIO
 
 config COMEDI_RTD520
 	tristate "Real Time Devices PCI4520/DM7520 support"
-	select COMEDI_8255
 	---help---
 	  Enable support for Real Time Devices PCI4520/DM7520
 
@@ -1095,11 +1105,19 @@ config COMEDI_SSV_DNP
 	  To compile this driver as a module, choose M here: the module will be
 	  called ssv_dnp.
 
+config COMEDI_MITE
+	tristate
+
+config COMEDI_NI_TIOCMD
+	tristate
+	select COMEDI_NI_TIO
+	select COMEDI_MITE
+
 endif # COMEDI_PCI_DRIVERS
 
 menuconfig COMEDI_PCMCIA_DRIVERS
-	tristate "Comedi PCMCIA drivers"
-	depends on COMEDI && (PCMCIA || PCCARD)
+	bool "Comedi PCMCIA drivers"
+	depends on PCMCIA
 	---help---
 	  Enable comedi PCMCIA and PCCARD drivers to be built
 
@@ -1107,7 +1125,7 @@ menuconfig COMEDI_PCMCIA_DRIVERS
 	  kernel: saying N will just cause the configurator to skip all
 	  the questions about PCMCIA comedi drivers.
 
-if COMEDI_PCMCIA_DRIVERS && PCMCIA
+if COMEDI_PCMCIA_DRIVERS
 
 config COMEDI_CB_DAS16_CS
 	tristate "CB DAS16 series PCMCIA support"
@@ -1130,7 +1148,6 @@ config COMEDI_DAS08_CS
 
 config COMEDI_NI_DAQ_700_CS
 	tristate "NI DAQCard-700 PCMCIA support"
-	depends on COMEDI_NI_COMMON
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQCard-700 DIO
 
@@ -1139,7 +1156,6 @@ config COMEDI_NI_DAQ_700_CS
 
 config COMEDI_NI_DAQ_DIO24_CS
 	tristate "NI DAQ-Card DIO-24 PCMCIA support"
-	depends on COMEDI_NI_COMMON
 	select COMEDI_8255
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQ-Card DIO-24
@@ -1158,7 +1174,7 @@ config COMEDI_NI_LABPC_CS
 
 config COMEDI_NI_MIO_CS
 	tristate "NI DAQCard E series PCMCIA support"
-	depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
+	select COMEDI_NI_TIO
 	select COMEDI_8255
 	select COMEDI_FC
 	---help---
@@ -1181,8 +1197,8 @@ config COMEDI_QUATECH_DAQP_CS
 endif # COMEDI_PCMCIA_DRIVERS
 
 menuconfig COMEDI_USB_DRIVERS
-	tristate "Comedi USB drivers"
-	depends on COMEDI && USB
+	bool "Comedi USB drivers"
+	depends on USB
 	---help---
 	  Enable comedi USB drivers to be built
 
@@ -1190,7 +1206,7 @@ menuconfig COMEDI_USB_DRIVERS
 	  kernel: saying N will just cause the configurator to skip all
 	  the questions about USB comedi drivers.
 
-if COMEDI_USB_DRIVERS && USB
+if COMEDI_USB_DRIVERS
 
 config COMEDI_DT9812
 	tristate "DataTranslation DT9812 USB module support"
@@ -1237,60 +1253,8 @@ config COMEDI_VMK80XX
 
 endif # COMEDI_USB_DRIVERS
 
-menuconfig COMEDI_NI_COMMON
-	tristate "Comedi National Instruments card support"
-	depends on COMEDI
-	---help---
-	  Enable comedi support for National Instruments cards.
-	  Modules in this section are used by many comedi NI drivers.
-
-	  Note that the answer to this question won't directly affect the
-	  kernel: saying N will just cause the configurator to skip all
-	  the questions about National Instruments cards.
-
-if COMEDI_NI_COMMON
-
-config COMEDI_MITE
-	tristate "NI Mite PCI interface chip support"
-	depends on PCI
-	---help---
-	  Enable support for National Instruments Mite PCI interface chip
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called mite.
-
-config COMEDI_NI_TIO
-	tristate "NI general purpose counter support"
-	depends on COMEDI_MITE
-	---help---
-	  Enable support for National Instruments general purpose counters.
-	  This module is not used directly by end-users. Rather, it
-	  is used by other drivers (for example ni_660x and ni_pcimio)
-	  to provide support for NI's general purpose counters.
-
-	  To compile this driver as a modules, choose M here: two modules will
-	  be build: ni_tio and ni_tiocmd.
-
-config COMEDI_NI_LABPC
-	tristate "NI Lab-PC and compatibles ISA and PCI support"
-	depends on COMEDI_MITE
-	select COMEDI_8255
-	select COMEDI_FC
-	depends on VIRT_TO_BUS
-	---help---
-	  Enable support for National Instruments Lab-PC and compatibles
-	  Lab-PC-1200, Lab-PC-1200AI, Lab-PC+ and PCI-1200.
-	  Kernel-level ISA plug-and-play support for the lab-pc-1200 boards has
-	  not yet been added to the driver.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called ni_labpc.
-
-endif # COMEDI_NI_COMMON
-
 config COMEDI_8255
 	tristate "Generic 8255 support"
-	depends on COMEDI
 	---help---
 	  Enable generic 8255 support.
 
@@ -1305,31 +1269,24 @@ config COMEDI_8255
 	  called 8255.
 
 config COMEDI_FC
-	tristate "Comedi shared functions for low-level driver support"
-	depends on COMEDI
-	---help---
-	  Enable support for shared functions for low-level drivers.
-	  This module is not used directly by end-users. Rather, it
-	  is used by many other comedi drivers.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called comedi_fc.
+	tristate
 
 config COMEDI_AMPLC_DIO200
 	tristate
-	depends on COMEDI
 	select COMEDI_8255
 
 config COMEDI_AMPLC_PC236
 	tristate
-	depends on COMEDI
 	select COMEDI_8255
 
 config COMEDI_AMPLC_PC263
 	tristate
-	depends on COMEDI
 
 config COMEDI_DAS08
 	tristate
-	depends on COMEDI
 	select COMEDI_8255
+
+config COMEDI_NI_TIO
+	tristate
+
+endif # COMEDI
diff --git a/drivers/staging/comedi/comedi_compat32.c b/drivers/staging/comedi/comedi_compat32.c
index 41a7a62ba49..0a5057f0919 100644
--- a/drivers/staging/comedi/comedi_compat32.c
+++ b/drivers/staging/comedi/comedi_compat32.c
@@ -26,6 +26,8 @@
 
 #define __NO_VERSION__
 #include <linux/uaccess.h>
+#include <linux/compat.h>
+#include <linux/fs.h>
 #include "comedi.h"
 #include "comedi_compat32.h"
 
diff --git a/drivers/staging/comedi/comedi_compat32.h b/drivers/staging/comedi/comedi_compat32.h
index 0340a8949c6..60cf51c4a79 100644
--- a/drivers/staging/comedi/comedi_compat32.h
+++ b/drivers/staging/comedi/comedi_compat32.h
@@ -27,17 +27,15 @@
 #ifndef _COMEDI_COMPAT32_H
 #define _COMEDI_COMPAT32_H
 
-#include <linux/compat.h>
-#include <linux/fs.h>
-
 #ifdef CONFIG_COMPAT
 
+struct file;
 extern long comedi_compat_ioctl(struct file *file, unsigned int cmd,
 				unsigned long arg);
 
 #else /* CONFIG_COMPAT */
 
-#define comedi_compat_ioctl 0	/* NULL */
+#define comedi_compat_ioctl	NULL
 
 #endif /* CONFIG_COMPAT */
 
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 76776571ed9..e82126407e9 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -24,7 +24,6 @@
 #undef DEBUG
 
 #define __NO_VERSION__
-#include "comedi_fops.h"
 #include "comedi_compat32.h"
 
 #include <linux/module.h>
@@ -49,7 +48,7 @@
 #include <linux/io.h>
 #include <linux/uaccess.h>
 
-#include "internal.h"
+#include "comedi_internal.h"
 
 MODULE_AUTHOR("http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi core module");
@@ -92,36 +91,6 @@ static DEFINE_SPINLOCK(comedi_file_info_table_lock);
 static struct comedi_device_file_info
 *comedi_file_info_table[COMEDI_NUM_MINORS];
 
-static int do_devconfig_ioctl(struct comedi_device *dev,
-			      struct comedi_devconfig __user *arg);
-static int do_bufconfig_ioctl(struct comedi_device *dev,
-			      struct comedi_bufconfig __user *arg);
-static int do_devinfo_ioctl(struct comedi_device *dev,
-			    struct comedi_devinfo __user *arg,
-			    struct file *file);
-static int do_subdinfo_ioctl(struct comedi_device *dev,
-			     struct comedi_subdinfo __user *arg, void *file);
-static int do_chaninfo_ioctl(struct comedi_device *dev,
-			     struct comedi_chaninfo __user *arg);
-static int do_bufinfo_ioctl(struct comedi_device *dev,
-			    struct comedi_bufinfo __user *arg, void *file);
-static int do_cmd_ioctl(struct comedi_device *dev,
-			struct comedi_cmd __user *arg, void *file);
-static int do_lock_ioctl(struct comedi_device *dev, unsigned int arg,
-			 void *file);
-static int do_unlock_ioctl(struct comedi_device *dev, unsigned int arg,
-			   void *file);
-static int do_cancel_ioctl(struct comedi_device *dev, unsigned int arg,
-			   void *file);
-static int do_cmdtest_ioctl(struct comedi_device *dev,
-			    struct comedi_cmd __user *arg, void *file);
-static int do_insnlist_ioctl(struct comedi_device *dev,
-			     struct comedi_insnlist __user *arg, void *file);
-static int do_insn_ioctl(struct comedi_device *dev,
-			 struct comedi_insn __user *arg, void *file);
-static int do_poll_ioctl(struct comedi_device *dev, unsigned int subd,
-			 void *file);
-
 static void do_become_nonbusy(struct comedi_device *dev,
 			      struct comedi_subdevice *s);
 static int do_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
@@ -172,28 +141,19 @@ static int resize_async_buffer(struct comedi_device *dev,
 
 /* sysfs attribute files */
 
-static const unsigned bytes_per_kibi = 1024;
-
 static ssize_t show_max_read_buffer_kb(struct device *dev,
 				       struct device_attribute *attr, char *buf)
 {
-	ssize_t retval;
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned max_buffer_size_kb = 0;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
+	struct comedi_subdevice *s = comedi_get_read_subdevice(info);
+	unsigned int size = 0;
 
 	mutex_lock(&info->device->mutex);
-	if (read_subdevice &&
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) &&
-	    read_subdevice->async) {
-		max_buffer_size_kb = read_subdevice->async->max_bufsize /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", max_buffer_size_kb);
+	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
+		size = s->async->max_bufsize / 1024;
 	mutex_unlock(&info->device->mutex);
 
-	return retval;
+	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
 static ssize_t store_max_read_buffer_kb(struct device *dev,
@@ -201,52 +161,40 @@ static ssize_t store_max_read_buffer_kb(struct device *dev,
 					const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_max_size_kb;
-	unsigned int new_max_size;
-	int ret;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_max_size_kb);
-	if (ret)
-		return ret;
-	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
+	struct comedi_subdevice *s = comedi_get_read_subdevice(info);
+	unsigned int size;
+	int err;
+
+	err = kstrtouint(buf, 10, &size);
+	if (err)
+		return err;
+	if (size > (UINT_MAX / 1024))
 		return -EINVAL;
-	new_max_size = new_max_size_kb * bytes_per_kibi;
+	size *= 1024;
 
 	mutex_lock(&info->device->mutex);
-	if (read_subdevice == NULL ||
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) == 0 ||
-	    read_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	read_subdevice->async->max_bufsize = new_max_size;
+	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
+		s->async->max_bufsize = size;
+	else
+		err = -EINVAL;
 	mutex_unlock(&info->device->mutex);
 
-	return count;
+	return err ? err : count;
 }
 
 static ssize_t show_read_buffer_kb(struct device *dev,
 				   struct device_attribute *attr, char *buf)
 {
-	ssize_t retval;
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned buffer_size_kb = 0;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
+	struct comedi_subdevice *s = comedi_get_read_subdevice(info);
+	unsigned int size = 0;
 
 	mutex_lock(&info->device->mutex);
-	if (read_subdevice &&
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) &&
-	    read_subdevice->async) {
-		buffer_size_kb = read_subdevice->async->prealloc_bufsz /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", buffer_size_kb);
+	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
+		size = s->async->prealloc_bufsz / 1024;
 	mutex_unlock(&info->device->mutex);
 
-	return retval;
+	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
 static ssize_t store_read_buffer_kb(struct device *dev,
@@ -254,57 +202,41 @@ static ssize_t store_read_buffer_kb(struct device *dev,
 				    const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_size_kb;
-	unsigned int new_size;
-	int retval;
-	int ret;
-	struct comedi_subdevice *const read_subdevice =
-	    comedi_get_read_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_size_kb);
-	if (ret)
-		return ret;
-	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
+	struct comedi_subdevice *s = comedi_get_read_subdevice(info);
+	unsigned int size;
+	int err;
+
+	err = kstrtouint(buf, 10, &size);
+	if (err)
+		return err;
+	if (size > (UINT_MAX / 1024))
 		return -EINVAL;
-	new_size = new_size_kb * bytes_per_kibi;
+	size *= 1024;
 
 	mutex_lock(&info->device->mutex);
-	if (read_subdevice == NULL ||
-	    (read_subdevice->subdev_flags & SDF_CMD_READ) == 0 ||
-	    read_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	retval = resize_async_buffer(info->device, read_subdevice,
-				     read_subdevice->async, new_size);
+	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
+		err = resize_async_buffer(info->device, s, s->async, size);
+	else
+		err = -EINVAL;
 	mutex_unlock(&info->device->mutex);
 
-	if (retval < 0)
-		return retval;
-	return count;
+	return err ? err : count;
 }
 
 static ssize_t show_max_write_buffer_kb(struct device *dev,
 					struct device_attribute *attr,
 					char *buf)
 {
-	ssize_t retval;
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned max_buffer_size_kb = 0;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
+	struct comedi_subdevice *s = comedi_get_write_subdevice(info);
+	unsigned int size = 0;
 
 	mutex_lock(&info->device->mutex);
-	if (write_subdevice &&
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) &&
-	    write_subdevice->async) {
-		max_buffer_size_kb = write_subdevice->async->max_bufsize /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", max_buffer_size_kb);
+	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
+		size = s->async->max_bufsize / 1024;
 	mutex_unlock(&info->device->mutex);
 
-	return retval;
+	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
 static ssize_t store_max_write_buffer_kb(struct device *dev,
@@ -312,52 +244,40 @@ static ssize_t store_max_write_buffer_kb(struct device *dev,
 					 const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_max_size_kb;
-	unsigned int new_max_size;
-	int ret;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_max_size_kb);
-	if (ret)
-		return ret;
-	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
+	struct comedi_subdevice *s = comedi_get_write_subdevice(info);
+	unsigned int size;
+	int err;
+
+	err = kstrtouint(buf, 10, &size);
+	if (err)
+		return err;
+	if (size > (UINT_MAX / 1024))
 		return -EINVAL;
-	new_max_size = new_max_size_kb * bytes_per_kibi;
+	size *= 1024;
 
 	mutex_lock(&info->device->mutex);
-	if (write_subdevice == NULL ||
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) == 0 ||
-	    write_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	write_subdevice->async->max_bufsize = new_max_size;
+	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
+		s->async->max_bufsize = size;
+	else
+		err = -EINVAL;
 	mutex_unlock(&info->device->mutex);
 
-	return count;
+	return err ? err : count;
 }
 
 static ssize_t show_write_buffer_kb(struct device *dev,
 				    struct device_attribute *attr, char *buf)
 {
-	ssize_t retval;
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned buffer_size_kb = 0;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
+	struct comedi_subdevice *s = comedi_get_write_subdevice(info);
+	unsigned int size = 0;
 
 	mutex_lock(&info->device->mutex);
-	if (write_subdevice &&
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) &&
-	    write_subdevice->async) {
-		buffer_size_kb = write_subdevice->async->prealloc_bufsz /
-		    bytes_per_kibi;
-	}
-	retval = snprintf(buf, PAGE_SIZE, "%i\n", buffer_size_kb);
+	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
+		size = s->async->prealloc_bufsz / 1024;
 	mutex_unlock(&info->device->mutex);
 
-	return retval;
+	return snprintf(buf, PAGE_SIZE, "%i\n", size);
 }
 
 static ssize_t store_write_buffer_kb(struct device *dev,
@@ -365,34 +285,25 @@ static ssize_t store_write_buffer_kb(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned int new_size_kb;
-	unsigned int new_size;
-	int retval;
-	int ret;
-	struct comedi_subdevice *const write_subdevice =
-	    comedi_get_write_subdevice(info);
-
-	ret = kstrtouint(buf, 10, &new_size_kb);
-	if (ret)
-		return ret;
-	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
+	struct comedi_subdevice *s = comedi_get_write_subdevice(info);
+	unsigned int size;
+	int err;
+
+	err = kstrtouint(buf, 10, &size);
+	if (err)
+		return err;
+	if (size > (UINT_MAX / 1024))
 		return -EINVAL;
-	new_size = ((uint64_t) new_size_kb) * bytes_per_kibi;
+	size *= 1024;
 
 	mutex_lock(&info->device->mutex);
-	if (write_subdevice == NULL ||
-	    (write_subdevice->subdev_flags & SDF_CMD_WRITE) == 0 ||
-	    write_subdevice->async == NULL) {
-		mutex_unlock(&info->device->mutex);
-		return -EINVAL;
-	}
-	retval = resize_async_buffer(info->device, write_subdevice,
-				     write_subdevice->async, new_size);
+	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
+		err = resize_async_buffer(info->device, s, s->async, size);
+	else
+		err = -EINVAL;
 	mutex_unlock(&info->device->mutex);
 
-	if (retval < 0)
-		return retval;
-	return count;
+	return err ? err : count;
 }
 
 static struct device_attribute comedi_dev_attrs[] = {
@@ -407,98 +318,6 @@ static struct device_attribute comedi_dev_attrs[] = {
 	__ATTR_NULL
 };
 
-static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
-				  unsigned long arg)
-{
-	const unsigned minor = iminor(file->f_dentry->d_inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
-	struct comedi_device *dev;
-	int rc;
-
-	if (dev_file_info == NULL || dev_file_info->device == NULL)
-		return -ENODEV;
-	dev = dev_file_info->device;
-
-	mutex_lock(&dev->mutex);
-
-	/* Device config is special, because it must work on
-	 * an unconfigured device. */
-	if (cmd == COMEDI_DEVCONFIG) {
-		rc = do_devconfig_ioctl(dev,
-					(struct comedi_devconfig __user *)arg);
-		goto done;
-	}
-
-	if (!dev->attached) {
-		DPRINTK("no driver configured on /dev/comedi%i\n", dev->minor);
-		rc = -ENODEV;
-		goto done;
-	}
-
-	switch (cmd) {
-	case COMEDI_BUFCONFIG:
-		rc = do_bufconfig_ioctl(dev,
-					(struct comedi_bufconfig __user *)arg);
-		break;
-	case COMEDI_DEVINFO:
-		rc = do_devinfo_ioctl(dev, (struct comedi_devinfo __user *)arg,
-				      file);
-		break;
-	case COMEDI_SUBDINFO:
-		rc = do_subdinfo_ioctl(dev,
-				       (struct comedi_subdinfo __user *)arg,
-				       file);
-		break;
-	case COMEDI_CHANINFO:
-		rc = do_chaninfo_ioctl(dev, (void __user *)arg);
-		break;
-	case COMEDI_RANGEINFO:
-		rc = do_rangeinfo_ioctl(dev, (void __user *)arg);
-		break;
-	case COMEDI_BUFINFO:
-		rc = do_bufinfo_ioctl(dev,
-				      (struct comedi_bufinfo __user *)arg,
-				      file);
-		break;
-	case COMEDI_LOCK:
-		rc = do_lock_ioctl(dev, arg, file);
-		break;
-	case COMEDI_UNLOCK:
-		rc = do_unlock_ioctl(dev, arg, file);
-		break;
-	case COMEDI_CANCEL:
-		rc = do_cancel_ioctl(dev, arg, file);
-		break;
-	case COMEDI_CMD:
-		rc = do_cmd_ioctl(dev, (struct comedi_cmd __user *)arg, file);
-		break;
-	case COMEDI_CMDTEST:
-		rc = do_cmdtest_ioctl(dev, (struct comedi_cmd __user *)arg,
-				      file);
-		break;
-	case COMEDI_INSNLIST:
-		rc = do_insnlist_ioctl(dev,
-				       (struct comedi_insnlist __user *)arg,
-				       file);
-		break;
-	case COMEDI_INSN:
-		rc = do_insn_ioctl(dev, (struct comedi_insn __user *)arg,
-				   file);
-		break;
-	case COMEDI_POLL:
-		rc = do_poll_ioctl(dev, arg, file);
-		break;
-	default:
-		rc = -ENOTTY;
-		break;
-	}
-
-done:
-	mutex_unlock(&dev->mutex);
-	return rc;
-}
-
 /*
 	COMEDI_DEVCONFIG
 	device config ioctl
@@ -1709,6 +1528,98 @@ static int do_poll_ioctl(struct comedi_device *dev, unsigned int arg,
 	return -EINVAL;
 }
 
+static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
+				  unsigned long arg)
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	struct comedi_device *dev;
+	int rc;
+
+	if (dev_file_info == NULL || dev_file_info->device == NULL)
+		return -ENODEV;
+	dev = dev_file_info->device;
+
+	mutex_lock(&dev->mutex);
+
+	/* Device config is special, because it must work on
+	 * an unconfigured device. */
+	if (cmd == COMEDI_DEVCONFIG) {
+		rc = do_devconfig_ioctl(dev,
+					(struct comedi_devconfig __user *)arg);
+		goto done;
+	}
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on /dev/comedi%i\n", dev->minor);
+		rc = -ENODEV;
+		goto done;
+	}
+
+	switch (cmd) {
+	case COMEDI_BUFCONFIG:
+		rc = do_bufconfig_ioctl(dev,
+					(struct comedi_bufconfig __user *)arg);
+		break;
+	case COMEDI_DEVINFO:
+		rc = do_devinfo_ioctl(dev, (struct comedi_devinfo __user *)arg,
+				      file);
+		break;
+	case COMEDI_SUBDINFO:
+		rc = do_subdinfo_ioctl(dev,
+				       (struct comedi_subdinfo __user *)arg,
+				       file);
+		break;
+	case COMEDI_CHANINFO:
+		rc = do_chaninfo_ioctl(dev, (void __user *)arg);
+		break;
+	case COMEDI_RANGEINFO:
+		rc = do_rangeinfo_ioctl(dev, (void __user *)arg);
+		break;
+	case COMEDI_BUFINFO:
+		rc = do_bufinfo_ioctl(dev,
+				      (struct comedi_bufinfo __user *)arg,
+				      file);
+		break;
+	case COMEDI_LOCK:
+		rc = do_lock_ioctl(dev, arg, file);
+		break;
+	case COMEDI_UNLOCK:
+		rc = do_unlock_ioctl(dev, arg, file);
+		break;
+	case COMEDI_CANCEL:
+		rc = do_cancel_ioctl(dev, arg, file);
+		break;
+	case COMEDI_CMD:
+		rc = do_cmd_ioctl(dev, (struct comedi_cmd __user *)arg, file);
+		break;
+	case COMEDI_CMDTEST:
+		rc = do_cmdtest_ioctl(dev, (struct comedi_cmd __user *)arg,
+				      file);
+		break;
+	case COMEDI_INSNLIST:
+		rc = do_insnlist_ioctl(dev,
+				       (struct comedi_insnlist __user *)arg,
+				       file);
+		break;
+	case COMEDI_INSN:
+		rc = do_insn_ioctl(dev, (struct comedi_insn __user *)arg,
+				   file);
+		break;
+	case COMEDI_POLL:
+		rc = do_poll_ioctl(dev, arg, file);
+		break;
+	default:
+		rc = -ENOTTY;
+		break;
+	}
+
+done:
+	mutex_unlock(&dev->mutex);
+	return rc;
+}
+
 static int do_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	int ret = 0;
@@ -2270,7 +2181,7 @@ static int comedi_fasync(int fd, struct file *file, int on)
 	return fasync_helper(fd, file, on, &dev->async_queue);
 }
 
-const struct file_operations comedi_fops = {
+static const struct file_operations comedi_fops = {
 	.owner = THIS_MODULE,
 	.unlocked_ioctl = comedi_unlocked_ioctl,
 	.compat_ioctl = comedi_compat_ioctl,
@@ -2284,7 +2195,7 @@ const struct file_operations comedi_fops = {
 	.llseek = noop_llseek,
 };
 
-struct class *comedi_class;
+static struct class *comedi_class;
 static struct cdev comedi_cdev;
 
 static void comedi_cleanup_legacy_minors(void)
diff --git a/drivers/staging/comedi/comedi_fops.h b/drivers/staging/comedi/comedi_fops.h
deleted file mode 100644
index 006cf14c577..00000000000
--- a/drivers/staging/comedi/comedi_fops.h
+++ /dev/null
@@ -1,11 +0,0 @@
-
-#ifndef _COMEDI_FOPS_H
-#define _COMEDI_FOPS_H
-#include <linux/types.h>
-
-extern struct class *comedi_class;
-extern const struct file_operations comedi_fops;
-extern bool comedi_autoconfig;
-extern struct comedi_driver *comedi_drivers;
-
-#endif /* _COMEDI_FOPS_H */
diff --git a/drivers/staging/comedi/internal.h b/drivers/staging/comedi/comedi_internal.h
index 7ed20a04eef..e70ef0515d9 100644
--- a/drivers/staging/comedi/internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -1,3 +1,8 @@
+#ifndef _COMEDI_INTERNAL_H
+#define _COMEDI_INTERNAL_H
+
+#include <linux/types.h>
+
 /*
  * various internal comedi stuff
  */
@@ -14,3 +19,7 @@ int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 
 extern unsigned int comedi_default_buf_size_kb;
 extern unsigned int comedi_default_buf_maxsize_kb;
+extern bool comedi_autoconfig;
+extern struct comedi_driver *comedi_drivers;
+
+#endif /* _COMEDI_INTERNAL_H */
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index 134be93eaa6..f713783ef62 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -40,6 +40,7 @@
 #include <linux/uaccess.h>
 #include <linux/io.h>
 #include <linux/timer.h>
+#include <linux/pci.h>
 
 #include "comedi.h"
 
@@ -77,7 +78,7 @@ struct comedi_subdevice {
 	unsigned runflags;
 	spinlock_t spin_lock;
 
-	int io_bits;
+	unsigned int io_bits;
 
 	unsigned int maxdata;	/* if maxdata==0, use list */
 	const unsigned int *maxdata_list;	/* list is channel specific */
@@ -180,7 +181,6 @@ struct comedi_async {
 			unsigned int x);
 };
 
-struct pci_dev;
 struct usb_interface;
 
 struct comedi_driver {
@@ -292,6 +292,8 @@ static inline struct comedi_subdevice *comedi_get_write_subdevice(
 	return info->device->write_subdev;
 }
 
+int comedi_alloc_subdevices(struct comedi_device *, int);
+
 void comedi_device_detach(struct comedi_device *dev);
 int comedi_device_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it);
@@ -310,7 +312,8 @@ int comedi_driver_unregister(struct comedi_driver *);
 	module_driver(__comedi_driver, comedi_driver_register, \
 			comedi_driver_unregister)
 
-struct pci_driver;
+int comedi_pci_enable(struct pci_dev *, const char *);
+void comedi_pci_disable(struct pci_dev *);
 
 int comedi_pci_driver_register(struct comedi_driver *, struct pci_driver *);
 void comedi_pci_driver_unregister(struct comedi_driver *, struct pci_driver *);
@@ -412,26 +415,6 @@ struct comedi_lrange {
 
 /* some silly little inline functions */
 
-static inline int alloc_subdevices(struct comedi_device *dev,
-				   unsigned int num_subdevices)
-{
-	unsigned i;
-
-	dev->n_subdevices = num_subdevices;
-	dev->subdevices =
-	    kcalloc(num_subdevices, sizeof(struct comedi_subdevice),
-		    GFP_KERNEL);
-	if (!dev->subdevices)
-		return -ENOMEM;
-	for (i = 0; i < num_subdevices; ++i) {
-		dev->subdevices[i].device = dev;
-		dev->subdevices[i].async_dma_dir = DMA_NONE;
-		spin_lock_init(&dev->subdevices[i].spin_lock);
-		dev->subdevices[i].minor = -1;
-	}
-	return 0;
-}
-
 static inline int alloc_private(struct comedi_device *dev, int size)
 {
 	dev->private = kzalloc(size, GFP_KERNEL);
@@ -463,6 +446,11 @@ static inline void comedi_set_hw_dev(struct comedi_device *dev,
 	}
 }
 
+static inline struct pci_dev *comedi_to_pci_dev(struct comedi_device *dev)
+{
+	return dev->hw_dev ? to_pci_dev(dev->hw_dev) : NULL;
+}
+
 int comedi_buf_put(struct comedi_async *async, short x);
 int comedi_buf_get(struct comedi_async *async, short *x);
 
@@ -524,17 +512,4 @@ int comedi_usb_auto_config(struct usb_interface *intf,
 			   struct comedi_driver *driver);
 void comedi_usb_auto_unconfig(struct usb_interface *intf);
 
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-#define CONFIG_COMEDI_PCI
-#endif
-#ifdef CONFIG_COMEDI_PCI_DRIVERS_MODULE
-#define CONFIG_COMEDI_PCI
-#endif
-#ifdef CONFIG_COMEDI_PCMCIA_DRIVERS
-#define CONFIG_COMEDI_PCMCIA
-#endif
-#ifdef CONFIG_COMEDI_PCMCIA_DRIVERS_MODULE
-#define CONFIG_COMEDI_PCMCIA
-#endif
-
 #endif /* _COMEDIDEV_H */
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index aeac1caba3f..c0fdb00783e 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -24,7 +24,6 @@
 #define _GNU_SOURCE
 
 #define __NO_VERSION__
-#include "comedi_fops.h"
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/pci.h>
@@ -45,7 +44,7 @@
 #include <linux/io.h>
 
 #include "comedidev.h"
-#include "internal.h"
+#include "comedi_internal.h"
 
 static int postconfig(struct comedi_device *dev);
 static int insn_rw_emulate_bits(struct comedi_device *dev,
@@ -57,6 +56,31 @@ static int poll_invalid(struct comedi_device *dev, struct comedi_subdevice *s);
 
 struct comedi_driver *comedi_drivers;
 
+int comedi_alloc_subdevices(struct comedi_device *dev, int num_subdevices)
+{
+	struct comedi_subdevice *s;
+	int i;
+
+	if (num_subdevices < 1)
+		return -EINVAL;
+
+	s = kcalloc(num_subdevices, sizeof(*s), GFP_KERNEL);
+	if (!s)
+		return -ENOMEM;
+	dev->subdevices = s;
+	dev->n_subdevices = num_subdevices;
+
+	for (i = 0; i < num_subdevices; ++i) {
+		s = dev->subdevices + i;
+		s->device = dev;
+		s->async_dma_dir = DMA_NONE;
+		spin_lock_init(&s->spin_lock);
+		s->minor = -1;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(comedi_alloc_subdevices);
+
 static void cleanup_device(struct comedi_device *dev)
 {
 	int i;
@@ -301,18 +325,41 @@ static int postconfig(struct comedi_device *dev)
 	return 0;
 }
 
-/* generic recognize function for drivers
- * that register their supported board names */
+/*
+ * Generic recognize function for drivers that register their supported
+ * board names.
+ *
+ * 'driv->board_name' points to a 'const char *' member within the
+ * zeroth element of an array of some private board information
+ * structure, say 'struct foo_board' containing a member 'const char
+ * *board_name' that is initialized to point to a board name string that
+ * is one of the candidates matched against this function's 'name'
+ * parameter.
+ *
+ * 'driv->offset' is the size of the private board information
+ * structure, say 'sizeof(struct foo_board)', and 'driv->num_names' is
+ * the length of the array of private board information structures.
+ *
+ * If one of the board names in the array of private board information
+ * structures matches the name supplied to this function, the function
+ * returns a pointer to the pointer to the board name, otherwise it
+ * returns NULL.  The return value ends up in the 'board_ptr' member of
+ * a 'struct comedi_device' that the low-level comedi driver's
+ * 'attach()' hook can convert to a point to a particular element of its
+ * array of private board information structures by subtracting the
+ * offset of the member that points to the board name.  (No subtraction
+ * is required if the board name pointer is the first member of the
+ * private board information structure, which is generally the case.)
+ */
 static void *comedi_recognize(struct comedi_driver *driv, const char *name)
 {
-	unsigned i;
-	const char *const *name_ptr = driv->board_name;
+	char **name_ptr = (char **)driv->board_name;
+	int i;
+
 	for (i = 0; i < driv->num_names; i++) {
 		if (strcmp(*name_ptr, name) == 0)
-			return (void *)name_ptr;
-		name_ptr =
-		    (const char *const *)((const char *)name_ptr +
-					  driv->offset);
+			return name_ptr;
+		name_ptr = (void *)name_ptr + driv->offset;
 	}
 
 	return NULL;
@@ -385,39 +432,6 @@ static int insn_rw_emulate_bits(struct comedi_device *dev,
 	return 1;
 }
 
-static inline unsigned long uvirt_to_kva(pgd_t *pgd, unsigned long adr)
-{
-	unsigned long ret = 0UL;
-	pmd_t *pmd;
-	pte_t *ptep, pte;
-	pud_t *pud;
-
-	if (!pgd_none(*pgd)) {
-		pud = pud_offset(pgd, adr);
-		pmd = pmd_offset(pud, adr);
-		if (!pmd_none(*pmd)) {
-			ptep = pte_offset_kernel(pmd, adr);
-			pte = *ptep;
-			if (pte_present(pte)) {
-				ret = (unsigned long)
-				    page_address(pte_page(pte));
-				ret |= (adr & (PAGE_SIZE - 1));
-			}
-		}
-	}
-	return ret;
-}
-
-static inline unsigned long kvirt_to_kva(unsigned long adr)
-{
-	unsigned long va, kva;
-
-	va = adr;
-	kva = uvirt_to_kva(pgd_offset_k(va), va);
-
-	return kva;
-}
-
 int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 		     unsigned long new_size)
 {
@@ -909,6 +923,40 @@ static void comedi_auto_unconfig(struct device *hardware_device)
 	comedi_free_board_minor(minor);
 }
 
+/**
+ * comedi_pci_enable() - Enable the PCI device and request the regions.
+ * @pdev: pci_dev struct
+ * @res_name: name for the requested reqource
+ */
+int comedi_pci_enable(struct pci_dev *pdev, const char *res_name)
+{
+	int rc;
+
+	rc = pci_enable_device(pdev);
+	if (rc < 0)
+		return rc;
+
+	rc = pci_request_regions(pdev, res_name);
+	if (rc < 0)
+		pci_disable_device(pdev);
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(comedi_pci_enable);
+
+/**
+ * comedi_pci_disable() - Release the regions and disable the PCI device.
+ * @pdev: pci_dev struct
+ *
+ * This must be matched with a previous successful call to comedi_pci_enable().
+ */
+void comedi_pci_disable(struct pci_dev *pdev)
+{
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+EXPORT_SYMBOL_GPL(comedi_pci_disable);
+
 static int comedi_old_pci_auto_config(struct pci_dev *pcidev,
 				      struct comedi_driver *driver)
 {
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 27e39e4eb6b..4c9977b8a5a 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -60,15 +60,15 @@ I/O port base address can be found in the output of 'lspci -v'.
    set up the subdevice in the attach function of the driver by
    calling:
 
-     subdev_8255_init(device, subdevice, callback_function, arg)
+     subdev_8255_init(device, subdevice, io_function, iobase)
 
    device and subdevice are pointers to the device and subdevice
-   structures.  callback_function will be called to provide the
+   structures.  io_function will be called to provide the
    low-level input/output to the device, i.e., actual register
-   access.  callback_function will be called with the value of arg
+   access.  io_function will be called with the value of iobase
    as the last parameter.  If the 8255 device is mapped as 4
-   consecutive I/O ports, you can use NULL for callback_function
-   and the I/O port base for arg, and an internal function will
+   consecutive I/O ports, you can use NULL for io_function
+   and the I/O port base for iobase, and an internal function will
    handle the register access.
 
    In addition, if the main driver handles interrupts, you can
@@ -84,10 +84,10 @@ I/O port base address can be found in the output of 'lspci -v'.
 #include <linux/slab.h>
 #include "8255.h"
 
-#define _8255_SIZE 4
+#define _8255_SIZE	4
 
-#define _8255_DATA 0
-#define _8255_CR 3
+#define _8255_DATA	0
+#define _8255_CR	3
 
 #define CR_C_LO_IO	0x01
 #define CR_B_IO		0x02
@@ -97,23 +97,30 @@ I/O port base address can be found in the output of 'lspci -v'.
 #define CR_A_MODE(a)	((a)<<5)
 #define CR_CW		0x80
 
-struct subdev_8255_struct {
-	unsigned long cb_arg;
-	int (*cb_func) (int, int, int, unsigned long);
-	int have_irq;
+struct subdev_8255_private {
+	unsigned long iobase;
+	int (*io) (int, int, int, unsigned long);
 };
 
-#define CALLBACK_ARG	(((struct subdev_8255_struct *)s->private)->cb_arg)
-#define CALLBACK_FUNC	(((struct subdev_8255_struct *)s->private)->cb_func)
-#define subdevpriv	((struct subdev_8255_struct *)s->private)
+static int subdev_8255_io(int dir, int port, int data, unsigned long iobase)
+{
+	if (dir) {
+		outb(data, iobase + port);
+		return 0;
+	} else {
+		return inb(iobase + port);
+	}
+}
 
 void subdev_8255_interrupt(struct comedi_device *dev,
 			   struct comedi_subdevice *s)
 {
+	struct subdev_8255_private *spriv = s->private;
+	unsigned long iobase = spriv->iobase;
 	short d;
 
-	d = CALLBACK_FUNC(0, _8255_DATA, 0, CALLBACK_ARG);
-	d |= (CALLBACK_FUNC(0, _8255_DATA + 1, 0, CALLBACK_ARG) << 8);
+	d = spriv->io(0, _8255_DATA, 0, iobase);
+	d |= (spriv->io(0, _8255_DATA + 1, 0, iobase) << 8);
 
 	comedi_buf_put(s->async, d);
 	s->async->events |= COMEDI_CB_EOS;
@@ -122,46 +129,48 @@ void subdev_8255_interrupt(struct comedi_device *dev,
 }
 EXPORT_SYMBOL(subdev_8255_interrupt);
 
-static int subdev_8255_cb(int dir, int port, int data, unsigned long arg)
-{
-	unsigned long iobase = arg;
-
-	if (dir) {
-		outb(data, iobase + port);
-		return 0;
-	} else {
-		return inb(iobase + port);
-	}
-}
-
 static int subdev_8255_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= (data[0] & data[1]);
-
-		if (data[0] & 0xff)
-			CALLBACK_FUNC(1, _8255_DATA, s->state & 0xff,
-				      CALLBACK_ARG);
-		if (data[0] & 0xff00)
-			CALLBACK_FUNC(1, _8255_DATA + 1, (s->state >> 8) & 0xff,
-				      CALLBACK_ARG);
-		if (data[0] & 0xff0000)
-			CALLBACK_FUNC(1, _8255_DATA + 2,
-				      (s->state >> 16) & 0xff, CALLBACK_ARG);
+	struct subdev_8255_private *spriv = s->private;
+	unsigned long iobase = spriv->iobase;
+	unsigned int mask;
+	unsigned int bits;
+	unsigned int v;
+
+	mask = data[0];
+	bits = data[1];
+
+	if (mask) {
+		v = s->state;
+		v &= ~mask;
+		v |= (bits & mask);
+
+		if (mask & 0xff)
+			spriv->io(1, _8255_DATA, v & 0xff, iobase);
+		if (mask & 0xff00)
+			spriv->io(1, _8255_DATA + 1, (v >> 8) & 0xff, iobase);
+		if (mask & 0xff0000)
+			spriv->io(1, _8255_DATA + 2, (v >> 16) & 0xff, iobase);
+
+		s->state = v;
 	}
 
-	data[1] = CALLBACK_FUNC(0, _8255_DATA, 0, CALLBACK_ARG);
-	data[1] |= (CALLBACK_FUNC(0, _8255_DATA + 1, 0, CALLBACK_ARG) << 8);
-	data[1] |= (CALLBACK_FUNC(0, _8255_DATA + 2, 0, CALLBACK_ARG) << 16);
+	v = spriv->io(0, _8255_DATA, 0, iobase);
+	v |= (spriv->io(0, _8255_DATA + 1, 0, iobase) << 8);
+	v |= (spriv->io(0, _8255_DATA + 2, 0, iobase) << 16);
+
+	data[1] = v;
 
-	return 2;
+	return insn->n;
 }
 
-static void do_config(struct comedi_device *dev, struct comedi_subdevice *s)
+static void subdev_8255_do_config(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
 {
+	struct subdev_8255_private *spriv = s->private;
+	unsigned long iobase = spriv->iobase;
 	int config;
 
 	config = CR_CW;
@@ -174,7 +183,8 @@ static void do_config(struct comedi_device *dev, struct comedi_subdevice *s)
 		config |= CR_C_LO_IO;
 	if (!(s->io_bits & 0xf00000))
 		config |= CR_C_HI_IO;
-	CALLBACK_FUNC(1, _8255_CR, config, CALLBACK_ARG);
+
+	spriv->io(1, _8255_CR, config, iobase);
 }
 
 static int subdev_8255_insn_config(struct comedi_device *dev,
@@ -209,7 +219,7 @@ static int subdev_8255_insn_config(struct comedi_device *dev,
 		return -EINVAL;
 	}
 
-	do_config(dev, s);
+	subdev_8255_do_config(dev, s);
 
 	return 1;
 }
@@ -307,50 +317,50 @@ static int subdev_8255_cancel(struct comedi_device *dev,
 }
 
 int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*cb) (int, int, int, unsigned long),
-		     unsigned long arg)
+		     int (*io) (int, int, int, unsigned long),
+		     unsigned long iobase)
 {
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 24;
-	s->range_table = &range_digital;
-	s->maxdata = 1;
-
-	s->private = kmalloc(sizeof(struct subdev_8255_struct), GFP_KERNEL);
-	if (!s->private)
+	struct subdev_8255_private *spriv;
+
+	spriv = kzalloc(sizeof(*spriv), GFP_KERNEL);
+	if (!spriv)
 		return -ENOMEM;
 
-	CALLBACK_ARG = arg;
-	if (cb == NULL)
-		CALLBACK_FUNC = subdev_8255_cb;
-	else
-		CALLBACK_FUNC = cb;
-	s->insn_bits = subdev_8255_insn;
-	s->insn_config = subdev_8255_insn_config;
+	spriv->iobase	= iobase;
+	spriv->io	= io ? io : subdev_8255_io;
+
+	s->private	= spriv;
+
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 24;
+	s->range_table	= &range_digital;
+	s->maxdata	= 1;
+	s->insn_bits	= subdev_8255_insn;
+	s->insn_config	= subdev_8255_insn_config;
 
-	s->state = 0;
-	s->io_bits = 0;
-	do_config(dev, s);
+	s->state	= 0;
+	s->io_bits	= 0;
+
+	subdev_8255_do_config(dev, s);
 
 	return 0;
 }
 EXPORT_SYMBOL(subdev_8255_init);
 
 int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
-			 int (*cb) (int, int, int, unsigned long),
-			 unsigned long arg)
+			 int (*io) (int, int, int, unsigned long),
+			 unsigned long iobase)
 {
 	int ret;
 
-	ret = subdev_8255_init(dev, s, cb, arg);
-	if (ret < 0)
+	ret = subdev_8255_init(dev, s, io, iobase);
+	if (ret)
 		return ret;
 
-	s->do_cmdtest = subdev_8255_cmdtest;
-	s->do_cmd = subdev_8255_cmd;
-	s->cancel = subdev_8255_cancel;
-
-	subdevpriv->have_irq = 1;
+	s->do_cmdtest	= subdev_8255_cmdtest;
+	s->do_cmd	= subdev_8255_cmd;
+	s->cancel	= subdev_8255_cancel;
 
 	return 0;
 }
@@ -371,6 +381,7 @@ EXPORT_SYMBOL(subdev_8255_cleanup);
 static int dev_8255_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
+	struct comedi_subdevice *s;
 	int ret;
 	unsigned long iobase;
 	int i;
@@ -383,51 +394,45 @@ static int dev_8255_attach(struct comedi_device *dev,
 			break;
 	}
 	if (i == 0) {
-		printk(KERN_WARNING
-		       "comedi%d: 8255: no devices specified\n", dev->minor);
+		dev_warn(dev->class_dev, "no devices specified\n");
 		return -EINVAL;
 	}
 
-	ret = alloc_subdevices(dev, i);
-	if (ret < 0) {
-		/* FIXME this printk call should give a proper message, the
-		 * below line just maintains previous functionality */
-		printk("comedi%d: 8255:", dev->minor);
+	ret = comedi_alloc_subdevices(dev, i);
+	if (ret)
 		return ret;
-	}
-
-	printk(KERN_INFO "comedi%d: 8255:", dev->minor);
 
 	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
 		iobase = it->options[i];
 
-		printk(" 0x%04lx", iobase);
 		if (!request_region(iobase, _8255_SIZE, "8255")) {
-			printk(" (I/O port conflict)");
+			dev_warn(dev->class_dev,
+				"0x%04lx (I/O port conflict)\n", iobase);
 
-			dev->subdevices[i].type = COMEDI_SUBD_UNUSED;
+			s->type = COMEDI_SUBD_UNUSED;
 		} else {
-			subdev_8255_init(dev, dev->subdevices + i, NULL,
-					 iobase);
+			ret = subdev_8255_init(dev, s, NULL, iobase);
+			if (ret)
+				return ret;
+			dev_info(dev->class_dev, "0x%04lx\n", iobase);
 		}
 	}
 
-	printk("\n");
-
 	return 0;
 }
 
 static void dev_8255_detach(struct comedi_device *dev)
 {
-	int i;
-	unsigned long iobase;
 	struct comedi_subdevice *s;
+	struct subdev_8255_private *spriv;
+	int i;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
 		s = dev->subdevices + i;
 		if (s->type != COMEDI_SUBD_UNUSED) {
-			iobase = CALLBACK_ARG;
-			release_region(iobase, _8255_SIZE);
+			spriv = s->private;
+			release_region(spriv->iobase, _8255_SIZE);
 		}
 		subdev_8255_cleanup(dev, s);
 	}
diff --git a/drivers/staging/comedi/drivers/8255.h b/drivers/staging/comedi/drivers/8255.h
index b6314c9b7ea..1e589b4b8b7 100644
--- a/drivers/staging/comedi/drivers/8255.h
+++ b/drivers/staging/comedi/drivers/8255.h
@@ -27,11 +27,11 @@
 #include "../comedidev.h"
 
 int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*cb) (int, int, int, unsigned long),
-		     unsigned long arg);
+		     int (*io) (int, int, int, unsigned long),
+		     unsigned long iobase);
 int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
-			 int (*cb) (int, int, int, unsigned long),
-			 unsigned long arg);
+			 int (*io) (int, int, int, unsigned long),
+			 unsigned long iobase);
 void subdev_8255_cleanup(struct comedi_device *dev, struct comedi_subdevice *s);
 void subdev_8255_interrupt(struct comedi_device *dev,
 			   struct comedi_subdevice *s);
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 170da609195..57b19e44d86 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -55,7 +55,6 @@ obj-$(CONFIG_COMEDI_MULTIQ3)		+= multiq3.o
 obj-$(CONFIG_COMEDI_POC)		+= poc.o
 
 # Comedi PCI drivers
-obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= 8255.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_035)	+= addi_apci_035.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1032)	+= addi_apci_1032.o
 obj-$(CONFIG_COMEDI_ADDI_APCI_1500)	+= addi_apci_1500.o
@@ -132,7 +131,7 @@ obj-$(CONFIG_COMEDI_VMK80XX)		+= vmk80xx.o
 # Comedi NI drivers
 obj-$(CONFIG_COMEDI_MITE)		+= mite.o
 obj-$(CONFIG_COMEDI_NI_TIO)		+= ni_tio.o
-obj-$(CONFIG_COMEDI_NI_TIO)		+= ni_tiocmd.o
+obj-$(CONFIG_COMEDI_NI_TIOCMD)		+= ni_tiocmd.o
 obj-$(CONFIG_COMEDI_NI_LABPC)		+= ni_labpc.o
 
 obj-$(CONFIG_COMEDI_8255)		+= 8255.o
diff --git a/drivers/staging/comedi/drivers/acl7225b.c b/drivers/staging/comedi/drivers/acl7225b.c
index 4e4fc418655..ddba5db1e2e 100644
--- a/drivers/staging/comedi/drivers/acl7225b.c
+++ b/drivers/staging/comedi/drivers/acl7225b.c
@@ -27,15 +27,10 @@ struct boardtype {
 	int io_range;		/*  len of I/O space */
 };
 
-#define this_board ((const struct boardtype *)dev->board_ptr)
-
 static int acl7225b_do_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -48,43 +43,43 @@ static int acl7225b_do_insn(struct comedi_device *dev,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int acl7225b_di_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + (unsigned long)s->private) |
 	    (inb(dev->iobase + (unsigned long)s->private + 1) << 8);
 
-	return 2;
+	return insn->n;
 }
 
 static int acl7225b_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
+	const struct boardtype *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int iobase, iorange;
+	int ret;
 
 	iobase = it->options[0];
-	iorange = this_board->io_range;
+	iorange = board->io_range;
 	printk(KERN_INFO "comedi%d: acl7225b: board=%s 0x%04x\n", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 	if (!request_region(iobase, iorange, "acl7225b")) {
 		printk(KERN_ERR "comedi%d: request_region failed - I/O port conflict\n",
 			dev->minor);
 		return -EIO;
 	}
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 	dev->iobase = iobase;
 	dev->irq = 0;
 
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* Relays outputs */
@@ -121,8 +116,10 @@ static int acl7225b_attach(struct comedi_device *dev,
 
 static void acl7225b_detach(struct comedi_device *dev)
 {
+	const struct boardtype *board = comedi_board(dev);
+
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
+		release_region(dev->iobase, board->io_range);
 }
 
 static const struct boardtype boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_amcc_s5933.h b/drivers/staging/comedi/drivers/addi-data/addi_amcc_s5933.h
index 35a3ea19359..95f7dc61cd0 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_amcc_s5933.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_amcc_s5933.h
@@ -22,12 +22,6 @@
 
 #include "../../comedidev.h"
 
-#include "../comedi_pci.h"
-
-#ifdef PCI_SUPPORT_VER1
-#error     No support for 2.1.55 and older
-#endif
-
 /* written on base0 */
 #define FIFO_ADVANCE_ON_BYTE_2	0x20000000
 
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index 44aaf8351ba..a3d4ed25fb0 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -613,7 +613,6 @@ static const struct addi_board boardtypes[] = {
 		.i_IorangeBase0		= 128,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.i_NbrTTLChannel	= 48,
-		.pr_TTLRangelist	= &range_apci16xx_ttl,
 		.reset			= i_APCI16XX_Reset,
 		.ttl_config		= i_APCI16XX_InsnConfigInitTTLIO,
 		.ttl_bits		= i_APCI16XX_InsnBitsReadTTLIO,
@@ -626,7 +625,6 @@ static const struct addi_board boardtypes[] = {
 		.i_IorangeBase0		= 128,
 		.i_PCIEeprom		= ADDIDATA_NO_EEPROM,
 		.i_NbrTTLChannel	= 96,
-		.pr_TTLRangelist	= &range_apci16xx_ttl,
 		.reset			= i_APCI16XX_Reset,
 		.ttl_config		= i_APCI16XX_InsnConfigInitTTLIO,
 		.ttl_bits		= i_APCI16XX_InsnBitsReadTTLIO,
@@ -651,7 +649,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AiMaxdata		= 4095,
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -678,7 +675,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AiMaxdata		= 4095,
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -705,7 +701,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AiMaxdata		= 4095,
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -732,7 +727,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AiMaxdata		= 65535,
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -759,7 +753,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AiMaxdata		= 65535,
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -786,7 +779,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AiMaxdata		= 65535,
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -816,7 +808,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -851,7 +842,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -886,7 +876,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -921,7 +910,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -956,7 +944,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -991,7 +978,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1026,7 +1012,6 @@ static const struct addi_board boardtypes[] = {
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.pr_AoRangelist		= &range_apci3XXX_ao,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1057,7 +1042,6 @@ static const struct addi_board boardtypes[] = {
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.pr_AoRangelist		= &range_apci3XXX_ao,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1088,7 +1072,6 @@ static const struct addi_board boardtypes[] = {
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.pr_AoRangelist		= &range_apci3XXX_ao,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1119,7 +1102,6 @@ static const struct addi_board boardtypes[] = {
 		.pr_AiRangelist		= &range_apci3XXX_ai,
 		.pr_AoRangelist		= &range_apci3XXX_ao,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 10000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1153,7 +1135,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1192,7 +1173,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1231,7 +1211,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1270,7 +1249,6 @@ static const struct addi_board boardtypes[] = {
 		.i_NbrDoChannel		= 4,
 		.i_DoMaxdata		= 1,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.b_AvailableConvertUnit	= 6,
 		.ui_MinAcquisitiontimeNs = 5000,
 		.interrupt		= v_APCI3XXX_Interrupt,
@@ -1413,7 +1391,6 @@ static const struct addi_board boardtypes[] = {
 		.i_AoMaxdata		= 4095,
 		.pr_AoRangelist		= &range_apci3XXX_ao,
 		.i_NbrTTLChannel	= 24,
-		.pr_TTLRangelist	= &range_apci3XXX_ttl,
 		.interrupt		= v_APCI3XXX_Interrupt,
 		.reset			= i_APCI3XXX_Reset,
 		.ao_write		= i_APCI3XXX_InsnWriteAnalogOutput,
@@ -1686,10 +1663,9 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		devpriv->s_BoardInfos.ui_Address = io_addr[2];
 #endif
 	} else {
-		/* Update-0.7.57->0.7.68dev->n_subdevices = 7; */
 		n_subdevices = 7;
-		ret = alloc_subdevices(dev, n_subdevices);
-		if (ret < 0)
+		ret = comedi_alloc_subdevices(dev, n_subdevices);
+		if (ret)
 			return ret;
 
 		/*  Allocate and Initialise AI Subdevice Structures */
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
index 2c3f34703dd..b7bbb7164f5 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.h
@@ -85,7 +85,6 @@ struct addi_board {
 	int i_DoMaxdata;	/*  data to set all channels high */
 
 	int i_NbrTTLChannel;	/*  Number of TTL channels */
-	const struct comedi_lrange *pr_TTLRangelist;	/* rangelist for TTL */
 
 	int i_Dma;		/*  dma present or not */
 	int i_Timer;		/*    timer subdevice present or not */
diff --git a/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h b/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h
index 349e93c23e9..c26c28c31b9 100644
--- a/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h
+++ b/drivers/staging/comedi/drivers/addi-data/amcc_s5933_58.h
@@ -23,10 +23,6 @@
 #include <linux/pci.h>
 #include "../../comedidev.h"
 
-#ifdef PCI_SUPPORT_VER1
-#error    Sorry, no support for 2.1.55 and older! :-((((
-#endif
-
 /***********Added by sarath for compatibility with APCI3120
 
 *************************/
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c
index a76ed2553fb..595238feaf4 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_APCI1710.c
@@ -62,9 +62,8 @@ void i_ADDI_AttachPCI1710(struct comedi_device *dev)
 	int ret = 0;
 	int n_subdevices = 9;
 
-	/* Update-0.7.57->0.7.68dev->n_subdevices = 9; */
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return;
 
 	/*  Allocate and Initialise Timer Subdevice Structures */
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci16xx.h b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci16xx.h
index 5bf91e13a03..a12df4bc88a 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci16xx.h
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci16xx.h
@@ -41,21 +41,6 @@
 
 #ifdef __KERNEL__
 
-static const struct comedi_lrange range_apci16xx_ttl = { 12,
-	{BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1),
-	 BIP_RANGE(1)}
-};
-
 /*
 +----------------------------------------------------------------------------+
 |                       TTL INISIALISATION FUNCTION                          |
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3xxx.h b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3xxx.h
index cce9e12e820..e10b7e51033 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3xxx.h
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3xxx.h
@@ -42,20 +42,6 @@ static const struct comedi_lrange range_apci3XXX_ai = { 8, {BIP_RANGE(10),
 						     UNI_RANGE(1)}
 };
 
-static const struct comedi_lrange range_apci3XXX_ttl = { 12, {BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1),
-						       BIP_RANGE(1)}
-};
-
 static const struct comedi_lrange range_apci3XXX_ao = { 2, {BIP_RANGE(10),
 						     UNI_RANGE(10)}
 };
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index de8c68af321..3bec0f6e4a8 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -41,301 +41,230 @@ References:
 	- adl_pci9111.c		copied the entire pci setup section
 	- adl_pci9118.c
 */
+
+#include "../comedidev.h"
+
 /*
- * These headers should be followed by a blank line, and any comments
- * you wish to say about the driver.  The comment area is the place
- * to put any known bugs, limitations, unsupported features, supported
- * command triggers, whether or not commands are supported on particular
- * subdevices, etc.
- *
- * Somewhere in the comment should be information about configuration
- * options that are used with comedi_config.
+ * PCI-6208/6216-GL register map
  */
-#include "../comedidev.h"
-#include "comedi_pci.h"
+#define PCI6208_AO_CONTROL(x)		(0x00 + (2 * (x)))
+#define PCI6208_AO_STATUS		0x00
+#define PCI6208_AO_STATUS_DATA_SEND	(1 << 0)
+#define PCI6208_DIO			0x40
+#define PCI6208_DIO_DO_MASK		(0x0f)
+#define PCI6208_DIO_DO_SHIFT		(0)
+#define PCI6208_DIO_DI_MASK		(0xf0)
+#define PCI6208_DIO_DI_SHIFT		(4)
+
+#define PCI6208_MAX_AO_CHANNELS		8
 
-/* Board descriptions */
 struct pci6208_board {
 	const char *name;
-	unsigned short dev_id;	/* `lspci` will show you this */
+	unsigned short dev_id;
 	int ao_chans;
-	/* int ao_bits; */
 };
 
 static const struct pci6208_board pci6208_boards[] = {
-	/*{
-	   .name = "pci6208v",
-	   .dev_id = 0x6208,      // not sure
-	   .ao_chans = 8
-	   // , .ao_bits = 16
-	   },
-	   {
-	   .name = "pci6216v",
-	   .dev_id = 0x6208,      // not sure
-	   .ao_chans = 16
-	   // , .ao_bits = 16
-	   }, */
 	{
-	 .name = "pci6208a",
-	 .dev_id = 0x6208,
-	 .ao_chans = 8
-	 /* ,    .ao_bits = 16 */
-	 }
+		.name		= "pci6208a",
+		.dev_id		= 0x6208,
+		.ao_chans	= 8,
+	},
 };
 
-/* Will be initialized in pci6208_find device(). */
-#define thisboard ((const struct pci6208_board *)dev->board_ptr)
-
 struct pci6208_private {
-	int data;
-	struct pci_dev *pci_dev;	/* for a PCI device */
-	unsigned int ao_readback[2];	/* Used for AO readback */
+	unsigned int ao_readback[PCI6208_MAX_AO_CHANNELS];
 };
 
-#define devpriv ((struct pci6208_private *)dev->private)
-
 static int pci6208_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	int i = 0, Data_Read;
-	unsigned short chan = CR_CHAN(insn->chanspec);
+	struct pci6208_private *devpriv = dev->private;
+	int chan = CR_CHAN(insn->chanspec);
 	unsigned long invert = 1 << (16 - 1);
-	unsigned long out_value;
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
+	unsigned long value = 0;
+	unsigned short status;
+	int i;
+
 	for (i = 0; i < insn->n; i++) {
-		out_value = data[i] ^ invert;
-		/* a typical programming sequence */
+		value = data[i] ^ invert;
+
 		do {
-			Data_Read = (inw(dev->iobase) & 1);
-		} while (Data_Read);
-		outw(out_value, dev->iobase + (0x02 * chan));
-		devpriv->ao_readback[chan] = out_value;
+			status = inw(dev->iobase + PCI6208_AO_STATUS);
+		} while (status & PCI6208_AO_STATUS_DATA_SEND);
+
+		outw(value, dev->iobase + PCI6208_AO_CONTROL(chan));
 	}
+	devpriv->ao_readback[chan] = value;
 
-	/* return the number of samples read/written */
-	return i;
+	return insn->n;
 }
 
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
 static int pci6208_ao_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	int i;
+	struct pci6208_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
+	int i;
 
 	for (i = 0; i < insn->n; i++)
 		data[i] = devpriv->ao_readback[chan];
 
-	return i;
+	return insn->n;
 }
 
-/* DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The
- * comedi core can convert between insn_bits and insn_read/write */
-/* static int pci6208_dio_insn_bits(struct comedi_device *dev,
- *					struct comedi_subdevice *s, */
-/* struct comedi_insn *insn,unsigned int *data) */
-/* { */
-/* if(insn->n!=2)return -EINVAL; */
-
-	/* The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit. */
-/* if(data[0]){ */
-/* s->state &= ~data[0]; */
-/* s->state |= data[0]&data[1]; */
-		/* Write out the new digital output lines */
-		/* outw(s->state,dev->iobase + SKEL_DIO); */
-/* } */
-
-	/* on return, data[1] contains the value of the digital
-	 * input and output lines. */
-	/* data[1]=inw(dev->iobase + SKEL_DIO); */
-	/* or we could just return the software copy of the output values if
-	 * it was a purely digital output subdevice */
-	/* data[1]=s->state; */
-
-/* return 2; */
-/* } */
-
-/* static int pci6208_dio_insn_config(struct comedi_device *dev,
- *					struct comedi_subdevice *s, */
-/* struct comedi_insn *insn,unsigned int *data) */
-/* { */
-/* int chan=CR_CHAN(insn->chanspec); */
-
-	/* The input or output configuration of each digital line is
-	 * configured by a special insn_config instruction.  chanspec
-	 * contains the channel to be changed, and data[0] contains the
-	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
-
-/* if(data[0]==COMEDI_OUTPUT){ */
-/* s->io_bits |= 1<<chan; */
-/* }else{ */
-/* s->io_bits &= ~(1<<chan); */
-/* } */
-	/* outw(s->io_bits,dev->iobase + SKEL_DIO_CONFIG); */
-
-/* return 1; */
-/* } */
-
-static int pci6208_find_device(struct comedi_device *dev, int bus, int slot)
+static int pci6208_dio_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	struct pci_dev *pci_dev = NULL;
-	int i;
+	unsigned int mask = data[0] & PCI6208_DIO_DO_MASK;
+	unsigned int bits = data[1];
 
-	for_each_pci_dev(pci_dev) {
-		if (pci_dev->vendor == PCI_VENDOR_ID_ADLINK) {
-			for (i = 0; i < ARRAY_SIZE(pci6208_boards); i++) {
-				if (pci6208_boards[i].dev_id ==
-					pci_dev->device) {
-					/*
-					 * was a particular bus/slot requested?
-					*/
-					if ((bus != 0) || (slot != 0)) {
-						/*
-						 * are we on the
-						 * wrong bus/slot?
-						*/
-						if (pci_dev->bus->number
-						    != bus ||
-						    PCI_SLOT(pci_dev->devfn)
-						    != slot) {
-							continue;
-						}
-					}
-					dev->board_ptr = pci6208_boards + i;
-					goto found;
-				}
-			}
-		}
-	}
-
-	printk(KERN_ERR "comedi%d: no supported board found! "
-			"(req. bus/slot : %d/%d)\n",
-			dev->minor, bus, slot);
-	return -EIO;
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= bits & mask;
 
-found:
-	printk("comedi%d: found %s (b:s:f=%d:%d:%d) , irq=%d\n",
-	       dev->minor,
-	       pci6208_boards[i].name,
-	       pci_dev->bus->number,
-	       PCI_SLOT(pci_dev->devfn),
-	       PCI_FUNC(pci_dev->devfn), pci_dev->irq);
-
-	/*  TODO: Warn about non-tested boards. */
-	/* switch(board->device_id) */
-	/* { */
-	/* }; */
+		outw(s->state, dev->iobase + PCI6208_DIO);
+	}
 
-	devpriv->pci_dev = pci_dev;
+	s->state = inw(dev->iobase + PCI6208_DIO);
+	data[1] = s->state;
 
-	return 0;
+	return insn->n;
 }
 
-static int
-pci6208_pci_setup(struct pci_dev *pci_dev, unsigned long *io_base_ptr,
-		  int dev_minor)
+static int pci6208_dio_insn_config(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
-	unsigned long io_base, io_range, lcr_io_base, lcr_io_range;
-
-	/*  Enable PCI device and request regions */
-	if (comedi_pci_enable(pci_dev, "adl_pci6208") < 0) {
-		printk(KERN_ERR "comedi%d: Failed to enable PCI device "
-			"and request regions\n",
-			dev_minor);
-		return -EIO;
+	int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask = 1 << chan;
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] = (s->io_bits & mask) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		break;
+	default:
+		return -EINVAL;
 	}
-	/* Read local configuration register
-	 * base address [PCI_BASE_ADDRESS #1].
-	 */
-	lcr_io_base = pci_resource_start(pci_dev, 1);
-	lcr_io_range = pci_resource_len(pci_dev, 1);
-
-	printk(KERN_INFO "comedi%d: local config registers at address"
-			" 0x%4lx [0x%4lx]\n",
-			dev_minor, lcr_io_base, lcr_io_range);
-
-	/*  Read PCI6208 register base address [PCI_BASE_ADDRESS #2]. */
-	io_base = pci_resource_start(pci_dev, 2);
-	io_range = pci_resource_end(pci_dev, 2) - io_base + 1;
 
-	printk("comedi%d: 6208 registers at address 0x%4lx [0x%4lx]\n",
-	       dev_minor, io_base, io_range);
+	return insn->n;
+}
 
-	*io_base_ptr = io_base;
-	/* devpriv->io_range = io_range; */
-	/* devpriv->is_valid=0; */
-	/* devpriv->lcr_io_base=lcr_io_base; */
-	/* devpriv->lcr_io_range=lcr_io_range; */
+static struct pci_dev *pci6208_find_device(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
+{
+	const struct pci6208_board *thisboard;
+	struct pci_dev *pci_dev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
 
-	return 0;
+	for_each_pci_dev(pci_dev) {
+		if (pci_dev->vendor != PCI_VENDOR_ID_ADLINK)
+			continue;
+		for (i = 0; i < ARRAY_SIZE(pci6208_boards); i++) {
+			thisboard = &pci6208_boards[i];
+			if (thisboard->dev_id != pci_dev->device)
+				continue;
+			/* was a particular bus/slot requested? */
+			if (bus || slot) {
+				/* are we on the wrong bus/slot? */
+				if (pci_dev->bus->number != bus ||
+				    PCI_SLOT(pci_dev->devfn) != slot)
+					continue;
+			}
+			dev_dbg(dev->class_dev,
+				"Found %s on bus %d, slot, %d, irq=%d\n",
+				thisboard->name,
+				pci_dev->bus->number,
+				PCI_SLOT(pci_dev->devfn),
+				pci_dev->irq);
+			dev->board_ptr = thisboard;
+			return pci_dev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
 
 static int pci6208_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	const struct pci6208_board *thisboard;
+	struct pci6208_private *devpriv;
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	int retval;
-	unsigned long io_base;
+	int ret;
 
-	printk(KERN_INFO "comedi%d: pci6208: ", dev->minor);
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
 
-	retval = alloc_private(dev, sizeof(struct pci6208_private));
-	if (retval < 0)
-		return retval;
-
-	retval = pci6208_find_device(dev, it->options[0], it->options[1]);
-	if (retval < 0)
-		return retval;
-
-	retval = pci6208_pci_setup(devpriv->pci_dev, &io_base, dev->minor);
-	if (retval < 0)
-		return retval;
+	pcidev = pci6208_find_device(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	thisboard = comedi_board(dev);
 
-	dev->iobase = io_base;
 	dev->board_name = thisboard->name;
 
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	ret = comedi_pci_enable(pcidev, dev->driver->driver_name);
+	if (ret) {
+		dev_err(dev->class_dev,
+			"Failed to enable PCI device and request regions\n");
+		return ret;
+	}
+	dev->iobase = pci_resource_start(pcidev, 2);
+
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;	/* anything else to add here?? */
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = 0xffff;	/* 16-bit DAC */
-	s->range_table = &range_bipolar10;	/* this needs to be checked. */
-	s->insn_write = pci6208_ao_winsn;
-	s->insn_read = pci6208_ao_rinsn;
-
-	/* s=dev->subdevices+1; */
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= thisboard->ao_chans;
+	s->maxdata	= 0xffff;
+	s->range_table	= &range_bipolar10;
+	s->insn_write	= pci6208_ao_winsn;
+	s->insn_read	= pci6208_ao_rinsn;
+
+	s = dev->subdevices + 1;
 	/* digital i/o subdevice */
-	/* s->type=COMEDI_SUBD_DIO; */
-	/* s->subdev_flags=SDF_READABLE|SDF_WRITABLE; */
-	/* s->n_chan=16; */
-	/* s->maxdata=1; */
-	/* s->range_table=&range_digital; */
-	/* s->insn_bits = pci6208_dio_insn_bits; */
-	/* s->insn_config = pci6208_dio_insn_config; */
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 8;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci6208_dio_insn_bits;
+	s->insn_config	= pci6208_dio_insn_config;
+
+	s->io_bits	= 0x0f;
+	s->state	= inw(dev->iobase + PCI6208_DIO);
 
-	printk(KERN_INFO "attached\n");
+	dev_info(dev->class_dev, "%s: %s, I/O base=0x%04lx\n",
+		dev->driver->driver_name, dev->board_name, dev->iobase);
 
-	return 1;
+	return 0;
 }
 
 static void pci6208_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
@@ -357,11 +286,7 @@ static void __devexit adl_pci6208_pci_remove(struct pci_dev *dev)
 	comedi_pci_auto_unconfig(dev);
 }
 
-/* This is used by modprobe to translate PCI IDs to drivers.  Should
- * only be used for PCI and ISA-PnP devices */
 static DEFINE_PCI_DEVICE_TABLE(adl_pci6208_pci_table) = {
-	/* { PCI_VENDOR_ID_ADLINK, 0x6208, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, */
-	/* { PCI_VENDOR_ID_ADLINK, 0x6208, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }, */
 	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, 0x6208) },
 	{ 0 }
 };
diff --git a/drivers/staging/comedi/drivers/adl_pci7230.c b/drivers/staging/comedi/drivers/adl_pci7230.c
index e8053bc825f..7df4c960d5e 100644
--- a/drivers/staging/comedi/drivers/adl_pci7230.c
+++ b/drivers/staging/comedi/drivers/adl_pci7230.c
@@ -36,28 +36,17 @@ Configuration Options:
 
 #include "../comedidev.h"
 #include <linux/kernel.h>
-#include "comedi_pci.h"
 
 #define PCI7230_DI      0x00
 #define PCI7230_DO	    0x00
 
 #define PCI_DEVICE_ID_PCI7230 0x7230
 
-struct adl_pci7230_private {
-	int data;
-	struct pci_dev *pci_dev;
-};
-
-#define devpriv ((struct adl_pci7230_private *)dev->private)
-
 static int adl_pci7230_do_insn_bits(struct comedi_device *dev,
 	struct comedi_subdevice *s,
 	struct comedi_insn *insn,
 	unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -65,7 +54,7 @@ static int adl_pci7230_do_insn_bits(struct comedi_device *dev,
 		outl((s->state  << 16) & 0xffffffff, dev->iobase + PCI7230_DO);
 	}
 
-	return 2;
+	return insn->n;
 }
 
 static int adl_pci7230_di_insn_bits(struct comedi_device *dev,
@@ -73,52 +62,55 @@ static int adl_pci7230_di_insn_bits(struct comedi_device *dev,
 	struct comedi_insn *insn,
 	unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inl(dev->iobase + PCI7230_DI) & 0xffffffff;
 
-	return 2;
+	return insn->n;
 }
 
-static int adl_pci7230_attach(struct comedi_device *dev,
+static struct pci_dev *adl_pci7230_find_pci(struct comedi_device *dev,
 	struct comedi_devconfig *it)
 {
 	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
+		    pcidev->device != PCI_DEVICE_ID_PCI7230)
+			continue;
+		if (bus || slot) {
+			/* requested particular bus/slot */
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		return pcidev;
+	}
+	printk(KERN_ERR "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+		dev->minor, bus, slot);
+	return NULL;
+}
+
+static int adl_pci7230_attach(struct comedi_device *dev,
+	struct comedi_devconfig *it)
+{
 	struct comedi_subdevice *s;
-	int bus, slot;
+	struct pci_dev *pcidev;
+	int ret;
 
 	printk(KERN_INFO "comedi%d: adl_pci7230\n", dev->minor);
 
 	dev->board_name = "pci7230";
-	bus = it->options[0];
-	slot = it->options[1];
-
-	if (alloc_private(dev, sizeof(struct adl_pci7230_private)) < 0)
-		return -ENOMEM;
 
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_ADLINK &&
-			pcidev->device == PCI_DEVICE_ID_PCI7230) {
-			if (bus || slot) {
-				/* requested particular bus/slot */
-				if (pcidev->bus->number != bus ||
-					PCI_SLOT(pcidev->devfn) != slot) {
-					continue;
-				}
-			}
-			devpriv->pci_dev = pcidev;
-			break;
-		}
-	}
-	if (pcidev == NULL) {
-		printk(KERN_ERR "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-			dev->minor, bus, slot);
+	pcidev = adl_pci7230_find_pci(dev, it);
+	if (!pcidev)
 		return -EIO;
-	}
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
 	if (comedi_pci_enable(pcidev, "adl_pci7230") < 0) {
 		printk(KERN_ERR "comedi%d: Failed to enable PCI device and request regions\n",
 			dev->minor);
@@ -152,10 +144,12 @@ static int adl_pci7230_attach(struct comedi_device *dev,
 
 static void adl_pci7230_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/adl_pci7296.c b/drivers/staging/comedi/drivers/adl_pci7296.c
index b4dae3b7598..19b47af9c10 100644
--- a/drivers/staging/comedi/drivers/adl_pci7296.c
+++ b/drivers/staging/comedi/drivers/adl_pci7296.c
@@ -37,7 +37,6 @@ Configuration Options:
 #include "../comedidev.h"
 #include <linux/kernel.h>
 
-#include "comedi_pci.h"
 #include "8255.h"
 /* #include "8253.h" */
 
@@ -48,98 +47,96 @@ Configuration Options:
 
 #define PCI_DEVICE_ID_PCI7296 0x7296
 
-struct adl_pci7296_private {
-	int data;
-	struct pci_dev *pci_dev;
-};
+static struct pci_dev *adl_pci7296_find_pci(struct comedi_device *dev,
+					    struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 
-#define devpriv ((struct adl_pci7296_private *)dev->private)
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
+		    pcidev->device != PCI_DEVICE_ID_PCI7296)
+			continue;
+		if (bus || slot) {
+			/* requested particular bus/slot */
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		return pcidev;
+	}
+	printk(KERN_ERR
+		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+	       dev->minor, bus, slot);
+	return NULL;
+}
 
 static int adl_pci7296_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
-	struct pci_dev *pcidev = NULL;
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	int bus, slot;
 	int ret;
 
 	printk(KERN_INFO "comedi%d: attach adl_pci7432\n", dev->minor);
 
 	dev->board_name = "pci7432";
-	bus = it->options[0];
-	slot = it->options[1];
 
-	if (alloc_private(dev, sizeof(struct adl_pci7296_private)) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	pcidev = adl_pci7296_find_pci(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_ADLINK &&
-		    pcidev->device == PCI_DEVICE_ID_PCI7296) {
-			if (bus || slot) {
-				/* requested particular bus/slot */
-				if (pcidev->bus->number != bus
-				    || PCI_SLOT(pcidev->devfn) != slot) {
-					continue;
-				}
-			}
-			devpriv->pci_dev = pcidev;
-			if (comedi_pci_enable(pcidev, "adl_pci7296") < 0) {
-				printk(KERN_ERR "comedi%d: Failed to enable PCI device and request regions\n",
-				     dev->minor);
-				return -EIO;
-			}
-
-			dev->iobase = pci_resource_start(pcidev, 2);
-			printk(KERN_INFO "comedi: base addr %4lx\n",
-				dev->iobase);
-
-			/*  four 8255 digital io subdevices */
-			s = dev->subdevices + 0;
-			subdev_8255_init(dev, s, NULL,
-					 (unsigned long)(dev->iobase));
-
-			s = dev->subdevices + 1;
-			ret = subdev_8255_init(dev, s, NULL,
-					       (unsigned long)(dev->iobase +
-							       PORT2A));
-			if (ret < 0)
-				return ret;
-
-			s = dev->subdevices + 2;
-			ret = subdev_8255_init(dev, s, NULL,
-					       (unsigned long)(dev->iobase +
-							       PORT3A));
-			if (ret < 0)
-				return ret;
-
-			s = dev->subdevices + 3;
-			ret = subdev_8255_init(dev, s, NULL,
-					       (unsigned long)(dev->iobase +
-							       PORT4A));
-			if (ret < 0)
-				return ret;
-
-			printk(KERN_DEBUG "comedi%d: adl_pci7432 attached\n",
-				dev->minor);
-
-			return 1;
-		}
+	if (comedi_pci_enable(pcidev, "adl_pci7296") < 0) {
+		printk(KERN_ERR
+			"comedi%d: Failed to enable PCI device and request regions\n",
+			dev->minor);
+		return -EIO;
 	}
 
-	printk(KERN_ERR "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-	       dev->minor, bus, slot);
-	return -EIO;
+	dev->iobase = pci_resource_start(pcidev, 2);
+	printk(KERN_INFO "comedi: base addr %4lx\n", dev->iobase);
+
+	/*  four 8255 digital io subdevices */
+	s = dev->subdevices + 0;
+	subdev_8255_init(dev, s, NULL, (unsigned long)(dev->iobase));
+
+	s = dev->subdevices + 1;
+	ret = subdev_8255_init(dev, s, NULL,
+				(unsigned long)(dev->iobase + PORT2A));
+	if (ret < 0)
+		return ret;
+
+	s = dev->subdevices + 2;
+	ret = subdev_8255_init(dev, s, NULL,
+				(unsigned long)(dev->iobase + PORT3A));
+	if (ret < 0)
+		return ret;
+
+	s = dev->subdevices + 3;
+	ret = subdev_8255_init(dev, s, NULL,
+				(unsigned long)(dev->iobase + PORT4A));
+	if (ret < 0)
+		return ret;
+
+	printk(KERN_DEBUG "comedi%d: adl_pci7432 attached\n", dev->minor);
+
+	return 0;
 }
 
 static void adl_pci7296_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 	if (dev->subdevices) {
 		subdev_8255_cleanup(dev, dev->subdevices + 0);
diff --git a/drivers/staging/comedi/drivers/adl_pci7432.c b/drivers/staging/comedi/drivers/adl_pci7432.c
index 9cbfb61a447..6b8d9408e3b 100644
--- a/drivers/staging/comedi/drivers/adl_pci7432.c
+++ b/drivers/staging/comedi/drivers/adl_pci7432.c
@@ -36,20 +36,12 @@ Configuration Options:
 
 #include "../comedidev.h"
 #include <linux/kernel.h>
-#include "comedi_pci.h"
 
 #define PCI7432_DI      0x00
 #define PCI7432_DO	    0x00
 
 #define PCI_DEVICE_ID_PCI7432 0x7432
 
-struct adl_pci7432_private {
-	int data;
-	struct pci_dev *pci_dev;
-};
-
-#define devpriv ((struct adl_pci7432_private *)dev->private)
-
 static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_insn *insn,
@@ -58,9 +50,6 @@ static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
 	printk(KERN_DEBUG "comedi: pci7432_do_insn_bits called\n");
 	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -69,7 +58,7 @@ static int adl_pci7432_do_insn_bits(struct comedi_device *dev,
 		       dev->iobase + PCI7432_DO);
 		outl(s->state & 0xffffffff, dev->iobase + PCI7432_DO);
 	}
-	return 2;
+	return insn->n;
 }
 
 static int adl_pci7432_di_insn_bits(struct comedi_device *dev,
@@ -80,93 +69,97 @@ static int adl_pci7432_di_insn_bits(struct comedi_device *dev,
 	printk(KERN_DEBUG "comedi: pci7432_di_insn_bits called\n");
 	printk(KERN_DEBUG "comedi: data0: %8x data1: %8x\n", data[0], data[1]);
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inl(dev->iobase + PCI7432_DI) & 0xffffffff;
 	printk(KERN_DEBUG "comedi: data1 %8x\n", data[1]);
 
-	return 2;
+	return insn->n;
+}
+
+static struct pci_dev *adl_pci7432_find_pci(struct comedi_device *dev,
+					    struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
+		    pcidev->device != PCI_DEVICE_ID_PCI7432)
+			continue;
+		if (bus || slot) {
+			/* requested particular bus/slot */
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		return pcidev;
+	}
+	printk(KERN_ERR
+		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+	       dev->minor, bus, slot);
+	return NULL;
 }
 
 static int adl_pci7432_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
-	struct pci_dev *pcidev = NULL;
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	int bus, slot;
+	int ret;
 
 	printk(KERN_INFO "comedi%d: attach adl_pci7432\n", dev->minor);
 
 	dev->board_name = "pci7432";
-	bus = it->options[0];
-	slot = it->options[1];
 
-	if (alloc_private(dev, sizeof(struct adl_pci7432_private)) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	pcidev = adl_pci7432_find_pci(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_ADLINK &&
-		    pcidev->device == PCI_DEVICE_ID_PCI7432) {
-			if (bus || slot) {
-				/* requested particular bus/slot */
-				if (pcidev->bus->number != bus
-				    || PCI_SLOT(pcidev->devfn) != slot) {
-					continue;
-				}
-			}
-			devpriv->pci_dev = pcidev;
-			if (comedi_pci_enable(pcidev, "adl_pci7432") < 0) {
-				printk(KERN_ERR "comedi%d: Failed to enable PCI device and request regions\n",
-				     dev->minor);
-				return -EIO;
-			}
-			dev->iobase = pci_resource_start(pcidev, 2);
-			printk(KERN_INFO "comedi: base addr %4lx\n",
-				dev->iobase);
-
-			s = dev->subdevices + 0;
-			s->type = COMEDI_SUBD_DI;
-			s->subdev_flags =
-			    SDF_READABLE | SDF_GROUND | SDF_COMMON;
-			s->n_chan = 32;
-			s->maxdata = 1;
-			s->len_chanlist = 32;
-			s->io_bits = 0x00000000;
-			s->range_table = &range_digital;
-			s->insn_bits = adl_pci7432_di_insn_bits;
-
-			s = dev->subdevices + 1;
-			s->type = COMEDI_SUBD_DO;
-			s->subdev_flags =
-			    SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-			s->n_chan = 32;
-			s->maxdata = 1;
-			s->len_chanlist = 32;
-			s->io_bits = 0xffffffff;
-			s->range_table = &range_digital;
-			s->insn_bits = adl_pci7432_do_insn_bits;
-
-			printk(KERN_DEBUG "comedi%d: adl_pci7432 attached\n",
-				dev->minor);
-			return 1;
-		}
+	if (comedi_pci_enable(pcidev, "adl_pci7432") < 0) {
+		printk(KERN_ERR "comedi%d: Failed to enable PCI device and request regions\n",
+			dev->minor);
+		return -EIO;
 	}
-
-	printk(KERN_ERR "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-	       dev->minor, bus, slot);
-	return -EIO;
+	dev->iobase = pci_resource_start(pcidev, 2);
+	printk(KERN_INFO "comedi: base addr %4lx\n", dev->iobase);
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
+	s->n_chan = 32;
+	s->maxdata = 1;
+	s->len_chanlist = 32;
+	s->io_bits = 0x00000000;
+	s->range_table = &range_digital;
+	s->insn_bits = adl_pci7432_di_insn_bits;
+
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->n_chan = 32;
+	s->maxdata = 1;
+	s->len_chanlist = 32;
+	s->io_bits = 0xffffffff;
+	s->range_table = &range_digital;
+	s->insn_bits = adl_pci7432_do_insn_bits;
+
+	printk(KERN_DEBUG "comedi%d: adl_pci7432 attached\n", dev->minor);
+	return 0;
 }
 
 static void adl_pci7432_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index 409ef13ad09..247ef00a7c6 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -38,7 +38,6 @@ Configuration Options:
 #include <linux/kernel.h>
 #include <linux/delay.h>
 #include "comedi_fc.h"
-#include "comedi_pci.h"
 #include "8253.h"
 
 #define PCI8164_AXIS_X  0x00
@@ -56,13 +55,6 @@ Configuration Options:
 
 #define PCI_DEVICE_ID_PCI8164 0x8164
 
-struct adl_pci8164_private {
-	int data;
-	struct pci_dev *pci_dev;
-};
-
-#define devpriv ((struct adl_pci8164_private *)dev->private)
-
 /*
  all the read commands are the same except for the addition a constant
  * const to the data for inw()
@@ -224,102 +216,112 @@ static int adl_pci8164_insn_write_buf1(struct comedi_device *dev,
 	return 2;
 }
 
+static struct pci_dev *adl_pci8164_find_pci(struct comedi_device *dev,
+					    struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK ||
+		    pcidev->device != PCI_DEVICE_ID_PCI8164)
+			continue;
+		if (bus || slot) {
+			/* requested particular bus/slot */
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		return pcidev;
+	}
+	printk(KERN_ERR
+		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+		dev->minor, bus, slot);
+	return NULL;
+}
+
 static int adl_pci8164_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
-	struct pci_dev *pcidev = NULL;
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	int bus, slot;
+	int ret;
 
 	printk(KERN_INFO "comedi: attempt to attach...\n");
 	printk(KERN_INFO "comedi%d: adl_pci8164\n", dev->minor);
 
 	dev->board_name = "pci8164";
-	bus = it->options[0];
-	slot = it->options[1];
 
-	if (alloc_private(dev, sizeof(struct adl_pci8164_private)) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	pcidev = adl_pci8164_find_pci(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_ADLINK &&
-		    pcidev->device == PCI_DEVICE_ID_PCI8164) {
-			if (bus || slot) {
-				/* requested particular bus/slot */
-				if (pcidev->bus->number != bus
-					|| PCI_SLOT(pcidev->devfn) != slot)
-					continue;
-			}
-			devpriv->pci_dev = pcidev;
-			if (comedi_pci_enable(pcidev, "adl_pci8164") < 0) {
-				printk(KERN_ERR "comedi%d: Failed to enable "
-				"PCI device and request regions\n", dev->minor);
-				return -EIO;
-			}
-			dev->iobase = pci_resource_start(pcidev, 2);
-			printk(KERN_DEBUG "comedi: base addr %4lx\n",
-				   dev->iobase);
-
-			s = dev->subdevices + 0;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_msts;
-			s->insn_write = adl_pci8164_insn_write_cmd;
-
-			s = dev->subdevices + 1;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_ssts;
-			s->insn_write = adl_pci8164_insn_write_otp;
-
-			s = dev->subdevices + 2;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_buf0;
-			s->insn_write = adl_pci8164_insn_write_buf0;
-
-			s = dev->subdevices + 3;
-			s->type = COMEDI_SUBD_PROC;
-			s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-			s->n_chan = 4;
-			s->maxdata = 0xffff;
-			s->len_chanlist = 4;
-			/* s->range_table = &range_axis; */
-			s->insn_read = adl_pci8164_insn_read_buf1;
-			s->insn_write = adl_pci8164_insn_write_buf1;
-
-			printk(KERN_INFO "comedi: attached\n");
-
-			return 1;
-		}
+	if (comedi_pci_enable(pcidev, "adl_pci8164") < 0) {
+		printk(KERN_ERR "comedi%d: Failed to enable "
+		"PCI device and request regions\n", dev->minor);
+		return -EIO;
 	}
-
-	printk(KERN_ERR "comedi%d: no supported board found!"
-		   "(req. bus/slot : %d/%d)\n", dev->minor, bus, slot);
-	return -EIO;
+	dev->iobase = pci_resource_start(pcidev, 2);
+	printk(KERN_DEBUG "comedi: base addr %4lx\n", dev->iobase);
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_PROC;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xffff;
+	s->len_chanlist = 4;
+	/* s->range_table = &range_axis; */
+	s->insn_read = adl_pci8164_insn_read_msts;
+	s->insn_write = adl_pci8164_insn_write_cmd;
+
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_PROC;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xffff;
+	s->len_chanlist = 4;
+	/* s->range_table = &range_axis; */
+	s->insn_read = adl_pci8164_insn_read_ssts;
+	s->insn_write = adl_pci8164_insn_write_otp;
+
+	s = dev->subdevices + 2;
+	s->type = COMEDI_SUBD_PROC;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xffff;
+	s->len_chanlist = 4;
+	/* s->range_table = &range_axis; */
+	s->insn_read = adl_pci8164_insn_read_buf0;
+	s->insn_write = adl_pci8164_insn_write_buf0;
+
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_PROC;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xffff;
+	s->len_chanlist = 4;
+	/* s->range_table = &range_axis; */
+	s->insn_read = adl_pci8164_insn_read_buf1;
+	s->insn_write = adl_pci8164_insn_write_buf1;
+
+	printk(KERN_INFO "comedi: attached\n");
+	return 0;
 }
 
 static void adl_pci8164_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index ccfb1a52154..a31dae6e07d 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -81,7 +81,6 @@ TODO:
 #include <linux/interrupt.h>
 
 #include "8253.h"
-#include "comedi_pci.h"
 #include "comedi_fc.h"
 
 #define PCI9111_DRIVER_NAME	"adl_pci9111"
@@ -339,7 +338,6 @@ static const struct pci9111_board pci9111_boards[] = {
 /*  Private data structure */
 
 struct pci9111_private_data {
-	struct pci_dev *pci_device;
 	unsigned long io_range;	/*  PCI6503 io range */
 
 	unsigned long lcr_io_base; /* Local configuration register base
@@ -1154,7 +1152,7 @@ static int pci9111_di_insn_bits(struct comedi_device *dev,
 	bits = pci9111_di_get_bits();
 	data[1] = bits;
 
-	return 2;
+	return insn->n;
 }
 
 /*  Digital outputs */
@@ -1180,7 +1178,7 @@ static int pci9111_do_insn_bits(struct comedi_device *dev,
 
 	data[1] = bits;
 
-	return 2;
+	return insn->n;
 }
 
 /*  ------------------------------------------------------------------ */
@@ -1210,17 +1208,48 @@ static int pci9111_reset(struct comedi_device *dev)
 	return 0;
 }
 
-/*  Attach */
-/*       - Register PCI device */
-/*       - Declare device driver capability */
+static struct pci_dev *pci9111_find_pci(struct comedi_device *dev,
+					struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_ADLINK)
+			continue;
+		for (i = 0; i < pci9111_board_nbr; i++) {
+			if (pcidev->device != pci9111_boards[i].device_id)
+				continue;
+			if (bus || slot) {
+				/* requested particular bus/slot */
+				if (pcidev->bus->number != bus ||
+				    PCI_SLOT(pcidev->devfn) != slot)
+					continue;
+			}
+			dev->board_ptr = pci9111_boards + i;
+			printk(KERN_ERR
+				"comedi%d: found %s (b:s:f=%d:%d:%d), irq=%d\n",
+				dev->minor, pci9111_boards[i].name,
+				pcidev->bus->number, PCI_SLOT(pcidev->devfn),
+				PCI_FUNC(pcidev->devfn), pcidev->irq);
+			return pcidev;
+		}
+	}
+	printk(KERN_ERR
+		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+		dev->minor, bus, slot);
+	return NULL;
+}
 
 static int pci9111_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *subdevice;
 	unsigned long io_base, io_range, lcr_io_base, lcr_io_range;
-	struct pci_dev *pci_device = NULL;
-	int error, i;
+	int error;
 	const struct pci9111_board *board;
 
 	if (alloc_private(dev, sizeof(struct pci9111_private_data)) < 0)
@@ -1230,65 +1259,26 @@ static int pci9111_attach(struct comedi_device *dev,
 	printk(KERN_ERR "comedi%d: " PCI9111_DRIVER_NAME " driver\n",
 								dev->minor);
 
-	for_each_pci_dev(pci_device) {
-		if (pci_device->vendor == PCI_VENDOR_ID_ADLINK) {
-			for (i = 0; i < pci9111_board_nbr; i++) {
-				if (pci9111_boards[i].device_id ==
-				    pci_device->device) {
-					/* was a particular bus/slot
-					 * requested? */
-					if ((it->options[0] != 0)
-					    || (it->options[1] != 0)) {
-						/* are we on the wrong
-						 * bus/slot? */
-						if (pci_device->bus->number !=
-						    it->options[0]
-						    ||
-						    PCI_SLOT(pci_device->devfn)
-						    != it->options[1]) {
-							continue;
-						}
-					}
-
-					dev->board_ptr = pci9111_boards + i;
-					board =
-					    (struct pci9111_board *)
-					    dev->board_ptr;
-					dev_private->pci_device = pci_device;
-					goto found;
-				}
-			}
-		}
-	}
-
-	printk(KERN_ERR
-		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-			dev->minor, it->options[0], it->options[1]);
-	return -EIO;
-
-found:
-
-	printk(KERN_ERR "comedi%d: found %s (b:s:f=%d:%d:%d) , irq=%d\n",
-	       dev->minor,
-	       pci9111_boards[i].name,
-	       pci_device->bus->number,
-	       PCI_SLOT(pci_device->devfn),
-	       PCI_FUNC(pci_device->devfn), pci_device->irq);
+	pcidev = pci9111_find_pci(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	board = (struct pci9111_board *)dev->board_ptr;
 
 	/*  TODO: Warn about non-tested boards. */
 
 	/*  Read local configuration register base address
 	 *  [PCI_BASE_ADDRESS #1]. */
 
-	lcr_io_base = pci_resource_start(pci_device, 1);
-	lcr_io_range = pci_resource_len(pci_device, 1);
+	lcr_io_base = pci_resource_start(pcidev, 1);
+	lcr_io_range = pci_resource_len(pcidev, 1);
 
 	printk
 	    ("comedi%d: local configuration registers at address 0x%4lx [0x%4lx]\n",
 	     dev->minor, lcr_io_base, lcr_io_range);
 
 	/*  Enable PCI device and request regions */
-	if (comedi_pci_enable(pci_device, PCI9111_DRIVER_NAME) < 0) {
+	if (comedi_pci_enable(pcidev, PCI9111_DRIVER_NAME) < 0) {
 		printk
 		    ("comedi%d: Failed to enable PCI device and request regions\n",
 		     dev->minor);
@@ -1296,8 +1286,8 @@ found:
 	}
 	/*  Read PCI6308 register base address [PCI_BASE_ADDRESS #2]. */
 
-	io_base = pci_resource_start(pci_device, 2);
-	io_range = pci_resource_len(pci_device, 2);
+	io_base = pci_resource_start(pcidev, 2);
+	io_range = pci_resource_len(pcidev, 2);
 
 	printk(KERN_ERR "comedi%d: 6503 registers at address 0x%4lx [0x%4lx]\n",
 	       dev->minor, io_base, io_range);
@@ -1314,21 +1304,22 @@ found:
 	/*  Irq setup */
 
 	dev->irq = 0;
-	if (pci_device->irq > 0) {
-		if (request_irq(pci_device->irq, pci9111_interrupt,
+	if (pcidev->irq > 0) {
+		dev->irq = pcidev->irq;
+
+		if (request_irq(dev->irq, pci9111_interrupt,
 				IRQF_SHARED, PCI9111_DRIVER_NAME, dev) != 0) {
 			printk(KERN_ERR
 				"comedi%d: unable to allocate irq  %u\n",
-					dev->minor, pci_device->irq);
+					dev->minor, dev->irq);
 			return -EINVAL;
 		}
 	}
-	dev->irq = pci_device->irq;
 
 	/*  TODO: Add external multiplexer setup (according to option[2]). */
 
-	error = alloc_subdevices(dev, 4);
-	if (error < 0)
+	error = comedi_alloc_subdevices(dev, 4);
+	if (error)
 		return error;
 
 	subdevice = dev->subdevices + 0;
@@ -1384,16 +1375,18 @@ found:
 
 static void pci9111_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->private != NULL) {
 		if (dev_private->is_valid)
 			pci9111_reset(dev);
 	}
 	if (dev->irq != 0)
 		free_irq(dev->irq, dev);
-	if (dev_private != NULL && dev_private->pci_device != NULL) {
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(dev_private->pci_device);
-		pci_dev_put(dev_private->pci_device);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index 78645863297..a1f74c2590e 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -71,7 +71,6 @@ Configuration options:
 
 #include "amcc_s5933.h"
 #include "8253.h"
-#include "comedi_pci.h"
 #include "comedi_fc.h"
 
 #define PCI_VENDOR_ID_AMCC	0x10e8
@@ -251,7 +250,6 @@ struct boardtype {
 struct pci9118_private {
 	unsigned long iobase_a;	/* base+size for AMCC chip */
 	unsigned int master;	/* master capable */
-	struct pci_dev *pcidev;	/* ptr to actual pcidev */
 	unsigned int usemux;	/* we want to use external multiplexor! */
 #ifdef PCI9118_PARANOIDCHECK
 	unsigned short chanlist[PCI9118_CHANLEN + 1];	/*
@@ -300,7 +298,7 @@ struct pci9118_private {
 	short *ai_data;
 	short ao_data[2];			/* data output buffer */
 	unsigned int ai_scans;			/* number of scans to do */
-	char dma_doublebuf;			/* we can use double buffring */
+	char dma_doublebuf;			/* we can use double buffering */
 	unsigned int dma_actbuf;		/* which buffer is used now */
 	short *dmabuf_virt[2];			/*
 						 * pointers to begin of
@@ -493,7 +491,7 @@ static int pci9118_insn_bits_di(struct comedi_device *dev,
 {
 	data[1] = inl(dev->iobase + PCI9118_DI) & 0xf;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -510,7 +508,7 @@ static int pci9118_insn_bits_do(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -1736,7 +1734,7 @@ static int check_channel_list(struct comedi_device *dev,
 							"can't be mixtured!");
 				return 0;
 			}
-			if ((!devpriv->usemux) & (differencial) &
+			if (!devpriv->usemux && differencial &&
 			    (CR_CHAN(chanlist[i]) >= this_board->n_aichand)) {
 				comedi_error(dev,
 					     "If AREF_DIFF is used then is "
@@ -2111,24 +2109,56 @@ static int pci9118_reset(struct comedi_device *dev)
 	return 0;
 }
 
+static struct pci_dev *pci9118_find_pci(struct comedi_device *dev,
+					struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_AMCC)
+			continue;
+		if (pcidev->device != this_board->device_id)
+			continue;
+		if (bus || slot) {
+			/* requested particular bus/slot */
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		/*
+		 * Look for device that isn't in use.
+		 * Enable PCI device and request regions.
+		 */
+		if (comedi_pci_enable(pcidev, "adl_pci9118"))
+			continue;
+		printk(KERN_ERR ", b:s:f=%d:%d:%d, io=0x%4lx, 0x%4lx",
+			pcidev->bus->number,
+			PCI_SLOT(pcidev->devfn),
+			PCI_FUNC(pcidev->devfn),
+			(unsigned long)pci_resource_start(pcidev, 2),
+			(unsigned long)pci_resource_start(pcidev, 0));
+		return pcidev;
+	}
+	printk(KERN_ERR
+		"comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+		dev->minor, bus, slot);
+	return NULL;
+}
+
 static int pci9118_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int ret, pages, i;
 	unsigned short master;
 	unsigned int irq;
-	unsigned long iobase_a, iobase_9;
-	struct pci_dev *pcidev;
-	int opt_bus, opt_slot;
-	const char *errstr;
-	unsigned char pci_bus, pci_slot, pci_func;
 	u16 u16w;
 
 	printk("comedi%d: adl_pci9118: board=%s", dev->minor, this_board->name);
 
-	opt_bus = it->options[0];
-	opt_slot = it->options[1];
 	if (it->options[3] & 1)
 		master = 0;	/* user don't want use bus master */
 	else
@@ -2140,61 +2170,20 @@ static int pci9118_attach(struct comedi_device *dev,
 		return -ENOMEM;
 	}
 
-	/* Look for matching PCI device */
-	errstr = "not found!";
-	pcidev = NULL;
-	while (NULL != (pcidev = pci_get_device(PCI_VENDOR_ID_AMCC,
-						this_board->device_id,
-						pcidev))) {
-		/* Found matching vendor/device. */
-		if (opt_bus || opt_slot) {
-			/* Check bus/slot. */
-			if (opt_bus != pcidev->bus->number
-			    || opt_slot != PCI_SLOT(pcidev->devfn))
-				continue;	/* no match */
-		}
-		/*
-		 * Look for device that isn't in use.
-		 * Enable PCI device and request regions.
-		 */
-		if (comedi_pci_enable(pcidev, "adl_pci9118")) {
-			errstr =
-			    "failed to enable PCI device and request regions!";
-			continue;
-		}
-		break;
-	}
-
-	if (!pcidev) {
-		if (opt_bus || opt_slot) {
-			printk(KERN_ERR " - Card at b:s %d:%d %s\n",
-			       opt_bus, opt_slot, errstr);
-		} else {
-			printk(KERN_ERR " - Card %s\n", errstr);
-		}
+	pcidev = pci9118_find_pci(dev, it);
+	if (!pcidev)
 		return -EIO;
-	}
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
 	if (master)
 		pci_set_master(pcidev);
 
-
-	pci_bus = pcidev->bus->number;
-	pci_slot = PCI_SLOT(pcidev->devfn);
-	pci_func = PCI_FUNC(pcidev->devfn);
 	irq = pcidev->irq;
-	iobase_a = pci_resource_start(pcidev, 0);
-	iobase_9 = pci_resource_start(pcidev, 2);
-
-	printk(KERN_ERR ", b:s:f=%d:%d:%d, io=0x%4lx, 0x%4lx", pci_bus,
-				pci_slot, pci_func, iobase_9, iobase_a);
+	devpriv->iobase_a = pci_resource_start(pcidev, 0);
+	dev->iobase = pci_resource_start(pcidev, 2);
 
-	dev->iobase = iobase_9;
 	dev->board_name = this_board->name;
 
-	devpriv->pcidev = pcidev;
-	devpriv->iobase_a = iobase_a;
-
 	pci9118_reset(dev);
 
 	if (it->options[3] & 2)
@@ -2276,12 +2265,12 @@ static int pci9118_attach(struct comedi_device *dev,
 
 	printk(".\n");
 
-	pci_read_config_word(devpriv->pcidev, PCI_COMMAND, &u16w);
-	pci_write_config_word(devpriv->pcidev, PCI_COMMAND, u16w | 64);
+	pci_read_config_word(pcidev, PCI_COMMAND, &u16w);
+	pci_write_config_word(pcidev, PCI_COMMAND, u16w | 64);
 				/* Enable parity check for parity error */
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
@@ -2355,17 +2344,13 @@ static int pci9118_attach(struct comedi_device *dev,
 
 static void pci9118_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->private) {
 		if (devpriv->valid)
 			pci9118_reset(dev);
 		if (dev->irq)
 			free_irq(dev->irq, dev);
-		if (devpriv->pcidev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pcidev);
-
-			pci_dev_put(devpriv->pcidev);
-		}
 		if (devpriv->dmabuf_virt[0])
 			free_pages((unsigned long)devpriv->dmabuf_virt[0],
 				   devpriv->dmabuf_pages[0]);
@@ -2373,6 +2358,12 @@ static void pci9118_detach(struct comedi_device *dev)
 			free_pages((unsigned long)devpriv->dmabuf_virt[1],
 				   devpriv->dmabuf_pages[1]);
 	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+
+		pci_dev_put(pcidev);
+	}
 }
 
 static const struct boardtype boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/adq12b.c b/drivers/staging/comedi/drivers/adq12b.c
index 7d585a12050..6df51c8a602 100644
--- a/drivers/staging/comedi/drivers/adq12b.c
+++ b/drivers/staging/comedi/drivers/adq12b.c
@@ -125,8 +125,6 @@ struct adq12b_board {
 	int do_chans;
 };
 
-#define thisboard ((const struct adq12b_board *)dev->board_ptr)
-
 struct adq12b_private {
 	int unipolar;		/* option 2 of comedi_config (1 is iobase) */
 	int differential;	/* option 3 of comedi_config */
@@ -195,7 +193,7 @@ static int adq12b_di_insn_bits(struct comedi_device *dev,
 	/* only bits 0-4 have information about digital inputs */
 	data[1] = (inb(dev->iobase + ADQ12B_STINR) & (0x1f));
 
-	return 2;
+	return insn->n;
 }
 
 static int adq12b_do_insn_bits(struct comedi_device *dev,
@@ -217,14 +215,16 @@ static int adq12b_do_insn_bits(struct comedi_device *dev,
 
 	data[1] = devpriv->digital_state;
 
-	return 2;
+	return insn->n;
 }
 
 static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct adq12b_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	int unipolar, differential;
+	int ret;
 
 	iobase = it->options[0];
 	unipolar = it->options[1];
@@ -250,11 +250,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	dev->iobase = iobase;
 
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 /*
  * Allocate the private structure area.  alloc_private() is a
@@ -272,22 +268,19 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->last_channel = -1;
 	devpriv->last_range = -1;
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
 	if (differential) {
 		s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
-		s->n_chan = thisboard->ai_diff_chans;
+		s->n_chan = board->ai_diff_chans;
 	} else {
 		s->subdev_flags = SDF_READABLE | SDF_GROUND;
-		s->n_chan = thisboard->ai_se_chans;
+		s->n_chan = board->ai_se_chans;
 	}
 
 	if (unipolar)
@@ -295,7 +288,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	else
 		s->range_table = &range_adq12b_ai_bipolar;
 
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->maxdata = (1 << board->ai_bits) - 1;
 
 	s->len_chanlist = 4;	/* This is the maximum chanlist length that
 				   the board can handle */
@@ -305,7 +298,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* digital input subdevice */
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE;
-	s->n_chan = thisboard->di_chans;
+	s->n_chan = board->di_chans;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
 	s->insn_bits = adq12b_di_insn_bits;
@@ -314,7 +307,7 @@ static int adq12b_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* digital output subdevice */
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->do_chans;
+	s->n_chan = board->do_chans;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
 	s->insn_bits = adq12b_do_insn_bits;
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index de8c98cfe36..31986608eaf 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -45,8 +45,6 @@ Configuration options:
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #include "8253.h"
 #include "amcc_s5933.h"
 
@@ -250,7 +248,6 @@ static const struct boardtype boardtypes[] = {
 };
 
 struct pci1710_private {
-	struct pci_dev *pcidev;	/*  ptr to PCI device */
 	char valid;		/*  card is usable */
 	char neverending_ai;	/*  we do unlimited AI */
 	unsigned int CntrlReg;	/*  Control register */
@@ -437,7 +434,7 @@ static int pci171x_insn_bits_di(struct comedi_device *dev,
 {
 	data[1] = inw(dev->iobase + PCI171x_DI);
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -454,7 +451,7 @@ static int pci171x_insn_bits_do(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -1335,35 +1332,26 @@ static int pci1710_reset(struct comedi_device *dev)
 	DPRINTK("adv_pci1710 EDBG: END: pci1710_reset(...)\n");
 }
 
-static int pci1710_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static struct pci_dev *pci1710_find_pci_dev(struct comedi_device *dev,
+					    struct comedi_devconfig *it)
 {
-	struct comedi_subdevice *s;
-	int ret, subdev, n_subdevices;
-	unsigned int irq;
-	unsigned long iobase;
-	struct pci_dev *pcidev;
-	int opt_bus, opt_slot;
-	const char *errstr;
-	unsigned char pci_bus, pci_slot, pci_func;
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int board_index = this_board - boardtypes;
 	int i;
-	int board_index;
 
-	dev_info(dev->hw_dev, "comedi%d: adv_pci1710:\n", dev->minor);
-
-	opt_bus = it->options[0];
-	opt_slot = it->options[1];
-
-	ret = alloc_private(dev, sizeof(struct pci1710_private));
-	if (ret < 0)
-		return -ENOMEM;
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_ADVANTECH)
+			continue;
+		if (pci_is_enabled(pcidev))
+			continue;
 
-	/* Look for matching PCI device */
-	errstr = "not found!";
-	pcidev = NULL;
-	board_index = this_board - boardtypes;
-	while (NULL != (pcidev = pci_get_device(PCI_VENDOR_ID_ADVANTECH,
-						PCI_ANY_ID, pcidev))) {
 		if (strcmp(this_board->name, DRV_NAME) == 0) {
 			for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
 				if (pcidev->device == boardtypes[i].device_id) {
@@ -1377,51 +1365,42 @@ static int pci1710_attach(struct comedi_device *dev,
 			if (pcidev->device != boardtypes[board_index].device_id)
 				continue;
 		}
-
-		/* Found matching vendor/device. */
-		if (opt_bus || opt_slot) {
-			/* Check bus/slot. */
-			if (opt_bus != pcidev->bus->number
-			    || opt_slot != PCI_SLOT(pcidev->devfn))
-				continue;	/* no match */
-		}
-		/*
-		 * Look for device that isn't in use.
-		 * Enable PCI device and request regions.
-		 */
-		if (comedi_pci_enable(pcidev, DRV_NAME)) {
-			errstr =
-			    "failed to enable PCI device and request regions!";
-			continue;
-		}
-		/*  fixup board_ptr in case we were using the dummy entry with the driver name */
 		dev->board_ptr = &boardtypes[board_index];
-		break;
+		return pcidev;
 	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
 
-	if (!pcidev) {
-		if (opt_bus || opt_slot) {
-			dev_err(dev->hw_dev, "- Card at b:s %d:%d %s\n",
-				opt_bus, opt_slot, errstr);
-		} else {
-			dev_err(dev->hw_dev, "- Card %s\n", errstr);
-		}
-		return -EIO;
-	}
+static int pci1710_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev;
+	struct comedi_subdevice *s;
+	int ret, subdev, n_subdevices;
+	unsigned int irq;
 
-	pci_bus = pcidev->bus->number;
-	pci_slot = PCI_SLOT(pcidev->devfn);
-	pci_func = PCI_FUNC(pcidev->devfn);
-	irq = pcidev->irq;
-	iobase = pci_resource_start(pcidev, 2);
+	dev_info(dev->class_dev, DRV_NAME ": attach\n");
 
-	dev_dbg(dev->hw_dev, "b:s:f=%d:%d:%d, io=0x%4lx\n", pci_bus, pci_slot,
-		pci_func, iobase);
+	ret = alloc_private(dev, sizeof(struct pci1710_private));
+	if (ret < 0)
+		return -ENOMEM;
+
+	pcidev = pci1710_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	ret = comedi_pci_enable(pcidev, DRV_NAME);
+	if (ret)
+		return ret;
 
-	dev->iobase = iobase;
+	dev->iobase = pci_resource_start(pcidev, 2);
+	irq = pcidev->irq;
 
 	dev->board_name = this_board->name;
-	devpriv->pcidev = pcidev;
 
 	n_subdevices = 0;
 	if (this_board->n_aichan)
@@ -1435,8 +1414,8 @@ static int pci1710_attach(struct comedi_device *dev,
 	if (this_board->n_counter)
 		n_subdevices++;
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
 
 	pci1710_reset(dev);
@@ -1446,14 +1425,15 @@ static int pci1710_attach(struct comedi_device *dev,
 			if (request_irq(irq, interrupt_service_pci1710,
 					IRQF_SHARED, "Advantech PCI-1710",
 					dev)) {
-				dev_dbg(dev->hw_dev, "unable to allocate IRQ %d, DISABLING IT",
+				dev_dbg(dev->class_dev,
+					"unable to allocate IRQ %d, DISABLING IT",
 					irq);
 				irq = 0;	/* Can't use IRQ */
 			} else {
-				dev_dbg(dev->hw_dev, "irq=%u", irq);
+				dev_dbg(dev->class_dev, "irq=%u", irq);
 			}
 		} else {
-			dev_dbg(dev->hw_dev, "IRQ disabled");
+			dev_dbg(dev->class_dev, "IRQ disabled");
 		}
 	} else {
 		irq = 0;
@@ -1553,16 +1533,18 @@ static int pci1710_attach(struct comedi_device *dev,
 
 static void pci1710_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->private) {
 		if (devpriv->valid)
 			pci1710_reset(dev);
 		if (dev->irq)
 			free_irq(dev->irq, dev);
-		if (devpriv->pcidev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pcidev);
-			pci_dev_put(devpriv->pcidev);
-		}
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index 336addcbce3..da5ee69d2c9 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -50,8 +50,6 @@ TODO:
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe	/* Advantech PCI vendor ID */
 
 /* hardware types of the cards */
@@ -154,7 +152,6 @@ static const struct pci1723_board boardtypes[] = {
 struct pci1723_private {
 	int valid;		/* card is usable; */
 
-	struct pci_dev *pcidev;
 	unsigned char da_range[8];	/* D/A output range for each channel */
 
 	short ao_data[8];	/* data output buffer */
@@ -286,79 +283,62 @@ static int pci1723_dio_insn_bits(struct comedi_device *dev,
 		outw(s->state, dev->iobase + PCI1723_WRITE_DIGITAL_OUTPUT_CMD);
 	}
 	data[1] = inw(dev->iobase + PCI1723_READ_DIGITAL_INPUT_DATA);
-	return 2;
+	return insn->n;
+}
+
+static struct pci_dev *pci1723_find_pci_dev(struct comedi_device *dev,
+					    struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_ADVANTECH)
+			continue;
+		if (pci_is_enabled(pcidev))
+			continue;
+		return pcidev;
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
 
 static int pci1723_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int ret, subdev, n_subdevices;
-	struct pci_dev *pcidev;
-	unsigned int iobase;
-	unsigned char pci_bus, pci_slot, pci_func;
-	int opt_bus, opt_slot;
-	const char *errstr;
 
 	printk(KERN_ERR "comedi%d: adv_pci1723: board=%s",
 						dev->minor, this_board->name);
 
-	opt_bus = it->options[0];
-	opt_slot = it->options[1];
-
 	ret = alloc_private(dev, sizeof(struct pci1723_private));
 	if (ret < 0) {
 		printk(" - Allocation failed!\n");
 		return -ENOMEM;
 	}
 
-	/* Look for matching PCI device */
-	errstr = "not found!";
-	pcidev = NULL;
-	while (NULL != (pcidev =
-			pci_get_device(PCI_VENDOR_ID_ADVANTECH,
-				       this_board->device_id, pcidev))) {
-		/* Found matching vendor/device. */
-		if (opt_bus || opt_slot) {
-			/* Check bus/slot. */
-			if (opt_bus != pcidev->bus->number
-			    || opt_slot != PCI_SLOT(pcidev->devfn))
-				continue;	/* no match */
-		}
-		/*
-		 * Look for device that isn't in use.
-		 * Enable PCI device and request regions.
-		 */
-		if (comedi_pci_enable(pcidev, "adv_pci1723")) {
-			errstr =
-			    "failed to enable PCI device and request regions!";
-			continue;
-		}
-		break;
-	}
-
-	if (!pcidev) {
-		if (opt_bus || opt_slot) {
-			printk(KERN_ERR " - Card at b:s %d:%d %s\n",
-						     opt_bus, opt_slot, errstr);
-		} else {
-			printk(KERN_ERR " - Card %s\n", errstr);
-		}
+	pcidev = pci1723_find_pci_dev(dev, it);
+	if (!pcidev)
 		return -EIO;
-	}
-
-	pci_bus = pcidev->bus->number;
-	pci_slot = PCI_SLOT(pcidev->devfn);
-	pci_func = PCI_FUNC(pcidev->devfn);
-	iobase = pci_resource_start(pcidev, 2);
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	printk(KERN_ERR ", b:s:f=%d:%d:%d, io=0x%4x",
-					   pci_bus, pci_slot, pci_func, iobase);
+	ret = comedi_pci_enable(pcidev, "adv_pci1723");
+	if (ret)
+		return ret;
 
-	dev->iobase = iobase;
+	dev->iobase = pci_resource_start(pcidev, 2);
 
 	dev->board_name = this_board->name;
-	devpriv->pcidev = pcidev;
 
 	n_subdevices = 0;
 
@@ -367,11 +347,9 @@ static int pci1723_attach(struct comedi_device *dev,
 	if (this_board->n_diochan)
 		n_subdevices++;
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0) {
-		printk(" - Allocation failed!\n");
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
-	}
 
 	pci1723_reset(dev);
 	subdev = 0;
@@ -433,14 +411,16 @@ static int pci1723_attach(struct comedi_device *dev,
 
 static void pci1723_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->private) {
 		if (devpriv->valid)
 			pci1723_reset(dev);
-		if (devpriv->pcidev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pcidev);
-			pci_dev_put(devpriv->pcidev);
-		}
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 43a32dc12cd..97f06dc8e48 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -33,7 +33,6 @@ Configuration options:
 
 #include <linux/delay.h>
 
-#include "comedi_pci.h"
 #include "8255.h"
 #include "8253.h"
 
@@ -383,9 +382,6 @@ static const struct dio_boardtype boardtypes[] = {
 };
 
 struct pci_dio_private {
-	struct pci_dio_private *prev;	/*  previous private struct */
-	struct pci_dio_private *next;	/*  next private struct */
-	struct pci_dev *pcidev;	/*  pointer to board's pci_dev */
 	char valid;		/*  card is usable */
 	char GlobalIrqEnabled;	/*  1= any IRQ source is enabled */
 	/*  PCI-1760 specific data */
@@ -405,8 +401,6 @@ struct pci_dio_private {
 	unsigned short IDIFiltrHigh[8];	/*  IDI's filter value high signal */
 };
 
-static struct pci_dio_private *pci_priv;	/* list of allocated cards */
-
 #define devpriv ((struct pci_dio_private *)dev->private)
 #define this_board ((const struct dio_boardtype *)dev->board_ptr)
 
@@ -425,7 +419,7 @@ static int pci_dio_insn_bits_di_b(struct comedi_device *dev,
 		data[1] |= inb(dev->iobase + d->addr + i) << (8 * i);
 
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -442,7 +436,7 @@ static int pci_dio_insn_bits_di_w(struct comedi_device *dev,
 	for (i = 0; i < d->regs; i++)
 		data[1] |= inw(dev->iobase + d->addr + 2 * i) << (16 * i);
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -464,7 +458,7 @@ static int pci_dio_insn_bits_do_b(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -486,7 +480,7 @@ static int pci_dio_insn_bits_do_w(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -635,7 +629,7 @@ static int pci1760_insn_bits_di(struct comedi_device *dev,
 {
 	data[1] = inb(dev->iobase + IMB3);
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -664,7 +658,7 @@ static int pci1760_insn_bits_do(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -1056,86 +1050,60 @@ static int pci_dio_add_8254(struct comedi_device *dev,
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static int CheckAndAllocCard(struct comedi_device *dev,
-			     struct comedi_devconfig *it,
-			     struct pci_dev *pcidev)
+static struct pci_dev *pci_dio_find_pci_dev(struct comedi_device *dev,
+					    struct comedi_devconfig *it)
 {
-	struct pci_dio_private *pr, *prev;
-
-	for (pr = pci_priv, prev = NULL; pr != NULL; prev = pr, pr = pr->next) {
-		if (pr->pcidev == pcidev)
-			return 0; /* this card is used, look for another */
-
-	}
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
 
-	if (prev) {
-		devpriv->prev = prev;
-		prev->next = devpriv;
-	} else {
-		pci_priv = devpriv;
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pci_is_enabled(pcidev))
+			continue;
+		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
+			if (boardtypes[i].vendor_id != pcidev->vendor)
+				continue;
+			if (boardtypes[i].device_id != pcidev->device)
+				continue;
+			dev->board_ptr = boardtypes + i;
+			return pcidev;
+		}
 	}
-
-	devpriv->pcidev = pcidev;
-
-	return 1;
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
 
 static int pci_dio_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
 	int ret, subdev, n_subdevices, i, j;
-	unsigned long iobase;
-	struct pci_dev *pcidev = NULL;
-
 
 	ret = alloc_private(dev, sizeof(struct pci_dio_private));
 	if (ret < 0)
 		return -ENOMEM;
 
-	for_each_pci_dev(pcidev) {
-		/*  loop through cards supported by this driver */
-		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
-			if (boardtypes[i].vendor_id != pcidev->vendor)
-				continue;
-			if (boardtypes[i].device_id != pcidev->device)
-				continue;
-			/*  was a particular bus/slot requested? */
-			if (it->options[0] || it->options[1]) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			ret = CheckAndAllocCard(dev, it, pcidev);
-			if (ret != 1)
-				continue;
-			dev->board_ptr = boardtypes + i;
-			break;
-		}
-		if (dev->board_ptr)
-			break;
-	}
-
-	if (!dev->board_ptr) {
-		dev_err(dev->hw_dev, "Error: Requested type of the card was not found!\n");
+	pcidev = pci_dio_find_pci_dev(dev, it);
+	if (!pcidev)
 		return -EIO;
-	}
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
 	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
-		dev_err(dev->hw_dev, "Error: Can't enable PCI device and request regions!\n");
+		dev_err(dev->class_dev,
+			"Error: Can't enable PCI device and request regions!\n");
 		return -EIO;
 	}
-	iobase = pci_resource_start(pcidev, this_board->main_pci_region);
-	dev_dbg(dev->hw_dev, "b:s:f=%d:%d:%d, io=0x%4lx\n",
-		pcidev->bus->number, PCI_SLOT(pcidev->devfn),
-		PCI_FUNC(pcidev->devfn), iobase);
 
-	dev->iobase = iobase;
+	dev->iobase = pci_resource_start(pcidev, this_board->main_pci_region);
 	dev->board_name = this_board->name;
 
 	if (this_board->cardtype == TYPE_PCI1760) {
@@ -1157,8 +1125,8 @@ static int pci_dio_attach(struct comedi_device *dev,
 				n_subdevices++;
 	}
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
 
 	subdev = 0;
@@ -1212,6 +1180,7 @@ static int pci_dio_attach(struct comedi_device *dev,
 
 static void pci_dio_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	int i, j;
 	struct comedi_subdevice *s;
 	int subdev;
@@ -1244,17 +1213,11 @@ static void pci_dio_detach(struct comedi_device *dev)
 			s = dev->subdevices + i;
 			s->private = NULL;
 		}
-		if (devpriv->pcidev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pcidev);
-			pci_dev_put(devpriv->pcidev);
-		}
-		if (devpriv->prev)
-			devpriv->prev->next = devpriv->next;
-		else
-			pci_priv = devpriv->next;
-		if (devpriv->next)
-			devpriv->next->prev = devpriv->prev;
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/aio_aio12_8.c b/drivers/staging/comedi/drivers/aio_aio12_8.c
index 64d82bc4ffe..f7d453f8fe3 100644
--- a/drivers/staging/comedi/drivers/aio_aio12_8.c
+++ b/drivers/staging/comedi/drivers/aio_aio12_8.c
@@ -80,8 +80,6 @@ static const struct aio12_8_boardtype board_types[] = {
 	 .name = "aio_aio12_8"},
 };
 
-#define	thisboard	((const struct aio12_8_boardtype  *) dev->board_ptr)
-
 struct aio12_8_private {
 	unsigned int ao_readback[4];
 };
@@ -167,8 +165,10 @@ static const struct comedi_lrange range_aio_aio12_8 = {
 static int aio_aio12_8_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
+	const struct aio12_8_boardtype *board = comedi_board(dev);
 	int iobase;
 	struct comedi_subdevice *s;
+	int ret;
 
 	iobase = it->options[0];
 	if (!request_region(iobase, 24, "aio_aio12_8")) {
@@ -176,15 +176,16 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 		return -EIO;
 	}
 
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	dev->iobase = iobase;
 
 	if (alloc_private(dev, sizeof(struct aio12_8_private)) < 0)
 		return -ENOMEM;
 
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
diff --git a/drivers/staging/comedi/drivers/aio_iiro_16.c b/drivers/staging/comedi/drivers/aio_iiro_16.c
index 04f6f94b1f4..ba1e3bbf2df 100644
--- a/drivers/staging/comedi/drivers/aio_iiro_16.c
+++ b/drivers/staging/comedi/drivers/aio_iiro_16.c
@@ -57,24 +57,11 @@ static const struct aio_iiro_16_board aio_iiro_16_boards[] = {
 	 .do_ = 16},
 };
 
-#define	thisboard	((const struct aio_iiro_16_board *) dev->board_ptr)
-
-struct aio_iiro_16_private {
-	int data;
-	struct pci_dev *pci_dev;
-	unsigned int ao_readback[2];
-};
-
-#define	devpriv	((struct aio_iiro_16_private *) dev->private)
-
 static int aio_iiro_16_dio_insn_bits_write(struct comedi_device *dev,
 					   struct comedi_subdevice *s,
 					   struct comedi_insn *insn,
 					   unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
@@ -85,7 +72,7 @@ static int aio_iiro_16_dio_insn_bits_write(struct comedi_device *dev,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int aio_iiro_16_dio_insn_bits_read(struct comedi_device *dev,
@@ -93,25 +80,24 @@ static int aio_iiro_16_dio_insn_bits_read(struct comedi_device *dev,
 					  struct comedi_insn *insn,
 					  unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = 0;
 	data[1] |= inb(dev->iobase + AIO_IIRO_16_INPUT_0_7);
 	data[1] |= inb(dev->iobase + AIO_IIRO_16_INPUT_8_15) << 8;
 
-	return 2;
+	return insn->n;
 }
 
 static int aio_iiro_16_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
+	const struct aio_iiro_16_board *board = comedi_board(dev);
 	int iobase;
 	struct comedi_subdevice *s;
+	int ret;
 
 	printk(KERN_INFO "comedi%d: aio_iiro_16: ", dev->minor);
 
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	iobase = it->options[0];
 
@@ -122,11 +108,9 @@ static int aio_iiro_16_attach(struct comedi_device *dev,
 
 	dev->iobase = iobase;
 
-	if (alloc_private(dev, sizeof(struct aio_iiro_16_private)) < 0)
-		return -ENOMEM;
-
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	s->type = COMEDI_SUBD_DIO;
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index c9c5d97b3ca..6c81e377262 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -210,21 +210,11 @@ order they appear in the channel list.
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #include "8255.h"
 #include "8253.h"
 
 #define DIO200_DRIVER_NAME	"amplc_dio200"
 
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA_MODULE
-#define CONFIG_COMEDI_AMPLC_DIO200_ISA
-#endif
-
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI_MODULE
-#define CONFIG_COMEDI_AMPLC_DIO200_PCI
-#endif
-
 /* PCI IDs */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI272 0x000a
@@ -282,12 +272,12 @@ enum dio200_model {
 };
 
 enum dio200_layout {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
 	pc212_layout,
 	pc214_layout,
 #endif
 	pc215_layout,
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
 	pc218_layout,
 #endif
 	pc272_layout
@@ -302,7 +292,7 @@ struct dio200_board {
 };
 
 static const struct dio200_board dio200_boards[] = {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
 	{
 	 .name = "pc212e",
 	 .bustype = isa_bustype,
@@ -334,7 +324,7 @@ static const struct dio200_board dio200_boards[] = {
 	 .layout = pc272_layout,
 	 },
 #endif
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI)
 	{
 	 .name = "pci215",
 	 .devid = PCI_DEVICE_ID_AMPLICON_PCI215,
@@ -377,7 +367,7 @@ struct dio200_layout_struct {
 };
 
 static const struct dio200_layout_struct dio200_layouts[] = {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
 	[pc212_layout] = {
 			  .n_subdevs = 6,
 			  .sdtype = {sd_8255, sd_8254, sd_8254, sd_8254,
@@ -406,7 +396,7 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 			  .has_int_sce = 1,
 			  .has_clk_gat_sce = 1,
 			  },
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA)
 	[pc218_layout] = {
 			  .n_subdevs = 7,
 			  .sdtype = {sd_8254, sd_8254, sd_8255, sd_8254,
@@ -429,40 +419,14 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 			  },
 };
 
-/*
- * PCI driver table.
- */
-
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, dio200_pci_table);
-#endif /* CONFIG_COMEDI_AMPLC_DIO200_PCI */
-
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct dio200_board *)dev->board_ptr)
-#define thislayout (&dio200_layouts[((struct dio200_board *) \
-		    dev->board_ptr)->layout])
-
 /* this structure is for data unique to this hardware driver.  If
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device struct.
  */
 struct dio200_private {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-	struct pci_dev *pci_dev;	/* PCI device */
-#endif
 	int intr_sd;
 };
 
-#define devpriv ((struct dio200_private *)dev->private)
-
 struct dio200_subdev_8254 {
 	unsigned long iobase;	/* Counter base address */
 	unsigned long clk_sce_iobase;	/* CLK_SCE base address */
@@ -486,157 +450,78 @@ struct dio200_subdev_intr {
 };
 
 /*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
+ * This function looks for a board matching the supplied PCI device.
  */
-static int dio200_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static void dio200_detach(struct comedi_device *dev);
-static struct comedi_driver driver_amplc_dio200 = {
-	.driver_name = DIO200_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = dio200_attach,
-	.detach = dio200_detach,
-	.board_name = &dio200_boards[0].name,
-	.offset = sizeof(struct dio200_board),
-	.num_names = ARRAY_SIZE(dio200_boards),
-};
-
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-static int __devinit driver_amplc_dio200_pci_probe(struct pci_dev *dev,
-						   const struct pci_device_id
-						   *ent)
-{
-	return comedi_pci_auto_config(dev, &driver_amplc_dio200);
-}
-
-static void __devexit driver_amplc_dio200_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_amplc_dio200_pci_driver = {
-	.id_table = dio200_pci_table,
-	.probe = &driver_amplc_dio200_pci_probe,
-	.remove = __devexit_p(&driver_amplc_dio200_pci_remove)
-};
-
-static int __init driver_amplc_dio200_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_amplc_dio200);
-	if (retval < 0)
-		return retval;
-
-	driver_amplc_dio200_pci_driver.name =
-	    (char *)driver_amplc_dio200.driver_name;
-	return pci_register_driver(&driver_amplc_dio200_pci_driver);
-}
-
-static void __exit driver_amplc_dio200_cleanup_module(void)
+static const struct dio200_board *
+dio200_find_pci_board(struct pci_dev *pci_dev)
 {
-	pci_unregister_driver(&driver_amplc_dio200_pci_driver);
-	comedi_driver_unregister(&driver_amplc_dio200);
-}
+	unsigned int i;
 
-module_init(driver_amplc_dio200_init_module);
-module_exit(driver_amplc_dio200_cleanup_module);
-#else
-static int __init driver_amplc_dio200_init_module(void)
-{
-	return comedi_driver_register(&driver_amplc_dio200);
-}
-
-static void __exit driver_amplc_dio200_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_amplc_dio200);
+	for (i = 0; i < ARRAY_SIZE(dio200_boards); i++)
+		if (dio200_boards[i].bustype == pci_bustype &&
+		    pci_dev->device == dio200_boards[i].devid)
+			return &dio200_boards[i];
+	return NULL;
 }
 
-module_init(driver_amplc_dio200_init_module);
-module_exit(driver_amplc_dio200_cleanup_module);
-#endif
-
 /*
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-static int
-dio200_find_pci(struct comedi_device *dev, int bus, int slot,
-		struct pci_dev **pci_dev_p)
+static struct pci_dev *dio200_find_pci_dev(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
 {
+	const struct dio200_board *thisboard = comedi_board(dev);
 	struct pci_dev *pci_dev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 
-	*pci_dev_p = NULL;
-
-	/* Look for matching PCI device. */
-	for (pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON, PCI_ANY_ID, NULL);
-	     pci_dev != NULL;
-	     pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON,
-				      PCI_ANY_ID, pci_dev)) {
-		/* If bus/slot specified, check them. */
+	for_each_pci_dev(pci_dev) {
 		if (bus || slot) {
-			if (bus != pci_dev->bus->number
-			    || slot != PCI_SLOT(pci_dev->devfn))
+			if (bus != pci_dev->bus->number ||
+			    slot != PCI_SLOT(pci_dev->devfn))
 				continue;
 		}
+		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
+			continue;
+
 		if (thisboard->model == anypci_model) {
-			/* Match any supported model. */
-			int i;
-
-			for (i = 0; i < ARRAY_SIZE(dio200_boards); i++) {
-				if (dio200_boards[i].bustype != pci_bustype)
-					continue;
-				if (pci_dev->device == dio200_boards[i].devid) {
-					/* Change board_ptr to matched board. */
-					dev->board_ptr = &dio200_boards[i];
-					break;
-				}
-			}
-			if (i == ARRAY_SIZE(dio200_boards))
+			/* Wildcard board matches any supported PCI board. */
+			const struct dio200_board *foundboard;
+
+			foundboard = dio200_find_pci_board(pci_dev);
+			if (foundboard == NULL)
 				continue;
+			/* Replace wildcard board_ptr. */
+			dev->board_ptr = foundboard;
 		} else {
 			/* Match specific model name. */
 			if (pci_dev->device != thisboard->devid)
 				continue;
 		}
-
-		/* Found a match. */
-		*pci_dev_p = pci_dev;
-		return 0;
-	}
-	/* No match found. */
-	if (bus || slot) {
-		printk(KERN_ERR
-		       "comedi%d: error! no %s found at pci %02x:%02x!\n",
-		       dev->minor, thisboard->name, bus, slot);
-	} else {
-		printk(KERN_ERR "comedi%d: error! no %s found!\n",
-		       dev->minor, thisboard->name);
+		return pci_dev;
 	}
-	return -EIO;
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
-#endif
 
 /*
  * This function checks and requests an I/O region, reporting an error
  * if there is a conflict.
  */
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
 static int
-dio200_request_region(unsigned minor, unsigned long from, unsigned long extent)
+dio200_request_region(struct comedi_device *dev,
+		      unsigned long from, unsigned long extent)
 {
 	if (!from || !request_region(from, extent, DIO200_DRIVER_NAME)) {
-		printk(KERN_ERR "comedi%d: I/O port conflict (%#lx,%lu)!\n",
-		       minor, from, extent);
+		dev_err(dev->class_dev, "I/O port conflict (%#lx,%lu)!\n",
+			from, extent);
 		return -EIO;
 	}
 	return 0;
 }
-#endif
 
 /*
  * 'insn_bits' function for an 'INTERRUPT' subdevice.
@@ -656,7 +541,7 @@ dio200_subdev_intr_insn_bits(struct comedi_device *dev,
 		data[0] = 0;
 	}
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -1030,8 +915,7 @@ dio200_subdev_intr_init(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
 	if (!subpriv) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+		dev_err(dev->class_dev, "error! out of memory!\n");
 		return -ENOMEM;
 	}
 	subpriv->iobase = iobase;
@@ -1080,6 +964,7 @@ dio200_subdev_intr_cleanup(struct comedi_device *dev,
 static irqreturn_t dio200_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct dio200_private *devpriv = dev->private;
 	int handled;
 
 	if (!dev->attached)
@@ -1284,8 +1169,7 @@ dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	subpriv = kzalloc(sizeof(*subpriv), GFP_KERNEL);
 	if (!subpriv) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+		dev_err(dev->class_dev, "error! out of memory!\n");
 		return -ENOMEM;
 	}
 
@@ -1337,99 +1221,52 @@ dio200_subdev_8254_cleanup(struct comedi_device *dev,
 	kfree(subpriv);
 }
 
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static void dio200_report_attach(struct comedi_device *dev, unsigned int irq)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	char tmpbuf[60];
+	int tmplen;
+
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA) &&
+	    thisboard->bustype == isa_bustype)
+		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
+				   "(base %#lx) ", dev->iobase);
+	else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI) &&
+		 thisboard->bustype == pci_bustype)
+		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
+				   "(pci %s) ", pci_name(pcidev));
+	else
+		tmplen = 0;
+	if (irq)
+		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
+				    "(irq %u%s) ", irq,
+				    (dev->irq ? "" : " UNAVAILABLE"));
+	else
+		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
+				    "(no irq) ");
+	dev_info(dev->class_dev, "%s %sattached\n", dev->board_name, tmpbuf);
+}
+
+static int dio200_common_attach(struct comedi_device *dev, unsigned long iobase,
+				unsigned int irq, unsigned long req_irq_flags)
 {
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct dio200_private *devpriv = dev->private;
+	const struct dio200_layout_struct *layout =
+		&dio200_layouts[thisboard->layout];
 	struct comedi_subdevice *s;
-	unsigned long iobase = 0;
-	unsigned int irq = 0;
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-	struct pci_dev *pci_dev = NULL;
-	int bus = 0, slot = 0;
-#endif
-	const struct dio200_layout_struct *layout;
-	int share_irq = 0;
 	int sdx;
-	unsigned n;
+	unsigned int n;
 	int ret;
 
-	printk(KERN_DEBUG "comedi%d: %s: attach\n", dev->minor,
-	       DIO200_DRIVER_NAME);
-
-	ret = alloc_private(dev, sizeof(struct dio200_private));
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
-		return ret;
-	}
-
-	/* Process options. */
-	switch (thisboard->bustype) {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
-	case isa_bustype:
-		iobase = it->options[0];
-		irq = it->options[1];
-		share_irq = 0;
-		break;
-#endif
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-	case pci_bustype:
-		bus = it->options[0];
-		slot = it->options[1];
-		share_irq = 1;
-
-		ret = dio200_find_pci(dev, bus, slot, &pci_dev);
-		if (ret < 0)
-			return ret;
-		devpriv->pci_dev = pci_dev;
-		break;
-#endif
-	default:
-		printk(KERN_ERR
-		       "comedi%d: %s: BUG! cannot determine board type!\n",
-		       dev->minor, DIO200_DRIVER_NAME);
-		return -EINVAL;
-		break;
-	}
-
 	devpriv->intr_sd = -1;
-
-	/* Enable device and reserve I/O spaces. */
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-	if (pci_dev) {
-		ret = comedi_pci_enable(pci_dev, DIO200_DRIVER_NAME);
-		if (ret < 0) {
-			printk(KERN_ERR
-			       "comedi%d: error! cannot enable PCI device and request regions!\n",
-			       dev->minor);
-			return ret;
-		}
-		iobase = pci_resource_start(pci_dev, 2);
-		irq = pci_dev->irq;
-	} else
-#endif
-	{
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
-		ret = dio200_request_region(dev->minor, iobase, DIO200_IO_SIZE);
-		if (ret < 0)
-			return ret;
-#endif
-	}
 	dev->iobase = iobase;
+	dev->board_name = thisboard->name;
 
-	layout = thislayout;
-
-	ret = alloc_subdevices(dev, layout->n_subdevs);
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+	ret = comedi_alloc_subdevices(dev, layout->n_subdevs);
+	if (ret)
 		return ret;
-	}
 
 	for (n = 0; n < dev->n_subdevices; n++) {
 		s = &dev->subdevices[n];
@@ -1441,7 +1278,6 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 						      layout->has_clk_gat_sce);
 			if (ret < 0)
 				return ret;
-
 			break;
 		case sd_8255:
 			/* digital i/o subdevice (8255) */
@@ -1449,7 +1285,6 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 					       iobase + layout->sdinfo[n]);
 			if (ret < 0)
 				return ret;
-
 			break;
 		case sd_intr:
 			/* 'INTERRUPT' subdevice */
@@ -1462,7 +1297,6 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 							      has_int_sce);
 				if (ret < 0)
 					return ret;
-
 				devpriv->intr_sd = n;
 			} else {
 				s->type = COMEDI_SUBD_UNUSED;
@@ -1473,60 +1307,125 @@ static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			break;
 		}
 	}
-
 	sdx = devpriv->intr_sd;
 	if (sdx >= 0 && sdx < dev->n_subdevices)
 		dev->read_subdev = &dev->subdevices[sdx];
-
-	dev->board_name = thisboard->name;
-
 	if (irq) {
-		unsigned long flags = share_irq ? IRQF_SHARED : 0;
-
-		if (request_irq(irq, dio200_interrupt, flags,
+		if (request_irq(irq, dio200_interrupt, req_irq_flags,
 				DIO200_DRIVER_NAME, dev) >= 0) {
 			dev->irq = irq;
 		} else {
-			printk(KERN_WARNING
-			       "comedi%d: warning! irq %u unavailable!\n",
-			       dev->minor, irq);
+			dev_warn(dev->class_dev,
+				 "warning! irq %u unavailable!\n", irq);
 		}
 	}
+	dio200_report_attach(dev, irq);
+	return 1;
+}
 
-	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	switch (thisboard->bustype) {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
-	case isa_bustype:
-		printk("(base %#lx) ", iobase);
-		break;
-#endif
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-	case pci_bustype:
-		printk("(pci %s) ", pci_name(pci_dev));
-		break;
-#endif
-	default:
-		break;
+static int dio200_pci_common_attach(struct comedi_device *dev,
+				    struct pci_dev *pci_dev)
+{
+	unsigned long iobase;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pci_dev->dev);
+
+	ret = comedi_pci_enable(pci_dev, DIO200_DRIVER_NAME);
+	if (ret < 0) {
+		dev_err(dev->class_dev,
+			"error! cannot enable PCI device and request regions!\n");
+		return ret;
 	}
-	if (irq)
-		printk("(irq %u%s) ", irq, (dev->irq ? "" : " UNAVAILABLE"));
-	else
-		printk("(no irq) ");
+	iobase = pci_resource_start(pci_dev, 2);
+	return dio200_common_attach(dev, iobase, pci_dev->irq, IRQF_SHARED);
+}
+
+/*
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
+ */
+static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	const struct dio200_board *thisboard = comedi_board(dev);
+	int ret;
 
-	printk("attached\n");
+	dev_info(dev->class_dev, DIO200_DRIVER_NAME ": attach\n");
 
-	return 1;
+	ret = alloc_private(dev, sizeof(struct dio200_private));
+	if (ret < 0) {
+		dev_err(dev->class_dev, "error! out of memory!\n");
+		return ret;
+	}
+
+	/* Process options and reserve resources according to bus type. */
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_ISA) &&
+	    thisboard->bustype == isa_bustype) {
+		unsigned long iobase;
+		unsigned int irq;
+
+		iobase = it->options[0];
+		irq = it->options[1];
+		ret = dio200_request_region(dev, iobase, DIO200_IO_SIZE);
+		if (ret < 0)
+			return ret;
+		return dio200_common_attach(dev, iobase, irq, 0);
+	} else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI) &&
+		   thisboard->bustype == pci_bustype) {
+		struct pci_dev *pci_dev;
+
+		pci_dev = dio200_find_pci_dev(dev, it);
+		if (!pci_dev)
+			return -EIO;
+		return dio200_pci_common_attach(dev, pci_dev);
+	} else {
+		dev_err(dev->class_dev, DIO200_DRIVER_NAME
+			": BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+}
+
+/*
+ * The attach_pci hook (if non-NULL) is called at PCI probe time in preference
+ * to the "manual" attach hook.  dev->board_ptr is NULL on entry.  There should
+ * be a board entry matching the supplied PCI device.
+ */
+static int __devinit dio200_attach_pci(struct comedi_device *dev,
+				       struct pci_dev *pci_dev)
+{
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI))
+		return -EINVAL;
+
+	dev_info(dev->class_dev, DIO200_DRIVER_NAME ": attach pci %s\n",
+		 pci_name(pci_dev));
+	ret = alloc_private(dev, sizeof(struct dio200_private));
+	if (ret < 0) {
+		dev_err(dev->class_dev, "error! out of memory!\n");
+		return ret;
+	}
+	dev->board_ptr = dio200_find_pci_board(pci_dev);
+	if (dev->board_ptr == NULL) {
+		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+	return dio200_pci_common_attach(dev, pci_dev);
 }
 
 static void dio200_detach(struct comedi_device *dev)
 {
+	const struct dio200_board *thisboard = comedi_board(dev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	const struct dio200_layout_struct *layout;
 	unsigned n;
 
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->subdevices) {
-		layout = thislayout;
+		layout = &dio200_layouts[thisboard->layout];
 		for (n = 0; n < dev->n_subdevices; n++) {
 			struct comedi_subdevice *s = &dev->subdevices[n];
 			switch (layout->sdtype[n]) {
@@ -1544,23 +1443,65 @@ static void dio200_detach(struct comedi_device *dev)
 			}
 		}
 	}
-	if (devpriv) {
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_PCI
-		if (devpriv->pci_dev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		} else
-#endif
-		{
-#ifdef CONFIG_COMEDI_AMPLC_DIO200_ISA
-			if (dev->iobase)
-				release_region(dev->iobase, DIO200_IO_SIZE);
-#endif
-		}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
+	} else {
+		if (dev->iobase)
+			release_region(dev->iobase, DIO200_IO_SIZE);
 	}
 }
 
+/*
+ * The struct comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attach/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
+ */
+static struct comedi_driver amplc_dio200_driver = {
+	.driver_name = DIO200_DRIVER_NAME,
+	.module = THIS_MODULE,
+	.attach = dio200_attach,
+	.attach_pci = dio200_attach_pci,
+	.detach = dio200_detach,
+	.board_name = &dio200_boards[0].name,
+	.offset = sizeof(struct dio200_board),
+	.num_names = ARRAY_SIZE(dio200_boards),
+};
+
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_DIO200_PCI)
+static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272) },
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, dio200_pci_table);
+
+static int __devinit amplc_dio200_pci_probe(struct pci_dev *dev,
+						   const struct pci_device_id
+						   *ent)
+{
+	return comedi_pci_auto_config(dev, &amplc_dio200_driver);
+}
+
+static void __devexit amplc_dio200_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static struct pci_driver amplc_dio200_pci_driver = {
+	.name = DIO200_DRIVER_NAME,
+	.id_table = dio200_pci_table,
+	.probe = &amplc_dio200_pci_probe,
+	.remove = __devexit_p(&amplc_dio200_pci_remove)
+};
+module_comedi_pci_driver(amplc_dio200_driver, amplc_dio200_pci_driver);
+#else
+module_comedi_driver(amplc_dio200_driver);
+#endif
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index 57ba3228b1a..aabba9886b7 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -56,21 +56,11 @@ unused.
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #include "8255.h"
 #include "plx9052.h"
 
 #define PC236_DRIVER_NAME	"amplc_pc236"
 
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA_MODULE
-#define CONFIG_COMEDI_AMPLC_PC236_ISA
-#endif
-
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI_MODULE
-#define CONFIG_COMEDI_AMPLC_PC236_PCI
-#endif
-
 /* PCI236 PCI configuration register information */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI236 0x0009
@@ -108,416 +98,114 @@ enum pc236_model { pc36at_model, pci236_model, anypci_model };
 
 struct pc236_board {
 	const char *name;
-	const char *fancy_name;
 	unsigned short devid;
 	enum pc236_bustype bustype;
 	enum pc236_model model;
 };
 static const struct pc236_board pc236_boards[] = {
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA)
 	{
-	 .name = "pc36at",
-	 .fancy_name = "PC36AT",
-	 .bustype = isa_bustype,
-	 .model = pc36at_model,
-	 },
+		.name = "pc36at",
+		.bustype = isa_bustype,
+		.model = pc36at_model,
+	},
 #endif
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI)
 	{
-	 .name = "pci236",
-	 .fancy_name = "PCI236",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI236,
-	 .bustype = pci_bustype,
-	 .model = pci236_model,
-	 },
+		.name = "pci236",
+		.devid = PCI_DEVICE_ID_AMPLICON_PCI236,
+		.bustype = pci_bustype,
+		.model = pci236_model,
+	},
 	{
-	 .name = PC236_DRIVER_NAME,
-	 .fancy_name = PC236_DRIVER_NAME,
-	 .devid = PCI_DEVICE_ID_INVALID,
-	 .bustype = pci_bustype,
-	 .model = anypci_model,	/* wildcard */
-	 },
+		.name = PC236_DRIVER_NAME,
+		.devid = PCI_DEVICE_ID_INVALID,
+		.bustype = pci_bustype,
+		.model = anypci_model,	/* wildcard */
+	},
 #endif
 };
 
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-static DEFINE_PCI_DEVICE_TABLE(pc236_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI236) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, pc236_pci_table);
-#endif /* CONFIG_COMEDI_AMPLC_PC236_PCI */
-
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct pc236_board *)dev->board_ptr)
-
 /* this structure is for data unique to this hardware driver.  If
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device struct.
  */
 struct pc236_private {
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	/* PCI device */
-	struct pci_dev *pci_dev;
 	unsigned long lcr_iobase; /* PLX PCI9052 config registers in PCIBAR1 */
-#endif
 	int enable_irq;
 };
 
-#define devpriv ((struct pc236_private *)dev->private)
-
 /*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
+ * This function looks for a board matching the supplied PCI device.
  */
-static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static void pc236_detach(struct comedi_device *dev);
-static struct comedi_driver driver_amplc_pc236 = {
-	.driver_name = PC236_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = pc236_attach,
-	.detach = pc236_detach,
-	.board_name = &pc236_boards[0].name,
-	.offset = sizeof(struct pc236_board),
-	.num_names = ARRAY_SIZE(pc236_boards),
-};
-
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-static int __devinit driver_amplc_pc236_pci_probe(struct pci_dev *dev,
-						  const struct pci_device_id
-						  *ent)
+static const struct pc236_board *pc236_find_pci_board(struct pci_dev *pci_dev)
 {
-	return comedi_pci_auto_config(dev, &driver_amplc_pc236);
-}
+	unsigned int i;
 
-static void __devexit driver_amplc_pc236_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
+	for (i = 0; i < ARRAY_SIZE(pc236_boards); i++)
+		if (pc236_boards[i].bustype == pci_bustype &&
+		    pci_dev->device == pc236_boards[i].devid)
+			return &pc236_boards[i];
+	return NULL;
 }
 
-static struct pci_driver driver_amplc_pc236_pci_driver = {
-	.id_table = pc236_pci_table,
-	.probe = &driver_amplc_pc236_pci_probe,
-	.remove = __devexit_p(&driver_amplc_pc236_pci_remove)
-};
-
-static int __init driver_amplc_pc236_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_amplc_pc236);
-	if (retval < 0)
-		return retval;
-
-	driver_amplc_pc236_pci_driver.name =
-	    (char *)driver_amplc_pc236.driver_name;
-	return pci_register_driver(&driver_amplc_pc236_pci_driver);
-}
-
-static void __exit driver_amplc_pc236_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_amplc_pc236_pci_driver);
-	comedi_driver_unregister(&driver_amplc_pc236);
-}
-
-module_init(driver_amplc_pc236_init_module);
-module_exit(driver_amplc_pc236_cleanup_module);
-#else
-static int __init driver_amplc_pc236_init_module(void)
-{
-	return comedi_driver_register(&driver_amplc_pc236);
-}
-
-static void __exit driver_amplc_pc236_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_amplc_pc236);
-}
-
-module_init(driver_amplc_pc236_init_module);
-module_exit(driver_amplc_pc236_cleanup_module);
-#endif
-
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
-static int pc236_request_region(unsigned minor, unsigned long from,
-				unsigned long extent);
-#endif
-static void pc236_intr_disable(struct comedi_device *dev);
-static void pc236_intr_enable(struct comedi_device *dev);
-static int pc236_intr_check(struct comedi_device *dev);
-static int pc236_intr_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int pc236_intr_cmdtest(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_cmd *cmd);
-static int pc236_intr_cmd(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-static int pc236_intr_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s);
-static irqreturn_t pc236_interrupt(int irq, void *d);
-
 /*
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-static int
-pc236_find_pci(struct comedi_device *dev, int bus, int slot,
-	       struct pci_dev **pci_dev_p)
+static struct pci_dev *pc236_find_pci_dev(struct comedi_device *dev,
+					  struct comedi_devconfig *it)
 {
+	const struct pc236_board *thisboard = comedi_board(dev);
 	struct pci_dev *pci_dev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 
-	*pci_dev_p = NULL;
-
-	/* Look for matching PCI device. */
-	for (pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON, PCI_ANY_ID, NULL);
-	     pci_dev != NULL;
-	     pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON,
-				      PCI_ANY_ID, pci_dev)) {
-		/* If bus/slot specified, check them. */
+	for_each_pci_dev(pci_dev) {
 		if (bus || slot) {
-			if (bus != pci_dev->bus->number
-			    || slot != PCI_SLOT(pci_dev->devfn))
+			if (bus != pci_dev->bus->number ||
+			    slot != PCI_SLOT(pci_dev->devfn))
 				continue;
 		}
+		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
+			continue;
+
 		if (thisboard->model == anypci_model) {
-			/* Match any supported model. */
-			int i;
-
-			for (i = 0; i < ARRAY_SIZE(pc236_boards); i++) {
-				if (pc236_boards[i].bustype != pci_bustype)
-					continue;
-				if (pci_dev->device == pc236_boards[i].devid) {
-					/* Change board_ptr to matched board. */
-					dev->board_ptr = &pc236_boards[i];
-					break;
-				}
-			}
-			if (i == ARRAY_SIZE(pc236_boards))
+			/* Wildcard board matches any supported PCI board. */
+			const struct pc236_board *foundboard;
+
+			foundboard = pc236_find_pci_board(pci_dev);
+			if (foundboard == NULL)
 				continue;
+			/* Replace wildcard board_ptr. */
+			dev->board_ptr = foundboard;
 		} else {
 			/* Match specific model name. */
 			if (pci_dev->device != thisboard->devid)
 				continue;
 		}
-
-		/* Found a match. */
-		*pci_dev_p = pci_dev;
-		return 0;
-	}
-	/* No match found. */
-	if (bus || slot) {
-		printk(KERN_ERR
-		       "comedi%d: error! no %s found at pci %02x:%02x!\n",
-		       dev->minor, thisboard->name, bus, slot);
-	} else {
-		printk(KERN_ERR "comedi%d: error! no %s found!\n",
-		       dev->minor, thisboard->name);
-	}
-	return -EIO;
-}
-#endif
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase = 0;
-	unsigned int irq = 0;
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	struct pci_dev *pci_dev = NULL;
-	int bus = 0, slot = 0;
-#endif
-	int share_irq = 0;
-	int ret;
-
-	printk(KERN_DEBUG "comedi%d: %s: attach\n", dev->minor,
-	       PC236_DRIVER_NAME);
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	ret = alloc_private(dev, sizeof(struct pc236_private));
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
-		return ret;
-	}
-	/* Process options. */
-	switch (thisboard->bustype) {
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
-	case isa_bustype:
-		iobase = it->options[0];
-		irq = it->options[1];
-		share_irq = 0;
-		break;
-#endif
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	case pci_bustype:
-		bus = it->options[0];
-		slot = it->options[1];
-		share_irq = 1;
-
-		ret = pc236_find_pci(dev, bus, slot, &pci_dev);
-		if (ret < 0)
-			return ret;
-		devpriv->pci_dev = pci_dev;
-		break;
-#endif
-	default:
-		printk(KERN_ERR
-		       "comedi%d: %s: BUG! cannot determine board type!\n",
-		       dev->minor, PC236_DRIVER_NAME);
-		return -EINVAL;
-		break;
-	}
-
-/*
- * Initialize dev->board_name.
- */
-	dev->board_name = thisboard->name;
-
-	/* Enable device and reserve I/O spaces. */
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	if (pci_dev) {
-
-		ret = comedi_pci_enable(pci_dev, PC236_DRIVER_NAME);
-		if (ret < 0) {
-			printk(KERN_ERR
-			       "comedi%d: error! cannot enable PCI device and request regions!\n",
-			       dev->minor);
-			return ret;
-		}
-		devpriv->lcr_iobase = pci_resource_start(pci_dev, 1);
-		iobase = pci_resource_start(pci_dev, 2);
-		irq = pci_dev->irq;
-	} else
-#endif
-	{
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
-		ret = pc236_request_region(dev->minor, iobase, PC236_IO_SIZE);
-		if (ret < 0)
-			return ret;
-#endif
-	}
-	dev->iobase = iobase;
-
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	ret = alloc_subdevices(dev, 2);
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
-		return ret;
-	}
-
-	s = dev->subdevices + 0;
-	/* digital i/o subdevice (8255) */
-	ret = subdev_8255_init(dev, s, NULL, iobase);
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
-		return ret;
-	}
-	s = dev->subdevices + 1;
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_UNUSED;
-	pc236_intr_disable(dev);
-	if (irq) {
-		unsigned long flags = share_irq ? IRQF_SHARED : 0;
-
-		if (request_irq(irq, pc236_interrupt, flags,
-				PC236_DRIVER_NAME, dev) >= 0) {
-			dev->irq = irq;
-			s->type = COMEDI_SUBD_DI;
-			s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
-			s->n_chan = 1;
-			s->maxdata = 1;
-			s->range_table = &range_digital;
-			s->insn_bits = pc236_intr_insn;
-			s->do_cmdtest = pc236_intr_cmdtest;
-			s->do_cmd = pc236_intr_cmd;
-			s->cancel = pc236_intr_cancel;
-		}
-	}
-	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	switch (thisboard->bustype) {
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
-	case isa_bustype:
-		printk("(base %#lx) ", iobase);
-		break;
-#endif
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	case pci_bustype:
-		printk("(pci %s) ", pci_name(pci_dev));
-		break;
-#endif
-	default:
-		break;
-	}
-	if (irq)
-		printk("(irq %u%s) ", irq, (dev->irq ? "" : " UNAVAILABLE"));
-	else
-		printk("(no irq) ");
-
-	printk("attached\n");
-
-	return 1;
-}
-
-static void pc236_detach(struct comedi_device *dev)
-{
-	if (devpriv)
-		pc236_intr_disable(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 0);
-	if (devpriv) {
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-		if (devpriv->pci_dev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		} else
-#endif
-		{
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
-			if (dev->iobase)
-				release_region(dev->iobase, PC236_IO_SIZE);
-#endif
-		}
+		return pci_dev;
 	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
 
 /*
  * This function checks and requests an I/O region, reporting an error
  * if there is a conflict.
  */
-#ifdef CONFIG_COMEDI_AMPLC_PC236_ISA
-static int pc236_request_region(unsigned minor, unsigned long from,
+static int pc236_request_region(struct comedi_device *dev, unsigned long from,
 				unsigned long extent)
 {
 	if (!from || !request_region(from, extent, PC236_DRIVER_NAME)) {
-		printk(KERN_ERR "comedi%d: I/O port conflict (%#lx,%lu)!\n",
-		       minor, from, extent);
+		dev_err(dev->class_dev, "I/O port conflict (%#lx,%lu)!\n",
+		       from, extent);
 		return -EIO;
 	}
 	return 0;
 }
-#endif
 
 /*
  * This function is called to mark the interrupt as disabled (no command
@@ -526,14 +214,13 @@ static int pc236_request_region(unsigned minor, unsigned long from,
  */
 static void pc236_intr_disable(struct comedi_device *dev)
 {
+	struct pc236_private *devpriv = dev->private;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = 0;
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	if (devpriv->lcr_iobase)
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) && devpriv->lcr_iobase)
 		outl(PCI236_INTR_DISABLE, devpriv->lcr_iobase + PLX9052_INTCSR);
-#endif
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 }
 
@@ -544,14 +231,13 @@ static void pc236_intr_disable(struct comedi_device *dev)
  */
 static void pc236_intr_enable(struct comedi_device *dev)
 {
+	struct pc236_private *devpriv = dev->private;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = 1;
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-	if (devpriv->lcr_iobase)
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) && devpriv->lcr_iobase)
 		outl(PCI236_INTR_ENABLE, devpriv->lcr_iobase + PLX9052_INTCSR);
-#endif
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 }
 
@@ -564,14 +250,15 @@ static void pc236_intr_enable(struct comedi_device *dev)
  */
 static int pc236_intr_check(struct comedi_device *dev)
 {
+	struct pc236_private *devpriv = dev->private;
 	int retval = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	if (devpriv->enable_irq) {
 		retval = 1;
-#ifdef CONFIG_COMEDI_AMPLC_PC236_PCI
-		if (devpriv->lcr_iobase) {
+		if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) &&
+		    devpriv->lcr_iobase) {
 			if ((inl(devpriv->lcr_iobase + PLX9052_INTCSR)
 			     & PLX9052_INTCSR_LI1STAT_MASK)
 			    == PLX9052_INTCSR_LI1STAT_INACTIVE) {
@@ -582,7 +269,6 @@ static int pc236_intr_check(struct comedi_device *dev)
 				     devpriv->lcr_iobase + PLX9052_INTCSR);
 			}
 		}
-#endif
 	}
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
@@ -598,7 +284,7 @@ static int pc236_intr_insn(struct comedi_device *dev,
 			   unsigned int *data)
 {
 	data[1] = 0;
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -721,6 +407,236 @@ static irqreturn_t pc236_interrupt(int irq, void *d)
 	return IRQ_RETVAL(handled);
 }
 
+static void pc236_report_attach(struct comedi_device *dev, unsigned int irq)
+{
+	const struct pc236_board *thisboard = comedi_board(dev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	char tmpbuf[60];
+	int tmplen;
+
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA) &&
+	    thisboard->bustype == isa_bustype)
+		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
+				   "(base %#lx) ", dev->iobase);
+	else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) &&
+		 thisboard->bustype == pci_bustype) {
+		tmplen = scnprintf(tmpbuf, sizeof(tmpbuf),
+				   "(pci %s) ", pci_name(pcidev));
+	} else
+		tmplen = 0;
+	if (irq)
+		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
+				    "(irq %u%s) ", irq,
+				    (dev->irq ? "" : " UNAVAILABLE"));
+	else
+		tmplen += scnprintf(&tmpbuf[tmplen], sizeof(tmpbuf) - tmplen,
+				    "(no irq) ");
+	dev_info(dev->class_dev, "%s %sattached\n",
+		 dev->board_name, tmpbuf);
+}
+
+static int pc236_common_attach(struct comedi_device *dev, unsigned long iobase,
+			       unsigned int irq, unsigned long req_irq_flags)
+{
+	const struct pc236_board *thisboard = comedi_board(dev);
+	struct comedi_subdevice *s;
+	int ret;
+
+	dev->board_name = thisboard->name;
+	dev->iobase = iobase;
+
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
+
+	s = dev->subdevices + 0;
+	/* digital i/o subdevice (8255) */
+	ret = subdev_8255_init(dev, s, NULL, iobase);
+	if (ret < 0) {
+		dev_err(dev->class_dev, "error! out of memory!\n");
+		return ret;
+	}
+	s = dev->subdevices + 1;
+	dev->read_subdev = s;
+	s->type = COMEDI_SUBD_UNUSED;
+	pc236_intr_disable(dev);
+	if (irq) {
+		if (request_irq(irq, pc236_interrupt, req_irq_flags,
+				PC236_DRIVER_NAME, dev) >= 0) {
+			dev->irq = irq;
+			s->type = COMEDI_SUBD_DI;
+			s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
+			s->n_chan = 1;
+			s->maxdata = 1;
+			s->range_table = &range_digital;
+			s->insn_bits = pc236_intr_insn;
+			s->do_cmdtest = pc236_intr_cmdtest;
+			s->do_cmd = pc236_intr_cmd;
+			s->cancel = pc236_intr_cancel;
+		}
+	}
+	pc236_report_attach(dev, irq);
+	return 1;
+}
+
+static int pc236_pci_common_attach(struct comedi_device *dev,
+				   struct pci_dev *pci_dev)
+{
+	struct pc236_private *devpriv = dev->private;
+	unsigned long iobase;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pci_dev->dev);
+
+	ret = comedi_pci_enable(pci_dev, PC236_DRIVER_NAME);
+	if (ret < 0) {
+		dev_err(dev->class_dev,
+			"error! cannot enable PCI device and request regions!\n");
+		return ret;
+	}
+	devpriv->lcr_iobase = pci_resource_start(pci_dev, 1);
+	iobase = pci_resource_start(pci_dev, 2);
+	return pc236_common_attach(dev, iobase, pci_dev->irq, IRQF_SHARED);
+}
+
+/*
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
+ */
+static int pc236_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	const struct pc236_board *thisboard = comedi_board(dev);
+	int ret;
+
+	dev_info(dev->class_dev, PC236_DRIVER_NAME ": attach\n");
+	ret = alloc_private(dev, sizeof(struct pc236_private));
+	if (ret < 0) {
+		dev_err(dev->class_dev, "error! out of memory!\n");
+		return ret;
+	}
+	/* Process options according to bus type. */
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_ISA) &&
+	    thisboard->bustype == isa_bustype) {
+		unsigned long iobase = it->options[0];
+		unsigned int irq = it->options[1];
+		ret = pc236_request_region(dev, iobase, PC236_IO_SIZE);
+		if (ret < 0)
+			return ret;
+		return pc236_common_attach(dev, iobase, irq, 0);
+	} else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI) &&
+		   thisboard->bustype == pci_bustype) {
+		struct pci_dev *pci_dev;
+
+		pci_dev = pc236_find_pci_dev(dev, it);
+		if (!pci_dev)
+			return -EIO;
+		return pc236_pci_common_attach(dev, pci_dev);
+	} else {
+		dev_err(dev->class_dev, PC236_DRIVER_NAME
+			": BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+}
+
+/*
+ * The attach_pci hook (if non-NULL) is called at PCI probe time in preference
+ * to the "manual" attach hook.  dev->board_ptr is NULL on entry.  There should
+ * be a board entry matching the supplied PCI device.
+ */
+static int __devinit pc236_attach_pci(struct comedi_device *dev,
+				      struct pci_dev *pci_dev)
+{
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI))
+		return -EINVAL;
+
+	dev_info(dev->class_dev, PC236_DRIVER_NAME ": attach pci %s\n",
+		 pci_name(pci_dev));
+	ret = alloc_private(dev, sizeof(struct pc236_private));
+	if (ret < 0) {
+		dev_err(dev->class_dev, "error! out of memory!\n");
+		return ret;
+	}
+	dev->board_ptr = pc236_find_pci_board(pci_dev);
+	if (dev->board_ptr == NULL) {
+		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+	return pc236_pci_common_attach(dev, pci_dev);
+}
+
+static void pc236_detach(struct comedi_device *dev)
+{
+	struct pc236_private *devpriv = dev->private;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (devpriv)
+		pc236_intr_disable(dev);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->subdevices)
+		subdev_8255_cleanup(dev, dev->subdevices + 0);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
+	} else {
+		if (dev->iobase)
+			release_region(dev->iobase, PC236_IO_SIZE);
+	}
+}
+
+/*
+ * The struct comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attach/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
+ */
+static struct comedi_driver amplc_pc236_driver = {
+	.driver_name = PC236_DRIVER_NAME,
+	.module = THIS_MODULE,
+	.attach = pc236_attach,
+	.attach_pci = pc236_attach_pci,
+	.detach = pc236_detach,
+	.board_name = &pc236_boards[0].name,
+	.offset = sizeof(struct pc236_board),
+	.num_names = ARRAY_SIZE(pc236_boards),
+};
+
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC236_PCI)
+static DEFINE_PCI_DEVICE_TABLE(pc236_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI236) },
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, pc236_pci_table);
+
+static int __devinit amplc_pc236_pci_probe(struct pci_dev *dev,
+					   const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &amplc_pc236_driver);
+}
+
+static void __devexit amplc_pc236_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static struct pci_driver amplc_pc236_pci_driver = {
+	.name = PC236_DRIVER_NAME,
+	.id_table = pc236_pci_table,
+	.probe = &amplc_pc236_pci_probe,
+	.remove = __devexit_p(&amplc_pc236_pci_remove)
+};
+
+module_comedi_pci_driver(amplc_pc236_driver, amplc_pc236_pci_driver);
+#else
+module_comedi_driver(amplc_pc236_driver);
+#endif
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index 974d7450051..40ec1ffebba 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -46,18 +46,8 @@ The state of the outputs can be read.
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #define PC263_DRIVER_NAME	"amplc_pc263"
 
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA_MODULE
-#define CONFIG_COMEDI_AMPLC_PC263_ISA
-#endif
-
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI_MODULE
-#define CONFIG_COMEDI_AMPLC_PC263_PCI
-#endif
-
 /* PCI263 PCI configuration register information */
 #define PCI_VENDOR_ID_AMPLICON 0x14dc
 #define PCI_DEVICE_ID_AMPLICON_PCI263 0x000c
@@ -75,422 +65,301 @@ enum pc263_model { pc263_model, pci263_model, anypci_model };
 
 struct pc263_board {
 	const char *name;
-	const char *fancy_name;
 	unsigned short devid;
 	enum pc263_bustype bustype;
 	enum pc263_model model;
 };
 static const struct pc263_board pc263_boards[] = {
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA)
 	{
-	 .name = "pc263",
-	 .fancy_name = "PC263",
-	 .bustype = isa_bustype,
-	 .model = pc263_model,
-	 },
+		.name = "pc263",
+		.bustype = isa_bustype,
+		.model = pc263_model,
+	},
 #endif
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI)
 	{
-	 .name = "pci263",
-	 .fancy_name = "PCI263",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI263,
-	 .bustype = pci_bustype,
-	 .model = pci263_model,
-	 },
+		.name = "pci263",
+		.devid = PCI_DEVICE_ID_AMPLICON_PCI263,
+		.bustype = pci_bustype,
+		.model = pci263_model,
+	},
 	{
-	 .name = PC263_DRIVER_NAME,
-	 .fancy_name = PC263_DRIVER_NAME,
-	 .devid = PCI_DEVICE_ID_INVALID,
-	 .bustype = pci_bustype,
-	 .model = anypci_model,	/* wildcard */
-	 },
+		.name = PC263_DRIVER_NAME,
+		.devid = PCI_DEVICE_ID_INVALID,
+		.bustype = pci_bustype,
+		.model = anypci_model,	/* wildcard */
+	},
 #endif
 };
 
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-static DEFINE_PCI_DEVICE_TABLE(pc263_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, pc263_pci_table);
-#endif /* CONFIG_COMEDI_AMPLC_PC263_PCI */
-
 /*
- * Useful for shorthand access to the particular board structure
+ * This function looks for a board matching the supplied PCI device.
  */
-#define thisboard ((const struct pc263_board *)dev->board_ptr)
-
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.
-*/
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-struct pc263_private {
-	/* PCI device. */
-	struct pci_dev *pci_dev;
-};
-
-#define devpriv ((struct pc263_private *)dev->private)
-#endif /* CONFIG_COMEDI_AMPLC_PC263_PCI */
+static const struct pc263_board *pc263_find_pci_board(struct pci_dev *pci_dev)
+{
+	unsigned int i;
 
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it);
-static void pc263_detach(struct comedi_device *dev);
-static struct comedi_driver driver_amplc_pc263 = {
-	.driver_name = PC263_DRIVER_NAME,
-	.module = THIS_MODULE,
-	.attach = pc263_attach,
-	.detach = pc263_detach,
-	.board_name = &pc263_boards[0].name,
-	.offset = sizeof(struct pc263_board),
-	.num_names = ARRAY_SIZE(pc263_boards),
-};
+	for (i = 0; i < ARRAY_SIZE(pc263_boards); i++)
+		if (pc263_boards[i].bustype == pci_bustype &&
+		    pci_dev->device == pc263_boards[i].devid)
+			return &pc263_boards[i];
+	return NULL;
+}
 
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
-static int pc263_request_region(unsigned minor, unsigned long from,
-				unsigned long extent);
-#endif
-static int pc263_dio_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int pc263_dio_insn_config(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
 
 /*
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-static int
-pc263_find_pci(struct comedi_device *dev, int bus, int slot,
-	       struct pci_dev **pci_dev_p)
+static struct pci_dev *pc263_find_pci_dev(struct comedi_device *dev,
+					  struct comedi_devconfig *it)
 {
+	const struct pc263_board *thisboard = comedi_board(dev);
 	struct pci_dev *pci_dev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 
-	*pci_dev_p = NULL;
-
-	/* Look for matching PCI device. */
-	for (pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON, PCI_ANY_ID, NULL);
-	     pci_dev != NULL;
-	     pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON,
-				      PCI_ANY_ID, pci_dev)) {
-		/* If bus/slot specified, check them. */
+	for_each_pci_dev(pci_dev) {
 		if (bus || slot) {
-			if (bus != pci_dev->bus->number
-			    || slot != PCI_SLOT(pci_dev->devfn))
+			if (bus != pci_dev->bus->number ||
+			    slot != PCI_SLOT(pci_dev->devfn))
 				continue;
 		}
+		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
+			continue;
+
 		if (thisboard->model == anypci_model) {
-			/* Match any supported model. */
-			int i;
-
-			for (i = 0; i < ARRAY_SIZE(pc263_boards); i++) {
-				if (pc263_boards[i].bustype != pci_bustype)
-					continue;
-				if (pci_dev->device == pc263_boards[i].devid) {
-					/* Change board_ptr to matched board. */
-					dev->board_ptr = &pc263_boards[i];
-					break;
-				}
-			}
-			if (i == ARRAY_SIZE(pc263_boards))
+			/* Wildcard board matches any supported PCI board. */
+			const struct pc263_board *foundboard;
+
+			foundboard = pc263_find_pci_board(pci_dev);
+			if (foundboard == NULL)
 				continue;
+			/* Replace wildcard board_ptr. */
+			dev->board_ptr = thisboard = foundboard;
 		} else {
 			/* Match specific model name. */
 			if (pci_dev->device != thisboard->devid)
 				continue;
 		}
-
-		/* Found a match. */
-		*pci_dev_p = pci_dev;
-		return 0;
-	}
-	/* No match found. */
-	if (bus || slot) {
-		printk(KERN_ERR
-		       "comedi%d: error! no %s found at pci %02x:%02x!\n",
-		       dev->minor, thisboard->name, bus, slot);
-	} else {
-		printk(KERN_ERR "comedi%d: error! no %s found!\n",
-		       dev->minor, thisboard->name);
+		return pci_dev;
 	}
-	return -EIO;
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
-#endif
-
 /*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
+ * This function checks and requests an I/O region, reporting an error
+ * if there is a conflict.
  */
-static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int pc263_request_region(struct comedi_device *dev, unsigned long from,
+				unsigned long extent)
 {
-	struct comedi_subdevice *s;
-	unsigned long iobase = 0;
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-	struct pci_dev *pci_dev = NULL;
-	int bus = 0, slot = 0;
-#endif
-	int ret;
-
-	printk(KERN_DEBUG "comedi%d: %s: attach\n", dev->minor,
-	       PC263_DRIVER_NAME);
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-	ret = alloc_private(dev, sizeof(struct pc263_private));
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
-		return ret;
+	if (!from || !request_region(from, extent, PC263_DRIVER_NAME)) {
+		dev_err(dev->class_dev, "I/O port conflict (%#lx,%lu)!\n",
+			from, extent);
+		return -EIO;
 	}
-#endif
-	/* Process options. */
-	switch (thisboard->bustype) {
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
-	case isa_bustype:
-		iobase = it->options[0];
-		break;
-#endif
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-	case pci_bustype:
-		bus = it->options[0];
-		slot = it->options[1];
+	return 0;
+}
 
-		ret = pc263_find_pci(dev, bus, slot, &pci_dev);
-		if (ret < 0)
-			return ret;
-		devpriv->pci_dev = pci_dev;
-		break;
-#endif
-	default:
-		printk(KERN_ERR
-		       "comedi%d: %s: BUG! cannot determine board type!\n",
-		       dev->minor, PC263_DRIVER_NAME);
-		return -EINVAL;
-		break;
+static int pc263_do_insn_bits(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn, unsigned int *data)
+{
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+		/* Write out the new digital output lines */
+		outb(s->state & 0xFF, dev->iobase);
+		outb(s->state >> 8, dev->iobase + 1);
 	}
+	return insn->n;
+}
 
-/*
- * Initialize dev->board_name.
- */
-	dev->board_name = thisboard->name;
+static void pc263_report_attach(struct comedi_device *dev)
+{
+	const struct pc263_board *thisboard = comedi_board(dev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	char tmpbuf[40];
+
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA) &&
+	    thisboard->bustype == isa_bustype)
+		snprintf(tmpbuf, sizeof(tmpbuf), "(base %#lx) ", dev->iobase);
+	else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI) &&
+		 thisboard->bustype == pci_bustype)
+		snprintf(tmpbuf, sizeof(tmpbuf), "(pci %s) ",
+			 pci_name(pcidev));
+	else
+		tmpbuf[0] = '\0';
+	dev_info(dev->class_dev, "%s %sattached\n", dev->board_name, tmpbuf);
+}
 
-	/* Enable device and reserve I/O spaces. */
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-	if (pci_dev) {
-		ret = comedi_pci_enable(pci_dev, PC263_DRIVER_NAME);
-		if (ret < 0) {
-			printk(KERN_ERR
-			       "comedi%d: error! cannot enable PCI device and "
-				"request regions!\n",
-			       dev->minor);
-			return ret;
-		}
-		iobase = pci_resource_start(pci_dev, 2);
-	} else
-#endif
-	{
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
-		ret = pc263_request_region(dev->minor, iobase, PC263_IO_SIZE);
-		if (ret < 0)
-			return ret;
-#endif
-	}
+static int pc263_common_attach(struct comedi_device *dev, unsigned long iobase)
+{
+	const struct pc263_board *thisboard = comedi_board(dev);
+	struct comedi_subdevice *s;
+	int ret;
+
+	dev->board_name = thisboard->name;
 	dev->iobase = iobase;
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
-	}
 
 	s = dev->subdevices + 0;
-	/* digital i/o subdevice */
-	s->type = COMEDI_SUBD_DIO;
+	/* digital output subdevice */
+	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 	s->n_chan = 16;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
-	s->insn_bits = pc263_dio_insn_bits;
-	s->insn_config = pc263_dio_insn_config;
-	/* all outputs */
-	s->io_bits = 0xffff;
+	s->insn_bits = pc263_do_insn_bits;
 	/* read initial relay state */
-	s->state = inb(dev->iobase);
-	s->state = s->state | (inb(dev->iobase) << 8);
-
-	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	switch (thisboard->bustype) {
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
-	case isa_bustype:
-		printk("(base %#lx) ", iobase);
-		break;
-#endif
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-		printk("(pci %s) ", pci_name(pci_dev));
-		break;
-#endif
-	default:
-		break;
-	}
-
-	printk("attached\n");
+	s->state = inb(dev->iobase) | (inb(dev->iobase + 1) << 8);
 
+	pc263_report_attach(dev);
 	return 1;
 }
 
-static void pc263_detach(struct comedi_device *dev)
+static int pc263_pci_common_attach(struct comedi_device *dev,
+				   struct pci_dev *pci_dev)
 {
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-	if (devpriv)
-#endif
-	{
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-		if (devpriv->pci_dev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		} else
-#endif
-		{
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
-			if (dev->iobase)
-				release_region(dev->iobase, PC263_IO_SIZE);
-#endif
-		}
+	unsigned long iobase;
+	int ret;
+
+	comedi_set_hw_dev(dev, &pci_dev->dev);
+
+	ret = comedi_pci_enable(pci_dev, PC263_DRIVER_NAME);
+	if (ret < 0) {
+		dev_err(dev->class_dev,
+			"error! cannot enable PCI device and request regions!\n");
+		return ret;
 	}
+	iobase = pci_resource_start(pci_dev, 2);
+	return pc263_common_attach(dev, iobase);
 }
 
 /*
- * This function checks and requests an I/O region, reporting an error
- * if there is a conflict.
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
  */
-#ifdef CONFIG_COMEDI_AMPLC_PC263_ISA
-static int pc263_request_region(unsigned minor, unsigned long from,
-				unsigned long extent)
+static int pc263_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	if (!from || !request_region(from, extent, PC263_DRIVER_NAME)) {
-		printk(KERN_ERR "comedi%d: I/O port conflict (%#lx,%lu)!\n",
-		       minor, from, extent);
-		return -EIO;
+	const struct pc263_board *thisboard = comedi_board(dev);
+	int ret;
+
+	dev_info(dev->class_dev, PC263_DRIVER_NAME ": attach\n");
+
+	/* Process options and reserve resources according to bus type. */
+	if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_ISA) &&
+	    thisboard->bustype == isa_bustype) {
+		unsigned long iobase = it->options[0];
+		ret = pc263_request_region(dev, iobase, PC263_IO_SIZE);
+		if (ret < 0)
+			return ret;
+		return pc263_common_attach(dev, iobase);
+	} else if (IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI) &&
+		   thisboard->bustype == pci_bustype) {
+		struct pci_dev *pci_dev;
+
+		pci_dev = pc263_find_pci_dev(dev, it);
+		if (!pci_dev)
+			return -EIO;
+		return pc263_pci_common_attach(dev, pci_dev);
+	} else {
+		dev_err(dev->class_dev, PC263_DRIVER_NAME
+			": BUG! cannot determine board type!\n");
+		return -EINVAL;
 	}
-	return 0;
 }
-#endif
-
-/* DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The
- * comedi core can convert between insn_bits and insn_read/write */
-static int pc263_dio_insn_bits(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+/*
+ * The attach_pci hook (if non-NULL) is called at PCI probe time in preference
+ * to the "manual" attach hook.  dev->board_ptr is NULL on entry.  There should
+ * be a board entry matching the supplied PCI device.
+ */
+static int __devinit pc263_attach_pci(struct comedi_device *dev,
+				      struct pci_dev *pci_dev)
 {
-	if (insn->n != 2)
+	if (!IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI))
 		return -EINVAL;
 
-	/* The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit. */
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
-		/* Write out the new digital output lines */
-		outb(s->state & 0xFF, dev->iobase);
-		outb(s->state >> 8, dev->iobase + 1);
+	dev_info(dev->class_dev, PC263_DRIVER_NAME ": attach pci %s\n",
+		 pci_name(pci_dev));
+	dev->board_ptr = pc263_find_pci_board(pci_dev);
+	if (dev->board_ptr == NULL) {
+		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
+		return -EINVAL;
 	}
-
-	/* on return, data[1] contains the value of the digital
-	 * input and output lines. */
-	/* or we could just return the software copy of the output values if
-	 * it was a purely digital output subdevice */
-	data[1] = s->state;
-
-	return 2;
+	return pc263_pci_common_attach(dev, pci_dev);
 }
 
-static int pc263_dio_insn_config(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
+static void pc263_detach(struct comedi_device *dev)
 {
-	if (insn->n != 1)
-		return -EINVAL;
-	return 1;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
+	} else {
+		if (dev->iobase)
+			release_region(dev->iobase, PC263_IO_SIZE);
+	}
 }
 
 /*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
+ * The struct comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attach/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
  */
-#ifdef CONFIG_COMEDI_AMPLC_PC263_PCI
-static int __devinit driver_amplc_pc263_pci_probe(struct pci_dev *dev,
+static struct comedi_driver amplc_pc263_driver = {
+	.driver_name = PC263_DRIVER_NAME,
+	.module = THIS_MODULE,
+	.attach = pc263_attach,
+	.attach_pci = pc263_attach_pci,
+	.detach = pc263_detach,
+	.board_name = &pc263_boards[0].name,
+	.offset = sizeof(struct pc263_board),
+	.num_names = ARRAY_SIZE(pc263_boards),
+};
+
+#if IS_ENABLED(CONFIG_COMEDI_AMPLC_PC263_PCI)
+static DEFINE_PCI_DEVICE_TABLE(pc263_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, pc263_pci_table);
+
+static int __devinit amplc_pc263_pci_probe(struct pci_dev *dev,
 						  const struct pci_device_id
 						  *ent)
 {
-	return comedi_pci_auto_config(dev, &driver_amplc_pc263);
+	return comedi_pci_auto_config(dev, &amplc_pc263_driver);
 }
 
-static void __devexit driver_amplc_pc263_pci_remove(struct pci_dev *dev)
+static void __devexit amplc_pc263_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_amplc_pc263_pci_driver = {
+static struct pci_driver amplc_pc263_pci_driver = {
+	.name = PC263_DRIVER_NAME,
 	.id_table = pc263_pci_table,
-	.probe = &driver_amplc_pc263_pci_probe,
-	.remove = __devexit_p(&driver_amplc_pc263_pci_remove)
+	.probe = &amplc_pc263_pci_probe,
+	.remove = __devexit_p(&amplc_pc263_pci_remove)
 };
-
-static int __init driver_amplc_pc263_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_amplc_pc263);
-	if (retval < 0)
-		return retval;
-
-	driver_amplc_pc263_pci_driver.name =
-	    (char *)driver_amplc_pc263.driver_name;
-	return pci_register_driver(&driver_amplc_pc263_pci_driver);
-}
-
-static void __exit driver_amplc_pc263_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_amplc_pc263_pci_driver);
-	comedi_driver_unregister(&driver_amplc_pc263);
-}
-
-module_init(driver_amplc_pc263_init_module);
-module_exit(driver_amplc_pc263_cleanup_module);
+module_comedi_pci_driver(amplc_pc263_driver, amplc_pc263_pci_driver);
 #else
-static int __init driver_amplc_pc263_init_module(void)
-{
-	return comedi_driver_register(&driver_amplc_pc263);
-}
-
-static void __exit driver_amplc_pc263_cleanup_module(void)
-{
-	comedi_driver_unregister(&driver_amplc_pc263);
-}
-
-module_init(driver_amplc_pc263_init_module);
-module_exit(driver_amplc_pc263_cleanup_module);
+module_comedi_driver(amplc_pc263_driver);
 #endif
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index fbf19cae874..4e17f13e57f 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -108,8 +108,6 @@ Caveats:
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #include "comedi_fc.h"
 #include "8253.h"
 
@@ -267,9 +265,6 @@ Caveats:
 /* Combine old and new bits. */
 #define COMBINE(old, new, mask)	(((old) & ~(mask)) | ((new) & (mask)))
 
-/* A generic null function pointer value.  */
-#define NULLFUNC	0
-
 /* Current CPU.  XXX should this be hard_smp_processor_id()? */
 #define THISCPU		smp_processor_id()
 
@@ -379,16 +374,10 @@ static const struct pci224_board pci224_boards[] = {
 	 },
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((struct pci224_board *)dev->board_ptr)
-
 /* this structure is for data unique to this hardware driver.  If
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device struct.  */
 struct pci224_private {
-	struct pci_dev *pci_dev;	/* PCI device */
 	const unsigned short *hwrange;
 	unsigned long iobase1;
 	unsigned long state;
@@ -407,8 +396,6 @@ struct pci224_private {
 	unsigned char intsce;
 };
 
-#define devpriv ((struct pci224_private *)dev->private)
-
 /*
  * Called from the 'insn_write' function to perform a single write.
  */
@@ -416,6 +403,8 @@ static void
 pci224_ao_set_data(struct comedi_device *dev, int chan, int range,
 		   unsigned int data)
 {
+	const struct pci224_board *thisboard = comedi_board(dev);
+	struct pci224_private *devpriv = dev->private;
 	unsigned short mangled;
 
 	/* Store unmangled data for readback. */
@@ -478,6 +467,7 @@ static int
 pci224_ao_insn_read(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data)
 {
+	struct pci224_private *devpriv = dev->private;
 	int i;
 	int chan;
 
@@ -506,6 +496,7 @@ pci224_cascade_ns_to_timer(int osc_base, unsigned int *d1, unsigned int *d2,
 static void pci224_ao_stop(struct comedi_device *dev,
 			   struct comedi_subdevice *s)
 {
+	struct pci224_private *devpriv = dev->private;
 	unsigned long flags;
 
 	if (!test_and_clear_bit(AO_CMD_STARTED, &devpriv->state))
@@ -549,6 +540,7 @@ static void pci224_ao_stop(struct comedi_device *dev,
 static void pci224_ao_start(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 
@@ -577,6 +569,7 @@ static void pci224_ao_start(struct comedi_device *dev,
 static void pci224_ao_handle_fifo(struct comedi_device *dev,
 				  struct comedi_subdevice *s)
 {
+	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int num_scans;
 	unsigned int room;
@@ -628,8 +621,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 			/* Nothing left to put in the FIFO. */
 			pci224_ao_stop(dev, s);
 			s->async->events |= COMEDI_CB_OVERFLOW;
-			printk(KERN_ERR "comedi%d: "
-			       "AO buffer underrun\n", dev->minor);
+			dev_err(dev->class_dev, "AO buffer underrun\n");
 		}
 	}
 	/* Determine how many new scans can be put in the FIFO. */
@@ -707,7 +699,7 @@ pci224_ao_inttrig_start(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (trignum != 0)
 		return -EINVAL;
 
-	s->async->inttrig = NULLFUNC;
+	s->async->inttrig = NULL;
 	pci224_ao_start(dev, s);
 
 	return 1;
@@ -724,6 +716,7 @@ static int
 pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		  struct comedi_cmd *cmd)
 {
+	struct pci224_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
 
@@ -994,6 +987,7 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
  */
 static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int range;
 	unsigned int i, j;
@@ -1166,6 +1160,8 @@ static void
 pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 		void *data, unsigned int num_bytes, unsigned int chan_index)
 {
+	const struct pci224_board *thisboard = comedi_board(dev);
+	struct pci224_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	short *array = data;
 	unsigned int length = num_bytes / sizeof(*array);
@@ -1196,6 +1192,7 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 static irqreturn_t pci224_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct pci224_private *devpriv = dev->private;
 	struct comedi_subdevice *s = &dev->subdevices[0];
 	struct comedi_cmd *cmd;
 	unsigned char intstat, valid_intstat;
@@ -1258,28 +1255,27 @@ static const struct pci224_board
  * This function looks for a PCI device matching the requested board name,
  * bus and slot.
  */
-static int
-pci224_find_pci(struct comedi_device *dev, int bus, int slot,
-		struct pci_dev **pci_dev_p)
+static struct pci_dev *pci224_find_pci_dev(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
 {
+	const struct pci224_board *thisboard = comedi_board(dev);
 	struct pci_dev *pci_dev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 
-	*pci_dev_p = NULL;
-
-	/* Look for matching PCI device. */
-	for (pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON, PCI_ANY_ID, NULL);
-	     pci_dev != NULL;
-	     pci_dev = pci_get_device(PCI_VENDOR_ID_AMPLICON, PCI_ANY_ID,
-				      pci_dev)) {
-		/* If bus/slot specified, check them. */
+	for_each_pci_dev(pci_dev) {
 		if (bus || slot) {
-			if (bus != pci_dev->bus->number
-			    || slot != PCI_SLOT(pci_dev->devfn))
+			if (bus != pci_dev->bus->number ||
+			    slot != PCI_SLOT(pci_dev->devfn))
 				continue;
 		}
+		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
+			continue;
+
 		if (thisboard->model == any_model) {
 			/* Match any supported model. */
 			const struct pci224_board *board_ptr;
+
 			board_ptr = pci224_find_pci_board(pci_dev);
 			if (board_ptr == NULL)
 				continue;
@@ -1290,21 +1286,26 @@ pci224_find_pci(struct comedi_device *dev, int bus, int slot,
 			if (thisboard->devid != pci_dev->device)
 				continue;
 		}
-
-		/* Found a match. */
-		*pci_dev_p = pci_dev;
-		return 0;
-	}
-	/* No match found. */
-	if (bus || slot) {
-		printk(KERN_ERR "comedi%d: error! "
-		       "no %s found at pci %02x:%02x!\n",
-		       dev->minor, thisboard->name, bus, slot);
-	} else {
-		printk(KERN_ERR "comedi%d: error! no %s found!\n",
-		       dev->minor, thisboard->name);
+		return pci_dev;
 	}
-	return -EIO;
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
+static void pci224_report_attach(struct comedi_device *dev, unsigned int irq)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	char tmpbuf[30];
+
+	if (irq)
+		snprintf(tmpbuf, sizeof(tmpbuf), "irq %u%s", irq,
+			 (dev->irq ? "" : " UNAVAILABLE"));
+	else
+		snprintf(tmpbuf, sizeof(tmpbuf), "no irq");
+	dev_info(dev->class_dev, "%s (pci %s) (%s) attached\n",
+		 dev->board_name, pci_name(pcidev), tmpbuf);
 }
 
 /*
@@ -1313,17 +1314,20 @@ pci224_find_pci(struct comedi_device *dev, int bus, int slot,
 static int pci224_attach_common(struct comedi_device *dev,
 				struct pci_dev *pci_dev, int *options)
 {
+	const struct pci224_board *thisboard = comedi_board(dev);
+	struct pci224_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	unsigned int irq;
 	unsigned n;
 	int ret;
 
-	devpriv->pci_dev = pci_dev;
+	comedi_set_hw_dev(dev, &pci_dev->dev);
+
 	ret = comedi_pci_enable(pci_dev, DRIVER_NAME);
 	if (ret < 0) {
-		printk(KERN_ERR
-		       "comedi%d: error! cannot enable PCI device "
-		       "and request regions!\n", dev->minor);
+		dev_err(dev->class_dev,
+			"error! cannot enable PCI device and request regions!\n"
+			);
 		return ret;
 	}
 	spin_lock_init(&devpriv->ao_spinlock);
@@ -1367,13 +1371,9 @@ static int pci224_attach_common(struct comedi_device *dev,
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
-	/* Allocate subdevices.  There is only one!  */
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
-	}
 
 	s = dev->subdevices + 0;
 	/* Analog output subdevice. */
@@ -1405,10 +1405,9 @@ static int pci224_attach_common(struct comedi_device *dev,
 		if (options) {
 			for (n = 2; n < 3 + s->n_chan; n++) {
 				if (options[n] < 0 || options[n] > 1) {
-					printk(KERN_WARNING
-					       "comedi%d: %s: warning! bad options[%u]=%d\n",
-					       dev->minor, DRIVER_NAME, n,
-					       options[n]);
+					dev_warn(dev->class_dev, DRIVER_NAME
+						 ": warning! bad options[%u]=%d\n",
+						 n, options[n]);
 				}
 			}
 		}
@@ -1437,9 +1436,9 @@ static int pci224_attach_common(struct comedi_device *dev,
 			devpriv->hwrange = hwrange_pci224_external;
 		} else {
 			if (options && options[2] != 0) {
-				printk(KERN_WARNING "comedi%d: %s: warning! "
-				       "bad options[2]=%d\n",
-				       dev->minor, DRIVER_NAME, options[2]);
+				dev_warn(dev->class_dev, DRIVER_NAME
+					 ": warning! bad options[2]=%d\n",
+					 options[2]);
 			}
 			s->range_table = &range_pci224_internal;
 			devpriv->hwrange = hwrange_pci224_internal;
@@ -1452,71 +1451,56 @@ static int pci224_attach_common(struct comedi_device *dev,
 		ret = request_irq(irq, pci224_interrupt, IRQF_SHARED,
 				  DRIVER_NAME, dev);
 		if (ret < 0) {
-			printk(KERN_ERR "comedi%d: error! "
-			       "unable to allocate irq %u\n", dev->minor, irq);
+			dev_err(dev->class_dev,
+				"error! unable to allocate irq %u\n", irq);
 			return ret;
 		} else {
 			dev->irq = irq;
 		}
 	}
 
-	printk(KERN_INFO "comedi%d: %s ", dev->minor, dev->board_name);
-	printk("(pci %s) ", pci_name(pci_dev));
-	if (irq)
-		printk("(irq %u%s) ", irq, (dev->irq ? "" : " UNAVAILABLE"));
-	else
-		printk("(no irq) ");
-
-
-	printk("attached\n");
-
+	pci224_report_attach(dev, irq);
 	return 1;
 }
 
 static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct pci_dev *pci_dev;
-	int bus, slot;
 	int ret;
 
-	printk(KERN_DEBUG "comedi%d: %s: attach\n", dev->minor, DRIVER_NAME);
+	dev_info(dev->class_dev, DRIVER_NAME ": attach\n");
 
-	bus = it->options[0];
-	slot = it->options[1];
 	ret = alloc_private(dev, sizeof(struct pci224_private));
 	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+		dev_err(dev->class_dev, "error! out of memory!\n");
 		return ret;
 	}
 
-	ret = pci224_find_pci(dev, bus, slot, &pci_dev);
-	if (ret < 0)
-		return ret;
+	pci_dev = pci224_find_pci_dev(dev, it);
+	if (!pci_dev)
+		return -EIO;
 
 	return pci224_attach_common(dev, pci_dev, it->options);
 }
 
-static int
+static int __devinit
 pci224_attach_pci(struct comedi_device *dev, struct pci_dev *pci_dev)
 {
 	int ret;
 
-	printk(KERN_DEBUG "comedi%d: %s: attach_pci %s\n", dev->minor,
-	       DRIVER_NAME, pci_name(pci_dev));
+	dev_info(dev->class_dev, DRIVER_NAME ": attach_pci %s\n",
+		 pci_name(pci_dev));
 
 	ret = alloc_private(dev, sizeof(struct pci224_private));
 	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: error! out of memory!\n",
-		       dev->minor);
+		dev_err(dev->class_dev, "error! out of memory!\n");
 		return ret;
 	}
 
 	dev->board_ptr = pci224_find_pci_board(pci_dev);
 	if (dev->board_ptr == NULL) {
-		printk(KERN_ERR
-		       "comedi%d: %s: BUG! cannot determine board type!\n",
-		       dev->minor, DRIVER_NAME);
+		dev_err(dev->class_dev,
+			DRIVER_NAME ": BUG! cannot determine board type!\n");
 		return -EINVAL;
 	}
 	return pci224_attach_common(dev, pci_dev, NULL);
@@ -1524,6 +1508,9 @@ pci224_attach_pci(struct comedi_device *dev, struct pci_dev *pci_dev)
 
 static void pci224_detach(struct comedi_device *dev)
 {
+	struct pci224_private *devpriv = dev->private;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->subdevices) {
@@ -1537,11 +1524,11 @@ static void pci224_detach(struct comedi_device *dev)
 		kfree(devpriv->ao_readback);
 		kfree(devpriv->ao_scan_vals);
 		kfree(devpriv->ao_scan_order);
-		if (devpriv->pci_dev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index d4c80b1281f..1b67d0c61fa 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -193,7 +193,6 @@ for (or detection of) various hardware problems added by Ian Abbott.
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "comedi_pci.h"
 #include "8253.h"
 #include "8255.h"
 
@@ -431,9 +430,6 @@ enum {
 /* Combine old and new bits. */
 #define COMBINE(old, new, mask)	(((old) & ~(mask)) | ((new) & (mask)))
 
-/* A generic null function pointer value.  */
-#define NULLFUNC	0
-
 /* Current CPU.  XXX should this be hard_smp_processor_id()? */
 #define THISCPU		smp_processor_id()
 
@@ -500,17 +496,10 @@ static const struct pci230_board pci230_boards[] = {
 	 },
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define n_pci230_boards ARRAY_SIZE(pci230_boards)
-#define thisboard ((const struct pci230_board *)dev->board_ptr)
-
 /* this structure is for data unique to this hardware driver.  If
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device struct.  */
 struct pci230_private {
-	struct pci_dev *pci_dev;
 	spinlock_t isr_spinlock;	/* Interrupt spin lock */
 	spinlock_t res_spinlock;	/* Shared resources spin lock */
 	spinlock_t ai_stop_spinlock;	/* Spin lock for stopping AI command */
@@ -549,8 +538,6 @@ struct pci230_private {
 	unsigned char res_owner[NUM_RESOURCES];	/* Shared resource owners. */
 };
 
-#define devpriv ((struct pci230_private *)dev->private)
-
 /* PCI230 clock source periods in ns */
 static const unsigned int pci230_timebase[8] = {
 	[CLK_10MHZ] = TIMEBASE_10MHZ,
@@ -588,49 +575,14 @@ static const struct comedi_lrange pci230_ao_range = { 2, {
 /* PCI230 daccon bipolar flag for each analogue output range. */
 static const unsigned char pci230_ao_bipolar[2] = { 0, 1 };
 
-static int pci230_ai_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int pci230_ao_winsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int pci230_ao_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
-				    unsigned int mode, uint64_t ns,
-				    unsigned int round);
-static void pci230_ns_to_single_timer(unsigned int *ns, unsigned int round);
-static void pci230_cancel_ct(struct comedi_device *dev, unsigned int ct);
-static irqreturn_t pci230_interrupt(int irq, void *d);
-static int pci230_ao_cmdtest(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_cmd *cmd);
-static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int pci230_ao_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static void pci230_ao_stop(struct comedi_device *dev,
-			   struct comedi_subdevice *s);
-static void pci230_handle_ao_nofifo(struct comedi_device *dev,
-				    struct comedi_subdevice *s);
-static int pci230_handle_ao_fifo(struct comedi_device *dev,
-				 struct comedi_subdevice *s);
-static int pci230_ai_cmdtest(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_cmd *cmd);
-static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int pci230_ai_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static void pci230_ai_stop(struct comedi_device *dev,
-			   struct comedi_subdevice *s);
-static void pci230_handle_ai(struct comedi_device *dev,
-			     struct comedi_subdevice *s);
-
 static short pci230_ai_read(struct comedi_device *dev)
 {
-	/* Read sample. */
-	short data = (short)inw(dev->iobase + PCI230_ADCDATA);
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci230_private *devpriv = dev->private;
+	short data;
 
+	/* Read sample. */
+	data = (short)inw(dev->iobase + PCI230_ADCDATA);
 	/* PCI230 is 12 bit - stored in upper bits of 16 bit register (lower
 	 * four bits reserved for expansion). */
 	/* PCI230+ is 16 bit AI. */
@@ -647,12 +599,14 @@ static short pci230_ai_read(struct comedi_device *dev)
 static inline unsigned short pci230_ao_mangle_datum(struct comedi_device *dev,
 						    short datum)
 {
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci230_private *devpriv = dev->private;
+
 	/* If a bipolar range was specified, mangle it (straight binary->twos
 	 * complement). */
 	if (devpriv->ao_bipolar)
 		datum ^= 1 << (thisboard->ao_bits - 1);
 
-
 	/* PCI230 is 12 bit - stored in upper bits of 16 bit register (lower
 	 * four bits reserved for expansion). */
 	/* PCI230+ is also 12 bit AO. */
@@ -663,6 +617,8 @@ static inline unsigned short pci230_ao_mangle_datum(struct comedi_device *dev,
 static inline void pci230_ao_write_nofifo(struct comedi_device *dev,
 					  short datum, unsigned int chan)
 {
+	struct pci230_private *devpriv = dev->private;
+
 	/* Store unmangled datum to be read back later. */
 	devpriv->ao_readback[chan] = datum;
 
@@ -676,6 +632,8 @@ static inline void pci230_ao_write_nofifo(struct comedi_device *dev,
 static inline void pci230_ao_write_fifo(struct comedi_device *dev, short datum,
 					unsigned int chan)
 {
+	struct pci230_private *devpriv = dev->private;
+
 	/* Store unmangled datum to be read back later. */
 	devpriv->ao_readback[chan] = datum;
 
@@ -684,277 +642,10 @@ static inline void pci230_ao_write_fifo(struct comedi_device *dev, short datum,
 	     dev->iobase + PCI230P2_DACDATA);
 }
 
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int pci230_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	unsigned long iobase1, iobase2;
-	/* PCI230's I/O spaces 1 and 2 respectively. */
-	struct pci_dev *pci_dev = NULL;
-	int i = 0, irq_hdl, rc;
-
-	printk("comedi%d: amplc_pci230: attach %s %d,%d\n", dev->minor,
-	       thisboard->name, it->options[0], it->options[1]);
-
-	/* Allocate the private structure area using alloc_private().
-	 * Macro defined in comedidev.h - memsets struct fields to 0. */
-	if ((alloc_private(dev, sizeof(struct pci230_private))) < 0)
-		return -ENOMEM;
-
-	spin_lock_init(&devpriv->isr_spinlock);
-	spin_lock_init(&devpriv->res_spinlock);
-	spin_lock_init(&devpriv->ai_stop_spinlock);
-	spin_lock_init(&devpriv->ao_stop_spinlock);
-	/* Find card */
-	for_each_pci_dev(pci_dev) {
-		if (it->options[0] || it->options[1]) {
-			/* Match against bus/slot options. */
-			if (it->options[0] != pci_dev->bus->number ||
-			    it->options[1] != PCI_SLOT(pci_dev->devfn))
-				continue;
-		}
-		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
-			continue;
-		if (thisboard->id == PCI_DEVICE_ID_INVALID) {
-			/* The name was specified as "amplc_pci230" which is
-			 * used to match any supported device.  Replace the
-			 * current dev->board_ptr with one that matches the
-			 * PCI device ID. */
-			for (i = 0; i < n_pci230_boards; i++) {
-				if (pci_dev->device == pci230_boards[i].id) {
-					if (pci230_boards[i].min_hwver > 0) {
-						/* Check for a '+' model.
-						 * First check length of
-						 * registers. */
-						if (pci_resource_len(pci_dev, 3)
-						    < 32) {
-							/* Not a '+' model. */
-							continue;
-						}
-						/* TODO: temporarily enable the
-						 * PCI device and read the
-						 * hardware version register.
-						 * For now assume it's okay. */
-					}
-					/* Change board_ptr to matched board */
-					dev->board_ptr = &pci230_boards[i];
-					break;
-				}
-			}
-			if (i < n_pci230_boards)
-				break;
-		} else {
-			/* The name was specified as a specific device name.
-			 * The current dev->board_ptr is correct.  Check
-			 * whether it matches the PCI device ID. */
-			if (thisboard->id == pci_dev->device) {
-				/* Check minimum hardware version. */
-				if (thisboard->min_hwver > 0) {
-					/* Looking for a '+' model.  First
-					 * check length of registers. */
-					if (pci_resource_len(pci_dev, 3) < 32) {
-						/* Not a '+' model. */
-						continue;
-					}
-					/* TODO: temporarily enable the PCI
-					 * device and read the hardware version
-					 * register.  For now, assume it's
-					 * okay. */
-					break;
-				} else {
-					break;
-				}
-			}
-		}
-	}
-	if (!pci_dev) {
-		printk("comedi%d: No %s card found\n", dev->minor,
-		       thisboard->name);
-		return -EIO;
-	}
-	devpriv->pci_dev = pci_dev;
-
-	/*
-	 * Initialize dev->board_name.
-	 */
-	dev->board_name = thisboard->name;
-
-	/* Enable PCI device and reserve I/O spaces. */
-	if (comedi_pci_enable(pci_dev, "amplc_pci230") < 0) {
-		printk("comedi%d: failed to enable PCI device "
-		       "and request regions\n", dev->minor);
-		return -EIO;
-	}
-
-	/* Read base addresses of the PCI230's two I/O regions from PCI
-	 * configuration register. */
-	iobase1 = pci_resource_start(pci_dev, 2);
-	iobase2 = pci_resource_start(pci_dev, 3);
-
-	printk("comedi%d: %s I/O region 1 0x%04lx I/O region 2 0x%04lx\n",
-	       dev->minor, dev->board_name, iobase1, iobase2);
-
-	devpriv->iobase1 = iobase1;
-	dev->iobase = iobase2;
-
-	/* Read bits of DACCON register - only the output range. */
-	devpriv->daccon = inw(dev->iobase + PCI230_DACCON) & PCI230_DAC_OR_MASK;
-
-	/* Read hardware version register and set extended function register
-	 * if they exist. */
-	if (pci_resource_len(pci_dev, 3) >= 32) {
-		unsigned short extfunc = 0;
-
-		devpriv->hwver = inw(dev->iobase + PCI230P_HWVER);
-		if (devpriv->hwver < thisboard->min_hwver) {
-			printk("comedi%d: %s - bad hardware version "
-			       "- got %u, need %u\n", dev->minor,
-			       dev->board_name, devpriv->hwver,
-			       thisboard->min_hwver);
-			return -EIO;
-		}
-		if (devpriv->hwver > 0) {
-			if (!thisboard->have_dio) {
-				/* No DIO ports.  Route counters' external gates
-				 * to the EXTTRIG signal (PCI260+ pin 17).
-				 * (Otherwise, they would be routed to DIO
-				 * inputs PC0, PC1 and PC2 which don't exist
-				 * on PCI260[+].) */
-				extfunc |= PCI230P_EXTFUNC_GAT_EXTTRIG;
-			}
-			if ((thisboard->ao_chans > 0)
-			    && (devpriv->hwver >= 2)) {
-				/* Enable DAC FIFO functionality. */
-				extfunc |= PCI230P2_EXTFUNC_DACFIFO;
-			}
-		}
-		outw(extfunc, dev->iobase + PCI230P_EXTFUNC);
-		if ((extfunc & PCI230P2_EXTFUNC_DACFIFO) != 0) {
-			/* Temporarily enable DAC FIFO, reset it and disable
-			 * FIFO wraparound. */
-			outw(devpriv->daccon | PCI230P2_DAC_FIFO_EN
-			     | PCI230P2_DAC_FIFO_RESET,
-			     dev->iobase + PCI230_DACCON);
-			/* Clear DAC FIFO channel enable register. */
-			outw(0, dev->iobase + PCI230P2_DACEN);
-			/* Disable DAC FIFO. */
-			outw(devpriv->daccon, dev->iobase + PCI230_DACCON);
-		}
-	}
-
-	/* Disable board's interrupts. */
-	outb(0, devpriv->iobase1 + PCI230_INT_SCE);
-
-	/* Set ADC to a reasonable state. */
-	devpriv->adcg = 0;
-	devpriv->adccon = PCI230_ADC_TRIG_NONE | PCI230_ADC_IM_SE
-	    | PCI230_ADC_IR_BIP;
-	outw(1 << 0, dev->iobase + PCI230_ADCEN);
-	outw(devpriv->adcg, dev->iobase + PCI230_ADCG);
-	outw(devpriv->adccon | PCI230_ADC_FIFO_RESET,
-	     dev->iobase + PCI230_ADCCON);
-
-	/* Register the interrupt handler. */
-	irq_hdl = request_irq(devpriv->pci_dev->irq, pci230_interrupt,
-			      IRQF_SHARED, "amplc_pci230", dev);
-	if (irq_hdl < 0) {
-		printk("comedi%d: unable to register irq, "
-		       "commands will not be available %d\n", dev->minor,
-		       devpriv->pci_dev->irq);
-	} else {
-		dev->irq = devpriv->pci_dev->irq;
-		printk("comedi%d: registered irq %u\n", dev->minor,
-		       devpriv->pci_dev->irq);
-	}
-
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = &pci230_ai_range;
-	s->insn_read = &pci230_ai_rinsn;
-	s->len_chanlist = 256;	/* but there are restrictions. */
-	/* Only register commands if the interrupt handler is installed. */
-	if (irq_hdl == 0) {
-		dev->read_subdev = s;
-		s->subdev_flags |= SDF_CMD_READ;
-		s->do_cmd = &pci230_ai_cmd;
-		s->do_cmdtest = &pci230_ai_cmdtest;
-		s->cancel = pci230_ai_cancel;
-	}
-
-	s = dev->subdevices + 1;
-	/* analog output subdevice */
-	if (thisboard->ao_chans > 0) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = thisboard->ao_chans;
-		s->maxdata = (1 << thisboard->ao_bits) - 1;
-		s->range_table = &pci230_ao_range;
-		s->insn_write = &pci230_ao_winsn;
-		s->insn_read = &pci230_ao_rinsn;
-		s->len_chanlist = thisboard->ao_chans;
-		/* Only register commands if the interrupt handler is
-		 * installed. */
-		if (irq_hdl == 0) {
-			dev->write_subdev = s;
-			s->subdev_flags |= SDF_CMD_WRITE;
-			s->do_cmd = &pci230_ao_cmd;
-			s->do_cmdtest = &pci230_ao_cmdtest;
-			s->cancel = pci230_ao_cancel;
-		}
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	s = dev->subdevices + 2;
-	/* digital i/o subdevice */
-	if (thisboard->have_dio) {
-		rc = subdev_8255_init(dev, s, NULL,
-				      (devpriv->iobase1 + PCI230_PPI_X_BASE));
-		if (rc < 0)
-			return rc;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	printk("comedi%d: attached\n", dev->minor);
-
-	return 1;
-}
-
-static void pci230_detach(struct comedi_device *dev)
-{
-	if (dev->subdevices && thisboard->have_dio)
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (devpriv) {
-		if (devpriv->pci_dev) {
-			if (dev->iobase)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
-	}
-}
-
 static int get_resources(struct comedi_device *dev, unsigned int res_mask,
 			 unsigned char owner)
 {
+	struct pci230_private *devpriv = dev->private;
 	int ok;
 	unsigned int i;
 	unsigned int b;
@@ -997,6 +688,7 @@ static inline int get_one_resource(struct comedi_device *dev,
 static void put_resources(struct comedi_device *dev, unsigned int res_mask,
 			  unsigned char owner)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned int i;
 	unsigned int b;
 	unsigned long irqflags;
@@ -1026,6 +718,86 @@ static inline void put_all_resources(struct comedi_device *dev,
 	put_resources(dev, (1U << NUM_RESOURCES) - 1, owner);
 }
 
+static unsigned int divide_ns(uint64_t ns, unsigned int timebase,
+			      unsigned int round_mode)
+{
+	uint64_t div;
+	unsigned int rem;
+
+	div = ns;
+	rem = do_div(div, timebase);
+	round_mode &= TRIG_ROUND_MASK;
+	switch (round_mode) {
+	default:
+	case TRIG_ROUND_NEAREST:
+		div += (rem + (timebase / 2)) / timebase;
+		break;
+	case TRIG_ROUND_DOWN:
+		break;
+	case TRIG_ROUND_UP:
+		div += (rem + timebase - 1) / timebase;
+		break;
+	}
+	return div > UINT_MAX ? UINT_MAX : (unsigned int)div;
+}
+
+/* Given desired period in ns, returns the required internal clock source
+ * and gets the initial count. */
+static unsigned int pci230_choose_clk_count(uint64_t ns, unsigned int *count,
+					    unsigned int round_mode)
+{
+	unsigned int clk_src, cnt;
+
+	for (clk_src = CLK_10MHZ;; clk_src++) {
+		cnt = divide_ns(ns, pci230_timebase[clk_src], round_mode);
+		if ((cnt <= 65536) || (clk_src == CLK_1KHZ))
+			break;
+
+	}
+	*count = cnt;
+	return clk_src;
+}
+
+static void pci230_ns_to_single_timer(unsigned int *ns, unsigned int round)
+{
+	unsigned int count;
+	unsigned int clk_src;
+
+	clk_src = pci230_choose_clk_count(*ns, &count, round);
+	*ns = count * pci230_timebase[clk_src];
+	return;
+}
+
+static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
+				    unsigned int mode, uint64_t ns,
+				    unsigned int round)
+{
+	struct pci230_private *devpriv = dev->private;
+	unsigned int clk_src;
+	unsigned int count;
+
+	/* Set mode. */
+	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct, mode);
+	/* Determine clock source and count. */
+	clk_src = pci230_choose_clk_count(ns, &count, round);
+	/* Program clock source. */
+	outb(CLK_CONFIG(ct, clk_src), devpriv->iobase1 + PCI230_ZCLK_SCE);
+	/* Set initial count. */
+	if (count >= 65536)
+		count = 0;
+
+	i8254_write(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct, count);
+}
+
+static void pci230_cancel_ct(struct comedi_device *dev, unsigned int ct)
+{
+	struct pci230_private *devpriv = dev->private;
+
+	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct,
+		       I8254_MODE1);
+	/* Counter ct, 8254 mode 1, initial count not written. */
+}
+
 /*
  *  COMEDI_SUBD_AI instruction;
  */
@@ -1033,6 +805,7 @@ static int pci230_ai_rinsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned int n, i;
 	unsigned int chan, range, aref;
 	unsigned int gainshift;
@@ -1118,9 +891,7 @@ static int pci230_ai_rinsn(struct comedi_device *dev,
 			udelay(1);
 		}
 		if (i == TIMEOUT) {
-			/* printk() should be used instead of printk()
-			 * whenever the code can be called from real-time. */
-			printk("timeout\n");
+			dev_err(dev->class_dev, "timeout\n");
 			return -ETIMEDOUT;
 		}
 
@@ -1139,6 +910,7 @@ static int pci230_ao_winsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
 {
+	struct pci230_private *devpriv = dev->private;
 	int i;
 	int chan, range;
 
@@ -1168,6 +940,7 @@ static int pci230_ao_rinsn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
 {
+	struct pci230_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -1180,6 +953,8 @@ static int pci230_ao_rinsn(struct comedi_device *dev,
 static int pci230_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci230_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
 
@@ -1389,10 +1164,201 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+static void pci230_ao_stop(struct comedi_device *dev,
+			   struct comedi_subdevice *s)
+{
+	struct pci230_private *devpriv = dev->private;
+	unsigned long irqflags;
+	unsigned char intsrc;
+	int started;
+	struct comedi_cmd *cmd;
+
+	spin_lock_irqsave(&devpriv->ao_stop_spinlock, irqflags);
+	started = test_and_clear_bit(AO_CMD_STARTED, &devpriv->state);
+	spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
+	if (!started)
+		return;
+	cmd = &s->async->cmd;
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/* Stop scan rate generator. */
+		pci230_cancel_ct(dev, 1);
+	}
+	/* Determine interrupt source. */
+	if (devpriv->hwver < 2) {
+		/* Not using DAC FIFO.  Using CT1 interrupt. */
+		intsrc = PCI230_INT_ZCLK_CT1;
+	} else {
+		/* Using DAC FIFO interrupt. */
+		intsrc = PCI230P2_INT_DAC;
+	}
+	/* Disable interrupt and wait for interrupt routine to finish running
+	 * unless we are called from the interrupt routine. */
+	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
+	devpriv->int_en &= ~intsrc;
+	while (devpriv->intr_running && devpriv->intr_cpuid != THISCPU) {
+		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
+		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
+	}
+	if (devpriv->ier != devpriv->int_en) {
+		devpriv->ier = devpriv->int_en;
+		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
+	}
+	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
+	if (devpriv->hwver >= 2) {
+		/* Using DAC FIFO.  Reset FIFO, clear underrun error,
+		 * disable FIFO. */
+		devpriv->daccon &= PCI230_DAC_OR_MASK;
+		outw(devpriv->daccon | PCI230P2_DAC_FIFO_RESET
+		     | PCI230P2_DAC_FIFO_UNDERRUN_CLEAR,
+		     dev->iobase + PCI230_DACCON);
+	}
+	/* Release resources. */
+	put_all_resources(dev, OWNER_AOCMD);
+}
+
+static void pci230_handle_ao_nofifo(struct comedi_device *dev,
+				    struct comedi_subdevice *s)
+{
+	struct pci230_private *devpriv = dev->private;
+	short data;
+	int i, ret;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+
+	if (!devpriv->ao_continuous && (devpriv->ao_scan_count == 0))
+		return;
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		/* Read sample from Comedi's circular buffer. */
+		ret = comedi_buf_get(s->async, &data);
+		if (ret == 0) {
+			s->async->events |= COMEDI_CB_OVERFLOW;
+			pci230_ao_stop(dev, s);
+			comedi_error(dev, "AO buffer underrun");
+			return;
+		}
+		/* Write value to DAC. */
+		pci230_ao_write_nofifo(dev, data, CR_CHAN(cmd->chanlist[i]));
+	}
+	async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
+	if (!devpriv->ao_continuous) {
+		devpriv->ao_scan_count--;
+		if (devpriv->ao_scan_count == 0) {
+			/* End of acquisition. */
+			async->events |= COMEDI_CB_EOA;
+			pci230_ao_stop(dev, s);
+		}
+	}
+}
+
+/* Loads DAC FIFO (if using it) from buffer. */
+/* Returns 0 if AO finished due to completion or error, 1 if still going. */
+static int pci230_handle_ao_fifo(struct comedi_device *dev,
+				 struct comedi_subdevice *s)
+{
+	struct pci230_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int num_scans;
+	unsigned int room;
+	unsigned short dacstat;
+	unsigned int i, n;
+	unsigned int bytes_per_scan;
+	unsigned int events = 0;
+	int running;
+
+	/* Get DAC FIFO status. */
+	dacstat = inw(dev->iobase + PCI230_DACCON);
+	/* Determine number of scans available in buffer. */
+	bytes_per_scan = cmd->chanlist_len * sizeof(short);
+	num_scans = comedi_buf_read_n_available(async) / bytes_per_scan;
+	if (!devpriv->ao_continuous) {
+		/* Fixed number of scans. */
+		if (num_scans > devpriv->ao_scan_count)
+			num_scans = devpriv->ao_scan_count;
+		if (devpriv->ao_scan_count == 0) {
+			/* End of acquisition. */
+			events |= COMEDI_CB_EOA;
+		}
+	}
+	if (events == 0) {
+		/* Check for FIFO underrun. */
+		if ((dacstat & PCI230P2_DAC_FIFO_UNDERRUN_LATCHED) != 0) {
+			comedi_error(dev, "AO FIFO underrun");
+			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
+		}
+		/* Check for buffer underrun if FIFO less than half full
+		 * (otherwise there will be loads of "DAC FIFO not half full"
+		 * interrupts). */
+		if ((num_scans == 0)
+		    && ((dacstat & PCI230P2_DAC_FIFO_HALF) == 0)) {
+			comedi_error(dev, "AO buffer underrun");
+			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
+		}
+	}
+	if (events == 0) {
+		/* Determine how much room is in the FIFO (in samples). */
+		if ((dacstat & PCI230P2_DAC_FIFO_FULL) != 0)
+			room = PCI230P2_DAC_FIFOROOM_FULL;
+		else if ((dacstat & PCI230P2_DAC_FIFO_HALF) != 0)
+			room = PCI230P2_DAC_FIFOROOM_HALFTOFULL;
+		else if ((dacstat & PCI230P2_DAC_FIFO_EMPTY) != 0)
+			room = PCI230P2_DAC_FIFOROOM_EMPTY;
+		else
+			room = PCI230P2_DAC_FIFOROOM_ONETOHALF;
+		/* Convert room to number of scans that can be added. */
+		room /= cmd->chanlist_len;
+		/* Determine number of scans to process. */
+		if (num_scans > room)
+			num_scans = room;
+		/* Process scans. */
+		for (n = 0; n < num_scans; n++) {
+			for (i = 0; i < cmd->chanlist_len; i++) {
+				short datum;
+
+				comedi_buf_get(async, &datum);
+				pci230_ao_write_fifo(dev, datum,
+						     CR_CHAN(cmd->chanlist[i]));
+			}
+		}
+		events |= COMEDI_CB_EOS | COMEDI_CB_BLOCK;
+		if (!devpriv->ao_continuous) {
+			devpriv->ao_scan_count -= num_scans;
+			if (devpriv->ao_scan_count == 0) {
+				/* All data for the command has been written
+				 * to FIFO.  Set FIFO interrupt trigger level
+				 * to 'empty'. */
+				devpriv->daccon = (devpriv->daccon
+						   &
+						   ~PCI230P2_DAC_INT_FIFO_MASK)
+				    | PCI230P2_DAC_INT_FIFO_EMPTY;
+				outw(devpriv->daccon,
+				     dev->iobase + PCI230_DACCON);
+			}
+		}
+		/* Check if FIFO underrun occurred while writing to FIFO. */
+		dacstat = inw(dev->iobase + PCI230_DACCON);
+		if ((dacstat & PCI230P2_DAC_FIFO_UNDERRUN_LATCHED) != 0) {
+			comedi_error(dev, "AO FIFO underrun");
+			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
+		}
+	}
+	if ((events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
+	    != 0) {
+		/* Stopping AO due to completion or error. */
+		pci230_ao_stop(dev, s);
+		running = 0;
+	} else {
+		running = 1;
+	}
+	async->events |= events;
+	return running;
+}
+
 static int pci230_ao_inttrig_scan_begin(struct comedi_device *dev,
 					struct comedi_subdevice *s,
 					unsigned int trig_num)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
 
 	if (trig_num != 0)
@@ -1417,6 +1383,8 @@ static int pci230_ao_inttrig_scan_begin(struct comedi_device *dev,
 		/* Delay.  Should driver be responsible for this? */
 		/* XXX TODO: See if DAC busy bit can be used. */
 		udelay(8);
+	} else {
+		spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
 	}
 
 	return 1;
@@ -1425,6 +1393,7 @@ static int pci230_ao_inttrig_scan_begin(struct comedi_device *dev,
 static void pci230_ao_start(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	struct pci230_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned long irqflags;
@@ -1518,7 +1487,7 @@ static int pci230_ao_inttrig_start(struct comedi_device *dev,
 	if (trig_num != 0)
 		return -EINVAL;
 
-	s->async->inttrig = NULLFUNC;
+	s->async->inttrig = NULL;
 	pci230_ao_start(dev, s);
 
 	return 1;
@@ -1526,6 +1495,7 @@ static int pci230_ao_inttrig_start(struct comedi_device *dev,
 
 static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned short daccon;
 	unsigned int range;
 
@@ -1601,6 +1571,13 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
+static int pci230_ao_cancel(struct comedi_device *dev,
+			    struct comedi_subdevice *s)
+{
+	pci230_ao_stop(dev, s);
+	return 0;
+}
+
 static int pci230_ai_check_scan_period(struct comedi_cmd *cmd)
 {
 	unsigned int min_scan_period, chanlist_len;
@@ -1628,6 +1605,8 @@ static int pci230_ai_check_scan_period(struct comedi_cmd *cmd)
 static int pci230_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci230_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
 
@@ -1993,13 +1972,9 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 					(s->n_chan / 2) - 1);
 			}
 			if ((errors & buggy_chan0_err) != 0) {
-				/* Use printk instead of DPRINTK here. */
-				printk("comedi: comedi%d: amplc_pci230: "
-				       "ai_cmdtest: Buggy PCI230+/260+ "
-				       "h/w version %u requires first channel "
-				       "of multi-channel sequence to be 0 "
-				       "(corrected in h/w version 4)\n",
-				       dev->minor, devpriv->hwver);
+				dev_info(dev->class_dev,
+					 "amplc_pci230: ai_cmdtest: Buggy PCI230+/260+ h/w version %u requires first channel of multi-channel sequence to be 0 (corrected in h/w version 4)\n",
+					 devpriv->hwver);
 			}
 		}
 	}
@@ -2013,6 +1988,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 static void pci230_ai_update_fifo_trigger_level(struct comedi_device *dev,
 						struct comedi_subdevice *s)
 {
+	struct pci230_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int scanlen = cmd->scan_end_arg;
 	unsigned int wake;
@@ -2057,6 +2033,7 @@ static int pci230_ai_inttrig_convert(struct comedi_device *dev,
 				     struct comedi_subdevice *s,
 				     unsigned int trig_num)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
 
 	if (trig_num != 0)
@@ -2099,6 +2076,7 @@ static int pci230_ai_inttrig_scan_begin(struct comedi_device *dev,
 					struct comedi_subdevice *s,
 					unsigned int trig_num)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
 	unsigned char zgat;
 
@@ -2118,9 +2096,56 @@ static int pci230_ai_inttrig_scan_begin(struct comedi_device *dev,
 	return 1;
 }
 
+static void pci230_ai_stop(struct comedi_device *dev,
+			   struct comedi_subdevice *s)
+{
+	struct pci230_private *devpriv = dev->private;
+	unsigned long irqflags;
+	struct comedi_cmd *cmd;
+	int started;
+
+	spin_lock_irqsave(&devpriv->ai_stop_spinlock, irqflags);
+	started = test_and_clear_bit(AI_CMD_STARTED, &devpriv->state);
+	spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
+	if (!started)
+		return;
+	cmd = &s->async->cmd;
+	if (cmd->convert_src == TRIG_TIMER) {
+		/* Stop conversion rate generator. */
+		pci230_cancel_ct(dev, 2);
+	}
+	if (cmd->scan_begin_src != TRIG_FOLLOW) {
+		/* Stop scan period monostable. */
+		pci230_cancel_ct(dev, 0);
+	}
+	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
+	/* Disable ADC interrupt and wait for interrupt routine to finish
+	 * running unless we are called from the interrupt routine. */
+	devpriv->int_en &= ~PCI230_INT_ADC;
+	while (devpriv->intr_running && devpriv->intr_cpuid != THISCPU) {
+		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
+		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
+	}
+	if (devpriv->ier != devpriv->int_en) {
+		devpriv->ier = devpriv->int_en;
+		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
+	}
+	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
+	/* Reset FIFO, disable FIFO and set start conversion source to none.
+	 * Keep se/diff and bip/uni settings */
+	devpriv->adccon = (devpriv->adccon & (PCI230_ADC_IR_MASK
+					      | PCI230_ADC_IM_MASK)) |
+	    PCI230_ADC_TRIG_NONE;
+	outw(devpriv->adccon | PCI230_ADC_FIFO_RESET,
+	     dev->iobase + PCI230_ADCCON);
+	/* Release resources. */
+	put_all_resources(dev, OWNER_AICMD);
+}
+
 static void pci230_ai_start(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
 	unsigned short conv;
 	struct comedi_async *async = s->async;
@@ -2263,14 +2288,108 @@ static int pci230_ai_inttrig_start(struct comedi_device *dev,
 	if (trig_num != 0)
 		return -EINVAL;
 
-	s->async->inttrig = NULLFUNC;
+	s->async->inttrig = NULL;
 	pci230_ai_start(dev, s);
 
 	return 1;
 }
 
+static void pci230_handle_ai(struct comedi_device *dev,
+			     struct comedi_subdevice *s)
+{
+	struct pci230_private *devpriv = dev->private;
+	unsigned int events = 0;
+	unsigned int status_fifo;
+	unsigned int i;
+	unsigned int todo;
+	unsigned int fifoamount;
+	struct comedi_async *async = s->async;
+	unsigned int scanlen = async->cmd.scan_end_arg;
+
+	/* Determine number of samples to read. */
+	if (devpriv->ai_continuous) {
+		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
+	} else if (devpriv->ai_scan_count == 0) {
+		todo = 0;
+	} else if ((devpriv->ai_scan_count > PCI230_ADC_FIFOLEVEL_HALFFULL)
+		   || (scanlen > PCI230_ADC_FIFOLEVEL_HALFFULL)) {
+		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
+	} else {
+		todo = (devpriv->ai_scan_count * scanlen)
+		    - devpriv->ai_scan_pos;
+		if (todo > PCI230_ADC_FIFOLEVEL_HALFFULL)
+			todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
+	}
+	if (todo == 0)
+		return;
+	fifoamount = 0;
+	for (i = 0; i < todo; i++) {
+		if (fifoamount == 0) {
+			/* Read FIFO state. */
+			status_fifo = inw(dev->iobase + PCI230_ADCCON);
+			if ((status_fifo & PCI230_ADC_FIFO_FULL_LATCHED) != 0) {
+				/* Report error otherwise FIFO overruns will go
+				 * unnoticed by the caller. */
+				comedi_error(dev, "AI FIFO overrun");
+				events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
+				break;
+			} else if ((status_fifo & PCI230_ADC_FIFO_EMPTY) != 0) {
+				/* FIFO empty. */
+				break;
+			} else if ((status_fifo & PCI230_ADC_FIFO_HALF) != 0) {
+				/* FIFO half full. */
+				fifoamount = PCI230_ADC_FIFOLEVEL_HALFFULL;
+			} else {
+				/* FIFO not empty. */
+				if (devpriv->hwver > 0) {
+					/* Read PCI230+/260+ ADC FIFO level. */
+					fifoamount = inw(dev->iobase
+							 + PCI230P_ADCFFLEV);
+					if (fifoamount == 0) {
+						/* Shouldn't happen. */
+						break;
+					}
+				} else {
+					fifoamount = 1;
+				}
+			}
+		}
+		/* Read sample and store in Comedi's circular buffer. */
+		if (comedi_buf_put(async, pci230_ai_read(dev)) == 0) {
+			events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
+			comedi_error(dev, "AI buffer overflow");
+			break;
+		}
+		fifoamount--;
+		devpriv->ai_scan_pos++;
+		if (devpriv->ai_scan_pos == scanlen) {
+			/* End of scan. */
+			devpriv->ai_scan_pos = 0;
+			devpriv->ai_scan_count--;
+			async->events |= COMEDI_CB_EOS;
+		}
+	}
+	if (!devpriv->ai_continuous && (devpriv->ai_scan_count == 0)) {
+		/* End of acquisition. */
+		events |= COMEDI_CB_EOA;
+	} else {
+		/* More samples required, tell Comedi to block. */
+		events |= COMEDI_CB_BLOCK;
+	}
+	async->events |= events;
+	if ((async->events & (COMEDI_CB_EOA | COMEDI_CB_ERROR |
+			      COMEDI_CB_OVERFLOW)) != 0) {
+		/* disable hardware conversions */
+		pci230_ai_stop(dev, s);
+	} else {
+		/* update FIFO interrupt trigger level */
+		pci230_ai_update_fifo_trigger_level(dev, s);
+	}
+}
+
 static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned int i, chan, range, diff;
 	unsigned int res_mask;
 	unsigned short adccon, adcen;
@@ -2462,81 +2581,11 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-static unsigned int divide_ns(uint64_t ns, unsigned int timebase,
-			      unsigned int round_mode)
-{
-	uint64_t div;
-	unsigned int rem;
-
-	div = ns;
-	rem = do_div(div, timebase);
-	round_mode &= TRIG_ROUND_MASK;
-	switch (round_mode) {
-	default:
-	case TRIG_ROUND_NEAREST:
-		div += (rem + (timebase / 2)) / timebase;
-		break;
-	case TRIG_ROUND_DOWN:
-		break;
-	case TRIG_ROUND_UP:
-		div += (rem + timebase - 1) / timebase;
-		break;
-	}
-	return div > UINT_MAX ? UINT_MAX : (unsigned int)div;
-}
-
-/* Given desired period in ns, returns the required internal clock source
- * and gets the initial count. */
-static unsigned int pci230_choose_clk_count(uint64_t ns, unsigned int *count,
-					    unsigned int round_mode)
-{
-	unsigned int clk_src, cnt;
-
-	for (clk_src = CLK_10MHZ;; clk_src++) {
-		cnt = divide_ns(ns, pci230_timebase[clk_src], round_mode);
-		if ((cnt <= 65536) || (clk_src == CLK_1KHZ))
-			break;
-
-	}
-	*count = cnt;
-	return clk_src;
-}
-
-static void pci230_ns_to_single_timer(unsigned int *ns, unsigned int round)
-{
-	unsigned int count;
-	unsigned int clk_src;
-
-	clk_src = pci230_choose_clk_count(*ns, &count, round);
-	*ns = count * pci230_timebase[clk_src];
-	return;
-}
-
-static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
-				    unsigned int mode, uint64_t ns,
-				    unsigned int round)
-{
-	unsigned int clk_src;
-	unsigned int count;
-
-	/* Set mode. */
-	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct, mode);
-	/* Determine clock source and count. */
-	clk_src = pci230_choose_clk_count(ns, &count, round);
-	/* Program clock source. */
-	outb(CLK_CONFIG(ct, clk_src), devpriv->iobase1 + PCI230_ZCLK_SCE);
-	/* Set initial count. */
-	if (count >= 65536)
-		count = 0;
-
-	i8254_write(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct, count);
-}
-
-static void pci230_cancel_ct(struct comedi_device *dev, unsigned int ct)
+static int pci230_ai_cancel(struct comedi_device *dev,
+			    struct comedi_subdevice *s)
 {
-	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct,
-		       I8254_MODE1);
-	/* Counter ct, 8254 mode 1, initial count not written. */
+	pci230_ai_stop(dev, s);
+	return 0;
 }
 
 /* Interrupt handler */
@@ -2544,6 +2593,7 @@ static irqreturn_t pci230_interrupt(int irq, void *d)
 {
 	unsigned char status_int, valid_status_int;
 	struct comedi_device *dev = (struct comedi_device *)d;
+	struct pci230_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	unsigned long irqflags;
 
@@ -2603,373 +2653,302 @@ static irqreturn_t pci230_interrupt(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static void pci230_handle_ao_nofifo(struct comedi_device *dev,
-				    struct comedi_subdevice *s)
+/* Check if PCI device matches a specific board. */
+static bool pci230_match_pci_board(const struct pci230_board *board,
+				   struct pci_dev *pci_dev)
 {
-	short data;
-	int i, ret;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
-
-	if (!devpriv->ao_continuous && (devpriv->ao_scan_count == 0))
-		return;
-
+	/* assume pci_dev->device != PCI_DEVICE_ID_INVALID */
+	if (board->id != pci_dev->device)
+		return false;
+	if (board->min_hwver == 0)
+		return true;
+	/* Looking for a '+' model.  First check length of registers. */
+	if (pci_resource_len(pci_dev, 3) < 32)
+		return false;	/* Not a '+' model. */
+	/* TODO: temporarily enable PCI device and read the hardware version
+	 * register.  For now, assume it's okay. */
+	return true;
+}
 
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		/* Read sample from Comedi's circular buffer. */
-		ret = comedi_buf_get(s->async, &data);
-		if (ret == 0) {
-			s->async->events |= COMEDI_CB_OVERFLOW;
-			pci230_ao_stop(dev, s);
-			comedi_error(dev, "AO buffer underrun");
-			return;
-		}
-		/* Write value to DAC. */
-		pci230_ao_write_nofifo(dev, data, CR_CHAN(cmd->chanlist[i]));
-	}
+/* Look for board matching PCI device. */
+static const struct pci230_board *pci230_find_pci_board(struct pci_dev *pci_dev)
+{
+	unsigned int i;
 
-	async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-	if (!devpriv->ao_continuous) {
-		devpriv->ao_scan_count--;
-		if (devpriv->ao_scan_count == 0) {
-			/* End of acquisition. */
-			async->events |= COMEDI_CB_EOA;
-			pci230_ao_stop(dev, s);
-		}
-	}
+	for (i = 0; i < ARRAY_SIZE(pci230_boards); i++)
+		if (pci230_match_pci_board(&pci230_boards[i], pci_dev))
+			return &pci230_boards[i];
+	return NULL;
 }
 
-/* Loads DAC FIFO (if using it) from buffer. */
-/* Returns 0 if AO finished due to completion or error, 1 if still going. */
-static int pci230_handle_ao_fifo(struct comedi_device *dev,
-				 struct comedi_subdevice *s)
+/* Look for PCI device matching requested board name, bus and slot. */
+static struct pci_dev *pci230_find_pci_dev(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
 {
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
-	unsigned int num_scans;
-	unsigned int room;
-	unsigned short dacstat;
-	unsigned int i, n;
-	unsigned int bytes_per_scan;
-	unsigned int events = 0;
-	int running;
-
-	/* Get DAC FIFO status. */
-	dacstat = inw(dev->iobase + PCI230_DACCON);
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci_dev *pci_dev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 
-	/* Determine number of scans available in buffer. */
-	bytes_per_scan = cmd->chanlist_len * sizeof(short);
-	num_scans = comedi_buf_read_n_available(async) / bytes_per_scan;
-	if (!devpriv->ao_continuous) {
-		/* Fixed number of scans. */
-		if (num_scans > devpriv->ao_scan_count)
-			num_scans = devpriv->ao_scan_count;
+	for_each_pci_dev(pci_dev) {
+		/* Check vendor ID (same for all supported PCI boards). */
+		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
+			continue;
+		/* If bus/slot specified, check them. */
+		if ((bus || slot) &&
+		    (bus != pci_dev->bus->number ||
+		     slot != PCI_SLOT(pci_dev->devfn)))
+			continue;
+		if (thisboard->id == PCI_DEVICE_ID_INVALID) {
+			/* Wildcard board matches any supported PCI board. */
+			const struct pci230_board *foundboard;
 
-		if (devpriv->ao_scan_count == 0) {
-			/* End of acquisition. */
-			events |= COMEDI_CB_EOA;
-		}
-	}
-	if (events == 0) {
-		/* Check for FIFO underrun. */
-		if ((dacstat & PCI230P2_DAC_FIFO_UNDERRUN_LATCHED) != 0) {
-			comedi_error(dev, "AO FIFO underrun");
-			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
-		}
-		/* Check for buffer underrun if FIFO less than half full
-		 * (otherwise there will be loads of "DAC FIFO not half full"
-		 * interrupts). */
-		if ((num_scans == 0)
-		    && ((dacstat & PCI230P2_DAC_FIFO_HALF) == 0)) {
-			comedi_error(dev, "AO buffer underrun");
-			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
+			foundboard = pci230_find_pci_board(pci_dev);
+			if (foundboard == NULL)
+				continue;
+			/* Replace wildcard board_ptr. */
+			dev->board_ptr = foundboard;
+		} else {
+			/* Need to match a specific board. */
+			if (!pci230_match_pci_board(thisboard, pci_dev))
+				continue;
 		}
+		return pci_dev;
 	}
-	if (events == 0) {
-		/* Determine how much room is in the FIFO (in samples). */
-		if ((dacstat & PCI230P2_DAC_FIFO_FULL) != 0)
-			room = PCI230P2_DAC_FIFOROOM_FULL;
-		else if ((dacstat & PCI230P2_DAC_FIFO_HALF) != 0)
-			room = PCI230P2_DAC_FIFOROOM_HALFTOFULL;
-		else if ((dacstat & PCI230P2_DAC_FIFO_EMPTY) != 0)
-			room = PCI230P2_DAC_FIFOROOM_EMPTY;
-		else
-			room = PCI230P2_DAC_FIFOROOM_ONETOHALF;
-
-		/* Convert room to number of scans that can be added. */
-		room /= cmd->chanlist_len;
-		/* Determine number of scans to process. */
-		if (num_scans > room)
-			num_scans = room;
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
 
-		/* Process scans. */
-		for (n = 0; n < num_scans; n++) {
-			for (i = 0; i < cmd->chanlist_len; i++) {
-				short datum;
+static int pci230_alloc_private(struct comedi_device *dev)
+{
+	struct pci230_private *devpriv;
+	int err;
 
-				comedi_buf_get(async, &datum);
-				pci230_ao_write_fifo(dev, datum,
-						     CR_CHAN(cmd->chanlist[i]));
-			}
-		}
-		events |= COMEDI_CB_EOS | COMEDI_CB_BLOCK;
-		if (!devpriv->ao_continuous) {
-			devpriv->ao_scan_count -= num_scans;
-			if (devpriv->ao_scan_count == 0) {
-				/* All data for the command has been written
-				 * to FIFO.  Set FIFO interrupt trigger level
-				 * to 'empty'. */
-				devpriv->daccon = (devpriv->daccon
-						   &
-						   ~PCI230P2_DAC_INT_FIFO_MASK)
-				    | PCI230P2_DAC_INT_FIFO_EMPTY;
-				outw(devpriv->daccon,
-				     dev->iobase + PCI230_DACCON);
-			}
-		}
-		/* Check if FIFO underrun occurred while writing to FIFO. */
-		dacstat = inw(dev->iobase + PCI230_DACCON);
-		if ((dacstat & PCI230P2_DAC_FIFO_UNDERRUN_LATCHED) != 0) {
-			comedi_error(dev, "AO FIFO underrun");
-			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
-		}
-	}
-	if ((events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
-	    != 0) {
-		/* Stopping AO due to completion or error. */
-		pci230_ao_stop(dev, s);
-		running = 0;
-	} else {
-		running = 1;
+	/* sets dev->private to allocated memory */
+	err = alloc_private(dev, sizeof(struct pci230_private));
+	if (err) {
+		dev_err(dev->class_dev, "error! out of memory!\n");
+		return err;
 	}
-	async->events |= events;
-	return running;
+	devpriv = dev->private;
+	spin_lock_init(&devpriv->isr_spinlock);
+	spin_lock_init(&devpriv->res_spinlock);
+	spin_lock_init(&devpriv->ai_stop_spinlock);
+	spin_lock_init(&devpriv->ao_stop_spinlock);
+	return 0;
 }
 
-static void pci230_handle_ai(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
+/* Common part of attach and attach_pci. */
+static int pci230_attach_common(struct comedi_device *dev,
+				struct pci_dev *pci_dev)
 {
-	unsigned int events = 0;
-	unsigned int status_fifo;
-	unsigned int i;
-	unsigned int todo;
-	unsigned int fifoamount;
-	struct comedi_async *async = s->async;
-	unsigned int scanlen = async->cmd.scan_end_arg;
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci230_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	unsigned long iobase1, iobase2;
+	/* PCI230's I/O spaces 1 and 2 respectively. */
+	int irq_hdl, rc;
 
-	/* Determine number of samples to read. */
-	if (devpriv->ai_continuous) {
-		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
-	} else if (devpriv->ai_scan_count == 0) {
-		todo = 0;
-	} else if ((devpriv->ai_scan_count > PCI230_ADC_FIFOLEVEL_HALFFULL)
-		   || (scanlen > PCI230_ADC_FIFOLEVEL_HALFFULL)) {
-		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
-	} else {
-		todo = (devpriv->ai_scan_count * scanlen)
-		    - devpriv->ai_scan_pos;
-		if (todo > PCI230_ADC_FIFOLEVEL_HALFFULL)
-			todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
+	comedi_set_hw_dev(dev, &pci_dev->dev);
 
+	dev->board_name = thisboard->name;
+	/* Enable PCI device and reserve I/O spaces. */
+	if (comedi_pci_enable(pci_dev, "amplc_pci230") < 0) {
+		dev_err(dev->class_dev,
+			"failed to enable PCI device and request regions\n");
+		return -EIO;
 	}
+	/* Read base addresses of the PCI230's two I/O regions from PCI
+	 * configuration register. */
+	iobase1 = pci_resource_start(pci_dev, 2);
+	iobase2 = pci_resource_start(pci_dev, 3);
+	dev_dbg(dev->class_dev,
+		"%s I/O region 1 0x%04lx I/O region 2 0x%04lx\n",
+		dev->board_name, iobase1, iobase2);
+	devpriv->iobase1 = iobase1;
+	dev->iobase = iobase2;
+	/* Read bits of DACCON register - only the output range. */
+	devpriv->daccon = inw(dev->iobase + PCI230_DACCON) & PCI230_DAC_OR_MASK;
+	/* Read hardware version register and set extended function register
+	 * if they exist. */
+	if (pci_resource_len(pci_dev, 3) >= 32) {
+		unsigned short extfunc = 0;
 
-	if (todo == 0)
-		return;
-
-
-	fifoamount = 0;
-	for (i = 0; i < todo; i++) {
-		if (fifoamount == 0) {
-			/* Read FIFO state. */
-			status_fifo = inw(dev->iobase + PCI230_ADCCON);
-
-			if ((status_fifo & PCI230_ADC_FIFO_FULL_LATCHED) != 0) {
-				/* Report error otherwise FIFO overruns will go
-				 * unnoticed by the caller. */
-				comedi_error(dev, "AI FIFO overrun");
-				events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
-				break;
-			} else if ((status_fifo & PCI230_ADC_FIFO_EMPTY) != 0) {
-				/* FIFO empty. */
-				break;
-			} else if ((status_fifo & PCI230_ADC_FIFO_HALF) != 0) {
-				/* FIFO half full. */
-				fifoamount = PCI230_ADC_FIFOLEVEL_HALFFULL;
-			} else {
-				/* FIFO not empty. */
-				if (devpriv->hwver > 0) {
-					/* Read PCI230+/260+ ADC FIFO level. */
-					fifoamount = inw(dev->iobase
-							 + PCI230P_ADCFFLEV);
-					if (fifoamount == 0) {
-						/* Shouldn't happen. */
-						break;
-					}
-				} else {
-					fifoamount = 1;
-				}
-			}
+		devpriv->hwver = inw(dev->iobase + PCI230P_HWVER);
+		if (devpriv->hwver < thisboard->min_hwver) {
+			dev_err(dev->class_dev,
+				"%s - bad hardware version - got %u, need %u\n",
+				dev->board_name, devpriv->hwver,
+				thisboard->min_hwver);
+			return -EIO;
 		}
-
-		/* Read sample and store in Comedi's circular buffer. */
-		if (comedi_buf_put(async, pci230_ai_read(dev)) == 0) {
-			events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
-			comedi_error(dev, "AI buffer overflow");
-			break;
+		if (devpriv->hwver > 0) {
+			if (!thisboard->have_dio) {
+				/* No DIO ports.  Route counters' external gates
+				 * to the EXTTRIG signal (PCI260+ pin 17).
+				 * (Otherwise, they would be routed to DIO
+				 * inputs PC0, PC1 and PC2 which don't exist
+				 * on PCI260[+].) */
+				extfunc |= PCI230P_EXTFUNC_GAT_EXTTRIG;
+			}
+			if ((thisboard->ao_chans > 0)
+			    && (devpriv->hwver >= 2)) {
+				/* Enable DAC FIFO functionality. */
+				extfunc |= PCI230P2_EXTFUNC_DACFIFO;
+			}
 		}
-		fifoamount--;
-		devpriv->ai_scan_pos++;
-		if (devpriv->ai_scan_pos == scanlen) {
-			/* End of scan. */
-			devpriv->ai_scan_pos = 0;
-			devpriv->ai_scan_count--;
-			async->events |= COMEDI_CB_EOS;
+		outw(extfunc, dev->iobase + PCI230P_EXTFUNC);
+		if ((extfunc & PCI230P2_EXTFUNC_DACFIFO) != 0) {
+			/* Temporarily enable DAC FIFO, reset it and disable
+			 * FIFO wraparound. */
+			outw(devpriv->daccon | PCI230P2_DAC_FIFO_EN
+			     | PCI230P2_DAC_FIFO_RESET,
+			     dev->iobase + PCI230_DACCON);
+			/* Clear DAC FIFO channel enable register. */
+			outw(0, dev->iobase + PCI230P2_DACEN);
+			/* Disable DAC FIFO. */
+			outw(devpriv->daccon, dev->iobase + PCI230_DACCON);
 		}
 	}
-
-	if (!devpriv->ai_continuous && (devpriv->ai_scan_count == 0)) {
-		/* End of acquisition. */
-		events |= COMEDI_CB_EOA;
-	} else {
-		/* More samples required, tell Comedi to block. */
-		events |= COMEDI_CB_BLOCK;
-	}
-	async->events |= events;
-
-	if ((async->events & (COMEDI_CB_EOA | COMEDI_CB_ERROR |
-			      COMEDI_CB_OVERFLOW)) != 0) {
-		/* disable hardware conversions */
-		pci230_ai_stop(dev, s);
+	/* Disable board's interrupts. */
+	outb(0, devpriv->iobase1 + PCI230_INT_SCE);
+	/* Set ADC to a reasonable state. */
+	devpriv->adcg = 0;
+	devpriv->adccon = PCI230_ADC_TRIG_NONE | PCI230_ADC_IM_SE
+	    | PCI230_ADC_IR_BIP;
+	outw(1 << 0, dev->iobase + PCI230_ADCEN);
+	outw(devpriv->adcg, dev->iobase + PCI230_ADCG);
+	outw(devpriv->adccon | PCI230_ADC_FIFO_RESET,
+	     dev->iobase + PCI230_ADCCON);
+	/* Register the interrupt handler. */
+	irq_hdl = request_irq(pci_dev->irq, pci230_interrupt,
+			      IRQF_SHARED, "amplc_pci230", dev);
+	if (irq_hdl < 0) {
+		dev_warn(dev->class_dev,
+			 "unable to register irq %u, commands will not be available\n",
+			 pci_dev->irq);
 	} else {
-		/* update FIFO interrupt trigger level */
-		pci230_ai_update_fifo_trigger_level(dev, s);
+		dev->irq = pci_dev->irq;
+		dev_dbg(dev->class_dev, "registered irq %u\n", pci_dev->irq);
 	}
-}
-
-static void pci230_ao_stop(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	unsigned long irqflags;
-	unsigned char intsrc;
-	int started;
-	struct comedi_cmd *cmd;
-
-	spin_lock_irqsave(&devpriv->ao_stop_spinlock, irqflags);
-	started = test_and_clear_bit(AO_CMD_STARTED, &devpriv->state);
-	spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
-	if (!started)
-		return;
 
+	rc = comedi_alloc_subdevices(dev, 3);
+	if (rc)
+		return rc;
 
-	cmd = &s->async->cmd;
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		/* Stop scan rate generator. */
-		pci230_cancel_ct(dev, 1);
+	s = dev->subdevices + 0;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = &pci230_ai_range;
+	s->insn_read = &pci230_ai_rinsn;
+	s->len_chanlist = 256;	/* but there are restrictions. */
+	/* Only register commands if the interrupt handler is installed. */
+	if (irq_hdl == 0) {
+		dev->read_subdev = s;
+		s->subdev_flags |= SDF_CMD_READ;
+		s->do_cmd = &pci230_ai_cmd;
+		s->do_cmdtest = &pci230_ai_cmdtest;
+		s->cancel = pci230_ai_cancel;
 	}
-
-	/* Determine interrupt source. */
-	if (devpriv->hwver < 2) {
-		/* Not using DAC FIFO.  Using CT1 interrupt. */
-		intsrc = PCI230_INT_ZCLK_CT1;
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	if (thisboard->ao_chans > 0) {
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
+		s->n_chan = thisboard->ao_chans;
+		s->maxdata = (1 << thisboard->ao_bits) - 1;
+		s->range_table = &pci230_ao_range;
+		s->insn_write = &pci230_ao_winsn;
+		s->insn_read = &pci230_ao_rinsn;
+		s->len_chanlist = thisboard->ao_chans;
+		/* Only register commands if the interrupt handler is
+		 * installed. */
+		if (irq_hdl == 0) {
+			dev->write_subdev = s;
+			s->subdev_flags |= SDF_CMD_WRITE;
+			s->do_cmd = &pci230_ao_cmd;
+			s->do_cmdtest = &pci230_ao_cmdtest;
+			s->cancel = pci230_ao_cancel;
+		}
 	} else {
-		/* Using DAC FIFO interrupt. */
-		intsrc = PCI230P2_INT_DAC;
-	}
-	/* Disable interrupt and wait for interrupt routine to finish running
-	 * unless we are called from the interrupt routine. */
-	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	devpriv->int_en &= ~intsrc;
-	while (devpriv->intr_running && devpriv->intr_cpuid != THISCPU) {
-		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
-		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	}
-	if (devpriv->ier != devpriv->int_en) {
-		devpriv->ier = devpriv->int_en;
-		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
+		s->type = COMEDI_SUBD_UNUSED;
 	}
-	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
-
-	if (devpriv->hwver >= 2) {
-		/* Using DAC FIFO.  Reset FIFO, clear underrun error,
-		 * disable FIFO. */
-		devpriv->daccon &= PCI230_DAC_OR_MASK;
-		outw(devpriv->daccon | PCI230P2_DAC_FIFO_RESET
-		     | PCI230P2_DAC_FIFO_UNDERRUN_CLEAR,
-		     dev->iobase + PCI230_DACCON);
+	s = dev->subdevices + 2;
+	/* digital i/o subdevice */
+	if (thisboard->have_dio) {
+		rc = subdev_8255_init(dev, s, NULL,
+				      (devpriv->iobase1 + PCI230_PPI_X_BASE));
+		if (rc < 0)
+			return rc;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
 	}
-
-	/* Release resources. */
-	put_all_resources(dev, OWNER_AOCMD);
+	dev_info(dev->class_dev, "attached\n");
+	return 1;
 }
 
-static int pci230_ao_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s)
+static int pci230_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	pci230_ao_stop(dev, s);
-	return 0;
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci_dev *pci_dev;
+	int rc;
+
+	dev_info(dev->class_dev, "amplc_pci230: attach %s %d,%d\n",
+		 thisboard->name, it->options[0], it->options[1]);
+	rc = pci230_alloc_private(dev); /* sets dev->private */
+	if (rc)
+		return rc;
+	pci_dev = pci230_find_pci_dev(dev, it);
+	if (!pci_dev)
+		return -EIO;
+	return pci230_attach_common(dev, pci_dev);
 }
 
-static void pci230_ai_stop(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
+static int __devinit pci230_attach_pci(struct comedi_device *dev,
+				       struct pci_dev *pci_dev)
 {
-	unsigned long irqflags;
-	struct comedi_cmd *cmd;
-	int started;
-
-	spin_lock_irqsave(&devpriv->ai_stop_spinlock, irqflags);
-	started = test_and_clear_bit(AI_CMD_STARTED, &devpriv->state);
-	spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
-	if (!started)
-		return;
-
-
-	cmd = &s->async->cmd;
-	if (cmd->convert_src == TRIG_TIMER) {
-		/* Stop conversion rate generator. */
-		pci230_cancel_ct(dev, 2);
-	}
-	if (cmd->scan_begin_src != TRIG_FOLLOW) {
-		/* Stop scan period monostable. */
-		pci230_cancel_ct(dev, 0);
-	}
-
-	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	/* Disable ADC interrupt and wait for interrupt routine to finish
-	 * running unless we are called from the interrupt routine. */
-	devpriv->int_en &= ~PCI230_INT_ADC;
-	while (devpriv->intr_running && devpriv->intr_cpuid != THISCPU) {
-		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
-		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	}
-	if (devpriv->ier != devpriv->int_en) {
-		devpriv->ier = devpriv->int_en;
-		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
+	int rc;
+
+	dev_info(dev->class_dev, "amplc_pci230: attach pci %s\n",
+		 pci_name(pci_dev));
+	rc = pci230_alloc_private(dev); /* sets dev->private */
+	if (rc)
+		return rc;
+	dev->board_ptr = pci230_find_pci_board(pci_dev);
+	if (dev->board_ptr == NULL) {
+		dev_err(dev->class_dev,
+			"amplc_pci230: BUG! cannot determine board type!\n");
+		return -EINVAL;
 	}
-	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
-
-	/* Reset FIFO, disable FIFO and set start conversion source to none.
-	 * Keep se/diff and bip/uni settings */
-	devpriv->adccon = (devpriv->adccon & (PCI230_ADC_IR_MASK
-					      | PCI230_ADC_IM_MASK)) |
-	    PCI230_ADC_TRIG_NONE;
-	outw(devpriv->adccon | PCI230_ADC_FIFO_RESET,
-	     dev->iobase + PCI230_ADCCON);
-
-	/* Release resources. */
-	put_all_resources(dev, OWNER_AICMD);
+	return pci230_attach_common(dev, pci_dev);
 }
 
-static int pci230_ai_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s)
+static void pci230_detach(struct comedi_device *dev)
 {
-	pci230_ai_stop(dev, s);
-	return 0;
+	const struct pci230_board *thisboard = comedi_board(dev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (dev->subdevices && thisboard->have_dio)
+		subdev_8255_cleanup(dev, dev->subdevices + 2);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
+	}
 }
 
 static struct comedi_driver amplc_pci230_driver = {
 	.driver_name	= "amplc_pci230",
 	.module		= THIS_MODULE,
 	.attach		= pci230_attach,
+	.attach_pci	= pci230_attach_pci,
 	.detach		= pci230_detach,
 	.board_name	= &pci230_boards[0].name,
 	.offset		= sizeof(pci230_boards[0]),
diff --git a/drivers/staging/comedi/drivers/c6xdigio.c b/drivers/staging/comedi/drivers/c6xdigio.c
index fb9951a746a..41ed8576f30 100644
--- a/drivers/staging/comedi/drivers/c6xdigio.c
+++ b/drivers/staging/comedi/drivers/c6xdigio.c
@@ -433,8 +433,8 @@ static int c6xdigio_attach(struct comedi_device *dev,
 	dev->iobase = iobase;
 	dev->board_name = "c6xdigio";
 
-	result = alloc_subdevices(dev, 2);	/*  3 with encoder_init write */
-	if (result < 0)
+	result = comedi_alloc_subdevices(dev, 2);
+	if (result)
 		return result;
 
 	/*  Make sure that PnP ports get activated */
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 35159235a1b..58d45299bf8 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -19,6 +19,13 @@
     along with this program; if not, write to the Free Software
     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 
+    PCMCIA support code for this driver is adapted from the dummy_cs.c
+    driver of the Linux PCMCIA Card Services package.
+
+    The initial developer of the original code is David A. Hinds
+    <dahinds@users.sourceforge.net>.  Portions created by David A. Hinds
+    are Copyright (C) 1999 David A. Hinds.  All Rights Reserved.
+
 */
 /*
 Driver: cb_das16_cs
@@ -35,7 +42,6 @@ Status: experimental
 #include <linux/slab.h>
 #include "../comedidev.h"
 #include <linux/delay.h>
-#include <linux/pci.h>
 
 #include <pcmcia/cistpl.h>
 #include <pcmcia/ds.h>
@@ -59,207 +65,39 @@ struct das16cs_board {
 	int device_id;
 	int n_ao_chans;
 };
+
 static const struct das16cs_board das16cs_boards[] = {
 	{
-	 .device_id = 0x0000,	/* unknown */
-	 .name = "PC-CARD DAS16/16",
-	 .n_ao_chans = 0,
-	 },
-	{
-	 .device_id = 0x0039,
-	 .name = "PC-CARD DAS16/16-AO",
-	 .n_ao_chans = 2,
-	 },
-	{
-	 .device_id = 0x4009,
-	 .name = "PCM-DAS16s/16",
-	 .n_ao_chans = 0,
-	 },
+		.name		= "PC-CARD DAS16/16-AO",
+		.device_id	= 0x0039,
+		.n_ao_chans	= 2,
+	}, {
+		.name		= "PCM-DAS16s/16",
+		.device_id	= 0x4009,
+		.n_ao_chans	= 0,
+	}, {
+		.name		= "PC-CARD DAS16/16",
+		.device_id	= 0x0000,	/* unknown */
+		.n_ao_chans	= 0,
+	},
 };
 
-#define n_boards ARRAY_SIZE(das16cs_boards)
-#define thisboard ((const struct das16cs_board *)dev->board_ptr)
-
 struct das16cs_private {
-	struct pcmcia_device *link;
-
 	unsigned int ao_readback[2];
 	unsigned short status1;
 	unsigned short status2;
 };
-#define devpriv ((struct das16cs_private *)dev->private)
-
-static int das16cs_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static void das16cs_detach(struct comedi_device *dev);
-static struct comedi_driver driver_das16cs = {
-	.driver_name = "cb_das16_cs",
-	.module = THIS_MODULE,
-	.attach = das16cs_attach,
-	.detach = das16cs_detach,
-};
 
 static struct pcmcia_device *cur_dev;
 
-static const struct comedi_lrange das16cs_ai_range = { 4, {
-							   RANGE(-10, 10),
-							   RANGE(-5, 5),
-							   RANGE(-2.5, 2.5),
-							   RANGE(-1.25, 1.25),
-							   }
-};
-
-static irqreturn_t das16cs_interrupt(int irq, void *d);
-static int das16cs_ai_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das16cs_ai_cmd(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-static int das16cs_ai_cmdtest(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_cmd *cmd);
-static int das16cs_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das16cs_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das16cs_dio_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int das16cs_dio_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-static int das16cs_timer_insn_read(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-static int das16cs_timer_insn_config(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data);
-
-static const struct das16cs_board *das16cs_probe(struct comedi_device *dev,
-						 struct pcmcia_device *link)
-{
-	int i;
-
-	for (i = 0; i < n_boards; i++) {
-		if (das16cs_boards[i].device_id == link->card_id)
-			return das16cs_boards + i;
-	}
-
-	dev_dbg(dev->hw_dev, "unknown board!\n");
-
-	return NULL;
-}
-
-static int das16cs_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct pcmcia_device *link;
-	struct comedi_subdevice *s;
-	int ret;
-	int i;
-
-	dev_dbg(dev->hw_dev, "comedi%d: cb_das16_cs: attached\n", dev->minor);
-
-	link = cur_dev;		/* XXX hack */
-	if (!link)
-		return -EIO;
-
-	dev->iobase = link->resource[0]->start;
-	dev_dbg(dev->hw_dev, "I/O base=0x%04lx\n", dev->iobase);
-
-	dev_dbg(dev->hw_dev, "fingerprint:\n");
-	for (i = 0; i < 48; i += 2)
-		dev_dbg(dev->hw_dev, "%04x\n", inw(dev->iobase + i));
-
-
-	ret = request_irq(link->irq, das16cs_interrupt,
-			  IRQF_SHARED, "cb_das16_cs", dev);
-	if (ret < 0)
-		return ret;
-
-	dev->irq = link->irq;
-
-	dev_dbg(dev->hw_dev, "irq=%u\n", dev->irq);
-
-	dev->board_ptr = das16cs_probe(dev, link);
-	if (!dev->board_ptr)
-		return -EIO;
-
-	dev->board_name = thisboard->name;
-
-	if (alloc_private(dev, sizeof(struct das16cs_private)) < 0)
-		return -ENOMEM;
-
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	dev->read_subdev = s;
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = 16;
-	s->maxdata = 0xffff;
-	s->range_table = &das16cs_ai_range;
-	s->len_chanlist = 16;
-	s->insn_read = das16cs_ai_rinsn;
-	s->do_cmd = das16cs_ai_cmd;
-	s->do_cmdtest = das16cs_ai_cmdtest;
-
-	s = dev->subdevices + 1;
-	/* analog output subdevice */
-	if (thisboard->n_ao_chans) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = thisboard->n_ao_chans;
-		s->maxdata = 0xffff;
-		s->range_table = &range_bipolar10;
-		s->insn_write = &das16cs_ao_winsn;
-		s->insn_read = &das16cs_ao_rinsn;
-	}
-
-	s = dev->subdevices + 2;
-	/* digital i/o subdevice */
-	if (1) {
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 8;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->insn_bits = das16cs_dio_insn_bits;
-		s->insn_config = das16cs_dio_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	s = dev->subdevices + 3;
-	/* timer subdevice */
-	if (0) {
-		s->type = COMEDI_SUBD_TIMER;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 1;
-		s->maxdata = 0xff;
-		s->range_table = &range_unknown;
-		s->insn_read = das16cs_timer_insn_read;
-		s->insn_config = das16cs_timer_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
+static const struct comedi_lrange das16cs_ai_range = {
+	4, {
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1.25),
 	}
-
-
-	return 1;
-}
-
-static void das16cs_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-}
+};
 
 static irqreturn_t das16cs_interrupt(int irq, void *d)
 {
@@ -267,48 +105,53 @@ static irqreturn_t das16cs_interrupt(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-/*
- * "instructions" read/write data in "one-shot" or "software-triggered"
- * mode.
- */
 static int das16cs_ai_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct das16cs_private *devpriv = dev->private;
+	int chan = CR_CHAN(insn->chanspec);
+	int range = CR_RANGE(insn->chanspec);
+	int aref = CR_AREF(insn->chanspec);
 	int i;
 	int to;
-	int aref;
-	int range;
-	int chan;
-	static int range_bits[] = { 0x800, 0x000, 0x100, 0x200 };
 
-	chan = CR_CHAN(insn->chanspec);
-	aref = CR_AREF(insn->chanspec);
-	range = CR_RANGE(insn->chanspec);
-
-	outw(chan, dev->iobase + 2);
+	outw(chan, dev->iobase + DAS16CS_DIO_MUX);
 
 	devpriv->status1 &= ~0xf320;
 	devpriv->status1 |= (aref == AREF_DIFF) ? 0 : 0x0020;
-	outw(devpriv->status1, dev->iobase + 4);
+	outw(devpriv->status1, dev->iobase + DAS16CS_MISC1);
 
 	devpriv->status2 &= ~0xff00;
-	devpriv->status2 |= range_bits[range];
-	outw(devpriv->status2, dev->iobase + 6);
+	switch (range) {
+	case 0:
+		devpriv->status2 |= 0x800;
+		break;
+	case 1:
+		devpriv->status2 |= 0x000;
+		break;
+	case 2:
+		devpriv->status2 |= 0x100;
+		break;
+	case 3:
+		devpriv->status2 |= 0x200;
+		break;
+	}
+	outw(devpriv->status2, dev->iobase + DAS16CS_MISC2);
 
 	for (i = 0; i < insn->n; i++) {
-		outw(0, dev->iobase);
+		outw(0, dev->iobase + DAS16CS_ADC_DATA);
 
 #define TIMEOUT 1000
 		for (to = 0; to < TIMEOUT; to++) {
-			if (inw(dev->iobase + 4) & 0x0080)
+			if (inw(dev->iobase + DAS16CS_MISC1) & 0x0080)
 				break;
 		}
 		if (to == TIMEOUT) {
-			dev_dbg(dev->hw_dev, "cb_das16_cs: ai timeout\n");
+			dev_dbg(dev->class_dev, "cb_das16_cs: ai timeout\n");
 			return -ETIME;
 		}
-		data[i] = (unsigned short)inw(dev->iobase + 0);
+		data[i] = inw(dev->iobase + DAS16CS_ADC_DATA);
 	}
 
 	return i;
@@ -326,13 +169,6 @@ static int das16cs_ai_cmdtest(struct comedi_device *dev,
 	int err = 0;
 	int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
-
 	/* step 1: make sure trigger sources are trivially valid */
 
 	tmp = cmd->start_src;
@@ -483,6 +319,7 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct das16cs_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 	unsigned short status1;
@@ -493,7 +330,7 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 		devpriv->ao_readback[chan] = data[i];
 		d = data[i];
 
-		outw(devpriv->status1, dev->iobase + 4);
+		outw(devpriv->status1, dev->iobase + DAS16CS_MISC1);
 		udelay(1);
 
 		status1 = devpriv->status1 & ~0xf;
@@ -502,34 +339,32 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 		else
 			status1 |= 0x0008;
 
-/*		printk("0x%04x\n",status1);*/
-		outw(status1, dev->iobase + 4);
+		outw(status1, dev->iobase + DAS16CS_MISC1);
 		udelay(1);
 
 		for (bit = 15; bit >= 0; bit--) {
 			int b = (d >> bit) & 0x1;
 			b <<= 1;
-/*			printk("0x%04x\n",status1 | b | 0x0000);*/
-			outw(status1 | b | 0x0000, dev->iobase + 4);
+			outw(status1 | b | 0x0000, dev->iobase + DAS16CS_MISC1);
 			udelay(1);
-/*			printk("0x%04x\n",status1 | b | 0x0004);*/
-			outw(status1 | b | 0x0004, dev->iobase + 4);
+			outw(status1 | b | 0x0004, dev->iobase + DAS16CS_MISC1);
 			udelay(1);
 		}
-/*		make high both DAC0CS and DAC1CS to load
-		new data and update analog output*/
-		outw(status1 | 0x9, dev->iobase + 4);
+		/*
+		 * Make both DAC0CS and DAC1CS high to load
+		 * the new data and update analog the output
+		 */
+		outw(status1 | 0x9, dev->iobase + DAS16CS_MISC1);
 	}
 
 	return i;
 }
 
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
 static int das16cs_ao_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct das16cs_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -539,36 +374,27 @@ static int das16cs_ao_rinsn(struct comedi_device *dev,
 	return i;
 }
 
-/* DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The
- * comedi core can convert between insn_bits and insn_read/write */
 static int das16cs_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
 
-		outw(s->state, dev->iobase + 16);
+		outw(s->state, dev->iobase + DAS16CS_DIO);
 	}
 
-	/* on return, data[1] contains the value of the digital
-	 * input and output lines. */
-	data[1] = inw(dev->iobase + 16);
+	data[1] = inw(dev->iobase + DAS16CS_DIO);
 
-	return 2;
+	return insn->n;
 }
 
 static int das16cs_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
+	struct das16cs_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 	int bits;
 
@@ -598,110 +424,116 @@ static int das16cs_dio_insn_config(struct comedi_device *dev,
 	devpriv->status2 |= (s->io_bits & 0xf0) ? 0x0080 : 0;
 	devpriv->status2 |= (s->io_bits & 0x0f) ? 0x0040 : 0;
 
-	outw(devpriv->status2, dev->iobase + 6);
+	outw(devpriv->status2, dev->iobase + DAS16CS_MISC2);
 
 	return insn->n;
 }
 
-static int das16cs_timer_insn_read(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn, unsigned int *data)
-{
-	return -EINVAL;
-}
-
-static int das16cs_timer_insn_config(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data)
+static const struct das16cs_board *das16cs_probe(struct comedi_device *dev,
+						 struct pcmcia_device *link)
 {
-	return -EINVAL;
-}
-
-/* PCMCIA stuff */
-
-/*======================================================================
-
-    The following pcmcia code for the pcm-das08 is adapted from the
-    dummy_cs.c driver of the Linux PCMCIA Card Services package.
-
-    The initial developer of the original code is David A. Hinds
-    <dahinds@users.sourceforge.net>.  Portions created by David A. Hinds
-    are Copyright (C) 1999 David A. Hinds.  All Rights Reserved.
-
-======================================================================*/
-
-#if defined(CONFIG_PCMCIA) || defined(CONFIG_PCMCIA_MODULE)
-
-static void das16cs_pcmcia_config(struct pcmcia_device *link);
-static void das16cs_pcmcia_release(struct pcmcia_device *link);
-static int das16cs_pcmcia_suspend(struct pcmcia_device *p_dev);
-static int das16cs_pcmcia_resume(struct pcmcia_device *p_dev);
+	int i;
 
-/*
-   The attach() and detach() entry points are used to create and destroy
-   "instances" of the driver, where each instance represents everything
-   needed to manage one actual PCMCIA card.
-*/
+	for (i = 0; i < ARRAY_SIZE(das16cs_boards); i++) {
+		if (das16cs_boards[i].device_id == link->card_id)
+			return das16cs_boards + i;
+	}
 
-static int das16cs_pcmcia_attach(struct pcmcia_device *);
-static void das16cs_pcmcia_detach(struct pcmcia_device *);
+	dev_dbg(dev->class_dev, "unknown board!\n");
 
-/*
-   You'll also need to prototype all the functions that will actually
-   be used to talk to your device.  See 'memory_cs' for a good example
-   of a fully self-sufficient driver; the other drivers rely more or
-   less on other parts of the kernel.
-*/
+	return NULL;
+}
 
-struct local_info_t {
+static int das16cs_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	const struct das16cs_board *thisboard;
 	struct pcmcia_device *link;
-	int stop;
-	struct bus_operations *bus;
-};
-
-/*======================================================================
+	struct comedi_subdevice *s;
+	int ret;
 
-    das16cs_pcmcia_attach() creates an "instance" of the driver, allocating
-    local data structures for one device.  The device is registered
-    with Card Services.
+	link = cur_dev;		/* XXX hack */
+	if (!link)
+		return -EIO;
 
-    The dev_link structure is initialized, but we don't actually
-    configure the card at this point -- we wait until we receive a
-    card insertion event.
+	dev->board_ptr = das16cs_probe(dev, link);
+	if (!dev->board_ptr)
+		return -EIO;
+	thisboard = comedi_board(dev);
 
-======================================================================*/
+	dev->board_name = thisboard->name;
 
-static int das16cs_pcmcia_attach(struct pcmcia_device *link)
-{
-	struct local_info_t *local;
+	dev->iobase = link->resource[0]->start;
 
-	dev_dbg(&link->dev, "das16cs_pcmcia_attach()\n");
+	ret = request_irq(link->irq, das16cs_interrupt,
+			  IRQF_SHARED, "cb_das16_cs", dev);
+	if (ret < 0)
+		return ret;
+	dev->irq = link->irq;
 
-	/* Allocate space for private device-specific data */
-	local = kzalloc(sizeof(struct local_info_t), GFP_KERNEL);
-	if (!local)
+	if (alloc_private(dev, sizeof(struct das16cs_private)) < 0)
 		return -ENOMEM;
-	local->link = link;
-	link->priv = local;
 
-	cur_dev = link;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
-	das16cs_pcmcia_config(link);
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan	= 16;
+	s->maxdata	= 0xffff;
+	s->range_table	= &das16cs_ai_range;
+	s->len_chanlist	= 16;
+	s->insn_read	= das16cs_ai_rinsn;
+	s->do_cmd	= das16cs_ai_cmd;
+	s->do_cmdtest	= das16cs_ai_cmdtest;
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	if (thisboard->n_ao_chans) {
+		s->type		= COMEDI_SUBD_AO;
+		s->subdev_flags	= SDF_WRITABLE;
+		s->n_chan	= thisboard->n_ao_chans;
+		s->maxdata	= 0xffff;
+		s->range_table	= &range_bipolar10;
+		s->insn_write	= &das16cs_ao_winsn;
+		s->insn_read	= &das16cs_ao_rinsn;
+	} else {
+		s->type		= COMEDI_SUBD_UNUSED;
+	}
+
+	s = dev->subdevices + 2;
+	/* digital i/o subdevice */
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 8;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= das16cs_dio_insn_bits;
+	s->insn_config	= das16cs_dio_insn_config;
+
+	dev_info(dev->class_dev, "%s: %s, I/O base=0x%04lx, irq=%u\n",
+		dev->driver->driver_name, dev->board_name,
+		dev->iobase, dev->irq);
 
 	return 0;
-}				/* das16cs_pcmcia_attach */
+}
 
-static void das16cs_pcmcia_detach(struct pcmcia_device *link)
+static void das16cs_detach(struct comedi_device *dev)
 {
-	dev_dbg(&link->dev, "das16cs_pcmcia_detach\n");
-
-	((struct local_info_t *)link->priv)->stop = 1;
-	das16cs_pcmcia_release(link);
-	/* This points to the parent struct local_info_t struct */
-	kfree(link->priv);
-}				/* das16cs_pcmcia_detach */
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+}
 
+static struct comedi_driver driver_das16cs = {
+	.driver_name	= "cb_das16_cs",
+	.module		= THIS_MODULE,
+	.attach		= das16cs_attach,
+	.detach		= das16cs_detach,
+};
 
 static int das16cs_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
@@ -712,20 +544,16 @@ static int das16cs_pcmcia_config_loop(struct pcmcia_device *p_dev,
 	return pcmcia_request_io(p_dev);
 }
 
-static void das16cs_pcmcia_config(struct pcmcia_device *link)
+static int das16cs_pcmcia_attach(struct pcmcia_device *link)
 {
 	int ret;
 
-	dev_dbg(&link->dev, "das16cs_pcmcia_config\n");
-
 	/* Do we need to allocate an interrupt? */
 	link->config_flags |= CONF_ENABLE_IRQ | CONF_AUTO_SET_IO;
 
 	ret = pcmcia_loop_config(link, das16cs_pcmcia_config_loop, NULL);
-	if (ret) {
-		dev_warn(&link->dev, "no configuration found\n");
+	if (ret)
 		goto failed;
-	}
 
 	if (!link->irq)
 		goto failed;
@@ -734,99 +562,60 @@ static void das16cs_pcmcia_config(struct pcmcia_device *link)
 	if (ret)
 		goto failed;
 
-	return;
+	cur_dev = link;
+	return 0;
 
 failed:
-	das16cs_pcmcia_release(link);
-}				/* das16cs_pcmcia_config */
-
-static void das16cs_pcmcia_release(struct pcmcia_device *link)
-{
-	dev_dbg(&link->dev, "das16cs_pcmcia_release\n");
 	pcmcia_disable_device(link);
-}				/* das16cs_pcmcia_release */
-
-static int das16cs_pcmcia_suspend(struct pcmcia_device *link)
-{
-	struct local_info_t *local = link->priv;
-
-	/* Mark the device as stopped, to block IO until later */
-	local->stop = 1;
-
-	return 0;
-}				/* das16cs_pcmcia_suspend */
+	return ret;
+}
 
-static int das16cs_pcmcia_resume(struct pcmcia_device *link)
+static void das16cs_pcmcia_detach(struct pcmcia_device *link)
 {
-	struct local_info_t *local = link->priv;
-
-	local->stop = 0;
-	return 0;
-}				/* das16cs_pcmcia_resume */
-
-/*====================================================================*/
+	pcmcia_disable_device(link);
+	cur_dev = NULL;
+}
 
 static const struct pcmcia_device_id das16cs_id_table[] = {
 	PCMCIA_DEVICE_MANF_CARD(0x01c5, 0x0039),
 	PCMCIA_DEVICE_MANF_CARD(0x01c5, 0x4009),
 	PCMCIA_DEVICE_NULL
 };
-
 MODULE_DEVICE_TABLE(pcmcia, das16cs_id_table);
-MODULE_AUTHOR("David A. Schleef <ds@schleef.org>");
-MODULE_DESCRIPTION("Comedi driver for Computer Boards PC-CARD DAS16/16");
-MODULE_LICENSE("GPL");
 
-struct pcmcia_driver das16cs_driver = {
-	.probe = das16cs_pcmcia_attach,
-	.remove = das16cs_pcmcia_detach,
-	.suspend = das16cs_pcmcia_suspend,
-	.resume = das16cs_pcmcia_resume,
-	.id_table = das16cs_id_table,
-	.owner = THIS_MODULE,
-	.name = "cb_das16_cs",
+static struct pcmcia_driver das16cs_driver = {
+	.name		= "cb_das16_cs",
+	.owner		= THIS_MODULE,
+	.probe		= das16cs_pcmcia_attach,
+	.remove		= das16cs_pcmcia_detach,
+	.id_table	= das16cs_id_table,
 };
 
-static int __init init_das16cs_pcmcia_cs(void)
-{
-	pcmcia_register_driver(&das16cs_driver);
-	return 0;
-}
-
-static void __exit exit_das16cs_pcmcia_cs(void)
-{
-	pr_debug("das16cs_pcmcia_cs: unloading\n");
-	pcmcia_unregister_driver(&das16cs_driver);
-}
-
-int __init init_module(void)
+static int __init das16cs_init(void)
 {
 	int ret;
 
-	ret = init_das16cs_pcmcia_cs();
+	ret = comedi_driver_register(&driver_das16cs);
 	if (ret < 0)
 		return ret;
 
-	return comedi_driver_register(&driver_das16cs);
-}
-
-void __exit cleanup_module(void)
-{
-	exit_das16cs_pcmcia_cs();
-	comedi_driver_unregister(&driver_das16cs);
-}
+	ret = pcmcia_register_driver(&das16cs_driver);
+	if (ret < 0) {
+		comedi_driver_unregister(&driver_das16cs);
+		return ret;
+	}
 
-#else
-static int __init driver_das16cs_init_module(void)
-{
-	return comedi_driver_register(&driver_das16cs);
+	return 0;
 }
+module_init(das16cs_init);
 
-static void __exit driver_das16cs_cleanup_module(void)
+static void __exit das16cs_exit(void)
 {
+	pcmcia_unregister_driver(&das16cs_driver);
 	comedi_driver_unregister(&driver_das16cs);
 }
+module_exit(das16cs_exit);
 
-module_init(driver_das16cs_init_module);
-module_exit(driver_das16cs_cleanup_module);
-#endif /* CONFIG_PCMCIA */
+MODULE_AUTHOR("David A. Schleef <ds@schleef.org>");
+MODULE_DESCRIPTION("Comedi driver for Computer Boards PC-CARD DAS16/16");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index ee9e084bb96..2b6a637c349 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -28,7 +28,8 @@
 */
 /*
 Driver: cb_pcidas
-Description: MeasurementComputing PCI-DAS series with the AMCC S5933 PCI controller
+Description: MeasurementComputing PCI-DAS series
+  with the AMCC S5933 PCI controller
 Author: Ivan Martinez <imr@oersted.dtu.dk>,
   Frank Mori Hess <fmhess@users.sourceforge.net>
 Updated: 2003-3-11
@@ -57,8 +58,8 @@ range and aref.
 AI Triggering:
    For start_src == TRIG_EXT, the A/D EXTERNAL TRIGGER IN (pin 45) is used.
    For 1602 series, the start_arg is interpreted as follows:
-     start_arg == 0                   => gated triger (level high)
-     start_arg == CR_INVERT           => gated triger (level low)
+     start_arg == 0                   => gated trigger (level high)
+     start_arg == CR_INVERT           => gated trigger (level low)
      start_arg == CR_EDGE             => Rising edge
      start_arg == CR_EDGE | CR_INVERT => Falling edge
    For the other boards the trigger will be done on rising edge
@@ -77,92 +78,74 @@ analog triggering on 1602 series
 #include "8253.h"
 #include "8255.h"
 #include "amcc_s5933.h"
-#include "comedi_pci.h"
 #include "comedi_fc.h"
 
-#undef CB_PCIDAS_DEBUG		/*  disable debugging code */
-/* #define CB_PCIDAS_DEBUG         enable debugging code */
-
 /* PCI vendor number of ComputerBoards/MeasurementComputing */
 #define PCI_VENDOR_ID_CB	0x1307
-#define TIMER_BASE 100		/*  10MHz master clock */
-#define AI_BUFFER_SIZE 1024	/*  maximum fifo size of any supported board */
-#define AO_BUFFER_SIZE 1024	/*  maximum fifo size of any supported board */
-#define NUM_CHANNELS_8800 8
-#define NUM_CHANNELS_7376 1
-#define NUM_CHANNELS_8402 2
-#define NUM_CHANNELS_DAC08 1
-
-/* PCI-DAS base addresses */
-
-/* indices of base address regions */
-#define S5933_BADRINDEX 0
-#define CONT_STAT_BADRINDEX 1
-#define ADC_FIFO_BADRINDEX 2
-#define PACER_BADRINDEX 3
-#define AO_BADRINDEX 4
-/* sizes of io regions */
-#define CONT_STAT_SIZE 10
-#define ADC_FIFO_SIZE 4
-#define PACER_SIZE 12
-#define AO_SIZE 4
+
+#define TIMER_BASE		100	/* 10MHz master clock */
+#define AI_BUFFER_SIZE		1024	/* max ai fifo size */
+#define AO_BUFFER_SIZE		1024	/* max ao fifo size */
+#define NUM_CHANNELS_8800	8
+#define NUM_CHANNELS_7376	1
+#define NUM_CHANNELS_8402	2
+#define NUM_CHANNELS_DAC08	1
 
 /* Control/Status registers */
-#define INT_ADCFIFO	0	/*  INTERRUPT / ADC FIFO register */
-#define   INT_EOS 0x1		/*  interrupt end of scan */
-#define   INT_FHF 0x2		/*  interrupt fifo half full */
-#define   INT_FNE 0x3		/*  interrupt fifo not empty */
-#define   INT_MASK 0x3		/*  mask of interrupt select bits */
-#define   INTE 0x4		/*  interrupt enable */
-#define   DAHFIE 0x8		/*  dac half full interrupt enable */
-#define   EOAIE	0x10		/*  end of acquisition interrupt enable */
-#define   DAHFI	0x20		/*  dac half full read status / write interrupt clear */
-#define   EOAI 0x40		/*  read end of acq. interrupt status / write clear */
-#define   INT 0x80		/*  read interrupt status / write clear */
-#define   EOBI 0x200		/*  read end of burst interrupt status */
-#define   ADHFI 0x400		/*  read half-full interrupt status */
-#define   ADNEI 0x800		/*  read fifo not empty interrupt latch status */
-#define   ADNE 0x1000		/*  read, fifo not empty (realtime, not latched) status */
-#define   DAEMIE	0x1000	/*  write, dac empty interrupt enable */
-#define   LADFUL 0x2000		/*  read fifo overflow / write clear */
-#define   DAEMI 0x4000		/*  dac fifo empty interrupt status / write clear */
-
-#define ADCMUX_CONT	2	/*  ADC CHANNEL MUX AND CONTROL register */
-#define   BEGIN_SCAN(x)	((x) & 0xf)
-#define   END_SCAN(x)	(((x) & 0xf) << 4)
-#define   GAIN_BITS(x)	(((x) & 0x3) << 8)
-#define   UNIP	0x800		/*  Analog front-end unipolar for range */
-#define   SE	0x400		/*  Inputs in single-ended mode */
-#define   PACER_MASK	0x3000	/*  pacer source bits */
-#define   PACER_INT 0x1000	/*  internal pacer */
-#define   PACER_EXT_FALL	0x2000	/*  external falling edge */
-#define   PACER_EXT_RISE	0x3000	/*  external rising edge */
-#define   EOC	0x4000		/*  adc not busy */
-
-#define TRIG_CONTSTAT 4		/*  TRIGGER CONTROL/STATUS register */
-#define   SW_TRIGGER 0x1	/*  software start trigger */
-#define   EXT_TRIGGER 0x2	/*  external start trigger */
-#define   ANALOG_TRIGGER 0x3	/*  external analog trigger */
-#define   TRIGGER_MASK	0x3	/*  mask of bits that determine start trigger */
-#define   TGPOL	0x04		/*  invert the edge/level of the external trigger (1602 only) */
-#define   TGSEL	0x08		/*  if set edge triggered, otherwise level trigerred (1602 only) */
-#define   TGEN	0x10		/*  enable external start trigger */
-#define   BURSTE 0x20		/*  burst mode enable */
-#define   XTRCL	0x80		/*  clear external trigger */
-
-#define CALIBRATION_REG	6	/*  CALIBRATION register */
-#define   SELECT_8800_BIT	0x100	/*  select 8800 caldac */
-#define   SELECT_TRIMPOT_BIT	0x200	/*  select ad7376 trim pot */
-#define   SELECT_DAC08_BIT	0x400	/*  select dac08 caldac */
+#define INT_ADCFIFO		0	/* INTERRUPT / ADC FIFO register */
+#define   INT_EOS		0x1	/* int end of scan */
+#define   INT_FHF		0x2	/* int fifo half full */
+#define   INT_FNE		0x3	/* int fifo not empty */
+#define   INT_MASK		0x3	/* mask of int select bits */
+#define   INTE			0x4	/* int enable */
+#define   DAHFIE		0x8	/* dac half full int enable */
+#define   EOAIE			0x10	/* end of acq. int enable */
+#define   DAHFI			0x20	/* dac half full status / clear */
+#define   EOAI			0x40	/* end of acq. int status / clear */
+#define   INT			0x80	/* int status / clear */
+#define   EOBI			0x200	/* end of burst int status */
+#define   ADHFI			0x400	/* half-full int status */
+#define   ADNEI			0x800	/* fifo not empty int status (latch) */
+#define   ADNE			0x1000	/* fifo not empty status (realtime) */
+#define   DAEMIE		0x1000	/* dac empty int enable */
+#define   LADFUL		0x2000	/* fifo overflow / clear */
+#define   DAEMI			0x4000	/* dac fifo empty int status / clear */
+
+#define ADCMUX_CONT		2	/* ADC CHANNEL MUX AND CONTROL reg */
+#define   BEGIN_SCAN(x)		((x) & 0xf)
+#define   END_SCAN(x)		(((x) & 0xf) << 4)
+#define   GAIN_BITS(x)		(((x) & 0x3) << 8)
+#define   UNIP			0x800	/* Analog front-end unipolar mode */
+#define   SE			0x400	/* Inputs in single-ended mode */
+#define   PACER_MASK		0x3000	/* pacer source bits */
+#define   PACER_INT		0x1000	/* int. pacer */
+#define   PACER_EXT_FALL	0x2000	/* ext. falling edge */
+#define   PACER_EXT_RISE	0x3000	/* ext. rising edge */
+#define   EOC			0x4000	/* adc not busy */
+
+#define TRIG_CONTSTAT		 4	/* TRIGGER CONTROL/STATUS register */
+#define   SW_TRIGGER		0x1	/* software start trigger */
+#define   EXT_TRIGGER		0x2	/* ext. start trigger */
+#define   ANALOG_TRIGGER	0x3	/* ext. analog trigger */
+#define   TRIGGER_MASK		0x3	/* start trigger mask */
+#define   TGPOL			0x04	/* invert trigger (1602 only) */
+#define   TGSEL			0x08	/* edge/level trigerred (1602 only) */
+#define   TGEN			0x10	/* enable external start trigger */
+#define   BURSTE		0x20	/* burst mode enable */
+#define   XTRCL			0x80	/* clear external trigger */
+
+#define CALIBRATION_REG		6	/* CALIBRATION register */
+#define   SELECT_8800_BIT	0x100	/* select 8800 caldac */
+#define   SELECT_TRIMPOT_BIT	0x200	/* select ad7376 trim pot */
+#define   SELECT_DAC08_BIT	0x400	/* select dac08 caldac */
 #define   CAL_SRC_BITS(x)	(((x) & 0x7) << 11)
-#define   CAL_EN_BIT	0x4000	/*  read calibration source instead of analog input channel 0 */
-#define   SERIAL_DATA_IN_BIT	0x8000	/*  serial data stream going to 8800 and 7376 */
+#define   CAL_EN_BIT		0x4000	/* calibration source enable */
+#define   SERIAL_DATA_IN_BIT	0x8000	/* serial data bit going to caldac */
+
+#define DAC_CSR			0x8	/* dac control and status register */
+#define   DACEN			0x02	/* dac enable */
+#define   DAC_MODE_UPDATE_BOTH	0x80	/* update both dacs */
 
-#define DAC_CSR	0x8		/*  dac control and status register */
-enum dac_csr_bits {
-	DACEN = 0x2,		/*  dac enable */
-	DAC_MODE_UPDATE_BOTH = 0x80,	/*  update both dacs when dac0 is written */
-};
 static inline unsigned int DAC_RANGE(unsigned int channel, unsigned int range)
 {
 	return (range & 0x3) << (8 + 2 * (channel & 0x1));
@@ -174,27 +157,26 @@ static inline unsigned int DAC_RANGE_MASK(unsigned int channel)
 };
 
 /* bits for 1602 series only */
-enum dac_csr_bits_1602 {
-	DAC_EMPTY = 0x1,	/*  dac fifo empty, read, write clear */
-	DAC_START = 0x4,	/*  start/arm dac fifo operations */
-	DAC_PACER_MASK = 0x18,	/*  bits that set dac pacer source */
-	DAC_PACER_INT = 0x8,	/*  dac internal pacing */
-	DAC_PACER_EXT_FALL = 0x10,	/*  dac external pacing, falling edge */
-	DAC_PACER_EXT_RISE = 0x18,	/*  dac external pacing, rising edge */
-};
+#define   DAC_EMPTY		0x1	/* fifo empty, read, write clear */
+#define   DAC_START		0x4	/* start/arm fifo operations */
+#define   DAC_PACER_MASK	0x18	/* bits that set pacer source */
+#define   DAC_PACER_INT		0x8	/* int. pacing */
+#define   DAC_PACER_EXT_FALL	0x10	/* ext. pacing, falling edge */
+#define   DAC_PACER_EXT_RISE	0x18	/* ext. pacing, rising edge */
+
 static inline unsigned int DAC_CHAN_EN(unsigned int channel)
 {
 	return 1 << (5 + (channel & 0x1));	/*  enable channel 0 or 1 */
 };
 
 /* analog input fifo */
-#define ADCDATA	0		/*  ADC DATA register */
-#define ADCFIFOCLR	2	/*  ADC FIFO CLEAR */
+#define ADCDATA			0	/* ADC DATA register */
+#define ADCFIFOCLR		2	/* ADC FIFO CLEAR */
 
 /* pacer, counter, dio registers */
-#define ADC8254 0
-#define DIO_8255 4
-#define DAC8254 8
+#define ADC8254			0
+#define DIO_8255		4
+#define DAC8254			8
 
 /* analog output registers for 100x, 1200 series */
 static inline unsigned int DAC_DATA_REG(unsigned int channel)
@@ -203,11 +185,11 @@ static inline unsigned int DAC_DATA_REG(unsigned int channel)
 }
 
 /* analog output registers for 1602 series*/
-#define DACDATA	0		/*  DAC DATA register */
-#define DACFIFOCLR	2	/*  DAC FIFO CLEAR */
+#define DACDATA			0	/* DAC DATA register */
+#define DACFIFOCLR		2	/* DAC FIFO CLEAR */
+
+#define IS_UNIPOLAR		0x4	/* unipolar range mask */
 
-/* bit in hexadecimal representation of range index that indicates unipolar input range */
-#define IS_UNIPOLAR 0x4
 /* analog input ranges for most boards */
 static const struct comedi_lrange cb_pcidas_ranges = {
 	8,
@@ -257,523 +239,175 @@ enum trimpot_model {
 struct cb_pcidas_board {
 	const char *name;
 	unsigned short device_id;
-	int ai_se_chans;	/*  Inputs in single-ended mode */
-	int ai_diff_chans;	/*  Inputs in differential mode */
+	int ai_nchan;		/*  Inputs in single-ended mode */
 	int ai_bits;		/*  analog input resolution */
 	int ai_speed;		/*  fastest conversion period in ns */
 	int ao_nchan;		/*  number of analog out channels */
 	int has_ao_fifo;	/*  analog output has fifo */
-	int ao_scan_speed;	/*  analog output speed for 1602 series (for a scan, not conversion) */
+	int ao_scan_speed;	/*  analog output scan speed for 1602 series */
 	int fifo_size;		/*  number of samples fifo can hold */
 	const struct comedi_lrange *ranges;
 	enum trimpot_model trimpot;
 	unsigned has_dac08:1;
-	unsigned has_ai_trig_gated:1;	/* Tells if the AI trigger can be gated */
-	unsigned has_ai_trig_invert:1;	/* Tells if the AI trigger can be inverted */
+	unsigned is_1602:1;
 };
 
 static const struct cb_pcidas_board cb_pcidas_boards[] = {
 	{
-	 .name = "pci-das1602/16",
-	 .device_id = 0x1,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 2,
-	 .has_ao_fifo = 1,
-	 .ao_scan_speed = 10000,
-	 .fifo_size = 512,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD8402,
-	 .has_dac08 = 1,
-	 .has_ai_trig_gated = 1,
-	 .has_ai_trig_invert = 1,
-	 },
-	{
-	 .name = "pci-das1200",
-	 .device_id = 0xF,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .ai_speed = 3200,
-	 .ao_nchan = 2,
-	 .has_ao_fifo = 0,
-	 .fifo_size = 1024,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD7376,
-	 .has_dac08 = 0,
-	 .has_ai_trig_gated = 0,
-	 .has_ai_trig_invert = 0,
-	 },
-	{
-	 .name = "pci-das1602/12",
-	 .device_id = 0x10,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .ai_speed = 3200,
-	 .ao_nchan = 2,
-	 .has_ao_fifo = 1,
-	 .ao_scan_speed = 4000,
-	 .fifo_size = 1024,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD7376,
-	 .has_dac08 = 0,
-	 .has_ai_trig_gated = 1,
-	 .has_ai_trig_invert = 1,
-	 },
-	{
-	 .name = "pci-das1200/jr",
-	 .device_id = 0x19,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .ai_speed = 3200,
-	 .ao_nchan = 0,
-	 .has_ao_fifo = 0,
-	 .fifo_size = 1024,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD7376,
-	 .has_dac08 = 0,
-	 .has_ai_trig_gated = 0,
-	 .has_ai_trig_invert = 0,
-	 },
-	{
-	 .name = "pci-das1602/16/jr",
-	 .device_id = 0x1C,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 16,
-	 .ai_speed = 5000,
-	 .ao_nchan = 0,
-	 .has_ao_fifo = 0,
-	 .fifo_size = 512,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD8402,
-	 .has_dac08 = 1,
-	 .has_ai_trig_gated = 1,
-	 .has_ai_trig_invert = 1,
-	 },
-	{
-	 .name = "pci-das1000",
-	 .device_id = 0x4C,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .ai_speed = 4000,
-	 .ao_nchan = 0,
-	 .has_ao_fifo = 0,
-	 .fifo_size = 1024,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD7376,
-	 .has_dac08 = 0,
-	 .has_ai_trig_gated = 0,
-	 .has_ai_trig_invert = 0,
-	 },
-	{
-	 .name = "pci-das1001",
-	 .device_id = 0x1a,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .ai_speed = 6800,
-	 .ao_nchan = 2,
-	 .has_ao_fifo = 0,
-	 .fifo_size = 1024,
-	 .ranges = &cb_pcidas_alt_ranges,
-	 .trimpot = AD7376,
-	 .has_dac08 = 0,
-	 .has_ai_trig_gated = 0,
-	 .has_ai_trig_invert = 0,
-	 },
-	{
-	 .name = "pci-das1002",
-	 .device_id = 0x1b,
-	 .ai_se_chans = 16,
-	 .ai_diff_chans = 8,
-	 .ai_bits = 12,
-	 .ai_speed = 6800,
-	 .ao_nchan = 2,
-	 .has_ao_fifo = 0,
-	 .fifo_size = 1024,
-	 .ranges = &cb_pcidas_ranges,
-	 .trimpot = AD7376,
-	 .has_dac08 = 0,
-	 .has_ai_trig_gated = 0,
-	 .has_ai_trig_invert = 0,
-	 },
+		.name		= "pci-das1602/16",
+		.device_id	= 0x1,
+		.ai_nchan	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.ao_nchan	= 2,
+		.has_ao_fifo	= 1,
+		.ao_scan_speed	= 10000,
+		.fifo_size	= 512,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD8402,
+		.has_dac08	= 1,
+		.is_1602	= 1,
+	}, {
+		.name		= "pci-das1200",
+		.device_id	= 0xF,
+		.ai_nchan	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 3200,
+		.ao_nchan	= 2,
+		.fifo_size	= 1024,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD7376,
+	}, {
+		.name		= "pci-das1602/12",
+		.device_id	= 0x10,
+		.ai_nchan	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 3200,
+		.ao_nchan	= 2,
+		.has_ao_fifo	= 1,
+		.ao_scan_speed	= 4000,
+		.fifo_size	= 1024,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD7376,
+		.is_1602	= 1,
+	}, {
+		.name		= "pci-das1200/jr",
+		.device_id	= 0x19,
+		.ai_nchan	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 3200,
+		.fifo_size	= 1024,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD7376,
+	}, {
+		.name		= "pci-das1602/16/jr",
+		.device_id	= 0x1C,
+		.ai_nchan	= 16,
+		.ai_bits	= 16,
+		.ai_speed	= 5000,
+		.fifo_size	= 512,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD8402,
+		.has_dac08	= 1,
+		.is_1602	= 1,
+	}, {
+		.name		= "pci-das1000",
+		.device_id	= 0x4C,
+		.ai_nchan	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 4000,
+		.fifo_size	= 1024,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD7376,
+	}, {
+		.name		= "pci-das1001",
+		.device_id	= 0x1a,
+		.ai_nchan	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 6800,
+		.ao_nchan	= 2,
+		.fifo_size	= 1024,
+		.ranges		= &cb_pcidas_alt_ranges,
+		.trimpot	= AD7376,
+	}, {
+		.name		= "pci-das1002",
+		.device_id	= 0x1b,
+		.ai_nchan	= 16,
+		.ai_bits	= 12,
+		.ai_speed	= 6800,
+		.ao_nchan	= 2,
+		.fifo_size	= 1024,
+		.ranges		= &cb_pcidas_ranges,
+		.trimpot	= AD7376,
+	},
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct cb_pcidas_board *)dev->board_ptr)
-
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.  */
 struct cb_pcidas_private {
-	/* would be useful for a PCI device */
-	struct pci_dev *pci_dev;
-	/*  base addresses */
+	/* base addresses */
 	unsigned long s5933_config;
 	unsigned long control_status;
 	unsigned long adc_fifo;
 	unsigned long pacer_counter_dio;
 	unsigned long ao_registers;
-	/*  divisors of master clock for analog input pacing */
+	/* divisors of master clock for analog input pacing */
 	unsigned int divisor1;
 	unsigned int divisor2;
-	volatile unsigned int count;	/*  number of analog input samples remaining */
-	volatile unsigned int adc_fifo_bits;	/*  bits to write to interrupt/adcfifo register */
-	volatile unsigned int s5933_intcsr_bits;	/*  bits to write to amcc s5933 interrupt control/status register */
-	volatile unsigned int ao_control_bits;	/*  bits to write to ao control and status register */
+	/* number of analog input samples remaining */
+	unsigned int count;
+	/* bits to write to registers */
+	unsigned int adc_fifo_bits;
+	unsigned int s5933_intcsr_bits;
+	unsigned int ao_control_bits;
+	/* fifo buffers */
 	short ai_buffer[AI_BUFFER_SIZE];
 	short ao_buffer[AO_BUFFER_SIZE];
-	/*  divisors of master clock for analog output pacing */
+	/* divisors of master clock for analog output pacing */
 	unsigned int ao_divisor1;
 	unsigned int ao_divisor2;
-	volatile unsigned int ao_count;	/*  number of analog output samples remaining */
-	int ao_value[2];	/*  remember what the analog outputs are set to, to allow readback */
-	unsigned int caldac_value[NUM_CHANNELS_8800];	/*  for readback of caldac */
-	unsigned int trimpot_value[NUM_CHANNELS_8402];	/*  for readback of trimpot */
+	/* number of analog output samples remaining */
+	unsigned int ao_count;
+	/* cached values for readback */
+	int ao_value[2];
+	unsigned int caldac_value[NUM_CHANNELS_8800];
+	unsigned int trimpot_value[NUM_CHANNELS_8402];
 	unsigned int dac08_value;
 	unsigned int calibration_source;
 };
 
-/*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct cb_pcidas_private *)dev->private)
-
-static int cb_pcidas_ai_rinsn(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-static int ai_config_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int cb_pcidas_ao_nofifo_winsn(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data);
-static int cb_pcidas_ao_fifo_winsn(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-static int cb_pcidas_ao_readback_insn(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data);
-static int cb_pcidas_ai_cmd(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_cmd *cmd);
-static int cb_pcidas_ao_cmd(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
-				struct comedi_subdevice *subdev,
-				unsigned int trig_num);
-static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_cmd *cmd);
-static irqreturn_t cb_pcidas_interrupt(int irq, void *d);
-static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status);
-static int cb_pcidas_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s);
-static int cb_pcidas_ao_cancel(struct comedi_device *dev,
-			       struct comedi_subdevice *s);
-static void cb_pcidas_load_counters(struct comedi_device *dev, unsigned int *ns,
-				    int round_flags);
-static int eeprom_read_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int caldac_read_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int caldac_write_insn(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data);
-static int trimpot_read_insn(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data);
-static int cb_pcidas_trimpot_write(struct comedi_device *dev,
-				   unsigned int channel, unsigned int value);
-static int trimpot_write_insn(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-static int dac08_read_insn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data);
-static int dac08_write(struct comedi_device *dev, unsigned int value);
-static int dac08_write_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
-			     uint8_t value);
-static int trimpot_7376_write(struct comedi_device *dev, uint8_t value);
-static int trimpot_8402_write(struct comedi_device *dev, unsigned int channel,
-			      uint8_t value);
-static int nvram_read(struct comedi_device *dev, unsigned int address,
-		      uint8_t *data);
-
 static inline unsigned int cal_enable_bits(struct comedi_device *dev)
 {
-	return CAL_EN_BIT | CAL_SRC_BITS(devpriv->calibration_source);
-}
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.
- */
-static int cb_pcidas_attach(struct comedi_device *dev,
-			    struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	struct pci_dev *pcidev = NULL;
-	int index;
-	int i;
-
-/*
- * Allocate the private structure area.
- */
-	if (alloc_private(dev, sizeof(struct cb_pcidas_private)) < 0)
-		return -ENOMEM;
+	struct cb_pcidas_private *devpriv = dev->private;
 
-/*
- * Probe the device to determine what device in the series it is.
- */
-
-	for_each_pci_dev(pcidev) {
-		/*  is it not a computer boards card? */
-		if (pcidev->vendor != PCI_VENDOR_ID_CB)
-			continue;
-		/*  loop through cards supported by this driver */
-		for (index = 0; index < ARRAY_SIZE(cb_pcidas_boards); index++) {
-			if (cb_pcidas_boards[index].device_id != pcidev->device)
-				continue;
-			/*  was a particular bus/slot requested? */
-			if (it->options[0] || it->options[1]) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			devpriv->pci_dev = pcidev;
-			dev->board_ptr = cb_pcidas_boards + index;
-			goto found;
-		}
-	}
-
-	dev_err(dev->hw_dev, "No supported ComputerBoards/MeasurementComputing card found on requested position\n");
-	return -EIO;
-
-found:
-
-	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n",
-		cb_pcidas_boards[index].name, pcidev->bus->number,
-		PCI_SLOT(pcidev->devfn));
-
-	/*
-	 * Enable PCI device and reserve I/O ports.
-	 */
-	if (comedi_pci_enable(pcidev, "cb_pcidas")) {
-		dev_err(dev->hw_dev, "Failed to enable PCI device and request regions\n");
-		return -EIO;
-	}
-	/*
-	 * Initialize devpriv->control_status and devpriv->adc_fifo to point to
-	 * their base address.
-	 */
-	devpriv->s5933_config =
-	    pci_resource_start(devpriv->pci_dev, S5933_BADRINDEX);
-	devpriv->control_status =
-	    pci_resource_start(devpriv->pci_dev, CONT_STAT_BADRINDEX);
-	devpriv->adc_fifo =
-	    pci_resource_start(devpriv->pci_dev, ADC_FIFO_BADRINDEX);
-	devpriv->pacer_counter_dio =
-	    pci_resource_start(devpriv->pci_dev, PACER_BADRINDEX);
-	if (thisboard->ao_nchan) {
-		devpriv->ao_registers =
-		    pci_resource_start(devpriv->pci_dev, AO_BADRINDEX);
-	}
-	/*  disable and clear interrupts on amcc s5933 */
-	outl(INTCSR_INBOX_INTR_STATUS,
-	     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
-
-	/*  get irq */
-	if (request_irq(devpriv->pci_dev->irq, cb_pcidas_interrupt,
-			IRQF_SHARED, "cb_pcidas", dev)) {
-		dev_dbg(dev->hw_dev, "unable to allocate irq %d\n",
-			devpriv->pci_dev->irq);
-		return -EINVAL;
-	}
-	dev->irq = devpriv->pci_dev->irq;
-
-	/* Initialize dev->board_name */
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the subdevice structures.
- */
-	if (alloc_subdevices(dev, 7) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* analog input subdevice */
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
-	/* WARNING: Number of inputs in differential mode is ignored */
-	s->n_chan = thisboard->ai_se_chans;
-	s->len_chanlist = thisboard->ai_se_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = thisboard->ranges;
-	s->insn_read = cb_pcidas_ai_rinsn;
-	s->insn_config = ai_config_insn;
-	s->do_cmd = cb_pcidas_ai_cmd;
-	s->do_cmdtest = cb_pcidas_ai_cmdtest;
-	s->cancel = cb_pcidas_cancel;
-
-	/* analog output subdevice */
-	s = dev->subdevices + 1;
-	if (thisboard->ao_nchan) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = thisboard->ao_nchan;
-		/*  analog out resolution is the same as analog input resolution, so use ai_bits */
-		s->maxdata = (1 << thisboard->ai_bits) - 1;
-		s->range_table = &cb_pcidas_ao_ranges;
-		s->insn_read = cb_pcidas_ao_readback_insn;
-		if (thisboard->has_ao_fifo) {
-			dev->write_subdev = s;
-			s->subdev_flags |= SDF_CMD_WRITE;
-			s->insn_write = cb_pcidas_ao_fifo_winsn;
-			s->do_cmdtest = cb_pcidas_ao_cmdtest;
-			s->do_cmd = cb_pcidas_ao_cmd;
-			s->cancel = cb_pcidas_ao_cancel;
-		} else {
-			s->insn_write = cb_pcidas_ao_nofifo_winsn;
-		}
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	/* 8255 */
-	s = dev->subdevices + 2;
-	subdev_8255_init(dev, s, NULL, devpriv->pacer_counter_dio + DIO_8255);
-
-	/*  serial EEPROM, */
-	s = dev->subdevices + 3;
-	s->type = COMEDI_SUBD_MEMORY;
-	s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
-	s->n_chan = 256;
-	s->maxdata = 0xff;
-	s->insn_read = eeprom_read_insn;
-
-	/*  8800 caldac */
-	s = dev->subdevices + 4;
-	s->type = COMEDI_SUBD_CALIB;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-	s->n_chan = NUM_CHANNELS_8800;
-	s->maxdata = 0xff;
-	s->insn_read = caldac_read_insn;
-	s->insn_write = caldac_write_insn;
-	for (i = 0; i < s->n_chan; i++)
-		caldac_8800_write(dev, i, s->maxdata / 2);
-
-	/*  trim potentiometer */
-	s = dev->subdevices + 5;
-	s->type = COMEDI_SUBD_CALIB;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-	if (thisboard->trimpot == AD7376) {
-		s->n_chan = NUM_CHANNELS_7376;
-		s->maxdata = 0x7f;
-	} else {
-		s->n_chan = NUM_CHANNELS_8402;
-		s->maxdata = 0xff;
-	}
-	s->insn_read = trimpot_read_insn;
-	s->insn_write = trimpot_write_insn;
-	for (i = 0; i < s->n_chan; i++)
-		cb_pcidas_trimpot_write(dev, i, s->maxdata / 2);
-
-	/*  dac08 caldac */
-	s = dev->subdevices + 6;
-	if (thisboard->has_dac08) {
-		s->type = COMEDI_SUBD_CALIB;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-		s->n_chan = NUM_CHANNELS_DAC08;
-		s->insn_read = dac08_read_insn;
-		s->insn_write = dac08_write_insn;
-		s->maxdata = 0xff;
-		dac08_write(dev, s->maxdata / 2);
-	} else
-		s->type = COMEDI_SUBD_UNUSED;
-
-	/*  make sure mailbox 4 is empty */
-	inl(devpriv->s5933_config + AMCC_OP_REG_IMB4);
-	/* Set bits to enable incoming mailbox interrupts on amcc s5933. */
-	devpriv->s5933_intcsr_bits =
-	    INTCSR_INBOX_BYTE(3) | INTCSR_INBOX_SELECT(3) |
-	    INTCSR_INBOX_FULL_INT;
-	/*  clear and enable interrupt on amcc s5933 */
-	outl(devpriv->s5933_intcsr_bits | INTCSR_INBOX_INTR_STATUS,
-	     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
-
-	return 1;
-}
-
-static void cb_pcidas_detach(struct comedi_device *dev)
-{
-	if (devpriv) {
-		if (devpriv->s5933_config) {
-			outl(INTCSR_INBOX_INTR_STATUS,
-			     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
-		}
-	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->subdevices)
-		subdev_8255_cleanup(dev, dev->subdevices + 2);
-	if (devpriv && devpriv->pci_dev) {
-		if (devpriv->s5933_config)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
-	}
+	return CAL_EN_BIT | CAL_SRC_BITS(devpriv->calibration_source);
 }
 
-/*
- * "instructions" read/write data in "one-shot" or "software-triggered"
- * mode.
- */
 static int cb_pcidas_ai_rinsn(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	int n, i;
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int aref = CR_AREF(insn->chanspec);
 	unsigned int bits;
-	static const int timeout = 10000;
-	int channel;
-	/*  enable calibration input if appropriate */
+	int n, i;
+
+	/* enable calibration input if appropriate */
 	if (insn->chanspec & CR_ALT_SOURCE) {
 		outw(cal_enable_bits(dev),
 		     devpriv->control_status + CALIBRATION_REG);
-		channel = 0;
+		chan = 0;
 	} else {
 		outw(0, devpriv->control_status + CALIBRATION_REG);
-		channel = CR_CHAN(insn->chanspec);
 	}
-	/*  set mux limits and gain */
-	bits = BEGIN_SCAN(channel) |
-	    END_SCAN(channel) | GAIN_BITS(CR_RANGE(insn->chanspec));
-	/*  set unipolar/bipolar */
-	if (CR_RANGE(insn->chanspec) & IS_UNIPOLAR)
+
+	/* set mux limits and gain */
+	bits = BEGIN_SCAN(chan) | END_SCAN(chan) | GAIN_BITS(range);
+	/* set unipolar/bipolar */
+	if (range & IS_UNIPOLAR)
 		bits |= UNIP;
-	/*  set singleended/differential */
-	if (CR_AREF(insn->chanspec) != AREF_DIFF)
+	/* set single-ended/differential */
+	if (aref != AREF_DIFF)
 		bits |= SE;
 	outw(bits, devpriv->control_status + ADCMUX_CONT);
 
@@ -787,11 +421,11 @@ static int cb_pcidas_ai_rinsn(struct comedi_device *dev,
 
 		/* wait for conversion to end */
 		/* return -ETIMEDOUT if there is a timeout */
-		for (i = 0; i < timeout; i++) {
+		for (i = 0; i < 10000; i++) {
 			if (inw(devpriv->control_status + ADCMUX_CONT) & EOC)
 				break;
 		}
-		if (i == timeout)
+		if (i == 10000)
 			return -ETIMEDOUT;
 
 		/* read data */
@@ -802,37 +436,28 @@ static int cb_pcidas_ai_rinsn(struct comedi_device *dev,
 	return n;
 }
 
-static int ai_config_calibration_source(struct comedi_device *dev,
-					unsigned int *data)
-{
-	static const int num_calibration_sources = 8;
-	unsigned int source = data[1];
-
-	if (source >= num_calibration_sources) {
-		dev_err(dev->hw_dev, "invalid calibration source: %i\n",
-			source);
-		return -EINVAL;
-	}
-
-	devpriv->calibration_source = source;
-
-	return 2;
-}
-
 static int ai_config_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	struct cb_pcidas_private *devpriv = dev->private;
 	int id = data[0];
+	unsigned int source = data[1];
 
 	switch (id) {
 	case INSN_CONFIG_ALT_SOURCE:
-		return ai_config_calibration_source(dev, data);
+		if (source >= 8) {
+			dev_err(dev->class_dev,
+				"invalid calibration source: %i\n",
+				source);
+			return -EINVAL;
+		}
+		devpriv->calibration_source = source;
 		break;
 	default:
 		return -EINVAL;
 		break;
 	}
-	return -EINVAL;
+	return insn->n;
 }
 
 /* analog output insn for pcidas-1000 and 1200 series */
@@ -841,25 +466,26 @@ static int cb_pcidas_ao_nofifo_winsn(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
-	int channel;
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
 	unsigned long flags;
 
-	/*  set channel and range */
-	channel = CR_CHAN(insn->chanspec);
+	/* set channel and range */
 	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->ao_control_bits &=
-	    ~DAC_MODE_UPDATE_BOTH & ~DAC_RANGE_MASK(channel);
-	devpriv->ao_control_bits |=
-	    DACEN | DAC_RANGE(channel, CR_RANGE(insn->chanspec));
+	devpriv->ao_control_bits &= (~DAC_MODE_UPDATE_BOTH &
+				     ~DAC_RANGE_MASK(chan));
+	devpriv->ao_control_bits |= (DACEN | DAC_RANGE(chan, range));
 	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	/*  remember value for readback */
-	devpriv->ao_value[channel] = data[0];
-	/*  send data */
-	outw(data[0], devpriv->ao_registers + DAC_DATA_REG(channel));
+	/* remember value for readback */
+	devpriv->ao_value[chan] = data[0];
 
-	return 1;
+	/* send data */
+	outw(data[0], devpriv->ao_registers + DAC_DATA_REG(chan));
+
+	return insn->n;
 }
 
 /* analog output insn for pcidas-1602 series */
@@ -867,46 +493,84 @@ static int cb_pcidas_ao_fifo_winsn(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
-	int channel;
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
 	unsigned long flags;
 
-	/*  clear dac fifo */
+	/* clear dac fifo */
 	outw(0, devpriv->ao_registers + DACFIFOCLR);
 
-	/*  set channel and range */
-	channel = CR_CHAN(insn->chanspec);
+	/* set channel and range */
 	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->ao_control_bits &=
-	    ~DAC_CHAN_EN(0) & ~DAC_CHAN_EN(1) & ~DAC_RANGE_MASK(channel) &
-	    ~DAC_PACER_MASK;
-	devpriv->ao_control_bits |=
-	    DACEN | DAC_RANGE(channel,
-			      CR_RANGE(insn->
-				       chanspec)) | DAC_CHAN_EN(channel) |
-	    DAC_START;
+	devpriv->ao_control_bits &= (~DAC_CHAN_EN(0) & ~DAC_CHAN_EN(1) &
+				     ~DAC_RANGE_MASK(chan) & ~DAC_PACER_MASK);
+	devpriv->ao_control_bits |= (DACEN | DAC_RANGE(chan, range) |
+				     DAC_CHAN_EN(chan) | DAC_START);
 	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	/*  remember value for readback */
-	devpriv->ao_value[channel] = data[0];
-	/*  send data */
+	/* remember value for readback */
+	devpriv->ao_value[chan] = data[0];
+
+	/* send data */
 	outw(data[0], devpriv->ao_registers + DACDATA);
 
-	return 1;
+	return insn->n;
 }
 
-/* analog output readback insn */
-/* XXX loses track of analog output value back after an analog ouput command is executed */
 static int cb_pcidas_ao_readback_insn(struct comedi_device *dev,
 				      struct comedi_subdevice *s,
 				      struct comedi_insn *insn,
 				      unsigned int *data)
 {
+	struct cb_pcidas_private *devpriv = dev->private;
+
 	data[0] = devpriv->ao_value[CR_CHAN(insn->chanspec)];
 
 	return 1;
 }
 
+static int wait_for_nvram_ready(unsigned long s5933_base_addr)
+{
+	static const int timeout = 1000;
+	unsigned int i;
+
+	for (i = 0; i < timeout; i++) {
+		if ((inb(s5933_base_addr +
+			 AMCC_OP_REG_MCSR_NVCMD) & MCSR_NV_BUSY)
+		    == 0)
+			return 0;
+		udelay(1);
+	}
+	return -1;
+}
+
+static int nvram_read(struct comedi_device *dev, unsigned int address,
+			uint8_t *data)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned long iobase = devpriv->s5933_config;
+
+	if (wait_for_nvram_ready(iobase) < 0)
+		return -ETIMEDOUT;
+
+	outb(MCSR_NV_ENABLE | MCSR_NV_LOAD_LOW_ADDR,
+	     iobase + AMCC_OP_REG_MCSR_NVCMD);
+	outb(address & 0xff, iobase + AMCC_OP_REG_MCSR_NVDATA);
+	outb(MCSR_NV_ENABLE | MCSR_NV_LOAD_HIGH_ADDR,
+	     iobase + AMCC_OP_REG_MCSR_NVCMD);
+	outb((address >> 8) & 0xff, iobase + AMCC_OP_REG_MCSR_NVDATA);
+	outb(MCSR_NV_ENABLE | MCSR_NV_READ, iobase + AMCC_OP_REG_MCSR_NVCMD);
+
+	if (wait_for_nvram_ready(iobase) < 0)
+		return -ETIMEDOUT;
+
+	*data = inb(iobase + AMCC_OP_REG_MCSR_NVDATA);
+
+	return 0;
+}
+
 static int eeprom_read_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -923,6 +587,56 @@ static int eeprom_read_insn(struct comedi_device *dev,
 	return 1;
 }
 
+static void write_calibration_bitstream(struct comedi_device *dev,
+					unsigned int register_bits,
+					unsigned int bitstream,
+					unsigned int bitstream_length)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	static const int write_delay = 1;
+	unsigned int bit;
+
+	for (bit = 1 << (bitstream_length - 1); bit; bit >>= 1) {
+		if (bitstream & bit)
+			register_bits |= SERIAL_DATA_IN_BIT;
+		else
+			register_bits &= ~SERIAL_DATA_IN_BIT;
+		udelay(write_delay);
+		outw(register_bits, devpriv->control_status + CALIBRATION_REG);
+	}
+}
+
+static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
+			     uint8_t value)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	static const int num_caldac_channels = 8;
+	static const int bitstream_length = 11;
+	unsigned int bitstream = ((address & 0x7) << 8) | value;
+	static const int caldac_8800_udelay = 1;
+
+	if (address >= num_caldac_channels) {
+		comedi_error(dev, "illegal caldac channel");
+		return -1;
+	}
+
+	if (value == devpriv->caldac_value[address])
+		return 1;
+
+	devpriv->caldac_value[address] = value;
+
+	write_calibration_bitstream(dev, cal_enable_bits(dev), bitstream,
+				    bitstream_length);
+
+	udelay(caldac_8800_udelay);
+	outw(cal_enable_bits(dev) | SELECT_8800_BIT,
+	     devpriv->control_status + CALIBRATION_REG);
+	udelay(caldac_8800_udelay);
+	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
+
+	return 1;
+}
+
 static int caldac_write_insn(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
@@ -936,51 +650,112 @@ static int caldac_read_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct cb_pcidas_private *devpriv = dev->private;
+
 	data[0] = devpriv->caldac_value[CR_CHAN(insn->chanspec)];
 
 	return 1;
 }
 
 /* 1602/16 pregain offset */
-static int dac08_write(struct comedi_device *dev, unsigned int value)
+static void dac08_write(struct comedi_device *dev, unsigned int value)
 {
-	if (devpriv->dac08_value == value)
-		return 1;
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned long cal_reg;
 
-	devpriv->dac08_value = value;
+	if (devpriv->dac08_value != value) {
+		devpriv->dac08_value = value;
 
-	outw(cal_enable_bits(dev) | (value & 0xff),
-	     devpriv->control_status + CALIBRATION_REG);
-	udelay(1);
-	outw(cal_enable_bits(dev) | SELECT_DAC08_BIT | (value & 0xff),
-	     devpriv->control_status + CALIBRATION_REG);
-	udelay(1);
-	outw(cal_enable_bits(dev) | (value & 0xff),
-	     devpriv->control_status + CALIBRATION_REG);
-	udelay(1);
+		cal_reg = devpriv->control_status + CALIBRATION_REG;
 
-	return 1;
+		value &= 0xff;
+		value |= cal_enable_bits(dev);
+
+		/* latch the new value into the caldac */
+		outw(value, cal_reg);
+		udelay(1);
+		outw(value | SELECT_DAC08_BIT, cal_reg);
+		udelay(1);
+		outw(value, cal_reg);
+		udelay(1);
+	}
 }
 
 static int dac08_write_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	return dac08_write(dev, data[0]);
+	int i;
+
+	for (i = 0; i < insn->n; i++)
+		dac08_write(dev, data[i]);
+
+	return insn->n;
 }
 
 static int dac08_read_insn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
 {
+	struct cb_pcidas_private *devpriv = dev->private;
+
 	data[0] = devpriv->dac08_value;
 
 	return 1;
 }
 
+static int trimpot_7376_write(struct comedi_device *dev, uint8_t value)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	static const int bitstream_length = 7;
+	unsigned int bitstream = value & 0x7f;
+	unsigned int register_bits;
+	static const int ad7376_udelay = 1;
+
+	register_bits = cal_enable_bits(dev) | SELECT_TRIMPOT_BIT;
+	udelay(ad7376_udelay);
+	outw(register_bits, devpriv->control_status + CALIBRATION_REG);
+
+	write_calibration_bitstream(dev, register_bits, bitstream,
+				    bitstream_length);
+
+	udelay(ad7376_udelay);
+	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
+
+	return 0;
+}
+
+/* For 1602/16 only
+ * ch 0 : adc gain
+ * ch 1 : adc postgain offset */
+static int trimpot_8402_write(struct comedi_device *dev, unsigned int channel,
+			      uint8_t value)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	static const int bitstream_length = 10;
+	unsigned int bitstream = ((channel & 0x3) << 8) | (value & 0xff);
+	unsigned int register_bits;
+	static const int ad8402_udelay = 1;
+
+	register_bits = cal_enable_bits(dev) | SELECT_TRIMPOT_BIT;
+	udelay(ad8402_udelay);
+	outw(register_bits, devpriv->control_status + CALIBRATION_REG);
+
+	write_calibration_bitstream(dev, register_bits, bitstream,
+				    bitstream_length);
+
+	udelay(ad8402_udelay);
+	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
+
+	return 0;
+}
+
 static int cb_pcidas_trimpot_write(struct comedi_device *dev,
 				   unsigned int channel, unsigned int value)
 {
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
+
 	if (devpriv->trimpot_value[channel] == value)
 		return 1;
 
@@ -1014,6 +789,7 @@ static int trimpot_read_insn(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
+	struct cb_pcidas_private *devpriv = dev->private;
 	unsigned int channel = CR_CHAN(insn->chanspec);
 
 	data[0] = devpriv->trimpot_value[channel];
@@ -1025,18 +801,13 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
 	int tmp;
 	int i, gain, start_chan;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
-
-	/* step 1: make sure trigger sources are trivially valid */
+	/* step 1: trigger sources are trivially valid */
 
 	tmp = cmd->start_src;
 	cmd->start_src &= TRIG_NOW | TRIG_EXT;
@@ -1066,7 +837,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* step 2: trigger sources are unique and mutually compatible */
 
 	if (cmd->start_src != TRIG_NOW && cmd->start_src != TRIG_EXT)
 		err++;
@@ -1092,7 +863,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 2;
 
-	/* step 3: make sure arguments are trivially compatible */
+	/* step 3: arguments are trivially compatible */
 
 	switch (cmd->start_src) {
 	case TRIG_EXT:
@@ -1103,8 +874,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 			    ~(CR_FLAGS_MASK & ~(CR_EDGE | CR_INVERT));
 			err++;
 		}
-		if (!thisboard->has_ai_trig_invert &&
-		    (cmd->start_arg & CR_INVERT)) {
+		if (!thisboard->is_1602 && (cmd->start_arg & CR_INVERT)) {
 			cmd->start_arg &= (CR_FLAGS_MASK & ~CR_INVERT);
 			err++;
 		}
@@ -1198,9 +968,27 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+static void cb_pcidas_load_counters(struct comedi_device *dev, unsigned int *ns,
+				    int rounding_flags)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+
+	i8253_cascade_ns_to_timer_2div(TIMER_BASE, &(devpriv->divisor1),
+				       &(devpriv->divisor2), ns,
+				       rounding_flags & TRIG_ROUND_MASK);
+
+	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
+	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 1,
+		   devpriv->divisor1, 2);
+	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 2,
+		   devpriv->divisor2, 2);
+}
+
 static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int bits;
@@ -1230,10 +1018,6 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 		bits |= PACER_INT;
 	outw(bits, devpriv->control_status + ADCMUX_CONT);
 
-#ifdef CB_PCIDAS_DEBUG
-	dev_dbg(dev->hw_dev, "comedi: sent 0x%x to adcmux control\n", bits);
-#endif
-
 	/*  load counters */
 	if (cmd->convert_src == TRIG_TIMER)
 		cb_pcidas_load_counters(dev, &cmd->convert_arg,
@@ -1250,17 +1034,18 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	devpriv->adc_fifo_bits |= INTE;
 	devpriv->adc_fifo_bits &= ~INT_MASK;
 	if (cmd->flags & TRIG_WAKE_EOS) {
-		if (cmd->convert_src == TRIG_NOW && cmd->chanlist_len > 1)
-			devpriv->adc_fifo_bits |= INT_EOS;	/*  interrupt end of burst */
-		else
-			devpriv->adc_fifo_bits |= INT_FNE;	/*  interrupt fifo not empty */
+		if (cmd->convert_src == TRIG_NOW && cmd->chanlist_len > 1) {
+			/* interrupt end of burst */
+			devpriv->adc_fifo_bits |= INT_EOS;
+		} else {
+			/* interrupt fifo not empty */
+			devpriv->adc_fifo_bits |= INT_FNE;
+		}
 	} else {
-		devpriv->adc_fifo_bits |= INT_FHF;	/* interrupt fifo half full */
+		/* interrupt fifo half full */
+		devpriv->adc_fifo_bits |= INT_FHF;
 	}
-#ifdef CB_PCIDAS_DEBUG
-	dev_dbg(dev->hw_dev, "comedi: adc_fifo_bits are 0x%x\n",
-		devpriv->adc_fifo_bits);
-#endif
+
 	/*  enable (and clear) interrupts */
 	outw(devpriv->adc_fifo_bits | EOAI | INT | LADFUL,
 	     devpriv->control_status + INT_ADCFIFO);
@@ -1272,11 +1057,12 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 		bits |= SW_TRIGGER;
 	else if (cmd->start_src == TRIG_EXT) {
 		bits |= EXT_TRIGGER | TGEN | XTRCL;
-		if (thisboard->has_ai_trig_invert
-		    && (cmd->start_arg & CR_INVERT))
-			bits |= TGPOL;
-		if (thisboard->has_ai_trig_gated && (cmd->start_arg & CR_EDGE))
-			bits |= TGSEL;
+		if (thisboard->is_1602) {
+			if (cmd->start_arg & CR_INVERT)
+				bits |= TGPOL;
+			if (cmd->start_arg & CR_EDGE)
+				bits |= TGSEL;
+		}
 	} else {
 		comedi_error(dev, "bug!");
 		return -1;
@@ -1284,9 +1070,6 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	if (cmd->convert_src == TRIG_NOW && cmd->chanlist_len > 1)
 		bits |= BURSTE;
 	outw(bits, devpriv->control_status + TRIG_CONTSTAT);
-#ifdef CB_PCIDAS_DEBUG
-	dev_dbg(dev->hw_dev, "comedi: sent 0x%x to trig control\n", bits);
-#endif
 
 	return 0;
 }
@@ -1295,17 +1078,12 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
 	int tmp;
 
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
-
-	/* step 1: make sure trigger sources are trivially valid */
+	/* step 1: trigger sources are trivially valid */
 
 	tmp = cmd->start_src;
 	cmd->start_src &= TRIG_INT;
@@ -1335,7 +1113,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 1;
 
-	/* step 2: make sure trigger sources are unique and mutually compatible */
+	/* step 2: trigger sources are unique and mutually compatible */
 
 	if (cmd->scan_begin_src != TRIG_TIMER &&
 	    cmd->scan_begin_src != TRIG_EXT)
@@ -1346,7 +1124,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 2;
 
-	/* step 3: make sure arguments are trivially compatible */
+	/* step 3: arguments are trivially compatible */
 
 	if (cmd->start_arg != 0) {
 		cmd->start_arg = 0;
@@ -1407,9 +1185,77 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+/* cancel analog input command */
+static int cb_pcidas_cancel(struct comedi_device *dev,
+			    struct comedi_subdevice *s)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->spinlock, flags);
+	/*  disable interrupts */
+	devpriv->adc_fifo_bits &= ~INTE & ~EOAIE;
+	outw(devpriv->adc_fifo_bits, devpriv->control_status + INT_ADCFIFO);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	/*  disable start trigger source and burst mode */
+	outw(0, devpriv->control_status + TRIG_CONTSTAT);
+	/*  software pacer source */
+	outw(0, devpriv->control_status + ADCMUX_CONT);
+
+	return 0;
+}
+
+static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				unsigned int trig_num)
+{
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned int num_bytes, num_points = thisboard->fifo_size;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned long flags;
+
+	if (trig_num != 0)
+		return -EINVAL;
+
+	/*  load up fifo */
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_count < num_points)
+		num_points = devpriv->ao_count;
+
+	num_bytes = cfc_read_array_from_buffer(s, devpriv->ao_buffer,
+					       num_points * sizeof(short));
+	num_points = num_bytes / sizeof(short);
+
+	if (cmd->stop_src == TRIG_COUNT)
+		devpriv->ao_count -= num_points;
+	/*  write data to board's fifo */
+	outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer, num_bytes);
+
+	/*  enable dac half-full and empty interrupts */
+	spin_lock_irqsave(&dev->spinlock, flags);
+	devpriv->adc_fifo_bits |= DAEMIE | DAHFIE;
+
+	/*  enable and clear interrupts */
+	outw(devpriv->adc_fifo_bits | DAEMI | DAHFI,
+	     devpriv->control_status + INT_ADCFIFO);
+
+	/*  start dac */
+	devpriv->ao_control_bits |= DAC_START | DACEN | DAC_EMPTY;
+	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
+
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	async->inttrig = NULL;
+
+	return 0;
+}
+
 static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
+	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int i;
@@ -1473,59 +1319,88 @@ static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 	return 0;
 }
 
-static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				unsigned int trig_num)
+/* cancel analog output command */
+static int cb_pcidas_ao_cancel(struct comedi_device *dev,
+			       struct comedi_subdevice *s)
 {
-	unsigned int num_bytes, num_points = thisboard->fifo_size;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	struct cb_pcidas_private *devpriv = dev->private;
 	unsigned long flags;
 
-	if (trig_num != 0)
-		return -EINVAL;
+	spin_lock_irqsave(&dev->spinlock, flags);
+	/*  disable interrupts */
+	devpriv->adc_fifo_bits &= ~DAHFIE & ~DAEMIE;
+	outw(devpriv->adc_fifo_bits, devpriv->control_status + INT_ADCFIFO);
 
-	/*  load up fifo */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_count < num_points)
-		num_points = devpriv->ao_count;
+	/*  disable output */
+	devpriv->ao_control_bits &= ~DACEN & ~DAC_PACER_MASK;
+	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	num_bytes = cfc_read_array_from_buffer(s, devpriv->ao_buffer,
-					       num_points * sizeof(short));
-	num_points = num_bytes / sizeof(short);
+	return 0;
+}
 
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ao_count -= num_points;
-	/*  write data to board's fifo */
-	outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer, num_bytes);
+static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
+{
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->write_subdev;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int half_fifo = thisboard->fifo_size / 2;
+	unsigned int num_points;
+	unsigned long flags;
 
-	/*  enable dac half-full and empty interrupts */
-	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->adc_fifo_bits |= DAEMIE | DAHFIE;
-#ifdef CB_PCIDAS_DEBUG
-	dev_dbg(dev->hw_dev, "comedi: adc_fifo_bits are 0x%x\n",
-		devpriv->adc_fifo_bits);
-#endif
-	/*  enable and clear interrupts */
-	outw(devpriv->adc_fifo_bits | DAEMI | DAHFI,
-	     devpriv->control_status + INT_ADCFIFO);
+	async->events = 0;
 
-	/*  start dac */
-	devpriv->ao_control_bits |= DAC_START | DACEN | DAC_EMPTY;
-	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
-#ifdef CB_PCIDAS_DEBUG
-	dev_dbg(dev->hw_dev, "comedi: sent 0x%x to dac control\n",
-		devpriv->ao_control_bits);
-#endif
-	spin_unlock_irqrestore(&dev->spinlock, flags);
+	if (status & DAEMI) {
+		/*  clear dac empty interrupt latch */
+		spin_lock_irqsave(&dev->spinlock, flags);
+		outw(devpriv->adc_fifo_bits | DAEMI,
+		     devpriv->control_status + INT_ADCFIFO);
+		spin_unlock_irqrestore(&dev->spinlock, flags);
+		if (inw(devpriv->ao_registers + DAC_CSR) & DAC_EMPTY) {
+			if (cmd->stop_src == TRIG_NONE ||
+			    (cmd->stop_src == TRIG_COUNT
+			     && devpriv->ao_count)) {
+				comedi_error(dev, "dac fifo underflow");
+				cb_pcidas_ao_cancel(dev, s);
+				async->events |= COMEDI_CB_ERROR;
+			}
+			async->events |= COMEDI_CB_EOA;
+		}
+	} else if (status & DAHFI) {
+		unsigned int num_bytes;
 
-	async->inttrig = NULL;
+		/*  figure out how many points we are writing to fifo */
+		num_points = half_fifo;
+		if (cmd->stop_src == TRIG_COUNT &&
+		    devpriv->ao_count < num_points)
+			num_points = devpriv->ao_count;
+		num_bytes =
+		    cfc_read_array_from_buffer(s, devpriv->ao_buffer,
+					       num_points * sizeof(short));
+		num_points = num_bytes / sizeof(short);
 
-	return 0;
+		if (async->cmd.stop_src == TRIG_COUNT)
+			devpriv->ao_count -= num_points;
+		/*  write data to board's fifo */
+		outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer,
+		      num_points);
+		/*  clear half-full interrupt latch */
+		spin_lock_irqsave(&dev->spinlock, flags);
+		outw(devpriv->adc_fifo_bits | DAHFI,
+		     devpriv->control_status + INT_ADCFIFO);
+		spin_unlock_irqrestore(&dev->spinlock, flags);
+	}
+
+	comedi_event(dev, s);
 }
 
 static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = (struct comedi_device *)d;
+	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
 	int status, s5933_status;
@@ -1541,11 +1416,6 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	async->events = 0;
 
 	s5933_status = inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR);
-#ifdef CB_PCIDAS_DEBUG
-	dev_dbg(dev->hw_dev, "intcsr 0x%x\n", s5933_status);
-	dev_dbg(dev->hw_dev, "mbef 0x%x\n",
-		inl(devpriv->s5933_config + AMCC_OP_REG_MBEF));
-#endif
 
 	if ((INTCSR_INTR_ASSERTED & s5933_status) == 0)
 		return IRQ_NONE;
@@ -1557,10 +1427,6 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 
 	status = inw(devpriv->control_status + INT_ADCFIFO);
-#ifdef CB_PCIDAS_DEBUG
-	if ((status & (INT | EOAI | LADFUL | DAHFI | DAEMI)) == 0)
-		comedi_error(dev, "spurious interrupt");
-#endif
 
 	/*  check for analog output interrupt */
 	if (status & (DAHFI | DAEMI))
@@ -1596,7 +1462,9 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 					INT_ADCFIFO)) == 0)
 				break;
 			cfc_write_to_buffer(s, inw(devpriv->adc_fifo));
-			if (async->cmd.stop_src == TRIG_COUNT && --devpriv->count == 0) {	/* end of acquisition */
+			if (async->cmd.stop_src == TRIG_COUNT &&
+			    --devpriv->count == 0) {
+				/* end of acquisition */
 				cb_pcidas_cancel(dev, s);
 				async->events |= COMEDI_CB_EOA;
 				break;
@@ -1633,243 +1501,226 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
+static struct pci_dev *cb_pcidas_find_pci_device(struct comedi_device *dev,
+						 struct comedi_devconfig *it)
 {
-	struct comedi_subdevice *s = dev->write_subdev;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
-	unsigned int half_fifo = thisboard->fifo_size / 2;
-	unsigned int num_points;
-	unsigned long flags;
-
-	async->events = 0;
+	const struct cb_pcidas_board *thisboard;
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
 
-	if (status & DAEMI) {
-		/*  clear dac empty interrupt latch */
-		spin_lock_irqsave(&dev->spinlock, flags);
-		outw(devpriv->adc_fifo_bits | DAEMI,
-		     devpriv->control_status + INT_ADCFIFO);
-		spin_unlock_irqrestore(&dev->spinlock, flags);
-		if (inw(devpriv->ao_registers + DAC_CSR) & DAC_EMPTY) {
-			if (cmd->stop_src == TRIG_NONE ||
-			    (cmd->stop_src == TRIG_COUNT
-			     && devpriv->ao_count)) {
-				comedi_error(dev, "dac fifo underflow");
-				cb_pcidas_ao_cancel(dev, s);
-				async->events |= COMEDI_CB_ERROR;
+	for_each_pci_dev(pcidev) {
+		/*  is it not a computer boards card? */
+		if (pcidev->vendor != PCI_VENDOR_ID_CB)
+			continue;
+		/*  loop through cards supported by this driver */
+		for (i = 0; i < ARRAY_SIZE(cb_pcidas_boards); i++) {
+			thisboard = &cb_pcidas_boards[i];
+			if (thisboard->device_id != pcidev->device)
+				continue;
+			/*  was a particular bus/slot requested? */
+			if (bus || slot) {
+				/*  are we on the wrong bus/slot? */
+				if (pcidev->bus->number != bus ||
+				    PCI_SLOT(pcidev->devfn) != slot) {
+					continue;
+				}
 			}
-			async->events |= COMEDI_CB_EOA;
+			dev_dbg(dev->class_dev,
+				"Found %s on bus %i, slot %i\n",
+				thisboard->name,
+				pcidev->bus->number, PCI_SLOT(pcidev->devfn));
+			dev->board_ptr = thisboard;
+			return pcidev;
 		}
-	} else if (status & DAHFI) {
-		unsigned int num_bytes;
-
-		/*  figure out how many points we are writing to fifo */
-		num_points = half_fifo;
-		if (cmd->stop_src == TRIG_COUNT &&
-		    devpriv->ao_count < num_points)
-			num_points = devpriv->ao_count;
-		num_bytes =
-		    cfc_read_array_from_buffer(s, devpriv->ao_buffer,
-					       num_points * sizeof(short));
-		num_points = num_bytes / sizeof(short);
-
-		if (async->cmd.stop_src == TRIG_COUNT)
-			devpriv->ao_count -= num_points;
-		/*  write data to board's fifo */
-		outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer,
-		      num_points);
-		/*  clear half-full interrupt latch */
-		spin_lock_irqsave(&dev->spinlock, flags);
-		outw(devpriv->adc_fifo_bits | DAHFI,
-		     devpriv->control_status + INT_ADCFIFO);
-		spin_unlock_irqrestore(&dev->spinlock, flags);
 	}
-
-	comedi_event(dev, s);
+	dev_err(dev->class_dev, "No supported card found\n");
+	return NULL;
 }
 
-/* cancel analog input command */
-static int cb_pcidas_cancel(struct comedi_device *dev,
-			    struct comedi_subdevice *s)
+static int cb_pcidas_attach(struct comedi_device *dev,
+			    struct comedi_devconfig *it)
 {
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->spinlock, flags);
-	/*  disable interrupts */
-	devpriv->adc_fifo_bits &= ~INTE & ~EOAIE;
-	outw(devpriv->adc_fifo_bits, devpriv->control_status + INT_ADCFIFO);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
+	const struct cb_pcidas_board *thisboard;
+	struct cb_pcidas_private *devpriv;
+	struct pci_dev *pcidev;
+	struct comedi_subdevice *s;
+	int i;
+	int ret;
 
-	/*  disable start trigger source and burst mode */
-	outw(0, devpriv->control_status + TRIG_CONTSTAT);
-	/*  software pacer source */
-	outw(0, devpriv->control_status + ADCMUX_CONT);
+	if (alloc_private(dev, sizeof(struct cb_pcidas_private)) < 0)
+		return -ENOMEM;
+	devpriv = dev->private;
 
-	return 0;
-}
+	pcidev = cb_pcidas_find_pci_device(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	thisboard = comedi_board(dev);
 
-/* cancel analog output command */
-static int cb_pcidas_ao_cancel(struct comedi_device *dev,
-			       struct comedi_subdevice *s)
-{
-	unsigned long flags;
+	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
+		dev_err(dev->class_dev,
+			"Failed to enable PCI device and request regions\n");
+		return -EIO;
+	}
 
-	spin_lock_irqsave(&dev->spinlock, flags);
-	/*  disable interrupts */
-	devpriv->adc_fifo_bits &= ~DAHFIE & ~DAEMIE;
-	outw(devpriv->adc_fifo_bits, devpriv->control_status + INT_ADCFIFO);
+	devpriv->s5933_config = pci_resource_start(pcidev, 0);
+	devpriv->control_status = pci_resource_start(pcidev, 1);
+	devpriv->adc_fifo = pci_resource_start(pcidev, 2);
+	devpriv->pacer_counter_dio = pci_resource_start(pcidev, 3);
+	if (thisboard->ao_nchan)
+		devpriv->ao_registers = pci_resource_start(pcidev, 4);
 
-	/*  disable output */
-	devpriv->ao_control_bits &= ~DACEN & ~DAC_PACER_MASK;
-	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
+	/*  disable and clear interrupts on amcc s5933 */
+	outl(INTCSR_INBOX_INTR_STATUS,
+	     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 
-	return 0;
-}
+	if (request_irq(pcidev->irq, cb_pcidas_interrupt,
+			IRQF_SHARED, dev->driver->driver_name, dev)) {
+		dev_dbg(dev->class_dev, "unable to allocate irq %d\n",
+			pcidev->irq);
+		return -EINVAL;
+	}
+	dev->irq = pcidev->irq;
 
-static void cb_pcidas_load_counters(struct comedi_device *dev, unsigned int *ns,
-				    int rounding_flags)
-{
-	i8253_cascade_ns_to_timer_2div(TIMER_BASE, &(devpriv->divisor1),
-				       &(devpriv->divisor2), ns,
-				       rounding_flags & TRIG_ROUND_MASK);
+	dev->board_name = thisboard->name;
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 1,
-		   devpriv->divisor1, 2);
-	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 2,
-		   devpriv->divisor2, 2);
-}
+	ret = comedi_alloc_subdevices(dev, 7);
+	if (ret)
+		return ret;
 
-static void write_calibration_bitstream(struct comedi_device *dev,
-					unsigned int register_bits,
-					unsigned int bitstream,
-					unsigned int bitstream_length)
-{
-	static const int write_delay = 1;
-	unsigned int bit;
+	s = dev->subdevices + 0;
+	/* analog input subdevice */
+	dev->read_subdev = s;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
+	/* WARNING: Number of inputs in differential mode is ignored */
+	s->n_chan = thisboard->ai_nchan;
+	s->len_chanlist = thisboard->ai_nchan;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = thisboard->ranges;
+	s->insn_read = cb_pcidas_ai_rinsn;
+	s->insn_config = ai_config_insn;
+	s->do_cmd = cb_pcidas_ai_cmd;
+	s->do_cmdtest = cb_pcidas_ai_cmdtest;
+	s->cancel = cb_pcidas_cancel;
 
-	for (bit = 1 << (bitstream_length - 1); bit; bit >>= 1) {
-		if (bitstream & bit)
-			register_bits |= SERIAL_DATA_IN_BIT;
-		else
-			register_bits &= ~SERIAL_DATA_IN_BIT;
-		udelay(write_delay);
-		outw(register_bits, devpriv->control_status + CALIBRATION_REG);
+	/* analog output subdevice */
+	s = dev->subdevices + 1;
+	if (thisboard->ao_nchan) {
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
+		s->n_chan = thisboard->ao_nchan;
+		/*
+		 * analog out resolution is the same as
+		 * analog input resolution, so use ai_bits
+		 */
+		s->maxdata = (1 << thisboard->ai_bits) - 1;
+		s->range_table = &cb_pcidas_ao_ranges;
+		s->insn_read = cb_pcidas_ao_readback_insn;
+		if (thisboard->has_ao_fifo) {
+			dev->write_subdev = s;
+			s->subdev_flags |= SDF_CMD_WRITE;
+			s->insn_write = cb_pcidas_ao_fifo_winsn;
+			s->do_cmdtest = cb_pcidas_ao_cmdtest;
+			s->do_cmd = cb_pcidas_ao_cmd;
+			s->cancel = cb_pcidas_ao_cancel;
+		} else {
+			s->insn_write = cb_pcidas_ao_nofifo_winsn;
+		}
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
 	}
-}
 
-static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
-			     uint8_t value)
-{
-	static const int num_caldac_channels = 8;
-	static const int bitstream_length = 11;
-	unsigned int bitstream = ((address & 0x7) << 8) | value;
-	static const int caldac_8800_udelay = 1;
+	/* 8255 */
+	s = dev->subdevices + 2;
+	ret = subdev_8255_init(dev, s, NULL,
+			       devpriv->pacer_counter_dio + DIO_8255);
+	if (ret)
+		return ret;
 
-	if (address >= num_caldac_channels) {
-		comedi_error(dev, "illegal caldac channel");
-		return -1;
-	}
+	/*  serial EEPROM, */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_MEMORY;
+	s->subdev_flags = SDF_READABLE | SDF_INTERNAL;
+	s->n_chan = 256;
+	s->maxdata = 0xff;
+	s->insn_read = eeprom_read_insn;
 
-	if (value == devpriv->caldac_value[address])
-		return 1;
+	/*  8800 caldac */
+	s = dev->subdevices + 4;
+	s->type = COMEDI_SUBD_CALIB;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+	s->n_chan = NUM_CHANNELS_8800;
+	s->maxdata = 0xff;
+	s->insn_read = caldac_read_insn;
+	s->insn_write = caldac_write_insn;
+	for (i = 0; i < s->n_chan; i++)
+		caldac_8800_write(dev, i, s->maxdata / 2);
 
-	devpriv->caldac_value[address] = value;
+	/*  trim potentiometer */
+	s = dev->subdevices + 5;
+	s->type = COMEDI_SUBD_CALIB;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+	if (thisboard->trimpot == AD7376) {
+		s->n_chan = NUM_CHANNELS_7376;
+		s->maxdata = 0x7f;
+	} else {
+		s->n_chan = NUM_CHANNELS_8402;
+		s->maxdata = 0xff;
+	}
+	s->insn_read = trimpot_read_insn;
+	s->insn_write = trimpot_write_insn;
+	for (i = 0; i < s->n_chan; i++)
+		cb_pcidas_trimpot_write(dev, i, s->maxdata / 2);
 
-	write_calibration_bitstream(dev, cal_enable_bits(dev), bitstream,
-				    bitstream_length);
+	/*  dac08 caldac */
+	s = dev->subdevices + 6;
+	if (thisboard->has_dac08) {
+		s->type = COMEDI_SUBD_CALIB;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+		s->n_chan = NUM_CHANNELS_DAC08;
+		s->insn_read = dac08_read_insn;
+		s->insn_write = dac08_write_insn;
+		s->maxdata = 0xff;
+		dac08_write(dev, s->maxdata / 2);
+	} else
+		s->type = COMEDI_SUBD_UNUSED;
 
-	udelay(caldac_8800_udelay);
-	outw(cal_enable_bits(dev) | SELECT_8800_BIT,
-	     devpriv->control_status + CALIBRATION_REG);
-	udelay(caldac_8800_udelay);
-	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
+	/*  make sure mailbox 4 is empty */
+	inl(devpriv->s5933_config + AMCC_OP_REG_IMB4);
+	/* Set bits to enable incoming mailbox interrupts on amcc s5933. */
+	devpriv->s5933_intcsr_bits =
+	    INTCSR_INBOX_BYTE(3) | INTCSR_INBOX_SELECT(3) |
+	    INTCSR_INBOX_FULL_INT;
+	/*  clear and enable interrupt on amcc s5933 */
+	outl(devpriv->s5933_intcsr_bits | INTCSR_INBOX_INTR_STATUS,
+	     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 
 	return 1;
 }
 
-static int trimpot_7376_write(struct comedi_device *dev, uint8_t value)
-{
-	static const int bitstream_length = 7;
-	unsigned int bitstream = value & 0x7f;
-	unsigned int register_bits;
-	static const int ad7376_udelay = 1;
-
-	register_bits = cal_enable_bits(dev) | SELECT_TRIMPOT_BIT;
-	udelay(ad7376_udelay);
-	outw(register_bits, devpriv->control_status + CALIBRATION_REG);
-
-	write_calibration_bitstream(dev, register_bits, bitstream,
-				    bitstream_length);
-
-	udelay(ad7376_udelay);
-	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
-
-	return 0;
-}
-
-/* For 1602/16 only
- * ch 0 : adc gain
- * ch 1 : adc postgain offset */
-static int trimpot_8402_write(struct comedi_device *dev, unsigned int channel,
-			      uint8_t value)
-{
-	static const int bitstream_length = 10;
-	unsigned int bitstream = ((channel & 0x3) << 8) | (value & 0xff);
-	unsigned int register_bits;
-	static const int ad8402_udelay = 1;
-
-	register_bits = cal_enable_bits(dev) | SELECT_TRIMPOT_BIT;
-	udelay(ad8402_udelay);
-	outw(register_bits, devpriv->control_status + CALIBRATION_REG);
-
-	write_calibration_bitstream(dev, register_bits, bitstream,
-				    bitstream_length);
-
-	udelay(ad8402_udelay);
-	outw(cal_enable_bits(dev), devpriv->control_status + CALIBRATION_REG);
-
-	return 0;
-}
-
-static int wait_for_nvram_ready(unsigned long s5933_base_addr)
+static void cb_pcidas_detach(struct comedi_device *dev)
 {
-	static const int timeout = 1000;
-	unsigned int i;
+	struct cb_pcidas_private *devpriv = dev->private;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	for (i = 0; i < timeout; i++) {
-		if ((inb(s5933_base_addr +
-			 AMCC_OP_REG_MCSR_NVCMD) & MCSR_NV_BUSY)
-		    == 0)
-			return 0;
-		udelay(1);
+	if (devpriv) {
+		if (devpriv->s5933_config) {
+			outl(INTCSR_INBOX_INTR_STATUS,
+			     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
+		}
+	}
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->subdevices)
+		subdev_8255_cleanup(dev, dev->subdevices + 2);
+	if (pcidev) {
+		if (devpriv->s5933_config)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
-	return -1;
-}
-
-static int nvram_read(struct comedi_device *dev, unsigned int address,
-			uint8_t *data)
-{
-	unsigned long iobase = devpriv->s5933_config;
-
-	if (wait_for_nvram_ready(iobase) < 0)
-		return -ETIMEDOUT;
-
-	outb(MCSR_NV_ENABLE | MCSR_NV_LOAD_LOW_ADDR,
-	     iobase + AMCC_OP_REG_MCSR_NVCMD);
-	outb(address & 0xff, iobase + AMCC_OP_REG_MCSR_NVDATA);
-	outb(MCSR_NV_ENABLE | MCSR_NV_LOAD_HIGH_ADDR,
-	     iobase + AMCC_OP_REG_MCSR_NVCMD);
-	outb((address >> 8) & 0xff, iobase + AMCC_OP_REG_MCSR_NVDATA);
-	outb(MCSR_NV_ENABLE | MCSR_NV_READ, iobase + AMCC_OP_REG_MCSR_NVCMD);
-
-	if (wait_for_nvram_ready(iobase) < 0)
-		return -ETIMEDOUT;
-
-	*data = inb(iobase + AMCC_OP_REG_MCSR_NVDATA);
-
-	return 0;
 }
 
 static struct comedi_driver cb_pcidas_driver = {
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 9d0b8754ff5..65cbaabf645 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -87,7 +87,6 @@ TODO:
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "comedi_pci.h"
 #include "8253.h"
 #include "8255.h"
 #include "plx9080.h"
@@ -1048,8 +1047,6 @@ struct ext_clock_info {
 
 /* this structure is for data unique to this hardware driver. */
 struct pcidas64_private {
-
-	struct pci_dev *hw_dev;	/*  pointer to board's pci_dev struct */
 	/*  base addresses (physical) */
 	resource_size_t plx9080_phys_iobase;
 	resource_size_t main_phys_iobase;
@@ -1153,7 +1150,7 @@ static int eeprom_read_insn(struct comedi_device *dev,
 static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd);
 static unsigned int get_divisor(unsigned int ns, unsigned int flags);
 static void i2c_write(struct comedi_device *dev, unsigned int address,
-		      const uint8_t * data, unsigned int length);
+		      const uint8_t *data, unsigned int length);
 static void caldac_write(struct comedi_device *dev, unsigned int channel,
 			 unsigned int value);
 static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
@@ -1230,7 +1227,7 @@ static unsigned int hw_revision(const struct comedi_device *dev,
 }
 
 static void set_dac_range_bits(struct comedi_device *dev,
-			       volatile uint16_t * bits, unsigned int channel,
+			       volatile uint16_t *bits, unsigned int channel,
 			       unsigned int range)
 {
 	unsigned int code = board(dev)->ao_range_code[range];
@@ -1345,9 +1342,11 @@ static int setup_subdevices(struct comedi_device *dev)
 	struct comedi_subdevice *s;
 	void __iomem *dio_8255_iobase;
 	int i;
+	int ret;
 
-	if (alloc_subdevices(dev, 10) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 10);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* analog input subdevice */
@@ -1552,12 +1551,13 @@ static void init_stc_registers(struct comedi_device *dev)
 
 static int alloc_and_init_dma_members(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	int i;
 
 	/*  alocate pci dma buffers */
 	for (i = 0; i < ai_dma_ring_count(board(dev)); i++) {
 		priv(dev)->ai_buffer[i] =
-		    pci_alloc_consistent(priv(dev)->hw_dev, DMA_BUFFER_SIZE,
+		    pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
 					 &priv(dev)->ai_buffer_bus_addr[i]);
 		if (priv(dev)->ai_buffer[i] == NULL)
 			return -ENOMEM;
@@ -1566,7 +1566,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 	for (i = 0; i < AO_DMA_RING_COUNT; i++) {
 		if (ao_cmd_is_supported(board(dev))) {
 			priv(dev)->ao_buffer[i] =
-			    pci_alloc_consistent(priv(dev)->hw_dev,
+			    pci_alloc_consistent(pcidev,
 						 DMA_BUFFER_SIZE,
 						 &priv(dev)->
 						 ao_buffer_bus_addr[i]);
@@ -1577,7 +1577,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 	}
 	/*  allocate dma descriptors */
 	priv(dev)->ai_dma_desc =
-	    pci_alloc_consistent(priv(dev)->hw_dev,
+	    pci_alloc_consistent(pcidev,
 				 sizeof(struct plx_dma_desc) *
 				 ai_dma_ring_count(board(dev)),
 				 &priv(dev)->ai_dma_desc_bus_addr);
@@ -1588,7 +1588,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 		    priv(dev)->ai_dma_desc_bus_addr);
 	if (ao_cmd_is_supported(board(dev))) {
 		priv(dev)->ao_dma_desc =
-		    pci_alloc_consistent(priv(dev)->hw_dev,
+		    pci_alloc_consistent(pcidev,
 					 sizeof(struct plx_dma_desc) *
 					 AO_DMA_RING_COUNT,
 					 &priv(dev)->ao_dma_desc_bus_addr);
@@ -1649,14 +1649,43 @@ static inline void warn_external_queue(struct comedi_device *dev)
 		     "Use internal AI channel queue (channels must be consecutive and use same range/aref)");
 }
 
+static struct pci_dev *cb_pcidas64_find_pci_dev(struct comedi_device *dev,
+						struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(pcidas64_boards); i++) {
+			if (pcidas64_boards[i].device_id != pcidev->device)
+				continue;
+			dev->board_ptr = pcidas64_boards + i;
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
 /*
  * Attach is called by the Comedi core to configure the driver
  * for a particular board.
  */
 static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	struct pci_dev *pcidev = NULL;
-	int index;
+	struct pci_dev *pcidev;
 	uint32_t local_range, local_decode;
 	int retval;
 
@@ -1666,45 +1695,14 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (alloc_private(dev, sizeof(struct pcidas64_private)) < 0)
 		return -ENOMEM;
 
-/*
- * Probe the device to determine what device in the series it is.
- */
-
-	for_each_pci_dev(pcidev) {
-		/*  is it not a computer boards card? */
-		if (pcidev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
-			continue;
-		/*  loop through cards supported by this driver */
-		for (index = 0; index < ARRAY_SIZE(pcidas64_boards); index++) {
-			if (pcidas64_boards[index].device_id != pcidev->device)
-				continue;
-			/*  was a particular bus/slot requested? */
-			if (it->options[0] || it->options[1]) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			priv(dev)->hw_dev = pcidev;
-			dev->board_ptr = pcidas64_boards + index;
-			break;
-		}
-		if (dev->board_ptr)
-			break;
-	}
-
-	if (dev->board_ptr == NULL) {
-		printk
-		    ("No supported ComputerBoards/MeasurementComputing card found\n");
+	pcidev = cb_pcidas64_find_pci_dev(dev, it);
+	if (!pcidev)
 		return -EIO;
-	}
-
-	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n", board(dev)->name,
-		pcidev->bus->number, PCI_SLOT(pcidev->devfn));
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
 	if (comedi_pci_enable(pcidev, dev->driver->driver_name)) {
-		dev_warn(dev->hw_dev, "failed to enable PCI device and request regions\n");
+		dev_warn(dev->class_dev,
+			 "failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 	pci_set_master(pcidev);
@@ -1712,10 +1710,11 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* Initialize dev->board_name */
 	dev->board_name = board(dev)->name;
 
+	dev->iobase = pci_resource_start(pcidev, MAIN_BADDRINDEX);
+
 	priv(dev)->plx9080_phys_iobase =
 	    pci_resource_start(pcidev, PLX9080_BADDRINDEX);
-	priv(dev)->main_phys_iobase =
-	    pci_resource_start(pcidev, MAIN_BADDRINDEX);
+	priv(dev)->main_phys_iobase = dev->iobase;
 	priv(dev)->dio_counter_phys_iobase =
 	    pci_resource_start(pcidev, DIO_COUNTER_BADDRINDEX);
 
@@ -1732,7 +1731,7 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	if (!priv(dev)->plx9080_iobase || !priv(dev)->main_iobase
 	    || !priv(dev)->dio_counter_iobase) {
-		dev_warn(dev->hw_dev, "failed to remap io memory\n");
+		dev_warn(dev->class_dev, "failed to remap io memory\n");
 		return -ENOMEM;
 	}
 
@@ -1768,19 +1767,19 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	priv(dev)->hw_revision =
 	    hw_revision(dev, readw(priv(dev)->main_iobase + HW_STATUS_REG));
-	dev_dbg(dev->hw_dev, "stc hardware revision %i\n",
+	dev_dbg(dev->class_dev, "stc hardware revision %i\n",
 		priv(dev)->hw_revision);
 	init_plx9080(dev);
 	init_stc_registers(dev);
 	/*  get irq */
 	if (request_irq(pcidev->irq, handle_interrupt, IRQF_SHARED,
 			"cb_pcidas64", dev)) {
-		dev_dbg(dev->hw_dev, "unable to allocate irq %u\n",
+		dev_dbg(dev->class_dev, "unable to allocate irq %u\n",
 			pcidev->irq);
 		return -EINVAL;
 	}
 	dev->irq = pcidev->irq;
-	dev_dbg(dev->hw_dev, "irq %u\n", dev->irq);
+	dev_dbg(dev->class_dev, "irq %u\n", dev->irq);
 
 	retval = setup_subdevices(dev);
 	if (retval < 0)
@@ -1792,12 +1791,13 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	unsigned int i;
 
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (priv(dev)) {
-		if (priv(dev)->hw_dev) {
+		if (pcidev) {
 			if (priv(dev)->plx9080_iobase) {
 				disable_plx_interrupts(dev);
 				iounmap(priv(dev)->plx9080_iobase);
@@ -1809,7 +1809,7 @@ static void detach(struct comedi_device *dev)
 			/*  free pci dma buffers */
 			for (i = 0; i < ai_dma_ring_count(board(dev)); i++) {
 				if (priv(dev)->ai_buffer[i])
-					pci_free_consistent(priv(dev)->hw_dev,
+					pci_free_consistent(pcidev,
 							    DMA_BUFFER_SIZE,
 							    priv(dev)->
 							    ai_buffer[i],
@@ -1819,7 +1819,7 @@ static void detach(struct comedi_device *dev)
 			}
 			for (i = 0; i < AO_DMA_RING_COUNT; i++) {
 				if (priv(dev)->ao_buffer[i])
-					pci_free_consistent(priv(dev)->hw_dev,
+					pci_free_consistent(pcidev,
 							    DMA_BUFFER_SIZE,
 							    priv(dev)->
 							    ao_buffer[i],
@@ -1829,7 +1829,7 @@ static void detach(struct comedi_device *dev)
 			}
 			/*  free dma descriptors */
 			if (priv(dev)->ai_dma_desc)
-				pci_free_consistent(priv(dev)->hw_dev,
+				pci_free_consistent(pcidev,
 						    sizeof(struct plx_dma_desc)
 						    *
 						    ai_dma_ring_count(board
@@ -1838,20 +1838,22 @@ static void detach(struct comedi_device *dev)
 						    priv(dev)->
 						    ai_dma_desc_bus_addr);
 			if (priv(dev)->ao_dma_desc)
-				pci_free_consistent(priv(dev)->hw_dev,
+				pci_free_consistent(pcidev,
 						    sizeof(struct plx_dma_desc)
 						    * AO_DMA_RING_COUNT,
 						    priv(dev)->ao_dma_desc,
 						    priv(dev)->
 						    ao_dma_desc_bus_addr);
-			if (priv(dev)->main_phys_iobase)
-				comedi_pci_disable(priv(dev)->hw_dev);
-
-			pci_dev_put(priv(dev)->hw_dev);
 		}
 	}
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 4);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+
+		pci_dev_put(pcidev);
+	}
 }
 
 static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -2001,7 +2003,7 @@ static int ai_config_calibration_source(struct comedi_device *dev,
 	else
 		num_calibration_sources = 8;
 	if (source >= num_calibration_sources) {
-		dev_dbg(dev->hw_dev, "invalid calibration source: %i\n",
+		dev_dbg(dev->class_dev, "invalid calibration source: %i\n",
 			source);
 		return -EINVAL;
 	}
@@ -2833,7 +2835,8 @@ static void pio_drain_ai_fifo_16(struct comedi_device *dev)
 		}
 
 		if (num_samples < 0) {
-			dev_err(dev->hw_dev, "cb_pcidas64: bug! num_samples < 0\n");
+			dev_err(dev->class_dev,
+				"cb_pcidas64: bug! num_samples < 0\n");
 			break;
 		}
 
@@ -3614,7 +3617,7 @@ static int di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	data[1] = bits;
 	data[0] = 0;
 
-	return 2;
+	return insn->n;
 }
 
 static int do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -3630,7 +3633,7 @@ static int do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int dio_60xx_config_insn(struct comedi_device *dev,
@@ -3673,7 +3676,7 @@ static int dio_60xx_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = readb(priv(dev)->dio_counter_iobase + DIO_DATA_60XX_REG);
 
-	return 2;
+	return insn->n;
 }
 
 static void caldac_write(struct comedi_device *dev, unsigned int channel,
@@ -4191,7 +4194,7 @@ static void i2c_stop(struct comedi_device *dev)
 }
 
 static void i2c_write(struct comedi_device *dev, unsigned int address,
-		      const uint8_t * data, unsigned int length)
+		      const uint8_t *data, unsigned int length)
 {
 	unsigned int i;
 	uint8_t bitstream;
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index 25ebca11ead..12660a384e5 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -48,10 +48,8 @@ Please report success/failure with other different cards to
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
 #include "8255.h"
 
-
 /* PCI vendor number of ComputerBoards */
 #define PCI_VENDOR_ID_CB        0x1307
 #define EEPROM_SIZE	128	/*  number of entries in eeprom */
@@ -218,9 +216,6 @@ static const struct cb_pcidda_board cb_pcidda_boards[] = {
 struct cb_pcidda_private {
 	int data;
 
-	/* would be useful for a PCI device */
-	struct pci_dev *pci_dev;
-
 	unsigned long digitalio;
 	unsigned long dac;
 
@@ -257,6 +252,36 @@ static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
 static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 				unsigned int range);
 
+static struct pci_dev *cb_pcidda_find_pci_dev(struct comedi_device *dev,
+					      struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_CB)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(cb_pcidda_boards); i++) {
+			if (cb_pcidda_boards[i].device_id != pcidev->device)
+				continue;
+			dev->board_ptr = cb_pcidda_boards + i;
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
 /*
  * Attach is called by the Comedi core to configure the driver
  * for a particular board.
@@ -264,10 +289,10 @@ static void cb_pcidda_calibrate(struct comedi_device *dev, unsigned int channel,
 static int cb_pcidda_attach(struct comedi_device *dev,
 			    struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	struct pci_dev *pcidev = NULL;
 	int index;
-
+	int ret;
 
 /*
  * Allocate the private structure area.
@@ -275,51 +300,26 @@ static int cb_pcidda_attach(struct comedi_device *dev,
 	if (alloc_private(dev, sizeof(struct cb_pcidda_private)) < 0)
 		return -ENOMEM;
 
-/*
- * Probe the device to determine what device in the series it is.
- */
-
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_CB) {
-			if (it->options[0] || it->options[1]) {
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			for (index = 0; index < ARRAY_SIZE(cb_pcidda_boards); index++) {
-				if (cb_pcidda_boards[index].device_id ==
-				    pcidev->device) {
-					goto found;
-				}
-			}
-		}
-	}
-	if (!pcidev) {
-		dev_err(dev->hw_dev, "Not a ComputerBoards/MeasurementComputing card on requested position\n");
+	pcidev = cb_pcidda_find_pci_dev(dev, it);
+	if (!pcidev)
 		return -EIO;
-	}
-found:
-	devpriv->pci_dev = pcidev;
-	dev->board_ptr = cb_pcidda_boards + index;
-	/*  "thisboard" macro can be used from here. */
-	dev_dbg(dev->hw_dev, "Found %s at requested position\n",
-		thisboard->name);
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
 	/*
 	 * Enable PCI device and request regions.
 	 */
 	if (comedi_pci_enable(pcidev, thisboard->name)) {
-		dev_err(dev->hw_dev, "cb_pcidda: failed to enable PCI device and request regions\n");
+		dev_err(dev->class_dev,
+			"cb_pcidda: failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 
 /*
  * Allocate the I/O ports.
  */
-	devpriv->digitalio =
-	    pci_resource_start(devpriv->pci_dev, DIGITALIO_BADRINDEX);
-	devpriv->dac = pci_resource_start(devpriv->pci_dev, DAC_BADRINDEX);
+	devpriv->digitalio = pci_resource_start(pcidev, DIGITALIO_BADRINDEX);
+	devpriv->dac = pci_resource_start(pcidev, DAC_BADRINDEX);
+	dev->iobase = devpriv->dac;
 
 /*
  * Warn about the status of the driver.
@@ -335,11 +335,9 @@ found:
  */
 	dev->board_name = thisboard->name;
 
-/*
- * Allocate the subdevice structures.
- */
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* analog output subdevice */
@@ -360,10 +358,10 @@ found:
 	s = dev->subdevices + 2;
 	subdev_8255_init(dev, s, NULL, devpriv->digitalio + PORT2A);
 
-	dev_dbg(dev->hw_dev, "eeprom:\n");
+	dev_dbg(dev->class_dev, "eeprom:\n");
 	for (index = 0; index < EEPROM_SIZE; index++) {
 		devpriv->eeprom_data[index] = cb_pcidda_read_eeprom(dev, index);
-		dev_dbg(dev->hw_dev, "%i:0x%x\n", index,
+		dev_dbg(dev->class_dev, "%i:0x%x\n", index,
 			devpriv->eeprom_data[index]);
 	}
 
@@ -376,12 +374,12 @@ found:
 
 static void cb_pcidda_detach(struct comedi_device *dev)
 {
-	if (devpriv) {
-		if (devpriv->pci_dev) {
-			if (devpriv->dac)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 	if (dev->subdevices) {
 		subdev_8255_cleanup(dev, dev->subdevices + 1);
diff --git a/drivers/staging/comedi/drivers/cb_pcidio.c b/drivers/staging/comedi/drivers/cb_pcidio.c
index 713132c8dbb..e370d0d81bb 100644
--- a/drivers/staging/comedi/drivers/cb_pcidio.c
+++ b/drivers/staging/comedi/drivers/cb_pcidio.c
@@ -41,7 +41,6 @@ Passing a zero for an option is the same as leaving it unspecified.
 
 /*------------------------------ HEADER FILES ---------------------------------*/
 #include "../comedidev.h"
-#include "comedi_pci.h"
 #include "8255.h"
 
 /*-------------------------- MACROS and DATATYPES -----------------------------*/
@@ -91,74 +90,46 @@ static const struct pcidio_board pcidio_boards[] = {
  */
 #define thisboard ((const struct pcidio_board *)dev->board_ptr)
 
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.  */
-struct pcidio_private {
-	int data;		/*  currently unused */
-
-	/* would be useful for a PCI device */
-	struct pci_dev *pci_dev;
-
-	/* used for DO readback, currently unused */
-	unsigned int do_readback[4];	/* up to 4 unsigned int suffice to hold 96 bits for PCI-DIO96 */
-
-	unsigned long dio_reg_base;	/*  address of port A of the first 8255 chip on board */
-};
-
-/*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct pcidio_private *)dev->private)
-
-static int pcidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static struct pci_dev *pcidio_find_pci_dev(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
 {
 	struct pci_dev *pcidev = NULL;
-	int index;
+	int bus = it->options[0];
+	int slot = it->options[1];
 	int i;
 
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct pcidio_private)) < 0)
-		return -ENOMEM;
-/*
- * If you can probe the device to determine what device in a series
- * it is, this is the place to do it.  Otherwise, dev->board_ptr
- * should already be initialized.
- */
-/*
- * Probe the device to determine what device in the series it is.
- */
-
 	for_each_pci_dev(pcidev) {
-		/*  is it not a computer boards card? */
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+				slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
 		if (pcidev->vendor != PCI_VENDOR_ID_CB)
 			continue;
-		/*  loop through cards supported by this driver */
-		for (index = 0; index < ARRAY_SIZE(pcidio_boards); index++) {
-			if (pcidio_boards[index].dev_id != pcidev->device)
+		for (i = 0; i < ARRAY_SIZE(pcidio_boards); i++) {
+			if (pcidio_boards[i].dev_id != pcidev->device)
 				continue;
 
-			/*  was a particular bus/slot requested? */
-			if (it->options[0] || it->options[1]) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			dev->board_ptr = pcidio_boards + index;
-			goto found;
+			dev->board_ptr = pcidio_boards + i;
+			return pcidev;
 		}
 	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
 
-	dev_err(dev->hw_dev, "No supported ComputerBoards/MeasurementComputing card found on requested position\n");
-	return -EIO;
+static int pcidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev;
+	int i;
+	int ret;
 
-found:
+	pcidev = pcidio_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
 /*
  * Initialize dev->board_name.  Note that we can use the "thisboard"
@@ -166,30 +137,20 @@ found:
  */
 	dev->board_name = thisboard->name;
 
-	devpriv->pci_dev = pcidev;
-	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n", thisboard->name,
-		devpriv->pci_dev->bus->number,
-		PCI_SLOT(devpriv->pci_dev->devfn));
 	if (comedi_pci_enable(pcidev, thisboard->name))
 		return -EIO;
 
-	devpriv->dio_reg_base
-	    =
-	    pci_resource_start(devpriv->pci_dev,
-			       pcidio_boards[index].dioregs_badrindex);
+	dev->iobase = pci_resource_start(pcidev, thisboard->dioregs_badrindex);
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, thisboard->n_8255) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, thisboard->n_8255);
+	if (ret)
+		return ret;
 
 	for (i = 0; i < thisboard->n_8255; i++) {
 		subdev_8255_init(dev, dev->subdevices + i,
-				 NULL, devpriv->dio_reg_base + i * 4);
-		dev_dbg(dev->hw_dev, "subdev %d: base = 0x%lx\n", i,
-			devpriv->dio_reg_base + i * 4);
+				 NULL, dev->iobase + i * 4);
+		dev_dbg(dev->class_dev, "subdev %d: base = 0x%lx\n", i,
+			dev->iobase + i * 4);
 	}
 
 	return 1;
@@ -197,12 +158,12 @@ found:
 
 static void pcidio_detach(struct comedi_device *dev)
 {
-	if (devpriv) {
-		if (devpriv->pci_dev) {
-			if (devpriv->dio_reg_base)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 	if (dev->subdevices) {
 		int i;
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index 5f834d02ec2..c632a89f3ae 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -45,7 +45,6 @@ See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "comedi_pci.h"
 #include "plx9052.h"
 #include "8255.h"
 
@@ -57,11 +56,7 @@ See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
 /* Registers for the PCIM-DAS1602/16 */
 
 /* sizes of io regions (bytes) */
-#define BADR0_SIZE 2		/* ?? */
-#define BADR1_SIZE 4
-#define BADR2_SIZE 6
 #define BADR3_SIZE 16
-#define BADR4_SIZE 4
 
 /* DAC Offsets */
 #define ADC_TRIG 0
@@ -123,8 +118,6 @@ static const struct cb_pcimdas_board cb_pcimdas_boards[] = {
 	 },
 };
 
-#define N_BOARDS 1		/*  Max number of boards supported */
-
 /*
  * Useful for shorthand access to the particular board structure
  */
@@ -137,27 +130,11 @@ static const struct cb_pcimdas_board cb_pcimdas_boards[] = {
  * struct.
  */
 struct cb_pcimdas_private {
-	int data;
-
-	/*  would be useful for a PCI device */
-	struct pci_dev *pci_dev;
-
 	/* base addresses */
-	unsigned long BADR0;
-	unsigned long BADR1;
-	unsigned long BADR2;
 	unsigned long BADR3;
-	unsigned long BADR4;
 
 	/* Used for AO readback */
 	unsigned int ao_readback[2];
-
-	/*  Used for DIO */
-	unsigned short int port_a;	/*  copy of BADR4+0 */
-	unsigned short int port_b;	/*  copy of BADR4+1 */
-	unsigned short int port_c;	/*  copy of BADR4+2 */
-	unsigned short int dio_mode;	/*  copy of BADR4+3 */
-
 };
 
 /*
@@ -176,6 +153,37 @@ static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data);
 
+static struct pci_dev *cb_pcimdas_find_pci_dev(struct comedi_device *dev,
+					       struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+				slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(cb_pcimdas_boards); i++) {
+			if (cb_pcimdas_boards[i].device_id != pcidev->device)
+				continue;
+
+			dev->board_ptr = cb_pcimdas_boards + i;
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
 /*
  * Attach is called by the Comedi core to configure the driver
  * for a particular board.  If you specified a board_name array
@@ -185,10 +193,10 @@ static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
 static int cb_pcimdas_attach(struct comedi_device *dev,
 			     struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	struct pci_dev *pcidev = NULL;
-	int index;
-	/* int i; */
+	unsigned long iobase_8255;
+	int ret;
 
 /*
  * Allocate the private structure area.
@@ -196,86 +204,46 @@ static int cb_pcimdas_attach(struct comedi_device *dev,
 	if (alloc_private(dev, sizeof(struct cb_pcimdas_private)) < 0)
 		return -ENOMEM;
 
-/*
- * Probe the device to determine what device in the series it is.
- */
-
-	for_each_pci_dev(pcidev) {
-		/*  is it not a computer boards card? */
-		if (pcidev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
-			continue;
-		/*  loop through cards supported by this driver */
-		for (index = 0; index < N_BOARDS; index++) {
-			if (cb_pcimdas_boards[index].device_id !=
-			    pcidev->device)
-				continue;
-			/*  was a particular bus/slot requested? */
-			if (it->options[0] || it->options[1]) {
-				/*  are we on the wrong bus/slot? */
-				if (pcidev->bus->number != it->options[0] ||
-				    PCI_SLOT(pcidev->devfn) != it->options[1]) {
-					continue;
-				}
-			}
-			devpriv->pci_dev = pcidev;
-			dev->board_ptr = cb_pcimdas_boards + index;
-			goto found;
-		}
-	}
-
-	dev_err(dev->hw_dev, "No supported ComputerBoards/MeasurementComputing card found on requested position\n");
-	return -EIO;
-
-found:
-
-	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n",
-		cb_pcimdas_boards[index].name, pcidev->bus->number,
-		PCI_SLOT(pcidev->devfn));
+	pcidev = cb_pcimdas_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
 	/*  Warn about non-tested features */
 	switch (thisboard->device_id) {
 	case 0x56:
 		break;
 	default:
-		dev_dbg(dev->hw_dev, "THIS CARD IS UNSUPPORTED.\n"
+		dev_dbg(dev->class_dev, "THIS CARD IS UNSUPPORTED.\n");
+		dev_dbg(dev->class_dev,
 			"PLEASE REPORT USAGE TO <mocelet@sucs.org>\n");
 	}
 
 	if (comedi_pci_enable(pcidev, "cb_pcimdas")) {
-		dev_err(dev->hw_dev, "Failed to enable PCI device and request regions\n");
+		dev_err(dev->class_dev,
+			"Failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 
-	devpriv->BADR0 = pci_resource_start(devpriv->pci_dev, 0);
-	devpriv->BADR1 = pci_resource_start(devpriv->pci_dev, 1);
-	devpriv->BADR2 = pci_resource_start(devpriv->pci_dev, 2);
-	devpriv->BADR3 = pci_resource_start(devpriv->pci_dev, 3);
-	devpriv->BADR4 = pci_resource_start(devpriv->pci_dev, 4);
-
-	dev_dbg(dev->hw_dev, "devpriv->BADR0 = 0x%lx\n", devpriv->BADR0);
-	dev_dbg(dev->hw_dev, "devpriv->BADR1 = 0x%lx\n", devpriv->BADR1);
-	dev_dbg(dev->hw_dev, "devpriv->BADR2 = 0x%lx\n", devpriv->BADR2);
-	dev_dbg(dev->hw_dev, "devpriv->BADR3 = 0x%lx\n", devpriv->BADR3);
-	dev_dbg(dev->hw_dev, "devpriv->BADR4 = 0x%lx\n", devpriv->BADR4);
+	dev->iobase = pci_resource_start(pcidev, 2);
+	devpriv->BADR3 = pci_resource_start(pcidev, 3);
+	iobase_8255 = pci_resource_start(pcidev, 4);
 
 /* Dont support IRQ yet */
 /*  get irq */
-/* if(request_irq(devpriv->pci_dev->irq, cb_pcimdas_interrupt, IRQF_SHARED, "cb_pcimdas", dev )) */
+/* if(request_irq(pcidev->irq, cb_pcimdas_interrupt, IRQF_SHARED, "cb_pcimdas", dev )) */
 /* { */
-/* printk(" unable to allocate irq %u\n", devpriv->pci_dev->irq); */
+/* printk(" unable to allocate irq %u\n", pcidev->irq); */
 /* return -EINVAL; */
 /* } */
-/* dev->irq = devpriv->pci_dev->irq; */
+/* dev->irq = pcidev->irq; */
 
 	/* Initialize dev->board_name */
 	dev->board_name = thisboard->name;
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* dev->read_subdev=s; */
@@ -303,7 +271,7 @@ found:
 	s = dev->subdevices + 2;
 	/* digital i/o subdevice */
 	if (thisboard->has_dio)
-		subdev_8255_init(dev, s, NULL, devpriv->BADR4);
+		subdev_8255_init(dev, s, NULL, iobase_8255);
 	else
 		s->type = COMEDI_SUBD_UNUSED;
 
@@ -312,14 +280,14 @@ found:
 
 static void cb_pcimdas_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->irq)
 		free_irq(dev->irq, dev);
-	if (devpriv) {
-		if (devpriv->pci_dev) {
-			if (devpriv->BADR0)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
@@ -368,7 +336,7 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
 		/* trigger conversion */
-		outw(0, devpriv->BADR2 + 0);
+		outw(0, dev->iobase + 0);
 
 #define TIMEOUT 1000		/* typically takes 5 loops on a lightly loaded Pentium 100MHz, */
 		/* this is likely to be 100 loops on a 2GHz machine, so set 1000 as the limit. */
@@ -384,7 +352,7 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 			return -ETIMEDOUT;
 		}
 		/* read data */
-		d = inw(devpriv->BADR2 + 0);
+		d = inw(dev->iobase + 0);
 
 		/* mangle the data as necessary */
 		/* d ^= 1<<(thisboard->ai_bits-1); // 16 bit data from ADC, so no mangle needed. */
@@ -408,10 +376,10 @@ static int cb_pcimdas_ao_winsn(struct comedi_device *dev,
 	for (i = 0; i < insn->n; i++) {
 		switch (chan) {
 		case 0:
-			outw(data[i] & 0x0FFF, devpriv->BADR2 + DAC0_OFFSET);
+			outw(data[i] & 0x0FFF, dev->iobase + DAC0_OFFSET);
 			break;
 		case 1:
-			outw(data[i] & 0x0FFF, devpriv->BADR2 + DAC1_OFFSET);
+			outw(data[i] & 0x0FFF, dev->iobase + DAC1_OFFSET);
 			break;
 		default:
 			return -1;
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index b339685e234..a80146133c0 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -86,8 +86,6 @@ Configuration Options:
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #include "8255.h"
 
 /* device ids of the cards we support -- currently only 1 card supported */
@@ -238,12 +236,9 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
  */
 	dev->board_name = thisboard->name;
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	err = comedi_alloc_subdevices(dev, 2);
+	if (err)
+		return err;
 
 	s = dev->subdevices + 0;
 
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
index 29412de06c3..5ed324c4f62 100644
--- a/drivers/staging/comedi/drivers/comedi_bond.c
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -133,8 +133,6 @@ static int bonding_dio_insn_bits(struct comedi_device *dev,
 {
 #define LSAMPL_BITS (sizeof(unsigned int)*8)
 	unsigned nchans = LSAMPL_BITS, num_done = 0, i;
-	if (insn->n != 2)
-		return -EINVAL;
 
 	if (devpriv->nchans < nchans)
 		nchans = devpriv->nchans;
@@ -336,6 +334,7 @@ static int bonding_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
+	int ret;
 
 	LOG_MSG("comedi%d\n", dev->minor);
 
@@ -358,12 +357,9 @@ static int bonding_attach(struct comedi_device *dev,
 	 */
 	dev->board_name = devpriv->name;
 
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	s->type = COMEDI_SUBD_DIO;
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index bff5dcd7610..9a63cac2434 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -110,7 +110,7 @@ static int parport_insn_a(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = inb(dev->iobase + PARPORT_A);
 
-	return 2;
+	return insn->n;
 }
 
 static int parport_insn_config_a(struct comedi_device *dev,
@@ -139,7 +139,7 @@ static int parport_insn_b(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = (inb(dev->iobase + PARPORT_B) >> 3);
 
-	return 2;
+	return insn->n;
 }
 
 static int parport_insn_c(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -155,18 +155,15 @@ static int parport_insn_c(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = devpriv->c_data & 0xf;
 
-	return 2;
+	return insn->n;
 }
 
 static int parport_intr_insn(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n < 1)
-		return -EINVAL;
-
 	data[1] = 0;
-	return 2;
+	return insn->n;
 }
 
 static int parport_intr_cmdtest(struct comedi_device *dev,
@@ -315,9 +312,10 @@ static int parport_attach(struct comedi_device *dev,
 	}
 	dev->board_name = "parport";
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
+
 	ret = alloc_private(dev, sizeof(struct parport_private));
 	if (ret < 0)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/comedi_pci.h b/drivers/staging/comedi/drivers/comedi_pci.h
deleted file mode 100644
index c14a036a053..00000000000
--- a/drivers/staging/comedi/drivers/comedi_pci.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
-    comedi/drivers/comedi_pci.h
-    Various PCI functions for drivers.
-
-    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You 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 _COMEDI_PCI_H_
-#define _COMEDI_PCI_H_
-
-#include <linux/pci.h>
-
-/*
- * Enable the PCI device and request the regions.
- */
-static inline int comedi_pci_enable(struct pci_dev *pdev, const char *res_name)
-{
-	int rc;
-
-	rc = pci_enable_device(pdev);
-	if (rc < 0)
-		return rc;
-
-	rc = pci_request_regions(pdev, res_name);
-	if (rc < 0)
-		pci_disable_device(pdev);
-
-	return rc;
-}
-
-/*
- * Release the regions and disable the PCI device.
- *
- * This must be matched with a previous successful call to comedi_pci_enable().
- */
-static inline void comedi_pci_disable(struct pci_dev *pdev)
-{
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-}
-
-#endif
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index 873e37450bb..523a809708b 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -67,8 +67,6 @@ struct waveform_board {
 
 #define N_CHANS 8
 
-#define thisboard ((const struct waveform_board *)dev->board_ptr)
-
 /* Data unique to this driver */
 struct waveform_private {
 	struct timer_list timer;
@@ -429,12 +427,14 @@ static int waveform_ao_insn_write(struct comedi_device *dev,
 static int waveform_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
+	const struct waveform_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int amplitude = it->options[0];
 	int period = it->options[1];
 	int i;
+	int ret;
 
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	if (alloc_private(dev, sizeof(struct waveform_private)) < 0)
 		return -ENOMEM;
@@ -448,17 +448,17 @@ static int waveform_attach(struct comedi_device *dev,
 	devpriv->uvolt_amplitude = amplitude;
 	devpriv->usec_period = period;
 
-	dev->n_subdevices = 2;
-	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	dev->read_subdev = s;
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->n_chan = board->ai_chans;
+	s->maxdata = (1 << board->ai_bits) - 1;
 	s->range_table = &waveform_ai_ranges;
 	s->len_chanlist = s->n_chan * 2;
 	s->insn_read = waveform_ai_insn_read;
@@ -471,8 +471,8 @@ static int waveform_attach(struct comedi_device *dev,
 	/* analog output subdevice (loopback) */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->n_chan = board->ai_chans;
+	s->maxdata = (1 << board->ai_bits) - 1;
 	s->range_table = &waveform_ai_ranges;
 	s->len_chanlist = s->n_chan * 2;
 	s->insn_write = waveform_ao_insn_write;
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index b8bac80f2ba..944cfeeb2b2 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -36,8 +36,6 @@ Configuration Options:
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 enum contec_model {
 	PIO1616L = 0,
 };
@@ -59,34 +57,22 @@ static const struct contec_board contec_boards[] = {
 
 #define thisboard ((const struct contec_board *)dev->board_ptr)
 
-struct contec_private {
-	int data;
-
-	struct pci_dev *pci_dev;
-
-};
-
-#define devpriv ((struct contec_private *)dev->private)
-
 static int contec_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
 
-	dev_dbg(dev->hw_dev, "contec_do_insn_bits called\n");
-	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
-
-	if (insn->n != 2)
-		return -EINVAL;
+	dev_dbg(dev->class_dev, "contec_do_insn_bits called\n");
+	dev_dbg(dev->class_dev, "data: %d %d\n", data[0], data[1]);
 
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
-		dev_dbg(dev->hw_dev, "out: %d on %lx\n", s->state,
+		dev_dbg(dev->class_dev, "out: %d on %lx\n", s->state,
 			dev->iobase + thisboard->out_offs);
 		outw(s->state, dev->iobase + thisboard->out_offs);
 	}
-	return 2;
+	return insn->n;
 }
 
 static int contec_di_insn_bits(struct comedi_device *dev,
@@ -94,87 +80,96 @@ static int contec_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_insn *insn, unsigned int *data)
 {
 
-	dev_dbg(dev->hw_dev, "contec_di_insn_bits called\n");
-	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
-
-	if (insn->n != 2)
-		return -EINVAL;
+	dev_dbg(dev->class_dev, "contec_di_insn_bits called\n");
+	dev_dbg(dev->class_dev, "data: %d %d\n", data[0], data[1]);
 
 	data[1] = inw(dev->iobase + thisboard->in_offs);
 
-	return 2;
+	return insn->n;
 }
 
-static int contec_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static struct pci_dev *contec_find_pci_dev(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
 {
 	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+				slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_CONTEC ||
+		    pcidev->device != PCI_DEVICE_ID_PIO1616L)
+			continue;
+
+		dev->board_ptr = contec_boards + 0;
+		return pcidev;
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
+static int contec_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
+	int ret;
 
 	printk("comedi%d: contec: ", dev->minor);
 
 	dev->board_name = thisboard->name;
 
-	if (alloc_private(dev, sizeof(struct contec_private)) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	pcidev = contec_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	for_each_pci_dev(pcidev) {
-		if (pcidev->vendor == PCI_VENDOR_ID_CONTEC &&
-		    pcidev->device == PCI_DEVICE_ID_PIO1616L) {
-			if (it->options[0] || it->options[1]) {
-				/* Check bus and slot. */
-				if (it->options[0] != pcidev->bus->number ||
-				    it->options[1] != PCI_SLOT(pcidev->devfn)) {
-					continue;
-				}
-			}
-			devpriv->pci_dev = pcidev;
-			if (comedi_pci_enable(pcidev, "contec_pci_dio")) {
-				printk
-				    ("error enabling PCI device and request regions!\n");
-				return -EIO;
-			}
-			dev->iobase = pci_resource_start(pcidev, 0);
-			printk(" base addr %lx ", dev->iobase);
-
-			dev->board_ptr = contec_boards + 0;
-
-			s = dev->subdevices + 0;
-
-			s->type = COMEDI_SUBD_DI;
-			s->subdev_flags = SDF_READABLE;
-			s->n_chan = 16;
-			s->maxdata = 1;
-			s->range_table = &range_digital;
-			s->insn_bits = contec_di_insn_bits;
-
-			s = dev->subdevices + 1;
-			s->type = COMEDI_SUBD_DO;
-			s->subdev_flags = SDF_WRITABLE;
-			s->n_chan = 16;
-			s->maxdata = 1;
-			s->range_table = &range_digital;
-			s->insn_bits = contec_do_insn_bits;
-
-			printk("attached\n");
-
-			return 1;
-		}
+	if (comedi_pci_enable(pcidev, "contec_pci_dio")) {
+		printk("error enabling PCI device and request regions!\n");
+		return -EIO;
 	}
+	dev->iobase = pci_resource_start(pcidev, 0);
+	printk(" base addr %lx ", dev->iobase);
+
+	s = dev->subdevices + 0;
 
-	printk("card not present!\n");
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = contec_di_insn_bits;
 
-	return -EIO;
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = contec_do_insn_bits;
+
+	printk("attached\n");
+
+	return 1;
 }
 
 static void contec_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pci_dev);
-		pci_dev_put(devpriv->pci_dev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index 696b58ca2e5..ef28385c148 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -118,11 +118,12 @@ Configuration options:
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#include "comedi_pci.h"
 #include "8255.h"
 
-#define DAQBOARD2000_SUBSYSTEM_IDS2 	0x00021616	/* Daqboard/2000 - 2 Dacs */
-#define DAQBOARD2000_SUBSYSTEM_IDS4 	0x00041616	/* Daqboard/2000 - 4 Dacs */
+#define PCI_VENDOR_ID_IOTECH		0x1616
+
+#define DAQBOARD2000_SUBSYSTEM_IDS2 	0x0002	/* Daqboard/2000 - 2 Dacs */
+#define DAQBOARD2000_SUBSYSTEM_IDS4 	0x0004	/* Daqboard/2000 - 4 Dacs */
 
 #define DAQBOARD2000_DAQ_SIZE 		0x1002
 #define DAQBOARD2000_PLX_SIZE 		0x100
@@ -316,10 +317,8 @@ struct daqboard2000_private {
 	enum {
 		card_daqboard_2000
 	} card;
-	struct pci_dev *pci_dev;
 	void *daq;
-	void *plx;
-	int got_regions;
+	void __iomem *plx;
 	unsigned int ao_readback[2];
 };
 
@@ -486,7 +485,7 @@ static int daqboard2000_ao_insn_write(struct comedi_device *dev,
 
 static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 {
-	dev_dbg(dev->hw_dev, "daqboard2000_resetLocalBus\n");
+	dev_dbg(dev->class_dev, "daqboard2000_resetLocalBus\n");
 	writel(DAQBOARD2000_SECRLocalBusHi, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRLocalBusLo, devpriv->plx + 0x6c);
@@ -495,7 +494,7 @@ static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 
 static void daqboard2000_reloadPLX(struct comedi_device *dev)
 {
-	dev_dbg(dev->hw_dev, "daqboard2000_reloadPLX\n");
+	dev_dbg(dev->class_dev, "daqboard2000_reloadPLX\n");
 	writel(DAQBOARD2000_SECRReloadLo, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRReloadHi, devpriv->plx + 0x6c);
@@ -506,7 +505,7 @@ static void daqboard2000_reloadPLX(struct comedi_device *dev)
 
 static void daqboard2000_pulseProgPin(struct comedi_device *dev)
 {
-	dev_dbg(dev->hw_dev, "daqboard2000_pulseProgPin 1\n");
+	dev_dbg(dev->class_dev, "daqboard2000_pulseProgPin 1\n");
 	writel(DAQBOARD2000_SECRProgPinHi, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRProgPinLo, devpriv->plx + 0x6c);
@@ -558,14 +557,14 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 	secr = readl(devpriv->plx + 0x6c);
 	if (!(secr & DAQBOARD2000_EEPROM_PRESENT)) {
 #ifdef DEBUG_EEPROM
-		dev_dbg(dev->hw_dev, "no serial eeprom\n");
+		dev_dbg(dev->class_dev, "no serial eeprom\n");
 #endif
 		return -EIO;
 	}
 
 	for (retry = 0; retry < 3; retry++) {
 #ifdef DEBUG_EEPROM
-		dev_dbg(dev->hw_dev, "Programming EEPROM try %x\n", retry);
+		dev_dbg(dev->class_dev, "Programming EEPROM try %x\n", retry);
 #endif
 
 		daqboard2000_resetLocalBus(dev);
@@ -576,8 +575,8 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 				if (cpld_array[i] == 0xff
 				    && cpld_array[i + 1] == 0x20) {
 #ifdef DEBUG_EEPROM
-					dev_dbg(dev->hw_dev, "Preamble found at %d\n",
-						i);
+					dev_dbg(dev->class_dev,
+						"Preamble found at %d\n", i);
 #endif
 					break;
 				}
@@ -590,7 +589,7 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 			}
 			if (i >= len) {
 #ifdef DEBUG_EEPROM
-				dev_dbg(dev->hw_dev, "Programmed\n");
+				dev_dbg(dev->class_dev, "Programmed\n");
 #endif
 				daqboard2000_resetLocalBus(dev);
 				daqboard2000_reloadPLX(dev);
@@ -704,85 +703,79 @@ static int daqboard2000_8255_cb(int dir, int port, int data,
 	return result;
 }
 
+static struct pci_dev *daqboard2000_find_pci_dev(struct comedi_device *dev,
+						 struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_IOTECH ||
+		    pcidev->device != 0x0409)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
+			if (boardtypes[i].id != pcidev->subsystem_device)
+				continue;
+			dev->board_ptr = boardtypes + i;
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
 static int daqboard2000_attach(struct comedi_device *dev,
 			       struct comedi_devconfig *it)
 {
-	int result = 0;
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	struct pci_dev *card = NULL;
 	void *aux_data;
 	unsigned int aux_len;
-	int bus, slot;
-
-	bus = it->options[0];
-	slot = it->options[1];
+	int result;
 
 	result = alloc_private(dev, sizeof(struct daqboard2000_private));
 	if (result < 0)
 		return -ENOMEM;
 
-	for (card = pci_get_device(0x1616, 0x0409, NULL);
-	     card != NULL; card = pci_get_device(0x1616, 0x0409, card)) {
-		if (bus || slot) {
-			/* requested particular bus/slot */
-			if (card->bus->number != bus ||
-			    PCI_SLOT(card->devfn) != slot) {
-				continue;
-			}
-		}
-		break;		/* found one */
-	}
-	if (!card) {
-		if (bus || slot)
-			dev_err(dev->hw_dev, "no daqboard2000 found at bus/slot: %d/%d\n",
-				bus, slot);
-		else
-			dev_err(dev->hw_dev, "no daqboard2000 found\n");
+	pcidev = daqboard2000_find_pci_dev(dev, it);
+	if (!pcidev)
 		return -EIO;
-	} else {
-		u32 id;
-		int i;
-		devpriv->pci_dev = card;
-		id = ((u32) card->
-		      subsystem_device << 16) | card->subsystem_vendor;
-		for (i = 0; i < ARRAY_SIZE(boardtypes); i++) {
-			if (boardtypes[i].id == id) {
-				dev_dbg(dev->hw_dev, "%s\n",
-					boardtypes[i].name);
-				dev->board_ptr = boardtypes + i;
-			}
-		}
-		if (!dev->board_ptr) {
-			printk
-			    (" unknown subsystem id %08x (pretend it is an ids2)",
-			     id);
-			dev->board_ptr = boardtypes;
-		}
-	}
+	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	result = comedi_pci_enable(card, "daqboard2000");
+	result = comedi_pci_enable(pcidev, "daqboard2000");
 	if (result < 0) {
-		dev_err(dev->hw_dev, "failed to enable PCI device and request regions\n");
+		dev_err(dev->class_dev,
+			"failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
-	devpriv->got_regions = 1;
+	dev->iobase = pci_resource_start(pcidev, 2);
+
 	devpriv->plx =
-	    ioremap(pci_resource_start(card, 0), DAQBOARD2000_PLX_SIZE);
-	devpriv->daq =
-	    ioremap(pci_resource_start(card, 2), DAQBOARD2000_DAQ_SIZE);
+	    ioremap(pci_resource_start(pcidev, 0), DAQBOARD2000_PLX_SIZE);
+	devpriv->daq = ioremap(dev->iobase, DAQBOARD2000_DAQ_SIZE);
 	if (!devpriv->plx || !devpriv->daq)
 		return -ENOMEM;
 
-	result = alloc_subdevices(dev, 3);
-	if (result < 0)
-		goto out;
+	result = comedi_alloc_subdevices(dev, 3);
+	if (result)
+		return result;
 
 	readl(devpriv->plx + 0x6c);
 
 	/*
 	   u8 interrupt;
 	   Windows code does restore interrupts, but since we don't use them...
-	   pci_read_config_byte(card, PCI_INTERRUPT_LINE, &interrupt);
+	   pci_read_config_byte(pcidev, PCI_INTERRUPT_LINE, &interrupt);
 	   printk("Interrupt before is: %x\n", interrupt);
 	 */
 
@@ -792,7 +785,8 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	if (aux_data && aux_len) {
 		result = initialize_daqboard2000(dev, aux_data, aux_len);
 	} else {
-		dev_dbg(dev->hw_dev, "no FPGA initialization code, aborting\n");
+		dev_dbg(dev->class_dev,
+			"no FPGA initialization code, aborting\n");
 		result = -EIO;
 	}
 	if (result < 0)
@@ -801,7 +795,7 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	daqboard2000_initializeDac(dev);
 	/*
 	   Windows code does restore interrupts, but since we don't use them...
-	   pci_read_config_byte(card, PCI_INTERRUPT_LINE, &interrupt);
+	   pci_read_config_byte(pcidev, PCI_INTERRUPT_LINE, &interrupt);
 	   printk("Interrupt after is: %x\n", interrupt);
 	 */
 
@@ -838,6 +832,8 @@ out:
 
 static void daqboard2000_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
 	if (dev->irq)
@@ -847,11 +843,11 @@ static void daqboard2000_detach(struct comedi_device *dev)
 			iounmap(devpriv->daq);
 		if (devpriv->plx)
 			iounmap(devpriv->plx);
-		if (devpriv->pci_dev) {
-			if (devpriv->got_regions)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
@@ -874,7 +870,7 @@ static void __devexit daqboard2000_pci_remove(struct pci_dev *dev)
 }
 
 static DEFINE_PCI_DEVICE_TABLE(daqboard2000_pci_table) = {
-	{ PCI_DEVICE(0x1616, 0x0409) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_IOTECH, 0x0409) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, daqboard2000_pci_table);
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index 1f319435e23..874e02e4766 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -32,7 +32,7 @@
  *   [ComputerBoards] DAS08 (isa-das08), DAS08-PGM (das08-pgm),
  *   DAS08-PGH (das08-pgh), DAS08-PGL (das08-pgl), DAS08-AOH (das08-aoh),
  *   DAS08-AOL (das08-aol), DAS08-AOM (das08-aom), DAS08/JR-AO (das08/jr-ao),
- *   DAS08/JR-16-AO (das08jr-16-ao), PCI-DAS08 (das08),
+ *   DAS08/JR-16-AO (das08jr-16-ao), PCI-DAS08 (pci-das08 or das08),
  *   PC104-DAS08 (pc104-das08), DAS08/JR/16 (das08jr/16)
  * Status: works
  *
@@ -55,25 +55,15 @@
 
 #include <linux/delay.h>
 
-#include "comedi_pci.h"
 #include "8255.h"
+#include "8253.h"
 #include "das08.h"
 
 #define DRV_NAME "das08"
 
-#ifdef CONFIG_COMEDI_DAS08_ISA_MODULE
-#define CONFIG_COMEDI_DAS08_ISA
-#endif
-#ifdef CONFIG_COMEDI_DAS08_PCI_MODULE
-#define CONFIG_COMEDI_DAS08_PCI
-#endif
-#ifdef CONFIG_COMEDI_DAS08_CS_MODULE
-#define CONFIG_COMEDI_DAS08_CS
-#endif
-
-#if defined(CONFIG_COMEDI_DAS08_ISA) || defined(CONFIG_COMEDI_DAS08_PCI)
-#define DO_COMEDI_DRIVER_REGISTER
-#endif
+#define DO_COMEDI_DRIVER_REGISTER \
+	(IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) || \
+	 IS_ENABLED(CONFIG_COMEDI_DAS08_PCI))
 
 #define PCI_VENDOR_ID_COMPUTERBOARDS 0x1307
 #define PCI_DEVICE_ID_PCIDAS08 0x29
@@ -168,29 +158,6 @@
 
 /* gainlist same as _pgx_ below */
 
-static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int das08_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-static int das08_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data);
-#ifdef CONFIG_COMEDI_DAS08_ISA
-static int das08jr_di_rbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das08jr_do_wbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das08jr_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das08ao_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-#endif
-static void i8254_set_mode_low(unsigned int base, int channel,
-			       unsigned int mode);
-
 static const struct comedi_lrange range_das08_pgl = { 9, {
 							  BIP_RANGE(10),
 							  BIP_RANGE(5),
@@ -269,278 +236,13 @@ static const int *const das08_gainlists[] = {
 	das08_pgm_gainlist,
 };
 
-#ifdef DO_COMEDI_DRIVER_REGISTER
-static const struct das08_board_struct das08_boards[] = {
-#ifdef CONFIG_COMEDI_DAS08_ISA
-	{
-	 .name = "isa-das08",	/*  cio-das08.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pg_none,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .ao_nbits = 12,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 8,
-	 .i8254_offset = 4,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08-pgm",	/*  cio-das08pgx.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pgm,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08-pgh",	/*  cio-das08pgx.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pgh,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08-pgl",	/*  cio-das08pgx.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pgl,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08-aoh",	/*  cio-das08_aox.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pgh,
-	 .ai_encoding = das08_encode12,
-	 .ao = das08ao_ao_winsn,	/*  8 */
-	 .ao_nbits = 12,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0x0c,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08-aol",	/*  cio-das08_aox.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pgl,
-	 .ai_encoding = das08_encode12,
-	 .ao = das08ao_ao_winsn,	/*  8 */
-	 .ao_nbits = 12,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0x0c,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08-aom",	/*  cio-das08_aox.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pgm,
-	 .ai_encoding = das08_encode12,
-	 .ao = das08ao_ao_winsn,	/*  8 */
-	 .ao_nbits = 12,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0x0c,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08/jr-ao",	/*  cio-das08-jr-ao.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pg_none,
-	 .ai_encoding = das08_encode12,
-	 .ao = das08jr_ao_winsn,
-	 .ao_nbits = 12,
-	 .di = das08jr_di_rbits,
-	 .do_ = das08jr_do_wbits,
-	 .do_nchan = 8,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "das08jr-16-ao",	/*  cio-das08jr-16-ao.pdf */
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 16,
-	 .ai_pg = das08_pg_none,
-	 .ai_encoding = das08_encode12,
-	 .ao = das08jr_ao_winsn,
-	 .ao_nbits = 16,
-	 .di = das08jr_di_rbits,
-	 .do_ = das08jr_do_wbits,
-	 .do_nchan = 8,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0x04,
-	 .iosize = 16,		/*  unchecked */
-	 },
-	{
-	 .name = "pc104-das08",
-	 .bustype = pc104,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_pg_none,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .ao_nbits = 0,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0,
-	 .i8254_offset = 4,
-	 .iosize = 16,		/*  unchecked */
-	 },
-#if 0
-	{
-	 .name = "das08/f",
-	 },
-	{
-	 .name = "das08jr",
-	 },
-#endif
-	{
-	 .name = "das08jr/16",
-	 .bustype = isa,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 16,
-	 .ai_pg = das08_pg_none,
-	 .ai_encoding = das08_encode16,
-	 .ao = NULL,
-	 .ao_nbits = 0,
-	 .di = das08jr_di_rbits,
-	 .do_ = das08jr_do_wbits,
-	 .do_nchan = 8,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0,
-	 .iosize = 16,		/*  unchecked */
-	 },
-#if 0
-	{
-	 .name = "das48-pga",	/*  cio-das48-pga.pdf */
-	 },
-	{
-	 .name = "das08-pga-g2",	/*  a KM board */
-	 },
-#endif
-#endif /* CONFIG_COMEDI_DAS08_ISA */
-#ifdef CONFIG_COMEDI_DAS08_PCI
-	{
-	 .name = "das08",	/*  pci-das08 */
-	 .id = PCI_DEVICE_ID_PCIDAS08,
-	 .bustype = pci,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_bipolar5,
-	 .ai_encoding = das08_encode12,
-	 .ao = NULL,
-	 .ao_nbits = 0,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 4,
-	 .i8255_offset = 0,
-	 .i8254_offset = 4,
-	 .iosize = 8,
-	 },
-#endif /* CONFIG_COMEDI_DAS08_PCI */
-};
-#endif /* DO_COMEDI_DRIVER_REGISTER */
-
-#ifdef CONFIG_COMEDI_DAS08_CS
-struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS] = {
-	{
-	 .name = "pcm-das08",
-	 .id = 0x0,		/*  XXX */
-	 .bustype = pcmcia,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_bipolar5,
-	 .ai_encoding = das08_pcm_encode12,
-	 .ao = NULL,
-	 .ao_nbits = 0,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 3,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0,
-	 .iosize = 16,
-	 },
-	/*  duplicate so driver name can be used also */
-	{
-	 .name = "das08_cs",
-	 .id = 0x0,		/*  XXX */
-	 .bustype = pcmcia,
-	 .ai = das08_ai_rinsn,
-	 .ai_nbits = 12,
-	 .ai_pg = das08_bipolar5,
-	 .ai_encoding = das08_pcm_encode12,
-	 .ao = NULL,
-	 .ao_nbits = 0,
-	 .di = das08_di_rbits,
-	 .do_ = das08_do_wbits,
-	 .do_nchan = 3,
-	 .i8255_offset = 0,
-	 .i8254_offset = 0,
-	 .iosize = 16,
-	 },
-};
-#endif
-
-#ifdef CONFIG_COMEDI_DAS08_PCI
-static DEFINE_PCI_DEVICE_TABLE(das08_pci_table) = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_DEVICE_ID_PCIDAS08) },
-	{0}
-};
-
-MODULE_DEVICE_TABLE(pci, das08_pci_table);
-#endif
-
-#define devpriv ((struct das08_private_struct *)dev->private)
-#define thisboard ((const struct das08_board_struct *)dev->board_ptr)
-
 #define TIMEOUT 100000
 
 static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	struct das08_private_struct *devpriv = dev->private;
 	int i, n;
 	int chan;
 	int range;
@@ -572,7 +274,7 @@ static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		/* clear over-range bits for 16-bit boards */
 		if (thisboard->ai_nbits == 16)
 			if (inb(dev->iobase + DAS08_MSB) & 0x80)
-				printk(KERN_INFO "das08: over-range\n");
+				dev_info(dev->class_dev, "over-range\n");
 
 		/* trigger conversion */
 		outb_p(0, dev->iobase + DAS08_TRIG_12BIT);
@@ -582,7 +284,7 @@ static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 				break;
 		}
 		if (i == TIMEOUT) {
-			printk(KERN_ERR "das08: timeout\n");
+			dev_err(dev->class_dev, "timeout\n");
 			return -ETIME;
 		}
 		msb = inb(dev->iobase + DAS08_MSB);
@@ -612,12 +314,13 @@ static int das08_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	data[0] = 0;
 	data[1] = DAS08_IP(inb(dev->iobase + DAS08_STATUS));
 
-	return 2;
+	return insn->n;
 }
 
 static int das08_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	struct das08_private_struct *devpriv = dev->private;
 	int wbits;
 
 	/*  get current settings of digital output lines */
@@ -636,26 +339,25 @@ static int das08_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = wbits;
 
-	return 2;
+	return insn->n;
 }
 
-#ifdef CONFIG_COMEDI_DAS08_ISA
-static int das08jr_di_rbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int __maybe_unused
+das08jr_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
+		 struct comedi_insn *insn, unsigned int *data)
 {
 	data[0] = 0;
 	data[1] = inb(dev->iobase + DAS08JR_DIO);
 
-	return 2;
+	return insn->n;
 }
-#endif
 
-#ifdef CONFIG_COMEDI_DAS08_ISA
-static int das08jr_do_wbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int __maybe_unused
+das08jr_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
+		 struct comedi_insn *insn, unsigned int *data)
 {
+	struct das08_private_struct *devpriv = dev->private;
+
 	/*  null bits we are going to set */
 	devpriv->do_bits &= ~data[0];
 	/*  set new bit values */
@@ -664,14 +366,12 @@ static int das08jr_do_wbits(struct comedi_device *dev,
 
 	data[1] = devpriv->do_bits;
 
-	return 2;
+	return insn->n;
 }
-#endif
 
-#ifdef CONFIG_COMEDI_DAS08_ISA
-static int das08jr_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int __maybe_unused
+das08jr_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
+		 struct comedi_insn *insn, unsigned int *data)
 {
 	int n;
 	int lsb, msb;
@@ -697,7 +397,6 @@ static int das08jr_ao_winsn(struct comedi_device *dev,
 
 	return n;
 }
-#endif
 
 /*
  *
@@ -705,10 +404,9 @@ static int das08jr_ao_winsn(struct comedi_device *dev,
  * a different method to force an update.
  *
  */
-#ifdef CONFIG_COMEDI_DAS08_ISA
-static int das08ao_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int __maybe_unused
+das08ao_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
+		 struct comedi_insn *insn, unsigned int *data)
 {
 	int n;
 	int lsb, msb;
@@ -734,104 +432,25 @@ static int das08ao_ao_winsn(struct comedi_device *dev,
 
 	return n;
 }
-#endif
-
-static unsigned int i8254_read_channel_low(unsigned int base, int chan)
-{
-	unsigned int msb, lsb;
-
-	/* The following instructions must be in order.
-	   We must avoid other process reading the counter's value in the
-	   middle.
-	   The spin_lock isn't needed since ioctl calls grab the big kernel
-	   lock automatically */
-	/*spin_lock(sp); */
-	outb(chan << 6, base + I8254_CTRL);
-	base += chan;
-	lsb = inb(base);
-	msb = inb(base);
-	/*spin_unlock(sp); */
-
-	return lsb | (msb << 8);
-}
 
-static void i8254_write_channel_low(unsigned int base, int chan,
-				    unsigned int value)
-{
-	unsigned int msb, lsb;
-
-	lsb = value & 0xFF;
-	msb = value >> 8;
-
-	/* write lsb, then msb */
-	base += chan;
-	/* See comments in i8254_read_channel_low */
-	/*spin_lock(sp); */
-	outb(lsb, base);
-	outb(msb, base);
-	/*spin_unlock(sp); */
-}
-
-static unsigned int i8254_read_channel(struct i8254_struct *st, int channel)
-{
-	int chan = st->logic2phys[channel];
-
-	return i8254_read_channel_low(st->iobase, chan);
-}
-
-static void i8254_write_channel(struct i8254_struct *st, int channel,
-				unsigned int value)
-{
-	int chan = st->logic2phys[channel];
-
-	i8254_write_channel_low(st->iobase, chan, value);
-}
-
-static void i8254_initialize(struct i8254_struct *st)
+static void i8254_initialize(struct comedi_device *dev)
 {
+	struct das08_private_struct *devpriv = dev->private;
+	unsigned int mode = I8254_MODE0 | I8254_BINARY;
 	int i;
-	for (i = 0; i < 3; ++i)
-		i8254_set_mode_low(st->iobase, i, st->mode[i]);
-}
-
-static void i8254_set_mode_low(unsigned int base, int channel,
-			       unsigned int mode)
-{
-	outb((channel << 6) | 0x30 | (mode & 0x0F), base + I8254_CTRL);
-}
-
-static void i8254_set_mode(struct i8254_struct *st, int channel,
-			   unsigned int mode)
-{
-	int chan = st->logic2phys[channel];
 
-	st->mode[chan] = mode;
-	return i8254_set_mode_low(st->iobase, chan, mode);
-}
-
-static unsigned int i8254_read_status_low(unsigned int base, int channel)
-{
-	outb(0xE0 | (2 << channel), base + I8254_CTRL);
-	return inb(base + channel);
-}
-
-static unsigned int i8254_read_status(struct i8254_struct *st, int channel)
-{
-	int chan = st->logic2phys[channel];
-
-	return i8254_read_status_low(st->iobase, chan);
+	for (i = 0; i < 3; ++i)
+		i8254_set_mode(devpriv->i8254_iobase, 0, i, mode);
 }
 
 static int das08_counter_read(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
+	struct das08_private_struct *devpriv = dev->private;
 	int chan = insn->chanspec;
 
-	/* printk("Reading counter channel %d ",chan); */
-	data[0] = i8254_read_channel(&devpriv->i8254, chan);
-	/* printk("=> 0x%08X\n",data[0]); */
-
+	data[0] = i8254_read(devpriv->i8254_iobase, 0, chan);
 	return 1;
 }
 
@@ -839,11 +458,10 @@ static int das08_counter_write(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct das08_private_struct *devpriv = dev->private;
 	int chan = insn->chanspec;
 
-	/* printk("Writing counter channel %d with 0x%04X\n",chan,data[0]); */
-	i8254_write_channel(&devpriv->i8254, chan, data[0]);
-
+	i8254_write(devpriv->i8254_iobase, 0, chan, data[0]);
 	return 1;
 }
 
@@ -851,6 +469,7 @@ static int das08_counter_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
+	struct das08_private_struct *devpriv = dev->private;
 	int chan = insn->chanspec;
 
 	if (insn->n != 2)
@@ -858,10 +477,10 @@ static int das08_counter_config(struct comedi_device *dev,
 
 	switch (data[0]) {
 	case INSN_CONFIG_SET_COUNTER_MODE:
-		i8254_set_mode(&devpriv->i8254, chan, data[1]);
+		i8254_set_mode(devpriv->i8254_iobase, 0, chan, data[1]);
 		break;
 	case INSN_CONFIG_8254_READ_STATUS:
-		data[1] = i8254_read_status(&devpriv->i8254, chan);
+		data[1] = i8254_status(devpriv->i8254_iobase, 0, chan);
 		break;
 	default:
 		return -EINVAL;
@@ -870,39 +489,280 @@ static int das08_counter_config(struct comedi_device *dev,
 	return 2;
 }
 
-#ifdef DO_COMEDI_DRIVER_REGISTER
-static int das08_attach(struct comedi_device *dev, struct comedi_devconfig *it);
+#if DO_COMEDI_DRIVER_REGISTER
+static const struct das08_board_struct das08_boards[] = {
+#if IS_ENABLED(CONFIG_COMEDI_DAS08_ISA)
+	{
+		.name = "isa-das08",	/*  cio-das08.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pg_none,
+		.ai_encoding = das08_encode12,
+		.ao = NULL,
+		.ao_nbits = 12,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 8,
+		.i8254_offset = 4,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08-pgm",	/*  cio-das08pgx.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pgm,
+		.ai_encoding = das08_encode12,
+		.ao = NULL,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08-pgh",	/*  cio-das08pgx.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pgh,
+		.ai_encoding = das08_encode12,
+		.ao = NULL,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08-pgl",	/*  cio-das08pgx.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pgl,
+		.ai_encoding = das08_encode12,
+		.ao = NULL,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08-aoh",	/*  cio-das08_aox.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pgh,
+		.ai_encoding = das08_encode12,
+		.ao = das08ao_ao_winsn,	/*  8 */
+		.ao_nbits = 12,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0x0c,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08-aol",	/*  cio-das08_aox.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pgl,
+		.ai_encoding = das08_encode12,
+		.ao = das08ao_ao_winsn,	/*  8 */
+		.ao_nbits = 12,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0x0c,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08-aom",	/*  cio-das08_aox.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pgm,
+		.ai_encoding = das08_encode12,
+		.ao = das08ao_ao_winsn,	/*  8 */
+		.ao_nbits = 12,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0x0c,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08/jr-ao",	/*  cio-das08-jr-ao.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pg_none,
+		.ai_encoding = das08_encode12,
+		.ao = das08jr_ao_winsn,
+		.ao_nbits = 12,
+		.di = das08jr_di_rbits,
+		.do_ = das08jr_do_wbits,
+		.do_nchan = 8,
+		.i8255_offset = 0,
+		.i8254_offset = 0,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "das08jr-16-ao",	/*  cio-das08jr-16-ao.pdf */
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 16,
+		.ai_pg = das08_pg_none,
+		.ai_encoding = das08_encode12,
+		.ao = das08jr_ao_winsn,
+		.ao_nbits = 16,
+		.di = das08jr_di_rbits,
+		.do_ = das08jr_do_wbits,
+		.do_nchan = 8,
+		.i8255_offset = 0,
+		.i8254_offset = 0x04,
+		.iosize = 16,		/*  unchecked */
+	},
+	{
+		.name = "pc104-das08",
+		.bustype = pc104,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_pg_none,
+		.ai_encoding = das08_encode12,
+		.ao = NULL,
+		.ao_nbits = 0,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0,
+		.i8254_offset = 4,
+		.iosize = 16,		/*  unchecked */
+	},
+#if 0
+	{
+		.name = "das08/f",
+	},
+	{
+		.name = "das08jr",
+	},
+#endif
+	{
+		.name = "das08jr/16",
+		.bustype = isa,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 16,
+		.ai_pg = das08_pg_none,
+		.ai_encoding = das08_encode16,
+		.ao = NULL,
+		.ao_nbits = 0,
+		.di = das08jr_di_rbits,
+		.do_ = das08jr_do_wbits,
+		.do_nchan = 8,
+		.i8255_offset = 0,
+		.i8254_offset = 0,
+		.iosize = 16,		/*  unchecked */
+	},
+#if 0
+	{
+		.name = "das48-pga",	/*  cio-das48-pga.pdf */
+	},
+	{
+		.name = "das08-pga-g2",	/*  a KM board */
+	},
+#endif
+#endif /* IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) */
+#if IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+	{
+		.name = "pci-das08",	/*  pci-das08 */
+		.id = PCI_DEVICE_ID_PCIDAS08,
+		.bustype = pci,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_bipolar5,
+		.ai_encoding = das08_encode12,
+		.ao = NULL,
+		.ao_nbits = 0,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 4,
+		.i8255_offset = 0,
+		.i8254_offset = 4,
+		.iosize = 8,
+	},
+	{ /* wildcard entry matches any supported PCI device */
+		.name = DRV_NAME,
+		.id = PCI_ANY_ID,
+		.bustype = pci,
+	},
+#endif /* IS_ENABLED(CONFIG_COMEDI_DAS08_PCI) */
+};
+#endif /* DO_COMEDI_DRIVER_REGISTER */
 
-static struct comedi_driver driver_das08 = {
-	.driver_name = DRV_NAME,
-	.module = THIS_MODULE,
-	.attach = das08_attach,
-	.detach = das08_common_detach,
-	.board_name = &das08_boards[0].name,
-	.num_names = sizeof(das08_boards) / sizeof(struct das08_board_struct),
-	.offset = sizeof(struct das08_board_struct),
+#if IS_ENABLED(CONFIG_COMEDI_DAS08_CS)
+struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS] = {
+	{
+		.name = "pcm-das08",
+		.id = 0x0,		/*  XXX */
+		.bustype = pcmcia,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_bipolar5,
+		.ai_encoding = das08_pcm_encode12,
+		.ao = NULL,
+		.ao_nbits = 0,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 3,
+		.i8255_offset = 0,
+		.i8254_offset = 0,
+		.iosize = 16,
+	},
+	/*  duplicate so driver name can be used also */
+	{
+		.name = "das08_cs",
+		.id = 0x0,		/*  XXX */
+		.bustype = pcmcia,
+		.ai = das08_ai_rinsn,
+		.ai_nbits = 12,
+		.ai_pg = das08_bipolar5,
+		.ai_encoding = das08_pcm_encode12,
+		.ao = NULL,
+		.ao_nbits = 0,
+		.di = das08_di_rbits,
+		.do_ = das08_do_wbits,
+		.do_nchan = 3,
+		.i8255_offset = 0,
+		.i8254_offset = 0,
+		.iosize = 16,
+	},
 };
+EXPORT_SYMBOL_GPL(das08_cs_boards);
 #endif
 
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 {
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	struct das08_private_struct *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int ret;
 
-	/*  allocate ioports for non-pcmcia, non-pci boards */
-	if ((thisboard->bustype != pcmcia) && (thisboard->bustype != pci)) {
-		printk(KERN_INFO " iobase 0x%lx\n", iobase);
-		if (!request_region(iobase, thisboard->iosize, DRV_NAME)) {
-			printk(KERN_ERR " I/O port conflict\n");
-			return -EIO;
-		}
-	}
 	dev->iobase = iobase;
 
 	dev->board_name = thisboard->name;
 
-	ret = alloc_subdevices(dev, 6);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 6);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
@@ -984,16 +844,9 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->insn_read = das08_counter_read;
 		s->insn_write = das08_counter_write;
 		s->insn_config = das08_counter_config;
-		/* Set-up the 8254 structure */
-		devpriv->i8254.channels = 3;
-		devpriv->i8254.logic2phys[0] = 0;
-		devpriv->i8254.logic2phys[1] = 1;
-		devpriv->i8254.logic2phys[2] = 2;
-		devpriv->i8254.iobase = iobase + thisboard->i8254_offset;
-		devpriv->i8254.mode[0] =
-		    devpriv->i8254.mode[1] =
-		    devpriv->i8254.mode[2] = I8254_MODE0 | I8254_BINARY;
-		i8254_initialize(&devpriv->i8254);
+
+		devpriv->i8254_iobase = iobase + thisboard->i8254_offset;
+		i8254_initialize(dev);
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
@@ -1002,155 +855,246 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 }
 EXPORT_SYMBOL_GPL(das08_common_attach);
 
-#ifdef DO_COMEDI_DRIVER_REGISTER
-static int das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+static int das08_pci_attach_common(struct comedi_device *dev,
+				   struct pci_dev *pdev)
 {
-	int ret;
 	unsigned long iobase;
-#ifdef CONFIG_COMEDI_DAS08_PCI
-	unsigned long pci_iobase = 0;
-	struct pci_dev *pdev = NULL;
+	unsigned long pci_iobase;
+	struct das08_private_struct *devpriv = dev->private;
+
+	if (!IS_ENABLED(CONFIG_COMEDI_DAS08_PCI))
+		return -EINVAL;
+
+	devpriv->pdev = pdev;
+	/*  enable PCI device and reserve I/O spaces */
+	if (comedi_pci_enable(pdev, dev->driver->driver_name)) {
+		dev_err(dev->class_dev,
+			"Error enabling PCI device and requesting regions\n");
+		return -EIO;
+	}
+	/*  read base addresses */
+	pci_iobase = pci_resource_start(pdev, 1);
+	iobase = pci_resource_start(pdev, 2);
+	dev_info(dev->class_dev, "pcibase 0x%lx  iobase 0x%lx\n",
+		 pci_iobase, iobase);
+	devpriv->pci_iobase = pci_iobase;
+#if 0
+	/* We could enable pci-das08's interrupt here to make it possible
+	* to do timed input in this driver, but there is little point since
+	* conversions would have to be started by the interrupt handler
+	* so you might as well use comedi_rt_timer to emulate commands
+	*/
+	/* set source of interrupt trigger to counter2 output */
+	outb(CNTRL_INTR | CNTRL_DIR, pci_iobase + CNTRL);
+	/* Enable local interrupt 1 and pci interrupt */
+	outw(INTR1_ENABLE | PCI_INTR_ENABLE, pci_iobase + INTCSR);
+#endif
+	return das08_common_attach(dev, iobase);
+}
+
+static const struct das08_board_struct *
+das08_find_pci_board(struct pci_dev *pdev)
+{
+#if DO_COMEDI_DRIVER_REGISTER
+	unsigned int i;
+	for (i = 0; i < ARRAY_SIZE(das08_boards); i++)
+		if (das08_boards[i].bustype == pci &&
+		    pdev->device == das08_boards[i].id)
+			return &das08_boards[i];
 #endif
+	return NULL;
+}
 
+/* only called in the PCI probe path, via comedi_pci_auto_config() */
+static int __devinit __maybe_unused
+das08_attach_pci(struct comedi_device *dev, struct pci_dev *pdev)
+{
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_COMEDI_DAS08_PCI))
+		return -EINVAL;
 	ret = alloc_private(dev, sizeof(struct das08_private_struct));
 	if (ret < 0)
 		return ret;
+	dev_info(dev->class_dev, "attach pci %s\n", pci_name(pdev));
+	dev->board_ptr = das08_find_pci_board(pdev);
+	if (dev->board_ptr == NULL) {
+		dev_err(dev->class_dev, "BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+	return das08_pci_attach_common(dev, pdev);
+}
 
-	printk(KERN_INFO "comedi%d: das08: ", dev->minor);
-#ifdef CONFIG_COMEDI_DAS08_PCI
-	/*  deal with a pci board */
-	if (thisboard->bustype == pci) {
-		if (it->options[0] || it->options[1]) {
-			printk("bus %i slot %i ",
-			       it->options[0], it->options[1]);
-		}
-		printk("\n");
-		/*  find card */
-		for_each_pci_dev(pdev) {
-			if (pdev->vendor == PCI_VENDOR_ID_COMPUTERBOARDS
-			    && pdev->device == PCI_DEVICE_ID_PCIDAS08) {
-				if (it->options[0] || it->options[1]) {
-					if (pdev->bus->number == it->options[0]
-					    && PCI_SLOT(pdev->devfn) ==
-					    it->options[1]) {
-						break;
-					}
-				} else {
-					break;
-				}
-			}
+static struct pci_dev *das08_find_pci(struct comedi_device *dev,
+				      int bus, int slot)
+{
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	struct pci_dev *pdev;
+	unsigned int matchid;
+
+	if (bus || slot)
+		dev_dbg(dev->class_dev, "Looking for %s at PCI %02X:%02X\n",
+			thisboard->name, bus, slot);
+	else
+		dev_dbg(dev->class_dev, "Looking for %s on PCI buses\n",
+			thisboard->name);
+
+	matchid = thisboard->id;
+	pdev = NULL;
+	for_each_pci_dev(pdev) {
+		if ((bus || slot) &&
+		    (bus != pdev->bus->number || slot != PCI_SLOT(pdev->devfn)))
+			continue;
+		if (pdev->vendor != PCI_VENDOR_ID_COMPUTERBOARDS)
+			continue;
+		if (matchid == PCI_ANY_ID) {
+			/* wildcard board matches any supported PCI board */
+			const struct das08_board_struct *foundboard;
+			foundboard = das08_find_pci_board(pdev);
+			if (foundboard == NULL)
+				continue;
+			/* replace wildcard board_ptr */
+			dev->board_ptr = thisboard = foundboard;
+		} else {
+			/* match specific PCI board */
+			if (pdev->device != matchid)
+				continue;
 		}
-		if (!pdev) {
-			printk(KERN_ERR "No pci das08 cards found\n");
+		/* found a match */
+		dev_info(dev->class_dev, "Found %s at PCI %s\n",
+			 thisboard->name, pci_name(pdev));
+		return pdev;
+	}
+	/* no match found */
+	if (bus || slot)
+		dev_err(dev->class_dev,
+			"No %s cards found at PCI %02X:%02X\n",
+			thisboard->name, bus, slot);
+	else
+		dev_err(dev->class_dev, "No %s cards found on PCI buses\n",
+			thisboard->name);
+	return NULL;
+}
+
+static int __maybe_unused
+das08_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	struct das08_private_struct *devpriv;
+	int ret;
+	unsigned long iobase;
+
+	ret = alloc_private(dev, sizeof(struct das08_private_struct));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
+
+	dev_info(dev->class_dev, "attach\n");
+	if (IS_ENABLED(CONFIG_COMEDI_DAS08_PCI) && thisboard->bustype == pci) {
+		struct pci_dev *pdev;
+		pdev = das08_find_pci(dev, it->options[0], it->options[1]);
+		if (pdev == NULL)
 			return -EIO;
-		}
-		devpriv->pdev = pdev;
-		/*  enable PCI device and reserve I/O spaces */
-		if (comedi_pci_enable(pdev, DRV_NAME)) {
-			printk(KERN_ERR " Error enabling PCI device and "
-						"requesting regions\n");
+		return das08_pci_attach_common(dev, pdev);
+	} else if (IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) &&
+		   (thisboard->bustype == isa || thisboard->bustype == pc104)) {
+		iobase = it->options[0];
+		dev_info(dev->class_dev, "iobase 0x%lx\n", iobase);
+		if (!request_region(iobase, thisboard->iosize, DRV_NAME)) {
+			dev_err(dev->class_dev, "I/O port conflict\n");
 			return -EIO;
 		}
-		/*  read base addresses */
-		pci_iobase = pci_resource_start(pdev, 1);
-		iobase = pci_resource_start(pdev, 2);
-		printk(KERN_INFO "pcibase 0x%lx  iobase 0x%lx\n",
-							pci_iobase, iobase);
-		devpriv->pci_iobase = pci_iobase;
-#if 0
-/* We could enable to pci-das08's interrupt here to make it possible
- * to do timed input in this driver, but there is little point since
- * conversions would have to be started by the interrupt handler
- * so you might as well use comedi_rt_timer to emulate commands
- */
-		/* set source of interrupt trigger to counter2 output */
-		outb(CNTRL_INTR | CNTRL_DIR, pci_iobase + CNTRL);
-		/* Enable local interrupt 1 and pci interrupt */
-		outw(INTR1_ENABLE | PCI_INTR_ENABLE, pci_iobase + INTCSR);
-#endif
+		return das08_common_attach(dev, iobase);
 	} else
-#endif /* CONFIG_COMEDI_DAS08_PCI */
-	{
-		iobase = it->options[0];
-	}
-	printk(KERN_INFO "\n");
-
-	return das08_common_attach(dev, iobase);
+		return -EIO;
 }
-#endif /* DO_COMEDI_DRIVER_REGISTER */
 
 void das08_common_detach(struct comedi_device *dev)
 {
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 4);
-	if ((thisboard->bustype != pcmcia) && (thisboard->bustype != pci)) {
+}
+EXPORT_SYMBOL_GPL(das08_common_detach);
+
+static void __maybe_unused das08_detach(struct comedi_device *dev)
+{
+	const struct das08_board_struct *thisboard = comedi_board(dev);
+	struct das08_private_struct *devpriv = dev->private;
+
+	das08_common_detach(dev);
+	if (IS_ENABLED(CONFIG_COMEDI_DAS08_ISA) &&
+	    (thisboard->bustype == isa || thisboard->bustype == pc104)) {
 		if (dev->iobase)
 			release_region(dev->iobase, thisboard->iosize);
-	}
-#ifdef CONFIG_COMEDI_DAS08_PCI
-	if (devpriv) {
-		if (devpriv->pdev) {
+	} else if (IS_ENABLED(CONFIG_COMEDI_DAS08_PCI) &&
+		   thisboard->bustype == pci) {
+		if (devpriv && devpriv->pdev) {
 			if (devpriv->pci_iobase)
 				comedi_pci_disable(devpriv->pdev);
-
 			pci_dev_put(devpriv->pdev);
 		}
 	}
-#endif
 }
-EXPORT_SYMBOL_GPL(das08_common_detach);
 
-#ifdef CONFIG_COMEDI_DAS08_PCI
-static int __devinit driver_das08_pci_probe(struct pci_dev *dev,
+#if DO_COMEDI_DRIVER_REGISTER
+static struct comedi_driver das08_driver = {
+	.driver_name = DRV_NAME,
+	.module = THIS_MODULE,
+	.attach = das08_attach,
+	.attach_pci = das08_attach_pci,
+	.detach = das08_detach,
+	.board_name = &das08_boards[0].name,
+	.num_names = sizeof(das08_boards) / sizeof(struct das08_board_struct),
+	.offset = sizeof(struct das08_board_struct),
+};
+#endif
+
+#if IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+static DEFINE_PCI_DEVICE_TABLE(das08_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_DEVICE_ID_PCIDAS08) },
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, das08_pci_table);
+
+static int __devinit das08_pci_probe(struct pci_dev *dev,
 					    const struct pci_device_id *ent)
 {
-	return comedi_pci_auto_config(dev, &driver_das08);
+	return comedi_pci_auto_config(dev, &das08_driver);
 }
 
-static void __devexit driver_das08_pci_remove(struct pci_dev *dev)
+static void __devexit das08_pci_remove(struct pci_dev *dev)
 {
 	comedi_pci_auto_unconfig(dev);
 }
 
-static struct pci_driver driver_das08_pci_driver = {
+static struct pci_driver das08_pci_driver = {
 	.id_table = das08_pci_table,
-	.probe = &driver_das08_pci_probe,
-	.remove = __devexit_p(&driver_das08_pci_remove)
+	.name =  DRV_NAME,
+	.probe = &das08_pci_probe,
+	.remove = __devexit_p(&das08_pci_remove)
 };
 #endif /* CONFIG_COMEDI_DAS08_PCI */
 
-static int __init driver_das08_init_module(void)
-{
-	int retval = 0;
-
-#ifdef DO_COMEDI_DRIVER_REGISTER
-	retval = comedi_driver_register(&driver_das08);
-	if (retval < 0)
-		return retval;
-#endif
-#ifdef CONFIG_COMEDI_DAS08_PCI
-	driver_das08_pci_driver.name = (char *)driver_das08.driver_name;
-	retval = pci_register_driver(&driver_das08_pci_driver);
+#if DO_COMEDI_DRIVER_REGISTER
+#if IS_ENABLED(CONFIG_COMEDI_DAS08_PCI)
+module_comedi_pci_driver(das08_driver, das08_pci_driver);
+#else
+module_comedi_driver(das08_driver);
 #endif
-	return retval;
+#else /* DO_COMEDI_DRIVER_REGISTER */
+static int __init das08_init(void)
+{
+	return 0;
 }
 
-static void __exit driver_das08_cleanup_module(void)
+static void __exit das08_exit(void)
 {
-#ifdef CONFIG_COMEDI_DAS08_PCI
-	pci_unregister_driver(&driver_das08_pci_driver);
-#endif
-#ifdef DO_COMEDI_DRIVER_REGISTER
-	comedi_driver_unregister(&driver_das08);
-#endif
 }
 
-module_init(driver_das08_init_module);
-module_exit(driver_das08_cleanup_module);
-
-#ifdef CONFIG_COMEDI_DAS08_CS
-EXPORT_SYMBOL_GPL(das08_cs_boards);
-#endif
+module_init(das08_init);
+module_exit(das08_exit);
+#endif /* DO_COMEDI_DRIVER_REGISTER */
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index 0b92f24b261..27b6d4ec903 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -49,25 +49,13 @@ struct das08_board_struct {
 	unsigned int iosize;	/*  number of ioports used */
 };
 
-struct i8254_struct {
-	int channels;		/*  available channels. Some could be used internally. */
-	int logic2phys[3];	/*  to know which physical channel is. */
-	int mode[3];		/*  the index is the real counter. */
-	unsigned int iobase;
-};
-
-#define I8254_CNT0 0
-#define I8254_CNT1 1
-#define I8254_CNT2 2
-#define I8254_CTRL 3
-
 struct das08_private_struct {
 	unsigned int do_mux_bits;	/*  bits for do/mux register on boards without separate do register */
 	unsigned int do_bits;	/*  bits for do register on boards with register dedicated to digital out only */
 	const unsigned int *pg_gainlist;
 	struct pci_dev *pdev;	/*  struct for pci-das08 */
 	unsigned int pci_iobase;	/*  additional base address for pci-das08 */
-	struct i8254_struct i8254;
+	unsigned int i8254_iobase;
 };
 
 #define NUM_DAS08_CS_BOARDS 2
diff --git a/drivers/staging/comedi/drivers/das08_cs.c b/drivers/staging/comedi/drivers/das08_cs.c
index 4ad398aad72..f5700de7b6c 100644
--- a/drivers/staging/comedi/drivers/das08_cs.c
+++ b/drivers/staging/comedi/drivers/das08_cs.c
@@ -20,6 +20,13 @@
     along with this program; if not, write to the Free Software
     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 
+    PCMCIA support code for this driver is adapted from the dummy_cs.c
+    driver of the Linux PCMCIA Card Services package.
+
+    The initial developer of the original code is David A. Hinds
+    <dahinds@users.sourceforge.net>.  Portions created by David A. Hinds
+    are Copyright (C) 1999 David A. Hinds.  All Rights Reserved.
+
 *****************************************************************
 
 */
@@ -53,24 +60,10 @@ Command support does not exist, but could be added for this board.
 
 static struct pcmcia_device *cur_dev;
 
-#define thisboard ((const struct das08_board_struct *)dev->board_ptr)
-
-static int das08_cs_attach(struct comedi_device *dev,
-			   struct comedi_devconfig *it);
-
-static struct comedi_driver driver_das08_cs = {
-	.driver_name = "das08_cs",
-	.module = THIS_MODULE,
-	.attach = das08_cs_attach,
-	.detach = das08_common_detach,
-	.board_name = &das08_cs_boards[0].name,
-	.num_names = ARRAY_SIZE(das08_cs_boards),
-	.offset = sizeof(struct das08_board_struct),
-};
-
 static int das08_cs_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
+	const struct das08_board_struct *thisboard = comedi_board(dev);
 	int ret;
 	unsigned long iobase;
 	struct pcmcia_device *link = cur_dev;	/*  XXX hack */
@@ -79,82 +72,34 @@ static int das08_cs_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 
-	dev_info(dev->hw_dev, "comedi%d: das08_cs:\n", dev->minor);
+	dev_info(dev->class_dev, "das08_cs: attach\n");
 	/*  deal with a pci board */
 
 	if (thisboard->bustype == pcmcia) {
 		if (link == NULL) {
-			dev_err(dev->hw_dev, "no pcmcia cards found\n");
+			dev_err(dev->class_dev, "no pcmcia cards found\n");
 			return -EIO;
 		}
 		iobase = link->resource[0]->start;
 	} else {
-		dev_err(dev->hw_dev, "bug! board does not have PCMCIA bustype\n");
+		dev_err(dev->class_dev,
+			"bug! board does not have PCMCIA bustype\n");
 		return -EINVAL;
 	}
 
 	return das08_common_attach(dev, iobase);
 }
 
-/*======================================================================
-
-    The following pcmcia code for the pcm-das08 is adapted from the
-    dummy_cs.c driver of the Linux PCMCIA Card Services package.
-
-    The initial developer of the original code is David A. Hinds
-    <dahinds@users.sourceforge.net>.  Portions created by David A. Hinds
-    are Copyright (C) 1999 David A. Hinds.  All Rights Reserved.
-
-======================================================================*/
-
-static void das08_pcmcia_config(struct pcmcia_device *link);
-static void das08_pcmcia_release(struct pcmcia_device *link);
-static int das08_pcmcia_suspend(struct pcmcia_device *p_dev);
-static int das08_pcmcia_resume(struct pcmcia_device *p_dev);
-
-static int das08_pcmcia_attach(struct pcmcia_device *);
-static void das08_pcmcia_detach(struct pcmcia_device *);
-
-struct local_info_t {
-	struct pcmcia_device *link;
-	int stop;
-	struct bus_operations *bus;
+static struct comedi_driver driver_das08_cs = {
+	.driver_name	= "das08_cs",
+	.module		= THIS_MODULE,
+	.attach		= das08_cs_attach,
+	.detach		= das08_common_detach,
+	.board_name	= &das08_cs_boards[0].name,
+	.num_names	= ARRAY_SIZE(das08_cs_boards),
+	.offset		= sizeof(struct das08_board_struct),
 };
 
-static int das08_pcmcia_attach(struct pcmcia_device *link)
-{
-	struct local_info_t *local;
-
-	dev_dbg(&link->dev, "das08_pcmcia_attach()\n");
-
-	/* Allocate space for private device-specific data */
-	local = kzalloc(sizeof(struct local_info_t), GFP_KERNEL);
-	if (!local)
-		return -ENOMEM;
-	local->link = link;
-	link->priv = local;
-
-	cur_dev = link;
-
-	das08_pcmcia_config(link);
-
-	return 0;
-}				/* das08_pcmcia_attach */
-
-static void das08_pcmcia_detach(struct pcmcia_device *link)
-{
-
-	dev_dbg(&link->dev, "das08_pcmcia_detach\n");
-
-	((struct local_info_t *)link->priv)->stop = 1;
-	das08_pcmcia_release(link);
-
-	/* This points to the parent struct local_info_t struct */
-	kfree(link->priv);
-
-}				/* das08_pcmcia_detach */
-
-
 static int das08_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
 {
@@ -164,19 +109,15 @@ static int das08_pcmcia_config_loop(struct pcmcia_device *p_dev,
 	return pcmcia_request_io(p_dev);
 }
 
-static void das08_pcmcia_config(struct pcmcia_device *link)
+static int das08_pcmcia_attach(struct pcmcia_device *link)
 {
 	int ret;
 
-	dev_dbg(&link->dev, "das08_pcmcia_config\n");
-
 	link->config_flags |= CONF_ENABLE_IRQ | CONF_AUTO_SET_IO;
 
 	ret = pcmcia_loop_config(link, das08_pcmcia_config_loop, NULL);
-	if (ret) {
-		dev_warn(&link->dev, "no configuration found\n");
+	if (ret)
 		goto failed;
-	}
 
 	if (!link->irq)
 		goto failed;
@@ -185,87 +126,61 @@ static void das08_pcmcia_config(struct pcmcia_device *link)
 	if (ret)
 		goto failed;
 
-	return;
+	cur_dev = link;
+	return 0;
 
 failed:
-	das08_pcmcia_release(link);
-
-}				/* das08_pcmcia_config */
-
-static void das08_pcmcia_release(struct pcmcia_device *link)
-{
-	dev_dbg(&link->dev, "das08_pcmcia_release\n");
 	pcmcia_disable_device(link);
-}				/* das08_pcmcia_release */
-
-static int das08_pcmcia_suspend(struct pcmcia_device *link)
-{
-	struct local_info_t *local = link->priv;
-	/* Mark the device as stopped, to block IO until later */
-	local->stop = 1;
-
-	return 0;
-}				/* das08_pcmcia_suspend */
+	return ret;
+}
 
-static int das08_pcmcia_resume(struct pcmcia_device *link)
+static void das08_pcmcia_detach(struct pcmcia_device *link)
 {
-	struct local_info_t *local = link->priv;
-
-	local->stop = 0;
-	return 0;
-}				/* das08_pcmcia_resume */
-
-/*====================================================================*/
+	pcmcia_disable_device(link);
+	cur_dev = NULL;
+}
 
 static const struct pcmcia_device_id das08_cs_id_table[] = {
 	PCMCIA_DEVICE_MANF_CARD(0x01c5, 0x4001),
 	PCMCIA_DEVICE_NULL
 };
-
 MODULE_DEVICE_TABLE(pcmcia, das08_cs_id_table);
-MODULE_AUTHOR("David A. Schleef <ds@schleef.org>, "
-	      "Frank Mori Hess <fmhess@users.sourceforge.net>");
-MODULE_DESCRIPTION("Comedi driver for ComputerBoards DAS-08 PCMCIA boards");
-MODULE_LICENSE("GPL");
 
-struct pcmcia_driver das08_cs_driver = {
-	.probe = das08_pcmcia_attach,
-	.remove = das08_pcmcia_detach,
-	.suspend = das08_pcmcia_suspend,
-	.resume = das08_pcmcia_resume,
-	.id_table = das08_cs_id_table,
-	.owner = THIS_MODULE,
-	.name = "pcm-das08",
+static struct pcmcia_driver das08_cs_driver = {
+	.name		= "pcm-das08",
+	.owner		= THIS_MODULE,
+	.probe		= das08_pcmcia_attach,
+	.remove		= das08_pcmcia_detach,
+	.id_table	= das08_cs_id_table,
 };
 
-static int __init init_das08_pcmcia_cs(void)
-{
-	pcmcia_register_driver(&das08_cs_driver);
-	return 0;
-}
-
-static void __exit exit_das08_pcmcia_cs(void)
-{
-	pr_debug("das08_pcmcia_cs: unloading\n");
-	pcmcia_unregister_driver(&das08_cs_driver);
-}
-
 static int __init das08_cs_init_module(void)
 {
 	int ret;
 
-	ret = init_das08_pcmcia_cs();
+	ret = comedi_driver_register(&driver_das08_cs);
 	if (ret < 0)
 		return ret;
 
-	return comedi_driver_register(&driver_das08_cs);
+	ret = pcmcia_register_driver(&das08_cs_driver);
+	if (ret < 0) {
+		comedi_driver_unregister(&driver_das08_cs);
+		return ret;
+	}
+
+	return 0;
+
 }
+module_init(das08_cs_init_module);
 
 static void __exit das08_cs_exit_module(void)
 {
-	exit_das08_pcmcia_cs();
+	pcmcia_unregister_driver(&das08_cs_driver);
 	comedi_driver_unregister(&driver_das08_cs);
 }
-
-module_init(das08_cs_init_module);
 module_exit(das08_cs_exit_module);
+
+MODULE_AUTHOR("David A. Schleef <ds@schleef.org>, "
+	      "Frank Mori Hess <fmhess@users.sourceforge.net>");
+MODULE_DESCRIPTION("Comedi driver for ComputerBoards DAS-08 PCMCIA boards");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 998444c1ba3..895cc7783c9 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -393,11 +393,11 @@ struct das16_private_struct {
 	volatile short timer_mode;	/*  true if using timer mode */
 };
 #define devpriv ((struct das16_private_struct *)(dev->private))
-#define thisboard ((struct das16_board *)(dev->board_ptr))
 
 static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_cmd *cmd)
 {
+	const struct das16_board *board = comedi_board(dev);
 	int err = 0, tmp;
 	int gain, start_chan, i;
 	int mask;
@@ -411,7 +411,7 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 	tmp = cmd->scan_begin_src;
 	mask = TRIG_FOLLOW;
 	/*  if board supports burst mode */
-	if (thisboard->size > 0x400)
+	if (board->size > 0x400)
 		mask |= TRIG_TIMER | TRIG_EXT;
 	cmd->scan_begin_src &= mask;
 	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
@@ -420,7 +420,7 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 	tmp = cmd->convert_src;
 	mask = TRIG_TIMER | TRIG_EXT;
 	/*  if board supports burst mode */
-	if (thisboard->size > 0x400)
+	if (board->size > 0x400)
 		mask |= TRIG_NOW;
 	cmd->convert_src &= mask;
 	if (!cmd->convert_src || tmp != cmd->convert_src)
@@ -483,15 +483,15 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 	/*  check against maximum frequency */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		if (cmd->scan_begin_arg <
-		    thisboard->ai_speed * cmd->chanlist_len) {
+		    board->ai_speed * cmd->chanlist_len) {
 			cmd->scan_begin_arg =
-			    thisboard->ai_speed * cmd->chanlist_len;
+			    board->ai_speed * cmd->chanlist_len;
 			err++;
 		}
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < thisboard->ai_speed) {
-			cmd->convert_arg = thisboard->ai_speed;
+		if (cmd->convert_arg < board->ai_speed) {
+			cmd->convert_arg = board->ai_speed;
 			err++;
 		}
 	}
@@ -614,6 +614,7 @@ static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
 
 static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct das16_board *board = comedi_board(dev);
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int byte;
@@ -637,7 +638,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	    cmd->stop_arg * cmd->chanlist_len * sizeof(uint16_t);
 
 	/*  disable conversions for das1600 mode */
-	if (thisboard->size > 0x400)
+	if (board->size > 0x400)
 		outb(DAS1600_CONV_DISABLE, dev->iobase + DAS1600_CONV);
 
 	/*  set scan limits */
@@ -648,9 +649,9 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* set gain (this is also burst rate register but according to
 	 * computer boards manual, burst rate does nothing, even on
 	 * keithley cards) */
-	if (thisboard->ai_pg != das16_pg_none) {
+	if (board->ai_pg != das16_pg_none) {
 		range = CR_RANGE(cmd->chanlist[0]);
-		outb((das16_gainlists[thisboard->ai_pg])[range],
+		outb((das16_gainlists[board->ai_pg])[range],
 		     dev->iobase + DAS16_GAIN);
 	}
 
@@ -663,7 +664,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* enable counters */
 	byte = 0;
 	/* Enable burst mode if appropriate. */
-	if (thisboard->size > 0x400) {
+	if (board->size > 0x400) {
 		if (cmd->convert_src == TRIG_NOW) {
 			outb(DAS1600_BURST_VAL, dev->iobase + DAS1600_BURST);
 			/*  set burst length */
@@ -710,7 +711,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	outb(devpriv->control_state, dev->iobase + DAS16_CONTROL);
 
 	/* Enable conversions if using das1600 mode */
-	if (thisboard->size > 0x400)
+	if (board->size > 0x400)
 		outb(0, dev->iobase + DAS1600_CONV);
 
 
@@ -719,6 +720,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static int das16_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct das16_board *board = comedi_board(dev);
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
@@ -735,7 +737,7 @@ static int das16_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* disable burst mode */
-	if (thisboard->size > 0x400)
+	if (board->size > 0x400)
 		outb(0, dev->iobase + DAS1600_BURST);
 
 
@@ -755,6 +757,7 @@ static void das16_reset(struct comedi_device *dev)
 static int das16_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	const struct das16_board *board = comedi_board(dev);
 	int i, n;
 	int range;
 	int chan;
@@ -770,9 +773,9 @@ static int das16_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	outb(chan, dev->iobase + DAS16_MUX);
 
 	/* set gain */
-	if (thisboard->ai_pg != das16_pg_none) {
+	if (board->ai_pg != das16_pg_none) {
 		range = CR_RANGE(insn->chanspec);
-		outb((das16_gainlists[thisboard->ai_pg])[range],
+		outb((das16_gainlists[board->ai_pg])[range],
 		     dev->iobase + DAS16_GAIN);
 	}
 
@@ -790,7 +793,7 @@ static int das16_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		}
 		msb = inb(dev->iobase + DAS16_AI_MSB);
 		lsb = inb(dev->iobase + DAS16_AI_LSB);
-		if (thisboard->ai_nbits == 12)
+		if (board->ai_nbits == 12)
 			data[n] = ((lsb >> 4) & 0xf) | (msb << 4);
 		else
 			data[n] = lsb | (msb << 8);
@@ -809,7 +812,7 @@ static int das16_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
 	data[1] = bits;
 	data[0] = 0;
 
-	return 2;
+	return insn->n;
 }
 
 static int das16_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -829,12 +832,13 @@ static int das16_do_wbits(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	outb(s->state, dev->iobase + DAS16_DIO);
 
-	return 2;
+	return insn->n;
 }
 
 static int das16_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	const struct das16_board *board = comedi_board(dev);
 	int i;
 	int lsb, msb;
 	int chan;
@@ -842,7 +846,7 @@ static int das16_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++) {
-		if (thisboard->ao_nbits == 12) {
+		if (board->ao_nbits == 12) {
 			lsb = (data[i] << 4) & 0xff;
 			msb = (data[i] >> 4) & 0xff;
 		} else {
@@ -892,6 +896,7 @@ static int disable_dma_on_even(struct comedi_device *dev)
 
 static void das16_interrupt(struct comedi_device *dev)
 {
+	const struct das16_board *board = comedi_board(dev);
 	unsigned long dma_flags, spin_flags;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
@@ -953,7 +958,7 @@ static void das16_interrupt(struct comedi_device *dev)
 		set_dma_count(devpriv->dma_chan, devpriv->dma_transfer_size);
 		enable_dma(devpriv->dma_chan);
 		/* reenable conversions for das1600 mode, (stupid hardware) */
-		if (thisboard->size > 0x400 && devpriv->timer_mode == 0)
+		if (board->size > 0x400 && devpriv->timer_mode == 0)
 			outb(0x00, dev->iobase + DAS1600_CONV);
 
 	}
@@ -1015,6 +1020,7 @@ static void reg_dump(struct comedi_device *dev)
 
 static int das16_probe(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct das16_board *board = comedi_board(dev);
 	int status;
 	int diobits;
 
@@ -1039,9 +1045,9 @@ static int das16_probe(struct comedi_device *dev, struct comedi_devconfig *it)
 	diobits = inb(dev->iobase + DAS16_DIO) & 0xf0;
 
 	printk(KERN_INFO " id bits are 0x%02x\n", diobits);
-	if (thisboard->id != diobits) {
+	if (board->id != diobits) {
 		printk(KERN_INFO " requested board's id bits are 0x%x (ignore)\n",
-		       thisboard->id);
+		       board->id);
 	}
 
 	return 0;
@@ -1071,12 +1077,13 @@ static void das16_ai_munge(struct comedi_device *dev,
 			   unsigned int num_bytes,
 			   unsigned int start_chan_index)
 {
+	const struct das16_board *board = comedi_board(dev);
 	unsigned int i, num_samples = num_bytes / sizeof(short);
 	short *data = array;
 
 	for (i = 0; i < num_samples; i++) {
 		data[i] = le16_to_cpu(data[i]);
-		if (thisboard->ai_nbits == 12)
+		if (board->ai_nbits == 12)
 			data[i] = (data[i] >> 4) & 0xfff;
 
 	}
@@ -1092,6 +1099,7 @@ static void das16_ai_munge(struct comedi_device *dev,
  */
 static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct das16_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned int irq;
@@ -1130,9 +1138,9 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 
-	if (thisboard->size < 0x400) {
-		printk(" 0x%04lx-0x%04lx\n", iobase, iobase + thisboard->size);
-		if (!request_region(iobase, thisboard->size, "das16")) {
+	if (board->size < 0x400) {
+		printk(" 0x%04lx-0x%04lx\n", iobase, iobase + board->size);
+		if (!request_region(iobase, board->size, "das16")) {
 			printk(KERN_ERR " I/O port conflict\n");
 			return -EIO;
 		}
@@ -1140,18 +1148,18 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		printk(KERN_INFO " 0x%04lx-0x%04lx 0x%04lx-0x%04lx\n",
 		       iobase, iobase + 0x0f,
 		       iobase + 0x400,
-		       iobase + 0x400 + (thisboard->size & 0x3ff));
+		       iobase + 0x400 + (board->size & 0x3ff));
 		if (!request_region(iobase, 0x10, "das16")) {
 			printk(KERN_ERR " I/O port conflict:  0x%04lx-0x%04lx\n",
 			       iobase, iobase + 0x0f);
 			return -EIO;
 		}
-		if (!request_region(iobase + 0x400, thisboard->size & 0x3ff,
+		if (!request_region(iobase + 0x400, board->size & 0x3ff,
 				    "das16")) {
 			release_region(iobase, 0x10);
 			printk(KERN_ERR " I/O port conflict:  0x%04lx-0x%04lx\n",
 			       iobase + 0x400,
-			       iobase + 0x400 + (thisboard->size & 0x3ff));
+			       iobase + 0x400 + (board->size & 0x3ff));
 			return -EIO;
 		}
 	}
@@ -1163,10 +1171,10 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		printk(KERN_ERR " id bits do not match selected board, aborting\n");
 		return -EINVAL;
 	}
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	/*  get master clock speed */
-	if (thisboard->size < 0x400) {
+	if (board->size < 0x400) {
 		if (it->options[3])
 			devpriv->clockbase = 1000 / it->options[3];
 		else
@@ -1222,7 +1230,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	/*  get any user-defined input range */
-	if (thisboard->ai_pg == das16_pg_none &&
+	if (board->ai_pg == das16_pg_none &&
 	    (it->options[4] || it->options[5])) {
 		/*  allocate single-range range table */
 		devpriv->user_ai_range_table =
@@ -1256,14 +1264,14 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	devpriv->timer_mode = timer_mode ? 1 : 0;
 
-	ret = alloc_subdevices(dev, 5);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 5);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
 	dev->read_subdev = s;
 	/* ai */
-	if (thisboard->ai) {
+	if (board->ai) {
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
 		if (devpriv->ai_singleended) {
@@ -1275,15 +1283,15 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->len_chanlist = 8;
 			s->subdev_flags |= SDF_DIFF;
 		}
-		s->maxdata = (1 << thisboard->ai_nbits) - 1;
+		s->maxdata = (1 << board->ai_nbits) - 1;
 		if (devpriv->user_ai_range_table) { /*  user defined ai range */
 			s->range_table = devpriv->user_ai_range_table;
 		} else if (devpriv->ai_unipolar) {
-			s->range_table = das16_ai_uni_lranges[thisboard->ai_pg];
+			s->range_table = das16_ai_uni_lranges[board->ai_pg];
 		} else {
-			s->range_table = das16_ai_bip_lranges[thisboard->ai_pg];
+			s->range_table = das16_ai_bip_lranges[board->ai_pg];
 		}
-		s->insn_read = thisboard->ai;
+		s->insn_read = board->ai;
 		s->do_cmdtest = das16_cmd_test;
 		s->do_cmd = das16_cmd_exec;
 		s->cancel = das16_cancel;
@@ -1294,44 +1302,44 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	s = dev->subdevices + 1;
 	/* ao */
-	if (thisboard->ao) {
+	if (board->ao) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = 2;
-		s->maxdata = (1 << thisboard->ao_nbits) - 1;
+		s->maxdata = (1 << board->ao_nbits) - 1;
 		/*  user defined ao range */
 		if (devpriv->user_ao_range_table)
 			s->range_table = devpriv->user_ao_range_table;
 		else
 			s->range_table = &range_unknown;
 
-		s->insn_write = thisboard->ao;
+		s->insn_write = board->ao;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = dev->subdevices + 2;
 	/* di */
-	if (thisboard->di) {
+	if (board->di) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
 		s->n_chan = 4;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
-		s->insn_bits = thisboard->di;
+		s->insn_bits = board->di;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = dev->subdevices + 3;
 	/* do */
-	if (thisboard->do_) {
+	if (board->do_) {
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 		s->n_chan = 4;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
-		s->insn_bits = thisboard->do_;
+		s->insn_bits = board->do_;
 		/*  initialize digital output lines */
 		outb(s->state, dev->iobase + DAS16_DIO);
 	} else {
@@ -1340,9 +1348,9 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	s = dev->subdevices + 4;
 	/* 8255 */
-	if (thisboard->i8255_offset != 0) {
+	if (board->i8255_offset != 0) {
 		subdev_8255_init(dev, s, NULL, (dev->iobase +
-						thisboard->i8255_offset));
+						board->i8255_offset));
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
@@ -1353,7 +1361,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	outb(devpriv->control_state, dev->iobase + DAS16_CONTROL);
 
 	/*  turn on das1600 mode if available */
-	if (thisboard->size > 0x400) {
+	if (board->size > 0x400) {
 		outb(DAS1600_ENABLE_VAL, dev->iobase + DAS1600_ENABLE);
 		outb(0, dev->iobase + DAS1600_CONV);
 		outb(0, dev->iobase + DAS1600_BURST);
@@ -1364,6 +1372,8 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void das16_detach(struct comedi_device *dev)
 {
+	const struct das16_board *board = comedi_board(dev);
+
 	das16_reset(dev);
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 4);
@@ -1384,12 +1394,12 @@ static void das16_detach(struct comedi_device *dev)
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->iobase) {
-		if (thisboard->size < 0x400) {
-			release_region(dev->iobase, thisboard->size);
+		if (board->size < 0x400) {
+			release_region(dev->iobase, board->size);
 		} else {
 			release_region(dev->iobase, 0x10);
 			release_region(dev->iobase + 0x400,
-				       thisboard->size & 0x3ff);
+				       board->size & 0x3ff);
 		}
 	}
 }
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index d2e1490cd80..20092634786 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -150,7 +150,6 @@ struct das16m1_private_struct {
 	unsigned int divisor2;	/*  divides master clock to obtain conversion speed */
 };
 #define devpriv ((struct das16m1_private_struct *)(dev->private))
-#define thisboard ((const struct das16m1_board *)(dev->board_ptr))
 
 static inline short munge_sample(short data)
 {
@@ -168,6 +167,7 @@ static void munge_sample_array(short *array, unsigned int num_elements)
 static int das16m1_cmd_test(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct das16m1_board *board = comedi_board(dev);
 	unsigned int err = 0, tmp, i;
 
 	/* make sure triggers are valid */
@@ -225,8 +225,8 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < thisboard->ai_speed) {
-			cmd->convert_arg = thisboard->ai_speed;
+		if (cmd->convert_arg < board->ai_speed) {
+			cmd->convert_arg = board->ai_speed;
 			err++;
 		}
 	}
@@ -428,7 +428,7 @@ static int das16m1_di_rbits(struct comedi_device *dev,
 	data[1] = bits;
 	data[0] = 0;
 
-	return 2;
+	return insn->n;
 }
 
 static int das16m1_do_wbits(struct comedi_device *dev,
@@ -449,7 +449,7 @@ static int das16m1_do_wbits(struct comedi_device *dev,
 
 	outb(devpriv->do_bits, dev->iobase + DAS16M1_DIO);
 
-	return 2;
+	return insn->n;
 }
 
 static void das16m1_handler(struct comedi_device *dev, unsigned int status)
@@ -600,6 +600,7 @@ static int das16m1_irq_bits(unsigned int irq)
 static int das16m1_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	const struct das16m1_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int ret;
 	unsigned int irq;
@@ -611,7 +612,7 @@ static int das16m1_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	if (!request_region(iobase, DAS16M1_SIZE, dev->driver->driver_name)) {
 		comedi_error(dev, "I/O port conflict\n");
@@ -645,8 +646,8 @@ static int das16m1_attach(struct comedi_device *dev,
 		return -EINVAL;
 	}
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 2ac344354c1..25e7e56a376 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -183,46 +183,6 @@ enum {
 	das1802hr, das1802hr_da, das1801hc, das1802hc, das1801ao, das1802ao
 };
 
-static int das1800_probe(struct comedi_device *dev);
-static int das1800_cancel(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-static irqreturn_t das1800_interrupt(int irq, void *d);
-static int das1800_ai_poll(struct comedi_device *dev,
-			   struct comedi_subdevice *s);
-static void das1800_ai_handler(struct comedi_device *dev);
-static void das1800_handle_dma(struct comedi_device *dev,
-			       struct comedi_subdevice *s, unsigned int status);
-static void das1800_flush_dma(struct comedi_device *dev,
-			      struct comedi_subdevice *s);
-static void das1800_flush_dma_channel(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      unsigned int channel, uint16_t *buffer);
-static void das1800_handle_fifo_half_full(struct comedi_device *dev,
-					  struct comedi_subdevice *s);
-static void das1800_handle_fifo_not_empty(struct comedi_device *dev,
-					  struct comedi_subdevice *s);
-static int das1800_ai_do_cmdtest(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_cmd *cmd);
-static int das1800_ai_do_cmd(struct comedi_device *dev,
-			     struct comedi_subdevice *s);
-static int das1800_ai_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das1800_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das1800_di_rbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int das1800_do_wbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-
-static int das1800_set_frequency(struct comedi_device *dev);
-static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode);
-static unsigned int suggest_transfer_size(struct comedi_cmd *cmd);
-
 /* analog input ranges */
 static const struct comedi_lrange range_ai_das1801 = {
 	8,
@@ -515,400 +475,182 @@ static const struct comedi_lrange range_ao_2 = {
 };
 */
 
-static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
-			    unsigned int dma1)
+static inline uint16_t munge_bipolar_sample(const struct comedi_device *dev,
+					    uint16_t sample)
 {
-	unsigned long flags;
-
-	/*  need an irq to do dma */
-	if (dev->irq && dma0) {
-		/* encode dma0 and dma1 into 2 digit hexadecimal for switch */
-		switch ((dma0 & 0x7) | (dma1 << 4)) {
-		case 0x5:	/*  dma0 == 5 */
-			devpriv->dma_bits |= DMA_CH5;
-			break;
-		case 0x6:	/*  dma0 == 6 */
-			devpriv->dma_bits |= DMA_CH6;
-			break;
-		case 0x7:	/*  dma0 == 7 */
-			devpriv->dma_bits |= DMA_CH7;
-			break;
-		case 0x65:	/*  dma0 == 5, dma1 == 6 */
-			devpriv->dma_bits |= DMA_CH5_CH6;
-			break;
-		case 0x76:	/*  dma0 == 6, dma1 == 7 */
-			devpriv->dma_bits |= DMA_CH6_CH7;
-			break;
-		case 0x57:	/*  dma0 == 7, dma1 == 5 */
-			devpriv->dma_bits |= DMA_CH7_CH5;
-			break;
-		default:
-			dev_err(dev->hw_dev, " only supports dma channels 5 through 7\n"
-				" Dual dma only allows the following combinations:\n"
-				" dma 5,6 / 6,7 / or 7,5\n");
-			return -EINVAL;
-			break;
-		}
-		if (request_dma(dma0, dev->driver->driver_name)) {
-			dev_err(dev->hw_dev, "failed to allocate dma channel %i\n",
-				dma0);
-			return -EINVAL;
-		}
-		devpriv->dma0 = dma0;
-		devpriv->dma_current = dma0;
-		if (dma1) {
-			if (request_dma(dma1, dev->driver->driver_name)) {
-				dev_err(dev->hw_dev, "failed to allocate dma channel %i\n",
-					dma1);
-				return -EINVAL;
-			}
-			devpriv->dma1 = dma1;
-		}
-		devpriv->ai_buf0 = kmalloc(DMA_BUF_SIZE, GFP_KERNEL | GFP_DMA);
-		if (devpriv->ai_buf0 == NULL)
-			return -ENOMEM;
-		devpriv->dma_current_buf = devpriv->ai_buf0;
-		if (dma1) {
-			devpriv->ai_buf1 =
-			    kmalloc(DMA_BUF_SIZE, GFP_KERNEL | GFP_DMA);
-			if (devpriv->ai_buf1 == NULL)
-				return -ENOMEM;
-		}
-		flags = claim_dma_lock();
-		disable_dma(devpriv->dma0);
-		set_dma_mode(devpriv->dma0, DMA_MODE_READ);
-		if (dma1) {
-			disable_dma(devpriv->dma1);
-			set_dma_mode(devpriv->dma1, DMA_MODE_READ);
-		}
-		release_dma_lock(flags);
-	}
-	return 0;
+	sample += 1 << (thisboard->resolution - 1);
+	return sample;
 }
 
-static int das1800_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
+static void munge_data(struct comedi_device *dev, uint16_t * array,
+		       unsigned int num_elements)
 {
-	struct comedi_subdevice *s;
-	unsigned long iobase = it->options[0];
-	unsigned int irq = it->options[1];
-	unsigned int dma0 = it->options[2];
-	unsigned int dma1 = it->options[3];
-	unsigned long iobase2;
-	int board;
-	int retval;
-
-	/* allocate and initialize dev->private */
-	if (alloc_private(dev, sizeof(struct das1800_private)) < 0)
-		return -ENOMEM;
-
-	printk(KERN_DEBUG "comedi%d: %s: io 0x%lx", dev->minor,
-	       dev->driver->driver_name, iobase);
-	if (irq) {
-		printk(KERN_CONT ", irq %u", irq);
-		if (dma0) {
-			printk(KERN_CONT ", dma %u", dma0);
-			if (dma1)
-				printk(KERN_CONT " and %u", dma1);
-		}
-	}
-	printk(KERN_CONT "\n");
-
-	if (iobase == 0) {
-		dev_err(dev->hw_dev, "io base address required\n");
-		return -EINVAL;
-	}
+	unsigned int i;
+	int unipolar;
 
-	/* check if io addresses are available */
-	if (!request_region(iobase, DAS1800_SIZE, dev->driver->driver_name)) {
-		printk
-		    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
-		     iobase, iobase + DAS1800_SIZE - 1);
-		return -EIO;
-	}
-	dev->iobase = iobase;
+	/* see if card is using a unipolar or bipolar range so we can munge data correctly */
+	unipolar = inb(dev->iobase + DAS1800_CONTROL_C) & UB;
 
-	board = das1800_probe(dev);
-	if (board < 0) {
-		dev_err(dev->hw_dev, "unable to determine board type\n");
-		return -ENODEV;
+	/* convert to unsigned type if we are in a bipolar mode */
+	if (!unipolar) {
+		for (i = 0; i < num_elements; i++)
+			array[i] = munge_bipolar_sample(dev, array[i]);
 	}
+}
 
-	dev->board_ptr = das1800_boards + board;
-	dev->board_name = thisboard->name;
-
-	/*  if it is an 'ao' board with fancy analog out then we need extra io ports */
-	if (thisboard->ao_ability == 2) {
-		iobase2 = iobase + IOBASE2;
-		if (!request_region(iobase2, DAS1800_SIZE,
-				    dev->driver->driver_name)) {
-			printk
-			    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
-			     iobase2, iobase2 + DAS1800_SIZE - 1);
-			return -EIO;
-		}
-		devpriv->iobase2 = iobase2;
-	}
+static void das1800_handle_fifo_half_full(struct comedi_device *dev,
+					  struct comedi_subdevice *s)
+{
+	int numPoints = 0;	/* number of points to read */
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	/* grab our IRQ */
-	if (irq) {
-		if (request_irq(irq, das1800_interrupt, 0,
-				dev->driver->driver_name, dev)) {
-			dev_dbg(dev->hw_dev, "unable to allocate irq %u\n",
-				irq);
-			return -EINVAL;
-		}
-	}
-	dev->irq = irq;
+	numPoints = FIFO_SIZE / 2;
+	/* if we only need some of the points */
+	if (cmd->stop_src == TRIG_COUNT && devpriv->count < numPoints)
+		numPoints = devpriv->count;
+	insw(dev->iobase + DAS1800_FIFO, devpriv->ai_buf0, numPoints);
+	munge_data(dev, devpriv->ai_buf0, numPoints);
+	cfc_write_array_to_buffer(s, devpriv->ai_buf0,
+				  numPoints * sizeof(devpriv->ai_buf0[0]));
+	if (cmd->stop_src == TRIG_COUNT)
+		devpriv->count -= numPoints;
+	return;
+}
 
-	/*  set bits that tell card which irq to use */
-	switch (irq) {
-	case 0:
-		break;
-	case 3:
-		devpriv->irq_dma_bits |= 0x8;
-		break;
-	case 5:
-		devpriv->irq_dma_bits |= 0x10;
-		break;
-	case 7:
-		devpriv->irq_dma_bits |= 0x18;
-		break;
-	case 10:
-		devpriv->irq_dma_bits |= 0x28;
-		break;
-	case 11:
-		devpriv->irq_dma_bits |= 0x30;
-		break;
-	case 15:
-		devpriv->irq_dma_bits |= 0x38;
-		break;
-	default:
-		dev_err(dev->hw_dev, "irq out of range\n");
-		return -EINVAL;
-		break;
-	}
+static void das1800_handle_fifo_not_empty(struct comedi_device *dev,
+					  struct comedi_subdevice *s)
+{
+	short dpnt;
+	int unipolar;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	retval = das1800_init_dma(dev, dma0, dma1);
-	if (retval < 0)
-		return retval;
+	unipolar = inb(dev->iobase + DAS1800_CONTROL_C) & UB;
 
-	if (devpriv->ai_buf0 == NULL) {
-		devpriv->ai_buf0 =
-		    kmalloc(FIFO_SIZE * sizeof(uint16_t), GFP_KERNEL);
-		if (devpriv->ai_buf0 == NULL)
-			return -ENOMEM;
+	while (inb(dev->iobase + DAS1800_STATUS) & FNE) {
+		if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0)
+			break;
+		dpnt = inw(dev->iobase + DAS1800_FIFO);
+		/* convert to unsigned type if we are in a bipolar mode */
+		if (!unipolar)
+			;
+		dpnt = munge_bipolar_sample(dev, dpnt);
+		cfc_write_to_buffer(s, dpnt);
+		if (cmd->stop_src == TRIG_COUNT)
+			devpriv->count--;
 	}
 
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-	/* analog input subdevice */
-	s = dev->subdevices + 0;
-	dev->read_subdev = s;
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND | SDF_CMD_READ;
-	if (thisboard->common)
-		s->subdev_flags |= SDF_COMMON;
-	s->n_chan = thisboard->qram_len;
-	s->len_chanlist = thisboard->qram_len;
-	s->maxdata = (1 << thisboard->resolution) - 1;
-	s->range_table = thisboard->range_ai;
-	s->do_cmd = das1800_ai_do_cmd;
-	s->do_cmdtest = das1800_ai_do_cmdtest;
-	s->insn_read = das1800_ai_rinsn;
-	s->poll = das1800_ai_poll;
-	s->cancel = das1800_cancel;
-
-	/* analog out */
-	s = dev->subdevices + 1;
-	if (thisboard->ao_ability == 1) {
-		s->type = COMEDI_SUBD_AO;
-		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = thisboard->ao_n_chan;
-		s->maxdata = (1 << thisboard->resolution) - 1;
-		s->range_table = &range_ao_1;
-		s->insn_write = das1800_ao_winsn;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
+	return;
+}
 
-	/* di */
-	s = dev->subdevices + 2;
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 4;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = das1800_di_rbits;
+/* Utility function used by das1800_flush_dma() and das1800_handle_dma().
+ * Assumes dma lock is held */
+static void das1800_flush_dma_channel(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      unsigned int channel, uint16_t *buffer)
+{
+	unsigned int num_bytes, num_samples;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	/* do */
-	s = dev->subdevices + 3;
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = thisboard->do_n_chan;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = das1800_do_wbits;
+	disable_dma(channel);
 
-	das1800_cancel(dev, dev->read_subdev);
+	/* clear flip-flop to make sure 2-byte registers
+	 * get set correctly */
+	clear_dma_ff(channel);
 
-	/*  initialize digital out channels */
-	outb(devpriv->do_bits, dev->iobase + DAS1800_DIGITAL);
+	/*  figure out how many points to read */
+	num_bytes = devpriv->dma_transfer_size - get_dma_residue(channel);
+	num_samples = num_bytes / sizeof(short);
 
-	/*  initialize analog out channels */
-	if (thisboard->ao_ability == 1) {
-		/*  select 'update' dac channel for baseAddress + 0x0 */
-		outb(DAC(thisboard->ao_n_chan - 1),
-		     dev->iobase + DAS1800_SELECT);
-		outw(devpriv->ao_update_bits, dev->iobase + DAS1800_DAC);
-	}
+	/* if we only need some of the points */
+	if (cmd->stop_src == TRIG_COUNT && devpriv->count < num_samples)
+		num_samples = devpriv->count;
 
-	return 0;
-};
+	munge_data(dev, buffer, num_samples);
+	cfc_write_array_to_buffer(s, buffer, num_bytes);
+	if (s->async->cmd.stop_src == TRIG_COUNT)
+		devpriv->count -= num_samples;
 
-static void das1800_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		release_region(dev->iobase, DAS1800_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->private) {
-		if (devpriv->iobase2)
-			release_region(devpriv->iobase2, DAS1800_SIZE);
-		if (devpriv->dma0)
-			free_dma(devpriv->dma0);
-		if (devpriv->dma1)
-			free_dma(devpriv->dma1);
-		kfree(devpriv->ai_buf0);
-		kfree(devpriv->ai_buf1);
-	}
-};
+	return;
+}
 
-/* probes and checks das-1800 series board type
- */
-static int das1800_probe(struct comedi_device *dev)
+/* flushes remaining data from board when external trigger has stopped acquisition
+ * and we are using dma transfers */
+static void das1800_flush_dma(struct comedi_device *dev,
+			      struct comedi_subdevice *s)
 {
-	int id;
-	int board;
+	unsigned long flags;
+	const int dual_dma = devpriv->irq_dma_bits & DMA_DUAL;
 
-	id = (inb(dev->iobase + DAS1800_DIGITAL) >> 4) & 0xf;	/* get id bits */
-	board = ((struct das1800_board *)dev->board_ptr) - das1800_boards;
+	flags = claim_dma_lock();
+	das1800_flush_dma_channel(dev, s, devpriv->dma_current,
+				  devpriv->dma_current_buf);
 
-	switch (id) {
-	case 0x3:
-		if (board == das1801st_da || board == das1802st_da ||
-		    board == das1701st_da || board == das1702st_da) {
-			dev_dbg(dev->hw_dev, "Board model: %s\n",
-				das1800_boards[board].name);
-			return board;
-		}
-		printk
-		    (" Board model (probed, not recommended): das-1800st-da series\n");
-		return das1801st;
-		break;
-	case 0x4:
-		if (board == das1802hr_da || board == das1702hr_da) {
-			dev_dbg(dev->hw_dev, "Board model: %s\n",
-				das1800_boards[board].name);
-			return board;
-		}
-		printk
-		    (" Board model (probed, not recommended): das-1802hr-da\n");
-		return das1802hr;
-		break;
-	case 0x5:
-		if (board == das1801ao || board == das1802ao ||
-		    board == das1701ao || board == das1702ao) {
-			dev_dbg(dev->hw_dev, "Board model: %s\n",
-				das1800_boards[board].name);
-			return board;
-		}
-		printk
-		    (" Board model (probed, not recommended): das-1800ao series\n");
-		return das1801ao;
-		break;
-	case 0x6:
-		if (board == das1802hr || board == das1702hr) {
-			dev_dbg(dev->hw_dev, "Board model: %s\n",
-				das1800_boards[board].name);
-			return board;
-		}
-		printk
-		    (" Board model (probed, not recommended): das-1802hr\n");
-		return das1802hr;
-		break;
-	case 0x7:
-		if (board == das1801st || board == das1802st ||
-		    board == das1701st || board == das1702st) {
-			dev_dbg(dev->hw_dev, "Board model: %s\n",
-				das1800_boards[board].name);
-			return board;
-		}
-		printk
-		    (" Board model (probed, not recommended): das-1800st series\n");
-		return das1801st;
-		break;
-	case 0x8:
-		if (board == das1801hc || board == das1802hc) {
-			dev_dbg(dev->hw_dev, "Board model: %s\n",
-				das1800_boards[board].name);
-			return board;
+	if (dual_dma) {
+		/*  switch to other channel and flush it */
+		if (devpriv->dma_current == devpriv->dma0) {
+			devpriv->dma_current = devpriv->dma1;
+			devpriv->dma_current_buf = devpriv->ai_buf1;
+		} else {
+			devpriv->dma_current = devpriv->dma0;
+			devpriv->dma_current_buf = devpriv->ai_buf0;
 		}
-		printk
-		    (" Board model (probed, not recommended): das-1800hc series\n");
-		return das1801hc;
-		break;
-	default:
-		printk
-		    (" Board model: probe returned 0x%x (unknown, please report)\n",
-		     id);
-		return board;
-		break;
+		das1800_flush_dma_channel(dev, s, devpriv->dma_current,
+					  devpriv->dma_current_buf);
 	}
-	return -1;
-}
 
-static int das1800_ai_poll(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	unsigned long flags;
+	release_dma_lock(flags);
 
-	/*  prevent race with interrupt handler */
-	spin_lock_irqsave(&dev->spinlock, flags);
-	das1800_ai_handler(dev);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
+	/*  get any remaining samples in fifo */
+	das1800_handle_fifo_not_empty(dev, s);
 
-	return s->async->buf_write_count - s->async->buf_read_count;
+	return;
 }
 
-static irqreturn_t das1800_interrupt(int irq, void *d)
+static void das1800_handle_dma(struct comedi_device *dev,
+			       struct comedi_subdevice *s, unsigned int status)
 {
-	struct comedi_device *dev = d;
-	unsigned int status;
-
-	if (dev->attached == 0) {
-		comedi_error(dev, "premature interrupt");
-		return IRQ_HANDLED;
-	}
+	unsigned long flags;
+	const int dual_dma = devpriv->irq_dma_bits & DMA_DUAL;
 
-	/* Prevent race with das1800_ai_poll() on multi processor systems.
-	 * Also protects indirect addressing in das1800_ai_handler */
-	spin_lock(&dev->spinlock);
-	status = inb(dev->iobase + DAS1800_STATUS);
+	flags = claim_dma_lock();
+	das1800_flush_dma_channel(dev, s, devpriv->dma_current,
+				  devpriv->dma_current_buf);
+	/*  re-enable  dma channel */
+	set_dma_addr(devpriv->dma_current,
+		     virt_to_bus(devpriv->dma_current_buf));
+	set_dma_count(devpriv->dma_current, devpriv->dma_transfer_size);
+	enable_dma(devpriv->dma_current);
+	release_dma_lock(flags);
 
-	/* if interrupt was not caused by das-1800 */
-	if (!(status & INT)) {
-		spin_unlock(&dev->spinlock);
-		return IRQ_NONE;
+	if (status & DMATC) {
+		/*  clear DMATC interrupt bit */
+		outb(CLEAR_INTR_MASK & ~DMATC, dev->iobase + DAS1800_STATUS);
+		/*  switch dma channels for next time, if appropriate */
+		if (dual_dma) {
+			/*  read data from the other channel next time */
+			if (devpriv->dma_current == devpriv->dma0) {
+				devpriv->dma_current = devpriv->dma1;
+				devpriv->dma_current_buf = devpriv->ai_buf1;
+			} else {
+				devpriv->dma_current = devpriv->dma0;
+				devpriv->dma_current_buf = devpriv->ai_buf0;
+			}
+		}
 	}
-	/* clear the interrupt status bit INT */
-	outb(CLEAR_INTR_MASK & ~INT, dev->iobase + DAS1800_STATUS);
-	/*  handle interrupt */
-	das1800_ai_handler(dev);
 
-	spin_unlock(&dev->spinlock);
-	return IRQ_HANDLED;
+	return;
+}
+
+static int das1800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
+{
+	outb(0x0, dev->iobase + DAS1800_STATUS);	/* disable conversions */
+	outb(0x0, dev->iobase + DAS1800_CONTROL_B);	/* disable interrupts and dma */
+	outb(0x0, dev->iobase + DAS1800_CONTROL_A);	/* disable and clear fifo and stop triggering */
+	if (devpriv->dma0)
+		disable_dma(devpriv->dma0);
+	if (devpriv->dma1)
+		disable_dma(devpriv->dma1);
+	return 0;
 }
 
 /* the guts of the interrupt handler, that is shared with das1800_ai_poll */
@@ -965,182 +707,75 @@ static void das1800_ai_handler(struct comedi_device *dev)
 	return;
 }
 
-static void das1800_handle_dma(struct comedi_device *dev,
-			       struct comedi_subdevice *s, unsigned int status)
+static int das1800_ai_poll(struct comedi_device *dev,
+			   struct comedi_subdevice *s)
 {
 	unsigned long flags;
-	const int dual_dma = devpriv->irq_dma_bits & DMA_DUAL;
-
-	flags = claim_dma_lock();
-	das1800_flush_dma_channel(dev, s, devpriv->dma_current,
-				  devpriv->dma_current_buf);
-	/*  re-enable  dma channel */
-	set_dma_addr(devpriv->dma_current,
-		     virt_to_bus(devpriv->dma_current_buf));
-	set_dma_count(devpriv->dma_current, devpriv->dma_transfer_size);
-	enable_dma(devpriv->dma_current);
-	release_dma_lock(flags);
 
-	if (status & DMATC) {
-		/*  clear DMATC interrupt bit */
-		outb(CLEAR_INTR_MASK & ~DMATC, dev->iobase + DAS1800_STATUS);
-		/*  switch dma channels for next time, if appropriate */
-		if (dual_dma) {
-			/*  read data from the other channel next time */
-			if (devpriv->dma_current == devpriv->dma0) {
-				devpriv->dma_current = devpriv->dma1;
-				devpriv->dma_current_buf = devpriv->ai_buf1;
-			} else {
-				devpriv->dma_current = devpriv->dma0;
-				devpriv->dma_current_buf = devpriv->ai_buf0;
-			}
-		}
-	}
-
-	return;
-}
+	/*  prevent race with interrupt handler */
+	spin_lock_irqsave(&dev->spinlock, flags);
+	das1800_ai_handler(dev);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-static inline uint16_t munge_bipolar_sample(const struct comedi_device *dev,
-					    uint16_t sample)
-{
-	sample += 1 << (thisboard->resolution - 1);
-	return sample;
+	return s->async->buf_write_count - s->async->buf_read_count;
 }
 
-static void munge_data(struct comedi_device *dev, uint16_t * array,
-		       unsigned int num_elements)
+static irqreturn_t das1800_interrupt(int irq, void *d)
 {
-	unsigned int i;
-	int unipolar;
-
-	/* see if card is using a unipolar or bipolar range so we can munge data correctly */
-	unipolar = inb(dev->iobase + DAS1800_CONTROL_C) & UB;
+	struct comedi_device *dev = d;
+	unsigned int status;
 
-	/* convert to unsigned type if we are in a bipolar mode */
-	if (!unipolar) {
-		for (i = 0; i < num_elements; i++)
-			array[i] = munge_bipolar_sample(dev, array[i]);
+	if (dev->attached == 0) {
+		comedi_error(dev, "premature interrupt");
+		return IRQ_HANDLED;
 	}
-}
 
-/* Utility function used by das1800_flush_dma() and das1800_handle_dma().
- * Assumes dma lock is held */
-static void das1800_flush_dma_channel(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      unsigned int channel, uint16_t *buffer)
-{
-	unsigned int num_bytes, num_samples;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	disable_dma(channel);
-
-	/* clear flip-flop to make sure 2-byte registers
-	 * get set correctly */
-	clear_dma_ff(channel);
-
-	/*  figure out how many points to read */
-	num_bytes = devpriv->dma_transfer_size - get_dma_residue(channel);
-	num_samples = num_bytes / sizeof(short);
-
-	/* if we only need some of the points */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->count < num_samples)
-		num_samples = devpriv->count;
-
-	munge_data(dev, buffer, num_samples);
-	cfc_write_array_to_buffer(s, buffer, num_bytes);
-	if (s->async->cmd.stop_src == TRIG_COUNT)
-		devpriv->count -= num_samples;
-
-	return;
-}
-
-/* flushes remaining data from board when external trigger has stopped acquisition
- * and we are using dma transfers */
-static void das1800_flush_dma(struct comedi_device *dev,
-			      struct comedi_subdevice *s)
-{
-	unsigned long flags;
-	const int dual_dma = devpriv->irq_dma_bits & DMA_DUAL;
-
-	flags = claim_dma_lock();
-	das1800_flush_dma_channel(dev, s, devpriv->dma_current,
-				  devpriv->dma_current_buf);
+	/* Prevent race with das1800_ai_poll() on multi processor systems.
+	 * Also protects indirect addressing in das1800_ai_handler */
+	spin_lock(&dev->spinlock);
+	status = inb(dev->iobase + DAS1800_STATUS);
 
-	if (dual_dma) {
-		/*  switch to other channel and flush it */
-		if (devpriv->dma_current == devpriv->dma0) {
-			devpriv->dma_current = devpriv->dma1;
-			devpriv->dma_current_buf = devpriv->ai_buf1;
-		} else {
-			devpriv->dma_current = devpriv->dma0;
-			devpriv->dma_current_buf = devpriv->ai_buf0;
-		}
-		das1800_flush_dma_channel(dev, s, devpriv->dma_current,
-					  devpriv->dma_current_buf);
+	/* if interrupt was not caused by das-1800 */
+	if (!(status & INT)) {
+		spin_unlock(&dev->spinlock);
+		return IRQ_NONE;
 	}
+	/* clear the interrupt status bit INT */
+	outb(CLEAR_INTR_MASK & ~INT, dev->iobase + DAS1800_STATUS);
+	/*  handle interrupt */
+	das1800_ai_handler(dev);
 
-	release_dma_lock(flags);
-
-	/*  get any remaining samples in fifo */
-	das1800_handle_fifo_not_empty(dev, s);
-
-	return;
-}
-
-static void das1800_handle_fifo_half_full(struct comedi_device *dev,
-					  struct comedi_subdevice *s)
-{
-	int numPoints = 0;	/* number of points to read */
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	numPoints = FIFO_SIZE / 2;
-	/* if we only need some of the points */
-	if (cmd->stop_src == TRIG_COUNT && devpriv->count < numPoints)
-		numPoints = devpriv->count;
-	insw(dev->iobase + DAS1800_FIFO, devpriv->ai_buf0, numPoints);
-	munge_data(dev, devpriv->ai_buf0, numPoints);
-	cfc_write_array_to_buffer(s, devpriv->ai_buf0,
-				  numPoints * sizeof(devpriv->ai_buf0[0]));
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count -= numPoints;
-	return;
+	spin_unlock(&dev->spinlock);
+	return IRQ_HANDLED;
 }
 
-static void das1800_handle_fifo_not_empty(struct comedi_device *dev,
-					  struct comedi_subdevice *s)
+/* converts requested conversion timing to timing compatible with
+ * hardware, used only when card is in 'burst mode'
+ */
+static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
 {
-	short dpnt;
-	int unipolar;
-	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int micro_sec;
 
-	unipolar = inb(dev->iobase + DAS1800_CONTROL_C) & UB;
+	/*  in burst mode, the maximum conversion time is 64 microseconds */
+	if (convert_arg > 64000)
+		convert_arg = 64000;
 
-	while (inb(dev->iobase + DAS1800_STATUS) & FNE) {
-		if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0)
-			break;
-		dpnt = inw(dev->iobase + DAS1800_FIFO);
-		/* convert to unsigned type if we are in a bipolar mode */
-		if (!unipolar)
-			;
-		dpnt = munge_bipolar_sample(dev, dpnt);
-		cfc_write_to_buffer(s, dpnt);
-		if (cmd->stop_src == TRIG_COUNT)
-			devpriv->count--;
+	/*  the conversion time must be an integral number of microseconds */
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		micro_sec = (convert_arg + 500) / 1000;
+		break;
+	case TRIG_ROUND_DOWN:
+		micro_sec = convert_arg / 1000;
+		break;
+	case TRIG_ROUND_UP:
+		micro_sec = (convert_arg - 1) / 1000 + 1;
+		break;
 	}
 
-	return;
-}
-
-static int das1800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	outb(0x0, dev->iobase + DAS1800_STATUS);	/* disable conversions */
-	outb(0x0, dev->iobase + DAS1800_CONTROL_B);	/* disable interrupts and dma */
-	outb(0x0, dev->iobase + DAS1800_CONTROL_A);	/* disable and clear fifo and stop triggering */
-	if (devpriv->dma0)
-		disable_dma(devpriv->dma0);
-	if (devpriv->dma1)
-		disable_dma(devpriv->dma1);
-	return 0;
+	/*  return number of nanoseconds */
+	return micro_sec * 1000;
 }
 
 /* test analog input cmd */
@@ -1322,10 +957,6 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-/* analog input cmd interface */
-
-/* first, some utility functions used in the main ai_do_cmd() */
-
 /* returns appropriate bits for control register a, depending on command */
 static int control_a_bits(struct comedi_cmd cmd)
 {
@@ -1396,6 +1027,25 @@ static int control_c_bits(struct comedi_cmd cmd)
 	return control_c;
 }
 
+/* loads counters with divisor1, divisor2 from private structure */
+static int das1800_set_frequency(struct comedi_device *dev)
+{
+	int err = 0;
+
+	/*  counter 1, mode 2 */
+	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 1, devpriv->divisor1,
+		       2))
+		err++;
+	/*  counter 2, mode 2 */
+	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 2, devpriv->divisor2,
+		       2))
+		err++;
+	if (err)
+		return -1;
+
+	return 0;
+}
+
 /* sets up counters */
 static int setup_counters(struct comedi_device *dev, struct comedi_cmd cmd)
 {
@@ -1436,6 +1086,44 @@ static int setup_counters(struct comedi_device *dev, struct comedi_cmd cmd)
 	return 0;
 }
 
+/* utility function that suggests a dma transfer size based on the conversion period 'ns' */
+static unsigned int suggest_transfer_size(struct comedi_cmd *cmd)
+{
+	unsigned int size = DMA_BUF_SIZE;
+	static const int sample_size = 2;	/*  size in bytes of one sample from board */
+	unsigned int fill_time = 300000000;	/*  target time in nanoseconds for filling dma buffer */
+	unsigned int max_size;	/*  maximum size we will allow for a transfer */
+
+	/*  make dma buffer fill in 0.3 seconds for timed modes */
+	switch (cmd->scan_begin_src) {
+	case TRIG_FOLLOW:	/*  not in burst mode */
+		if (cmd->convert_src == TRIG_TIMER)
+			size = (fill_time / cmd->convert_arg) * sample_size;
+		break;
+	case TRIG_TIMER:
+		size = (fill_time / (cmd->scan_begin_arg * cmd->chanlist_len)) *
+		    sample_size;
+		break;
+	default:
+		size = DMA_BUF_SIZE;
+		break;
+	}
+
+	/*  set a minimum and maximum size allowed */
+	max_size = DMA_BUF_SIZE;
+	/*  if we are taking limited number of conversions, limit transfer size to that */
+	if (cmd->stop_src == TRIG_COUNT &&
+	    cmd->stop_arg * cmd->chanlist_len * sample_size < max_size)
+		max_size = cmd->stop_arg * cmd->chanlist_len * sample_size;
+
+	if (size > max_size)
+		size = max_size;
+	if (size < sample_size)
+		size = sample_size;
+
+	return size;
+}
+
 /* sets up dma */
 static void setup_dma(struct comedi_device *dev, struct comedi_cmd cmd)
 {
@@ -1665,7 +1353,7 @@ static int das1800_di_rbits(struct comedi_device *dev,
 	data[1] = inb(dev->iobase + DAS1800_DIGITAL) & 0xf;
 	data[0] = 0;
 
-	return 2;
+	return insn->n;
 }
 
 /* writes to digital output channels */
@@ -1686,94 +1374,365 @@ static int das1800_do_wbits(struct comedi_device *dev,
 
 	data[1] = devpriv->do_bits;
 
-	return 2;
+	return insn->n;
 }
 
-/* loads counters with divisor1, divisor2 from private structure */
-static int das1800_set_frequency(struct comedi_device *dev)
+static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
+			    unsigned int dma1)
 {
-	int err = 0;
-
-	/*  counter 1, mode 2 */
-	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 1, devpriv->divisor1,
-		       2))
-		err++;
-	/*  counter 2, mode 2 */
-	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 2, devpriv->divisor2,
-		       2))
-		err++;
-	if (err)
-		return -1;
+	unsigned long flags;
 
+	/*  need an irq to do dma */
+	if (dev->irq && dma0) {
+		/* encode dma0 and dma1 into 2 digit hexadecimal for switch */
+		switch ((dma0 & 0x7) | (dma1 << 4)) {
+		case 0x5:	/*  dma0 == 5 */
+			devpriv->dma_bits |= DMA_CH5;
+			break;
+		case 0x6:	/*  dma0 == 6 */
+			devpriv->dma_bits |= DMA_CH6;
+			break;
+		case 0x7:	/*  dma0 == 7 */
+			devpriv->dma_bits |= DMA_CH7;
+			break;
+		case 0x65:	/*  dma0 == 5, dma1 == 6 */
+			devpriv->dma_bits |= DMA_CH5_CH6;
+			break;
+		case 0x76:	/*  dma0 == 6, dma1 == 7 */
+			devpriv->dma_bits |= DMA_CH6_CH7;
+			break;
+		case 0x57:	/*  dma0 == 7, dma1 == 5 */
+			devpriv->dma_bits |= DMA_CH7_CH5;
+			break;
+		default:
+			dev_err(dev->class_dev,
+				"only supports dma channels 5 through 7\n");
+			dev_err(dev->class_dev,
+				"Dual dma only allows the following combinations:\n");
+			dev_err(dev->class_dev,
+				"dma 5,6 / 6,7 / or 7,5\n");
+			return -EINVAL;
+			break;
+		}
+		if (request_dma(dma0, dev->driver->driver_name)) {
+			dev_err(dev->class_dev,
+				"failed to allocate dma channel %i\n", dma0);
+			return -EINVAL;
+		}
+		devpriv->dma0 = dma0;
+		devpriv->dma_current = dma0;
+		if (dma1) {
+			if (request_dma(dma1, dev->driver->driver_name)) {
+				dev_err(dev->class_dev,
+					"failed to allocate dma channel %i\n",
+					dma1);
+				return -EINVAL;
+			}
+			devpriv->dma1 = dma1;
+		}
+		devpriv->ai_buf0 = kmalloc(DMA_BUF_SIZE, GFP_KERNEL | GFP_DMA);
+		if (devpriv->ai_buf0 == NULL)
+			return -ENOMEM;
+		devpriv->dma_current_buf = devpriv->ai_buf0;
+		if (dma1) {
+			devpriv->ai_buf1 =
+			    kmalloc(DMA_BUF_SIZE, GFP_KERNEL | GFP_DMA);
+			if (devpriv->ai_buf1 == NULL)
+				return -ENOMEM;
+		}
+		flags = claim_dma_lock();
+		disable_dma(devpriv->dma0);
+		set_dma_mode(devpriv->dma0, DMA_MODE_READ);
+		if (dma1) {
+			disable_dma(devpriv->dma1);
+			set_dma_mode(devpriv->dma1, DMA_MODE_READ);
+		}
+		release_dma_lock(flags);
+	}
 	return 0;
 }
 
-/* converts requested conversion timing to timing compatible with
- * hardware, used only when card is in 'burst mode'
- */
-static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
+static int das1800_probe(struct comedi_device *dev)
 {
-	unsigned int micro_sec;
+	int id;
+	int board;
 
-	/*  in burst mode, the maximum conversion time is 64 microseconds */
-	if (convert_arg > 64000)
-		convert_arg = 64000;
+	id = (inb(dev->iobase + DAS1800_DIGITAL) >> 4) & 0xf;	/* get id bits */
+	board = ((struct das1800_board *)dev->board_ptr) - das1800_boards;
 
-	/*  the conversion time must be an integral number of microseconds */
-	switch (round_mode) {
-	case TRIG_ROUND_NEAREST:
-	default:
-		micro_sec = (convert_arg + 500) / 1000;
+	switch (id) {
+	case 0x3:
+		if (board == das1801st_da || board == das1802st_da ||
+		    board == das1701st_da || board == das1702st_da) {
+			dev_dbg(dev->class_dev, "Board model: %s\n",
+				das1800_boards[board].name);
+			return board;
+		}
+		printk
+		    (" Board model (probed, not recommended): das-1800st-da series\n");
+		return das1801st;
 		break;
-	case TRIG_ROUND_DOWN:
-		micro_sec = convert_arg / 1000;
+	case 0x4:
+		if (board == das1802hr_da || board == das1702hr_da) {
+			dev_dbg(dev->class_dev, "Board model: %s\n",
+				das1800_boards[board].name);
+			return board;
+		}
+		printk
+		    (" Board model (probed, not recommended): das-1802hr-da\n");
+		return das1802hr;
 		break;
-	case TRIG_ROUND_UP:
-		micro_sec = (convert_arg - 1) / 1000 + 1;
+	case 0x5:
+		if (board == das1801ao || board == das1802ao ||
+		    board == das1701ao || board == das1702ao) {
+			dev_dbg(dev->class_dev, "Board model: %s\n",
+				das1800_boards[board].name);
+			return board;
+		}
+		printk
+		    (" Board model (probed, not recommended): das-1800ao series\n");
+		return das1801ao;
+		break;
+	case 0x6:
+		if (board == das1802hr || board == das1702hr) {
+			dev_dbg(dev->class_dev, "Board model: %s\n",
+				das1800_boards[board].name);
+			return board;
+		}
+		printk
+		    (" Board model (probed, not recommended): das-1802hr\n");
+		return das1802hr;
+		break;
+	case 0x7:
+		if (board == das1801st || board == das1802st ||
+		    board == das1701st || board == das1702st) {
+			dev_dbg(dev->class_dev, "Board model: %s\n",
+				das1800_boards[board].name);
+			return board;
+		}
+		printk
+		    (" Board model (probed, not recommended): das-1800st series\n");
+		return das1801st;
+		break;
+	case 0x8:
+		if (board == das1801hc || board == das1802hc) {
+			dev_dbg(dev->class_dev, "Board model: %s\n",
+				das1800_boards[board].name);
+			return board;
+		}
+		printk
+		    (" Board model (probed, not recommended): das-1800hc series\n");
+		return das1801hc;
+		break;
+	default:
+		printk
+		    (" Board model: probe returned 0x%x (unknown, please report)\n",
+		     id);
+		return board;
 		break;
 	}
-
-	/*  return number of nanoseconds */
-	return micro_sec * 1000;
+	return -1;
 }
 
-/* utility function that suggests a dma transfer size based on the conversion period 'ns' */
-static unsigned int suggest_transfer_size(struct comedi_cmd *cmd)
+static int das1800_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
 {
-	unsigned int size = DMA_BUF_SIZE;
-	static const int sample_size = 2;	/*  size in bytes of one sample from board */
-	unsigned int fill_time = 300000000;	/*  target time in nanoseconds for filling dma buffer */
-	unsigned int max_size;	/*  maximum size we will allow for a transfer */
+	struct comedi_subdevice *s;
+	unsigned long iobase = it->options[0];
+	unsigned int irq = it->options[1];
+	unsigned int dma0 = it->options[2];
+	unsigned int dma1 = it->options[3];
+	unsigned long iobase2;
+	int board;
+	int retval;
 
-	/*  make dma buffer fill in 0.3 seconds for timed modes */
-	switch (cmd->scan_begin_src) {
-	case TRIG_FOLLOW:	/*  not in burst mode */
-		if (cmd->convert_src == TRIG_TIMER)
-			size = (fill_time / cmd->convert_arg) * sample_size;
+	/* allocate and initialize dev->private */
+	if (alloc_private(dev, sizeof(struct das1800_private)) < 0)
+		return -ENOMEM;
+
+	printk(KERN_DEBUG "comedi%d: %s: io 0x%lx", dev->minor,
+	       dev->driver->driver_name, iobase);
+	if (irq) {
+		printk(KERN_CONT ", irq %u", irq);
+		if (dma0) {
+			printk(KERN_CONT ", dma %u", dma0);
+			if (dma1)
+				printk(KERN_CONT " and %u", dma1);
+		}
+	}
+	printk(KERN_CONT "\n");
+
+	if (iobase == 0) {
+		dev_err(dev->class_dev, "io base address required\n");
+		return -EINVAL;
+	}
+
+	/* check if io addresses are available */
+	if (!request_region(iobase, DAS1800_SIZE, dev->driver->driver_name)) {
+		printk
+		    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
+		     iobase, iobase + DAS1800_SIZE - 1);
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	board = das1800_probe(dev);
+	if (board < 0) {
+		dev_err(dev->class_dev, "unable to determine board type\n");
+		return -ENODEV;
+	}
+
+	dev->board_ptr = das1800_boards + board;
+	dev->board_name = thisboard->name;
+
+	/*  if it is an 'ao' board with fancy analog out then we need extra io ports */
+	if (thisboard->ao_ability == 2) {
+		iobase2 = iobase + IOBASE2;
+		if (!request_region(iobase2, DAS1800_SIZE,
+				    dev->driver->driver_name)) {
+			printk
+			    (" I/O port conflict: failed to allocate ports 0x%lx to 0x%lx\n",
+			     iobase2, iobase2 + DAS1800_SIZE - 1);
+			return -EIO;
+		}
+		devpriv->iobase2 = iobase2;
+	}
+
+	/* grab our IRQ */
+	if (irq) {
+		if (request_irq(irq, das1800_interrupt, 0,
+				dev->driver->driver_name, dev)) {
+			dev_dbg(dev->class_dev, "unable to allocate irq %u\n",
+				irq);
+			return -EINVAL;
+		}
+	}
+	dev->irq = irq;
+
+	/*  set bits that tell card which irq to use */
+	switch (irq) {
+	case 0:
 		break;
-	case TRIG_TIMER:
-		size = (fill_time / (cmd->scan_begin_arg * cmd->chanlist_len)) *
-		    sample_size;
+	case 3:
+		devpriv->irq_dma_bits |= 0x8;
+		break;
+	case 5:
+		devpriv->irq_dma_bits |= 0x10;
+		break;
+	case 7:
+		devpriv->irq_dma_bits |= 0x18;
+		break;
+	case 10:
+		devpriv->irq_dma_bits |= 0x28;
+		break;
+	case 11:
+		devpriv->irq_dma_bits |= 0x30;
+		break;
+	case 15:
+		devpriv->irq_dma_bits |= 0x38;
 		break;
 	default:
-		size = DMA_BUF_SIZE;
+		dev_err(dev->class_dev, "irq out of range\n");
+		return -EINVAL;
 		break;
 	}
 
-	/*  set a minimum and maximum size allowed */
-	max_size = DMA_BUF_SIZE;
-	/*  if we are taking limited number of conversions, limit transfer size to that */
-	if (cmd->stop_src == TRIG_COUNT &&
-	    cmd->stop_arg * cmd->chanlist_len * sample_size < max_size)
-		max_size = cmd->stop_arg * cmd->chanlist_len * sample_size;
+	retval = das1800_init_dma(dev, dma0, dma1);
+	if (retval < 0)
+		return retval;
 
-	if (size > max_size)
-		size = max_size;
-	if (size < sample_size)
-		size = sample_size;
+	if (devpriv->ai_buf0 == NULL) {
+		devpriv->ai_buf0 =
+		    kmalloc(FIFO_SIZE * sizeof(uint16_t), GFP_KERNEL);
+		if (devpriv->ai_buf0 == NULL)
+			return -ENOMEM;
+	}
 
-	return size;
-}
+	retval = comedi_alloc_subdevices(dev, 4);
+	if (retval)
+		return retval;
+
+	/* analog input subdevice */
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND | SDF_CMD_READ;
+	if (thisboard->common)
+		s->subdev_flags |= SDF_COMMON;
+	s->n_chan = thisboard->qram_len;
+	s->len_chanlist = thisboard->qram_len;
+	s->maxdata = (1 << thisboard->resolution) - 1;
+	s->range_table = thisboard->range_ai;
+	s->do_cmd = das1800_ai_do_cmd;
+	s->do_cmdtest = das1800_ai_do_cmdtest;
+	s->insn_read = das1800_ai_rinsn;
+	s->poll = das1800_ai_poll;
+	s->cancel = das1800_cancel;
+
+	/* analog out */
+	s = dev->subdevices + 1;
+	if (thisboard->ao_ability == 1) {
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITABLE;
+		s->n_chan = thisboard->ao_n_chan;
+		s->maxdata = (1 << thisboard->resolution) - 1;
+		s->range_table = &range_ao_1;
+		s->insn_write = das1800_ao_winsn;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	/* di */
+	s = dev->subdevices + 2;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 4;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = das1800_di_rbits;
+
+	/* do */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = thisboard->do_n_chan;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = das1800_do_wbits;
+
+	das1800_cancel(dev, dev->read_subdev);
+
+	/*  initialize digital out channels */
+	outb(devpriv->do_bits, dev->iobase + DAS1800_DIGITAL);
+
+	/*  initialize analog out channels */
+	if (thisboard->ao_ability == 1) {
+		/*  select 'update' dac channel for baseAddress + 0x0 */
+		outb(DAC(thisboard->ao_n_chan - 1),
+		     dev->iobase + DAS1800_SELECT);
+		outw(devpriv->ao_update_bits, dev->iobase + DAS1800_DAC);
+	}
+
+	return 0;
+};
+
+static void das1800_detach(struct comedi_device *dev)
+{
+	if (dev->iobase)
+		release_region(dev->iobase, DAS1800_SIZE);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->private) {
+		if (devpriv->iobase2)
+			release_region(devpriv->iobase2, DAS1800_SIZE);
+		if (devpriv->dma0)
+			free_dma(devpriv->dma0);
+		if (devpriv->dma1)
+			free_dma(devpriv->dma1);
+		kfree(devpriv->ai_buf0);
+		kfree(devpriv->ai_buf1);
+	}
+};
 
 static struct comedi_driver das1800_driver = {
 	.driver_name	= "das1800",
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index 881f392d1db..e3afcfa9efc 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -155,7 +155,7 @@ static irqreturn_t intr_handler(int irq, void *d)
 	struct comedi_subdevice *s = dev->subdevices;
 
 	if (!dev->attached || devpriv->das6402_ignoreirq) {
-		dev_warn(dev->hw_dev, "BUG: spurious interrupt\n");
+		dev_warn(dev->class_dev, "BUG: spurious interrupt\n");
 		return IRQ_HANDLED;
 	}
 #ifdef DEBUG
@@ -202,7 +202,7 @@ static int das6402_ai_cancel(struct comedi_device *dev,
 	 */
 
 	devpriv->das6402_ignoreirq = 1;
-	dev_dbg(dev->hw_dev, "Stopping acquisition\n");
+	dev_dbg(dev->class_dev, "Stopping acquisition\n");
 	devpriv->das6402_ignoreirq = 1;
 	outb_p(0x02, dev->iobase + 10);	/* disable external trigging */
 	outw_p(SCANL, dev->iobase + 2);	/* resets the card fifo */
@@ -218,7 +218,7 @@ static int das6402_ai_mode2(struct comedi_device *dev,
 			    struct comedi_subdevice *s, comedi_trig * it)
 {
 	devpriv->das6402_ignoreirq = 1;
-	dev_dbg(dev->hw_dev, "Starting acquisition\n");
+	dev_dbg(dev->class_dev, "Starting acquisition\n");
 	outb_p(0x03, dev->iobase + 10);	/* enable external trigging */
 	outw_p(SCANL, dev->iobase + 2);	/* resets the card fifo */
 	outb_p(IRQ | CONVSRC | BURSTEN | INTE, dev->iobase + 9);
@@ -289,7 +289,7 @@ static int das6402_attach(struct comedi_device *dev,
 		iobase = 0x300;
 
 	if (!request_region(iobase, DAS6402_SIZE, "das6402")) {
-		dev_err(dev->hw_dev, "I/O port conflict\n");
+		dev_err(dev->class_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	dev->iobase = iobase;
@@ -297,7 +297,7 @@ static int das6402_attach(struct comedi_device *dev,
 	/* should do a probe here */
 
 	irq = it->options[0];
-	dev_dbg(dev->hw_dev, "( irq = %u )\n", irq);
+	dev_dbg(dev->class_dev, "( irq = %u )\n", irq);
 	ret = request_irq(irq, intr_handler, 0, "das6402", dev);
 	if (ret < 0)
 		return ret;
@@ -307,8 +307,8 @@ static int das6402_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
 
 	/* ai subdevice */
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index a3a54e1c5c9..a0959a5e874 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -296,46 +296,47 @@ static int das800_probe(struct comedi_device *dev)
 	switch (id_bits) {
 	case 0x0:
 		if (board == das800) {
-			dev_dbg(dev->hw_dev, "Board model: DAS-800\n");
+			dev_dbg(dev->class_dev, "Board model: DAS-800\n");
 			return board;
 		}
 		if (board == ciodas800) {
-			dev_dbg(dev->hw_dev, "Board model: CIO-DAS800\n");
+			dev_dbg(dev->class_dev, "Board model: CIO-DAS800\n");
 			return board;
 		}
-		dev_dbg(dev->hw_dev, "Board model (probed): DAS-800\n");
+		dev_dbg(dev->class_dev, "Board model (probed): DAS-800\n");
 		return das800;
 		break;
 	case 0x2:
 		if (board == das801) {
-			dev_dbg(dev->hw_dev, "Board model: DAS-801\n");
+			dev_dbg(dev->class_dev, "Board model: DAS-801\n");
 			return board;
 		}
 		if (board == ciodas801) {
-			dev_dbg(dev->hw_dev, "Board model: CIO-DAS801\n");
+			dev_dbg(dev->class_dev, "Board model: CIO-DAS801\n");
 			return board;
 		}
-		dev_dbg(dev->hw_dev, "Board model (probed): DAS-801\n");
+		dev_dbg(dev->class_dev, "Board model (probed): DAS-801\n");
 		return das801;
 		break;
 	case 0x3:
 		if (board == das802) {
-			dev_dbg(dev->hw_dev, "Board model: DAS-802\n");
+			dev_dbg(dev->class_dev, "Board model: DAS-802\n");
 			return board;
 		}
 		if (board == ciodas802) {
-			dev_dbg(dev->hw_dev, "Board model: CIO-DAS802\n");
+			dev_dbg(dev->class_dev, "Board model: CIO-DAS802\n");
 			return board;
 		}
 		if (board == ciodas80216) {
-			dev_dbg(dev->hw_dev, "Board model: CIO-DAS802/16\n");
+			dev_dbg(dev->class_dev, "Board model: CIO-DAS802/16\n");
 			return board;
 		}
-		dev_dbg(dev->hw_dev, "Board model (probed): DAS-802\n");
+		dev_dbg(dev->class_dev, "Board model (probed): DAS-802\n");
 		return das802;
 		break;
 	default:
-		dev_dbg(dev->hw_dev, "Board model: probe returned 0x%x (unknown)\n",
+		dev_dbg(dev->class_dev,
+			"Board model: probe returned 0x%x (unknown)\n",
 			id_bits);
 		return board;
 		break;
@@ -450,7 +451,7 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 		/* otherwise, stop taking data */
 	} else {
 		spin_unlock_irqrestore(&dev->spinlock, irq_flags);
-		disable_das800(dev);	/* diable hardware triggered conversions */
+		disable_das800(dev);	/* disable hardware triggered conversions */
 		async->events |= COMEDI_CB_EOA;
 	}
 	comedi_event(dev, s);
@@ -465,43 +466,44 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	unsigned int irq = it->options[1];
 	unsigned long irq_flags;
 	int board;
+	int ret;
 
-	dev_info(dev->hw_dev, "comedi%d: das800: io 0x%lx\n", dev->minor,
-		 iobase);
+	dev_info(dev->class_dev, "das800: io 0x%lx\n", iobase);
 	if (irq)
-		dev_dbg(dev->hw_dev, "irq %u\n", irq);
+		dev_dbg(dev->class_dev, "irq %u\n", irq);
 
 	/* allocate and initialize dev->private */
 	if (alloc_private(dev, sizeof(struct das800_private)) < 0)
 		return -ENOMEM;
 
 	if (iobase == 0) {
-		dev_err(dev->hw_dev, "io base address required for das800\n");
+		dev_err(dev->class_dev,
+			"io base address required for das800\n");
 		return -EINVAL;
 	}
 
 	/* check if io addresses are available */
 	if (!request_region(iobase, DAS800_SIZE, "das800")) {
-		dev_err(dev->hw_dev, "I/O port conflict\n");
+		dev_err(dev->class_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	dev->iobase = iobase;
 
 	board = das800_probe(dev);
 	if (board < 0) {
-		dev_dbg(dev->hw_dev, "unable to determine board type\n");
+		dev_dbg(dev->class_dev, "unable to determine board type\n");
 		return -ENODEV;
 	}
 	dev->board_ptr = das800_boards + board;
 
 	/* grab our IRQ */
 	if (irq == 1 || irq > 7) {
-		dev_err(dev->hw_dev, "irq out of range\n");
+		dev_err(dev->class_dev, "irq out of range\n");
 		return -EINVAL;
 	}
 	if (irq) {
 		if (request_irq(irq, das800_interrupt, 0, "das800", dev)) {
-			dev_err(dev->hw_dev, "unable to allocate irq %u\n",
+			dev_err(dev->class_dev, "unable to allocate irq %u\n",
 				irq);
 			return -EINVAL;
 		}
@@ -510,8 +512,9 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = thisboard->name;
 
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	/* analog input subdevice */
 	s = dev->subdevices + 0;
@@ -872,7 +875,7 @@ static int das800_di_rbits(struct comedi_device *dev,
 	data[1] = bits;
 	data[0] = 0;
 
-	return 2;
+	return insn->n;
 }
 
 static int das800_do_wbits(struct comedi_device *dev,
@@ -896,7 +899,7 @@ static int das800_do_wbits(struct comedi_device *dev,
 
 	data[1] = wbits;
 
-	return 2;
+	return insn->n;
 }
 
 /* loads counters with divisor1, divisor2 from private structure */
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index 83828903db4..7107f590b1f 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -78,9 +78,6 @@ Configuration Options:
 #define DMM32AT_DIOC 0x0e
 #define DMM32AT_DIOCONF 0x0f
 
-#define dmm_inb(cdev, reg) inb((cdev->iobase)+reg)
-#define dmm_outb(cdev, reg, valu) outb(valu, (cdev->iobase)+reg)
-
 /* Board register values. */
 
 /* DMM32AT_DACSTAT 0x04 */
@@ -159,45 +156,10 @@ static const struct comedi_lrange dmm32at_aoranges = {
 	 }
 };
 
-/*
- * Board descriptions for two imaginary boards.  Describing the
- * boards in this way is optional, and completely driver-dependent.
- * Some drivers use arrays such as this, other do not.
- */
 struct dmm32at_board {
 	const char *name;
-	int ai_chans;
-	int ai_bits;
-	const struct comedi_lrange *ai_ranges;
-	int ao_chans;
-	int ao_bits;
-	const struct comedi_lrange *ao_ranges;
-	int have_dio;
-	int dio_chans;
-};
-static const struct dmm32at_board dmm32at_boards[] = {
-	{
-	 .name = "dmm32at",
-	 .ai_chans = 32,
-	 .ai_bits = 16,
-	 .ai_ranges = &dmm32at_airanges,
-	 .ao_chans = 4,
-	 .ao_bits = 12,
-	 .ao_ranges = &dmm32at_aoranges,
-	 .have_dio = 1,
-	 .dio_chans = 24,
-	 },
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct dmm32at_board *)dev->board_ptr)
-
-/* this structure is for data unique to this hardware driver.  If
- * several hardware drivers keep similar information in this structure,
- * feel free to suggest moving the variable to the struct comedi_device struct.
- */
 struct dmm32at_private {
 
 	int data;
@@ -210,259 +172,6 @@ struct dmm32at_private {
 
 };
 
-/*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct dmm32at_private *)dev->private)
-
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static int dmm32at_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static void dmm32at_detach(struct comedi_device *dev);
-static struct comedi_driver driver_dmm32at = {
-	.driver_name = "dmm32at",
-	.module = THIS_MODULE,
-	.attach = dmm32at_attach,
-	.detach = dmm32at_detach,
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-/* Most drivers will support multiple types of boards by
- * having an array of board structures.  These were defined
- * in dmm32at_boards[] above.  Note that the element 'name'
- * was first in the structure -- Comedi uses this fact to
- * extract the name of the board without knowing any details
- * about the structure except for its length.
- * When a device is attached (by comedi_config), the name
- * of the device is given to Comedi, and Comedi tries to
- * match it by going through the list of board names.  If
- * there is a match, the address of the pointer is put
- * into dev->board_ptr and driver->attach() is called.
- *
- * Note that these are not necessary if you can determine
- * the type of board in software.  ISA PnP, PCI, and PCMCIA
- * devices are such boards.
- */
-	.board_name = &dmm32at_boards[0].name,
-	.offset = sizeof(struct dmm32at_board),
-	.num_names = ARRAY_SIZE(dmm32at_boards),
-};
-
-/* prototypes for driver functions below */
-static int dmm32at_ai_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int dmm32at_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int dmm32at_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int dmm32at_dio_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int dmm32at_dio_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-static int dmm32at_ai_cmdtest(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_cmd *cmd);
-static int dmm32at_ai_cmd(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-static int dmm32at_ai_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s);
-static int dmm32at_ns_to_timer(unsigned int *ns, int round);
-static irqreturn_t dmm32at_isr(int irq, void *d);
-void dmm32at_setaitimer(struct comedi_device *dev, unsigned int nansec);
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int dmm32at_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	int ret;
-	struct comedi_subdevice *s;
-	unsigned char aihi, ailo, fifostat, aistat, intstat, airback;
-	unsigned long iobase;
-	unsigned int irq;
-
-	iobase = it->options[0];
-	irq = it->options[1];
-
-	printk(KERN_INFO "comedi%d: dmm32at: attaching\n", dev->minor);
-	printk(KERN_DEBUG "dmm32at: probing at address 0x%04lx, irq %u\n",
-	       iobase, irq);
-
-	/* register address space */
-	if (!request_region(iobase, DMM32AT_MEMSIZE, thisboard->name)) {
-		printk(KERN_ERR "comedi%d: dmm32at: I/O port conflict\n",
-		       dev->minor);
-		return -EIO;
-	}
-	dev->iobase = iobase;
-
-	/* the following just makes sure the board is there and gets
-	   it to a known state */
-
-	/* reset the board */
-	dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_RESET);
-
-	/* allow a millisecond to reset */
-	udelay(1000);
-
-	/* zero scan and fifo control */
-	dmm_outb(dev, DMM32AT_FIFOCNTRL, 0x0);
-
-	/* zero interrupt and clock control */
-	dmm_outb(dev, DMM32AT_INTCLOCK, 0x0);
-
-	/* write a test channel range, the high 3 bits should drop */
-	dmm_outb(dev, DMM32AT_AILOW, 0x80);
-	dmm_outb(dev, DMM32AT_AIHIGH, 0xff);
-
-	/* set the range at 10v unipolar */
-	dmm_outb(dev, DMM32AT_AICONF, DMM32AT_RANGE_U10);
-
-	/* should take 10 us to settle, here's a hundred */
-	udelay(100);
-
-	/* read back the values */
-	ailo = dmm_inb(dev, DMM32AT_AILOW);
-	aihi = dmm_inb(dev, DMM32AT_AIHIGH);
-	fifostat = dmm_inb(dev, DMM32AT_FIFOSTAT);
-	aistat = dmm_inb(dev, DMM32AT_AISTAT);
-	intstat = dmm_inb(dev, DMM32AT_INTCLOCK);
-	airback = dmm_inb(dev, DMM32AT_AIRBACK);
-
-	printk(KERN_DEBUG "dmm32at: lo=0x%02x hi=0x%02x fifostat=0x%02x\n",
-	       ailo, aihi, fifostat);
-	printk(KERN_DEBUG
-	       "dmm32at: aistat=0x%02x intstat=0x%02x airback=0x%02x\n",
-	       aistat, intstat, airback);
-
-	if ((ailo != 0x00) || (aihi != 0x1f) || (fifostat != 0x80) ||
-	    (aistat != 0x60 || (intstat != 0x00) || airback != 0x0c)) {
-		printk(KERN_ERR "dmmat32: board detection failed\n");
-		return -EIO;
-	}
-
-	/* board is there, register interrupt */
-	if (irq) {
-		ret = request_irq(irq, dmm32at_isr, 0, thisboard->name, dev);
-		if (ret < 0) {
-			printk(KERN_ERR "dmm32at: irq conflict\n");
-			return ret;
-		}
-		dev->irq = irq;
-	}
-
-/*
- * If you can probe the device to determine what device in a series
- * it is, this is the place to do it.  Otherwise, dev->board_ptr
- * should already be initialized.
- */
-	/* dev->board_ptr = dmm32at_probe(dev); */
-
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
-
-/*
- * Allocate the private structure area.  alloc_private() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_private(dev, sizeof(struct dmm32at_private)) < 0)
-		return -ENOMEM;
-
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	dev->read_subdev = s;
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	/* we support single-ended (ground) and differential */
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = thisboard->ai_ranges;
-	s->len_chanlist = 32;	/* This is the maximum chanlist length that
-				   the board can handle */
-	s->insn_read = dmm32at_ai_rinsn;
-	s->do_cmd = dmm32at_ai_cmd;
-	s->do_cmdtest = dmm32at_ai_cmdtest;
-	s->cancel = dmm32at_ai_cancel;
-
-	s = dev->subdevices + 1;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = (1 << thisboard->ao_bits) - 1;
-	s->range_table = thisboard->ao_ranges;
-	s->insn_write = dmm32at_ao_winsn;
-	s->insn_read = dmm32at_ao_rinsn;
-
-	s = dev->subdevices + 2;
-	/* digital i/o subdevice */
-	if (thisboard->have_dio) {
-
-		/* get access to the DIO regs */
-		dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_DIOACC);
-		/* set the DIO's to the defualt input setting */
-		devpriv->dio_config = DMM32AT_DIRA | DMM32AT_DIRB |
-		    DMM32AT_DIRCL | DMM32AT_DIRCH | DMM32AT_DIENABLE;
-		dmm_outb(dev, DMM32AT_DIOCONF, devpriv->dio_config);
-
-		/* set up the subdevice */
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = thisboard->dio_chans;
-		s->maxdata = 1;
-		s->state = 0;
-		s->range_table = &range_digital;
-		s->insn_bits = dmm32at_dio_insn_bits;
-		s->insn_config = dmm32at_dio_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	/* success */
-	printk(KERN_INFO "comedi%d: dmm32at: attached\n", dev->minor);
-
-	return 1;
-
-}
-
-static void dmm32at_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->iobase)
-		release_region(dev->iobase, DMM32AT_MEMSIZE);
-}
-
-/*
- * "instructions" read/write data in "one-shot" or "software-triggered"
- * mode.
- */
-
 static int dmm32at_ai_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
@@ -482,17 +191,17 @@ static int dmm32at_ai_rinsn(struct comedi_device *dev,
 	/* printk("channel=0x%02x, range=%d\n",chan,range); */
 
 	/* zero scan and fifo control and reset fifo */
-	dmm_outb(dev, DMM32AT_FIFOCNTRL, DMM32AT_FIFORESET);
+	outb(DMM32AT_FIFORESET, dev->iobase + DMM32AT_FIFOCNTRL);
 
 	/* write the ai channel range regs */
-	dmm_outb(dev, DMM32AT_AILOW, chan);
-	dmm_outb(dev, DMM32AT_AIHIGH, chan);
+	outb(chan, dev->iobase + DMM32AT_AILOW);
+	outb(chan, dev->iobase + DMM32AT_AIHIGH);
 	/* set the range bits */
-	dmm_outb(dev, DMM32AT_AICONF, dmm32at_rangebits[range]);
+	outb(dmm32at_rangebits[range], dev->iobase + DMM32AT_AICONF);
 
 	/* wait for circuit to settle */
 	for (i = 0; i < 40000; i++) {
-		status = dmm_inb(dev, DMM32AT_AIRBACK);
+		status = inb(dev->iobase + DMM32AT_AIRBACK);
 		if ((status & DMM32AT_STATUS) == 0)
 			break;
 	}
@@ -504,10 +213,10 @@ static int dmm32at_ai_rinsn(struct comedi_device *dev,
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
 		/* trigger conversion */
-		dmm_outb(dev, DMM32AT_CONV, 0xff);
+		outb(0xff, dev->iobase + DMM32AT_CONV);
 		/* wait for conversion to end */
 		for (i = 0; i < 40000; i++) {
-			status = dmm_inb(dev, DMM32AT_AISTAT);
+			status = inb(dev->iobase + DMM32AT_AISTAT);
 			if ((status & DMM32AT_STATUS) == 0)
 				break;
 		}
@@ -517,8 +226,8 @@ static int dmm32at_ai_rinsn(struct comedi_device *dev,
 		}
 
 		/* read data */
-		lsb = dmm_inb(dev, DMM32AT_AILSB);
-		msb = dmm_inb(dev, DMM32AT_AIMSB);
+		lsb = inb(dev->iobase + DMM32AT_AILSB);
+		msb = inb(dev->iobase + DMM32AT_AIMSB);
 
 		/* invert sign bit to make range unsigned, this is an
 		   idiosyncrasy of the diamond board, it return
@@ -535,6 +244,12 @@ static int dmm32at_ai_rinsn(struct comedi_device *dev,
 	return n;
 }
 
+static int dmm32at_ns_to_timer(unsigned int *ns, int round)
+{
+	/* trivial timer */
+	return *ns;
+}
+
 static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
@@ -543,15 +258,6 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	int tmp;
 	int start_chan, gain, i;
 
-	/* printk("dmmat32 in command test\n"); */
-
-	/* cmdtest tests a particular command to see if it is valid.
-	 * Using the cmdtest ioctl, a user can create a valid cmd
-	 * and then have it executes by the cmd ioctl.
-	 *
-	 * cmdtest returns 1,2,3,4 or 0, depending on which tests
-	 * the command passes. */
-
 	/* step 1: make sure trigger sources are trivially valid */
 
 	tmp = cmd->start_src;
@@ -721,8 +427,39 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+static void dmm32at_setaitimer(struct comedi_device *dev, unsigned int nansec)
+{
+	unsigned char lo1, lo2, hi2;
+	unsigned short both2;
+
+	/* based on 10mhz clock */
+	lo1 = 200;
+	both2 = nansec / 20000;
+	hi2 = (both2 & 0xff00) >> 8;
+	lo2 = both2 & 0x00ff;
+
+	/* set the counter frequency to 10mhz */
+	outb(0, dev->iobase + DMM32AT_CNTRDIO);
+
+	/* get access to the clock regs */
+	outb(DMM32AT_CLKACC, dev->iobase + DMM32AT_CNTRL);
+
+	/* write the counter 1 control word and low byte to counter */
+	outb(DMM32AT_CLKCT1, dev->iobase + DMM32AT_CLKCT);
+	outb(lo1, dev->iobase + DMM32AT_CLK1);
+
+	/* write the counter 2 control word and low byte then to counter */
+	outb(DMM32AT_CLKCT2, dev->iobase + DMM32AT_CLKCT);
+	outb(lo2, dev->iobase + DMM32AT_CLK2);
+	outb(hi2, dev->iobase + DMM32AT_CLK2);
+
+	/* enable the ai conversion interrupt and the clock to start scans */
+	outb(DMM32AT_ADINT | DMM32AT_CLKSEL, dev->iobase + DMM32AT_INTCLOCK);
+}
+
 static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct dmm32at_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int i, range;
 	unsigned char chanlo, chanhi, status;
@@ -738,20 +475,20 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	range = CR_RANGE(cmd->chanlist[0]);
 
 	/* reset fifo */
-	dmm_outb(dev, DMM32AT_FIFOCNTRL, DMM32AT_FIFORESET);
+	outb(DMM32AT_FIFORESET, dev->iobase + DMM32AT_FIFOCNTRL);
 
 	/* set scan enable */
-	dmm_outb(dev, DMM32AT_FIFOCNTRL, DMM32AT_SCANENABLE);
+	outb(DMM32AT_SCANENABLE, dev->iobase + DMM32AT_FIFOCNTRL);
 
 	/* write the ai channel range regs */
-	dmm_outb(dev, DMM32AT_AILOW, chanlo);
-	dmm_outb(dev, DMM32AT_AIHIGH, chanhi);
+	outb(chanlo, dev->iobase + DMM32AT_AILOW);
+	outb(chanhi, dev->iobase + DMM32AT_AIHIGH);
 
 	/* set the range bits */
-	dmm_outb(dev, DMM32AT_AICONF, dmm32at_rangebits[range]);
+	outb(dmm32at_rangebits[range], dev->iobase + DMM32AT_AICONF);
 
 	/* reset the interrupt just in case */
-	dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_INTRESET);
+	outb(DMM32AT_INTRESET, dev->iobase + DMM32AT_CNTRL);
 
 	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_scans_left = cmd->stop_arg;
@@ -762,7 +499,7 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	/* wait for circuit to settle */
 	for (i = 0; i < 40000; i++) {
-		status = dmm_inb(dev, DMM32AT_AIRBACK);
+		status = inb(dev->iobase + DMM32AT_AIRBACK);
 		if ((status & DMM32AT_STATUS) == 0)
 			break;
 	}
@@ -776,8 +513,8 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		dmm32at_setaitimer(dev, cmd->scan_begin_arg);
 	} else {
 		/* start the interrups and initiate a single scan */
-		dmm_outb(dev, DMM32AT_INTCLOCK, DMM32AT_ADINT);
-		dmm_outb(dev, DMM32AT_CONV, 0xff);
+		outb(DMM32AT_ADINT, dev->iobase + DMM32AT_INTCLOCK);
+		outb(0xff, dev->iobase + DMM32AT_CONV);
 	}
 
 /*	printk("dmmat32 in command\n"); */
@@ -795,24 +532,27 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static int dmm32at_ai_cancel(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
+	struct dmm32at_private *devpriv = dev->private;
+
 	devpriv->ai_scans_left = 1;
 	return 0;
 }
 
 static irqreturn_t dmm32at_isr(int irq, void *d)
 {
+	struct comedi_device *dev = d;
+	struct dmm32at_private *devpriv = dev->private;
 	unsigned char intstat;
 	unsigned int samp;
 	unsigned short msb, lsb;
 	int i;
-	struct comedi_device *dev = d;
 
 	if (!dev->attached) {
 		comedi_error(dev, "spurious interrupt");
 		return IRQ_HANDLED;
 	}
 
-	intstat = dmm_inb(dev, DMM32AT_INTCLOCK);
+	intstat = inb(dev->iobase + DMM32AT_INTCLOCK);
 
 	if (intstat & DMM32AT_ADINT) {
 		struct comedi_subdevice *s = dev->read_subdev;
@@ -820,8 +560,8 @@ static irqreturn_t dmm32at_isr(int irq, void *d)
 
 		for (i = 0; i < cmd->chanlist_len; i++) {
 			/* read data */
-			lsb = dmm_inb(dev, DMM32AT_AILSB);
-			msb = dmm_inb(dev, DMM32AT_AIMSB);
+			lsb = inb(dev->iobase + DMM32AT_AILSB);
+			msb = inb(dev->iobase + DMM32AT_AIMSB);
 
 			/* invert sign bit to make range unsigned */
 			samp = ((msb ^ 0x0080) << 8) + lsb;
@@ -832,7 +572,7 @@ static irqreturn_t dmm32at_isr(int irq, void *d)
 			devpriv->ai_scans_left--;
 			if (devpriv->ai_scans_left == 0) {
 				/* disable further interrupts and clocks */
-				dmm_outb(dev, DMM32AT_INTCLOCK, 0x0);
+				outb(0x0, dev->iobase + DMM32AT_INTCLOCK);
 				/* set the buffer to be flushed with an EOF */
 				s->async->events |= COMEDI_CB_EOA;
 			}
@@ -843,31 +583,15 @@ static irqreturn_t dmm32at_isr(int irq, void *d)
 	}
 
 	/* reset the interrupt */
-	dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_INTRESET);
+	outb(DMM32AT_INTRESET, dev->iobase + DMM32AT_CNTRL);
 	return IRQ_HANDLED;
 }
 
-/* This function doesn't require a particular form, this is just
- * what happens to be used in some of the drivers.  It should
- * convert ns nanoseconds to a counter value suitable for programming
- * the device.  Also, it should adjust ns so that it cooresponds to
- * the actual time that the device will use. */
-static int dmm32at_ns_to_timer(unsigned int *ns, int round)
-{
-	/* trivial timer */
-	/* if your timing is done through two cascaded timers, the
-	 * i8253_cascade_ns_to_timer() function in 8253.h can be
-	 * very helpful.  There are also i8254_load() and i8254_mm_load()
-	 * which can be used to load values into the ubiquitous 8254 counters
-	 */
-
-	return *ns;
-}
-
 static int dmm32at_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct dmm32at_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 	unsigned char hi, lo, status;
@@ -884,12 +608,12 @@ static int dmm32at_ao_winsn(struct comedi_device *dev,
 		hi = (data[i] >> 8) + chan * (1 << 6);
 		/* printk("writing 0x%02x  0x%02x\n",hi,lo); */
 		/* write the low and high values to the board */
-		dmm_outb(dev, DMM32AT_DACLSB, lo);
-		dmm_outb(dev, DMM32AT_DACMSB, hi);
+		outb(lo, dev->iobase + DMM32AT_DACLSB);
+		outb(hi, dev->iobase + DMM32AT_DACMSB);
 
 		/* wait for circuit to settle */
 		for (i = 0; i < 40000; i++) {
-			status = dmm_inb(dev, DMM32AT_DACSTAT);
+			status = inb(dev->iobase + DMM32AT_DACSTAT);
 			if ((status & DMM32AT_DACBUSY) == 0)
 				break;
 		}
@@ -898,7 +622,7 @@ static int dmm32at_ao_winsn(struct comedi_device *dev,
 			return -ETIMEDOUT;
 		}
 		/* dummy read to update trigger the output */
-		status = dmm_inb(dev, DMM32AT_DACMSB);
+		status = inb(dev->iobase + DMM32AT_DACMSB);
 
 	}
 
@@ -906,12 +630,11 @@ static int dmm32at_ao_winsn(struct comedi_device *dev,
 	return i;
 }
 
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
 static int dmm32at_ao_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct dmm32at_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -921,20 +644,13 @@ static int dmm32at_ao_rinsn(struct comedi_device *dev,
 	return i;
 }
 
-/* DIO devices are slightly special.  Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels.  The
- * comedi core can convert between insn_bits and insn_read/write */
 static int dmm32at_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
+	struct dmm32at_private *devpriv = dev->private;
 	unsigned char diobits;
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	/* The insn data is a mask in data[0] and the new data
 	 * in data[1], each channel cooresponding to a bit. */
 	if (data[0]) {
@@ -945,29 +661,29 @@ static int dmm32at_dio_insn_bits(struct comedi_device *dev,
 	}
 
 	/* get access to the DIO regs */
-	dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_DIOACC);
+	outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
 
 	/* if either part of dio is set for output */
 	if (((devpriv->dio_config & DMM32AT_DIRCL) == 0) ||
 	    ((devpriv->dio_config & DMM32AT_DIRCH) == 0)) {
 		diobits = (s->state & 0x00ff0000) >> 16;
-		dmm_outb(dev, DMM32AT_DIOC, diobits);
+		outb(diobits, dev->iobase + DMM32AT_DIOC);
 	}
 	if ((devpriv->dio_config & DMM32AT_DIRB) == 0) {
 		diobits = (s->state & 0x0000ff00) >> 8;
-		dmm_outb(dev, DMM32AT_DIOB, diobits);
+		outb(diobits, dev->iobase + DMM32AT_DIOB);
 	}
 	if ((devpriv->dio_config & DMM32AT_DIRA) == 0) {
 		diobits = (s->state & 0x000000ff);
-		dmm_outb(dev, DMM32AT_DIOA, diobits);
+		outb(diobits, dev->iobase + DMM32AT_DIOA);
 	}
 
 	/* now read the state back in */
-	s->state = dmm_inb(dev, DMM32AT_DIOC);
+	s->state = inb(dev->iobase + DMM32AT_DIOC);
 	s->state <<= 8;
-	s->state |= dmm_inb(dev, DMM32AT_DIOB);
+	s->state |= inb(dev->iobase + DMM32AT_DIOB);
 	s->state <<= 8;
-	s->state |= dmm_inb(dev, DMM32AT_DIOA);
+	s->state |= inb(dev->iobase + DMM32AT_DIOA);
 	data[1] = s->state;
 
 	/* on return, data[1] contains the value of the digital
@@ -977,13 +693,14 @@ static int dmm32at_dio_insn_bits(struct comedi_device *dev,
 	 * it was a purely digital output subdevice */
 	/* data[1]=s->state; */
 
-	return 2;
+	return insn->n;
 }
 
 static int dmm32at_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
+	struct dmm32at_private *devpriv = dev->private;
 	unsigned char chanbit;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -1010,60 +727,181 @@ static int dmm32at_dio_insn_config(struct comedi_device *dev,
 	else
 		devpriv->dio_config |= chanbit;
 	/* get access to the DIO regs */
-	dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_DIOACC);
+	outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
 	/* set the DIO's to the new configuration setting */
-	dmm_outb(dev, DMM32AT_DIOCONF, devpriv->dio_config);
+	outb(devpriv->dio_config, dev->iobase + DMM32AT_DIOCONF);
 
 	return 1;
 }
 
-void dmm32at_setaitimer(struct comedi_device *dev, unsigned int nansec)
+static int dmm32at_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
 {
-	unsigned char lo1, lo2, hi2;
-	unsigned short both2;
+	const struct dmm32at_board *board = comedi_board(dev);
+	struct dmm32at_private *devpriv;
+	int ret;
+	struct comedi_subdevice *s;
+	unsigned char aihi, ailo, fifostat, aistat, intstat, airback;
+	unsigned long iobase;
+	unsigned int irq;
 
-	/* based on 10mhz clock */
-	lo1 = 200;
-	both2 = nansec / 20000;
-	hi2 = (both2 & 0xff00) >> 8;
-	lo2 = both2 & 0x00ff;
+	iobase = it->options[0];
+	irq = it->options[1];
 
-	/* set the counter frequency to 10mhz */
-	dmm_outb(dev, DMM32AT_CNTRDIO, 0);
+	printk(KERN_INFO "comedi%d: dmm32at: attaching\n", dev->minor);
+	printk(KERN_DEBUG "dmm32at: probing at address 0x%04lx, irq %u\n",
+	       iobase, irq);
 
-	/* get access to the clock regs */
-	dmm_outb(dev, DMM32AT_CNTRL, DMM32AT_CLKACC);
+	/* register address space */
+	if (!request_region(iobase, DMM32AT_MEMSIZE, board->name)) {
+		printk(KERN_ERR "comedi%d: dmm32at: I/O port conflict\n",
+		       dev->minor);
+		return -EIO;
+	}
+	dev->iobase = iobase;
 
-	/* write the counter 1 control word and low byte to counter */
-	dmm_outb(dev, DMM32AT_CLKCT, DMM32AT_CLKCT1);
-	dmm_outb(dev, DMM32AT_CLK1, lo1);
+	/* the following just makes sure the board is there and gets
+	   it to a known state */
 
-	/* write the counter 2 control word and low byte then to counter */
-	dmm_outb(dev, DMM32AT_CLKCT, DMM32AT_CLKCT2);
-	dmm_outb(dev, DMM32AT_CLK2, lo2);
-	dmm_outb(dev, DMM32AT_CLK2, hi2);
+	/* reset the board */
+	outb(DMM32AT_RESET, dev->iobase + DMM32AT_CNTRL);
 
-	/* enable the ai conversion interrupt and the clock to start scans */
-	dmm_outb(dev, DMM32AT_INTCLOCK, DMM32AT_ADINT | DMM32AT_CLKSEL);
+	/* allow a millisecond to reset */
+	udelay(1000);
 
-}
+	/* zero scan and fifo control */
+	outb(0x0, dev->iobase + DMM32AT_FIFOCNTRL);
+
+	/* zero interrupt and clock control */
+	outb(0x0, dev->iobase + DMM32AT_INTCLOCK);
+
+	/* write a test channel range, the high 3 bits should drop */
+	outb(0x80, dev->iobase + DMM32AT_AILOW);
+	outb(0xff, dev->iobase + DMM32AT_AIHIGH);
+
+	/* set the range at 10v unipolar */
+	outb(DMM32AT_RANGE_U10, dev->iobase + DMM32AT_AICONF);
+
+	/* should take 10 us to settle, here's a hundred */
+	udelay(100);
+
+	/* read back the values */
+	ailo = inb(dev->iobase + DMM32AT_AILOW);
+	aihi = inb(dev->iobase + DMM32AT_AIHIGH);
+	fifostat = inb(dev->iobase + DMM32AT_FIFOSTAT);
+	aistat = inb(dev->iobase + DMM32AT_AISTAT);
+	intstat = inb(dev->iobase + DMM32AT_INTCLOCK);
+	airback = inb(dev->iobase + DMM32AT_AIRBACK);
+
+	printk(KERN_DEBUG "dmm32at: lo=0x%02x hi=0x%02x fifostat=0x%02x\n",
+	       ailo, aihi, fifostat);
+	printk(KERN_DEBUG
+	       "dmm32at: aistat=0x%02x intstat=0x%02x airback=0x%02x\n",
+	       aistat, intstat, airback);
+
+	if ((ailo != 0x00) || (aihi != 0x1f) || (fifostat != 0x80) ||
+	    (aistat != 0x60 || (intstat != 0x00) || airback != 0x0c)) {
+		printk(KERN_ERR "dmmat32: board detection failed\n");
+		return -EIO;
+	}
+
+	/* board is there, register interrupt */
+	if (irq) {
+		ret = request_irq(irq, dmm32at_isr, 0, board->name, dev);
+		if (ret < 0) {
+			printk(KERN_ERR "dmm32at: irq conflict\n");
+			return ret;
+		}
+		dev->irq = irq;
+	}
+
+	dev->board_name = board->name;
+
+	if (alloc_private(dev, sizeof(*devpriv)) < 0)
+		return -ENOMEM;
+	devpriv = dev->private;
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	/* we support single-ended (ground) and differential */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan = 32;
+	s->maxdata = 0xffff;
+	s->range_table = &dmm32at_airanges;
+	s->len_chanlist = 32;	/* This is the maximum chanlist length that
+				   the board can handle */
+	s->insn_read = dmm32at_ai_rinsn;
+	s->do_cmd = dmm32at_ai_cmd;
+	s->do_cmdtest = dmm32at_ai_cmdtest;
+	s->cancel = dmm32at_ai_cancel;
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 0x0fff;
+	s->range_table = &dmm32at_aoranges;
+	s->insn_write = dmm32at_ao_winsn;
+	s->insn_read = dmm32at_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital i/o subdevice */
+
+	/* get access to the DIO regs */
+	outb(DMM32AT_DIOACC, dev->iobase + DMM32AT_CNTRL);
+	/* set the DIO's to the defualt input setting */
+	devpriv->dio_config = DMM32AT_DIRA | DMM32AT_DIRB |
+		DMM32AT_DIRCL | DMM32AT_DIRCH | DMM32AT_DIENABLE;
+	outb(devpriv->dio_config, dev->iobase + DMM32AT_DIOCONF);
+
+	/* set up the subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 24;
+	s->maxdata = 1;
+	s->state = 0;
+	s->range_table = &range_digital;
+	s->insn_bits = dmm32at_dio_insn_bits;
+	s->insn_config = dmm32at_dio_insn_config;
+
+	/* success */
+	printk(KERN_INFO "comedi%d: dmm32at: attached\n", dev->minor);
+
+	return 1;
 
-/*
- * A convenient macro that defines init_module() and cleanup_module(),
- * as necessary.
- */
-static int __init driver_dmm32at_init_module(void)
-{
-	return comedi_driver_register(&driver_dmm32at);
 }
 
-static void __exit driver_dmm32at_cleanup_module(void)
+static void dmm32at_detach(struct comedi_device *dev)
 {
-	comedi_driver_unregister(&driver_dmm32at);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+	if (dev->iobase)
+		release_region(dev->iobase, DMM32AT_MEMSIZE);
 }
 
-module_init(driver_dmm32at_init_module);
-module_exit(driver_dmm32at_cleanup_module);
+static const struct dmm32at_board dmm32at_boards[] = {
+	{
+		.name		= "dmm32at",
+	},
+};
+
+static struct comedi_driver dmm32at_driver = {
+	.driver_name	= "dmm32at",
+	.module		= THIS_MODULE,
+	.attach		= dmm32at_attach,
+	.detach		= dmm32at_detach,
+	.board_name	= &dmm32at_boards[0].name,
+	.offset		= sizeof(struct dmm32at_board),
+	.num_names	= ARRAY_SIZE(dmm32at_boards),
+};
+module_comedi_driver(dmm32at_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index 625bd617a8e..d332269375a 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -535,8 +535,6 @@ static int dt2801_dio_insn_bits(struct comedi_device *dev,
 	if (s == dev->subdevices + 4)
 		which = 1;
 
-	if (insn->n != 2)
-		return -EINVAL;
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -548,7 +546,7 @@ static int dt2801_dio_insn_bits(struct comedi_device *dev,
 	dt2801_writedata(dev, which);
 	dt2801_readdata(dev, data + 1);
 
-	return 2;
+	return insn->n;
 }
 
 static int dt2801_dio_insn_config(struct comedi_device *dev,
@@ -626,15 +624,15 @@ havetype:
 	printk("dt2801: %s at port 0x%lx", boardtype.name, iobase);
 
 	n_ai_chans = probe_number_of_ai_chans(dev);
-	printk(" (ai channels = %d)", n_ai_chans);
+	printk(" (ai channels = %d)\n", n_ai_chans);
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		goto out;
 
 	ret = alloc_private(dev, sizeof(struct dt2801_private));
 	if (ret < 0)
-		goto out;
+		return ret;
 
 	dev->board_name = boardtype.name;
 
@@ -688,8 +686,6 @@ havetype:
 
 	ret = 0;
 out:
-	printk("\n");
-
 	return ret;
 }
 
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index 106ffea30b9..290b933c5f9 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -211,8 +211,6 @@ struct dt2811_board {
 	const struct comedi_lrange *unip_5;
 };
 
-#define this_board ((const struct dt2811_board *)dev->board_ptr)
-
 enum { card_2811_pgh, card_2811_pgl };
 
 struct dt2811_private {
@@ -354,28 +352,22 @@ static int dt2811_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + DT2811_DIO);
 
-	return 2;
+	return insn->n;
 }
 
 static int dt2811_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	s->state &= ~data[0];
 	s->state |= data[0] & data[1];
 	outb(s->state, dev->iobase + DT2811_DIO);
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -404,6 +396,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* unsigned long irqs; */
 	/* long flags; */
 
+	const struct dt2811_board *board = comedi_board(dev);
 	int ret;
 	struct comedi_subdevice *s;
 	unsigned long iobase;
@@ -418,7 +411,7 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	dev->iobase = iobase;
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 #if 0
 	outb(0, dev->iobase + DT2811_ADCSR);
@@ -466,8 +459,8 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 #endif
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct dt2811_private));
@@ -527,13 +520,13 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	switch (it->options[3]) {
 	case 0:
 	default:
-		s->range_table = this_board->bip_5;
+		s->range_table = board->bip_5;
 		break;
 	case 1:
-		s->range_table = this_board->bip_2_5;
+		s->range_table = board->bip_2_5;
 		break;
 	case 2:
-		s->range_table = this_board->unip_5;
+		s->range_table = board->unip_5;
 		break;
 	}
 
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index fa4ade61be5..2e39ebe36fb 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -338,8 +338,8 @@ static int dt2814_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 	}
 
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct dt2814_private));
diff --git a/drivers/staging/comedi/drivers/dt2815.c b/drivers/staging/comedi/drivers/dt2815.c
index bbab712be4b..45b20bee436 100644
--- a/drivers/staging/comedi/drivers/dt2815.c
+++ b/drivers/staging/comedi/drivers/dt2815.c
@@ -166,6 +166,7 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	int i;
 	const struct comedi_lrange *current_range_type, *voltage_range_type;
 	unsigned long iobase;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: dt2815: 0x%04lx ", dev->minor, iobase);
@@ -177,8 +178,10 @@ static int dt2815_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->iobase = iobase;
 	dev->board_name = "dt2815";
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
+
 	if (alloc_private(dev, sizeof(struct dt2815_private)) < 0)
 		return -ENOMEM;
 
diff --git a/drivers/staging/comedi/drivers/dt2817.c b/drivers/staging/comedi/drivers/dt2817.c
index 1ee10e7bf1d..beba0447b3e 100644
--- a/drivers/staging/comedi/drivers/dt2817.c
+++ b/drivers/staging/comedi/drivers/dt2817.c
@@ -119,7 +119,7 @@ static int dt2817_dio_insn_bits(struct comedi_device *dev,
 	data[1] |= (inb(dev->iobase + DT2817_DATA + 2) << 16);
 	data[1] |= (inb(dev->iobase + DT2817_DATA + 3) << 24);
 
-	return 2;
+	return insn->n;
 }
 
 static int dt2817_attach(struct comedi_device *dev, struct comedi_devconfig *it)
@@ -137,8 +137,8 @@ static int dt2817_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->iobase = iobase;
 	dev->board_name = "dt2817";
 
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 736d8facaee..1f0b40e4bdd 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -221,8 +221,6 @@ struct dt282x_board {
 	int dabits;
 };
 
-#define this_board ((const struct dt282x_board *)dev->board_ptr)
-
 struct dt282x_private {
 	int ad_2scomp;		/* we have 2's comp jumper set  */
 	int da0_2scomp;		/* same, for DAC0               */
@@ -257,11 +255,8 @@ struct dt282x_private {
  *    Some useless abstractions
  */
 #define chan_to_DAC(a)	((a)&1)
-#define update_dacsr(a)	outw(devpriv->dacsr|(a), dev->iobase+DT2821_DACSR)
-#define update_adcsr(a)	outw(devpriv->adcsr|(a), dev->iobase+DT2821_ADCSR)
 #define mux_busy() (inw(dev->iobase+DT2821_ADCSR)&DT2821_MUXBUSY)
 #define ad_done() (inw(dev->iobase+DT2821_ADCSR)&DT2821_ADDONE)
-#define update_supcsr(a) outw(devpriv->supcsr|(a), dev->iobase+DT2821_SUPCSR)
 
 /*
  *    danger! macro abuse... a is the expression to wait on, and b is
@@ -319,7 +314,7 @@ static void dt282x_ao_dma_interrupt(struct comedi_device *dev)
 	int i;
 	struct comedi_subdevice *s = dev->subdevices + 1;
 
-	update_supcsr(DT2821_CLRDMADNE);
+	outw(devpriv->supcsr | DT2821_CLRDMADNE, dev->iobase + DT2821_SUPCSR);
 
 	if (!s->async->prealloc_buf) {
 		printk(KERN_ERR "async->data disappeared.  dang!\n");
@@ -352,7 +347,7 @@ static void dt282x_ai_dma_interrupt(struct comedi_device *dev)
 	int ret;
 	struct comedi_subdevice *s = dev->subdevices;
 
-	update_supcsr(DT2821_CLRDMADNE);
+	outw(devpriv->supcsr | DT2821_CLRDMADNE, dev->iobase + DT2821_SUPCSR);
 
 	if (!s->async->prealloc_buf) {
 		printk(KERN_ERR "async->data disappeared.  dang!\n");
@@ -389,7 +384,7 @@ static void dt282x_ai_dma_interrupt(struct comedi_device *dev)
 	/* XXX probably wrong */
 	if (!devpriv->ntrig) {
 		devpriv->supcsr &= ~(DT2821_DDMA);
-		update_supcsr(0);
+		outw(devpriv->supcsr, dev->iobase + DT2821_SUPCSR);
 	}
 #endif
 	/* restart the channel */
@@ -515,7 +510,8 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 			s->async->events |= COMEDI_CB_EOA;
 		} else {
 			if (supcsr & DT2821_SCDN)
-				update_supcsr(DT2821_STRIG);
+				outw(devpriv->supcsr | DT2821_STRIG,
+					dev->iobase + DT2821_SUPCSR);
 		}
 		handled = 1;
 	}
@@ -536,7 +532,8 @@ static void dt282x_load_changain(struct comedi_device *dev, int n,
 	for (i = 0; i < n; i++) {
 		chan = CR_CHAN(chanlist[i]);
 		range = CR_RANGE(chanlist[i]);
-		update_adcsr((range << 4) | (chan));
+		outw(devpriv->adcsr | (range << 4) | chan,
+			dev->iobase + DT2821_ADCSR);
 	}
 	outw(n - 1, dev->iobase + DT2821_CHANCSR);
 }
@@ -555,15 +552,16 @@ static int dt282x_ai_insn_read(struct comedi_device *dev,
 
 	/* XXX should we really be enabling the ad clock here? */
 	devpriv->adcsr = DT2821_ADCLK;
-	update_adcsr(0);
+	outw(devpriv->adcsr, dev->iobase + DT2821_ADCSR);
 
 	dt282x_load_changain(dev, 1, &insn->chanspec);
 
-	update_supcsr(DT2821_PRLD);
+	outw(devpriv->supcsr | DT2821_PRLD, dev->iobase + DT2821_SUPCSR);
 	wait_for(!mux_busy(), comedi_error(dev, "timeout\n"); return -ETIME;);
 
 	for (i = 0; i < insn->n; i++) {
-		update_supcsr(DT2821_STRIG);
+		outw(devpriv->supcsr | DT2821_STRIG,
+			dev->iobase + DT2821_SUPCSR);
 		wait_for(ad_done(), comedi_error(dev, "timeout\n");
 			 return -ETIME;);
 
@@ -580,6 +578,7 @@ static int dt282x_ai_insn_read(struct comedi_device *dev,
 static int dt282x_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct dt282x_board *board = comedi_board(dev);
 	int err = 0;
 	int tmp;
 
@@ -658,8 +657,8 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 		cmd->convert_arg = SLOWEST_TIMER;
 		err++;
 	}
-	if (cmd->convert_arg < this_board->ai_speed) {
-		cmd->convert_arg = this_board->ai_speed;
+	if (cmd->convert_arg < board->ai_speed) {
+		cmd->convert_arg = board->ai_speed;
 		err++;
 	}
 	if (cmd->scan_end_arg != cmd->chanlist_len) {
@@ -694,6 +693,7 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 
 static int dt282x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct dt282x_board *board = comedi_board(dev);
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int timer;
 
@@ -706,8 +706,8 @@ static int dt282x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	dt282x_disable_dma(dev);
 
-	if (cmd->convert_arg < this_board->ai_speed)
-		cmd->convert_arg = this_board->ai_speed;
+	if (cmd->convert_arg < board->ai_speed)
+		cmd->convert_arg = board->ai_speed;
 	timer = dt282x_ns_to_timer(&cmd->convert_arg, TRIG_ROUND_NEAREST);
 	outw(timer, dev->iobase + DT2821_TMRCTR);
 
@@ -718,7 +718,8 @@ static int dt282x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/* external trigger */
 		devpriv->supcsr = DT2821_ERRINTEN | DT2821_DS0 | DT2821_DS1;
 	}
-	update_supcsr(DT2821_CLRDMADNE | DT2821_BUFFB | DT2821_ADCINIT);
+	outw(devpriv->supcsr | DT2821_CLRDMADNE | DT2821_BUFFB | DT2821_ADCINIT,
+		dev->iobase + DT2821_SUPCSR);
 
 	devpriv->ntrig = cmd->stop_arg * cmd->scan_end_arg;
 	devpriv->nread = devpriv->ntrig;
@@ -729,7 +730,7 @@ static int dt282x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (devpriv->ntrig) {
 		prep_ai_dma(dev, 1, 0);
 		devpriv->supcsr |= DT2821_DDMA;
-		update_supcsr(0);
+		outw(devpriv->supcsr, dev->iobase + DT2821_SUPCSR);
 	}
 
 	devpriv->adcsr = 0;
@@ -737,16 +738,17 @@ static int dt282x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	dt282x_load_changain(dev, cmd->chanlist_len, cmd->chanlist);
 
 	devpriv->adcsr = DT2821_ADCLK | DT2821_IADDONE;
-	update_adcsr(0);
+	outw(devpriv->adcsr, dev->iobase + DT2821_ADCSR);
 
-	update_supcsr(DT2821_PRLD);
+	outw(devpriv->supcsr | DT2821_PRLD, dev->iobase + DT2821_SUPCSR);
 	wait_for(!mux_busy(), comedi_error(dev, "timeout\n"); return -ETIME;);
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW) {
-		update_supcsr(DT2821_STRIG);
+		outw(devpriv->supcsr | DT2821_STRIG,
+			dev->iobase + DT2821_SUPCSR);
 	} else {
 		devpriv->supcsr |= DT2821_XTRIG;
-		update_supcsr(0);
+		outw(devpriv->supcsr, dev->iobase + DT2821_SUPCSR);
 	}
 
 	return 0;
@@ -766,10 +768,10 @@ static int dt282x_ai_cancel(struct comedi_device *dev,
 	dt282x_disable_dma(dev);
 
 	devpriv->adcsr = 0;
-	update_adcsr(0);
+	outw(devpriv->adcsr, dev->iobase + DT2821_ADCSR);
 
 	devpriv->supcsr = 0;
-	update_supcsr(DT2821_ADCINIT);
+	outw(devpriv->supcsr | DT2821_ADCINIT, dev->iobase + DT2821_SUPCSR);
 
 	return 0;
 }
@@ -845,11 +847,11 @@ static int dt282x_ao_insn_write(struct comedi_device *dev,
 			d ^= (1 << (boardtype.dabits - 1));
 	}
 
-	update_dacsr(0);
+	outw(devpriv->dacsr, dev->iobase + DT2821_DACSR);
 
 	outw(d, dev->iobase + DT2821_DADAT);
 
-	update_supcsr(DT2821_DACON);
+	outw(devpriv->supcsr | DT2821_DACON, dev->iobase + DT2821_SUPCSR);
 
 	return 1;
 }
@@ -972,7 +974,7 @@ static int dt282x_ao_inttrig(struct comedi_device *dev,
 	}
 	prep_ao_dma(dev, 1, size);
 
-	update_supcsr(DT2821_STRIG);
+	outw(devpriv->supcsr | DT2821_STRIG, dev->iobase + DT2821_SUPCSR);
 	s->async->inttrig = NULL;
 
 	return 1;
@@ -993,7 +995,8 @@ static int dt282x_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	dt282x_disable_dma(dev);
 
 	devpriv->supcsr = DT2821_ERRINTEN | DT2821_DS1 | DT2821_DDMA;
-	update_supcsr(DT2821_CLRDMADNE | DT2821_BUFFB | DT2821_DACINIT);
+	outw(devpriv->supcsr | DT2821_CLRDMADNE | DT2821_BUFFB | DT2821_DACINIT,
+		dev->iobase + DT2821_SUPCSR);
 
 	devpriv->ntrig = cmd->stop_arg * cmd->chanlist_len;
 	devpriv->nread = devpriv->ntrig;
@@ -1005,7 +1008,7 @@ static int dt282x_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(timer, dev->iobase + DT2821_TMRCTR);
 
 	devpriv->dacsr = DT2821_SSEL | DT2821_DACLK | DT2821_IDARDY;
-	update_dacsr(0);
+	outw(devpriv->dacsr, dev->iobase + DT2821_DACSR);
 
 	s->async->inttrig = dt282x_ao_inttrig;
 
@@ -1018,10 +1021,10 @@ static int dt282x_ao_cancel(struct comedi_device *dev,
 	dt282x_disable_dma(dev);
 
 	devpriv->dacsr = 0;
-	update_dacsr(0);
+	outw(devpriv->dacsr, dev->iobase + DT2821_DACSR);
 
 	devpriv->supcsr = 0;
-	update_supcsr(DT2821_DACINIT);
+	outw(devpriv->supcsr | DT2821_DACINIT, dev->iobase + DT2821_SUPCSR);
 
 	return 0;
 }
@@ -1038,7 +1041,7 @@ static int dt282x_dio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = inw(dev->iobase + DT2821_DIODAT);
 
-	return 2;
+	return insn->n;
 }
 
 static int dt282x_dio_insn_config(struct comedi_device *dev,
@@ -1176,12 +1179,13 @@ static int dt282x_grab_dma(struct comedi_device *dev, int dma1, int dma2)
  */
 static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct dt282x_board *board = comedi_board(dev);
 	int i, irq;
 	int ret;
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	iobase = it->options[opt_iobase];
 	if (!iobase)
@@ -1267,8 +1271,8 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 
-	ret = alloc_subdevices(dev, 3);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 0d273269b57..a6fe6c9be87 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -63,8 +63,6 @@ AO commands are not supported.
 #include "../comedidev.h"
 #include <linux/delay.h>
 
-#include "comedi_pci.h"
-
 #define PCI_VENDOR_ID_DT	0x1116
 
 static const struct comedi_lrange range_dt3000_ai = { 4, {
@@ -161,7 +159,6 @@ static const struct dt3k_boardtype dt3k_boardtypes[] = {
 	 },
 };
 
-#define n_dt3k_boards sizeof(dt3k_boardtypes)/sizeof(struct dt3k_boardtype)
 #define this_board ((const struct dt3k_boardtype *)dev->board_ptr)
 
 #define DT3000_SIZE		(4*0x1000)
@@ -251,10 +248,7 @@ static const struct dt3k_boardtype dt3k_boardtypes[] = {
 #define DT3000_CHANNEL_MODE_DI		1
 
 struct dt3k_private {
-
-	struct pci_dev *pci_dev;
-	resource_size_t phys_addr;
-	void *io_addr;
+	void __iomem *io_addr;
 	unsigned int lock;
 	unsigned int ao_readback[2];
 	unsigned int ai_front;
@@ -291,7 +285,7 @@ static int dt3k_send_cmd(struct comedi_device *dev, unsigned int cmd)
 	if ((status & DT3000_COMPLETION_MASK) == DT3000_NOERROR)
 		return 0;
 
-	dev_dbg(dev->hw_dev, "dt3k_send_cmd() timeout/error status=0x%04x\n",
+	dev_dbg(dev->class_dev, "dt3k_send_cmd() timeout/error status=0x%04x\n",
 		status);
 
 	return -ETIME;
@@ -392,7 +386,7 @@ static void dt3k_ai_empty_fifo(struct comedi_device *dev,
 	if (count < 0)
 		count += AI_FIFO_DEPTH;
 
-	dev_dbg(dev->hw_dev, "reading %d samples\n", count);
+	dev_dbg(dev->class_dev, "reading %d samples\n", count);
 
 	rear = devpriv->ai_rear;
 
@@ -580,7 +574,7 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	int ret;
 	unsigned int mode;
 
-	dev_dbg(dev->hw_dev, "dt3k_ai_cmd:\n");
+	dev_dbg(dev->class_dev, "dt3k_ai_cmd:\n");
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		chan = CR_CHAN(cmd->chanlist[i]);
 		range = CR_RANGE(cmd->chanlist[i]);
@@ -591,15 +585,15 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	aref = CR_AREF(cmd->chanlist[0]);
 
 	writew(cmd->scan_end_arg, devpriv->io_addr + DPR_Params(0));
-	dev_dbg(dev->hw_dev, "param[0]=0x%04x\n", cmd->scan_end_arg);
+	dev_dbg(dev->class_dev, "param[0]=0x%04x\n", cmd->scan_end_arg);
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		divider = dt3k_ns_to_timer(50, &cmd->convert_arg,
 					   cmd->flags & TRIG_ROUND_MASK);
 		writew((divider >> 16), devpriv->io_addr + DPR_Params(1));
-		dev_dbg(dev->hw_dev, "param[1]=0x%04x\n", divider >> 16);
+		dev_dbg(dev->class_dev, "param[1]=0x%04x\n", divider >> 16);
 		writew((divider & 0xffff), devpriv->io_addr + DPR_Params(2));
-		dev_dbg(dev->hw_dev, "param[2]=0x%04x\n", divider & 0xffff);
+		dev_dbg(dev->class_dev, "param[2]=0x%04x\n", divider & 0xffff);
 	} else {
 		/* not supported */
 	}
@@ -608,21 +602,21 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		tscandiv = dt3k_ns_to_timer(100, &cmd->scan_begin_arg,
 					    cmd->flags & TRIG_ROUND_MASK);
 		writew((tscandiv >> 16), devpriv->io_addr + DPR_Params(3));
-		dev_dbg(dev->hw_dev, "param[3]=0x%04x\n", tscandiv >> 16);
+		dev_dbg(dev->class_dev, "param[3]=0x%04x\n", tscandiv >> 16);
 		writew((tscandiv & 0xffff), devpriv->io_addr + DPR_Params(4));
-		dev_dbg(dev->hw_dev, "param[4]=0x%04x\n", tscandiv & 0xffff);
+		dev_dbg(dev->class_dev, "param[4]=0x%04x\n", tscandiv & 0xffff);
 	} else {
 		/* not supported */
 	}
 
 	mode = DT3000_AD_RETRIG_INTERNAL | 0 | 0;
 	writew(mode, devpriv->io_addr + DPR_Params(5));
-	dev_dbg(dev->hw_dev, "param[5]=0x%04x\n", mode);
+	dev_dbg(dev->class_dev, "param[5]=0x%04x\n", mode);
 	writew(aref == AREF_DIFF, devpriv->io_addr + DPR_Params(6));
-	dev_dbg(dev->hw_dev, "param[6]=0x%04x\n", aref == AREF_DIFF);
+	dev_dbg(dev->class_dev, "param[6]=0x%04x\n", aref == AREF_DIFF);
 
 	writew(AI_FIFO_DEPTH / 2, devpriv->io_addr + DPR_Params(7));
-	dev_dbg(dev->hw_dev, "param[7]=0x%04x\n", AI_FIFO_DEPTH / 2);
+	dev_dbg(dev->class_dev, "param[7]=0x%04x\n", AI_FIFO_DEPTH / 2);
 
 	writew(SUBS_AI, devpriv->io_addr + DPR_SubSys);
 	ret = dt3k_send_cmd(dev, CMD_CONFIG);
@@ -747,9 +741,6 @@ static int dt3k_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[1] & data[0];
@@ -757,7 +748,7 @@ static int dt3k_dio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = dt3k_readsingle(dev, SUBS_DIN, 0, 0);
 
-	return 2;
+	return insn->n;
 }
 
 static int dt3k_mem_insn_read(struct comedi_device *dev,
@@ -780,112 +771,73 @@ static int dt3k_mem_insn_read(struct comedi_device *dev,
 	return i;
 }
 
-static int setup_pci(struct comedi_device *dev)
-{
-	resource_size_t addr;
-	int ret;
-
-	ret = comedi_pci_enable(devpriv->pci_dev, "dt3000");
-	if (ret < 0)
-		return ret;
-
-	addr = pci_resource_start(devpriv->pci_dev, 0);
-	devpriv->phys_addr = addr;
-	devpriv->io_addr = ioremap(devpriv->phys_addr, DT3000_SIZE);
-	if (!devpriv->io_addr)
-		return -ENOMEM;
-#if DEBUG
-	printk("0x%08llx mapped to %p, ",
-	       (unsigned long long)devpriv->phys_addr, devpriv->io_addr);
-#endif
-
-	return 0;
-}
-
-static struct pci_dev *dt_pci_find_device(struct pci_dev *from, int *board)
+static struct pci_dev *dt3000_find_pci_dev(struct comedi_device *dev,
+					   struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
 	int i;
 
-	for (from = pci_get_device(PCI_VENDOR_ID_DT, PCI_ANY_ID, from);
-	     from != NULL;
-	     from = pci_get_device(PCI_VENDOR_ID_DT, PCI_ANY_ID, from)) {
-		for (i = 0; i < n_dt3k_boards; i++) {
-			if (from->device == dt3k_boardtypes[i].device_id) {
-				*board = i;
-				return from;
-			}
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
 		}
-		printk
-		    ("unknown Data Translation PCI device found with device_id=0x%04x\n",
-		     from->device);
-	}
-	*board = -1;
-	return from;
-}
-
-static int dt_pci_probe(struct comedi_device *dev, int bus, int slot)
-{
-	int board;
-	int ret;
-	struct pci_dev *pcidev;
-
-	pcidev = NULL;
-	while ((pcidev = dt_pci_find_device(pcidev, &board)) != NULL) {
-		if ((bus == 0 && slot == 0) ||
-		    (pcidev->bus->number == bus &&
-		     PCI_SLOT(pcidev->devfn) == slot)) {
-			break;
+		if (pcidev->vendor != PCI_VENDOR_ID_DT)
+			continue;
+		for (i = 0; i < ARRAY_SIZE(dt3k_boardtypes); i++) {
+			if (dt3k_boardtypes[i].device_id != pcidev->device)
+				continue;
+			dev->board_ptr = dt3k_boardtypes + i;
+			return pcidev;
 		}
 	}
-	devpriv->pci_dev = pcidev;
-
-	if (board >= 0)
-		dev->board_ptr = dt3k_boardtypes + board;
-
-	if (!devpriv->pci_dev)
-		return 0;
-
-	ret = setup_pci(dev);
-	if (ret < 0)
-		return ret;
-
-	return 1;
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
 }
 
 static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *s;
-	int bus, slot;
 	int ret = 0;
 
-	dev_dbg(dev->hw_dev, "dt3000:\n");
-	bus = it->options[0];
-	slot = it->options[1];
+	dev_dbg(dev->class_dev, "dt3000:\n");
 
 	ret = alloc_private(dev, sizeof(struct dt3k_private));
 	if (ret < 0)
 		return ret;
 
-	ret = dt_pci_probe(dev, bus, slot);
+	pcidev = dt3000_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
+	ret = comedi_pci_enable(pcidev, "dt3000");
 	if (ret < 0)
 		return ret;
-	if (ret == 0) {
-		dev_warn(dev->hw_dev, "no DT board found\n");
-		return -ENODEV;
-	}
+
+	dev->iobase = pci_resource_start(pcidev, 0);
+	devpriv->io_addr = ioremap(dev->iobase, DT3000_SIZE);
+	if (!devpriv->io_addr)
+		return -ENOMEM;
 
 	dev->board_name = this_board->name;
 
-	if (request_irq(devpriv->pci_dev->irq, dt3k_interrupt, IRQF_SHARED,
+	if (request_irq(pcidev->irq, dt3k_interrupt, IRQF_SHARED,
 			"dt3000", dev)) {
-		dev_err(dev->hw_dev, "unable to allocate IRQ %u\n",
-			devpriv->pci_dev->irq);
+		dev_err(dev->class_dev, "unable to allocate IRQ %u\n",
+			pcidev->irq);
 		return -EINVAL;
 	}
-	dev->irq = devpriv->pci_dev->irq;
+	dev->irq = pcidev->irq;
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices;
@@ -946,17 +898,19 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void dt3000_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
-		if (devpriv->pci_dev) {
-			if (devpriv->phys_addr)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
 		if (devpriv->io_addr)
 			iounmap(devpriv->io_addr);
 	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
+	}
 }
 
 static struct comedi_driver dt3000_driver = {
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
index 22cda5c76ce..40821c7303e 100644
--- a/drivers/staging/comedi/drivers/dt9812.c
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -1021,6 +1021,7 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int i;
 	struct comedi_subdevice *s;
+	int ret;
 
 	dev->board_name = "dt9812";
 
@@ -1035,9 +1036,9 @@ static int dt9812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	devpriv->serial = it->options[0];
 
-	/* Allocate subdevices */
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	/* digital input subdevice */
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index b0cec7b1b0c..064be9aae3a 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -38,7 +38,6 @@
 */
 
 #include "../comedidev.h"
-#include "comedi_pci.h"
 #include <linux/mutex.h>
 
 #define PCI_VENDOR_ID_DYNALOG		0x10b5
@@ -46,8 +45,6 @@
 
 #define READ_TIMEOUT 50
 
-static DEFINE_MUTEX(start_stop_sem);
-
 static const struct comedi_lrange range_pci1050_ai = { 3, {
 							  BIP_RANGE(10),
 							  BIP_RANGE(5),
@@ -103,12 +100,8 @@ static const struct boardtype boardtypes[] = {
 };
 
 struct dyna_pci10xx_private {
-	struct pci_dev *pci_dev;	/*  ptr to PCI device */
-	char valid;			/*  card is usable */
 	struct mutex mutex;
-
-	/* device base address registers */
-	unsigned long BADR0, BADR1, BADR2, BADR3, BADR4, BADR5;
+	unsigned long BADR3;
 };
 
 #define thisboard ((const struct boardtype *)dev->board_ptr)
@@ -136,13 +129,13 @@ static int dyna_pci10xx_insn_read_ai(struct comedi_device *dev,
 	for (n = 0; n < insn->n; n++) {
 		/* trigger conversion */
 		smp_mb();
-		outw_p(0x0000 + range + chan, devpriv->BADR2 + 2);
+		outw_p(0x0000 + range + chan, dev->iobase + 2);
 		udelay(10);
 		/* read data */
 		for (counter = 0; counter < READ_TIMEOUT; counter++) {
-			d = inw_p(devpriv->BADR2);
+			d = inw_p(dev->iobase);
 
-			/* check if read is successfull if the EOC bit is set */
+			/* check if read is successful if the EOC bit is set */
 			if (d & (1 << 15))
 				goto conv_finish;
 		}
@@ -176,7 +169,7 @@ static int dyna_pci10xx_insn_write_ao(struct comedi_device *dev,
 	for (n = 0; n < insn->n; n++) {
 		smp_mb();
 		/* trigger conversion and write data */
-		outw_p(data[n], devpriv->BADR2);
+		outw_p(data[n], dev->iobase);
 		udelay(10);
 	}
 	mutex_unlock(&devpriv->mutex);
@@ -190,9 +183,6 @@ static int dyna_pci10xx_di_insn_bits(struct comedi_device *dev,
 {
 	u16 d = 0;
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	mutex_lock(&devpriv->mutex);
 	smp_mb();
 	d = inw_p(devpriv->BADR3);
@@ -202,7 +192,7 @@ static int dyna_pci10xx_di_insn_bits(struct comedi_device *dev,
 	data[1] = d;
 	data[0] = s->state;
 	mutex_unlock(&devpriv->mutex);
-	return 2;
+	return insn->n;
 }
 
 /* digital output bit interface */
@@ -210,9 +200,6 @@ static int dyna_pci10xx_do_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	/* The insn data is a mask in data[0] and the new data
 	 * in data[1], each channel cooresponding to a bit.
 	 * s->state contains the previous write data
@@ -233,109 +220,77 @@ static int dyna_pci10xx_do_insn_bits(struct comedi_device *dev,
 	 */
 	data[1] = s->state;
 	mutex_unlock(&devpriv->mutex);
-	return 2;
+	return insn->n;
 }
 
-/******************************************************************************/
-/*********************** INITIALIZATION FUNCTIONS *****************************/
-/******************************************************************************/
+static struct pci_dev *dyna_pci10xx_find_pci_dev(struct comedi_device *dev,
+						 struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (bus != pcidev->bus->number ||
+			    slot != PCI_SLOT(pcidev->devfn))
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_DYNALOG)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
+			if (pcidev->device != boardtypes[i].device_id)
+				continue;
+
+			dev->board_ptr = &boardtypes[i];
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
 
 static int dyna_pci10xx_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
-	struct comedi_subdevice *s;
 	struct pci_dev *pcidev;
-	unsigned int opt_bus, opt_slot;
-	int board_index, i;
-
-	mutex_lock(&start_stop_sem);
+	struct comedi_subdevice *s;
+	int ret;
 
 	if (alloc_private(dev, sizeof(struct dyna_pci10xx_private)) < 0) {
 		printk(KERN_ERR "comedi: dyna_pci10xx: "
 			"failed to allocate memory!\n");
-		mutex_unlock(&start_stop_sem);
 		return -ENOMEM;
 	}
 
-	opt_bus = it->options[0];
-	opt_slot = it->options[1];
+	pcidev = dyna_pci10xx_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+
 	dev->board_name = thisboard->name;
 	dev->irq = 0;
 
-	/*
-	 * Probe the PCI bus and located the matching device
-	 */
-	for (pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
-		pcidev != NULL;
-		pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pcidev)) {
-
-		board_index = -1;
-		for (i = 0; i < ARRAY_SIZE(boardtypes); ++i) {
-			if ((pcidev->vendor == PCI_VENDOR_ID_DYNALOG) &&
-				(pcidev->device == boardtypes[i].device_id)) {
-					board_index = i;
-					break;
-				}
-		}
-		if (board_index < 0)
-			continue;
-
-		/* Found matching vendor/device. */
-		if (opt_bus || opt_slot) {
-			/* Check bus/slot. */
-			if (opt_bus != pcidev->bus->number
-			    || opt_slot != PCI_SLOT(pcidev->devfn))
-				continue;	/* no match */
-		}
-
-		goto found;
-	}
-	printk(KERN_ERR "comedi: dyna_pci10xx: no supported device found!\n");
-	mutex_unlock(&start_stop_sem);
-	return -EIO;
-
-found:
-
-	if (!pcidev) {
-		if (opt_bus || opt_slot) {
-			printk(KERN_ERR "comedi: dyna_pci10xx: "
-				"invalid PCI device at b:s %d:%d\n",
-				opt_bus, opt_slot);
-		} else {
-			printk(KERN_ERR "comedi: dyna_pci10xx: "
-				"invalid PCI device\n");
-		}
-		mutex_unlock(&start_stop_sem);
-		return -EIO;
-	}
-
 	if (comedi_pci_enable(pcidev, DRV_NAME)) {
 		printk(KERN_ERR "comedi: dyna_pci10xx: "
 			"failed to enable PCI device and request regions!");
-		mutex_unlock(&start_stop_sem);
 		return -EIO;
 	}
 
 	mutex_init(&devpriv->mutex);
-	dev->board_ptr = &boardtypes[board_index];
-	devpriv->pci_dev = pcidev;
 
 	printk(KERN_INFO "comedi: dyna_pci10xx: device found!\n");
 
-	/* initialize device base address registers */
-	devpriv->BADR0 = pci_resource_start(pcidev, 0);
-	devpriv->BADR1 = pci_resource_start(pcidev, 1);
-	devpriv->BADR2 = pci_resource_start(pcidev, 2);
+	dev->iobase = pci_resource_start(pcidev, 2);
 	devpriv->BADR3 = pci_resource_start(pcidev, 3);
-	devpriv->BADR4 = pci_resource_start(pcidev, 4);
-	devpriv->BADR5 = pci_resource_start(pcidev, 5);
 
-	if (alloc_subdevices(dev, 4) < 0) {
-		printk(KERN_ERR "comedi: dyna_pci10xx: "
-			"failed allocating subdevices\n");
-		mutex_unlock(&start_stop_sem);
-		return -ENOMEM;
-	}
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	/* analog input */
 	s = dev->subdevices + 0;
@@ -378,20 +333,22 @@ found:
 	s->state = 0;
 	s->insn_bits = dyna_pci10xx_do_insn_bits;
 
-	devpriv->valid = 1;
-	mutex_unlock(&start_stop_sem);
-
 	printk(KERN_INFO "comedi: dyna_pci10xx: %s - device setup completed!\n",
-		boardtypes[board_index].name);
+		thisboard->name);
 
 	return 1;
 }
 
 static void dyna_pci10xx_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pci_dev) {
-		comedi_pci_disable(devpriv->pci_dev);
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (devpriv)
 		mutex_destroy(&devpriv->mutex);
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/fl512.c b/drivers/staging/comedi/drivers/fl512.c
index d23814450b4..d1da80976f8 100644
--- a/drivers/staging/comedi/drivers/fl512.c
+++ b/drivers/staging/comedi/drivers/fl512.c
@@ -111,6 +111,7 @@ static int fl512_ao_insn_readback(struct comedi_device *dev,
 static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	unsigned long iobase;
+	int ret;
 
 	/* pointer to the subdevice: Analog in, Analog out,
 	   (not made ->and Digital IO) */
@@ -131,8 +132,9 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	printk(KERN_DEBUG "malloc ok\n");
 #endif
 
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
 	/*
 	 * this if the definitions of the supdevices, 2 have been defined
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 8aece08bd0d..79f580841de 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -49,7 +49,6 @@ support could be added to this driver.
 #include "../comedidev.h"
 #include <linux/delay.h>
 
-#include "comedi_pci.h"
 #include "plx9080.h"
 #include "comedi_fc.h"
 
@@ -297,8 +296,8 @@ struct hpdi_private {
 	resource_size_t plx9080_phys_iobase;
 	resource_size_t hpdi_phys_iobase;
 	/*  base addresses (ioremapped) */
-	void *plx9080_iobase;
-	void *hpdi_iobase;
+	void __iomem *plx9080_iobase;
+	void __iomem *hpdi_iobase;
 	uint32_t *dio_buffer[NUM_DMA_BUFFERS];	/*  dma buffers */
 	/* physical addresses of dma buffers */
 	dma_addr_t dio_buffer_phys_addr[NUM_DMA_BUFFERS];
@@ -364,7 +363,7 @@ static void disable_plx_interrupts(struct comedi_device *dev)
 static void init_plx9080(struct comedi_device *dev)
 {
 	uint32_t bits;
-	void *plx_iobase = priv(dev)->plx9080_iobase;
+	void __iomem *plx_iobase = priv(dev)->plx9080_iobase;
 
 	/*  plx9080 dump */
 	DEBUG_PRINT(" plx interrupt status 0x%x\n",
@@ -431,9 +430,11 @@ static void init_plx9080(struct comedi_device *dev)
 static int setup_subdevices(struct comedi_device *dev)
 {
 	struct comedi_subdevice *s;
+	int ret;
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* analog input subdevice */
@@ -632,7 +633,7 @@ static int hpdi_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	printk(KERN_WARNING " irq %u\n", dev->irq);
 
-	/*  alocate pci dma buffers */
+	/*  allocate pci dma buffers */
 	for (i = 0; i < NUM_DMA_BUFFERS; i++) {
 		priv(dev)->dio_buffer[i] =
 		    pci_alloc_consistent(priv(dev)->hw_dev, DMA_BUFFER_SIZE,
@@ -673,10 +674,10 @@ static void hpdi_detach(struct comedi_device *dev)
 	if ((priv(dev)) && (priv(dev)->hw_dev)) {
 		if (priv(dev)->plx9080_iobase) {
 			disable_plx_interrupts(dev);
-			iounmap((void *)priv(dev)->plx9080_iobase);
+			iounmap(priv(dev)->plx9080_iobase);
 		}
 		if (priv(dev)->hpdi_iobase)
-			iounmap((void *)priv(dev)->hpdi_iobase);
+			iounmap(priv(dev)->hpdi_iobase);
 		/*  free pci dma buffers */
 		for (i = 0; i < NUM_DMA_BUFFERS; i++) {
 			if (priv(dev)->dio_buffer[i])
@@ -903,7 +904,7 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	uint32_t next_transfer_addr;
 	int j;
 	int num_samples = 0;
-	void *pci_addr_reg;
+	void __iomem *pci_addr_reg;
 
 	if (channel)
 		pci_addr_reg =
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index fdc596fb099..b10ebdbc1f7 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -148,7 +148,7 @@ struct boardtype {
 struct icp_multi_private {
 	struct pcilst_struct *card;	/*  pointer to card */
 	char valid;		/*  card is usable */
-	void *io_addr;		/*  Pointer to mapped io address */
+	void __iomem *io_addr;		/*  Pointer to mapped io address */
 	resource_size_t phys_iobase;	/*  Physical io address */
 	unsigned int AdcCmdStatus;	/*  ADC Command/Status register */
 	unsigned int DacCmdStatus;	/*  DAC Command/Status register */
@@ -542,7 +542,7 @@ static int icp_multi_insn_bits_di(struct comedi_device *dev,
 {
 	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -585,7 +585,7 @@ static int icp_multi_insn_bits_do(struct comedi_device *dev,
 #ifdef ICP_MULTI_EXTDEBUG
 	printk(KERN_DEBUG "icp multi EDBG: END: icp_multi_insn_bits_do(...)\n");
 #endif
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -835,7 +835,7 @@ static int icp_multi_attach(struct comedi_device *dev,
 	printk(KERN_WARNING
 	       "icp_multi EDBG: BGN: icp_multi_attach(...)\n");
 
-	/*  Alocate private data storage space */
+	/*  Allocate private data storage space */
 	ret = alloc_private(dev, sizeof(struct icp_multi_private));
 	if (ret < 0)
 		return ret;
@@ -903,8 +903,8 @@ static int icp_multi_attach(struct comedi_device *dev,
 	if (this_board->n_ctrs)
 		n_subdevices++;
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
 
 	icp_multi_reset(dev);
diff --git a/drivers/staging/comedi/drivers/icp_multi.h b/drivers/staging/comedi/drivers/icp_multi.h
index 68acefe1688..dbf9908cfde 100644
--- a/drivers/staging/comedi/drivers/icp_multi.h
+++ b/drivers/staging/comedi/drivers/icp_multi.h
@@ -11,7 +11,6 @@
 #define _ICP_MULTI_H_
 
 #include "../comedidev.h"
-#include "comedi_pci.h"
 
 /****************************************************************************/
 
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index f0a579a04a7..0f9cfe662b9 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -135,15 +135,15 @@ options for PCI-20341M:
 #define PCI20341_SCANLIST		0x80	/* Channel/Gain Scan List */
 
 union pci20xxx_subdev_private {
-	void *iobase;
+	void __iomem *iobase;
 	struct {
-		void *iobase;
+		void __iomem *iobase;
 		const struct comedi_lrange *ao_range_list[2];
 					/* range of channels of ao module */
 		unsigned int last_data[2];
 	} pci20006;
 	struct {
-		void *iobase;
+		void __iomem *iobase;
 		int timebase;
 		int settling_time;
 		int ai_gain;
@@ -152,7 +152,7 @@ union pci20xxx_subdev_private {
 
 struct pci20xxx_private {
 
-	void *ioaddr;
+	void __iomem *ioaddr;
 	union pci20xxx_subdev_private subdev_private[PCI20000_MODULES];
 };
 
@@ -202,15 +202,15 @@ static int pci20xxx_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	union pci20xxx_subdev_private *sdp;
 
-	ret = alloc_subdevices(dev, 1 + PCI20000_MODULES);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 1 + PCI20000_MODULES);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct pci20xxx_private));
 	if (ret < 0)
 		return ret;
 
-	devpriv->ioaddr = (void *)(unsigned long)it->options[0];
+	devpriv->ioaddr = (void __iomem *)(unsigned long)it->options[0];
 	dev->board_name = "pci20kc";
 
 	/* Check PCI-20001 C-2A Carrier Board ID */
@@ -565,7 +565,7 @@ static int pci20xxx_dio_insn_bits(struct comedi_device *dev,
 	data[1] |= readb(devpriv->ioaddr + PCI20000_DIO_2) << 16;
 	data[1] |= readb(devpriv->ioaddr + PCI20000_DIO_3) << 24;
 
-	return 2;
+	return insn->n;
 }
 
 static void pci20xxx_dio_config(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index d536a11edb9..93f94cd7bae 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -49,7 +49,6 @@ Devices: [JR3] PCI force sensor board (jr3_pci)
 #include <linux/slab.h>
 #include <linux/timer.h>
 #include <linux/kernel.h>
-#include "comedi_pci.h"
 #include "jr3_pci.h"
 
 #define PCI_VENDOR_ID_JR3 0x1762
@@ -350,13 +349,13 @@ static int jr3_pci_open(struct comedi_device *dev)
 	int i;
 	struct jr3_pci_dev_private *devpriv = dev->private;
 
-	dev_dbg(dev->hw_dev, "jr3_pci_open\n");
+	dev_dbg(dev->class_dev, "jr3_pci_open\n");
 	for (i = 0; i < devpriv->n_channels; i++) {
 		struct jr3_pci_subdev_private *p;
 
 		p = dev->subdevices[i].private;
 		if (p) {
-			dev_dbg(dev->hw_dev, "serial: %p %d (%d)\n", p,
+			dev_dbg(dev->class_dev, "serial: %p %d (%d)\n", p,
 				p->serial_no, p->channel_no);
 		}
 	}
@@ -435,7 +434,8 @@ static int jr3_download_firmware(struct comedi_device *dev, const u8 * data,
 					break;
 				more = more
 				    && read_idm_word(data, size, &pos, &addr);
-				dev_dbg(dev->hw_dev, "Loading#%d %4.4x bytes at %4.4x\n",
+				dev_dbg(dev->class_dev,
+					"Loading#%d %4.4x bytes at %4.4x\n",
 					i, count, addr);
 				while (more && count > 0) {
 					if (addr & 0x4000) {
@@ -755,7 +755,8 @@ static int jr3_pci_attach(struct comedi_device *dev,
 	opt_slot = it->options[1];
 
 	if (sizeof(struct jr3_channel) != 0xc00) {
-		dev_err(dev->hw_dev, "sizeof(struct jr3_channel) = %x [expected %x]\n",
+		dev_err(dev->class_dev,
+			"sizeof(struct jr3_channel) = %x [expected %x]\n",
 			(unsigned)sizeof(struct jr3_channel), 0xc00);
 		return -EINVAL;
 	}
@@ -810,7 +811,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 		}
 	}
 	if (!card) {
-		dev_err(dev->hw_dev, "no jr3_pci found\n");
+		dev_err(dev->class_dev, "no jr3_pci found\n");
 		return -EIO;
 	} else {
 		devpriv->pci_dev = card;
@@ -827,9 +828,9 @@ static int jr3_pci_attach(struct comedi_device *dev,
 	if (!devpriv->iobase)
 		return -ENOMEM;
 
-	result = alloc_subdevices(dev, devpriv->n_channels);
-	if (result < 0)
-		goto out;
+	result = comedi_alloc_subdevices(dev, devpriv->n_channels);
+	if (result)
+		return result;
 
 	dev->open = jr3_pci_open;
 	for (i = 0; i < devpriv->n_channels; i++) {
@@ -845,7 +846,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 
 			p = dev->subdevices[i].private;
 			p->channel = &devpriv->iobase->channel[i].data;
-			dev_dbg(dev->hw_dev, "p->channel %p %p (%tx)\n",
+			dev_dbg(dev->class_dev, "p->channel %p %p (%tx)\n",
 				p->channel, devpriv->iobase,
 				((char *)(p->channel) -
 				 (char *)(devpriv->iobase)));
@@ -886,7 +887,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 	devpriv->iobase->channel[0].reset = 0;
 
 	result = comedi_load_firmware(dev, "jr3pci.idm", jr3_download_firmware);
-	dev_dbg(dev->hw_dev, "Firmare load %d\n", result);
+	dev_dbg(dev->class_dev, "Firmare load %d\n", result);
 
 	if (result < 0)
 		goto out;
@@ -904,7 +905,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
  */
 	msleep_interruptible(25);
 	for (i = 0; i < 0x18; i++) {
-		dev_dbg(dev->hw_dev, "%c\n",
+		dev_dbg(dev->class_dev, "%c\n",
 			get_u16(&devpriv->iobase->channel[0].
 				data.copyright[i]) >> 8);
 	}
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index 09d191844bf..d4e9292483a 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -40,8 +40,6 @@ Kolter Electronic PCI Counter Card.
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #define CNT_DRIVER_NAME         "ke_counter"
 #define PCI_VENDOR_ID_KOLTER    0x1001
 #define CNT_CARD_DEVICE_ID      0x0014
@@ -64,17 +62,6 @@ static const struct cnt_board_struct cnt_boards[] = {
 	 .cnt_bits = 24}
 };
 
-#define cnt_board_nbr (sizeof(cnt_boards)/sizeof(struct cnt_board_struct))
-
-/*-- device private structure -----------------------------------------------*/
-
-struct cnt_device_private {
-
-	struct pci_dev *pcidev;
-};
-
-#define devpriv ((struct cnt_device_private *)dev->private)
-
 /*-- counter write ----------------------------------------------------------*/
 
 /* This should be used only for resetting the counters; maybe it is better
@@ -124,62 +111,58 @@ static int cnt_rinsn(struct comedi_device *dev,
 	return 1;
 }
 
+static struct pci_dev *cnt_find_pci_dev(struct comedi_device *dev,
+					struct comedi_devconfig *it)
+{
+	const struct cnt_board_struct *board;
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	/* Probe the device to determine what device in the series it is. */
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_KOLTER)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(cnt_boards); i++) {
+			board = &cnt_boards[i];
+			if (board->device_id != pcidev->device)
+				continue;
+
+			dev->board_ptr = board;
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
 static int cnt_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct cnt_board_struct *board;
+	struct pci_dev *pcidev;
 	struct comedi_subdevice *subdevice;
-	struct pci_dev *pci_device = NULL;
-	struct cnt_board_struct *board;
 	unsigned long io_base;
-	int error, i;
+	int error;
 
-	/* allocate device private structure */
-	error = alloc_private(dev, sizeof(struct cnt_device_private));
-	if (error < 0)
-		return error;
+	pcidev = cnt_find_pci_dev(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	board = comedi_board(dev);
 
-	/* Probe the device to determine what device in the series it is. */
-	for_each_pci_dev(pci_device) {
-		if (pci_device->vendor == PCI_VENDOR_ID_KOLTER) {
-			for (i = 0; i < cnt_board_nbr; i++) {
-				if (cnt_boards[i].device_id ==
-				    pci_device->device) {
-					/* was a particular bus/slot requested? */
-					if ((it->options[0] != 0)
-					    || (it->options[1] != 0)) {
-						/* are we on the wrong bus/slot? */
-						if (pci_device->bus->number !=
-						    it->options[0]
-						    ||
-						    PCI_SLOT(pci_device->devfn)
-						    != it->options[1]) {
-							continue;
-						}
-					}
-
-					dev->board_ptr = cnt_boards + i;
-					board =
-					    (struct cnt_board_struct *)
-					    dev->board_ptr;
-					goto found;
-				}
-			}
-		}
-	}
-	printk(KERN_WARNING
-	       "comedi%d: no supported board found! (req. bus/slot: %d/%d)\n",
-	       dev->minor, it->options[0], it->options[1]);
-	return -EIO;
-
-found:
-	printk(KERN_INFO
-	       "comedi%d: found %s at PCI bus %d, slot %d\n", dev->minor,
-	       board->name, pci_device->bus->number,
-	       PCI_SLOT(pci_device->devfn));
-	devpriv->pcidev = pci_device;
 	dev->board_name = board->name;
 
 	/* enable PCI device and request regions */
-	error = comedi_pci_enable(pci_device, CNT_DRIVER_NAME);
+	error = comedi_pci_enable(pcidev, CNT_DRIVER_NAME);
 	if (error < 0) {
 		printk(KERN_WARNING "comedi%d: "
 		       "failed to enable PCI device and request regions!\n",
@@ -188,12 +171,11 @@ found:
 	}
 
 	/* read register base address [PCI_BASE_ADDRESS #0] */
-	io_base = pci_resource_start(pci_device, 0);
+	io_base = pci_resource_start(pcidev, 0);
 	dev->iobase = io_base;
 
-	/* allocate the subdevice structures */
-	error = alloc_subdevices(dev, 1);
-	if (error < 0)
+	error = comedi_alloc_subdevices(dev, 1);
+	if (error)
 		return error;
 
 	subdevice = dev->subdevices + 0;
@@ -221,10 +203,12 @@ found:
 
 static void cnt_detach(struct comedi_device *dev)
 {
-	if (devpriv && devpriv->pcidev) {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (pcidev) {
 		if (dev->iobase)
-			comedi_pci_disable(devpriv->pcidev);
-		pci_dev_put(devpriv->pcidev);
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 8ca1b54600d..9a8258e6fa4 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -58,7 +58,6 @@ broken.
 #include <linux/list.h>
 #include <linux/spinlock.h>
 
-#include "comedi_pci.h"
 #include "me4000.h"
 #if 0
 /* file removed due to GPL incompatibility */
@@ -105,42 +104,6 @@ static int ai_write_chanlist(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd);
 
-/*-----------------------------------------------------------------------------
-  Meilhaus inline functions
-  ---------------------------------------------------------------------------*/
-
-static inline void me4000_outb(struct comedi_device *dev, unsigned char value,
-			       unsigned long port)
-{
-	PORT_PDEBUG("--> 0x%02X port 0x%04lX\n", value, port);
-	outb(value, port);
-}
-
-static inline void me4000_outl(struct comedi_device *dev, unsigned long value,
-			       unsigned long port)
-{
-	PORT_PDEBUG("--> 0x%08lX port 0x%04lX\n", value, port);
-	outl(value, port);
-}
-
-static inline unsigned long me4000_inl(struct comedi_device *dev,
-				       unsigned long port)
-{
-	unsigned long value;
-	value = inl(port);
-	PORT_PDEBUG("<-- 0x%08lX port 0x%04lX\n", value, port);
-	return value;
-}
-
-static inline unsigned char me4000_inb(struct comedi_device *dev,
-				       unsigned long port)
-{
-	unsigned char value;
-	value = inb(port);
-	PORT_PDEBUG("<-- 0x%08X port 0x%04lX\n", value, port);
-	return value;
-}
-
 static const struct comedi_lrange me4000_ai_range = {
 	4,
 	{
@@ -164,8 +127,6 @@ static int me4000_probe(struct comedi_device *dev, struct comedi_devconfig *it)
 	int result, i;
 	struct me4000_board *board;
 
-	CALL_PDEBUG("In me4000_probe()\n");
-
 	/* Allocate private memory */
 	if (alloc_private(dev, sizeof(struct me4000_info)) < 0)
 		return -ENOMEM;
@@ -309,9 +270,6 @@ found:
 
 static int get_registers(struct comedi_device *dev, struct pci_dev *pci_dev_p)
 {
-
-	CALL_PDEBUG("In get_registers()\n");
-
     /*--------------------------- plx regbase -------------------------------*/
 
 	info->plx_regbase = pci_resource_start(pci_dev_p, 1);
@@ -363,8 +321,6 @@ static int init_board_info(struct comedi_device *dev, struct pci_dev *pci_dev_p)
 {
 	int result;
 
-	CALL_PDEBUG("In init_board_info()\n");
-
 	/* Init spin locks */
 	/* spin_lock_init(&info->preload_lock); */
 	/* spin_lock_init(&info->ai_ctrl_lock); */
@@ -395,8 +351,6 @@ static int init_ao_context(struct comedi_device *dev)
 {
 	int i;
 
-	CALL_PDEBUG("In init_ao_context()\n");
-
 	for (i = 0; i < thisboard->ao.count; i++) {
 		/* spin_lock_init(&info->ao_context[i].use_lock); */
 		info->ao_context[i].irq = info->irq;
@@ -476,9 +430,6 @@ static int init_ao_context(struct comedi_device *dev)
 
 static int init_ai_context(struct comedi_device *dev)
 {
-
-	CALL_PDEBUG("In init_ai_context()\n");
-
 	info->ai_context.irq = info->irq;
 
 	info->ai_context.ctrl_reg = info->me4000_regbase + ME4000_AI_CTRL_REG;
@@ -510,9 +461,6 @@ static int init_ai_context(struct comedi_device *dev)
 
 static int init_dio_context(struct comedi_device *dev)
 {
-
-	CALL_PDEBUG("In init_dio_context()\n");
-
 	info->dio_context.dir_reg = info->me4000_regbase + ME4000_DIO_DIR_REG;
 	info->dio_context.ctrl_reg = info->me4000_regbase + ME4000_DIO_CTRL_REG;
 	info->dio_context.port_0_reg =
@@ -529,9 +477,6 @@ static int init_dio_context(struct comedi_device *dev)
 
 static int init_cnt_context(struct comedi_device *dev)
 {
-
-	CALL_PDEBUG("In init_cnt_context()\n");
-
 	info->cnt_context.ctrl_reg = info->timer_regbase + ME4000_CNT_CTRL_REG;
 	info->cnt_context.counter_0_reg =
 	    info->timer_regbase + ME4000_CNT_COUNTER_0_REG;
@@ -555,8 +500,6 @@ static int xilinx_download(struct comedi_device *dev)
 	int idx = 0;
 	int size = 0;
 
-	CALL_PDEBUG("In xilinx_download()\n");
-
 	init_waitqueue_head(&queue);
 
 	/*
@@ -635,59 +578,46 @@ static int reset_board(struct comedi_device *dev)
 {
 	unsigned long icr;
 
-	CALL_PDEBUG("In reset_board()\n");
-
 	/* Make a hardware reset */
-	icr = me4000_inl(dev, info->plx_regbase + PLX_ICR);
+	icr = inl(info->plx_regbase + PLX_ICR);
 	icr |= 0x40000000;
-	me4000_outl(dev, icr, info->plx_regbase + PLX_ICR);
+	outl(icr, info->plx_regbase + PLX_ICR);
 	icr &= ~0x40000000;
-	me4000_outl(dev, icr, info->plx_regbase + PLX_ICR);
+	outl(icr, info->plx_regbase + PLX_ICR);
 
 	/* 0x8000 to the DACs means an output voltage of 0V */
-	me4000_outl(dev, 0x8000,
-		    info->me4000_regbase + ME4000_AO_00_SINGLE_REG);
-	me4000_outl(dev, 0x8000,
-		    info->me4000_regbase + ME4000_AO_01_SINGLE_REG);
-	me4000_outl(dev, 0x8000,
-		    info->me4000_regbase + ME4000_AO_02_SINGLE_REG);
-	me4000_outl(dev, 0x8000,
-		    info->me4000_regbase + ME4000_AO_03_SINGLE_REG);
+	outl(0x8000, info->me4000_regbase + ME4000_AO_00_SINGLE_REG);
+	outl(0x8000, info->me4000_regbase + ME4000_AO_01_SINGLE_REG);
+	outl(0x8000, info->me4000_regbase + ME4000_AO_02_SINGLE_REG);
+	outl(0x8000, info->me4000_regbase + ME4000_AO_03_SINGLE_REG);
 
 	/* Set both stop bits in the analog input control register */
-	me4000_outl(dev,
-		    ME4000_AI_CTRL_BIT_IMMEDIATE_STOP | ME4000_AI_CTRL_BIT_STOP,
-		    info->me4000_regbase + ME4000_AI_CTRL_REG);
+	outl(ME4000_AI_CTRL_BIT_IMMEDIATE_STOP | ME4000_AI_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AI_CTRL_REG);
 
 	/* Set both stop bits in the analog output control register */
-	me4000_outl(dev,
-		    ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		    info->me4000_regbase + ME4000_AO_00_CTRL_REG);
-	me4000_outl(dev,
-		    ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		    info->me4000_regbase + ME4000_AO_01_CTRL_REG);
-	me4000_outl(dev,
-		    ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		    info->me4000_regbase + ME4000_AO_02_CTRL_REG);
-	me4000_outl(dev,
-		    ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
-		    info->me4000_regbase + ME4000_AO_03_CTRL_REG);
+	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_00_CTRL_REG);
+	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_01_CTRL_REG);
+	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_02_CTRL_REG);
+	outl(ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_03_CTRL_REG);
 
 	/* Enable interrupts on the PLX */
-	me4000_outl(dev, 0x43, info->plx_regbase + PLX_INTCSR);
+	outl(0x43, info->plx_regbase + PLX_INTCSR);
 
 	/* Set the adustment register for AO demux */
-	me4000_outl(dev, ME4000_AO_DEMUX_ADJUST_VALUE,
+	outl(ME4000_AO_DEMUX_ADJUST_VALUE,
 		    info->me4000_regbase + ME4000_AO_DEMUX_ADJUST_REG);
 
 	/*
 	 * Set digital I/O direction for port 0
 	 * to output on isolated versions
 	 */
-	if (!(me4000_inl(dev, info->me4000_regbase + ME4000_DIO_DIR_REG) & 0x1)) {
-		me4000_outl(dev, 0x1,
-			    info->me4000_regbase + ME4000_DIO_CTRL_REG);
-	}
+	if (!(inl(info->me4000_regbase + ME4000_DIO_DIR_REG) & 0x1))
+		outl(0x1, info->me4000_regbase + ME4000_DIO_CTRL_REG);
 
 	return 0;
 }
@@ -709,8 +639,6 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 	unsigned long tmp;
 	long lval;
 
-	CALL_PDEBUG("In me4000_ai_insn_read()\n");
-
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
@@ -779,36 +707,34 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 	entry |= ME4000_AI_LIST_LAST_ENTRY;
 
 	/* Clear channel list, data fifo and both stop bits */
-	tmp = me4000_inl(dev, info->ai_context.ctrl_reg);
+	tmp = inl(info->ai_context.ctrl_reg);
 	tmp &= ~(ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
 		 ME4000_AI_CTRL_BIT_DATA_FIFO |
 		 ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
-	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+	outl(tmp, info->ai_context.ctrl_reg);
 
 	/* Set the acquisition mode to single */
 	tmp &= ~(ME4000_AI_CTRL_BIT_MODE_0 | ME4000_AI_CTRL_BIT_MODE_1 |
 		 ME4000_AI_CTRL_BIT_MODE_2);
-	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+	outl(tmp, info->ai_context.ctrl_reg);
 
 	/* Enable channel list and data fifo */
 	tmp |= ME4000_AI_CTRL_BIT_CHANNEL_FIFO | ME4000_AI_CTRL_BIT_DATA_FIFO;
-	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+	outl(tmp, info->ai_context.ctrl_reg);
 
 	/* Generate channel list entry */
-	me4000_outl(dev, entry, info->ai_context.channel_list_reg);
+	outl(entry, info->ai_context.channel_list_reg);
 
 	/* Set the timer to maximum sample rate */
-	me4000_outl(dev, ME4000_AI_MIN_TICKS, info->ai_context.chan_timer_reg);
-	me4000_outl(dev, ME4000_AI_MIN_TICKS,
-		    info->ai_context.chan_pre_timer_reg);
+	outl(ME4000_AI_MIN_TICKS, info->ai_context.chan_timer_reg);
+	outl(ME4000_AI_MIN_TICKS, info->ai_context.chan_pre_timer_reg);
 
 	/* Start conversion by dummy read */
-	me4000_inl(dev, info->ai_context.start_reg);
+	inl(info->ai_context.start_reg);
 
 	/* Wait until ready */
 	udelay(10);
-	if (!
-	    (me4000_inl(dev, info->ai_context.status_reg) &
+	if (!(inl(info->ai_context.status_reg) &
 	     ME4000_AI_STATUS_BIT_EF_DATA)) {
 		printk(KERN_ERR
 		       "comedi%d: me4000: me4000_ai_insn_read(): "
@@ -817,7 +743,7 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 	}
 
 	/* Read value from data fifo */
-	lval = me4000_inl(dev, info->ai_context.data_reg) & 0xFFFF;
+	lval = inl(info->ai_context.data_reg) & 0xFFFF;
 	data[0] = lval ^ 0x8000;
 
 	return 1;
@@ -828,15 +754,13 @@ static int me4000_ai_cancel(struct comedi_device *dev,
 {
 	unsigned long tmp;
 
-	CALL_PDEBUG("In me4000_ai_cancel()\n");
-
 	/* Stop any running conversion */
-	tmp = me4000_inl(dev, info->ai_context.ctrl_reg);
+	tmp = inl(info->ai_context.ctrl_reg);
 	tmp &= ~(ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
-	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+	outl(tmp, info->ai_context.ctrl_reg);
 
 	/* Clear the control register */
-	me4000_outl(dev, 0x0, info->ai_context.ctrl_reg);
+	outl(0x0, info->ai_context.ctrl_reg);
 
 	return 0;
 }
@@ -847,8 +771,6 @@ static int ai_check_chanlist(struct comedi_device *dev,
 	int aref;
 	int i;
 
-	CALL_PDEBUG("In ai_check_chanlist()\n");
-
 	/* Check whether a channel list is available */
 	if (!cmd->chanlist_len) {
 		printk(KERN_ERR
@@ -934,25 +856,18 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 
 	int rest;
 
-	CALL_PDEBUG("In ai_round_cmd_args()\n");
-
 	*init_ticks = 0;
 	*scan_ticks = 0;
 	*chan_ticks = 0;
 
-	PDEBUG("ai_round_cmd_arg(): start_arg = %d\n", cmd->start_arg);
-	PDEBUG("ai_round_cmd_arg(): scan_begin_arg = %d\n",
-	       cmd->scan_begin_arg);
-	PDEBUG("ai_round_cmd_arg(): convert_arg = %d\n", cmd->convert_arg);
-
 	if (cmd->start_arg) {
 		*init_ticks = (cmd->start_arg * 33) / 1000;
 		rest = (cmd->start_arg * 33) % 1000;
 
-		if (cmd->flags & TRIG_ROUND_NEAREST) {
+		if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_NEAREST) {
 			if (rest > 33)
 				(*init_ticks)++;
-		} else if (cmd->flags & TRIG_ROUND_UP) {
+		} else if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_UP) {
 			if (rest)
 				(*init_ticks)++;
 		}
@@ -962,10 +877,10 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 		*scan_ticks = (cmd->scan_begin_arg * 33) / 1000;
 		rest = (cmd->scan_begin_arg * 33) % 1000;
 
-		if (cmd->flags & TRIG_ROUND_NEAREST) {
+		if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_NEAREST) {
 			if (rest > 33)
 				(*scan_ticks)++;
-		} else if (cmd->flags & TRIG_ROUND_UP) {
+		} else if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_UP) {
 			if (rest)
 				(*scan_ticks)++;
 		}
@@ -975,19 +890,15 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 		*chan_ticks = (cmd->convert_arg * 33) / 1000;
 		rest = (cmd->convert_arg * 33) % 1000;
 
-		if (cmd->flags & TRIG_ROUND_NEAREST) {
+		if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_NEAREST) {
 			if (rest > 33)
 				(*chan_ticks)++;
-		} else if (cmd->flags & TRIG_ROUND_UP) {
+		} else if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_UP) {
 			if (rest)
 				(*chan_ticks)++;
 		}
 	}
 
-	PDEBUG("ai_round_cmd_args(): init_ticks = %d\n", *init_ticks);
-	PDEBUG("ai_round_cmd_args(): scan_ticks = %d\n", *scan_ticks);
-	PDEBUG("ai_round_cmd_args(): chan_ticks = %d\n", *chan_ticks);
-
 	return 0;
 }
 
@@ -995,21 +906,16 @@ static void ai_write_timer(struct comedi_device *dev,
 			   unsigned int init_ticks,
 			   unsigned int scan_ticks, unsigned int chan_ticks)
 {
-
-	CALL_PDEBUG("In ai_write_timer()\n");
-
-	me4000_outl(dev, init_ticks - 1,
-		    info->ai_context.scan_pre_timer_low_reg);
-	me4000_outl(dev, 0x0, info->ai_context.scan_pre_timer_high_reg);
+	outl(init_ticks - 1, info->ai_context.scan_pre_timer_low_reg);
+	outl(0x0, info->ai_context.scan_pre_timer_high_reg);
 
 	if (scan_ticks) {
-		me4000_outl(dev, scan_ticks - 1,
-			    info->ai_context.scan_timer_low_reg);
-		me4000_outl(dev, 0x0, info->ai_context.scan_timer_high_reg);
+		outl(scan_ticks - 1, info->ai_context.scan_timer_low_reg);
+		outl(0x0, info->ai_context.scan_timer_high_reg);
 	}
 
-	me4000_outl(dev, chan_ticks - 1, info->ai_context.chan_pre_timer_reg);
-	me4000_outl(dev, chan_ticks - 1, info->ai_context.chan_timer_reg);
+	outl(chan_ticks - 1, info->ai_context.chan_pre_timer_reg);
+	outl(chan_ticks - 1, info->ai_context.chan_timer_reg);
 }
 
 static int ai_prepare(struct comedi_device *dev,
@@ -1021,13 +927,11 @@ static int ai_prepare(struct comedi_device *dev,
 
 	unsigned long tmp = 0;
 
-	CALL_PDEBUG("In ai_prepare()\n");
-
 	/* Write timer arguments */
 	ai_write_timer(dev, init_ticks, scan_ticks, chan_ticks);
 
 	/* Reset control register */
-	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+	outl(tmp, info->ai_context.ctrl_reg);
 
 	/* Start sources */
 	if ((cmd->start_src == TRIG_EXT &&
@@ -1060,12 +964,12 @@ static int ai_prepare(struct comedi_device *dev,
 
 	/* Stop triggers */
 	if (cmd->stop_src == TRIG_COUNT) {
-		me4000_outl(dev, cmd->chanlist_len * cmd->stop_arg,
+		outl(cmd->chanlist_len * cmd->stop_arg,
 			    info->ai_context.sample_counter_reg);
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
 	} else if (cmd->stop_src == TRIG_NONE &&
 		   cmd->scan_end_src == TRIG_COUNT) {
-		me4000_outl(dev, cmd->scan_end_arg,
+		outl(cmd->scan_end_arg,
 			    info->ai_context.sample_counter_reg);
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
 	} else {
@@ -1073,7 +977,7 @@ static int ai_prepare(struct comedi_device *dev,
 	}
 
 	/* Write the setup to the control register */
-	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+	outl(tmp, info->ai_context.ctrl_reg);
 
 	/* Write the channel list */
 	ai_write_chanlist(dev, s, cmd);
@@ -1090,8 +994,6 @@ static int ai_write_chanlist(struct comedi_device *dev,
 	unsigned int aref;
 	int i;
 
-	CALL_PDEBUG("In ai_write_chanlist()\n");
-
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		chan = CR_CHAN(cmd->chanlist[i]);
 		rang = CR_RANGE(cmd->chanlist[i]);
@@ -1113,7 +1015,7 @@ static int ai_write_chanlist(struct comedi_device *dev,
 		else
 			entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED;
 
-		me4000_outl(dev, entry, info->ai_context.channel_list_reg);
+		outl(entry, info->ai_context.channel_list_reg);
 	}
 
 	return 0;
@@ -1128,8 +1030,6 @@ static int me4000_ai_do_cmd(struct comedi_device *dev,
 	unsigned int chan_ticks = 0;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	CALL_PDEBUG("In me4000_ai_do_cmd()\n");
-
 	/* Reset the analog input */
 	err = me4000_ai_cancel(dev, s);
 	if (err)
@@ -1147,7 +1047,7 @@ static int me4000_ai_do_cmd(struct comedi_device *dev,
 		return err;
 
 	/* Start acquistion by dummy read */
-	me4000_inl(dev, info->ai_context.start_reg);
+	inl(info->ai_context.start_reg);
 
 	return 0;
 }
@@ -1174,34 +1074,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	unsigned int scan_ticks;
 	int err = 0;
 
-	CALL_PDEBUG("In me4000_ai_do_cmd_test()\n");
-
-	PDEBUG("me4000_ai_do_cmd_test(): subdev         = %d\n", cmd->subdev);
-	PDEBUG("me4000_ai_do_cmd_test(): flags          = %08X\n", cmd->flags);
-	PDEBUG("me4000_ai_do_cmd_test(): start_src      = %08X\n",
-	       cmd->start_src);
-	PDEBUG("me4000_ai_do_cmd_test(): start_arg      = %d\n",
-	       cmd->start_arg);
-	PDEBUG("me4000_ai_do_cmd_test(): scan_begin_src = %08X\n",
-	       cmd->scan_begin_src);
-	PDEBUG("me4000_ai_do_cmd_test(): scan_begin_arg = %d\n",
-	       cmd->scan_begin_arg);
-	PDEBUG("me4000_ai_do_cmd_test(): convert_src    = %08X\n",
-	       cmd->convert_src);
-	PDEBUG("me4000_ai_do_cmd_test(): convert_arg    = %d\n",
-	       cmd->convert_arg);
-	PDEBUG("me4000_ai_do_cmd_test(): scan_end_src   = %08X\n",
-	       cmd->scan_end_src);
-	PDEBUG("me4000_ai_do_cmd_test(): scan_end_arg   = %d\n",
-	       cmd->scan_end_arg);
-	PDEBUG("me4000_ai_do_cmd_test(): stop_src       = %08X\n",
-	       cmd->stop_src);
-	PDEBUG("me4000_ai_do_cmd_test(): stop_arg       = %d\n", cmd->stop_arg);
-	PDEBUG("me4000_ai_do_cmd_test(): chanlist       = %d\n",
-	       (unsigned int)cmd->chanlist);
-	PDEBUG("me4000_ai_do_cmd_test(): chanlist_len   = %d\n",
-	       cmd->chanlist_len);
-
 	/* Only rounding flags are implemented */
 	cmd->flags &= TRIG_ROUND_NEAREST | TRIG_ROUND_UP | TRIG_ROUND_DOWN;
 
@@ -1544,12 +1416,8 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 	int c = 0;
 	long lval;
 
-	ISR_PDEBUG("me4000_ai_isr() is executed\n");
-
-	if (!dev->attached) {
-		ISR_PDEBUG("me4000_ai_isr() premature interrupt\n");
+	if (!dev->attached)
 		return IRQ_NONE;
-	}
 
 	/* Reset all events */
 	s->async->events = 0;
@@ -1562,19 +1430,14 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 		return IRQ_HANDLED;
 	}
 
-	if (me4000_inl(dev,
-		       ai_context->irq_status_reg) &
+	if (inl(ai_context->irq_status_reg) &
 	    ME4000_IRQ_STATUS_BIT_AI_HF) {
-		ISR_PDEBUG
-		    ("me4000_ai_isr(): Fifo half full interrupt occurred\n");
-
 		/* Read status register to find out what happened */
-		tmp = me4000_inl(dev, ai_context->ctrl_reg);
+		tmp = inl(ai_context->ctrl_reg);
 
 		if (!(tmp & ME4000_AI_STATUS_BIT_FF_DATA) &&
 		    !(tmp & ME4000_AI_STATUS_BIT_HF_DATA) &&
 		    (tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
-			ISR_PDEBUG("me4000_ai_isr(): Fifo full\n");
 			c = ME4000_AI_FIFO_COUNT;
 
 			/*
@@ -1584,7 +1447,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 				 ME4000_AI_CTRL_BIT_SC_IRQ);
-			me4000_outl(dev, tmp, ai_context->ctrl_reg);
+			outl(tmp, ai_context->ctrl_reg);
 
 			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
 
@@ -1594,8 +1457,6 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 		} else if ((tmp & ME4000_AI_STATUS_BIT_FF_DATA)
 			   && !(tmp & ME4000_AI_STATUS_BIT_HF_DATA)
 			   && (tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
-			ISR_PDEBUG("me4000_ai_isr(): Fifo half full\n");
-
 			s->async->events |= COMEDI_CB_BLOCK;
 
 			c = ME4000_AI_FIFO_COUNT / 2;
@@ -1612,7 +1473,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 				 ME4000_AI_CTRL_BIT_SC_IRQ);
-			me4000_outl(dev, tmp, ai_context->ctrl_reg);
+			outl(tmp, ai_context->ctrl_reg);
 
 			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
 
@@ -1621,8 +1482,6 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			       "Undefined FIFO state\n", dev->minor);
 		}
 
-		ISR_PDEBUG("me4000_ai_isr(): Try to read %d values\n", c);
-
 		for (i = 0; i < c; i++) {
 			/* Read value from data fifo */
 			lval = inl(ai_context->data_reg) & 0xFFFF;
@@ -1636,7 +1495,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 				tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 				tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
 					 ME4000_AI_CTRL_BIT_SC_IRQ);
-				me4000_outl(dev, tmp, ai_context->ctrl_reg);
+				outl(tmp, ai_context->ctrl_reg);
 
 				s->async->events |= COMEDI_CB_OVERFLOW;
 
@@ -1649,28 +1508,23 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 		}
 
 		/* Work is done, so reset the interrupt */
-		ISR_PDEBUG("me4000_ai_isr(): Reset fifo half full interrupt\n");
 		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
-		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		outl(tmp, ai_context->ctrl_reg);
 		tmp &= ~ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
-		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		outl(tmp, ai_context->ctrl_reg);
 	}
 
-	if (me4000_inl(dev,
-		       ai_context->irq_status_reg) & ME4000_IRQ_STATUS_BIT_SC) {
-		ISR_PDEBUG
-		    ("me4000_ai_isr(): Sample counter interrupt occurred\n");
-
+	if (inl(ai_context->irq_status_reg) & ME4000_IRQ_STATUS_BIT_SC) {
 		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOA;
 
 		/*
 		 * Acquisition is complete, so stop
 		 * conversion and disable all interrupts
 		 */
-		tmp = me4000_inl(dev, ai_context->ctrl_reg);
+		tmp = inl(ai_context->ctrl_reg);
 		tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
 		tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ);
-		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		outl(tmp, ai_context->ctrl_reg);
 
 		/* Poll data until fifo empty */
 		while (inl(ai_context->ctrl_reg) & ME4000_AI_STATUS_BIT_EF_DATA) {
@@ -1688,16 +1542,12 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 		}
 
 		/* Work is done, so reset the interrupt */
-		ISR_PDEBUG
-		    ("me4000_ai_isr(): Reset interrupt from sample counter\n");
 		tmp |= ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
-		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		outl(tmp, ai_context->ctrl_reg);
 		tmp &= ~ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
-		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		outl(tmp, ai_context->ctrl_reg);
 	}
 
-	ISR_PDEBUG("me4000_ai_isr(): Events = 0x%X\n", s->async->events);
-
 	if (s->async->events)
 		comedi_event(dev, s);
 
@@ -1718,8 +1568,6 @@ static int me4000_ao_insn_write(struct comedi_device *dev,
 	int aref = CR_AREF(insn->chanspec);
 	unsigned long tmp;
 
-	CALL_PDEBUG("In me4000_ao_insn_write()\n");
-
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
@@ -1751,15 +1599,15 @@ static int me4000_ao_insn_write(struct comedi_device *dev,
 	}
 
 	/* Stop any running conversion */
-	tmp = me4000_inl(dev, info->ao_context[chan].ctrl_reg);
+	tmp = inl(info->ao_context[chan].ctrl_reg);
 	tmp |= ME4000_AO_CTRL_BIT_IMMEDIATE_STOP;
-	me4000_outl(dev, tmp, info->ao_context[chan].ctrl_reg);
+	outl(tmp, info->ao_context[chan].ctrl_reg);
 
 	/* Clear control register and set to single mode */
-	me4000_outl(dev, 0x0, info->ao_context[chan].ctrl_reg);
+	outl(0x0, info->ao_context[chan].ctrl_reg);
 
 	/* Write data value */
-	me4000_outl(dev, data[0], info->ao_context[chan].single_reg);
+	outl(data[0], info->ao_context[chan].single_reg);
 
 	/* Store in the mirror */
 	info->ao_context[chan].mirror = data[0];
@@ -1795,20 +1643,6 @@ static int me4000_dio_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-
-	CALL_PDEBUG("In me4000_dio_insn_bits()\n");
-
-	/* Length of data must be 2 (mask and new data, see below) */
-	if (insn->n == 0)
-		return 0;
-
-	if (insn->n != 2) {
-		printk
-		    ("comedi%d: me4000: me4000_dio_insn_bits(): "
-		     "Invalid instruction length\n", dev->minor);
-		return -EINVAL;
-	}
-
 	/*
 	 * The insn data consists of a mask in data[0] and the new data
 	 * in data[1]. The mask defines which bits we are concerning about.
@@ -1824,25 +1658,24 @@ static int me4000_dio_insn_bits(struct comedi_device *dev,
 		s->state |= data[0] & data[1];
 
 		/* Write out the new digital output lines */
-		me4000_outl(dev, (s->state >> 0) & 0xFF,
+		outl((s->state >> 0) & 0xFF,
 			    info->dio_context.port_0_reg);
-		me4000_outl(dev, (s->state >> 8) & 0xFF,
+		outl((s->state >> 8) & 0xFF,
 			    info->dio_context.port_1_reg);
-		me4000_outl(dev, (s->state >> 16) & 0xFF,
+		outl((s->state >> 16) & 0xFF,
 			    info->dio_context.port_2_reg);
-		me4000_outl(dev, (s->state >> 24) & 0xFF,
+		outl((s->state >> 24) & 0xFF,
 			    info->dio_context.port_3_reg);
 	}
 
 	/* On return, data[1] contains the value of
 	   the digital input and output lines. */
-	data[1] =
-	    ((me4000_inl(dev, info->dio_context.port_0_reg) & 0xFF) << 0) |
-	    ((me4000_inl(dev, info->dio_context.port_1_reg) & 0xFF) << 8) |
-	    ((me4000_inl(dev, info->dio_context.port_2_reg) & 0xFF) << 16) |
-	    ((me4000_inl(dev, info->dio_context.port_3_reg) & 0xFF) << 24);
+	data[1] = ((inl(info->dio_context.port_0_reg) & 0xFF) << 0) |
+		  ((inl(info->dio_context.port_1_reg) & 0xFF) << 8) |
+		  ((inl(info->dio_context.port_2_reg) & 0xFF) << 16) |
+		  ((inl(info->dio_context.port_3_reg) & 0xFF) << 24);
 
-	return 2;
+	return insn->n;
 }
 
 static int me4000_dio_insn_config(struct comedi_device *dev,
@@ -1852,8 +1685,6 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 	unsigned long tmp;
 	int chan = CR_CHAN(insn->chanspec);
 
-	CALL_PDEBUG("In me4000_dio_insn_config()\n");
-
 	switch (data[0]) {
 	default:
 		return -EINVAL;
@@ -1874,7 +1705,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 	 * On the ME-4000 it is only possible to switch port wise (8 bit)
 	 */
 
-	tmp = me4000_inl(dev, info->dio_context.ctrl_reg);
+	tmp = inl(info->dio_context.ctrl_reg);
 
 	if (data[0] == INSN_CONFIG_DIO_OUTPUT) {
 		if (chan < 8) {
@@ -1888,7 +1719,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 			 * If one the first port is a fixed output
 			 * port and the second is a fixed input port.
 			 */
-			if (!me4000_inl(dev, info->dio_context.dir_reg))
+			if (!inl(info->dio_context.dir_reg))
 				return -ENODEV;
 
 			s->io_bits |= 0xFF00;
@@ -1915,7 +1746,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 			 * If one the first port is a fixed output
 			 * port and the second is a fixed input port.
 			 */
-			if (!me4000_inl(dev, info->dio_context.dir_reg))
+			if (!inl(info->dio_context.dir_reg))
 				return -ENODEV;
 
 			s->io_bits &= ~0xFF;
@@ -1938,7 +1769,7 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 		}
 	}
 
-	me4000_outl(dev, tmp, info->dio_context.ctrl_reg);
+	outl(tmp, info->dio_context.ctrl_reg);
 
 	return 1;
 }
@@ -1949,24 +1780,21 @@ static int me4000_dio_insn_config(struct comedi_device *dev,
 
 static int cnt_reset(struct comedi_device *dev, unsigned int channel)
 {
-
-	CALL_PDEBUG("In cnt_reset()\n");
-
 	switch (channel) {
 	case 0:
-		me4000_outb(dev, 0x30, info->cnt_context.ctrl_reg);
-		me4000_outb(dev, 0x00, info->cnt_context.counter_0_reg);
-		me4000_outb(dev, 0x00, info->cnt_context.counter_0_reg);
+		outb(0x30, info->cnt_context.ctrl_reg);
+		outb(0x00, info->cnt_context.counter_0_reg);
+		outb(0x00, info->cnt_context.counter_0_reg);
 		break;
 	case 1:
-		me4000_outb(dev, 0x70, info->cnt_context.ctrl_reg);
-		me4000_outb(dev, 0x00, info->cnt_context.counter_1_reg);
-		me4000_outb(dev, 0x00, info->cnt_context.counter_1_reg);
+		outb(0x70, info->cnt_context.ctrl_reg);
+		outb(0x00, info->cnt_context.counter_1_reg);
+		outb(0x00, info->cnt_context.counter_1_reg);
 		break;
 	case 2:
-		me4000_outb(dev, 0xB0, info->cnt_context.ctrl_reg);
-		me4000_outb(dev, 0x00, info->cnt_context.counter_2_reg);
-		me4000_outb(dev, 0x00, info->cnt_context.counter_2_reg);
+		outb(0xB0, info->cnt_context.ctrl_reg);
+		outb(0x00, info->cnt_context.counter_2_reg);
+		outb(0x00, info->cnt_context.counter_2_reg);
 		break;
 	default:
 		printk(KERN_ERR
@@ -1983,8 +1811,6 @@ static int cnt_config(struct comedi_device *dev, unsigned int channel,
 {
 	int tmp = 0;
 
-	CALL_PDEBUG("In cnt_config()\n");
-
 	switch (channel) {
 	case 0:
 		tmp |= ME4000_CNT_COUNTER_0;
@@ -2030,7 +1856,7 @@ static int cnt_config(struct comedi_device *dev, unsigned int channel,
 
 	/* Write the control word */
 	tmp |= 0x30;
-	me4000_outb(dev, tmp, info->cnt_context.ctrl_reg);
+	outb(tmp, info->cnt_context.ctrl_reg);
 
 	return 0;
 }
@@ -2042,8 +1868,6 @@ static int me4000_cnt_insn_config(struct comedi_device *dev,
 
 	int err;
 
-	CALL_PDEBUG("In me4000_cnt_insn_config()\n");
-
 	switch (data[0]) {
 	case GPCT_RESET:
 		if (insn->n != 1) {
@@ -2088,8 +1912,6 @@ static int me4000_cnt_insn_read(struct comedi_device *dev,
 
 	unsigned short tmp;
 
-	CALL_PDEBUG("In me4000_cnt_insn_read()\n");
-
 	if (insn->n == 0)
 		return 0;
 
@@ -2103,21 +1925,21 @@ static int me4000_cnt_insn_read(struct comedi_device *dev,
 
 	switch (insn->chanspec) {
 	case 0:
-		tmp = me4000_inb(dev, info->cnt_context.counter_0_reg);
+		tmp = inb(info->cnt_context.counter_0_reg);
 		data[0] = tmp;
-		tmp = me4000_inb(dev, info->cnt_context.counter_0_reg);
+		tmp = inb(info->cnt_context.counter_0_reg);
 		data[0] |= tmp << 8;
 		break;
 	case 1:
-		tmp = me4000_inb(dev, info->cnt_context.counter_1_reg);
+		tmp = inb(info->cnt_context.counter_1_reg);
 		data[0] = tmp;
-		tmp = me4000_inb(dev, info->cnt_context.counter_1_reg);
+		tmp = inb(info->cnt_context.counter_1_reg);
 		data[0] |= tmp << 8;
 		break;
 	case 2:
-		tmp = me4000_inb(dev, info->cnt_context.counter_2_reg);
+		tmp = inb(info->cnt_context.counter_2_reg);
 		data[0] = tmp;
-		tmp = me4000_inb(dev, info->cnt_context.counter_2_reg);
+		tmp = inb(info->cnt_context.counter_2_reg);
 		data[0] |= tmp << 8;
 		break;
 	default:
@@ -2138,8 +1960,6 @@ static int me4000_cnt_insn_write(struct comedi_device *dev,
 
 	unsigned short tmp;
 
-	CALL_PDEBUG("In me4000_cnt_insn_write()\n");
-
 	if (insn->n == 0) {
 		return 0;
 	} else if (insn->n > 1) {
@@ -2153,21 +1973,21 @@ static int me4000_cnt_insn_write(struct comedi_device *dev,
 	switch (insn->chanspec) {
 	case 0:
 		tmp = data[0] & 0xFF;
-		me4000_outb(dev, tmp, info->cnt_context.counter_0_reg);
+		outb(tmp, info->cnt_context.counter_0_reg);
 		tmp = (data[0] >> 8) & 0xFF;
-		me4000_outb(dev, tmp, info->cnt_context.counter_0_reg);
+		outb(tmp, info->cnt_context.counter_0_reg);
 		break;
 	case 1:
 		tmp = data[0] & 0xFF;
-		me4000_outb(dev, tmp, info->cnt_context.counter_1_reg);
+		outb(tmp, info->cnt_context.counter_1_reg);
 		tmp = (data[0] >> 8) & 0xFF;
-		me4000_outb(dev, tmp, info->cnt_context.counter_1_reg);
+		outb(tmp, info->cnt_context.counter_1_reg);
 		break;
 	case 2:
 		tmp = data[0] & 0xFF;
-		me4000_outb(dev, tmp, info->cnt_context.counter_2_reg);
+		outb(tmp, info->cnt_context.counter_2_reg);
 		tmp = (data[0] >> 8) & 0xFF;
-		me4000_outb(dev, tmp, info->cnt_context.counter_2_reg);
+		outb(tmp, info->cnt_context.counter_2_reg);
 		break;
 	default:
 		printk(KERN_ERR
@@ -2185,19 +2005,13 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int result;
 
-	CALL_PDEBUG("In me4000_attach()\n");
-
 	result = me4000_probe(dev, it);
 	if (result)
 		return result;
 
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.  It relies on
-	 * n_subdevices being set correctly.
-	 */
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	result = comedi_alloc_subdevices(dev, 4);
+	if (result)
+		return result;
 
     /*=========================================================================
       Analog input subdevice
@@ -2277,10 +2091,9 @@ static int me4000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	 * Check for optoisolated ME-4000 version. If one the first
 	 * port is a fixed output port and the second is a fixed input port.
 	 */
-	if (!me4000_inl(dev, info->dio_context.dir_reg)) {
+	if (!inl(info->dio_context.dir_reg)) {
 		s->io_bits |= 0xFF;
-		me4000_outl(dev, ME4000_DIO_CTRL_BIT_MODE_0,
-			    info->dio_context.dir_reg);
+		outl(ME4000_DIO_CTRL_BIT_MODE_0, info->dio_context.dir_reg);
 	}
 
     /*=========================================================================
diff --git a/drivers/staging/comedi/drivers/me4000.h b/drivers/staging/comedi/drivers/me4000.h
index 733b19243c7..5a4df4e4b23 100644
--- a/drivers/staging/comedi/drivers/me4000.h
+++ b/drivers/staging/comedi/drivers/me4000.h
@@ -25,43 +25,6 @@
 #define _ME4000_H_
 
 /*=============================================================================
-  Debug section
-  ===========================================================================*/
-
-#undef ME4000_CALL_DEBUG	/*  Debug function entry and exit */
-#undef ME4000_PORT_DEBUG	/*  Debug port access */
-#undef ME4000_ISR_DEBUG		/*  Debug the interrupt service routine */
-#undef ME4000_DEBUG		/*  General purpose debug masseges */
-
-#ifdef ME4000_CALL_DEBUG
-#undef CALL_PDEBUG
-#define CALL_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor, ##args)
-#else
-# define CALL_PDEBUG(fmt, args...)	/*  no debugging, do nothing */
-#endif
-
-#ifdef ME4000_PORT_DEBUG
-#undef PORT_PDEBUG
-#define PORT_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
-#else
-#define PORT_PDEBUG(fmt, args...)	/*  no debugging, do nothing */
-#endif
-
-#ifdef ME4000_ISR_DEBUG
-#undef ISR_PDEBUG
-#define ISR_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
-#else
-#define ISR_PDEBUG(fmt, args...)	/*  no debugging, do nothing */
-#endif
-
-#ifdef ME4000_DEBUG
-#undef PDEBUG
-#define PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
-#else
-#define PDEBUG(fmt, args...)	/*  no debugging, do nothing */
-#endif
-
-/*=============================================================================
   PCI vendor and device IDs
   ===========================================================================*/
 
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index ffe251250e6..8c6f8b93b27 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -54,8 +54,6 @@ from http://www.comedi.org
 #include <linux/sched.h>
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 /*#include "me2600_fw.h" */
 
 #define ME_DRIVER_NAME		"me_daq"
@@ -233,11 +231,8 @@ static const struct me_board me_boards[] = {
 	 }
 };
 
-#define me_board_nbr (sizeof(me_boards)/sizeof(struct me_board))
-
 /* Private data structure */
 struct me_private_data {
-	struct pci_dev *pci_device;
 	void __iomem *plx_regbase;	/* PLX configuration base address */
 	void __iomem *me_regbase;	/* Base address of the Meilhaus card */
 	unsigned long plx_regbase_size;	/* Size of PLX configuration space */
@@ -333,7 +328,7 @@ static int me_dio_insn_bits(struct comedi_device *dev,
 		data[1] |= readw(dev_private->me_regbase + ME_DIO_PORT_B) << 16;
 	}
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -612,9 +607,42 @@ static int me_reset(struct comedi_device *dev)
 	return 0;
 }
 
+static struct pci_dev *me_find_pci_dev(struct comedi_device *dev,
+				       struct comedi_devconfig *it)
+{
+	const struct me_board *board;
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (bus || slot) {
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		if (pcidev->vendor != PCI_VENDOR_ID_MEILHAUS)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(me_boards); i++) {
+			board = &me_boards[i];
+			if (board->device_id != pcidev->device)
+				continue;
+
+			dev->board_ptr = board;
+			return pcidev;
+		}
+	}
+	dev_err(dev->class_dev,
+		"No supported board found! (req. bus %d, slot %d)\n",
+		bus, slot);
+	return NULL;
+}
+
 static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	struct pci_dev *pci_device = NULL;
+	struct pci_dev *pci_device;
 	struct comedi_subdevice *subdevice;
 	struct me_board *board;
 	resource_size_t plx_regbase_tmp;
@@ -624,54 +652,17 @@ static int me_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	resource_size_t swap_regbase_tmp;
 	unsigned long swap_regbase_size_tmp;
 	resource_size_t regbase_tmp;
-	int result, error, i;
+	int result, error;
 
 	/* Allocate private memory */
 	if (alloc_private(dev, sizeof(struct me_private_data)) < 0)
 		return -ENOMEM;
 
-	/* Probe the device to determine what device in the series it is. */
-	for_each_pci_dev(pci_device) {
-		if (pci_device->vendor == PCI_VENDOR_ID_MEILHAUS) {
-			for (i = 0; i < me_board_nbr; i++) {
-				if (me_boards[i].device_id ==
-				    pci_device->device) {
-					/*
-					 * was a particular bus/slot requested?
-					 */
-					if ((it->options[0] != 0)
-					    || (it->options[1] != 0)) {
-						/*
-						 * are we on the wrong bus/slot?
-						 */
-						if (pci_device->bus->number !=
-						    it->options[0]
-						    ||
-						    PCI_SLOT(pci_device->devfn)
-						    != it->options[1]) {
-							continue;
-						}
-					}
-
-					dev->board_ptr = me_boards + i;
-					board =
-					    (struct me_board *)dev->board_ptr;
-					dev_private->pci_device = pci_device;
-					goto found;
-				}
-			}
-		}
-	}
-
-	printk(KERN_ERR
-	       "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
-	       dev->minor, it->options[0], it->options[1]);
-	return -EIO;
-
-found:
-	printk(KERN_INFO "comedi%d: found %s at PCI bus %d, slot %d\n",
-	       dev->minor, me_boards[i].name,
-	       pci_device->bus->number, PCI_SLOT(pci_device->devfn));
+	pci_device = me_find_pci_dev(dev, it);
+	if (!pci_device)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pci_device->dev);
+	board = (struct me_board *)dev->board_ptr;
 
 	/* Enable PCI device and request PCI regions */
 	if (comedi_pci_enable(pci_device, ME_DRIVER_NAME) < 0) {
@@ -763,9 +754,8 @@ found:
 
 	me_reset(dev);
 
-	/* device driver capabilities */
-	error = alloc_subdevices(dev, 3);
-	if (error < 0)
+	error = comedi_alloc_subdevices(dev, 3);
+	if (error)
 		return error;
 
 	subdevice = dev->subdevices + 0;
@@ -808,6 +798,8 @@ found:
 
 static void me_detach(struct comedi_device *dev)
 {
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
 	if (dev_private) {
 		if (dev_private->me_regbase) {
 			me_reset(dev);
@@ -815,11 +807,11 @@ static void me_detach(struct comedi_device *dev)
 		}
 		if (dev_private->plx_regbase)
 			iounmap(dev_private->plx_regbase);
-		if (dev_private->pci_device) {
-			if (dev_private->plx_regbase_size)
-				comedi_pci_disable(dev_private->pci_device);
-			pci_dev_put(dev_private->pci_device);
-		}
+	}
+	if (pcidev) {
+		if (dev_private->plx_regbase_size)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
 	}
 }
 
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index 13e9c807169..a93166d6a8f 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -52,7 +52,6 @@
 #include "mite.h"
 
 #include "comedi_fc.h"
-#include "comedi_pci.h"
 #include "../comedidev.h"
 
 
diff --git a/drivers/staging/comedi/drivers/mpc624.c b/drivers/staging/comedi/drivers/mpc624.c
index 4304e864a4d..b928b6763cd 100644
--- a/drivers/staging/comedi/drivers/mpc624.c
+++ b/drivers/staging/comedi/drivers/mpc624.c
@@ -285,6 +285,7 @@ static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
 	unsigned long iobase;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: mpc624 [0x%04lx, ", dev->minor, iobase);
@@ -348,9 +349,9 @@ static int mpc624_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		devpriv->ulConvertionRate = MPC624_SPEED_3_52_kHz;
 	}
 
-	/*  Subdevices structures */
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	s->type = COMEDI_SUBD_AI;
diff --git a/drivers/staging/comedi/drivers/mpc8260cpm.c b/drivers/staging/comedi/drivers/mpc8260cpm.c
index 364470e4458..a7fda8f01e8 100644
--- a/drivers/staging/comedi/drivers/mpc8260cpm.c
+++ b/drivers/staging/comedi/drivers/mpc8260cpm.c
@@ -113,7 +113,7 @@ static int mpc8260cpm_dio_bits(struct comedi_device *dev,
 
 	p = cpm_pdat((int)s->private);
 
-	return 2;
+	return insn->n;
 }
 
 static int mpc8260cpm_attach(struct comedi_device *dev,
@@ -121,6 +121,7 @@ static int mpc8260cpm_attach(struct comedi_device *dev,
 {
 	struct comedi_subdevice *s;
 	int i;
+	int ret;
 
 	printk("comedi%d: mpc8260cpm: ", dev->minor);
 
@@ -131,8 +132,9 @@ static int mpc8260cpm_attach(struct comedi_device *dev,
 	if (alloc_private(dev, sizeof(struct mpc8260cpm_private)) < 0)
 		return -ENOMEM;
 
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	ret =comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	for (i = 0; i < 4; i++) {
 		s = dev->subdevices + i;
diff --git a/drivers/staging/comedi/drivers/multiq3.c b/drivers/staging/comedi/drivers/multiq3.c
index e951e73d66f..eccbe1fb4f2 100644
--- a/drivers/staging/comedi/drivers/multiq3.c
+++ b/drivers/staging/comedi/drivers/multiq3.c
@@ -161,28 +161,22 @@ static int multiq3_di_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inw(dev->iobase + MULTIQ3_DIGIN_PORT);
 
-	return 2;
+	return insn->n;
 }
 
 static int multiq3_do_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	s->state &= ~data[0];
 	s->state |= (data[0] & data[1]);
 	outw(s->state, dev->iobase + MULTIQ3_DIGOUT_PORT);
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int multiq3_encoder_insn_read(struct comedi_device *dev,
@@ -255,8 +249,9 @@ static int multiq3_attach(struct comedi_device *dev,
 	else
 		printk(KERN_WARNING "comedi%d: no irq\n", dev->minor);
 	dev->board_name = "multiq3";
-	result = alloc_subdevices(dev, 5);
-	if (result < 0)
+
+	result = comedi_alloc_subdevices(dev, 5);
+	if (result)
 		return result;
 
 	result = alloc_private(dev, sizeof(struct multiq3_private));
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index b02aa0efcd8..a80c52fb273 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -174,22 +174,17 @@ static int ni6527_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = readb(devpriv->mite->daq_io_addr + Port_Register(0));
 	data[1] |= readb(devpriv->mite->daq_io_addr + Port_Register(1)) << 8;
 	data[1] |= readb(devpriv->mite->daq_io_addr + Port_Register(2)) << 16;
 
-	return 2;
+	return insn->n;
 }
 
 static int ni6527_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -211,7 +206,7 @@ static int ni6527_do_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static irqreturn_t ni6527_interrupt(int irq, void *d)
@@ -339,11 +334,8 @@ static int ni6527_intr_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n < 1)
-		return -EINVAL;
-
 	data[1] = 0;
-	return 2;
+	return insn->n;
 }
 
 static int ni6527_intr_insn_config(struct comedi_device *dev,
@@ -397,8 +389,8 @@ static int ni6527_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	printk(KERN_INFO "comedi board: %s, ID=0x%02x\n", dev->board_name,
 		readb(devpriv->mite->daq_io_addr + ID_Register));
 
-	ret = alloc_subdevices(dev, 3);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 0d27a9323bc..bce39f1ea36 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -415,8 +415,7 @@ static int ni_65xx_dio_insn_bits(struct comedi_device *dev,
 	const unsigned max_ports_per_bitfield = 5;
 	unsigned read_bits = 0;
 	unsigned j;
-	if (insn->n != 2)
-		return -EINVAL;
+
 	base_bitfield_channel = CR_CHAN(insn->chanspec);
 	for (j = 0; j < max_ports_per_bitfield; ++j) {
 		const unsigned port_offset =
@@ -602,11 +601,8 @@ static int ni_65xx_intr_insn_bits(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n < 1)
-		return -EINVAL;
-
 	data[1] = 0;
-	return 2;
+	return insn->n;
 }
 
 static int ni_65xx_intr_insn_config(struct comedi_device *dev,
@@ -678,8 +674,8 @@ static int ni_65xx_attach(struct comedi_device *dev,
 	printk(KERN_INFO " ID=0x%02x",
 	       readb(private(dev)->mite->daq_io_addr + ID_Register));
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 8c40730e296..5e863ff343d 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -1093,10 +1093,9 @@ static int ni_660x_attach(struct comedi_device *dev,
 
 	printk(KERN_INFO " %s ", dev->board_name);
 
-	dev->n_subdevices = 2 + NI_660X_MAX_NUM_COUNTERS;
-
-	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2 + NI_660X_MAX_NUM_COUNTERS);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* Old GENERAL-PURPOSE COUNTER/TIME (GPCT) subdevice, no longer used */
@@ -1286,7 +1285,7 @@ static int ni_660x_dio_insn_bits(struct comedi_device *dev,
 	data[1] =
 	    (ni_660x_read_register(dev, 0,
 				   DIO32Input) >> base_bitfield_channel);
-	return 2;
+	return insn->n;
 }
 
 static void ni_660x_select_pfi_output(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index a9cf94fd0c3..9c57618f2c5 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -61,43 +61,27 @@ Commands are not supported.
 /* Board description*/
 
 struct ni_670x_board {
-	unsigned short dev_id;
 	const char *name;
+	unsigned short dev_id;
 	unsigned short ao_chans;
-	unsigned short ao_bits;
 };
 
 static const struct ni_670x_board ni_670x_boards[] = {
 	{
-	 .dev_id = 0x2c90,
-	 .name = "PCI-6703",
-	 .ao_chans = 16,
-	 .ao_bits = 16,
-	 },
-	{
-	 .dev_id = 0x1920,
-	 .name = "PXI-6704",
-	 .ao_chans = 32,
-	 .ao_bits = 16,
-	 },
-	{
-	 .dev_id = 0x1290,
-	 .name = "PCI-6704",
-	 .ao_chans = 32,
-	 .ao_bits = 16,
-	 },
-};
-
-static DEFINE_PCI_DEVICE_TABLE(ni_670x_pci_table) = {
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c90)},
-	{PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1920)},
-	{0}
+		.name		= "PCI-6703",
+		.dev_id		= 0x2c90,
+		.ao_chans	= 16,
+	}, {
+		.name		= "PXI-6704",
+		.dev_id		= 0x1920,
+		.ao_chans	= 32,
+	}, {
+		.name		= "PCI-6704",
+		.dev_id		= 0x1290,
+		.ao_chans	= 32,
+	},
 };
 
-MODULE_DEVICE_TABLE(pci, ni_670x_pci_table);
-
-#define thisboard ((struct ni_670x_board *)dev->board_ptr)
-
 struct ni_670x_private {
 
 	struct mite_struct *mite;
@@ -106,162 +90,13 @@ struct ni_670x_private {
 	unsigned int ao_readback[32];
 };
 
-#define devpriv ((struct ni_670x_private *)dev->private)
-#define n_ni_670x_boards ARRAY_SIZE(ni_670x_boards)
-
-static int ni_670x_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it);
-static void ni_670x_detach(struct comedi_device *dev);
-
-static struct comedi_driver driver_ni_670x = {
-	.driver_name = "ni_670x",
-	.module = THIS_MODULE,
-	.attach = ni_670x_attach,
-	.detach = ni_670x_detach,
-};
-
-static int __devinit driver_ni_670x_pci_probe(struct pci_dev *dev,
-					      const struct pci_device_id *ent)
-{
-	return comedi_pci_auto_config(dev, &driver_ni_670x);
-}
-
-static void __devexit driver_ni_670x_pci_remove(struct pci_dev *dev)
-{
-	comedi_pci_auto_unconfig(dev);
-}
-
-static struct pci_driver driver_ni_670x_pci_driver = {
-	.id_table = ni_670x_pci_table,
-	.probe = &driver_ni_670x_pci_probe,
-	.remove = __devexit_p(&driver_ni_670x_pci_remove)
-};
-
-static int __init driver_ni_670x_init_module(void)
-{
-	int retval;
-
-	retval = comedi_driver_register(&driver_ni_670x);
-	if (retval < 0)
-		return retval;
-
-	driver_ni_670x_pci_driver.name = (char *)driver_ni_670x.driver_name;
-	return pci_register_driver(&driver_ni_670x_pci_driver);
-}
-
-static void __exit driver_ni_670x_cleanup_module(void)
-{
-	pci_unregister_driver(&driver_ni_670x_pci_driver);
-	comedi_driver_unregister(&driver_ni_670x);
-}
-
-module_init(driver_ni_670x_init_module);
-module_exit(driver_ni_670x_cleanup_module);
-
 static struct comedi_lrange range_0_20mA = { 1, {RANGE_mA(0, 20)} };
 
-static int ni_670x_find_device(struct comedi_device *dev, int bus, int slot);
-
-static int ni_670x_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int ni_670x_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data);
-static int ni_670x_dio_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data);
-static int ni_670x_dio_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data);
-
-static int ni_670x_attach(struct comedi_device *dev,
-			  struct comedi_devconfig *it)
-{
-	struct comedi_subdevice *s;
-	int ret;
-	int i;
-
-	printk(KERN_INFO "comedi%d: ni_670x: ", dev->minor);
-
-	ret = alloc_private(dev, sizeof(struct ni_670x_private));
-	if (ret < 0)
-		return ret;
-
-	ret = ni_670x_find_device(dev, it->options[0], it->options[1]);
-	if (ret < 0)
-		return ret;
-
-	ret = mite_setup(devpriv->mite);
-	if (ret < 0) {
-		printk(KERN_WARNING "error setting up mite\n");
-		return ret;
-	}
-	dev->board_name = thisboard->name;
-	dev->irq = mite_irq(devpriv->mite);
-	printk(KERN_INFO " %s", dev->board_name);
-
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
-
-	s = dev->subdevices + 0;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = 0xffff;
-	if (s->n_chan == 32) {
-		const struct comedi_lrange **range_table_list;
-
-		range_table_list = kmalloc(sizeof(struct comedi_lrange *) * 32,
-					   GFP_KERNEL);
-		if (!range_table_list)
-			return -ENOMEM;
-		s->range_table_list = range_table_list;
-		for (i = 0; i < 16; i++) {
-			range_table_list[i] = &range_bipolar10;
-			range_table_list[16 + i] = &range_0_20mA;
-		}
-	} else {
-		s->range_table = &range_bipolar10;
-	}
-	s->insn_write = &ni_670x_ao_winsn;
-	s->insn_read = &ni_670x_ao_rinsn;
-
-	s = dev->subdevices + 1;
-	/* digital i/o subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 8;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = ni_670x_dio_insn_bits;
-	s->insn_config = ni_670x_dio_insn_config;
-
-	/* Config of misc registers */
-	writel(0x10, devpriv->mite->daq_io_addr + MISC_CONTROL_OFFSET);
-	/* Config of ao registers */
-	writel(0x00, devpriv->mite->daq_io_addr + AO_CONTROL_OFFSET);
-
-	printk(KERN_INFO "attached\n");
-
-	return 1;
-}
-
-static void ni_670x_detach(struct comedi_device *dev)
-{
-	kfree(dev->subdevices[0].range_table_list);
-	if (dev->private && devpriv->mite)
-		mite_unsetup(devpriv->mite);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-}
-
 static int ni_670x_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct ni_670x_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -292,6 +127,7 @@ static int ni_670x_ao_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
+	struct ni_670x_private *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -305,29 +141,29 @@ static int ni_670x_dio_insn_bits(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
+	struct ni_670x_private *devpriv = dev->private;
+	void __iomem *io_addr = devpriv->mite->daq_io_addr +
+					DIO_PORT0_DATA_OFFSET;
+	unsigned int mask = data[0];
+	unsigned int bits = data[1];
+
+	if (mask) {
+		s->state &= ~mask;
+		s->state |= (bits & mask);
 
-	/* The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit. */
-	if (data[0]) {
-		s->state &= ~data[0];
-		s->state |= data[0] & data[1];
-		writel(s->state,
-		       devpriv->mite->daq_io_addr + DIO_PORT0_DATA_OFFSET);
+		writel(s->state, io_addr);
 	}
 
-	/* on return, data[1] contains the value of the digital
-	 * input lines. */
-	data[1] = readl(devpriv->mite->daq_io_addr + DIO_PORT0_DATA_OFFSET);
+	data[1] = readl(io_addr);
 
-	return 2;
+	return insn->n;
 }
 
 static int ni_670x_dio_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn, unsigned int *data)
 {
+	struct ni_670x_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 
 	switch (data[0]) {
@@ -353,6 +189,7 @@ static int ni_670x_dio_insn_config(struct comedi_device *dev,
 
 static int ni_670x_find_device(struct comedi_device *dev, int bus, int slot)
 {
+	struct ni_670x_private *devpriv = dev->private;
 	struct mite_struct *mite;
 	int i;
 
@@ -365,7 +202,7 @@ static int ni_670x_find_device(struct comedi_device *dev, int bus, int slot)
 				continue;
 		}
 
-		for (i = 0; i < n_ni_670x_boards; i++) {
+		for (i = 0; i < ARRAY_SIZE(ni_670x_boards); i++) {
 			if (mite_device_id(mite) == ni_670x_boards[i].dev_id) {
 				dev->board_ptr = ni_670x_boards + i;
 				devpriv->mite = mite;
@@ -374,11 +211,136 @@ static int ni_670x_find_device(struct comedi_device *dev, int bus, int slot)
 			}
 		}
 	}
-	printk(KERN_INFO "no device found\n");
+	dev_warn(dev->class_dev, "no device found\n");
 	mite_list_devices();
 	return -EIO;
 }
 
+static int ni_670x_attach(struct comedi_device *dev,
+			  struct comedi_devconfig *it)
+{
+	const struct ni_670x_board *thisboard;
+	struct ni_670x_private *devpriv;
+	struct comedi_subdevice *s;
+	int ret;
+	int i;
+
+	ret = alloc_private(dev, sizeof(*devpriv));
+	if (ret < 0)
+		return ret;
+	devpriv = dev->private;
+
+	ret = ni_670x_find_device(dev, it->options[0], it->options[1]);
+	if (ret < 0)
+		return ret;
+	thisboard = comedi_board(dev);
+
+	ret = mite_setup(devpriv->mite);
+	if (ret < 0) {
+		dev_warn(dev->class_dev, "error setting up mite\n");
+		return ret;
+	}
+	dev->board_name = thisboard->name;
+	dev->irq = mite_irq(devpriv->mite);
+
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
+
+	s = dev->subdevices + 0;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = thisboard->ao_chans;
+	s->maxdata = 0xffff;
+	if (s->n_chan == 32) {
+		const struct comedi_lrange **range_table_list;
+
+		range_table_list = kmalloc(sizeof(struct comedi_lrange *) * 32,
+					   GFP_KERNEL);
+		if (!range_table_list)
+			return -ENOMEM;
+		s->range_table_list = range_table_list;
+		for (i = 0; i < 16; i++) {
+			range_table_list[i] = &range_bipolar10;
+			range_table_list[16 + i] = &range_0_20mA;
+		}
+	} else {
+		s->range_table = &range_bipolar10;
+	}
+	s->insn_write = &ni_670x_ao_winsn;
+	s->insn_read = &ni_670x_ao_rinsn;
+
+	s = dev->subdevices + 1;
+	/* digital i/o subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = ni_670x_dio_insn_bits;
+	s->insn_config = ni_670x_dio_insn_config;
+
+	/* Config of misc registers */
+	writel(0x10, devpriv->mite->daq_io_addr + MISC_CONTROL_OFFSET);
+	/* Config of ao registers */
+	writel(0x00, devpriv->mite->daq_io_addr + AO_CONTROL_OFFSET);
+
+	dev_info(dev->class_dev, "%s: %s attached\n",
+		dev->driver->driver_name, dev->board_name);
+
+	return 0;
+}
+
+static void ni_670x_detach(struct comedi_device *dev)
+{
+	struct ni_670x_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+
+	if (dev->n_subdevices) {
+		s = dev->subdevices + 0;
+		if (s)
+			kfree(s->range_table_list);
+	}
+	if (devpriv && devpriv->mite)
+		mite_unsetup(devpriv->mite);
+	if (dev->irq)
+		free_irq(dev->irq, dev);
+}
+
+static struct comedi_driver ni_670x_driver = {
+	.driver_name	= "ni_670x",
+	.module		= THIS_MODULE,
+	.attach		= ni_670x_attach,
+	.detach		= ni_670x_detach,
+};
+
+static int __devinit ni_670x_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *ent)
+{
+	return comedi_pci_auto_config(dev, &ni_670x_driver);
+}
+
+static void __devexit ni_670x_pci_remove(struct pci_dev *dev)
+{
+	comedi_pci_auto_unconfig(dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(ni_670x_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x2c90) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_NI, 0x1920) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, ni_670x_pci_table);
+
+static struct pci_driver ni_670x_pci_driver = {
+	.name		="ni_670x",
+	.id_table	= ni_670x_pci_table,
+	.probe		= ni_670x_pci_probe,
+	.remove		= __devexit_p(ni_670x_pci_remove),
+};
+module_comedi_pci_driver(ni_670x_driver, ni_670x_pci_driver);
+
 MODULE_AUTHOR("Comedi http://www.comedi.org");
 MODULE_DESCRIPTION("Comedi low-level driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index ae896a09415..b53a4286f8c 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -755,6 +755,7 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	unsigned int dma = it->options[2];
 	static const int timeout = 2000;
 	int i;
+	int ret;
 
 	printk("comedi%d: %s: io 0x%lx", dev->minor, dev->driver->driver_name,
 	       iobase);
@@ -826,8 +827,9 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->board_ptr = a2150_boards + a2150_probe(dev);
 	dev->board_name = thisboard->name;
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	/* analog input subdevice */
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index c43dd8ada1d..62c8c44a8d2 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -157,8 +157,6 @@ struct atao_board {
 	int n_ao_chans;
 };
 
-#define thisboard ((struct atao_board *)dev->board_ptr)
-
 struct atao_private {
 
 	unsigned short cfg1;
@@ -241,9 +239,6 @@ static int atao_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
@@ -252,7 +247,7 @@ static int atao_dio_insn_bits(struct comedi_device *dev,
 
 	data[1] = inw(dev->iobase + ATAO_DIN);
 
-	return 2;
+	return insn->n;
 }
 
 static int atao_dio_insn_config(struct comedi_device *dev,
@@ -335,9 +330,11 @@ static int atao_calib_insn_write(struct comedi_device *dev,
 
 static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct atao_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	int ao_unipolar;
+	int ret;
 
 	iobase = it->options[0];
 	if (iobase == 0)
@@ -352,21 +349,20 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	dev->iobase = iobase;
 
-	/* dev->board_ptr = atao_probe(dev); */
-
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	if (alloc_private(dev, sizeof(struct atao_private)) < 0)
 		return -ENOMEM;
 
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->n_ao_chans;
+	s->n_chan = board->n_ao_chans;
 	s->maxdata = (1 << 12) - 1;
 	if (ao_unipolar)
 		s->range_table = &range_unipolar10;
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 4f614532674..2c78d3dd242 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -110,8 +110,6 @@ struct atmio16_board_t {
 	int has_8255;
 };
 
-#define boardtype ((const struct atmio16_board_t *)dev->board_ptr)
-
 /* range structs */
 static const struct comedi_lrange range_atmio16d_ai_10_bipolar = { 4, {
 								       BIP_RANGE
@@ -238,10 +236,6 @@ static irqreturn_t atmio16d_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = dev->subdevices + 0;
 
-#ifdef DEBUG1
-	printk(KERN_DEBUG "atmio16d_interrupt!\n");
-#endif
-
 	comedi_buf_put(s->async, inw(dev->iobase + AD_FIFO_REG));
 
 	comedi_event(dev, s);
@@ -253,9 +247,7 @@ static int atmio16d_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_cmd *cmd)
 {
 	int err = 0, tmp;
-#ifdef DEBUG1
-	printk(KERN_DEBUG "atmio16d_ai_cmdtest\n");
-#endif
+
 	/* make sure triggers are valid */
 	tmp = cmd->start_src;
 	cmd->start_src &= TRIG_NOW;
@@ -357,9 +349,7 @@ static int atmio16d_ai_cmd(struct comedi_device *dev,
 	unsigned int timer, base_clock;
 	unsigned int sample_count, tmp, chan, gain;
 	int i;
-#ifdef DEBUG1
-	printk(KERN_DEBUG "atmio16d_ai_cmd\n");
-#endif
+
 	/* This is slowly becoming a working command interface. *
 	 * It is still uber-experimental */
 
@@ -519,9 +509,6 @@ static int atmio16d_ai_insn_read(struct comedi_device *dev,
 	int gain;
 	int status;
 
-#ifdef DEBUG1
-	printk(KERN_DEBUG "atmio16d_ai_insn_read\n");
-#endif
 	chan = CR_CHAN(insn->chanspec);
 	gain = CR_RANGE(insn->chanspec);
 
@@ -540,9 +527,6 @@ static int atmio16d_ai_insn_read(struct comedi_device *dev,
 		for (t = 0; t < ATMIO16D_TIMEOUT; t++) {
 			/* check conversion status */
 			status = inw(dev->iobase + STAT_REG);
-#ifdef DEBUG1
-			printk(KERN_DEBUG "status=%x\n", status);
-#endif
 			if (status & STAT_AD_CONVAVAIL) {
 				/* read the data now */
 				data[i] = inw(dev->iobase + AD_FIFO_REG);
@@ -574,9 +558,6 @@ static int atmio16d_ao_insn_read(struct comedi_device *dev,
 				 struct comedi_insn *insn, unsigned int *data)
 {
 	int i;
-#ifdef DEBUG1
-	printk(KERN_DEBUG "atmio16d_ao_insn_read\n");
-#endif
 
 	for (i = 0; i < insn->n; i++)
 		data[i] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
@@ -590,9 +571,6 @@ static int atmio16d_ao_insn_write(struct comedi_device *dev,
 	int i;
 	int chan;
 	int d;
-#ifdef DEBUG1
-	printk(KERN_DEBUG "atmio16d_ao_insn_write\n");
-#endif
 
 	chan = CR_CHAN(insn->chanspec);
 
@@ -621,9 +599,6 @@ static int atmio16d_dio_insn_bits(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] | data[1]);
@@ -631,7 +606,7 @@ static int atmio16d_dio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = inw(dev->iobase + MIO_16_DIG_IN_REG);
 
-	return 2;
+	return insn->n;
 }
 
 static int atmio16d_dio_insn_config(struct comedi_device *dev,
@@ -693,6 +668,7 @@ static int atmio16d_dio_insn_config(struct comedi_device *dev,
 static int atmio16d_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
+	const struct atmio16_board_t *board = comedi_board(dev);
 	unsigned int irq;
 	unsigned long iobase;
 	int ret;
@@ -708,11 +684,10 @@ static int atmio16d_attach(struct comedi_device *dev,
 	}
 	dev->iobase = iobase;
 
-	/* board name */
-	dev->board_name = boardtype->name;
+	dev->board_name = board->name;
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct atmio16d_private));
@@ -811,7 +786,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 
 	/* 8255 subdevice */
 	s++;
-	if (boardtype->has_8255)
+	if (board->has_8255)
 		subdev_8255_init(dev, s, NULL, dev->iobase);
 	else
 		s->type = COMEDI_SUBD_UNUSED;
@@ -831,7 +806,9 @@ static int atmio16d_attach(struct comedi_device *dev,
 
 static void atmio16d_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices && boardtype->has_8255)
+	const struct atmio16_board_t *board = comedi_board(dev);
+
+	if (dev->subdevices && board->has_8255)
 		subdev_8255_cleanup(dev, dev->subdevices + 3);
 	if (dev->irq)
 		free_irq(dev->irq, dev);
diff --git a/drivers/staging/comedi/drivers/ni_daq_700.c b/drivers/staging/comedi/drivers/ni_daq_700.c
index 75764e8d27e..83016b41185 100644
--- a/drivers/staging/comedi/drivers/ni_daq_700.c
+++ b/drivers/staging/comedi/drivers/ni_daq_700.c
@@ -37,8 +37,6 @@ port, bit 0; channel 8 corresponds to the input port, bit 0.
 
 Direction configuration: channels 0-7 output, 8-15 input (8225 device
 emu as port A output, port B input, port C N/A).
-
-IRQ is assigned but not used.
 */
 
 #include <linux/interrupt.h>
@@ -53,111 +51,13 @@ IRQ is assigned but not used.
 
 static struct pcmcia_device *pcmcia_cur_dev;
 
-#define DIO700_SIZE 8		/*  size of io region used by board */
-
-static int dio700_attach(struct comedi_device *dev,
-			 struct comedi_devconfig *it);
-static void dio700_detach(struct comedi_device *dev);
-
-enum dio700_bustype { pcmcia_bustype };
-
 struct dio700_board {
 	const char *name;
-	int device_id;		/*  device id for pcmcia board */
-	enum dio700_bustype bustype;	/*  PCMCIA */
-	int have_dio;		/*  have daqcard-700 dio */
-	/*  function pointers so we can use inb/outb or readb/writeb */
-	/*  as appropriate */
-	unsigned int (*read_byte) (unsigned int address);
-	void (*write_byte) (unsigned int byte, unsigned int address);
-};
-
-static const struct dio700_board dio700_boards[] = {
-	{
-	 .name = "daqcard-700",
-	  /*  0x10b is manufacturer id, 0x4743 is device id */
-	 .device_id = 0x4743,
-	 .bustype = pcmcia_bustype,
-	 .have_dio = 1,
-	 },
-	{
-	 .name = "ni_daq_700",
-	  /*  0x10b is manufacturer id, 0x4743 is device id */
-	 .device_id = 0x4743,
-	 .bustype = pcmcia_bustype,
-	 .have_dio = 1,
-	 },
-};
-
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct dio700_board *)dev->board_ptr)
-
-struct dio700_private {
-
-	int data;		/* number of data points left to be taken */
 };
 
-#define devpriv ((struct dio700_private *)dev->private)
-
-static struct comedi_driver driver_dio700 = {
-	.driver_name = "ni_daq_700",
-	.module = THIS_MODULE,
-	.attach = dio700_attach,
-	.detach = dio700_detach,
-	.num_names = ARRAY_SIZE(dio700_boards),
-	.board_name = &dio700_boards[0].name,
-	.offset = sizeof(struct dio700_board),
-};
-
-/*	the real driver routines	*/
-
-#define _700_SIZE 8
-
-#define _700_DATA 0
-
 #define DIO_W		0x04
 #define DIO_R		0x05
 
-struct subdev_700_struct {
-	unsigned long cb_arg;
-	int (*cb_func) (int, int, int, unsigned long);
-	int have_irq;
-};
-
-#define CALLBACK_ARG	(((struct subdev_700_struct *)s->private)->cb_arg)
-#define CALLBACK_FUNC	(((struct subdev_700_struct *)s->private)->cb_func)
-#define subdevpriv	((struct subdev_700_struct *)s->private)
-
-static void do_config(struct comedi_device *dev, struct comedi_subdevice *s);
-
-void subdev_700_interrupt(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	short d;
-
-	d = CALLBACK_FUNC(0, _700_DATA, 0, CALLBACK_ARG);
-
-	comedi_buf_put(s->async, d);
-	s->async->events |= COMEDI_CB_EOS;
-
-	comedi_event(dev, s);
-}
-EXPORT_SYMBOL(subdev_700_interrupt);
-
-static int subdev_700_cb(int dir, int port, int data, unsigned long arg)
-{
-	/* port is always A for output and B for input (8255 emu) */
-	unsigned long iobase = arg;
-
-	if (dir) {
-		outb(data, iobase + DIO_W);
-		return 0;
-	} else {
-		return inb(iobase + DIO_R);
-	}
-}
-
 static int subdev_700_insn(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_insn *insn,
 			   unsigned int *data)
@@ -167,20 +67,20 @@ static int subdev_700_insn(struct comedi_device *dev,
 		s->state |= (data[0] & data[1]);
 
 		if (data[0] & 0xff)
-			CALLBACK_FUNC(1, _700_DATA, s->state & 0xff,
-				      CALLBACK_ARG);
+			outb(s->state & 0xff, dev->iobase + DIO_W);
 	}
 
 	data[1] = s->state & 0xff;
-	data[1] |= CALLBACK_FUNC(0, _700_DATA, 0, CALLBACK_ARG) << 8;
+	data[1] |= inb(dev->iobase + DIO_R);
 
-	return 2;
+	return insn->n;
 }
 
 static int subdev_700_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn, unsigned int *data)
 {
+	unsigned int chan = 1 << CR_CHAN(insn->chanspec);
 
 	switch (data[0]) {
 	case INSN_CONFIG_DIO_INPUT:
@@ -188,291 +88,81 @@ static int subdev_700_insn_config(struct comedi_device *dev,
 	case INSN_CONFIG_DIO_OUTPUT:
 		break;
 	case INSN_CONFIG_DIO_QUERY:
-		data[1] =
-		    (s->
-		     io_bits & (1 << CR_CHAN(insn->chanspec))) ? COMEDI_OUTPUT :
-		    COMEDI_INPUT;
-		return insn->n;
+		data[1] = (s->io_bits & chan) ? COMEDI_OUTPUT : COMEDI_INPUT;
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	return 1;
-}
-
-static void do_config(struct comedi_device *dev, struct comedi_subdevice *s)
-{				/* use powerup defaults */
-	return;
-}
-
-static int subdev_700_cmdtest(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_cmd *cmd)
-{
-	int err = 0;
-	unsigned int tmp;
-
-	/* step 1 */
-
-	tmp = cmd->start_src;
-	cmd->start_src &= TRIG_NOW;
-	if (!cmd->start_src || tmp != cmd->start_src)
-		err++;
-
-	tmp = cmd->scan_begin_src;
-	cmd->scan_begin_src &= TRIG_EXT;
-	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
-		err++;
-
-	tmp = cmd->convert_src;
-	cmd->convert_src &= TRIG_FOLLOW;
-	if (!cmd->convert_src || tmp != cmd->convert_src)
-		err++;
-
-	tmp = cmd->scan_end_src;
-	cmd->scan_end_src &= TRIG_COUNT;
-	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
-		err++;
-
-	tmp = cmd->stop_src;
-	cmd->stop_src &= TRIG_NONE;
-	if (!cmd->stop_src || tmp != cmd->stop_src)
-		err++;
-
-	if (err)
-		return 1;
-
-	/* step 2 */
-
-	if (err)
-		return 2;
-
-	/* step 3 */
-
-	if (cmd->start_arg != 0) {
-		cmd->start_arg = 0;
-		err++;
-	}
-	if (cmd->scan_begin_arg != 0) {
-		cmd->scan_begin_arg = 0;
-		err++;
-	}
-	if (cmd->convert_arg != 0) {
-		cmd->convert_arg = 0;
-		err++;
-	}
-	if (cmd->scan_end_arg != 1) {
-		cmd->scan_end_arg = 1;
-		err++;
-	}
-	if (cmd->stop_arg != 0) {
-		cmd->stop_arg = 0;
-		err++;
-	}
-
-	if (err)
-		return 3;
-
-	/* step 4 */
-
-	if (err)
-		return 4;
-
-	return 0;
-}
-
-static int subdev_700_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	/* FIXME */
-
-	return 0;
-}
-
-static int subdev_700_cancel(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
-{
-	/* FIXME */
-
-	return 0;
-}
-
-int subdev_700_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		    int (*cb) (int, int, int, unsigned long), unsigned long arg)
-{
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 16;
-	s->range_table = &range_digital;
-	s->maxdata = 1;
-
-	s->private = kmalloc(sizeof(struct subdev_700_struct), GFP_KERNEL);
-	if (!s->private)
-		return -ENOMEM;
-
-	CALLBACK_ARG = arg;
-	if (cb == NULL)
-		CALLBACK_FUNC = subdev_700_cb;
-	 else
-		CALLBACK_FUNC = cb;
-
-	s->insn_bits = subdev_700_insn;
-	s->insn_config = subdev_700_insn_config;
-
-	s->state = 0;
-	s->io_bits = 0x00ff;
-	do_config(dev, s);
-
-	return 0;
-}
-EXPORT_SYMBOL(subdev_700_init);
-
-int subdev_700_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
-			int (*cb) (int, int, int, unsigned long),
-			unsigned long arg)
-{
-	int ret;
-
-	ret = subdev_700_init(dev, s, cb, arg);
-	if (ret < 0)
-		return ret;
-
-	s->do_cmdtest = subdev_700_cmdtest;
-	s->do_cmd = subdev_700_cmd;
-	s->cancel = subdev_700_cancel;
-
-	subdevpriv->have_irq = 1;
-
-	return 0;
+	return insn->n;
 }
-EXPORT_SYMBOL(subdev_700_init_irq);
-
-void subdev_700_cleanup(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	if (s->private)
-		if (subdevpriv->have_irq)
-
-			kfree(s->private);
-}
-EXPORT_SYMBOL(subdev_700_cleanup);
 
 static int dio700_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct dio700_board *thisboard = comedi_board(dev);
 	struct comedi_subdevice *s;
-	unsigned long iobase = 0;
-#ifdef incomplete
-	unsigned int irq = 0;
-#endif
 	struct pcmcia_device *link;
+	int ret;
 
-	/* allocate and initialize dev->private */
-	if (alloc_private(dev, sizeof(struct dio700_private)) < 0)
-		return -ENOMEM;
-
-	/*  get base address, irq etc. based on bustype */
-	switch (thisboard->bustype) {
-	case pcmcia_bustype:
-		link = pcmcia_cur_dev;	/* XXX hack */
-		if (!link)
-			return -EIO;
-		iobase = link->resource[0]->start;
-#ifdef incomplete
-		irq = link->irq;
-#endif
-		break;
-	default:
-		printk(KERN_ERR "bug! couldn't determine board type\n");
-		return -EINVAL;
-		break;
-	}
-	printk(KERN_ERR "comedi%d: ni_daq_700: %s, io 0x%lx", dev->minor,
-	       thisboard->name, iobase);
-#ifdef incomplete
-	if (irq)
-		printk(", irq %u", irq);
-
-#endif
-
-	printk("\n");
+	link = pcmcia_cur_dev;	/* XXX hack */
+	if (!link)
+		return -EIO;
 
-	if (iobase == 0) {
-		printk(KERN_ERR "io base address is zero!\n");
+	dev->iobase = link->resource[0]->start;
+	if (!dev->iobase) {
+		dev_err(dev->class_dev, "io base address is zero!\n");
 		return -EINVAL;
 	}
 
-	dev->iobase = iobase;
-
-#ifdef incomplete
-	/* grab our IRQ */
-	dev->irq = irq;
-#endif
-
 	dev->board_name = thisboard->name;
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	/* DAQCard-700 dio */
 	s = dev->subdevices + 0;
-	subdev_700_init(dev, s, NULL, dev->iobase);
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 16;
+	s->range_table	= &range_digital;
+	s->maxdata	= 1;
+	s->insn_bits	= subdev_700_insn;
+	s->insn_config	= subdev_700_insn_config;
+
+	s->state	= 0;
+	s->io_bits	= 0x00ff;
+
+	dev_info(dev->class_dev, "%s: %s, io 0x%lx\n",
+		dev->driver->driver_name,
+		dev->board_name,
+		dev->iobase);
 
 	return 0;
-};
+}
 
 static void dio700_detach(struct comedi_device *dev)
 {
-	if (dev->subdevices)
-		subdev_700_cleanup(dev, dev->subdevices + 0);
-	if (thisboard->bustype != pcmcia_bustype && dev->iobase)
-		release_region(dev->iobase, DIO700_SIZE);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-};
-
-static void dio700_config(struct pcmcia_device *link);
-static void dio700_release(struct pcmcia_device *link);
-static int dio700_cs_suspend(struct pcmcia_device *p_dev);
-static int dio700_cs_resume(struct pcmcia_device *p_dev);
-
-static int dio700_cs_attach(struct pcmcia_device *);
-static void dio700_cs_detach(struct pcmcia_device *);
+	/* nothing to cleanup */
+}
 
-struct local_info_t {
-	struct pcmcia_device *link;
-	int stop;
-	struct bus_operations *bus;
+static const struct dio700_board dio700_boards[] = {
+	{
+		.name		= "daqcard-700",
+	}, {
+		.name		= "ni_daq_700",
+	},
 };
 
-static int dio700_cs_attach(struct pcmcia_device *link)
-{
-	struct local_info_t *local;
-
-	printk(KERN_INFO "ni_daq_700:  cs-attach\n");
-
-	dev_dbg(&link->dev, "dio700_cs_attach()\n");
-
-	/* Allocate space for private device-specific data */
-	local = kzalloc(sizeof(struct local_info_t), GFP_KERNEL);
-	if (!local)
-		return -ENOMEM;
-	local->link = link;
-	link->priv = local;
-
-	pcmcia_cur_dev = link;
-
-	dio700_config(link);
-
-	return 0;
-}				/* dio700_cs_attach */
-
-static void dio700_cs_detach(struct pcmcia_device *link)
-{
-	((struct local_info_t *)link->priv)->stop = 1;
-	dio700_release(link);
-
-	/* This points to the parent struct local_info_t struct */
-	kfree(link->priv);
-}
+static struct comedi_driver driver_dio700 = {
+	.driver_name	= "ni_daq_700",
+	.module		= THIS_MODULE,
+	.attach		= dio700_attach,
+	.detach		= dio700_detach,
+	.board_name	= &dio700_boards[0].name,
+	.num_names	= ARRAY_SIZE(dio700_boards),
+	.offset		= sizeof(struct dio700_board),
+};
 
 static int dio700_pcmcia_config_loop(struct pcmcia_device *p_dev,
 				void *priv_data)
@@ -483,112 +173,78 @@ static int dio700_pcmcia_config_loop(struct pcmcia_device *p_dev,
 	return pcmcia_request_io(p_dev);
 }
 
-static void dio700_config(struct pcmcia_device *link)
+static int dio700_cs_attach(struct pcmcia_device *link)
 {
 	int ret;
 
-	printk(KERN_INFO "ni_daq_700:  cs-config\n");
-
-	dev_dbg(&link->dev, "dio700_config\n");
-
 	link->config_flags |= CONF_ENABLE_IRQ | CONF_AUTO_AUDIO |
 		CONF_AUTO_SET_IO;
 
 	ret = pcmcia_loop_config(link, dio700_pcmcia_config_loop, NULL);
-	if (ret) {
-		dev_warn(&link->dev, "no configuration found\n");
+	if (ret)
 		goto failed;
-	}
 
 	if (!link->irq)
 		goto failed;
 
 	ret = pcmcia_enable_device(link);
-	if (ret != 0)
+	if (ret)
 		goto failed;
 
-	return;
+	pcmcia_cur_dev = link;
+	return 0;
 
 failed:
-	printk(KERN_INFO "ni_daq_700 cs failed");
-	dio700_release(link);
-
-}				/* dio700_config */
-
-static void dio700_release(struct pcmcia_device *link)
-{
-	dev_dbg(&link->dev, "dio700_release\n");
-
 	pcmcia_disable_device(link);
-}				/* dio700_release */
-
-static int dio700_cs_suspend(struct pcmcia_device *link)
-{
-	struct local_info_t *local = link->priv;
-
-	/* Mark the device as stopped, to block IO until later */
-	local->stop = 1;
-	return 0;
-}				/* dio700_cs_suspend */
+	return ret;
+}
 
-static int dio700_cs_resume(struct pcmcia_device *link)
+static void dio700_cs_detach(struct pcmcia_device *link)
 {
-	struct local_info_t *local = link->priv;
-
-	local->stop = 0;
-	return 0;
-}				/* dio700_cs_resume */
-
-/*====================================================================*/
+	pcmcia_disable_device(link);
+	pcmcia_cur_dev = NULL;
+}
 
 static const struct pcmcia_device_id dio700_cs_ids[] = {
-	/* N.B. These IDs should match those in dio700_boards */
-	PCMCIA_DEVICE_MANF_CARD(0x010b, 0x4743),	/* daqcard-700 */
+	PCMCIA_DEVICE_MANF_CARD(0x010b, 0x4743),
 	PCMCIA_DEVICE_NULL
 };
-
-
 MODULE_DEVICE_TABLE(pcmcia, dio700_cs_ids);
-MODULE_AUTHOR("Fred Brooks <nsaspook@nsaspook.com>");
-MODULE_DESCRIPTION("Comedi driver for National Instruments "
-		   "PCMCIA DAQCard-700 DIO");
-MODULE_LICENSE("GPL");
 
-struct pcmcia_driver dio700_cs_driver = {
-	.probe = dio700_cs_attach,
-	.remove = dio700_cs_detach,
-	.suspend = dio700_cs_suspend,
-	.resume = dio700_cs_resume,
-	.id_table = dio700_cs_ids,
-	.owner = THIS_MODULE,
-	.name = "ni_daq_700",
+static struct pcmcia_driver dio700_cs_driver = {
+	.name		= "ni_daq_700",
+	.owner		= THIS_MODULE,
+	.probe		= dio700_cs_attach,
+	.remove		= dio700_cs_detach,
+	.id_table	= dio700_cs_ids,
 };
 
-static int __init init_dio700_cs(void)
-{
-	pcmcia_register_driver(&dio700_cs_driver);
-	return 0;
-}
-
-static void __exit exit_dio700_cs(void)
-{
-	pr_debug("ni_daq_700: unloading\n");
-	pcmcia_unregister_driver(&dio700_cs_driver);
-}
-
-int __init init_module(void)
+static int __init dio700_cs_init(void)
 {
 	int ret;
 
-	ret = init_dio700_cs();
+	ret = comedi_driver_register(&driver_dio700);
 	if (ret < 0)
 		return ret;
 
-	return comedi_driver_register(&driver_dio700);
+	ret = pcmcia_register_driver(&dio700_cs_driver);
+	if (ret < 0) {
+		comedi_driver_unregister(&driver_dio700);
+		return ret;
+	}
+
+	return 0;
 }
+module_init(dio700_cs_init);
 
-void __exit cleanup_module(void)
+static void __exit dio700_cs_exit(void)
 {
-	exit_dio700_cs();
+	pcmcia_unregister_driver(&dio700_cs_driver);
 	comedi_driver_unregister(&driver_dio700);
 }
+module_exit(dio700_cs_exit);
+
+MODULE_AUTHOR("Fred Brooks <nsaspook@nsaspook.com>");
+MODULE_DESCRIPTION(
+	"Comedi driver for National Instruments PCMCIA DAQCard-700 DIO");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_daq_dio24.c b/drivers/staging/comedi/drivers/ni_daq_dio24.c
index 493a2278863..e27cae0eb8a 100644
--- a/drivers/staging/comedi/drivers/ni_daq_dio24.c
+++ b/drivers/staging/comedi/drivers/ni_daq_dio24.c
@@ -116,6 +116,7 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	unsigned int irq = 0;
 #endif
 	struct pcmcia_device *link;
+	int ret;
 
 	/* allocate and initialize dev->private */
 	if (alloc_private(dev, sizeof(struct dio24_private)) < 0)
@@ -158,8 +159,9 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = thisboard->name;
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	/* 8255 dio */
 	s = dev->subdevices + 0;
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index 53349777246..ab8b787c78b 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -536,6 +536,7 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 	unsigned long dma_flags;
 #endif
 	short lsb, msb;
+	int ret;
 
 	printk(KERN_ERR "comedi%d: ni_labpc: %s, io 0x%lx", dev->minor,
 								thisboard->name,
@@ -622,8 +623,9 @@ int labpc_common_attach(struct comedi_device *dev, unsigned long iobase,
 
 	dev->board_name = thisboard->name;
 
-	if (alloc_subdevices(dev, 5) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 5);
+	if (ret)
+		return ret;
 
 	/* analog input subdevice */
 	s = dev->subdevices + 0;
@@ -1386,6 +1388,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		break;
 	default:
 		comedi_error(dev, "bug with start_src");
+		spin_unlock_irqrestore(&dev->spinlock, flags);
 		return -1;
 		break;
 	}
@@ -1398,6 +1401,7 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		break;
 	default:
 		comedi_error(dev, "bug with stop_src");
+		spin_unlock_irqrestore(&dev->spinlock, flags);
 		return -1;
 	}
 	devpriv->write_byte(devpriv->command2_bits, dev->iobase + COMMAND2_REG);
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index fd232bc5f87..cf0e0d147f8 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -3567,8 +3567,7 @@ static int ni_dio_insn_bits(struct comedi_device *dev,
 #ifdef DEBUG_DIO
 	printk("ni_dio_insn_bits() mask=0x%x bits=0x%x\n", data[0], data[1]);
 #endif
-	if (insn->n != 2)
-		return -EINVAL;
+
 	if (data[0]) {
 		/* Perform check to make sure we're not using the
 		   serial part of the dio */
@@ -3585,7 +3584,7 @@ static int ni_dio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = devpriv->stc_readw(dev, DIO_Parallel_Input_Register);
 
-	return 2;
+	return insn->n;
 }
 
 static int ni_m_series_dio_insn_config(struct comedi_device *dev,
@@ -3629,8 +3628,7 @@ static int ni_m_series_dio_insn_bits(struct comedi_device *dev,
 	printk("ni_m_series_dio_insn_bits() mask=0x%x bits=0x%x\n", data[0],
 	       data[1]);
 #endif
-	if (insn->n != 2)
-		return -EINVAL;
+
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -3638,7 +3636,7 @@ static int ni_m_series_dio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = ni_readl(M_Offset_Static_Digital_Input);
 
-	return 2;
+	return insn->n;
 }
 
 static int ni_cdio_cmdtest(struct comedi_device *dev,
@@ -4406,14 +4404,16 @@ static int ni_E_init(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	unsigned j;
 	enum ni_gpct_variant counter_variant;
+	int ret;
 
 	if (boardtype.n_aochan > MAX_N_AO_CHAN) {
 		printk("bug! boardtype.n_aochan > MAX_N_AO_CHAN\n");
 		return -EINVAL;
 	}
 
-	if (alloc_subdevices(dev, NI_NUM_SUBDEVICES) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, NI_NUM_SUBDEVICES);
+	if (ret)
+		return ret;
 
 	/* analog input subdevice */
 
@@ -5394,7 +5394,7 @@ static int ni_pfi_insn_bits(struct comedi_device *dev,
 		ni_writew(s->state, M_Offset_PFI_DO);
 	}
 	data[1] = ni_readw(M_Offset_PFI_DI);
-	return 2;
+	return insn->n;
 }
 
 static int ni_pfi_insn_config(struct comedi_device *dev,
@@ -5483,12 +5483,9 @@ static int ni_rtsi_insn_bits(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = 0;
 
-	return 2;
+	return insn->n;
 }
 
 /* Find best multiplier/divider to try and get the PLL running at 80 MHz
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 37b700830e2..0a55de96803 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -745,8 +745,6 @@ static int ni_pcidio_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -754,7 +752,7 @@ static int ni_pcidio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = readl(devpriv->mite->daq_io_addr + Port_IO(0));
 
-	return 2;
+	return insn->n;
 }
 
 static int ni_pcidio_cmdtest(struct comedi_device *dev,
@@ -1248,8 +1246,8 @@ static int nidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	else
 		n_subdevices = 1;
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
 
 	if (!this_board->is_diodaq) {
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 3974c0d98d2..89f4d43c6d0 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1593,7 +1593,7 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
 
-	dev_info(dev->hw_dev, "comedi%d: ni_pcimio:\n", dev->minor);
+	dev_info(dev->class_dev, "ni_pcimio: attach\n");
 
 	ret = ni_alloc_private(dev);
 	if (ret < 0)
@@ -1603,7 +1603,7 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 
-	dev_dbg(dev->hw_dev, "%s\n", boardtype.name);
+	dev_dbg(dev->class_dev, "%s\n", boardtype.name);
 	dev->board_name = boardtype.name;
 
 	if (boardtype.reg_type & ni_reg_m_series_mask) {
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index 2e7753f988a..bb72d0bc297 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -148,8 +148,6 @@ struct pcl711_board {
 	const struct comedi_lrange *ai_range_type;
 };
 
-#define this_board ((const struct pcl711_board *)dev->board_ptr)
-
 struct pcl711_private {
 
 	int board;
@@ -169,6 +167,7 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 	int lo, hi;
 	int data;
 	struct comedi_device *dev = d;
+	const struct pcl711_board *board = comedi_board(dev);
 	struct comedi_subdevice *s = dev->subdevices + 0;
 
 	if (!dev->attached) {
@@ -184,7 +183,7 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 
 	/* FIXME! Nothing else sets ntrig! */
 	if (!(--devpriv->ntrig)) {
-		if (this_board->is_8112)
+		if (board->is_8112)
 			outb(1, dev->iobase + PCL711_MODE);
 		else
 			outb(0, dev->iobase + PCL711_MODE);
@@ -197,13 +196,14 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 
 static void pcl711_set_changain(struct comedi_device *dev, int chan)
 {
+	const struct pcl711_board *board = comedi_board(dev);
 	int chan_register;
 
 	outb(CR_RANGE(chan), dev->iobase + PCL711_GAIN);
 
 	chan_register = CR_CHAN(chan);
 
-	if (this_board->is_8112) {
+	if (board->is_8112) {
 
 		/*
 		 *  Set the correct channel.  The two channel banks are switched
@@ -225,6 +225,7 @@ static void pcl711_set_changain(struct comedi_device *dev, int chan)
 static int pcl711_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
+	const struct pcl711_board *board = comedi_board(dev);
 	int i, n;
 	int hi, lo;
 
@@ -237,7 +238,7 @@ static int pcl711_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 		 */
 		outb(1, dev->iobase + PCL711_MODE);
 
-		if (!this_board->is_8112)
+		if (!board->is_8112)
 			outb(0, dev->iobase + PCL711_SOFTTRIG);
 
 		i = PCL711_TIMEOUT;
@@ -448,13 +449,10 @@ static int pcl711_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + PCL711_DI_LO) |
 	    (inb(dev->iobase + PCL711_DI_HI) << 8);
 
-	return 2;
+	return insn->n;
 }
 
 /* Digital port write - Untested on 8112 */
@@ -462,9 +460,6 @@ static int pcl711_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
@@ -476,11 +471,12 @@ static int pcl711_do_insn_bits(struct comedi_device *dev,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl711_board *board = comedi_board(dev);
 	int ret;
 	unsigned long iobase;
 	unsigned int irq;
@@ -498,12 +494,11 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* there should be a sanity check here */
 
-	/* set up some name stuff */
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	/* grab our IRQ */
 	irq = it->options[1];
-	if (irq > this_board->maxirq) {
+	if (irq > board->maxirq) {
 		printk(KERN_ERR "irq out of range\n");
 		return -EINVAL;
 	}
@@ -517,8 +512,8 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	dev->irq = irq;
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct pcl711_private));
@@ -529,10 +524,10 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* AI subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
-	s->n_chan = this_board->n_aichan;
+	s->n_chan = board->n_aichan;
 	s->maxdata = 0xfff;
 	s->len_chanlist = 1;
-	s->range_table = this_board->ai_range_type;
+	s->range_table = board->ai_range_type;
 	s->insn_read = pcl711_ai_insn;
 	if (irq) {
 		dev->read_subdev = s;
@@ -545,7 +540,7 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* AO subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = this_board->n_aochan;
+	s->n_chan = board->n_aochan;
 	s->maxdata = 0xfff;
 	s->len_chanlist = 1;
 	s->range_table = &range_bipolar5;
@@ -577,7 +572,7 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	   this is the "base value" for the mode register, which is
 	   used for the irq on the PCL711
 	 */
-	if (this_board->is_pcl711b)
+	if (board->is_pcl711b)
 		devpriv->mode = (dev->irq << 4);
 
 	/* clear DAC */
diff --git a/drivers/staging/comedi/drivers/pcl724.c b/drivers/staging/comedi/drivers/pcl724.c
index 1f66fe1c7d5..c8fe23ca899 100644
--- a/drivers/staging/comedi/drivers/pcl724.c
+++ b/drivers/staging/comedi/drivers/pcl724.c
@@ -67,8 +67,6 @@ struct pcl724_board {
 	char is_pet48;
 };
 
-#define this_board ((const struct pcl724_board *)dev->board_ptr)
-
 static int subdev_8255_cb(int dir, int port, int data, unsigned long arg)
 {
 	unsigned long iobase = arg;
@@ -100,6 +98,7 @@ static int subdev_8255mapped_cb(int dir, int port, int data,
 
 static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl724_board *board = comedi_board(dev);
 	unsigned long iobase;
 	unsigned int iorange;
 	int ret, i, n_subdevices;
@@ -108,12 +107,12 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 
 	iobase = it->options[0];
-	iorange = this_board->io_range;
-	if ((this_board->can_have96) && ((it->options[1] == 1)
+	iorange = board->io_range;
+	if ((board->can_have96) && ((it->options[1] == 1)
 					 || (it->options[1] == 96)))
 		iorange = PCL722_96_SIZE; /* PCL-724 in 96 DIO configuration */
 	printk(KERN_INFO "comedi%d: pcl724: board=%s, 0x%03lx ", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 	if (!request_region(iobase, iorange, "pcl724")) {
 		printk("I/O port conflict\n");
 		return -EIO;
@@ -121,14 +120,14 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->iobase = iobase;
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 #ifdef PCL724_IRQ
 	irq = 0;
-	if (this_board->IRQbits != 0) {	/* board support IRQ */
+	if (board->IRQbits != 0) {	/* board support IRQ */
 		irq = it->options[1];
 		if (irq) {	/* we want to use IRQ */
-			if (((1 << irq) & this_board->IRQbits) == 0) {
+			if (((1 << irq) & board->IRQbits) == 0) {
 				printk(KERN_WARNING
 				       ", IRQ %u is out of allowed range, "
 				       "DISABLING IT", irq);
@@ -152,17 +151,17 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	printk("\n");
 
-	n_subdevices = this_board->numofports;
-	if ((this_board->can_have96) && ((it->options[1] == 1)
+	n_subdevices = board->numofports;
+	if ((board->can_have96) && ((it->options[1] == 1)
 					 || (it->options[1] == 96)))
 		n_subdevices = 4;	/*  PCL-724 in 96 DIO configuration */
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
-		if (this_board->is_pet48) {
+		if (board->is_pet48) {
 			subdev_8255_init(dev, dev->subdevices + i,
 					 subdev_8255mapped_cb,
 					 (unsigned long)(dev->iobase +
@@ -179,6 +178,7 @@ static int pcl724_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcl724_detach(struct comedi_device *dev)
 {
+	const struct pcl724_board *board = comedi_board(dev);
 	int i;
 
 	for (i = 0; i < dev->n_subdevices; i++)
@@ -187,7 +187,7 @@ static void pcl724_detach(struct comedi_device *dev)
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 #endif
-	release_region(dev->iobase, this_board->io_range);
+	release_region(dev->iobase, board->io_range);
 }
 
 static const struct pcl724_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcl725.c b/drivers/staging/comedi/drivers/pcl725.c
index 83a6fa53ddd..d5b60cf7c93 100644
--- a/drivers/staging/comedi/drivers/pcl725.c
+++ b/drivers/staging/comedi/drivers/pcl725.c
@@ -23,9 +23,6 @@ Devices: [Advantech] PCL-725 (pcl725)
 static int pcl725_do_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -34,24 +31,22 @@ static int pcl725_do_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl725_di_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + PCL725_DI);
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
 	unsigned long iobase;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: pcl725: 0x%04lx ", dev->minor, iobase);
@@ -63,8 +58,9 @@ static int pcl725_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->iobase = iobase;
 	dev->irq = 0;
 
-	if (alloc_subdevices(dev, 2) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* do */
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index d25c30c694e..2b10f1d8308 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -145,8 +145,6 @@ static const struct pcl726_board boardtypes[] = {
 	 &rangelist_728[0],},
 };
 
-#define this_board ((const struct pcl726_board *)dev->board_ptr)
-
 struct pcl726_private {
 
 	int bipolar[12];
@@ -197,38 +195,37 @@ static int pcl726_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
+	const struct pcl726_board *board = comedi_board(dev);
 
-	data[1] = inb(dev->iobase + this_board->di_lo) |
-	    (inb(dev->iobase + this_board->di_hi) << 8);
+	data[1] = inb(dev->iobase + board->di_lo) |
+	    (inb(dev->iobase + board->di_hi) << 8);
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl726_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
+	const struct pcl726_board *board = comedi_board(dev);
 
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
 	}
 	if (data[1] & 0x00ff)
-		outb(s->state & 0xff, dev->iobase + this_board->do_lo);
+		outb(s->state & 0xff, dev->iobase + board->do_lo);
 	if (data[1] & 0xff00)
-		outb((s->state >> 8), dev->iobase + this_board->do_hi);
+		outb((s->state >> 8), dev->iobase + board->do_hi);
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl726_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	unsigned int iorange;
@@ -238,9 +235,9 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 
 	iobase = it->options[0];
-	iorange = this_board->io_range;
+	iorange = board->io_range;
 	printk(KERN_WARNING "comedi%d: pcl726: board=%s, 0x%03lx ", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 	if (!request_region(iobase, iorange, "pcl726")) {
 		printk(KERN_WARNING "I/O port conflict\n");
 		return -EIO;
@@ -248,7 +245,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->iobase = iobase;
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	ret = alloc_private(dev, sizeof(struct pcl726_private));
 	if (ret < 0)
@@ -289,31 +286,31 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	printk("\n");
 
-	ret = alloc_subdevices(dev, 3);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
 	/* ao */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-	s->n_chan = this_board->n_aochan;
+	s->n_chan = board->n_aochan;
 	s->maxdata = 0xfff;
 	s->len_chanlist = 1;
 	s->insn_write = pcl726_ao_insn;
 	s->insn_read = pcl726_ao_insn_read;
 	s->range_table_list = devpriv->rangelist;
-	for (i = 0; i < this_board->n_aochan; i++) {
+	for (i = 0; i < board->n_aochan; i++) {
 		int j;
 
 		j = it->options[2 + 1];
-		if ((j < 0) || (j >= this_board->num_of_ranges)) {
+		if ((j < 0) || (j >= board->num_of_ranges)) {
 			printk
 			    ("Invalid range for channel %d! Must be 0<=%d<%d\n",
-			     i, j, this_board->num_of_ranges - 1);
+			     i, j, board->num_of_ranges - 1);
 			j = 0;
 		}
-		devpriv->rangelist[i] = this_board->range_type_list[j];
+		devpriv->rangelist[i] = board->range_type_list[j];
 		if (devpriv->rangelist[i]->range[0].min ==
 		    -devpriv->rangelist[i]->range[0].max)
 			devpriv->bipolar[i] = 1;	/* bipolar range */
@@ -321,7 +318,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	s = dev->subdevices + 1;
 	/* di */
-	if (!this_board->have_dio) {
+	if (!board->have_dio) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
 		s->type = COMEDI_SUBD_DI;
@@ -335,7 +332,7 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	s = dev->subdevices + 2;
 	/* do */
-	if (!this_board->have_dio) {
+	if (!board->have_dio) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
 		s->type = COMEDI_SUBD_DO;
@@ -352,12 +349,14 @@ static int pcl726_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcl726_detach(struct comedi_device *dev)
 {
+	const struct pcl726_board *board = comedi_board(dev);
+
 #ifdef ACL6126_IRQ
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 #endif
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
+		release_region(dev->iobase, board->io_range);
 }
 
 static struct comedi_driver pcl726_driver = {
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index e11704added..4675ec57082 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -32,14 +32,9 @@ struct pcl730_board {
 	unsigned int io_range;	/*  len of I/O space */
 };
 
-#define this_board ((const struct pcl730_board *)dev->board_ptr)
-
 static int pcl730_do_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -53,41 +48,41 @@ static int pcl730_do_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl730_di_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + ((unsigned long)s->private)) |
 	    (inb(dev->iobase + ((unsigned long)s->private) + 1) << 8);
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl730_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	unsigned int iorange;
+	int ret;
 
 	iobase = it->options[0];
-	iorange = this_board->io_range;
+	iorange = board->io_range;
 	printk(KERN_INFO "comedi%d: pcl730: board=%s 0x%04lx ", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 	if (!request_region(iobase, iorange, "pcl730")) {
 		printk("I/O port conflict\n");
 		return -EIO;
 	}
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 	dev->iobase = iobase;
 	dev->irq = 0;
 
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* Isolated do */
@@ -136,8 +131,10 @@ static int pcl730_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcl730_detach(struct comedi_device *dev)
 {
+	const struct pcl730_board *board = comedi_board(dev);
+
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
+		release_region(dev->iobase, board->io_range);
 }
 
 static const struct pcl730_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 51f4ca9f792..578fd8920be 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -119,9 +119,6 @@
 
 #include "8253.h"
 
-/* if this is defined then a lot of messages is printed */
-#undef PCL812_EXTDEBUG
-
 /* hardware types of the cards */
 #define boardPCL812PG	      0	/* and ACL-8112PG */
 #define boardPCL813B	      1
@@ -336,8 +333,6 @@ struct pcl812_board {
 	unsigned char haveMPC508;	/*  1=board use MPC508A multiplexor */
 };
 
-#define this_board ((const struct pcl812_board *)dev->board_ptr)
-
 struct pcl812_private {
 
 	unsigned char valid;	/*  =1 device is OK */
@@ -506,13 +501,10 @@ static int pcl812_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + PCL812_DI_LO);
 	data[1] |= inb(dev->iobase + PCL812_DI_HI) << 8;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -522,9 +514,6 @@ static int pcl812_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
@@ -533,26 +522,8 @@ static int pcl812_do_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
-}
-
-#ifdef PCL812_EXTDEBUG
-/*
-==============================================================================
-*/
-static void pcl812_cmdtest_out(int e, struct comedi_cmd *cmd)
-{
-	printk(KERN_INFO "pcl812 e=%d startsrc=%x scansrc=%x convsrc=%x\n", e,
-	       cmd->start_src, cmd->scan_begin_src, cmd->convert_src);
-	printk(KERN_INFO "pcl812 e=%d startarg=%d scanarg=%d convarg=%d\n", e,
-	       cmd->start_arg, cmd->scan_begin_arg, cmd->convert_arg);
-	printk(KERN_INFO "pcl812 e=%d stopsrc=%x scanend=%x\n", e,
-	       cmd->stop_src, cmd->scan_end_src);
-	printk(KERN_INFO "pcl812 e=%d stoparg=%d scanendarg=%d "
-	       "chanlistlen=%d\n", e, cmd->stop_arg, cmd->scan_end_arg,
-	       cmd->chanlist_len);
+	return insn->n;
 }
-#endif
 
 /*
 ==============================================================================
@@ -560,13 +531,10 @@ static void pcl812_cmdtest_out(int e, struct comedi_cmd *cmd)
 static int pcl812_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct pcl812_board *board = comedi_board(dev);
 	int err = 0;
 	int tmp, divisor1, divisor2;
 
-#ifdef PCL812_EXTDEBUG
-	printk("pcl812 EDBG: BGN: pcl812_ai_cmdtest(...)\n");
-	pcl812_cmdtest_out(-1, cmd);
-#endif
 	/* step 1: make sure trigger sources are trivially valid */
 
 	tmp = cmd->start_src;
@@ -598,60 +566,19 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	if (!cmd->stop_src || tmp != cmd->stop_src)
 		err++;
 
-	if (err) {
-#ifdef PCL812_EXTDEBUG
-		pcl812_cmdtest_out(1, cmd);
-		printk
-		    ("pcl812 EDBG: BGN: pcl812_ai_cmdtest(...) err=%d ret=1\n",
-		     err);
-#endif
+	if (err)
 		return 1;
-	}
 
 	/*
 	 * step 2: make sure trigger sources are
 	 * unique and mutually compatible
 	 */
 
-	if (cmd->start_src != TRIG_NOW) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
-
-	if (cmd->scan_begin_src != TRIG_FOLLOW) {
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		err++;
-	}
-
-	if (devpriv->use_ext_trg) {
-		if (cmd->convert_src != TRIG_EXT) {
-			cmd->convert_src = TRIG_EXT;
-			err++;
-		}
-	} else {
-		if (cmd->convert_src != TRIG_TIMER) {
-			cmd->convert_src = TRIG_TIMER;
-			err++;
-		}
-	}
-
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
-
 	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
 		err++;
 
-	if (err) {
-#ifdef PCL812_EXTDEBUG
-		pcl812_cmdtest_out(2, cmd);
-		printk
-		    ("pcl812 EDBG: BGN: pcl812_ai_cmdtest(...) err=%d ret=2\n",
-		     err);
-#endif
+	if (err)
 		return 2;
-	}
 
 	/* step 3: make sure arguments are trivially compatible */
 
@@ -666,8 +593,8 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < this_board->ai_ns_min) {
-			cmd->convert_arg = this_board->ai_ns_min;
+		if (cmd->convert_arg < board->ai_ns_min) {
+			cmd->convert_arg = board->ai_ns_min;
 			err++;
 		}
 	} else {		/* TRIG_EXT */
@@ -682,7 +609,7 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 		err++;
 	}
 	if (cmd->chanlist_len > MAX_CHANLIST_LEN) {
-		cmd->chanlist_len = this_board->n_aichan;
+		cmd->chanlist_len = board->n_aichan;
 		err++;
 	}
 	if (cmd->scan_end_arg != cmd->chanlist_len) {
@@ -701,37 +628,24 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 		}
 	}
 
-	if (err) {
-#ifdef PCL812_EXTDEBUG
-		pcl812_cmdtest_out(3, cmd);
-		printk
-		    ("pcl812 EDBG: BGN: pcl812_ai_cmdtest(...) err=%d ret=3\n",
-		     err);
-#endif
+	if (err)
 		return 3;
-	}
 
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		tmp = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(this_board->i8254_osc_base, &divisor1,
+		i8253_cascade_ns_to_timer(board->i8254_osc_base, &divisor1,
 					  &divisor2, &cmd->convert_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
+		if (cmd->convert_arg < board->ai_ns_min)
+			cmd->convert_arg = board->ai_ns_min;
 		if (tmp != cmd->convert_arg)
 			err++;
 	}
 
-	if (err) {
-#ifdef PCL812_EXTDEBUG
-		printk
-		    ("pcl812 EDBG: BGN: pcl812_ai_cmdtest(...) err=%d ret=4\n",
-		     err);
-#endif
+	if (err)
 		return 4;
-	}
 
 	return 0;
 }
@@ -741,13 +655,10 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 */
 static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct pcl812_board *board = comedi_board(dev);
 	unsigned int divisor1 = 0, divisor2 = 0, i, dma_flags, bytes;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: BGN: pcl812_ai_cmd(...)\n");
-#endif
-
 	if (cmd->start_src != TRIG_NOW)
 		return -EINVAL;
 	if (cmd->scan_begin_src != TRIG_FOLLOW)
@@ -767,9 +678,9 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		return -EINVAL;
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
-		i8253_cascade_ns_to_timer(this_board->i8254_osc_base,
+		if (cmd->convert_arg < board->ai_ns_min)
+			cmd->convert_arg = board->ai_ns_min;
+		i8253_cascade_ns_to_timer(board->i8254_osc_base,
 					  &divisor1, &divisor2,
 					  &cmd->convert_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
@@ -871,13 +782,6 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		set_dma_count(devpriv->dma, devpriv->dmabytestomove[0]);
 		release_dma_lock(dma_flags);
 		enable_dma(devpriv->dma);
-#ifdef PCL812_EXTDEBUG
-		printk
-		    ("pcl812 EDBG:   DMA %d PTR 0x%0x/0x%0x LEN %u/%u EOS %d\n",
-		     devpriv->dma, devpriv->hwdmaptr[0],
-		     devpriv->hwdmaptr[1], devpriv->dmabytestomove[0],
-		     devpriv->dmabytestomove[1], devpriv->ai_eos);
-#endif
 	}
 
 	switch (cmd->convert_src) {
@@ -891,10 +795,6 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	else							/*  let's go! */
 		outb(devpriv->mode_reg_int | 6, dev->iobase + PCL812_MODE);
 
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: END: pcl812_ai_cmd(...)\n");
-#endif
-
 	return 0;
 }
 
@@ -1014,9 +914,6 @@ static irqreturn_t interrupt_pcl812_ai_dma(int irq, void *d)
 	int len, bufptr;
 	short *ptr;
 
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: BGN: interrupt_pcl812_ai_dma(...)\n");
-#endif
 	ptr = (short *)devpriv->dmabuf[devpriv->next_dma_buf];
 	len = (devpriv->dmabytestomove[devpriv->next_dma_buf] >> 1) -
 	    devpriv->ai_poll_ptr;
@@ -1049,9 +946,6 @@ static irqreturn_t interrupt_pcl812_ai_dma(int irq, void *d)
 
 	transfer_from_dma_buf(dev, s, ptr, bufptr, len);
 
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: END: interrupt_pcl812_ai_dma(...)\n");
-#endif
 	return IRQ_HANDLED;
 }
 
@@ -1168,10 +1062,6 @@ static void setup_range_channel(struct comedi_device *dev,
 static void start_pacer(struct comedi_device *dev, int mode,
 			unsigned int divisor1, unsigned int divisor2)
 {
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: BGN: start_pacer(%d,%u,%u)\n", mode,
-	       divisor1, divisor2);
-#endif
 	outb(0xb4, dev->iobase + PCL812_CTRCTL);
 	outb(0x74, dev->iobase + PCL812_CTRCTL);
 	udelay(1);
@@ -1182,9 +1072,6 @@ static void start_pacer(struct comedi_device *dev, int mode,
 		outb(divisor1 & 0xff, dev->iobase + PCL812_CTR1);
 		outb((divisor1 >> 8) & 0xff, dev->iobase + PCL812_CTR1);
 	}
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: END: start_pacer(...)\n");
-#endif
 }
 
 /*
@@ -1192,6 +1079,7 @@ static void start_pacer(struct comedi_device *dev, int mode,
 */
 static void free_resources(struct comedi_device *dev)
 {
+	const struct pcl812_board *board = comedi_board(dev);
 
 	if (dev->private) {
 		if (devpriv->dmabuf[0])
@@ -1204,7 +1092,7 @@ static void free_resources(struct comedi_device *dev)
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
+		release_region(dev->iobase, board->io_range);
 }
 
 /*
@@ -1213,9 +1101,6 @@ static void free_resources(struct comedi_device *dev)
 static int pcl812_ai_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: BGN: pcl812_ai_cancel(...)\n");
-#endif
 	if (devpriv->ai_dma)
 		disable_dma(devpriv->dma);
 	outb(0, dev->iobase + PCL812_CLRINT);	/* clear INT request */
@@ -1223,9 +1108,6 @@ static int pcl812_ai_cancel(struct comedi_device *dev,
 	outb(devpriv->mode_reg_int | 0, dev->iobase + PCL812_MODE);
 	start_pacer(dev, -1, 0, 0);	/*  stop 8254 */
 	outb(0, dev->iobase + PCL812_CLRINT);	/* clear INT request */
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: END: pcl812_ai_cancel(...)\n");
-#endif
 	return 0;
 }
 
@@ -1234,15 +1116,14 @@ static int pcl812_ai_cancel(struct comedi_device *dev,
 */
 static void pcl812_reset(struct comedi_device *dev)
 {
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: BGN: pcl812_reset(...)\n");
-#endif
+	const struct pcl812_board *board = comedi_board(dev);
+
 	outb(0, dev->iobase + PCL812_MUX);
 	outb(0 + devpriv->range_correction, dev->iobase + PCL812_GAIN);
 	devpriv->old_chan_reg = -1;	/*  invalidate chain/gain memory */
 	devpriv->old_gain_reg = -1;
 
-	switch (this_board->board_type) {
+	switch (board->board_type) {
 	case boardPCL812PG:
 	case boardPCL812:
 	case boardACL8112:
@@ -1266,13 +1147,11 @@ static void pcl812_reset(struct comedi_device *dev)
 		break;
 	}
 	udelay(5);
-#ifdef PCL812_EXTDEBUG
-	printk(KERN_DEBUG "pcl812 EDBG: END: pcl812_reset(...)\n");
-#endif
 }
 
 static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl812_board *board = comedi_board(dev);
 	int ret, subdev;
 	unsigned long iobase;
 	unsigned int irq;
@@ -1283,9 +1162,9 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: pcl812:  board=%s, ioport=0x%03lx",
-	       dev->minor, this_board->name, iobase);
+	       dev->minor, board->name, iobase);
 
-	if (!request_region(iobase, this_board->io_range, "pcl812")) {
+	if (!request_region(iobase, board->io_range, "pcl812")) {
 		printk("I/O port conflict\n");
 		return -EIO;
 	}
@@ -1297,13 +1176,13 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;	/* Can't alloc mem */
 	}
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	irq = 0;
-	if (this_board->IRQbits != 0) {	/* board support IRQ */
+	if (board->IRQbits != 0) {	/* board support IRQ */
 		irq = it->options[1];
 		if (irq) {	/* we want to use IRQ */
-			if (((1 << irq) & this_board->IRQbits) == 0) {
+			if (((1 << irq) & board->IRQbits) == 0) {
 				printk
 				    (", IRQ %u is out of allowed range, "
 				     "DISABLING IT", irq);
@@ -1328,9 +1207,9 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	devpriv->dma = dma;
 	if (!dev->irq)
 		goto no_dma;	/* if we haven't IRQ, we can't use DMA */
-	if (this_board->DMAbits != 0) {	/* board support DMA */
+	if (board->DMAbits != 0) {	/* board support DMA */
 		dma = it->options[2];
-		if (((1 << dma) & this_board->DMAbits) == 0) {
+		if (((1 << dma) & board->DMAbits) == 0) {
 			printk(", DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
@@ -1369,17 +1248,17 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 no_dma:
 
 	n_subdevices = 0;
-	if (this_board->n_aichan > 0)
+	if (board->n_aichan > 0)
 		n_subdevices++;
-	if (this_board->n_aochan > 0)
+	if (board->n_aochan > 0)
 		n_subdevices++;
-	if (this_board->n_dichan > 0)
+	if (board->n_dichan > 0)
 		n_subdevices++;
-	if (this_board->n_dochan > 0)
+	if (board->n_dochan > 0)
 		n_subdevices++;
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0) {
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret) {
 		free_resources(dev);
 		return ret;
 	}
@@ -1387,46 +1266,46 @@ no_dma:
 	subdev = 0;
 
 	/* analog input */
-	if (this_board->n_aichan > 0) {
+	if (board->n_aichan > 0) {
 		s = dev->subdevices + subdev;
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags = SDF_READABLE;
-		switch (this_board->board_type) {
+		switch (board->board_type) {
 		case boardA821:
 			if (it->options[2] == 1) {
-				s->n_chan = this_board->n_aichan_diff;
+				s->n_chan = board->n_aichan_diff;
 				s->subdev_flags |= SDF_DIFF;
 				devpriv->use_diff = 1;
 			} else {
-				s->n_chan = this_board->n_aichan;
+				s->n_chan = board->n_aichan;
 				s->subdev_flags |= SDF_GROUND;
 			}
 			break;
 		case boardACL8112:
 		case boardACL8216:
 			if (it->options[4] == 1) {
-				s->n_chan = this_board->n_aichan_diff;
+				s->n_chan = board->n_aichan_diff;
 				s->subdev_flags |= SDF_DIFF;
 				devpriv->use_diff = 1;
 			} else {
-				s->n_chan = this_board->n_aichan;
+				s->n_chan = board->n_aichan;
 				s->subdev_flags |= SDF_GROUND;
 			}
 			break;
 		default:
-			s->n_chan = this_board->n_aichan;
+			s->n_chan = board->n_aichan;
 			s->subdev_flags |= SDF_GROUND;
 			break;
 		}
-		s->maxdata = this_board->ai_maxdata;
+		s->maxdata = board->ai_maxdata;
 		s->len_chanlist = MAX_CHANLIST_LEN;
-		s->range_table = this_board->rangelist_ai;
-		if (this_board->board_type == boardACL8216)
+		s->range_table = board->rangelist_ai;
+		if (board->board_type == boardACL8216)
 			s->insn_read = acl8216_ai_insn_read;
 		else
 			s->insn_read = pcl812_ai_insn_read;
 
-		devpriv->use_MPC = this_board->haveMPC508;
+		devpriv->use_MPC = board->haveMPC508;
 		s->cancel = pcl812_ai_cancel;
 		if (dev->irq) {
 			dev->read_subdev = s;
@@ -1435,7 +1314,7 @@ no_dma:
 			s->do_cmd = pcl812_ai_cmd;
 			s->poll = pcl812_ai_poll;
 		}
-		switch (this_board->board_type) {
+		switch (board->board_type) {
 		case boardPCL812PG:
 			if (it->options[4] == 1)
 				s->range_table = &range_pcl812pg2_ai;
@@ -1529,17 +1408,17 @@ no_dma:
 	}
 
 	/* analog output */
-	if (this_board->n_aochan > 0) {
+	if (board->n_aochan > 0) {
 		s = dev->subdevices + subdev;
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = this_board->n_aochan;
+		s->n_chan = board->n_aochan;
 		s->maxdata = 0xfff;
 		s->len_chanlist = 1;
-		s->range_table = this_board->rangelist_ao;
+		s->range_table = board->rangelist_ao;
 		s->insn_read = pcl812_ao_insn_read;
 		s->insn_write = pcl812_ao_insn_write;
-		switch (this_board->board_type) {
+		switch (board->board_type) {
 		case boardA821:
 			if (it->options[3] == 1)
 				s->range_table = &range_unipolar10;
@@ -1558,32 +1437,32 @@ no_dma:
 	}
 
 	/* digital input */
-	if (this_board->n_dichan > 0) {
+	if (board->n_dichan > 0) {
 		s = dev->subdevices + subdev;
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
-		s->n_chan = this_board->n_dichan;
+		s->n_chan = board->n_dichan;
 		s->maxdata = 1;
-		s->len_chanlist = this_board->n_dichan;
+		s->len_chanlist = board->n_dichan;
 		s->range_table = &range_digital;
 		s->insn_bits = pcl812_di_insn_bits;
 		subdev++;
 	}
 
 	/* digital output */
-	if (this_board->n_dochan > 0) {
+	if (board->n_dochan > 0) {
 		s = dev->subdevices + subdev;
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = this_board->n_dochan;
+		s->n_chan = board->n_dochan;
 		s->maxdata = 1;
-		s->len_chanlist = this_board->n_dochan;
+		s->len_chanlist = board->n_dochan;
 		s->range_table = &range_digital;
 		s->insn_bits = pcl812_do_insn_bits;
 		subdev++;
 	}
 
-	switch (this_board->board_type) {
+	switch (board->board_type) {
 	case boardACL8216:
 		devpriv->ai_is16b = 1;
 	case boardPCL812PG:
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index cc67b6d4605..ba6911f063c 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -126,7 +126,6 @@ struct pcl816_board {
 };
 
 #define devpriv ((struct pcl816_private *)dev->private)
-#define this_board ((const struct pcl816_board *)dev->board_ptr)
 
 #ifdef unused
 static int RTC_lock;	/* RTC lock */
@@ -415,8 +414,8 @@ static irqreturn_t interrupt_pcl816(int irq, void *d)
 	}
 
 	outb(0, dev->iobase + PCL816_CLRINT);	/* clear INT request */
-	if ((!dev->irq) | (!devpriv->irq_free) | (!devpriv->irq_blocked) |
-	    (!devpriv->int816_mode)) {
+	if (!dev->irq || !devpriv->irq_free || !devpriv->irq_blocked ||
+	    !devpriv->int816_mode) {
 		if (devpriv->irq_was_now_closed) {
 			devpriv->irq_was_now_closed = 0;
 			/*  comedi_error(dev,"last IRQ.."); */
@@ -451,6 +450,7 @@ static void pcl816_cmdtest_out(int e, struct comedi_cmd *cmd)
 static int pcl816_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
+	const struct pcl816_board *board = comedi_board(dev);
 	int err = 0;
 	int tmp, divisor1 = 0, divisor2 = 0;
 
@@ -493,26 +493,11 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 	 * are unique and mutually compatible
 	 */
 
-	if (cmd->start_src != TRIG_NOW) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
-
-	if (cmd->scan_begin_src != TRIG_FOLLOW) {
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		err++;
-	}
-
 	if (cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_TIMER) {
 		cmd->convert_src = TRIG_TIMER;
 		err++;
 	}
 
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
-
 	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
 		err++;
 
@@ -531,8 +516,8 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 		err++;
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < this_board->ai_ns_min) {
-			cmd->convert_arg = this_board->ai_ns_min;
+		if (cmd->convert_arg < board->ai_ns_min) {
+			cmd->convert_arg = board->ai_ns_min;
 			err++;
 		}
 	} else {		/* TRIG_EXT */
@@ -565,12 +550,12 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 	if (cmd->convert_src == TRIG_TIMER) {
 		tmp = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(this_board->i8254_osc_base,
+		i8253_cascade_ns_to_timer(board->i8254_osc_base,
 					  &divisor1, &divisor2,
 					  &cmd->convert_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
+		if (cmd->convert_arg < board->ai_ns_min)
+			cmd->convert_arg = board->ai_ns_min;
 		if (tmp != cmd->convert_arg)
 			err++;
 	}
@@ -592,6 +577,7 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 
 static int pcl816_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct pcl816_board *board = comedi_board(dev);
 	unsigned int divisor1 = 0, divisor2 = 0, dma_flags, bytes, dmairq;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int seglen;
@@ -609,10 +595,10 @@ static int pcl816_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		return -EBUSY;
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
+		if (cmd->convert_arg < board->ai_ns_min)
+			cmd->convert_arg = board->ai_ns_min;
 
-		i8253_cascade_ns_to_timer(this_board->i8254_osc_base, &divisor1,
+		i8253_cascade_ns_to_timer(board->i8254_osc_base, &divisor1,
 					  &divisor2, &cmd->convert_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
 
@@ -1028,6 +1014,7 @@ static int set_rtc_irq_bit(unsigned char bit)
 
 static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl816_board *board = comedi_board(dev);
 	int ret;
 	unsigned long iobase;
 	unsigned int irq, dma;
@@ -1038,9 +1025,9 @@ static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* claim our I/O space */
 	iobase = it->options[0];
 	printk("comedi%d: pcl816:  board=%s, ioport=0x%03lx", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 
-	if (!request_region(iobase, this_board->io_range, "pcl816")) {
+	if (!request_region(iobase, board->io_range, "pcl816")) {
 		printk("I/O port conflict\n");
 		return -EIO;
 	}
@@ -1056,15 +1043,14 @@ static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;	/* Can't alloc mem */
 
-	/* set up some name stuff */
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	/* grab our IRQ */
 	irq = 0;
-	if (this_board->IRQbits != 0) {	/* board support IRQ */
+	if (board->IRQbits != 0) {	/* board support IRQ */
 		irq = it->options[1];
 		if (irq) {	/* we want to use IRQ */
-			if (((1 << irq) & this_board->IRQbits) == 0) {
+			if (((1 << irq) & board->IRQbits) == 0) {
 				printk
 				    (", IRQ %u is out of allowed range, "
 				     "DISABLING IT", irq);
@@ -1134,12 +1120,12 @@ no_rtc:
 	if ((devpriv->irq_free == 0) && (devpriv->dma_rtc == 0))
 		goto no_dma;	/* if we haven't IRQ, we can't use DMA */
 
-	if (this_board->DMAbits != 0) {	/* board support DMA */
+	if (board->DMAbits != 0) {	/* board support DMA */
 		dma = it->options[2];
 		if (dma < 1)
 			goto no_dma;	/* DMA disabled */
 
-		if (((1 << dma) & this_board->DMAbits) == 0) {
+		if (((1 << dma) & board->DMAbits) == 0) {
 			printk(", DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
@@ -1185,30 +1171,30 @@ no_rtc:
 
 no_dma:
 
-/*  if (this_board->n_aochan > 0)
+/*  if (board->n_aochan > 0)
     subdevs[1] = COMEDI_SUBD_AO;
-  if (this_board->n_dichan > 0)
+  if (board->n_dichan > 0)
     subdevs[2] = COMEDI_SUBD_DI;
-  if (this_board->n_dochan > 0)
+  if (board->n_dochan > 0)
     subdevs[3] = COMEDI_SUBD_DO;
 */
 
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
-	if (this_board->n_aichan > 0) {
+	if (board->n_aichan > 0) {
 		s->type = COMEDI_SUBD_AI;
 		devpriv->sub_ai = s;
 		dev->read_subdev = s;
 		s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
-		s->n_chan = this_board->n_aichan;
+		s->n_chan = board->n_aichan;
 		s->subdev_flags |= SDF_DIFF;
 		/* printk (", %dchans DIFF DAC - %d", s->n_chan, i); */
-		s->maxdata = this_board->ai_maxdata;
-		s->len_chanlist = this_board->ai_chanlist;
-		s->range_table = this_board->ai_range_type;
+		s->maxdata = board->ai_maxdata;
+		s->len_chanlist = board->ai_chanlist;
+		s->range_table = board->ai_range_type;
 		s->cancel = pcl816_ai_cancel;
 		s->do_cmdtest = pcl816_ai_cmdtest;
 		s->do_cmd = pcl816_ai_cmd;
@@ -1221,25 +1207,25 @@ no_dma:
 #if 0
 case COMEDI_SUBD_AO:
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-	s->n_chan = this_board->n_aochan;
-	s->maxdata = this_board->ao_maxdata;
-	s->len_chanlist = this_board->ao_chanlist;
-	s->range_table = this_board->ao_range_type;
+	s->n_chan = board->n_aochan;
+	s->maxdata = board->ao_maxdata;
+	s->len_chanlist = board->ao_chanlist;
+	s->range_table = board->ao_range_type;
 	break;
 
 case COMEDI_SUBD_DI:
 	s->subdev_flags = SDF_READABLE;
-	s->n_chan = this_board->n_dichan;
+	s->n_chan = board->n_dichan;
 	s->maxdata = 1;
-	s->len_chanlist = this_board->n_dichan;
+	s->len_chanlist = board->n_dichan;
 	s->range_table = &range_digital;
 	break;
 
 case COMEDI_SUBD_DO:
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = this_board->n_dochan;
+	s->n_chan = board->n_dochan;
 	s->maxdata = 1;
-	s->len_chanlist = this_board->n_dochan;
+	s->len_chanlist = board->n_dochan;
 	s->range_table = &range_digital;
 	break;
 #endif
@@ -1253,6 +1239,8 @@ case COMEDI_SUBD_DO:
 
 static void pcl816_detach(struct comedi_device *dev)
 {
+	const struct pcl816_board *board = comedi_board(dev);
+
 	if (dev->private) {
 		pcl816_ai_cancel(dev, devpriv->sub_ai);
 		pcl816_reset(dev);
@@ -1275,7 +1263,7 @@ static void pcl816_detach(struct comedi_device *dev)
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
+		release_region(dev->iobase, board->io_range);
 #ifdef unused
 	if (devpriv->dma_rtc)
 		RTC_lock--;
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 1406c9720f5..34169c16fb9 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -326,7 +326,6 @@ static const unsigned int muxonechan[] = { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0
 };
 
 #define devpriv ((struct pcl818_private *)dev->private)
-#define this_board ((const struct pcl818_board *)dev->board_ptr)
 
 /*
 ==============================================================================
@@ -443,13 +442,10 @@ static int pcl818_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + PCL818_DI_LO) |
 	    (inb(dev->iobase + PCL818_DI_HI) << 8);
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -462,9 +458,6 @@ static int pcl818_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	s->state &= ~data[0];
 	s->state |= (data[0] & data[1]);
 
@@ -473,7 +466,7 @@ static int pcl818_do_insn_bits(struct comedi_device *dev,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -953,7 +946,7 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 	int divisor1 = 0, divisor2 = 0;
 	unsigned int seglen;
 
-	dev_dbg(dev->hw_dev, "pcl818_ai_cmd_mode()\n");
+	dev_dbg(dev->class_dev, "pcl818_ai_cmd_mode()\n");
 	if ((!dev->irq) && (!devpriv->dma_rtc)) {
 		comedi_error(dev, "IRQ not defined!");
 		return -EINVAL;
@@ -1056,7 +1049,7 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 		break;
 	}
 #endif
-	dev_dbg(dev->hw_dev, "pcl818_ai_cmd_mode() end\n");
+	dev_dbg(dev->class_dev, "pcl818_ai_cmd_mode() end\n");
 	return 0;
 }
 
@@ -1264,6 +1257,7 @@ static int check_single_ended(unsigned int port)
 static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
+	const struct pcl818_board *board = comedi_board(dev);
 	int err = 0;
 	int tmp, divisor1 = 0, divisor2 = 0;
 
@@ -1299,22 +1293,9 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* step 2: make sure trigger sources are unique and mutually compatible */
 
-	if (cmd->start_src != TRIG_NOW) {
-		cmd->start_src = TRIG_NOW;
-		err++;
-	}
-	if (cmd->scan_begin_src != TRIG_FOLLOW) {
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		err++;
-	}
 	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
 		err++;
 
-	if (cmd->scan_end_src != TRIG_COUNT) {
-		cmd->scan_end_src = TRIG_COUNT;
-		err++;
-	}
-
 	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
 		err++;
 
@@ -1334,8 +1315,8 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (cmd->convert_arg < this_board->ns_min) {
-			cmd->convert_arg = this_board->ns_min;
+		if (cmd->convert_arg < board->ns_min) {
+			cmd->convert_arg = board->ns_min;
 			err++;
 		}
 	} else {		/* TRIG_EXT */
@@ -1371,8 +1352,8 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		i8253_cascade_ns_to_timer(devpriv->i8253_osc_base, &divisor1,
 					  &divisor2, &cmd->convert_arg,
 					  cmd->flags & TRIG_ROUND_MASK);
-		if (cmd->convert_arg < this_board->ns_min)
-			cmd->convert_arg = this_board->ns_min;
+		if (cmd->convert_arg < board->ns_min)
+			cmd->convert_arg = board->ns_min;
 		if (tmp != cmd->convert_arg)
 			err++;
 	}
@@ -1399,7 +1380,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval;
 
-	dev_dbg(dev->hw_dev, "pcl818_ai_cmd()\n");
+	dev_dbg(dev->class_dev, "pcl818_ai_cmd()\n");
 	devpriv->ai_n_chan = cmd->chanlist_len;
 	devpriv->ai_chanlist = cmd->chanlist;
 	devpriv->ai_flags = cmd->flags;
@@ -1417,7 +1398,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		if (cmd->convert_src == TRIG_TIMER) {	/*  mode 1 */
 			devpriv->ai_timer1 = cmd->convert_arg;
 			retval = pcl818_ai_cmd_mode(1, dev, s);
-			dev_dbg(dev->hw_dev, "pcl818_ai_cmd() end\n");
+			dev_dbg(dev->class_dev, "pcl818_ai_cmd() end\n");
 			return retval;
 		}
 		if (cmd->convert_src == TRIG_EXT) {	/*  mode 3 */
@@ -1436,7 +1417,7 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
 	if (devpriv->irq_blocked > 0) {
-		dev_dbg(dev->hw_dev, "pcl818_ai_cancel()\n");
+		dev_dbg(dev->class_dev, "pcl818_ai_cancel()\n");
 		devpriv->irq_was_now_closed = 1;
 
 		switch (devpriv->ai_mode) {
@@ -1486,7 +1467,7 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 	}
 
 end:
-	dev_dbg(dev->hw_dev, "pcl818_ai_cancel() end\n");
+	dev_dbg(dev->class_dev, "pcl818_ai_cancel() end\n");
 	return 0;
 }
 
@@ -1519,6 +1500,8 @@ static int pcl818_check(unsigned long iobase)
 */
 static void pcl818_reset(struct comedi_device *dev)
 {
+	const struct pcl818_board *board = comedi_board(dev);
+
 	if (devpriv->usefifo) {	/*  FIFO shutdown */
 		outb(0, dev->iobase + PCL818_FI_INTCLR);
 		outb(0, dev->iobase + PCL818_FI_FLUSH);
@@ -1537,7 +1520,7 @@ static void pcl818_reset(struct comedi_device *dev)
 	outb(0xb0, dev->iobase + PCL818_CTRCTL);	/* Stop pacer */
 	outb(0x70, dev->iobase + PCL818_CTRCTL);
 	outb(0x30, dev->iobase + PCL818_CTRCTL);
-	if (this_board->is_818) {
+	if (board->is_818) {
 		outb(0, dev->iobase + PCL818_RANGE);
 	} else {
 		outb(0, dev->iobase + PCL718_DA2_LO);
@@ -1636,6 +1619,7 @@ static int rtc_setfreq_irq(int freq)
 
 static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcl818_board *board = comedi_board(dev);
 	int ret;
 	unsigned long iobase;
 	unsigned int irq;
@@ -1651,9 +1635,10 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	iobase = it->options[0];
 	printk
 	    ("comedi%d: pcl818:  board=%s, ioport=0x%03lx",
-	     dev->minor, this_board->name, iobase);
-	devpriv->io_range = this_board->io_range;
-	if ((this_board->fifo) && (it->options[2] == -1)) {	/*  we've board with FIFO and we want to use FIFO */
+	     dev->minor, board->name, iobase);
+	devpriv->io_range = board->io_range;
+	if ((board->fifo) && (it->options[2] == -1)) {
+		/*  we've board with FIFO and we want to use FIFO */
 		devpriv->io_range = PCLx1xFIFO_RANGE;
 		devpriv->usefifo = 1;
 	}
@@ -1669,14 +1654,14 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -EIO;
 	}
 
-	/* set up some name stuff */
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
+
 	/* grab our IRQ */
 	irq = 0;
-	if (this_board->IRQbits != 0) {	/* board support IRQ */
+	if (board->IRQbits != 0) {	/* board support IRQ */
 		irq = it->options[1];
 		if (irq) {	/* we want to use IRQ */
-			if (((1 << irq) & this_board->IRQbits) == 0) {
+			if (((1 << irq) & board->IRQbits) == 0) {
 				printk
 				    (", IRQ %u is out of allowed range, DISABLING IT",
 				     irq);
@@ -1740,11 +1725,11 @@ no_rtc:
 	devpriv->dma = dma;
 	if ((devpriv->irq_free == 0) && (devpriv->dma_rtc == 0))
 		goto no_dma;	/* if we haven't IRQ, we can't use DMA */
-	if (this_board->DMAbits != 0) {	/* board support DMA */
+	if (board->DMAbits != 0) {	/* board support DMA */
 		dma = it->options[2];
 		if (dma < 1)
 			goto no_dma;	/* DMA disabled */
-		if (((1 << dma) & this_board->DMAbits) == 0) {
+		if (((1 << dma) & board->DMAbits) == 0) {
 			printk(KERN_ERR "DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
@@ -1774,29 +1759,29 @@ no_rtc:
 
 no_dma:
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	s = dev->subdevices + 0;
-	if (!this_board->n_aichan_se) {
+	if (!board->n_aichan_se) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
 		s->type = COMEDI_SUBD_AI;
 		devpriv->sub_ai = s;
 		s->subdev_flags = SDF_READABLE;
 		if (check_single_ended(dev->iobase)) {
-			s->n_chan = this_board->n_aichan_se;
+			s->n_chan = board->n_aichan_se;
 			s->subdev_flags |= SDF_COMMON | SDF_GROUND;
 			printk(", %dchans S.E. DAC", s->n_chan);
 		} else {
-			s->n_chan = this_board->n_aichan_diff;
+			s->n_chan = board->n_aichan_diff;
 			s->subdev_flags |= SDF_DIFF;
 			printk(", %dchans DIFF DAC", s->n_chan);
 		}
-		s->maxdata = this_board->ai_maxdata;
+		s->maxdata = board->ai_maxdata;
 		s->len_chanlist = s->n_chan;
-		s->range_table = this_board->ai_range_type;
+		s->range_table = board->ai_range_type;
 		s->cancel = pcl818_ai_cancel;
 		s->insn_read = pcl818_ai_insn_read;
 		if ((irq) || (devpriv->dma_rtc)) {
@@ -1805,7 +1790,7 @@ no_dma:
 			s->do_cmdtest = ai_cmdtest;
 			s->do_cmd = ai_cmd;
 		}
-		if (this_board->is_818) {
+		if (board->is_818) {
 			if ((it->options[4] == 1) || (it->options[4] == 10))
 				s->range_table = &range_pcl818l_h_ai;	/*  secondary range list jumper selectable */
 		} else {
@@ -1845,15 +1830,15 @@ no_dma:
 	}
 
 	s = dev->subdevices + 1;
-	if (!this_board->n_aochan) {
+	if (!board->n_aochan) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = this_board->n_aochan;
-		s->maxdata = this_board->ao_maxdata;
-		s->len_chanlist = this_board->n_aochan;
-		s->range_table = this_board->ao_range_type;
+		s->n_chan = board->n_aochan;
+		s->maxdata = board->ao_maxdata;
+		s->len_chanlist = board->n_aochan;
+		s->range_table = board->ao_range_type;
 		s->insn_read = pcl818_ao_insn_read;
 		s->insn_write = pcl818_ao_insn_write;
 #ifdef unused
@@ -1864,7 +1849,7 @@ no_dma:
 		}
 #endif
 #endif
-		if (this_board->is_818) {
+		if (board->is_818) {
 			if ((it->options[4] == 1) || (it->options[4] == 10))
 				s->range_table = &range_unipolar10;
 			if (it->options[4] == 2)
@@ -1878,27 +1863,27 @@ no_dma:
 	}
 
 	s = dev->subdevices + 2;
-	if (!this_board->n_dichan) {
+	if (!board->n_dichan) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
-		s->n_chan = this_board->n_dichan;
+		s->n_chan = board->n_dichan;
 		s->maxdata = 1;
-		s->len_chanlist = this_board->n_dichan;
+		s->len_chanlist = board->n_dichan;
 		s->range_table = &range_digital;
 		s->insn_bits = pcl818_di_insn_bits;
 	}
 
 	s = dev->subdevices + 3;
-	if (!this_board->n_dochan) {
+	if (!board->n_dochan) {
 		s->type = COMEDI_SUBD_UNUSED;
 	} else {
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = this_board->n_dochan;
+		s->n_chan = board->n_dochan;
 		s->maxdata = 1;
-		s->len_chanlist = this_board->n_dochan;
+		s->len_chanlist = board->n_dochan;
 		s->range_table = &range_digital;
 		s->insn_bits = pcl818_do_insn_bits;
 	}
@@ -1910,9 +1895,9 @@ no_dma:
 		devpriv->i8253_osc_base = 1000;
 
 	/* max sampling speed */
-	devpriv->ns_min = this_board->ns_min;
+	devpriv->ns_min = board->ns_min;
 
-	if (!this_board->is_818) {
+	if (!board->is_818) {
 		if ((it->options[6] == 1) || (it->options[6] == 100))
 			devpriv->ns_min = 10000;	/* extended PCL718 to 100kHz DAC */
 	}
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index 7492b8f1d49..62c22ccfb78 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -76,10 +76,6 @@ struct priv_pcm3724 {
 	int dio_2;
 };
 
-#define this_board ((const struct pcm3724_board *)dev->board_ptr)
-
-/* (setq c-basic-offset 8) */
-
 static int subdev_8255_cb(int dir, int port, int data, unsigned long arg)
 {
 	unsigned long iobase = arg;
@@ -234,12 +230,13 @@ static int subdev_3724_insn_config(struct comedi_device *dev,
 static int pcm3724_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	const struct pcm3724_board *board = comedi_board(dev);
 	unsigned long iobase;
 	unsigned int iorange;
 	int ret, i, n_subdevices;
 
 	iobase = it->options[0];
-	iorange = this_board->io_range;
+	iorange = board->io_range;
 
 	ret = alloc_private(dev, sizeof(struct priv_pcm3724));
 	if (ret < 0)
@@ -249,20 +246,20 @@ static int pcm3724_attach(struct comedi_device *dev,
 	((struct priv_pcm3724 *)(dev->private))->dio_2 = 0;
 
 	printk(KERN_INFO "comedi%d: pcm3724: board=%s, 0x%03lx ", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 	if (!iobase || !request_region(iobase, iorange, "pcm3724")) {
 		printk("I/O port conflict\n");
 		return -EIO;
 	}
 
 	dev->iobase = iobase;
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 	printk(KERN_INFO "\n");
 
-	n_subdevices = this_board->numofports;
+	n_subdevices = board->numofports;
 
-	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, n_subdevices);
+	if (ret)
 		return ret;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
@@ -275,6 +272,7 @@ static int pcm3724_attach(struct comedi_device *dev,
 
 static void pcm3724_detach(struct comedi_device *dev)
 {
+	const struct pcm3724_board *board = comedi_board(dev);
 	int i;
 
 	if (dev->subdevices) {
@@ -282,7 +280,7 @@ static void pcm3724_detach(struct comedi_device *dev)
 			subdev_8255_cleanup(dev, dev->subdevices + i);
 	}
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->io_range);
+		release_region(dev->iobase, board->io_range);
 }
 
 static const struct pcm3724_board boardtypes[] = {
diff --git a/drivers/staging/comedi/drivers/pcm3730.c b/drivers/staging/comedi/drivers/pcm3730.c
index f8d1c644daf..d65e0bda2c4 100644
--- a/drivers/staging/comedi/drivers/pcm3730.c
+++ b/drivers/staging/comedi/drivers/pcm3730.c
@@ -32,8 +32,6 @@ static int pcm3730_do_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -41,17 +39,15 @@ static int pcm3730_do_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int pcm3730_di_insn_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
 	data[1] = inb(dev->iobase + (unsigned long)(s->private));
-	return 2;
+	return insn->n;
 }
 
 static int pcm3730_attach(struct comedi_device *dev,
@@ -59,6 +55,7 @@ static int pcm3730_attach(struct comedi_device *dev,
 {
 	struct comedi_subdevice *s;
 	unsigned long iobase;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: pcm3730: 0x%04lx ", dev->minor, iobase);
@@ -71,8 +68,9 @@ static int pcm3730_attach(struct comedi_device *dev,
 	dev->iobase = dev->iobase;
 	dev->irq = 0;
 
-	if (alloc_subdevices(dev, 6) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 6);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	s->type = COMEDI_SUBD_DO;
diff --git a/drivers/staging/comedi/drivers/pcmad.c b/drivers/staging/comedi/drivers/pcmad.c
index 1ec7d5cb346..54d19c94396 100644
--- a/drivers/staging/comedi/drivers/pcmad.c
+++ b/drivers/staging/comedi/drivers/pcmad.c
@@ -58,8 +58,6 @@ struct pcmad_board_struct {
 	int n_ai_bits;
 };
 
-#define this_board ((const struct pcmad_board_struct *)(dev->board_ptr))
-
 struct pcmad_priv_struct {
 	int differential;
 	int twos_comp;
@@ -72,6 +70,7 @@ static int pcmad_ai_insn_read(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
+	const struct pcmad_board_struct *board = comedi_board(dev);
 	int i;
 	int chan;
 	int n;
@@ -89,7 +88,7 @@ static int pcmad_ai_insn_read(struct comedi_device *dev,
 		data[n] |= (inb(dev->iobase + PCMAD_MSB) << 8);
 
 		if (devpriv->twos_comp)
-			data[n] ^= (1 << (this_board->n_ai_bits - 1));
+			data[n] ^= (1 << (board->n_ai_bits - 1));
 	}
 
 	return n;
@@ -104,6 +103,7 @@ static int pcmad_ai_insn_read(struct comedi_device *dev,
  */
 static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcmad_board_struct *board = comedi_board(dev);
 	int ret;
 	struct comedi_subdevice *s;
 	unsigned long iobase;
@@ -117,15 +117,15 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	printk(KERN_CONT "\n");
 	dev->iobase = iobase;
 
-	ret = alloc_subdevices(dev, 1);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct pcmad_priv_struct));
 	if (ret < 0)
 		return ret;
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	s = dev->subdevices + 0;
 	s->type = COMEDI_SUBD_AI;
@@ -133,7 +133,7 @@ static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->n_chan = 16;		/* XXX */
 	s->len_chanlist = 1;
 	s->insn_read = pcmad_ai_insn_read;
-	s->maxdata = (1 << this_board->n_ai_bits) - 1;
+	s->maxdata = (1 << board->n_ai_bits) - 1;
 	s->range_table = &range_unknown;
 
 	return 0;
diff --git a/drivers/staging/comedi/drivers/pcmda12.c b/drivers/staging/comedi/drivers/pcmda12.c
index 4786148b4fd..291ce7c1bdb 100644
--- a/drivers/staging/comedi/drivers/pcmda12.c
+++ b/drivers/staging/comedi/drivers/pcmda12.c
@@ -80,11 +80,6 @@ static const struct comedi_lrange pcmda12_ranges = {
 	 }
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct pcmda12_board *)dev->board_ptr)
-
 struct pcmda12_private {
 
 	unsigned int ao_readback[CHANS];
@@ -167,8 +162,10 @@ static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 static int pcmda12_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
+	const struct pcmda12_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	unsigned long iobase;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO
@@ -181,11 +178,7 @@ static int pcmda12_attach(struct comedi_device *dev,
 	}
 	dev->iobase = iobase;
 
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 /*
  * Allocate the private structure area.  alloc_private() is a
@@ -198,17 +191,9 @@ static int pcmda12_attach(struct comedi_device *dev,
 
 	devpriv->simultaneous_xfer_mode = it->options[1];
 
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 *
-	 * Allocate 2 subdevs (32 + 16 DIO lines) or 3 32 DIO subdevs for the
-	 * 96-channel version of the board.
-	 */
-	if (alloc_subdevices(dev, 1) < 0) {
-		printk(KERN_ERR "cannot allocate subdevice data structures\n");
-		return -ENOMEM;
-	}
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices;
 	s->private = NULL;
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index efed168d2ba..3d2e6f01c4b 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -183,11 +183,6 @@ static const struct comedi_lrange ranges_ao = {
 	  RANGE(-2.5, 2.5), RANGE(-2.5, 7.5)}
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct pcmmio_board *)dev->board_ptr)
-
 /* this structure is for data unique to this subdevice.  */
 struct pcmmio_subdev_private {
 
@@ -280,8 +275,6 @@ static int pcmmio_dio_insn_bits(struct comedi_device *dev,
 				struct comedi_insn *insn, unsigned int *data)
 {
 	int byte_no;
-	if (insn->n != 2)
-		return -EINVAL;
 
 	/* NOTE:
 	   reading a 0 means this channel was high
@@ -351,7 +344,7 @@ static int pcmmio_dio_insn_bits(struct comedi_device *dev,
 	printk(KERN_DEBUG "s->state %08x data_out %08x\n", s->state, data[1]);
 #endif
 
-	return 2;
+	return insn->n;
 }
 
 /* The input or output configuration of each digital line is
@@ -423,7 +416,9 @@ static int pcmmio_dio_insn_config(struct comedi_device *dev,
 
 static void switch_page(struct comedi_device *dev, int asic, int page)
 {
-	if (asic < 0 || asic >= thisboard->dio_num_asics)
+	const struct pcmmio_board *board = comedi_board(dev);
+
+	if (asic < 0 || asic >= board->dio_num_asics)
 		return;		/* paranoia */
 	if (page < 0 || page >= NUM_PAGES)
 		return;		/* more paranoia */
@@ -439,9 +434,10 @@ static void switch_page(struct comedi_device *dev, int asic, int page)
 static void init_asics(struct comedi_device *dev)
 {				/* sets up an
 				   ASIC chip to defaults */
+	const struct pcmmio_board *board = comedi_board(dev);
 	int asic;
 
-	for (asic = 0; asic < thisboard->dio_num_asics; ++asic) {
+	for (asic = 0; asic < board->dio_num_asics; ++asic) {
 		int port, page;
 		unsigned long baseaddr = devpriv->asics[asic].iobase;
 
@@ -476,7 +472,9 @@ static void init_asics(struct comedi_device *dev)
 #ifdef notused
 static void lock_port(struct comedi_device *dev, int asic, int port)
 {
-	if (asic < 0 || asic >= thisboard->dio_num_asics)
+	const struct pcmmio_board *board = comedi_board(dev);
+
+	if (asic < 0 || asic >= board->dio_num_asics)
 		return;		/* paranoia */
 	if (port < 0 || port >= PORTS_PER_ASIC)
 		return;		/* more paranoia */
@@ -490,7 +488,9 @@ static void lock_port(struct comedi_device *dev, int asic, int port)
 
 static void unlock_port(struct comedi_device *dev, int asic, int port)
 {
-	if (asic < 0 || asic >= thisboard->dio_num_asics)
+	const struct pcmmio_board *board = comedi_board(dev);
+
+	if (asic < 0 || asic >= board->dio_num_asics)
 		return;		/* paranoia */
 	if (port < 0 || port >= PORTS_PER_ASIC)
 		return;		/* more paranoia */
@@ -512,7 +512,7 @@ static void pcmmio_stop_intr(struct comedi_device *dev,
 
 	subpriv->dio.intr.enabled_mask = 0;
 	subpriv->dio.intr.active = 0;
-	s->async->inttrig = 0;
+	s->async->inttrig = NULL;
 	nports = subpriv->dio.intr.num_asic_chans / CHANS_PER_PORT;
 	firstport = subpriv->dio.intr.asic_chan / CHANS_PER_PORT;
 	switch_page(dev, asic, PAGE_ENAB);
@@ -778,7 +778,7 @@ pcmmio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
 		return -EINVAL;
 
 	spin_lock_irqsave(&subpriv->dio.intr.spinlock, flags);
-	s->async->inttrig = 0;
+	s->async->inttrig = NULL;
 	if (subpriv->dio.intr.active)
 		event = pcmmio_start_intr(dev, s);
 	spin_unlock_irqrestore(&subpriv->dio.intr.spinlock, flags);
@@ -1012,11 +1012,13 @@ static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcmmio_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int sdev_no, chans_left, n_dio_subdevs, n_subdevs, port, asic,
 	    thisasic_chanct = 0;
 	unsigned long iobase;
 	unsigned int irq[MAX_ASICS];
+	int ret;
 
 	iobase = it->options[0];
 	irq[0] = it->options[1];
@@ -1027,17 +1029,13 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->iobase = iobase;
 
 	if (!iobase || !request_region(iobase,
-				       thisboard->total_iosize,
+				       board->total_iosize,
 				       dev->driver->driver_name)) {
 		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
 		return -EIO;
 	}
 
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 /*
  * Allocate the private structure area.  alloc_private() is a
@@ -1061,7 +1059,7 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		spin_lock_init(&devpriv->asics[asic].spinlock);
 	}
 
-	chans_left = CHANS_PER_ASIC * thisboard->dio_num_asics;
+	chans_left = CHANS_PER_ASIC * board->dio_num_asics;
 	n_dio_subdevs = CALC_N_DIO_SUBDEVS(chans_left);
 	n_subdevs = n_dio_subdevs + 2;
 	devpriv->sprivs =
@@ -1072,29 +1070,22 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				dev->minor);
 		return -ENOMEM;
 	}
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 *
-	 * Allocate 1 AI + 1 AO + 2 DIO subdevs (24 lines per DIO)
-	 */
-	if (alloc_subdevices(dev, n_subdevs) < 0) {
-		printk(KERN_ERR "comedi%d: cannot allocate subdevice data structures\n",
-				dev->minor);
-		return -ENOMEM;
-	}
+
+	ret = comedi_alloc_subdevices(dev, n_subdevs);
+	if (ret)
+		return ret;
 
 	/* First, AI */
 	sdev_no = 0;
 	s = dev->subdevices + sdev_no;
 	s->private = devpriv->sprivs + sdev_no;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = thisboard->ai_range_table;
+	s->maxdata = (1 << board->ai_bits) - 1;
+	s->range_table = board->ai_range_table;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
 	s->type = COMEDI_SUBD_AI;
-	s->n_chan = thisboard->n_ai_chans;
+	s->n_chan = board->n_ai_chans;
 	s->len_chanlist = s->n_chan;
-	s->insn_read = thisboard->ai_rinsn;
+	s->insn_read = board->ai_rinsn;
 	subpriv->iobase = dev->iobase + 0;
 	/* initialize the resource enable register by clearing it */
 	outb(0, subpriv->iobase + 3);
@@ -1104,14 +1095,14 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	++sdev_no;
 	s = dev->subdevices + sdev_no;
 	s->private = devpriv->sprivs + sdev_no;
-	s->maxdata = (1 << thisboard->ao_bits) - 1;
-	s->range_table = thisboard->ao_range_table;
+	s->maxdata = (1 << board->ao_bits) - 1;
+	s->range_table = board->ao_range_table;
 	s->subdev_flags = SDF_READABLE;
 	s->type = COMEDI_SUBD_AO;
-	s->n_chan = thisboard->n_ao_chans;
+	s->n_chan = board->n_ao_chans;
 	s->len_chanlist = s->n_chan;
-	s->insn_read = thisboard->ao_rinsn;
-	s->insn_write = thisboard->ao_winsn;
+	s->insn_read = board->ao_rinsn;
+	s->insn_write = board->ao_winsn;
 	subpriv->iobase = dev->iobase + 8;
 	/* initialize the resource enable register by clearing it */
 	outb(0, subpriv->iobase + 3);
@@ -1192,7 +1183,7 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	for (asic = 0; irq[0] && asic < MAX_ASICS; ++asic) {
 		if (irq[asic]
 		    && request_irq(irq[asic], interrupt_pcmmio,
-				   IRQF_SHARED, thisboard->name, dev)) {
+				   IRQF_SHARED, board->name, dev)) {
 			int i;
 			/* unroll the allocated irqs.. */
 			for (i = asic - 1; i >= 0; --i) {
@@ -1211,7 +1202,7 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	if (irq[0]) {
 		printk(KERN_DEBUG "comedi%d: irq: %u\n", dev->minor, irq[0]);
-		if (thisboard->dio_num_asics == 2 && irq[1])
+		if (board->dio_num_asics == 2 && irq[1])
 			printk(KERN_DEBUG "comedi%d: second ASIC irq: %u\n",
 					dev->minor, irq[1]);
 	} else {
@@ -1225,10 +1216,11 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcmmio_detach(struct comedi_device *dev)
 {
+	const struct pcmmio_board *board = comedi_board(dev);
 	int i;
 
 	if (dev->iobase)
-		release_region(dev->iobase, thisboard->total_iosize);
+		release_region(dev->iobase, board->total_iosize);
 	for (i = 0; i < MAX_ASICS; ++i) {
 		if (devpriv && devpriv->asics[i].irq)
 			free_irq(devpriv->asics[i].irq, dev);
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index 623381d50da..feef3d02f35 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -155,11 +155,6 @@ struct pcmuio_board {
 	const int num_ports;
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct pcmuio_board *)dev->board_ptr)
-
 /* this structure is for data unique to this subdevice.  */
 struct pcmuio_subdev_private {
 	/* mapping of halfwords (bytes) in port/chanarray to iobase */
@@ -216,8 +211,6 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 				struct comedi_insn *insn, unsigned int *data)
 {
 	int byte_no;
-	if (insn->n != 2)
-		return -EINVAL;
 
 	/* NOTE:
 	   reading a 0 means this channel was high
@@ -232,7 +225,7 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 
 #ifdef DAMMIT_ITS_BROKEN
 	/* DEBUG */
-	dev_dbg(dev->hw_dev, "write mask: %08x  data: %08x\n", data[0],
+	dev_dbg(dev->class_dev, "write mask: %08x  data: %08x\n", data[0],
 		data[1]);
 #endif
 
@@ -269,7 +262,7 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 		}
 #ifdef DAMMIT_ITS_BROKEN
 		/* DEBUG */
-		dev_dbg(dev->hw_dev, "data_out_byte %02x\n", (unsigned)byte);
+		dev_dbg(dev->class_dev, "data_out_byte %02x\n", (unsigned)byte);
 #endif
 		/* save the digital input lines for this byte.. */
 		s->state |= ((unsigned int)byte) << offset;
@@ -280,11 +273,11 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 
 #ifdef DAMMIT_ITS_BROKEN
 	/* DEBUG */
-	dev_dbg(dev->hw_dev, "s->state %08x data_out %08x\n", s->state,
+	dev_dbg(dev->class_dev, "s->state %08x data_out %08x\n", s->state,
 		data[1]);
 #endif
 
-	return 2;
+	return insn->n;
 }
 
 /* The input or output configuration of each digital line is
@@ -354,7 +347,9 @@ static int pcmuio_dio_insn_config(struct comedi_device *dev,
 
 static void switch_page(struct comedi_device *dev, int asic, int page)
 {
-	if (asic < 0 || asic >= thisboard->num_asics)
+	const struct pcmuio_board *board = comedi_board(dev);
+
+	if (asic < 0 || asic >= board->num_asics)
 		return;		/* paranoia */
 	if (page < 0 || page >= NUM_PAGES)
 		return;		/* more paranoia */
@@ -370,9 +365,10 @@ static void switch_page(struct comedi_device *dev, int asic, int page)
 static void init_asics(struct comedi_device *dev)
 {				/* sets up an
 				   ASIC chip to defaults */
+	const struct pcmuio_board *board = comedi_board(dev);
 	int asic;
 
-	for (asic = 0; asic < thisboard->num_asics; ++asic) {
+	for (asic = 0; asic < board->num_asics; ++asic) {
 		int port, page;
 		unsigned long baseaddr = dev->iobase + asic * ASIC_IOSIZE;
 
@@ -407,7 +403,9 @@ static void init_asics(struct comedi_device *dev)
 #ifdef notused
 static void lock_port(struct comedi_device *dev, int asic, int port)
 {
-	if (asic < 0 || asic >= thisboard->num_asics)
+	const struct pcmuio_board *board = comedi_board(dev);
+
+	if (asic < 0 || asic >= board->num_asics)
 		return;		/* paranoia */
 	if (port < 0 || port >= PORTS_PER_ASIC)
 		return;		/* more paranoia */
@@ -420,7 +418,9 @@ static void lock_port(struct comedi_device *dev, int asic, int port)
 
 static void unlock_port(struct comedi_device *dev, int asic, int port)
 {
-	if (asic < 0 || asic >= thisboard->num_asics)
+	const struct pcmuio_board *board = comedi_board(dev);
+
+	if (asic < 0 || asic >= board->num_asics)
 		return;		/* paranoia */
 	if (port < 0 || port >= PORTS_PER_ASIC)
 		return;		/* more paranoia */
@@ -747,39 +747,38 @@ pcmuio_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct pcmuio_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int sdev_no, chans_left, n_subdevs, port, asic, thisasic_chanct = 0;
 	unsigned long iobase;
 	unsigned int irq[MAX_ASICS];
+	int ret;
 
 	iobase = it->options[0];
 	irq[0] = it->options[1];
 	irq[1] = it->options[2];
 
-	dev_dbg(dev->hw_dev, "comedi%d: %s: io: %lx attached\n", dev->minor,
+	dev_dbg(dev->class_dev, "%s: io: %lx attach\n",
 		dev->driver->driver_name, iobase);
 
 	dev->iobase = iobase;
 
 	if (!iobase || !request_region(iobase,
-				       thisboard->num_asics * ASIC_IOSIZE,
+				       board->num_asics * ASIC_IOSIZE,
 				       dev->driver->driver_name)) {
-		dev_err(dev->hw_dev, "I/O port conflict\n");
+		dev_err(dev->class_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 /*
  * Allocate the private structure area.  alloc_private() is a
  * convenient macro defined in comedidev.h.
  */
 	if (alloc_private(dev, sizeof(struct pcmuio_private)) < 0) {
-		dev_warn(dev->hw_dev, "cannot allocate private data structure\n");
+		dev_warn(dev->class_dev,
+			 "cannot allocate private data structure\n");
 		return -ENOMEM;
 	}
 
@@ -792,27 +791,21 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		spin_lock_init(&devpriv->asics[asic].spinlock);
 	}
 
-	chans_left = CHANS_PER_ASIC * thisboard->num_asics;
+	chans_left = CHANS_PER_ASIC * board->num_asics;
 	n_subdevs = CALC_N_SUBDEVS(chans_left);
 	devpriv->sprivs =
 	    kcalloc(n_subdevs, sizeof(struct pcmuio_subdev_private),
 		    GFP_KERNEL);
 	if (!devpriv->sprivs) {
-		dev_warn(dev->hw_dev, "cannot allocate subdevice private data structures\n");
-		return -ENOMEM;
-	}
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 *
-	 * Allocate 2 subdevs (32 + 16 DIO lines) or 3 32 DIO subdevs for the
-	 * 96-channel version of the board.
-	 */
-	if (alloc_subdevices(dev, n_subdevs) < 0) {
-		dev_dbg(dev->hw_dev, "cannot allocate subdevice data structures\n");
+		dev_warn(dev->class_dev,
+			 "cannot allocate subdevice private data structures\n");
 		return -ENOMEM;
 	}
 
+	ret = comedi_alloc_subdevices(dev, n_subdevs);
+	if (ret)
+		return ret;
+
 	port = 0;
 	asic = 0;
 	for (sdev_no = 0; sdev_no < (int)dev->n_subdevices; ++sdev_no) {
@@ -881,7 +874,7 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	for (asic = 0; irq[0] && asic < MAX_ASICS; ++asic) {
 		if (irq[asic]
 		    && request_irq(irq[asic], interrupt_pcmuio,
-				   IRQF_SHARED, thisboard->name, dev)) {
+				   IRQF_SHARED, board->name, dev)) {
 			int i;
 			/* unroll the allocated irqs.. */
 			for (i = asic - 1; i >= 0; --i) {
@@ -897,11 +890,12 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				   irqs.. */
 
 	if (irq[0]) {
-		dev_dbg(dev->hw_dev, "irq: %u\n", irq[0]);
-		if (irq[1] && thisboard->num_asics == 2)
-			dev_dbg(dev->hw_dev, "second ASIC irq: %u\n", irq[1]);
+		dev_dbg(dev->class_dev, "irq: %u\n", irq[0]);
+		if (irq[1] && board->num_asics == 2)
+			dev_dbg(dev->class_dev, "second ASIC irq: %u\n",
+				irq[1]);
 	} else {
-		dev_dbg(dev->hw_dev, "(IRQ mode disabled)\n");
+		dev_dbg(dev->class_dev, "(IRQ mode disabled)\n");
 	}
 
 
@@ -910,10 +904,11 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void pcmuio_detach(struct comedi_device *dev)
 {
+	const struct pcmuio_board *board = comedi_board(dev);
 	int i;
 
 	if (dev->iobase)
-		release_region(dev->iobase, ASIC_IOSIZE * thisboard->num_asics);
+		release_region(dev->iobase, ASIC_IOSIZE * board->num_asics);
 	for (i = 0; i < MAX_ASICS; ++i) {
 		if (devpriv->asics[i].irq)
 			free_irq(devpriv->asics[i].irq, dev);
diff --git a/drivers/staging/comedi/drivers/poc.c b/drivers/staging/comedi/drivers/poc.c
index e7120480687..c253bb9ef33 100644
--- a/drivers/staging/comedi/drivers/poc.c
+++ b/drivers/staging/comedi/drivers/poc.c
@@ -57,8 +57,6 @@ struct boarddef_struct {
 	const struct comedi_lrange *range;
 };
 
-#define this_board ((const struct boarddef_struct *)dev->board_ptr)
-
 static int readback_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_insn *insn, unsigned int *data)
 {
@@ -101,23 +99,18 @@ static int pcl733_insn_bits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	data[1] = inb(dev->iobase + 0);
 	data[1] |= (inb(dev->iobase + 1) << 8);
 	data[1] |= (inb(dev->iobase + 2) << 16);
 	data[1] |= (inb(dev->iobase + 3) << 24);
 
-	return 2;
+	return insn->n;
 }
 
 static int pcl734_insn_bits(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= (data[0] & data[1]);
@@ -132,27 +125,29 @@ static int pcl734_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct boarddef_struct *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	unsigned int iosize;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: poc: using %s iobase 0x%lx\n", dev->minor,
-	       this_board->name, iobase);
+	       board->name, iobase);
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
 	if (iobase == 0) {
 		printk(KERN_ERR "io base address required\n");
 		return -EINVAL;
 	}
 
-	iosize = this_board->iosize;
+	iosize = board->iosize;
 	/* check if io addresses are available */
 	if (!request_region(iobase, iosize, "dac02")) {
 		printk(KERN_ERR "I/O port conflict: failed to allocate ports "
@@ -161,20 +156,22 @@ static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	dev->iobase = iobase;
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
-	if (alloc_private(dev, sizeof(unsigned int) * this_board->n_chan) < 0)
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
+
+	if (alloc_private(dev, sizeof(unsigned int) * board->n_chan) < 0)
 		return -ENOMEM;
 
 	/* analog output subdevice */
 	s = dev->subdevices + 0;
-	s->type = this_board->type;
-	s->n_chan = this_board->n_chan;
-	s->maxdata = (1 << this_board->n_bits) - 1;
-	s->range_table = this_board->range;
-	s->insn_write = this_board->winsn;
-	s->insn_read = this_board->rinsn;
-	s->insn_bits = this_board->insnbits;
+	s->type = board->type;
+	s->n_chan = board->n_chan;
+	s->maxdata = (1 << board->n_bits) - 1;
+	s->range_table = board->range;
+	s->insn_write = board->winsn;
+	s->insn_read = board->rinsn;
+	s->insn_bits = board->insnbits;
 	if (s->type == COMEDI_SUBD_AO || s->type == COMEDI_SUBD_DO)
 		s->subdev_flags = SDF_WRITABLE;
 
@@ -183,8 +180,10 @@ static int poc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void poc_detach(struct comedi_device *dev)
 {
+	const struct boarddef_struct *board = comedi_board(dev);
+
 	if (dev->iobase)
-		release_region(dev->iobase, this_board->iosize);
+		release_region(dev->iobase, board->iosize);
 }
 
 static const struct boarddef_struct boards[] = {
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index 2f130b3095e..a029147c9b6 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -871,8 +871,8 @@ static int daqp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->iobase = local->link->resource[0]->start;
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	printk(KERN_INFO "comedi%d: attaching daqp%d (io 0x%04lx)\n",
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index 1678a0ccb8c..112fdc3e9c6 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -105,7 +105,6 @@ Configuration options:
 #include <linux/delay.h>
 
 #include "../comedidev.h"
-#include "comedi_pci.h"
 
 #define DRV_NAME "rtd520"
 
@@ -195,110 +194,79 @@ Configuration options:
 ======================================================================*/
 
 /*
-  The board has 3 input modes and the gains of 1,2,4,...32 (, 64, 128)
-*/
-static const struct comedi_lrange rtd_ai_7520_range = { 18, {
-							     /* +-5V input range gain steps */
-							     BIP_RANGE(5.0),
-							     BIP_RANGE(5.0 / 2),
-							     BIP_RANGE(5.0 / 4),
-							     BIP_RANGE(5.0 / 8),
-							     BIP_RANGE(5.0 /
-								       16),
-							     BIP_RANGE(5.0 /
-								       32),
-							     /* +-10V input range gain steps */
-							     BIP_RANGE(10.0),
-							     BIP_RANGE(10.0 /
-								       2),
-							     BIP_RANGE(10.0 /
-								       4),
-							     BIP_RANGE(10.0 /
-								       8),
-							     BIP_RANGE(10.0 /
-								       16),
-							     BIP_RANGE(10.0 /
-								       32),
-							     /* +10V input range gain steps */
-							     UNI_RANGE(10.0),
-							     UNI_RANGE(10.0 /
-								       2),
-							     UNI_RANGE(10.0 /
-								       4),
-							     UNI_RANGE(10.0 /
-								       8),
-							     UNI_RANGE(10.0 /
-								       16),
-							     UNI_RANGE(10.0 /
-								       32),
-
-							     }
+ * The board has 3 input modes and the gains of 1,2,4,...32 (, 64, 128)
+ */
+static const struct comedi_lrange rtd_ai_7520_range = {
+	18, {
+		/* +-5V input range gain steps */
+		BIP_RANGE(5.0),
+		BIP_RANGE(5.0 / 2),
+		BIP_RANGE(5.0 / 4),
+		BIP_RANGE(5.0 / 8),
+		BIP_RANGE(5.0 / 16),
+		BIP_RANGE(5.0 / 32),
+		/* +-10V input range gain steps */
+		BIP_RANGE(10.0),
+		BIP_RANGE(10.0 / 2),
+		BIP_RANGE(10.0 / 4),
+		BIP_RANGE(10.0 / 8),
+		BIP_RANGE(10.0 / 16),
+		BIP_RANGE(10.0 / 32),
+		/* +10V input range gain steps */
+		UNI_RANGE(10.0),
+		UNI_RANGE(10.0 / 2),
+		UNI_RANGE(10.0 / 4),
+		UNI_RANGE(10.0 / 8),
+		UNI_RANGE(10.0 / 16),
+		UNI_RANGE(10.0 / 32),
+	}
 };
 
 /* PCI4520 has two more gains (6 more entries) */
-static const struct comedi_lrange rtd_ai_4520_range = { 24, {
-							     /* +-5V input range gain steps */
-							     BIP_RANGE(5.0),
-							     BIP_RANGE(5.0 / 2),
-							     BIP_RANGE(5.0 / 4),
-							     BIP_RANGE(5.0 / 8),
-							     BIP_RANGE(5.0 /
-								       16),
-							     BIP_RANGE(5.0 /
-								       32),
-							     BIP_RANGE(5.0 /
-								       64),
-							     BIP_RANGE(5.0 /
-								       128),
-							     /* +-10V input range gain steps */
-							     BIP_RANGE(10.0),
-							     BIP_RANGE(10.0 /
-								       2),
-							     BIP_RANGE(10.0 /
-								       4),
-							     BIP_RANGE(10.0 /
-								       8),
-							     BIP_RANGE(10.0 /
-								       16),
-							     BIP_RANGE(10.0 /
-								       32),
-							     BIP_RANGE(10.0 /
-								       64),
-							     BIP_RANGE(10.0 /
-								       128),
-							     /* +10V input range gain steps */
-							     UNI_RANGE(10.0),
-							     UNI_RANGE(10.0 /
-								       2),
-							     UNI_RANGE(10.0 /
-								       4),
-							     UNI_RANGE(10.0 /
-								       8),
-							     UNI_RANGE(10.0 /
-								       16),
-							     UNI_RANGE(10.0 /
-								       32),
-							     UNI_RANGE(10.0 /
-								       64),
-							     UNI_RANGE(10.0 /
-								       128),
-							     }
+static const struct comedi_lrange rtd_ai_4520_range = {
+	24, {
+		/* +-5V input range gain steps */
+		BIP_RANGE(5.0),
+		BIP_RANGE(5.0 / 2),
+		BIP_RANGE(5.0 / 4),
+		BIP_RANGE(5.0 / 8),
+		BIP_RANGE(5.0 / 16),
+		BIP_RANGE(5.0 / 32),
+		BIP_RANGE(5.0 / 64),
+		BIP_RANGE(5.0 / 128),
+		/* +-10V input range gain steps */
+		BIP_RANGE(10.0),
+		BIP_RANGE(10.0 / 2),
+		BIP_RANGE(10.0 / 4),
+		BIP_RANGE(10.0 / 8),
+		BIP_RANGE(10.0 / 16),
+		BIP_RANGE(10.0 / 32),
+		BIP_RANGE(10.0 / 64),
+		BIP_RANGE(10.0 / 128),
+		/* +10V input range gain steps */
+		UNI_RANGE(10.0),
+		UNI_RANGE(10.0 / 2),
+		UNI_RANGE(10.0 / 4),
+		UNI_RANGE(10.0 / 8),
+		UNI_RANGE(10.0 / 16),
+		UNI_RANGE(10.0 / 32),
+		UNI_RANGE(10.0 / 64),
+		UNI_RANGE(10.0 / 128),
+	}
 };
 
 /* Table order matches range values */
-static const struct comedi_lrange rtd_ao_range = { 4, {
-						       RANGE(0, 5),
-						       RANGE(0, 10),
-						       RANGE(-5, 5),
-						       RANGE(-10, 10),
-						       }
+static const struct comedi_lrange rtd_ao_range = {
+	4, {
+		UNI_RANGE(5),
+		UNI_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(10),
+	}
 };
 
-/*
-  Board descriptions
- */
 struct rtdBoard {
-	const char *name;	/* must be first */
+	const char *name;
 	int device_id;
 	int aiChans;
 	int aiBits;
@@ -309,31 +277,25 @@ struct rtdBoard {
 
 static const struct rtdBoard rtd520Boards[] = {
 	{
-	 .name = "DM7520",
-	 .device_id = 0x7520,
-	 .aiChans = 16,
-	 .aiBits = 12,
-	 .aiMaxGain = 32,
-	 .range10Start = 6,
-	 .rangeUniStart = 12,
-	 },
-	{
-	 .name = "PCI4520",
-	 .device_id = 0x4520,
-	 .aiChans = 16,
-	 .aiBits = 12,
-	 .aiMaxGain = 128,
-	 .range10Start = 8,
-	 .rangeUniStart = 16,
-	 },
+		.name		= "DM7520",
+		.device_id	= 0x7520,
+		.aiChans	= 16,
+		.aiBits		= 12,
+		.aiMaxGain	= 32,
+		.range10Start	= 6,
+		.rangeUniStart	= 12,
+	}, {
+		.name		= "PCI4520",
+		.device_id	= 0x4520,
+		.aiChans	= 16,
+		.aiBits		= 12,
+		.aiMaxGain	= 128,
+		.range10Start	= 8,
+		.rangeUniStart	= 16,
+	},
 };
 
 /*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct rtdBoard *)dev->board_ptr)
-
-/*
    This structure is for data unique to this hardware driver.
    This is also unique for each board in the system.
 */
@@ -348,10 +310,6 @@ struct rtdPrivate {
 	int transCount;		/* # to transfer data. 0->1/2FIFO */
 	int flags;		/* flag event modes */
 
-	/* PCI device info */
-	struct pci_dev *pci_dev;
-	int got_regions;	/* non-zero if PCI regions owned */
-
 	/* channel list info */
 	/* chanBipolar tracks whether a channel is bipolar (and needs +2048) */
 	unsigned char chanBipolar[RTD_MAX_CHANLIST / 8];	/* bit array */
@@ -400,747 +358,46 @@ struct rtdPrivate {
 	(((array)[(index)/8] &= ~(1 << ((index) & 0x7))))
 
 /*
- * most drivers define the following macro to make it easy to
- * access the private structure.
- */
-#define devpriv ((struct rtdPrivate *)dev->private)
-
-/* Macros to access registers */
-
-/* Reset board */
-#define RtdResetBoard(dev) \
-	writel(0, devpriv->las0+LAS0_BOARD_RESET)
-
-/* Reset channel gain table read pointer */
-#define RtdResetCGT(dev) \
-	writel(0, devpriv->las0+LAS0_CGT_RESET)
-
-/* Reset channel gain table read and write pointers */
-#define RtdClearCGT(dev) \
-	writel(0, devpriv->las0+LAS0_CGT_CLEAR)
-
-/* Reset channel gain table read and write pointers */
-#define RtdEnableCGT(dev, v) \
-	writel((v > 0) ? 1 : 0, devpriv->las0+LAS0_CGT_ENABLE)
-
-/* Write channel gain table entry */
-#define RtdWriteCGTable(dev, v) \
-	writel(v, devpriv->las0+LAS0_CGT_WRITE)
-
-/* Write Channel Gain Latch */
-#define RtdWriteCGLatch(dev, v) \
-	writel(v, devpriv->las0+LAS0_CGL_WRITE)
-
-/* Reset ADC FIFO */
-#define RtdAdcClearFifo(dev) \
-	writel(0, devpriv->las0+LAS0_ADC_FIFO_CLEAR)
-
-/* Set ADC start conversion source select (write only) */
-#define RtdAdcConversionSource(dev, v) \
-	writel(v, devpriv->las0+LAS0_ADC_CONVERSION)
-
-/* Set burst start source select (write only) */
-#define RtdBurstStartSource(dev, v) \
-	writel(v, devpriv->las0+LAS0_BURST_START)
-
-/* Set Pacer start source select (write only) */
-#define RtdPacerStartSource(dev, v) \
-	writel(v, devpriv->las0+LAS0_PACER_START)
-
-/* Set Pacer stop source select (write only) */
-#define RtdPacerStopSource(dev, v) \
-	writel(v, devpriv->las0+LAS0_PACER_STOP)
-
-/* Set Pacer clock source select (write only) 0=external 1=internal */
-#define RtdPacerClockSource(dev, v) \
-	writel((v > 0) ? 1 : 0, devpriv->las0+LAS0_PACER_SELECT)
-
-/* Set sample counter source select (write only) */
-#define RtdAdcSampleCounterSource(dev, v) \
-	writel(v, devpriv->las0+LAS0_ADC_SCNT_SRC)
-
-/* Set Pacer trigger mode select (write only) 0=single cycle, 1=repeat */
-#define RtdPacerTriggerMode(dev, v) \
-	writel((v > 0) ? 1 : 0, devpriv->las0+LAS0_PACER_REPEAT)
-
-/* Set About counter stop enable (write only) */
-#define RtdAboutStopEnable(dev, v) \
-	writel((v > 0) ? 1 : 0, devpriv->las0+LAS0_ACNT_STOP_ENABLE)
-
-/* Set external trigger polarity (write only) 0=positive edge, 1=negative */
-#define RtdTriggerPolarity(dev, v) \
-	writel((v > 0) ? 1 : 0, devpriv->las0+LAS0_ETRG_POLARITY)
-
-/* Start single ADC conversion */
-#define RtdAdcStart(dev) \
-	writew(0, devpriv->las0+LAS0_ADC)
-
-/* Read one ADC data value (12bit (with sign extend) as 16bit) */
-/* Note: matches what DMA would get.  Actual value >> 3 */
-#define RtdAdcFifoGet(dev) \
-	readw(devpriv->las1+LAS1_ADC_FIFO)
-
-/* Read two ADC data values (DOESN'T WORK) */
-#define RtdAdcFifoGet2(dev) \
-	readl(devpriv->las1+LAS1_ADC_FIFO)
-
-/* FIFO status */
-#define RtdFifoStatus(dev) \
-	readl(devpriv->las0+LAS0_ADC)
-
-/* pacer start/stop read=start, write=stop*/
-#define RtdPacerStart(dev) \
-	readl(devpriv->las0+LAS0_PACER)
-#define RtdPacerStop(dev) \
-	writel(0, devpriv->las0+LAS0_PACER)
-
-/* Interrupt status */
-#define RtdInterruptStatus(dev) \
-	readw(devpriv->las0+LAS0_IT)
-
-/* Interrupt mask */
-#define RtdInterruptMask(dev, v) \
-	writew((devpriv->intMask = (v)), devpriv->las0+LAS0_IT)
-
-/* Interrupt status clear (only bits set in mask) */
-#define RtdInterruptClear(dev) \
-	readw(devpriv->las0+LAS0_CLEAR)
-
-/* Interrupt clear mask */
-#define RtdInterruptClearMask(dev, v) \
-	writew((devpriv->intClearMask = (v)), devpriv->las0+LAS0_CLEAR)
-
-/* Interrupt overrun status */
-#define RtdInterruptOverrunStatus(dev) \
-	readl(devpriv->las0+LAS0_OVERRUN)
-
-/* Interrupt overrun clear */
-#define RtdInterruptOverrunClear(dev) \
-	writel(0, devpriv->las0+LAS0_OVERRUN)
-
-/* Pacer counter, 24bit */
-#define RtdPacerCount(dev) \
-	readl(devpriv->las0+LAS0_PCLK)
-#define RtdPacerCounter(dev, v) \
-	writel((v) & 0xffffff, devpriv->las0+LAS0_PCLK)
-
-/* Burst counter, 10bit */
-#define RtdBurstCount(dev) \
-	readl(devpriv->las0+LAS0_BCLK)
-#define RtdBurstCounter(dev, v) \
-	writel((v) & 0x3ff, devpriv->las0+LAS0_BCLK)
-
-/* Delay counter, 16bit */
-#define RtdDelayCount(dev) \
-	readl(devpriv->las0+LAS0_DCLK)
-#define RtdDelayCounter(dev, v) \
-	writel((v) & 0xffff, devpriv->las0+LAS0_DCLK)
-
-/* About counter, 16bit */
-#define RtdAboutCount(dev) \
-	readl(devpriv->las0+LAS0_ACNT)
-#define RtdAboutCounter(dev, v) \
-	writel((v) & 0xffff, devpriv->las0+LAS0_ACNT)
-
-/* ADC sample counter, 10bit */
-#define RtdAdcSampleCount(dev) \
-	readl(devpriv->las0+LAS0_ADC_SCNT)
-#define RtdAdcSampleCounter(dev, v) \
-	writel((v) & 0x3ff, devpriv->las0+LAS0_ADC_SCNT)
-
-/* User Timer/Counter (8254) */
-#define RtdUtcCounterGet(dev, n) \
-	readb(devpriv->las0 \
-		+ ((n <= 0) ? LAS0_UTC0 : ((1 == n) ? LAS0_UTC1 : LAS0_UTC2)))
-
-#define RtdUtcCounterPut(dev, n, v) \
-	writeb((v) & 0xff, devpriv->las0 \
-		+ ((n <= 0) ? LAS0_UTC0 : ((1 == n) ? LAS0_UTC1 : LAS0_UTC2)))
-
-/* Set UTC (8254) control byte  */
-#define RtdUtcCtrlPut(dev, n, v) \
-	writeb(devpriv->utcCtrl[(n) & 3] = (((n) & 3) << 6) | ((v) & 0x3f), \
-		devpriv->las0 + LAS0_UTC_CTRL)
-
-/* Set UTCn clock source (write only) */
-#define RtdUtcClockSource(dev, n, v) \
-	writew(v, devpriv->las0 \
-		+ ((n <= 0) ? LAS0_UTC0_CLOCK : \
-			((1 == n) ? LAS0_UTC1_CLOCK : LAS0_UTC2_CLOCK)))
-
-/* Set UTCn gate source (write only) */
-#define RtdUtcGateSource(dev, n, v) \
-	writew(v, devpriv->las0 \
-		+ ((n <= 0) ? LAS0_UTC0_GATE : \
-			((1 == n) ? LAS0_UTC1_GATE : LAS0_UTC2_GATE)))
-
-/* User output N source select (write only) */
-#define RtdUsrOutSource(dev, n, v) \
-	writel(v, devpriv->las0+((n <= 0) ? LAS0_UOUT0_SELECT : \
-				LAS0_UOUT1_SELECT))
-
-/* Digital IO */
-#define RtdDio0Read(dev) \
-	(readw(devpriv->las0+LAS0_DIO0) & 0xff)
-#define RtdDio0Write(dev, v) \
-	writew((v) & 0xff, devpriv->las0+LAS0_DIO0)
-
-#define RtdDio1Read(dev) \
-	(readw(devpriv->las0+LAS0_DIO1) & 0xff)
-#define RtdDio1Write(dev, v) \
-	writew((v) & 0xff, devpriv->las0+LAS0_DIO1)
-
-#define RtdDioStatusRead(dev) \
-	(readw(devpriv->las0+LAS0_DIO_STATUS) & 0xff)
-#define RtdDioStatusWrite(dev, v) \
-	writew((devpriv->dioStatus = (v)), devpriv->las0+LAS0_DIO_STATUS)
-
-#define RtdDio0CtrlRead(dev) \
-	(readw(devpriv->las0+LAS0_DIO0_CTRL) & 0xff)
-#define RtdDio0CtrlWrite(dev, v) \
-	writew((v) & 0xff, devpriv->las0+LAS0_DIO0_CTRL)
-
-/* Digital to Analog converter */
-/* Write one data value (sign + 12bit + marker bits) */
-/* Note: matches what DMA would put.  Actual value << 3 */
-#define RtdDacFifoPut(dev, n, v) \
-	writew((v), devpriv->las1 + (((n) == 0) ? LAS1_DAC1_FIFO : \
-				LAS1_DAC2_FIFO))
-
-/* Start single DAC conversion */
-#define RtdDacUpdate(dev, n) \
-	writew(0, devpriv->las0 + (((n) == 0) ? LAS0_DAC1 : LAS0_DAC2))
-
-/* Start single DAC conversion on both DACs */
-#define RtdDacBothUpdate(dev) \
-	writew(0, devpriv->las0+LAS0_DAC)
-
-/* Set DAC output type and range */
-#define RtdDacRange(dev, n, v) \
-	writew((v) & 7, devpriv->las0 \
-		+(((n) == 0) ? LAS0_DAC1_CTRL : LAS0_DAC2_CTRL))
-
-/* Reset DAC FIFO */
-#define RtdDacClearFifo(dev, n) \
-	writel(0, devpriv->las0+(((n) == 0) ? LAS0_DAC1_RESET : \
-				LAS0_DAC2_RESET))
-
-/* Set source for DMA 0 (write only, shadow?) */
-#define RtdDma0Source(dev, n) \
-	writel((n) & 0xf, devpriv->las0+LAS0_DMA0_SRC)
-
-/* Set source for DMA 1 (write only, shadow?) */
-#define RtdDma1Source(dev, n) \
-	writel((n) & 0xf, devpriv->las0+LAS0_DMA1_SRC)
-
-/* Reset board state for DMA 0 */
-#define RtdDma0Reset(dev) \
-	writel(0, devpriv->las0+LAS0_DMA0_RESET)
-
-/* Reset board state for DMA 1 */
-#define RtdDma1Reset(dev) \
-	writel(0, devpriv->las0+LAS0_DMA1_SRC)
-
-/* PLX9080 interrupt mask and status */
-#define RtdPlxInterruptRead(dev) \
-	readl(devpriv->lcfg+LCFG_ITCSR)
-#define RtdPlxInterruptWrite(dev, v) \
-	writel(v, devpriv->lcfg+LCFG_ITCSR)
-
-/* Set  mode for DMA 0 */
-#define RtdDma0Mode(dev, m) \
-	writel((m), devpriv->lcfg+LCFG_DMAMODE0)
-
-/* Set PCI address for DMA 0 */
-#define RtdDma0PciAddr(dev, a) \
-	writel((a), devpriv->lcfg+LCFG_DMAPADR0)
-
-/* Set local address for DMA 0 */
-#define RtdDma0LocalAddr(dev, a) \
-	writel((a), devpriv->lcfg+LCFG_DMALADR0)
-
-/* Set byte count for DMA 0 */
-#define RtdDma0Count(dev, c) \
-	writel((c), devpriv->lcfg+LCFG_DMASIZ0)
-
-/* Set next descriptor for DMA 0 */
-#define RtdDma0Next(dev, a) \
-	writel((a), devpriv->lcfg+LCFG_DMADPR0)
-
-/* Set  mode for DMA 1 */
-#define RtdDma1Mode(dev, m) \
-	writel((m), devpriv->lcfg+LCFG_DMAMODE1)
-
-/* Set PCI address for DMA 1 */
-#define RtdDma1PciAddr(dev, a) \
-	writel((a), devpriv->lcfg+LCFG_DMAADR1)
-
-/* Set local address for DMA 1 */
-#define RtdDma1LocalAddr(dev, a) \
-	writel((a), devpriv->lcfg+LCFG_DMALADR1)
-
-/* Set byte count for DMA 1 */
-#define RtdDma1Count(dev, c) \
-	writel((c), devpriv->lcfg+LCFG_DMASIZ1)
-
-/* Set next descriptor for DMA 1 */
-#define RtdDma1Next(dev, a) \
-	writel((a), devpriv->lcfg+LCFG_DMADPR1)
-
-/* Set control for DMA 0 (write only, shadow?) */
-#define RtdDma0Control(dev, n) \
-	writeb(devpriv->dma0Control = (n), devpriv->lcfg+LCFG_DMACSR0)
-
-/* Get status for DMA 0 */
-#define RtdDma0Status(dev) \
-	readb(devpriv->lcfg+LCFG_DMACSR0)
-
-/* Set control for DMA 1 (write only, shadow?) */
-#define RtdDma1Control(dev, n) \
-	writeb(devpriv->dma1Control = (n), devpriv->lcfg+LCFG_DMACSR1)
-
-/* Get status for DMA 1 */
-#define RtdDma1Status(dev) \
-	readb(devpriv->lcfg+LCFG_DMACSR1)
-
-static int rtd_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data);
-static int rtd_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data);
-static int rtd_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data);
-static int rtd_dio_insn_bits(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data);
-static int rtd_dio_insn_config(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int rtd_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_cmd *cmd);
-static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int rtd_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s);
-/*
- * static int rtd_ai_poll(struct comedi_device *dev,
- *			  struct comedi_subdevice *s);
- */
-static int rtd_ns_to_timer(unsigned int *ns, int roundMode);
-static irqreturn_t rtd_interrupt(int irq, void *d);
-static int rtd520_probe_fifo_depth(struct comedi_device *dev);
-
-/*
- * Attach is called by the Comedi core to configure the driver
- * for a particular board.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that
- * address.
- */
-static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{				/* board name and options flags */
-	struct comedi_subdevice *s;
-	struct pci_dev *pcidev;
-	int ret;
-	resource_size_t physLas0;	/* configuration */
-	resource_size_t physLas1;	/* data area */
-	resource_size_t physLcfg;	/* PLX9080 */
-#ifdef USE_DMA
-	int index;
-#endif
-
-	printk(KERN_INFO "comedi%d: rtd520 attaching.\n", dev->minor);
-
-#if defined(CONFIG_COMEDI_DEBUG) && defined(USE_DMA)
-	/* You can set this a load time: modprobe comedi comedi_debug=1 */
-	if (0 == comedi_debug)	/* force DMA debug printks */
-		comedi_debug = 1;
-#endif
-
-	/*
-	 * Allocate the private structure area.  alloc_private() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_private(dev, sizeof(struct rtdPrivate)) < 0)
-		return -ENOMEM;
-
-	/*
-	 * Probe the device to determine what device in the series it is.
-	 */
-	for (pcidev = pci_get_device(PCI_VENDOR_ID_RTD, PCI_ANY_ID, NULL);
-	     pcidev != NULL;
-	     pcidev = pci_get_device(PCI_VENDOR_ID_RTD, PCI_ANY_ID, pcidev)) {
-		int i;
-
-		if (it->options[0] || it->options[1]) {
-			if (pcidev->bus->number != it->options[0]
-			    || PCI_SLOT(pcidev->devfn) != it->options[1]) {
-				continue;
-			}
-		}
-		for (i = 0; i < ARRAY_SIZE(rtd520Boards); ++i) {
-			if (pcidev->device == rtd520Boards[i].device_id) {
-				dev->board_ptr = &rtd520Boards[i];
-				break;
-			}
-		}
-		if (dev->board_ptr)
-			break;	/* found one */
-	}
-	if (!pcidev) {
-		if (it->options[0] && it->options[1]) {
-			printk(KERN_INFO "No RTD card at bus=%d slot=%d.\n",
-			       it->options[0], it->options[1]);
-		} else {
-			printk(KERN_INFO "No RTD card found.\n");
-		}
-		return -EIO;
-	}
-	devpriv->pci_dev = pcidev;
-	dev->board_name = thisboard->name;
-
-	ret = comedi_pci_enable(pcidev, DRV_NAME);
-	if (ret < 0) {
-		printk(KERN_INFO "Failed to enable PCI device and request regions.\n");
-		return ret;
-	}
-	devpriv->got_regions = 1;
-
-	/*
-	 * Initialize base addresses
-	 */
-	/* Get the physical address from PCI config */
-	physLas0 = pci_resource_start(devpriv->pci_dev, LAS0_PCIINDEX);
-	physLas1 = pci_resource_start(devpriv->pci_dev, LAS1_PCIINDEX);
-	physLcfg = pci_resource_start(devpriv->pci_dev, LCFG_PCIINDEX);
-	/* Now have the kernel map this into memory */
-	/* ASSUME page aligned */
-	devpriv->las0 = ioremap_nocache(physLas0, LAS0_PCISIZE);
-	devpriv->las1 = ioremap_nocache(physLas1, LAS1_PCISIZE);
-	devpriv->lcfg = ioremap_nocache(physLcfg, LCFG_PCISIZE);
-
-	if (!devpriv->las0 || !devpriv->las1 || !devpriv->lcfg)
-		return -ENOMEM;
-
-
-	DPRINTK("%s: LAS0=%llx, LAS1=%llx, CFG=%llx.\n", dev->board_name,
-		(unsigned long long)physLas0, (unsigned long long)physLas1,
-		(unsigned long long)physLcfg);
-	{			/* The RTD driver does this */
-		unsigned char pci_latency;
-		u16 revision;
-		/*uint32_t epld_version; */
-
-		pci_read_config_word(devpriv->pci_dev, PCI_REVISION_ID,
-				     &revision);
-		DPRINTK("%s: PCI revision %d.\n", dev->board_name, revision);
-
-		pci_read_config_byte(devpriv->pci_dev,
-				     PCI_LATENCY_TIMER, &pci_latency);
-		if (pci_latency < 32) {
-			printk(KERN_INFO "%s: PCI latency changed from %d to %d\n",
-			       dev->board_name, pci_latency, 32);
-			pci_write_config_byte(devpriv->pci_dev,
-					      PCI_LATENCY_TIMER, 32);
-		} else {
-			DPRINTK("rtd520: PCI latency = %d\n", pci_latency);
-		}
-
-		/*
-		 * Undocumented EPLD version (doesn't match RTD driver results)
-		 */
-		/*DPRINTK ("rtd520: Reading epld from %p\n",
-		   devpriv->las0+0);
-		   epld_version = readl (devpriv->las0+0);
-		   if ((epld_version & 0xF0) >> 4 == 0x0F) {
-		   DPRINTK("rtd520: pre-v8 EPLD. (%x)\n", epld_version);
-		   } else {
-		   DPRINTK("rtd520: EPLD version %x.\n", epld_version >> 4);
-		   } */
-	}
-
-	/* Show board configuration */
-	printk(KERN_INFO "%s:", dev->board_name);
-
-	/*
-	 * Allocate the subdevice structures.  alloc_subdevice() is a
-	 * convenient macro defined in comedidev.h.
-	 */
-	if (alloc_subdevices(dev, 4) < 0)
-		return -ENOMEM;
-
-
-	s = dev->subdevices + 0;
-	dev->read_subdev = s;
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags =
-	    SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = thisboard->aiChans;
-	s->maxdata = (1 << thisboard->aiBits) - 1;
-	if (thisboard->aiMaxGain <= 32)
-		s->range_table = &rtd_ai_7520_range;
-	else
-		s->range_table = &rtd_ai_4520_range;
-
-	s->len_chanlist = RTD_MAX_CHANLIST;	/* devpriv->fifoLen */
-	s->insn_read = rtd_ai_rinsn;
-	s->do_cmd = rtd_ai_cmd;
-	s->do_cmdtest = rtd_ai_cmdtest;
-	s->cancel = rtd_ai_cancel;
-	/* s->poll = rtd_ai_poll; *//* not ready yet */
-
-	s = dev->subdevices + 1;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 2;
-	s->maxdata = (1 << thisboard->aiBits) - 1;
-	s->range_table = &rtd_ao_range;
-	s->insn_write = rtd_ao_winsn;
-	s->insn_read = rtd_ao_rinsn;
-
-	s = dev->subdevices + 2;
-	/* digital i/o subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	/* we only support port 0 right now.  Ignoring port 1 and user IO */
-	s->n_chan = 8;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = rtd_dio_insn_bits;
-	s->insn_config = rtd_dio_insn_config;
-
-	/* timer/counter subdevices (not currently supported) */
-	s = dev->subdevices + 3;
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 3;
-	s->maxdata = 0xffff;
-
-	/* initialize board, per RTD spec */
-	/* also, initialize shadow registers */
-	RtdResetBoard(dev);
-	udelay(100);		/* needed? */
-	RtdPlxInterruptWrite(dev, 0);
-	RtdInterruptMask(dev, 0);	/* and sets shadow */
-	RtdInterruptClearMask(dev, ~0);	/* and sets shadow */
-	RtdInterruptClear(dev);	/* clears bits set by mask */
-	RtdInterruptOverrunClear(dev);
-	RtdClearCGT(dev);
-	RtdAdcClearFifo(dev);
-	RtdDacClearFifo(dev, 0);
-	RtdDacClearFifo(dev, 1);
-	/* clear digital IO fifo */
-	RtdDioStatusWrite(dev, 0);	/* safe state, set shadow */
-	RtdUtcCtrlPut(dev, 0, 0x30);	/* safe state, set shadow */
-	RtdUtcCtrlPut(dev, 1, 0x30);	/* safe state, set shadow */
-	RtdUtcCtrlPut(dev, 2, 0x30);	/* safe state, set shadow */
-	RtdUtcCtrlPut(dev, 3, 0);	/* safe state, set shadow */
-	/* TODO: set user out source ??? */
-
-	/* check if our interrupt is available and get it */
-	ret = request_irq(devpriv->pci_dev->irq, rtd_interrupt,
-			  IRQF_SHARED, DRV_NAME, dev);
-
-	if (ret < 0) {
-		printk("Could not get interrupt! (%u)\n",
-		       devpriv->pci_dev->irq);
-		return ret;
-	}
-	dev->irq = devpriv->pci_dev->irq;
-	printk(KERN_INFO "( irq=%u )", dev->irq);
-
-	ret = rtd520_probe_fifo_depth(dev);
-	if (ret < 0)
-		return ret;
-
-	devpriv->fifoLen = ret;
-	printk("( fifoLen=%d )", devpriv->fifoLen);
-
-#ifdef USE_DMA
-	if (dev->irq > 0) {
-		printk("( DMA buff=%d )\n", DMA_CHAIN_COUNT);
-		/*
-		 * The PLX9080 has 2 DMA controllers, but there could be
-		 * 4 sources: ADC, digital, DAC1, and DAC2.  Since only the
-		 * ADC supports cmd mode right now, this isn't an issue (yet)
-		 */
-		devpriv->dma0Offset = 0;
-
-		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
-			devpriv->dma0Buff[index] =
-			    pci_alloc_consistent(devpriv->pci_dev,
-						 sizeof(u16) *
-						 devpriv->fifoLen / 2,
-						 &devpriv->
-						 dma0BuffPhysAddr[index]);
-			if (devpriv->dma0Buff[index] == NULL) {
-				ret = -ENOMEM;
-				goto rtd_attach_die_error;
-			}
-			/*DPRINTK ("buff[%d] @ %p virtual, %x PCI\n",
-			   index,
-			   devpriv->dma0Buff[index],
-			   devpriv->dma0BuffPhysAddr[index]); */
-		}
-
-		/*
-		 * setup DMA descriptor ring (use cpu_to_le32 for byte
-		 * ordering?)
-		 */
-		devpriv->dma0Chain =
-		    pci_alloc_consistent(devpriv->pci_dev,
-					 sizeof(struct plx_dma_desc) *
-					 DMA_CHAIN_COUNT,
-					 &devpriv->dma0ChainPhysAddr);
-		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
-			devpriv->dma0Chain[index].pci_start_addr =
-			    devpriv->dma0BuffPhysAddr[index];
-			devpriv->dma0Chain[index].local_start_addr =
-			    DMALADDR_ADC;
-			devpriv->dma0Chain[index].transfer_size =
-			    sizeof(u16) * devpriv->fifoLen / 2;
-			devpriv->dma0Chain[index].next =
-			    (devpriv->dma0ChainPhysAddr + ((index +
-							    1) %
-							   (DMA_CHAIN_COUNT))
-			     * sizeof(devpriv->dma0Chain[0]))
-			    | DMA_TRANSFER_BITS;
-			/*DPRINTK ("ring[%d] @%lx PCI: %x, local: %x, N: 0x%x, next: %x\n",
-			   index,
-			   ((long)devpriv->dma0ChainPhysAddr
-			   + (index * sizeof(devpriv->dma0Chain[0]))),
-			   devpriv->dma0Chain[index].pci_start_addr,
-			   devpriv->dma0Chain[index].local_start_addr,
-			   devpriv->dma0Chain[index].transfer_size,
-			   devpriv->dma0Chain[index].next); */
-		}
-
-		if (devpriv->dma0Chain == NULL) {
-			ret = -ENOMEM;
-			goto rtd_attach_die_error;
-		}
-
-		RtdDma0Mode(dev, DMA_MODE_BITS);
-		/* set DMA trigger source */
-		RtdDma0Source(dev, DMAS_ADFIFO_HALF_FULL);
-	} else {
-		printk(KERN_INFO "( no IRQ->no DMA )");
-	}
-#endif /* USE_DMA */
-
-	if (dev->irq) {		/* enable plx9080 interrupts */
-		RtdPlxInterruptWrite(dev, ICS_PIE | ICS_PLIE);
-	}
-
-	printk("\ncomedi%d: rtd520 driver attached.\n", dev->minor);
-
-	return 1;
+  Given a desired period and the clock period (both in ns),
+  return the proper counter value (divider-1).
+  Sets the original period to be the true value.
+  Note: you have to check if the value is larger than the counter range!
+*/
+static int rtd_ns_to_timer_base(unsigned int *nanosec,	/* desired period (in ns) */
+				int round_mode, int base)
+{				/* clock period (in ns) */
+	int divider;
 
-#if 0
-	/* hit an error, clean up memory and return ret */
-/* rtd_attach_die_error: */
-#ifdef USE_DMA
-	for (index = 0; index < DMA_CHAIN_COUNT; index++) {
-		if (NULL != devpriv->dma0Buff[index]) {	/* free buffer memory */
-			pci_free_consistent(devpriv->pci_dev,
-					    sizeof(u16) * devpriv->fifoLen / 2,
-					    devpriv->dma0Buff[index],
-					    devpriv->dma0BuffPhysAddr[index]);
-			devpriv->dma0Buff[index] = NULL;
-		}
-	}
-	if (NULL != devpriv->dma0Chain) {
-		pci_free_consistent(devpriv->pci_dev,
-				    sizeof(struct plx_dma_desc)
-				    * DMA_CHAIN_COUNT,
-				    devpriv->dma0Chain,
-				    devpriv->dma0ChainPhysAddr);
-		devpriv->dma0Chain = NULL;
-	}
-#endif /* USE_DMA */
-	/* subdevices and priv are freed by the core */
-	if (dev->irq) {
-		/* disable interrupt controller */
-		RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
-				     & ~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E));
-		free_irq(dev->irq, dev);
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		divider = (*nanosec + base / 2) / base;
+		break;
+	case TRIG_ROUND_DOWN:
+		divider = (*nanosec) / base;
+		break;
+	case TRIG_ROUND_UP:
+		divider = (*nanosec + base - 1) / base;
+		break;
 	}
+	if (divider < 2)
+		divider = 2;	/* min is divide by 2 */
 
-	/* release all regions that were allocated */
-	if (devpriv->las0)
-		iounmap(devpriv->las0);
-
-	if (devpriv->las1)
-		iounmap(devpriv->las1);
-
-	if (devpriv->lcfg)
-		iounmap(devpriv->lcfg);
-
-	if (devpriv->pci_dev)
-		pci_dev_put(devpriv->pci_dev);
+	/* Note: we don't check for max, because different timers
+	   have different ranges */
 
-	return ret;
-#endif
+	*nanosec = base * divider;
+	return divider - 1;	/* countdown is divisor+1 */
 }
 
-static void rtd_detach(struct comedi_device *dev)
+/*
+  Given a desired period (in ns),
+  return the proper counter value (divider-1) for the internal clock.
+  Sets the original period to be the true value.
+*/
+static int rtd_ns_to_timer(unsigned int *ns, int round_mode)
 {
-#ifdef USE_DMA
-	int index;
-#endif
-
-	if (devpriv) {
-		/* Shut down any board ops by resetting it */
-#ifdef USE_DMA
-		if (devpriv->lcfg) {
-			RtdDma0Control(dev, 0);	/* disable DMA */
-			RtdDma1Control(dev, 0);	/* disable DMA */
-			RtdPlxInterruptWrite(dev, ICS_PIE | ICS_PLIE);
-		}
-#endif /* USE_DMA */
-		if (devpriv->las0) {
-			RtdResetBoard(dev);
-			RtdInterruptMask(dev, 0);
-			RtdInterruptClearMask(dev, ~0);
-			RtdInterruptClear(dev);	/* clears bits set by mask */
-		}
-#ifdef USE_DMA
-		/* release DMA */
-		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
-			if (NULL != devpriv->dma0Buff[index]) {
-				pci_free_consistent(devpriv->pci_dev,
-						    sizeof(u16) *
-						    devpriv->fifoLen / 2,
-						    devpriv->dma0Buff[index],
-						    devpriv->
-						    dma0BuffPhysAddr[index]);
-				devpriv->dma0Buff[index] = NULL;
-			}
-		}
-		if (NULL != devpriv->dma0Chain) {
-			pci_free_consistent(devpriv->pci_dev,
-					    sizeof(struct plx_dma_desc) *
-					    DMA_CHAIN_COUNT, devpriv->dma0Chain,
-					    devpriv->dma0ChainPhysAddr);
-			devpriv->dma0Chain = NULL;
-		}
-#endif /* USE_DMA */
-		if (dev->irq) {
-			RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
-					     & ~(ICS_PLIE | ICS_DMA0_E |
-						 ICS_DMA1_E));
-			free_irq(dev->irq, dev);
-		}
-		if (devpriv->las0)
-			iounmap(devpriv->las0);
-		if (devpriv->las1)
-			iounmap(devpriv->las1);
-		if (devpriv->lcfg)
-			iounmap(devpriv->lcfg);
-		if (devpriv->pci_dev) {
-			if (devpriv->got_regions)
-				comedi_pci_disable(devpriv->pci_dev);
-			pci_dev_put(devpriv->pci_dev);
-		}
-	}
+	return rtd_ns_to_timer_base(ns, round_mode, RTD_CLOCK_BASE);
 }
 
 /*
@@ -1149,6 +406,8 @@ static void rtd_detach(struct comedi_device *dev)
 static unsigned short rtdConvertChanGain(struct comedi_device *dev,
 					 unsigned int comediChan, int chanIndex)
 {				/* index in channel list */
+	const struct rtdBoard *thisboard = comedi_board(dev);
+	struct rtdPrivate *devpriv = dev->private;
 	unsigned int chan, range, aref;
 	unsigned short r = 0;
 
@@ -1201,17 +460,21 @@ static unsigned short rtdConvertChanGain(struct comedi_device *dev,
 static void rtd_load_channelgain_list(struct comedi_device *dev,
 				      unsigned int n_chan, unsigned int *list)
 {
+	struct rtdPrivate *devpriv = dev->private;
+
 	if (n_chan > 1) {	/* setup channel gain table */
 		int ii;
-		RtdClearCGT(dev);
-		RtdEnableCGT(dev, 1);	/* enable table */
+
+		writel(0, devpriv->las0 + LAS0_CGT_CLEAR);
+		writel(1, devpriv->las0 + LAS0_CGT_ENABLE);
 		for (ii = 0; ii < n_chan; ii++) {
-			RtdWriteCGTable(dev, rtdConvertChanGain(dev, list[ii],
-								ii));
+			writel(rtdConvertChanGain(dev, list[ii], ii),
+				devpriv->las0 + LAS0_CGT_WRITE);
 		}
 	} else {		/* just use the channel gain latch */
-		RtdEnableCGT(dev, 0);	/* disable table, enable latch */
-		RtdWriteCGLatch(dev, rtdConvertChanGain(dev, list[0], 0));
+		writel(0, devpriv->las0 + LAS0_CGT_ENABLE);
+		writel(rtdConvertChanGain(dev, list[0], 0),
+			devpriv->las0 + LAS0_CGL_WRITE);
 	}
 }
 
@@ -1219,21 +482,23 @@ static void rtd_load_channelgain_list(struct comedi_device *dev,
 empty status flag clears */
 static int rtd520_probe_fifo_depth(struct comedi_device *dev)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	unsigned int chanspec = CR_PACK(0, 0, AREF_GROUND);
 	unsigned i;
 	static const unsigned limit = 0x2000;
 	unsigned fifo_size = 0;
 
-	RtdAdcClearFifo(dev);
+	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
 	rtd_load_channelgain_list(dev, 1, &chanspec);
-	RtdAdcConversionSource(dev, 0);	/* software */
+	/* ADC conversion trigger source: SOFTWARE */
+	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
 	/* convert  samples */
 	for (i = 0; i < limit; ++i) {
 		unsigned fifo_status;
 		/* trigger conversion */
-		RtdAdcStart(dev);
+		writew(0, devpriv->las0 + LAS0_ADC);
 		udelay(1);
-		fifo_status = RtdFifoStatus(dev);
+		fifo_status = readl(devpriv->las0 + LAS0_ADC);
 		if ((fifo_status & FS_ADC_HEMPTY) == 0) {
 			fifo_size = 2 * i;
 			break;
@@ -1244,7 +509,7 @@ static int rtd520_probe_fifo_depth(struct comedi_device *dev)
 		       DRV_NAME);
 		return -EIO;
 	}
-	RtdAdcClearFifo(dev);
+	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
 	if (fifo_size != 0x400 && fifo_size != 0x2000) {
 		printk
 		    (KERN_INFO "\ncomedi: %s: unexpected fifo size of %i, expected 1024 or 8192.\n",
@@ -1266,26 +531,27 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 			struct comedi_subdevice *s, struct comedi_insn *insn,
 			unsigned int *data)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	int n, ii;
 	int stat;
 
 	/* clear any old fifo data */
-	RtdAdcClearFifo(dev);
+	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
 
 	/* write channel to multiplexer and clear channel gain table */
 	rtd_load_channelgain_list(dev, 1, &insn->chanspec);
 
-	/* set conversion source */
-	RtdAdcConversionSource(dev, 0);	/* software */
+	/* ADC conversion trigger source: SOFTWARE */
+	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
 
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
 		s16 d;
 		/* trigger conversion */
-		RtdAdcStart(dev);
+		writew(0, devpriv->las0 + LAS0_ADC);
 
 		for (ii = 0; ii < RTD_ADC_TIMEOUT; ++ii) {
-			stat = RtdFifoStatus(dev);
+			stat = readl(devpriv->las0 + LAS0_ADC);
 			if (stat & FS_ADC_NOT_EMPTY)	/* 1 -> not empty */
 				break;
 			WAIT_QUIETLY;
@@ -1298,7 +564,7 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 		}
 
 		/* read data */
-		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 		/*printk ("rtd520: Got 0x%x after %d usec\n", d, ii+1); */
 		d = d >> 3;	/* low 3 bits are marker lines */
 		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, 0))
@@ -1321,6 +587,7 @@ static int rtd_ai_rinsn(struct comedi_device *dev,
 static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		     int count)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	int ii;
 
 	for (ii = 0; ii < count; ii++) {
@@ -1328,17 +595,17 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 		s16 d;
 
 		if (0 == devpriv->aiCount) {	/* done */
-			d = RtdAdcFifoGet(dev);	/* Read N and discard */
+			d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 			continue;
 		}
 #if 0
-		if (0 == (RtdFifoStatus(dev) & FS_ADC_NOT_EMPTY)) {	/* DEBUG */
+		if (!(readl(devpriv->las0 + LAS0_ADC) & FS_ADC_NOT_EMPTY)) {
 			DPRINTK("comedi: READ OOPS on %d of %d\n", ii + 1,
 				count);
 			break;
 		}
 #endif
-		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+		d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 
 		d = d >> 3;	/* low 3 bits are marker lines */
 		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
@@ -1361,9 +628,11 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 */
 static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	while (RtdFifoStatus(dev) & FS_ADC_NOT_EMPTY) {	/* 1 -> not empty */
+	struct rtdPrivate *devpriv = dev->private;
+
+	while (readl(devpriv->las0 + LAS0_ADC) & FS_ADC_NOT_EMPTY) {
 		short sample;
-		s16 d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+		s16 d = readw(devpriv->las1 + LAS1_ADC_FIFO);
 
 		if (0 == devpriv->aiCount) {	/* done */
 			continue;	/* read rest */
@@ -1391,6 +660,7 @@ static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 */
 void abort_dma(struct comedi_device *dev, unsigned int channel)
 {				/* DMA channel 0, 1 */
+	struct rtdPrivate *devpriv = dev->private;
 	unsigned long dma_cs_addr;	/* the control/status register */
 	uint8_t status;
 	unsigned int ii;
@@ -1449,6 +719,7 @@ abortDmaExit:
 */
 static int ai_process_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	int ii, n;
 	s16 *dp;
 
@@ -1511,17 +782,19 @@ static int ai_process_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 				 void *d)
 {				/* our data *//* cpu context (ignored) */
-	struct comedi_device *dev = d;	/* must be called "dev" for devpriv */
+	struct comedi_device *dev = d;
+	struct comedi_subdevice *s = dev->subdevices + 0;	/* analog in subdevice */
+	struct rtdPrivate *devpriv = dev->private;
+	u32 overrun;
 	u16 status;
 	u16 fifoStatus;
-	struct comedi_subdevice *s = dev->subdevices + 0;	/* analog in subdevice */
 
 	if (!dev->attached)
 		return IRQ_NONE;
 
 	devpriv->intCount++;	/* DEBUG statistics */
 
-	fifoStatus = RtdFifoStatus(dev);
+	fifoStatus = readl(devpriv->las0 + LAS0_ADC);
 	/* check for FIFO full, this automatically halts the ADC! */
 	if (!(fifoStatus & FS_ADC_NOT_FULL)) {	/* 0 -> full */
 		DPRINTK("rtd520: FIFO full! fifo_status=0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);	/* should be all 0s */
@@ -1529,26 +802,26 @@ static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 	}
 #ifdef USE_DMA
 	if (devpriv->flags & DMA0_ACTIVE) {	/* Check DMA */
-		u32 istatus = RtdPlxInterruptRead(dev);
+		u32 istatus = readl(devpriv->lcfg + LCFG_ITCSR);
 
 		if (istatus & ICS_DMA0_A) {
 			if (ai_process_dma(dev, s) < 0) {
 				DPRINTK
 				    ("rtd520: comedi read buffer overflow (DMA) with %ld to go!\n",
 				     devpriv->aiCount);
-				RtdDma0Control(dev,
-					       (devpriv->dma0Control &
-						~PLX_DMA_START_BIT)
-					       | PLX_CLEAR_DMA_INTR_BIT);
+				devpriv->dma0Control &= ~PLX_DMA_START_BIT;
+				devpriv->dma0Control |= PLX_CLEAR_DMA_INTR_BIT;
+				writeb(devpriv->dma0Control,
+					devpriv->lcfg + LCFG_DMACSR0);
 				goto abortTransfer;
 			}
 
 			/*DPRINTK ("rtd520: DMA transfer: %ld to go, istatus %x\n",
 			   devpriv->aiCount, istatus); */
-			RtdDma0Control(dev,
-				       (devpriv->
-					dma0Control & ~PLX_DMA_START_BIT)
-				       | PLX_CLEAR_DMA_INTR_BIT);
+			devpriv->dma0Control &= ~PLX_DMA_START_BIT;
+			devpriv->dma0Control |= PLX_CLEAR_DMA_INTR_BIT;
+			writeb(devpriv->dma0Control,
+				devpriv->lcfg + LCFG_DMACSR0);
 			if (0 == devpriv->aiCount) {	/* counted down */
 				DPRINTK("rtd520: Samples Done (DMA).\n");
 				goto transferDone;
@@ -1561,7 +834,7 @@ static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 	/* Fall through and check for other interrupt sources */
 #endif /* USE_DMA */
 
-	status = RtdInterruptStatus(dev);
+	status = readw(devpriv->las0 + LAS0_IT);
 	/* if interrupt was not caused by our board, or handled above */
 	if (0 == status)
 		return IRQ_HANDLED;
@@ -1611,33 +884,37 @@ static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
 		DPRINTK("rtd520: unknown interrupt source!\n");
 	}
 
-	if (0xffff & RtdInterruptOverrunStatus(dev)) {	/* interrupt overrun */
+	overrun = readl(devpriv->las0 + LAS0_OVERRUN) & 0xffff;
+	if (overrun) {
 		DPRINTK
 		    ("rtd520: Interrupt overrun with %ld to go! over_status=0x%x\n",
-		     devpriv->aiCount, 0xffff & RtdInterruptOverrunStatus(dev));
+		     devpriv->aiCount, overrun);
 		goto abortTransfer;
 	}
 
 	/* clear the interrupt */
-	RtdInterruptClearMask(dev, status);
-	RtdInterruptClear(dev);
+	devpriv->intClearMask = status;
+	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	readw(devpriv->las0 + LAS0_CLEAR);
 	return IRQ_HANDLED;
 
 abortTransfer:
-	RtdAdcClearFifo(dev);	/* clears full flag */
+	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
 	s->async->events |= COMEDI_CB_ERROR;
 	devpriv->aiCount = 0;	/* stop and don't transfer any more */
 	/* fall into transferDone */
 
 transferDone:
-	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
-	RtdPacerStop(dev);	/* Stop PACER */
-	RtdAdcConversionSource(dev, 0);	/* software trigger only */
-	RtdInterruptMask(dev, 0);	/* mask out SAMPLE */
+	/* pacer stop source: SOFTWARE */
+	writel(0, devpriv->las0 + LAS0_PACER_STOP);
+	writel(0, devpriv->las0 + LAS0_PACER);	/* stop pacer */
+	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
+	devpriv->intMask = 0;
+	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
 #ifdef USE_DMA
 	if (devpriv->flags & DMA0_ACTIVE) {
-		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
-				     RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		writel(readl(devpriv->lcfg + LCFG_ITCSR) & ~ICS_DMA0_E,
+			devpriv->lcfg + LCFG_ITCSR);
 		abort_dma(dev, 0);
 		devpriv->flags &= ~DMA0_ACTIVE;
 		/* if Using DMA, then we should have read everything by now */
@@ -1649,7 +926,7 @@ transferDone:
 #endif /* USE_DMA */
 
 	if (devpriv->aiCount > 0) {	/* there shouldn't be anything left */
-		fifoStatus = RtdFifoStatus(dev);
+		fifoStatus = readl(devpriv->las0 + LAS0_ADC);
 		DPRINTK("rtd520: Finishing up. %ld remain, fifoStat=%x\n", devpriv->aiCount, (fifoStatus ^ 0x6666) & 0x7777);	/* should read all 0s */
 		ai_read_dregs(dev, s);	/* read anything left in FIFO */
 	}
@@ -1658,15 +935,16 @@ transferDone:
 	comedi_event(dev, s);
 
 	/* clear the interrupt */
-	status = RtdInterruptStatus(dev);
-	RtdInterruptClearMask(dev, status);
-	RtdInterruptClear(dev);
+	status = readw(devpriv->las0 + LAS0_IT);
+	devpriv->intClearMask = status;
+	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	readw(devpriv->las0 + LAS0_CLEAR);
 
-	fifoStatus = RtdFifoStatus(dev);	/* DEBUG */
+	fifoStatus = readl(devpriv->las0 + LAS0_ADC);
+	overrun = readl(devpriv->las0 + LAS0_OVERRUN) & 0xffff;
 	DPRINTK
 	    ("rtd520: Acquisition complete. %ld ints, intStat=%x, overStat=%x\n",
-	     devpriv->intCount, status,
-	     0xffff & RtdInterruptOverrunStatus(dev));
+	     devpriv->intCount, status, overrun);
 
 	return IRQ_HANDLED;
 }
@@ -1895,28 +1173,33 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 */
 static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int timer;
 
 	/* stop anything currently running */
-	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
-	RtdPacerStop(dev);	/* make sure PACER is stopped */
-	RtdAdcConversionSource(dev, 0);	/* software trigger only */
-	RtdInterruptMask(dev, 0);
+	/* pacer stop source: SOFTWARE */
+	writel(0, devpriv->las0 + LAS0_PACER_STOP);
+	writel(0, devpriv->las0 + LAS0_PACER);	/* stop pacer */
+	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
+	devpriv->intMask = 0;
+	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
 #ifdef USE_DMA
 	if (devpriv->flags & DMA0_ACTIVE) {	/* cancel anything running */
-		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
-				     RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		writel(readl(devpriv->lcfg + LCFG_ITCSR) & ~ICS_DMA0_E,
+			devpriv->lcfg + LCFG_ITCSR);
 		abort_dma(dev, 0);
 		devpriv->flags &= ~DMA0_ACTIVE;
-		if (RtdPlxInterruptRead(dev) & ICS_DMA0_A) {	/*clear pending int */
-			RtdDma0Control(dev, PLX_CLEAR_DMA_INTR_BIT);
+		if (readl(devpriv->lcfg + LCFG_ITCSR) & ICS_DMA0_A) {
+			devpriv->dma0Control = PLX_CLEAR_DMA_INTR_BIT;
+			writeb(devpriv->dma0Control,
+				devpriv->lcfg + LCFG_DMACSR0);
 		}
 	}
-	RtdDma0Reset(dev);	/* reset onboard state */
+	writel(0, devpriv->las0 + LAS0_DMA0_RESET);
 #endif /* USE_DMA */
-	RtdAdcClearFifo(dev);	/* clear any old data */
-	RtdInterruptOverrunClear(dev);
+	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
+	writel(0, devpriv->las0 + LAS0_OVERRUN);
 	devpriv->intCount = 0;
 
 	if (!dev->irq) {	/* we need interrupts for this */
@@ -1931,15 +1214,20 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* setup the common case and override if needed */
 	if (cmd->chanlist_len > 1) {
 		/*DPRINTK ("rtd520: Multi channel setup\n"); */
-		RtdPacerStartSource(dev, 0);	/* software triggers pacer */
-		RtdBurstStartSource(dev, 1);	/* PACER triggers burst */
-		RtdAdcConversionSource(dev, 2);	/* BURST triggers ADC */
+		/* pacer start source: SOFTWARE */
+		writel(0, devpriv->las0 + LAS0_PACER_START);
+		/* burst trigger source: PACER */
+		writel(1, devpriv->las0 + LAS0_BURST_START);
+		/* ADC conversion trigger source: BURST */
+		writel(2, devpriv->las0 + LAS0_ADC_CONVERSION);
 	} else {		/* single channel */
 		/*DPRINTK ("rtd520: single channel setup\n"); */
-		RtdPacerStartSource(dev, 0);	/* software triggers pacer */
-		RtdAdcConversionSource(dev, 1);	/* PACER triggers ADC */
+		/* pacer start source: SOFTWARE */
+		writel(0, devpriv->las0 + LAS0_PACER_START);
+		/* ADC conversion trigger source: PACER */
+		writel(1, devpriv->las0 + LAS0_ADC_CONVERSION);
 	}
-	RtdAboutCounter(dev, devpriv->fifoLen / 2 - 1);	/* 1/2 FIFO */
+	writel((devpriv->fifoLen / 2 - 1) & 0xffff, devpriv->las0 + LAS0_ACNT);
 
 	if (TRIG_TIMER == cmd->scan_begin_src) {
 		/* scan_begin_arg is in nanoseconds */
@@ -1973,7 +1261,8 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			devpriv->flags &= ~SEND_EOS;
 		} else {
 			/* interrupt for each transfer */
-			RtdAboutCounter(dev, devpriv->transCount - 1);
+			writel((devpriv->transCount - 1) & 0xffff,
+				devpriv->las0 + LAS0_ACNT);
 		}
 
 		DPRINTK
@@ -1984,8 +1273,10 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->transCount = 0;
 		devpriv->flags &= ~SEND_EOS;
 	}
-	RtdPacerClockSource(dev, 1);	/* use INTERNAL 8Mhz clock source */
-	RtdAboutStopEnable(dev, 1);	/* just interrupt, dont stop */
+	/* pacer clock source: INTERNAL 8MHz */
+	writel(1, devpriv->las0 + LAS0_PACER_SELECT);
+	/* just interrupt, don't stop */
+	writel(1, devpriv->las0 + LAS0_ACNT_STOP_ENABLE);
 
 	/* BUG??? these look like enumerated values, but they are bit fields */
 
@@ -2015,12 +1306,13 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 					TRIG_ROUND_NEAREST);
 		/* set PACER clock */
 		/*DPRINTK ("rtd520: loading %d into pacer\n", timer); */
-		RtdPacerCounter(dev, timer);
+		writel(timer & 0xffffff, devpriv->las0 + LAS0_PCLK);
 
 		break;
 
 	case TRIG_EXT:
-		RtdPacerStartSource(dev, 1);	/* EXTERNALy trigger pacer */
+		/* pacer start source: EXTERNAL */
+		writel(1, devpriv->las0 + LAS0_PACER_START);
 		break;
 
 	default:
@@ -2036,13 +1328,14 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 						TRIG_ROUND_NEAREST);
 			/* setup BURST clock */
 			/*DPRINTK ("rtd520: loading %d into burst\n", timer); */
-			RtdBurstCounter(dev, timer);
+			writel(timer & 0x3ff, devpriv->las0 + LAS0_BCLK);
 		}
 
 		break;
 
 	case TRIG_EXT:		/* external */
-		RtdBurstStartSource(dev, 2);	/* EXTERNALy trigger burst */
+		/* burst trigger source: EXTERNAL */
+		writel(2, devpriv->las0 + LAS0_BURST_START);
 		break;
 
 	default:
@@ -2053,12 +1346,14 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	/* This doesn't seem to work.  There is no way to clear an interrupt
 	   that the priority controller has queued! */
-	RtdInterruptClearMask(dev, ~0);	/* clear any existing flags */
-	RtdInterruptClear(dev);
+	devpriv->intClearMask = ~0;
+	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	readw(devpriv->las0 + LAS0_CLEAR);
 
 	/* TODO: allow multiple interrupt sources */
 	if (devpriv->transCount > 0) {	/* transfer every N samples */
-		RtdInterruptMask(dev, IRQM_ADC_ABOUT_CNT);
+		devpriv->intMask = IRQM_ADC_ABOUT_CNT;
+		writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
 		DPRINTK("rtd520: Transferring every %d\n", devpriv->transCount);
 	} else {		/* 1/2 FIFO transfers */
 #ifdef USE_DMA
@@ -2066,27 +1361,32 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 		/* point to first transfer in ring */
 		devpriv->dma0Offset = 0;
-		RtdDma0Mode(dev, DMA_MODE_BITS);
-		RtdDma0Next(dev,	/* point to first block */
-			    devpriv->dma0Chain[DMA_CHAIN_COUNT - 1].next);
-		RtdDma0Source(dev, DMAS_ADFIFO_HALF_FULL);	/* set DMA trigger source */
-
-		RtdPlxInterruptWrite(dev,	/* enable interrupt */
-				     RtdPlxInterruptRead(dev) | ICS_DMA0_E);
+		writel(DMA_MODE_BITS, devpriv->lcfg + LCFG_DMAMODE0);
+		/* point to first block */
+		writel(devpriv->dma0Chain[DMA_CHAIN_COUNT - 1].next,
+			devpriv->lcfg + LCFG_DMADPR0);
+		writel(DMAS_ADFIFO_HALF_FULL, devpriv->las0 + LAS0_DMA0_SRC);
+		writel(readl(devpriv->lcfg + LCFG_ITCSR) | ICS_DMA0_E,
+			devpriv->lcfg + LCFG_ITCSR);
 		/* Must be 2 steps.  See PLX app note about "Starting a DMA transfer" */
-		RtdDma0Control(dev, PLX_DMA_EN_BIT);	/* enable DMA (clear INTR?) */
-		RtdDma0Control(dev, PLX_DMA_EN_BIT | PLX_DMA_START_BIT);	/*start DMA */
+		devpriv->dma0Control = PLX_DMA_EN_BIT;
+		writeb(devpriv->dma0Control,
+			devpriv->lcfg + LCFG_DMACSR0);
+		devpriv->dma0Control |= PLX_DMA_START_BIT;
+		writeb(devpriv->dma0Control,
+			devpriv->lcfg + LCFG_DMACSR0);
 		DPRINTK("rtd520: Using DMA0 transfers. plxInt %x RtdInt %x\n",
-			RtdPlxInterruptRead(dev), devpriv->intMask);
+			readl(devpriv->lcfg + LCFG_ITCSR), devpriv->intMask);
 #else /* USE_DMA */
-		RtdInterruptMask(dev, IRQM_ADC_ABOUT_CNT);
+		devpriv->intMask = IRQM_ADC_ABOUT_CNT;
+		writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
 		DPRINTK("rtd520: Transferring every 1/2 FIFO\n");
 #endif /* USE_DMA */
 	}
 
 	/* BUG: start_src is ASSUMED to be TRIG_NOW */
 	/* BUG? it seems like things are running before the "start" */
-	RtdPacerStart(dev);	/* Start PACER */
+	readl(devpriv->las0 + LAS0_PACER);	/* start pacer */
 	return 0;
 }
 
@@ -2095,85 +1395,48 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 */
 static int rtd_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	struct rtdPrivate *devpriv = dev->private;
+	u32 overrun;
 	u16 status;
 
-	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
-	RtdPacerStop(dev);	/* Stop PACER */
-	RtdAdcConversionSource(dev, 0);	/* software trigger only */
-	RtdInterruptMask(dev, 0);
+	/* pacer stop source: SOFTWARE */
+	writel(0, devpriv->las0 + LAS0_PACER_STOP);
+	writel(0, devpriv->las0 + LAS0_PACER);	/* stop pacer */
+	writel(0, devpriv->las0 + LAS0_ADC_CONVERSION);
+	devpriv->intMask = 0;
+	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
 	devpriv->aiCount = 0;	/* stop and don't transfer any more */
 #ifdef USE_DMA
 	if (devpriv->flags & DMA0_ACTIVE) {
-		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
-				     RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		writel(readl(devpriv->lcfg + LCFG_ITCSR) & ~ICS_DMA0_E,
+			devpriv->lcfg + LCFG_ITCSR);
 		abort_dma(dev, 0);
 		devpriv->flags &= ~DMA0_ACTIVE;
 	}
 #endif /* USE_DMA */
-	status = RtdInterruptStatus(dev);
+	status = readw(devpriv->las0 + LAS0_IT);
+	overrun = readl(devpriv->las0 + LAS0_OVERRUN) & 0xffff;
 	DPRINTK
 	    ("rtd520: Acquisition canceled. %ld ints, intStat=%x, overStat=%x\n",
-	     devpriv->intCount, status,
-	     0xffff & RtdInterruptOverrunStatus(dev));
+	     devpriv->intCount, status, overrun);
 	return 0;
 }
 
 /*
-  Given a desired period and the clock period (both in ns),
-  return the proper counter value (divider-1).
-  Sets the original period to be the true value.
-  Note: you have to check if the value is larger than the counter range!
-*/
-static int rtd_ns_to_timer_base(unsigned int *nanosec,	/* desired period (in ns) */
-				int round_mode, int base)
-{				/* clock period (in ns) */
-	int divider;
-
-	switch (round_mode) {
-	case TRIG_ROUND_NEAREST:
-	default:
-		divider = (*nanosec + base / 2) / base;
-		break;
-	case TRIG_ROUND_DOWN:
-		divider = (*nanosec) / base;
-		break;
-	case TRIG_ROUND_UP:
-		divider = (*nanosec + base - 1) / base;
-		break;
-	}
-	if (divider < 2)
-		divider = 2;	/* min is divide by 2 */
-
-	/* Note: we don't check for max, because different timers
-	   have different ranges */
-
-	*nanosec = base * divider;
-	return divider - 1;	/* countdown is divisor+1 */
-}
-
-/*
-  Given a desired period (in ns),
-  return the proper counter value (divider-1) for the internal clock.
-  Sets the original period to be the true value.
-*/
-static int rtd_ns_to_timer(unsigned int *ns, int round_mode)
-{
-	return rtd_ns_to_timer_base(ns, round_mode, RTD_CLOCK_BASE);
-}
-
-/*
   Output one (or more) analog values to a single port as fast as possible.
 */
 static int rtd_ao_winsn(struct comedi_device *dev,
 			struct comedi_subdevice *s, struct comedi_insn *insn,
 			unsigned int *data)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 	int range = CR_RANGE(insn->chanspec);
 
 	/* Configure the output range (table index matches the range values) */
-	RtdDacRange(dev, chan, range);
+	writew(range & 7, devpriv->las0 +
+		((chan == 0) ? LAS0_DAC1_CTRL : LAS0_DAC2_CTRL));
 
 	/* Writing a list of values to an AO channel is probably not
 	 * very useful, but that's how the interface is defined. */
@@ -2197,13 +1460,14 @@ static int rtd_ao_winsn(struct comedi_device *dev,
 		     chan, range, data[i], val);
 
 		/* a typical programming sequence */
-		RtdDacFifoPut(dev, chan, val);	/* put the value in */
-		RtdDacUpdate(dev, chan);	/* trigger the conversion */
+		writew(val, devpriv->las1 +
+			((chan == 0) ? LAS1_DAC1_FIFO : LAS1_DAC2_FIFO));
+		writew(0, devpriv->las0 + ((chan == 0) ? LAS0_DAC1 : LAS0_DAC2));
 
 		devpriv->aoValue[chan] = data[i];	/* save for read back */
 
 		for (ii = 0; ii < RTD_DAC_TIMEOUT; ++ii) {
-			stat = RtdFifoStatus(dev);
+			stat = readl(devpriv->las0 + LAS0_ADC);
 			/* 1 -> not empty */
 			if (stat & ((0 == chan) ? FS_DAC1_NOT_EMPTY :
 				    FS_DAC2_NOT_EMPTY))
@@ -2228,6 +1492,7 @@ static int rtd_ao_rinsn(struct comedi_device *dev,
 			struct comedi_subdevice *s, struct comedi_insn *insn,
 			unsigned int *data)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
@@ -2252,8 +1517,7 @@ static int rtd_dio_insn_bits(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
+	struct rtdPrivate *devpriv = dev->private;
 
 	/* The insn data is a mask in data[0] and the new data
 	 * in data[1], each channel cooresponding to a bit. */
@@ -2262,15 +1526,15 @@ static int rtd_dio_insn_bits(struct comedi_device *dev,
 		s->state |= data[0] & data[1];
 
 		/* Write out the new digital output lines */
-		RtdDio0Write(dev, s->state);
+		writew(s->state & 0xff, devpriv->las0 + LAS0_DIO0);
 	}
 	/* on return, data[1] contains the value of the digital
 	 * input lines. */
-	data[1] = RtdDio0Read(dev);
+	data[1] = readw(devpriv->las0 + LAS0_DIO0) & 0xff;
 
 	/*DPRINTK("rtd520:port_0 wrote: 0x%x read: 0x%x\n", s->state, data[1]); */
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -2280,6 +1544,7 @@ static int rtd_dio_insn_config(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
+	struct rtdPrivate *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 
 	/* The input or output configuration of each digital line is
@@ -2304,9 +1569,11 @@ static int rtd_dio_insn_config(struct comedi_device *dev,
 
 	DPRINTK("rtd520: port_0_direction=0x%x (1 means out)\n", s->io_bits);
 	/* TODO support digital match interrupts and strobes */
-	RtdDioStatusWrite(dev, 0x01);	/* make Dio0Ctrl point to direction */
-	RtdDio0CtrlWrite(dev, s->io_bits);	/* set direction 1 means Out */
-	RtdDioStatusWrite(dev, 0);	/* make Dio0Ctrl clear interrupts */
+	devpriv->dioStatus = 0x01;	/* set direction */
+	writew(devpriv->dioStatus, devpriv->las0 + LAS0_DIO_STATUS);
+	writew(s->io_bits & 0xff, devpriv->las0 + LAS0_DIO0_CTRL);
+	devpriv->dioStatus = 0x00;	/* clear interrupts */
+	writew(devpriv->dioStatus, devpriv->las0 + LAS0_DIO_STATUS);
 
 	/* port1 can only be all input or all output */
 
@@ -2315,6 +1582,382 @@ static int rtd_dio_insn_config(struct comedi_device *dev,
 	return 1;
 }
 
+static struct pci_dev *rtd_find_pci(struct comedi_device *dev,
+				    struct comedi_devconfig *it)
+{
+	const struct rtdBoard *thisboard;
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for_each_pci_dev(pcidev) {
+		if (pcidev->vendor != PCI_VENDOR_ID_RTD)
+			continue;
+		if (bus || slot) {
+			if (pcidev->bus->number != bus ||
+			    PCI_SLOT(pcidev->devfn) != slot)
+				continue;
+		}
+		for (i = 0; i < ARRAY_SIZE(rtd520Boards); i++) {
+			thisboard = &rtd520Boards[i];
+			if (pcidev->device == thisboard->device_id) {
+				dev->board_ptr = thisboard;
+				return pcidev;
+			}
+		}
+	}
+	dev_warn(dev->class_dev,
+		"no supported board found! (req. bus/slot: %d/%d)\n",
+		bus, slot);
+	return NULL;
+}
+
+static int rtd_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{				/* board name and options flags */
+	const struct rtdBoard *thisboard;
+	struct rtdPrivate *devpriv;
+	struct pci_dev *pcidev;
+	struct comedi_subdevice *s;
+	int ret;
+	resource_size_t physLas1;	/* data area */
+	resource_size_t physLcfg;	/* PLX9080 */
+#ifdef USE_DMA
+	int index;
+#endif
+
+	printk(KERN_INFO "comedi%d: rtd520 attaching.\n", dev->minor);
+
+#if defined(CONFIG_COMEDI_DEBUG) && defined(USE_DMA)
+	/* You can set this a load time: modprobe comedi comedi_debug=1 */
+	if (0 == comedi_debug)	/* force DMA debug printks */
+		comedi_debug = 1;
+#endif
+
+	/*
+	 * Allocate the private structure area.  alloc_private() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_private(dev, sizeof(struct rtdPrivate)) < 0)
+		return -ENOMEM;
+	devpriv = dev->private;
+
+	pcidev = rtd_find_pci(dev, it);
+	if (!pcidev)
+		return -EIO;
+	comedi_set_hw_dev(dev, &pcidev->dev);
+	thisboard = comedi_board(dev);
+
+	dev->board_name = thisboard->name;
+
+	ret = comedi_pci_enable(pcidev, DRV_NAME);
+	if (ret < 0) {
+		printk(KERN_INFO "Failed to enable PCI device and request regions.\n");
+		return ret;
+	}
+
+	/*
+	 * Initialize base addresses
+	 */
+	/* Get the physical address from PCI config */
+	dev->iobase = pci_resource_start(pcidev, LAS0_PCIINDEX);
+	physLas1 = pci_resource_start(pcidev, LAS1_PCIINDEX);
+	physLcfg = pci_resource_start(pcidev, LCFG_PCIINDEX);
+	/* Now have the kernel map this into memory */
+	/* ASSUME page aligned */
+	devpriv->las0 = ioremap_nocache(dev->iobase, LAS0_PCISIZE);
+	devpriv->las1 = ioremap_nocache(physLas1, LAS1_PCISIZE);
+	devpriv->lcfg = ioremap_nocache(physLcfg, LCFG_PCISIZE);
+
+	if (!devpriv->las0 || !devpriv->las1 || !devpriv->lcfg)
+		return -ENOMEM;
+
+	{			/* The RTD driver does this */
+		unsigned char pci_latency;
+		u16 revision;
+		/*uint32_t epld_version; */
+
+		pci_read_config_word(pcidev, PCI_REVISION_ID,
+				     &revision);
+		DPRINTK("%s: PCI revision %d.\n", dev->board_name, revision);
+
+		pci_read_config_byte(pcidev,
+				     PCI_LATENCY_TIMER, &pci_latency);
+		if (pci_latency < 32) {
+			printk(KERN_INFO "%s: PCI latency changed from %d to %d\n",
+			       dev->board_name, pci_latency, 32);
+			pci_write_config_byte(pcidev,
+					      PCI_LATENCY_TIMER, 32);
+		} else {
+			DPRINTK("rtd520: PCI latency = %d\n", pci_latency);
+		}
+
+		/*
+		 * Undocumented EPLD version (doesn't match RTD driver results)
+		 */
+		/*DPRINTK ("rtd520: Reading epld from %p\n",
+		   devpriv->las0+0);
+		   epld_version = readl (devpriv->las0+0);
+		   if ((epld_version & 0xF0) >> 4 == 0x0F) {
+		   DPRINTK("rtd520: pre-v8 EPLD. (%x)\n", epld_version);
+		   } else {
+		   DPRINTK("rtd520: EPLD version %x.\n", epld_version >> 4);
+		   } */
+	}
+
+	/* Show board configuration */
+	printk(KERN_INFO "%s:", dev->board_name);
+
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags =
+	    SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan = thisboard->aiChans;
+	s->maxdata = (1 << thisboard->aiBits) - 1;
+	if (thisboard->aiMaxGain <= 32)
+		s->range_table = &rtd_ai_7520_range;
+	else
+		s->range_table = &rtd_ai_4520_range;
+
+	s->len_chanlist = RTD_MAX_CHANLIST;	/* devpriv->fifoLen */
+	s->insn_read = rtd_ai_rinsn;
+	s->do_cmd = rtd_ai_cmd;
+	s->do_cmdtest = rtd_ai_cmdtest;
+	s->cancel = rtd_ai_cancel;
+	/* s->poll = rtd_ai_poll; *//* not ready yet */
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 2;
+	s->maxdata = (1 << thisboard->aiBits) - 1;
+	s->range_table = &rtd_ao_range;
+	s->insn_write = rtd_ao_winsn;
+	s->insn_read = rtd_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital i/o subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	/* we only support port 0 right now.  Ignoring port 1 and user IO */
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = rtd_dio_insn_bits;
+	s->insn_config = rtd_dio_insn_config;
+
+	/* timer/counter subdevices (not currently supported) */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 3;
+	s->maxdata = 0xffff;
+
+	/* initialize board, per RTD spec */
+	/* also, initialize shadow registers */
+	writel(0, devpriv->las0 + LAS0_BOARD_RESET);
+	udelay(100);		/* needed? */
+	writel(0, devpriv->lcfg + LCFG_ITCSR);
+	devpriv->intMask = 0;
+	writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
+	devpriv->intClearMask = ~0;
+	writew(devpriv->intClearMask, devpriv->las0 + LAS0_CLEAR);
+	readw(devpriv->las0 + LAS0_CLEAR);
+	writel(0, devpriv->las0 + LAS0_OVERRUN);
+	writel(0, devpriv->las0 + LAS0_CGT_CLEAR);
+	writel(0, devpriv->las0 + LAS0_ADC_FIFO_CLEAR);
+	writel(0, devpriv->las0 + LAS0_DAC1_RESET);
+	writel(0, devpriv->las0 + LAS0_DAC2_RESET);
+	/* clear digital IO fifo */
+	devpriv->dioStatus = 0;
+	writew(devpriv->dioStatus, devpriv->las0 + LAS0_DIO_STATUS);
+	devpriv->utcCtrl[0] = (0 << 6) | 0x30;
+	devpriv->utcCtrl[1] = (1 << 6) | 0x30;
+	devpriv->utcCtrl[2] = (2 << 6) | 0x30;
+	devpriv->utcCtrl[3] = (3 << 6) | 0x00;
+	writeb(devpriv->utcCtrl[0], devpriv->las0 + LAS0_UTC_CTRL);
+	writeb(devpriv->utcCtrl[1], devpriv->las0 + LAS0_UTC_CTRL);
+	writeb(devpriv->utcCtrl[2], devpriv->las0 + LAS0_UTC_CTRL);
+	writeb(devpriv->utcCtrl[3], devpriv->las0 + LAS0_UTC_CTRL);
+	/* TODO: set user out source ??? */
+
+	/* check if our interrupt is available and get it */
+	ret = request_irq(pcidev->irq, rtd_interrupt,
+			  IRQF_SHARED, DRV_NAME, dev);
+
+	if (ret < 0) {
+		printk("Could not get interrupt! (%u)\n",
+		       pcidev->irq);
+		return ret;
+	}
+	dev->irq = pcidev->irq;
+	printk(KERN_INFO "( irq=%u )", dev->irq);
+
+	ret = rtd520_probe_fifo_depth(dev);
+	if (ret < 0)
+		return ret;
+
+	devpriv->fifoLen = ret;
+	printk("( fifoLen=%d )", devpriv->fifoLen);
+
+#ifdef USE_DMA
+	if (dev->irq > 0) {
+		printk("( DMA buff=%d )\n", DMA_CHAIN_COUNT);
+		/*
+		 * The PLX9080 has 2 DMA controllers, but there could be
+		 * 4 sources: ADC, digital, DAC1, and DAC2.  Since only the
+		 * ADC supports cmd mode right now, this isn't an issue (yet)
+		 */
+		devpriv->dma0Offset = 0;
+
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			devpriv->dma0Buff[index] =
+			    pci_alloc_consistent(pcidev,
+						 sizeof(u16) *
+						 devpriv->fifoLen / 2,
+						 &devpriv->
+						 dma0BuffPhysAddr[index]);
+			if (devpriv->dma0Buff[index] == NULL) {
+				ret = -ENOMEM;
+				goto rtd_attach_die_error;
+			}
+			/*DPRINTK ("buff[%d] @ %p virtual, %x PCI\n",
+			   index,
+			   devpriv->dma0Buff[index],
+			   devpriv->dma0BuffPhysAddr[index]); */
+		}
+
+		/*
+		 * setup DMA descriptor ring (use cpu_to_le32 for byte
+		 * ordering?)
+		 */
+		devpriv->dma0Chain =
+		    pci_alloc_consistent(pcidev,
+					 sizeof(struct plx_dma_desc) *
+					 DMA_CHAIN_COUNT,
+					 &devpriv->dma0ChainPhysAddr);
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			devpriv->dma0Chain[index].pci_start_addr =
+			    devpriv->dma0BuffPhysAddr[index];
+			devpriv->dma0Chain[index].local_start_addr =
+			    DMALADDR_ADC;
+			devpriv->dma0Chain[index].transfer_size =
+			    sizeof(u16) * devpriv->fifoLen / 2;
+			devpriv->dma0Chain[index].next =
+			    (devpriv->dma0ChainPhysAddr + ((index +
+							    1) %
+							   (DMA_CHAIN_COUNT))
+			     * sizeof(devpriv->dma0Chain[0]))
+			    | DMA_TRANSFER_BITS;
+			/*DPRINTK ("ring[%d] @%lx PCI: %x, local: %x, N: 0x%x, next: %x\n",
+			   index,
+			   ((long)devpriv->dma0ChainPhysAddr
+			   + (index * sizeof(devpriv->dma0Chain[0]))),
+			   devpriv->dma0Chain[index].pci_start_addr,
+			   devpriv->dma0Chain[index].local_start_addr,
+			   devpriv->dma0Chain[index].transfer_size,
+			   devpriv->dma0Chain[index].next); */
+		}
+
+		if (devpriv->dma0Chain == NULL) {
+			ret = -ENOMEM;
+			goto rtd_attach_die_error;
+		}
+
+		writel(DMA_MODE_BITS, devpriv->lcfg + LCFG_DMAMODE0);
+		/* set DMA trigger source */
+		writel(DMAS_ADFIFO_HALF_FULL, devpriv->las0 + LAS0_DMA0_SRC);
+	} else {
+		printk(KERN_INFO "( no IRQ->no DMA )");
+	}
+#endif /* USE_DMA */
+
+	if (dev->irq)
+		writel(ICS_PIE | ICS_PLIE, devpriv->lcfg + LCFG_ITCSR);
+
+	printk("\ncomedi%d: rtd520 driver attached.\n", dev->minor);
+
+	return 1;
+}
+
+static void rtd_detach(struct comedi_device *dev)
+{
+	struct rtdPrivate *devpriv = dev->private;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+#ifdef USE_DMA
+	int index;
+#endif
+
+	if (devpriv) {
+		/* Shut down any board ops by resetting it */
+#ifdef USE_DMA
+		if (devpriv->lcfg) {
+			devpriv->dma0Control = 0;
+			devpriv->dma1Control = 0;
+			writeb(devpriv->dma0Control,
+				devpriv->lcfg + LCFG_DMACSR0);
+			writeb(devpriv->dma1Control,
+				devpriv->lcfg + LCFG_DMACSR1);
+			writel(ICS_PIE | ICS_PLIE, devpriv->lcfg + LCFG_ITCSR);
+		}
+#endif /* USE_DMA */
+		if (devpriv->las0) {
+			writel(0, devpriv->las0 + LAS0_BOARD_RESET);
+			devpriv->intMask = 0;
+			writew(devpriv->intMask, devpriv->las0 + LAS0_IT);
+			devpriv->intClearMask = ~0;
+			writew(devpriv->intClearMask,
+				devpriv->las0 + LAS0_CLEAR);
+			readw(devpriv->las0 + LAS0_CLEAR);
+		}
+#ifdef USE_DMA
+		/* release DMA */
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			if (NULL != devpriv->dma0Buff[index]) {
+				pci_free_consistent(pcidev,
+						    sizeof(u16) *
+						    devpriv->fifoLen / 2,
+						    devpriv->dma0Buff[index],
+						    devpriv->
+						    dma0BuffPhysAddr[index]);
+				devpriv->dma0Buff[index] = NULL;
+			}
+		}
+		if (NULL != devpriv->dma0Chain) {
+			pci_free_consistent(pcidev,
+					    sizeof(struct plx_dma_desc) *
+					    DMA_CHAIN_COUNT, devpriv->dma0Chain,
+					    devpriv->dma0ChainPhysAddr);
+			devpriv->dma0Chain = NULL;
+		}
+#endif /* USE_DMA */
+		if (dev->irq) {
+			writel(readl(devpriv->lcfg + LCFG_ITCSR) &
+				~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E),
+				devpriv->lcfg + LCFG_ITCSR);
+			free_irq(dev->irq, dev);
+		}
+		if (devpriv->las0)
+			iounmap(devpriv->las0);
+		if (devpriv->las1)
+			iounmap(devpriv->las1);
+		if (devpriv->lcfg)
+			iounmap(devpriv->lcfg);
+	}
+	if (pcidev) {
+		if (dev->iobase)
+			comedi_pci_disable(pcidev);
+		pci_dev_put(pcidev);
+	}
+}
+
 static struct comedi_driver rtd520_driver = {
 	.driver_name	= "rtd520",
 	.module		= THIS_MODULE,
diff --git a/drivers/staging/comedi/drivers/rti800.c b/drivers/staging/comedi/drivers/rti800.c
index f0eb52a7788..f7fa940d978 100644
--- a/drivers/staging/comedi/drivers/rti800.c
+++ b/drivers/staging/comedi/drivers/rti800.c
@@ -138,8 +138,6 @@ struct rti800_board {
 	int has_ao;
 };
 
-#define this_board ((const struct rti800_board *)dev->board_ptr)
-
 static irqreturn_t rti800_interrupt(int irq, void *dev);
 
 struct rti800_private {
@@ -265,19 +263,14 @@ static int rti800_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
 	data[1] = inb(dev->iobase + RTI800_DI);
-	return 2;
+	return insn->n;
 }
 
 static int rti800_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
@@ -287,7 +280,7 @@ static int rti800_do_insn_bits(struct comedi_device *dev,
 
 	data[1] = s->state;
 
-	return 2;
+	return insn->n;
 }
 
 /*
@@ -309,6 +302,7 @@ static int rti800_do_insn_bits(struct comedi_device *dev,
 
 static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct rti800_board *board = comedi_board(dev);
 	unsigned int irq;
 	unsigned long iobase;
 	int ret;
@@ -347,10 +341,10 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		printk(KERN_INFO "( no irq )\n");
 	}
 
-	dev->board_name = this_board->name;
+	dev->board_name = board->name;
 
-	ret = alloc_subdevices(dev, 4);
-	if (ret < 0)
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
 		return ret;
 
 	ret = alloc_private(dev, sizeof(struct rti800_private));
@@ -386,7 +380,7 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 
 	s++;
-	if (this_board->has_ao) {
+	if (board->has_ao) {
 		/* ao subdevice (only on rti815) */
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE;
diff --git a/drivers/staging/comedi/drivers/rti802.c b/drivers/staging/comedi/drivers/rti802.c
index 09da5c21858..fc16508181d 100644
--- a/drivers/staging/comedi/drivers/rti802.c
+++ b/drivers/staging/comedi/drivers/rti802.c
@@ -92,6 +92,7 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	struct comedi_subdevice *s;
 	int i;
 	unsigned long iobase;
+	int ret;
 
 	iobase = it->options[0];
 	printk(KERN_INFO "comedi%d: rti802: 0x%04lx ", dev->minor, iobase);
@@ -103,10 +104,12 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	dev->board_name = "rti802";
 
-	if (alloc_subdevices(dev, 1) < 0
-	    || alloc_private(dev, sizeof(struct rti802_private))) {
+	if (alloc_private(dev, sizeof(struct rti802_private)))
 		return -ENOMEM;
-	}
+
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices;
 	/* ao subdevice */
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 7a56434eb22..737a194dfce 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -200,26 +200,13 @@ static const struct s526_board s526_boards[] = {
 #define ADDR_REG(reg) (dev->iobase + (reg))
 #define ADDR_CHAN_REG(reg, chan) (dev->iobase + (reg) + (chan) * 8)
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct s526_board *)dev->board_ptr)
-
 /* this structure is for data unique to this hardware driver.  If
    several hardware drivers keep similar information in this structure,
    feel free to suggest moving the variable to the struct comedi_device
    struct.
 */
 struct s526_private {
-
-	int data;
-
-	/* would be useful for a PCI device */
-	struct pci_dev *pci_dev;
-
-	/* Used for AO readback */
 	unsigned int ao_readback[2];
-
 	struct s526GPCTConfig s526_gpct_config[4];
 	unsigned short s526_ai_config;
 };
@@ -683,9 +670,6 @@ static int s526_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	/* The insn data is a mask in data[0] and the new data
 	 * in data[1], each channel cooresponding to a bit. */
 	if (data[0]) {
@@ -702,7 +686,7 @@ static int s526_dio_insn_bits(struct comedi_device *dev,
 	 * it was a purely digital output subdevice */
 	/* data[1]=s->state & 0xFF; */
 
-	return 2;
+	return insn->n;
 }
 
 static int s526_dio_insn_config(struct comedi_device *dev,
@@ -744,9 +728,11 @@ static int s526_dio_insn_config(struct comedi_device *dev,
 
 static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct s526_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
 	int iobase;
 	int i, n;
+	int ret;
 /* short value; */
 /* int subdev_channel = 0; */
 	union cmReg cmReg;
@@ -754,7 +740,7 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	printk(KERN_INFO "comedi%d: s526: ", dev->minor);
 
 	iobase = it->options[0];
-	if (!iobase || !request_region(iobase, S526_IOSIZE, thisboard->name)) {
+	if (!iobase || !request_region(iobase, S526_IOSIZE, board->name)) {
 		comedi_error(dev, "I/O port conflict");
 		return -EIO;
 	}
@@ -769,13 +755,7 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	}
 	***/
 
-/*
- * Initialize dev->board_name.  Note that we can use the "thisboard"
- * macro now, since we just initialized it in the last line.
- */
-	dev->board_ptr = &s526_boards[0];
-
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 /*
  * Allocate the private structure area.  alloc_private() is a
@@ -784,20 +764,16 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (alloc_private(dev, sizeof(struct s526_private)) < 0)
 		return -ENOMEM;
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	dev->n_subdevices = 4;
-	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* GENERAL-PURPOSE COUNTER/TIME (GPCT) */
 	s->type = COMEDI_SUBD_COUNTER;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_LSAMPL;
 	/* KG: What does SDF_LSAMPL (see multiq3.c) mean? */
-	s->n_chan = thisboard->gpct_chans;
+	s->n_chan = board->gpct_chans;
 	s->maxdata = 0x00ffffff;	/* 24 bit counter */
 	s->insn_read = s526_gpct_rinsn;
 	s->insn_config = s526_gpct_insn_config;
@@ -838,7 +814,7 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	s = dev->subdevices + 3;
 	/* digital i/o subdevice */
-	if (thisboard->have_dio) {
+	if (board->have_dio) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
 		s->n_chan = 8;
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 7beb8f6853a..f90578e5e72 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -74,8 +74,6 @@ INSN_CONFIG instructions:
 
 #include "../comedidev.h"
 
-#include "comedi_pci.h"
-
 #include "comedi_fc.h"
 #include "s626.h"
 
@@ -120,7 +118,7 @@ static const struct s626_board s626_boards[] = {
 
 struct s626_private {
 	struct pci_dev *pdev;
-	void *base_addr;
+	void __iomem *base_addr;
 	int got_regions;
 	short allocatedBuf;
 	uint8_t ai_cmd_running;	/*  ai_cmd is running */
@@ -209,71 +207,6 @@ static struct dio_private *dio_private_word[]={
 #define devpriv ((struct s626_private *)dev->private)
 #define diopriv ((struct dio_private *)s->private)
 
-/* ioctl routines */
-static int s626_ai_insn_config(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-/* static int s626_ai_rinsn(struct comedi_device *dev,struct comedi_subdevice *s,struct comedi_insn *insn,unsigned int *data); */
-static int s626_ai_insn_read(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data);
-static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s);
-static int s626_ai_cmdtest(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_cmd *cmd);
-static int s626_ai_cancel(struct comedi_device *dev,
-			  struct comedi_subdevice *s);
-static int s626_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int s626_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data);
-static int s626_dio_insn_bits(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-static int s626_dio_insn_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int s626_dio_set_irq(struct comedi_device *dev, unsigned int chan);
-static int s626_dio_reset_irq(struct comedi_device *dev, unsigned int gruop,
-			      unsigned int mask);
-static int s626_dio_clear_irq(struct comedi_device *dev);
-static int s626_enc_insn_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data);
-static int s626_enc_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data);
-static int s626_enc_insn_write(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data);
-static int s626_ns_to_timer(int *nanosec, int round_mode);
-static int s626_ai_load_polllist(uint8_t *ppl, struct comedi_cmd *cmd);
-static int s626_ai_inttrig(struct comedi_device *dev,
-			   struct comedi_subdevice *s, unsigned int trignum);
-static irqreturn_t s626_irq_handler(int irq, void *d);
-static unsigned int s626_ai_reg_to_uint(int data);
-/* static unsigned int s626_uint_to_reg(struct comedi_subdevice *s, int data); */
-
-/* end ioctl routines */
-
-/* internal routines */
-static void s626_dio_init(struct comedi_device *dev);
-static void ResetADC(struct comedi_device *dev, uint8_t * ppl);
-static void LoadTrimDACs(struct comedi_device *dev);
-static void WriteTrimDAC(struct comedi_device *dev, uint8_t LogicalChan,
-			 uint8_t DacData);
-static uint8_t I2Cread(struct comedi_device *dev, uint8_t addr);
-static uint32_t I2Chandshake(struct comedi_device *dev, uint32_t val);
-static void SetDAC(struct comedi_device *dev, uint16_t chan, short dacdata);
-static void SendDAC(struct comedi_device *dev, uint32_t val);
-static void WriteMISC2(struct comedi_device *dev, uint16_t NewImage);
-static void DEBItransfer(struct comedi_device *dev);
-static uint16_t DEBIread(struct comedi_device *dev, uint16_t addr);
-static void DEBIwrite(struct comedi_device *dev, uint16_t addr, uint16_t wdata);
-static void DEBIreplace(struct comedi_device *dev, uint16_t addr, uint16_t mask,
-			uint16_t wdata);
-static void CloseDMAB(struct comedi_device *dev, struct bufferDMA *pdma,
-		      size_t bsize);
-
 /*  COUNTER OBJECT ------------------------------------------------ */
 struct enc_private {
 	/*  Pointers to functions that differ for A and B counters: */
@@ -297,56 +230,6 @@ struct enc_private {
 
 #define encpriv ((struct enc_private *)(dev->subdevices+5)->private)
 
-/* counters routines */
-static void s626_timer_load(struct comedi_device *dev, struct enc_private *k,
-			    int tick);
-static uint32_t ReadLatch(struct comedi_device *dev, struct enc_private *k);
-static void ResetCapFlags_A(struct comedi_device *dev, struct enc_private *k);
-static void ResetCapFlags_B(struct comedi_device *dev, struct enc_private *k);
-static uint16_t GetMode_A(struct comedi_device *dev, struct enc_private *k);
-static uint16_t GetMode_B(struct comedi_device *dev, struct enc_private *k);
-static void SetMode_A(struct comedi_device *dev, struct enc_private *k,
-		      uint16_t Setup, uint16_t DisableIntSrc);
-static void SetMode_B(struct comedi_device *dev, struct enc_private *k,
-		      uint16_t Setup, uint16_t DisableIntSrc);
-static void SetEnable_A(struct comedi_device *dev, struct enc_private *k,
-			uint16_t enab);
-static void SetEnable_B(struct comedi_device *dev, struct enc_private *k,
-			uint16_t enab);
-static uint16_t GetEnable_A(struct comedi_device *dev, struct enc_private *k);
-static uint16_t GetEnable_B(struct comedi_device *dev, struct enc_private *k);
-static void SetLatchSource(struct comedi_device *dev, struct enc_private *k,
-			   uint16_t value);
-/* static uint16_t GetLatchSource(struct comedi_device *dev, struct enc_private *k ); */
-static void SetLoadTrig_A(struct comedi_device *dev, struct enc_private *k,
-			  uint16_t Trig);
-static void SetLoadTrig_B(struct comedi_device *dev, struct enc_private *k,
-			  uint16_t Trig);
-static uint16_t GetLoadTrig_A(struct comedi_device *dev, struct enc_private *k);
-static uint16_t GetLoadTrig_B(struct comedi_device *dev, struct enc_private *k);
-static void SetIntSrc_B(struct comedi_device *dev, struct enc_private *k,
-			uint16_t IntSource);
-static void SetIntSrc_A(struct comedi_device *dev, struct enc_private *k,
-			uint16_t IntSource);
-static uint16_t GetIntSrc_A(struct comedi_device *dev, struct enc_private *k);
-static uint16_t GetIntSrc_B(struct comedi_device *dev, struct enc_private *k);
-/* static void SetClkMult(struct comedi_device *dev, struct enc_private *k, uint16_t value ) ; */
-/* static uint16_t GetClkMult(struct comedi_device *dev, struct enc_private *k ) ; */
-/* static void SetIndexPol(struct comedi_device *dev, struct enc_private *k, uint16_t value ); */
-/* static uint16_t GetClkPol(struct comedi_device *dev, struct enc_private *k ) ; */
-/* static void SetIndexSrc( struct comedi_device *dev,struct enc_private *k, uint16_t value );  */
-/* static uint16_t GetClkSrc( struct comedi_device *dev,struct enc_private *k );  */
-/* static void SetIndexSrc( struct comedi_device *dev,struct enc_private *k, uint16_t value );  */
-/* static uint16_t GetIndexSrc( struct comedi_device *dev,struct enc_private *k );  */
-static void PulseIndex_A(struct comedi_device *dev, struct enc_private *k);
-static void PulseIndex_B(struct comedi_device *dev, struct enc_private *k);
-static void Preload(struct comedi_device *dev, struct enc_private *k,
-		    uint32_t value);
-static void CountersInit(struct comedi_device *dev);
-/* end internal routines */
-
-/*  Counter objects constructor. */
-
 /*  Counter overflow/index event flag masks for RDMISC2. */
 #define INDXMASK(C)		(1 << (((C) > 2) ? ((C) * 2 - 1) : ((C) * 2 +  4)))
 #define OVERMASK(C)		(1 << (((C) > 2) ? ((C) * 2 + 5) : ((C) * 2 + 10)))
@@ -355,106 +238,6 @@ static void CountersInit(struct comedi_device *dev);
 /*  Translation table to map IntSrc into equivalent RDMISC2 event flag  bits. */
 /* static const uint16_t EventBits[][4] = { EVBITS(0), EVBITS(1), EVBITS(2), EVBITS(3), EVBITS(4), EVBITS(5) }; */
 
-/* struct enc_private; */
-static struct enc_private enc_private_data[] = {
-	{
-	 .GetEnable = GetEnable_A,
-	 .GetIntSrc = GetIntSrc_A,
-	 .GetLoadTrig = GetLoadTrig_A,
-	 .GetMode = GetMode_A,
-	 .PulseIndex = PulseIndex_A,
-	 .SetEnable = SetEnable_A,
-	 .SetIntSrc = SetIntSrc_A,
-	 .SetLoadTrig = SetLoadTrig_A,
-	 .SetMode = SetMode_A,
-	 .ResetCapFlags = ResetCapFlags_A,
-	 .MyCRA = LP_CR0A,
-	 .MyCRB = LP_CR0B,
-	 .MyLatchLsw = LP_CNTR0ALSW,
-	 .MyEventBits = EVBITS(0),
-	 },
-	{
-	 .GetEnable = GetEnable_A,
-	 .GetIntSrc = GetIntSrc_A,
-	 .GetLoadTrig = GetLoadTrig_A,
-	 .GetMode = GetMode_A,
-	 .PulseIndex = PulseIndex_A,
-	 .SetEnable = SetEnable_A,
-	 .SetIntSrc = SetIntSrc_A,
-	 .SetLoadTrig = SetLoadTrig_A,
-	 .SetMode = SetMode_A,
-	 .ResetCapFlags = ResetCapFlags_A,
-	 .MyCRA = LP_CR1A,
-	 .MyCRB = LP_CR1B,
-	 .MyLatchLsw = LP_CNTR1ALSW,
-	 .MyEventBits = EVBITS(1),
-	 },
-	{
-	 .GetEnable = GetEnable_A,
-	 .GetIntSrc = GetIntSrc_A,
-	 .GetLoadTrig = GetLoadTrig_A,
-	 .GetMode = GetMode_A,
-	 .PulseIndex = PulseIndex_A,
-	 .SetEnable = SetEnable_A,
-	 .SetIntSrc = SetIntSrc_A,
-	 .SetLoadTrig = SetLoadTrig_A,
-	 .SetMode = SetMode_A,
-	 .ResetCapFlags = ResetCapFlags_A,
-	 .MyCRA = LP_CR2A,
-	 .MyCRB = LP_CR2B,
-	 .MyLatchLsw = LP_CNTR2ALSW,
-	 .MyEventBits = EVBITS(2),
-	 },
-	{
-	 .GetEnable = GetEnable_B,
-	 .GetIntSrc = GetIntSrc_B,
-	 .GetLoadTrig = GetLoadTrig_B,
-	 .GetMode = GetMode_B,
-	 .PulseIndex = PulseIndex_B,
-	 .SetEnable = SetEnable_B,
-	 .SetIntSrc = SetIntSrc_B,
-	 .SetLoadTrig = SetLoadTrig_B,
-	 .SetMode = SetMode_B,
-	 .ResetCapFlags = ResetCapFlags_B,
-	 .MyCRA = LP_CR0A,
-	 .MyCRB = LP_CR0B,
-	 .MyLatchLsw = LP_CNTR0BLSW,
-	 .MyEventBits = EVBITS(3),
-	 },
-	{
-	 .GetEnable = GetEnable_B,
-	 .GetIntSrc = GetIntSrc_B,
-	 .GetLoadTrig = GetLoadTrig_B,
-	 .GetMode = GetMode_B,
-	 .PulseIndex = PulseIndex_B,
-	 .SetEnable = SetEnable_B,
-	 .SetIntSrc = SetIntSrc_B,
-	 .SetLoadTrig = SetLoadTrig_B,
-	 .SetMode = SetMode_B,
-	 .ResetCapFlags = ResetCapFlags_B,
-	 .MyCRA = LP_CR1A,
-	 .MyCRB = LP_CR1B,
-	 .MyLatchLsw = LP_CNTR1BLSW,
-	 .MyEventBits = EVBITS(4),
-	 },
-	{
-	 .GetEnable = GetEnable_B,
-	 .GetIntSrc = GetIntSrc_B,
-	 .GetLoadTrig = GetLoadTrig_B,
-	 .GetMode = GetMode_B,
-	 .PulseIndex = PulseIndex_B,
-	 .SetEnable = SetEnable_B,
-	 .SetIntSrc = SetIntSrc_B,
-	 .SetLoadTrig = SetLoadTrig_B,
-	 .SetMode = SetMode_B,
-	 .ResetCapFlags = ResetCapFlags_B,
-	 .MyCRA = LP_CR2A,
-	 .MyCRB = LP_CR2B,
-	 .MyLatchLsw = LP_CNTR2BLSW,
-	 .MyEventBits = EVBITS(5),
-	 },
-};
-
 /*  enab/disable a function or test status bit(s) that are accessed */
 /*  through Main Control Registers 1 or 2. */
 #define MC_ENABLE(REGADRS, CTRLWORD)	writel(((uint32_t)(CTRLWORD) << 16) | (uint32_t)(CTRLWORD), devpriv->base_addr+(REGADRS))
@@ -488,498 +271,509 @@ static const struct comedi_lrange s626_range_table = { 2, {
 							   }
 };
 
-static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+/*  Execute a DEBI transfer.  This must be called from within a */
+/*  critical section. */
+static void DEBItransfer(struct comedi_device *dev)
 {
-/*   uint8_t	PollList; */
-/*   uint16_t	AdcData; */
-/*   uint16_t	StartVal; */
-/*   uint16_t	index; */
-/*   unsigned int data[16]; */
-	int result;
-	int i;
-	int ret;
-	resource_size_t resourceStart;
-	dma_addr_t appdma;
-	struct comedi_subdevice *s;
-	struct pci_dev *pdev = NULL;
+	/*  Initiate upload of shadow RAM to DEBI control register. */
+	MC_ENABLE(P_MC2, MC2_UPLD_DEBI);
 
-	if (alloc_private(dev, sizeof(struct s626_private)) < 0)
-		return -ENOMEM;
+	/*  Wait for completion of upload from shadow RAM to DEBI control */
+	/*  register. */
+	while (!MC_TEST(P_MC2, MC2_UPLD_DEBI))
+		;
 
-	for (i = 0; i < ARRAY_SIZE(s626_boards) && !pdev; i++) {
-		do {
-			pdev = pci_get_subsys(s626_boards[i].vendor_id,
-					      s626_boards[i].device_id,
-					      s626_boards[i].subvendor_id,
-					      s626_boards[i].subdevice_id,
-					      pdev);
+	/*  Wait until DEBI transfer is done. */
+	while (RR7146(P_PSR) & PSR_DEBI_S)
+		;
+}
 
-			if ((it->options[0] || it->options[1]) && pdev) {
-				/* matches requested bus/slot */
-				if (pdev->bus->number == it->options[0] &&
-				    PCI_SLOT(pdev->devfn) == it->options[1])
-					break;
-			} else
-				break;
-		} while (1);
-	}
-	devpriv->pdev = pdev;
+/*  Initialize the DEBI interface for all transfers. */
 
-	if (pdev == NULL) {
-		printk(KERN_ERR "s626_attach: Board not present!!!\n");
-		return -ENODEV;
-	}
+static uint16_t DEBIread(struct comedi_device *dev, uint16_t addr)
+{
+	uint16_t retval;
 
-	result = comedi_pci_enable(pdev, "s626");
-	if (result < 0) {
-		printk(KERN_ERR "s626_attach: comedi_pci_enable fails\n");
-		return -ENODEV;
-	}
-	devpriv->got_regions = 1;
+	/*  Set up DEBI control register value in shadow RAM. */
+	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
 
-	resourceStart = pci_resource_start(devpriv->pdev, 0);
+	/*  Execute the DEBI transfer. */
+	DEBItransfer(dev);
 
-	devpriv->base_addr = ioremap(resourceStart, SIZEOF_ADDRESS_SPACE);
-	if (devpriv->base_addr == NULL) {
-		printk(KERN_ERR "s626_attach: IOREMAP failed\n");
-		return -ENODEV;
-	}
+	/*  Fetch target register value. */
+	retval = (uint16_t) RR7146(P_DEBIAD);
 
-	if (devpriv->base_addr) {
-		/* disable master interrupt */
-		writel(0, devpriv->base_addr + P_IER);
+	/*  Return register value. */
+	return retval;
+}
 
-		/* soft reset */
-		writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+/*  Write a value to a gate array register. */
+static void DEBIwrite(struct comedi_device *dev, uint16_t addr, uint16_t wdata)
+{
 
-		/* DMA FIXME DMA// */
-		DEBUG("s626_attach: DMA ALLOCATION\n");
+	/*  Set up DEBI control register value in shadow RAM. */
+	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
+	WR7146(P_DEBIAD, wdata);
 
-		/* adc buffer allocation */
-		devpriv->allocatedBuf = 0;
+	/*  Execute the DEBI transfer. */
+	DEBItransfer(dev);
+}
 
-		devpriv->ANABuf.LogicalBase =
-		    pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE, &appdma);
+/* Replace the specified bits in a gate array register.  Imports: mask
+ * specifies bits that are to be preserved, wdata is new value to be
+ * or'd with the masked original.
+ */
+static void DEBIreplace(struct comedi_device *dev, uint16_t addr, uint16_t mask,
+			uint16_t wdata)
+{
 
-		if (devpriv->ANABuf.LogicalBase == NULL) {
-			printk(KERN_ERR "s626_attach: DMA Memory mapping error\n");
-			return -ENOMEM;
-		}
+	/*  Copy target gate array register into P_DEBIAD register. */
+	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
+	/* Set up DEBI control reg value in shadow RAM. */
+	DEBItransfer(dev);	/*  Execute the DEBI Read transfer. */
 
-		devpriv->ANABuf.PhysicalBase = appdma;
+	/*  Write back the modified image. */
+	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
+	/* Set up DEBI control reg value in shadow  RAM. */
 
-		DEBUG
-		    ("s626_attach: AllocDMAB ADC Logical=%p, bsize=%d, Physical=0x%x\n",
-		     devpriv->ANABuf.LogicalBase, DMABUF_SIZE,
-		     (uint32_t) devpriv->ANABuf.PhysicalBase);
+	WR7146(P_DEBIAD, wdata | ((uint16_t) RR7146(P_DEBIAD) & mask));
+	/* Modify the register image. */
+	DEBItransfer(dev);	/*  Execute the DEBI Write transfer. */
+}
 
-		devpriv->allocatedBuf++;
+/* **************  EEPROM ACCESS FUNCTIONS  ************** */
 
-		devpriv->RPSBuf.LogicalBase =
-		    pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE, &appdma);
+static uint32_t I2Chandshake(struct comedi_device *dev, uint32_t val)
+{
+	/*  Write I2C command to I2C Transfer Control shadow register. */
+	WR7146(P_I2CCTRL, val);
 
-		if (devpriv->RPSBuf.LogicalBase == NULL) {
-			printk(KERN_ERR "s626_attach: DMA Memory mapping error\n");
-			return -ENOMEM;
-		}
+	/*  Upload I2C shadow registers into working registers and wait for */
+	/*  upload confirmation. */
 
-		devpriv->RPSBuf.PhysicalBase = appdma;
+	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
+	while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
+		;
 
-		DEBUG
-		    ("s626_attach: AllocDMAB RPS Logical=%p, bsize=%d, Physical=0x%x\n",
-		     devpriv->RPSBuf.LogicalBase, DMABUF_SIZE,
-		     (uint32_t) devpriv->RPSBuf.PhysicalBase);
+	/*  Wait until I2C bus transfer is finished or an error occurs. */
+	while ((RR7146(P_I2CCTRL) & (I2C_BUSY | I2C_ERR)) == I2C_BUSY)
+		;
 
-		devpriv->allocatedBuf++;
+	/*  Return non-zero if I2C error occurred. */
+	return RR7146(P_I2CCTRL) & I2C_ERR;
 
-	}
+}
 
-	dev->board_ptr = s626_boards;
-	dev->board_name = thisboard->name;
+/*  Read uint8_t from EEPROM. */
+static uint8_t I2Cread(struct comedi_device *dev, uint8_t addr)
+{
+	uint8_t rtnval;
 
-	if (alloc_subdevices(dev, 6) < 0)
-		return -ENOMEM;
+	/*  Send EEPROM target address. */
+	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CW)
+			 /* Byte2 = I2C command: write to I2C EEPROM  device. */
+			 | I2C_B1(I2C_ATTRSTOP, addr)
+			 /* Byte1 = EEPROM internal target address. */
+			 | I2C_B0(I2C_ATTRNOP, 0))) {	/*  Byte0 = Not sent. */
+		/*  Abort function and declare error if handshake failed. */
+		return 0;
+	}
+	/*  Execute EEPROM read. */
+	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CR)
 
-	dev->iobase = (unsigned long)devpriv->base_addr;
-	dev->irq = devpriv->pdev->irq;
+			 /*  Byte2 = I2C */
+			 /*  command: read */
+			 /*  from I2C EEPROM */
+			 /*  device. */
+			 |I2C_B1(I2C_ATTRSTOP, 0)
 
-	/* set up interrupt handler */
-	if (dev->irq == 0) {
-		printk(KERN_ERR " unknown irq (bad)\n");
-	} else {
-		ret = request_irq(dev->irq, s626_irq_handler, IRQF_SHARED,
-				  "s626", dev);
+			 /*  Byte1 receives */
+			 /*  uint8_t from */
+			 /*  EEPROM. */
+			 |I2C_B0(I2C_ATTRNOP, 0))) {	/*  Byte0 = Not  sent. */
 
-		if (ret < 0) {
-			printk(KERN_ERR " irq not available\n");
-			dev->irq = 0;
-		}
+		/*  Abort function and declare error if handshake failed. */
+		return 0;
 	}
+	/*  Return copy of EEPROM value. */
+	rtnval = (uint8_t) (RR7146(P_I2CCTRL) >> 16);
+	return rtnval;
+}
 
-	DEBUG("s626_attach: -- it opts  %d,%d --\n",
-	      it->options[0], it->options[1]);
+/* ***********  DAC FUNCTIONS *********** */
 
-	s = dev->subdevices + 0;
-	/* analog input subdevice */
-	dev->read_subdev = s;
-	/* we support single-ended (ground) and differential */
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (0xffff >> 2);
-	s->range_table = &s626_range_table;
-	s->len_chanlist = thisboard->ai_chans;	/* This is the maximum chanlist
-						   length that the board can
-						   handle */
-	s->insn_config = s626_ai_insn_config;
-	s->insn_read = s626_ai_insn_read;
-	s->do_cmd = s626_ai_cmd;
-	s->do_cmdtest = s626_ai_cmdtest;
-	s->cancel = s626_ai_cancel;
+/*  Slot 0 base settings. */
+#define VECT0	(XSD2 | RSD3 | SIB_A2)
+/*  Slot 0 always shifts in  0xFF and store it to  FB_BUFFER2. */
 
-	s = dev->subdevices + 1;
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = (0x3fff);
-	s->range_table = &range_bipolar10;
-	s->insn_write = s626_ao_winsn;
-	s->insn_read = s626_ao_rinsn;
+/*  TrimDac LogicalChan-to-PhysicalChan mapping table. */
+static uint8_t trimchan[] = { 10, 9, 8, 3, 2, 7, 6, 1, 0, 5, 4 };
 
-	s = dev->subdevices + 2;
-	/* digital I/O subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = S626_DIO_CHANNELS;
-	s->maxdata = 1;
-	s->io_bits = 0xffff;
-	s->private = &dio_private_A;
-	s->range_table = &range_digital;
-	s->insn_config = s626_dio_insn_config;
-	s->insn_bits = s626_dio_insn_bits;
+/*  TrimDac LogicalChan-to-EepromAdrs mapping table. */
+static uint8_t trimadrs[] = { 0x40, 0x41, 0x42, 0x50, 0x51, 0x52, 0x53, 0x60, 0x61, 0x62, 0x63 };
 
-	s = dev->subdevices + 3;
-	/* digital I/O subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->io_bits = 0xffff;
-	s->private = &dio_private_B;
-	s->range_table = &range_digital;
-	s->insn_config = s626_dio_insn_config;
-	s->insn_bits = s626_dio_insn_bits;
+/* Private helper function: Transmit serial data to DAC via Audio
+ * channel 2.  Assumes: (1) TSL2 slot records initialized, and (2)
+ * Dacpol contains valid target image.
+ */
+static void SendDAC(struct comedi_device *dev, uint32_t val)
+{
 
-	s = dev->subdevices + 4;
-	/* digital I/O subdevice */
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 16;
-	s->maxdata = 1;
-	s->io_bits = 0xffff;
-	s->private = &dio_private_C;
-	s->range_table = &range_digital;
-	s->insn_config = s626_dio_insn_config;
-	s->insn_bits = s626_dio_insn_bits;
+	/* START THE SERIAL CLOCK RUNNING ------------- */
 
-	s = dev->subdevices + 5;
-	/* encoder (counter) subdevice */
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
-	s->n_chan = thisboard->enc_chans;
-	s->private = enc_private_data;
-	s->insn_config = s626_enc_insn_config;
-	s->insn_read = s626_enc_insn_read;
-	s->insn_write = s626_enc_insn_write;
-	s->maxdata = 0xffffff;
-	s->range_table = &range_unknown;
+	/* Assert DAC polarity control and enable gating of DAC serial clock
+	 * and audio bit stream signals.  At this point in time we must be
+	 * assured of being in time slot 0.  If we are not in slot 0, the
+	 * serial clock and audio stream signals will be disabled; this is
+	 * because the following DEBIwrite statement (which enables signals
+	 * to be passed through the gate array) would execute before the
+	 * trailing edge of WS1/WS3 (which turns off the signals), thus
+	 * causing the signals to be inactive during the DAC write.
+	 */
+	DEBIwrite(dev, LP_DACPOL, devpriv->Dacpol);
 
-	/* stop ai_command */
-	devpriv->ai_cmd_running = 0;
+	/* TRANSFER OUTPUT DWORD VALUE INTO A2'S OUTPUT FIFO ---------------- */
 
-	if (devpriv->base_addr && (devpriv->allocatedBuf == 2)) {
-		dma_addr_t pPhysBuf;
-		uint16_t chan;
+	/* Copy DAC setpoint value to DAC's output DMA buffer. */
 
-		/*  enab DEBI and audio pins, enable I2C interface. */
-		MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
-		/*  Configure DEBI operating mode. */
-		WR7146(P_DEBICFG, DEBI_CFG_SLAVE16	/*  Local bus is 16 */
-		       /*  bits wide. */
-		       | (DEBI_TOUT << DEBI_CFG_TOUT_BIT)
+	/* WR7146( (uint32_t)devpriv->pDacWBuf, val ); */
+	*devpriv->pDacWBuf = val;
 
-		       /*  Declare DEBI */
-		       /*  transfer timeout */
-		       /*  interval. */
-		       |DEBI_SWAP	/*  Set up byte lane */
-		       /*  steering. */
-		       | DEBI_CFG_INTEL);	/*  Intel-compatible */
-		/*  local bus (DEBI */
-		/*  never times out). */
-		DEBUG("s626_attach: %d debi init -- %d\n",
-		      DEBI_CFG_SLAVE16 | (DEBI_TOUT << DEBI_CFG_TOUT_BIT) |
-		      DEBI_SWAP | DEBI_CFG_INTEL,
-		      DEBI_CFG_INTEL | DEBI_CFG_TOQ | DEBI_CFG_INCQ |
-		      DEBI_CFG_16Q);
+	/* enab the output DMA transfer.  This will cause the DMAC to copy
+	 * the DAC's data value to A2's output FIFO.  The DMA transfer will
+	 * then immediately terminate because the protection address is
+	 * reached upon transfer of the first DWORD value.
+	 */
+	MC_ENABLE(P_MC1, MC1_A2OUT);
 
-		/* DEBI INIT S626 WR7146( P_DEBICFG, DEBI_CFG_INTEL | DEBI_CFG_TOQ */
-		/* | DEBI_CFG_INCQ| DEBI_CFG_16Q); //end */
+	/*  While the DMA transfer is executing ... */
 
-		/*  Paging is disabled. */
-		WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);	/*  Disable MMU paging. */
+	/* Reset Audio2 output FIFO's underflow flag (along with any other
+	 * FIFO underflow/overflow flags).  When set, this flag will
+	 * indicate that we have emerged from slot 0.
+	 */
+	WR7146(P_ISR, ISR_AFOU);
 
-		/*  Init GPIO so that ADC Start* is negated. */
-		WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
+	/* Wait for the DMA transfer to finish so that there will be data
+	 * available in the FIFO when time slot 1 tries to transfer a DWORD
+	 * from the FIFO to the output buffer register.  We test for DMA
+	 * Done by polling the DMAC enable flag; this flag is automatically
+	 * cleared when the transfer has finished.
+	 */
+	while ((RR7146(P_MC1) & MC1_A2OUT) != 0)
+		;
 
-		/* IsBoardRevA is a boolean that indicates whether the board is RevA.
-		 *
-		 * VERSION 2.01 CHANGE: REV A & B BOARDS NOW SUPPORTED BY DYNAMIC
-		 * EEPROM ADDRESS SELECTION.  Initialize the I2C interface, which
-		 * is used to access the onboard serial EEPROM.  The EEPROM's I2C
-		 * DeviceAddress is hardwired to a value that is dependent on the
-		 * 626 board revision.  On all board revisions, the EEPROM stores
-		 * TrimDAC calibration constants for analog I/O.  On RevB and
-		 * higher boards, the DeviceAddress is hardwired to 0 to enable
-		 * the EEPROM to also store the PCI SubVendorID and SubDeviceID;
-		 * this is the address at which the SAA7146 expects a
-		 * configuration EEPROM to reside.  On RevA boards, the EEPROM
-		 * device address, which is hardwired to 4, prevents the SAA7146
-		 * from retrieving PCI sub-IDs, so the SAA7146 uses its built-in
-		 * default values, instead.
-		 */
+	/* START THE OUTPUT STREAM TO THE TARGET DAC -------------------- */
 
-		/*     devpriv->I2Cards= IsBoardRevA ? 0xA8 : 0xA0; // Set I2C EEPROM */
-		/*  DeviceType (0xA0) */
-		/*  and DeviceAddress<<1. */
+	/* FIFO data is now available, so we enable execution of time slots
+	 * 1 and higher by clearing the EOS flag in slot 0.  Note that SD3
+	 * will be shifted in and stored in FB_BUFFER2 for end-of-slot-list
+	 * detection.
+	 */
+	SETVECT(0, XSD2 | RSD3 | SIB_A2);
 
-		devpriv->I2CAdrs = 0xA0;	/*  I2C device address for onboard */
-		/*  eeprom(revb) */
+	/* Wait for slot 1 to execute to ensure that the Packet will be
+	 * transmitted.  This is detected by polling the Audio2 output FIFO
+	 * underflow flag, which will be set when slot 1 execution has
+	 * finished transferring the DAC's data DWORD from the output FIFO
+	 * to the output buffer register.
+	 */
+	while ((RR7146(P_SSR) & SSR_AF2_OUT) == 0)
+		;
 
-		/*  Issue an I2C ABORT command to halt any I2C operation in */
-		/* progress and reset BUSY flag. */
-		WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);
-		/*  Write I2C control: abort any I2C activity. */
-		MC_ENABLE(P_MC2, MC2_UPLD_IIC);
-		/*  Invoke command  upload */
-		while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0)
-			;
-		/*  and wait for upload to complete. */
+	/* Set up to trap execution at slot 0 when the TSL sequencer cycles
+	 * back to slot 0 after executing the EOS in slot 5.  Also,
+	 * simultaneously shift out and in the 0x00 that is ALWAYS the value
+	 * stored in the last byte to be shifted out of the FIFO's DWORD
+	 * buffer register.
+	 */
+	SETVECT(0, XSD2 | XFIFO_2 | RSD2 | SIB_A2 | EOS);
 
-		/* Per SAA7146 data sheet, write to STATUS reg twice to
-		 * reset all  I2C error flags. */
-		for (i = 0; i < 2; i++) {
-			WR7146(P_I2CSTAT, I2C_CLKSEL);
-			/*  Write I2C control: reset  error flags. */
-			MC_ENABLE(P_MC2, MC2_UPLD_IIC);	/*  Invoke command upload */
-			while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
-				;
-			/* and wait for upload to complete. */
-		}
+	/* WAIT FOR THE TRANSACTION TO FINISH ----------------------- */
 
-		/* Init audio interface functional attributes: set DAC/ADC
-		 * serial clock rates, invert DAC serial clock so that
-		 * DAC data setup times are satisfied, enable DAC serial
-		 * clock out.
+	/* Wait for the TSL to finish executing all time slots before
+	 * exiting this function.  We must do this so that the next DAC
+	 * write doesn't start, thereby enabling clock/chip select signals:
+	 *
+	 * 1. Before the TSL sequence cycles back to slot 0, which disables
+	 *    the clock/cs signal gating and traps slot // list execution.
+	 *    we have not yet finished slot 5 then the clock/cs signals are
+	 *    still gated and we have not finished transmitting the stream.
+	 *
+	 * 2. While slots 2-5 are executing due to a late slot 0 trap.  In
+	 *    this case, the slot sequence is currently repeating, but with
+	 *    clock/cs signals disabled.  We must wait for slot 0 to trap
+	 *    execution before setting up the next DAC setpoint DMA transfer
+	 *    and enabling the clock/cs signals.  To detect the end of slot 5,
+	 *    we test for the FB_BUFFER2 MSB contents to be equal to 0xFF.  If
+	 *    the TSL has not yet finished executing slot 5 ...
+	 */
+	if ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) {
+		/* The trap was set on time and we are still executing somewhere
+		 * in slots 2-5, so we now wait for slot 0 to execute and trap
+		 * TSL execution.  This is detected when FB_BUFFER2 MSB changes
+		 * from 0xFF to 0x00, which slot 0 causes to happen by shifting
+		 * out/in on SD2 the 0x00 that is always referenced by slot 5.
 		 */
+		while ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0)
+			;
+	}
+	/* Either (1) we were too late setting the slot 0 trap; the TSL
+	 * sequencer restarted slot 0 before we could set the EOS trap flag,
+	 * or (2) we were not late and execution is now trapped at slot 0.
+	 * In either case, we must now change slot 0 so that it will store
+	 * value 0xFF (instead of 0x00) to FB_BUFFER2 next time it executes.
+	 * In order to do this, we reprogram slot 0 so that it will shift in
+	 * SD3, which is driven only by a pull-up resistor.
+	 */
+	SETVECT(0, RSD3 | SIB_A2 | EOS);
 
-		WR7146(P_ACON2, ACON2_INIT);
-
-		/* Set up TSL1 slot list, which is used to control the
-		 * accumulation of ADC data: RSD1 = shift data in on SD1.
-		 * SIB_A1  = store data uint8_t at next available location in
-		 * FB BUFFER1  register. */
-		WR7146(P_TSL1, RSD1 | SIB_A1);
-		/*  Fetch ADC high data uint8_t. */
-		WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);
-		/*  Fetch ADC low data uint8_t; end of TSL1. */
+	/* Wait for slot 0 to execute, at which time the TSL is setup for
+	 * the next DAC write.  This is detected when FB_BUFFER2 MSB changes
+	 * from 0x00 to 0xFF.
+	 */
+	while ((RR7146(P_FB_BUFFER2) & 0xFF000000) == 0)
+		;
+}
 
-		/*  enab TSL1 slot list so that it executes all the time. */
-		WR7146(P_ACON1, ACON1_ADCSTART);
+/*  Private helper function: Write setpoint to an application DAC channel. */
+static void SetDAC(struct comedi_device *dev, uint16_t chan, short dacdata)
+{
+	register uint16_t signmask;
+	register uint32_t WSImage;
 
-		/*  Initialize RPS registers used for ADC. */
+	/*  Adjust DAC data polarity and set up Polarity Control Register */
+	/*  image. */
+	signmask = 1 << chan;
+	if (dacdata < 0) {
+		dacdata = -dacdata;
+		devpriv->Dacpol |= signmask;
+	} else
+		devpriv->Dacpol &= ~signmask;
 
-		/* Physical start of RPS program. */
-		WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+	/*  Limit DAC setpoint value to valid range. */
+	if ((uint16_t) dacdata > 0x1FFF)
+		dacdata = 0x1FFF;
 
-		WR7146(P_RPSPAGE1, 0);
-		/*  RPS program performs no explicit mem writes. */
-		WR7146(P_RPS1_TOUT, 0);	/*  Disable RPS timeouts. */
+	/* Set up TSL2 records (aka "vectors") for DAC update.  Vectors V2
+	 * and V3 transmit the setpoint to the target DAC.  V4 and V5 send
+	 * data to a non-existent TrimDac channel just to keep the clock
+	 * running after sending data to the target DAC.  This is necessary
+	 * to eliminate the clock glitch that would otherwise occur at the
+	 * end of the target DAC's serial data stream.  When the sequence
+	 * restarts at V0 (after executing V5), the gate array automatically
+	 * disables gating for the DAC clock and all DAC chip selects.
+	 */
 
-		/* SAA7146 BUG WORKAROUND.  Initialize SAA7146 ADC interface
-		 * to a known state by invoking ADCs until FB BUFFER 1
-		 * register shows that it is correctly receiving ADC data.
-		 * This is necessary because the SAA7146 ADC interface does
-		 * not start up in a defined state after a PCI reset.
-		 */
+	WSImage = (chan & 2) ? WS1 : WS2;
+	/* Choose DAC chip select to be asserted. */
+	SETVECT(2, XSD2 | XFIFO_1 | WSImage);
+	/* Slot 2: Transmit high data byte to target DAC. */
+	SETVECT(3, XSD2 | XFIFO_0 | WSImage);
+	/* Slot 3: Transmit low data byte to target DAC. */
+	SETVECT(4, XSD2 | XFIFO_3 | WS3);
+	/* Slot 4: Transmit to non-existent TrimDac channel to keep clock */
+	SETVECT(5, XSD2 | XFIFO_2 | WS3 | EOS);
+	/* Slot 5: running after writing target DAC's low data byte. */
 
-/*     PollList = EOPL;		// Create a simple polling */
-/*				// list for analog input */
-/*				// channel 0. */
-/*     ResetADC( dev, &PollList ); */
+	/*  Construct and transmit target DAC's serial packet:
+	 * ( A10D DDDD ),( DDDD DDDD ),( 0x0F ),( 0x00 ) where A is chan<0>,
+	 * and D<12:0> is the DAC setpoint.  Append a WORD value (that writes
+	 * to a  non-existent TrimDac channel) that serves to keep the clock
+	 * running after the packet has been sent to the target DAC.
+	 */
+	SendDAC(dev, 0x0F000000
+		/* Continue clock after target DAC data (write to non-existent trimdac). */
+		| 0x00004000
+		/* Address the two main dual-DAC devices (TSL's chip select enables
+		 * target device). */
+		| ((uint32_t) (chan & 1) << 15)
+		/*  Address the DAC channel within the  device. */
+		| (uint32_t) dacdata);	/*  Include DAC setpoint data. */
 
-/*     s626_ai_rinsn(dev,dev->subdevices,NULL,data); //( &AdcData ); // */
-/*							//Get initial ADC */
-/*							//value. */
+}
 
-/*     StartVal = data[0]; */
+static void WriteTrimDAC(struct comedi_device *dev, uint8_t LogicalChan,
+			 uint8_t DacData)
+{
+	uint32_t chan;
 
-/*     // VERSION 2.01 CHANGE: TIMEOUT ADDED TO PREVENT HANGED EXECUTION. */
-/*     // Invoke ADCs until the new ADC value differs from the initial */
-/*     // value or a timeout occurs.  The timeout protects against the */
-/*     // possibility that the driver is restarting and the ADC data is a */
-/*     // fixed value resulting from the applied ADC analog input being */
-/*     // unusually quiet or at the rail. */
+	/*  Save the new setpoint in case the application needs to read it back later. */
+	devpriv->TrimSetpoint[LogicalChan] = (uint8_t) DacData;
 
-/*     for ( index = 0; index < 500; index++ ) */
-/*       { */
-/*	s626_ai_rinsn(dev,dev->subdevices,NULL,data); */
-/*	AdcData = data[0];	//ReadADC(  &AdcData ); */
-/*	if ( AdcData != StartVal ) */
-/*		break; */
-/*       } */
+	/*  Map logical channel number to physical channel number. */
+	chan = (uint32_t) trimchan[LogicalChan];
 
-		/*  end initADC */
+	/* Set up TSL2 records for TrimDac write operation.  All slots shift
+	 * 0xFF in from pulled-up SD3 so that the end of the slot sequence
+	 * can be detected.
+	 */
 
-		/*  init the DAC interface */
+	SETVECT(2, XSD2 | XFIFO_1 | WS3);
+	/* Slot 2: Send high uint8_t to target TrimDac. */
+	SETVECT(3, XSD2 | XFIFO_0 | WS3);
+	/* Slot 3: Send low uint8_t to target TrimDac. */
+	SETVECT(4, XSD2 | XFIFO_3 | WS1);
+	/* Slot 4: Send NOP high uint8_t to DAC0 to keep clock running. */
+	SETVECT(5, XSD2 | XFIFO_2 | WS1 | EOS);
+	/* Slot 5: Send NOP low  uint8_t to DAC0. */
 
-		/* Init Audio2's output DMAC attributes: burst length = 1
-		 * DWORD,  threshold = 1 DWORD.
-		 */
-		WR7146(P_PCI_BT_A, 0);
+	/* Construct and transmit target DAC's serial packet:
+	 * ( 0000 AAAA ), ( DDDD DDDD ),( 0x00 ),( 0x00 ) where A<3:0> is the
+	 * DAC channel's address, and D<7:0> is the DAC setpoint.  Append a
+	 * WORD value (that writes a channel 0 NOP command to a non-existent
+	 * main DAC channel) that serves to keep the clock running after the
+	 * packet has been sent to the target DAC.
+	 */
 
-		/* Init Audio2's output DMA physical addresses.  The protection
-		 * address is set to 1 DWORD past the base address so that a
-		 * single DWORD will be transferred each time a DMA transfer is
-		 * enabled. */
+	/*  Address the DAC channel within the trimdac device. */
+	SendDAC(dev, ((uint32_t) chan << 8)
+		| (uint32_t) DacData);	/*  Include DAC setpoint data. */
+}
 
-		pPhysBuf =
-		    devpriv->ANABuf.PhysicalBase +
-		    (DAC_WDMABUF_OS * sizeof(uint32_t));
+static void LoadTrimDACs(struct comedi_device *dev)
+{
+	register uint8_t i;
 
-		WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);	/*  Buffer base adrs. */
-		WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));	/*  Protection address. */
+	/*  Copy TrimDac setpoint values from EEPROM to TrimDacs. */
+	for (i = 0; i < ARRAY_SIZE(trimchan); i++)
+		WriteTrimDAC(dev, i, I2Cread(dev, trimadrs[i]));
+}
 
-		/* Cache Audio2's output DMA buffer logical address.  This is
-		 * where DAC data is buffered for A2 output DMA transfers. */
-		devpriv->pDacWBuf =
-		    (uint32_t *) devpriv->ANABuf.LogicalBase + DAC_WDMABUF_OS;
+/* ******  COUNTER FUNCTIONS  ******* */
+/* All counter functions address a specific counter by means of the
+ * "Counter" argument, which is a logical counter number.  The Counter
+ * argument may have any of the following legal values: 0=0A, 1=1A,
+ * 2=2A, 3=0B, 4=1B, 5=2B.
+ */
 
-		/* Audio2's output channels does not use paging.  The protection
-		 * violation handling bit is set so that the DMAC will
-		 * automatically halt and its PCI address pointer will be reset
-		 * when the protection address is reached. */
+/*  Read a counter's output latch. */
+static uint32_t ReadLatch(struct comedi_device *dev, struct enc_private *k)
+{
+	register uint32_t value;
 
-		WR7146(P_PAGEA2_OUT, 8);
+	/*  Latch counts and fetch LSW of latched counts value. */
+	value = (uint32_t) DEBIread(dev, k->MyLatchLsw);
 
-		/* Initialize time slot list 2 (TSL2), which is used to control
-		 * the clock generation for and serialization of data to be sent
-		 * to the DAC devices.  Slot 0 is a NOP that is used to trap TSL
-		 * execution; this permits other slots to be safely modified
-		 * without first turning off the TSL sequencer (which is
-		 * apparently impossible to do).  Also, SD3 (which is driven by a
-		 * pull-up resistor) is shifted in and stored to the MSB of
-		 * FB_BUFFER2 to be used as evidence that the slot sequence has
-		 * not yet finished executing.
-		 */
+	/*  Fetch MSW of latched counts and combine with LSW. */
+	value |= ((uint32_t) DEBIread(dev, k->MyLatchLsw + 2) << 16);
 
-		SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);
-		/*  Slot 0: Trap TSL execution, shift 0xFF into FB_BUFFER2. */
+	/*  Return latched counts. */
+	return value;
+}
 
-		/* Initialize slot 1, which is constant.  Slot 1 causes a
-		 * DWORD to be transferred from audio channel 2's output FIFO
-		 * to the FIFO's output buffer so that it can be serialized
-		 * and sent to the DAC during subsequent slots.  All remaining
-		 * slots are dynamically populated as required by the target
-		 * DAC device.
-		 */
-		SETVECT(1, LF_A2);
-		/*  Slot 1: Fetch DWORD from Audio2's output FIFO. */
+/* Return/set a counter pair's latch trigger source.  0: On read
+ * access, 1: A index latches A, 2: B index latches B, 3: A overflow
+ * latches B.
+ */
+static void SetLatchSource(struct comedi_device *dev, struct enc_private *k,
+			   uint16_t value)
+{
+	DEBIreplace(dev, k->MyCRB,
+		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_LATCHSRC)),
+		    (uint16_t) (value << CRBBIT_LATCHSRC));
+}
 
-		/*  Start DAC's audio interface (TSL2) running. */
-		WR7146(P_ACON1, ACON1_DACSTART);
+/*  Write value into counter preload register. */
+static void Preload(struct comedi_device *dev, struct enc_private *k,
+		    uint32_t value)
+{
+	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw), (uint16_t) value);
+	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw + 2),
+		  (uint16_t) (value >> 16));
+}
 
-		/* end init DAC interface */
+static unsigned int s626_ai_reg_to_uint(int data)
+{
+	unsigned int tempdata;
 
-		/* Init Trim DACs to calibrated values.  Do it twice because the
-		 * SAA7146 audio channel does not always reset properly and
-		 * sometimes causes the first few TrimDAC writes to malfunction.
-		 */
+	tempdata = (data >> 18);
+	if (tempdata & 0x2000)
+		tempdata &= 0x1fff;
+	else
+		tempdata += (1 << 13);
 
-		LoadTrimDACs(dev);
-		LoadTrimDACs(dev);	/*  Insurance. */
+	return tempdata;
+}
 
-		/* Manually init all gate array hardware in case this is a soft
-		 * reset (we have no way of determining whether this is a warm
-		 * or cold start).  This is necessary because the gate array will
-		 * reset only in response to a PCI hard reset; there is no soft
-		 * reset function. */
+/* static unsigned int s626_uint_to_reg(struct comedi_subdevice *s, int data){ */
+/*   return 0; */
+/* } */
 
-		/* Init all DAC outputs to 0V and init all DAC setpoint and
-		 * polarity images.
-		 */
-		for (chan = 0; chan < S626_DAC_CHANNELS; chan++)
-			SetDAC(dev, chan, 0);
+static int s626_dio_set_irq(struct comedi_device *dev, unsigned int chan)
+{
+	unsigned int group;
+	unsigned int bitmask;
+	unsigned int status;
 
-		/* Init image of WRMISC2 Battery Charger Enabled control bit.
-		 * This image is used when the state of the charger control bit,
-		 * which has no direct hardware readback mechanism, is queried.
-		 */
-		devpriv->ChargeEnabled = 0;
+	/* select dio bank */
+	group = chan / 16;
+	bitmask = 1 << (chan - (16 * group));
 
-		/* Init image of watchdog timer interval in WRMISC2.  This image
-		 * maintains the value of the control bits of MISC2 are
-		 * continuously reset to zero as long as the WD timer is disabled.
-		 */
-		devpriv->WDInterval = 0;
+	/* set channel to capture positive edge */
+	status = DEBIread(dev,
+			  ((struct dio_private *)(dev->subdevices + 2 +
+						  group)->private)->RDEdgSel);
+	DEBIwrite(dev,
+		  ((struct dio_private *)(dev->subdevices + 2 +
+					  group)->private)->WREdgSel,
+		  bitmask | status);
 
-		/* Init Counter Interrupt enab mask for RDMISC2.  This mask is
-		 * applied against MISC2 when testing to determine which timer
-		 * events are requesting interrupt service.
-		 */
-		devpriv->CounterIntEnabs = 0;
+	/* enable interrupt on selected channel */
+	status = DEBIread(dev,
+			  ((struct dio_private *)(dev->subdevices + 2 +
+						  group)->private)->RDIntSel);
+	DEBIwrite(dev,
+		  ((struct dio_private *)(dev->subdevices + 2 +
+					  group)->private)->WRIntSel,
+		  bitmask | status);
 
-		/*  Init counters. */
-		CountersInit(dev);
+	/* enable edge capture write command */
+	DEBIwrite(dev, LP_MISC1, MISC1_EDCAP);
 
-		/* Without modifying the state of the Battery Backup enab, disable
-		 * the watchdog timer, set DIO channels 0-5 to operate in the
-		 * standard DIO (vs. counter overflow) mode, disable the battery
-		 * charger, and reset the watchdog interval selector to zero.
-		 */
-		WriteMISC2(dev, (uint16_t) (DEBIread(dev,
-						     LP_RDMISC2) &
-					    MISC2_BATT_ENABLE));
+	/* enable edge capture on selected channel */
+	status = DEBIread(dev,
+			  ((struct dio_private *)(dev->subdevices + 2 +
+						  group)->private)->RDCapSel);
+	DEBIwrite(dev,
+		  ((struct dio_private *)(dev->subdevices + 2 +
+					  group)->private)->WRCapSel,
+		  bitmask | status);
 
-		/*  Initialize the digital I/O subsystem. */
-		s626_dio_init(dev);
+	return 0;
+}
 
-		/* enable interrupt test */
-		/*  writel(IRQ_GPIO3 | IRQ_RPS1,devpriv->base_addr+P_IER); */
-	}
+static int s626_dio_reset_irq(struct comedi_device *dev, unsigned int group,
+			      unsigned int mask)
+{
+	/* disable edge capture write command */
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
 
-	DEBUG("s626_attach: comedi%d s626 attached %04x\n", dev->minor,
-	      (uint32_t) devpriv->base_addr);
+	/* enable edge capture on selected channel */
+	DEBIwrite(dev,
+		  ((struct dio_private *)(dev->subdevices + 2 +
+					  group)->private)->WRCapSel, mask);
 
-	return 1;
+	return 0;
 }
 
-static unsigned int s626_ai_reg_to_uint(int data)
+static int s626_dio_clear_irq(struct comedi_device *dev)
 {
-	unsigned int tempdata;
+	unsigned int group;
 
-	tempdata = (data >> 18);
-	if (tempdata & 0x2000)
-		tempdata &= 0x1fff;
-	else
-		tempdata += (1 << 13);
+	/* disable edge capture write command */
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
 
-	return tempdata;
-}
+	for (group = 0; group < S626_DIO_BANKS; group++) {
+		/* clear pending events and interrupt */
+		DEBIwrite(dev,
+			  ((struct dio_private *)(dev->subdevices + 2 +
+						  group)->private)->WRCapSel,
+			  0xffff);
+	}
 
-/* static unsigned int s626_uint_to_reg(struct comedi_subdevice *s, int data){ */
-/*   return 0; */
-/* } */
+	return 0;
+}
 
 static irqreturn_t s626_irq_handler(int irq, void *d)
 {
@@ -995,8 +789,6 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 	uint8_t group;
 	uint16_t irqbit;
 
-	DEBUG("s626_irq_handler: interrupt request received!!!\n");
-
 	if (dev->attached == 0)
 		return IRQ_NONE;
 	/*  lock to avoid race with comedi_poll */
@@ -1014,14 +806,8 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 	/* clear interrupt */
 	writel(irqtype, devpriv->base_addr + P_ISR);
 
-	/* do somethings */
-	DEBUG("s626_irq_handler: interrupt type %d\n", irqtype);
-
 	switch (irqtype) {
 	case IRQ_RPS1:		/*  end_of_scan occurs */
-
-		DEBUG("s626_irq_handler: RPS1 irq detected\n");
-
 		/*  manage ai subdevice */
 		s = dev->subdevices;
 		cmd = &(s->async->cmd);
@@ -1044,9 +830,6 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 			if (cfc_write_to_buffer(s, tempdata) == 0)
 				printk
 				    ("s626_irq_handler: cfc_write_to_buffer error!\n");
-
-			DEBUG("s626_irq_handler: ai channel %d acquired: %d\n",
-			      i, tempdata);
 		}
 
 		/* end of scan occurs */
@@ -1067,23 +850,12 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 			irqstatus = 0;
 		}
 
-		if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT) {
-			DEBUG
-			    ("s626_irq_handler: enable interrupt on dio channel %d\n",
-			     cmd->scan_begin_arg);
-
+		if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT)
 			s626_dio_set_irq(dev, cmd->scan_begin_arg);
-
-			DEBUG("s626_irq_handler: External trigger is set!!!\n");
-		}
 		/*  tell comedi that data is there */
-		DEBUG("s626_irq_handler: events %d\n", s->async->events);
 		comedi_event(dev, s);
 		break;
 	case IRQ_GPIO3:	/* check dio and conter interrupt */
-
-		DEBUG("s626_irq_handler: GPIO3 irq detected\n");
-
 		/*  manage ai subdevice */
 		s = dev->subdevices;
 		cmd = &(s->async->cmd);
@@ -1103,36 +875,18 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 			/* check if interrupt is generated from dio channels */
 			if (irqbit) {
 				s626_dio_reset_irq(dev, group, irqbit);
-				DEBUG
-				    ("s626_irq_handler: check interrupt on dio group %d %d\n",
-				     group, i);
 				if (devpriv->ai_cmd_running) {
 					/* check if interrupt is an ai acquisition start trigger */
 					if ((irqbit >> (cmd->start_arg -
 							(16 * group)))
 					    == 1 && cmd->start_src == TRIG_EXT) {
-						DEBUG
-						    ("s626_irq_handler: Edge capture interrupt received from channel %d\n",
-						     cmd->start_arg);
-
 						/*  Start executing the RPS program. */
 						MC_ENABLE(P_MC1, MC1_ERPS1);
 
-						DEBUG
-						    ("s626_irq_handler: acquisition start triggered!!!\n");
-
 						if (cmd->scan_begin_src ==
 						    TRIG_EXT) {
-							DEBUG
-							    ("s626_ai_cmd: enable interrupt on dio channel %d\n",
-							     cmd->
-							     scan_begin_arg);
-
 							s626_dio_set_irq(dev,
 									 cmd->scan_begin_arg);
-
-							DEBUG
-							    ("s626_irq_handler: External scan trigger is set!!!\n");
 						}
 					}
 					if ((irqbit >> (cmd->scan_begin_arg -
@@ -1140,33 +894,16 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 					    == 1
 					    && cmd->scan_begin_src ==
 					    TRIG_EXT) {
-						DEBUG
-						    ("s626_irq_handler: Edge capture interrupt received from channel %d\n",
-						     cmd->scan_begin_arg);
-
 						/*  Trigger ADC scan loop start by setting RPS Signal 0. */
 						MC_ENABLE(P_MC2, MC2_ADC_RPS);
 
-						DEBUG
-						    ("s626_irq_handler: scan triggered!!! %d\n",
-						     devpriv->ai_sample_count);
 						if (cmd->convert_src ==
 						    TRIG_EXT) {
-
-							DEBUG
-							    ("s626_ai_cmd: enable interrupt on dio channel %d group %d\n",
-							     cmd->convert_arg -
-							     (16 * group),
-							     group);
-
 							devpriv->ai_convert_count
 							    = cmd->chanlist_len;
 
 							s626_dio_set_irq(dev,
 									 cmd->convert_arg);
-
-							DEBUG
-							    ("s626_irq_handler: External convert trigger is set!!!\n");
 						}
 
 						if (cmd->convert_src ==
@@ -1182,32 +919,15 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 							(16 * group)))
 					    == 1
 					    && cmd->convert_src == TRIG_EXT) {
-						DEBUG
-						    ("s626_irq_handler: Edge capture interrupt received from channel %d\n",
-						     cmd->convert_arg);
-
 						/*  Trigger ADC scan loop start by setting RPS Signal 0. */
 						MC_ENABLE(P_MC2, MC2_ADC_RPS);
 
-						DEBUG
-						    ("s626_irq_handler: adc convert triggered!!!\n");
-
 						devpriv->ai_convert_count--;
 
 						if (devpriv->ai_convert_count >
 						    0) {
-
-							DEBUG
-							    ("s626_ai_cmd: enable interrupt on dio channel %d group %d\n",
-							     cmd->convert_arg -
-							     (16 * group),
-							     group);
-
 							s626_dio_set_irq(dev,
 									 cmd->convert_arg);
-
-							DEBUG
-							    ("s626_irq_handler: External trigger is set!!!\n");
 						}
 					}
 				}
@@ -1219,44 +939,31 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 		irqbit = DEBIread(dev, LP_RDMISC2);
 
 		/* check interrupt on counters */
-		DEBUG("s626_irq_handler: check counters interrupt %d\n",
-		      irqbit);
-
 		if (irqbit & IRQ_COINT1A) {
-			DEBUG
-			    ("s626_irq_handler: interrupt on counter 1A overflow\n");
 			k = &encpriv[0];
 
 			/* clear interrupt capture flag */
 			k->ResetCapFlags(dev, k);
 		}
 		if (irqbit & IRQ_COINT2A) {
-			DEBUG
-			    ("s626_irq_handler: interrupt on counter 2A overflow\n");
 			k = &encpriv[1];
 
 			/* clear interrupt capture flag */
 			k->ResetCapFlags(dev, k);
 		}
 		if (irqbit & IRQ_COINT3A) {
-			DEBUG
-			    ("s626_irq_handler: interrupt on counter 3A overflow\n");
 			k = &encpriv[2];
 
 			/* clear interrupt capture flag */
 			k->ResetCapFlags(dev, k);
 		}
 		if (irqbit & IRQ_COINT1B) {
-			DEBUG
-			    ("s626_irq_handler: interrupt on counter 1B overflow\n");
 			k = &encpriv[3];
 
 			/* clear interrupt capture flag */
 			k->ResetCapFlags(dev, k);
 		}
 		if (irqbit & IRQ_COINT2B) {
-			DEBUG
-			    ("s626_irq_handler: interrupt on counter 2B overflow\n");
 			k = &encpriv[4];
 
 			/* clear interrupt capture flag */
@@ -1268,34 +975,23 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 					k->SetEnable(dev, k, CLKENAB_INDEX);
 
 				if (cmd->convert_src == TRIG_TIMER) {
-					DEBUG
-					    ("s626_irq_handler: conver timer trigger!!! %d\n",
-					     devpriv->ai_convert_count);
-
 					/*  Trigger ADC scan loop start by setting RPS Signal 0. */
 					MC_ENABLE(P_MC2, MC2_ADC_RPS);
 				}
 			}
 		}
 		if (irqbit & IRQ_COINT3B) {
-			DEBUG
-			    ("s626_irq_handler: interrupt on counter 3B overflow\n");
 			k = &encpriv[5];
 
 			/* clear interrupt capture flag */
 			k->ResetCapFlags(dev, k);
 
 			if (cmd->scan_begin_src == TRIG_TIMER) {
-				DEBUG
-				    ("s626_irq_handler: scan timer trigger!!!\n");
-
 				/*  Trigger ADC scan loop start by setting RPS Signal 0. */
 				MC_ENABLE(P_MC2, MC2_ADC_RPS);
 			}
 
 			if (cmd->convert_src == TRIG_TIMER) {
-				DEBUG
-				    ("s626_irq_handler: convert timer trigger is set\n");
 				k = &encpriv[4];
 				devpriv->ai_convert_count = cmd->chanlist_len;
 				k->SetEnable(dev, k, CLKENAB_ALWAYS);
@@ -1306,50 +1002,14 @@ static irqreturn_t s626_irq_handler(int irq, void *d)
 	/* enable interrupt */
 	writel(irqstatus, devpriv->base_addr + P_IER);
 
-	DEBUG("s626_irq_handler: exit interrupt service routine.\n");
-
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 	return IRQ_HANDLED;
 }
 
-static void s626_detach(struct comedi_device *dev)
-{
-	if (devpriv) {
-		/* stop ai_command */
-		devpriv->ai_cmd_running = 0;
-
-		if (devpriv->base_addr) {
-			/* interrupt mask */
-			WR7146(P_IER, 0);	/*  Disable master interrupt. */
-			WR7146(P_ISR, IRQ_GPIO3 | IRQ_RPS1);	/*  Clear board's IRQ status flag. */
-
-			/*  Disable the watchdog timer and battery charger. */
-			WriteMISC2(dev, 0);
-
-			/*  Close all interfaces on 7146 device. */
-			WR7146(P_MC1, MC1_SHUTDOWN);
-			WR7146(P_ACON1, ACON1_BASE);
-
-			CloseDMAB(dev, &devpriv->RPSBuf, DMABUF_SIZE);
-			CloseDMAB(dev, &devpriv->ANABuf, DMABUF_SIZE);
-		}
-
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-		if (devpriv->base_addr)
-			iounmap(devpriv->base_addr);
-		if (devpriv->pdev) {
-			if (devpriv->got_regions)
-				comedi_pci_disable(devpriv->pdev);
-			pci_dev_put(devpriv->pdev);
-		}
-	}
-}
-
 /*
  * this functions build the RPS program for hardware driven acquistion
  */
-void ResetADC(struct comedi_device *dev, uint8_t * ppl)
+static void ResetADC(struct comedi_device *dev, uint8_t *ppl)
 {
 	register uint32_t *pRPS;
 	uint32_t JmpAdrs;
@@ -1370,7 +1030,6 @@ void ResetADC(struct comedi_device *dev, uint8_t * ppl)
 	/*  Construct RPS program in RPSBuf DMA buffer */
 
 	if (cmd != NULL && cmd->scan_begin_src != TRIG_FOLLOW) {
-		DEBUG("ResetADC: scan_begin pause inserted\n");
 		/*  Wait for Start trigger. */
 		*pRPS++ = RPS_PAUSE | RPS_SIGADC;
 		*pRPS++ = RPS_CLRSIGNAL | RPS_SIGADC;
@@ -1460,7 +1119,6 @@ void ResetADC(struct comedi_device *dev, uint8_t * ppl)
 		}
 
 		if (cmd != NULL && cmd->convert_src != TRIG_NOW) {
-			DEBUG("ResetADC: convert pause inserted\n");
 			/*  Wait for Start trigger. */
 			*pRPS++ = RPS_PAUSE | RPS_SIGADC;
 			*pRPS++ = RPS_CLRSIGNAL | RPS_SIGADC;
@@ -1492,7 +1150,6 @@ void ResetADC(struct comedi_device *dev, uint8_t * ppl)
 			break;	/*  Exit poll list processing loop. */
 		}
 	}
-	DEBUG("ResetADC: ADC items %d\n", devpriv->AdcItems);
 
 	/* VERSION 2.01 CHANGE: DELAY CHANGED FROM 250NS to 2US.  Allow the
 	 * ADC to stabilize for 2 microseconds before starting the final
@@ -1529,7 +1186,6 @@ void ResetADC(struct comedi_device *dev, uint8_t * ppl)
 
 	/* invoke interrupt */
 	if (devpriv->ai_cmd_running == 1) {
-		DEBUG("ResetADC: insert irq in ADC RPS task\n");
 		*pRPS++ = RPS_IRQ;
 	}
 	/*  Restart RPS program at its beginning. */
@@ -1553,8 +1209,6 @@ static int s626_ai_insn_config(struct comedi_device *dev,
 /*   register uint8_t	i; */
 /*   register int32_t	*readaddr; */
 
-/*   DEBUG("as626_ai_rinsn: ai_rinsn enter\n");  */
-
 /*   Trigger ADC scan loop start by setting RPS Signal 0. */
 /*   MC_ENABLE( P_MC2, MC2_ADC_RPS ); */
 
@@ -1570,11 +1224,9 @@ static int s626_ai_insn_config(struct comedi_device *dev,
 /*  Convert ADC data to 16-bit integer values and copy to application buffer. */
 /*   for ( i = 0; i < devpriv->AdcItems; i++ ) { */
 /*     *data = s626_ai_reg_to_uint( *readaddr++ ); */
-/*     DEBUG("s626_ai_rinsn: data %d\n",*data); */
 /*     data++; */
 /*   } */
 
-/*   DEBUG("s626_ai_rinsn: ai_rinsn escape\n"); */
 /*   return i; */
 /* } */
 
@@ -1594,8 +1246,6 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 	 * corresponding interrupt to be generated if enabled
 	 */
 
-	DEBUG("s626_ai_insn_read: entering\n");
-
 	/* Convert application's ADC specification into form
 	 *  appropriate for register programming.
 	 */
@@ -1672,8 +1322,6 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 	if (n != 0)
 		data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
 
-	DEBUG("s626_ai_insn_read: samples %d, data %d\n", n, data[n - 1]);
-
 	return n;
 }
 
@@ -1700,18 +1348,76 @@ static int s626_ai_inttrig(struct comedi_device *dev,
 	if (trignum != 0)
 		return -EINVAL;
 
-	DEBUG("s626_ai_inttrig: trigger adc start...");
-
 	/*  Start executing the RPS program. */
 	MC_ENABLE(P_MC1, MC1_ERPS1);
 
 	s->async->inttrig = NULL;
 
-	DEBUG(" done\n");
-
 	return 1;
 }
 
+/* This function doesn't require a particular form, this is just what
+ * happens to be used in some of the drivers.  It should convert ns
+ * nanoseconds to a counter value suitable for programming the device.
+ * Also, it should adjust ns so that it cooresponds to the actual time
+ * that the device will use. */
+static int s626_ns_to_timer(int *nanosec, int round_mode)
+{
+	int divider, base;
+
+	base = 500;		/* 2MHz internal clock */
+
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		divider = (*nanosec + base / 2) / base;
+		break;
+	case TRIG_ROUND_DOWN:
+		divider = (*nanosec) / base;
+		break;
+	case TRIG_ROUND_UP:
+		divider = (*nanosec + base - 1) / base;
+		break;
+	}
+
+	*nanosec = base * divider;
+	return divider - 1;
+}
+
+static void s626_timer_load(struct comedi_device *dev, struct enc_private *k,
+			    int tick)
+{
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	/*  Preload upon */
+	    /*  index. */
+	    (INDXSRC_SOFT << BF_INDXSRC) |	/*  Disable hardware index. */
+	    (CLKSRC_TIMER << BF_CLKSRC) |	/*  Operating mode is Timer. */
+	    (CLKPOL_POS << BF_CLKPOL) |	/*  Active high clock. */
+	    (CNTDIR_DOWN << BF_CLKPOL) |	/*  Count direction is Down. */
+	    (CLKMULT_1X << BF_CLKMULT) |	/*  Clock multiplier is 1x. */
+	    (CLKENAB_INDEX << BF_CLKENAB);
+	uint16_t valueSrclatch = LATCHSRC_A_INDXA;
+	/*   uint16_t enab=CLKENAB_ALWAYS; */
+
+	k->SetMode(dev, k, Setup, FALSE);
+
+	/*  Set the preload register */
+	Preload(dev, k, tick);
+
+	/*  Software index pulse forces the preload register to load */
+	/*  into the counter */
+	k->SetLoadTrig(dev, k, 0);
+	k->PulseIndex(dev, k);
+
+	/* set reload on counter overflow */
+	k->SetLoadTrig(dev, k, 1);
+
+	/* set interrupt on overflow */
+	k->SetIntSrc(dev, k, INTSRC_OVER);
+
+	SetLatchSource(dev, k, valueSrclatch);
+	/*   k->SetEnable(dev,k,(uint16_t)(enab != 0)); */
+}
+
 /*  TO COMPLETE  */
 static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
@@ -1721,8 +1427,6 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct enc_private *k;
 	int tick;
 
-	DEBUG("s626_ai_cmd: entering command function\n");
-
 	if (devpriv->ai_cmd_running) {
 		printk(KERN_ERR "s626_ai_cmd: Another ai_cmd is running %d\n",
 		       dev->minor);
@@ -1742,12 +1446,8 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_cmd_running = 0;
 
 	/*  test if cmd is valid */
-	if (cmd == NULL) {
-		DEBUG("s626_ai_cmd: NULL command\n");
+	if (cmd == NULL)
 		return -EINVAL;
-	} else {
-		DEBUG("s626_ai_cmd: command received!!!\n");
-	}
 
 	if (dev->irq == 0) {
 		comedi_error(dev,
@@ -1771,18 +1471,11 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/* load timer value and enable interrupt */
 		s626_timer_load(dev, k, tick);
 		k->SetEnable(dev, k, CLKENAB_ALWAYS);
-
-		DEBUG("s626_ai_cmd: scan trigger timer is set with value %d\n",
-		      tick);
-
 		break;
 	case TRIG_EXT:
 		/*  set the digital line and interrupt for scan trigger */
 		if (cmd->start_src != TRIG_EXT)
 			s626_dio_set_irq(dev, cmd->scan_begin_arg);
-
-		DEBUG("s626_ai_cmd: External scan trigger is set!!!\n");
-
 		break;
 	}
 
@@ -1798,19 +1491,12 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/* load timer value and enable interrupt */
 		s626_timer_load(dev, k, tick);
 		k->SetEnable(dev, k, CLKENAB_INDEX);
-
-		DEBUG
-		    ("s626_ai_cmd: convert trigger timer is set with value %d\n",
-		     tick);
 		break;
 	case TRIG_EXT:
 		/*  set the digital line and interrupt for convert trigger */
 		if (cmd->scan_begin_src != TRIG_EXT
 		    && cmd->start_src == TRIG_EXT)
 			s626_dio_set_irq(dev, cmd->convert_arg);
-
-		DEBUG("s626_ai_cmd: External convert trigger is set!!!\n");
-
 		break;
 	}
 
@@ -1837,15 +1523,12 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/*  Start executing the RPS program. */
 		MC_ENABLE(P_MC1, MC1_ERPS1);
 
-		DEBUG("s626_ai_cmd: ADC triggered\n");
 		s->async->inttrig = NULL;
 		break;
 	case TRIG_EXT:
 		/* configure DIO channel for acquisition trigger */
 		s626_dio_set_irq(dev, cmd->start_arg);
 
-		DEBUG("s626_ai_cmd: External start trigger is set!!!\n");
-
 		s->async->inttrig = NULL;
 		break;
 	case TRIG_INT:
@@ -1856,8 +1539,6 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* enable interrupt */
 	writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->base_addr + P_IER);
 
-	DEBUG("s626_ai_cmd: command function terminated\n");
-
 	return 0;
 }
 
@@ -2044,34 +1725,6 @@ static int s626_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-/* This function doesn't require a particular form, this is just what
- * happens to be used in some of the drivers.  It should convert ns
- * nanoseconds to a counter value suitable for programming the device.
- * Also, it should adjust ns so that it cooresponds to the actual time
- * that the device will use. */
-static int s626_ns_to_timer(int *nanosec, int round_mode)
-{
-	int divider, base;
-
-	base = 500;		/* 2MHz internal clock */
-
-	switch (round_mode) {
-	case TRIG_ROUND_NEAREST:
-	default:
-		divider = (*nanosec + base / 2) / base;
-		break;
-	case TRIG_ROUND_DOWN:
-		divider = (*nanosec) / base;
-		break;
-	case TRIG_ROUND_UP:
-		divider = (*nanosec + base - 1) / base;
-		break;
-	}
-
-	*nanosec = base * divider;
-	return divider - 1;
-}
-
 static int s626_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_insn *insn, unsigned int *data)
 {
@@ -2128,7 +1781,6 @@ static void s626_dio_init(struct comedi_device *dev)
 		DEBIwrite(dev, diopriv->WRDOut, 0);	/*  Program all outputs */
 		/*  to inactive state. */
 	}
-	DEBUG("s626_dio_init: DIO initialized\n");
 }
 
 /* DIO devices are slightly special.  Although it is possible to
@@ -2141,18 +1793,6 @@ static int s626_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-
-	/* Length of data must be 2 (mask and new data, see below) */
-	if (insn->n == 0)
-		return 0;
-
-	if (insn->n != 2) {
-		printk
-		    ("comedi%d: s626: s626_dio_insn_bits(): Invalid instruction length\n",
-		     dev->minor);
-		return -EINVAL;
-	}
-
 	/*
 	 * The insn data consists of a mask in data[0] and the new data in
 	 * data[1]. The mask defines which bits we are concerning about.
@@ -2173,7 +1813,7 @@ static int s626_dio_insn_bits(struct comedi_device *dev,
 	}
 	data[1] = DEBIread(dev, diopriv->RDDIn);
 
-	return 2;
+	return insn->n;
 }
 
 static int s626_dio_insn_config(struct comedi_device *dev,
@@ -2204,87 +1844,6 @@ static int s626_dio_insn_config(struct comedi_device *dev,
 	return 1;
 }
 
-static int s626_dio_set_irq(struct comedi_device *dev, unsigned int chan)
-{
-	unsigned int group;
-	unsigned int bitmask;
-	unsigned int status;
-
-	/* select dio bank */
-	group = chan / 16;
-	bitmask = 1 << (chan - (16 * group));
-	DEBUG("s626_dio_set_irq: enable interrupt on dio channel %d group %d\n",
-	      chan - (16 * group), group);
-
-	/* set channel to capture positive edge */
-	status = DEBIread(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->RDEdgSel);
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WREdgSel,
-		  bitmask | status);
-
-	/* enable interrupt on selected channel */
-	status = DEBIread(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->RDIntSel);
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WRIntSel,
-		  bitmask | status);
-
-	/* enable edge capture write command */
-	DEBIwrite(dev, LP_MISC1, MISC1_EDCAP);
-
-	/* enable edge capture on selected channel */
-	status = DEBIread(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->RDCapSel);
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WRCapSel,
-		  bitmask | status);
-
-	return 0;
-}
-
-static int s626_dio_reset_irq(struct comedi_device *dev, unsigned int group,
-			      unsigned int mask)
-{
-	DEBUG
-	    ("s626_dio_reset_irq: disable  interrupt on dio channel %d group %d\n",
-	     mask, group);
-
-	/* disable edge capture write command */
-	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
-
-	/* enable edge capture on selected channel */
-	DEBIwrite(dev,
-		  ((struct dio_private *)(dev->subdevices + 2 +
-					  group)->private)->WRCapSel, mask);
-
-	return 0;
-}
-
-static int s626_dio_clear_irq(struct comedi_device *dev)
-{
-	unsigned int group;
-
-	/* disable edge capture write command */
-	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
-
-	for (group = 0; group < S626_DIO_BANKS; group++) {
-		/* clear pending events and interrupt */
-		DEBIwrite(dev,
-			  ((struct dio_private *)(dev->subdevices + 2 +
-						  group)->private)->WRCapSel,
-			  0xffff);
-	}
-
-	return 0;
-}
-
 /* Now this function initializes the value of the counter (data[0])
    and set the subdevice. To complete with trigger and interrupt
    configuration */
@@ -2306,8 +1865,6 @@ static int s626_enc_insn_config(struct comedi_device *dev,
 	uint16_t enab = CLKENAB_ALWAYS;
 	struct enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
 
-	DEBUG("s626_enc_insn_config: encoder config\n");
-
 	/*   (data==NULL) ? (Preloadvalue=0) : (Preloadvalue=data[0]); */
 
 	k->SetMode(dev, k, Setup, TRUE);
@@ -2327,14 +1884,9 @@ static int s626_enc_insn_read(struct comedi_device *dev,
 	int n;
 	struct enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
 
-	DEBUG("s626_enc_insn_read: encoder read channel %d\n",
-	      CR_CHAN(insn->chanspec));
-
 	for (n = 0; n < insn->n; n++)
 		data[n] = ReadLatch(dev, k);
 
-	DEBUG("s626_enc_insn_read: encoder sample %d\n", data[n]);
-
 	return n;
 }
 
@@ -2345,9 +1897,6 @@ static int s626_enc_insn_write(struct comedi_device *dev,
 
 	struct enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
 
-	DEBUG("s626_enc_insn_write: encoder write channel %d\n",
-	      CR_CHAN(insn->chanspec));
-
 	/*  Set the preload register */
 	Preload(dev, k, data[0]);
 
@@ -2357,348 +1906,9 @@ static int s626_enc_insn_write(struct comedi_device *dev,
 	k->PulseIndex(dev, k);
 	k->SetLoadTrig(dev, k, 2);
 
-	DEBUG("s626_enc_insn_write: End encoder write\n");
-
 	return 1;
 }
 
-static void s626_timer_load(struct comedi_device *dev, struct enc_private *k,
-			    int tick)
-{
-	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	/*  Preload upon */
-	    /*  index. */
-	    (INDXSRC_SOFT << BF_INDXSRC) |	/*  Disable hardware index. */
-	    (CLKSRC_TIMER << BF_CLKSRC) |	/*  Operating mode is Timer. */
-	    (CLKPOL_POS << BF_CLKPOL) |	/*  Active high clock. */
-	    (CNTDIR_DOWN << BF_CLKPOL) |	/*  Count direction is Down. */
-	    (CLKMULT_1X << BF_CLKMULT) |	/*  Clock multiplier is 1x. */
-	    (CLKENAB_INDEX << BF_CLKENAB);
-	uint16_t valueSrclatch = LATCHSRC_A_INDXA;
-	/*   uint16_t enab=CLKENAB_ALWAYS; */
-
-	k->SetMode(dev, k, Setup, FALSE);
-
-	/*  Set the preload register */
-	Preload(dev, k, tick);
-
-	/*  Software index pulse forces the preload register to load */
-	/*  into the counter */
-	k->SetLoadTrig(dev, k, 0);
-	k->PulseIndex(dev, k);
-
-	/* set reload on counter overflow */
-	k->SetLoadTrig(dev, k, 1);
-
-	/* set interrupt on overflow */
-	k->SetIntSrc(dev, k, INTSRC_OVER);
-
-	SetLatchSource(dev, k, valueSrclatch);
-	/*   k->SetEnable(dev,k,(uint16_t)(enab != 0)); */
-}
-
-/* ***********  DAC FUNCTIONS *********** */
-
-/*  Slot 0 base settings. */
-#define VECT0	(XSD2 | RSD3 | SIB_A2)
-/*  Slot 0 always shifts in  0xFF and store it to  FB_BUFFER2. */
-
-/*  TrimDac LogicalChan-to-PhysicalChan mapping table. */
-static uint8_t trimchan[] = { 10, 9, 8, 3, 2, 7, 6, 1, 0, 5, 4 };
-
-/*  TrimDac LogicalChan-to-EepromAdrs mapping table. */
-static uint8_t trimadrs[] = { 0x40, 0x41, 0x42, 0x50, 0x51, 0x52, 0x53, 0x60, 0x61, 0x62, 0x63 };
-
-static void LoadTrimDACs(struct comedi_device *dev)
-{
-	register uint8_t i;
-
-	/*  Copy TrimDac setpoint values from EEPROM to TrimDacs. */
-	for (i = 0; i < ARRAY_SIZE(trimchan); i++)
-		WriteTrimDAC(dev, i, I2Cread(dev, trimadrs[i]));
-}
-
-static void WriteTrimDAC(struct comedi_device *dev, uint8_t LogicalChan,
-			 uint8_t DacData)
-{
-	uint32_t chan;
-
-	/*  Save the new setpoint in case the application needs to read it back later. */
-	devpriv->TrimSetpoint[LogicalChan] = (uint8_t) DacData;
-
-	/*  Map logical channel number to physical channel number. */
-	chan = (uint32_t) trimchan[LogicalChan];
-
-	/* Set up TSL2 records for TrimDac write operation.  All slots shift
-	 * 0xFF in from pulled-up SD3 so that the end of the slot sequence
-	 * can be detected.
-	 */
-
-	SETVECT(2, XSD2 | XFIFO_1 | WS3);
-	/* Slot 2: Send high uint8_t to target TrimDac. */
-	SETVECT(3, XSD2 | XFIFO_0 | WS3);
-	/* Slot 3: Send low uint8_t to target TrimDac. */
-	SETVECT(4, XSD2 | XFIFO_3 | WS1);
-	/* Slot 4: Send NOP high uint8_t to DAC0 to keep clock running. */
-	SETVECT(5, XSD2 | XFIFO_2 | WS1 | EOS);
-	/* Slot 5: Send NOP low  uint8_t to DAC0. */
-
-	/* Construct and transmit target DAC's serial packet:
-	 * ( 0000 AAAA ), ( DDDD DDDD ),( 0x00 ),( 0x00 ) where A<3:0> is the
-	 * DAC channel's address, and D<7:0> is the DAC setpoint.  Append a
-	 * WORD value (that writes a channel 0 NOP command to a non-existent
-	 * main DAC channel) that serves to keep the clock running after the
-	 * packet has been sent to the target DAC.
-	 */
-
-	/*  Address the DAC channel within the trimdac device. */
-	SendDAC(dev, ((uint32_t) chan << 8)
-		| (uint32_t) DacData);	/*  Include DAC setpoint data. */
-}
-
-/* **************  EEPROM ACCESS FUNCTIONS  ************** */
-/*  Read uint8_t from EEPROM. */
-
-static uint8_t I2Cread(struct comedi_device *dev, uint8_t addr)
-{
-	uint8_t rtnval;
-
-	/*  Send EEPROM target address. */
-	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CW)
-			 /* Byte2 = I2C command: write to I2C EEPROM  device. */
-			 | I2C_B1(I2C_ATTRSTOP, addr)
-			 /* Byte1 = EEPROM internal target address. */
-			 | I2C_B0(I2C_ATTRNOP, 0))) {	/*  Byte0 = Not sent. */
-		/*  Abort function and declare error if handshake failed. */
-		DEBUG("I2Cread: error handshake I2Cread  a\n");
-		return 0;
-	}
-	/*  Execute EEPROM read. */
-	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CR)
-
-			 /*  Byte2 = I2C */
-			 /*  command: read */
-			 /*  from I2C EEPROM */
-			 /*  device. */
-			 |I2C_B1(I2C_ATTRSTOP, 0)
-
-			 /*  Byte1 receives */
-			 /*  uint8_t from */
-			 /*  EEPROM. */
-			 |I2C_B0(I2C_ATTRNOP, 0))) {	/*  Byte0 = Not  sent. */
-
-		/*  Abort function and declare error if handshake failed. */
-		DEBUG("I2Cread: error handshake I2Cread b\n");
-		return 0;
-	}
-	/*  Return copy of EEPROM value. */
-	rtnval = (uint8_t) (RR7146(P_I2CCTRL) >> 16);
-	return rtnval;
-}
-
-static uint32_t I2Chandshake(struct comedi_device *dev, uint32_t val)
-{
-	/*  Write I2C command to I2C Transfer Control shadow register. */
-	WR7146(P_I2CCTRL, val);
-
-	/*  Upload I2C shadow registers into working registers and wait for */
-	/*  upload confirmation. */
-
-	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
-	while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
-		;
-
-	/*  Wait until I2C bus transfer is finished or an error occurs. */
-	while ((RR7146(P_I2CCTRL) & (I2C_BUSY | I2C_ERR)) == I2C_BUSY)
-		;
-
-	/*  Return non-zero if I2C error occurred. */
-	return RR7146(P_I2CCTRL) & I2C_ERR;
-
-}
-
-/*  Private helper function: Write setpoint to an application DAC channel. */
-
-static void SetDAC(struct comedi_device *dev, uint16_t chan, short dacdata)
-{
-	register uint16_t signmask;
-	register uint32_t WSImage;
-
-	/*  Adjust DAC data polarity and set up Polarity Control Register */
-	/*  image. */
-	signmask = 1 << chan;
-	if (dacdata < 0) {
-		dacdata = -dacdata;
-		devpriv->Dacpol |= signmask;
-	} else
-		devpriv->Dacpol &= ~signmask;
-
-	/*  Limit DAC setpoint value to valid range. */
-	if ((uint16_t) dacdata > 0x1FFF)
-		dacdata = 0x1FFF;
-
-	/* Set up TSL2 records (aka "vectors") for DAC update.  Vectors V2
-	 * and V3 transmit the setpoint to the target DAC.  V4 and V5 send
-	 * data to a non-existent TrimDac channel just to keep the clock
-	 * running after sending data to the target DAC.  This is necessary
-	 * to eliminate the clock glitch that would otherwise occur at the
-	 * end of the target DAC's serial data stream.  When the sequence
-	 * restarts at V0 (after executing V5), the gate array automatically
-	 * disables gating for the DAC clock and all DAC chip selects.
-	 */
-
-	WSImage = (chan & 2) ? WS1 : WS2;
-	/* Choose DAC chip select to be asserted. */
-	SETVECT(2, XSD2 | XFIFO_1 | WSImage);
-	/* Slot 2: Transmit high data byte to target DAC. */
-	SETVECT(3, XSD2 | XFIFO_0 | WSImage);
-	/* Slot 3: Transmit low data byte to target DAC. */
-	SETVECT(4, XSD2 | XFIFO_3 | WS3);
-	/* Slot 4: Transmit to non-existent TrimDac channel to keep clock */
-	SETVECT(5, XSD2 | XFIFO_2 | WS3 | EOS);
-	/* Slot 5: running after writing target DAC's low data byte. */
-
-	/*  Construct and transmit target DAC's serial packet:
-	 * ( A10D DDDD ),( DDDD DDDD ),( 0x0F ),( 0x00 ) where A is chan<0>,
-	 * and D<12:0> is the DAC setpoint.  Append a WORD value (that writes
-	 * to a  non-existent TrimDac channel) that serves to keep the clock
-	 * running after the packet has been sent to the target DAC.
-	 */
-	SendDAC(dev, 0x0F000000
-		/* Continue clock after target DAC data (write to non-existent trimdac). */
-		| 0x00004000
-		/* Address the two main dual-DAC devices (TSL's chip select enables
-		 * target device). */
-		| ((uint32_t) (chan & 1) << 15)
-		/*  Address the DAC channel within the  device. */
-		| (uint32_t) dacdata);	/*  Include DAC setpoint data. */
-
-}
-
-/* Private helper function: Transmit serial data to DAC via Audio
- * channel 2.  Assumes: (1) TSL2 slot records initialized, and (2)
- * Dacpol contains valid target image.
- */
-
-static void SendDAC(struct comedi_device *dev, uint32_t val)
-{
-
-	/* START THE SERIAL CLOCK RUNNING ------------- */
-
-	/* Assert DAC polarity control and enable gating of DAC serial clock
-	 * and audio bit stream signals.  At this point in time we must be
-	 * assured of being in time slot 0.  If we are not in slot 0, the
-	 * serial clock and audio stream signals will be disabled; this is
-	 * because the following DEBIwrite statement (which enables signals
-	 * to be passed through the gate array) would execute before the
-	 * trailing edge of WS1/WS3 (which turns off the signals), thus
-	 * causing the signals to be inactive during the DAC write.
-	 */
-	DEBIwrite(dev, LP_DACPOL, devpriv->Dacpol);
-
-	/* TRANSFER OUTPUT DWORD VALUE INTO A2'S OUTPUT FIFO ---------------- */
-
-	/* Copy DAC setpoint value to DAC's output DMA buffer. */
-
-	/* WR7146( (uint32_t)devpriv->pDacWBuf, val ); */
-	*devpriv->pDacWBuf = val;
-
-	/* enab the output DMA transfer.  This will cause the DMAC to copy
-	 * the DAC's data value to A2's output FIFO.  The DMA transfer will
-	 * then immediately terminate because the protection address is
-	 * reached upon transfer of the first DWORD value.
-	 */
-	MC_ENABLE(P_MC1, MC1_A2OUT);
-
-	/*  While the DMA transfer is executing ... */
-
-	/* Reset Audio2 output FIFO's underflow flag (along with any other
-	 * FIFO underflow/overflow flags).  When set, this flag will
-	 * indicate that we have emerged from slot 0.
-	 */
-	WR7146(P_ISR, ISR_AFOU);
-
-	/* Wait for the DMA transfer to finish so that there will be data
-	 * available in the FIFO when time slot 1 tries to transfer a DWORD
-	 * from the FIFO to the output buffer register.  We test for DMA
-	 * Done by polling the DMAC enable flag; this flag is automatically
-	 * cleared when the transfer has finished.
-	 */
-	while ((RR7146(P_MC1) & MC1_A2OUT) != 0)
-		;
-
-	/* START THE OUTPUT STREAM TO THE TARGET DAC -------------------- */
-
-	/* FIFO data is now available, so we enable execution of time slots
-	 * 1 and higher by clearing the EOS flag in slot 0.  Note that SD3
-	 * will be shifted in and stored in FB_BUFFER2 for end-of-slot-list
-	 * detection.
-	 */
-	SETVECT(0, XSD2 | RSD3 | SIB_A2);
-
-	/* Wait for slot 1 to execute to ensure that the Packet will be
-	 * transmitted.  This is detected by polling the Audio2 output FIFO
-	 * underflow flag, which will be set when slot 1 execution has
-	 * finished transferring the DAC's data DWORD from the output FIFO
-	 * to the output buffer register.
-	 */
-	while ((RR7146(P_SSR) & SSR_AF2_OUT) == 0)
-		;
-
-	/* Set up to trap execution at slot 0 when the TSL sequencer cycles
-	 * back to slot 0 after executing the EOS in slot 5.  Also,
-	 * simultaneously shift out and in the 0x00 that is ALWAYS the value
-	 * stored in the last byte to be shifted out of the FIFO's DWORD
-	 * buffer register.
-	 */
-	SETVECT(0, XSD2 | XFIFO_2 | RSD2 | SIB_A2 | EOS);
-
-	/* WAIT FOR THE TRANSACTION TO FINISH ----------------------- */
-
-	/* Wait for the TSL to finish executing all time slots before
-	 * exiting this function.  We must do this so that the next DAC
-	 * write doesn't start, thereby enabling clock/chip select signals:
-	 *
-	 * 1. Before the TSL sequence cycles back to slot 0, which disables
-	 *    the clock/cs signal gating and traps slot // list execution.
-	 *    we have not yet finished slot 5 then the clock/cs signals are
-	 *    still gated and we have not finished transmitting the stream.
-	 *
-	 * 2. While slots 2-5 are executing due to a late slot 0 trap.  In
-	 *    this case, the slot sequence is currently repeating, but with
-	 *    clock/cs signals disabled.  We must wait for slot 0 to trap
-	 *    execution before setting up the next DAC setpoint DMA transfer
-	 *    and enabling the clock/cs signals.  To detect the end of slot 5,
-	 *    we test for the FB_BUFFER2 MSB contents to be equal to 0xFF.  If
-	 *    the TSL has not yet finished executing slot 5 ...
-	 */
-	if ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) {
-		/* The trap was set on time and we are still executing somewhere
-		 * in slots 2-5, so we now wait for slot 0 to execute and trap
-		 * TSL execution.  This is detected when FB_BUFFER2 MSB changes
-		 * from 0xFF to 0x00, which slot 0 causes to happen by shifting
-		 * out/in on SD2 the 0x00 that is always referenced by slot 5.
-		 */
-		while ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0)
-			;
-	}
-	/* Either (1) we were too late setting the slot 0 trap; the TSL
-	 * sequencer restarted slot 0 before we could set the EOS trap flag,
-	 * or (2) we were not late and execution is now trapped at slot 0.
-	 * In either case, we must now change slot 0 so that it will store
-	 * value 0xFF (instead of 0x00) to FB_BUFFER2 next time it executes.
-	 * In order to do this, we reprogram slot 0 so that it will shift in
-	 * SD3, which is driven only by a pull-up resistor.
-	 */
-	SETVECT(0, RSD3 | SIB_A2 | EOS);
-
-	/* Wait for slot 0 to execute, at which time the TSL is setup for
-	 * the next DAC write.  This is detected when FB_BUFFER2 MSB changes
-	 * from 0x00 to 0xFF.
-	 */
-	while ((RR7146(P_FB_BUFFER2) & 0xFF000000) == 0)
-		;
-}
-
 static void WriteMISC2(struct comedi_device *dev, uint16_t NewImage)
 {
 	DEBIwrite(dev, LP_MISC1, MISC1_WENABLE);	/*  enab writes to */
@@ -2707,83 +1917,12 @@ static void WriteMISC2(struct comedi_device *dev, uint16_t NewImage)
 	DEBIwrite(dev, LP_MISC1, MISC1_WDISABLE);	/*  Disable writes to MISC2. */
 }
 
-/*  Initialize the DEBI interface for all transfers. */
-
-static uint16_t DEBIread(struct comedi_device *dev, uint16_t addr)
-{
-	uint16_t retval;
-
-	/*  Set up DEBI control register value in shadow RAM. */
-	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
-
-	/*  Execute the DEBI transfer. */
-	DEBItransfer(dev);
-
-	/*  Fetch target register value. */
-	retval = (uint16_t) RR7146(P_DEBIAD);
-
-	/*  Return register value. */
-	return retval;
-}
-
-/*  Execute a DEBI transfer.  This must be called from within a */
-/*  critical section. */
-static void DEBItransfer(struct comedi_device *dev)
-{
-	/*  Initiate upload of shadow RAM to DEBI control register. */
-	MC_ENABLE(P_MC2, MC2_UPLD_DEBI);
-
-	/*  Wait for completion of upload from shadow RAM to DEBI control */
-	/*  register. */
-	while (!MC_TEST(P_MC2, MC2_UPLD_DEBI))
-		;
-
-	/*  Wait until DEBI transfer is done. */
-	while (RR7146(P_PSR) & PSR_DEBI_S)
-		;
-}
-
-/*  Write a value to a gate array register. */
-static void DEBIwrite(struct comedi_device *dev, uint16_t addr, uint16_t wdata)
-{
-
-	/*  Set up DEBI control register value in shadow RAM. */
-	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
-	WR7146(P_DEBIAD, wdata);
-
-	/*  Execute the DEBI transfer. */
-	DEBItransfer(dev);
-}
-
-/* Replace the specified bits in a gate array register.  Imports: mask
- * specifies bits that are to be preserved, wdata is new value to be
- * or'd with the masked original.
- */
-static void DEBIreplace(struct comedi_device *dev, uint16_t addr, uint16_t mask,
-			uint16_t wdata)
-{
-
-	/*  Copy target gate array register into P_DEBIAD register. */
-	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
-	/* Set up DEBI control reg value in shadow RAM. */
-	DEBItransfer(dev);	/*  Execute the DEBI Read transfer. */
-
-	/*  Write back the modified image. */
-	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
-	/* Set up DEBI control reg value in shadow  RAM. */
-
-	WR7146(P_DEBIAD, wdata | ((uint16_t) RR7146(P_DEBIAD) & mask));
-	/* Modify the register image. */
-	DEBItransfer(dev);	/*  Execute the DEBI Write transfer. */
-}
-
 static void CloseDMAB(struct comedi_device *dev, struct bufferDMA *pdma,
 		      size_t bsize)
 {
 	void *vbptr;
 	dma_addr_t vpptr;
 
-	DEBUG("CloseDMAB: Entering S626DRV_CloseDMAB():\n");
 	if (pdma == NULL)
 		return;
 	/* find the matching allocation from the board struct */
@@ -2792,44 +1931,13 @@ static void CloseDMAB(struct comedi_device *dev, struct bufferDMA *pdma,
 	vpptr = pdma->PhysicalBase;
 	if (vbptr) {
 		pci_free_consistent(devpriv->pdev, bsize, vbptr, vpptr);
-		pdma->LogicalBase = 0;
+		pdma->LogicalBase = NULL;
 		pdma->PhysicalBase = 0;
-
-		DEBUG("CloseDMAB(): Logical=%p, bsize=%d, Physical=0x%x\n",
-		      vbptr, bsize, (uint32_t) vpptr);
 	}
 }
 
-/* ******  COUNTER FUNCTIONS  ******* */
-/* All counter functions address a specific counter by means of the
- * "Counter" argument, which is a logical counter number.  The Counter
- * argument may have any of the following legal values: 0=0A, 1=1A,
- * 2=2A, 3=0B, 4=1B, 5=2B.
- */
-
-/* Forward declarations for functions that are common to both A and B counters: */
-
 /* ******  PRIVATE COUNTER FUNCTIONS ****** */
 
-/*  Read a counter's output latch. */
-
-static uint32_t ReadLatch(struct comedi_device *dev, struct enc_private *k)
-{
-	register uint32_t value;
-	/* DEBUG FIXME DEBUG("ReadLatch: Read Latch enter\n"); */
-
-	/*  Latch counts and fetch LSW of latched counts value. */
-	value = (uint32_t) DEBIread(dev, k->MyLatchLsw);
-
-	/*  Fetch MSW of latched counts and combine with LSW. */
-	value |= ((uint32_t) DEBIread(dev, k->MyLatchLsw + 2) << 16);
-
-	/*  DEBUG FIXME DEBUG("ReadLatch: Read Latch exit\n"); */
-
-	/*  Return latched counts. */
-	return value;
-}
-
 /*  Reset a counter's index and overflow event capture flags. */
 
 static void ResetCapFlags_A(struct comedi_device *dev, struct enc_private *k)
@@ -3055,7 +2163,6 @@ static void SetMode_B(struct comedi_device *dev, struct enc_private *k,
 static void SetEnable_A(struct comedi_device *dev, struct enc_private *k,
 			uint16_t enab)
 {
-	DEBUG("SetEnable_A: SetEnable_A enter 3541\n");
 	DEBIreplace(dev, k->MyCRB,
 		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)),
 		    (uint16_t) (enab << CRBBIT_CLKENAB_A));
@@ -3079,22 +2186,6 @@ static uint16_t GetEnable_B(struct comedi_device *dev, struct enc_private *k)
 	return (DEBIread(dev, k->MyCRB) >> CRBBIT_CLKENAB_B) & 1;
 }
 
-/* Return/set a counter pair's latch trigger source.  0: On read
- * access, 1: A index latches A, 2: B index latches B, 3: A overflow
- * latches B.
- */
-
-static void SetLatchSource(struct comedi_device *dev, struct enc_private *k,
-			   uint16_t value)
-{
-	DEBUG("SetLatchSource: SetLatchSource enter 3550\n");
-	DEBIreplace(dev, k->MyCRB,
-		    (uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_LATCHSRC)),
-		    (uint16_t) (value << CRBBIT_LATCHSRC));
-
-	DEBUG("SetLatchSource: SetLatchSource exit\n");
-}
-
 /*
  * static uint16_t GetLatchSource(struct comedi_device *dev, struct enc_private *k )
  * {
@@ -3240,7 +2331,6 @@ static uint16_t GetIntSrc_B(struct comedi_device *dev, struct enc_private *k)
 
 /* static void SetIndexSrc(struct comedi_device *dev, struct enc_private *k, uint16_t value )  */
 /* { */
-/*   DEBUG("SetIndexSrc: set index src enter 3700\n"); */
 /*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_INDXSRC ) | ( (value != 0) << STDBIT_INDXSRC ) ), FALSE ); */
 /* } */
 
@@ -3255,11 +2345,8 @@ static void PulseIndex_A(struct comedi_device *dev, struct enc_private *k)
 {
 	register uint16_t cra;
 
-	DEBUG("PulseIndex_A: pulse index enter\n");
-
 	cra = DEBIread(dev, k->MyCRA);	/*  Pulse index. */
 	DEBIwrite(dev, k->MyCRA, (uint16_t) (cra ^ CRAMSK_INDXPOL_A));
-	DEBUG("PulseIndex_A: pulse index step1\n");
 	DEBIwrite(dev, k->MyCRA, cra);
 }
 
@@ -3272,17 +2359,99 @@ static void PulseIndex_B(struct comedi_device *dev, struct enc_private *k)
 	DEBIwrite(dev, k->MyCRB, crb);
 }
 
-/*  Write value into counter preload register. */
-
-static void Preload(struct comedi_device *dev, struct enc_private *k,
-		    uint32_t value)
-{
-	DEBUG("Preload: preload enter\n");
-	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw), (uint16_t) value);	/*  Write value to preload register. */
-	DEBUG("Preload: preload step 1\n");
-	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw + 2),
-		  (uint16_t) (value >> 16));
-}
+static struct enc_private enc_private_data[] = {
+	{
+		.GetEnable	= GetEnable_A,
+		.GetIntSrc	= GetIntSrc_A,
+		.GetLoadTrig	= GetLoadTrig_A,
+		.GetMode	= GetMode_A,
+		.PulseIndex	= PulseIndex_A,
+		.SetEnable	= SetEnable_A,
+		.SetIntSrc	= SetIntSrc_A,
+		.SetLoadTrig	= SetLoadTrig_A,
+		.SetMode	= SetMode_A,
+		.ResetCapFlags	= ResetCapFlags_A,
+		.MyCRA		= LP_CR0A,
+		.MyCRB		= LP_CR0B,
+		.MyLatchLsw	= LP_CNTR0ALSW,
+		.MyEventBits	= EVBITS(0),
+	}, {
+		.GetEnable	= GetEnable_A,
+		.GetIntSrc	= GetIntSrc_A,
+		.GetLoadTrig	= GetLoadTrig_A,
+		.GetMode	= GetMode_A,
+		.PulseIndex	= PulseIndex_A,
+		.SetEnable	= SetEnable_A,
+		.SetIntSrc	= SetIntSrc_A,
+		.SetLoadTrig	= SetLoadTrig_A,
+		.SetMode	= SetMode_A,
+		.ResetCapFlags	= ResetCapFlags_A,
+		.MyCRA		= LP_CR1A,
+		.MyCRB		= LP_CR1B,
+		.MyLatchLsw	= LP_CNTR1ALSW,
+		.MyEventBits	= EVBITS(1),
+	}, {
+		.GetEnable	= GetEnable_A,
+		.GetIntSrc	= GetIntSrc_A,
+		.GetLoadTrig	= GetLoadTrig_A,
+		.GetMode	= GetMode_A,
+		.PulseIndex	= PulseIndex_A,
+		.SetEnable	= SetEnable_A,
+		.SetIntSrc	= SetIntSrc_A,
+		.SetLoadTrig	= SetLoadTrig_A,
+		.SetMode	= SetMode_A,
+		.ResetCapFlags	= ResetCapFlags_A,
+		.MyCRA		= LP_CR2A,
+		.MyCRB		= LP_CR2B,
+		.MyLatchLsw	= LP_CNTR2ALSW,
+		.MyEventBits	= EVBITS(2),
+	}, {
+		.GetEnable	= GetEnable_B,
+		.GetIntSrc	= GetIntSrc_B,
+		.GetLoadTrig	= GetLoadTrig_B,
+		.GetMode	= GetMode_B,
+		.PulseIndex	= PulseIndex_B,
+		.SetEnable	= SetEnable_B,
+		.SetIntSrc	= SetIntSrc_B,
+		.SetLoadTrig	= SetLoadTrig_B,
+		.SetMode	= SetMode_B,
+		.ResetCapFlags	= ResetCapFlags_B,
+		.MyCRA		= LP_CR0A,
+		.MyCRB		= LP_CR0B,
+		.MyLatchLsw	= LP_CNTR0BLSW,
+		.MyEventBits	= EVBITS(3),
+	}, {
+		.GetEnable	= GetEnable_B,
+		.GetIntSrc	= GetIntSrc_B,
+		.GetLoadTrig	= GetLoadTrig_B,
+		.GetMode	= GetMode_B,
+		.PulseIndex	= PulseIndex_B,
+		.SetEnable	= SetEnable_B,
+		.SetIntSrc	= SetIntSrc_B,
+		.SetLoadTrig	= SetLoadTrig_B,
+		.SetMode	= SetMode_B,
+		.ResetCapFlags	= ResetCapFlags_B,
+		.MyCRA		= LP_CR1A,
+		.MyCRB		= LP_CR1B,
+		.MyLatchLsw	= LP_CNTR1BLSW,
+		.MyEventBits	= EVBITS(4),
+	}, {
+		.GetEnable	= GetEnable_B,
+		.GetIntSrc	= GetIntSrc_B,
+		.GetLoadTrig	= GetLoadTrig_B,
+		.GetMode	= GetMode_B,
+		.PulseIndex	= PulseIndex_B,
+		.SetEnable	= SetEnable_B,
+		.SetIntSrc	= SetIntSrc_B,
+		.SetLoadTrig	= SetLoadTrig_B,
+		.SetMode	= SetMode_B,
+		.ResetCapFlags	= ResetCapFlags_B,
+		.MyCRA		= LP_CR2A,
+		.MyCRB		= LP_CR2B,
+		.MyLatchLsw	= LP_CNTR2BLSW,
+		.MyEventBits	= EVBITS(5),
+	},
+};
 
 static void CountersInit(struct comedi_device *dev)
 {
@@ -3305,8 +2474,505 @@ static void CountersInit(struct comedi_device *dev)
 		k->ResetCapFlags(dev, k);
 		k->SetEnable(dev, k, CLKENAB_ALWAYS);
 	}
-	DEBUG("CountersInit: counters initialized\n");
+}
+
+static struct pci_dev *s626_find_pci(struct comedi_device *dev,
+				     struct comedi_devconfig *it)
+{
+	struct pci_dev *pcidev = NULL;
+	int bus = it->options[0];
+	int slot = it->options[1];
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(s626_boards) && !pcidev; i++) {
+		do {
+			pcidev = pci_get_subsys(s626_boards[i].vendor_id,
+						s626_boards[i].device_id,
+						s626_boards[i].subvendor_id,
+						s626_boards[i].subdevice_id,
+						pcidev);
+
+			if ((bus || slot) && pcidev) {
+				/* matches requested bus/slot */
+				if (pcidev->bus->number == bus &&
+				    PCI_SLOT(pcidev->devfn) == slot)
+					break;
+			} else {
+				break;
+			}
+		} while (1);
+	}
+	return pcidev;
+}
+
+static int s626_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+/*   uint8_t	PollList; */
+/*   uint16_t	AdcData; */
+/*   uint16_t	StartVal; */
+/*   uint16_t	index; */
+/*   unsigned int data[16]; */
+	int result;
+	int i;
+	int ret;
+	resource_size_t resourceStart;
+	dma_addr_t appdma;
+	struct comedi_subdevice *s;
+
+	if (alloc_private(dev, sizeof(struct s626_private)) < 0)
+		return -ENOMEM;
+
+	devpriv->pdev = s626_find_pci(dev, it);
+	if (!devpriv->pdev) {
+		printk(KERN_ERR "s626_attach: Board not present!!!\n");
+		return -ENODEV;
+	}
+
+	result = comedi_pci_enable(devpriv->pdev, "s626");
+	if (result < 0) {
+		printk(KERN_ERR "s626_attach: comedi_pci_enable fails\n");
+		return -ENODEV;
+	}
+	devpriv->got_regions = 1;
+
+	resourceStart = pci_resource_start(devpriv->pdev, 0);
+
+	devpriv->base_addr = ioremap(resourceStart, SIZEOF_ADDRESS_SPACE);
+	if (devpriv->base_addr == NULL) {
+		printk(KERN_ERR "s626_attach: IOREMAP failed\n");
+		return -ENODEV;
+	}
+
+	if (devpriv->base_addr) {
+		/* disable master interrupt */
+		writel(0, devpriv->base_addr + P_IER);
+
+		/* soft reset */
+		writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+
+		/* DMA FIXME DMA// */
+
+		/* adc buffer allocation */
+		devpriv->allocatedBuf = 0;
+
+		devpriv->ANABuf.LogicalBase =
+		    pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE, &appdma);
+
+		if (devpriv->ANABuf.LogicalBase == NULL) {
+			printk(KERN_ERR "s626_attach: DMA Memory mapping error\n");
+			return -ENOMEM;
+		}
+
+		devpriv->ANABuf.PhysicalBase = appdma;
+
+		devpriv->allocatedBuf++;
+
+		devpriv->RPSBuf.LogicalBase =
+		    pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE, &appdma);
+
+		if (devpriv->RPSBuf.LogicalBase == NULL) {
+			printk(KERN_ERR "s626_attach: DMA Memory mapping error\n");
+			return -ENOMEM;
+		}
+
+		devpriv->RPSBuf.PhysicalBase = appdma;
+
+		devpriv->allocatedBuf++;
+
+	}
+
+	dev->board_ptr = s626_boards;
+	dev->board_name = thisboard->name;
+
+	ret = comedi_alloc_subdevices(dev, 6);
+	if (ret)
+		return ret;
+
+	dev->iobase = (unsigned long)devpriv->base_addr;
+	dev->irq = devpriv->pdev->irq;
+
+	/* set up interrupt handler */
+	if (dev->irq == 0) {
+		printk(KERN_ERR " unknown irq (bad)\n");
+	} else {
+		ret = request_irq(dev->irq, s626_irq_handler, IRQF_SHARED,
+				  "s626", dev);
+
+		if (ret < 0) {
+			printk(KERN_ERR " irq not available\n");
+			dev->irq = 0;
+		}
+	}
+
+	s = dev->subdevices + 0;
+	/* analog input subdevice */
+	dev->read_subdev = s;
+	/* we support single-ended (ground) and differential */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (0xffff >> 2);
+	s->range_table = &s626_range_table;
+	s->len_chanlist = thisboard->ai_chans;	/* This is the maximum chanlist
+						   length that the board can
+						   handle */
+	s->insn_config = s626_ai_insn_config;
+	s->insn_read = s626_ai_insn_read;
+	s->do_cmd = s626_ai_cmd;
+	s->do_cmdtest = s626_ai_cmdtest;
+	s->cancel = s626_ai_cancel;
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = thisboard->ao_chans;
+	s->maxdata = (0x3fff);
+	s->range_table = &range_bipolar10;
+	s->insn_write = s626_ao_winsn;
+	s->insn_read = s626_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_A;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 3;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_B;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 4;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_C;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 5;
+	/* encoder (counter) subdevice */
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
+	s->n_chan = thisboard->enc_chans;
+	s->private = enc_private_data;
+	s->insn_config = s626_enc_insn_config;
+	s->insn_read = s626_enc_insn_read;
+	s->insn_write = s626_enc_insn_write;
+	s->maxdata = 0xffffff;
+	s->range_table = &range_unknown;
+
+	/* stop ai_command */
+	devpriv->ai_cmd_running = 0;
+
+	if (devpriv->base_addr && (devpriv->allocatedBuf == 2)) {
+		dma_addr_t pPhysBuf;
+		uint16_t chan;
+
+		/*  enab DEBI and audio pins, enable I2C interface. */
+		MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
+		/*  Configure DEBI operating mode. */
+		WR7146(P_DEBICFG, DEBI_CFG_SLAVE16	/*  Local bus is 16 */
+		       /*  bits wide. */
+		       | (DEBI_TOUT << DEBI_CFG_TOUT_BIT)
+
+		       /*  Declare DEBI */
+		       /*  transfer timeout */
+		       /*  interval. */
+		       |DEBI_SWAP	/*  Set up byte lane */
+		       /*  steering. */
+		       | DEBI_CFG_INTEL);	/*  Intel-compatible */
+		/*  local bus (DEBI */
+		/*  never times out). */
+
+		/* DEBI INIT S626 WR7146( P_DEBICFG, DEBI_CFG_INTEL | DEBI_CFG_TOQ */
+		/* | DEBI_CFG_INCQ| DEBI_CFG_16Q); //end */
+
+		/*  Paging is disabled. */
+		WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);	/*  Disable MMU paging. */
+
+		/*  Init GPIO so that ADC Start* is negated. */
+		WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
+
+		/* IsBoardRevA is a boolean that indicates whether the board is RevA.
+		 *
+		 * VERSION 2.01 CHANGE: REV A & B BOARDS NOW SUPPORTED BY DYNAMIC
+		 * EEPROM ADDRESS SELECTION.  Initialize the I2C interface, which
+		 * is used to access the onboard serial EEPROM.  The EEPROM's I2C
+		 * DeviceAddress is hardwired to a value that is dependent on the
+		 * 626 board revision.  On all board revisions, the EEPROM stores
+		 * TrimDAC calibration constants for analog I/O.  On RevB and
+		 * higher boards, the DeviceAddress is hardwired to 0 to enable
+		 * the EEPROM to also store the PCI SubVendorID and SubDeviceID;
+		 * this is the address at which the SAA7146 expects a
+		 * configuration EEPROM to reside.  On RevA boards, the EEPROM
+		 * device address, which is hardwired to 4, prevents the SAA7146
+		 * from retrieving PCI sub-IDs, so the SAA7146 uses its built-in
+		 * default values, instead.
+		 */
+
+		/*     devpriv->I2Cards= IsBoardRevA ? 0xA8 : 0xA0; // Set I2C EEPROM */
+		/*  DeviceType (0xA0) */
+		/*  and DeviceAddress<<1. */
+
+		devpriv->I2CAdrs = 0xA0;	/*  I2C device address for onboard */
+		/*  eeprom(revb) */
+
+		/*  Issue an I2C ABORT command to halt any I2C operation in */
+		/* progress and reset BUSY flag. */
+		WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);
+		/*  Write I2C control: abort any I2C activity. */
+		MC_ENABLE(P_MC2, MC2_UPLD_IIC);
+		/*  Invoke command  upload */
+		while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0)
+			;
+		/*  and wait for upload to complete. */
+
+		/* Per SAA7146 data sheet, write to STATUS reg twice to
+		 * reset all  I2C error flags. */
+		for (i = 0; i < 2; i++) {
+			WR7146(P_I2CSTAT, I2C_CLKSEL);
+			/*  Write I2C control: reset  error flags. */
+			MC_ENABLE(P_MC2, MC2_UPLD_IIC);	/*  Invoke command upload */
+			while (!MC_TEST(P_MC2, MC2_UPLD_IIC))
+				;
+			/* and wait for upload to complete. */
+		}
+
+		/* Init audio interface functional attributes: set DAC/ADC
+		 * serial clock rates, invert DAC serial clock so that
+		 * DAC data setup times are satisfied, enable DAC serial
+		 * clock out.
+		 */
+
+		WR7146(P_ACON2, ACON2_INIT);
+
+		/* Set up TSL1 slot list, which is used to control the
+		 * accumulation of ADC data: RSD1 = shift data in on SD1.
+		 * SIB_A1  = store data uint8_t at next available location in
+		 * FB BUFFER1  register. */
+		WR7146(P_TSL1, RSD1 | SIB_A1);
+		/*  Fetch ADC high data uint8_t. */
+		WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);
+		/*  Fetch ADC low data uint8_t; end of TSL1. */
+
+		/*  enab TSL1 slot list so that it executes all the time. */
+		WR7146(P_ACON1, ACON1_ADCSTART);
+
+		/*  Initialize RPS registers used for ADC. */
+
+		/* Physical start of RPS program. */
+		WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+		WR7146(P_RPSPAGE1, 0);
+		/*  RPS program performs no explicit mem writes. */
+		WR7146(P_RPS1_TOUT, 0);	/*  Disable RPS timeouts. */
+
+		/* SAA7146 BUG WORKAROUND.  Initialize SAA7146 ADC interface
+		 * to a known state by invoking ADCs until FB BUFFER 1
+		 * register shows that it is correctly receiving ADC data.
+		 * This is necessary because the SAA7146 ADC interface does
+		 * not start up in a defined state after a PCI reset.
+		 */
+
+/*     PollList = EOPL;		// Create a simple polling */
+/*				// list for analog input */
+/*				// channel 0. */
+/*     ResetADC( dev, &PollList ); */
+
+/*     s626_ai_rinsn(dev,dev->subdevices,NULL,data); //( &AdcData ); // */
+/*							//Get initial ADC */
+/*							//value. */
+
+/*     StartVal = data[0]; */
+
+/*     // VERSION 2.01 CHANGE: TIMEOUT ADDED TO PREVENT HANGED EXECUTION. */
+/*     // Invoke ADCs until the new ADC value differs from the initial */
+/*     // value or a timeout occurs.  The timeout protects against the */
+/*     // possibility that the driver is restarting and the ADC data is a */
+/*     // fixed value resulting from the applied ADC analog input being */
+/*     // unusually quiet or at the rail. */
+
+/*     for ( index = 0; index < 500; index++ ) */
+/*       { */
+/*	s626_ai_rinsn(dev,dev->subdevices,NULL,data); */
+/*	AdcData = data[0];	//ReadADC(  &AdcData ); */
+/*	if ( AdcData != StartVal ) */
+/*		break; */
+/*       } */
+
+		/*  end initADC */
+
+		/*  init the DAC interface */
+
+		/* Init Audio2's output DMAC attributes: burst length = 1
+		 * DWORD,  threshold = 1 DWORD.
+		 */
+		WR7146(P_PCI_BT_A, 0);
+
+		/* Init Audio2's output DMA physical addresses.  The protection
+		 * address is set to 1 DWORD past the base address so that a
+		 * single DWORD will be transferred each time a DMA transfer is
+		 * enabled. */
+
+		pPhysBuf =
+		    devpriv->ANABuf.PhysicalBase +
+		    (DAC_WDMABUF_OS * sizeof(uint32_t));
+
+		WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);	/*  Buffer base adrs. */
+		WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));	/*  Protection address. */
+
+		/* Cache Audio2's output DMA buffer logical address.  This is
+		 * where DAC data is buffered for A2 output DMA transfers. */
+		devpriv->pDacWBuf =
+		    (uint32_t *) devpriv->ANABuf.LogicalBase + DAC_WDMABUF_OS;
+
+		/* Audio2's output channels does not use paging.  The protection
+		 * violation handling bit is set so that the DMAC will
+		 * automatically halt and its PCI address pointer will be reset
+		 * when the protection address is reached. */
+
+		WR7146(P_PAGEA2_OUT, 8);
+
+		/* Initialize time slot list 2 (TSL2), which is used to control
+		 * the clock generation for and serialization of data to be sent
+		 * to the DAC devices.  Slot 0 is a NOP that is used to trap TSL
+		 * execution; this permits other slots to be safely modified
+		 * without first turning off the TSL sequencer (which is
+		 * apparently impossible to do).  Also, SD3 (which is driven by a
+		 * pull-up resistor) is shifted in and stored to the MSB of
+		 * FB_BUFFER2 to be used as evidence that the slot sequence has
+		 * not yet finished executing.
+		 */
+
+		SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);
+		/*  Slot 0: Trap TSL execution, shift 0xFF into FB_BUFFER2. */
+
+		/* Initialize slot 1, which is constant.  Slot 1 causes a
+		 * DWORD to be transferred from audio channel 2's output FIFO
+		 * to the FIFO's output buffer so that it can be serialized
+		 * and sent to the DAC during subsequent slots.  All remaining
+		 * slots are dynamically populated as required by the target
+		 * DAC device.
+		 */
+		SETVECT(1, LF_A2);
+		/*  Slot 1: Fetch DWORD from Audio2's output FIFO. */
+
+		/*  Start DAC's audio interface (TSL2) running. */
+		WR7146(P_ACON1, ACON1_DACSTART);
+
+		/* end init DAC interface */
+
+		/* Init Trim DACs to calibrated values.  Do it twice because the
+		 * SAA7146 audio channel does not always reset properly and
+		 * sometimes causes the first few TrimDAC writes to malfunction.
+		 */
+
+		LoadTrimDACs(dev);
+		LoadTrimDACs(dev);	/*  Insurance. */
+
+		/* Manually init all gate array hardware in case this is a soft
+		 * reset (we have no way of determining whether this is a warm
+		 * or cold start).  This is necessary because the gate array will
+		 * reset only in response to a PCI hard reset; there is no soft
+		 * reset function. */
+
+		/* Init all DAC outputs to 0V and init all DAC setpoint and
+		 * polarity images.
+		 */
+		for (chan = 0; chan < S626_DAC_CHANNELS; chan++)
+			SetDAC(dev, chan, 0);
+
+		/* Init image of WRMISC2 Battery Charger Enabled control bit.
+		 * This image is used when the state of the charger control bit,
+		 * which has no direct hardware readback mechanism, is queried.
+		 */
+		devpriv->ChargeEnabled = 0;
+
+		/* Init image of watchdog timer interval in WRMISC2.  This image
+		 * maintains the value of the control bits of MISC2 are
+		 * continuously reset to zero as long as the WD timer is disabled.
+		 */
+		devpriv->WDInterval = 0;
+
+		/* Init Counter Interrupt enab mask for RDMISC2.  This mask is
+		 * applied against MISC2 when testing to determine which timer
+		 * events are requesting interrupt service.
+		 */
+		devpriv->CounterIntEnabs = 0;
+
+		/*  Init counters. */
+		CountersInit(dev);
+
+		/* Without modifying the state of the Battery Backup enab, disable
+		 * the watchdog timer, set DIO channels 0-5 to operate in the
+		 * standard DIO (vs. counter overflow) mode, disable the battery
+		 * charger, and reset the watchdog interval selector to zero.
+		 */
+		WriteMISC2(dev, (uint16_t) (DEBIread(dev,
+						     LP_RDMISC2) &
+					    MISC2_BATT_ENABLE));
+
+		/*  Initialize the digital I/O subsystem. */
+		s626_dio_init(dev);
+
+		/* enable interrupt test */
+		/*  writel(IRQ_GPIO3 | IRQ_RPS1,devpriv->base_addr+P_IER); */
+	}
+
+	return 1;
+}
+
+static void s626_detach(struct comedi_device *dev)
+{
+	if (devpriv) {
+		/* stop ai_command */
+		devpriv->ai_cmd_running = 0;
+
+		if (devpriv->base_addr) {
+			/* interrupt mask */
+			WR7146(P_IER, 0);	/*  Disable master interrupt. */
+			WR7146(P_ISR, IRQ_GPIO3 | IRQ_RPS1);	/*  Clear board's IRQ status flag. */
+
+			/*  Disable the watchdog timer and battery charger. */
+			WriteMISC2(dev, 0);
 
+			/*  Close all interfaces on 7146 device. */
+			WR7146(P_MC1, MC1_SHUTDOWN);
+			WR7146(P_ACON1, ACON1_BASE);
+
+			CloseDMAB(dev, &devpriv->RPSBuf, DMABUF_SIZE);
+			CloseDMAB(dev, &devpriv->ANABuf, DMABUF_SIZE);
+		}
+
+		if (dev->irq)
+			free_irq(dev->irq, dev);
+		if (devpriv->base_addr)
+			iounmap(devpriv->base_addr);
+		if (devpriv->pdev) {
+			if (devpriv->got_regions)
+				comedi_pci_disable(devpriv->pdev);
+			pci_dev_put(devpriv->pdev);
+		}
+	}
 }
 
 static struct comedi_driver s626_driver = {
diff --git a/drivers/staging/comedi/drivers/s626.h b/drivers/staging/comedi/drivers/s626.h
index 2d1afecbbb6..8a8f196cf15 100644
--- a/drivers/staging/comedi/drivers/s626.h
+++ b/drivers/staging/comedi/drivers/s626.h
@@ -62,12 +62,6 @@
    comedi_do_insn(cf,&insn); // executing configuration
 */
 
-#ifdef _DEBUG_
-#define DEBUG(...);        printk(__VA_ARGS__);
-#else
-#define DEBUG(...)
-#endif
-
 #if !defined(TRUE)
 #define TRUE    (1)
 #endif
@@ -76,11 +70,7 @@
 #define FALSE   (0)
 #endif
 
-#if !defined(INLINE)
-#define INLINE static __inline
-#endif
-
-#include<linux/slab.h>
+#include <linux/slab.h>
 
 #define S626_SIZE 0x0200
 #define SIZEOF_ADDRESS_SPACE		0x0200
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index 6342bc5ddb3..c18314be8c8 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -47,11 +47,6 @@ struct serial2002_board {
 	const char *name;
 };
 
-/*
- * Useful for shorthand access to the particular board structure
- */
-#define thisboard ((const struct serial2002_board *)dev->board_ptr)
-
 struct serial2002_range_table_t {
 
 	/*  HACK... */
@@ -648,7 +643,7 @@ err_alloc_configs:
 
 		if (result) {
 			if (devpriv->tty) {
-				filp_close(devpriv->tty, 0);
+				filp_close(devpriv->tty, NULL);
 				devpriv->tty = NULL;
 			}
 		}
@@ -658,8 +653,8 @@ err_alloc_configs:
 
 static void serial_2002_close(struct comedi_device *dev)
 {
-	if (!IS_ERR(devpriv->tty) && (devpriv->tty != 0))
-		filp_close(devpriv->tty, 0);
+	if (!IS_ERR(devpriv->tty) && devpriv->tty)
+		filp_close(devpriv->tty, NULL);
 }
 
 static int serial2002_di_rinsn(struct comedi_device *dev,
@@ -783,21 +778,24 @@ static int serial2002_ei_rinsn(struct comedi_device *dev,
 static int serial2002_attach(struct comedi_device *dev,
 			     struct comedi_devconfig *it)
 {
+	const struct serial2002_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
+	int ret;
 
-	dev_dbg(dev->hw_dev, "comedi%d: attached\n", dev->minor);
-	dev->board_name = thisboard->name;
+	dev_dbg(dev->class_dev, "serial2002: attach\n");
+	dev->board_name = board->name;
 	if (alloc_private(dev, sizeof(struct serial2002_private)) < 0)
 		return -ENOMEM;
 	dev->open = serial_2002_open;
 	dev->close = serial_2002_close;
 	devpriv->port = it->options[0];
 	devpriv->speed = it->options[1];
-	dev_dbg(dev->hw_dev, "/dev/ttyS%d @ %d\n", devpriv->port,
+	dev_dbg(dev->class_dev, "/dev/ttyS%d @ %d\n", devpriv->port,
 		devpriv->speed);
 
-	if (alloc_subdevices(dev, 5) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 5);
+	if (ret)
+		return ret;
 
 	/* digital input subdevice */
 	s = dev->subdevices + 0;
@@ -823,7 +821,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->subdev_flags = SDF_READABLE | SDF_GROUND;
 	s->n_chan = 0;
 	s->maxdata = 1;
-	s->range_table = 0;
+	s->range_table = NULL;
 	s->insn_read = &serial2002_ai_rinsn;
 
 	/* analog output subdevice */
@@ -832,7 +830,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->subdev_flags = SDF_WRITEABLE;
 	s->n_chan = 0;
 	s->maxdata = 1;
-	s->range_table = 0;
+	s->range_table = NULL;
 	s->insn_write = &serial2002_ao_winsn;
 	s->insn_read = &serial2002_ao_rinsn;
 
@@ -842,7 +840,7 @@ static int serial2002_attach(struct comedi_device *dev,
 	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
 	s->n_chan = 0;
 	s->maxdata = 1;
-	s->range_table = 0;
+	s->range_table = NULL;
 	s->insn_read = &serial2002_ei_rinsn;
 
 	return 1;
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
index 7d13ffa7f4f..9a68eebefca 100644
--- a/drivers/staging/comedi/drivers/skel.c
+++ b/drivers/staging/comedi/drivers/skel.c
@@ -210,6 +210,7 @@ static int skel_ns_to_timer(unsigned int *ns, int round);
 static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
+	int ret;
 
 	pr_info("comedi%d: skel: ", dev->minor);
 
@@ -233,12 +234,9 @@ static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (alloc_private(dev, sizeof(struct skel_private)) < 0)
 		return -ENOMEM;
 
-/*
- * Allocate the subdevice structures.  alloc_subdevice() is a
- * convenient macro defined in comedidev.h.
- */
-	if (alloc_subdevices(dev, 3) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* dev->read_subdev=s; */
@@ -563,9 +561,6 @@ static int skel_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	if (insn->n != 2)
-		return -EINVAL;
-
 	/* The insn data is a mask in data[0] and the new data
 	 * in data[1], each channel cooresponding to a bit. */
 	if (data[0]) {
@@ -582,7 +577,7 @@ static int skel_dio_insn_bits(struct comedi_device *dev,
 	 * it was a purely digital output subdevice */
 	/* data[1]=s->state; */
 
-	return 2;
+	return insn->n;
 }
 
 static int skel_dio_insn_config(struct comedi_device *dev,
@@ -616,7 +611,7 @@ static int skel_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-#ifdef CONFIG_COMEDI_PCI
+#ifdef CONFIG_COMEDI_PCI_DRIVERS
 static int __devinit driver_skel_pci_probe(struct pci_dev *dev,
 					   const struct pci_device_id *ent)
 {
diff --git a/drivers/staging/comedi/drivers/ssv_dnp.c b/drivers/staging/comedi/drivers/ssv_dnp.c
index 16c4f5a757b..84b9f2a4280 100644
--- a/drivers/staging/comedi/drivers/ssv_dnp.c
+++ b/drivers/staging/comedi/drivers/ssv_dnp.c
@@ -59,17 +59,6 @@ struct dnp_board {
 	int have_dio;
 };
 
-/* Useful for shorthand access to the particular board structure ----------- */
-#define thisboard ((const struct dnp_board *)dev->board_ptr)
-
-/* This structure is for data unique to the DNP driver --------------------- */
-struct dnp_private_data {
-
-};
-
-/* Shorthand macro for faster access to the private data ------------------- */
-#define devpriv ((dnp_private *)dev->private)
-
 /* ------------------------------------------------------------------------- */
 /* The insn_bits interface allows packed reading/writing of DIO channels.    */
 /* The comedi core can convert between insn_bits and insn_read/write, so you */
@@ -80,10 +69,6 @@ static int dnp_dio_insn_bits(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
-
-	if (insn->n != 2)
-		return -EINVAL;	/* insn uses data[0] and data[1]     */
-
 	/* The insn data is a mask in data[0] and the new data in data[1],   */
 	/* each channel cooresponding to a bit.                              */
 
@@ -117,7 +102,7 @@ static int dnp_dio_insn_bits(struct comedi_device *dev,
 	outb(PCDR, CSCIR);
 	data[0] += ((inb(CSCDR) & 0xF0) << 12);
 
-	return 2;
+	return insn->n;
 
 }
 
@@ -188,29 +173,17 @@ static int dnp_dio_insn_config(struct comedi_device *dev,
 
 static int dnp_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
+	const struct dnp_board *board = comedi_board(dev);
 	struct comedi_subdevice *s;
+	int ret;
 
 	printk(KERN_INFO "comedi%d: dnp: ", dev->minor);
 
-	/* Autoprobing: this should find out which board we have. Currently  */
-	/* only the 1486 board is supported and autoprobing is not           */
-	/* implemented :-)                                                   */
-	/* dev->board_ptr = dnp_probe(dev); */
-
-	/* Initialize the name of the board.                                 */
-	/* We can use the "thisboard" macro now.                             */
-	dev->board_name = thisboard->name;
-
-	/* Allocate the private structure area. alloc_private() is a         */
-	/* convenient macro defined in comedidev.h.                          */
-	if (alloc_private(dev, sizeof(struct dnp_private_data)) < 0)
-		return -ENOMEM;
-
-	/* Allocate the subdevice structures. alloc_subdevice() is a         */
-	/* convenient macro defined in comedidev.h.                          */
+	dev->board_name = board->name;
 
-	if (alloc_subdevices(dev, 1) < 0)
-		return -ENOMEM;
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret)
+		return ret;
 
 	s = dev->subdevices + 0;
 	/* digital i/o subdevice                                             */
diff --git a/drivers/staging/comedi/drivers/unioxx5.c b/drivers/staging/comedi/drivers/unioxx5.c
index d5f1f22aa70..9f1fdec62dc 100644
--- a/drivers/staging/comedi/drivers/unioxx5.c
+++ b/drivers/staging/comedi/drivers/unioxx5.c
@@ -443,6 +443,7 @@ static int unioxx5_attach(struct comedi_device *dev,
 {
 	int iobase, i, n_subd;
 	int id, num, ba;
+	int ret;
 
 	iobase = it->options[0];
 
@@ -468,10 +469,9 @@ static int unioxx5_attach(struct comedi_device *dev,
 		return -1;
 	}
 
-	if (alloc_subdevices(dev, n_subd) < 0) {
-		printk(KERN_ERR "out of memory\n");
-		return -ENOMEM;
-	}
+	ret = comedi_alloc_subdevices(dev, n_subd);
+	if (ret)
+		return ret;
 
 	/* initializing each of for same subdevices */
 	for (i = 0; i < n_subd; i++, iobase += UNIOXX5_SUBDEV_ODDS) {
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 13d9fd3efcf..848c7ec0697 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -1,6 +1,3 @@
-#define DRIVER_VERSION "v2.4"
-#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
-#define DRIVER_DESC "Stirling/ITL USB-DUX -- Bernd.Porr@f2s.com"
 /*
    comedi/drivers/usbdux.c
    Copyright (C) 2003-2007 Bernd Porr, Bernd.Porr@f2s.com
@@ -101,8 +98,6 @@ sampling rate. If you sample two channels you get 4kHz and so on.
 
 #include "../comedidev.h"
 
-#define BOARDNAME "usbdux"
-
 /* timeout for the USB-transfer in ms*/
 #define BULK_TIMEOUT 1000
 
@@ -317,8 +312,6 @@ static struct usbduxsub usbduxsub[NUMUSBDUX];
 
 static DEFINE_SEMAPHORE(start_stop_sem);
 
-static struct comedi_driver driver_usbdux;	/* see below for initializer */
-
 /*
  * Stops the data acquision
  * It should be safe to call this function from any context
@@ -1780,9 +1773,6 @@ static int usbdux_dio_insn_bits(struct comedi_device *dev,
 	if (!this_usbduxsub)
 		return -EFAULT;
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	down(&this_usbduxsub->sem);
 
 	if (!(this_usbduxsub->probed)) {
@@ -1812,7 +1802,7 @@ static int usbdux_dio_insn_bits(struct comedi_device *dev,
 
 	data[1] = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
 	up(&this_usbduxsub->sem);
-	return 2;
+	return insn->n;
 }
 
 /* reads the 4 counters, only two are used just now */
@@ -2302,6 +2292,231 @@ static void tidy_up(struct usbduxsub *usbduxsub_tmp)
 	usbduxsub_tmp->pwm_cmd_running = 0;
 }
 
+/* common part of attach and attach_usb */
+static int usbdux_attach_common(struct comedi_device *dev,
+				struct usbduxsub *udev,
+				void *aux_data, int aux_len)
+{
+	int ret;
+	struct comedi_subdevice *s = NULL;
+	int n_subdevs;
+
+	down(&udev->sem);
+	/* pointer back to the corresponding comedi device */
+	udev->comedidev = dev;
+
+	/* trying to upload the firmware into the chip */
+	if (aux_data)
+		firmwareUpload(udev, aux_data, aux_len);
+
+	dev->board_name = "usbdux";
+
+	/* set number of subdevices */
+	if (udev->high_speed) {
+		/* with pwm */
+		n_subdevs = 5;
+	} else {
+		/* without pwm */
+		n_subdevs = 4;
+	}
+
+	ret = comedi_alloc_subdevices(dev, n_subdevs);
+	if (ret) {
+		up(&udev->sem);
+		return ret;
+	}
+
+	/* private structure is also simply the usb-structure */
+	dev->private = udev;
+
+	/* the first subdevice is the A/D converter */
+	s = dev->subdevices + SUBDEV_AD;
+	/* the URBs get the comedi subdevice */
+	/* which is responsible for reading */
+	/* this is the subdevice which reads data */
+	dev->read_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* analog input */
+	s->type = COMEDI_SUBD_AI;
+	/* readable and ref is to ground */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	/* 8 channels */
+	s->n_chan = 8;
+	/* length of the channellist */
+	s->len_chanlist = 8;
+	/* callback functions */
+	s->insn_read = usbdux_ai_insn_read;
+	s->do_cmdtest = usbdux_ai_cmdtest;
+	s->do_cmd = usbdux_ai_cmd;
+	s->cancel = usbdux_ai_cancel;
+	/* max value from the A/D converter (12bit) */
+	s->maxdata = 0xfff;
+	/* range table to convert to physical units */
+	s->range_table = (&range_usbdux_ai_range);
+
+	/* analog out */
+	s = dev->subdevices + SUBDEV_DA;
+	/* analog out */
+	s->type = COMEDI_SUBD_AO;
+	/* backward pointer */
+	dev->write_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* are writable */
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
+	/* 4 channels */
+	s->n_chan = 4;
+	/* length of the channellist */
+	s->len_chanlist = 4;
+	/* 12 bit resolution */
+	s->maxdata = 0x0fff;
+	/* bipolar range */
+	s->range_table = (&range_usbdux_ao_range);
+	/* callback */
+	s->do_cmdtest = usbdux_ao_cmdtest;
+	s->do_cmd = usbdux_ao_cmd;
+	s->cancel = usbdux_ao_cancel;
+	s->insn_read = usbdux_ao_insn_read;
+	s->insn_write = usbdux_ao_insn_write;
+
+	/* digital I/O */
+	s = dev->subdevices + SUBDEV_DIO;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = (&range_digital);
+	s->insn_bits = usbdux_dio_insn_bits;
+	s->insn_config = usbdux_dio_insn_config;
+	/* we don't use it */
+	s->private = NULL;
+
+	/* counter */
+	s = dev->subdevices + SUBDEV_COUNTER;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xFFFF;
+	s->insn_read = usbdux_counter_read;
+	s->insn_write = usbdux_counter_write;
+	s->insn_config = usbdux_counter_config;
+
+	if (udev->high_speed) {
+		/* timer / pwm */
+		s = dev->subdevices + SUBDEV_PWM;
+		s->type = COMEDI_SUBD_PWM;
+		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
+		s->n_chan = 8;
+		/* this defines the max duty cycle resolution */
+		s->maxdata = udev->sizePwmBuf;
+		s->insn_write = usbdux_pwm_write;
+		s->insn_read = usbdux_pwm_read;
+		s->insn_config = usbdux_pwm_config;
+		usbdux_pwm_period(dev, s, PWM_DEFAULT_PERIOD);
+	}
+	/* finally decide that it's attached */
+	udev->attached = 1;
+
+	up(&udev->sem);
+
+	dev_info(&udev->interface->dev, "comedi%d: attached to usbdux.\n",
+		 dev->minor);
+
+	return 0;
+}
+
+/* is called when comedi-config is called */
+static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
+{
+	int ret;
+	int index;
+	int i;
+	void *aux_data;
+	int aux_len;
+
+	dev->private = NULL;
+
+	aux_data = comedi_aux_data(it->options, 0);
+	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+	if (aux_data == NULL)
+		aux_len = 0;
+	else if (aux_len == 0)
+		aux_data = NULL;
+
+	down(&start_stop_sem);
+	/* find a valid device which has been detected by the probe function of
+	 * the usb */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0) {
+		printk(KERN_ERR
+		       "comedi%d: usbdux: error: attach failed, no usbdux devs connected to the usb bus.\n",
+		       dev->minor);
+		ret = -ENODEV;
+	} else
+		ret = usbdux_attach_common(dev, &usbduxsub[index],
+					   aux_data, aux_len);
+	up(&start_stop_sem);
+	return ret;
+}
+
+/* is called from comedi_usb_auto_config() */
+static int usbdux_attach_usb(struct comedi_device *dev,
+			     struct usb_interface *uinterf)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub;
+
+	dev->private = NULL;
+
+	down(&start_stop_sem);
+	this_usbduxsub = usb_get_intfdata(uinterf);
+	if (!this_usbduxsub || !this_usbduxsub->probed) {
+		printk(KERN_ERR
+		       "comedi%d: usbdux: error: attach_usb failed, not connected\n",
+		       dev->minor);
+		ret = -ENODEV;
+	} else if (this_usbduxsub->attached) {
+		printk(KERN_ERR
+		       "comedi%d: usbdux: error: attach_usb failed, already attached\n",
+		       dev->minor);
+		ret = -ENODEV;
+	} else
+		ret = usbdux_attach_common(dev, this_usbduxsub, NULL, 0);
+	up(&start_stop_sem);
+	return ret;
+}
+
+static void usbdux_detach(struct comedi_device *dev)
+{
+	struct usbduxsub *usb = dev->private;
+
+	if (usb) {
+		down(&usb->sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		usb->comedidev = NULL;
+		up(&usb->sem);
+	}
+}
+
+static struct comedi_driver usbdux_driver = {
+	.driver_name	= "usbdux",
+	.module		= THIS_MODULE,
+	.attach		= usbdux_attach,
+	.detach		= usbdux_detach,
+	.attach_usb	= usbdux_attach_usb,
+};
+
 static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 						     void *context)
 {
@@ -2326,14 +2541,13 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 			"Could not upload firmware (err=%d)\n", ret);
 		goto out;
 	}
-	comedi_usb_auto_config(uinterf, &driver_usbdux);
+	comedi_usb_auto_config(uinterf, &usbdux_driver);
  out:
 	release_firmware(fw);
 }
 
-/* allocate memory for the urbs and initialise them */
-static int usbduxsub_probe(struct usb_interface *uinterf,
-			   const struct usb_device_id *id)
+static int usbdux_usb_probe(struct usb_interface *uinterf,
+			    const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(uinterf);
 	struct device *dev = &uinterf->dev;
@@ -2594,7 +2808,7 @@ static int usbduxsub_probe(struct usb_interface *uinterf,
 	return 0;
 }
 
-static void usbduxsub_disconnect(struct usb_interface *intf)
+static void usbdux_usb_disconnect(struct usb_interface *intf)
 {
 	struct usbduxsub *usbduxsub_tmp = usb_get_intfdata(intf);
 	struct usb_device *udev = interface_to_usbdev(intf);
@@ -2617,273 +2831,22 @@ static void usbduxsub_disconnect(struct usb_interface *intf)
 	dev_dbg(&intf->dev, "comedi_: disconnected from the usb\n");
 }
 
-/* common part of attach and attach_usb */
-static int usbdux_attach_common(struct comedi_device *dev,
-				struct usbduxsub *udev,
-				void *aux_data, int aux_len)
-{
-	int ret;
-	struct comedi_subdevice *s = NULL;
-
-	down(&udev->sem);
-	/* pointer back to the corresponding comedi device */
-	udev->comedidev = dev;
-
-	/* trying to upload the firmware into the chip */
-	if (aux_data)
-		firmwareUpload(udev, aux_data, aux_len);
-
-	dev->board_name = BOARDNAME;
-
-	/* set number of subdevices */
-	if (udev->high_speed) {
-		/* with pwm */
-		dev->n_subdevices = 5;
-	} else {
-		/* without pwm */
-		dev->n_subdevices = 4;
-	}
-
-	/* allocate space for the subdevices */
-	ret = alloc_subdevices(dev, dev->n_subdevices);
-	if (ret < 0) {
-		dev_err(&udev->interface->dev,
-			"comedi%d: error alloc space for subdev\n", dev->minor);
-		up(&udev->sem);
-		return ret;
-	}
-
-	/* private structure is also simply the usb-structure */
-	dev->private = udev;
-
-	/* the first subdevice is the A/D converter */
-	s = dev->subdevices + SUBDEV_AD;
-	/* the URBs get the comedi subdevice */
-	/* which is responsible for reading */
-	/* this is the subdevice which reads data */
-	dev->read_subdev = s;
-	/* the subdevice receives as private structure the */
-	/* usb-structure */
-	s->private = NULL;
-	/* analog input */
-	s->type = COMEDI_SUBD_AI;
-	/* readable and ref is to ground */
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
-	/* 8 channels */
-	s->n_chan = 8;
-	/* length of the channellist */
-	s->len_chanlist = 8;
-	/* callback functions */
-	s->insn_read = usbdux_ai_insn_read;
-	s->do_cmdtest = usbdux_ai_cmdtest;
-	s->do_cmd = usbdux_ai_cmd;
-	s->cancel = usbdux_ai_cancel;
-	/* max value from the A/D converter (12bit) */
-	s->maxdata = 0xfff;
-	/* range table to convert to physical units */
-	s->range_table = (&range_usbdux_ai_range);
-
-	/* analog out */
-	s = dev->subdevices + SUBDEV_DA;
-	/* analog out */
-	s->type = COMEDI_SUBD_AO;
-	/* backward pointer */
-	dev->write_subdev = s;
-	/* the subdevice receives as private structure the */
-	/* usb-structure */
-	s->private = NULL;
-	/* are writable */
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
-	/* 4 channels */
-	s->n_chan = 4;
-	/* length of the channellist */
-	s->len_chanlist = 4;
-	/* 12 bit resolution */
-	s->maxdata = 0x0fff;
-	/* bipolar range */
-	s->range_table = (&range_usbdux_ao_range);
-	/* callback */
-	s->do_cmdtest = usbdux_ao_cmdtest;
-	s->do_cmd = usbdux_ao_cmd;
-	s->cancel = usbdux_ao_cancel;
-	s->insn_read = usbdux_ao_insn_read;
-	s->insn_write = usbdux_ao_insn_write;
-
-	/* digital I/O */
-	s = dev->subdevices + SUBDEV_DIO;
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 8;
-	s->maxdata = 1;
-	s->range_table = (&range_digital);
-	s->insn_bits = usbdux_dio_insn_bits;
-	s->insn_config = usbdux_dio_insn_config;
-	/* we don't use it */
-	s->private = NULL;
-
-	/* counter */
-	s = dev->subdevices + SUBDEV_COUNTER;
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
-	s->n_chan = 4;
-	s->maxdata = 0xFFFF;
-	s->insn_read = usbdux_counter_read;
-	s->insn_write = usbdux_counter_write;
-	s->insn_config = usbdux_counter_config;
-
-	if (udev->high_speed) {
-		/* timer / pwm */
-		s = dev->subdevices + SUBDEV_PWM;
-		s->type = COMEDI_SUBD_PWM;
-		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
-		s->n_chan = 8;
-		/* this defines the max duty cycle resolution */
-		s->maxdata = udev->sizePwmBuf;
-		s->insn_write = usbdux_pwm_write;
-		s->insn_read = usbdux_pwm_read;
-		s->insn_config = usbdux_pwm_config;
-		usbdux_pwm_period(dev, s, PWM_DEFAULT_PERIOD);
-	}
-	/* finally decide that it's attached */
-	udev->attached = 1;
-
-	up(&udev->sem);
-
-	dev_info(&udev->interface->dev, "comedi%d: attached to usbdux.\n",
-		 dev->minor);
-
-	return 0;
-}
-
-/* is called when comedi-config is called */
-static int usbdux_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	int ret;
-	int index;
-	int i;
-	void *aux_data;
-	int aux_len;
-
-	dev->private = NULL;
-
-	aux_data = comedi_aux_data(it->options, 0);
-	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
-	if (aux_data == NULL)
-		aux_len = 0;
-	else if (aux_len == 0)
-		aux_data = NULL;
-
-	down(&start_stop_sem);
-	/* find a valid device which has been detected by the probe function of
-	 * the usb */
-	index = -1;
-	for (i = 0; i < NUMUSBDUX; i++) {
-		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
-			index = i;
-			break;
-		}
-	}
-
-	if (index < 0) {
-		printk(KERN_ERR
-		       "comedi%d: usbdux: error: attach failed, no usbdux devs connected to the usb bus.\n",
-		       dev->minor);
-		ret = -ENODEV;
-	} else
-		ret = usbdux_attach_common(dev, &usbduxsub[index],
-					   aux_data, aux_len);
-	up(&start_stop_sem);
-	return ret;
-}
-
-/* is called from comedi_usb_auto_config() */
-static int usbdux_attach_usb(struct comedi_device *dev,
-			     struct usb_interface *uinterf)
-{
-	int ret;
-	struct usbduxsub *this_usbduxsub;
-
-	dev->private = NULL;
-
-	down(&start_stop_sem);
-	this_usbduxsub = usb_get_intfdata(uinterf);
-	if (!this_usbduxsub || !this_usbduxsub->probed) {
-		printk(KERN_ERR
-		       "comedi%d: usbdux: error: attach_usb failed, not connected\n",
-		       dev->minor);
-		ret = -ENODEV;
-	} else if (this_usbduxsub->attached) {
-		printk(KERN_ERR
-		       "comedi%d: usbdux: error: attach_usb failed, already attached\n",
-		       dev->minor);
-		ret = -ENODEV;
-	} else
-		ret = usbdux_attach_common(dev, this_usbduxsub, NULL, 0);
-	up(&start_stop_sem);
-	return ret;
-}
-
-static void usbdux_detach(struct comedi_device *dev)
-{
-	struct usbduxsub *usb = dev->private;
-
-	if (usb) {
-		down(&usb->sem);
-		dev->private = NULL;
-		usb->attached = 0;
-		usb->comedidev = NULL;
-		up(&usb->sem);
-	}
-}
-
-/* main driver struct */
-static struct comedi_driver driver_usbdux = {
-	.driver_name = "usbdux",
-	.module = THIS_MODULE,
-	.attach = usbdux_attach,
-	.detach = usbdux_detach,
-	.attach_usb = usbdux_attach_usb,
+static const struct usb_device_id usbdux_usb_table[] = {
+	{ USB_DEVICE(0x13d8, 0x0001) },
+	{ USB_DEVICE(0x13d8, 0x0002) },
+	{ }
 };
 
-/* Table with the USB-devices: just now only testing IDs */
-static const struct usb_device_id usbduxsub_table[] = {
-	{USB_DEVICE(0x13d8, 0x0001)},
-	{USB_DEVICE(0x13d8, 0x0002)},
-	{}			/* Terminating entry */
-};
+MODULE_DEVICE_TABLE(usb, usbdux_usb_table);
 
-MODULE_DEVICE_TABLE(usb, usbduxsub_table);
-
-/* The usbduxsub-driver */
-static struct usb_driver usbduxsub_driver = {
-	.name = BOARDNAME,
-	.probe = usbduxsub_probe,
-	.disconnect = usbduxsub_disconnect,
-	.id_table = usbduxsub_table,
+static struct usb_driver usbdux_usb_driver = {
+	.name		= "usbdux",
+	.probe		= usbdux_usb_probe,
+	.disconnect	= usbdux_usb_disconnect,
+	.id_table	= usbdux_usb_table,
 };
+module_comedi_usb_driver(usbdux_driver, usbdux_usb_driver);
 
-/* Can't use the nice macro as I have also to initialise the USB */
-/* subsystem: */
-/* registering the usb-system _and_ the comedi-driver */
-static int __init init_usbdux(void)
-{
-	printk(KERN_INFO KBUILD_MODNAME ": "
-	       DRIVER_VERSION ":" DRIVER_DESC "\n");
-	usb_register(&usbduxsub_driver);
-	comedi_driver_register(&driver_usbdux);
-	return 0;
-}
-
-/* deregistering the comedi driver and the usb-subsystem */
-static void __exit exit_usbdux(void)
-{
-	comedi_driver_unregister(&driver_usbdux);
-	usb_deregister(&usbduxsub_driver);
-}
-
-module_init(init_usbdux);
-module_exit(exit_usbdux);
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Bernd Porr, BerndPorr@f2s.com");
+MODULE_DESCRIPTION("Stirling/ITL USB-DUX -- Bernd.Porr@f2s.com");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index 7b1d21a6fc5..d9911588c10 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -49,11 +49,6 @@
 #include "comedi_fc.h"
 #include "../comedidev.h"
 
-#define DRIVER_VERSION "v1.0"
-#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
-#define DRIVER_DESC "USB-DUXfast, BerndPorr@f2s.com"
-#define BOARDNAME "usbduxfast"
-
 /*
  * timeout for the USB-transfer
  */
@@ -127,11 +122,6 @@
 #define NUMUSBDUXFAST	16
 
 /*
- * number of subdevices
- */
-#define N_SUBDEVICES	1
-
-/*
  * analogue in subdevice
  */
 #define SUBDEV_AD	0
@@ -201,8 +191,6 @@ static struct usbduxfastsub_s usbduxfastsub[NUMUSBDUXFAST];
 
 static DEFINE_SEMAPHORE(start_stop_sem);
 
-static struct comedi_driver driver_usbduxfast;	/* see below for initializer */
-
 /*
  * bulk transfers to usbduxfast
  */
@@ -1441,6 +1429,149 @@ static void tidy_up(struct usbduxfastsub_s *udfs)
 	udfs->ai_cmd_running = 0;
 }
 
+/* common part of attach and attach_usb */
+static int usbduxfast_attach_common(struct comedi_device *dev,
+				    struct usbduxfastsub_s *udfs,
+				    void *aux_data, int aux_len)
+{
+	int ret;
+	struct comedi_subdevice *s;
+
+	down(&udfs->sem);
+	/* pointer back to the corresponding comedi device */
+	udfs->comedidev = dev;
+	/* trying to upload the firmware into the chip */
+	if (aux_data)
+		firmwareUpload(udfs, aux_data, aux_len);
+	dev->board_name = "usbduxfast";
+	ret = comedi_alloc_subdevices(dev, 1);
+	if (ret) {
+		up(&udfs->sem);
+		return ret;
+	}
+	/* private structure is also simply the usb-structure */
+	dev->private = udfs;
+	/* the first subdevice is the A/D converter */
+	s = dev->subdevices + SUBDEV_AD;
+	/*
+	 * the URBs get the comedi subdevice which is responsible for reading
+	 * this is the subdevice which reads data
+	 */
+	dev->read_subdev = s;
+	/* the subdevice receives as private structure the usb-structure */
+	s->private = NULL;
+	/* analog input */
+	s->type = COMEDI_SUBD_AI;
+	/* readable and ref is to ground */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	/* 16 channels */
+	s->n_chan = 16;
+	/* length of the channellist */
+	s->len_chanlist = 16;
+	/* callback functions */
+	s->insn_read = usbduxfast_ai_insn_read;
+	s->do_cmdtest = usbduxfast_ai_cmdtest;
+	s->do_cmd = usbduxfast_ai_cmd;
+	s->cancel = usbduxfast_ai_cancel;
+	/* max value from the A/D converter (12bit+1 bit for overflow) */
+	s->maxdata = 0x1000;
+	/* range table to convert to physical units */
+	s->range_table = &range_usbduxfast_ai_range;
+	/* finally decide that it's attached */
+	udfs->attached = 1;
+	up(&udfs->sem);
+	dev_info(dev->class_dev, "successfully attached to usbduxfast.\n");
+	return 0;
+}
+
+/* is called for COMEDI_DEVCONFIG ioctl (when comedi_config is run) */
+static int usbduxfast_attach(struct comedi_device *dev,
+			     struct comedi_devconfig *it)
+{
+	int ret;
+	int index;
+	int i;
+	void *aux_data;
+	int aux_len;
+
+	dev->private = NULL;
+
+	aux_data = comedi_aux_data(it->options, 0);
+	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+	if (aux_data == NULL)
+		aux_len = 0;
+	else if (aux_len == 0)
+		aux_data = NULL;
+	down(&start_stop_sem);
+	/*
+	 * find a valid device which has been detected by the
+	 * probe function of the usb
+	 */
+	index = -1;
+	for (i = 0; i < NUMUSBDUXFAST; i++) {
+		if (usbduxfastsub[i].probed && !usbduxfastsub[i].attached) {
+			index = i;
+			break;
+		}
+	}
+	if (index < 0) {
+		dev_err(dev->class_dev,
+			"usbduxfast: error: attach failed, no usbduxfast devs connected to the usb bus.\n");
+		ret = -ENODEV;
+	} else
+		ret = usbduxfast_attach_common(dev, &usbduxfastsub[index],
+					       aux_data, aux_len);
+	up(&start_stop_sem);
+	return ret;
+}
+
+/* is called from comedi_usb_auto_config() */
+static int usbduxfast_attach_usb(struct comedi_device *dev,
+				 struct usb_interface *uinterf)
+{
+	int ret;
+	struct usbduxfastsub_s *udfs;
+
+	dev->private = NULL;
+	down(&start_stop_sem);
+	udfs = usb_get_intfdata(uinterf);
+	if (!udfs || !udfs->probed) {
+		dev_err(dev->class_dev,
+			"usbduxfast: error: attach_usb failed, not connected\n");
+		ret = -ENODEV;
+	} else if (udfs->attached) {
+		dev_err(dev->class_dev,
+		       "usbduxfast: error: attach_usb failed, already attached\n");
+		ret = -ENODEV;
+	} else
+		ret = usbduxfast_attach_common(dev, udfs, NULL, 0);
+	up(&start_stop_sem);
+	return ret;
+}
+
+static void usbduxfast_detach(struct comedi_device *dev)
+{
+	struct usbduxfastsub_s *usb = dev->private;
+
+	if (usb) {
+		down(&usb->sem);
+		down(&start_stop_sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		usb->comedidev = NULL;
+		up(&start_stop_sem);
+		up(&usb->sem);
+	}
+}
+
+static struct comedi_driver usbduxfast_driver = {
+	.driver_name	= "usbduxfast",
+	.module		= THIS_MODULE,
+	.attach		= usbduxfast_attach,
+	.detach		= usbduxfast_detach,
+	.attach_usb	= usbduxfast_attach_usb,
+};
+
 static void usbduxfast_firmware_request_complete_handler(const struct firmware
 							 *fw, void *context)
 {
@@ -1463,16 +1594,13 @@ static void usbduxfast_firmware_request_complete_handler(const struct firmware
 		goto out;
 	}
 
-	comedi_usb_auto_config(uinterf, &driver_usbduxfast);
+	comedi_usb_auto_config(uinterf, &usbduxfast_driver);
  out:
 	release_firmware(fw);
 }
 
-/*
- * allocate memory for the urbs and initialise them
- */
-static int usbduxfastsub_probe(struct usb_interface *uinterf,
-			       const struct usb_device_id *id)
+static int usbduxfast_usb_probe(struct usb_interface *uinterf,
+				const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(uinterf);
 	int i;
@@ -1595,7 +1723,7 @@ static int usbduxfastsub_probe(struct usb_interface *uinterf,
 	return 0;
 }
 
-static void usbduxfastsub_disconnect(struct usb_interface *intf)
+static void usbduxfast_usb_disconnect(struct usb_interface *intf)
 {
 	struct usbduxfastsub_s *udfs = usb_get_intfdata(intf);
 	struct usb_device *udev = interface_to_usbdev(intf);
@@ -1624,183 +1752,25 @@ static void usbduxfastsub_disconnect(struct usb_interface *intf)
 #endif
 }
 
-/*
- * is called when comedi-config is called
- */
-static int usbduxfast_attach(struct comedi_device *dev,
-			     struct comedi_devconfig *it)
-{
-	int ret;
-	int index;
-	int i;
-	struct comedi_subdevice *s = NULL;
-	dev->private = NULL;
-
-	down(&start_stop_sem);
-	/*
-	 * find a valid device which has been detected by the
-	 * probe function of the usb
-	 */
-	index = -1;
-	for (i = 0; i < NUMUSBDUXFAST; i++) {
-		if (usbduxfastsub[i].probed && !usbduxfastsub[i].attached) {
-			index = i;
-			break;
-		}
-	}
-
-	if (index < 0) {
-		printk(KERN_ERR "comedi%d: usbduxfast: error: attach failed, "
-		       "no usbduxfast devs connected to the usb bus.\n",
-		       dev->minor);
-		up(&start_stop_sem);
-		return -ENODEV;
-	}
-
-	down(&(usbduxfastsub[index].sem));
-	/* pointer back to the corresponding comedi device */
-	usbduxfastsub[index].comedidev = dev;
-
-	/* trying to upload the firmware into the chip */
-	if (comedi_aux_data(it->options, 0) &&
-	    it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
-		firmwareUpload(&usbduxfastsub[index],
-			       comedi_aux_data(it->options, 0),
-			       it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
-	}
-
-	dev->board_name = BOARDNAME;
-
-	/* set number of subdevices */
-	dev->n_subdevices = N_SUBDEVICES;
-
-	/* allocate space for the subdevices */
-	ret = alloc_subdevices(dev, N_SUBDEVICES);
-	if (ret < 0) {
-		printk(KERN_ERR "comedi%d: usbduxfast: error alloc space for "
-		       "subdev\n", dev->minor);
-		up(&(usbduxfastsub[index].sem));
-		up(&start_stop_sem);
-		return ret;
-	}
-
-	printk(KERN_INFO "comedi%d: usbduxfast: usb-device %d is attached to "
-	       "comedi.\n", dev->minor, index);
-	/* private structure is also simply the usb-structure */
-	dev->private = usbduxfastsub + index;
-	/* the first subdevice is the A/D converter */
-	s = dev->subdevices + SUBDEV_AD;
-	/*
-	 * the URBs get the comedi subdevice which is responsible for reading
-	 * this is the subdevice which reads data
-	 */
-	dev->read_subdev = s;
-	/* the subdevice receives as private structure the usb-structure */
-	s->private = NULL;
-	/* analog input */
-	s->type = COMEDI_SUBD_AI;
-	/* readable and ref is to ground */
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
-	/* 16 channels */
-	s->n_chan = 16;
-	/* length of the channellist */
-	s->len_chanlist = 16;
-	/* callback functions */
-	s->insn_read = usbduxfast_ai_insn_read;
-	s->do_cmdtest = usbduxfast_ai_cmdtest;
-	s->do_cmd = usbduxfast_ai_cmd;
-	s->cancel = usbduxfast_ai_cancel;
-	/* max value from the A/D converter (12bit+1 bit for overflow) */
-	s->maxdata = 0x1000;
-	/* range table to convert to physical units */
-	s->range_table = &range_usbduxfast_ai_range;
-
-	/* finally decide that it's attached */
-	usbduxfastsub[index].attached = 1;
-
-	up(&(usbduxfastsub[index].sem));
-	up(&start_stop_sem);
-	printk(KERN_INFO "comedi%d: successfully attached to usbduxfast.\n",
-	       dev->minor);
-
-	return 0;
-}
-
-static void usbduxfast_detach(struct comedi_device *dev)
-{
-	struct usbduxfastsub_s *usb = dev->private;
-
-	if (usb) {
-		down(&usb->sem);
-		down(&start_stop_sem);
-		dev->private = NULL;
-		usb->attached = 0;
-		usb->comedidev = NULL;
-		up(&start_stop_sem);
-		up(&usb->sem);
-	}
-}
-
-/*
- * main driver struct
- */
-static struct comedi_driver driver_usbduxfast = {
-	.driver_name = "usbduxfast",
-	.module = THIS_MODULE,
-	.attach = usbduxfast_attach,
-	.detach = usbduxfast_detach
-};
-
-/*
- * Table with the USB-devices: just now only testing IDs
- */
-static const struct usb_device_id usbduxfastsub_table[] = {
+static const struct usb_device_id usbduxfast_usb_table[] = {
 	/* { USB_DEVICE(0x4b4, 0x8613) }, testing */
-	{USB_DEVICE(0x13d8, 0x0010)},	/* real ID */
-	{USB_DEVICE(0x13d8, 0x0011)},	/* real ID */
-	{}			/* Terminating entry */
+	{ USB_DEVICE(0x13d8, 0x0010) },	/* real ID */
+	{ USB_DEVICE(0x13d8, 0x0011) },	/* real ID */
+	{ }
 };
+MODULE_DEVICE_TABLE(usb, usbduxfast_usb_table);
 
-MODULE_DEVICE_TABLE(usb, usbduxfastsub_table);
-
-/*
- * The usbduxfastsub-driver
- */
-static struct usb_driver usbduxfastsub_driver = {
+static struct usb_driver usbduxfast_usb_driver = {
 #ifdef COMEDI_HAVE_USB_DRIVER_OWNER
-	.owner = THIS_MODULE,
+	.owner		= THIS_MODULE,
 #endif
-	.name = BOARDNAME,
-	.probe = usbduxfastsub_probe,
-	.disconnect = usbduxfastsub_disconnect,
-	.id_table = usbduxfastsub_table
+	.name		= "usbduxfast",
+	.probe		= usbduxfast_usb_probe,
+	.disconnect	= usbduxfast_usb_disconnect,
+	.id_table	= usbduxfast_usb_table,
 };
+module_comedi_usb_driver(usbduxfast_driver, usbduxfast_usb_driver);
 
-/*
- * Can't use the nice macro as I have also to initialise the USB subsystem:
- * registering the usb-system _and_ the comedi-driver
- */
-static int __init init_usbduxfast(void)
-{
-	printk(KERN_INFO
-	       KBUILD_MODNAME ": " DRIVER_VERSION ":" DRIVER_DESC "\n");
-	usb_register(&usbduxfastsub_driver);
-	comedi_driver_register(&driver_usbduxfast);
-	return 0;
-}
-
-/*
- * deregistering the comedi driver and the usb-subsystem
- */
-static void __exit exit_usbduxfast(void)
-{
-	comedi_driver_unregister(&driver_usbduxfast);
-	usb_deregister(&usbduxfastsub_driver);
-}
-
-module_init(init_usbduxfast);
-module_exit(exit_usbduxfast);
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Bernd Porr, BerndPorr@f2s.com");
+MODULE_DESCRIPTION("USB-DUXfast, BerndPorr@f2s.com");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 465afbdf406..543e604791e 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -1,6 +1,3 @@
-#define DRIVER_VERSION "v0.6"
-#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
-#define DRIVER_DESC "Stirling/ITL USB-DUX SIGMA -- Bernd.Porr@f2s.com"
 /*
    comedi/drivers/usbdux.c
    Copyright (C) 2011 Bernd Porr, Bernd.Porr@f2s.com
@@ -62,8 +59,6 @@ Status: testing
 #include "comedi_fc.h"
 #include "../comedidev.h"
 
-#define BOARDNAME "usbduxsigma"
-
 /* timeout for the USB-transfer in ms*/
 #define BULK_TIMEOUT 1000
 
@@ -267,8 +262,6 @@ static struct usbduxsub usbduxsub[NUMUSBDUX];
 
 static DEFINE_SEMAPHORE(start_stop_sem);
 
-static struct comedi_driver driver_usbduxsigma;	/* see below for initializer */
-
 /*
  * Stops the data acquision
  * It should be safe to call this function from any context
@@ -1358,7 +1351,7 @@ static int usbdux_ai_insn_read(struct comedi_device *dev,
 		/* 32 bits big endian from the A/D converter */
 		one = be32_to_cpu(*((int32_t *)
 				    ((this_usbduxsub->insnBuffer)+1)));
-		/* mask out the staus byte */
+		/* mask out the status byte */
 		one = one & 0x00ffffff;
 		/* turn it into an unsigned integer */
 		one = one ^ 0x00800000;
@@ -1845,9 +1838,6 @@ static int usbdux_dio_insn_bits(struct comedi_device *dev,
 	if (!this_usbduxsub)
 		return -EFAULT;
 
-	if (insn->n != 2)
-		return -EINVAL;
-
 	down(&this_usbduxsub->sem);
 
 	if (!(this_usbduxsub->probed)) {
@@ -1887,7 +1877,7 @@ static int usbdux_dio_insn_bits(struct comedi_device *dev,
 	s->state = data[1];
 
 	up(&this_usbduxsub->sem);
-	return 2;
+	return insn->n;
 }
 
 /***********************************/
@@ -2310,6 +2300,209 @@ static void tidy_up(struct usbduxsub *usbduxsub_tmp)
 	usbduxsub_tmp->pwm_cmd_running = 0;
 }
 
+/* common part of attach and attach_usb */
+static int usbduxsigma_attach_common(struct comedi_device *dev,
+				     struct usbduxsub *uds,
+				     void *aux_data, int aux_len)
+{
+	int ret;
+	struct comedi_subdevice *s;
+	int n_subdevs;
+	int offset;
+
+	down(&uds->sem);
+	/* pointer back to the corresponding comedi device */
+	uds->comedidev = dev;
+	/* trying to upload the firmware into the FX2 */
+	if (aux_data)
+		firmwareUpload(uds, aux_data, aux_len);
+	dev->board_name = "usbduxsigma";
+	/* set number of subdevices */
+	if (uds->high_speed)
+		n_subdevs = 4;	/* with pwm */
+	else
+		n_subdevs = 3;	/* without pwm */
+	ret = comedi_alloc_subdevices(dev, n_subdevs);
+	if (ret) {
+		up(&uds->sem);
+		return ret;
+	}
+	/* private structure is also simply the usb-structure */
+	dev->private = uds;
+	/* the first subdevice is the A/D converter */
+	s = dev->subdevices + SUBDEV_AD;
+	/* the URBs get the comedi subdevice */
+	/* which is responsible for reading */
+	/* this is the subdevice which reads data */
+	dev->read_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* analog input */
+	s->type = COMEDI_SUBD_AI;
+	/* readable and ref is to ground, 32 bit wide data! */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND |
+		SDF_CMD_READ | SDF_LSAMPL;
+	/* 16 A/D channels */
+	s->n_chan = NUMCHANNELS;
+	/* length of the channellist */
+	s->len_chanlist = NUMCHANNELS;
+	/* callback functions */
+	s->insn_read = usbdux_ai_insn_read;
+	s->do_cmdtest = usbdux_ai_cmdtest;
+	s->do_cmd = usbdux_ai_cmd;
+	s->cancel = usbdux_ai_cancel;
+	/* max value from the A/D converter (24bit) */
+	s->maxdata = 0x00FFFFFF;
+	/* range table to convert to physical units */
+	s->range_table = (&range_usbdux_ai_range);
+	/* analog output subdevice */
+	s = dev->subdevices + SUBDEV_DA;
+	/* analog out */
+	s->type = COMEDI_SUBD_AO;
+	/* backward pointer */
+	dev->write_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* are writable */
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
+	/* 4 channels */
+	s->n_chan = 4;
+	/* length of the channellist */
+	s->len_chanlist = 4;
+	/* 8 bit resolution */
+	s->maxdata = 0x00ff;
+	/* unipolar range */
+	s->range_table = (&range_usbdux_ao_range);
+	/* callback */
+	s->do_cmdtest = usbdux_ao_cmdtest;
+	s->do_cmd = usbdux_ao_cmd;
+	s->cancel = usbdux_ao_cancel;
+	s->insn_read = usbdux_ao_insn_read;
+	s->insn_write = usbdux_ao_insn_write;
+	/* digital I/O subdevice */
+	s = dev->subdevices + SUBDEV_DIO;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	/* 8 external and 16 internal channels */
+	s->n_chan = 24;
+	s->maxdata = 1;
+	s->range_table = (&range_digital);
+	s->insn_bits = usbdux_dio_insn_bits;
+	s->insn_config = usbdux_dio_insn_config;
+	/* we don't use it */
+	s->private = NULL;
+	if (uds->high_speed) {
+		/* timer / pwm subdevice */
+		s = dev->subdevices + SUBDEV_PWM;
+		s->type = COMEDI_SUBD_PWM;
+		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
+		s->n_chan = 8;
+		/* this defines the max duty cycle resolution */
+		s->maxdata = uds->sizePwmBuf;
+		s->insn_write = usbdux_pwm_write;
+		s->insn_read = usbdux_pwm_read;
+		s->insn_config = usbdux_pwm_config;
+		usbdux_pwm_period(dev, s, PWM_DEFAULT_PERIOD);
+	}
+	/* finally decide that it's attached */
+	uds->attached = 1;
+	up(&uds->sem);
+	offset = usbdux_getstatusinfo(dev, 0);
+	if (offset < 0)
+		dev_err(&uds->interface->dev,
+			"Communication to USBDUXSIGMA failed! Check firmware and cabling.");
+	dev_info(&uds->interface->dev,
+		 "comedi%d: attached, ADC_zero = %x\n", dev->minor, offset);
+	return 0;
+}
+
+/* is called for COMEDI_DEVCONFIG ioctl (when comedi_config is run) */
+static int usbduxsigma_attach(struct comedi_device *dev,
+			      struct comedi_devconfig *it)
+{
+	int ret;
+	int index;
+	int i;
+	void *aux_data;
+	int aux_len;
+
+	dev->private = NULL;
+
+	aux_data = comedi_aux_data(it->options, 0);
+	aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+	if (aux_data == NULL)
+		aux_len = 0;
+	else if (aux_len == 0)
+		aux_data = NULL;
+
+	down(&start_stop_sem);
+	/* find a valid device which has been detected by the probe function of
+	 * the usb */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
+			index = i;
+			break;
+		}
+	}
+	if (index < 0) {
+		dev_err(dev->class_dev,
+			"usbduxsigma: error: attach failed, dev not connected to the usb bus.\n");
+		ret = -ENODEV;
+	} else
+		ret = usbduxsigma_attach_common(dev, &usbduxsub[index],
+						aux_data, aux_len);
+	up(&start_stop_sem);
+	return ret;
+}
+
+/* is called from comedi_usb_auto_config() */
+static int usbduxsigma_attach_usb(struct comedi_device *dev,
+				  struct usb_interface *uinterf)
+{
+	int ret;
+	struct usbduxsub *uds;
+
+	dev->private = NULL;
+	down(&start_stop_sem);
+	uds = usb_get_intfdata(uinterf);
+	if (!uds || !uds->probed) {
+		dev_err(dev->class_dev,
+			"usbduxsigma: error: attach_usb failed, not connected\n");
+		ret = -ENODEV;
+	} else if (uds->attached) {
+		dev_err(dev->class_dev,
+		       "usbduxsigma: error: attach_usb failed, already attached\n");
+		ret = -ENODEV;
+	} else
+		ret = usbduxsigma_attach_common(dev, uds, NULL, 0);
+	up(&start_stop_sem);
+	return ret;
+}
+
+static void usbduxsigma_detach(struct comedi_device *dev)
+{
+	struct usbduxsub *usb = dev->private;
+
+	if (usb) {
+		down(&usb->sem);
+		dev->private = NULL;
+		usb->attached = 0;
+		usb->comedidev = NULL;
+		up(&usb->sem);
+	}
+}
+
+static struct comedi_driver usbduxsigma_driver = {
+	.driver_name	= "usbduxsigma",
+	.module		= THIS_MODULE,
+	.attach		= usbduxsigma_attach,
+	.detach		= usbduxsigma_detach,
+	.attach_usb	= usbduxsigma_attach_usb,
+};
+
 static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 						     void *context)
 {
@@ -2334,14 +2527,13 @@ static void usbdux_firmware_request_complete_handler(const struct firmware *fw,
 			"Could not upload firmware (err=%d)\n", ret);
 		goto out;
 	}
-	comedi_usb_auto_config(uinterf, &driver_usbduxsigma);
+	comedi_usb_auto_config(uinterf, &usbduxsigma_driver);
 out:
 	release_firmware(fw);
 }
 
-/* allocate memory for the urbs and initialise them */
-static int usbduxsigma_probe(struct usb_interface *uinterf,
-			   const struct usb_device_id *id)
+static int usbduxsigma_usb_probe(struct usb_interface *uinterf,
+				 const struct usb_device_id *id)
 {
 	struct usb_device *udev = interface_to_usbdev(uinterf);
 	struct device *dev = &uinterf->dev;
@@ -2605,7 +2797,7 @@ static int usbduxsigma_probe(struct usb_interface *uinterf,
 	return 0;
 }
 
-static void usbduxsigma_disconnect(struct usb_interface *intf)
+static void usbduxsigma_usb_disconnect(struct usb_interface *intf)
 {
 	struct usbduxsub *usbduxsub_tmp = usb_get_intfdata(intf);
 	struct usb_device *udev = interface_to_usbdev(intf);
@@ -2634,234 +2826,22 @@ static void usbduxsigma_disconnect(struct usb_interface *intf)
 	dev_info(&intf->dev, "comedi_: disconnected from the usb\n");
 }
 
-/* is called when comedi-config is called */
-static int usbduxsigma_attach(struct comedi_device *dev,
-			      struct comedi_devconfig *it)
-{
-	int ret;
-	int index;
-	int i;
-	struct usbduxsub *udev;
-
-	int offset;
-
-	struct comedi_subdevice *s = NULL;
-	dev->private = NULL;
-
-	down(&start_stop_sem);
-	/* find a valid device which has been detected by the probe function of
-	 * the usb */
-	index = -1;
-	for (i = 0; i < NUMUSBDUX; i++) {
-		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
-			index = i;
-			break;
-		}
-	}
-
-	if (index < 0) {
-		printk(KERN_ERR "comedi%d: usbduxsigma: error: attach failed,"
-		       "dev not connected to the usb bus.\n", dev->minor);
-		up(&start_stop_sem);
-		return -ENODEV;
-	}
-
-	udev = &usbduxsub[index];
-	down(&udev->sem);
-	/* pointer back to the corresponding comedi device */
-	udev->comedidev = dev;
-
-	/* trying to upload the firmware into the FX2 */
-	if (comedi_aux_data(it->options, 0) &&
-	    it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
-		firmwareUpload(udev, comedi_aux_data(it->options, 0),
-			       it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
-	}
-
-	dev->board_name = BOARDNAME;
-
-	/* set number of subdevices */
-	if (udev->high_speed) {
-		/* with pwm */
-		dev->n_subdevices = 4;
-	} else {
-		/* without pwm */
-		dev->n_subdevices = 3;
-	}
-
-	/* allocate space for the subdevices */
-	ret = alloc_subdevices(dev, dev->n_subdevices);
-	if (ret < 0) {
-		dev_err(&udev->interface->dev,
-			"comedi%d: no space for subdev\n", dev->minor);
-		up(&udev->sem);
-		up(&start_stop_sem);
-		return ret;
-	}
-
-	/* private structure is also simply the usb-structure */
-	dev->private = udev;
-
-	/* the first subdevice is the A/D converter */
-	s = dev->subdevices + SUBDEV_AD;
-	/* the URBs get the comedi subdevice */
-	/* which is responsible for reading */
-	/* this is the subdevice which reads data */
-	dev->read_subdev = s;
-	/* the subdevice receives as private structure the */
-	/* usb-structure */
-	s->private = NULL;
-	/* analog input */
-	s->type = COMEDI_SUBD_AI;
-	/* readable and ref is to ground, 32 bit wide data! */
-	s->subdev_flags = SDF_READABLE | SDF_GROUND |
-		SDF_CMD_READ | SDF_LSAMPL;
-	/* 16 A/D channels */
-	s->n_chan = NUMCHANNELS;
-	/* length of the channellist */
-	s->len_chanlist = NUMCHANNELS;
-	/* callback functions */
-	s->insn_read = usbdux_ai_insn_read;
-	s->do_cmdtest = usbdux_ai_cmdtest;
-	s->do_cmd = usbdux_ai_cmd;
-	s->cancel = usbdux_ai_cancel;
-	/* max value from the A/D converter (24bit) */
-	s->maxdata = 0x00FFFFFF;
-	/* range table to convert to physical units */
-	s->range_table = (&range_usbdux_ai_range);
-
-	/* analog out */
-	s = dev->subdevices + SUBDEV_DA;
-	/* analog out */
-	s->type = COMEDI_SUBD_AO;
-	/* backward pointer */
-	dev->write_subdev = s;
-	/* the subdevice receives as private structure the */
-	/* usb-structure */
-	s->private = NULL;
-	/* are writable */
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
-	/* 4 channels */
-	s->n_chan = 4;
-	/* length of the channellist */
-	s->len_chanlist = 4;
-	/* 8 bit resolution */
-	s->maxdata = 0x00ff;
-	/* unipolar range */
-	s->range_table = (&range_usbdux_ao_range);
-	/* callback */
-	s->do_cmdtest = usbdux_ao_cmdtest;
-	s->do_cmd = usbdux_ao_cmd;
-	s->cancel = usbdux_ao_cancel;
-	s->insn_read = usbdux_ao_insn_read;
-	s->insn_write = usbdux_ao_insn_write;
-
-	/* digital I/O */
-	s = dev->subdevices + SUBDEV_DIO;
-	s->type = COMEDI_SUBD_DIO;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	/* 8 external and 16 internal channels */
-	s->n_chan = 24;
-	s->maxdata = 1;
-	s->range_table = (&range_digital);
-	s->insn_bits = usbdux_dio_insn_bits;
-	s->insn_config = usbdux_dio_insn_config;
-	/* we don't use it */
-	s->private = NULL;
-
-	if (udev->high_speed) {
-		/* timer / pwm */
-		s = dev->subdevices + SUBDEV_PWM;
-		s->type = COMEDI_SUBD_PWM;
-		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
-		s->n_chan = 8;
-		/* this defines the max duty cycle resolution */
-		s->maxdata = udev->sizePwmBuf;
-		s->insn_write = usbdux_pwm_write;
-		s->insn_read = usbdux_pwm_read;
-		s->insn_config = usbdux_pwm_config;
-		usbdux_pwm_period(dev, s, PWM_DEFAULT_PERIOD);
-	}
-	/* finally decide that it's attached */
-	udev->attached = 1;
-
-	up(&udev->sem);
-
-	up(&start_stop_sem);
-
-	offset = usbdux_getstatusinfo(dev, 0);
-	if (offset < 0)
-		dev_err(&udev->interface->dev,
-			"Communication to USBDUXSIGMA failed!"
-			"Check firmware and cabling.");
-
-	dev_info(&udev->interface->dev,
-		 "comedi%d: attached, ADC_zero = %x", dev->minor, offset);
-
-	return 0;
-}
-
-static void usbduxsigma_detach(struct comedi_device *dev)
-{
-	struct usbduxsub *usb = dev->private;
-
-	if (usb) {
-		down(&usb->sem);
-		dev->private = NULL;
-		usb->attached = 0;
-		usb->comedidev = NULL;
-		up(&usb->sem);
-	}
-}
-
-/* main driver struct */
-static struct comedi_driver driver_usbduxsigma = {
-	.driver_name = "usbduxsigma",
-	.module = THIS_MODULE,
-	.attach = usbduxsigma_attach,
-	.detach = usbduxsigma_detach,
+static const struct usb_device_id usbduxsigma_usb_table[] = {
+	{ USB_DEVICE(0x13d8, 0x0020) },
+	{ USB_DEVICE(0x13d8, 0x0021) },
+	{ USB_DEVICE(0x13d8, 0x0022) },
+	{ }
 };
+MODULE_DEVICE_TABLE(usb, usbduxsigma_usb_table);
 
-/* Table with the USB-devices */
-static const struct usb_device_id usbduxsigma_table[] = {
-	{USB_DEVICE(0x13d8, 0x0020)},
-	{USB_DEVICE(0x13d8, 0x0021)},
-	{USB_DEVICE(0x13d8, 0x0022)},
-	{}			/* Terminating entry */
+static struct usb_driver usbduxsigma_usb_driver = {
+	.name		= "usbduxsigma",
+	.probe		= usbduxsigma_usb_probe,
+	.disconnect	= usbduxsigma_usb_disconnect,
+	.id_table	= usbduxsigma_usb_table,
 };
+module_comedi_usb_driver(usbduxsigma_driver, usbduxsigma_usb_driver);
 
-MODULE_DEVICE_TABLE(usb, usbduxsigma_table);
-
-/* The usbduxsub-driver */
-static struct usb_driver usbduxsigma_driver = {
-	.name = BOARDNAME,
-	.probe = usbduxsigma_probe,
-	.disconnect = usbduxsigma_disconnect,
-	.id_table = usbduxsigma_table,
-};
-
-/* Can't use the nice macro as I have also to initialise the USB */
-/* subsystem: */
-/* registering the usb-system _and_ the comedi-driver */
-static int __init init_usbduxsigma(void)
-{
-	printk(KERN_INFO KBUILD_MODNAME ": "
-	       DRIVER_VERSION ":" DRIVER_DESC "\n");
-	usb_register(&usbduxsigma_driver);
-	comedi_driver_register(&driver_usbduxsigma);
-	return 0;
-}
-
-/* deregistering the comedi driver and the usb-subsystem */
-static void __exit exit_usbduxsigma(void)
-{
-	comedi_driver_unregister(&driver_usbduxsigma);
-	usb_deregister(&usbduxsigma_driver);
-}
-
-module_init(init_usbduxsigma);
-module_exit(exit_usbduxsigma);
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Bernd Porr, BerndPorr@f2s.com");
+MODULE_DESCRIPTION("Stirling/ITL USB-DUX SIGMA -- Bernd.Porr@f2s.com");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index baee8d76763..94010fc0590 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -64,37 +64,11 @@ Changelog:
 
 #include "../comedidev.h"
 
-#define BOARDNAME "vmk80xx"
-
-MODULE_AUTHOR("Manuel Gebele <forensixs@gmx.de>");
-MODULE_DESCRIPTION("Velleman USB Board Low-Level Driver");
-MODULE_SUPPORTED_DEVICE("K8055/K8061 aka VM110/VM140");
-MODULE_VERSION("0.8.01");
-MODULE_LICENSE("GPL");
-
 enum {
 	DEVICE_VMK8055,
 	DEVICE_VMK8061
 };
 
-static const struct usb_device_id vmk80xx_id_table[] = {
-	{USB_DEVICE(0x10cf, 0x5500), .driver_info = DEVICE_VMK8055},
-	{USB_DEVICE(0x10cf, 0x5501), .driver_info = DEVICE_VMK8055},
-	{USB_DEVICE(0x10cf, 0x5502), .driver_info = DEVICE_VMK8055},
-	{USB_DEVICE(0x10cf, 0x5503), .driver_info = DEVICE_VMK8055},
-	{USB_DEVICE(0x10cf, 0x8061), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8062), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8063), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8064), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8065), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8066), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8067), .driver_info = DEVICE_VMK8061},
-	{USB_DEVICE(0x10cf, 0x8068), .driver_info = DEVICE_VMK8061},
-	{}			/* terminating entry */
-};
-
-MODULE_DEVICE_TABLE(usb, vmk80xx_id_table);
-
 #define VMK8055_DI_REG          0x00
 #define VMK8055_DO_REG          0x01
 #define VMK8055_AO1_REG         0x02
@@ -232,8 +206,6 @@ static struct vmk80xx_usb vmb[VMK80XX_MAX_BOARDS];
 
 static DEFINE_MUTEX(glb_mutex);
 
-static struct comedi_driver driver_vmk80xx;	/* see below for initializer */
-
 static void vmk80xx_tx_callback(struct urb *urb)
 {
 	struct vmk80xx_usb *dev = urb->context;
@@ -1127,44 +1099,25 @@ static int vmk80xx_pwm_winsn(struct comedi_device *cdev,
 	return n;
 }
 
-static int vmk80xx_attach(struct comedi_device *cdev,
-			  struct comedi_devconfig *it)
+static int vmk80xx_attach_common(struct comedi_device *cdev,
+				 struct vmk80xx_usb *dev)
 {
-	int i;
-	struct vmk80xx_usb *dev;
 	int n_subd;
 	struct comedi_subdevice *s;
-	int minor;
-
-	mutex_lock(&glb_mutex);
-
-	for (i = 0; i < VMK80XX_MAX_BOARDS; i++)
-		if (vmb[i].probed && !vmb[i].attached)
-			break;
-
-	if (i == VMK80XX_MAX_BOARDS) {
-		mutex_unlock(&glb_mutex);
-		return -ENODEV;
-	}
-
-	dev = &vmb[i];
+	int ret;
 
 	down(&dev->limit_sem);
-
 	cdev->board_name = dev->board.name;
 	cdev->private = dev;
-
 	if (dev->board.model == VMK8055_MODEL)
 		n_subd = 5;
 	else
 		n_subd = 6;
-
-	if (alloc_subdevices(cdev, n_subd) < 0) {
+	ret = comedi_alloc_subdevices(cdev, n_subd);
+	if (ret) {
 		up(&dev->limit_sem);
-		mutex_unlock(&glb_mutex);
-		return -ENOMEM;
+		return ret;
 	}
-
 	/* Analog input subdevice */
 	s = cdev->subdevices + VMK80XX_SUBD_AI;
 	s->type = COMEDI_SUBD_AI;
@@ -1173,7 +1126,6 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	s->maxdata = (1 << dev->board.ai_bits) - 1;
 	s->range_table = dev->board.range;
 	s->insn_read = vmk80xx_ai_rinsn;
-
 	/* Analog output subdevice */
 	s = cdev->subdevices + VMK80XX_SUBD_AO;
 	s->type = COMEDI_SUBD_AO;
@@ -1182,12 +1134,10 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	s->maxdata = (1 << dev->board.ao_bits) - 1;
 	s->range_table = dev->board.range;
 	s->insn_write = vmk80xx_ao_winsn;
-
 	if (dev->board.model == VMK8061_MODEL) {
 		s->subdev_flags |= SDF_READABLE;
 		s->insn_read = vmk80xx_ao_rinsn;
 	}
-
 	/* Digital input subdevice */
 	s = cdev->subdevices + VMK80XX_SUBD_DI;
 	s->type = COMEDI_SUBD_DI;
@@ -1196,7 +1146,6 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	s->maxdata = 1;
 	s->insn_read = vmk80xx_di_rinsn;
 	s->insn_bits = vmk80xx_di_bits;
-
 	/* Digital output subdevice */
 	s = cdev->subdevices + VMK80XX_SUBD_DO;
 	s->type = COMEDI_SUBD_DO;
@@ -1205,12 +1154,10 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	s->maxdata = 1;
 	s->insn_write = vmk80xx_do_winsn;
 	s->insn_bits = vmk80xx_do_bits;
-
 	if (dev->board.model == VMK8061_MODEL) {
 		s->subdev_flags |= SDF_READABLE;
 		s->insn_read = vmk80xx_do_rinsn;
 	}
-
 	/* Counter subdevice */
 	s = cdev->subdevices + VMK80XX_SUBD_CNT;
 	s->type = COMEDI_SUBD_COUNTER;
@@ -1218,13 +1165,11 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 	s->n_chan = dev->board.cnt_chans;
 	s->insn_read = vmk80xx_cnt_rinsn;
 	s->insn_config = vmk80xx_cnt_cinsn;
-
 	if (dev->board.model == VMK8055_MODEL) {
 		s->subdev_flags |= SDF_WRITEABLE;
 		s->maxdata = (1 << dev->board.cnt_bits) - 1;
 		s->insn_write = vmk80xx_cnt_winsn;
 	}
-
 	/* PWM subdevice */
 	if (dev->board.model == VMK8061_MODEL) {
 		s = cdev->subdevices + VMK80XX_SUBD_PWM;
@@ -1235,19 +1180,51 @@ static int vmk80xx_attach(struct comedi_device *cdev,
 		s->insn_read = vmk80xx_pwm_rinsn;
 		s->insn_write = vmk80xx_pwm_winsn;
 	}
-
 	dev->attached = 1;
+	dev_info(cdev->class_dev, "vmk80xx: board #%d [%s] attached\n",
+		 dev->count, dev->board.name);
+	up(&dev->limit_sem);
+	return 0;
+}
 
-	minor = cdev->minor;
-
-	printk(KERN_INFO
-	       "comedi%d: vmk80xx: board #%d [%s] attached to comedi\n",
-	       minor, dev->count, dev->board.name);
+/* called for COMEDI_DEVCONFIG ioctl for board_name "vmk80xx" */
+static int vmk80xx_attach(struct comedi_device *cdev,
+			  struct comedi_devconfig *it)
+{
+	int i;
+	int ret;
 
-	up(&dev->limit_sem);
+	mutex_lock(&glb_mutex);
+	for (i = 0; i < VMK80XX_MAX_BOARDS; i++)
+		if (vmb[i].probed && !vmb[i].attached)
+			break;
+	if (i == VMK80XX_MAX_BOARDS)
+		ret = -ENODEV;
+	else
+		ret = vmk80xx_attach_common(cdev, &vmb[i]);
 	mutex_unlock(&glb_mutex);
+	return ret;
+}
 
-	return 0;
+/* called via comedi_usb_auto_config() */
+static int vmk80xx_attach_usb(struct comedi_device *cdev,
+			      struct usb_interface *intf)
+{
+	int i;
+	int ret;
+
+	mutex_lock(&glb_mutex);
+	for (i = 0; i < VMK80XX_MAX_BOARDS; i++)
+		if (vmb[i].probed && vmb[i].intf == intf)
+			break;
+	if (i == VMK80XX_MAX_BOARDS)
+		ret = -ENODEV;
+	else if (vmb[i].attached)
+		ret = -EBUSY;
+	else
+		ret = vmk80xx_attach_common(cdev, &vmb[i]);
+	mutex_unlock(&glb_mutex);
+	return ret;
 }
 
 static void vmk80xx_detach(struct comedi_device *dev)
@@ -1262,8 +1239,16 @@ static void vmk80xx_detach(struct comedi_device *dev)
 	}
 }
 
-static int vmk80xx_probe(struct usb_interface *intf,
-			 const struct usb_device_id *id)
+static struct comedi_driver vmk80xx_driver = {
+	.module		= THIS_MODULE,
+	.driver_name	= "vmk80xx",
+	.attach		= vmk80xx_attach,
+	.detach		= vmk80xx_detach,
+	.attach_usb	= vmk80xx_attach_usb,
+};
+
+static int vmk80xx_usb_probe(struct usb_interface *intf,
+			     const struct usb_device_id *id)
 {
 	int i;
 	struct vmk80xx_usb *dev;
@@ -1405,7 +1390,7 @@ static int vmk80xx_probe(struct usb_interface *intf,
 
 	mutex_unlock(&glb_mutex);
 
-	comedi_usb_auto_config(intf, &driver_vmk80xx);
+	comedi_usb_auto_config(intf, &vmk80xx_driver);
 
 	return 0;
 error:
@@ -1414,7 +1399,7 @@ error:
 	return -ENODEV;
 }
 
-static void vmk80xx_disconnect(struct usb_interface *intf)
+static void vmk80xx_usb_disconnect(struct usb_interface *intf)
 {
 	struct vmk80xx_usb *dev = usb_get_intfdata(intf);
 
@@ -1442,41 +1427,35 @@ static void vmk80xx_disconnect(struct usb_interface *intf)
 	mutex_unlock(&glb_mutex);
 }
 
-/* TODO: Add support for suspend, resume, pre_reset,
- * post_reset and flush */
-static struct usb_driver vmk80xx_driver = {
-	.name = "vmk80xx",
-	.probe = vmk80xx_probe,
-	.disconnect = vmk80xx_disconnect,
-	.id_table = vmk80xx_id_table
+static const struct usb_device_id vmk80xx_usb_id_table[] = {
+	{ USB_DEVICE(0x10cf, 0x5500), .driver_info = DEVICE_VMK8055 },
+	{ USB_DEVICE(0x10cf, 0x5501), .driver_info = DEVICE_VMK8055 },
+	{ USB_DEVICE(0x10cf, 0x5502), .driver_info = DEVICE_VMK8055 },
+	{ USB_DEVICE(0x10cf, 0x5503), .driver_info = DEVICE_VMK8055 },
+	{ USB_DEVICE(0x10cf, 0x8061), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8062), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8063), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8064), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8065), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8066), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8067), .driver_info = DEVICE_VMK8061 },
+	{ USB_DEVICE(0x10cf, 0x8068), .driver_info = DEVICE_VMK8061 },
+	{ }
 };
+MODULE_DEVICE_TABLE(usb, vmk80xx_usb_id_table);
 
-static struct comedi_driver driver_vmk80xx = {
-	.module = THIS_MODULE,
-	.driver_name = "vmk80xx",
-	.attach = vmk80xx_attach,
-	.detach = vmk80xx_detach
+/* TODO: Add support for suspend, resume, pre_reset,
+ * post_reset and flush */
+static struct usb_driver vmk80xx_usb_driver = {
+	.name		= "vmk80xx",
+	.probe		= vmk80xx_usb_probe,
+	.disconnect	= vmk80xx_usb_disconnect,
+	.id_table	= vmk80xx_usb_id_table,
 };
+module_comedi_usb_driver(vmk80xx_driver, vmk80xx_usb_driver);
 
-static int __init vmk80xx_init(void)
-{
-	int retval;
-
-	printk(KERN_INFO "vmk80xx: version 0.8.01 "
-	       "Manuel Gebele <forensixs@gmx.de>\n");
-
-	retval = comedi_driver_register(&driver_vmk80xx);
-	if (retval < 0)
-		return retval;
-
-	return usb_register(&vmk80xx_driver);
-}
-
-static void __exit vmk80xx_exit(void)
-{
-	comedi_driver_unregister(&driver_vmk80xx);
-	usb_deregister(&vmk80xx_driver);
-}
-
-module_init(vmk80xx_init);
-module_exit(vmk80xx_exit);
+MODULE_AUTHOR("Manuel Gebele <forensixs@gmx.de>");
+MODULE_DESCRIPTION("Velleman USB Board Low-Level Driver");
+MODULE_SUPPORTED_DEVICE("K8055/K8061 aka VM110/VM140");
+MODULE_VERSION("0.8.01");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/proc.c b/drivers/staging/comedi/proc.c
index 2aa487b6018..bb7e70e3e94 100644
--- a/drivers/staging/comedi/proc.c
+++ b/drivers/staging/comedi/proc.c
@@ -30,7 +30,7 @@
 
 #define __NO_VERSION__
 #include "comedidev.h"
-#include "comedi_fops.h"
+#include "comedi_internal.h"
 #include <linux/proc_fs.h>
 #include <linux/string.h>
 
diff --git a/drivers/staging/comedi/range.c b/drivers/staging/comedi/range.c
index 148ec6fd6fd..41f95237789 100644
--- a/drivers/staging/comedi/range.c
+++ b/drivers/staging/comedi/range.c
@@ -23,7 +23,7 @@
 
 #include <linux/uaccess.h>
 #include "comedidev.h"
-#include "internal.h"
+#include "comedi_internal.h"
 
 const struct comedi_lrange range_bipolar10 = { 1, {BIP_RANGE(10)} };
 EXPORT_SYMBOL(range_bipolar10);
diff --git a/drivers/staging/cptm1217/clearpad_tm1217.c b/drivers/staging/cptm1217/clearpad_tm1217.c
index 5456f82c306..0d924d3a2ab 100644
--- a/drivers/staging/cptm1217/clearpad_tm1217.c
+++ b/drivers/staging/cptm1217/clearpad_tm1217.c
@@ -396,8 +396,8 @@ static int cp_tm1217_setup_gpio_irq(struct cp_tm1217_device *ts)
 
 	retval = gpio_to_irq(ts->gpio);
 	if (retval < 0) {
-		dev_err(ts->dev, "cp_tm1217: GPIO to IRQ failedi,"
-		" error %d\n", retval);
+		dev_err(ts->dev,
+			"cp_tm1217: GPIO to IRQ failed, error %d\n", retval);
 		gpio_free(ts->gpio);
 	}
 	dev_dbg(ts->dev,
diff --git a/drivers/staging/cptm1217/cp_tm1217.h b/drivers/staging/cptm1217/cp_tm1217.h
index a0ce31db53f..30bad357a05 100644
--- a/drivers/staging/cptm1217/cp_tm1217.h
+++ b/drivers/staging/cptm1217/cp_tm1217.h
@@ -1,8 +1,7 @@
 #ifndef __LINUX_I2C_CP_TM1217_H
 #define __LINUX_I2C_CP_TM1217_H
 
-struct cp_tm1217_platform_data
-{
+struct cp_tm1217_platform_data {
 	int gpio;		/* If not set uses the IRQ resource 0 */
 };
 
diff --git a/drivers/staging/csr/Kconfig b/drivers/staging/csr/Kconfig
new file mode 100644
index 00000000000..cee8d48d2af
--- /dev/null
+++ b/drivers/staging/csr/Kconfig
@@ -0,0 +1,9 @@
+config CSR_WIFI
+	tristate "CSR wireless driver"
+	depends on MMC && CFG80211_WEXT
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	help
+	  Driver for the CSR wireless SDIO device.
+
+	  If unsure, select N.
diff --git a/drivers/staging/csr/LICENSE.txt b/drivers/staging/csr/LICENSE.txt
new file mode 100644
index 00000000000..364853e5fed
--- /dev/null
+++ b/drivers/staging/csr/LICENSE.txt
@@ -0,0 +1,39 @@
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+Except as contained in this notice, the names of above-listed
+copyright holders and the names of any contributors shall not be used
+in advertising or otherwise to promote the sale, use or other dealings
+in this Software without prior written authorization.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+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 HOLDERS OR
+CONTRIBUTORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT
+OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+Alternatively, this software may be distributed under the terms of the
+GNU General Public License ("GPL") version 2 as published
+by the Free Software Foundation.
+
+As a special exception, if other files instantiate templates or use
+macros or inline functions from this file, or you compile this file
+and link it with other works to produce a work based on this file,
+this file does not by itself cause the resulting work to be covered by
+the GNU General Public License. However the source code for this file
+must still be made available in accordance with section (3) of the GNU
+General Public License.
+
+This exception does not invalidate any other reasons why a work based
+on this file might be covered by the GNU General Public License.
diff --git a/drivers/staging/csr/Makefile b/drivers/staging/csr/Makefile
new file mode 100644
index 00000000000..afda44b0a92
--- /dev/null
+++ b/drivers/staging/csr/Makefile
@@ -0,0 +1,75 @@
+ccflags-y	:= -DCSR_SME_USERSPACE -DCSR_SUPPORT_SME -DREMOTE_SYS_SAP -DCSR_WIFI_SECURITY_WAPI_ENABLE -DENABLE_SHUTDOWN -DUNIFI_DEBUG
+ccflags-y	+= -DSDIO_EXPORTS_STRUCT_DEVICE -DCSR_WIFI_SUPPORT_MMC_DRIVER -DCSR_WIFI_SINGLE_FUNCTION -DCSR_WIFI_SPLIT_PATCH
+ccflags-y	+= -DCSR_SUPPORT_WEXT -DREMOTE_SYS_SAP  -DREMOTE_MGT_SAP -DCSR_WIFI_SECURITY_WAPI_ENABLE -DCSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND -DENABLE_SHUTDOWN -DCSR_WIFI_NME_ENABLE -DCSR_WIFI_AP_ENABLE -DCSR_SUPPORT_WEXT_AP -DCSR_WIFI_REQUEUE_PACKET_TO_HAL
+
+obj-$(CONFIG_CSR_WIFI)	+= csr_wifi.o
+obj-$(CONFIG_CSR_WIFI)	+= csr_helper.o
+
+csr_wifi-y :=	bh.o				\
+		data_tx.o			\
+		drv.o				\
+		firmware.o			\
+		inet.o				\
+		init_hw.o			\
+		io.o				\
+		monitor.o			\
+		netdev.o			\
+		os.o				\
+		putest.o			\
+		sdio_events.o			\
+		sdio_mmc.o			\
+		sdio_stubs.o			\
+		sme_blocking.o			\
+		ul_int.o			\
+		unifi_dbg.o			\
+		unifi_event.o			\
+		unifi_pdu_processing.o		\
+		unifi_sme.o			\
+		csr_formatted_io.o		\
+		csr_wifi_hip_card_sdio.o	\
+		csr_wifi_hip_card_sdio_intr.o	\
+		csr_wifi_hip_card_sdio_mem.o	\
+		csr_wifi_hip_chiphelper.o	\
+		csr_wifi_hip_download.o		\
+		csr_wifi_hip_dump.o		\
+		csr_wifi_hip_packing.o		\
+		csr_wifi_hip_send.o		\
+		csr_wifi_hip_signals.o		\
+		csr_wifi_hip_ta_sampling.o	\
+		csr_wifi_hip_udi.o		\
+		csr_wifi_hip_unifi_signal_names.o	\
+		csr_wifi_hip_xbv.o		\
+		csr_wifi_nme_ap_converter_init.o		\
+		csr_wifi_nme_ap_free_downstream_contents.o	\
+		csr_wifi_nme_ap_free_upstream_contents.o	\
+		csr_wifi_nme_ap_serialize.o	\
+		csr_wifi_nme_ap_sef.o		\
+		csr_wifi_router_ctrl_sef.o	\
+		csr_wifi_router_sef.o		\
+		csr_wifi_router_transport.o	\
+		csr_wifi_sme_sef.o		\
+		csr_wifi_sme_converter_init.o	\
+		csr_wifi_sme_free_downstream_contents.o		\
+		csr_wifi_sme_free_upstream_contents.o		\
+		csr_wifi_sme_serialize.o			\
+		csr_wifi_router_ctrl_converter_init.o		\
+		csr_wifi_router_ctrl_free_downstream_contents.o	\
+		csr_wifi_router_ctrl_free_upstream_contents.o	\
+		csr_wifi_router_ctrl_serialize.o		\
+		csr_wifi_router_converter_init.o		\
+		csr_wifi_router_free_downstream_contents.o	\
+		csr_wifi_router_free_upstream_contents.o	\
+		csr_wifi_router_serialize.o			\
+		sme_mgt.o			\
+		sme_sys.o			\
+		sme_userspace.o 		\
+		sme_wext.o			\
+		wext_events.o
+
+csr_helper-y :=	csr_time.o			\
+		csr_util.o			\
+		csr_framework_ext.o		\
+		csr_wifi_serialize_primitive_types.o	\
+		csr_serialize_primitive_types.o	\
+		csr_msgconv.o			\
+		csr_panic.o
diff --git a/drivers/staging/csr/bh.c b/drivers/staging/csr/bh.c
new file mode 100644
index 00000000000..b089c28d561
--- /dev/null
+++ b/drivers/staging/csr/bh.c
@@ -0,0 +1,391 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     bh.c
+ *
+ * PURPOSE:
+ *      Provides an implementation for the driver bottom-half.
+ *      It is part of the porting exercise in Linux.
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "unifi_priv.h"
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_start_thread
+ *
+ *      Helper function to start a new thread.
+ *
+ *  Arguments:
+ *      priv            Pointer to OS driver structure for the device.
+ *      thread          Pointer to the thread object
+ *      func            The thread function
+ *
+ *  Returns:
+ *      0 on success or else a Linux error code.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_start_thread(unifi_priv_t *priv, struct uf_thread *thread, int (*func)(void *))
+{
+    if (thread->thread_task != NULL) {
+        unifi_error(priv, "%s thread already started\n", thread->name);
+        return 0;
+    }
+
+    /* Start the kernel thread that handles all h/w accesses. */
+    thread->thread_task = kthread_run(func, priv, "%s", thread->name);
+    if (IS_ERR(thread->thread_task)) {
+        return PTR_ERR(thread->thread_task);
+    }
+
+    /* Module parameter overides the thread priority */
+    if (bh_priority != -1) {
+        if (bh_priority >= 0 && bh_priority <= MAX_RT_PRIO) {
+            struct sched_param param;
+            priv->bh_thread.prio = bh_priority;
+            unifi_trace(priv, UDBG1, "%s thread (RT) priority = %d\n",
+                        thread->name, bh_priority);
+            param.sched_priority = bh_priority;
+            sched_setscheduler(thread->thread_task, SCHED_FIFO, &param);
+        } else if (bh_priority > MAX_RT_PRIO && bh_priority <= MAX_PRIO) {
+            priv->bh_thread.prio = bh_priority;
+            unifi_trace(priv, UDBG1, "%s thread priority = %d\n",
+                        thread->name, PRIO_TO_NICE(bh_priority));
+            set_user_nice(thread->thread_task, PRIO_TO_NICE(bh_priority));
+        } else {
+            priv->bh_thread.prio = DEFAULT_PRIO;
+            unifi_warning(priv, "%s thread unsupported (%d) priority\n",
+                          thread->name, bh_priority);
+        }
+    } else {
+        priv->bh_thread.prio = DEFAULT_PRIO;
+    }
+    unifi_trace(priv, UDBG2, "Started %s thread\n", thread->name);
+
+    return 0;
+} /* uf_start_thread() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_stop_thread
+ *
+ *      Helper function to stop a thread.
+ *
+ *  Arguments:
+ *      priv            Pointer to OS driver structure for the device.
+ *      thread          Pointer to the thread object
+ *
+ *  Returns:
+ *
+ * ---------------------------------------------------------------------------
+ */
+    void
+uf_stop_thread(unifi_priv_t *priv, struct uf_thread *thread)
+{
+    if (!thread->thread_task) {
+        unifi_notice(priv, "%s thread is already stopped\n", thread->name);
+        return;
+    }
+
+    unifi_trace(priv, UDBG2, "Stopping %s thread\n", thread->name);
+
+    kthread_stop(thread->thread_task);
+    thread->thread_task = NULL;
+
+} /* uf_stop_thread() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_wait_for_thread_to_stop
+ *
+ *      Helper function to wait until a thread is stopped.
+ *
+ *  Arguments:
+ *      priv    Pointer to OS driver structure for the device.
+ *
+ *  Returns:
+ *
+ * ---------------------------------------------------------------------------
+ */
+    void
+uf_wait_for_thread_to_stop(unifi_priv_t *priv, struct uf_thread *thread)
+{
+    /*
+     * kthread_stop() cannot handle the thread exiting while
+     * kthread_should_stop() is false, so sleep until kthread_stop()
+     * wakes us up.
+     */
+    unifi_trace(priv, UDBG2, "%s waiting for the stop signal.\n", thread->name);
+    set_current_state(TASK_INTERRUPTIBLE);
+    if (!kthread_should_stop()) {
+        unifi_trace(priv, UDBG2, "%s schedule....\n", thread->name);
+        schedule();
+    }
+
+    thread->thread_task = NULL;
+    unifi_trace(priv, UDBG2, "%s exiting....\n", thread->name);
+} /* uf_wait_for_thread_to_stop() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  handle_bh_error
+ *
+ *      This function reports an error returned from the HIP core bottom-half.
+ *      Normally, implemented during the porting exercise, passing the error
+ *      to the SME using unifi_sys_wifi_off_ind().
+ *      The SME will try to reset the device and go through
+ *      the initialisation of the UniFi.
+ *
+ *  Arguments:
+ *      priv            Pointer to OS driver structure for the device.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+    static void
+handle_bh_error(unifi_priv_t *priv)
+{
+    u8 conf_param = CONFIG_IND_ERROR;
+    u8 interfaceTag = 0; /* used as a loop counter */
+
+
+    /* Block unifi_run_bh() until the error has been handled. */
+    priv->bh_thread.block_thread = 1;
+
+    /* Consider UniFi to be uninitialised */
+    priv->init_progress = UNIFI_INIT_NONE;
+
+    /* Stop the network traffic */
+    for( interfaceTag =0; interfaceTag <CSR_WIFI_NUM_INTERFACES;interfaceTag ++) {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+        if (interfacePriv->netdev_registered == 1) {
+            netif_carrier_off(priv->netdev[interfaceTag]);
+        }
+    }
+
+#ifdef CSR_NATIVE_LINUX
+    /* Force any client waiting on an mlme_wait_for_reply() to abort. */
+    uf_abort_mlme(priv);
+
+    /* Cancel any pending workqueue tasks */
+    flush_workqueue(priv->unifi_workqueue);
+
+#endif /* CSR_NATIVE_LINUX */
+
+    unifi_error(priv, "handle_bh_error: fatal error is reported to the SME.\n");
+    /* Notify the clients (SME or unifi_manager) for the error. */
+    ul_log_config_ind(priv, &conf_param, sizeof(u8));
+
+} /* handle_bh_error() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  bh_thread_function
+ *
+ *      All hardware access happens in this thread.
+ *      This means there is no need for locks on the hardware and we don't need
+ *      to worry about reentrancy with the SDIO library.
+ *      Provides and example implementation on how to call unifi_bh(), which
+ *      is part of the HIP core API.
+ *
+ *      It processes the events generated by unifi_run_bh() to serialise calls
+ *      to unifi_bh(). It also demonstrates how the timeout parameter passed in
+ *      and returned from unifi_bh() needs to be handled.
+ *
+ *  Arguments:
+ *      arg             Pointer to OS driver structure for the device.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      When the bottom half of the driver needs to process signals, events,
+ *      or simply the host status (i.e sleep mode), it invokes unifi_run_bh().
+ *      Since we need all SDIO transaction to be in a single thread, the
+ *      unifi_run_bh() will wake up this thread to process it.
+ *
+ * ---------------------------------------------------------------------------
+ */
+static int
+bh_thread_function(void *arg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)arg;
+    CsrResult csrResult;
+    long ret;
+    u32 timeout, t;
+    struct uf_thread *this_thread;
+
+    unifi_trace(priv, UDBG2, "bh_thread_function starting\n");
+
+    this_thread = &priv->bh_thread;
+
+    t = timeout = 0;
+    while (!kthread_should_stop()) {
+        /* wait until an error occurs, or we need to process something. */
+        unifi_trace(priv, UDBG3, "bh_thread goes to sleep.\n");
+
+        if (timeout > 0) {
+            /* Convert t in ms to jiffies */
+            t = msecs_to_jiffies(timeout);
+            ret = wait_event_interruptible_timeout(this_thread->wakeup_q,
+                    (this_thread->wakeup_flag && !this_thread->block_thread) ||
+                    kthread_should_stop(),
+                    t);
+            timeout = (ret > 0) ? jiffies_to_msecs(ret) : 0;
+        } else {
+            ret = wait_event_interruptible(this_thread->wakeup_q,
+                    (this_thread->wakeup_flag && !this_thread->block_thread) ||
+                    kthread_should_stop());
+        }
+
+        if (kthread_should_stop()) {
+            unifi_trace(priv, UDBG2, "bh_thread: signalled to exit\n");
+            break;
+        }
+
+        if (ret < 0) {
+            unifi_notice(priv,
+                    "bh_thread: wait_event returned %d, thread will exit\n",
+                    ret);
+            uf_wait_for_thread_to_stop(priv, this_thread);
+            break;
+        }
+
+        this_thread->wakeup_flag = 0;
+
+        unifi_trace(priv, UDBG3, "bh_thread calls unifi_bh().\n");
+
+        CsrSdioClaim(priv->sdio);
+        csrResult = unifi_bh(priv->card, &timeout);
+        if(csrResult != CSR_RESULT_SUCCESS) {
+            if (csrResult == CSR_WIFI_HIP_RESULT_NO_DEVICE) {
+                CsrSdioRelease(priv->sdio);
+                uf_wait_for_thread_to_stop(priv, this_thread);
+                break;
+            }
+            /* Errors must be delivered to the error task */
+            handle_bh_error(priv);
+        }
+        CsrSdioRelease(priv->sdio);
+    }
+
+    /*
+     * I would normally try to call csr_sdio_remove_irq() here to make sure
+     * that we do not get any interrupts while this thread is not running.
+     * However, the MMC/SDIO driver tries to kill its' interrupt thread.
+     * The kernel threads implementation does not allow to kill threads
+     * from a signalled to stop thread.
+     * So, instead call csr_sdio_linux_remove_irq() always after calling
+     * uf_stop_thread() to kill this thread.
+     */
+
+    unifi_trace(priv, UDBG2, "bh_thread exiting....\n");
+    return 0;
+} /* bh_thread_function() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_init_bh
+ *
+ *      Helper function to start the bottom half of the driver.
+ *      All we need to do here is start the I/O bh thread.
+ *
+ *  Arguments:
+ *      priv            Pointer to OS driver structure for the device.
+ *
+ *  Returns:
+ *      0 on success or else a Linux error code.
+ * ---------------------------------------------------------------------------
+ */
+    int
+uf_init_bh(unifi_priv_t *priv)
+{
+    int r;
+
+    /* Enable mlme interface. */
+    priv->io_aborted = 0;
+
+
+    /* Start the BH thread */
+    r = uf_start_thread(priv, &priv->bh_thread, bh_thread_function);
+    if (r) {
+        unifi_error(priv,
+                "uf_init_bh: failed to start the BH thread.\n");
+        return r;
+    }
+
+    /* Allow interrupts */
+    r = csr_sdio_linux_install_irq(priv->sdio);
+    if (r) {
+        unifi_error(priv,
+                "uf_init_bh: failed to install the IRQ.\n");
+
+        uf_stop_thread(priv, &priv->bh_thread);
+    }
+
+    return r;
+} /* uf_init_bh() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_run_bh
+ *
+ *      Part of the HIP core lib API, implemented in the porting exercise.
+ *      The bottom half of the driver calls this function when
+ *      it wants to process anything that requires access to unifi.
+ *      We need to call unifi_bh() which in this implementation is done
+ *      by waking up the I/O thread.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to OS driver structure for the device.
+ *
+ *  Returns:
+ *      0 on success or else a Linux error code.
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_run_bh(void *ospriv)
+{
+    unifi_priv_t *priv = ospriv;
+
+    /*
+     * If an error has occured, we discard silently all messages from the bh
+     * until the error has been processed and the unifi has been reinitialised.
+     */
+    if (priv->bh_thread.block_thread == 1) {
+        unifi_trace(priv, UDBG3, "unifi_run_bh: discard message.\n");
+        /*
+         * Do not try to acknowledge a pending interrupt here.
+         * This function is called by unifi_send_signal() which in turn can be
+         * running in an atomic or 'disabled irq' level if a signal is sent
+         * from a workqueue task (i.e multicass addresses set).
+         * We can not hold the SDIO lock because it might sleep.
+         */
+        return CSR_RESULT_FAILURE;
+    }
+
+    priv->bh_thread.wakeup_flag = 1;
+    /* wake up I/O thread */
+    wake_up_interruptible(&priv->bh_thread.wakeup_q);
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_run_bh() */
+
diff --git a/drivers/staging/csr/csr_formatted_io.c b/drivers/staging/csr/csr_formatted_io.c
new file mode 100644
index 00000000000..7213cc8fb57
--- /dev/null
+++ b/drivers/staging/csr/csr_formatted_io.c
@@ -0,0 +1,27 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+#include <linux/kernel.h>
+#include "csr_formatted_io.h"
+
+s32 CsrSnprintf(char *dest, size_t n, const char *fmt, ...)
+{
+    s32 r;
+    va_list args;
+    va_start(args, fmt);
+    r = vsnprintf(dest, n, fmt, args);
+    va_end(args);
+
+    if (dest && (n > 0))
+    {
+        dest[n - 1] = '\0';
+    }
+
+    return r;
+}
diff --git a/drivers/staging/csr/csr_formatted_io.h b/drivers/staging/csr/csr_formatted_io.h
new file mode 100644
index 00000000000..2e238cb98d5
--- /dev/null
+++ b/drivers/staging/csr/csr_formatted_io.h
@@ -0,0 +1,25 @@
+#ifndef CSR_FORMATTED_IO_H__
+#define CSR_FORMATTED_IO_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <linux/types.h>
+
+s32 CsrSnprintf(char *dest, size_t n, const char *fmt, ...);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_framework_ext.c b/drivers/staging/csr/csr_framework_ext.c
new file mode 100644
index 00000000000..12e7ddf3220
--- /dev/null
+++ b/drivers/staging/csr/csr_framework_ext.c
@@ -0,0 +1,148 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/version.h>
+#include <linux/kthread.h>
+#include <linux/module.h>
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 34)
+#include <linux/slab.h>
+#endif
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 19)
+#include <linux/freezer.h>
+#endif
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 27)
+#include <asm/semaphore.h>
+#else
+#include <linux/semaphore.h>
+#endif
+
+#include <linux/bitops.h>
+
+#include "csr_framework_ext.h"
+#include "csr_panic.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexCreate
+ *
+ *  DESCRIPTION
+ *      Create a mutex and return a handle to the created mutex.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_NO_MORE_MUTEXES   in case of out of mutex resources
+ *          CSR_FE_RESULT_INVALID_POINTER   in case the mutexHandle pointer is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrMutexCreate(CsrMutexHandle *mutexHandle)
+{
+    if (mutexHandle == NULL)
+    {
+        return CSR_FE_RESULT_INVALID_POINTER;
+    }
+
+    sema_init(mutexHandle, 1);
+
+    return CSR_RESULT_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexDestroy
+ *
+ *  DESCRIPTION
+ *      Destroy the previously created mutex.
+ *
+ *  RETURNS
+ *      void
+ *
+ *----------------------------------------------------------------------------*/
+void CsrMutexDestroy(CsrMutexHandle *mutexHandle)
+{
+}
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexLock
+ *
+ *  DESCRIPTION
+ *      Lock the mutex refered to by the provided handle.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_INVALID_HANDLE    in case the mutexHandle is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrMutexLock(CsrMutexHandle *mutexHandle)
+{
+    if (mutexHandle == NULL)
+    {
+        return CSR_FE_RESULT_INVALID_POINTER;
+    }
+
+    if (down_interruptible(mutexHandle))
+    {
+        CsrPanic(CSR_TECH_FW, CSR_PANIC_FW_UNEXPECTED_VALUE, "CsrMutexLock Failed");
+        return CSR_FE_RESULT_INVALID_POINTER;
+    }
+
+    return CSR_RESULT_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexUnlock
+ *
+ *  DESCRIPTION
+ *      Unlock the mutex refered to by the provided handle.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_INVALID_HANDLE    in case the mutexHandle is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrMutexUnlock(CsrMutexHandle *mutexHandle)
+{
+    if (mutexHandle == NULL)
+    {
+        return CSR_FE_RESULT_INVALID_POINTER;
+    }
+
+    up(mutexHandle);
+
+    return CSR_RESULT_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrThreadSleep
+ *
+ *  DESCRIPTION
+ *      Sleep for a given period.
+ *
+ *  RETURNS
+ *      void
+ *
+ *----------------------------------------------------------------------------*/
+void CsrThreadSleep(u16 sleepTimeInMs)
+{
+    unsigned long t;
+
+    /* Convert t in ms to jiffies and round up */
+    t = ((sleepTimeInMs * HZ) + 999) / 1000;
+    schedule_timeout_uninterruptible(t);
+}
+EXPORT_SYMBOL_GPL(CsrThreadSleep);
diff --git a/drivers/staging/csr/csr_framework_ext.h b/drivers/staging/csr/csr_framework_ext.h
new file mode 100644
index 00000000000..66973e93a6b
--- /dev/null
+++ b/drivers/staging/csr/csr_framework_ext.h
@@ -0,0 +1,248 @@
+#ifndef CSR_FRAMEWORK_EXT_H__
+#define CSR_FRAMEWORK_EXT_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_result.h"
+#include "csr_framework_ext_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Result codes */
+#define CSR_FE_RESULT_NO_MORE_EVENTS    ((CsrResult) 0x0001)
+#define CSR_FE_RESULT_INVALID_POINTER   ((CsrResult) 0x0002)
+#define CSR_FE_RESULT_INVALID_HANDLE    ((CsrResult) 0x0003)
+#define CSR_FE_RESULT_NO_MORE_MUTEXES   ((CsrResult) 0x0004)
+#define CSR_FE_RESULT_TIMEOUT           ((CsrResult) 0x0005)
+#define CSR_FE_RESULT_NO_MORE_THREADS   ((CsrResult) 0x0006)
+
+/* Thread priorities */
+#define CSR_THREAD_PRIORITY_HIGHEST     ((u16) 0)
+#define CSR_THREAD_PRIORITY_HIGH        ((u16) 1)
+#define CSR_THREAD_PRIORITY_NORMAL      ((u16) 2)
+#define CSR_THREAD_PRIORITY_LOW         ((u16) 3)
+#define CSR_THREAD_PRIORITY_LOWEST      ((u16) 4)
+
+#define CSR_EVENT_WAIT_INFINITE         ((u16) 0xFFFF)
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrEventCreate
+ *
+ *  DESCRIPTION
+ *      Creates an event and returns a handle to the created event.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS          in case of success
+ *          CSR_FE_RESULT_NO_MORE_EVENTS   in case of out of event resources
+ *          CSR_FE_RESULT_INVALID_POINTER  in case the eventHandle pointer is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrEventCreate(CsrEventHandle *eventHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrEventWait
+ *
+ *  DESCRIPTION
+ *      Wait fore one or more of the event bits to be set.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS              in case of success
+ *          CSR_FE_RESULT_TIMEOUT              in case of timeout
+ *          CSR_FE_RESULT_INVALID_HANDLE       in case the eventHandle is invalid
+ *          CSR_FE_RESULT_INVALID_POINTER      in case the eventBits pointer is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrEventWait(CsrEventHandle *eventHandle, u16 timeoutInMs, u32 *eventBits);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrEventSet
+ *
+ *  DESCRIPTION
+ *      Set an event.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS              in case of success
+ *          CSR_FE_RESULT_INVALID_HANDLE       in case the eventHandle is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrEventSet(CsrEventHandle *eventHandle, u32 eventBits);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrEventDestroy
+ *
+ *  DESCRIPTION
+ *      Destroy the event associated.
+ *
+ *  RETURNS
+ *      void
+ *
+ *----------------------------------------------------------------------------*/
+void CsrEventDestroy(CsrEventHandle *eventHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexCreate
+ *
+ *  DESCRIPTION
+ *      Create a mutex and return a handle to the created mutex.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_NO_MORE_MUTEXES   in case of out of mutex resources
+ *          CSR_FE_RESULT_INVALID_POINTER   in case the mutexHandle pointer is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrMutexCreate(CsrMutexHandle *mutexHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexLock
+ *
+ *  DESCRIPTION
+ *      Lock the mutex refered to by the provided handle.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_INVALID_HANDLE    in case the mutexHandle is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrMutexLock(CsrMutexHandle *mutexHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexUnlock
+ *
+ *  DESCRIPTION
+ *      Unlock the mutex refered to by the provided handle.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_INVALID_HANDLE    in case the mutexHandle is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrMutexUnlock(CsrMutexHandle *mutexHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrMutexDestroy
+ *
+ *  DESCRIPTION
+ *      Destroy the previously created mutex.
+ *
+ *  RETURNS
+ *      void
+ *
+ *----------------------------------------------------------------------------*/
+void CsrMutexDestroy(CsrMutexHandle *mutexHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrGlobalMutexLock
+ *
+ *  DESCRIPTION
+ *      Lock the global mutex. The global mutex is a single pre-initialised
+ *      shared mutex, spinlock or similar that does not need to be created prior
+ *      to use. The limitation is that there is only one single lock shared
+ *      between all code. Consequently, it must only be used very briefly to
+ *      either protect simple one-time initialisation or to protect the creation
+ *      of a dedicated mutex by calling CsrMutexCreate.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrGlobalMutexLock(void);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrGlobalMutexUnlock
+ *
+ *  DESCRIPTION
+ *      Unlock the global mutex.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrGlobalMutexUnlock(void);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrThreadCreate
+ *
+ *  DESCRIPTION
+ *      Create thread function and return a handle to the created thread.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS           in case of success
+ *          CSR_FE_RESULT_NO_MORE_THREADS   in case of out of thread resources
+ *          CSR_FE_RESULT_INVALID_POINTER   in case one of the supplied pointers is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrThreadCreate(void (*threadFunction)(void *pointer), void *pointer,
+    u32 stackSize, u16 priority,
+    const char *threadName, CsrThreadHandle *threadHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrThreadGetHandle
+ *
+ *  DESCRIPTION
+ *      Return thread handle of calling thread.
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS             in case of success
+ *          CSR_FE_RESULT_INVALID_POINTER  in case the threadHandle pointer is invalid
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrThreadGetHandle(CsrThreadHandle *threadHandle);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrThreadEqual
+ *
+ *  DESCRIPTION
+ *      Compare thread handles
+ *
+ *  RETURNS
+ *      Possible values:
+ *          CSR_RESULT_SUCCESS             in case thread handles are identical
+ *          CSR_FE_RESULT_INVALID_POINTER  in case either threadHandle pointer is invalid
+ *          CSR_RESULT_FAILURE             otherwise
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrThreadEqual(CsrThreadHandle *threadHandle1, CsrThreadHandle *threadHandle2);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrThreadSleep
+ *
+ *  DESCRIPTION
+ *      Sleep for a given period.
+ *
+ *  RETURNS
+ *      void
+ *
+ *----------------------------------------------------------------------------*/
+void CsrThreadSleep(u16 sleepTimeInMs);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_framework_ext_types.h b/drivers/staging/csr/csr_framework_ext_types.h
new file mode 100644
index 00000000000..57194ee911e
--- /dev/null
+++ b/drivers/staging/csr/csr_framework_ext_types.h
@@ -0,0 +1,63 @@
+#ifndef CSR_FRAMEWORK_EXT_TYPES_H__
+#define CSR_FRAMEWORK_EXT_TYPES_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifdef __KERNEL__
+#include <linux/kthread.h>
+#include <linux/semaphore.h>
+#else
+#include <pthread.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#ifdef __KERNEL__
+
+struct CsrThread
+{
+    struct task_struct *thread_task;
+    char                name[16];
+};
+
+struct CsrEvent
+{
+    /* wait_queue for waking the kernel thread */
+    wait_queue_head_t wakeup_q;
+    unsigned int      wakeup_flag;
+};
+
+typedef struct CsrEvent CsrEventHandle;
+typedef struct semaphore CsrMutexHandle;
+typedef struct CsrThread CsrThreadHandle;
+
+#else /* __KERNEL __ */
+
+struct CsrEvent
+{
+    pthread_cond_t  event;
+    pthread_mutex_t mutex;
+    u32       eventBits;
+};
+
+typedef struct CsrEvent CsrEventHandle;
+typedef pthread_mutex_t CsrMutexHandle;
+typedef pthread_t CsrThreadHandle;
+
+#endif /* __KERNEL__ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_lib.h b/drivers/staging/csr/csr_lib.h
new file mode 100644
index 00000000000..b1a57d5d06f
--- /dev/null
+++ b/drivers/staging/csr/csr_lib.h
@@ -0,0 +1,188 @@
+#ifndef CSR_LIB_H__
+#define CSR_LIB_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_prim_defs.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct
+{
+    CsrPrim type;
+} CsrEvent;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEvent_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEvent
+ *
+ *----------------------------------------------------------------------------*/
+CsrEvent *CsrEvent_struct(u16 primtype, u16 msgtype);
+
+typedef struct
+{
+    CsrPrim  type;
+    u8 value;
+} CsrEventCsrUint8;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint8_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint8
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint8 *CsrEventCsrUint8_struct(u16 primtype, u16 msgtype, u8 value);
+
+typedef struct
+{
+    CsrPrim   type;
+    u16 value;
+} CsrEventCsrUint16;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint16_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint16
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint16 *CsrEventCsrUint16_struct(u16 primtype, u16 msgtype, u16 value);
+
+typedef struct
+{
+    CsrPrim   type;
+    u16 value1;
+    u8  value2;
+} CsrEventCsrUint16CsrUint8;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint16CsrUint8_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint16CsrUint8
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint16CsrUint8 *CsrEventCsrUint16CsrUint8_struct(u16 primtype, u16 msgtype, u16 value1, u8 value2);
+
+typedef struct
+{
+    CsrPrim   type;
+    u16 value1;
+    u16 value2;
+} CsrEventCsrUint16CsrUint16;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint16CsrUint16_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint16CsrUint16
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint16CsrUint16 *CsrEventCsrUint16CsrUint16_struct(u16 primtype, u16 msgtype, u16 value1, u16 value2);
+
+typedef struct
+{
+    CsrPrim   type;
+    u16 value1;
+    u32 value2;
+} CsrEventCsrUint16CsrUint32;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint16_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint16
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint16CsrUint32 *CsrEventCsrUint16CsrUint32_struct(u16 primtype, u16 msgtype, u16 value1, u32 value2);
+
+typedef struct
+{
+    CsrPrim        type;
+    u16      value1;
+    char *value2;
+} CsrEventCsrUint16CsrCharString;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint16CsrCharString_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint16CsrCharString
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint16CsrCharString *CsrEventCsrUint16CsrCharString_struct(u16 primtype, u16 msgtype, u16 value1, char *value2);
+
+typedef struct
+{
+    CsrPrim   type;
+    u32 value;
+} CsrEventCsrUint32;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint32_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint32
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint32 *CsrEventCsrUint32_struct(u16 primtype, u16 msgtype, u32 value);
+
+typedef struct
+{
+    CsrPrim   type;
+    u32 value1;
+    u16 value2;
+} CsrEventCsrUint32CsrUint16;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint32CsrUint16_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint32CsrUint16
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint32CsrUint16 *CsrEventCsrUint32CsrUint16_struct(u16 primtype, u16 msgtype, u32 value1, u32 value2);
+
+typedef struct
+{
+    CsrPrim        type;
+    u32      value1;
+    char *value2;
+} CsrEventCsrUint32CsrCharString;
+
+/*----------------------------------------------------------------------------*
+ *  CsrEventCsrUint32CsrCharString_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrEventCsrUint32CsrCharString
+ *
+ *----------------------------------------------------------------------------*/
+CsrEventCsrUint32CsrCharString *CsrEventCsrUint32CsrCharString_struct(u16 primtype, u16 msgtype, u32 value1, char *value2);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_LIB_H__ */
diff --git a/drivers/staging/csr/csr_log.h b/drivers/staging/csr/csr_log.h
new file mode 100644
index 00000000000..b2808569f8d
--- /dev/null
+++ b/drivers/staging/csr/csr_log.h
@@ -0,0 +1,249 @@
+#ifndef CSR_LOG_H__
+#define CSR_LOG_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_sched.h"
+#include "csr_panic.h"
+#include "csr_prim_defs.h"
+#include "csr_msgconv.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * Log filtering
+ */
+
+/*----------------------------------------------------*/
+/*  Filtering on environment specific log levels      */
+/*----------------------------------------------------*/
+typedef u32 CsrLogLevelEnvironment;
+#define CSR_LOG_LEVEL_ENVIRONMENT_OFF          ((CsrLogLevelEnvironment) 0x00000000) /* No environment data/events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BCI_ACL      ((CsrLogLevelEnvironment) 0x00000001) /* BlueCore Channel Interface HCI Acl data are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BCI_HCI      ((CsrLogLevelEnvironment) 0x00000002) /* BlueCore Channel Interface HCI Cmd/Evt data are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BCI_SCO      ((CsrLogLevelEnvironment) 0x00000004) /* BlueCore Channel Interface HCI Sco data are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BCI_VENDOR   ((CsrLogLevelEnvironment) 0x00000008) /* BlueCore Channel Interface HCI Vendor specific data are logged (This includes BCCMD, HQ, VM etc) */
+#define CSR_LOG_LEVEL_ENVIRONMENT_TRANSPORTS   ((CsrLogLevelEnvironment) 0x00000010) /* Transport protocol data is logged (This includes transport protocols like BCSP, H4 etc.) */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_REG    ((CsrLogLevelEnvironment) 0x00000020) /* Background Interrupt registration events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_UNREG  ((CsrLogLevelEnvironment) 0x00000040) /* Background Interrupt unregistration events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_SET    ((CsrLogLevelEnvironment) 0x00000080) /* Background Interrupt set events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_START  ((CsrLogLevelEnvironment) 0x00000100) /* Background Interrupt start events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_BGINT_DONE   ((CsrLogLevelEnvironment) 0x00000200) /* Background Interrupt done events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_PROTO        ((CsrLogLevelEnvironment) 0x00000400) /* Transport protocol events are logged */
+#define CSR_LOG_LEVEL_ENVIRONMENT_PROTO_LOC    ((CsrLogLevelEnvironment) 0x00000800) /* The Location where the transport protocol event occured are logged NB: This is a supplement to CSR_LOG_LEVEL_ENVIRONMENT_PROTO, it has no effect without it */
+/* The bit masks between here are reserved for future usage */
+#define CSR_LOG_LEVEL_ENVIRONMENT_ALL          ((CsrLogLevelEnvironment) 0xFFFFFFFF) /* All possible environment data/events are logged WARNING: By using this define the application also accepts future possible environment data/events in the logs */
+
+/*----------------------------------------------------*/
+/*  Filtering on task specific log levels             */
+/*----------------------------------------------------*/
+typedef u32 CsrLogLevelTask;
+#define CSR_LOG_LEVEL_TASK_OFF                 ((CsrLogLevelTask) 0x00000000) /* No events are logged for this task */
+#define CSR_LOG_LEVEL_TASK_TEXT                ((CsrLogLevelTask) 0x00000001) /* Text strings printed by a task are logged NB: This bit does not affect the CSR_LOG_TEXT_LEVEL interface. This has to be configured separately */
+#define CSR_LOG_LEVEL_TASK_TEXT_LOC            ((CsrLogLevelTask) 0x00000002) /* The locaction where the text string call occured are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_TEXT, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_STATE               ((CsrLogLevelTask) 0x00000004) /* FSM state transitions in a task are logged */
+#define CSR_LOG_LEVEL_TASK_STATE_NAME          ((CsrLogLevelTask) 0x00000008) /* The name of each state in a FSM state transition are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_STATE, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_STATE_LOC           ((CsrLogLevelTask) 0x00000010) /* The location where the FSM state transition occured are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_STATE, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_TASK_SWITCH         ((CsrLogLevelTask) 0x00000020) /* Activation and deactiation of a task are logged */
+#define CSR_LOG_LEVEL_TASK_MESSAGE_PUT         ((CsrLogLevelTask) 0x00000080) /* Message put operations are logged */
+#define CSR_LOG_LEVEL_TASK_MESSAGE_PUT_LOC     ((CsrLogLevelTask) 0x00000100) /* The location where a message was sent are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_MESSAGE_PUT, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_MESSAGE_GET         ((CsrLogLevelTask) 0x00000200) /* Message get operations are logged */
+#define CSR_LOG_LEVEL_TASK_MESSAGE_QUEUE_PUSH  ((CsrLogLevelTask) 0x00000400) /* Message push operations are logged */
+#define CSR_LOG_LEVEL_TASK_MESSAGE_QUEUE_POP   ((CsrLogLevelTask) 0x00000800) /* Message pop operations are logged */
+#define CSR_LOG_LEVEL_TASK_PRIM_ONLY_TYPE      ((CsrLogLevelTask) 0x00001000) /* Only the type of primitives in messages are logged. By default the entire primitive is serialized and logged */
+#define CSR_LOG_LEVEL_TASK_PRIM_APPLY_LIMIT    ((CsrLogLevelTask) 0x00002000) /* An upper limit (defined by CSR_LOG_PRIM_SIZE_UPPER_LIMIT) is applied to how much of a primitive in a message are logged. NB: This limit is only applied if CSR_LOG_LEVEL_TASK_PRIM_ONLY_TYPE is _not_ defined */
+#define CSR_LOG_LEVEL_TASK_TIMER_IN            ((CsrLogLevelTask) 0x00004000) /* TimedEventIn events are logged */
+#define CSR_LOG_LEVEL_TASK_TIMER_IN_LOC        ((CsrLogLevelTask) 0x00008000) /* The location where a timer was started are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_TIMER_IN, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_TIMER_CANCEL        ((CsrLogLevelTask) 0x00010000) /* TimedEventCancel events are logged */
+#define CSR_LOG_LEVEL_TASK_TIMER_CANCEL_LOC    ((CsrLogLevelTask) 0x00020000) /* The location where a timer was cancelled are logged. NB: This is a supplement to CSR_LOG_LEVEL_TASK_TIMER_CANCEL, it has no effect without it */
+#define CSR_LOG_LEVEL_TASK_TIMER_FIRE          ((CsrLogLevelTask) 0x00040000) /* TimedEventFire events are logged */
+#define CSR_LOG_LEVEL_TASK_TIMER_DONE          ((CsrLogLevelTask) 0x00080000) /* TimedEventDone events are logged */
+/* The bit masks between here are reserved for future usage */
+#define CSR_LOG_LEVEL_TASK_ALL                 ((CsrLogLevelTask) 0xFFFFFFFF & ~(CSR_LOG_LEVEL_TASK_PRIM_ONLY_TYPE | CSR_LOG_LEVEL_TASK_PRIM_APPLY_LIMIT)) /* All info possible to log for a task are logged. WARNING: By using this define the application also accepts future possible task data/events in the logs */
+
+u8 CsrLogEnvironmentIsFiltered(CsrLogLevelEnvironment level);
+CsrLogLevelTask CsrLogTaskFilterGet(CsrSchedQid taskId);
+u8 CsrLogTaskIsFiltered(CsrSchedQid taskId, CsrLogLevelTask level);
+
+/*
+ * Logging stuff
+ */
+#define CSR_LOG_STRINGIFY_REAL(a) #a
+#define CSR_LOG_STRINGIFY(a) CSR_LOG_STRINGIFY_REAL(a)
+
+#ifdef CSR_LOG_ASSERT_ENABLE
+#define CSR_LOG_ASSERT(cond)                        \
+    do {                                                \
+        if (!(cond))                                    \
+        {                                               \
+            char *panic_arg = "[" __FILE__ ":" CSR_LOG_STRINGIFY(__LINE__) "] - " CSR_LOG_STRINGIFY(cond); \
+            CsrPanic(CSR_TECH_FW, CSR_PANIC_FW_ASSERTION_FAIL, panic_arg); \
+        }                                               \
+    } while (0)
+#else
+#define CSR_LOG_ASSERT(cond)
+#endif
+
+typedef struct
+{
+    u16            primitiveType;
+    const char *primitiveName;
+    CsrMsgConvMsgEntry  *messageConv; /* Private - do not use */
+} CsrLogPrimitiveInformation;
+
+typedef struct
+{
+    const char        *techVer;
+    u32                   primitiveInfoCount;
+    CsrLogPrimitiveInformation *primitiveInfo;
+} CsrLogTechInformation;
+
+/*---------------------------------*/
+/*  Tech logging */
+/*---------------------------------*/
+typedef u8 bitmask8_t;
+typedef u16 bitmask16_t;
+typedef u32 bitmask32_t;
+
+#ifdef CSR_LOG_ENABLE
+#ifdef CSR_LOG_INCLUDE_FILE_NAME_AND_LINE_NUMBER
+/* DEPRECATED - replaced by csr_log_text.h */
+#define CSR_LOG_TEXT(text) \
+    do { \
+        if (!CsrLogTaskIsFiltered(CsrSchedTaskQueueGet(), CSR_LOG_LEVEL_TASK_TEXT)) \
+        { \
+            CsrLogTaskText(text, __LINE__, __FILE__); \
+        } \
+    } while (0)
+#else
+/* DEPRECATED - replaced by csr_log_text.h */
+#define CSR_LOG_TEXT(text) \
+    do { \
+        if (!CsrLogTaskIsFiltered(CsrSchedTaskQueueGet(), CSR_LOG_LEVEL_TASK_TEXT)) \
+        { \
+            CsrLogTaskText(text, 0, NULL); \
+        } \
+    } while (0)
+#endif
+#else
+#define CSR_LOG_TEXT(text)
+#endif
+
+/* DEPRECATED - replaced by csr_log_text.h */
+void CsrLogTaskText(const char *text,
+    u32 line,
+    const char *file);
+
+#define CSR_LOG_STATE_TRANSITION_MASK_FSM_NAME          (0x001)
+#define CSR_LOG_STATE_TRANSITION_MASK_NEXT_STATE        (0x002)
+#define CSR_LOG_STATE_TRANSITION_MASK_NEXT_STATE_STR    (0x004)
+#define CSR_LOG_STATE_TRANSITION_MASK_PREV_STATE        (0x008)
+#define CSR_LOG_STATE_TRANSITION_MASK_PREV_STATE_STR    (0x010)
+#define CSR_LOG_STATE_TRANSITION_MASK_EVENT             (0x020)
+#define CSR_LOG_STATE_TRANSITION_MASK_EVENT_STR         (0x040)
+
+/* DEPRECATED - replaced by csr_log_text.h */
+void CsrLogStateTransition(bitmask16_t mask,
+    u32 identifier,
+    const char *fsm_name,
+    u32 prev_state,
+    const char *prev_state_str,
+    u32 in_event,
+    const char *in_event_str,
+    u32 next_state,
+    const char *next_state_str,
+    u32 line,
+    const char *file);
+
+/*---------------------------------*/
+/*  BSP logging */
+/*---------------------------------*/
+void CsrLogSchedInit(u8 thread_id);
+void CsrLogSchedDeinit(u8 thread_id);
+
+void CsrLogSchedStart(u8 thread_id);
+void CsrLogSchedStop(u8 thread_id);
+
+void CsrLogInitTask(u8 thread_id, CsrSchedQid tskid, const char *tskName);
+void CsrLogDeinitTask(u16 task_id);
+
+void CsrLogActivate(CsrSchedQid tskid);
+void CsrLogDeactivate(CsrSchedQid tskid);
+
+#define SYNERGY_SERIALIZER_TYPE_DUMP    (0x000)
+#define SYNERGY_SERIALIZER_TYPE_SER     (0x001)
+
+void CsrLogMessagePut(u32 line,
+    const char *file,
+    CsrSchedQid src_task_id,
+    CsrSchedQid dst_taskid,
+    CsrSchedMsgId msg_id,
+    u16 prim_type,
+    const void *msg);
+
+void CsrLogMessageGet(CsrSchedQid src_task_id,
+    CsrSchedQid dst_taskid,
+    u8 get_res,
+    CsrSchedMsgId msg_id,
+    u16 prim_type,
+    const void *msg);
+
+void CsrLogTimedEventIn(u32 line,
+    const char *file,
+    CsrSchedQid task_id,
+    CsrSchedTid tid,
+    CsrTime requested_delay,
+    u16 fniarg,
+    const void *fnvarg);
+
+void CsrLogTimedEventFire(CsrSchedQid task_id,
+    CsrSchedTid tid);
+
+void CsrLogTimedEventDone(CsrSchedQid task_id,
+    CsrSchedTid tid);
+
+void CsrLogTimedEventCancel(u32 line,
+    const char *file,
+    CsrSchedQid task_id,
+    CsrSchedTid tid,
+    u8 cancel_res);
+
+void CsrLogBgintRegister(u8 thread_id,
+    CsrSchedBgint irq,
+    const char *callback,
+    const void *ptr);
+void CsrLogBgintUnregister(CsrSchedBgint irq);
+void CsrLogBgintSet(CsrSchedBgint irq);
+void CsrLogBgintServiceStart(CsrSchedBgint irq);
+void CsrLogBgintServiceDone(CsrSchedBgint irq);
+
+void CsrLogExceptionStateEvent(u16 prim_type,
+    CsrPrim msg_type,
+    u16 state,
+    u32 line,
+    const char *file);
+void CsrLogExceptionGeneral(u16 prim_type,
+    u16 state,
+    const char *text,
+    u32 line,
+    const char *file);
+void CsrLogExceptionWarning(u16 prim_type,
+    u16 state,
+    const char *text,
+    u32 line,
+    const char *file);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_log_configure.h b/drivers/staging/csr/csr_log_configure.h
new file mode 100644
index 00000000000..8842e4bf461
--- /dev/null
+++ b/drivers/staging/csr/csr_log_configure.h
@@ -0,0 +1,134 @@
+#ifndef CSR_LOG_CONFIGURE_H__
+#define CSR_LOG_CONFIGURE_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_log.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*---------------------------------*/
+/* Log init/deinit                 */
+/*---------------------------------*/
+void CsrLogInit(u8 size);
+void CsrLogDeinit(void);
+
+/*---------------------------------*/
+/* Log Framework Tech info         */
+/*---------------------------------*/
+void CsrLogTechInfoRegister(void);
+
+/* Set the logging level for the environment outside the scheduler context */
+void CsrLogLevelEnvironmentSet(CsrLogLevelEnvironment environmentLogLevel);
+
+
+/* Set the logging level for all scheduler tasks */
+/* This function call takes precedence over all previous calls to CsrLogLevelTaskSetSpecific() */
+void CsrLogLevelTaskSetAll(CsrLogLevelTask tasksLogLevelMask);
+
+/* Set the logging level for a given Task */
+/* This function can be used as a complement to CsrLogLevelTaskSetAll() to add more _or_ less log from a given task than what is set
+generally with CsrLogLevelTaskSetAll(). */
+void CsrLogLevelTaskSetSpecific(CsrSchedQid taskId, CsrLogLevelTask taskLogLevelMask);
+
+
+/*--------------------------------------------*/
+/*  Filtering on log text warning levels      */
+/*--------------------------------------------*/
+typedef u32 CsrLogLevelText;
+#define CSR_LOG_LEVEL_TEXT_OFF       ((CsrLogLevelText) 0x0000)
+
+#define CSR_LOG_LEVEL_TEXT_CRITICAL  ((CsrLogLevelText) 0x0001)
+#define CSR_LOG_LEVEL_TEXT_ERROR     ((CsrLogLevelText) 0x0002)
+#define CSR_LOG_LEVEL_TEXT_WARNING   ((CsrLogLevelText) 0x0004)
+#define CSR_LOG_LEVEL_TEXT_INFO      ((CsrLogLevelText) 0x0008)
+#define CSR_LOG_LEVEL_TEXT_DEBUG     ((CsrLogLevelText) 0x0010)
+
+#define CSR_LOG_LEVEL_TEXT_ALL       ((CsrLogLevelText) 0xFFFF)
+
+/* The log text interface is used by both scheduler tasks and components outside the scheduler context.
+ * Therefore a CsrLogTextTaskId is introduced. It is effectively considered as two u16's. The lower
+ * 16 bits corresponds one2one with the scheduler queueId's (CsrSchedQid) and as such these bits can not be used
+ * by components outside scheduler tasks. The upper 16 bits are allocated for use of components outside the
+ * scheduler like drivers etc. Components in this range is defined independently by each technology. To avoid
+ * clashes the technologies are only allowed to assign values within the same restrictive range as allies to
+ * primitive identifiers. eg. for the framework components outside the scheduler is only allowed to assign
+ * taskId's in the range 0x0600xxxx to 0x06FFxxxx. And so on for other technologies. */
+typedef u32 CsrLogTextTaskId;
+
+/* Set the text logging level for all Tasks */
+/* This function call takes precedence over all previous calls to CsrLogLevelTextSetTask() and CsrLogLevelTextSetTaskSubOrigin() */
+void CsrLogLevelTextSetAll(CsrLogLevelText warningLevelMask);
+
+/* Set the text logging level for a given Task */
+/* This function call takes precedence over all previous calls to CsrLogLevelTextSetTaskSubOrigin(), but it can be used as a complement to
+ * CsrLogLevelTextSetAll() to add more _or_ less log from a given task than what is set generally with CsrLogLevelTextSetAll(). */
+void CsrLogLevelTextSetTask(CsrLogTextTaskId taskId, CsrLogLevelText warningLevelMask);
+
+/* Set the text logging level for a given tasks subOrigin */
+/* This function can be used as a complement to CsrLogLevelTextSetAll() and CsrLogLevelTextSetTask() to add more _or_ less log from a given
+ * subOrigin within a task than what is set generally with CsrLogLevelTextSetAll() _or_ CsrLogLevelTextSetTask(). */
+void CsrLogLevelTextSetTaskSubOrigin(CsrLogTextTaskId taskId, u16 subOrigin, CsrLogLevelText warningLevelMask);
+
+/*******************************************************************************
+
+    NAME
+        CsrLogLevelTextSet
+
+    DESCRIPTION
+        Set the text logging level for a given origin and optionally sub origin
+        by name. If either string is NULL or zero length, it is interpreted as
+        all origins and/or all sub origins respectively. If originName is NULL
+        or zero length, subOriginName is ignored.
+
+        Passing NULL or zero length strings in both originName and subOriginName
+        is equivalent to calling CsrLogLevelTextSetAll, and overrides all
+        previous filter configurations for all origins and sub origins.
+
+        Passing NULL or a zero length string in subOriginName overrides all
+        previous filter configurations for all sub origins of the specified
+        origin.
+
+        Note: the supplied strings may be accessed after the function returns
+        and must remain valid and constant until CsrLogDeinit is called.
+
+        Note: when specifying an origin (originName is not NULL and not zero
+        length), this function can only be used for origins that use the
+        csr_log_text_2.h interface for registration and logging. Filtering for
+        origins that use the legacy csr_log_text.h interface must be be
+        configured using the legacy filter configuration functions that accept
+        a CsrLogTextTaskId as origin specifier. However, when not specifying an
+        origin this function also affects origins that have been registered with
+        the legacy csr_log_text.h interface. Furthermore, using this function
+        and the legacy filter configuration functions on the same origin is not
+        allowed.
+
+    PARAMETERS
+        originName - a string containing the name of the origin. Can be NULL or
+            zero length to set the log level for all origins. In this case, the
+            subOriginName parameter will be ignored.
+        subOriginName - a string containing the name of the sub origin. Can be
+            NULL or zero length to set the log level for all sub origins of the
+            specified origin.
+        warningLevelMask - The desired log level for the specified origin(s) and
+            sub origin(s).
+
+*******************************************************************************/
+void CsrLogLevelTextSet(const char *originName,
+    const char *subOriginName,
+    CsrLogLevelText warningLevelMask);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_log_text.h b/drivers/staging/csr/csr_log_text.h
new file mode 100644
index 00000000000..9fe6c90244c
--- /dev/null
+++ b/drivers/staging/csr/csr_log_text.h
@@ -0,0 +1,132 @@
+#ifndef CSR_LOG_TEXT_H__
+#define CSR_LOG_TEXT_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_log_configure.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct CsrLogSubOrigin
+{
+    u16            subOriginNumber;  /* Id of the given SubOrigin */
+    const char *subOriginName;    /* Prefix Text for this SubOrigin */
+} CsrLogSubOrigin;
+
+/* Register a task which is going to use the CSR_LOG_TEXT_XXX interface */
+#ifdef CSR_LOG_ENABLE
+void CsrLogTextRegister(CsrLogTextTaskId taskId, const char *taskName, u16 subOriginsLength, const CsrLogSubOrigin *subOrigins);
+#else
+#define CsrLogTextRegister(taskId, taskName, subOriginsLength, subOrigins)
+#endif
+
+/* CRITICAL: Conditions that are threatening to the integrity/stability of the
+   system as a whole. */
+#if defined(CSR_LOG_ENABLE) && !defined(CSR_LOG_LEVEL_TEXT_CRITICAL_DISABLE)
+void CsrLogTextCritical(CsrLogTextTaskId taskId, u16 subOrigin, const char *formatString, ...);
+void CsrLogTextBufferCritical(CsrLogTextTaskId taskId, u16 subOrigin, size_t bufferLength, const void *buffer, const char *formatString, ...);
+#define CSR_LOG_TEXT_CRITICAL(taskId_subOrigin_formatString_varargs) CsrLogTextCritical taskId_subOrigin_formatString_varargs
+#define CSR_LOG_TEXT_CONDITIONAL_CRITICAL(condition, logtextargs) {if (condition) {CSR_LOG_TEXT_CRITICAL(logtextargs);}}
+#define CSR_LOG_TEXT_BUFFER_CRITICAL(taskId_subOrigin_length_buffer_formatString_varargs) CsrLogTextBufferCritical taskId_subOrigin_length_buffer_formatString_varargs
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_CRITICAL(condition, logtextbufferargs) {if (condition) {CSR_LOG_TEXT_BUFFER_CRITICAL(logtextbufferargs);}}
+#else
+#define CSR_LOG_TEXT_CRITICAL(taskId_subOrigin_formatString_varargs)
+#define CSR_LOG_TEXT_CONDITIONAL_CRITICAL(condition, logtextargs)
+#define CSR_LOG_TEXT_BUFFER_CRITICAL(taskId_subOrigin_length_buffer_formatString_varargs)
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_CRITICAL(condition, logtextbufferargs)
+#endif
+
+/* ERROR: Malfunction of a component rendering it unable to operate correctly,
+   causing lack of functionality but not loss of system integrity/stability. */
+#if defined(CSR_LOG_ENABLE) && !defined(CSR_LOG_LEVEL_TEXT_ERROR_DISABLE)
+void CsrLogTextError(CsrLogTextTaskId taskId, u16 subOrigin, const char *formatString, ...);
+void CsrLogTextBufferError(CsrLogTextTaskId taskId, u16 subOrigin, size_t bufferLength, const void *buffer, const char *formatString, ...);
+#define CSR_LOG_TEXT_ERROR(taskId_subOrigin_formatString_varargs) CsrLogTextError taskId_subOrigin_formatString_varargs
+#define CSR_LOG_TEXT_CONDITIONAL_ERROR(condition, logtextargs) {if (condition) {CSR_LOG_TEXT_ERROR(logtextargs);}}
+#define CSR_LOG_TEXT_BUFFER_ERROR(taskId_subOrigin_length_buffer_formatString_varargs) CsrLogTextBufferError taskId_subOrigin_length_buffer_formatString_varargs
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_ERROR(condition, logtextbufferargs) {if (condition) {CSR_LOG_TEXT_BUFFER_ERROR(logtextbufferargs);}}
+#else
+#define CSR_LOG_TEXT_ERROR(taskId_subOrigin_formatString_varargs)
+#define CSR_LOG_TEXT_CONDITIONAL_ERROR(condition, logtextargs)
+#define CSR_LOG_TEXT_BUFFER_ERROR(taskId_subOrigin_length_buffer_formatString_varargs)
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_ERROR(condition, logtextbufferargs)
+#endif
+
+/* WARNING: Conditions that are unexpected and indicative of possible problems
+   or violations of specifications, where the result of such deviations does not
+   lead to malfunction of the component. */
+#if defined(CSR_LOG_ENABLE) && !defined(CSR_LOG_LEVEL_TEXT_WARNING_DISABLE)
+void CsrLogTextWarning(CsrLogTextTaskId taskId, u16 subOrigin, const char *formatString, ...);
+void CsrLogTextBufferWarning(CsrLogTextTaskId taskId, u16 subOrigin, size_t bufferLength, const void *buffer, const char *formatString, ...);
+#define CSR_LOG_TEXT_WARNING(taskId_subOrigin_formatString_varargs) CsrLogTextWarning taskId_subOrigin_formatString_varargs
+#define CSR_LOG_TEXT_CONDITIONAL_WARNING(condition, logtextargs) {if (condition) {CSR_LOG_TEXT_WARNING(logtextargs);}}
+#define CSR_LOG_TEXT_BUFFER_WARNING(taskId_subOrigin_length_buffer_formatString_varargs) CsrLogTextBufferWarning taskId_subOrigin_length_buffer_formatString_varargs
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_WARNING(condition, logtextbufferargs) {if (condition) {CSR_LOG_TEXT_BUFFER_WARNING(logtextbufferargs);}}
+#else
+#define CSR_LOG_TEXT_WARNING(taskId_subOrigin_formatString_varargs)
+#define CSR_LOG_TEXT_CONDITIONAL_WARNING(condition, logtextargs)
+#define CSR_LOG_TEXT_BUFFER_WARNING(taskId_subOrigin_length_buffer_formatString_varargs)
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_WARNING(condition, logtextbufferargs)
+#endif
+
+/* INFO: Important events that may aid in determining the conditions under which
+   the more severe conditions are encountered. */
+#if defined(CSR_LOG_ENABLE) && !defined(CSR_LOG_LEVEL_TEXT_INFO_DISABLE)
+void CsrLogTextInfo(CsrLogTextTaskId taskId, u16 subOrigin, const char *formatString, ...);
+void CsrLogTextBufferInfo(CsrLogTextTaskId taskId, u16 subOrigin, size_t bufferLength, const void *buffer, const char *formatString, ...);
+#define CSR_LOG_TEXT_INFO(taskId_subOrigin_formatString_varargs) CsrLogTextInfo taskId_subOrigin_formatString_varargs
+#define CSR_LOG_TEXT_CONDITIONAL_INFO(condition, logtextargs) {if (condition) {CSR_LOG_TEXT_INFO(logtextargs);}}
+#define CSR_LOG_TEXT_BUFFER_INFO(taskId_subOrigin_length_buffer_formatString_varargs) CsrLogTextBufferInfo taskId_subOrigin_length_buffer_formatString_varargs
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_INFO(condition, logtextbufferargs) {if (condition) {CSR_LOG_TEXT_BUFFER_INFO(logtextbufferargs);}}
+#else
+#define CSR_LOG_TEXT_INFO(taskId_subOrigin_formatString_varargs)
+#define CSR_LOG_TEXT_CONDITIONAL_INFO(condition, logtextargs)
+#define CSR_LOG_TEXT_BUFFER_INFO(taskId_subOrigin_length_buffer_formatString_varargs)
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_INFO(condition, logtextbufferargs)
+#endif
+
+/* DEBUG: Similar to INFO, but dedicated to events that occur more frequently. */
+#if defined(CSR_LOG_ENABLE) && !defined(CSR_LOG_LEVEL_TEXT_DEBUG_DISABLE)
+void CsrLogTextDebug(CsrLogTextTaskId taskId, u16 subOrigin, const char *formatString, ...);
+void CsrLogTextBufferDebug(CsrLogTextTaskId taskId, u16 subOrigin, size_t bufferLength, const void *buffer, const char *formatString, ...);
+#define CSR_LOG_TEXT_DEBUG(taskId_subOrigin_formatString_varargs) CsrLogTextDebug taskId_subOrigin_formatString_varargs
+#define CSR_LOG_TEXT_CONDITIONAL_DEBUG(condition, logtextargs) {if (condition) {CSR_LOG_TEXT_DEBUG(logtextargs);}}
+#define CSR_LOG_TEXT_BUFFER_DEBUG(taskId_subOrigin_length_buffer_formatString_varargs) CsrLogTextBufferDebug taskId_subOrigin_length_buffer_formatString_varargs
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_DEBUG(condition, logtextbufferargs) {if (condition) {CSR_LOG_TEXT_BUFFER_DEBUG(logtextbufferargs);}}
+#else
+#define CSR_LOG_TEXT_DEBUG(taskId_subOrigin_formatString_varargs)
+#define CSR_LOG_TEXT_CONDITIONAL_DEBUG(condition, logtextargs)
+#define CSR_LOG_TEXT_BUFFER_DEBUG(taskId_subOrigin_length_buffer_formatString_varargs)
+#define CSR_LOG_TEXT_BUFFER_CONDITIONAL_DEBUG(condition, logtextbufferargs)
+#endif
+
+/* CSR_LOG_TEXT_ASSERT (CRITICAL) */
+#ifdef CSR_LOG_ENABLE
+#define CSR_LOG_TEXT_ASSERT(origin, suborigin, condition) \
+    {if (!(condition)) {CSR_LOG_TEXT_CRITICAL((origin, suborigin, "Assertion \"%s\" failed at %s:%u", #condition, __FILE__, __LINE__));}}
+#else
+#define CSR_LOG_TEXT_ASSERT(origin, suborigin, condition)
+#endif
+
+/* CSR_LOG_TEXT_UNHANDLED_PRIM (CRITICAL) */
+#ifdef CSR_LOG_ENABLE
+#define CSR_LOG_TEXT_UNHANDLED_PRIMITIVE(origin, suborigin, primClass, primType) \
+    CSR_LOG_TEXT_CRITICAL((origin, suborigin, "Unhandled primitive 0x%04X:0x%04X at %s:%u", primClass, primType, __FILE__, __LINE__))
+#else
+#define CSR_LOG_TEXT_UNHANDLED_PRIMITIVE(origin, suborigin, primClass, primType)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_macro.h b/drivers/staging/csr/csr_macro.h
new file mode 100644
index 00000000000..57cbfcb0619
--- /dev/null
+++ b/drivers/staging/csr/csr_macro.h
@@ -0,0 +1,114 @@
+#ifndef CSR_MACRO_H__
+#define CSR_MACRO_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define FALSE	(0)
+#define TRUE	(1)
+
+/*------------------------------------------------------------------*/
+/* Bits - intended to operate on u32 values */
+/*------------------------------------------------------------------*/
+#define CSR_MASK_IS_SET(val, mask) (((val) & (mask)) == (mask))
+#define CSR_MASK_IS_UNSET(val, mask) ((((val) & (mask)) ^ mask) == (mask))
+#define CSR_MASK_SET(val, mask)    ((val) |= (mask))
+#define CSR_MASK_UNSET(val, mask)  ((val) = ((val) ^ (mask)) & (val)) /* Unsets the bits in val that are set in mask */
+#define CSR_BIT_IS_SET(val, bit)   ((u8) ((((val) & (1UL << (bit))) != 0)))
+#define CSR_BIT_SET(val, bit)      ((val) |= (1UL << (bit)))
+#define CSR_BIT_UNSET(val, bit)    ((val) &= ~(1UL << (bit)))
+#define CSR_BIT_TOGGLE(val, bit)   ((val) ^= (1UL << (bit)))
+
+/*------------------------------------------------------------------*/
+/* Endian conversion */
+/*------------------------------------------------------------------*/
+#define CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr)        (((u16) ((u8 *) (ptr))[0]) | ((u16) ((u8 *) (ptr))[1]) << 8)
+#define CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr)        (((u32) ((u8 *) (ptr))[0]) | ((u32) ((u8 *) (ptr))[1]) << 8 | \
+                                                       ((u32) ((u8 *) (ptr))[2]) << 16 | ((u32) ((u8 *) (ptr))[3]) << 24)
+#define CSR_COPY_UINT16_TO_LITTLE_ENDIAN(uint, ptr)    ((u8 *) (ptr))[0] = ((u8) ((uint) & 0x00FF)); \
+    ((u8 *) (ptr))[1] = ((u8) ((uint) >> 8))
+#define CSR_COPY_UINT32_TO_LITTLE_ENDIAN(uint, ptr)    ((u8 *) (ptr))[0] = ((u8) ((uint) & 0x000000FF)); \
+    ((u8 *) (ptr))[1] = ((u8) (((uint) >> 8) & 0x000000FF)); \
+    ((u8 *) (ptr))[2] = ((u8) (((uint) >> 16) & 0x000000FF)); \
+    ((u8 *) (ptr))[3] = ((u8) (((uint) >> 24) & 0x000000FF))
+#define CSR_GET_UINT16_FROM_BIG_ENDIAN(ptr) (((u16) ((u8 *) (ptr))[1]) | ((u16) ((u8 *) (ptr))[0]) << 8)
+#define CSR_GET_UINT24_FROM_BIG_ENDIAN(ptr) (((u32) ((u8 *) (ptr))[2]) | \
+                                             ((u32) ((u8 *) (ptr))[1]) << 8 | ((u32) ((u8 *) (ptr))[0]) << 16)
+#define CSR_GET_UINT32_FROM_BIG_ENDIAN(ptr) (((u32) ((u8 *) (ptr))[3]) | ((u32) ((u8 *) (ptr))[2]) << 8 | \
+                                             ((u32) ((u8 *) (ptr))[1]) << 16 | ((u32) ((u8 *) (ptr))[0]) << 24)
+#define CSR_COPY_UINT16_TO_BIG_ENDIAN(uint, ptr)    ((u8 *) (ptr))[1] = ((u8) ((uint) & 0x00FF)); \
+    ((u8 *) (ptr))[0] = ((u8) ((uint) >> 8))
+#define CSR_COPY_UINT24_TO_BIG_ENDIAN(uint, ptr)    ((u8 *) (ptr))[2] = ((u8) ((uint) & 0x000000FF)); \
+    ((u8 *) (ptr))[1] = ((u8) (((uint) >> 8) & 0x000000FF)); \
+    ((u8 *) (ptr))[0] = ((u8) (((uint) >> 16) & 0x000000FF))
+#define CSR_COPY_UINT32_TO_BIG_ENDIAN(uint, ptr)    ((u8 *) (ptr))[3] = ((u8) ((uint) & 0x000000FF)); \
+    ((u8 *) (ptr))[2] = ((u8) (((uint) >> 8) & 0x000000FF)); \
+    ((u8 *) (ptr))[1] = ((u8) (((uint) >> 16) & 0x000000FF)); \
+    ((u8 *) (ptr))[0] = ((u8) (((uint) >> 24) & 0x000000FF))
+
+/*------------------------------------------------------------------*/
+/* XAP conversion macros */
+/*------------------------------------------------------------------*/
+
+#define CSR_LSB16(a) ((u8) ((a) & 0x00ff))
+#define CSR_MSB16(b) ((u8) ((b) >> 8))
+
+#define CSR_CONVERT_8_FROM_XAP(output, input) \
+    (output) = ((u8) (input));(input) += 2
+
+#define CSR_CONVERT_16_FROM_XAP(output, input) \
+    (output) = (u16) ((((u16) (input)[1]) << 8) | \
+                            ((u16) (input)[0]));(input) += 2
+
+#define CSR_CONVERT_32_FROM_XAP(output, input) \
+    (output) = (((u32) (input)[1]) << 24) | \
+               (((u32) (input)[0]) << 16) | \
+               (((u32) (input)[3]) << 8) | \
+               ((u32) (input)[2]);input += 4
+
+#define CSR_ADD_UINT8_TO_XAP(output, input) \
+    (output)[0] = (input);  \
+    (output)[1] = 0;(output) += 2
+
+#define CSR_ADD_UINT16_TO_XAP(output, input) \
+    (output)[0] = ((u8) ((input) & 0x00FF));  \
+    (output)[1] = ((u8) ((input) >> 8));(output) += 2
+
+#define CSR_ADD_UINT32_TO_XAP(output, input) \
+    (output)[0] = ((u8) (((input) >> 16) & 0x00FF)); \
+    (output)[1] = ((u8) ((input) >> 24)); \
+    (output)[2] = ((u8) ((input) & 0x00FF)); \
+    (output)[3] = ((u8) (((input) >> 8) & 0x00FF));(output) += 4
+
+/*------------------------------------------------------------------*/
+/* Misc */
+/*------------------------------------------------------------------*/
+#define CSRMAX(a, b)    (((a) > (b)) ? (a) : (b))
+#define CSRMIN(a, b)    (((a) < (b)) ? (a) : (b))
+
+/* Use this macro on unused local variables that cannot be removed (such as
+   unused function parameters). This will quell warnings from certain compilers
+   and static code analysis tools like Lint and Valgrind. */
+#define CSR_UNUSED(x) ((void) (x))
+
+#define CSR_TOUPPER(character)  (((character) >= 'a') && ((character) <= 'z') ? ((character) - 0x20) : (character))
+#define CSR_TOLOWER(character)  (((character) >= 'A') && ((character) <= 'Z') ? ((character) + 0x20) : (character))
+#define CSR_ARRAY_SIZE(x)       (sizeof(x) / sizeof(*(x)))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_msg_transport.h b/drivers/staging/csr/csr_msg_transport.h
new file mode 100644
index 00000000000..b0095b02381
--- /dev/null
+++ b/drivers/staging/csr/csr_msg_transport.h
@@ -0,0 +1,25 @@
+#ifndef CSR_MSG_TRANSPORT_H__
+#define CSR_MSG_TRANSPORT_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CsrMsgTransport
+#define CsrMsgTransport CsrSchedMessagePut
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_MSG_TRANSPORT */
diff --git a/drivers/staging/csr/csr_msgconv.c b/drivers/staging/csr/csr_msgconv.c
new file mode 100644
index 00000000000..0081a255e91
--- /dev/null
+++ b/drivers/staging/csr/csr_msgconv.c
@@ -0,0 +1,292 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include "csr_panic.h"
+#include "csr_sched.h"
+#include "csr_msgconv.h"
+#include "csr_macro.h"
+
+static CsrMsgConvEntry *converter;
+
+CsrMsgConvPrimEntry *CsrMsgConvFind(u16 primType)
+{
+    CsrMsgConvPrimEntry *ptr = NULL;
+
+    if (converter)
+    {
+        ptr = converter->profile_converters;
+        while (ptr)
+        {
+            if (ptr->primType == primType)
+            {
+                break;
+            }
+            else
+            {
+                ptr = ptr->next;
+            }
+        }
+    }
+
+    return ptr;
+}
+
+static const CsrMsgConvMsgEntry *find_msg_converter(CsrMsgConvPrimEntry *ptr, u16 msgType)
+{
+    const CsrMsgConvMsgEntry *cv = ptr->conv;
+    if (ptr->lookupFunc)
+    {
+        return (const CsrMsgConvMsgEntry *) ptr->lookupFunc((CsrMsgConvMsgEntry *) cv, msgType);
+    }
+
+    while (cv)
+    {
+        if (cv->serFunc == NULL)
+        {
+            /* We've reached the end of the chain */
+            cv = NULL;
+            break;
+        }
+
+        if (cv->msgType == msgType)
+        {
+            break;
+        }
+        else
+        {
+            cv++;
+        }
+    }
+
+    return cv;
+}
+
+static void *deserialize_data(u16 primType,
+    size_t length,
+    u8 *data)
+{
+    CsrMsgConvPrimEntry *ptr;
+    u8 *ret;
+
+    ptr = CsrMsgConvFind(primType);
+
+    if (ptr)
+    {
+        const CsrMsgConvMsgEntry *cv;
+        u16 msgId = 0;
+        size_t offset = 0;
+        CsrUint16Des(&msgId, data, &offset);
+
+        cv = find_msg_converter(ptr, msgId);
+        if (cv)
+        {
+            ret = cv->deserFunc(data, length);
+        }
+        else
+        {
+            ret = NULL;
+        }
+    }
+    else
+    {
+        ret = NULL;
+    }
+
+    return ret;
+}
+
+static size_t sizeof_message(u16 primType, void *msg)
+{
+    CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
+    size_t ret;
+
+    if (ptr)
+    {
+        const CsrMsgConvMsgEntry *cv;
+        u16 msgId = *(u16 *) msg;
+
+        cv = find_msg_converter(ptr, msgId);
+        if (cv)
+        {
+            ret = cv->sizeofFunc(msg);
+        }
+        else
+        {
+            ret = 0;
+        }
+    }
+    else
+    {
+        ret = 0;
+    }
+
+    return ret;
+}
+
+static u8 free_message(u16 primType, u8 *data)
+{
+    CsrMsgConvPrimEntry *ptr;
+    u8 ret;
+
+    ptr = CsrMsgConvFind(primType);
+
+    if (ptr)
+    {
+        const CsrMsgConvMsgEntry *cv;
+        u16 msgId = *(u16 *) data;
+
+        cv = find_msg_converter(ptr, msgId);
+        if (cv)
+        {
+            cv->freeFunc(data);
+            ret = TRUE;
+        }
+        else
+        {
+            ret = FALSE;
+        }
+    }
+    else
+    {
+        ret = FALSE;
+    }
+
+    return ret;
+}
+
+static u8 *serialize_message(u16 primType,
+    void *msg,
+    size_t *length,
+    u8 *buffer)
+{
+    CsrMsgConvPrimEntry *ptr;
+    u8 *ret;
+
+    ptr = CsrMsgConvFind(primType);
+
+    *length = 0;
+
+    if (ptr)
+    {
+        const CsrMsgConvMsgEntry *cv;
+
+        cv = find_msg_converter(ptr, *(u16 *) msg);
+        if (cv)
+        {
+            ret = cv->serFunc(buffer, length, msg);
+        }
+        else
+        {
+            ret = NULL;
+        }
+    }
+    else
+    {
+        ret = NULL;
+    }
+
+    return ret;
+}
+
+size_t CsrMsgConvSizeof(u16 primType, void *msg)
+{
+    return sizeof_message(primType, msg);
+}
+
+u8 *CsrMsgConvSerialize(u8 *buffer, size_t maxBufferOffset, size_t *offset, u16 primType, void *msg)
+{
+    if (converter)
+    {
+        size_t serializedLength;
+        u8 *bufSerialized;
+        u8 *bufOffset = &buffer[*offset];
+        bufSerialized = converter->serialize_message(primType, msg, &serializedLength, bufOffset);
+        *offset += serializedLength;
+        return bufSerialized;
+    }
+    else
+    {
+        return NULL;
+    }
+}
+
+/* Insert profile converter at head of converter list. */
+void CsrMsgConvInsert(u16 primType, const CsrMsgConvMsgEntry *ce)
+{
+    CsrMsgConvPrimEntry *pc;
+    pc = CsrMsgConvFind(primType);
+
+    if (pc)
+    {
+        /* Already registered. Do nothing */
+    }
+    else
+    {
+        pc = kmalloc(sizeof(*pc), GFP_KERNEL);
+        pc->primType = primType;
+        pc->conv = ce;
+        pc->lookupFunc = NULL;
+        pc->next = converter->profile_converters;
+        converter->profile_converters = pc;
+    }
+}
+EXPORT_SYMBOL_GPL(CsrMsgConvInsert);
+
+CsrMsgConvMsgEntry *CsrMsgConvFindEntry(u16 primType, u16 msgType)
+{
+    CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
+    if (ptr)
+    {
+        return (CsrMsgConvMsgEntry *) find_msg_converter(ptr, msgType);
+    }
+    return NULL;
+}
+EXPORT_SYMBOL_GPL(CsrMsgConvFindEntry);
+
+CsrMsgConvMsgEntry *CsrMsgConvFindEntryByMsg(u16 primType, const void *msg)
+{
+    CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
+    if (ptr && msg)
+    {
+        u16 msgType = *((u16 *) msg);
+        return (CsrMsgConvMsgEntry *) find_msg_converter(ptr, msgType);
+    }
+    return NULL;
+}
+
+void CsrMsgConvCustomLookupRegister(u16 primType, CsrMsgCustomLookupFunc *lookupFunc)
+{
+    CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
+    if (ptr)
+    {
+        ptr->lookupFunc = lookupFunc;
+    }
+}
+EXPORT_SYMBOL_GPL(CsrMsgConvCustomLookupRegister);
+
+CsrMsgConvEntry *CsrMsgConvInit(void)
+{
+    if (!converter)
+    {
+        converter = kmalloc(sizeof(CsrMsgConvEntry), GFP_KERNEL);
+
+        converter->profile_converters = NULL;
+        converter->free_message = free_message;
+        converter->sizeof_message = sizeof_message;
+        converter->serialize_message = serialize_message;
+        converter->deserialize_data = deserialize_data;
+    }
+
+    return converter;
+}
+EXPORT_SYMBOL_GPL(CsrMsgConvInit);
diff --git a/drivers/staging/csr/csr_msgconv.h b/drivers/staging/csr/csr_msgconv.h
new file mode 100644
index 00000000000..09489f38e52
--- /dev/null
+++ b/drivers/staging/csr/csr_msgconv.h
@@ -0,0 +1,87 @@
+#ifndef CSR_MSGCONV_H__
+#define CSR_MSGCONV_H__
+
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/types.h>
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef size_t (CsrMsgSizeofFunc)(void *msg);
+typedef u8 *(CsrMsgSerializeFunc)(u8 *buffer, size_t *length, void *msg);
+typedef void (CsrMsgFreeFunc)(void *msg);
+typedef void *(CsrMsgDeserializeFunc)(u8 *buffer, size_t length);
+
+/* Converter entry for one message type */
+typedef struct CsrMsgConvMsgEntry
+{
+    u16              msgType;
+    CsrMsgSizeofFunc      *sizeofFunc;
+    CsrMsgSerializeFunc   *serFunc;
+    CsrMsgDeserializeFunc *deserFunc;
+    CsrMsgFreeFunc        *freeFunc;
+} CsrMsgConvMsgEntry;
+
+/* Optional lookup function */
+typedef CsrMsgConvMsgEntry *(CsrMsgCustomLookupFunc)(CsrMsgConvMsgEntry *ce, u16 msgType);
+
+/* All converter entries for one specific primitive */
+typedef struct CsrMsgConvPrimEntry
+{
+    u16                   primType;
+    const CsrMsgConvMsgEntry   *conv;
+    CsrMsgCustomLookupFunc     *lookupFunc;
+    struct CsrMsgConvPrimEntry *next;
+} CsrMsgConvPrimEntry;
+
+typedef struct
+{
+    CsrMsgConvPrimEntry *profile_converters;
+    void *(*deserialize_data)(u16 primType, size_t length, u8 * data);
+    u8 (*free_message)(u16 primType, u8 *data);
+    size_t (*sizeof_message)(u16 primType, void *msg);
+    u8 *(*serialize_message)(u16 primType, void *msg,
+                                   size_t * length,
+                                   u8 * buffer);
+} CsrMsgConvEntry;
+
+size_t CsrMsgConvSizeof(u16 primType, void *msg);
+u8 *CsrMsgConvSerialize(u8 *buffer, size_t maxBufferOffset, size_t *offset, u16 primType, void *msg);
+void CsrMsgConvCustomLookupRegister(u16 primType, CsrMsgCustomLookupFunc *lookupFunc);
+void CsrMsgConvInsert(u16 primType, const CsrMsgConvMsgEntry *ce);
+CsrMsgConvPrimEntry *CsrMsgConvFind(u16 primType);
+CsrMsgConvMsgEntry *CsrMsgConvFindEntry(u16 primType, u16 msgType);
+CsrMsgConvMsgEntry *CsrMsgConvFindEntryByMsg(u16 primType, const void *msg);
+CsrMsgConvEntry *CsrMsgConvInit(void);
+
+/* Prototypes for primitive type serializers */
+void CsrUint8Ser(u8 *buffer, size_t *offset, u8 value);
+void CsrUint16Ser(u8 *buffer, size_t *offset, u16 value);
+void CsrUint32Ser(u8 *buffer, size_t *offset, u32 value);
+void CsrMemCpySer(u8 *buffer, size_t *offset, const void *value, size_t length);
+void CsrCharStringSer(u8 *buffer, size_t *offset, const char *value);
+
+void CsrUint8Des(u8 *value, u8 *buffer, size_t *offset);
+void CsrUint16Des(u16 *value, u8 *buffer, size_t *offset);
+void CsrUint32Des(u32 *value, u8 *buffer, size_t *offset);
+void CsrMemCpyDes(void *value, u8 *buffer, size_t *offset, size_t length);
+void CsrCharStringDes(char **value, u8 *buffer, size_t *offset);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_panic.c b/drivers/staging/csr/csr_panic.c
new file mode 100644
index 00000000000..353a829bb74
--- /dev/null
+++ b/drivers/staging/csr/csr_panic.c
@@ -0,0 +1,21 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include "csr_panic.h"
+
+void CsrPanic(u8 tech, u16 reason, const char *p)
+{
+    BUG_ON(1);
+}
+EXPORT_SYMBOL_GPL(CsrPanic);
diff --git a/drivers/staging/csr/csr_panic.h b/drivers/staging/csr/csr_panic.h
new file mode 100644
index 00000000000..37989fc15bb
--- /dev/null
+++ b/drivers/staging/csr/csr_panic.h
@@ -0,0 +1,53 @@
+#ifndef CSR_PANIC_H__
+#define CSR_PANIC_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Synergy techonology ID definitions */
+#define CSR_TECH_FW     0
+#define CSR_TECH_BT     1
+#define CSR_TECH_WIFI   2
+#define CSR_TECH_GPS    3
+#define CSR_TECH_NFC    4
+
+/* Panic type ID definitions for technology type CSR_TECH_FW */
+#define CSR_PANIC_FW_UNEXPECTED_VALUE        0
+#define CSR_PANIC_FW_HEAP_EXHAUSTION         1
+#define CSR_PANIC_FW_INVALID_PFREE_POINTER   2
+#define CSR_PANIC_FW_EXCEPTION               3
+#define CSR_PANIC_FW_ASSERTION_FAIL          4
+#define CSR_PANIC_FW_NULL_TASK_HANDLER       5
+#define CSR_PANIC_FW_UNKNOWN_TASK            6
+#define CSR_PANIC_FW_QUEUE_ACCESS_VIOLATION  7
+#define CSR_PANIC_FW_TOO_MANY_MESSAGES       8
+#define CSR_PANIC_FW_TOO_MANY_TIMED_EVENTS   9
+#define CSR_PANIC_FW_ABCSP_SYNC_LOST        10
+#define CSR_PANIC_FW_OVERSIZE_ABCSP_PRIM    11
+#define CSR_PANIC_FW_H4_CORRUPTION          12
+#define CSR_PANIC_FW_H4_SYNC_LOST           13
+#define CSR_PANIC_FW_H4_RX_OVERRUN          14
+#define CSR_PANIC_FW_H4_TX_OVERRUN          15
+#define CSR_PANIC_FW_TM_BC_RESTART_FAIL     16
+#define CSR_PANIC_FW_TM_BC_START_FAIL       17
+#define CSR_PANIC_FW_TM_BC_BAD_STATE        18
+#define CSR_PANIC_FW_TM_BC_TRANSPORT_LOST   19
+
+/* Panic interface used by technologies */
+/* DEPRECATED - replaced by csr_log_text.h */
+void CsrPanic(u8 tech, u16 reason, const char *p);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_PANIC_H__ */
diff --git a/drivers/staging/csr/csr_prim_defs.h b/drivers/staging/csr/csr_prim_defs.h
new file mode 100644
index 00000000000..6a7f73dbb70
--- /dev/null
+++ b/drivers/staging/csr/csr_prim_defs.h
@@ -0,0 +1,62 @@
+#ifndef CSR_PRIM_DEFS_H__
+#define CSR_PRIM_DEFS_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/************************************************************************************
+ * Segmentation of primitives in upstream and downstream segment
+ ************************************************************************************/
+typedef u16 CsrPrim;
+#define CSR_PRIM_UPSTREAM                   ((CsrPrim) (0x8000))
+
+/************************************************************************************
+ * Primitive definitions for Synergy framework
+ ************************************************************************************/
+#define CSR_SYNERGY_EVENT_CLASS_BASE        ((u16) (0x0600))
+
+#define CSR_HCI_PRIM                        ((u16) (0x0000 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_BCCMD_PRIM                      ((u16) (0x0001 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_HQ_PRIM                         ((u16) (0x0002 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_VM_PRIM                         ((u16) (0x0003 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_TM_BLUECORE_PRIM                ((u16) (0x0004 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_FP_PRIM                         ((u16) (0x0005 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_IP_SOCKET_PRIM                  ((u16) (0x0006 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_IP_ETHER_PRIM                   ((u16) (0x0007 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_IP_IFCONFIG_PRIM                ((u16) (0x0008 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_IP_INTERNAL_PRIM                ((u16) (0x0009 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_FSAL_PRIM                       ((u16) (0x000A | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_DATA_STORE_PRIM                 ((u16) (0x000B | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_AM_PRIM                         ((u16) (0x000C | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_TLS_PRIM                        ((u16) (0x000D | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_DHCP_SERVER_PRIM                ((u16) (0x000E | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_TFTP_PRIM                       ((u16) (0x000F | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_DSPM_PRIM                       ((u16) (0x0010 | CSR_SYNERGY_EVENT_CLASS_BASE))
+#define CSR_TLS_INTERNAL_PRIM               ((u16) (0x0011 | CSR_SYNERGY_EVENT_CLASS_BASE))
+
+#define NUMBER_OF_CSR_FW_EVENTS             (CSR_DSPM_PRIM - CSR_SYNERGY_EVENT_CLASS_BASE + 1)
+
+#define CSR_SYNERGY_EVENT_CLASS_MISC_BASE   ((u16) (0x06A0))
+
+#define CSR_UI_PRIM                         ((u16) (0x0000 | CSR_SYNERGY_EVENT_CLASS_MISC_BASE))
+#define CSR_APP_PRIM                        ((u16) (0x0001 | CSR_SYNERGY_EVENT_CLASS_MISC_BASE))
+#define CSR_SDIO_PROBE_PRIM                 ((u16) (0x0002 | CSR_SYNERGY_EVENT_CLASS_MISC_BASE))
+
+#define NUMBER_OF_CSR_FW_MISC_EVENTS        (CSR_SDIO_PROBE_PRIM - CSR_SYNERGY_EVENT_CLASS_MISC_BASE + 1)
+
+#define CSR_ENV_PRIM                        ((u16) (0x00FF | CSR_SYNERGY_EVENT_CLASS_MISC_BASE))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_PRIM_DEFS_H__ */
diff --git a/drivers/staging/csr/csr_result.h b/drivers/staging/csr/csr_result.h
new file mode 100644
index 00000000000..c7c36d6b59e
--- /dev/null
+++ b/drivers/staging/csr/csr_result.h
@@ -0,0 +1,25 @@
+#ifndef CSR_RESULT_H__
+#define CSR_RESULT_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef u16 CsrResult;
+#define CSR_RESULT_SUCCESS  ((CsrResult) 0x0000)
+#define CSR_RESULT_FAILURE  ((CsrResult) 0xFFFF)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_sched.h b/drivers/staging/csr/csr_sched.h
new file mode 100644
index 00000000000..cc1b8bf6607
--- /dev/null
+++ b/drivers/staging/csr/csr_sched.h
@@ -0,0 +1,292 @@
+#ifndef CSR_SCHED_H__
+#define CSR_SCHED_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+#include <linux/types.h>
+#include "csr_time.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* An identifier issued by the scheduler. */
+typedef u32 CsrSchedIdentifier;
+
+/* A task identifier */
+typedef u16 CsrSchedTaskId;
+
+/* A queue identifier */
+typedef u16 CsrSchedQid;
+#define CSR_SCHED_QID_INVALID     ((CsrSchedQid) 0xFFFF)
+
+/* A message identifier */
+typedef CsrSchedIdentifier CsrSchedMsgId;
+
+/* A timer event identifier */
+typedef CsrSchedIdentifier CsrSchedTid;
+#define CSR_SCHED_TID_INVALID     ((CsrSchedTid) 0)
+
+/* Scheduler entry functions share this structure */
+typedef void (*schedEntryFunction_t)(void **inst);
+
+/* Time constants. */
+#define CSR_SCHED_TIME_MAX                ((CsrTime) 0xFFFFFFFF)
+#define CSR_SCHED_MILLISECOND             ((CsrTime) (1000))
+#define CSR_SCHED_SECOND                  ((CsrTime) (1000 * CSR_SCHED_MILLISECOND))
+#define CSR_SCHED_MINUTE                  ((CsrTime) (60 * CSR_SCHED_SECOND))
+
+/* Queue and primitive that identifies the environment */
+#define CSR_SCHED_TASK_ID        0xFFFF
+#define CSR_SCHED_PRIM                   (CSR_SCHED_TASK_ID)
+#define CSR_SCHED_EXCLUDED_MODULE_QUEUE      0xFFFF
+
+/*
+ * Background interrupt definitions
+ */
+typedef u16 CsrSchedBgint;
+#define CSR_SCHED_BGINT_INVALID ((CsrSchedBgint) 0xFFFF)
+
+typedef void (*CsrSchedBgintHandler)(void *);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedBgintReg
+ *
+ *  DESCRIPTION
+ *      Register a background interrupt handler function with the scheduler.
+ *        When CsrSchedBgint() is called from the foreground (e.g. an interrupt
+ *        routine) the registered function is called.
+ *
+ *        If "cb" is null then the interrupt is effectively disabled. If a
+ *        no bgints are available, CSR_SCHED_BGINT_INVALID is returned, otherwise
+ *        a CsrSchedBgint value is returned to be used in subsequent calls to
+ *        CsrSchedBgint().  id is a possibly NULL identifier used for logging
+ *        purposes only.
+ *
+ *  RETURNS
+ *      CsrSchedBgint -- CSR_SCHED_BGINT_INVALID denotes failure to obtain a CsrSchedBgintSet.
+ *
+ *----------------------------------------------------------------------------*/
+CsrSchedBgint CsrSchedBgintReg(CsrSchedBgintHandler cb,
+    void *context,
+    const char *id);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedBgintUnreg
+ *
+ *  DESCRIPTION
+ *      Unregister a background interrupt handler function.
+ *
+ *      ``irq'' is a background interrupt handle previously obtained
+ *      from a call to CsrSchedBgintReg().
+ *
+ *  RETURNS
+ *      void.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSchedBgintUnreg(CsrSchedBgint bgint);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedBgintSet
+ *
+ *  DESCRIPTION
+ *      Set background interrupt.
+ *
+ *  RETURNS
+ *      void.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSchedBgintSet(CsrSchedBgint bgint);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedMessagePut
+ *
+ *  DESCRIPTION
+ *      Sends a message consisting of the integer "mi" and the void * pointer
+ *      "mv" to the message queue "q".
+ *
+ *      "mi" and "mv" are neither inspected nor changed by the scheduler - the
+ *      task that owns "q" is expected to make sense of the values. "mv" may
+ *      be null.
+ *
+ *  NOTE
+ *      If "mv" is not null then it will typically be a chunk of kmalloc()ed
+ *      memory, though there is no need for it to be so. Tasks should normally
+ *      obey the convention that when a message built with kmalloc()ed memory
+ *      is given to CsrSchedMessagePut() then ownership of the memory is ceded to the
+ *      scheduler - and eventually to the recipient task. I.e., the receiver of
+ *      the message will be expected to kfree() the message storage.
+ *
+ *  RETURNS
+ *      void.
+ *
+ *----------------------------------------------------------------------------*/
+#if defined(CSR_LOG_ENABLE) && defined(CSR_LOG_INCLUDE_FILE_NAME_AND_LINE_NUMBER)
+void CsrSchedMessagePutStringLog(CsrSchedQid q,
+    u16 mi,
+    void *mv,
+    u32 line,
+    const char *file);
+#define CsrSchedMessagePut(q, mi, mv) CsrSchedMessagePutStringLog((q), (mi), (mv), __LINE__, __FILE__)
+#else
+void CsrSchedMessagePut(CsrSchedQid q,
+    u16 mi,
+    void *mv);
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedMessageBroadcast
+ *
+ *  DESCRIPTION
+ *      Sends a message to all tasks.
+ *
+ *      The user must supply a "factory function" that is called once
+ *      for every task that exists. The "factory function", msg_build_func,
+ *      must allocate and initialise the message and set the msg_build_ptr
+ *      to point to the message when done.
+ *
+ *  NOTE
+ *      N/A
+ *
+ *  RETURNS
+ *      void
+ *
+ *----------------------------------------------------------------------------*/
+#if defined(CSR_LOG_ENABLE) && defined(CSR_LOG_INCLUDE_FILE_NAME_AND_LINE_NUMBER)
+void CsrSchedMessageBroadcastStringLog(u16 mi,
+    void *(*msg_build_func)(void *),
+    void *msg_build_ptr,
+    u32 line,
+    const char *file);
+#define CsrSchedMessageBroadcast(mi, fn, ptr) CsrSchedMessageBroadcastStringLog((mi), (fn), (ptr), __LINE__, __FILE__)
+#else
+void CsrSchedMessageBroadcast(u16 mi,
+    void *(*msg_build_func)(void *),
+    void *msg_build_ptr);
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedMessageGet
+ *
+ *  DESCRIPTION
+ *      Obtains a message from the message queue belonging to the calling task.
+ *      The message consists of one or both of a u16 and a void *.
+ *
+ *  RETURNS
+ *      u8 - TRUE if a message has been obtained from the queue, else FALSE.
+ *      If a message is taken from the queue, then "*pmi" and "*pmv" are set to
+ *      the "mi" and "mv" passed to CsrSchedMessagePut() respectively.
+ *
+ *      "pmi" and "pmv" can be null, in which case the corresponding value from
+ *      them message is discarded.
+ *
+ *----------------------------------------------------------------------------*/
+u8 CsrSchedMessageGet(u16 *pmi, void **pmv);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedTimerSet
+ *
+ *  DESCRIPTION
+ *      Causes the void function "fn" to be called with the arguments
+ *      "fniarg" and "fnvarg" after "delay" has elapsed.
+ *
+ *      "delay" must be less than half the range of a CsrTime.
+ *
+ *      CsrSchedTimerSet() does nothing with "fniarg" and "fnvarg" except
+ *      deliver them via a call to "fn()".   (Unless CsrSchedTimerCancel()
+ *      is used to prevent delivery.)
+ *
+ *  NOTE
+ *      The function will be called at or after "delay"; the actual delay will
+ *      depend on the timing behaviour of the scheduler's tasks.
+ *
+ *  RETURNS
+ *      CsrSchedTid - A timed event identifier, can be used in CsrSchedTimerCancel().
+ *
+ *----------------------------------------------------------------------------*/
+#if defined(CSR_LOG_ENABLE) && defined(CSR_LOG_INCLUDE_FILE_NAME_AND_LINE_NUMBER)
+CsrSchedTid CsrSchedTimerSetStringLog(CsrTime delay,
+    void (*fn)(u16 mi, void *mv),
+    u16 fniarg,
+    void *fnvarg,
+    u32 line,
+    const char *file);
+#define CsrSchedTimerSet(d, fn, fni, fnv) CsrSchedTimerSetStringLog((d), (fn), (fni), (fnv), __LINE__, __FILE__)
+#else
+CsrSchedTid CsrSchedTimerSet(CsrTime delay,
+    void (*fn)(u16 mi, void *mv),
+    u16 fniarg,
+    void *fnvarg);
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedTimerCancel
+ *
+ *  DESCRIPTION
+ *      Attempts to prevent the timed event with identifier "eventid" from
+ *      occurring.
+ *
+ *  RETURNS
+ *      u8 - TRUE if cancelled, FALSE if the event has already occurred.
+ *
+ *----------------------------------------------------------------------------*/
+#if defined(CSR_LOG_ENABLE) && defined(CSR_LOG_INCLUDE_FILE_NAME_AND_LINE_NUMBER)
+u8 CsrSchedTimerCancelStringLog(CsrSchedTid eventid,
+    u16 *pmi,
+    void **pmv,
+    u32 line,
+    const char *file);
+#define CsrSchedTimerCancel(e, pmi, pmv) CsrSchedTimerCancelStringLog((e), (pmi), (pmv), __LINE__, __FILE__)
+#else
+u8 CsrSchedTimerCancel(CsrSchedTid eventid,
+    u16 *pmi,
+    void **pmv);
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedTaskQueueGet
+ *
+ *  DESCRIPTION
+ *      Return the queue identifier for the currently running queue
+ *
+ *  RETURNS
+ *      CsrSchedQid - The current task queue identifier, or 0xFFFF if not available.
+ *
+ *----------------------------------------------------------------------------*/
+CsrSchedQid CsrSchedTaskQueueGet(void);
+
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSchedTaskQueueGet
+ *
+ *  DESCRIPTION
+ *      Return the queue identifier for the currently running queue
+ *
+ *  RETURNS
+ *      char - The current task queue identifier, or 0xFFFF if not available.
+ *
+ *----------------------------------------------------------------------------*/
+char* CsrSchedTaskNameGet(CsrSchedQid );
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_sdio.h b/drivers/staging/csr/csr_sdio.h
new file mode 100644
index 00000000000..f0cda84f6a0
--- /dev/null
+++ b/drivers/staging/csr/csr_sdio.h
@@ -0,0 +1,731 @@
+#ifndef CSR_SDIO_H__
+#define CSR_SDIO_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_result.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Result Codes */
+#define CSR_SDIO_RESULT_INVALID_VALUE   ((CsrResult) 1) /* Invalid argument value */
+#define CSR_SDIO_RESULT_NO_DEVICE       ((CsrResult) 2) /* The specified device is no longer present */
+#define CSR_SDIO_RESULT_CRC_ERROR       ((CsrResult) 3) /* The transmitted/received data or command response contained a CRC error */
+#define CSR_SDIO_RESULT_TIMEOUT         ((CsrResult) 4) /* No command response or data received from device, or function enable/disable did not succeed within timeout period */
+#define CSR_SDIO_RESULT_NOT_RESET       ((CsrResult) 5) /* The device was not reset */
+
+/* Features (for use in features member of CsrSdioFunction) */
+#define CSR_SDIO_FEATURE_BYTE_MODE                   0x00000001 /* Transfer sizes do not have to be a multiple of block size */
+#define CSR_SDIO_FEATURE_DMA_CAPABLE_MEM_REQUIRED    0x00000002 /* Bulk operations require DMA friendly memory */
+
+/* CsrSdioFunctionId wildcards (for use in CsrSdioFunctionId members) */
+#define CSR_SDIO_ANY_MANF_ID        0xFFFF
+#define CSR_SDIO_ANY_CARD_ID        0xFFFF
+#define CSR_SDIO_ANY_SDIO_FUNCTION  0xFF
+#define CSR_SDIO_ANY_SDIO_INTERFACE 0xFF
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunctionId
+ *
+ *  DESCRIPTION
+ *      This structure describes one or more functions of a device, based on
+ *      four qualitative measures. The CsrSdioFunctionId wildcard defines can be
+ *      used for making the CsrSdioFunctionId match more than one function.
+ *
+ *  MEMBERS
+ *      manfId - Vendor ID (or CSR_SDIO_ANY_MANF_ID).
+ *      cardId - Device ID (or CSR_SDIO_ANY_CARD_ID).
+ *      sdioFunction - SDIO Function number (or CSR_SDIO_ANY_SDIO_FUNCTION).
+ *      sdioInterface - SDIO Standard Interface Code (or CSR_SDIO_ANY_SDIO_INTERFACE)
+ *
+ *----------------------------------------------------------------------------*/
+typedef struct
+{
+    u16 manfId;       /* Vendor ID to match or CSR_SDIO_ANY_MANF_ID */
+    u16 cardId;       /* Device ID to match or CSR_SDIO_ANY_CARD_ID */
+    u8  sdioFunction; /* SDIO Function number to match or CSR_SDIO_ANY_SDIO_FUNCTION */
+    u8  sdioInterface; /* SDIO Standard Interface Code to match or CSR_SDIO_ANY_SDIO_INTERFACE */
+} CsrSdioFunctionId;
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunction
+ *
+ *  DESCRIPTION
+ *      This structure represents a single function on a device.
+ *
+ *  MEMBERS
+ *      sdioId - A CsrSdioFunctionId describing this particular function. The
+ *               subfield shall not contain any CsrSdioFunctionId wildcards. The
+ *               subfields shall describe the specific single function
+ *               represented by this structure.
+ *      blockSize - Actual configured block size, or 0 if unconfigured.
+ *      features - Bit mask with any of CSR_SDIO_FEATURE_* set.
+ *      device - Handle of device containing the function. If two functions have
+ *               the same device handle, they reside on the same device.
+ *      driverData - For use by the Function Driver. The SDIO Driver shall not
+ *                   attempt to dereference the pointer.
+ *      priv - For use by the SDIO Driver. The Function Driver shall not attempt
+ *             to dereference the pointer.
+ *
+ *
+ *----------------------------------------------------------------------------*/
+typedef struct
+{
+    CsrSdioFunctionId sdioId;
+    u16         blockSize; /* Actual configured block size, or 0 if unconfigured */
+    u32         features; /* Bit mask with any of CSR_SDIO_FEATURE_* set */
+    void             *device; /* Handle of device containing the function */
+    void             *driverData; /* For use by the Function Driver */
+    void             *priv; /* For use by the SDIO Driver */
+} CsrSdioFunction;
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioInsertedCallback, CsrSdioRemovedCallback
+ *
+ *  DESCRIPTION
+ *      CsrSdioInsertedCallback is called when a function becomes available to
+ *      a registered Function Driver that supports the function.
+ *      CsrSdioRemovedCallback is called when a function is no longer available
+ *      to a Function Driver, either because the device has been removed, or the
+ *      Function Driver has been unregistered.
+ *
+ *      NOTE: These functions are implemented by the Function Driver, and are
+ *            passed as function pointers in the CsrSdioFunctionDriver struct.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *
+ *----------------------------------------------------------------------------*/
+typedef void (*CsrSdioInsertedCallback)(CsrSdioFunction *function);
+typedef void (*CsrSdioRemovedCallback)(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioInterruptDsrCallback, CsrSdioInterruptCallback
+ *
+ *  DESCRIPTION
+ *      CsrSdioInterruptCallback is called when an interrupt occurs on the
+ *      the device associated with the specified function.
+ *
+ *      NOTE: These functions are implemented by the Function Driver, and are
+ *            passed as function pointers in the CsrSdioFunctionDriver struct.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *
+ *  RETURNS (only CsrSdioInterruptCallback)
+ *      A pointer to a CsrSdioInterruptDsrCallback function.
+ *
+ *----------------------------------------------------------------------------*/
+typedef void (*CsrSdioInterruptDsrCallback)(CsrSdioFunction *function);
+typedef CsrSdioInterruptDsrCallback (*CsrSdioInterruptCallback)(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioSuspendCallback, CsrSdioResumeCallback
+ *
+ *  DESCRIPTION
+ *      CsrSdioSuspendCallback is called when the system is preparing to go
+ *      into a suspended state. CsrSdioResumeCallback is called when the system
+ *      has entered an active state again.
+ *
+ *      NOTE: These functions are implemented by the Function Driver, and are
+ *            passed as function pointers in the CsrSdioFunctionDriver struct.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *
+ *----------------------------------------------------------------------------*/
+typedef void (*CsrSdioSuspendCallback)(CsrSdioFunction *function);
+typedef void (*CsrSdioResumeCallback)(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioAsyncCallback, CsrSdioAsyncDsrCallback
+ *
+ *  DESCRIPTION
+ *      CsrSdioAsyncCallback is called when an asynchronous operation completes.
+ *
+ *      NOTE: These functions are implemented by the Function Driver, and are
+ *            passed as function pointers in the function calls that initiate
+ *            the operation.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      result - The result of the operation that completed. See the description
+ *               of the initiating function for possible result values.
+ *
+ *  RETURNS (only CsrSdioAsyncCallback)
+ *      A pointer to a CsrSdioAsyncDsrCallback function.
+ *
+ *----------------------------------------------------------------------------*/
+typedef void (*CsrSdioAsyncDsrCallback)(CsrSdioFunction *function, CsrResult result);
+typedef CsrSdioAsyncDsrCallback (*CsrSdioAsyncCallback)(CsrSdioFunction *function, CsrResult result);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunctionDriver
+ *
+ *  DESCRIPTION
+ *      Structure representing a Function Driver.
+ *
+ *  MEMBERS
+ *      inserted - Callback, see description of CsrSdioInsertedCallback.
+ *      removed - Callback, see description of CsrSdioRemovedCallback.
+ *      intr - Callback, see description of CsrSdioInterruptCallback.
+ *      suspend - Callback, see description of CsrSdioSuspendCallback.
+ *      resume - Callback, see description of CsrSdioResumeCallback.
+ *      ids - Array of CsrSdioFunctionId describing one or more functions that
+ *            are supported by the Function Driver.
+ *      idsCount - Length of the ids array.
+ *      priv - For use by the SDIO Driver. The Function Driver may initialise
+ *             it to NULL, but shall otherwise not access the pointer or attempt
+ *             to dereference it.
+ *
+ *----------------------------------------------------------------------------*/
+typedef struct
+{
+    CsrSdioInsertedCallback  inserted;
+    CsrSdioRemovedCallback   removed;
+    CsrSdioInterruptCallback intr;
+    CsrSdioSuspendCallback   suspend;
+    CsrSdioResumeCallback    resume;
+    CsrSdioFunctionId       *ids;
+    u8                 idsCount;
+    void                    *priv;          /* For use by the SDIO Driver */
+} CsrSdioFunctionDriver;
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunctionDriverRegister
+ *
+ *  DESCRIPTION
+ *      Register a Function Driver.
+ *
+ *  PARAMETERS
+ *      functionDriver - Pointer to struct describing the Function Driver.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The Function Driver was successfully
+ *                           registered.
+ *      CSR_RESULT_FAILURE - Unable to register the function driver,
+ *                                because of an unspecified/unknown error. The
+ *                                Function Driver has not been registered.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - The specified Function Driver pointer
+ *                                      does not point at a valid Function
+ *                                      Driver structure, or some of the members
+ *                                      contain invalid entries.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioFunctionDriverRegister(CsrSdioFunctionDriver *functionDriver);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunctionDriverUnregister
+ *
+ *  DESCRIPTION
+ *      Unregister a previously registered Function Driver.
+ *
+ *  PARAMETERS
+ *      functionDriver - pointer to struct describing the Function Driver.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSdioFunctionDriverUnregister(CsrSdioFunctionDriver *functionDriver);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunctionEnable, CsrSdioFunctionDisable
+ *
+ *  DESCRIPTION
+ *      Enable/disable the specified function by setting/clearing the
+ *      corresponding bit in the I/O Enable register in function 0, and then
+ *      periodically reading the related bit in the I/O Ready register until it
+ *      is set/clear, limited by an implementation defined timeout.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The specified function was enabled/disabled.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. The state of the
+ *                                  related bit in the I/O Enable register is
+ *                                  undefined.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device, or the related
+ *                                bit in the I/O ready register was not
+ *                                set/cleared within the timeout period.
+ *
+ *      NOTE: If the SDIO R5 response is available, and either of the
+ *            FUNCTION_NUMBER or OUT_OF_RANGE bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE shall be returned. If the ERROR bit
+ *            is set (but none of FUNCTION_NUMBER or OUT_OF_RANGE),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioFunctionEnable(CsrSdioFunction *function);
+CsrResult CsrSdioFunctionDisable(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioInterruptEnable, CsrSdioInterruptDisable
+ *
+ *  DESCRIPTION
+ *      Enable/disable the interrupt for the specified function by
+ *      setting/clearing the corresponding bit in the INT Enable register in
+ *      function 0.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The specified function was enabled/disabled.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. The state of the
+ *                                  related bit in the INT Enable register is
+ *                                  unchanged.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - The specified function cannot be
+ *                                      enabled/disabled, because it either
+ *                                      does not exist or it is not possible to
+ *                                      individually enable/disable functions.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
+ *
+ *      NOTE: If the SDIO R5 response is available, and either of the
+ *            FUNCTION_NUMBER or OUT_OF_RANGE bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE shall be returned. If the ERROR bit
+ *            is set (but none of FUNCTION_NUMBER or OUT_OF_RANGE),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioInterruptEnable(CsrSdioFunction *function);
+CsrResult CsrSdioInterruptDisable(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioInterruptAcknowledge
+ *
+ *  DESCRIPTION
+ *      Acknowledge that a signalled interrupt has been handled. Shall only
+ *      be called once, and exactly once for each signalled interrupt to the
+ *      corresponding function.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function to which the
+ *                 event was signalled.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSdioInterruptAcknowledge(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioInsertedAcknowledge, CsrSdioRemovedAcknowledge
+ *
+ *  DESCRIPTION
+ *      Acknowledge that a signalled inserted/removed event has been handled.
+ *      Shall only be called once, and exactly once for each signalled event to
+ *      the corresponding function.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function to which the
+ *                 inserted was signalled.
+ *      result (CsrSdioInsertedAcknowledge only)
+ *          CSR_RESULT_SUCCESS - The Function Driver has accepted the
+ *                                    function, and the function is attached to
+ *                                    the Function Driver until the
+ *                                    CsrSdioRemovedCallback is called and
+ *                                    acknowledged.
+ *          CSR_RESULT_FAILURE - Unable to accept the function. The
+ *                                    function is not attached to the Function
+ *                                    Driver, and it may be passed to another
+ *                                    Function Driver which supports the
+ *                                    function.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSdioInsertedAcknowledge(CsrSdioFunction *function, CsrResult result);
+void CsrSdioRemovedAcknowledge(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioSuspendAcknowledge, CsrSdioResumeAcknowledge
+ *
+ *  DESCRIPTION
+ *      Acknowledge that a signalled suspend event has been handled. Shall only
+ *      be called once, and exactly once for each signalled event to the
+ *      corresponding function.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function to which the
+ *                 event was signalled.
+ *      result
+ *          CSR_RESULT_SUCCESS - Successfully suspended/resumed.
+ *          CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSdioSuspendAcknowledge(CsrSdioFunction *function, CsrResult result);
+void CsrSdioResumeAcknowledge(CsrSdioFunction *function, CsrResult result);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioBlockSizeSet
+ *
+ *  DESCRIPTION
+ *      Set the block size to use for the function. The actual configured block
+ *      size shall be the minimum of:
+ *          1) Maximum block size supported by the function.
+ *          2) Maximum block size supported by the host controller.
+ *          3) The block size specified by the blockSize argument.
+ *
+ *      When this function returns, the actual configured block size is
+ *      available in the blockSize member of the function struct.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      blockSize - Block size to use for the function. Valid range is 1 to
+ *                  2048.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The block size register on the chip
+ *                                was updated.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. The configured block
+ *                                  size is undefined.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
+ *
+ *      NOTE: If the SDIO R5 response is available, and the FUNCTION_NUMBER
+ *            bits is set, CSR_SDIO_RESULT_INVALID_VALUE shall be returned.
+ *            If the ERROR bit is set (but not FUNCTION_NUMBER),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *      NOTE: Setting the block size requires two individual operations. The
+ *            implementation shall ignore the OUT_OF_RANGE bit of the SDIO R5
+ *            response for the first operation, as the partially configured
+ *            block size may be out of range, even if the final block size
+ *            (after the second operation) is in the valid range.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioBlockSizeSet(CsrSdioFunction *function, u16 blockSize);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioMaxBusClockFrequencySet
+ *
+ *  DESCRIPTION
+ *      Set the maximum clock frequency to use for the device associated with
+ *      the specified function. The actual configured clock frequency for the
+ *      device shall be the minimum of:
+ *          1) Maximum clock frequency supported by the device.
+ *          2) Maximum clock frequency supported by the host controller.
+ *          3) Maximum clock frequency specified for any function on the same
+ *             device.
+ *
+ *      If the clock frequency exceeds 25MHz, it is the responsibility of the
+ *      SDIO driver to enable high speed mode on the device, using the standard
+ *      defined procedure, before increasing the frequency beyond the limit.
+ *
+ *      Note that the clock frequency configured affects all functions on the
+ *      same device.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      maxFrequency - The maximum clock frequency for the function in Hertz.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The maximum clock frequency was succesfully
+ *                                set for the function.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *
+ *      NOTE: If the SDIO R5 response is available, and the FUNCTION_NUMBER
+ *            bits is set, CSR_SDIO_RESULT_INVALID_VALUE shall be returned.
+ *            If the ERROR bit is set (but not FUNCTION_NUMBER),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioMaxBusClockFrequencySet(CsrSdioFunction *function, u32 maxFrequency);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioRead8, CsrSdioWrite8, CsrSdioRead8Async, CsrSdioWrite8Async
+ *
+ *  DESCRIPTION
+ *      Read/write an 8bit value from/to the specified register address.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      address - Register address within the function.
+ *      data - The data to read/write.
+ *      callback - The function to call on operation completion.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The data was successfully read/written.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. No data read/written.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
+ *
+ *      NOTE: If the SDIO R5 response is available, and either of the
+ *            FUNCTION_NUMBER or OUT_OF_RANGE bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE shall be returned. If the ERROR bit
+ *            is set (but none of FUNCTION_NUMBER or OUT_OF_RANGE),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *      NOTE: The CsrSdioRead8Async and CsrSdioWrite8Async functions return
+ *            immediately, and the supplied callback function is called when the
+ *            operation is complete. The result value is given as an argument to
+ *            the callback function.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioRead8(CsrSdioFunction *function, u32 address, u8 *data);
+CsrResult CsrSdioWrite8(CsrSdioFunction *function, u32 address, u8 data);
+void CsrSdioRead8Async(CsrSdioFunction *function, u32 address, u8 *data, CsrSdioAsyncCallback callback);
+void CsrSdioWrite8Async(CsrSdioFunction *function, u32 address, u8 data, CsrSdioAsyncCallback callback);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioRead16, CsrSdioWrite16, CsrSdioRead16Async, CsrSdioWrite16Async
+ *
+ *  DESCRIPTION
+ *      Read/write a 16bit value from/to the specified register address.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      address - Register address within the function.
+ *      data - The data to read/write.
+ *      callback - The function to call on operation completion.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The data was successfully read/written.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. Data may have been
+ *                                  partially read/written.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
+ *
+ *      NOTE: If the SDIO R5 response is available, and either of the
+ *            FUNCTION_NUMBER or OUT_OF_RANGE bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE shall be returned. If the ERROR bit
+ *            is set (but none of FUNCTION_NUMBER or OUT_OF_RANGE),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *      NOTE: The CsrSdioRead16Async and CsrSdioWrite16Async functions return
+ *            immediately, and the supplied callback function is called when the
+ *            operation is complete. The result value is given as an argument to
+ *            the callback function.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioRead16(CsrSdioFunction *function, u32 address, u16 *data);
+CsrResult CsrSdioWrite16(CsrSdioFunction *function, u32 address, u16 data);
+void CsrSdioRead16Async(CsrSdioFunction *function, u32 address, u16 *data, CsrSdioAsyncCallback callback);
+void CsrSdioWrite16Async(CsrSdioFunction *function, u32 address, u16 data, CsrSdioAsyncCallback callback);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioF0Read8, CsrSdioF0Write8, CsrSdioF0Read8Async,
+ *      CsrSdioF0Write8Async
+ *
+ *  DESCRIPTION
+ *      Read/write an 8bit value from/to the specified register address in
+ *      function 0.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      address - Register address within the function.
+ *      data - The data to read/write.
+ *      callback - The function to call on operation completion.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The data was successfully read/written.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. No data read/written.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
+ *
+ *      NOTE: If the SDIO R5 response is available, and either of the
+ *            FUNCTION_NUMBER or OUT_OF_RANGE bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE shall be returned. If the ERROR bit
+ *            is set (but none of FUNCTION_NUMBER or OUT_OF_RANGE),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *      NOTE: The CsrSdioF0Read8Async and CsrSdioF0Write8Async functions return
+ *            immediately, and the supplied callback function is called when the
+ *            operation is complete. The result value is given as an argument to
+ *            the callback function.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioF0Read8(CsrSdioFunction *function, u32 address, u8 *data);
+CsrResult CsrSdioF0Write8(CsrSdioFunction *function, u32 address, u8 data);
+void CsrSdioF0Read8Async(CsrSdioFunction *function, u32 address, u8 *data, CsrSdioAsyncCallback callback);
+void CsrSdioF0Write8Async(CsrSdioFunction *function, u32 address, u8 data, CsrSdioAsyncCallback callback);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioRead, CsrSdioWrite, CsrSdioReadAsync, CsrSdioWriteAsync
+ *
+ *  DESCRIPTION
+ *      Read/write a specified number of bytes from/to the specified register
+ *      address.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *      address - Register address within the function.
+ *      data - The data to read/write.
+ *      length - Number of byte to read/write.
+ *      callback - The function to call on operation completion.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - The data was successfully read/written.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_INVALID_VALUE - One or more arguments were invalid.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured. Data may have been
+ *                                  partially read/written.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device.
+ *
+ *      NOTE: If the SDIO R5 response is available, and either of the
+ *            FUNCTION_NUMBER or OUT_OF_RANGE bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE shall be returned. If the ERROR bit
+ *            is set (but none of FUNCTION_NUMBER or OUT_OF_RANGE),
+ *            CSR_RESULT_FAILURE shall be returned. The ILLEGAL_COMMAND and
+ *            COM_CRC_ERROR bits shall be ignored.
+ *
+ *            If the CSPI response is available, and any of the
+ *            FUNCTION_DISABLED or CLOCK_DISABLED bits are set,
+ *            CSR_SDIO_RESULT_INVALID_VALUE will be returned.
+ *
+ *      NOTE: The CsrSdioF0Read8Async and CsrSdioF0Write8Async functions return
+ *            immediately, and the supplied callback function is called when the
+ *            operation is complete. The result value is given as an argument to
+ *            the callback function.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioRead(CsrSdioFunction *function, u32 address, void *data, u32 length);
+CsrResult CsrSdioWrite(CsrSdioFunction *function, u32 address, const void *data, u32 length);
+void CsrSdioReadAsync(CsrSdioFunction *function, u32 address, void *data, u32 length, CsrSdioAsyncCallback callback);
+void CsrSdioWriteAsync(CsrSdioFunction *function, u32 address, const void *data, u32 length, CsrSdioAsyncCallback callback);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioPowerOn, CsrSdioPowerOff
+ *
+ *  DESCRIPTION
+ *      Power on/off the device.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function that resides on
+ *                 the device to power on/off.
+ *
+ *  RETURNS (only CsrSdioPowerOn)
+ *      CSR_RESULT_SUCCESS - Power was succesfully reapplied and the device
+ *                                has been reinitialised.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured during reinitialisation.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device during
+ *                                reinitialisation.
+ *      CSR_SDIO_RESULT_NOT_RESET - The power was not removed by the
+ *                                  CsrSdioPowerOff call. The state of the
+ *                                  device is unchanged.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioPowerOn(CsrSdioFunction *function);
+void CsrSdioPowerOff(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioHardReset
+ *
+ *  DESCRIPTION
+ *      Perform a hardware reset of the device.
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function that resides on
+ *                 the device to hard reset.
+ *
+ *  RETURNS
+ *      CSR_RESULT_SUCCESS - Reset was succesfully performed and the device
+ *                                has been reinitialised.
+ *      CSR_RESULT_FAILURE - Unspecified/unknown error.
+ *      CSR_SDIO_RESULT_NO_DEVICE - The device does not exist anymore.
+ *      CSR_SDIO_RESULT_CRC_ERROR - A CRC error occured during reinitialisation.
+ *      CSR_SDIO_RESULT_TIMEOUT - No response from the device during
+ *                                reinitialisation.
+ *      CSR_SDIO_RESULT_NOT_RESET - The reset was not applied because it is not
+ *                                  supported. The state of the device is
+ *                                  unchanged.
+ *
+ *----------------------------------------------------------------------------*/
+CsrResult CsrSdioHardReset(CsrSdioFunction *function);
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrSdioFunctionActive, CsrSdioFunctionIdle
+ *
+ *  DESCRIPTION
+ *
+ *  PARAMETERS
+ *      function - Pointer to struct representing the function.
+ *
+ *----------------------------------------------------------------------------*/
+void CsrSdioFunctionActive(CsrSdioFunction *function);
+void CsrSdioFunctionIdle(CsrSdioFunction *function);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_serialize_primitive_types.c b/drivers/staging/csr/csr_serialize_primitive_types.c
new file mode 100644
index 00000000000..bf5e4ab9f95
--- /dev/null
+++ b/drivers/staging/csr/csr_serialize_primitive_types.c
@@ -0,0 +1,101 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include "csr_prim_defs.h"
+#include "csr_msgconv.h"
+#include "csr_macro.h"
+#include "csr_lib.h"
+
+void CsrUint8Des(u8 *value, u8 *buffer, size_t *offset)
+{
+    *value = buffer[*offset];
+    *offset += sizeof(*value);
+}
+EXPORT_SYMBOL_GPL(CsrUint8Des);
+
+void CsrUint16Des(u16 *value, u8 *buffer, size_t *offset)
+{
+    *value = (buffer[*offset + 0] << 0) |
+             (buffer[*offset + 1] << 8);
+    *offset += sizeof(*value);
+}
+EXPORT_SYMBOL_GPL(CsrUint16Des);
+
+void CsrUint32Des(u32 *value, u8 *buffer, size_t *offset)
+{
+    *value = (buffer[*offset + 0] << 0) |
+             (buffer[*offset + 1] << 8) |
+             (buffer[*offset + 2] << 16) |
+             (buffer[*offset + 3] << 24);
+    *offset += sizeof(*value);
+}
+EXPORT_SYMBOL_GPL(CsrUint32Des);
+
+void CsrMemCpyDes(void *value, u8 *buffer, size_t *offset, size_t length)
+{
+    memcpy(value, &buffer[*offset], length);
+    *offset += length;
+}
+EXPORT_SYMBOL_GPL(CsrMemCpyDes);
+
+void CsrCharStringDes(char **value, u8 *buffer, size_t *offset)
+{
+    *value = kstrdup((char *) &buffer[*offset], GFP_KERNEL);
+    *offset += strlen(*value) + 1;
+}
+EXPORT_SYMBOL_GPL(CsrCharStringDes);
+
+void CsrUint8Ser(u8 *buffer, size_t *offset, u8 value)
+{
+    buffer[*offset] = value;
+    *offset += sizeof(value);
+}
+EXPORT_SYMBOL_GPL(CsrUint8Ser);
+
+void CsrUint16Ser(u8 *buffer, size_t *offset, u16 value)
+{
+    buffer[*offset + 0] = (u8) ((value >> 0) & 0xFF);
+    buffer[*offset + 1] = (u8) ((value >> 8) & 0xFF);
+    *offset += sizeof(value);
+}
+EXPORT_SYMBOL_GPL(CsrUint16Ser);
+
+void CsrUint32Ser(u8 *buffer, size_t *offset, u32 value)
+{
+    buffer[*offset + 0] = (u8) ((value >> 0) & 0xFF);
+    buffer[*offset + 1] = (u8) ((value >> 8) & 0xFF);
+    buffer[*offset + 2] = (u8) ((value >> 16) & 0xFF);
+    buffer[*offset + 3] = (u8) ((value >> 24) & 0xFF);
+    *offset += sizeof(value);
+}
+EXPORT_SYMBOL_GPL(CsrUint32Ser);
+
+void CsrMemCpySer(u8 *buffer, size_t *offset, const void *value, size_t length)
+{
+    memcpy(&buffer[*offset], value, length);
+    *offset += length;
+}
+EXPORT_SYMBOL_GPL(CsrMemCpySer);
+
+void CsrCharStringSer(u8 *buffer, size_t *offset, const char *value)
+{
+    if (value)
+    {
+        strcpy(((char *) &buffer[*offset]), value);
+        *offset += strlen(value) + 1;
+    }
+    else
+    {
+        CsrUint8Ser(buffer, offset, 0);
+    }
+}
+EXPORT_SYMBOL_GPL(CsrCharStringSer);
diff --git a/drivers/staging/csr/csr_time.c b/drivers/staging/csr/csr_time.c
new file mode 100644
index 00000000000..83586ca34e8
--- /dev/null
+++ b/drivers/staging/csr/csr_time.c
@@ -0,0 +1,43 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/version.h>
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 33)
+#elif LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 16)
+#include <linux/autoconf.h>
+#include <linux/config.h>
+#endif
+
+#include <linux/time.h>
+#include <linux/module.h>
+
+#include "csr_time.h"
+
+CsrTime CsrTimeGet(CsrTime *high)
+{
+    struct timespec ts;
+    u64 time;
+    CsrTime low;
+
+    ts = current_kernel_time();
+    time = (u64) ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
+
+    if (high != NULL)
+    {
+        *high = (CsrTime) ((time >> 32) & 0xFFFFFFFF);
+    }
+
+    low = (CsrTime) (time & 0xFFFFFFFF);
+
+    return low;
+}
+EXPORT_SYMBOL_GPL(CsrTimeGet);
diff --git a/drivers/staging/csr/csr_time.h b/drivers/staging/csr/csr_time.h
new file mode 100644
index 00000000000..2a45f3e4024
--- /dev/null
+++ b/drivers/staging/csr/csr_time.h
@@ -0,0 +1,114 @@
+#ifndef CSR_TIME_H__
+#define CSR_TIME_H__
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*******************************************************************************
+
+    NAME
+        CsrTime
+
+    DESCRIPTION
+        Type to hold a value describing the current system time, which is a
+        measure of time elapsed since some arbitrarily defined fixed time
+        reference, usually associated with system startup.
+
+*******************************************************************************/
+typedef u32 CsrTime;
+
+
+/*******************************************************************************
+
+    NAME
+        CsrTimeUtc
+
+    DESCRIPTION
+        Type to hold a value describing a UTC wallclock time expressed in
+        seconds and milliseconds elapsed since midnight January 1st 1970.
+
+*******************************************************************************/
+typedef struct
+{
+    u32 sec;
+    u16 msec;
+} CsrTimeUtc;
+
+
+/*******************************************************************************
+
+    NAME
+        CsrTimeGet
+
+    DESCRIPTION
+        Returns the current system time in a low and a high part. The low part
+        is expressed in microseconds. The high part is incremented when the low
+        part wraps to provide an extended range.
+
+        The caller may provide a NULL pointer as the high parameter. In this case
+        the function just returns the low part and ignores the high parameter.
+
+        Although the time is expressed in microseconds the actual resolution is
+        platform dependent and can be less. It is recommended that the
+        resolution is at least 10 milliseconds.
+
+    PARAMETERS
+        high - Pointer to variable that will receive the high part of the
+               current system time. Passing NULL is valid.
+
+    RETURNS
+        Low part of current system time in microseconds.
+
+*******************************************************************************/
+CsrTime CsrTimeGet(CsrTime *high);
+
+
+/*------------------------------------------------------------------*/
+/* CsrTime Macros */
+/*------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrTimeAdd
+ *
+ *  DESCRIPTION
+ *      Add two time values. Adding the numbers can overflow the range of a
+ *      CsrTime, so the user must be cautious.
+ *
+ *  RETURNS
+ *      CsrTime - the sum of "t1" and "t2".
+ *
+ *----------------------------------------------------------------------------*/
+#define CsrTimeAdd(t1, t2) ((t1) + (t2))
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrTimeSub
+ *
+ *  DESCRIPTION
+ *      Subtract two time values. Subtracting the numbers can provoke an
+ *      underflow, so the user must be cautious.
+ *
+ *  RETURNS
+ *      CsrTime - "t1" - "t2".
+ *
+ *----------------------------------------------------------------------------*/
+#define CsrTimeSub(t1, t2)    ((s32) (t1) - (s32) (t2))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_util.c b/drivers/staging/csr/csr_util.c
new file mode 100644
index 00000000000..c3aa9d509e5
--- /dev/null
+++ b/drivers/staging/csr/csr_util.c
@@ -0,0 +1,15 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/module.h>
+
+MODULE_DESCRIPTION("CSR Operating System Kernel Abstraction");
+MODULE_AUTHOR("Cambridge Silicon Radio Ltd.");
+MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/staging/csr/csr_wifi_common.h b/drivers/staging/csr/csr_wifi_common.h
new file mode 100644
index 00000000000..cc41a94b8f2
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_common.h
@@ -0,0 +1,109 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_COMMON_H__
+#define CSR_WIFI_COMMON_H__
+
+#include <linux/types.h>
+#include "csr_result.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* MAC address */
+typedef struct
+{
+    u8 a[6];
+} CsrWifiMacAddress;
+
+/* IPv4 address */
+typedef struct
+{
+    u8 a[4];
+} CsrWifiIp4Address;
+
+/* IPv6 address */
+typedef struct
+{
+    u8 a[16];
+} CsrWifiIp6Address;
+
+typedef struct
+{
+    u8 ssid[32];
+    u8 length;
+} CsrWifiSsid;
+
+/*******************************************************************************
+
+  DESCRIPTION
+    Result values used on the Wifi Interfaces
+
+ VALUES
+    CSR_RESULT_SUCCESS
+                   - The request/procedure succeeded
+    CSR_RESULT_FAILURE
+                   - The request/procedure did not succeed because of an error
+    CSR_WIFI_RESULT_NOT_FOUND
+                   - The request did not succeed because some resource was not
+                     found.
+    CSR_WIFI_RESULT_TIMED_OUT
+                   - The request/procedure did not succeed because of a time out
+    CSR_WIFI_RESULT_CANCELLED
+                   - The request was canceled due to another conflicting
+                     request that was issued before this one was completed
+    CSR_WIFI_RESULT_INVALID_PARAMETER
+                   - The request/procedure did not succeed because it had an
+                     invalid parameter
+    CSR_WIFI_RESULT_NO_ROOM
+                   - The request did not succeed due to a lack of resources,
+                     e.g. out of memory problem.
+    CSR_WIFI_RESULT_UNSUPPORTED
+                   - The request/procedure did not succeed because the feature
+                     is not supported yet
+    CSR_WIFI_RESULT_UNAVAILABLE
+                   - The request cannot be processed at this time
+    CSR_WIFI_RESULT_WIFI_OFF
+                   - The requested action is not available because Wi-Fi is
+                     currently off
+    CSR_WIFI_RESULT_SECURITY_ERROR
+                   - The request/procedure did not succeed because of a security
+                     error
+    CSR_WIFI_RESULT_MIB_SET_FAILURE
+                   - MIB Set Failure: either the MIB OID to be written to does
+                     not exist or the MIB Value is invalid.
+    CSR_WIFI_RESULT_INVALID_INTERFACE_TAG
+                   - The supplied Interface Tag is not valid.
+    CSR_WIFI_RESULT_P2P_NOA_CONFIG_CONFLICT
+                   - The new NOA configuration conflicts with the existing NOA configuration
+                     hence not accepted"
+*******************************************************************************/
+#define CSR_WIFI_RESULT_NOT_FOUND                 ((CsrResult) 0x0001)
+#define CSR_WIFI_RESULT_TIMED_OUT                 ((CsrResult) 0x0002)
+#define CSR_WIFI_RESULT_CANCELLED                 ((CsrResult) 0x0003)
+#define CSR_WIFI_RESULT_INVALID_PARAMETER         ((CsrResult) 0x0004)
+#define CSR_WIFI_RESULT_NO_ROOM                   ((CsrResult) 0x0005)
+#define CSR_WIFI_RESULT_UNSUPPORTED               ((CsrResult) 0x0006)
+#define CSR_WIFI_RESULT_UNAVAILABLE               ((CsrResult) 0x0007)
+#define CSR_WIFI_RESULT_WIFI_OFF                  ((CsrResult) 0x0008)
+#define CSR_WIFI_RESULT_SECURITY_ERROR            ((CsrResult) 0x0009)
+#define CSR_WIFI_RESULT_MIB_SET_FAILURE           ((CsrResult) 0x000A)
+#define CSR_WIFI_RESULT_INVALID_INTERFACE_TAG     ((CsrResult) 0x000B)
+#define CSR_WIFI_RESULT_P2P_NOA_CONFIG_CONFLICT   ((CsrResult) 0x000C)
+
+#define CSR_WIFI_VERSION	"5.1.0.0"
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/drivers/staging/csr/csr_wifi_fsm.h b/drivers/staging/csr/csr_wifi_fsm.h
new file mode 100644
index 00000000000..073e2f8b553
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_fsm.h
@@ -0,0 +1,248 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_FSM_H
+#define CSR_WIFI_FSM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "csr_prim_defs.h"
+#include "csr_log_text.h"
+#include "csr_wifi_fsm_event.h"
+
+/* including this file for CsrWifiInterfaceMode*/
+#include "csr_wifi_common.h"
+
+#define CSR_WIFI_FSM_ENV       (0xFFFF)
+
+/**
+ * @brief
+ *   Toplevel FSM context data
+ *
+ * @par Description
+ *   Holds ALL FSM static and dynamic data for a FSM
+ */
+typedef struct CsrWifiFsmContext CsrWifiFsmContext;
+
+/**
+ * @brief
+ *   FSM External Wakeup CallbackFunction Pointer
+ *
+ * @par Description
+ *   Defines the external wakeup function for the FSM
+ *   to call when an external event is injected into the systen
+ *
+ * @param[in]    context : External context
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmExternalWakupCallbackPtr)(void *context);
+
+/**
+ * @brief
+ *   Initialises a top level FSM context
+ *
+ * @par Description
+ *   Initialises the FSM Context to an initial state and allocates
+ *   space for "maxProcesses" number of instances
+ *
+ * @param[in]    osaContext         : OSA context
+ * @param[in]    applicationContext : Internal fsm application context
+ * @param[in]    externalContext    : External context
+ * @param[in]    maxProcesses       : Max processes to allocate room for
+ *
+ * @return
+ *   CsrWifiFsmContext* fsm context
+ */
+extern CsrWifiFsmContext* CsrWifiFsmInit(void *applicationContext, void *externalContext, u16 maxProcesses, CsrLogTextTaskId loggingTaskId);
+
+/**
+ * @brief
+ *   Resets the FSM's back to first conditions
+ *
+ * @par Description
+ *   This function is used to free any dynamic resources allocated for the
+ *   given context by CsrWifiFsmInit().
+ *   The FSM's reset function is called to cleanup any fsm specific memory
+ *   The reset funtion does NOT need to free the fsm data pointer as
+ *   CsrWifiFsmShutdown() will do it.
+ *   the FSM's init function is call again to reinitialise the FSM context.
+ *   CsrWifiFsmReset() should NEVER be called when CsrWifiFsmExecute() is running.
+ *
+ * @param[in]    context    : FSM context
+ *
+ * @return
+ *   void
+ */
+extern void CsrWifiFsmReset(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   Frees resources allocated by CsrWifiFsmInit
+ *
+ * @par Description
+ *   This function is used to free any dynamic resources allocated for the
+ *   given context by CsrWifiFsmInit(), prior to complete termination of
+ *   the program.
+ *   The FSM's reset function is called to cleanup any fsm specific memory.
+ *   The reset funtion does NOT need to free the fsm data pointer as
+ *   CsrWifiFsmShutdown() will do it.
+ *   CsrWifiFsmShutdown() should NEVER be called when CsrWifiFsmExecute() is running.
+ *
+ * @param[in]    context       : FSM context
+ *
+ * @return
+ *   void
+ */
+extern void CsrWifiFsmShutdown(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   Executes the fsm context
+ *
+ * @par Description
+ *   Executes the FSM context and runs until ALL events in the context are processed.
+ *   When no more events are left to process then CsrWifiFsmExecute() returns to a time
+ *   specifying when to next call the CsrWifiFsmExecute()
+ *   Scheduling, threading, blocking and external event notification are outside
+ *   the scope of the FSM and CsrWifiFsmExecute().
+ *
+ * @param[in]    context  : FSM context
+ *
+ * @return
+ *   u32    Time in ms until next timeout or 0xFFFFFFFF for no timer set
+ */
+extern u32 CsrWifiFsmExecute(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   Adds an event to the FSM context's external event queue for processing
+ *
+ * @par Description
+ *   Adds an event to the contexts external queue
+ *   This is thread safe and adds an event to the fsm's external event queue.
+ *
+ * @param[in]    context      : FSM context
+ * @param[in]    event        : event to add to the event queue
+ * @param[in]    source       : source of the event (this can be a synergy task queue or an fsm instance id)
+ * @param[in]    destination  : destination of the event (This can be a fsm instance id or CSR_WIFI_FSM_ENV)
+ * @param[in]    id           : event id
+ *
+ * @return
+ *   void
+ */
+extern void CsrWifiFsmSendEventExternal(CsrWifiFsmContext *context, CsrWifiFsmEvent *event, u16 source, u16 destination, CsrPrim primtype, u16 id);
+
+/**
+ * @brief
+ *   Adds an Alien event to the FSM context's external event queue for processing
+ *
+ * @par Description
+ *   Adds an event to the contexts external queue
+ *   This is thread safe and adds an event to the fsm's external event queue.
+ *
+ * @param[in]    context      : FSM context
+ * @param[in]    event        : event to add to the event queue
+ * @param[in]    source       : source of the event (this can be a synergy task queue or an fsm instance id)
+ * @param[in]    destination  : destination of the event (This can be a fsm instance id or CSR_WIFI_FSM_ENV)
+ * @param[in]    id           : event id
+ */
+#define CsrWifiFsmSendAlienEventExternal(_context, _alienEvent, _source, _destination, _primtype, _id) \
+    { \
+        CsrWifiFsmAlienEvent *_evt = kmalloc(sizeof(CsrWifiFsmAlienEvent), GFP_KERNEL); \
+        _evt->alienEvent = _alienEvent; \
+        CsrWifiFsmSendEventExternal(_context, (CsrWifiFsmEvent *)_evt, _source, _destination, _primtype, _id); \
+    }
+
+
+/**
+ * @brief
+ *   Current time of day in ms
+ *
+ * @param[in]    context   : FSM context
+ *
+ * @return
+ *   u32 32 bit ms tick
+ */
+extern u32 CsrWifiFsmGetTimeOfDayMs(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   Gets the time until the next FSM timer expiry
+ *
+ * @par Description
+ *   Returns the next timeout time or 0 if no timers are set.
+ *
+ * @param[in]    context    : FSM context
+ *
+ * @return
+ *   u32    Time in ms until next timeout or 0xFFFFFFFF for no timer set
+ */
+extern u32 CsrWifiFsmGetNextTimeout(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   Fast forwards the fsm timers by ms Milliseconds
+ *
+ * @param[in]  context : FSM context
+ * @param[in]  ms      : Milliseconds to fast forward by
+ *
+ * @return
+ *   void
+ */
+extern void CsrWifiFsmFastForward(CsrWifiFsmContext *context, u16 ms);
+
+/**
+ * @brief
+ *   shift the current time of day by ms amount
+ *
+ * @par Description
+ *   usefull to speed up tests where time needs to pass
+ *
+ * @param[in]    context  : FSM context
+ * @param[in]    ms       : ms to adjust time by
+ *
+ * @return
+ *   void
+ */
+extern void CsrWifiFsmTestAdvanceTime(CsrWifiFsmContext *context, u32 ms);
+
+/**
+ * @brief
+ *    Check if the fsm has events to process
+ *
+ * @param[in]    context    : FSM context
+ *
+ * @return
+ *   u8 returns TRUE if there are events for the FSM to process
+ */
+extern u8 CsrWifiFsmHasEvents(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   function that installs the contexts wakeup function
+ *
+ * @param[in]    context    : FSM context
+ * @param[in]    callback   : Callback function pointer
+ *
+ * @return
+ *   void
+ */
+extern void CsrWifiFsmInstallWakeupCallback(CsrWifiFsmContext *context, CsrWifiFsmExternalWakupCallbackPtr callback);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_FSM_H */
+
diff --git a/drivers/staging/csr/csr_wifi_fsm_event.h b/drivers/staging/csr/csr_wifi_fsm_event.h
new file mode 100644
index 00000000000..57a5cafd40b
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_fsm_event.h
@@ -0,0 +1,50 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_FSM_EVENT_H
+#define CSR_WIFI_FSM_EVENT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+
+/**
+ * @brief
+ *   FSM event header.
+ *
+ * @par Description
+ *   All events MUST have this struct as the FIRST member.
+ *   The next member is used internally for linked lists
+ */
+typedef struct CsrWifiFsmEvent
+{
+    CsrPrim     type;
+    u16   primtype;
+    CsrSchedQid destination;
+    CsrSchedQid source;
+
+    /* Private pointer to allow an optimal Event list */
+    /* NOTE: Ignore this pointer.
+     *       Do not waste code initializing OR freeing it.
+     *       The pointer is used internally in the CsrWifiFsm code
+     *       to avoid a second malloc when queuing events.
+     */
+    struct CsrWifiFsmEvent *next;
+} CsrWifiFsmEvent;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_FSM_EVENT_H */
+
diff --git a/drivers/staging/csr/csr_wifi_fsm_types.h b/drivers/staging/csr/csr_wifi_fsm_types.h
new file mode 100644
index 00000000000..26752bf316e
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_fsm_types.h
@@ -0,0 +1,440 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_FSM_TYPES_H
+#define CSR_WIFI_FSM_TYPES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <linux/types.h>
+#include "csr_macro.h"
+#include "csr_panic.h"
+#include "csr_sched.h"
+
+#ifdef CSR_WIFI_FSM_MUTEX_ENABLE
+#include "csr_framework_ext.h"
+#endif
+
+#include "csr_wifi_fsm.h"
+
+#define CSR_WIFI_FSM_MAX_TRANSITION_HISTORY 10
+
+/**
+ * @brief
+ *   FSM event list header.
+ *
+ * @par Description
+ *   Singly linked list of events.
+ */
+typedef struct CsrWifiFsmEventList
+{
+    CsrWifiFsmEvent *first;
+    CsrWifiFsmEvent *last;
+} CsrWifiFsmEventList;
+
+
+/**
+ * @brief
+ *   FSM timer id.
+ *
+ * @par Description
+ *   Composite Id made up of the type, dest and a unique id so
+ *   CsrWifiFsmRemoveTimer knows where to look when removing the timer
+ */
+typedef struct CsrWifiFsmTimerId
+{
+    CsrPrim     type;
+    u16   primtype;
+    CsrSchedQid destination;
+    u16   uniqueid;
+} CsrWifiFsmTimerId;
+
+/**
+ * @brief
+ *   FSM timer header.
+ *
+ * @par Description
+ *   All timer MUST have this struct as the FIRST member.
+ *   The first members of the structure MUST remain compatable
+ *   with the CsrWifiFsmEvent so that timers are just specialised events
+ */
+typedef struct CsrWifiFsmTimer
+{
+    CsrPrim     type;
+    u16   primtype;
+    CsrSchedQid destination;
+    CsrSchedQid source;
+
+    /* Private pointer to allow an optimal Event list */
+    struct CsrWifiFsmTimer *next;
+
+    CsrWifiFsmTimerId timerid;
+    u32         timeoutTimeMs;
+} CsrWifiFsmTimer;
+
+
+/**
+ * @brief
+ *   Fsm Alien Event
+ *
+ * @par Description
+ *   Allows the wrapping of alien events that do not use CsrWifiFsmEvent
+ *   as the first member of the Event struct
+ */
+typedef struct
+{
+    CsrWifiFsmEvent event;
+    void           *alienEvent;
+} CsrWifiFsmAlienEvent;
+
+
+/**
+ * @brief
+ *   FSM timer list header.
+ *
+ * @par Description
+ *   Singly linked list of timers.
+ */
+typedef struct CsrWifiFsmTimerList
+{
+    CsrWifiFsmTimer *first;
+    CsrWifiFsmTimer *last;
+    u16        nexttimerid;
+} CsrWifiFsmTimerList;
+
+/**
+ * @brief
+ *   Process Entry Function Pointer
+ *
+ * @par Description
+ *   Defines the entry function for a processes.
+ *   Called at process initialisation.
+ *
+ * @param[in]    context : FSM context
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmProcEntryFnPtr)(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   Process Transition Function Pointer
+ *
+ * @par Description
+ *   Defines a transition function for a processes.
+ *   Called when an event causes a transition on a process
+ *
+ * @param[in]    CsrWifiFsmContext* : FSM context
+ * @param[in]    void* : FSM data (can be NULL)
+ * @param[in]    const CsrWifiFsmEvent*  : event to process
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmTransitionFnPtr)(CsrWifiFsmContext *context, void *fsmData, const CsrWifiFsmEvent *event);
+
+/**
+ * @brief
+ *   Process reset/shutdown Function Pointer
+ *
+ * @par Description
+ *   Defines the reset/shutdown function for a processes.
+ *   Called to reset or shutdown an fsm.
+ *
+ * @param[in]    context      : FSM context
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmProcResetFnPtr)(CsrWifiFsmContext *context);
+
+/**
+ * @brief
+ *   FSM Default Destination CallbackFunction Pointer
+ *
+ * @par Description
+ *   Defines the default destination function for the FSM
+ *   to call when an event does not have a valid destination.
+ *   This
+ *
+ * @param[in]    context : External context
+ *
+ * @return
+ *   u16 a valid destination OR CSR_WIFI_FSM_ENV
+ */
+typedef u16 (*CsrWifiFsmDestLookupCallbackPtr)(void *context, const CsrWifiFsmEvent *event);
+
+
+#ifdef CSR_WIFI_FSM_DUMP_ENABLE
+/**
+ * @brief
+ *   Trace Dump Function Pointer
+ *
+ * @par Description
+ *   Called when we want to trace the FSM
+ *
+ * @param[in]    context : FSM context
+ * @param[in]    id      : fsm id
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmDumpFnPtr)(CsrWifiFsmContext *context, void *fsmData);
+#endif
+
+/**
+ * @brief
+ *   Event ID to transition function entry
+ *
+ * @par Description
+ *   Event ID to Transition Entry in a state table.
+ */
+typedef struct
+{
+    u32                 eventid;
+    CsrWifiFsmTransitionFnPtr transition;
+#ifdef CSR_LOG_ENABLE
+    const char *transitionName;
+#endif
+} CsrWifiFsmEventEntry;
+
+/**
+ * @brief
+ *   Single State's Transition Table
+ *
+ * @par Description
+ *   Stores Data for a single State's event to
+ *   transition functions mapping
+ */
+typedef struct
+{
+    const u8              numEntries;
+    const u8               saveAll;
+    const CsrWifiFsmEventEntry *eventEntryArray; /* array of transition function pointers for state */
+#ifdef CSR_LOG_ENABLE
+    u16            stateNumber;
+    const char *stateName;
+#endif
+} CsrWifiFsmTableEntry;
+
+/**
+ * @brief
+ *   Process State Transtion table
+ *
+ * @par Description
+ *   Stores Data for a processes State to transition table
+ */
+typedef struct
+{
+    u16                   numStates;         /* number of states    */
+    const CsrWifiFsmTableEntry *aStateEventMatrix; /* state event matrix  */
+} CsrWifiFsmTransitionFunctionTable;
+
+/**
+ * @brief
+ *   Const Process definition
+ *
+ * @par Description
+ *   Constant process specification.
+ *   This is ALL the non dynamic data that defines
+ *   a process.
+ */
+typedef struct
+{
+    const char                    *processName;
+    const u32                         processId;
+    const CsrWifiFsmTransitionFunctionTable transitionTable;
+    const CsrWifiFsmTableEntry              unhandledTransitions;
+    const CsrWifiFsmTableEntry              ignoreFunctions;
+    const CsrWifiFsmProcEntryFnPtr          entryFn;
+    const CsrWifiFsmProcResetFnPtr          resetFn;
+#ifdef CSR_WIFI_FSM_DUMP_ENABLE
+    const CsrWifiFsmDumpFnPtr dumpFn;               /* Called to dump fsm specific trace if not NULL */
+#endif
+} CsrWifiFsmProcessStateMachine;
+
+#ifdef CSR_WIFI_FSM_DUMP_ENABLE
+/**
+ * @brief
+ *   Storage for state transition info
+ */
+typedef struct
+{
+    u16                 transitionNumber;
+    CsrWifiFsmEvent           event;
+    u16                 fromState;
+    u16                 toState;
+    CsrWifiFsmTransitionFnPtr transitionFn;
+    u16                 transitionCount; /* number consecutive of times this transition was seen */
+#ifdef CSR_LOG_ENABLE
+    const char *transitionName;
+#endif
+} CsrWifiFsmTransitionRecord;
+
+/**
+ * @brief
+ *   Storage for the last state X transitions
+ */
+typedef struct
+{
+    u16                  numTransitions;
+    CsrWifiFsmTransitionRecord records[CSR_WIFI_FSM_MAX_TRANSITION_HISTORY];
+} CsrWifiFsmTransitionRecords;
+#endif
+
+/**
+ * @brief
+ *   Dynamic Process data
+ *
+ * @par Description
+ *   Dynamic process data that is used to keep track of the
+ *   state and data for a process instance
+ */
+typedef struct
+{
+    const CsrWifiFsmProcessStateMachine *fsmInfo;         /* state machine info that is constant regardless of context */
+    u16                            instanceId;      /* Runtime process id */
+    u16                            state;           /* Current state */
+    void                                *params;          /* Instance user data */
+    CsrWifiFsmEventList                  savedEventQueue; /* The saved event queue */
+    struct CsrWifiFsmInstanceEntry      *subFsm;          /* Sub Fsm instance data */
+    struct CsrWifiFsmInstanceEntry      *subFsmCaller;    /* The Fsm instance that created the SubFsm and should be used for callbacks*/
+#ifdef CSR_WIFI_FSM_DUMP_ENABLE
+    CsrWifiFsmTransitionRecords transitionRecords;        /* Last X transitions in the FSM */
+#endif
+} CsrWifiFsmInstanceEntry;
+
+/**
+ * @brief
+ *   OnCreate Callback Function Pointer
+ *
+ * @par Description
+ *   Called when an fsm is created.
+ *
+ * @param[in]    extContext : External context
+ * @param[in]    instance : FSM instance
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmOnCreateFnPtr)(void *extContext, const CsrWifiFsmInstanceEntry *instance);
+
+/**
+ * @brief
+ *   OnTransition Callback Function Pointer
+ *
+ * @par Description
+ *   Called when an event is processed by a fsm
+ *
+ * @param[in]    extContext : External context
+ * @param[in]    eventEntryArray : Entry data
+ * @param[in]    event : Event
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmOnTransitionFnPtr)(void *extContext, const CsrWifiFsmEventEntry *eventEntryArray, const CsrWifiFsmEvent *event);
+
+/**
+ * @brief
+ *   OnStateChange Callback Function Pointer
+ *
+ * @par Description
+ *   Called when CsrWifiFsmNextState is called
+ *
+ * @param[in]    extContext : External context
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmOnStateChangeFnPtr)(void *extContext, u16 nextstate);
+
+/**
+ * @brief
+ *   OnIgnore,OnError or OnInvalid Callback Function Pointer
+ *
+ * @par Description
+ *   Called when an event is processed by a fsm
+ *
+ * @param[in]    extContext : External context
+ * @param[in]    event : Event
+ *
+ * @return
+ *   void
+ */
+typedef void (*CsrWifiFsmOnEventFnPtr)(void *extContext, const CsrWifiFsmEvent *event);
+
+/**
+ * @brief
+ *   Toplevel FSM context data
+ *
+ * @par Description
+ *   Holds ALL FSM static and dynamic data for a FSM
+ */
+struct CsrWifiFsmContext
+{
+    CsrWifiFsmEventList eventQueue;                           /* The internal event queue                     */
+    CsrWifiFsmEventList externalEventQueue;                   /* The external event queue                     */
+#ifdef CSR_WIFI_FSM_MUTEX_ENABLE
+    CsrMutexHandle externalEventQueueLock;                    /* The external event queue mutex               */
+#endif
+    u32                          timeOffset;            /* Amount to adjust the TimeOfDayMs by          */
+    CsrWifiFsmTimerList                timerQueue;            /* The internal timer queue                     */
+    u8                            useTempSaveList;       /* Should the temp save list be used            */
+    CsrWifiFsmEventList                tempSaveList;          /* The temp save event queue                    */
+    CsrWifiFsmEvent                   *eventForwardedOrSaved; /* The event that was forwarded or Saved        */
+    u16                          maxProcesses;          /* Size of instanceArray                        */
+    u16                          numProcesses;          /* Current number allocated in instanceArray    */
+    CsrWifiFsmInstanceEntry           *instanceArray;         /* Array of processes for this component        */
+    CsrWifiFsmInstanceEntry           *ownerInstance;         /* The Process that owns currentInstance (SubFsm support) */
+    CsrWifiFsmInstanceEntry           *currentInstance;       /* Current Process that is executing            */
+    CsrWifiFsmExternalWakupCallbackPtr externalEventFn;       /* External event Callback                      */
+    CsrWifiFsmOnEventFnPtr             appIgnoreCallback;     /* Application Ignore event Callback            */
+    CsrWifiFsmDestLookupCallbackPtr    appEvtDstCallback;     /* Application Lookup event Destination Function*/
+
+    void            *applicationContext;                      /* Internal fsm application context             */
+    void            *externalContext;                         /* External context (set by the user of the fsm)*/
+    CsrLogTextTaskId loggingTaskId;                           /* Task Id to use in any logging output         */
+
+#ifndef CSR_WIFI_FSM_SCHEDULER_DISABLED
+    CsrSchedTid schedTimerId;                                 /* Scheduler TimerId for use in Scheduler Tasks */
+    u32   schedTimerNexttimeoutMs;                      /* Next timeout time for the current timer      */
+#endif
+
+#ifdef CSR_WIFI_FSM_MUTEX_ENABLE
+#ifdef CSR_WIFI_FSM_TRANSITION_LOCK
+    CsrMutexHandle transitionLock;                     /* Lock when calling transition functions        */
+#endif
+#endif
+
+#ifdef CSR_LOG_ENABLE
+    CsrWifiFsmOnCreateFnPtr      onCreate;             /* Debug Transition Callback                    */
+    CsrWifiFsmOnTransitionFnPtr  onTransition;         /* Debug Transition Callback                    */
+    CsrWifiFsmOnTransitionFnPtr  onUnhandedCallback;   /* Unhanded event Callback                      */
+    CsrWifiFsmOnStateChangeFnPtr onStateChange;        /* Debug State Change Callback                  */
+    CsrWifiFsmOnEventFnPtr       onIgnoreCallback;     /* Ignore event Callback                        */
+    CsrWifiFsmOnEventFnPtr       onSaveCallback;       /* Save event Callback                          */
+    CsrWifiFsmOnEventFnPtr       onErrorCallback;      /* Error event Callback                         */
+    CsrWifiFsmOnEventFnPtr       onInvalidCallback;    /* Invalid event Callback                       */
+#endif
+#ifdef CSR_WIFI_FSM_DUMP_ENABLE
+    u16 masterTransitionNumber;                  /* Increments on every transition              */
+#endif
+};
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_FSM_TYPES_H */
diff --git a/drivers/staging/csr/csr_wifi_hip_card.h b/drivers/staging/csr/csr_wifi_hip_card.h
new file mode 100644
index 00000000000..9caf88c7887
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_card.h
@@ -0,0 +1,123 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ ******************************************************************************
+ * FILE : csr_wifi_hip_card.h
+ *
+ * PURPOSE : Defines abstract interface for hardware specific functions.
+ *           Note, this is a different file from one of the same name in the
+ *           Windows driver.
+ *
+ *****************************************************************************
+ */
+#ifndef __CARD_H__
+#define __CARD_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "csr_wifi_hip_card_sdio.h"
+#include "csr_wifi_hip_signals.h"
+#include "csr_wifi_hip_unifi_udi.h"
+
+
+/*****************************************************************************
+ * CardEnableInt -
+ */
+CsrResult CardEnableInt(card_t *card);
+
+/*****************************************************************************
+ * CardGenInt -
+ */
+CsrResult CardGenInt(card_t *card);
+
+/*****************************************************************************
+ * CardPendingInt -
+ */
+CsrResult CardPendingInt(card_t *card, u8 *pintr);
+
+/*****************************************************************************
+ * CardDisableInt -
+ */
+CsrResult CardDisableInt(card_t *card);
+
+/*****************************************************************************
+ * CardClearInt -
+ */
+CsrResult CardClearInt(card_t *card);
+
+/*****************************************************************************
+ * CardDisable -
+ */
+void CardDisable(card_t *card);
+
+/*****************************************************************************
+ * CardIntEnabled -
+ */
+CsrResult CardIntEnabled(card_t *card, u8 *enabled);
+
+/*****************************************************************************
+ * CardGetDataSlotSize
+ */
+u16 CardGetDataSlotSize(card_t *card);
+
+/*****************************************************************************
+ * CardWriteBulkData -
+ */
+CsrResult CardWriteBulkData(card_t *card, card_signal_t *csptr, unifi_TrafficQueue queue);
+
+
+/*****************************************************************************
+ * CardClearFromHostDataSlot -
+ */
+void CardClearFromHostDataSlot(card_t *card, const s16 aSlotNum);
+
+#ifdef CSR_WIFI_REQUEUE_PACKET_TO_HAL
+/*****************************************************************************
+ * CardClearFromHostDataSlotWithoutFreeingBulkData - Clear the data stot
+ * without freeing the bulk data
+ */
+
+void CardClearFromHostDataSlotWithoutFreeingBulkData(card_t *card, const s16 aSlotNum);
+#endif
+
+/*****************************************************************************
+ * CardGetFreeFromHostDataSlots -
+ */
+u16 CardGetFreeFromHostDataSlots(card_t *card);
+
+u16 CardAreAllFromHostDataSlotsEmpty(card_t *card);
+
+CsrResult card_start_processor(card_t *card, enum unifi_dbg_processors_select which);
+
+CsrResult card_wait_for_firmware_to_start(card_t *card, u32 *paddr);
+
+CsrResult unifi_dl_firmware(card_t *card, void *arg);
+CsrResult unifi_dl_patch(card_t *card, void *arg, u32 boot_ctrl);
+CsrResult unifi_do_loader_op(card_t *card, u32 op_addr, u8 opcode);
+void* unifi_dl_fw_read_start(card_t *card, s8 is_fw);
+
+CsrResult unifi_coredump_handle_request(card_t *card);
+
+CsrResult ConvertCsrSdioToCsrHipResult(card_t *card, CsrResult csrResult);
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+void unifi_debug_log_to_buf(const char *fmt, ...);
+void unifi_debug_string_to_buf(const char *str);
+void unifi_debug_hex_to_buf(const char *buff, u16 length);
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CARD_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio.c b/drivers/staging/csr/csr_wifi_hip_card_sdio.c
new file mode 100644
index 00000000000..44ab00c53fe
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio.c
@@ -0,0 +1,4163 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ * FILE: csr_wifi_hip_card_sdio.c
+ *
+ * PURPOSE: Implementation of the Card API for SDIO.
+ *
+ * NOTES:
+ *      CardInit() is called from the SDIO probe callback when a card is
+ *      inserted. This performs the basic SDIO initialisation, enabling i/o
+ *      etc.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/slab.h>
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "csr_wifi_hip_unifiversion.h"
+#include "csr_wifi_hip_card.h"
+#include "csr_wifi_hip_card_sdio.h"
+#include "csr_wifi_hip_chiphelper.h"
+
+
+/* Time to wait between attempts to read MAILBOX0 */
+#define MAILBOX1_TIMEOUT                10  /* in millisecs */
+#define MAILBOX1_ATTEMPTS               200 /* 2 seconds */
+
+#define MAILBOX2_TIMEOUT                5   /* in millisecs */
+#define MAILBOX2_ATTEMPTS               10  /* 50ms */
+
+#define RESET_SETTLE_DELAY              25  /* in millisecs */
+
+static CsrResult card_init_slots(card_t *card);
+static CsrResult card_hw_init(card_t *card);
+static CsrResult firmware_present_in_flash(card_t *card);
+static void bootstrap_chip_hw(card_t *card);
+static CsrResult unifi_reset_hardware(card_t *card);
+static CsrResult unifi_hip_init(card_t *card);
+static CsrResult card_access_panic(card_t *card);
+static CsrResult unifi_read_chip_version(card_t *card);
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_alloc_card
+ *
+ *      Allocate and initialise the card context structure.
+ *
+ *  Arguments:
+ *      sdio            Pointer to SDIO context pointer to pass to low
+ *                      level i/o functions.
+ *      ospriv          Pointer to O/S private struct to pass when calling
+ *                      callbacks to the higher level system.
+ *
+ *  Returns:
+ *      Pointer to card struct, which represents the driver context or
+ *      NULL if the allocation failed.
+ * ---------------------------------------------------------------------------
+ */
+card_t* unifi_alloc_card(CsrSdioFunction *sdio, void *ospriv)
+{
+    card_t *card;
+    u32 i;
+
+    func_enter();
+
+
+    card = kzalloc(sizeof(card_t), GFP_KERNEL);
+    if (card == NULL)
+    {
+        return NULL;
+    }
+
+    card->sdio_if = sdio;
+    card->ospriv  = ospriv;
+
+    card->unifi_interrupt_seq = 1;
+
+    /* Make these invalid. */
+    card->proc_select = (u32)(-1);
+    card->dmem_page = (u32)(-1);
+    card->pmem_page = (u32)(-1);
+
+    card->bh_reason_host = 0;
+    card->bh_reason_unifi = 0;
+
+    for (i = 0; i < sizeof(card->tx_q_paused_flag) / sizeof(card->tx_q_paused_flag[0]); i++)
+    {
+        card->tx_q_paused_flag[i] = 0;
+    }
+    card->memory_resources_allocated = 0;
+
+    card->low_power_mode = UNIFI_LOW_POWER_DISABLED;
+    card->periodic_wake_mode = UNIFI_PERIODIC_WAKE_HOST_DISABLED;
+
+    card->host_state = UNIFI_HOST_STATE_AWAKE;
+    card->intmode = CSR_WIFI_INTMODE_DEFAULT;
+
+    /*
+     * Memory resources for buffers are allocated when the chip is initialised
+     * because we need configuration information from the firmware.
+     */
+
+    /*
+     * Initialise wait queues and lists
+     */
+    card->fh_command_queue.q_body = card->fh_command_q_body;
+    card->fh_command_queue.q_length = UNIFI_SOFT_COMMAND_Q_LENGTH;
+
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        card->fh_traffic_queue[i].q_body = card->fh_traffic_q_body[i];
+        card->fh_traffic_queue[i].q_length = UNIFI_SOFT_TRAFFIC_Q_LENGTH;
+    }
+
+
+    /* Initialise mini-coredump pointers in case no coredump buffers
+     * are requested by the OS layer.
+     */
+    card->request_coredump_on_reset = 0;
+    card->dump_next_write = NULL;
+    card->dump_cur_read = NULL;
+    card->dump_buf = NULL;
+
+#ifdef UNIFI_DEBUG
+    /* Determine offset of LSB in pointer for later alignment sanity check.
+     * Synergy integer types have specific widths, which cause compiler
+     * warnings when casting pointer types, e.g. on 64-bit systems.
+     */
+    {
+        u32 val = 0x01234567;
+
+        if (*((u8 *)&val) == 0x01)
+        {
+            card->lsb = sizeof(void *) - 1;     /* BE */
+        }
+        else
+        {
+            card->lsb = 0;                      /* LE */
+        }
+    }
+#endif
+    func_exit();
+    return card;
+} /* unifi_alloc_card() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_init_card
+ *
+ *      Reset the hardware and perform HIP initialization
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *
+ *  Returns:
+ *      CsrResult code
+ *      CSR_RESULT_SUCCESS if successful
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_init_card(card_t *card, s32 led_mask)
+{
+    CsrResult r;
+
+    func_enter();
+
+    if (card == NULL)
+    {
+        func_exit_r(CSR_WIFI_HIP_RESULT_INVALID_VALUE);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    r = unifi_init(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        func_exit_r(r);
+        return r;
+    }
+
+    r = unifi_hip_init(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        func_exit_r(r);
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to start host protocol.\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_init
+ *
+ *      Init the hardware.
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *
+ *  Returns:
+ *      CsrResult code
+ *      CSR_RESULT_SUCCESS if successful
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_init(card_t *card)
+{
+    CsrResult r;
+    CsrResult csrResult;
+
+    func_enter();
+
+    if (card == NULL)
+    {
+        func_exit_r(CSR_WIFI_HIP_RESULT_INVALID_VALUE);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    /*
+     * Disable the SDIO interrupts while initialising UniFi.
+     * Re-enable them when f/w is running.
+     */
+    csrResult = CsrSdioInterruptDisable(card->sdio_if);
+    if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+    }
+
+    /*
+     * UniFi's PLL may start with a slow clock (~ 1 MHz) so initially
+     * set the SDIO bus clock to a similar value or SDIO accesses may
+     * fail.
+     */
+    csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_SAFE_HZ);
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+        func_exit_r(r);
+        return r;
+    }
+    card->sdio_clock_speed = UNIFI_SDIO_CLOCK_SAFE_HZ;
+
+    /*
+     * Reset UniFi. Note, this only resets the WLAN function part of the chip,
+     * the SDIO interface is not reset.
+     */
+    unifi_trace(card->ospriv, UDBG1, "Resetting UniFi\n");
+    r = unifi_reset_hardware(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to reset UniFi\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    /* Reset the power save mode, to be active until the MLME-reset is complete */
+    r = unifi_configure_low_power_mode(card,
+                                       UNIFI_LOW_POWER_DISABLED, UNIFI_PERIODIC_WAKE_HOST_DISABLED);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to set power save mode\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    /*
+     * Set initial value of page registers.
+     * The page registers will be maintained by unifi_read...() and
+     * unifi_write...().
+     */
+    card->proc_select = (u32)(-1);
+    card->dmem_page = (u32)(-1);
+    card->pmem_page = (u32)(-1);
+    r = unifi_write_direct16(card, ChipHelper_HOST_WINDOW3_PAGE(card->helper) * 2, 0);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write SHARED_DMEM_PAGE\n");
+        func_exit_r(r);
+        return r;
+    }
+    r = unifi_write_direct16(card, ChipHelper_HOST_WINDOW2_PAGE(card->helper) * 2, 0);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write PROG_MEM2_PAGE\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    /*
+     * If the driver has reset UniFi due to previous SDIO failure, this may
+     * have been due to a chip watchdog reset. In this case, the driver may
+     * have requested a mini-coredump which needs to be captured now the
+     * SDIO interface is alive.
+     */
+    (void)unifi_coredump_handle_request(card);
+
+    /*
+     * Probe to see if the UniFi has ROM/flash to boot from. CSR6xxx should do.
+     */
+    r = firmware_present_in_flash(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r == CSR_WIFI_HIP_RESULT_NOT_FOUND)
+    {
+        unifi_error(card->ospriv, "No firmware found\n");
+    }
+    else if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Probe for Flash failed\n");
+    }
+
+    func_exit_r(r);
+    return r;
+} /* unifi_init() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_download
+ *
+ *      Load the firmware.
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *      led_mask    Loader LED mask
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success
+ *      CsrResult error code on failure.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_download(card_t *card, s32 led_mask)
+{
+    CsrResult r;
+    void *dlpriv;
+
+    func_enter();
+
+    if (card == NULL)
+    {
+        func_exit_r(CSR_WIFI_HIP_RESULT_INVALID_VALUE);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    /* Set the loader led mask */
+    card->loader_led_mask = led_mask;
+
+    /* Get the firmware file information */
+    unifi_trace(card->ospriv, UDBG1, "downloading firmware...\n");
+
+    dlpriv = unifi_dl_fw_read_start(card, UNIFI_FW_STA);
+    if (dlpriv == NULL)
+    {
+        func_exit_r(CSR_WIFI_HIP_RESULT_NOT_FOUND);
+        return CSR_WIFI_HIP_RESULT_NOT_FOUND;
+    }
+
+    /* Download the firmware. */
+    r = unifi_dl_firmware(card, dlpriv);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to download firmware\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    /* Free the firmware file information. */
+    unifi_fw_read_stop(card->ospriv, dlpriv);
+
+    func_exit();
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_download() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_hip_init
+ *
+ *      This function performs the f/w initialisation sequence as described
+ *      in the Unifi Host Interface Protocol Specification.
+ *      It allocates memory for host-side slot data and signal queues.
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or else a CSR error code
+ *
+ *  Notes:
+ *      The firmware must have been downloaded.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult unifi_hip_init(card_t *card)
+{
+    CsrResult r;
+    CsrResult csrResult;
+
+    func_enter();
+
+    r = card_hw_init(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to establish communication with UniFi\n");
+        func_exit_r(r);
+        return r;
+    }
+#ifdef CSR_PRE_ALLOC_NET_DATA
+    /* if there is any preallocated netdata left from the prev session free it now */
+    prealloc_netdata_free(card);
+#endif
+    /*
+     * Allocate memory for host-side slot data and signal queues.
+     * We need the config info read from the firmware to know how much
+     * memory to allocate.
+     */
+    r = card_init_slots(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Init slots failed: %d\n", r);
+        func_exit_r(r);
+        return r;
+    }
+
+    unifi_trace(card->ospriv, UDBG2, "Sending first UniFi interrupt\n");
+
+    r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        func_exit_r(r);
+        return r;
+    }
+
+    /* Enable the SDIO interrupts now that the f/w is running. */
+    csrResult = CsrSdioInterruptEnable(card->sdio_if);
+    if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+    }
+
+    /* Signal the UniFi to start handling messages */
+    r = CardGenInt(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        func_exit_r(r);
+        return r;
+    }
+
+    func_exit();
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_hip_init() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  _build_sdio_config_data
+ *
+ *      Unpack the SDIO configuration information from a buffer read from
+ *      UniFi into a host structure.
+ *      The data is byte-swapped for a big-endian host if necessary by the
+ *      UNPACK... macros.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      cfg_data        Destination structure to unpack into.
+ *      cfg_data_buf    Source buffer to read from. This should be the raw
+ *                      data read from UniFi.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void _build_sdio_config_data(sdio_config_data_t *cfg_data,
+                                    const u8     *cfg_data_buf)
+{
+    s16 offset = 0;
+
+    cfg_data->version = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->sdio_ctrl_offset = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->fromhost_sigbuf_handle = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->tohost_sigbuf_handle = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->num_fromhost_sig_frags = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->num_tohost_sig_frags = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->num_fromhost_data_slots = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->num_tohost_data_slots = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->data_slot_size = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->initialised = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->overlay_size = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT32;
+
+    cfg_data->data_slot_round = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->sig_frag_size = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+    offset += SIZEOF_UINT16;
+
+    cfg_data->tohost_signal_padding = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cfg_data_buf + offset);
+} /* _build_sdio_config_data() */
+
+
+/*
+ * - Function ----------------------------------------------------------------
+ * card_hw_init()
+ *
+ *      Perform the initialisation procedure described in the UniFi Host
+ *      Interface Protocol document (section 3.3.8) and read the run-time
+ *      configuration information from the UniFi. This is stuff like number
+ *      of bulk data slots etc.
+ *
+ *      The card enumeration and SD initialisation has already been done by
+ *      the SDIO library, see card_sdio_init().
+ *
+ *      The initialisation is done when firmware is ready, i.e. this may need
+ *      to be called after a f/w download operation.
+ *
+ *      The initialisation procedure goes like this:
+ *       - Wait for UniFi to start-up by polling SHARED_MAILBOX1
+ *       - Find the symbol table and look up SLT_SDIO_SLOT_CONFIG
+ *       - Read the config structure
+ *       - Check the "SDIO initialised" flag, if not zero do a h/w reset and
+ *         start again
+ *       - Decide the number of bulk data slots to allocate, allocate them and
+ *         set "SDIO initialised" flag (and generate an interrupt) to say so.
+ *
+ * Arguments:
+ *      card        Pointer to card struct
+ *
+ * Returns:
+ *      CSR_RESULT_SUCEESS on success,
+ *      a CSR error code on failure
+ *
+ * Notes:
+ *      All data in the f/w is stored in a little endian format, without any
+ *      padding bytes. Every read from this memory has to be transformed in
+ *      host (cpu specific) format, before it is stored in driver's parameters
+ *      or/and structures. Athough unifi_card_read16() and unifi_read32() do perform
+ *      the convertion internally, unifi_readn() does not.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_hw_init(card_t *card)
+{
+    u32 slut_address;
+    u16 initialised;
+    u16 finger_print;
+    symbol_t slut;
+    sdio_config_data_t *cfg_data;
+    u8 cfg_data_buf[SDIO_CONFIG_DATA_SIZE];
+    CsrResult r;
+    void *dlpriv;
+    s16 major, minor;
+    s16 search_4slut_again;
+    CsrResult csrResult;
+
+    func_enter();
+
+    /*
+     * The device revision from the TPLMID_MANF and TPLMID_CARD fields
+     * of the CIS are available as
+     *   card->sdio_if->pDevice->ManfID
+     *   card->sdio_if->pDevice->AppID
+     */
+
+    /*
+     * Run in a loop so we can patch.
+     */
+    do
+    {
+        /* Reset these each time around the loop. */
+        search_4slut_again = 0;
+        cfg_data = NULL;
+
+        r = card_wait_for_firmware_to_start(card, &slut_address);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Firmware hasn't started\n");
+            func_exit_r(r);
+            return r;
+        }
+        unifi_trace(card->ospriv, UDBG4, "SLUT addr 0x%lX\n", slut_address);
+
+        /*
+         * Firmware has started, but doesn't know full clock configuration yet
+         * as some of the information may be in the MIB. Therefore we set an
+         * initial SDIO clock speed, faster than UNIFI_SDIO_CLOCK_SAFE_HZ, for
+         * the patch download and subsequent firmware initialisation, and
+         * full speed UNIFI_SDIO_CLOCK_MAX_HZ will be set once the f/w tells us
+         * that it is ready.
+         */
+        csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_INIT_HZ);
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            func_exit_r(r);
+            return r;
+        }
+        card->sdio_clock_speed = UNIFI_SDIO_CLOCK_INIT_HZ;
+
+        /*
+         * Check the SLUT fingerprint.
+         * The slut_address is a generic pointer so we must use unifi_card_read16().
+         */
+        unifi_trace(card->ospriv, UDBG4, "Looking for SLUT finger print\n");
+        finger_print = 0;
+        r = unifi_card_read16(card, slut_address, &finger_print);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to read SLUT finger print\n");
+            func_exit_r(r);
+            return r;
+        }
+
+        if (finger_print != SLUT_FINGERPRINT)
+        {
+            unifi_error(card->ospriv, "Failed to find Symbol lookup table fingerprint\n");
+            func_exit_r(CSR_RESULT_FAILURE);
+            return CSR_RESULT_FAILURE;
+        }
+
+        /* Symbol table starts imedately after the fingerprint */
+        slut_address += 2;
+
+        /* Search the table until either the end marker is found, or the
+         * loading of patch firmware invalidates the current table.
+         */
+        while (!search_4slut_again)
+        {
+            u16 s;
+            u32 l;
+
+            r = unifi_card_read16(card, slut_address, &s);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                func_exit_r(r);
+                return r;
+            }
+            slut_address += 2;
+
+            if (s == CSR_SLT_END)
+            {
+                unifi_trace(card->ospriv, UDBG3, "  found CSR_SLT_END\n");
+                break;
+            }
+
+            r = unifi_read32(card, slut_address, &l);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                func_exit_r(r);
+                return r;
+            }
+            slut_address += 4;
+
+            slut.id = s;
+            slut.obj = l;
+
+            unifi_trace(card->ospriv, UDBG3, "  found SLUT id %02d.%08lx\n", slut.id, slut.obj);
+            switch (slut.id)
+            {
+                case CSR_SLT_SDIO_SLOT_CONFIG:
+                    cfg_data = &card->config_data;
+                    /*
+                     * unifi_card_readn reads n bytes from the card, where data is stored
+                     * in a little endian format, without any padding bytes. So, we
+                     * can not just pass the cfg_data pointer or use the
+                     * sizeof(sdio_config_data_t) since the structure in the host can
+                     * be big endian formatted or have padding bytes for alignment.
+                     * We use a char buffer to read the data from the card.
+                     */
+                    r = unifi_card_readn(card, slut.obj, cfg_data_buf, SDIO_CONFIG_DATA_SIZE);
+                    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+                    {
+                        return r;
+                    }
+                    if (r != CSR_RESULT_SUCCESS)
+                    {
+                        unifi_error(card->ospriv, "Failed to read config data\n");
+                        func_exit_r(r);
+                        return r;
+                    }
+                    /* .. and then we copy the data to the host structure */
+                    _build_sdio_config_data(cfg_data, cfg_data_buf);
+
+                    /* Make sure the from host data slots are what we expect
+                        we reserve 2 for commands and there should be at least
+                        1 left for each access category */
+                    if ((cfg_data->num_fromhost_data_slots < UNIFI_RESERVED_COMMAND_SLOTS)
+                        || (cfg_data->num_fromhost_data_slots - UNIFI_RESERVED_COMMAND_SLOTS) / UNIFI_NO_OF_TX_QS == 0)
+                    {
+                        unifi_error(card->ospriv, "From host data slots %d\n", cfg_data->num_fromhost_data_slots);
+                        unifi_error(card->ospriv, "need to be (queues * x + 2) (UNIFI_RESERVED_COMMAND_SLOTS for commands)\n");
+                        func_exit_r(CSR_RESULT_FAILURE);
+                        return CSR_RESULT_FAILURE;
+                    }
+
+                    /* Configure SDIO to-block-size padding */
+                    if (card->sdio_io_block_pad)
+                    {
+                    /*
+                     * Firmware limits the maximum padding size via data_slot_round.
+                     * Therefore when padding to whole block sizes, the block size
+                     * must be configured correctly by adjusting CSR_WIFI_HIP_SDIO_BLOCK_SIZE.
+                     */
+                        if (cfg_data->data_slot_round < card->sdio_io_block_size)
+                        {
+                            unifi_error(card->ospriv,
+                                        "Configuration error: Block size of %d exceeds f/w data_slot_round of %d\n",
+                                        card->sdio_io_block_size, cfg_data->data_slot_round);
+                            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+                        }
+
+                        /*
+                         * To force the To-Host signals to be rounded up to the SDIO block
+                         * size, we need to write the To-Host Signal Padding Fragments
+                         * field of the SDIO configuration in UniFi.
+                         */
+                        if ((card->sdio_io_block_size % cfg_data->sig_frag_size) != 0)
+                        {
+                            unifi_error(card->ospriv, "Configuration error: Can not pad to-host signals.\n");
+                            func_exit_r(CSR_WIFI_HIP_RESULT_INVALID_VALUE);
+                            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+                        }
+                        cfg_data->tohost_signal_padding = (u16) (card->sdio_io_block_size / cfg_data->sig_frag_size);
+                        unifi_info(card->ospriv, "SDIO block size %d requires %d padding chunks\n",
+                                   card->sdio_io_block_size, cfg_data->tohost_signal_padding);
+                        r = unifi_card_write16(card, slut.obj + SDIO_TO_HOST_SIG_PADDING_OFFSET, cfg_data->tohost_signal_padding);
+                        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+                        {
+                            return r;
+                        }
+                        if (r != CSR_RESULT_SUCCESS)
+                        {
+                            unifi_error(card->ospriv, "Failed to write To-Host Signal Padding Fragments\n");
+                            func_exit_r(r);
+                            return r;
+                        }
+                    }
+
+                    /* Reconstruct the Generic Pointer address of the
+                     * SDIO Control Data Struct.
+                     */
+                    card->sdio_ctrl_addr = cfg_data->sdio_ctrl_offset | (UNIFI_SH_DMEM << 24);
+                    card->init_flag_addr = slut.obj + SDIO_INIT_FLAG_OFFSET;
+                    break;
+
+                case CSR_SLT_BUILD_ID_NUMBER:
+                {
+                    u32 n;
+                    r = unifi_read32(card, slut.obj, &n);
+                    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+                    {
+                        return r;
+                    }
+                    if (r != CSR_RESULT_SUCCESS)
+                    {
+                        unifi_error(card->ospriv, "Failed to read build id\n");
+                        func_exit_r(r);
+                        return r;
+                    }
+                    card->build_id = n;
+                }
+                break;
+
+                case CSR_SLT_BUILD_ID_STRING:
+                    r = unifi_readnz(card, slut.obj, card->build_id_string,
+                                     sizeof(card->build_id_string));
+                    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+                    {
+                        return r;
+                    }
+                    if (r != CSR_RESULT_SUCCESS)
+                    {
+                        unifi_error(card->ospriv, "Failed to read build string\n");
+                        func_exit_r(r);
+                        return r;
+                    }
+                    break;
+
+                case CSR_SLT_PERSISTENT_STORE_DB:
+                    break;
+
+                case CSR_SLT_BOOT_LOADER_CONTROL:
+
+                    /* This command copies most of the station firmware
+                     * image from ROM into program RAM.  It also clears
+                     * out the zerod data and sets up the initialised
+                     * data. */
+                    r = unifi_do_loader_op(card, slut.obj + 6, UNIFI_BOOT_LOADER_LOAD_STA);
+                    if (r != CSR_RESULT_SUCCESS)
+                    {
+                        unifi_error(card->ospriv, "Failed to write loader load image command\n");
+                        func_exit_r(r);
+                        return r;
+                    }
+
+                    dlpriv = unifi_dl_fw_read_start(card, UNIFI_FW_STA);
+
+                    /* dlpriv might be NULL, we still need to do the do_loader_op step. */
+                    if (dlpriv != NULL)
+                    {
+                    /* Download the firmware. */
+                        r = unifi_dl_patch(card, dlpriv, slut.obj);
+
+                    /* Free the firmware file information. */
+                        unifi_fw_read_stop(card->ospriv, dlpriv);
+
+                        if (r != CSR_RESULT_SUCCESS)
+                        {
+                            unifi_error(card->ospriv, "Failed to patch firmware\n");
+                            func_exit_r(r);
+                            return r;
+                        }
+                    }
+
+                    /* This command starts the firmware image that we want (the
+                    * station by default) with any patches required applied. */
+                    r = unifi_do_loader_op(card, slut.obj + 6, UNIFI_BOOT_LOADER_RESTART);
+                    if (r != CSR_RESULT_SUCCESS)
+                    {
+                        unifi_error(card->ospriv, "Failed to write loader restart command\n");
+                        func_exit_r(r);
+                        return r;
+                    }
+
+                    /* The now running patch f/w defines a new SLUT data structure -
+                     * the current one is no longer valid. We must drop out of the
+                     * processing loop and enumerate the new SLUT (which may appear
+                     * at a different offset).
+                     */
+                    search_4slut_again = 1;
+                    break;
+
+                case CSR_SLT_PANIC_DATA_PHY:
+                    card->panic_data_phy_addr = slut.obj;
+                    break;
+
+                case CSR_SLT_PANIC_DATA_MAC:
+                    card->panic_data_mac_addr = slut.obj;
+                    break;
+
+                default:
+                    /* do nothing */
+                    break;
+            }
+        } /* while */
+    } while (search_4slut_again);
+
+    /* Did we find the Config Data ? */
+    if (cfg_data == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to find SDIO_SLOT_CONFIG Symbol\n");
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+    }
+
+    /*
+     * Has ths card already been initialised?
+     * If so, return an error so we do a h/w reset and start again.
+     */
+    r = unifi_card_read16(card, card->init_flag_addr, &initialised);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read init flag at %08lx\n",
+                    card->init_flag_addr);
+        func_exit_r(r);
+        return r;
+    }
+    if (initialised != 0)
+    {
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+    }
+
+
+    /*
+     * Now check the UniFi firmware version
+     */
+    major = (cfg_data->version >> 8) & 0xFF;
+    minor = cfg_data->version & 0xFF;
+    unifi_info(card->ospriv, "UniFi f/w protocol version %d.%d (driver %d.%d)\n",
+               major, minor,
+               UNIFI_HIP_MAJOR_VERSION, UNIFI_HIP_MINOR_VERSION);
+
+    unifi_info(card->ospriv, "Firmware build %u: %s\n",
+               card->build_id, card->build_id_string);
+
+    if (major != UNIFI_HIP_MAJOR_VERSION)
+    {
+        unifi_error(card->ospriv, "UniFi f/w protocol major version (%d) is different from driver (v%d.%d)\n",
+                    major, UNIFI_HIP_MAJOR_VERSION, UNIFI_HIP_MINOR_VERSION);
+#ifndef CSR_WIFI_DISABLE_HIP_VERSION_CHECK
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+#endif
+    }
+    if (minor < UNIFI_HIP_MINOR_VERSION)
+    {
+        unifi_error(card->ospriv, "UniFi f/w protocol version (v%d.%d) is older than minimum required by driver (v%d.%d).\n",
+                    major, minor,
+                    UNIFI_HIP_MAJOR_VERSION, UNIFI_HIP_MINOR_VERSION);
+#ifndef CSR_WIFI_DISABLE_HIP_VERSION_CHECK
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+#endif
+    }
+
+    /* Read panic codes from a previous firmware panic. If the firmware has
+     * not panicked since power was applied (e.g. power-off hard reset)
+     * the stored panic codes will not be updated.
+     */
+    unifi_read_panic(card);
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* card_hw_init() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_wait_for_unifi_to_reset
+ *
+ *      Waits for a reset to complete by polling the WLAN function enable
+ *      bit (which is cleared on reset).
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on failure.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_wait_for_unifi_to_reset(card_t *card)
+{
+    s16 i;
+    CsrResult r;
+    u8 io_enable;
+    CsrResult csrResult;
+
+    func_enter();
+
+    r = CSR_RESULT_SUCCESS;
+    for (i = 0; i < MAILBOX2_ATTEMPTS; i++)
+    {
+        unifi_trace(card->ospriv, UDBG1, "waiting for reset to complete, attempt %d\n", i);
+        if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+        {
+            /* It's quite likely that this read will timeout for the
+             * first few tries - especially if we have reset via
+             * DBG_RESET.
+             */
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+            unifi_debug_log_to_buf("m0@%02X=", SDIO_IO_READY);
+#endif
+            csrResult = CsrSdioF0Read8(card->sdio_if, SDIO_IO_READY, &io_enable);
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+            if (csrResult != CSR_RESULT_SUCCESS)
+            {
+                unifi_debug_log_to_buf("error=%X\n", csrResult);
+            }
+            else
+            {
+                unifi_debug_log_to_buf("%X\n", io_enable);
+            }
+#endif
+            if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+            {
+                return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+            }
+            r = CSR_RESULT_SUCCESS;
+            if (csrResult != CSR_RESULT_SUCCESS)
+            {
+                r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            }
+        }
+        else
+        {
+            r = sdio_read_f0(card, SDIO_IO_ENABLE, &io_enable);
+        }
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r == CSR_RESULT_SUCCESS)
+        {
+            u16 mbox2;
+            s16 enabled = io_enable & (1 << card->function);
+
+            if (!enabled)
+            {
+                unifi_trace(card->ospriv, UDBG1,
+                            "Reset complete (function %d is disabled) in ~ %u msecs\n",
+                            card->function, i * MAILBOX2_TIMEOUT);
+
+                /* Enable WLAN function and verify MAILBOX2 is zero'd */
+                csrResult = CsrSdioFunctionEnable(card->sdio_if);
+                if (csrResult != CSR_RESULT_SUCCESS)
+                {
+                    r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+                    unifi_error(card->ospriv, "CsrSdioFunctionEnable failed %d\n", r);
+                    break;
+                }
+            }
+
+            r = unifi_read_direct16(card, ChipHelper_SDIO_HIP_HANDSHAKE(card->helper) * 2, &mbox2);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "read HIP_HANDSHAKE failed %d\n", r);
+                break;
+            }
+            if (mbox2 != 0)
+            {
+                unifi_error(card->ospriv, "MAILBOX2 non-zero after reset (mbox2 = %04x)\n", mbox2);
+                r = CSR_RESULT_FAILURE;
+            }
+            break;
+        }
+        else
+        {
+            if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+            {
+                /* We ignore read failures for the first few reads,
+                 * they are probably benign. */
+                if (i > MAILBOX2_ATTEMPTS / 4)
+                {
+                    unifi_trace(card->ospriv, UDBG1, "Failed to read CCCR IO Ready register while polling for reset\n");
+                }
+            }
+            else
+            {
+                unifi_trace(card->ospriv, UDBG1, "Failed to read CCCR IO Enable register while polling for reset\n");
+            }
+        }
+        CsrThreadSleep(MAILBOX2_TIMEOUT);
+    }
+
+    if (r == CSR_RESULT_SUCCESS && i == MAILBOX2_ATTEMPTS)
+    {
+        unifi_trace(card->ospriv, UDBG1, "Timeout waiting for UniFi to complete reset\n");
+        r = CSR_RESULT_FAILURE;
+    }
+
+    func_exit();
+    return r;
+} /* card_wait_for_unifi_to_reset() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_wait_for_unifi_to_disable
+ *
+ *      Waits for the function to become disabled by polling the
+ *      IO_READY bit.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on failure.
+ *
+ *  Notes: This function can only be used with
+ *         card->chip_id > SDIO_CARD_ID_UNIFI_2
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_wait_for_unifi_to_disable(card_t *card)
+{
+    s16 i;
+    CsrResult r;
+    u8 io_enable;
+    CsrResult csrResult;
+
+    func_enter();
+
+    if (card->chip_id <= SDIO_CARD_ID_UNIFI_2)
+    {
+        unifi_error(card->ospriv,
+                    "Function reset method not supported for chip_id=%d\n",
+                    card->chip_id);
+        func_exit();
+        return CSR_RESULT_FAILURE;
+    }
+
+    r = CSR_RESULT_SUCCESS;
+    for (i = 0; i < MAILBOX2_ATTEMPTS; i++)
+    {
+        unifi_trace(card->ospriv, UDBG1, "waiting for disable to complete, attempt %d\n", i);
+
+        /*
+         * It's quite likely that this read will timeout for the
+         * first few tries - especially if we have reset via
+         * DBG_RESET.
+         */
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        unifi_debug_log_to_buf("r0@%02X=", SDIO_IO_READY);
+#endif
+        csrResult = CsrSdioF0Read8(card->sdio_if, SDIO_IO_READY, &io_enable);
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            unifi_debug_log_to_buf("error=%X\n", csrResult);
+        }
+        else
+        {
+            unifi_debug_log_to_buf("%X\n", io_enable);
+        }
+#endif
+        if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        }
+        if (csrResult == CSR_RESULT_SUCCESS)
+        {
+            s16 enabled = io_enable & (1 << card->function);
+            r = CSR_RESULT_SUCCESS;
+            if (!enabled)
+            {
+                unifi_trace(card->ospriv, UDBG1,
+                            "Disable complete (function %d is disabled) in ~ %u msecs\n",
+                            card->function, i * MAILBOX2_TIMEOUT);
+
+                break;
+            }
+        }
+        else
+        {
+            /*
+             * We ignore read failures for the first few reads,
+             * they are probably benign.
+             */
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            if (i > (MAILBOX2_ATTEMPTS / 4))
+            {
+                unifi_trace(card->ospriv, UDBG1,
+                            "Failed to read CCCR IO Ready register while polling for disable\n");
+            }
+        }
+        CsrThreadSleep(MAILBOX2_TIMEOUT);
+    }
+
+    if ((r == CSR_RESULT_SUCCESS) && (i == MAILBOX2_ATTEMPTS))
+    {
+        unifi_trace(card->ospriv, UDBG1, "Timeout waiting for UniFi to complete disable\n");
+        r = CSR_RESULT_FAILURE;
+    }
+
+    func_exit();
+    return r;
+} /* card_wait_for_unifi_to_reset() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_wait_for_firmware_to_start
+ *
+ *      Polls the MAILBOX1 register for a non-zero value.
+ *      Then reads MAILBOX0 and forms the two values into a 32-bit address
+ *      which is returned to the caller.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      paddr           Pointer to receive the UniFi address formed
+ *                      by concatenating MAILBOX1 and MAILBOX0.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on failure.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult card_wait_for_firmware_to_start(card_t *card, u32 *paddr)
+{
+    s32 i;
+    u16 mbox0, mbox1;
+    CsrResult r;
+
+    func_enter();
+
+    /*
+     * Wait for UniFi to initialise its data structures by polling
+     * the SHARED_MAILBOX1 register.
+     * Experience shows this is typically 120ms.
+     */
+    CsrThreadSleep(MAILBOX1_TIMEOUT);
+
+    mbox1 = 0;
+    unifi_trace(card->ospriv, UDBG1, "waiting for MAILBOX1 to be non-zero...\n");
+    for (i = 0; i < MAILBOX1_ATTEMPTS; i++)
+    {
+        r = unifi_read_direct16(card, ChipHelper_MAILBOX1(card->helper) * 2, &mbox1);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            /* These reads can fail if UniFi isn't up yet, so try again */
+            unifi_warning(card->ospriv, "Failed to read UniFi Mailbox1 register\n");
+        }
+
+        if ((r == CSR_RESULT_SUCCESS) && (mbox1 != 0))
+        {
+            unifi_trace(card->ospriv, UDBG1, "MAILBOX1 ready (0x%04X) in %u millisecs\n",
+                        mbox1, i * MAILBOX1_TIMEOUT);
+
+            /* Read the MAILBOX1 again in case we caught the value as it
+             * changed. */
+            r = unifi_read_direct16(card, ChipHelper_MAILBOX1(card->helper) * 2, &mbox1);
+            if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                return r;
+            }
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "Failed to read UniFi Mailbox1 register for second time\n");
+                func_exit_r(r);
+                return r;
+            }
+            unifi_trace(card->ospriv, UDBG1, "MAILBOX1 value=0x%04X\n", mbox1);
+
+            break;
+        }
+
+        CsrThreadSleep(MAILBOX1_TIMEOUT);
+        if ((i % 100) == 99)
+        {
+            unifi_trace(card->ospriv, UDBG2, "MAILBOX1 not ready (0x%X), still trying...\n", mbox1);
+        }
+    }
+
+    if ((r == CSR_RESULT_SUCCESS) && (mbox1 == 0))
+    {
+        unifi_trace(card->ospriv, UDBG1, "Timeout waiting for firmware to start, Mailbox1 still 0 after %d ms\n",
+                    MAILBOX1_ATTEMPTS * MAILBOX1_TIMEOUT);
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+    }
+
+
+    /*
+     * Complete the reset handshake by setting MAILBOX2 to 0xFFFF
+     */
+    r = unifi_write_direct16(card, ChipHelper_SDIO_HIP_HANDSHAKE(card->helper) * 2, 0xFFFF);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write f/w startup handshake to MAILBOX2\n");
+        func_exit_r(r);
+        return r;
+    }
+
+
+    /*
+     * Read the Symbol Look Up Table (SLUT) offset.
+     * Top 16 bits are in mbox1, read the lower 16 bits from mbox0.
+     */
+    mbox0 = 0;
+    r = unifi_read_direct16(card, ChipHelper_MAILBOX0(card->helper) * 2, &mbox0);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read UniFi Mailbox0 register\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    *paddr = (((u32)mbox1 << 16) | mbox0);
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* card_wait_for_firmware_to_start() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_capture_panic
+ *
+ *      Attempt to capture panic codes from the firmware. This may involve
+ *      warm reset of the chip to regain access following a watchdog reset.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS if panic codes were captured, or none available
+ *      CSR_RESULT_FAILURE if the driver could not access function 1
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_capture_panic(card_t *card)
+{
+    func_enter();
+
+    /* The firmware must have previously initialised to read the panic addresses
+     * from the SLUT
+     */
+    if (!card->panic_data_phy_addr || !card->panic_data_mac_addr)
+    {
+        func_exit();
+        return CSR_RESULT_SUCCESS;
+    }
+
+    /* Ensure we can access function 1 following a panic/watchdog reset */
+    if (card_access_panic(card) == CSR_RESULT_SUCCESS)
+    {
+        /* Read the panic codes */
+        unifi_read_panic(card);
+    }
+    else
+    {
+        unifi_info(card->ospriv, "Unable to read panic codes");
+    }
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_access_panic
+ *      Attempt to read the WLAN SDIO function in order to read panic codes
+ *      and perform various reset steps to regain access if the read fails.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS if panic codes can be read
+ *      CSR error code if panic codes can not be read
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_access_panic(card_t *card)
+{
+    u16 data_u16 = 0;
+    s32 i;
+    CsrResult r, sr;
+
+    func_enter();
+
+    /* A chip version of zero means that the version never got succesfully read
+     * during reset. In this case give up because it will not be possible to
+     * verify the chip version.
+     */
+    if (!card->chip_version)
+    {
+        unifi_info(card->ospriv, "Unknown chip version\n");
+        return CSR_RESULT_FAILURE;
+    }
+
+    /* Ensure chip is awake or access to function 1 will fail */
+    r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "unifi_set_host_state() failed %d\n", r);
+        return CSR_RESULT_FAILURE; /* Card is probably unpowered */
+    }
+    CsrThreadSleep(20);
+
+    for (i = 0; i < 3; i++)
+    {
+        sr = CsrSdioRead16(card->sdio_if, CHIP_HELPER_UNIFI_GBL_CHIP_VERSION * 2, &data_u16);
+        if (sr != CSR_RESULT_SUCCESS || data_u16 != card->chip_version)
+        {
+            unifi_info(card->ospriv, "Failed to read valid chip version sr=%d (0x%04x want 0x%04x) try %d\n",
+                       sr, data_u16, card->chip_version, i);
+
+            /* Set clock speed low */
+            sr = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_SAFE_HZ);
+            if (sr != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "CsrSdioMaxBusClockFrequencySet() failed1 %d\n", sr);
+                r = ConvertCsrSdioToCsrHipResult(card, sr);
+            }
+            card->sdio_clock_speed = UNIFI_SDIO_CLOCK_SAFE_HZ;
+
+            /* First try re-enabling function in case a f/w watchdog reset disabled it */
+            if (i == 0)
+            {
+                unifi_info(card->ospriv, "Try function enable\n");
+                sr = CsrSdioFunctionEnable(card->sdio_if);
+                if (sr != CSR_RESULT_SUCCESS)
+                {
+                    r = ConvertCsrSdioToCsrHipResult(card, sr);
+                    unifi_error(card->ospriv, "CsrSdioFunctionEnable failed %d (HIP %d)\n", sr, r);
+                }
+                continue;
+            }
+
+            /* Second try, set awake */
+            unifi_info(card->ospriv, "Try set awake\n");
+
+            /* Ensure chip is awake */
+            r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+            if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                return r;
+            }
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "unifi_set_host_state() failed2 %d\n", r);
+            }
+
+            /* Set clock speed low in case setting the host state raised it, which
+             * would only happen if host state was previously TORPID
+             */
+            sr = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_SAFE_HZ);
+            if (sr != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "CsrSdioMaxBusClockFrequencySet() failed2 %d\n", sr);
+            }
+            card->sdio_clock_speed = UNIFI_SDIO_CLOCK_SAFE_HZ;
+
+            if (i == 1)
+            {
+                continue;
+            }
+
+            /* Perform a s/w reset to preserve as much as the card state as possible,
+             * (mainly the preserve RAM). The context will be lost for coredump - but as we
+             * were unable to access the WLAN function for panic, the coredump would have
+             * also failed without a reset.
+             */
+            unifi_info(card->ospriv, "Try s/w reset\n");
+
+            r = unifi_card_hard_reset(card);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "unifi_card_hard_reset() failed %d\n", r);
+            }
+        }
+        else
+        {
+            if (i > 0)
+            {
+                unifi_info(card->ospriv, "Read chip version 0x%x after %d retries\n", data_u16, i);
+            }
+            break;
+        }
+    }
+
+    r = ConvertCsrSdioToCsrHipResult(card, sr);
+    func_exit_r(r);
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read_panic
+ *      Reads, saves and prints panic codes stored by the firmware in UniFi's
+ *      preserve RAM by the last panic that occurred since chip was powered.
+ *      Nothing is saved if the panic codes are read as zero.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ * ---------------------------------------------------------------------------
+ */
+void unifi_read_panic(card_t *card)
+{
+    CsrResult r;
+    u16 p_code, p_arg;
+
+    func_enter();
+
+    /* The firmware must have previously initialised to read the panic addresses
+     * from the SLUT
+     */
+    if (!card->panic_data_phy_addr || !card->panic_data_mac_addr)
+    {
+        return;
+    }
+
+    /* Get the panic data from PHY */
+    r = unifi_card_read16(card, card->panic_data_phy_addr, &p_code);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "capture_panic: unifi_read16 %08x failed %d\n", card->panic_data_phy_addr, r);
+        p_code = 0;
+    }
+    if (p_code)
+    {
+        r = unifi_card_read16(card, card->panic_data_phy_addr + 2, &p_arg);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "capture_panic: unifi_read16 %08x failed %d\n", card->panic_data_phy_addr + 2, r);
+        }
+        unifi_error(card->ospriv, "Last UniFi PHY PANIC %04x arg %04x\n", p_code, p_arg);
+        card->last_phy_panic_code = p_code;
+        card->last_phy_panic_arg = p_arg;
+    }
+
+    /* Get the panic data from MAC */
+    r = unifi_card_read16(card, card->panic_data_mac_addr, &p_code);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "capture_panic: unifi_read16 %08x failed %d\n", card->panic_data_mac_addr, r);
+        p_code = 0;
+    }
+    if (p_code)
+    {
+        r = unifi_card_read16(card, card->panic_data_mac_addr + 2, &p_arg);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "capture_panic: unifi_read16 %08x failed %d\n", card->panic_data_mac_addr + 2, r);
+        }
+        unifi_error(card->ospriv, "Last UniFi MAC PANIC %04x arg %04x\n", p_code, p_arg);
+        card->last_mac_panic_code = p_code;
+        card->last_mac_panic_arg = p_arg;
+    }
+
+    func_exit();
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_allocate_memory_resources
+ *
+ *      Allocates memory for the from-host, to-host bulk data slots,
+ *      soft queue buffers and bulk data buffers.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on failure.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_allocate_memory_resources(card_t *card)
+{
+    s16 n, i, k, r;
+    sdio_config_data_t *cfg_data;
+
+    func_enter();
+
+    /* Reset any state carried forward from a previous life */
+    card->fh_command_queue.q_rd_ptr = 0;
+    card->fh_command_queue.q_wr_ptr = 0;
+    (void)CsrSnprintf(card->fh_command_queue.name, UNIFI_QUEUE_NAME_MAX_LENGTH,
+                      "fh_cmd_q");
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        card->fh_traffic_queue[i].q_rd_ptr = 0;
+        card->fh_traffic_queue[i].q_wr_ptr = 0;
+        (void)CsrSnprintf(card->fh_traffic_queue[i].name,
+                          UNIFI_QUEUE_NAME_MAX_LENGTH, "fh_data_q%d", i);
+    }
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+    unifi_ta_sampling_init(card);
+#endif
+    /* Convenience short-cut */
+    cfg_data = &card->config_data;
+
+    /*
+     * Allocate memory for the from-host and to-host signal buffers.
+     */
+    card->fh_buffer.buf = kmalloc(UNIFI_FH_BUF_SIZE, GFP_KERNEL);
+    if (card->fh_buffer.buf == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for F-H signals\n");
+        func_exit_r(CSR_WIFI_HIP_RESULT_NO_MEMORY);
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+    card->fh_buffer.bufsize = UNIFI_FH_BUF_SIZE;
+    card->fh_buffer.ptr = card->fh_buffer.buf;
+    card->fh_buffer.count = 0;
+
+    card->th_buffer.buf = kmalloc(UNIFI_FH_BUF_SIZE, GFP_KERNEL);
+    if (card->th_buffer.buf == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for T-H signals\n");
+        func_exit_r(CSR_WIFI_HIP_RESULT_NO_MEMORY);
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+    card->th_buffer.bufsize = UNIFI_FH_BUF_SIZE;
+    card->th_buffer.ptr = card->th_buffer.buf;
+    card->th_buffer.count = 0;
+
+
+    /*
+     * Allocate memory for the from-host and to-host bulk data slots.
+     * This is done as separate kmallocs because lots of smaller
+     * allocations are more likely to succeed than one huge one.
+     */
+
+    /* Allocate memory for the array of pointers */
+    n = cfg_data->num_fromhost_data_slots;
+
+    unifi_trace(card->ospriv, UDBG3, "Alloc from-host resources, %d slots.\n", n);
+    card->from_host_data = kmalloc(n * sizeof(slot_desc_t), GFP_KERNEL);
+    if (card->from_host_data == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for F-H bulk data array\n");
+        func_exit_r(CSR_WIFI_HIP_RESULT_NO_MEMORY);
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+
+    /* Initialise from-host bulk data slots */
+    for (i = 0; i < n; i++)
+    {
+        UNIFI_INIT_BULK_DATA(&card->from_host_data[i].bd);
+    }
+
+    /* Allocate memory for the array used for slot host tag mapping */
+    card->fh_slot_host_tag_record = kmalloc(n * sizeof(u32), GFP_KERNEL);
+
+    if (card->fh_slot_host_tag_record == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for F-H slot host tag mapping array\n");
+        func_exit_r(CSR_WIFI_HIP_RESULT_NO_MEMORY);
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+
+    /* Initialise host tag entries for from-host bulk data slots */
+    for (i = 0; i < n; i++)
+    {
+        card->fh_slot_host_tag_record[i] = CSR_WIFI_HIP_RESERVED_HOST_TAG;
+    }
+
+
+    /* Allocate memory for the array of pointers */
+    n = cfg_data->num_tohost_data_slots;
+
+    unifi_trace(card->ospriv, UDBG3, "Alloc to-host resources, %d slots.\n", n);
+    card->to_host_data = kmalloc(n * sizeof(bulk_data_desc_t), GFP_KERNEL);
+    if (card->to_host_data == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for T-H bulk data array\n");
+        func_exit_r(CSR_WIFI_HIP_RESULT_NO_MEMORY);
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+
+    /* Initialise to-host bulk data slots */
+    for (i = 0; i < n; i++)
+    {
+        UNIFI_INIT_BULK_DATA(&card->to_host_data[i]);
+    }
+
+    /*
+     * Initialise buffers for soft Q
+     */
+    for (i = 0; i < UNIFI_SOFT_COMMAND_Q_LENGTH; i++)
+    {
+        for (r = 0; r < UNIFI_MAX_DATA_REFERENCES; r++)
+        {
+            UNIFI_INIT_BULK_DATA(&card->fh_command_q_body[i].bulkdata[r]);
+        }
+    }
+
+    for (k = 0; k < UNIFI_NO_OF_TX_QS; k++)
+    {
+        for (i = 0; i < UNIFI_SOFT_TRAFFIC_Q_LENGTH; i++)
+        {
+            for (r = 0; r < UNIFI_MAX_DATA_REFERENCES; r++)
+            {
+                UNIFI_INIT_BULK_DATA(&card->fh_traffic_q_body[k][i].bulkdata[r]);
+            }
+        }
+    }
+
+    card->memory_resources_allocated = 1;
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* card_allocate_memory_resources() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_free_bulk_data
+ *
+ *      Free the data associated to a bulk data structure.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      bulk_data_slot  Pointer to bulk data structure
+ *
+ *  Returns:
+ *      None.
+ *
+ * ---------------------------------------------------------------------------
+ */
+static void unifi_free_bulk_data(card_t *card, bulk_data_desc_t *bulk_data_slot)
+{
+    if (bulk_data_slot->data_length != 0)
+    {
+        unifi_net_data_free(card->ospriv, bulk_data_slot);
+    }
+} /* unifi_free_bulk_data() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_free_memory_resources
+ *
+ *      Frees memory allocated for the from-host, to-host bulk data slots,
+ *      soft queue buffers and bulk data buffers.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void card_free_memory_resources(card_t *card)
+{
+    func_enter();
+
+    unifi_trace(card->ospriv, UDBG1, "Freeing card memory resources.\n");
+
+    /* Clear our internal queues */
+    unifi_cancel_pending_signals(card);
+
+
+    kfree(card->to_host_data);
+    card->to_host_data = NULL;
+
+    kfree(card->from_host_data);
+    card->from_host_data = NULL;
+
+    /* free the memory for slot host tag mapping array */
+    kfree(card->fh_slot_host_tag_record);
+    card->fh_slot_host_tag_record = NULL;
+
+    kfree(card->fh_buffer.buf);
+    card->fh_buffer.ptr = card->fh_buffer.buf = NULL;
+    card->fh_buffer.bufsize = 0;
+    card->fh_buffer.count = 0;
+
+    kfree(card->th_buffer.buf);
+    card->th_buffer.ptr = card->th_buffer.buf = NULL;
+    card->th_buffer.bufsize = 0;
+    card->th_buffer.count = 0;
+
+
+    card->memory_resources_allocated = 0;
+
+    func_exit();
+} /* card_free_memory_resources() */
+
+
+static void card_init_soft_queues(card_t *card)
+{
+    s16 i;
+
+    func_enter();
+
+    unifi_trace(card->ospriv, UDBG1, "Initialising internal signal queues.\n");
+    /* Reset any state carried forward from a previous life */
+    card->fh_command_queue.q_rd_ptr = 0;
+    card->fh_command_queue.q_wr_ptr = 0;
+    (void)CsrSnprintf(card->fh_command_queue.name, UNIFI_QUEUE_NAME_MAX_LENGTH,
+                      "fh_cmd_q");
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        card->fh_traffic_queue[i].q_rd_ptr = 0;
+        card->fh_traffic_queue[i].q_wr_ptr = 0;
+        (void)CsrSnprintf(card->fh_traffic_queue[i].name,
+                          UNIFI_QUEUE_NAME_MAX_LENGTH, "fh_data_q%d", i);
+    }
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+    unifi_ta_sampling_init(card);
+#endif
+    func_exit();
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_cancel_pending_signals
+ *
+ *      Free the signals and associated bulk data, pending in the core.
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void unifi_cancel_pending_signals(card_t *card)
+{
+    s16 i, n, r;
+    func_enter();
+
+    unifi_trace(card->ospriv, UDBG1, "Canceling pending signals.\n");
+
+    if (card->to_host_data)
+    {
+        /*
+         * Free any bulk data buffers allocated for the t-h slots
+         * This will clear all buffers that did not make it to
+         * unifi_receive_event() before cancel was request.
+         */
+        n = card->config_data.num_tohost_data_slots;
+        unifi_trace(card->ospriv, UDBG3, "Freeing to-host resources, %d slots.\n", n);
+        for (i = 0; i < n; i++)
+        {
+            unifi_free_bulk_data(card, &card->to_host_data[i]);
+        }
+    }
+
+    /*
+     * If any of the from-host bulk data has reached the card->from_host_data
+     * but not UniFi, we need to free the buffers here.
+     */
+    if (card->from_host_data)
+    {
+        /* Free any bulk data buffers allocated for the f-h slots */
+        n = card->config_data.num_fromhost_data_slots;
+        unifi_trace(card->ospriv, UDBG3, "Freeing from-host resources, %d slots.\n", n);
+        for (i = 0; i < n; i++)
+        {
+            unifi_free_bulk_data(card, &card->from_host_data[i].bd);
+        }
+
+        for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+        {
+            card->dynamic_slot_data.from_host_used_slots[i] = 0;
+            card->dynamic_slot_data.from_host_max_slots[i] = 0;
+            card->dynamic_slot_data.from_host_reserved_slots[i] = 0;
+        }
+    }
+
+    /*
+     * Free any bulk data buffers allocated in the soft queues.
+     * This covers the case where a bulk data pointer has reached the soft queue
+     * but not the card->from_host_data.
+     */
+    unifi_trace(card->ospriv, UDBG3, "Freeing cmd q resources.\n");
+    for (i = 0; i < UNIFI_SOFT_COMMAND_Q_LENGTH; i++)
+    {
+        for (r = 0; r < UNIFI_MAX_DATA_REFERENCES; r++)
+        {
+            unifi_free_bulk_data(card, &card->fh_command_q_body[i].bulkdata[r]);
+        }
+    }
+
+    unifi_trace(card->ospriv, UDBG3, "Freeing traffic q resources.\n");
+    for (n = 0; n < UNIFI_NO_OF_TX_QS; n++)
+    {
+        for (i = 0; i < UNIFI_SOFT_TRAFFIC_Q_LENGTH; i++)
+        {
+            for (r = 0; r < UNIFI_MAX_DATA_REFERENCES; r++)
+            {
+                unifi_free_bulk_data(card, &card->fh_traffic_q_body[n][i].bulkdata[r]);
+            }
+        }
+    }
+
+    card_init_soft_queues(card);
+
+    func_exit();
+} /* unifi_cancel_pending_signals() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_free_card
+ *
+ *      Free the memory allocated for the card structure and buffers.
+ *
+ *  Notes:
+ *      The porting layer is responsible for freeing any mini-coredump buffers
+ *      allocated when it called unifi_coredump_init(), by calling
+ *      unifi_coredump_free() before calling this function.
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void unifi_free_card(card_t *card)
+{
+    func_enter();
+#ifdef CSR_PRE_ALLOC_NET_DATA
+    prealloc_netdata_free(card);
+#endif
+    /* Free any memory allocated. */
+    card_free_memory_resources(card);
+
+    /* Warn if caller didn't free coredump buffers */
+    if (card->dump_buf)
+    {
+        unifi_error(card->ospriv, "Caller should call unifi_coredump_free()\n");
+        unifi_coredump_free(card); /* free anyway to prevent memory leak */
+    }
+
+    kfree(card);
+
+    func_exit();
+} /* unifi_free_card() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_init_slots
+ *
+ *      Allocate memory for host-side slot data and signal queues.
+ *
+ * Arguments:
+ *      card            Pointer to card object
+ *
+ * Returns:
+ *      CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_init_slots(card_t *card)
+{
+    CsrResult r;
+    u8 i;
+
+    func_enter();
+
+    /* Allocate the buffers we need, only once. */
+    if (card->memory_resources_allocated == 1)
+    {
+        card_free_memory_resources(card);
+    }
+    else
+    {
+        /* Initialise our internal command and traffic queues */
+        card_init_soft_queues(card);
+    }
+
+    r = card_allocate_memory_resources(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to allocate card memory resources.\n");
+        card_free_memory_resources(card);
+        func_exit_r(r);
+        return r;
+    }
+
+    if (card->sdio_ctrl_addr == 0)
+    {
+        unifi_error(card->ospriv, "Failed to find config struct!\n");
+        func_exit_r(CSR_WIFI_HIP_RESULT_INVALID_VALUE);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    /*
+     * Set initial counts.
+     */
+
+    card->from_host_data_head = 0;
+
+    /* Get initial signal counts from UniFi, in case it has not been reset. */
+    {
+        u16 s;
+
+        /* Get the from-host-signals-written count */
+        r = unifi_card_read16(card, card->sdio_ctrl_addr + 0, &s);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to read from-host sig written count\n");
+            func_exit_r(r);
+            return r;
+        }
+        card->from_host_signals_w = (s16)s;
+
+        /* Get the to-host-signals-written count */
+        r = unifi_card_read16(card, card->sdio_ctrl_addr + 6, &s);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to read to-host sig read count\n");
+            func_exit_r(r);
+            return r;
+        }
+        card->to_host_signals_r = (s16)s;
+    }
+
+    /* Set Initialised flag. */
+    r = unifi_card_write16(card, card->init_flag_addr, 0x0001);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write initialised flag\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    /* Dynamic queue reservation */
+    memset(&card->dynamic_slot_data, 0, sizeof(card_dynamic_slot_t));
+
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        card->dynamic_slot_data.from_host_max_slots[i] = card->config_data.num_fromhost_data_slots -
+                                                         UNIFI_RESERVED_COMMAND_SLOTS;
+        card->dynamic_slot_data.queue_stable[i] = FALSE;
+    }
+
+    card->dynamic_slot_data.packets_interval = UNIFI_PACKETS_INTERVAL;
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* card_init_slots() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_set_udi_hook
+ *
+ *      Registers the udi hook that reports the sent signals to the core.
+ *
+ *  Arguments:
+ *      card            Pointer to the card context struct
+ *      udi_fn          Pointer to the callback function.
+ *
+ *  Returns:
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE if the card pointer is invalid,
+ *      CSR_RESULT_SUCCESS on success.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_set_udi_hook(card_t *card, udi_func_t udi_fn)
+{
+    if (card == NULL)
+    {
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    if (card->udi_hook == NULL)
+    {
+        card->udi_hook = udi_fn;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_set_udi_hook() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_remove_udi_hook
+ *
+ *      Removes the udi hook that reports the sent signals from the core.
+ *
+ *  Arguments:
+ *      card            Pointer to the card context struct
+ *      udi_fn          Pointer to the callback function.
+ *
+ *  Returns:
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE if the card pointer is invalid,
+ *      CSR_RESULT_SUCCESS on success.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_remove_udi_hook(card_t *card, udi_func_t udi_fn)
+{
+    if (card == NULL)
+    {
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    if (card->udi_hook == udi_fn)
+    {
+        card->udi_hook = NULL;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_remove_udi_hook() */
+
+
+static void CardReassignDynamicReservation(card_t *card)
+{
+    u8 i;
+
+    func_enter();
+
+    unifi_trace(card->ospriv, UDBG5, "Packets Txed %d %d %d %d\n",
+                card->dynamic_slot_data.packets_txed[0],
+                card->dynamic_slot_data.packets_txed[1],
+                card->dynamic_slot_data.packets_txed[2],
+                card->dynamic_slot_data.packets_txed[3]);
+
+    /* Clear reservation and recalculate max slots */
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        card->dynamic_slot_data.queue_stable[i] = FALSE;
+        card->dynamic_slot_data.from_host_reserved_slots[i] = 0;
+        card->dynamic_slot_data.from_host_max_slots[i] = card->config_data.num_fromhost_data_slots -
+                                                         UNIFI_RESERVED_COMMAND_SLOTS;
+        card->dynamic_slot_data.packets_txed[i] = 0;
+
+        unifi_trace(card->ospriv, UDBG5, "CardReassignDynamicReservation: queue %d reserved %d Max %d\n", i,
+                    card->dynamic_slot_data.from_host_reserved_slots[i],
+                    card->dynamic_slot_data.from_host_max_slots[i]);
+    }
+
+    card->dynamic_slot_data.total_packets_txed = 0;
+    func_exit();
+}
+
+
+/* Algorithm to dynamically reserve slots. The logic is based mainly on the outstanding queue
+ * length. Slots are reserved for particular queues during an interval and cleared after the interval.
+ * Each queue has three associated variables.. a) used slots - the number of slots currently occupied
+ * by the queue b) reserved slots - number of slots reserved specifically for the queue c) max slots - total
+ * slots that this queue can actually use (may be higher than reserved slots and is dependent on reserved slots
+ * for other queues).
+ * This function is called when there are no slots available for a queue. It checks to see if there are enough
+ * unreserved slots sufficient for this request. If available these slots are reserved for the queue.
+ * If there are not enough unreserved slots, a fair share for each queue is calculated based on the total slots
+ * and the number of active queues (any queue with existing reservation is considered active). Queues needing
+ * less than their fair share are allowed to have the previously reserved slots. The remaining slots are
+ * distributed evenly among queues that need more than the fair share
+ *
+ * A better scheme would take current bandwidth per AC into consideration when reserving slots. An
+ * implementation scheme could consider the relative time/service period for slots in an AC. If the firmware
+ * services other ACs faster than a particular AC (packets wait in the slots longer) then it is fair to reserve
+ * less slots for the AC
+ */
+static void CardCheckDynamicReservation(card_t *card, unifi_TrafficQueue queue)
+{
+    u16 q_len, active_queues = 0, excess_queue_slots, div_extra_slots,
+              queue_fair_share, reserved_slots = 0, q, excess_need_queues = 0, unmovable_slots = 0;
+    s32 i;
+    q_t *sigq;
+    u16 num_data_slots = card->config_data.num_fromhost_data_slots - UNIFI_RESERVED_COMMAND_SLOTS;
+
+    func_enter();
+
+    /* Calculate the pending queue length */
+    sigq = &card->fh_traffic_queue[queue];
+    q_len = CSR_WIFI_HIP_Q_SLOTS_USED(sigq);
+
+    if (q_len <= card->dynamic_slot_data.from_host_reserved_slots[queue])
+    {
+        unifi_trace(card->ospriv, UDBG5, "queue %d q_len %d already has that many reserved slots, exiting\n", queue, q_len);
+        func_exit();
+        return;
+    }
+
+    /* Upper limit */
+    if (q_len > num_data_slots)
+    {
+        q_len = num_data_slots;
+    }
+
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        if (i != (s32)queue)
+        {
+            reserved_slots += card->dynamic_slot_data.from_host_reserved_slots[i];
+        }
+        if ((i == (s32)queue) || (card->dynamic_slot_data.from_host_reserved_slots[i] > 0))
+        {
+            active_queues++;
+        }
+    }
+
+    unifi_trace(card->ospriv, UDBG5, "CardCheckDynamicReservation: queue %d q_len %d\n", queue, q_len);
+    unifi_trace(card->ospriv, UDBG5, "Active queues %d reserved slots on other queues %d\n",
+                active_queues, reserved_slots);
+
+    if (reserved_slots + q_len <= num_data_slots)
+    {
+        card->dynamic_slot_data.from_host_reserved_slots[queue] = q_len;
+        if (q_len == num_data_slots)
+        {
+            /* This is the common case when just 1 stream is going */
+            card->dynamic_slot_data.queue_stable[queue] = TRUE;
+        }
+    }
+    else
+    {
+        queue_fair_share = num_data_slots / active_queues;
+        unifi_trace(card->ospriv, UDBG5, "queue fair share %d\n", queue_fair_share);
+
+        /* Evenly distribute slots among active queues */
+        /* Find out the queues that need excess of fair share. Also find slots allocated
+         * to queues less than their fair share, these slots cannot be reallocated (unmovable slots) */
+
+        card->dynamic_slot_data.from_host_reserved_slots[queue] = q_len;
+
+        for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+        {
+            if (card->dynamic_slot_data.from_host_reserved_slots[i] > queue_fair_share)
+            {
+                excess_need_queues++;
+            }
+            else
+            {
+                unmovable_slots += card->dynamic_slot_data.from_host_reserved_slots[i];
+            }
+        }
+
+        unifi_trace(card->ospriv, UDBG5, "Excess need queues %d\n", excess_need_queues);
+
+        /* Now find the slots per excess demand queue */
+        excess_queue_slots = (num_data_slots - unmovable_slots) / excess_need_queues;
+        div_extra_slots = (num_data_slots - unmovable_slots) - excess_queue_slots * excess_need_queues;
+        for (i = UNIFI_NO_OF_TX_QS - 1; i >= 0; i--)
+        {
+            if (card->dynamic_slot_data.from_host_reserved_slots[i] > excess_queue_slots)
+            {
+                card->dynamic_slot_data.from_host_reserved_slots[i] = excess_queue_slots;
+                if (div_extra_slots > 0)
+                {
+                    card->dynamic_slot_data.from_host_reserved_slots[i]++;
+                    div_extra_slots--;
+                }
+                /* No more slots will be allocated to this queue during the current interval */
+                card->dynamic_slot_data.queue_stable[i] = TRUE;
+                unifi_trace(card->ospriv, UDBG5, "queue stable %d\n", i);
+            }
+        }
+    }
+
+    /* Redistribute max slots */
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        reserved_slots = 0;
+        for (q = 0; q < UNIFI_NO_OF_TX_QS; q++)
+        {
+            if (i != q)
+            {
+                reserved_slots += card->dynamic_slot_data.from_host_reserved_slots[q];
+            }
+        }
+
+        card->dynamic_slot_data.from_host_max_slots[i] = num_data_slots - reserved_slots;
+        unifi_trace(card->ospriv, UDBG5, "queue %d reserved %d Max %d\n", i,
+                    card->dynamic_slot_data.from_host_reserved_slots[i],
+                    card->dynamic_slot_data.from_host_max_slots[i]);
+    }
+
+    func_exit();
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardClearFromHostDataSlot
+ *
+ *      Clear a the given data slot, making it available again.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *      slot            Index of the signal slot to clear.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void CardClearFromHostDataSlot(card_t *card, const s16 slot)
+{
+    u8 queue = card->from_host_data[slot].queue;
+    const void *os_data_ptr = card->from_host_data[slot].bd.os_data_ptr;
+
+    func_enter();
+
+    if (card->from_host_data[slot].bd.data_length == 0)
+    {
+        unifi_warning(card->ospriv,
+                      "Surprise: request to clear an already free FH data slot: %d\n",
+                      slot);
+        func_exit();
+        return;
+    }
+
+    if (os_data_ptr == NULL)
+    {
+        unifi_warning(card->ospriv,
+                      "Clearing FH data slot %d: has null payload, len=%d\n",
+                      slot, card->from_host_data[slot].bd.data_length);
+    }
+
+    /* Free card->from_host_data[slot].bd.os_net_ptr here. */
+    /* Mark slot as free by setting length to 0. */
+    unifi_free_bulk_data(card, &card->from_host_data[slot].bd);
+    if (queue < UNIFI_NO_OF_TX_QS)
+    {
+        if (card->dynamic_slot_data.from_host_used_slots[queue] == 0)
+        {
+            unifi_error(card->ospriv, "Goofed up used slots q = %d used slots = %d\n",
+                        queue,
+                        card->dynamic_slot_data.from_host_used_slots[queue]);
+        }
+        else
+        {
+            card->dynamic_slot_data.from_host_used_slots[queue]--;
+        }
+        card->dynamic_slot_data.packets_txed[queue]++;
+        card->dynamic_slot_data.total_packets_txed++;
+        if (card->dynamic_slot_data.total_packets_txed >= card->dynamic_slot_data.packets_interval)
+        {
+            CardReassignDynamicReservation(card);
+        }
+    }
+
+    unifi_trace(card->ospriv, UDBG4, "CardClearFromHostDataSlot: slot %d recycled %p\n", slot, os_data_ptr);
+
+    func_exit();
+} /* CardClearFromHostDataSlot() */
+
+
+#ifdef CSR_WIFI_REQUEUE_PACKET_TO_HAL
+/*
+ * ---------------------------------------------------------------------------
+ *  CardClearFromHostDataSlotWithoutFreeingBulkData
+ *
+ *      Clear the given data slot with out freeing the bulk data.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *      slot            Index of the signal slot to clear.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void CardClearFromHostDataSlotWithoutFreeingBulkData(card_t *card, const s16 slot)
+{
+    u8 queue = card->from_host_data[slot].queue;
+
+    /* Initialise the from_host data slot so it can be re-used,
+     * Set length field in from_host_data array to 0.
+     */
+    UNIFI_INIT_BULK_DATA(&card->from_host_data[slot].bd);
+
+    queue = card->from_host_data[slot].queue;
+
+    if (queue < UNIFI_NO_OF_TX_QS)
+    {
+        if (card->dynamic_slot_data.from_host_used_slots[queue] == 0)
+        {
+            unifi_error(card->ospriv, "Goofed up used slots q = %d used slots = %d\n",
+                        queue,
+                        card->dynamic_slot_data.from_host_used_slots[queue]);
+        }
+        else
+        {
+            card->dynamic_slot_data.from_host_used_slots[queue]--;
+        }
+        card->dynamic_slot_data.packets_txed[queue]++;
+        card->dynamic_slot_data.total_packets_txed++;
+        if (card->dynamic_slot_data.total_packets_txed >=
+            card->dynamic_slot_data.packets_interval)
+        {
+            CardReassignDynamicReservation(card);
+        }
+    }
+} /* CardClearFromHostDataSlotWithoutFreeingBulkData() */
+
+
+#endif
+
+u16 CardGetDataSlotSize(card_t *card)
+{
+    return card->config_data.data_slot_size;
+} /* CardGetDataSlotSize() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardGetFreeFromHostDataSlots
+ *
+ *      Retrieve the number of from-host bulk data slots available.
+ *
+ *  Arguments:
+ *      card            Pointer to the card context struct
+ *
+ *  Returns:
+ *      Number of free from-host bulk data slots.
+ * ---------------------------------------------------------------------------
+ */
+u16 CardGetFreeFromHostDataSlots(card_t *card)
+{
+    u16 i, n = 0;
+
+    func_enter();
+
+    /* First two slots reserved for MLME */
+    for (i = 0; i < card->config_data.num_fromhost_data_slots; i++)
+    {
+        if (card->from_host_data[i].bd.data_length == 0)
+        {
+            /* Free slot */
+            n++;
+        }
+    }
+
+    func_exit();
+    return n;
+} /* CardGetFreeFromHostDataSlots() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardAreAllFromHostDataSlotsEmpty
+ *
+ *      Returns the state of from-host bulk data slots.
+ *
+ *  Arguments:
+ *      card            Pointer to the card context struct
+ *
+ *  Returns:
+ *      1       The from-host bulk data slots are all empty (available).
+ *      0       Some or all the from-host bulk data slots are in use.
+ * ---------------------------------------------------------------------------
+ */
+u16 CardAreAllFromHostDataSlotsEmpty(card_t *card)
+{
+    u16 i;
+
+    for (i = 0; i < card->config_data.num_fromhost_data_slots; i++)
+    {
+        if (card->from_host_data[i].bd.data_length != 0)
+        {
+            return 0;
+        }
+    }
+
+    return 1;
+} /* CardGetFreeFromHostDataSlots() */
+
+
+static CsrResult unifi_identify_hw(card_t *card)
+{
+    func_enter();
+
+    card->chip_id = card->sdio_if->sdioId.cardId;
+    card->function = card->sdio_if->sdioId.sdioFunction;
+    card->sdio_io_block_size = card->sdio_if->blockSize;
+
+    /* If SDIO controller doesn't support byte mode CMD53, pad transfers to block sizes */
+    card->sdio_io_block_pad = (card->sdio_if->features & CSR_SDIO_FEATURE_BYTE_MODE)?FALSE : TRUE;
+
+    /*
+     * Setup the chip helper so that we can access the registers (and
+     * also tell what sub-type of HIP we should use).
+     */
+    card->helper = ChipHelper_GetVersionSdio((u8)card->chip_id);
+    if (!card->helper)
+    {
+        unifi_error(card->ospriv, "Null ChipHelper\n");
+    }
+
+    unifi_info(card->ospriv, "Chip ID 0x%02X  Function %u  Block Size %u  Name %s(%s)\n",
+               card->chip_id, card->function, card->sdio_io_block_size,
+               ChipHelper_MarketingName(card->helper),
+               ChipHelper_FriendlyName(card->helper));
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* unifi_identify_hw() */
+
+
+static CsrResult unifi_prepare_hw(card_t *card)
+{
+    CsrResult r;
+    CsrResult csrResult;
+    enum unifi_host_state old_state = card->host_state;
+
+    func_enter();
+
+    r = unifi_identify_hw(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to identify hw\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    unifi_trace(card->ospriv, UDBG1,
+                "%s mode SDIO\n", card->sdio_io_block_pad?"Block" : "Byte");
+    /*
+     * Chip must be a awake or blocks that are asleep may not get
+     * reset.  We can only do this after we have read the chip_id.
+     */
+    r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+
+    if (old_state == UNIFI_HOST_STATE_TORPID)
+    {
+        /* Ensure the initial clock rate is set; if a reset occured when the chip was
+         * TORPID, unifi_set_host_state() may have raised it to MAX.
+         */
+        csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_INIT_HZ);
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            func_exit_r(r);
+            return r;
+        }
+        card->sdio_clock_speed = UNIFI_SDIO_CLOCK_INIT_HZ;
+    }
+
+    /*
+     * The WLAN function must be enabled to access MAILBOX2 and DEBUG_RST
+     * registers.
+     */
+    csrResult = CsrSdioFunctionEnable(card->sdio_if);
+    if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+    }
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+        /* Can't enable WLAN function. Try resetting the SDIO block. */
+        unifi_error(card->ospriv, "Failed to re-enable function %d.\n", card->function);
+        func_exit_r(r);
+        return r;
+    }
+
+    /*
+     * Poke some registers to make sure the PLL has started,
+     * otherwise memory accesses are likely to fail.
+     */
+    bootstrap_chip_hw(card);
+
+    /* Try to read the chip version from register. */
+    r = unifi_read_chip_version(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        func_exit_r(r);
+        return r;
+    }
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* unifi_prepare_hw() */
+
+
+static CsrResult unifi_read_chip_version(card_t *card)
+{
+    u32 gbl_chip_version;
+    CsrResult r;
+    u16 ver;
+
+    func_enter();
+
+    gbl_chip_version = ChipHelper_GBL_CHIP_VERSION(card->helper);
+
+    /* Try to read the chip version from register. */
+    if (gbl_chip_version != 0)
+    {
+        r = unifi_read_direct16(card, gbl_chip_version * 2, &ver);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to read GBL_CHIP_VERSION\n");
+            func_exit_r(r);
+            return r;
+        }
+        card->chip_version = ver;
+    }
+    else
+    {
+        unifi_info(card->ospriv, "Unknown Chip ID, cannot locate GBL_CHIP_VERSION\n");
+        r = CSR_RESULT_FAILURE;
+    }
+
+    unifi_info(card->ospriv, "Chip Version 0x%04X\n", card->chip_version);
+
+    func_exit_r(r);
+    return r;
+} /* unifi_read_chip_version() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_reset_hardware
+ *
+ *      Execute the UniFi reset sequence.
+ *
+ *      Note: This may fail if the chip is going TORPID so retry at
+ *      least once.
+ *
+ *  Arguments:
+ *      card - pointer to card context structure
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error otherwise.
+ *
+ *  Notes:
+ *      Some platforms (e.g. Windows Vista) do not allow access to registers
+ *      that are necessary for a software soft reset.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult unifi_reset_hardware(card_t *card)
+{
+    CsrResult r;
+    u16 new_block_size = UNIFI_IO_BLOCK_SIZE;
+    CsrResult csrResult;
+
+    func_enter();
+
+    /* Errors returned by unifi_prepare_hw() are not critical at this point */
+    r = unifi_prepare_hw(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+
+    /* First try SDIO controller reset, which may power cycle the UniFi, assert
+     * its reset line, or not be implemented depending on the platform.
+     */
+    unifi_info(card->ospriv, "Calling CsrSdioHardReset\n");
+    csrResult = CsrSdioHardReset(card->sdio_if);
+    if (csrResult == CSR_RESULT_SUCCESS)
+    {
+        unifi_info(card->ospriv, "CsrSdioHardReset succeeded on reseting UniFi\n");
+        r = unifi_prepare_hw(card);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "unifi_prepare_hw failed after hard reset\n");
+            func_exit_r(r);
+            return r;
+        }
+    }
+    else if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+    }
+    else
+    {
+        /* Falling back to software hard reset methods */
+        unifi_info(card->ospriv, "Falling back to software hard reset\n");
+        r = unifi_card_hard_reset(card);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "software hard reset failed\n");
+            func_exit_r(r);
+            return r;
+        }
+
+        /* If we fell back to unifi_card_hard_reset() methods, chip version may
+         * not have been read. (Note in the unlikely event that it is zero,
+         * it will be harmlessly read again)
+         */
+        if (card->chip_version == 0)
+        {
+            r = unifi_read_chip_version(card);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                func_exit_r(r);
+                return r;
+            }
+        }
+    }
+
+#ifdef CSR_WIFI_HIP_SDIO_BLOCK_SIZE
+    new_block_size = CSR_WIFI_HIP_SDIO_BLOCK_SIZE;
+#endif
+
+    /* After hard reset, we need to restore the SDIO block size */
+    csrResult = CsrSdioBlockSizeSet(card->sdio_if, new_block_size);
+    r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+
+    /* Warn if a different block size was achieved by the transport */
+    if (card->sdio_if->blockSize != new_block_size)
+    {
+        unifi_info(card->ospriv,
+                   "Actually got block size %d\n", card->sdio_if->blockSize);
+    }
+
+    /* sdio_io_block_size always needs be updated from the achieved block size,
+     * as it is used by the OS layer to allocate memory in unifi_net_malloc().
+     * Controllers which don't support block mode (e.g. CSPI) will report a
+     * block size of zero.
+     */
+    if (card->sdio_if->blockSize == 0)
+    {
+        unifi_info(card->ospriv, "Block size 0, block mode not available\n");
+
+        /* Set sdio_io_block_size to 1 so that unifi_net_data_malloc() has a
+         * sensible rounding value. Elsewhere padding will already be
+         * disabled because the controller supports byte mode.
+         */
+        card->sdio_io_block_size = 1;
+
+        /* Controller features must declare support for byte mode */
+        if (!(card->sdio_if->features & CSR_SDIO_FEATURE_BYTE_MODE))
+        {
+            unifi_error(card->ospriv, "Requires byte mode\n");
+            r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+        }
+    }
+    else
+    {
+        /* Padding will be enabled if CSR_SDIO_FEATURE_BYTE_MODE isn't set */
+        card->sdio_io_block_size = card->sdio_if->blockSize;
+    }
+
+
+    func_exit_r(r);
+    return r;
+} /* unifi_reset_hardware() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_reset_method_io_enable
+ *
+ *      Issue a hard reset to the hw writing the IO_ENABLE.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      0 on success,
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE   if the card was ejected
+ *      CSR_RESULT_FAILURE         if an SDIO error occurred or if a response
+ *                                 was not seen in the expected time
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_reset_method_io_enable(card_t *card)
+{
+    CsrResult r;
+    CsrResult csrResult;
+
+    func_enter();
+
+    /*
+     * This resets only function 1, so should be used in
+     * preference to the method below (CSR_FUNC_EN)
+     */
+    unifi_trace(card->ospriv, UDBG1, "Hard reset (IO_ENABLE)\n");
+
+    csrResult = CsrSdioFunctionDisable(card->sdio_if);
+    if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+    }
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+        unifi_warning(card->ospriv, "SDIO error writing IO_ENABLE: %d\n", r);
+    }
+    else
+    {
+        /* Delay here to let the reset take affect. */
+        CsrThreadSleep(RESET_SETTLE_DELAY);
+
+        r = card_wait_for_unifi_to_disable(card);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+
+        if (r == CSR_RESULT_SUCCESS)
+        {
+            r = card_wait_for_unifi_to_reset(card);
+            if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                return r;
+            }
+        }
+    }
+
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_trace(card->ospriv, UDBG1, "Hard reset (CSR_FUNC_EN)\n");
+
+        r = sdio_write_f0(card, SDIO_CSR_FUNC_EN, 0);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_warning(card->ospriv, "SDIO error writing SDIO_CSR_FUNC_EN: %d\n", r);
+            func_exit_r(r);
+            return r;
+        }
+        else
+        {
+            /* Delay here to let the reset take affect. */
+            CsrThreadSleep(RESET_SETTLE_DELAY);
+
+            r = card_wait_for_unifi_to_reset(card);
+            if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                return r;
+            }
+        }
+    }
+
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_warning(card->ospriv, "card_reset_method_io_enable failed to reset UniFi\n");
+    }
+
+    func_exit();
+    return r;
+} /* card_reset_method_io_enable() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_reset_method_dbg_reset
+ *
+ *      Issue a hard reset to the hw writing the DBG_RESET.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS         on success,
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE   if the card was ejected
+ *      CSR_RESULT_FAILURE         if an SDIO error occurred or if a response
+ *                                 was not seen in the expected time
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult card_reset_method_dbg_reset(card_t *card)
+{
+    CsrResult r;
+
+    func_enter();
+
+    /*
+     * Prepare UniFi for h/w reset
+     */
+    if (card->host_state == UNIFI_HOST_STATE_TORPID)
+    {
+        r = unifi_set_host_state(card, UNIFI_HOST_STATE_DROWSY);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to set UNIFI_HOST_STATE_DROWSY\n");
+            func_exit_r(r);
+            return r;
+        }
+        CsrThreadSleep(5);
+    }
+
+    r = unifi_card_stop_processor(card, UNIFI_PROC_BOTH);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Can't stop processors\n");
+        func_exit();
+        return r;
+    }
+
+    unifi_trace(card->ospriv, UDBG1, "Hard reset (DBG_RESET)\n");
+
+    /*
+     * This register write may fail. The debug reset resets
+     * parts of the Function 0 sections of the chip, and
+     * therefore the response cannot be sent back to the host.
+     */
+    r = unifi_write_direct_8_or_16(card, ChipHelper_DBG_RESET(card->helper) * 2, 1);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_warning(card->ospriv, "SDIO error writing DBG_RESET: %d\n", r);
+        func_exit_r(r);
+        return r;
+    }
+
+    /* Delay here to let the reset take affect. */
+    CsrThreadSleep(RESET_SETTLE_DELAY);
+
+    r = card_wait_for_unifi_to_reset(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_warning(card->ospriv, "card_reset_method_dbg_reset failed to reset UniFi\n");
+    }
+
+    func_exit();
+    return r;
+} /* card_reset_method_dbg_reset() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_card_hard_reset
+ *
+ *      Issue reset to hardware, by writing to registers on the card.
+ *      Power to the card is preserved.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS         on success,
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE   if the card was ejected
+ *      CSR_RESULT_FAILURE         if an SDIO error occurred or if a response
+ *                                 was not seen in the expected time
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_card_hard_reset(card_t *card)
+{
+    CsrResult r;
+    const struct chip_helper_reset_values *init_data;
+    u32 chunks;
+
+    func_enter();
+
+    /* Clear cache of page registers */
+    card->proc_select = (u32)(-1);
+    card->dmem_page = (u32)(-1);
+    card->pmem_page = (u32)(-1);
+
+    /*
+     * We need to have a valid card->helper before we use software hard reset.
+     * If unifi_identify_hw() fails to get the card ID, it probably means
+     * that there is no way to talk to the h/w.
+     */
+    r = unifi_identify_hw(card);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "unifi_card_hard_reset failed to identify h/w\n");
+        func_exit();
+        return r;
+    }
+
+    /* Search for some reset code. */
+    chunks = ChipHelper_HostResetSequence(card->helper, &init_data);
+    if (chunks != 0)
+    {
+        unifi_error(card->ospriv,
+                    "Hard reset (Code download) is unsupported\n");
+
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+    }
+
+    if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+    {
+        /* The HIP spec considers this a bus-specific reset.
+         * This resets only function 1, so should be used in
+         * preference to the method below (CSR_FUNC_EN)
+         * If this method fails, it means that the f/w is probably
+         * not running. In this case, try the DBG_RESET method.
+         */
+        r = card_reset_method_io_enable(card);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r == CSR_RESULT_SUCCESS)
+        {
+            func_exit();
+            return r;
+        }
+    }
+
+    /* Software hard reset */
+    r = card_reset_method_dbg_reset(card);
+
+    func_exit_r(r);
+    return r;
+} /* unifi_card_hard_reset() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *  CardGenInt
+ *
+ *      Prod the card.
+ *      This function causes an internal interrupt to be raised in the
+ *      UniFi chip. It is used to signal the firmware that some action has
+ *      been completed.
+ *      The UniFi Host Interface asks that the value used increments for
+ *      debugging purposes.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS         on success,
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE   if the card was ejected
+ *      CSR_RESULT_FAILURE         if an SDIO error occurred or if a response
+ *                                 was not seen in the expected time
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardGenInt(card_t *card)
+{
+    CsrResult r;
+
+    func_enter();
+
+    if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+    {
+        r = sdio_write_f0(card, SDIO_CSR_FROM_HOST_SCRATCH0,
+                          (u8)card->unifi_interrupt_seq);
+    }
+    else
+    {
+        r = unifi_write_direct_8_or_16(card,
+                                       ChipHelper_SHARED_IO_INTERRUPT(card->helper) * 2,
+                                       (u8)card->unifi_interrupt_seq);
+    }
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error writing UNIFI_SHARED_IO_INTERRUPT: %d\n", r);
+        func_exit_r(r);
+        return r;
+    }
+
+    card->unifi_interrupt_seq++;
+
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+} /* CardGenInt() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardEnableInt
+ *
+ *      Enable the outgoing SDIO interrupt from UniFi to the host.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS            on success,
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE      if the card was ejected
+ *      CSR_RESULT_FAILURE            if an SDIO error occurred,
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardEnableInt(card_t *card)
+{
+    CsrResult r;
+    u8 int_enable;
+
+    r = sdio_read_f0(card, SDIO_INT_ENABLE, &int_enable);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error reading SDIO_INT_ENABLE\n");
+        return r;
+    }
+
+    int_enable |= (1 << card->function) | UNIFI_SD_INT_ENABLE_IENM;
+
+    r = sdio_write_f0(card, SDIO_INT_ENABLE, int_enable);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error writing SDIO_INT_ENABLE\n");
+        return r;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CardEnableInt() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardDisableInt
+ *
+ *      Disable the outgoing SDIO interrupt from UniFi to the host.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS            on success,
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE      if the card was ejected
+ *      CSR_RESULT_FAILURE            if an SDIO error occurred,
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardDisableInt(card_t *card)
+{
+    CsrResult r;
+    u8 int_enable;
+
+    r = sdio_read_f0(card, SDIO_INT_ENABLE, &int_enable);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error reading SDIO_INT_ENABLE\n");
+        return r;
+    }
+
+    int_enable &= ~(1 << card->function);
+
+    r = sdio_write_f0(card, SDIO_INT_ENABLE, int_enable);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error writing SDIO_INT_ENABLE\n");
+        return r;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CardDisableInt() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardPendingInt
+ *
+ *      Determine whether UniFi is currently asserting the SDIO interrupt
+ *      request.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *      pintr           Pointer to location to write interrupt status,
+ *                          TRUE if interrupt pending,
+ *                          FALSE if no interrupt pending.
+ *  Returns:
+ *      CSR_RESULT_SUCCESS            interrupt status read successfully
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE      if the card was ejected
+ *      CSR_RESULT_FAILURE            if an SDIO error occurred,
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardPendingInt(card_t *card, u8 *pintr)
+{
+    CsrResult r;
+    u8 pending;
+
+    *pintr = FALSE;
+
+    r = sdio_read_f0(card, SDIO_INT_PENDING, &pending);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error reading SDIO_INT_PENDING\n");
+        return r;
+    }
+
+    *pintr = (pending & (1 << card->function))?TRUE : FALSE;
+
+    return CSR_RESULT_SUCCESS;
+} /* CardPendingInt() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardClearInt
+ *
+ *      Clear the UniFi SDIO interrupt request.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS          if pending interrupt was cleared, or no pending interrupt.
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE    if the card was ejected
+ *      CSR_RESULT_FAILURE          if an SDIO error occurred,
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardClearInt(card_t *card)
+{
+    CsrResult r;
+    u8 intr;
+
+    if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+    {
+        /* CardPendingInt() sets intr, if there is a pending interrupt */
+        r = CardPendingInt(card, &intr);
+        if (intr == FALSE)
+        {
+            return r;
+        }
+
+        r = sdio_write_f0(card, SDIO_CSR_HOST_INT_CLEAR, 1);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "SDIO error writing SDIO_CSR_HOST_INT_CLEAR\n");
+        }
+    }
+    else
+    {
+        r = unifi_write_direct_8_or_16(card,
+                                       ChipHelper_SDIO_HOST_INT(card->helper) * 2,
+                                       0);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "SDIO error writing UNIFI_SDIO_HOST_INT\n");
+        }
+    }
+
+    return r;
+} /* CardClearInt() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardIntEnabled
+ *
+ *      Determine whether UniFi is currently asserting the SDIO interrupt
+ *      request.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *      enabled         Pointer to location to write interrupt enable status,
+ *                          TRUE if interrupts enabled,
+ *                          FALSE if interupts disabled.
+ *
+ *  Returns:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE      if the card was ejected
+ *      CSR_RESULT_FAILURE            if an SDIO error occurred,
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardIntEnabled(card_t *card, u8 *enabled)
+{
+    CsrResult r;
+    u8 int_enable;
+
+    r = sdio_read_f0(card, SDIO_INT_ENABLE, &int_enable);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "SDIO error reading SDIO_INT_ENABLE\n");
+        return r;
+    }
+
+    *enabled = (int_enable & (1 << card->function))?TRUE : FALSE;
+
+    return CSR_RESULT_SUCCESS;
+} /* CardIntEnabled() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CardWriteBulkData
+ *      Allocate slot in the pending bulkdata arrays and assign it to a signal's
+ *      bulkdata reference. The slot is then ready for UniFi's bulkdata commands
+ *      to transfer the data to/from the host.
+ *
+ *  Arguments:
+ *      card            Pointer to Card object
+ *      csptr           Pending signal pointer, including bulkdata ref
+ *      queue           Traffic queue that this signal is using
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS if a free slot was assigned
+ *      CSR_RESULT_FAILURE if no slot was available
+ * ---------------------------------------------------------------------------
+ */
+CsrResult CardWriteBulkData(card_t *card, card_signal_t *csptr, unifi_TrafficQueue queue)
+{
+    u16 i, slots[UNIFI_MAX_DATA_REFERENCES], j = 0;
+    u8 *packed_sigptr, num_slots_required = 0;
+    bulk_data_desc_t *bulkdata = csptr->bulkdata;
+    s16 h, nslots;
+
+    func_enter();
+
+    /* Count the number of slots required */
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++)
+    {
+        if (bulkdata[i].data_length != 0)
+        {
+            num_slots_required++;
+        }
+    }
+
+    /* Get the slot numbers */
+    if (num_slots_required != 0)
+    {
+        /* Last 2 slots for MLME */
+        if (queue == UNIFI_TRAFFIC_Q_MLME)
+        {
+            h = card->config_data.num_fromhost_data_slots - UNIFI_RESERVED_COMMAND_SLOTS;
+            for (i = 0; i < card->config_data.num_fromhost_data_slots; i++)
+            {
+                if (card->from_host_data[h].bd.data_length == 0)
+                {
+                    /* Free data slot, claim it */
+                    slots[j++] = h;
+                    if (j == num_slots_required)
+                    {
+                        break;
+                    }
+                }
+
+                if (++h >= card->config_data.num_fromhost_data_slots)
+                {
+                    h = 0;
+                }
+            }
+        }
+        else
+        {
+            if (card->dynamic_slot_data.from_host_used_slots[queue]
+                < card->dynamic_slot_data.from_host_max_slots[queue])
+            {
+                /* Data commands get a free slot only after a few checks */
+                nslots = card->config_data.num_fromhost_data_slots - UNIFI_RESERVED_COMMAND_SLOTS;
+
+                h = card->from_host_data_head;
+
+                for (i = 0; i < nslots; i++)
+                {
+                    if (card->from_host_data[h].bd.data_length == 0)
+                    {
+                        /* Free data slot, claim it */
+                        slots[j++] = h;
+                        if (j == num_slots_required)
+                        {
+                            break;
+                        }
+                    }
+
+                    if (++h >= nslots)
+                    {
+                        h = 0;
+                    }
+                }
+                card->from_host_data_head = h;
+            }
+        }
+
+        /* Required number of slots are not available, bail out */
+        if (j != num_slots_required)
+        {
+            unifi_trace(card->ospriv, UDBG5, "CardWriteBulkData: didn't find free slot/s\n");
+
+            /* If we haven't already reached the stable state we can ask for reservation */
+            if ((queue != UNIFI_TRAFFIC_Q_MLME) && (card->dynamic_slot_data.queue_stable[queue] == FALSE))
+            {
+                CardCheckDynamicReservation(card, queue);
+            }
+
+            for (i = 0; i < card->config_data.num_fromhost_data_slots; i++)
+            {
+                unifi_trace(card->ospriv, UDBG5, "fh data slot %d: %d\n", i, card->from_host_data[i].bd.data_length);
+            }
+            func_exit();
+            return CSR_RESULT_FAILURE;
+        }
+    }
+
+    packed_sigptr = csptr->sigbuf;
+
+    /* Fill in the slots with data */
+    j = 0;
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++)
+    {
+        if (bulkdata[i].data_length == 0)
+        {
+            /* Zero-out the DATAREF in the signal */
+            SET_PACKED_DATAREF_SLOT(packed_sigptr, i, 0);
+            SET_PACKED_DATAREF_LEN(packed_sigptr, i, 0);
+        }
+        else
+        {
+            /*
+             * Fill in the slot number in the SIGNAL structure but
+             * preserve the offset already in there
+             */
+            SET_PACKED_DATAREF_SLOT(packed_sigptr, i, slots[j] | (((u16)packed_sigptr[SIZEOF_SIGNAL_HEADER + (i * SIZEOF_DATAREF) + 1]) << 8));
+            SET_PACKED_DATAREF_LEN(packed_sigptr, i, bulkdata[i].data_length);
+
+            /* Do not copy the data, just store the information to them */
+            card->from_host_data[slots[j]].bd.os_data_ptr = bulkdata[i].os_data_ptr;
+            card->from_host_data[slots[j]].bd.os_net_buf_ptr = bulkdata[i].os_net_buf_ptr;
+            card->from_host_data[slots[j]].bd.data_length = bulkdata[i].data_length;
+            card->from_host_data[slots[j]].bd.net_buf_length = bulkdata[i].net_buf_length;
+            card->from_host_data[slots[j]].queue = queue;
+
+            unifi_trace(card->ospriv, UDBG4, "CardWriteBulkData sig=0x%x, fh slot %d = %p\n",
+                        GET_SIGNAL_ID(packed_sigptr), i, bulkdata[i].os_data_ptr);
+
+            /* Sanity-check that the bulk data desc being assigned to the slot
+             * actually has a payload.
+             */
+            if (!bulkdata[i].os_data_ptr)
+            {
+                unifi_error(card->ospriv, "Assign null os_data_ptr (len=%d) fh slot %d, i=%d, q=%d, sig=0x%x",
+                            bulkdata[i].data_length, slots[j], i, queue, GET_SIGNAL_ID(packed_sigptr));
+            }
+
+            j++;
+            if (queue < UNIFI_NO_OF_TX_QS)
+            {
+                card->dynamic_slot_data.from_host_used_slots[queue]++;
+            }
+        }
+    }
+
+    func_exit();
+
+    return CSR_RESULT_SUCCESS;
+} /*  CardWriteBulkData() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_find_data_slot
+ *
+ *      Dereference references to bulk data slots into pointers to real data.
+ *
+ *  Arguments:
+ *      card            Pointer to the card struct.
+ *      slot            Slot number from a signal structure
+ *
+ *  Returns:
+ *      Pointer to entry in bulk_data_slot array.
+ * ---------------------------------------------------------------------------
+ */
+bulk_data_desc_t* card_find_data_slot(card_t *card, s16 slot)
+{
+    s16 sn;
+    bulk_data_desc_t *bd;
+
+    sn = slot & 0x7FFF;
+
+    /* ?? check sanity of slot number ?? */
+
+    if (slot & SLOT_DIR_TO_HOST)
+    {
+        bd = &card->to_host_data[sn];
+    }
+    else
+    {
+        bd = &card->from_host_data[sn].bd;
+    }
+
+    return bd;
+} /* card_find_data_slot() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  firmware_present_in_flash
+ *
+ *      Probe for external Flash that looks like it might contain firmware.
+ *
+ *      If Flash is not present, reads always return 0x0008.
+ *      If Flash is present, but empty, reads return 0xFFFF.
+ *      Anything else is considered to be firmware.
+ *
+ *  Arguments:
+ *      card        Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS                 firmware is present in ROM or flash
+ *      CSR_WIFI_HIP_RESULT_NOT_FOUND      firmware is not present in ROM or flash
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE      if the card was ejected
+ *      CSR_RESULT_FAILURE                 if an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult firmware_present_in_flash(card_t *card)
+{
+    CsrResult r;
+    u16 m1, m5;
+
+    if (ChipHelper_HasRom(card->helper))
+    {
+        return CSR_RESULT_SUCCESS;
+    }
+    if (!ChipHelper_HasFlash(card->helper))
+    {
+        return CSR_WIFI_HIP_RESULT_NOT_FOUND;
+    }
+
+    /*
+     * Examine the Flash locations that are the power-on default reset
+     * vectors of the XAP processors.
+     * These are words 1 and 5 in Flash.
+     */
+    r = unifi_card_read16(card, UNIFI_MAKE_GP(EXT_FLASH, 2), &m1);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    r = unifi_card_read16(card, UNIFI_MAKE_GP(EXT_FLASH, 10), &m5);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    /* Check for uninitialised/missing flash */
+    if ((m1 == 0x0008) || (m1 == 0xFFFF) ||
+        (m1 == 0x0004) || (m5 == 0x0004) ||
+        (m5 == 0x0008) || (m5 == 0xFFFF))
+    {
+        return CSR_WIFI_HIP_RESULT_NOT_FOUND;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* firmware_present_in_flash() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  bootstrap_chip_hw
+ *
+ *      Perform chip specific magic to "Get It Working" TM.  This will
+ *      increase speed of PLLs in analogue and maybe enable some
+ *      on-chip regulators.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void bootstrap_chip_hw(card_t *card)
+{
+    const struct chip_helper_init_values *vals;
+    u32 i, len;
+    void *sdio = card->sdio_if;
+    CsrResult csrResult;
+
+    len = ChipHelper_ClockStartupSequence(card->helper, &vals);
+    if (len != 0)
+    {
+        for (i = 0; i < len; i++)
+        {
+            csrResult = CsrSdioWrite16(sdio, vals[i].addr * 2, vals[i].value);
+            if (csrResult != CSR_RESULT_SUCCESS)
+            {
+                unifi_warning(card->ospriv, "Failed to write bootstrap value %d\n", i);
+                /* Might not be fatal */
+            }
+
+            CsrThreadSleep(1);
+        }
+    }
+} /* bootstrap_chip_hw() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_card_stop_processor
+ *
+ *      Stop the UniFi XAP processors.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      which           One of UNIFI_PROC_MAC, UNIFI_PROC_PHY, UNIFI_PROC_BOTH
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS if successful, or CSR error code
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_card_stop_processor(card_t *card, enum unifi_dbg_processors_select which)
+{
+    CsrResult r = CSR_RESULT_SUCCESS;
+    u8 status;
+    s16 retry = 100;
+
+    while (retry--)
+    {
+        /* Select both XAPs */
+        r = unifi_set_proc_select(card, which);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            break;
+        }
+
+        /* Stop processors */
+        r = unifi_write_direct16(card, ChipHelper_DBG_EMU_CMD(card->helper) * 2, 2);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            break;
+        }
+
+        /* Read status */
+        r = unifi_read_direct_8_or_16(card,
+                                      ChipHelper_DBG_HOST_STOP_STATUS(card->helper) * 2,
+                                      &status);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            break;
+        }
+
+        if ((status & 1) == 1)
+        {
+            /* Success! */
+            return CSR_RESULT_SUCCESS;
+        }
+
+        /* Processors didn't stop, try again */
+    }
+
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        /* An SDIO error occurred */
+        unifi_error(card->ospriv, "Failed to stop processors: SDIO error\n");
+    }
+    else
+    {
+        /* If we reach here, we didn't the status in time. */
+        unifi_error(card->ospriv, "Failed to stop processors: timeout waiting for stopped status\n");
+        r = CSR_RESULT_FAILURE;
+    }
+
+    return r;
+} /* unifi_card_stop_processor() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  card_start_processor
+ *
+ *      Start the UniFi XAP processors.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      which           One of UNIFI_PROC_MAC, UNIFI_PROC_PHY, UNIFI_PROC_BOTH
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS or CSR error code
+ * ---------------------------------------------------------------------------
+ */
+CsrResult card_start_processor(card_t *card, enum unifi_dbg_processors_select which)
+{
+    CsrResult r;
+
+    /* Select both XAPs */
+    r = unifi_set_proc_select(card, which);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "unifi_set_proc_select failed: %d.\n", r);
+        return r;
+    }
+
+
+    r = unifi_write_direct_8_or_16(card,
+                                   ChipHelper_DBG_EMU_CMD(card->helper) * 2, 8);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    r = unifi_write_direct_8_or_16(card,
+                                   ChipHelper_DBG_EMU_CMD(card->helper) * 2, 0);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* card_start_processor() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_set_interrupt_mode
+ *
+ *      Configure the interrupt processing mode used by the HIP
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      mode            Interrupt mode to apply
+ *
+ *  Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+void unifi_set_interrupt_mode(card_t *card, u32 mode)
+{
+    if (mode == CSR_WIFI_INTMODE_RUN_BH_ONCE)
+    {
+        unifi_info(card->ospriv, "Scheduled interrupt mode");
+    }
+    card->intmode = mode;
+} /* unifi_set_interrupt_mode() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_start_processors
+ *
+ *      Start all UniFi XAP processors.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on error
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_start_processors(card_t *card)
+{
+    return card_start_processor(card, UNIFI_PROC_BOTH);
+} /* unifi_start_processors() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_request_max_sdio_clock
+ *
+ *      Requests that the maximum SDIO clock rate is set at the next suitable
+ *      opportunity (e.g. when the BH next runs, so as not to interfere with
+ *      any current operation).
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+void unifi_request_max_sdio_clock(card_t *card)
+{
+    card->request_max_clock = 1;
+} /* unifi_request_max_sdio_clock() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_set_host_state
+ *
+ *      Set the host deep-sleep state.
+ *
+ *      If transitioning to TORPID, the SDIO driver will be notified
+ *      that the SD bus will be unused (idle) and conversely, when
+ *      transitioning from TORPID that the bus will be used (active).
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      state           New deep-sleep state.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS            on success
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE      if the card was ejected
+ *      CSR_RESULT_FAILURE            if an SDIO error occurred
+ *
+ *  Notes:
+ *      We need to reduce the SDIO clock speed before trying to wake up the
+ *      chip. Actually, in the implementation below we reduce the clock speed
+ *      not just before we try to wake up the chip, but when we put the chip to
+ *      deep sleep. This means that if the f/w wakes up on its' own, we waste
+ *      a reduce/increace cycle. However, trying to eliminate this overhead is
+ *      proved difficult, as the current state machine in the HIP lib does at
+ *      least a CMD52 to disable the interrupts before we configure the host
+ *      state.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_set_host_state(card_t *card, enum unifi_host_state state)
+{
+    CsrResult r = CSR_RESULT_SUCCESS;
+    CsrResult csrResult;
+    static const char *const states[] = {
+        "AWAKE", "DROWSY", "TORPID"
+    };
+    static const u8 state_csr_host_wakeup[] = {
+        1, 3, 0
+    };
+    static const u8 state_io_abort[] = {
+        0, 2, 3
+    };
+
+    unifi_trace(card->ospriv, UDBG4, "State %s to %s\n",
+                states[card->host_state], states[state]);
+
+    if (card->host_state == UNIFI_HOST_STATE_TORPID)
+    {
+        CsrSdioFunctionActive(card->sdio_if);
+    }
+
+    /* Write the new state to UniFi. */
+    if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+    {
+        r = sdio_write_f0(card, SDIO_CSR_HOST_WAKEUP,
+                          (u8)((card->function << 4) | state_csr_host_wakeup[state]));
+    }
+    else
+    {
+        r = sdio_write_f0(card, SDIO_IO_ABORT, state_io_abort[state]);
+    }
+
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write UniFi deep sleep state\n");
+    }
+    else
+    {
+        /*
+         * If the chip was in state TORPID then we can now increase
+         * the maximum bus clock speed.
+         */
+        if (card->host_state == UNIFI_HOST_STATE_TORPID)
+        {
+            csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if,
+                                                       UNIFI_SDIO_CLOCK_MAX_HZ);
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            /* Non-fatal error */
+            if (r != CSR_RESULT_SUCCESS && r != CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                unifi_warning(card->ospriv,
+                              "Failed to increase the SDIO clock speed\n");
+            }
+            else
+            {
+                card->sdio_clock_speed = UNIFI_SDIO_CLOCK_MAX_HZ;
+            }
+        }
+
+        /*
+         * Cache the current state in the card structure to avoid
+         * unnecessary SDIO reads.
+         */
+        card->host_state = state;
+
+        if (state == UNIFI_HOST_STATE_TORPID)
+        {
+            /*
+             * If the chip is now in state TORPID then we must now decrease
+             * the maximum bus clock speed.
+             */
+            csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if,
+                                                       UNIFI_SDIO_CLOCK_SAFE_HZ);
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            if (r != CSR_RESULT_SUCCESS && r != CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                unifi_warning(card->ospriv,
+                              "Failed to decrease the SDIO clock speed\n");
+            }
+            else
+            {
+                card->sdio_clock_speed = UNIFI_SDIO_CLOCK_SAFE_HZ;
+            }
+            CsrSdioFunctionIdle(card->sdio_if);
+        }
+    }
+
+    return r;
+} /* unifi_set_host_state() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_card_info
+ *
+ *      Update the card information data structure
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      card_info       Pointer to info structure to update
+ *
+ *  Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+void unifi_card_info(card_t *card, card_info_t *card_info)
+{
+    card_info->chip_id = card->chip_id;
+    card_info->chip_version = card->chip_version;
+    card_info->fw_build = card->build_id;
+    card_info->fw_hip_version = card->config_data.version;
+    card_info->sdio_block_size = card->sdio_io_block_size;
+} /* unifi_card_info() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_check_io_status
+ *
+ *      Check UniFi for spontaneous reset and pending interrupt.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      status          Pointer to location to write chip status:
+ *                        0 if UniFi is running, and no interrupt pending
+ *                        1 if UniFi has spontaneously reset
+ *                        2 if there is a pending interrupt
+ *  Returns:
+ *      CSR_RESULT_SUCCESS if OK, or CSR error
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_check_io_status(card_t *card, s32 *status)
+{
+    u8 io_en;
+    CsrResult r;
+    u8 pending;
+
+    *status = 0;
+
+    r = sdio_read_f0(card, SDIO_IO_ENABLE, &io_en);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read SDIO_IO_ENABLE to check for spontaneous reset\n");
+        return r;
+    }
+
+    if ((io_en & (1 << card->function)) == 0)
+    {
+        s32 fw_count;
+        *status = 1;
+        unifi_error(card->ospriv, "UniFi has spontaneously reset.\n");
+
+        /*
+         * These reads are very likely to fail. We want to know if the function is really
+         * disabled or the SDIO driver just returns rubbish.
+         */
+        fw_count = unifi_read_shared_count(card, card->sdio_ctrl_addr + 4);
+        if (fw_count < 0)
+        {
+            unifi_error(card->ospriv, "Failed to read to-host sig written count\n");
+        }
+        else
+        {
+            unifi_error(card->ospriv, "thsw: %u (driver thinks is %u)\n",
+                        fw_count, card->to_host_signals_w);
+        }
+        fw_count = unifi_read_shared_count(card, card->sdio_ctrl_addr + 2);
+        if (fw_count < 0)
+        {
+            unifi_error(card->ospriv, "Failed to read from-host sig read count\n");
+        }
+        else
+        {
+            unifi_error(card->ospriv, "fhsr: %u (driver thinks is %u)\n",
+                        fw_count, card->from_host_signals_r);
+        }
+
+        return r;
+    }
+
+    unifi_info(card->ospriv, "UniFi function %d is enabled.\n", card->function);
+
+    /* See if we missed an SDIO interrupt */
+    r = CardPendingInt(card, &pending);
+    if (pending)
+    {
+        unifi_error(card->ospriv, "There is an unhandled pending interrupt.\n");
+        *status = 2;
+        return r;
+    }
+
+    return r;
+} /* unifi_check_io_status() */
+
+
+void unifi_get_hip_qos_info(card_t *card, unifi_HipQosInfo *hipqosinfo)
+{
+    s32 count_fhr;
+    s16 t;
+    u32 occupied_fh;
+
+    q_t *sigq;
+    u16 nslots, i;
+
+    memset(hipqosinfo, 0, sizeof(unifi_HipQosInfo));
+
+    nslots = card->config_data.num_fromhost_data_slots;
+
+    for (i = 0; i < nslots; i++)
+    {
+        if (card->from_host_data[i].bd.data_length == 0)
+        {
+            hipqosinfo->free_fh_bulkdata_slots++;
+        }
+    }
+
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        sigq = &card->fh_traffic_queue[i];
+        t = sigq->q_wr_ptr - sigq->q_rd_ptr;
+        if (t < 0)
+        {
+            t += sigq->q_length;
+        }
+        hipqosinfo->free_fh_sig_queue_slots[i] = (sigq->q_length - t) - 1;
+    }
+
+    count_fhr = unifi_read_shared_count(card, card->sdio_ctrl_addr + 2);
+    if (count_fhr < 0)
+    {
+        unifi_error(card->ospriv, "Failed to read from-host sig read count - %d\n", count_fhr);
+        hipqosinfo->free_fh_fw_slots = 0xfa;
+        return;
+    }
+
+    occupied_fh = (card->from_host_signals_w - count_fhr) % 128;
+
+    hipqosinfo->free_fh_fw_slots = (u16)(card->config_data.num_fromhost_sig_frags - occupied_fh);
+}
+
+
+
+CsrResult ConvertCsrSdioToCsrHipResult(card_t *card, CsrResult csrResult)
+{
+    CsrResult r = CSR_RESULT_FAILURE;
+
+    switch (csrResult)
+    {
+        case CSR_RESULT_SUCCESS:
+            r = CSR_RESULT_SUCCESS;
+            break;
+        /* Timeout errors */
+        case CSR_SDIO_RESULT_TIMEOUT:
+        /* Integrity errors */
+        case CSR_SDIO_RESULT_CRC_ERROR:
+            r = CSR_RESULT_FAILURE;
+            break;
+        case CSR_SDIO_RESULT_NO_DEVICE:
+            r = CSR_WIFI_HIP_RESULT_NO_DEVICE;
+            break;
+        case CSR_SDIO_RESULT_INVALID_VALUE:
+            r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            break;
+        case CSR_RESULT_FAILURE:
+            r = CSR_RESULT_FAILURE;
+            break;
+        default:
+            unifi_warning(card->ospriv, "Unrecognised csrResult error code: %d\n", csrResult);
+            break;
+    }
+
+    return r;
+} /* ConvertCsrSdioToCsrHipResult() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio.h b/drivers/staging/csr/csr_wifi_hip_card_sdio.h
new file mode 100644
index 00000000000..dc2ed70f7ed
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio.h
@@ -0,0 +1,702 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *  FILE:     csr_wifi_hip_card_sdio.h
+ *
+ *  PURPOSE:
+ *      Internal header for Card API for SDIO.
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __CARD_SDIO_H__
+#define __CARD_SDIO_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_unifi_udi.h"
+#include "csr_wifi_hip_unifihw.h"
+#include "csr_wifi_hip_unifiversion.h"
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+#include "csr_wifi_hip_ta_sampling.h"
+#endif
+#include "csr_wifi_hip_xbv.h"
+#include "csr_wifi_hip_chiphelper.h"
+
+
+/*
+ *
+ * Configuration items.
+ * Which of these should go in a platform unifi_config.h file?
+ *
+ */
+
+/*
+ * When the traffic queues contain more signals than there is space for on
+ * UniFi, a limiting algorithm comes into play.
+ * If a traffic queue has enough slots free to buffer more traffic from the
+ * network stack, then the following check is applied. The number of free
+ * slots is RESUME_XMIT_THRESHOLD.
+ */
+#define RESUME_XMIT_THRESHOLD           4
+
+
+/*
+ * When reading signals from UniFi, the host processes pending all signals
+ * and then acknowledges them together in a single write to update the
+ * to-host-chunks-read location.
+ * When there is more than one bulk data transfer (e.g. one received data
+ * packet and a request for the payload data of a transmitted packet), the
+ * update can be delayed significantly. This ties up resources on chip.
+ *
+ * To remedy this problem, to-host-chunks-read is updated after processing
+ * a signal if TO_HOST_FLUSH_THRESHOLD bytes of bulk data have been
+ * transferred since the last update.
+ */
+#define TO_HOST_FLUSH_THRESHOLD (500 * 5)
+
+
+/* SDIO Card Common Control Registers */
+#define SDIO_CCCR_SDIO_REVISION     (0x00)
+#define SDIO_SD_SPEC_REVISION       (0x01)
+#define SDIO_IO_ENABLE              (0x02)
+#define SDIO_IO_READY               (0x03)
+#define SDIO_INT_ENABLE             (0x04)
+#define SDIO_INT_PENDING            (0x05)
+#define SDIO_IO_ABORT               (0x06)
+#define SDIO_BUS_IFACE_CONTROL      (0x07)
+#define SDIO_CARD_CAPABILOTY        (0x08)
+#define SDIO_COMMON_CIS_POINTER     (0x09)
+#define SDIO_BUS_SUSPEND            (0x0C)
+#define SDIO_FUNCTION_SELECT        (0x0D)
+#define SDIO_EXEC_FLAGS             (0x0E)
+#define SDIO_READY_FLAGS            (0x0F)
+#define SDIO_FN0_BLOCK_SIZE         (0x10)
+#define SDIO_POWER_CONTROL          (0x12)
+#define SDIO_VENDOR_START           (0xF0)
+
+#define SDIO_CSR_HOST_WAKEUP        (0xf0)
+#define SDIO_CSR_HOST_INT_CLEAR     (0xf1)
+#define SDIO_CSR_FROM_HOST_SCRATCH0 (0xf2)
+#define SDIO_CSR_FROM_HOST_SCRATCH1 (0xf3)
+#define SDIO_CSR_TO_HOST_SCRATCH0   (0xf4)
+#define SDIO_CSR_TO_HOST_SCRATCH1   (0xf5)
+#define SDIO_CSR_FUNC_EN            (0xf6)
+#define SDIO_CSR_CSPI_MODE          (0xf7)
+#define SDIO_CSR_CSPI_STATUS        (0xf8)
+#define SDIO_CSR_CSPI_PADDING       (0xf9)
+
+
+#define UNIFI_SD_INT_ENABLE_IENM 0x0001    /* Master INT Enable */
+
+#ifdef CSR_PRE_ALLOC_NET_DATA
+#define BULK_DATA_PRE_ALLOC_NUM 16
+#endif
+
+/*
+ * Structure to hold configuration information read from UniFi.
+ */
+typedef struct
+{
+    /*
+     * The version of the SDIO signal queues and bulk data pools
+     * configuration structure. The MSB is the major version number, used to
+     * indicate incompatible changes. The LSB gives the minor revision number,
+     * used to indicate changes that maintain backwards compatibility.
+     */
+    u16 version;
+
+    /*
+     * offset from the start of the shared data memory to the SD IO
+     * control structure.
+     */
+    u16 sdio_ctrl_offset;
+
+    /* Buffer handle of the from-host signal queue */
+    u16 fromhost_sigbuf_handle;
+
+    /* Buffer handle of the to-host signal queue */
+    u16 tohost_sigbuf_handle;
+
+    /*
+     * Maximum number of signal primitive or bulk data command fragments that may be
+     * pending in the to-hw signal queue.
+     */
+    u16 num_fromhost_sig_frags;
+
+    /*
+     * Number of signal primitive or bulk data command fragments that must be pending
+     * in the to-host signal queue before the host will generate an interrupt
+     * to indicate that it has read a signal. This will usually be the total
+     * capacity of the to-host signal buffer less the size of the largest signal
+     * primitive divided by the signal primitive fragment size, but may be set
+     * to 1 to request interrupts every time that the host read a signal.
+     * Note that the hw may place more signals in the to-host signal queue
+     * than indicated by this field.
+     */
+    u16 num_tohost_sig_frags;
+
+    /*
+     * Number of to-hw bulk data slots. Slots are numbered from 0 (zero) to
+     * one less than the value in this field
+     */
+    u16 num_fromhost_data_slots;
+
+    /*
+     * Number of frm-hw bulk data slots. Slots are numbered from 0 (zero) to
+     * one less than the value in this field
+     */
+    u16 num_tohost_data_slots;
+
+    /*
+     * Size of the bulk data slots (2 octets)
+     * The size of the bulk data slots in octets. This will usually be
+     * the size of the largest MSDU. The value should always be even.
+     */
+    u16 data_slot_size;
+
+    /*
+     * Indicates that the host has finished the initialisation sequence.
+     * Initialised to 0x0000 by the firmware, and set to 0x0001 by us.
+     */
+    u16 initialised;
+
+    /* Added by protocol version 0x0001 */
+    u32 overlay_size;
+
+    /* Added by protocol version 0x0300 */
+    u16 data_slot_round;
+    u16 sig_frag_size;
+
+    /* Added by protocol version 0x0500 */
+    u16 tohost_signal_padding;
+} sdio_config_data_t;
+
+/*
+ * These values may change with versions of the Host Interface Protocol.
+ */
+/*
+ * Size of config info block pointed to by the CSR_SLT_SDIO_SLOT_CONFIG
+ * entry in the f/w symbol table
+ */
+#define SDIO_CONFIG_DATA_SIZE 30
+
+/* Offset of the INIT flag in the config info block. */
+#define SDIO_INIT_FLAG_OFFSET 0x12
+#define SDIO_TO_HOST_SIG_PADDING_OFFSET 0x1C
+
+
+/* Structure for a bulk data transfer command */
+typedef struct
+{
+    u16 cmd_and_len;   /* bits 12-15 cmd, bits 0-11 len */
+    u16 data_slot;     /* slot number, perhaps OR'd with SLOT_DIR_TO_HOST */
+    u16 offset;
+    u16 buffer_handle;
+} bulk_data_cmd_t;
+
+
+/* Bulk Data signal command values */
+#define SDIO_CMD_SIGNAL                 0x00
+#define SDIO_CMD_TO_HOST_TRANSFER       0x01
+#define SDIO_CMD_TO_HOST_TRANSFER_ACK   0x02 /*deprecated*/
+#define SDIO_CMD_FROM_HOST_TRANSFER     0x03
+#define SDIO_CMD_FROM_HOST_TRANSFER_ACK 0x04 /*deprecated*/
+#define SDIO_CMD_CLEAR_SLOT             0x05
+#define SDIO_CMD_OVERLAY_TRANSFER       0x06
+#define SDIO_CMD_OVERLAY_TRANSFER_ACK   0x07 /*deprecated*/
+#define SDIO_CMD_FROM_HOST_AND_CLEAR    0x08
+#define SDIO_CMD_PADDING                0x0f
+
+#define SLOT_DIR_TO_HOST 0x8000
+
+
+/* Initialise bulkdata slot
+ *  params:
+ *      bulk_data_desc_t *bulk_data_slot
+ */
+#define UNIFI_INIT_BULK_DATA(bulk_data_slot)        \
+    {                                               \
+        (bulk_data_slot)->os_data_ptr = NULL;       \
+        (bulk_data_slot)->data_length = 0;          \
+        (bulk_data_slot)->os_net_buf_ptr = NULL;    \
+        (bulk_data_slot)->net_buf_length = 0;       \
+    }
+
+/*
+ * Structure to contain a SIGNAL datagram.
+ * This is used to build signal queues between the main driver and the
+ * i/o thread.
+ * The fields are:
+ *      sigbuf          Contains the HIP signal is wire-format (i.e. packed,
+ *                      little-endian)
+ *      bulkdata        Contains a copy of any associated bulk data
+ *      signal_length   The size of the signal in the sigbuf
+ */
+typedef struct card_signal
+{
+    u8 sigbuf[UNIFI_PACKED_SIGBUF_SIZE];
+
+    /* Length of the SIGNAL inside sigbuf */
+    u16 signal_length;
+
+    bulk_data_desc_t bulkdata[UNIFI_MAX_DATA_REFERENCES];
+} card_signal_t;
+
+
+/*
+ * Control structure for a generic ring buffer.
+ */
+#define UNIFI_QUEUE_NAME_MAX_LENGTH     16
+typedef struct
+{
+    card_signal_t *q_body;
+
+    /* Num elements in queue (capacity is one less than this!) */
+    u16 q_length;
+
+    u16 q_wr_ptr;
+    u16 q_rd_ptr;
+
+    char name[UNIFI_QUEUE_NAME_MAX_LENGTH];
+} q_t;
+
+
+#define UNIFI_RESERVED_COMMAND_SLOTS   2
+
+/* Considering approx 500 us per packet giving 0.5 secs */
+#define UNIFI_PACKETS_INTERVAL         1000
+
+/*
+ * Dynamic slot reservation for QoS
+ */
+typedef struct
+{
+    u16 from_host_used_slots[UNIFI_NO_OF_TX_QS];
+    u16 from_host_max_slots[UNIFI_NO_OF_TX_QS];
+    u16 from_host_reserved_slots[UNIFI_NO_OF_TX_QS];
+
+    /* Parameters to determine if a queue was active.
+       If number of packets sent is greater than the threshold
+       for the queue, the queue is considered active and no
+       re reservation is done, it is important not to keep this
+       value too low */
+    /* Packets sent during this interval */
+    u16 packets_txed[UNIFI_NO_OF_TX_QS];
+    u16 total_packets_txed;
+
+    /* Number of packets to see if slots need to be reassigned */
+    u16 packets_interval;
+
+    /* Once a queue reaches a stable state, avoid processing */
+    u8 queue_stable[UNIFI_NO_OF_TX_QS];
+} card_dynamic_slot_t;
+
+
+/* These are type-safe and don't write incorrect values to the
+ * structure. */
+
+/* Return queue slots used count
+ *  params:
+ *      const q_t *q
+ *  returns:
+ *      u16
+ */
+#define CSR_WIFI_HIP_Q_SLOTS_USED(q)     \
+    (((q)->q_wr_ptr - (q)->q_rd_ptr < 0)? \
+     ((q)->q_wr_ptr - (q)->q_rd_ptr + (q)->q_length) : ((q)->q_wr_ptr - (q)->q_rd_ptr))
+
+/* Return queue slots free count
+ *  params:
+ *      const q_t *q
+ *  returns:
+ *      u16
+ */
+#define CSR_WIFI_HIP_Q_SLOTS_FREE(q)     \
+    ((q)->q_length - CSR_WIFI_HIP_Q_SLOTS_USED((q)) - 1)
+
+/* Return slot signal data pointer
+ *  params:
+ *      const q_t *q
+ *      u16 slot
+ *  returns:
+ *      card_signal_t *
+ */
+#define CSR_WIFI_HIP_Q_SLOT_DATA(q, slot)    \
+    ((q)->q_body + slot)
+
+/* Return queue next read slot
+ *  params:
+ *      const q_t *q
+ *  returns:
+ *      u16 slot offset
+ */
+#define CSR_WIFI_HIP_Q_NEXT_R_SLOT(q)    \
+    ((q)->q_rd_ptr)
+
+/* Return queue next write slot
+ *  params:
+ *      const q_t *q
+ *  returns:
+ *      u16 slot offset
+ */
+#define CSR_WIFI_HIP_Q_NEXT_W_SLOT(q)    \
+    ((q)->q_wr_ptr)
+
+/* Return updated queue pointer wrapped around its length
+ *  params:
+ *      const q_t *q
+ *      u16 x     amount to add to queue pointer
+ *  returns:
+ *      u16 wrapped queue pointer
+ */
+#define CSR_WIFI_HIP_Q_WRAP(q, x)    \
+    ((((x) >= (q)->q_length)?((x) % (q)->q_length) : (x)))
+
+/* Advance queue read pointer
+ *  params:
+ *      const q_t *q
+ */
+#define CSR_WIFI_HIP_Q_INC_R(q)  \
+    ((q)->q_rd_ptr = CSR_WIFI_HIP_Q_WRAP((q), (q)->q_rd_ptr + 1))
+
+/* Advance queue write pointer
+ *  params:
+ *      const q_t *q
+ */
+#define CSR_WIFI_HIP_Q_INC_W(q)  \
+    ((q)->q_wr_ptr = CSR_WIFI_HIP_Q_WRAP((q), (q)->q_wr_ptr + 1))
+
+enum unifi_host_state
+{
+    UNIFI_HOST_STATE_AWAKE   = 0,
+    UNIFI_HOST_STATE_DROWSY  = 1,
+    UNIFI_HOST_STATE_TORPID  = 2
+};
+
+typedef struct
+{
+    bulk_data_desc_t   bd;
+    unifi_TrafficQueue queue; /* Used for dynamic slot reservation */
+} slot_desc_t;
+
+/*
+ * Structure describing a UniFi SDIO card.
+ */
+struct card
+{
+    /*
+     * Back pointer for the higher level OS code. This is passed as
+     * an argument to callbacks (e.g. for received data and indications).
+     */
+    void *ospriv;
+
+    /*
+     * mapping of HIP slot to MA-PACKET.req host tag, the
+     * array is indexed by slot numbers and each index stores
+     * information of the last host tag it was used for
+     */
+    u32 *fh_slot_host_tag_record;
+
+
+    /* Info read from Symbol Table during probe */
+    u32     build_id;
+    char build_id_string[128];
+
+    /* Retrieve from SDIO driver. */
+    u16 chip_id;
+
+    /* Read from GBL_CHIP_VERSION. */
+    u16 chip_version;
+
+    /* From the SDIO driver (probably 1) */
+    u8 function;
+
+    /* This is sused to get the register addresses and things. */
+    ChipDescript *helper;
+
+    /*
+     * Bit mask of PIOs for the loader to waggle during download.
+     * We assume these are connected to LEDs. The main firmware gets
+     * the mask from a MIB entry.
+     */
+    s32 loader_led_mask;
+
+    /*
+     * Support for flow control. When the from-host queue of signals
+     * is full, we ask the host upper layer to stop sending packets. When
+     * the queue drains we tell it that it can send packets again.
+     * We use this flag to remember the current state.
+     */
+#define card_is_tx_q_paused(card, q)   (card->tx_q_paused_flag[q])
+#define card_tx_q_unpause(card, q)   (card->tx_q_paused_flag[q] = 0)
+#define card_tx_q_pause(card, q)   (card->tx_q_paused_flag[q] = 1)
+
+    u16 tx_q_paused_flag[UNIFI_TRAFFIC_Q_MAX + 1 + UNIFI_NO_OF_TX_QS]; /* defensive more than big enough */
+
+    /* UDI callback for logging UniFi interactions */
+    udi_func_t udi_hook;
+
+    u8 bh_reason_host;
+    u8 bh_reason_unifi;
+
+    /* SDIO clock speed request from OS layer */
+    u8 request_max_clock;
+
+    /* Last SDIO clock frequency set */
+    u32 sdio_clock_speed;
+
+    /*
+     * Current host state (copy of value in IOABORT register and
+     * spinlock to protect it.
+     */
+    enum unifi_host_state host_state;
+
+    enum unifi_low_power_mode     low_power_mode;
+    enum unifi_periodic_wake_mode periodic_wake_mode;
+
+    /*
+     * Ring buffer of signal structs for a queue of data packets from
+     * the host.
+     * The queue is empty when fh_data_q_num_rd == fh_data_q_num_wr.
+     * To add a packet to the queue, copy it to index given by
+     * (fh_data_q_num_wr%UNIFI_SOFT_Q_LENGTH) and advance fh_data_q_num_wr.
+     * To take a packet from the queue, copy data from index given by
+     * (fh_data_q_num_rd%UNIFI_SOFT_Q_LENGTH) and advance fh_data_q_num_rd.
+     * fh_data_q_num_rd and fh_data_q_num_rd are both modulo 256.
+     */
+    card_signal_t fh_command_q_body[UNIFI_SOFT_COMMAND_Q_LENGTH];
+    q_t           fh_command_queue;
+
+    card_signal_t fh_traffic_q_body[UNIFI_NO_OF_TX_QS][UNIFI_SOFT_TRAFFIC_Q_LENGTH];
+    q_t           fh_traffic_queue[UNIFI_NO_OF_TX_QS];
+
+    /*
+     * Signal counts from UniFi SDIO Control Data Structure.
+     * These are cached and synchronised with the UniFi before and after
+     * a batch of operations.
+     *
+     * These are the modulo-256 count of signals written to or read from UniFi
+     * The value is incremented for every signal.
+     */
+    s32 from_host_signals_w;
+    s32 from_host_signals_r;
+    s32 to_host_signals_r;
+    s32 to_host_signals_w;
+
+
+    /* Should specify buffer size as a number of signals */
+    /*
+     * Enough for 10 th and 10 fh data slots:
+     *   1 * 10 * 8 =  80
+     *   2 * 10 * 8 = 160
+     */
+#define UNIFI_FH_BUF_SIZE 1024
+    struct sigbuf
+    {
+        u8 *buf;     /* buffer area */
+        u8 *ptr;     /* current pos */
+        u16 count;   /* signal count */
+        u16 bufsize;
+    } fh_buffer;
+    struct sigbuf th_buffer;
+
+
+    /*
+     * Field to use for the incrementing value to write to the UniFi
+     * SHARED_IO_INTERRUPT register.
+     * Flag to say we need to generate an interrupt at end of processing.
+     */
+    u32 unifi_interrupt_seq;
+    u8  generate_interrupt;
+
+
+    /* Pointers to the bulk data slots */
+    slot_desc_t      *from_host_data;
+    bulk_data_desc_t *to_host_data;
+
+
+    /*
+     * Index of the next (hopefully) free data slot.
+     * This is an optimisation that starts searching at a more likely point
+     * than the beginning.
+     */
+    s16 from_host_data_head;
+
+    /* Dynamic slot allocation for queues */
+    card_dynamic_slot_t dynamic_slot_data;
+
+    /*
+     * SDIO specific fields
+     */
+
+    /* Interface pointer for the SDIO library */
+    CsrSdioFunction *sdio_if;
+
+    /* Copy of config_data struct from the card */
+    sdio_config_data_t config_data;
+
+    /* SDIO address of the Initialised flag and Control Data struct */
+    u32 init_flag_addr;
+    u32 sdio_ctrl_addr;
+
+    /* The last value written to the Shared Data Memory Page register */
+    u32 proc_select;
+    u32 dmem_page;
+    u32 pmem_page;
+
+    /* SDIO traffic counters limited to 32 bits for Synergy compatibility */
+    u32 sdio_bytes_read;
+    u32 sdio_bytes_written;
+
+    u8 memory_resources_allocated;
+
+    /* UniFi SDIO I/O Block size. */
+    u16 sdio_io_block_size;
+
+    /* Pad transfer sizes to SDIO block boundaries */
+    u8 sdio_io_block_pad;
+
+    /* Read from the XBV */
+    struct FWOV fwov;
+
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+    /* TA sampling */
+    ta_data_t ta_sampling;
+#endif
+
+    /* Auto-coredump */
+    s16             request_coredump_on_reset; /* request coredump on next reset */
+    struct coredump_buf *dump_buf;                  /* root node */
+    struct coredump_buf *dump_next_write;           /* node to fill at next dump */
+    struct coredump_buf *dump_cur_read;             /* valid node to read, or NULL */
+
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+    struct cmd_profile
+    {
+        u32 cmd52_count;
+        u32 cmd53_count;
+        u32 tx_count;
+        u32 tx_cfm_count;
+        u32 rx_count;
+        u32 bh_count;
+        u32 process_count;
+        u32 protocol_count;
+
+        u32 cmd52_f0_r_count;
+        u32 cmd52_f0_w_count;
+        u32 cmd52_r8or16_count;
+        u32 cmd52_w8or16_count;
+        u32 cmd52_r16_count;
+        u32 cmd52_w16_count;
+        u32 cmd52_r32_count;
+
+        u32 sdio_cmd_signal;
+        u32 sdio_cmd_clear_slot;
+        u32 sdio_cmd_to_host;
+        u32 sdio_cmd_from_host;
+        u32 sdio_cmd_from_host_and_clear;
+    } hip_prof;
+    struct cmd_profile cmd_prof;
+#endif
+
+    /* Interrupt processing mode flags */
+    u32 intmode;
+
+#ifdef UNIFI_DEBUG
+    u8 lsb;
+#endif
+
+    /* Historic firmware panic codes */
+    u32 panic_data_phy_addr;
+    u32 panic_data_mac_addr;
+    u16 last_phy_panic_code;
+    u16 last_phy_panic_arg;
+    u16 last_mac_panic_code;
+    u16 last_mac_panic_arg;
+#ifdef CSR_PRE_ALLOC_NET_DATA
+    bulk_data_desc_t bulk_data_desc_list[BULK_DATA_PRE_ALLOC_NUM];
+    u16        prealloc_netdata_r;
+    u16        prealloc_netdata_w;
+#endif
+}; /* struct card */
+
+
+/* Reset types */
+enum unifi_reset_type
+{
+    UNIFI_COLD_RESET = 1,
+    UNIFI_WARM_RESET = 2
+};
+
+/*
+ * unifi_set_host_state() implements signalling for waking UniFi from
+ * deep sleep. The host indicates to UniFi that it is in one of three states:
+ *   Torpid - host has nothing to send, UniFi can go to sleep.
+ *   Drowsy - host has data to send to UniFi. UniFi will respond with an
+ *            SDIO interrupt. When hosts responds it moves to Awake.
+ *   Awake  - host has data to transfer, UniFi must stay awake.
+ *            When host has finished, it moves to Torpid.
+ */
+CsrResult unifi_set_host_state(card_t *card, enum unifi_host_state state);
+
+
+CsrResult unifi_set_proc_select(card_t *card, enum unifi_dbg_processors_select select);
+s32 card_read_signal_counts(card_t *card);
+bulk_data_desc_t* card_find_data_slot(card_t *card, s16 slot);
+
+
+CsrResult unifi_read32(card_t *card, u32 unifi_addr, u32 *pdata);
+CsrResult unifi_readnz(card_t *card, u32 unifi_addr,
+                       void *pdata, u16 len);
+s32 unifi_read_shared_count(card_t *card, u32 addr);
+
+CsrResult unifi_writen(card_t *card, u32 unifi_addr, void *pdata, u16 len);
+
+CsrResult unifi_bulk_rw(card_t *card, u32 handle,
+                        void *pdata, u32 len, s16 direction);
+CsrResult unifi_bulk_rw_noretry(card_t *card, u32 handle,
+                                void *pdata, u32 len, s16 direction);
+#define UNIFI_SDIO_READ       0
+#define UNIFI_SDIO_WRITE      1
+
+CsrResult unifi_read_8_or_16(card_t *card, u32 unifi_addr, u8 *pdata);
+CsrResult unifi_write_8_or_16(card_t *card, u32 unifi_addr, u8 data);
+CsrResult unifi_read_direct_8_or_16(card_t *card, u32 addr, u8 *pdata);
+CsrResult unifi_write_direct_8_or_16(card_t *card, u32 addr, u8 data);
+
+CsrResult unifi_read_direct16(card_t *card, u32 addr, u16 *pdata);
+CsrResult unifi_read_direct32(card_t *card, u32 addr, u32 *pdata);
+CsrResult unifi_read_directn(card_t *card, u32 addr, void *pdata, u16 len);
+
+CsrResult unifi_write_direct16(card_t *card, u32 addr, u16 data);
+CsrResult unifi_write_directn(card_t *card, u32 addr, void *pdata, u16 len);
+
+CsrResult sdio_read_f0(card_t *card, u32 addr, u8 *pdata);
+CsrResult sdio_write_f0(card_t *card, u32 addr, u8 data);
+
+void unifi_read_panic(card_t *card);
+#ifdef CSR_PRE_ALLOC_NET_DATA
+void prealloc_netdata_free(card_t *card);
+CsrResult prealloc_netdata_alloc(card_t *card);
+#endif
+/* For diagnostic use */
+void dump(void *mem, u16 len);
+void dump16(void *mem, u16 len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CARD_SDIO_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c b/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c
new file mode 100644
index 00000000000..97f645c0681
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio_intr.c
@@ -0,0 +1,2595 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     csr_wifi_hip_card_sdio_intr.c
+ *
+ *  PURPOSE:
+ *      Interrupt processing for the UniFi SDIO driver.
+ *
+ *      We may need another signal queue of responses to UniFi to hold
+ *      bulk data commands generated by read_to_host_signals().
+ *
+ * ---------------------------------------------------------------------------
+ */
+#undef CSR_WIFI_HIP_NOISY
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "csr_wifi_hip_card.h"
+#include "csr_wifi_hip_xbv.h"
+
+
+/*
+ * If the SDIO link is idle for this time (in milliseconds),
+ * signal UniFi to go into Deep Sleep.
+ * Valid return value of unifi_bh().
+ */
+#define UNIFI_DEFAULT_HOST_IDLE_TIMEOUT 5
+/*
+ * If the UniFi has not woken up for this time (in milliseconds),
+ * signal the bottom half to take action.
+ * Valid return value of unifi_bh().
+ */
+#define UNIFI_DEFAULT_WAKE_TIMEOUT      1000
+
+
+static CsrResult process_bh(card_t *card);
+static CsrResult handle_host_protocol(card_t *card, u8 *processed_something);
+
+static CsrResult flush_fh_buffer(card_t *card);
+
+static CsrResult check_fh_sig_slots(card_t *card, u16 needed, s32 *space);
+
+static CsrResult read_to_host_signals(card_t *card, s32 *processed);
+static CsrResult process_to_host_signals(card_t *card, s32 *processed);
+
+static CsrResult process_bulk_data_command(card_t *card,
+                                           const u8 *cmdptr,
+                                           s16 cmd, u16 len);
+static CsrResult process_clear_slot_command(card_t         *card,
+                                            const u8 *cmdptr);
+static CsrResult process_fh_cmd_queue(card_t *card, s32 *processed);
+static CsrResult process_fh_traffic_queue(card_t *card, s32 *processed);
+static void restart_packet_flow(card_t *card);
+static CsrResult process_clock_request(card_t *card);
+
+#ifdef CSR_WIFI_HIP_NOISY
+s16 dump_fh_buf = 0;
+#endif /* CSR_WIFI_HIP_NOISY */
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+
+/*
+ * The unifi_debug_output buffer can be used to debug the HIP behaviour offline
+ * i.e. without using the tracing functions that change the timing.
+ *
+ * Call unifi_debug_log_to_buf() with printf arguments to store a string into
+ * unifi_debug_output. When unifi_debug_buf_dump() is called, the contents of the
+ * buffer are dumped with dump_str() which has to be implemented in the
+ * OS layer, during the porting exercise. The offset printed, holds the
+ * offset where the last character is (always a zero).
+ *
+ */
+
+#define UNIFI_DEBUG_GBUFFER_SIZE       8192
+static char unifi_debug_output[UNIFI_DEBUG_GBUFFER_SIZE];
+static char *unifi_dbgbuf_ptr = unifi_debug_output;
+static char *unifi_dbgbuf_start = unifi_debug_output;
+
+static void append_char(char c)
+{
+    /* write char and advance pointer */
+    *unifi_dbgbuf_ptr++ = c;
+    /* wrap pointer at end of buffer */
+    if ((unifi_dbgbuf_ptr - unifi_debug_output) >= UNIFI_DEBUG_GBUFFER_SIZE)
+    {
+        unifi_dbgbuf_ptr = unifi_debug_output;
+    }
+} /* append_char() */
+
+
+void unifi_debug_string_to_buf(const char *str)
+{
+    const char *p = str;
+    while (*p)
+    {
+        append_char(*p);
+        p++;
+    }
+    /* Update start-of-buffer pointer */
+    unifi_dbgbuf_start = unifi_dbgbuf_ptr + 1;
+    if ((unifi_dbgbuf_start - unifi_debug_output) >= UNIFI_DEBUG_GBUFFER_SIZE)
+    {
+        unifi_dbgbuf_start = unifi_debug_output;
+    }
+}
+
+
+void unifi_debug_log_to_buf(const char *fmt, ...)
+{
+#define DEBUG_BUFFER_SIZE       80
+    static char s[DEBUG_BUFFER_SIZE];
+    va_list args;
+
+    va_start(args, fmt);
+    vsnprintf(s, DEBUG_BUFFER_SIZE, fmt, args);
+    va_end(args);
+
+    unifi_debug_string_to_buf(s);
+} /* unifi_debug_log_to_buf() */
+
+
+/* Convert signed 32 bit (or less) integer to string */
+static void CsrUInt16ToHex(u16 number, char *str)
+{
+    u16 index;
+    u16 currentValue;
+
+    for (index = 0; index < 4; index++)
+    {
+        currentValue = (u16) (number & 0x000F);
+        number >>= 4;
+        str[3 - index] = (char) (currentValue > 9 ? currentValue + 55 : currentValue + '0');
+    }
+    str[4] = '\0';
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_debug_hex_to_buf
+ *
+ *  puts the contents of the passed buffer into the debug buffer as a hex string
+ *
+ *  Arguments:
+ *      buff         buffer to print as hex
+ *      length       number of chars to print
+ *
+ *  Returns:
+ *      None.
+ *
+ * ---------------------------------------------------------------------------
+ */
+void unifi_debug_hex_to_buf(const char *buff, u16 length)
+{
+    char s[5];
+    u16 i;
+
+    for (i = 0; i < length; i = i + 2)
+    {
+        CsrUInt16ToHex(*((u16 *)(buff + i)), s);
+        unifi_debug_string_to_buf(s);
+    }
+}
+
+
+void unifi_debug_buf_dump(void)
+{
+    s32 offset = unifi_dbgbuf_ptr - unifi_debug_output;
+
+    unifi_error(NULL, "HIP debug buffer offset=%d\n", offset);
+    dump_str(unifi_debug_output + offset, UNIFI_DEBUG_GBUFFER_SIZE - offset);
+    dump_str(unifi_debug_output, offset);
+} /* unifi_debug_buf_dump() */
+
+
+#endif /* CSR_WIFI_HIP_DEBUG_OFFLINE */
+
+#ifdef CSR_PRE_ALLOC_NET_DATA
+#define NETDATA_PRE_ALLOC_BUF_SIZE 8000
+
+void prealloc_netdata_free(card_t *card)
+{
+    unifi_warning(card->ospriv, "prealloc_netdata_free: IN: w=%d r=%d\n", card->prealloc_netdata_w, card->prealloc_netdata_r);
+
+    while (card->bulk_data_desc_list[card->prealloc_netdata_r].data_length != 0)
+    {
+        unifi_warning(card->ospriv, "prealloc_netdata_free: r=%d\n", card->prealloc_netdata_r);
+
+        unifi_net_data_free(card->ospriv, &card->bulk_data_desc_list[card->prealloc_netdata_r]);
+        card->prealloc_netdata_r++;
+        card->prealloc_netdata_r %= BULK_DATA_PRE_ALLOC_NUM;
+    }
+    card->prealloc_netdata_r = card->prealloc_netdata_w = 0;
+
+    unifi_warning(card->ospriv, "prealloc_netdata_free: OUT: w=%d r=%d\n", card->prealloc_netdata_w, card->prealloc_netdata_r);
+}
+
+
+CsrResult prealloc_netdata_alloc(card_t *card)
+{
+    CsrResult r;
+
+    unifi_trace(card->ospriv, UDBG5, "prealloc_netdata_alloc: IN: w=%d r=%d\n", card->prealloc_netdata_w, card->prealloc_netdata_r);
+
+    while (card->bulk_data_desc_list[card->prealloc_netdata_w].data_length == 0)
+    {
+        r = unifi_net_data_malloc(card->ospriv, &card->bulk_data_desc_list[card->prealloc_netdata_w], NETDATA_PRE_ALLOC_BUF_SIZE);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "prealloc_netdata_alloc: Failed to allocate t-h bulk data\n");
+            return CSR_RESULT_FAILURE;
+        }
+        card->prealloc_netdata_w++;
+        card->prealloc_netdata_w %= BULK_DATA_PRE_ALLOC_NUM;
+    }
+    unifi_trace(card->ospriv, UDBG5, "prealloc_netdata_alloc: OUT: w=%d r=%d\n", card->prealloc_netdata_w, card->prealloc_netdata_r);
+
+    return CSR_RESULT_SUCCESS;
+}
+
+
+static CsrResult prealloc_netdata_get(card_t *card, bulk_data_desc_t *bulk_data_slot, u32 size)
+{
+    CsrResult r;
+
+    unifi_trace(card->ospriv, UDBG5, "prealloc_netdata_get: IN: w=%d r=%d\n", card->prealloc_netdata_w, card->prealloc_netdata_r);
+
+    if (card->bulk_data_desc_list[card->prealloc_netdata_r].data_length == 0)
+    {
+        unifi_error(card->ospriv, "prealloc_netdata_get: data_length = 0\n");
+    }
+
+    if ((size > NETDATA_PRE_ALLOC_BUF_SIZE) || (card->bulk_data_desc_list[card->prealloc_netdata_r].data_length == 0))
+    {
+        unifi_warning(card->ospriv, "prealloc_netdata_get: Calling net_data_malloc\n");
+
+        r = unifi_net_data_malloc(card->ospriv, bulk_data_slot, size);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "prealloc_netdata_get: Failed to allocate t-h bulk data\n");
+            return CSR_RESULT_FAILURE;
+        }
+        return CSR_RESULT_SUCCESS;
+    }
+
+    *bulk_data_slot = card->bulk_data_desc_list[card->prealloc_netdata_r];
+    card->bulk_data_desc_list[card->prealloc_netdata_r].os_data_ptr = NULL;
+    card->bulk_data_desc_list[card->prealloc_netdata_r].os_net_buf_ptr = NULL;
+    card->bulk_data_desc_list[card->prealloc_netdata_r].net_buf_length = 0;
+    card->bulk_data_desc_list[card->prealloc_netdata_r].data_length = 0;
+
+    card->prealloc_netdata_r++;
+    card->prealloc_netdata_r %= BULK_DATA_PRE_ALLOC_NUM;
+
+    unifi_trace(card->ospriv, UDBG5, "prealloc_netdata_get: OUT: w=%d r=%d\n", card->prealloc_netdata_w, card->prealloc_netdata_r);
+
+    return CSR_RESULT_SUCCESS;
+}
+
+
+#endif
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_sdio_interrupt_handler
+ *
+ *      This function should be called by the OS-dependent code to handle
+ *      an SDIO interrupt from the UniFi.
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes: This function may be called in DRS context. In this case,
+ *         tracing with the unifi_trace(), etc, is not allowed.
+ * ---------------------------------------------------------------------------
+ */
+void unifi_sdio_interrupt_handler(card_t *card)
+{
+    /*
+     * Set the flag to say reason for waking was SDIO interrupt.
+     * Then ask the OS layer to run the unifi_bh to give attention to the UniFi.
+     */
+    card->bh_reason_unifi = 1;
+    (void)unifi_run_bh(card->ospriv);
+} /*  sdio_interrupt_handler() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_configure_low_power_mode
+ *
+ *      This function should be called by the OS-dependent when
+ *      the deep sleep signaling needs to be enabled or disabled.
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *      low_power_mode  Disable/Enable the deep sleep signaling
+ *      periodic_wake_mode UniFi wakes host periodically.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or a CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_configure_low_power_mode(card_t                       *card,
+                                         enum unifi_low_power_mode     low_power_mode,
+                                         enum unifi_periodic_wake_mode periodic_wake_mode)
+{
+    card->low_power_mode = low_power_mode;
+    card->periodic_wake_mode = periodic_wake_mode;
+
+    unifi_trace(card->ospriv, UDBG1,
+                "unifi_configure_low_power_mode: new mode = %s, wake_host = %s\n",
+                (low_power_mode == UNIFI_LOW_POWER_DISABLED)?"disabled" : "enabled",
+                (periodic_wake_mode == UNIFI_PERIODIC_WAKE_HOST_DISABLED)?"FALSE" : "TRUE");
+
+    (void)unifi_run_bh(card->ospriv);
+    return CSR_RESULT_SUCCESS;
+} /* unifi_configure_low_power_mode() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_force_low_power_mode
+ *
+ *      This function should be called by the OS-dependent when
+ *      UniFi needs to be set to the low power mode (e.g. on suspend)
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or a CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_force_low_power_mode(card_t *card)
+{
+    if (card->low_power_mode == UNIFI_LOW_POWER_DISABLED)
+    {
+        unifi_error(card->ospriv, "Attempt to set mode to TORPID when lower power mode is disabled\n");
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    return unifi_set_host_state(card, UNIFI_HOST_STATE_TORPID);
+} /* unifi_force_low_power_mode() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_bh
+ *
+ *      This function should be called by the OS-dependent code when
+ *      host and/or UniFi has requested an exchange of messages.
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or a CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_bh(card_t *card, u32 *remaining)
+{
+    CsrResult r;
+    CsrResult csrResult;
+    u8 pending;
+    s32 iostate, j;
+    const enum unifi_low_power_mode low_power_mode = card->low_power_mode;
+    u16 data_slots_used = 0;
+
+
+    /* Process request to raise the maximum SDIO clock */
+    r = process_clock_request(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Error setting maximum SDIO clock\n");
+        goto exit;
+    }
+
+    /*
+     * Why was the BH thread woken?
+     * If it was an SDIO interrupt, UniFi is awake and we need to process it.
+     * If it was a host process queueing data, then we need to awaken UniFi.
+     *
+     * Priority of flags is top down.
+     *
+     * ----------------------------------------------------------+
+     *    \state|   AWAKE      |    DROWSY      |    TORPID      |
+     * flag\    |              |                |                |
+     * ---------+--------------+----------------+----------------|
+     *          | do the host  | go to AWAKE and| go to AWAKE and|
+     *   unifi  | protocol     | do the host    | do the host    |
+     *          |              | protocol       | protocol       |
+     * ---------+--------------+----------------+----------------|
+     *          | do the host  |                |                |
+     *   host   | protocol     |  do nothing    | go to DROWSY   |
+     *          |              |                |                |
+     * ---------+--------------+----------------+----------------|
+     *          |              |                | should not     |
+     *  timeout | go to TORPID | error, unifi   | occur          |
+     *          |              | didn't wake up | do nothing     |
+     * ----------------------------------------------------------+
+     *
+     * Note that if we end up in the AWAKE state we always do the host protocol.
+     */
+
+    do
+    {
+        /*
+         * When the host state is set to DROWSY, then we can not disable the
+         * interrupts as UniFi can generate an interrupt even when the INT_ENABLE
+         * register has the interrupts disabled. This interrupt will be lost.
+         */
+        if (card->host_state == UNIFI_HOST_STATE_DROWSY || card->host_state == UNIFI_HOST_STATE_TORPID)
+        {
+            u8 reason_unifi;
+
+            /*
+             * An interrupt may occur while or after we cache the reason.
+             * This interrupt will cause the unifi_bh() to be scheduled again.
+             * Any interrupt that has happened before the register is read
+             * and is considered spurious has to acknowledged.
+             */
+            reason_unifi = card->bh_reason_unifi;
+
+            /*
+             * If an interrupt is received, check if it was a real one,
+             * set the host state to AWAKE and run the BH.
+             */
+            r = CardPendingInt(card, &pending);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                goto exit;
+            }
+
+            if (pending)
+            {
+                unifi_trace(card->ospriv, UDBG5,
+                            "UNIFI_HOST_STATE_%s: Set state to AWAKE.\n",
+                            (card->host_state == UNIFI_HOST_STATE_TORPID)?"TORPID" : "DROWSY");
+
+                r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+                if (r == CSR_RESULT_SUCCESS)
+                {
+                    (*remaining) = 0;
+                    break;
+                }
+            }
+            else if (reason_unifi)
+            {
+                CsrSdioInterruptAcknowledge(card->sdio_if);
+            }
+
+            /*
+             * If an chip is in TORPID, and the host wants to wake it up,
+             * set the host state to DROWSY and wait for the wake-up interrupt.
+             */
+            if ((card->host_state == UNIFI_HOST_STATE_TORPID) && card->bh_reason_host)
+            {
+                r = unifi_set_host_state(card, UNIFI_HOST_STATE_DROWSY);
+                if (r == CSR_RESULT_SUCCESS)
+                {
+                    /*
+                     * set the timeout value to UNIFI_DEFAULT_WAKE_TIMEOUT
+                     * to capture a wake error.
+                     */
+                    card->bh_reason_host = 0;
+                    (*remaining) = UNIFI_DEFAULT_WAKE_TIMEOUT;
+                    return CSR_RESULT_SUCCESS;
+                }
+
+                goto exit;
+            }
+
+            /*
+             * If the chip is in DROWSY, and the timeout expires,
+             * we need to reset the chip. This should never occur.
+             * (If it does, check that the calling thread set "remaining"
+             * according to the time remaining when unifi_bh() was called).
+             */
+            if ((card->host_state == UNIFI_HOST_STATE_DROWSY) && ((*remaining) == 0))
+            {
+                unifi_error(card->ospriv, "UniFi did not wake up on time...\n");
+
+                /*
+                 * Check if Function1 has gone away or
+                 * if we missed an SDIO interrupt.
+                 */
+                r = unifi_check_io_status(card, &iostate);
+                if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+                {
+                    goto exit;
+                }
+                /* Need to reset and reboot */
+                return CSR_RESULT_FAILURE;
+            }
+        }
+        else
+        {
+            if (card->bh_reason_unifi || card->bh_reason_host)
+            {
+                break;
+            }
+
+            if (((*remaining) == 0) && (low_power_mode == UNIFI_LOW_POWER_ENABLED))
+            {
+                r = unifi_set_host_state(card, UNIFI_HOST_STATE_TORPID);
+                if (r == CSR_RESULT_SUCCESS)
+                {
+                    (*remaining) = 0;
+                    return CSR_RESULT_SUCCESS;
+                }
+
+                goto exit;
+            }
+        }
+
+        /* No need to run the host protocol */
+        return CSR_RESULT_SUCCESS;
+    } while (0);
+
+
+    /* Disable the SDIO interrupts while doing SDIO ops */
+    csrResult = CsrSdioInterruptDisable(card->sdio_if);
+    if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        r = CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        goto exit;
+    }
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+        unifi_error(card->ospriv, "Failed to disable SDIO interrupts. unifi_bh queues error.\n");
+        goto exit;
+    }
+
+    /* Now that the interrupts are disabled, ack the interrupt */
+    CsrSdioInterruptAcknowledge(card->sdio_if);
+
+    /* Run the HIP */
+    r = process_bh(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        goto exit;
+    }
+
+    /*
+     * If host is now idle, schedule a timer for the delay before we
+     * let UniFi go into deep sleep.
+     * If the timer goes off, we will move to TORPID state.
+     * If UniFi raises an interrupt in the meantime, we will cancel
+     * the timer and start a new one when we become idle.
+     */
+    for (j = 0; j < UNIFI_NO_OF_TX_QS; j++)
+    {
+        data_slots_used += CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_traffic_queue[j]);
+    }
+
+    if ((low_power_mode == UNIFI_LOW_POWER_ENABLED) && (data_slots_used == 0))
+    {
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+        if (card->ta_sampling.traffic_type != CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_PERIODIC)
+        {
+#endif
+        /* return the UNIFI_DEFAULT_HOST_IDLE_TIMEOUT, so we can go to sleep. */
+        unifi_trace(card->ospriv, UDBG5,
+                    "Traffic is not periodic, set timer for TORPID.\n");
+        (*remaining) = UNIFI_DEFAULT_HOST_IDLE_TIMEOUT;
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+    }
+    else
+    {
+        unifi_trace(card->ospriv, UDBG5,
+                    "Traffic is periodic, set unifi to TORPID immediately.\n");
+        if (CardAreAllFromHostDataSlotsEmpty(card) == 1)
+        {
+            r = unifi_set_host_state(card, UNIFI_HOST_STATE_TORPID);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                goto exit;
+            }
+        }
+    }
+#endif
+    }
+
+    csrResult = CsrSdioInterruptEnable(card->sdio_if);
+    if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+    {
+        r = CSR_WIFI_HIP_RESULT_NO_DEVICE;
+    }
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+        unifi_error(card->ospriv, "Failed to enable SDIO interrupt\n");
+    }
+
+exit:
+
+    unifi_trace(card->ospriv, UDBG4, "New state=%d\n", card->host_state);
+
+    if (r != CSR_RESULT_SUCCESS)
+    {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        unifi_debug_buf_dump();
+#endif
+        /* If an interrupt has been raised, ack it here */
+        if (card->bh_reason_unifi)
+        {
+            CsrSdioInterruptAcknowledge(card->sdio_if);
+        }
+
+        unifi_error(card->ospriv,
+                    "unifi_bh: state=%d %c, clock=%dkHz, interrupt=%d host=%d, power_save=%s\n",
+                    card->host_state,
+                    (card->host_state == UNIFI_HOST_STATE_AWAKE)?'A' : (card->host_state == UNIFI_HOST_STATE_DROWSY)?'D' : 'T',
+                    card->sdio_clock_speed / 1000,
+                    card->bh_reason_unifi, card->bh_reason_host,
+                    (low_power_mode == UNIFI_LOW_POWER_DISABLED)?"disabled" : "enabled");
+
+        /* Try to capture firmware panic codes */
+        (void)unifi_capture_panic(card);
+
+        /* Ask for a mini-coredump when the driver has reset UniFi */
+        (void)unifi_coredump_request_at_next_reset(card, 1);
+    }
+
+    return r;
+} /* unifi_bh() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_clock_request
+ *
+ *      Handle request from the OS layer to increase the SDIO clock speed.
+ *      The fast clock is limited until the firmware has indicated that it has
+ *      completed initialisation to the OS layer.
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_clock_request(card_t *card)
+{
+    CsrResult r = CSR_RESULT_SUCCESS;
+    CsrResult csrResult;
+
+    if (!card->request_max_clock)
+    {
+        return CSR_RESULT_SUCCESS;   /* No pending request */
+    }
+
+    /*
+     * The SDIO clock speed request from the OS layer is only acted upon if
+     * the UniFi is awake. If it was in any other state, the clock speed will
+     * transition through SAFE to MAX while the host wakes it up, and the
+     * final speed reached will be UNIFI_SDIO_CLOCK_MAX_HZ.
+     * This assumes that the SME never requests low power mode while the f/w
+     * initialisation takes place.
+     */
+    if (card->host_state == UNIFI_HOST_STATE_AWAKE)
+    {
+        unifi_trace(card->ospriv, UDBG1, "Set SDIO max clock\n");
+        csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_MAX_HZ);
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+        }
+        else
+        {
+            card->sdio_clock_speed = UNIFI_SDIO_CLOCK_MAX_HZ;  /* log the new freq */
+        }
+    }
+    else
+    {
+        unifi_trace(card->ospriv, UDBG1, "Will set SDIO max clock after wakeup\n");
+    }
+
+    /* Cancel the request now that it has been acted upon, or is about to be
+     * by the wakeup mechanism
+     */
+    card->request_max_clock = 0;
+
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_bh
+ *
+ *      Exchange messages with UniFi
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_bh(card_t *card)
+{
+    CsrResult r;
+    u8 more;
+    more = FALSE;
+
+    /* Process the reasons (interrupt, signals) */
+    do
+    {
+        /*
+         * Run in a while loop, to save clearing the interrupts
+         * every time around the outside loop.
+         */
+        do
+        {
+            /* If configured to run the HIP just once, skip first loop */
+            if (card->intmode & CSR_WIFI_INTMODE_RUN_BH_ONCE)
+            {
+                break;
+            }
+
+            r = handle_host_protocol(card, &more);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+            unifi_debug_log_to_buf("c52=%d c53=%d tx=%d txc=%d rx=%d s=%d t=%d fc=%d\n",
+                                   card->cmd_prof.cmd52_count,
+                                   card->cmd_prof.cmd53_count,
+                                   card->cmd_prof.tx_count,
+                                   card->cmd_prof.tx_cfm_count,
+                                   card->cmd_prof.rx_count,
+                                   card->cmd_prof.sdio_cmd_signal,
+                                   card->cmd_prof.sdio_cmd_to_host,
+                                   card->cmd_prof.sdio_cmd_from_host_and_clear
+                                   );
+
+            card->cmd_prof.cmd52_count = card->cmd_prof.cmd53_count = 0;
+            card->cmd_prof.tx_count = card->cmd_prof.tx_cfm_count = card->cmd_prof.rx_count = 0;
+
+            card->cmd_prof.cmd52_f0_r_count = 0;
+            card->cmd_prof.cmd52_f0_w_count = 0;
+            card->cmd_prof.cmd52_r8or16_count = 0;
+            card->cmd_prof.cmd52_w8or16_count = 0;
+            card->cmd_prof.cmd52_r16_count = 0;
+            card->cmd_prof.cmd52_w16_count = 0;
+            card->cmd_prof.cmd52_r32_count = 0;
+
+            card->cmd_prof.sdio_cmd_signal = 0;
+            card->cmd_prof.sdio_cmd_clear_slot = 0;
+            card->cmd_prof.sdio_cmd_to_host = 0;
+            card->cmd_prof.sdio_cmd_from_host = 0;
+            card->cmd_prof.sdio_cmd_from_host_and_clear = 0;
+#endif
+
+
+        } while (more || card->bh_reason_unifi || card->bh_reason_host);
+
+        /* Acknowledge the h/w interrupt */
+        r = CardClearInt(card);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to acknowledge interrupt.\n");
+            return r;
+        }
+
+        /*
+         * UniFi may have tried to generate an interrupt during the
+         * CardClearInt() was running. So, we need to run the host
+         * protocol again, to check if there are any pending requests.
+         */
+        r = handle_host_protocol(card, &more);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return r;
+        }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+        unifi_debug_log_to_buf("c52=%d c53=%d tx=%d txc=%d rx=%d s=%d t=%d fc=%d\n",
+                               card->cmd_prof.cmd52_count,
+                               card->cmd_prof.cmd53_count,
+                               card->cmd_prof.tx_count,
+                               card->cmd_prof.tx_cfm_count,
+                               card->cmd_prof.rx_count,
+                               card->cmd_prof.sdio_cmd_signal,
+                               card->cmd_prof.sdio_cmd_to_host,
+                               card->cmd_prof.sdio_cmd_from_host_and_clear
+                               );
+
+        card->cmd_prof.cmd52_count = card->cmd_prof.cmd53_count = 0;
+        card->cmd_prof.tx_count = card->cmd_prof.tx_cfm_count = card->cmd_prof.rx_count = 0;
+
+        card->cmd_prof.cmd52_f0_r_count = 0;
+        card->cmd_prof.cmd52_f0_w_count = 0;
+        card->cmd_prof.cmd52_r8or16_count = 0;
+        card->cmd_prof.cmd52_w8or16_count = 0;
+        card->cmd_prof.cmd52_r16_count = 0;
+        card->cmd_prof.cmd52_w16_count = 0;
+        card->cmd_prof.cmd52_r32_count = 0;
+
+        card->cmd_prof.sdio_cmd_signal = 0;
+        card->cmd_prof.sdio_cmd_clear_slot = 0;
+        card->cmd_prof.sdio_cmd_to_host = 0;
+        card->cmd_prof.sdio_cmd_from_host = 0;
+        card->cmd_prof.sdio_cmd_from_host_and_clear = 0;
+#endif
+        /* If configured to run the HIP just once, work is now done */
+        if (card->intmode & CSR_WIFI_INTMODE_RUN_BH_ONCE)
+        {
+            break;
+        }
+
+    } while (more || card->bh_reason_unifi || card->bh_reason_host);
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    if ((card->intmode & CSR_WIFI_INTMODE_RUN_BH_ONCE) == 0)
+    {
+        unifi_debug_log_to_buf("proc=%d\n",
+                               card->cmd_prof.process_count);
+    }
+#endif
+
+    return CSR_RESULT_SUCCESS;
+} /* process_bh() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  handle_host_protocol
+ *
+ *      This function implements the Host Interface Protocol (HIP) as
+ *      described in the Host Interface Protocol Specification.
+ *
+ *  Arguments:
+ *      card                 Pointer to card context structure.
+ *      processed_something  Pointer to location to update processing status:
+ *                              TRUE when data was transferred
+ *                              FALSE when no data was transferred (queues empty)
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or CSR error code.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult handle_host_protocol(card_t *card, u8 *processed_something)
+{
+    CsrResult r;
+    s32 done;
+
+    *processed_something = FALSE;
+
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv, "   ========================     \n");
+#endif /* CSR_WIFI_HIP_NOISY */
+
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+    card->cmd_prof.process_count++;
+#endif
+
+    card->bh_reason_unifi = card->bh_reason_host = 0;
+    card->generate_interrupt = 0;
+
+
+    /*
+     * (Re)fill the T-H signal buffer
+     */
+    r = read_to_host_signals(card, &done);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Error occured reading to-host signals\n");
+        return r;
+    }
+    if (done > 0)
+    {
+        *processed_something = TRUE;
+    }
+
+    /*
+     * Process any to-host signals.
+     * Perform any requested CMD53 transfers here, but just queue any
+     * bulk data command responses.
+     */
+    r = process_to_host_signals(card, &done);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Error occured processing to-host signals\n");
+        return r;
+    }
+
+    /* Now send any signals in the F-H queues */
+    /* Give precedence to the command queue */
+    r = process_fh_cmd_queue(card, &done);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Error occured processing from-host signals\n");
+        return r;
+    }
+    if (done > 0)
+    {
+        *processed_something = TRUE;
+    }
+
+    r = process_fh_traffic_queue(card, &done);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Error occured processing from-host data signals\n");
+        return r;
+    }
+    if (done > 0)
+    {
+        *processed_something = TRUE;
+    }
+
+    /* Flush out the batch of signals to the UniFi. */
+    r = flush_fh_buffer(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to copy from-host signals to UniFi\n");
+        return r;
+    }
+
+
+    /*
+     * Send the host interrupt to say the queues have been modified.
+     */
+    if (card->generate_interrupt)
+    {
+        r = CardGenInt(card);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to notify UniFi that queues have been modified.\n");
+            return r;
+        }
+    }
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+#ifdef CSR_WIFI_RX_PATH_SPLIT_DONT_USE_WQ
+    unifi_rx_queue_flush(card->ospriv);
+#endif
+#endif
+
+    /* See if we can re-enable transmission now */
+    restart_packet_flow(card);
+
+#ifdef CSR_PRE_ALLOC_NET_DATA
+    r = prealloc_netdata_alloc(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "prealloc_netdata failed\n");
+        return r;
+    }
+#endif
+
+    /*
+     * Don't put the thread sleep if we just interacted with the chip,
+     * there might be more to do if we look again.
+     */
+    return r;
+} /* handle_host_protocol() */
+
+
+/*
+ *      Rounds the given signal length in bytes to a whole number
+ *      of sig_frag_size.
+ */
+#define GET_CHUNKS_FOR(SIG_FRAG_SIZE, LENGTH) (((LENGTH) + ((SIG_FRAG_SIZE)-1)) / (SIG_FRAG_SIZE))
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  read_to_host_signals
+ *
+ *      Read everything pending in the UniFi TH signal buffer.
+ *      Only do it if the local buffer is empty.
+ *
+ *  Arguments:
+ *      card        Pointer to card context struct
+ *      processed   Number of signals read:
+ *                      0 if there were no signals pending,
+ *                      1 if we read at least one signal
+ *  Returns:
+ *      CSR error code if an error occurred.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult read_to_host_signals(card_t *card, s32 *processed)
+{
+    s32 count_thw, count_thr;
+    s32 unread_chunks, unread_bytes;
+    CsrResult r;
+
+    *processed = 0;
+
+    /* Read any pending signals or bulk data commands */
+    count_thw = unifi_read_shared_count(card, card->sdio_ctrl_addr + 4);
+    if (count_thw < 0)
+    {
+        unifi_error(card->ospriv, "Failed to read to-host sig written count\n");
+        return CSR_RESULT_FAILURE;
+    }
+    card->to_host_signals_w = count_thw; /* diag */
+
+    count_thr = card->to_host_signals_r;
+
+    if (count_thw == count_thr)
+    {
+        return CSR_RESULT_SUCCESS;
+    }
+
+    unread_chunks =
+        (((count_thw - count_thr) + 128) % 128) - card->th_buffer.count;
+
+    if (unread_chunks == 0)
+    {
+        return CSR_RESULT_SUCCESS;
+    }
+
+    unread_bytes = card->config_data.sig_frag_size * unread_chunks;
+
+
+    r = unifi_bulk_rw(card,
+                      card->config_data.tohost_sigbuf_handle,
+                      card->th_buffer.ptr,
+                      unread_bytes,
+                      UNIFI_SDIO_READ);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read ToHost signal\n");
+        return r;
+    }
+
+    card->th_buffer.ptr += unread_bytes;
+    card->th_buffer.count += (u16)unread_chunks;
+
+    *processed = 1;
+
+    return CSR_RESULT_SUCCESS;
+} /* read_to_host_signals() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  update_to_host_signals_r
+ *
+ *      Advance the shared-memory count of chunks read from the to-host
+ *      signal buffer.
+ *      Raise a UniFi internal interrupt to tell the firmware that the
+ *      count has changed.
+ *
+ *  Arguments:
+ *      card            Pointer to card context struct
+ *      pending         Number of chunks remaining
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success or CSR error code
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult update_to_host_signals_r(card_t *card, s16 pending)
+{
+    CsrResult r;
+
+    card->to_host_signals_r =
+        (card->to_host_signals_r + (card->th_buffer.count - pending)) % 128;
+    card->th_buffer.count = pending;
+
+    /* Update the count of signals read */
+    r = unifi_write_8_or_16(card, card->sdio_ctrl_addr + 6,
+                            (u8)card->to_host_signals_r);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to update to-host signals read\n");
+        return r;
+    }
+
+    r = CardGenInt(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to notify UniFi that we processed to-host signals.\n");
+        return r;
+    }
+
+    card->generate_interrupt = 0;
+
+    return CSR_RESULT_SUCCESS;
+} /* update_to_host_signals_r() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  read_unpack_cmd
+ *
+ *      Converts a wire-formatted command to the host bulk_data_cmd_t structure.
+ *
+ *  Arguments:
+ *      ptr             Pointer to the command
+ *      bulk_data_cmd   Pointer to the host structure
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void read_unpack_cmd(const u8 *ptr, bulk_data_cmd_t *bulk_data_cmd)
+{
+    s16 index = 0;
+    bulk_data_cmd->cmd_and_len = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+    bulk_data_cmd->data_slot = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+    bulk_data_cmd->offset = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+    bulk_data_cmd->buffer_handle = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+} /* read_unpack_cmd */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_to_host_signals
+ *
+ *      Read and dispatch signals from the UniFi
+ *
+ *  Arguments:
+ *      card        Pointer to card context struct
+ *      processed   Pointer to location to write processing result:
+ *                      0 if there were no signals pending,
+ *                      1 if we read at least one signal
+ *
+ *  Returns:
+ *      CSR error code if there was an error
+ *
+ *  Notes:
+ *      Since bulk data transfers can take a long time, if we wait until
+ *      all are done before we acknowledge the signals, the UniFi runs out
+ *      of buffer space. Therefore we keep a count of the bytes transferred
+ *      in bulk data commands, and update the to-host-signals-read count
+ *      if we've done a large transfer.
+ *
+ *      All data in the f/w is stored in a little endian format, without any
+ *      padding bytes. Every read from the memory has to be transformed in
+ *      host (cpu specific) format, before we can process it. Therefore we
+ *      use read_unpack_cmd() and read_unpack_signal() to convert the raw data
+ *      contained in the card->th_buffer.buf to host structures.
+ *      Important: UDI clients use wire-formatted structures, so we need to
+ *      indicate all data, as we have read it from the device.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_to_host_signals(card_t *card, s32 *processed)
+{
+    s16 pending;
+    s16 remaining;
+    u8 *bufptr;
+    bulk_data_param_t data_ptrs;
+    s16 cmd;
+    u16 sig_len;
+    s16 i;
+    u16 chunks_in_buf;
+    u16 bytes_transferred = 0;
+    CsrResult r = CSR_RESULT_SUCCESS;
+
+    *processed = 0;
+
+    pending = card->th_buffer.count;
+
+    /* Are there new to-host signals? */
+    unifi_trace(card->ospriv, UDBG4, "handling %d to-host chunks\n", pending);
+
+    if (!pending)
+    {
+        return CSR_RESULT_SUCCESS;
+    }
+
+    /*
+     * This is a pointer to the raw data we have read from the f/w.
+     * Can be a signal or a command. Note that we need to convert
+     * it to a host structure before we process it.
+     */
+    bufptr = card->th_buffer.buf;
+
+    while (pending > 0)
+    {
+        s16 f_flush_count = 0;
+
+        /*
+         * Command and length are common to signal and bulk data msgs.
+         * If command == 0 (i.e. a signal), len is number of bytes
+         * *following* the 2-byte header.
+         */
+        cmd = bufptr[1] >> 4;
+        sig_len = bufptr[0] + ((bufptr[1] & 0x0F) << 8);
+
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "Received UniFi msg cmd=%d, len=%d\n",
+                    cmd, sig_len);
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+        if ((sig_len == 0) &&
+            ((cmd != SDIO_CMD_CLEAR_SLOT) && (cmd != SDIO_CMD_PADDING)))
+        {
+            unifi_error(card->ospriv, "incomplete signal or command: has size zero\n");
+            return CSR_RESULT_FAILURE;
+        }
+        /*
+         * Make sure the buffer contains a complete message.
+         * Signals may occupy multiple chunks, bulk-data commands occupy
+         * one chunk.
+         */
+        if (cmd == SDIO_CMD_SIGNAL)
+        {
+            chunks_in_buf = GET_CHUNKS_FOR(card->config_data.sig_frag_size, (u16)(sig_len + 2));
+        }
+        else
+        {
+            chunks_in_buf = 1;
+        }
+
+        if (chunks_in_buf > (u16)pending)
+        {
+            unifi_error(card->ospriv, "incomplete signal (0x%x?): need %d chunks, got %d\n",
+                        GET_SIGNAL_ID(bufptr + 2),
+                        chunks_in_buf, pending);
+            unifi_error(card->ospriv, " thsw=%d, thsr=%d\n",
+                        card->to_host_signals_w,
+                        card->to_host_signals_r);
+            return CSR_RESULT_FAILURE;
+        }
+
+
+        switch (cmd)
+        {
+            case SDIO_CMD_SIGNAL:
+                /* This is a signal. Read the rest of it and then handle it. */
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+                card->cmd_prof.sdio_cmd_signal++;
+#endif
+
+                for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++)
+                {
+                    /* Retrieve dataRefs[i].DataLength */
+                    u16 data_len = GET_PACKED_DATAREF_LEN(bufptr + 2, i);
+
+                    /*
+                     * The bulk data length in the signal can not be greater than
+                     * the maximun length allowed by the SDIO config structure.
+                     */
+                    if (data_len > card->config_data.data_slot_size)
+                    {
+                        unifi_error(card->ospriv,
+                                    "Bulk Data length (%d) exceeds Maximum Bulk Data length (%d)\n",
+                                    data_len, card->config_data.data_slot_size);
+                        return CSR_RESULT_FAILURE;
+                    }
+
+                    /*
+                     * Len here might not be the same as the length in the
+                     * bulk data slot.  The slot length will always be even,
+                     * but len could be odd.
+                     */
+                    if (data_len != 0)
+                    {
+                    /* Retrieve dataRefs[i].SlotNumber */
+                        s16 slot = GET_PACKED_DATAREF_SLOT(bufptr + 2, i);
+
+                        if (slot >= card->config_data.num_tohost_data_slots)
+                        {
+                            unifi_error(card->ospriv, "!!!bad slot number in to-host signal: %d, sig 0x%X\n",
+                                        slot, cmd);
+                            return CSR_RESULT_FAILURE;
+                        }
+
+                        data_ptrs.d[i].os_data_ptr = card->to_host_data[slot].os_data_ptr;
+                        data_ptrs.d[i].os_net_buf_ptr = card->to_host_data[slot].os_net_buf_ptr;
+                        data_ptrs.d[i].net_buf_length = card->to_host_data[slot].net_buf_length;
+                        data_ptrs.d[i].data_length = data_len;
+                    }
+                    else
+                    {
+                        UNIFI_INIT_BULK_DATA(&data_ptrs.d[i]);
+                    }
+                }
+
+            /*
+             * Log the signal to the UDI, before call unifi_receive_event() as
+             * it can modify the bulk data.
+             */
+                if (card->udi_hook)
+                {
+                    (*card->udi_hook)(card->ospriv, bufptr + 2, sig_len,
+                                      &data_ptrs, UDI_LOG_TO_HOST);
+                }
+
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+                if (GET_SIGNAL_ID(bufptr + 2) == CSR_MA_PACKET_CONFIRM_ID)
+                {
+                    card->cmd_prof.tx_cfm_count++;
+                }
+                else if (GET_SIGNAL_ID(bufptr + 2) == CSR_MA_PACKET_INDICATION_ID)
+                {
+                    if (data_ptrs.d[0].os_data_ptr)
+                    {
+                        if ((*data_ptrs.d[0].os_data_ptr) & 0x08)
+                        {
+                            card->cmd_prof.rx_count++;
+                        }
+                    }
+                }
+#endif
+                /*
+                 * Check if the signal is MA-PACKET.cfm and if so check the status.
+                 * If the status is failure, search through the slot records to find
+                 * if any slots are occupied for this host tag. This can happen if
+                 * f/w has not downloaded the bulkdata and before that itself it has
+                 * signalled the confirm with failure. If it finds a slot with that
+                 * host tag then, it clears the corresponding slot
+                 */
+
+                if (GET_SIGNAL_ID(bufptr + 2) == CSR_MA_PACKET_CONFIRM_ID)
+                {
+                    /* Get host tag and transmission status */
+                    u32 host_tag = GET_PACKED_MA_PACKET_CONFIRM_HOST_TAG(bufptr + 2);
+                    u16 status = GET_PACKED_MA_PACKET_CONFIRM_TRANSMISSION_STATUS(bufptr + 2);
+
+                    unifi_trace(card->ospriv, UDBG4, "process_to_host_signals signal ID=%x host Tag=%x status=%x\n",
+                                GET_SIGNAL_ID(bufptr + 2), host_tag, status);
+
+                    /* If transmission status is failure then search through the slot records
+                     * and if for any slot records the clear slot is not done then do it now
+                     */
+
+                    if (status && (card->fh_slot_host_tag_record))
+                    {
+                        u16 num_fh_slots = card->config_data.num_fromhost_data_slots;
+
+                        /* search through the list of slot records and match with host tag
+                         * If a slot is not yet cleared then clear the slot from here
+                         */
+                        for (i = 0; i < num_fh_slots; i++)
+                        {
+                            if (card->fh_slot_host_tag_record[i] == host_tag)
+                            {
+#ifdef CSR_WIFI_REQUEUE_PACKET_TO_HAL
+                                /* Invoke the HAL module function to requeue it back to HAL Queues */
+                                r = unifi_reque_ma_packet_request(card->ospriv, host_tag, status, &card->from_host_data[i].bd);
+                                card->fh_slot_host_tag_record[i] = CSR_WIFI_HIP_RESERVED_HOST_TAG;
+                                if (CSR_RESULT_SUCCESS != r)
+                                {
+                                    unifi_trace(card->ospriv, UDBG5, "process_to_host_signals: Failed to requeue Packet(hTag:%x) back to HAL \n", host_tag);
+                                    CardClearFromHostDataSlot(card, i);
+                                }
+                                else
+                                {
+                                    CardClearFromHostDataSlotWithoutFreeingBulkData(card, i);
+                                }
+
+#else
+                                unifi_trace(card->ospriv, UDBG4, "process_to_host_signals Clear slot=%x host tag=%x\n", i, host_tag);
+                                card->fh_slot_host_tag_record[i] = CSR_WIFI_HIP_RESERVED_HOST_TAG;
+
+                                /* Set length field in from_host_data array to 0 */
+                                CardClearFromHostDataSlot(card, i);
+#endif
+                                break;
+                            }
+                        }
+                    }
+                }
+
+                /* Pass event to OS layer */
+                unifi_receive_event(card->ospriv, bufptr + 2, sig_len, &data_ptrs);
+
+                /* Initialise the to_host data, so it can be re-used. */
+                for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++)
+                {
+                /* The slot is only valid if the length is non-zero. */
+                    if (GET_PACKED_DATAREF_LEN(bufptr + 2, i) != 0)
+                    {
+                        s16 slot = GET_PACKED_DATAREF_SLOT(bufptr + 2, i);
+                        if (slot < card->config_data.num_tohost_data_slots)
+                        {
+                            UNIFI_INIT_BULK_DATA(&card->to_host_data[slot]);
+                        }
+                    }
+                }
+
+#ifndef CSR_WIFI_DEFER_TH_FLUSH
+                /*
+                 * If we have previously transferred a lot of data, ack
+                 * the signals read so far, so f/w can reclaim the buffer
+                 * memory sooner.
+                 */
+                if (bytes_transferred >= TO_HOST_FLUSH_THRESHOLD)
+                {
+                    f_flush_count = 1;
+                }
+#endif
+                break;
+
+
+            case SDIO_CMD_CLEAR_SLOT:
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+                card->cmd_prof.sdio_cmd_clear_slot++;
+#endif
+                /* This is a clear slot command. */
+                if (sig_len != 0)
+                {
+                    unifi_error(card->ospriv, "process_to_host_signals: clear slot, bad data len: 0x%X at offset %d\n",
+                                sig_len, bufptr - card->th_buffer.buf);
+                    return CSR_RESULT_FAILURE;
+                }
+
+                r = process_clear_slot_command(card, bufptr);
+                if (r != CSR_RESULT_SUCCESS)
+                {
+                    unifi_error(card->ospriv, "Failed to process clear slot\n");
+                    return r;
+                }
+                break;
+
+            case SDIO_CMD_TO_HOST_TRANSFER:
+            case SDIO_CMD_FROM_HOST_TRANSFER:
+            case SDIO_CMD_FROM_HOST_AND_CLEAR:
+            case SDIO_CMD_OVERLAY_TRANSFER:
+                /* This is a bulk data command. */
+                if (sig_len & 1)
+                {
+                    unifi_error(card->ospriv, "process_to_host_signals: bulk data, bad data len: 0x%X at offset %d\n",
+                                sig_len, bufptr - card->th_buffer.buf);
+                    return CSR_RESULT_FAILURE;
+                }
+
+                r = process_bulk_data_command(card, bufptr, cmd, sig_len);
+                if (r != CSR_RESULT_SUCCESS)
+                {
+                    unifi_error(card->ospriv, "Failed to process bulk cmd\n");
+                    return r;
+                }
+                /* Count the bytes transferred */
+                bytes_transferred += sig_len;
+
+                if (cmd == SDIO_CMD_FROM_HOST_AND_CLEAR)
+                {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+                    card->cmd_prof.sdio_cmd_from_host_and_clear++;
+#endif
+#ifndef CSR_WIFI_DEFER_TH_FLUSH
+                    f_flush_count = 1;
+#endif
+                }
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+                else if (cmd == SDIO_CMD_FROM_HOST_TRANSFER)
+                {
+                    card->cmd_prof.sdio_cmd_from_host++;
+                }
+                else if (cmd == SDIO_CMD_TO_HOST_TRANSFER)
+                {
+                    card->cmd_prof.sdio_cmd_to_host++;
+                }
+#endif
+                break;
+
+            case SDIO_CMD_PADDING:
+                break;
+
+            default:
+                unifi_error(card->ospriv, "Unrecognised to-host command: %d\n", cmd);
+                break;
+        }
+
+        bufptr += chunks_in_buf * card->config_data.sig_frag_size;
+        pending -= chunks_in_buf;
+
+        /*
+         * Write out the host signal count when a significant
+         * number of bytes of bulk data have been transferred or
+         * when we have performed a CopyFromHostAndClear.
+         */
+        if (f_flush_count)
+        {
+            r = update_to_host_signals_r(card, pending);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            bytes_transferred = 0;
+        }
+    }
+
+    if (pending)
+    {
+        unifi_warning(card->ospriv, "proc_th_sigs: %d unprocessed\n", pending);
+    }
+
+    /* If we processed any signals, write the updated count to UniFi */
+    if (card->th_buffer.count != pending)
+    {
+        r = update_to_host_signals_r(card, pending);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return r;
+        }
+    }
+
+    /*
+     * Reset the buffer pointer, copying down any un-processed signals.
+     * This can happen if we enable the optimisation in read_to_host_signals()
+     * that limits the length to whole blocks.
+     */
+    remaining = card->th_buffer.ptr - bufptr;
+    if (remaining < 0)
+    {
+        unifi_error(card->ospriv, "Processing TH signals overran the buffer\n");
+        return CSR_RESULT_FAILURE;
+    }
+    if (remaining > 0)
+    {
+        /* Use a safe copy because source and destination may overlap */
+        u8 *d = card->th_buffer.buf;
+        u8 *s = bufptr;
+        s32 n = remaining;
+        while (n--)
+        {
+            *d++ = *s++;
+        }
+    }
+    card->th_buffer.ptr = card->th_buffer.buf + remaining;
+
+
+    /* If we reach here then we processed something */
+    *processed = 1;
+    return CSR_RESULT_SUCCESS;
+} /* process_to_host_signals() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_clear_slot_command
+ *
+ *      Process a clear slot command fom the UniFi.
+ *
+ *  Arguments:
+ *   card       Pointer to card context struct
+ *   bdcmd      Pointer to bulk-data command msg from UniFi
+ *
+ *  Returns:
+ *      0 on success, CSR error code on error
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_clear_slot_command(card_t *card, const u8 *cmdptr)
+{
+    u16 data_slot;
+    s16 slot;
+
+    data_slot = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(cmdptr + SIZEOF_UINT16);
+
+    unifi_trace(card->ospriv, UDBG4, "Processing clear slot cmd, slot=0x%X\n",
+                data_slot);
+
+    slot = data_slot & 0x7FFF;
+
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv, "CMD clear data slot 0x%04x\n", data_slot);
+#endif /* CSR_WIFI_HIP_NOISY */
+
+    if (data_slot & SLOT_DIR_TO_HOST)
+    {
+        if (slot >= card->config_data.num_tohost_data_slots)
+        {
+            unifi_error(card->ospriv,
+                        "Invalid to-host data slot in SDIO_CMD_CLEAR_SLOT: %d\n",
+                        slot);
+            return CSR_RESULT_FAILURE;
+        }
+        /* clear to-host data slot */
+        unifi_warning(card->ospriv, "Unexpected clear to-host data slot cmd: 0x%04x\n",
+                      data_slot);
+    }
+    else
+    {
+        if (slot >= card->config_data.num_fromhost_data_slots)
+        {
+            unifi_error(card->ospriv,
+                        "Invalid from-host data slot in SDIO_CMD_CLEAR_SLOT: %d\n",
+                        slot);
+            return CSR_RESULT_FAILURE;
+        }
+
+        /*
+         * The driver is the owner to clear all slots now
+         * Ref - comment in process_fh_traffic_queue
+         * so it will just ignore the clear slot command from firmware
+         * and return success
+         */
+        return CSR_RESULT_SUCCESS;
+
+        /* Set length field in from_host_data array to 0 */
+        /* CardClearFromHostDataSlot(card, slot); */
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* process_clear_slot_command() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_bulk_data_command
+ *
+ *      Process a bulk data request from the UniFi.
+ *
+ *  Arguments:
+ *   card       Pointer to card context struct
+ *   bdcmd      Pointer to bulk-data command msg from UniFi
+ *   cmd, len   Decoded values of command and length from the msg header
+ *              Cmd will only be one of:
+ *                      SDIO_CMD_TO_HOST_TRANSFER
+ *                      SDIO_CMD_FROM_HOST_TRANSFER
+ *                      SDIO_CMD_FROM_HOST_AND_CLEAR
+ *                      SDIO_CMD_OVERLAY_TRANSFER
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on error
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_bulk_data_command(card_t *card, const u8 *cmdptr,
+                                           s16 cmd, u16 len)
+{
+    bulk_data_desc_t *bdslot;
+#ifdef CSR_WIFI_ALIGNMENT_WORKAROUND
+    u8 *host_bulk_data_slot;
+#endif
+    bulk_data_cmd_t bdcmd;
+    s16 offset;
+    s16 slot;
+    s16 dir;
+    CsrResult r;
+
+    read_unpack_cmd(cmdptr, &bdcmd);
+
+    unifi_trace(card->ospriv, UDBG4, "Processing bulk data cmd %d %s, len=%d, slot=0x%X\n",
+                cmd, lookup_bulkcmd_name(cmd), len, bdcmd.data_slot);
+
+    /*
+     * Round up the transfer length if required.
+     * This is useful to force all transfers to be a multiple of the SDIO block
+     * size, so the SDIO driver won't try to use a byte-mode CMD53. These are
+     * broken on some hardware platforms.
+     */
+    if (card->sdio_io_block_pad)
+    {
+        len = (len + card->sdio_io_block_size - 1) & ~(card->sdio_io_block_size - 1);
+        unifi_trace(card->ospriv, UDBG4, "Rounded bulk data length up to %d\n", len);
+    }
+
+    slot = bdcmd.data_slot & 0x7FFF;
+
+    if (cmd == SDIO_CMD_OVERLAY_TRANSFER)
+    {
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;     /* Not used on CSR6xxx */
+    }
+    else
+    {
+        if (bdcmd.data_slot & SLOT_DIR_TO_HOST)
+        {
+            /* Request is for to-host bulk data */
+
+            /* Check sanity of slot number */
+            if (slot >= card->config_data.num_tohost_data_slots)
+            {
+                unifi_error(card->ospriv,
+                            "Invalid to-host data slot in SDIO bulk xfr req: %d\n",
+                            slot);
+                return CSR_RESULT_FAILURE;
+            }
+
+            /* Allocate memory for card->to_host_data[slot] bulk data here. */
+#ifdef CSR_PRE_ALLOC_NET_DATA
+            r = prealloc_netdata_get(card, &card->to_host_data[slot], len);
+#else
+            r = unifi_net_data_malloc(card->ospriv, &card->to_host_data[slot], len);
+#endif
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "Failed to allocate t-h bulk data\n");
+                return CSR_RESULT_FAILURE;
+            }
+
+            bdslot = &card->to_host_data[slot];
+
+            /* Make sure that the buffer is 4-bytes aligned */
+            r = unifi_net_dma_align(card->ospriv, bdslot);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "Failed to align t-h bulk data buffer for DMA\n");
+                return CSR_RESULT_FAILURE;
+            }
+        }
+        else
+        {
+            /* Request is for from-host bulk data */
+
+            if (slot >= card->config_data.num_fromhost_data_slots)
+            {
+                unifi_error(card->ospriv,
+                            "Invalid from-host data slot in SDIO bulk xfr req: %d\n",
+                            slot);
+                return CSR_RESULT_FAILURE;
+            }
+            bdslot = &card->from_host_data[slot].bd;
+        }
+        offset = bdcmd.offset;
+    }
+    /* Do the transfer */
+    dir = (cmd == SDIO_CMD_TO_HOST_TRANSFER)?
+          UNIFI_SDIO_READ : UNIFI_SDIO_WRITE;
+
+    unifi_trace(card->ospriv, UDBG4,
+                "Bulk %c %s len=%d, handle %d - slot=%d %p+(%d)\n",
+                (dir == UNIFI_SDIO_READ)?'R' : 'W',
+                lookup_bulkcmd_name(cmd),
+                len,
+                bdcmd.buffer_handle,
+                slot, bdslot->os_data_ptr, offset);
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv, "Bulk %s len=%d, handle %d - slot=%d %p+(%d)\n",
+                lookup_bulkcmd_name(cmd),
+                len,
+                bdcmd.buffer_handle,
+                slot, bdslot->os_data_ptr, offset);
+#endif /* CSR_WIFI_HIP_NOISY */
+
+
+    if (bdslot->os_data_ptr == NULL)
+    {
+        unifi_error(card->ospriv, "Null os_data_ptr - Bulk %s handle %d - slot=%d o=(%d)\n",
+                    lookup_bulkcmd_name(cmd),
+                    bdcmd.buffer_handle,
+                    slot,
+                    offset);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+#ifdef CSR_WIFI_ALIGNMENT_WORKAROUND
+    /* if os_data_ptr is not 4-byte aligned, then allocate a new buffer and copy data
+    to new buffer to ensure the address passed to unifi_bulk_rw is 4-byte aligned */
+
+    if (len != 0 && (dir == UNIFI_SDIO_WRITE) && (((ptrdiff_t)bdslot->os_data_ptr + offset) & 3))
+    {
+        host_bulk_data_slot = kmalloc(len, GFP_KERNEL);
+
+        if (!host_bulk_data_slot)
+        {
+            unifi_error(card->ospriv, " failed to allocate request_data before unifi_bulk_rw\n");
+            return -1;
+        }
+
+        memcpy((void *)host_bulk_data_slot,
+                  (void *)(bdslot->os_data_ptr + offset), len);
+
+        r = unifi_bulk_rw(card,
+                          bdcmd.buffer_handle,
+                          (void *)host_bulk_data_slot,
+                          len,
+                          dir);
+    }
+    else
+#endif
+    {
+        r = unifi_bulk_rw(card,
+                          bdcmd.buffer_handle,
+                          (void *)(bdslot->os_data_ptr + offset),
+                          len,
+                          dir);
+    }
+
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv,
+                    "Failed: %s hlen=%d, slen=%d, handle %d - slot=%d %p+0x%X\n",
+                    lookup_bulkcmd_name(cmd),
+                    len,                    /* Header length */
+                    bdslot->data_length,    /* Length stored in slot */
+                    bdcmd.buffer_handle,
+                    slot, bdslot->os_data_ptr, offset);
+        return r;
+    }
+
+    bdslot->data_length = len;
+
+    if (cmd == SDIO_CMD_FROM_HOST_AND_CLEAR)
+    {
+        if (slot >= card->config_data.num_fromhost_data_slots)
+        {
+            unifi_error(card->ospriv,
+                        "Invalid from-host data slot in SDIO_CMD_FROM_HOST_AND_CLEAR: %d\n",
+                        slot);
+            return CSR_RESULT_FAILURE;
+        }
+
+#ifdef CSR_WIFI_ALIGNMENT_WORKAROUND
+        /* moving this check before we clear host data slot */
+        if ((len != 0) && (dir == UNIFI_SDIO_WRITE) && (((ptrdiff_t)bdslot->os_data_ptr + offset) & 3))
+        {
+            kfree(host_bulk_data_slot);
+        }
+#endif
+
+        if (card->fh_slot_host_tag_record)
+        {
+            unifi_trace(card->ospriv, UDBG5, "CopyFromHostAndClearSlot Reset entry for slot=%d\n", slot);
+
+            /* reset the host tag entry for the corresponding slot */
+            card->fh_slot_host_tag_record[slot] = CSR_WIFI_HIP_RESERVED_HOST_TAG;
+        }
+
+
+        /* Set length field in from_host_data array to 0 */
+        CardClearFromHostDataSlot(card, slot);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* process_bulk_data_command() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  check_fh_sig_slots
+ *
+ *      Check whether there are <n> free signal slots available on UniFi.
+ *      This takes into account the signals already batched since the
+ *      from_host_signal counts were last read.
+ *      If the from_host_signal counts indicate not enough space, we read
+ *      the latest count from UniFi to see if some more have been freed.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS, otherwise CSR error code on error.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult check_fh_sig_slots(card_t *card, u16 needed, s32 *space_fh)
+{
+    u32 count_fhw;
+    u32 occupied_fh, slots_fh;
+    s32 count_fhr;
+
+    count_fhw = card->from_host_signals_w;
+    count_fhr = card->from_host_signals_r;
+    slots_fh = card->config_data.num_fromhost_sig_frags;
+
+    /* Only read the space in from-host queue if necessary */
+    occupied_fh = (count_fhw - count_fhr) % 128;
+
+    if (slots_fh < occupied_fh)
+    {
+        *space_fh = 0;
+    }
+    else
+    {
+        *space_fh = slots_fh - occupied_fh;
+    }
+
+    if ((occupied_fh != 0) && (*space_fh < needed))
+    {
+        count_fhr = unifi_read_shared_count(card, card->sdio_ctrl_addr + 2);
+        if (count_fhr < 0)
+        {
+            unifi_error(card->ospriv, "Failed to read from-host sig read count\n");
+            return CSR_RESULT_FAILURE;
+        }
+        card->from_host_signals_r = count_fhr; /* diag */
+
+        occupied_fh = (count_fhw - count_fhr) % 128;
+        *space_fh = slots_fh - occupied_fh;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* check_fh_sig_slots() */
+
+
+/*
+* If we are padding the From-Host signals to the SDIO block size,
+* we need to round up the needed_chunks to the SDIO block size.
+*/
+#define ROUND_UP_NEEDED_CHUNKS(_card, _needed_chunks) \
+    { \
+        u16 _chunks_per_block; \
+        u16 _chunks_in_last_block; \
+ \
+        if (_card->sdio_io_block_pad) \
+        { \
+            _chunks_per_block = _card->sdio_io_block_size / _card->config_data.sig_frag_size; \
+            _chunks_in_last_block = _needed_chunks % _chunks_per_block; \
+            if (_chunks_in_last_block != 0) \
+            { \
+                _needed_chunks = _needed_chunks + (_chunks_per_block - _chunks_in_last_block); \
+            } \
+        } \
+    }
+
+
+#define ROUND_UP_SPACE_CHUNKS(_card, _space_chunks) \
+    { \
+        u16 _chunks_per_block; \
+ \
+        if (_card->sdio_io_block_pad) \
+        { \
+            _chunks_per_block = _card->sdio_io_block_size / _card->config_data.sig_frag_size; \
+            _space_chunks = ((_space_chunks / _chunks_per_block) * _chunks_per_block); \
+        } \
+    }
+
+
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_fh_cmd_queue
+ *
+ *      Take one signal off the from-host queue and copy it to the UniFi.
+ *      Does nothing if the UniFi has no slots free.
+ *
+ *  Arguments:
+ *      card       Pointer to card context struct
+ *      processed  Location to write:
+ *                      0 if there is nothing on the queue to process
+ *                      1 if a signal was successfully processed
+ *
+ *  Returns:
+ *      CSR error code if an error occurred.
+ *
+ *  Notes:
+ *      The from-host queue contains signal requests from the network driver
+ *      and any UDI clients interspersed. UDI clients' requests have been stored
+ *      in the from-host queue using the wire-format structures, as they arrive.
+ *      All other requests are stored in the from-host queue using the host
+ *      (cpu specific) structures. We use the is_packed member of the card_signal_t
+ *      structure that describes the queue to make the distiction.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_fh_cmd_queue(card_t *card, s32 *processed)
+{
+    q_t *sigq = &card->fh_command_queue;
+
+    CsrResult r;
+    u16 pending_sigs;
+    u16 pending_chunks;
+    u16 needed_chunks;
+    s32 space_chunks;
+    u16 q_index;
+
+    *processed = 0;
+
+    /* Get the number of pending signals. */
+    pending_sigs = CSR_WIFI_HIP_Q_SLOTS_USED(sigq);
+    unifi_trace(card->ospriv, UDBG5, "proc_fh: %d pending\n", pending_sigs);
+    if (pending_sigs == 0)
+    {
+        /* Nothing to do */
+        return CSR_RESULT_SUCCESS;
+    }
+
+    /* Work out how many chunks we have waiting to send */
+    for (pending_chunks = 0, q_index = CSR_WIFI_HIP_Q_NEXT_R_SLOT(sigq);
+         q_index != CSR_WIFI_HIP_Q_NEXT_W_SLOT(sigq);
+         q_index = CSR_WIFI_HIP_Q_WRAP(sigq, q_index + 1))
+    {
+        card_signal_t *csptr = CSR_WIFI_HIP_Q_SLOT_DATA(sigq, q_index);
+
+        /*
+         * Note that GET_CHUNKS_FOR() needs the size of the packed
+         * (wire-formatted) structure
+         */
+        pending_chunks += GET_CHUNKS_FOR(card->config_data.sig_frag_size, (u16)(csptr->signal_length + 2));
+    }
+
+    /*
+     * Check whether UniFi has space for all the buffered bulk-data
+     * commands and signals as well.
+     */
+    needed_chunks = pending_chunks + card->fh_buffer.count;
+
+    /* Round up to the block size if necessary */
+    ROUND_UP_NEEDED_CHUNKS(card, needed_chunks);
+
+    r = check_fh_sig_slots(card, needed_chunks, &space_chunks);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        /* Error */
+        unifi_error(card->ospriv, "Failed to read fh sig count\n");
+        return r;
+    }
+
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv, "proc_fh: %d chunks free, need %d\n",
+                space_chunks, needed_chunks);
+#endif /* CSR_WIFI_HIP_NOISY */
+
+
+    /*
+     * Coalesce as many from-host signals as possible
+     * into a single block and write using a single CMD53
+     */
+    if (needed_chunks > (u16)space_chunks)
+    {
+        /* Round up to the block size if necessary */
+        ROUND_UP_SPACE_CHUNKS(card, space_chunks);
+
+        /*
+         * If the f/w has less free chunks than those already pending
+         * return immediately.
+         */
+        if ((u16)space_chunks <= card->fh_buffer.count)
+        {
+            /*
+             * No room in UniFi for any signals after the buffered bulk
+             * data commands have been sent.
+             */
+            unifi_error(card->ospriv, "not enough room to send signals, need %d chunks, %d free\n",
+                        card->fh_buffer.count, space_chunks);
+            card->generate_interrupt = 1;
+            return CSR_RESULT_SUCCESS;
+        }
+        pending_chunks = (u16)(space_chunks - card->fh_buffer.count);
+    }
+
+    while (pending_sigs-- && pending_chunks > 0)
+    {
+        card_signal_t *csptr;
+        s16 i;
+        u16 sig_chunks, total_length, free_chunks_in_fh_buffer;
+        bulk_data_param_t bulkdata;
+        u8 *packed_sigptr;
+        u16 signal_length = 0;
+
+        /* Retrieve the entry at the head of the queue */
+        q_index = CSR_WIFI_HIP_Q_NEXT_R_SLOT(sigq);
+
+        /* Get a pointer to the containing card_signal_t struct */
+        csptr = CSR_WIFI_HIP_Q_SLOT_DATA(sigq, q_index);
+
+        /* Get the new length of the packed signal */
+        signal_length = csptr->signal_length;
+
+        if ((signal_length & 1) || (signal_length > UNIFI_PACKED_SIGBUF_SIZE))
+        {
+            unifi_error(card->ospriv, "process_fh_queue: Bad len: %d\n", signal_length);
+            return CSR_RESULT_FAILURE;
+        }
+
+        /* Need space for 2-byte SDIO protocol header + signal */
+        sig_chunks = GET_CHUNKS_FOR(card->config_data.sig_frag_size, (u16)(signal_length + 2));
+
+        free_chunks_in_fh_buffer = GET_CHUNKS_FOR(card->config_data.sig_frag_size,
+                                                  (u16)((card->fh_buffer.buf + UNIFI_FH_BUF_SIZE) - card->fh_buffer.ptr));
+        if (free_chunks_in_fh_buffer < sig_chunks)
+        {
+            /* No more room */
+            unifi_notice(card->ospriv, "proc_fh_cmd_q: no room in fh buffer for 0x%.4X, deferring\n",
+                         (u16)(GET_SIGNAL_ID(csptr->sigbuf)));
+            break;
+        }
+
+        packed_sigptr = csptr->sigbuf;
+
+        /* Claim and set up a from-host data slot */
+        if (CSR_RESULT_FAILURE == CardWriteBulkData(card, csptr, UNIFI_TRAFFIC_Q_MLME))
+        {
+            unifi_notice(card->ospriv, "proc_fh_cmd_q: no fh data slots for 0x%.4X, deferring\n",
+                         (u16)(GET_SIGNAL_ID(csptr->sigbuf)));
+            break;
+        }
+
+        for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++)
+        {
+            if (csptr->bulkdata[i].data_length == 0)
+            {
+                UNIFI_INIT_BULK_DATA(&bulkdata.d[i]);
+            }
+            else
+            {
+                bulkdata.d[i].os_data_ptr = csptr->bulkdata[i].os_data_ptr;
+                bulkdata.d[i].data_length = csptr->bulkdata[i].data_length;
+            }
+
+            /* Pass the free responsibility to the lower layer. */
+            UNIFI_INIT_BULK_DATA(&csptr->bulkdata[i]);
+        }
+
+        unifi_trace(card->ospriv, UDBG2, "Sending signal 0x%.4X\n",
+                    GET_SIGNAL_ID(packed_sigptr));
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "Sending signal 0x%.4X\n",
+                    GET_SIGNAL_ID(packed_sigptr));
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+
+        /* Append packed signal to F-H buffer */
+        total_length = sig_chunks * card->config_data.sig_frag_size;
+
+        card->fh_buffer.ptr[0] = (u8)(signal_length & 0xff);
+        card->fh_buffer.ptr[1] =
+            (u8)(((signal_length >> 8) & 0xf) | (SDIO_CMD_SIGNAL << 4));
+
+        memcpy(card->fh_buffer.ptr + 2, packed_sigptr, signal_length);
+        memset(card->fh_buffer.ptr + 2 + signal_length, 0,
+                  total_length - (2 + signal_length));
+
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "proc_fh: fh_buffer %d bytes \n",
+                    signal_length + 2);
+        dump(card->fh_buffer.ptr, signal_length + 2);
+        unifi_trace(card->ospriv, UDBG1, " \n");
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+        card->fh_buffer.ptr += total_length;
+        card->fh_buffer.count += sig_chunks;
+
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "Added %d to fh buf, len now %d, count %d\n",
+                    signal_length,
+                    card->fh_buffer.ptr - card->fh_buffer.buf,
+                    card->fh_buffer.count);
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+        (*processed)++;
+        pending_chunks -= sig_chunks;
+
+        /* Log the signal to the UDI. */
+        /* UDI will get the packed structure */
+        /* Can not log the unpacked signal, unless we reconstruct it! */
+        if (card->udi_hook)
+        {
+            (*card->udi_hook)(card->ospriv, packed_sigptr, signal_length,
+                              &bulkdata, UDI_LOG_FROM_HOST);
+        }
+
+        /* Remove entry from q */
+        csptr->signal_length = 0;
+        CSR_WIFI_HIP_Q_INC_R(sigq);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* process_fh_cmd_queue() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_fh_traffic_queue
+ *
+ *      Take signals off the from-host queue and copy them to the UniFi.
+ *      Does nothing if the UniFi has no slots free.
+ *
+ *  Arguments:
+ *      card       Pointer to card context struct
+ *      sigq       Pointer to the traffic queue
+ *      processed  Pointer to location to write:
+ *                      0 if there is nothing on the queue to process
+ *                      1 if a signal was successfully processed
+ *
+ *  Returns:
+ *      CSR error code if an error occurred.
+ *
+ *  Notes:
+ *      The from-host queue contains signal requests from the network driver
+ *      and any UDI clients interspersed.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult process_fh_traffic_queue(card_t *card, s32 *processed)
+{
+    q_t *sigq = card->fh_traffic_queue;
+
+    CsrResult r;
+    s16 n = 0;
+    s32 q_no;
+    u16 pending_sigs = 0;
+    u16 pending_chunks = 0;
+    u16 needed_chunks;
+    s32 space_chunks;
+    u16 q_index;
+    u32 host_tag = 0;
+    u16 slot_num = 0;
+
+    *processed = 0;
+
+    /* calculate how many signals are in queues and how many chunks are needed. */
+    for (n = UNIFI_NO_OF_TX_QS - 1; n >= 0; n--)
+    {
+        /* Get the number of pending signals. */
+        pending_sigs += CSR_WIFI_HIP_Q_SLOTS_USED(&sigq[n]);
+        unifi_trace(card->ospriv, UDBG5, "proc_fh%d: %d pending\n", n, pending_sigs);
+
+        /* Work out how many chunks we have waiting to send */
+        for (q_index = CSR_WIFI_HIP_Q_NEXT_R_SLOT(&sigq[n]);
+             q_index != CSR_WIFI_HIP_Q_NEXT_W_SLOT(&sigq[n]);
+             q_index = CSR_WIFI_HIP_Q_WRAP(&sigq[n], q_index + 1))
+        {
+            card_signal_t *csptr = CSR_WIFI_HIP_Q_SLOT_DATA(&sigq[n], q_index);
+
+            /*
+             * Note that GET_CHUNKS_FOR() needs the size of the packed
+             * (wire-formatted) structure
+             */
+            pending_chunks += GET_CHUNKS_FOR(card->config_data.sig_frag_size, (u16)(csptr->signal_length + 2));
+        }
+    }
+
+    /* If there are no pending signals, just return */
+    if (pending_sigs == 0)
+    {
+        /* Nothing to do */
+        return CSR_RESULT_SUCCESS;
+    }
+
+    /*
+     * Check whether UniFi has space for all the buffered bulk-data
+     * commands and signals as well.
+     */
+    needed_chunks = pending_chunks + card->fh_buffer.count;
+
+    /* Round up to the block size if necessary */
+    ROUND_UP_NEEDED_CHUNKS(card, needed_chunks);
+
+    r = check_fh_sig_slots(card, needed_chunks, &space_chunks);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        /* Error */
+        unifi_error(card->ospriv, "Failed to read fh sig count\n");
+        return r;
+    }
+
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv,
+                "process_fh_traffic_queue: %d chunks free, need %d\n",
+                space_chunks, needed_chunks);
+    read_fhsr(card);            /* debugging only */
+#endif /* CSR_WIFI_HIP_NOISY */
+
+    /* Coalesce as many from-host signals as possible
+       into a single block and write using a single CMD53 */
+    if (needed_chunks > (u16)space_chunks)
+    {
+        /* Round up to the block size if necessary */
+        ROUND_UP_SPACE_CHUNKS(card, space_chunks);
+
+        if ((u16)space_chunks <= card->fh_buffer.count)
+        {
+            /*
+             * No room in UniFi for any signals after the buffered bulk
+             * data commands have been sent.
+             */
+            unifi_error(card->ospriv, "not enough room to send signals, need %d chunks, %d free\n",
+                        card->fh_buffer.count, space_chunks);
+            card->generate_interrupt = 1;
+            return 0;
+        }
+
+        pending_chunks = (u16)space_chunks - card->fh_buffer.count;
+    }
+
+    q_no = UNIFI_NO_OF_TX_QS - 1;
+
+    /*
+     * pending_sigs will be exhausted if there are is no restriction to the pending
+     * signals per queue. pending_chunks may be exhausted if there is a restriction.
+     * q_no check will be exhausted if there is a restriction and our round-robin
+     * algorith fails to fill all chunks.
+     */
+    do
+    {
+        card_signal_t *csptr;
+        u16 sig_chunks, total_length, free_chunks_in_fh_buffer;
+        bulk_data_param_t bulkdata;
+        u8 *packed_sigptr;
+        u16 signal_length = 0;
+
+        /* if this queue is empty go to next one. */
+        if (CSR_WIFI_HIP_Q_SLOTS_USED(&sigq[q_no]) == 0)
+        {
+            q_no--;
+            continue;
+        }
+
+        /* Retrieve the entry at the head of the queue */
+        q_index = CSR_WIFI_HIP_Q_NEXT_R_SLOT(&sigq[q_no]);
+
+        /* Get a pointer to the containing card_signal_t struct */
+        csptr = CSR_WIFI_HIP_Q_SLOT_DATA(&sigq[q_no], q_index);
+
+        /* Get the new length of the packed signal */
+        signal_length = csptr->signal_length;
+
+        if ((signal_length & 1) || (signal_length > UNIFI_PACKED_SIGBUF_SIZE))
+        {
+            unifi_error(card->ospriv, "process_fh_traffic_queue: Bad len: %d\n", signal_length);
+            return CSR_RESULT_FAILURE;
+        }
+
+        /* Need space for 2-byte SDIO protocol header + signal */
+        sig_chunks = GET_CHUNKS_FOR(card->config_data.sig_frag_size, (u16)(signal_length + 2));
+        free_chunks_in_fh_buffer = GET_CHUNKS_FOR(card->config_data.sig_frag_size,
+                                                  (u16)((card->fh_buffer.buf + UNIFI_FH_BUF_SIZE) - card->fh_buffer.ptr));
+        if (free_chunks_in_fh_buffer < sig_chunks)
+        {
+            /* No more room */
+            unifi_notice(card->ospriv, "process_fh_traffic_queue: no more chunks.\n");
+            break;
+        }
+
+        packed_sigptr = csptr->sigbuf;
+        /* Claim and set up a from-host data slot */
+        if (CSR_RESULT_FAILURE == CardWriteBulkData(card, csptr, (unifi_TrafficQueue)q_no))
+        {
+            q_no--;
+            continue;
+        }
+
+        /* Sanity check: MA-PACKET.req must have a valid bulk data */
+        if ((csptr->bulkdata[0].data_length == 0) || (csptr->bulkdata[0].os_data_ptr == NULL))
+        {
+            unifi_error(card->ospriv, "MA-PACKET.req with empty bulk data (%d bytes in %p)\n",
+                        csptr->bulkdata[0].data_length, csptr->bulkdata[0].os_data_ptr);
+            dump(packed_sigptr, signal_length);
+            return CSR_RESULT_FAILURE;
+        }
+
+        bulkdata.d[0].os_data_ptr = csptr->bulkdata[0].os_data_ptr;
+        bulkdata.d[0].data_length = csptr->bulkdata[0].data_length;
+        bulkdata.d[0].os_net_buf_ptr = csptr->bulkdata[0].os_net_buf_ptr;
+        bulkdata.d[0].net_buf_length = csptr->bulkdata[0].net_buf_length;
+
+        /* The driver owns clearing of HIP slots for following scenario
+         * - driver has requested a MA-PACKET.req signal
+         * - The f/w after receiving the signal decides it can't send it out due to various reasons
+         * - So the f/w without downloading the bulk data decides to just send a confirmation with fail
+         * - and then sends a clear slot signal to HIP
+         *
+         * But in some cases the clear slot signal never comes and the slot remains --NOT-- freed for ever
+         *
+         * To handle this, HIP will keep the record of host tag for each occupied slot
+         * and then based on status of that Host tag and slot the driver will decide if the slot is
+         * cleared by f/w signal or the slot has to be freed by driver
+         */
+
+        if (card->fh_slot_host_tag_record)
+        {
+            /* Update the f-h slot record for the corresponding host tag */
+            host_tag = GET_PACKED_MA_PACKET_REQUEST_HOST_TAG(packed_sigptr);
+            slot_num = GET_PACKED_DATAREF_SLOT(packed_sigptr, 0) & 0x00FF;
+
+            unifi_trace(card->ospriv, UDBG5,
+                        "process_fh_traffic_queue signal ID =%x fh slot=%x Host tag =%x\n",
+                        GET_SIGNAL_ID(packed_sigptr), slot_num, host_tag);
+            card->fh_slot_host_tag_record[slot_num] = host_tag;
+        }
+        UNIFI_INIT_BULK_DATA(&bulkdata.d[1]);
+        UNIFI_INIT_BULK_DATA(&csptr->bulkdata[0]);
+        UNIFI_INIT_BULK_DATA(&csptr->bulkdata[1]);
+
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+        if (bulkdata.d[0].os_data_ptr)
+        {
+            if ((*bulkdata.d[0].os_data_ptr) & 0x08)
+            {
+                card->cmd_prof.tx_count++;
+            }
+        }
+#endif
+        unifi_trace(card->ospriv, UDBG3, "Sending signal 0x%.4X\n",
+                    GET_SIGNAL_ID(packed_sigptr));
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "Sending signal 0x%.4X\n",
+                    GET_SIGNAL_ID(packed_sigptr));
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+        /* Append packed signal to F-H buffer */
+        total_length = sig_chunks * card->config_data.sig_frag_size;
+
+        card->fh_buffer.ptr[0] = (u8)(signal_length & 0xff);
+        card->fh_buffer.ptr[1] =
+            (u8)(((signal_length >> 8) & 0xf) | (SDIO_CMD_SIGNAL << 4));
+
+        memcpy(card->fh_buffer.ptr + 2, packed_sigptr, signal_length);
+        memset(card->fh_buffer.ptr + 2 + signal_length, 0,
+                  total_length - (2 + signal_length));
+
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "proc_fh: fh_buffer %d bytes \n",
+                    signal_length + 2);
+        dump(card->fh_buffer.ptr, signal_length + 2);
+        unifi_trace(card->ospriv, UDBG1, " \n");
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+        card->fh_buffer.ptr += total_length;
+        card->fh_buffer.count += sig_chunks;
+
+#ifdef CSR_WIFI_HIP_NOISY
+        unifi_error(card->ospriv, "Added %d to fh buf, len now %d, count %d\n",
+                    signal_length,
+                    card->fh_buffer.ptr - card->fh_buffer.buf,
+                    card->fh_buffer.count);
+#endif  /* CSR_WIFI_HIP_NOISY */
+
+        (*processed)++;
+        pending_sigs--;
+        pending_chunks -= sig_chunks;
+
+        /* Log the signal to the UDI. */
+        /* UDI will get the packed structure */
+        /* Can not log the unpacked signal, unless we reconstruct it! */
+        if (card->udi_hook)
+        {
+            (*card->udi_hook)(card->ospriv, packed_sigptr, signal_length,
+                              &bulkdata, UDI_LOG_FROM_HOST);
+        }
+
+        /* Remove entry from q */
+        csptr->signal_length = 0;
+        /* Note that the traffic queue has only one valid bulk data buffer. */
+        csptr->bulkdata[0].data_length = 0;
+
+        CSR_WIFI_HIP_Q_INC_R(&sigq[q_no]);
+    } while ((pending_sigs > 0) && (pending_chunks > 0) && (q_no >= 0));
+
+    return CSR_RESULT_SUCCESS;
+} /* process_fh_traffic_queue() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  flush_fh_buffer
+ *
+ *      Write out the cache from-hosts signals to the UniFi.
+ *
+ *  Arguments:
+ *      card       Pointer to card context struct
+ *
+ *  Returns:
+ *      CSR error code if an SDIO error occurred.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult flush_fh_buffer(card_t *card)
+{
+    CsrResult r;
+    u16 len;
+    u16 sig_units;
+    u16 data_round;
+    u16 chunks_in_last_block;
+    u16 padding_chunks;
+    u16 i;
+
+    len = card->fh_buffer.ptr - card->fh_buffer.buf;
+
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv, "fh_buffer is at %p, ptr= %p\n",
+                card->fh_buffer.buf, card->fh_buffer.ptr);
+#endif /* CSR_WIFI_HIP_NOISY */
+
+    if (len == 0)
+    {
+        return CSR_RESULT_SUCCESS;
+    }
+
+#ifdef CSR_WIFI_HIP_NOISY
+    if (dump_fh_buf)
+    {
+        dump(card->fh_buffer.buf, len);
+        dump_fh_buf = 0;
+    }
+#endif /* CSR_WIFI_HIP_NOISY */
+
+    if (card->sdio_io_block_pad)
+    {
+        /* Both of these are powers of 2 */
+        sig_units = card->config_data.sig_frag_size;
+        data_round = card->sdio_io_block_size;
+
+        if (data_round > sig_units)
+        {
+            chunks_in_last_block = (len % data_round) / sig_units;
+
+            if (chunks_in_last_block != 0)
+            {
+                padding_chunks = (data_round / sig_units) - chunks_in_last_block;
+
+                memset(card->fh_buffer.ptr, 0, padding_chunks * sig_units);
+                for (i = 0; i < padding_chunks; i++)
+                {
+                    card->fh_buffer.ptr[1] = SDIO_CMD_PADDING << 4;
+                    card->fh_buffer.ptr += sig_units;
+                }
+
+                card->fh_buffer.count += padding_chunks;
+                len += padding_chunks * sig_units;
+            }
+        }
+    }
+
+    r = unifi_bulk_rw(card,
+                      card->config_data.fromhost_sigbuf_handle,
+                      card->fh_buffer.buf,
+                      len, UNIFI_SDIO_WRITE);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write fh signals: %u bytes, error %d\n", len, r);
+        return r;
+    }
+
+    /* Update from-host-signals-written signal count */
+    card->from_host_signals_w =
+        (card->from_host_signals_w + card->fh_buffer.count) % 128u;
+    r = unifi_write_8_or_16(card, card->sdio_ctrl_addr + 0,
+                            (u8)card->from_host_signals_w);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write fh signal count %u with error %d\n",
+                    card->from_host_signals_w, r);
+        return r;
+    }
+    card->generate_interrupt = 1;
+
+    /* Reset the fh buffer pointer */
+    card->fh_buffer.ptr = card->fh_buffer.buf;
+    card->fh_buffer.count = 0;
+
+#ifdef CSR_WIFI_HIP_NOISY
+    unifi_error(card->ospriv, "END flush: fh len %d, count %d\n",
+                card->fh_buffer.ptr - card->fh_buffer.buf,
+                card->fh_buffer.count);
+#endif /* CSR_WIFI_HIP_NOISY */
+
+    return CSR_RESULT_SUCCESS;
+} /* flush_fh_buffer() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  restart_packet_flow
+ *
+ *      This function is called before the bottom-half thread sleeps.
+ *      It checks whether both data and signal resources are available and
+ *      then calls the OS-layer function to re-enable packet transmission.
+ *
+ *  Arguments:
+ *      card       Pointer to card context struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void restart_packet_flow(card_t *card)
+{
+    u8 q;
+
+    /*
+     * We only look at the fh_traffic_queue, because that is where packets from
+     * the network stack are placed.
+     */
+    for (q = 0; q <= UNIFI_TRAFFIC_Q_VO; q++)
+    {
+        if (card_is_tx_q_paused(card, q) &&
+            CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_traffic_queue[q]) >= RESUME_XMIT_THRESHOLD)
+        {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+            unifi_debug_log_to_buf("U");
+#endif
+            card_tx_q_unpause(card, q);
+            unifi_restart_xmit(card->ospriv, (unifi_TrafficQueue)q);
+        }
+    }
+} /* restart_packet_flow() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_card_sdio_mem.c b/drivers/staging/csr/csr_wifi_hip_card_sdio_mem.c
new file mode 100644
index 00000000000..17867f60df1
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_card_sdio_mem.c
@@ -0,0 +1,1713 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ * FILE: csr_wifi_hip_card_sdio_mem.c
+ *
+ * PURPOSE: Implementation of the Card API for SDIO.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_card.h"
+
+#define SDIO_RETRIES    3
+#define CSR_WIFI_HIP_SDIO_TRACE_DATA_LENGTH 16
+
+
+#define retryable_sdio_error(_csrResult) (((_csrResult) == CSR_SDIO_RESULT_CRC_ERROR) || ((_csrResult) == CSR_SDIO_RESULT_TIMEOUT))
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  retrying_read8
+ *  retrying_write8
+ *
+ *      These functions provide the first level of retry for SDIO operations.
+ *      If an SDIO command fails for reason of a response timeout or CRC
+ *      error, it is retried immediately. If three attempts fail we report a
+ *      failure.
+ *      If the command failed for any other reason, the failure is reported
+ *      immediately.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      funcnum         The SDIO function to access.
+ *                      Function 0 is the Card Configuration Register space,
+ *                      function 1/2 is the UniFi register space.
+ *      addr            Address to access
+ *      pdata           Pointer in which to return the value read.
+ *      data            Value to write.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS  on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult retrying_read8(card_t *card, s16 funcnum, u32 addr, u8 *pdata)
+{
+    CsrSdioFunction *sdio = card->sdio_if;
+    CsrResult r = CSR_RESULT_SUCCESS;
+    s16 retries;
+    CsrResult csrResult = CSR_RESULT_SUCCESS;
+
+    retries = 0;
+    while (retries++ < SDIO_RETRIES)
+    {
+        if (funcnum == 0)
+        {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+            unifi_debug_log_to_buf("r0@%02X", addr);
+#endif
+            csrResult = CsrSdioF0Read8(sdio, addr, pdata);
+        }
+        else
+        {
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+            unifi_error(card->ospriv,
+                        "retrying_read_f0_8: F1 8-bit reads are not allowed.\n");
+            return CSR_RESULT_FAILURE;
+#else
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+            unifi_debug_log_to_buf("r@%02X", addr);
+#endif
+            csrResult = CsrSdioRead8(sdio, addr, pdata);
+#endif
+        }
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            unifi_debug_log_to_buf("error=%X\n", csrResult);
+        }
+        else
+        {
+            unifi_debug_log_to_buf("=%X\n", *pdata);
+        }
+#endif
+        if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        }
+        /*
+         * Try again for retryable (CRC or TIMEOUT) errors,
+         * break on success or fatal error
+         */
+        if (!retryable_sdio_error(csrResult))
+        {
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+            card->cmd_prof.cmd52_count++;
+#endif
+            break;
+        }
+        unifi_trace(card->ospriv, UDBG2, "retryable SDIO error reading F%d 0x%lX\n", funcnum, addr);
+    }
+
+    if ((csrResult == CSR_RESULT_SUCCESS) && (retries > 1))
+    {
+        unifi_warning(card->ospriv, "Read succeeded after %d attempts\n", retries);
+    }
+
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read from UniFi (addr 0x%lX) after %d tries\n",
+                    addr, retries - 1);
+        /* Report any SDIO error as a general i/o error */
+        r = CSR_RESULT_FAILURE;
+    }
+
+    return r;
+} /* retrying_read8() */
+
+
+static CsrResult retrying_write8(card_t *card, s16 funcnum, u32 addr, u8 data)
+{
+    CsrSdioFunction *sdio = card->sdio_if;
+    CsrResult r = CSR_RESULT_SUCCESS;
+    s16 retries;
+    CsrResult csrResult = CSR_RESULT_SUCCESS;
+
+    retries = 0;
+    while (retries++ < SDIO_RETRIES)
+    {
+        if (funcnum == 0)
+        {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+            unifi_debug_log_to_buf("w0@%02X=%X", addr, data);
+#endif
+            csrResult = CsrSdioF0Write8(sdio, addr, data);
+        }
+        else
+        {
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+            unifi_error(card->ospriv,
+                        "retrying_write_f0_8: F1 8-bit writes are not allowed.\n");
+            return CSR_RESULT_FAILURE;
+#else
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+            unifi_debug_log_to_buf("w@%02X=%X", addr, data);
+#endif
+            csrResult = CsrSdioWrite8(sdio, addr, data);
+#endif
+        }
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            unifi_debug_log_to_buf(",error=%X", csrResult);
+        }
+        unifi_debug_string_to_buf("\n");
+#endif
+        if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        }
+        /*
+         * Try again for retryable (CRC or TIMEOUT) errors,
+         * break on success or fatal error
+         */
+        if (!retryable_sdio_error(csrResult))
+        {
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+            card->cmd_prof.cmd52_count++;
+#endif
+            break;
+        }
+        unifi_trace(card->ospriv, UDBG2, "retryable SDIO error writing %02X to F%d 0x%lX\n",
+                    data, funcnum, addr);
+    }
+
+    if ((csrResult == CSR_RESULT_SUCCESS) && (retries > 1))
+    {
+        unifi_warning(card->ospriv, "Write succeeded after %d attempts\n", retries);
+    }
+
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write to UniFi (addr 0x%lX) after %d tries\n",
+                    addr, retries - 1);
+        /* Report any SDIO error as a general i/o error */
+        r = CSR_RESULT_FAILURE;
+    }
+
+    return r;
+} /* retrying_write8() */
+
+
+static CsrResult retrying_read16(card_t *card, s16 funcnum,
+                                 u32 addr, u16 *pdata)
+{
+    CsrSdioFunction *sdio = card->sdio_if;
+    CsrResult r = CSR_RESULT_SUCCESS;
+    s16 retries;
+    CsrResult csrResult = CSR_RESULT_SUCCESS;
+
+    retries = 0;
+    while (retries++ < SDIO_RETRIES)
+    {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        unifi_debug_log_to_buf("r@%02X", addr);
+#endif
+        csrResult = CsrSdioRead16(sdio, addr, pdata);
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            unifi_debug_log_to_buf("error=%X\n", csrResult);
+        }
+        else
+        {
+            unifi_debug_log_to_buf("=%X\n", *pdata);
+        }
+#endif
+        if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        }
+
+        /*
+         * Try again for retryable (CRC or TIMEOUT) errors,
+         * break on success or fatal error
+         */
+        if (!retryable_sdio_error(csrResult))
+        {
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+            card->cmd_prof.cmd52_count++;
+#endif
+            break;
+        }
+        unifi_trace(card->ospriv, UDBG2, "retryable SDIO error reading F%d 0x%lX\n", funcnum, addr);
+    }
+
+    if ((csrResult == CSR_RESULT_SUCCESS) && (retries > 1))
+    {
+        unifi_warning(card->ospriv, "Read succeeded after %d attempts\n", retries);
+    }
+
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read from UniFi (addr 0x%lX) after %d tries\n",
+                    addr, retries - 1);
+        /* Report any SDIO error as a general i/o error */
+        r = CSR_RESULT_FAILURE;
+    }
+
+    return r;
+} /* retrying_read16() */
+
+
+static CsrResult retrying_write16(card_t *card, s16 funcnum,
+                                  u32 addr, u16 data)
+{
+    CsrSdioFunction *sdio = card->sdio_if;
+    CsrResult r = CSR_RESULT_SUCCESS;
+    s16 retries;
+    CsrResult csrResult = CSR_RESULT_SUCCESS;
+
+    retries = 0;
+    while (retries++ < SDIO_RETRIES)
+    {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        unifi_debug_log_to_buf("w@%02X=%X", addr, data);
+#endif
+        csrResult = CsrSdioWrite16(sdio, addr, data);
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            unifi_debug_log_to_buf(",error=%X", csrResult);
+        }
+        unifi_debug_string_to_buf("\n");
+#endif
+        if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        }
+
+        /*
+         * Try again for retryable (CRC or TIMEOUT) errors,
+         * break on success or fatal error
+         */
+        if (!retryable_sdio_error(csrResult))
+        {
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+            card->cmd_prof.cmd52_count++;
+#endif
+            break;
+        }
+        unifi_trace(card->ospriv, UDBG2, "retryable SDIO error writing %02X to F%d 0x%lX\n",
+                    data, funcnum, addr);
+    }
+
+    if ((csrResult == CSR_RESULT_SUCCESS) && (retries > 1))
+    {
+        unifi_warning(card->ospriv, "Write succeeded after %d attempts\n", retries);
+    }
+
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write to UniFi (addr 0x%lX) after %d tries\n",
+                    addr, retries - 1);
+        /* Report any SDIO error as a general i/o error */
+        r = CSR_RESULT_FAILURE;
+    }
+
+    return r;
+} /* retrying_write16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  sdio_read_f0
+ *
+ *      Reads a byte value from the CCCR (func 0) area of UniFi.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to read from
+ *      pdata   Pointer in which to store the read value.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+CsrResult sdio_read_f0(card_t *card, u32 addr, u8 *pdata)
+{
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_f0_r_count++;
+#endif
+    return retrying_read8(card, 0, addr, pdata);
+} /* sdio_read_f0() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  sdio_write_f0
+ *
+ *      Writes a byte value to the CCCR (func 0) area of UniFi.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to read from
+ *      data    Data value to write.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+CsrResult sdio_write_f0(card_t *card, u32 addr, u8 data)
+{
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_f0_w_count++;
+#endif
+    return retrying_write8(card, 0, addr, data);
+} /* sdio_write_f0() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_read_direct_8_or_16
+ *
+ *      Read a 8-bit value from the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to read from
+ *      pdata   Pointer in which to return data.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_read_direct_8_or_16(card_t *card, u32 addr, u8 *pdata)
+{
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+    u16 w;
+    CsrResult r;
+
+    r = retrying_read16(card, card->function, addr, &w);
+    *pdata = (u8)(w & 0xFF);
+    return r;
+#else
+    return retrying_read8(card, card->function, addr, pdata);
+#endif
+} /* unifi_read_direct_8_or_16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_write_direct_8_or_16
+ *
+ *      Write a byte value to the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to write to
+ *      data    Value to write.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error
+ *
+ *  Notes:
+ *      If 8-bit write is used, the even address *must* be written second.
+ *      This is because writes to odd bytes are cached and not committed
+ *      to memory until the preceding even address is written.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_write_direct_8_or_16(card_t *card, u32 addr, u8 data)
+{
+    if (addr & 1)
+    {
+        unifi_warning(card->ospriv,
+                      "Warning: Byte write to an odd address (0x%lX) is dangerous\n",
+                      addr);
+    }
+
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+    return retrying_write16(card, card->function, addr, (u16)data);
+#else
+    return retrying_write8(card, card->function, addr, data);
+#endif
+} /* unifi_write_direct_8_or_16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read_direct16
+ *
+ *      Read a 16-bit value from the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to read from
+ *      pdata   Pointer in which to return data.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      The even address *must* be read first. This is because reads from
+ *      odd bytes are cached and read from memory when the preceding
+ *      even address is read.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_read_direct16(card_t *card, u32 addr, u16 *pdata)
+{
+    return retrying_read16(card, card->function, addr, pdata);
+} /* unifi_read_direct16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_write_direct16
+ *
+ *      Write a 16-bit value to the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to write to
+ *      data    Value to write.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      The even address *must* be written second. This is because writes to
+ *      odd bytes are cached and not committed to memory until the preceding
+ *      even address is written.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_write_direct16(card_t *card, u32 addr, u16 data)
+{
+    return retrying_write16(card, card->function, addr, data);
+} /* unifi_write_direct16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read_direct32
+ *
+ *      Read a 32-bit value from the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card    Pointer to card structure.
+ *      addr    Address to read from
+ *      pdata   Pointer in which to return data.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_read_direct32(card_t *card, u32 addr, u32 *pdata)
+{
+    CsrResult r;
+    u16 w0, w1;
+
+    r = retrying_read16(card, card->function, addr, &w0);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    r = retrying_read16(card, card->function, addr + 2, &w1);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    *pdata = ((u32)w1 << 16) | (u32)w0;
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_read_direct32() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read_directn_match
+ *
+ *      Read multiple 8-bit values from the UniFi SDIO interface,
+ *      stopping when either we have read 'len' bytes or we have read
+ *      a octet equal to 'match'.  If 'match' is not a valid octet
+ *      then this function is the same as 'unifi_read_directn'.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      addr            Start address to read from.
+ *      pdata           Pointer to which to write data.
+ *      len             Maximum umber of bytes to read
+ *      match           The value to stop reading at.
+ *      num             Pointer to buffer to write number of bytes read
+ *
+ *  Returns:
+ *      number of octets read on success, negative error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      The even address *must* be read first. This is because reads from
+ *      odd bytes are cached and read from memory when the preceding
+ *      even address is read.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult unifi_read_directn_match(card_t *card, u32 addr, void *pdata, u16 len, s8 m, u32 *num)
+{
+    CsrResult r;
+    u32 i;
+    u8 *cptr;
+    u16 w;
+
+    *num = 0;
+
+    cptr = (u8 *)pdata;
+    for (i = 0; i < len; i += 2)
+    {
+        r = retrying_read16(card, card->function, addr, &w);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return r;
+        }
+
+        *cptr++ = ((u8)w & 0xFF);
+        if ((m >= 0) && (((s8)w & 0xFF) == m))
+        {
+            break;
+        }
+
+        if (i + 1 == len)
+        {
+            /* The len is odd. Ignore the last high byte */
+            break;
+        }
+
+        *cptr++ = ((u8)(w >> 8) & 0xFF);
+        if ((m >= 0) && (((s8)(w >> 8) & 0xFF) == m))
+        {
+            break;
+        }
+
+        addr += 2;
+    }
+
+    *num = (s32)(cptr - (u8 *)pdata);
+    return CSR_RESULT_SUCCESS;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read_directn
+ *
+ *      Read multiple 8-bit values from the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      addr            Start address to read from.
+ *      pdata           Pointer to which to write data.
+ *      len             Number of bytes to read
+ *
+ *  Returns:
+ *      0 on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      The even address *must* be read first. This is because reads from
+ *      odd bytes are cached and read from memory when the preceding
+ *      even address is read.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_read_directn(card_t *card, u32 addr, void *pdata, u16 len)
+{
+    u32 num;
+
+    return unifi_read_directn_match(card, addr, pdata, len, -1, &num);
+} /* unifi_read_directn() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_write_directn
+ *
+ *      Write multiple 8-bit values to the UniFi SDIO interface.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      addr            Start address to write to.
+ *      pdata           Source data pointer.
+ *      len             Number of bytes to write, must be even.
+ *
+ *  Returns:
+ *      0 on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      The UniFi has a peculiar 16-bit bus architecture. Writes are only
+ *      committed to memory when an even address is accessed. Writes to
+ *      odd addresses are cached and only committed if the next write is
+ *      to the preceding address.
+ *      This means we must write data as pairs of bytes in reverse order.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_write_directn(card_t *card, u32 addr, void *pdata, u16 len)
+{
+    CsrResult r;
+    u8 *cptr;
+    s16 signed_len;
+
+    cptr = (u8 *)pdata;
+    signed_len = (s16)len;
+    while (signed_len > 0)
+    {
+        /* This is UniFi-1 specific code. CSPI not supported so 8-bit write allowed */
+        r = retrying_write16(card, card->function, addr, *cptr);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return r;
+        }
+
+        cptr += 2;
+        addr += 2;
+        signed_len -= 2;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_write_directn() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  set_dmem_page
+ *  set_pmem_page
+ *
+ *      Set up the page register for the shared data memory window or program
+ *      memory window.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      dmem_addr       UniFi shared-data-memory address to access.
+ *      pmem_addr       UniFi program memory address to access. This includes
+ *                        External FLASH memory at    0x000000
+ *                        Processor program memory at 0x200000
+ *                        External SRAM at memory     0x400000
+ *      paddr           Location to write an SDIO address (24-bit) for
+ *                       use in a unifi_read_direct or unifi_write_direct call.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE card was ejected
+ *      CSR_RESULT_FAILURE an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult set_dmem_page(card_t *card, u32 dmem_addr, u32 *paddr)
+{
+    u16 page, addr;
+    u32 len;
+    CsrResult r;
+
+    *paddr = 0;
+
+    if (!ChipHelper_DecodeWindow(card->helper,
+                                 CHIP_HELPER_WINDOW_3,
+                                 CHIP_HELPER_WT_SHARED,
+                                 dmem_addr / 2,
+                                 &page, &addr, &len))
+    {
+        unifi_error(card->ospriv, "Failed to decode SHARED_DMEM_PAGE %08lx\n", dmem_addr);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    if (page != card->dmem_page)
+    {
+        unifi_trace(card->ospriv, UDBG6, "setting dmem page=0x%X, addr=0x%lX\n", page, addr);
+
+        /* change page register */
+        r = unifi_write_direct16(card, ChipHelper_HOST_WINDOW3_PAGE(card->helper) * 2, page);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to write SHARED_DMEM_PAGE\n");
+            return r;
+        }
+
+        card->dmem_page = page;
+    }
+
+    *paddr = ((s32)addr * 2) + (dmem_addr & 1);
+
+    return CSR_RESULT_SUCCESS;
+} /* set_dmem_page() */
+
+
+static CsrResult set_pmem_page(card_t *card, u32 pmem_addr,
+                               enum chip_helper_window_type mem_type, u32 *paddr)
+{
+    u16 page, addr;
+    u32 len;
+    CsrResult r;
+
+    *paddr = 0;
+
+    if (!ChipHelper_DecodeWindow(card->helper,
+                                 CHIP_HELPER_WINDOW_2,
+                                 mem_type,
+                                 pmem_addr / 2,
+                                 &page, &addr, &len))
+    {
+        unifi_error(card->ospriv, "Failed to decode PROG MEM PAGE %08lx %d\n", pmem_addr, mem_type);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    if (page != card->pmem_page)
+    {
+        unifi_trace(card->ospriv, UDBG6, "setting pmem page=0x%X, addr=0x%lX\n", page, addr);
+
+        /* change page register */
+        r = unifi_write_direct16(card, ChipHelper_HOST_WINDOW2_PAGE(card->helper) * 2, page);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to write PROG MEM PAGE\n");
+            return r;
+        }
+
+        card->pmem_page = page;
+    }
+
+    *paddr = ((s32)addr * 2) + (pmem_addr & 1);
+
+    return CSR_RESULT_SUCCESS;
+} /* set_pmem_page() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  set_page
+ *
+ *      Sets up the appropriate page register to access the given address.
+ *      Returns the sdio address at which the unifi address can be accessed.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      generic_addr    UniFi internal address to access, in Generic Pointer
+ *                      format, i.e. top byte is space indicator.
+ *      paddr           Location to write page address
+ *                          SDIO address (24-bit) for use in a unifi_read_direct or
+ *                          unifi_write_direct call
+ *
+ *  Returns:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE  the address is invalid
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult set_page(card_t *card, u32 generic_addr, u32 *paddr)
+{
+    s32 space;
+    u32 addr;
+    CsrResult r = CSR_RESULT_SUCCESS;
+
+    if (!paddr)
+    {
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+    *paddr = 0;
+    space = UNIFI_GP_SPACE(generic_addr);
+    addr = UNIFI_GP_OFFSET(generic_addr);
+    switch (space)
+    {
+        case UNIFI_SH_DMEM:
+            /* Shared Data Memory is accessed via the Shared Data Memory window */
+            r = set_dmem_page(card, addr, paddr);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            break;
+
+        case UNIFI_EXT_FLASH:
+            if (!ChipHelper_HasFlash(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08lX (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            /* External FLASH is accessed via the Program Memory window */
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_FLASH, paddr);
+            break;
+
+        case UNIFI_EXT_SRAM:
+            if (!ChipHelper_HasExtSram(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08l (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            /* External SRAM is accessed via the Program Memory window */
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_EXT_SRAM, paddr);
+            break;
+
+        case UNIFI_REGISTERS:
+            /* Registers are accessed directly */
+            *paddr = addr;
+            break;
+
+        case UNIFI_PHY_DMEM:
+            r = unifi_set_proc_select(card, UNIFI_PROC_PHY);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            *paddr = ChipHelper_DATA_MEMORY_RAM_OFFSET(card->helper) * 2 + addr;
+            break;
+
+        case UNIFI_MAC_DMEM:
+            r = unifi_set_proc_select(card, UNIFI_PROC_MAC);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            *paddr = ChipHelper_DATA_MEMORY_RAM_OFFSET(card->helper) * 2 + addr;
+            break;
+
+        case UNIFI_BT_DMEM:
+            if (!ChipHelper_HasBt(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08lX (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            r = unifi_set_proc_select(card, UNIFI_PROC_BT);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            *paddr = ChipHelper_DATA_MEMORY_RAM_OFFSET(card->helper) * 2 + addr;
+            break;
+
+        case UNIFI_PHY_PMEM:
+            r = unifi_set_proc_select(card, UNIFI_PROC_PHY);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_CODE_RAM, paddr);
+            break;
+
+        case UNIFI_MAC_PMEM:
+            r = unifi_set_proc_select(card, UNIFI_PROC_MAC);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_CODE_RAM, paddr);
+            break;
+
+        case UNIFI_BT_PMEM:
+            if (!ChipHelper_HasBt(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08lX (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            r = unifi_set_proc_select(card, UNIFI_PROC_BT);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_CODE_RAM, paddr);
+            break;
+
+        case UNIFI_PHY_ROM:
+            if (!ChipHelper_HasRom(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08lX (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            r = unifi_set_proc_select(card, UNIFI_PROC_PHY);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_ROM, paddr);
+            break;
+
+        case UNIFI_MAC_ROM:
+            if (!ChipHelper_HasRom(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08lX (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            r = unifi_set_proc_select(card, UNIFI_PROC_MAC);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_ROM, paddr);
+            break;
+
+        case UNIFI_BT_ROM:
+            if (!ChipHelper_HasRom(card->helper) || !ChipHelper_HasBt(card->helper))
+            {
+                unifi_error(card->ospriv, "Bad address space for chip in generic pointer 0x%08lX (helper=0x%x)\n",
+                            generic_addr, card->helper);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            r = unifi_set_proc_select(card, UNIFI_PROC_BT);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                return r;
+            }
+            r = set_pmem_page(card, addr, CHIP_HELPER_WT_ROM, paddr);
+            break;
+
+        default:
+            unifi_error(card->ospriv, "Bad address space %d in generic pointer 0x%08lX (helper=0x%x)\n",
+                        space, generic_addr, card->helper);
+            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    return r;
+} /* set_page() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_set_proc_select
+ *
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      select          Which XAP core to select
+ *
+ *  Returns:
+ *      0 on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_set_proc_select(card_t *card, enum unifi_dbg_processors_select select)
+{
+    CsrResult r;
+
+    /* Verify the the select value is allowed. */
+    switch (select)
+    {
+        case UNIFI_PROC_MAC:
+        case UNIFI_PROC_PHY:
+        case UNIFI_PROC_BOTH:
+            break;
+
+
+        default:
+            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    if (card->proc_select != (u32)select)
+    {
+        r = unifi_write_direct16(card,
+                                 ChipHelper_DBG_HOST_PROC_SELECT(card->helper) * 2,
+                                 (u8)select);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to write to Proc Select register\n");
+            return r;
+        }
+
+        card->proc_select = (u32)select;
+    }
+
+    return CSR_RESULT_SUCCESS;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_read_8_or_16
+ *
+ * Performs a byte read of the given address in shared data memory.
+ * Set up the shared data memory page register as required.
+ *
+ * Arguments:
+ * card Pointer to card structure.
+ * unifi_addr UniFi shared-data-memory address to access.
+ * pdata Pointer to a byte variable for the value read.
+ *
+ * Returns:
+ * CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ * CSR_WIFI_HIP_RESULT_NO_DEVICE card was ejected
+ * CSR_RESULT_FAILURE an SDIO error occurred
+ * CSR_WIFI_HIP_RESULT_INVALID_VALUE a bad generic pointer was specified
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_read_8_or_16(card_t *card, u32 unifi_addr, u8 *pdata)
+{
+    u32 sdio_addr;
+    CsrResult r;
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+    u16 w;
+#endif
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_r8or16_count++;
+#endif
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+    r = retrying_read16(card, card->function, sdio_addr, &w);
+    *pdata = (u8)(w & 0xFF);
+    return r;
+#else
+    return retrying_read8(card, card->function, sdio_addr, pdata);
+#endif
+} /* unifi_read_8_or_16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_write_8_or_16
+ *
+ * Performs a byte write of the given address in shared data memory.
+ * Set up the shared data memory page register as required.
+ *
+ * Arguments:
+ * card Pointer to card context struct.
+ * unifi_addr UniFi shared-data-memory address to access.
+ * data Value to write.
+ *
+ * Returns:
+ * CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ * CSR_WIFI_HIP_RESULT_NO_DEVICE card was ejected
+ * CSR_RESULT_FAILURE an SDIO error occurred
+ * CSR_WIFI_HIP_RESULT_INVALID_VALUE a bad generic pointer was specified
+ *
+ * Notes:
+ * Beware using unifi_write8() because byte writes are not safe on UniFi.
+ * Writes to odd bytes are cached, writes to even bytes perform a 16-bit
+ * write with the previously cached odd byte.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_write_8_or_16(card_t *card, u32 unifi_addr, u8 data)
+{
+    u32 sdio_addr;
+    CsrResult r;
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+    u16 w;
+#endif
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    if (sdio_addr & 1)
+    {
+        unifi_warning(card->ospriv,
+                      "Warning: Byte write to an odd address (0x%lX) is dangerous\n",
+                      sdio_addr);
+    }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_w8or16_count++;
+#endif
+#ifdef CSR_WIFI_TRANSPORT_CSPI
+    w = data;
+    return retrying_write16(card, card->function, sdio_addr, w);
+#else
+    return retrying_write8(card, card->function, sdio_addr, data);
+#endif
+} /* unifi_write_8_or_16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_card_read16
+ *
+ *      Performs a 16-bit read of the given address in shared data memory.
+ *      Set up the shared data memory page register as required.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      unifi_addr      UniFi shared-data-memory address to access.
+ *      pdata           Pointer to a 16-bit int variable for the value read.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE  a bad generic pointer was specified
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_card_read16(card_t *card, u32 unifi_addr, u16 *pdata)
+{
+    u32 sdio_addr;
+    CsrResult r;
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_r16_count++;
+#endif
+    return unifi_read_direct16(card, sdio_addr, pdata);
+} /* unifi_card_read16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_card_write16
+ *
+ *      Performs a 16-bit write of the given address in shared data memory.
+ *      Set up the shared data memory page register as required.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      unifi_addr      UniFi shared-data-memory address to access.
+ *      pdata           Pointer to a byte variable for the value write.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE  a bad generic pointer was specified
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_card_write16(card_t *card, u32 unifi_addr, u16 data)
+{
+    u32 sdio_addr;
+    CsrResult r;
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_w16_count++;
+#endif
+    return unifi_write_direct16(card, sdio_addr, data);
+} /* unifi_card_write16() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read32
+ *
+ *      Performs a 32-bit read of the given address in shared data memory.
+ *      Set up the shared data memory page register as required.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      unifi_addr      UniFi shared-data-memory address to access.
+ *      pdata           Pointer to a int variable for the value read.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE  a bad generic pointer was specified
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_read32(card_t *card, u32 unifi_addr, u32 *pdata)
+{
+    u32 sdio_addr;
+    CsrResult r;
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+    card->cmd_prof.cmd52_r32_count++;
+#endif
+    return unifi_read_direct32(card, sdio_addr, pdata);
+} /* unifi_read32() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_card_readn
+ *  unifi_readnz
+ *
+ *      Read multiple 8-bit values from the UniFi SDIO interface.
+ *      This function interprets the address as a GenericPointer as
+ *      defined in the UniFi Host Interface Protocol Specification.
+ *      The readnz version of this function will stop when it reads a
+ *      zero octet.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      unifi_addr      UniFi shared-data-memory address to access.
+ *      pdata           Pointer to which to write data.
+ *      len             Number of bytes to read
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE  a bad generic pointer was specified
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_readn_match(card_t *card, u32 unifi_addr, void *pdata, u16 len, s8 match)
+{
+    u32 sdio_addr;
+    CsrResult r;
+    u32 num;
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    r = unifi_read_directn_match(card, sdio_addr, pdata, len, match, &num);
+    return r;
+} /* unifi_readn_match() */
+
+
+CsrResult unifi_card_readn(card_t *card, u32 unifi_addr, void *pdata, u16 len)
+{
+    return unifi_readn_match(card, unifi_addr, pdata, len, -1);
+} /* unifi_card_readn() */
+
+
+CsrResult unifi_readnz(card_t *card, u32 unifi_addr, void *pdata, u16 len)
+{
+    return unifi_readn_match(card, unifi_addr, pdata, len, 0);
+} /* unifi_readnz() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read_shared_count
+ *
+ *      Read signal count locations, checking for an SDIO error.  The
+ *      signal count locations only contain a valid number if the
+ *      highest bit isn't set.
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure.
+ *      addr            Shared-memory address to read.
+ *
+ *  Returns:
+ *      Value read from memory (0-127) or -1 on error
+ * ---------------------------------------------------------------------------
+ */
+s32 unifi_read_shared_count(card_t *card, u32 addr)
+{
+    u8 b;
+    /* I've increased this count, because I have seen cases where
+     * there were three reads in a row with the top bit set.  I'm not
+     * sure why this might have happened, but I can't see a problem
+     * with increasing this limit.  It's better to take a while to
+     * recover than to fail. */
+#define SHARED_READ_RETRY_LIMIT 10
+    s32 i;
+
+    /*
+     * Get the to-host-signals-written count.
+     * The top-bit will be set if the firmware was in the process of
+     * changing the value, in which case we read again.
+     */
+    /* Limit the number of repeats so we don't freeze */
+    for (i = 0; i < SHARED_READ_RETRY_LIMIT; i++)
+    {
+        CsrResult r;
+        r = unifi_read_8_or_16(card, addr, &b);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return -1;
+        }
+        if (!(b & 0x80))
+        {
+            /* There is a chance that the MSB may have contained invalid data
+             * (overflow) at the time it was read. Therefore mask off the MSB.
+             * This avoids a race between driver read and firmware write of the
+             * word, the value we need is in the lower 8 bits anway.
+             */
+            return (s32)(b & 0xff);
+        }
+    }
+
+    return -1;                  /* this function has changed in WMM mods */
+} /* unifi_read_shared_count() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_writen
+ *
+ *      Write multiple 8-bit values to the UniFi SDIO interface using CMD52
+ *      This function interprets the address as a GenericPointer as
+ *      defined in the UniFi Host Interface Protocol Specification.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      unifi_addr      UniFi shared-data-memory address to access.
+ *      pdata           Pointer to which to write data.
+ *      len             Number of bytes to write
+ *
+ *  Returns:
+ *      0 on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE    an odd length or length too big.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_writen(card_t *card, u32 unifi_addr, void *pdata, u16 len)
+{
+    u32 sdio_addr;
+    CsrResult r;
+
+    r = set_page(card, unifi_addr, &sdio_addr);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    return unifi_write_directn(card, sdio_addr, pdata, len);
+} /* unifi_writen() */
+
+
+static CsrResult csr_sdio_block_rw(card_t *card, s16 funcnum,
+                                   u32 addr, u8 *pdata,
+                                   u16 count, s16 dir_is_write)
+{
+    CsrResult csrResult;
+
+    if (dir_is_write == UNIFI_SDIO_READ)
+    {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        unifi_debug_log_to_buf("r@%02X#%X=", addr, count);
+#endif
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+        unifi_debug_log_to_buf("R");
+#endif
+        csrResult = CsrSdioRead(card->sdio_if, addr, pdata, count);
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+        unifi_debug_log_to_buf("<");
+#endif
+    }
+    else
+    {
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+        unifi_debug_log_to_buf("w@%02X#%X=", addr, count);
+        unifi_debug_hex_to_buf(pdata, count > CSR_WIFI_HIP_SDIO_TRACE_DATA_LENGTH?CSR_WIFI_HIP_SDIO_TRACE_DATA_LENGTH : count);
+#endif
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+        unifi_debug_log_to_buf("W");
+#endif
+        csrResult = CsrSdioWrite(card->sdio_if, addr, pdata, count);
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+        unifi_debug_log_to_buf(">");
+#endif
+    }
+#ifdef CSR_WIFI_HIP_DATA_PLANE_PROFILE
+    card->cmd_prof.cmd53_count++;
+#endif
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_SDIO_TRACE)
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_debug_log_to_buf("error=%X", csrResult);
+    }
+    else if (dir_is_write == UNIFI_SDIO_READ)
+    {
+        unifi_debug_hex_to_buf(pdata, count > CSR_WIFI_HIP_SDIO_TRACE_DATA_LENGTH?CSR_WIFI_HIP_SDIO_TRACE_DATA_LENGTH : count);
+    }
+    unifi_debug_string_to_buf("\n");
+#endif
+    return csrResult;  /* CSR SDIO (not HIP) error code */
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_bulk_rw
+ *
+ *      Transfer bulk data to or from the UniFi SDIO interface.
+ *      This function is used to read or write signals and bulk data.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      handle          Value to put in the Register Address field of the CMD53 req.
+ *      data            Pointer to data to write.
+ *      direction       One of UNIFI_SDIO_READ or UNIFI_SDIO_WRITE
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      This function uses SDIO CMD53, which is the block transfer mode.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_bulk_rw(card_t *card, u32 handle, void *pdata,
+                        u32 len, s16 direction)
+{
+#define CMD53_RETRIES 3
+    /*
+     * Ideally instead of sleeping, we want to busy wait.
+     * Currently there is no framework API to do this. When it becomes available,
+     * we can use it to busy wait using usecs
+     */
+#define REWIND_RETRIES          15    /* when REWIND_DELAY==1msec, or 250 when REWIND_DELAY==50usecs */
+#define REWIND_POLLING_RETRIES  5
+#define REWIND_DELAY            1     /* msec or 50usecs */
+    CsrResult csrResult;              /* SDIO error code */
+    CsrResult r = CSR_RESULT_SUCCESS; /* HIP error code */
+    s16 retries = CMD53_RETRIES;
+    s16 stat_retries;
+    u8 stat;
+    s16 dump_read;
+#ifdef UNIFI_DEBUG
+    u8 *pdata_lsb = ((u8 *)&pdata) + card->lsb;
+#endif
+#ifdef CSR_WIFI_MAKE_FAKE_CMD53_ERRORS
+    static s16 fake_error;
+#endif
+
+    dump_read = 0;
+#ifdef UNIFI_DEBUG
+    if (*pdata_lsb & 1)
+    {
+        unifi_notice(card->ospriv, "CD53 request on a unaligned buffer (addr: 0x%X) dir %s-Host\n",
+                     pdata, (direction == UNIFI_SDIO_READ)?"To" : "From");
+        if (direction == UNIFI_SDIO_WRITE)
+        {
+            dump(pdata, (u16)len);
+        }
+        else
+        {
+            dump_read = 1;
+        }
+    }
+#endif
+
+    /* Defensive checks */
+    if (!pdata)
+    {
+        unifi_error(card->ospriv, "Null pdata for unifi_bulk_rw() len: %d\n", len);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+    if ((len & 1) || (len > 0xffff))
+    {
+        unifi_error(card->ospriv, "Impossible CMD53 length requested: %d\n", len);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    while (1)
+    {
+        csrResult = csr_sdio_block_rw(card, card->function, handle,
+                                      (u8 *)pdata, (u16)len,
+                                      direction);
+        if (csrResult == CSR_SDIO_RESULT_NO_DEVICE)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_DEVICE;
+        }
+#ifdef CSR_WIFI_MAKE_FAKE_CMD53_ERRORS
+        if (++fake_error > 100)
+        {
+            fake_error = 90;
+            unifi_warning(card->ospriv, "Faking a CMD53 error,\n");
+            if (csrResult == CSR_RESULT_SUCCESS)
+            {
+                csrResult = CSR_RESULT_FAILURE;
+            }
+        }
+#endif
+        if (csrResult == CSR_RESULT_SUCCESS)
+        {
+            if (dump_read)
+            {
+                dump(pdata, (u16)len);
+            }
+            break;
+        }
+
+        /*
+         * At this point the SDIO driver should have written the I/O Abort
+         * register to notify UniFi that the command has failed.
+         * UniFi-1 and UniFi-2 (not UF6xxx) use the same register to store the
+         * Deep Sleep State. This means we have to restore the Deep Sleep
+         * State (AWAKE in any case since we can not perform a CD53 in any other
+         * state) by rewriting the I/O Abort register to its previous value.
+         */
+        if (card->chip_id <= SDIO_CARD_ID_UNIFI_2)
+        {
+            (void)unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+        }
+
+        /* If csr_sdio_block_rw() failed in a non-retryable way, or retries exhausted
+         * then stop retrying
+         */
+        if (!retryable_sdio_error(csrResult))
+        {
+            unifi_error(card->ospriv, "Fatal error in a CMD53 transfer\n");
+            break;
+        }
+
+        /*
+         * These happen from time to time, try again
+         */
+        if (--retries == 0)
+        {
+            break;
+        }
+
+        unifi_trace(card->ospriv, UDBG4,
+                    "Error in a CMD53 transfer, retrying (h:%d,l:%u)...\n",
+                    (s16)handle & 0xff, len);
+
+        /* The transfer failed, rewind and try again */
+        r = unifi_write_8_or_16(card, card->sdio_ctrl_addr + 8,
+                                (u8)(handle & 0xff));
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            /*
+             * If we can't even do CMD52 (register read/write) then
+             * stop here.
+             */
+            unifi_error(card->ospriv, "Failed to write REWIND cmd\n");
+            return r;
+        }
+
+        /* Signal the UniFi to look for the rewind request. */
+        r = CardGenInt(card);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return r;
+        }
+
+        /* Wait for UniFi to acknowledge the rewind */
+        stat_retries = REWIND_RETRIES;
+        while (1)
+        {
+            r = unifi_read_8_or_16(card, card->sdio_ctrl_addr + 8, &stat);
+            if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+            {
+                return r;
+            }
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "Failed to read REWIND status\n");
+                return CSR_RESULT_FAILURE;
+            }
+
+            if (stat == 0)
+            {
+                break;
+            }
+            if (--stat_retries == 0)
+            {
+                unifi_error(card->ospriv, "Timeout waiting for REWIND ready\n");
+                return CSR_RESULT_FAILURE;
+            }
+
+            /* Poll for the ack a few times */
+            if (stat_retries < REWIND_RETRIES - REWIND_POLLING_RETRIES)
+            {
+                CsrThreadSleep(REWIND_DELAY);
+            }
+        }
+    }
+
+    /* The call to csr_sdio_block_rw() still failed after retrying */
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Block %s failed after %d retries\n",
+                    (direction == UNIFI_SDIO_READ)?"read" : "write",
+                    CMD53_RETRIES - retries);
+        /* Report any SDIO error as a general i/o error */
+        return CSR_RESULT_FAILURE;
+    }
+
+    /* Collect some stats */
+    if (direction == UNIFI_SDIO_READ)
+    {
+        card->sdio_bytes_read += len;
+    }
+    else
+    {
+        card->sdio_bytes_written += len;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_bulk_rw() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_bulk_rw_noretry
+ *
+ *      Transfer bulk data to or from the UniFi SDIO interface.
+ *      This function is used to read or write signals and bulk data.
+ *
+ *  Arguments:
+ *      card            Pointer to card structure.
+ *      handle          Value to put in the Register Address field of
+ *                      the CMD53 req.
+ *      data            Pointer to data to write.
+ *      direction       One of UNIFI_SDIO_READ or UNIFI_SDIO_WRITE
+ *
+ *  Returns:
+ *      0 on success, non-zero error code on error:
+ *      CSR_WIFI_HIP_RESULT_NO_DEVICE  card was ejected
+ *      CSR_RESULT_FAILURE     an SDIO error occurred
+ *
+ *  Notes:
+ *      This function uses SDIO CMD53, which is the block transfer mode.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_bulk_rw_noretry(card_t *card, u32 handle, void *pdata,
+                                u32 len, s16 direction)
+{
+    CsrResult csrResult;
+
+    csrResult = csr_sdio_block_rw(card, card->function, handle,
+                                  (u8 *)pdata, (u16)len, direction);
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Block %s failed\n",
+                    (direction == UNIFI_SDIO_READ)?"read" : "write");
+        return csrResult;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_bulk_rw_noretry() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_chiphelper.c b/drivers/staging/csr/csr_wifi_hip_chiphelper.c
new file mode 100644
index 00000000000..5cf5b8a5a1e
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_chiphelper.c
@@ -0,0 +1,793 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_macro.h"
+#include "csr_wifi_hip_chiphelper_private.h"
+
+#ifndef nelem
+#define nelem(a) (sizeof(a) / sizeof(a[0]))
+#endif
+
+#define counted(foo) { nelem(foo), foo }
+#define null_counted()  { 0, NULL }
+
+/* The init values are a set of register writes that we must
+   perform when we first connect to the chip to get it working.
+   They swicth on the correct clocks and possibly set the host
+   interface as a wkaeup source.  They should not be used if
+   proper HIP opperation is required, but are useful before we
+   do a code download. */
+static const struct chip_helper_init_values init_vals_v1[] = {
+    { 0xFDBB, 0xFFFF },
+    { 0xFDB6, 0x03FF },
+    { 0xFDB1, 0x01E3 },
+    { 0xFDB3, 0x0FFF },
+    { 0xFEE3, 0x08F0 },
+    { 0xFEE7, 0x3C3F },
+    { 0xFEE6, 0x0050 },
+    { 0xFDBA, 0x0000 }
+};
+
+static const struct chip_helper_init_values init_vals_v2[] = {
+    { 0xFDB6, 0x0FFF },
+    { 0xF023, 0x3F3F },
+    { 0xFDB1, 0x01E3 },
+    { 0xFDB3, 0x0FFF },
+    { 0xF003, 0x08F0 },
+    { 0xF007, 0x3C3F },
+    { 0xF006, 0x0050 }
+};
+
+
+static const struct chip_helper_init_values init_vals_v22_v23[] = {
+    { 0xF81C, 0x00FF },
+    /*{ 0x????, 0x???? }, */
+    { 0xF80C, 0x1FFF },
+    { 0xFA25, 0x001F },
+    { 0xF804, 0x00FF },
+    { 0xF802, 0x0FFF },
+    /*{ 0x????, 0x???? },
+      { 0x????, 0x???? },
+      { 0x????, 0x???? }*/
+};
+
+static const u16 reset_program_a_v1_or_v2[] = {
+    0x0000
+};
+static const u16 reset_program_b_v1_or_v2[] = {
+    0x0010, 0xFE00, 0xA021, 0xFF00, 0x8111, 0x0009, 0x0CA4, 0x0114,
+    0x0280, 0x04F8, 0xFE00, 0x6F25, 0x06E0, 0x0010, 0xFC00, 0x0121,
+    0xFC00, 0x0225, 0xFE00, 0x7125, 0xFE00, 0x6D11, 0x03F0, 0xFE00,
+    0x6E25, 0x0008, 0x00E0
+};
+
+static const struct chip_helper_reset_values reset_program_v1_or_v2[] =
+{
+    {
+        MAKE_GP(REGISTERS, 0x000C),
+        nelem(reset_program_a_v1_or_v2),
+        reset_program_a_v1_or_v2
+    },
+    {
+        MAKE_GP(MAC_PMEM, 0x000000),
+        nelem(reset_program_b_v1_or_v2),
+        reset_program_b_v1_or_v2
+    }
+};
+
+static const struct chip_map_address_t unifi_map_address_v1_v2[] =
+{
+    { 0xFE9F, 0xFE7B },     /* PM1_BANK_SELECT */
+    { 0xFE9E, 0xFE78 },     /* PM2_BANK_SELECT */
+    { 0xFE9D, 0xFE7E },     /* SHARED_DMEM_PAGE */
+    { 0xFE91, 0xFE90 },     /* PROC_SELECT */
+    { 0xFE8D, 0xFE8C },     /* STOP_STATUS */
+};
+
+static const struct chip_map_address_t unifi_map_address_v22_v23[] =
+{
+    { 0xF8F9, 0xF8AC },     /* GW1_CONFIG */
+    { 0xF8FA, 0xF8AD },     /* GW2_CONFIG */
+    { 0xF8FB, 0xF8AE },     /* GW3_CONFIG */
+    { 0xF830, 0xF81E },     /* PROC_SELECT */
+    { 0xF831, 0xF81F },     /* STOP_STATUS */
+    { 0xF8FC, 0xF8AF },     /* IO_LOG_ADDRESS */
+};
+
+static const struct chip_device_regs_t unifi_device_regs_null =
+{
+    0xFE81,                     /* GBL_CHIP_VERSION */
+    0x0000,                     /* GBL_MISC_ENABLES */
+    0x0000,                     /* DBG_EMU_CMD */
+    {
+        0x0000,                 /* HOST.DBG_PROC_SELECT */
+        0x0000,                 /* HOST.DBG_STOP_STATUS */
+        0x0000,                 /* HOST.WINDOW1_PAGE */
+        0x0000,                 /* HOST.WINDOW2_PAGE */
+        0x0000,                 /* HOST.WINDOW3_PAGE */
+        0x0000                  /* HOST.IO_LOG_ADDR */
+    },
+    {
+        0x0000,                 /* SPI.DBG_PROC_SELECT */
+        0x0000,                 /* SPI.DBG_STOP_STATUS */
+        0x0000,                 /* SPI.WINDOW1_PAGE */
+        0x0000,                 /* SPI.WINDOW2_PAGE */
+        0x0000,                 /* SPI.WINDOW3_PAGE */
+        0x0000                  /* SPI.IO_LOG_ADDR */
+    },
+    0x0000,                     /* DBG_RESET */
+    0x0000,                     /* > DBG_RESET_VALUE */
+    0x0000,                     /* DBG_RESET_WARN */
+    0x0000,                     /* DBG_RESET_WARN_VALUE */
+    0x0000,                     /* DBG_RESET_RESULT */
+    0xFFE9,                     /* XAP_PCH */
+    0xFFEA,                     /* XAP_PCL */
+    0x0000,                     /* PROC_PC_SNOOP */
+    0x0000,                     /* WATCHDOG_DISABLE */
+    0x0000,                     /* MAILBOX0 */
+    0x0000,                     /* MAILBOX1 */
+    0x0000,                     /* MAILBOX2 */
+    0x0000,                     /* MAILBOX3 */
+    0x0000,                     /* SDIO_HOST_INT */
+    0x0000,                     /* SHARED_IO_INTERRUPT */
+    0x0000,                     /* SDIO HIP HANDSHAKE */
+    0x0000                      /* COEX_STATUS */
+};
+
+/* UF105x */
+static const struct chip_device_regs_t unifi_device_regs_v1 =
+{
+    0xFE81,                     /* GBL_CHIP_VERSION */
+    0xFE87,                     /* GBL_MISC_ENABLES */
+    0xFE9C,                     /* DBG_EMU_CMD */
+    {
+        0xFE90,                 /* HOST.DBG_PROC_SELECT */
+        0xFE8C,                 /* HOST.DBG_STOP_STATUS */
+        0xFE7B,                 /* HOST.WINDOW1_PAGE */
+        0xFE78,                 /* HOST.WINDOW2_PAGE */
+        0xFE7E,                 /* HOST.WINDOW3_PAGE */
+        0x0000                  /* HOST.IO_LOG_ADDR */
+    },
+    {
+        0xFE91,                 /* SPI.DBG_PROC_SELECT */
+        0xFE8D,                 /* SPI.DBG_STOP_STATUS */
+        0xFE9F,                 /* SPI.WINDOW1_PAGE */
+        0xFE9E,                 /* SPI.WINDOW2_PAGE */
+        0xFE9D,                 /* SPI.WINDOW3_PAGE */
+        0x0000                  /* SPI.IO_LOG_ADDR */
+    },
+    0xFE92,                     /* DBG_RESET */
+    0x0001,                     /* > DBG_RESET_VALUE */
+    0xFDA0,                     /* DBG_RESET_WARN (HOST_SELECT) */
+    0x0000,                     /* DBG_RESET_WARN_VALUE */
+    0xFE92,                     /* DBG_RESET_RESULT */
+    0xFFE9,                     /* XAP_PCH */
+    0xFFEA,                     /* XAP_PCL */
+    0x0051,                     /* PROC_PC_SNOOP */
+    0xFE70,                     /* WATCHDOG_DISABLE */
+    0xFE6B,                     /* MAILBOX0 */
+    0xFE6A,                     /* MAILBOX1 */
+    0xFE69,                     /* MAILBOX2 */
+    0xFE68,                     /* MAILBOX3 */
+    0xFE67,                     /* SDIO_HOST_INT */
+    0xFE65,                     /* SHARED_IO_INTERRUPT */
+    0xFDE9,                     /* SDIO HIP HANDSHAKE */
+    0x0000                      /* COEX_STATUS */
+};
+
+/* UF2... */
+static const struct chip_device_regs_t unifi_device_regs_v2 =
+{
+    0xFE81,                     /* GBL_CHIP_VERSION */
+    0xFE87,                     /* GBL_MISC_ENABLES */
+    0xFE9C,                     /* DBG_EMU_CMD */
+    {
+        0xFE90,                 /* HOST.DBG_PROC_SELECT */
+        0xFE8C,                 /* HOST.DBG_STOP_STATUS */
+        0xFE7B,                 /* HOST.WINDOW1_PAGE */
+        0xFE78,                 /* HOST.WINDOW2_PAGE */
+        0xFE7E,                 /* HOST.WINDOW3_PAGE */
+        0x0000                  /* HOST.IO_LOG_ADDR */
+    },
+    {
+        0xFE91,                 /* SPI.DBG_PROC_SELECT */
+        0xFE8D,                 /* SPI.DBG_STOP_STATUS */
+        0xFE9F,                 /* SPI.WINDOW1_PAGE */
+        0xFE9E,                 /* SPI.WINDOW2_PAGE */
+        0xFE9D,                 /* SPI.WINDOW3_PAGE */
+        0x0000                  /* SPI.IO_LOG_ADDR */
+    },
+    0xFE92,                     /* DBG_RESET */
+    0x0000,                     /* > DBG_RESET_VALUE */
+    0xFDE9,                     /* DBG_RESET_WARN (TEST_FLASH_DATA - SHARED_MAILBOX2B) */
+    0xFFFF,                     /* DBG_RESET_WARN_VALUE */
+    0xFDE9,                     /* DBG_RESET_RESULT (TEST_FLASH_DATA) */
+    0xFFE9,                     /* XAP_PCH */
+    0xFFEA,                     /* XAP_PCL */
+    0x0051,                     /* PROC_PC_SNOOP */
+    0xFE70,                     /* WATCHDOG_DISABLE */
+    0xFE6B,                     /* MAILBOX0 */
+    0xFE6A,                     /* MAILBOX1 */
+    0xFE69,                     /* MAILBOX2 */
+    0xFE68,                     /* MAILBOX3 */
+    0xFE67,                     /* SDIO_HOST_INT */
+    0xFE65,                     /* SHARED_IO_INTERRUPT */
+    0xFE69,                     /* SDIO HIP HANDSHAKE */
+    0x0000                      /* COEX_STATUS */
+};
+
+/* UF60xx */
+static const struct chip_device_regs_t unifi_device_regs_v22_v23 =
+{
+    0xFE81,                     /* GBL_CHIP_VERSION */
+    0xF84F,                     /* GBL_MISC_ENABLES */
+    0xF81D,                     /* DBG_EMU_CMD */
+    {
+        0xF81E,                 /* HOST.DBG_PROC_SELECT */
+        0xF81F,                 /* HOST.DBG_STOP_STATUS */
+        0xF8AC,                 /* HOST.WINDOW1_PAGE */
+        0xF8AD,                 /* HOST.WINDOW2_PAGE */
+        0xF8AE,                 /* HOST.WINDOW3_PAGE */
+        0xF8AF                  /* HOST.IO_LOG_ADDR */
+    },
+    {
+        0xF830,                 /* SPI.DBG_PROC_SELECT */
+        0xF831,                 /* SPI.DBG_STOP_STATUS */
+        0xF8F9,                 /* SPI.WINDOW1_PAGE */
+        0xF8FA,                 /* SPI.WINDOW2_PAGE */
+        0xF8FB,                 /* SPI.WINDOW3_PAGE */
+        0xF8FC                  /* SPI.IO_LOG_ADDR */
+    },
+    0xF82F,                     /* DBG_RESET */
+    0x0001,                     /* > DBG_RESET_VALUE */
+    0x0000,                     /* DBG_RESET_WARN */
+    0x0000,                     /* DBG_RESET_WARN_VALUE */
+    0xF82F,                     /* DBG_RESET_RESULT */
+    0xFFE9,                     /* XAP_PCH */
+    0xFFEA,                     /* XAP_PCL */
+    0x001B,                     /* PROC_PC_SNOOP */
+    0x0055,                     /* WATCHDOG_DISABLE */
+    0xF84B,                     /* MAILBOX0 */
+    0xF84C,                     /* MAILBOX1 */
+    0xF84D,                     /* MAILBOX2 */
+    0xF84E,                     /* MAILBOX3 */
+    0xF92F,                     /* SDIO_HOST_INT */
+    0xF92B,                     /* SDIO_FROMHOST_SCRTACH0 / SHARED_IO_INTERRUPT */
+    0xF84D,                     /* SDIO HIP HANDSHAKE (MAILBOX2) */
+    0xF9FB                      /* COEX_STATUS */
+};
+
+/* Program memory window on UF105x. */
+static const struct window_shift_info_t prog_window_array_unifi_v1_v2[CHIP_HELPER_WT_COUNT] =
+{
+    { TRUE, 11, 0x0200 }, /* CODE RAM */
+    { TRUE, 11, 0x0000 }, /* FLASH */
+    { TRUE, 11, 0x0400 }, /* External SRAM */
+    { FALSE, 0, 0 },      /* ROM */
+    { FALSE, 0, 0 }       /* SHARED */
+};
+
+/* Shared memory window on UF105x. */
+static const struct window_shift_info_t shared_window_array_unifi_v1_v2[CHIP_HELPER_WT_COUNT] =
+{
+    { FALSE, 0, 0 },      /* CODE RAM */
+    { FALSE, 0, 0 },      /* FLASH */
+    { FALSE, 0, 0 },      /* External SRAM */
+    { FALSE, 0, 0 },      /* ROM */
+    { TRUE, 11, 0x0000 }  /* SHARED */
+};
+
+/* One of the Generic Windows on UF60xx and later. */
+static const struct window_shift_info_t generic_window_array_unifi_v22_v23[CHIP_HELPER_WT_COUNT] =
+{
+    { TRUE, 11, 0x3800 }, /* CODE RAM */
+    { FALSE, 0, 0 },      /* FLASH */
+    { FALSE, 0, 0 },      /* External SRAM */
+    { TRUE, 11, 0x2000 }, /* ROM */
+    { TRUE, 11, 0x0000 }  /* SHARED */
+};
+
+/* The three windows on UF105x. */
+static const struct window_info_t prog1_window_unifi_v1_v2  = { 0x0000, 0x2000, 0x0080, prog_window_array_unifi_v1_v2 };
+static const struct window_info_t prog2_window_unifi_v1_v2  = { 0x2000, 0x2000, 0x0000, prog_window_array_unifi_v1_v2 };
+static const struct window_info_t shared_window_unifi_v1_v2 = { 0x4000, 0x2000, 0x0000, shared_window_array_unifi_v1_v2 };
+
+/* The three windows on UF60xx and later. */
+static const struct window_info_t generic1_window_unifi_v22_v23 = { 0x0000, 0x2000, 0x0080, generic_window_array_unifi_v22_v23 };
+static const struct window_info_t generic2_window_unifi_v22_v23 = { 0x2000, 0x2000, 0x0000, generic_window_array_unifi_v22_v23 };
+static const struct window_info_t generic3_window_unifi_v22_v23 = { 0x4000, 0x2000, 0x0000, generic_window_array_unifi_v22_v23 };
+
+static const struct chip_device_desc_t chip_device_desc_null =
+{
+    { FALSE, 0x0000, 0x0000, 0x00 },
+    "",
+    "",
+    null_counted(),                         /* init */
+    null_counted(),                         /* reset_prog */
+    &unifi_device_regs_null,                /* regs */
+    {
+        FALSE,                              /* has_flash */
+        FALSE,                              /* has_ext_sram */
+        FALSE,                              /* has_rom */
+        FALSE,                              /* has_bt */
+        FALSE,                              /* has_wlan */
+    },
+    null_counted(),
+    /* prog_offset */
+    {
+        0x00000000,
+        0x00000000,
+        0x00000000,
+        0x00000000
+    },
+    /* data_offset */
+    {
+        0x0000                              /* ram */
+    },
+    /* windows */
+    {
+        NULL,
+        NULL,
+        NULL
+    }
+};
+
+static const struct chip_device_desc_t unifi_device_desc_v1 =
+{
+    { FALSE, 0xf0ff, 0x1001, 0x01 },        /* UF105x R01 */
+    "UF105x",
+    "UniFi-1",
+    counted(init_vals_v1),                  /* init */
+    counted(reset_program_v1_or_v2),        /* reset_prog */
+    &unifi_device_regs_v1,                  /* regs */
+    {
+        TRUE,                               /* has_flash    */
+        TRUE,                               /* has_ext_sram */
+        FALSE,                              /* has_rom      */
+        FALSE,                              /* has_bt       */
+        TRUE,                               /* has_wlan */
+    },
+    counted(unifi_map_address_v1_v2),       /* map */
+    /* prog_offset */
+    {
+        0x00100000,                         /* ram */
+        0x00000000,                         /* rom (invalid) */
+        0x00000000,                         /* flash */
+        0x00200000,                         /* ext_ram */
+    },
+    /* data_offset */
+    {
+        0x8000                              /* ram */
+    },
+    /* windows */
+    {
+        &prog1_window_unifi_v1_v2,
+        &prog2_window_unifi_v1_v2,
+        &shared_window_unifi_v1_v2
+    }
+};
+
+static const struct chip_device_desc_t unifi_device_desc_v2 =
+{
+    { FALSE, 0xf0ff, 0x2001, 0x02 },        /* UF2... R02 */
+    "UF2...",
+    "UniFi-2",
+    counted(init_vals_v2),                  /* init */
+    counted(reset_program_v1_or_v2),        /* reset_prog */
+    &unifi_device_regs_v2,                  /* regs */
+    {
+        TRUE,                               /* has_flash    */
+        TRUE,                               /* has_ext_sram */
+        FALSE,                              /* has_rom      */
+        FALSE,                              /* has_bt      */
+        TRUE,                               /* has_wlan */
+    },
+    counted(unifi_map_address_v1_v2),       /* map */
+    /* prog_offset */
+    {
+        0x00100000,                         /* ram */
+        0x00000000,                         /* rom (invalid) */
+        0x00000000,                         /* flash */
+        0x00200000,                         /* ext_ram */
+    },
+    /* data_offset */
+    {
+        0x8000                              /* ram */
+    },
+    /* windows */
+    {
+        &prog1_window_unifi_v1_v2,
+        &prog2_window_unifi_v1_v2,
+        &shared_window_unifi_v1_v2
+    }
+};
+
+static const struct chip_device_desc_t unifi_device_desc_v3 =
+{
+    { FALSE, 0xf0ff, 0x3001, 0x02 },        /* UF2... R03 */
+    "UF2...",
+    "UniFi-3",
+    counted(init_vals_v2),                  /* init */
+    counted(reset_program_v1_or_v2),        /* reset_prog */
+    &unifi_device_regs_v2,                  /* regs */
+    {
+        TRUE,                               /* has_flash    */
+        TRUE,                               /* has_ext_sram */
+        FALSE,                              /* has_rom      */
+        FALSE,                              /* has_bt      */
+        TRUE,                               /* has_wlan */
+    },
+    counted(unifi_map_address_v1_v2),       /* map */
+    /* prog_offset */
+    {
+        0x00100000,                         /* ram */
+        0x00000000,                         /* rom (invalid) */
+        0x00000000,                         /* flash */
+        0x00200000,                         /* ext_ram */
+    },
+    /* data_offset */
+    {
+        0x8000                              /* ram */
+    },
+    /* windows */
+    {
+        &prog1_window_unifi_v1_v2,
+        &prog2_window_unifi_v1_v2,
+        &shared_window_unifi_v1_v2
+    }
+};
+
+static const struct chip_device_desc_t unifi_device_desc_v22 =
+{
+    { FALSE, 0x00ff, 0x0022, 0x07 },        /* UF60xx */
+    "UF60xx",
+    "UniFi-4",
+    counted(init_vals_v22_v23),             /* init */
+    null_counted(),                         /* reset_prog */
+    &unifi_device_regs_v22_v23,             /* regs */
+    {
+        FALSE,                              /* has_flash    */
+        FALSE,                              /* has_ext_sram */
+        TRUE,                               /* has_rom      */
+        FALSE,                              /* has_bt       */
+        TRUE,                               /* has_wlan */
+    },
+    counted(unifi_map_address_v22_v23),     /* map */
+    /* prog_offset */
+    {
+        0x00C00000,                         /* ram */
+        0x00000000,                         /* rom */
+        0x00000000,                         /* flash (invalid) */
+        0x00000000,                         /* ext_ram (invalid) */
+    },
+    /* data_offset */
+    {
+        0x8000                              /* ram */
+    },
+    /* windows */
+    {
+        &generic1_window_unifi_v22_v23,
+        &generic2_window_unifi_v22_v23,
+        &generic3_window_unifi_v22_v23
+    }
+};
+
+static const struct chip_device_desc_t unifi_device_desc_v23 =
+{
+    { FALSE, 0x00ff, 0x0023, 0x08 },        /* UF.... */
+    "UF....",
+    "UF.... (5)",
+    counted(init_vals_v22_v23),             /* init */
+    null_counted(),                         /* reset_prog */
+    &unifi_device_regs_v22_v23,             /* regs */
+    {
+        FALSE,                              /* has_flash    */
+        FALSE,                              /* has_ext_sram */
+        TRUE,                               /* has_rom      */
+        TRUE,                               /* has_bt       */
+        TRUE,                               /* has_wlan */
+    },
+    counted(unifi_map_address_v22_v23),
+    /* prog_offset */
+    {
+        0x00C00000,                         /* ram */
+        0x00000000,                         /* rom */
+        0x00000000,                         /* flash (invalid) */
+        0x00000000,                         /* ext_sram (invalid) */
+    },
+    /* data_offset */
+    {
+        0x8000                              /* ram */
+    },
+    /* windows */
+    {
+        &generic1_window_unifi_v22_v23,
+        &generic2_window_unifi_v22_v23,
+        &generic3_window_unifi_v22_v23
+    }
+};
+
+static const struct chip_device_desc_t hyd_wlan_subsys_desc_v1 =
+{
+    { FALSE, 0x00ff, 0x0044, 0x00 },        /* UF.... */
+    "HYD...",
+    "HYD...    ",
+    counted(init_vals_v22_v23),             /* init */
+    null_counted(),                         /* reset_prog */
+    &unifi_device_regs_v22_v23,             /* regs */
+    {
+        FALSE,                              /* has_flash    */
+        FALSE,                              /* has_ext_sram */
+        TRUE,                               /* has_rom      */
+        FALSE,                              /* has_bt       */
+        TRUE,                               /* has_wlan */
+    },
+    counted(unifi_map_address_v22_v23),
+    /* prog_offset */
+    {
+        0x00C00000,                         /* ram */
+        0x00000000,                         /* rom */
+        0x00000000,                         /* flash (invalid) */
+        0x00000000,                         /* ext_sram (invalid) */
+    },
+    /* data_offset */
+    {
+        0x8000                              /* ram */
+    },
+    /* windows */
+    {
+        &generic1_window_unifi_v22_v23,
+        &generic2_window_unifi_v22_v23,
+        &generic3_window_unifi_v22_v23
+    }
+};
+
+
+/* This is the list of all chips that we know about.  I'm
+   assuming that the order here will be important - we
+   might have multiple entries witrh the same SDIO id for
+   instance.  The first one in this list will be the one
+   that is returned if a search is done on only that id.
+   The client will then have to call GetVersionXXX again
+   but with more detailed info.
+
+   I don't know if we need to signal this up to the client
+   in some way?
+
+   (We get the SDIO id before we know anything else about
+   the chip.  We might not be able to read any of the other
+   registers at first, but we still need to know about the
+   chip). */
+static const struct chip_device_desc_t *chip_ver_to_desc[] =
+{
+    &unifi_device_desc_v1,      /* UF105x R01 */
+    &unifi_device_desc_v2,      /* UF2... R02 */
+    &unifi_device_desc_v3,      /* UF2... R03 */
+    &unifi_device_desc_v22,     /* UF60xx */
+    &unifi_device_desc_v23,     /* UF.... */
+    &hyd_wlan_subsys_desc_v1
+};
+
+ChipDescript* ChipHelper_GetVersionSdio(u8 sdio_ver)
+{
+    u32 i;
+
+    for (i = 0; i < nelem(chip_ver_to_desc); i++)
+    {
+        if (chip_ver_to_desc[i]->chip_version.sdio == sdio_ver)
+        {
+            return chip_ver_to_desc[i];
+        }
+    }
+
+    return &chip_device_desc_null;
+}
+
+
+ChipDescript* ChipHelper_GetVersionAny(u16 from_FF9A, u16 from_FE81)
+{
+    u32 i;
+
+    if ((from_FF9A & 0xFF00) != 0)
+    {
+        for (i = 0; i < nelem(chip_ver_to_desc); i++)
+        {
+            if (chip_ver_to_desc[i]->chip_version.pre_bc7 &&
+                ((from_FF9A & chip_ver_to_desc[i]->chip_version.mask) ==
+                 chip_ver_to_desc[i]->chip_version.result))
+            {
+                return chip_ver_to_desc[i];
+            }
+        }
+    }
+    else
+    {
+        for (i = 0; i < nelem(chip_ver_to_desc); i++)
+        {
+            if (!chip_ver_to_desc[i]->chip_version.pre_bc7 &&
+                ((from_FE81 & chip_ver_to_desc[i]->chip_version.mask) ==
+                 chip_ver_to_desc[i]->chip_version.result))
+            {
+                return chip_ver_to_desc[i];
+            }
+        }
+    }
+
+    return &chip_device_desc_null;
+}
+
+
+ChipDescript* ChipHelper_GetVersionUniFi(u16 ver)
+{
+    return ChipHelper_GetVersionAny(0x0000, ver);
+}
+
+
+ChipDescript *ChipHelper_Null(void)
+{
+    return &chip_device_desc_null;
+}
+
+
+ChipDescript* ChipHelper_GetVersionBlueCore(enum chip_helper_bluecore_age bc_age, u16 version)
+{
+    if (bc_age == chip_helper_bluecore_pre_bc7)
+    {
+        return ChipHelper_GetVersionAny(version, 0x0000);
+    }
+    else
+    {
+        return ChipHelper_GetVersionAny(0x0000, version);
+    }
+}
+
+
+/* Expand the DEF0 functions into simple code to return the
+   correct thing.  The DEF1 functions expand to nothing in
+   this X macro expansion. */
+#define CHIP_HELPER_DEF0_C_DEF(ret_type, name, info)            \
+    ret_type ChipHelper_ ## name(ChipDescript * chip_help)           \
+    {                                                               \
+        return chip_help->info;                                     \
+    }
+#define CHIP_HELPER_DEF1_C_DEF(ret_type, name, type1, name1)
+
+CHIP_HELPER_LIST(C_DEF)
+
+/*
+ * Map register addresses between HOST and SPI access.
+ */
+u16 ChipHelper_MapAddress_SPI2HOST(ChipDescript *chip_help, u16 addr)
+{
+    u32 i;
+    for (i = 0; i < chip_help->map.len; i++)
+    {
+        if (chip_help->map.vals[i].spi == addr)
+        {
+            return chip_help->map.vals[i].host;
+        }
+    }
+    return addr;
+}
+
+
+u16 ChipHelper_MapAddress_HOST2SPI(ChipDescript *chip_help, u16 addr)
+{
+    u32 i;
+    for (i = 0; i < chip_help->map.len; i++)
+    {
+        if (chip_help->map.vals[i].host == addr)
+        {
+            return chip_help->map.vals[i].spi;
+        }
+    }
+    return addr;
+}
+
+
+/* The address returned by this function is the start of the
+   window in the address space, that is where we can start
+   accessing data from.  If a section of the window at the
+   start is unusable because something else is cluttering up
+   the address map then that is taken into account and this
+   function returns that address justt past that. */
+u16 ChipHelper_WINDOW_ADDRESS(ChipDescript                 *chip_help,
+                                    enum chip_helper_window_index window)
+{
+    if (window < CHIP_HELPER_WINDOW_COUNT &&
+        chip_help->windows[window] != NULL)
+    {
+        return chip_help->windows[window]->address + chip_help->windows[window]->blocked;
+    }
+    return 0;
+}
+
+
+/* This returns the size of the window minus any blocked section */
+u16 ChipHelper_WINDOW_SIZE(ChipDescript                 *chip_help,
+                                 enum chip_helper_window_index window)
+{
+    if (window < CHIP_HELPER_WINDOW_COUNT &&
+        chip_help->windows[window] != NULL)
+    {
+        return chip_help->windows[window]->size - chip_help->windows[window]->blocked;
+    }
+    return 0;
+}
+
+
+/* Get the register writes we should do to make sure that
+   the chip is running with most clocks on. */
+u32 ChipHelper_ClockStartupSequence(ChipDescript                          *chip_help,
+                                          const struct chip_helper_init_values **val)
+{
+    *val = chip_help->init.vals;
+    return chip_help->init.len;
+}
+
+
+/* Get the set of values tat we should write to the chip to perform a reset. */
+u32 ChipHelper_HostResetSequence(ChipDescript                           *chip_help,
+                                       const struct chip_helper_reset_values **val)
+{
+    *val = chip_help->reset_prog.vals;
+    return chip_help->reset_prog.len;
+}
+
+
+/* Decode a windowed access to the chip. */
+s32 ChipHelper_DecodeWindow(ChipDescript *chip_help,
+                                 enum chip_helper_window_index window,
+                                 enum chip_helper_window_type type,
+                                 u32 offset,
+                                 u16 *page, u16 *addr, u32 *len)
+{
+    const struct window_info_t *win;
+    const struct window_shift_info_t *mode;
+    u16 of, pg;
+
+    if (window >= CHIP_HELPER_WINDOW_COUNT)
+    {
+        return FALSE;
+    }
+    if ((win = chip_help->windows[window]) == NULL)
+    {
+        return FALSE;
+    }
+    if (type >= CHIP_HELPER_WT_COUNT)
+    {
+        return FALSE;
+    }
+    if ((mode = &win->mode[type]) == NULL)
+    {
+        return FALSE;
+    }
+    if (!mode->allowed)
+    {
+        return FALSE;
+    }
+
+    pg = (u16)(offset >> mode->page_shift) + mode->page_offset;
+    of = (u16)(offset & ((1 << mode->page_shift) - 1));
+    /* If 'blocked' is zero this does nothing, else decrease
+       the page register and increase the offset until we aren't
+       in the blocked region of the window. */
+    while (of < win->blocked)
+    {
+        of += 1 << mode->page_shift;
+        pg--;
+    }
+    *page = pg;
+    *addr = win->address + of;
+    *len = win->size - of;
+    return TRUE;
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_chiphelper.h b/drivers/staging/csr/csr_wifi_hip_chiphelper.h
new file mode 100644
index 00000000000..24737ae8a2d
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_chiphelper.h
@@ -0,0 +1,471 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_HIP_CHIPHELPER_H__
+#define CSR_WIFI_HIP_CHIPHELPER_H__
+
+
+#include <linux/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* The age of the BlueCore chip.  This is probably not useful, if
+   you know the age then you can probably work out the version directly. */
+enum chip_helper_bluecore_age
+{
+    chip_helper_bluecore_pre_bc7,
+    chip_helper_bluecore_bc7_or_later
+};
+
+/* We support up to three windowed regions at the moment.
+   Don't reorder these - they're used to index into an array. */
+enum chip_helper_window_index
+{
+    CHIP_HELPER_WINDOW_1        = 0,
+    CHIP_HELPER_WINDOW_2        = 1,
+    CHIP_HELPER_WINDOW_3        = 2,
+    CHIP_HELPER_WINDOW_COUNT    = 3
+};
+
+/* These are the things that we can access through a window.
+   Don't reorder these - they're used to index into an array. */
+enum chip_helper_window_type
+{
+    CHIP_HELPER_WT_CODE_RAM = 0,
+    CHIP_HELPER_WT_FLASH    = 1,
+    CHIP_HELPER_WT_EXT_SRAM = 2,
+    CHIP_HELPER_WT_ROM      = 3,
+    CHIP_HELPER_WT_SHARED   = 4,
+    CHIP_HELPER_WT_COUNT    = 5
+};
+
+/* Commands to stop and start the XAP */
+enum chip_helper_dbg_emu_cmd_enum
+{
+    CHIP_HELPER_DBG_EMU_CMD_XAP_STEP_MASK   = 0x0001,
+    CHIP_HELPER_DBG_EMU_CMD_XAP_RUN_B_MASK  = 0x0002,
+    CHIP_HELPER_DBG_EMU_CMD_XAP_BRK_MASK    = 0x0004,
+    CHIP_HELPER_DBG_EMU_CMD_XAP_WAKEUP_MASK = 0x0008
+};
+
+/* Bitmasks for Stop and sleep status: DBG_SPI_STOP_STATUS & DBG_HOST_STOP_STATUS */
+enum chip_helper_dbg_stop_status_enum
+{
+    CHIP_HELPER_DBG_STOP_STATUS_NONE_MASK               = 0x0000,
+    CHIP_HELPER_DBG_STOP_STATUS_P0_MASK                 = 0x0001,
+    CHIP_HELPER_DBG_STOP_STATUS_P1_MASK                 = 0x0002,
+    CHIP_HELPER_DBG_STOP_STATUS_P2_MASK                 = 0x0004,
+    CHIP_HELPER_DBG_STOP_STATUS_SLEEP_STATUS_P0_MASK    = 0x0008,
+    CHIP_HELPER_DBG_STOP_STATUS_SLEEP_STATUS_P1_MASK    = 0x0010,
+    CHIP_HELPER_DBG_STOP_STATUS_SLEEP_STATUS_P2_MASK    = 0x0020,
+    /* Legacy names/alias */
+    CHIP_HELPER_DBG_STOP_STATUS_MAC_MASK                = 0x0001,
+    CHIP_HELPER_DBG_STOP_STATUS_PHY_MASK                = 0x0002,
+    CHIP_HELPER_DBG_STOP_STATUS_BT_MASK                 = 0x0004,
+    CHIP_HELPER_DBG_STOP_STATUS_SLEEP_STATUS_MAC_MASK   = 0x0008,
+    CHIP_HELPER_DBG_STOP_STATUS_SLEEP_STATUS_PHY_MASK   = 0x0010,
+    CHIP_HELPER_DBG_STOP_STATUS_SLEEP_STATUS_BT_MASK    = 0x0020
+};
+
+/* Codes to disable the watchdog */
+enum chip_helper_watchdog_disable_enum
+{
+    CHIP_HELPER_WATCHDOG_DISABLE_CODE1 = 0x6734,
+    CHIP_HELPER_WATCHDOG_DISABLE_CODE2 = 0xD6BF,
+    CHIP_HELPER_WATCHDOG_DISABLE_CODE3 = 0xC31E
+};
+
+/* Other bits have changed between versions */
+enum chip_helper_gbl_misc_enum
+{
+    CHIP_HELPER_GBL_MISC_SPI_STOP_OUT_EN_MASK  = 0x0001,
+    CHIP_HELPER_GBL_MISC_MMU_INIT_DONE_MASK    = 0x0004
+};
+
+/* Coex status register, contains interrupt status and reset pullup status.
+ * CHIP_HELPER_COEX_STATUS_RST_PULLS_MSB_MASK can be used to check
+ * for WAPI on R03 chips and later. */
+enum chip_helper_coex_status_mask_enum
+{
+    CHIP_HELPER_COEX_STATUS_RST_PULLS_LSB_MASK   = 0x0001,
+    CHIP_HELPER_COEX_STATUS_RST_PULLS_MSB_MASK   = 0x0008,
+    CHIP_HELPER_COEX_STATUS_WL_FEC_PINS_LSB_MASK = 0x0010,
+    CHIP_HELPER_COEX_STATUS_WL_FEC_PINS_MSB_MASK = 0x0080,
+    CHIP_HELPER_COEX_STATUS_INT_UART_MASK        = 0x0100,
+    CHIP_HELPER_COEX_STATUS_INT_BT_LEG_MASK      = 0x0200
+};
+
+/* How to select the different CPUs */
+enum chip_helper_dbg_proc_sel_enum
+{
+    CHIP_HELPER_DBG_PROC_SEL_MAC  = 0,
+    CHIP_HELPER_DBG_PROC_SEL_PHY  = 1,
+    CHIP_HELPER_DBG_PROC_SEL_BT   = 2,
+    CHIP_HELPER_DBG_PROC_SEL_NONE = 2,
+    CHIP_HELPER_DBG_PROC_SEL_BOTH = 3
+};
+
+/* These are the only registers that we have to know the
+   address of before we know the chip version. */
+enum chip_helper_fixed_registers
+{
+    /* This is the address of GBL_CHIP_VERISON on BC7,
+       UF105x, UF60xx and
+       anything later than that. */
+    CHIP_HELPER_UNIFI_GBL_CHIP_VERSION  = 0xFE81,
+
+    CHIP_HELPER_OLD_BLUECORE_GBL_CHIP_VERSION = 0xFF9A
+
+                                                /* This isn't used at the moment (but might be needed
+                                                to distinguish the BlueCore sub version?) */
+                                                /* CHIP_HELPER_OLD_BLUECORE_ANA_VERSION_ID = 0xFF7D */
+};
+
+/* Address-value pairs for defining initialisation values */
+struct chip_helper_init_values
+{
+    u16 addr;
+    u16 value;
+};
+
+/* A block of data that should be written to the device */
+struct chip_helper_reset_values
+{
+    u32        gp_address;
+    u32        len;
+    const u16 *data;
+};
+
+/*
+ * This is the C API.
+ */
+
+/* opaque type */
+typedef const struct chip_device_desc_t ChipDescript;
+
+/* Return a NULL descriptor */
+ChipDescript* ChipHelper_Null(void);
+
+/* This should get the correct version for any CSR chip.
+   The two parameters are what is read from addresses
+   0xFF9A and 0xFE81 (OLD_BLUECORE_GBL_CHIP_VERSION and
+   UNIFI_GBL_CHIP_VERSION).  These should give a unique identity
+   for most (all?) chips.
+
+   FF9A is the old GBL_CHIP_VERSION register.  If the high
+   eight bits are zero then the chip is a new (BC7 +) one
+   and FE81 is the _new_ GBL_CHIP_VERSION register. */
+ChipDescript* ChipHelper_GetVersionAny(u16 from_FF9A, u16 from_FE81);
+
+/* The chip is a UniFi, but we don't know which type
+   The parameter is the value of UNIFI_GBL_CHIP_VERSION (0xFE81) */
+ChipDescript* ChipHelper_GetVersionUniFi(u16 version);
+
+/* This gets the version from the SDIO device id.  This only
+   gives quite a coarse grained version, so we should update once
+   we hav access to the function N registers. */
+ChipDescript* ChipHelper_GetVersionSdio(u8 sdio_version);
+
+/* The chip is some sort of BlueCore.  If "age" is "pre_bc7" then
+   "version" is what was read from FF9A.  If "age" is bc7_or_later
+   then "version" is read from FE81.  If we don't know if we're pre
+   or post BC7 then we should use "GetVersionAny". */
+ChipDescript* ChipHelper_GetVersionBlueCore(enum chip_helper_bluecore_age age,
+                                            u16                     version);
+
+/* The main functions of this class are built with an X macro.  This
+   means we can generate the C and C++ versions from the same source
+   without the two diverging.
+
+   The DEF0 functions are simple and take no parameters.  The first
+   parameter to the macro is the return type.  The second parameter
+   is the function name and the third parameter is where to get the
+   info from (this is hidden from the user).
+
+   The DEF1 functions take one parameter. This time the third macro
+   parameter is the type of this parameter, and the fourth macro
+   parameter is the name of the parameter. The bodies of these
+   functions are hand written. */
+#define CHIP_HELPER_LIST(m)                                             \
+    CHIP_HELPER_DEF0(m, (const char *, FriendlyName, friendly_name))     \
+    CHIP_HELPER_DEF0(m, (const char *, MarketingName, marketing_name))  \
+    CHIP_HELPER_DEF0(m, (u16, DBG_EMU_CMD, regs->dbg_emu_cmd))       \
+    CHIP_HELPER_DEF0(m, (u16, DBG_HOST_PROC_SELECT, regs->host.dbg_proc_select)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_HOST_STOP_STATUS, regs->host.dbg_stop_status)) \
+    CHIP_HELPER_DEF0(m, (u16, HOST_WINDOW1_PAGE, regs->host.window1_page)) \
+    CHIP_HELPER_DEF0(m, (u16, HOST_WINDOW2_PAGE, regs->host.window2_page)) \
+    CHIP_HELPER_DEF0(m, (u16, HOST_WINDOW3_PAGE, regs->host.window3_page)) \
+    CHIP_HELPER_DEF0(m, (u16, HOST_IO_LOG_ADDR, regs->host.io_log_addr)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_SPI_PROC_SELECT, regs->spi.dbg_proc_select)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_SPI_STOP_STATUS, regs->spi.dbg_stop_status)) \
+    CHIP_HELPER_DEF0(m, (u16, SPI_WINDOW1_PAGE, regs->spi.window1_page)) \
+    CHIP_HELPER_DEF0(m, (u16, SPI_WINDOW2_PAGE, regs->spi.window2_page)) \
+    CHIP_HELPER_DEF0(m, (u16, SPI_WINDOW3_PAGE, regs->spi.window3_page)) \
+    CHIP_HELPER_DEF0(m, (u16, SPI_IO_LOG_ADDR, regs->spi.io_log_addr)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_RESET, regs->dbg_reset))           \
+    CHIP_HELPER_DEF0(m, (u16, DBG_RESET_VALUE, regs->dbg_reset_value)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_RESET_WARN, regs->dbg_reset_warn)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_RESET_WARN_VALUE, regs->dbg_reset_warn_value)) \
+    CHIP_HELPER_DEF0(m, (u16, DBG_RESET_RESULT, regs->dbg_reset_result)) \
+    CHIP_HELPER_DEF0(m, (u16, WATCHDOG_DISABLE, regs->watchdog_disable)) \
+    CHIP_HELPER_DEF0(m, (u16, PROC_PC_SNOOP, regs->proc_pc_snoop))   \
+    CHIP_HELPER_DEF0(m, (u16, GBL_CHIP_VERSION, regs->gbl_chip_version)) \
+    CHIP_HELPER_DEF0(m, (u16, GBL_MISC_ENABLES, regs->gbl_misc_enables)) \
+    CHIP_HELPER_DEF0(m, (u16, XAP_PCH, regs->xap_pch))               \
+    CHIP_HELPER_DEF0(m, (u16, XAP_PCL, regs->xap_pcl))               \
+    CHIP_HELPER_DEF0(m, (u16, MAILBOX0, regs->mailbox0))             \
+    CHIP_HELPER_DEF0(m, (u16, MAILBOX1, regs->mailbox1))             \
+    CHIP_HELPER_DEF0(m, (u16, MAILBOX2, regs->mailbox2))             \
+    CHIP_HELPER_DEF0(m, (u16, MAILBOX3, regs->mailbox3))             \
+    CHIP_HELPER_DEF0(m, (u16, SDIO_HIP_HANDSHAKE, regs->sdio_hip_handshake))   \
+    CHIP_HELPER_DEF0(m, (u16, SDIO_HOST_INT, regs->sdio_host_int))   \
+    CHIP_HELPER_DEF0(m, (u16, COEX_STATUS, regs->coex_status))       \
+    CHIP_HELPER_DEF0(m, (u16, SHARED_IO_INTERRUPT, regs->shared_io_interrupt)) \
+    CHIP_HELPER_DEF0(m, (u32, PROGRAM_MEMORY_RAM_OFFSET, prog_offset.ram)) \
+    CHIP_HELPER_DEF0(m, (u32, PROGRAM_MEMORY_ROM_OFFSET, prog_offset.rom)) \
+    CHIP_HELPER_DEF0(m, (u32, PROGRAM_MEMORY_FLASH_OFFSET, prog_offset.flash)) \
+    CHIP_HELPER_DEF0(m, (u32, PROGRAM_MEMORY_EXT_SRAM_OFFSET, prog_offset.ext_sram)) \
+    CHIP_HELPER_DEF0(m, (u16, DATA_MEMORY_RAM_OFFSET, data_offset.ram)) \
+    CHIP_HELPER_DEF0(m, (s32, HasFlash, bools.has_flash))              \
+    CHIP_HELPER_DEF0(m, (s32, HasExtSram, bools.has_ext_sram))         \
+    CHIP_HELPER_DEF0(m, (s32, HasRom, bools.has_rom))                  \
+    CHIP_HELPER_DEF0(m, (s32, HasBt, bools.has_bt))                    \
+    CHIP_HELPER_DEF0(m, (s32, HasWLan, bools.has_wlan))                \
+    CHIP_HELPER_DEF1(m, (u16, WINDOW_ADDRESS, enum chip_helper_window_index, window)) \
+    CHIP_HELPER_DEF1(m, (u16, WINDOW_SIZE, enum chip_helper_window_index, window)) \
+    CHIP_HELPER_DEF1(m, (u16, MapAddress_SPI2HOST, u16, addr))          \
+    CHIP_HELPER_DEF1(m, (u16, MapAddress_HOST2SPI, u16, addr))          \
+    CHIP_HELPER_DEF1(m, (u32, ClockStartupSequence, const struct chip_helper_init_values **, val)) \
+    CHIP_HELPER_DEF1(m, (u32, HostResetSequence, const struct chip_helper_reset_values **, val))
+
+/* Some magic to help the expansion */
+#define CHIP_HELPER_DEF0(a, b) \
+    CHIP_HELPER_DEF0_ ## a b
+#define CHIP_HELPER_DEF1(a, b) \
+    CHIP_HELPER_DEF1_ ## a b
+
+/* Macros so that when we expand the list we get "C" function prototypes. */
+#define CHIP_HELPER_DEF0_C_DEC(ret_type, name, info)    \
+    ret_type ChipHelper_ ## name(ChipDescript * chip_help);
+#define CHIP_HELPER_DEF1_C_DEC(ret_type, name, type1, name1)   \
+    ret_type ChipHelper_ ## name(ChipDescript * chip_help, type1 name1);
+
+CHIP_HELPER_LIST(C_DEC)
+
+/* FriendlyName
+   MarketingName
+
+   These two functions return human readable strings that describe
+   the chip.  FriendlyName returns something that a software engineer
+   at CSR might understand.  MarketingName returns something more like
+   an external name for a CSR chip.
+*/
+/* DBG_EMU_CMD
+   WATCHDOG_DISABLE
+   PROC_PC_SNOOP
+   GBL_CHIP_VERSION
+   XAP_PCH
+   XAP_PCL
+
+   These registers are used to control the XAPs.
+*/
+/* DBG_HOST_PROC_SELECT  DBG_HOST_STOP_STATUS
+   HOST_WINDOW1_PAGE HOST_WINDOW2_PAGE HOST_WINDOW3_PAGE
+   HOST_IO_LOG_ADDR
+   DBG_SPI_PROC_SELECT  DBG_SPI_STOP_STATUS
+   SPI_WINDOW1_PAGE SPI_WINDOW2_PAGE SPI_WINDOW3_PAGE
+   SPI_IO_LOG_ADDR
+
+   These register are used to control the XAPs and the memory
+   windows, normally while debugging the code on chip.  There
+   are two versons of these registers, one for access via SPI
+   and another for access via the host interface.
+*/
+/*  DBG_RESET
+    DBG_RESET_VALUE
+    DBG_RESET_WARN
+    DBG_RESET_WARN_VALUE
+    DBG_RESET_RESULT
+
+    These registers are used to reset the XAP.  This can be
+    quite complex for some chips.  If DBG_RESET_WARN is non
+    zero the DBG_RESET_WARN_VALUE should be written to address
+    DBG_RESET_WARN before the reset is perfeormed.  DBG_RESET_VALUE
+    should then be written to DBG_RESET to make the reset happen.
+    The DBG_RESET_RESULT register should contain 0 if the reset
+    was successful.
+*/
+/*  GBL_MISC_ENABLES
+
+    This register controls some special chip features.  It
+    should be used with care is it changes quite a lot between
+    chip versions.
+*/
+/*  MAILBOX0
+    MAILBOX1
+    MAILBOX2
+    MAILBOX3
+
+    The mailbox registers are for communication between the host
+    and the firmware.  There use is described in part by the host
+    interface protcol specifcation.
+*/
+/*  SDIO_HIP_HANDSHAKE
+
+    This is one of the more important SDIO HIP registers.  On some
+    chips it has the same value as one of the mailbox registers
+    and on other chips it is different.
+*/
+/*  SDIO_HOST_INT
+    SHARED_IO_INTERRUPT
+
+    These registers are used by some versions of the host interface
+    protocol specification.  Their names should probably be changed
+    to hide the registers and to expose the functions more.
+*/
+/*  COEX_STATUS
+
+    Coex status register, contains interrupt status and reset
+    pullup status.  The latter is used to detect WAPI.
+*/
+/*  PROGRAM_MEMORY_RAM_OFFSET
+    PROGRAM_MEMORY_ROM_OFFSET
+    PROGRAM_MEMORY_FLASH_OFFSET
+    PROGRAM_MEMORY_EXT_SRAM_OFFSET
+    DATA_MEMORY_RAM_OFFSET
+
+    These are constants that describe the offset of the different
+    memory types in the two different address spaces.
+*/
+/*  HasFlash HasExtSram HasRom
+    HasBt HasWLan
+
+    These are a set of bools describing the chip.
+*/
+/*  WINDOW_ADDRESS WINDOW_SIZE
+
+    These two functions return the size and address of the windows.
+    The address is the address of the lowest value in the address
+    map that is part of the window and the size is the number of
+    visible words.
+
+    Some of the windows have thier lowest portion covered by
+    registers.  For these windows address is the first address
+    after the registers and size is the siave excluding the part
+    covered by registers.
+*/
+/*  MapAddress_SPI2HOST
+    MapAddress_HOST2SPI
+
+    The debugging interface is duplicated on UniFi and later chips
+    so that there are two versions - one over the SPI interaface and
+    the other over the SDIO interface.  These functions map the
+    registers between these two interfaces.
+*/
+/*  ClockStartupSequence
+
+    This function returns the list of register value pairs that
+    should be forced into UniFi to enable SPI communication.  This
+    set of registers is not needed if the firmware is running, but
+    will be needed if the device is being booted from cold.  These
+    register writes enable the clocks and setup the PLL to a basic
+    working state.  SPI access might be unreliable until these writes
+    have occured (And they may take mulitple goes).
+*/
+/*  HostResetSequence
+
+    This returns a number of chunks of data and generic pointers.
+    All of the XAPs should be stopped.  The data should be written
+    to the generic pointers.  The instruction pointer for the MAC
+    should then be set to the start of program memory and then the
+    MAC should be "go"d.  This will reset the chip in a reliable
+    and orderly manner without resetting the SDIO interface.  It
+    is therefore not needed if the chip is being accessed by the
+    SPI interface (the DBG_RESET_ mechanism can be used instead).
+*/
+
+/* The Decode Window function is more complex.  For the window
+   'window' it tries to return the address and page register
+   value needed to see offset 'offset' of memory type 'type'.
+
+   It return 1 on success and 0 on failure.  'page' is what
+   should be written to the page register.  'addr' is the
+   address in the XAPs 16 address map to read from.  'len'
+   is the length that we can read without having to change
+   the page registers. */
+s32 ChipHelper_DecodeWindow(ChipDescript *chip_help,
+                                 enum chip_helper_window_index window,
+                                 enum chip_helper_window_type type,
+                                 u32 offset,
+                                 u16 *page, u16 *addr, u32 *len);
+
+#ifdef __cplusplus
+/* Close the extern "C" */
+}
+
+/*
+ * This is the C++ API.
+ */
+
+class ChipHelper
+{
+public:
+    /* If this constructor is used then a GetVersionXXX function
+       should be called next. */
+    ChipHelper();
+
+    /* copy constructor */
+    ChipHelper(ChipDescript * desc);
+
+    /* The default constructor assume a BC7 / UF105x series chip
+       and that the number given is the value of UNIFI_GBL_CHIP_VERSION
+       (0xFE81) */
+    ChipHelper(u16 version);
+
+    /* This returns the C interface magic token from a C++ instance. */
+    ChipDescript* GetDescript() const
+    {
+        return m_desc;
+    }
+
+
+    /* Clear out theis class (set it to the null token). */
+    void ClearVersion();
+
+    /* Load this class with data for a specific chip. */
+    void GetVersionAny(u16 from_FF9A, u16 from_FE81);
+    void GetVersionUniFi(u16 version);
+    void GetVersionBlueCore(chip_helper_bluecore_age age, u16 version);
+    void GetVersionSdio(u8 sdio_version);
+
+    /* Helpers to build the definitions of the member functions. */
+#define CHIP_HELPER_DEF0_CPP_DEC(ret_type, name, info)    \
+    ret_type name() const;
+#define CHIP_HELPER_DEF1_CPP_DEC(ret_type, name, type1, name1)   \
+    ret_type name(type1 name1) const;
+
+    CHIP_HELPER_LIST(CPP_DEC)
+
+
+    /* The DecodeWindow function, see the description of the C version. */
+    s32 DecodeWindow(chip_helper_window_index window,
+                          chip_helper_window_type type,
+                          u32 offset,
+                          u16 &page, u16 &addr, u32 &len) const;
+
+private:
+    ChipDescript *m_desc;
+};
+
+#endif /* __cplusplus */
+
+#endif
diff --git a/drivers/staging/csr/csr_wifi_hip_chiphelper_private.h b/drivers/staging/csr/csr_wifi_hip_chiphelper_private.h
new file mode 100644
index 00000000000..cb0ea4b63e6
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_chiphelper_private.h
@@ -0,0 +1,208 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_HIP_CHIPHELPER_PRIVATE_H__
+#define CSR_WIFI_HIP_CHIPHELPER_PRIVATE_H__
+
+
+#include "csr_wifi_hip_chiphelper.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+/* This GP stuff should be somewhere else? */
+
+/* Memory spaces encoded in top byte of Generic Pointer type */
+#define UNIFI_SH_DMEM   0x01    /* Shared Data Memory */
+#define UNIFI_EXT_FLASH 0x02    /* External FLASH */
+#define UNIFI_EXT_SRAM  0x03    /* External SRAM */
+#define UNIFI_REGISTERS 0x04    /* Registers */
+#define UNIFI_PHY_DMEM  0x10    /* PHY Data Memory */
+#define UNIFI_PHY_PMEM  0x11    /* PHY Program Memory */
+#define UNIFI_PHY_ROM   0x12    /* PHY ROM */
+#define UNIFI_MAC_DMEM  0x20    /* MAC Data Memory */
+#define UNIFI_MAC_PMEM  0x21    /* MAC Program Memory */
+#define UNIFI_MAC_ROM   0x22    /* MAC ROM */
+#define UNIFI_BT_DMEM   0x30    /* BT Data Memory */
+#define UNIFI_BT_PMEM   0x31    /* BT Program Memory */
+#define UNIFI_BT_ROM    0x32    /* BT ROM */
+
+#define MAKE_GP(R, O)  (((UNIFI_ ## R) << 24) | (O))
+#define GP_OFFSET(GP)  ((GP) & 0xFFFFFF)
+#define GP_SPACE(GP)   (((GP) >> 24) & 0xFF)
+
+
+/* Address value pairs */
+struct val_array_t
+{
+    u32                             len;
+    const struct chip_helper_init_values *vals;
+};
+
+/* Just a (counted) u16 array */
+struct data_array_t
+{
+    u32        len;
+    const u16 *vals;
+};
+
+struct reset_prog_t
+{
+    u32                              len;
+    const struct chip_helper_reset_values *vals;
+};
+
+/* The addresses of registers that are equivalent but on
+   different host transports. */
+struct chip_map_address_t
+{
+    u16 spi, host;
+};
+
+struct map_array_t
+{
+    u32                        len;
+    const struct chip_map_address_t *vals;
+};
+
+struct chip_device_regs_per_transport_t
+{
+    u16 dbg_proc_select;
+    u16 dbg_stop_status;
+    u16 window1_page;    /* PROG_PMEM1 or GW1 */
+    u16 window2_page;    /* PROG_PMEM2 or GW2 */
+    u16 window3_page;    /* SHARED or GW3 */
+    u16 io_log_addr;
+};
+
+struct chip_device_regs_t
+{
+    u16                               gbl_chip_version;
+    u16                               gbl_misc_enables;
+    u16                               dbg_emu_cmd;
+    struct chip_device_regs_per_transport_t host;
+    struct chip_device_regs_per_transport_t spi;
+    u16                               dbg_reset;
+    u16                               dbg_reset_value;
+    u16                               dbg_reset_warn;
+    u16                               dbg_reset_warn_value;
+    u16                               dbg_reset_result;
+    u16                               xap_pch;
+    u16                               xap_pcl;
+    u16                               proc_pc_snoop;
+    u16                               watchdog_disable;
+    u16                               mailbox0;
+    u16                               mailbox1;
+    u16                               mailbox2;
+    u16                               mailbox3;
+    u16                               sdio_host_int;
+    u16                               shared_io_interrupt;
+    u16                               sdio_hip_handshake;
+    u16                               coex_status; /* Allows WAPI detection */
+};
+
+/* If allowed is false then this window does not provide this
+   type of access.
+   This describes how addresses should be shifted to make the
+   "page" address.  The address is shifted left by 'page_shift'
+   and then has 'page_offset' added.  This value should then be
+   written to the page register. */
+struct window_shift_info_t
+{
+    s32  allowed;
+    u32 page_shift;
+    u16 page_offset;
+};
+
+/* Each window has an address and size.  These are obvious.  It then
+   has a description for each type of memory that might be accessed
+   through it.  There might also be a start to the offset of the window.
+   This means that that number of addresses at the start of the window
+   are unusable. */
+struct window_info_t
+{
+    u16                         address;
+    u16                         size;
+    u16                         blocked;
+    const struct window_shift_info_t *mode;
+};
+
+/* If GBL_CHIP_VERSION and'ed with 'mask' and is equal to 'result'
+   then this is the correct set of info.  If pre_bc7 is true then the
+   address of GBL_CHIP_VERSION is FF9A, else its FE81. */
+struct chip_version_t
+{
+    s32  pre_bc7;
+    u16 mask;
+    u16 result;
+    u8  sdio;
+};
+
+struct chip_device_desc_t
+{
+    struct chip_version_t chip_version;
+
+    /* This is a text string that a human might find useful (BC02, UF105x) */
+    const char *friendly_name;
+    /* This is what we show to customers */
+    const char *marketing_name;
+
+    /* Initialisation values to write following a reset */
+    struct val_array_t init;
+
+    /* Binary sequence for hard reset */
+    struct reset_prog_t reset_prog;
+
+    /* The register map */
+    const struct chip_device_regs_t *regs;
+
+    /* Some misc. info on the chip */
+    struct
+    {
+        u32 has_flash     : 1;
+        u32 has_ext_sram  : 1;
+        u32 has_rom       : 1;
+        u32 has_bt        : 1;
+        u32 has_wlan      : 1;
+    } bools;
+
+    /* This table is used to remap register addresses depending on what
+       host interface is used.  On the BC7 and later chips there are
+       multiple sets of memory window registers, on for each host
+       interafce (SDIO / SPI).  The correct one is needed. */
+    struct map_array_t map;
+
+    /* The offsets into the program address space of the different types of memory.
+       The RAM offset is probably the most useful. */
+    struct
+    {
+        u32 ram;
+        u32 rom;
+        u32 flash;
+        u32 ext_sram;
+    } prog_offset;
+
+    /* The offsets into the data address space of interesting things. */
+    struct
+    {
+        u16 ram;
+        /* maybe add shared / page tables? */
+    } data_offset;
+
+    /* Information on the different windows */
+    const struct window_info_t *windows[CHIP_HELPER_WINDOW_COUNT];
+};
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* CSR_WIFI_HIP_CHIPHELPER_PRIVATE_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_conversions.h b/drivers/staging/csr/csr_wifi_hip_conversions.h
new file mode 100644
index 00000000000..7d045c06936
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_conversions.h
@@ -0,0 +1,81 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * FILE: csr_wifi_hip_conversions.h
+ *
+ * PURPOSE:
+ *      This header file provides the macros for converting to and from
+ *      wire format.
+ *      These macros *MUST* work for little-endian AND big-endian hosts.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __CSR_WIFI_HIP_CONVERSIONS_H__
+#define __CSR_WIFI_HIP_CONVERSIONS_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define SIZEOF_UINT16           2
+#define SIZEOF_UINT32           4
+#define SIZEOF_UINT64           8
+
+#define SIZEOF_SIGNAL_HEADER    6
+#define SIZEOF_DATAREF          4
+
+
+/*
+ * Macro to retrieve the signal ID from a wire-format signal.
+ */
+#define GET_SIGNAL_ID(_buf)     CSR_GET_UINT16_FROM_LITTLE_ENDIAN((_buf))
+
+/*
+ * Macros to retrieve and set the DATAREF fields in a packed (i.e. wire-format)
+ * HIP signal.
+ */
+#define GET_PACKED_DATAREF_SLOT(_buf, _ref)                             \
+    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(((_buf) + SIZEOF_SIGNAL_HEADER + ((_ref) * SIZEOF_DATAREF) + 0))
+
+#define GET_PACKED_DATAREF_LEN(_buf, _ref)                              \
+    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(((_buf) + SIZEOF_SIGNAL_HEADER + ((_ref) * SIZEOF_DATAREF) + 2))
+
+#define SET_PACKED_DATAREF_SLOT(_buf, _ref, _slot)                      \
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN((_slot), ((_buf) + SIZEOF_SIGNAL_HEADER + ((_ref) * SIZEOF_DATAREF) + 0))
+
+#define SET_PACKED_DATAREF_LEN(_buf, _ref, _len)                        \
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN((_len), ((_buf) + SIZEOF_SIGNAL_HEADER + ((_ref) * SIZEOF_DATAREF) + 2))
+
+#define GET_PACKED_MA_PACKET_REQUEST_FRAME_PRIORITY(_buf)              \
+    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(((_buf) + SIZEOF_SIGNAL_HEADER + UNIFI_MAX_DATA_REFERENCES * SIZEOF_DATAREF + 8))
+
+#define GET_PACKED_MA_PACKET_REQUEST_HOST_TAG(_buf)                     \
+    CSR_GET_UINT32_FROM_LITTLE_ENDIAN(((_buf) + SIZEOF_SIGNAL_HEADER + UNIFI_MAX_DATA_REFERENCES * SIZEOF_DATAREF + 4))
+
+#define GET_PACKED_MA_PACKET_CONFIRM_HOST_TAG(_buf)                     \
+    CSR_GET_UINT32_FROM_LITTLE_ENDIAN(((_buf) + SIZEOF_SIGNAL_HEADER + UNIFI_MAX_DATA_REFERENCES * SIZEOF_DATAREF + 8))
+
+#define GET_PACKED_MA_PACKET_CONFIRM_TRANSMISSION_STATUS(_buf)                     \
+    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(((_buf) + SIZEOF_SIGNAL_HEADER + UNIFI_MAX_DATA_REFERENCES * SIZEOF_DATAREF + 2))
+
+
+s32 get_packed_struct_size(const u8 *buf);
+CsrResult read_unpack_signal(const u8 *ptr, CSR_SIGNAL *sig);
+CsrResult write_pack(const CSR_SIGNAL *sig, u8 *ptr, u16 *sig_len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CSR_WIFI_HIP_CONVERSIONS_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_hip_download.c b/drivers/staging/csr/csr_wifi_hip_download.c
new file mode 100644
index 00000000000..8e4a4608ba5
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_download.c
@@ -0,0 +1,835 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ * FILE: csr_wifi_hip_download.c
+ *
+ * PURPOSE:
+ *      Routines for downloading firmware to UniFi.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/slab.h>
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_unifiversion.h"
+#include "csr_wifi_hip_card.h"
+#include "csr_wifi_hip_xbv.h"
+
+#undef CSR_WIFI_IGNORE_PATCH_VERSION_MISMATCH
+
+static CsrResult do_patch_download(card_t *card, void *dlpriv,
+                                   xbv1_t *pfwinfo, u32 boot_ctrl_addr);
+
+static CsrResult do_patch_convert_download(card_t *card,
+                                           void *dlpriv, xbv1_t *pfwinfo);
+
+/*
+ * ---------------------------------------------------------------------------
+ *  _find_in_slut
+ *
+ *      Find the offset of the appropriate object in the SLUT of a card
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      psym            Pointer to symbol object.
+ *                         id set up by caller
+ *                         obj will be set up by this function
+ *      pslut           Pointer to SLUT address, if 0xffffffff then it must be
+ *                         read from the chip.
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success
+ *      Non-zero on error,
+ *      CSR_WIFI_HIP_RESULT_NOT_FOUND if not found
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult _find_in_slut(card_t *card, symbol_t *psym, u32 *pslut)
+{
+    u32 slut_address;
+    u16 finger_print;
+    CsrResult r;
+    CsrResult csrResult;
+
+    /* Get SLUT address */
+    if (*pslut == 0xffffffff)
+    {
+        r = card_wait_for_firmware_to_start(card, &slut_address);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Firmware hasn't started\n");
+            func_exit_r(r);
+            return r;
+        }
+        *pslut = slut_address;
+
+        /*
+         * Firmware has started so set the SDIO bus clock to the initial speed,
+         * faster than UNIFI_SDIO_CLOCK_SAFE_HZ, to speed up the f/w download.
+         */
+        csrResult = CsrSdioMaxBusClockFrequencySet(card->sdio_if, UNIFI_SDIO_CLOCK_INIT_HZ);
+        if (csrResult != CSR_RESULT_SUCCESS)
+        {
+            r = ConvertCsrSdioToCsrHipResult(card, csrResult);
+            func_exit_r(r);
+            return r;
+        }
+        card->sdio_clock_speed = UNIFI_SDIO_CLOCK_INIT_HZ;
+    }
+    else
+    {
+        slut_address = *pslut;  /* Use previously discovered address */
+    }
+    unifi_trace(card->ospriv, UDBG4, "SLUT addr: 0x%lX\n", slut_address);
+
+    /*
+     * Check the SLUT fingerprint.
+     * The slut_address is a generic pointer so we must use unifi_card_read16().
+     */
+    unifi_trace(card->ospriv, UDBG4, "Looking for SLUT finger print\n");
+    finger_print = 0;
+    r = unifi_card_read16(card, slut_address, &finger_print);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        return r;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to read SLUT finger print\n");
+        func_exit_r(r);
+        return r;
+    }
+
+    if (finger_print != SLUT_FINGERPRINT)
+    {
+        unifi_error(card->ospriv, "Failed to find SLUT fingerprint\n");
+        func_exit_r(CSR_RESULT_FAILURE);
+        return CSR_RESULT_FAILURE;
+    }
+
+    /* Symbol table starts imedately after the fingerprint */
+    slut_address += 2;
+
+    while (1)
+    {
+        u16 id;
+        u32 obj;
+
+        r = unifi_card_read16(card, slut_address, &id);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            func_exit_r(r);
+            return r;
+        }
+        slut_address += 2;
+
+        if (id == CSR_SLT_END)
+        {
+            /* End of table reached: not found */
+            r = CSR_WIFI_HIP_RESULT_RANGE;
+            break;
+        }
+
+        r = unifi_read32(card, slut_address, &obj);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            func_exit_r(r);
+            return r;
+        }
+        slut_address += 4;
+
+        unifi_trace(card->ospriv, UDBG3, "  found SLUT id %02d.%08lx\n", id, obj);
+
+        r = CSR_WIFI_HIP_RESULT_NOT_FOUND;
+        /* Found search term? */
+        if (id == psym->id)
+        {
+            unifi_trace(card->ospriv, UDBG1, " matched SLUT id %02d.%08lx\n", id, obj);
+            psym->obj = obj;
+            r = CSR_RESULT_SUCCESS;
+            break;
+        }
+    }
+
+    func_exit_r(r);
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  do_patch_convert_download
+ *
+ *      Download the given firmware image to the UniFi, converting from FWDL
+ *      to PTDL XBV format.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      dlpriv          Pointer to source firmware image
+ *      fwinfo          Pointer to source firmware info struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on error
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult do_patch_convert_download(card_t *card, void *dlpriv, xbv1_t *pfwinfo)
+{
+    CsrResult r;
+    u32 slut_base = 0xffffffff;
+    void *pfw;
+    u32 psize;
+    symbol_t sym;
+
+    /* Reset the chip to guarantee that the ROM loader is running */
+    r = unifi_init(card);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv,
+                    "do_patch_convert_download: failed to re-init UniFi\n");
+        return r;
+    }
+
+    /* If no unifi_helper is running, the firmware version must be read */
+    if (card->build_id == 0)
+    {
+        u32 ver = 0;
+        sym.id = CSR_SLT_BUILD_ID_NUMBER;
+        sym.obj = 0; /* To be updated by _find_in_slut() */
+
+        unifi_trace(card->ospriv, UDBG1, "Need f/w version\n");
+
+        /* Find chip build id entry in SLUT */
+        r = _find_in_slut(card, &sym, &slut_base);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to find CSR_SLT_BUILD_ID_NUMBER\n");
+            return CSR_RESULT_FAILURE;
+        }
+
+        /* Read running f/w version */
+        r = unifi_read32(card, sym.obj, &ver);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to read f/w id\n");
+            return CSR_RESULT_FAILURE;
+        }
+        card->build_id = ver;
+    }
+
+    /* Convert the ptest firmware to a patch against the running firmware */
+    pfw = xbv_to_patch(card, unifi_fw_read, dlpriv, pfwinfo, &psize);
+    if (!pfw)
+    {
+        unifi_error(card->ospriv, "Failed to convert f/w to patch");
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+    else
+    {
+        void *desc;
+        sym.id = CSR_SLT_BOOT_LOADER_CONTROL;
+        sym.obj = 0; /* To be updated by _find_in_slut() */
+
+        /* Find boot loader control entry in SLUT */
+        r = _find_in_slut(card, &sym, &slut_base);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to find BOOT_LOADER_CONTROL\n");
+            return CSR_RESULT_FAILURE;
+        }
+
+        r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to wake UniFi\n");
+        }
+
+        /* Get a dlpriv for the patch buffer so that unifi_fw_read() can
+         * access it.
+         */
+        desc = unifi_fw_open_buffer(card->ospriv, pfw, psize);
+        if (!desc)
+        {
+            return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+        }
+
+        /* Download the patch */
+        unifi_info(card->ospriv, "Downloading converted f/w as patch\n");
+        r = unifi_dl_patch(card, desc, sym.obj);
+        kfree(pfw);
+        unifi_fw_close_buffer(card->ospriv, desc);
+
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Converted patch download failed\n");
+            func_exit_r(r);
+            return r;
+        }
+        else
+        {
+            unifi_trace(card->ospriv, UDBG1, "Converted patch downloaded\n");
+        }
+
+        /* This command starts the firmware */
+        r = unifi_do_loader_op(card, sym.obj + 6, UNIFI_BOOT_LOADER_RESTART);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to write loader restart cmd\n");
+        }
+
+        func_exit_r(r);
+        return r;
+    }
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_dl_firmware
+ *
+ *      Download the given firmware image to the UniFi.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      dlpriv          A context pointer from the calling function to be
+ *                      passed when calling unifi_fw_read().
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success,
+ *      CSR_WIFI_HIP_RESULT_NO_MEMORY         memory allocation failed
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         error in XBV file
+ *      CSR_RESULT_FAILURE            SDIO error
+ *
+ *  Notes:
+ *      Stops and resets the chip, does the download and runs the new
+ *      firmware.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_dl_firmware(card_t *card, void *dlpriv)
+{
+    xbv1_t *fwinfo;
+    CsrResult r;
+
+    func_enter();
+
+    fwinfo = kmalloc(sizeof(xbv1_t), GFP_KERNEL);
+    if (fwinfo == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for firmware\n");
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+
+    /*
+     * Scan the firmware file to find the TLVs we are interested in.
+     * These are:
+     *   - check we support the file format version in VERF
+     *   - SLTP Symbol Lookup Table Pointer
+     *   - FWDL firmware download segments
+     *   - FWOV firmware overlay segment
+     *   - VMEQ Register probe tests to verify matching h/w
+     */
+    r = xbv1_parse(card, unifi_fw_read, dlpriv, fwinfo);
+    if (r != CSR_RESULT_SUCCESS || fwinfo->mode != xbv_firmware)
+    {
+        unifi_error(card->ospriv, "File type is %s, expected firmware.\n",
+                    fwinfo->mode == xbv_patch?"patch" : "unknown");
+        kfree(fwinfo);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    /* UF6xxx doesn't accept firmware, only patches. Therefore we convert
+     * the file to patch format with version numbers matching the current
+     * running firmware, and then download via the patch mechanism.
+     * The sole purpose of this is to support production test firmware across
+     * different ROM releases, the test firmware being provided in non-patch
+     * format.
+     */
+    if (card->chip_id > SDIO_CARD_ID_UNIFI_2)
+    {
+        unifi_info(card->ospriv, "Must convert f/w to patch format\n");
+        r = do_patch_convert_download(card, dlpriv, fwinfo);
+    }
+    else
+    {
+        /* Older UniFi chips allowed firmware to be directly loaded onto the
+         * chip, which is no longer supported.
+         */
+        unifi_error(card->ospriv, "Only patch downloading supported\n");
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    kfree(fwinfo);
+    func_exit_r(r);
+    return r;
+} /* unifi_dl_firmware() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_dl_patch
+ *
+ *      Load the given patch set into UniFi.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      dlpriv          The os specific handle to the firmware file.
+ *      boot_ctrl       The address of the boot loader control structure.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success,
+ *      CSR_WIFI_HIP_RESULT_NO_MEMORY         memory allocation failed
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         error in XBV file
+ *      CSR_RESULT_FAILURE            SDIO error
+ *
+ *  Notes:
+ *      This ends up telling UniFi to restart.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_dl_patch(card_t *card, void *dlpriv, u32 boot_ctrl)
+{
+    xbv1_t *fwinfo;
+    CsrResult r;
+
+    func_enter();
+
+    unifi_info(card->ospriv, "unifi_dl_patch %p %08x\n", dlpriv, boot_ctrl);
+
+    fwinfo = kmalloc(sizeof(xbv1_t), GFP_KERNEL);
+    if (fwinfo == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate memory for patches\n");
+        func_exit();
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+
+    /*
+     * Scan the firmware file to find the TLVs we are interested in.
+     * These are:
+     *   - check we support the file format version in VERF
+     *   - FWID The build ID of the ROM that we can patch
+     *   - PTDL patch download segments
+     */
+    r = xbv1_parse(card, unifi_fw_read, dlpriv, fwinfo);
+    if (r != CSR_RESULT_SUCCESS || fwinfo->mode != xbv_patch)
+    {
+        kfree(fwinfo);
+        unifi_error(card->ospriv, "Failed to read in patch file\n");
+        func_exit();
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    /*
+     * We have to check the build id read from the SLUT against that
+     * for the patch file.  They have to match exactly.
+     *    "card->build_id" == XBV1.PTCH.FWID
+     */
+    if (card->build_id != fwinfo->build_id)
+    {
+        unifi_error(card->ospriv, "Wrong patch file for chip (chip = %lu, file = %lu)\n",
+                    card->build_id, fwinfo->build_id);
+        kfree(fwinfo);
+#ifndef CSR_WIFI_IGNORE_PATCH_VERSION_MISMATCH
+        func_exit();
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+#else
+        fwinfo = NULL;
+        dlpriv = NULL;
+        return CSR_RESULT_SUCCESS;
+#endif
+    }
+
+    r = do_patch_download(card, dlpriv, fwinfo, boot_ctrl);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to patch image\n");
+    }
+
+    kfree(fwinfo);
+
+    func_exit_r(r);
+    return r;
+} /* unifi_dl_patch() */
+
+
+void* unifi_dl_fw_read_start(card_t *card, s8 is_fw)
+{
+    card_info_t card_info;
+
+    unifi_card_info(card, &card_info);
+    unifi_trace(card->ospriv, UDBG5,
+                "id=%d, ver=0x%x, fw_build=%u, fw_hip=0x%x, block_size=%d\n",
+                card_info.chip_id, card_info.chip_version,
+                card_info.fw_build, card_info.fw_hip_version,
+                card_info.sdio_block_size);
+
+    return unifi_fw_read_start(card->ospriv, is_fw, &card_info);
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  safe_read_shared_location
+ *
+ *      Read a shared memory location repeatedly until we get two readings
+ *      the same.
+ *
+ *  Arguments:
+ *      card            Pointer to card context struct.
+ *      unifi_addr      UniFi shared-data-memory address to access.
+ *      pdata           Pointer to a byte variable for the value read.
+ *
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on failure
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult safe_read_shared_location(card_t *card, u32 address, u8 *pdata)
+{
+    CsrResult r;
+    u16 limit = 1000;
+    u8 b, b2;
+
+    *pdata = 0;
+
+    r = unifi_read_8_or_16(card, address, &b);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        return r;
+    }
+
+    while (limit--)
+    {
+        r = unifi_read_8_or_16(card, address, &b2);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            return r;
+        }
+
+        /* When we have a stable value, return it */
+        if (b == b2)
+        {
+            *pdata = b;
+            return CSR_RESULT_SUCCESS;
+        }
+
+        b = b2;
+    }
+
+    return CSR_RESULT_FAILURE;
+} /* safe_read_shared_location() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_do_loader_op
+ *
+ *      Send a loader / boot_loader command to the UniFi and wait for
+ *      it to complete.
+ *
+ *  Arguments:
+ *      card            Pointer to card context struct.
+ *      op_addr         The address of the loader operation control word.
+ *      opcode          The operation to perform.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS    on success
+ *      CSR_RESULT_FAILURE    SDIO error or SDIO/XAP timeout
+ * ---------------------------------------------------------------------------
+ */
+
+/*
+ * Ideally instead of sleeping, we want to busy wait.
+ * Currently there is no framework API to do this. When it becomes available,
+ * we can use it to busy wait using usecs
+ */
+#define OPERATION_TIMEOUT_LOOPS (100)  /* when OPERATION_TIMEOUT_DELAY==1, (500) otherwise */
+#define OPERATION_TIMEOUT_DELAY 1      /* msec, or 200usecs */
+
+CsrResult unifi_do_loader_op(card_t *card, u32 op_addr, u8 opcode)
+{
+    CsrResult r;
+    s16 op_retries;
+
+    unifi_trace(card->ospriv, UDBG4, "Loader cmd 0x%0x -> 0x%08x\n", opcode, op_addr);
+
+    /* Set the Operation command byte to the opcode */
+    r = unifi_write_8_or_16(card, op_addr, opcode);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to write loader copy command\n");
+        return r;
+    }
+
+    /* Wait for Operation command byte to be Idle */
+    /* Typically takes ~100us */
+    op_retries = 0;
+    r = CSR_RESULT_SUCCESS;
+    while (1)
+    {
+        u8 op;
+
+        /*
+         * Read the memory location until two successive reads give
+         * the same value.
+         * Then handle it.
+         */
+        r = safe_read_shared_location(card, op_addr, &op);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Failed to read loader status\n");
+            break;
+        }
+
+        if (op == UNIFI_LOADER_IDLE)
+        {
+            /* Success */
+            break;
+        }
+
+        if (op != opcode)
+        {
+            unifi_error(card->ospriv, "Error reported by loader: 0x%X\n", op);
+            r = CSR_RESULT_FAILURE;
+            break;
+        }
+
+        /* Allow 500us timeout */
+        if (++op_retries >= OPERATION_TIMEOUT_LOOPS)
+        {
+            unifi_error(card->ospriv, "Timeout waiting for loader to ack transfer\n");
+            /* Stop XAPs to aid post-mortem */
+            r = unifi_card_stop_processor(card, UNIFI_PROC_BOTH);
+            if (r != CSR_RESULT_SUCCESS)
+            {
+                unifi_error(card->ospriv, "Failed to stop UniFi processors\n");
+            }
+            else
+            {
+                r = CSR_RESULT_FAILURE;
+            }
+            break;
+        }
+        CsrThreadSleep(OPERATION_TIMEOUT_DELAY);
+    } /* Loop exits with r != CSR_RESULT_SUCCESS on error */
+
+    return r;
+}     /* unifi_do_loader_op() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  send_ptdl_to_unifi
+ *
+ *      Copy a patch block from userland to the UniFi.
+ *      This function reads data, 2K at a time, from userland and writes
+ *      it to the UniFi.
+ *
+ *  Arguments:
+ *      card            A pointer to the card structure
+ *      dlpriv          The os specific handle for the firmware file
+ *      ptdl            A pointer ot the PTDL block
+ *      handle          The buffer handle to use for the xfer
+ *      op_addr         The address of the loader operation control word
+ *
+ *  Returns:
+ *      Number of bytes sent (Positive) or negative value indicating
+ *      error code:
+ *      CSR_WIFI_HIP_RESULT_NO_MEMORY         memory allocation failed
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         error in XBV file
+ *      CSR_RESULT_FAILURE            SDIO error
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult send_ptdl_to_unifi(card_t *card, void *dlpriv,
+                                    const struct PTDL *ptdl, u32 handle,
+                                    u32 op_addr)
+{
+    u32 offset;
+    u8 *buf;
+    s32 data_len;
+    u32 write_len;
+    CsrResult r;
+    const u16 buf_size = 2 * 1024;
+
+    offset = ptdl->dl_offset;
+    data_len = ptdl->dl_size;
+
+    if (data_len > buf_size)
+    {
+        unifi_error(card->ospriv, "PTDL block is too large (%u)\n",
+                    ptdl->dl_size);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    buf = kmalloc(buf_size, GFP_KERNEL);
+    if (buf == NULL)
+    {
+        unifi_error(card->ospriv, "Failed to allocate transfer buffer for firmware download\n");
+        return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+    }
+
+    r = CSR_RESULT_SUCCESS;
+
+    if (unifi_fw_read(card->ospriv, dlpriv, offset, buf, data_len) != data_len)
+    {
+        unifi_error(card->ospriv, "Failed to read from file\n");
+    }
+    else
+    {
+        /* We can always round these if the host wants to */
+        if (card->sdio_io_block_pad)
+        {
+            write_len = (data_len + (card->sdio_io_block_size - 1)) &
+                        ~(card->sdio_io_block_size - 1);
+
+            /* Zero out the rest of the buffer (This isn't needed, but it
+             * makes debugging things later much easier). */
+            memset(buf + data_len, 0, write_len - data_len);
+        }
+        else
+        {
+            write_len = data_len;
+        }
+
+        r = unifi_bulk_rw_noretry(card, handle, buf, write_len, UNIFI_SDIO_WRITE);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "CMD53 failed writing %d bytes to handle %ld\n",
+                        data_len, handle);
+        }
+        else
+        {
+            /*
+             * Can change the order of things to overlap read from file
+             * with copy to unifi
+             */
+            r = unifi_do_loader_op(card, op_addr, UNIFI_BOOT_LOADER_PATCH);
+        }
+    }
+
+    kfree(buf);
+
+    if (r != CSR_RESULT_SUCCESS && r != CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        unifi_error(card->ospriv, "Failed to copy block of %u bytes to UniFi\n",
+                    ptdl->dl_size);
+    }
+
+    return r;
+} /* send_ptdl_to_unifi() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  do_patch_download
+ *
+ *      This function downloads a set of patches to UniFi and then
+ *      causes it to restart.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct.
+ *      dlpriv          A context pointer from the calling function to be
+ *                      used when reading the XBV file.  This can be NULL
+ *                      in which case not patches are applied.
+ *      pfwinfo         Pointer to a fwinfo struct describing the f/w
+ *                      XBV file.
+ *      boot_ctrl_addr  The address of the boot loader control structure.
+ *
+ *  Returns:
+ *      0 on success, or an error code
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE for a bad laoader version number
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult do_patch_download(card_t *card, void *dlpriv, xbv1_t *pfwinfo, u32 boot_ctrl_addr)
+{
+    CsrResult r;
+    s32 i;
+    u16 loader_version;
+    u16 handle;
+    u32 total_bytes;
+
+    /*
+     * Read info from the SDIO Loader Control Data Structure
+     */
+    /* Check the loader version */
+    r = unifi_card_read16(card, boot_ctrl_addr, &loader_version);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Patch download: Failed to read loader version\n");
+        return r;
+    }
+    unifi_trace(card->ospriv, UDBG2, "Patch download: boot loader version 0x%04X\n", loader_version);
+    switch (loader_version)
+    {
+        case 0x0000:
+            break;
+
+        default:
+            unifi_error(card->ospriv, "Patch loader version (0x%04X) is not supported by this driver\n",
+                        loader_version);
+            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    /* Retrieve the handle to use with CMD53 */
+    r = unifi_card_read16(card, boot_ctrl_addr + 4, &handle);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Patch download: Failed to read loader handle\n");
+        return r;
+    }
+
+    /* Set the mask of LEDs to flash */
+    if (card->loader_led_mask)
+    {
+        r = unifi_card_write16(card, boot_ctrl_addr + 2,
+                               (u16)card->loader_led_mask);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Patch download: Failed to write LED mask\n");
+            return r;
+        }
+    }
+
+    total_bytes = 0;
+
+    /* Copy download data to UniFi memory */
+    for (i = 0; i < pfwinfo->num_ptdl; i++)
+    {
+        unifi_trace(card->ospriv, UDBG3, "Patch download: %d Downloading for %d from offset %d\n",
+                    i,
+                    pfwinfo->ptdl[i].dl_size,
+                    pfwinfo->ptdl[i].dl_offset);
+
+        r = send_ptdl_to_unifi(card, dlpriv, &pfwinfo->ptdl[i],
+                               handle, boot_ctrl_addr + 6);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            return r;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Patch failed after %u bytes\n",
+                        total_bytes);
+            return r;
+        }
+        total_bytes += pfwinfo->ptdl[i].dl_size;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* do_patch_download() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_dump.c b/drivers/staging/csr/csr_wifi_hip_dump.c
new file mode 100644
index 00000000000..d67b460e7a8
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_dump.c
@@ -0,0 +1,865 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ * FILE: csr_wifi_hip_dump.c
+ *
+ * PURPOSE:
+ *      Routines for retrieving and buffering core status from the UniFi
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/slab.h>
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_unifiversion.h"
+#include "csr_wifi_hip_card.h"
+
+/* Locations to capture in dump (XAP words) */
+#define HIP_CDUMP_FIRST_CPUREG      (0xFFE0) /* First CPU register */
+#define HIP_CDUMP_FIRST_LO          (0)      /* Start of low address range */
+#define HIP_CDUMP_FIRST_HI_MAC      (0x3C00) /* Start of MAC high area */
+#define HIP_CDUMP_FIRST_HI_PHY      (0x1C00) /* Start of PHY high area */
+#define HIP_CDUMP_FIRST_SH          (0)      /* Start of shared memory area */
+
+#define HIP_CDUMP_NCPUREGS    (10)           /* No. of 16-bit XAP registers */
+#define HIP_CDUMP_NWORDS_LO   (0x0100)       /* Low area size in 16-bit words */
+#define HIP_CDUMP_NWORDS_HI   (0x0400)       /* High area size in 16-bit words */
+#define HIP_CDUMP_NWORDS_SH   (0x0500)       /* Shared memory area size, 16-bit words */
+
+#define HIP_CDUMP_NUM_ZONES 7                /* Number of UniFi memory areas to capture */
+
+/* Mini-coredump state */
+typedef struct coredump_buf
+{
+    u16  count;                       /* serial number of dump */
+    CsrTime    timestamp;                   /* host's system time at capture */
+    s16   requestor;                   /* request: 0=auto dump, 1=manual */
+    u16  chip_ver;
+    u32  fw_ver;
+    u16 *zone[HIP_CDUMP_NUM_ZONES];
+
+    struct coredump_buf *next;              /* circular list */
+    struct coredump_buf *prev;              /* circular list */
+} coredump_buffer;
+
+/* Structure used to describe a zone of chip memory captured by mini-coredump */
+struct coredump_zone
+{
+    unifi_coredump_space_t           space;  /* XAP memory space this zone covers */
+    enum unifi_dbg_processors_select cpu;    /* XAP CPU core selector */
+    u32                        gp;     /* Generic Pointer to memory zone on XAP */
+    u16                        offset; /* 16-bit XAP word offset of zone in memory space */
+    u16                        length; /* Length of zone in XAP words */
+};
+
+static CsrResult unifi_coredump_from_sdio(card_t *card, coredump_buffer *dump_buf);
+static CsrResult unifi_coredump_read_zones(card_t *card, coredump_buffer *dump_buf);
+static CsrResult unifi_coredump_read_zone(card_t *card, u16 *zone,
+                                          const struct coredump_zone *def);
+static s32 get_value_from_coredump(const coredump_buffer *dump,
+                                        const unifi_coredump_space_t space, const u16 offset);
+
+/* Table of chip memory zones we capture on mini-coredump */
+static const struct coredump_zone zonedef_table[HIP_CDUMP_NUM_ZONES] = {
+    { UNIFI_COREDUMP_MAC_REG,  UNIFI_PROC_MAC, UNIFI_MAKE_GP(REGISTERS, HIP_CDUMP_FIRST_CPUREG * 2), HIP_CDUMP_FIRST_CPUREG, HIP_CDUMP_NCPUREGS },
+    { UNIFI_COREDUMP_PHY_REG,  UNIFI_PROC_PHY, UNIFI_MAKE_GP(REGISTERS, HIP_CDUMP_FIRST_CPUREG * 2), HIP_CDUMP_FIRST_CPUREG, HIP_CDUMP_NCPUREGS },
+    { UNIFI_COREDUMP_SH_DMEM,  UNIFI_PROC_INVALID, UNIFI_MAKE_GP(SH_DMEM, HIP_CDUMP_FIRST_SH * 2),   HIP_CDUMP_FIRST_SH,     HIP_CDUMP_NWORDS_SH },
+    { UNIFI_COREDUMP_MAC_DMEM, UNIFI_PROC_MAC, UNIFI_MAKE_GP(MAC_DMEM, HIP_CDUMP_FIRST_LO * 2),      HIP_CDUMP_FIRST_LO,     HIP_CDUMP_NWORDS_LO },
+    { UNIFI_COREDUMP_MAC_DMEM, UNIFI_PROC_MAC, UNIFI_MAKE_GP(MAC_DMEM, HIP_CDUMP_FIRST_HI_MAC * 2),  HIP_CDUMP_FIRST_HI_MAC, HIP_CDUMP_NWORDS_HI },
+    { UNIFI_COREDUMP_PHY_DMEM, UNIFI_PROC_PHY, UNIFI_MAKE_GP(PHY_DMEM, HIP_CDUMP_FIRST_LO * 2),      HIP_CDUMP_FIRST_LO,     HIP_CDUMP_NWORDS_LO },
+    { UNIFI_COREDUMP_PHY_DMEM, UNIFI_PROC_PHY, UNIFI_MAKE_GP(PHY_DMEM, HIP_CDUMP_FIRST_HI_PHY * 2),  HIP_CDUMP_FIRST_HI_PHY, HIP_CDUMP_NWORDS_HI },
+};
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_request_at_next_reset
+ *
+ *      Request that a mini-coredump is performed when the driver has
+ *      completed resetting the UniFi device.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      enable          If non-zero, sets the request.
+ *                      If zero, cancels any pending request.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS or CSR HIP error code
+ *
+ *  Notes:
+ *      This function is typically called once the driver has detected that
+ *      the UniFi device has become unresponsive due to crash, or internal
+ *      watchdog reset. The driver must reset it to regain communication and,
+ *      immediately after that, the mini-coredump can be captured.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_coredump_request_at_next_reset(card_t *card, s8 enable)
+{
+    CsrResult r;
+
+    func_enter();
+
+    if (enable)
+    {
+        unifi_trace(card->ospriv, UDBG2, "Mini-coredump requested after reset\n");
+    }
+
+    if (card == NULL)
+    {
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+    else
+    {
+        card->request_coredump_on_reset = enable?1 : 0;
+        r = CSR_RESULT_SUCCESS;
+    }
+
+    func_exit_r(r);
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_handle_request
+ *
+ *      Performs a coredump now, if one was requested, and clears the request.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS or CSR HIP error code
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_coredump_handle_request(card_t *card)
+{
+    CsrResult r = CSR_RESULT_SUCCESS;
+
+    func_enter();
+
+    if (card == NULL)
+    {
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+    else
+    {
+        if (card->request_coredump_on_reset == 1)
+        {
+            card->request_coredump_on_reset = 0;
+            r = unifi_coredump_capture(card, NULL);
+        }
+    }
+
+    func_exit_r(r);
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_capture
+ *
+ *      Capture the current status of the UniFi device.
+ *      Various registers are buffered for future offline inspection.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      req             Pointer to request struct, or NULL:
+ *                          A coredump requested manually by the user app
+ *                          will have a request struct pointer, an automatic
+ *                          coredump will have a NULL pointer.
+ *  Returns:
+ *      CSR_RESULT_SUCCESS  on success,
+ *      CSR_RESULT_FAILURE  SDIO error
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE  Initialisation not complete
+ *
+ *  Notes:
+ *      The result is a filled entry in the circular buffer of core dumps,
+ *      values from which can be extracted to userland via an ioctl.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_coredump_capture(card_t *card, struct unifi_coredump_req *req)
+{
+    CsrResult r = CSR_RESULT_SUCCESS;
+    static u16 dump_seq_no = 1;
+    CsrTime time_of_capture;
+
+    func_enter();
+
+    if (card->dump_next_write == NULL)
+    {
+        r = CSR_RESULT_SUCCESS;
+        goto done;
+    }
+
+    /* Reject forced capture before initialisation has happened */
+    if (card->helper == NULL)
+    {
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+        goto done;
+    }
+
+
+    /*
+     * Force a mini-coredump capture right now
+     */
+    time_of_capture = CsrTimeGet(NULL);
+    unifi_info(card->ospriv, "Mini-coredump capture at t=%u\n", time_of_capture);
+
+    /* Wake up the processors so we can talk to them */
+    r = unifi_set_host_state(card, UNIFI_HOST_STATE_AWAKE);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to wake UniFi\n");
+        goto done;
+    }
+    CsrThreadSleep(20);
+
+    /* Stop both XAPs */
+    unifi_trace(card->ospriv, UDBG4, "Stopping XAPs for coredump capture\n");
+    r = unifi_card_stop_processor(card, UNIFI_PROC_BOTH);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to stop UniFi XAPs\n");
+        goto done;
+    }
+
+    /* Dump core into the next available slot in the circular list */
+    r = unifi_coredump_from_sdio(card, card->dump_next_write);
+    if (r == CSR_RESULT_SUCCESS)
+    {
+        /* Record whether the dump was manual or automatic */
+        card->dump_next_write->requestor = (req?1 : 0);
+        card->dump_next_write->timestamp = time_of_capture;
+        /* Advance to the next buffer */
+        card->dump_next_write->count = dump_seq_no++;
+        card->dump_cur_read = card->dump_next_write;
+        card->dump_next_write = card->dump_next_write->next;
+
+        /* Sequence no. of zero indicates slot not in use, so handle wrap */
+        if (dump_seq_no == 0)
+        {
+            dump_seq_no = 1;
+        }
+
+        unifi_trace(card->ospriv, UDBG3,
+                    "Coredump (%p), SeqNo=%d, cur_read=%p, next_write=%p\n",
+                    req,
+                    card->dump_cur_read->count,
+                    card->dump_cur_read, card->dump_next_write);
+    }
+
+    /* Start both XAPs */
+    unifi_trace(card->ospriv, UDBG4, "Restart XAPs after coredump\n");
+    r = card_start_processor(card, UNIFI_PROC_BOTH);
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Failed to start UniFi XAPs\n");
+        goto done;
+    }
+
+done:
+    func_exit_r(r);
+    return r;
+} /* unifi_coredump_capture() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  get_value_from_coredump
+ *
+ *
+ *
+ *  Arguments:
+ *      dump                Pointer to buffered coredump data
+ *      offset_in_space     XAP memory space to retrieve from the buffer (there
+ *                          may be more than one zone covering the same memory
+ *                          space, but starting from different offsets).
+ *      offset              Offset within the XAP memory space to be retrieved
+ *
+ *  Returns:
+ *      >=0                  Register value on success
+ *      <0                   Register out of range of any captured zones
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+static s32 get_value_from_coredump(const coredump_buffer       *coreDump,
+                                        const unifi_coredump_space_t space,
+                                        const u16              offset_in_space)
+{
+    s32 r = -1;
+    u16 offset_in_zone;
+    u32 zone_end_offset;
+    s32 i;
+    const struct coredump_zone *def = &zonedef_table[0];
+
+    /* Search zone def table for a match with the requested memory space */
+    for (i = 0; i < HIP_CDUMP_NUM_ZONES; i++, def++)
+    {
+        if (space == def->space)
+        {
+            zone_end_offset = def->offset + def->length;
+
+            /* Is the space offset contained in this zone? */
+            if (offset_in_space < zone_end_offset &&
+                offset_in_space >= def->offset)
+            {
+                /* Calculate the offset of data within the zone buffer */
+                offset_in_zone = offset_in_space - def->offset;
+                r = (s32) * (coreDump->zone[i] + offset_in_zone);
+
+                unifi_trace(NULL, UDBG6,
+                            "sp %d, offs 0x%04x = 0x%04x (in z%d 0x%04x->0x%04x)\n",
+                            space, offset_in_space, r,
+                            i, def->offset, zone_end_offset - 1);
+                break;
+            }
+        }
+    }
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_get_value
+ *
+ *      Retrieve the value of a register buffered from a previous core dump,
+ *      so that it may be reported back to application code.
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      req_reg         Pointer to request parameter partially filled. This
+ *                      function puts in the values retrieved from the dump.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, or:
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         Null parameter error
+ *      CSR_WIFI_HIP_RESULT_RANGE                 Register out of range
+ *      CSR_WIFI_HIP_RESULT_NOT_FOUND             Dump index not (yet) captured
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_coredump_get_value(card_t *card, struct unifi_coredump_req *req)
+{
+    CsrResult r;
+    s32 i = 0;
+    coredump_buffer *find_dump = NULL;
+
+    func_enter();
+
+    if (req == NULL || card == NULL)
+    {
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+        goto done;
+    }
+    req->value = -1;
+    if (card->dump_buf == NULL)
+    {
+        unifi_trace(card->ospriv, UDBG2, "No coredump buffers\n");
+        r = CSR_WIFI_HIP_RESULT_NOT_FOUND;     /* Coredumping disabled */
+        goto done;
+    }
+    if (card->dump_cur_read == NULL)
+    {
+        unifi_trace(card->ospriv, UDBG4, "No coredumps captured\n");
+        r = CSR_WIFI_HIP_RESULT_NOT_FOUND;     /* No coredump yet captured */
+        goto done;
+    }
+
+    /* Find the requested dump buffer */
+    switch (req->index)
+    {
+        case 0:     /* Newest */
+            find_dump = card->dump_cur_read;
+            break;
+        case -1:    /* Oldest: The next used slot forward */
+            for (find_dump = card->dump_cur_read->next;
+                 (find_dump->count == 0) && (find_dump != card->dump_cur_read);
+                 find_dump = card->dump_cur_read->next)
+            {
+            }
+            break;
+        default:    /* Number of steps back from current read position */
+            for (i = 0, find_dump = card->dump_cur_read;
+                 i < req->index;
+                 i++, find_dump = find_dump->prev)
+            {
+                /* Walk the list for the index'th entry, but
+                 * stop when about to wrap. */
+                unifi_trace(card->ospriv, UDBG6,
+                            "%d: %d, @%p, p=%p, n=%p, cr=%p, h=%p\n",
+                            i, find_dump->count, find_dump, find_dump->prev,
+                            find_dump->next, card->dump_cur_read, card->dump_buf);
+                if (find_dump->prev == card->dump_cur_read)
+                {
+                    /* Wrapped but still not found, index out of range */
+                    if (i != req->index)
+                    {
+                        unifi_trace(card->ospriv, UDBG6,
+                                    "Dump index %d not found %d\n", req->index, i);
+                        r = CSR_WIFI_HIP_RESULT_NOT_FOUND;
+                        goto done;
+                    }
+                    break;
+                }
+            }
+            break;
+    }
+
+    /* Check if the slot is actually filled with a core dump */
+    if (find_dump->count == 0)
+    {
+        unifi_trace(card->ospriv, UDBG4, "Not captured %d\n", req->index);
+        r = CSR_WIFI_HIP_RESULT_NOT_FOUND;
+        goto done;
+    }
+
+    unifi_trace(card->ospriv, UDBG6, "Req index %d, found seq %d at step %d\n",
+                req->index, find_dump->count, i);
+
+    /* Find the appropriate entry in the buffer */
+    req->value = get_value_from_coredump(find_dump, req->space, (u16)req->offset);
+    if (req->value < 0)
+    {
+        r = CSR_WIFI_HIP_RESULT_RANGE;     /* Un-captured register */
+        unifi_trace(card->ospriv, UDBG4,
+                    "Can't read space %d, reg 0x%x from coredump buffer %d\n",
+                    req->space, req->offset, req->index);
+    }
+    else
+    {
+        r = CSR_RESULT_SUCCESS;
+    }
+
+    /* Update the private request structure with the found values */
+    req->chip_ver = find_dump->chip_ver;
+    req->fw_ver = find_dump->fw_ver;
+    req->timestamp = find_dump->timestamp;
+    req->requestor = find_dump->requestor;
+    req->serial = find_dump->count;
+
+done:
+    func_exit_r(r);
+    return r;
+} /* unifi_coredump_get_value() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_read_zone
+ *
+ *      Captures a UniFi memory zone into a buffer on the host
+ *
+ *  Arguments:
+ *      card          Pointer to card struct
+ *      zonebuf       Pointer to on-host buffer to dump the memory zone into
+ *      def           Pointer to description of the memory zone to read from UniFi.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS                   on success, or:
+ *      CSR_RESULT_FAILURE                   SDIO error
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         Parameter error
+ *
+ *  Notes:
+ *      It is assumed that the caller has already stopped the XAPs
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult unifi_coredump_read_zone(card_t *card, u16 *zonebuf, const struct coredump_zone *def)
+{
+    CsrResult r;
+
+    func_enter();
+
+    if (zonebuf == NULL || def == NULL)
+    {
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+        goto done;
+    }
+
+    /* Select XAP CPU if necessary */
+    if (def->cpu != UNIFI_PROC_INVALID)
+    {
+        if (def->cpu != UNIFI_PROC_MAC && def->cpu != UNIFI_PROC_PHY)
+        {
+            r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            goto done;
+        }
+        r = unifi_set_proc_select(card, def->cpu);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            goto done;
+        }
+    }
+
+    unifi_trace(card->ospriv, UDBG4,
+                "Dump sp %d, offs 0x%04x, 0x%04x words @GP=%08x CPU %d\n",
+                def->space, def->offset, def->length, def->gp, def->cpu);
+
+    /* Read on-chip RAM (byte-wise) */
+    r = unifi_card_readn(card, def->gp, zonebuf, (u16)(def->length * 2));
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        goto done;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Can't read UniFi shared data area\n");
+        goto done;
+    }
+
+done:
+    func_exit_r(r);
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_read_zones
+ *
+ *      Walks through the table of on-chip memory zones defined in zonedef_table,
+ *      and reads each of them from the UniFi chip
+ *
+ *  Arguments:
+ *      card          Pointer to card struct
+ *      dump_buf      Buffer into which register values will be dumped
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS                   on success, or:
+ *      CSR_RESULT_FAILURE                   SDIO error
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         Parameter error
+ *
+ *  Notes:
+ *      It is assumed that the caller has already stopped the XAPs
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult unifi_coredump_read_zones(card_t *card, coredump_buffer *dump_buf)
+{
+    CsrResult r = CSR_RESULT_SUCCESS;
+    s32 i;
+
+    func_enter();
+
+    /* Walk the table of coredump zone definitions and read them from the chip */
+    for (i = 0;
+         (i < HIP_CDUMP_NUM_ZONES) && (r == 0);
+         i++)
+    {
+        r = unifi_coredump_read_zone(card, dump_buf->zone[i], &zonedef_table[i]);
+    }
+
+    func_exit_r(r);
+    return r;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_from_sdio
+ *
+ *      Capture the status of the UniFi processors, over SDIO
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *      reg_buffer      Buffer into which register values will be dumped
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS                   on success, or:
+ *      CSR_RESULT_FAILURE                   SDIO error
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE         Parameter error
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult unifi_coredump_from_sdio(card_t *card, coredump_buffer *dump_buf)
+{
+    u16 val;
+    CsrResult r;
+    u32 sdio_addr;
+
+    func_enter();
+
+    if (dump_buf == NULL)
+    {
+        r = CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+        goto done;
+    }
+
+
+    /* Chip and firmware version */
+    unifi_trace(card->ospriv, UDBG4, "Get chip version\n");
+    sdio_addr = 2 * ChipHelper_GBL_CHIP_VERSION(card->helper);
+    if (sdio_addr != 0)
+    {
+        r = unifi_read_direct16(card, sdio_addr, &val);
+        if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+        {
+            goto done;
+        }
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "Can't read GBL_CHIP_VERSION\n");
+            goto done;
+        }
+    }
+    dump_buf->chip_ver = val;
+    dump_buf->fw_ver = card->build_id;
+
+    unifi_trace(card->ospriv, UDBG4, "chip_ver 0x%04x, fw_ver %u\n",
+                dump_buf->chip_ver, dump_buf->fw_ver);
+
+    /* Capture the memory zones required from UniFi */
+    r = unifi_coredump_read_zones(card, dump_buf);
+    if (r == CSR_WIFI_HIP_RESULT_NO_DEVICE)
+    {
+        goto done;
+    }
+    if (r != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(card->ospriv, "Can't read UniFi memory areas\n");
+        goto done;
+    }
+
+done:
+    func_exit_r(r);
+    return r;
+} /* unifi_coredump_from_sdio() */
+
+
+#ifndef UNIFI_DISABLE_COREDUMP
+/*
+ * ---------------------------------------------------------------------------
+ *  new_coredump_node
+ *
+ *      Allocates a coredump linked-list node, and links it to the previous.
+ *
+ *  Arguments:
+ *      ospriv          OS context
+ *      prevnode        Previous node to link into
+ *
+ *  Returns:
+ *      Pointer to valid coredump_buffer on success
+ *      NULL on memory allocation failure
+ *
+ *  Notes:
+ *      Allocates "all or nothing"
+ * ---------------------------------------------------------------------------
+ */
+static
+coredump_buffer* new_coredump_node(void *ospriv, coredump_buffer *prevnode)
+{
+    coredump_buffer *newnode = NULL;
+    u16 *newzone = NULL;
+    s32 i;
+    u32 zone_size;
+
+    /* Allocate node header */
+    newnode = kzalloc(sizeof(coredump_buffer), GFP_KERNEL);
+    if (newnode == NULL)
+    {
+        return NULL;
+    }
+
+    /* Allocate chip memory zone capture buffers */
+    for (i = 0; i < HIP_CDUMP_NUM_ZONES; i++)
+    {
+        zone_size = sizeof(u16) * zonedef_table[i].length;
+        newzone = kzalloc(zone_size, GFP_KERNEL);
+        newnode->zone[i] = newzone;
+        if (newzone == NULL)
+        {
+            unifi_error(ospriv, "Out of memory on coredump zone %d (%d words)\n",
+                        i, zonedef_table[i].length);
+            break;
+        }
+    }
+
+    /* Clean up if any zone alloc failed */
+    if (newzone == NULL)
+    {
+        for (i = 0; newnode->zone[i] != NULL; i++)
+        {
+            kfree(newnode->zone[i]);
+            newnode->zone[i] = NULL;
+        }
+    }
+
+    /* Link to previous node */
+    newnode->prev = prevnode;
+    if (prevnode)
+    {
+        prevnode->next = newnode;
+    }
+    newnode->next = NULL;
+
+    return newnode;
+}
+
+
+#endif /* UNIFI_DISABLE_COREDUMP */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_init
+ *
+ *      Allocates buffers for the automatic SDIO core dump
+ *
+ *  Arguments:
+ *      card                Pointer to card struct
+ *      num_dump_buffers    Number of buffers to reserve for coredumps
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS               on success, or:
+ *      CSR_WIFI_HIP_RESULT_NO_MEMORY         memory allocation failed
+ *
+ *  Notes:
+ *      Allocates space in advance, to be used for the last n coredump buffers
+ *      the intention being that the size is sufficient for at least one dump,
+ *      probably several.
+ *      It's probably advisable to have at least 2 coredump buffers to allow
+ *      one to be enquired with the unifi_coredump tool, while leaving another
+ *      free for capturing.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_coredump_init(card_t *card, u16 num_dump_buffers)
+{
+#ifndef UNIFI_DISABLE_COREDUMP
+    void *ospriv = card->ospriv;
+    coredump_buffer *prev = NULL;
+    coredump_buffer *newnode = NULL;
+    u32 i = 0;
+#endif
+
+    func_enter();
+
+    card->request_coredump_on_reset = 0;
+    card->dump_next_write = NULL;
+    card->dump_cur_read = NULL;
+    card->dump_buf = NULL;
+
+#ifndef UNIFI_DISABLE_COREDUMP
+    unifi_trace(ospriv, UDBG1,
+                "Allocate buffers for %d core dumps\n", num_dump_buffers);
+    if (num_dump_buffers == 0)
+    {
+        goto done;
+    }
+
+    /* Root node */
+    card->dump_buf = new_coredump_node(ospriv, NULL);
+    if (card->dump_buf == NULL)
+    {
+        goto fail;
+    }
+    prev = card->dump_buf;
+    newnode = card->dump_buf;
+
+    /* Add each subsequent node at tail */
+    for (i = 1; i < num_dump_buffers; i++)
+    {
+        newnode = new_coredump_node(ospriv, prev);
+        if (newnode == NULL)
+        {
+            goto fail;
+        }
+        prev = newnode;
+    }
+
+    /* Link the first and last nodes to make the list circular */
+    card->dump_buf->prev = newnode;
+    newnode->next = card->dump_buf;
+
+    /* Set initial r/w access pointers */
+    card->dump_next_write = card->dump_buf;
+    card->dump_cur_read = NULL;
+
+    unifi_trace(ospriv, UDBG2, "Core dump configured (%d dumps max)\n", i);
+
+done:
+#endif
+    func_exit();
+    return CSR_RESULT_SUCCESS;
+
+#ifndef UNIFI_DISABLE_COREDUMP
+fail:
+    /* Unwind what we allocated so far */
+    unifi_error(ospriv, "Out of memory allocating core dump node %d\n", i);
+    unifi_coredump_free(card);
+    func_exit();
+    return CSR_WIFI_HIP_RESULT_NO_MEMORY;
+#endif
+} /* unifi_coreump_init() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_coredump_free
+ *
+ *      Free all memory dynamically allocated for core dump
+ *
+ *  Arguments:
+ *      card            Pointer to card struct
+ *
+ *  Returns:
+ *      None
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+void unifi_coredump_free(card_t *card)
+{
+    void *ospriv = card->ospriv;
+    coredump_buffer *node, *del_node;
+    s16 i = 0;
+    s16 j;
+
+    func_enter();
+    unifi_trace(ospriv, UDBG2, "Core dump de-configured\n");
+
+    if (card->dump_buf == NULL)
+    {
+        return;
+    }
+
+    node = card->dump_buf;
+    do
+    {
+        /* Free payload zones */
+        for (j = 0; j < HIP_CDUMP_NUM_ZONES; j++)
+        {
+            kfree(node->zone[j]);
+            node->zone[j] = NULL;
+        }
+
+        /* Detach */
+        del_node = node;
+        node = node->next;
+
+        /* Free header */
+        kfree(del_node);
+        i++;
+    } while ((node != NULL) && (node != card->dump_buf));
+
+    unifi_trace(ospriv, UDBG3, "Freed %d coredump buffers\n", i);
+
+    card->dump_buf = NULL;
+    card->dump_next_write = NULL;
+    card->dump_cur_read = NULL;
+
+    func_exit();
+} /* unifi_coredump_free() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_packing.c b/drivers/staging/csr/csr_wifi_hip_packing.c
new file mode 100644
index 00000000000..0768aefc6d1
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_packing.c
@@ -0,0 +1,4804 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#include "csr_wifi_hip_signals.h"
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  get_packed_struct_size
+ *
+ *      Examine a buffer containing a UniFi signal in wire-format.
+ *      The first two bytes contain the signal ID, decode the signal ID and
+ *      return the size, in  bytes, of the signal, not including any bulk
+ *      data.
+ *
+ *      WARNING: This function is auto-generated, DO NOT EDIT!
+ *
+ *  Arguments:
+ *      buf     Pointer to buffer to decode.
+ *
+ *  Returns:
+ *      0 if the signal ID is not recognised (i.e. zero length),
+ *      otherwise the number of bytes occupied by the signal in the buffer.
+ *      This is useful for stepping past the signal to the object in the buffer.
+ * ---------------------------------------------------------------------------
+ */
+s32 get_packed_struct_size(const u8 *buf)
+{
+    s32 size = 0;
+    u16 sig_id;
+
+    sig_id = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(buf);
+
+    size += SIZEOF_UINT16;
+    size += SIZEOF_UINT16;
+    size += SIZEOF_UINT16;
+    switch (sig_id)
+    {
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+        case CSR_MA_PACKET_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT64;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+        case CSR_MLME_SET_TIM_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_TRIGGERED_GET_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+        case CSR_DEBUG_GENERIC_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CANCEL_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+        case CSR_MA_PACKET_CANCEL_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_VIF_AVAILABILITY_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLACKOUT_ENDED_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+        case CSR_MA_VIF_AVAILABILITY_RESPONSE_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT32;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 32 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT32;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLOCKACK_ERROR_INDICATION_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += 48 / 8;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT32;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_REQUEST_ID:
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            size += SIZEOF_UINT16;
+            break;
+#endif
+        default:
+            size = 0;
+    }
+    return size;
+} /* get_packed_struct_size() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  read_unpack_signal
+ *
+ *      Unpack a wire-format signal into a host-native structure.
+ *      This function handles any necessary conversions for endianness and
+ *      places no restrictions on packing or alignment for the structure
+ *      definition.
+ *
+ *      WARNING: This function is auto-generated, DO NOT EDIT!
+ *
+ *  Arguments:
+ *      ptr             Signal buffer to unpack.
+ *      sig             Pointer to destination structure to populate.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success,
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE if the ID of signal was not recognised.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult read_unpack_signal(const u8 *ptr, CSR_SIGNAL *sig)
+{
+    s32 index = 0;
+
+    sig->SignalPrimitiveHeader.SignalId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+
+    sig->SignalPrimitiveHeader.ReceiverProcessId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+
+    sig->SignalPrimitiveHeader.SenderProcessId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+    index += SIZEOF_UINT16;
+
+    switch (sig->SignalPrimitiveHeader.SignalId)
+    {
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+            sig->u.MlmeSetPacketFilterConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+            sig->u.MlmeSetkeysConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+            sig->u.MlmeConfigQueueConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+            sig->u.MlmeAddAutonomousScanConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanConfirm.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+            sig->u.MlmeAddBlackoutConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutConfirm.BlackoutId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_REQUEST_ID:
+            sig->u.MlmeDelBlackoutRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutRequest.BlackoutId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+            sig->u.MlmeGetKeySequenceConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[0] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[1] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[2] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[3] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[4] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[5] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[6] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[7] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_CONFIRM_ID:
+            sig->u.MlmeSmStartConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+            sig->u.MlmeStopAggregationConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeStopAggregationConfirm.PeerQstaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeStopAggregationConfirm.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationConfirm.Direction = (CSR_DIRECTION) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_REQUEST_ID:
+            sig->u.MlmeDelTspecRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecRequest.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecRequest.Direction = (CSR_DIRECTION) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            sig->u.DebugWord16Indication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[0] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[1] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[2] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[3] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[4] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[5] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[6] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[7] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[8] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[9] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[10] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[11] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[12] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[13] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[14] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugWord16Indication.DebugWords[15] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            sig->u.DebugGenericConfirm.DebugVariable.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugVariable.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[0] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[1] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[2] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[3] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[4] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[5] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[6] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericConfirm.DebugWords[7] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_MA_PACKET_INDICATION_ID:
+            sig->u.MaPacketIndication.Data.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.Data.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MaPacketIndication.LocalTime.x, &ptr[index], 64 / 8);
+            index += 64 / 8;
+            sig->u.MaPacketIndication.Ifindex = (CSR_IFINTERFACE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.Channel = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.ReceptionStatus = (CSR_RECEPTION_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.Rssi = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.Snr = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketIndication.ReceivedRate = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_MLME_SET_TIM_REQUEST_ID:
+            sig->u.MlmeSetTimRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimRequest.AssociationId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimRequest.TimValue = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            sig->u.MlmeConnectedIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectedIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectedIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectedIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectedIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectedIndication.ConnectionStatus = (CSR_CONNECTION_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeConnectedIndication.PeerMacAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID:
+            sig->u.MlmeDelRxTriggerRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerRequest.TriggerId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_TRIGGERED_GET_INDICATION_ID:
+            sig->u.MlmeTriggeredGetIndication.MibAttributeValue.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.MibAttributeValue.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.Status = (CSR_MIB_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.ErrorIndex = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeTriggeredGetIndication.TriggeredId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_REQUEST_ID:
+            sig->u.MlmeScanRequest.ChannelList.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.ChannelList.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.InformationElements.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.InformationElements.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.Ifindex = (CSR_IFINTERFACE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.ScanType = (CSR_SCAN_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.ProbeDelay = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeScanRequest.MinChannelTime = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanRequest.MaxChannelTime = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+            sig->u.MlmeDeletekeysConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_REQUEST_ID:
+            sig->u.MlmeGetNextRequest.MibAttribute.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextRequest.MibAttribute.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+            sig->u.MlmeSetChannelConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_REQUEST_ID:
+            sig->u.MlmeStartAggregationRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeStartAggregationRequest.PeerQstaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeStartAggregationRequest.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.Direction = (CSR_DIRECTION) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.StartingSequenceNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.BufferSize = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationRequest.BlockAckTimeout = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_REQUEST_ID:
+            sig->u.MlmeHlSyncRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeHlSyncRequest.GroupAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+        case CSR_DEBUG_GENERIC_REQUEST_ID:
+            sig->u.DebugGenericRequest.DebugVariable.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugVariable.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[0] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[1] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[2] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[3] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[4] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[5] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[6] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericRequest.DebugWords[7] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+            sig->u.MlmeLeaveConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID:
+            sig->u.MlmeDelTriggeredGetRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetRequest.TriggeredId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID:
+            sig->u.MlmeAddMulticastAddressRequest.Data.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressRequest.Data.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressRequest.NumberOfMulticastGroupAddresses = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_REQUEST_ID:
+            sig->u.MlmeResetRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeResetRequest.StaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeResetRequest.SetDefaultMib = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CANCEL_REQUEST_ID:
+            sig->u.MlmeScanCancelRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanCancelRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanCancelRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanCancelRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanCancelRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+            sig->u.MlmeAddTriggeredGetConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetConfirm.TriggeredId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_REQUEST_ID:
+            sig->u.MlmeSetPacketFilterRequest.InformationElements.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterRequest.InformationElements.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterRequest.PacketFilterMode = (CSR_PACKET_FILTER_MODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetPacketFilterRequest.ArpFilterAddress = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+            sig->u.MlmeDelRxTriggerConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerConfirm.TriggerId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelRxTriggerConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_REQUEST_ID:
+            sig->u.MlmeConnectStatusRequest.InformationElements.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusRequest.InformationElements.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusRequest.ConnectionStatus = (CSR_CONNECTION_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeConnectStatusRequest.StaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeConnectStatusRequest.AssociationId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusRequest.AssociationCapabilityInformation = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_REQUEST_ID:
+            sig->u.MlmeLeaveRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeLeaveRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_REQUEST_ID:
+            sig->u.MlmeConfigQueueRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.QueueIndex = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.Aifs = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.Cwmin = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.Cwmax = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConfigQueueRequest.TxopLimit = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+            sig->u.MlmeDelTspecConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecConfirm.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTspecConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            sig->u.MlmeSetTimConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetTimConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_INDICATION_ID:
+            sig->u.MlmeMeasureIndication.MeasurementReportSet.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureIndication.MeasurementReportSet.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureIndication.DialogToken = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+            sig->u.MlmeDelBlackoutConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutConfirm.BlackoutId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelBlackoutConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+            sig->u.MlmeDelTriggeredGetConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelTriggeredGetConfirm.TriggeredId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            sig->u.DebugGenericIndication.DebugVariable.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugVariable.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[0] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[1] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[2] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[3] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[4] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[5] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[6] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugGenericIndication.DebugWords[7] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_MA_PACKET_CANCEL_REQUEST_ID:
+            sig->u.MaPacketCancelRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketCancelRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketCancelRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketCancelRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketCancelRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketCancelRequest.HostTag = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+            sig->u.MlmeModifyBssParameterConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+            sig->u.MlmePauseAutonomousScanConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanConfirm.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_REQUEST_ID:
+            sig->u.MaPacketRequest.Data.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketRequest.Data.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketRequest.TransmitRate = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketRequest.HostTag = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MaPacketRequest.Priority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MaPacketRequest.Ra.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MaPacketRequest.TransmissionControl = (CSR_TRANSMISSION_CONTROL) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID:
+            sig->u.MlmeModifyBssParameterRequest.Data.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.Data.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.BeaconPeriod = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.DtimPeriod = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeModifyBssParameterRequest.CapabilityInformation = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeModifyBssParameterRequest.Bssid.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeModifyBssParameterRequest.RtsThreshold = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID:
+            sig->u.MlmeAddRxTriggerRequest.InformationElements.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerRequest.InformationElements.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerRequest.TriggerId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerRequest.Priority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_VIF_AVAILABILITY_INDICATION_ID:
+            sig->u.MaVifAvailabilityIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityIndication.Multicast = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID:
+            sig->u.MlmeHlSyncCancelRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeHlSyncCancelRequest.GroupAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID:
+            sig->u.MlmeDelAutonomousScanRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanRequest.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLACKOUT_ENDED_INDICATION_ID:
+            sig->u.MlmeBlackoutEndedIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlackoutEndedIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlackoutEndedIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlackoutEndedIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlackoutEndedIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlackoutEndedIndication.BlackoutId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID:
+            sig->u.MlmeAutonomousScanDoneIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanDoneIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanDoneIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanDoneIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanDoneIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanDoneIndication.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanDoneIndication.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID:
+            sig->u.MlmeGetKeySequenceRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceRequest.KeyId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetKeySequenceRequest.KeyType = (CSR_KEY_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeGetKeySequenceRequest.Address.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_REQUEST_ID:
+            sig->u.MlmeSetChannelRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.Ifindex = (CSR_IFINTERFACE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.Channel = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeSetChannelRequest.Address.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeSetChannelRequest.AvailabilityDuration = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetChannelRequest.AvailabilityInterval = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+            sig->u.MlmeMeasureConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureConfirm.DialogToken = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID:
+            sig->u.MlmeAddTriggeredGetRequest.MibAttribute.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetRequest.MibAttribute.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTriggeredGetRequest.TriggeredId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID:
+            sig->u.MlmeAutonomousScanLossIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanLossIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanLossIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanLossIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAutonomousScanLossIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeAutonomousScanLossIndication.Bssid.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+        case CSR_MA_VIF_AVAILABILITY_RESPONSE_ID:
+            sig->u.MaVifAvailabilityResponse.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityResponse.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityResponse.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityResponse.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityResponse.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaVifAvailabilityResponse.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_REQUEST_ID:
+            sig->u.MlmeAddTemplateRequest.Data1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateRequest.Data1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateRequest.Data2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateRequest.Data2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateRequest.FrameType = (CSR_FRAME_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateRequest.MinTransmitRate = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+            sig->u.MlmePowermgtConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+            sig->u.MlmeAddPeriodicConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicConfirm.PeriodicId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_CONFIRM_ID:
+            sig->u.MlmeGetConfirm.MibAttributeValue.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetConfirm.MibAttributeValue.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetConfirm.Status = (CSR_MIB_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetConfirm.ErrorIndex = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+            sig->u.MlmeGetNextConfirm.MibAttributeValue.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextConfirm.MibAttributeValue.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextConfirm.Status = (CSR_MIB_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetNextConfirm.ErrorIndex = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_REQUEST_ID:
+            sig->u.MlmeStopAggregationRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeStopAggregationRequest.PeerQstaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeStopAggregationRequest.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopAggregationRequest.Direction = (CSR_DIRECTION) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+            sig->u.MlmeAddRxTriggerConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerConfirm.TriggerId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddRxTriggerConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_REQUEST_ID:
+            sig->u.MlmeAddBlackoutRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.BlackoutId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.BlackoutType = (CSR_BLACKOUT_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.BlackoutSource = (CSR_BLACKOUT_SOURCE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddBlackoutRequest.BlackoutStartReference = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeAddBlackoutRequest.BlackoutPeriod = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeAddBlackoutRequest.BlackoutDuration = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            memcpy(sig->u.MlmeAddBlackoutRequest.PeerStaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeAddBlackoutRequest.BlackoutCount = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_REQUEST_ID:
+            sig->u.MlmeDeletekeysRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysRequest.KeyId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDeletekeysRequest.KeyType = (CSR_KEY_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeDeletekeysRequest.Address.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_CONFIRM_ID:
+            sig->u.MlmeResetConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeResetConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+            sig->u.MlmeHlSyncConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeHlSyncConfirm.GroupAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeHlSyncConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID:
+            sig->u.MlmeAddAutonomousScanRequest.ChannelList.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.ChannelList.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.InformationElements.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.InformationElements.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.Ifindex = (CSR_IFINTERFACE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.ChannelStartingFactor = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.ScanType = (CSR_SCAN_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.ProbeDelay = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeAddAutonomousScanRequest.MinChannelTime = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddAutonomousScanRequest.MaxChannelTime = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_REQUEST_ID:
+            sig->u.MlmeSetRequest.MibAttributeValue.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetRequest.MibAttributeValue.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_REQUEST_ID:
+            sig->u.MlmeSmStartRequest.Beacon.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.Beacon.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.BssParameters.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.BssParameters.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.Ifindex = (CSR_IFINTERFACE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.Channel = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeSmStartRequest.InterfaceAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            memcpy(sig->u.MlmeSmStartRequest.Bssid.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeSmStartRequest.BeaconPeriod = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.DtimPeriod = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSmStartRequest.CapabilityInformation = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+            sig->u.MlmeConnectStatusConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeConnectStatusConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+            sig->u.MlmeDelAutonomousScanConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelAutonomousScanConfirm.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_REQUEST_ID:
+            sig->u.MlmeDelPeriodicRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicRequest.PeriodicId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_REQUEST_ID:
+            sig->u.MlmeSetkeysRequest.Key.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.Key.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.Length = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.KeyId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.KeyType = (CSR_KEY_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeSetkeysRequest.Address.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[0] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[1] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[2] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[3] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[4] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[5] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[6] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetkeysRequest.SequenceNumber[7] = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(&sig->u.MlmeSetkeysRequest.CipherSuiteSelector, &ptr[index], 32 / 8);
+            index += 32 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID:
+            sig->u.MlmePauseAutonomousScanRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanRequest.AutonomousScanId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePauseAutonomousScanRequest.Pause = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_REQUEST_ID:
+            sig->u.MlmeGetRequest.MibAttribute.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetRequest.MibAttribute.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeGetRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_REQUEST_ID:
+            sig->u.MlmePowermgtRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.PowerManagementMode = (CSR_POWER_MANAGEMENT_MODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.ReceiveDtims = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.ListenInterval = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmePowermgtRequest.TrafficWindow = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            sig->u.MaPacketErrorIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketErrorIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketErrorIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketErrorIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketErrorIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MaPacketErrorIndication.PeerQstaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MaPacketErrorIndication.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketErrorIndication.SequenceNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_REQUEST_ID:
+            sig->u.MlmeAddPeriodicRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.PeriodicId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.MaximumLatency = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeAddPeriodicRequest.PeriodicSchedulingMode = (CSR_PERIODIC_SCHEDULING_MODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.WakeHost = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddPeriodicRequest.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_REQUEST_ID:
+            sig->u.MlmeAddTspecRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.Direction = (CSR_DIRECTION) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.PsScheme = (CSR_PS_SCHEME) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.MediumTime = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecRequest.ServiceStartTime = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeAddTspecRequest.ServiceInterval = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            sig->u.MlmeAddTspecRequest.MinimumDataRate = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+            sig->u.MlmeAddMulticastAddressConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddMulticastAddressConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+            sig->u.MlmeAddTspecConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecConfirm.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTspecConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+            sig->u.MlmeHlSyncCancelConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeHlSyncCancelConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CONFIRM_ID:
+            sig->u.MlmeScanConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeScanConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            sig->u.DebugStringIndication.DebugMessage.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugStringIndication.DebugMessage.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugStringIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.DebugStringIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+            sig->u.MlmeAddTemplateConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateConfirm.FrameType = (CSR_FRAME_TYPE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeAddTemplateConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLOCKACK_ERROR_INDICATION_ID:
+            sig->u.MlmeBlockackErrorIndication.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlockackErrorIndication.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlockackErrorIndication.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlockackErrorIndication.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlockackErrorIndication.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeBlockackErrorIndication.ResultCode = (CSR_REASON_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeBlockackErrorIndication.PeerQstaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CONFIRM_ID:
+            sig->u.MlmeSetConfirm.MibAttributeValue.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetConfirm.MibAttributeValue.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetConfirm.Status = (CSR_MIB_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeSetConfirm.ErrorIndex = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_REQUEST_ID:
+            sig->u.MlmeMeasureRequest.MeasurementRequestSet.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureRequest.MeasurementRequestSet.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeMeasureRequest.DialogToken = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+            sig->u.MlmeStartAggregationConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(sig->u.MlmeStartAggregationConfirm.PeerQstaAddress.x, &ptr[index], 48 / 8);
+            index += 48 / 8;
+            sig->u.MlmeStartAggregationConfirm.UserPriority = (CSR_PRIORITY) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.Direction = (CSR_DIRECTION) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStartAggregationConfirm.SequenceNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+            sig->u.MlmeStopMeasureConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureConfirm.DialogToken = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_CONFIRM_ID:
+            sig->u.MaPacketConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.TransmissionStatus = (CSR_TRANSMISSION_STATUS) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.RetryCount = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.Rate = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MaPacketConfirm.HostTag = CSR_GET_UINT32_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT32;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+            sig->u.MlmeDelPeriodicConfirm.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicConfirm.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicConfirm.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicConfirm.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicConfirm.VirtualInterfaceIdentifier = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicConfirm.PeriodicId = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeDelPeriodicConfirm.ResultCode = (CSR_RESULT_CODE) CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_REQUEST_ID:
+            sig->u.MlmeStopMeasureRequest.Dummydataref1.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureRequest.Dummydataref1.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureRequest.Dummydataref2.SlotNumber = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureRequest.Dummydataref2.DataLength = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            sig->u.MlmeStopMeasureRequest.DialogToken = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+
+        default:
+            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+    return CSR_RESULT_SUCCESS;
+} /* read_unpack_signal() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  write_pack
+ *
+ *      Convert a signal structure, in host-native format, to the
+ *      little-endian wire format specified in the UniFi Host Interface
+ *      Protocol Specification.
+ *
+ *      WARNING: This function is auto-generated, DO NOT EDIT!
+ *
+ *  Arguments:
+ *      sig             Pointer to signal structure to pack.
+ *      ptr             Destination buffer to pack into.
+ *      sig_len         Returns the length of the packed signal, i.e. the
+ *                      number of bytes written to ptr.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success,
+ *      CSR_WIFI_HIP_RESULT_INVALID_VALUE if the ID of signal was not recognised.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult write_pack(const CSR_SIGNAL *sig, u8 *ptr, u16 *sig_len)
+{
+    s16 index = 0;
+
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->SignalPrimitiveHeader.SignalId, ptr + index);
+    index += SIZEOF_UINT16;
+
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->SignalPrimitiveHeader.ReceiverProcessId, ptr + index);
+    index += SIZEOF_UINT16;
+
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->SignalPrimitiveHeader.SenderProcessId, ptr + index);
+    index += SIZEOF_UINT16;
+
+    switch (sig->SignalPrimitiveHeader.SignalId)
+    {
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanConfirm.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.BlackoutId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutRequest.BlackoutId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[0], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[1], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[2], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[3], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[4], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[5], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[6], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceConfirm.SequenceNumber[7], ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeStopAggregationConfirm.PeerQstaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.Direction, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecRequest.Direction, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[0], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[1], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[2], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[3], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[4], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[5], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[6], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[7], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[8], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[9], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[10], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[11], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[12], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[13], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[14], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugWord16Indication.DebugWords[15], ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugVariable.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugVariable.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[0], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[1], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[2], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[3], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[4], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[5], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[6], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericConfirm.DebugWords[7], ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_MA_PACKET_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Data.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Data.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MaPacketIndication.LocalTime.x, 64 / 8);
+            index += 64 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Ifindex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Channel, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.ReceptionStatus, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Rssi, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.Snr, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketIndication.ReceivedRate, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_MLME_SET_TIM_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.AssociationId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimRequest.TimValue, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectedIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectedIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectedIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectedIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectedIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectedIndication.ConnectionStatus, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeConnectedIndication.PeerMacAddress.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerRequest.TriggerId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_TRIGGERED_GET_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.MibAttributeValue.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.MibAttributeValue.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.Status, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.ErrorIndex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeTriggeredGetIndication.TriggeredId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.ChannelList.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.ChannelList.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.InformationElements.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.InformationElements.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.Ifindex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.ScanType, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.ProbeDelay, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.MinChannelTime, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanRequest.MaxChannelTime, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextRequest.MibAttribute.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextRequest.MibAttribute.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeStartAggregationRequest.PeerQstaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.Direction, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.StartingSequenceNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.BufferSize, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationRequest.BlockAckTimeout, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeHlSyncRequest.GroupAddress.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+        case CSR_DEBUG_GENERIC_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugVariable.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugVariable.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[0], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[1], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[2], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[3], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[4], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[5], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[6], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericRequest.DebugWords[7], ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetRequest.TriggeredId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressRequest.Data.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressRequest.Data.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressRequest.NumberOfMulticastGroupAddresses, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeResetRequest.StaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetRequest.SetDefaultMib, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CANCEL_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanCancelRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanCancelRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanCancelRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanCancelRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanCancelRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetConfirm.TriggeredId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.InformationElements.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.InformationElements.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.PacketFilterMode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeSetPacketFilterRequest.ArpFilterAddress, ptr + index);
+            index += SIZEOF_UINT32;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.TriggerId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelRxTriggerConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.InformationElements.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.InformationElements.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.ConnectionStatus, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeConnectStatusRequest.StaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.AssociationId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusRequest.AssociationCapabilityInformation, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeLeaveRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.QueueIndex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Aifs, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Cwmin, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.Cwmax, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConfigQueueRequest.TxopLimit, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTspecConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetTimConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureIndication.MeasurementReportSet.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureIndication.MeasurementReportSet.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureIndication.DialogToken, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.BlackoutId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelBlackoutConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelTriggeredGetConfirm.TriggeredId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugVariable.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugVariable.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[0], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[1], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[2], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[3], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[4], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[5], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[6], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugGenericIndication.DebugWords[7], ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+        case CSR_MA_PACKET_CANCEL_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketCancelRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketCancelRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketCancelRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketCancelRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketCancelRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MaPacketCancelRequest.HostTag, ptr + index);
+            index += SIZEOF_UINT32;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanConfirm.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.Data.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.Data.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.TransmitRate, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.HostTag, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.Priority, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MaPacketRequest.Ra.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketRequest.TransmissionControl, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.Data.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.Data.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.BeaconPeriod, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.DtimPeriod, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.CapabilityInformation, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeModifyBssParameterRequest.Bssid.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeModifyBssParameterRequest.RtsThreshold, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.InformationElements.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.InformationElements.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.TriggerId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerRequest.Priority, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_VIF_AVAILABILITY_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityIndication.Multicast, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeHlSyncCancelRequest.GroupAddress.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanRequest.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLACKOUT_ENDED_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlackoutEndedIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlackoutEndedIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlackoutEndedIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlackoutEndedIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlackoutEndedIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlackoutEndedIndication.BlackoutId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanDoneIndication.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.KeyId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetKeySequenceRequest.KeyType, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeGetKeySequenceRequest.Address.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.Ifindex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.Channel, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeSetChannelRequest.Address.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.AvailabilityDuration, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetChannelRequest.AvailabilityInterval, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureConfirm.DialogToken, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetRequest.MibAttribute.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetRequest.MibAttribute.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTriggeredGetRequest.TriggeredId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanLossIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanLossIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanLossIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanLossIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAutonomousScanLossIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeAutonomousScanLossIndication.Bssid.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+        case CSR_MA_VIF_AVAILABILITY_RESPONSE_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityResponse.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityResponse.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityResponse.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityResponse.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityResponse.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaVifAvailabilityResponse.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.Data1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.Data1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.Data2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.Data2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.FrameType, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateRequest.MinTransmitRate, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.PeriodicId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetConfirm.MibAttributeValue.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetConfirm.MibAttributeValue.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetConfirm.Status, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetConfirm.ErrorIndex, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextConfirm.MibAttributeValue.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextConfirm.MibAttributeValue.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextConfirm.Status, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetNextConfirm.ErrorIndex, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeStopAggregationRequest.PeerQstaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopAggregationRequest.Direction, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.TriggerId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddRxTriggerConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutType, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutSource, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutStartReference, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutPeriod, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutDuration, ptr + index);
+            index += SIZEOF_UINT32;
+            memcpy(ptr + index, sig->u.MlmeAddBlackoutRequest.PeerStaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddBlackoutRequest.BlackoutCount, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.KeyId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDeletekeysRequest.KeyType, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeDeletekeysRequest.Address.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeResetConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeHlSyncConfirm.GroupAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.ChannelList.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.ChannelList.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.InformationElements.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.InformationElements.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.Ifindex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.ChannelStartingFactor, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.ScanType, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.ProbeDelay, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.MinChannelTime, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddAutonomousScanRequest.MaxChannelTime, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetRequest.MibAttributeValue.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetRequest.MibAttributeValue.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.Beacon.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.Beacon.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.BssParameters.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.BssParameters.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.Ifindex, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.Channel, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeSmStartRequest.InterfaceAddress.x, 48 / 8);
+            index += 48 / 8;
+            memcpy(ptr + index, sig->u.MlmeSmStartRequest.Bssid.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.BeaconPeriod, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.DtimPeriod, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSmStartRequest.CapabilityInformation, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeConnectStatusConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelAutonomousScanConfirm.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicRequest.PeriodicId, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.Key.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.Key.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.Length, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.KeyId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.KeyType, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeSetkeysRequest.Address.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[0], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[1], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[2], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[3], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[4], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[5], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[6], ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetkeysRequest.SequenceNumber[7], ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, &sig->u.MlmeSetkeysRequest.CipherSuiteSelector, 32 / 8);
+            index += 32 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.AutonomousScanId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePauseAutonomousScanRequest.Pause, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetRequest.MibAttribute.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetRequest.MibAttribute.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeGetRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.PowerManagementMode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.ReceiveDtims, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.ListenInterval, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmePowermgtRequest.TrafficWindow, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MaPacketErrorIndication.PeerQstaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketErrorIndication.SequenceNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.PeriodicId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.MaximumLatency, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.PeriodicSchedulingMode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.WakeHost, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddPeriodicRequest.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.Direction, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.PsScheme, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.MediumTime, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.ServiceStartTime, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.ServiceInterval, ptr + index);
+            index += SIZEOF_UINT32;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecRequest.MinimumDataRate, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddMulticastAddressConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTspecConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeHlSyncCancelConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeScanConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugStringIndication.DebugMessage.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugStringIndication.DebugMessage.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugStringIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.DebugStringIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.FrameType, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeAddTemplateConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLOCKACK_ERROR_INDICATION_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlockackErrorIndication.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlockackErrorIndication.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlockackErrorIndication.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlockackErrorIndication.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlockackErrorIndication.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeBlockackErrorIndication.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeBlockackErrorIndication.PeerQstaAddress.x, 48 / 8);
+            index += 48 / 8;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetConfirm.MibAttributeValue.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetConfirm.MibAttributeValue.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetConfirm.Status, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeSetConfirm.ErrorIndex, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureRequest.MeasurementRequestSet.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureRequest.MeasurementRequestSet.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeMeasureRequest.DialogToken, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            memcpy(ptr + index, sig->u.MlmeStartAggregationConfirm.PeerQstaAddress.x, 48 / 8);
+            index += 48 / 8;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.UserPriority, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.Direction, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStartAggregationConfirm.SequenceNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureConfirm.DialogToken, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+        case CSR_MA_PACKET_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.TransmissionStatus, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.RetryCount, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.Rate, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT32_TO_LITTLE_ENDIAN(sig->u.MaPacketConfirm.HostTag, ptr + index);
+            index += SIZEOF_UINT32;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.VirtualInterfaceIdentifier, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.PeriodicId, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeDelPeriodicConfirm.ResultCode, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_REQUEST_ID:
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureRequest.Dummydataref1.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureRequest.Dummydataref1.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureRequest.Dummydataref2.SlotNumber, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureRequest.Dummydataref2.DataLength, ptr + index);
+            index += SIZEOF_UINT16;
+            CSR_COPY_UINT16_TO_LITTLE_ENDIAN(sig->u.MlmeStopMeasureRequest.DialogToken, ptr + index);
+            index += SIZEOF_UINT16;
+            break;
+#endif
+
+        default:
+            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    *sig_len = index;
+
+    return CSR_RESULT_SUCCESS;
+} /* write_pack() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_send.c b/drivers/staging/csr/csr_wifi_hip_send.c
new file mode 100644
index 00000000000..684d30459d7
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_send.c
@@ -0,0 +1,422 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ***************************************************************************
+ *
+ *  FILE:     csr_wifi_hip_send.c
+ *
+ *  PURPOSE:
+ *      Code for adding a signal request to the from-host queue.
+ *      When the driver bottom-half is run, it will take requests from the
+ *      queue and pass them to the UniFi.
+ *
+ * ***************************************************************************
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "csr_wifi_hip_sigs.h"
+#include "csr_wifi_hip_card.h"
+
+unifi_TrafficQueue unifi_frame_priority_to_queue(CSR_PRIORITY priority)
+{
+    switch (priority)
+    {
+        case CSR_QOS_UP0:
+        case CSR_QOS_UP3:
+            return UNIFI_TRAFFIC_Q_BE;
+        case CSR_QOS_UP1:
+        case CSR_QOS_UP2:
+            return UNIFI_TRAFFIC_Q_BK;
+        case CSR_QOS_UP4:
+        case CSR_QOS_UP5:
+            return UNIFI_TRAFFIC_Q_VI;
+        case CSR_QOS_UP6:
+        case CSR_QOS_UP7:
+        case CSR_MANAGEMENT:
+            return UNIFI_TRAFFIC_Q_VO;
+        default:
+            return UNIFI_TRAFFIC_Q_BE;
+    }
+}
+
+
+CSR_PRIORITY unifi_get_default_downgrade_priority(unifi_TrafficQueue queue)
+{
+    switch (queue)
+    {
+        case UNIFI_TRAFFIC_Q_BE:
+            return CSR_QOS_UP0;
+        case UNIFI_TRAFFIC_Q_BK:
+            return CSR_QOS_UP1;
+        case UNIFI_TRAFFIC_Q_VI:
+            return CSR_QOS_UP5;
+        case UNIFI_TRAFFIC_Q_VO:
+            return CSR_QOS_UP6;
+        default:
+            return CSR_QOS_UP0;
+    }
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  send_signal
+ *
+ *      This function queues a signal for sending to UniFi.  It first checks
+ *      that there is space on the fh_signal_queue for another entry, then
+ *      claims any bulk data slots required and copies data into them. Then
+ *      increments the fh_signal_queue write count.
+ *
+ *      The fh_signal_queue is later processed by the driver bottom half
+ *      (in unifi_bh()).
+ *
+ *      This function call unifi_pause_xmit() to pause the flow of data plane
+ *      packets when:
+ *        - the fh_signal_queue ring buffer is full
+ *        - there are less than UNIFI_MAX_DATA_REFERENCES (2) bulk data
+ *          slots available.
+ *
+ *  Arguments:
+ *      card            Pointer to card context structure
+ *      sigptr          Pointer to the signal to write to UniFi.
+ *      siglen          Number of bytes pointer to by sigptr.
+ *      bulkdata        Array of pointers to an associated bulk data.
+ *      sigq            To which from-host queue to add the signal.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success
+ *      CSR_WIFI_HIP_RESULT_NO_SPACE if there were insufficient data slots or
+ *                              no free signal queue entry
+ *
+ * Notes:
+ *      Calls unifi_pause_xmit() when the last slots are used.
+ * ---------------------------------------------------------------------------
+ */
+static CsrResult send_signal(card_t *card, const u8 *sigptr, u32 siglen,
+                             const bulk_data_param_t *bulkdata,
+                             q_t *sigq, u32 priority_q, u32 run_bh)
+{
+    u16 i, data_slot_size;
+    card_signal_t *csptr;
+    s16 qe;
+    CsrResult r;
+    s16 debug_print = 0;
+
+    data_slot_size = CardGetDataSlotSize(card);
+
+    /* Check that the fh_data_queue has a free slot */
+    if (!CSR_WIFI_HIP_Q_SLOTS_FREE(sigq))
+    {
+        unifi_trace(card->ospriv, UDBG3, "send_signal: %s full\n", sigq->name);
+
+        return CSR_WIFI_HIP_RESULT_NO_SPACE;
+    }
+
+    /*
+     * Now add the signal to the From Host signal queue
+     */
+    /* Get next slot on queue */
+    qe = CSR_WIFI_HIP_Q_NEXT_W_SLOT(sigq);
+    csptr = CSR_WIFI_HIP_Q_SLOT_DATA(sigq, qe);
+
+    /* Make up the card_signal struct */
+    csptr->signal_length = (u16)siglen;
+    memcpy((void *)csptr->sigbuf, (void *)sigptr, siglen);
+
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; ++i)
+    {
+        if ((bulkdata != NULL) && (bulkdata->d[i].data_length != 0))
+        {
+            u32 datalen = bulkdata->d[i].data_length;
+
+            /* Make sure data will fit in a bulk data slot */
+            if (bulkdata->d[i].os_data_ptr == NULL)
+            {
+                unifi_error(card->ospriv, "send_signal - NULL bulkdata[%d]\n", i);
+                debug_print++;
+                csptr->bulkdata[i].data_length = 0;
+            }
+            else
+            {
+                if (datalen > data_slot_size)
+                {
+                    unifi_error(card->ospriv,
+                                "send_signal - Invalid data length %u (@%p), "
+                                "truncating\n",
+                                datalen, bulkdata->d[i].os_data_ptr);
+                    datalen = data_slot_size;
+                    debug_print++;
+                }
+                /* Store the bulk data info in the soft queue. */
+                csptr->bulkdata[i].os_data_ptr = (u8 *)bulkdata->d[i].os_data_ptr;
+                csptr->bulkdata[i].os_net_buf_ptr = (u8 *)bulkdata->d[i].os_net_buf_ptr;
+                csptr->bulkdata[i].net_buf_length = bulkdata->d[i].net_buf_length;
+                csptr->bulkdata[i].data_length = datalen;
+            }
+        }
+        else
+        {
+            UNIFI_INIT_BULK_DATA(&csptr->bulkdata[i]);
+        }
+    }
+
+    if (debug_print)
+    {
+        const u8 *sig = sigptr;
+
+        unifi_error(card->ospriv, "Signal(%d): %02x %02x %02x %02x %02x %02x %02x %02x"
+                    " %02x %02x %02x %02x %02x %02x %02x %02x\n",
+                    siglen,
+                    sig[0], sig[1], sig[2], sig[3],
+                    sig[4], sig[5], sig[6], sig[7],
+                    sig[8], sig[9], sig[10], sig[11],
+                    sig[12], sig[13], sig[14], sig[15]);
+        unifi_error(card->ospriv, "Bulkdata pointer %p(%d), %p(%d)\n",
+                    bulkdata != NULL?bulkdata->d[0].os_data_ptr : NULL,
+                    bulkdata != NULL?bulkdata->d[0].data_length : 0,
+                    bulkdata != NULL?bulkdata->d[1].os_data_ptr : NULL,
+                    bulkdata != NULL?bulkdata->d[1].data_length : 0);
+    }
+
+    /* Advance the written count to say there is a new entry */
+    CSR_WIFI_HIP_Q_INC_W(sigq);
+
+    /*
+     * Set the flag to say reason for waking was a host request.
+     * Then ask the OS layer to run the unifi_bh.
+     */
+    if (run_bh == 1)
+    {
+        card->bh_reason_host = 1;
+        r = unifi_run_bh(card->ospriv);
+        if (r != CSR_RESULT_SUCCESS)
+        {
+            unifi_error(card->ospriv, "failed to run bh.\n");
+            card->bh_reason_host = 0;
+
+            /*
+             * The bulk data buffer will be freed by the caller.
+             * We need to invalidate the description of the bulk data in our
+             * soft queue, to prevent the core freeing the bulk data again later.
+             */
+            for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; ++i)
+            {
+                if (csptr->bulkdata[i].data_length != 0)
+                {
+                    csptr->bulkdata[i].os_data_ptr = csptr->bulkdata[i].os_net_buf_ptr = NULL;
+                    csptr->bulkdata[i].net_buf_length = csptr->bulkdata[i].data_length = 0;
+                }
+            }
+            return r;
+        }
+    }
+    else
+    {
+        unifi_error(card->ospriv, "run_bh=%d, bh not called.\n", run_bh);
+    }
+
+    /*
+     * Have we used up all the fh signal list entries?
+     */
+    if (CSR_WIFI_HIP_Q_SLOTS_FREE(sigq) == 0)
+    {
+        /* We have filled the queue, so stop the upper layer. The command queue
+         * is an exception, as suspending due to that being full could delay
+         * resume/retry until new commands or data are received.
+         */
+        if (sigq != &card->fh_command_queue)
+        {
+            /*
+             * Must call unifi_pause_xmit() *before* setting the paused flag.
+             * (the unifi_pause_xmit call should not be after setting the flag because of the possibility of being interrupted
+             * by the bh thread between our setting the flag and the call to unifi_pause_xmit()
+             * If bh thread then cleared the flag, we would end up paused, but without the flag set)
+             * Instead, setting it afterwards means that if this thread is interrupted by the bh thread
+             * the pause flag is still guaranteed to end up set
+             * However the potential deadlock now is that if bh thread emptied the queue and cleared the flag before this thread's
+             * call to unifi_pause_xmit(), then bh thread may not run again because it will be waiting for
+             * a packet to appear in the queue but nothing ever will because xmit is paused.
+             * So we will end up with the queue paused, and the flag set to say it is paused, but bh never runs to unpause it.
+             * (Note even this bad situation would not persist long in practice, because something else (eg rx, or tx in different queue)
+             * is likely to wake bh thread quite soon)
+             * But to avoid this deadlock completely, after setting the flag we check that there is something left in the queue.
+             * If there is, we know that bh thread has not emptied the queue yet.
+             * Since bh thread checks to unpause the queue *after* taking packets from the queue, we know that it is still going to make at
+             * least one more check to see whether it needs to unpause the queue.  So all is well.
+             * If there are no packets in the queue, then the deadlock described above might happen.  To make sure it does not, we
+             * unpause the queue here. A possible side effect is that unifi_restart_xmit() may (rarely) be called for second time
+             *  unnecessarily, which is harmless
+             */
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+            unifi_debug_log_to_buf("P");
+#endif
+            unifi_pause_xmit(card->ospriv, (unifi_TrafficQueue)priority_q);
+            card_tx_q_pause(card, priority_q);
+            if (CSR_WIFI_HIP_Q_SLOTS_USED(sigq) == 0)
+            {
+                card_tx_q_unpause(card, priority_q);
+                unifi_restart_xmit(card->ospriv, (unifi_TrafficQueue) priority_q);
+            }
+        }
+        else
+        {
+            unifi_warning(card->ospriv,
+                          "send_signal: fh_cmd_q full, not pausing (run_bh=%d)\n",
+                          run_bh);
+        }
+    }
+
+    func_exit();
+
+    return CSR_RESULT_SUCCESS;
+} /*  send_signal() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_send_signal
+ *
+ *    Invokes send_signal() to queue a signal in the command or traffic queue
+ *    If sigptr pointer is NULL, it pokes the bh to check if UniFi is responsive.
+ *
+ *  Arguments:
+ *      card        Pointer to card context struct
+ *      sigptr      Pointer to signal from card.
+ *      siglen      Size of the signal
+ *      bulkdata    Pointer to the bulk data of the signal
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success
+ *      CSR_WIFI_HIP_RESULT_NO_SPACE if there were insufficient data slots or no free signal queue entry
+ *
+ *  Notes:
+ *      unifi_send_signal() is used to queue signals, created by the driver,
+ *      to the device. Signals are constructed using the UniFi packed structures.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_send_signal(card_t *card, const u8 *sigptr, u32 siglen,
+                            const bulk_data_param_t *bulkdata)
+{
+    q_t *sig_soft_q;
+    u16 signal_id;
+    CsrResult r;
+    u32 run_bh;
+    u32 priority_q;
+
+    /* A NULL signal pointer is a request to check if UniFi is responsive */
+    if (sigptr == NULL)
+    {
+        card->bh_reason_host = 1;
+        return unifi_run_bh(card->ospriv);
+    }
+
+    priority_q = 0;
+    run_bh = 1;
+    signal_id = GET_SIGNAL_ID(sigptr);
+    /*
+     * If the signal is a CSR_MA_PACKET_REQUEST ,
+     * we send it using the traffic soft queue. Else we use the command soft queue.
+     */
+    if (signal_id == CSR_MA_PACKET_REQUEST_ID)
+    {
+        u16 frame_priority;
+
+        if (card->periodic_wake_mode == UNIFI_PERIODIC_WAKE_HOST_ENABLED)
+        {
+            run_bh = 0;
+        }
+
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE) && defined (CSR_WIFI_HIP_DATA_PLANE_PROFILE)
+        unifi_debug_log_to_buf("D");
+#endif
+        /* Sanity check: MA-PACKET.req must have a valid bulk data */
+        if ((bulkdata->d[0].data_length == 0) || (bulkdata->d[0].os_data_ptr == NULL))
+        {
+            unifi_error(card->ospriv, "MA-PACKET.req with empty bulk data (%d bytes in %p)\n",
+                        bulkdata->d[0].data_length, bulkdata->d[0].os_data_ptr);
+            dump((void *)sigptr, siglen);
+            return CSR_RESULT_FAILURE;
+        }
+
+        /* Map the frame priority to a traffic queue index. */
+        frame_priority = GET_PACKED_MA_PACKET_REQUEST_FRAME_PRIORITY(sigptr);
+        priority_q = unifi_frame_priority_to_queue((CSR_PRIORITY)frame_priority);
+
+        sig_soft_q = &card->fh_traffic_queue[priority_q];
+    }
+    else
+    {
+        sig_soft_q = &card->fh_command_queue;
+    }
+
+    r = send_signal(card, sigptr, siglen, bulkdata, sig_soft_q, priority_q, run_bh);
+    /* On error, the caller must free or requeue bulkdata buffers */
+
+    return r;
+} /* unifi_send_signal() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_send_resources_available
+ *
+ *      Examines whether there is available space to queue
+ *      a signal in the command or traffic queue
+ *
+ *  Arguments:
+ *      card        Pointer to card context struct
+ *      sigptr      Pointer to signal.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS if resources available
+ *      CSR_WIFI_HIP_RESULT_NO_SPACE if there was no free signal queue entry
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_send_resources_available(card_t *card, const u8 *sigptr)
+{
+    q_t *sig_soft_q;
+    u16 signal_id = GET_SIGNAL_ID(sigptr);
+
+    /*
+     * If the signal is a CSR_MA_PACKET_REQUEST ,
+     * we send it using the traffic soft queue. Else we use the command soft queue.
+     */
+    if (signal_id == CSR_MA_PACKET_REQUEST_ID)
+    {
+        u16 frame_priority;
+        u32 priority_q;
+
+        /* Map the frame priority to a traffic queue index. */
+        frame_priority = GET_PACKED_MA_PACKET_REQUEST_FRAME_PRIORITY(sigptr);
+        priority_q = unifi_frame_priority_to_queue((CSR_PRIORITY)frame_priority);
+
+        sig_soft_q = &card->fh_traffic_queue[priority_q];
+    }
+    else
+    {
+        sig_soft_q = &card->fh_command_queue;
+    }
+
+    /* Check that the fh_data_queue has a free slot */
+    if (!CSR_WIFI_HIP_Q_SLOTS_FREE(sig_soft_q))
+    {
+        unifi_notice(card->ospriv, "unifi_send_resources_available: %s full\n",
+                     sig_soft_q->name);
+        return CSR_WIFI_HIP_RESULT_NO_SPACE;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_send_resources_available() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_signals.c b/drivers/staging/csr/csr_wifi_hip_signals.c
new file mode 100644
index 00000000000..3c821320df0
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_signals.c
@@ -0,0 +1,1313 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+
+/* Generated by hip_dd_l_c_gen.pl */
+
+#include "csr_wifi_hip_signals.h"
+
+#include "csr_wifi_hip_unifi.h"
+
+s32 SigGetSize(const CSR_SIGNAL *aSignal)
+{
+    switch (aSignal->SignalPrimitiveHeader.SignalId)
+    {
+        case CSR_MA_PACKET_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_PACKET_REQUEST);
+        case CSR_MA_PACKET_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_PACKET_CONFIRM);
+        case CSR_MA_PACKET_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_PACKET_INDICATION);
+        case CSR_MA_PACKET_CANCEL_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_PACKET_CANCEL_REQUEST);
+        case CSR_MA_VIF_AVAILABILITY_RESPONSE_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_VIF_AVAILABILITY_RESPONSE);
+        case CSR_MA_VIF_AVAILABILITY_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_VIF_AVAILABILITY_INDICATION);
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MA_PACKET_ERROR_INDICATION);
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_RESET_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_RESET_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_GET_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_GET_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_GET_NEXT_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_GET_NEXT_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_POWERMGT_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_POWERMGT_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SCAN_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SCAN_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_HL_SYNC_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_HL_SYNC_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_MEASURE_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_MEASURE_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_MEASURE_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SETKEYS_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SETKEYS_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DELETEKEYS_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DELETEKEYS_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_CONNECTED_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CANCEL_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SCAN_CANCEL_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_HL_SYNC_CANCEL_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_HL_SYNC_CANCEL_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_PERIODIC_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_PERIODIC_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_PERIODIC_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_PERIODIC_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_PACKET_FILTER_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_PACKET_FILTER_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_STOP_MEASURE_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_STOP_MEASURE_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_TRIGGERED_GET_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_TRIGGERED_GET_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_TRIGGERED_GET_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_TRIGGERED_GET_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_TRIGGERED_GET_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_TRIGGERED_GET_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_BLACKOUT_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_BLACKOUT_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLACKOUT_ENDED_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_BLACKOUT_ENDED_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_BLACKOUT_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_BLACKOUT_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_RX_TRIGGER_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_RX_TRIGGER_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_RX_TRIGGER_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_RX_TRIGGER_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_CONNECT_STATUS_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_CONNECT_STATUS_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_TEMPLATE_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_TEMPLATE_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_CONFIG_QUEUE_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_CONFIG_QUEUE_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_TSPEC_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_TSPEC_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_TSPEC_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_DEL_TSPEC_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_START_AGGREGATION_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_START_AGGREGATION_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLOCKACK_ERROR_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_BLOCKACK_ERROR_INDICATION);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_STOP_AGGREGATION_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_STOP_AGGREGATION_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SM_START_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SM_START_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_LEAVE_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_LEAVE_CONFIRM);
+#endif
+        case CSR_MLME_SET_TIM_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_TIM_REQUEST);
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_TIM_CONFIRM);
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_GET_KEY_SEQUENCE_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_GET_KEY_SEQUENCE_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_CHANNEL_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_SET_CHANNEL_CONFIRM);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST);
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM);
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_DEBUG_STRING_INDICATION);
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_DEBUG_WORD16_INDICATION);
+        case CSR_DEBUG_GENERIC_REQUEST_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_DEBUG_GENERIC_REQUEST);
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_DEBUG_GENERIC_CONFIRM);
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            return offsetof(struct CSR_SIGNAL_PRIMITIVE, u) + sizeof(CSR_DEBUG_GENERIC_INDICATION);
+        default:
+            return 0;
+    }
+}
+
+
+s32 SigGetDataRefs(CSR_SIGNAL *aSignal, CSR_DATAREF **aDataRef)
+{
+    s32 numRefs = 0;
+
+    switch (aSignal->SignalPrimitiveHeader.SignalId)
+    {
+        case CSR_MA_PACKET_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaPacketRequest.Data;
+            aDataRef[numRefs++] = &aSignal->u.MaPacketRequest.Dummydataref2;
+            break;
+        case CSR_MA_PACKET_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaPacketConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MaPacketConfirm.Dummydataref2;
+            break;
+        case CSR_MA_PACKET_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaPacketIndication.Data;
+            aDataRef[numRefs++] = &aSignal->u.MaPacketIndication.Dummydataref2;
+            break;
+        case CSR_MA_PACKET_CANCEL_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaPacketCancelRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MaPacketCancelRequest.Dummydataref2;
+            break;
+        case CSR_MA_VIF_AVAILABILITY_RESPONSE_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaVifAvailabilityResponse.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MaVifAvailabilityResponse.Dummydataref2;
+            break;
+        case CSR_MA_VIF_AVAILABILITY_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaVifAvailabilityIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MaVifAvailabilityIndication.Dummydataref2;
+            break;
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MaPacketErrorIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MaPacketErrorIndication.Dummydataref2;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeResetRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeResetRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeResetConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeResetConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetRequest.MibAttribute;
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetConfirm.MibAttributeValue;
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetRequest.MibAttributeValue;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetConfirm.MibAttributeValue;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetNextRequest.MibAttribute;
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetNextRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetNextConfirm.MibAttributeValue;
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetNextConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmePowermgtRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmePowermgtRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmePowermgtConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmePowermgtConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeScanRequest.ChannelList;
+            aDataRef[numRefs++] = &aSignal->u.MlmeScanRequest.InformationElements;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeScanConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeScanConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeMeasureRequest.MeasurementRequestSet;
+            aDataRef[numRefs++] = &aSignal->u.MlmeMeasureRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeMeasureConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeMeasureConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeMeasureIndication.MeasurementReportSet;
+            aDataRef[numRefs++] = &aSignal->u.MlmeMeasureIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetkeysRequest.Key;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetkeysRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetkeysConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetkeysConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDeletekeysRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDeletekeysRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDeletekeysConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDeletekeysConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAutonomousScanLossIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAutonomousScanLossIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeConnectedIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeConnectedIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CANCEL_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeScanCancelRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeScanCancelRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncCancelRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncCancelRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncCancelConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeHlSyncCancelConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddPeriodicRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddPeriodicRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddPeriodicConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddPeriodicConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelPeriodicRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelPeriodicRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelPeriodicConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelPeriodicConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddAutonomousScanRequest.ChannelList;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddAutonomousScanRequest.InformationElements;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddAutonomousScanConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddAutonomousScanConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelAutonomousScanRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelAutonomousScanRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelAutonomousScanConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelAutonomousScanConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetPacketFilterRequest.InformationElements;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetPacketFilterRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetPacketFilterConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetPacketFilterConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopMeasureRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopMeasureRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopMeasureConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopMeasureConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmePauseAutonomousScanRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmePauseAutonomousScanRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmePauseAutonomousScanConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmePauseAutonomousScanConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAutonomousScanDoneIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAutonomousScanDoneIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTriggeredGetRequest.MibAttribute;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTriggeredGetRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTriggeredGetConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTriggeredGetConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTriggeredGetRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTriggeredGetRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTriggeredGetConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTriggeredGetConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_TRIGGERED_GET_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeTriggeredGetIndication.MibAttributeValue;
+            aDataRef[numRefs++] = &aSignal->u.MlmeTriggeredGetIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddBlackoutRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddBlackoutRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddBlackoutConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddBlackoutConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLACKOUT_ENDED_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeBlackoutEndedIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeBlackoutEndedIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelBlackoutRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelBlackoutRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelBlackoutConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelBlackoutConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddRxTriggerRequest.InformationElements;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddRxTriggerRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddRxTriggerConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddRxTriggerConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelRxTriggerRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelRxTriggerRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelRxTriggerConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelRxTriggerConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeConnectStatusRequest.InformationElements;
+            aDataRef[numRefs++] = &aSignal->u.MlmeConnectStatusRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeConnectStatusConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeConnectStatusConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeModifyBssParameterRequest.Data;
+            aDataRef[numRefs++] = &aSignal->u.MlmeModifyBssParameterRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeModifyBssParameterConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeModifyBssParameterConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTemplateRequest.Data1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTemplateRequest.Data2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTemplateConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTemplateConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeConfigQueueRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeConfigQueueRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeConfigQueueConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeConfigQueueConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTspecRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTspecRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTspecConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddTspecConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTspecRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTspecRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTspecConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeDelTspecConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeStartAggregationRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeStartAggregationRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeStartAggregationConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeStartAggregationConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLOCKACK_ERROR_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeBlockackErrorIndication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeBlockackErrorIndication.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopAggregationRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopAggregationRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopAggregationConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeStopAggregationConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSmStartRequest.Beacon;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSmStartRequest.BssParameters;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSmStartConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSmStartConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeLeaveRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeLeaveRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeLeaveConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeLeaveConfirm.Dummydataref2;
+            break;
+#endif
+        case CSR_MLME_SET_TIM_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetTimRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetTimRequest.Dummydataref2;
+            break;
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetTimConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetTimConfirm.Dummydataref2;
+            break;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetKeySequenceRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetKeySequenceRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetKeySequenceConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeGetKeySequenceConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetChannelRequest.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetChannelRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetChannelConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeSetChannelConfirm.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddMulticastAddressRequest.Data;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddMulticastAddressRequest.Dummydataref2;
+            break;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddMulticastAddressConfirm.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.MlmeAddMulticastAddressConfirm.Dummydataref2;
+            break;
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.DebugStringIndication.DebugMessage;
+            aDataRef[numRefs++] = &aSignal->u.DebugStringIndication.Dummydataref2;
+            break;
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.DebugWord16Indication.Dummydataref1;
+            aDataRef[numRefs++] = &aSignal->u.DebugWord16Indication.Dummydataref2;
+            break;
+        case CSR_DEBUG_GENERIC_REQUEST_ID:
+            aDataRef[numRefs++] = &aSignal->u.DebugGenericRequest.DebugVariable;
+            aDataRef[numRefs++] = &aSignal->u.DebugGenericRequest.Dummydataref2;
+            break;
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            aDataRef[numRefs++] = &aSignal->u.DebugGenericConfirm.DebugVariable;
+            aDataRef[numRefs++] = &aSignal->u.DebugGenericConfirm.Dummydataref2;
+            break;
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            aDataRef[numRefs++] = &aSignal->u.DebugGenericIndication.DebugVariable;
+            aDataRef[numRefs++] = &aSignal->u.DebugGenericIndication.Dummydataref2;
+            break;
+        default:
+            return 0;
+    }
+    return numRefs;
+}
+
+
+u32 SigGetFilterPos(u16 aSigID)
+{
+    switch (aSigID)
+    {
+        case CSR_MA_PACKET_REQUEST_ID:
+            return 0x00000001;
+        case CSR_MA_PACKET_CONFIRM_ID:
+            return 0x00000002;
+        case CSR_MA_PACKET_INDICATION_ID:
+            return 0x00000004;
+        case CSR_MA_PACKET_CANCEL_REQUEST_ID:
+            return 0x00000008;
+        case CSR_MA_VIF_AVAILABILITY_RESPONSE_ID:
+            return 0x00000010;
+        case CSR_MA_VIF_AVAILABILITY_INDICATION_ID:
+            return 0x00000020;
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            return 0x00000040;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_REQUEST_ID:
+            return 0x00000080;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_RESET_CONFIRM_ID:
+            return 0x00000100;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_REQUEST_ID:
+            return 0x00000200;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_CONFIRM_ID:
+            return 0x00000400;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_REQUEST_ID:
+            return 0x00000800;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CONFIRM_ID:
+            return 0x00001000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_REQUEST_ID:
+            return 0x00002000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+            return 0x00004000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_REQUEST_ID:
+            return 0x00008000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+            return 0x00010001;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_REQUEST_ID:
+            return 0x00010002;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CONFIRM_ID:
+            return 0x00010004;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_REQUEST_ID:
+            return 0x00010008;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+            return 0x00010010;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_REQUEST_ID:
+            return 0x00010020;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+            return 0x00010040;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MEASURE_INDICATION_ID:
+            return 0x00010080;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_REQUEST_ID:
+            return 0x00010100;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+            return 0x00010200;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_REQUEST_ID:
+            return 0x00010400;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+            return 0x00010800;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID:
+            return 0x00011000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            return 0x00012000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SCAN_CANCEL_REQUEST_ID:
+            return 0x00014000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID:
+            return 0x00018000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+            return 0x00020001;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_REQUEST_ID:
+            return 0x00020002;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+            return 0x00020004;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_REQUEST_ID:
+            return 0x00020008;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+            return 0x00020010;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID:
+            return 0x00020020;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+            return 0x00020040;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID:
+            return 0x00020080;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+            return 0x00020100;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_REQUEST_ID:
+            return 0x00020200;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+            return 0x00020400;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_REQUEST_ID:
+            return 0x00020800;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+            return 0x00021000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID:
+            return 0x00022000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+            return 0x00024000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID:
+            return 0x00028000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID:
+            return 0x00030001;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+            return 0x00030002;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID:
+            return 0x00030004;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+            return 0x00030008;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_TRIGGERED_GET_INDICATION_ID:
+            return 0x00030010;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_REQUEST_ID:
+            return 0x00030020;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+            return 0x00030040;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLACKOUT_ENDED_INDICATION_ID:
+            return 0x00030080;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_REQUEST_ID:
+            return 0x00030100;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+            return 0x00030200;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID:
+            return 0x00030400;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+            return 0x00030800;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID:
+            return 0x00031000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+            return 0x00032000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_REQUEST_ID:
+            return 0x00034000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+            return 0x00038000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID:
+            return 0x00040001;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+            return 0x00040002;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_REQUEST_ID:
+            return 0x00040004;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+            return 0x00040008;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_REQUEST_ID:
+            return 0x00040010;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+            return 0x00040020;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_REQUEST_ID:
+            return 0x00040040;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+            return 0x00040080;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_REQUEST_ID:
+            return 0x00040100;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+            return 0x00040200;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_REQUEST_ID:
+            return 0x00040400;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+            return 0x00040800;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_BLOCKACK_ERROR_INDICATION_ID:
+            return 0x00041000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_REQUEST_ID:
+            return 0x00042000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+            return 0x00044000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_REQUEST_ID:
+            return 0x00048000;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SM_START_CONFIRM_ID:
+            return 0x00050001;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_REQUEST_ID:
+            return 0x00050002;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+            return 0x00050004;
+#endif
+        case CSR_MLME_SET_TIM_REQUEST_ID:
+            return 0x00050008;
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            return 0x00050010;
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID:
+            return 0x00050020;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+            return 0x00050040;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_REQUEST_ID:
+            return 0x00050080;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+            return 0x00050100;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID:
+            return 0x00050200;
+#endif
+#ifdef CSR_WIFI_HIP_FULL_SIGNAL_SET
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+            return 0x00050400;
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            return 0x00050800;
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            return 0x00051000;
+        case CSR_DEBUG_GENERIC_REQUEST_ID:
+            return 0x00052000;
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            return 0x00054000;
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            return 0x00058000;
+        default:
+            break;
+    }
+    return 0xffffffff;
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_signals.h b/drivers/staging/csr/csr_wifi_hip_signals.h
new file mode 100644
index 00000000000..5f841556bbe
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_signals.h
@@ -0,0 +1,137 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ *****************************************************************************
+ *
+ * FILE: csr_wifi_hip_signals.h
+ *
+ * PURPOSE:
+ *      Header file wrapping the auto-generated code in csr_wifi_hip_sigs.h
+ *      and csr_wifi_hip_signals.c -
+ *      csr_wifi_hip_sigs.h provides structures defining UniFi signals and
+ *      csr_wifi_hip_signals.c provides SigGetSize() and SigGetDataRefs().
+ *
+ *****************************************************************************
+ */
+#ifndef __CSR_WIFI_HIP_SIGNALS_H__
+#define __CSR_WIFI_HIP_SIGNALS_H__
+
+#include <linux/types.h>
+#include "csr_wifi_hip_sigs.h"
+
+
+/****************************************************************************/
+/* INFORMATION ELEMENTS */
+/****************************************************************************/
+
+/* Information Element ID's - shouldn't be in here, but nowhere better yet */
+#define IE_SSID_ID                       0
+#define IE_SUPPORTED_RATES_ID            1
+#define IE_FH_PARAM_SET_ID               2
+#define IE_DS_PARAM_SET_ID               3
+#define IE_CF_PARAM_SET_ID               4
+#define IE_TIM_ID                        5
+#define IE_IBSS_PARAM_SET_ID             6
+#define IE_COUNTRY_ID                    7
+#define IE_HOPPING_PATTERN_PARAMS_ID     8
+#define IE_HOPPING_PATTERN_TABLE_ID      9
+#define IE_REQUEST_ID                    10
+#define IE_QBSS_LOAD_ID                  11
+#define IE_EDCA_PARAM_SET_ID             12
+#define IE_TRAFFIC_SPEC_ID               13
+#define IE_TRAFFIC_CLASS_ID              14
+#define IE_SCHEDULE_ID                   15
+#define IE_CHALLENGE_TEXT_ID             16
+#define IE_POWER_CONSTRAINT_ID           32
+#define IE_POWER_CAPABILITY_ID           33
+#define IE_TPC_REQUEST_ID                34
+#define IE_TPC_REPORT_ID                 35
+#define IE_SUPPORTED_CHANNELS_ID         36
+#define IE_CHANNEL_SWITCH_ANNOUNCE_ID    37
+#define IE_MEASUREMENT_REQUEST_ID        38
+#define IE_MEASUREMENT_REPORT_ID         39
+#define IE_QUIET_ID                      40
+#define IE_IBSS_DFS_ID                   41
+#define IE_ERP_INFO_ID                   42
+#define IE_TS_DELAY_ID                   43
+#define IE_TCLAS_PROCESSING_ID           44
+#define IE_QOS_CAPABILITY_ID             46
+#define IE_RSN_ID                        48
+#define IE_EXTENDED_SUPPORTED_RATES_ID   50
+#define IE_AP_CHANNEL_REPORT_ID          52
+#define IE_RCPI_ID                       53
+#define IE_WPA_ID                       221
+
+
+/* The maximum number of data references in a signal structure */
+#define UNIFI_MAX_DATA_REFERENCES 2
+
+/* The space to allow for a wire-format signal structure */
+#define UNIFI_PACKED_SIGBUF_SIZE   64
+
+
+/******************************************************************************/
+/* SIGNAL PARAMETER VALUES */
+/******************************************************************************/
+
+/* ifIndex */
+#define UNIFI_IF_2G4 1
+#define UNIFI_IF_5G  2
+
+/* SendProcessId */
+#define HOST_PROC_ID 0xc000
+
+#define SIG_CAP_ESS             0x0001
+#define SIG_CAP_IBSS            0x0002
+#define SIG_CAP_CF_POLLABLE     0x0004
+#define SIG_CAP_CF_POLL_REQUEST 0x0008
+#define SIG_CAP_PRIVACY         0x0010
+#define SIG_CAP_SHORT_PREAMBLE  0x0020
+#define SIG_CAP_DSSSOFDM        0x2000
+
+/******************************************************************************/
+/* FUNCTION DECLARATIONS */
+/******************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+/******************************************************************************
+ * SigGetNumDataRefs - Retrieve pointers to data-refs from a signal.
+ *
+ * PARAMETERS:
+ *   aSignal  - Pointer to signal to retrieve the data refs of.
+ *   aDataRef - Address of a pointer to the structure that the data refs
+ *              pointers will be stored.
+ *
+ * RETURNS:
+ *   The number of data-refs in the signal.
+ */
+s32 SigGetDataRefs(CSR_SIGNAL *aSignal, CSR_DATAREF **aDataRef);
+
+/******************************************************************************
+ * SigGetSize - Retrieve the size (in bytes) of a given signal.
+ *
+ * PARAMETERS:
+ *   aSignal  - Pointer to signal to retrieve size of.
+ *
+ * RETURNS:
+ *   The size (in bytes) of the given signal.
+ */
+s32 SigGetSize(const CSR_SIGNAL *aSignal);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+
+#endif /* __CSR_WIFI_HIP_SIGNALS_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_sigs.h b/drivers/staging/csr/csr_wifi_hip_sigs.h
new file mode 100644
index 00000000000..2b9f51d7f29
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_sigs.h
@@ -0,0 +1,1425 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+
+/* Generated by hip_dd_l_h_gen.pl */
+
+#ifndef CSR_WIFI_HIP_SIGS_H
+#define CSR_WIFI_HIP_SIGS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef s16 csr_place_holding_type;
+
+typedef u16 CSR_ASSOCIATION_ID;
+
+typedef u16 CSR_AUTONOMOUS_SCAN_ID;
+
+typedef u16 CSR_BEACON_PERIODS;
+
+typedef u16 CSR_BLACKOUT_ID;
+
+typedef enum CSR_BLACKOUT_SOURCE
+{
+    CSR_DOT11_LOCAL                               = 0x0000,
+    CSR_DOT11_REMOTE                              = 0x0001,
+    CSR_OTHER_RADIO                               = 0x0002,
+    CSR_NOT_LINKED                                = 0x0004
+} CSR_BLACKOUT_SOURCE;
+
+typedef enum CSR_BLACKOUT_TYPE
+{
+    CSR_LOCAL_DEVICE_ONLY                         = 0x0001,
+    CSR_SPECIFIED_PEER                            = 0x0002,
+    CSR_CURRENT_CHANNEL                           = 0x0004,
+    CSR_P2P                                       = 0x0008
+} CSR_BLACKOUT_TYPE;
+
+typedef enum CSR_BOOT_LOADER_OPERATION
+{
+    CSR_BOOT_LOADER_IDLE                          = 0x00,
+    CSR_BOOT_LOADER_RESTART                       = 0x01,
+    CSR_BOOT_LOADER_PATCH                         = 0x02,
+    CSR_BOOT_LOADER_IMAGE_0                       = 0x10,
+    CSR_BOOT_LOADER_IMAGE_1                       = 0x11,
+    CSR_BOOT_LOADER_IMAGE_2                       = 0x12,
+    CSR_BOOT_LOADER_IMAGE_3                       = 0x13
+} CSR_BOOT_LOADER_OPERATION;
+
+typedef u16 CSR_CAPABILITY_INFORMATION;
+
+typedef u16 CSR_CHANNEL_STARTING_FACTOR;
+
+typedef u32 CSR_CIPHER_SUITE_SELECTOR;
+
+typedef u32 CSR_CLIENT_TAG;
+
+typedef enum CSR_CONNECTION_STATUS
+{
+    CSR_DISCONNECTED                              = 0x0000,
+    CSR_CONNECTED_AWAKE                           = 0x0001
+} CSR_CONNECTION_STATUS;
+
+typedef s16 CSR_DECIBELS;
+
+typedef enum CSR_DIRECTION
+{
+    CSR_TRANSMIT                                  = 0x0000,
+    CSR_RECEIVE                                   = 0x0001,
+    CSR_BIDIRECTIONAL                             = 0x0003
+} CSR_DIRECTION;
+
+typedef enum CSR_FRAME_TYPE
+{
+    CSR_RESERVED                                  = 0x0000,
+    CSR_BEACON                                    = 0x0001,
+    CSR_PROBE_RESPONSE                            = 0x0002,
+    CSR_BEACON_AND_PROBE_RESPONSE                 = 0x0003,
+    CSR_PROBE_REQUEST                             = 0x0004
+} CSR_FRAME_TYPE;
+
+typedef u32 CSR_IPV4_ADDRESS;
+
+typedef enum CSR_IFINTERFACE
+{
+    CSR_INDEX_2G4                                 = 0x0001,
+    CSR_INDEX_5G                                  = 0x0002
+} CSR_IFINTERFACE;
+
+typedef enum CSR_KEY_TYPE
+{
+    CSR_GROUP                                     = 0x0000,
+    CSR_PAIRWISE                                  = 0x0001,
+    CSR_PEER_KEY                                  = 0x0002,
+    CSR_IGTK                                      = 0x0003
+} CSR_KEY_TYPE;
+
+typedef enum CSR_LOADER_OPERATION
+{
+    CSR_LOADER_IDLE                               = 0x0000,
+    CSR_LOADER_COPY                               = 0x0001
+} CSR_LOADER_OPERATION;
+
+typedef struct CSR_MAC_ADDRESS
+{
+    u8 x[6];
+} CSR_MACADDRESS;
+
+typedef enum CSR_MIB_STATUS
+{
+    CSR_MIB_SUCCESSFUL                            = 0x0000,
+    CSR_MIB_INVALID_PARAMETERS                    = 0x0001,
+    CSR_MIB_WRITE_ONLY                            = 0x0002,
+    CSR_MIB_READ_ONLY                             = 0x0003
+} CSR_MIB_STATUS;
+
+typedef enum CSR_MEMORY_SPACE
+{
+    CSR_NONE                                      = 0x00,
+    CSR_SHARED_DATA_MEMORY                        = 0x01,
+    CSR_EXTERNAL_FLASH_MEMORY                     = 0x02,
+    CSR_EXTERNAL_SRAM                             = 0x03,
+    CSR_REGISTERS                                 = 0x04,
+    CSR_PHY_PROCESSOR_DATA_MEMORY                 = 0x10,
+    CSR_PHY_PROCESSOR_PROGRAM_MEMORY              = 0x11,
+    CSR_PHY_PROCESSOR_ROM                         = 0x12,
+    CSR_MAC_PROCESSOR_DATA_MEMORY                 = 0x20,
+    CSR_MAC_PROCESSOR_PROGRAM_MEMORY              = 0x21,
+    CSR_MAC_PROCESSOR_ROM                         = 0x22,
+    CSR_BT_PROCESSOR_DATA_MEMORY                  = 0x30,
+    CSR_BT_PROCESSOR_PROGRAM_MEMORY               = 0x31,
+    CSR_BT_PROCESSOR_ROM                          = 0x32
+} CSR_MEMORY_SPACE;
+
+typedef u16 CSR_MICROSECONDS16;
+
+typedef u32 CSR_MICROSECONDS32;
+
+typedef u16 CSR_NATURAL16;
+
+typedef enum CSR_PS_SCHEME
+{
+    CSR_LEGACY_PS                                 = 0x0001,
+    CSR_U_APSD                                    = 0x0002,
+    CSR_S_APSD                                    = 0x0004
+} CSR_PS_SCHEME;
+
+typedef enum CSR_PACKET_FILTER_MODE
+{
+    CSR_PFM_OPT_OUT                               = 0x0000,
+    CSR_PFM_OPT_IN                                = 0x0003
+} CSR_PACKET_FILTER_MODE;
+
+typedef u16 CSR_PERIODIC_ID;
+
+typedef enum CSR_PERIODIC_SCHEDULING_MODE
+{
+    CSR_PSM_PERIODIC_SCHEDULE_PS_POLL             = 0x0001,
+    CSR_PSM_PERIODIC_SCHEDULE_PM_BIT              = 0x0002,
+    CSR_PSM_PERIODIC_SCHEDULE_UAPSD               = 0x0004,
+    CSR_PSM_PERIODIC_SCHEDULE_SAPSD               = 0x0008
+} CSR_PERIODIC_SCHEDULING_MODE;
+
+typedef enum CSR_POWER_MANAGEMENT_MODE
+{
+    CSR_PMM_ACTIVE_MODE                           = 0x0000,
+    CSR_PMM_POWER_SAVE                            = 0x0001,
+    CSR_PMM_FAST_POWER_SAVE                       = 0x0002
+} CSR_POWER_MANAGEMENT_MODE;
+
+typedef enum CSR_PRIORITY
+{
+    CSR_QOS_UP0                                   = 0x0000,
+    CSR_QOS_UP1                                   = 0x0001,
+    CSR_QOS_UP2                                   = 0x0002,
+    CSR_QOS_UP3                                   = 0x0003,
+    CSR_QOS_UP4                                   = 0x0004,
+    CSR_QOS_UP5                                   = 0x0005,
+    CSR_QOS_UP6                                   = 0x0006,
+    CSR_QOS_UP7                                   = 0x0007,
+    CSR_CONTENTION                                = 0x8000,
+    CSR_MANAGEMENT                                = 0x8010
+} CSR_PRIORITY;
+
+typedef enum CSR_REASON_CODE
+{
+    CSR_UNSPECIFIED_REASON                        = 0x0001,
+    CSR_INVALID_INFORMATION_ELEMENT               = 0x000d,
+    CSR_QOS_UNSPECIFIED_REASON                    = 0x0020,
+    CSR_QOS_EXCESSIVE_NOT_ACK                     = 0x0022,
+    CSR_QOS_TXOP_LIMIT_EXCEEDED                   = 0x0023,
+    CSR_QSTA_LEAVING                              = 0x0024,
+    CSR_UNKNOWN_BA                                = 0x0026,
+    CSR_UNKNOWN_TS                                = 0x0026,
+    CSR_TIMEOUT                                   = 0x0027
+} CSR_REASON_CODE;
+
+typedef enum CSR_RECEPTION_STATUS
+{
+    CSR_RX_SUCCESS                                = 0x0000,
+    CSR_RX_FAILURE_UNSPECIFIED                    = 0x0001,
+    CSR_MICHAEL_MIC_ERROR                         = 0x0002,
+    CSR_DECRYPTION_ERROR                          = 0x0003,
+    CSR_NO_TEMPORAL_KEY_AVAILABLE                 = 0x0004,
+    CSR_UNSUPPORTED_MODULATION                    = 0x0011,
+    CSR_BAD_FCS                                   = 0x0012,
+    CSR_BAD_SIGNAL                                = 0x0013
+} CSR_RECEPTION_STATUS;
+
+typedef enum CSR_RESULT_CODE
+{
+    CSR_RC_SUCCESS                                = 0x0000,
+    CSR_RC_UNSPECIFIED_FAILURE                    = 0x0001,
+    CSR_RC_REFUSED                                = 0x0003,
+    CSR_RC_INVALID_PARAMETERS                     = 0x0026,
+    CSR_RC_REJECTED_INVALID_IE                    = 0x0028,
+    CSR_RC_REJECTED_INVALID_GROUP_CIPHER          = 0x0029,
+    CSR_RC_REJECTED_INVALID_PAIRWISE_CIPHER       = 0x002a,
+    CSR_RC_TIMEOUT                                = 0x8000,
+    CSR_RC_TOO_MANY_SIMULTANEOUS_REQUESTS         = 0x8001,
+    CSR_RC_BSS_ALREADY_STARTED_OR_JOINED          = 0x8002,
+    CSR_RC_NOT_SUPPORTED                          = 0x8003,
+    CSR_RC_TRANSMISSION_FAILURE                   = 0x8004,
+    CSR_RC_RESET_REQUIRED_BEFORE_START            = 0x8006,
+    CSR_RC_INSUFFICIENT_RESOURCE                  = 0x8007,
+    CSR_RC_NO_BUFFERED_BROADCAST_MULTICAST_FRAMES = 0x8008,
+    CSR_RC_INVALID_UNICAST_CIPHER                 = 0xf02f,
+    CSR_RC_INVALID_MULTICAST_CIPHER               = 0xf030
+} CSR_RESULT_CODE;
+
+typedef enum CSR_SCAN_TYPE
+{
+    CSR_SC_ACTIVE_SCAN                            = 0x0000,
+    CSR_SC_PASSIVE_SCAN                           = 0x0001
+} CSR_SCAN_TYPE;
+
+typedef enum CSR_SIGNAL_ID
+{
+    CSR_MA_PACKET_REQUEST_ID                      = 0x0110,
+    CSR_MA_PACKET_CONFIRM_ID                      = 0x0111,
+    CSR_MA_PACKET_INDICATION_ID                   = 0x0113,
+    CSR_MA_PACKET_CANCEL_REQUEST_ID               = 0x0114,
+    CSR_MA_VIF_AVAILABILITY_RESPONSE_ID           = 0x0116,
+    CSR_MA_VIF_AVAILABILITY_INDICATION_ID         = 0x0117,
+    CSR_MA_PACKET_ERROR_INDICATION_ID             = 0x011b,
+    CSR_MLME_RESET_REQUEST_ID                     = 0x0200,
+    CSR_MLME_RESET_CONFIRM_ID                     = 0x0201,
+    CSR_MLME_GET_REQUEST_ID                       = 0x0204,
+    CSR_MLME_GET_CONFIRM_ID                       = 0x0205,
+    CSR_MLME_SET_REQUEST_ID                       = 0x0208,
+    CSR_MLME_SET_CONFIRM_ID                       = 0x0209,
+    CSR_MLME_GET_NEXT_REQUEST_ID                  = 0x020c,
+    CSR_MLME_GET_NEXT_CONFIRM_ID                  = 0x020d,
+    CSR_MLME_POWERMGT_REQUEST_ID                  = 0x0210,
+    CSR_MLME_POWERMGT_CONFIRM_ID                  = 0x0211,
+    CSR_MLME_SCAN_REQUEST_ID                      = 0x0214,
+    CSR_MLME_SCAN_CONFIRM_ID                      = 0x0215,
+    CSR_MLME_HL_SYNC_REQUEST_ID                   = 0x0244,
+    CSR_MLME_HL_SYNC_CONFIRM_ID                   = 0x0245,
+    CSR_MLME_MEASURE_REQUEST_ID                   = 0x0258,
+    CSR_MLME_MEASURE_CONFIRM_ID                   = 0x0259,
+    CSR_MLME_MEASURE_INDICATION_ID                = 0x025b,
+    CSR_MLME_SETKEYS_REQUEST_ID                   = 0x0268,
+    CSR_MLME_SETKEYS_CONFIRM_ID                   = 0x0269,
+    CSR_MLME_DELETEKEYS_REQUEST_ID                = 0x026c,
+    CSR_MLME_DELETEKEYS_CONFIRM_ID                = 0x026d,
+    CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION_ID   = 0x0287,
+    CSR_MLME_CONNECTED_INDICATION_ID              = 0x028b,
+    CSR_MLME_SCAN_CANCEL_REQUEST_ID               = 0x028c,
+    CSR_MLME_HL_SYNC_CANCEL_REQUEST_ID            = 0x0298,
+    CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID            = 0x0299,
+    CSR_MLME_ADD_PERIODIC_REQUEST_ID              = 0x02a0,
+    CSR_MLME_ADD_PERIODIC_CONFIRM_ID              = 0x02a1,
+    CSR_MLME_DEL_PERIODIC_REQUEST_ID              = 0x02a4,
+    CSR_MLME_DEL_PERIODIC_CONFIRM_ID              = 0x02a5,
+    CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST_ID       = 0x02a8,
+    CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID       = 0x02a9,
+    CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST_ID       = 0x02ac,
+    CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID       = 0x02ad,
+    CSR_MLME_SET_PACKET_FILTER_REQUEST_ID         = 0x02b8,
+    CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID         = 0x02b9,
+    CSR_MLME_STOP_MEASURE_REQUEST_ID              = 0x02bc,
+    CSR_MLME_STOP_MEASURE_CONFIRM_ID              = 0x02bd,
+    CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST_ID     = 0x02cc,
+    CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID     = 0x02cd,
+    CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION_ID   = 0x02db,
+    CSR_MLME_ADD_TRIGGERED_GET_REQUEST_ID         = 0x02dc,
+    CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID         = 0x02dd,
+    CSR_MLME_DEL_TRIGGERED_GET_REQUEST_ID         = 0x02e0,
+    CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID         = 0x02e1,
+    CSR_MLME_TRIGGERED_GET_INDICATION_ID          = 0x02e7,
+    CSR_MLME_ADD_BLACKOUT_REQUEST_ID              = 0x02f8,
+    CSR_MLME_ADD_BLACKOUT_CONFIRM_ID              = 0x02f9,
+    CSR_MLME_BLACKOUT_ENDED_INDICATION_ID         = 0x02fb,
+    CSR_MLME_DEL_BLACKOUT_REQUEST_ID              = 0x02fc,
+    CSR_MLME_DEL_BLACKOUT_CONFIRM_ID              = 0x02fd,
+    CSR_MLME_ADD_RX_TRIGGER_REQUEST_ID            = 0x0304,
+    CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID            = 0x0305,
+    CSR_MLME_DEL_RX_TRIGGER_REQUEST_ID            = 0x0308,
+    CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID            = 0x0309,
+    CSR_MLME_CONNECT_STATUS_REQUEST_ID            = 0x0310,
+    CSR_MLME_CONNECT_STATUS_CONFIRM_ID            = 0x0311,
+    CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST_ID      = 0x0314,
+    CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID      = 0x0315,
+    CSR_MLME_ADD_TEMPLATE_REQUEST_ID              = 0x0318,
+    CSR_MLME_ADD_TEMPLATE_CONFIRM_ID              = 0x0319,
+    CSR_MLME_CONFIG_QUEUE_REQUEST_ID              = 0x031c,
+    CSR_MLME_CONFIG_QUEUE_CONFIRM_ID              = 0x031d,
+    CSR_MLME_ADD_TSPEC_REQUEST_ID                 = 0x0320,
+    CSR_MLME_ADD_TSPEC_CONFIRM_ID                 = 0x0321,
+    CSR_MLME_DEL_TSPEC_REQUEST_ID                 = 0x0324,
+    CSR_MLME_DEL_TSPEC_CONFIRM_ID                 = 0x0325,
+    CSR_MLME_START_AGGREGATION_REQUEST_ID         = 0x0328,
+    CSR_MLME_START_AGGREGATION_CONFIRM_ID         = 0x0329,
+    CSR_MLME_BLOCKACK_ERROR_INDICATION_ID         = 0x032b,
+    CSR_MLME_STOP_AGGREGATION_REQUEST_ID          = 0x032c,
+    CSR_MLME_STOP_AGGREGATION_CONFIRM_ID          = 0x032d,
+    CSR_MLME_SM_START_REQUEST_ID                  = 0x0334,
+    CSR_MLME_SM_START_CONFIRM_ID                  = 0x0335,
+    CSR_MLME_LEAVE_REQUEST_ID                     = 0x0338,
+    CSR_MLME_LEAVE_CONFIRM_ID                     = 0x0339,
+    CSR_MLME_SET_TIM_REQUEST_ID                   = 0x033c,
+    CSR_MLME_SET_TIM_CONFIRM_ID                   = 0x033d,
+    CSR_MLME_GET_KEY_SEQUENCE_REQUEST_ID          = 0x0340,
+    CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID          = 0x0341,
+    CSR_MLME_SET_CHANNEL_REQUEST_ID               = 0x034c,
+    CSR_MLME_SET_CHANNEL_CONFIRM_ID               = 0x034d,
+    CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST_ID     = 0x040c,
+    CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID     = 0x040d,
+    CSR_DEBUG_STRING_INDICATION_ID                = 0x0803,
+    CSR_DEBUG_WORD16_INDICATION_ID                = 0x0807,
+    CSR_DEBUG_GENERIC_REQUEST_ID                  = 0x0808,
+    CSR_DEBUG_GENERIC_CONFIRM_ID                  = 0x0809,
+    CSR_DEBUG_GENERIC_INDICATION_ID               = 0x080b
+} CSR_SIGNAL_ID;
+
+typedef u16 CSR_SIMPLE_POINTER;
+
+typedef u16 CSR_STARTING_SEQUENCE_NUMBER;
+
+typedef enum CSR_SYMBOL_ID
+{
+    CSR_SLT_END                                   = 0x0000,
+    CSR_SLT_PCI_SLOT_CONFIG                       = 0x0001,
+    CSR_SLT_SDIO_SLOT_CONFIG                      = 0x0002,
+    CSR_SLT_BUILD_ID_NUMBER                       = 0x0003,
+    CSR_SLT_BUILD_ID_STRING                       = 0x0004,
+    CSR_SLT_PERSISTENT_STORE_DB                   = 0x0005,
+    CSR_SLT_RESET_VECTOR_PHY                      = 0x0006,
+    CSR_SLT_RESET_VECTOR_MAC                      = 0x0007,
+    CSR_SLT_SDIO_LOADER_CONTROL                   = 0x0008,
+    CSR_SLT_TEST_CMD                              = 0x0009,
+    CSR_SLT_TEST_ALIVE_COUNTER                    = 0x000a,
+    CSR_SLT_TEST_PARAMETERS                       = 0x000b,
+    CSR_SLT_TEST_RESULTS                          = 0x000c,
+    CSR_SLT_TEST_VERSION                          = 0x000d,
+    CSR_SLT_MIB_PSID_RANGES                       = 0x000e,
+    CSR_SLT_KIP_TABLE                             = 0x000f,
+    CSR_SLT_PANIC_DATA_PHY                        = 0x0010,
+    CSR_SLT_PANIC_DATA_MAC                        = 0x0011,
+    CSR_SLT_BOOT_LOADER_CONTROL                   = 0x0012,
+    CSR_SLT_SOFT_MAC                              = 0x0013
+} CSR_SYMBOL_ID;
+
+typedef struct CSR_TSF_TIME
+{
+    u8 x[8];
+} CSR_TSF_TIME;
+
+typedef u16 CSR_TIME_UNITS;
+
+typedef enum CSR_TRANSMISSION_CONTROL
+{
+    CSR_TRIGGERED                                 = 0x0001,
+    CSR_END_OF_SERVICE                            = 0x0002,
+    CSR_NO_CONFIRM_REQUIRED                       = 0x0004,
+    CSR_ALLOW_BA                                  = 0x0008
+} CSR_TRANSMISSION_CONTROL;
+
+typedef enum CSR_TRANSMISSION_STATUS
+{
+    CSR_TX_SUCCESSFUL                             = 0x0000,
+    CSR_TX_RETRY_LIMIT                            = 0x0001,
+    CSR_TX_LIFETIME                               = 0x0002,
+    CSR_TX_NO_BSS                                 = 0x0003,
+    CSR_TX_EXCESSIVE_DATA_LENGTH                  = 0x0004,
+    CSR_TX_UNSUPPORTED_PRIORITY                   = 0x0006,
+    CSR_TX_UNAVAILABLE_PRIORITY                   = 0x0007,
+    CSR_TX_UNAVAILABLE_KEY_MAPPING                = 0x000a,
+    CSR_TX_EDCA_TIMEOUT                           = 0x000b,
+    CSR_TX_BLOCK_ACK_TIMEOUT                      = 0x000c,
+    CSR_TX_FAIL_TRANSMISSION_VIF_INTERRUPTED      = 0x000d,
+    CSR_TX_REJECTED_PEER_STATION_SLEEPING         = 0x000e,
+    CSR_TX_REJECTED_DTIM_ENDED                    = 0x000f,
+    CSR_TX_REJECTED_DTIM_STARTED                  = 0x0010
+} CSR_TRANSMISSION_STATUS;
+
+typedef u16 CSR_TRIGGER_ID;
+
+typedef u16 CSR_TRIGGERED_ID;
+
+typedef enum CSR_HIP_VERSIONS
+{
+    CSR_HIP_ENG_VERSION                           = 0x0001,
+    CSR_HIP_VERSION                               = 0x0900
+} CSR_HIP_VERSIONS;
+
+typedef u16 CSR_BUFFER_HANDLE;
+
+typedef u16 CSR_CHANNEL_NUMBER;
+
+typedef struct CSR_DATA_REFERENCE
+{
+    u16 SlotNumber;
+    u16 DataLength;
+} CSR_DATAREF;
+
+typedef u16 CSR_DIALOG_TOKEN;
+
+typedef struct CSR_GENERIC_POINTER
+{
+    u32        MemoryOffset;
+    CSR_MEMORY_SPACE MemorySpace;
+} CSR_GENERIC_POINTER;
+
+typedef struct CSR_MLME_CONFIG_QUEUE_CONFIRM
+{
+    CSR_DATAREF     Dummydataref1;
+    CSR_DATAREF     Dummydataref2;
+    CSR_RESULT_CODE ResultCode;
+} CSR_MLME_CONFIG_QUEUE_CONFIRM;
+
+typedef struct CSR_MLME_CONFIG_QUEUE_REQUEST
+{
+    CSR_DATAREF   Dummydataref1;
+    CSR_DATAREF   Dummydataref2;
+    CSR_NATURAL16 QueueIndex;
+    CSR_NATURAL16 Aifs;
+    CSR_NATURAL16 Cwmin;
+    CSR_NATURAL16 Cwmax;
+    CSR_NATURAL16 TxopLimit;
+} CSR_MLME_CONFIG_QUEUE_REQUEST;
+
+typedef struct CSR_MLME_GET_CONFIRM
+{
+    CSR_DATAREF    MibAttributeValue;
+    CSR_DATAREF    Dummydataref2;
+    CSR_MIB_STATUS Status;
+    CSR_NATURAL16  ErrorIndex;
+} CSR_MLME_GET_CONFIRM;
+
+typedef struct CSR_MLME_GET_REQUEST
+{
+    CSR_DATAREF MibAttribute;
+    CSR_DATAREF Dummydataref2;
+} CSR_MLME_GET_REQUEST;
+
+typedef struct CSR_MLME_GET_NEXT_CONFIRM
+{
+    CSR_DATAREF    MibAttributeValue;
+    CSR_DATAREF    Dummydataref2;
+    CSR_MIB_STATUS Status;
+    CSR_NATURAL16  ErrorIndex;
+} CSR_MLME_GET_NEXT_CONFIRM;
+
+typedef struct CSR_MLME_GET_NEXT_REQUEST
+{
+    CSR_DATAREF MibAttribute;
+    CSR_DATAREF Dummydataref2;
+} CSR_MLME_GET_NEXT_REQUEST;
+
+typedef struct CSR_MLME_HL_SYNC_CONFIRM
+{
+    CSR_DATAREF     Dummydataref1;
+    CSR_DATAREF     Dummydataref2;
+    CSR_MACADDRESS  GroupAddress;
+    CSR_RESULT_CODE ResultCode;
+} CSR_MLME_HL_SYNC_CONFIRM;
+
+typedef struct CSR_MLME_HL_SYNC_REQUEST
+{
+    CSR_DATAREF    Dummydataref1;
+    CSR_DATAREF    Dummydataref2;
+    CSR_MACADDRESS GroupAddress;
+} CSR_MLME_HL_SYNC_REQUEST;
+
+typedef struct CSR_MLME_HL_SYNC_CANCEL_CONFIRM
+{
+    CSR_DATAREF     Dummydataref1;
+    CSR_DATAREF     Dummydataref2;
+    CSR_RESULT_CODE ResultCode;
+} CSR_MLME_HL_SYNC_CANCEL_CONFIRM;
+
+typedef struct CSR_MLME_HL_SYNC_CANCEL_REQUEST
+{
+    CSR_DATAREF    Dummydataref1;
+    CSR_DATAREF    Dummydataref2;
+    CSR_MACADDRESS GroupAddress;
+} CSR_MLME_HL_SYNC_CANCEL_REQUEST;
+
+typedef struct CSR_MLME_MEASURE_CONFIRM
+{
+    CSR_DATAREF      Dummydataref1;
+    CSR_DATAREF      Dummydataref2;
+    CSR_RESULT_CODE  ResultCode;
+    CSR_DIALOG_TOKEN DialogToken;
+} CSR_MLME_MEASURE_CONFIRM;
+
+typedef struct CSR_MLME_MEASURE_INDICATION
+{
+    CSR_DATAREF      MeasurementReportSet;
+    CSR_DATAREF      Dummydataref2;
+    CSR_DIALOG_TOKEN DialogToken;
+} CSR_MLME_MEASURE_INDICATION;
+
+typedef struct CSR_MLME_MEASURE_REQUEST
+{
+    CSR_DATAREF      MeasurementRequestSet;
+    CSR_DATAREF      Dummydataref2;
+    CSR_DIALOG_TOKEN DialogToken;
+} CSR_MLME_MEASURE_REQUEST;
+
+typedef struct CSR_MLME_RESET_CONFIRM
+{
+    CSR_DATAREF     Dummydataref1;
+    CSR_DATAREF     Dummydataref2;
+    CSR_RESULT_CODE ResultCode;
+} CSR_MLME_RESET_CONFIRM;
+
+typedef struct CSR_MLME_RESET_REQUEST
+{
+    CSR_DATAREF    Dummydataref1;
+    CSR_DATAREF    Dummydataref2;
+    CSR_MACADDRESS StaAddress;
+    s16       SetDefaultMib;
+} CSR_MLME_RESET_REQUEST;
+
+typedef struct CSR_MLME_SET_CONFIRM
+{
+    CSR_DATAREF    MibAttributeValue;
+    CSR_DATAREF    Dummydataref2;
+    CSR_MIB_STATUS Status;
+    CSR_NATURAL16  ErrorIndex;
+} CSR_MLME_SET_CONFIRM;
+
+typedef struct CSR_MLME_SET_REQUEST
+{
+    CSR_DATAREF MibAttributeValue;
+    CSR_DATAREF Dummydataref2;
+} CSR_MLME_SET_REQUEST;
+
+typedef struct CSR_MLME_STOP_MEASURE_CONFIRM
+{
+    CSR_DATAREF      Dummydataref1;
+    CSR_DATAREF      Dummydataref2;
+    CSR_RESULT_CODE  ResultCode;
+    CSR_DIALOG_TOKEN DialogToken;
+} CSR_MLME_STOP_MEASURE_CONFIRM;
+
+typedef struct CSR_MLME_STOP_MEASURE_REQUEST
+{
+    CSR_DATAREF      Dummydataref1;
+    CSR_DATAREF      Dummydataref2;
+    CSR_DIALOG_TOKEN DialogToken;
+} CSR_MLME_STOP_MEASURE_REQUEST;
+
+typedef u16 CSR_PROCESS_ID;
+
+typedef u16 CSR_RATE;
+
+typedef u16 CSR_SEQUENCE_NUMBER;
+
+typedef struct CSR_SIGNAL_PRIMITIVE_HEADER
+{
+    s16       SignalId;
+    CSR_PROCESS_ID ReceiverProcessId;
+    CSR_PROCESS_ID SenderProcessId;
+} CSR_SIGNAL_PRIMITIVE_HEADER;
+
+typedef u16 CSR_TRAFFIC_WINDOW;
+
+typedef u16 CSR_VIF_IDENTIFIER;
+
+typedef struct CSR_DEBUG_GENERIC_CONFIRM
+{
+    CSR_DATAREF   DebugVariable;
+    CSR_DATAREF   Dummydataref2;
+    CSR_NATURAL16 DebugWords[8];
+} CSR_DEBUG_GENERIC_CONFIRM;
+
+typedef struct CSR_DEBUG_GENERIC_INDICATION
+{
+    CSR_DATAREF   DebugVariable;
+    CSR_DATAREF   Dummydataref2;
+    CSR_NATURAL16 DebugWords[8];
+} CSR_DEBUG_GENERIC_INDICATION;
+
+typedef struct CSR_DEBUG_GENERIC_REQUEST
+{
+    CSR_DATAREF   DebugVariable;
+    CSR_DATAREF   Dummydataref2;
+    CSR_NATURAL16 DebugWords[8];
+} CSR_DEBUG_GENERIC_REQUEST;
+
+typedef struct CSR_DEBUG_STRING_INDICATION
+{
+    CSR_DATAREF DebugMessage;
+    CSR_DATAREF Dummydataref2;
+} CSR_DEBUG_STRING_INDICATION;
+
+typedef struct CSR_DEBUG_WORD16_INDICATION
+{
+    CSR_DATAREF   Dummydataref1;
+    CSR_DATAREF   Dummydataref2;
+    CSR_NATURAL16 DebugWords[16];
+} CSR_DEBUG_WORD16_INDICATION;
+
+typedef struct CSR_MA_PACKET_CONFIRM
+{
+    CSR_DATAREF             Dummydataref1;
+    CSR_DATAREF             Dummydataref2;
+    CSR_VIF_IDENTIFIER      VirtualInterfaceIdentifier;
+    CSR_TRANSMISSION_STATUS TransmissionStatus;
+    CSR_NATURAL16           RetryCount;
+    CSR_RATE                Rate;
+    CSR_CLIENT_TAG          HostTag;
+} CSR_MA_PACKET_CONFIRM;
+
+typedef struct CSR_MA_PACKET_INDICATION
+{
+    CSR_DATAREF          Data;
+    CSR_DATAREF          Dummydataref2;
+    CSR_VIF_IDENTIFIER   VirtualInterfaceIdentifier;
+    CSR_TSF_TIME         LocalTime;
+    CSR_IFINTERFACE      Ifindex;
+    CSR_CHANNEL_NUMBER   Channel;
+    CSR_RECEPTION_STATUS ReceptionStatus;
+    CSR_DECIBELS         Rssi;
+    CSR_DECIBELS         Snr;
+    CSR_RATE             ReceivedRate;
+} CSR_MA_PACKET_INDICATION;
+
+typedef struct CSR_MA_PACKET_REQUEST
+{
+    CSR_DATAREF              Data;
+    CSR_DATAREF              Dummydataref2;
+    CSR_VIF_IDENTIFIER       VirtualInterfaceIdentifier;
+    CSR_RATE                 TransmitRate;
+    CSR_CLIENT_TAG           HostTag;
+    CSR_PRIORITY             Priority;
+    CSR_MACADDRESS           Ra;
+    CSR_TRANSMISSION_CONTROL TransmissionControl;
+} CSR_MA_PACKET_REQUEST;
+
+typedef struct CSR_MA_PACKET_CANCEL_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_CLIENT_TAG     HostTag;
+} CSR_MA_PACKET_CANCEL_REQUEST;
+
+typedef struct CSR_MA_PACKET_ERROR_INDICATION
+{
+    CSR_DATAREF         Dummydataref1;
+    CSR_DATAREF         Dummydataref2;
+    CSR_VIF_IDENTIFIER  VirtualInterfaceIdentifier;
+    CSR_MACADDRESS      PeerQstaAddress;
+    CSR_PRIORITY        UserPriority;
+    CSR_SEQUENCE_NUMBER SequenceNumber;
+} CSR_MA_PACKET_ERROR_INDICATION;
+
+typedef struct CSR_MA_VIF_AVAILABILITY_INDICATION
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    s16           Multicast;
+} CSR_MA_VIF_AVAILABILITY_INDICATION;
+
+typedef struct CSR_MA_VIF_AVAILABILITY_RESPONSE
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MA_VIF_AVAILABILITY_RESPONSE;
+
+typedef struct CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM
+{
+    CSR_DATAREF            Dummydataref1;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE        ResultCode;
+    CSR_AUTONOMOUS_SCAN_ID AutonomousScanId;
+} CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM;
+
+typedef struct CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST
+{
+    CSR_DATAREF                 ChannelList;
+    CSR_DATAREF                 InformationElements;
+    CSR_VIF_IDENTIFIER          VirtualInterfaceIdentifier;
+    CSR_AUTONOMOUS_SCAN_ID      AutonomousScanId;
+    CSR_IFINTERFACE             Ifindex;
+    CSR_CHANNEL_STARTING_FACTOR ChannelStartingFactor;
+    CSR_SCAN_TYPE               ScanType;
+    CSR_MICROSECONDS32          ProbeDelay;
+    CSR_TIME_UNITS              MinChannelTime;
+    CSR_TIME_UNITS              MaxChannelTime;
+} CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST;
+
+typedef struct CSR_MLME_ADD_BLACKOUT_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_BLACKOUT_ID    BlackoutId;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_ADD_BLACKOUT_CONFIRM;
+
+typedef struct CSR_MLME_ADD_BLACKOUT_REQUEST
+{
+    CSR_DATAREF         Dummydataref1;
+    CSR_DATAREF         Dummydataref2;
+    CSR_VIF_IDENTIFIER  VirtualInterfaceIdentifier;
+    CSR_BLACKOUT_ID     BlackoutId;
+    CSR_BLACKOUT_TYPE   BlackoutType;
+    CSR_BLACKOUT_SOURCE BlackoutSource;
+    CSR_MICROSECONDS32  BlackoutStartReference;
+    CSR_MICROSECONDS32  BlackoutPeriod;
+    CSR_MICROSECONDS32  BlackoutDuration;
+    CSR_MACADDRESS      PeerStaAddress;
+    CSR_NATURAL16       BlackoutCount;
+} CSR_MLME_ADD_BLACKOUT_REQUEST;
+
+typedef struct CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM;
+
+typedef struct CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST
+{
+    CSR_DATAREF        Data;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_NATURAL16      NumberOfMulticastGroupAddresses;
+} CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST;
+
+typedef struct CSR_MLME_ADD_PERIODIC_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PERIODIC_ID    PeriodicId;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_ADD_PERIODIC_CONFIRM;
+
+typedef struct CSR_MLME_ADD_PERIODIC_REQUEST
+{
+    CSR_DATAREF                  Dummydataref1;
+    CSR_DATAREF                  Dummydataref2;
+    CSR_VIF_IDENTIFIER           VirtualInterfaceIdentifier;
+    CSR_PERIODIC_ID              PeriodicId;
+    CSR_MICROSECONDS32           MaximumLatency;
+    CSR_PERIODIC_SCHEDULING_MODE PeriodicSchedulingMode;
+    s16                     WakeHost;
+    CSR_PRIORITY                 UserPriority;
+} CSR_MLME_ADD_PERIODIC_REQUEST;
+
+typedef struct CSR_MLME_ADD_RX_TRIGGER_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_TRIGGER_ID     TriggerId;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_ADD_RX_TRIGGER_CONFIRM;
+
+typedef struct CSR_MLME_ADD_RX_TRIGGER_REQUEST
+{
+    CSR_DATAREF        InformationElements;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_TRIGGER_ID     TriggerId;
+    CSR_PRIORITY       Priority;
+} CSR_MLME_ADD_RX_TRIGGER_REQUEST;
+
+typedef struct CSR_MLME_ADD_TEMPLATE_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_FRAME_TYPE     FrameType;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_ADD_TEMPLATE_CONFIRM;
+
+typedef struct CSR_MLME_ADD_TEMPLATE_REQUEST
+{
+    CSR_DATAREF        Data1;
+    CSR_DATAREF        Data2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_FRAME_TYPE     FrameType;
+    CSR_RATE           MinTransmitRate;
+} CSR_MLME_ADD_TEMPLATE_REQUEST;
+
+typedef struct CSR_MLME_ADD_TRIGGERED_GET_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+    CSR_TRIGGERED_ID   TriggeredId;
+} CSR_MLME_ADD_TRIGGERED_GET_CONFIRM;
+
+typedef struct CSR_MLME_ADD_TRIGGERED_GET_REQUEST
+{
+    CSR_DATAREF        MibAttribute;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_TRIGGERED_ID   TriggeredId;
+} CSR_MLME_ADD_TRIGGERED_GET_REQUEST;
+
+typedef struct CSR_MLME_ADD_TSPEC_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PRIORITY       UserPriority;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_ADD_TSPEC_CONFIRM;
+
+typedef struct CSR_MLME_ADD_TSPEC_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PRIORITY       UserPriority;
+    CSR_DIRECTION      Direction;
+    CSR_PS_SCHEME      PsScheme;
+    CSR_NATURAL16      MediumTime;
+    CSR_MICROSECONDS32 ServiceStartTime;
+    CSR_MICROSECONDS32 ServiceInterval;
+    CSR_RATE           MinimumDataRate;
+} CSR_MLME_ADD_TSPEC_REQUEST;
+
+typedef struct CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION
+{
+    CSR_DATAREF            Dummydataref1;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE        ResultCode;
+    CSR_AUTONOMOUS_SCAN_ID AutonomousScanId;
+} CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION;
+
+typedef struct CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_MACADDRESS     Bssid;
+} CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION;
+
+typedef struct CSR_MLME_BLACKOUT_ENDED_INDICATION
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_BLACKOUT_ID    BlackoutId;
+} CSR_MLME_BLACKOUT_ENDED_INDICATION;
+
+typedef struct CSR_MLME_BLOCKACK_ERROR_INDICATION
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_REASON_CODE    ResultCode;
+    CSR_MACADDRESS     PeerQstaAddress;
+} CSR_MLME_BLOCKACK_ERROR_INDICATION;
+
+typedef struct CSR_MLME_CONNECTED_INDICATION
+{
+    CSR_DATAREF           Dummydataref1;
+    CSR_DATAREF           Dummydataref2;
+    CSR_VIF_IDENTIFIER    VirtualInterfaceIdentifier;
+    CSR_CONNECTION_STATUS ConnectionStatus;
+    CSR_MACADDRESS        PeerMacAddress;
+} CSR_MLME_CONNECTED_INDICATION;
+
+typedef struct CSR_MLME_CONNECT_STATUS_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_CONNECT_STATUS_CONFIRM;
+
+typedef struct CSR_MLME_CONNECT_STATUS_REQUEST
+{
+    CSR_DATAREF                InformationElements;
+    CSR_DATAREF                Dummydataref2;
+    CSR_VIF_IDENTIFIER         VirtualInterfaceIdentifier;
+    CSR_CONNECTION_STATUS      ConnectionStatus;
+    CSR_MACADDRESS             StaAddress;
+    CSR_ASSOCIATION_ID         AssociationId;
+    CSR_CAPABILITY_INFORMATION AssociationCapabilityInformation;
+} CSR_MLME_CONNECT_STATUS_REQUEST;
+
+typedef struct CSR_MLME_DELETEKEYS_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_DELETEKEYS_CONFIRM;
+
+typedef struct CSR_MLME_DELETEKEYS_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_NATURAL16      KeyId;
+    CSR_KEY_TYPE       KeyType;
+    CSR_MACADDRESS     Address;
+} CSR_MLME_DELETEKEYS_REQUEST;
+
+typedef struct CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM
+{
+    CSR_DATAREF            Dummydataref1;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE        ResultCode;
+    CSR_AUTONOMOUS_SCAN_ID AutonomousScanId;
+} CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM;
+
+typedef struct CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST
+{
+    CSR_DATAREF            Dummydataref1;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_AUTONOMOUS_SCAN_ID AutonomousScanId;
+} CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST;
+
+typedef struct CSR_MLME_DEL_BLACKOUT_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_BLACKOUT_ID    BlackoutId;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_DEL_BLACKOUT_CONFIRM;
+
+typedef struct CSR_MLME_DEL_BLACKOUT_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_BLACKOUT_ID    BlackoutId;
+} CSR_MLME_DEL_BLACKOUT_REQUEST;
+
+typedef struct CSR_MLME_DEL_PERIODIC_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PERIODIC_ID    PeriodicId;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_DEL_PERIODIC_CONFIRM;
+
+typedef struct CSR_MLME_DEL_PERIODIC_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PERIODIC_ID    PeriodicId;
+} CSR_MLME_DEL_PERIODIC_REQUEST;
+
+typedef struct CSR_MLME_DEL_RX_TRIGGER_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_TRIGGER_ID     TriggerId;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_DEL_RX_TRIGGER_CONFIRM;
+
+typedef struct CSR_MLME_DEL_RX_TRIGGER_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_TRIGGER_ID     TriggerId;
+} CSR_MLME_DEL_RX_TRIGGER_REQUEST;
+
+typedef struct CSR_MLME_DEL_TRIGGERED_GET_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+    CSR_TRIGGERED_ID   TriggeredId;
+} CSR_MLME_DEL_TRIGGERED_GET_CONFIRM;
+
+typedef struct CSR_MLME_DEL_TRIGGERED_GET_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_TRIGGERED_ID   TriggeredId;
+} CSR_MLME_DEL_TRIGGERED_GET_REQUEST;
+
+typedef struct CSR_MLME_DEL_TSPEC_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PRIORITY       UserPriority;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_DEL_TSPEC_CONFIRM;
+
+typedef struct CSR_MLME_DEL_TSPEC_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_PRIORITY       UserPriority;
+    CSR_DIRECTION      Direction;
+} CSR_MLME_DEL_TSPEC_REQUEST;
+
+typedef struct CSR_MLME_GET_KEY_SEQUENCE_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+    CSR_NATURAL16      SequenceNumber[8];
+} CSR_MLME_GET_KEY_SEQUENCE_CONFIRM;
+
+typedef struct CSR_MLME_GET_KEY_SEQUENCE_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_NATURAL16      KeyId;
+    CSR_KEY_TYPE       KeyType;
+    CSR_MACADDRESS     Address;
+} CSR_MLME_GET_KEY_SEQUENCE_REQUEST;
+
+typedef struct CSR_MLME_LEAVE_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_LEAVE_CONFIRM;
+
+typedef struct CSR_MLME_LEAVE_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+} CSR_MLME_LEAVE_REQUEST;
+
+typedef struct CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM;
+
+typedef struct CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST
+{
+    CSR_DATAREF                Data;
+    CSR_DATAREF                Dummydataref2;
+    CSR_VIF_IDENTIFIER         VirtualInterfaceIdentifier;
+    CSR_TIME_UNITS             BeaconPeriod;
+    CSR_BEACON_PERIODS         DtimPeriod;
+    CSR_CAPABILITY_INFORMATION CapabilityInformation;
+    CSR_MACADDRESS             Bssid;
+    CSR_NATURAL16              RtsThreshold;
+} CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST;
+
+typedef struct CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM
+{
+    CSR_DATAREF            Dummydataref1;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE        ResultCode;
+    CSR_AUTONOMOUS_SCAN_ID AutonomousScanId;
+} CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM;
+
+typedef struct CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST
+{
+    CSR_DATAREF            Dummydataref1;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_AUTONOMOUS_SCAN_ID AutonomousScanId;
+    s16               Pause;
+} CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST;
+
+typedef struct CSR_MLME_POWERMGT_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_POWERMGT_CONFIRM;
+
+typedef struct CSR_MLME_POWERMGT_REQUEST
+{
+    CSR_DATAREF               Dummydataref1;
+    CSR_DATAREF               Dummydataref2;
+    CSR_VIF_IDENTIFIER        VirtualInterfaceIdentifier;
+    CSR_POWER_MANAGEMENT_MODE PowerManagementMode;
+    s16                  ReceiveDtims;
+    CSR_BEACON_PERIODS        ListenInterval;
+    CSR_TRAFFIC_WINDOW        TrafficWindow;
+} CSR_MLME_POWERMGT_REQUEST;
+
+typedef struct CSR_MLME_SCAN_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_SCAN_CONFIRM;
+
+typedef struct CSR_MLME_SCAN_REQUEST
+{
+    CSR_DATAREF        ChannelList;
+    CSR_DATAREF        InformationElements;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_IFINTERFACE    Ifindex;
+    CSR_SCAN_TYPE      ScanType;
+    CSR_MICROSECONDS32 ProbeDelay;
+    CSR_TIME_UNITS     MinChannelTime;
+    CSR_TIME_UNITS     MaxChannelTime;
+} CSR_MLME_SCAN_REQUEST;
+
+typedef struct CSR_MLME_SCAN_CANCEL_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+} CSR_MLME_SCAN_CANCEL_REQUEST;
+
+typedef struct CSR_MLME_SETKEYS_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_SETKEYS_CONFIRM;
+
+typedef struct CSR_MLME_SETKEYS_REQUEST
+{
+    CSR_DATAREF               Key;
+    CSR_DATAREF               Dummydataref2;
+    CSR_VIF_IDENTIFIER        VirtualInterfaceIdentifier;
+    CSR_NATURAL16             Length;
+    CSR_NATURAL16             KeyId;
+    CSR_KEY_TYPE              KeyType;
+    CSR_MACADDRESS            Address;
+    CSR_NATURAL16             SequenceNumber[8];
+    CSR_CIPHER_SUITE_SELECTOR CipherSuiteSelector;
+} CSR_MLME_SETKEYS_REQUEST;
+
+typedef struct CSR_MLME_SET_CHANNEL_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_SET_CHANNEL_CONFIRM;
+
+typedef struct CSR_MLME_SET_CHANNEL_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_IFINTERFACE    Ifindex;
+    CSR_CHANNEL_NUMBER Channel;
+    CSR_MACADDRESS     Address;
+    CSR_TIME_UNITS     AvailabilityDuration;
+    CSR_TIME_UNITS     AvailabilityInterval;
+} CSR_MLME_SET_CHANNEL_REQUEST;
+
+typedef struct CSR_MLME_SET_PACKET_FILTER_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_SET_PACKET_FILTER_CONFIRM;
+
+typedef struct CSR_MLME_SET_PACKET_FILTER_REQUEST
+{
+    CSR_DATAREF            InformationElements;
+    CSR_DATAREF            Dummydataref2;
+    CSR_VIF_IDENTIFIER     VirtualInterfaceIdentifier;
+    CSR_PACKET_FILTER_MODE PacketFilterMode;
+    CSR_IPV4_ADDRESS       ArpFilterAddress;
+} CSR_MLME_SET_PACKET_FILTER_REQUEST;
+
+typedef struct CSR_MLME_SET_TIM_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_SET_TIM_CONFIRM;
+
+typedef struct CSR_MLME_SET_TIM_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_ASSOCIATION_ID AssociationId;
+    s16           TimValue;
+} CSR_MLME_SET_TIM_REQUEST;
+
+typedef struct CSR_MLME_SM_START_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_SM_START_CONFIRM;
+
+typedef struct CSR_MLME_SM_START_REQUEST
+{
+    CSR_DATAREF                Beacon;
+    CSR_DATAREF                BssParameters;
+    CSR_VIF_IDENTIFIER         VirtualInterfaceIdentifier;
+    CSR_IFINTERFACE            Ifindex;
+    CSR_CHANNEL_NUMBER         Channel;
+    CSR_MACADDRESS             InterfaceAddress;
+    CSR_MACADDRESS             Bssid;
+    CSR_TIME_UNITS             BeaconPeriod;
+    CSR_BEACON_PERIODS         DtimPeriod;
+    CSR_CAPABILITY_INFORMATION CapabilityInformation;
+} CSR_MLME_SM_START_REQUEST;
+
+typedef struct CSR_MLME_START_AGGREGATION_CONFIRM
+{
+    CSR_DATAREF         Dummydataref1;
+    CSR_DATAREF         Dummydataref2;
+    CSR_VIF_IDENTIFIER  VirtualInterfaceIdentifier;
+    CSR_MACADDRESS      PeerQstaAddress;
+    CSR_PRIORITY        UserPriority;
+    CSR_DIRECTION       Direction;
+    CSR_RESULT_CODE     ResultCode;
+    CSR_SEQUENCE_NUMBER SequenceNumber;
+} CSR_MLME_START_AGGREGATION_CONFIRM;
+
+typedef struct CSR_MLME_START_AGGREGATION_REQUEST
+{
+    CSR_DATAREF                  Dummydataref1;
+    CSR_DATAREF                  Dummydataref2;
+    CSR_VIF_IDENTIFIER           VirtualInterfaceIdentifier;
+    CSR_MACADDRESS               PeerQstaAddress;
+    CSR_PRIORITY                 UserPriority;
+    CSR_DIRECTION                Direction;
+    CSR_STARTING_SEQUENCE_NUMBER StartingSequenceNumber;
+    CSR_NATURAL16                BufferSize;
+    CSR_TIME_UNITS               BlockAckTimeout;
+} CSR_MLME_START_AGGREGATION_REQUEST;
+
+typedef struct CSR_MLME_STOP_AGGREGATION_CONFIRM
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_MACADDRESS     PeerQstaAddress;
+    CSR_PRIORITY       UserPriority;
+    CSR_DIRECTION      Direction;
+    CSR_RESULT_CODE    ResultCode;
+} CSR_MLME_STOP_AGGREGATION_CONFIRM;
+
+typedef struct CSR_MLME_STOP_AGGREGATION_REQUEST
+{
+    CSR_DATAREF        Dummydataref1;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_MACADDRESS     PeerQstaAddress;
+    CSR_PRIORITY       UserPriority;
+    CSR_DIRECTION      Direction;
+} CSR_MLME_STOP_AGGREGATION_REQUEST;
+
+typedef struct CSR_MLME_TRIGGERED_GET_INDICATION
+{
+    CSR_DATAREF        MibAttributeValue;
+    CSR_DATAREF        Dummydataref2;
+    CSR_VIF_IDENTIFIER VirtualInterfaceIdentifier;
+    CSR_MIB_STATUS     Status;
+    CSR_NATURAL16      ErrorIndex;
+    CSR_TRIGGERED_ID   TriggeredId;
+} CSR_MLME_TRIGGERED_GET_INDICATION;
+
+typedef struct CSR_SIGNAL_PRIMITIVE
+{
+    CSR_SIGNAL_PRIMITIVE_HEADER SignalPrimitiveHeader;
+    union
+    {
+        CSR_MA_PACKET_REQUEST                    MaPacketRequest;
+        CSR_MA_PACKET_CONFIRM                    MaPacketConfirm;
+        CSR_MA_PACKET_INDICATION                 MaPacketIndication;
+        CSR_MA_PACKET_CANCEL_REQUEST             MaPacketCancelRequest;
+        CSR_MA_VIF_AVAILABILITY_RESPONSE         MaVifAvailabilityResponse;
+        CSR_MA_VIF_AVAILABILITY_INDICATION       MaVifAvailabilityIndication;
+        CSR_MA_PACKET_ERROR_INDICATION           MaPacketErrorIndication;
+        CSR_MLME_RESET_REQUEST                   MlmeResetRequest;
+        CSR_MLME_RESET_CONFIRM                   MlmeResetConfirm;
+        CSR_MLME_GET_REQUEST                     MlmeGetRequest;
+        CSR_MLME_GET_CONFIRM                     MlmeGetConfirm;
+        CSR_MLME_SET_REQUEST                     MlmeSetRequest;
+        CSR_MLME_SET_CONFIRM                     MlmeSetConfirm;
+        CSR_MLME_GET_NEXT_REQUEST                MlmeGetNextRequest;
+        CSR_MLME_GET_NEXT_CONFIRM                MlmeGetNextConfirm;
+        CSR_MLME_POWERMGT_REQUEST                MlmePowermgtRequest;
+        CSR_MLME_POWERMGT_CONFIRM                MlmePowermgtConfirm;
+        CSR_MLME_SCAN_REQUEST                    MlmeScanRequest;
+        CSR_MLME_SCAN_CONFIRM                    MlmeScanConfirm;
+        CSR_MLME_HL_SYNC_REQUEST                 MlmeHlSyncRequest;
+        CSR_MLME_HL_SYNC_CONFIRM                 MlmeHlSyncConfirm;
+        CSR_MLME_MEASURE_REQUEST                 MlmeMeasureRequest;
+        CSR_MLME_MEASURE_CONFIRM                 MlmeMeasureConfirm;
+        CSR_MLME_MEASURE_INDICATION              MlmeMeasureIndication;
+        CSR_MLME_SETKEYS_REQUEST                 MlmeSetkeysRequest;
+        CSR_MLME_SETKEYS_CONFIRM                 MlmeSetkeysConfirm;
+        CSR_MLME_DELETEKEYS_REQUEST              MlmeDeletekeysRequest;
+        CSR_MLME_DELETEKEYS_CONFIRM              MlmeDeletekeysConfirm;
+        CSR_MLME_AUTONOMOUS_SCAN_LOSS_INDICATION MlmeAutonomousScanLossIndication;
+        CSR_MLME_CONNECTED_INDICATION            MlmeConnectedIndication;
+        CSR_MLME_SCAN_CANCEL_REQUEST             MlmeScanCancelRequest;
+        CSR_MLME_HL_SYNC_CANCEL_REQUEST          MlmeHlSyncCancelRequest;
+        CSR_MLME_HL_SYNC_CANCEL_CONFIRM          MlmeHlSyncCancelConfirm;
+        CSR_MLME_ADD_PERIODIC_REQUEST            MlmeAddPeriodicRequest;
+        CSR_MLME_ADD_PERIODIC_CONFIRM            MlmeAddPeriodicConfirm;
+        CSR_MLME_DEL_PERIODIC_REQUEST            MlmeDelPeriodicRequest;
+        CSR_MLME_DEL_PERIODIC_CONFIRM            MlmeDelPeriodicConfirm;
+        CSR_MLME_ADD_AUTONOMOUS_SCAN_REQUEST     MlmeAddAutonomousScanRequest;
+        CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM     MlmeAddAutonomousScanConfirm;
+        CSR_MLME_DEL_AUTONOMOUS_SCAN_REQUEST     MlmeDelAutonomousScanRequest;
+        CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM     MlmeDelAutonomousScanConfirm;
+        CSR_MLME_SET_PACKET_FILTER_REQUEST       MlmeSetPacketFilterRequest;
+        CSR_MLME_SET_PACKET_FILTER_CONFIRM       MlmeSetPacketFilterConfirm;
+        CSR_MLME_STOP_MEASURE_REQUEST            MlmeStopMeasureRequest;
+        CSR_MLME_STOP_MEASURE_CONFIRM            MlmeStopMeasureConfirm;
+        CSR_MLME_PAUSE_AUTONOMOUS_SCAN_REQUEST   MlmePauseAutonomousScanRequest;
+        CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM   MlmePauseAutonomousScanConfirm;
+        CSR_MLME_AUTONOMOUS_SCAN_DONE_INDICATION MlmeAutonomousScanDoneIndication;
+        CSR_MLME_ADD_TRIGGERED_GET_REQUEST       MlmeAddTriggeredGetRequest;
+        CSR_MLME_ADD_TRIGGERED_GET_CONFIRM       MlmeAddTriggeredGetConfirm;
+        CSR_MLME_DEL_TRIGGERED_GET_REQUEST       MlmeDelTriggeredGetRequest;
+        CSR_MLME_DEL_TRIGGERED_GET_CONFIRM       MlmeDelTriggeredGetConfirm;
+        CSR_MLME_TRIGGERED_GET_INDICATION        MlmeTriggeredGetIndication;
+        CSR_MLME_ADD_BLACKOUT_REQUEST            MlmeAddBlackoutRequest;
+        CSR_MLME_ADD_BLACKOUT_CONFIRM            MlmeAddBlackoutConfirm;
+        CSR_MLME_BLACKOUT_ENDED_INDICATION       MlmeBlackoutEndedIndication;
+        CSR_MLME_DEL_BLACKOUT_REQUEST            MlmeDelBlackoutRequest;
+        CSR_MLME_DEL_BLACKOUT_CONFIRM            MlmeDelBlackoutConfirm;
+        CSR_MLME_ADD_RX_TRIGGER_REQUEST          MlmeAddRxTriggerRequest;
+        CSR_MLME_ADD_RX_TRIGGER_CONFIRM          MlmeAddRxTriggerConfirm;
+        CSR_MLME_DEL_RX_TRIGGER_REQUEST          MlmeDelRxTriggerRequest;
+        CSR_MLME_DEL_RX_TRIGGER_CONFIRM          MlmeDelRxTriggerConfirm;
+        CSR_MLME_CONNECT_STATUS_REQUEST          MlmeConnectStatusRequest;
+        CSR_MLME_CONNECT_STATUS_CONFIRM          MlmeConnectStatusConfirm;
+        CSR_MLME_MODIFY_BSS_PARAMETER_REQUEST    MlmeModifyBssParameterRequest;
+        CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM    MlmeModifyBssParameterConfirm;
+        CSR_MLME_ADD_TEMPLATE_REQUEST            MlmeAddTemplateRequest;
+        CSR_MLME_ADD_TEMPLATE_CONFIRM            MlmeAddTemplateConfirm;
+        CSR_MLME_CONFIG_QUEUE_REQUEST            MlmeConfigQueueRequest;
+        CSR_MLME_CONFIG_QUEUE_CONFIRM            MlmeConfigQueueConfirm;
+        CSR_MLME_ADD_TSPEC_REQUEST               MlmeAddTspecRequest;
+        CSR_MLME_ADD_TSPEC_CONFIRM               MlmeAddTspecConfirm;
+        CSR_MLME_DEL_TSPEC_REQUEST               MlmeDelTspecRequest;
+        CSR_MLME_DEL_TSPEC_CONFIRM               MlmeDelTspecConfirm;
+        CSR_MLME_START_AGGREGATION_REQUEST       MlmeStartAggregationRequest;
+        CSR_MLME_START_AGGREGATION_CONFIRM       MlmeStartAggregationConfirm;
+        CSR_MLME_BLOCKACK_ERROR_INDICATION       MlmeBlockackErrorIndication;
+        CSR_MLME_STOP_AGGREGATION_REQUEST        MlmeStopAggregationRequest;
+        CSR_MLME_STOP_AGGREGATION_CONFIRM        MlmeStopAggregationConfirm;
+        CSR_MLME_SM_START_REQUEST                MlmeSmStartRequest;
+        CSR_MLME_SM_START_CONFIRM                MlmeSmStartConfirm;
+        CSR_MLME_LEAVE_REQUEST                   MlmeLeaveRequest;
+        CSR_MLME_LEAVE_CONFIRM                   MlmeLeaveConfirm;
+        CSR_MLME_SET_TIM_REQUEST                 MlmeSetTimRequest;
+        CSR_MLME_SET_TIM_CONFIRM                 MlmeSetTimConfirm;
+        CSR_MLME_GET_KEY_SEQUENCE_REQUEST        MlmeGetKeySequenceRequest;
+        CSR_MLME_GET_KEY_SEQUENCE_CONFIRM        MlmeGetKeySequenceConfirm;
+        CSR_MLME_SET_CHANNEL_REQUEST             MlmeSetChannelRequest;
+        CSR_MLME_SET_CHANNEL_CONFIRM             MlmeSetChannelConfirm;
+        CSR_MLME_ADD_MULTICAST_ADDRESS_REQUEST   MlmeAddMulticastAddressRequest;
+        CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM   MlmeAddMulticastAddressConfirm;
+        CSR_DEBUG_STRING_INDICATION              DebugStringIndication;
+        CSR_DEBUG_WORD16_INDICATION              DebugWord16Indication;
+        CSR_DEBUG_GENERIC_REQUEST                DebugGenericRequest;
+        CSR_DEBUG_GENERIC_CONFIRM                DebugGenericConfirm;
+        CSR_DEBUG_GENERIC_INDICATION             DebugGenericIndication;
+    } u;
+} CSR_SIGNAL;
+
+#define SIG_FILTER_SIZE 6
+
+u32 SigGetFilterPos(u16 aSigID);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/csr/csr_wifi_hip_ta_sampling.c b/drivers/staging/csr/csr_wifi_hip_ta_sampling.c
new file mode 100644
index 00000000000..f1df36aa87e
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_ta_sampling.c
@@ -0,0 +1,541 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     csr_wifi_hip_ta_sampling.c
+ *
+ *  PURPOSE:
+ *      The traffic analysis sampling module.
+ *      This gathers data which is sent to the SME and used to analyse
+ *      the traffic behaviour.
+ *
+ * Provides:
+ *      unifi_ta_sampling_init - Initialise the internal state
+ *      unifi_ta_sample        - Sampling function, call this for every data packet
+ *
+ * Calls these external functions which must be provided:
+ *      unifi_ta_indicate_sampling - Pass sample data to the SME.
+ *      unifi_ta_indicate_protocol - Report certain data packet types to the SME.
+ * ---------------------------------------------------------------------------
+ */
+
+#include "csr_wifi_hip_card_sdio.h"
+
+/* Maximum number of Tx frames we store each CYCLE_1, for detecting period */
+#define TA_MAX_INTERVALS_IN_C1          100
+
+/* Number of intervals in CYCLE_1 (one second), for detecting periodic */
+/* Must match size of unifi_TrafficStats.intervals - 1 */
+#define TA_INTERVALS_NUM               10
+
+/* Step (in msecs) between intervals, for detecting periodic */
+/* We are only interested in periods up to 100ms, i.e. between beacons */
+/* This is correct for TA_INTERVALS_NUM=10 */
+#define TA_INTERVALS_STEP               10
+
+
+enum ta_frame_identity
+{
+    TA_FRAME_UNKNOWN,
+    TA_FRAME_ETHERNET_UNINTERESTING,
+    TA_FRAME_ETHERNET_INTERESTING
+};
+
+
+#define TA_ETHERNET_TYPE_OFFSET     6
+#define TA_LLC_HEADER_SIZE          8
+#define TA_IP_TYPE_OFFSET           17
+#define TA_UDP_SOURCE_PORT_OFFSET   28
+#define TA_UDP_DEST_PORT_OFFSET     (TA_UDP_SOURCE_PORT_OFFSET + 2)
+#define TA_BOOTP_CLIENT_MAC_ADDR_OFFSET 64
+#define TA_DHCP_MESSAGE_TYPE_OFFSET 278
+#define TA_DHCP_MESSAGE_TYPE_ACK    0x05
+#define TA_PROTO_TYPE_IP            0x0800
+#define TA_PROTO_TYPE_EAP           0x888E
+#define TA_PROTO_TYPE_WAI           0x8864
+#define TA_PROTO_TYPE_ARP           0x0806
+#define TA_IP_TYPE_TCP              0x06
+#define TA_IP_TYPE_UDP              0x11
+#define TA_UDP_PORT_BOOTPC          0x0044
+#define TA_UDP_PORT_BOOTPS          0x0043
+#define TA_EAPOL_TYPE_OFFSET        9
+#define TA_EAPOL_TYPE_START         0x01
+
+#define snap_802_2                  0xAAAA0300
+#define oui_rfc1042                 0x00000000
+#define oui_8021h                   0x0000f800
+static const u8 aironet_snap[5] = { 0x00, 0x40, 0x96, 0x00, 0x00 };
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ta_detect_protocol
+ *
+ *      Internal only.
+ *      Detects a specific protocol in a frame and indicates a TA event.
+ *
+ *  Arguments:
+ *      ta              The pointer to the TA module.
+ *      direction       The direction of the frame (tx or rx).
+ *      data            Pointer to the structure that contains the data.
+ *
+ *  Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+static enum ta_frame_identity ta_detect_protocol(card_t *card, CsrWifiRouterCtrlProtocolDirection direction,
+                                                 const bulk_data_desc_t *data,
+                                                 const u8 *saddr,
+                                                 const u8 *sta_macaddr)
+{
+    ta_data_t *tad = &card->ta_sampling;
+    u16 proto;
+    u16 source_port, dest_port;
+    CsrWifiMacAddress srcAddress;
+    u32 snap_hdr, oui_hdr;
+
+    if (data->data_length < TA_LLC_HEADER_SIZE)
+    {
+        return TA_FRAME_UNKNOWN;
+    }
+
+    snap_hdr = (((u32)data->os_data_ptr[0]) << 24) |
+               (((u32)data->os_data_ptr[1]) << 16) |
+               (((u32)data->os_data_ptr[2]) << 8);
+    if (snap_hdr != snap_802_2)
+    {
+        return TA_FRAME_UNKNOWN;
+    }
+
+    if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM)
+    {
+        /*
+         * Here we would use the custom filter to detect interesting frames.
+         */
+    }
+
+    oui_hdr = (((u32)data->os_data_ptr[3]) << 24) |
+              (((u32)data->os_data_ptr[4]) << 16) |
+              (((u32)data->os_data_ptr[5]) << 8);
+    if ((oui_hdr == oui_rfc1042) || (oui_hdr == oui_8021h))
+    {
+        proto = (data->os_data_ptr[TA_ETHERNET_TYPE_OFFSET] * 256) +
+                data->os_data_ptr[TA_ETHERNET_TYPE_OFFSET + 1];
+
+        /* The only interesting IP frames are the DHCP */
+        if (proto == TA_PROTO_TYPE_IP)
+        {
+            if (data->data_length > TA_IP_TYPE_OFFSET)
+            {
+                if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM)
+                {
+                    ta_l4stats_t *ta_l4stats = &tad->ta_l4stats;
+                    u8 l4proto = data->os_data_ptr[TA_IP_TYPE_OFFSET];
+
+                    if (l4proto == TA_IP_TYPE_TCP)
+                    {
+                        if (direction == CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX)
+                        {
+                            ta_l4stats->txTcpBytesCount += data->data_length;
+                        }
+                        else
+                        {
+                            ta_l4stats->rxTcpBytesCount += data->data_length;
+                        }
+                    }
+                    else if (l4proto == TA_IP_TYPE_UDP)
+                    {
+                        if (direction == CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX)
+                        {
+                            ta_l4stats->txUdpBytesCount += data->data_length;
+                        }
+                        else
+                        {
+                            ta_l4stats->rxUdpBytesCount += data->data_length;
+                        }
+                    }
+                }
+
+                /* detect DHCP frames */
+                if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP)
+                {
+                    /* DHCP frames are UDP frames with BOOTP ports */
+                    if (data->os_data_ptr[TA_IP_TYPE_OFFSET] == TA_IP_TYPE_UDP)
+                    {
+                        if (data->data_length > TA_UDP_DEST_PORT_OFFSET)
+                        {
+                            source_port = (data->os_data_ptr[TA_UDP_SOURCE_PORT_OFFSET] * 256) +
+                                          data->os_data_ptr[TA_UDP_SOURCE_PORT_OFFSET + 1];
+                            dest_port = (data->os_data_ptr[TA_UDP_DEST_PORT_OFFSET] * 256) +
+                                        data->os_data_ptr[TA_UDP_DEST_PORT_OFFSET + 1];
+
+                            if (((source_port == TA_UDP_PORT_BOOTPC) && (dest_port == TA_UDP_PORT_BOOTPS)) ||
+                                ((source_port == TA_UDP_PORT_BOOTPS) && (dest_port == TA_UDP_PORT_BOOTPC)))
+                            {
+                                /* The DHCP should have at least a message type (request, ack, nack, etc) */
+                                if (data->data_length > TA_DHCP_MESSAGE_TYPE_OFFSET + 6)
+                                {
+                                    UNIFI_MAC_ADDRESS_COPY(srcAddress.a, saddr);
+
+                                    if (direction == CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX)
+                                    {
+                                        unifi_ta_indicate_protocol(card->ospriv,
+                                                                   CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP,
+                                                                   direction,
+                                                                   &srcAddress);
+                                        return TA_FRAME_ETHERNET_UNINTERESTING;
+                                    }
+
+                                    /* DHCPACK is a special indication */
+                                    if (UNIFI_MAC_ADDRESS_CMP(data->os_data_ptr + TA_BOOTP_CLIENT_MAC_ADDR_OFFSET, sta_macaddr) == TRUE)
+                                    {
+                                        if (data->os_data_ptr[TA_DHCP_MESSAGE_TYPE_OFFSET] == TA_DHCP_MESSAGE_TYPE_ACK)
+                                        {
+                                            unifi_ta_indicate_protocol(card->ospriv,
+                                                                       CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP_ACK,
+                                                                       direction,
+                                                                       &srcAddress);
+                                        }
+                                        else
+                                        {
+                                            unifi_ta_indicate_protocol(card->ospriv,
+                                                                       CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP,
+                                                                       direction,
+                                                                       &srcAddress);
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            return TA_FRAME_ETHERNET_INTERESTING;
+        }
+
+        /* detect protocol type EAPOL or WAI (treated as equivalent here) */
+        if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_EAPOL)
+        {
+            if (TA_PROTO_TYPE_EAP == proto || TA_PROTO_TYPE_WAI == proto)
+            {
+                if ((TA_PROTO_TYPE_WAI == proto) || (direction != CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX) ||
+                    (data->os_data_ptr[TA_EAPOL_TYPE_OFFSET] == TA_EAPOL_TYPE_START))
+                {
+                    UNIFI_MAC_ADDRESS_COPY(srcAddress.a, saddr);
+                    unifi_ta_indicate_protocol(card->ospriv,
+                                               CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_EAPOL,
+                                               direction, &srcAddress);
+                }
+                return TA_FRAME_ETHERNET_UNINTERESTING;
+            }
+        }
+
+        /* detect protocol type 0x0806 (ARP) */
+        if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_ARP)
+        {
+            if (proto == TA_PROTO_TYPE_ARP)
+            {
+                UNIFI_MAC_ADDRESS_COPY(srcAddress.a, saddr);
+                unifi_ta_indicate_protocol(card->ospriv,
+                                           CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_ARP,
+                                           direction, &srcAddress);
+                return TA_FRAME_ETHERNET_UNINTERESTING;
+            }
+        }
+
+        return TA_FRAME_ETHERNET_INTERESTING;
+    }
+    else if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_AIRONET)
+    {
+        /* detect Aironet frames */
+        if (!memcmp(data->os_data_ptr + 3, aironet_snap, 5))
+        {
+            UNIFI_MAC_ADDRESS_COPY(srcAddress.a, saddr);
+            unifi_ta_indicate_protocol(card->ospriv, CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_AIRONET,
+                                       direction, &srcAddress);
+        }
+    }
+
+    return TA_FRAME_ETHERNET_UNINTERESTING;
+} /* ta_detect_protocol() */
+
+
+static void tas_reset_data(ta_data_t *tad)
+{
+    s16 i;
+
+    for (i = 0; i < (TA_INTERVALS_NUM + 1); i++)
+    {
+        tad->stats.intervals[i] = 0;
+    }
+
+    tad->stats.rxFramesNum = 0;
+    tad->stats.txFramesNum = 0;
+    tad->stats.rxBytesCount = 0;
+    tad->stats.txBytesCount = 0;
+    tad->stats.rxMeanRate = 0;
+
+    tad->rx_sum_rate = 0;
+
+    tad->ta_l4stats.rxTcpBytesCount = 0;
+    tad->ta_l4stats.txTcpBytesCount = 0;
+    tad->ta_l4stats.rxUdpBytesCount = 0;
+    tad->ta_l4stats.txUdpBytesCount = 0;
+} /* tas_reset_data() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  API.
+ *  unifi_ta_sampling_init
+ *
+ *      (Re)Initialise the Traffic Analysis sampling module.
+ *      Resets the counters and timestamps.
+ *
+ *  Arguments:
+ *      tad             Pointer to a ta_data_t structure containing the
+ *                      context for this device instance.
+ *      drv_priv        An opaque pointer that the TA sampling module will
+ *                      pass in call-outs.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void unifi_ta_sampling_init(card_t *card)
+{
+    (void)unifi_ta_configure(card, CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_RESET, NULL);
+
+    card->ta_sampling.packet_filter = CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_NONE;
+    card->ta_sampling.traffic_type = CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_OCCASIONAL;
+} /* unifi_ta_sampling_init() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  API.
+ *  unifi_ta_sample
+ *
+ *      Sample a data frame for the TA module.
+ *      This function stores all the useful information it can extract from
+ *      the frame and detects any specific protocols.
+ *
+ *  Arguments:
+ *      tad             The pointer to the TA sampling context struct.
+ *      direction       The direction of the frame (rx, tx)
+ *      data            Pointer to the frame data
+ *      saddr           Source MAC address of frame.
+ *      timestamp       Time (in msecs) that the frame was received.
+ *      rate            Reported data rate for the rx frame (0 for tx frames)
+ *
+ *  Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+void unifi_ta_sample(card_t                            *card,
+                     CsrWifiRouterCtrlProtocolDirection direction,
+                     const bulk_data_desc_t            *data,
+                     const u8                    *saddr,
+                     const u8                    *sta_macaddr,
+                     u32                          timestamp,
+                     u16                          rate)
+{
+    ta_data_t *tad = &card->ta_sampling;
+    enum ta_frame_identity identity;
+    u32 time_delta;
+
+
+
+    /* Step1: Check for specific frames */
+    if (tad->packet_filter != CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_NONE)
+    {
+        identity = ta_detect_protocol(card, direction, data, saddr, sta_macaddr);
+    }
+    else
+    {
+        identity = TA_FRAME_ETHERNET_INTERESTING;
+    }
+
+
+    /* Step2: Update the information in the current record */
+    if (direction == CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_RX)
+    {
+        /* Update the Rx packet count and the throughput count */
+        tad->stats.rxFramesNum++;
+        tad->stats.rxBytesCount += data->data_length;
+
+        /* Accumulate packet Rx rates for later averaging */
+        tad->rx_sum_rate += rate;
+    }
+    else
+    {
+        if (identity == TA_FRAME_ETHERNET_INTERESTING)
+        {
+            /*
+             * Store the period between the last and the current frame.
+             * There is not point storing more than TA_MAX_INTERVALS_IN_C1 periods,
+             * the traffic will be bursty or continuous.
+             */
+            if (tad->stats.txFramesNum < TA_MAX_INTERVALS_IN_C1)
+            {
+                u32 interval;
+                u32 index_in_intervals;
+
+                interval = timestamp - tad->tx_last_ts;
+                tad->tx_last_ts = timestamp;
+                index_in_intervals = (interval + TA_INTERVALS_STEP / 2 - 1) / TA_INTERVALS_STEP;
+
+                /* If the interval is interesting, update the t1_intervals count */
+                if (index_in_intervals <= TA_INTERVALS_NUM)
+                {
+                    unifi_trace(card->ospriv, UDBG5,
+                                "unifi_ta_sample: TX interval=%d index=%d\n",
+                                interval, index_in_intervals);
+                    tad->stats.intervals[index_in_intervals]++;
+                }
+            }
+        }
+
+        /* Update the Tx packet count... */
+        tad->stats.txFramesNum++;
+        /* ... and the number of bytes for throughput. */
+        tad->stats.txBytesCount += data->data_length;
+    }
+
+    /*
+     * If more than one second has elapsed since the last report, send
+     * another one.
+     */
+    /* Unsigned subtraction handles wrap-around from 0xFFFFFFFF to 0 */
+    time_delta = timestamp - tad->last_indication_time;
+    if (time_delta >= 1000)
+    {
+        /*
+         * rxFramesNum can be flashed in tas_reset_data() by another thread.
+         * Use a temp to avoid division by zero.
+         */
+        u32 temp_rxFramesNum;
+        temp_rxFramesNum = tad->stats.rxFramesNum;
+
+        /* Calculate this interval's mean frame Rx rate from the sum */
+        if (temp_rxFramesNum)
+        {
+            tad->stats.rxMeanRate = tad->rx_sum_rate / temp_rxFramesNum;
+        }
+        unifi_trace(card->ospriv, UDBG5,
+                    "unifi_ta_sample: RX fr=%lu, r=%u, sum=%lu, av=%lu\n",
+                    tad->stats.rxFramesNum, rate,
+                    tad->rx_sum_rate, tad->stats.rxMeanRate);
+
+        /*
+         * Send the information collected in the stats struct
+         * to the SME and reset the counters.
+         */
+        if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM)
+        {
+            u32 rxTcpThroughput = tad->ta_l4stats.rxTcpBytesCount / time_delta;
+            u32 txTcpThroughput = tad->ta_l4stats.txTcpBytesCount / time_delta;
+            u32 rxUdpThroughput = tad->ta_l4stats.rxUdpBytesCount / time_delta;
+            u32 txUdpThroughput = tad->ta_l4stats.txUdpBytesCount / time_delta;
+
+            unifi_ta_indicate_l4stats(card->ospriv,
+                                      rxTcpThroughput,
+                                      txTcpThroughput,
+                                      rxUdpThroughput,
+                                      txUdpThroughput
+                                      );
+        }
+        unifi_ta_indicate_sampling(card->ospriv, &tad->stats);
+        tas_reset_data(tad);
+        tad->last_indication_time = timestamp;
+    }
+} /* unifi_ta_sample() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  External API.
+ *  unifi_ta_configure
+ *
+ *      Configures the TA module parameters.
+ *
+ *  Arguments:
+ *      ta              The pointer to the TA module.
+ *      config_type     The type of the configuration request
+ *      config          Pointer to the configuration parameters.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code otherwise
+ * ---------------------------------------------------------------------------
+ */
+CsrResult unifi_ta_configure(card_t                               *card,
+                             CsrWifiRouterCtrlTrafficConfigType    config_type,
+                             const CsrWifiRouterCtrlTrafficConfig *config)
+{
+    ta_data_t *tad = &card->ta_sampling;
+
+    /* Reinitialise our data when we are reset */
+    if (config_type == CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_RESET)
+    {
+        /* Reset the stats to zero */
+        tas_reset_data(tad);
+
+        /* Reset the timer variables */
+        tad->tx_last_ts = 0;
+        tad->last_indication_time = 0;
+
+        return CSR_RESULT_SUCCESS;
+    }
+
+    if (config_type == CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_FILTER)
+    {
+        tad->packet_filter = config->packetFilter;
+
+        if (tad->packet_filter & CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM)
+        {
+            tad->custom_filter = config->customFilter;
+        }
+
+        return CSR_RESULT_SUCCESS;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_ta_configure() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  External API.
+ *  unifi_ta_classification
+ *
+ *      Configures the current TA classification.
+ *
+ *  Arguments:
+ *      ta              The pointer to the TA module.
+ *      traffic_type    The classification type
+ *      period          The traffic period if the type is periodic
+ *
+ *  Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+void unifi_ta_classification(card_t                      *card,
+                             CsrWifiRouterCtrlTrafficType traffic_type,
+                             u16                    period)
+{
+    unifi_trace(card->ospriv, UDBG3,
+                "Changed current ta classification to: %d\n", traffic_type);
+
+    card->ta_sampling.traffic_type = traffic_type;
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_ta_sampling.h b/drivers/staging/csr/csr_wifi_hip_ta_sampling.h
new file mode 100644
index 00000000000..46c630b4bee
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_ta_sampling.h
@@ -0,0 +1,75 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     csr_wifi_hip_ta_sampling.h
+ *
+ *  PURPOSE:
+ *      This file contains Traffic Analysis definitions common to the
+ *      sampling and analysis modules.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __TA_SAMPLING_H__
+#define __TA_SAMPLING_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "csr_wifi_hip_unifi.h"
+
+typedef struct ta_l4stats
+{
+    u32 rxTcpBytesCount;
+    u32 txTcpBytesCount;
+    u32 rxUdpBytesCount;
+    u32 txUdpBytesCount;
+} ta_l4stats_t;
+
+/*
+ * Context structure to preserve state between calls.
+ */
+
+typedef struct ta_data
+{
+    /* Current packet filter configuration */
+    u16 packet_filter;
+
+    /* Current packet custom filter configuration */
+    CsrWifiRouterCtrlTrafficFilter custom_filter;
+
+    /* The timestamp of the last tx packet processed. */
+    u32 tx_last_ts;
+
+    /* The timestamp of the last packet processed. */
+    u32 last_indication_time;
+
+    /* Statistics */
+    CsrWifiRouterCtrlTrafficStats stats;
+
+    /* Current traffic classification */
+    CsrWifiRouterCtrlTrafficType traffic_type;
+
+    /* Sum of packet rx rates for this interval used to calculate mean */
+    u32    rx_sum_rate;
+    ta_l4stats_t ta_l4stats;
+} ta_data_t;
+
+
+void unifi_ta_sampling_init(card_t *card);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __TA_SAMPLING_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_udi.c b/drivers/staging/csr/csr_wifi_hip_udi.c
new file mode 100644
index 00000000000..07cfd36c497
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_udi.c
@@ -0,0 +1,268 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     csr_wifi_hip_card_udi.c
+ *
+ *  PURPOSE:
+ *      Maintain a list of callbacks to log UniFi exchanges to one or more
+ *      debug/monitoring client applications.
+ *
+ * NOTES:
+ *      Just call the UDI driver log fn directly for now.
+ *      When done properly, each open() on the UDI device will install
+ *      a log function. We will call all log fns whenever a signal is written
+ *      to or read form the UniFi.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_card.h"
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_print_status
+ *
+ *      Print status info to given character buffer.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+s32 unifi_print_status(card_t *card, char *str, s32 *remain)
+{
+    char *p = str;
+    sdio_config_data_t *cfg;
+    u16 i, n;
+    s32 remaining = *remain;
+    s32 written;
+#ifdef CSR_UNSAFE_SDIO_ACCESS
+    s32 iostate;
+    CsrResult r;
+    static const char *const states[] = {
+        "AWAKE", "DROWSY", "TORPID"
+    };
+    #define SHARED_READ_RETRY_LIMIT 10
+    u8 b;
+#endif
+
+    if (remaining <= 0)
+    {
+        return 0;
+    }
+
+    i = n = 0;
+    written = CsrSnprintf(p, remaining, "Chip ID %u\n",
+                          (u16)card->chip_id);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "Chip Version %04X\n",
+                          card->chip_version);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "HIP v%u.%u\n",
+                          (card->config_data.version >> 8) & 0xFF,
+                          card->config_data.version & 0xFF);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "Build %lu: %s\n",
+                          card->build_id, card->build_id_string);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    cfg = &card->config_data;
+
+    written = CsrSnprintf(p, remaining, "sdio ctrl offset          %u\n",
+                          cfg->sdio_ctrl_offset);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "fromhost sigbuf handle    %u\n",
+                          cfg->fromhost_sigbuf_handle);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "tohost_sigbuf_handle      %u\n",
+                          cfg->tohost_sigbuf_handle);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "num_fromhost_sig_frags    %u\n",
+                          cfg->num_fromhost_sig_frags);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "num_tohost_sig_frags      %u\n",
+                          cfg->num_tohost_sig_frags);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "num_fromhost_data_slots   %u\n",
+                          cfg->num_fromhost_data_slots);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "num_tohost_data_slots     %u\n",
+                          cfg->num_tohost_data_slots);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "data_slot_size            %u\n",
+                          cfg->data_slot_size);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    /* Added by protocol version 0x0001 */
+    written = CsrSnprintf(p, remaining, "overlay_size              %u\n",
+                          (u16)cfg->overlay_size);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    /* Added by protocol version 0x0300 */
+    written = CsrSnprintf(p, remaining, "data_slot_round           %u\n",
+                          cfg->data_slot_round);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "sig_frag_size             %u\n",
+                          cfg->sig_frag_size);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    /* Added by protocol version 0x0300 */
+    written = CsrSnprintf(p, remaining, "tohost_sig_pad            %u\n",
+                          cfg->tohost_signal_padding);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    written = CsrSnprintf(p, remaining, "\nInternal state:\n");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    written = CsrSnprintf(p, remaining, "Last PHY PANIC: %04x:%04x\n",
+                          card->last_phy_panic_code, card->last_phy_panic_arg);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "Last MAC PANIC: %04x:%04x\n",
+                          card->last_mac_panic_code, card->last_mac_panic_arg);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    written = CsrSnprintf(p, remaining, "fhsr: %u\n",
+                          (u16)card->from_host_signals_r);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "fhsw: %u\n",
+                          (u16)card->from_host_signals_w);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "thsr: %u\n",
+                          (u16)card->to_host_signals_r);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "thsw: %u\n",
+                          (u16)card->to_host_signals_w);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining,
+                          "fh buffer contains: %u signals, %u bytes\n",
+                          card->fh_buffer.count,
+                          card->fh_buffer.ptr - card->fh_buffer.buf);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    written = CsrSnprintf(p, remaining, "paused: ");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    for (i = 0; i < sizeof(card->tx_q_paused_flag) / sizeof(card->tx_q_paused_flag[0]); i++)
+    {
+        written = CsrSnprintf(p, remaining, card->tx_q_paused_flag[i]?"1" : "0");
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+    }
+    written = CsrSnprintf(p, remaining, "\n");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    written = CsrSnprintf(p, remaining,
+                          "fh command q: %u waiting, %u free of %u:\n",
+                          CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_command_queue),
+                          CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_command_queue),
+                          UNIFI_SOFT_COMMAND_Q_LENGTH);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    for (i = 0; i < UNIFI_NO_OF_TX_QS; i++)
+    {
+        written = CsrSnprintf(p, remaining,
+                              "fh traffic q[%u]: %u waiting, %u free of %u:\n",
+                              i,
+                              CSR_WIFI_HIP_Q_SLOTS_USED(&card->fh_traffic_queue[i]),
+                              CSR_WIFI_HIP_Q_SLOTS_FREE(&card->fh_traffic_queue[i]),
+                              UNIFI_SOFT_TRAFFIC_Q_LENGTH);
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+    }
+
+    written = CsrSnprintf(p, remaining, "fh data slots free: %u\n",
+                          card->from_host_data?CardGetFreeFromHostDataSlots(card) : 0);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+
+    written = CsrSnprintf(p, remaining, "From host data slots:");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    n = card->config_data.num_fromhost_data_slots;
+    for (i = 0; i < n && card->from_host_data; i++)
+    {
+        written = CsrSnprintf(p, remaining, " %u",
+                              (u16)card->from_host_data[i].bd.data_length);
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+    }
+    written = CsrSnprintf(p, remaining, "\n");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    written = CsrSnprintf(p, remaining, "To host data slots:");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    n = card->config_data.num_tohost_data_slots;
+    for (i = 0; i < n && card->to_host_data; i++)
+    {
+        written = CsrSnprintf(p, remaining, " %u",
+                              (u16)card->to_host_data[i].data_length);
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+    }
+
+    written = CsrSnprintf(p, remaining, "\n");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+#ifdef CSR_UNSAFE_SDIO_ACCESS
+    written = CsrSnprintf(p, remaining, "Host State: %s\n", states[card->host_state]);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    r = unifi_check_io_status(card, &iostate);
+    if (iostate == 1)
+    {
+        written = CsrSnprintf(p, remaining, "I/O Check: F1 disabled\n");
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+    }
+    else
+    {
+        if (iostate == 1)
+        {
+            written = CsrSnprintf(p, remaining, "I/O Check: pending interrupt\n");
+            UNIFI_SNPRINTF_RET(p, remaining, written);
+        }
+
+        written = CsrSnprintf(p, remaining, "BH reason interrupt = %d\n",
+                              card->bh_reason_unifi);
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+        written = CsrSnprintf(p, remaining, "BH reason host      = %d\n",
+                              card->bh_reason_host);
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+
+        for (i = 0; i < SHARED_READ_RETRY_LIMIT; i++)
+        {
+            r = unifi_read_8_or_16(card, card->sdio_ctrl_addr + 2, &b);
+            if ((r == CSR_RESULT_SUCCESS) && (!(b & 0x80)))
+            {
+                written = CsrSnprintf(p, remaining, "fhsr: %u (driver thinks is %u)\n",
+                                      b, card->from_host_signals_r);
+                UNIFI_SNPRINTF_RET(p, remaining, written);
+                break;
+            }
+        }
+        iostate = unifi_read_shared_count(card, card->sdio_ctrl_addr + 4);
+        written = CsrSnprintf(p, remaining, "thsw: %u (driver thinks is %u)\n",
+                              iostate, card->to_host_signals_w);
+        UNIFI_SNPRINTF_RET(p, remaining, written);
+    }
+#endif
+
+    written = CsrSnprintf(p, remaining, "\nStats:\n");
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "Total SDIO bytes: R=%lu W=%lu\n",
+                          card->sdio_bytes_read, card->sdio_bytes_written);
+
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+    written = CsrSnprintf(p, remaining, "Interrupts generated on card: %lu\n",
+                          card->unifi_interrupt_seq);
+    UNIFI_SNPRINTF_RET(p, remaining, written);
+
+    *remain = remaining;
+    return (p - str);
+} /* unifi_print_status() */
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_unifi.h b/drivers/staging/csr/csr_wifi_hip_unifi.h
new file mode 100644
index 00000000000..dc3c60b4970
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_unifi.h
@@ -0,0 +1,880 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * FILE : csr_wifi_hip_unifi.h
+ *
+ * PURPOSE : Public API for the UniFi HIP core library.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __CSR_WIFI_HIP_UNIFI_H__
+#define __CSR_WIFI_HIP_UNIFI_H__ 1
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+#include "csr_wifi_router_ctrl_prim.h"
+#include "csr_wifi_router_prim.h"
+#else
+#include "csr_time.h"
+#endif
+
+/* SDIO chip ID numbers */
+
+/* Manufacturer id */
+#define SDIO_MANF_ID_CSR              0x032a
+
+/* Device id */
+#define SDIO_CARD_ID_UNIFI_1          0x0001
+#define SDIO_CARD_ID_UNIFI_2          0x0002
+#define SDIO_CARD_ID_UNIFI_3          0x0007
+#define SDIO_CARD_ID_UNIFI_4          0x0008
+
+/* Function number for WLAN */
+#define SDIO_WLAN_FUNC_ID_UNIFI_1          0x0001
+#define SDIO_WLAN_FUNC_ID_UNIFI_2          0x0001
+#define SDIO_WLAN_FUNC_ID_UNIFI_3          0x0001
+#define SDIO_WLAN_FUNC_ID_UNIFI_4          0x0002
+
+/* Maximum SDIO bus clock supported. */
+#define UNIFI_SDIO_CLOCK_MAX_HZ    50000000  /* Hz */
+
+/*
+ * Initialisation SDIO bus clock.
+ *
+ * The initialisation clock speed should be used from when the chip has been
+ * reset until the first MLME-reset has been received (i.e. during firmware
+ * initialisation), unless UNIFI_SDIO_CLOCK_SAFE_HZ applies.
+ */
+#define UNIFI_SDIO_CLOCK_INIT_HZ    12500000 /* Hz */
+
+/*
+ * Safe SDIO bus clock.
+ *
+ * The safe speed should be used when the chip is in deep sleep or
+ * it's state is unknown (just after reset / power on).
+ */
+#define UNIFI_SDIO_CLOCK_SAFE_HZ    1000000  /* Hz */
+
+/* I/O default block size to use for UniFi. */
+#define UNIFI_IO_BLOCK_SIZE     64
+
+#define UNIFI_WOL_OFF   0
+#define UNIFI_WOL_SDIO  1
+#define UNIFI_WOL_PIO   2
+
+/* The number of Tx traffic queues */
+#define UNIFI_NO_OF_TX_QS              4
+
+#define CSR_WIFI_HIP_RESERVED_HOST_TAG 0xFFFFFFFF
+
+/*
+ * The number of slots in the from-host queues.
+ *
+ * UNIFI_SOFT_TRAFFIC_Q_LENGTH is the number of slots in the traffic queues
+ * and there will be UNIFI_NO_OF_TX_QS of them.
+ * Traffic queues are used for data packets.
+ *
+ * UNIFI_SOFT_COMMAND_Q_LENGTH is the number of slots in the command queue.
+ * The command queue is used for MLME management requests.
+ *
+ * Queues are ring buffers and so must always have 1 unused slot.
+ */
+#define UNIFI_SOFT_TRAFFIC_Q_LENGTH (20 + 1)
+#define UNIFI_SOFT_COMMAND_Q_LENGTH (16 + 1)
+
+#include "csr_framework_ext.h"  /* from the synergy porting folder */
+#include "csr_sdio.h"           /* from the synergy porting folder */
+#include "csr_macro.h"          /* from the synergy porting folder */
+#include "csr_formatted_io.h"   /* from the synergy gsp folder */
+#include "csr_wifi_result.h"
+
+/* Utility MACROS. Note that UNIFI_MAC_ADDRESS_CMP returns TRUE on success */
+#define UNIFI_MAC_ADDRESS_COPY(dst, src) \
+    do { (dst)[0] = (src)[0]; (dst)[1] = (src)[1]; \
+         (dst)[2] = (src)[2]; (dst)[3] = (src)[3]; \
+         (dst)[4] = (src)[4]; (dst)[5] = (src)[5]; \
+    } while (0)
+
+#define UNIFI_MAC_ADDRESS_CMP(addr1, addr2) \
+    (((addr1)[0] == (addr2)[0]) && ((addr1)[1] == (addr2)[1]) && \
+     ((addr1)[2] == (addr2)[2]) && ((addr1)[3] == (addr2)[3]) && \
+     ((addr1)[4] == (addr2)[4]) && ((addr1)[5] == (addr2)[5]))
+
+/* Traffic queue ordered according to priority
+ * EAPOL/Uncontrolled port Queue should be the last
+ */
+typedef enum
+{
+    UNIFI_TRAFFIC_Q_BK = 0,
+    UNIFI_TRAFFIC_Q_BE,
+    UNIFI_TRAFFIC_Q_VI,
+    UNIFI_TRAFFIC_Q_VO,
+    UNIFI_TRAFFIC_Q_EAPOL,    /* Non existant in HIP */
+    UNIFI_TRAFFIC_Q_MAX,      /* Non existant */
+    UNIFI_TRAFFIC_Q_MLME      /* Non existant */
+} unifi_TrafficQueue;
+
+/*
+ * Structure describing a bulk data slot.
+ * This structure is shared between the HIP core library and the OS
+ * layer. See the definition of unifi_net_data_malloc() for more details.
+ *
+ * The data_length field is used to indicate empty/occupied state.
+ * Needs to be defined before #include "unifi_os.h".
+ */
+typedef struct _bulk_data_desc
+{
+    const u8 *os_data_ptr;
+    u32       data_length;
+    const void     *os_net_buf_ptr;
+    u32       net_buf_length;
+} bulk_data_desc_t;
+
+/* Structure of an entry in the Symbol Look Up Table (SLUT). */
+typedef struct _symbol
+{
+    u16 id;
+    u32 obj;
+} symbol_t;
+
+/*
+ * Header files need to be included from the current directory,
+ * the SME library, the synergy framework and the OS layer.
+ * A thin OS layer needs to be implemented in the porting exercise.
+ *
+ * Note that unifi_os.h should be included only in unifi.h
+ */
+
+#include "unifi_os.h"
+
+/*
+ * Contains the HIP core definitions selected in the porting exercise, such as
+ * UNIFI_PAD_BULK_DATA_TO_BLOCK_SIZE and UNIFI_PAD_SIGNALS_TO_BLOCK_SIZE.
+ * Implemented in the OS layer, as part of the porting exersice.
+ */
+#include "unifi_config.h"
+
+#include "csr_wifi_hip_signals.h" /* from this dir */
+
+/*
+ * The card structure is an opaque pointer that is used to pass context
+ * to the upper-edge API functions.
+ */
+typedef struct card card_t;
+
+
+/*
+ * This structure describes all of the bulk data that 'might' be
+ * associated with a signal.
+ */
+typedef struct _bulk_data_param
+{
+    bulk_data_desc_t d[UNIFI_MAX_DATA_REFERENCES];
+} bulk_data_param_t;
+
+
+/*
+ * This structure describes the chip and HIP core lib
+ * information that exposed to the OS layer.
+ */
+typedef struct _card_info
+{
+    u16 chip_id;
+    u16 chip_version;
+    u32 fw_build;
+    u16 fw_hip_version;
+    u32 sdio_block_size;
+} card_info_t;
+
+
+/*
+ * Mini-coredump definitions
+ */
+/* Definition of XAP memory ranges used by the mini-coredump system.
+ * Note that, these values are NOT the same as UNIFI_REGISTERS, etc
+ * in unifihw.h which don't allow selection of register areas for each XAP.
+ */
+typedef enum unifi_coredump_space
+{
+    UNIFI_COREDUMP_MAC_REG,
+    UNIFI_COREDUMP_PHY_REG,
+    UNIFI_COREDUMP_SH_DMEM,
+    UNIFI_COREDUMP_MAC_DMEM,
+    UNIFI_COREDUMP_PHY_DMEM,
+    UNIFI_COREDUMP_TRIGGER_MAGIC = 0xFEED
+} unifi_coredump_space_t;
+
+/* Structure used to request a register value from a mini-coredump buffer */
+typedef struct unifi_coredump_req
+{
+    /* From user */
+    s32               index;       /* 0=newest, -1=oldest */
+    unifi_coredump_space_t space;       /* memory space */
+    u32              offset;      /* register offset in space */
+    /* From driver */
+    u32 drv_build;                /* Driver build id */
+    u32 chip_ver;                 /* Chip version */
+    u32 fw_ver;                   /* Firmware version */
+    s32  requestor;                /* Requestor: 0=auto dump, 1=manual */
+    CsrTime   timestamp;                /* time of capture by driver */
+    u32 serial;                   /* capture serial number */
+    s32  value;                    /* register value */
+} unifi_coredump_req_t;                 /* mini-coredumped reg value request */
+
+
+/**
+ * @defgroup upperedge Upper edge API
+ *
+ * The following functions are implemented in the HIP core lib.
+ */
+
+/**
+ *
+ * Initialise the HIP core lib.
+ * Note that the OS layer must initialise the SDIO glue layer and obtain
+ * an SDIO function context, prior to this call.
+ *
+ * @param sdiopriv the SDIO function context.
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @return \p card_t the HIP core lib API context.
+ *
+ * @ingroup upperedge
+ */
+card_t* unifi_alloc_card(CsrSdioFunction *sdiopriv, void *ospriv);
+
+
+/**
+ *
+ * Initialise the UniFi chip.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @param led_mask the led mask to apply to UniFi.
+ *
+ * @return \b 0 if UniFi is initialized.
+ *
+ * @return \b -CSR_EIO if an I/O error occured while initializing UniFi
+ *
+ * @return \b -CSR_ENODEV if the card is no longer present.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_init_card(card_t *card, s32 led_mask);
+
+/**
+ *
+ * De-Initialise the HIP core lib.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @ingroup upperedge
+ */
+void unifi_free_card(card_t *card);
+
+/**
+ *
+ * Cancel all the signals pending in the HIP core lib.
+ * Normally used during a system suspend when the power is retained on UniFi.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @ingroup upperedge
+ */
+void unifi_cancel_pending_signals(card_t *card);
+
+/**
+ *
+ * Send a signal to UniFi.
+ * Normally it is called from unifi_sys_hip_req() and the OS layer
+ * Tx data plane.
+ *
+ * Note that the bulkdata buffers ownership is passed to the HIP core lib.
+ * These buffers must be allocated using unifi_net_data_malloc().
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @param sigptr pointer to the signal.
+ *
+ * @param siglen size of the signal.
+ *
+ * @param bulkdata pointer to the bulk data associated with the signal.
+ *
+ * @return \b 0 signal is sent.
+ *
+ * @return \b -CSR_EIO if an error occured while sending the signal
+ *
+ * @return \b -CSR_ENODEV if the card is no longer present.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_send_signal(card_t *card, const u8 *sigptr,
+                            u32 siglen,
+                            const bulk_data_param_t *bulkdata);
+
+/**
+ *
+ * Check if the HIP core lib has resources to send a signal.
+ * Normally there no need to use this function.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @param sigptr pointer to the signal.
+ *
+ * @return \b 0 if there are resources for the signal.
+ *
+ * @return \b -CSR_ENOSPC if there are not enough resources
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_send_resources_available(card_t *card, const u8 *sigptr);
+
+/**
+ *
+ * Read the UniFi chip and the HIP core lib information.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @param card_info pointer to save the information.
+ *
+ * @ingroup upperedge
+ */
+void unifi_card_info(card_t *card, card_info_t *card_info);
+
+/**
+ *
+ * Print the UniFi I/O and Interrupt status.
+ * Normally it is used for debug purposes only.
+ *
+ * @param card the HIP core lib API context.
+
+ * @param status buffer for the chip status
+ *
+ * @return \b 0 if the check was performed.
+ *
+ * @return \b -CSR_EIO if an error occured while checking the status.
+ *
+ * @return \b -CSR_ENODEV if the card is no longer present.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_check_io_status(card_t *card, s32 *status);
+
+
+/**
+ *
+ * Run the HIP core lib Botton-Half.
+ * Whenever the HIP core lib want this function to be called
+ * by the OS layer, it calls unifi_run_bh().
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @param remaining pointer to return the time (in msecs) that this function
+ * should be re-scheduled. A return value of 0 means that no re-scheduling
+ * is required. If unifi_bh() is called before the timeout expires,
+ * the caller must pass in the remaining time.
+ *
+ * @return \b 0 if no error occured.
+ *
+ * @return \b -CSR_ENODEV if the card is no longer present.
+ *
+ * @return \b -CSR_E* if an error occured while running the bottom half.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_bh(card_t *card, u32 *remaining);
+
+
+/**
+ * UniFi Low Power Mode (Deep Sleep Signaling)
+ *
+ * unifi_low_power_mode defines the UniFi Deep Sleep Signaling status.
+ * Use with unifi_configure_low_power_mode() to enable/disable
+ * the Deep Sleep Signaling.
+ */
+enum unifi_low_power_mode
+{
+    UNIFI_LOW_POWER_DISABLED,
+    UNIFI_LOW_POWER_ENABLED
+};
+
+/**
+ * Periodic Wake Host Mode
+ *
+ * unifi_periodic_wake_mode defines the Periodic Wake Host Mode.
+ * It can only be set to UNIFI_PERIODIC_WAKE_HOST_ENABLED if
+ * low_power_mode == UNIFI_LOW_POWER_ENABLED.
+ */
+enum unifi_periodic_wake_mode
+{
+    UNIFI_PERIODIC_WAKE_HOST_DISABLED,
+    UNIFI_PERIODIC_WAKE_HOST_ENABLED
+};
+
+/**
+ *
+ * Run the HIP core lib Botton-Half.
+ * Whenever the HIP core lib want this function to be called
+ * by the OS layer, it calls unifi_run_bh().
+ *
+ * Typically, the SME is responsible for configuring these parameters,
+ * so unifi_sys_configure_power_mode_req() is usually implemented
+ * as a direct call to unifi_configure_low_power_mode().
+ *
+ * Note: When polling mode is used instead of interrupts,
+ * low_power_mode must never be set to UNIFI_LOW_POWER_ENABLED.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @param low_power_mode the Low Power Mode.
+ *
+ * @param periodic_wake_mode the Periodic Wake Mode.
+ *
+ * @return \b 0 if no error occured.
+ *
+ * @return \b -CSR_E* if the request failed.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_configure_low_power_mode(card_t                       *card,
+                                         enum unifi_low_power_mode     low_power_mode,
+                                         enum unifi_periodic_wake_mode periodic_wake_mode);
+
+/**
+ *
+ * Forces the UniFi chip to enter a Deep Sleep state.
+ * This is normally called by the OS layer when the platform suspends.
+ *
+ * Note that if the UniFi Low Power Mode is disabled this call fails.
+ *
+ * @param card the HIP core lib API context.
+ *
+ * @return \b 0 if no error occured.
+ *
+ * @return \b -CSR_ENODEV if the card is no longer present.
+ *
+ * @return \b -CSR_E* if the request failed.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_force_low_power_mode(card_t *card);
+
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+/**
+ * Configure the Traffic Analysis sampling
+ *
+ * Enable or disable statistics gathering.
+ * Enable or disable particular packet detection.
+ *
+ * @param card the HIP core context
+ * @param config_type the item to configure
+ * @param config pointer to struct containing config info
+ *
+ * @return \b 0 if configuration was successful
+ *
+ * @return \b -CSR_EINVAL if a parameter had an invalid value
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_ta_configure(card_t                               *card,
+                             CsrWifiRouterCtrlTrafficConfigType    config_type,
+                             const CsrWifiRouterCtrlTrafficConfig *config);
+
+/**
+ * Pass a packet for Traffic Analysis sampling
+ *
+ * @param card the HIP core context
+ * @param direction the direction (Rx or Tx) of the frame.
+ * @param data pointer to bulkdata struct containing the packet
+ * @param saddr the source address of the packet
+ * @param sta_macaddr the MAC address of the UniFi chip
+ * @param timestamp the current time in msecs
+ *
+ * @ingroup upperedge
+ */
+void unifi_ta_sample(card_t                            *card,
+                     CsrWifiRouterCtrlProtocolDirection direction,
+                     const bulk_data_desc_t            *data,
+                     const u8                    *saddr,
+                     const u8                    *sta_macaddr,
+                     u32                          timestamp,
+                     u16                          rate);
+
+/**
+ * Notify the HIP core lib for a detected Traffic Classification.
+ * Typically, the SME is responsible for configuring these parameters,
+ * so unifi_sys_traffic_classification_req() is usually implemented
+ * as a direct call to unifi_ta_classification().
+ *
+ * @param card the HIP core context.
+ * @param traffic_type the detected traffic type.
+ * @param period The detected period of the traffic.
+ *
+ * @ingroup upperedge
+ */
+void unifi_ta_classification(card_t                      *card,
+                             CsrWifiRouterCtrlTrafficType traffic_type,
+                             u16                    period);
+
+#endif
+/**
+ * Use software to hard reset the chip.
+ * This is a subset of the unifi_init_card() functionality and should
+ * only be used only to reset a paniced chip before a coredump is taken.
+ *
+ * @param card the HIP core context.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_card_hard_reset(card_t *card);
+
+
+CsrResult unifi_card_readn(card_t *card, u32 unifi_addr, void *pdata, u16 len);
+CsrResult unifi_card_read16(card_t *card, u32 unifi_addr, u16 *pdata);
+CsrResult unifi_card_write16(card_t *card, u32 unifi_addr, u16 data);
+
+
+enum unifi_dbg_processors_select
+{
+    UNIFI_PROC_MAC,
+    UNIFI_PROC_PHY,
+    UNIFI_PROC_BT,
+    UNIFI_PROC_BOTH,
+    UNIFI_PROC_INVALID
+};
+
+CsrResult unifi_card_stop_processor(card_t *card, enum unifi_dbg_processors_select which);
+
+/**
+ * Call-outs from the HIP core lib to the OS layer.
+ * The following functions need to be implemented during the porting exercise.
+ */
+
+/**
+ * Selects appropriate queue according to priority
+ * Helps maintain uniformity in queue selection between the HIP
+ * and the OS layers.
+ *
+ * @param priority priority of the packet
+ *
+ * @return \b Traffic queue to which a packet of this priority belongs
+ *
+ * @ingroup upperedge
+ */
+unifi_TrafficQueue
+unifi_frame_priority_to_queue(CSR_PRIORITY priority);
+
+/**
+ * Returns the priority corresponding to a particular Queue when that is used
+ * when downgrading a packet to a lower AC.
+ * Helps maintain uniformity in queue - priority mapping between the HIP
+ * and the OS layers.
+ *
+ * @param queue
+ *
+ * @return \b Highest priority corresponding to this queue
+ *
+ * @ingroup upperedge
+ */
+CSR_PRIORITY unifi_get_default_downgrade_priority(unifi_TrafficQueue queue);
+
+/**
+ *
+ * Flow control callbacks.
+ * unifi_pause_xmit() is called when the HIP core lib does not have any
+ * resources to store data packets. The OS layer needs to pause
+ * the Tx data plane until unifi_restart_xmit() is called.
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @ingroup upperedge
+ */
+void unifi_pause_xmit(void *ospriv, unifi_TrafficQueue queue);
+void unifi_restart_xmit(void *ospriv, unifi_TrafficQueue queue);
+
+/**
+ *
+ * Request to run the Bottom-Half.
+ * The HIP core lib calls this function to request that unifi_bh()
+ * needs to be run by the OS layer. It can be called anytime, i.e.
+ * when the unifi_bh() is running.
+ * Since unifi_bh() is not re-entrant, usually unifi_run_bh() sets
+ * an event to a thread that schedules a call to unifi_bh().
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_run_bh(void *ospriv);
+
+/**
+ *
+ * Delivers a signal received from UniFi to the OS layer.
+ * Normally, the data signals should be delivered to the data plane
+ * and all the rest to the SME (unifi_sys_hip_ind()).
+ *
+ * Note that the OS layer is responsible for freeing the bulkdata
+ * buffers, using unifi_net_data_free().
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param sigptr pointer to the signal.
+ *
+ * @param siglen size of the signal.
+ *
+ * @param bulkdata pointer to the bulk data associated with the signal.
+ *
+ * @ingroup upperedge
+ */
+void unifi_receive_event(void *ospriv,
+                         u8 *sigdata, u32 siglen,
+                         const bulk_data_param_t *bulkdata);
+
+#ifdef CSR_WIFI_REQUEUE_PACKET_TO_HAL
+/**
+ *
+ * Used to reque the failed ma packet request back to hal queues
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param host_tag host tag for the packet to requeue.
+ *
+ * @param bulkDataDesc pointer to the bulk data.
+ *
+ * @ingroup upperedge
+ */
+CsrResult unifi_reque_ma_packet_request(void *ospriv, u32 host_tag,
+                                        u16 status,
+                                        bulk_data_desc_t *bulkDataDesc);
+
+#endif
+typedef struct
+{
+    u16 free_fh_sig_queue_slots[UNIFI_NO_OF_TX_QS];
+    u16 free_fh_bulkdata_slots;
+    u16 free_fh_fw_slots;
+} unifi_HipQosInfo;
+
+void unifi_get_hip_qos_info(card_t *card, unifi_HipQosInfo *hipqosinfo);
+
+
+/**
+ * Functions that read a portion of a firmware file.
+ *
+ * Note: If the UniFi chip runs the f/w from ROM, the HIP core may never
+ * call these functions. Also, the HIP core may call these functions even if
+ * a f/w file is not available. In this case, it is safe to fail the request.
+ */
+#define UNIFI_FW_STA    1   /* Identify STA firmware file */
+
+/**
+ *
+ * Ask the OS layer to initialise a read from a f/w file.
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param is_fw if 0 the request if for the loader file, if 1 the request
+ * is for a f/w file.
+ *
+ * @param info a card_info_t structure containing versions information.
+ * Note that some members of the structure may not be initialised.
+ *
+ * @return \p NULL if the file is not available, or a pointer which contains
+ * OS specific information for the file (typically the contents of the file)
+ * that the HIP core uses when calling unifi_fw_read() and unifi_fw_read_stop()
+ *
+ * @ingroup upperedge
+ */
+void* unifi_fw_read_start(void *ospriv, s8 is_fw, const card_info_t *info);
+
+/**
+ *
+ * Ask the OS layer to return a portion from a f/w file.
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param arg the OS pointer returned by unifi_fw_read_start().
+ *
+ * @param offset the offset in the f/w file to read the read from.
+ *
+ * @param buf the buffer to store the returned data.
+ *
+ * @param len the size in bytes of the requested read.
+ *
+ * @ingroup upperedge
+ */
+s32 unifi_fw_read(void *ospriv, void *arg, u32 offset, void *buf, u32 len);
+
+/**
+ *
+ * Ask the OS layer to finish reading from a f/w file.
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param dlpriv the OS pointer returned by unifi_fw_read_start().
+ *
+ * @ingroup upperedge
+ */
+void unifi_fw_read_stop(void *ospriv, void *dlpriv);
+
+/**
+ *
+ * Ask OS layer for a handle to a dynamically allocated firmware buffer
+ * (primarily intended for production test images which may need conversion)
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param fwbuf pointer to dynamically allocated buffer
+ *
+ * @param len length of provided buffer in bytes
+ *
+ * @ingroup upperedge
+ */
+void* unifi_fw_open_buffer(void *ospriv, void *fwbuf, u32 len);
+
+/**
+ *
+ * Release a handle to a dynamically allocated firmware buffer
+ * (primarily intended for production test images which may need conversion)
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param fwbuf pointer to dynamically allocated buffer
+ *
+ * @ingroup upperedge
+ */
+void unifi_fw_close_buffer(void *ospriv, void *fwbuf);
+
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+/*
+ * Driver must provide these.
+ *
+ * A simple implementation will just call
+ * unifi_sys_traffic_protocol_ind() or unifi_sys_traffic_classification_ind()
+ * respectively. See sme_csr_userspace/sme_userspace.c.
+ */
+/**
+ *
+ * Indicates a detected packet of type packet_type.
+ * Typically, this information is processed by the SME so
+ * unifi_ta_indicate_protocol() needs to schedule a call to
+ * unifi_sys_traffic_protocol_ind().
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param packet_type the detected packet type.
+ *
+ * @param direction the direction of the packet (Rx, Tx).
+ *
+ * @param src_addr the source address of the packet.
+ *
+ * @ingroup upperedge
+ */
+void unifi_ta_indicate_protocol(void                              *ospriv,
+                                CsrWifiRouterCtrlTrafficPacketType packet_type,
+                                CsrWifiRouterCtrlProtocolDirection direction,
+                                const CsrWifiMacAddress           *src_addr);
+
+/**
+ *
+ * Indicates statistics for the sample data over a period.
+ * Typically, this information is processed by the SME so
+ * unifi_ta_indicate_sampling() needs to schedule a call to
+ * unifi_sys_traffic_sample_ind().
+ *
+ * @param ospriv the OS layer context.
+ *
+ * @param stats the pointer to the structure that contains the statistics.
+ *
+ * @ingroup upperedge
+ */
+void unifi_ta_indicate_sampling(void *ospriv, CsrWifiRouterCtrlTrafficStats *stats);
+void unifi_ta_indicate_l4stats(void     *ospriv,
+                               u32 rxTcpThroughput,
+                               u32 txTcpThroughput,
+                               u32 rxUdpThroughput,
+                               u32 txUdpThroughput);
+#endif
+
+void unifi_rx_queue_flush(void *ospriv);
+
+/**
+ * Call-out from the SDIO glue layer.
+ *
+ * The glue layer needs to call unifi_sdio_interrupt_handler() every time
+ * an interrupts occurs.
+ *
+ * @param card the HIP core context.
+ *
+ * @ingroup bottomedge
+ */
+void unifi_sdio_interrupt_handler(card_t *card);
+
+
+/* HELPER FUNCTIONS */
+
+/*
+ * unifi_init() and unifi_download() implement a subset of unifi_init_card functionality
+ * that excludes HIP initialization.
+ */
+CsrResult unifi_init(card_t *card);
+CsrResult unifi_download(card_t *card, s32 led_mask);
+
+/*
+ * unifi_start_processors() ensures both on-chip processors are running
+ */
+CsrResult unifi_start_processors(card_t *card);
+
+CsrResult unifi_capture_panic(card_t *card);
+
+/*
+ * Configure HIP interrupt processing mode
+ */
+#define CSR_WIFI_INTMODE_DEFAULT        0
+#define CSR_WIFI_INTMODE_RUN_BH_ONCE    1       /* Run BH once per interrupt */
+
+void unifi_set_interrupt_mode(card_t *card, u32 mode);
+
+/*
+ * unifi_request_max_clock() requests that max SDIO clock speed is set at the
+ * next suitable opportunity.
+ */
+void unifi_request_max_sdio_clock(card_t *card);
+
+
+/* Functions to lookup bulk data command names. */
+const char* lookup_bulkcmd_name(u16 id);
+
+/* Function to log HIP's global debug buffer */
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+void unifi_debug_buf_dump(void);
+void unifi_debug_log_to_buf(const char *fmt, ...);
+void unifi_debug_hex_to_buf(const char *buff, u16 length);
+#endif
+
+/* Mini-coredump utility functions */
+CsrResult unifi_coredump_get_value(card_t *card, struct unifi_coredump_req *req);
+CsrResult unifi_coredump_capture(card_t *card, struct unifi_coredump_req *req);
+CsrResult unifi_coredump_request_at_next_reset(card_t *card, s8 enable);
+CsrResult unifi_coredump_init(card_t *card, u16 num_dump_buffers);
+void unifi_coredump_free(card_t *card);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CSR_WIFI_HIP_UNIFI_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_unifi_signal_names.c b/drivers/staging/csr/csr_wifi_hip_unifi_signal_names.c
new file mode 100644
index 00000000000..7c13df295c1
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_unifi_signal_names.c
@@ -0,0 +1,46 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include "csr_wifi_hip_unifi.h"
+
+struct sig_name
+{
+    s16             id;
+    const char *name;
+};
+
+static const struct sig_name Unifi_bulkcmd_names[] = {
+    {  0, "SignalCmd" },
+    {  1, "CopyToHost" },
+    {  2, "CopyToHostAck" },
+    {  3, "CopyFromHost" },
+    {  4, "CopyFromHostAck" },
+    {  5, "ClearSlot" },
+    {  6, "CopyOverlay" },
+    {  7, "CopyOverlayAck" },
+    {  8, "CopyFromHostAndClearSlot" },
+    {  15, "Padding" }
+};
+
+const char* lookup_bulkcmd_name(u16 id)
+{
+    if (id < 9)
+    {
+        return Unifi_bulkcmd_names[id].name;
+    }
+    if (id == 15)
+    {
+        return "Padding";
+    }
+
+    return "UNKNOWN";
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_unifi_udi.h b/drivers/staging/csr/csr_wifi_hip_unifi_udi.h
new file mode 100644
index 00000000000..83032d0b4ec
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_unifi_udi.h
@@ -0,0 +1,76 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     csr_wifi_hip_unifi_udi.h
+ *
+ *  PURPOSE:
+ *      Declarations and definitions for the UniFi Debug Interface.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __CSR_WIFI_HIP_UNIFI_UDI_H__
+#define __CSR_WIFI_HIP_UNIFI_UDI_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_signals.h"
+
+
+/*
+ * Support for tracing the wire protocol.
+ */
+enum udi_log_direction
+{
+    UDI_LOG_FROM_HOST   = 0x0000,
+    UDI_LOG_TO_HOST     = 0x0001
+};
+
+typedef void (*udi_func_t)(void *ospriv, u8 *sigdata,
+                           u32 signal_len,
+                           const bulk_data_param_t *bulkdata,
+                           enum udi_log_direction dir);
+
+CsrResult unifi_set_udi_hook(card_t *card, udi_func_t udi_fn);
+CsrResult unifi_remove_udi_hook(card_t *card, udi_func_t udi_fn);
+
+
+/*
+ * Function to print current status info to a string.
+ * This is used in the linux /proc interface and might be useful
+ * in other systems.
+ */
+s32 unifi_print_status(card_t *card, char *str, s32 *remain);
+
+#define UNIFI_SNPRINTF_RET(buf_p, remain, written)                  \
+    do {                                                            \
+        if (written >= remain) {                                    \
+            if (remain >= 2) {                                      \
+                buf_p[remain - 2] = '\n';                           \
+                buf_p[remain - 1] = 0;                              \
+            }                                                       \
+            buf_p += remain;                                        \
+            remain = 0;                                             \
+        } else if (written > 0) {                                   \
+            buf_p += written;                                       \
+            remain -= written;                                      \
+        }                                                           \
+    } while (0)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CSR_WIFI_HIP_UNIFI_UDI_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_unifihw.h b/drivers/staging/csr/csr_wifi_hip_unifihw.h
new file mode 100644
index 00000000000..5ffd6ba38d3
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_unifihw.h
@@ -0,0 +1,67 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * File: csr_wifi_hip_unifihw.h
+ *
+ *      Definitions of various chip registers, addresses, values etc.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __UNIFIHW_H__
+#define __UNIFIHW_H__ 1
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Symbol Look Up Table fingerprint. IDs are in sigs.h */
+#define SLUT_FINGERPRINT        0xD397
+
+
+/* Values of LoaderOperation */
+#define UNIFI_LOADER_IDLE       0x00
+#define UNIFI_LOADER_COPY       0x01
+#define UNIFI_LOADER_ERROR_MASK 0xF0
+
+/* Values of BootLoaderOperation */
+#define UNIFI_BOOT_LOADER_IDLE       0x00
+#define UNIFI_BOOT_LOADER_RESTART    0x01
+#define UNIFI_BOOT_LOADER_PATCH      0x02
+#define UNIFI_BOOT_LOADER_LOAD_STA   0x10
+#define UNIFI_BOOT_LOADER_LOAD_PTEST 0x11
+
+
+/* Memory spaces encoded in top byte of Generic Pointer type */
+#define UNIFI_SH_DMEM   0x01    /* Shared Data Memory */
+#define UNIFI_EXT_FLASH 0x02    /* External FLASH */
+#define UNIFI_EXT_SRAM  0x03    /* External SRAM */
+#define UNIFI_REGISTERS 0x04    /* Registers */
+#define UNIFI_PHY_DMEM  0x10    /* PHY Data Memory */
+#define UNIFI_PHY_PMEM  0x11    /* PHY Program Memory */
+#define UNIFI_PHY_ROM   0x12    /* PHY ROM */
+#define UNIFI_MAC_DMEM  0x20    /* MAC Data Memory */
+#define UNIFI_MAC_PMEM  0x21    /* MAC Program Memory */
+#define UNIFI_MAC_ROM   0x22    /* MAC ROM */
+#define UNIFI_BT_DMEM   0x30    /* BT Data Memory */
+#define UNIFI_BT_PMEM   0x31    /* BT Program Memory */
+#define UNIFI_BT_ROM    0x32    /* BT ROM */
+
+#define UNIFI_MAKE_GP(R, O)  (((UNIFI_ ## R) << 24) | (O))
+#define UNIFI_GP_OFFSET(GP)  ((GP) & 0xFFFFFF)
+#define UNIFI_GP_SPACE(GP)   (((GP) >> 24) & 0xFF)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UNIFIHW_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_unifiversion.h b/drivers/staging/csr/csr_wifi_hip_unifiversion.h
new file mode 100644
index 00000000000..e1fdbb27a46
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_unifiversion.h
@@ -0,0 +1,38 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     unifiversion.h
+ *
+ *  PURPOSE:
+ *      Version information for the portable UniFi driver.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#ifndef __UNIFIVERSION_H__
+#define __UNIFIVERSION_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * The minimum version of Host Interface Protocol required by the driver.
+ */
+#define UNIFI_HIP_MAJOR_VERSION 9
+#define UNIFI_HIP_MINOR_VERSION 1
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UNIFIVERSION_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hip_xbv.c b/drivers/staging/csr/csr_wifi_hip_xbv.c
new file mode 100644
index 00000000000..071f80a49f1
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_xbv.c
@@ -0,0 +1,1076 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ * FILE: csr_wifi_hip_xbv.c
+ *
+ * PURPOSE:
+ *      Routines for downloading firmware to UniFi.
+ *
+ *      UniFi firmware files use a nested TLV (Tag-Length-Value) format.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/slab.h>
+
+#ifdef CSR_WIFI_XBV_TEST
+/* Standalone test harness */
+#include "unifi_xbv.h"
+#include "csr_wifi_hip_unifihw.h"
+#else
+/* Normal driver build */
+#include "csr_wifi_hip_unifiversion.h"
+#include "csr_wifi_hip_card.h"
+#define DBG_TAG(t)
+#endif
+
+#include "csr_wifi_hip_xbv.h"
+
+#define STREAM_CHECKSUM 0x6d34        /* Sum of uint16s in each patch stream */
+
+/* XBV sizes used in patch conversion
+ */
+#define PTDL_MAX_SIZE 2048            /* Max bytes allowed per PTDL */
+#define PTDL_HDR_SIZE (4 + 2 + 6 + 2) /* sizeof(fw_id, sec_len, patch_cmd, csum) */
+
+/* Struct to represent a buffer for reading firmware file */
+
+typedef struct
+{
+    void      *dlpriv;
+    s32   ioffset;
+    fwreadfn_t iread;
+} ct_t;
+
+/* Struct to represent a TLV field */
+typedef struct
+{
+    char t_name[4];
+    u32     t_len;
+} tag_t;
+
+
+#define TAG_EQ(i, v)    (((i)[0] == (v)[0]) &&  \
+                         ((i)[1] == (v)[1]) &&  \
+                         ((i)[2] == (v)[2]) &&  \
+                         ((i)[3] == (v)[3]))
+
+/* We create a small stack on the stack that contains an enum
+ * indicating the containing list segments, and the offset at which
+ * those lists end.  This enables a lot more error checking. */
+typedef enum
+{
+    xbv_xbv1,
+    /*xbv_info,*/
+    xbv_fw,
+    xbv_vers,
+    xbv_vand,
+    xbv_ptch,
+    xbv_other
+} xbv_container;
+
+#define XBV_STACK_SIZE 6
+#define XBV_MAX_OFFS   0x7fffffff
+
+typedef struct
+{
+    struct
+    {
+        xbv_container container;
+        s32      ioffset_end;
+    } s[XBV_STACK_SIZE];
+    u32 ptr;
+} xbv_stack_t;
+
+static s32 read_tag(card_t *card, ct_t *ct, tag_t *tag);
+static s32 read_bytes(card_t *card, ct_t *ct, void *buf, u32 len);
+static s32 read_uint(card_t *card, ct_t *ct, u32 *u, u32 len);
+static s32 xbv_check(xbv1_t *fwinfo, const xbv_stack_t *stack,
+                          xbv_mode new_mode, xbv_container old_cont);
+static s32 xbv_push(xbv1_t *fwinfo, xbv_stack_t *stack,
+                         xbv_mode new_mode, xbv_container old_cont,
+                         xbv_container new_cont, u32 ioff);
+
+static u32 write_uint16(void *buf, const u32 offset,
+                              const u16 val);
+static u32 write_uint32(void *buf, const u32 offset,
+                              const u32 val);
+static u32 write_bytes(void *buf, const u32 offset,
+                             const u8 *data, const u32 len);
+static u32 write_tag(void *buf, const u32 offset,
+                           const char *tag_str);
+static u32 write_chunk(void *buf, const u32 offset,
+                             const char *tag_str,
+                             const u32 payload_len);
+static u16 calc_checksum(void *buf, const u32 offset,
+                               const u32 bytes_len);
+static u32 calc_patch_size(const xbv1_t *fwinfo);
+
+static u32 write_xbv_header(void *buf, const u32 offset,
+                                  const u32 file_payload_length);
+static u32 write_ptch_header(void *buf, const u32 offset,
+                                   const u32 fw_id);
+static u32 write_patchcmd(void *buf, const u32 offset,
+                                const u32 dst_genaddr, const u16 len);
+static u32 write_reset_ptdl(void *buf, const u32 offset,
+                                  const xbv1_t *fwinfo, u32 fw_id);
+static u32 write_fwdl_to_ptdl(void *buf, const u32 offset,
+                                    fwreadfn_t readfn, const struct FWDL *fwdl,
+                                    const void *fw_buf, const u32 fw_id,
+                                    void *rdbuf);
+
+/*
+ * ---------------------------------------------------------------------------
+ *  parse_xbv1
+ *
+ *      Scan the firmware file to find the TLVs we are interested in.
+ *      Actions performed:
+ *        - check we support the file format version in VERF
+ *      Store these TLVs if we have a firmware image:
+ *        - SLTP Symbol Lookup Table Pointer
+ *        - FWDL firmware download segments
+ *        - FWOL firmware overlay segment
+ *        - VMEQ Register probe tests to verify matching h/w
+ *      Store these TLVs if we have a patch file:
+ *        - FWID the firmware build ID that this file patches
+ *        - PTDL The actual patches
+ *
+ *      The structure pointed to by fwinfo is cleared and
+ *      'fwinfo->mode' is set to 'unknown'.  The 'fwinfo->mode'
+ *      variable is set to 'firmware' or 'patch' once we know which
+ *      sort of XBV file we have.
+ *
+ *  Arguments:
+ *      readfn          Pointer to function to call to read from the file.
+ *      dlpriv          Opaque pointer arg to pass to readfn.
+ *      fwinfo          Pointer to fwinfo struct to fill in.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR error code on failure
+ * ---------------------------------------------------------------------------
+ */
+CsrResult xbv1_parse(card_t *card, fwreadfn_t readfn, void *dlpriv, xbv1_t *fwinfo)
+{
+    ct_t ct;
+    tag_t tag;
+    xbv_stack_t stack;
+
+    ct.dlpriv = dlpriv;
+    ct.ioffset = 0;
+    ct.iread = readfn;
+
+    memset(fwinfo, 0, sizeof(xbv1_t));
+    fwinfo->mode = xbv_unknown;
+
+    /* File must start with XBV1 triplet */
+    if (read_tag(card, &ct, &tag) <= 0)
+    {
+        unifi_error(NULL, "File is not UniFi firmware\n");
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    DBG_TAG(tag.t_name);
+
+    if (!TAG_EQ(tag.t_name, "XBV1"))
+    {
+        unifi_error(NULL, "File is not UniFi firmware (%s)\n", tag.t_name);
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    stack.ptr = 0;
+    stack.s[stack.ptr].container = xbv_xbv1;
+    stack.s[stack.ptr].ioffset_end = XBV_MAX_OFFS;
+
+    /* Now scan the file */
+    while (1)
+    {
+        s32 n;
+
+        n = read_tag(card, &ct, &tag);
+        if (n < 0)
+        {
+            unifi_error(NULL, "No tag\n");
+            return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+        }
+        if (n == 0)
+        {
+            /* End of file */
+            break;
+        }
+
+        DBG_TAG(tag.t_name);
+
+        /* File format version */
+        if (TAG_EQ(tag.t_name, "VERF"))
+        {
+            u32 version;
+
+            if (xbv_check(fwinfo, &stack, xbv_unknown, xbv_xbv1) ||
+                (tag.t_len != 2) ||
+                read_uint(card, &ct, &version, 2))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            if (version != 0)
+            {
+                unifi_error(NULL, "Unsupported firmware file version: %d.%d\n",
+                            version >> 8, version & 0xFF);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+        }
+        else if (TAG_EQ(tag.t_name, "LIST"))
+        {
+            char name[4];
+            u32 list_end;
+
+            list_end = ct.ioffset + tag.t_len;
+
+            if (read_bytes(card, &ct, name, 4))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            DBG_TAG(name);
+            if (TAG_EQ(name, "FW  "))
+            {
+                if (xbv_push(fwinfo, &stack, xbv_firmware, xbv_xbv1, xbv_fw, list_end))
+                {
+                    return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+                }
+            }
+            else if (TAG_EQ(name, "VERS"))
+            {
+                if (xbv_push(fwinfo, &stack, xbv_firmware, xbv_fw, xbv_vers, list_end) ||
+                    (fwinfo->vers.num_vand != 0))
+                {
+                    return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+                }
+            }
+            else if (TAG_EQ(name, "VAND"))
+            {
+                struct VAND *vand;
+
+                if (xbv_push(fwinfo, &stack, xbv_firmware, xbv_vers, xbv_vand, list_end) ||
+                    (fwinfo->vers.num_vand >= MAX_VAND))
+                {
+                    return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+                }
+
+                /* Get a new VAND */
+                vand = fwinfo->vand + fwinfo->vers.num_vand++;
+
+                /* Fill it in */
+                vand->first = fwinfo->num_vmeq;
+                vand->count = 0;
+            }
+            else if (TAG_EQ(name, "PTCH"))
+            {
+                if (xbv_push(fwinfo, &stack, xbv_patch, xbv_xbv1, xbv_ptch, list_end))
+                {
+                    return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+                }
+            }
+            else
+            {
+                /* Skip over any other lists.  We dont bother to push
+                 * the new list type now as we would only pop it at
+                 * the end of the outer loop. */
+                ct.ioffset += tag.t_len - 4;
+            }
+        }
+        else if (TAG_EQ(tag.t_name, "SLTP"))
+        {
+            u32 addr;
+
+            if (xbv_check(fwinfo, &stack, xbv_firmware, xbv_fw) ||
+                (tag.t_len != 4) ||
+                (fwinfo->slut_addr != 0) ||
+                read_uint(card, &ct, &addr, 4))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            fwinfo->slut_addr = addr;
+        }
+        else if (TAG_EQ(tag.t_name, "FWDL"))
+        {
+            u32 addr;
+            struct FWDL *fwdl;
+
+            if (xbv_check(fwinfo, &stack, xbv_firmware, xbv_fw) ||
+                (fwinfo->num_fwdl >= MAX_FWDL) ||
+                (read_uint(card, &ct, &addr, 4)))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            fwdl = fwinfo->fwdl + fwinfo->num_fwdl++;
+
+            fwdl->dl_size = tag.t_len - 4;
+            fwdl->dl_addr = addr;
+            fwdl->dl_offset = ct.ioffset;
+
+            ct.ioffset += tag.t_len - 4;
+        }
+        else if (TAG_EQ(tag.t_name, "FWOV"))
+        {
+            if (xbv_check(fwinfo, &stack, xbv_firmware, xbv_fw) ||
+                (fwinfo->fwov.dl_size != 0) ||
+                (fwinfo->fwov.dl_offset != 0))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            fwinfo->fwov.dl_size = tag.t_len;
+            fwinfo->fwov.dl_offset = ct.ioffset;
+
+            ct.ioffset += tag.t_len;
+        }
+        else if (TAG_EQ(tag.t_name, "VMEQ"))
+        {
+            u32 temp[3];
+            struct VAND *vand;
+            struct VMEQ *vmeq;
+
+            if (xbv_check(fwinfo, &stack, xbv_firmware, xbv_vand) ||
+                (fwinfo->num_vmeq >= MAX_VMEQ) ||
+                (fwinfo->vers.num_vand == 0) ||
+                (tag.t_len != 8) ||
+                read_uint(card, &ct, &temp[0], 4) ||
+                read_uint(card, &ct, &temp[1], 2) ||
+                read_uint(card, &ct, &temp[2], 2))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            /* Get the last VAND */
+            vand = fwinfo->vand + (fwinfo->vers.num_vand - 1);
+
+            /* Get a new VMEQ */
+            vmeq = fwinfo->vmeq + fwinfo->num_vmeq++;
+
+            /* Note that this VAND contains another VMEQ */
+            vand->count++;
+
+            /* Fill in the VMEQ */
+            vmeq->addr = temp[0];
+            vmeq->mask = (u16)temp[1];
+            vmeq->value = (u16)temp[2];
+        }
+        else if (TAG_EQ(tag.t_name, "FWID"))
+        {
+            u32 build_id;
+
+            if (xbv_check(fwinfo, &stack, xbv_patch, xbv_ptch) ||
+                (tag.t_len != 4) ||
+                (fwinfo->build_id != 0) ||
+                read_uint(card, &ct, &build_id, 4))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            fwinfo->build_id = build_id;
+        }
+        else if (TAG_EQ(tag.t_name, "PTDL"))
+        {
+            struct PTDL *ptdl;
+
+            if (xbv_check(fwinfo, &stack, xbv_patch, xbv_ptch) ||
+                (fwinfo->num_ptdl >= MAX_PTDL))
+            {
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+
+            /* Allocate a new PTDL */
+            ptdl = fwinfo->ptdl + fwinfo->num_ptdl++;
+
+            ptdl->dl_size = tag.t_len;
+            ptdl->dl_offset = ct.ioffset;
+
+            ct.ioffset += tag.t_len;
+        }
+        else
+        {
+            /*
+             * If we get here it is a tag we are not interested in,
+             * just skip over it.
+             */
+            ct.ioffset += tag.t_len;
+        }
+
+        /* Check to see if we are at the end of the currently stacked
+         * segment.  We could finish more than one list at a time. */
+        while (ct.ioffset >= stack.s[stack.ptr].ioffset_end)
+        {
+            if (ct.ioffset > stack.s[stack.ptr].ioffset_end)
+            {
+                unifi_error(NULL,
+                            "XBV file has overrun stack'd segment %d (%d > %d)\n",
+                            stack.ptr, ct.ioffset, stack.s[stack.ptr].ioffset_end);
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            if (stack.ptr <= 0)
+            {
+                unifi_error(NULL, "XBV file has underrun stack pointer\n");
+                return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+            }
+            stack.ptr--;
+        }
+    }
+
+    if (stack.ptr != 0)
+    {
+        unifi_error(NULL, "Last list of XBV is not complete.\n");
+        return CSR_WIFI_HIP_RESULT_INVALID_VALUE;
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* xbv1_parse() */
+
+
+/* Check the the XBV file is of a consistant sort (either firmware or
+ * patch) and that we are in the correct containing list type. */
+static s32 xbv_check(xbv1_t *fwinfo, const xbv_stack_t *stack,
+                          xbv_mode new_mode, xbv_container old_cont)
+{
+    /* If the new file mode is unknown the current packet could be in
+     * either (any) type of XBV file, and we cant make a decission at
+     * this time. */
+    if (new_mode != xbv_unknown)
+    {
+        if (fwinfo->mode == xbv_unknown)
+        {
+            fwinfo->mode = new_mode;
+        }
+        else if (fwinfo->mode != new_mode)
+        {
+            return -1;
+        }
+    }
+    /* If the current stack top doesn't match what we expect then the
+     * file is corrupt. */
+    if (stack->s[stack->ptr].container != old_cont)
+    {
+        return -1;
+    }
+    return 0;
+}
+
+
+/* Make checks as above and then enter a new list */
+static s32 xbv_push(xbv1_t *fwinfo, xbv_stack_t *stack,
+                         xbv_mode new_mode, xbv_container old_cont,
+                         xbv_container new_cont, u32 new_ioff)
+{
+    if (xbv_check(fwinfo, stack, new_mode, old_cont))
+    {
+        return -1;
+    }
+
+    /* Check that our stack won't overflow. */
+    if (stack->ptr >= (XBV_STACK_SIZE - 1))
+    {
+        return -1;
+    }
+
+    /* Add the new list element to the top of the stack. */
+    stack->ptr++;
+    stack->s[stack->ptr].container = new_cont;
+    stack->s[stack->ptr].ioffset_end = new_ioff;
+
+    return 0;
+}
+
+
+static u32 xbv2uint(u8 *ptr, s32 len)
+{
+    u32 u = 0;
+    s16 i;
+
+    for (i = 0; i < len; i++)
+    {
+        u32 b;
+        b = ptr[i];
+        u += b << (i * 8);
+    }
+    return u;
+}
+
+
+static s32 read_tag(card_t *card, ct_t *ct, tag_t *tag)
+{
+    u8 buf[8];
+    s32 n;
+
+    n = (*ct->iread)(card->ospriv, ct->dlpriv, ct->ioffset, buf, 8);
+    if (n <= 0)
+    {
+        return n;
+    }
+
+    /* read the tag and length */
+    if (n != 8)
+    {
+        return -1;
+    }
+
+    /* get section tag */
+    memcpy(tag->t_name, buf, 4);
+
+    /* get section length */
+    tag->t_len = xbv2uint(buf + 4, 4);
+
+    ct->ioffset += 8;
+
+    return 8;
+} /* read_tag() */
+
+
+static s32 read_bytes(card_t *card, ct_t *ct, void *buf, u32 len)
+{
+    /* read the tag value */
+    if ((*ct->iread)(card->ospriv, ct->dlpriv, ct->ioffset, buf, len) != (s32)len)
+    {
+        return -1;
+    }
+
+    ct->ioffset += len;
+
+    return 0;
+} /* read_bytes() */
+
+
+static s32 read_uint(card_t *card, ct_t *ct, u32 *u, u32 len)
+{
+    u8 buf[4];
+
+    /* Integer cannot be more than 4 bytes */
+    if (len > 4)
+    {
+        return -1;
+    }
+
+    if (read_bytes(card, ct, buf, len))
+    {
+        return -1;
+    }
+
+    *u = xbv2uint(buf, len);
+
+    return 0;
+} /* read_uint() */
+
+
+static u32 write_uint16(void *buf, const u32 offset, const u16 val)
+{
+    u8 *dst = (u8 *)buf + offset;
+    *dst++ = (u8)(val & 0xff); /* LSB first */
+    *dst = (u8)(val >> 8);
+    return sizeof(u16);
+}
+
+
+static u32 write_uint32(void *buf, const u32 offset, const u32 val)
+{
+    (void)write_uint16(buf, offset + 0, (u16)(val & 0xffff));
+    (void)write_uint16(buf, offset + 2, (u16)(val >> 16));
+    return sizeof(u32);
+}
+
+
+static u32 write_bytes(void *buf, const u32 offset, const u8 *data, const u32 len)
+{
+    u32 i;
+    u8 *dst = (u8 *)buf + offset;
+
+    for (i = 0; i < len; i++)
+    {
+        *dst++ = *((u8 *)data + i);
+    }
+    return len;
+}
+
+
+static u32 write_tag(void *buf, const u32 offset, const char *tag_str)
+{
+    u8 *dst = (u8 *)buf + offset;
+    memcpy(dst, tag_str, 4);
+    return 4;
+}
+
+
+static u32 write_chunk(void *buf, const u32 offset, const char *tag_str, const u32 payload_len)
+{
+    u32 written = 0;
+    written += write_tag(buf, offset, tag_str);
+    written += write_uint32(buf, written + offset, (u32)payload_len);
+
+    return written;
+}
+
+
+static u16 calc_checksum(void *buf, const u32 offset, const u32 bytes_len)
+{
+    u32 i;
+    u8 *src = (u8 *)buf + offset;
+    u16 sum = 0;
+    u16 val;
+
+    for (i = 0; i < bytes_len / 2; i++)
+    {
+        /* Contents copied to file is LE, host might not be */
+        val = (u16) * src++;         /* LSB */
+        val += (u16)(*src++) << 8;   /* MSB */
+        sum += val;
+    }
+
+    /* Total of uint16s in the stream plus the stored check value
+     * should equal STREAM_CHECKSUM when decoded.
+     */
+    return (STREAM_CHECKSUM - sum);
+}
+
+
+#define PTDL_RESET_DATA_SIZE  20  /* Size of reset vectors PTDL */
+
+static u32 calc_patch_size(const xbv1_t *fwinfo)
+{
+    s16 i;
+    u32 size = 0;
+
+    /*
+     * Work out how big an equivalent patch format file must be for this image.
+     * This only needs to be approximate, so long as it's large enough.
+     */
+    if (fwinfo->mode != xbv_firmware)
+    {
+        return 0;
+    }
+
+    /* Payload (which will get put into a series of PTDLs) */
+    for (i = 0; i < fwinfo->num_fwdl; i++)
+    {
+        size += fwinfo->fwdl[i].dl_size;
+    }
+
+    /* Another PTDL at the end containing reset vectors */
+    size += PTDL_RESET_DATA_SIZE;
+
+    /* PTDL headers. Add one for remainder, one for reset vectors */
+    size += ((fwinfo->num_fwdl / PTDL_MAX_SIZE) + 2) * PTDL_HDR_SIZE;
+
+    /* Another 1K sufficient to cover miscellaneous headers */
+    size += 1024;
+
+    return size;
+}
+
+
+static u32 write_xbv_header(void *buf, const u32 offset, const u32 file_payload_length)
+{
+    u32 written = 0;
+
+    /* The length value given to the XBV chunk is the length of all subsequent
+     * contents of the file, excluding the 8 byte size of the XBV1 header itself
+     * (The added 6 bytes thus accounts for the size of the VERF)
+     */
+    written += write_chunk(buf, offset + written, (char *)"XBV1", file_payload_length + 6);
+
+    written += write_chunk(buf, offset + written, (char *)"VERF", 2);
+    written += write_uint16(buf,  offset + written, 0);      /* File version */
+
+    return written;
+}
+
+
+static u32 write_ptch_header(void *buf, const u32 offset, const u32 fw_id)
+{
+    u32 written = 0;
+
+    /* LIST is written with a zero length, to be updated later */
+    written += write_chunk(buf, offset + written, (char *)"LIST", 0);
+    written += write_tag(buf, offset + written, (char *)"PTCH");        /* List type */
+
+    written += write_chunk(buf, offset + written, (char *)"FWID", 4);
+    written += write_uint32(buf, offset + written, fw_id);
+
+
+    return written;
+}
+
+
+#define UF_REGION_PHY  1
+#define UF_REGION_MAC  2
+#define UF_MEMPUT_MAC  0x0000
+#define UF_MEMPUT_PHY  0x1000
+
+static u32 write_patchcmd(void *buf, const u32 offset, const u32 dst_genaddr, const u16 len)
+{
+    u32 written = 0;
+    u32 region = (dst_genaddr >> 28);
+    u16 cmd_and_len = UF_MEMPUT_MAC;
+
+    if (region == UF_REGION_PHY)
+    {
+        cmd_and_len = UF_MEMPUT_PHY;
+    }
+    else if (region != UF_REGION_MAC)
+    {
+        return 0; /* invalid */
+    }
+
+    /* Write the command and data length */
+    cmd_and_len |= len;
+    written += write_uint16(buf, offset + written, cmd_and_len);
+
+    /* Write the destination generic address */
+    written += write_uint16(buf, offset + written, (u16)(dst_genaddr >> 16));
+    written += write_uint16(buf, offset + written, (u16)(dst_genaddr & 0xffff));
+
+    /* The data payload should be appended to the command */
+    return written;
+}
+
+
+static u32 write_fwdl_to_ptdl(void *buf, const u32 offset, fwreadfn_t readfn,
+                                    const struct FWDL *fwdl, const void *dlpriv,
+                                    const u32 fw_id, void *fw_buf)
+{
+    u32 written = 0;
+    s16 chunks = 0;
+    u32 left = fwdl->dl_size;      /* Bytes left in this fwdl */
+    u32 dl_addr = fwdl->dl_addr;   /* Target address of fwdl image on XAP */
+    u32 dl_offs = fwdl->dl_offset; /* Offset of fwdl image data in source */
+    u16 csum;
+    u32 csum_start_offs;           /* first offset to include in checksum */
+    u32 sec_data_len;              /* section data byte count */
+    u32 sec_len;                   /* section data + header byte count */
+
+    /* FWDL maps to one or more PTDLs, as max size for a PTDL is 1K words */
+    while (left)
+    {
+        /* Calculate amount to be transferred */
+        sec_data_len = CSRMIN(left, PTDL_MAX_SIZE - PTDL_HDR_SIZE);
+        sec_len = sec_data_len + PTDL_HDR_SIZE;
+
+        /* Write PTDL header + entire PTDL size */
+        written += write_chunk(buf, offset + written, (char *)"PTDL", sec_len);
+        /* bug digest implies 4 bytes of padding here, but that seems wrong */
+
+        /* Checksum starts here */
+        csum_start_offs = offset + written;
+
+        /* Patch-chunk header: fw_id. Note that this is in XAP word order */
+        written += write_uint16(buf, offset + written, (u16)(fw_id >> 16));
+        written += write_uint16(buf, offset + written, (u16)(fw_id & 0xffff));
+
+        /* Patch-chunk header: section length in uint16s */
+        written += write_uint16(buf, offset + written, (u16)(sec_len / 2));
+
+
+        /* Write the appropriate patch command for the data's destination ptr */
+        written += write_patchcmd(buf, offset + written, dl_addr, (u16)(sec_data_len / 2));
+
+        /* Write the data itself (limited to the max chunk length) */
+        if (readfn(NULL, (void *)dlpriv, dl_offs, fw_buf, sec_data_len) < 0)
+        {
+            return 0;
+        }
+
+        written += write_bytes(buf,
+                               offset + written,
+                               fw_buf,
+                               sec_data_len);
+
+        /* u16 checksum calculated over data written */
+        csum = calc_checksum(buf, csum_start_offs, written - (csum_start_offs - offset));
+        written += write_uint16(buf, offset + written, csum);
+
+        left -= sec_data_len;
+        dl_addr += sec_data_len;
+        dl_offs += sec_data_len;
+        chunks++;
+    }
+
+    return written;
+}
+
+
+#define SEC_CMD_LEN         ((4 + 2) * 2) /* sizeof(cmd, vector) per XAP */
+#define PTDL_VEC_HDR_SIZE   (4 + 2 + 2)   /* sizeof(fw_id, sec_len, csum) */
+#define UF_MAC_START_VEC    0x00c00000    /* Start address of image on MAC */
+#define UF_PHY_START_VEC    0x00c00000    /* Start address of image on PHY */
+#define UF_MAC_START_CMD    0x6000        /* MAC "Set start address" command */
+#define UF_PHY_START_CMD    0x7000        /* PHY "Set start address" command */
+
+static u32 write_reset_ptdl(void *buf, const u32 offset, const xbv1_t *fwinfo, u32 fw_id)
+{
+    u32 written = 0;
+    u16 csum;
+    u32 csum_start_offs;                 /* first offset to include in checksum */
+    u32 sec_len;                         /* section data + header byte count */
+
+    sec_len = SEC_CMD_LEN + PTDL_VEC_HDR_SIZE; /* Total section byte length */
+
+    /* Write PTDL header + entire PTDL size */
+    written += write_chunk(buf, offset + written, (char *)"PTDL", sec_len);
+
+    /* Checksum starts here */
+    csum_start_offs = offset + written;
+
+    /* Patch-chunk header: fw_id. Note that this is in XAP word order */
+    written += write_uint16(buf, offset + written, (u16)(fw_id >> 16));
+    written += write_uint16(buf, offset + written, (u16)(fw_id & 0xffff));
+
+    /* Patch-chunk header: section length in uint16s */
+    written += write_uint16(buf, offset + written, (u16)(sec_len / 2));
+
+    /*
+     * Restart addresses to be executed on subsequent loader restart command.
+     */
+
+    /* Setup the MAC start address, note word ordering */
+    written += write_uint16(buf, offset + written, UF_MAC_START_CMD);
+    written += write_uint16(buf, offset + written, (UF_MAC_START_VEC >> 16));
+    written += write_uint16(buf, offset + written, (UF_MAC_START_VEC & 0xffff));
+
+    /* Setup the PHY start address, note word ordering */
+    written += write_uint16(buf, offset + written, UF_PHY_START_CMD);
+    written += write_uint16(buf, offset + written, (UF_PHY_START_VEC >> 16));
+    written += write_uint16(buf, offset + written, (UF_PHY_START_VEC & 0xffff));
+
+    /* u16 checksum calculated over data written */
+    csum = calc_checksum(buf, csum_start_offs, written - (csum_start_offs - offset));
+    written += write_uint16(buf, offset + written, csum);
+
+    return written;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  read_slut
+ *
+ *      desc
+ *
+ *  Arguments:
+ *      readfn          Pointer to function to call to read from the file.
+ *      dlpriv          Opaque pointer arg to pass to readfn.
+ *      addr            Offset into firmware image of SLUT.
+ *      fwinfo          Pointer to fwinfo struct to fill in.
+ *
+ *  Returns:
+ *      Number of SLUT entries in the f/w, or -1 if the image was corrupt.
+ * ---------------------------------------------------------------------------
+ */
+s32 xbv1_read_slut(card_t *card, fwreadfn_t readfn, void *dlpriv, xbv1_t *fwinfo,
+                        symbol_t *slut, u32 slut_len)
+{
+    s16 i;
+    s32 offset;
+    u32 magic;
+    u32 count = 0;
+    ct_t ct;
+
+    if (fwinfo->mode != xbv_firmware)
+    {
+        return -1;
+    }
+
+    /* Find the d/l segment containing the SLUT */
+    /* This relies on the SLUT being entirely contained in one segment */
+    offset = -1;
+    for (i = 0; i < fwinfo->num_fwdl; i++)
+    {
+        if ((fwinfo->slut_addr >= fwinfo->fwdl[i].dl_addr) &&
+            (fwinfo->slut_addr < (fwinfo->fwdl[i].dl_addr + fwinfo->fwdl[i].dl_size)))
+        {
+            offset = fwinfo->fwdl[i].dl_offset +
+                     (fwinfo->slut_addr - fwinfo->fwdl[i].dl_addr);
+        }
+    }
+    if (offset < 0)
+    {
+        return -1;
+    }
+
+    ct.dlpriv = dlpriv;
+    ct.ioffset = offset;
+    ct.iread = readfn;
+
+    if (read_uint(card, &ct, &magic, 2))
+    {
+        return -1;
+    }
+    if (magic != SLUT_FINGERPRINT)
+    {
+        return -1;
+    }
+
+    while (count < slut_len)
+    {
+        u32 id, obj;
+
+        /* Read Symbol Id */
+        if (read_uint(card, &ct, &id, 2))
+        {
+            return -1;
+        }
+
+        /* Check for end of table marker */
+        if (id == CSR_SLT_END)
+        {
+            break;
+        }
+
+        /* Read Symbol Value */
+        if (read_uint(card, &ct, &obj, 4))
+        {
+            return -1;
+        }
+
+        slut[count].id  = (u16)id;
+        slut[count].obj = obj;
+        count++;
+    }
+
+    return count;
+} /* read_slut() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  xbv_to_patch
+ *
+ *      Convert (the relevant parts of) a firmware xbv file into a patch xbv
+ *
+ *  Arguments:
+ *      card
+ *      fw_buf - pointer to xbv firmware image
+ *      fwinfo - structure describing the firmware image
+ *      size   - pointer to location into which size of f/w is written.
+ *
+ *  Returns:
+ *      Pointer to firmware image, or NULL on error. Caller must free this
+ *      buffer via kfree() once it's finished with.
+ *
+ *  Notes:
+ *      The input fw_buf should have been checked via xbv1_parse prior to
+ *      calling this function, so the input image is assumed valid.
+ * ---------------------------------------------------------------------------
+ */
+#define PTCH_LIST_SIZE 16         /* sizeof PTCH+FWID chunk in LIST header */
+
+void* xbv_to_patch(card_t *card, fwreadfn_t readfn,
+                   const void *fw_buf, const xbv1_t *fwinfo, u32 *size)
+{
+    void *patch_buf = NULL;
+    u32 patch_buf_size;
+    u32 payload_offs = 0;           /* Start of XBV payload */
+    s16 i;
+    u32 patch_offs = 0;
+    u32 list_len_offs = 0;          /* Offset of PTDL LIST length parameter */
+    u32 ptdl_start_offs = 0;        /* Offset of first PTDL chunk */
+    u32 fw_id;
+    void *rdbuf;
+
+    if (!fw_buf || !fwinfo || !card)
+    {
+        return NULL;
+    }
+
+    if (fwinfo->mode != xbv_firmware)
+    {
+        unifi_error(NULL, "Not a firmware file\n");
+        return NULL;
+    }
+
+    /* Pre-allocate read buffer for chunk conversion */
+    rdbuf = kmalloc(PTDL_MAX_SIZE, GFP_KERNEL);
+    if (!rdbuf)
+    {
+        unifi_error(card, "Couldn't alloc conversion buffer\n");
+        return NULL;
+    }
+
+    /* Loader requires patch file's build ID to match the running firmware's */
+    fw_id = card->build_id;
+
+    /* Firmware XBV1 contains VERF, optional INFO, SLUT(s), FWDL(s)          */
+    /* Other chunks should get skipped.                                      */
+    /* VERF should be sanity-checked against chip version                    */
+
+    /* Patch    XBV1 contains VERF, optional INFO, PTCH                      */
+    /*          PTCH contains FWID, optional INFO, PTDL(s), PTDL(start_vec)  */
+    /* Each FWDL is split into PTDLs (each is 1024 XAP words max)            */
+    /* Each PTDL contains running ROM f/w version, and checksum              */
+    /* MAC/PHY reset addresses (known) are added into a final PTDL           */
+
+    /* The input image has already been parsed, and loaded into fwinfo, so we
+     * can use that to build the output image
+     */
+    patch_buf_size = calc_patch_size(fwinfo);
+
+    patch_buf = kmalloc(patch_buf_size, GFP_KERNEL);
+    if (!patch_buf)
+    {
+        kfree(rdbuf);
+        unifi_error(NULL, "Can't malloc buffer for patch conversion\n");
+        return NULL;
+    }
+
+    memset(patch_buf, 0xdd, patch_buf_size);
+
+    /* Write XBV + VERF headers */
+    patch_offs += write_xbv_header(patch_buf, patch_offs, 0);
+    payload_offs = patch_offs;
+
+    /* Write patch (LIST) header */
+    list_len_offs = patch_offs + 4;    /* Save LIST.length offset for later update */
+    patch_offs += write_ptch_header(patch_buf, patch_offs, fw_id);
+
+    /* Save start offset of the PTDL chunks */
+    ptdl_start_offs = patch_offs;
+
+    /* Write LIST of firmware PTDL blocks */
+    for (i = 0; i < fwinfo->num_fwdl; i++)
+    {
+        patch_offs += write_fwdl_to_ptdl(patch_buf,
+                                         patch_offs,
+                                         readfn,
+                                         &fwinfo->fwdl[i],
+                                         fw_buf,
+                                         fw_id,
+                                         rdbuf);
+    }
+
+    /* Write restart-vector PTDL last */
+    patch_offs += write_reset_ptdl(patch_buf, patch_offs, fwinfo, fw_id);
+
+    /* Now the length is known, update the LIST.length */
+    (void)write_uint32(patch_buf, list_len_offs,
+                       (patch_offs - ptdl_start_offs) + PTCH_LIST_SIZE);
+
+    /* Re write XBV headers just to fill in the correct file size */
+    (void)write_xbv_header(patch_buf, 0, (patch_offs - payload_offs));
+
+    unifi_trace(card->ospriv, UDBG1, "XBV:PTCH size %u, fw_id %u\n",
+                patch_offs, fw_id);
+    if (size)
+    {
+        *size = patch_offs;
+    }
+    kfree(rdbuf);
+
+    return patch_buf;
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_hip_xbv.h b/drivers/staging/csr/csr_wifi_hip_xbv.h
new file mode 100644
index 00000000000..9b60a7e2dc7
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hip_xbv.h
@@ -0,0 +1,127 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/*
+ * ---------------------------------------------------------------------------
+ * FILE: csr_wifi_hip_xbv.h
+ *
+ * PURPOSE:
+ *      Definitions and declarations for code to read XBV files - the UniFi
+ *      firmware download file format.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __XBV_H__
+#define __XBV_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_XBV_TEST
+/* Driver includes */
+#include "csr_wifi_hip_unifi.h"
+#endif
+
+
+struct VMEQ
+{
+    u32 addr;
+    u16 mask;
+    u16 value;
+};
+
+struct VAND
+{
+    u32 first;
+    u32 count;
+};
+
+struct VERS
+{
+    u32 num_vand;
+};
+
+struct FWDL
+{
+    u32 dl_addr;
+    u32 dl_size;
+    u32 dl_offset;
+};
+
+struct FWOV
+{
+    u32 dl_size;
+    u32 dl_offset;
+};
+
+struct PTDL
+{
+    u32 dl_size;
+    u32 dl_offset;
+};
+
+#define MAX_VMEQ 64
+#define MAX_VAND 64
+#define MAX_FWDL 256
+#define MAX_PTDL 256
+
+/* An XBV1 file can either contain firmware or patches (at the
+ * moment).  The 'mode' member of the xbv1_t structure tells us which
+ * one is the case. */
+typedef enum
+{
+    xbv_unknown,
+    xbv_firmware,
+    xbv_patch
+} xbv_mode;
+
+typedef struct
+{
+    xbv_mode mode;
+
+    /* Parts of a Firmware XBV1 */
+
+    struct VMEQ vmeq[MAX_VMEQ];
+    u32   num_vmeq;
+    struct VAND vand[MAX_VAND];
+    struct VERS vers;
+
+    u32 slut_addr;
+
+    /* F/W download image, possibly more than one part */
+    struct FWDL fwdl[MAX_FWDL];
+    s16    num_fwdl;
+
+    /* F/W overlay image, add r not used */
+    struct FWOV fwov;
+
+    /* Parts of a Patch XBV1 */
+
+    u32 build_id;
+
+    struct PTDL ptdl[MAX_PTDL];
+    s16    num_ptdl;
+}  xbv1_t;
+
+
+typedef s32 (*fwreadfn_t)(void *ospriv, void *dlpriv, u32 offset, void *buf, u32 len);
+
+CsrResult xbv1_parse(card_t *card, fwreadfn_t readfn, void *dlpriv, xbv1_t *fwinfo);
+s32 xbv1_read_slut(card_t *card, fwreadfn_t readfn, void *dlpriv, xbv1_t *fwinfo,
+                        symbol_t *slut, u32 slut_len);
+void* xbv_to_patch(card_t *card, fwreadfn_t readfn, const void *fw_buf, const xbv1_t *fwinfo,
+                   u32 *size);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __XBV_H__ */
diff --git a/drivers/staging/csr/csr_wifi_hostio_prim.h b/drivers/staging/csr/csr_wifi_hostio_prim.h
new file mode 100644
index 00000000000..bf7c55c6e84
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_hostio_prim.h
@@ -0,0 +1,27 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+
+#ifndef CSR_WIFI_HOSTIO_H
+#define CSR_WIFI_HOSTIO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define CSR_WIFI_HOSTIO_PRIM 0x0453
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_HOSTIO_H */
+
diff --git a/drivers/staging/csr/csr_wifi_lib.h b/drivers/staging/csr/csr_wifi_lib.h
new file mode 100644
index 00000000000..eb56f620887
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_lib.h
@@ -0,0 +1,112 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+#ifndef CSR_WIFI_LIB_H__
+#define CSR_WIFI_LIB_H__
+
+#include "csr_wifi_fsm_event.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiFsmEventInit
+ *
+ *  DESCRIPTION
+ *      Macro to initialise the members of a CsrWifiFsmEvent.
+ *----------------------------------------------------------------------------*/
+#define CsrWifiFsmEventInit(evt, p_primtype, p_msgtype, p_dst, p_src) \
+    (evt)->primtype = p_primtype; \
+    (evt)->type = p_msgtype; \
+    (evt)->destination = p_dst; \
+    (evt)->source = p_src
+
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiEvent_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrWifiEvent
+ *
+ *----------------------------------------------------------------------------*/
+CsrWifiFsmEvent* CsrWifiEvent_struct(u16 primtype, u16 msgtype, CsrSchedQid dst, CsrSchedQid src);
+
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u8        value;
+} CsrWifiEventCsrUint8;
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiEventCsrUint8_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrWifiEventCsrUint8
+ *
+ *----------------------------------------------------------------------------*/
+CsrWifiEventCsrUint8* CsrWifiEventCsrUint8_struct(u16 primtype, u16 msgtype, CsrSchedQid dst, CsrSchedQid src, u8 value);
+
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       value;
+} CsrWifiEventCsrUint16;
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiEventCsrUint16_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrWifiEventCsrUint16
+ *
+ *----------------------------------------------------------------------------*/
+CsrWifiEventCsrUint16* CsrWifiEventCsrUint16_struct(u16 primtype, u16 msgtype, CsrSchedQid dst, CsrSchedQid src, u16 value);
+
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u32       value;
+} CsrWifiEventCsrUint32;
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiEventCsrUint32_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrWifiEventCsrUint32
+ *
+ *----------------------------------------------------------------------------*/
+CsrWifiEventCsrUint32* CsrWifiEventCsrUint32_struct(u16 primtype, u16 msgtype, CsrSchedQid dst, CsrSchedQid src, u32 value);
+
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       value16;
+    u8        value8;
+} CsrWifiEventCsrUint16CsrUint8;
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiEventCsrUint16CsrUint8_struct
+ *
+ *  DESCRIPTION
+ *      Generic message creator.
+ *      Allocates and fills in a message with the signature CsrWifiEventCsrUint16CsrUint8
+ *
+ *----------------------------------------------------------------------------*/
+CsrWifiEventCsrUint16CsrUint8* CsrWifiEventCsrUint16CsrUint8_struct(u16 primtype, u16 msgtype, CsrSchedQid dst, CsrSchedQid src, u16 value16, u8 value8);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_msgconv.h b/drivers/staging/csr/csr_wifi_msgconv.h
new file mode 100644
index 00000000000..7ec35d70e14
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_msgconv.h
@@ -0,0 +1,58 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_MSGCONV_H__
+#define CSR_WIFI_MSGCONV_H__
+
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+void CsrUint16SerBigEndian(u8 *ptr, size_t *len, u16 v);
+void CsrUint24SerBigEndian(u8 *ptr, size_t *len, u32 v);
+void CsrUint32SerBigEndian(u8 *ptr, size_t *len, u32 v);
+
+void CsrUint16DesBigEndian(u16 *v, u8 *buffer, size_t *offset);
+void CsrUint24DesBigEndian(u32 *v, u8 *buffer, size_t *offset);
+void CsrUint32DesBigEndian(u32 *v, u8 *buffer, size_t *offset);
+
+void CsrUint24Ser(u8 *ptr, size_t *len, u32 v);
+void CsrUint24Des(u32 *v, u8 *buffer, size_t *offset);
+
+
+size_t CsrWifiEventSizeof(void *msg);
+u8* CsrWifiEventSer(u8 *ptr, size_t *len, void *msg);
+void* CsrWifiEventDes(u8 *buffer, size_t length);
+
+size_t CsrWifiEventCsrUint8Sizeof(void *msg);
+u8* CsrWifiEventCsrUint8Ser(u8 *ptr, size_t *len, void *msg);
+void* CsrWifiEventCsrUint8Des(u8 *buffer, size_t length);
+
+size_t CsrWifiEventCsrUint16Sizeof(void *msg);
+u8* CsrWifiEventCsrUint16Ser(u8 *ptr, size_t *len, void *msg);
+void* CsrWifiEventCsrUint16Des(u8 *buffer, size_t length);
+
+size_t CsrWifiEventCsrUint32Sizeof(void *msg);
+u8* CsrWifiEventCsrUint32Ser(u8 *ptr, size_t *len, void *msg);
+void* CsrWifiEventCsrUint32Des(u8 *buffer, size_t length);
+
+size_t CsrWifiEventCsrUint16CsrUint8Sizeof(void *msg);
+u8* CsrWifiEventCsrUint16CsrUint8Ser(u8 *ptr, size_t *len, void *msg);
+void* CsrWifiEventCsrUint16CsrUint8Des(u8 *buffer, size_t length);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_MSGCONV_H__ */
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_converter_init.c b/drivers/staging/csr/csr_wifi_nme_ap_converter_init.c
new file mode 100644
index 00000000000..0689d6f1cab
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_converter_init.c
@@ -0,0 +1,90 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#include "csr_msgconv.h"
+#include "csr_macro.h"
+
+#ifdef CSR_WIFI_NME_ENABLE
+#ifdef CSR_WIFI_AP_ENABLE
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_NME_AP_MODULE
+#include "csr_wifi_nme_ap_serialize.h"
+#include "csr_wifi_nme_ap_prim.h"
+
+static CsrMsgConvMsgEntry csrwifinmeap_conv_lut[] = {
+    { CSR_WIFI_NME_AP_CONFIG_SET_REQ, CsrWifiNmeApConfigSetReqSizeof, CsrWifiNmeApConfigSetReqSer, CsrWifiNmeApConfigSetReqDes, CsrWifiNmeApConfigSetReqSerFree },
+    { CSR_WIFI_NME_AP_WPS_REGISTER_REQ, CsrWifiNmeApWpsRegisterReqSizeof, CsrWifiNmeApWpsRegisterReqSer, CsrWifiNmeApWpsRegisterReqDes, CsrWifiNmeApWpsRegisterReqSerFree },
+    { CSR_WIFI_NME_AP_START_REQ, CsrWifiNmeApStartReqSizeof, CsrWifiNmeApStartReqSer, CsrWifiNmeApStartReqDes, CsrWifiNmeApStartReqSerFree },
+    { CSR_WIFI_NME_AP_STOP_REQ, CsrWifiNmeApStopReqSizeof, CsrWifiNmeApStopReqSer, CsrWifiNmeApStopReqDes, CsrWifiNmeApStopReqSerFree },
+    { CSR_WIFI_NME_AP_WMM_PARAM_UPDATE_REQ, CsrWifiNmeApWmmParamUpdateReqSizeof, CsrWifiNmeApWmmParamUpdateReqSer, CsrWifiNmeApWmmParamUpdateReqDes, CsrWifiNmeApWmmParamUpdateReqSerFree },
+    { CSR_WIFI_NME_AP_STA_REMOVE_REQ, CsrWifiNmeApStaRemoveReqSizeof, CsrWifiNmeApStaRemoveReqSer, CsrWifiNmeApStaRemoveReqDes, CsrWifiNmeApStaRemoveReqSerFree },
+    { CSR_WIFI_NME_AP_CONFIG_SET_CFM, CsrWifiNmeApConfigSetCfmSizeof, CsrWifiNmeApConfigSetCfmSer, CsrWifiNmeApConfigSetCfmDes, CsrWifiNmeApConfigSetCfmSerFree },
+    { CSR_WIFI_NME_AP_WPS_REGISTER_CFM, CsrWifiNmeApWpsRegisterCfmSizeof, CsrWifiNmeApWpsRegisterCfmSer, CsrWifiNmeApWpsRegisterCfmDes, CsrWifiNmeApWpsRegisterCfmSerFree },
+    { CSR_WIFI_NME_AP_START_CFM, CsrWifiNmeApStartCfmSizeof, CsrWifiNmeApStartCfmSer, CsrWifiNmeApStartCfmDes, CsrWifiNmeApStartCfmSerFree },
+    { CSR_WIFI_NME_AP_STOP_CFM, CsrWifiNmeApStopCfmSizeof, CsrWifiNmeApStopCfmSer, CsrWifiNmeApStopCfmDes, CsrWifiNmeApStopCfmSerFree },
+    { CSR_WIFI_NME_AP_STOP_IND, CsrWifiNmeApStopIndSizeof, CsrWifiNmeApStopIndSer, CsrWifiNmeApStopIndDes, CsrWifiNmeApStopIndSerFree },
+    { CSR_WIFI_NME_AP_WMM_PARAM_UPDATE_CFM, CsrWifiNmeApWmmParamUpdateCfmSizeof, CsrWifiNmeApWmmParamUpdateCfmSer, CsrWifiNmeApWmmParamUpdateCfmDes, CsrWifiNmeApWmmParamUpdateCfmSerFree },
+    { CSR_WIFI_NME_AP_STATION_IND, CsrWifiNmeApStationIndSizeof, CsrWifiNmeApStationIndSer, CsrWifiNmeApStationIndDes, CsrWifiNmeApStationIndSerFree },
+
+    { 0, NULL, NULL, NULL, NULL },
+};
+
+CsrMsgConvMsgEntry* CsrWifiNmeApConverterLookup(CsrMsgConvMsgEntry *ce, u16 msgType)
+{
+    if (msgType & CSR_PRIM_UPSTREAM)
+    {
+        u16 idx = (msgType & ~CSR_PRIM_UPSTREAM) + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_COUNT;
+        if (idx < (CSR_WIFI_NME_AP_PRIM_UPSTREAM_COUNT + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_COUNT) &&
+            csrwifinmeap_conv_lut[idx].msgType == msgType)
+        {
+            return &csrwifinmeap_conv_lut[idx];
+        }
+    }
+    else
+    {
+        if (msgType < CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_COUNT &&
+            csrwifinmeap_conv_lut[msgType].msgType == msgType)
+        {
+            return &csrwifinmeap_conv_lut[msgType];
+        }
+    }
+    return NULL;
+}
+
+
+void CsrWifiNmeApConverterInit(void)
+{
+    CsrMsgConvInsert(CSR_WIFI_NME_AP_PRIM, csrwifinmeap_conv_lut);
+    CsrMsgConvCustomLookupRegister(CSR_WIFI_NME_AP_PRIM, CsrWifiNmeApConverterLookup);
+}
+
+
+#ifdef CSR_LOG_ENABLE
+static const CsrLogPrimitiveInformation csrwifinmeap_conv_info = {
+    CSR_WIFI_NME_AP_PRIM,
+    (char *)"CSR_WIFI_NME_AP_PRIM",
+    csrwifinmeap_conv_lut
+};
+const CsrLogPrimitiveInformation* CsrWifiNmeApTechInfoGet(void)
+{
+    return &csrwifinmeap_conv_info;
+}
+
+
+#endif /* CSR_LOG_ENABLE */
+#endif /* EXCLUDE_CSR_WIFI_NME_AP_MODULE */
+#endif /* CSR_WIFI_NME_ENABLE */
+#endif /* CSR_WIFI_AP_ENABLE */
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_converter_init.h b/drivers/staging/csr/csr_wifi_nme_ap_converter_init.h
new file mode 100644
index 00000000000..4072c06a152
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_converter_init.h
@@ -0,0 +1,49 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_AP_CONVERTER_INIT_H__
+#define CSR_WIFI_NME_AP_CONVERTER_INIT_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_converter_init.h
+#endif
+#ifndef CSR_WIFI_AP_ENABLE
+#error CSR_WIFI_AP_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_converter_init.h
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_NME_AP_MODULE
+
+#include "csr_msgconv.h"
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+
+extern const CsrLogPrimitiveInformation* CsrWifiNmeApTechInfoGet(void);
+#endif /* CSR_LOG_ENABLE */
+
+extern void CsrWifiNmeApConverterInit(void);
+
+#else /* EXCLUDE_CSR_WIFI_NME_AP_MODULE */
+
+#define CsrWifiNmeApConverterInit()
+
+#endif /* EXCLUDE_CSR_WIFI_NME_AP_MODULE */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_AP_CONVERTER_INIT_H__ */
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_free_downstream_contents.c b/drivers/staging/csr/csr_wifi_nme_ap_free_downstream_contents.c
new file mode 100644
index 00000000000..ab9358873ec
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_free_downstream_contents.c
@@ -0,0 +1,84 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_nme_ap_prim.h"
+#include "csr_wifi_nme_ap_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiNmeApFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_NME_AP_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiNmeApFreeDownstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_NME_AP_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiNmeApPrim *) message))
+    {
+        case CSR_WIFI_NME_AP_CONFIG_SET_REQ:
+        {
+            CsrWifiNmeApConfigSetReq *p = (CsrWifiNmeApConfigSetReq *)message;
+            kfree(p->apMacConfig.macAddressList);
+            p->apMacConfig.macAddressList = NULL;
+            break;
+        }
+        case CSR_WIFI_NME_AP_START_REQ:
+        {
+            CsrWifiNmeApStartReq *p = (CsrWifiNmeApStartReq *)message;
+            switch (p->apCredentials.authType)
+            {
+                case CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL:
+                    switch (p->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase)
+                    {
+                        case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE:
+                            kfree(p->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase);
+                            p->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase = NULL;
+                            break;
+                        default:
+                            break;
+                    }
+                    break;
+                default:
+                    break;
+            }
+            {
+                u16 i3;
+                for (i3 = 0; i3 < p->p2pGoParam.operatingChanList.channelEntryListCount; i3++)
+                {
+                    kfree(p->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel);
+                    p->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel = NULL;
+                }
+            }
+            kfree(p->p2pGoParam.operatingChanList.channelEntryList);
+            p->p2pGoParam.operatingChanList.channelEntryList = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_free_upstream_contents.c b/drivers/staging/csr/csr_wifi_nme_ap_free_upstream_contents.c
new file mode 100644
index 00000000000..2786a6bbff9
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_free_upstream_contents.c
@@ -0,0 +1,39 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#include "csr_wifi_nme_ap_prim.h"
+#include "csr_wifi_nme_ap_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiNmeApFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_NME_AP_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiNmeApFreeUpstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_NME_AP_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_lib.h b/drivers/staging/csr/csr_wifi_nme_ap_lib.h
new file mode 100644
index 00000000000..d4014709112
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_lib.h
@@ -0,0 +1,523 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_AP_LIB_H__
+#define CSR_WIFI_NME_AP_LIB_H__
+
+#include "csr_sched.h"
+#include "csr_macro.h"
+#include "csr_msg_transport.h"
+
+#include "csr_wifi_lib.h"
+
+#include "csr_wifi_nme_ap_prim.h"
+#include "csr_wifi_nme_task.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_lib.h
+#endif
+#ifndef CSR_WIFI_AP_ENABLE
+#error CSR_WIFI_AP_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_lib.h
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiNmeApFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_NME_AP upstream message. Does not
+ *      free the message itself, and can only be used for upstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_NME_AP upstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiNmeApFreeUpstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiNmeApFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_NME_AP downstream message. Does not
+ *      free the message itself, and can only be used for downstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_NME_AP downstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiNmeApFreeDownstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ * Enum to string functions
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiNmeApPersCredentialTypeToString(CsrWifiNmeApPersCredentialType value);
+
+
+/*----------------------------------------------------------------------------*
+ * CsrPrim Type toString function.
+ * Converts a message type to the String name of the Message
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiNmeApPrimTypeToString(CsrPrim msgType);
+
+/*----------------------------------------------------------------------------*
+ * Lookup arrays for PrimType name Strings
+ *----------------------------------------------------------------------------*/
+extern const char *CsrWifiNmeApUpstreamPrimNames[CSR_WIFI_NME_AP_PRIM_UPSTREAM_COUNT];
+extern const char *CsrWifiNmeApDownstreamPrimNames[CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_COUNT];
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApConfigSetReqSend
+
+  DESCRIPTION
+    This primitive passes AP configuration info for NME. This can be sent at
+    any time but will be acted upon when the AP is started again. This
+    information is common to both P2P GO and AP
+
+  PARAMETERS
+    queue       - Message Source Task Queue (Cfm's will be sent to this Queue)
+    apConfig    - AP configuration for the NME.
+    apMacConfig - MAC configuration to be acted on when
+                  CSR_WIFI_NME_AP_START.request is sent.
+
+*******************************************************************************/
+#define CsrWifiNmeApConfigSetReqCreate(msg__, dst__, src__, apConfig__, apMacConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_CONFIG_SET_REQ, dst__, src__); \
+    msg__->apConfig = (apConfig__); \
+    msg__->apMacConfig = (apMacConfig__);
+
+#define CsrWifiNmeApConfigSetReqSendTo(dst__, src__, apConfig__, apMacConfig__) \
+    { \
+        CsrWifiNmeApConfigSetReq *msg__; \
+        CsrWifiNmeApConfigSetReqCreate(msg__, dst__, src__, apConfig__, apMacConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApConfigSetReqSend(src__, apConfig__, apMacConfig__) \
+    CsrWifiNmeApConfigSetReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, apConfig__, apMacConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiNmeApConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeApConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeApConfigSetCfm *msg__; \
+        CsrWifiNmeApConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApConfigSetCfmSend(dst__, status__) \
+    CsrWifiNmeApConfigSetCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStaRemoveReqSend
+
+  DESCRIPTION
+    This primitive disconnects a connected station. If keepBlocking is set to
+    TRUE, the station with the specified MAC address is not allowed to
+    connect. If the requested station is not already connected,it may be
+    blocked based on keepBlocking parameter.
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    staMacAddress - Mac Address of the station to be disconnected or blocked
+    keepBlocking  - If TRUE, the station is blocked. If FALSE and the station is
+                    connected, disconnect the station. If FALSE and the station
+                    is not connected, no action is taken.
+
+*******************************************************************************/
+#define CsrWifiNmeApStaRemoveReqCreate(msg__, dst__, src__, interfaceTag__, staMacAddress__, keepBlocking__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStaRemoveReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_STA_REMOVE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->staMacAddress = (staMacAddress__); \
+    msg__->keepBlocking = (keepBlocking__);
+
+#define CsrWifiNmeApStaRemoveReqSendTo(dst__, src__, interfaceTag__, staMacAddress__, keepBlocking__) \
+    { \
+        CsrWifiNmeApStaRemoveReq *msg__; \
+        CsrWifiNmeApStaRemoveReqCreate(msg__, dst__, src__, interfaceTag__, staMacAddress__, keepBlocking__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStaRemoveReqSend(src__, interfaceTag__, staMacAddress__, keepBlocking__) \
+    CsrWifiNmeApStaRemoveReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__, staMacAddress__, keepBlocking__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStartReqSend
+
+  DESCRIPTION
+    This primitive requests NME to started the AP operation.
+
+  PARAMETERS
+    queue          - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag   - Interface identifier; unique identifier of an interface
+    apType         - AP Type specifies the Legacy AP or P2P GO operation
+    cloakSsid      - Indicates whether the SSID should be cloaked (hidden and
+                     not broadcast in beacon) or not
+    ssid           - Service Set Identifier
+    ifIndex        - Radio interface
+    channel        - Channel number of the channel to use
+    apCredentials  - Security credential configuration.
+    maxConnections - Maximum number of stations/P2P clients allowed
+    p2pGoParam     - P2P specific GO parameters.
+    wpsEnabled     - Indicates whether WPS should be enabled or not
+
+*******************************************************************************/
+#define CsrWifiNmeApStartReqCreate(msg__, dst__, src__, interfaceTag__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, apCredentials__, maxConnections__, p2pGoParam__, wpsEnabled__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStartReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_START_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->apType = (apType__); \
+    msg__->cloakSsid = (cloakSsid__); \
+    msg__->ssid = (ssid__); \
+    msg__->ifIndex = (ifIndex__); \
+    msg__->channel = (channel__); \
+    msg__->apCredentials = (apCredentials__); \
+    msg__->maxConnections = (maxConnections__); \
+    msg__->p2pGoParam = (p2pGoParam__); \
+    msg__->wpsEnabled = (wpsEnabled__);
+
+#define CsrWifiNmeApStartReqSendTo(dst__, src__, interfaceTag__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, apCredentials__, maxConnections__, p2pGoParam__, wpsEnabled__) \
+    { \
+        CsrWifiNmeApStartReq *msg__; \
+        CsrWifiNmeApStartReqCreate(msg__, dst__, src__, interfaceTag__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, apCredentials__, maxConnections__, p2pGoParam__, wpsEnabled__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStartReqSend(src__, interfaceTag__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, apCredentials__, maxConnections__, p2pGoParam__, wpsEnabled__) \
+    CsrWifiNmeApStartReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, apCredentials__, maxConnections__, p2pGoParam__, wpsEnabled__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStartCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of CSR_WIFI_NME_AP_START.request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface identifier; unique identifier of an interface
+    status       - Status of the request.
+    ssid         - Service Set Identifier
+
+*******************************************************************************/
+#define CsrWifiNmeApStartCfmCreate(msg__, dst__, src__, interfaceTag__, status__, ssid__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStartCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_START_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->ssid = (ssid__);
+
+#define CsrWifiNmeApStartCfmSendTo(dst__, src__, interfaceTag__, status__, ssid__) \
+    { \
+        CsrWifiNmeApStartCfm *msg__; \
+        CsrWifiNmeApStartCfmCreate(msg__, dst__, src__, interfaceTag__, status__, ssid__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStartCfmSend(dst__, interfaceTag__, status__, ssid__) \
+    CsrWifiNmeApStartCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__, ssid__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStationIndSend
+
+  DESCRIPTION
+    This primitive indicates that a station has joined or a previously joined
+    station has left the BSS/group
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    interfaceTag      - Interface Identifier; unique identifier of an interface
+    mediaStatus       - Indicates whether the station is connected or
+                        disconnected
+    peerMacAddress    - MAC address of the station
+    peerDeviceAddress - P2P Device Address
+
+*******************************************************************************/
+#define CsrWifiNmeApStationIndCreate(msg__, dst__, src__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStationInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_STATION_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->mediaStatus = (mediaStatus__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->peerDeviceAddress = (peerDeviceAddress__);
+
+#define CsrWifiNmeApStationIndSendTo(dst__, src__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__) \
+    { \
+        CsrWifiNmeApStationInd *msg__; \
+        CsrWifiNmeApStationIndCreate(msg__, dst__, src__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStationIndSend(dst__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__) \
+    CsrWifiNmeApStationIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStopReqSend
+
+  DESCRIPTION
+    This primitive requests NME to stop the AP operation.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiNmeApStopReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStopReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_STOP_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiNmeApStopReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiNmeApStopReq *msg__; \
+        CsrWifiNmeApStopReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStopReqSend(src__, interfaceTag__) \
+    CsrWifiNmeApStopReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStopIndSend
+
+  DESCRIPTION
+    Indicates that AP operation had stopped because of some unrecoverable
+    error after AP operation was started successfully. NME sends this signal
+    after failing to restart the AP operation internally following an error
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    apType       - Reports AP Type (P2PGO or AP)
+    status       - Error Status
+
+*******************************************************************************/
+#define CsrWifiNmeApStopIndCreate(msg__, dst__, src__, interfaceTag__, apType__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStopInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_STOP_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->apType = (apType__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeApStopIndSendTo(dst__, src__, interfaceTag__, apType__, status__) \
+    { \
+        CsrWifiNmeApStopInd *msg__; \
+        CsrWifiNmeApStopIndCreate(msg__, dst__, src__, interfaceTag__, apType__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStopIndSend(dst__, interfaceTag__, apType__, status__) \
+    CsrWifiNmeApStopIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, apType__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStopCfmSend
+
+  DESCRIPTION
+    This primitive confirms that the AP operation is stopped. NME shall send
+    this primitive in response to the request even if AP operation has
+    already been stopped
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface identifier; unique identifier of an interface
+    status       - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiNmeApStopCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApStopCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_STOP_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeApStopCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiNmeApStopCfm *msg__; \
+        CsrWifiNmeApStopCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApStopCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiNmeApStopCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWmmParamUpdateReqSend
+
+  DESCRIPTION
+    Application uses this primitive to update the WMM parameters
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    wmmApParams   - WMM Access point parameters per access category. The array
+                    index corresponds to the ACI
+    wmmApBcParams - WMM station parameters per access category to be advertised
+                    in the beacons and probe response The array index
+                    corresponds to the ACI
+
+*******************************************************************************/
+#define CsrWifiNmeApWmmParamUpdateReqCreate(msg__, dst__, src__, wmmApParams__, wmmApBcParams__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApWmmParamUpdateReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_WMM_PARAM_UPDATE_REQ, dst__, src__); \
+    memcpy(msg__->wmmApParams, (wmmApParams__), sizeof(CsrWifiSmeWmmAcParams) * 4); \
+    memcpy(msg__->wmmApBcParams, (wmmApBcParams__), sizeof(CsrWifiSmeWmmAcParams) * 4);
+
+#define CsrWifiNmeApWmmParamUpdateReqSendTo(dst__, src__, wmmApParams__, wmmApBcParams__) \
+    { \
+        CsrWifiNmeApWmmParamUpdateReq *msg__; \
+        CsrWifiNmeApWmmParamUpdateReqCreate(msg__, dst__, src__, wmmApParams__, wmmApBcParams__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApWmmParamUpdateReqSend(src__, wmmApParams__, wmmApBcParams__) \
+    CsrWifiNmeApWmmParamUpdateReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, wmmApParams__, wmmApBcParams__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWmmParamUpdateCfmSend
+
+  DESCRIPTION
+    A confirm for for the WMM parameters update
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiNmeApWmmParamUpdateCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApWmmParamUpdateCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_WMM_PARAM_UPDATE_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeApWmmParamUpdateCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeApWmmParamUpdateCfm *msg__; \
+        CsrWifiNmeApWmmParamUpdateCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApWmmParamUpdateCfmSend(dst__, status__) \
+    CsrWifiNmeApWmmParamUpdateCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWpsRegisterReqSend
+
+  DESCRIPTION
+    This primitive allows the NME to accept the WPS registration from an
+    enrollee. Such registration procedure can be cancelled by sending
+    CSR_WIFI_NME_WPS_CANCEL.request.
+
+  PARAMETERS
+    queue                    - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag             - Interface Identifier; unique identifier of an
+                               interface
+    selectedDevicePasswordId - Selected password type
+    selectedConfigMethod     - Selected WPS configuration method type
+    pin                      - PIN value.
+                               Relevant if selected device password ID is PIN.4
+                               digit pin is passed by sending the pin digits in
+                               pin[0]..pin[3] and rest of the contents filled
+                               with '-'.
+
+*******************************************************************************/
+#define CsrWifiNmeApWpsRegisterReqCreate(msg__, dst__, src__, interfaceTag__, selectedDevicePasswordId__, selectedConfigMethod__, pin__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApWpsRegisterReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_WPS_REGISTER_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->selectedDevicePasswordId = (selectedDevicePasswordId__); \
+    msg__->selectedConfigMethod = (selectedConfigMethod__); \
+    memcpy(msg__->pin, (pin__), sizeof(u8) * 8);
+
+#define CsrWifiNmeApWpsRegisterReqSendTo(dst__, src__, interfaceTag__, selectedDevicePasswordId__, selectedConfigMethod__, pin__) \
+    { \
+        CsrWifiNmeApWpsRegisterReq *msg__; \
+        CsrWifiNmeApWpsRegisterReqCreate(msg__, dst__, src__, interfaceTag__, selectedDevicePasswordId__, selectedConfigMethod__, pin__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApWpsRegisterReqSend(src__, interfaceTag__, selectedDevicePasswordId__, selectedConfigMethod__, pin__) \
+    CsrWifiNmeApWpsRegisterReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__, selectedDevicePasswordId__, selectedConfigMethod__, pin__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWpsRegisterCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of WPS procedure.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface identifier; unique identifier of an interface
+    status       - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiNmeApWpsRegisterCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeApWpsRegisterCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_AP_PRIM, CSR_WIFI_NME_AP_WPS_REGISTER_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeApWpsRegisterCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiNmeApWpsRegisterCfm *msg__; \
+        CsrWifiNmeApWpsRegisterCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeApWpsRegisterCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiNmeApWpsRegisterCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_AP_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_prim.h b/drivers/staging/csr/csr_wifi_nme_ap_prim.h
new file mode 100644
index 00000000000..fc44560b28b
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_prim.h
@@ -0,0 +1,503 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_AP_PRIM_H__
+#define CSR_WIFI_NME_AP_PRIM_H__
+
+#include <linux/types.h>
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+#include "csr_wifi_common.h"
+#include "csr_result.h"
+#include "csr_wifi_fsm_event.h"
+#include "csr_wifi_sme_ap_prim.h"
+#include "csr_wifi_nme_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_prim.h
+#endif
+#ifndef CSR_WIFI_AP_ENABLE
+#error CSR_WIFI_AP_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_prim.h
+#endif
+
+#define CSR_WIFI_NME_AP_PRIM                                            (0x0426)
+
+typedef CsrPrim CsrWifiNmeApPrim;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApPersCredentialType
+
+  DESCRIPTION
+    NME Credential Types
+
+ VALUES
+    CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PSK
+                   - Use PSK as credential.
+    CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE
+                   - Use the specified passphrase as credential
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeApPersCredentialType;
+#define CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PSK          ((CsrWifiNmeApPersCredentialType) 0x00)
+#define CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE   ((CsrWifiNmeApPersCredentialType) 0x01)
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApConfig
+
+  DESCRIPTION
+    Structure holding AP config data.
+
+  MEMBERS
+    apGroupkeyTimeout - Access point group key timeout.
+    apStrictGtkRekey  - Access point strict GTK rekey flag. If set TRUE, the AP
+                        shall rekey GTK every time a connected STA leaves BSS.
+    apGmkTimeout      - Access point GMK timeout
+    apResponseTimeout - Response timeout
+    apRetransLimit    - Max allowed retransmissions
+
+*******************************************************************************/
+typedef struct
+{
+    u16 apGroupkeyTimeout;
+    u8   apStrictGtkRekey;
+    u16 apGmkTimeout;
+    u16 apResponseTimeout;
+    u8  apRetransLimit;
+} CsrWifiNmeApConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApAuthPers
+
+  DESCRIPTION
+
+  MEMBERS
+    authSupport                     - Credential type value (as defined in the
+                                      enumeration type).
+    rsnCapabilities                 - RSN capabilities mask
+    wapiCapabilities                - WAPI capabilities mask
+    pskOrPassphrase                 - Credential type value (as defined in the
+                                      enumeration type).
+    authPers_credentials            - Union containing credentials which depends
+                                      on credentialType parameter.
+    authPers_credentialspsk         -
+    authPers_credentialspassphrase  -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeApAuthSupportMask      authSupport;
+    CsrWifiSmeApRsnCapabilitiesMask  rsnCapabilities;
+    CsrWifiSmeApWapiCapabilitiesMask wapiCapabilities;
+    CsrWifiNmeApPersCredentialType   pskOrPassphrase;
+    union {
+        CsrWifiNmePsk        psk;
+        CsrWifiNmePassphrase passphrase;
+    } authPers_credentials;
+} CsrWifiNmeApAuthPers;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApCredentials
+
+  DESCRIPTION
+    Structure containing the Credentials data.
+
+  MEMBERS
+    authType                     - Authentication type
+    nmeAuthType                  - Authentication parameters
+    nmeAuthTypeopenSystemEmpty   -
+    nmeAuthTypeauthwep           -
+    nmeAuthTypeauthTypePersonal  -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeApAuthType authType;
+    union {
+        CsrWifiSmeEmpty      openSystemEmpty;
+        CsrWifiSmeWepAuth    authwep;
+        CsrWifiNmeApAuthPers authTypePersonal;
+    } nmeAuthType;
+} CsrWifiNmeApCredentials;
+
+
+/* Downstream */
+#define CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST            (0x0000)
+
+#define CSR_WIFI_NME_AP_CONFIG_SET_REQ                    ((CsrWifiNmeApPrim) (0x0000 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_WPS_REGISTER_REQ                  ((CsrWifiNmeApPrim) (0x0001 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_START_REQ                         ((CsrWifiNmeApPrim) (0x0002 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_STOP_REQ                          ((CsrWifiNmeApPrim) (0x0003 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_WMM_PARAM_UPDATE_REQ              ((CsrWifiNmeApPrim) (0x0004 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_STA_REMOVE_REQ                    ((CsrWifiNmeApPrim) (0x0005 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST))
+
+
+#define CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_HIGHEST           (0x0005 + CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST)
+
+/* Upstream */
+#define CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST              (0x0000 + CSR_PRIM_UPSTREAM)
+
+#define CSR_WIFI_NME_AP_CONFIG_SET_CFM                    ((CsrWifiNmeApPrim)(0x0000 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_WPS_REGISTER_CFM                  ((CsrWifiNmeApPrim)(0x0001 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_START_CFM                         ((CsrWifiNmeApPrim)(0x0002 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_STOP_CFM                          ((CsrWifiNmeApPrim)(0x0003 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_STOP_IND                          ((CsrWifiNmeApPrim)(0x0004 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_WMM_PARAM_UPDATE_CFM              ((CsrWifiNmeApPrim)(0x0005 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_AP_STATION_IND                       ((CsrWifiNmeApPrim)(0x0006 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST))
+
+#define CSR_WIFI_NME_AP_PRIM_UPSTREAM_HIGHEST             (0x0006 + CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST)
+
+#define CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_COUNT             (CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_HIGHEST + 1 - CSR_WIFI_NME_AP_PRIM_DOWNSTREAM_LOWEST)
+#define CSR_WIFI_NME_AP_PRIM_UPSTREAM_COUNT               (CSR_WIFI_NME_AP_PRIM_UPSTREAM_HIGHEST   + 1 - CSR_WIFI_NME_AP_PRIM_UPSTREAM_LOWEST)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApConfigSetReq
+
+  DESCRIPTION
+    This primitive passes AP configuration info for NME. This can be sent at
+    any time but will be acted upon when the AP is started again. This
+    information is common to both P2P GO and AP
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    apConfig    - AP configuration for the NME.
+    apMacConfig - MAC configuration to be acted on when
+                  CSR_WIFI_NME_AP_START.request is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrWifiNmeApConfig    apConfig;
+    CsrWifiSmeApMacConfig apMacConfig;
+} CsrWifiNmeApConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWpsRegisterReq
+
+  DESCRIPTION
+    This primitive allows the NME to accept the WPS registration from an
+    enrollee. Such registration procedure can be cancelled by sending
+    CSR_WIFI_NME_WPS_CANCEL.request.
+
+  MEMBERS
+    common                   - Common header for use with the CsrWifiFsm Module
+    interfaceTag             - Interface Identifier; unique identifier of an
+                               interface
+    selectedDevicePasswordId - Selected password type
+    selectedConfigMethod     - Selected WPS configuration method type
+    pin                      - PIN value.
+                               Relevant if selected device password ID is PIN.4
+                               digit pin is passed by sending the pin digits in
+                               pin[0]..pin[3] and rest of the contents filled
+                               with '-'.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    CsrWifiSmeWpsDpid       selectedDevicePasswordId;
+    CsrWifiSmeWpsConfigType selectedConfigMethod;
+    u8                pin[8];
+} CsrWifiNmeApWpsRegisterReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStartReq
+
+  DESCRIPTION
+    This primitive requests NME to started the AP operation.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface identifier; unique identifier of an interface
+    apType         - AP Type specifies the Legacy AP or P2P GO operation
+    cloakSsid      - Indicates whether the SSID should be cloaked (hidden and
+                     not broadcast in beacon) or not
+    ssid           - Service Set Identifier
+    ifIndex        - Radio interface
+    channel        - Channel number of the channel to use
+    apCredentials  - Security credential configuration.
+    maxConnections - Maximum number of stations/P2P clients allowed
+    p2pGoParam     - P2P specific GO parameters.
+    wpsEnabled     - Indicates whether WPS should be enabled or not
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    CsrWifiSmeApType        apType;
+    u8                 cloakSsid;
+    CsrWifiSsid             ssid;
+    CsrWifiSmeRadioIF       ifIndex;
+    u8                channel;
+    CsrWifiNmeApCredentials apCredentials;
+    u8                maxConnections;
+    CsrWifiSmeApP2pGoConfig p2pGoParam;
+    u8                 wpsEnabled;
+} CsrWifiNmeApStartReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStopReq
+
+  DESCRIPTION
+    This primitive requests NME to stop the AP operation.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiNmeApStopReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWmmParamUpdateReq
+
+  DESCRIPTION
+    Application uses this primitive to update the WMM parameters
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    wmmApParams   - WMM Access point parameters per access category. The array
+                    index corresponds to the ACI
+    wmmApBcParams - WMM station parameters per access category to be advertised
+                    in the beacons and probe response The array index
+                    corresponds to the ACI
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrWifiSmeWmmAcParams wmmApParams[4];
+    CsrWifiSmeWmmAcParams wmmApBcParams[4];
+} CsrWifiNmeApWmmParamUpdateReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStaRemoveReq
+
+  DESCRIPTION
+    This primitive disconnects a connected station. If keepBlocking is set to
+    TRUE, the station with the specified MAC address is not allowed to
+    connect. If the requested station is not already connected,it may be
+    blocked based on keepBlocking parameter.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    staMacAddress - Mac Address of the station to be disconnected or blocked
+    keepBlocking  - If TRUE, the station is blocked. If FALSE and the station is
+                    connected, disconnect the station. If FALSE and the station
+                    is not connected, no action is taken.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    CsrWifiMacAddress staMacAddress;
+    u8           keepBlocking;
+} CsrWifiNmeApStaRemoveReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeApConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWpsRegisterCfm
+
+  DESCRIPTION
+    This primitive reports the result of WPS procedure.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface identifier; unique identifier of an interface
+    status       - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiNmeApWpsRegisterCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStartCfm
+
+  DESCRIPTION
+    This primitive reports the result of CSR_WIFI_NME_AP_START.request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface identifier; unique identifier of an interface
+    status       - Status of the request.
+    ssid         - Service Set Identifier
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+    CsrWifiSsid     ssid;
+} CsrWifiNmeApStartCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStopCfm
+
+  DESCRIPTION
+    This primitive confirms that the AP operation is stopped. NME shall send
+    this primitive in response to the request even if AP operation has
+    already been stopped
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface identifier; unique identifier of an interface
+    status       - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiNmeApStopCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStopInd
+
+  DESCRIPTION
+    Indicates that AP operation had stopped because of some unrecoverable
+    error after AP operation was started successfully. NME sends this signal
+    after failing to restart the AP operation internally following an error
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    apType       - Reports AP Type (P2PGO or AP)
+    status       - Error Status
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent  common;
+    u16        interfaceTag;
+    CsrWifiSmeApType apType;
+    CsrResult        status;
+} CsrWifiNmeApStopInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApWmmParamUpdateCfm
+
+  DESCRIPTION
+    A confirm for for the WMM parameters update
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeApWmmParamUpdateCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeApStationInd
+
+  DESCRIPTION
+    This primitive indicates that a station has joined or a previously joined
+    station has left the BSS/group
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    interfaceTag      - Interface Identifier; unique identifier of an interface
+    mediaStatus       - Indicates whether the station is connected or
+                        disconnected
+    peerMacAddress    - MAC address of the station
+    peerDeviceAddress - P2P Device Address
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    u16             interfaceTag;
+    CsrWifiSmeMediaStatus mediaStatus;
+    CsrWifiMacAddress     peerMacAddress;
+    CsrWifiMacAddress     peerDeviceAddress;
+} CsrWifiNmeApStationInd;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_AP_PRIM_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_sef.c b/drivers/staging/csr/csr_wifi_nme_ap_sef.c
new file mode 100644
index 00000000000..e048848883d
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_sef.c
@@ -0,0 +1,30 @@
+/*****************************************************************************
+
+  FILE: csr_wifi_nme_sef.c
+
+  (c) Cambridge Silicon Radio Limited 2010
+
+  Refer to LICENSE.txt included with this source for details
+  on the license terms.
+
+ *****************************************************************************/
+#include "csr_wifi_nme_ap_sef.h"
+#include "unifi_priv.h"
+
+void CsrWifiNmeApUpstreamStateHandlers(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    switch(msg->type) {
+        case CSR_WIFI_NME_AP_START_CFM:
+            CsrWifiNmeApStartCfmHandler(drvpriv, msg);
+            break;
+        case CSR_WIFI_NME_AP_STOP_CFM:
+            CsrWifiNmeApStopCfmHandler(drvpriv, msg);
+            break;
+        case CSR_WIFI_NME_AP_CONFIG_SET_CFM:
+            CsrWifiNmeApConfigSetCfmHandler(drvpriv,msg);
+            break;
+        default:
+	    unifi_error(drvpriv, "CsrWifiNmeApUpstreamStateHandlers: unhandled NME_AP message type 0x%.4X\n",msg->type);
+            break;
+    }
+}
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_sef.h b/drivers/staging/csr/csr_wifi_nme_ap_sef.h
new file mode 100644
index 00000000000..3f353633fa5
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_sef.h
@@ -0,0 +1,31 @@
+/*****************************************************************************
+    FILE: csr_wifi_nme_sef.h
+    (c) Cambridge Silicon Radio Limited 2010
+
+    Refer to LICENSE.txt included with this source for details
+    on the license terms.
+
+*****************************************************************************/
+#ifndef CSR_WIFI_ROUTER_SEF_CSR_WIFI_NME_H__
+#define CSR_WIFI_ROUTER_SEF_CSR_WIFI_NME_H__
+
+#include "csr_wifi_nme_prim.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void CsrWifiNmeApUpstreamStateHandlers(void* drvpriv, CsrWifiFsmEvent* msg);
+
+
+extern void CsrWifiNmeApConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiNmeApStartCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiNmeApStopCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_SEF_CSR_WIFI_NME_H__ */
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_serialize.c b/drivers/staging/csr/csr_wifi_nme_ap_serialize.c
new file mode 100644
index 00000000000..1a901a70d19
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_serialize.c
@@ -0,0 +1,909 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/string.h>
+#include <linux/slab.h>
+#include "csr_msgconv.h"
+
+#ifdef CSR_WIFI_NME_ENABLE
+#ifdef CSR_WIFI_AP_ENABLE
+
+#include "csr_wifi_nme_ap_prim.h"
+#include "csr_wifi_nme_ap_serialize.h"
+
+void CsrWifiNmeApPfree(void *ptr)
+{
+    kfree(ptr);
+}
+
+
+size_t CsrWifiNmeApConfigSetReqSizeof(void *msg)
+{
+    CsrWifiNmeApConfigSetReq *primitive = (CsrWifiNmeApConfigSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 104) */
+    bufferSize += 2;  /* u16 primitive->apConfig.apGroupkeyTimeout */
+    bufferSize += 1;  /* u8 primitive->apConfig.apStrictGtkRekey */
+    bufferSize += 2;  /* u16 primitive->apConfig.apGmkTimeout */
+    bufferSize += 2;  /* u16 primitive->apConfig.apResponseTimeout */
+    bufferSize += 1;  /* u8 primitive->apConfig.apRetransLimit */
+    bufferSize += 1;  /* CsrWifiSmeApPhySupportMask primitive->apMacConfig.phySupportedBitmap */
+    bufferSize += 2;  /* u16 primitive->apMacConfig.beaconInterval */
+    bufferSize += 1;  /* u8 primitive->apMacConfig.dtimPeriod */
+    bufferSize += 2;  /* u16 primitive->apMacConfig.maxListenInterval */
+    bufferSize += 1;  /* u8 primitive->apMacConfig.supportedRatesCount */
+    bufferSize += 20; /* u8 primitive->apMacConfig.supportedRates[20] */
+    bufferSize += 1;  /* CsrWifiSmePreambleType primitive->apMacConfig.preamble */
+    bufferSize += 1;  /* u8 primitive->apMacConfig.shortSlotTimeEnabled */
+    bufferSize += 1;  /* CsrWifiSmeCtsProtectionType primitive->apMacConfig.ctsProtectionType */
+    bufferSize += 1;  /* u8 primitive->apMacConfig.wmmEnabled */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApParams[i2].cwMin */
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApParams[i2].cwMax */
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApParams[i2].aifs */
+            bufferSize += 2; /* u16 primitive->apMacConfig.wmmApParams[i2].txopLimit */
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApParams[i2].admissionControlMandatory */
+        }
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApBcParams[i2].cwMin */
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApBcParams[i2].cwMax */
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApBcParams[i2].aifs */
+            bufferSize += 2; /* u16 primitive->apMacConfig.wmmApBcParams[i2].txopLimit */
+            bufferSize += 1; /* u8 primitive->apMacConfig.wmmApBcParams[i2].admissionControlMandatory */
+        }
+    }
+    bufferSize += 1;         /* CsrWifiSmeApAccessType primitive->apMacConfig.accessType */
+    bufferSize += 1;         /* u8 primitive->apMacConfig.macAddressListCount */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->apMacConfig.macAddressListCount; i2++)
+        {
+            bufferSize += 6; /* u8 primitive->apMacConfig.macAddressList[i2].a[6] */
+        }
+    }
+    bufferSize += 1;         /* u8 primitive->apMacConfig.apHtParams.greenfieldSupported */
+    bufferSize += 1;         /* u8 primitive->apMacConfig.apHtParams.shortGi20MHz */
+    bufferSize += 1;         /* u8 primitive->apMacConfig.apHtParams.rxStbc */
+    bufferSize += 1;         /* u8 primitive->apMacConfig.apHtParams.rifsModeAllowed */
+    bufferSize += 1;         /* u8 primitive->apMacConfig.apHtParams.htProtection */
+    bufferSize += 1;         /* u8 primitive->apMacConfig.apHtParams.dualCtsProtection */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApConfigSetReq *primitive = (CsrWifiNmeApConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->apConfig.apGroupkeyTimeout);
+    CsrUint8Ser(ptr, len, (u8) primitive->apConfig.apStrictGtkRekey);
+    CsrUint16Ser(ptr, len, (u16) primitive->apConfig.apGmkTimeout);
+    CsrUint16Ser(ptr, len, (u16) primitive->apConfig.apResponseTimeout);
+    CsrUint8Ser(ptr, len, (u8) primitive->apConfig.apRetransLimit);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.phySupportedBitmap);
+    CsrUint16Ser(ptr, len, (u16) primitive->apMacConfig.beaconInterval);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.dtimPeriod);
+    CsrUint16Ser(ptr, len, (u16) primitive->apMacConfig.maxListenInterval);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.supportedRatesCount);
+    CsrMemCpySer(ptr, len, (const void *) primitive->apMacConfig.supportedRates, ((u16) (20)));
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.preamble);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.shortSlotTimeEnabled);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.ctsProtectionType);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmEnabled);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApParams[i2].cwMin);
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApParams[i2].cwMax);
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApParams[i2].aifs);
+            CsrUint16Ser(ptr, len, (u16) primitive->apMacConfig.wmmApParams[i2].txopLimit);
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApParams[i2].admissionControlMandatory);
+        }
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApBcParams[i2].cwMin);
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApBcParams[i2].cwMax);
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApBcParams[i2].aifs);
+            CsrUint16Ser(ptr, len, (u16) primitive->apMacConfig.wmmApBcParams[i2].txopLimit);
+            CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.wmmApBcParams[i2].admissionControlMandatory);
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.accessType);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.macAddressListCount);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->apMacConfig.macAddressListCount; i2++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->apMacConfig.macAddressList[i2].a, ((u16) (6)));
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.apHtParams.greenfieldSupported);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.apHtParams.shortGi20MHz);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.apHtParams.rxStbc);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.apHtParams.rifsModeAllowed);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.apHtParams.htProtection);
+    CsrUint8Ser(ptr, len, (u8) primitive->apMacConfig.apHtParams.dualCtsProtection);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApConfigSetReq *primitive = kmalloc(sizeof(CsrWifiNmeApConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->apConfig.apGroupkeyTimeout, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apConfig.apStrictGtkRekey, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->apConfig.apGmkTimeout, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->apConfig.apResponseTimeout, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apConfig.apRetransLimit, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.phySupportedBitmap, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->apMacConfig.beaconInterval, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.dtimPeriod, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->apMacConfig.maxListenInterval, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.supportedRatesCount, buffer, &offset);
+    CsrMemCpyDes(primitive->apMacConfig.supportedRates, buffer, &offset, ((u16) (20)));
+    CsrUint8Des((u8 *) &primitive->apMacConfig.preamble, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.shortSlotTimeEnabled, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.ctsProtectionType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.wmmEnabled, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApParams[i2].cwMin, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApParams[i2].cwMax, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApParams[i2].aifs, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->apMacConfig.wmmApParams[i2].txopLimit, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApParams[i2].admissionControlMandatory, buffer, &offset);
+        }
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApBcParams[i2].cwMin, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApBcParams[i2].cwMax, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApBcParams[i2].aifs, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->apMacConfig.wmmApBcParams[i2].txopLimit, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apMacConfig.wmmApBcParams[i2].admissionControlMandatory, buffer, &offset);
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->apMacConfig.accessType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.macAddressListCount, buffer, &offset);
+    primitive->apMacConfig.macAddressList = NULL;
+    if (primitive->apMacConfig.macAddressListCount)
+    {
+        primitive->apMacConfig.macAddressList = kmalloc(sizeof(CsrWifiMacAddress) * primitive->apMacConfig.macAddressListCount, GFP_KERNEL);
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->apMacConfig.macAddressListCount; i2++)
+        {
+            CsrMemCpyDes(primitive->apMacConfig.macAddressList[i2].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->apMacConfig.apHtParams.greenfieldSupported, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.apHtParams.shortGi20MHz, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.apHtParams.rxStbc, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.apHtParams.rifsModeAllowed, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.apHtParams.htProtection, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apMacConfig.apHtParams.dualCtsProtection, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiNmeApConfigSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiNmeApConfigSetReq *primitive = (CsrWifiNmeApConfigSetReq *) voidPrimitivePointer;
+    kfree(primitive->apMacConfig.macAddressList);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiNmeApWpsRegisterReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 17) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiSmeWpsDpid primitive->selectedDevicePasswordId */
+    bufferSize += 2; /* CsrWifiSmeWpsConfigType primitive->selectedConfigMethod */
+    bufferSize += 8; /* u8 primitive->pin[8] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApWpsRegisterReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApWpsRegisterReq *primitive = (CsrWifiNmeApWpsRegisterReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->selectedDevicePasswordId);
+    CsrUint16Ser(ptr, len, (u16) primitive->selectedConfigMethod);
+    CsrMemCpySer(ptr, len, (const void *) primitive->pin, ((u16) (8)));
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApWpsRegisterReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApWpsRegisterReq *primitive = kmalloc(sizeof(CsrWifiNmeApWpsRegisterReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->selectedDevicePasswordId, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->selectedConfigMethod, buffer, &offset);
+    CsrMemCpyDes(primitive->pin, buffer, &offset, ((u16) (8)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApStartReqSizeof(void *msg)
+{
+    CsrWifiNmeApStartReq *primitive = (CsrWifiNmeApStartReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 112) */
+    bufferSize += 2;  /* u16 primitive->interfaceTag */
+    bufferSize += 1;  /* CsrWifiSmeApType primitive->apType */
+    bufferSize += 1;  /* u8 primitive->cloakSsid */
+    bufferSize += 32; /* u8 primitive->ssid.ssid[32] */
+    bufferSize += 1;  /* u8 primitive->ssid.length */
+    bufferSize += 1;  /* CsrWifiSmeRadioIF primitive->ifIndex */
+    bufferSize += 1;  /* u8 primitive->channel */
+    bufferSize += 1;  /* CsrWifiSmeApAuthType primitive->apCredentials.authType */
+    switch (primitive->apCredentials.authType)
+    {
+        case CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM:
+            bufferSize += 1; /* u8 primitive->apCredentials.nmeAuthType.openSystemEmpty.empty */
+            break;
+        case CSR_WIFI_SME_AP_AUTH_TYPE_WEP:
+            bufferSize += 1; /* CsrWifiSmeWepCredentialType primitive->apCredentials.nmeAuthType.authwep.wepKeyType */
+            switch (primitive->apCredentials.nmeAuthType.authwep.wepKeyType)
+            {
+                case CSR_WIFI_SME_CREDENTIAL_TYPE_WEP128:
+                    bufferSize += 1;  /* CsrWifiSmeWepAuthMode primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.wepAuthType */
+                    bufferSize += 1;  /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.selectedWepKey */
+                    bufferSize += 13; /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key1[13] */
+                    bufferSize += 13; /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key2[13] */
+                    bufferSize += 13; /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key3[13] */
+                    bufferSize += 13; /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key4[13] */
+                    break;
+                case CSR_WIFI_SME_CREDENTIAL_TYPE_WEP64:
+                    bufferSize += 1;  /* CsrWifiSmeWepAuthMode primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.wepAuthType */
+                    bufferSize += 1;  /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.selectedWepKey */
+                    bufferSize += 5;  /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key1[5] */
+                    bufferSize += 5;  /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key2[5] */
+                    bufferSize += 5;  /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key3[5] */
+                    bufferSize += 5;  /* u8 primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key4[5] */
+                    break;
+                default:
+                    break;
+            }
+            break;
+        case CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL:
+            bufferSize += 1; /* CsrWifiSmeApAuthSupportMask primitive->apCredentials.nmeAuthType.authTypePersonal.authSupport */
+            bufferSize += 2; /* CsrWifiSmeApRsnCapabilitiesMask primitive->apCredentials.nmeAuthType.authTypePersonal.rsnCapabilities */
+            bufferSize += 2; /* CsrWifiSmeApWapiCapabilitiesMask primitive->apCredentials.nmeAuthType.authTypePersonal.wapiCapabilities */
+            bufferSize += 1; /* CsrWifiNmeApPersCredentialType primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase */
+            switch (primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase)
+            {
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PSK:
+                    bufferSize += 2;                                                                                                                                                                                                                      /* u16 primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.psk.encryptionMode */
+                    bufferSize += 32;                                                                                                                                                                                                                     /* u8 primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.psk.psk[32] */
+                    break;
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE:
+                    bufferSize += 2;                                                                                                                                                                                                                      /* u16 primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.encryptionMode */
+                    bufferSize += (primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase ? strlen(primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase) : 0) + 1; /* char* primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase (0 byte len + 1 for NULL Term) */
+                    break;
+                default:
+                    break;
+            }
+            break;
+        default:
+            break;
+    }
+    bufferSize += 1; /* u8 primitive->maxConnections */
+    bufferSize += 1; /* CsrWifiSmeP2pGroupCapabilityMask primitive->p2pGoParam.groupCapability */
+    bufferSize += 3; /* u8 primitive->p2pGoParam.operatingChanList.country[3] */
+    bufferSize += 1; /* u8 primitive->p2pGoParam.operatingChanList.channelEntryListCount */
+    {
+        u16 i3;
+        for (i3 = 0; i3 < primitive->p2pGoParam.operatingChanList.channelEntryListCount; i3++)
+        {
+            bufferSize += 1;                                                                                  /* u8 primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingClass */
+            bufferSize += 1;                                                                                  /* u8 primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount */
+            bufferSize += primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount; /* u8 primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel */
+        }
+    }
+    bufferSize += 1;                                                                                          /* u8 primitive->p2pGoParam.opPsEnabled */
+    bufferSize += 1;                                                                                          /* u8 primitive->p2pGoParam.ctWindow */
+    bufferSize += 1;                                                                                          /* CsrWifiSmeP2pNoaConfigMethod primitive->p2pGoParam.noaConfigMethod */
+    bufferSize += 1;                                                                                          /* u8 primitive->p2pGoParam.allowNoaWithNonP2pDevices */
+    bufferSize += 1;                                                                                          /* u8 primitive->wpsEnabled */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApStartReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApStartReq *primitive = (CsrWifiNmeApStartReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->apType);
+    CsrUint8Ser(ptr, len, (u8) primitive->cloakSsid);
+    CsrMemCpySer(ptr, len, (const void *) primitive->ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->ssid.length);
+    CsrUint8Ser(ptr, len, (u8) primitive->ifIndex);
+    CsrUint8Ser(ptr, len, (u8) primitive->channel);
+    CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.authType);
+    switch (primitive->apCredentials.authType)
+    {
+        case CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM:
+            CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.openSystemEmpty.empty);
+            break;
+        case CSR_WIFI_SME_AP_AUTH_TYPE_WEP:
+            CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authwep.wepKeyType);
+            switch (primitive->apCredentials.nmeAuthType.authwep.wepKeyType)
+            {
+                case CSR_WIFI_SME_CREDENTIAL_TYPE_WEP128:
+                    CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.wepAuthType);
+                    CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.selectedWepKey);
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key1, ((u16) (13)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key2, ((u16) (13)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key3, ((u16) (13)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key4, ((u16) (13)));
+                    break;
+                case CSR_WIFI_SME_CREDENTIAL_TYPE_WEP64:
+                    CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.wepAuthType);
+                    CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.selectedWepKey);
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key1, ((u16) (5)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key2, ((u16) (5)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key3, ((u16) (5)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key4, ((u16) (5)));
+                    break;
+                default:
+                    break;
+            }
+            break;
+        case CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL:
+            CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authTypePersonal.authSupport);
+            CsrUint16Ser(ptr, len, (u16) primitive->apCredentials.nmeAuthType.authTypePersonal.rsnCapabilities);
+            CsrUint16Ser(ptr, len, (u16) primitive->apCredentials.nmeAuthType.authTypePersonal.wapiCapabilities);
+            CsrUint8Ser(ptr, len, (u8) primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase);
+            switch (primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase)
+            {
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PSK:
+                    CsrUint16Ser(ptr, len, (u16) primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.psk.encryptionMode);
+                    CsrMemCpySer(ptr, len, (const void *) primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.psk.psk, ((u16) (32)));
+                    break;
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE:
+                    CsrUint16Ser(ptr, len, (u16) primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.encryptionMode);
+                    CsrCharStringSer(ptr, len, primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase);
+                    break;
+                default:
+                    break;
+            }
+            break;
+        default:
+            break;
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->maxConnections);
+    CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.groupCapability);
+    CsrMemCpySer(ptr, len, (const void *) primitive->p2pGoParam.operatingChanList.country, ((u16) (3)));
+    CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.operatingChanList.channelEntryListCount);
+    {
+        u16 i3;
+        for (i3 = 0; i3 < primitive->p2pGoParam.operatingChanList.channelEntryListCount; i3++)
+        {
+            CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingClass);
+            CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount);
+            if (primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount)
+            {
+                CsrMemCpySer(ptr, len, (const void *) primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel, ((u16) (primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount)));
+            }
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.opPsEnabled);
+    CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.ctWindow);
+    CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.noaConfigMethod);
+    CsrUint8Ser(ptr, len, (u8) primitive->p2pGoParam.allowNoaWithNonP2pDevices);
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsEnabled);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApStartReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApStartReq *primitive = kmalloc(sizeof(CsrWifiNmeApStartReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->cloakSsid, buffer, &offset);
+    CsrMemCpyDes(primitive->ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->ssid.length, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ifIndex, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->channel, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apCredentials.authType, buffer, &offset);
+    switch (primitive->apCredentials.authType)
+    {
+        case CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM:
+            CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.openSystemEmpty.empty, buffer, &offset);
+            break;
+        case CSR_WIFI_SME_AP_AUTH_TYPE_WEP:
+            CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authwep.wepKeyType, buffer, &offset);
+            switch (primitive->apCredentials.nmeAuthType.authwep.wepKeyType)
+            {
+                case CSR_WIFI_SME_CREDENTIAL_TYPE_WEP128:
+                    CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.wepAuthType, buffer, &offset);
+                    CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.selectedWepKey, buffer, &offset);
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key1, buffer, &offset, ((u16) (13)));
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key2, buffer, &offset, ((u16) (13)));
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key3, buffer, &offset, ((u16) (13)));
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep128Key.key4, buffer, &offset, ((u16) (13)));
+                    break;
+                case CSR_WIFI_SME_CREDENTIAL_TYPE_WEP64:
+                    CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.wepAuthType, buffer, &offset);
+                    CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.selectedWepKey, buffer, &offset);
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key1, buffer, &offset, ((u16) (5)));
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key2, buffer, &offset, ((u16) (5)));
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key3, buffer, &offset, ((u16) (5)));
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authwep.wepCredentials.wep64Key.key4, buffer, &offset, ((u16) (5)));
+                    break;
+                default:
+                    break;
+            }
+            break;
+        case CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL:
+            CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authTypePersonal.authSupport, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->apCredentials.nmeAuthType.authTypePersonal.rsnCapabilities, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->apCredentials.nmeAuthType.authTypePersonal.wapiCapabilities, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase, buffer, &offset);
+            switch (primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase)
+            {
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PSK:
+                    CsrUint16Des((u16 *) &primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.psk.encryptionMode, buffer, &offset);
+                    CsrMemCpyDes(primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.psk.psk, buffer, &offset, ((u16) (32)));
+                    break;
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE:
+                    CsrUint16Des((u16 *) &primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.encryptionMode, buffer, &offset);
+                    CsrCharStringDes(&primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase, buffer, &offset);
+                    break;
+                default:
+                    break;
+            }
+            break;
+        default:
+            break;
+    }
+    CsrUint8Des((u8 *) &primitive->maxConnections, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->p2pGoParam.groupCapability, buffer, &offset);
+    CsrMemCpyDes(primitive->p2pGoParam.operatingChanList.country, buffer, &offset, ((u16) (3)));
+    CsrUint8Des((u8 *) &primitive->p2pGoParam.operatingChanList.channelEntryListCount, buffer, &offset);
+    primitive->p2pGoParam.operatingChanList.channelEntryList = NULL;
+    if (primitive->p2pGoParam.operatingChanList.channelEntryListCount)
+    {
+        primitive->p2pGoParam.operatingChanList.channelEntryList = kmalloc(sizeof(CsrWifiSmeApP2pOperatingChanEntry) * primitive->p2pGoParam.operatingChanList.channelEntryListCount, GFP_KERNEL);
+    }
+    {
+        u16 i3;
+        for (i3 = 0; i3 < primitive->p2pGoParam.operatingChanList.channelEntryListCount; i3++)
+        {
+            CsrUint8Des((u8 *) &primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingClass, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount, buffer, &offset);
+            if (primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount)
+            {
+                primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel = kmalloc(primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount, GFP_KERNEL);
+                CsrMemCpyDes(primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel, buffer, &offset, ((u16) (primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannelCount)));
+            }
+            else
+            {
+                primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel = NULL;
+            }
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->p2pGoParam.opPsEnabled, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->p2pGoParam.ctWindow, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->p2pGoParam.noaConfigMethod, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->p2pGoParam.allowNoaWithNonP2pDevices, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->wpsEnabled, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiNmeApStartReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiNmeApStartReq *primitive = (CsrWifiNmeApStartReq *) voidPrimitivePointer;
+    switch (primitive->apCredentials.authType)
+    {
+        case CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL:
+            switch (primitive->apCredentials.nmeAuthType.authTypePersonal.pskOrPassphrase)
+            {
+                case CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PASSPHRASE:
+                    kfree(primitive->apCredentials.nmeAuthType.authTypePersonal.authPers_credentials.passphrase.passphrase);
+                    break;
+                default:
+                    break;
+            }
+            break;
+        default:
+            break;
+    }
+    {
+        u16 i3;
+        for (i3 = 0; i3 < primitive->p2pGoParam.operatingChanList.channelEntryListCount; i3++)
+        {
+            kfree(primitive->p2pGoParam.operatingChanList.channelEntryList[i3].operatingChannel);
+        }
+    }
+    kfree(primitive->p2pGoParam.operatingChanList.channelEntryList);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiNmeApWmmParamUpdateReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 51) */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 4; i1++)
+        {
+            bufferSize += 1; /* u8 primitive->wmmApParams[i1].cwMin */
+            bufferSize += 1; /* u8 primitive->wmmApParams[i1].cwMax */
+            bufferSize += 1; /* u8 primitive->wmmApParams[i1].aifs */
+            bufferSize += 2; /* u16 primitive->wmmApParams[i1].txopLimit */
+            bufferSize += 1; /* u8 primitive->wmmApParams[i1].admissionControlMandatory */
+        }
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 4; i1++)
+        {
+            bufferSize += 1; /* u8 primitive->wmmApBcParams[i1].cwMin */
+            bufferSize += 1; /* u8 primitive->wmmApBcParams[i1].cwMax */
+            bufferSize += 1; /* u8 primitive->wmmApBcParams[i1].aifs */
+            bufferSize += 2; /* u16 primitive->wmmApBcParams[i1].txopLimit */
+            bufferSize += 1; /* u8 primitive->wmmApBcParams[i1].admissionControlMandatory */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApWmmParamUpdateReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApWmmParamUpdateReq *primitive = (CsrWifiNmeApWmmParamUpdateReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 4; i1++)
+        {
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApParams[i1].cwMin);
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApParams[i1].cwMax);
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApParams[i1].aifs);
+            CsrUint16Ser(ptr, len, (u16) primitive->wmmApParams[i1].txopLimit);
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApParams[i1].admissionControlMandatory);
+        }
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 4; i1++)
+        {
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApBcParams[i1].cwMin);
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApBcParams[i1].cwMax);
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApBcParams[i1].aifs);
+            CsrUint16Ser(ptr, len, (u16) primitive->wmmApBcParams[i1].txopLimit);
+            CsrUint8Ser(ptr, len, (u8) primitive->wmmApBcParams[i1].admissionControlMandatory);
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApWmmParamUpdateReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApWmmParamUpdateReq *primitive = kmalloc(sizeof(CsrWifiNmeApWmmParamUpdateReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 4; i1++)
+        {
+            CsrUint8Des((u8 *) &primitive->wmmApParams[i1].cwMin, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->wmmApParams[i1].cwMax, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->wmmApParams[i1].aifs, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->wmmApParams[i1].txopLimit, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->wmmApParams[i1].admissionControlMandatory, buffer, &offset);
+        }
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 4; i1++)
+        {
+            CsrUint8Des((u8 *) &primitive->wmmApBcParams[i1].cwMin, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->wmmApBcParams[i1].cwMax, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->wmmApBcParams[i1].aifs, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->wmmApBcParams[i1].txopLimit, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->wmmApBcParams[i1].admissionControlMandatory, buffer, &offset);
+        }
+    }
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApStaRemoveReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 12) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->staMacAddress.a[6] */
+    bufferSize += 1; /* u8 primitive->keepBlocking */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApStaRemoveReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApStaRemoveReq *primitive = (CsrWifiNmeApStaRemoveReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->staMacAddress.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->keepBlocking);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApStaRemoveReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApStaRemoveReq *primitive = kmalloc(sizeof(CsrWifiNmeApStaRemoveReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->staMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->keepBlocking, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApWpsRegisterCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApWpsRegisterCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApWpsRegisterCfm *primitive = (CsrWifiNmeApWpsRegisterCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApWpsRegisterCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApWpsRegisterCfm *primitive = kmalloc(sizeof(CsrWifiNmeApWpsRegisterCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApStartCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 40) */
+    bufferSize += 2;  /* u16 primitive->interfaceTag */
+    bufferSize += 2;  /* CsrResult primitive->status */
+    bufferSize += 32; /* u8 primitive->ssid.ssid[32] */
+    bufferSize += 1;  /* u8 primitive->ssid.length */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApStartCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApStartCfm *primitive = (CsrWifiNmeApStartCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrMemCpySer(ptr, len, (const void *) primitive->ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->ssid.length);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApStartCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApStartCfm *primitive = kmalloc(sizeof(CsrWifiNmeApStartCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrMemCpyDes(primitive->ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->ssid.length, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApStopCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApStopCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApStopCfm *primitive = (CsrWifiNmeApStopCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApStopCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApStopCfm *primitive = kmalloc(sizeof(CsrWifiNmeApStopCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApStopIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeApType primitive->apType */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApStopIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApStopInd *primitive = (CsrWifiNmeApStopInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->apType);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApStopIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApStopInd *primitive = kmalloc(sizeof(CsrWifiNmeApStopInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->apType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiNmeApStationIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 18) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeMediaStatus primitive->mediaStatus */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 6; /* u8 primitive->peerDeviceAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiNmeApStationIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiNmeApStationInd *primitive = (CsrWifiNmeApStationInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->mediaStatus);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerDeviceAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiNmeApStationIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiNmeApStationInd *primitive = kmalloc(sizeof(CsrWifiNmeApStationInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mediaStatus, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrMemCpyDes(primitive->peerDeviceAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+#endif /* CSR_WIFI_NME_ENABLE */
+#endif /* CSR_WIFI_AP_ENABLE */
diff --git a/drivers/staging/csr/csr_wifi_nme_ap_serialize.h b/drivers/staging/csr/csr_wifi_nme_ap_serialize.h
new file mode 100644
index 00000000000..0f578294722
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_ap_serialize.h
@@ -0,0 +1,103 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_AP_SERIALIZE_H__
+#define CSR_WIFI_NME_AP_SERIALIZE_H__
+
+#include "csr_wifi_msgconv.h"
+
+#include "csr_wifi_nme_ap_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_serialize.h
+#endif
+#ifndef CSR_WIFI_AP_ENABLE
+#error CSR_WIFI_AP_ENABLE MUST be defined inorder to use csr_wifi_nme_ap_serialize.h
+#endif
+
+extern void CsrWifiNmeApPfree(void *ptr);
+
+extern u8* CsrWifiNmeApConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApConfigSetReqSizeof(void *msg);
+extern void CsrWifiNmeApConfigSetReqSerFree(void *msg);
+
+extern u8* CsrWifiNmeApWpsRegisterReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApWpsRegisterReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApWpsRegisterReqSizeof(void *msg);
+#define CsrWifiNmeApWpsRegisterReqSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApStartReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApStartReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApStartReqSizeof(void *msg);
+extern void CsrWifiNmeApStartReqSerFree(void *msg);
+
+#define CsrWifiNmeApStopReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeApStopReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeApStopReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeApStopReqSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApWmmParamUpdateReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApWmmParamUpdateReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApWmmParamUpdateReqSizeof(void *msg);
+#define CsrWifiNmeApWmmParamUpdateReqSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApStaRemoveReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApStaRemoveReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApStaRemoveReqSizeof(void *msg);
+#define CsrWifiNmeApStaRemoveReqSerFree CsrWifiNmeApPfree
+
+#define CsrWifiNmeApConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeApConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeApConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeApConfigSetCfmSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApWpsRegisterCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApWpsRegisterCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApWpsRegisterCfmSizeof(void *msg);
+#define CsrWifiNmeApWpsRegisterCfmSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApStartCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApStartCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApStartCfmSizeof(void *msg);
+#define CsrWifiNmeApStartCfmSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApStopCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApStopCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApStopCfmSizeof(void *msg);
+#define CsrWifiNmeApStopCfmSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApStopIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApStopIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApStopIndSizeof(void *msg);
+#define CsrWifiNmeApStopIndSerFree CsrWifiNmeApPfree
+
+#define CsrWifiNmeApWmmParamUpdateCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeApWmmParamUpdateCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeApWmmParamUpdateCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeApWmmParamUpdateCfmSerFree CsrWifiNmeApPfree
+
+extern u8* CsrWifiNmeApStationIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeApStationIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeApStationIndSizeof(void *msg);
+#define CsrWifiNmeApStationIndSerFree CsrWifiNmeApPfree
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* CSR_WIFI_NME_AP_SERIALIZE_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_nme_converter_init.h b/drivers/staging/csr/csr_wifi_nme_converter_init.h
new file mode 100644
index 00000000000..6661914fb40
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_converter_init.h
@@ -0,0 +1,46 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_CONVERTER_INIT_H__
+#define CSR_WIFI_NME_CONVERTER_INIT_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_converter_init.h
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_NME_MODULE
+
+#include "csr_msgconv.h"
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+
+extern const CsrLogPrimitiveInformation* CsrWifiNmeTechInfoGet(void);
+#endif /* CSR_LOG_ENABLE */
+
+extern void CsrWifiNmeConverterInit(void);
+
+#else /* EXCLUDE_CSR_WIFI_NME_MODULE */
+
+#define CsrWifiNmeConverterInit()
+
+#endif /* EXCLUDE_CSR_WIFI_NME_MODULE */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_CONVERTER_INIT_H__ */
diff --git a/drivers/staging/csr/csr_wifi_nme_lib.h b/drivers/staging/csr/csr_wifi_nme_lib.h
new file mode 100644
index 00000000000..709ece46497
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_lib.h
@@ -0,0 +1,1054 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_LIB_H__
+#define CSR_WIFI_NME_LIB_H__
+
+#include "csr_sched.h"
+#include "csr_macro.h"
+#include "csr_msg_transport.h"
+
+#include "csr_wifi_lib.h"
+
+#include "csr_wifi_nme_prim.h"
+#include "csr_wifi_nme_task.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_lib.h
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiNmeFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_NME upstream message. Does not
+ *      free the message itself, and can only be used for upstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_NME upstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiNmeFreeUpstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiNmeFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_NME downstream message. Does not
+ *      free the message itself, and can only be used for downstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_NME downstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiNmeFreeDownstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ * Enum to string functions
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiNmeAuthModeToString(CsrWifiNmeAuthMode value);
+const char* CsrWifiNmeBssTypeToString(CsrWifiNmeBssType value);
+const char* CsrWifiNmeCcxOptionsMaskToString(CsrWifiNmeCcxOptionsMask value);
+const char* CsrWifiNmeConfigActionToString(CsrWifiNmeConfigAction value);
+const char* CsrWifiNmeConnectionStatusToString(CsrWifiNmeConnectionStatus value);
+const char* CsrWifiNmeCredentialTypeToString(CsrWifiNmeCredentialType value);
+const char* CsrWifiNmeEapMethodToString(CsrWifiNmeEapMethod value);
+const char* CsrWifiNmeEncryptionToString(CsrWifiNmeEncryption value);
+const char* CsrWifiNmeIndicationsToString(CsrWifiNmeIndications value);
+const char* CsrWifiNmeSecErrorToString(CsrWifiNmeSecError value);
+const char* CsrWifiNmeSimCardTypeToString(CsrWifiNmeSimCardType value);
+const char* CsrWifiNmeUmtsAuthResultToString(CsrWifiNmeUmtsAuthResult value);
+const char* CsrWifiNmeWmmQosInfoToString(CsrWifiNmeWmmQosInfo value);
+
+
+/*----------------------------------------------------------------------------*
+ * CsrPrim Type toString function.
+ * Converts a message type to the String name of the Message
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiNmePrimTypeToString(CsrPrim msgType);
+
+/*----------------------------------------------------------------------------*
+ * Lookup arrays for PrimType name Strings
+ *----------------------------------------------------------------------------*/
+extern const char *CsrWifiNmeUpstreamPrimNames[CSR_WIFI_NME_PRIM_UPSTREAM_COUNT];
+extern const char *CsrWifiNmeDownstreamPrimNames[CSR_WIFI_NME_PRIM_DOWNSTREAM_COUNT];
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConnectionStatusGetReqSend
+
+  DESCRIPTION
+    Requests the current connection status of the NME.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiNmeConnectionStatusGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeConnectionStatusGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_CONNECTION_STATUS_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiNmeConnectionStatusGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiNmeConnectionStatusGetReq *msg__; \
+        CsrWifiNmeConnectionStatusGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeConnectionStatusGetReqSend(src__, interfaceTag__) \
+    CsrWifiNmeConnectionStatusGetReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConnectionStatusGetCfmSend
+
+  DESCRIPTION
+    Reports the connection status of the NME.
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    status           - Indicates the success or otherwise of the requested
+                       operation.
+    connectionStatus - NME current connection status
+
+*******************************************************************************/
+#define CsrWifiNmeConnectionStatusGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionStatus__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeConnectionStatusGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_CONNECTION_STATUS_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->connectionStatus = (connectionStatus__);
+
+#define CsrWifiNmeConnectionStatusGetCfmSendTo(dst__, src__, interfaceTag__, status__, connectionStatus__) \
+    { \
+        CsrWifiNmeConnectionStatusGetCfm *msg__; \
+        CsrWifiNmeConnectionStatusGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionStatus__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeConnectionStatusGetCfmSend(dst__, interfaceTag__, status__, connectionStatus__) \
+    CsrWifiNmeConnectionStatusGetCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__, connectionStatus__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEventMaskSetReqSend
+
+  DESCRIPTION
+    The wireless manager application may register with the NME to receive
+    notification of interesting events. Indications will be sent only if the
+    wireless manager explicitly registers to be notified of that event.
+    indMask is a bit mask of values defined in CsrWifiNmeIndicationsMask.
+
+  PARAMETERS
+    queue   - Message Source Task Queue (Cfm's will be sent to this Queue)
+    indMask - Set mask with values from CsrWifiNmeIndications
+
+*******************************************************************************/
+#define CsrWifiNmeEventMaskSetReqCreate(msg__, dst__, src__, indMask__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeEventMaskSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_EVENT_MASK_SET_REQ, dst__, src__); \
+    msg__->indMask = (indMask__);
+
+#define CsrWifiNmeEventMaskSetReqSendTo(dst__, src__, indMask__) \
+    { \
+        CsrWifiNmeEventMaskSetReq *msg__; \
+        CsrWifiNmeEventMaskSetReqCreate(msg__, dst__, src__, indMask__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeEventMaskSetReqSend(src__, indMask__) \
+    CsrWifiNmeEventMaskSetReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, indMask__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEventMaskSetCfmSend
+
+  DESCRIPTION
+    The NME calls the primitive to report the result of the request
+    primitive.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiNmeEventMaskSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeEventMaskSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_EVENT_MASK_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeEventMaskSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeEventMaskSetCfm *msg__; \
+        CsrWifiNmeEventMaskSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeEventMaskSetCfmSend(dst__, status__) \
+    CsrWifiNmeEventMaskSetCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileConnectReqSend
+
+  DESCRIPTION
+    Requests the NME to attempt to connect to the specified profile.
+    Overrides any current connection attempt.
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    profileIdentity - Identity (BSSID, SSID) of profile to be connected to.
+                      It must match an existing profile in the NME.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileConnectReqCreate(msg__, dst__, src__, interfaceTag__, profileIdentity__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileConnectReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_CONNECT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->profileIdentity = (profileIdentity__);
+
+#define CsrWifiNmeProfileConnectReqSendTo(dst__, src__, interfaceTag__, profileIdentity__) \
+    { \
+        CsrWifiNmeProfileConnectReq *msg__; \
+        CsrWifiNmeProfileConnectReqCreate(msg__, dst__, src__, interfaceTag__, profileIdentity__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileConnectReqSend(src__, interfaceTag__, profileIdentity__) \
+    CsrWifiNmeProfileConnectReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__, profileIdentity__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileConnectCfmSend
+
+  DESCRIPTION
+    Reports the status of the NME PROFILE CONNECT REQ. If unsuccessful the
+    connectAttempt parameters contain details of the APs that the NME
+    attempted to connect to before reporting the failure of the request.
+
+  PARAMETERS
+    queue                - Destination Task Queue
+    interfaceTag         - Interface Identifier; unique identifier of an
+                           interface
+    status               - Indicates the success or otherwise of the requested
+                           operation.
+    connectAttemptsCount - This parameter is relevant only if
+                           status!=CSR_WIFI_NME_STATUS_SUCCESS.
+                           Number of connection attempt elements provided with
+                           this primitive
+    connectAttempts      - This parameter is relevant only if
+                           status!=CSR_WIFI_NME_STATUS_SUCCESS.
+                           Points to the list of connection attempt elements
+                           provided with this primitive
+                           Each element of the list provides information about
+                           an AP on which the connection attempt was made and
+                           the error that occurred during the attempt.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileConnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectAttemptsCount__, connectAttempts__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileConnectCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_CONNECT_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->connectAttemptsCount = (connectAttemptsCount__); \
+    msg__->connectAttempts = (connectAttempts__);
+
+#define CsrWifiNmeProfileConnectCfmSendTo(dst__, src__, interfaceTag__, status__, connectAttemptsCount__, connectAttempts__) \
+    { \
+        CsrWifiNmeProfileConnectCfm *msg__; \
+        CsrWifiNmeProfileConnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectAttemptsCount__, connectAttempts__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileConnectCfmSend(dst__, interfaceTag__, status__, connectAttemptsCount__, connectAttempts__) \
+    CsrWifiNmeProfileConnectCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__, connectAttemptsCount__, connectAttempts__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteAllReqSend
+
+  DESCRIPTION
+    Deletes all profiles present in the NME, but does NOT modify the
+    preferred profile list.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiNmeProfileDeleteAllReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileDeleteAllReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_DELETE_ALL_REQ, dst__, src__);
+
+#define CsrWifiNmeProfileDeleteAllReqSendTo(dst__, src__) \
+    { \
+        CsrWifiNmeProfileDeleteAllReq *msg__; \
+        CsrWifiNmeProfileDeleteAllReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileDeleteAllReqSend(src__) \
+    CsrWifiNmeProfileDeleteAllReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteAllCfmSend
+
+  DESCRIPTION
+    Reports the status of the CSR_WIFI_NME_PROFILE_DELETE_ALL_REQ.
+    Returns always CSR_WIFI_NME_STATUS_SUCCESS.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Indicates the success or otherwise of the requested operation, but
+             in this case it always set to success.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileDeleteAllCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileDeleteAllCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_DELETE_ALL_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeProfileDeleteAllCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeProfileDeleteAllCfm *msg__; \
+        CsrWifiNmeProfileDeleteAllCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileDeleteAllCfmSend(dst__, status__) \
+    CsrWifiNmeProfileDeleteAllCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteReqSend
+
+  DESCRIPTION
+    Will delete the profile with a matching identity, but does NOT modify the
+    preferred profile list.
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    profileIdentity - Identity (BSSID, SSID) of profile to be deleted.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileDeleteReqCreate(msg__, dst__, src__, profileIdentity__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileDeleteReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_DELETE_REQ, dst__, src__); \
+    msg__->profileIdentity = (profileIdentity__);
+
+#define CsrWifiNmeProfileDeleteReqSendTo(dst__, src__, profileIdentity__) \
+    { \
+        CsrWifiNmeProfileDeleteReq *msg__; \
+        CsrWifiNmeProfileDeleteReqCreate(msg__, dst__, src__, profileIdentity__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileDeleteReqSend(src__, profileIdentity__) \
+    CsrWifiNmeProfileDeleteReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, profileIdentity__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteCfmSend
+
+  DESCRIPTION
+    Reports the status of the CSR_WIFI_NME_PROFILE_DELETE_REQ.
+    Returns CSR_WIFI_NME_STATUS_NOT_FOUND if there is no matching profile.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Indicates the success or otherwise of the requested operation.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileDeleteCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileDeleteCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_DELETE_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeProfileDeleteCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeProfileDeleteCfm *msg__; \
+        CsrWifiNmeProfileDeleteCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileDeleteCfmSend(dst__, status__) \
+    CsrWifiNmeProfileDeleteCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDisconnectIndSend
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that informs that application that the current profile
+    connection has disconnected. The indication will contain information
+    about APs that it attempted to maintain the connection via i.e. in the
+    case of failed roaming.
+
+  PARAMETERS
+    queue                - Destination Task Queue
+    interfaceTag         - Interface Identifier; unique identifier of an
+                           interface
+    connectAttemptsCount - Number of connection attempt elements provided with
+                           this primitive
+    connectAttempts      - Points to the list of connection attempt elements
+                           provided with this primitive
+                           Each element of the list provides information about
+                           an AP on which the connection attempt was made and
+                           the error occurred during the attempt.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileDisconnectIndCreate(msg__, dst__, src__, interfaceTag__, connectAttemptsCount__, connectAttempts__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileDisconnectInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_DISCONNECT_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->connectAttemptsCount = (connectAttemptsCount__); \
+    msg__->connectAttempts = (connectAttempts__);
+
+#define CsrWifiNmeProfileDisconnectIndSendTo(dst__, src__, interfaceTag__, connectAttemptsCount__, connectAttempts__) \
+    { \
+        CsrWifiNmeProfileDisconnectInd *msg__; \
+        CsrWifiNmeProfileDisconnectIndCreate(msg__, dst__, src__, interfaceTag__, connectAttemptsCount__, connectAttempts__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileDisconnectIndSend(dst__, interfaceTag__, connectAttemptsCount__, connectAttempts__) \
+    CsrWifiNmeProfileDisconnectIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, connectAttemptsCount__, connectAttempts__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileOrderSetReqSend
+
+  DESCRIPTION
+    Defines the preferred order that profiles present in the NME should be
+    used during the NME auto-connect behaviour.
+    If profileIdentitysCount == 0, it removes any existing preferred profile
+    list already present in the NME, effectively disabling the auto-connect
+    behaviour.
+    NOTE: Profile identities that do not match any profile stored in the NME
+    are ignored during the auto-connect procedure.
+    NOTE: during auto-connect the NME will only attempt to join an existing
+    adhoc network and it will never attempt to host an adhoc network; for
+    hosting and adhoc network, use CSR_WIFI_NME_PROFILE_CONNECT_REQ
+
+  PARAMETERS
+    queue                 - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag          - Interface Identifier; unique identifier of an
+                            interface
+    profileIdentitysCount - The number of profiles identities in the list.
+    profileIdentitys      - Points to the list of profile identities.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileOrderSetReqCreate(msg__, dst__, src__, interfaceTag__, profileIdentitysCount__, profileIdentitys__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileOrderSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_ORDER_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->profileIdentitysCount = (profileIdentitysCount__); \
+    msg__->profileIdentitys = (profileIdentitys__);
+
+#define CsrWifiNmeProfileOrderSetReqSendTo(dst__, src__, interfaceTag__, profileIdentitysCount__, profileIdentitys__) \
+    { \
+        CsrWifiNmeProfileOrderSetReq *msg__; \
+        CsrWifiNmeProfileOrderSetReqCreate(msg__, dst__, src__, interfaceTag__, profileIdentitysCount__, profileIdentitys__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileOrderSetReqSend(src__, interfaceTag__, profileIdentitysCount__, profileIdentitys__) \
+    CsrWifiNmeProfileOrderSetReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__, profileIdentitysCount__, profileIdentitys__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileOrderSetCfmSend
+
+  DESCRIPTION
+    Confirmation to UNIFI_NME_PROFILE_ORDER_SET.request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Indicates the success or otherwise of the requested
+                   operation.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileOrderSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileOrderSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_ORDER_SET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeProfileOrderSetCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiNmeProfileOrderSetCfm *msg__; \
+        CsrWifiNmeProfileOrderSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileOrderSetCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiNmeProfileOrderSetCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileSetReqSend
+
+  DESCRIPTION
+    Creates or updates an existing profile in the NME that matches the unique
+    identity of the profile. Each profile is identified by the combination of
+    BSSID and SSID. The profile contains all the required credentials for
+    attempting to connect to the network. Creating or updating a profile via
+    the NME PROFILE SET REQ does NOT add the profile to the preferred profile
+    list within the NME used for the NME auto-connect behaviour.
+
+  PARAMETERS
+    queue   - Message Source Task Queue (Cfm's will be sent to this Queue)
+    profile - Specifies the identity and credentials of the network.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileSetReqCreate(msg__, dst__, src__, profile__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_SET_REQ, dst__, src__); \
+    msg__->profile = (profile__);
+
+#define CsrWifiNmeProfileSetReqSendTo(dst__, src__, profile__) \
+    { \
+        CsrWifiNmeProfileSetReq *msg__; \
+        CsrWifiNmeProfileSetReqCreate(msg__, dst__, src__, profile__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileSetReqSend(src__, profile__) \
+    CsrWifiNmeProfileSetReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, profile__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileSetCfmSend
+
+  DESCRIPTION
+    Reports the status of the NME PROFILE SET REQ; the request will only fail
+    if the details specified in the profile contains an invalid combination
+    of parameters for example specifying the profile as cloaked but not
+    specifying the SSID. The NME doesn't limit the number of profiles that
+    may be created. The NME assumes that the entity configuring it is aware
+    of the appropriate limits.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Indicates the success or otherwise of the requested operation.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeProfileSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeProfileSetCfm *msg__; \
+        CsrWifiNmeProfileSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileSetCfmSend(dst__, status__) \
+    CsrWifiNmeProfileSetCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileUpdateIndSend
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that informs that application that the contained profile has
+    changed.
+    For example, either the credentials EAP-FAST PAC file or the session data
+    within the profile has changed.
+    It is up to the application whether it stores this updated profile or
+    not.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    profile      - The identity and credentials of the network.
+
+*******************************************************************************/
+#define CsrWifiNmeProfileUpdateIndCreate(msg__, dst__, src__, interfaceTag__, profile__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeProfileUpdateInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_PROFILE_UPDATE_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->profile = (profile__);
+
+#define CsrWifiNmeProfileUpdateIndSendTo(dst__, src__, interfaceTag__, profile__) \
+    { \
+        CsrWifiNmeProfileUpdateInd *msg__; \
+        CsrWifiNmeProfileUpdateIndCreate(msg__, dst__, src__, interfaceTag__, profile__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeProfileUpdateIndSend(dst__, interfaceTag__, profile__) \
+    CsrWifiNmeProfileUpdateIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, profile__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimGsmAuthIndSend
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that requests the UICC Manager to perform a GSM
+    authentication on behalf of the NME. This indication is generated when
+    the NME is attempting to connect to a profile configured for EAP-SIM. An
+    application MUST register to receive this indication for the NME to
+    support the EAP-SIM credential types. Otherwise the NME has no route to
+    obtain the information from the UICC. EAP-SIM authentication requires 2
+    or 3 GSM authentication rounds and therefore 2 or 3 RANDS (GSM Random
+    Challenges) are included.
+
+  PARAMETERS
+    queue       - Destination Task Queue
+    randsLength - GSM RAND is 16 bytes long hence valid values are 32 (2 RANDS)
+                  or 48 (3 RANDs).
+    rands       - 2 or 3 RANDs values.
+
+*******************************************************************************/
+#define CsrWifiNmeSimGsmAuthIndCreate(msg__, dst__, src__, randsLength__, rands__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeSimGsmAuthInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_SIM_GSM_AUTH_IND, dst__, src__); \
+    msg__->randsLength = (randsLength__); \
+    msg__->rands = (rands__);
+
+#define CsrWifiNmeSimGsmAuthIndSendTo(dst__, src__, randsLength__, rands__) \
+    { \
+        CsrWifiNmeSimGsmAuthInd *msg__; \
+        CsrWifiNmeSimGsmAuthIndCreate(msg__, dst__, src__, randsLength__, rands__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeSimGsmAuthIndSend(dst__, randsLength__, rands__) \
+    CsrWifiNmeSimGsmAuthIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, randsLength__, rands__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimGsmAuthResSend
+
+  DESCRIPTION
+    Response from the application that received the NME SIM GSM AUTH IND. For
+    each GSM authentication round a GSM Ciphering key (Kc) and a signed
+    response (SRES) are produced. Since 2 or 3 GSM authentication rounds are
+    used the 2 or 3 Kc's obtained respectively are combined into one buffer
+    and similarly the 2 or 3 SRES's obtained are combined into another
+    buffer. The order of Kc values (SRES values respectively) in their buffer
+    is the same as that of their corresponding RAND values in the incoming
+    indication.
+
+  PARAMETERS
+    status     - Indicates the outcome of the requested operation:
+                 STATUS_SUCCESS or STATUS_ERROR
+    kcsLength  - Length in Bytes of Kc buffer. Legal values are: 16 or 24.
+    kcs        - Kc buffer holding 2 or 3 Kc values.
+    sresLength - Length in Bytes of SRES buffer. Legal values are: 8 or 12.
+    sres       - SRES buffer holding 2 or 3 SRES values.
+
+*******************************************************************************/
+#define CsrWifiNmeSimGsmAuthResCreate(msg__, dst__, src__, status__, kcsLength__, kcs__, sresLength__, sres__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeSimGsmAuthRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_SIM_GSM_AUTH_RES, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->kcsLength = (kcsLength__); \
+    msg__->kcs = (kcs__); \
+    msg__->sresLength = (sresLength__); \
+    msg__->sres = (sres__);
+
+#define CsrWifiNmeSimGsmAuthResSendTo(dst__, src__, status__, kcsLength__, kcs__, sresLength__, sres__) \
+    { \
+        CsrWifiNmeSimGsmAuthRes *msg__; \
+        CsrWifiNmeSimGsmAuthResCreate(msg__, dst__, src__, status__, kcsLength__, kcs__, sresLength__, sres__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeSimGsmAuthResSend(src__, status__, kcsLength__, kcs__, sresLength__, sres__) \
+    CsrWifiNmeSimGsmAuthResSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, status__, kcsLength__, kcs__, sresLength__, sres__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimImsiGetIndSend
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that requests the IMSI and UICC type from the UICC Manager.
+    This indication is generated when the NME is attempting to connect to a
+    profile configured for EAP-SIM/AKA. An application MUST register to
+    receive this indication for the NME to support the EAP-SIM/AKA credential
+    types. Otherwise the NME has no route to obtain the information from the
+    UICC.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+
+*******************************************************************************/
+#define CsrWifiNmeSimImsiGetIndCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeSimImsiGetInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_SIM_IMSI_GET_IND, dst__, src__);
+
+#define CsrWifiNmeSimImsiGetIndSendTo(dst__, src__) \
+    { \
+        CsrWifiNmeSimImsiGetInd *msg__; \
+        CsrWifiNmeSimImsiGetIndCreate(msg__, dst__, src__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeSimImsiGetIndSend(dst__) \
+    CsrWifiNmeSimImsiGetIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimImsiGetResSend
+
+  DESCRIPTION
+    Response from the application that received the NME SIM IMSI GET IND.
+
+  PARAMETERS
+    status   - Indicates the outcome of the requested operation: STATUS_SUCCESS
+               or STATUS_ERROR.
+    imsi     - The value of the IMSI obtained from the UICC.
+    cardType - The UICC type (GSM only (SIM), UMTS only (USIM), Both).
+
+*******************************************************************************/
+#define CsrWifiNmeSimImsiGetResCreate(msg__, dst__, src__, status__, imsi__, cardType__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeSimImsiGetRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_SIM_IMSI_GET_RES, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->imsi = (imsi__); \
+    msg__->cardType = (cardType__);
+
+#define CsrWifiNmeSimImsiGetResSendTo(dst__, src__, status__, imsi__, cardType__) \
+    { \
+        CsrWifiNmeSimImsiGetRes *msg__; \
+        CsrWifiNmeSimImsiGetResCreate(msg__, dst__, src__, status__, imsi__, cardType__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeSimImsiGetResSend(src__, status__, imsi__, cardType__) \
+    CsrWifiNmeSimImsiGetResSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, status__, imsi__, cardType__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimUmtsAuthIndSend
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that requests the UICC Manager to perform a UMTS
+    authentication on behalf of the NME. This indication is generated when
+    the NME is attempting to connect to a profile configured for EAP-AKA. An
+    application MUST register to receive this indication for the NME to
+    support the EAP-AKA credential types. Otherwise the NME has no route to
+    obtain the information from the USIM. EAP-AKA requires one UMTS
+    authentication round and therefore only one RAND and one AUTN values are
+    included.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    rand   - UMTS RAND value.
+    autn   - UMTS AUTN value.
+
+*******************************************************************************/
+#define CsrWifiNmeSimUmtsAuthIndCreate(msg__, dst__, src__, rand__, autn__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeSimUmtsAuthInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_SIM_UMTS_AUTH_IND, dst__, src__); \
+    memcpy(msg__->rand, (rand__), sizeof(u8) * 16); \
+    memcpy(msg__->autn, (autn__), sizeof(u8) * 16);
+
+#define CsrWifiNmeSimUmtsAuthIndSendTo(dst__, src__, rand__, autn__) \
+    { \
+        CsrWifiNmeSimUmtsAuthInd *msg__; \
+        CsrWifiNmeSimUmtsAuthIndCreate(msg__, dst__, src__, rand__, autn__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeSimUmtsAuthIndSend(dst__, rand__, autn__) \
+    CsrWifiNmeSimUmtsAuthIndSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, rand__, autn__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimUmtsAuthResSend
+
+  DESCRIPTION
+    Response from the application that received the NME SIM UMTS AUTH IND.
+    The values of umtsCipherKey, umtsIntegrityKey, resParameterLength and
+    resParameter are only meanigful when result = UMTS_AUTH_RESULT_SUCCESS.
+    The value of auts is only meaningful when
+    result=UMTS_AUTH_RESULT_SYNC_FAIL.
+
+  PARAMETERS
+    status             - Indicates the outcome of the requested operation:
+                         STATUS_SUCCESS or STATUS_ERROR.
+    result             - The result of UMTS authentication as performed by the
+                         UICC which could be: Success, Authentication Reject or
+                         Synchronisation Failure. For all these 3 outcomes the
+                         value of status is success.
+    umtsCipherKey      - The UMTS Cipher Key as calculated and returned by the
+                         UICC.
+    umtsIntegrityKey   - The UMTS Integrity Key as calculated and returned by
+                         the UICC.
+    resParameterLength - The length (in bytes) of the RES parameter (min=4; max
+                         = 16).
+    resParameter       - The RES parameter as calculated and returned by the
+                         UICC.
+    auts               - The AUTS parameter as calculated and returned by the
+                         UICC.
+
+*******************************************************************************/
+#define CsrWifiNmeSimUmtsAuthResCreate(msg__, dst__, src__, status__, result__, umtsCipherKey__, umtsIntegrityKey__, resParameterLength__, resParameter__, auts__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeSimUmtsAuthRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_SIM_UMTS_AUTH_RES, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->result = (result__); \
+    memcpy(msg__->umtsCipherKey, (umtsCipherKey__), sizeof(u8) * 16); \
+    memcpy(msg__->umtsIntegrityKey, (umtsIntegrityKey__), sizeof(u8) * 16); \
+    msg__->resParameterLength = (resParameterLength__); \
+    msg__->resParameter = (resParameter__); \
+    memcpy(msg__->auts, (auts__), sizeof(u8) * 14);
+
+#define CsrWifiNmeSimUmtsAuthResSendTo(dst__, src__, status__, result__, umtsCipherKey__, umtsIntegrityKey__, resParameterLength__, resParameter__, auts__) \
+    { \
+        CsrWifiNmeSimUmtsAuthRes *msg__; \
+        CsrWifiNmeSimUmtsAuthResCreate(msg__, dst__, src__, status__, result__, umtsCipherKey__, umtsIntegrityKey__, resParameterLength__, resParameter__, auts__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeSimUmtsAuthResSend(src__, status__, result__, umtsCipherKey__, umtsIntegrityKey__, resParameterLength__, resParameter__, auts__) \
+    CsrWifiNmeSimUmtsAuthResSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, status__, result__, umtsCipherKey__, umtsIntegrityKey__, resParameterLength__, resParameter__, auts__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsCancelReqSend
+
+  DESCRIPTION
+    Requests the NME to cancel any WPS procedure that it is currently
+    performing. This includes WPS registrar activities started because of
+    CSR_WIFI_NME_AP_REGISTER.request
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiNmeWpsCancelReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeWpsCancelReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_WPS_CANCEL_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiNmeWpsCancelReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiNmeWpsCancelReq *msg__; \
+        CsrWifiNmeWpsCancelReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeWpsCancelReqSend(src__, interfaceTag__) \
+    CsrWifiNmeWpsCancelReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsCancelCfmSend
+
+  DESCRIPTION
+    Reports the status of the NME WPS REQ, the request is always SUCCESSFUL.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Only returns CSR_WIFI_NME_STATUS_SUCCESS
+
+*******************************************************************************/
+#define CsrWifiNmeWpsCancelCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeWpsCancelCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_WPS_CANCEL_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeWpsCancelCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiNmeWpsCancelCfm *msg__; \
+        CsrWifiNmeWpsCancelCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeWpsCancelCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiNmeWpsCancelCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsCfmSend
+
+  DESCRIPTION
+    Reports the status of the NME WPS REQ.
+    If CSR_WIFI_NME_STATUS_SUCCESS, the profile parameter contains the
+    identity and credentials of the AP.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Indicates the success or otherwise of the requested
+                   operation.
+    profile      - This parameter is relevant only if
+                   status==CSR_WIFI_NME_STATUS_SUCCESS.
+                   The identity and credentials of the network.
+
+*******************************************************************************/
+#define CsrWifiNmeWpsCfmCreate(msg__, dst__, src__, interfaceTag__, status__, profile__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeWpsCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_WPS_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->profile = (profile__);
+
+#define CsrWifiNmeWpsCfmSendTo(dst__, src__, interfaceTag__, status__, profile__) \
+    { \
+        CsrWifiNmeWpsCfm *msg__; \
+        CsrWifiNmeWpsCfmCreate(msg__, dst__, src__, interfaceTag__, status__, profile__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeWpsCfmSend(dst__, interfaceTag__, status__, profile__) \
+    CsrWifiNmeWpsCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, interfaceTag__, status__, profile__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsConfigSetReqSend
+
+  DESCRIPTION
+    This primitive passes the WPS information for the device to NME. This may
+    be accepted only if no interface is active.
+
+  PARAMETERS
+    queue     - Message Source Task Queue (Cfm's will be sent to this Queue)
+    wpsConfig - WPS config.
+
+*******************************************************************************/
+#define CsrWifiNmeWpsConfigSetReqCreate(msg__, dst__, src__, wpsConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeWpsConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_WPS_CONFIG_SET_REQ, dst__, src__); \
+    msg__->wpsConfig = (wpsConfig__);
+
+#define CsrWifiNmeWpsConfigSetReqSendTo(dst__, src__, wpsConfig__) \
+    { \
+        CsrWifiNmeWpsConfigSetReq *msg__; \
+        CsrWifiNmeWpsConfigSetReqCreate(msg__, dst__, src__, wpsConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeWpsConfigSetReqSend(src__, wpsConfig__) \
+    CsrWifiNmeWpsConfigSetReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, wpsConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsConfigSetCfmSend
+
+  DESCRIPTION
+    Confirm.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiNmeWpsConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeWpsConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_WPS_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiNmeWpsConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiNmeWpsConfigSetCfm *msg__; \
+        CsrWifiNmeWpsConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeWpsConfigSetCfmSend(dst__, status__) \
+    CsrWifiNmeWpsConfigSetCfmSendTo(dst__, CSR_WIFI_NME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsReqSend
+
+  DESCRIPTION
+    Requests the NME to look for WPS enabled APs and attempt to perform WPS
+    to determine the appropriate security credentials to connect to the AP.
+    If the PIN == '00000000' then 'push button mode' is indicated, otherwise
+    the PIN has to match that of the AP. 4 digit pin is passed by sending the
+    pin digits in pin[0]..pin[3] and rest of the contents filled with '-'.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    pin          - PIN value.
+    ssid         - Service Set identifier
+    bssid        - ID of Basic Service Set for which a WPS connection attempt is
+                   being made.
+
+*******************************************************************************/
+#define CsrWifiNmeWpsReqCreate(msg__, dst__, src__, interfaceTag__, pin__, ssid__, bssid__) \
+    msg__ = kmalloc(sizeof(CsrWifiNmeWpsReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_NME_PRIM, CSR_WIFI_NME_WPS_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    memcpy(msg__->pin, (pin__), sizeof(u8) * 8); \
+    msg__->ssid = (ssid__); \
+    msg__->bssid = (bssid__);
+
+#define CsrWifiNmeWpsReqSendTo(dst__, src__, interfaceTag__, pin__, ssid__, bssid__) \
+    { \
+        CsrWifiNmeWpsReq *msg__; \
+        CsrWifiNmeWpsReqCreate(msg__, dst__, src__, interfaceTag__, pin__, ssid__, bssid__); \
+        CsrMsgTransport(dst__, CSR_WIFI_NME_PRIM, msg__); \
+    }
+
+#define CsrWifiNmeWpsReqSend(src__, interfaceTag__, pin__, ssid__, bssid__) \
+    CsrWifiNmeWpsReqSendTo(CSR_WIFI_NME_IFACEQUEUE, src__, interfaceTag__, pin__, ssid__, bssid__)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_nme_prim.h b/drivers/staging/csr/csr_wifi_nme_prim.h
new file mode 100644
index 00000000000..20dc77971f9
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_prim.h
@@ -0,0 +1,1666 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_PRIM_H__
+#define CSR_WIFI_NME_PRIM_H__
+
+#include <linux/types.h>
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+#include "csr_wifi_common.h"
+#include "csr_result.h"
+#include "csr_wifi_fsm_event.h"
+#include "csr_wifi_sme_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_prim.h
+#endif
+
+#define CSR_WIFI_NME_PRIM                                               (0x0424)
+
+typedef CsrPrim CsrWifiNmePrim;
+
+typedef void (*CsrWifiNmeFrameFreeFunction)(void *frame);
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeAuthMode
+
+  DESCRIPTION
+    WiFi Authentication Mode
+
+ VALUES
+    CSR_WIFI_NME_AUTH_MODE_80211_OPEN
+                   - Connects to an open system network (i.e. no authentication,
+                     no encryption) or to a WEP enabled network.
+    CSR_WIFI_NME_AUTH_MODE_80211_SHARED
+                   - Connect to a WEP enabled network.
+    CSR_WIFI_NME_AUTH_MODE_8021X_WPA
+                   - Connects to a WPA Enterprise enabled network.
+    CSR_WIFI_NME_AUTH_MODE_8021X_WPAPSK
+                   - Connects to a WPA with Pre-Shared Key enabled network.
+    CSR_WIFI_NME_AUTH_MODE_8021X_WPA2
+                   - Connects to a WPA2 Enterprise enabled network.
+    CSR_WIFI_NME_AUTH_MODE_8021X_WPA2PSK
+                   - Connects to a WPA2 with Pre-Shared Key enabled network.
+    CSR_WIFI_NME_AUTH_MODE_8021X_CCKM
+                   - Connects to a CCKM enabled network.
+    CSR_WIFI_NME_AUTH_MODE_WAPI_WAI
+                   - Connects to a WAPI Enterprise enabled network.
+    CSR_WIFI_NME_AUTH_MODE_WAPI_WAIPSK
+                   - Connects to a WAPI with Pre-Shared Key enabled network.
+    CSR_WIFI_NME_AUTH_MODE_8021X_OTHER1X
+                   - For future use.
+
+*******************************************************************************/
+typedef u16 CsrWifiNmeAuthMode;
+#define CSR_WIFI_NME_AUTH_MODE_80211_OPEN      ((CsrWifiNmeAuthMode) 0x0001)
+#define CSR_WIFI_NME_AUTH_MODE_80211_SHARED    ((CsrWifiNmeAuthMode) 0x0002)
+#define CSR_WIFI_NME_AUTH_MODE_8021X_WPA       ((CsrWifiNmeAuthMode) 0x0004)
+#define CSR_WIFI_NME_AUTH_MODE_8021X_WPAPSK    ((CsrWifiNmeAuthMode) 0x0008)
+#define CSR_WIFI_NME_AUTH_MODE_8021X_WPA2      ((CsrWifiNmeAuthMode) 0x0010)
+#define CSR_WIFI_NME_AUTH_MODE_8021X_WPA2PSK   ((CsrWifiNmeAuthMode) 0x0020)
+#define CSR_WIFI_NME_AUTH_MODE_8021X_CCKM      ((CsrWifiNmeAuthMode) 0x0040)
+#define CSR_WIFI_NME_AUTH_MODE_WAPI_WAI        ((CsrWifiNmeAuthMode) 0x0080)
+#define CSR_WIFI_NME_AUTH_MODE_WAPI_WAIPSK     ((CsrWifiNmeAuthMode) 0x0100)
+#define CSR_WIFI_NME_AUTH_MODE_8021X_OTHER1X   ((CsrWifiNmeAuthMode) 0x0200)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeBssType
+
+  DESCRIPTION
+    Type of BSS
+
+ VALUES
+    CSR_WIFI_NME_BSS_TYPE_INFRASTRUCTURE
+                   - Infrastructure BSS type where access to the network is via
+                     one or several Access Points.
+    CSR_WIFI_NME_BSS_TYPE_ADHOC
+                   - Adhoc or Independent BSS Type where one Station acts as a
+                     host and future stations can join the adhoc network without
+                     needing an access point.
+    CSR_WIFI_NME_BSS_TYPE_RESERVED
+                   - To be in sync with SME.This is not used.
+    CSR_WIFI_NME_BSS_TYPE_P2P
+                   - P2P mode of operation.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeBssType;
+#define CSR_WIFI_NME_BSS_TYPE_INFRASTRUCTURE   ((CsrWifiNmeBssType) 0x00)
+#define CSR_WIFI_NME_BSS_TYPE_ADHOC            ((CsrWifiNmeBssType) 0x01)
+#define CSR_WIFI_NME_BSS_TYPE_RESERVED         ((CsrWifiNmeBssType) 0x02)
+#define CSR_WIFI_NME_BSS_TYPE_P2P              ((CsrWifiNmeBssType) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeCcxOptionsMask
+
+  DESCRIPTION
+    Enumeration type defining possible mask values for setting CCX options.
+
+ VALUES
+    CSR_WIFI_NME_CCX_OPTION_NONE - No CCX option is set.
+    CSR_WIFI_NME_CCX_OPTION_CCKM - CCX option cckm is set.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeCcxOptionsMask;
+#define CSR_WIFI_NME_CCX_OPTION_NONE   ((CsrWifiNmeCcxOptionsMask) 0x00)
+#define CSR_WIFI_NME_CCX_OPTION_CCKM   ((CsrWifiNmeCcxOptionsMask) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConfigAction
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_PIN_ENTRY_PUSH_BUTTON -
+    CSR_WIFI_PIN_ENTRY_DISPLAY_PIN -
+    CSR_WIFI_PIN_ENTRY_ENTER_PIN   -
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeConfigAction;
+#define CSR_WIFI_PIN_ENTRY_PUSH_BUTTON   ((CsrWifiNmeConfigAction) 0x00)
+#define CSR_WIFI_PIN_ENTRY_DISPLAY_PIN   ((CsrWifiNmeConfigAction) 0x01)
+#define CSR_WIFI_PIN_ENTRY_ENTER_PIN     ((CsrWifiNmeConfigAction) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConnectionStatus
+
+  DESCRIPTION
+    Indicate the NME Connection Status when connecting or when disconnecting
+
+ VALUES
+    CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_DISCONNECTED
+                   - NME is disconnected.
+    CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_CONNECTING
+                   - NME is in the process of connecting.
+    CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_AUTHENTICATING
+                   - NME is in the authentication stage of a connection attempt.
+    CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_CONNECTED
+                   - NME is connected.
+    CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_DISCONNECTING
+                   - NME is in the process of disconnecting.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeConnectionStatus;
+#define CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_DISCONNECTED     ((CsrWifiNmeConnectionStatus) 0x00)
+#define CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_CONNECTING       ((CsrWifiNmeConnectionStatus) 0x01)
+#define CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_AUTHENTICATING   ((CsrWifiNmeConnectionStatus) 0x02)
+#define CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_CONNECTED        ((CsrWifiNmeConnectionStatus) 0x03)
+#define CSR_WIFI_NME_CONNECTION_STATUS_CONNECTION_STATUS_DISCONNECTING    ((CsrWifiNmeConnectionStatus) 0x04)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeCredentialType
+
+  DESCRIPTION
+    NME Credential Types
+
+ VALUES
+    CSR_WIFI_NME_CREDENTIAL_TYPE_OPEN_SYSTEM
+                   - Credential Type Open System.
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WEP64
+                   - Credential Type WEP-64
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WEP128
+                   - Credential Type WEP-128
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WPA_PSK
+                   - Credential Type WPA Pre-Shared Key
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WPA_PASSPHRASE
+                   - Credential Type WPA pass phrase
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WPA2_PSK
+                   - Credential Type WPA2 Pre-Shared Key.
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WPA2_PASSPHRASE
+                   - Credential Type WPA2 pass phrase
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WAPI_PSK
+                   - Credential Type WAPI Pre-Shared Key.
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WAPI_PASSPHRASE
+                   - Credential Type WAPI pass phrase
+    CSR_WIFI_NME_CREDENTIAL_TYPE_WAPI
+                   - Credential Type WAPI certificates
+    CSR_WIFI_NME_CREDENTIAL_TYPE_8021X
+                   - Credential Type 802.1X: the associated type supports
+                     FAST/LEAP/TLS/TTLS/PEAP/etc.
+
+*******************************************************************************/
+typedef u16 CsrWifiNmeCredentialType;
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_OPEN_SYSTEM       ((CsrWifiNmeCredentialType) 0x0000)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WEP64             ((CsrWifiNmeCredentialType) 0x0001)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WEP128            ((CsrWifiNmeCredentialType) 0x0002)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WPA_PSK           ((CsrWifiNmeCredentialType) 0x0003)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WPA_PASSPHRASE    ((CsrWifiNmeCredentialType) 0x0004)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WPA2_PSK          ((CsrWifiNmeCredentialType) 0x0005)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WPA2_PASSPHRASE   ((CsrWifiNmeCredentialType) 0x0006)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WAPI_PSK          ((CsrWifiNmeCredentialType) 0x0007)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WAPI_PASSPHRASE   ((CsrWifiNmeCredentialType) 0x0008)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_WAPI              ((CsrWifiNmeCredentialType) 0x0009)
+#define CSR_WIFI_NME_CREDENTIAL_TYPE_8021X             ((CsrWifiNmeCredentialType) 0x000A)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEapMethod
+
+  DESCRIPTION
+    Outer EAP method with possibly inner method.
+
+ VALUES
+    CSR_WIFI_NME_EAP_METHOD_TLS
+                   - EAP-TLS Method.
+    CSR_WIFI_NME_EAP_METHOD_TTLS_MSCHAPV2
+                   - EAP-TTLS Method with MSCHAPV2.
+    CSR_WIFI_NME_EAP_METHOD_PEAP_GTC
+                   - EAP-PEAP Method with GTC.
+    CSR_WIFI_NME_EAP_METHOD_PEAP_MSCHAPV2
+                   - EAP-PEAP Method with MSCHAPV2.
+    CSR_WIFI_NME_EAP_METHOD_SIM
+                   - EAP-SIM Method.
+    CSR_WIFI_NME_EAP_METHOD_AKA
+                   - EAP-AKA Method.
+    CSR_WIFI_NME_EAP_METHOD_FAST_GTC
+                   - EAP-FAST Method with GTC.
+    CSR_WIFI_NME_EAP_METHOD_FAST_MSCHAPV2
+                   - EAP-FAST Method with MSCHAPV2.
+    CSR_WIFI_NME_EAP_METHOD_LEAP
+                   - EAP-LEAP Method.
+
+*******************************************************************************/
+typedef u16 CsrWifiNmeEapMethod;
+#define CSR_WIFI_NME_EAP_METHOD_TLS             ((CsrWifiNmeEapMethod) 0x0001)
+#define CSR_WIFI_NME_EAP_METHOD_TTLS_MSCHAPV2   ((CsrWifiNmeEapMethod) 0x0002)
+#define CSR_WIFI_NME_EAP_METHOD_PEAP_GTC        ((CsrWifiNmeEapMethod) 0x0004)
+#define CSR_WIFI_NME_EAP_METHOD_PEAP_MSCHAPV2   ((CsrWifiNmeEapMethod) 0x0008)
+#define CSR_WIFI_NME_EAP_METHOD_SIM             ((CsrWifiNmeEapMethod) 0x0010)
+#define CSR_WIFI_NME_EAP_METHOD_AKA             ((CsrWifiNmeEapMethod) 0x0020)
+#define CSR_WIFI_NME_EAP_METHOD_FAST_GTC        ((CsrWifiNmeEapMethod) 0x0040)
+#define CSR_WIFI_NME_EAP_METHOD_FAST_MSCHAPV2   ((CsrWifiNmeEapMethod) 0x0080)
+#define CSR_WIFI_NME_EAP_METHOD_LEAP            ((CsrWifiNmeEapMethod) 0x0100)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEncryption
+
+  DESCRIPTION
+    WiFi Encryption method
+
+ VALUES
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_NONE
+                   - No encryprion set.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_WEP40
+                   - 40 bytes WEP key for peer to peer communication.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_WEP104
+                   - 104 bytes WEP key for peer to peer communication.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_TKIP
+                   - TKIP key for peer to peer communication.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_CCMP
+                   - CCMP key for peer to peer communication.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_SMS4
+                   - SMS4 key for peer to peer communication.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_WEP40
+                   - 40 bytes WEP key for broadcast messages.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_WEP104
+                   - 104 bytes WEP key for broadcast messages.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_TKIP
+                   - TKIP key for broadcast messages.
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_CCMP
+                   - CCMP key for broadcast messages
+    CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_SMS4
+                   - SMS4 key for broadcast messages.
+
+*******************************************************************************/
+typedef u16 CsrWifiNmeEncryption;
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_NONE              ((CsrWifiNmeEncryption) 0x0000)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_WEP40    ((CsrWifiNmeEncryption) 0x0001)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_WEP104   ((CsrWifiNmeEncryption) 0x0002)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_TKIP     ((CsrWifiNmeEncryption) 0x0004)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_CCMP     ((CsrWifiNmeEncryption) 0x0008)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_SMS4     ((CsrWifiNmeEncryption) 0x0010)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_WEP40       ((CsrWifiNmeEncryption) 0x0020)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_WEP104      ((CsrWifiNmeEncryption) 0x0040)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_TKIP        ((CsrWifiNmeEncryption) 0x0080)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_CCMP        ((CsrWifiNmeEncryption) 0x0100)
+#define CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_SMS4        ((CsrWifiNmeEncryption) 0x0200)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeIndications
+
+  DESCRIPTION
+    NME indications
+
+ VALUES
+    CSR_WIFI_NME_INDICATIONS_IND_AP_STATION
+                   - NME AP Station Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_AP_STOP
+                   - NME AP Stop Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_SIM_UMTS_AUTH
+                   - NME UMTS Authentication Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_P2P_GROUP_START
+                   - NME P2P Group Start Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_P2P_GROUP_STATUS
+                   - NME P2P Group Status Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_P2P_GROUP_ROLE
+                   - NME P2P Group Role Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_PROFILE_DISCONNECT
+                   - NME Profile Disconnect Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_PROFILE_UPDATE
+                   - NME Profile Update Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_SIM_IMSI_GET
+                   - NME GET IMSI Indication.
+    CSR_WIFI_NME_INDICATIONS_IND_SIM_GSM_AUTH
+                   - NME GSM Authentication Indication.
+    CSR_WIFI_NME_INDICATIONS_ALL
+                   - Used to register for all available indications
+
+*******************************************************************************/
+typedef u32 CsrWifiNmeIndications;
+#define CSR_WIFI_NME_INDICATIONS_IND_AP_STATION           ((CsrWifiNmeIndications) 0x00100000)
+#define CSR_WIFI_NME_INDICATIONS_IND_AP_STOP              ((CsrWifiNmeIndications) 0x00200000)
+#define CSR_WIFI_NME_INDICATIONS_IND_SIM_UMTS_AUTH        ((CsrWifiNmeIndications) 0x01000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_P2P_GROUP_START      ((CsrWifiNmeIndications) 0x02000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_P2P_GROUP_STATUS     ((CsrWifiNmeIndications) 0x04000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_P2P_GROUP_ROLE       ((CsrWifiNmeIndications) 0x08000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_PROFILE_DISCONNECT   ((CsrWifiNmeIndications) 0x10000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_PROFILE_UPDATE       ((CsrWifiNmeIndications) 0x20000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_SIM_IMSI_GET         ((CsrWifiNmeIndications) 0x40000000)
+#define CSR_WIFI_NME_INDICATIONS_IND_SIM_GSM_AUTH         ((CsrWifiNmeIndications) 0x80000000)
+#define CSR_WIFI_NME_INDICATIONS_ALL                      ((CsrWifiNmeIndications) 0xFFFFFFFF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSecError
+
+  DESCRIPTION
+    NME Security Errors
+    place holder for the security library abort reason
+
+ VALUES
+    CSR_WIFI_NME_SEC_ERROR_SEC_ERROR_UNKNOWN
+                   - Unknown Security Error.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeSecError;
+#define CSR_WIFI_NME_SEC_ERROR_SEC_ERROR_UNKNOWN   ((CsrWifiNmeSecError) 0x00)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimCardType
+
+  DESCRIPTION
+    (U)SIM Card (or UICC) types
+
+ VALUES
+    CSR_WIFI_NME_SIM_CARD_TYPE_2G   - 2G SIM card, capable of performing GSM
+                                      authentication only.
+    CSR_WIFI_NME_SIM_CARD_TYPE_3G   - UICC supporting USIM application, capable
+                                      of performing UMTS authentication only.
+    CSR_WIFI_NME_SIM_CARD_TYPE_2G3G - UICC supporting both USIM and SIM
+                                      applications, capable of performing both
+                                      UMTS and GSM authentications.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeSimCardType;
+#define CSR_WIFI_NME_SIM_CARD_TYPE_2G     ((CsrWifiNmeSimCardType) 0x01)
+#define CSR_WIFI_NME_SIM_CARD_TYPE_3G     ((CsrWifiNmeSimCardType) 0x02)
+#define CSR_WIFI_NME_SIM_CARD_TYPE_2G3G   ((CsrWifiNmeSimCardType) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeUmtsAuthResult
+
+  DESCRIPTION
+    Only relevant for UMTS Authentication. It indicates if the UICC has
+    successfully authenticated the network or otherwise.
+
+ VALUES
+    CSR_WIFI_NME_UMTS_AUTH_RESULT_SUCCESS
+                   - Successful outcome from USIM indicating that the card has
+                     successfully authenticated the network.
+    CSR_WIFI_NME_UMTS_AUTH_RESULT_SYNC_FAIL
+                   - Unsuccessful outcome from USIM indicating that the card is
+                     requesting the network to synchronise and re-try again. If
+                     no further request is received an NME timer will expire and
+                     the authentication is aborted.
+    CSR_WIFI_NME_UMTS_AUTH_RESULT_REJECT
+                   - Unsuccessful outcome from USIM indicating that the card has
+                     rejected the network and that the authentication is
+                     aborted.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeUmtsAuthResult;
+#define CSR_WIFI_NME_UMTS_AUTH_RESULT_SUCCESS     ((CsrWifiNmeUmtsAuthResult) 0x00)
+#define CSR_WIFI_NME_UMTS_AUTH_RESULT_SYNC_FAIL   ((CsrWifiNmeUmtsAuthResult) 0x01)
+#define CSR_WIFI_NME_UMTS_AUTH_RESULT_REJECT      ((CsrWifiNmeUmtsAuthResult) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWmmQosInfo
+
+  DESCRIPTION
+    Defines bits for the QoS Info octect as defined in the WMM specification.
+    The values of this type are used across the NME/SME/Router API's and they
+    must be kept consistent with the corresponding types in the .xml of the
+    other interfaces
+
+ VALUES
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_ALL
+                   - WMM AP may deliver all buffered frames.
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_VO
+                   - To enable the triggering and delivery of QoS Voice.
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_VI
+                   - To enable the triggering and delivery of QoS Video.
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_BK
+                   - To enable the triggering and delivery of QoS Background.
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_BE
+                   - To enable the triggering and delivery of QoS Best Effort.
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_TWO
+                   - WMM AP may deliver a maximum of 2 buffered frames per
+                     Unscheduled Service Period (USP).
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_FOUR
+                   - WMM AP may deliver a maximum of 4 buffered frames per USP.
+    CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_SIX
+                   - WMM AP may deliver a maximum of 6 buffered frames per USP.
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeWmmQosInfo;
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_ALL    ((CsrWifiNmeWmmQosInfo) 0x00)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_VO            ((CsrWifiNmeWmmQosInfo) 0x01)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_VI            ((CsrWifiNmeWmmQosInfo) 0x02)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_BK            ((CsrWifiNmeWmmQosInfo) 0x04)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_BE            ((CsrWifiNmeWmmQosInfo) 0x08)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_TWO    ((CsrWifiNmeWmmQosInfo) 0x20)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_FOUR   ((CsrWifiNmeWmmQosInfo) 0x40)
+#define CSR_WIFI_NME_WMM_QOS_INFO_AC_MAX_SP_SIX    ((CsrWifiNmeWmmQosInfo) 0x60)
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEapMethodMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiNmeEapMethod.
+
+*******************************************************************************/
+typedef u16 CsrWifiNmeEapMethodMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEncryptionMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiNmeEncryption
+
+*******************************************************************************/
+typedef u16 CsrWifiNmeEncryptionMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeIndicationsMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiNmeIndications
+
+*******************************************************************************/
+typedef u32 CsrWifiNmeIndicationsMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeNmeIndicationsMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiNmeNmeIndications.
+    Used to overlap the unused portion of the unifi_IndicationsMask For NME
+    specific indications
+
+*******************************************************************************/
+typedef u32 CsrWifiNmeNmeIndicationsMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWmmQosInfoMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiNmeWmmQosInfo
+
+*******************************************************************************/
+typedef u8 CsrWifiNmeWmmQosInfoMask;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEmpty
+
+  DESCRIPTION
+    Empty Structure to indicate that no credentials are available.
+
+  MEMBERS
+    empty  - Only element of the empty structure (always set to 0).
+
+*******************************************************************************/
+typedef struct
+{
+    u8 empty;
+} CsrWifiNmeEmpty;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmePassphrase
+
+  DESCRIPTION
+    Structure holding the ASCII Pass Phrase data.
+
+  MEMBERS
+    encryptionMode - Encryption type as defined in CsrWifiSmeEncryption.
+    passphrase     - Pass phrase ASCII value.
+
+*******************************************************************************/
+typedef struct
+{
+    u16      encryptionMode;
+    char *passphrase;
+} CsrWifiNmePassphrase;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmePsk
+
+  DESCRIPTION
+    Structure holding the Pre-Shared Key data.
+
+  MEMBERS
+    encryptionMode - Encryption type as defined in CsrWifiSmeEncryption.
+    psk            - Pre-Shared Key value.
+
+*******************************************************************************/
+typedef struct
+{
+    u16 encryptionMode;
+    u8  psk[32];
+} CsrWifiNmePsk;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWapiCredentials
+
+  DESCRIPTION
+    Structure holding WAPI credentials data.
+
+  MEMBERS
+    certificateLength   - Length in bytes of the following client certificate.
+    certificate         - The actual client certificate data (if present).
+                          DER/PEM format supported.
+    privateKeyLength    - Length in bytes of the following private key.
+    privateKey          - The actual private key. DER/PEM format.
+    caCertificateLength - Length in bytes of the following certificate authority
+                          certificate.
+    caCertificate       - The actual certificate authority certificate data. If
+                          not supplied the received certificate authority
+                          certificate is assumed to be validate, if present the
+                          received certificate is validated against it. DER/PEM
+                          format supported.
+
+*******************************************************************************/
+typedef struct
+{
+    u32 certificateLength;
+    u8 *certificate;
+    u16 privateKeyLength;
+    u8 *privateKey;
+    u32 caCertificateLength;
+    u8 *caCertificate;
+} CsrWifiNmeWapiCredentials;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConnectAttempt
+
+  DESCRIPTION
+    Structure holding Connection attempt data.
+
+  MEMBERS
+    bssid         - Id of Basic Service Set connections attempt have been made
+                    to.
+    status        - Status returned to indicate the success or otherwise of the
+                    connection attempt.
+    securityError - Security error status indicating the nature of the failure
+                    to connect.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress  bssid;
+    CsrResult          status;
+    CsrWifiNmeSecError securityError;
+} CsrWifiNmeConnectAttempt;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEapCredentials
+
+  DESCRIPTION
+    Supports the use of multiple EAP methods via a single structure. The
+    methods required are indicated by the value set in the eapMethodMask
+
+  MEMBERS
+    eapMethodMask
+                   - Bit mask of supported EAP methods
+                     Currently only supports the setting of one bit.
+                     Required for all the EAP methods.
+    authMode
+                   - Bit mask representing the authentication types that may be
+                     supported by a suitable AP. An AP must support at least one
+                     of the authentication types specified to be considered for
+                     connection. Required for all EAP methods.
+    encryptionMode
+                   - Bit mask representing the encryption types that may be
+                     supported by a suitable AP. An AP must support a suitable
+                     mix of the pairwise and group encryption types requested to
+                     be considered for connection. Required for all EAP methods.
+    userName
+                   - User name. Required for all EAP methods except: SIM or AKA.
+    userPassword
+                   - User Password. Required for all EAP methods except: TLS,
+                     SIM or AKA.
+    authServerUserIdentity
+                   - Authentication server user Identity. Required for all EAP
+                     methods except: TLS, SIM, AKA or FAST.
+    clientCertificateLength
+                   - Length in bytes of the following client certificate (if
+                     present). Only required for TLS.
+    clientCertificate
+                   - The actual client certificate data (if present). Only
+                     required for TLS. DER/PEM format supported.
+    certificateAuthorityCertificateLength
+                   - Length in bytes of the following certificate authority
+                     certificate (if present). Optional for TLS, TTLS, PEAP.
+    certificateAuthorityCertificate
+                   - The actual certificate authority certificate data (if
+                     present). If not supplied the received certificate
+                     authority certificate is assumed to be valid, if present
+                     the received certificate is validated against it. Optional
+                     for TLS, TTLS, PEAP. DER/PEM format supported.
+    privateKeyLength
+                   - Length in bytes of the following private key (if present).
+                     Only required for TLS.
+    privateKey
+                   - The actual private key (if present). Only required for TLS.
+                     DER/PEM format, maybe password protected.
+    privateKeyPassword
+                   - Optional password to protect the private key.
+    sessionLength
+                   - Length in bytes of the following session field Supported
+                     for all EAP methods except: SIM or AKA.
+    session
+                   - Session information to support faster re-authentication.
+                     Supported for all EAP methods except: SIM or AKA.
+    allowPacProvisioning
+                   - If TRUE: PAC provisioning is allowed 'over-the_air';
+                     If FALSE: a PAC must be supplied.
+                     Only required for FAST.
+    pacLength
+                   - Length the following PAC field. If allowPacProvisioning is
+                     FALSE then the PAC MUST be supplied (i.e. non-zero). Only
+                     required for FAST.
+    pac
+                   - The actual PAC data. If allowPacProvisioning is FALSE then
+                     the PAC MUST be supplied. Only required for FAST.
+    pacPassword
+                   - Optional password to protect the PAC. Only required for
+                     FAST.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiNmeEapMethodMask  eapMethodMask;
+    CsrWifiSmeAuthModeMask   authMode;
+    CsrWifiNmeEncryptionMask encryptionMode;
+    char           *userName;
+    char           *userPassword;
+    char           *authServerUserIdentity;
+    u32                clientCertificateLength;
+    u8                *clientCertificate;
+    u32                certificateAuthorityCertificateLength;
+    u8                *certificateAuthorityCertificate;
+    u16                privateKeyLength;
+    u8                *privateKey;
+    char           *privateKeyPassword;
+    u32                sessionLength;
+    u8                *session;
+    u8                  allowPacProvisioning;
+    u32                pacLength;
+    u8                *pac;
+    char           *pacPassword;
+} CsrWifiNmeEapCredentials;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmePeerConfig
+
+  DESCRIPTION
+    Structure holding Peer Config data.
+
+  MEMBERS
+    p2pDeviceId         -
+    groupCapabilityMask -
+    groupOwnerIntent    -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress                p2pDeviceId;
+    CsrWifiSmeP2pGroupCapabilityMask groupCapabilityMask;
+    u8                         groupOwnerIntent;
+} CsrWifiNmePeerConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileIdentity
+
+  DESCRIPTION
+    The identity of a profile is defined as the unique combination the BSSID
+    and SSID.
+
+  MEMBERS
+    bssid  - ID of Basic Service Set for or the P2pDevice address of the GO for
+             which a connection attempt was made.
+    ssid   - Service Set Id.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress bssid;
+    CsrWifiSsid       ssid;
+} CsrWifiNmeProfileIdentity;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWep128Keys
+
+  DESCRIPTION
+    Structure holding WEP Authentication Type and WEP keys that can be used
+    when using WEP128.
+
+  MEMBERS
+    wepAuthType    - Mask to select the WEP authentication type (Open or Shared)
+    selectedWepKey - Index to one of the four keys below indicating the
+                     currently used WEP key.
+    key1           - Value for key number 1.
+    key2           - Value for key number 2.
+    key3           - Value for key number 3.
+    key4           - Value for key number 4.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeAuthModeMask wepAuthType;
+    u8               selectedWepKey;
+    u8               key1[13];
+    u8               key2[13];
+    u8               key3[13];
+    u8               key4[13];
+} CsrWifiNmeWep128Keys;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWep64Keys
+
+  DESCRIPTION
+    Structure for holding WEP Authentication Type and WEP keys that can be
+    used when using WEP64.
+
+  MEMBERS
+    wepAuthType    - Mask to select the WEP authentication type (Open or Shared)
+    selectedWepKey - Index to one of the four keys below indicating the
+                     currently used WEP key.
+    key1           - Value for key number 1.
+    key2           - Value for key number 2.
+    key3           - Value for key number 3.
+    key4           - Value for key number 4.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeAuthModeMask wepAuthType;
+    u8               selectedWepKey;
+    u8               key1[5];
+    u8               key2[5];
+    u8               key3[5];
+    u8               key4[5];
+} CsrWifiNmeWep64Keys;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeCredentials
+
+  DESCRIPTION
+    Structure containing the Credentials data.
+
+  MEMBERS
+    credentialType            - Credential type value (as defined in the
+                                enumeration type).
+    credential                - Union containing credentials which depends on
+                                credentialType parameter.
+    credentialeap             -
+    credentialwapiPassphrase  -
+    credentialwpa2Passphrase  -
+    credentialwpa2Psk         -
+    credentialwapiPsk         -
+    credentialwpaPassphrase   -
+    credentialwapi            -
+    credentialwep128Key       -
+    credentialwpaPsk          -
+    credentialopenSystem      -
+    credentialwep64Key        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiNmeCredentialType credentialType;
+    union {
+        CsrWifiNmeEapCredentials  eap;
+        CsrWifiNmePassphrase      wapiPassphrase;
+        CsrWifiNmePassphrase      wpa2Passphrase;
+        CsrWifiNmePsk             wpa2Psk;
+        CsrWifiNmePsk             wapiPsk;
+        CsrWifiNmePassphrase      wpaPassphrase;
+        CsrWifiNmeWapiCredentials wapi;
+        CsrWifiNmeWep128Keys      wep128Key;
+        CsrWifiNmePsk             wpaPsk;
+        CsrWifiNmeEmpty           openSystem;
+        CsrWifiNmeWep64Keys       wep64Key;
+    } credential;
+} CsrWifiNmeCredentials;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfile
+
+  DESCRIPTION
+    Structure containing the Profile data.
+
+  MEMBERS
+    profileIdentity - Profile Identity.
+    wmmQosInfoMask  - Mask for WMM QoS information.
+    bssType         - Type of BSS (Infrastructure or Adhoc).
+    channelNo       - Channel Number.
+    ccxOptionsMask  - Options mask for Cisco Compatible Extentions.
+    cloakedSsid     - Flag to decide whether the SSID is cloaked (not
+                      transmitted) or not.
+    credentials     - Credentials data.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiNmeProfileIdentity profileIdentity;
+    CsrWifiNmeWmmQosInfoMask  wmmQosInfoMask;
+    CsrWifiNmeBssType         bssType;
+    u8                  channelNo;
+    u8                  ccxOptionsMask;
+    u8                   cloakedSsid;
+    CsrWifiNmeCredentials     credentials;
+} CsrWifiNmeProfile;
+
+
+/* Downstream */
+#define CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST            (0x0000)
+
+#define CSR_WIFI_NME_PROFILE_SET_REQ                      ((CsrWifiNmePrim) (0x0000 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_DELETE_REQ                   ((CsrWifiNmePrim) (0x0001 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_DELETE_ALL_REQ               ((CsrWifiNmePrim) (0x0002 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_ORDER_SET_REQ                ((CsrWifiNmePrim) (0x0003 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_CONNECT_REQ                  ((CsrWifiNmePrim) (0x0004 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_WPS_REQ                              ((CsrWifiNmePrim) (0x0005 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_WPS_CANCEL_REQ                       ((CsrWifiNmePrim) (0x0006 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_CONNECTION_STATUS_GET_REQ            ((CsrWifiNmePrim) (0x0007 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_SIM_IMSI_GET_RES                     ((CsrWifiNmePrim) (0x0008 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_SIM_GSM_AUTH_RES                     ((CsrWifiNmePrim) (0x0009 + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_SIM_UMTS_AUTH_RES                    ((CsrWifiNmePrim) (0x000A + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_WPS_CONFIG_SET_REQ                   ((CsrWifiNmePrim) (0x000B + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_NME_EVENT_MASK_SET_REQ                   ((CsrWifiNmePrim) (0x000C + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST))
+
+
+#define CSR_WIFI_NME_PRIM_DOWNSTREAM_HIGHEST           (0x000C + CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST)
+
+/* Upstream */
+#define CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST              (0x0000 + CSR_PRIM_UPSTREAM)
+
+#define CSR_WIFI_NME_PROFILE_SET_CFM                      ((CsrWifiNmePrim)(0x0000 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_DELETE_CFM                   ((CsrWifiNmePrim)(0x0001 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_DELETE_ALL_CFM               ((CsrWifiNmePrim)(0x0002 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_ORDER_SET_CFM                ((CsrWifiNmePrim)(0x0003 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_CONNECT_CFM                  ((CsrWifiNmePrim)(0x0004 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_WPS_CFM                              ((CsrWifiNmePrim)(0x0005 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_WPS_CANCEL_CFM                       ((CsrWifiNmePrim)(0x0006 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_CONNECTION_STATUS_GET_CFM            ((CsrWifiNmePrim)(0x0007 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_UPDATE_IND                   ((CsrWifiNmePrim)(0x0008 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_PROFILE_DISCONNECT_IND               ((CsrWifiNmePrim)(0x0009 + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_SIM_IMSI_GET_IND                     ((CsrWifiNmePrim)(0x000A + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_SIM_GSM_AUTH_IND                     ((CsrWifiNmePrim)(0x000B + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_SIM_UMTS_AUTH_IND                    ((CsrWifiNmePrim)(0x000C + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_WPS_CONFIG_SET_CFM                   ((CsrWifiNmePrim)(0x000D + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_NME_EVENT_MASK_SET_CFM                   ((CsrWifiNmePrim)(0x000E + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST))
+
+#define CSR_WIFI_NME_PRIM_UPSTREAM_HIGHEST             (0x000E + CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST)
+
+#define CSR_WIFI_NME_PRIM_DOWNSTREAM_COUNT             (CSR_WIFI_NME_PRIM_DOWNSTREAM_HIGHEST + 1 - CSR_WIFI_NME_PRIM_DOWNSTREAM_LOWEST)
+#define CSR_WIFI_NME_PRIM_UPSTREAM_COUNT               (CSR_WIFI_NME_PRIM_UPSTREAM_HIGHEST   + 1 - CSR_WIFI_NME_PRIM_UPSTREAM_LOWEST)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileSetReq
+
+  DESCRIPTION
+    Creates or updates an existing profile in the NME that matches the unique
+    identity of the profile. Each profile is identified by the combination of
+    BSSID and SSID. The profile contains all the required credentials for
+    attempting to connect to the network. Creating or updating a profile via
+    the NME PROFILE SET REQ does NOT add the profile to the preferred profile
+    list within the NME used for the NME auto-connect behaviour.
+
+  MEMBERS
+    common  - Common header for use with the CsrWifiFsm Module
+    profile - Specifies the identity and credentials of the network.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    CsrWifiNmeProfile profile;
+} CsrWifiNmeProfileSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteReq
+
+  DESCRIPTION
+    Will delete the profile with a matching identity, but does NOT modify the
+    preferred profile list.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    profileIdentity - Identity (BSSID, SSID) of profile to be deleted.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    CsrWifiNmeProfileIdentity profileIdentity;
+} CsrWifiNmeProfileDeleteReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteAllReq
+
+  DESCRIPTION
+    Deletes all profiles present in the NME, but does NOT modify the
+    preferred profile list.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiNmeProfileDeleteAllReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileOrderSetReq
+
+  DESCRIPTION
+    Defines the preferred order that profiles present in the NME should be
+    used during the NME auto-connect behaviour.
+    If profileIdentitysCount == 0, it removes any existing preferred profile
+    list already present in the NME, effectively disabling the auto-connect
+    behaviour.
+    NOTE: Profile identities that do not match any profile stored in the NME
+    are ignored during the auto-connect procedure.
+    NOTE: during auto-connect the NME will only attempt to join an existing
+    adhoc network and it will never attempt to host an adhoc network; for
+    hosting and adhoc network, use CSR_WIFI_NME_PROFILE_CONNECT_REQ
+
+  MEMBERS
+    common                - Common header for use with the CsrWifiFsm Module
+    interfaceTag          - Interface Identifier; unique identifier of an
+                            interface
+    profileIdentitysCount - The number of profiles identities in the list.
+    profileIdentitys      - Points to the list of profile identities.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent            common;
+    u16                  interfaceTag;
+    u8                   profileIdentitysCount;
+    CsrWifiNmeProfileIdentity *profileIdentitys;
+} CsrWifiNmeProfileOrderSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileConnectReq
+
+  DESCRIPTION
+    Requests the NME to attempt to connect to the specified profile.
+    Overrides any current connection attempt.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    profileIdentity - Identity (BSSID, SSID) of profile to be connected to.
+                      It must match an existing profile in the NME.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrWifiNmeProfileIdentity profileIdentity;
+} CsrWifiNmeProfileConnectReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsReq
+
+  DESCRIPTION
+    Requests the NME to look for WPS enabled APs and attempt to perform WPS
+    to determine the appropriate security credentials to connect to the AP.
+    If the PIN == '00000000' then 'push button mode' is indicated, otherwise
+    the PIN has to match that of the AP. 4 digit pin is passed by sending the
+    pin digits in pin[0]..pin[3] and rest of the contents filled with '-'.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    pin          - PIN value.
+    ssid         - Service Set identifier
+    bssid        - ID of Basic Service Set for which a WPS connection attempt is
+                   being made.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    u8          pin[8];
+    CsrWifiSsid       ssid;
+    CsrWifiMacAddress bssid;
+} CsrWifiNmeWpsReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsCancelReq
+
+  DESCRIPTION
+    Requests the NME to cancel any WPS procedure that it is currently
+    performing. This includes WPS registrar activities started because of
+    CSR_WIFI_NME_AP_REGISTER.request
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiNmeWpsCancelReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConnectionStatusGetReq
+
+  DESCRIPTION
+    Requests the current connection status of the NME.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiNmeConnectionStatusGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimImsiGetRes
+
+  DESCRIPTION
+    Response from the application that received the NME SIM IMSI GET IND.
+
+  MEMBERS
+    common   - Common header for use with the CsrWifiFsm Module
+    status   - Indicates the outcome of the requested operation: STATUS_SUCCESS
+               or STATUS_ERROR.
+    imsi     - The value of the IMSI obtained from the UICC.
+    cardType - The UICC type (GSM only (SIM), UMTS only (USIM), Both).
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrResult             status;
+    char        *imsi;
+    CsrWifiNmeSimCardType cardType;
+} CsrWifiNmeSimImsiGetRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimGsmAuthRes
+
+  DESCRIPTION
+    Response from the application that received the NME SIM GSM AUTH IND. For
+    each GSM authentication round a GSM Ciphering key (Kc) and a signed
+    response (SRES) are produced. Since 2 or 3 GSM authentication rounds are
+    used the 2 or 3 Kc's obtained respectively are combined into one buffer
+    and similarly the 2 or 3 SRES's obtained are combined into another
+    buffer. The order of Kc values (SRES values respectively) in their buffer
+    is the same as that of their corresponding RAND values in the incoming
+    indication.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    status     - Indicates the outcome of the requested operation:
+                 STATUS_SUCCESS or STATUS_ERROR
+    kcsLength  - Length in Bytes of Kc buffer. Legal values are: 16 or 24.
+    kcs        - Kc buffer holding 2 or 3 Kc values.
+    sresLength - Length in Bytes of SRES buffer. Legal values are: 8 or 12.
+    sres       - SRES buffer holding 2 or 3 SRES values.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+    u8        kcsLength;
+    u8       *kcs;
+    u8        sresLength;
+    u8       *sres;
+} CsrWifiNmeSimGsmAuthRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimUmtsAuthRes
+
+  DESCRIPTION
+    Response from the application that received the NME SIM UMTS AUTH IND.
+    The values of umtsCipherKey, umtsIntegrityKey, resParameterLength and
+    resParameter are only meanigful when result = UMTS_AUTH_RESULT_SUCCESS.
+    The value of auts is only meaningful when
+    result=UMTS_AUTH_RESULT_SYNC_FAIL.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    status             - Indicates the outcome of the requested operation:
+                         STATUS_SUCCESS or STATUS_ERROR.
+    result             - The result of UMTS authentication as performed by the
+                         UICC which could be: Success, Authentication Reject or
+                         Synchronisation Failure. For all these 3 outcomes the
+                         value of status is success.
+    umtsCipherKey      - The UMTS Cipher Key as calculated and returned by the
+                         UICC.
+    umtsIntegrityKey   - The UMTS Integrity Key as calculated and returned by
+                         the UICC.
+    resParameterLength - The length (in bytes) of the RES parameter (min=4; max
+                         = 16).
+    resParameter       - The RES parameter as calculated and returned by the
+                         UICC.
+    auts               - The AUTS parameter as calculated and returned by the
+                         UICC.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent          common;
+    CsrResult                status;
+    CsrWifiNmeUmtsAuthResult result;
+    u8                 umtsCipherKey[16];
+    u8                 umtsIntegrityKey[16];
+    u8                 resParameterLength;
+    u8                *resParameter;
+    u8                 auts[14];
+} CsrWifiNmeSimUmtsAuthRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsConfigSetReq
+
+  DESCRIPTION
+    This primitive passes the WPS information for the device to NME. This may
+    be accepted only if no interface is active.
+
+  MEMBERS
+    common    - Common header for use with the CsrWifiFsm Module
+    wpsConfig - WPS config.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    CsrWifiSmeWpsConfig wpsConfig;
+} CsrWifiNmeWpsConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEventMaskSetReq
+
+  DESCRIPTION
+    The wireless manager application may register with the NME to receive
+    notification of interesting events. Indications will be sent only if the
+    wireless manager explicitly registers to be notified of that event.
+    indMask is a bit mask of values defined in CsrWifiNmeIndicationsMask.
+
+  MEMBERS
+    common  - Common header for use with the CsrWifiFsm Module
+    indMask - Set mask with values from CsrWifiNmeIndications
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    CsrWifiNmeIndicationsMask indMask;
+} CsrWifiNmeEventMaskSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileSetCfm
+
+  DESCRIPTION
+    Reports the status of the NME PROFILE SET REQ; the request will only fail
+    if the details specified in the profile contains an invalid combination
+    of parameters for example specifying the profile as cloaked but not
+    specifying the SSID. The NME doesn't limit the number of profiles that
+    may be created. The NME assumes that the entity configuring it is aware
+    of the appropriate limits.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Indicates the success or otherwise of the requested operation.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeProfileSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteCfm
+
+  DESCRIPTION
+    Reports the status of the CSR_WIFI_NME_PROFILE_DELETE_REQ.
+    Returns CSR_WIFI_NME_STATUS_NOT_FOUND if there is no matching profile.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Indicates the success or otherwise of the requested operation.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeProfileDeleteCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDeleteAllCfm
+
+  DESCRIPTION
+    Reports the status of the CSR_WIFI_NME_PROFILE_DELETE_ALL_REQ.
+    Returns always CSR_WIFI_NME_STATUS_SUCCESS.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Indicates the success or otherwise of the requested operation, but
+             in this case it always set to success.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeProfileDeleteAllCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileOrderSetCfm
+
+  DESCRIPTION
+    Confirmation to UNIFI_NME_PROFILE_ORDER_SET.request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Indicates the success or otherwise of the requested
+                   operation.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiNmeProfileOrderSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileConnectCfm
+
+  DESCRIPTION
+    Reports the status of the NME PROFILE CONNECT REQ. If unsuccessful the
+    connectAttempt parameters contain details of the APs that the NME
+    attempted to connect to before reporting the failure of the request.
+
+  MEMBERS
+    common               - Common header for use with the CsrWifiFsm Module
+    interfaceTag         - Interface Identifier; unique identifier of an
+                           interface
+    status               - Indicates the success or otherwise of the requested
+                           operation.
+    connectAttemptsCount - This parameter is relevant only if
+                           status!=CSR_WIFI_NME_STATUS_SUCCESS.
+                           Number of connection attempt elements provided with
+                           this primitive
+    connectAttempts      - This parameter is relevant only if
+                           status!=CSR_WIFI_NME_STATUS_SUCCESS.
+                           Points to the list of connection attempt elements
+                           provided with this primitive
+                           Each element of the list provides information about
+                           an AP on which the connection attempt was made and
+                           the error that occurred during the attempt.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrResult                 status;
+    u8                  connectAttemptsCount;
+    CsrWifiNmeConnectAttempt *connectAttempts;
+} CsrWifiNmeProfileConnectCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsCfm
+
+  DESCRIPTION
+    Reports the status of the NME WPS REQ.
+    If CSR_WIFI_NME_STATUS_SUCCESS, the profile parameter contains the
+    identity and credentials of the AP.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Indicates the success or otherwise of the requested
+                   operation.
+    profile      - This parameter is relevant only if
+                   status==CSR_WIFI_NME_STATUS_SUCCESS.
+                   The identity and credentials of the network.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    CsrResult         status;
+    CsrWifiNmeProfile profile;
+} CsrWifiNmeWpsCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsCancelCfm
+
+  DESCRIPTION
+    Reports the status of the NME WPS REQ, the request is always SUCCESSFUL.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Only returns CSR_WIFI_NME_STATUS_SUCCESS
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiNmeWpsCancelCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeConnectionStatusGetCfm
+
+  DESCRIPTION
+    Reports the connection status of the NME.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    status           - Indicates the success or otherwise of the requested
+                       operation.
+    connectionStatus - NME current connection status
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent            common;
+    u16                  interfaceTag;
+    CsrResult                  status;
+    CsrWifiNmeConnectionStatus connectionStatus;
+} CsrWifiNmeConnectionStatusGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileUpdateInd
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that informs that application that the contained profile has
+    changed.
+    For example, either the credentials EAP-FAST PAC file or the session data
+    within the profile has changed.
+    It is up to the application whether it stores this updated profile or
+    not.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    profile      - The identity and credentials of the network.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    CsrWifiNmeProfile profile;
+} CsrWifiNmeProfileUpdateInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeProfileDisconnectInd
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that informs that application that the current profile
+    connection has disconnected. The indication will contain information
+    about APs that it attempted to maintain the connection via i.e. in the
+    case of failed roaming.
+
+  MEMBERS
+    common               - Common header for use with the CsrWifiFsm Module
+    interfaceTag         - Interface Identifier; unique identifier of an
+                           interface
+    connectAttemptsCount - Number of connection attempt elements provided with
+                           this primitive
+    connectAttempts      - Points to the list of connection attempt elements
+                           provided with this primitive
+                           Each element of the list provides information about
+                           an AP on which the connection attempt was made and
+                           the error occurred during the attempt.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    u8                  connectAttemptsCount;
+    CsrWifiNmeConnectAttempt *connectAttempts;
+} CsrWifiNmeProfileDisconnectInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimImsiGetInd
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that requests the IMSI and UICC type from the UICC Manager.
+    This indication is generated when the NME is attempting to connect to a
+    profile configured for EAP-SIM/AKA. An application MUST register to
+    receive this indication for the NME to support the EAP-SIM/AKA credential
+    types. Otherwise the NME has no route to obtain the information from the
+    UICC.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiNmeSimImsiGetInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimGsmAuthInd
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that requests the UICC Manager to perform a GSM
+    authentication on behalf of the NME. This indication is generated when
+    the NME is attempting to connect to a profile configured for EAP-SIM. An
+    application MUST register to receive this indication for the NME to
+    support the EAP-SIM credential types. Otherwise the NME has no route to
+    obtain the information from the UICC. EAP-SIM authentication requires 2
+    or 3 GSM authentication rounds and therefore 2 or 3 RANDS (GSM Random
+    Challenges) are included.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    randsLength - GSM RAND is 16 bytes long hence valid values are 32 (2 RANDS)
+                  or 48 (3 RANDs).
+    rands       - 2 or 3 RANDs values.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u8        randsLength;
+    u8       *rands;
+} CsrWifiNmeSimGsmAuthInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeSimUmtsAuthInd
+
+  DESCRIPTION
+    Indication generated from the NME (if an application subscribes to
+    receive it) that requests the UICC Manager to perform a UMTS
+    authentication on behalf of the NME. This indication is generated when
+    the NME is attempting to connect to a profile configured for EAP-AKA. An
+    application MUST register to receive this indication for the NME to
+    support the EAP-AKA credential types. Otherwise the NME has no route to
+    obtain the information from the USIM. EAP-AKA requires one UMTS
+    authentication round and therefore only one RAND and one AUTN values are
+    included.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    rand   - UMTS RAND value.
+    autn   - UMTS AUTN value.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u8        rand[16];
+    u8        autn[16];
+} CsrWifiNmeSimUmtsAuthInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeWpsConfigSetCfm
+
+  DESCRIPTION
+    Confirm.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeWpsConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiNmeEventMaskSetCfm
+
+  DESCRIPTION
+    The NME calls the primitive to report the result of the request
+    primitive.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiNmeEventMaskSetCfm;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_PRIM_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_nme_serialize.h b/drivers/staging/csr/csr_wifi_nme_serialize.h
new file mode 100644
index 00000000000..c6b163660a3
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_serialize.h
@@ -0,0 +1,174 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_SERIALIZE_H__
+#define CSR_WIFI_NME_SERIALIZE_H__
+
+#include "csr_wifi_msgconv.h"
+#include "csr_wifi_nme_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_serialize.h
+#endif
+
+extern void CsrWifiNmePfree(void *ptr);
+
+extern u8* CsrWifiNmeProfileSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileSetReqSizeof(void *msg);
+extern void CsrWifiNmeProfileSetReqSerFree(void *msg);
+
+extern u8* CsrWifiNmeProfileDeleteReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileDeleteReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileDeleteReqSizeof(void *msg);
+#define CsrWifiNmeProfileDeleteReqSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeProfileDeleteAllReqSer CsrWifiEventSer
+#define CsrWifiNmeProfileDeleteAllReqDes CsrWifiEventDes
+#define CsrWifiNmeProfileDeleteAllReqSizeof CsrWifiEventSizeof
+#define CsrWifiNmeProfileDeleteAllReqSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeProfileOrderSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileOrderSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileOrderSetReqSizeof(void *msg);
+extern void CsrWifiNmeProfileOrderSetReqSerFree(void *msg);
+
+extern u8* CsrWifiNmeProfileConnectReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileConnectReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileConnectReqSizeof(void *msg);
+#define CsrWifiNmeProfileConnectReqSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeWpsReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeWpsReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeWpsReqSizeof(void *msg);
+#define CsrWifiNmeWpsReqSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeWpsCancelReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeWpsCancelReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeWpsCancelReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeWpsCancelReqSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeConnectionStatusGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeConnectionStatusGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeConnectionStatusGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeConnectionStatusGetReqSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeSimImsiGetResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeSimImsiGetResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeSimImsiGetResSizeof(void *msg);
+extern void CsrWifiNmeSimImsiGetResSerFree(void *msg);
+
+extern u8* CsrWifiNmeSimGsmAuthResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeSimGsmAuthResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeSimGsmAuthResSizeof(void *msg);
+extern void CsrWifiNmeSimGsmAuthResSerFree(void *msg);
+
+extern u8* CsrWifiNmeSimUmtsAuthResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeSimUmtsAuthResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeSimUmtsAuthResSizeof(void *msg);
+extern void CsrWifiNmeSimUmtsAuthResSerFree(void *msg);
+
+extern u8* CsrWifiNmeWpsConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeWpsConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeWpsConfigSetReqSizeof(void *msg);
+extern void CsrWifiNmeWpsConfigSetReqSerFree(void *msg);
+
+#define CsrWifiNmeEventMaskSetReqSer CsrWifiEventCsrUint32Ser
+#define CsrWifiNmeEventMaskSetReqDes CsrWifiEventCsrUint32Des
+#define CsrWifiNmeEventMaskSetReqSizeof CsrWifiEventCsrUint32Sizeof
+#define CsrWifiNmeEventMaskSetReqSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeProfileSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeProfileSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeProfileSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeProfileSetCfmSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeProfileDeleteCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeProfileDeleteCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeProfileDeleteCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeProfileDeleteCfmSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeProfileDeleteAllCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeProfileDeleteAllCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeProfileDeleteAllCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeProfileDeleteAllCfmSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeProfileOrderSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileOrderSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileOrderSetCfmSizeof(void *msg);
+#define CsrWifiNmeProfileOrderSetCfmSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeProfileConnectCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileConnectCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileConnectCfmSizeof(void *msg);
+extern void CsrWifiNmeProfileConnectCfmSerFree(void *msg);
+
+extern u8* CsrWifiNmeWpsCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeWpsCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeWpsCfmSizeof(void *msg);
+extern void CsrWifiNmeWpsCfmSerFree(void *msg);
+
+extern u8* CsrWifiNmeWpsCancelCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeWpsCancelCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeWpsCancelCfmSizeof(void *msg);
+#define CsrWifiNmeWpsCancelCfmSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeConnectionStatusGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeConnectionStatusGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeConnectionStatusGetCfmSizeof(void *msg);
+#define CsrWifiNmeConnectionStatusGetCfmSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeProfileUpdateIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileUpdateIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileUpdateIndSizeof(void *msg);
+extern void CsrWifiNmeProfileUpdateIndSerFree(void *msg);
+
+extern u8* CsrWifiNmeProfileDisconnectIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeProfileDisconnectIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeProfileDisconnectIndSizeof(void *msg);
+extern void CsrWifiNmeProfileDisconnectIndSerFree(void *msg);
+
+#define CsrWifiNmeSimImsiGetIndSer CsrWifiEventSer
+#define CsrWifiNmeSimImsiGetIndDes CsrWifiEventDes
+#define CsrWifiNmeSimImsiGetIndSizeof CsrWifiEventSizeof
+#define CsrWifiNmeSimImsiGetIndSerFree CsrWifiNmePfree
+
+extern u8* CsrWifiNmeSimGsmAuthIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeSimGsmAuthIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeSimGsmAuthIndSizeof(void *msg);
+extern void CsrWifiNmeSimGsmAuthIndSerFree(void *msg);
+
+extern u8* CsrWifiNmeSimUmtsAuthIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiNmeSimUmtsAuthIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiNmeSimUmtsAuthIndSizeof(void *msg);
+#define CsrWifiNmeSimUmtsAuthIndSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeWpsConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeWpsConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeWpsConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeWpsConfigSetCfmSerFree CsrWifiNmePfree
+
+#define CsrWifiNmeEventMaskSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiNmeEventMaskSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiNmeEventMaskSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiNmeEventMaskSetCfmSerFree CsrWifiNmePfree
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* CSR_WIFI_NME_SERIALIZE_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_nme_task.h b/drivers/staging/csr/csr_wifi_nme_task.h
new file mode 100644
index 00000000000..76f44dbe26a
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_nme_task.h
@@ -0,0 +1,38 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_NME_TASK_H__
+#define CSR_WIFI_NME_TASK_H__
+
+#include <linux/types.h>
+#include "csr_sched.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_NME_ENABLE
+#error CSR_WIFI_NME_ENABLE MUST be defined inorder to use csr_wifi_nme_task.h
+#endif
+
+#define CSR_WIFI_NME_LOG_ID 0x1203FFFF
+extern CsrSchedQid CSR_WIFI_NME_IFACEQUEUE;
+void CsrWifiNmeInit(void **gash);
+void CsrWifiNmeDeinit(void **gash);
+void CsrWifiNmeHandler(void **gash);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_NME_TASK_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_private_common.h b/drivers/staging/csr/csr_wifi_private_common.h
new file mode 100644
index 00000000000..47309984e2a
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_private_common.h
@@ -0,0 +1,89 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_PRIVATE_COMMON_H__
+#define CSR_WIFI_PRIVATE_COMMON_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief maximum number of STAs allowed to be connected
+ *
+ * @par Description
+ *   min & max Beacon Interval
+ */
+#define CSR_WIFI_AP_MAX_ASSOC_STA   8
+
+/** Number of only b rates */
+#define CSR_WIFI_SME_AP_MAX_ONLY_B_RATES        4
+
+
+/** Number of mandatory b rates */
+#define CSR_WIFI_SME_AP_MAX_MANDATORY_B_RATES   2
+
+
+/** Number of mandatory bg rates */
+#define CSR_WIFI_SME_AP_MAX_MANDATORY_BG_RATES  4
+
+
+/** Number of bg rates */
+#define CSR_WIFI_SME_AP_MAX_BG_RATES            12
+
+
+/** Number of no b only g rates */
+#define CSR_WIFI_SME_AP_MAX_NO_B_ONLY_G_RATES   8
+
+
+/** Number of mandatory g rates */
+#define CSR_WIFI_SME_AP_MAX_MANDATORY_G_RATES   7
+
+
+/* Number of g mandatory rates */
+#define CSR_WIFI_SME_AP_G_MANDATORY_RATES_NUM   7
+
+
+/* Number of b mandatory rates */
+#define CSR_WIFI_SME_AP_B_MANDATORY_RATES_NUM   2
+
+
+/* Number of b/g mandatory rates */
+#define CSR_WIFI_SME_AP_BG_MANDATORY_RATES_NUM   4
+
+
+/* The maximum allowed length of SSID */
+#define CSR_WIFI_SME_AP_SSID_MAX_LENGTH         32
+
+/* Refer 8.4.2.27 RSN element - we support TKIP, WPA2, WAPI and PSK only, no pmkid, group cipher suite */
+#define CSR_WIFI_SME_RSN_PACKED_SIZE (1 + 1 + 2 + 4 + 2 + 4 * 2 + 2 + 4 * 1 + 2 + 24)
+
+/* Refer 7.3.2.9 (ISO/IEC 8802-11:2006) WAPI element - we support WAPI PSK only, no bkid, group cipher suite */
+#define CSR_WIFI_SME_WAPI_PACKED_SIZE (1 + 1 + 2 + 2 + 4 * 1 + 2 + 4 * 1 + 4 + 2 + 24)
+
+
+/* Common structure for NME and SME to maintain Interface mode*/
+typedef u8 CsrWifiInterfaceMode;
+#define  CSR_WIFI_MODE_NONE                             ((CsrWifiInterfaceMode) 0xFF)
+#define  CSR_WIFI_MODE_STA                              ((CsrWifiInterfaceMode) 0x00)
+#define  CSR_WIFI_MODE_AP                               ((CsrWifiInterfaceMode) 0x01)
+#define  CSR_WIFI_MODE_P2P_DEVICE                       ((CsrWifiInterfaceMode) 0x02)
+#define  CSR_WIFI_MODE_P2P_CLI                          ((CsrWifiInterfaceMode) 0x03)
+#define  CSR_WIFI_MODE_P2P_GO                           ((CsrWifiInterfaceMode) 0x04)
+#define  CSR_WIFI_MODE_AMP                              ((CsrWifiInterfaceMode) 0x05)
+#define  CSR_WIFI_MODE_WPS_ENROLLEE                     ((CsrWifiInterfaceMode) 0x06)
+#define  CSR_WIFI_MODE_IBSS                             ((CsrWifiInterfaceMode) 0x07)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/drivers/staging/csr/csr_wifi_result.h b/drivers/staging/csr/csr_wifi_result.h
new file mode 100644
index 00000000000..2f87cda9003
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_result.h
@@ -0,0 +1,35 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_RESULT_H__
+#define CSR_WIFI_RESULT_H__
+
+#include "csr_result.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* THIS FILE SHOULD CONTAIN ONLY RESULT CODES */
+
+/* Result Codes */
+#define CSR_WIFI_HIP_RESULT_INVALID_VALUE    ((CsrResult) 1) /* Invalid argument value */
+#define CSR_WIFI_HIP_RESULT_NO_DEVICE        ((CsrResult) 2) /* The specified device is no longer present */
+#define CSR_WIFI_HIP_RESULT_NO_SPACE         ((CsrResult) 3) /* A queue or buffer is full */
+#define CSR_WIFI_HIP_RESULT_NO_MEMORY        ((CsrResult) 4) /* Fatal error, no memory */
+#define CSR_WIFI_HIP_RESULT_RANGE            ((CsrResult) 5) /* Request exceeds the range of a file or a buffer */
+#define CSR_WIFI_HIP_RESULT_NOT_FOUND        ((CsrResult) 6) /* A file (typically a f/w patch) is not found */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_RESULT_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_router_converter_init.c b/drivers/staging/csr/csr_wifi_router_converter_init.c
new file mode 100644
index 00000000000..775c013d051
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_converter_init.c
@@ -0,0 +1,82 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#include "csr_msgconv.h"
+#include "csr_macro.h"
+
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_ROUTER_MODULE
+#include "csr_wifi_router_serialize.h"
+#include "csr_wifi_router_prim.h"
+
+static CsrMsgConvMsgEntry csrwifirouter_conv_lut[] = {
+    { CSR_WIFI_ROUTER_MA_PACKET_SUBSCRIBE_REQ, CsrWifiRouterMaPacketSubscribeReqSizeof, CsrWifiRouterMaPacketSubscribeReqSer, CsrWifiRouterMaPacketSubscribeReqDes, CsrWifiRouterMaPacketSubscribeReqSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_UNSUBSCRIBE_REQ, CsrWifiRouterMaPacketUnsubscribeReqSizeof, CsrWifiRouterMaPacketUnsubscribeReqSer, CsrWifiRouterMaPacketUnsubscribeReqDes, CsrWifiRouterMaPacketUnsubscribeReqSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_REQ, CsrWifiRouterMaPacketReqSizeof, CsrWifiRouterMaPacketReqSer, CsrWifiRouterMaPacketReqDes, CsrWifiRouterMaPacketReqSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_RES, CsrWifiRouterMaPacketResSizeof, CsrWifiRouterMaPacketResSer, CsrWifiRouterMaPacketResDes, CsrWifiRouterMaPacketResSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_CANCEL_REQ, CsrWifiRouterMaPacketCancelReqSizeof, CsrWifiRouterMaPacketCancelReqSer, CsrWifiRouterMaPacketCancelReqDes, CsrWifiRouterMaPacketCancelReqSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_SUBSCRIBE_CFM, CsrWifiRouterMaPacketSubscribeCfmSizeof, CsrWifiRouterMaPacketSubscribeCfmSer, CsrWifiRouterMaPacketSubscribeCfmDes, CsrWifiRouterMaPacketSubscribeCfmSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_UNSUBSCRIBE_CFM, CsrWifiRouterMaPacketUnsubscribeCfmSizeof, CsrWifiRouterMaPacketUnsubscribeCfmSer, CsrWifiRouterMaPacketUnsubscribeCfmDes, CsrWifiRouterMaPacketUnsubscribeCfmSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_CFM, CsrWifiRouterMaPacketCfmSizeof, CsrWifiRouterMaPacketCfmSer, CsrWifiRouterMaPacketCfmDes, CsrWifiRouterMaPacketCfmSerFree },
+    { CSR_WIFI_ROUTER_MA_PACKET_IND, CsrWifiRouterMaPacketIndSizeof, CsrWifiRouterMaPacketIndSer, CsrWifiRouterMaPacketIndDes, CsrWifiRouterMaPacketIndSerFree },
+
+    { 0, NULL, NULL, NULL, NULL },
+};
+
+CsrMsgConvMsgEntry* CsrWifiRouterConverterLookup(CsrMsgConvMsgEntry *ce, u16 msgType)
+{
+    if (msgType & CSR_PRIM_UPSTREAM)
+    {
+        u16 idx = (msgType & ~CSR_PRIM_UPSTREAM) + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT;
+        if (idx < (CSR_WIFI_ROUTER_PRIM_UPSTREAM_COUNT + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT) &&
+            csrwifirouter_conv_lut[idx].msgType == msgType)
+        {
+            return &csrwifirouter_conv_lut[idx];
+        }
+    }
+    else
+    {
+        if (msgType < CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT &&
+            csrwifirouter_conv_lut[msgType].msgType == msgType)
+        {
+            return &csrwifirouter_conv_lut[msgType];
+        }
+    }
+    return NULL;
+}
+
+
+void CsrWifiRouterConverterInit(void)
+{
+    CsrMsgConvInsert(CSR_WIFI_ROUTER_PRIM, csrwifirouter_conv_lut);
+    CsrMsgConvCustomLookupRegister(CSR_WIFI_ROUTER_PRIM, CsrWifiRouterConverterLookup);
+}
+
+
+#ifdef CSR_LOG_ENABLE
+static const CsrLogPrimitiveInformation csrwifirouter_conv_info = {
+    CSR_WIFI_ROUTER_PRIM,
+    (char *)"CSR_WIFI_ROUTER_PRIM",
+    csrwifirouter_conv_lut
+};
+const CsrLogPrimitiveInformation* CsrWifiRouterTechInfoGet(void)
+{
+    return &csrwifirouter_conv_info;
+}
+
+
+#endif /* CSR_LOG_ENABLE */
+#endif /* EXCLUDE_CSR_WIFI_ROUTER_MODULE */
diff --git a/drivers/staging/csr/csr_wifi_router_converter_init.h b/drivers/staging/csr/csr_wifi_router_converter_init.h
new file mode 100644
index 00000000000..2a293e457ff
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_converter_init.h
@@ -0,0 +1,42 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_CONVERTER_INIT_H__
+#define CSR_WIFI_ROUTER_CONVERTER_INIT_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_ROUTER_MODULE
+
+#include "csr_msgconv.h"
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+
+extern const CsrLogPrimitiveInformation* CsrWifiRouterTechInfoGet(void);
+#endif /* CSR_LOG_ENABLE */
+
+extern void CsrWifiRouterConverterInit(void);
+
+#else /* EXCLUDE_CSR_WIFI_ROUTER_MODULE */
+
+#define CsrWifiRouterConverterInit()
+
+#endif /* EXCLUDE_CSR_WIFI_ROUTER_MODULE */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_CONVERTER_INIT_H__ */
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_converter_init.c b/drivers/staging/csr/csr_wifi_router_ctrl_converter_init.c
new file mode 100644
index 00000000000..a02e307e5a8
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_converter_init.c
@@ -0,0 +1,134 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#include "csr_msgconv.h"
+#include "csr_macro.h"
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_ROUTER_CTRL_MODULE
+#include "csr_wifi_router_ctrl_serialize.h"
+#include "csr_wifi_router_ctrl_prim.h"
+
+static CsrMsgConvMsgEntry csrwifirouterctrl_conv_lut[] = {
+    { CSR_WIFI_ROUTER_CTRL_CONFIGURE_POWER_MODE_REQ, CsrWifiRouterCtrlConfigurePowerModeReqSizeof, CsrWifiRouterCtrlConfigurePowerModeReqSer, CsrWifiRouterCtrlConfigurePowerModeReqDes, CsrWifiRouterCtrlConfigurePowerModeReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_HIP_REQ, CsrWifiRouterCtrlHipReqSizeof, CsrWifiRouterCtrlHipReqSer, CsrWifiRouterCtrlHipReqDes, CsrWifiRouterCtrlHipReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_REQ, CsrWifiRouterCtrlMediaStatusReqSizeof, CsrWifiRouterCtrlMediaStatusReqSer, CsrWifiRouterCtrlMediaStatusReqDes, CsrWifiRouterCtrlMediaStatusReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_RES, CsrWifiRouterCtrlMulticastAddressResSizeof, CsrWifiRouterCtrlMulticastAddressResSer, CsrWifiRouterCtrlMulticastAddressResDes, CsrWifiRouterCtrlMulticastAddressResSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PORT_CONFIGURE_REQ, CsrWifiRouterCtrlPortConfigureReqSizeof, CsrWifiRouterCtrlPortConfigureReqSer, CsrWifiRouterCtrlPortConfigureReqDes, CsrWifiRouterCtrlPortConfigureReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_REQ, CsrWifiRouterCtrlQosControlReqSizeof, CsrWifiRouterCtrlQosControlReqSer, CsrWifiRouterCtrlQosControlReqDes, CsrWifiRouterCtrlQosControlReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_SUSPEND_RES, CsrWifiRouterCtrlSuspendResSizeof, CsrWifiRouterCtrlSuspendResSer, CsrWifiRouterCtrlSuspendResDes, CsrWifiRouterCtrlSuspendResSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_REQ, CsrWifiRouterCtrlTclasAddReqSizeof, CsrWifiRouterCtrlTclasAddReqSer, CsrWifiRouterCtrlTclasAddReqDes, CsrWifiRouterCtrlTclasAddReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_RESUME_RES, CsrWifiRouterCtrlResumeResSizeof, CsrWifiRouterCtrlResumeResSer, CsrWifiRouterCtrlResumeResDes, CsrWifiRouterCtrlResumeResSerFree },
+    { CSR_WIFI_ROUTER_CTRL_RAW_SDIO_DEINITIALISE_REQ, CsrWifiRouterCtrlRawSdioDeinitialiseReqSizeof, CsrWifiRouterCtrlRawSdioDeinitialiseReqSer, CsrWifiRouterCtrlRawSdioDeinitialiseReqDes, CsrWifiRouterCtrlRawSdioDeinitialiseReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_RAW_SDIO_INITIALISE_REQ, CsrWifiRouterCtrlRawSdioInitialiseReqSizeof, CsrWifiRouterCtrlRawSdioInitialiseReqSer, CsrWifiRouterCtrlRawSdioInitialiseReqDes, CsrWifiRouterCtrlRawSdioInitialiseReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_REQ, CsrWifiRouterCtrlTclasDelReqSizeof, CsrWifiRouterCtrlTclasDelReqSer, CsrWifiRouterCtrlTclasDelReqDes, CsrWifiRouterCtrlTclasDelReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TRAFFIC_CLASSIFICATION_REQ, CsrWifiRouterCtrlTrafficClassificationReqSizeof, CsrWifiRouterCtrlTrafficClassificationReqSer, CsrWifiRouterCtrlTrafficClassificationReqDes, CsrWifiRouterCtrlTrafficClassificationReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_REQ, CsrWifiRouterCtrlTrafficConfigReqSizeof, CsrWifiRouterCtrlTrafficConfigReqSer, CsrWifiRouterCtrlTrafficConfigReqDes, CsrWifiRouterCtrlTrafficConfigReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_OFF_REQ, CsrWifiRouterCtrlWifiOffReqSizeof, CsrWifiRouterCtrlWifiOffReqSer, CsrWifiRouterCtrlWifiOffReqDes, CsrWifiRouterCtrlWifiOffReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_OFF_RES, CsrWifiRouterCtrlWifiOffResSizeof, CsrWifiRouterCtrlWifiOffResSer, CsrWifiRouterCtrlWifiOffResDes, CsrWifiRouterCtrlWifiOffResSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_ON_REQ, CsrWifiRouterCtrlWifiOnReqSizeof, CsrWifiRouterCtrlWifiOnReqSer, CsrWifiRouterCtrlWifiOnReqDes, CsrWifiRouterCtrlWifiOnReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_ON_RES, CsrWifiRouterCtrlWifiOnResSizeof, CsrWifiRouterCtrlWifiOnResSer, CsrWifiRouterCtrlWifiOnResDes, CsrWifiRouterCtrlWifiOnResSerFree },
+    { CSR_WIFI_ROUTER_CTRL_M4_TRANSMIT_REQ, CsrWifiRouterCtrlM4TransmitReqSizeof, CsrWifiRouterCtrlM4TransmitReqSer, CsrWifiRouterCtrlM4TransmitReqDes, CsrWifiRouterCtrlM4TransmitReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_MODE_SET_REQ, CsrWifiRouterCtrlModeSetReqSizeof, CsrWifiRouterCtrlModeSetReqSer, CsrWifiRouterCtrlModeSetReqDes, CsrWifiRouterCtrlModeSetReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PEER_ADD_REQ, CsrWifiRouterCtrlPeerAddReqSizeof, CsrWifiRouterCtrlPeerAddReqSer, CsrWifiRouterCtrlPeerAddReqDes, CsrWifiRouterCtrlPeerAddReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PEER_DEL_REQ, CsrWifiRouterCtrlPeerDelReqSizeof, CsrWifiRouterCtrlPeerDelReqSer, CsrWifiRouterCtrlPeerDelReqDes, CsrWifiRouterCtrlPeerDelReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PEER_UPDATE_REQ, CsrWifiRouterCtrlPeerUpdateReqSizeof, CsrWifiRouterCtrlPeerUpdateReqSer, CsrWifiRouterCtrlPeerUpdateReqDes, CsrWifiRouterCtrlPeerUpdateReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_CAPABILITIES_REQ, CsrWifiRouterCtrlCapabilitiesReqSizeof, CsrWifiRouterCtrlCapabilitiesReqSer, CsrWifiRouterCtrlCapabilitiesReqDes, CsrWifiRouterCtrlCapabilitiesReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ENABLE_REQ, CsrWifiRouterCtrlBlockAckEnableReqSizeof, CsrWifiRouterCtrlBlockAckEnableReqSer, CsrWifiRouterCtrlBlockAckEnableReqDes, CsrWifiRouterCtrlBlockAckEnableReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_DISABLE_REQ, CsrWifiRouterCtrlBlockAckDisableReqSizeof, CsrWifiRouterCtrlBlockAckDisableReqSer, CsrWifiRouterCtrlBlockAckDisableReqDes, CsrWifiRouterCtrlBlockAckDisableReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_RX_PKT_REQ, CsrWifiRouterCtrlWapiRxPktReqSizeof, CsrWifiRouterCtrlWapiRxPktReqSer, CsrWifiRouterCtrlWapiRxPktReqDes, CsrWifiRouterCtrlWapiRxPktReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_MULTICAST_FILTER_REQ, CsrWifiRouterCtrlWapiMulticastFilterReqSizeof, CsrWifiRouterCtrlWapiMulticastFilterReqSer, CsrWifiRouterCtrlWapiMulticastFilterReqDes, CsrWifiRouterCtrlWapiMulticastFilterReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_FILTER_REQ, CsrWifiRouterCtrlWapiUnicastFilterReqSizeof, CsrWifiRouterCtrlWapiUnicastFilterReqSer, CsrWifiRouterCtrlWapiUnicastFilterReqDes, CsrWifiRouterCtrlWapiUnicastFilterReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_PKT_REQ, CsrWifiRouterCtrlWapiUnicastTxPktReqSizeof, CsrWifiRouterCtrlWapiUnicastTxPktReqSer, CsrWifiRouterCtrlWapiUnicastTxPktReqDes, CsrWifiRouterCtrlWapiUnicastTxPktReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_FILTER_REQ, CsrWifiRouterCtrlWapiFilterReqSizeof, CsrWifiRouterCtrlWapiFilterReqSer, CsrWifiRouterCtrlWapiFilterReqDes, CsrWifiRouterCtrlWapiFilterReqSerFree },
+    { CSR_WIFI_ROUTER_CTRL_HIP_IND, CsrWifiRouterCtrlHipIndSizeof, CsrWifiRouterCtrlHipIndSer, CsrWifiRouterCtrlHipIndDes, CsrWifiRouterCtrlHipIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_IND, CsrWifiRouterCtrlMulticastAddressIndSizeof, CsrWifiRouterCtrlMulticastAddressIndSer, CsrWifiRouterCtrlMulticastAddressIndDes, CsrWifiRouterCtrlMulticastAddressIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PORT_CONFIGURE_CFM, CsrWifiRouterCtrlPortConfigureCfmSizeof, CsrWifiRouterCtrlPortConfigureCfmSer, CsrWifiRouterCtrlPortConfigureCfmDes, CsrWifiRouterCtrlPortConfigureCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_RESUME_IND, CsrWifiRouterCtrlResumeIndSizeof, CsrWifiRouterCtrlResumeIndSer, CsrWifiRouterCtrlResumeIndDes, CsrWifiRouterCtrlResumeIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_SUSPEND_IND, CsrWifiRouterCtrlSuspendIndSizeof, CsrWifiRouterCtrlSuspendIndSer, CsrWifiRouterCtrlSuspendIndDes, CsrWifiRouterCtrlSuspendIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_CFM, CsrWifiRouterCtrlTclasAddCfmSizeof, CsrWifiRouterCtrlTclasAddCfmSer, CsrWifiRouterCtrlTclasAddCfmDes, CsrWifiRouterCtrlTclasAddCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_RAW_SDIO_DEINITIALISE_CFM, CsrWifiRouterCtrlRawSdioDeinitialiseCfmSizeof, CsrWifiRouterCtrlRawSdioDeinitialiseCfmSer, CsrWifiRouterCtrlRawSdioDeinitialiseCfmDes, CsrWifiRouterCtrlRawSdioDeinitialiseCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_RAW_SDIO_INITIALISE_CFM, CsrWifiRouterCtrlRawSdioInitialiseCfmSizeof, CsrWifiRouterCtrlRawSdioInitialiseCfmSer, CsrWifiRouterCtrlRawSdioInitialiseCfmDes, CsrWifiRouterCtrlRawSdioInitialiseCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_CFM, CsrWifiRouterCtrlTclasDelCfmSizeof, CsrWifiRouterCtrlTclasDelCfmSer, CsrWifiRouterCtrlTclasDelCfmDes, CsrWifiRouterCtrlTclasDelCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TRAFFIC_PROTOCOL_IND, CsrWifiRouterCtrlTrafficProtocolIndSizeof, CsrWifiRouterCtrlTrafficProtocolIndSer, CsrWifiRouterCtrlTrafficProtocolIndDes, CsrWifiRouterCtrlTrafficProtocolIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_TRAFFIC_SAMPLE_IND, CsrWifiRouterCtrlTrafficSampleIndSizeof, CsrWifiRouterCtrlTrafficSampleIndSer, CsrWifiRouterCtrlTrafficSampleIndDes, CsrWifiRouterCtrlTrafficSampleIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_OFF_IND, CsrWifiRouterCtrlWifiOffIndSizeof, CsrWifiRouterCtrlWifiOffIndSer, CsrWifiRouterCtrlWifiOffIndDes, CsrWifiRouterCtrlWifiOffIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_OFF_CFM, CsrWifiRouterCtrlWifiOffCfmSizeof, CsrWifiRouterCtrlWifiOffCfmSer, CsrWifiRouterCtrlWifiOffCfmDes, CsrWifiRouterCtrlWifiOffCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_ON_IND, CsrWifiRouterCtrlWifiOnIndSizeof, CsrWifiRouterCtrlWifiOnIndSer, CsrWifiRouterCtrlWifiOnIndDes, CsrWifiRouterCtrlWifiOnIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WIFI_ON_CFM, CsrWifiRouterCtrlWifiOnCfmSizeof, CsrWifiRouterCtrlWifiOnCfmSer, CsrWifiRouterCtrlWifiOnCfmDes, CsrWifiRouterCtrlWifiOnCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_M4_READY_TO_SEND_IND, CsrWifiRouterCtrlM4ReadyToSendIndSizeof, CsrWifiRouterCtrlM4ReadyToSendIndSer, CsrWifiRouterCtrlM4ReadyToSendIndDes, CsrWifiRouterCtrlM4ReadyToSendIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_M4_TRANSMITTED_IND, CsrWifiRouterCtrlM4TransmittedIndSizeof, CsrWifiRouterCtrlM4TransmittedIndSer, CsrWifiRouterCtrlM4TransmittedIndDes, CsrWifiRouterCtrlM4TransmittedIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_MIC_FAILURE_IND, CsrWifiRouterCtrlMicFailureIndSizeof, CsrWifiRouterCtrlMicFailureIndSer, CsrWifiRouterCtrlMicFailureIndDes, CsrWifiRouterCtrlMicFailureIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_CONNECTED_IND, CsrWifiRouterCtrlConnectedIndSizeof, CsrWifiRouterCtrlConnectedIndSer, CsrWifiRouterCtrlConnectedIndDes, CsrWifiRouterCtrlConnectedIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PEER_ADD_CFM, CsrWifiRouterCtrlPeerAddCfmSizeof, CsrWifiRouterCtrlPeerAddCfmSer, CsrWifiRouterCtrlPeerAddCfmDes, CsrWifiRouterCtrlPeerAddCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PEER_DEL_CFM, CsrWifiRouterCtrlPeerDelCfmSizeof, CsrWifiRouterCtrlPeerDelCfmSer, CsrWifiRouterCtrlPeerDelCfmDes, CsrWifiRouterCtrlPeerDelCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_UNEXPECTED_FRAME_IND, CsrWifiRouterCtrlUnexpectedFrameIndSizeof, CsrWifiRouterCtrlUnexpectedFrameIndSer, CsrWifiRouterCtrlUnexpectedFrameIndDes, CsrWifiRouterCtrlUnexpectedFrameIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_PEER_UPDATE_CFM, CsrWifiRouterCtrlPeerUpdateCfmSizeof, CsrWifiRouterCtrlPeerUpdateCfmSer, CsrWifiRouterCtrlPeerUpdateCfmDes, CsrWifiRouterCtrlPeerUpdateCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_CAPABILITIES_CFM, CsrWifiRouterCtrlCapabilitiesCfmSizeof, CsrWifiRouterCtrlCapabilitiesCfmSer, CsrWifiRouterCtrlCapabilitiesCfmDes, CsrWifiRouterCtrlCapabilitiesCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ENABLE_CFM, CsrWifiRouterCtrlBlockAckEnableCfmSizeof, CsrWifiRouterCtrlBlockAckEnableCfmSer, CsrWifiRouterCtrlBlockAckEnableCfmDes, CsrWifiRouterCtrlBlockAckEnableCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_DISABLE_CFM, CsrWifiRouterCtrlBlockAckDisableCfmSizeof, CsrWifiRouterCtrlBlockAckDisableCfmSer, CsrWifiRouterCtrlBlockAckDisableCfmDes, CsrWifiRouterCtrlBlockAckDisableCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ERROR_IND, CsrWifiRouterCtrlBlockAckErrorIndSizeof, CsrWifiRouterCtrlBlockAckErrorIndSer, CsrWifiRouterCtrlBlockAckErrorIndDes, CsrWifiRouterCtrlBlockAckErrorIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_STA_INACTIVE_IND, CsrWifiRouterCtrlStaInactiveIndSizeof, CsrWifiRouterCtrlStaInactiveIndSer, CsrWifiRouterCtrlStaInactiveIndDes, CsrWifiRouterCtrlStaInactiveIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_RX_MIC_CHECK_IND, CsrWifiRouterCtrlWapiRxMicCheckIndSizeof, CsrWifiRouterCtrlWapiRxMicCheckIndSer, CsrWifiRouterCtrlWapiRxMicCheckIndDes, CsrWifiRouterCtrlWapiRxMicCheckIndSerFree },
+    { CSR_WIFI_ROUTER_CTRL_MODE_SET_CFM, CsrWifiRouterCtrlModeSetCfmSizeof, CsrWifiRouterCtrlModeSetCfmSer, CsrWifiRouterCtrlModeSetCfmDes, CsrWifiRouterCtrlModeSetCfmSerFree },
+    { CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_ENCRYPT_IND, CsrWifiRouterCtrlWapiUnicastTxEncryptIndSizeof, CsrWifiRouterCtrlWapiUnicastTxEncryptIndSer, CsrWifiRouterCtrlWapiUnicastTxEncryptIndDes, CsrWifiRouterCtrlWapiUnicastTxEncryptIndSerFree },
+
+    { 0, NULL, NULL, NULL, NULL },
+};
+
+CsrMsgConvMsgEntry* CsrWifiRouterCtrlConverterLookup(CsrMsgConvMsgEntry *ce, u16 msgType)
+{
+    if (msgType & CSR_PRIM_UPSTREAM)
+    {
+        u16 idx = (msgType & ~CSR_PRIM_UPSTREAM) + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT;
+        if (idx < (CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_COUNT + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT) &&
+            csrwifirouterctrl_conv_lut[idx].msgType == msgType)
+        {
+            return &csrwifirouterctrl_conv_lut[idx];
+        }
+    }
+    else
+    {
+        if (msgType < CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT &&
+            csrwifirouterctrl_conv_lut[msgType].msgType == msgType)
+        {
+            return &csrwifirouterctrl_conv_lut[msgType];
+        }
+    }
+    return NULL;
+}
+
+
+void CsrWifiRouterCtrlConverterInit(void)
+{
+    CsrMsgConvInsert(CSR_WIFI_ROUTER_CTRL_PRIM, csrwifirouterctrl_conv_lut);
+    CsrMsgConvCustomLookupRegister(CSR_WIFI_ROUTER_CTRL_PRIM, CsrWifiRouterCtrlConverterLookup);
+}
+
+
+#ifdef CSR_LOG_ENABLE
+static const CsrLogPrimitiveInformation csrwifirouterctrl_conv_info = {
+    CSR_WIFI_ROUTER_CTRL_PRIM,
+    (char *)"CSR_WIFI_ROUTER_CTRL_PRIM",
+    csrwifirouterctrl_conv_lut
+};
+const CsrLogPrimitiveInformation* CsrWifiRouterCtrlTechInfoGet(void)
+{
+    return &csrwifirouterctrl_conv_info;
+}
+
+
+#endif /* CSR_LOG_ENABLE */
+#endif /* EXCLUDE_CSR_WIFI_ROUTER_CTRL_MODULE */
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_converter_init.h b/drivers/staging/csr/csr_wifi_router_ctrl_converter_init.h
new file mode 100644
index 00000000000..0c9d26b7a0b
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_converter_init.h
@@ -0,0 +1,42 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_CTRL_CONVERTER_INIT_H__
+#define CSR_WIFI_ROUTER_CTRL_CONVERTER_INIT_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_ROUTER_CTRL_MODULE
+
+#include "csr_msgconv.h"
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+
+extern const CsrLogPrimitiveInformation* CsrWifiRouterCtrlTechInfoGet(void);
+#endif /* CSR_LOG_ENABLE */
+
+extern void CsrWifiRouterCtrlConverterInit(void);
+
+#else /* EXCLUDE_CSR_WIFI_ROUTER_CTRL_MODULE */
+
+#define CsrWifiRouterCtrlConverterInit()
+
+#endif /* EXCLUDE_CSR_WIFI_ROUTER_CTRL_MODULE */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_CTRL_CONVERTER_INIT_H__ */
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_free_downstream_contents.c b/drivers/staging/csr/csr_wifi_router_ctrl_free_downstream_contents.c
new file mode 100644
index 00000000000..7fa85fb4d6c
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_free_downstream_contents.c
@@ -0,0 +1,108 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_router_ctrl_prim.h"
+#include "csr_wifi_router_ctrl_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiRouterCtrlFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_ROUTER_CTRL_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterCtrlFreeDownstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_ROUTER_CTRL_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiRouterCtrlPrim *) message))
+    {
+        case CSR_WIFI_ROUTER_CTRL_HIP_REQ:
+        {
+            CsrWifiRouterCtrlHipReq *p = (CsrWifiRouterCtrlHipReq *)message;
+            kfree(p->mlmeCommand);
+            p->mlmeCommand = NULL;
+            kfree(p->dataRef1);
+            p->dataRef1 = NULL;
+            kfree(p->dataRef2);
+            p->dataRef2 = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_RES:
+        {
+            CsrWifiRouterCtrlMulticastAddressRes *p = (CsrWifiRouterCtrlMulticastAddressRes *)message;
+            kfree(p->getAddresses);
+            p->getAddresses = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_REQ:
+        {
+            CsrWifiRouterCtrlTclasAddReq *p = (CsrWifiRouterCtrlTclasAddReq *)message;
+            kfree(p->tclas);
+            p->tclas = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_REQ:
+        {
+            CsrWifiRouterCtrlTclasDelReq *p = (CsrWifiRouterCtrlTclasDelReq *)message;
+            kfree(p->tclas);
+            p->tclas = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WIFI_ON_REQ:
+        {
+            CsrWifiRouterCtrlWifiOnReq *p = (CsrWifiRouterCtrlWifiOnReq *)message;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WIFI_ON_RES:
+        {
+            CsrWifiRouterCtrlWifiOnRes *p = (CsrWifiRouterCtrlWifiOnRes *)message;
+            kfree(p->smeVersions.smeBuild);
+            p->smeVersions.smeBuild = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WAPI_RX_PKT_REQ:
+        {
+            CsrWifiRouterCtrlWapiRxPktReq *p = (CsrWifiRouterCtrlWapiRxPktReq *)message;
+            kfree(p->signal);
+            p->signal = NULL;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_PKT_REQ:
+        {
+            CsrWifiRouterCtrlWapiUnicastTxPktReq *p = (CsrWifiRouterCtrlWapiUnicastTxPktReq *)message;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_free_upstream_contents.c b/drivers/staging/csr/csr_wifi_router_ctrl_free_upstream_contents.c
new file mode 100644
index 00000000000..954b3defc49
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_free_upstream_contents.c
@@ -0,0 +1,87 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_router_ctrl_prim.h"
+#include "csr_wifi_router_ctrl_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiRouterCtrlFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_ROUTER_CTRL_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterCtrlFreeUpstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_ROUTER_CTRL_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiRouterCtrlPrim *) message))
+    {
+        case CSR_WIFI_ROUTER_CTRL_HIP_IND:
+        {
+            CsrWifiRouterCtrlHipInd *p = (CsrWifiRouterCtrlHipInd *)message;
+            kfree(p->mlmeCommand);
+            p->mlmeCommand = NULL;
+            kfree(p->dataRef1);
+            p->dataRef1 = NULL;
+            kfree(p->dataRef2);
+            p->dataRef2 = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_IND:
+        {
+            CsrWifiRouterCtrlMulticastAddressInd *p = (CsrWifiRouterCtrlMulticastAddressInd *)message;
+            kfree(p->setAddresses);
+            p->setAddresses = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WIFI_ON_IND:
+        {
+            CsrWifiRouterCtrlWifiOnInd *p = (CsrWifiRouterCtrlWifiOnInd *)message;
+            kfree(p->versions.routerBuild);
+            p->versions.routerBuild = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WAPI_RX_MIC_CHECK_IND:
+        {
+            CsrWifiRouterCtrlWapiRxMicCheckInd *p = (CsrWifiRouterCtrlWapiRxMicCheckInd *)message;
+            kfree(p->signal);
+            p->signal = NULL;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+        case CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_ENCRYPT_IND:
+        {
+            CsrWifiRouterCtrlWapiUnicastTxEncryptInd *p = (CsrWifiRouterCtrlWapiUnicastTxEncryptInd *)message;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_lib.h b/drivers/staging/csr/csr_wifi_router_ctrl_lib.h
new file mode 100644
index 00000000000..93d0fadf5e6
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_lib.h
@@ -0,0 +1,2092 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_CTRL_LIB_H__
+#define CSR_WIFI_ROUTER_CTRL_LIB_H__
+
+#include "csr_sched.h"
+#include "csr_macro.h"
+#include "csr_msg_transport.h"
+
+#include "csr_wifi_lib.h"
+
+#include "csr_wifi_router_ctrl_prim.h"
+#include "csr_wifi_router_task.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiRouterCtrlFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_ROUTER_CTRL upstream message. Does not
+ *      free the message itself, and can only be used for upstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_ROUTER_CTRL upstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterCtrlFreeUpstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiRouterCtrlFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_ROUTER_CTRL downstream message. Does not
+ *      free the message itself, and can only be used for downstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_ROUTER_CTRL downstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterCtrlFreeDownstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ * Enum to string functions
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiRouterCtrlBlockAckRoleToString(CsrWifiRouterCtrlBlockAckRole value);
+const char* CsrWifiRouterCtrlControlIndicationToString(CsrWifiRouterCtrlControlIndication value);
+const char* CsrWifiRouterCtrlListActionToString(CsrWifiRouterCtrlListAction value);
+const char* CsrWifiRouterCtrlLowPowerModeToString(CsrWifiRouterCtrlLowPowerMode value);
+const char* CsrWifiRouterCtrlMediaStatusToString(CsrWifiRouterCtrlMediaStatus value);
+const char* CsrWifiRouterCtrlModeToString(CsrWifiRouterCtrlMode value);
+const char* CsrWifiRouterCtrlPeerStatusToString(CsrWifiRouterCtrlPeerStatus value);
+const char* CsrWifiRouterCtrlPortActionToString(CsrWifiRouterCtrlPortAction value);
+const char* CsrWifiRouterCtrlPowersaveTypeToString(CsrWifiRouterCtrlPowersaveType value);
+const char* CsrWifiRouterCtrlProtocolDirectionToString(CsrWifiRouterCtrlProtocolDirection value);
+const char* CsrWifiRouterCtrlQoSControlToString(CsrWifiRouterCtrlQoSControl value);
+const char* CsrWifiRouterCtrlQueueConfigToString(CsrWifiRouterCtrlQueueConfig value);
+const char* CsrWifiRouterCtrlTrafficConfigTypeToString(CsrWifiRouterCtrlTrafficConfigType value);
+const char* CsrWifiRouterCtrlTrafficPacketTypeToString(CsrWifiRouterCtrlTrafficPacketType value);
+const char* CsrWifiRouterCtrlTrafficTypeToString(CsrWifiRouterCtrlTrafficType value);
+
+
+/*----------------------------------------------------------------------------*
+ * CsrPrim Type toString function.
+ * Converts a message type to the String name of the Message
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiRouterCtrlPrimTypeToString(CsrPrim msgType);
+
+/*----------------------------------------------------------------------------*
+ * Lookup arrays for PrimType name Strings
+ *----------------------------------------------------------------------------*/
+extern const char *CsrWifiRouterCtrlUpstreamPrimNames[CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_COUNT];
+extern const char *CsrWifiRouterCtrlDownstreamPrimNames[CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT];
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckDisableReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag    -
+    clientData      -
+    macAddress      -
+    trafficStreamID -
+    role            -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlBlockAckDisableReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckDisableReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_DISABLE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->macAddress = (macAddress__); \
+    msg__->trafficStreamID = (trafficStreamID__); \
+    msg__->role = (role__);
+
+#define CsrWifiRouterCtrlBlockAckDisableReqSendTo(dst__, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__) \
+    { \
+        CsrWifiRouterCtrlBlockAckDisableReq *msg__; \
+        CsrWifiRouterCtrlBlockAckDisableReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlBlockAckDisableReqSend(src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__) \
+    CsrWifiRouterCtrlBlockAckDisableReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckDisableCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlBlockAckDisableCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckDisableCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_DISABLE_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlBlockAckDisableCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlBlockAckDisableCfm *msg__; \
+        CsrWifiRouterCtrlBlockAckDisableCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlBlockAckDisableCfmSend(dst__, clientData__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlBlockAckDisableCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckEnableReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag    -
+    clientData      -
+    macAddress      -
+    trafficStreamID -
+    role            -
+    bufferSize      -
+    timeout         -
+    ssn             -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlBlockAckEnableReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__, bufferSize__, timeout__, ssn__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckEnableReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ENABLE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->macAddress = (macAddress__); \
+    msg__->trafficStreamID = (trafficStreamID__); \
+    msg__->role = (role__); \
+    msg__->bufferSize = (bufferSize__); \
+    msg__->timeout = (timeout__); \
+    msg__->ssn = (ssn__);
+
+#define CsrWifiRouterCtrlBlockAckEnableReqSendTo(dst__, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__, bufferSize__, timeout__, ssn__) \
+    { \
+        CsrWifiRouterCtrlBlockAckEnableReq *msg__; \
+        CsrWifiRouterCtrlBlockAckEnableReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__, bufferSize__, timeout__, ssn__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlBlockAckEnableReqSend(src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__, bufferSize__, timeout__, ssn__) \
+    CsrWifiRouterCtrlBlockAckEnableReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, macAddress__, trafficStreamID__, role__, bufferSize__, timeout__, ssn__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckEnableCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlBlockAckEnableCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckEnableCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ENABLE_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlBlockAckEnableCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlBlockAckEnableCfm *msg__; \
+        CsrWifiRouterCtrlBlockAckEnableCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlBlockAckEnableCfmSend(dst__, clientData__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlBlockAckEnableCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckErrorIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue           - Destination Task Queue
+    clientData      -
+    interfaceTag    -
+    trafficStreamID -
+    peerMacAddress  -
+    status          -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlBlockAckErrorIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, trafficStreamID__, peerMacAddress__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckErrorInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ERROR_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->trafficStreamID = (trafficStreamID__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlBlockAckErrorIndSendTo(dst__, src__, clientData__, interfaceTag__, trafficStreamID__, peerMacAddress__, status__) \
+    { \
+        CsrWifiRouterCtrlBlockAckErrorInd *msg__; \
+        CsrWifiRouterCtrlBlockAckErrorIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, trafficStreamID__, peerMacAddress__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlBlockAckErrorIndSend(dst__, clientData__, interfaceTag__, trafficStreamID__, peerMacAddress__, status__) \
+    CsrWifiRouterCtrlBlockAckErrorIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, trafficStreamID__, peerMacAddress__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlCapabilitiesReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    clientData -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlCapabilitiesReqCreate(msg__, dst__, src__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlCapabilitiesReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_CAPABILITIES_REQ, dst__, src__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlCapabilitiesReqSendTo(dst__, src__, clientData__) \
+    { \
+        CsrWifiRouterCtrlCapabilitiesReq *msg__; \
+        CsrWifiRouterCtrlCapabilitiesReqCreate(msg__, dst__, src__, clientData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlCapabilitiesReqSend(src__, clientData__) \
+    CsrWifiRouterCtrlCapabilitiesReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlCapabilitiesCfmSend
+
+  DESCRIPTION
+    The router sends this primitive to confirm the size of the queues of the
+    HIP.
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    clientData       -
+    commandQueueSize - Size of command queue
+    trafficQueueSize - Size of traffic queue (per AC)
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlCapabilitiesCfmCreate(msg__, dst__, src__, clientData__, commandQueueSize__, trafficQueueSize__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlCapabilitiesCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_CAPABILITIES_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->commandQueueSize = (commandQueueSize__); \
+    msg__->trafficQueueSize = (trafficQueueSize__);
+
+#define CsrWifiRouterCtrlCapabilitiesCfmSendTo(dst__, src__, clientData__, commandQueueSize__, trafficQueueSize__) \
+    { \
+        CsrWifiRouterCtrlCapabilitiesCfm *msg__; \
+        CsrWifiRouterCtrlCapabilitiesCfmCreate(msg__, dst__, src__, clientData__, commandQueueSize__, trafficQueueSize__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlCapabilitiesCfmSend(dst__, clientData__, commandQueueSize__, trafficQueueSize__) \
+    CsrWifiRouterCtrlCapabilitiesCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, commandQueueSize__, trafficQueueSize__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlConfigurePowerModeReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    clientData -
+    mode       -
+    wakeHost   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlConfigurePowerModeReqCreate(msg__, dst__, src__, clientData__, mode__, wakeHost__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlConfigurePowerModeReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_CONFIGURE_POWER_MODE_REQ, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->mode = (mode__); \
+    msg__->wakeHost = (wakeHost__);
+
+#define CsrWifiRouterCtrlConfigurePowerModeReqSendTo(dst__, src__, clientData__, mode__, wakeHost__) \
+    { \
+        CsrWifiRouterCtrlConfigurePowerModeReq *msg__; \
+        CsrWifiRouterCtrlConfigurePowerModeReqCreate(msg__, dst__, src__, clientData__, mode__, wakeHost__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlConfigurePowerModeReqSend(src__, clientData__, mode__, wakeHost__) \
+    CsrWifiRouterCtrlConfigurePowerModeReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__, mode__, wakeHost__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlConnectedIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+    peerStatus     -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlConnectedIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, peerStatus__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlConnectedInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_CONNECTED_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->peerStatus = (peerStatus__);
+
+#define CsrWifiRouterCtrlConnectedIndSendTo(dst__, src__, clientData__, interfaceTag__, peerMacAddress__, peerStatus__) \
+    { \
+        CsrWifiRouterCtrlConnectedInd *msg__; \
+        CsrWifiRouterCtrlConnectedIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, peerStatus__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlConnectedIndSend(dst__, clientData__, interfaceTag__, peerMacAddress__, peerStatus__) \
+    CsrWifiRouterCtrlConnectedIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, peerMacAddress__, peerStatus__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlHipReqSend
+
+  DESCRIPTION
+    This primitive is used for transferring MLME messages to the HIP.
+
+  PARAMETERS
+    queue             - Message Source Task Queue (Cfm's will be sent to this Queue)
+    mlmeCommandLength - Length of the MLME signal
+    mlmeCommand       - Pointer to the MLME signal
+    dataRef1Length    - Length of the dataRef1 bulk data
+    dataRef1          - Pointer to the bulk data 1
+    dataRef2Length    - Length of the dataRef2 bulk data
+    dataRef2          - Pointer to the bulk data 2
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlHipReqCreate(msg__, dst__, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlHipReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_HIP_REQ, dst__, src__); \
+    msg__->mlmeCommandLength = (mlmeCommandLength__); \
+    msg__->mlmeCommand = (mlmeCommand__); \
+    msg__->dataRef1Length = (dataRef1Length__); \
+    msg__->dataRef1 = (dataRef1__); \
+    msg__->dataRef2Length = (dataRef2Length__); \
+    msg__->dataRef2 = (dataRef2__);
+
+#define CsrWifiRouterCtrlHipReqSendTo(dst__, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__) \
+    { \
+        CsrWifiRouterCtrlHipReq *msg__; \
+        CsrWifiRouterCtrlHipReqCreate(msg__, dst__, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlHipReqSend(src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__) \
+    CsrWifiRouterCtrlHipReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlHipIndSend
+
+  DESCRIPTION
+    This primitive is used for transferring MLME messages from the HIP.
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    mlmeCommandLength - Length of the MLME signal
+    mlmeCommand       - Pointer to the MLME signal
+    dataRef1Length    - Length of the dataRef1 bulk data
+    dataRef1          - Pointer to the bulk data 1
+    dataRef2Length    - Length of the dataRef2 bulk data
+    dataRef2          - Pointer to the bulk data 2
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlHipIndCreate(msg__, dst__, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlHipInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_HIP_IND, dst__, src__); \
+    msg__->mlmeCommandLength = (mlmeCommandLength__); \
+    msg__->mlmeCommand = (mlmeCommand__); \
+    msg__->dataRef1Length = (dataRef1Length__); \
+    msg__->dataRef1 = (dataRef1__); \
+    msg__->dataRef2Length = (dataRef2Length__); \
+    msg__->dataRef2 = (dataRef2__);
+
+#define CsrWifiRouterCtrlHipIndSendTo(dst__, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__) \
+    { \
+        CsrWifiRouterCtrlHipInd *msg__; \
+        CsrWifiRouterCtrlHipIndCreate(msg__, dst__, src__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlHipIndSend(dst__, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__) \
+    CsrWifiRouterCtrlHipIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, mlmeCommandLength__, mlmeCommand__, dataRef1Length__, dataRef1__, dataRef2Length__, dataRef2__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlM4ReadyToSendIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlM4ReadyToSendIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlM4ReadyToSendInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_M4_READY_TO_SEND_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__);
+
+#define CsrWifiRouterCtrlM4ReadyToSendIndSendTo(dst__, src__, clientData__, interfaceTag__, peerMacAddress__) \
+    { \
+        CsrWifiRouterCtrlM4ReadyToSendInd *msg__; \
+        CsrWifiRouterCtrlM4ReadyToSendIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlM4ReadyToSendIndSend(dst__, clientData__, interfaceTag__, peerMacAddress__) \
+    CsrWifiRouterCtrlM4ReadyToSendIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, peerMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlM4TransmitReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    clientData   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlM4TransmitReqCreate(msg__, dst__, src__, interfaceTag__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlM4TransmitReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_M4_TRANSMIT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlM4TransmitReqSendTo(dst__, src__, interfaceTag__, clientData__) \
+    { \
+        CsrWifiRouterCtrlM4TransmitReq *msg__; \
+        CsrWifiRouterCtrlM4TransmitReqCreate(msg__, dst__, src__, interfaceTag__, clientData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlM4TransmitReqSend(src__, interfaceTag__, clientData__) \
+    CsrWifiRouterCtrlM4TransmitReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlM4TransmittedIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+    status         -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlM4TransmittedIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlM4TransmittedInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_M4_TRANSMITTED_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlM4TransmittedIndSendTo(dst__, src__, clientData__, interfaceTag__, peerMacAddress__, status__) \
+    { \
+        CsrWifiRouterCtrlM4TransmittedInd *msg__; \
+        CsrWifiRouterCtrlM4TransmittedIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlM4TransmittedIndSend(dst__, clientData__, interfaceTag__, peerMacAddress__, status__) \
+    CsrWifiRouterCtrlM4TransmittedIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, peerMacAddress__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMediaStatusReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    clientData   -
+    mediaStatus  -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlMediaStatusReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, mediaStatus__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlMediaStatusReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->mediaStatus = (mediaStatus__);
+
+#define CsrWifiRouterCtrlMediaStatusReqSendTo(dst__, src__, interfaceTag__, clientData__, mediaStatus__) \
+    { \
+        CsrWifiRouterCtrlMediaStatusReq *msg__; \
+        CsrWifiRouterCtrlMediaStatusReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, mediaStatus__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlMediaStatusReqSend(src__, interfaceTag__, clientData__, mediaStatus__) \
+    CsrWifiRouterCtrlMediaStatusReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, mediaStatus__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMicFailureIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+    unicastPdu     -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlMicFailureIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, unicastPdu__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlMicFailureInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_MIC_FAILURE_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->unicastPdu = (unicastPdu__);
+
+#define CsrWifiRouterCtrlMicFailureIndSendTo(dst__, src__, clientData__, interfaceTag__, peerMacAddress__, unicastPdu__) \
+    { \
+        CsrWifiRouterCtrlMicFailureInd *msg__; \
+        CsrWifiRouterCtrlMicFailureIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, unicastPdu__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlMicFailureIndSend(dst__, clientData__, interfaceTag__, peerMacAddress__, unicastPdu__) \
+    CsrWifiRouterCtrlMicFailureIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, peerMacAddress__, unicastPdu__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlModeSetReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue               - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag        -
+    clientData          -
+    mode                -
+    bssid               - BSSID of the network the device is going to be a part
+                          of
+    protection          - Set to TRUE if encryption is enabled for the
+                          connection/broadcast frames
+    intraBssDistEnabled - If set to TRUE, intra BSS destribution will be
+                          enabled. If set to FALSE, any unicast PDU which does
+                          not have the RA as the the local MAC address, shall be
+                          ignored. This field is interpreted by the receive if
+                          mode is set to CSR_WIFI_ROUTER_CTRL_MODE_P2PGO
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlModeSetReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, mode__, bssid__, protection__, intraBssDistEnabled__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlModeSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_MODE_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->mode = (mode__); \
+    msg__->bssid = (bssid__); \
+    msg__->protection = (protection__); \
+    msg__->intraBssDistEnabled = (intraBssDistEnabled__);
+
+#define CsrWifiRouterCtrlModeSetReqSendTo(dst__, src__, interfaceTag__, clientData__, mode__, bssid__, protection__, intraBssDistEnabled__) \
+    { \
+        CsrWifiRouterCtrlModeSetReq *msg__; \
+        CsrWifiRouterCtrlModeSetReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, mode__, bssid__, protection__, intraBssDistEnabled__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlModeSetReqSend(src__, interfaceTag__, clientData__, mode__, bssid__, protection__, intraBssDistEnabled__) \
+    CsrWifiRouterCtrlModeSetReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, mode__, bssid__, protection__, intraBssDistEnabled__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlModeSetCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    mode         -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlModeSetCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, mode__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlModeSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_MODE_SET_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->mode = (mode__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlModeSetCfmSendTo(dst__, src__, clientData__, interfaceTag__, mode__, status__) \
+    { \
+        CsrWifiRouterCtrlModeSetCfm *msg__; \
+        CsrWifiRouterCtrlModeSetCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, mode__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlModeSetCfmSend(dst__, clientData__, interfaceTag__, mode__, status__) \
+    CsrWifiRouterCtrlModeSetCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, mode__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMulticastAddressIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    clientData        -
+    interfaceTag      -
+    action            -
+    setAddressesCount -
+    setAddresses      -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlMulticastAddressIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, action__, setAddressesCount__, setAddresses__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlMulticastAddressInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->action = (action__); \
+    msg__->setAddressesCount = (setAddressesCount__); \
+    msg__->setAddresses = (setAddresses__);
+
+#define CsrWifiRouterCtrlMulticastAddressIndSendTo(dst__, src__, clientData__, interfaceTag__, action__, setAddressesCount__, setAddresses__) \
+    { \
+        CsrWifiRouterCtrlMulticastAddressInd *msg__; \
+        CsrWifiRouterCtrlMulticastAddressIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, action__, setAddressesCount__, setAddresses__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlMulticastAddressIndSend(dst__, clientData__, interfaceTag__, action__, setAddressesCount__, setAddresses__) \
+    CsrWifiRouterCtrlMulticastAddressIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, action__, setAddressesCount__, setAddresses__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMulticastAddressResSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    interfaceTag      -
+    clientData        -
+    status            -
+    action            -
+    getAddressesCount -
+    getAddresses      -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlMulticastAddressResCreate(msg__, dst__, src__, interfaceTag__, clientData__, status__, action__, getAddressesCount__, getAddresses__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlMulticastAddressRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_RES, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->status = (status__); \
+    msg__->action = (action__); \
+    msg__->getAddressesCount = (getAddressesCount__); \
+    msg__->getAddresses = (getAddresses__);
+
+#define CsrWifiRouterCtrlMulticastAddressResSendTo(dst__, src__, interfaceTag__, clientData__, status__, action__, getAddressesCount__, getAddresses__) \
+    { \
+        CsrWifiRouterCtrlMulticastAddressRes *msg__; \
+        CsrWifiRouterCtrlMulticastAddressResCreate(msg__, dst__, src__, interfaceTag__, clientData__, status__, action__, getAddressesCount__, getAddresses__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlMulticastAddressResSend(src__, interfaceTag__, clientData__, status__, action__, getAddressesCount__, getAddresses__) \
+    CsrWifiRouterCtrlMulticastAddressResSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, status__, action__, getAddressesCount__, getAddresses__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerAddReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue          - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag   -
+    clientData     -
+    peerMacAddress -
+    associationId  -
+    staInfo        -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPeerAddReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, peerMacAddress__, associationId__, staInfo__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPeerAddReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PEER_ADD_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->associationId = (associationId__); \
+    msg__->staInfo = (staInfo__);
+
+#define CsrWifiRouterCtrlPeerAddReqSendTo(dst__, src__, interfaceTag__, clientData__, peerMacAddress__, associationId__, staInfo__) \
+    { \
+        CsrWifiRouterCtrlPeerAddReq *msg__; \
+        CsrWifiRouterCtrlPeerAddReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, peerMacAddress__, associationId__, staInfo__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPeerAddReqSend(src__, interfaceTag__, clientData__, peerMacAddress__, associationId__, staInfo__) \
+    CsrWifiRouterCtrlPeerAddReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, peerMacAddress__, associationId__, staInfo__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerAddCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    clientData       -
+    interfaceTag     -
+    peerMacAddress   -
+    peerRecordHandle -
+    status           -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPeerAddCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, peerRecordHandle__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPeerAddCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PEER_ADD_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->peerRecordHandle = (peerRecordHandle__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlPeerAddCfmSendTo(dst__, src__, clientData__, interfaceTag__, peerMacAddress__, peerRecordHandle__, status__) \
+    { \
+        CsrWifiRouterCtrlPeerAddCfm *msg__; \
+        CsrWifiRouterCtrlPeerAddCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__, peerRecordHandle__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPeerAddCfmSend(dst__, clientData__, interfaceTag__, peerMacAddress__, peerRecordHandle__, status__) \
+    CsrWifiRouterCtrlPeerAddCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, peerMacAddress__, peerRecordHandle__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerDelReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue            - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag     -
+    clientData       -
+    peerRecordHandle -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPeerDelReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, peerRecordHandle__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPeerDelReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PEER_DEL_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->peerRecordHandle = (peerRecordHandle__);
+
+#define CsrWifiRouterCtrlPeerDelReqSendTo(dst__, src__, interfaceTag__, clientData__, peerRecordHandle__) \
+    { \
+        CsrWifiRouterCtrlPeerDelReq *msg__; \
+        CsrWifiRouterCtrlPeerDelReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, peerRecordHandle__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPeerDelReqSend(src__, interfaceTag__, clientData__, peerRecordHandle__) \
+    CsrWifiRouterCtrlPeerDelReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, peerRecordHandle__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerDelCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPeerDelCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPeerDelCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PEER_DEL_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlPeerDelCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlPeerDelCfm *msg__; \
+        CsrWifiRouterCtrlPeerDelCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPeerDelCfmSend(dst__, clientData__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlPeerDelCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerUpdateReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue            - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag     -
+    clientData       -
+    peerRecordHandle -
+    powersaveMode    -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPeerUpdateReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, peerRecordHandle__, powersaveMode__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPeerUpdateReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PEER_UPDATE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->peerRecordHandle = (peerRecordHandle__); \
+    msg__->powersaveMode = (powersaveMode__);
+
+#define CsrWifiRouterCtrlPeerUpdateReqSendTo(dst__, src__, interfaceTag__, clientData__, peerRecordHandle__, powersaveMode__) \
+    { \
+        CsrWifiRouterCtrlPeerUpdateReq *msg__; \
+        CsrWifiRouterCtrlPeerUpdateReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, peerRecordHandle__, powersaveMode__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPeerUpdateReqSend(src__, interfaceTag__, clientData__, peerRecordHandle__, powersaveMode__) \
+    CsrWifiRouterCtrlPeerUpdateReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, peerRecordHandle__, powersaveMode__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerUpdateCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPeerUpdateCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPeerUpdateCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PEER_UPDATE_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlPeerUpdateCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlPeerUpdateCfm *msg__; \
+        CsrWifiRouterCtrlPeerUpdateCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPeerUpdateCfmSend(dst__, clientData__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlPeerUpdateCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPortConfigureReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue                  - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag           -
+    clientData             -
+    uncontrolledPortAction -
+    controlledPortAction   -
+    macAddress             -
+    setProtection          -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPortConfigureReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, uncontrolledPortAction__, controlledPortAction__, macAddress__, setProtection__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPortConfigureReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PORT_CONFIGURE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->uncontrolledPortAction = (uncontrolledPortAction__); \
+    msg__->controlledPortAction = (controlledPortAction__); \
+    msg__->macAddress = (macAddress__); \
+    msg__->setProtection = (setProtection__);
+
+#define CsrWifiRouterCtrlPortConfigureReqSendTo(dst__, src__, interfaceTag__, clientData__, uncontrolledPortAction__, controlledPortAction__, macAddress__, setProtection__) \
+    { \
+        CsrWifiRouterCtrlPortConfigureReq *msg__; \
+        CsrWifiRouterCtrlPortConfigureReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, uncontrolledPortAction__, controlledPortAction__, macAddress__, setProtection__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPortConfigureReqSend(src__, interfaceTag__, clientData__, uncontrolledPortAction__, controlledPortAction__, macAddress__, setProtection__) \
+    CsrWifiRouterCtrlPortConfigureReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, uncontrolledPortAction__, controlledPortAction__, macAddress__, setProtection__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPortConfigureCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+    macAddress   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlPortConfigureCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__, macAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlPortConfigureCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_PORT_CONFIGURE_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->macAddress = (macAddress__);
+
+#define CsrWifiRouterCtrlPortConfigureCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__, macAddress__) \
+    { \
+        CsrWifiRouterCtrlPortConfigureCfm *msg__; \
+        CsrWifiRouterCtrlPortConfigureCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__, macAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlPortConfigureCfmSend(dst__, clientData__, interfaceTag__, status__, macAddress__) \
+    CsrWifiRouterCtrlPortConfigureCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__, macAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlQosControlReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    clientData   -
+    control      -
+    queueConfig  -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlQosControlReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, control__, queueConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlQosControlReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->control = (control__); \
+    msg__->queueConfig = (queueConfig__);
+
+#define CsrWifiRouterCtrlQosControlReqSendTo(dst__, src__, interfaceTag__, clientData__, control__, queueConfig__) \
+    { \
+        CsrWifiRouterCtrlQosControlReq *msg__; \
+        CsrWifiRouterCtrlQosControlReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, control__, queueConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlQosControlReqSend(src__, interfaceTag__, clientData__, control__, queueConfig__) \
+    CsrWifiRouterCtrlQosControlReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, control__, queueConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioDeinitialiseReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    clientData -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqCreate(msg__, dst__, src__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlRawSdioDeinitialiseReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_RAW_SDIO_DEINITIALISE_REQ, dst__, src__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqSendTo(dst__, src__, clientData__) \
+    { \
+        CsrWifiRouterCtrlRawSdioDeinitialiseReq *msg__; \
+        CsrWifiRouterCtrlRawSdioDeinitialiseReqCreate(msg__, dst__, src__, clientData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqSend(src__, clientData__) \
+    CsrWifiRouterCtrlRawSdioDeinitialiseReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioDeinitialiseCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    clientData -
+    result     -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlRawSdioDeinitialiseCfmCreate(msg__, dst__, src__, clientData__, result__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlRawSdioDeinitialiseCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_RAW_SDIO_DEINITIALISE_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->result = (result__);
+
+#define CsrWifiRouterCtrlRawSdioDeinitialiseCfmSendTo(dst__, src__, clientData__, result__) \
+    { \
+        CsrWifiRouterCtrlRawSdioDeinitialiseCfm *msg__; \
+        CsrWifiRouterCtrlRawSdioDeinitialiseCfmCreate(msg__, dst__, src__, clientData__, result__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlRawSdioDeinitialiseCfmSend(dst__, clientData__, result__) \
+    CsrWifiRouterCtrlRawSdioDeinitialiseCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, result__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioInitialiseReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    clientData -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlRawSdioInitialiseReqCreate(msg__, dst__, src__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlRawSdioInitialiseReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_RAW_SDIO_INITIALISE_REQ, dst__, src__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlRawSdioInitialiseReqSendTo(dst__, src__, clientData__) \
+    { \
+        CsrWifiRouterCtrlRawSdioInitialiseReq *msg__; \
+        CsrWifiRouterCtrlRawSdioInitialiseReqCreate(msg__, dst__, src__, clientData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlRawSdioInitialiseReqSend(src__, clientData__) \
+    CsrWifiRouterCtrlRawSdioInitialiseReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioInitialiseCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    clientData       -
+    result           -
+    byteRead         -
+    byteWrite        -
+    firmwareDownload -
+    reset            -
+    coreDumpPrepare  -
+    byteBlockRead    -
+    gpRead16         -
+    gpWrite16        -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlRawSdioInitialiseCfmCreate(msg__, dst__, src__, clientData__, result__, byteRead__, byteWrite__, firmwareDownload__, reset__, coreDumpPrepare__, byteBlockRead__, gpRead16__, gpWrite16__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlRawSdioInitialiseCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_RAW_SDIO_INITIALISE_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->result = (result__); \
+    msg__->byteRead = (byteRead__); \
+    msg__->byteWrite = (byteWrite__); \
+    msg__->firmwareDownload = (firmwareDownload__); \
+    msg__->reset = (reset__); \
+    msg__->coreDumpPrepare = (coreDumpPrepare__); \
+    msg__->byteBlockRead = (byteBlockRead__); \
+    msg__->gpRead16 = (gpRead16__); \
+    msg__->gpWrite16 = (gpWrite16__);
+
+#define CsrWifiRouterCtrlRawSdioInitialiseCfmSendTo(dst__, src__, clientData__, result__, byteRead__, byteWrite__, firmwareDownload__, reset__, coreDumpPrepare__, byteBlockRead__, gpRead16__, gpWrite16__) \
+    { \
+        CsrWifiRouterCtrlRawSdioInitialiseCfm *msg__; \
+        CsrWifiRouterCtrlRawSdioInitialiseCfmCreate(msg__, dst__, src__, clientData__, result__, byteRead__, byteWrite__, firmwareDownload__, reset__, coreDumpPrepare__, byteBlockRead__, gpRead16__, gpWrite16__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlRawSdioInitialiseCfmSend(dst__, clientData__, result__, byteRead__, byteWrite__, firmwareDownload__, reset__, coreDumpPrepare__, byteBlockRead__, gpRead16__, gpWrite16__) \
+    CsrWifiRouterCtrlRawSdioInitialiseCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, result__, byteRead__, byteWrite__, firmwareDownload__, reset__, coreDumpPrepare__, byteBlockRead__, gpRead16__, gpWrite16__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlResumeIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue           - Destination Task Queue
+    clientData      -
+    powerMaintained -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlResumeIndCreate(msg__, dst__, src__, clientData__, powerMaintained__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlResumeInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_RESUME_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->powerMaintained = (powerMaintained__);
+
+#define CsrWifiRouterCtrlResumeIndSendTo(dst__, src__, clientData__, powerMaintained__) \
+    { \
+        CsrWifiRouterCtrlResumeInd *msg__; \
+        CsrWifiRouterCtrlResumeIndCreate(msg__, dst__, src__, clientData__, powerMaintained__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlResumeIndSend(dst__, clientData__, powerMaintained__) \
+    CsrWifiRouterCtrlResumeIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, powerMaintained__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlResumeResSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    clientData -
+    status     -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlResumeResCreate(msg__, dst__, src__, clientData__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlResumeRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_RESUME_RES, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlResumeResSendTo(dst__, src__, clientData__, status__) \
+    { \
+        CsrWifiRouterCtrlResumeRes *msg__; \
+        CsrWifiRouterCtrlResumeResCreate(msg__, dst__, src__, clientData__, status__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlResumeResSend(src__, clientData__, status__) \
+    CsrWifiRouterCtrlResumeResSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlStaInactiveIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    staAddress   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlStaInactiveIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, staAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlStaInactiveInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_STA_INACTIVE_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->staAddress = (staAddress__);
+
+#define CsrWifiRouterCtrlStaInactiveIndSendTo(dst__, src__, clientData__, interfaceTag__, staAddress__) \
+    { \
+        CsrWifiRouterCtrlStaInactiveInd *msg__; \
+        CsrWifiRouterCtrlStaInactiveIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, staAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlStaInactiveIndSend(dst__, clientData__, interfaceTag__, staAddress__) \
+    CsrWifiRouterCtrlStaInactiveIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, staAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlSuspendIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue       - Destination Task Queue
+    clientData  -
+    hardSuspend -
+    d3Suspend   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlSuspendIndCreate(msg__, dst__, src__, clientData__, hardSuspend__, d3Suspend__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlSuspendInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_SUSPEND_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->hardSuspend = (hardSuspend__); \
+    msg__->d3Suspend = (d3Suspend__);
+
+#define CsrWifiRouterCtrlSuspendIndSendTo(dst__, src__, clientData__, hardSuspend__, d3Suspend__) \
+    { \
+        CsrWifiRouterCtrlSuspendInd *msg__; \
+        CsrWifiRouterCtrlSuspendIndCreate(msg__, dst__, src__, clientData__, hardSuspend__, d3Suspend__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlSuspendIndSend(dst__, clientData__, hardSuspend__, d3Suspend__) \
+    CsrWifiRouterCtrlSuspendIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, hardSuspend__, d3Suspend__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlSuspendResSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    clientData -
+    status     -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlSuspendResCreate(msg__, dst__, src__, clientData__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlSuspendRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_SUSPEND_RES, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlSuspendResSendTo(dst__, src__, clientData__, status__) \
+    { \
+        CsrWifiRouterCtrlSuspendRes *msg__; \
+        CsrWifiRouterCtrlSuspendResCreate(msg__, dst__, src__, clientData__, status__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlSuspendResSend(src__, clientData__, status__) \
+    CsrWifiRouterCtrlSuspendResSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasAddReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    clientData   -
+    tclasLength  -
+    tclas        -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTclasAddReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, tclasLength__, tclas__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTclasAddReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->tclasLength = (tclasLength__); \
+    msg__->tclas = (tclas__);
+
+#define CsrWifiRouterCtrlTclasAddReqSendTo(dst__, src__, interfaceTag__, clientData__, tclasLength__, tclas__) \
+    { \
+        CsrWifiRouterCtrlTclasAddReq *msg__; \
+        CsrWifiRouterCtrlTclasAddReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, tclasLength__, tclas__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTclasAddReqSend(src__, interfaceTag__, clientData__, tclasLength__, tclas__) \
+    CsrWifiRouterCtrlTclasAddReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, tclasLength__, tclas__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasAddCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTclasAddCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTclasAddCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlTclasAddCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlTclasAddCfm *msg__; \
+        CsrWifiRouterCtrlTclasAddCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTclasAddCfmSend(dst__, clientData__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlTclasAddCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasDelReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    clientData   -
+    tclasLength  -
+    tclas        -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTclasDelReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, tclasLength__, tclas__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTclasDelReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->tclasLength = (tclasLength__); \
+    msg__->tclas = (tclas__);
+
+#define CsrWifiRouterCtrlTclasDelReqSendTo(dst__, src__, interfaceTag__, clientData__, tclasLength__, tclas__) \
+    { \
+        CsrWifiRouterCtrlTclasDelReq *msg__; \
+        CsrWifiRouterCtrlTclasDelReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, tclasLength__, tclas__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTclasDelReqSend(src__, interfaceTag__, clientData__, tclasLength__, tclas__) \
+    CsrWifiRouterCtrlTclasDelReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, tclasLength__, tclas__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasDelCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTclasDelCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTclasDelCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlTclasDelCfmSendTo(dst__, src__, clientData__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlTclasDelCfm *msg__; \
+        CsrWifiRouterCtrlTclasDelCfmCreate(msg__, dst__, src__, clientData__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTclasDelCfmSend(dst__, clientData__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlTclasDelCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficClassificationReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    clientData   -
+    trafficType  -
+    period       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTrafficClassificationReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, trafficType__, period__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTrafficClassificationReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TRAFFIC_CLASSIFICATION_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->trafficType = (trafficType__); \
+    msg__->period = (period__);
+
+#define CsrWifiRouterCtrlTrafficClassificationReqSendTo(dst__, src__, interfaceTag__, clientData__, trafficType__, period__) \
+    { \
+        CsrWifiRouterCtrlTrafficClassificationReq *msg__; \
+        CsrWifiRouterCtrlTrafficClassificationReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, trafficType__, period__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTrafficClassificationReqSend(src__, interfaceTag__, clientData__, trafficType__, period__) \
+    CsrWifiRouterCtrlTrafficClassificationReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, trafficType__, period__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficConfigReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue             - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag      -
+    clientData        -
+    trafficConfigType -
+    config            -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTrafficConfigReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, trafficConfigType__, config__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTrafficConfigReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->clientData = (clientData__); \
+    msg__->trafficConfigType = (trafficConfigType__); \
+    msg__->config = (config__);
+
+#define CsrWifiRouterCtrlTrafficConfigReqSendTo(dst__, src__, interfaceTag__, clientData__, trafficConfigType__, config__) \
+    { \
+        CsrWifiRouterCtrlTrafficConfigReq *msg__; \
+        CsrWifiRouterCtrlTrafficConfigReqCreate(msg__, dst__, src__, interfaceTag__, clientData__, trafficConfigType__, config__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTrafficConfigReqSend(src__, interfaceTag__, clientData__, trafficConfigType__, config__) \
+    CsrWifiRouterCtrlTrafficConfigReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, clientData__, trafficConfigType__, config__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficProtocolIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    packetType   -
+    direction    -
+    srcAddress   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTrafficProtocolIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, packetType__, direction__, srcAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTrafficProtocolInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TRAFFIC_PROTOCOL_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->packetType = (packetType__); \
+    msg__->direction = (direction__); \
+    msg__->srcAddress = (srcAddress__);
+
+#define CsrWifiRouterCtrlTrafficProtocolIndSendTo(dst__, src__, clientData__, interfaceTag__, packetType__, direction__, srcAddress__) \
+    { \
+        CsrWifiRouterCtrlTrafficProtocolInd *msg__; \
+        CsrWifiRouterCtrlTrafficProtocolIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, packetType__, direction__, srcAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTrafficProtocolIndSend(dst__, clientData__, interfaceTag__, packetType__, direction__, srcAddress__) \
+    CsrWifiRouterCtrlTrafficProtocolIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, packetType__, direction__, srcAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficSampleIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    stats        -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlTrafficSampleIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, stats__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlTrafficSampleInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_TRAFFIC_SAMPLE_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->stats = (stats__);
+
+#define CsrWifiRouterCtrlTrafficSampleIndSendTo(dst__, src__, clientData__, interfaceTag__, stats__) \
+    { \
+        CsrWifiRouterCtrlTrafficSampleInd *msg__; \
+        CsrWifiRouterCtrlTrafficSampleIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, stats__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlTrafficSampleIndSend(dst__, clientData__, interfaceTag__, stats__) \
+    CsrWifiRouterCtrlTrafficSampleIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, stats__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlUnexpectedFrameIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlUnexpectedFrameIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlUnexpectedFrameInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_UNEXPECTED_FRAME_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__);
+
+#define CsrWifiRouterCtrlUnexpectedFrameIndSendTo(dst__, src__, clientData__, interfaceTag__, peerMacAddress__) \
+    { \
+        CsrWifiRouterCtrlUnexpectedFrameInd *msg__; \
+        CsrWifiRouterCtrlUnexpectedFrameIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, peerMacAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlUnexpectedFrameIndSend(dst__, clientData__, interfaceTag__, peerMacAddress__) \
+    CsrWifiRouterCtrlUnexpectedFrameIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, peerMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiFilterReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag    -
+    isWapiConnected -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiFilterReqCreate(msg__, dst__, src__, interfaceTag__, isWapiConnected__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiFilterReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_FILTER_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->isWapiConnected = (isWapiConnected__);
+
+#define CsrWifiRouterCtrlWapiFilterReqSendTo(dst__, src__, interfaceTag__, isWapiConnected__) \
+    { \
+        CsrWifiRouterCtrlWapiFilterReq *msg__; \
+        CsrWifiRouterCtrlWapiFilterReqCreate(msg__, dst__, src__, interfaceTag__, isWapiConnected__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiFilterReqSend(src__, interfaceTag__, isWapiConnected__) \
+    CsrWifiRouterCtrlWapiFilterReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, isWapiConnected__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiMulticastFilterReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiMulticastFilterReqCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiMulticastFilterReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_MULTICAST_FILTER_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlWapiMulticastFilterReqSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlWapiMulticastFilterReq *msg__; \
+        CsrWifiRouterCtrlWapiMulticastFilterReqCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiMulticastFilterReqSend(src__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlWapiMulticastFilterReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiRxMicCheckIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    signalLength -
+    signal       -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiRxMicCheckIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, signalLength__, signal__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiRxMicCheckInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_RX_MIC_CHECK_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->signalLength = (signalLength__); \
+    msg__->signal = (signal__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiRouterCtrlWapiRxMicCheckIndSendTo(dst__, src__, clientData__, interfaceTag__, signalLength__, signal__, dataLength__, data__) \
+    { \
+        CsrWifiRouterCtrlWapiRxMicCheckInd *msg__; \
+        CsrWifiRouterCtrlWapiRxMicCheckIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, signalLength__, signal__, dataLength__, data__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiRxMicCheckIndSend(dst__, clientData__, interfaceTag__, signalLength__, signal__, dataLength__, data__) \
+    CsrWifiRouterCtrlWapiRxMicCheckIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, signalLength__, signal__, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiRxPktReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    signalLength -
+    signal       -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiRxPktReqCreate(msg__, dst__, src__, interfaceTag__, signalLength__, signal__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiRxPktReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_RX_PKT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->signalLength = (signalLength__); \
+    msg__->signal = (signal__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiRouterCtrlWapiRxPktReqSendTo(dst__, src__, interfaceTag__, signalLength__, signal__, dataLength__, data__) \
+    { \
+        CsrWifiRouterCtrlWapiRxPktReq *msg__; \
+        CsrWifiRouterCtrlWapiRxPktReqCreate(msg__, dst__, src__, interfaceTag__, signalLength__, signal__, dataLength__, data__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiRxPktReqSend(src__, interfaceTag__, signalLength__, signal__, dataLength__, data__) \
+    CsrWifiRouterCtrlWapiRxPktReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, signalLength__, signal__, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiUnicastFilterReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiUnicastFilterReqCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiUnicastFilterReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_FILTER_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlWapiUnicastFilterReqSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterCtrlWapiUnicastFilterReq *msg__; \
+        CsrWifiRouterCtrlWapiUnicastFilterReqCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiUnicastFilterReqSend(src__, interfaceTag__, status__) \
+    CsrWifiRouterCtrlWapiUnicastFilterReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiUnicastTxEncryptIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    clientData   -
+    interfaceTag -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiUnicastTxEncryptIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiUnicastTxEncryptInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_ENCRYPT_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiRouterCtrlWapiUnicastTxEncryptIndSendTo(dst__, src__, clientData__, interfaceTag__, dataLength__, data__) \
+    { \
+        CsrWifiRouterCtrlWapiUnicastTxEncryptInd *msg__; \
+        CsrWifiRouterCtrlWapiUnicastTxEncryptIndCreate(msg__, dst__, src__, clientData__, interfaceTag__, dataLength__, data__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiUnicastTxEncryptIndSend(dst__, clientData__, interfaceTag__, dataLength__, data__) \
+    CsrWifiRouterCtrlWapiUnicastTxEncryptIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, interfaceTag__, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiUnicastTxPktReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWapiUnicastTxPktReqCreate(msg__, dst__, src__, interfaceTag__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWapiUnicastTxPktReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_PKT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiRouterCtrlWapiUnicastTxPktReqSendTo(dst__, src__, interfaceTag__, dataLength__, data__) \
+    { \
+        CsrWifiRouterCtrlWapiUnicastTxPktReq *msg__; \
+        CsrWifiRouterCtrlWapiUnicastTxPktReqCreate(msg__, dst__, src__, interfaceTag__, dataLength__, data__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWapiUnicastTxPktReqSend(src__, interfaceTag__, dataLength__, data__) \
+    CsrWifiRouterCtrlWapiUnicastTxPktReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    clientData -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOffReqCreate(msg__, dst__, src__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOffReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_OFF_REQ, dst__, src__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlWifiOffReqSendTo(dst__, src__, clientData__) \
+    { \
+        CsrWifiRouterCtrlWifiOffReq *msg__; \
+        CsrWifiRouterCtrlWifiOffReqCreate(msg__, dst__, src__, clientData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOffReqSend(src__, clientData__) \
+    CsrWifiRouterCtrlWifiOffReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    clientData        -
+    controlIndication -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOffIndCreate(msg__, dst__, src__, clientData__, controlIndication__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOffInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_OFF_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->controlIndication = (controlIndication__);
+
+#define CsrWifiRouterCtrlWifiOffIndSendTo(dst__, src__, clientData__, controlIndication__) \
+    { \
+        CsrWifiRouterCtrlWifiOffInd *msg__; \
+        CsrWifiRouterCtrlWifiOffIndCreate(msg__, dst__, src__, clientData__, controlIndication__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOffIndSend(dst__, clientData__, controlIndication__) \
+    CsrWifiRouterCtrlWifiOffIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, controlIndication__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffResSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    clientData -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOffResCreate(msg__, dst__, src__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOffRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_OFF_RES, dst__, src__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlWifiOffResSendTo(dst__, src__, clientData__) \
+    { \
+        CsrWifiRouterCtrlWifiOffRes *msg__; \
+        CsrWifiRouterCtrlWifiOffResCreate(msg__, dst__, src__, clientData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOffResSend(src__, clientData__) \
+    CsrWifiRouterCtrlWifiOffResSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    clientData -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOffCfmCreate(msg__, dst__, src__, clientData__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOffCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_OFF_CFM, dst__, src__); \
+    msg__->clientData = (clientData__);
+
+#define CsrWifiRouterCtrlWifiOffCfmSendTo(dst__, src__, clientData__) \
+    { \
+        CsrWifiRouterCtrlWifiOffCfm *msg__; \
+        CsrWifiRouterCtrlWifiOffCfmCreate(msg__, dst__, src__, clientData__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOffCfmSend(dst__, clientData__) \
+    CsrWifiRouterCtrlWifiOffCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnReqSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    clientData -
+    dataLength - Number of bytes in the buffer pointed to by 'data'
+    data       - Pointer to the buffer containing 'dataLength' bytes
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOnReqCreate(msg__, dst__, src__, clientData__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_ON_REQ, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiRouterCtrlWifiOnReqSendTo(dst__, src__, clientData__, dataLength__, data__) \
+    { \
+        CsrWifiRouterCtrlWifiOnReq *msg__; \
+        CsrWifiRouterCtrlWifiOnReqCreate(msg__, dst__, src__, clientData__, dataLength__, data__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOnReqSend(src__, clientData__, dataLength__, data__) \
+    CsrWifiRouterCtrlWifiOnReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnIndSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    clientData -
+    status     -
+    versions   -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOnIndCreate(msg__, dst__, src__, clientData__, status__, versions__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_ON_IND, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->status = (status__); \
+    msg__->versions = (versions__);
+
+#define CsrWifiRouterCtrlWifiOnIndSendTo(dst__, src__, clientData__, status__, versions__) \
+    { \
+        CsrWifiRouterCtrlWifiOnInd *msg__; \
+        CsrWifiRouterCtrlWifiOnIndCreate(msg__, dst__, src__, clientData__, status__, versions__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOnIndSend(dst__, clientData__, status__, versions__) \
+    CsrWifiRouterCtrlWifiOnIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, status__, versions__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnResSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    clientData          -
+    status              -
+    numInterfaceAddress -
+    stationMacAddress   - array size 1 MUST match CSR_WIFI_NUM_INTERFACES
+    smeVersions         -
+    scheduledInterrupt  -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOnResCreate(msg__, dst__, src__, clientData__, status__, numInterfaceAddress__, stationMacAddress__, smeVersions__, scheduledInterrupt__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_ON_RES, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->status = (status__); \
+    msg__->numInterfaceAddress = (numInterfaceAddress__); \
+    memcpy(msg__->stationMacAddress, (stationMacAddress__), sizeof(CsrWifiMacAddress) * 2); \
+    msg__->smeVersions = (smeVersions__); \
+    msg__->scheduledInterrupt = (scheduledInterrupt__);
+
+#define CsrWifiRouterCtrlWifiOnResSendTo(dst__, src__, clientData__, status__, numInterfaceAddress__, stationMacAddress__, smeVersions__, scheduledInterrupt__) \
+    { \
+        CsrWifiRouterCtrlWifiOnRes *msg__; \
+        CsrWifiRouterCtrlWifiOnResCreate(msg__, dst__, src__, clientData__, status__, numInterfaceAddress__, stationMacAddress__, smeVersions__, scheduledInterrupt__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOnResSend(src__, clientData__, status__, numInterfaceAddress__, stationMacAddress__, smeVersions__, scheduledInterrupt__) \
+    CsrWifiRouterCtrlWifiOnResSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, clientData__, status__, numInterfaceAddress__, stationMacAddress__, smeVersions__, scheduledInterrupt__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnCfmSend
+
+  DESCRIPTION
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    clientData -
+    status     -
+
+*******************************************************************************/
+#define CsrWifiRouterCtrlWifiOnCfmCreate(msg__, dst__, src__, clientData__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_CTRL_PRIM, CSR_WIFI_ROUTER_CTRL_WIFI_ON_CFM, dst__, src__); \
+    msg__->clientData = (clientData__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterCtrlWifiOnCfmSendTo(dst__, src__, clientData__, status__) \
+    { \
+        CsrWifiRouterCtrlWifiOnCfm *msg__; \
+        CsrWifiRouterCtrlWifiOnCfmCreate(msg__, dst__, src__, clientData__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_CTRL_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterCtrlWifiOnCfmSend(dst__, clientData__, status__) \
+    CsrWifiRouterCtrlWifiOnCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, clientData__, status__)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_CTRL_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_prim.h b/drivers/staging/csr/csr_wifi_router_ctrl_prim.h
new file mode 100644
index 00000000000..ec972ac0b5a
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_prim.h
@@ -0,0 +1,2122 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_CTRL_PRIM_H__
+#define CSR_WIFI_ROUTER_CTRL_PRIM_H__
+
+#include <linux/types.h>
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+#include "csr_wifi_common.h"
+#include "csr_result.h"
+#include "csr_wifi_fsm_event.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CSR_WIFI_ROUTER_CTRL_PRIM                                       (0x0401)
+
+typedef CsrPrim CsrWifiRouterCtrlPrim;
+
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioByteWrite)(u8 func, u32 address, u8 data);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioByteRead)(u8 func, u32 address, u8 *pdata);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioFirmwareDownload)(u32 length, const u8 *pdata);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioReset)(void);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioCoreDumpPrepare)(u8 suspendSme);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioByteBlockRead)(u8 func, u32 address, u8 *pdata, u32 length);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioGpRead16)(u8 func, u32 address, u16 *pdata);
+typedef CsrResult (*CsrWifiRouterCtrlRawSdioGpWrite16)(u8 func, u32 address, u16 data);
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckRole
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR
+                   -
+    CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlBlockAckRole;
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR   ((CsrWifiRouterCtrlBlockAckRole) 0x00)
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT    ((CsrWifiRouterCtrlBlockAckRole) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlControlIndication
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_CONTROL_INDICATION_ERROR
+                   -
+    CSR_WIFI_ROUTER_CTRL_CONTROL_INDICATION_EXIT
+                   -
+    CSR_WIFI_ROUTER_CTRL_CONTROL_INDICATION_USER_REQUESTED
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlControlIndication;
+#define CSR_WIFI_ROUTER_CTRL_CONTROL_INDICATION_ERROR            ((CsrWifiRouterCtrlControlIndication) 0x01)
+#define CSR_WIFI_ROUTER_CTRL_CONTROL_INDICATION_EXIT             ((CsrWifiRouterCtrlControlIndication) 0x02)
+#define CSR_WIFI_ROUTER_CTRL_CONTROL_INDICATION_USER_REQUESTED   ((CsrWifiRouterCtrlControlIndication) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlListAction
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_LIST_ACTION_GET
+                   -
+    CSR_WIFI_ROUTER_CTRL_LIST_ACTION_ADD
+                   -
+    CSR_WIFI_ROUTER_CTRL_LIST_ACTION_REMOVE
+                   -
+    CSR_WIFI_ROUTER_CTRL_LIST_ACTION_FLUSH
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlListAction;
+#define CSR_WIFI_ROUTER_CTRL_LIST_ACTION_GET      ((CsrWifiRouterCtrlListAction) 0x00)
+#define CSR_WIFI_ROUTER_CTRL_LIST_ACTION_ADD      ((CsrWifiRouterCtrlListAction) 0x01)
+#define CSR_WIFI_ROUTER_CTRL_LIST_ACTION_REMOVE   ((CsrWifiRouterCtrlListAction) 0x02)
+#define CSR_WIFI_ROUTER_CTRL_LIST_ACTION_FLUSH    ((CsrWifiRouterCtrlListAction) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlLowPowerMode
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_LOW_POWER_MODE_DISABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_LOW_POWER_MODE_ENABLED
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlLowPowerMode;
+#define CSR_WIFI_ROUTER_CTRL_LOW_POWER_MODE_DISABLED   ((CsrWifiRouterCtrlLowPowerMode) 0x0000)
+#define CSR_WIFI_ROUTER_CTRL_LOW_POWER_MODE_ENABLED    ((CsrWifiRouterCtrlLowPowerMode) 0x0001)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMediaStatus
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_CONNECTED
+                   -
+    CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_DISCONNECTED
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlMediaStatus;
+#define CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_CONNECTED      ((CsrWifiRouterCtrlMediaStatus) 0x00)
+#define CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_DISCONNECTED   ((CsrWifiRouterCtrlMediaStatus) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMode
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_MODE_NONE    -
+    CSR_WIFI_ROUTER_CTRL_MODE_IBSS    -
+    CSR_WIFI_ROUTER_CTRL_MODE_STA     -
+    CSR_WIFI_ROUTER_CTRL_MODE_AP      -
+    CSR_WIFI_ROUTER_CTRL_MODE_MONITOR -
+    CSR_WIFI_ROUTER_CTRL_MODE_AMP     -
+    CSR_WIFI_ROUTER_CTRL_MODE_P2P     -
+    CSR_WIFI_ROUTER_CTRL_MODE_P2PGO   -
+    CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI  -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlMode;
+#define CSR_WIFI_ROUTER_CTRL_MODE_NONE      ((CsrWifiRouterCtrlMode) 0x00)
+#define CSR_WIFI_ROUTER_CTRL_MODE_IBSS      ((CsrWifiRouterCtrlMode) 0x01)
+#define CSR_WIFI_ROUTER_CTRL_MODE_STA       ((CsrWifiRouterCtrlMode) 0x02)
+#define CSR_WIFI_ROUTER_CTRL_MODE_AP        ((CsrWifiRouterCtrlMode) 0x03)
+#define CSR_WIFI_ROUTER_CTRL_MODE_MONITOR   ((CsrWifiRouterCtrlMode) 0x04)
+#define CSR_WIFI_ROUTER_CTRL_MODE_AMP       ((CsrWifiRouterCtrlMode) 0x05)
+#define CSR_WIFI_ROUTER_CTRL_MODE_P2P       ((CsrWifiRouterCtrlMode) 0x06)
+#define CSR_WIFI_ROUTER_CTRL_MODE_P2PGO     ((CsrWifiRouterCtrlMode) 0x07)
+#define CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI    ((CsrWifiRouterCtrlMode) 0x08)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerStatus
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE
+                   -
+    CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE
+                   -
+    CSR_WIFI_ROUTER_CTRL_PEER_DISCONNECTED
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlPeerStatus;
+#define CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE       ((CsrWifiRouterCtrlPeerStatus) 0x00)
+#define CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE   ((CsrWifiRouterCtrlPeerStatus) 0x01)
+#define CSR_WIFI_ROUTER_CTRL_PEER_DISCONNECTED           ((CsrWifiRouterCtrlPeerStatus) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPortAction
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN
+                   -
+    CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD
+                   -
+    CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_BLOCK
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlPortAction;
+#define CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN             ((CsrWifiRouterCtrlPortAction) 0x0000)
+#define CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD   ((CsrWifiRouterCtrlPortAction) 0x0001)
+#define CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_BLOCK     ((CsrWifiRouterCtrlPortAction) 0x0002)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPowersaveType
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_AC_BK_PS_INFO_PRESENT
+                   - If set, AC BK PS info is present in b4 and b5
+    CSR_WIFI_ROUTER_CTRL_AC_BE_PS_INFO_PRESENT
+                   - If set, AC BE PS info is present in b6 and b7
+    CSR_WIFI_ROUTER_CTRL_AC_VI_PS_INFO_PRESENT
+                   - If set, AC VI PS info is present in b8 and b9
+    CSR_WIFI_ROUTER_CTRL_AC_VO_PS_INFO_PRESENT
+                   - If set, AC VO PS info is present in b10 and b11
+    CSR_WIFI_ROUTER_CTRL_AC_BK_TRIGGER_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_BK_DELIVERY_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_BE_TRIGGER_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_BE_DELIVERY_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_VI_TRIGGER_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_VI_DELIVERY_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_VO_TRIGGER_ENABLED
+                   -
+    CSR_WIFI_ROUTER_CTRL_AC_VO_DELIVERY_ENABLED
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlPowersaveType;
+#define CSR_WIFI_ROUTER_CTRL_AC_BK_PS_INFO_PRESENT    ((CsrWifiRouterCtrlPowersaveType) 0x0001)
+#define CSR_WIFI_ROUTER_CTRL_AC_BE_PS_INFO_PRESENT    ((CsrWifiRouterCtrlPowersaveType) 0x0002)
+#define CSR_WIFI_ROUTER_CTRL_AC_VI_PS_INFO_PRESENT    ((CsrWifiRouterCtrlPowersaveType) 0x0004)
+#define CSR_WIFI_ROUTER_CTRL_AC_VO_PS_INFO_PRESENT    ((CsrWifiRouterCtrlPowersaveType) 0x0008)
+#define CSR_WIFI_ROUTER_CTRL_AC_BK_TRIGGER_ENABLED    ((CsrWifiRouterCtrlPowersaveType) 0x0010)
+#define CSR_WIFI_ROUTER_CTRL_AC_BK_DELIVERY_ENABLED   ((CsrWifiRouterCtrlPowersaveType) 0x0020)
+#define CSR_WIFI_ROUTER_CTRL_AC_BE_TRIGGER_ENABLED    ((CsrWifiRouterCtrlPowersaveType) 0x0040)
+#define CSR_WIFI_ROUTER_CTRL_AC_BE_DELIVERY_ENABLED   ((CsrWifiRouterCtrlPowersaveType) 0x0080)
+#define CSR_WIFI_ROUTER_CTRL_AC_VI_TRIGGER_ENABLED    ((CsrWifiRouterCtrlPowersaveType) 0x0100)
+#define CSR_WIFI_ROUTER_CTRL_AC_VI_DELIVERY_ENABLED   ((CsrWifiRouterCtrlPowersaveType) 0x0200)
+#define CSR_WIFI_ROUTER_CTRL_AC_VO_TRIGGER_ENABLED    ((CsrWifiRouterCtrlPowersaveType) 0x0400)
+#define CSR_WIFI_ROUTER_CTRL_AC_VO_DELIVERY_ENABLED   ((CsrWifiRouterCtrlPowersaveType) 0x0800)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlProtocolDirection
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_RX
+                   -
+    CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlProtocolDirection;
+#define CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_RX   ((CsrWifiRouterCtrlProtocolDirection) 0x0000)
+#define CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX   ((CsrWifiRouterCtrlProtocolDirection) 0x0001)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlQoSControl
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_OFF
+                   -
+    CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_WMM_ON
+                   -
+    CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_80211_ON
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlQoSControl;
+#define CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_OFF        ((CsrWifiRouterCtrlQoSControl) 0x0000)
+#define CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_WMM_ON     ((CsrWifiRouterCtrlQoSControl) 0x0001)
+#define CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_80211_ON   ((CsrWifiRouterCtrlQoSControl) 0x0002)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlQueueConfig
+
+  DESCRIPTION
+    Defines which Queues are enabled for use.
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_QUEUE_BE_ENABLE
+                   -
+    CSR_WIFI_ROUTER_CTRL_QUEUE_BK_ENABLE
+                   -
+    CSR_WIFI_ROUTER_CTRL_QUEUE_VI_ENABLE
+                   -
+    CSR_WIFI_ROUTER_CTRL_QUEUE_VO_ENABLE
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlQueueConfig;
+#define CSR_WIFI_ROUTER_CTRL_QUEUE_BE_ENABLE   ((CsrWifiRouterCtrlQueueConfig) 0x01)
+#define CSR_WIFI_ROUTER_CTRL_QUEUE_BK_ENABLE   ((CsrWifiRouterCtrlQueueConfig) 0x02)
+#define CSR_WIFI_ROUTER_CTRL_QUEUE_VI_ENABLE   ((CsrWifiRouterCtrlQueueConfig) 0x04)
+#define CSR_WIFI_ROUTER_CTRL_QUEUE_VO_ENABLE   ((CsrWifiRouterCtrlQueueConfig) 0x08)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficConfigType
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_RESET
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_FILTER
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_CLS
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlTrafficConfigType;
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_RESET    ((CsrWifiRouterCtrlTrafficConfigType) 0x0000)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_FILTER   ((CsrWifiRouterCtrlTrafficConfigType) 0x0001)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_CLS      ((CsrWifiRouterCtrlTrafficConfigType) 0x0002)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficPacketType
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_NONE
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_EAPOL
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP_ACK
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_ARP
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_AIRONET
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_ALL
+                   -
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlTrafficPacketType;
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_NONE       ((CsrWifiRouterCtrlTrafficPacketType) 0x0000)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_EAPOL      ((CsrWifiRouterCtrlTrafficPacketType) 0x0001)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP       ((CsrWifiRouterCtrlTrafficPacketType) 0x0002)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_DHCP_ACK   ((CsrWifiRouterCtrlTrafficPacketType) 0x0004)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_ARP        ((CsrWifiRouterCtrlTrafficPacketType) 0x0008)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_AIRONET    ((CsrWifiRouterCtrlTrafficPacketType) 0x0010)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM     ((CsrWifiRouterCtrlTrafficPacketType) 0x0020)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_ALL        ((CsrWifiRouterCtrlTrafficPacketType) 0x00FF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficType
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_OCCASIONAL
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_BURSTY
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_PERIODIC
+                   -
+    CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_CONTINUOUS
+                   -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlTrafficType;
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_OCCASIONAL   ((CsrWifiRouterCtrlTrafficType) 0x00)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_BURSTY       ((CsrWifiRouterCtrlTrafficType) 0x01)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_PERIODIC     ((CsrWifiRouterCtrlTrafficType) 0x02)
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_TYPE_CONTINUOUS   ((CsrWifiRouterCtrlTrafficType) 0x03)
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerRecordHandle
+
+  DESCRIPTION
+
+*******************************************************************************/
+typedef u32 CsrWifiRouterCtrlPeerRecordHandle;
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPowersaveTypeMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by
+    CsrWifiRouterCtrlPowersaveType
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlPowersaveTypeMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlQueueConfigMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiRouterCtrlQueueConfig
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlQueueConfigMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRequestorInfo
+
+  DESCRIPTION
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterCtrlRequestorInfo;
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficStreamId
+
+  DESCRIPTION
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterCtrlTrafficStreamId;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlSmeVersions
+
+  DESCRIPTION
+
+  MEMBERS
+    firmwarePatch -
+    smeBuild      -
+    smeHip        -
+
+*******************************************************************************/
+typedef struct
+{
+    u32      firmwarePatch;
+    char *smeBuild;
+    u32      smeHip;
+} CsrWifiRouterCtrlSmeVersions;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlStaInfo
+
+  DESCRIPTION
+
+  MEMBERS
+    wmmOrQosEnabled     -
+    powersaveMode       -
+    maxSpLength         -
+    listenIntervalInTus -
+
+*******************************************************************************/
+typedef struct
+{
+    u8                            wmmOrQosEnabled;
+    CsrWifiRouterCtrlPowersaveTypeMask powersaveMode;
+    u8                           maxSpLength;
+    u16                          listenIntervalInTus;
+} CsrWifiRouterCtrlStaInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficFilter
+
+  DESCRIPTION
+
+  MEMBERS
+    etherType     -
+    ipType        -
+    udpSourcePort -
+    udpDestPort   -
+
+*******************************************************************************/
+typedef struct
+{
+    u32 etherType;
+    u8  ipType;
+    u32 udpSourcePort;
+    u32 udpDestPort;
+} CsrWifiRouterCtrlTrafficFilter;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficStats
+
+  DESCRIPTION
+
+  MEMBERS
+    rxMeanRate   - Mean rx data rate over the interval
+    rxFramesNum  - Keep number of Rx frames per second, for CYCLE_3.
+    txFramesNum  - Keep number of Tx frames per second, for CYCLE_3.
+    rxBytesCount - Keep calculated Rx throughput per second, for CYCLE_2.
+    txBytesCount - Keep calculated Tx throughput per second, for CYCLE_2.
+    intervals    - array size 11 MUST match TA_INTERVALS_NUM
+
+*******************************************************************************/
+typedef struct
+{
+    u32 rxMeanRate;
+    u32 rxFramesNum;
+    u32 txFramesNum;
+    u32 rxBytesCount;
+    u32 txBytesCount;
+    u8  intervals[11];
+} CsrWifiRouterCtrlTrafficStats;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlVersions
+
+  DESCRIPTION
+
+  MEMBERS
+    chipId        -
+    chipVersion   -
+    firmwareBuild -
+    firmwareHip   -
+    routerBuild   -
+    routerHip     -
+
+*******************************************************************************/
+typedef struct
+{
+    u32      chipId;
+    u32      chipVersion;
+    u32      firmwareBuild;
+    u32      firmwareHip;
+    char *routerBuild;
+    u32      routerHip;
+} CsrWifiRouterCtrlVersions;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficConfig
+
+  DESCRIPTION
+
+  MEMBERS
+    packetFilter -
+    customFilter -
+
+*******************************************************************************/
+typedef struct
+{
+    u16                      packetFilter;
+    CsrWifiRouterCtrlTrafficFilter customFilter;
+} CsrWifiRouterCtrlTrafficConfig;
+
+
+/* Downstream */
+#define CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST            (0x0000)
+
+#define CSR_WIFI_ROUTER_CTRL_CONFIGURE_POWER_MODE_REQ     ((CsrWifiRouterCtrlPrim) (0x0000 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_HIP_REQ                      ((CsrWifiRouterCtrlPrim) (0x0001 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_MEDIA_STATUS_REQ             ((CsrWifiRouterCtrlPrim) (0x0002 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_RES        ((CsrWifiRouterCtrlPrim) (0x0003 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PORT_CONFIGURE_REQ           ((CsrWifiRouterCtrlPrim) (0x0004 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_REQ              ((CsrWifiRouterCtrlPrim) (0x0005 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_SUSPEND_RES                  ((CsrWifiRouterCtrlPrim) (0x0006 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_REQ                ((CsrWifiRouterCtrlPrim) (0x0007 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_RESUME_RES                   ((CsrWifiRouterCtrlPrim) (0x0008 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_RAW_SDIO_DEINITIALISE_REQ    ((CsrWifiRouterCtrlPrim) (0x0009 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_RAW_SDIO_INITIALISE_REQ      ((CsrWifiRouterCtrlPrim) (0x000A + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_REQ                ((CsrWifiRouterCtrlPrim) (0x000B + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_CLASSIFICATION_REQ   ((CsrWifiRouterCtrlPrim) (0x000C + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_REQ           ((CsrWifiRouterCtrlPrim) (0x000D + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_OFF_REQ                 ((CsrWifiRouterCtrlPrim) (0x000E + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_OFF_RES                 ((CsrWifiRouterCtrlPrim) (0x000F + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_ON_REQ                  ((CsrWifiRouterCtrlPrim) (0x0010 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_ON_RES                  ((CsrWifiRouterCtrlPrim) (0x0011 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_M4_TRANSMIT_REQ              ((CsrWifiRouterCtrlPrim) (0x0012 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_MODE_SET_REQ                 ((CsrWifiRouterCtrlPrim) (0x0013 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PEER_ADD_REQ                 ((CsrWifiRouterCtrlPrim) (0x0014 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PEER_DEL_REQ                 ((CsrWifiRouterCtrlPrim) (0x0015 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PEER_UPDATE_REQ              ((CsrWifiRouterCtrlPrim) (0x0016 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_CAPABILITIES_REQ             ((CsrWifiRouterCtrlPrim) (0x0017 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ENABLE_REQ         ((CsrWifiRouterCtrlPrim) (0x0018 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_DISABLE_REQ        ((CsrWifiRouterCtrlPrim) (0x0019 + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_RX_PKT_REQ              ((CsrWifiRouterCtrlPrim) (0x001A + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_MULTICAST_FILTER_REQ    ((CsrWifiRouterCtrlPrim) (0x001B + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_FILTER_REQ      ((CsrWifiRouterCtrlPrim) (0x001C + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_PKT_REQ      ((CsrWifiRouterCtrlPrim) (0x001D + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_FILTER_REQ              ((CsrWifiRouterCtrlPrim) (0x001E + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST))
+
+
+#define CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_HIGHEST           (0x001E + CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST)
+
+/* Upstream */
+#define CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST              (0x0000 + CSR_PRIM_UPSTREAM)
+
+#define CSR_WIFI_ROUTER_CTRL_HIP_IND                      ((CsrWifiRouterCtrlPrim)(0x0000 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_MULTICAST_ADDRESS_IND        ((CsrWifiRouterCtrlPrim)(0x0001 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PORT_CONFIGURE_CFM           ((CsrWifiRouterCtrlPrim)(0x0002 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_RESUME_IND                   ((CsrWifiRouterCtrlPrim)(0x0003 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_SUSPEND_IND                  ((CsrWifiRouterCtrlPrim)(0x0004 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TCLAS_ADD_CFM                ((CsrWifiRouterCtrlPrim)(0x0005 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_RAW_SDIO_DEINITIALISE_CFM    ((CsrWifiRouterCtrlPrim)(0x0006 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_RAW_SDIO_INITIALISE_CFM      ((CsrWifiRouterCtrlPrim)(0x0007 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TCLAS_DEL_CFM                ((CsrWifiRouterCtrlPrim)(0x0008 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_PROTOCOL_IND         ((CsrWifiRouterCtrlPrim)(0x0009 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_TRAFFIC_SAMPLE_IND           ((CsrWifiRouterCtrlPrim)(0x000A + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_OFF_IND                 ((CsrWifiRouterCtrlPrim)(0x000B + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_OFF_CFM                 ((CsrWifiRouterCtrlPrim)(0x000C + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_ON_IND                  ((CsrWifiRouterCtrlPrim)(0x000D + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WIFI_ON_CFM                  ((CsrWifiRouterCtrlPrim)(0x000E + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_M4_READY_TO_SEND_IND         ((CsrWifiRouterCtrlPrim)(0x000F + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_M4_TRANSMITTED_IND           ((CsrWifiRouterCtrlPrim)(0x0010 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_MIC_FAILURE_IND              ((CsrWifiRouterCtrlPrim)(0x0011 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_CONNECTED_IND                ((CsrWifiRouterCtrlPrim)(0x0012 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PEER_ADD_CFM                 ((CsrWifiRouterCtrlPrim)(0x0013 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PEER_DEL_CFM                 ((CsrWifiRouterCtrlPrim)(0x0014 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_UNEXPECTED_FRAME_IND         ((CsrWifiRouterCtrlPrim)(0x0015 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_PEER_UPDATE_CFM              ((CsrWifiRouterCtrlPrim)(0x0016 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_CAPABILITIES_CFM             ((CsrWifiRouterCtrlPrim)(0x0017 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ENABLE_CFM         ((CsrWifiRouterCtrlPrim)(0x0018 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_DISABLE_CFM        ((CsrWifiRouterCtrlPrim)(0x0019 + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ERROR_IND          ((CsrWifiRouterCtrlPrim)(0x001A + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_STA_INACTIVE_IND             ((CsrWifiRouterCtrlPrim)(0x001B + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_RX_MIC_CHECK_IND        ((CsrWifiRouterCtrlPrim)(0x001C + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_MODE_SET_CFM                 ((CsrWifiRouterCtrlPrim)(0x001D + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_CTRL_WAPI_UNICAST_TX_ENCRYPT_IND  ((CsrWifiRouterCtrlPrim)(0x001E + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST))
+
+#define CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_HIGHEST             (0x001E + CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST)
+
+#define CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT             (CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_HIGHEST + 1 - CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST)
+#define CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_COUNT               (CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_HIGHEST   + 1 - CSR_WIFI_ROUTER_CTRL_PRIM_UPSTREAM_LOWEST)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlConfigurePowerModeReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    mode       -
+    wakeHost   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrWifiRouterCtrlLowPowerMode  mode;
+    u8                        wakeHost;
+} CsrWifiRouterCtrlConfigurePowerModeReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlHipReq
+
+  DESCRIPTION
+    This primitive is used for transferring MLME messages to the HIP.
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    mlmeCommandLength - Length of the MLME signal
+    mlmeCommand       - Pointer to the MLME signal
+    dataRef1Length    - Length of the dataRef1 bulk data
+    dataRef1          - Pointer to the bulk data 1
+    dataRef2Length    - Length of the dataRef2 bulk data
+    dataRef2          - Pointer to the bulk data 2
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       mlmeCommandLength;
+    u8       *mlmeCommand;
+    u16       dataRef1Length;
+    u8       *dataRef1;
+    u16       dataRef2Length;
+    u8       *dataRef2;
+} CsrWifiRouterCtrlHipReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMediaStatusReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    clientData   -
+    mediaStatus  -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrWifiRouterCtrlMediaStatus   mediaStatus;
+} CsrWifiRouterCtrlMediaStatusReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMulticastAddressRes
+
+  DESCRIPTION
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    interfaceTag      -
+    clientData        -
+    status            -
+    action            -
+    getAddressesCount -
+    getAddresses      -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      status;
+    CsrWifiRouterCtrlListAction    action;
+    u8                       getAddressesCount;
+    CsrWifiMacAddress             *getAddresses;
+} CsrWifiRouterCtrlMulticastAddressRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPortConfigureReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common                 - Common header for use with the CsrWifiFsm Module
+    interfaceTag           -
+    clientData             -
+    uncontrolledPortAction -
+    controlledPortAction   -
+    macAddress             -
+    setProtection          -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrWifiRouterCtrlPortAction    uncontrolledPortAction;
+    CsrWifiRouterCtrlPortAction    controlledPortAction;
+    CsrWifiMacAddress              macAddress;
+    u8                        setProtection;
+} CsrWifiRouterCtrlPortConfigureReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlQosControlReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    clientData   -
+    control      -
+    queueConfig  -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                  common;
+    u16                        interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo   clientData;
+    CsrWifiRouterCtrlQoSControl      control;
+    CsrWifiRouterCtrlQueueConfigMask queueConfig;
+} CsrWifiRouterCtrlQosControlReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlSuspendRes
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    status     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      status;
+} CsrWifiRouterCtrlSuspendRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasAddReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    clientData   -
+    tclasLength  -
+    tclas        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      tclasLength;
+    u8                      *tclas;
+} CsrWifiRouterCtrlTclasAddReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlResumeRes
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    status     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      status;
+} CsrWifiRouterCtrlResumeRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioDeinitialiseReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlRawSdioDeinitialiseReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioInitialiseReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlRawSdioInitialiseReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasDelReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    clientData   -
+    tclasLength  -
+    tclas        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      tclasLength;
+    u8                      *tclas;
+} CsrWifiRouterCtrlTclasDelReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficClassificationReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    clientData   -
+    trafficType  -
+    period       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrWifiRouterCtrlTrafficType   trafficType;
+    u16                      period;
+} CsrWifiRouterCtrlTrafficClassificationReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficConfigReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    interfaceTag      -
+    clientData        -
+    trafficConfigType -
+    config            -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                    common;
+    u16                          interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo     clientData;
+    CsrWifiRouterCtrlTrafficConfigType trafficConfigType;
+    CsrWifiRouterCtrlTrafficConfig     config;
+} CsrWifiRouterCtrlTrafficConfigReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlWifiOffReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffRes
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlWifiOffRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    dataLength - Number of bytes in the buffer pointed to by 'data'
+    data       - Pointer to the buffer containing 'dataLength' bytes
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u32                      dataLength;
+    u8                      *data;
+} CsrWifiRouterCtrlWifiOnReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnRes
+
+  DESCRIPTION
+
+  MEMBERS
+    common              - Common header for use with the CsrWifiFsm Module
+    clientData          -
+    status              -
+    numInterfaceAddress -
+    stationMacAddress   - array size 1 MUST match CSR_WIFI_NUM_INTERFACES
+    smeVersions         -
+    scheduledInterrupt  -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      status;
+    u16                      numInterfaceAddress;
+    CsrWifiMacAddress              stationMacAddress[2];
+    CsrWifiRouterCtrlSmeVersions   smeVersions;
+    u8                        scheduledInterrupt;
+} CsrWifiRouterCtrlWifiOnRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlM4TransmitReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    clientData   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlM4TransmitReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlModeSetReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common              - Common header for use with the CsrWifiFsm Module
+    interfaceTag        -
+    clientData          -
+    mode                -
+    bssid               - BSSID of the network the device is going to be a part
+                          of
+    protection          - Set to TRUE if encryption is enabled for the
+                          connection/broadcast frames
+    intraBssDistEnabled - If set to TRUE, intra BSS destribution will be
+                          enabled. If set to FALSE, any unicast PDU which does
+                          not have the RA as the the local MAC address, shall be
+                          ignored. This field is interpreted by the receive if
+                          mode is set to CSR_WIFI_ROUTER_CTRL_MODE_P2PGO
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrWifiRouterCtrlMode          mode;
+    CsrWifiMacAddress              bssid;
+    u8                        protection;
+    u8                        intraBssDistEnabled;
+} CsrWifiRouterCtrlModeSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerAddReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   -
+    clientData     -
+    peerMacAddress -
+    associationId  -
+    staInfo        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrWifiMacAddress              peerMacAddress;
+    u16                      associationId;
+    CsrWifiRouterCtrlStaInfo       staInfo;
+} CsrWifiRouterCtrlPeerAddReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerDelReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     -
+    clientData       -
+    peerRecordHandle -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                   common;
+    u16                         interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo    clientData;
+    CsrWifiRouterCtrlPeerRecordHandle peerRecordHandle;
+} CsrWifiRouterCtrlPeerDelReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerUpdateReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     -
+    clientData       -
+    peerRecordHandle -
+    powersaveMode    -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                    common;
+    u16                          interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo     clientData;
+    CsrWifiRouterCtrlPeerRecordHandle  peerRecordHandle;
+    CsrWifiRouterCtrlPowersaveTypeMask powersaveMode;
+} CsrWifiRouterCtrlPeerUpdateReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlCapabilitiesReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlCapabilitiesReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckEnableReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    -
+    clientData      -
+    macAddress      -
+    trafficStreamID -
+    role            -
+    bufferSize      -
+    timeout         -
+    ssn             -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                  common;
+    u16                        interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo   clientData;
+    CsrWifiMacAddress                macAddress;
+    CsrWifiRouterCtrlTrafficStreamId trafficStreamID;
+    CsrWifiRouterCtrlBlockAckRole    role;
+    u16                        bufferSize;
+    u16                        timeout;
+    u16                        ssn;
+} CsrWifiRouterCtrlBlockAckEnableReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckDisableReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    -
+    clientData      -
+    macAddress      -
+    trafficStreamID -
+    role            -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                  common;
+    u16                        interfaceTag;
+    CsrWifiRouterCtrlRequestorInfo   clientData;
+    CsrWifiMacAddress                macAddress;
+    CsrWifiRouterCtrlTrafficStreamId trafficStreamID;
+    CsrWifiRouterCtrlBlockAckRole    role;
+} CsrWifiRouterCtrlBlockAckDisableReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiRxPktReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    signalLength -
+    signal       -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u16       signalLength;
+    u8       *signal;
+    u16       dataLength;
+    u8       *data;
+} CsrWifiRouterCtrlWapiRxPktReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiMulticastFilterReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u8        status;
+} CsrWifiRouterCtrlWapiMulticastFilterReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiUnicastFilterReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u8        status;
+} CsrWifiRouterCtrlWapiUnicastFilterReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiUnicastTxPktReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u16       dataLength;
+    u8       *data;
+} CsrWifiRouterCtrlWapiUnicastTxPktReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiFilterReq
+
+  DESCRIPTION
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    -
+    isWapiConnected -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u8         isWapiConnected;
+} CsrWifiRouterCtrlWapiFilterReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlHipInd
+
+  DESCRIPTION
+    This primitive is used for transferring MLME messages from the HIP.
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    mlmeCommandLength - Length of the MLME signal
+    mlmeCommand       - Pointer to the MLME signal
+    dataRef1Length    - Length of the dataRef1 bulk data
+    dataRef1          - Pointer to the bulk data 1
+    dataRef2Length    - Length of the dataRef2 bulk data
+    dataRef2          - Pointer to the bulk data 2
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       mlmeCommandLength;
+    u8       *mlmeCommand;
+    u16       dataRef1Length;
+    u8       *dataRef1;
+    u16       dataRef2Length;
+    u8       *dataRef2;
+} CsrWifiRouterCtrlHipInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMulticastAddressInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    clientData        -
+    interfaceTag      -
+    action            -
+    setAddressesCount -
+    setAddresses      -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlListAction    action;
+    u8                       setAddressesCount;
+    CsrWifiMacAddress             *setAddresses;
+} CsrWifiRouterCtrlMulticastAddressInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPortConfigureCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+    macAddress   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+    CsrWifiMacAddress              macAddress;
+} CsrWifiRouterCtrlPortConfigureCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlResumeInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    clientData      -
+    powerMaintained -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u8                        powerMaintained;
+} CsrWifiRouterCtrlResumeInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlSuspendInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    clientData  -
+    hardSuspend -
+    d3Suspend   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u8                        hardSuspend;
+    u8                        d3Suspend;
+} CsrWifiRouterCtrlSuspendInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasAddCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+} CsrWifiRouterCtrlTclasAddCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioDeinitialiseCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    result     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      result;
+} CsrWifiRouterCtrlRawSdioDeinitialiseCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlRawSdioInitialiseCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    clientData       -
+    result           -
+    byteRead         -
+    byteWrite        -
+    firmwareDownload -
+    reset            -
+    coreDumpPrepare  -
+    byteBlockRead    -
+    gpRead16         -
+    gpWrite16        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                          common;
+    CsrWifiRouterCtrlRequestorInfo           clientData;
+    CsrResult                                result;
+    CsrWifiRouterCtrlRawSdioByteRead         byteRead;
+    CsrWifiRouterCtrlRawSdioByteWrite        byteWrite;
+    CsrWifiRouterCtrlRawSdioFirmwareDownload firmwareDownload;
+    CsrWifiRouterCtrlRawSdioReset            reset;
+    CsrWifiRouterCtrlRawSdioCoreDumpPrepare  coreDumpPrepare;
+    CsrWifiRouterCtrlRawSdioByteBlockRead    byteBlockRead;
+    CsrWifiRouterCtrlRawSdioGpRead16         gpRead16;
+    CsrWifiRouterCtrlRawSdioGpWrite16        gpWrite16;
+} CsrWifiRouterCtrlRawSdioInitialiseCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTclasDelCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+} CsrWifiRouterCtrlTclasDelCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficProtocolInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    packetType   -
+    direction    -
+    srcAddress   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                    common;
+    CsrWifiRouterCtrlRequestorInfo     clientData;
+    u16                          interfaceTag;
+    CsrWifiRouterCtrlTrafficPacketType packetType;
+    CsrWifiRouterCtrlProtocolDirection direction;
+    CsrWifiMacAddress                  srcAddress;
+} CsrWifiRouterCtrlTrafficProtocolInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlTrafficSampleInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    stats        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlTrafficStats  stats;
+} CsrWifiRouterCtrlTrafficSampleInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    clientData        -
+    controlIndication -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                    common;
+    CsrWifiRouterCtrlRequestorInfo     clientData;
+    CsrWifiRouterCtrlControlIndication controlIndication;
+} CsrWifiRouterCtrlWifiOffInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOffCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+} CsrWifiRouterCtrlWifiOffCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    status     -
+    versions   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      status;
+    CsrWifiRouterCtrlVersions      versions;
+} CsrWifiRouterCtrlWifiOnInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWifiOnCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    clientData -
+    status     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    CsrResult                      status;
+} CsrWifiRouterCtrlWifiOnCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlM4ReadyToSendInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiMacAddress              peerMacAddress;
+} CsrWifiRouterCtrlM4ReadyToSendInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlM4TransmittedInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+    status         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiMacAddress              peerMacAddress;
+    CsrResult                      status;
+} CsrWifiRouterCtrlM4TransmittedInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlMicFailureInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+    unicastPdu     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiMacAddress              peerMacAddress;
+    u8                        unicastPdu;
+} CsrWifiRouterCtrlMicFailureInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlConnectedInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+    peerStatus     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiMacAddress              peerMacAddress;
+    CsrWifiRouterCtrlPeerStatus    peerStatus;
+} CsrWifiRouterCtrlConnectedInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerAddCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    clientData       -
+    interfaceTag     -
+    peerMacAddress   -
+    peerRecordHandle -
+    status           -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                   common;
+    CsrWifiRouterCtrlRequestorInfo    clientData;
+    u16                         interfaceTag;
+    CsrWifiMacAddress                 peerMacAddress;
+    CsrWifiRouterCtrlPeerRecordHandle peerRecordHandle;
+    CsrResult                         status;
+} CsrWifiRouterCtrlPeerAddCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerDelCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+} CsrWifiRouterCtrlPeerDelCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlUnexpectedFrameInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    clientData     -
+    interfaceTag   -
+    peerMacAddress -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiMacAddress              peerMacAddress;
+} CsrWifiRouterCtrlUnexpectedFrameInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlPeerUpdateCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+} CsrWifiRouterCtrlPeerUpdateCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlCapabilitiesCfm
+
+  DESCRIPTION
+    The router sends this primitive to confirm the size of the queues of the
+    HIP.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    clientData       -
+    commandQueueSize - Size of command queue
+    trafficQueueSize - Size of traffic queue (per AC)
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      commandQueueSize;
+    u16                      trafficQueueSize;
+} CsrWifiRouterCtrlCapabilitiesCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckEnableCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+} CsrWifiRouterCtrlBlockAckEnableCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckDisableCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrResult                      status;
+} CsrWifiRouterCtrlBlockAckDisableCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlBlockAckErrorInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    clientData      -
+    interfaceTag    -
+    trafficStreamID -
+    peerMacAddress  -
+    status          -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                  common;
+    CsrWifiRouterCtrlRequestorInfo   clientData;
+    u16                        interfaceTag;
+    CsrWifiRouterCtrlTrafficStreamId trafficStreamID;
+    CsrWifiMacAddress                peerMacAddress;
+    CsrResult                        status;
+} CsrWifiRouterCtrlBlockAckErrorInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlStaInactiveInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    staAddress   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiMacAddress              staAddress;
+} CsrWifiRouterCtrlStaInactiveInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiRxMicCheckInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    signalLength -
+    signal       -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    u16                      signalLength;
+    u8                      *signal;
+    u16                      dataLength;
+    u8                      *data;
+} CsrWifiRouterCtrlWapiRxMicCheckInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlModeSetCfm
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    mode         -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    CsrWifiRouterCtrlMode          mode;
+    CsrResult                      status;
+} CsrWifiRouterCtrlModeSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterCtrlWapiUnicastTxEncryptInd
+
+  DESCRIPTION
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    clientData   -
+    interfaceTag -
+    dataLength   -
+    data         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrWifiRouterCtrlRequestorInfo clientData;
+    u16                      interfaceTag;
+    u16                      dataLength;
+    u8                      *data;
+} CsrWifiRouterCtrlWapiUnicastTxEncryptInd;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_CTRL_PRIM_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_sef.c b/drivers/staging/csr/csr_wifi_router_ctrl_sef.c
new file mode 100644
index 00000000000..33d92b698c5
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_sef.c
@@ -0,0 +1,45 @@
+/*****************************************************************************
+
+  (c) Cambridge Silicon Radio Limited 2010
+  Confidential information of CSR
+
+  Refer to LICENSE.txt included with this source for details
+  on the license terms.
+
+ *****************************************************************************/
+#include "csr_wifi_router_ctrl_sef.h"
+
+const CsrWifiRouterCtrlStateHandlerType CsrWifiRouterCtrlDownstreamStateHandlers[CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT] =
+{
+    /* 0x0000 */ CsrWifiRouterCtrlConfigurePowerModeReqHandler,
+    /* 0x0001 */ CsrWifiRouterCtrlHipReqHandler,
+    /* 0x0002 */ CsrWifiRouterCtrlMediaStatusReqHandler,
+    /* 0x0003 */ CsrWifiRouterCtrlMulticastAddressResHandler,
+    /* 0x0004 */ CsrWifiRouterCtrlPortConfigureReqHandler,
+    /* 0x0005 */ CsrWifiRouterCtrlQosControlReqHandler,
+    /* 0x0006 */ CsrWifiRouterCtrlSuspendResHandler,
+    /* 0x0007 */ CsrWifiRouterCtrlTclasAddReqHandler,
+    /* 0x0008 */ CsrWifiRouterCtrlResumeResHandler,
+    /* 0x0009 */ CsrWifiRouterCtrlRawSdioDeinitialiseReqHandler,
+    /* 0x000A */ CsrWifiRouterCtrlRawSdioInitialiseReqHandler,
+    /* 0x000B */ CsrWifiRouterCtrlTclasDelReqHandler,
+    /* 0x000C */ CsrWifiRouterCtrlTrafficClassificationReqHandler,
+    /* 0x000D */ CsrWifiRouterCtrlTrafficConfigReqHandler,
+    /* 0x000E */ CsrWifiRouterCtrlWifiOffReqHandler,
+    /* 0x000F */ CsrWifiRouterCtrlWifiOffResHandler,
+    /* 0x0010 */ CsrWifiRouterCtrlWifiOnReqHandler,
+    /* 0x0011 */ CsrWifiRouterCtrlWifiOnResHandler,
+    /* 0x0012 */ CsrWifiRouterCtrlM4TransmitReqHandler,
+    /* 0x0013 */ CsrWifiRouterCtrlModeSetReqHandler,
+    /* 0x0014 */ CsrWifiRouterCtrlPeerAddReqHandler,
+    /* 0x0015 */ CsrWifiRouterCtrlPeerDelReqHandler,
+    /* 0x0016 */ CsrWifiRouterCtrlPeerUpdateReqHandler,
+    /* 0x0017 */ CsrWifiRouterCtrlCapabilitiesReqHandler,
+    /* 0x0018 */ CsrWifiRouterCtrlBlockAckEnableReqHandler,
+    /* 0x0019 */ CsrWifiRouterCtrlBlockAckDisableReqHandler,
+    /* 0x001A */ CsrWifiRouterCtrlWapiRxPktReqHandler,
+    /* 0x001B */ CsrWifiRouterCtrlWapiMulticastFilterReqHandler,
+    /* 0x001C */ CsrWifiRouterCtrlWapiUnicastFilterReqHandler,
+    /* 0x001D */ CsrWifiRouterCtrlWapiUnicastTxPktReqHandler,
+    /* 0x001E */ CsrWifiRouterCtrlWapiFilterReqHandler,
+};
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_sef.h b/drivers/staging/csr/csr_wifi_router_ctrl_sef.h
new file mode 100644
index 00000000000..e0ee5cf45f9
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_sef.h
@@ -0,0 +1,58 @@
+/*****************************************************************************
+
+  (c) Cambridge Silicon Radio Limited 2010
+  Confidential information of CSR
+
+  Refer to LICENSE.txt included with this source for details
+  on the license terms.
+
+ *****************************************************************************/
+#ifndef CSR_WIFI_ROUTER_SEF_CSR_WIFI_ROUTER_CTRL_H__
+#define CSR_WIFI_ROUTER_SEF_CSR_WIFI_ROUTER_CTRL_H__
+
+#include "csr_wifi_router_ctrl_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    typedef void (*CsrWifiRouterCtrlStateHandlerType)(void* drvpriv, CsrWifiFsmEvent* msg);
+
+    extern const CsrWifiRouterCtrlStateHandlerType CsrWifiRouterCtrlDownstreamStateHandlers[CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_COUNT];
+
+    extern void CsrWifiRouterCtrlConfigurePowerModeReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlHipReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlMediaStatusReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlMulticastAddressResHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlPortConfigureReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlQosControlReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlSuspendResHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlTclasAddReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlResumeResHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlRawSdioDeinitialiseReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlRawSdioInitialiseReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlTclasDelReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlTrafficClassificationReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlTrafficConfigReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWifiOffReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWifiOffResHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWifiOnReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWifiOnResHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlM4TransmitReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlModeSetReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlPeerAddReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlPeerDelReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlPeerUpdateReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlCapabilitiesReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlBlockAckEnableReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlBlockAckDisableReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWapiMulticastFilterReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWapiRxPktReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWapiUnicastTxPktReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWapiUnicastFilterReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterCtrlWapiFilterReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_SEF_CSR_WIFI_ROUTER_CTRL_H__ */
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_serialize.c b/drivers/staging/csr/csr_wifi_router_ctrl_serialize.c
new file mode 100644
index 00000000000..3eda1b66b33
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_serialize.c
@@ -0,0 +1,2591 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/string.h>
+#include <linux/slab.h>
+#include "csr_msgconv.h"
+#include "csr_wifi_router_ctrl_prim.h"
+#include "csr_wifi_router_ctrl_serialize.h"
+
+void CsrWifiRouterCtrlPfree(void *ptr)
+{
+    kfree(ptr);
+}
+
+
+size_t CsrWifiRouterCtrlConfigurePowerModeReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrWifiRouterCtrlLowPowerMode primitive->mode */
+    bufferSize += 1; /* u8 primitive->wakeHost */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlConfigurePowerModeReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlConfigurePowerModeReq *primitive = (CsrWifiRouterCtrlConfigurePowerModeReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->mode);
+    CsrUint8Ser(ptr, len, (u8) primitive->wakeHost);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlConfigurePowerModeReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlConfigurePowerModeReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlConfigurePowerModeReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mode, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->wakeHost, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlHipReqSizeof(void *msg)
+{
+    CsrWifiRouterCtrlHipReq *primitive = (CsrWifiRouterCtrlHipReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 12) */
+    bufferSize += 2;                            /* u16 primitive->mlmeCommandLength */
+    bufferSize += primitive->mlmeCommandLength; /* u8 primitive->mlmeCommand */
+    bufferSize += 2;                            /* u16 primitive->dataRef1Length */
+    bufferSize += primitive->dataRef1Length;    /* u8 primitive->dataRef1 */
+    bufferSize += 2;                            /* u16 primitive->dataRef2Length */
+    bufferSize += primitive->dataRef2Length;    /* u8 primitive->dataRef2 */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlHipReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlHipReq *primitive = (CsrWifiRouterCtrlHipReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->mlmeCommandLength);
+    if (primitive->mlmeCommandLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mlmeCommand, ((u16) (primitive->mlmeCommandLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->dataRef1Length);
+    if (primitive->dataRef1Length)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->dataRef1, ((u16) (primitive->dataRef1Length)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->dataRef2Length);
+    if (primitive->dataRef2Length)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->dataRef2, ((u16) (primitive->dataRef2Length)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlHipReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlHipReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlHipReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mlmeCommandLength, buffer, &offset);
+    if (primitive->mlmeCommandLength)
+    {
+        primitive->mlmeCommand = kmalloc(primitive->mlmeCommandLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mlmeCommand, buffer, &offset, ((u16) (primitive->mlmeCommandLength)));
+    }
+    else
+    {
+        primitive->mlmeCommand = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->dataRef1Length, buffer, &offset);
+    if (primitive->dataRef1Length)
+    {
+        primitive->dataRef1 = kmalloc(primitive->dataRef1Length, GFP_KERNEL);
+        CsrMemCpyDes(primitive->dataRef1, buffer, &offset, ((u16) (primitive->dataRef1Length)));
+    }
+    else
+    {
+        primitive->dataRef1 = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->dataRef2Length, buffer, &offset);
+    if (primitive->dataRef2Length)
+    {
+        primitive->dataRef2 = kmalloc(primitive->dataRef2Length, GFP_KERNEL);
+        CsrMemCpyDes(primitive->dataRef2, buffer, &offset, ((u16) (primitive->dataRef2Length)));
+    }
+    else
+    {
+        primitive->dataRef2 = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlHipReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlHipReq *primitive = (CsrWifiRouterCtrlHipReq *) voidPrimitivePointer;
+    kfree(primitive->mlmeCommand);
+    kfree(primitive->dataRef1);
+    kfree(primitive->dataRef2);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlMediaStatusReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 1; /* CsrWifiRouterCtrlMediaStatus primitive->mediaStatus */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlMediaStatusReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlMediaStatusReq *primitive = (CsrWifiRouterCtrlMediaStatusReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint8Ser(ptr, len, (u8) primitive->mediaStatus);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlMediaStatusReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlMediaStatusReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlMediaStatusReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mediaStatus, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlMulticastAddressResSizeof(void *msg)
+{
+    CsrWifiRouterCtrlMulticastAddressRes *primitive = (CsrWifiRouterCtrlMulticastAddressRes *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 17) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiRouterCtrlListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->getAddressesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressesCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->getAddresses[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlMulticastAddressResSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlMulticastAddressRes *primitive = (CsrWifiRouterCtrlMulticastAddressRes *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->getAddressesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressesCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->getAddresses[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlMulticastAddressResDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlMulticastAddressRes *primitive = kmalloc(sizeof(CsrWifiRouterCtrlMulticastAddressRes), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->getAddressesCount, buffer, &offset);
+    primitive->getAddresses = NULL;
+    if (primitive->getAddressesCount)
+    {
+        primitive->getAddresses = kmalloc(sizeof(CsrWifiMacAddress) * primitive->getAddressesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressesCount; i1++)
+        {
+            CsrMemCpyDes(primitive->getAddresses[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlMulticastAddressResSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlMulticastAddressRes *primitive = (CsrWifiRouterCtrlMulticastAddressRes *) voidPrimitivePointer;
+    kfree(primitive->getAddresses);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlPortConfigureReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 18) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrWifiRouterCtrlPortAction primitive->uncontrolledPortAction */
+    bufferSize += 2; /* CsrWifiRouterCtrlPortAction primitive->controlledPortAction */
+    bufferSize += 6; /* u8 primitive->macAddress.a[6] */
+    bufferSize += 1; /* u8 primitive->setProtection */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPortConfigureReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPortConfigureReq *primitive = (CsrWifiRouterCtrlPortConfigureReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->uncontrolledPortAction);
+    CsrUint16Ser(ptr, len, (u16) primitive->controlledPortAction);
+    CsrMemCpySer(ptr, len, (const void *) primitive->macAddress.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->setProtection);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPortConfigureReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPortConfigureReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPortConfigureReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->uncontrolledPortAction, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->controlledPortAction, buffer, &offset);
+    CsrMemCpyDes(primitive->macAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->setProtection, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlQosControlReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrWifiRouterCtrlQoSControl primitive->control */
+    bufferSize += 1; /* CsrWifiRouterCtrlQueueConfigMask primitive->queueConfig */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlQosControlReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlQosControlReq *primitive = (CsrWifiRouterCtrlQosControlReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->control);
+    CsrUint8Ser(ptr, len, (u8) primitive->queueConfig);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlQosControlReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlQosControlReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlQosControlReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->control, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->queueConfig, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlSuspendResSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlSuspendResSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlSuspendRes *primitive = (CsrWifiRouterCtrlSuspendRes *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlSuspendResDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlSuspendRes *primitive = kmalloc(sizeof(CsrWifiRouterCtrlSuspendRes), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTclasAddReqSizeof(void *msg)
+{
+    CsrWifiRouterCtrlTclasAddReq *primitive = (CsrWifiRouterCtrlTclasAddReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 2;                      /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2;                      /* u16 primitive->tclasLength */
+    bufferSize += primitive->tclasLength; /* u8 primitive->tclas */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTclasAddReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTclasAddReq *primitive = (CsrWifiRouterCtrlTclasAddReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->tclasLength);
+    if (primitive->tclasLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->tclas, ((u16) (primitive->tclasLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTclasAddReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTclasAddReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTclasAddReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->tclasLength, buffer, &offset);
+    if (primitive->tclasLength)
+    {
+        primitive->tclas = kmalloc(primitive->tclasLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->tclas, buffer, &offset, ((u16) (primitive->tclasLength)));
+    }
+    else
+    {
+        primitive->tclas = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlTclasAddReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlTclasAddReq *primitive = (CsrWifiRouterCtrlTclasAddReq *) voidPrimitivePointer;
+    kfree(primitive->tclas);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlResumeResSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlResumeResSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlResumeRes *primitive = (CsrWifiRouterCtrlResumeRes *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlResumeResDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlResumeRes *primitive = kmalloc(sizeof(CsrWifiRouterCtrlResumeRes), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTclasDelReqSizeof(void *msg)
+{
+    CsrWifiRouterCtrlTclasDelReq *primitive = (CsrWifiRouterCtrlTclasDelReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 2;                      /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2;                      /* u16 primitive->tclasLength */
+    bufferSize += primitive->tclasLength; /* u8 primitive->tclas */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTclasDelReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTclasDelReq *primitive = (CsrWifiRouterCtrlTclasDelReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->tclasLength);
+    if (primitive->tclasLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->tclas, ((u16) (primitive->tclasLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTclasDelReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTclasDelReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTclasDelReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->tclasLength, buffer, &offset);
+    if (primitive->tclasLength)
+    {
+        primitive->tclas = kmalloc(primitive->tclasLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->tclas, buffer, &offset, ((u16) (primitive->tclasLength)));
+    }
+    else
+    {
+        primitive->tclas = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlTclasDelReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlTclasDelReq *primitive = (CsrWifiRouterCtrlTclasDelReq *) voidPrimitivePointer;
+    kfree(primitive->tclas);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlTrafficClassificationReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 1; /* CsrWifiRouterCtrlTrafficType primitive->trafficType */
+    bufferSize += 2; /* u16 primitive->period */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTrafficClassificationReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTrafficClassificationReq *primitive = (CsrWifiRouterCtrlTrafficClassificationReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint8Ser(ptr, len, (u8) primitive->trafficType);
+    CsrUint16Ser(ptr, len, (u16) primitive->period);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTrafficClassificationReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTrafficClassificationReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTrafficClassificationReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->trafficType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->period, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTrafficConfigReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 24) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrWifiRouterCtrlTrafficConfigType primitive->trafficConfigType */
+    bufferSize += 2; /* u16 primitive->config.packetFilter */
+    bufferSize += 4; /* u32 primitive->config.customFilter.etherType */
+    bufferSize += 1; /* u8 primitive->config.customFilter.ipType */
+    bufferSize += 4; /* u32 primitive->config.customFilter.udpSourcePort */
+    bufferSize += 4; /* u32 primitive->config.customFilter.udpDestPort */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTrafficConfigReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTrafficConfigReq *primitive = (CsrWifiRouterCtrlTrafficConfigReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->trafficConfigType);
+    CsrUint16Ser(ptr, len, (u16) primitive->config.packetFilter);
+    CsrUint32Ser(ptr, len, (u32) primitive->config.customFilter.etherType);
+    CsrUint8Ser(ptr, len, (u8) primitive->config.customFilter.ipType);
+    CsrUint32Ser(ptr, len, (u32) primitive->config.customFilter.udpSourcePort);
+    CsrUint32Ser(ptr, len, (u32) primitive->config.customFilter.udpDestPort);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTrafficConfigReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTrafficConfigReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTrafficConfigReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->trafficConfigType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->config.packetFilter, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->config.customFilter.etherType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->config.customFilter.ipType, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->config.customFilter.udpSourcePort, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->config.customFilter.udpDestPort, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlWifiOnReqSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWifiOnReq *primitive = (CsrWifiRouterCtrlWifiOnReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2;                     /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 4;                     /* u32 primitive->dataLength */
+    bufferSize += primitive->dataLength; /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWifiOnReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWifiOnReq *primitive = (CsrWifiRouterCtrlWifiOnReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint32Ser(ptr, len, (u32) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWifiOnReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWifiOnReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWifiOnReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWifiOnReq *primitive = (CsrWifiRouterCtrlWifiOnReq *) voidPrimitivePointer;
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlWifiOnResSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWifiOnRes *primitive = (CsrWifiRouterCtrlWifiOnRes *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 30) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 2; /* u16 primitive->numInterfaceAddress */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 2; i1++)
+        {
+            bufferSize += 6;                                                                            /* u8 primitive->stationMacAddress[i1].a[6] */
+        }
+    }
+    bufferSize += 4;                                                                                    /* u32 primitive->smeVersions.firmwarePatch */
+    bufferSize += (primitive->smeVersions.smeBuild ? strlen(primitive->smeVersions.smeBuild) : 0) + 1;  /* char* primitive->smeVersions.smeBuild (0 byte len + 1 for NULL Term) */
+    bufferSize += 4;                                                                                    /* u32 primitive->smeVersions.smeHip */
+    bufferSize += 1;                                                                                    /* u8 primitive->scheduledInterrupt */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWifiOnResSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWifiOnRes *primitive = (CsrWifiRouterCtrlWifiOnRes *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->numInterfaceAddress);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 2; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->stationMacAddress[i1].a, ((u16) (6)));
+        }
+    }
+    CsrUint32Ser(ptr, len, (u32) primitive->smeVersions.firmwarePatch);
+    CsrCharStringSer(ptr, len, primitive->smeVersions.smeBuild);
+    CsrUint32Ser(ptr, len, (u32) primitive->smeVersions.smeHip);
+    CsrUint8Ser(ptr, len, (u8) primitive->scheduledInterrupt);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWifiOnResDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWifiOnRes *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnRes), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->numInterfaceAddress, buffer, &offset);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 2; i1++)
+        {
+            CsrMemCpyDes(primitive->stationMacAddress[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+    CsrUint32Des((u32 *) &primitive->smeVersions.firmwarePatch, buffer, &offset);
+    CsrCharStringDes(&primitive->smeVersions.smeBuild, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->smeVersions.smeHip, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scheduledInterrupt, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWifiOnResSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWifiOnRes *primitive = (CsrWifiRouterCtrlWifiOnRes *) voidPrimitivePointer;
+    kfree(primitive->smeVersions.smeBuild);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlM4TransmitReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlM4TransmitReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlM4TransmitReq *primitive = (CsrWifiRouterCtrlM4TransmitReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlM4TransmitReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlM4TransmitReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlM4TransmitReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlModeSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 16) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 1; /* CsrWifiRouterCtrlMode primitive->mode */
+    bufferSize += 6; /* u8 primitive->bssid.a[6] */
+    bufferSize += 1; /* u8 primitive->protection */
+    bufferSize += 1; /* u8 primitive->intraBssDistEnabled */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlModeSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlModeSetReq *primitive = (CsrWifiRouterCtrlModeSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint8Ser(ptr, len, (u8) primitive->mode);
+    CsrMemCpySer(ptr, len, (const void *) primitive->bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->protection);
+    CsrUint8Ser(ptr, len, (u8) primitive->intraBssDistEnabled);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlModeSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlModeSetReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlModeSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mode, buffer, &offset);
+    CsrMemCpyDes(primitive->bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->protection, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->intraBssDistEnabled, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlPeerAddReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 21) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 2; /* u16 primitive->associationId */
+    bufferSize += 1; /* u8 primitive->staInfo.wmmOrQosEnabled */
+    bufferSize += 2; /* CsrWifiRouterCtrlPowersaveTypeMask primitive->staInfo.powersaveMode */
+    bufferSize += 1; /* u8 primitive->staInfo.maxSpLength */
+    bufferSize += 2; /* u16 primitive->staInfo.listenIntervalInTus */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPeerAddReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPeerAddReq *primitive = (CsrWifiRouterCtrlPeerAddReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->associationId);
+    CsrUint8Ser(ptr, len, (u8) primitive->staInfo.wmmOrQosEnabled);
+    CsrUint16Ser(ptr, len, (u16) primitive->staInfo.powersaveMode);
+    CsrUint8Ser(ptr, len, (u8) primitive->staInfo.maxSpLength);
+    CsrUint16Ser(ptr, len, (u16) primitive->staInfo.listenIntervalInTus);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPeerAddReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPeerAddReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPeerAddReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->associationId, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->staInfo.wmmOrQosEnabled, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->staInfo.powersaveMode, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->staInfo.maxSpLength, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->staInfo.listenIntervalInTus, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlPeerDelReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 4; /* CsrWifiRouterCtrlPeerRecordHandle primitive->peerRecordHandle */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPeerDelReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPeerDelReq *primitive = (CsrWifiRouterCtrlPeerDelReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint32Ser(ptr, len, (u32) primitive->peerRecordHandle);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPeerDelReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPeerDelReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPeerDelReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->peerRecordHandle, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlPeerUpdateReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 4; /* CsrWifiRouterCtrlPeerRecordHandle primitive->peerRecordHandle */
+    bufferSize += 2; /* CsrWifiRouterCtrlPowersaveTypeMask primitive->powersaveMode */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPeerUpdateReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPeerUpdateReq *primitive = (CsrWifiRouterCtrlPeerUpdateReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint32Ser(ptr, len, (u32) primitive->peerRecordHandle);
+    CsrUint16Ser(ptr, len, (u16) primitive->powersaveMode);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPeerUpdateReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPeerUpdateReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPeerUpdateReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->peerRecordHandle, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->powersaveMode, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlBlockAckEnableReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 21) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 6; /* u8 primitive->macAddress.a[6] */
+    bufferSize += 1; /* CsrWifiRouterCtrlTrafficStreamId primitive->trafficStreamID */
+    bufferSize += 1; /* CsrWifiRouterCtrlBlockAckRole primitive->role */
+    bufferSize += 2; /* u16 primitive->bufferSize */
+    bufferSize += 2; /* u16 primitive->timeout */
+    bufferSize += 2; /* u16 primitive->ssn */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlBlockAckEnableReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlBlockAckEnableReq *primitive = (CsrWifiRouterCtrlBlockAckEnableReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrMemCpySer(ptr, len, (const void *) primitive->macAddress.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->trafficStreamID);
+    CsrUint8Ser(ptr, len, (u8) primitive->role);
+    CsrUint16Ser(ptr, len, (u16) primitive->bufferSize);
+    CsrUint16Ser(ptr, len, (u16) primitive->timeout);
+    CsrUint16Ser(ptr, len, (u16) primitive->ssn);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlBlockAckEnableReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlBlockAckEnableReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckEnableReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrMemCpyDes(primitive->macAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->trafficStreamID, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->role, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->bufferSize, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->timeout, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->ssn, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlBlockAckDisableReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 6; /* u8 primitive->macAddress.a[6] */
+    bufferSize += 1; /* CsrWifiRouterCtrlTrafficStreamId primitive->trafficStreamID */
+    bufferSize += 1; /* CsrWifiRouterCtrlBlockAckRole primitive->role */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlBlockAckDisableReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlBlockAckDisableReq *primitive = (CsrWifiRouterCtrlBlockAckDisableReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrMemCpySer(ptr, len, (const void *) primitive->macAddress.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->trafficStreamID);
+    CsrUint8Ser(ptr, len, (u8) primitive->role);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlBlockAckDisableReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlBlockAckDisableReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckDisableReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrMemCpyDes(primitive->macAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->trafficStreamID, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->role, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlWapiRxPktReqSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWapiRxPktReq *primitive = (CsrWifiRouterCtrlWapiRxPktReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2;                       /* u16 primitive->interfaceTag */
+    bufferSize += 2;                       /* u16 primitive->signalLength */
+    bufferSize += primitive->signalLength; /* u8 primitive->signal */
+    bufferSize += 2;                       /* u16 primitive->dataLength */
+    bufferSize += primitive->dataLength;   /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWapiRxPktReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWapiRxPktReq *primitive = (CsrWifiRouterCtrlWapiRxPktReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->signalLength);
+    if (primitive->signalLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->signal, ((u16) (primitive->signalLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWapiRxPktReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWapiRxPktReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWapiRxPktReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->signalLength, buffer, &offset);
+    if (primitive->signalLength)
+    {
+        primitive->signal = kmalloc(primitive->signalLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->signal, buffer, &offset, ((u16) (primitive->signalLength)));
+    }
+    else
+    {
+        primitive->signal = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWapiRxPktReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWapiRxPktReq *primitive = (CsrWifiRouterCtrlWapiRxPktReq *) voidPrimitivePointer;
+    kfree(primitive->signal);
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlWapiUnicastTxPktReqSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWapiUnicastTxPktReq *primitive = (CsrWifiRouterCtrlWapiUnicastTxPktReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2;                     /* u16 primitive->interfaceTag */
+    bufferSize += 2;                     /* u16 primitive->dataLength */
+    bufferSize += primitive->dataLength; /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWapiUnicastTxPktReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWapiUnicastTxPktReq *primitive = (CsrWifiRouterCtrlWapiUnicastTxPktReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWapiUnicastTxPktReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWapiUnicastTxPktReq *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWapiUnicastTxPktReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWapiUnicastTxPktReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWapiUnicastTxPktReq *primitive = (CsrWifiRouterCtrlWapiUnicastTxPktReq *) voidPrimitivePointer;
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlHipIndSizeof(void *msg)
+{
+    CsrWifiRouterCtrlHipInd *primitive = (CsrWifiRouterCtrlHipInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 12) */
+    bufferSize += 2;                            /* u16 primitive->mlmeCommandLength */
+    bufferSize += primitive->mlmeCommandLength; /* u8 primitive->mlmeCommand */
+    bufferSize += 2;                            /* u16 primitive->dataRef1Length */
+    bufferSize += primitive->dataRef1Length;    /* u8 primitive->dataRef1 */
+    bufferSize += 2;                            /* u16 primitive->dataRef2Length */
+    bufferSize += primitive->dataRef2Length;    /* u8 primitive->dataRef2 */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlHipIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlHipInd *primitive = (CsrWifiRouterCtrlHipInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->mlmeCommandLength);
+    if (primitive->mlmeCommandLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mlmeCommand, ((u16) (primitive->mlmeCommandLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->dataRef1Length);
+    if (primitive->dataRef1Length)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->dataRef1, ((u16) (primitive->dataRef1Length)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->dataRef2Length);
+    if (primitive->dataRef2Length)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->dataRef2, ((u16) (primitive->dataRef2Length)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlHipIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlHipInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlHipInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mlmeCommandLength, buffer, &offset);
+    if (primitive->mlmeCommandLength)
+    {
+        primitive->mlmeCommand = kmalloc(primitive->mlmeCommandLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mlmeCommand, buffer, &offset, ((u16) (primitive->mlmeCommandLength)));
+    }
+    else
+    {
+        primitive->mlmeCommand = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->dataRef1Length, buffer, &offset);
+    if (primitive->dataRef1Length)
+    {
+        primitive->dataRef1 = kmalloc(primitive->dataRef1Length, GFP_KERNEL);
+        CsrMemCpyDes(primitive->dataRef1, buffer, &offset, ((u16) (primitive->dataRef1Length)));
+    }
+    else
+    {
+        primitive->dataRef1 = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->dataRef2Length, buffer, &offset);
+    if (primitive->dataRef2Length)
+    {
+        primitive->dataRef2 = kmalloc(primitive->dataRef2Length, GFP_KERNEL);
+        CsrMemCpyDes(primitive->dataRef2, buffer, &offset, ((u16) (primitive->dataRef2Length)));
+    }
+    else
+    {
+        primitive->dataRef2 = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlHipIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlHipInd *primitive = (CsrWifiRouterCtrlHipInd *) voidPrimitivePointer;
+    kfree(primitive->mlmeCommand);
+    kfree(primitive->dataRef1);
+    kfree(primitive->dataRef2);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlMulticastAddressIndSizeof(void *msg)
+{
+    CsrWifiRouterCtrlMulticastAddressInd *primitive = (CsrWifiRouterCtrlMulticastAddressInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiRouterCtrlListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->setAddressesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressesCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->setAddresses[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlMulticastAddressIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlMulticastAddressInd *primitive = (CsrWifiRouterCtrlMulticastAddressInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->setAddressesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressesCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->setAddresses[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlMulticastAddressIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlMulticastAddressInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlMulticastAddressInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->setAddressesCount, buffer, &offset);
+    primitive->setAddresses = NULL;
+    if (primitive->setAddressesCount)
+    {
+        primitive->setAddresses = kmalloc(sizeof(CsrWifiMacAddress) * primitive->setAddressesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressesCount; i1++)
+        {
+            CsrMemCpyDes(primitive->setAddresses[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlMulticastAddressIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlMulticastAddressInd *primitive = (CsrWifiRouterCtrlMulticastAddressInd *) voidPrimitivePointer;
+    kfree(primitive->setAddresses);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlPortConfigureCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 6; /* u8 primitive->macAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPortConfigureCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPortConfigureCfm *primitive = (CsrWifiRouterCtrlPortConfigureCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrMemCpySer(ptr, len, (const void *) primitive->macAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPortConfigureCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPortConfigureCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPortConfigureCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrMemCpyDes(primitive->macAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlSuspendIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 1; /* u8 primitive->hardSuspend */
+    bufferSize += 1; /* u8 primitive->d3Suspend */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlSuspendIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlSuspendInd *primitive = (CsrWifiRouterCtrlSuspendInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint8Ser(ptr, len, (u8) primitive->hardSuspend);
+    CsrUint8Ser(ptr, len, (u8) primitive->d3Suspend);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlSuspendIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlSuspendInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlSuspendInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->hardSuspend, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->d3Suspend, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTclasAddCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTclasAddCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTclasAddCfm *primitive = (CsrWifiRouterCtrlTclasAddCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTclasAddCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTclasAddCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTclasAddCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlRawSdioDeinitialiseCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->result */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlRawSdioDeinitialiseCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlRawSdioDeinitialiseCfm *primitive = (CsrWifiRouterCtrlRawSdioDeinitialiseCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->result);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlRawSdioDeinitialiseCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlRawSdioDeinitialiseCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlRawSdioDeinitialiseCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlRawSdioInitialiseCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 39) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->result */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioByteRead primitive->byteRead */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioByteWrite primitive->byteWrite */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioFirmwareDownload primitive->firmwareDownload */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioReset primitive->reset */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioCoreDumpPrepare primitive->coreDumpPrepare */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioByteBlockRead primitive->byteBlockRead */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioGpRead16 primitive->gpRead16 */
+    bufferSize += 4; /* CsrWifiRouterCtrlRawSdioGpWrite16 primitive->gpWrite16 */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlRawSdioInitialiseCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlRawSdioInitialiseCfm *primitive = (CsrWifiRouterCtrlRawSdioInitialiseCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->result);
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->byteRead */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->byteWrite */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->firmwareDownload */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->reset */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->coreDumpPrepare */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->byteBlockRead */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->gpRead16 */
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->gpWrite16 */
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlRawSdioInitialiseCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlRawSdioInitialiseCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlRawSdioInitialiseCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result, buffer, &offset);
+    primitive->byteRead = NULL;         /* Special for Function Pointers... */
+    offset += 4;
+    primitive->byteWrite = NULL;        /* Special for Function Pointers... */
+    offset += 4;
+    primitive->firmwareDownload = NULL; /* Special for Function Pointers... */
+    offset += 4;
+    primitive->reset = NULL;            /* Special for Function Pointers... */
+    offset += 4;
+    primitive->coreDumpPrepare = NULL;  /* Special for Function Pointers... */
+    offset += 4;
+    primitive->byteBlockRead = NULL;    /* Special for Function Pointers... */
+    offset += 4;
+    primitive->gpRead16 = NULL;         /* Special for Function Pointers... */
+    offset += 4;
+    primitive->gpWrite16 = NULL;        /* Special for Function Pointers... */
+    offset += 4;
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTclasDelCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTclasDelCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTclasDelCfm *primitive = (CsrWifiRouterCtrlTclasDelCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTclasDelCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTclasDelCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTclasDelCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTrafficProtocolIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 17) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrWifiRouterCtrlTrafficPacketType primitive->packetType */
+    bufferSize += 2; /* CsrWifiRouterCtrlProtocolDirection primitive->direction */
+    bufferSize += 6; /* u8 primitive->srcAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTrafficProtocolIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTrafficProtocolInd *primitive = (CsrWifiRouterCtrlTrafficProtocolInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->packetType);
+    CsrUint16Ser(ptr, len, (u16) primitive->direction);
+    CsrMemCpySer(ptr, len, (const void *) primitive->srcAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTrafficProtocolIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTrafficProtocolInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTrafficProtocolInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->packetType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->direction, buffer, &offset);
+    CsrMemCpyDes(primitive->srcAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlTrafficSampleIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 38) */
+    bufferSize += 2;  /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2;  /* u16 primitive->interfaceTag */
+    bufferSize += 4;  /* u32 primitive->stats.rxMeanRate */
+    bufferSize += 4;  /* u32 primitive->stats.rxFramesNum */
+    bufferSize += 4;  /* u32 primitive->stats.txFramesNum */
+    bufferSize += 4;  /* u32 primitive->stats.rxBytesCount */
+    bufferSize += 4;  /* u32 primitive->stats.txBytesCount */
+    bufferSize += 11; /* u8 primitive->stats.intervals[11] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlTrafficSampleIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlTrafficSampleInd *primitive = (CsrWifiRouterCtrlTrafficSampleInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint32Ser(ptr, len, (u32) primitive->stats.rxMeanRate);
+    CsrUint32Ser(ptr, len, (u32) primitive->stats.rxFramesNum);
+    CsrUint32Ser(ptr, len, (u32) primitive->stats.txFramesNum);
+    CsrUint32Ser(ptr, len, (u32) primitive->stats.rxBytesCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->stats.txBytesCount);
+    CsrMemCpySer(ptr, len, (const void *) primitive->stats.intervals, ((u16) (11)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlTrafficSampleIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlTrafficSampleInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlTrafficSampleInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->stats.rxMeanRate, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->stats.rxFramesNum, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->stats.txFramesNum, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->stats.rxBytesCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->stats.txBytesCount, buffer, &offset);
+    CsrMemCpyDes(primitive->stats.intervals, buffer, &offset, ((u16) (11)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlWifiOnIndSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWifiOnInd *primitive = (CsrWifiRouterCtrlWifiOnInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 27) */
+    bufferSize += 2;                                                                                    /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2;                                                                                    /* CsrResult primitive->status */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.chipId */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.chipVersion */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.firmwareBuild */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.firmwareHip */
+    bufferSize += (primitive->versions.routerBuild ? strlen(primitive->versions.routerBuild) : 0) + 1;  /* char* primitive->versions.routerBuild (0 byte len + 1 for NULL Term) */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.routerHip */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWifiOnIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWifiOnInd *primitive = (CsrWifiRouterCtrlWifiOnInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.chipId);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.chipVersion);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.firmwareBuild);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.firmwareHip);
+    CsrCharStringSer(ptr, len, primitive->versions.routerBuild);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.routerHip);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWifiOnIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWifiOnInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.chipId, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.chipVersion, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.firmwareBuild, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.firmwareHip, buffer, &offset);
+    CsrCharStringDes(&primitive->versions.routerBuild, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.routerHip, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWifiOnIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWifiOnInd *primitive = (CsrWifiRouterCtrlWifiOnInd *) voidPrimitivePointer;
+    kfree(primitive->versions.routerBuild);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlWifiOnCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWifiOnCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWifiOnCfm *primitive = (CsrWifiRouterCtrlWifiOnCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWifiOnCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWifiOnCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWifiOnCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlM4ReadyToSendIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlM4ReadyToSendIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlM4ReadyToSendInd *primitive = (CsrWifiRouterCtrlM4ReadyToSendInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlM4ReadyToSendIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlM4ReadyToSendInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlM4ReadyToSendInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlM4TransmittedIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlM4TransmittedIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlM4TransmittedInd *primitive = (CsrWifiRouterCtrlM4TransmittedInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlM4TransmittedIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlM4TransmittedInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlM4TransmittedInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlMicFailureIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 14) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 1; /* u8 primitive->unicastPdu */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlMicFailureIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlMicFailureInd *primitive = (CsrWifiRouterCtrlMicFailureInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->unicastPdu);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlMicFailureIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlMicFailureInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlMicFailureInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->unicastPdu, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlConnectedIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 14) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 1; /* CsrWifiRouterCtrlPeerStatus primitive->peerStatus */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlConnectedIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlConnectedInd *primitive = (CsrWifiRouterCtrlConnectedInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->peerStatus);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlConnectedIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlConnectedInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlConnectedInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->peerStatus, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlPeerAddCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 19) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 4; /* CsrWifiRouterCtrlPeerRecordHandle primitive->peerRecordHandle */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPeerAddCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPeerAddCfm *primitive = (CsrWifiRouterCtrlPeerAddCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrUint32Ser(ptr, len, (u32) primitive->peerRecordHandle);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPeerAddCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPeerAddCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPeerAddCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint32Des((u32 *) &primitive->peerRecordHandle, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlPeerDelCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPeerDelCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPeerDelCfm *primitive = (CsrWifiRouterCtrlPeerDelCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPeerDelCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPeerDelCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPeerDelCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlUnexpectedFrameIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlUnexpectedFrameIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlUnexpectedFrameInd *primitive = (CsrWifiRouterCtrlUnexpectedFrameInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlUnexpectedFrameIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlUnexpectedFrameInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlUnexpectedFrameInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlPeerUpdateCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlPeerUpdateCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlPeerUpdateCfm *primitive = (CsrWifiRouterCtrlPeerUpdateCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlPeerUpdateCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlPeerUpdateCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlPeerUpdateCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlCapabilitiesCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->commandQueueSize */
+    bufferSize += 2; /* u16 primitive->trafficQueueSize */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlCapabilitiesCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlCapabilitiesCfm *primitive = (CsrWifiRouterCtrlCapabilitiesCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->commandQueueSize);
+    CsrUint16Ser(ptr, len, (u16) primitive->trafficQueueSize);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlCapabilitiesCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlCapabilitiesCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlCapabilitiesCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->commandQueueSize, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->trafficQueueSize, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlBlockAckEnableCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlBlockAckEnableCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlBlockAckEnableCfm *primitive = (CsrWifiRouterCtrlBlockAckEnableCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlBlockAckEnableCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlBlockAckEnableCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckEnableCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlBlockAckDisableCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlBlockAckDisableCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlBlockAckDisableCfm *primitive = (CsrWifiRouterCtrlBlockAckDisableCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlBlockAckDisableCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlBlockAckDisableCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckDisableCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlBlockAckErrorIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 16) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiRouterCtrlTrafficStreamId primitive->trafficStreamID */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlBlockAckErrorIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlBlockAckErrorInd *primitive = (CsrWifiRouterCtrlBlockAckErrorInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->trafficStreamID);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlBlockAckErrorIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlBlockAckErrorInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlBlockAckErrorInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->trafficStreamID, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlStaInactiveIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 6; /* u8 primitive->staAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlStaInactiveIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlStaInactiveInd *primitive = (CsrWifiRouterCtrlStaInactiveInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->staAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlStaInactiveIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlStaInactiveInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlStaInactiveInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->staAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlWapiRxMicCheckIndSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWapiRxMicCheckInd *primitive = (CsrWifiRouterCtrlWapiRxMicCheckInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2;                       /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2;                       /* u16 primitive->interfaceTag */
+    bufferSize += 2;                       /* u16 primitive->signalLength */
+    bufferSize += primitive->signalLength; /* u8 primitive->signal */
+    bufferSize += 2;                       /* u16 primitive->dataLength */
+    bufferSize += primitive->dataLength;   /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWapiRxMicCheckIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWapiRxMicCheckInd *primitive = (CsrWifiRouterCtrlWapiRxMicCheckInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->signalLength);
+    if (primitive->signalLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->signal, ((u16) (primitive->signalLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWapiRxMicCheckIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWapiRxMicCheckInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWapiRxMicCheckInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->signalLength, buffer, &offset);
+    if (primitive->signalLength)
+    {
+        primitive->signal = kmalloc(primitive->signalLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->signal, buffer, &offset, ((u16) (primitive->signalLength)));
+    }
+    else
+    {
+        primitive->signal = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWapiRxMicCheckIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWapiRxMicCheckInd *primitive = (CsrWifiRouterCtrlWapiRxMicCheckInd *) voidPrimitivePointer;
+    kfree(primitive->signal);
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterCtrlModeSetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiRouterCtrlMode primitive->mode */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlModeSetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlModeSetCfm *primitive = (CsrWifiRouterCtrlModeSetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->mode);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlModeSetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlModeSetCfm *primitive = kmalloc(sizeof(CsrWifiRouterCtrlModeSetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterCtrlWapiUnicastTxEncryptIndSizeof(void *msg)
+{
+    CsrWifiRouterCtrlWapiUnicastTxEncryptInd *primitive = (CsrWifiRouterCtrlWapiUnicastTxEncryptInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2;                     /* CsrWifiRouterCtrlRequestorInfo primitive->clientData */
+    bufferSize += 2;                     /* u16 primitive->interfaceTag */
+    bufferSize += 2;                     /* u16 primitive->dataLength */
+    bufferSize += primitive->dataLength; /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterCtrlWapiUnicastTxEncryptIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterCtrlWapiUnicastTxEncryptInd *primitive = (CsrWifiRouterCtrlWapiUnicastTxEncryptInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->clientData);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiRouterCtrlWapiUnicastTxEncryptIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterCtrlWapiUnicastTxEncryptInd *primitive = kmalloc(sizeof(CsrWifiRouterCtrlWapiUnicastTxEncryptInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->clientData, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiRouterCtrlWapiUnicastTxEncryptIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterCtrlWapiUnicastTxEncryptInd *primitive = (CsrWifiRouterCtrlWapiUnicastTxEncryptInd *) voidPrimitivePointer;
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_router_ctrl_serialize.h b/drivers/staging/csr/csr_wifi_router_ctrl_serialize.h
new file mode 100644
index 00000000000..2c2a229f4bf
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_ctrl_serialize.h
@@ -0,0 +1,341 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_CTRL_SERIALIZE_H__
+#define CSR_WIFI_ROUTER_CTRL_SERIALIZE_H__
+
+#include "csr_wifi_msgconv.h"
+
+#include "csr_wifi_router_ctrl_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern void CsrWifiRouterCtrlPfree(void *ptr);
+
+extern u8* CsrWifiRouterCtrlConfigurePowerModeReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlConfigurePowerModeReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlConfigurePowerModeReqSizeof(void *msg);
+#define CsrWifiRouterCtrlConfigurePowerModeReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlHipReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlHipReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlHipReqSizeof(void *msg);
+extern void CsrWifiRouterCtrlHipReqSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlMediaStatusReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlMediaStatusReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlMediaStatusReqSizeof(void *msg);
+#define CsrWifiRouterCtrlMediaStatusReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlMulticastAddressResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlMulticastAddressResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlMulticastAddressResSizeof(void *msg);
+extern void CsrWifiRouterCtrlMulticastAddressResSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlPortConfigureReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPortConfigureReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPortConfigureReqSizeof(void *msg);
+#define CsrWifiRouterCtrlPortConfigureReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlQosControlReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlQosControlReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlQosControlReqSizeof(void *msg);
+#define CsrWifiRouterCtrlQosControlReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlSuspendResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlSuspendResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlSuspendResSizeof(void *msg);
+#define CsrWifiRouterCtrlSuspendResSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTclasAddReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTclasAddReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTclasAddReqSizeof(void *msg);
+extern void CsrWifiRouterCtrlTclasAddReqSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlResumeResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlResumeResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlResumeResSizeof(void *msg);
+#define CsrWifiRouterCtrlResumeResSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiRouterCtrlRawSdioDeinitialiseReqSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlRawSdioInitialiseReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiRouterCtrlRawSdioInitialiseReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiRouterCtrlRawSdioInitialiseReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiRouterCtrlRawSdioInitialiseReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTclasDelReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTclasDelReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTclasDelReqSizeof(void *msg);
+extern void CsrWifiRouterCtrlTclasDelReqSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlTrafficClassificationReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTrafficClassificationReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTrafficClassificationReqSizeof(void *msg);
+#define CsrWifiRouterCtrlTrafficClassificationReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTrafficConfigReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTrafficConfigReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTrafficConfigReqSizeof(void *msg);
+#define CsrWifiRouterCtrlTrafficConfigReqSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlWifiOffReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiRouterCtrlWifiOffReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiRouterCtrlWifiOffReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiRouterCtrlWifiOffReqSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlWifiOffResSer CsrWifiEventCsrUint16Ser
+#define CsrWifiRouterCtrlWifiOffResDes CsrWifiEventCsrUint16Des
+#define CsrWifiRouterCtrlWifiOffResSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiRouterCtrlWifiOffResSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlWifiOnReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWifiOnReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWifiOnReqSizeof(void *msg);
+extern void CsrWifiRouterCtrlWifiOnReqSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlWifiOnResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWifiOnResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWifiOnResSizeof(void *msg);
+extern void CsrWifiRouterCtrlWifiOnResSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlM4TransmitReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlM4TransmitReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlM4TransmitReqSizeof(void *msg);
+#define CsrWifiRouterCtrlM4TransmitReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlModeSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlModeSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlModeSetReqSizeof(void *msg);
+#define CsrWifiRouterCtrlModeSetReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlPeerAddReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPeerAddReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPeerAddReqSizeof(void *msg);
+#define CsrWifiRouterCtrlPeerAddReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlPeerDelReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPeerDelReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPeerDelReqSizeof(void *msg);
+#define CsrWifiRouterCtrlPeerDelReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlPeerUpdateReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPeerUpdateReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPeerUpdateReqSizeof(void *msg);
+#define CsrWifiRouterCtrlPeerUpdateReqSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlCapabilitiesReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiRouterCtrlCapabilitiesReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiRouterCtrlCapabilitiesReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiRouterCtrlCapabilitiesReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlBlockAckEnableReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlBlockAckEnableReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlBlockAckEnableReqSizeof(void *msg);
+#define CsrWifiRouterCtrlBlockAckEnableReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlBlockAckDisableReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlBlockAckDisableReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlBlockAckDisableReqSizeof(void *msg);
+#define CsrWifiRouterCtrlBlockAckDisableReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlWapiRxPktReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWapiRxPktReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWapiRxPktReqSizeof(void *msg);
+extern void CsrWifiRouterCtrlWapiRxPktReqSerFree(void *msg);
+
+#define CsrWifiRouterCtrlWapiMulticastFilterReqSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiRouterCtrlWapiMulticastFilterReqDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiRouterCtrlWapiMulticastFilterReqSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiRouterCtrlWapiMulticastFilterReqSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlWapiUnicastFilterReqSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiRouterCtrlWapiUnicastFilterReqDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiRouterCtrlWapiUnicastFilterReqSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiRouterCtrlWapiUnicastFilterReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlWapiUnicastTxPktReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWapiUnicastTxPktReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWapiUnicastTxPktReqSizeof(void *msg);
+extern void CsrWifiRouterCtrlWapiUnicastTxPktReqSerFree(void *msg);
+
+#define CsrWifiRouterCtrlWapiFilterReqSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiRouterCtrlWapiFilterReqDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiRouterCtrlWapiFilterReqSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiRouterCtrlWapiFilterReqSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlHipIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlHipIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlHipIndSizeof(void *msg);
+extern void CsrWifiRouterCtrlHipIndSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlMulticastAddressIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlMulticastAddressIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlMulticastAddressIndSizeof(void *msg);
+extern void CsrWifiRouterCtrlMulticastAddressIndSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlPortConfigureCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPortConfigureCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPortConfigureCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlPortConfigureCfmSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlResumeIndSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiRouterCtrlResumeIndDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiRouterCtrlResumeIndSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiRouterCtrlResumeIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlSuspendIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlSuspendIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlSuspendIndSizeof(void *msg);
+#define CsrWifiRouterCtrlSuspendIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTclasAddCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTclasAddCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTclasAddCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlTclasAddCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlRawSdioDeinitialiseCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlRawSdioDeinitialiseCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlRawSdioDeinitialiseCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlRawSdioDeinitialiseCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlRawSdioInitialiseCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlRawSdioInitialiseCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlRawSdioInitialiseCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlRawSdioInitialiseCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTclasDelCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTclasDelCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTclasDelCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlTclasDelCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTrafficProtocolIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTrafficProtocolIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTrafficProtocolIndSizeof(void *msg);
+#define CsrWifiRouterCtrlTrafficProtocolIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlTrafficSampleIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlTrafficSampleIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlTrafficSampleIndSizeof(void *msg);
+#define CsrWifiRouterCtrlTrafficSampleIndSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlWifiOffIndSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiRouterCtrlWifiOffIndDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiRouterCtrlWifiOffIndSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiRouterCtrlWifiOffIndSerFree CsrWifiRouterCtrlPfree
+
+#define CsrWifiRouterCtrlWifiOffCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiRouterCtrlWifiOffCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiRouterCtrlWifiOffCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiRouterCtrlWifiOffCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlWifiOnIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWifiOnIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWifiOnIndSizeof(void *msg);
+extern void CsrWifiRouterCtrlWifiOnIndSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlWifiOnCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWifiOnCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWifiOnCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlWifiOnCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlM4ReadyToSendIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlM4ReadyToSendIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlM4ReadyToSendIndSizeof(void *msg);
+#define CsrWifiRouterCtrlM4ReadyToSendIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlM4TransmittedIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlM4TransmittedIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlM4TransmittedIndSizeof(void *msg);
+#define CsrWifiRouterCtrlM4TransmittedIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlMicFailureIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlMicFailureIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlMicFailureIndSizeof(void *msg);
+#define CsrWifiRouterCtrlMicFailureIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlConnectedIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlConnectedIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlConnectedIndSizeof(void *msg);
+#define CsrWifiRouterCtrlConnectedIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlPeerAddCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPeerAddCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPeerAddCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlPeerAddCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlPeerDelCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPeerDelCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPeerDelCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlPeerDelCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlUnexpectedFrameIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlUnexpectedFrameIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlUnexpectedFrameIndSizeof(void *msg);
+#define CsrWifiRouterCtrlUnexpectedFrameIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlPeerUpdateCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlPeerUpdateCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlPeerUpdateCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlPeerUpdateCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlCapabilitiesCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlCapabilitiesCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlCapabilitiesCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlCapabilitiesCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlBlockAckEnableCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlBlockAckEnableCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlBlockAckEnableCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlBlockAckEnableCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlBlockAckDisableCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlBlockAckDisableCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlBlockAckDisableCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlBlockAckDisableCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlBlockAckErrorIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlBlockAckErrorIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlBlockAckErrorIndSizeof(void *msg);
+#define CsrWifiRouterCtrlBlockAckErrorIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlStaInactiveIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlStaInactiveIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlStaInactiveIndSizeof(void *msg);
+#define CsrWifiRouterCtrlStaInactiveIndSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlWapiRxMicCheckIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWapiRxMicCheckIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWapiRxMicCheckIndSizeof(void *msg);
+extern void CsrWifiRouterCtrlWapiRxMicCheckIndSerFree(void *msg);
+
+extern u8* CsrWifiRouterCtrlModeSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlModeSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlModeSetCfmSizeof(void *msg);
+#define CsrWifiRouterCtrlModeSetCfmSerFree CsrWifiRouterCtrlPfree
+
+extern u8* CsrWifiRouterCtrlWapiUnicastTxEncryptIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterCtrlWapiUnicastTxEncryptIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterCtrlWapiUnicastTxEncryptIndSizeof(void *msg);
+extern void CsrWifiRouterCtrlWapiUnicastTxEncryptIndSerFree(void *msg);
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* CSR_WIFI_ROUTER_CTRL_SERIALIZE_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_router_free_downstream_contents.c b/drivers/staging/csr/csr_wifi_router_free_downstream_contents.c
new file mode 100644
index 00000000000..c4badc565a9
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_free_downstream_contents.c
@@ -0,0 +1,53 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_router_prim.h"
+#include "csr_wifi_router_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiRouterFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_ROUTER_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterFreeDownstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_ROUTER_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiRouterPrim *) message))
+    {
+        case CSR_WIFI_ROUTER_MA_PACKET_REQ:
+        {
+            CsrWifiRouterMaPacketReq *p = (CsrWifiRouterMaPacketReq *)message;
+            kfree(p->frame);
+            p->frame = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_router_free_upstream_contents.c b/drivers/staging/csr/csr_wifi_router_free_upstream_contents.c
new file mode 100644
index 00000000000..de1086d7158
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_free_upstream_contents.c
@@ -0,0 +1,53 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_router_prim.h"
+#include "csr_wifi_router_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiRouterFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_ROUTER_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterFreeUpstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_ROUTER_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiRouterPrim *) message))
+    {
+        case CSR_WIFI_ROUTER_MA_PACKET_IND:
+        {
+            CsrWifiRouterMaPacketInd *p = (CsrWifiRouterMaPacketInd *)message;
+            kfree(p->frame);
+            p->frame = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_router_lib.h b/drivers/staging/csr/csr_wifi_router_lib.h
new file mode 100644
index 00000000000..06a2214714b
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_lib.h
@@ -0,0 +1,427 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_LIB_H__
+#define CSR_WIFI_ROUTER_LIB_H__
+
+#include "csr_sched.h"
+#include "csr_macro.h"
+#include "csr_msg_transport.h"
+
+#include "csr_wifi_lib.h"
+
+#include "csr_wifi_router_prim.h"
+#include "csr_wifi_router_task.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiRouterFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_ROUTER upstream message. Does not
+ *      free the message itself, and can only be used for upstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_ROUTER upstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterFreeUpstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiRouterFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_ROUTER downstream message. Does not
+ *      free the message itself, and can only be used for downstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_ROUTER downstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiRouterFreeDownstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ * Enum to string functions
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiRouterAppTypeToString(CsrWifiRouterAppType value);
+const char* CsrWifiRouterEncapsulationToString(CsrWifiRouterEncapsulation value);
+const char* CsrWifiRouterOuiToString(CsrWifiRouterOui value);
+const char* CsrWifiRouterPriorityToString(CsrWifiRouterPriority value);
+
+
+/*----------------------------------------------------------------------------*
+ * CsrPrim Type toString function.
+ * Converts a message type to the String name of the Message
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiRouterPrimTypeToString(CsrPrim msgType);
+
+/*----------------------------------------------------------------------------*
+ * Lookup arrays for PrimType name Strings
+ *----------------------------------------------------------------------------*/
+extern const char *CsrWifiRouterUpstreamPrimNames[CSR_WIFI_ROUTER_PRIM_UPSTREAM_COUNT];
+extern const char *CsrWifiRouterDownstreamPrimNames[CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT];
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketCancelReqSend
+
+  DESCRIPTION
+    This primitive is used to request cancellation of a previously send
+    CsrWifiRouterMaPacketReq.
+    The frame may already have been transmitted so there is no guarantees
+    that the CsrWifiRouterMaPacketCancelReq actually cancels the transmission
+    of the frame in question.
+    If the cancellation fails, the Router will send, if required,
+    CsrWifiRouterMaPacketCfm.
+    If the cancellation succeeds, the Router will not send
+    CsrWifiRouterMaPacketCfm.
+
+  PARAMETERS
+    queue          - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    hostTag        - The hostTag for the frame, which should be cancelled.
+    priority       - Priority of the frame, which should be cancelled
+    peerMacAddress - Destination MAC address of the frame, which should be
+                     cancelled
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketCancelReqCreate(msg__, dst__, src__, interfaceTag__, hostTag__, priority__, peerMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketCancelReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_CANCEL_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->hostTag = (hostTag__); \
+    msg__->priority = (priority__); \
+    msg__->peerMacAddress = (peerMacAddress__);
+
+#define CsrWifiRouterMaPacketCancelReqSendTo(dst__, src__, interfaceTag__, hostTag__, priority__, peerMacAddress__) \
+    { \
+        CsrWifiRouterMaPacketCancelReq *msg__; \
+        CsrWifiRouterMaPacketCancelReqCreate(msg__, dst__, src__, interfaceTag__, hostTag__, priority__, peerMacAddress__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketCancelReqSend(src__, interfaceTag__, hostTag__, priority__, peerMacAddress__) \
+    CsrWifiRouterMaPacketCancelReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, hostTag__, priority__, peerMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketReqSend
+
+  DESCRIPTION
+    A task sends this primitive to transmit a frame.
+
+  PARAMETERS
+    queue              - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+    frameLength        - Length of the frame to be sent in bytes
+    frame              - Pointer to the frame to be sent
+    freeFunction       - Pointer to function to be used to free the frame
+    priority           - Priority of the frame, which should be sent
+    hostTag            - An application shall set the bits b31..b28 using one of
+                         the CSR_WIFI_ROUTER_APP_TYPE_* masks. Bits b0..b27 can
+                         be used by the requestor without any restrictions, but
+                         the hostTag shall be unique so the hostTag for
+                         CSR_WIFI_ROUTER_APP _TYPE_OTHER should be constructured
+                         in the following way [ CSR_WIFI_ROUTER_APP_TYPE_OTHER
+                         (4 bits) | SubscriptionHandle (8 bits) | Sequence no.
+                         (20 bits) ]. If the hostTag is not unique, the
+                         behaviour of the system is unpredicatable with respect
+                         to data/management frame transfer.
+    cfmRequested       - Indicates if the requestor needs a confirm for packet
+                         requests sent under this subscription. If set to TRUE,
+                         the router will send a confirm, else it will not send
+                         any confirm
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketReqCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, frameLength__, frame__, freeFunction__, priority__, hostTag__, cfmRequested__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->subscriptionHandle = (subscriptionHandle__); \
+    msg__->frameLength = (frameLength__); \
+    msg__->frame = (frame__); \
+    msg__->freeFunction = (freeFunction__); \
+    msg__->priority = (priority__); \
+    msg__->hostTag = (hostTag__); \
+    msg__->cfmRequested = (cfmRequested__);
+
+#define CsrWifiRouterMaPacketReqSendTo(dst__, src__, interfaceTag__, subscriptionHandle__, frameLength__, frame__, freeFunction__, priority__, hostTag__, cfmRequested__) \
+    { \
+        CsrWifiRouterMaPacketReq *msg__; \
+        CsrWifiRouterMaPacketReqCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, frameLength__, frame__, freeFunction__, priority__, hostTag__, cfmRequested__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketReqSend(src__, interfaceTag__, subscriptionHandle__, frameLength__, frame__, freeFunction__, priority__, hostTag__, cfmRequested__) \
+    CsrWifiRouterMaPacketReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, subscriptionHandle__, frameLength__, frame__, freeFunction__, priority__, hostTag__, cfmRequested__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketIndSend
+
+  DESCRIPTION
+    The router sends the primitive to a subscribed task when it receives a
+    frame matching the subscription.
+
+  PARAMETERS
+    queue              - Destination Task Queue
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+    result             - Status of the operation
+    frameLength        - Length of the received frame in bytes
+    frame              - Pointer to the received frame
+    freeFunction       - Pointer to function to be used to free the frame
+    rssi               - Received signal strength indication in dBm
+    snr                - Signal to Noise Ratio
+    rate               - Transmission/Reception rate
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketIndCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, result__, frameLength__, frame__, freeFunction__, rssi__, snr__, rate__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->subscriptionHandle = (subscriptionHandle__); \
+    msg__->result = (result__); \
+    msg__->frameLength = (frameLength__); \
+    msg__->frame = (frame__); \
+    msg__->freeFunction = (freeFunction__); \
+    msg__->rssi = (rssi__); \
+    msg__->snr = (snr__); \
+    msg__->rate = (rate__);
+
+#define CsrWifiRouterMaPacketIndSendTo(dst__, src__, interfaceTag__, subscriptionHandle__, result__, frameLength__, frame__, freeFunction__, rssi__, snr__, rate__) \
+    { \
+        CsrWifiRouterMaPacketInd *msg__; \
+        CsrWifiRouterMaPacketIndCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, result__, frameLength__, frame__, freeFunction__, rssi__, snr__, rate__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketIndSend(dst__, interfaceTag__, subscriptionHandle__, result__, frameLength__, frame__, freeFunction__, rssi__, snr__, rate__) \
+    CsrWifiRouterMaPacketIndSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, interfaceTag__, subscriptionHandle__, result__, frameLength__, frame__, freeFunction__, rssi__, snr__, rate__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketResSend
+
+  DESCRIPTION
+    A task send this primitive to confirm the reception of the received
+    frame.
+
+  PARAMETERS
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+    result             - Status of the operation
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketResCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, result__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketRes), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_RES, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->subscriptionHandle = (subscriptionHandle__); \
+    msg__->result = (result__);
+
+#define CsrWifiRouterMaPacketResSendTo(dst__, src__, interfaceTag__, subscriptionHandle__, result__) \
+    { \
+        CsrWifiRouterMaPacketRes *msg__; \
+        CsrWifiRouterMaPacketResCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, result__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketResSend(src__, interfaceTag__, subscriptionHandle__, result__) \
+    CsrWifiRouterMaPacketResSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, subscriptionHandle__, result__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketCfmSend
+
+  DESCRIPTION
+    The router sends the primitive to confirm the result of the transmission
+    of the packet of the corresponding CSR_WIFI_ROUTER MA_PACKET_REQ request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    result       - Status of the operation
+    hostTag      - The hostTrag will match the hostTag sent in the request.
+    rate         - Transmission/Reception rate
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketCfmCreate(msg__, dst__, src__, interfaceTag__, result__, hostTag__, rate__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->result = (result__); \
+    msg__->hostTag = (hostTag__); \
+    msg__->rate = (rate__);
+
+#define CsrWifiRouterMaPacketCfmSendTo(dst__, src__, interfaceTag__, result__, hostTag__, rate__) \
+    { \
+        CsrWifiRouterMaPacketCfm *msg__; \
+        CsrWifiRouterMaPacketCfmCreate(msg__, dst__, src__, interfaceTag__, result__, hostTag__, rate__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketCfmSend(dst__, interfaceTag__, result__, hostTag__, rate__) \
+    CsrWifiRouterMaPacketCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, interfaceTag__, result__, hostTag__, rate__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketSubscribeReqSend
+
+  DESCRIPTION
+    A task can use this primitive to subscribe for a particular OUI/protocol
+    and transmit and receive frames matching the subscription.
+    NOTE: Multiple subscriptions for a given protocol and OUI will result in
+    the first subscription receiving the data and not the subsequent
+    subscriptions.
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    encapsulation - Specifies the encapsulation type, which will be used for the
+                    subscription
+    protocol      - Together with the OUI, specifies the protocol, which a task
+                    wants to subscribe to
+    oui           - Specifies the OUI for the protocol, which a task wants to
+                    subscribe to
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketSubscribeReqCreate(msg__, dst__, src__, interfaceTag__, encapsulation__, protocol__, oui__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketSubscribeReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_SUBSCRIBE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->encapsulation = (encapsulation__); \
+    msg__->protocol = (protocol__); \
+    msg__->oui = (oui__);
+
+#define CsrWifiRouterMaPacketSubscribeReqSendTo(dst__, src__, interfaceTag__, encapsulation__, protocol__, oui__) \
+    { \
+        CsrWifiRouterMaPacketSubscribeReq *msg__; \
+        CsrWifiRouterMaPacketSubscribeReqCreate(msg__, dst__, src__, interfaceTag__, encapsulation__, protocol__, oui__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketSubscribeReqSend(src__, interfaceTag__, encapsulation__, protocol__, oui__) \
+    CsrWifiRouterMaPacketSubscribeReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, encapsulation__, protocol__, oui__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketSubscribeCfmSend
+
+  DESCRIPTION
+    The router sends this primitive to confirm the result of the
+    subscription.
+
+  PARAMETERS
+    queue              - Destination Task Queue
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - Handle to the subscription
+                         This handle must be used in all subsequent requests
+    status             - Status of the operation
+    allocOffset        - Size of the offset for the frames of the subscription
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketSubscribeCfmCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, status__, allocOffset__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketSubscribeCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_SUBSCRIBE_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->subscriptionHandle = (subscriptionHandle__); \
+    msg__->status = (status__); \
+    msg__->allocOffset = (allocOffset__);
+
+#define CsrWifiRouterMaPacketSubscribeCfmSendTo(dst__, src__, interfaceTag__, subscriptionHandle__, status__, allocOffset__) \
+    { \
+        CsrWifiRouterMaPacketSubscribeCfm *msg__; \
+        CsrWifiRouterMaPacketSubscribeCfmCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__, status__, allocOffset__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketSubscribeCfmSend(dst__, interfaceTag__, subscriptionHandle__, status__, allocOffset__) \
+    CsrWifiRouterMaPacketSubscribeCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, interfaceTag__, subscriptionHandle__, status__, allocOffset__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketUnsubscribeReqSend
+
+  DESCRIPTION
+    A task sends this primitive to unsubscribe a subscription
+
+  PARAMETERS
+    queue              - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketUnsubscribeReqCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketUnsubscribeReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_UNSUBSCRIBE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->subscriptionHandle = (subscriptionHandle__);
+
+#define CsrWifiRouterMaPacketUnsubscribeReqSendTo(dst__, src__, interfaceTag__, subscriptionHandle__) \
+    { \
+        CsrWifiRouterMaPacketUnsubscribeReq *msg__; \
+        CsrWifiRouterMaPacketUnsubscribeReqCreate(msg__, dst__, src__, interfaceTag__, subscriptionHandle__); \
+        CsrMsgTransport(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketUnsubscribeReqSend(src__, interfaceTag__, subscriptionHandle__) \
+    CsrWifiRouterMaPacketUnsubscribeReqSendTo(CSR_WIFI_ROUTER_IFACEQUEUE, src__, interfaceTag__, subscriptionHandle__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketUnsubscribeCfmSend
+
+  DESCRIPTION
+    The router sends this primitive to confirm the result of the
+    unsubscription.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Status of the operation
+
+*******************************************************************************/
+#define CsrWifiRouterMaPacketUnsubscribeCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiRouterMaPacketUnsubscribeCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_ROUTER_PRIM, CSR_WIFI_ROUTER_MA_PACKET_UNSUBSCRIBE_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiRouterMaPacketUnsubscribeCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiRouterMaPacketUnsubscribeCfm *msg__; \
+        CsrWifiRouterMaPacketUnsubscribeCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_ROUTER_PRIM, msg__); \
+    }
+
+#define CsrWifiRouterMaPacketUnsubscribeCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiRouterMaPacketUnsubscribeCfmSendTo(dst__, CSR_WIFI_ROUTER_IFACEQUEUE, interfaceTag__, status__)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_router_prim.h b/drivers/staging/csr/csr_wifi_router_prim.h
new file mode 100644
index 00000000000..c61486fbb6d
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_prim.h
@@ -0,0 +1,430 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_PRIM_H__
+#define CSR_WIFI_ROUTER_PRIM_H__
+
+#include <linux/types.h>
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+#include "csr_wifi_common.h"
+#include "csr_result.h"
+#include "csr_wifi_fsm_event.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CSR_WIFI_ROUTER_PRIM                                            (0x0400)
+
+typedef CsrPrim CsrWifiRouterPrim;
+
+typedef void (*CsrWifiRouterFrameFreeFunction)(void *frame);
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterAppType
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_APP_TYPE_SME   -
+    CSR_WIFI_ROUTER_APP_TYPE_PAL   -
+    CSR_WIFI_ROUTER_APP_TYPE_NME   -
+    CSR_WIFI_ROUTER_APP_TYPE_OTHER -
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterAppType;
+#define CSR_WIFI_ROUTER_APP_TYPE_SME     ((CsrWifiRouterAppType) 0x0)
+#define CSR_WIFI_ROUTER_APP_TYPE_PAL     ((CsrWifiRouterAppType) 0x1)
+#define CSR_WIFI_ROUTER_APP_TYPE_NME     ((CsrWifiRouterAppType) 0x2)
+#define CSR_WIFI_ROUTER_APP_TYPE_OTHER   ((CsrWifiRouterAppType) 0x3)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterEncapsulation
+
+  DESCRIPTION
+    Indicates the type of encapsulation used for the subscription
+
+ VALUES
+    CSR_WIFI_ROUTER_ENCAPSULATION_ETHERNET
+                   - Ethernet encapsulation
+    CSR_WIFI_ROUTER_ENCAPSULATION_LLC_SNAP
+                   - LLC/SNAP encapsulation
+
+*******************************************************************************/
+typedef u8 CsrWifiRouterEncapsulation;
+#define CSR_WIFI_ROUTER_ENCAPSULATION_ETHERNET   ((CsrWifiRouterEncapsulation) 0x00)
+#define CSR_WIFI_ROUTER_ENCAPSULATION_LLC_SNAP   ((CsrWifiRouterEncapsulation) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterOui
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_ROUTER_OUI_RFC_1042 -
+    CSR_WIFI_ROUTER_OUI_BT       -
+
+*******************************************************************************/
+typedef u32 CsrWifiRouterOui;
+#define CSR_WIFI_ROUTER_OUI_RFC_1042   ((CsrWifiRouterOui) 0x000000)
+#define CSR_WIFI_ROUTER_OUI_BT         ((CsrWifiRouterOui) 0x001958)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterPriority
+
+  DESCRIPTION
+    As defined in the IEEE 802.11 standards
+
+ VALUES
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP0
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP1
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP2
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP3
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP4
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP5
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP6
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_QOS_UP7
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_CONTENTION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_ROUTER_PRIORITY_MANAGEMENT
+                   - See IEEE 802.11 Standard
+
+*******************************************************************************/
+typedef u16 CsrWifiRouterPriority;
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP0      ((CsrWifiRouterPriority) 0x0000)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP1      ((CsrWifiRouterPriority) 0x0001)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP2      ((CsrWifiRouterPriority) 0x0002)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP3      ((CsrWifiRouterPriority) 0x0003)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP4      ((CsrWifiRouterPriority) 0x0004)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP5      ((CsrWifiRouterPriority) 0x0005)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP6      ((CsrWifiRouterPriority) 0x0006)
+#define CSR_WIFI_ROUTER_PRIORITY_QOS_UP7      ((CsrWifiRouterPriority) 0x0007)
+#define CSR_WIFI_ROUTER_PRIORITY_CONTENTION   ((CsrWifiRouterPriority) 0x8000)
+#define CSR_WIFI_ROUTER_PRIORITY_MANAGEMENT   ((CsrWifiRouterPriority) 0x8010)
+
+
+/* Downstream */
+#define CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST            (0x0000)
+
+#define CSR_WIFI_ROUTER_MA_PACKET_SUBSCRIBE_REQ           ((CsrWifiRouterPrim) (0x0000 + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_UNSUBSCRIBE_REQ         ((CsrWifiRouterPrim) (0x0001 + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_REQ                     ((CsrWifiRouterPrim) (0x0002 + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_RES                     ((CsrWifiRouterPrim) (0x0003 + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_CANCEL_REQ              ((CsrWifiRouterPrim) (0x0004 + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST))
+
+
+#define CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_HIGHEST           (0x0004 + CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST)
+
+/* Upstream */
+#define CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST              (0x0000 + CSR_PRIM_UPSTREAM)
+
+#define CSR_WIFI_ROUTER_MA_PACKET_SUBSCRIBE_CFM           ((CsrWifiRouterPrim)(0x0000 + CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_UNSUBSCRIBE_CFM         ((CsrWifiRouterPrim)(0x0001 + CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_CFM                     ((CsrWifiRouterPrim)(0x0002 + CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_ROUTER_MA_PACKET_IND                     ((CsrWifiRouterPrim)(0x0003 + CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST))
+
+#define CSR_WIFI_ROUTER_PRIM_UPSTREAM_HIGHEST             (0x0003 + CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST)
+
+#define CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT             (CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_HIGHEST + 1 - CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST)
+#define CSR_WIFI_ROUTER_PRIM_UPSTREAM_COUNT               (CSR_WIFI_ROUTER_PRIM_UPSTREAM_HIGHEST   + 1 - CSR_WIFI_ROUTER_PRIM_UPSTREAM_LOWEST)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketSubscribeReq
+
+  DESCRIPTION
+    A task can use this primitive to subscribe for a particular OUI/protocol
+    and transmit and receive frames matching the subscription.
+    NOTE: Multiple subscriptions for a given protocol and OUI will result in
+    the first subscription receiving the data and not the subsequent
+    subscriptions.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    encapsulation - Specifies the encapsulation type, which will be used for the
+                    subscription
+    protocol      - Together with the OUI, specifies the protocol, which a task
+                    wants to subscribe to
+    oui           - Specifies the OUI for the protocol, which a task wants to
+                    subscribe to
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent            common;
+    u16                  interfaceTag;
+    CsrWifiRouterEncapsulation encapsulation;
+    u16                  protocol;
+    u32                  oui;
+} CsrWifiRouterMaPacketSubscribeReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketUnsubscribeReq
+
+  DESCRIPTION
+    A task sends this primitive to unsubscribe a subscription
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u8        subscriptionHandle;
+} CsrWifiRouterMaPacketUnsubscribeReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketReq
+
+  DESCRIPTION
+    A task sends this primitive to transmit a frame.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+    frameLength        - Length of the frame to be sent in bytes
+    frame              - Pointer to the frame to be sent
+    freeFunction       - Pointer to function to be used to free the frame
+    priority           - Priority of the frame, which should be sent
+    hostTag            - An application shall set the bits b31..b28 using one of
+                         the CSR_WIFI_ROUTER_APP_TYPE_* masks. Bits b0..b27 can
+                         be used by the requestor without any restrictions, but
+                         the hostTag shall be unique so the hostTag for
+                         CSR_WIFI_ROUTER_APP _TYPE_OTHER should be constructured
+                         in the following way [ CSR_WIFI_ROUTER_APP_TYPE_OTHER
+                         (4 bits) | SubscriptionHandle (8 bits) | Sequence no.
+                         (20 bits) ]. If the hostTag is not unique, the
+                         behaviour of the system is unpredicatable with respect
+                         to data/management frame transfer.
+    cfmRequested       - Indicates if the requestor needs a confirm for packet
+                         requests sent under this subscription. If set to TRUE,
+                         the router will send a confirm, else it will not send
+                         any confirm
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    u8                       subscriptionHandle;
+    u16                      frameLength;
+    u8                      *frame;
+    CsrWifiRouterFrameFreeFunction freeFunction;
+    CsrWifiRouterPriority          priority;
+    u32                      hostTag;
+    u8                        cfmRequested;
+} CsrWifiRouterMaPacketReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketRes
+
+  DESCRIPTION
+    A task send this primitive to confirm the reception of the received
+    frame.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+    result             - Status of the operation
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u8        subscriptionHandle;
+    CsrResult       result;
+} CsrWifiRouterMaPacketRes;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketCancelReq
+
+  DESCRIPTION
+    This primitive is used to request cancellation of a previously send
+    CsrWifiRouterMaPacketReq.
+    The frame may already have been transmitted so there is no guarantees
+    that the CsrWifiRouterMaPacketCancelReq actually cancels the transmission
+    of the frame in question.
+    If the cancellation fails, the Router will send, if required,
+    CsrWifiRouterMaPacketCfm.
+    If the cancellation succeeds, the Router will not send
+    CsrWifiRouterMaPacketCfm.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    hostTag        - The hostTag for the frame, which should be cancelled.
+    priority       - Priority of the frame, which should be cancelled
+    peerMacAddress - Destination MAC address of the frame, which should be
+                     cancelled
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    u16             interfaceTag;
+    u32             hostTag;
+    CsrWifiRouterPriority priority;
+    CsrWifiMacAddress     peerMacAddress;
+} CsrWifiRouterMaPacketCancelReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketSubscribeCfm
+
+  DESCRIPTION
+    The router sends this primitive to confirm the result of the
+    subscription.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - Handle to the subscription
+                         This handle must be used in all subsequent requests
+    status             - Status of the operation
+    allocOffset        - Size of the offset for the frames of the subscription
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    u8        subscriptionHandle;
+    CsrResult       status;
+    u16       allocOffset;
+} CsrWifiRouterMaPacketSubscribeCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketUnsubscribeCfm
+
+  DESCRIPTION
+    The router sends this primitive to confirm the result of the
+    unsubscription.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Status of the operation
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiRouterMaPacketUnsubscribeCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketCfm
+
+  DESCRIPTION
+    The router sends the primitive to confirm the result of the transmission
+    of the packet of the corresponding CSR_WIFI_ROUTER MA_PACKET_REQ request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    result       - Status of the operation
+    hostTag      - The hostTrag will match the hostTag sent in the request.
+    rate         - Transmission/Reception rate
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       result;
+    u32       hostTag;
+    u16       rate;
+} CsrWifiRouterMaPacketCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiRouterMaPacketInd
+
+  DESCRIPTION
+    The router sends the primitive to a subscribed task when it receives a
+    frame matching the subscription.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    interfaceTag       - Interface Identifier; unique identifier of an interface
+    subscriptionHandle - The handle of the subscription
+    result             - Status of the operation
+    frameLength        - Length of the received frame in bytes
+    frame              - Pointer to the received frame
+    freeFunction       - Pointer to function to be used to free the frame
+    rssi               - Received signal strength indication in dBm
+    snr                - Signal to Noise Ratio
+    rate               - Transmission/Reception rate
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    u16                      interfaceTag;
+    u8                       subscriptionHandle;
+    CsrResult                      result;
+    u16                      frameLength;
+    u8                      *frame;
+    CsrWifiRouterFrameFreeFunction freeFunction;
+    s16                       rssi;
+    s16                       snr;
+    u16                      rate;
+} CsrWifiRouterMaPacketInd;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_PRIM_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_router_sef.c b/drivers/staging/csr/csr_wifi_router_sef.c
new file mode 100644
index 00000000000..45a10fb7729
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_sef.c
@@ -0,0 +1,19 @@
+/*****************************************************************************
+
+  (c) Cambridge Silicon Radio Limited 2010
+  Confidential information of CSR
+
+  Refer to LICENSE.txt included with this source for details
+  on the license terms.
+
+ *****************************************************************************/
+#include "csr_wifi_router_sef.h"
+
+const CsrWifiRouterStateHandlerType CsrWifiRouterDownstreamStateHandlers[CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT] =
+{
+    /* 0x0000 */ CsrWifiRouterMaPacketSubscribeReqHandler,
+    /* 0x0001 */ CsrWifiRouterMaPacketUnsubscribeReqHandler,
+    /* 0x0002 */ CsrWifiRouterMaPacketReqHandler,
+    /* 0x0003 */ CsrWifiRouterMaPacketResHandler,
+    /* 0x0004 */ CsrWifiRouterMaPacketCancelReqHandler,
+};
diff --git a/drivers/staging/csr/csr_wifi_router_sef.h b/drivers/staging/csr/csr_wifi_router_sef.h
new file mode 100644
index 00000000000..49dd158fa98
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_sef.h
@@ -0,0 +1,33 @@
+/*****************************************************************************
+
+  (c) Cambridge Silicon Radio Limited 2010
+  Confidential information of CSR
+
+  Refer to LICENSE.txt included with this source for details
+  on the license terms.
+
+ *****************************************************************************/
+#ifndef CSR_WIFI_ROUTER_SEF_CSR_WIFI_ROUTER_H__
+#define CSR_WIFI_ROUTER_SEF_CSR_WIFI_ROUTER_H__
+
+#include "csr_wifi_router_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    typedef void (*CsrWifiRouterStateHandlerType)(void* drvpriv, CsrWifiFsmEvent* msg);
+
+    extern const CsrWifiRouterStateHandlerType CsrWifiRouterDownstreamStateHandlers[CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_COUNT];
+
+    extern void CsrWifiRouterMaPacketSubscribeReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterMaPacketUnsubscribeReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterMaPacketReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterMaPacketResHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+    extern void CsrWifiRouterMaPacketCancelReqHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_SEF_CSR_WIFI_ROUTER_H__ */
diff --git a/drivers/staging/csr/csr_wifi_router_serialize.c b/drivers/staging/csr/csr_wifi_router_serialize.c
new file mode 100644
index 00000000000..4eccf5d6c28
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_serialize.c
@@ -0,0 +1,418 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_msgconv.h"
+#include "csr_wifi_router_prim.h"
+#include "csr_wifi_router_serialize.h"
+
+void CsrWifiRouterPfree(void *ptr)
+{
+    kfree(ptr);
+}
+
+
+size_t CsrWifiRouterMaPacketSubscribeReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 12) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiRouterEncapsulation primitive->encapsulation */
+    bufferSize += 2; /* u16 primitive->protocol */
+    bufferSize += 4; /* u32 primitive->oui */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketSubscribeReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketSubscribeReq *primitive = (CsrWifiRouterMaPacketSubscribeReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->encapsulation);
+    CsrUint16Ser(ptr, len, (u16) primitive->protocol);
+    CsrUint32Ser(ptr, len, (u32) primitive->oui);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketSubscribeReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketSubscribeReq *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketSubscribeReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->encapsulation, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->protocol, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->oui, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterMaPacketReqSizeof(void *msg)
+{
+    CsrWifiRouterMaPacketReq *primitive = (CsrWifiRouterMaPacketReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 20) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 1;                      /* u8 primitive->subscriptionHandle */
+    bufferSize += 2;                      /* u16 primitive->frameLength */
+    bufferSize += primitive->frameLength; /* u8 primitive->frame */
+    bufferSize += 4;                      /* CsrWifiRouterFrameFreeFunction primitive->freeFunction */
+    bufferSize += 2;                      /* CsrWifiRouterPriority primitive->priority */
+    bufferSize += 4;                      /* u32 primitive->hostTag */
+    bufferSize += 1;                      /* u8 primitive->cfmRequested */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketReq *primitive = (CsrWifiRouterMaPacketReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->subscriptionHandle);
+    CsrUint16Ser(ptr, len, (u16) primitive->frameLength);
+    if (primitive->frameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->frame, ((u16) (primitive->frameLength)));
+    }
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->freeFunction */
+    CsrUint16Ser(ptr, len, (u16) primitive->priority);
+    CsrUint32Ser(ptr, len, (u32) primitive->hostTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->cfmRequested);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketReq *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->subscriptionHandle, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->frameLength, buffer, &offset);
+    if (primitive->frameLength)
+    {
+        primitive->frame = kmalloc(primitive->frameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->frame, buffer, &offset, ((u16) (primitive->frameLength)));
+    }
+    else
+    {
+        primitive->frame = NULL;
+    }
+    primitive->freeFunction = NULL; /* Special for Function Pointers... */
+    offset += 4;
+    CsrUint16Des((u16 *) &primitive->priority, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->hostTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->cfmRequested, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiRouterMaPacketReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterMaPacketReq *primitive = (CsrWifiRouterMaPacketReq *) voidPrimitivePointer;
+    kfree(primitive->frame);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiRouterMaPacketResSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* u8 primitive->subscriptionHandle */
+    bufferSize += 2; /* CsrResult primitive->result */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketResSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketRes *primitive = (CsrWifiRouterMaPacketRes *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->subscriptionHandle);
+    CsrUint16Ser(ptr, len, (u16) primitive->result);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketResDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketRes *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketRes), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->subscriptionHandle, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterMaPacketCancelReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 17) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 4; /* u32 primitive->hostTag */
+    bufferSize += 2; /* CsrWifiRouterPriority primitive->priority */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketCancelReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketCancelReq *primitive = (CsrWifiRouterMaPacketCancelReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint32Ser(ptr, len, (u32) primitive->hostTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->priority);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketCancelReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketCancelReq *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketCancelReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->hostTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->priority, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterMaPacketSubscribeCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* u8 primitive->subscriptionHandle */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 2; /* u16 primitive->allocOffset */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketSubscribeCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketSubscribeCfm *primitive = (CsrWifiRouterMaPacketSubscribeCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->subscriptionHandle);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->allocOffset);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketSubscribeCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketSubscribeCfm *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketSubscribeCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->subscriptionHandle, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->allocOffset, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterMaPacketUnsubscribeCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketUnsubscribeCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketUnsubscribeCfm *primitive = (CsrWifiRouterMaPacketUnsubscribeCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketUnsubscribeCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketUnsubscribeCfm *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketUnsubscribeCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterMaPacketCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->result */
+    bufferSize += 4; /* u32 primitive->hostTag */
+    bufferSize += 2; /* u16 primitive->rate */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketCfm *primitive = (CsrWifiRouterMaPacketCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->result);
+    CsrUint32Ser(ptr, len, (u32) primitive->hostTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->rate);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketCfm *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->hostTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->rate, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiRouterMaPacketIndSizeof(void *msg)
+{
+    CsrWifiRouterMaPacketInd *primitive = (CsrWifiRouterMaPacketInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 21) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 1;                      /* u8 primitive->subscriptionHandle */
+    bufferSize += 2;                      /* CsrResult primitive->result */
+    bufferSize += 2;                      /* u16 primitive->frameLength */
+    bufferSize += primitive->frameLength; /* u8 primitive->frame */
+    bufferSize += 4;                      /* CsrWifiRouterFrameFreeFunction primitive->freeFunction */
+    bufferSize += 2;                      /* s16 primitive->rssi */
+    bufferSize += 2;                      /* s16 primitive->snr */
+    bufferSize += 2;                      /* u16 primitive->rate */
+    return bufferSize;
+}
+
+
+u8* CsrWifiRouterMaPacketIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiRouterMaPacketInd *primitive = (CsrWifiRouterMaPacketInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->subscriptionHandle);
+    CsrUint16Ser(ptr, len, (u16) primitive->result);
+    CsrUint16Ser(ptr, len, (u16) primitive->frameLength);
+    if (primitive->frameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->frame, ((u16) (primitive->frameLength)));
+    }
+    CsrUint32Ser(ptr, len, 0); /* Special for Function Pointers... primitive->freeFunction */
+    CsrUint16Ser(ptr, len, (u16) primitive->rssi);
+    CsrUint16Ser(ptr, len, (u16) primitive->snr);
+    CsrUint16Ser(ptr, len, (u16) primitive->rate);
+    return(ptr);
+}
+
+
+void* CsrWifiRouterMaPacketIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiRouterMaPacketInd *primitive = kmalloc(sizeof(CsrWifiRouterMaPacketInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->subscriptionHandle, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->frameLength, buffer, &offset);
+    if (primitive->frameLength)
+    {
+        primitive->frame = kmalloc(primitive->frameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->frame, buffer, &offset, ((u16) (primitive->frameLength)));
+    }
+    else
+    {
+        primitive->frame = NULL;
+    }
+    primitive->freeFunction = NULL; /* Special for Function Pointers... */
+    offset += 4;
+    CsrUint16Des((u16 *) &primitive->rssi, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->snr, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->rate, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiRouterMaPacketIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiRouterMaPacketInd *primitive = (CsrWifiRouterMaPacketInd *) voidPrimitivePointer;
+    kfree(primitive->frame);
+    kfree(primitive);
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_router_serialize.h b/drivers/staging/csr/csr_wifi_router_serialize.h
new file mode 100644
index 00000000000..07e21b2b436
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_serialize.h
@@ -0,0 +1,75 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_SERIALIZE_H__
+#define CSR_WIFI_ROUTER_SERIALIZE_H__
+
+#include "csr_wifi_msgconv.h"
+#include "csr_wifi_router_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern void CsrWifiRouterPfree(void *ptr);
+
+extern u8* CsrWifiRouterMaPacketSubscribeReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketSubscribeReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketSubscribeReqSizeof(void *msg);
+#define CsrWifiRouterMaPacketSubscribeReqSerFree CsrWifiRouterPfree
+
+#define CsrWifiRouterMaPacketUnsubscribeReqSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiRouterMaPacketUnsubscribeReqDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiRouterMaPacketUnsubscribeReqSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiRouterMaPacketUnsubscribeReqSerFree CsrWifiRouterPfree
+
+extern u8* CsrWifiRouterMaPacketReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketReqSizeof(void *msg);
+extern void CsrWifiRouterMaPacketReqSerFree(void *msg);
+
+extern u8* CsrWifiRouterMaPacketResSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketResDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketResSizeof(void *msg);
+#define CsrWifiRouterMaPacketResSerFree CsrWifiRouterPfree
+
+extern u8* CsrWifiRouterMaPacketCancelReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketCancelReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketCancelReqSizeof(void *msg);
+#define CsrWifiRouterMaPacketCancelReqSerFree CsrWifiRouterPfree
+
+extern u8* CsrWifiRouterMaPacketSubscribeCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketSubscribeCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketSubscribeCfmSizeof(void *msg);
+#define CsrWifiRouterMaPacketSubscribeCfmSerFree CsrWifiRouterPfree
+
+extern u8* CsrWifiRouterMaPacketUnsubscribeCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketUnsubscribeCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketUnsubscribeCfmSizeof(void *msg);
+#define CsrWifiRouterMaPacketUnsubscribeCfmSerFree CsrWifiRouterPfree
+
+extern u8* CsrWifiRouterMaPacketCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketCfmSizeof(void *msg);
+#define CsrWifiRouterMaPacketCfmSerFree CsrWifiRouterPfree
+
+extern u8* CsrWifiRouterMaPacketIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiRouterMaPacketIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiRouterMaPacketIndSizeof(void *msg);
+extern void CsrWifiRouterMaPacketIndSerFree(void *msg);
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* CSR_WIFI_ROUTER_SERIALIZE_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_router_task.h b/drivers/staging/csr/csr_wifi_router_task.h
new file mode 100644
index 00000000000..4e51fae4cda
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_task.h
@@ -0,0 +1,33 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_ROUTER_TASK_H__
+#define CSR_WIFI_ROUTER_TASK_H__
+
+#include "csr_sched.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CSR_WIFI_ROUTER_LOG_ID 0x1201FFFF
+extern CsrSchedQid CSR_WIFI_ROUTER_IFACEQUEUE;
+void CsrWifiRouterInit(void **gash);
+void CsrWifiRouterDeinit(void **gash);
+void CsrWifiRouterHandler(void **gash);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_TASK_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_router_transport.c b/drivers/staging/csr/csr_wifi_router_transport.c
new file mode 100644
index 00000000000..e905ead9389
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_router_transport.c
@@ -0,0 +1,199 @@
+/** @file router_transport.c
+ *
+ *
+ * Copyright (C) Cambridge Silicon Radio Ltd 2006-2010. All rights reserved.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ ****************************************************************************/
+
+#include "unifi_priv.h"
+
+#include "csr_sched.h"
+#include "csr_msgconv.h"
+
+#include "sme_userspace.h"
+
+#include "csr_wifi_hostio_prim.h"
+#include "csr_wifi_router_lib.h"
+#include "csr_wifi_router_sef.h"
+#include "csr_wifi_router_converter_init.h"
+#include "csr_wifi_router_ctrl_lib.h"
+#include "csr_wifi_router_ctrl_sef.h"
+#include "csr_wifi_router_ctrl_converter_init.h"
+#include "csr_wifi_sme_prim.h"
+#include "csr_wifi_sme_sef.h"
+#include "csr_wifi_sme_converter_init.h"
+#ifdef CSR_SUPPORT_WEXT
+#ifdef CSR_SUPPORT_WEXT_AP
+#include "csr_wifi_nme_ap_prim.h"
+#include "csr_wifi_nme_ap_sef.h"
+#include "csr_wifi_nme_ap_converter_init.h"
+#endif
+#endif
+
+static unifi_priv_t *drvpriv = NULL;
+void CsrWifiRouterTransportInit(unifi_priv_t *priv)
+{
+    unifi_trace(priv, UDBG1, "CsrWifiRouterTransportInit: \n");
+
+    drvpriv = priv;
+    (void)CsrMsgConvInit();
+    CsrWifiRouterConverterInit();
+    CsrWifiRouterCtrlConverterInit();
+    CsrWifiSmeConverterInit();
+#ifdef CSR_SUPPORT_WEXT
+#ifdef CSR_SUPPORT_WEXT_AP
+    CsrWifiNmeApConverterInit();
+#endif
+#endif
+}
+
+void CsrWifiRouterTransportRecv(unifi_priv_t *priv, u8* buffer, size_t bufferLength)
+{
+    CsrMsgConvMsgEntry* msgEntry;
+    u16 primType;
+    CsrSchedQid src;
+    CsrSchedQid dest;
+    u16 msgType;
+    size_t offset = 0;
+    CsrWifiFsmEvent* msg;
+
+    /* Decode the prim and message type */
+    CsrUint16Des(&primType, buffer, &offset);
+    CsrUint16Des(&src, buffer, &offset);
+    CsrUint16Des(&dest, buffer, &offset);
+    CsrUint16Des(&msgType, buffer, &offset);
+    offset -= 2; /* Adjust as the Deserialise Function will read this as well */
+
+    unifi_trace(priv, UDBG4, "CsrWifiRouterTransportRecv: primType=0x%.4X, msgType=0x%.4X, bufferLength=%d\n",
+                primType, msgType, bufferLength);
+
+    /* Special handling for HOSTIO messages.... */
+    if (primType == CSR_WIFI_HOSTIO_PRIM)
+    {
+        CsrWifiRouterCtrlHipReq req = {{CSR_WIFI_ROUTER_CTRL_HIP_REQ, CSR_WIFI_ROUTER_CTRL_PRIM, dest, src, NULL}, 0, NULL, 0, NULL, 0, NULL};
+
+        req.mlmeCommandLength = bufferLength;
+        req.mlmeCommand = buffer;
+
+        offset += 8;/* Skip the id, src, dest and slot number */
+        CsrUint16Des(&req.dataRef1Length, buffer, &offset);
+        offset += 2; /* Skip the slot number */
+        CsrUint16Des(&req.dataRef2Length, buffer, &offset);
+
+        if (req.dataRef1Length)
+        {
+            u16 dr1Offset = (bufferLength - req.dataRef2Length) - req.dataRef1Length;
+            req.dataRef1 = &buffer[dr1Offset];
+        }
+
+        if (req.dataRef2Length)
+        {
+            u16 dr2Offset = bufferLength - req.dataRef2Length;
+            req.dataRef2 = &buffer[dr2Offset];
+        }
+
+        /* Copy the hip data but strip off the prim type */
+        req.mlmeCommandLength -= (req.dataRef1Length + req.dataRef2Length + 6);
+        req.mlmeCommand = &buffer[6];
+
+        CsrWifiRouterCtrlHipReqHandler(priv, &req.common);
+        return;
+    }
+
+    msgEntry = CsrMsgConvFindEntry(primType, msgType);
+    if (!msgEntry)
+    {
+        unifi_error(priv, "CsrWifiRouterTransportDeserialiseAndSend can not process the message. primType=0x%.4X, msgType=0x%.4X\n",
+                    primType, msgType);
+        dump(buffer, bufferLength);
+        return;
+    }
+
+    msg = (CsrWifiFsmEvent*)(msgEntry->deserFunc)(&buffer[offset], bufferLength - offset);
+
+    msg->primtype = primType;
+    msg->type = msgType;
+    msg->source = src;
+    msg->destination = dest;
+
+    switch(primType)
+    {
+    case CSR_WIFI_ROUTER_CTRL_PRIM:
+        CsrWifiRouterCtrlDownstreamStateHandlers[msg->type - CSR_WIFI_ROUTER_CTRL_PRIM_DOWNSTREAM_LOWEST](priv, msg);
+        CsrWifiRouterCtrlFreeDownstreamMessageContents(CSR_WIFI_ROUTER_CTRL_PRIM, msg);
+        break;
+    case CSR_WIFI_ROUTER_PRIM:
+        CsrWifiRouterDownstreamStateHandlers[msg->type - CSR_WIFI_ROUTER_PRIM_DOWNSTREAM_LOWEST](priv, msg);
+        CsrWifiRouterFreeDownstreamMessageContents(CSR_WIFI_ROUTER_PRIM, msg);
+        break;
+        case CSR_WIFI_SME_PRIM:
+            CsrWifiSmeUpstreamStateHandlers[msg->type - CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST](priv, msg);
+            CsrWifiSmeFreeUpstreamMessageContents(CSR_WIFI_SME_PRIM, msg);
+            break;
+#ifdef CSR_SUPPORT_WEXT
+#ifdef CSR_SUPPORT_WEXT_AP
+        case CSR_WIFI_NME_AP_PRIM:
+            CsrWifiNmeApUpstreamStateHandlers(priv, msg);
+            CsrWifiNmeApFreeUpstreamMessageContents(CSR_WIFI_NME_AP_PRIM, msg);
+            break;
+#endif
+#endif
+        default:
+            unifi_error(priv, "CsrWifiRouterTransportDeserialiseAndSend unhandled prim type 0x%.4X\n", primType);
+            break;
+    }
+    kfree(msg);
+}
+
+static void CsrWifiRouterTransportSerialiseAndSend(u16 primType, void* msg)
+{
+    CsrWifiFsmEvent* evt = (CsrWifiFsmEvent*)msg;
+    CsrMsgConvMsgEntry* msgEntry;
+    size_t msgSize;
+    size_t encodeBufferLen = 0;
+    size_t offset = 0;
+    u8* encodeBuffer;
+
+    unifi_trace(drvpriv, UDBG4, "CsrWifiRouterTransportSerialiseAndSend: primType=0x%.4X, msgType=0x%.4X\n",
+                primType, evt->type);
+
+    msgEntry = CsrMsgConvFindEntry(primType, evt->type);
+    if (!msgEntry)
+    {
+        unifi_error(drvpriv, "CsrWifiRouterTransportSerialiseAndSend can not process the message. primType=0x%.4X, msgType=0x%.4X\n",
+                    primType, evt->type);
+        return;
+    }
+
+    msgSize = 6 + (msgEntry->sizeofFunc)((void*)msg);
+
+    encodeBuffer = kmalloc(msgSize, GFP_KERNEL);
+
+    /* Encode PrimType */
+    CsrUint16Ser(encodeBuffer, &encodeBufferLen, primType);
+    CsrUint16Ser(encodeBuffer, &encodeBufferLen, evt->source);
+    CsrUint16Ser(encodeBuffer, &encodeBufferLen, evt->destination);
+
+    (void)(msgEntry->serFunc)(&encodeBuffer[encodeBufferLen], &offset, msg);
+    encodeBufferLen += offset;
+
+    uf_sme_queue_message(drvpriv, encodeBuffer, encodeBufferLen);
+
+    /* Do not use msgEntry->freeFunc because the memory is owned by the driver */
+    kfree(msg);
+}
+
+#if defined(CSR_LOG_ENABLE) && defined(CSR_LOG_INCLUDE_FILE_NAME_AND_LINE_NUMBER)
+void CsrSchedMessagePutStringLog(CsrSchedQid q, u16 mi, void *mv, u32 line, char *file)
+#else
+void CsrSchedMessagePut(CsrSchedQid q, u16 mi, void *mv)
+#endif
+{
+    CsrWifiFsmEvent* evt = (CsrWifiFsmEvent*)mv;
+    evt->destination = q;
+    CsrWifiRouterTransportSerialiseAndSend(mi, mv);
+}
+
diff --git a/drivers/staging/csr/csr_wifi_serialize_primitive_types.c b/drivers/staging/csr/csr_wifi_serialize_primitive_types.c
new file mode 100644
index 00000000000..dd93d006119
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_serialize_primitive_types.c
@@ -0,0 +1,256 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include "csr_macro.h"
+#include "csr_msgconv.h"
+#include "csr_wifi_msgconv.h"
+#include "csr_wifi_lib.h"
+
+void CsrUint24Des(u32 *v, u8 *buffer, size_t *offset)
+{
+    u32 val;
+
+    val = ((buffer[(*offset) + 2] << 16) |
+           (buffer[(*offset) + 1] << 8) |
+           (buffer[(*offset)]));
+
+    *offset += 3;
+    *v = val;
+}
+
+
+/* Big endian :e.g WSC, TCLAS */
+void CsrUint16DesBigEndian(u16 *v, u8 *buffer, size_t *offset)
+{
+    u16 val;
+
+    val = (buffer[(*offset)] << 8) | (buffer[(*offset) + 1]);
+    *offset += 2;
+
+    *v = val;
+}
+
+
+void CsrUint24DesBigEndian(u32 *v, u8 *buffer, size_t *offset)
+{
+    u32 val;
+
+    val = ((buffer[(*offset)] << 16) |
+           (buffer[(*offset) + 1] << 8) |
+           (buffer[(*offset) + 2]));
+
+    *offset += 3;
+    *v = val;
+}
+
+
+void CsrUint32DesBigEndian(u32 *v, u8 *buffer, size_t *offset)
+{
+    u32 val;
+
+    val = ((buffer[(*offset)] << 24) |
+           (buffer[(*offset) + 1] << 16) |
+           (buffer[(*offset) + 2] << 8) |
+           (buffer[(*offset) + 3]));
+
+    *offset += 4;
+    *v = val;
+}
+
+
+void CsrUint24Ser(u8 *ptr, size_t *len, u32 v)
+{
+    ptr[(*len) + 2] = (u8)((v & 0x00ff0000) >> 16);
+    ptr[(*len) + 1] = (u8)((v & 0x0000ff00) >> 8);
+    ptr[(*len)]     = (u8)((v & 0x000000ff));
+
+    *len += 3;
+}
+
+
+/* Big endian :e.g WSC, TCLAS */
+void CsrUint16SerBigEndian(u8 *ptr, size_t *len, u16 v)
+{
+    ptr[(*len)] = (u8)((v & 0xff00) >> 8);
+    ptr[(*len) + 1] = (u8)((v & 0x00ff));
+
+    *len += 2;
+}
+
+
+void CsrUint32SerBigEndian(u8 *ptr, size_t *len, u32 v)
+{
+    ptr[(*len)] = (u8)((v & 0xff000000) >> 24);
+    ptr[(*len) + 1] = (u8)((v & 0x00ff0000) >> 16);
+    ptr[(*len) + 2] = (u8)((v & 0x0000ff00) >> 8);
+    ptr[(*len) + 3] = (u8)((v & 0x000000ff));
+
+    *len += 4;
+}
+
+
+void CsrUint24SerBigEndian(u8 *ptr, size_t *len, u32 v)
+{
+    ptr[(*len)] = (u8)((v & 0x00ff0000) >> 16);
+    ptr[(*len) + 1] = (u8)((v & 0x0000ff00) >> 8);
+    ptr[(*len) + 2] = (u8)((v & 0x000000ff));
+
+    *len += 3;
+}
+
+
+size_t CsrWifiEventSizeof(void *msg)
+{
+    return 2;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventSizeof);
+
+u8* CsrWifiEventSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiFsmEvent *primitive = (CsrWifiFsmEvent *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->type);
+    return(ptr);
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventSer);
+
+void* CsrWifiEventDes(u8 *buffer, size_t length)
+{
+    CsrWifiFsmEvent *primitive = kmalloc(sizeof(CsrWifiFsmEvent), GFP_KERNEL);
+    size_t offset = 0;
+    CsrUint16Des(&primitive->type, buffer, &offset);
+
+    return primitive;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventDes);
+
+size_t CsrWifiEventCsrUint8Sizeof(void *msg)
+{
+    return 3;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint8Sizeof);
+
+u8* CsrWifiEventCsrUint8Ser(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiEventCsrUint8 *primitive = (CsrWifiEventCsrUint8 *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, primitive->value);
+    return(ptr);
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint8Ser);
+
+
+void* CsrWifiEventCsrUint8Des(u8 *buffer, size_t length)
+{
+    CsrWifiEventCsrUint8 *primitive = kmalloc(sizeof(CsrWifiEventCsrUint8), GFP_KERNEL);
+
+    size_t offset = 0;
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des(&primitive->value, buffer, &offset);
+
+    return primitive;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint8Des);
+
+
+size_t CsrWifiEventCsrUint16Sizeof(void *msg)
+{
+    return 4;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint16Sizeof);
+
+
+u8* CsrWifiEventCsrUint16Ser(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiEventCsrUint16 *primitive = (CsrWifiEventCsrUint16 *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, primitive->value);
+    return(ptr);
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint16Ser);
+
+void* CsrWifiEventCsrUint16Des(u8 *buffer, size_t length)
+{
+    CsrWifiEventCsrUint16 *primitive = kmalloc(sizeof(CsrWifiEventCsrUint16), GFP_KERNEL);
+
+    size_t offset = 0;
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des(&primitive->value, buffer, &offset);
+
+    return primitive;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint16Des);
+
+
+size_t CsrWifiEventCsrUint32Sizeof(void *msg)
+{
+    return 6;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint32Sizeof);
+
+u8* CsrWifiEventCsrUint32Ser(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiEventCsrUint32 *primitive = (CsrWifiEventCsrUint32 *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint32Ser(ptr, len, primitive->value);
+    return(ptr);
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint32Ser);
+
+
+void* CsrWifiEventCsrUint32Des(u8 *buffer, size_t length)
+{
+    CsrWifiEventCsrUint32 *primitive = kmalloc(sizeof(CsrWifiEventCsrUint32), GFP_KERNEL);
+
+    size_t offset = 0;
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint32Des(&primitive->value, buffer, &offset);
+
+    return primitive;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint32Des);
+
+size_t CsrWifiEventCsrUint16CsrUint8Sizeof(void *msg)
+{
+    return 5;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint16CsrUint8Sizeof);
+
+u8* CsrWifiEventCsrUint16CsrUint8Ser(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiEventCsrUint16CsrUint8 *primitive = (CsrWifiEventCsrUint16CsrUint8 *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, primitive->value16);
+    CsrUint8Ser(ptr, len, primitive->value8);
+    return(ptr);
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint16CsrUint8Ser);
+
+
+void* CsrWifiEventCsrUint16CsrUint8Des(u8 *buffer, size_t length)
+{
+    CsrWifiEventCsrUint16CsrUint8 *primitive = kmalloc(sizeof(CsrWifiEventCsrUint16CsrUint8), GFP_KERNEL);
+
+    size_t offset = 0;
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des(&primitive->value16, buffer, &offset);
+    CsrUint8Des(&primitive->value8, buffer, &offset);
+
+    return primitive;
+}
+EXPORT_SYMBOL_GPL(CsrWifiEventCsrUint16CsrUint8Des);
+
+
diff --git a/drivers/staging/csr/csr_wifi_sme_ap_lib.h b/drivers/staging/csr/csr_wifi_sme_ap_lib.h
new file mode 100644
index 00000000000..350cb9ec301
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_ap_lib.h
@@ -0,0 +1,783 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_AP_LIB_H__
+#define CSR_WIFI_SME_AP_LIB_H__
+
+#include "csr_sched.h"
+#include "csr_macro.h"
+#include "csr_msg_transport.h"
+
+#include "csr_wifi_lib.h"
+
+#include "csr_wifi_sme_ap_prim.h"
+#include "csr_wifi_sme_task.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_AP_ENABLE
+#error CSR_WIFI_AP_ENABLE MUST be defined inorder to use csr_wifi_sme_ap_lib.h
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiSmeApFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_SME_AP upstream message. Does not
+ *      free the message itself, and can only be used for upstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_SME_AP upstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiSmeApFreeUpstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiSmeApFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_SME_AP downstream message. Does not
+ *      free the message itself, and can only be used for downstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_SME_AP downstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiSmeApFreeDownstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ * Enum to string functions
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiSmeApAccessTypeToString(CsrWifiSmeApAccessType value);
+const char* CsrWifiSmeApAuthSupportToString(CsrWifiSmeApAuthSupport value);
+const char* CsrWifiSmeApAuthTypeToString(CsrWifiSmeApAuthType value);
+const char* CsrWifiSmeApDirectionToString(CsrWifiSmeApDirection value);
+const char* CsrWifiSmeApPhySupportToString(CsrWifiSmeApPhySupport value);
+const char* CsrWifiSmeApTypeToString(CsrWifiSmeApType value);
+
+
+/*----------------------------------------------------------------------------*
+ * CsrPrim Type toString function.
+ * Converts a message type to the String name of the Message
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiSmeApPrimTypeToString(CsrPrim msgType);
+
+/*----------------------------------------------------------------------------*
+ * Lookup arrays for PrimType name Strings
+ *----------------------------------------------------------------------------*/
+extern const char *CsrWifiSmeApUpstreamPrimNames[CSR_WIFI_SME_AP_PRIM_UPSTREAM_COUNT];
+extern const char *CsrWifiSmeApDownstreamPrimNames[CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_COUNT];
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApActiveBaGetReqSend
+
+  DESCRIPTION
+    This primitive used to retrieve information related to the active block
+    ack sessions
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+
+*******************************************************************************/
+#define CsrWifiSmeApActiveBaGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApActiveBaGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_ACTIVE_BA_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeApActiveBaGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeApActiveBaGetReq *msg__; \
+        CsrWifiSmeApActiveBaGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApActiveBaGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeApActiveBaGetReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApActiveBaGetCfmSend
+
+  DESCRIPTION
+    This primitive carries the information related to the active ba sessions
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    interfaceTag     -
+    status           - Reports the result of the request
+    activeBaCount    - Number of active block ack session
+    activeBaSessions - Points to a buffer containing an array of
+                       CsrWifiSmeApBaSession structures.
+
+*******************************************************************************/
+#define CsrWifiSmeApActiveBaGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, activeBaCount__, activeBaSessions__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApActiveBaGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_ACTIVE_BA_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->activeBaCount = (activeBaCount__); \
+    msg__->activeBaSessions = (activeBaSessions__);
+
+#define CsrWifiSmeApActiveBaGetCfmSendTo(dst__, src__, interfaceTag__, status__, activeBaCount__, activeBaSessions__) \
+    { \
+        CsrWifiSmeApActiveBaGetCfm *msg__; \
+        CsrWifiSmeApActiveBaGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, activeBaCount__, activeBaSessions__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApActiveBaGetCfmSend(dst__, interfaceTag__, status__, activeBaCount__, activeBaSessions__) \
+    CsrWifiSmeApActiveBaGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, activeBaCount__, activeBaSessions__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBaDeleteReqSend
+
+  DESCRIPTION
+    This primitive is used to delete an active block ack session
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+    reason       -
+    baSession    - BA session to be deleted
+
+*******************************************************************************/
+#define CsrWifiSmeApBaDeleteReqCreate(msg__, dst__, src__, interfaceTag__, reason__, baSession__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApBaDeleteReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_BA_DELETE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->reason = (reason__); \
+    msg__->baSession = (baSession__);
+
+#define CsrWifiSmeApBaDeleteReqSendTo(dst__, src__, interfaceTag__, reason__, baSession__) \
+    { \
+        CsrWifiSmeApBaDeleteReq *msg__; \
+        CsrWifiSmeApBaDeleteReqCreate(msg__, dst__, src__, interfaceTag__, reason__, baSession__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApBaDeleteReqSend(src__, interfaceTag__, reason__, baSession__) \
+    CsrWifiSmeApBaDeleteReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__, reason__, baSession__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBaDeleteCfmSend
+
+  DESCRIPTION
+    This primitive confirms the BA is deleted
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag -
+    status       - Reports the result of the request
+    baSession    - deleted BA session
+
+*******************************************************************************/
+#define CsrWifiSmeApBaDeleteCfmCreate(msg__, dst__, src__, interfaceTag__, status__, baSession__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApBaDeleteCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_BA_DELETE_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->baSession = (baSession__);
+
+#define CsrWifiSmeApBaDeleteCfmSendTo(dst__, src__, interfaceTag__, status__, baSession__) \
+    { \
+        CsrWifiSmeApBaDeleteCfm *msg__; \
+        CsrWifiSmeApBaDeleteCfmCreate(msg__, dst__, src__, interfaceTag__, status__, baSession__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApBaDeleteCfmSend(dst__, interfaceTag__, status__, baSession__) \
+    CsrWifiSmeApBaDeleteCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, baSession__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStartReqSend
+
+  DESCRIPTION
+    This primitive requests the SME to start AP or GO functionality
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag    -
+    initialPresence - Set to 0, if Not in a group fomration phase, set to 1 ,
+                      during group formation phase
+    apType          - apType : Legacy AP or P2PGO
+    cloakSsid       - cloakSsid flag.
+    ssid            - ssid.
+    ifIndex         - Radio Interface
+    channel         - channel.
+    maxConnections  - Maximum Stations + P2PClients allowed
+    apCredentials   - AP security credeitals used to advertise in beacon /probe
+                      response
+    smeApConfig     - AP configuration
+    p2pGoParam      - P2P specific GO parameters. Ignored if it is a leagacy AP
+
+*******************************************************************************/
+#define CsrWifiSmeApBeaconingStartReqCreate(msg__, dst__, src__, interfaceTag__, initialPresence__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, maxConnections__, apCredentials__, smeApConfig__, p2pGoParam__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApBeaconingStartReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_BEACONING_START_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->initialPresence = (initialPresence__); \
+    msg__->apType = (apType__); \
+    msg__->cloakSsid = (cloakSsid__); \
+    msg__->ssid = (ssid__); \
+    msg__->ifIndex = (ifIndex__); \
+    msg__->channel = (channel__); \
+    msg__->maxConnections = (maxConnections__); \
+    msg__->apCredentials = (apCredentials__); \
+    msg__->smeApConfig = (smeApConfig__); \
+    msg__->p2pGoParam = (p2pGoParam__);
+
+#define CsrWifiSmeApBeaconingStartReqSendTo(dst__, src__, interfaceTag__, initialPresence__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, maxConnections__, apCredentials__, smeApConfig__, p2pGoParam__) \
+    { \
+        CsrWifiSmeApBeaconingStartReq *msg__; \
+        CsrWifiSmeApBeaconingStartReqCreate(msg__, dst__, src__, interfaceTag__, initialPresence__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, maxConnections__, apCredentials__, smeApConfig__, p2pGoParam__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApBeaconingStartReqSend(src__, interfaceTag__, initialPresence__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, maxConnections__, apCredentials__, smeApConfig__, p2pGoParam__) \
+    CsrWifiSmeApBeaconingStartReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__, initialPresence__, apType__, cloakSsid__, ssid__, ifIndex__, channel__, maxConnections__, apCredentials__, smeApConfig__, p2pGoParam__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStartCfmSend
+
+  DESCRIPTION
+    This primitive confirms the completion of the request along with the
+    status
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag -
+    status       -
+    secIeLength  -
+    secIe        -
+
+*******************************************************************************/
+#define CsrWifiSmeApBeaconingStartCfmCreate(msg__, dst__, src__, interfaceTag__, status__, secIeLength__, secIe__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApBeaconingStartCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_BEACONING_START_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->secIeLength = (secIeLength__); \
+    msg__->secIe = (secIe__);
+
+#define CsrWifiSmeApBeaconingStartCfmSendTo(dst__, src__, interfaceTag__, status__, secIeLength__, secIe__) \
+    { \
+        CsrWifiSmeApBeaconingStartCfm *msg__; \
+        CsrWifiSmeApBeaconingStartCfmCreate(msg__, dst__, src__, interfaceTag__, status__, secIeLength__, secIe__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApBeaconingStartCfmSend(dst__, interfaceTag__, status__, secIeLength__, secIe__) \
+    CsrWifiSmeApBeaconingStartCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, secIeLength__, secIe__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStopReqSend
+
+  DESCRIPTION
+    This primitive requests the SME to STOP AP or P2PGO operation
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+
+*******************************************************************************/
+#define CsrWifiSmeApBeaconingStopReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApBeaconingStopReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_BEACONING_STOP_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeApBeaconingStopReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeApBeaconingStopReq *msg__; \
+        CsrWifiSmeApBeaconingStopReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApBeaconingStopReqSend(src__, interfaceTag__) \
+    CsrWifiSmeApBeaconingStopReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStopCfmSend
+
+  DESCRIPTION
+    This primitive confirms AP or P2PGO operation is terminated
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiSmeApBeaconingStopCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApBeaconingStopCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_BEACONING_STOP_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeApBeaconingStopCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeApBeaconingStopCfm *msg__; \
+        CsrWifiSmeApBeaconingStopCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApBeaconingStopCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeApBeaconingStopCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApErrorIndSend
+
+  DESCRIPTION
+    This primitve is sent by SME to indicate some error in AP operationi
+    after AP operations were started successfully and continuing the AP
+    operation may lead to undesired behaviour. It is the responsibility of
+    the upper layers to stop AP operation if needed
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Range 0-1
+    apType       -
+    status       - Contains the error status
+
+*******************************************************************************/
+#define CsrWifiSmeApErrorIndCreate(msg__, dst__, src__, interfaceTag__, apType__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApErrorInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_ERROR_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->apType = (apType__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeApErrorIndSendTo(dst__, src__, interfaceTag__, apType__, status__) \
+    { \
+        CsrWifiSmeApErrorInd *msg__; \
+        CsrWifiSmeApErrorIndCreate(msg__, dst__, src__, interfaceTag__, apType__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApErrorIndSend(dst__, interfaceTag__, apType__, status__) \
+    CsrWifiSmeApErrorIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, apType__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaConnectStartIndSend
+
+  DESCRIPTION
+    This primitive indicates that a stations request to join the group/BSS is
+    accepted
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   -
+    peerMacAddress -
+
+*******************************************************************************/
+#define CsrWifiSmeApStaConnectStartIndCreate(msg__, dst__, src__, interfaceTag__, peerMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApStaConnectStartInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_STA_CONNECT_START_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->peerMacAddress = (peerMacAddress__);
+
+#define CsrWifiSmeApStaConnectStartIndSendTo(dst__, src__, interfaceTag__, peerMacAddress__) \
+    { \
+        CsrWifiSmeApStaConnectStartInd *msg__; \
+        CsrWifiSmeApStaConnectStartIndCreate(msg__, dst__, src__, interfaceTag__, peerMacAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApStaConnectStartIndSend(dst__, interfaceTag__, peerMacAddress__) \
+    CsrWifiSmeApStaConnectStartIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, peerMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaDisconnectReqSend
+
+  DESCRIPTION
+    This primitive tells SME to deauth ot disassociate a particular station
+    within BSS
+
+  PARAMETERS
+    queue          - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag   -
+    deauthReason   -
+    disassocReason -
+    peerMacaddress -
+    keepBlocking   - If TRUE, the station is blocked. If FALSE and the station
+                     is connected, disconnect the station. If FALSE and the
+                     station is not connected, no action is taken.
+
+*******************************************************************************/
+#define CsrWifiSmeApStaDisconnectReqCreate(msg__, dst__, src__, interfaceTag__, deauthReason__, disassocReason__, peerMacaddress__, keepBlocking__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApStaDisconnectReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_STA_DISCONNECT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->deauthReason = (deauthReason__); \
+    msg__->disassocReason = (disassocReason__); \
+    msg__->peerMacaddress = (peerMacaddress__); \
+    msg__->keepBlocking = (keepBlocking__);
+
+#define CsrWifiSmeApStaDisconnectReqSendTo(dst__, src__, interfaceTag__, deauthReason__, disassocReason__, peerMacaddress__, keepBlocking__) \
+    { \
+        CsrWifiSmeApStaDisconnectReq *msg__; \
+        CsrWifiSmeApStaDisconnectReqCreate(msg__, dst__, src__, interfaceTag__, deauthReason__, disassocReason__, peerMacaddress__, keepBlocking__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApStaDisconnectReqSend(src__, interfaceTag__, deauthReason__, disassocReason__, peerMacaddress__, keepBlocking__) \
+    CsrWifiSmeApStaDisconnectReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__, deauthReason__, disassocReason__, peerMacaddress__, keepBlocking__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaDisconnectCfmSend
+
+  DESCRIPTION
+    This primitive confirms the station is disconnected
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   -
+    status         -
+    peerMacaddress -
+
+*******************************************************************************/
+#define CsrWifiSmeApStaDisconnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__, peerMacaddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApStaDisconnectCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_STA_DISCONNECT_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->peerMacaddress = (peerMacaddress__);
+
+#define CsrWifiSmeApStaDisconnectCfmSendTo(dst__, src__, interfaceTag__, status__, peerMacaddress__) \
+    { \
+        CsrWifiSmeApStaDisconnectCfm *msg__; \
+        CsrWifiSmeApStaDisconnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__, peerMacaddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApStaDisconnectCfmSend(dst__, interfaceTag__, status__, peerMacaddress__) \
+    CsrWifiSmeApStaDisconnectCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, peerMacaddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaNotifyIndSend
+
+  DESCRIPTION
+    This primitive indicates that a station has joined or a previously joined
+    station has left the BSS/group
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    interfaceTag      -
+    mediaStatus       -
+    peerMacAddress    -
+    peerDeviceAddress -
+    disassocReason    -
+    deauthReason      -
+    WpsRegistration   -
+    secIeLength       -
+    secIe             -
+    groupKeyId        -
+    seqNumber         -
+
+*******************************************************************************/
+#define CsrWifiSmeApStaNotifyIndCreate(msg__, dst__, src__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__, disassocReason__, deauthReason__, WpsRegistration__, secIeLength__, secIe__, groupKeyId__, seqNumber__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApStaNotifyInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_STA_NOTIFY_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->mediaStatus = (mediaStatus__); \
+    msg__->peerMacAddress = (peerMacAddress__); \
+    msg__->peerDeviceAddress = (peerDeviceAddress__); \
+    msg__->disassocReason = (disassocReason__); \
+    msg__->deauthReason = (deauthReason__); \
+    msg__->WpsRegistration = (WpsRegistration__); \
+    msg__->secIeLength = (secIeLength__); \
+    msg__->secIe = (secIe__); \
+    msg__->groupKeyId = (groupKeyId__); \
+    memcpy(msg__->seqNumber, (seqNumber__), sizeof(u16) * 8);
+
+#define CsrWifiSmeApStaNotifyIndSendTo(dst__, src__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__, disassocReason__, deauthReason__, WpsRegistration__, secIeLength__, secIe__, groupKeyId__, seqNumber__) \
+    { \
+        CsrWifiSmeApStaNotifyInd *msg__; \
+        CsrWifiSmeApStaNotifyIndCreate(msg__, dst__, src__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__, disassocReason__, deauthReason__, WpsRegistration__, secIeLength__, secIe__, groupKeyId__, seqNumber__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApStaNotifyIndSend(dst__, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__, disassocReason__, deauthReason__, WpsRegistration__, secIeLength__, secIe__, groupKeyId__, seqNumber__) \
+    CsrWifiSmeApStaNotifyIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, mediaStatus__, peerMacAddress__, peerDeviceAddress__, disassocReason__, deauthReason__, WpsRegistration__, secIeLength__, secIe__, groupKeyId__, seqNumber__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWmmParamUpdateReqSend
+
+  DESCRIPTION
+    Application uses this primitive to update the WMM parameters on the fly
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag  -
+    wmmApParams   - WMM parameters to be used for local firmware queue
+                    configuration
+    wmmApBcParams - WMM parameters to be advertised in beacon/probe response
+
+*******************************************************************************/
+#define CsrWifiSmeApWmmParamUpdateReqCreate(msg__, dst__, src__, interfaceTag__, wmmApParams__, wmmApBcParams__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWmmParamUpdateReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WMM_PARAM_UPDATE_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    memcpy(msg__->wmmApParams, (wmmApParams__), sizeof(CsrWifiSmeWmmAcParams) * 4); \
+    memcpy(msg__->wmmApBcParams, (wmmApBcParams__), sizeof(CsrWifiSmeWmmAcParams) * 4);
+
+#define CsrWifiSmeApWmmParamUpdateReqSendTo(dst__, src__, interfaceTag__, wmmApParams__, wmmApBcParams__) \
+    { \
+        CsrWifiSmeApWmmParamUpdateReq *msg__; \
+        CsrWifiSmeApWmmParamUpdateReqCreate(msg__, dst__, src__, interfaceTag__, wmmApParams__, wmmApBcParams__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWmmParamUpdateReqSend(src__, interfaceTag__, wmmApParams__, wmmApBcParams__) \
+    CsrWifiSmeApWmmParamUpdateReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__, wmmApParams__, wmmApBcParams__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWmmParamUpdateCfmSend
+
+  DESCRIPTION
+    A confirm for CSR_WIFI_SME_AP_WMM_PARAM_UPDATE.request
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiSmeApWmmParamUpdateCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWmmParamUpdateCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WMM_PARAM_UPDATE_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeApWmmParamUpdateCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeApWmmParamUpdateCfm *msg__; \
+        CsrWifiSmeApWmmParamUpdateCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWmmParamUpdateCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeApWmmParamUpdateCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsConfigurationReqSend
+
+  DESCRIPTION
+    This primitive passes the WPS information for the device to SME. This may
+    be accepted only if no interface is active.
+
+  PARAMETERS
+    queue     - Message Source Task Queue (Cfm's will be sent to this Queue)
+    wpsConfig - WPS config.
+
+*******************************************************************************/
+#define CsrWifiSmeApWpsConfigurationReqCreate(msg__, dst__, src__, wpsConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWpsConfigurationReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WPS_CONFIGURATION_REQ, dst__, src__); \
+    msg__->wpsConfig = (wpsConfig__);
+
+#define CsrWifiSmeApWpsConfigurationReqSendTo(dst__, src__, wpsConfig__) \
+    { \
+        CsrWifiSmeApWpsConfigurationReq *msg__; \
+        CsrWifiSmeApWpsConfigurationReqCreate(msg__, dst__, src__, wpsConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWpsConfigurationReqSend(src__, wpsConfig__) \
+    CsrWifiSmeApWpsConfigurationReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, wpsConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsConfigurationCfmSend
+
+  DESCRIPTION
+    Confirm.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiSmeApWpsConfigurationCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWpsConfigurationCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WPS_CONFIGURATION_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeApWpsConfigurationCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeApWpsConfigurationCfm *msg__; \
+        CsrWifiSmeApWpsConfigurationCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWpsConfigurationCfmSend(dst__, status__) \
+    CsrWifiSmeApWpsConfigurationCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationFinishedReqSend
+
+  DESCRIPTION
+    This primitive tells SME that WPS registration procedure has finished
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag -
+
+*******************************************************************************/
+#define CsrWifiSmeApWpsRegistrationFinishedReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWpsRegistrationFinishedReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WPS_REGISTRATION_FINISHED_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeApWpsRegistrationFinishedReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeApWpsRegistrationFinishedReq *msg__; \
+        CsrWifiSmeApWpsRegistrationFinishedReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWpsRegistrationFinishedReqSend(src__, interfaceTag__) \
+    CsrWifiSmeApWpsRegistrationFinishedReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationFinishedCfmSend
+
+  DESCRIPTION
+    A confirm for UNIFI_MGT_AP_WPS_REGISTRATION_FINISHED.request
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiSmeApWpsRegistrationFinishedCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWpsRegistrationFinishedCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WPS_REGISTRATION_FINISHED_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeApWpsRegistrationFinishedCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeApWpsRegistrationFinishedCfm *msg__; \
+        CsrWifiSmeApWpsRegistrationFinishedCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWpsRegistrationFinishedCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeApWpsRegistrationFinishedCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationStartedReqSend
+
+  DESCRIPTION
+    This primitive tells SME that WPS registration procedure has started
+
+  PARAMETERS
+    queue                    - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag             -
+    SelectedDevicePasswordId -
+    SelectedconfigMethod     -
+
+*******************************************************************************/
+#define CsrWifiSmeApWpsRegistrationStartedReqCreate(msg__, dst__, src__, interfaceTag__, SelectedDevicePasswordId__, SelectedconfigMethod__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWpsRegistrationStartedReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WPS_REGISTRATION_STARTED_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->SelectedDevicePasswordId = (SelectedDevicePasswordId__); \
+    msg__->SelectedconfigMethod = (SelectedconfigMethod__);
+
+#define CsrWifiSmeApWpsRegistrationStartedReqSendTo(dst__, src__, interfaceTag__, SelectedDevicePasswordId__, SelectedconfigMethod__) \
+    { \
+        CsrWifiSmeApWpsRegistrationStartedReq *msg__; \
+        CsrWifiSmeApWpsRegistrationStartedReqCreate(msg__, dst__, src__, interfaceTag__, SelectedDevicePasswordId__, SelectedconfigMethod__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWpsRegistrationStartedReqSend(src__, interfaceTag__, SelectedDevicePasswordId__, SelectedconfigMethod__) \
+    CsrWifiSmeApWpsRegistrationStartedReqSendTo(CSR_WIFI_SME_IFACEQUEUE, src__, interfaceTag__, SelectedDevicePasswordId__, SelectedconfigMethod__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationStartedCfmSend
+
+  DESCRIPTION
+    A confirm for UNIFI_MGT_AP_WPS_REGISTRATION_STARTED.request
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+#define CsrWifiSmeApWpsRegistrationStartedCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeApWpsRegistrationStartedCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_AP_PRIM, CSR_WIFI_SME_AP_WPS_REGISTRATION_STARTED_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeApWpsRegistrationStartedCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeApWpsRegistrationStartedCfm *msg__; \
+        CsrWifiSmeApWpsRegistrationStartedCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_AP_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeApWpsRegistrationStartedCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeApWpsRegistrationStartedCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_SME_AP_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_sme_ap_prim.h b/drivers/staging/csr/csr_wifi_sme_ap_prim.h
new file mode 100644
index 00000000000..93b64e9154c
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_ap_prim.h
@@ -0,0 +1,1038 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_AP_PRIM_H__
+#define CSR_WIFI_SME_AP_PRIM_H__
+
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+#include "csr_wifi_common.h"
+#include "csr_result.h"
+#include "csr_wifi_fsm_event.h"
+#include "csr_wifi_sme_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef CSR_WIFI_AP_ENABLE
+#error CSR_WIFI_AP_ENABLE MUST be defined inorder to use csr_wifi_sme_ap_prim.h
+#endif
+
+#define CSR_WIFI_SME_AP_PRIM                                            (0x0407)
+
+typedef CsrPrim CsrWifiSmeApPrim;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApAccessType
+
+  DESCRIPTION
+    Allow or deny STAs based on MAC address
+
+ VALUES
+    CSR_WIFI_AP_ACCESS_TYPE_NONE  - None
+    CSR_WIFI_AP_ACCESS_TYPE_ALLOW - Allow only if MAC address is from the list
+    CSR_WIFI_AP_ACCESS_TYPE_DENY  - Disallow if MAC address is from the list
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApAccessType;
+#define CSR_WIFI_AP_ACCESS_TYPE_NONE    ((CsrWifiSmeApAccessType) 0x00)
+#define CSR_WIFI_AP_ACCESS_TYPE_ALLOW   ((CsrWifiSmeApAccessType) 0x01)
+#define CSR_WIFI_AP_ACCESS_TYPE_DENY    ((CsrWifiSmeApAccessType) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApAuthSupport
+
+  DESCRIPTION
+    Define bits for AP authentication support
+
+ VALUES
+    CSR_WIFI_SME_RSN_AUTH_WPAPSK  - RSN WPA-PSK Support
+    CSR_WIFI_SME_RSN_AUTH_WPA2PSK - RSN WPA2-PSK Support
+    CSR_WIFI_SME_AUTH_WAPIPSK     - WAPI-PSK Support
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApAuthSupport;
+#define CSR_WIFI_SME_RSN_AUTH_WPAPSK    ((CsrWifiSmeApAuthSupport) 0x01)
+#define CSR_WIFI_SME_RSN_AUTH_WPA2PSK   ((CsrWifiSmeApAuthSupport) 0x02)
+#define CSR_WIFI_SME_AUTH_WAPIPSK       ((CsrWifiSmeApAuthSupport) 0x04)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApAuthType
+
+  DESCRIPTION
+    Definition of the SME AP Authentication Options
+
+ VALUES
+    CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM
+                   - Open  authentication
+    CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL
+                   - Personal authentication using a passphrase or a pre-shared
+                     key.
+    CSR_WIFI_SME_AP_AUTH_TYPE_WEP
+                   - WEP authentication. This can be either open or shared key
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApAuthType;
+#define CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM   ((CsrWifiSmeApAuthType) 0x00)
+#define CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL      ((CsrWifiSmeApAuthType) 0x01)
+#define CSR_WIFI_SME_AP_AUTH_TYPE_WEP           ((CsrWifiSmeApAuthType) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApDirection
+
+  DESCRIPTION
+    Definition of Direction
+
+ VALUES
+    CSR_WIFI_AP_DIRECTION_RECEIPIENT - Receipient
+    CSR_WIFI_AP_DIRECTION_ORIGINATOR - Originator
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApDirection;
+#define CSR_WIFI_AP_DIRECTION_RECEIPIENT   ((CsrWifiSmeApDirection) 0x00)
+#define CSR_WIFI_AP_DIRECTION_ORIGINATOR   ((CsrWifiSmeApDirection) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApPhySupport
+
+  DESCRIPTION
+    Define bits for CsrWifiSmeApPhySupportMask
+
+ VALUES
+    CSR_WIFI_SME_AP_PHY_SUPPORT_A - 802.11a. It is not supported in the current
+                                    release.
+    CSR_WIFI_SME_AP_PHY_SUPPORT_B - 802.11b
+    CSR_WIFI_SME_AP_PHY_SUPPORT_G - 802.11g
+    CSR_WIFI_SME_AP_PHY_SUPPORT_N - 802.11n
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApPhySupport;
+#define CSR_WIFI_SME_AP_PHY_SUPPORT_A   ((CsrWifiSmeApPhySupport) 0x01)
+#define CSR_WIFI_SME_AP_PHY_SUPPORT_B   ((CsrWifiSmeApPhySupport) 0x02)
+#define CSR_WIFI_SME_AP_PHY_SUPPORT_G   ((CsrWifiSmeApPhySupport) 0x04)
+#define CSR_WIFI_SME_AP_PHY_SUPPORT_N   ((CsrWifiSmeApPhySupport) 0x08)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApType
+
+  DESCRIPTION
+    Definition of AP types
+
+ VALUES
+    CSR_WIFI_AP_TYPE_LEGACY - Legacy AP
+    CSR_WIFI_AP_TYPE_P2P    - P2P Group Owner(GO)
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApType;
+#define CSR_WIFI_AP_TYPE_LEGACY   ((CsrWifiSmeApType) 0x00)
+#define CSR_WIFI_AP_TYPE_P2P      ((CsrWifiSmeApType) 0x01)
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApAuthSupportMask
+
+  DESCRIPTION
+    See CsrWifiSmeApAuthSupport for bit definitions
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApAuthSupportMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApPhySupportMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeApPhySupport
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeApPhySupportMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApRsnCapabilities
+
+  DESCRIPTION
+    Set to 0 for the current release
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeApRsnCapabilities;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApRsnCapabilitiesMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeApRsnCapabilities
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeApRsnCapabilitiesMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWapiCapabilities
+
+  DESCRIPTION
+    Ignored by the stack as WAPI is not supported for AP operations in the
+    current release
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeApWapiCapabilities;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWapiCapabilitiesMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeApWapiCapabilities
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeApWapiCapabilitiesMask;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApHtParams
+
+  DESCRIPTION
+    Structure holding HT parameters
+
+  MEMBERS
+    greenfieldSupported - Indicates if the AP supports Htgreenfield operation
+                          subject to the chip capability. If the chip does not
+                          support Htgreenfield operation, this parameter will be
+                          ignored.
+                          NOTE: if shortGi20MHz is set to TRUE and the chip
+                          supports short GI operation for 20MHz this field will
+                          be be ignored and the AP will not support Htgreenfield
+                          operation.
+                          NOTE: This field is ignored by the Wi-Fi stack for the
+                          current release. It implies that AP does not support
+                          greenfield operation.
+    shortGi20MHz        - Indicates if the AP support short GI operation for
+                          20MHz subject to the chip capability.If the chip does
+                          not support short GI for 20MHz, this parameter is
+                          ignored
+    rxStbc              - Support for STBC for receive. 0 => No support for STBC
+                          , 1=> Use STBC for Rx
+    rifsModeAllowed     - RIFS Mode is allowed to protect overlapping non-HT BSS
+    htProtection        - Deprecated
+    dualCtsProtection   - Dual CTS Protection enabled
+
+*******************************************************************************/
+typedef struct
+{
+    u8  greenfieldSupported;
+    u8  shortGi20MHz;
+    u8 rxStbc;
+    u8  rifsModeAllowed;
+    u8 htProtection;
+    u8  dualCtsProtection;
+} CsrWifiSmeApHtParams;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApP2pOperatingChanEntry
+
+  DESCRIPTION
+
+  MEMBERS
+    operatingClass        - Channel operating class
+    operatingChannelCount - Number of channels in this entry
+    operatingChannel      - List of channels
+
+*******************************************************************************/
+typedef struct
+{
+    u8  operatingClass;
+    u8  operatingChannelCount;
+    u8 *operatingChannel;
+} CsrWifiSmeApP2pOperatingChanEntry;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApP2pOperatingChanList
+
+  DESCRIPTION
+    This structure contains the lists of P2P operating channels
+
+  MEMBERS
+    country               - Country
+    channelEntryListCount - Number of entries
+    channelEntryList      - List of entries
+
+*******************************************************************************/
+typedef struct
+{
+    u8                           country[3];
+    u8                           channelEntryListCount;
+    CsrWifiSmeApP2pOperatingChanEntry *channelEntryList;
+} CsrWifiSmeApP2pOperatingChanList;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApAuthPers
+
+  DESCRIPTION
+
+  MEMBERS
+    authSupport        -
+    encryptionModeMask -
+    rsnCapabilities    -
+    wapiCapabilities   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeApAuthSupportMask      authSupport;
+    CsrWifiSmeEncryptionMask         encryptionModeMask;
+    CsrWifiSmeApRsnCapabilitiesMask  rsnCapabilities;
+    CsrWifiSmeApWapiCapabilitiesMask wapiCapabilities;
+} CsrWifiSmeApAuthPers;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBaSession
+
+  DESCRIPTION
+
+  MEMBERS
+    peerMacAddress - Indicates MAC address of the peer station
+    tid            - Specifies the TID of the MSDUs for which this Block Ack has
+                     been set up. Range: 0-15
+    direction      - Specifies if the AP is the originator or the recipient of
+                     the data stream that uses the Block Ack.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress     peerMacAddress;
+    u8              tid;
+    CsrWifiSmeApDirection direction;
+} CsrWifiSmeApBaSession;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApMacConfig
+
+  DESCRIPTION
+    Structure holding AP MAC configuration.
+
+  MEMBERS
+    phySupportedBitmap   - Indicates supported physical layers
+    beaconInterval       - Beacon interval in terms of TUs
+    dtimPeriod           - DTIM period in terms of number of beacon intervals
+    maxListenInterval    - Maximum allowed listen interval as number of beacon
+                           intervals
+    supportedRatesCount  - Number of supported rates. Range : 0  to 20
+    supportedRates       - List of supportedRates. A rate is specied in the
+                           units of 500kbps. An entry for a basic rate shall
+                           have the MSB set to 1.
+    preamble             - Preamble to be advertised in beacons and probe
+                           responses
+    shortSlotTimeEnabled - TRUE indicates the AP shall use short slot time if
+                           all the stations use short slot operation.
+    ctsProtectionType    - CTS protection to be used
+    wmmEnabled           - Indicate whether WMM is enabled or not. If set to
+                           FALSE,the WMM parameters shall be ignored by the
+                           receiver.
+    wmmApParams          - WMM parameters to be used for local firmware queue
+                           configuration. Array index corresponds to the ACI.
+    wmmApBcParams        - WMM parameters to be advertised in beacon/probe
+                           response. Array index corresponds to the ACI
+    accessType           - Specifies whether the MAC addresses from the list
+                           should be allowed or denied
+    macAddressListCount  - Number of MAC addresses
+    macAddressList       - List of MAC addresses
+    apHtParams           - AP HT parameters. The stack shall use these
+                           parameters only if phySupportedBitmap indicates
+                           support for IEEE 802.11n
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeApPhySupportMask  phySupportedBitmap;
+    u16                   beaconInterval;
+    u8                    dtimPeriod;
+    u16                   maxListenInterval;
+    u8                    supportedRatesCount;
+    u8                    supportedRates[20];
+    CsrWifiSmePreambleType      preamble;
+    u8                     shortSlotTimeEnabled;
+    CsrWifiSmeCtsProtectionType ctsProtectionType;
+    u8                     wmmEnabled;
+    CsrWifiSmeWmmAcParams       wmmApParams[4];
+    CsrWifiSmeWmmAcParams       wmmApBcParams[4];
+    CsrWifiSmeApAccessType      accessType;
+    u8                    macAddressListCount;
+    CsrWifiMacAddress          *macAddressList;
+    CsrWifiSmeApHtParams        apHtParams;
+} CsrWifiSmeApMacConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApP2pGoConfig
+
+  DESCRIPTION
+
+  MEMBERS
+    groupCapability           - Indicates the P2P group capabilities
+    operatingChanList         - List of operating channels in the order of
+                                decreasing priority. It may contain channel
+                                entry/entries not supported by the wifi stack.
+                                These shall be filtered out by the wifi stack
+    opPsEnabled               - Indicates whether opportunistic power save can
+                                be used.
+                                Note: This parameter is ignored by the WiFi
+                                stack for the current release
+    ctWindow                  - Define Client Traffic window to be used in terms
+                                of number of TUs. Range: 0 to 127.
+                                Note: This parameter is ignored by the WiFi
+                                stack for the current release.
+    noaConfigMethod           - Notice of Absence configuration method.
+                                Note: This parameter is ignored by the WiFi
+                                stack for the current release.
+    allowNoaWithNonP2pDevices - Indicates if NOA should be allowed if non P2P
+                                devices are connected. If allowed the non P2P
+                                devices may suffer in throughput.
+                                Note: This parameter is ignored by the WiFi
+                                stack for the current release.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeP2pGroupCapabilityMask groupCapability;
+    CsrWifiSmeApP2pOperatingChanList operatingChanList;
+    u8                          opPsEnabled;
+    u8                         ctWindow;
+    CsrWifiSmeP2pNoaConfigMethod     noaConfigMethod;
+    u8                          allowNoaWithNonP2pDevices;
+} CsrWifiSmeApP2pGoConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApCredentials
+
+  DESCRIPTION
+
+  MEMBERS
+    authType                    -
+    smeAuthType                 -
+    smeAuthTypeopenSystemEmpty  -
+    smeAuthTypeauthwep          -
+    smeAuthTypeauthPers         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeApAuthType authType;
+    union {
+        CsrWifiSmeEmpty      openSystemEmpty;
+        CsrWifiSmeWepAuth    authwep;
+        CsrWifiSmeApAuthPers authPers;
+    } smeAuthType;
+} CsrWifiSmeApCredentials;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApSecConfig
+
+  DESCRIPTION
+
+  MEMBERS
+    apCredentials -
+    wpsEnabled    -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeApCredentials apCredentials;
+    u8                 wpsEnabled;
+} CsrWifiSmeApSecConfig;
+
+
+/* Downstream */
+#define CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST            (0x0000)
+
+#define CSR_WIFI_SME_AP_BEACONING_START_REQ               ((CsrWifiSmeApPrim) (0x0000 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_BEACONING_STOP_REQ                ((CsrWifiSmeApPrim) (0x0001 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WPS_REGISTRATION_STARTED_REQ      ((CsrWifiSmeApPrim) (0x0002 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WPS_REGISTRATION_FINISHED_REQ     ((CsrWifiSmeApPrim) (0x0003 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WMM_PARAM_UPDATE_REQ              ((CsrWifiSmeApPrim) (0x0004 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_STA_DISCONNECT_REQ                ((CsrWifiSmeApPrim) (0x0005 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WPS_CONFIGURATION_REQ             ((CsrWifiSmeApPrim) (0x0006 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_ACTIVE_BA_GET_REQ                 ((CsrWifiSmeApPrim) (0x0007 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_BA_DELETE_REQ                     ((CsrWifiSmeApPrim) (0x0008 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST))
+
+
+#define CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_HIGHEST           (0x0008 + CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST)
+
+/* Upstream */
+#define CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST              (0x0000 + CSR_PRIM_UPSTREAM)
+
+#define CSR_WIFI_SME_AP_BEACONING_START_CFM               ((CsrWifiSmeApPrim)(0x0000 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_BEACONING_STOP_CFM                ((CsrWifiSmeApPrim)(0x0001 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_STA_NOTIFY_IND                    ((CsrWifiSmeApPrim)(0x0002 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_STA_CONNECT_START_IND             ((CsrWifiSmeApPrim)(0x0003 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WPS_REGISTRATION_STARTED_CFM      ((CsrWifiSmeApPrim)(0x0004 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WPS_REGISTRATION_FINISHED_CFM     ((CsrWifiSmeApPrim)(0x0005 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WMM_PARAM_UPDATE_CFM              ((CsrWifiSmeApPrim)(0x0006 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_STA_DISCONNECT_CFM                ((CsrWifiSmeApPrim)(0x0007 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_WPS_CONFIGURATION_CFM             ((CsrWifiSmeApPrim)(0x0008 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_ERROR_IND                         ((CsrWifiSmeApPrim)(0x0009 + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_ACTIVE_BA_GET_CFM                 ((CsrWifiSmeApPrim)(0x000A + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AP_BA_DELETE_CFM                     ((CsrWifiSmeApPrim)(0x000B + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST))
+
+#define CSR_WIFI_SME_AP_PRIM_UPSTREAM_HIGHEST             (0x000B + CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST)
+
+#define CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_COUNT             (CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_HIGHEST + 1 - CSR_WIFI_SME_AP_PRIM_DOWNSTREAM_LOWEST)
+#define CSR_WIFI_SME_AP_PRIM_UPSTREAM_COUNT               (CSR_WIFI_SME_AP_PRIM_UPSTREAM_HIGHEST   + 1 - CSR_WIFI_SME_AP_PRIM_UPSTREAM_LOWEST)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStartReq
+
+  DESCRIPTION
+    This primitive requests the SME to start AP or GO functionality
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    -
+    initialPresence - Set to 0, if Not in a group fomration phase, set to 1 ,
+                      during group formation phase
+    apType          - apType : Legacy AP or P2PGO
+    cloakSsid       - cloakSsid flag.
+    ssid            - ssid.
+    ifIndex         - Radio Interface
+    channel         - channel.
+    maxConnections  - Maximum Stations + P2PClients allowed
+    apCredentials   - AP security credeitals used to advertise in beacon /probe
+                      response
+    smeApConfig     - AP configuration
+    p2pGoParam      - P2P specific GO parameters. Ignored if it is a leagacy AP
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    u8                initialPresence;
+    CsrWifiSmeApType        apType;
+    u8                 cloakSsid;
+    CsrWifiSsid             ssid;
+    CsrWifiSmeRadioIF       ifIndex;
+    u8                channel;
+    u8                maxConnections;
+    CsrWifiSmeApSecConfig   apCredentials;
+    CsrWifiSmeApMacConfig   smeApConfig;
+    CsrWifiSmeApP2pGoConfig p2pGoParam;
+} CsrWifiSmeApBeaconingStartReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStopReq
+
+  DESCRIPTION
+    This primitive requests the SME to STOP AP or P2PGO operation
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeApBeaconingStopReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationStartedReq
+
+  DESCRIPTION
+    This primitive tells SME that WPS registration procedure has started
+
+  MEMBERS
+    common                   - Common header for use with the CsrWifiFsm Module
+    interfaceTag             -
+    SelectedDevicePasswordId -
+    SelectedconfigMethod     -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    CsrWifiSmeWpsDpid       SelectedDevicePasswordId;
+    CsrWifiSmeWpsConfigType SelectedconfigMethod;
+} CsrWifiSmeApWpsRegistrationStartedReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationFinishedReq
+
+  DESCRIPTION
+    This primitive tells SME that WPS registration procedure has finished
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeApWpsRegistrationFinishedReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWmmParamUpdateReq
+
+  DESCRIPTION
+    Application uses this primitive to update the WMM parameters on the fly
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  -
+    wmmApParams   - WMM parameters to be used for local firmware queue
+                    configuration
+    wmmApBcParams - WMM parameters to be advertised in beacon/probe response
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    u16             interfaceTag;
+    CsrWifiSmeWmmAcParams wmmApParams[4];
+    CsrWifiSmeWmmAcParams wmmApBcParams[4];
+} CsrWifiSmeApWmmParamUpdateReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaDisconnectReq
+
+  DESCRIPTION
+    This primitive tells SME to deauth ot disassociate a particular station
+    within BSS
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   -
+    deauthReason   -
+    disassocReason -
+    peerMacaddress -
+    keepBlocking   - If TRUE, the station is blocked. If FALSE and the station
+                     is connected, disconnect the station. If FALSE and the
+                     station is not connected, no action is taken.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrWifiSmeIEEE80211Reason deauthReason;
+    CsrWifiSmeIEEE80211Reason disassocReason;
+    CsrWifiMacAddress         peerMacaddress;
+    u8                   keepBlocking;
+} CsrWifiSmeApStaDisconnectReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsConfigurationReq
+
+  DESCRIPTION
+    This primitive passes the WPS information for the device to SME. This may
+    be accepted only if no interface is active.
+
+  MEMBERS
+    common    - Common header for use with the CsrWifiFsm Module
+    wpsConfig - WPS config.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    CsrWifiSmeWpsConfig wpsConfig;
+} CsrWifiSmeApWpsConfigurationReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApActiveBaGetReq
+
+  DESCRIPTION
+    This primitive used to retrieve information related to the active block
+    ack sessions
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeApActiveBaGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBaDeleteReq
+
+  DESCRIPTION
+    This primitive is used to delete an active block ack session
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    reason       -
+    baSession    - BA session to be deleted
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrWifiSmeIEEE80211Reason reason;
+    CsrWifiSmeApBaSession     baSession;
+} CsrWifiSmeApBaDeleteReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStartCfm
+
+  DESCRIPTION
+    This primitive confirms the completion of the request along with the
+    status
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+    secIeLength  -
+    secIe        -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+    u16       secIeLength;
+    u8       *secIe;
+} CsrWifiSmeApBeaconingStartCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBeaconingStopCfm
+
+  DESCRIPTION
+    This primitive confirms AP or P2PGO operation is terminated
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeApBeaconingStopCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaNotifyInd
+
+  DESCRIPTION
+    This primitive indicates that a station has joined or a previously joined
+    station has left the BSS/group
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    interfaceTag      -
+    mediaStatus       -
+    peerMacAddress    -
+    peerDeviceAddress -
+    disassocReason    -
+    deauthReason      -
+    WpsRegistration   -
+    secIeLength       -
+    secIe             -
+    groupKeyId        -
+    seqNumber         -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrWifiSmeMediaStatus     mediaStatus;
+    CsrWifiMacAddress         peerMacAddress;
+    CsrWifiMacAddress         peerDeviceAddress;
+    CsrWifiSmeIEEE80211Reason disassocReason;
+    CsrWifiSmeIEEE80211Reason deauthReason;
+    CsrWifiSmeWpsRegistration WpsRegistration;
+    u8                  secIeLength;
+    u8                 *secIe;
+    u8                  groupKeyId;
+    u16                 seqNumber[8];
+} CsrWifiSmeApStaNotifyInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaConnectStartInd
+
+  DESCRIPTION
+    This primitive indicates that a stations request to join the group/BSS is
+    accepted
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   -
+    peerMacAddress -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    CsrWifiMacAddress peerMacAddress;
+} CsrWifiSmeApStaConnectStartInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationStartedCfm
+
+  DESCRIPTION
+    A confirm for UNIFI_MGT_AP_WPS_REGISTRATION_STARTED.request
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeApWpsRegistrationStartedCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsRegistrationFinishedCfm
+
+  DESCRIPTION
+    A confirm for UNIFI_MGT_AP_WPS_REGISTRATION_FINISHED.request
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeApWpsRegistrationFinishedCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWmmParamUpdateCfm
+
+  DESCRIPTION
+    A confirm for CSR_WIFI_SME_AP_WMM_PARAM_UPDATE.request
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeApWmmParamUpdateCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApStaDisconnectCfm
+
+  DESCRIPTION
+    This primitive confirms the station is disconnected
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   -
+    status         -
+    peerMacaddress -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    CsrResult         status;
+    CsrWifiMacAddress peerMacaddress;
+} CsrWifiSmeApStaDisconnectCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApWpsConfigurationCfm
+
+  DESCRIPTION
+    Confirm.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeApWpsConfigurationCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApErrorInd
+
+  DESCRIPTION
+    This primitve is sent by SME to indicate some error in AP operationi
+    after AP operations were started successfully and continuing the AP
+    operation may lead to undesired behaviour. It is the responsibility of
+    the upper layers to stop AP operation if needed
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Range 0-1
+    apType       -
+    status       - Contains the error status
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent  common;
+    u16        interfaceTag;
+    CsrWifiSmeApType apType;
+    CsrResult        status;
+} CsrWifiSmeApErrorInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApActiveBaGetCfm
+
+  DESCRIPTION
+    This primitive carries the information related to the active ba sessions
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     -
+    status           - Reports the result of the request
+    activeBaCount    - Number of active block ack session
+    activeBaSessions - Points to a buffer containing an array of
+                       CsrWifiSmeApBaSession structures.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent        common;
+    u16              interfaceTag;
+    CsrResult              status;
+    u16              activeBaCount;
+    CsrWifiSmeApBaSession *activeBaSessions;
+} CsrWifiSmeApActiveBaGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeApBaDeleteCfm
+
+  DESCRIPTION
+    This primitive confirms the BA is deleted
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag -
+    status       - Reports the result of the request
+    baSession    - deleted BA session
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    u16             interfaceTag;
+    CsrResult             status;
+    CsrWifiSmeApBaSession baSession;
+} CsrWifiSmeApBaDeleteCfm;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_SME_AP_PRIM_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_sme_converter_init.c b/drivers/staging/csr/csr_wifi_sme_converter_init.c
new file mode 100644
index 00000000000..31835f06bbc
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_converter_init.c
@@ -0,0 +1,201 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#include "csr_msgconv.h"
+#include "csr_macro.h"
+
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_SME_MODULE
+#include "csr_wifi_sme_serialize.h"
+#include "csr_wifi_sme_prim.h"
+
+static CsrMsgConvMsgEntry csrwifisme_conv_lut[] = {
+    { CSR_WIFI_SME_ACTIVATE_REQ, CsrWifiSmeActivateReqSizeof, CsrWifiSmeActivateReqSer, CsrWifiSmeActivateReqDes, CsrWifiSmeActivateReqSerFree },
+    { CSR_WIFI_SME_ADHOC_CONFIG_GET_REQ, CsrWifiSmeAdhocConfigGetReqSizeof, CsrWifiSmeAdhocConfigGetReqSer, CsrWifiSmeAdhocConfigGetReqDes, CsrWifiSmeAdhocConfigGetReqSerFree },
+    { CSR_WIFI_SME_ADHOC_CONFIG_SET_REQ, CsrWifiSmeAdhocConfigSetReqSizeof, CsrWifiSmeAdhocConfigSetReqSer, CsrWifiSmeAdhocConfigSetReqDes, CsrWifiSmeAdhocConfigSetReqSerFree },
+    { CSR_WIFI_SME_BLACKLIST_REQ, CsrWifiSmeBlacklistReqSizeof, CsrWifiSmeBlacklistReqSer, CsrWifiSmeBlacklistReqDes, CsrWifiSmeBlacklistReqSerFree },
+    { CSR_WIFI_SME_CALIBRATION_DATA_GET_REQ, CsrWifiSmeCalibrationDataGetReqSizeof, CsrWifiSmeCalibrationDataGetReqSer, CsrWifiSmeCalibrationDataGetReqDes, CsrWifiSmeCalibrationDataGetReqSerFree },
+    { CSR_WIFI_SME_CALIBRATION_DATA_SET_REQ, CsrWifiSmeCalibrationDataSetReqSizeof, CsrWifiSmeCalibrationDataSetReqSer, CsrWifiSmeCalibrationDataSetReqDes, CsrWifiSmeCalibrationDataSetReqSerFree },
+    { CSR_WIFI_SME_CCX_CONFIG_GET_REQ, CsrWifiSmeCcxConfigGetReqSizeof, CsrWifiSmeCcxConfigGetReqSer, CsrWifiSmeCcxConfigGetReqDes, CsrWifiSmeCcxConfigGetReqSerFree },
+    { CSR_WIFI_SME_CCX_CONFIG_SET_REQ, CsrWifiSmeCcxConfigSetReqSizeof, CsrWifiSmeCcxConfigSetReqSer, CsrWifiSmeCcxConfigSetReqDes, CsrWifiSmeCcxConfigSetReqSerFree },
+    { CSR_WIFI_SME_COEX_CONFIG_GET_REQ, CsrWifiSmeCoexConfigGetReqSizeof, CsrWifiSmeCoexConfigGetReqSer, CsrWifiSmeCoexConfigGetReqDes, CsrWifiSmeCoexConfigGetReqSerFree },
+    { CSR_WIFI_SME_COEX_CONFIG_SET_REQ, CsrWifiSmeCoexConfigSetReqSizeof, CsrWifiSmeCoexConfigSetReqSer, CsrWifiSmeCoexConfigSetReqDes, CsrWifiSmeCoexConfigSetReqSerFree },
+    { CSR_WIFI_SME_COEX_INFO_GET_REQ, CsrWifiSmeCoexInfoGetReqSizeof, CsrWifiSmeCoexInfoGetReqSer, CsrWifiSmeCoexInfoGetReqDes, CsrWifiSmeCoexInfoGetReqSerFree },
+    { CSR_WIFI_SME_CONNECT_REQ, CsrWifiSmeConnectReqSizeof, CsrWifiSmeConnectReqSer, CsrWifiSmeConnectReqDes, CsrWifiSmeConnectReqSerFree },
+    { CSR_WIFI_SME_CONNECTION_CONFIG_GET_REQ, CsrWifiSmeConnectionConfigGetReqSizeof, CsrWifiSmeConnectionConfigGetReqSer, CsrWifiSmeConnectionConfigGetReqDes, CsrWifiSmeConnectionConfigGetReqSerFree },
+    { CSR_WIFI_SME_CONNECTION_INFO_GET_REQ, CsrWifiSmeConnectionInfoGetReqSizeof, CsrWifiSmeConnectionInfoGetReqSer, CsrWifiSmeConnectionInfoGetReqDes, CsrWifiSmeConnectionInfoGetReqSerFree },
+    { CSR_WIFI_SME_CONNECTION_STATS_GET_REQ, CsrWifiSmeConnectionStatsGetReqSizeof, CsrWifiSmeConnectionStatsGetReqSer, CsrWifiSmeConnectionStatsGetReqDes, CsrWifiSmeConnectionStatsGetReqSerFree },
+    { CSR_WIFI_SME_DEACTIVATE_REQ, CsrWifiSmeDeactivateReqSizeof, CsrWifiSmeDeactivateReqSer, CsrWifiSmeDeactivateReqDes, CsrWifiSmeDeactivateReqSerFree },
+    { CSR_WIFI_SME_DISCONNECT_REQ, CsrWifiSmeDisconnectReqSizeof, CsrWifiSmeDisconnectReqSer, CsrWifiSmeDisconnectReqDes, CsrWifiSmeDisconnectReqSerFree },
+    { CSR_WIFI_SME_EVENT_MASK_SET_REQ, CsrWifiSmeEventMaskSetReqSizeof, CsrWifiSmeEventMaskSetReqSer, CsrWifiSmeEventMaskSetReqDes, CsrWifiSmeEventMaskSetReqSerFree },
+    { CSR_WIFI_SME_HOST_CONFIG_GET_REQ, CsrWifiSmeHostConfigGetReqSizeof, CsrWifiSmeHostConfigGetReqSer, CsrWifiSmeHostConfigGetReqDes, CsrWifiSmeHostConfigGetReqSerFree },
+    { CSR_WIFI_SME_HOST_CONFIG_SET_REQ, CsrWifiSmeHostConfigSetReqSizeof, CsrWifiSmeHostConfigSetReqSer, CsrWifiSmeHostConfigSetReqDes, CsrWifiSmeHostConfigSetReqSerFree },
+    { CSR_WIFI_SME_KEY_REQ, CsrWifiSmeKeyReqSizeof, CsrWifiSmeKeyReqSer, CsrWifiSmeKeyReqDes, CsrWifiSmeKeyReqSerFree },
+    { CSR_WIFI_SME_LINK_QUALITY_GET_REQ, CsrWifiSmeLinkQualityGetReqSizeof, CsrWifiSmeLinkQualityGetReqSer, CsrWifiSmeLinkQualityGetReqDes, CsrWifiSmeLinkQualityGetReqSerFree },
+    { CSR_WIFI_SME_MIB_CONFIG_GET_REQ, CsrWifiSmeMibConfigGetReqSizeof, CsrWifiSmeMibConfigGetReqSer, CsrWifiSmeMibConfigGetReqDes, CsrWifiSmeMibConfigGetReqSerFree },
+    { CSR_WIFI_SME_MIB_CONFIG_SET_REQ, CsrWifiSmeMibConfigSetReqSizeof, CsrWifiSmeMibConfigSetReqSer, CsrWifiSmeMibConfigSetReqDes, CsrWifiSmeMibConfigSetReqSerFree },
+    { CSR_WIFI_SME_MIB_GET_NEXT_REQ, CsrWifiSmeMibGetNextReqSizeof, CsrWifiSmeMibGetNextReqSer, CsrWifiSmeMibGetNextReqDes, CsrWifiSmeMibGetNextReqSerFree },
+    { CSR_WIFI_SME_MIB_GET_REQ, CsrWifiSmeMibGetReqSizeof, CsrWifiSmeMibGetReqSer, CsrWifiSmeMibGetReqDes, CsrWifiSmeMibGetReqSerFree },
+    { CSR_WIFI_SME_MIB_SET_REQ, CsrWifiSmeMibSetReqSizeof, CsrWifiSmeMibSetReqSer, CsrWifiSmeMibSetReqDes, CsrWifiSmeMibSetReqSerFree },
+    { CSR_WIFI_SME_MULTICAST_ADDRESS_REQ, CsrWifiSmeMulticastAddressReqSizeof, CsrWifiSmeMulticastAddressReqSer, CsrWifiSmeMulticastAddressReqDes, CsrWifiSmeMulticastAddressReqSerFree },
+    { CSR_WIFI_SME_PACKET_FILTER_SET_REQ, CsrWifiSmePacketFilterSetReqSizeof, CsrWifiSmePacketFilterSetReqSer, CsrWifiSmePacketFilterSetReqDes, CsrWifiSmePacketFilterSetReqSerFree },
+    { CSR_WIFI_SME_PERMANENT_MAC_ADDRESS_GET_REQ, CsrWifiSmePermanentMacAddressGetReqSizeof, CsrWifiSmePermanentMacAddressGetReqSer, CsrWifiSmePermanentMacAddressGetReqDes, CsrWifiSmePermanentMacAddressGetReqSerFree },
+    { CSR_WIFI_SME_PMKID_REQ, CsrWifiSmePmkidReqSizeof, CsrWifiSmePmkidReqSer, CsrWifiSmePmkidReqDes, CsrWifiSmePmkidReqSerFree },
+    { CSR_WIFI_SME_POWER_CONFIG_GET_REQ, CsrWifiSmePowerConfigGetReqSizeof, CsrWifiSmePowerConfigGetReqSer, CsrWifiSmePowerConfigGetReqDes, CsrWifiSmePowerConfigGetReqSerFree },
+    { CSR_WIFI_SME_POWER_CONFIG_SET_REQ, CsrWifiSmePowerConfigSetReqSizeof, CsrWifiSmePowerConfigSetReqSer, CsrWifiSmePowerConfigSetReqDes, CsrWifiSmePowerConfigSetReqSerFree },
+    { CSR_WIFI_SME_REGULATORY_DOMAIN_INFO_GET_REQ, CsrWifiSmeRegulatoryDomainInfoGetReqSizeof, CsrWifiSmeRegulatoryDomainInfoGetReqSer, CsrWifiSmeRegulatoryDomainInfoGetReqDes, CsrWifiSmeRegulatoryDomainInfoGetReqSerFree },
+    { CSR_WIFI_SME_ROAMING_CONFIG_GET_REQ, CsrWifiSmeRoamingConfigGetReqSizeof, CsrWifiSmeRoamingConfigGetReqSer, CsrWifiSmeRoamingConfigGetReqDes, CsrWifiSmeRoamingConfigGetReqSerFree },
+    { CSR_WIFI_SME_ROAMING_CONFIG_SET_REQ, CsrWifiSmeRoamingConfigSetReqSizeof, CsrWifiSmeRoamingConfigSetReqSer, CsrWifiSmeRoamingConfigSetReqDes, CsrWifiSmeRoamingConfigSetReqSerFree },
+    { CSR_WIFI_SME_SCAN_CONFIG_GET_REQ, CsrWifiSmeScanConfigGetReqSizeof, CsrWifiSmeScanConfigGetReqSer, CsrWifiSmeScanConfigGetReqDes, CsrWifiSmeScanConfigGetReqSerFree },
+    { CSR_WIFI_SME_SCAN_CONFIG_SET_REQ, CsrWifiSmeScanConfigSetReqSizeof, CsrWifiSmeScanConfigSetReqSer, CsrWifiSmeScanConfigSetReqDes, CsrWifiSmeScanConfigSetReqSerFree },
+    { CSR_WIFI_SME_SCAN_FULL_REQ, CsrWifiSmeScanFullReqSizeof, CsrWifiSmeScanFullReqSer, CsrWifiSmeScanFullReqDes, CsrWifiSmeScanFullReqSerFree },
+    { CSR_WIFI_SME_SCAN_RESULTS_FLUSH_REQ, CsrWifiSmeScanResultsFlushReqSizeof, CsrWifiSmeScanResultsFlushReqSer, CsrWifiSmeScanResultsFlushReqDes, CsrWifiSmeScanResultsFlushReqSerFree },
+    { CSR_WIFI_SME_SCAN_RESULTS_GET_REQ, CsrWifiSmeScanResultsGetReqSizeof, CsrWifiSmeScanResultsGetReqSer, CsrWifiSmeScanResultsGetReqDes, CsrWifiSmeScanResultsGetReqSerFree },
+    { CSR_WIFI_SME_SME_STA_CONFIG_GET_REQ, CsrWifiSmeSmeStaConfigGetReqSizeof, CsrWifiSmeSmeStaConfigGetReqSer, CsrWifiSmeSmeStaConfigGetReqDes, CsrWifiSmeSmeStaConfigGetReqSerFree },
+    { CSR_WIFI_SME_SME_STA_CONFIG_SET_REQ, CsrWifiSmeSmeStaConfigSetReqSizeof, CsrWifiSmeSmeStaConfigSetReqSer, CsrWifiSmeSmeStaConfigSetReqDes, CsrWifiSmeSmeStaConfigSetReqSerFree },
+    { CSR_WIFI_SME_STATION_MAC_ADDRESS_GET_REQ, CsrWifiSmeStationMacAddressGetReqSizeof, CsrWifiSmeStationMacAddressGetReqSer, CsrWifiSmeStationMacAddressGetReqDes, CsrWifiSmeStationMacAddressGetReqSerFree },
+    { CSR_WIFI_SME_TSPEC_REQ, CsrWifiSmeTspecReqSizeof, CsrWifiSmeTspecReqSer, CsrWifiSmeTspecReqDes, CsrWifiSmeTspecReqSerFree },
+    { CSR_WIFI_SME_VERSIONS_GET_REQ, CsrWifiSmeVersionsGetReqSizeof, CsrWifiSmeVersionsGetReqSer, CsrWifiSmeVersionsGetReqDes, CsrWifiSmeVersionsGetReqSerFree },
+    { CSR_WIFI_SME_WIFI_FLIGHTMODE_REQ, CsrWifiSmeWifiFlightmodeReqSizeof, CsrWifiSmeWifiFlightmodeReqSer, CsrWifiSmeWifiFlightmodeReqDes, CsrWifiSmeWifiFlightmodeReqSerFree },
+    { CSR_WIFI_SME_WIFI_OFF_REQ, CsrWifiSmeWifiOffReqSizeof, CsrWifiSmeWifiOffReqSer, CsrWifiSmeWifiOffReqDes, CsrWifiSmeWifiOffReqSerFree },
+    { CSR_WIFI_SME_WIFI_ON_REQ, CsrWifiSmeWifiOnReqSizeof, CsrWifiSmeWifiOnReqSer, CsrWifiSmeWifiOnReqDes, CsrWifiSmeWifiOnReqSerFree },
+    { CSR_WIFI_SME_CLOAKED_SSIDS_SET_REQ, CsrWifiSmeCloakedSsidsSetReqSizeof, CsrWifiSmeCloakedSsidsSetReqSer, CsrWifiSmeCloakedSsidsSetReqDes, CsrWifiSmeCloakedSsidsSetReqSerFree },
+    { CSR_WIFI_SME_CLOAKED_SSIDS_GET_REQ, CsrWifiSmeCloakedSsidsGetReqSizeof, CsrWifiSmeCloakedSsidsGetReqSer, CsrWifiSmeCloakedSsidsGetReqDes, CsrWifiSmeCloakedSsidsGetReqSerFree },
+    { CSR_WIFI_SME_SME_COMMON_CONFIG_GET_REQ, CsrWifiSmeSmeCommonConfigGetReqSizeof, CsrWifiSmeSmeCommonConfigGetReqSer, CsrWifiSmeSmeCommonConfigGetReqDes, CsrWifiSmeSmeCommonConfigGetReqSerFree },
+    { CSR_WIFI_SME_SME_COMMON_CONFIG_SET_REQ, CsrWifiSmeSmeCommonConfigSetReqSizeof, CsrWifiSmeSmeCommonConfigSetReqSer, CsrWifiSmeSmeCommonConfigSetReqDes, CsrWifiSmeSmeCommonConfigSetReqSerFree },
+    { CSR_WIFI_SME_INTERFACE_CAPABILITY_GET_REQ, CsrWifiSmeInterfaceCapabilityGetReqSizeof, CsrWifiSmeInterfaceCapabilityGetReqSer, CsrWifiSmeInterfaceCapabilityGetReqDes, CsrWifiSmeInterfaceCapabilityGetReqSerFree },
+    { CSR_WIFI_SME_WPS_CONFIGURATION_REQ, CsrWifiSmeWpsConfigurationReqSizeof, CsrWifiSmeWpsConfigurationReqSer, CsrWifiSmeWpsConfigurationReqDes, CsrWifiSmeWpsConfigurationReqSerFree },
+    { CSR_WIFI_SME_SET_REQ, CsrWifiSmeSetReqSizeof, CsrWifiSmeSetReqSer, CsrWifiSmeSetReqDes, CsrWifiSmeSetReqSerFree },
+    { CSR_WIFI_SME_ACTIVATE_CFM, CsrWifiSmeActivateCfmSizeof, CsrWifiSmeActivateCfmSer, CsrWifiSmeActivateCfmDes, CsrWifiSmeActivateCfmSerFree },
+    { CSR_WIFI_SME_ADHOC_CONFIG_GET_CFM, CsrWifiSmeAdhocConfigGetCfmSizeof, CsrWifiSmeAdhocConfigGetCfmSer, CsrWifiSmeAdhocConfigGetCfmDes, CsrWifiSmeAdhocConfigGetCfmSerFree },
+    { CSR_WIFI_SME_ADHOC_CONFIG_SET_CFM, CsrWifiSmeAdhocConfigSetCfmSizeof, CsrWifiSmeAdhocConfigSetCfmSer, CsrWifiSmeAdhocConfigSetCfmDes, CsrWifiSmeAdhocConfigSetCfmSerFree },
+    { CSR_WIFI_SME_ASSOCIATION_COMPLETE_IND, CsrWifiSmeAssociationCompleteIndSizeof, CsrWifiSmeAssociationCompleteIndSer, CsrWifiSmeAssociationCompleteIndDes, CsrWifiSmeAssociationCompleteIndSerFree },
+    { CSR_WIFI_SME_ASSOCIATION_START_IND, CsrWifiSmeAssociationStartIndSizeof, CsrWifiSmeAssociationStartIndSer, CsrWifiSmeAssociationStartIndDes, CsrWifiSmeAssociationStartIndSerFree },
+    { CSR_WIFI_SME_BLACKLIST_CFM, CsrWifiSmeBlacklistCfmSizeof, CsrWifiSmeBlacklistCfmSer, CsrWifiSmeBlacklistCfmDes, CsrWifiSmeBlacklistCfmSerFree },
+    { CSR_WIFI_SME_CALIBRATION_DATA_GET_CFM, CsrWifiSmeCalibrationDataGetCfmSizeof, CsrWifiSmeCalibrationDataGetCfmSer, CsrWifiSmeCalibrationDataGetCfmDes, CsrWifiSmeCalibrationDataGetCfmSerFree },
+    { CSR_WIFI_SME_CALIBRATION_DATA_SET_CFM, CsrWifiSmeCalibrationDataSetCfmSizeof, CsrWifiSmeCalibrationDataSetCfmSer, CsrWifiSmeCalibrationDataSetCfmDes, CsrWifiSmeCalibrationDataSetCfmSerFree },
+    { CSR_WIFI_SME_CCX_CONFIG_GET_CFM, CsrWifiSmeCcxConfigGetCfmSizeof, CsrWifiSmeCcxConfigGetCfmSer, CsrWifiSmeCcxConfigGetCfmDes, CsrWifiSmeCcxConfigGetCfmSerFree },
+    { CSR_WIFI_SME_CCX_CONFIG_SET_CFM, CsrWifiSmeCcxConfigSetCfmSizeof, CsrWifiSmeCcxConfigSetCfmSer, CsrWifiSmeCcxConfigSetCfmDes, CsrWifiSmeCcxConfigSetCfmSerFree },
+    { CSR_WIFI_SME_COEX_CONFIG_GET_CFM, CsrWifiSmeCoexConfigGetCfmSizeof, CsrWifiSmeCoexConfigGetCfmSer, CsrWifiSmeCoexConfigGetCfmDes, CsrWifiSmeCoexConfigGetCfmSerFree },
+    { CSR_WIFI_SME_COEX_CONFIG_SET_CFM, CsrWifiSmeCoexConfigSetCfmSizeof, CsrWifiSmeCoexConfigSetCfmSer, CsrWifiSmeCoexConfigSetCfmDes, CsrWifiSmeCoexConfigSetCfmSerFree },
+    { CSR_WIFI_SME_COEX_INFO_GET_CFM, CsrWifiSmeCoexInfoGetCfmSizeof, CsrWifiSmeCoexInfoGetCfmSer, CsrWifiSmeCoexInfoGetCfmDes, CsrWifiSmeCoexInfoGetCfmSerFree },
+    { CSR_WIFI_SME_CONNECT_CFM, CsrWifiSmeConnectCfmSizeof, CsrWifiSmeConnectCfmSer, CsrWifiSmeConnectCfmDes, CsrWifiSmeConnectCfmSerFree },
+    { CSR_WIFI_SME_CONNECTION_CONFIG_GET_CFM, CsrWifiSmeConnectionConfigGetCfmSizeof, CsrWifiSmeConnectionConfigGetCfmSer, CsrWifiSmeConnectionConfigGetCfmDes, CsrWifiSmeConnectionConfigGetCfmSerFree },
+    { CSR_WIFI_SME_CONNECTION_INFO_GET_CFM, CsrWifiSmeConnectionInfoGetCfmSizeof, CsrWifiSmeConnectionInfoGetCfmSer, CsrWifiSmeConnectionInfoGetCfmDes, CsrWifiSmeConnectionInfoGetCfmSerFree },
+    { CSR_WIFI_SME_CONNECTION_QUALITY_IND, CsrWifiSmeConnectionQualityIndSizeof, CsrWifiSmeConnectionQualityIndSer, CsrWifiSmeConnectionQualityIndDes, CsrWifiSmeConnectionQualityIndSerFree },
+    { CSR_WIFI_SME_CONNECTION_STATS_GET_CFM, CsrWifiSmeConnectionStatsGetCfmSizeof, CsrWifiSmeConnectionStatsGetCfmSer, CsrWifiSmeConnectionStatsGetCfmDes, CsrWifiSmeConnectionStatsGetCfmSerFree },
+    { CSR_WIFI_SME_DEACTIVATE_CFM, CsrWifiSmeDeactivateCfmSizeof, CsrWifiSmeDeactivateCfmSer, CsrWifiSmeDeactivateCfmDes, CsrWifiSmeDeactivateCfmSerFree },
+    { CSR_WIFI_SME_DISCONNECT_CFM, CsrWifiSmeDisconnectCfmSizeof, CsrWifiSmeDisconnectCfmSer, CsrWifiSmeDisconnectCfmDes, CsrWifiSmeDisconnectCfmSerFree },
+    { CSR_WIFI_SME_EVENT_MASK_SET_CFM, CsrWifiSmeEventMaskSetCfmSizeof, CsrWifiSmeEventMaskSetCfmSer, CsrWifiSmeEventMaskSetCfmDes, CsrWifiSmeEventMaskSetCfmSerFree },
+    { CSR_WIFI_SME_HOST_CONFIG_GET_CFM, CsrWifiSmeHostConfigGetCfmSizeof, CsrWifiSmeHostConfigGetCfmSer, CsrWifiSmeHostConfigGetCfmDes, CsrWifiSmeHostConfigGetCfmSerFree },
+    { CSR_WIFI_SME_HOST_CONFIG_SET_CFM, CsrWifiSmeHostConfigSetCfmSizeof, CsrWifiSmeHostConfigSetCfmSer, CsrWifiSmeHostConfigSetCfmDes, CsrWifiSmeHostConfigSetCfmSerFree },
+    { CSR_WIFI_SME_IBSS_STATION_IND, CsrWifiSmeIbssStationIndSizeof, CsrWifiSmeIbssStationIndSer, CsrWifiSmeIbssStationIndDes, CsrWifiSmeIbssStationIndSerFree },
+    { CSR_WIFI_SME_KEY_CFM, CsrWifiSmeKeyCfmSizeof, CsrWifiSmeKeyCfmSer, CsrWifiSmeKeyCfmDes, CsrWifiSmeKeyCfmSerFree },
+    { CSR_WIFI_SME_LINK_QUALITY_GET_CFM, CsrWifiSmeLinkQualityGetCfmSizeof, CsrWifiSmeLinkQualityGetCfmSer, CsrWifiSmeLinkQualityGetCfmDes, CsrWifiSmeLinkQualityGetCfmSerFree },
+    { CSR_WIFI_SME_MEDIA_STATUS_IND, CsrWifiSmeMediaStatusIndSizeof, CsrWifiSmeMediaStatusIndSer, CsrWifiSmeMediaStatusIndDes, CsrWifiSmeMediaStatusIndSerFree },
+    { CSR_WIFI_SME_MIB_CONFIG_GET_CFM, CsrWifiSmeMibConfigGetCfmSizeof, CsrWifiSmeMibConfigGetCfmSer, CsrWifiSmeMibConfigGetCfmDes, CsrWifiSmeMibConfigGetCfmSerFree },
+    { CSR_WIFI_SME_MIB_CONFIG_SET_CFM, CsrWifiSmeMibConfigSetCfmSizeof, CsrWifiSmeMibConfigSetCfmSer, CsrWifiSmeMibConfigSetCfmDes, CsrWifiSmeMibConfigSetCfmSerFree },
+    { CSR_WIFI_SME_MIB_GET_CFM, CsrWifiSmeMibGetCfmSizeof, CsrWifiSmeMibGetCfmSer, CsrWifiSmeMibGetCfmDes, CsrWifiSmeMibGetCfmSerFree },
+    { CSR_WIFI_SME_MIB_GET_NEXT_CFM, CsrWifiSmeMibGetNextCfmSizeof, CsrWifiSmeMibGetNextCfmSer, CsrWifiSmeMibGetNextCfmDes, CsrWifiSmeMibGetNextCfmSerFree },
+    { CSR_WIFI_SME_MIB_SET_CFM, CsrWifiSmeMibSetCfmSizeof, CsrWifiSmeMibSetCfmSer, CsrWifiSmeMibSetCfmDes, CsrWifiSmeMibSetCfmSerFree },
+    { CSR_WIFI_SME_MIC_FAILURE_IND, CsrWifiSmeMicFailureIndSizeof, CsrWifiSmeMicFailureIndSer, CsrWifiSmeMicFailureIndDes, CsrWifiSmeMicFailureIndSerFree },
+    { CSR_WIFI_SME_MULTICAST_ADDRESS_CFM, CsrWifiSmeMulticastAddressCfmSizeof, CsrWifiSmeMulticastAddressCfmSer, CsrWifiSmeMulticastAddressCfmDes, CsrWifiSmeMulticastAddressCfmSerFree },
+    { CSR_WIFI_SME_PACKET_FILTER_SET_CFM, CsrWifiSmePacketFilterSetCfmSizeof, CsrWifiSmePacketFilterSetCfmSer, CsrWifiSmePacketFilterSetCfmDes, CsrWifiSmePacketFilterSetCfmSerFree },
+    { CSR_WIFI_SME_PERMANENT_MAC_ADDRESS_GET_CFM, CsrWifiSmePermanentMacAddressGetCfmSizeof, CsrWifiSmePermanentMacAddressGetCfmSer, CsrWifiSmePermanentMacAddressGetCfmDes, CsrWifiSmePermanentMacAddressGetCfmSerFree },
+    { CSR_WIFI_SME_PMKID_CANDIDATE_LIST_IND, CsrWifiSmePmkidCandidateListIndSizeof, CsrWifiSmePmkidCandidateListIndSer, CsrWifiSmePmkidCandidateListIndDes, CsrWifiSmePmkidCandidateListIndSerFree },
+    { CSR_WIFI_SME_PMKID_CFM, CsrWifiSmePmkidCfmSizeof, CsrWifiSmePmkidCfmSer, CsrWifiSmePmkidCfmDes, CsrWifiSmePmkidCfmSerFree },
+    { CSR_WIFI_SME_POWER_CONFIG_GET_CFM, CsrWifiSmePowerConfigGetCfmSizeof, CsrWifiSmePowerConfigGetCfmSer, CsrWifiSmePowerConfigGetCfmDes, CsrWifiSmePowerConfigGetCfmSerFree },
+    { CSR_WIFI_SME_POWER_CONFIG_SET_CFM, CsrWifiSmePowerConfigSetCfmSizeof, CsrWifiSmePowerConfigSetCfmSer, CsrWifiSmePowerConfigSetCfmDes, CsrWifiSmePowerConfigSetCfmSerFree },
+    { CSR_WIFI_SME_REGULATORY_DOMAIN_INFO_GET_CFM, CsrWifiSmeRegulatoryDomainInfoGetCfmSizeof, CsrWifiSmeRegulatoryDomainInfoGetCfmSer, CsrWifiSmeRegulatoryDomainInfoGetCfmDes, CsrWifiSmeRegulatoryDomainInfoGetCfmSerFree },
+    { CSR_WIFI_SME_ROAM_COMPLETE_IND, CsrWifiSmeRoamCompleteIndSizeof, CsrWifiSmeRoamCompleteIndSer, CsrWifiSmeRoamCompleteIndDes, CsrWifiSmeRoamCompleteIndSerFree },
+    { CSR_WIFI_SME_ROAM_START_IND, CsrWifiSmeRoamStartIndSizeof, CsrWifiSmeRoamStartIndSer, CsrWifiSmeRoamStartIndDes, CsrWifiSmeRoamStartIndSerFree },
+    { CSR_WIFI_SME_ROAMING_CONFIG_GET_CFM, CsrWifiSmeRoamingConfigGetCfmSizeof, CsrWifiSmeRoamingConfigGetCfmSer, CsrWifiSmeRoamingConfigGetCfmDes, CsrWifiSmeRoamingConfigGetCfmSerFree },
+    { CSR_WIFI_SME_ROAMING_CONFIG_SET_CFM, CsrWifiSmeRoamingConfigSetCfmSizeof, CsrWifiSmeRoamingConfigSetCfmSer, CsrWifiSmeRoamingConfigSetCfmDes, CsrWifiSmeRoamingConfigSetCfmSerFree },
+    { CSR_WIFI_SME_SCAN_CONFIG_GET_CFM, CsrWifiSmeScanConfigGetCfmSizeof, CsrWifiSmeScanConfigGetCfmSer, CsrWifiSmeScanConfigGetCfmDes, CsrWifiSmeScanConfigGetCfmSerFree },
+    { CSR_WIFI_SME_SCAN_CONFIG_SET_CFM, CsrWifiSmeScanConfigSetCfmSizeof, CsrWifiSmeScanConfigSetCfmSer, CsrWifiSmeScanConfigSetCfmDes, CsrWifiSmeScanConfigSetCfmSerFree },
+    { CSR_WIFI_SME_SCAN_FULL_CFM, CsrWifiSmeScanFullCfmSizeof, CsrWifiSmeScanFullCfmSer, CsrWifiSmeScanFullCfmDes, CsrWifiSmeScanFullCfmSerFree },
+    { CSR_WIFI_SME_SCAN_RESULT_IND, CsrWifiSmeScanResultIndSizeof, CsrWifiSmeScanResultIndSer, CsrWifiSmeScanResultIndDes, CsrWifiSmeScanResultIndSerFree },
+    { CSR_WIFI_SME_SCAN_RESULTS_FLUSH_CFM, CsrWifiSmeScanResultsFlushCfmSizeof, CsrWifiSmeScanResultsFlushCfmSer, CsrWifiSmeScanResultsFlushCfmDes, CsrWifiSmeScanResultsFlushCfmSerFree },
+    { CSR_WIFI_SME_SCAN_RESULTS_GET_CFM, CsrWifiSmeScanResultsGetCfmSizeof, CsrWifiSmeScanResultsGetCfmSer, CsrWifiSmeScanResultsGetCfmDes, CsrWifiSmeScanResultsGetCfmSerFree },
+    { CSR_WIFI_SME_SME_STA_CONFIG_GET_CFM, CsrWifiSmeSmeStaConfigGetCfmSizeof, CsrWifiSmeSmeStaConfigGetCfmSer, CsrWifiSmeSmeStaConfigGetCfmDes, CsrWifiSmeSmeStaConfigGetCfmSerFree },
+    { CSR_WIFI_SME_SME_STA_CONFIG_SET_CFM, CsrWifiSmeSmeStaConfigSetCfmSizeof, CsrWifiSmeSmeStaConfigSetCfmSer, CsrWifiSmeSmeStaConfigSetCfmDes, CsrWifiSmeSmeStaConfigSetCfmSerFree },
+    { CSR_WIFI_SME_STATION_MAC_ADDRESS_GET_CFM, CsrWifiSmeStationMacAddressGetCfmSizeof, CsrWifiSmeStationMacAddressGetCfmSer, CsrWifiSmeStationMacAddressGetCfmDes, CsrWifiSmeStationMacAddressGetCfmSerFree },
+    { CSR_WIFI_SME_TSPEC_IND, CsrWifiSmeTspecIndSizeof, CsrWifiSmeTspecIndSer, CsrWifiSmeTspecIndDes, CsrWifiSmeTspecIndSerFree },
+    { CSR_WIFI_SME_TSPEC_CFM, CsrWifiSmeTspecCfmSizeof, CsrWifiSmeTspecCfmSer, CsrWifiSmeTspecCfmDes, CsrWifiSmeTspecCfmSerFree },
+    { CSR_WIFI_SME_VERSIONS_GET_CFM, CsrWifiSmeVersionsGetCfmSizeof, CsrWifiSmeVersionsGetCfmSer, CsrWifiSmeVersionsGetCfmDes, CsrWifiSmeVersionsGetCfmSerFree },
+    { CSR_WIFI_SME_WIFI_FLIGHTMODE_CFM, CsrWifiSmeWifiFlightmodeCfmSizeof, CsrWifiSmeWifiFlightmodeCfmSer, CsrWifiSmeWifiFlightmodeCfmDes, CsrWifiSmeWifiFlightmodeCfmSerFree },
+    { CSR_WIFI_SME_WIFI_OFF_IND, CsrWifiSmeWifiOffIndSizeof, CsrWifiSmeWifiOffIndSer, CsrWifiSmeWifiOffIndDes, CsrWifiSmeWifiOffIndSerFree },
+    { CSR_WIFI_SME_WIFI_OFF_CFM, CsrWifiSmeWifiOffCfmSizeof, CsrWifiSmeWifiOffCfmSer, CsrWifiSmeWifiOffCfmDes, CsrWifiSmeWifiOffCfmSerFree },
+    { CSR_WIFI_SME_WIFI_ON_CFM, CsrWifiSmeWifiOnCfmSizeof, CsrWifiSmeWifiOnCfmSer, CsrWifiSmeWifiOnCfmDes, CsrWifiSmeWifiOnCfmSerFree },
+    { CSR_WIFI_SME_CLOAKED_SSIDS_SET_CFM, CsrWifiSmeCloakedSsidsSetCfmSizeof, CsrWifiSmeCloakedSsidsSetCfmSer, CsrWifiSmeCloakedSsidsSetCfmDes, CsrWifiSmeCloakedSsidsSetCfmSerFree },
+    { CSR_WIFI_SME_CLOAKED_SSIDS_GET_CFM, CsrWifiSmeCloakedSsidsGetCfmSizeof, CsrWifiSmeCloakedSsidsGetCfmSer, CsrWifiSmeCloakedSsidsGetCfmDes, CsrWifiSmeCloakedSsidsGetCfmSerFree },
+    { CSR_WIFI_SME_WIFI_ON_IND, CsrWifiSmeWifiOnIndSizeof, CsrWifiSmeWifiOnIndSer, CsrWifiSmeWifiOnIndDes, CsrWifiSmeWifiOnIndSerFree },
+    { CSR_WIFI_SME_SME_COMMON_CONFIG_GET_CFM, CsrWifiSmeSmeCommonConfigGetCfmSizeof, CsrWifiSmeSmeCommonConfigGetCfmSer, CsrWifiSmeSmeCommonConfigGetCfmDes, CsrWifiSmeSmeCommonConfigGetCfmSerFree },
+    { CSR_WIFI_SME_SME_COMMON_CONFIG_SET_CFM, CsrWifiSmeSmeCommonConfigSetCfmSizeof, CsrWifiSmeSmeCommonConfigSetCfmSer, CsrWifiSmeSmeCommonConfigSetCfmDes, CsrWifiSmeSmeCommonConfigSetCfmSerFree },
+    { CSR_WIFI_SME_INTERFACE_CAPABILITY_GET_CFM, CsrWifiSmeInterfaceCapabilityGetCfmSizeof, CsrWifiSmeInterfaceCapabilityGetCfmSer, CsrWifiSmeInterfaceCapabilityGetCfmDes, CsrWifiSmeInterfaceCapabilityGetCfmSerFree },
+    { CSR_WIFI_SME_ERROR_IND, CsrWifiSmeErrorIndSizeof, CsrWifiSmeErrorIndSer, CsrWifiSmeErrorIndDes, CsrWifiSmeErrorIndSerFree },
+    { CSR_WIFI_SME_INFO_IND, CsrWifiSmeInfoIndSizeof, CsrWifiSmeInfoIndSer, CsrWifiSmeInfoIndDes, CsrWifiSmeInfoIndSerFree },
+    { CSR_WIFI_SME_CORE_DUMP_IND, CsrWifiSmeCoreDumpIndSizeof, CsrWifiSmeCoreDumpIndSer, CsrWifiSmeCoreDumpIndDes, CsrWifiSmeCoreDumpIndSerFree },
+    { CSR_WIFI_SME_AMP_STATUS_CHANGE_IND, CsrWifiSmeAmpStatusChangeIndSizeof, CsrWifiSmeAmpStatusChangeIndSer, CsrWifiSmeAmpStatusChangeIndDes, CsrWifiSmeAmpStatusChangeIndSerFree },
+    { CSR_WIFI_SME_WPS_CONFIGURATION_CFM, CsrWifiSmeWpsConfigurationCfmSizeof, CsrWifiSmeWpsConfigurationCfmSer, CsrWifiSmeWpsConfigurationCfmDes, CsrWifiSmeWpsConfigurationCfmSerFree },
+
+    { 0, NULL, NULL, NULL, NULL },
+};
+
+CsrMsgConvMsgEntry* CsrWifiSmeConverterLookup(CsrMsgConvMsgEntry *ce, u16 msgType)
+{
+    if (msgType & CSR_PRIM_UPSTREAM)
+    {
+        u16 idx = (msgType & ~CSR_PRIM_UPSTREAM) + CSR_WIFI_SME_PRIM_DOWNSTREAM_COUNT;
+        if (idx < (CSR_WIFI_SME_PRIM_UPSTREAM_COUNT + CSR_WIFI_SME_PRIM_DOWNSTREAM_COUNT) &&
+            csrwifisme_conv_lut[idx].msgType == msgType)
+        {
+            return &csrwifisme_conv_lut[idx];
+        }
+    }
+    else
+    {
+        if (msgType < CSR_WIFI_SME_PRIM_DOWNSTREAM_COUNT &&
+            csrwifisme_conv_lut[msgType].msgType == msgType)
+        {
+            return &csrwifisme_conv_lut[msgType];
+        }
+    }
+    return NULL;
+}
+
+
+void CsrWifiSmeConverterInit(void)
+{
+    CsrMsgConvInsert(CSR_WIFI_SME_PRIM, csrwifisme_conv_lut);
+    CsrMsgConvCustomLookupRegister(CSR_WIFI_SME_PRIM, CsrWifiSmeConverterLookup);
+}
+
+
+#ifdef CSR_LOG_ENABLE
+static const CsrLogPrimitiveInformation csrwifisme_conv_info = {
+    CSR_WIFI_SME_PRIM,
+    (char *)"CSR_WIFI_SME_PRIM",
+    csrwifisme_conv_lut
+};
+const CsrLogPrimitiveInformation* CsrWifiSmeTechInfoGet(void)
+{
+    return &csrwifisme_conv_info;
+}
+
+
+#endif /* CSR_LOG_ENABLE */
+#endif /* EXCLUDE_CSR_WIFI_SME_MODULE */
diff --git a/drivers/staging/csr/csr_wifi_sme_converter_init.h b/drivers/staging/csr/csr_wifi_sme_converter_init.h
new file mode 100644
index 00000000000..fb895dec768
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_converter_init.h
@@ -0,0 +1,42 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_CONVERTER_INIT_H__
+#define CSR_WIFI_SME_CONVERTER_INIT_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef EXCLUDE_CSR_WIFI_SME_MODULE
+
+#include "csr_msgconv.h"
+
+#ifdef CSR_LOG_ENABLE
+#include "csr_log.h"
+
+extern const CsrLogPrimitiveInformation* CsrWifiSmeTechInfoGet(void);
+#endif /* CSR_LOG_ENABLE */
+
+extern void CsrWifiSmeConverterInit(void);
+
+#else /* EXCLUDE_CSR_WIFI_SME_MODULE */
+
+#define CsrWifiSmeConverterInit()
+
+#endif /* EXCLUDE_CSR_WIFI_SME_MODULE */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_SME_CONVERTER_INIT_H__ */
diff --git a/drivers/staging/csr/csr_wifi_sme_free_downstream_contents.c b/drivers/staging/csr/csr_wifi_sme_free_downstream_contents.c
new file mode 100644
index 00000000000..03b5ddb22cd
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_free_downstream_contents.c
@@ -0,0 +1,187 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_sme_prim.h"
+#include "csr_wifi_sme_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiSmeFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_SME_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiSmeFreeDownstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_SME_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiSmePrim *) message))
+    {
+        case CSR_WIFI_SME_BLACKLIST_REQ:
+        {
+            CsrWifiSmeBlacklistReq *p = (CsrWifiSmeBlacklistReq *)message;
+            kfree(p->setAddresses);
+            p->setAddresses = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CALIBRATION_DATA_SET_REQ:
+        {
+            CsrWifiSmeCalibrationDataSetReq *p = (CsrWifiSmeCalibrationDataSetReq *)message;
+            kfree(p->calibrationData);
+            p->calibrationData = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CONNECT_REQ:
+        {
+            CsrWifiSmeConnectReq *p = (CsrWifiSmeConnectReq *)message;
+            kfree(p->connectionConfig.mlmeAssociateReqInformationElements);
+            p->connectionConfig.mlmeAssociateReqInformationElements = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MIB_GET_NEXT_REQ:
+        {
+            CsrWifiSmeMibGetNextReq *p = (CsrWifiSmeMibGetNextReq *)message;
+            kfree(p->mibAttribute);
+            p->mibAttribute = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MIB_GET_REQ:
+        {
+            CsrWifiSmeMibGetReq *p = (CsrWifiSmeMibGetReq *)message;
+            kfree(p->mibAttribute);
+            p->mibAttribute = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MIB_SET_REQ:
+        {
+            CsrWifiSmeMibSetReq *p = (CsrWifiSmeMibSetReq *)message;
+            kfree(p->mibAttribute);
+            p->mibAttribute = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MULTICAST_ADDRESS_REQ:
+        {
+            CsrWifiSmeMulticastAddressReq *p = (CsrWifiSmeMulticastAddressReq *)message;
+            kfree(p->setAddresses);
+            p->setAddresses = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_PACKET_FILTER_SET_REQ:
+        {
+            CsrWifiSmePacketFilterSetReq *p = (CsrWifiSmePacketFilterSetReq *)message;
+            kfree(p->filter);
+            p->filter = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_PMKID_REQ:
+        {
+            CsrWifiSmePmkidReq *p = (CsrWifiSmePmkidReq *)message;
+            kfree(p->setPmkids);
+            p->setPmkids = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_SCAN_CONFIG_SET_REQ:
+        {
+            CsrWifiSmeScanConfigSetReq *p = (CsrWifiSmeScanConfigSetReq *)message;
+            kfree(p->scanConfig.passiveChannelList);
+            p->scanConfig.passiveChannelList = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_SCAN_FULL_REQ:
+        {
+            CsrWifiSmeScanFullReq *p = (CsrWifiSmeScanFullReq *)message;
+            kfree(p->ssid);
+            p->ssid = NULL;
+            kfree(p->channelList);
+            p->channelList = NULL;
+            kfree(p->probeIe);
+            p->probeIe = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_TSPEC_REQ:
+        {
+            CsrWifiSmeTspecReq *p = (CsrWifiSmeTspecReq *)message;
+            kfree(p->tspec);
+            p->tspec = NULL;
+            kfree(p->tclas);
+            p->tclas = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_WIFI_FLIGHTMODE_REQ:
+        {
+            CsrWifiSmeWifiFlightmodeReq *p = (CsrWifiSmeWifiFlightmodeReq *)message;
+            {
+                u16 i1;
+                for (i1 = 0; i1 < p->mibFilesCount; i1++)
+                {
+                    kfree(p->mibFiles[i1].data);
+                    p->mibFiles[i1].data = NULL;
+                }
+            }
+            kfree(p->mibFiles);
+            p->mibFiles = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_WIFI_ON_REQ:
+        {
+            CsrWifiSmeWifiOnReq *p = (CsrWifiSmeWifiOnReq *)message;
+            {
+                u16 i1;
+                for (i1 = 0; i1 < p->mibFilesCount; i1++)
+                {
+                    kfree(p->mibFiles[i1].data);
+                    p->mibFiles[i1].data = NULL;
+                }
+            }
+            kfree(p->mibFiles);
+            p->mibFiles = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CLOAKED_SSIDS_SET_REQ:
+        {
+            CsrWifiSmeCloakedSsidsSetReq *p = (CsrWifiSmeCloakedSsidsSetReq *)message;
+            kfree(p->cloakedSsids.cloakedSsids);
+            p->cloakedSsids.cloakedSsids = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_WPS_CONFIGURATION_REQ:
+        {
+            CsrWifiSmeWpsConfigurationReq *p = (CsrWifiSmeWpsConfigurationReq *)message;
+            kfree(p->wpsConfig.secondaryDeviceType);
+            p->wpsConfig.secondaryDeviceType = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_SET_REQ:
+        {
+            CsrWifiSmeSetReq *p = (CsrWifiSmeSetReq *)message;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_sme_free_upstream_contents.c b/drivers/staging/csr/csr_wifi_sme_free_upstream_contents.c
new file mode 100644
index 00000000000..c04767baaa5
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_free_upstream_contents.c
@@ -0,0 +1,275 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/slab.h>
+#include "csr_wifi_sme_prim.h"
+#include "csr_wifi_sme_lib.h"
+
+/*----------------------------------------------------------------------------*
+ *  NAME
+ *      CsrWifiSmeFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *
+ *
+ *  PARAMETERS
+ *      eventClass: only the value CSR_WIFI_SME_PRIM will be handled
+ *      message:    the message to free
+ *----------------------------------------------------------------------------*/
+void CsrWifiSmeFreeUpstreamMessageContents(u16 eventClass, void *message)
+{
+    if (eventClass != CSR_WIFI_SME_PRIM)
+    {
+        return;
+    }
+    if (NULL == message)
+    {
+        return;
+    }
+
+    switch (*((CsrWifiSmePrim *) message))
+    {
+        case CSR_WIFI_SME_ASSOCIATION_COMPLETE_IND:
+        {
+            CsrWifiSmeAssociationCompleteInd *p = (CsrWifiSmeAssociationCompleteInd *)message;
+            kfree(p->connectionInfo.beaconFrame);
+            p->connectionInfo.beaconFrame = NULL;
+            kfree(p->connectionInfo.associationReqFrame);
+            p->connectionInfo.associationReqFrame = NULL;
+            kfree(p->connectionInfo.associationRspFrame);
+            p->connectionInfo.associationRspFrame = NULL;
+            kfree(p->connectionInfo.assocScanInfoElements);
+            p->connectionInfo.assocScanInfoElements = NULL;
+            kfree(p->connectionInfo.assocReqInfoElements);
+            p->connectionInfo.assocReqInfoElements = NULL;
+            kfree(p->connectionInfo.assocRspInfoElements);
+            p->connectionInfo.assocRspInfoElements = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_BLACKLIST_CFM:
+        {
+            CsrWifiSmeBlacklistCfm *p = (CsrWifiSmeBlacklistCfm *)message;
+            kfree(p->getAddresses);
+            p->getAddresses = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CALIBRATION_DATA_GET_CFM:
+        {
+            CsrWifiSmeCalibrationDataGetCfm *p = (CsrWifiSmeCalibrationDataGetCfm *)message;
+            kfree(p->calibrationData);
+            p->calibrationData = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CONNECTION_CONFIG_GET_CFM:
+        {
+            CsrWifiSmeConnectionConfigGetCfm *p = (CsrWifiSmeConnectionConfigGetCfm *)message;
+            kfree(p->connectionConfig.mlmeAssociateReqInformationElements);
+            p->connectionConfig.mlmeAssociateReqInformationElements = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CONNECTION_INFO_GET_CFM:
+        {
+            CsrWifiSmeConnectionInfoGetCfm *p = (CsrWifiSmeConnectionInfoGetCfm *)message;
+            kfree(p->connectionInfo.beaconFrame);
+            p->connectionInfo.beaconFrame = NULL;
+            kfree(p->connectionInfo.associationReqFrame);
+            p->connectionInfo.associationReqFrame = NULL;
+            kfree(p->connectionInfo.associationRspFrame);
+            p->connectionInfo.associationRspFrame = NULL;
+            kfree(p->connectionInfo.assocScanInfoElements);
+            p->connectionInfo.assocScanInfoElements = NULL;
+            kfree(p->connectionInfo.assocReqInfoElements);
+            p->connectionInfo.assocReqInfoElements = NULL;
+            kfree(p->connectionInfo.assocRspInfoElements);
+            p->connectionInfo.assocRspInfoElements = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MEDIA_STATUS_IND:
+        {
+            CsrWifiSmeMediaStatusInd *p = (CsrWifiSmeMediaStatusInd *)message;
+            kfree(p->connectionInfo.beaconFrame);
+            p->connectionInfo.beaconFrame = NULL;
+            kfree(p->connectionInfo.associationReqFrame);
+            p->connectionInfo.associationReqFrame = NULL;
+            kfree(p->connectionInfo.associationRspFrame);
+            p->connectionInfo.associationRspFrame = NULL;
+            kfree(p->connectionInfo.assocScanInfoElements);
+            p->connectionInfo.assocScanInfoElements = NULL;
+            kfree(p->connectionInfo.assocReqInfoElements);
+            p->connectionInfo.assocReqInfoElements = NULL;
+            kfree(p->connectionInfo.assocRspInfoElements);
+            p->connectionInfo.assocRspInfoElements = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MIB_GET_CFM:
+        {
+            CsrWifiSmeMibGetCfm *p = (CsrWifiSmeMibGetCfm *)message;
+            kfree(p->mibAttribute);
+            p->mibAttribute = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MIB_GET_NEXT_CFM:
+        {
+            CsrWifiSmeMibGetNextCfm *p = (CsrWifiSmeMibGetNextCfm *)message;
+            kfree(p->mibAttribute);
+            p->mibAttribute = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_MULTICAST_ADDRESS_CFM:
+        {
+            CsrWifiSmeMulticastAddressCfm *p = (CsrWifiSmeMulticastAddressCfm *)message;
+            kfree(p->getAddresses);
+            p->getAddresses = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_PMKID_CANDIDATE_LIST_IND:
+        {
+            CsrWifiSmePmkidCandidateListInd *p = (CsrWifiSmePmkidCandidateListInd *)message;
+            kfree(p->pmkidCandidates);
+            p->pmkidCandidates = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_PMKID_CFM:
+        {
+            CsrWifiSmePmkidCfm *p = (CsrWifiSmePmkidCfm *)message;
+            kfree(p->getPmkids);
+            p->getPmkids = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_SCAN_CONFIG_GET_CFM:
+        {
+            CsrWifiSmeScanConfigGetCfm *p = (CsrWifiSmeScanConfigGetCfm *)message;
+            kfree(p->scanConfig.passiveChannelList);
+            p->scanConfig.passiveChannelList = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_SCAN_RESULT_IND:
+        {
+            CsrWifiSmeScanResultInd *p = (CsrWifiSmeScanResultInd *)message;
+            kfree(p->result.informationElements);
+            p->result.informationElements = NULL;
+            switch (p->result.p2pDeviceRole)
+            {
+                case CSR_WIFI_SME_P2P_ROLE_GO:
+                {
+                    u16 i4;
+                    for (i4 = 0; i4 < p->result.deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                    {
+                        kfree(p->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType);
+                        p->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType = NULL;
+                    }
+                }
+                    kfree(p->result.deviceInfo.groupInfo.p2PClientInfo);
+                    p->result.deviceInfo.groupInfo.p2PClientInfo = NULL;
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+                    kfree(p->result.deviceInfo.standalonedevInfo.secDeviceType);
+                    p->result.deviceInfo.standalonedevInfo.secDeviceType = NULL;
+                    break;
+                default:
+                    break;
+            }
+            break;
+        }
+        case CSR_WIFI_SME_SCAN_RESULTS_GET_CFM:
+        {
+            CsrWifiSmeScanResultsGetCfm *p = (CsrWifiSmeScanResultsGetCfm *)message;
+            {
+                u16 i1;
+                for (i1 = 0; i1 < p->scanResultsCount; i1++)
+                {
+                    kfree(p->scanResults[i1].informationElements);
+                    p->scanResults[i1].informationElements = NULL;
+                    switch (p->scanResults[i1].p2pDeviceRole)
+                    {
+                        case CSR_WIFI_SME_P2P_ROLE_GO:
+                        {
+                            u16 i4;
+                            for (i4 = 0; i4 < p->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                            {
+                                kfree(p->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType);
+                                p->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType = NULL;
+                            }
+                        }
+                            kfree(p->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo);
+                            p->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo = NULL;
+                            break;
+                        case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+                            kfree(p->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType);
+                            p->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType = NULL;
+                            break;
+                        default:
+                            break;
+                    }
+                }
+            }
+            kfree(p->scanResults);
+            p->scanResults = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_TSPEC_IND:
+        {
+            CsrWifiSmeTspecInd *p = (CsrWifiSmeTspecInd *)message;
+            kfree(p->tspec);
+            p->tspec = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_TSPEC_CFM:
+        {
+            CsrWifiSmeTspecCfm *p = (CsrWifiSmeTspecCfm *)message;
+            kfree(p->tspec);
+            p->tspec = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_VERSIONS_GET_CFM:
+        {
+            CsrWifiSmeVersionsGetCfm *p = (CsrWifiSmeVersionsGetCfm *)message;
+            kfree(p->versions.routerBuild);
+            p->versions.routerBuild = NULL;
+            kfree(p->versions.smeBuild);
+            p->versions.smeBuild = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CLOAKED_SSIDS_GET_CFM:
+        {
+            CsrWifiSmeCloakedSsidsGetCfm *p = (CsrWifiSmeCloakedSsidsGetCfm *)message;
+            kfree(p->cloakedSsids.cloakedSsids);
+            p->cloakedSsids.cloakedSsids = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_ERROR_IND:
+        {
+            CsrWifiSmeErrorInd *p = (CsrWifiSmeErrorInd *)message;
+            kfree(p->errorMessage);
+            p->errorMessage = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_INFO_IND:
+        {
+            CsrWifiSmeInfoInd *p = (CsrWifiSmeInfoInd *)message;
+            kfree(p->infoMessage);
+            p->infoMessage = NULL;
+            break;
+        }
+        case CSR_WIFI_SME_CORE_DUMP_IND:
+        {
+            CsrWifiSmeCoreDumpInd *p = (CsrWifiSmeCoreDumpInd *)message;
+            kfree(p->data);
+            p->data = NULL;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_sme_lib.h b/drivers/staging/csr/csr_wifi_sme_lib.h
new file mode 100644
index 00000000000..3ca74560825
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_lib.h
@@ -0,0 +1,4313 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_LIB_H__
+#define CSR_WIFI_SME_LIB_H__
+
+#include "csr_sched.h"
+#include "csr_macro.h"
+#include "csr_msg_transport.h"
+
+#include "csr_wifi_lib.h"
+
+#include "csr_wifi_sme_prim.h"
+#include "csr_wifi_sme_task.h"
+
+
+#ifndef CSR_WIFI_SME_LIB_DESTINATION_QUEUE
+# ifdef CSR_WIFI_NME_ENABLE
+# include "csr_wifi_nme_task.h"
+# define CSR_WIFI_SME_LIB_DESTINATION_QUEUE CSR_WIFI_NME_IFACEQUEUE
+# else
+# define CSR_WIFI_SME_LIB_DESTINATION_QUEUE CSR_WIFI_SME_IFACEQUEUE
+# endif
+#endif
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiSmeFreeUpstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_SME upstream message. Does not
+ *      free the message itself, and can only be used for upstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_SME upstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiSmeFreeUpstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ *  CsrWifiSmeFreeDownstreamMessageContents
+ *
+ *  DESCRIPTION
+ *      Free the allocated memory in a CSR_WIFI_SME downstream message. Does not
+ *      free the message itself, and can only be used for downstream messages.
+ *
+ *  PARAMETERS
+ *      Deallocates the resources in a CSR_WIFI_SME downstream message
+ *----------------------------------------------------------------------------*/
+void CsrWifiSmeFreeDownstreamMessageContents(u16 eventClass, void *message);
+
+/*----------------------------------------------------------------------------*
+ * Enum to string functions
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiSme80211NetworkTypeToString(CsrWifiSme80211NetworkType value);
+const char* CsrWifiSme80211PrivacyModeToString(CsrWifiSme80211PrivacyMode value);
+const char* CsrWifiSme80211dTrustLevelToString(CsrWifiSme80211dTrustLevel value);
+const char* CsrWifiSmeAmpStatusToString(CsrWifiSmeAmpStatus value);
+const char* CsrWifiSmeAuthModeToString(CsrWifiSmeAuthMode value);
+const char* CsrWifiSmeBasicUsabilityToString(CsrWifiSmeBasicUsability value);
+const char* CsrWifiSmeBssTypeToString(CsrWifiSmeBssType value);
+const char* CsrWifiSmeCoexSchemeToString(CsrWifiSmeCoexScheme value);
+const char* CsrWifiSmeControlIndicationToString(CsrWifiSmeControlIndication value);
+const char* CsrWifiSmeCtsProtectionTypeToString(CsrWifiSmeCtsProtectionType value);
+const char* CsrWifiSmeD3AutoScanModeToString(CsrWifiSmeD3AutoScanMode value);
+const char* CsrWifiSmeEncryptionToString(CsrWifiSmeEncryption value);
+const char* CsrWifiSmeFirmwareDriverInterfaceToString(CsrWifiSmeFirmwareDriverInterface value);
+const char* CsrWifiSmeHostPowerModeToString(CsrWifiSmeHostPowerMode value);
+const char* CsrWifiSmeIEEE80211ReasonToString(CsrWifiSmeIEEE80211Reason value);
+const char* CsrWifiSmeIEEE80211ResultToString(CsrWifiSmeIEEE80211Result value);
+const char* CsrWifiSmeIndicationsToString(CsrWifiSmeIndications value);
+const char* CsrWifiSmeKeyTypeToString(CsrWifiSmeKeyType value);
+const char* CsrWifiSmeListActionToString(CsrWifiSmeListAction value);
+const char* CsrWifiSmeMediaStatusToString(CsrWifiSmeMediaStatus value);
+const char* CsrWifiSmeP2pCapabilityToString(CsrWifiSmeP2pCapability value);
+const char* CsrWifiSmeP2pGroupCapabilityToString(CsrWifiSmeP2pGroupCapability value);
+const char* CsrWifiSmeP2pNoaConfigMethodToString(CsrWifiSmeP2pNoaConfigMethod value);
+const char* CsrWifiSmeP2pRoleToString(CsrWifiSmeP2pRole value);
+const char* CsrWifiSmeP2pStatusToString(CsrWifiSmeP2pStatus value);
+const char* CsrWifiSmePacketFilterModeToString(CsrWifiSmePacketFilterMode value);
+const char* CsrWifiSmePowerSaveLevelToString(CsrWifiSmePowerSaveLevel value);
+const char* CsrWifiSmePreambleTypeToString(CsrWifiSmePreambleType value);
+const char* CsrWifiSmeRadioIFToString(CsrWifiSmeRadioIF value);
+const char* CsrWifiSmeRegulatoryDomainToString(CsrWifiSmeRegulatoryDomain value);
+const char* CsrWifiSmeRoamReasonToString(CsrWifiSmeRoamReason value);
+const char* CsrWifiSmeScanTypeToString(CsrWifiSmeScanType value);
+const char* CsrWifiSmeTrafficTypeToString(CsrWifiSmeTrafficType value);
+const char* CsrWifiSmeTspecCtrlToString(CsrWifiSmeTspecCtrl value);
+const char* CsrWifiSmeTspecResultCodeToString(CsrWifiSmeTspecResultCode value);
+const char* CsrWifiSmeWepAuthModeToString(CsrWifiSmeWepAuthMode value);
+const char* CsrWifiSmeWepCredentialTypeToString(CsrWifiSmeWepCredentialType value);
+const char* CsrWifiSmeWmmModeToString(CsrWifiSmeWmmMode value);
+const char* CsrWifiSmeWmmQosInfoToString(CsrWifiSmeWmmQosInfo value);
+const char* CsrWifiSmeWpsConfigTypeToString(CsrWifiSmeWpsConfigType value);
+const char* CsrWifiSmeWpsDeviceCategoryToString(CsrWifiSmeWpsDeviceCategory value);
+const char* CsrWifiSmeWpsDeviceSubCategoryToString(CsrWifiSmeWpsDeviceSubCategory value);
+const char* CsrWifiSmeWpsDpidToString(CsrWifiSmeWpsDpid value);
+const char* CsrWifiSmeWpsRegistrationToString(CsrWifiSmeWpsRegistration value);
+
+
+/*----------------------------------------------------------------------------*
+ * CsrPrim Type toString function.
+ * Converts a message type to the String name of the Message
+ *----------------------------------------------------------------------------*/
+const char* CsrWifiSmePrimTypeToString(CsrPrim msgType);
+
+/*----------------------------------------------------------------------------*
+ * Lookup arrays for PrimType name Strings
+ *----------------------------------------------------------------------------*/
+extern const char *CsrWifiSmeUpstreamPrimNames[CSR_WIFI_SME_PRIM_UPSTREAM_COUNT];
+extern const char *CsrWifiSmeDownstreamPrimNames[CSR_WIFI_SME_PRIM_DOWNSTREAM_COUNT];
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeActivateReqSend
+
+  DESCRIPTION
+    The WMA sends this primitive to activate the SME.
+    The WMA must activate the SME before it can send any other primitive.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeActivateReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeActivateReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ACTIVATE_REQ, dst__, src__);
+
+#define CsrWifiSmeActivateReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeActivateReq *msg__; \
+        CsrWifiSmeActivateReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeActivateReqSend(src__) \
+    CsrWifiSmeActivateReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeActivateCfmSend
+
+  DESCRIPTION
+    The SME sends this primitive when the activation is complete.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeActivateCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeActivateCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ACTIVATE_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeActivateCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeActivateCfm *msg__; \
+        CsrWifiSmeActivateCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeActivateCfmSend(dst__, status__) \
+    CsrWifiSmeActivateCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the adHocConfig parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeAdhocConfigGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAdhocConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ADHOC_CONFIG_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeAdhocConfigGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeAdhocConfigGetReq *msg__; \
+        CsrWifiSmeAdhocConfigGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAdhocConfigGetReqSend(src__) \
+    CsrWifiSmeAdhocConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue       - Destination Task Queue
+    status      - Reports the result of the request
+    adHocConfig - Contains the values used when starting an Ad-hoc (IBSS)
+                  connection.
+
+*******************************************************************************/
+#define CsrWifiSmeAdhocConfigGetCfmCreate(msg__, dst__, src__, status__, adHocConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAdhocConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ADHOC_CONFIG_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->adHocConfig = (adHocConfig__);
+
+#define CsrWifiSmeAdhocConfigGetCfmSendTo(dst__, src__, status__, adHocConfig__) \
+    { \
+        CsrWifiSmeAdhocConfigGetCfm *msg__; \
+        CsrWifiSmeAdhocConfigGetCfmCreate(msg__, dst__, src__, status__, adHocConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAdhocConfigGetCfmSend(dst__, status__, adHocConfig__) \
+    CsrWifiSmeAdhocConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, adHocConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the adHocConfig parameter.
+
+  PARAMETERS
+    queue       - Message Source Task Queue (Cfm's will be sent to this Queue)
+    adHocConfig - Sets the values to use when starting an ad hoc network.
+
+*******************************************************************************/
+#define CsrWifiSmeAdhocConfigSetReqCreate(msg__, dst__, src__, adHocConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAdhocConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ADHOC_CONFIG_SET_REQ, dst__, src__); \
+    msg__->adHocConfig = (adHocConfig__);
+
+#define CsrWifiSmeAdhocConfigSetReqSendTo(dst__, src__, adHocConfig__) \
+    { \
+        CsrWifiSmeAdhocConfigSetReq *msg__; \
+        CsrWifiSmeAdhocConfigSetReqCreate(msg__, dst__, src__, adHocConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAdhocConfigSetReqSend(src__, adHocConfig__) \
+    CsrWifiSmeAdhocConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, adHocConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeAdhocConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAdhocConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ADHOC_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeAdhocConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeAdhocConfigSetCfm *msg__; \
+        CsrWifiSmeAdhocConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAdhocConfigSetCfmSend(dst__, status__) \
+    CsrWifiSmeAdhocConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAmpStatusChangeIndSend
+
+  DESCRIPTION
+    Indication of change to AMP activity.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface on which the AMP activity changed.
+    ampStatus    - The new status of AMP activity.Range: {AMP_ACTIVE,
+                   AMP_INACTIVE}.
+
+*******************************************************************************/
+#define CsrWifiSmeAmpStatusChangeIndCreate(msg__, dst__, src__, interfaceTag__, ampStatus__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAmpStatusChangeInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_AMP_STATUS_CHANGE_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->ampStatus = (ampStatus__);
+
+#define CsrWifiSmeAmpStatusChangeIndSendTo(dst__, src__, interfaceTag__, ampStatus__) \
+    { \
+        CsrWifiSmeAmpStatusChangeInd *msg__; \
+        CsrWifiSmeAmpStatusChangeIndCreate(msg__, dst__, src__, interfaceTag__, ampStatus__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAmpStatusChangeIndSend(dst__, interfaceTag__, ampStatus__) \
+    CsrWifiSmeAmpStatusChangeIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, ampStatus__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAssociationCompleteIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it completes an attempt to associate with an AP. If
+    the association was successful, status will be set to
+    CSR_WIFI_SME_STATUS_SUCCESS, otherwise status and deauthReason shall be
+    set to appropriate error codes.
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the association procedure
+    connectionInfo - This parameter is relevant only if result is
+                     CSR_WIFI_SME_STATUS_SUCCESS:
+                     it points to the connection information for the new network
+    deauthReason   - This parameter is relevant only if result is not
+                     CSR_WIFI_SME_STATUS_SUCCESS:
+                     if the AP deauthorised the station, it gives the reason of
+                     the deauthorization
+
+*******************************************************************************/
+#define CsrWifiSmeAssociationCompleteIndCreate(msg__, dst__, src__, interfaceTag__, status__, connectionInfo__, deauthReason__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAssociationCompleteInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ASSOCIATION_COMPLETE_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->connectionInfo = (connectionInfo__); \
+    msg__->deauthReason = (deauthReason__);
+
+#define CsrWifiSmeAssociationCompleteIndSendTo(dst__, src__, interfaceTag__, status__, connectionInfo__, deauthReason__) \
+    { \
+        CsrWifiSmeAssociationCompleteInd *msg__; \
+        CsrWifiSmeAssociationCompleteIndCreate(msg__, dst__, src__, interfaceTag__, status__, connectionInfo__, deauthReason__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAssociationCompleteIndSend(dst__, interfaceTag__, status__, connectionInfo__, deauthReason__) \
+    CsrWifiSmeAssociationCompleteIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, connectionInfo__, deauthReason__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAssociationStartIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it begins an attempt to associate with an AP.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    address      - BSSID of the associating network
+    ssid         - Service Set identifier of the associating network
+
+*******************************************************************************/
+#define CsrWifiSmeAssociationStartIndCreate(msg__, dst__, src__, interfaceTag__, address__, ssid__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeAssociationStartInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ASSOCIATION_START_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->address = (address__); \
+    msg__->ssid = (ssid__);
+
+#define CsrWifiSmeAssociationStartIndSendTo(dst__, src__, interfaceTag__, address__, ssid__) \
+    { \
+        CsrWifiSmeAssociationStartInd *msg__; \
+        CsrWifiSmeAssociationStartIndCreate(msg__, dst__, src__, interfaceTag__, address__, ssid__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeAssociationStartIndSend(dst__, interfaceTag__, address__, ssid__) \
+    CsrWifiSmeAssociationStartIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, address__, ssid__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeBlacklistReqSend
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to notify the
+    driver of any networks that should not be connected to. The interface
+    allows the wireless manager application to query, add, remove, and flush
+    the BSSIDs that the driver may not connect or roam to.
+    When this primitive adds to the black list the BSSID to which the SME is
+    currently connected, the SME will try to roam, if applicable, to another
+    BSSID in the same ESS; if the roaming procedure fails, the SME will
+    disconnect.
+
+  PARAMETERS
+    queue           - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    action          - The value of the CsrWifiSmeListAction parameter instructs
+                      the driver to modify or provide the list of blacklisted
+                      networks.
+    setAddressCount - Number of BSSIDs sent with this primitive
+    setAddresses    - Pointer to the list of BBSIDs sent with the primitive, set
+                      to NULL if none is sent.
+
+*******************************************************************************/
+#define CsrWifiSmeBlacklistReqCreate(msg__, dst__, src__, interfaceTag__, action__, setAddressCount__, setAddresses__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeBlacklistReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_BLACKLIST_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->action = (action__); \
+    msg__->setAddressCount = (setAddressCount__); \
+    msg__->setAddresses = (setAddresses__);
+
+#define CsrWifiSmeBlacklistReqSendTo(dst__, src__, interfaceTag__, action__, setAddressCount__, setAddresses__) \
+    { \
+        CsrWifiSmeBlacklistReq *msg__; \
+        CsrWifiSmeBlacklistReqCreate(msg__, dst__, src__, interfaceTag__, action__, setAddressCount__, setAddresses__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeBlacklistReqSend(src__, interfaceTag__, action__, setAddressCount__, setAddresses__) \
+    CsrWifiSmeBlacklistReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, action__, setAddressCount__, setAddresses__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeBlacklistCfmSend
+
+  DESCRIPTION
+    The SME will call this primitive when the action on the blacklist has
+    completed. For a GET action, this primitive also reports the list of
+    BBSIDs in the blacklist.
+
+  PARAMETERS
+    queue           - Destination Task Queue
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    status          - Reports the result of the request
+    action          - Action in the request
+    getAddressCount - This parameter is only relevant if action is
+                      CSR_WIFI_SME_LIST_ACTION_GET:
+                      number of BSSIDs sent with this primitive
+    getAddresses    - Pointer to the list of BBSIDs sent with the primitive, set
+                      to NULL if none is sent.
+
+*******************************************************************************/
+#define CsrWifiSmeBlacklistCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, getAddressCount__, getAddresses__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeBlacklistCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_BLACKLIST_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->action = (action__); \
+    msg__->getAddressCount = (getAddressCount__); \
+    msg__->getAddresses = (getAddresses__);
+
+#define CsrWifiSmeBlacklistCfmSendTo(dst__, src__, interfaceTag__, status__, action__, getAddressCount__, getAddresses__) \
+    { \
+        CsrWifiSmeBlacklistCfm *msg__; \
+        CsrWifiSmeBlacklistCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, getAddressCount__, getAddresses__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeBlacklistCfmSend(dst__, interfaceTag__, status__, action__, getAddressCount__, getAddresses__) \
+    CsrWifiSmeBlacklistCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, action__, getAddressCount__, getAddresses__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataGetReqSend
+
+  DESCRIPTION
+    This primitive retrieves the Wi-Fi radio calibration data.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeCalibrationDataGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCalibrationDataGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CALIBRATION_DATA_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeCalibrationDataGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeCalibrationDataGetReq *msg__; \
+        CsrWifiSmeCalibrationDataGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCalibrationDataGetReqSend(src__) \
+    CsrWifiSmeCalibrationDataGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue                 - Destination Task Queue
+    status                - Reports the result of the request
+    calibrationDataLength - Number of bytes in the buffer pointed by
+                            calibrationData
+    calibrationData       - Pointer to a buffer of length calibrationDataLength
+                            containing the calibration data
+
+*******************************************************************************/
+#define CsrWifiSmeCalibrationDataGetCfmCreate(msg__, dst__, src__, status__, calibrationDataLength__, calibrationData__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCalibrationDataGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CALIBRATION_DATA_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->calibrationDataLength = (calibrationDataLength__); \
+    msg__->calibrationData = (calibrationData__);
+
+#define CsrWifiSmeCalibrationDataGetCfmSendTo(dst__, src__, status__, calibrationDataLength__, calibrationData__) \
+    { \
+        CsrWifiSmeCalibrationDataGetCfm *msg__; \
+        CsrWifiSmeCalibrationDataGetCfmCreate(msg__, dst__, src__, status__, calibrationDataLength__, calibrationData__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCalibrationDataGetCfmSend(dst__, status__, calibrationDataLength__, calibrationData__) \
+    CsrWifiSmeCalibrationDataGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, calibrationDataLength__, calibrationData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataSetReqSend
+
+  DESCRIPTION
+    This primitive sets the Wi-Fi radio calibration data.
+    The usage of the primitive with proper calibration data will avoid
+    time-consuming configuration after power-up.
+
+  PARAMETERS
+    queue                 - Message Source Task Queue (Cfm's will be sent to this Queue)
+    calibrationDataLength - Number of bytes in the buffer pointed by
+                            calibrationData
+    calibrationData       - Pointer to a buffer of length calibrationDataLength
+                            containing the calibration data
+
+*******************************************************************************/
+#define CsrWifiSmeCalibrationDataSetReqCreate(msg__, dst__, src__, calibrationDataLength__, calibrationData__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCalibrationDataSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CALIBRATION_DATA_SET_REQ, dst__, src__); \
+    msg__->calibrationDataLength = (calibrationDataLength__); \
+    msg__->calibrationData = (calibrationData__);
+
+#define CsrWifiSmeCalibrationDataSetReqSendTo(dst__, src__, calibrationDataLength__, calibrationData__) \
+    { \
+        CsrWifiSmeCalibrationDataSetReq *msg__; \
+        CsrWifiSmeCalibrationDataSetReqCreate(msg__, dst__, src__, calibrationDataLength__, calibrationData__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCalibrationDataSetReqSend(src__, calibrationDataLength__, calibrationData__) \
+    CsrWifiSmeCalibrationDataSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, calibrationDataLength__, calibrationData__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeCalibrationDataSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCalibrationDataSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CALIBRATION_DATA_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeCalibrationDataSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeCalibrationDataSetCfm *msg__; \
+        CsrWifiSmeCalibrationDataSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCalibrationDataSetCfmSend(dst__, status__) \
+    CsrWifiSmeCalibrationDataSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the CcxConfig parameter.
+    CURRENTLY NOT SUPPORTED.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeCcxConfigGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCcxConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CCX_CONFIG_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeCcxConfigGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeCcxConfigGetReq *msg__; \
+        CsrWifiSmeCcxConfigGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCcxConfigGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeCcxConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    ccxConfig    - Currently not supported
+
+*******************************************************************************/
+#define CsrWifiSmeCcxConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, ccxConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCcxConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CCX_CONFIG_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->ccxConfig = (ccxConfig__);
+
+#define CsrWifiSmeCcxConfigGetCfmSendTo(dst__, src__, interfaceTag__, status__, ccxConfig__) \
+    { \
+        CsrWifiSmeCcxConfigGetCfm *msg__; \
+        CsrWifiSmeCcxConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, ccxConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCcxConfigGetCfmSend(dst__, interfaceTag__, status__, ccxConfig__) \
+    CsrWifiSmeCcxConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, ccxConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the CcxConfig parameter.
+    CURRENTLY NOT SUPPORTED.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    ccxConfig    - Currently not supported
+
+*******************************************************************************/
+#define CsrWifiSmeCcxConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, ccxConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCcxConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CCX_CONFIG_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->ccxConfig = (ccxConfig__);
+
+#define CsrWifiSmeCcxConfigSetReqSendTo(dst__, src__, interfaceTag__, ccxConfig__) \
+    { \
+        CsrWifiSmeCcxConfigSetReq *msg__; \
+        CsrWifiSmeCcxConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, ccxConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCcxConfigSetReqSend(src__, interfaceTag__, ccxConfig__) \
+    CsrWifiSmeCcxConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, ccxConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeCcxConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCcxConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CCX_CONFIG_SET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeCcxConfigSetCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeCcxConfigSetCfm *msg__; \
+        CsrWifiSmeCcxConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCcxConfigSetCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeCcxConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the CloakedSsids parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeCloakedSsidsGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCloakedSsidsGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CLOAKED_SSIDS_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeCloakedSsidsGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeCloakedSsidsGetReq *msg__; \
+        CsrWifiSmeCloakedSsidsGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCloakedSsidsGetReqSend(src__) \
+    CsrWifiSmeCloakedSsidsGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    status       - Reports the result of the request
+    cloakedSsids - Reports list of cloaked SSIDs that are explicitly scanned for
+                   by the driver
+
+*******************************************************************************/
+#define CsrWifiSmeCloakedSsidsGetCfmCreate(msg__, dst__, src__, status__, cloakedSsids__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCloakedSsidsGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CLOAKED_SSIDS_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->cloakedSsids = (cloakedSsids__);
+
+#define CsrWifiSmeCloakedSsidsGetCfmSendTo(dst__, src__, status__, cloakedSsids__) \
+    { \
+        CsrWifiSmeCloakedSsidsGetCfm *msg__; \
+        CsrWifiSmeCloakedSsidsGetCfmCreate(msg__, dst__, src__, status__, cloakedSsids__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCloakedSsidsGetCfmSend(dst__, status__, cloakedSsids__) \
+    CsrWifiSmeCloakedSsidsGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, cloakedSsids__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsSetReqSend
+
+  DESCRIPTION
+    This primitive sets the list of cloaked SSIDs for which the WMA possesses
+    profiles.
+    When the driver detects a cloaked AP, the SME will explicitly scan for it
+    using the list of cloaked SSIDs provided it, and, if the scan succeeds,
+    it will report the AP to the WMA either via CSR_WIFI_SME_SCAN_RESULT_IND
+    (if registered) or via CSR_WIFI_SCAN_RESULT_GET_CFM.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    cloakedSsids - Sets the list of cloaked SSIDs
+
+*******************************************************************************/
+#define CsrWifiSmeCloakedSsidsSetReqCreate(msg__, dst__, src__, cloakedSsids__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCloakedSsidsSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CLOAKED_SSIDS_SET_REQ, dst__, src__); \
+    msg__->cloakedSsids = (cloakedSsids__);
+
+#define CsrWifiSmeCloakedSsidsSetReqSendTo(dst__, src__, cloakedSsids__) \
+    { \
+        CsrWifiSmeCloakedSsidsSetReq *msg__; \
+        CsrWifiSmeCloakedSsidsSetReqCreate(msg__, dst__, src__, cloakedSsids__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCloakedSsidsSetReqSend(src__, cloakedSsids__) \
+    CsrWifiSmeCloakedSsidsSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, cloakedSsids__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeCloakedSsidsSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCloakedSsidsSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CLOAKED_SSIDS_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeCloakedSsidsSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeCloakedSsidsSetCfm *msg__; \
+        CsrWifiSmeCloakedSsidsSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCloakedSsidsSetCfmSend(dst__, status__) \
+    CsrWifiSmeCloakedSsidsSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the CoexConfig parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeCoexConfigGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoexConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_COEX_CONFIG_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeCoexConfigGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeCoexConfigGetReq *msg__; \
+        CsrWifiSmeCoexConfigGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoexConfigGetReqSend(src__) \
+    CsrWifiSmeCoexConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    status     - Reports the result of the request
+    coexConfig - Reports the parameters used to configure the coexistence
+                 behaviour
+
+*******************************************************************************/
+#define CsrWifiSmeCoexConfigGetCfmCreate(msg__, dst__, src__, status__, coexConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoexConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_COEX_CONFIG_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->coexConfig = (coexConfig__);
+
+#define CsrWifiSmeCoexConfigGetCfmSendTo(dst__, src__, status__, coexConfig__) \
+    { \
+        CsrWifiSmeCoexConfigGetCfm *msg__; \
+        CsrWifiSmeCoexConfigGetCfmCreate(msg__, dst__, src__, status__, coexConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoexConfigGetCfmSend(dst__, status__, coexConfig__) \
+    CsrWifiSmeCoexConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, coexConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the CoexConfig parameter.
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    coexConfig - Configures the coexistence behaviour
+
+*******************************************************************************/
+#define CsrWifiSmeCoexConfigSetReqCreate(msg__, dst__, src__, coexConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoexConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_COEX_CONFIG_SET_REQ, dst__, src__); \
+    msg__->coexConfig = (coexConfig__);
+
+#define CsrWifiSmeCoexConfigSetReqSendTo(dst__, src__, coexConfig__) \
+    { \
+        CsrWifiSmeCoexConfigSetReq *msg__; \
+        CsrWifiSmeCoexConfigSetReqCreate(msg__, dst__, src__, coexConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoexConfigSetReqSend(src__, coexConfig__) \
+    CsrWifiSmeCoexConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, coexConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeCoexConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoexConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_COEX_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeCoexConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeCoexConfigSetCfm *msg__; \
+        CsrWifiSmeCoexConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoexConfigSetCfmSend(dst__, status__) \
+    CsrWifiSmeCoexConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexInfoGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the CoexInfo parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeCoexInfoGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoexInfoGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_COEX_INFO_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeCoexInfoGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeCoexInfoGetReq *msg__; \
+        CsrWifiSmeCoexInfoGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoexInfoGetReqSend(src__) \
+    CsrWifiSmeCoexInfoGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexInfoGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue    - Destination Task Queue
+    status   - Reports the result of the request
+    coexInfo - Reports information and state related to coexistence.
+
+*******************************************************************************/
+#define CsrWifiSmeCoexInfoGetCfmCreate(msg__, dst__, src__, status__, coexInfo__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoexInfoGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_COEX_INFO_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->coexInfo = (coexInfo__);
+
+#define CsrWifiSmeCoexInfoGetCfmSendTo(dst__, src__, status__, coexInfo__) \
+    { \
+        CsrWifiSmeCoexInfoGetCfm *msg__; \
+        CsrWifiSmeCoexInfoGetCfmCreate(msg__, dst__, src__, status__, coexInfo__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoexInfoGetCfmSend(dst__, status__, coexInfo__) \
+    CsrWifiSmeCoexInfoGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, coexInfo__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to start the
+    process of joining an 802.11 wireless network or to start an ad hoc
+    network.
+    The structure pointed by connectionConfig contains parameters describing
+    the network to join or, in case of an ad hoc network, to host or join.
+    The SME will select a network, perform the IEEE 802.11 Join, Authenticate
+    and Associate exchanges.
+    The SME selects the networks from the current scan list that match both
+    the SSID and BSSID, however either or both of these may be the wildcard
+    value. Using this rule, the following operations are possible:
+      * To connect to a network by name, specify the SSID and set the BSSID to
+        0xFF 0xFF 0xFF 0xFF 0xFF 0xFF. If there are two or more networks visible,
+        the SME will select the one with the strongest signal.
+      * To connect to a specific network, specify the BSSID. The SSID is
+        optional, but if given it must match the SSID of the network. An empty
+        SSID may be specified by setting the SSID length to zero. Please note
+        that if the BSSID is specified (i.e. not equal to 0xFF 0xFF 0xFF 0xFF
+        0xFF 0xFF), the SME will not attempt to roam if signal conditions become
+        poor, even if there is an alternative AP with an SSID that matches the
+        current network SSID.
+      * To connect to any network matching the other parameters (i.e. security,
+        etc), set the SSID length to zero and set the BSSID to 0xFF 0xFF 0xFF
+        0xFF 0xFF 0xFF. In this case, the SME will order all available networks
+        by their signal strengths and will iterate through this list until it
+        successfully connects.
+    NOTE: Specifying the BSSID will restrict the selection to one specific
+    network. If SSID and BSSID are given, they must both match the network
+    for it to be selected. To select a network based on the SSID only, the
+    wireless manager application must set the BSSID to 0xFF 0xFF 0xFF 0xFF
+    0xFF 0xFF.
+    The SME will try to connect to each network that matches the provided
+    parameters, one by one, until it succeeds or has tried unsuccessfully
+    with all the matching networks.
+    If there is no network that matches the parameters and the request allows
+    to host an ad hoc network, the SME will advertise a new ad hoc network
+    instead.
+    If the SME cannot connect, it will notify the failure in the confirm.
+
+  PARAMETERS
+    queue            - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    connectionConfig - Describes the candidate network to join or to host.
+
+*******************************************************************************/
+#define CsrWifiSmeConnectReqCreate(msg__, dst__, src__, interfaceTag__, connectionConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->connectionConfig = (connectionConfig__);
+
+#define CsrWifiSmeConnectReqSendTo(dst__, src__, interfaceTag__, connectionConfig__) \
+    { \
+        CsrWifiSmeConnectReq *msg__; \
+        CsrWifiSmeConnectReqCreate(msg__, dst__, src__, interfaceTag__, connectionConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectReqSend(src__, interfaceTag__, connectionConfig__) \
+    CsrWifiSmeConnectReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, connectionConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectCfmSend
+
+  DESCRIPTION
+    The SME calls this primitive when the connection exchange is complete or
+    all connection attempts fail.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request.
+                   CSR_WIFI_SME_STATUS_NOT_FOUND: all attempts by the SME to
+                   locate the requested AP failed
+
+*******************************************************************************/
+#define CsrWifiSmeConnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECT_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeConnectCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeConnectCfm *msg__; \
+        CsrWifiSmeConnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeConnectCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the ConnectionConfig parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionConfigGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_CONFIG_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeConnectionConfigGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeConnectionConfigGetReq *msg__; \
+        CsrWifiSmeConnectionConfigGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionConfigGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeConnectionConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    status           - Reports the result of the request
+    connectionConfig - Parameters used by the SME for selecting a network
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_CONFIG_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->connectionConfig = (connectionConfig__);
+
+#define CsrWifiSmeConnectionConfigGetCfmSendTo(dst__, src__, interfaceTag__, status__, connectionConfig__) \
+    { \
+        CsrWifiSmeConnectionConfigGetCfm *msg__; \
+        CsrWifiSmeConnectionConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionConfigGetCfmSend(dst__, interfaceTag__, status__, connectionConfig__) \
+    CsrWifiSmeConnectionConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, connectionConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionInfoGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the ConnectionInfo parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionInfoGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionInfoGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_INFO_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeConnectionInfoGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeConnectionInfoGetReq *msg__; \
+        CsrWifiSmeConnectionInfoGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionInfoGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeConnectionInfoGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionInfoGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the request
+    connectionInfo - Information about the current connection
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionInfoGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionInfo__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionInfoGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_INFO_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->connectionInfo = (connectionInfo__);
+
+#define CsrWifiSmeConnectionInfoGetCfmSendTo(dst__, src__, interfaceTag__, status__, connectionInfo__) \
+    { \
+        CsrWifiSmeConnectionInfoGetCfm *msg__; \
+        CsrWifiSmeConnectionInfoGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionInfo__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionInfoGetCfmSend(dst__, interfaceTag__, status__, connectionInfo__) \
+    CsrWifiSmeConnectionInfoGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, connectionInfo__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionQualityIndSend
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it whenever the value of the current connection quality
+    parameters change by more than a certain configurable amount.
+    The wireless manager application may configure the trigger thresholds for
+    this indication using the field in smeConfig parameter of
+    CSR_WIFI_SME_SME_CONFIG_SET_REQ.
+    Connection quality messages can be suppressed by setting both thresholds
+    to zero.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    linkQuality  - Indicates the quality of the link
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionQualityIndCreate(msg__, dst__, src__, interfaceTag__, linkQuality__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionQualityInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_QUALITY_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->linkQuality = (linkQuality__);
+
+#define CsrWifiSmeConnectionQualityIndSendTo(dst__, src__, interfaceTag__, linkQuality__) \
+    { \
+        CsrWifiSmeConnectionQualityInd *msg__; \
+        CsrWifiSmeConnectionQualityIndCreate(msg__, dst__, src__, interfaceTag__, linkQuality__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionQualityIndSend(dst__, interfaceTag__, linkQuality__) \
+    CsrWifiSmeConnectionQualityIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, linkQuality__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionStatsGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the ConnectionStats parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionStatsGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionStatsGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_STATS_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeConnectionStatsGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeConnectionStatsGetReq *msg__; \
+        CsrWifiSmeConnectionStatsGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionStatsGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeConnectionStatsGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionStatsGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue           - Destination Task Queue
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    status          - Reports the result of the request
+    connectionStats - Statistics for current connection.
+
+*******************************************************************************/
+#define CsrWifiSmeConnectionStatsGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionStats__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeConnectionStatsGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CONNECTION_STATS_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->connectionStats = (connectionStats__);
+
+#define CsrWifiSmeConnectionStatsGetCfmSendTo(dst__, src__, interfaceTag__, status__, connectionStats__) \
+    { \
+        CsrWifiSmeConnectionStatsGetCfm *msg__; \
+        CsrWifiSmeConnectionStatsGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, connectionStats__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeConnectionStatsGetCfmSend(dst__, interfaceTag__, status__, connectionStats__) \
+    CsrWifiSmeConnectionStatsGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, connectionStats__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoreDumpIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive Wi-Fi Chip core dump data.
+    The core dump data may be fragmented and sent using more than one
+    indication.
+    To indicate that all the data has been sent, the last indication contains
+    a 'length' of 0 and 'data' of NULL.
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    dataLength - Number of bytes in the buffer pointed to by 'data'
+    data       - Pointer to the buffer containing 'dataLength' bytes of core
+                 dump data
+
+*******************************************************************************/
+#define CsrWifiSmeCoreDumpIndCreate(msg__, dst__, src__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeCoreDumpInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_CORE_DUMP_IND, dst__, src__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiSmeCoreDumpIndSendTo(dst__, src__, dataLength__, data__) \
+    { \
+        CsrWifiSmeCoreDumpInd *msg__; \
+        CsrWifiSmeCoreDumpIndCreate(msg__, dst__, src__, dataLength__, data__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeCoreDumpIndSend(dst__, dataLength__, data__) \
+    CsrWifiSmeCoreDumpIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeactivateReqSend
+
+  DESCRIPTION
+    The WMA sends this primitive to deactivate the SME.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeDeactivateReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeDeactivateReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_DEACTIVATE_REQ, dst__, src__);
+
+#define CsrWifiSmeDeactivateReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeDeactivateReq *msg__; \
+        CsrWifiSmeDeactivateReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeDeactivateReqSend(src__) \
+    CsrWifiSmeDeactivateReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeactivateCfmSend
+
+  DESCRIPTION
+    The SME sends this primitive when the deactivation is complete.
+    The WMA cannot send any more primitives until it actives the SME again
+    sending another CSR_WIFI_SME_ACTIVATE_REQ.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeDeactivateCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeDeactivateCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_DEACTIVATE_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeDeactivateCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeDeactivateCfm *msg__; \
+        CsrWifiSmeDeactivateCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeDeactivateCfmSend(dst__, status__) \
+    CsrWifiSmeDeactivateCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDisconnectReqSend
+
+  DESCRIPTION
+    The wireless manager application may disconnect from the current network
+    by calling this primitive
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeDisconnectReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeDisconnectReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_DISCONNECT_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeDisconnectReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeDisconnectReq *msg__; \
+        CsrWifiSmeDisconnectReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeDisconnectReqSend(src__, interfaceTag__) \
+    CsrWifiSmeDisconnectReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDisconnectCfmSend
+
+  DESCRIPTION
+    On reception of CSR_WIFI_SME_DISCONNECT_REQ the SME will perform a
+    disconnect operation, sending a CsrWifiSmeMediaStatusInd with
+    CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED and then call this primitive when
+    disconnection is complete.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeDisconnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeDisconnectCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_DISCONNECT_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeDisconnectCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeDisconnectCfm *msg__; \
+        CsrWifiSmeDisconnectCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeDisconnectCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeDisconnectCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeErrorIndSend
+
+  DESCRIPTION
+    Important error message indicating a error of some importance
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    errorMessage - Contains the error message.
+
+*******************************************************************************/
+#define CsrWifiSmeErrorIndCreate(msg__, dst__, src__, errorMessage__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeErrorInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ERROR_IND, dst__, src__); \
+    msg__->errorMessage = (errorMessage__);
+
+#define CsrWifiSmeErrorIndSendTo(dst__, src__, errorMessage__) \
+    { \
+        CsrWifiSmeErrorInd *msg__; \
+        CsrWifiSmeErrorIndCreate(msg__, dst__, src__, errorMessage__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeErrorIndSend(dst__, errorMessage__) \
+    CsrWifiSmeErrorIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, errorMessage__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEventMaskSetReqSend
+
+  DESCRIPTION
+    The wireless manager application may register with the SME to receive
+    notification of interesting events. Indications will be sent only if the
+    wireless manager explicitly registers to be notified of that event.
+    indMask is a bit mask of values defined in CsrWifiSmeIndicationsMask.
+
+  PARAMETERS
+    queue   - Message Source Task Queue (Cfm's will be sent to this Queue)
+    indMask - Set mask with values from CsrWifiSmeIndications
+
+*******************************************************************************/
+#define CsrWifiSmeEventMaskSetReqCreate(msg__, dst__, src__, indMask__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeEventMaskSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_EVENT_MASK_SET_REQ, dst__, src__); \
+    msg__->indMask = (indMask__);
+
+#define CsrWifiSmeEventMaskSetReqSendTo(dst__, src__, indMask__) \
+    { \
+        CsrWifiSmeEventMaskSetReq *msg__; \
+        CsrWifiSmeEventMaskSetReqCreate(msg__, dst__, src__, indMask__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeEventMaskSetReqSend(src__, indMask__) \
+    CsrWifiSmeEventMaskSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, indMask__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEventMaskSetCfmSend
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the request
+    primitive.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeEventMaskSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeEventMaskSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_EVENT_MASK_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeEventMaskSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeEventMaskSetCfm *msg__; \
+        CsrWifiSmeEventMaskSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeEventMaskSetCfmSend(dst__, status__) \
+    CsrWifiSmeEventMaskSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the hostConfig parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeHostConfigGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeHostConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_HOST_CONFIG_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeHostConfigGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeHostConfigGetReq *msg__; \
+        CsrWifiSmeHostConfigGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeHostConfigGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeHostConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    hostConfig   - Current host power state.
+
+*******************************************************************************/
+#define CsrWifiSmeHostConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, hostConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeHostConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_HOST_CONFIG_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->hostConfig = (hostConfig__);
+
+#define CsrWifiSmeHostConfigGetCfmSendTo(dst__, src__, interfaceTag__, status__, hostConfig__) \
+    { \
+        CsrWifiSmeHostConfigGetCfm *msg__; \
+        CsrWifiSmeHostConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, hostConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeHostConfigGetCfmSend(dst__, interfaceTag__, status__, hostConfig__) \
+    CsrWifiSmeHostConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, hostConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the hostConfig parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    hostConfig   - Communicates a change of host power state (for example, on
+                   mains power, on battery power etc) and of the periodicity of
+                   traffic data
+
+*******************************************************************************/
+#define CsrWifiSmeHostConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, hostConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeHostConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_HOST_CONFIG_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->hostConfig = (hostConfig__);
+
+#define CsrWifiSmeHostConfigSetReqSendTo(dst__, src__, interfaceTag__, hostConfig__) \
+    { \
+        CsrWifiSmeHostConfigSetReq *msg__; \
+        CsrWifiSmeHostConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, hostConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeHostConfigSetReqSend(src__, interfaceTag__, hostConfig__) \
+    CsrWifiSmeHostConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, hostConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeHostConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeHostConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_HOST_CONFIG_SET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeHostConfigSetCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeHostConfigSetCfm *msg__; \
+        CsrWifiSmeHostConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeHostConfigSetCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeHostConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeIbssStationIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to indicate that a station has joined or
+    left the ad-hoc network.
+
+  PARAMETERS
+    queue       - Destination Task Queue
+    address     - MAC address of the station that has joined or left
+    isconnected - TRUE if the station joined, FALSE if the station left
+
+*******************************************************************************/
+#define CsrWifiSmeIbssStationIndCreate(msg__, dst__, src__, address__, isconnected__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeIbssStationInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_IBSS_STATION_IND, dst__, src__); \
+    msg__->address = (address__); \
+    msg__->isconnected = (isconnected__);
+
+#define CsrWifiSmeIbssStationIndSendTo(dst__, src__, address__, isconnected__) \
+    { \
+        CsrWifiSmeIbssStationInd *msg__; \
+        CsrWifiSmeIbssStationIndCreate(msg__, dst__, src__, address__, isconnected__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeIbssStationIndSend(dst__, address__, isconnected__) \
+    CsrWifiSmeIbssStationIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, address__, isconnected__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeInfoIndSend
+
+  DESCRIPTION
+    Message indicating a some info about current activity. Mostly of interest
+    in testing but may be useful in the field.
+
+  PARAMETERS
+    queue       - Destination Task Queue
+    infoMessage - Contains the message.
+
+*******************************************************************************/
+#define CsrWifiSmeInfoIndCreate(msg__, dst__, src__, infoMessage__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeInfoInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_INFO_IND, dst__, src__); \
+    msg__->infoMessage = (infoMessage__);
+
+#define CsrWifiSmeInfoIndSendTo(dst__, src__, infoMessage__) \
+    { \
+        CsrWifiSmeInfoInd *msg__; \
+        CsrWifiSmeInfoIndCreate(msg__, dst__, src__, infoMessage__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeInfoIndSend(dst__, infoMessage__) \
+    CsrWifiSmeInfoIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, infoMessage__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeInterfaceCapabilityGetReqSend
+
+  DESCRIPTION
+    The Wireless Manager calls this primitive to ask the SME for the
+    capabilities of the supported interfaces
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeInterfaceCapabilityGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeInterfaceCapabilityGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_INTERFACE_CAPABILITY_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeInterfaceCapabilityGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeInterfaceCapabilityGetReq *msg__; \
+        CsrWifiSmeInterfaceCapabilityGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeInterfaceCapabilityGetReqSend(src__) \
+    CsrWifiSmeInterfaceCapabilityGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeInterfaceCapabilityGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue         - Destination Task Queue
+    status        - Result of the request
+    numInterfaces - Number of the interfaces supported
+    capBitmap     - Points to the list of capabilities bitmaps provided for each
+                    interface.
+                    The bits represent the following capabilities:
+                    -bits 7 to 4-Reserved
+                    -bit 3-AMP
+                    -bit 2-P2P
+                    -bit 1-AP
+                    -bit 0-STA
+
+*******************************************************************************/
+#define CsrWifiSmeInterfaceCapabilityGetCfmCreate(msg__, dst__, src__, status__, numInterfaces__, capBitmap__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeInterfaceCapabilityGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_INTERFACE_CAPABILITY_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->numInterfaces = (numInterfaces__); \
+    memcpy(msg__->capBitmap, (capBitmap__), sizeof(u8) * 2);
+
+#define CsrWifiSmeInterfaceCapabilityGetCfmSendTo(dst__, src__, status__, numInterfaces__, capBitmap__) \
+    { \
+        CsrWifiSmeInterfaceCapabilityGetCfm *msg__; \
+        CsrWifiSmeInterfaceCapabilityGetCfmCreate(msg__, dst__, src__, status__, numInterfaces__, capBitmap__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeInterfaceCapabilityGetCfmSend(dst__, status__, numInterfaces__, capBitmap__) \
+    CsrWifiSmeInterfaceCapabilityGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, numInterfaces__, capBitmap__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeKeyReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to add or remove
+    keys that the chip should use for encryption of data.
+    The interface allows the wireless manager application to add and remove
+    keys according to the specified action.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    action       - The value of the CsrWifiSmeListAction parameter instructs the
+                   driver to modify or provide the list of keys.
+                   CSR_WIFI_SME_LIST_ACTION_GET is not supported here.
+    key          - Key to be added or removed
+
+*******************************************************************************/
+#define CsrWifiSmeKeyReqCreate(msg__, dst__, src__, interfaceTag__, action__, key__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeKeyReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_KEY_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->action = (action__); \
+    msg__->key = (key__);
+
+#define CsrWifiSmeKeyReqSendTo(dst__, src__, interfaceTag__, action__, key__) \
+    { \
+        CsrWifiSmeKeyReq *msg__; \
+        CsrWifiSmeKeyReqCreate(msg__, dst__, src__, interfaceTag__, action__, key__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeKeyReqSend(src__, interfaceTag__, action__, key__) \
+    CsrWifiSmeKeyReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, action__, key__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeKeyCfmSend
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the request
+    primitive.
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the request
+    action         - Action in the request
+    keyType        - Type of the key added/deleted
+    peerMacAddress - Peer MAC Address of the key added/deleted
+
+*******************************************************************************/
+#define CsrWifiSmeKeyCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, keyType__, peerMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeKeyCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_KEY_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->action = (action__); \
+    msg__->keyType = (keyType__); \
+    msg__->peerMacAddress = (peerMacAddress__);
+
+#define CsrWifiSmeKeyCfmSendTo(dst__, src__, interfaceTag__, status__, action__, keyType__, peerMacAddress__) \
+    { \
+        CsrWifiSmeKeyCfm *msg__; \
+        CsrWifiSmeKeyCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, keyType__, peerMacAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeKeyCfmSend(dst__, interfaceTag__, status__, action__, keyType__, peerMacAddress__) \
+    CsrWifiSmeKeyCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, action__, keyType__, peerMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeLinkQualityGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the LinkQuality parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeLinkQualityGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeLinkQualityGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_LINK_QUALITY_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeLinkQualityGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeLinkQualityGetReq *msg__; \
+        CsrWifiSmeLinkQualityGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeLinkQualityGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeLinkQualityGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeLinkQualityGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    linkQuality  - Indicates the quality of the link
+
+*******************************************************************************/
+#define CsrWifiSmeLinkQualityGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, linkQuality__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeLinkQualityGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_LINK_QUALITY_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->linkQuality = (linkQuality__);
+
+#define CsrWifiSmeLinkQualityGetCfmSendTo(dst__, src__, interfaceTag__, status__, linkQuality__) \
+    { \
+        CsrWifiSmeLinkQualityGetCfm *msg__; \
+        CsrWifiSmeLinkQualityGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, linkQuality__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeLinkQualityGetCfmSend(dst__, interfaceTag__, status__, linkQuality__) \
+    CsrWifiSmeLinkQualityGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, linkQuality__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMediaStatusIndSend
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it when a network connection is established, lost or has moved to
+    another AP.
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    mediaStatus    - Indicates the media status
+    connectionInfo - This parameter is relevant only if the mediaStatus is
+                     CSR_WIFI_SME_MEDIA_STATUS_CONNECTED:
+                     it points to the connection information for the new network
+    disassocReason - This parameter is relevant only if the mediaStatus is
+                     CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED:
+                     if a disassociation has occurred it gives the reason of the
+                     disassociation
+    deauthReason   - This parameter is relevant only if the mediaStatus is
+                     CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED:
+                     if a deauthentication has occurred it gives the reason of
+                     the deauthentication
+
+*******************************************************************************/
+#define CsrWifiSmeMediaStatusIndCreate(msg__, dst__, src__, interfaceTag__, mediaStatus__, connectionInfo__, disassocReason__, deauthReason__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMediaStatusInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MEDIA_STATUS_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->mediaStatus = (mediaStatus__); \
+    msg__->connectionInfo = (connectionInfo__); \
+    msg__->disassocReason = (disassocReason__); \
+    msg__->deauthReason = (deauthReason__);
+
+#define CsrWifiSmeMediaStatusIndSendTo(dst__, src__, interfaceTag__, mediaStatus__, connectionInfo__, disassocReason__, deauthReason__) \
+    { \
+        CsrWifiSmeMediaStatusInd *msg__; \
+        CsrWifiSmeMediaStatusIndCreate(msg__, dst__, src__, interfaceTag__, mediaStatus__, connectionInfo__, disassocReason__, deauthReason__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMediaStatusIndSend(dst__, interfaceTag__, mediaStatus__, connectionInfo__, disassocReason__, deauthReason__) \
+    CsrWifiSmeMediaStatusIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, mediaStatus__, connectionInfo__, disassocReason__, deauthReason__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the MibConfig parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeMibConfigGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_CONFIG_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeMibConfigGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeMibConfigGetReq *msg__; \
+        CsrWifiSmeMibConfigGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibConfigGetReqSend(src__) \
+    CsrWifiSmeMibConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue     - Destination Task Queue
+    status    - Reports the result of the request
+    mibConfig - Reports various IEEE 802.11 attributes as currently configured
+
+*******************************************************************************/
+#define CsrWifiSmeMibConfigGetCfmCreate(msg__, dst__, src__, status__, mibConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_CONFIG_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->mibConfig = (mibConfig__);
+
+#define CsrWifiSmeMibConfigGetCfmSendTo(dst__, src__, status__, mibConfig__) \
+    { \
+        CsrWifiSmeMibConfigGetCfm *msg__; \
+        CsrWifiSmeMibConfigGetCfmCreate(msg__, dst__, src__, status__, mibConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibConfigGetCfmSend(dst__, status__, mibConfig__) \
+    CsrWifiSmeMibConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, mibConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the MibConfig parameter.
+
+  PARAMETERS
+    queue     - Message Source Task Queue (Cfm's will be sent to this Queue)
+    mibConfig - Conveys the desired value of various IEEE 802.11 attributes as
+                currently configured
+
+*******************************************************************************/
+#define CsrWifiSmeMibConfigSetReqCreate(msg__, dst__, src__, mibConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_CONFIG_SET_REQ, dst__, src__); \
+    msg__->mibConfig = (mibConfig__);
+
+#define CsrWifiSmeMibConfigSetReqSendTo(dst__, src__, mibConfig__) \
+    { \
+        CsrWifiSmeMibConfigSetReq *msg__; \
+        CsrWifiSmeMibConfigSetReqCreate(msg__, dst__, src__, mibConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibConfigSetReqSend(src__, mibConfig__) \
+    CsrWifiSmeMibConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, mibConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeMibConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeMibConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeMibConfigSetCfm *msg__; \
+        CsrWifiSmeMibConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibConfigSetCfmSend(dst__, status__) \
+    CsrWifiSmeMibConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetCfmSend
+
+  DESCRIPTION
+    The SME calls this primitive to return the requested MIB variable values.
+
+  PARAMETERS
+    queue              - Destination Task Queue
+    status             - Reports the result of the request
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to the VarBind or VarBindList containing the
+                         names and values of the MIB variables requested
+
+*******************************************************************************/
+#define CsrWifiSmeMibGetCfmCreate(msg__, dst__, src__, status__, mibAttributeLength__, mibAttribute__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->mibAttributeLength = (mibAttributeLength__); \
+    msg__->mibAttribute = (mibAttribute__);
+
+#define CsrWifiSmeMibGetCfmSendTo(dst__, src__, status__, mibAttributeLength__, mibAttribute__) \
+    { \
+        CsrWifiSmeMibGetCfm *msg__; \
+        CsrWifiSmeMibGetCfmCreate(msg__, dst__, src__, status__, mibAttributeLength__, mibAttribute__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibGetCfmSend(dst__, status__, mibAttributeLength__, mibAttribute__) \
+    CsrWifiSmeMibGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, mibAttributeLength__, mibAttribute__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetNextReqSend
+
+  DESCRIPTION
+    To read a sequence of MIB parameters, for example a table, call this
+    primitive to find the name of the next MIB variable
+
+  PARAMETERS
+    queue              - Message Source Task Queue (Cfm's will be sent to this Queue)
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to a VarBind or VarBindList containing the
+                         name(s) of the MIB variable(s) to search from.
+
+*******************************************************************************/
+#define CsrWifiSmeMibGetNextReqCreate(msg__, dst__, src__, mibAttributeLength__, mibAttribute__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibGetNextReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_GET_NEXT_REQ, dst__, src__); \
+    msg__->mibAttributeLength = (mibAttributeLength__); \
+    msg__->mibAttribute = (mibAttribute__);
+
+#define CsrWifiSmeMibGetNextReqSendTo(dst__, src__, mibAttributeLength__, mibAttribute__) \
+    { \
+        CsrWifiSmeMibGetNextReq *msg__; \
+        CsrWifiSmeMibGetNextReqCreate(msg__, dst__, src__, mibAttributeLength__, mibAttribute__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibGetNextReqSend(src__, mibAttributeLength__, mibAttribute__) \
+    CsrWifiSmeMibGetNextReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, mibAttributeLength__, mibAttribute__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetNextCfmSend
+
+  DESCRIPTION
+    The SME calls this primitive to return the requested MIB name(s).
+    The wireless manager application can then read the value of the MIB
+    variable using CSR_WIFI_SME_MIB_GET_REQ, using the names provided.
+
+  PARAMETERS
+    queue              - Destination Task Queue
+    status             - Reports the result of the request
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to a VarBind or VarBindList containing the
+                         name(s) of the MIB variable(s) lexicographically
+                         following the name(s) given in the request
+
+*******************************************************************************/
+#define CsrWifiSmeMibGetNextCfmCreate(msg__, dst__, src__, status__, mibAttributeLength__, mibAttribute__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibGetNextCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_GET_NEXT_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->mibAttributeLength = (mibAttributeLength__); \
+    msg__->mibAttribute = (mibAttribute__);
+
+#define CsrWifiSmeMibGetNextCfmSendTo(dst__, src__, status__, mibAttributeLength__, mibAttribute__) \
+    { \
+        CsrWifiSmeMibGetNextCfm *msg__; \
+        CsrWifiSmeMibGetNextCfmCreate(msg__, dst__, src__, status__, mibAttributeLength__, mibAttribute__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibGetNextCfmSend(dst__, status__, mibAttributeLength__, mibAttribute__) \
+    CsrWifiSmeMibGetNextCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, mibAttributeLength__, mibAttribute__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to retrieve one or
+    more MIB variables.
+
+  PARAMETERS
+    queue              - Message Source Task Queue (Cfm's will be sent to this Queue)
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to the VarBind or VarBindList containing the
+                         names of the MIB variables to be retrieved
+
+*******************************************************************************/
+#define CsrWifiSmeMibGetReqCreate(msg__, dst__, src__, mibAttributeLength__, mibAttribute__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_GET_REQ, dst__, src__); \
+    msg__->mibAttributeLength = (mibAttributeLength__); \
+    msg__->mibAttribute = (mibAttribute__);
+
+#define CsrWifiSmeMibGetReqSendTo(dst__, src__, mibAttributeLength__, mibAttribute__) \
+    { \
+        CsrWifiSmeMibGetReq *msg__; \
+        CsrWifiSmeMibGetReqCreate(msg__, dst__, src__, mibAttributeLength__, mibAttribute__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibGetReqSend(src__, mibAttributeLength__, mibAttribute__) \
+    CsrWifiSmeMibGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, mibAttributeLength__, mibAttribute__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibSetReqSend
+
+  DESCRIPTION
+    The SME provides raw access to the MIB on the chip, which may be used by
+    some configuration or diagnostic utilities, but is not normally needed by
+    the wireless manager application.
+    The MIB access functions use BER encoded names (OID) of the MIB
+    parameters and BER encoded values, as described in the chip Host
+    Interface Protocol Specification.
+    The MIB parameters are described in 'Wi-Fi 5.0.0 Management Information
+    Base Reference Guide'.
+    The wireless manager application calls this primitive to set one or more
+    MIB variables
+
+  PARAMETERS
+    queue              - Message Source Task Queue (Cfm's will be sent to this Queue)
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to the VarBind or VarBindList containing the
+                         names and values of the MIB variables to set
+
+*******************************************************************************/
+#define CsrWifiSmeMibSetReqCreate(msg__, dst__, src__, mibAttributeLength__, mibAttribute__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_SET_REQ, dst__, src__); \
+    msg__->mibAttributeLength = (mibAttributeLength__); \
+    msg__->mibAttribute = (mibAttribute__);
+
+#define CsrWifiSmeMibSetReqSendTo(dst__, src__, mibAttributeLength__, mibAttribute__) \
+    { \
+        CsrWifiSmeMibSetReq *msg__; \
+        CsrWifiSmeMibSetReqCreate(msg__, dst__, src__, mibAttributeLength__, mibAttribute__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibSetReqSend(src__, mibAttributeLength__, mibAttribute__) \
+    CsrWifiSmeMibSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, mibAttributeLength__, mibAttribute__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibSetCfmSend
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the set primitive.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeMibSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMibSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIB_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeMibSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeMibSetCfm *msg__; \
+        CsrWifiSmeMibSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMibSetCfmSend(dst__, status__) \
+    CsrWifiSmeMibSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMicFailureIndSend
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it whenever the chip firmware reports a MIC failure.
+
+  PARAMETERS
+    queue         - Destination Task Queue
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    secondFailure - TRUE if this indication is for a second failure in 60
+                    seconds
+    count         - The number of MIC failure events since the connection was
+                    established
+    address       - MAC address of the transmitter that caused the MIC failure
+    keyType       - Type of key for which the failure occurred
+
+*******************************************************************************/
+#define CsrWifiSmeMicFailureIndCreate(msg__, dst__, src__, interfaceTag__, secondFailure__, count__, address__, keyType__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMicFailureInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MIC_FAILURE_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->secondFailure = (secondFailure__); \
+    msg__->count = (count__); \
+    msg__->address = (address__); \
+    msg__->keyType = (keyType__);
+
+#define CsrWifiSmeMicFailureIndSendTo(dst__, src__, interfaceTag__, secondFailure__, count__, address__, keyType__) \
+    { \
+        CsrWifiSmeMicFailureInd *msg__; \
+        CsrWifiSmeMicFailureIndCreate(msg__, dst__, src__, interfaceTag__, secondFailure__, count__, address__, keyType__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMicFailureIndSend(dst__, interfaceTag__, secondFailure__, count__, address__, keyType__) \
+    CsrWifiSmeMicFailureIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, secondFailure__, count__, address__, keyType__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMulticastAddressReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to specify the
+    multicast addresses which the chip should recognise. The interface allows
+    the wireless manager application to query, add, remove and flush the
+    multicast addresses for the network interface according to the specified
+    action.
+
+  PARAMETERS
+    queue             - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag      - Interface Identifier; unique identifier of an interface
+    action            - The value of the CsrWifiSmeListAction parameter
+                        instructs the driver to modify or provide the list of
+                        MAC addresses.
+    setAddressesCount - Number of MAC addresses sent with the primitive
+    setAddresses      - Pointer to the list of MAC Addresses sent with the
+                        primitive, set to NULL if none is sent.
+
+*******************************************************************************/
+#define CsrWifiSmeMulticastAddressReqCreate(msg__, dst__, src__, interfaceTag__, action__, setAddressesCount__, setAddresses__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMulticastAddressReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MULTICAST_ADDRESS_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->action = (action__); \
+    msg__->setAddressesCount = (setAddressesCount__); \
+    msg__->setAddresses = (setAddresses__);
+
+#define CsrWifiSmeMulticastAddressReqSendTo(dst__, src__, interfaceTag__, action__, setAddressesCount__, setAddresses__) \
+    { \
+        CsrWifiSmeMulticastAddressReq *msg__; \
+        CsrWifiSmeMulticastAddressReqCreate(msg__, dst__, src__, interfaceTag__, action__, setAddressesCount__, setAddresses__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMulticastAddressReqSend(src__, interfaceTag__, action__, setAddressesCount__, setAddresses__) \
+    CsrWifiSmeMulticastAddressReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, action__, setAddressesCount__, setAddresses__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMulticastAddressCfmSend
+
+  DESCRIPTION
+    The SME will call this primitive when the operation is complete. For a
+    GET action, this primitive reports the current list of MAC addresses.
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    interfaceTag      - Interface Identifier; unique identifier of an interface
+    status            - Reports the result of the request
+    action            - Action in the request
+    getAddressesCount - This parameter is only relevant if action is
+                        CSR_WIFI_SME_LIST_ACTION_GET:
+                        number of MAC addresses sent with the primitive
+    getAddresses      - Pointer to the list of MAC Addresses sent with the
+                        primitive, set to NULL if none is sent.
+
+*******************************************************************************/
+#define CsrWifiSmeMulticastAddressCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, getAddressesCount__, getAddresses__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeMulticastAddressCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_MULTICAST_ADDRESS_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->action = (action__); \
+    msg__->getAddressesCount = (getAddressesCount__); \
+    msg__->getAddresses = (getAddresses__);
+
+#define CsrWifiSmeMulticastAddressCfmSendTo(dst__, src__, interfaceTag__, status__, action__, getAddressesCount__, getAddresses__) \
+    { \
+        CsrWifiSmeMulticastAddressCfm *msg__; \
+        CsrWifiSmeMulticastAddressCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, getAddressesCount__, getAddresses__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeMulticastAddressCfmSend(dst__, interfaceTag__, status__, action__, getAddressesCount__, getAddresses__) \
+    CsrWifiSmeMulticastAddressCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, action__, getAddressesCount__, getAddresses__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePacketFilterSetReqSend
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to enable or
+    disable filtering of broadcast packets: uninteresting broadcast packets
+    will be dropped by the Wi-Fi chip, instead of passing them up to the
+    host.
+    This has the advantage of saving power in the host application processor
+    as it removes the need to process unwanted packets.
+    All broadcast packets are filtered according to the filter and the filter
+    mode provided, except ARP packets, which are filtered using
+    arpFilterAddress.
+    Filters are not cumulative: only the parameters specified in the most
+    recent successful request are significant.
+    For more information, see 'UniFi Firmware API Specification'.
+
+  PARAMETERS
+    queue            - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    filterLength     - Length of the filter in bytes.
+                       filterLength=0 disables the filter previously set
+    filter           - Points to the first byte of the filter provided, if any.
+                       This shall include zero or more instance of the
+                       information elements of one of these types
+                         * Traffic Classification (TCLAS) elements
+                         * WMM-SA TCLAS elements
+    mode             - Specifies whether the filter selects or excludes packets
+                       matching the filter
+    arpFilterAddress - IPv4 address to be used for filtering the ARP packets.
+                         * If the specified address is the IPv4 broadcast address
+                           (255.255.255.255), all ARP packets are reported to the
+                           host,
+                         * If the specified address is NOT the IPv4 broadcast
+                           address, only ARP packets with the specified address in
+                           the Source or Target Protocol Address fields are reported
+                           to the host
+
+*******************************************************************************/
+#define CsrWifiSmePacketFilterSetReqCreate(msg__, dst__, src__, interfaceTag__, filterLength__, filter__, mode__, arpFilterAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePacketFilterSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PACKET_FILTER_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->filterLength = (filterLength__); \
+    msg__->filter = (filter__); \
+    msg__->mode = (mode__); \
+    msg__->arpFilterAddress = (arpFilterAddress__);
+
+#define CsrWifiSmePacketFilterSetReqSendTo(dst__, src__, interfaceTag__, filterLength__, filter__, mode__, arpFilterAddress__) \
+    { \
+        CsrWifiSmePacketFilterSetReq *msg__; \
+        CsrWifiSmePacketFilterSetReqCreate(msg__, dst__, src__, interfaceTag__, filterLength__, filter__, mode__, arpFilterAddress__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePacketFilterSetReqSend(src__, interfaceTag__, filterLength__, filter__, mode__, arpFilterAddress__) \
+    CsrWifiSmePacketFilterSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, filterLength__, filter__, mode__, arpFilterAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePacketFilterSetCfmSend
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the set primitive.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmePacketFilterSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePacketFilterSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PACKET_FILTER_SET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmePacketFilterSetCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmePacketFilterSetCfm *msg__; \
+        CsrWifiSmePacketFilterSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePacketFilterSetCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmePacketFilterSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePermanentMacAddressGetReqSend
+
+  DESCRIPTION
+    This primitive retrieves the MAC address stored in EEPROM
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmePermanentMacAddressGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePermanentMacAddressGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PERMANENT_MAC_ADDRESS_GET_REQ, dst__, src__);
+
+#define CsrWifiSmePermanentMacAddressGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmePermanentMacAddressGetReq *msg__; \
+        CsrWifiSmePermanentMacAddressGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePermanentMacAddressGetReqSend(src__) \
+    CsrWifiSmePermanentMacAddressGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePermanentMacAddressGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue               - Destination Task Queue
+    status              - Reports the result of the request
+    permanentMacAddress - MAC address stored in the EEPROM
+
+*******************************************************************************/
+#define CsrWifiSmePermanentMacAddressGetCfmCreate(msg__, dst__, src__, status__, permanentMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePermanentMacAddressGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PERMANENT_MAC_ADDRESS_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->permanentMacAddress = (permanentMacAddress__);
+
+#define CsrWifiSmePermanentMacAddressGetCfmSendTo(dst__, src__, status__, permanentMacAddress__) \
+    { \
+        CsrWifiSmePermanentMacAddressGetCfm *msg__; \
+        CsrWifiSmePermanentMacAddressGetCfmCreate(msg__, dst__, src__, status__, permanentMacAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePermanentMacAddressGetCfmSend(dst__, status__, permanentMacAddress__) \
+    CsrWifiSmePermanentMacAddressGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, permanentMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidCandidateListIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it when a new network supporting preauthentication and/or PMK
+    caching is seen.
+
+  PARAMETERS
+    queue                - Destination Task Queue
+    interfaceTag         - Interface Identifier; unique identifier of an
+                           interface
+    pmkidCandidatesCount - Number of PMKID candidates provided
+    pmkidCandidates      - Points to the first PMKID candidate
+
+*******************************************************************************/
+#define CsrWifiSmePmkidCandidateListIndCreate(msg__, dst__, src__, interfaceTag__, pmkidCandidatesCount__, pmkidCandidates__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePmkidCandidateListInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PMKID_CANDIDATE_LIST_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->pmkidCandidatesCount = (pmkidCandidatesCount__); \
+    msg__->pmkidCandidates = (pmkidCandidates__);
+
+#define CsrWifiSmePmkidCandidateListIndSendTo(dst__, src__, interfaceTag__, pmkidCandidatesCount__, pmkidCandidates__) \
+    { \
+        CsrWifiSmePmkidCandidateListInd *msg__; \
+        CsrWifiSmePmkidCandidateListIndCreate(msg__, dst__, src__, interfaceTag__, pmkidCandidatesCount__, pmkidCandidates__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePmkidCandidateListIndSend(dst__, interfaceTag__, pmkidCandidatesCount__, pmkidCandidates__) \
+    CsrWifiSmePmkidCandidateListIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, pmkidCandidatesCount__, pmkidCandidates__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to request an
+    operation on the SME PMKID list.
+    The action argument specifies the operation to perform.
+    When the connection is complete, the wireless manager application may
+    then send and receive EAPOL packets to complete WPA or WPA2
+    authentication if appropriate.
+    The wireless manager application can then pass the resulting encryption
+    keys using this primitive.
+
+  PARAMETERS
+    queue          - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    action         - The value of the CsrWifiSmeListAction parameter instructs
+                     the driver to modify or provide the list of PMKIDs.
+    setPmkidsCount - Number of PMKIDs sent with the primitive
+    setPmkids      - Pointer to the list of PMKIDs sent with the primitive, set
+                     to NULL if none is sent.
+
+*******************************************************************************/
+#define CsrWifiSmePmkidReqCreate(msg__, dst__, src__, interfaceTag__, action__, setPmkidsCount__, setPmkids__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePmkidReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PMKID_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->action = (action__); \
+    msg__->setPmkidsCount = (setPmkidsCount__); \
+    msg__->setPmkids = (setPmkids__);
+
+#define CsrWifiSmePmkidReqSendTo(dst__, src__, interfaceTag__, action__, setPmkidsCount__, setPmkids__) \
+    { \
+        CsrWifiSmePmkidReq *msg__; \
+        CsrWifiSmePmkidReqCreate(msg__, dst__, src__, interfaceTag__, action__, setPmkidsCount__, setPmkids__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePmkidReqSend(src__, interfaceTag__, action__, setPmkidsCount__, setPmkids__) \
+    CsrWifiSmePmkidReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, action__, setPmkidsCount__, setPmkids__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidCfmSend
+
+  DESCRIPTION
+    The SME will call this primitive when the operation is complete. For a
+    GET action, this primitive reports the current list of PMKIDs
+
+  PARAMETERS
+    queue          - Destination Task Queue
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the request
+    action         - Action in the request
+    getPmkidsCount - This parameter is only relevant if action is
+                     CSR_WIFI_SME_LIST_ACTION_GET:
+                     number of PMKIDs sent with the primitive
+    getPmkids      - Pointer to the list of PMKIDs sent with the primitive, set
+                     to NULL if none is sent.
+
+*******************************************************************************/
+#define CsrWifiSmePmkidCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, getPmkidsCount__, getPmkids__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePmkidCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_PMKID_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->action = (action__); \
+    msg__->getPmkidsCount = (getPmkidsCount__); \
+    msg__->getPmkids = (getPmkids__);
+
+#define CsrWifiSmePmkidCfmSendTo(dst__, src__, interfaceTag__, status__, action__, getPmkidsCount__, getPmkids__) \
+    { \
+        CsrWifiSmePmkidCfm *msg__; \
+        CsrWifiSmePmkidCfmCreate(msg__, dst__, src__, interfaceTag__, status__, action__, getPmkidsCount__, getPmkids__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePmkidCfmSend(dst__, interfaceTag__, status__, action__, getPmkidsCount__, getPmkids__) \
+    CsrWifiSmePmkidCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, action__, getPmkidsCount__, getPmkids__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the PowerConfig parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmePowerConfigGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePowerConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_POWER_CONFIG_GET_REQ, dst__, src__);
+
+#define CsrWifiSmePowerConfigGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmePowerConfigGetReq *msg__; \
+        CsrWifiSmePowerConfigGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePowerConfigGetReqSend(src__) \
+    CsrWifiSmePowerConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue       - Destination Task Queue
+    status      - Reports the result of the request
+    powerConfig - Returns the current parameters for the power configuration of
+                  the firmware
+
+*******************************************************************************/
+#define CsrWifiSmePowerConfigGetCfmCreate(msg__, dst__, src__, status__, powerConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePowerConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_POWER_CONFIG_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->powerConfig = (powerConfig__);
+
+#define CsrWifiSmePowerConfigGetCfmSendTo(dst__, src__, status__, powerConfig__) \
+    { \
+        CsrWifiSmePowerConfigGetCfm *msg__; \
+        CsrWifiSmePowerConfigGetCfmCreate(msg__, dst__, src__, status__, powerConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePowerConfigGetCfmSend(dst__, status__, powerConfig__) \
+    CsrWifiSmePowerConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, powerConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the PowerConfig parameter.
+
+  PARAMETERS
+    queue       - Message Source Task Queue (Cfm's will be sent to this Queue)
+    powerConfig - Power saving configuration
+
+*******************************************************************************/
+#define CsrWifiSmePowerConfigSetReqCreate(msg__, dst__, src__, powerConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePowerConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_POWER_CONFIG_SET_REQ, dst__, src__); \
+    msg__->powerConfig = (powerConfig__);
+
+#define CsrWifiSmePowerConfigSetReqSendTo(dst__, src__, powerConfig__) \
+    { \
+        CsrWifiSmePowerConfigSetReq *msg__; \
+        CsrWifiSmePowerConfigSetReqCreate(msg__, dst__, src__, powerConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePowerConfigSetReqSend(src__, powerConfig__) \
+    CsrWifiSmePowerConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, powerConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmePowerConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmePowerConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_POWER_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmePowerConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmePowerConfigSetCfm *msg__; \
+        CsrWifiSmePowerConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmePowerConfigSetCfmSend(dst__, status__) \
+    CsrWifiSmePowerConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRegulatoryDomainInfoGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the RegulatoryDomainInfo parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeRegulatoryDomainInfoGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRegulatoryDomainInfoGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_REGULATORY_DOMAIN_INFO_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeRegulatoryDomainInfoGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeRegulatoryDomainInfoGetReq *msg__; \
+        CsrWifiSmeRegulatoryDomainInfoGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRegulatoryDomainInfoGetReqSend(src__) \
+    CsrWifiSmeRegulatoryDomainInfoGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRegulatoryDomainInfoGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    status     - Reports the result of the request
+    regDomInfo - Reports information and state related to regulatory domain
+                 operation.
+
+*******************************************************************************/
+#define CsrWifiSmeRegulatoryDomainInfoGetCfmCreate(msg__, dst__, src__, status__, regDomInfo__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRegulatoryDomainInfoGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_REGULATORY_DOMAIN_INFO_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->regDomInfo = (regDomInfo__);
+
+#define CsrWifiSmeRegulatoryDomainInfoGetCfmSendTo(dst__, src__, status__, regDomInfo__) \
+    { \
+        CsrWifiSmeRegulatoryDomainInfoGetCfm *msg__; \
+        CsrWifiSmeRegulatoryDomainInfoGetCfmCreate(msg__, dst__, src__, status__, regDomInfo__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRegulatoryDomainInfoGetCfmSend(dst__, status__, regDomInfo__) \
+    CsrWifiSmeRegulatoryDomainInfoGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, regDomInfo__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamCompleteIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it completes an attempt to roam to an AP. If the roam
+    attempt was successful, status will be set to CSR_WIFI_SME_SUCCESS,
+    otherwise it shall be set to the appropriate error code.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the roaming procedure
+
+*******************************************************************************/
+#define CsrWifiSmeRoamCompleteIndCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRoamCompleteInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ROAM_COMPLETE_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeRoamCompleteIndSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeRoamCompleteInd *msg__; \
+        CsrWifiSmeRoamCompleteIndCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRoamCompleteIndSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeRoamCompleteIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamStartIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it begins an attempt to roam to an AP.
+    If the wireless manager application connect request specified the SSID
+    and the BSSID was set to the broadcast address (0xFF 0xFF 0xFF 0xFF 0xFF
+    0xFF), the SME monitors the signal quality and maintains a list of
+    candidates to roam to. When the signal quality of the current connection
+    falls below a threshold, and there is a candidate with better quality,
+    the SME will attempt to the candidate AP.
+    If the roaming procedure succeeds, the SME will also issue a Media
+    Connect indication to inform the wireless manager application of the
+    change.
+    NOTE: to prevent the SME from initiating roaming the WMA must specify the
+    BSSID in the connection request; this forces the SME to connect only to
+    that AP.
+    The wireless manager application can obtain statistics for roaming
+    purposes using CSR_WIFI_SME_CONNECTION_QUALITY_IND and
+    CSR_WIFI_SME_CONNECTION_STATS_GET_REQ.
+    When the wireless manager application wishes to roam to another AP, it
+    must issue a connection request specifying the BSSID of the desired AP.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    roamReason   - Indicates the reason for starting the roaming procedure
+    reason80211  - Indicates the reason for deauthentication or disassociation
+
+*******************************************************************************/
+#define CsrWifiSmeRoamStartIndCreate(msg__, dst__, src__, interfaceTag__, roamReason__, reason80211__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRoamStartInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ROAM_START_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->roamReason = (roamReason__); \
+    msg__->reason80211 = (reason80211__);
+
+#define CsrWifiSmeRoamStartIndSendTo(dst__, src__, interfaceTag__, roamReason__, reason80211__) \
+    { \
+        CsrWifiSmeRoamStartInd *msg__; \
+        CsrWifiSmeRoamStartIndCreate(msg__, dst__, src__, interfaceTag__, roamReason__, reason80211__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRoamStartIndSend(dst__, interfaceTag__, roamReason__, reason80211__) \
+    CsrWifiSmeRoamStartIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, roamReason__, reason80211__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the RoamingConfig parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeRoamingConfigGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRoamingConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ROAMING_CONFIG_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeRoamingConfigGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeRoamingConfigGetReq *msg__; \
+        CsrWifiSmeRoamingConfigGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRoamingConfigGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeRoamingConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue         - Destination Task Queue
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    status        - Reports the result of the request
+    roamingConfig - Reports the roaming behaviour of the driver and firmware
+
+*******************************************************************************/
+#define CsrWifiSmeRoamingConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, roamingConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRoamingConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ROAMING_CONFIG_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->roamingConfig = (roamingConfig__);
+
+#define CsrWifiSmeRoamingConfigGetCfmSendTo(dst__, src__, interfaceTag__, status__, roamingConfig__) \
+    { \
+        CsrWifiSmeRoamingConfigGetCfm *msg__; \
+        CsrWifiSmeRoamingConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, roamingConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRoamingConfigGetCfmSend(dst__, interfaceTag__, status__, roamingConfig__) \
+    CsrWifiSmeRoamingConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, roamingConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the RoamingConfig parameter.
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    roamingConfig - Desired roaming behaviour values
+
+*******************************************************************************/
+#define CsrWifiSmeRoamingConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, roamingConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRoamingConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ROAMING_CONFIG_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->roamingConfig = (roamingConfig__);
+
+#define CsrWifiSmeRoamingConfigSetReqSendTo(dst__, src__, interfaceTag__, roamingConfig__) \
+    { \
+        CsrWifiSmeRoamingConfigSetReq *msg__; \
+        CsrWifiSmeRoamingConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, roamingConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRoamingConfigSetReqSend(src__, interfaceTag__, roamingConfig__) \
+    CsrWifiSmeRoamingConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, roamingConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive sets the value of the RoamingConfig parameter.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeRoamingConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeRoamingConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_ROAMING_CONFIG_SET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeRoamingConfigSetCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeRoamingConfigSetCfm *msg__; \
+        CsrWifiSmeRoamingConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeRoamingConfigSetCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeRoamingConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the ScanConfig parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeScanConfigGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_CONFIG_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeScanConfigGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeScanConfigGetReq *msg__; \
+        CsrWifiSmeScanConfigGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanConfigGetReqSend(src__) \
+    CsrWifiSmeScanConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue      - Destination Task Queue
+    status     - Reports the result of the request
+    scanConfig - Returns the current parameters for the autonomous scanning
+                 behaviour of the firmware
+
+*******************************************************************************/
+#define CsrWifiSmeScanConfigGetCfmCreate(msg__, dst__, src__, status__, scanConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_CONFIG_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->scanConfig = (scanConfig__);
+
+#define CsrWifiSmeScanConfigGetCfmSendTo(dst__, src__, status__, scanConfig__) \
+    { \
+        CsrWifiSmeScanConfigGetCfm *msg__; \
+        CsrWifiSmeScanConfigGetCfmCreate(msg__, dst__, src__, status__, scanConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanConfigGetCfmSend(dst__, status__, scanConfig__) \
+    CsrWifiSmeScanConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, scanConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the ScanConfig parameter.
+    The SME normally configures the firmware to perform autonomous scanning
+    without involving the host.
+    The firmware passes beacon / probe response or indicates loss of beacon
+    on certain changes of state, for example:
+      * A new AP is seen for the first time
+      * An AP is no longer visible
+      * The signal strength of an AP changes by more than a certain amount, as
+        configured by the thresholds in the scanConfig parameter
+    In addition to the autonomous scan, the wireless manager application may
+    request a scan at any time using CSR_WIFI_SME_SCAN_FULL_REQ.
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    scanConfig - Reports the configuration for the autonomous scanning behaviour
+                 of the firmware
+
+*******************************************************************************/
+#define CsrWifiSmeScanConfigSetReqCreate(msg__, dst__, src__, scanConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_CONFIG_SET_REQ, dst__, src__); \
+    msg__->scanConfig = (scanConfig__);
+
+#define CsrWifiSmeScanConfigSetReqSendTo(dst__, src__, scanConfig__) \
+    { \
+        CsrWifiSmeScanConfigSetReq *msg__; \
+        CsrWifiSmeScanConfigSetReqCreate(msg__, dst__, src__, scanConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanConfigSetReqSend(src__, scanConfig__) \
+    CsrWifiSmeScanConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, scanConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeScanConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeScanConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeScanConfigSetCfm *msg__; \
+        CsrWifiSmeScanConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanConfigSetCfmSend(dst__, status__) \
+    CsrWifiSmeScanConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanFullReqSend
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to request a
+    full scan.
+    Channels are scanned actively or passively according to the requirement
+    set by regulatory domain.
+    If the SME receives this primitive while a full scan is going on, the new
+    request is buffered and it will be served after the current full scan is
+    completed.
+
+  PARAMETERS
+    queue            - Message Source Task Queue (Cfm's will be sent to this Queue)
+    ssidCount        - Number of SSIDs provided.
+                       If it is 0, the SME will attempt to detect any network
+    ssid             - Points to the first SSID provided, if any.
+    bssid            - BSS identifier.
+                       If it is equal to FF-FF-FF-FF-FF, the SME will listen for
+                       messages from any BSS.
+                       If it is different from FF-FF-FF-FF-FF and any SSID is
+                       provided, one SSID must match the network of the BSS.
+    forceScan        - Forces the scan even if the SME is in a state which would
+                       normally prevent it (e.g. autonomous scan is running).
+    bssType          - Type of BSS to scan for
+    scanType         - Type of scan to perform
+    channelListCount - Number of channels provided.
+                       If it is 0, the SME will initiate a scan of all the
+                       supported channels that are permitted by the current
+                       regulatory domain.
+    channelList      - Points to the first channel , or NULL if channelListCount
+                       is zero.
+    probeIeLength    - Length of the information element in bytes to be sent
+                       with the probe message.
+    probeIe          - Points to the first byte of the information element to be
+                       sent with the probe message.
+
+*******************************************************************************/
+#define CsrWifiSmeScanFullReqCreate(msg__, dst__, src__, ssidCount__, ssid__, bssid__, forceScan__, bssType__, scanType__, channelListCount__, channelList__, probeIeLength__, probeIe__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanFullReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_FULL_REQ, dst__, src__); \
+    msg__->ssidCount = (ssidCount__); \
+    msg__->ssid = (ssid__); \
+    msg__->bssid = (bssid__); \
+    msg__->forceScan = (forceScan__); \
+    msg__->bssType = (bssType__); \
+    msg__->scanType = (scanType__); \
+    msg__->channelListCount = (channelListCount__); \
+    msg__->channelList = (channelList__); \
+    msg__->probeIeLength = (probeIeLength__); \
+    msg__->probeIe = (probeIe__);
+
+#define CsrWifiSmeScanFullReqSendTo(dst__, src__, ssidCount__, ssid__, bssid__, forceScan__, bssType__, scanType__, channelListCount__, channelList__, probeIeLength__, probeIe__) \
+    { \
+        CsrWifiSmeScanFullReq *msg__; \
+        CsrWifiSmeScanFullReqCreate(msg__, dst__, src__, ssidCount__, ssid__, bssid__, forceScan__, bssType__, scanType__, channelListCount__, channelList__, probeIeLength__, probeIe__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanFullReqSend(src__, ssidCount__, ssid__, bssid__, forceScan__, bssType__, scanType__, channelListCount__, channelList__, probeIeLength__, probeIe__) \
+    CsrWifiSmeScanFullReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, ssidCount__, ssid__, bssid__, forceScan__, bssType__, scanType__, channelListCount__, channelList__, probeIeLength__, probeIe__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanFullCfmSend
+
+  DESCRIPTION
+    The SME calls this primitive when the results from the scan are
+    available.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeScanFullCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanFullCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_FULL_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeScanFullCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeScanFullCfm *msg__; \
+        CsrWifiSmeScanFullCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanFullCfmSend(dst__, status__) \
+    CsrWifiSmeScanFullCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultIndSend
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it whenever a scan indication is received from the firmware.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    result - Points to a buffer containing a scan result.
+
+*******************************************************************************/
+#define CsrWifiSmeScanResultIndCreate(msg__, dst__, src__, result__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanResultInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_RESULT_IND, dst__, src__); \
+    msg__->result = (result__);
+
+#define CsrWifiSmeScanResultIndSendTo(dst__, src__, result__) \
+    { \
+        CsrWifiSmeScanResultInd *msg__; \
+        CsrWifiSmeScanResultIndCreate(msg__, dst__, src__, result__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanResultIndSend(dst__, result__) \
+    CsrWifiSmeScanResultIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, result__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsFlushReqSend
+
+  DESCRIPTION
+    The Wireless Manager calls this primitive to ask the SME to delete all
+    scan results from its cache, except for the scan result of any currently
+    connected network.
+    As scan results are received by the SME from the firmware, they are
+    cached in the SME memory.
+    Any time the Wireless Manager requests scan results, they are returned
+    from the SME internal cache.
+    For some applications it may be desirable to clear this cache prior to
+    requesting that a scan be performed; this will ensure that the cache then
+    only contains the networks detected in the most recent scan.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeScanResultsFlushReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanResultsFlushReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_RESULTS_FLUSH_REQ, dst__, src__);
+
+#define CsrWifiSmeScanResultsFlushReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeScanResultsFlushReq *msg__; \
+        CsrWifiSmeScanResultsFlushReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanResultsFlushReqSend(src__) \
+    CsrWifiSmeScanResultsFlushReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsFlushCfmSend
+
+  DESCRIPTION
+    The SME will call this primitive when the cache has been cleared.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeScanResultsFlushCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanResultsFlushCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_RESULTS_FLUSH_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeScanResultsFlushCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeScanResultsFlushCfm *msg__; \
+        CsrWifiSmeScanResultsFlushCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanResultsFlushCfmSend(dst__, status__) \
+    CsrWifiSmeScanResultsFlushCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsGetReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to retrieve the
+    current set of scan results, either after receiving a successful
+    CSR_WIFI_SME_SCAN_FULL_CFM, or to get autonomous scan results.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeScanResultsGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanResultsGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_RESULTS_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeScanResultsGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeScanResultsGetReq *msg__; \
+        CsrWifiSmeScanResultsGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanResultsGetReqSend(src__) \
+    CsrWifiSmeScanResultsGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsGetCfmSend
+
+  DESCRIPTION
+    The SME sends this primitive to provide the current set of scan results.
+
+  PARAMETERS
+    queue            - Destination Task Queue
+    status           - Reports the result of the request
+    scanResultsCount - Number of scan results
+    scanResults      - Points to a buffer containing an array of
+                       CsrWifiSmeScanResult structures.
+
+*******************************************************************************/
+#define CsrWifiSmeScanResultsGetCfmCreate(msg__, dst__, src__, status__, scanResultsCount__, scanResults__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeScanResultsGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SCAN_RESULTS_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->scanResultsCount = (scanResultsCount__); \
+    msg__->scanResults = (scanResults__);
+
+#define CsrWifiSmeScanResultsGetCfmSendTo(dst__, src__, status__, scanResultsCount__, scanResults__) \
+    { \
+        CsrWifiSmeScanResultsGetCfm *msg__; \
+        CsrWifiSmeScanResultsGetCfmCreate(msg__, dst__, src__, status__, scanResultsCount__, scanResults__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeScanResultsGetCfmSend(dst__, status__, scanResultsCount__, scanResults__) \
+    CsrWifiSmeScanResultsGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, scanResultsCount__, scanResults__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSetReqSend
+
+  DESCRIPTION
+    Used to pass custom data to the SME. Format is the same as 802.11 Info
+    Elements => | Id | Length | Data
+    1) Cmanr Test Mode "Id:0 Length:1 Data:0x00 = OFF 0x01 = ON" "0x00 0x01
+    (0x00|0x01)"
+
+  PARAMETERS
+    queue      - Message Source Task Queue (Cfm's will be sent to this Queue)
+    dataLength - Number of bytes in the buffer pointed to by 'data'
+    data       - Pointer to the buffer containing 'dataLength' bytes
+
+*******************************************************************************/
+#define CsrWifiSmeSetReqCreate(msg__, dst__, src__, dataLength__, data__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SET_REQ, dst__, src__); \
+    msg__->dataLength = (dataLength__); \
+    msg__->data = (data__);
+
+#define CsrWifiSmeSetReqSendTo(dst__, src__, dataLength__, data__) \
+    { \
+        CsrWifiSmeSetReq *msg__; \
+        CsrWifiSmeSetReqCreate(msg__, dst__, src__, dataLength__, data__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSetReqSend(src__, dataLength__, data__) \
+    CsrWifiSmeSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, dataLength__, data__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the Sme common parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeSmeCommonConfigGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeCommonConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_COMMON_CONFIG_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeSmeCommonConfigGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeSmeCommonConfigGetReq *msg__; \
+        CsrWifiSmeSmeCommonConfigGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeCommonConfigGetReqSend(src__) \
+    CsrWifiSmeSmeCommonConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    status       - Reports the result of the request
+    deviceConfig - Configuration options in the SME
+
+*******************************************************************************/
+#define CsrWifiSmeSmeCommonConfigGetCfmCreate(msg__, dst__, src__, status__, deviceConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeCommonConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_COMMON_CONFIG_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->deviceConfig = (deviceConfig__);
+
+#define CsrWifiSmeSmeCommonConfigGetCfmSendTo(dst__, src__, status__, deviceConfig__) \
+    { \
+        CsrWifiSmeSmeCommonConfigGetCfm *msg__; \
+        CsrWifiSmeSmeCommonConfigGetCfmCreate(msg__, dst__, src__, status__, deviceConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeCommonConfigGetCfmSend(dst__, status__, deviceConfig__) \
+    CsrWifiSmeSmeCommonConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, deviceConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the Sme common.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    deviceConfig - Configuration options in the SME
+
+*******************************************************************************/
+#define CsrWifiSmeSmeCommonConfigSetReqCreate(msg__, dst__, src__, deviceConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeCommonConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_COMMON_CONFIG_SET_REQ, dst__, src__); \
+    msg__->deviceConfig = (deviceConfig__);
+
+#define CsrWifiSmeSmeCommonConfigSetReqSendTo(dst__, src__, deviceConfig__) \
+    { \
+        CsrWifiSmeSmeCommonConfigSetReq *msg__; \
+        CsrWifiSmeSmeCommonConfigSetReqCreate(msg__, dst__, src__, deviceConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeCommonConfigSetReqSend(src__, deviceConfig__) \
+    CsrWifiSmeSmeCommonConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, deviceConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigSetCfmSend
+
+  DESCRIPTION
+    Reports the result of the request
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeSmeCommonConfigSetCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeCommonConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_COMMON_CONFIG_SET_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeSmeCommonConfigSetCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeSmeCommonConfigSetCfm *msg__; \
+        CsrWifiSmeSmeCommonConfigSetCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeCommonConfigSetCfmSend(dst__, status__) \
+    CsrWifiSmeSmeCommonConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the SmeStaConfig parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+#define CsrWifiSmeSmeStaConfigGetReqCreate(msg__, dst__, src__, interfaceTag__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeStaConfigGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_STA_CONFIG_GET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__);
+
+#define CsrWifiSmeSmeStaConfigGetReqSendTo(dst__, src__, interfaceTag__) \
+    { \
+        CsrWifiSmeSmeStaConfigGetReq *msg__; \
+        CsrWifiSmeSmeStaConfigGetReqCreate(msg__, dst__, src__, interfaceTag__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeStaConfigGetReqSend(src__, interfaceTag__) \
+    CsrWifiSmeSmeStaConfigGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    smeConfig    - Current SME Station Parameters
+
+*******************************************************************************/
+#define CsrWifiSmeSmeStaConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, smeConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeStaConfigGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_STA_CONFIG_GET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->smeConfig = (smeConfig__);
+
+#define CsrWifiSmeSmeStaConfigGetCfmSendTo(dst__, src__, interfaceTag__, status__, smeConfig__) \
+    { \
+        CsrWifiSmeSmeStaConfigGetCfm *msg__; \
+        CsrWifiSmeSmeStaConfigGetCfmCreate(msg__, dst__, src__, interfaceTag__, status__, smeConfig__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeStaConfigGetCfmSend(dst__, interfaceTag__, status__, smeConfig__) \
+    CsrWifiSmeSmeStaConfigGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, smeConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigSetReqSend
+
+  DESCRIPTION
+    This primitive sets the value of the SmeConfig parameter.
+
+  PARAMETERS
+    queue        - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    smeConfig    - SME Station Parameters to be set
+
+*******************************************************************************/
+#define CsrWifiSmeSmeStaConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, smeConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeStaConfigSetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_STA_CONFIG_SET_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->smeConfig = (smeConfig__);
+
+#define CsrWifiSmeSmeStaConfigSetReqSendTo(dst__, src__, interfaceTag__, smeConfig__) \
+    { \
+        CsrWifiSmeSmeStaConfigSetReq *msg__; \
+        CsrWifiSmeSmeStaConfigSetReqCreate(msg__, dst__, src__, interfaceTag__, smeConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeStaConfigSetReqSend(src__, interfaceTag__, smeConfig__) \
+    CsrWifiSmeSmeStaConfigSetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, smeConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigSetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue        - Destination Task Queue
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeSmeStaConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeSmeStaConfigSetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_SME_STA_CONFIG_SET_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeSmeStaConfigSetCfmSendTo(dst__, src__, interfaceTag__, status__) \
+    { \
+        CsrWifiSmeSmeStaConfigSetCfm *msg__; \
+        CsrWifiSmeSmeStaConfigSetCfmCreate(msg__, dst__, src__, interfaceTag__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeSmeStaConfigSetCfmSend(dst__, interfaceTag__, status__) \
+    CsrWifiSmeSmeStaConfigSetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeStationMacAddressGetReqSend
+
+  DESCRIPTION
+    This primitives is used to retrieve the current MAC address used by the
+    station.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeStationMacAddressGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeStationMacAddressGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_STATION_MAC_ADDRESS_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeStationMacAddressGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeStationMacAddressGetReq *msg__; \
+        CsrWifiSmeStationMacAddressGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeStationMacAddressGetReqSend(src__) \
+    CsrWifiSmeStationMacAddressGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeStationMacAddressGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue             - Destination Task Queue
+    status            - Reports the result of the request
+    stationMacAddress - Current MAC address of the station.
+
+*******************************************************************************/
+#define CsrWifiSmeStationMacAddressGetCfmCreate(msg__, dst__, src__, status__, stationMacAddress__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeStationMacAddressGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_STATION_MAC_ADDRESS_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    memcpy(msg__->stationMacAddress, (stationMacAddress__), sizeof(CsrWifiMacAddress) * 2);
+
+#define CsrWifiSmeStationMacAddressGetCfmSendTo(dst__, src__, status__, stationMacAddress__) \
+    { \
+        CsrWifiSmeStationMacAddressGetCfm *msg__; \
+        CsrWifiSmeStationMacAddressGetCfmCreate(msg__, dst__, src__, status__, stationMacAddress__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeStationMacAddressGetCfmSend(dst__, status__, stationMacAddress__) \
+    CsrWifiSmeStationMacAddressGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, stationMacAddress__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecReqSend
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to use the
+    TSPEC feature.
+    The chip supports the use of TSPECs and TCLAS for the use of IEEE
+    802.11/WMM Quality of Service features.
+    The API allows the wireless manager application to supply a correctly
+    formatted TSPEC and TCLAS pair to the driver.
+    After performing basic validation, the driver negotiates the installation
+    of the TSPEC with the AP as defined by the 802.11 specification.
+    The driver retains all TSPEC and TCLAS pairs until they are specifically
+    removed.
+    It is not compulsory for a TSPEC to have a TCLAS (NULL is used to
+    indicate that no TCLAS is supplied), while a TCLASS always require a
+    TSPEC.
+    The format of the TSPEC element is specified in 'WMM (including WMM Power
+    Save) Specification - Version 1.1' and 'ANSI/IEEE Std 802.11-REVmb/D3.0'.
+    For more information, see 'UniFi Configuring WMM and WMM-PS'.
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    action        - Specifies the action to be carried out on the list of TSPECs.
+                    CSR_WIFI_SME_LIST_ACTION_FLUSH is not applicable here.
+    transactionId - Unique Transaction ID for the TSPEC, as assigned by the
+                    driver
+    strict        - If it set to false, allows the SME to perform automatic
+                    TSPEC negotiation
+    ctrlMask      - Additional TSPEC configuration for CCX.
+                    Set mask with values from CsrWifiSmeTspecCtrl.
+                    CURRENTLY NOT SUPPORTED
+    tspecLength   - Length of the TSPEC.
+    tspec         - Points to the first byte of the TSPEC
+    tclasLength   - Length of the TCLAS.
+                    If it is equal to 0, no TCLASS is provided for the TSPEC
+    tclas         - Points to the first byte of the TCLAS, if any.
+
+*******************************************************************************/
+#define CsrWifiSmeTspecReqCreate(msg__, dst__, src__, interfaceTag__, action__, transactionId__, strict__, ctrlMask__, tspecLength__, tspec__, tclasLength__, tclas__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeTspecReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_TSPEC_REQ, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->action = (action__); \
+    msg__->transactionId = (transactionId__); \
+    msg__->strict = (strict__); \
+    msg__->ctrlMask = (ctrlMask__); \
+    msg__->tspecLength = (tspecLength__); \
+    msg__->tspec = (tspec__); \
+    msg__->tclasLength = (tclasLength__); \
+    msg__->tclas = (tclas__);
+
+#define CsrWifiSmeTspecReqSendTo(dst__, src__, interfaceTag__, action__, transactionId__, strict__, ctrlMask__, tspecLength__, tspec__, tclasLength__, tclas__) \
+    { \
+        CsrWifiSmeTspecReq *msg__; \
+        CsrWifiSmeTspecReqCreate(msg__, dst__, src__, interfaceTag__, action__, transactionId__, strict__, ctrlMask__, tspecLength__, tspec__, tclasLength__, tclas__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeTspecReqSend(src__, interfaceTag__, action__, transactionId__, strict__, ctrlMask__, tspecLength__, tspec__, tclasLength__, tclas__) \
+    CsrWifiSmeTspecReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, interfaceTag__, action__, transactionId__, strict__, ctrlMask__, tspecLength__, tspec__, tclasLength__, tclas__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecIndSend
+
+  DESCRIPTION
+    The SME will send this primitive to all the task that have registered to
+    receive it when a status change in the TSPEC occurs.
+
+  PARAMETERS
+    queue           - Destination Task Queue
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    transactionId   - Unique Transaction ID for the TSPEC, as assigned by the
+                      driver
+    tspecResultCode - Specifies the TSPEC operation requested by the peer
+                      station
+    tspecLength     - Length of the TSPEC.
+    tspec           - Points to the first byte of the TSPEC
+
+*******************************************************************************/
+#define CsrWifiSmeTspecIndCreate(msg__, dst__, src__, interfaceTag__, transactionId__, tspecResultCode__, tspecLength__, tspec__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeTspecInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_TSPEC_IND, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->transactionId = (transactionId__); \
+    msg__->tspecResultCode = (tspecResultCode__); \
+    msg__->tspecLength = (tspecLength__); \
+    msg__->tspec = (tspec__);
+
+#define CsrWifiSmeTspecIndSendTo(dst__, src__, interfaceTag__, transactionId__, tspecResultCode__, tspecLength__, tspec__) \
+    { \
+        CsrWifiSmeTspecInd *msg__; \
+        CsrWifiSmeTspecIndCreate(msg__, dst__, src__, interfaceTag__, transactionId__, tspecResultCode__, tspecLength__, tspec__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeTspecIndSend(dst__, interfaceTag__, transactionId__, tspecResultCode__, tspecLength__, tspec__) \
+    CsrWifiSmeTspecIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, transactionId__, tspecResultCode__, tspecLength__, tspec__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecCfmSend
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the TSpec primitive
+    request.
+
+  PARAMETERS
+    queue           - Destination Task Queue
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    status          - Reports the result of the request
+    transactionId   - Unique Transaction ID for the TSPEC, as assigned by the
+                      driver
+    tspecResultCode - Specifies the result of the negotiated TSPEC operation
+    tspecLength     - Length of the TSPEC.
+    tspec           - Points to the first byte of the TSPEC
+
+*******************************************************************************/
+#define CsrWifiSmeTspecCfmCreate(msg__, dst__, src__, interfaceTag__, status__, transactionId__, tspecResultCode__, tspecLength__, tspec__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeTspecCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_TSPEC_CFM, dst__, src__); \
+    msg__->interfaceTag = (interfaceTag__); \
+    msg__->status = (status__); \
+    msg__->transactionId = (transactionId__); \
+    msg__->tspecResultCode = (tspecResultCode__); \
+    msg__->tspecLength = (tspecLength__); \
+    msg__->tspec = (tspec__);
+
+#define CsrWifiSmeTspecCfmSendTo(dst__, src__, interfaceTag__, status__, transactionId__, tspecResultCode__, tspecLength__, tspec__) \
+    { \
+        CsrWifiSmeTspecCfm *msg__; \
+        CsrWifiSmeTspecCfmCreate(msg__, dst__, src__, interfaceTag__, status__, transactionId__, tspecResultCode__, tspecLength__, tspec__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeTspecCfmSend(dst__, interfaceTag__, status__, transactionId__, tspecResultCode__, tspecLength__, tspec__) \
+    CsrWifiSmeTspecCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, interfaceTag__, status__, transactionId__, tspecResultCode__, tspecLength__, tspec__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeVersionsGetReqSend
+
+  DESCRIPTION
+    This primitive gets the value of the Versions parameter.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeVersionsGetReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeVersionsGetReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_VERSIONS_GET_REQ, dst__, src__);
+
+#define CsrWifiSmeVersionsGetReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeVersionsGetReq *msg__; \
+        CsrWifiSmeVersionsGetReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeVersionsGetReqSend(src__) \
+    CsrWifiSmeVersionsGetReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeVersionsGetCfmSend
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  PARAMETERS
+    queue    - Destination Task Queue
+    status   - Reports the result of the request
+    versions - Version IDs of the product
+
+*******************************************************************************/
+#define CsrWifiSmeVersionsGetCfmCreate(msg__, dst__, src__, status__, versions__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeVersionsGetCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_VERSIONS_GET_CFM, dst__, src__); \
+    msg__->status = (status__); \
+    msg__->versions = (versions__);
+
+#define CsrWifiSmeVersionsGetCfmSendTo(dst__, src__, status__, versions__) \
+    { \
+        CsrWifiSmeVersionsGetCfm *msg__; \
+        CsrWifiSmeVersionsGetCfmCreate(msg__, dst__, src__, status__, versions__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeVersionsGetCfmSend(dst__, status__, versions__) \
+    CsrWifiSmeVersionsGetCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__, versions__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiFlightmodeReqSend
+
+  DESCRIPTION
+    The wireless manager application may call this primitive on boot-up of
+    the platform to ensure that the chip is placed in a mode that prevents
+    any emission of RF energy.
+    This primitive is an alternative to CSR_WIFI_SME_WIFI_ON_REQ.
+    As in CSR_WIFI_SME_WIFI_ON_REQ, it causes the download of the patch file
+    (if any) and the programming of the initial MIB settings (if supplied by
+    the WMA), but it also ensures that the chip is left in its lowest
+    possible power-mode with the radio subsystems disabled.
+    This feature is useful on platforms where power cannot be removed from
+    the chip (leaving the chip not initialised will cause it to consume more
+    power so calling this function ensures that the chip is initialised into
+    a low power mode but without entering a state where it could emit any RF
+    energy).
+    NOTE: this primitive does not cause the Wi-Fi to change state: Wi-Fi
+    stays conceptually off. Configuration primitives can be sent after
+    CSR_WIFI_SME_WIFI_FLIGHTMODE_REQ and the configuration will be maintained.
+    Requests that require the state of the Wi-Fi to be ON will return
+    CSR_WIFI_SME_STATUS_WIFI_OFF in their confirms.
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    address       - Optionally specifies a station MAC address.
+                    In normal use, the manager should set the address to 0xFF
+                    0xFF 0xFF 0xFF 0xFF 0xFF, which will cause the chip to use
+                    the MAC address in the MIB.
+    mibFilesCount - Number of provided data blocks with initial MIB values
+    mibFiles      - Points to the first data block with initial MIB values.
+                    These data blocks are typically the contents of the provided
+                    files ufmib.dat and localmib.dat, available from the host
+                    file system, if they exist.
+                    These files typically contain radio tuning and calibration
+                    values.
+                    More values can be created using the Host Tools.
+
+*******************************************************************************/
+#define CsrWifiSmeWifiFlightmodeReqCreate(msg__, dst__, src__, address__, mibFilesCount__, mibFiles__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiFlightmodeReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_FLIGHTMODE_REQ, dst__, src__); \
+    msg__->address = (address__); \
+    msg__->mibFilesCount = (mibFilesCount__); \
+    msg__->mibFiles = (mibFiles__);
+
+#define CsrWifiSmeWifiFlightmodeReqSendTo(dst__, src__, address__, mibFilesCount__, mibFiles__) \
+    { \
+        CsrWifiSmeWifiFlightmodeReq *msg__; \
+        CsrWifiSmeWifiFlightmodeReqCreate(msg__, dst__, src__, address__, mibFilesCount__, mibFiles__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiFlightmodeReqSend(src__, address__, mibFilesCount__, mibFiles__) \
+    CsrWifiSmeWifiFlightmodeReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, address__, mibFilesCount__, mibFiles__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiFlightmodeCfmSend
+
+  DESCRIPTION
+    The SME calls this primitive when the chip is initialised for low power
+    mode and with the radio subsystem disabled. To leave flight mode, and
+    enable Wi-Fi, the wireless manager application should call
+    CSR_WIFI_SME_WIFI_ON_REQ.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeWifiFlightmodeCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiFlightmodeCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_FLIGHTMODE_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeWifiFlightmodeCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeWifiFlightmodeCfm *msg__; \
+        CsrWifiSmeWifiFlightmodeCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiFlightmodeCfmSend(dst__, status__) \
+    CsrWifiSmeWifiFlightmodeCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOffReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to turn off the
+    chip, thus saving power when Wi-Fi is not in use.
+
+  PARAMETERS
+    queue  - Message Source Task Queue (Cfm's will be sent to this Queue)
+
+*******************************************************************************/
+#define CsrWifiSmeWifiOffReqCreate(msg__, dst__, src__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiOffReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_OFF_REQ, dst__, src__);
+
+#define CsrWifiSmeWifiOffReqSendTo(dst__, src__) \
+    { \
+        CsrWifiSmeWifiOffReq *msg__; \
+        CsrWifiSmeWifiOffReqCreate(msg__, dst__, src__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiOffReqSend(src__) \
+    CsrWifiSmeWifiOffReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOffIndSend
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it to report that the chip has been turned off.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    reason - Indicates the reason why the Wi-Fi has been switched off.
+
+*******************************************************************************/
+#define CsrWifiSmeWifiOffIndCreate(msg__, dst__, src__, reason__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiOffInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_OFF_IND, dst__, src__); \
+    msg__->reason = (reason__);
+
+#define CsrWifiSmeWifiOffIndSendTo(dst__, src__, reason__) \
+    { \
+        CsrWifiSmeWifiOffInd *msg__; \
+        CsrWifiSmeWifiOffIndCreate(msg__, dst__, src__, reason__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiOffIndSend(dst__, reason__) \
+    CsrWifiSmeWifiOffIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, reason__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOffCfmSend
+
+  DESCRIPTION
+    After receiving CSR_WIFI_SME_WIFI_OFF_REQ, if the chip is connected to a
+    network, the SME will perform a disconnect operation, will send a
+    CSR_WIFI_SME_MEDIA_STATUS_IND with
+    CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED, and then will call
+    CSR_WIFI_SME_WIFI_OFF_CFM when the chip is off.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeWifiOffCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiOffCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_OFF_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeWifiOffCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeWifiOffCfm *msg__; \
+        CsrWifiSmeWifiOffCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiOffCfmSend(dst__, status__) \
+    CsrWifiSmeWifiOffCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOnReqSend
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to turn on the
+    Wi-Fi chip.
+    If the Wi-Fi chip is currently off, the SME turns the Wi-Fi chip on,
+    downloads the patch file (if any), and programs the initial MIB settings
+    (if supplied by the WMA).
+    The patch file is not provided with the SME API; its downloading is
+    automatic and handled internally by the system.
+    The MIB settings, when provided, override the default values that the
+    firmware loads from EEPROM.
+    If the Wi-Fi chip is already on, the SME takes no action and returns a
+    successful status in the confirm.
+
+  PARAMETERS
+    queue         - Message Source Task Queue (Cfm's will be sent to this Queue)
+    address       - Optionally specifies a station MAC address.
+                    In normal use, the manager should set the address to 0xFF
+                    0xFF 0xFF 0xFF 0xFF 0xFF, which will cause the chip to use
+                    the MAC address in the MIB
+    mibFilesCount - Number of provided data blocks with initial MIB values
+    mibFiles      - Points to the first data block with initial MIB values.
+                    These data blocks are typically the contents of the provided
+                    files ufmib.dat and localmib.dat, available from the host
+                    file system, if they exist.
+                    These files typically contain radio tuning and calibration
+                    values.
+                    More values can be created using the Host Tools.
+
+*******************************************************************************/
+#define CsrWifiSmeWifiOnReqCreate(msg__, dst__, src__, address__, mibFilesCount__, mibFiles__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiOnReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_ON_REQ, dst__, src__); \
+    msg__->address = (address__); \
+    msg__->mibFilesCount = (mibFilesCount__); \
+    msg__->mibFiles = (mibFiles__);
+
+#define CsrWifiSmeWifiOnReqSendTo(dst__, src__, address__, mibFilesCount__, mibFiles__) \
+    { \
+        CsrWifiSmeWifiOnReq *msg__; \
+        CsrWifiSmeWifiOnReqCreate(msg__, dst__, src__, address__, mibFilesCount__, mibFiles__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiOnReqSend(src__, address__, mibFilesCount__, mibFiles__) \
+    CsrWifiSmeWifiOnReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, address__, mibFilesCount__, mibFiles__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOnIndSend
+
+  DESCRIPTION
+    The SME sends this primitive to all tasks that have registered to receive
+    it once the chip becomes available and ready to use.
+
+  PARAMETERS
+    queue   - Destination Task Queue
+    address - Current MAC address
+
+*******************************************************************************/
+#define CsrWifiSmeWifiOnIndCreate(msg__, dst__, src__, address__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiOnInd), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_ON_IND, dst__, src__); \
+    msg__->address = (address__);
+
+#define CsrWifiSmeWifiOnIndSendTo(dst__, src__, address__) \
+    { \
+        CsrWifiSmeWifiOnInd *msg__; \
+        CsrWifiSmeWifiOnIndCreate(msg__, dst__, src__, address__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiOnIndSend(dst__, address__) \
+    CsrWifiSmeWifiOnIndSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, address__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOnCfmSend
+
+  DESCRIPTION
+    The SME sends this primitive to the task that has sent the request once
+    the chip has been initialised and is available for use.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Reports the result of the request
+
+*******************************************************************************/
+#define CsrWifiSmeWifiOnCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWifiOnCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WIFI_ON_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeWifiOnCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeWifiOnCfm *msg__; \
+        CsrWifiSmeWifiOnCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWifiOnCfmSend(dst__, status__) \
+    CsrWifiSmeWifiOnCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfigurationReqSend
+
+  DESCRIPTION
+    This primitive passes the WPS information for the device to SME. This may
+    be accepted only if no interface is active.
+
+  PARAMETERS
+    queue     - Message Source Task Queue (Cfm's will be sent to this Queue)
+    wpsConfig - WPS config.
+
+*******************************************************************************/
+#define CsrWifiSmeWpsConfigurationReqCreate(msg__, dst__, src__, wpsConfig__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWpsConfigurationReq), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WPS_CONFIGURATION_REQ, dst__, src__); \
+    msg__->wpsConfig = (wpsConfig__);
+
+#define CsrWifiSmeWpsConfigurationReqSendTo(dst__, src__, wpsConfig__) \
+    { \
+        CsrWifiSmeWpsConfigurationReq *msg__; \
+        CsrWifiSmeWpsConfigurationReqCreate(msg__, dst__, src__, wpsConfig__); \
+        CsrMsgTransport(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWpsConfigurationReqSend(src__, wpsConfig__) \
+    CsrWifiSmeWpsConfigurationReqSendTo(CSR_WIFI_SME_LIB_DESTINATION_QUEUE, src__, wpsConfig__)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfigurationCfmSend
+
+  DESCRIPTION
+    Confirm.
+
+  PARAMETERS
+    queue  - Destination Task Queue
+    status - Status of the request.
+
+*******************************************************************************/
+#define CsrWifiSmeWpsConfigurationCfmCreate(msg__, dst__, src__, status__) \
+    msg__ = kmalloc(sizeof(CsrWifiSmeWpsConfigurationCfm), GFP_KERNEL); \
+    CsrWifiFsmEventInit(&msg__->common, CSR_WIFI_SME_PRIM, CSR_WIFI_SME_WPS_CONFIGURATION_CFM, dst__, src__); \
+    msg__->status = (status__);
+
+#define CsrWifiSmeWpsConfigurationCfmSendTo(dst__, src__, status__) \
+    { \
+        CsrWifiSmeWpsConfigurationCfm *msg__; \
+        CsrWifiSmeWpsConfigurationCfmCreate(msg__, dst__, src__, status__); \
+        CsrSchedMessagePut(dst__, CSR_WIFI_SME_PRIM, msg__); \
+    }
+
+#define CsrWifiSmeWpsConfigurationCfmSend(dst__, status__) \
+    CsrWifiSmeWpsConfigurationCfmSendTo(dst__, CSR_WIFI_SME_IFACEQUEUE, status__)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_SME_LIB_H__ */
diff --git a/drivers/staging/csr/csr_wifi_sme_prim.h b/drivers/staging/csr/csr_wifi_sme_prim.h
new file mode 100644
index 00000000000..55cac5059bf
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_prim.h
@@ -0,0 +1,6519 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_PRIM_H__
+#define CSR_WIFI_SME_PRIM_H__
+
+#include <linux/types.h>
+#include "csr_prim_defs.h"
+#include "csr_sched.h"
+#include "csr_wifi_common.h"
+#include "csr_result.h"
+#include "csr_wifi_fsm_event.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CSR_WIFI_SME_PRIM                                               (0x0404)
+
+typedef CsrPrim CsrWifiSmePrim;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSme80211NetworkType
+
+  DESCRIPTION
+    Indicates the physical layer of the network
+
+ VALUES
+    CSR_WIFI_SME_80211_NETWORK_TYPE_DS
+                   - Direct-sequence spread spectrum
+    CSR_WIFI_SME_80211_NETWORK_TYPE_OFDM24
+                   - Orthogonal Frequency Division Multiplexing at 2.4 GHz
+    CSR_WIFI_SME_80211_NETWORK_TYPE_OFDM5
+                   - Orthogonal Frequency Division Multiplexing at 5 GHz
+    CSR_WIFI_SME_80211_NETWORK_TYPE_AUTO
+                   - Automatic
+
+*******************************************************************************/
+typedef u8 CsrWifiSme80211NetworkType;
+#define CSR_WIFI_SME_80211_NETWORK_TYPE_DS       ((CsrWifiSme80211NetworkType) 0x00)
+#define CSR_WIFI_SME_80211_NETWORK_TYPE_OFDM24   ((CsrWifiSme80211NetworkType) 0x01)
+#define CSR_WIFI_SME_80211_NETWORK_TYPE_OFDM5    ((CsrWifiSme80211NetworkType) 0x02)
+#define CSR_WIFI_SME_80211_NETWORK_TYPE_AUTO     ((CsrWifiSme80211NetworkType) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSme80211PrivacyMode
+
+  DESCRIPTION
+    Bits to enable or disable the privacy mode
+
+ VALUES
+    CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED
+                   - Privacy mode is enabled: use of WEP for confidentiality is
+                     required.
+    CSR_WIFI_SME_80211_PRIVACY_MODE_ENABLED
+                   - Privacy mode is disabled
+
+*******************************************************************************/
+typedef u8 CsrWifiSme80211PrivacyMode;
+#define CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED   ((CsrWifiSme80211PrivacyMode) 0x00)
+#define CSR_WIFI_SME_80211_PRIVACY_MODE_ENABLED    ((CsrWifiSme80211PrivacyMode) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSme80211dTrustLevel
+
+  DESCRIPTION
+    Level of trust for the information coming from the network
+
+ VALUES
+    CSR_WIFI_SME_80211D_TRUST_LEVEL_STRICT
+                   - Start with passive scanning and only accept country IE for
+                     updating channel lists
+    CSR_WIFI_SME_80211D_TRUST_LEVEL_ADJUNCT
+                   - As above plus accept adjunct technology location
+                     information
+    CSR_WIFI_SME_80211D_TRUST_LEVEL_BSS
+                   - As above accept plus receiving channel from infrastructure
+                     networks
+    CSR_WIFI_SME_80211D_TRUST_LEVEL_IBSS
+                   - As above accept plus receiving channel from the ad hoc
+                     networks
+    CSR_WIFI_SME_80211D_TRUST_LEVEL_MIB
+                   - Start with active scanning with list of active channels
+                     from the MIB and accept as above
+    CSR_WIFI_SME_80211D_TRUST_LEVEL_DISABLED
+                   - Start with active scanning with list of active channels
+                     from the MIB and ignore any channel information from the
+                     network
+
+*******************************************************************************/
+typedef u8 CsrWifiSme80211dTrustLevel;
+#define CSR_WIFI_SME_80211D_TRUST_LEVEL_STRICT     ((CsrWifiSme80211dTrustLevel) 0x01)
+#define CSR_WIFI_SME_80211D_TRUST_LEVEL_ADJUNCT    ((CsrWifiSme80211dTrustLevel) 0x02)
+#define CSR_WIFI_SME_80211D_TRUST_LEVEL_BSS        ((CsrWifiSme80211dTrustLevel) 0x03)
+#define CSR_WIFI_SME_80211D_TRUST_LEVEL_IBSS       ((CsrWifiSme80211dTrustLevel) 0x04)
+#define CSR_WIFI_SME_80211D_TRUST_LEVEL_MIB        ((CsrWifiSme80211dTrustLevel) 0x05)
+#define CSR_WIFI_SME_80211D_TRUST_LEVEL_DISABLED   ((CsrWifiSme80211dTrustLevel) 0x06)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAmpStatus
+
+  DESCRIPTION
+    AMP Current Status
+
+ VALUES
+    CSR_WIFI_SME_AMP_ACTIVE   - AMP ACTIVE.
+    CSR_WIFI_SME_AMP_INACTIVE - AMP INACTIVE
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeAmpStatus;
+#define CSR_WIFI_SME_AMP_ACTIVE     ((CsrWifiSmeAmpStatus) 0x00)
+#define CSR_WIFI_SME_AMP_INACTIVE   ((CsrWifiSmeAmpStatus) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAuthMode
+
+  DESCRIPTION
+    Define bits for CsrWifiSmeAuthMode
+
+ VALUES
+    CSR_WIFI_SME_AUTH_MODE_80211_OPEN
+                   - Connects to an open system network (i.e. no authentication,
+                     no encryption) or to a WEP enabled network.
+    CSR_WIFI_SME_AUTH_MODE_80211_SHARED
+                   - Connect to a WEP enabled network.
+    CSR_WIFI_SME_AUTH_MODE_8021X_WPA
+                   - Connects to a WPA Enterprise enabled network.
+    CSR_WIFI_SME_AUTH_MODE_8021X_WPAPSK
+                   - Connects to a WPA with Pre-Shared Key enabled network.
+    CSR_WIFI_SME_AUTH_MODE_8021X_WPA2
+                   - Connects to a WPA2 Enterprise enabled network.
+    CSR_WIFI_SME_AUTH_MODE_8021X_WPA2PSK
+                   - Connects to a WPA2 with Pre-Shared Key enabled network.
+    CSR_WIFI_SME_AUTH_MODE_8021X_CCKM
+                   - Connects to a CCKM enabled network.
+    CSR_WIFI_SME_AUTH_MODE_WAPI_WAI
+                   - Connects to a WAPI Enterprise enabled network.
+    CSR_WIFI_SME_AUTH_MODE_WAPI_WAIPSK
+                   - Connects to a WAPI with Pre-Shared Key enabled network.
+    CSR_WIFI_SME_AUTH_MODE_8021X_OTHER1X
+                   - For future use.
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeAuthMode;
+#define CSR_WIFI_SME_AUTH_MODE_80211_OPEN      ((CsrWifiSmeAuthMode) 0x0001)
+#define CSR_WIFI_SME_AUTH_MODE_80211_SHARED    ((CsrWifiSmeAuthMode) 0x0002)
+#define CSR_WIFI_SME_AUTH_MODE_8021X_WPA       ((CsrWifiSmeAuthMode) 0x0004)
+#define CSR_WIFI_SME_AUTH_MODE_8021X_WPAPSK    ((CsrWifiSmeAuthMode) 0x0008)
+#define CSR_WIFI_SME_AUTH_MODE_8021X_WPA2      ((CsrWifiSmeAuthMode) 0x0010)
+#define CSR_WIFI_SME_AUTH_MODE_8021X_WPA2PSK   ((CsrWifiSmeAuthMode) 0x0020)
+#define CSR_WIFI_SME_AUTH_MODE_8021X_CCKM      ((CsrWifiSmeAuthMode) 0x0040)
+#define CSR_WIFI_SME_AUTH_MODE_WAPI_WAI        ((CsrWifiSmeAuthMode) 0x0080)
+#define CSR_WIFI_SME_AUTH_MODE_WAPI_WAIPSK     ((CsrWifiSmeAuthMode) 0x0100)
+#define CSR_WIFI_SME_AUTH_MODE_8021X_OTHER1X   ((CsrWifiSmeAuthMode) 0x0200)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeBasicUsability
+
+  DESCRIPTION
+    Indicates the usability level of a channel
+
+ VALUES
+    CSR_WIFI_SME_BASIC_USABILITY_UNUSABLE
+                   - Not usable; connection not recommended
+    CSR_WIFI_SME_BASIC_USABILITY_POOR
+                   - Poor quality; connect only if nothing better is available
+    CSR_WIFI_SME_BASIC_USABILITY_SATISFACTORY
+                   - Quality is satisfactory
+    CSR_WIFI_SME_BASIC_USABILITY_NOT_CONNECTED
+                   - Not connected
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeBasicUsability;
+#define CSR_WIFI_SME_BASIC_USABILITY_UNUSABLE        ((CsrWifiSmeBasicUsability) 0x00)
+#define CSR_WIFI_SME_BASIC_USABILITY_POOR            ((CsrWifiSmeBasicUsability) 0x01)
+#define CSR_WIFI_SME_BASIC_USABILITY_SATISFACTORY    ((CsrWifiSmeBasicUsability) 0x02)
+#define CSR_WIFI_SME_BASIC_USABILITY_NOT_CONNECTED   ((CsrWifiSmeBasicUsability) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeBssType
+
+  DESCRIPTION
+    Indicates the BSS type
+
+ VALUES
+    CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE
+                   - Infrastructure BSS.
+    CSR_WIFI_SME_BSS_TYPE_ADHOC
+                   - Ad hoc or Independent BSS.
+    CSR_WIFI_SME_BSS_TYPE_ANY_BSS
+                   - Specifies any type of BSS
+    CSR_WIFI_SME_BSS_TYPE_P2P
+                   - Specifies P2P
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeBssType;
+#define CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE   ((CsrWifiSmeBssType) 0x00)
+#define CSR_WIFI_SME_BSS_TYPE_ADHOC            ((CsrWifiSmeBssType) 0x01)
+#define CSR_WIFI_SME_BSS_TYPE_ANY_BSS          ((CsrWifiSmeBssType) 0x02)
+#define CSR_WIFI_SME_BSS_TYPE_P2P              ((CsrWifiSmeBssType) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexScheme
+
+  DESCRIPTION
+    Options for the coexistence signalling
+    Same as MibValues
+
+ VALUES
+    CSR_WIFI_SME_COEX_SCHEME_DISABLED
+                   - The coexistence signalling is disabled
+    CSR_WIFI_SME_COEX_SCHEME_CSR
+                   - Basic CSR coexistence signalling
+    CSR_WIFI_SME_COEX_SCHEME_CSR_CHANNEL
+                   - Full CSR coexistence signalling
+    CSR_WIFI_SME_COEX_SCHEME_PTA
+                   - Packet Traffic Arbitrator coexistence signalling
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeCoexScheme;
+#define CSR_WIFI_SME_COEX_SCHEME_DISABLED      ((CsrWifiSmeCoexScheme) 0x00)
+#define CSR_WIFI_SME_COEX_SCHEME_CSR           ((CsrWifiSmeCoexScheme) 0x01)
+#define CSR_WIFI_SME_COEX_SCHEME_CSR_CHANNEL   ((CsrWifiSmeCoexScheme) 0x02)
+#define CSR_WIFI_SME_COEX_SCHEME_PTA           ((CsrWifiSmeCoexScheme) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeControlIndication
+
+  DESCRIPTION
+    Indicates the reason why the Wi-Fi has been switched off.
+    The values of this type are used across the NME/SME/Router API's and they
+    must be kept consistent with the corresponding types in the .xml of the
+    ottherinterfaces
+
+ VALUES
+    CSR_WIFI_SME_CONTROL_INDICATION_ERROR
+                   - An unrecoverable error (for example, an unrecoverable SDIO
+                     error) has occurred.
+                     The wireless manager application should reinitialise the
+                     chip by calling CSR_WIFI_SME_WIFI_ON_REQ.
+    CSR_WIFI_SME_CONTROL_INDICATION_EXIT
+                   - The chip became unavailable due to an external action, for
+                     example, when a plug-in card is ejected or the driver is
+                     unloaded.
+    CSR_WIFI_SME_CONTROL_INDICATION_USER_REQUESTED
+                   - The Wi-Fi has been switched off as the wireless manager
+                     application has sent CSR_WIFI_SME_WIFI_OFF_REQ
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeControlIndication;
+#define CSR_WIFI_SME_CONTROL_INDICATION_ERROR            ((CsrWifiSmeControlIndication) 0x01)
+#define CSR_WIFI_SME_CONTROL_INDICATION_EXIT             ((CsrWifiSmeControlIndication) 0x02)
+#define CSR_WIFI_SME_CONTROL_INDICATION_USER_REQUESTED   ((CsrWifiSmeControlIndication) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCtsProtectionType
+
+  DESCRIPTION
+    SME CTS Protection Types
+
+ VALUES
+    CSR_WIFI_SME_CTS_PROTECTION_AUTOMATIC
+                   - AP CTS Protection automatic based on non-ERP station in own
+                     BSS or neighbouring BSS on the same channel based on OLBC.
+                     This requires monitoring of beacons from other APs.
+    CSR_WIFI_SME_CTS_PROTECTION_FORCE_ENABLED
+                   - AP CTS Protection Force enabled
+    CSR_WIFI_SME_CTS_PROTECTION_FORCE_DISABLED
+                   - AP CTS Protection Force disabled.
+    CSR_WIFI_SME_CTS_PROTECTION_AUTOMATIC_NO_OLBC
+                   - AP CTS Protection automatic without considering OLBC but
+                     considering non-ERP station in the own BSS Valid only if AP
+                     is configured to work in 802.11bg or 802.11g mode otherwise
+                     this option specifies the same behaviour as AUTOMATIC
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeCtsProtectionType;
+#define CSR_WIFI_SME_CTS_PROTECTION_AUTOMATIC           ((CsrWifiSmeCtsProtectionType) 0x00)
+#define CSR_WIFI_SME_CTS_PROTECTION_FORCE_ENABLED       ((CsrWifiSmeCtsProtectionType) 0x01)
+#define CSR_WIFI_SME_CTS_PROTECTION_FORCE_DISABLED      ((CsrWifiSmeCtsProtectionType) 0x02)
+#define CSR_WIFI_SME_CTS_PROTECTION_AUTOMATIC_NO_OLBC   ((CsrWifiSmeCtsProtectionType) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeD3AutoScanMode
+
+  DESCRIPTION
+    Autonomous scan status while in D3 suspended period
+
+ VALUES
+    CSR_WIFI_SME_D3AUTO_SCAN_MODE_PSON
+                   - Autonomous scan stays on
+    CSR_WIFI_SME_D3AUTO_SCAN_MODE_PSOFF
+                   - Autonomous scan is switched off
+    CSR_WIFI_SME_D3AUTO_SCAN_MODE_PSAUTO
+                   - Automatically select autoscanning behaviour.
+                     CURRENTLY NOT SUPPORTED
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeD3AutoScanMode;
+#define CSR_WIFI_SME_D3AUTO_SCAN_MODE_PSON     ((CsrWifiSmeD3AutoScanMode) 0x00)
+#define CSR_WIFI_SME_D3AUTO_SCAN_MODE_PSOFF    ((CsrWifiSmeD3AutoScanMode) 0x01)
+#define CSR_WIFI_SME_D3AUTO_SCAN_MODE_PSAUTO   ((CsrWifiSmeD3AutoScanMode) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEncryption
+
+  DESCRIPTION
+    Defines bits for CsrWifiSmeEncryption
+    For a WEP enabled network, the caller must specify the correct
+    combination of flags in the encryptionModeMask.
+
+ VALUES
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_NONE
+                   - No encryption set
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP40
+                   - Selects 40 byte key WEP for unicast communication
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP104
+                   - Selects 104 byte key WEP for unicast communication
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_TKIP
+                   - Selects TKIP for unicast communication
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_CCMP
+                   - Selects CCMP for unicast communication
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_SMS4
+                   - Selects SMS4 for unicast communication
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP40
+                   - Selects 40 byte key WEP for broadcast messages
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP104
+                   - Selects 104 byte key WEP for broadcast messages
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_TKIP
+                   - Selects a TKIP for broadcast messages
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_CCMP
+                   - Selects CCMP for broadcast messages
+    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_SMS4
+                   - Selects SMS4 for broadcast messages
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeEncryption;
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_NONE              ((CsrWifiSmeEncryption) 0x0000)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP40    ((CsrWifiSmeEncryption) 0x0001)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP104   ((CsrWifiSmeEncryption) 0x0002)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_TKIP     ((CsrWifiSmeEncryption) 0x0004)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_CCMP     ((CsrWifiSmeEncryption) 0x0008)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_SMS4     ((CsrWifiSmeEncryption) 0x0010)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP40       ((CsrWifiSmeEncryption) 0x0020)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP104      ((CsrWifiSmeEncryption) 0x0040)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_TKIP        ((CsrWifiSmeEncryption) 0x0080)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_CCMP        ((CsrWifiSmeEncryption) 0x0100)
+#define CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_SMS4        ((CsrWifiSmeEncryption) 0x0200)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeFirmwareDriverInterface
+
+  DESCRIPTION
+    Type of communication between Host and Firmware
+
+ VALUES
+    CSR_WIFI_SME_FIRMWARE_DRIVER_INTERFACE_UNIT_DATA_INTERFACE
+                   - No preformated header. NOT SUPPORTED in the current release
+    CSR_WIFI_SME_FIRMWARE_DRIVER_INTERFACE_PACKET_INTERFACE
+                   - Preformated IEEE 802.11 header for user plane
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeFirmwareDriverInterface;
+#define CSR_WIFI_SME_FIRMWARE_DRIVER_INTERFACE_UNIT_DATA_INTERFACE   ((CsrWifiSmeFirmwareDriverInterface) 0x00)
+#define CSR_WIFI_SME_FIRMWARE_DRIVER_INTERFACE_PACKET_INTERFACE      ((CsrWifiSmeFirmwareDriverInterface) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostPowerMode
+
+  DESCRIPTION
+    Defines the power mode
+
+ VALUES
+    CSR_WIFI_SME_HOST_POWER_MODE_ACTIVE
+                   - Host device is running on external power.
+    CSR_WIFI_SME_HOST_POWER_MODE_POWER_SAVE
+                   - Host device is running on (internal) battery power.
+    CSR_WIFI_SME_HOST_POWER_MODE_FULL_POWER_SAVE
+                   - For future use.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeHostPowerMode;
+#define CSR_WIFI_SME_HOST_POWER_MODE_ACTIVE            ((CsrWifiSmeHostPowerMode) 0x00)
+#define CSR_WIFI_SME_HOST_POWER_MODE_POWER_SAVE        ((CsrWifiSmeHostPowerMode) 0x01)
+#define CSR_WIFI_SME_HOST_POWER_MODE_FULL_POWER_SAVE   ((CsrWifiSmeHostPowerMode) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeIEEE80211Reason
+
+  DESCRIPTION
+    As definined in the IEEE 802.11 standards
+
+ VALUES
+    CSR_WIFI_SME_IEEE80211_REASON_SUCCESS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNSPECIFIED_REASON
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_AUTHENTICATION_NOT_VALID
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_DEAUTHENTICATED_LEAVE_BSS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_INACTIVITY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_AP_OVERLOAD
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_CLASS_2FRAME_ERROR
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_CLASS_3FRAME_ERROR
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_LEAVE_BSS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_ASSOCIATION_NOT_AUTHENTICATED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_POWER_CAPABILITY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_SUPPORTED_CHANNELS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_INFORMATION_ELEMENT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_MICHAEL_MIC_FAILURE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_FOURWAY_HANDSHAKE_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_GROUP_KEY_UPDATE_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_HANDSHAKE_ELEMENT_DIFFERENT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_GROUP_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_PAIRWISE_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_AKMP
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNSUPPORTED_RSN_IEVERSION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_RSN_IECAPABILITIES
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_DOT1X_AUTH_FAILED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_CIPHER_REJECTED_BY_POLICY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_SERVICE_CHANGE_PRECLUDES_TS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_QOS_UNSPECIFIED_REASON
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_QOS_INSUFFICIENT_BANDWIDTH
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_QOS_EXCESSIVE_NOT_ACK
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_QOS_TXOPLIMIT_EXCEEDED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_QSTA_LEAVING
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_END_TS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_END_DLS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_END_BA
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNKNOWN_TS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNKNOWN_BA
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNKNOWN_DLS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_STAKEY_MISMATCH
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNICAST_KEY_NEGOTIATION_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_MULTICAST_KEY_ANNOUNCEMENT_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INCOMPATIBLE_UNICAST_KEY_NEGOTIATION_IE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_MULTICAST_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_UNICAST_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_UNSUPPORTED_WAPI_IE_VERSION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_INVALID_WAPI_CAPABILITY_IE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_REASON_WAI_CERTIFICATE_AUTHENTICATION_FAILED
+                   - See IEEE 802.11 Standard
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeIEEE80211Reason;
+#define CSR_WIFI_SME_IEEE80211_REASON_SUCCESS                                   ((CsrWifiSmeIEEE80211Reason) 0x0000)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNSPECIFIED_REASON                        ((CsrWifiSmeIEEE80211Reason) 0x0001)
+#define CSR_WIFI_SME_IEEE80211_REASON_AUTHENTICATION_NOT_VALID                  ((CsrWifiSmeIEEE80211Reason) 0x0002)
+#define CSR_WIFI_SME_IEEE80211_REASON_DEAUTHENTICATED_LEAVE_BSS                 ((CsrWifiSmeIEEE80211Reason) 0x0003)
+#define CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_INACTIVITY                  ((CsrWifiSmeIEEE80211Reason) 0x0004)
+#define CSR_WIFI_SME_IEEE80211_REASON_AP_OVERLOAD                               ((CsrWifiSmeIEEE80211Reason) 0x0005)
+#define CSR_WIFI_SME_IEEE80211_REASON_CLASS_2FRAME_ERROR                        ((CsrWifiSmeIEEE80211Reason) 0x0006)
+#define CSR_WIFI_SME_IEEE80211_REASON_CLASS_3FRAME_ERROR                        ((CsrWifiSmeIEEE80211Reason) 0x0007)
+#define CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_LEAVE_BSS                   ((CsrWifiSmeIEEE80211Reason) 0x0008)
+#define CSR_WIFI_SME_IEEE80211_REASON_ASSOCIATION_NOT_AUTHENTICATED             ((CsrWifiSmeIEEE80211Reason) 0x0009)
+#define CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_POWER_CAPABILITY            ((CsrWifiSmeIEEE80211Reason) 0x000a)
+#define CSR_WIFI_SME_IEEE80211_REASON_DISASSOCIATED_SUPPORTED_CHANNELS          ((CsrWifiSmeIEEE80211Reason) 0x000b)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_INFORMATION_ELEMENT               ((CsrWifiSmeIEEE80211Reason) 0x000d)
+#define CSR_WIFI_SME_IEEE80211_REASON_MICHAEL_MIC_FAILURE                       ((CsrWifiSmeIEEE80211Reason) 0x000e)
+#define CSR_WIFI_SME_IEEE80211_REASON_FOURWAY_HANDSHAKE_TIMEOUT                 ((CsrWifiSmeIEEE80211Reason) 0x000f)
+#define CSR_WIFI_SME_IEEE80211_REASON_GROUP_KEY_UPDATE_TIMEOUT                  ((CsrWifiSmeIEEE80211Reason) 0x0010)
+#define CSR_WIFI_SME_IEEE80211_REASON_HANDSHAKE_ELEMENT_DIFFERENT               ((CsrWifiSmeIEEE80211Reason) 0x0011)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_GROUP_CIPHER                      ((CsrWifiSmeIEEE80211Reason) 0x0012)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_PAIRWISE_CIPHER                   ((CsrWifiSmeIEEE80211Reason) 0x0013)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_AKMP                              ((CsrWifiSmeIEEE80211Reason) 0x0014)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNSUPPORTED_RSN_IEVERSION                 ((CsrWifiSmeIEEE80211Reason) 0x0015)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_RSN_IECAPABILITIES                ((CsrWifiSmeIEEE80211Reason) 0x0016)
+#define CSR_WIFI_SME_IEEE80211_REASON_DOT1X_AUTH_FAILED                         ((CsrWifiSmeIEEE80211Reason) 0x0017)
+#define CSR_WIFI_SME_IEEE80211_REASON_CIPHER_REJECTED_BY_POLICY                 ((CsrWifiSmeIEEE80211Reason) 0x0018)
+#define CSR_WIFI_SME_IEEE80211_REASON_SERVICE_CHANGE_PRECLUDES_TS               ((CsrWifiSmeIEEE80211Reason) 0x001F)
+#define CSR_WIFI_SME_IEEE80211_REASON_QOS_UNSPECIFIED_REASON                    ((CsrWifiSmeIEEE80211Reason) 0x0020)
+#define CSR_WIFI_SME_IEEE80211_REASON_QOS_INSUFFICIENT_BANDWIDTH                ((CsrWifiSmeIEEE80211Reason) 0x0021)
+#define CSR_WIFI_SME_IEEE80211_REASON_QOS_EXCESSIVE_NOT_ACK                     ((CsrWifiSmeIEEE80211Reason) 0x0022)
+#define CSR_WIFI_SME_IEEE80211_REASON_QOS_TXOPLIMIT_EXCEEDED                    ((CsrWifiSmeIEEE80211Reason) 0x0023)
+#define CSR_WIFI_SME_IEEE80211_REASON_QSTA_LEAVING                              ((CsrWifiSmeIEEE80211Reason) 0x0024)
+#define CSR_WIFI_SME_IEEE80211_REASON_END_TS                                    ((CsrWifiSmeIEEE80211Reason) 0x0025)
+#define CSR_WIFI_SME_IEEE80211_REASON_END_DLS                                   ((CsrWifiSmeIEEE80211Reason) 0x0025)
+#define CSR_WIFI_SME_IEEE80211_REASON_END_BA                                    ((CsrWifiSmeIEEE80211Reason) 0x0025)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNKNOWN_TS                                ((CsrWifiSmeIEEE80211Reason) 0x0026)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNKNOWN_BA                                ((CsrWifiSmeIEEE80211Reason) 0x0026)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNKNOWN_DLS                               ((CsrWifiSmeIEEE80211Reason) 0x0026)
+#define CSR_WIFI_SME_IEEE80211_REASON_TIMEOUT                                   ((CsrWifiSmeIEEE80211Reason) 0x0027)
+#define CSR_WIFI_SME_IEEE80211_REASON_STAKEY_MISMATCH                           ((CsrWifiSmeIEEE80211Reason) 0x002d)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNICAST_KEY_NEGOTIATION_TIMEOUT           ((CsrWifiSmeIEEE80211Reason) 0xf019)
+#define CSR_WIFI_SME_IEEE80211_REASON_MULTICAST_KEY_ANNOUNCEMENT_TIMEOUT        ((CsrWifiSmeIEEE80211Reason) 0xf01a)
+#define CSR_WIFI_SME_IEEE80211_REASON_INCOMPATIBLE_UNICAST_KEY_NEGOTIATION_IE   ((CsrWifiSmeIEEE80211Reason) 0xf01b)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_MULTICAST_CIPHER                  ((CsrWifiSmeIEEE80211Reason) 0xf01c)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_UNICAST_CIPHER                    ((CsrWifiSmeIEEE80211Reason) 0xf01d)
+#define CSR_WIFI_SME_IEEE80211_REASON_UNSUPPORTED_WAPI_IE_VERSION               ((CsrWifiSmeIEEE80211Reason) 0xf01e)
+#define CSR_WIFI_SME_IEEE80211_REASON_INVALID_WAPI_CAPABILITY_IE                ((CsrWifiSmeIEEE80211Reason) 0xf01f)
+#define CSR_WIFI_SME_IEEE80211_REASON_WAI_CERTIFICATE_AUTHENTICATION_FAILED     ((CsrWifiSmeIEEE80211Reason) 0xf020)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeIEEE80211Result
+
+  DESCRIPTION
+    As definined in the IEEE 802.11 standards
+
+ VALUES
+    CSR_WIFI_SME_IEEE80211_RESULT_SUCCESS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_UNSPECIFIED_FAILURE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_CAPABILITIES_MISMATCH
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REASSOCIATION_DENIED_NO_ASSOCIATION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_EXTERNAL_REASON
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AUTHENTICATION_MISMATCH
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_INVALID_AUTHENTICATION_SEQUENCE_NUMBER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_CHALLENGE_FAILURE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AUTHENTICATION_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AP_OUT_OF_MEMORY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_BASIC_RATES_MISMATCH
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SHORT_PREAMBLE_REQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_PBCC_MODULATION_REQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_CHANNEL_AGILITY_REQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SPECTRUM_MANAGEMENT_REQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_POWER_CAPABILITY_UNACCEPTABLE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SUPPORTED_CHANNELS_UNACCEPTABLE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SHORT_SLOT_REQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_DSSS_OFDMREQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_NO_HT_SUPPORT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_R0KH_UNREACHABLE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_PCO_TRANSITION_SUPPORT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_ASSOCIATION_REQUEST_REJECTED_TEMPORARILY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_ROBUST_MANAGEMENT_FRAME_POLICY_VIOLATION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_FAILURE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AP_BANDWIDTH_INSUFFICIENT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_POOR_OPERATING_CHANNEL
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_QOS_REQUIRED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_REASON_UNSPECIFIED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_PARAMETERS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_WITH_SUGGESTED_TSPEC_CHANGES
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_IE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_GROUP_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_PAIRWISE_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_AKMP
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_UNSUPPORTED_RSN_VERSION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_RSN_CAPABILITY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_SECURITY_POLICY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_FOR_DELAY_PERIOD
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_NOT_ALLOWED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_NOT_PRESENT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_NOT_QSTA
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_LISTEN_INTERVAL_TOO_LARGE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_FT_ACTION_FRAME_COUNT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_PMKID
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_MDIE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_FTIE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_UNSPECIFIED_QOS_FAILURE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_WRONG_POLICY
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INSUFFICIENT_BANDWIDTH
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_TSPEC_PARAMETERS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_TIMEOUT
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_TOO_MANY_SIMULTANEOUS_REQUESTS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_BSS_ALREADY_STARTED_OR_JOINED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_NOT_SUPPORTED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_TRANSMISSION_FAILURE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_NOT_AUTHENTICATED
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_RESET_REQUIRED_BEFORE_START
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_LM_INFO_UNAVAILABLE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_UNICAST_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_MULTICAST_CIPHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_UNSUPPORTED_WAPI_IE_VERSION
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_IEEE80211_RESULT_INVALID_WAPI_CAPABILITY_IE
+                   - See IEEE 802.11 Standard
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeIEEE80211Result;
+#define CSR_WIFI_SME_IEEE80211_RESULT_SUCCESS                                          ((CsrWifiSmeIEEE80211Result) 0x0000)
+#define CSR_WIFI_SME_IEEE80211_RESULT_UNSPECIFIED_FAILURE                              ((CsrWifiSmeIEEE80211Result) 0x0001)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_CAPABILITIES_MISMATCH                    ((CsrWifiSmeIEEE80211Result) 0x000a)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REASSOCIATION_DENIED_NO_ASSOCIATION              ((CsrWifiSmeIEEE80211Result) 0x000b)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_EXTERNAL_REASON                          ((CsrWifiSmeIEEE80211Result) 0x000c)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AUTHENTICATION_MISMATCH                  ((CsrWifiSmeIEEE80211Result) 0x000d)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_INVALID_AUTHENTICATION_SEQUENCE_NUMBER   ((CsrWifiSmeIEEE80211Result) 0x000e)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_CHALLENGE_FAILURE                        ((CsrWifiSmeIEEE80211Result) 0x000f)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AUTHENTICATION_TIMEOUT                   ((CsrWifiSmeIEEE80211Result) 0x0010)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AP_OUT_OF_MEMORY                         ((CsrWifiSmeIEEE80211Result) 0x0011)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_BASIC_RATES_MISMATCH                     ((CsrWifiSmeIEEE80211Result) 0x0012)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SHORT_PREAMBLE_REQUIRED                  ((CsrWifiSmeIEEE80211Result) 0x0013)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_PBCC_MODULATION_REQUIRED                 ((CsrWifiSmeIEEE80211Result) 0x0014)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_CHANNEL_AGILITY_REQUIRED                 ((CsrWifiSmeIEEE80211Result) 0x0015)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SPECTRUM_MANAGEMENT_REQUIRED             ((CsrWifiSmeIEEE80211Result) 0x0016)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_POWER_CAPABILITY_UNACCEPTABLE            ((CsrWifiSmeIEEE80211Result) 0x0017)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SUPPORTED_CHANNELS_UNACCEPTABLE          ((CsrWifiSmeIEEE80211Result) 0x0018)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_SHORT_SLOT_REQUIRED                      ((CsrWifiSmeIEEE80211Result) 0x0019)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_DSSS_OFDMREQUIRED                        ((CsrWifiSmeIEEE80211Result) 0x001a)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_NO_HT_SUPPORT                            ((CsrWifiSmeIEEE80211Result) 0x001b)
+#define CSR_WIFI_SME_IEEE80211_RESULT_R0KH_UNREACHABLE                                 ((CsrWifiSmeIEEE80211Result) 0x001c)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_PCO_TRANSITION_SUPPORT                   ((CsrWifiSmeIEEE80211Result) 0x001d)
+#define CSR_WIFI_SME_IEEE80211_RESULT_ASSOCIATION_REQUEST_REJECTED_TEMPORARILY         ((CsrWifiSmeIEEE80211Result) 0x001e)
+#define CSR_WIFI_SME_IEEE80211_RESULT_ROBUST_MANAGEMENT_FRAME_POLICY_VIOLATION         ((CsrWifiSmeIEEE80211Result) 0x001f)
+#define CSR_WIFI_SME_IEEE80211_RESULT_FAILURE                                          ((CsrWifiSmeIEEE80211Result) 0x0020)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_AP_BANDWIDTH_INSUFFICIENT                ((CsrWifiSmeIEEE80211Result) 0x0021)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_POOR_OPERATING_CHANNEL                   ((CsrWifiSmeIEEE80211Result) 0x0022)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_QOS_REQUIRED                             ((CsrWifiSmeIEEE80211Result) 0x0023)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_REASON_UNSPECIFIED                       ((CsrWifiSmeIEEE80211Result) 0x0025)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED                                          ((CsrWifiSmeIEEE80211Result) 0x0025)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_PARAMETERS                               ((CsrWifiSmeIEEE80211Result) 0x0026)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_WITH_SUGGESTED_TSPEC_CHANGES            ((CsrWifiSmeIEEE80211Result) 0x0027)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_IE                              ((CsrWifiSmeIEEE80211Result) 0x0028)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_GROUP_CIPHER                    ((CsrWifiSmeIEEE80211Result) 0x0029)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_PAIRWISE_CIPHER                 ((CsrWifiSmeIEEE80211Result) 0x002a)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_AKMP                            ((CsrWifiSmeIEEE80211Result) 0x002b)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_UNSUPPORTED_RSN_VERSION                 ((CsrWifiSmeIEEE80211Result) 0x002c)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_INVALID_RSN_CAPABILITY                  ((CsrWifiSmeIEEE80211Result) 0x002d)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_SECURITY_POLICY                         ((CsrWifiSmeIEEE80211Result) 0x002e)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_FOR_DELAY_PERIOD                        ((CsrWifiSmeIEEE80211Result) 0x002f)
+#define CSR_WIFI_SME_IEEE80211_RESULT_NOT_ALLOWED                                      ((CsrWifiSmeIEEE80211Result) 0x0030)
+#define CSR_WIFI_SME_IEEE80211_RESULT_NOT_PRESENT                                      ((CsrWifiSmeIEEE80211Result) 0x0031)
+#define CSR_WIFI_SME_IEEE80211_RESULT_NOT_QSTA                                         ((CsrWifiSmeIEEE80211Result) 0x0032)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REJECTED_LISTEN_INTERVAL_TOO_LARGE               ((CsrWifiSmeIEEE80211Result) 0x0033)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_FT_ACTION_FRAME_COUNT                    ((CsrWifiSmeIEEE80211Result) 0x0034)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_PMKID                                    ((CsrWifiSmeIEEE80211Result) 0x0035)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_MDIE                                     ((CsrWifiSmeIEEE80211Result) 0x0036)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_FTIE                                     ((CsrWifiSmeIEEE80211Result) 0x0037)
+#define CSR_WIFI_SME_IEEE80211_RESULT_UNSPECIFIED_QOS_FAILURE                          ((CsrWifiSmeIEEE80211Result) 0x00c8)
+#define CSR_WIFI_SME_IEEE80211_RESULT_WRONG_POLICY                                     ((CsrWifiSmeIEEE80211Result) 0x00c9)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INSUFFICIENT_BANDWIDTH                           ((CsrWifiSmeIEEE80211Result) 0x00ca)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_TSPEC_PARAMETERS                         ((CsrWifiSmeIEEE80211Result) 0x00cb)
+#define CSR_WIFI_SME_IEEE80211_RESULT_TIMEOUT                                          ((CsrWifiSmeIEEE80211Result) 0x8000)
+#define CSR_WIFI_SME_IEEE80211_RESULT_TOO_MANY_SIMULTANEOUS_REQUESTS                   ((CsrWifiSmeIEEE80211Result) 0x8001)
+#define CSR_WIFI_SME_IEEE80211_RESULT_BSS_ALREADY_STARTED_OR_JOINED                    ((CsrWifiSmeIEEE80211Result) 0x8002)
+#define CSR_WIFI_SME_IEEE80211_RESULT_NOT_SUPPORTED                                    ((CsrWifiSmeIEEE80211Result) 0x8003)
+#define CSR_WIFI_SME_IEEE80211_RESULT_TRANSMISSION_FAILURE                             ((CsrWifiSmeIEEE80211Result) 0x8004)
+#define CSR_WIFI_SME_IEEE80211_RESULT_REFUSED_NOT_AUTHENTICATED                        ((CsrWifiSmeIEEE80211Result) 0x8005)
+#define CSR_WIFI_SME_IEEE80211_RESULT_RESET_REQUIRED_BEFORE_START                      ((CsrWifiSmeIEEE80211Result) 0x8006)
+#define CSR_WIFI_SME_IEEE80211_RESULT_LM_INFO_UNAVAILABLE                              ((CsrWifiSmeIEEE80211Result) 0x8007)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_UNICAST_CIPHER                           ((CsrWifiSmeIEEE80211Result) 0xf02f)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_MULTICAST_CIPHER                         ((CsrWifiSmeIEEE80211Result) 0xf030)
+#define CSR_WIFI_SME_IEEE80211_RESULT_UNSUPPORTED_WAPI_IE_VERSION                      ((CsrWifiSmeIEEE80211Result) 0xf031)
+#define CSR_WIFI_SME_IEEE80211_RESULT_INVALID_WAPI_CAPABILITY_IE                       ((CsrWifiSmeIEEE80211Result) 0xf032)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeIndications
+
+  DESCRIPTION
+    Defines bits for CsrWifiSmeIndicationsMask
+
+ VALUES
+    CSR_WIFI_SME_INDICATIONS_NONE
+                   - Used to cancel the registrations for receiving indications
+    CSR_WIFI_SME_INDICATIONS_WIFIOFF
+                   - Used to register for CSR_WIFI_SME_WIFI_OFF_IND events
+    CSR_WIFI_SME_INDICATIONS_SCANRESULT
+                   - Used to register for CSR_WIFI_SME_SCAN_RESULT_IND events
+    CSR_WIFI_SME_INDICATIONS_CONNECTIONQUALITY
+                   - Used to register for CSR_WIFI_SME_CONNECTION_QUALITY_IND
+                     events
+    CSR_WIFI_SME_INDICATIONS_MEDIASTATUS
+                   - Used to register for CSR_WIFI_SME_MEDIA_STATUS_IND events
+    CSR_WIFI_SME_INDICATIONS_MICFAILURE
+                   - Used to register for CSR_WIFI_SME_MICFAILURE_IND events
+    CSR_WIFI_SME_INDICATIONS_PMKIDCANDIDATELIST
+                   - Used to register for CSR_WIFI_SME_PMKIDCANDIDATE_LIST_IND
+                     events
+    CSR_WIFI_SME_INDICATIONS_TSPEC
+                   - Used to register for CSR_WIFI_SME_TSPEC_IND events
+    CSR_WIFI_SME_INDICATIONS_ROAMSTART
+                   - Used to register for CSR_WIFI_SME_ROAM_START_IND events
+    CSR_WIFI_SME_INDICATIONS_ROAMCOMPLETE
+                   - Used to register for CSR_WIFI_SME_ROAM_COMPLETE_IND events
+    CSR_WIFI_SME_INDICATIONS_ASSOCIATIONSTART
+                   - Used to register for CSR_WIFI_SME_ASSOCIATION_START_IND
+                     events
+    CSR_WIFI_SME_INDICATIONS_ASSOCIATIONCOMPLETE
+                   - Used to register for CSR_WIFI_SME_ASSOCIATION_COMPLETE_IND
+                     events
+    CSR_WIFI_SME_INDICATIONS_IBSSSTATION
+                   - Used to register for CSR_WIFI_SME_IBSS_STATION_IND events
+    CSR_WIFI_SME_INDICATIONS_WIFION
+                   - Used to register for CSR_WIFI_SME_WIFI_ON_IND events
+    CSR_WIFI_SME_INDICATIONS_ERROR
+                   - Used to register for CSR_WIFI_SME_ERROR_IND events
+    CSR_WIFI_SME_INDICATIONS_INFO
+                   - Used to register for CSR_WIFI_SME_INFO_IND events
+    CSR_WIFI_SME_INDICATIONS_COREDUMP
+                   - Used to register for CSR_WIFI_SME_CORE_DUMP_IND events
+    CSR_WIFI_SME_INDICATIONS_ALL
+                   - Used to register for all available indications
+
+*******************************************************************************/
+typedef u32 CsrWifiSmeIndications;
+#define CSR_WIFI_SME_INDICATIONS_NONE                  ((CsrWifiSmeIndications) 0x00000000)
+#define CSR_WIFI_SME_INDICATIONS_WIFIOFF               ((CsrWifiSmeIndications) 0x00000001)
+#define CSR_WIFI_SME_INDICATIONS_SCANRESULT            ((CsrWifiSmeIndications) 0x00000002)
+#define CSR_WIFI_SME_INDICATIONS_CONNECTIONQUALITY     ((CsrWifiSmeIndications) 0x00000004)
+#define CSR_WIFI_SME_INDICATIONS_MEDIASTATUS           ((CsrWifiSmeIndications) 0x00000008)
+#define CSR_WIFI_SME_INDICATIONS_MICFAILURE            ((CsrWifiSmeIndications) 0x00000010)
+#define CSR_WIFI_SME_INDICATIONS_PMKIDCANDIDATELIST    ((CsrWifiSmeIndications) 0x00000020)
+#define CSR_WIFI_SME_INDICATIONS_TSPEC                 ((CsrWifiSmeIndications) 0x00000040)
+#define CSR_WIFI_SME_INDICATIONS_ROAMSTART             ((CsrWifiSmeIndications) 0x00000080)
+#define CSR_WIFI_SME_INDICATIONS_ROAMCOMPLETE          ((CsrWifiSmeIndications) 0x00000100)
+#define CSR_WIFI_SME_INDICATIONS_ASSOCIATIONSTART      ((CsrWifiSmeIndications) 0x00000200)
+#define CSR_WIFI_SME_INDICATIONS_ASSOCIATIONCOMPLETE   ((CsrWifiSmeIndications) 0x00000400)
+#define CSR_WIFI_SME_INDICATIONS_IBSSSTATION           ((CsrWifiSmeIndications) 0x00000800)
+#define CSR_WIFI_SME_INDICATIONS_WIFION                ((CsrWifiSmeIndications) 0x00001000)
+#define CSR_WIFI_SME_INDICATIONS_ERROR                 ((CsrWifiSmeIndications) 0x00002000)
+#define CSR_WIFI_SME_INDICATIONS_INFO                  ((CsrWifiSmeIndications) 0x00004000)
+#define CSR_WIFI_SME_INDICATIONS_COREDUMP              ((CsrWifiSmeIndications) 0x00008000)
+#define CSR_WIFI_SME_INDICATIONS_ALL                   ((CsrWifiSmeIndications) 0xFFFFFFFF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeKeyType
+
+  DESCRIPTION
+    Indicates the type of the key
+
+ VALUES
+    CSR_WIFI_SME_KEY_TYPE_GROUP    - Key for broadcast communication
+    CSR_WIFI_SME_KEY_TYPE_PAIRWISE - Key for unicast communication
+    CSR_WIFI_SME_KEY_TYPE_STAKEY   - Key for direct link communication to
+                                     another station in infrastructure networks
+    CSR_WIFI_SME_KEY_TYPE_IGTK     - Integrity Group Temporal Key
+    CSR_WIFI_SME_KEY_TYPE_CCKM     - Key for Cisco Centralized Key Management
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeKeyType;
+#define CSR_WIFI_SME_KEY_TYPE_GROUP      ((CsrWifiSmeKeyType) 0x00)
+#define CSR_WIFI_SME_KEY_TYPE_PAIRWISE   ((CsrWifiSmeKeyType) 0x01)
+#define CSR_WIFI_SME_KEY_TYPE_STAKEY     ((CsrWifiSmeKeyType) 0x02)
+#define CSR_WIFI_SME_KEY_TYPE_IGTK       ((CsrWifiSmeKeyType) 0x03)
+#define CSR_WIFI_SME_KEY_TYPE_CCKM       ((CsrWifiSmeKeyType) 0x04)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeListAction
+
+  DESCRIPTION
+    Identifies the type of action to be performed on a list of items
+    The values of this type are used across the NME/SME/Router API's and they
+    must be kept consistent with the corresponding types in the .xml of the
+    ottherinterfaces
+
+ VALUES
+    CSR_WIFI_SME_LIST_ACTION_GET    - Retrieve the current list of items
+    CSR_WIFI_SME_LIST_ACTION_ADD    - Add one or more items
+    CSR_WIFI_SME_LIST_ACTION_REMOVE - Remove one or more items
+    CSR_WIFI_SME_LIST_ACTION_FLUSH  - Remove all items
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeListAction;
+#define CSR_WIFI_SME_LIST_ACTION_GET      ((CsrWifiSmeListAction) 0x00)
+#define CSR_WIFI_SME_LIST_ACTION_ADD      ((CsrWifiSmeListAction) 0x01)
+#define CSR_WIFI_SME_LIST_ACTION_REMOVE   ((CsrWifiSmeListAction) 0x02)
+#define CSR_WIFI_SME_LIST_ACTION_FLUSH    ((CsrWifiSmeListAction) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMediaStatus
+
+  DESCRIPTION
+    Indicates the connection status
+    The values of this type are used across the NME/SME/Router API's and they
+    must be kept consistent with the corresponding types in the .xml of the
+    ottherinterfaces
+
+ VALUES
+    CSR_WIFI_SME_MEDIA_STATUS_CONNECTED
+                   - Value CSR_WIFI_SME_MEDIA_STATUS_CONNECTED can happen in two
+                     situations:
+                       * A network connection is established. Specifically, this is
+                         when the MLME_ASSOCIATION completes or the first peer
+                         relationship is established in an IBSS. In a WPA/WPA2
+                         network, this indicates that the stack is ready to perform
+                         the 4-way handshake or 802.1x authentication if CSR NME
+                         security library is not used. If CSR NME security library
+                         is used this indicates, completion of 4way handshake or
+                         802.1x authentication
+                       * The SME roams to another AP on the same ESS
+                     During the AP operation, it indicates that the peer station
+                     is connected to the AP and is ready for data transfer.
+    CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED
+                   - Value CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED can happen in
+                     two situations:
+                       * when the connection to a network is lost and there is no
+                         alternative on the same ESS to roam to
+                       * when a CSR_WIFI_SME_DISCONNECT_REQ request is issued
+                     During AP or P2PGO operation, it indicates that the peer
+                     station has disconnected from the AP
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeMediaStatus;
+#define CSR_WIFI_SME_MEDIA_STATUS_CONNECTED      ((CsrWifiSmeMediaStatus) 0x00)
+#define CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED   ((CsrWifiSmeMediaStatus) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pCapability
+
+  DESCRIPTION
+    Defines P2P Device Capabilities
+
+ VALUES
+    CSR_WIFI_SME_P2P_SERVICE_DISCOVERY_CAPABILITY
+                   - This field is set to 1 if the P2P Device supports Service
+                     Discovery, and to 0 otherwise
+    CSR_WIFI_SME_P2P_CLIENT_DISCOVERABILITY_CAPABILITY
+                   - This field is set to 1 when the P2P Device supports P2P
+                     Client Discoverability, and to 0 otherwise.
+    CSR_WIFI_SME_P2P_CONCURRENT_OPERATION_CAPABILITY
+                   - This field is set to 1 when the P2P Device supports
+                     Concurrent Operation with WLAN, and to 0 otherwise.
+    CSR_WIFI_SME_P2P_MANAGED_DEVICE_CAPABILITY
+                   - This field is set to 1 when the P2P interface of the P2P
+                     Device is capable of being managed by the WLAN
+                     (infrastructure network) based on P2P coexistence
+                     parameters, and to 0 otherwise
+    CSR_WIFI_SME_P2P_INVITAION_CAPABILITY
+                   - This field is set to 1 if the P2P Device is capable of
+                     processing P2P Invitation Procedure signaling, and to 0
+                     otherwise.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pCapability;
+#define CSR_WIFI_SME_P2P_SERVICE_DISCOVERY_CAPABILITY        ((CsrWifiSmeP2pCapability) 0x01)
+#define CSR_WIFI_SME_P2P_CLIENT_DISCOVERABILITY_CAPABILITY   ((CsrWifiSmeP2pCapability) 0x02)
+#define CSR_WIFI_SME_P2P_CONCURRENT_OPERATION_CAPABILITY     ((CsrWifiSmeP2pCapability) 0x04)
+#define CSR_WIFI_SME_P2P_MANAGED_DEVICE_CAPABILITY           ((CsrWifiSmeP2pCapability) 0x08)
+#define CSR_WIFI_SME_P2P_INVITAION_CAPABILITY                ((CsrWifiSmeP2pCapability) 0x20)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pGroupCapability
+
+  DESCRIPTION
+    Define bits for P2P Group Capability
+
+ VALUES
+    CSR_WIFI_P2P_GRP_CAP_GO
+                   - Indicates if the local device has become a GO after GO
+                     negotiation
+    CSR_WIFI_P2P_GRP_CAP_PERSISTENT
+                   - Persistent group
+    CSR_WIFI_P2P_GRP_CAP_INTRABSS_DIST
+                   - Intra-BSS data distribution support
+    CSR_WIFI_P2P_GRP_CAP_CROSS_CONN
+                   - Support of cross connection
+    CSR_WIFI_P2P_GRP_CAP_PERSISTENT_RECONNECT
+                   - Support of persistent reconnect
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pGroupCapability;
+#define CSR_WIFI_P2P_GRP_CAP_GO                     ((CsrWifiSmeP2pGroupCapability) 0x01)
+#define CSR_WIFI_P2P_GRP_CAP_PERSISTENT             ((CsrWifiSmeP2pGroupCapability) 0x02)
+#define CSR_WIFI_P2P_GRP_CAP_INTRABSS_DIST          ((CsrWifiSmeP2pGroupCapability) 0x08)
+#define CSR_WIFI_P2P_GRP_CAP_CROSS_CONN             ((CsrWifiSmeP2pGroupCapability) 0x10)
+#define CSR_WIFI_P2P_GRP_CAP_PERSISTENT_RECONNECT   ((CsrWifiSmeP2pGroupCapability) 0x20)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pNoaConfigMethod
+
+  DESCRIPTION
+    Notice of Absece Configuration
+
+ VALUES
+    CSR_WIFI_P2P_NOA_NONE         - Do not use NOA
+    CSR_WIFI_P2P_NOA_AUTONOMOUS   - NOA based on the traffic analysis
+    CSR_WIFI_P2P_NOA_USER_DEFINED - NOA as specified by the user
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pNoaConfigMethod;
+#define CSR_WIFI_P2P_NOA_NONE           ((CsrWifiSmeP2pNoaConfigMethod) 0x00)
+#define CSR_WIFI_P2P_NOA_AUTONOMOUS     ((CsrWifiSmeP2pNoaConfigMethod) 0x01)
+#define CSR_WIFI_P2P_NOA_USER_DEFINED   ((CsrWifiSmeP2pNoaConfigMethod) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pRole
+
+  DESCRIPTION
+    Definition of roles for a P2P Device
+
+ VALUES
+    CSR_WIFI_SME_P2P_ROLE_NONE       - A non-P2PDevice device
+    CSR_WIFI_SME_P2P_ROLE_STANDALONE - A Standalone P2P device
+    CSR_WIFI_SME_P2P_ROLE_GO         - Role Assumed is that of a Group Owner
+                                       within a P2P Group
+    CSR_WIFI_SME_P2P_ROLE_CLI        - Role Assumed is that of a P2P Client
+                                       within a P2P Group
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pRole;
+#define CSR_WIFI_SME_P2P_ROLE_NONE         ((CsrWifiSmeP2pRole) 0x00)
+#define CSR_WIFI_SME_P2P_ROLE_STANDALONE   ((CsrWifiSmeP2pRole) 0x01)
+#define CSR_WIFI_SME_P2P_ROLE_GO           ((CsrWifiSmeP2pRole) 0x02)
+#define CSR_WIFI_SME_P2P_ROLE_CLI          ((CsrWifiSmeP2pRole) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pStatus
+
+  DESCRIPTION
+    This data type enumerates the outcome of P2P procedure
+
+ VALUES
+    CSR_WIFI_SME_P2P_STATUS_SUCCESS
+                   - Success
+    CSR_WIFI_SME_P2P_STATUS_FAIL_INFO_UNAVAILABLE
+                   - Fail; information is currently unavailable
+    CSR_WIFI_SME_P2P_STATUS_FAIL_INCOMPATIBLE_PARAM
+                   - Fail; incompatible parameters
+    CSR_WIFI_SME_P2P_STATUS_FAIL_LIMIT_REACHED
+                   - Fail; limit reached
+    CSR_WIFI_SME_P2P_STATUS_FAIL_INVALID_PARAM
+                   - Fail; invalid parameters
+    CSR_WIFI_SME_P2P_STATUS_FAIL_ACCOMODATE
+                   - Fail; unable to accommodate request
+    CSR_WIFI_SME_P2P_STATUS_FAIL_PREV_ERROR
+                   - Fail; previous protocol error, or disruptive behavior
+    CSR_WIFI_SME_P2P_STATUS_FAIL_COMMON_CHANNELS
+                   - Fail; no common channels
+    CSR_WIFI_SME_P2P_STATUS_FAIL_UNKNOWN_GROUP
+                   - Fail; unknown P2P Group
+    CSR_WIFI_SME_P2P_STATUS_FAIL_GO_INTENT
+                   - Fail: both P2P Devices indicated an Intent of 15 in Group
+                     Owner Negotiation
+    CSR_WIFI_SME_P2P_STATUS_FAIL_PROVISION_METHOD_INCOMPATIBLE
+                   - Fail; incompatible provisioning method
+    CSR_WIFI_SME_P2P_STATUS_FAIL_REJECT
+                   - Fail: rejected by user
+    CSR_WIFI_SME_P2P_STATUS_FAIL_RESERVED
+                   - Fail; Status Reserved
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pStatus;
+#define CSR_WIFI_SME_P2P_STATUS_SUCCESS                              ((CsrWifiSmeP2pStatus) 0x00)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_INFO_UNAVAILABLE                ((CsrWifiSmeP2pStatus) 0x01)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_INCOMPATIBLE_PARAM              ((CsrWifiSmeP2pStatus) 0x02)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_LIMIT_REACHED                   ((CsrWifiSmeP2pStatus) 0x03)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_INVALID_PARAM                   ((CsrWifiSmeP2pStatus) 0x04)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_ACCOMODATE                      ((CsrWifiSmeP2pStatus) 0x05)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_PREV_ERROR                      ((CsrWifiSmeP2pStatus) 0x06)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_COMMON_CHANNELS                 ((CsrWifiSmeP2pStatus) 0x07)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_UNKNOWN_GROUP                   ((CsrWifiSmeP2pStatus) 0x08)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_GO_INTENT                       ((CsrWifiSmeP2pStatus) 0x09)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_PROVISION_METHOD_INCOMPATIBLE   ((CsrWifiSmeP2pStatus) 0x0A)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_REJECT                          ((CsrWifiSmeP2pStatus) 0x0B)
+#define CSR_WIFI_SME_P2P_STATUS_FAIL_RESERVED                        ((CsrWifiSmeP2pStatus) 0xFF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePacketFilterMode
+
+  DESCRIPTION
+    Options for the filter mode parameter.
+    The Values here match the HIP interface
+
+ VALUES
+    CSR_WIFI_SME_PACKET_FILTER_MODE_OPT_OUT
+                   - Broadcast packets are always reported to the host unless
+                     they match at least one of the specified TCLAS IEs.
+    CSR_WIFI_SME_PACKET_FILTER_MODE_OPT_IN
+                   - Broadcast packets are reported to the host only if they
+                     match at least one of the specified TCLAS IEs.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmePacketFilterMode;
+#define CSR_WIFI_SME_PACKET_FILTER_MODE_OPT_OUT   ((CsrWifiSmePacketFilterMode) 0x00)
+#define CSR_WIFI_SME_PACKET_FILTER_MODE_OPT_IN    ((CsrWifiSmePacketFilterMode) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerSaveLevel
+
+  DESCRIPTION
+    Power Save Level options as defined in the IEEE 802.11 standards
+    First 3 values are set to match the mlme PowerManagementMode
+
+ VALUES
+    CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW  - No power save: the driver will remain
+                                         active at all times
+    CSR_WIFI_SME_POWER_SAVE_LEVEL_HIGH - Enter power save after all packets in
+                                         the queues are transmitted and received
+    CSR_WIFI_SME_POWER_SAVE_LEVEL_MED  - Enter power save after all packets in
+                                         the queues are transmitted and received
+                                         and a further configurable delay
+                                         (default 1s) has elapsed
+    CSR_WIFI_SME_POWER_SAVE_LEVEL_AUTO - The SME will decide when to enter power
+                                         save mode according to the traffic
+                                         analysis
+
+*******************************************************************************/
+typedef u8 CsrWifiSmePowerSaveLevel;
+#define CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW    ((CsrWifiSmePowerSaveLevel) 0x00)
+#define CSR_WIFI_SME_POWER_SAVE_LEVEL_HIGH   ((CsrWifiSmePowerSaveLevel) 0x01)
+#define CSR_WIFI_SME_POWER_SAVE_LEVEL_MED    ((CsrWifiSmePowerSaveLevel) 0x02)
+#define CSR_WIFI_SME_POWER_SAVE_LEVEL_AUTO   ((CsrWifiSmePowerSaveLevel) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePreambleType
+
+  DESCRIPTION
+    SME Preamble Types
+
+ VALUES
+    CSR_WIFI_SME_USE_LONG_PREAMBLE  - Use legacy (long) preamble
+    CSR_WIFI_SME_USE_SHORT_PREAMBLE - Use short PPDU format
+
+*******************************************************************************/
+typedef u8 CsrWifiSmePreambleType;
+#define CSR_WIFI_SME_USE_LONG_PREAMBLE    ((CsrWifiSmePreambleType) 0x00)
+#define CSR_WIFI_SME_USE_SHORT_PREAMBLE   ((CsrWifiSmePreambleType) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRadioIF
+
+  DESCRIPTION
+    Indicates the frequency
+
+ VALUES
+    CSR_WIFI_SME_RADIO_IF_GHZ_2_4 - Indicates the 2.4 GHZ frequency
+    CSR_WIFI_SME_RADIO_IF_GHZ_5_0 - Future use: currently not supported
+    CSR_WIFI_SME_RADIO_IF_BOTH    - Future use: currently not supported
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeRadioIF;
+#define CSR_WIFI_SME_RADIO_IF_GHZ_2_4   ((CsrWifiSmeRadioIF) 0x01)
+#define CSR_WIFI_SME_RADIO_IF_GHZ_5_0   ((CsrWifiSmeRadioIF) 0x02)
+#define CSR_WIFI_SME_RADIO_IF_BOTH      ((CsrWifiSmeRadioIF) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRegulatoryDomain
+
+  DESCRIPTION
+    Indicates the regulatory domain as defined in IEEE 802.11 standards
+
+ VALUES
+    CSR_WIFI_SME_REGULATORY_DOMAIN_OTHER
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_FCC
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_IC
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_ETSI
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_SPAIN
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_FRANCE
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_JAPAN
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_JAPANBIS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_CHINA
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_CHINABIS
+                   - See IEEE 802.11 Standard
+    CSR_WIFI_SME_REGULATORY_DOMAIN_NONE
+                   - See IEEE 802.11 Standard
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeRegulatoryDomain;
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_OTHER      ((CsrWifiSmeRegulatoryDomain) 0x00)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_FCC        ((CsrWifiSmeRegulatoryDomain) 0x10)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_IC         ((CsrWifiSmeRegulatoryDomain) 0x20)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_ETSI       ((CsrWifiSmeRegulatoryDomain) 0x30)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_SPAIN      ((CsrWifiSmeRegulatoryDomain) 0x31)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_FRANCE     ((CsrWifiSmeRegulatoryDomain) 0x32)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_JAPAN      ((CsrWifiSmeRegulatoryDomain) 0x40)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_JAPANBIS   ((CsrWifiSmeRegulatoryDomain) 0x41)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_CHINA      ((CsrWifiSmeRegulatoryDomain) 0x50)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_CHINABIS   ((CsrWifiSmeRegulatoryDomain) 0x51)
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_NONE       ((CsrWifiSmeRegulatoryDomain) 0xFF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamReason
+
+  DESCRIPTION
+    Indicates the reason for roaming
+
+ VALUES
+    CSR_WIFI_SME_ROAM_REASON_BEACONLOST
+                   - The station cannot receive the beacon signal any more
+    CSR_WIFI_SME_ROAM_REASON_DISASSOCIATED
+                   - The station has been disassociated
+    CSR_WIFI_SME_ROAM_REASON_DEAUTHENTICATED
+                   - The station has been deauthenticated
+    CSR_WIFI_SME_ROAM_REASON_BETTERAPFOUND
+                   - A better AP has been found
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeRoamReason;
+#define CSR_WIFI_SME_ROAM_REASON_BEACONLOST        ((CsrWifiSmeRoamReason) 0x00)
+#define CSR_WIFI_SME_ROAM_REASON_DISASSOCIATED     ((CsrWifiSmeRoamReason) 0x01)
+#define CSR_WIFI_SME_ROAM_REASON_DEAUTHENTICATED   ((CsrWifiSmeRoamReason) 0x02)
+#define CSR_WIFI_SME_ROAM_REASON_BETTERAPFOUND     ((CsrWifiSmeRoamReason) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanType
+
+  DESCRIPTION
+    Identifies the type of scan to be performed
+
+ VALUES
+    CSR_WIFI_SME_SCAN_TYPE_ALL     - Scan actively or passively according to the
+                                     regulatory domain restrictions
+    CSR_WIFI_SME_SCAN_TYPE_ACTIVE  - Scan actively only: send probes and listen
+                                     for answers
+    CSR_WIFI_SME_SCAN_TYPE_PASSIVE - Scan passively only: listen for beacon
+                                     messages
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeScanType;
+#define CSR_WIFI_SME_SCAN_TYPE_ALL       ((CsrWifiSmeScanType) 0x00)
+#define CSR_WIFI_SME_SCAN_TYPE_ACTIVE    ((CsrWifiSmeScanType) 0x01)
+#define CSR_WIFI_SME_SCAN_TYPE_PASSIVE   ((CsrWifiSmeScanType) 0x02)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTrafficType
+
+  DESCRIPTION
+    Identifies the type of traffic going on on the connection.
+    The values of this type are used across the NME/SME/Router API's and they
+    must be kept consistent with the corresponding types in the .xml of the
+    ottherinterfaces
+
+ VALUES
+    CSR_WIFI_SME_TRAFFIC_TYPE_OCCASIONAL
+                   - During the last 30 seconds there were fewer than 20 packets
+                     per seconds in each second in both directions
+    CSR_WIFI_SME_TRAFFIC_TYPE_BURSTY
+                   - During the last 30 seconds there was at least one second
+                     during which more than 20 packets were received in either
+                     direction
+    CSR_WIFI_SME_TRAFFIC_TYPE_PERIODIC
+                   - During the last 5 seconds there were at least 10 packets
+                     received each second and a defined period for the traffic
+                     can be recognized
+    CSR_WIFI_SME_TRAFFIC_TYPE_CONTINUOUS
+                   - During the last 5 seconds there were at least 20 packets
+                     received each second in either direction
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeTrafficType;
+#define CSR_WIFI_SME_TRAFFIC_TYPE_OCCASIONAL   ((CsrWifiSmeTrafficType) 0x00)
+#define CSR_WIFI_SME_TRAFFIC_TYPE_BURSTY       ((CsrWifiSmeTrafficType) 0x01)
+#define CSR_WIFI_SME_TRAFFIC_TYPE_PERIODIC     ((CsrWifiSmeTrafficType) 0x02)
+#define CSR_WIFI_SME_TRAFFIC_TYPE_CONTINUOUS   ((CsrWifiSmeTrafficType) 0x03)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecCtrl
+
+  DESCRIPTION
+    Defines bits for CsrWifiSmeTspecCtrlMask for additional CCX configuration.
+    CURRENTLY NOT SUPPORTED.
+
+ VALUES
+    CSR_WIFI_SME_TSPEC_CTRL_STRICT
+                   - No automatic negotiation
+    CSR_WIFI_SME_TSPEC_CTRL_CCX_SIGNALLING
+                   - Signalling TSPEC
+    CSR_WIFI_SME_TSPEC_CTRL_CCX_VOICE
+                   - Voice traffic TSPEC
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeTspecCtrl;
+#define CSR_WIFI_SME_TSPEC_CTRL_STRICT           ((CsrWifiSmeTspecCtrl) 0x01)
+#define CSR_WIFI_SME_TSPEC_CTRL_CCX_SIGNALLING   ((CsrWifiSmeTspecCtrl) 0x02)
+#define CSR_WIFI_SME_TSPEC_CTRL_CCX_VOICE        ((CsrWifiSmeTspecCtrl) 0x04)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecResultCode
+
+  DESCRIPTION
+    Defines the result of the TSPEC exchanges with the AP
+
+ VALUES
+    CSR_WIFI_SME_TSPEC_RESULT_SUCCESS
+                   - TSPEC command has been processed correctly
+    CSR_WIFI_SME_TSPEC_RESULT_UNSPECIFIED_QOS_FAILURE
+                   - The Access Point reported a failure
+    CSR_WIFI_SME_TSPEC_RESULT_FAILURE
+                   - Internal failure in the SME
+    CSR_WIFI_SME_TSPEC_RESULT_INVALID_TSPEC_PARAMETERS
+                   - The TSPEC parameters are invalid
+    CSR_WIFI_SME_TSPEC_RESULT_INVALID_TCLAS_PARAMETERS
+                   - The TCLASS parameters are invalid
+    CSR_WIFI_SME_TSPEC_RESULT_INSUFFICIENT_BANDWIDTH
+                   - As specified by the WMM Spec
+    CSR_WIFI_SME_TSPEC_RESULT_WRONG_POLICY
+                   - As specified by the WMM Spec
+    CSR_WIFI_SME_TSPEC_RESULT_REJECTED_WITH_SUGGESTED_CHANGES
+                   - As specified by the WMM Spec
+    CSR_WIFI_SME_TSPEC_RESULT_TIMEOUT
+                   - The TSPEC negotiation timed out
+    CSR_WIFI_SME_TSPEC_RESULT_NOT_SUPPORTED
+                   - The Access Point does not support the TSPEC
+    CSR_WIFI_SME_TSPEC_RESULT_IE_LENGTH_INCORRECT
+                   - The length of the TSPEC is not correct
+    CSR_WIFI_SME_TSPEC_RESULT_INVALID_TRANSACTION_ID
+                   - The TSPEC transaction id is not in the list
+    CSR_WIFI_SME_TSPEC_RESULT_INSTALLED
+                   - The TSPEC has been installed and it is now active.
+    CSR_WIFI_SME_TSPEC_RESULT_TID_ALREADY_INSTALLED
+                   - The Traffic ID has already been installed
+    CSR_WIFI_SME_TSPEC_RESULT_TSPEC_REMOTELY_DELETED
+                   - The AP has deleted the TSPEC
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeTspecResultCode;
+#define CSR_WIFI_SME_TSPEC_RESULT_SUCCESS                           ((CsrWifiSmeTspecResultCode) 0x00)
+#define CSR_WIFI_SME_TSPEC_RESULT_UNSPECIFIED_QOS_FAILURE           ((CsrWifiSmeTspecResultCode) 0x01)
+#define CSR_WIFI_SME_TSPEC_RESULT_FAILURE                           ((CsrWifiSmeTspecResultCode) 0x02)
+#define CSR_WIFI_SME_TSPEC_RESULT_INVALID_TSPEC_PARAMETERS          ((CsrWifiSmeTspecResultCode) 0x05)
+#define CSR_WIFI_SME_TSPEC_RESULT_INVALID_TCLAS_PARAMETERS          ((CsrWifiSmeTspecResultCode) 0x06)
+#define CSR_WIFI_SME_TSPEC_RESULT_INSUFFICIENT_BANDWIDTH            ((CsrWifiSmeTspecResultCode) 0x07)
+#define CSR_WIFI_SME_TSPEC_RESULT_WRONG_POLICY                      ((CsrWifiSmeTspecResultCode) 0x08)
+#define CSR_WIFI_SME_TSPEC_RESULT_REJECTED_WITH_SUGGESTED_CHANGES   ((CsrWifiSmeTspecResultCode) 0x09)
+#define CSR_WIFI_SME_TSPEC_RESULT_TIMEOUT                           ((CsrWifiSmeTspecResultCode) 0x0D)
+#define CSR_WIFI_SME_TSPEC_RESULT_NOT_SUPPORTED                     ((CsrWifiSmeTspecResultCode) 0x0E)
+#define CSR_WIFI_SME_TSPEC_RESULT_IE_LENGTH_INCORRECT               ((CsrWifiSmeTspecResultCode) 0x10)
+#define CSR_WIFI_SME_TSPEC_RESULT_INVALID_TRANSACTION_ID            ((CsrWifiSmeTspecResultCode) 0x11)
+#define CSR_WIFI_SME_TSPEC_RESULT_INSTALLED                         ((CsrWifiSmeTspecResultCode) 0x12)
+#define CSR_WIFI_SME_TSPEC_RESULT_TID_ALREADY_INSTALLED             ((CsrWifiSmeTspecResultCode) 0x13)
+#define CSR_WIFI_SME_TSPEC_RESULT_TSPEC_REMOTELY_DELETED            ((CsrWifiSmeTspecResultCode) 0x14)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWepAuthMode
+
+  DESCRIPTION
+    Define bits for CsrWifiSmeWepAuthMode
+
+ VALUES
+    CSR_WIFI_SME_WEP_AUTH_MODE_OPEN   - Open-WEP enabled network
+    CSR_WIFI_SME_WEP_AUTH_MODE_SHARED - Shared-key WEP enabled network.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWepAuthMode;
+#define CSR_WIFI_SME_WEP_AUTH_MODE_OPEN     ((CsrWifiSmeWepAuthMode) 0x00)
+#define CSR_WIFI_SME_WEP_AUTH_MODE_SHARED   ((CsrWifiSmeWepAuthMode) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWepCredentialType
+
+  DESCRIPTION
+    Definition of types of WEP Credentials
+
+ VALUES
+    CSR_WIFI_SME_CREDENTIAL_TYPE_WEP64
+                   - WEP 64 credential
+    CSR_WIFI_SME_CREDENTIAL_TYPE_WEP128
+                   - WEP 128 credential
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWepCredentialType;
+#define CSR_WIFI_SME_CREDENTIAL_TYPE_WEP64    ((CsrWifiSmeWepCredentialType) 0x00)
+#define CSR_WIFI_SME_CREDENTIAL_TYPE_WEP128   ((CsrWifiSmeWepCredentialType) 0x01)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWmmMode
+
+  DESCRIPTION
+    Defines bits for CsrWifiSmeWmmModeMask: enable/disable WMM features.
+
+ VALUES
+    CSR_WIFI_SME_WMM_MODE_DISABLED   - Disables the WMM features.
+    CSR_WIFI_SME_WMM_MODE_AC_ENABLED - Enables support for WMM-AC.
+    CSR_WIFI_SME_WMM_MODE_PS_ENABLED - Enables support for WMM Power Save.
+    CSR_WIFI_SME_WMM_MODE_SA_ENABLED - Currently not supported
+    CSR_WIFI_SME_WMM_MODE_ENABLED    - Enables support for all currently
+                                       available WMM features.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWmmMode;
+#define CSR_WIFI_SME_WMM_MODE_DISABLED     ((CsrWifiSmeWmmMode) 0x00)
+#define CSR_WIFI_SME_WMM_MODE_AC_ENABLED   ((CsrWifiSmeWmmMode) 0x01)
+#define CSR_WIFI_SME_WMM_MODE_PS_ENABLED   ((CsrWifiSmeWmmMode) 0x02)
+#define CSR_WIFI_SME_WMM_MODE_SA_ENABLED   ((CsrWifiSmeWmmMode) 0x04)
+#define CSR_WIFI_SME_WMM_MODE_ENABLED      ((CsrWifiSmeWmmMode) 0xFF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWmmQosInfo
+
+  DESCRIPTION
+    Defines bits for the QoS Info Octect as defined in the WMM specification.
+    The first four values define one bit each that can be set or cleared.
+    Each of the last four values define all the remaining 4 bits and only one
+    of them at the time shall be used.
+    For more information, see 'WMM (including WMM Power Save) Specification -
+    Version 1.1' and 'UniFi Configuring WMM and WMM-PS, Application Note'.
+
+ VALUES
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_ALL
+                   - WMM AP may deliver all buffered frames
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_VO
+                   - Enable UAPSD(both trigger and delivery) for Voice Access
+                     Category
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_VI
+                   - Enable UAPSD(both trigger and delivery) for  Video Access
+                     Category
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_BK
+                   - Enable UAPSD(both trigger and delivery) for  Background
+                     Access Category
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_BE
+                   - Enable UAPSD(both trigger and delivery) for  Best Effort
+                     Access Category
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_TWO
+                   - WMM AP may deliver a maximum of 2 buffered frames (MSDUs
+                     and MMPDUs) per USP
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_FOUR
+                   - WMM AP may deliver a maximum of 4 buffered frames (MSDUs
+                     and MMPDUs) per USP
+    CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_SIX
+                   - WMM AP may deliver a maximum of 6 buffered frames (MSDUs
+                     and MMPDUs) per USP
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWmmQosInfo;
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_ALL    ((CsrWifiSmeWmmQosInfo) 0x00)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_VO            ((CsrWifiSmeWmmQosInfo) 0x01)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_VI            ((CsrWifiSmeWmmQosInfo) 0x02)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_BK            ((CsrWifiSmeWmmQosInfo) 0x04)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_BE            ((CsrWifiSmeWmmQosInfo) 0x08)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_TWO    ((CsrWifiSmeWmmQosInfo) 0x20)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_FOUR   ((CsrWifiSmeWmmQosInfo) 0x40)
+#define CSR_WIFI_SME_WMM_QOS_INFO_AC_MAX_SP_SIX    ((CsrWifiSmeWmmQosInfo) 0x60)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfigType
+
+  DESCRIPTION
+    WPS config methods supported/used by a device
+
+ VALUES
+    CSR_WIFI_WPS_CONFIG_LABEL
+                   - Label
+    CSR_WIFI_WPS_CONFIG_DISPLAY
+                   - Display
+    CSR_WIFI_WPS_CONFIG_EXT_NFC
+                   - External NFC : Not supported in this release
+    CSR_WIFI_WPS_CONFIG_INT_NFC
+                   - Internal NFC : Not supported in this release
+    CSR_WIFI_WPS_CONFIG_NFC_IFACE
+                   - NFC interface : Not supported in this release
+    CSR_WIFI_WPS_CONFIG_PBC
+                   - PBC
+    CSR_WIFI_WPS_CONFIG_KEYPAD
+                   - KeyPad
+    CSR_WIFI_WPS_CONFIG_VIRTUAL_PBC
+                   - PBC through software user interface
+    CSR_WIFI_WPS_CONFIG_PHYSICAL_PBC
+                   - Physical PBC
+    CSR_WIFI_WPS_CONFIG_VIRTUAL_DISPLAY
+                   - Virtual Display : via html config page etc
+    CSR_WIFI_WPS_CONFIG_PHYSICAL_DISPLAY
+                   - Physical Display : Attached to the device
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeWpsConfigType;
+#define CSR_WIFI_WPS_CONFIG_LABEL              ((CsrWifiSmeWpsConfigType) 0x0004)
+#define CSR_WIFI_WPS_CONFIG_DISPLAY            ((CsrWifiSmeWpsConfigType) 0x0008)
+#define CSR_WIFI_WPS_CONFIG_EXT_NFC            ((CsrWifiSmeWpsConfigType) 0x0010)
+#define CSR_WIFI_WPS_CONFIG_INT_NFC            ((CsrWifiSmeWpsConfigType) 0x0020)
+#define CSR_WIFI_WPS_CONFIG_NFC_IFACE          ((CsrWifiSmeWpsConfigType) 0x0040)
+#define CSR_WIFI_WPS_CONFIG_PBC                ((CsrWifiSmeWpsConfigType) 0x0080)
+#define CSR_WIFI_WPS_CONFIG_KEYPAD             ((CsrWifiSmeWpsConfigType) 0x0100)
+#define CSR_WIFI_WPS_CONFIG_VIRTUAL_PBC        ((CsrWifiSmeWpsConfigType) 0x0280)
+#define CSR_WIFI_WPS_CONFIG_PHYSICAL_PBC       ((CsrWifiSmeWpsConfigType) 0x0480)
+#define CSR_WIFI_WPS_CONFIG_VIRTUAL_DISPLAY    ((CsrWifiSmeWpsConfigType) 0x2008)
+#define CSR_WIFI_WPS_CONFIG_PHYSICAL_DISPLAY   ((CsrWifiSmeWpsConfigType) 0x4008)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsDeviceCategory
+
+  DESCRIPTION
+    Wps Primary Device Types
+
+ VALUES
+    CSR_WIFI_SME_WPS_CATEGORY_UNSPECIFIED
+                   - Unspecified.
+    CSR_WIFI_SME_WPS_CATEGORY_COMPUTER
+                   - Computer.
+    CSR_WIFI_SME_WPS_CATEGORY_INPUT_DEV
+                   - Input device
+    CSR_WIFI_SME_WPS_CATEGORY_PRT_SCAN_FX_CP
+                   - Printer Scanner Fax Copier etc
+    CSR_WIFI_SME_WPS_CATEGORY_CAMERA
+                   - Camera
+    CSR_WIFI_SME_WPS_CATEGORY_STORAGE
+                   - Storage
+    CSR_WIFI_SME_WPS_CATEGORY_NET_INFRA
+                   - Net Infra
+    CSR_WIFI_SME_WPS_CATEGORY_DISPLAY
+                   - Display
+    CSR_WIFI_SME_WPS_CATEGORY_MULTIMEDIA
+                   - Multimedia
+    CSR_WIFI_SME_WPS_CATEGORY_GAMING
+                   - Gaming.
+    CSR_WIFI_SME_WPS_CATEGORY_TELEPHONE
+                   - Telephone.
+    CSR_WIFI_SME_WPS_CATEGORY_AUDIO
+                   - Audio
+    CSR_WIFI_SME_WPS_CATEOARY_OTHERS
+                   - Others.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWpsDeviceCategory;
+#define CSR_WIFI_SME_WPS_CATEGORY_UNSPECIFIED      ((CsrWifiSmeWpsDeviceCategory) 0x00)
+#define CSR_WIFI_SME_WPS_CATEGORY_COMPUTER         ((CsrWifiSmeWpsDeviceCategory) 0x01)
+#define CSR_WIFI_SME_WPS_CATEGORY_INPUT_DEV        ((CsrWifiSmeWpsDeviceCategory) 0x02)
+#define CSR_WIFI_SME_WPS_CATEGORY_PRT_SCAN_FX_CP   ((CsrWifiSmeWpsDeviceCategory) 0x03)
+#define CSR_WIFI_SME_WPS_CATEGORY_CAMERA           ((CsrWifiSmeWpsDeviceCategory) 0x04)
+#define CSR_WIFI_SME_WPS_CATEGORY_STORAGE          ((CsrWifiSmeWpsDeviceCategory) 0x05)
+#define CSR_WIFI_SME_WPS_CATEGORY_NET_INFRA        ((CsrWifiSmeWpsDeviceCategory) 0x06)
+#define CSR_WIFI_SME_WPS_CATEGORY_DISPLAY          ((CsrWifiSmeWpsDeviceCategory) 0x07)
+#define CSR_WIFI_SME_WPS_CATEGORY_MULTIMEDIA       ((CsrWifiSmeWpsDeviceCategory) 0x08)
+#define CSR_WIFI_SME_WPS_CATEGORY_GAMING           ((CsrWifiSmeWpsDeviceCategory) 0x09)
+#define CSR_WIFI_SME_WPS_CATEGORY_TELEPHONE        ((CsrWifiSmeWpsDeviceCategory) 0x0A)
+#define CSR_WIFI_SME_WPS_CATEGORY_AUDIO            ((CsrWifiSmeWpsDeviceCategory) 0x0B)
+#define CSR_WIFI_SME_WPS_CATEOARY_OTHERS           ((CsrWifiSmeWpsDeviceCategory) 0xFF)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsDeviceSubCategory
+
+  DESCRIPTION
+    Wps Secondary Device Types
+
+ VALUES
+    CSR_WIFI_SME_WPS_SUB_CATEGORY_UNSPECIFIED
+                   - Unspecied
+    CSR_WIFI_SME_WPS_STORAGE_SUB_CATEGORY_NAS
+                   - Network Associated Storage
+    CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_PRNTR
+                   - Printer or print server
+    CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_WM
+                   - Windows mobile
+    CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_TUNER
+                   - Audio tuner/receiver
+    CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_DIG_STL
+                   - Digital still camera
+    CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_AP
+                   - Access Point
+    CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_TV
+                   - TV.
+    CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_DAR
+                   - DAR.
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_KEYBD
+                   - Keyboard.
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_PC
+                   - PC.
+    CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_XBOX
+                   - Xbox.
+    CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_SCNR
+                   - Scanner
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_SERVER
+                   - Server
+    CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_ROUTER
+                   - Router
+    CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_PVR
+                   - PVR
+    CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_SPEAKER
+                   - Speaker
+    CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_FP_SM
+                   - Feature phone - Single mode
+    CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_VIDEO
+                   - Video camera
+    CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_PIC_FRM
+                   - Picture frame
+    CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_XBOX_360
+                   - Xbox360
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_MOUSE
+                   - Mouse
+    CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_SWITCH
+                   - Switch
+    CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_PMP
+                   - Portable music player
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_JOYSTK
+                   - Joy stick
+    CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_PLAY_STN
+                   - Play-station
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_MED_CENT
+                   - Media Center
+    CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_MCX
+                   - MCX
+    CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_FP_DM
+                   - Feature phone - Dual mode
+    CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_WEB
+                   - Web camera
+    CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_FAX
+                   - Fax
+    CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_PROJECTOR
+                   - Projector
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_TRKBL
+                   - Track Ball
+    CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_ST_BOX
+                   - Set-Top-Box
+    CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_GATEWAY
+                   - GateWay.
+    CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_SECURITY
+                   - Security camera
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_ULTRA_MOB_PC
+                   - Ultra mobile PC.
+    CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_CONSOLE
+                   - Game console/Game console adapter
+    CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_CPR
+                   - Copier
+    CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_HEADSET
+                   - Headset(headphones + microphone)
+    CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_SP_SM
+                   - Smart phone - Single mode
+    CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_MONITOR
+                   - Monitor.
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_GAME_CTRL
+                   - Game control.
+    CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_ALL
+                   - All-in-One
+    CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_MEDIA
+                   - Media Server/Media Adapter/Media Extender
+    CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_SP_DM
+                   - Smart phone - Dual mode
+    CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_PORT_DEV
+                   - Portable gaming device
+    CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_HEADPHONE
+                   - Headphone.
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_NOTEBOOK
+                   - Notebook.
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_REMOTE
+                   - Remote control
+    CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_MIC
+                   - Microphone
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_DESKTOP
+                   - Desktop.
+    CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_VP
+                   - Portable video player
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_MID
+                   - Mobile internet device
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_TOUCH_SCRN
+                   - Touch screen
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_BIOMET_RDR
+                   - Biometric reader
+    CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_NETBOOK
+                   - Netbook
+    CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_BRCD_RDR
+                   - Bar code reader.
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWpsDeviceSubCategory;
+#define CSR_WIFI_SME_WPS_SUB_CATEGORY_UNSPECIFIED             ((CsrWifiSmeWpsDeviceSubCategory) 0x00)
+#define CSR_WIFI_SME_WPS_STORAGE_SUB_CATEGORY_NAS             ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_PRNTR              ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_WM            ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_TUNER             ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_DIG_STL          ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_AP            ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_TV              ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_DAR                  ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_KEYBD         ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_PC             ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_XBOX             ((CsrWifiSmeWpsDeviceSubCategory) 0x01)
+#define CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_SCNR               ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_SERVER         ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_ROUTER        ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_PVR                  ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_SPEAKER           ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_FP_SM         ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_VIDEO            ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_PIC_FRM         ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_XBOX_360         ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_MOUSE         ((CsrWifiSmeWpsDeviceSubCategory) 0x02)
+#define CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_SWITCH        ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_PMP               ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_JOYSTK        ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_PLAY_STN         ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_MED_CENT       ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_MCX                  ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_FP_DM         ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_WEB              ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_FAX                ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_PROJECTOR       ((CsrWifiSmeWpsDeviceSubCategory) 0x03)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_TRKBL         ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_ST_BOX               ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_NET_INFRA_SUB_CATEGORY_GATEWAY       ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_CAMERA_SUB_CATEGORY_SECURITY         ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_ULTRA_MOB_PC   ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_CONSOLE          ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_CPR                ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_HEADSET           ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_SP_SM         ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_DISPLAY_SUB_CATEGORY_MONITOR         ((CsrWifiSmeWpsDeviceSubCategory) 0x04)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_GAME_CTRL     ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_PSFC_SUB_CATEGORY_ALL                ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_MEDIA                ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_TELEPHONE_SUB_CATEGORY_SP_DM         ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_GAMING_SUB_CATEGORY_PORT_DEV         ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_HEADPHONE         ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_NOTEBOOK       ((CsrWifiSmeWpsDeviceSubCategory) 0x05)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_REMOTE        ((CsrWifiSmeWpsDeviceSubCategory) 0x06)
+#define CSR_WIFI_SME_WPS_AUDIO_SUB_CATEGORY_MIC               ((CsrWifiSmeWpsDeviceSubCategory) 0x06)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_DESKTOP        ((CsrWifiSmeWpsDeviceSubCategory) 0x06)
+#define CSR_WIFI_SME_WPS_MM_SUB_CATEGORY_VP                   ((CsrWifiSmeWpsDeviceSubCategory) 0x06)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_MID            ((CsrWifiSmeWpsDeviceSubCategory) 0x07)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_TOUCH_SCRN    ((CsrWifiSmeWpsDeviceSubCategory) 0x07)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_BIOMET_RDR    ((CsrWifiSmeWpsDeviceSubCategory) 0x08)
+#define CSR_WIFI_SME_WPS_COMPUTER_SUB_CATEGORY_NETBOOK        ((CsrWifiSmeWpsDeviceSubCategory) 0x08)
+#define CSR_WIFI_SME_WPS_INPUT_DEV_SUB_CATEGORY_BRCD_RDR      ((CsrWifiSmeWpsDeviceSubCategory) 0x09)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsDpid
+
+  DESCRIPTION
+    Device Password ID for the chosen config method
+
+ VALUES
+    CSR_WIFI_SME_WPS_DPID_PIN       - PIN
+    CSR_WIFI_SME_WPS_DPID_USER      - User specified : Used only during P2P GO
+                                      negotiation procedure
+    CSR_WIFI_SME_WPS_DPID_MACHINE   - Machine specified i: Not used in this
+                                      release
+    CSR_WIFI_SME_WPS_DPID_REKEY     - Rekey : Not used in this release
+    CSR_WIFI_SME_WPS_DPID_PBC       - PBC
+    CSR_WIFI_SME_WPS_DPID_REGISTRAR - Registrar specified : Used only in P2P Go
+                                      negotiation procedure
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeWpsDpid;
+#define CSR_WIFI_SME_WPS_DPID_PIN         ((CsrWifiSmeWpsDpid) 0x0000)
+#define CSR_WIFI_SME_WPS_DPID_USER        ((CsrWifiSmeWpsDpid) 0x0001)
+#define CSR_WIFI_SME_WPS_DPID_MACHINE     ((CsrWifiSmeWpsDpid) 0x0002)
+#define CSR_WIFI_SME_WPS_DPID_REKEY       ((CsrWifiSmeWpsDpid) 0x0003)
+#define CSR_WIFI_SME_WPS_DPID_PBC         ((CsrWifiSmeWpsDpid) 0x0004)
+#define CSR_WIFI_SME_WPS_DPID_REGISTRAR   ((CsrWifiSmeWpsDpid) 0x0005)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsRegistration
+
+  DESCRIPTION
+
+ VALUES
+    CSR_WIFI_SME_WPS_REG_NOT_REQUIRED - No encryption set
+    CSR_WIFI_SME_WPS_REG_REQUIRED     - No encryption set
+    CSR_WIFI_SME_WPS_REG_UNKNOWN      - No encryption set
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWpsRegistration;
+#define CSR_WIFI_SME_WPS_REG_NOT_REQUIRED   ((CsrWifiSmeWpsRegistration) 0x00)
+#define CSR_WIFI_SME_WPS_REG_REQUIRED       ((CsrWifiSmeWpsRegistration) 0x01)
+#define CSR_WIFI_SME_WPS_REG_UNKNOWN        ((CsrWifiSmeWpsRegistration) 0x02)
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAuthModeMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeAuthMode
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeAuthModeMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEncryptionMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeEncryption
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeEncryptionMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeIndicationsMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeIndications
+
+*******************************************************************************/
+typedef u32 CsrWifiSmeIndicationsMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pCapabilityMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeP2pCapability
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pCapabilityMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pGroupCapabilityMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeP2pGroupCapability
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeP2pGroupCapabilityMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecCtrlMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeTspecCtrl
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeTspecCtrlMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWmmModeMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeWmmMode
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWmmModeMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWmmQosInfoMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeWmmQosInfo
+
+*******************************************************************************/
+typedef u8 CsrWifiSmeWmmQosInfoMask;
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfigTypeMask
+
+  DESCRIPTION
+    Mask type for use with the values defined by CsrWifiSmeWpsConfigType
+
+*******************************************************************************/
+typedef u16 CsrWifiSmeWpsConfigTypeMask;
+
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdHocConfig
+
+  DESCRIPTION
+    Defines values to use when starting an Ad-hoc (IBSS) network.
+
+  MEMBERS
+    atimWindowTu          - ATIM window specified for IBSS
+    beaconPeriodTu        - Interval between beacon packets
+    joinOnlyAttempts      - Maximum number of attempts to join an ad-hoc network.
+                            The default value is 1.
+                            Set to 0 for infinite attempts.
+    joinAttemptIntervalMs - Time between scans for joining the requested IBSS.
+
+*******************************************************************************/
+typedef struct
+{
+    u16 atimWindowTu;
+    u16 beaconPeriodTu;
+    u16 joinOnlyAttempts;
+    u16 joinAttemptIntervalMs;
+} CsrWifiSmeAdHocConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAvailabilityConfig
+
+  DESCRIPTION
+
+  MEMBERS
+    listenChannel        -
+    availabilityDuration -
+    avalabilityPeriod    -
+
+*******************************************************************************/
+typedef struct
+{
+    u8  listenChannel;
+    u16 availabilityDuration;
+    u16 avalabilityPeriod;
+} CsrWifiSmeAvailabilityConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfig
+
+  DESCRIPTION
+    This type is reserved for future use and should not be used.
+
+  MEMBERS
+    keepAliveTimeMs    - NOT USED
+    apRoamingEnabled   - NOT USED
+    measurementsMask   - NOT USED
+    ccxRadioMgtEnabled - NOT USED
+
+*******************************************************************************/
+typedef struct
+{
+    u8 keepAliveTimeMs;
+    u8  apRoamingEnabled;
+    u8 measurementsMask;
+    u8  ccxRadioMgtEnabled;
+} CsrWifiSmeCcxConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfig
+
+  DESCRIPTION
+    Parameters for the coexistence behaviour.
+
+  MEMBERS
+    coexEnableSchemeManagement     - Enables the Coexistence Management Scheme
+    coexPeriodicWakeHost           - If TRUE the firmware wakes up the host
+                                     periodically according to the traffic
+                                     period and latency parameters; the host
+                                     will then send the data to transmit only
+                                     when woken up.
+                                     If FALSE, the firmware does not wake up the
+                                     host and the host will send the data to
+                                     transmit to the firmware whenever there is
+                                     data to transmit
+    coexTrafficBurstyLatencyMs     - Period of awakening for the firmware used
+                                     when bursty traffic is detected
+    coexTrafficContinuousLatencyMs - Period of awakening for the firmware used
+                                     when continuous traffic is detected
+    coexObexBlackoutDurationMs     - Blackout Duration when a Obex Connection is
+                                     used
+    coexObexBlackoutPeriodMs       - Blackout Period when a Obex Connection is
+                                     used
+    coexA2dpBrBlackoutDurationMs   - Blackout Duration when a Basic Rate A2DP
+                                     Connection streaming data
+    coexA2dpBrBlackoutPeriodMs     - Blackout Period when a Basic Rate A2DP
+                                     Connection streaming data
+    coexA2dpEdrBlackoutDurationMs  - Blackout Duration when an Enhanced Data
+                                     Rate A2DP Connection streaming data
+    coexA2dpEdrBlackoutPeriodMs    - Blackout Period when an Enhanced Data Rate
+                                     A2DP Connection streaming data
+    coexPagingBlackoutDurationMs   - Blackout Duration when a BT page is active
+    coexPagingBlackoutPeriodMs     - Blackout Period when a BT page is active
+    coexInquiryBlackoutDurationMs  - Blackout Duration when a BT inquiry is
+                                     active
+    coexInquiryBlackoutPeriodMs    - Blackout Period when a BT inquiry is active
+
+*******************************************************************************/
+typedef struct
+{
+    u8   coexEnableSchemeManagement;
+    u8   coexPeriodicWakeHost;
+    u16 coexTrafficBurstyLatencyMs;
+    u16 coexTrafficContinuousLatencyMs;
+    u16 coexObexBlackoutDurationMs;
+    u16 coexObexBlackoutPeriodMs;
+    u16 coexA2dpBrBlackoutDurationMs;
+    u16 coexA2dpBrBlackoutPeriodMs;
+    u16 coexA2dpEdrBlackoutDurationMs;
+    u16 coexA2dpEdrBlackoutPeriodMs;
+    u16 coexPagingBlackoutDurationMs;
+    u16 coexPagingBlackoutPeriodMs;
+    u16 coexInquiryBlackoutDurationMs;
+    u16 coexInquiryBlackoutPeriodMs;
+} CsrWifiSmeCoexConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionStats
+
+  DESCRIPTION
+    Indicates the statistics of the connection.
+    The dot11 fields are defined in the Annex D of the IEEE 802.11 standard.
+
+  MEMBERS
+    unifiTxDataRate
+                   - The bit rate currently in use for transmissions of unicast
+                     data frames; a data rate in units of 500kbit/s.
+                     On an infrastructure BSS, this is the data rate used in
+                     communicating with the associated access point; if there is
+                     none, an error is returned.
+                     On an IBSS, this is the data rate used for the last
+                     transmission of a unicast data frame to any station in the
+                     IBSS. If no such transmission has been made, an error is
+                     returned.
+    unifiRxDataRate
+                   - As above for receiving data
+    dot11RetryCount
+                   - See IEEE 802.11 Standard
+    dot11MultipleRetryCount
+                   - See IEEE 802.11 Standard
+    dot11AckFailureCount
+                   - See IEEE 802.11 Standard
+    dot11FrameDuplicateCount
+                   - See IEEE 802.11 Standard
+    dot11FcsErrorCount
+                   - See IEEE 802.11 Standard
+    dot11RtsSuccessCount
+                   - See IEEE 802.11 Standard
+    dot11RtsFailureCount
+                   - See IEEE 802.11 Standard
+    dot11FailedCount
+                   - See IEEE 802.11 Standard
+    dot11TransmittedFragmentCount
+                   - See IEEE 802.11 Standard
+    dot11TransmittedFrameCount
+                   - See IEEE 802.11 Standard
+    dot11WepExcludedCount
+                   - See IEEE 802.11 Standard
+    dot11WepIcvErrorCount
+                   - See IEEE 802.11 Standard
+    dot11WepUndecryptableCount
+                   - See IEEE 802.11 Standard
+    dot11MulticastReceivedFrameCount
+                   - See IEEE 802.11 Standard
+    dot11MulticastTransmittedFrameCount
+                   - See IEEE 802.11 Standard
+    dot11ReceivedFragmentCount
+                   - See IEEE 802.11 Standard
+    dot11Rsna4WayHandshakeFailures
+                   - See IEEE 802.11 Standard
+    dot11RsnaTkipCounterMeasuresInvoked
+                   - See IEEE 802.11 Standard
+    dot11RsnaStatsTkipLocalMicFailures
+                   - See IEEE 802.11 Standard
+    dot11RsnaStatsTkipReplays
+                   - See IEEE 802.11 Standard
+    dot11RsnaStatsTkipIcvErrors
+                   - See IEEE 802.11 Standard
+    dot11RsnaStatsCcmpReplays
+                   - See IEEE 802.11 Standard
+    dot11RsnaStatsCcmpDecryptErrors
+                   - See IEEE 802.11 Standard
+
+*******************************************************************************/
+typedef struct
+{
+    u8  unifiTxDataRate;
+    u8  unifiRxDataRate;
+    u32 dot11RetryCount;
+    u32 dot11MultipleRetryCount;
+    u32 dot11AckFailureCount;
+    u32 dot11FrameDuplicateCount;
+    u32 dot11FcsErrorCount;
+    u32 dot11RtsSuccessCount;
+    u32 dot11RtsFailureCount;
+    u32 dot11FailedCount;
+    u32 dot11TransmittedFragmentCount;
+    u32 dot11TransmittedFrameCount;
+    u32 dot11WepExcludedCount;
+    u32 dot11WepIcvErrorCount;
+    u32 dot11WepUndecryptableCount;
+    u32 dot11MulticastReceivedFrameCount;
+    u32 dot11MulticastTransmittedFrameCount;
+    u32 dot11ReceivedFragmentCount;
+    u32 dot11Rsna4WayHandshakeFailures;
+    u32 dot11RsnaTkipCounterMeasuresInvoked;
+    u32 dot11RsnaStatsTkipLocalMicFailures;
+    u32 dot11RsnaStatsTkipReplays;
+    u32 dot11RsnaStatsTkipIcvErrors;
+    u32 dot11RsnaStatsCcmpReplays;
+    u32 dot11RsnaStatsCcmpDecryptErrors;
+} CsrWifiSmeConnectionStats;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDataBlock
+
+  DESCRIPTION
+    Holds a generic data block to be passed through the interface
+
+  MEMBERS
+    length - Length of the data block
+    data   - Points to the first byte of the data block
+
+*******************************************************************************/
+typedef struct
+{
+    u16 length;
+    u8 *data;
+} CsrWifiSmeDataBlock;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEmpty
+
+  DESCRIPTION
+    Empty Structure to indicate that no parameters are available.
+
+  MEMBERS
+    empty  - Only element of the empty structure (always set to 0).
+
+*******************************************************************************/
+typedef struct
+{
+    u8 empty;
+} CsrWifiSmeEmpty;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeLinkQuality
+
+  DESCRIPTION
+    Indicates the quality of the link
+
+  MEMBERS
+    unifiRssi - Indicates the received signal strength indication of the link in
+                dBm
+    unifiSnr  - Indicates the signal to noise ratio of the link in dB
+
+*******************************************************************************/
+typedef struct
+{
+    s16 unifiRssi;
+    s16 unifiSnr;
+} CsrWifiSmeLinkQuality;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfig
+
+  DESCRIPTION
+    Allows low level configuration in the chip.
+
+  MEMBERS
+    unifiFixMaxTxDataRate       - This attribute is used in combination with
+                                  unifiFixTxDataRate. If it is FALSE, then
+                                  unifiFixTxDataRate specifies a specific data
+                                  rate to use. If it is TRUE, unifiFixTxDataRate
+                                  instead specifies a maximum data rate.
+    unifiFixTxDataRate          - Transmit rate for unicast data.
+                                  See MIB description for more details
+    dot11RtsThreshold           - See IEEE 802.11 Standard
+    dot11FragmentationThreshold - See IEEE 802.11 Standard
+    dot11CurrentTxPowerLevel    - See IEEE 802.11 Standard
+
+*******************************************************************************/
+typedef struct
+{
+    u8   unifiFixMaxTxDataRate;
+    u8  unifiFixTxDataRate;
+    u16 dot11RtsThreshold;
+    u16 dot11FragmentationThreshold;
+    u16 dot11CurrentTxPowerLevel;
+} CsrWifiSmeMibConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pProfileIdentity
+
+  DESCRIPTION
+    Details to be filled in
+
+  MEMBERS
+    listenChannel        -
+    availabilityDuration -
+    avalabilityPeriod    -
+
+*******************************************************************************/
+typedef struct
+{
+    u8  listenChannel;
+    u16 availabilityDuration;
+    u16 avalabilityPeriod;
+} CsrWifiSmeP2pProfileIdentity;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkid
+
+  DESCRIPTION
+    Defines a PMKID association with BSS
+
+  MEMBERS
+    bssid  - BSS identifier
+    pmkid  - PMKID
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress bssid;
+    u8          pmkid[16];
+} CsrWifiSmePmkid;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidCandidate
+
+  DESCRIPTION
+    Information for a PMKID candidate
+
+  MEMBERS
+    bssid          - BSS identifier
+    preAuthAllowed - Indicates whether preauthentication is allowed
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress bssid;
+    u8           preAuthAllowed;
+} CsrWifiSmePmkidCandidate;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidList
+
+  DESCRIPTION
+    NOT USED
+    Used in the Sync access API
+
+  MEMBERS
+    pmkidsCount - Number of PMKIDs in the list
+    pmkids      - Points to the first PMKID in the list
+
+*******************************************************************************/
+typedef struct
+{
+    u8         pmkidsCount;
+    CsrWifiSmePmkid *pmkids;
+} CsrWifiSmePmkidList;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRegulatoryDomainInfo
+
+  DESCRIPTION
+    Regulatory domain options.
+
+  MEMBERS
+    dot11MultiDomainCapabilityImplemented
+                   - TRUE is the multi domain capability is implemented
+    dot11MultiDomainCapabilityEnabled
+                   - TRUE is the multi domain capability is enabled
+    currentRegulatoryDomain
+                   - Current regulatory domain
+    currentCountryCode
+                   - Current country code as defined by the IEEE 802.11
+                     standards
+
+*******************************************************************************/
+typedef struct
+{
+    u8                    dot11MultiDomainCapabilityImplemented;
+    u8                    dot11MultiDomainCapabilityEnabled;
+    CsrWifiSmeRegulatoryDomain currentRegulatoryDomain;
+    u8                   currentCountryCode[2];
+} CsrWifiSmeRegulatoryDomainInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingBandData
+
+  DESCRIPTION
+    Thresholds to define one usability level category for the received signal
+
+  MEMBERS
+    rssiHighThreshold - Received Signal Strength Indication upper bound in dBm
+                        for the usability level
+    rssiLowThreshold  - Received Signal Strength Indication lower bound in dBm
+                        for the usability level
+    snrHighThreshold  - Signal to Noise Ratio upper bound in dB for the
+                        usability level
+    snrLowThreshold   - Signal to Noise Ratio lower bound in dB for the
+                        usability level
+
+*******************************************************************************/
+typedef struct
+{
+    s16 rssiHighThreshold;
+    s16 rssiLowThreshold;
+    s16 snrHighThreshold;
+    s16 snrLowThreshold;
+} CsrWifiSmeRoamingBandData;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigData
+
+  DESCRIPTION
+    Configures the scanning behaviour of the driver and firmware
+
+  MEMBERS
+    intervalSeconds         - All the channels will be scanned once in this time
+                              interval.
+                              If connected, the channel scans are spread across
+                              the interval.
+                              If disconnected, all the channels will be scanned
+                              together
+    validitySeconds         - How long the scan result are cached
+    minActiveChannelTimeTu  - Minimum time of listening on a channel being
+                              actively scanned before leaving if no probe
+                              responses or beacon frames have been received
+    maxActiveChannelTimeTu  - Maximum time of listening on a channel being
+                              actively scanned
+    minPassiveChannelTimeTu - Minimum time of listening on a channel being
+                              passive scanned before leaving if no beacon frames
+                              have been received
+    maxPassiveChannelTimeTu - Maximum time of listening on a channel being
+                              passively scanned
+
+*******************************************************************************/
+typedef struct
+{
+    u16 intervalSeconds;
+    u16 validitySeconds;
+    u16 minActiveChannelTimeTu;
+    u16 maxActiveChannelTimeTu;
+    u16 minPassiveChannelTimeTu;
+    u16 maxPassiveChannelTimeTu;
+} CsrWifiSmeScanConfigData;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTsfTime
+
+  DESCRIPTION
+    Time stamp representation
+
+  MEMBERS
+    data   - TSF Bytes
+
+*******************************************************************************/
+typedef struct
+{
+    u8 data[8];
+} CsrWifiSmeTsfTime;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeVersions
+
+  DESCRIPTION
+    Reports version information for the chip, the firmware and the driver and
+    the SME.
+
+  MEMBERS
+    chipId        - Chip ID
+    chipVersion   - Chip version ID
+    firmwareBuild - Firmware Rom build number
+    firmwarePatch - Firmware Patch build number (if applicable)
+    firmwareHip   - Firmware HIP protocol version number
+    routerBuild   - Router build number
+    routerHip     - Router HIP protocol version number
+    smeBuild      - SME build number
+    smeHip        - SME HIP protocol version number
+
+*******************************************************************************/
+typedef struct
+{
+    u32      chipId;
+    u32      chipVersion;
+    u32      firmwareBuild;
+    u32      firmwarePatch;
+    u32      firmwareHip;
+    char *routerBuild;
+    u32      routerHip;
+    char *smeBuild;
+    u32      smeHip;
+} CsrWifiSmeVersions;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWmmAcParams
+
+  DESCRIPTION
+    Structure holding WMM AC params data.
+
+  MEMBERS
+    cwMin                     - Exponent for the calculation of CWmin. Range: 0
+                                to 15
+    cwMax                     - Exponent for the calculation of CWmax. Range: 0
+                                to15
+    aifs                      - Arbitration Inter Frame Spacing in terms of
+                                number of timeslots. Range 2 to 15
+    txopLimit                 - TXOP Limit in the units of 32 microseconds
+    admissionControlMandatory - Indicates whether the admission control is
+                                mandatory or not. Current release does not
+                                support admission control , hence shall be set
+                                to FALSE.
+
+*******************************************************************************/
+typedef struct
+{
+    u8  cwMin;
+    u8  cwMax;
+    u8  aifs;
+    u16 txopLimit;
+    u8   admissionControlMandatory;
+} CsrWifiSmeWmmAcParams;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsDeviceType
+
+  DESCRIPTION
+    Structure holding AP WPS device type data.
+
+  MEMBERS
+    deviceDetails - category , sub category etc
+
+*******************************************************************************/
+typedef struct
+{
+    u8 deviceDetails[8];
+} CsrWifiSmeWpsDeviceType;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsDeviceTypeCommon
+
+  DESCRIPTION
+
+  MEMBERS
+    spportWps  -
+    deviceType -
+
+*******************************************************************************/
+typedef struct
+{
+    u8  spportWps;
+    u8 deviceType;
+} CsrWifiSmeWpsDeviceTypeCommon;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsInfo
+
+  DESCRIPTION
+
+  MEMBERS
+    version         -
+    configMethods   -
+    devicePassworId -
+
+*******************************************************************************/
+typedef struct
+{
+    u16 version;
+    u16 configMethods;
+    u16 devicePassworId;
+} CsrWifiSmeWpsInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidConfig
+
+  DESCRIPTION
+    List of cloaked SSIDs .
+
+  MEMBERS
+    cloakedSsidsCount - Number of cloaked SSID
+    cloakedSsids      - Points to the first byte of the first SSID provided
+
+*******************************************************************************/
+typedef struct
+{
+    u8     cloakedSsidsCount;
+    CsrWifiSsid *cloakedSsids;
+} CsrWifiSmeCloakedSsidConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexInfo
+
+  DESCRIPTION
+    Information and state related to coexistence.
+
+  MEMBERS
+    hasTrafficData            - TRUE if any Wi-Fi traffic is detected
+    currentTrafficType        - Current type of traffic
+    currentPeriodMs           - Period of the traffic as detected by the traffic
+                                analysis.
+                                If the traffic is not periodic, it is set to 0.
+    currentPowerSave          - Current power save level
+    currentCoexPeriodMs       - Period of awakening for the firmware used when
+                                periodic traffic is detected.
+                                If the traffic is not periodic, it is set to 0.
+    currentCoexLatencyMs      - Period of awakening for the firmware used when
+                                non-periodic traffic is detected
+    hasBtDevice               - TRUE if there is a Bluetooth device connected
+    currentBlackoutDurationUs - Current blackout duration for protecting
+                                Bluetooth
+    currentBlackoutPeriodUs   - Current blackout period
+    currentCoexScheme         - Defines the scheme for the coexistence
+                                signalling
+
+*******************************************************************************/
+typedef struct
+{
+    u8                  hasTrafficData;
+    CsrWifiSmeTrafficType    currentTrafficType;
+    u16                currentPeriodMs;
+    CsrWifiSmePowerSaveLevel currentPowerSave;
+    u16                currentCoexPeriodMs;
+    u16                currentCoexLatencyMs;
+    u8                  hasBtDevice;
+    u32                currentBlackoutDurationUs;
+    u32                currentBlackoutPeriodUs;
+    CsrWifiSmeCoexScheme     currentCoexScheme;
+} CsrWifiSmeCoexInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionConfig
+
+  DESCRIPTION
+    Specifies the parameters that the SME should use in selecting a network.
+
+  MEMBERS
+    ssid
+                   - Service Set identifier
+    bssid
+                   - BSS identifier
+    bssType
+                   - Indicates the type of BSS
+    ifIndex
+                   - Indicates the radio interface
+    privacyMode
+                   - Specifies whether the privacy mode is enabled or disabled.
+    authModeMask
+                   - Sets the authentication options that the SME can use while
+                     associating to the AP
+                     Set mask with values from CsrWifiSmeAuthMode
+    encryptionModeMask
+                   - Sets the encryption options that the SME can use while
+                     associating to the AP
+                     Set mask with values from CsrWifiSmeEncryption
+    mlmeAssociateReqInformationElementsLength
+                   - Length in bytes of information elements to be sent in the
+                     Association Request.
+    mlmeAssociateReqInformationElements
+                   - Points to the first byte of the information elements, if
+                     any.
+    wmmQosInfo
+                   - This parameter allows the driver's WMM behaviour to be
+                     configured.
+                     To enable support for WMM, use
+                     CSR_WIFI_SME_SME_CONFIG_SET_REQ with the
+                     CSR_WIFI_SME_WMM_MODE_AC_ENABLED bit set in wmmModeMask
+                     field in smeConfig parameter.
+                     Set mask with values from CsrWifiSmeWmmQosInfo
+    adhocJoinOnly
+                   - This parameter is relevant only if bssType is NOT set to
+                     CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE:
+                     if TRUE the SME will only try to join an ad-hoc network if
+                     there is one already established;
+                     if FALSE the SME will try to join an ad-hoc network if
+                     there is one already established or it will try to
+                     establish a new one
+    adhocChannel
+                   - This parameter is relevant only if bssType is NOT set to
+                     CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE:
+                     it indicates the channel to use joining an ad hoc network.
+                     Setting this to 0 causes the SME to select a channel from
+                     those permitted in the regulatory domain.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSsid                ssid;
+    CsrWifiMacAddress          bssid;
+    CsrWifiSmeBssType          bssType;
+    CsrWifiSmeRadioIF          ifIndex;
+    CsrWifiSme80211PrivacyMode privacyMode;
+    CsrWifiSmeAuthModeMask     authModeMask;
+    CsrWifiSmeEncryptionMask   encryptionModeMask;
+    u16                  mlmeAssociateReqInformationElementsLength;
+    u8                  *mlmeAssociateReqInformationElements;
+    CsrWifiSmeWmmQosInfoMask   wmmQosInfo;
+    u8                    adhocJoinOnly;
+    u8                   adhocChannel;
+} CsrWifiSmeConnectionConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionInfo
+
+  DESCRIPTION
+    Parameters that the SME should use in selecting a network
+
+  MEMBERS
+    ssid                        - Service set identifier
+    bssid                       - BSS identifier
+    networkType80211            - Physical layer used for the connection
+    channelNumber               - Channel number
+    channelFrequency            - Channel frequency
+    authMode                    - Authentication mode used for the connection
+    pairwiseCipher              - Encryption type for peer to peer communication
+    groupCipher                 - Encryption type for broadcast and multicast
+                                  communication
+    ifIndex                     - Indicates the radio interface
+    atimWindowTu                - ATIM window specified for IBSS
+    beaconPeriodTu              - Interval between beacon packets
+    reassociation               - Indicates whether a reassociation occurred
+    beaconFrameLength           - Indicates the number of bytes of the beacon
+                                  frame
+    beaconFrame                 - Points at the first byte of the beacon frame
+    associationReqFrameLength   - Indicates the number of bytes of the
+                                  association request frame
+    associationReqFrame         - Points at the first byte of the association
+                                  request frame
+    associationRspFrameLength   - Indicates the number of bytes of the
+                                  association response frame
+    associationRspFrame         - Points at the first byte of the association
+                                  response frame
+    assocScanInfoElementsLength - Indicates the number of bytes in the buffer
+                                  pointed by assocScanInfoElements
+    assocScanInfoElements       - Pointer to the buffer containing the
+                                  information elements of the probe response
+                                  received after the probe requests sent before
+                                  attempting to authenticate to the network
+    assocReqCapabilities        - Reports the content of the Capability
+                                  information element as specified in the
+                                  association request.
+    assocReqListenIntervalTu    - Listen Interval specified in the association
+                                  request
+    assocReqApAddress           - AP address to which the association requests
+                                  has been sent
+    assocReqInfoElementsLength  - Indicates the number of bytes of the
+                                  association request information elements
+    assocReqInfoElements        - Points at the first byte of the association
+                                  request information elements
+    assocRspResult              - Result reported in the association response
+    assocRspCapabilityInfo      - Reports the content of the Capability
+                                  information element as received in the
+                                  association response.
+    assocRspAssociationId       - Reports the association ID received in the
+                                  association response.
+    assocRspInfoElementsLength  - Indicates the number of bytes of the
+                                  association response information elements
+    assocRspInfoElements        - Points at the first byte of the association
+                                  response information elements
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSsid                ssid;
+    CsrWifiMacAddress          bssid;
+    CsrWifiSme80211NetworkType networkType80211;
+    u8                   channelNumber;
+    u16                  channelFrequency;
+    CsrWifiSmeAuthMode         authMode;
+    CsrWifiSmeEncryption       pairwiseCipher;
+    CsrWifiSmeEncryption       groupCipher;
+    CsrWifiSmeRadioIF          ifIndex;
+    u16                  atimWindowTu;
+    u16                  beaconPeriodTu;
+    u8                    reassociation;
+    u16                  beaconFrameLength;
+    u8                  *beaconFrame;
+    u16                  associationReqFrameLength;
+    u8                  *associationReqFrame;
+    u16                  associationRspFrameLength;
+    u8                  *associationRspFrame;
+    u16                  assocScanInfoElementsLength;
+    u8                  *assocScanInfoElements;
+    u16                  assocReqCapabilities;
+    u16                  assocReqListenIntervalTu;
+    CsrWifiMacAddress          assocReqApAddress;
+    u16                  assocReqInfoElementsLength;
+    u8                  *assocReqInfoElements;
+    CsrWifiSmeIEEE80211Result  assocRspResult;
+    u16                  assocRspCapabilityInfo;
+    u16                  assocRspAssociationId;
+    u16                  assocRspInfoElementsLength;
+    u8                  *assocRspInfoElements;
+} CsrWifiSmeConnectionInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeviceConfig
+
+  DESCRIPTION
+    General configuration options in the SME
+
+  MEMBERS
+    trustLevel              - Level of trust of the information coming from the
+                              network
+    countryCode             - Country code as specified by IEEE 802.11 standard
+    firmwareDriverInterface - Specifies the type of communication between Host
+                              and Firmware
+    enableStrictDraftN      - If TRUE TKIP is disallowed when connecting to
+                              802.11n enabled access points
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSme80211dTrustLevel        trustLevel;
+    u8                          countryCode[2];
+    CsrWifiSmeFirmwareDriverInterface firmwareDriverInterface;
+    u8                           enableStrictDraftN;
+} CsrWifiSmeDeviceConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeviceInfo
+
+  DESCRIPTION
+    P2P Information for a P2P Device
+
+  MEMBERS
+    deviceAddress            - Device Address of the P2P device
+    configMethods            - Supported WPS configuration methods.
+    p2PDeviceCap             - P2P device capabilities
+    primDeviceType           - Primary WPS device type
+    secondaryDeviceTypeCount - Number of secondary device types
+    secDeviceType            - list of secondary WPS device types
+    deviceName               - Device name without up to 32 characters'\0'.
+    deviceNameLength         - Number of characters of the device name
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress           deviceAddress;
+    CsrWifiSmeWpsConfigTypeMask configMethods;
+    CsrWifiSmeP2pCapabilityMask p2PDeviceCap;
+    CsrWifiSmeWpsDeviceType     primDeviceType;
+    u8                    secondaryDeviceTypeCount;
+    CsrWifiSmeWpsDeviceType    *secDeviceType;
+    u8                    deviceName[32];
+    u8                    deviceNameLength;
+} CsrWifiSmeDeviceInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeviceInfoCommon
+
+  DESCRIPTION
+    Structure holding device information.
+
+  MEMBERS
+    p2pDeviceAddress          -
+    primaryDeviceType         -
+    secondaryDeviceTypesCount -
+    secondaryDeviceTypes      -
+    deviceNameLength          -
+    deviceName                -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress             p2pDeviceAddress;
+    CsrWifiSmeWpsDeviceTypeCommon primaryDeviceType;
+    u8                      secondaryDeviceTypesCount;
+    u8                      secondaryDeviceTypes[10];
+    u8                      deviceNameLength;
+    u8                      deviceName[32];
+} CsrWifiSmeDeviceInfoCommon;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfig
+
+  DESCRIPTION
+    Defines the host power state (for example, on mains power, on battery
+    power etc) and the periodicity of the traffic data.
+
+  MEMBERS
+    powerMode               - The wireless manager application should use the
+                              powerMode parameter to inform the SME of the host
+                              power state.
+    applicationDataPeriodMs - The applicationDataPeriodMs parameter allows a
+                              wireless manager application to inform the SME
+                              that an application is running that generates
+                              periodic network traffic and the period of the
+                              traffic.
+                              An example of such an application is a VoIP client.
+                              The wireless manager application should set
+                              applicationDataPeriodMs to the period in
+                              milliseconds between data packets or zero if no
+                              periodic application is running.
+                              Voip etc 0 = No Periodic Data
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeHostPowerMode powerMode;
+    u16               applicationDataPeriodMs;
+} CsrWifiSmeHostConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeKey
+
+  DESCRIPTION
+    Information for a key to be used for encryption
+
+  MEMBERS
+    keyType       - Specifies whether the key is a pairwise or group key; it
+                    should be set to CSR_WIFI_SME_GROUP_KEY or
+                    CSR_WIFI_SME_PAIRWISE_KEY, as required.
+    keyIndex      - Specifies which WEP key (0-3) to set; it should be set to 0
+                    for a WPA/WPA2 pairwise key and non-zero for a WPA/WPA2
+                    group key.
+    wepTxKey      - If wepTxKey is TRUE, and the key is a WEP key, the key will
+                    be selected for encrypting transmitted packets.
+                    To select a previously defined key as the transmit
+                    encryption key, set keyIndex to the required key, wepTxKey
+                    to TRUE and the keyLength to 0.
+    keyRsc        - Key Receive Sequence Counter
+    authenticator - If TRUE the WMA will act as authenticator.
+                    CURRENTLY NOT SUPPORTED
+    address       - BSS identifier of the AP
+    keyLength     - Length of the key in bytes
+    key           - Points to the first byte of the key
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeKeyType keyType;
+    u8          keyIndex;
+    u8           wepTxKey;
+    u16         keyRsc[8];
+    u8           authenticator;
+    CsrWifiMacAddress address;
+    u8          keyLength;
+    u8          key[32];
+} CsrWifiSmeKey;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pClientInfoType
+
+  DESCRIPTION
+    P2P Information for a P2P Client
+
+  MEMBERS
+    p2PClientInterfaceAddress - MAC address of the P2P Client
+    clientDeviceInfo          - Device Information
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiMacAddress    p2PClientInterfaceAddress;
+    CsrWifiSmeDeviceInfo clientDeviceInfo;
+} CsrWifiSmeP2pClientInfoType;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeP2pGroupInfo
+
+  DESCRIPTION
+    P2P Information for a P2P Group
+
+  MEMBERS
+    groupCapability    - P2P group capabilities
+    p2pDeviceAddress   - Device Address of the GO
+    p2pClientInfoCount - Number of P2P Clients that belong to the group.
+    p2PClientInfo      - Pointer to the list containing client information for
+                         each client in the group
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeP2pGroupCapabilityMask groupCapability;
+    CsrWifiMacAddress                p2pDeviceAddress;
+    u8                         p2pClientInfoCount;
+    CsrWifiSmeP2pClientInfoType     *p2PClientInfo;
+} CsrWifiSmeP2pGroupInfo;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfig
+
+  DESCRIPTION
+    Configures the power-save behaviour of the driver and firmware.
+
+  MEMBERS
+    powerSaveLevel         - Power Save Level option
+    listenIntervalTu       - Interval for waking to receive beacon frames
+    rxDtims                - If TRUE, wake for DTIM every beacon period, to
+                             allow the reception broadcast packets
+    d3AutoScanMode         - Defines whether the autonomous scanning will be
+                             turned off or will stay on during a D3 suspended
+                             period
+    clientTrafficWindow    - Deprecated
+    opportunisticPowerSave - Deprecated
+    noticeOfAbsence        - Deprecated
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmePowerSaveLevel powerSaveLevel;
+    u16                listenIntervalTu;
+    u8                  rxDtims;
+    CsrWifiSmeD3AutoScanMode d3AutoScanMode;
+    u8                 clientTrafficWindow;
+    u8                  opportunisticPowerSave;
+    u8                  noticeOfAbsence;
+} CsrWifiSmePowerConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfig
+
+  DESCRIPTION
+    Configures the roaming behaviour of the driver and firmware
+
+  MEMBERS
+    roamingBands             - Defines the thresholds to determine the usability
+                               level of the current connection.
+                               roamingBands is indexed by the first 3 entries of
+                               the CsrWifiSmeBasicUsability enum
+    disableSmoothRoaming     - Disable the RSSI/SNR triggers from the Firmware
+                               that the SME uses to detect the quality of the
+                               connection.
+                               This implicitly disables disableRoamScans
+    disableRoamScans         - Disables the scanning for the roaming operation
+    reconnectLimit           - Maximum number of times SME may reconnect in the
+                               given interval
+    reconnectLimitIntervalMs - Interval for maximum number of times SME may
+                               reconnect to the same Access Point
+    roamScanCfg              - Scanning behaviour for the specifically aimed at
+                               improving roaming performance.
+                               roamScanCfg is indexed by the first 3 entries of
+                               the CsrWifiSmeBasicUsability enum
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeRoamingBandData roamingBands[3];
+    u8                   disableSmoothRoaming;
+    u8                   disableRoamScans;
+    u8                  reconnectLimit;
+    u16                 reconnectLimitIntervalMs;
+    CsrWifiSmeScanConfigData  roamScanCfg[3];
+} CsrWifiSmeRoamingConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfig
+
+  DESCRIPTION
+    Parameters for the autonomous scanning behaviour of the system
+
+  MEMBERS
+    scanCfg                 - Scan configuration data.
+                              Indexed by the CsrWifiSmeBasicUsability enum
+    disableAutonomousScans  - Enables or disables the autonomous scan
+    maxResults              - Maximum number of results to be cached in the SME
+    highRssiThreshold       - High received signal strength indication threshold
+                              in dBm for an AP above which the system will
+                              report scan indications
+    lowRssiThreshold        - Low received signal strength indication threshold
+                              in dBm for an AP below which the system will
+                              report scan indications
+    deltaRssiThreshold      - Minimum difference for received signal strength
+                              indication in dBm for an AP which trigger a scan
+                              indication to be sent.
+    highSnrThreshold        - High Signal to Noise Ratio threshold in dB for an
+                              AP above which the system will report scan
+                              indications
+    lowSnrThreshold         - Low Signal to Noise Ratio threshold in dB for an
+                              AP below which the system will report scan
+                              indications
+    deltaSnrThreshold       - Minimum difference for Signal to Noise Ratio in dB
+                              for an AP which trigger a scan indication to be
+                              sent.
+    passiveChannelListCount - Number of channels to be scanned passively.
+    passiveChannelList      - Points to the first channel to be scanned
+                              passively , if any.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeScanConfigData scanCfg[4];
+    u8                  disableAutonomousScans;
+    u16                maxResults;
+    s8                  highRssiThreshold;
+    s8                  lowRssiThreshold;
+    s8                  deltaRssiThreshold;
+    s8                  highSnrThreshold;
+    s8                  lowSnrThreshold;
+    s8                  deltaSnrThreshold;
+    u16                passiveChannelListCount;
+    u8                *passiveChannelList;
+} CsrWifiSmeScanConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResult
+
+  DESCRIPTION
+    This structure defines the scan result for each BSS found
+
+  MEMBERS
+    ssid                         - Service set identifier
+    bssid                        - BSS identifier
+    rssi                         - Received signal strength indication in dBm
+    snr                          - Signal to noise ratio in dB
+    ifIndex                      - Indicates the radio interface
+    beaconPeriodTu               - Interval between beacon frames
+    timeStamp                    - Timestamp in the BSS
+    localTime                    - Timestamp in the Access Point
+    channelFrequency             - Channel frequency
+    capabilityInformation        - Capabilities of the BSS.
+    channelNumber                - Channel number
+    usability                    - Indicates the usability level.
+    bssType                      - Type of BSS.
+    informationElementsLength    - Number of bytes of the information elements
+                                   received as part of the beacon or probe
+                                   response.
+    informationElements          - Points to the first byte of the IEs received
+                                   as part of the beacon or probe response.
+                                   The format of the IEs is as specified in the
+                                   IEEE 802.11 specification.
+    p2pDeviceRole                - Role of the P2P device.
+                                   Relevant only if bssType is
+                                   CSR_WIFI_SME_BSS_TYPE_P2P
+    deviceInfo                   - Union containing P2P device info which
+                                   depends on p2pDeviceRole parameter.
+    deviceInforeservedCli        -
+    deviceInfogroupInfo          -
+    deviceInforeservedNone       -
+    deviceInfostandalonedevInfo  -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSsid              ssid;
+    CsrWifiMacAddress        bssid;
+    s16                 rssi;
+    s16                 snr;
+    CsrWifiSmeRadioIF        ifIndex;
+    u16                beaconPeriodTu;
+    CsrWifiSmeTsfTime        timeStamp;
+    CsrWifiSmeTsfTime        localTime;
+    u16                channelFrequency;
+    u16                capabilityInformation;
+    u8                 channelNumber;
+    CsrWifiSmeBasicUsability usability;
+    CsrWifiSmeBssType        bssType;
+    u16                informationElementsLength;
+    u8                *informationElements;
+    CsrWifiSmeP2pRole        p2pDeviceRole;
+    union {
+        CsrWifiSmeEmpty        reservedCli;
+        CsrWifiSmeP2pGroupInfo groupInfo;
+        CsrWifiSmeEmpty        reservedNone;
+        CsrWifiSmeDeviceInfo   standalonedevInfo;
+    } deviceInfo;
+} CsrWifiSmeScanResult;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeStaConfig
+
+  DESCRIPTION
+    Station configuration options in the SME
+
+  MEMBERS
+    connectionQualityRssiChangeTrigger - Sets the difference of RSSI
+                                         measurements which triggers reports
+                                         from the Firmware
+    connectionQualitySnrChangeTrigger  - Sets the difference of SNR measurements
+                                         which triggers reports from the
+                                         Firmware
+    wmmModeMask                        - Mask containing one or more values from
+                                         CsrWifiSmeWmmMode
+    ifIndex                            - Indicates the band of frequencies used
+    allowUnicastUseGroupCipher         - If TRUE, it allows to use groupwise
+                                         keys if no pairwise key is specified
+    enableOpportunisticKeyCaching      - If TRUE, enables the Opportunistic Key
+                                         Caching feature
+
+*******************************************************************************/
+typedef struct
+{
+    u8              connectionQualityRssiChangeTrigger;
+    u8              connectionQualitySnrChangeTrigger;
+    CsrWifiSmeWmmModeMask wmmModeMask;
+    CsrWifiSmeRadioIF     ifIndex;
+    u8               allowUnicastUseGroupCipher;
+    u8               enableOpportunisticKeyCaching;
+} CsrWifiSmeStaConfig;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWep128Keys
+
+  DESCRIPTION
+    Structure holding WEP Authentication Type and WEP keys that can be used
+    when using WEP128.
+
+  MEMBERS
+    wepAuthType    - Mask to select the WEP authentication type (Open or Shared)
+    selectedWepKey - Index to one of the four keys below indicating the
+                     currently used WEP key. Mapping From SME/User -> firmware.
+                     Key 1 -> Index 0. Key 2 -> Index 1. key 3 -> Index 2. Key
+                     4-> Index 3.
+    key1           - Value for key number 1.
+    key2           - Value for key number 2.
+    key3           - Value for key number 3.
+    key4           - Value for key number 4.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeWepAuthMode wepAuthType;
+    u8              selectedWepKey;
+    u8              key1[13];
+    u8              key2[13];
+    u8              key3[13];
+    u8              key4[13];
+} CsrWifiSmeWep128Keys;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWep64Keys
+
+  DESCRIPTION
+    Structure holding WEP Authentication Type and WEP keys that can be used
+    when using WEP64.
+
+  MEMBERS
+    wepAuthType    - Mask to select the WEP authentication type (Open or Shared)
+    selectedWepKey - Index to one of the four keys below indicating the
+                     currently used WEP key. Mapping From SME/User -> firmware.
+                     Key 1 -> Index 0. Key 2 -> Index 1. key 3 -> Index 2. Key
+                     4-> Index 3.
+    key1           - Value for key number 1.
+    key2           - Value for key number 2.
+    key3           - Value for key number 3.
+    key4           - Value for key number 4.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeWepAuthMode wepAuthType;
+    u8              selectedWepKey;
+    u8              key1[5];
+    u8              key2[5];
+    u8              key3[5];
+    u8              key4[5];
+} CsrWifiSmeWep64Keys;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWepAuth
+
+  DESCRIPTION
+    WEP authentication parameter structure
+
+  MEMBERS
+    wepKeyType               - WEP key try (128 bit or 64 bit)
+    wepCredentials           - Union containing credentials which depends on
+                               wepKeyType parameter.
+    wepCredentialswep128Key  -
+    wepCredentialswep64Key   -
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiSmeWepCredentialType wepKeyType;
+    union {
+        CsrWifiSmeWep128Keys wep128Key;
+        CsrWifiSmeWep64Keys  wep64Key;
+    } wepCredentials;
+} CsrWifiSmeWepAuth;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfig
+
+  DESCRIPTION
+    Structure holding AP WPS Config data.
+
+  MEMBERS
+    wpsVersion               - wpsVersion should be 0x10 for WPS1.0h or 0x20 for
+                               WSC2.0
+    uuid                     - uuid.
+    deviceName               - Device name upto 32 characters without '\0'.
+    deviceNameLength         - deviceNameLen.
+    manufacturer             - manufacturer: CSR
+    manufacturerLength       - manufacturerLen.
+    modelName                - modelName Unifi
+    modelNameLength          - modelNameLen.
+    modelNumber              - modelNumber
+    modelNumberLength        - modelNumberLen.
+    serialNumber             - serialNumber
+    primDeviceType           - Primary WPS device type
+    secondaryDeviceTypeCount - Number of secondary device types
+    secondaryDeviceType      - list of secondary WPS device types
+    configMethods            - Supported WPS config methods
+    rfBands                  - RfBands.
+    osVersion                - Os version on which the device is running
+
+*******************************************************************************/
+typedef struct
+{
+    u8                    wpsVersion;
+    u8                    uuid[16];
+    u8                    deviceName[32];
+    u8                    deviceNameLength;
+    u8                    manufacturer[64];
+    u8                    manufacturerLength;
+    u8                    modelName[32];
+    u8                    modelNameLength;
+    u8                    modelNumber[32];
+    u8                    modelNumberLength;
+    u8                    serialNumber[32];
+    CsrWifiSmeWpsDeviceType     primDeviceType;
+    u8                    secondaryDeviceTypeCount;
+    CsrWifiSmeWpsDeviceType    *secondaryDeviceType;
+    CsrWifiSmeWpsConfigTypeMask configMethods;
+    u8                    rfBands;
+    u8                    osVersion[4];
+} CsrWifiSmeWpsConfig;
+
+
+/* Downstream */
+#define CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST            (0x0000)
+
+#define CSR_WIFI_SME_ACTIVATE_REQ                         ((CsrWifiSmePrim) (0x0000 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_ADHOC_CONFIG_GET_REQ                 ((CsrWifiSmePrim) (0x0001 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_ADHOC_CONFIG_SET_REQ                 ((CsrWifiSmePrim) (0x0002 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_BLACKLIST_REQ                        ((CsrWifiSmePrim) (0x0003 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CALIBRATION_DATA_GET_REQ             ((CsrWifiSmePrim) (0x0004 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CALIBRATION_DATA_SET_REQ             ((CsrWifiSmePrim) (0x0005 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CCX_CONFIG_GET_REQ                   ((CsrWifiSmePrim) (0x0006 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CCX_CONFIG_SET_REQ                   ((CsrWifiSmePrim) (0x0007 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_COEX_CONFIG_GET_REQ                  ((CsrWifiSmePrim) (0x0008 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_COEX_CONFIG_SET_REQ                  ((CsrWifiSmePrim) (0x0009 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_COEX_INFO_GET_REQ                    ((CsrWifiSmePrim) (0x000A + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECT_REQ                          ((CsrWifiSmePrim) (0x000B + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_CONFIG_GET_REQ            ((CsrWifiSmePrim) (0x000C + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_INFO_GET_REQ              ((CsrWifiSmePrim) (0x000D + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_STATS_GET_REQ             ((CsrWifiSmePrim) (0x000E + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_DEACTIVATE_REQ                       ((CsrWifiSmePrim) (0x000F + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_DISCONNECT_REQ                       ((CsrWifiSmePrim) (0x0010 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_EVENT_MASK_SET_REQ                   ((CsrWifiSmePrim) (0x0011 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_HOST_CONFIG_GET_REQ                  ((CsrWifiSmePrim) (0x0012 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_HOST_CONFIG_SET_REQ                  ((CsrWifiSmePrim) (0x0013 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_KEY_REQ                              ((CsrWifiSmePrim) (0x0014 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_LINK_QUALITY_GET_REQ                 ((CsrWifiSmePrim) (0x0015 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_CONFIG_GET_REQ                   ((CsrWifiSmePrim) (0x0016 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_CONFIG_SET_REQ                   ((CsrWifiSmePrim) (0x0017 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_GET_NEXT_REQ                     ((CsrWifiSmePrim) (0x0018 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_GET_REQ                          ((CsrWifiSmePrim) (0x0019 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_SET_REQ                          ((CsrWifiSmePrim) (0x001A + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_MULTICAST_ADDRESS_REQ                ((CsrWifiSmePrim) (0x001B + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_PACKET_FILTER_SET_REQ                ((CsrWifiSmePrim) (0x001C + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_PERMANENT_MAC_ADDRESS_GET_REQ        ((CsrWifiSmePrim) (0x001D + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_PMKID_REQ                            ((CsrWifiSmePrim) (0x001E + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_POWER_CONFIG_GET_REQ                 ((CsrWifiSmePrim) (0x001F + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_POWER_CONFIG_SET_REQ                 ((CsrWifiSmePrim) (0x0020 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_INFO_GET_REQ       ((CsrWifiSmePrim) (0x0021 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_ROAMING_CONFIG_GET_REQ               ((CsrWifiSmePrim) (0x0022 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_ROAMING_CONFIG_SET_REQ               ((CsrWifiSmePrim) (0x0023 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_CONFIG_GET_REQ                  ((CsrWifiSmePrim) (0x0024 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_CONFIG_SET_REQ                  ((CsrWifiSmePrim) (0x0025 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_FULL_REQ                        ((CsrWifiSmePrim) (0x0026 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_RESULTS_FLUSH_REQ               ((CsrWifiSmePrim) (0x0027 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_RESULTS_GET_REQ                 ((CsrWifiSmePrim) (0x0028 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_STA_CONFIG_GET_REQ               ((CsrWifiSmePrim) (0x0029 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_STA_CONFIG_SET_REQ               ((CsrWifiSmePrim) (0x002A + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_STATION_MAC_ADDRESS_GET_REQ          ((CsrWifiSmePrim) (0x002B + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_TSPEC_REQ                            ((CsrWifiSmePrim) (0x002C + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_VERSIONS_GET_REQ                     ((CsrWifiSmePrim) (0x002D + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_FLIGHTMODE_REQ                  ((CsrWifiSmePrim) (0x002E + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_OFF_REQ                         ((CsrWifiSmePrim) (0x002F + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_ON_REQ                          ((CsrWifiSmePrim) (0x0030 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CLOAKED_SSIDS_SET_REQ                ((CsrWifiSmePrim) (0x0031 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_CLOAKED_SSIDS_GET_REQ                ((CsrWifiSmePrim) (0x0032 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_COMMON_CONFIG_GET_REQ            ((CsrWifiSmePrim) (0x0033 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_COMMON_CONFIG_SET_REQ            ((CsrWifiSmePrim) (0x0034 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_INTERFACE_CAPABILITY_GET_REQ         ((CsrWifiSmePrim) (0x0035 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_WPS_CONFIGURATION_REQ                ((CsrWifiSmePrim) (0x0036 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+#define CSR_WIFI_SME_SET_REQ                              ((CsrWifiSmePrim) (0x0037 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST))
+
+
+#define CSR_WIFI_SME_PRIM_DOWNSTREAM_HIGHEST           (0x0037 + CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST)
+
+/* Upstream */
+#define CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST              (0x0000 + CSR_PRIM_UPSTREAM)
+
+#define CSR_WIFI_SME_ACTIVATE_CFM                         ((CsrWifiSmePrim)(0x0000 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ADHOC_CONFIG_GET_CFM                 ((CsrWifiSmePrim)(0x0001 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ADHOC_CONFIG_SET_CFM                 ((CsrWifiSmePrim)(0x0002 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ASSOCIATION_COMPLETE_IND             ((CsrWifiSmePrim)(0x0003 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ASSOCIATION_START_IND                ((CsrWifiSmePrim)(0x0004 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_BLACKLIST_CFM                        ((CsrWifiSmePrim)(0x0005 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CALIBRATION_DATA_GET_CFM             ((CsrWifiSmePrim)(0x0006 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CALIBRATION_DATA_SET_CFM             ((CsrWifiSmePrim)(0x0007 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CCX_CONFIG_GET_CFM                   ((CsrWifiSmePrim)(0x0008 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CCX_CONFIG_SET_CFM                   ((CsrWifiSmePrim)(0x0009 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_COEX_CONFIG_GET_CFM                  ((CsrWifiSmePrim)(0x000A + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_COEX_CONFIG_SET_CFM                  ((CsrWifiSmePrim)(0x000B + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_COEX_INFO_GET_CFM                    ((CsrWifiSmePrim)(0x000C + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECT_CFM                          ((CsrWifiSmePrim)(0x000D + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_CONFIG_GET_CFM            ((CsrWifiSmePrim)(0x000E + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_INFO_GET_CFM              ((CsrWifiSmePrim)(0x000F + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_QUALITY_IND               ((CsrWifiSmePrim)(0x0010 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CONNECTION_STATS_GET_CFM             ((CsrWifiSmePrim)(0x0011 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_DEACTIVATE_CFM                       ((CsrWifiSmePrim)(0x0012 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_DISCONNECT_CFM                       ((CsrWifiSmePrim)(0x0013 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_EVENT_MASK_SET_CFM                   ((CsrWifiSmePrim)(0x0014 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_HOST_CONFIG_GET_CFM                  ((CsrWifiSmePrim)(0x0015 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_HOST_CONFIG_SET_CFM                  ((CsrWifiSmePrim)(0x0016 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_IBSS_STATION_IND                     ((CsrWifiSmePrim)(0x0017 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_KEY_CFM                              ((CsrWifiSmePrim)(0x0018 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_LINK_QUALITY_GET_CFM                 ((CsrWifiSmePrim)(0x0019 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MEDIA_STATUS_IND                     ((CsrWifiSmePrim)(0x001A + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_CONFIG_GET_CFM                   ((CsrWifiSmePrim)(0x001B + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_CONFIG_SET_CFM                   ((CsrWifiSmePrim)(0x001C + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_GET_CFM                          ((CsrWifiSmePrim)(0x001D + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_GET_NEXT_CFM                     ((CsrWifiSmePrim)(0x001E + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIB_SET_CFM                          ((CsrWifiSmePrim)(0x001F + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MIC_FAILURE_IND                      ((CsrWifiSmePrim)(0x0020 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_MULTICAST_ADDRESS_CFM                ((CsrWifiSmePrim)(0x0021 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_PACKET_FILTER_SET_CFM                ((CsrWifiSmePrim)(0x0022 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_PERMANENT_MAC_ADDRESS_GET_CFM        ((CsrWifiSmePrim)(0x0023 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_PMKID_CANDIDATE_LIST_IND             ((CsrWifiSmePrim)(0x0024 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_PMKID_CFM                            ((CsrWifiSmePrim)(0x0025 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_POWER_CONFIG_GET_CFM                 ((CsrWifiSmePrim)(0x0026 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_POWER_CONFIG_SET_CFM                 ((CsrWifiSmePrim)(0x0027 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_REGULATORY_DOMAIN_INFO_GET_CFM       ((CsrWifiSmePrim)(0x0028 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ROAM_COMPLETE_IND                    ((CsrWifiSmePrim)(0x0029 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ROAM_START_IND                       ((CsrWifiSmePrim)(0x002A + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ROAMING_CONFIG_GET_CFM               ((CsrWifiSmePrim)(0x002B + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ROAMING_CONFIG_SET_CFM               ((CsrWifiSmePrim)(0x002C + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_CONFIG_GET_CFM                  ((CsrWifiSmePrim)(0x002D + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_CONFIG_SET_CFM                  ((CsrWifiSmePrim)(0x002E + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_FULL_CFM                        ((CsrWifiSmePrim)(0x002F + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_RESULT_IND                      ((CsrWifiSmePrim)(0x0030 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_RESULTS_FLUSH_CFM               ((CsrWifiSmePrim)(0x0031 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SCAN_RESULTS_GET_CFM                 ((CsrWifiSmePrim)(0x0032 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_STA_CONFIG_GET_CFM               ((CsrWifiSmePrim)(0x0033 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_STA_CONFIG_SET_CFM               ((CsrWifiSmePrim)(0x0034 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_STATION_MAC_ADDRESS_GET_CFM          ((CsrWifiSmePrim)(0x0035 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_TSPEC_IND                            ((CsrWifiSmePrim)(0x0036 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_TSPEC_CFM                            ((CsrWifiSmePrim)(0x0037 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_VERSIONS_GET_CFM                     ((CsrWifiSmePrim)(0x0038 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_FLIGHTMODE_CFM                  ((CsrWifiSmePrim)(0x0039 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_OFF_IND                         ((CsrWifiSmePrim)(0x003A + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_OFF_CFM                         ((CsrWifiSmePrim)(0x003B + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_ON_CFM                          ((CsrWifiSmePrim)(0x003C + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CLOAKED_SSIDS_SET_CFM                ((CsrWifiSmePrim)(0x003D + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CLOAKED_SSIDS_GET_CFM                ((CsrWifiSmePrim)(0x003E + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_WIFI_ON_IND                          ((CsrWifiSmePrim)(0x003F + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_COMMON_CONFIG_GET_CFM            ((CsrWifiSmePrim)(0x0040 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_SME_COMMON_CONFIG_SET_CFM            ((CsrWifiSmePrim)(0x0041 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_INTERFACE_CAPABILITY_GET_CFM         ((CsrWifiSmePrim)(0x0042 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_ERROR_IND                            ((CsrWifiSmePrim)(0x0043 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_INFO_IND                             ((CsrWifiSmePrim)(0x0044 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_CORE_DUMP_IND                        ((CsrWifiSmePrim)(0x0045 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_AMP_STATUS_CHANGE_IND                ((CsrWifiSmePrim)(0x0046 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+#define CSR_WIFI_SME_WPS_CONFIGURATION_CFM                ((CsrWifiSmePrim)(0x0047 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST))
+
+#define CSR_WIFI_SME_PRIM_UPSTREAM_HIGHEST             (0x0047 + CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST)
+
+#define CSR_WIFI_SME_PRIM_DOWNSTREAM_COUNT             (CSR_WIFI_SME_PRIM_DOWNSTREAM_HIGHEST + 1 - CSR_WIFI_SME_PRIM_DOWNSTREAM_LOWEST)
+#define CSR_WIFI_SME_PRIM_UPSTREAM_COUNT               (CSR_WIFI_SME_PRIM_UPSTREAM_HIGHEST   + 1 - CSR_WIFI_SME_PRIM_UPSTREAM_LOWEST)
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeActivateReq
+
+  DESCRIPTION
+    The WMA sends this primitive to activate the SME.
+    The WMA must activate the SME before it can send any other primitive.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeActivateReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the adHocConfig parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeAdhocConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the adHocConfig parameter.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    adHocConfig - Sets the values to use when starting an ad hoc network.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrWifiSmeAdHocConfig adHocConfig;
+} CsrWifiSmeAdhocConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeBlacklistReq
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to notify the
+    driver of any networks that should not be connected to. The interface
+    allows the wireless manager application to query, add, remove, and flush
+    the BSSIDs that the driver may not connect or roam to.
+    When this primitive adds to the black list the BSSID to which the SME is
+    currently connected, the SME will try to roam, if applicable, to another
+    BSSID in the same ESS; if the roaming procedure fails, the SME will
+    disconnect.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    action          - The value of the CsrWifiSmeListAction parameter instructs
+                      the driver to modify or provide the list of blacklisted
+                      networks.
+    setAddressCount - Number of BSSIDs sent with this primitive
+    setAddresses    - Pointer to the list of BBSIDs sent with the primitive, set
+                      to NULL if none is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrWifiSmeListAction action;
+    u8             setAddressCount;
+    CsrWifiMacAddress   *setAddresses;
+} CsrWifiSmeBlacklistReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataGetReq
+
+  DESCRIPTION
+    This primitive retrieves the Wi-Fi radio calibration data.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeCalibrationDataGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataSetReq
+
+  DESCRIPTION
+    This primitive sets the Wi-Fi radio calibration data.
+    The usage of the primitive with proper calibration data will avoid
+    time-consuming configuration after power-up.
+
+  MEMBERS
+    common                - Common header for use with the CsrWifiFsm Module
+    calibrationDataLength - Number of bytes in the buffer pointed by
+                            calibrationData
+    calibrationData       - Pointer to a buffer of length calibrationDataLength
+                            containing the calibration data
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       calibrationDataLength;
+    u8       *calibrationData;
+} CsrWifiSmeCalibrationDataSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the CcxConfig parameter.
+    CURRENTLY NOT SUPPORTED.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeCcxConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the CcxConfig parameter.
+    CURRENTLY NOT SUPPORTED.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    ccxConfig    - Currently not supported
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    u16           interfaceTag;
+    CsrWifiSmeCcxConfig ccxConfig;
+} CsrWifiSmeCcxConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the CoexConfig parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeCoexConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the CoexConfig parameter.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    coexConfig - Configures the coexistence behaviour
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrWifiSmeCoexConfig coexConfig;
+} CsrWifiSmeCoexConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexInfoGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the CoexInfo parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeCoexInfoGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to start the
+    process of joining an 802.11 wireless network or to start an ad hoc
+    network.
+    The structure pointed by connectionConfig contains parameters describing
+    the network to join or, in case of an ad hoc network, to host or join.
+    The SME will select a network, perform the IEEE 802.11 Join, Authenticate
+    and Associate exchanges.
+    The SME selects the networks from the current scan list that match both
+    the SSID and BSSID, however either or both of these may be the wildcard
+    value. Using this rule, the following operations are possible:
+      * To connect to a network by name, specify the SSID and set the BSSID to
+        0xFF 0xFF 0xFF 0xFF 0xFF 0xFF. If there are two or more networks visible,
+        the SME will select the one with the strongest signal.
+      * To connect to a specific network, specify the BSSID. The SSID is
+        optional, but if given it must match the SSID of the network. An empty
+        SSID may be specified by setting the SSID length to zero. Please note
+        that if the BSSID is specified (i.e. not equal to 0xFF 0xFF 0xFF 0xFF
+        0xFF 0xFF), the SME will not attempt to roam if signal conditions become
+        poor, even if there is an alternative AP with an SSID that matches the
+        current network SSID.
+      * To connect to any network matching the other parameters (i.e. security,
+        etc), set the SSID length to zero and set the BSSID to 0xFF 0xFF 0xFF
+        0xFF 0xFF 0xFF. In this case, the SME will order all available networks
+        by their signal strengths and will iterate through this list until it
+        successfully connects.
+    NOTE: Specifying the BSSID will restrict the selection to one specific
+    network. If SSID and BSSID are given, they must both match the network
+    for it to be selected. To select a network based on the SSID only, the
+    wireless manager application must set the BSSID to 0xFF 0xFF 0xFF 0xFF
+    0xFF 0xFF.
+    The SME will try to connect to each network that matches the provided
+    parameters, one by one, until it succeeds or has tried unsuccessfully
+    with all the matching networks.
+    If there is no network that matches the parameters and the request allows
+    to host an ad hoc network, the SME will advertise a new ad hoc network
+    instead.
+    If the SME cannot connect, it will notify the failure in the confirm.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    connectionConfig - Describes the candidate network to join or to host.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent            common;
+    u16                  interfaceTag;
+    CsrWifiSmeConnectionConfig connectionConfig;
+} CsrWifiSmeConnectReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the ConnectionConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeConnectionConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionInfoGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the ConnectionInfo parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeConnectionInfoGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionStatsGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the ConnectionStats parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeConnectionStatsGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeactivateReq
+
+  DESCRIPTION
+    The WMA sends this primitive to deactivate the SME.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeDeactivateReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDisconnectReq
+
+  DESCRIPTION
+    The wireless manager application may disconnect from the current network
+    by calling this primitive
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeDisconnectReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEventMaskSetReq
+
+  DESCRIPTION
+    The wireless manager application may register with the SME to receive
+    notification of interesting events. Indications will be sent only if the
+    wireless manager explicitly registers to be notified of that event.
+    indMask is a bit mask of values defined in CsrWifiSmeIndicationsMask.
+
+  MEMBERS
+    common  - Common header for use with the CsrWifiFsm Module
+    indMask - Set mask with values from CsrWifiSmeIndications
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    CsrWifiSmeIndicationsMask indMask;
+} CsrWifiSmeEventMaskSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the hostConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeHostConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the hostConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    hostConfig   - Communicates a change of host power state (for example, on
+                   mains power, on battery power etc) and of the periodicity of
+                   traffic data
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrWifiSmeHostConfig hostConfig;
+} CsrWifiSmeHostConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeKeyReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to add or remove
+    keys that the chip should use for encryption of data.
+    The interface allows the wireless manager application to add and remove
+    keys according to the specified action.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    action       - The value of the CsrWifiSmeListAction parameter instructs the
+                   driver to modify or provide the list of keys.
+                   CSR_WIFI_SME_LIST_ACTION_GET is not supported here.
+    key          - Key to be added or removed
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrWifiSmeListAction action;
+    CsrWifiSmeKey        key;
+} CsrWifiSmeKeyReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeLinkQualityGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the LinkQuality parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeLinkQualityGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the MibConfig parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeMibConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the MibConfig parameter.
+
+  MEMBERS
+    common    - Common header for use with the CsrWifiFsm Module
+    mibConfig - Conveys the desired value of various IEEE 802.11 attributes as
+                currently configured
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    CsrWifiSmeMibConfig mibConfig;
+} CsrWifiSmeMibConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetNextReq
+
+  DESCRIPTION
+    To read a sequence of MIB parameters, for example a table, call this
+    primitive to find the name of the next MIB variable
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to a VarBind or VarBindList containing the
+                         name(s) of the MIB variable(s) to search from.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       mibAttributeLength;
+    u8       *mibAttribute;
+} CsrWifiSmeMibGetNextReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to retrieve one or
+    more MIB variables.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to the VarBind or VarBindList containing the
+                         names of the MIB variables to be retrieved
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       mibAttributeLength;
+    u8       *mibAttribute;
+} CsrWifiSmeMibGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibSetReq
+
+  DESCRIPTION
+    The SME provides raw access to the MIB on the chip, which may be used by
+    some configuration or diagnostic utilities, but is not normally needed by
+    the wireless manager application.
+    The MIB access functions use BER encoded names (OID) of the MIB
+    parameters and BER encoded values, as described in the chip Host
+    Interface Protocol Specification.
+    The MIB parameters are described in 'Wi-Fi 5.0.0 Management Information
+    Base Reference Guide'.
+    The wireless manager application calls this primitive to set one or more
+    MIB variables
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to the VarBind or VarBindList containing the
+                         names and values of the MIB variables to set
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       mibAttributeLength;
+    u8       *mibAttribute;
+} CsrWifiSmeMibSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMulticastAddressReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to specify the
+    multicast addresses which the chip should recognise. The interface allows
+    the wireless manager application to query, add, remove and flush the
+    multicast addresses for the network interface according to the specified
+    action.
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    interfaceTag      - Interface Identifier; unique identifier of an interface
+    action            - The value of the CsrWifiSmeListAction parameter
+                        instructs the driver to modify or provide the list of
+                        MAC addresses.
+    setAddressesCount - Number of MAC addresses sent with the primitive
+    setAddresses      - Pointer to the list of MAC Addresses sent with the
+                        primitive, set to NULL if none is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrWifiSmeListAction action;
+    u8             setAddressesCount;
+    CsrWifiMacAddress   *setAddresses;
+} CsrWifiSmeMulticastAddressReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePacketFilterSetReq
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to enable or
+    disable filtering of broadcast packets: uninteresting broadcast packets
+    will be dropped by the Wi-Fi chip, instead of passing them up to the
+    host.
+    This has the advantage of saving power in the host application processor
+    as it removes the need to process unwanted packets.
+    All broadcast packets are filtered according to the filter and the filter
+    mode provided, except ARP packets, which are filtered using
+    arpFilterAddress.
+    Filters are not cumulative: only the parameters specified in the most
+    recent successful request are significant.
+    For more information, see 'UniFi Firmware API Specification'.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    filterLength     - Length of the filter in bytes.
+                       filterLength=0 disables the filter previously set
+    filter           - Points to the first byte of the filter provided, if any.
+                       This shall include zero or more instance of the
+                       information elements of one of these types
+                         * Traffic Classification (TCLAS) elements
+                         * WMM-SA TCLAS elements
+    mode             - Specifies whether the filter selects or excludes packets
+                       matching the filter
+    arpFilterAddress - IPv4 address to be used for filtering the ARP packets.
+                         * If the specified address is the IPv4 broadcast address
+                           (255.255.255.255), all ARP packets are reported to the
+                           host,
+                         * If the specified address is NOT the IPv4 broadcast
+                           address, only ARP packets with the specified address in
+                           the Source or Target Protocol Address fields are reported
+                           to the host
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent            common;
+    u16                  interfaceTag;
+    u16                  filterLength;
+    u8                  *filter;
+    CsrWifiSmePacketFilterMode mode;
+    CsrWifiIp4Address          arpFilterAddress;
+} CsrWifiSmePacketFilterSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePermanentMacAddressGetReq
+
+  DESCRIPTION
+    This primitive retrieves the MAC address stored in EEPROM
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmePermanentMacAddressGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to request an
+    operation on the SME PMKID list.
+    The action argument specifies the operation to perform.
+    When the connection is complete, the wireless manager application may
+    then send and receive EAPOL packets to complete WPA or WPA2
+    authentication if appropriate.
+    The wireless manager application can then pass the resulting encryption
+    keys using this primitive.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    action         - The value of the CsrWifiSmeListAction parameter instructs
+                     the driver to modify or provide the list of PMKIDs.
+    setPmkidsCount - Number of PMKIDs sent with the primitive
+    setPmkids      - Pointer to the list of PMKIDs sent with the primitive, set
+                     to NULL if none is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrWifiSmeListAction action;
+    u8             setPmkidsCount;
+    CsrWifiSmePmkid     *setPmkids;
+} CsrWifiSmePmkidReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the PowerConfig parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmePowerConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the PowerConfig parameter.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    powerConfig - Power saving configuration
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrWifiSmePowerConfig powerConfig;
+} CsrWifiSmePowerConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRegulatoryDomainInfoGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the RegulatoryDomainInfo parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeRegulatoryDomainInfoGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the RoamingConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeRoamingConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the RoamingConfig parameter.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    roamingConfig - Desired roaming behaviour values
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    CsrWifiSmeRoamingConfig roamingConfig;
+} CsrWifiSmeRoamingConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the ScanConfig parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeScanConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the ScanConfig parameter.
+    The SME normally configures the firmware to perform autonomous scanning
+    without involving the host.
+    The firmware passes beacon / probe response or indicates loss of beacon
+    on certain changes of state, for example:
+      * A new AP is seen for the first time
+      * An AP is no longer visible
+      * The signal strength of an AP changes by more than a certain amount, as
+        configured by the thresholds in the scanConfig parameter
+    In addition to the autonomous scan, the wireless manager application may
+    request a scan at any time using CSR_WIFI_SME_SCAN_FULL_REQ.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    scanConfig - Reports the configuration for the autonomous scanning behaviour
+                 of the firmware
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrWifiSmeScanConfig scanConfig;
+} CsrWifiSmeScanConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanFullReq
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to request a
+    full scan.
+    Channels are scanned actively or passively according to the requirement
+    set by regulatory domain.
+    If the SME receives this primitive while a full scan is going on, the new
+    request is buffered and it will be served after the current full scan is
+    completed.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    ssidCount        - Number of SSIDs provided.
+                       If it is 0, the SME will attempt to detect any network
+    ssid             - Points to the first SSID provided, if any.
+    bssid            - BSS identifier.
+                       If it is equal to FF-FF-FF-FF-FF, the SME will listen for
+                       messages from any BSS.
+                       If it is different from FF-FF-FF-FF-FF and any SSID is
+                       provided, one SSID must match the network of the BSS.
+    forceScan        - Forces the scan even if the SME is in a state which would
+                       normally prevent it (e.g. autonomous scan is running).
+    bssType          - Type of BSS to scan for
+    scanType         - Type of scan to perform
+    channelListCount - Number of channels provided.
+                       If it is 0, the SME will initiate a scan of all the
+                       supported channels that are permitted by the current
+                       regulatory domain.
+    channelList      - Points to the first channel , or NULL if channelListCount
+                       is zero.
+    probeIeLength    - Length of the information element in bytes to be sent
+                       with the probe message.
+    probeIe          - Points to the first byte of the information element to be
+                       sent with the probe message.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent    common;
+    u8           ssidCount;
+    CsrWifiSsid       *ssid;
+    CsrWifiMacAddress  bssid;
+    u8            forceScan;
+    CsrWifiSmeBssType  bssType;
+    CsrWifiSmeScanType scanType;
+    u16          channelListCount;
+    u8          *channelList;
+    u16          probeIeLength;
+    u8          *probeIe;
+} CsrWifiSmeScanFullReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsFlushReq
+
+  DESCRIPTION
+    The Wireless Manager calls this primitive to ask the SME to delete all
+    scan results from its cache, except for the scan result of any currently
+    connected network.
+    As scan results are received by the SME from the firmware, they are
+    cached in the SME memory.
+    Any time the Wireless Manager requests scan results, they are returned
+    from the SME internal cache.
+    For some applications it may be desirable to clear this cache prior to
+    requesting that a scan be performed; this will ensure that the cache then
+    only contains the networks detected in the most recent scan.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeScanResultsFlushReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsGetReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to retrieve the
+    current set of scan results, either after receiving a successful
+    CSR_WIFI_SME_SCAN_FULL_CFM, or to get autonomous scan results.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeScanResultsGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the SmeStaConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+} CsrWifiSmeSmeStaConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the SmeConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    smeConfig    - SME Station Parameters to be set
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    u16           interfaceTag;
+    CsrWifiSmeStaConfig smeConfig;
+} CsrWifiSmeSmeStaConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeStationMacAddressGetReq
+
+  DESCRIPTION
+    This primitives is used to retrieve the current MAC address used by the
+    station.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeStationMacAddressGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecReq
+
+  DESCRIPTION
+    The wireless manager application should call this primitive to use the
+    TSPEC feature.
+    The chip supports the use of TSPECs and TCLAS for the use of IEEE
+    802.11/WMM Quality of Service features.
+    The API allows the wireless manager application to supply a correctly
+    formatted TSPEC and TCLAS pair to the driver.
+    After performing basic validation, the driver negotiates the installation
+    of the TSPEC with the AP as defined by the 802.11 specification.
+    The driver retains all TSPEC and TCLAS pairs until they are specifically
+    removed.
+    It is not compulsory for a TSPEC to have a TCLAS (NULL is used to
+    indicate that no TCLAS is supplied), while a TCLASS always require a
+    TSPEC.
+    The format of the TSPEC element is specified in 'WMM (including WMM Power
+    Save) Specification - Version 1.1' and 'ANSI/IEEE Std 802.11-REVmb/D3.0'.
+    For more information, see 'UniFi Configuring WMM and WMM-PS'.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    action        - Specifies the action to be carried out on the list of TSPECs.
+                    CSR_WIFI_SME_LIST_ACTION_FLUSH is not applicable here.
+    transactionId - Unique Transaction ID for the TSPEC, as assigned by the
+                    driver
+    strict        - If it set to false, allows the SME to perform automatic
+                    TSPEC negotiation
+    ctrlMask      - Additional TSPEC configuration for CCX.
+                    Set mask with values from CsrWifiSmeTspecCtrl.
+                    CURRENTLY NOT SUPPORTED
+    tspecLength   - Length of the TSPEC.
+    tspec         - Points to the first byte of the TSPEC
+    tclasLength   - Length of the TCLAS.
+                    If it is equal to 0, no TCLASS is provided for the TSPEC
+    tclas         - Points to the first byte of the TCLAS, if any.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    CsrWifiSmeListAction    action;
+    u32               transactionId;
+    u8                 strict;
+    CsrWifiSmeTspecCtrlMask ctrlMask;
+    u16               tspecLength;
+    u8               *tspec;
+    u16               tclasLength;
+    u8               *tclas;
+} CsrWifiSmeTspecReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeVersionsGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the Versions parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeVersionsGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiFlightmodeReq
+
+  DESCRIPTION
+    The wireless manager application may call this primitive on boot-up of
+    the platform to ensure that the chip is placed in a mode that prevents
+    any emission of RF energy.
+    This primitive is an alternative to CSR_WIFI_SME_WIFI_ON_REQ.
+    As in CSR_WIFI_SME_WIFI_ON_REQ, it causes the download of the patch file
+    (if any) and the programming of the initial MIB settings (if supplied by
+    the WMA), but it also ensures that the chip is left in its lowest
+    possible power-mode with the radio subsystems disabled.
+    This feature is useful on platforms where power cannot be removed from
+    the chip (leaving the chip not initialised will cause it to consume more
+    power so calling this function ensures that the chip is initialised into
+    a low power mode but without entering a state where it could emit any RF
+    energy).
+    NOTE: this primitive does not cause the Wi-Fi to change state: Wi-Fi
+    stays conceptually off. Configuration primitives can be sent after
+    CSR_WIFI_SME_WIFI_FLIGHTMODE_REQ and the configuration will be maintained.
+    Requests that require the state of the Wi-Fi to be ON will return
+    CSR_WIFI_SME_STATUS_WIFI_OFF in their confirms.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    address       - Optionally specifies a station MAC address.
+                    In normal use, the manager should set the address to 0xFF
+                    0xFF 0xFF 0xFF 0xFF 0xFF, which will cause the chip to use
+                    the MAC address in the MIB.
+    mibFilesCount - Number of provided data blocks with initial MIB values
+    mibFiles      - Points to the first data block with initial MIB values.
+                    These data blocks are typically the contents of the provided
+                    files ufmib.dat and localmib.dat, available from the host
+                    file system, if they exist.
+                    These files typically contain radio tuning and calibration
+                    values.
+                    More values can be created using the Host Tools.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrWifiMacAddress    address;
+    u16            mibFilesCount;
+    CsrWifiSmeDataBlock *mibFiles;
+} CsrWifiSmeWifiFlightmodeReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOffReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to turn off the
+    chip, thus saving power when Wi-Fi is not in use.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeWifiOffReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOnReq
+
+  DESCRIPTION
+    The wireless manager application calls this primitive to turn on the
+    Wi-Fi chip.
+    If the Wi-Fi chip is currently off, the SME turns the Wi-Fi chip on,
+    downloads the patch file (if any), and programs the initial MIB settings
+    (if supplied by the WMA).
+    The patch file is not provided with the SME API; its downloading is
+    automatic and handled internally by the system.
+    The MIB settings, when provided, override the default values that the
+    firmware loads from EEPROM.
+    If the Wi-Fi chip is already on, the SME takes no action and returns a
+    successful status in the confirm.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    address       - Optionally specifies a station MAC address.
+                    In normal use, the manager should set the address to 0xFF
+                    0xFF 0xFF 0xFF 0xFF 0xFF, which will cause the chip to use
+                    the MAC address in the MIB
+    mibFilesCount - Number of provided data blocks with initial MIB values
+    mibFiles      - Points to the first data block with initial MIB values.
+                    These data blocks are typically the contents of the provided
+                    files ufmib.dat and localmib.dat, available from the host
+                    file system, if they exist.
+                    These files typically contain radio tuning and calibration
+                    values.
+                    More values can be created using the Host Tools.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrWifiMacAddress    address;
+    u16            mibFilesCount;
+    CsrWifiSmeDataBlock *mibFiles;
+} CsrWifiSmeWifiOnReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsSetReq
+
+  DESCRIPTION
+    This primitive sets the list of cloaked SSIDs for which the WMA possesses
+    profiles.
+    When the driver detects a cloaked AP, the SME will explicitly scan for it
+    using the list of cloaked SSIDs provided it, and, if the scan succeeds,
+    it will report the AP to the WMA either via CSR_WIFI_SME_SCAN_RESULT_IND
+    (if registered) or via CSR_WIFI_SCAN_RESULT_GET_CFM.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    cloakedSsids - Sets the list of cloaked SSIDs
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent             common;
+    CsrWifiSmeCloakedSsidConfig cloakedSsids;
+} CsrWifiSmeCloakedSsidsSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the CloakedSsids parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeCloakedSsidsGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigGetReq
+
+  DESCRIPTION
+    This primitive gets the value of the Sme common parameter.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeSmeCommonConfigGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigSetReq
+
+  DESCRIPTION
+    This primitive sets the value of the Sme common.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    deviceConfig - Configuration options in the SME
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent        common;
+    CsrWifiSmeDeviceConfig deviceConfig;
+} CsrWifiSmeSmeCommonConfigSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeInterfaceCapabilityGetReq
+
+  DESCRIPTION
+    The Wireless Manager calls this primitive to ask the SME for the
+    capabilities of the supported interfaces
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+} CsrWifiSmeInterfaceCapabilityGetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfigurationReq
+
+  DESCRIPTION
+    This primitive passes the WPS information for the device to SME. This may
+    be accepted only if no interface is active.
+
+  MEMBERS
+    common    - Common header for use with the CsrWifiFsm Module
+    wpsConfig - WPS config.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    CsrWifiSmeWpsConfig wpsConfig;
+} CsrWifiSmeWpsConfigurationReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSetReq
+
+  DESCRIPTION
+    Used to pass custom data to the SME. Format is the same as 802.11 Info
+    Elements => | Id | Length | Data
+    1) Cmanr Test Mode "Id:0 Length:1 Data:0x00 = OFF 0x01 = ON" "0x00 0x01
+    (0x00|0x01)"
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    dataLength - Number of bytes in the buffer pointed to by 'data'
+    data       - Pointer to the buffer containing 'dataLength' bytes
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u32       dataLength;
+    u8       *data;
+} CsrWifiSmeSetReq;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeActivateCfm
+
+  DESCRIPTION
+    The SME sends this primitive when the activation is complete.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeActivateCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    status      - Reports the result of the request
+    adHocConfig - Contains the values used when starting an Ad-hoc (IBSS)
+                  connection.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrResult             status;
+    CsrWifiSmeAdHocConfig adHocConfig;
+} CsrWifiSmeAdhocConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAdhocConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeAdhocConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAssociationCompleteInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it completes an attempt to associate with an AP. If
+    the association was successful, status will be set to
+    CSR_WIFI_SME_STATUS_SUCCESS, otherwise status and deauthReason shall be
+    set to appropriate error codes.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the association procedure
+    connectionInfo - This parameter is relevant only if result is
+                     CSR_WIFI_SME_STATUS_SUCCESS:
+                     it points to the connection information for the new network
+    deauthReason   - This parameter is relevant only if result is not
+                     CSR_WIFI_SME_STATUS_SUCCESS:
+                     if the AP deauthorised the station, it gives the reason of
+                     the deauthorization
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrResult                 status;
+    CsrWifiSmeConnectionInfo  connectionInfo;
+    CsrWifiSmeIEEE80211Reason deauthReason;
+} CsrWifiSmeAssociationCompleteInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAssociationStartInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it begins an attempt to associate with an AP.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    address      - BSSID of the associating network
+    ssid         - Service Set identifier of the associating network
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    CsrWifiMacAddress address;
+    CsrWifiSsid       ssid;
+} CsrWifiSmeAssociationStartInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeBlacklistCfm
+
+  DESCRIPTION
+    The SME will call this primitive when the action on the blacklist has
+    completed. For a GET action, this primitive also reports the list of
+    BBSIDs in the blacklist.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    status          - Reports the result of the request
+    action          - Action in the request
+    getAddressCount - This parameter is only relevant if action is
+                      CSR_WIFI_SME_LIST_ACTION_GET:
+                      number of BSSIDs sent with this primitive
+    getAddresses    - Pointer to the list of BBSIDs sent with the primitive, set
+                      to NULL if none is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrResult            status;
+    CsrWifiSmeListAction action;
+    u8             getAddressCount;
+    CsrWifiMacAddress   *getAddresses;
+} CsrWifiSmeBlacklistCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common                - Common header for use with the CsrWifiFsm Module
+    status                - Reports the result of the request
+    calibrationDataLength - Number of bytes in the buffer pointed by
+                            calibrationData
+    calibrationData       - Pointer to a buffer of length calibrationDataLength
+                            containing the calibration data
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+    u16       calibrationDataLength;
+    u8       *calibrationData;
+} CsrWifiSmeCalibrationDataGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCalibrationDataSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeCalibrationDataSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    ccxConfig    - Currently not supported
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    u16           interfaceTag;
+    CsrResult           status;
+    CsrWifiSmeCcxConfig ccxConfig;
+} CsrWifiSmeCcxConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCcxConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeCcxConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    status     - Reports the result of the request
+    coexConfig - Reports the parameters used to configure the coexistence
+                 behaviour
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrResult            status;
+    CsrWifiSmeCoexConfig coexConfig;
+} CsrWifiSmeCoexConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeCoexConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoexInfoGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common   - Common header for use with the CsrWifiFsm Module
+    status   - Reports the result of the request
+    coexInfo - Reports information and state related to coexistence.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent    common;
+    CsrResult          status;
+    CsrWifiSmeCoexInfo coexInfo;
+} CsrWifiSmeCoexInfoGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectCfm
+
+  DESCRIPTION
+    The SME calls this primitive when the connection exchange is complete or
+    all connection attempts fail.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request.
+                   CSR_WIFI_SME_STATUS_NOT_FOUND: all attempts by the SME to
+                   locate the requested AP failed
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeConnectCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    interfaceTag     - Interface Identifier; unique identifier of an interface
+    status           - Reports the result of the request
+    connectionConfig - Parameters used by the SME for selecting a network
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent            common;
+    u16                  interfaceTag;
+    CsrResult                  status;
+    CsrWifiSmeConnectionConfig connectionConfig;
+} CsrWifiSmeConnectionConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionInfoGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the request
+    connectionInfo - Information about the current connection
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent          common;
+    u16                interfaceTag;
+    CsrResult                status;
+    CsrWifiSmeConnectionInfo connectionInfo;
+} CsrWifiSmeConnectionInfoGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionQualityInd
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it whenever the value of the current connection quality
+    parameters change by more than a certain configurable amount.
+    The wireless manager application may configure the trigger thresholds for
+    this indication using the field in smeConfig parameter of
+    CSR_WIFI_SME_SME_CONFIG_SET_REQ.
+    Connection quality messages can be suppressed by setting both thresholds
+    to zero.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    linkQuality  - Indicates the quality of the link
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    u16             interfaceTag;
+    CsrWifiSmeLinkQuality linkQuality;
+} CsrWifiSmeConnectionQualityInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeConnectionStatsGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    status          - Reports the result of the request
+    connectionStats - Statistics for current connection.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrResult                 status;
+    CsrWifiSmeConnectionStats connectionStats;
+} CsrWifiSmeConnectionStatsGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDeactivateCfm
+
+  DESCRIPTION
+    The SME sends this primitive when the deactivation is complete.
+    The WMA cannot send any more primitives until it actives the SME again
+    sending another CSR_WIFI_SME_ACTIVATE_REQ.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeDeactivateCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeDisconnectCfm
+
+  DESCRIPTION
+    On reception of CSR_WIFI_SME_DISCONNECT_REQ the SME will perform a
+    disconnect operation, sending a CsrWifiSmeMediaStatusInd with
+    CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED and then call this primitive when
+    disconnection is complete.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeDisconnectCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeEventMaskSetCfm
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the request
+    primitive.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeEventMaskSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    hostConfig   - Current host power state.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrResult            status;
+    CsrWifiSmeHostConfig hostConfig;
+} CsrWifiSmeHostConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeHostConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeHostConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeIbssStationInd
+
+  DESCRIPTION
+    The SME will send this primitive to indicate that a station has joined or
+    left the ad-hoc network.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    address     - MAC address of the station that has joined or left
+    isconnected - TRUE if the station joined, FALSE if the station left
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    CsrWifiMacAddress address;
+    u8           isconnected;
+} CsrWifiSmeIbssStationInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeKeyCfm
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the request
+    primitive.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the request
+    action         - Action in the request
+    keyType        - Type of the key added/deleted
+    peerMacAddress - Peer MAC Address of the key added/deleted
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrResult            status;
+    CsrWifiSmeListAction action;
+    CsrWifiSmeKeyType    keyType;
+    CsrWifiMacAddress    peerMacAddress;
+} CsrWifiSmeKeyCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeLinkQualityGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    linkQuality  - Indicates the quality of the link
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    u16             interfaceTag;
+    CsrResult             status;
+    CsrWifiSmeLinkQuality linkQuality;
+} CsrWifiSmeLinkQualityGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMediaStatusInd
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it when a network connection is established, lost or has moved to
+    another AP.
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    mediaStatus    - Indicates the media status
+    connectionInfo - This parameter is relevant only if the mediaStatus is
+                     CSR_WIFI_SME_MEDIA_STATUS_CONNECTED:
+                     it points to the connection information for the new network
+    disassocReason - This parameter is relevant only if the mediaStatus is
+                     CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED:
+                     if a disassociation has occurred it gives the reason of the
+                     disassociation
+    deauthReason   - This parameter is relevant only if the mediaStatus is
+                     CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED:
+                     if a deauthentication has occurred it gives the reason of
+                     the deauthentication
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrWifiSmeMediaStatus     mediaStatus;
+    CsrWifiSmeConnectionInfo  connectionInfo;
+    CsrWifiSmeIEEE80211Reason disassocReason;
+    CsrWifiSmeIEEE80211Reason deauthReason;
+} CsrWifiSmeMediaStatusInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common    - Common header for use with the CsrWifiFsm Module
+    status    - Reports the result of the request
+    mibConfig - Reports various IEEE 802.11 attributes as currently configured
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    CsrResult           status;
+    CsrWifiSmeMibConfig mibConfig;
+} CsrWifiSmeMibConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeMibConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetCfm
+
+  DESCRIPTION
+    The SME calls this primitive to return the requested MIB variable values.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    status             - Reports the result of the request
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to the VarBind or VarBindList containing the
+                         names and values of the MIB variables requested
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+    u16       mibAttributeLength;
+    u8       *mibAttribute;
+} CsrWifiSmeMibGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibGetNextCfm
+
+  DESCRIPTION
+    The SME calls this primitive to return the requested MIB name(s).
+    The wireless manager application can then read the value of the MIB
+    variable using CSR_WIFI_SME_MIB_GET_REQ, using the names provided.
+
+  MEMBERS
+    common             - Common header for use with the CsrWifiFsm Module
+    status             - Reports the result of the request
+    mibAttributeLength - Length of mibAttribute
+    mibAttribute       - Points to a VarBind or VarBindList containing the
+                         name(s) of the MIB variable(s) lexicographically
+                         following the name(s) given in the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+    u16       mibAttributeLength;
+    u8       *mibAttribute;
+} CsrWifiSmeMibGetNextCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMibSetCfm
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the set primitive.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeMibSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMicFailureInd
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it whenever the chip firmware reports a MIC failure.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    secondFailure - TRUE if this indication is for a second failure in 60
+                    seconds
+    count         - The number of MIC failure events since the connection was
+                    established
+    address       - MAC address of the transmitter that caused the MIC failure
+    keyType       - Type of key for which the failure occurred
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    u16         interfaceTag;
+    u8           secondFailure;
+    u16         count;
+    CsrWifiMacAddress address;
+    CsrWifiSmeKeyType keyType;
+} CsrWifiSmeMicFailureInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeMulticastAddressCfm
+
+  DESCRIPTION
+    The SME will call this primitive when the operation is complete. For a
+    GET action, this primitive reports the current list of MAC addresses.
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    interfaceTag      - Interface Identifier; unique identifier of an interface
+    status            - Reports the result of the request
+    action            - Action in the request
+    getAddressesCount - This parameter is only relevant if action is
+                        CSR_WIFI_SME_LIST_ACTION_GET:
+                        number of MAC addresses sent with the primitive
+    getAddresses      - Pointer to the list of MAC Addresses sent with the
+                        primitive, set to NULL if none is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrResult            status;
+    CsrWifiSmeListAction action;
+    u8             getAddressesCount;
+    CsrWifiMacAddress   *getAddresses;
+} CsrWifiSmeMulticastAddressCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePacketFilterSetCfm
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the set primitive.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmePacketFilterSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePermanentMacAddressGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common              - Common header for use with the CsrWifiFsm Module
+    status              - Reports the result of the request
+    permanentMacAddress - MAC address stored in the EEPROM
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    CsrResult         status;
+    CsrWifiMacAddress permanentMacAddress;
+} CsrWifiSmePermanentMacAddressGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidCandidateListInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it when a new network supporting preauthentication and/or PMK
+    caching is seen.
+
+  MEMBERS
+    common               - Common header for use with the CsrWifiFsm Module
+    interfaceTag         - Interface Identifier; unique identifier of an
+                           interface
+    pmkidCandidatesCount - Number of PMKID candidates provided
+    pmkidCandidates      - Points to the first PMKID candidate
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    u8                  pmkidCandidatesCount;
+    CsrWifiSmePmkidCandidate *pmkidCandidates;
+} CsrWifiSmePmkidCandidateListInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePmkidCfm
+
+  DESCRIPTION
+    The SME will call this primitive when the operation is complete. For a
+    GET action, this primitive reports the current list of PMKIDs
+
+  MEMBERS
+    common         - Common header for use with the CsrWifiFsm Module
+    interfaceTag   - Interface Identifier; unique identifier of an interface
+    status         - Reports the result of the request
+    action         - Action in the request
+    getPmkidsCount - This parameter is only relevant if action is
+                     CSR_WIFI_SME_LIST_ACTION_GET:
+                     number of PMKIDs sent with the primitive
+    getPmkids      - Pointer to the list of PMKIDs sent with the primitive, set
+                     to NULL if none is sent.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    u16            interfaceTag;
+    CsrResult            status;
+    CsrWifiSmeListAction action;
+    u8             getPmkidsCount;
+    CsrWifiSmePmkid     *getPmkids;
+} CsrWifiSmePmkidCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    status      - Reports the result of the request
+    powerConfig - Returns the current parameters for the power configuration of
+                  the firmware
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrResult             status;
+    CsrWifiSmePowerConfig powerConfig;
+} CsrWifiSmePowerConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmePowerConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmePowerConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRegulatoryDomainInfoGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    status     - Reports the result of the request
+    regDomInfo - Reports information and state related to regulatory domain
+                 operation.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent                common;
+    CsrResult                      status;
+    CsrWifiSmeRegulatoryDomainInfo regDomInfo;
+} CsrWifiSmeRegulatoryDomainInfoGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamCompleteInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it completes an attempt to roam to an AP. If the roam
+    attempt was successful, status will be set to CSR_WIFI_SME_SUCCESS,
+    otherwise it shall be set to the appropriate error code.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the roaming procedure
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeRoamCompleteInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamStartInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive it whenever it begins an attempt to roam to an AP.
+    If the wireless manager application connect request specified the SSID
+    and the BSSID was set to the broadcast address (0xFF 0xFF 0xFF 0xFF 0xFF
+    0xFF), the SME monitors the signal quality and maintains a list of
+    candidates to roam to. When the signal quality of the current connection
+    falls below a threshold, and there is a candidate with better quality,
+    the SME will attempt to the candidate AP.
+    If the roaming procedure succeeds, the SME will also issue a Media
+    Connect indication to inform the wireless manager application of the
+    change.
+    NOTE: to prevent the SME from initiating roaming the WMA must specify the
+    BSSID in the connection request; this forces the SME to connect only to
+    that AP.
+    The wireless manager application can obtain statistics for roaming
+    purposes using CSR_WIFI_SME_CONNECTION_QUALITY_IND and
+    CSR_WIFI_SME_CONNECTION_STATS_GET_REQ.
+    When the wireless manager application wishes to roam to another AP, it
+    must issue a connection request specifying the BSSID of the desired AP.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    roamReason   - Indicates the reason for starting the roaming procedure
+    reason80211  - Indicates the reason for deauthentication or disassociation
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrWifiSmeRoamReason      roamReason;
+    CsrWifiSmeIEEE80211Reason reason80211;
+} CsrWifiSmeRoamStartInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    interfaceTag  - Interface Identifier; unique identifier of an interface
+    status        - Reports the result of the request
+    roamingConfig - Reports the roaming behaviour of the driver and firmware
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent         common;
+    u16               interfaceTag;
+    CsrResult               status;
+    CsrWifiSmeRoamingConfig roamingConfig;
+} CsrWifiSmeRoamingConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeRoamingConfigSetCfm
+
+  DESCRIPTION
+    This primitive sets the value of the RoamingConfig parameter.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeRoamingConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    status     - Reports the result of the request
+    scanConfig - Returns the current parameters for the autonomous scanning
+                 behaviour of the firmware
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrResult            status;
+    CsrWifiSmeScanConfig scanConfig;
+} CsrWifiSmeScanConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeScanConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanFullCfm
+
+  DESCRIPTION
+    The SME calls this primitive when the results from the scan are
+    available.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeScanFullCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultInd
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it whenever a scan indication is received from the firmware.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    result - Points to a buffer containing a scan result.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent      common;
+    CsrWifiSmeScanResult result;
+} CsrWifiSmeScanResultInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsFlushCfm
+
+  DESCRIPTION
+    The SME will call this primitive when the cache has been cleared.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeScanResultsFlushCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeScanResultsGetCfm
+
+  DESCRIPTION
+    The SME sends this primitive to provide the current set of scan results.
+
+  MEMBERS
+    common           - Common header for use with the CsrWifiFsm Module
+    status           - Reports the result of the request
+    scanResultsCount - Number of scan results
+    scanResults      - Points to a buffer containing an array of
+                       CsrWifiSmeScanResult structures.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent       common;
+    CsrResult             status;
+    u16             scanResultsCount;
+    CsrWifiSmeScanResult *scanResults;
+} CsrWifiSmeScanResultsGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+    smeConfig    - Current SME Station Parameters
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    u16           interfaceTag;
+    CsrResult           status;
+    CsrWifiSmeStaConfig smeConfig;
+} CsrWifiSmeSmeStaConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeStaConfigSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface Identifier; unique identifier of an interface
+    status       - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u16       interfaceTag;
+    CsrResult       status;
+} CsrWifiSmeSmeStaConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeStationMacAddressGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common            - Common header for use with the CsrWifiFsm Module
+    status            - Reports the result of the request
+    stationMacAddress - Current MAC address of the station.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    CsrResult         status;
+    CsrWifiMacAddress stationMacAddress[2];
+} CsrWifiSmeStationMacAddressGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the task that have registered to
+    receive it when a status change in the TSPEC occurs.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    transactionId   - Unique Transaction ID for the TSPEC, as assigned by the
+                      driver
+    tspecResultCode - Specifies the TSPEC operation requested by the peer
+                      station
+    tspecLength     - Length of the TSPEC.
+    tspec           - Points to the first byte of the TSPEC
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    u32                 transactionId;
+    CsrWifiSmeTspecResultCode tspecResultCode;
+    u16                 tspecLength;
+    u8                 *tspec;
+} CsrWifiSmeTspecInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeTspecCfm
+
+  DESCRIPTION
+    The SME calls the primitive to report the result of the TSpec primitive
+    request.
+
+  MEMBERS
+    common          - Common header for use with the CsrWifiFsm Module
+    interfaceTag    - Interface Identifier; unique identifier of an interface
+    status          - Reports the result of the request
+    transactionId   - Unique Transaction ID for the TSPEC, as assigned by the
+                      driver
+    tspecResultCode - Specifies the result of the negotiated TSPEC operation
+    tspecLength     - Length of the TSPEC.
+    tspec           - Points to the first byte of the TSPEC
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent           common;
+    u16                 interfaceTag;
+    CsrResult                 status;
+    u32                 transactionId;
+    CsrWifiSmeTspecResultCode tspecResultCode;
+    u16                 tspecLength;
+    u8                 *tspec;
+} CsrWifiSmeTspecCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeVersionsGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common   - Common header for use with the CsrWifiFsm Module
+    status   - Reports the result of the request
+    versions - Version IDs of the product
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent    common;
+    CsrResult          status;
+    CsrWifiSmeVersions versions;
+} CsrWifiSmeVersionsGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiFlightmodeCfm
+
+  DESCRIPTION
+    The SME calls this primitive when the chip is initialised for low power
+    mode and with the radio subsystem disabled. To leave flight mode, and
+    enable Wi-Fi, the wireless manager application should call
+    CSR_WIFI_SME_WIFI_ON_REQ.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeWifiFlightmodeCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOffInd
+
+  DESCRIPTION
+    The SME sends this primitive to all the tasks that have registered to
+    receive it to report that the chip has been turned off.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    reason - Indicates the reason why the Wi-Fi has been switched off.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent             common;
+    CsrWifiSmeControlIndication reason;
+} CsrWifiSmeWifiOffInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOffCfm
+
+  DESCRIPTION
+    After receiving CSR_WIFI_SME_WIFI_OFF_REQ, if the chip is connected to a
+    network, the SME will perform a disconnect operation, will send a
+    CSR_WIFI_SME_MEDIA_STATUS_IND with
+    CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED, and then will call
+    CSR_WIFI_SME_WIFI_OFF_CFM when the chip is off.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeWifiOffCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOnCfm
+
+  DESCRIPTION
+    The SME sends this primitive to the task that has sent the request once
+    the chip has been initialised and is available for use.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeWifiOnCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsSetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeCloakedSsidsSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCloakedSsidsGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    status       - Reports the result of the request
+    cloakedSsids - Reports list of cloaked SSIDs that are explicitly scanned for
+                   by the driver
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent             common;
+    CsrResult                   status;
+    CsrWifiSmeCloakedSsidConfig cloakedSsids;
+} CsrWifiSmeCloakedSsidsGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWifiOnInd
+
+  DESCRIPTION
+    The SME sends this primitive to all tasks that have registered to receive
+    it once the chip becomes available and ready to use.
+
+  MEMBERS
+    common  - Common header for use with the CsrWifiFsm Module
+    address - Current MAC address
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent   common;
+    CsrWifiMacAddress address;
+} CsrWifiSmeWifiOnInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    status       - Reports the result of the request
+    deviceConfig - Configuration options in the SME
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent        common;
+    CsrResult              status;
+    CsrWifiSmeDeviceConfig deviceConfig;
+} CsrWifiSmeSmeCommonConfigGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeSmeCommonConfigSetCfm
+
+  DESCRIPTION
+    Reports the result of the request
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Reports the result of the request
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeSmeCommonConfigSetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeInterfaceCapabilityGetCfm
+
+  DESCRIPTION
+    This primitive reports the result of the request.
+
+  MEMBERS
+    common        - Common header for use with the CsrWifiFsm Module
+    status        - Result of the request
+    numInterfaces - Number of the interfaces supported
+    capBitmap     - Points to the list of capabilities bitmaps provided for each
+                    interface.
+                    The bits represent the following capabilities:
+                    -bits 7 to 4-Reserved
+                    -bit 3-AMP
+                    -bit 2-P2P
+                    -bit 1-AP
+                    -bit 0-STA
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+    u16       numInterfaces;
+    u8        capBitmap[2];
+} CsrWifiSmeInterfaceCapabilityGetCfm;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeErrorInd
+
+  DESCRIPTION
+    Important error message indicating a error of some importance
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    errorMessage - Contains the error message.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    char  *errorMessage;
+} CsrWifiSmeErrorInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeInfoInd
+
+  DESCRIPTION
+    Message indicating a some info about current activity. Mostly of interest
+    in testing but may be useful in the field.
+
+  MEMBERS
+    common      - Common header for use with the CsrWifiFsm Module
+    infoMessage - Contains the message.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    char  *infoMessage;
+} CsrWifiSmeInfoInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeCoreDumpInd
+
+  DESCRIPTION
+    The SME will send this primitive to all the tasks that have registered to
+    receive Wi-Fi Chip core dump data.
+    The core dump data may be fragmented and sent using more than one
+    indication.
+    To indicate that all the data has been sent, the last indication contains
+    a 'length' of 0 and 'data' of NULL.
+
+  MEMBERS
+    common     - Common header for use with the CsrWifiFsm Module
+    dataLength - Number of bytes in the buffer pointed to by 'data'
+    data       - Pointer to the buffer containing 'dataLength' bytes of core
+                 dump data
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    u32       dataLength;
+    u8       *data;
+} CsrWifiSmeCoreDumpInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeAmpStatusChangeInd
+
+  DESCRIPTION
+    Indication of change to AMP activity.
+
+  MEMBERS
+    common       - Common header for use with the CsrWifiFsm Module
+    interfaceTag - Interface on which the AMP activity changed.
+    ampStatus    - The new status of AMP activity.Range: {AMP_ACTIVE,
+                   AMP_INACTIVE}.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent     common;
+    u16           interfaceTag;
+    CsrWifiSmeAmpStatus ampStatus;
+} CsrWifiSmeAmpStatusChangeInd;
+
+/*******************************************************************************
+
+  NAME
+    CsrWifiSmeWpsConfigurationCfm
+
+  DESCRIPTION
+    Confirm.
+
+  MEMBERS
+    common - Common header for use with the CsrWifiFsm Module
+    status - Status of the request.
+
+*******************************************************************************/
+typedef struct
+{
+    CsrWifiFsmEvent common;
+    CsrResult       status;
+} CsrWifiSmeWpsConfigurationCfm;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_SME_PRIM_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_sme_sef.c b/drivers/staging/csr/csr_wifi_sme_sef.c
new file mode 100644
index 00000000000..cf32254335c
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_sef.c
@@ -0,0 +1,85 @@
+/*****************************************************************************
+
+  (c) Cambridge Silicon Radio Limited 2010
+  Confidential information of CSR
+
+  Refer to LICENSE.txt included with this source for details
+  on the license terms.
+
+ *****************************************************************************/
+#include "csr_wifi_sme_sef.h"
+
+const CsrWifiSmeStateHandlerType CsrWifiSmeUpstreamStateHandlers[CSR_WIFI_SME_PRIM_UPSTREAM_COUNT] =
+{
+    /* 0x8000 */ CsrWifiSmeActivateCfmHandler,
+    /* 0x8001 */ CsrWifiSmeAdhocConfigGetCfmHandler,
+    /* 0x8002 */ CsrWifiSmeAdhocConfigSetCfmHandler,
+    /* 0x8003 */ CsrWifiSmeAssociationCompleteIndHandler,
+    /* 0x8004 */ CsrWifiSmeAssociationStartIndHandler,
+    /* 0x8005 */ CsrWifiSmeBlacklistCfmHandler,
+    /* 0x8006 */ CsrWifiSmeCalibrationDataGetCfmHandler,
+    /* 0x8007 */ CsrWifiSmeCalibrationDataSetCfmHandler,
+    /* 0x8008 */ CsrWifiSmeCcxConfigGetCfmHandler,
+    /* 0x8009 */ CsrWifiSmeCcxConfigSetCfmHandler,
+    /* 0x800A */ CsrWifiSmeCoexConfigGetCfmHandler,
+    /* 0x800B */ CsrWifiSmeCoexConfigSetCfmHandler,
+    /* 0x800C */ CsrWifiSmeCoexInfoGetCfmHandler,
+    /* 0x800D */ CsrWifiSmeConnectCfmHandler,
+    /* 0x800E */ CsrWifiSmeConnectionConfigGetCfmHandler,
+    /* 0x800F */ CsrWifiSmeConnectionInfoGetCfmHandler,
+    /* 0x8010 */ CsrWifiSmeConnectionQualityIndHandler,
+    /* 0x8011 */ CsrWifiSmeConnectionStatsGetCfmHandler,
+    /* 0x8012 */ CsrWifiSmeDeactivateCfmHandler,
+    /* 0x8013 */ CsrWifiSmeDisconnectCfmHandler,
+    /* 0x8014 */ CsrWifiSmeEventMaskSetCfmHandler,
+    /* 0x8015 */ CsrWifiSmeHostConfigGetCfmHandler,
+    /* 0x8016 */ CsrWifiSmeHostConfigSetCfmHandler,
+    /* 0x8017 */ CsrWifiSmeIbssStationIndHandler,
+    /* 0x8018 */ CsrWifiSmeKeyCfmHandler,
+    /* 0x8019 */ CsrWifiSmeLinkQualityGetCfmHandler,
+    /* 0x801A */ CsrWifiSmeMediaStatusIndHandler,
+    /* 0x801B */ CsrWifiSmeMibConfigGetCfmHandler,
+    /* 0x801C */ CsrWifiSmeMibConfigSetCfmHandler,
+    /* 0x801D */ CsrWifiSmeMibGetCfmHandler,
+    /* 0x801E */ CsrWifiSmeMibGetNextCfmHandler,
+    /* 0x801F */ CsrWifiSmeMibSetCfmHandler,
+    /* 0x8020 */ CsrWifiSmeMicFailureIndHandler,
+    /* 0x8021 */ CsrWifiSmeMulticastAddressCfmHandler,
+    /* 0x8022 */ CsrWifiSmePacketFilterSetCfmHandler,
+    /* 0x8023 */ CsrWifiSmePermanentMacAddressGetCfmHandler,
+    /* 0x8024 */ CsrWifiSmePmkidCandidateListIndHandler,
+    /* 0x8025 */ CsrWifiSmePmkidCfmHandler,
+    /* 0x8026 */ CsrWifiSmePowerConfigGetCfmHandler,
+    /* 0x8027 */ CsrWifiSmePowerConfigSetCfmHandler,
+    /* 0x8028 */ CsrWifiSmeRegulatoryDomainInfoGetCfmHandler,
+    /* 0x8029 */ CsrWifiSmeRoamCompleteIndHandler,
+    /* 0x802A */ CsrWifiSmeRoamStartIndHandler,
+    /* 0x802B */ CsrWifiSmeRoamingConfigGetCfmHandler,
+    /* 0x802C */ CsrWifiSmeRoamingConfigSetCfmHandler,
+    /* 0x802D */ CsrWifiSmeScanConfigGetCfmHandler,
+    /* 0x802E */ CsrWifiSmeScanConfigSetCfmHandler,
+    /* 0x802F */ CsrWifiSmeScanFullCfmHandler,
+    /* 0x8030 */ CsrWifiSmeScanResultIndHandler,
+    /* 0x8031 */ CsrWifiSmeScanResultsFlushCfmHandler,
+    /* 0x8032 */ CsrWifiSmeScanResultsGetCfmHandler,
+    /* 0x8033 */ CsrWifiSmeSmeStaConfigGetCfmHandler,
+    /* 0x8034 */ CsrWifiSmeSmeStaConfigSetCfmHandler,
+    /* 0x8035 */ CsrWifiSmeStationMacAddressGetCfmHandler,
+    /* 0x8036 */ CsrWifiSmeTspecIndHandler,
+    /* 0x8037 */ CsrWifiSmeTspecCfmHandler,
+    /* 0x8038 */ CsrWifiSmeVersionsGetCfmHandler,
+    /* 0x8039 */ CsrWifiSmeWifiFlightmodeCfmHandler,
+    /* 0x803A */ CsrWifiSmeWifiOffIndHandler,
+    /* 0x803B */ CsrWifiSmeWifiOffCfmHandler,
+    /* 0x803C */ CsrWifiSmeWifiOnCfmHandler,
+    /* 0x803D */ CsrWifiSmeCloakedSsidsSetCfmHandler,
+    /* 0x803E */ CsrWifiSmeCloakedSsidsGetCfmHandler,
+    /* 0x803F */ CsrWifiSmeWifiOnIndHandler,
+    /* 0x8040 */ CsrWifiSmeSmeCommonConfigGetCfmHandler,
+    /* 0x8041 */ CsrWifiSmeSmeCommonConfigSetCfmHandler,
+    /* 0x8042 */ CsrWifiSmeGetInterfaceCapabilityCfmHandler,
+    /* 0x8043 */ CsrWifiSmeErrorIndHandler,
+    /* 0x8044 */ CsrWifiSmeInfoIndHandler,
+    /* 0x8045 */ CsrWifiSmeCoreDumpIndHandler,
+    /* 0x8046 */ CsrWifiSmeAmpStatusChangeIndHandler,
+};
diff --git a/drivers/staging/csr/csr_wifi_sme_sef.h b/drivers/staging/csr/csr_wifi_sme_sef.h
new file mode 100644
index 00000000000..c8741811b2e
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_sef.h
@@ -0,0 +1,101 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2010
+            Confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+#ifndef CSR_WIFI_ROUTER_SEF_CSR_WIFI_SME_H__
+#define CSR_WIFI_ROUTER_SEF_CSR_WIFI_SME_H__
+
+#include "csr_wifi_sme_prim.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef void (*CsrWifiSmeStateHandlerType)(void* drvpriv, CsrWifiFsmEvent* msg);
+
+extern const CsrWifiSmeStateHandlerType CsrWifiSmeUpstreamStateHandlers[CSR_WIFI_SME_PRIM_UPSTREAM_COUNT];
+
+
+extern void CsrWifiSmeActivateCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeAdhocConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeAdhocConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeAssociationCompleteIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeAssociationStartIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeBlacklistCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCalibrationDataGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCalibrationDataSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCcxConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCcxConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCoexConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCoexConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCoexInfoGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeConnectCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeConnectionConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeConnectionInfoGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeConnectionQualityIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeConnectionStatsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeDeactivateCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeDisconnectCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeEventMaskSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeHostConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeHostConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeIbssStationIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeKeyCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeLinkQualityGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMediaStatusIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMibConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMibConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMibGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMibGetNextCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMibSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMicFailureIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeMulticastAddressCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmePacketFilterSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmePermanentMacAddressGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmePmkidCandidateListIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmePmkidCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmePowerConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmePowerConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeRegulatoryDomainInfoGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeRoamCompleteIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeRoamStartIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeRoamingConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeRoamingConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeScanConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeScanConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeScanFullCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeScanResultIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeScanResultsFlushCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeScanResultsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeSmeStaConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeSmeStaConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeStationMacAddressGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeTspecIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeTspecCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeVersionsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeWifiFlightmodeCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeWifiOffIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeWifiOffCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeWifiOnCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCloakedSsidsSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCloakedSsidsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeWifiOnIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeSmeCommonConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeSmeCommonConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeGetInterfaceCapabilityCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeErrorIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeInfoIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeCoreDumpIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+extern void CsrWifiSmeAmpStatusChangeIndHandler(void* drvpriv, CsrWifiFsmEvent* msg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_ROUTER_SEF_CSR_WIFI_SME_H__ */
diff --git a/drivers/staging/csr/csr_wifi_sme_serialize.c b/drivers/staging/csr/csr_wifi_sme_serialize.c
new file mode 100644
index 00000000000..7d7e1d8b5ed
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_serialize.c
@@ -0,0 +1,5809 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+#include <linux/string.h>
+#include <linux/slab.h>
+#include "csr_msgconv.h"
+#include "csr_wifi_sme_prim.h"
+#include "csr_wifi_sme_serialize.h"
+
+void CsrWifiSmePfree(void *ptr)
+{
+    kfree(ptr);
+}
+
+
+size_t CsrWifiSmeAdhocConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2; /* u16 primitive->adHocConfig.atimWindowTu */
+    bufferSize += 2; /* u16 primitive->adHocConfig.beaconPeriodTu */
+    bufferSize += 2; /* u16 primitive->adHocConfig.joinOnlyAttempts */
+    bufferSize += 2; /* u16 primitive->adHocConfig.joinAttemptIntervalMs */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeAdhocConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeAdhocConfigSetReq *primitive = (CsrWifiSmeAdhocConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.atimWindowTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.beaconPeriodTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.joinOnlyAttempts);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.joinAttemptIntervalMs);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeAdhocConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeAdhocConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeAdhocConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.atimWindowTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.beaconPeriodTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.joinOnlyAttempts, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.joinAttemptIntervalMs, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeBlacklistReqSizeof(void *msg)
+{
+    CsrWifiSmeBlacklistReq *primitive = (CsrWifiSmeBlacklistReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->setAddressCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->setAddresses[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeBlacklistReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeBlacklistReq *primitive = (CsrWifiSmeBlacklistReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->setAddressCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->setAddresses[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeBlacklistReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeBlacklistReq *primitive = kmalloc(sizeof(CsrWifiSmeBlacklistReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->setAddressCount, buffer, &offset);
+    primitive->setAddresses = NULL;
+    if (primitive->setAddressCount)
+    {
+        primitive->setAddresses = kmalloc(sizeof(CsrWifiMacAddress) * primitive->setAddressCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressCount; i1++)
+        {
+            CsrMemCpyDes(primitive->setAddresses[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeBlacklistReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeBlacklistReq *primitive = (CsrWifiSmeBlacklistReq *) voidPrimitivePointer;
+    kfree(primitive->setAddresses);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCalibrationDataSetReqSizeof(void *msg)
+{
+    CsrWifiSmeCalibrationDataSetReq *primitive = (CsrWifiSmeCalibrationDataSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 6) */
+    bufferSize += 2;                                /* u16 primitive->calibrationDataLength */
+    bufferSize += primitive->calibrationDataLength; /* u8 primitive->calibrationData */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCalibrationDataSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCalibrationDataSetReq *primitive = (CsrWifiSmeCalibrationDataSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->calibrationDataLength);
+    if (primitive->calibrationDataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->calibrationData, ((u16) (primitive->calibrationDataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCalibrationDataSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCalibrationDataSetReq *primitive = kmalloc(sizeof(CsrWifiSmeCalibrationDataSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->calibrationDataLength, buffer, &offset);
+    if (primitive->calibrationDataLength)
+    {
+        primitive->calibrationData = kmalloc(primitive->calibrationDataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->calibrationData, buffer, &offset, ((u16) (primitive->calibrationDataLength)));
+    }
+    else
+    {
+        primitive->calibrationData = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeCalibrationDataSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeCalibrationDataSetReq *primitive = (CsrWifiSmeCalibrationDataSetReq *) voidPrimitivePointer;
+    kfree(primitive->calibrationData);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCcxConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* u8 primitive->ccxConfig.keepAliveTimeMs */
+    bufferSize += 1; /* u8 primitive->ccxConfig.apRoamingEnabled */
+    bufferSize += 1; /* u8 primitive->ccxConfig.measurementsMask */
+    bufferSize += 1; /* u8 primitive->ccxConfig.ccxRadioMgtEnabled */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCcxConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCcxConfigSetReq *primitive = (CsrWifiSmeCcxConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.keepAliveTimeMs);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.apRoamingEnabled);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.measurementsMask);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.ccxRadioMgtEnabled);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCcxConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCcxConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeCcxConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.keepAliveTimeMs, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.apRoamingEnabled, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.measurementsMask, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.ccxRadioMgtEnabled, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeCoexConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 29) */
+    bufferSize += 1; /* u8 primitive->coexConfig.coexEnableSchemeManagement */
+    bufferSize += 1; /* u8 primitive->coexConfig.coexPeriodicWakeHost */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexTrafficBurstyLatencyMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexTrafficContinuousLatencyMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexObexBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexObexBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpBrBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpBrBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpEdrBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpEdrBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexPagingBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexPagingBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexInquiryBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexInquiryBlackoutPeriodMs */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCoexConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCoexConfigSetReq *primitive = (CsrWifiSmeCoexConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexConfig.coexEnableSchemeManagement);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexConfig.coexPeriodicWakeHost);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexTrafficBurstyLatencyMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexTrafficContinuousLatencyMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexObexBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexObexBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpBrBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpBrBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpEdrBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpEdrBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexPagingBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexPagingBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexInquiryBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexInquiryBlackoutPeriodMs);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCoexConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCoexConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeCoexConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexConfig.coexEnableSchemeManagement, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexConfig.coexPeriodicWakeHost, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexTrafficBurstyLatencyMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexTrafficContinuousLatencyMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexObexBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexObexBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpBrBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpBrBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpEdrBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpEdrBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexPagingBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexPagingBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexInquiryBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexInquiryBlackoutPeriodMs, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeConnectReqSizeof(void *msg)
+{
+    CsrWifiSmeConnectReq *primitive = (CsrWifiSmeConnectReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 57) */
+    bufferSize += 2;                                                                     /* u16 primitive->interfaceTag */
+    bufferSize += 32;                                                                    /* u8 primitive->connectionConfig.ssid.ssid[32] */
+    bufferSize += 1;                                                                     /* u8 primitive->connectionConfig.ssid.length */
+    bufferSize += 6;                                                                     /* u8 primitive->connectionConfig.bssid.a[6] */
+    bufferSize += 1;                                                                     /* CsrWifiSmeBssType primitive->connectionConfig.bssType */
+    bufferSize += 1;                                                                     /* CsrWifiSmeRadioIF primitive->connectionConfig.ifIndex */
+    bufferSize += 1;                                                                     /* CsrWifiSme80211PrivacyMode primitive->connectionConfig.privacyMode */
+    bufferSize += 2;                                                                     /* CsrWifiSmeAuthModeMask primitive->connectionConfig.authModeMask */
+    bufferSize += 2;                                                                     /* CsrWifiSmeEncryptionMask primitive->connectionConfig.encryptionModeMask */
+    bufferSize += 2;                                                                     /* u16 primitive->connectionConfig.mlmeAssociateReqInformationElementsLength */
+    bufferSize += primitive->connectionConfig.mlmeAssociateReqInformationElementsLength; /* u8 primitive->connectionConfig.mlmeAssociateReqInformationElements */
+    bufferSize += 1;                                                                     /* CsrWifiSmeWmmQosInfoMask primitive->connectionConfig.wmmQosInfo */
+    bufferSize += 1;                                                                     /* u8 primitive->connectionConfig.adhocJoinOnly */
+    bufferSize += 1;                                                                     /* u8 primitive->connectionConfig.adhocChannel */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeConnectReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeConnectReq *primitive = (CsrWifiSmeConnectReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionConfig.ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.ssid.length);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionConfig.bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.bssType);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.ifIndex);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.privacyMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionConfig.authModeMask);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionConfig.encryptionModeMask);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionConfig.mlmeAssociateReqInformationElementsLength);
+    if (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionConfig.mlmeAssociateReqInformationElements, ((u16) (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)));
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.wmmQosInfo);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.adhocJoinOnly);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.adhocChannel);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeConnectReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeConnectReq *primitive = kmalloc(sizeof(CsrWifiSmeConnectReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionConfig.ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->connectionConfig.ssid.length, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionConfig.bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->connectionConfig.bssType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.ifIndex, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.privacyMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionConfig.authModeMask, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionConfig.encryptionModeMask, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionConfig.mlmeAssociateReqInformationElementsLength, buffer, &offset);
+    if (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)
+    {
+        primitive->connectionConfig.mlmeAssociateReqInformationElements = kmalloc(primitive->connectionConfig.mlmeAssociateReqInformationElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionConfig.mlmeAssociateReqInformationElements, buffer, &offset, ((u16) (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)));
+    }
+    else
+    {
+        primitive->connectionConfig.mlmeAssociateReqInformationElements = NULL;
+    }
+    CsrUint8Des((u8 *) &primitive->connectionConfig.wmmQosInfo, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.adhocJoinOnly, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.adhocChannel, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeConnectReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeConnectReq *primitive = (CsrWifiSmeConnectReq *) voidPrimitivePointer;
+    kfree(primitive->connectionConfig.mlmeAssociateReqInformationElements);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeHostConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeHostPowerMode primitive->hostConfig.powerMode */
+    bufferSize += 2; /* u16 primitive->hostConfig.applicationDataPeriodMs */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeHostConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeHostConfigSetReq *primitive = (CsrWifiSmeHostConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->hostConfig.powerMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->hostConfig.applicationDataPeriodMs);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeHostConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeHostConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeHostConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->hostConfig.powerMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->hostConfig.applicationDataPeriodMs, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeKeyReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 65) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* CsrWifiSmeKeyType primitive->key.keyType */
+    bufferSize += 1; /* u8 primitive->key.keyIndex */
+    bufferSize += 1; /* u8 primitive->key.wepTxKey */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 8; i2++)
+        {
+            bufferSize += 2; /* u16 primitive->key.keyRsc[8] */
+        }
+    }
+    bufferSize += 1;         /* u8 primitive->key.authenticator */
+    bufferSize += 6;         /* u8 primitive->key.address.a[6] */
+    bufferSize += 1;         /* u8 primitive->key.keyLength */
+    bufferSize += 32;        /* u8 primitive->key.key[32] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeKeyReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeKeyReq *primitive = (CsrWifiSmeKeyReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->key.keyType);
+    CsrUint8Ser(ptr, len, (u8) primitive->key.keyIndex);
+    CsrUint8Ser(ptr, len, (u8) primitive->key.wepTxKey);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 8; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->key.keyRsc[i2]);
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->key.authenticator);
+    CsrMemCpySer(ptr, len, (const void *) primitive->key.address.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->key.keyLength);
+    CsrMemCpySer(ptr, len, (const void *) primitive->key.key, ((u16) (32)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmeKeyReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeKeyReq *primitive = kmalloc(sizeof(CsrWifiSmeKeyReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->key.keyType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->key.keyIndex, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->key.wepTxKey, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 8; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->key.keyRsc[i2], buffer, &offset);
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->key.authenticator, buffer, &offset);
+    CsrMemCpyDes(primitive->key.address.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->key.keyLength, buffer, &offset);
+    CsrMemCpyDes(primitive->key.key, buffer, &offset, ((u16) (32)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeMibConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 1; /* u8 primitive->mibConfig.unifiFixMaxTxDataRate */
+    bufferSize += 1; /* u8 primitive->mibConfig.unifiFixTxDataRate */
+    bufferSize += 2; /* u16 primitive->mibConfig.dot11RtsThreshold */
+    bufferSize += 2; /* u16 primitive->mibConfig.dot11FragmentationThreshold */
+    bufferSize += 2; /* u16 primitive->mibConfig.dot11CurrentTxPowerLevel */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibConfigSetReq *primitive = (CsrWifiSmeMibConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->mibConfig.unifiFixMaxTxDataRate);
+    CsrUint8Ser(ptr, len, (u8) primitive->mibConfig.unifiFixTxDataRate);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibConfig.dot11RtsThreshold);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibConfig.dot11FragmentationThreshold);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibConfig.dot11CurrentTxPowerLevel);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeMibConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mibConfig.unifiFixMaxTxDataRate, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mibConfig.unifiFixTxDataRate, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibConfig.dot11RtsThreshold, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibConfig.dot11FragmentationThreshold, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibConfig.dot11CurrentTxPowerLevel, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeMibGetNextReqSizeof(void *msg)
+{
+    CsrWifiSmeMibGetNextReq *primitive = (CsrWifiSmeMibGetNextReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 6) */
+    bufferSize += 2;                             /* u16 primitive->mibAttributeLength */
+    bufferSize += primitive->mibAttributeLength; /* u8 primitive->mibAttribute */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibGetNextReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibGetNextReq *primitive = (CsrWifiSmeMibGetNextReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibAttributeLength);
+    if (primitive->mibAttributeLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mibAttribute, ((u16) (primitive->mibAttributeLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibGetNextReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibGetNextReq *primitive = kmalloc(sizeof(CsrWifiSmeMibGetNextReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibAttributeLength, buffer, &offset);
+    if (primitive->mibAttributeLength)
+    {
+        primitive->mibAttribute = kmalloc(primitive->mibAttributeLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mibAttribute, buffer, &offset, ((u16) (primitive->mibAttributeLength)));
+    }
+    else
+    {
+        primitive->mibAttribute = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMibGetNextReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMibGetNextReq *primitive = (CsrWifiSmeMibGetNextReq *) voidPrimitivePointer;
+    kfree(primitive->mibAttribute);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeMibGetReqSizeof(void *msg)
+{
+    CsrWifiSmeMibGetReq *primitive = (CsrWifiSmeMibGetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 6) */
+    bufferSize += 2;                             /* u16 primitive->mibAttributeLength */
+    bufferSize += primitive->mibAttributeLength; /* u8 primitive->mibAttribute */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibGetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibGetReq *primitive = (CsrWifiSmeMibGetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibAttributeLength);
+    if (primitive->mibAttributeLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mibAttribute, ((u16) (primitive->mibAttributeLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibGetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibGetReq *primitive = kmalloc(sizeof(CsrWifiSmeMibGetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibAttributeLength, buffer, &offset);
+    if (primitive->mibAttributeLength)
+    {
+        primitive->mibAttribute = kmalloc(primitive->mibAttributeLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mibAttribute, buffer, &offset, ((u16) (primitive->mibAttributeLength)));
+    }
+    else
+    {
+        primitive->mibAttribute = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMibGetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMibGetReq *primitive = (CsrWifiSmeMibGetReq *) voidPrimitivePointer;
+    kfree(primitive->mibAttribute);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeMibSetReqSizeof(void *msg)
+{
+    CsrWifiSmeMibSetReq *primitive = (CsrWifiSmeMibSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 6) */
+    bufferSize += 2;                             /* u16 primitive->mibAttributeLength */
+    bufferSize += primitive->mibAttributeLength; /* u8 primitive->mibAttribute */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibSetReq *primitive = (CsrWifiSmeMibSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibAttributeLength);
+    if (primitive->mibAttributeLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mibAttribute, ((u16) (primitive->mibAttributeLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibSetReq *primitive = kmalloc(sizeof(CsrWifiSmeMibSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibAttributeLength, buffer, &offset);
+    if (primitive->mibAttributeLength)
+    {
+        primitive->mibAttribute = kmalloc(primitive->mibAttributeLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mibAttribute, buffer, &offset, ((u16) (primitive->mibAttributeLength)));
+    }
+    else
+    {
+        primitive->mibAttribute = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMibSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMibSetReq *primitive = (CsrWifiSmeMibSetReq *) voidPrimitivePointer;
+    kfree(primitive->mibAttribute);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeMulticastAddressReqSizeof(void *msg)
+{
+    CsrWifiSmeMulticastAddressReq *primitive = (CsrWifiSmeMulticastAddressReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->setAddressesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressesCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->setAddresses[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMulticastAddressReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMulticastAddressReq *primitive = (CsrWifiSmeMulticastAddressReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->setAddressesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressesCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->setAddresses[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMulticastAddressReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMulticastAddressReq *primitive = kmalloc(sizeof(CsrWifiSmeMulticastAddressReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->setAddressesCount, buffer, &offset);
+    primitive->setAddresses = NULL;
+    if (primitive->setAddressesCount)
+    {
+        primitive->setAddresses = kmalloc(sizeof(CsrWifiMacAddress) * primitive->setAddressesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setAddressesCount; i1++)
+        {
+            CsrMemCpyDes(primitive->setAddresses[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMulticastAddressReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMulticastAddressReq *primitive = (CsrWifiSmeMulticastAddressReq *) voidPrimitivePointer;
+    kfree(primitive->setAddresses);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmePacketFilterSetReqSizeof(void *msg)
+{
+    CsrWifiSmePacketFilterSetReq *primitive = (CsrWifiSmePacketFilterSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2;                       /* u16 primitive->interfaceTag */
+    bufferSize += 2;                       /* u16 primitive->filterLength */
+    bufferSize += primitive->filterLength; /* u8 primitive->filter */
+    bufferSize += 1;                       /* CsrWifiSmePacketFilterMode primitive->mode */
+    bufferSize += 4;                       /* u8 primitive->arpFilterAddress.a[4] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePacketFilterSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePacketFilterSetReq *primitive = (CsrWifiSmePacketFilterSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->filterLength);
+    if (primitive->filterLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->filter, ((u16) (primitive->filterLength)));
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->mode);
+    CsrMemCpySer(ptr, len, (const void *) primitive->arpFilterAddress.a, ((u16) (4)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmePacketFilterSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePacketFilterSetReq *primitive = kmalloc(sizeof(CsrWifiSmePacketFilterSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->filterLength, buffer, &offset);
+    if (primitive->filterLength)
+    {
+        primitive->filter = kmalloc(primitive->filterLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->filter, buffer, &offset, ((u16) (primitive->filterLength)));
+    }
+    else
+    {
+        primitive->filter = NULL;
+    }
+    CsrUint8Des((u8 *) &primitive->mode, buffer, &offset);
+    CsrMemCpyDes(primitive->arpFilterAddress.a, buffer, &offset, ((u16) (4)));
+
+    return primitive;
+}
+
+
+void CsrWifiSmePacketFilterSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmePacketFilterSetReq *primitive = (CsrWifiSmePacketFilterSetReq *) voidPrimitivePointer;
+    kfree(primitive->filter);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmePmkidReqSizeof(void *msg)
+{
+    CsrWifiSmePmkidReq *primitive = (CsrWifiSmePmkidReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 29) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->setPmkidsCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setPmkidsCount; i1++)
+        {
+            bufferSize += 6;  /* u8 primitive->setPmkids[i1].bssid.a[6] */
+            bufferSize += 16; /* u8 primitive->setPmkids[i1].pmkid[16] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePmkidReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePmkidReq *primitive = (CsrWifiSmePmkidReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->setPmkidsCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setPmkidsCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->setPmkids[i1].bssid.a, ((u16) (6)));
+            CsrMemCpySer(ptr, len, (const void *) primitive->setPmkids[i1].pmkid, ((u16) (16)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmePmkidReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePmkidReq *primitive = kmalloc(sizeof(CsrWifiSmePmkidReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->setPmkidsCount, buffer, &offset);
+    primitive->setPmkids = NULL;
+    if (primitive->setPmkidsCount)
+    {
+        primitive->setPmkids = kmalloc(sizeof(CsrWifiSmePmkid) * primitive->setPmkidsCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->setPmkidsCount; i1++)
+        {
+            CsrMemCpyDes(primitive->setPmkids[i1].bssid.a, buffer, &offset, ((u16) (6)));
+            CsrMemCpyDes(primitive->setPmkids[i1].pmkid, buffer, &offset, ((u16) (16)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmePmkidReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmePmkidReq *primitive = (CsrWifiSmePmkidReq *) voidPrimitivePointer;
+    kfree(primitive->setPmkids);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmePowerConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 1; /* CsrWifiSmePowerSaveLevel primitive->powerConfig.powerSaveLevel */
+    bufferSize += 2; /* u16 primitive->powerConfig.listenIntervalTu */
+    bufferSize += 1; /* u8 primitive->powerConfig.rxDtims */
+    bufferSize += 1; /* CsrWifiSmeD3AutoScanMode primitive->powerConfig.d3AutoScanMode */
+    bufferSize += 1; /* u8 primitive->powerConfig.clientTrafficWindow */
+    bufferSize += 1; /* u8 primitive->powerConfig.opportunisticPowerSave */
+    bufferSize += 1; /* u8 primitive->powerConfig.noticeOfAbsence */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePowerConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePowerConfigSetReq *primitive = (CsrWifiSmePowerConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.powerSaveLevel);
+    CsrUint16Ser(ptr, len, (u16) primitive->powerConfig.listenIntervalTu);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.rxDtims);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.d3AutoScanMode);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.clientTrafficWindow);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.opportunisticPowerSave);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.noticeOfAbsence);
+    return(ptr);
+}
+
+
+void* CsrWifiSmePowerConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePowerConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmePowerConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.powerSaveLevel, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->powerConfig.listenIntervalTu, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.rxDtims, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.d3AutoScanMode, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.clientTrafficWindow, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.opportunisticPowerSave, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.noticeOfAbsence, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeRoamingConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 70) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].rssiHighThreshold */
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].rssiLowThreshold */
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].snrHighThreshold */
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].snrLowThreshold */
+        }
+    }
+    bufferSize += 1;         /* u8 primitive->roamingConfig.disableSmoothRoaming */
+    bufferSize += 1;         /* u8 primitive->roamingConfig.disableRoamScans */
+    bufferSize += 1;         /* u8 primitive->roamingConfig.reconnectLimit */
+    bufferSize += 2;         /* u16 primitive->roamingConfig.reconnectLimitIntervalMs */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].intervalSeconds */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].validitySeconds */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].minActiveChannelTimeTu */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].maxActiveChannelTimeTu */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].minPassiveChannelTimeTu */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].maxPassiveChannelTimeTu */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeRoamingConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeRoamingConfigSetReq *primitive = (CsrWifiSmeRoamingConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].rssiHighThreshold);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].rssiLowThreshold);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].snrHighThreshold);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].snrLowThreshold);
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->roamingConfig.disableSmoothRoaming);
+    CsrUint8Ser(ptr, len, (u8) primitive->roamingConfig.disableRoamScans);
+    CsrUint8Ser(ptr, len, (u8) primitive->roamingConfig.reconnectLimit);
+    CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.reconnectLimitIntervalMs);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].intervalSeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].validitySeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].minActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].maxActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].minPassiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].maxPassiveChannelTimeTu);
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeRoamingConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeRoamingConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeRoamingConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].rssiHighThreshold, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].rssiLowThreshold, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].snrHighThreshold, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].snrLowThreshold, buffer, &offset);
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->roamingConfig.disableSmoothRoaming, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->roamingConfig.disableRoamScans, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->roamingConfig.reconnectLimit, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->roamingConfig.reconnectLimitIntervalMs, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].intervalSeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].validitySeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].minActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].maxActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].minPassiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].maxPassiveChannelTimeTu, buffer, &offset);
+        }
+    }
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeScanConfigSetReqSizeof(void *msg)
+{
+    CsrWifiSmeScanConfigSetReq *primitive = (CsrWifiSmeScanConfigSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 63) */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].intervalSeconds */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].validitySeconds */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].minActiveChannelTimeTu */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].maxActiveChannelTimeTu */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].minPassiveChannelTimeTu */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].maxPassiveChannelTimeTu */
+        }
+    }
+    bufferSize += 1;                                             /* u8 primitive->scanConfig.disableAutonomousScans */
+    bufferSize += 2;                                             /* u16 primitive->scanConfig.maxResults */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.highRssiThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.lowRssiThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.deltaRssiThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.highSnrThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.lowSnrThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.deltaSnrThreshold */
+    bufferSize += 2;                                             /* u16 primitive->scanConfig.passiveChannelListCount */
+    bufferSize += primitive->scanConfig.passiveChannelListCount; /* u8 primitive->scanConfig.passiveChannelList */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeScanConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeScanConfigSetReq *primitive = (CsrWifiSmeScanConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].intervalSeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].validitySeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].minActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].maxActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].minPassiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].maxPassiveChannelTimeTu);
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.disableAutonomousScans);
+    CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.maxResults);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.highRssiThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.lowRssiThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.deltaRssiThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.highSnrThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.lowSnrThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.deltaSnrThreshold);
+    CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.passiveChannelListCount);
+    if (primitive->scanConfig.passiveChannelListCount)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->scanConfig.passiveChannelList, ((u16) (primitive->scanConfig.passiveChannelListCount)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeScanConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeScanConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeScanConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].intervalSeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].validitySeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].minActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].maxActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].minPassiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].maxPassiveChannelTimeTu, buffer, &offset);
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->scanConfig.disableAutonomousScans, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->scanConfig.maxResults, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.highRssiThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.lowRssiThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.deltaRssiThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.highSnrThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.lowSnrThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.deltaSnrThreshold, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->scanConfig.passiveChannelListCount, buffer, &offset);
+    if (primitive->scanConfig.passiveChannelListCount)
+    {
+        primitive->scanConfig.passiveChannelList = kmalloc(primitive->scanConfig.passiveChannelListCount, GFP_KERNEL);
+        CsrMemCpyDes(primitive->scanConfig.passiveChannelList, buffer, &offset, ((u16) (primitive->scanConfig.passiveChannelListCount)));
+    }
+    else
+    {
+        primitive->scanConfig.passiveChannelList = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeScanConfigSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeScanConfigSetReq *primitive = (CsrWifiSmeScanConfigSetReq *) voidPrimitivePointer;
+    kfree(primitive->scanConfig.passiveChannelList);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeScanFullReqSizeof(void *msg)
+{
+    CsrWifiSmeScanFullReq *primitive = (CsrWifiSmeScanFullReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 52) */
+    bufferSize += 1; /* u8 primitive->ssidCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->ssidCount; i1++)
+        {
+            bufferSize += 32;                  /* u8 primitive->ssid[i1].ssid[32] */
+            bufferSize += 1;                   /* u8 primitive->ssid[i1].length */
+        }
+    }
+    bufferSize += 6;                           /* u8 primitive->bssid.a[6] */
+    bufferSize += 1;                           /* u8 primitive->forceScan */
+    bufferSize += 1;                           /* CsrWifiSmeBssType primitive->bssType */
+    bufferSize += 1;                           /* CsrWifiSmeScanType primitive->scanType */
+    bufferSize += 2;                           /* u16 primitive->channelListCount */
+    bufferSize += primitive->channelListCount; /* u8 primitive->channelList */
+    bufferSize += 2;                           /* u16 primitive->probeIeLength */
+    bufferSize += primitive->probeIeLength;    /* u8 primitive->probeIe */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeScanFullReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeScanFullReq *primitive = (CsrWifiSmeScanFullReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->ssidCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->ssidCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->ssid[i1].ssid, ((u16) (32)));
+            CsrUint8Ser(ptr, len, (u8) primitive->ssid[i1].length);
+        }
+    }
+    CsrMemCpySer(ptr, len, (const void *) primitive->bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->forceScan);
+    CsrUint8Ser(ptr, len, (u8) primitive->bssType);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanType);
+    CsrUint16Ser(ptr, len, (u16) primitive->channelListCount);
+    if (primitive->channelListCount)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->channelList, ((u16) (primitive->channelListCount)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->probeIeLength);
+    if (primitive->probeIeLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->probeIe, ((u16) (primitive->probeIeLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeScanFullReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeScanFullReq *primitive = kmalloc(sizeof(CsrWifiSmeScanFullReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ssidCount, buffer, &offset);
+    primitive->ssid = NULL;
+    if (primitive->ssidCount)
+    {
+        primitive->ssid = kmalloc(sizeof(CsrWifiSsid) * primitive->ssidCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->ssidCount; i1++)
+        {
+            CsrMemCpyDes(primitive->ssid[i1].ssid, buffer, &offset, ((u16) (32)));
+            CsrUint8Des((u8 *) &primitive->ssid[i1].length, buffer, &offset);
+        }
+    }
+    CsrMemCpyDes(primitive->bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->forceScan, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->bssType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->channelListCount, buffer, &offset);
+    if (primitive->channelListCount)
+    {
+        primitive->channelList = kmalloc(primitive->channelListCount, GFP_KERNEL);
+        CsrMemCpyDes(primitive->channelList, buffer, &offset, ((u16) (primitive->channelListCount)));
+    }
+    else
+    {
+        primitive->channelList = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->probeIeLength, buffer, &offset);
+    if (primitive->probeIeLength)
+    {
+        primitive->probeIe = kmalloc(primitive->probeIeLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->probeIe, buffer, &offset, ((u16) (primitive->probeIeLength)));
+    }
+    else
+    {
+        primitive->probeIe = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeScanFullReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeScanFullReq *primitive = (CsrWifiSmeScanFullReq *) voidPrimitivePointer;
+    kfree(primitive->ssid);
+    kfree(primitive->channelList);
+    kfree(primitive->probeIe);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeSmeStaConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* u8 primitive->smeConfig.connectionQualityRssiChangeTrigger */
+    bufferSize += 1; /* u8 primitive->smeConfig.connectionQualitySnrChangeTrigger */
+    bufferSize += 1; /* CsrWifiSmeWmmModeMask primitive->smeConfig.wmmModeMask */
+    bufferSize += 1; /* CsrWifiSmeRadioIF primitive->smeConfig.ifIndex */
+    bufferSize += 1; /* u8 primitive->smeConfig.allowUnicastUseGroupCipher */
+    bufferSize += 1; /* u8 primitive->smeConfig.enableOpportunisticKeyCaching */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeSmeStaConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeSmeStaConfigSetReq *primitive = (CsrWifiSmeSmeStaConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.connectionQualityRssiChangeTrigger);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.connectionQualitySnrChangeTrigger);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.wmmModeMask);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.ifIndex);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.allowUnicastUseGroupCipher);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.enableOpportunisticKeyCaching);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeSmeStaConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeSmeStaConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeSmeStaConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.connectionQualityRssiChangeTrigger, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.connectionQualitySnrChangeTrigger, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.wmmModeMask, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.ifIndex, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.allowUnicastUseGroupCipher, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.enableOpportunisticKeyCaching, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeTspecReqSizeof(void *msg)
+{
+    CsrWifiSmeTspecReq *primitive = (CsrWifiSmeTspecReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 18) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 1;                      /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 4;                      /* u32 primitive->transactionId */
+    bufferSize += 1;                      /* u8 primitive->strict */
+    bufferSize += 1;                      /* CsrWifiSmeTspecCtrlMask primitive->ctrlMask */
+    bufferSize += 2;                      /* u16 primitive->tspecLength */
+    bufferSize += primitive->tspecLength; /* u8 primitive->tspec */
+    bufferSize += 2;                      /* u16 primitive->tclasLength */
+    bufferSize += primitive->tclasLength; /* u8 primitive->tclas */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeTspecReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeTspecReq *primitive = (CsrWifiSmeTspecReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint32Ser(ptr, len, (u32) primitive->transactionId);
+    CsrUint8Ser(ptr, len, (u8) primitive->strict);
+    CsrUint8Ser(ptr, len, (u8) primitive->ctrlMask);
+    CsrUint16Ser(ptr, len, (u16) primitive->tspecLength);
+    if (primitive->tspecLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->tspec, ((u16) (primitive->tspecLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->tclasLength);
+    if (primitive->tclasLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->tclas, ((u16) (primitive->tclasLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeTspecReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeTspecReq *primitive = kmalloc(sizeof(CsrWifiSmeTspecReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->transactionId, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->strict, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ctrlMask, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->tspecLength, buffer, &offset);
+    if (primitive->tspecLength)
+    {
+        primitive->tspec = kmalloc(primitive->tspecLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->tspec, buffer, &offset, ((u16) (primitive->tspecLength)));
+    }
+    else
+    {
+        primitive->tspec = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->tclasLength, buffer, &offset);
+    if (primitive->tclasLength)
+    {
+        primitive->tclas = kmalloc(primitive->tclasLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->tclas, buffer, &offset, ((u16) (primitive->tclasLength)));
+    }
+    else
+    {
+        primitive->tclas = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeTspecReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeTspecReq *primitive = (CsrWifiSmeTspecReq *) voidPrimitivePointer;
+    kfree(primitive->tspec);
+    kfree(primitive->tclas);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeWifiFlightmodeReqSizeof(void *msg)
+{
+    CsrWifiSmeWifiFlightmodeReq *primitive = (CsrWifiSmeWifiFlightmodeReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 14) */
+    bufferSize += 6; /* u8 primitive->address.a[6] */
+    bufferSize += 2; /* u16 primitive->mibFilesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            bufferSize += 2;                              /* u16 primitive->mibFiles[i1].length */
+            bufferSize += primitive->mibFiles[i1].length; /* u8 primitive->mibFiles[i1].data */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeWifiFlightmodeReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeWifiFlightmodeReq *primitive = (CsrWifiSmeWifiFlightmodeReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrMemCpySer(ptr, len, (const void *) primitive->address.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->mibFilesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->mibFiles[i1].length);
+            if (primitive->mibFiles[i1].length)
+            {
+                CsrMemCpySer(ptr, len, (const void *) primitive->mibFiles[i1].data, ((u16) (primitive->mibFiles[i1].length)));
+            }
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeWifiFlightmodeReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeWifiFlightmodeReq *primitive = kmalloc(sizeof(CsrWifiSmeWifiFlightmodeReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrMemCpyDes(primitive->address.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->mibFilesCount, buffer, &offset);
+    primitive->mibFiles = NULL;
+    if (primitive->mibFilesCount)
+    {
+        primitive->mibFiles = kmalloc(sizeof(CsrWifiSmeDataBlock) * primitive->mibFilesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            CsrUint16Des((u16 *) &primitive->mibFiles[i1].length, buffer, &offset);
+            if (primitive->mibFiles[i1].length)
+            {
+                primitive->mibFiles[i1].data = kmalloc(primitive->mibFiles[i1].length, GFP_KERNEL);
+                CsrMemCpyDes(primitive->mibFiles[i1].data, buffer, &offset, ((u16) (primitive->mibFiles[i1].length)));
+            }
+            else
+            {
+                primitive->mibFiles[i1].data = NULL;
+            }
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeWifiFlightmodeReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeWifiFlightmodeReq *primitive = (CsrWifiSmeWifiFlightmodeReq *) voidPrimitivePointer;
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            kfree(primitive->mibFiles[i1].data);
+        }
+    }
+    kfree(primitive->mibFiles);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeWifiOnReqSizeof(void *msg)
+{
+    CsrWifiSmeWifiOnReq *primitive = (CsrWifiSmeWifiOnReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 14) */
+    bufferSize += 6; /* u8 primitive->address.a[6] */
+    bufferSize += 2; /* u16 primitive->mibFilesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            bufferSize += 2;                              /* u16 primitive->mibFiles[i1].length */
+            bufferSize += primitive->mibFiles[i1].length; /* u8 primitive->mibFiles[i1].data */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeWifiOnReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeWifiOnReq *primitive = (CsrWifiSmeWifiOnReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrMemCpySer(ptr, len, (const void *) primitive->address.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->mibFilesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->mibFiles[i1].length);
+            if (primitive->mibFiles[i1].length)
+            {
+                CsrMemCpySer(ptr, len, (const void *) primitive->mibFiles[i1].data, ((u16) (primitive->mibFiles[i1].length)));
+            }
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeWifiOnReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeWifiOnReq *primitive = kmalloc(sizeof(CsrWifiSmeWifiOnReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrMemCpyDes(primitive->address.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->mibFilesCount, buffer, &offset);
+    primitive->mibFiles = NULL;
+    if (primitive->mibFilesCount)
+    {
+        primitive->mibFiles = kmalloc(sizeof(CsrWifiSmeDataBlock) * primitive->mibFilesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            CsrUint16Des((u16 *) &primitive->mibFiles[i1].length, buffer, &offset);
+            if (primitive->mibFiles[i1].length)
+            {
+                primitive->mibFiles[i1].data = kmalloc(primitive->mibFiles[i1].length, GFP_KERNEL);
+                CsrMemCpyDes(primitive->mibFiles[i1].data, buffer, &offset, ((u16) (primitive->mibFiles[i1].length)));
+            }
+            else
+            {
+                primitive->mibFiles[i1].data = NULL;
+            }
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeWifiOnReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeWifiOnReq *primitive = (CsrWifiSmeWifiOnReq *) voidPrimitivePointer;
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->mibFilesCount; i1++)
+        {
+            kfree(primitive->mibFiles[i1].data);
+        }
+    }
+    kfree(primitive->mibFiles);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCloakedSsidsSetReqSizeof(void *msg)
+{
+    CsrWifiSmeCloakedSsidsSetReq *primitive = (CsrWifiSmeCloakedSsidsSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 37) */
+    bufferSize += 1; /* u8 primitive->cloakedSsids.cloakedSsidsCount */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->cloakedSsids.cloakedSsidsCount; i2++)
+        {
+            bufferSize += 32; /* u8 primitive->cloakedSsids.cloakedSsids[i2].ssid[32] */
+            bufferSize += 1;  /* u8 primitive->cloakedSsids.cloakedSsids[i2].length */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCloakedSsidsSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCloakedSsidsSetReq *primitive = (CsrWifiSmeCloakedSsidsSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->cloakedSsids.cloakedSsidsCount);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->cloakedSsids.cloakedSsidsCount; i2++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->cloakedSsids.cloakedSsids[i2].ssid, ((u16) (32)));
+            CsrUint8Ser(ptr, len, (u8) primitive->cloakedSsids.cloakedSsids[i2].length);
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCloakedSsidsSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCloakedSsidsSetReq *primitive = kmalloc(sizeof(CsrWifiSmeCloakedSsidsSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->cloakedSsids.cloakedSsidsCount, buffer, &offset);
+    primitive->cloakedSsids.cloakedSsids = NULL;
+    if (primitive->cloakedSsids.cloakedSsidsCount)
+    {
+        primitive->cloakedSsids.cloakedSsids = kmalloc(sizeof(CsrWifiSsid) * primitive->cloakedSsids.cloakedSsidsCount, GFP_KERNEL);
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->cloakedSsids.cloakedSsidsCount; i2++)
+        {
+            CsrMemCpyDes(primitive->cloakedSsids.cloakedSsids[i2].ssid, buffer, &offset, ((u16) (32)));
+            CsrUint8Des((u8 *) &primitive->cloakedSsids.cloakedSsids[i2].length, buffer, &offset);
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeCloakedSsidsSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeCloakedSsidsSetReq *primitive = (CsrWifiSmeCloakedSsidsSetReq *) voidPrimitivePointer;
+    kfree(primitive->cloakedSsids.cloakedSsids);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeSmeCommonConfigSetReqSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 1; /* CsrWifiSme80211dTrustLevel primitive->deviceConfig.trustLevel */
+    bufferSize += 2; /* u8 primitive->deviceConfig.countryCode[2] */
+    bufferSize += 1; /* CsrWifiSmeFirmwareDriverInterface primitive->deviceConfig.firmwareDriverInterface */
+    bufferSize += 1; /* u8 primitive->deviceConfig.enableStrictDraftN */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeSmeCommonConfigSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeSmeCommonConfigSetReq *primitive = (CsrWifiSmeSmeCommonConfigSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->deviceConfig.trustLevel);
+    CsrMemCpySer(ptr, len, (const void *) primitive->deviceConfig.countryCode, ((u16) (2)));
+    CsrUint8Ser(ptr, len, (u8) primitive->deviceConfig.firmwareDriverInterface);
+    CsrUint8Ser(ptr, len, (u8) primitive->deviceConfig.enableStrictDraftN);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeSmeCommonConfigSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeSmeCommonConfigSetReq *primitive = kmalloc(sizeof(CsrWifiSmeSmeCommonConfigSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->deviceConfig.trustLevel, buffer, &offset);
+    CsrMemCpyDes(primitive->deviceConfig.countryCode, buffer, &offset, ((u16) (2)));
+    CsrUint8Des((u8 *) &primitive->deviceConfig.firmwareDriverInterface, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->deviceConfig.enableStrictDraftN, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeWpsConfigurationReqSizeof(void *msg)
+{
+    CsrWifiSmeWpsConfigurationReq *primitive = (CsrWifiSmeWpsConfigurationReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 240) */
+    bufferSize += 1;  /* u8 primitive->wpsConfig.wpsVersion */
+    bufferSize += 16; /* u8 primitive->wpsConfig.uuid[16] */
+    bufferSize += 32; /* u8 primitive->wpsConfig.deviceName[32] */
+    bufferSize += 1;  /* u8 primitive->wpsConfig.deviceNameLength */
+    bufferSize += 64; /* u8 primitive->wpsConfig.manufacturer[64] */
+    bufferSize += 1;  /* u8 primitive->wpsConfig.manufacturerLength */
+    bufferSize += 32; /* u8 primitive->wpsConfig.modelName[32] */
+    bufferSize += 1;  /* u8 primitive->wpsConfig.modelNameLength */
+    bufferSize += 32; /* u8 primitive->wpsConfig.modelNumber[32] */
+    bufferSize += 1;  /* u8 primitive->wpsConfig.modelNumberLength */
+    bufferSize += 32; /* u8 primitive->wpsConfig.serialNumber[32] */
+    bufferSize += 8;  /* u8 primitive->wpsConfig.primDeviceType.deviceDetails[8] */
+    bufferSize += 1;  /* u8 primitive->wpsConfig.secondaryDeviceTypeCount */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->wpsConfig.secondaryDeviceTypeCount; i2++)
+        {
+            bufferSize += 8; /* u8 primitive->wpsConfig.secondaryDeviceType[i2].deviceDetails[8] */
+        }
+    }
+    bufferSize += 2;         /* CsrWifiSmeWpsConfigTypeMask primitive->wpsConfig.configMethods */
+    bufferSize += 1;         /* u8 primitive->wpsConfig.rfBands */
+    bufferSize += 4;         /* u8 primitive->wpsConfig.osVersion[4] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeWpsConfigurationReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeWpsConfigurationReq *primitive = (CsrWifiSmeWpsConfigurationReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.wpsVersion);
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.uuid, ((u16) (16)));
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.deviceName, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.deviceNameLength);
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.manufacturer, ((u16) (64)));
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.manufacturerLength);
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.modelName, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.modelNameLength);
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.modelNumber, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.modelNumberLength);
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.serialNumber, ((u16) (32)));
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.primDeviceType.deviceDetails, ((u16) (8)));
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.secondaryDeviceTypeCount);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->wpsConfig.secondaryDeviceTypeCount; i2++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.secondaryDeviceType[i2].deviceDetails, ((u16) (8)));
+        }
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->wpsConfig.configMethods);
+    CsrUint8Ser(ptr, len, (u8) primitive->wpsConfig.rfBands);
+    CsrMemCpySer(ptr, len, (const void *) primitive->wpsConfig.osVersion, ((u16) (4)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmeWpsConfigurationReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeWpsConfigurationReq *primitive = kmalloc(sizeof(CsrWifiSmeWpsConfigurationReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->wpsConfig.wpsVersion, buffer, &offset);
+    CsrMemCpyDes(primitive->wpsConfig.uuid, buffer, &offset, ((u16) (16)));
+    CsrMemCpyDes(primitive->wpsConfig.deviceName, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->wpsConfig.deviceNameLength, buffer, &offset);
+    CsrMemCpyDes(primitive->wpsConfig.manufacturer, buffer, &offset, ((u16) (64)));
+    CsrUint8Des((u8 *) &primitive->wpsConfig.manufacturerLength, buffer, &offset);
+    CsrMemCpyDes(primitive->wpsConfig.modelName, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->wpsConfig.modelNameLength, buffer, &offset);
+    CsrMemCpyDes(primitive->wpsConfig.modelNumber, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->wpsConfig.modelNumberLength, buffer, &offset);
+    CsrMemCpyDes(primitive->wpsConfig.serialNumber, buffer, &offset, ((u16) (32)));
+    CsrMemCpyDes(primitive->wpsConfig.primDeviceType.deviceDetails, buffer, &offset, ((u16) (8)));
+    CsrUint8Des((u8 *) &primitive->wpsConfig.secondaryDeviceTypeCount, buffer, &offset);
+    primitive->wpsConfig.secondaryDeviceType = NULL;
+    if (primitive->wpsConfig.secondaryDeviceTypeCount)
+    {
+        primitive->wpsConfig.secondaryDeviceType = kmalloc(sizeof(CsrWifiSmeWpsDeviceType) * primitive->wpsConfig.secondaryDeviceTypeCount, GFP_KERNEL);
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->wpsConfig.secondaryDeviceTypeCount; i2++)
+        {
+            CsrMemCpyDes(primitive->wpsConfig.secondaryDeviceType[i2].deviceDetails, buffer, &offset, ((u16) (8)));
+        }
+    }
+    CsrUint16Des((u16 *) &primitive->wpsConfig.configMethods, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->wpsConfig.rfBands, buffer, &offset);
+    CsrMemCpyDes(primitive->wpsConfig.osVersion, buffer, &offset, ((u16) (4)));
+
+    return primitive;
+}
+
+
+void CsrWifiSmeWpsConfigurationReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeWpsConfigurationReq *primitive = (CsrWifiSmeWpsConfigurationReq *) voidPrimitivePointer;
+    kfree(primitive->wpsConfig.secondaryDeviceType);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeSetReqSizeof(void *msg)
+{
+    CsrWifiSmeSetReq *primitive = (CsrWifiSmeSetReq *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 4;                     /* u32 primitive->dataLength */
+    bufferSize += primitive->dataLength; /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeSetReqSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeSetReq *primitive = (CsrWifiSmeSetReq *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint32Ser(ptr, len, (u32) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeSetReqDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeSetReq *primitive = kmalloc(sizeof(CsrWifiSmeSetReq), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeSetReqSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeSetReq *primitive = (CsrWifiSmeSetReq *) voidPrimitivePointer;
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeAdhocConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 2; /* u16 primitive->adHocConfig.atimWindowTu */
+    bufferSize += 2; /* u16 primitive->adHocConfig.beaconPeriodTu */
+    bufferSize += 2; /* u16 primitive->adHocConfig.joinOnlyAttempts */
+    bufferSize += 2; /* u16 primitive->adHocConfig.joinAttemptIntervalMs */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeAdhocConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeAdhocConfigGetCfm *primitive = (CsrWifiSmeAdhocConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.atimWindowTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.beaconPeriodTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.joinOnlyAttempts);
+    CsrUint16Ser(ptr, len, (u16) primitive->adHocConfig.joinAttemptIntervalMs);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeAdhocConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeAdhocConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeAdhocConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.atimWindowTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.beaconPeriodTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.joinOnlyAttempts, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->adHocConfig.joinAttemptIntervalMs, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeAssociationCompleteIndSizeof(void *msg)
+{
+    CsrWifiSmeAssociationCompleteInd *primitive = (CsrWifiSmeAssociationCompleteInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 98) */
+    bufferSize += 2;                                                     /* u16 primitive->interfaceTag */
+    bufferSize += 2;                                                     /* CsrResult primitive->status */
+    bufferSize += 32;                                                    /* u8 primitive->connectionInfo.ssid.ssid[32] */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.ssid.length */
+    bufferSize += 6;                                                     /* u8 primitive->connectionInfo.bssid.a[6] */
+    bufferSize += 1;                                                     /* CsrWifiSme80211NetworkType primitive->connectionInfo.networkType80211 */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.channelNumber */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.channelFrequency */
+    bufferSize += 2;                                                     /* CsrWifiSmeAuthMode primitive->connectionInfo.authMode */
+    bufferSize += 2;                                                     /* CsrWifiSmeEncryption primitive->connectionInfo.pairwiseCipher */
+    bufferSize += 2;                                                     /* CsrWifiSmeEncryption primitive->connectionInfo.groupCipher */
+    bufferSize += 1;                                                     /* CsrWifiSmeRadioIF primitive->connectionInfo.ifIndex */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.atimWindowTu */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.beaconPeriodTu */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.reassociation */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.beaconFrameLength */
+    bufferSize += primitive->connectionInfo.beaconFrameLength;           /* u8 primitive->connectionInfo.beaconFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.associationReqFrameLength */
+    bufferSize += primitive->connectionInfo.associationReqFrameLength;   /* u8 primitive->connectionInfo.associationReqFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.associationRspFrameLength */
+    bufferSize += primitive->connectionInfo.associationRspFrameLength;   /* u8 primitive->connectionInfo.associationRspFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocScanInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocScanInfoElementsLength; /* u8 primitive->connectionInfo.assocScanInfoElements */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqCapabilities */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqListenIntervalTu */
+    bufferSize += 6;                                                     /* u8 primitive->connectionInfo.assocReqApAddress.a[6] */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocReqInfoElementsLength;  /* u8 primitive->connectionInfo.assocReqInfoElements */
+    bufferSize += 2;                                                     /* CsrWifiSmeIEEE80211Result primitive->connectionInfo.assocRspResult */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspCapabilityInfo */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspAssociationId */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocRspInfoElementsLength;  /* u8 primitive->connectionInfo.assocRspInfoElements */
+    bufferSize += 2;                                                     /* CsrWifiSmeIEEE80211Reason primitive->deauthReason */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeAssociationCompleteIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeAssociationCompleteInd *primitive = (CsrWifiSmeAssociationCompleteInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.ssid.length);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.networkType80211);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.channelNumber);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.channelFrequency);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.authMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.pairwiseCipher);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.groupCipher);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.ifIndex);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.atimWindowTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.beaconPeriodTu);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.reassociation);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.beaconFrameLength);
+    if (primitive->connectionInfo.beaconFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.beaconFrame, ((u16) (primitive->connectionInfo.beaconFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.associationReqFrameLength);
+    if (primitive->connectionInfo.associationReqFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.associationReqFrame, ((u16) (primitive->connectionInfo.associationReqFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.associationRspFrameLength);
+    if (primitive->connectionInfo.associationRspFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.associationRspFrame, ((u16) (primitive->connectionInfo.associationRspFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocScanInfoElementsLength);
+    if (primitive->connectionInfo.assocScanInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocScanInfoElements, ((u16) (primitive->connectionInfo.assocScanInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqCapabilities);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqListenIntervalTu);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocReqApAddress.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqInfoElementsLength);
+    if (primitive->connectionInfo.assocReqInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocReqInfoElements, ((u16) (primitive->connectionInfo.assocReqInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspResult);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspCapabilityInfo);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspAssociationId);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspInfoElementsLength);
+    if (primitive->connectionInfo.assocRspInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocRspInfoElements, ((u16) (primitive->connectionInfo.assocRspInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->deauthReason);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeAssociationCompleteIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeAssociationCompleteInd *primitive = kmalloc(sizeof(CsrWifiSmeAssociationCompleteInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->connectionInfo.ssid.length, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->connectionInfo.networkType80211, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.channelNumber, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.channelFrequency, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.authMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.pairwiseCipher, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.groupCipher, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.ifIndex, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.atimWindowTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.beaconPeriodTu, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.reassociation, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.beaconFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.beaconFrameLength)
+    {
+        primitive->connectionInfo.beaconFrame = kmalloc(primitive->connectionInfo.beaconFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.beaconFrame, buffer, &offset, ((u16) (primitive->connectionInfo.beaconFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.beaconFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.associationReqFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.associationReqFrameLength)
+    {
+        primitive->connectionInfo.associationReqFrame = kmalloc(primitive->connectionInfo.associationReqFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.associationReqFrame, buffer, &offset, ((u16) (primitive->connectionInfo.associationReqFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.associationReqFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.associationRspFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.associationRspFrameLength)
+    {
+        primitive->connectionInfo.associationRspFrame = kmalloc(primitive->connectionInfo.associationRspFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.associationRspFrame, buffer, &offset, ((u16) (primitive->connectionInfo.associationRspFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.associationRspFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocScanInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocScanInfoElementsLength)
+    {
+        primitive->connectionInfo.assocScanInfoElements = kmalloc(primitive->connectionInfo.assocScanInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocScanInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocScanInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocScanInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqCapabilities, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqListenIntervalTu, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.assocReqApAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocReqInfoElementsLength)
+    {
+        primitive->connectionInfo.assocReqInfoElements = kmalloc(primitive->connectionInfo.assocReqInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocReqInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocReqInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocReqInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspResult, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspCapabilityInfo, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspAssociationId, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocRspInfoElementsLength)
+    {
+        primitive->connectionInfo.assocRspInfoElements = kmalloc(primitive->connectionInfo.assocRspInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocRspInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocRspInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocRspInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->deauthReason, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeAssociationCompleteIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeAssociationCompleteInd *primitive = (CsrWifiSmeAssociationCompleteInd *) voidPrimitivePointer;
+    kfree(primitive->connectionInfo.beaconFrame);
+    kfree(primitive->connectionInfo.associationReqFrame);
+    kfree(primitive->connectionInfo.associationRspFrame);
+    kfree(primitive->connectionInfo.assocScanInfoElements);
+    kfree(primitive->connectionInfo.assocReqInfoElements);
+    kfree(primitive->connectionInfo.assocRspInfoElements);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeAssociationStartIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 44) */
+    bufferSize += 2;  /* u16 primitive->interfaceTag */
+    bufferSize += 6;  /* u8 primitive->address.a[6] */
+    bufferSize += 32; /* u8 primitive->ssid.ssid[32] */
+    bufferSize += 1;  /* u8 primitive->ssid.length */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeAssociationStartIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeAssociationStartInd *primitive = (CsrWifiSmeAssociationStartInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrMemCpySer(ptr, len, (const void *) primitive->address.a, ((u16) (6)));
+    CsrMemCpySer(ptr, len, (const void *) primitive->ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->ssid.length);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeAssociationStartIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeAssociationStartInd *primitive = kmalloc(sizeof(CsrWifiSmeAssociationStartInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrMemCpyDes(primitive->address.a, buffer, &offset, ((u16) (6)));
+    CsrMemCpyDes(primitive->ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->ssid.length, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeBlacklistCfmSizeof(void *msg)
+{
+    CsrWifiSmeBlacklistCfm *primitive = (CsrWifiSmeBlacklistCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->getAddressCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->getAddresses[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeBlacklistCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeBlacklistCfm *primitive = (CsrWifiSmeBlacklistCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->getAddressCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->getAddresses[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeBlacklistCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeBlacklistCfm *primitive = kmalloc(sizeof(CsrWifiSmeBlacklistCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->getAddressCount, buffer, &offset);
+    primitive->getAddresses = NULL;
+    if (primitive->getAddressCount)
+    {
+        primitive->getAddresses = kmalloc(sizeof(CsrWifiMacAddress) * primitive->getAddressCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressCount; i1++)
+        {
+            CsrMemCpyDes(primitive->getAddresses[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeBlacklistCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeBlacklistCfm *primitive = (CsrWifiSmeBlacklistCfm *) voidPrimitivePointer;
+    kfree(primitive->getAddresses);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCalibrationDataGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeCalibrationDataGetCfm *primitive = (CsrWifiSmeCalibrationDataGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2;                                /* CsrResult primitive->status */
+    bufferSize += 2;                                /* u16 primitive->calibrationDataLength */
+    bufferSize += primitive->calibrationDataLength; /* u8 primitive->calibrationData */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCalibrationDataGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCalibrationDataGetCfm *primitive = (CsrWifiSmeCalibrationDataGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->calibrationDataLength);
+    if (primitive->calibrationDataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->calibrationData, ((u16) (primitive->calibrationDataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCalibrationDataGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCalibrationDataGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeCalibrationDataGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->calibrationDataLength, buffer, &offset);
+    if (primitive->calibrationDataLength)
+    {
+        primitive->calibrationData = kmalloc(primitive->calibrationDataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->calibrationData, buffer, &offset, ((u16) (primitive->calibrationDataLength)));
+    }
+    else
+    {
+        primitive->calibrationData = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeCalibrationDataGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeCalibrationDataGetCfm *primitive = (CsrWifiSmeCalibrationDataGetCfm *) voidPrimitivePointer;
+    kfree(primitive->calibrationData);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCcxConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->ccxConfig.keepAliveTimeMs */
+    bufferSize += 1; /* u8 primitive->ccxConfig.apRoamingEnabled */
+    bufferSize += 1; /* u8 primitive->ccxConfig.measurementsMask */
+    bufferSize += 1; /* u8 primitive->ccxConfig.ccxRadioMgtEnabled */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCcxConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCcxConfigGetCfm *primitive = (CsrWifiSmeCcxConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.keepAliveTimeMs);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.apRoamingEnabled);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.measurementsMask);
+    CsrUint8Ser(ptr, len, (u8) primitive->ccxConfig.ccxRadioMgtEnabled);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCcxConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCcxConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeCcxConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.keepAliveTimeMs, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.apRoamingEnabled, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.measurementsMask, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->ccxConfig.ccxRadioMgtEnabled, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeCcxConfigSetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCcxConfigSetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCcxConfigSetCfm *primitive = (CsrWifiSmeCcxConfigSetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCcxConfigSetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCcxConfigSetCfm *primitive = kmalloc(sizeof(CsrWifiSmeCcxConfigSetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeCoexConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 31) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->coexConfig.coexEnableSchemeManagement */
+    bufferSize += 1; /* u8 primitive->coexConfig.coexPeriodicWakeHost */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexTrafficBurstyLatencyMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexTrafficContinuousLatencyMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexObexBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexObexBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpBrBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpBrBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpEdrBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexA2dpEdrBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexPagingBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexPagingBlackoutPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexInquiryBlackoutDurationMs */
+    bufferSize += 2; /* u16 primitive->coexConfig.coexInquiryBlackoutPeriodMs */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCoexConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCoexConfigGetCfm *primitive = (CsrWifiSmeCoexConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexConfig.coexEnableSchemeManagement);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexConfig.coexPeriodicWakeHost);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexTrafficBurstyLatencyMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexTrafficContinuousLatencyMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexObexBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexObexBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpBrBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpBrBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpEdrBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexA2dpEdrBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexPagingBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexPagingBlackoutPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexInquiryBlackoutDurationMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexConfig.coexInquiryBlackoutPeriodMs);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCoexConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCoexConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeCoexConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexConfig.coexEnableSchemeManagement, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexConfig.coexPeriodicWakeHost, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexTrafficBurstyLatencyMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexTrafficContinuousLatencyMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexObexBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexObexBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpBrBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpBrBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpEdrBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexA2dpEdrBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexPagingBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexPagingBlackoutPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexInquiryBlackoutDurationMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexConfig.coexInquiryBlackoutPeriodMs, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeCoexInfoGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 24) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->coexInfo.hasTrafficData */
+    bufferSize += 1; /* CsrWifiSmeTrafficType primitive->coexInfo.currentTrafficType */
+    bufferSize += 2; /* u16 primitive->coexInfo.currentPeriodMs */
+    bufferSize += 1; /* CsrWifiSmePowerSaveLevel primitive->coexInfo.currentPowerSave */
+    bufferSize += 2; /* u16 primitive->coexInfo.currentCoexPeriodMs */
+    bufferSize += 2; /* u16 primitive->coexInfo.currentCoexLatencyMs */
+    bufferSize += 1; /* u8 primitive->coexInfo.hasBtDevice */
+    bufferSize += 4; /* u32 primitive->coexInfo.currentBlackoutDurationUs */
+    bufferSize += 4; /* u32 primitive->coexInfo.currentBlackoutPeriodUs */
+    bufferSize += 1; /* CsrWifiSmeCoexScheme primitive->coexInfo.currentCoexScheme */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCoexInfoGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCoexInfoGetCfm *primitive = (CsrWifiSmeCoexInfoGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexInfo.hasTrafficData);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexInfo.currentTrafficType);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexInfo.currentPeriodMs);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexInfo.currentPowerSave);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexInfo.currentCoexPeriodMs);
+    CsrUint16Ser(ptr, len, (u16) primitive->coexInfo.currentCoexLatencyMs);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexInfo.hasBtDevice);
+    CsrUint32Ser(ptr, len, (u32) primitive->coexInfo.currentBlackoutDurationUs);
+    CsrUint32Ser(ptr, len, (u32) primitive->coexInfo.currentBlackoutPeriodUs);
+    CsrUint8Ser(ptr, len, (u8) primitive->coexInfo.currentCoexScheme);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCoexInfoGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCoexInfoGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeCoexInfoGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexInfo.hasTrafficData, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexInfo.currentTrafficType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexInfo.currentPeriodMs, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexInfo.currentPowerSave, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexInfo.currentCoexPeriodMs, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->coexInfo.currentCoexLatencyMs, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexInfo.hasBtDevice, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->coexInfo.currentBlackoutDurationUs, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->coexInfo.currentBlackoutPeriodUs, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->coexInfo.currentCoexScheme, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeConnectCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeConnectCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeConnectCfm *primitive = (CsrWifiSmeConnectCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeConnectCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeConnectCfm *primitive = kmalloc(sizeof(CsrWifiSmeConnectCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeConnectionConfigGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeConnectionConfigGetCfm *primitive = (CsrWifiSmeConnectionConfigGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 59) */
+    bufferSize += 2;                                                                     /* u16 primitive->interfaceTag */
+    bufferSize += 2;                                                                     /* CsrResult primitive->status */
+    bufferSize += 32;                                                                    /* u8 primitive->connectionConfig.ssid.ssid[32] */
+    bufferSize += 1;                                                                     /* u8 primitive->connectionConfig.ssid.length */
+    bufferSize += 6;                                                                     /* u8 primitive->connectionConfig.bssid.a[6] */
+    bufferSize += 1;                                                                     /* CsrWifiSmeBssType primitive->connectionConfig.bssType */
+    bufferSize += 1;                                                                     /* CsrWifiSmeRadioIF primitive->connectionConfig.ifIndex */
+    bufferSize += 1;                                                                     /* CsrWifiSme80211PrivacyMode primitive->connectionConfig.privacyMode */
+    bufferSize += 2;                                                                     /* CsrWifiSmeAuthModeMask primitive->connectionConfig.authModeMask */
+    bufferSize += 2;                                                                     /* CsrWifiSmeEncryptionMask primitive->connectionConfig.encryptionModeMask */
+    bufferSize += 2;                                                                     /* u16 primitive->connectionConfig.mlmeAssociateReqInformationElementsLength */
+    bufferSize += primitive->connectionConfig.mlmeAssociateReqInformationElementsLength; /* u8 primitive->connectionConfig.mlmeAssociateReqInformationElements */
+    bufferSize += 1;                                                                     /* CsrWifiSmeWmmQosInfoMask primitive->connectionConfig.wmmQosInfo */
+    bufferSize += 1;                                                                     /* u8 primitive->connectionConfig.adhocJoinOnly */
+    bufferSize += 1;                                                                     /* u8 primitive->connectionConfig.adhocChannel */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeConnectionConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeConnectionConfigGetCfm *primitive = (CsrWifiSmeConnectionConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionConfig.ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.ssid.length);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionConfig.bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.bssType);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.ifIndex);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.privacyMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionConfig.authModeMask);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionConfig.encryptionModeMask);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionConfig.mlmeAssociateReqInformationElementsLength);
+    if (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionConfig.mlmeAssociateReqInformationElements, ((u16) (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)));
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.wmmQosInfo);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.adhocJoinOnly);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionConfig.adhocChannel);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeConnectionConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeConnectionConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeConnectionConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionConfig.ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->connectionConfig.ssid.length, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionConfig.bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->connectionConfig.bssType, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.ifIndex, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.privacyMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionConfig.authModeMask, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionConfig.encryptionModeMask, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionConfig.mlmeAssociateReqInformationElementsLength, buffer, &offset);
+    if (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)
+    {
+        primitive->connectionConfig.mlmeAssociateReqInformationElements = kmalloc(primitive->connectionConfig.mlmeAssociateReqInformationElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionConfig.mlmeAssociateReqInformationElements, buffer, &offset, ((u16) (primitive->connectionConfig.mlmeAssociateReqInformationElementsLength)));
+    }
+    else
+    {
+        primitive->connectionConfig.mlmeAssociateReqInformationElements = NULL;
+    }
+    CsrUint8Des((u8 *) &primitive->connectionConfig.wmmQosInfo, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.adhocJoinOnly, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionConfig.adhocChannel, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeConnectionConfigGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeConnectionConfigGetCfm *primitive = (CsrWifiSmeConnectionConfigGetCfm *) voidPrimitivePointer;
+    kfree(primitive->connectionConfig.mlmeAssociateReqInformationElements);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeConnectionInfoGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeConnectionInfoGetCfm *primitive = (CsrWifiSmeConnectionInfoGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 96) */
+    bufferSize += 2;                                                     /* u16 primitive->interfaceTag */
+    bufferSize += 2;                                                     /* CsrResult primitive->status */
+    bufferSize += 32;                                                    /* u8 primitive->connectionInfo.ssid.ssid[32] */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.ssid.length */
+    bufferSize += 6;                                                     /* u8 primitive->connectionInfo.bssid.a[6] */
+    bufferSize += 1;                                                     /* CsrWifiSme80211NetworkType primitive->connectionInfo.networkType80211 */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.channelNumber */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.channelFrequency */
+    bufferSize += 2;                                                     /* CsrWifiSmeAuthMode primitive->connectionInfo.authMode */
+    bufferSize += 2;                                                     /* CsrWifiSmeEncryption primitive->connectionInfo.pairwiseCipher */
+    bufferSize += 2;                                                     /* CsrWifiSmeEncryption primitive->connectionInfo.groupCipher */
+    bufferSize += 1;                                                     /* CsrWifiSmeRadioIF primitive->connectionInfo.ifIndex */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.atimWindowTu */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.beaconPeriodTu */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.reassociation */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.beaconFrameLength */
+    bufferSize += primitive->connectionInfo.beaconFrameLength;           /* u8 primitive->connectionInfo.beaconFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.associationReqFrameLength */
+    bufferSize += primitive->connectionInfo.associationReqFrameLength;   /* u8 primitive->connectionInfo.associationReqFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.associationRspFrameLength */
+    bufferSize += primitive->connectionInfo.associationRspFrameLength;   /* u8 primitive->connectionInfo.associationRspFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocScanInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocScanInfoElementsLength; /* u8 primitive->connectionInfo.assocScanInfoElements */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqCapabilities */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqListenIntervalTu */
+    bufferSize += 6;                                                     /* u8 primitive->connectionInfo.assocReqApAddress.a[6] */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocReqInfoElementsLength;  /* u8 primitive->connectionInfo.assocReqInfoElements */
+    bufferSize += 2;                                                     /* CsrWifiSmeIEEE80211Result primitive->connectionInfo.assocRspResult */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspCapabilityInfo */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspAssociationId */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocRspInfoElementsLength;  /* u8 primitive->connectionInfo.assocRspInfoElements */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeConnectionInfoGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeConnectionInfoGetCfm *primitive = (CsrWifiSmeConnectionInfoGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.ssid.length);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.networkType80211);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.channelNumber);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.channelFrequency);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.authMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.pairwiseCipher);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.groupCipher);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.ifIndex);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.atimWindowTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.beaconPeriodTu);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.reassociation);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.beaconFrameLength);
+    if (primitive->connectionInfo.beaconFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.beaconFrame, ((u16) (primitive->connectionInfo.beaconFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.associationReqFrameLength);
+    if (primitive->connectionInfo.associationReqFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.associationReqFrame, ((u16) (primitive->connectionInfo.associationReqFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.associationRspFrameLength);
+    if (primitive->connectionInfo.associationRspFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.associationRspFrame, ((u16) (primitive->connectionInfo.associationRspFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocScanInfoElementsLength);
+    if (primitive->connectionInfo.assocScanInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocScanInfoElements, ((u16) (primitive->connectionInfo.assocScanInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqCapabilities);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqListenIntervalTu);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocReqApAddress.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqInfoElementsLength);
+    if (primitive->connectionInfo.assocReqInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocReqInfoElements, ((u16) (primitive->connectionInfo.assocReqInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspResult);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspCapabilityInfo);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspAssociationId);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspInfoElementsLength);
+    if (primitive->connectionInfo.assocRspInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocRspInfoElements, ((u16) (primitive->connectionInfo.assocRspInfoElementsLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeConnectionInfoGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeConnectionInfoGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeConnectionInfoGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->connectionInfo.ssid.length, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->connectionInfo.networkType80211, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.channelNumber, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.channelFrequency, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.authMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.pairwiseCipher, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.groupCipher, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.ifIndex, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.atimWindowTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.beaconPeriodTu, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.reassociation, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.beaconFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.beaconFrameLength)
+    {
+        primitive->connectionInfo.beaconFrame = kmalloc(primitive->connectionInfo.beaconFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.beaconFrame, buffer, &offset, ((u16) (primitive->connectionInfo.beaconFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.beaconFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.associationReqFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.associationReqFrameLength)
+    {
+        primitive->connectionInfo.associationReqFrame = kmalloc(primitive->connectionInfo.associationReqFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.associationReqFrame, buffer, &offset, ((u16) (primitive->connectionInfo.associationReqFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.associationReqFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.associationRspFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.associationRspFrameLength)
+    {
+        primitive->connectionInfo.associationRspFrame = kmalloc(primitive->connectionInfo.associationRspFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.associationRspFrame, buffer, &offset, ((u16) (primitive->connectionInfo.associationRspFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.associationRspFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocScanInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocScanInfoElementsLength)
+    {
+        primitive->connectionInfo.assocScanInfoElements = kmalloc(primitive->connectionInfo.assocScanInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocScanInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocScanInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocScanInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqCapabilities, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqListenIntervalTu, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.assocReqApAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocReqInfoElementsLength)
+    {
+        primitive->connectionInfo.assocReqInfoElements = kmalloc(primitive->connectionInfo.assocReqInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocReqInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocReqInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocReqInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspResult, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspCapabilityInfo, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspAssociationId, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocRspInfoElementsLength)
+    {
+        primitive->connectionInfo.assocRspInfoElements = kmalloc(primitive->connectionInfo.assocRspInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocRspInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocRspInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocRspInfoElements = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeConnectionInfoGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeConnectionInfoGetCfm *primitive = (CsrWifiSmeConnectionInfoGetCfm *) voidPrimitivePointer;
+    kfree(primitive->connectionInfo.beaconFrame);
+    kfree(primitive->connectionInfo.associationReqFrame);
+    kfree(primitive->connectionInfo.associationRspFrame);
+    kfree(primitive->connectionInfo.assocScanInfoElements);
+    kfree(primitive->connectionInfo.assocReqInfoElements);
+    kfree(primitive->connectionInfo.assocRspInfoElements);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeConnectionQualityIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* s16 primitive->linkQuality.unifiRssi */
+    bufferSize += 2; /* s16 primitive->linkQuality.unifiSnr */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeConnectionQualityIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeConnectionQualityInd *primitive = (CsrWifiSmeConnectionQualityInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->linkQuality.unifiRssi);
+    CsrUint16Ser(ptr, len, (u16) primitive->linkQuality.unifiSnr);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeConnectionQualityIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeConnectionQualityInd *primitive = kmalloc(sizeof(CsrWifiSmeConnectionQualityInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->linkQuality.unifiRssi, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->linkQuality.unifiSnr, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeConnectionStatsGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 101) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->connectionStats.unifiTxDataRate */
+    bufferSize += 1; /* u8 primitive->connectionStats.unifiRxDataRate */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RetryCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11MultipleRetryCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11AckFailureCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11FrameDuplicateCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11FcsErrorCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RtsSuccessCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RtsFailureCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11FailedCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11TransmittedFragmentCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11TransmittedFrameCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11WepExcludedCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11WepIcvErrorCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11WepUndecryptableCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11MulticastReceivedFrameCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11MulticastTransmittedFrameCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11ReceivedFragmentCount */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11Rsna4WayHandshakeFailures */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RsnaTkipCounterMeasuresInvoked */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RsnaStatsTkipLocalMicFailures */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RsnaStatsTkipReplays */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RsnaStatsTkipIcvErrors */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RsnaStatsCcmpReplays */
+    bufferSize += 4; /* u32 primitive->connectionStats.dot11RsnaStatsCcmpDecryptErrors */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeConnectionStatsGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeConnectionStatsGetCfm *primitive = (CsrWifiSmeConnectionStatsGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionStats.unifiTxDataRate);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionStats.unifiRxDataRate);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RetryCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11MultipleRetryCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11AckFailureCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11FrameDuplicateCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11FcsErrorCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RtsSuccessCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RtsFailureCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11FailedCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11TransmittedFragmentCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11TransmittedFrameCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11WepExcludedCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11WepIcvErrorCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11WepUndecryptableCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11MulticastReceivedFrameCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11MulticastTransmittedFrameCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11ReceivedFragmentCount);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11Rsna4WayHandshakeFailures);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RsnaTkipCounterMeasuresInvoked);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RsnaStatsTkipLocalMicFailures);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RsnaStatsTkipReplays);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RsnaStatsTkipIcvErrors);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RsnaStatsCcmpReplays);
+    CsrUint32Ser(ptr, len, (u32) primitive->connectionStats.dot11RsnaStatsCcmpDecryptErrors);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeConnectionStatsGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeConnectionStatsGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeConnectionStatsGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionStats.unifiTxDataRate, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionStats.unifiRxDataRate, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RetryCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11MultipleRetryCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11AckFailureCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11FrameDuplicateCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11FcsErrorCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RtsSuccessCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RtsFailureCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11FailedCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11TransmittedFragmentCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11TransmittedFrameCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11WepExcludedCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11WepIcvErrorCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11WepUndecryptableCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11MulticastReceivedFrameCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11MulticastTransmittedFrameCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11ReceivedFragmentCount, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11Rsna4WayHandshakeFailures, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RsnaTkipCounterMeasuresInvoked, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RsnaStatsTkipLocalMicFailures, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RsnaStatsTkipReplays, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RsnaStatsTkipIcvErrors, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RsnaStatsCcmpReplays, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->connectionStats.dot11RsnaStatsCcmpDecryptErrors, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeDisconnectCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeDisconnectCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeDisconnectCfm *primitive = (CsrWifiSmeDisconnectCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeDisconnectCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeDisconnectCfm *primitive = kmalloc(sizeof(CsrWifiSmeDisconnectCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeHostConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSmeHostPowerMode primitive->hostConfig.powerMode */
+    bufferSize += 2; /* u16 primitive->hostConfig.applicationDataPeriodMs */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeHostConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeHostConfigGetCfm *primitive = (CsrWifiSmeHostConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->hostConfig.powerMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->hostConfig.applicationDataPeriodMs);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeHostConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeHostConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeHostConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->hostConfig.powerMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->hostConfig.applicationDataPeriodMs, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeHostConfigSetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeHostConfigSetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeHostConfigSetCfm *primitive = (CsrWifiSmeHostConfigSetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeHostConfigSetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeHostConfigSetCfm *primitive = kmalloc(sizeof(CsrWifiSmeHostConfigSetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeIbssStationIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 6; /* u8 primitive->address.a[6] */
+    bufferSize += 1; /* u8 primitive->isconnected */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeIbssStationIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeIbssStationInd *primitive = (CsrWifiSmeIbssStationInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrMemCpySer(ptr, len, (const void *) primitive->address.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->isconnected);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeIbssStationIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeIbssStationInd *primitive = kmalloc(sizeof(CsrWifiSmeIbssStationInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrMemCpyDes(primitive->address.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->isconnected, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeKeyCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* CsrWifiSmeKeyType primitive->keyType */
+    bufferSize += 6; /* u8 primitive->peerMacAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeKeyCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeKeyCfm *primitive = (CsrWifiSmeKeyCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->keyType);
+    CsrMemCpySer(ptr, len, (const void *) primitive->peerMacAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmeKeyCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeKeyCfm *primitive = kmalloc(sizeof(CsrWifiSmeKeyCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->keyType, buffer, &offset);
+    CsrMemCpyDes(primitive->peerMacAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeLinkQualityGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 2; /* s16 primitive->linkQuality.unifiRssi */
+    bufferSize += 2; /* s16 primitive->linkQuality.unifiSnr */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeLinkQualityGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeLinkQualityGetCfm *primitive = (CsrWifiSmeLinkQualityGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->linkQuality.unifiRssi);
+    CsrUint16Ser(ptr, len, (u16) primitive->linkQuality.unifiSnr);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeLinkQualityGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeLinkQualityGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeLinkQualityGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->linkQuality.unifiRssi, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->linkQuality.unifiSnr, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeMediaStatusIndSizeof(void *msg)
+{
+    CsrWifiSmeMediaStatusInd *primitive = (CsrWifiSmeMediaStatusInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 99) */
+    bufferSize += 2;                                                     /* u16 primitive->interfaceTag */
+    bufferSize += 1;                                                     /* CsrWifiSmeMediaStatus primitive->mediaStatus */
+    bufferSize += 32;                                                    /* u8 primitive->connectionInfo.ssid.ssid[32] */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.ssid.length */
+    bufferSize += 6;                                                     /* u8 primitive->connectionInfo.bssid.a[6] */
+    bufferSize += 1;                                                     /* CsrWifiSme80211NetworkType primitive->connectionInfo.networkType80211 */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.channelNumber */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.channelFrequency */
+    bufferSize += 2;                                                     /* CsrWifiSmeAuthMode primitive->connectionInfo.authMode */
+    bufferSize += 2;                                                     /* CsrWifiSmeEncryption primitive->connectionInfo.pairwiseCipher */
+    bufferSize += 2;                                                     /* CsrWifiSmeEncryption primitive->connectionInfo.groupCipher */
+    bufferSize += 1;                                                     /* CsrWifiSmeRadioIF primitive->connectionInfo.ifIndex */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.atimWindowTu */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.beaconPeriodTu */
+    bufferSize += 1;                                                     /* u8 primitive->connectionInfo.reassociation */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.beaconFrameLength */
+    bufferSize += primitive->connectionInfo.beaconFrameLength;           /* u8 primitive->connectionInfo.beaconFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.associationReqFrameLength */
+    bufferSize += primitive->connectionInfo.associationReqFrameLength;   /* u8 primitive->connectionInfo.associationReqFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.associationRspFrameLength */
+    bufferSize += primitive->connectionInfo.associationRspFrameLength;   /* u8 primitive->connectionInfo.associationRspFrame */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocScanInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocScanInfoElementsLength; /* u8 primitive->connectionInfo.assocScanInfoElements */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqCapabilities */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqListenIntervalTu */
+    bufferSize += 6;                                                     /* u8 primitive->connectionInfo.assocReqApAddress.a[6] */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocReqInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocReqInfoElementsLength;  /* u8 primitive->connectionInfo.assocReqInfoElements */
+    bufferSize += 2;                                                     /* CsrWifiSmeIEEE80211Result primitive->connectionInfo.assocRspResult */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspCapabilityInfo */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspAssociationId */
+    bufferSize += 2;                                                     /* u16 primitive->connectionInfo.assocRspInfoElementsLength */
+    bufferSize += primitive->connectionInfo.assocRspInfoElementsLength;  /* u8 primitive->connectionInfo.assocRspInfoElements */
+    bufferSize += 2;                                                     /* CsrWifiSmeIEEE80211Reason primitive->disassocReason */
+    bufferSize += 2;                                                     /* CsrWifiSmeIEEE80211Reason primitive->deauthReason */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMediaStatusIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMediaStatusInd *primitive = (CsrWifiSmeMediaStatusInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->mediaStatus);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.ssid.length);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.bssid.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.networkType80211);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.channelNumber);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.channelFrequency);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.authMode);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.pairwiseCipher);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.groupCipher);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.ifIndex);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.atimWindowTu);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.beaconPeriodTu);
+    CsrUint8Ser(ptr, len, (u8) primitive->connectionInfo.reassociation);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.beaconFrameLength);
+    if (primitive->connectionInfo.beaconFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.beaconFrame, ((u16) (primitive->connectionInfo.beaconFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.associationReqFrameLength);
+    if (primitive->connectionInfo.associationReqFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.associationReqFrame, ((u16) (primitive->connectionInfo.associationReqFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.associationRspFrameLength);
+    if (primitive->connectionInfo.associationRspFrameLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.associationRspFrame, ((u16) (primitive->connectionInfo.associationRspFrameLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocScanInfoElementsLength);
+    if (primitive->connectionInfo.assocScanInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocScanInfoElements, ((u16) (primitive->connectionInfo.assocScanInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqCapabilities);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqListenIntervalTu);
+    CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocReqApAddress.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocReqInfoElementsLength);
+    if (primitive->connectionInfo.assocReqInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocReqInfoElements, ((u16) (primitive->connectionInfo.assocReqInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspResult);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspCapabilityInfo);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspAssociationId);
+    CsrUint16Ser(ptr, len, (u16) primitive->connectionInfo.assocRspInfoElementsLength);
+    if (primitive->connectionInfo.assocRspInfoElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->connectionInfo.assocRspInfoElements, ((u16) (primitive->connectionInfo.assocRspInfoElementsLength)));
+    }
+    CsrUint16Ser(ptr, len, (u16) primitive->disassocReason);
+    CsrUint16Ser(ptr, len, (u16) primitive->deauthReason);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMediaStatusIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMediaStatusInd *primitive = kmalloc(sizeof(CsrWifiSmeMediaStatusInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mediaStatus, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->connectionInfo.ssid.length, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->connectionInfo.networkType80211, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.channelNumber, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.channelFrequency, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.authMode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.pairwiseCipher, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.groupCipher, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.ifIndex, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.atimWindowTu, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.beaconPeriodTu, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->connectionInfo.reassociation, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.beaconFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.beaconFrameLength)
+    {
+        primitive->connectionInfo.beaconFrame = kmalloc(primitive->connectionInfo.beaconFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.beaconFrame, buffer, &offset, ((u16) (primitive->connectionInfo.beaconFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.beaconFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.associationReqFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.associationReqFrameLength)
+    {
+        primitive->connectionInfo.associationReqFrame = kmalloc(primitive->connectionInfo.associationReqFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.associationReqFrame, buffer, &offset, ((u16) (primitive->connectionInfo.associationReqFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.associationReqFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.associationRspFrameLength, buffer, &offset);
+    if (primitive->connectionInfo.associationRspFrameLength)
+    {
+        primitive->connectionInfo.associationRspFrame = kmalloc(primitive->connectionInfo.associationRspFrameLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.associationRspFrame, buffer, &offset, ((u16) (primitive->connectionInfo.associationRspFrameLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.associationRspFrame = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocScanInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocScanInfoElementsLength)
+    {
+        primitive->connectionInfo.assocScanInfoElements = kmalloc(primitive->connectionInfo.assocScanInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocScanInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocScanInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocScanInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqCapabilities, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqListenIntervalTu, buffer, &offset);
+    CsrMemCpyDes(primitive->connectionInfo.assocReqApAddress.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocReqInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocReqInfoElementsLength)
+    {
+        primitive->connectionInfo.assocReqInfoElements = kmalloc(primitive->connectionInfo.assocReqInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocReqInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocReqInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocReqInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspResult, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspCapabilityInfo, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspAssociationId, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->connectionInfo.assocRspInfoElementsLength, buffer, &offset);
+    if (primitive->connectionInfo.assocRspInfoElementsLength)
+    {
+        primitive->connectionInfo.assocRspInfoElements = kmalloc(primitive->connectionInfo.assocRspInfoElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->connectionInfo.assocRspInfoElements, buffer, &offset, ((u16) (primitive->connectionInfo.assocRspInfoElementsLength)));
+    }
+    else
+    {
+        primitive->connectionInfo.assocRspInfoElements = NULL;
+    }
+    CsrUint16Des((u16 *) &primitive->disassocReason, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->deauthReason, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMediaStatusIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMediaStatusInd *primitive = (CsrWifiSmeMediaStatusInd *) voidPrimitivePointer;
+    kfree(primitive->connectionInfo.beaconFrame);
+    kfree(primitive->connectionInfo.associationReqFrame);
+    kfree(primitive->connectionInfo.associationRspFrame);
+    kfree(primitive->connectionInfo.assocScanInfoElements);
+    kfree(primitive->connectionInfo.assocReqInfoElements);
+    kfree(primitive->connectionInfo.assocRspInfoElements);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeMibConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->mibConfig.unifiFixMaxTxDataRate */
+    bufferSize += 1; /* u8 primitive->mibConfig.unifiFixTxDataRate */
+    bufferSize += 2; /* u16 primitive->mibConfig.dot11RtsThreshold */
+    bufferSize += 2; /* u16 primitive->mibConfig.dot11FragmentationThreshold */
+    bufferSize += 2; /* u16 primitive->mibConfig.dot11CurrentTxPowerLevel */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibConfigGetCfm *primitive = (CsrWifiSmeMibConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->mibConfig.unifiFixMaxTxDataRate);
+    CsrUint8Ser(ptr, len, (u8) primitive->mibConfig.unifiFixTxDataRate);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibConfig.dot11RtsThreshold);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibConfig.dot11FragmentationThreshold);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibConfig.dot11CurrentTxPowerLevel);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeMibConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mibConfig.unifiFixMaxTxDataRate, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->mibConfig.unifiFixTxDataRate, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibConfig.dot11RtsThreshold, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibConfig.dot11FragmentationThreshold, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibConfig.dot11CurrentTxPowerLevel, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeMibGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeMibGetCfm *primitive = (CsrWifiSmeMibGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2;                             /* CsrResult primitive->status */
+    bufferSize += 2;                             /* u16 primitive->mibAttributeLength */
+    bufferSize += primitive->mibAttributeLength; /* u8 primitive->mibAttribute */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibGetCfm *primitive = (CsrWifiSmeMibGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibAttributeLength);
+    if (primitive->mibAttributeLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mibAttribute, ((u16) (primitive->mibAttributeLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeMibGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibAttributeLength, buffer, &offset);
+    if (primitive->mibAttributeLength)
+    {
+        primitive->mibAttribute = kmalloc(primitive->mibAttributeLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mibAttribute, buffer, &offset, ((u16) (primitive->mibAttributeLength)));
+    }
+    else
+    {
+        primitive->mibAttribute = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMibGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMibGetCfm *primitive = (CsrWifiSmeMibGetCfm *) voidPrimitivePointer;
+    kfree(primitive->mibAttribute);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeMibGetNextCfmSizeof(void *msg)
+{
+    CsrWifiSmeMibGetNextCfm *primitive = (CsrWifiSmeMibGetNextCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2;                             /* CsrResult primitive->status */
+    bufferSize += 2;                             /* u16 primitive->mibAttributeLength */
+    bufferSize += primitive->mibAttributeLength; /* u8 primitive->mibAttribute */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMibGetNextCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMibGetNextCfm *primitive = (CsrWifiSmeMibGetNextCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->mibAttributeLength);
+    if (primitive->mibAttributeLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->mibAttribute, ((u16) (primitive->mibAttributeLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMibGetNextCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMibGetNextCfm *primitive = kmalloc(sizeof(CsrWifiSmeMibGetNextCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->mibAttributeLength, buffer, &offset);
+    if (primitive->mibAttributeLength)
+    {
+        primitive->mibAttribute = kmalloc(primitive->mibAttributeLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->mibAttribute, buffer, &offset, ((u16) (primitive->mibAttributeLength)));
+    }
+    else
+    {
+        primitive->mibAttribute = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMibGetNextCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMibGetNextCfm *primitive = (CsrWifiSmeMibGetNextCfm *) voidPrimitivePointer;
+    kfree(primitive->mibAttribute);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeMicFailureIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* u8 primitive->secondFailure */
+    bufferSize += 2; /* u16 primitive->count */
+    bufferSize += 6; /* u8 primitive->address.a[6] */
+    bufferSize += 1; /* CsrWifiSmeKeyType primitive->keyType */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMicFailureIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMicFailureInd *primitive = (CsrWifiSmeMicFailureInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->secondFailure);
+    CsrUint16Ser(ptr, len, (u16) primitive->count);
+    CsrMemCpySer(ptr, len, (const void *) primitive->address.a, ((u16) (6)));
+    CsrUint8Ser(ptr, len, (u8) primitive->keyType);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMicFailureIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMicFailureInd *primitive = kmalloc(sizeof(CsrWifiSmeMicFailureInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->secondFailure, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->count, buffer, &offset);
+    CsrMemCpyDes(primitive->address.a, buffer, &offset, ((u16) (6)));
+    CsrUint8Des((u8 *) &primitive->keyType, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeMulticastAddressCfmSizeof(void *msg)
+{
+    CsrWifiSmeMulticastAddressCfm *primitive = (CsrWifiSmeMulticastAddressCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->getAddressesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressesCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->getAddresses[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeMulticastAddressCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeMulticastAddressCfm *primitive = (CsrWifiSmeMulticastAddressCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->getAddressesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressesCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->getAddresses[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeMulticastAddressCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeMulticastAddressCfm *primitive = kmalloc(sizeof(CsrWifiSmeMulticastAddressCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->getAddressesCount, buffer, &offset);
+    primitive->getAddresses = NULL;
+    if (primitive->getAddressesCount)
+    {
+        primitive->getAddresses = kmalloc(sizeof(CsrWifiMacAddress) * primitive->getAddressesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getAddressesCount; i1++)
+        {
+            CsrMemCpyDes(primitive->getAddresses[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeMulticastAddressCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeMulticastAddressCfm *primitive = (CsrWifiSmeMulticastAddressCfm *) voidPrimitivePointer;
+    kfree(primitive->getAddresses);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmePacketFilterSetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePacketFilterSetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePacketFilterSetCfm *primitive = (CsrWifiSmePacketFilterSetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmePacketFilterSetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePacketFilterSetCfm *primitive = kmalloc(sizeof(CsrWifiSmePacketFilterSetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmePermanentMacAddressGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 11) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 6; /* u8 primitive->permanentMacAddress.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePermanentMacAddressGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePermanentMacAddressGetCfm *primitive = (CsrWifiSmePermanentMacAddressGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrMemCpySer(ptr, len, (const void *) primitive->permanentMacAddress.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmePermanentMacAddressGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePermanentMacAddressGetCfm *primitive = kmalloc(sizeof(CsrWifiSmePermanentMacAddressGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrMemCpyDes(primitive->permanentMacAddress.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmePmkidCandidateListIndSizeof(void *msg)
+{
+    CsrWifiSmePmkidCandidateListInd *primitive = (CsrWifiSmePmkidCandidateListInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* u8 primitive->pmkidCandidatesCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->pmkidCandidatesCount; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->pmkidCandidates[i1].bssid.a[6] */
+            bufferSize += 1; /* u8 primitive->pmkidCandidates[i1].preAuthAllowed */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePmkidCandidateListIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePmkidCandidateListInd *primitive = (CsrWifiSmePmkidCandidateListInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->pmkidCandidatesCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->pmkidCandidatesCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->pmkidCandidates[i1].bssid.a, ((u16) (6)));
+            CsrUint8Ser(ptr, len, (u8) primitive->pmkidCandidates[i1].preAuthAllowed);
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmePmkidCandidateListIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePmkidCandidateListInd *primitive = kmalloc(sizeof(CsrWifiSmePmkidCandidateListInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->pmkidCandidatesCount, buffer, &offset);
+    primitive->pmkidCandidates = NULL;
+    if (primitive->pmkidCandidatesCount)
+    {
+        primitive->pmkidCandidates = kmalloc(sizeof(CsrWifiSmePmkidCandidate) * primitive->pmkidCandidatesCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->pmkidCandidatesCount; i1++)
+        {
+            CsrMemCpyDes(primitive->pmkidCandidates[i1].bssid.a, buffer, &offset, ((u16) (6)));
+            CsrUint8Des((u8 *) &primitive->pmkidCandidates[i1].preAuthAllowed, buffer, &offset);
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmePmkidCandidateListIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmePmkidCandidateListInd *primitive = (CsrWifiSmePmkidCandidateListInd *) voidPrimitivePointer;
+    kfree(primitive->pmkidCandidates);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmePmkidCfmSizeof(void *msg)
+{
+    CsrWifiSmePmkidCfm *primitive = (CsrWifiSmePmkidCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 31) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSmeListAction primitive->action */
+    bufferSize += 1; /* u8 primitive->getPmkidsCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getPmkidsCount; i1++)
+        {
+            bufferSize += 6;  /* u8 primitive->getPmkids[i1].bssid.a[6] */
+            bufferSize += 16; /* u8 primitive->getPmkids[i1].pmkid[16] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePmkidCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePmkidCfm *primitive = (CsrWifiSmePmkidCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->action);
+    CsrUint8Ser(ptr, len, (u8) primitive->getPmkidsCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getPmkidsCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->getPmkids[i1].bssid.a, ((u16) (6)));
+            CsrMemCpySer(ptr, len, (const void *) primitive->getPmkids[i1].pmkid, ((u16) (16)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmePmkidCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePmkidCfm *primitive = kmalloc(sizeof(CsrWifiSmePmkidCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->action, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->getPmkidsCount, buffer, &offset);
+    primitive->getPmkids = NULL;
+    if (primitive->getPmkidsCount)
+    {
+        primitive->getPmkids = kmalloc(sizeof(CsrWifiSmePmkid) * primitive->getPmkidsCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->getPmkidsCount; i1++)
+        {
+            CsrMemCpyDes(primitive->getPmkids[i1].bssid.a, buffer, &offset, ((u16) (6)));
+            CsrMemCpyDes(primitive->getPmkids[i1].pmkid, buffer, &offset, ((u16) (16)));
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmePmkidCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmePmkidCfm *primitive = (CsrWifiSmePmkidCfm *) voidPrimitivePointer;
+    kfree(primitive->getPmkids);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmePowerConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSmePowerSaveLevel primitive->powerConfig.powerSaveLevel */
+    bufferSize += 2; /* u16 primitive->powerConfig.listenIntervalTu */
+    bufferSize += 1; /* u8 primitive->powerConfig.rxDtims */
+    bufferSize += 1; /* CsrWifiSmeD3AutoScanMode primitive->powerConfig.d3AutoScanMode */
+    bufferSize += 1; /* u8 primitive->powerConfig.clientTrafficWindow */
+    bufferSize += 1; /* u8 primitive->powerConfig.opportunisticPowerSave */
+    bufferSize += 1; /* u8 primitive->powerConfig.noticeOfAbsence */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmePowerConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmePowerConfigGetCfm *primitive = (CsrWifiSmePowerConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.powerSaveLevel);
+    CsrUint16Ser(ptr, len, (u16) primitive->powerConfig.listenIntervalTu);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.rxDtims);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.d3AutoScanMode);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.clientTrafficWindow);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.opportunisticPowerSave);
+    CsrUint8Ser(ptr, len, (u8) primitive->powerConfig.noticeOfAbsence);
+    return(ptr);
+}
+
+
+void* CsrWifiSmePowerConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmePowerConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmePowerConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.powerSaveLevel, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->powerConfig.listenIntervalTu, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.rxDtims, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.d3AutoScanMode, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.clientTrafficWindow, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.opportunisticPowerSave, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->powerConfig.noticeOfAbsence, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeRegulatoryDomainInfoGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->regDomInfo.dot11MultiDomainCapabilityImplemented */
+    bufferSize += 1; /* u8 primitive->regDomInfo.dot11MultiDomainCapabilityEnabled */
+    bufferSize += 1; /* CsrWifiSmeRegulatoryDomain primitive->regDomInfo.currentRegulatoryDomain */
+    bufferSize += 2; /* u8 primitive->regDomInfo.currentCountryCode[2] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeRegulatoryDomainInfoGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeRegulatoryDomainInfoGetCfm *primitive = (CsrWifiSmeRegulatoryDomainInfoGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->regDomInfo.dot11MultiDomainCapabilityImplemented);
+    CsrUint8Ser(ptr, len, (u8) primitive->regDomInfo.dot11MultiDomainCapabilityEnabled);
+    CsrUint8Ser(ptr, len, (u8) primitive->regDomInfo.currentRegulatoryDomain);
+    CsrMemCpySer(ptr, len, (const void *) primitive->regDomInfo.currentCountryCode, ((u16) (2)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmeRegulatoryDomainInfoGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeRegulatoryDomainInfoGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeRegulatoryDomainInfoGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->regDomInfo.dot11MultiDomainCapabilityImplemented, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->regDomInfo.dot11MultiDomainCapabilityEnabled, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->regDomInfo.currentRegulatoryDomain, buffer, &offset);
+    CsrMemCpyDes(primitive->regDomInfo.currentCountryCode, buffer, &offset, ((u16) (2)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeRoamCompleteIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeRoamCompleteIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeRoamCompleteInd *primitive = (CsrWifiSmeRoamCompleteInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeRoamCompleteIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeRoamCompleteInd *primitive = kmalloc(sizeof(CsrWifiSmeRoamCompleteInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeRoamStartIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 1; /* CsrWifiSmeRoamReason primitive->roamReason */
+    bufferSize += 2; /* CsrWifiSmeIEEE80211Reason primitive->reason80211 */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeRoamStartIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeRoamStartInd *primitive = (CsrWifiSmeRoamStartInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint8Ser(ptr, len, (u8) primitive->roamReason);
+    CsrUint16Ser(ptr, len, (u16) primitive->reason80211);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeRoamStartIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeRoamStartInd *primitive = kmalloc(sizeof(CsrWifiSmeRoamStartInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->roamReason, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->reason80211, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeRoamingConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 72) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].rssiHighThreshold */
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].rssiLowThreshold */
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].snrHighThreshold */
+            bufferSize += 2; /* s16 primitive->roamingConfig.roamingBands[i2].snrLowThreshold */
+        }
+    }
+    bufferSize += 1;         /* u8 primitive->roamingConfig.disableSmoothRoaming */
+    bufferSize += 1;         /* u8 primitive->roamingConfig.disableRoamScans */
+    bufferSize += 1;         /* u8 primitive->roamingConfig.reconnectLimit */
+    bufferSize += 2;         /* u16 primitive->roamingConfig.reconnectLimitIntervalMs */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].intervalSeconds */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].validitySeconds */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].minActiveChannelTimeTu */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].maxActiveChannelTimeTu */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].minPassiveChannelTimeTu */
+            bufferSize += 2; /* u16 primitive->roamingConfig.roamScanCfg[i2].maxPassiveChannelTimeTu */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeRoamingConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeRoamingConfigGetCfm *primitive = (CsrWifiSmeRoamingConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].rssiHighThreshold);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].rssiLowThreshold);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].snrHighThreshold);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamingBands[i2].snrLowThreshold);
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->roamingConfig.disableSmoothRoaming);
+    CsrUint8Ser(ptr, len, (u8) primitive->roamingConfig.disableRoamScans);
+    CsrUint8Ser(ptr, len, (u8) primitive->roamingConfig.reconnectLimit);
+    CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.reconnectLimitIntervalMs);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].intervalSeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].validitySeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].minActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].maxActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].minPassiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->roamingConfig.roamScanCfg[i2].maxPassiveChannelTimeTu);
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeRoamingConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeRoamingConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeRoamingConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].rssiHighThreshold, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].rssiLowThreshold, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].snrHighThreshold, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamingBands[i2].snrLowThreshold, buffer, &offset);
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->roamingConfig.disableSmoothRoaming, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->roamingConfig.disableRoamScans, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->roamingConfig.reconnectLimit, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->roamingConfig.reconnectLimitIntervalMs, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 3; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].intervalSeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].validitySeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].minActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].maxActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].minPassiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->roamingConfig.roamScanCfg[i2].maxPassiveChannelTimeTu, buffer, &offset);
+        }
+    }
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeRoamingConfigSetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeRoamingConfigSetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeRoamingConfigSetCfm *primitive = (CsrWifiSmeRoamingConfigSetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeRoamingConfigSetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeRoamingConfigSetCfm *primitive = kmalloc(sizeof(CsrWifiSmeRoamingConfigSetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeScanConfigGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeScanConfigGetCfm *primitive = (CsrWifiSmeScanConfigGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 65) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].intervalSeconds */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].validitySeconds */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].minActiveChannelTimeTu */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].maxActiveChannelTimeTu */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].minPassiveChannelTimeTu */
+            bufferSize += 2;                                     /* u16 primitive->scanConfig.scanCfg[i2].maxPassiveChannelTimeTu */
+        }
+    }
+    bufferSize += 1;                                             /* u8 primitive->scanConfig.disableAutonomousScans */
+    bufferSize += 2;                                             /* u16 primitive->scanConfig.maxResults */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.highRssiThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.lowRssiThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.deltaRssiThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.highSnrThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.lowSnrThreshold */
+    bufferSize += 1;                                             /* s8 primitive->scanConfig.deltaSnrThreshold */
+    bufferSize += 2;                                             /* u16 primitive->scanConfig.passiveChannelListCount */
+    bufferSize += primitive->scanConfig.passiveChannelListCount; /* u8 primitive->scanConfig.passiveChannelList */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeScanConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeScanConfigGetCfm *primitive = (CsrWifiSmeScanConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].intervalSeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].validitySeconds);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].minActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].maxActiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].minPassiveChannelTimeTu);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.scanCfg[i2].maxPassiveChannelTimeTu);
+        }
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.disableAutonomousScans);
+    CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.maxResults);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.highRssiThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.lowRssiThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.deltaRssiThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.highSnrThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.lowSnrThreshold);
+    CsrUint8Ser(ptr, len, (u8) primitive->scanConfig.deltaSnrThreshold);
+    CsrUint16Ser(ptr, len, (u16) primitive->scanConfig.passiveChannelListCount);
+    if (primitive->scanConfig.passiveChannelListCount)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->scanConfig.passiveChannelList, ((u16) (primitive->scanConfig.passiveChannelListCount)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeScanConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeScanConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeScanConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < 4; i2++)
+        {
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].intervalSeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].validitySeconds, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].minActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].maxActiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].minPassiveChannelTimeTu, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanConfig.scanCfg[i2].maxPassiveChannelTimeTu, buffer, &offset);
+        }
+    }
+    CsrUint8Des((u8 *) &primitive->scanConfig.disableAutonomousScans, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->scanConfig.maxResults, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.highRssiThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.lowRssiThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.deltaRssiThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.highSnrThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.lowSnrThreshold, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->scanConfig.deltaSnrThreshold, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->scanConfig.passiveChannelListCount, buffer, &offset);
+    if (primitive->scanConfig.passiveChannelListCount)
+    {
+        primitive->scanConfig.passiveChannelList = kmalloc(primitive->scanConfig.passiveChannelListCount, GFP_KERNEL);
+        CsrMemCpyDes(primitive->scanConfig.passiveChannelList, buffer, &offset, ((u16) (primitive->scanConfig.passiveChannelListCount)));
+    }
+    else
+    {
+        primitive->scanConfig.passiveChannelList = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeScanConfigGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeScanConfigGetCfm *primitive = (CsrWifiSmeScanConfigGetCfm *) voidPrimitivePointer;
+    kfree(primitive->scanConfig.passiveChannelList);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeScanResultIndSizeof(void *msg)
+{
+    CsrWifiSmeScanResultInd *primitive = (CsrWifiSmeScanResultInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 149) */
+    bufferSize += 32;                                          /* u8 primitive->result.ssid.ssid[32] */
+    bufferSize += 1;                                           /* u8 primitive->result.ssid.length */
+    bufferSize += 6;                                           /* u8 primitive->result.bssid.a[6] */
+    bufferSize += 2;                                           /* s16 primitive->result.rssi */
+    bufferSize += 2;                                           /* s16 primitive->result.snr */
+    bufferSize += 1;                                           /* CsrWifiSmeRadioIF primitive->result.ifIndex */
+    bufferSize += 2;                                           /* u16 primitive->result.beaconPeriodTu */
+    bufferSize += 8;                                           /* u8 primitive->result.timeStamp.data[8] */
+    bufferSize += 8;                                           /* u8 primitive->result.localTime.data[8] */
+    bufferSize += 2;                                           /* u16 primitive->result.channelFrequency */
+    bufferSize += 2;                                           /* u16 primitive->result.capabilityInformation */
+    bufferSize += 1;                                           /* u8 primitive->result.channelNumber */
+    bufferSize += 1;                                           /* CsrWifiSmeBasicUsability primitive->result.usability */
+    bufferSize += 1;                                           /* CsrWifiSmeBssType primitive->result.bssType */
+    bufferSize += 2;                                           /* u16 primitive->result.informationElementsLength */
+    bufferSize += primitive->result.informationElementsLength; /* u8 primitive->result.informationElements */
+    bufferSize += 1;                                           /* CsrWifiSmeP2pRole primitive->result.p2pDeviceRole */
+    switch (primitive->result.p2pDeviceRole)
+    {
+        case CSR_WIFI_SME_P2P_ROLE_CLI:
+            bufferSize += 1; /* u8 primitive->result.deviceInfo.reservedCli.empty */
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_GO:
+            bufferSize += 1; /* CsrWifiSmeP2pGroupCapabilityMask primitive->result.deviceInfo.groupInfo.groupCapability */
+            bufferSize += 6; /* u8 primitive->result.deviceInfo.groupInfo.p2pDeviceAddress.a[6] */
+            bufferSize += 1; /* u8 primitive->result.deviceInfo.groupInfo.p2pClientInfoCount */
+            {
+                u16 i4;
+                for (i4 = 0; i4 < primitive->result.deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                {
+                    bufferSize += 6; /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].p2PClientInterfaceAddress.a[6] */
+                    bufferSize += 6; /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceAddress.a[6] */
+                    bufferSize += 2; /* CsrWifiSmeWpsConfigTypeMask primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.configMethods */
+                    bufferSize += 1; /* CsrWifiSmeP2pCapabilityMask primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.p2PDeviceCap */
+                    bufferSize += 8; /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.primDeviceType.deviceDetails[8] */
+                    bufferSize += 1; /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount */
+                    {
+                        u16 i6;
+                        for (i6 = 0; i6 < primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount; i6++)
+                        {
+                            bufferSize += 8; /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType[i6].deviceDetails[8] */
+                        }
+                    }
+                    bufferSize += 32;        /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceName[32] */
+                    bufferSize += 1;         /* u8 primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceNameLength */
+                }
+            }
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_NONE:
+            bufferSize += 1; /* u8 primitive->result.deviceInfo.reservedNone.empty */
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+            bufferSize += 6; /* u8 primitive->result.deviceInfo.standalonedevInfo.deviceAddress.a[6] */
+            bufferSize += 2; /* CsrWifiSmeWpsConfigTypeMask primitive->result.deviceInfo.standalonedevInfo.configMethods */
+            bufferSize += 1; /* CsrWifiSmeP2pCapabilityMask primitive->result.deviceInfo.standalonedevInfo.p2PDeviceCap */
+            bufferSize += 8; /* u8 primitive->result.deviceInfo.standalonedevInfo.primDeviceType.deviceDetails[8] */
+            bufferSize += 1; /* u8 primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount */
+            {
+                u16 i4;
+                for (i4 = 0; i4 < primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount; i4++)
+                {
+                    bufferSize += 8; /* u8 primitive->result.deviceInfo.standalonedevInfo.secDeviceType[i4].deviceDetails[8] */
+                }
+            }
+            bufferSize += 32;        /* u8 primitive->result.deviceInfo.standalonedevInfo.deviceName[32] */
+            bufferSize += 1;         /* u8 primitive->result.deviceInfo.standalonedevInfo.deviceNameLength */
+            break;
+        default:
+            break;
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeScanResultIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeScanResultInd *primitive = (CsrWifiSmeScanResultInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrMemCpySer(ptr, len, (const void *) primitive->result.ssid.ssid, ((u16) (32)));
+    CsrUint8Ser(ptr, len, (u8) primitive->result.ssid.length);
+    CsrMemCpySer(ptr, len, (const void *) primitive->result.bssid.a, ((u16) (6)));
+    CsrUint16Ser(ptr, len, (u16) primitive->result.rssi);
+    CsrUint16Ser(ptr, len, (u16) primitive->result.snr);
+    CsrUint8Ser(ptr, len, (u8) primitive->result.ifIndex);
+    CsrUint16Ser(ptr, len, (u16) primitive->result.beaconPeriodTu);
+    CsrMemCpySer(ptr, len, (const void *) primitive->result.timeStamp.data, ((u16) (8)));
+    CsrMemCpySer(ptr, len, (const void *) primitive->result.localTime.data, ((u16) (8)));
+    CsrUint16Ser(ptr, len, (u16) primitive->result.channelFrequency);
+    CsrUint16Ser(ptr, len, (u16) primitive->result.capabilityInformation);
+    CsrUint8Ser(ptr, len, (u8) primitive->result.channelNumber);
+    CsrUint8Ser(ptr, len, (u8) primitive->result.usability);
+    CsrUint8Ser(ptr, len, (u8) primitive->result.bssType);
+    CsrUint16Ser(ptr, len, (u16) primitive->result.informationElementsLength);
+    if (primitive->result.informationElementsLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->result.informationElements, ((u16) (primitive->result.informationElementsLength)));
+    }
+    CsrUint8Ser(ptr, len, (u8) primitive->result.p2pDeviceRole);
+    switch (primitive->result.p2pDeviceRole)
+    {
+        case CSR_WIFI_SME_P2P_ROLE_CLI:
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.reservedCli.empty);
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_GO:
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.groupInfo.groupCapability);
+            CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.groupInfo.p2pDeviceAddress.a, ((u16) (6)));
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.groupInfo.p2pClientInfoCount);
+            {
+                u16 i4;
+                for (i4 = 0; i4 < primitive->result.deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                {
+                    CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].p2PClientInterfaceAddress.a, ((u16) (6)));
+                    CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceAddress.a, ((u16) (6)));
+                    CsrUint16Ser(ptr, len, (u16) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.configMethods);
+                    CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.p2PDeviceCap);
+                    CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.primDeviceType.deviceDetails, ((u16) (8)));
+                    CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount);
+                    {
+                        u16 i6;
+                        for (i6 = 0; i6 < primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount; i6++)
+                        {
+                            CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType[i6].deviceDetails, ((u16) (8)));
+                        }
+                    }
+                    CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceName, ((u16) (32)));
+                    CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceNameLength);
+                }
+            }
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_NONE:
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.reservedNone.empty);
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+            CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.standalonedevInfo.deviceAddress.a, ((u16) (6)));
+            CsrUint16Ser(ptr, len, (u16) primitive->result.deviceInfo.standalonedevInfo.configMethods);
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.standalonedevInfo.p2PDeviceCap);
+            CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.standalonedevInfo.primDeviceType.deviceDetails, ((u16) (8)));
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount);
+            {
+                u16 i4;
+                for (i4 = 0; i4 < primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount; i4++)
+                {
+                    CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.standalonedevInfo.secDeviceType[i4].deviceDetails, ((u16) (8)));
+                }
+            }
+            CsrMemCpySer(ptr, len, (const void *) primitive->result.deviceInfo.standalonedevInfo.deviceName, ((u16) (32)));
+            CsrUint8Ser(ptr, len, (u8) primitive->result.deviceInfo.standalonedevInfo.deviceNameLength);
+            break;
+        default:
+            break;
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeScanResultIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeScanResultInd *primitive = kmalloc(sizeof(CsrWifiSmeScanResultInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrMemCpyDes(primitive->result.ssid.ssid, buffer, &offset, ((u16) (32)));
+    CsrUint8Des((u8 *) &primitive->result.ssid.length, buffer, &offset);
+    CsrMemCpyDes(primitive->result.bssid.a, buffer, &offset, ((u16) (6)));
+    CsrUint16Des((u16 *) &primitive->result.rssi, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result.snr, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->result.ifIndex, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result.beaconPeriodTu, buffer, &offset);
+    CsrMemCpyDes(primitive->result.timeStamp.data, buffer, &offset, ((u16) (8)));
+    CsrMemCpyDes(primitive->result.localTime.data, buffer, &offset, ((u16) (8)));
+    CsrUint16Des((u16 *) &primitive->result.channelFrequency, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result.capabilityInformation, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->result.channelNumber, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->result.usability, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->result.bssType, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->result.informationElementsLength, buffer, &offset);
+    if (primitive->result.informationElementsLength)
+    {
+        primitive->result.informationElements = kmalloc(primitive->result.informationElementsLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->result.informationElements, buffer, &offset, ((u16) (primitive->result.informationElementsLength)));
+    }
+    else
+    {
+        primitive->result.informationElements = NULL;
+    }
+    CsrUint8Des((u8 *) &primitive->result.p2pDeviceRole, buffer, &offset);
+    switch (primitive->result.p2pDeviceRole)
+    {
+        case CSR_WIFI_SME_P2P_ROLE_CLI:
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.reservedCli.empty, buffer, &offset);
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_GO:
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.groupInfo.groupCapability, buffer, &offset);
+            CsrMemCpyDes(primitive->result.deviceInfo.groupInfo.p2pDeviceAddress.a, buffer, &offset, ((u16) (6)));
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.groupInfo.p2pClientInfoCount, buffer, &offset);
+            primitive->result.deviceInfo.groupInfo.p2PClientInfo = NULL;
+            if (primitive->result.deviceInfo.groupInfo.p2pClientInfoCount)
+            {
+                primitive->result.deviceInfo.groupInfo.p2PClientInfo = kmalloc(sizeof(CsrWifiSmeP2pClientInfoType) * primitive->result.deviceInfo.groupInfo.p2pClientInfoCount, GFP_KERNEL);
+            }
+            {
+                u16 i4;
+                for (i4 = 0; i4 < primitive->result.deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                {
+                    CsrMemCpyDes(primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].p2PClientInterfaceAddress.a, buffer, &offset, ((u16) (6)));
+                    CsrMemCpyDes(primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceAddress.a, buffer, &offset, ((u16) (6)));
+                    CsrUint16Des((u16 *) &primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.configMethods, buffer, &offset);
+                    CsrUint8Des((u8 *) &primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.p2PDeviceCap, buffer, &offset);
+                    CsrMemCpyDes(primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.primDeviceType.deviceDetails, buffer, &offset, ((u16) (8)));
+                    CsrUint8Des((u8 *) &primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount, buffer, &offset);
+                    primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType = NULL;
+                    if (primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount)
+                    {
+                        primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType = kmalloc(sizeof(CsrWifiSmeWpsDeviceType) * primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount, GFP_KERNEL);
+                    }
+                    {
+                        u16 i6;
+                        for (i6 = 0; i6 < primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount; i6++)
+                        {
+                            CsrMemCpyDes(primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType[i6].deviceDetails, buffer, &offset, ((u16) (8)));
+                        }
+                    }
+                    CsrMemCpyDes(primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceName, buffer, &offset, ((u16) (32)));
+                    CsrUint8Des((u8 *) &primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceNameLength, buffer, &offset);
+                }
+            }
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_NONE:
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.reservedNone.empty, buffer, &offset);
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+            CsrMemCpyDes(primitive->result.deviceInfo.standalonedevInfo.deviceAddress.a, buffer, &offset, ((u16) (6)));
+            CsrUint16Des((u16 *) &primitive->result.deviceInfo.standalonedevInfo.configMethods, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.standalonedevInfo.p2PDeviceCap, buffer, &offset);
+            CsrMemCpyDes(primitive->result.deviceInfo.standalonedevInfo.primDeviceType.deviceDetails, buffer, &offset, ((u16) (8)));
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount, buffer, &offset);
+            primitive->result.deviceInfo.standalonedevInfo.secDeviceType = NULL;
+            if (primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount)
+            {
+                primitive->result.deviceInfo.standalonedevInfo.secDeviceType = kmalloc(sizeof(CsrWifiSmeWpsDeviceType) * primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount, GFP_KERNEL);
+            }
+            {
+                u16 i4;
+                for (i4 = 0; i4 < primitive->result.deviceInfo.standalonedevInfo.secondaryDeviceTypeCount; i4++)
+                {
+                    CsrMemCpyDes(primitive->result.deviceInfo.standalonedevInfo.secDeviceType[i4].deviceDetails, buffer, &offset, ((u16) (8)));
+                }
+            }
+            CsrMemCpyDes(primitive->result.deviceInfo.standalonedevInfo.deviceName, buffer, &offset, ((u16) (32)));
+            CsrUint8Des((u8 *) &primitive->result.deviceInfo.standalonedevInfo.deviceNameLength, buffer, &offset);
+            break;
+        default:
+            break;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeScanResultIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeScanResultInd *primitive = (CsrWifiSmeScanResultInd *) voidPrimitivePointer;
+    kfree(primitive->result.informationElements);
+    switch (primitive->result.p2pDeviceRole)
+    {
+        case CSR_WIFI_SME_P2P_ROLE_GO:
+        {
+            u16 i4;
+            for (i4 = 0; i4 < primitive->result.deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+            {
+                kfree(primitive->result.deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType);
+            }
+        }
+            kfree(primitive->result.deviceInfo.groupInfo.p2PClientInfo);
+            break;
+        case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+            kfree(primitive->result.deviceInfo.standalonedevInfo.secDeviceType);
+            break;
+        default:
+            break;
+    }
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeScanResultsGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeScanResultsGetCfm *primitive = (CsrWifiSmeScanResultsGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 153) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 2; /* u16 primitive->scanResultsCount */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->scanResultsCount; i1++)
+        {
+            bufferSize += 32;                                                   /* u8 primitive->scanResults[i1].ssid.ssid[32] */
+            bufferSize += 1;                                                    /* u8 primitive->scanResults[i1].ssid.length */
+            bufferSize += 6;                                                    /* u8 primitive->scanResults[i1].bssid.a[6] */
+            bufferSize += 2;                                                    /* s16 primitive->scanResults[i1].rssi */
+            bufferSize += 2;                                                    /* s16 primitive->scanResults[i1].snr */
+            bufferSize += 1;                                                    /* CsrWifiSmeRadioIF primitive->scanResults[i1].ifIndex */
+            bufferSize += 2;                                                    /* u16 primitive->scanResults[i1].beaconPeriodTu */
+            bufferSize += 8;                                                    /* u8 primitive->scanResults[i1].timeStamp.data[8] */
+            bufferSize += 8;                                                    /* u8 primitive->scanResults[i1].localTime.data[8] */
+            bufferSize += 2;                                                    /* u16 primitive->scanResults[i1].channelFrequency */
+            bufferSize += 2;                                                    /* u16 primitive->scanResults[i1].capabilityInformation */
+            bufferSize += 1;                                                    /* u8 primitive->scanResults[i1].channelNumber */
+            bufferSize += 1;                                                    /* CsrWifiSmeBasicUsability primitive->scanResults[i1].usability */
+            bufferSize += 1;                                                    /* CsrWifiSmeBssType primitive->scanResults[i1].bssType */
+            bufferSize += 2;                                                    /* u16 primitive->scanResults[i1].informationElementsLength */
+            bufferSize += primitive->scanResults[i1].informationElementsLength; /* u8 primitive->scanResults[i1].informationElements */
+            bufferSize += 1;                                                    /* CsrWifiSmeP2pRole primitive->scanResults[i1].p2pDeviceRole */
+            switch (primitive->scanResults[i1].p2pDeviceRole)
+            {
+                case CSR_WIFI_SME_P2P_ROLE_CLI:
+                    bufferSize += 1; /* u8 primitive->scanResults[i1].deviceInfo.reservedCli.empty */
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_GO:
+                    bufferSize += 1; /* CsrWifiSmeP2pGroupCapabilityMask primitive->scanResults[i1].deviceInfo.groupInfo.groupCapability */
+                    bufferSize += 6; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2pDeviceAddress.a[6] */
+                    bufferSize += 1; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount */
+                    {
+                        u16 i4;
+                        for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                        {
+                            bufferSize += 6; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].p2PClientInterfaceAddress.a[6] */
+                            bufferSize += 6; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceAddress.a[6] */
+                            bufferSize += 2; /* CsrWifiSmeWpsConfigTypeMask primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.configMethods */
+                            bufferSize += 1; /* CsrWifiSmeP2pCapabilityMask primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.p2PDeviceCap */
+                            bufferSize += 8; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.primDeviceType.deviceDetails[8] */
+                            bufferSize += 1; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount */
+                            {
+                                u16 i6;
+                                for (i6 = 0; i6 < primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount; i6++)
+                                {
+                                    bufferSize += 8; /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType[i6].deviceDetails[8] */
+                                }
+                            }
+                            bufferSize += 32;        /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceName[32] */
+                            bufferSize += 1;         /* u8 primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceNameLength */
+                        }
+                    }
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_NONE:
+                    bufferSize += 1; /* u8 primitive->scanResults[i1].deviceInfo.reservedNone.empty */
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+                    bufferSize += 6; /* u8 primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceAddress.a[6] */
+                    bufferSize += 2; /* CsrWifiSmeWpsConfigTypeMask primitive->scanResults[i1].deviceInfo.standalonedevInfo.configMethods */
+                    bufferSize += 1; /* CsrWifiSmeP2pCapabilityMask primitive->scanResults[i1].deviceInfo.standalonedevInfo.p2PDeviceCap */
+                    bufferSize += 8; /* u8 primitive->scanResults[i1].deviceInfo.standalonedevInfo.primDeviceType.deviceDetails[8] */
+                    bufferSize += 1; /* u8 primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount */
+                    {
+                        u16 i4;
+                        for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount; i4++)
+                        {
+                            bufferSize += 8; /* u8 primitive->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType[i4].deviceDetails[8] */
+                        }
+                    }
+                    bufferSize += 32;        /* u8 primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceName[32] */
+                    bufferSize += 1;         /* u8 primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceNameLength */
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeScanResultsGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeScanResultsGetCfm *primitive = (CsrWifiSmeScanResultsGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->scanResultsCount);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->scanResultsCount; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].ssid.ssid, ((u16) (32)));
+            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].ssid.length);
+            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].bssid.a, ((u16) (6)));
+            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].rssi);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].snr);
+            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].ifIndex);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].beaconPeriodTu);
+            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].timeStamp.data, ((u16) (8)));
+            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].localTime.data, ((u16) (8)));
+            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].channelFrequency);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].capabilityInformation);
+            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].channelNumber);
+            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].usability);
+            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].bssType);
+            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].informationElementsLength);
+            if (primitive->scanResults[i1].informationElementsLength)
+            {
+                CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].informationElements, ((u16) (primitive->scanResults[i1].informationElementsLength)));
+            }
+            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].p2pDeviceRole);
+            switch (primitive->scanResults[i1].p2pDeviceRole)
+            {
+                case CSR_WIFI_SME_P2P_ROLE_CLI:
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.reservedCli.empty);
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_GO:
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.groupInfo.groupCapability);
+                    CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.groupInfo.p2pDeviceAddress.a, ((u16) (6)));
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount);
+                    {
+                        u16 i4;
+                        for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                        {
+                            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].p2PClientInterfaceAddress.a, ((u16) (6)));
+                            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceAddress.a, ((u16) (6)));
+                            CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.configMethods);
+                            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.p2PDeviceCap);
+                            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.primDeviceType.deviceDetails, ((u16) (8)));
+                            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount);
+                            {
+                                u16 i6;
+                                for (i6 = 0; i6 < primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount; i6++)
+                                {
+                                    CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType[i6].deviceDetails, ((u16) (8)));
+                                }
+                            }
+                            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceName, ((u16) (32)));
+                            CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceNameLength);
+                        }
+                    }
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_NONE:
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.reservedNone.empty);
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+                    CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceAddress.a, ((u16) (6)));
+                    CsrUint16Ser(ptr, len, (u16) primitive->scanResults[i1].deviceInfo.standalonedevInfo.configMethods);
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.standalonedevInfo.p2PDeviceCap);
+                    CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.standalonedevInfo.primDeviceType.deviceDetails, ((u16) (8)));
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount);
+                    {
+                        u16 i4;
+                        for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount; i4++)
+                        {
+                            CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType[i4].deviceDetails, ((u16) (8)));
+                        }
+                    }
+                    CsrMemCpySer(ptr, len, (const void *) primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceName, ((u16) (32)));
+                    CsrUint8Ser(ptr, len, (u8) primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceNameLength);
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeScanResultsGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeScanResultsGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeScanResultsGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->scanResultsCount, buffer, &offset);
+    primitive->scanResults = NULL;
+    if (primitive->scanResultsCount)
+    {
+        primitive->scanResults = kmalloc(sizeof(CsrWifiSmeScanResult) * primitive->scanResultsCount, GFP_KERNEL);
+    }
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->scanResultsCount; i1++)
+        {
+            CsrMemCpyDes(primitive->scanResults[i1].ssid.ssid, buffer, &offset, ((u16) (32)));
+            CsrUint8Des((u8 *) &primitive->scanResults[i1].ssid.length, buffer, &offset);
+            CsrMemCpyDes(primitive->scanResults[i1].bssid.a, buffer, &offset, ((u16) (6)));
+            CsrUint16Des((u16 *) &primitive->scanResults[i1].rssi, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanResults[i1].snr, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->scanResults[i1].ifIndex, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanResults[i1].beaconPeriodTu, buffer, &offset);
+            CsrMemCpyDes(primitive->scanResults[i1].timeStamp.data, buffer, &offset, ((u16) (8)));
+            CsrMemCpyDes(primitive->scanResults[i1].localTime.data, buffer, &offset, ((u16) (8)));
+            CsrUint16Des((u16 *) &primitive->scanResults[i1].channelFrequency, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanResults[i1].capabilityInformation, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->scanResults[i1].channelNumber, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->scanResults[i1].usability, buffer, &offset);
+            CsrUint8Des((u8 *) &primitive->scanResults[i1].bssType, buffer, &offset);
+            CsrUint16Des((u16 *) &primitive->scanResults[i1].informationElementsLength, buffer, &offset);
+            if (primitive->scanResults[i1].informationElementsLength)
+            {
+                primitive->scanResults[i1].informationElements = kmalloc(primitive->scanResults[i1].informationElementsLength, GFP_KERNEL);
+                CsrMemCpyDes(primitive->scanResults[i1].informationElements, buffer, &offset, ((u16) (primitive->scanResults[i1].informationElementsLength)));
+            }
+            else
+            {
+                primitive->scanResults[i1].informationElements = NULL;
+            }
+            CsrUint8Des((u8 *) &primitive->scanResults[i1].p2pDeviceRole, buffer, &offset);
+            switch (primitive->scanResults[i1].p2pDeviceRole)
+            {
+                case CSR_WIFI_SME_P2P_ROLE_CLI:
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.reservedCli.empty, buffer, &offset);
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_GO:
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.groupInfo.groupCapability, buffer, &offset);
+                    CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.groupInfo.p2pDeviceAddress.a, buffer, &offset, ((u16) (6)));
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount, buffer, &offset);
+                    primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo = NULL;
+                    if (primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount)
+                    {
+                        primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo = kmalloc(sizeof(CsrWifiSmeP2pClientInfoType) * primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount, GFP_KERNEL);
+                    }
+                    {
+                        u16 i4;
+                        for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                        {
+                            CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].p2PClientInterfaceAddress.a, buffer, &offset, ((u16) (6)));
+                            CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceAddress.a, buffer, &offset, ((u16) (6)));
+                            CsrUint16Des((u16 *) &primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.configMethods, buffer, &offset);
+                            CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.p2PDeviceCap, buffer, &offset);
+                            CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.primDeviceType.deviceDetails, buffer, &offset, ((u16) (8)));
+                            CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount, buffer, &offset);
+                            primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType = NULL;
+                            if (primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount)
+                            {
+                                primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType = kmalloc(sizeof(CsrWifiSmeWpsDeviceType) * primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount, GFP_KERNEL);
+                            }
+                            {
+                                u16 i6;
+                                for (i6 = 0; i6 < primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secondaryDeviceTypeCount; i6++)
+                                {
+                                    CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType[i6].deviceDetails, buffer, &offset, ((u16) (8)));
+                                }
+                            }
+                            CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceName, buffer, &offset, ((u16) (32)));
+                            CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.deviceNameLength, buffer, &offset);
+                        }
+                    }
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_NONE:
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.reservedNone.empty, buffer, &offset);
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+                    CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceAddress.a, buffer, &offset, ((u16) (6)));
+                    CsrUint16Des((u16 *) &primitive->scanResults[i1].deviceInfo.standalonedevInfo.configMethods, buffer, &offset);
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.standalonedevInfo.p2PDeviceCap, buffer, &offset);
+                    CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.standalonedevInfo.primDeviceType.deviceDetails, buffer, &offset, ((u16) (8)));
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount, buffer, &offset);
+                    primitive->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType = NULL;
+                    if (primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount)
+                    {
+                        primitive->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType = kmalloc(sizeof(CsrWifiSmeWpsDeviceType) * primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount, GFP_KERNEL);
+                    }
+                    {
+                        u16 i4;
+                        for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.standalonedevInfo.secondaryDeviceTypeCount; i4++)
+                        {
+                            CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType[i4].deviceDetails, buffer, &offset, ((u16) (8)));
+                        }
+                    }
+                    CsrMemCpyDes(primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceName, buffer, &offset, ((u16) (32)));
+                    CsrUint8Des((u8 *) &primitive->scanResults[i1].deviceInfo.standalonedevInfo.deviceNameLength, buffer, &offset);
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeScanResultsGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeScanResultsGetCfm *primitive = (CsrWifiSmeScanResultsGetCfm *) voidPrimitivePointer;
+    {
+        u16 i1;
+        for (i1 = 0; i1 < primitive->scanResultsCount; i1++)
+        {
+            kfree(primitive->scanResults[i1].informationElements);
+            switch (primitive->scanResults[i1].p2pDeviceRole)
+            {
+                case CSR_WIFI_SME_P2P_ROLE_GO:
+                {
+                    u16 i4;
+                    for (i4 = 0; i4 < primitive->scanResults[i1].deviceInfo.groupInfo.p2pClientInfoCount; i4++)
+                    {
+                        kfree(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo[i4].clientDeviceInfo.secDeviceType);
+                    }
+                }
+                    kfree(primitive->scanResults[i1].deviceInfo.groupInfo.p2PClientInfo);
+                    break;
+                case CSR_WIFI_SME_P2P_ROLE_STANDALONE:
+                    kfree(primitive->scanResults[i1].deviceInfo.standalonedevInfo.secDeviceType);
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+    kfree(primitive->scanResults);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeSmeStaConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->smeConfig.connectionQualityRssiChangeTrigger */
+    bufferSize += 1; /* u8 primitive->smeConfig.connectionQualitySnrChangeTrigger */
+    bufferSize += 1; /* CsrWifiSmeWmmModeMask primitive->smeConfig.wmmModeMask */
+    bufferSize += 1; /* CsrWifiSmeRadioIF primitive->smeConfig.ifIndex */
+    bufferSize += 1; /* u8 primitive->smeConfig.allowUnicastUseGroupCipher */
+    bufferSize += 1; /* u8 primitive->smeConfig.enableOpportunisticKeyCaching */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeSmeStaConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeSmeStaConfigGetCfm *primitive = (CsrWifiSmeSmeStaConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.connectionQualityRssiChangeTrigger);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.connectionQualitySnrChangeTrigger);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.wmmModeMask);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.ifIndex);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.allowUnicastUseGroupCipher);
+    CsrUint8Ser(ptr, len, (u8) primitive->smeConfig.enableOpportunisticKeyCaching);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeSmeStaConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeSmeStaConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeSmeStaConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.connectionQualityRssiChangeTrigger, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.connectionQualitySnrChangeTrigger, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.wmmModeMask, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.ifIndex, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.allowUnicastUseGroupCipher, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->smeConfig.enableOpportunisticKeyCaching, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeSmeStaConfigSetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 7) */
+    bufferSize += 2; /* u16 primitive->interfaceTag */
+    bufferSize += 2; /* CsrResult primitive->status */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeSmeStaConfigSetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeSmeStaConfigSetCfm *primitive = (CsrWifiSmeSmeStaConfigSetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeSmeStaConfigSetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeSmeStaConfigSetCfm *primitive = kmalloc(sizeof(CsrWifiSmeSmeStaConfigSetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeStationMacAddressGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 17) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 2; i1++)
+        {
+            bufferSize += 6; /* u8 primitive->stationMacAddress[i1].a[6] */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeStationMacAddressGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeStationMacAddressGetCfm *primitive = (CsrWifiSmeStationMacAddressGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 2; i1++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->stationMacAddress[i1].a, ((u16) (6)));
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeStationMacAddressGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeStationMacAddressGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeStationMacAddressGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    {
+        u16 i1;
+        for (i1 = 0; i1 < 2; i1++)
+        {
+            CsrMemCpyDes(primitive->stationMacAddress[i1].a, buffer, &offset, ((u16) (6)));
+        }
+    }
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeTspecIndSizeof(void *msg)
+{
+    CsrWifiSmeTspecInd *primitive = (CsrWifiSmeTspecInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 13) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 4;                      /* u32 primitive->transactionId */
+    bufferSize += 1;                      /* CsrWifiSmeTspecResultCode primitive->tspecResultCode */
+    bufferSize += 2;                      /* u16 primitive->tspecLength */
+    bufferSize += primitive->tspecLength; /* u8 primitive->tspec */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeTspecIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeTspecInd *primitive = (CsrWifiSmeTspecInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint32Ser(ptr, len, (u32) primitive->transactionId);
+    CsrUint8Ser(ptr, len, (u8) primitive->tspecResultCode);
+    CsrUint16Ser(ptr, len, (u16) primitive->tspecLength);
+    if (primitive->tspecLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->tspec, ((u16) (primitive->tspecLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeTspecIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeTspecInd *primitive = kmalloc(sizeof(CsrWifiSmeTspecInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->transactionId, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->tspecResultCode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->tspecLength, buffer, &offset);
+    if (primitive->tspecLength)
+    {
+        primitive->tspec = kmalloc(primitive->tspecLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->tspec, buffer, &offset, ((u16) (primitive->tspecLength)));
+    }
+    else
+    {
+        primitive->tspec = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeTspecIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeTspecInd *primitive = (CsrWifiSmeTspecInd *) voidPrimitivePointer;
+    kfree(primitive->tspec);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeTspecCfmSizeof(void *msg)
+{
+    CsrWifiSmeTspecCfm *primitive = (CsrWifiSmeTspecCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 15) */
+    bufferSize += 2;                      /* u16 primitive->interfaceTag */
+    bufferSize += 2;                      /* CsrResult primitive->status */
+    bufferSize += 4;                      /* u32 primitive->transactionId */
+    bufferSize += 1;                      /* CsrWifiSmeTspecResultCode primitive->tspecResultCode */
+    bufferSize += 2;                      /* u16 primitive->tspecLength */
+    bufferSize += primitive->tspecLength; /* u8 primitive->tspec */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeTspecCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeTspecCfm *primitive = (CsrWifiSmeTspecCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->interfaceTag);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint32Ser(ptr, len, (u32) primitive->transactionId);
+    CsrUint8Ser(ptr, len, (u8) primitive->tspecResultCode);
+    CsrUint16Ser(ptr, len, (u16) primitive->tspecLength);
+    if (primitive->tspecLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->tspec, ((u16) (primitive->tspecLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeTspecCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeTspecCfm *primitive = kmalloc(sizeof(CsrWifiSmeTspecCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->interfaceTag, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->transactionId, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->tspecResultCode, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->tspecLength, buffer, &offset);
+    if (primitive->tspecLength)
+    {
+        primitive->tspec = kmalloc(primitive->tspecLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->tspec, buffer, &offset, ((u16) (primitive->tspecLength)));
+    }
+    else
+    {
+        primitive->tspec = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeTspecCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeTspecCfm *primitive = (CsrWifiSmeTspecCfm *) voidPrimitivePointer;
+    kfree(primitive->tspec);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeVersionsGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeVersionsGetCfm *primitive = (CsrWifiSmeVersionsGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 33) */
+    bufferSize += 2;                                                                                    /* CsrResult primitive->status */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.chipId */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.chipVersion */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.firmwareBuild */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.firmwarePatch */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.firmwareHip */
+    bufferSize += (primitive->versions.routerBuild ? strlen(primitive->versions.routerBuild) : 0) + 1;  /* char* primitive->versions.routerBuild (0 byte len + 1 for NULL Term) */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.routerHip */
+    bufferSize += (primitive->versions.smeBuild ? strlen(primitive->versions.smeBuild) : 0) + 1;        /* char* primitive->versions.smeBuild (0 byte len + 1 for NULL Term) */
+    bufferSize += 4;                                                                                    /* u32 primitive->versions.smeHip */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeVersionsGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeVersionsGetCfm *primitive = (CsrWifiSmeVersionsGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.chipId);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.chipVersion);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.firmwareBuild);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.firmwarePatch);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.firmwareHip);
+    CsrCharStringSer(ptr, len, primitive->versions.routerBuild);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.routerHip);
+    CsrCharStringSer(ptr, len, primitive->versions.smeBuild);
+    CsrUint32Ser(ptr, len, (u32) primitive->versions.smeHip);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeVersionsGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeVersionsGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeVersionsGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.chipId, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.chipVersion, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.firmwareBuild, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.firmwarePatch, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.firmwareHip, buffer, &offset);
+    CsrCharStringDes(&primitive->versions.routerBuild, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.routerHip, buffer, &offset);
+    CsrCharStringDes(&primitive->versions.smeBuild, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->versions.smeHip, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeVersionsGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeVersionsGetCfm *primitive = (CsrWifiSmeVersionsGetCfm *) voidPrimitivePointer;
+    kfree(primitive->versions.routerBuild);
+    kfree(primitive->versions.smeBuild);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCloakedSsidsGetCfmSizeof(void *msg)
+{
+    CsrWifiSmeCloakedSsidsGetCfm *primitive = (CsrWifiSmeCloakedSsidsGetCfm *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 39) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* u8 primitive->cloakedSsids.cloakedSsidsCount */
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->cloakedSsids.cloakedSsidsCount; i2++)
+        {
+            bufferSize += 32; /* u8 primitive->cloakedSsids.cloakedSsids[i2].ssid[32] */
+            bufferSize += 1;  /* u8 primitive->cloakedSsids.cloakedSsids[i2].length */
+        }
+    }
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCloakedSsidsGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCloakedSsidsGetCfm *primitive = (CsrWifiSmeCloakedSsidsGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->cloakedSsids.cloakedSsidsCount);
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->cloakedSsids.cloakedSsidsCount; i2++)
+        {
+            CsrMemCpySer(ptr, len, (const void *) primitive->cloakedSsids.cloakedSsids[i2].ssid, ((u16) (32)));
+            CsrUint8Ser(ptr, len, (u8) primitive->cloakedSsids.cloakedSsids[i2].length);
+        }
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCloakedSsidsGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCloakedSsidsGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeCloakedSsidsGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->cloakedSsids.cloakedSsidsCount, buffer, &offset);
+    primitive->cloakedSsids.cloakedSsids = NULL;
+    if (primitive->cloakedSsids.cloakedSsidsCount)
+    {
+        primitive->cloakedSsids.cloakedSsids = kmalloc(sizeof(CsrWifiSsid) * primitive->cloakedSsids.cloakedSsidsCount, GFP_KERNEL);
+    }
+    {
+        u16 i2;
+        for (i2 = 0; i2 < primitive->cloakedSsids.cloakedSsidsCount; i2++)
+        {
+            CsrMemCpyDes(primitive->cloakedSsids.cloakedSsids[i2].ssid, buffer, &offset, ((u16) (32)));
+            CsrUint8Des((u8 *) &primitive->cloakedSsids.cloakedSsids[i2].length, buffer, &offset);
+        }
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeCloakedSsidsGetCfmSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeCloakedSsidsGetCfm *primitive = (CsrWifiSmeCloakedSsidsGetCfm *) voidPrimitivePointer;
+    kfree(primitive->cloakedSsids.cloakedSsids);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeWifiOnIndSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 6; /* u8 primitive->address.a[6] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeWifiOnIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeWifiOnInd *primitive = (CsrWifiSmeWifiOnInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrMemCpySer(ptr, len, (const void *) primitive->address.a, ((u16) (6)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmeWifiOnIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeWifiOnInd *primitive = kmalloc(sizeof(CsrWifiSmeWifiOnInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrMemCpyDes(primitive->address.a, buffer, &offset, ((u16) (6)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeSmeCommonConfigGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 10) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 1; /* CsrWifiSme80211dTrustLevel primitive->deviceConfig.trustLevel */
+    bufferSize += 2; /* u8 primitive->deviceConfig.countryCode[2] */
+    bufferSize += 1; /* CsrWifiSmeFirmwareDriverInterface primitive->deviceConfig.firmwareDriverInterface */
+    bufferSize += 1; /* u8 primitive->deviceConfig.enableStrictDraftN */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeSmeCommonConfigGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeSmeCommonConfigGetCfm *primitive = (CsrWifiSmeSmeCommonConfigGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint8Ser(ptr, len, (u8) primitive->deviceConfig.trustLevel);
+    CsrMemCpySer(ptr, len, (const void *) primitive->deviceConfig.countryCode, ((u16) (2)));
+    CsrUint8Ser(ptr, len, (u8) primitive->deviceConfig.firmwareDriverInterface);
+    CsrUint8Ser(ptr, len, (u8) primitive->deviceConfig.enableStrictDraftN);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeSmeCommonConfigGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeSmeCommonConfigGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeSmeCommonConfigGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->deviceConfig.trustLevel, buffer, &offset);
+    CsrMemCpyDes(primitive->deviceConfig.countryCode, buffer, &offset, ((u16) (2)));
+    CsrUint8Des((u8 *) &primitive->deviceConfig.firmwareDriverInterface, buffer, &offset);
+    CsrUint8Des((u8 *) &primitive->deviceConfig.enableStrictDraftN, buffer, &offset);
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeInterfaceCapabilityGetCfmSizeof(void *msg)
+{
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 9) */
+    bufferSize += 2; /* CsrResult primitive->status */
+    bufferSize += 2; /* u16 primitive->numInterfaces */
+    bufferSize += 2; /* u8 primitive->capBitmap[2] */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeInterfaceCapabilityGetCfmSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeInterfaceCapabilityGetCfm *primitive = (CsrWifiSmeInterfaceCapabilityGetCfm *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint16Ser(ptr, len, (u16) primitive->status);
+    CsrUint16Ser(ptr, len, (u16) primitive->numInterfaces);
+    CsrMemCpySer(ptr, len, (const void *) primitive->capBitmap, ((u16) (2)));
+    return(ptr);
+}
+
+
+void* CsrWifiSmeInterfaceCapabilityGetCfmDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeInterfaceCapabilityGetCfm *primitive = kmalloc(sizeof(CsrWifiSmeInterfaceCapabilityGetCfm), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->status, buffer, &offset);
+    CsrUint16Des((u16 *) &primitive->numInterfaces, buffer, &offset);
+    CsrMemCpyDes(primitive->capBitmap, buffer, &offset, ((u16) (2)));
+
+    return primitive;
+}
+
+
+size_t CsrWifiSmeErrorIndSizeof(void *msg)
+{
+    CsrWifiSmeErrorInd *primitive = (CsrWifiSmeErrorInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 3) */
+    bufferSize += (primitive->errorMessage ? strlen(primitive->errorMessage) : 0) + 1; /* char* primitive->errorMessage (0 byte len + 1 for NULL Term) */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeErrorIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeErrorInd *primitive = (CsrWifiSmeErrorInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrCharStringSer(ptr, len, primitive->errorMessage);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeErrorIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeErrorInd *primitive = kmalloc(sizeof(CsrWifiSmeErrorInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrCharStringDes(&primitive->errorMessage, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeErrorIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeErrorInd *primitive = (CsrWifiSmeErrorInd *) voidPrimitivePointer;
+    kfree(primitive->errorMessage);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeInfoIndSizeof(void *msg)
+{
+    CsrWifiSmeInfoInd *primitive = (CsrWifiSmeInfoInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 3) */
+    bufferSize += (primitive->infoMessage ? strlen(primitive->infoMessage) : 0) + 1; /* char* primitive->infoMessage (0 byte len + 1 for NULL Term) */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeInfoIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeInfoInd *primitive = (CsrWifiSmeInfoInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrCharStringSer(ptr, len, primitive->infoMessage);
+    return(ptr);
+}
+
+
+void* CsrWifiSmeInfoIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeInfoInd *primitive = kmalloc(sizeof(CsrWifiSmeInfoInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrCharStringDes(&primitive->infoMessage, buffer, &offset);
+
+    return primitive;
+}
+
+
+void CsrWifiSmeInfoIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeInfoInd *primitive = (CsrWifiSmeInfoInd *) voidPrimitivePointer;
+    kfree(primitive->infoMessage);
+    kfree(primitive);
+}
+
+
+size_t CsrWifiSmeCoreDumpIndSizeof(void *msg)
+{
+    CsrWifiSmeCoreDumpInd *primitive = (CsrWifiSmeCoreDumpInd *) msg;
+    size_t bufferSize = 2;
+
+    /* Calculate the Size of the Serialised Data. Could be more efficient (Try 8) */
+    bufferSize += 4;                     /* u32 primitive->dataLength */
+    bufferSize += primitive->dataLength; /* u8 primitive->data */
+    return bufferSize;
+}
+
+
+u8* CsrWifiSmeCoreDumpIndSer(u8 *ptr, size_t *len, void *msg)
+{
+    CsrWifiSmeCoreDumpInd *primitive = (CsrWifiSmeCoreDumpInd *)msg;
+    *len = 0;
+    CsrUint16Ser(ptr, len, primitive->common.type);
+    CsrUint32Ser(ptr, len, (u32) primitive->dataLength);
+    if (primitive->dataLength)
+    {
+        CsrMemCpySer(ptr, len, (const void *) primitive->data, ((u16) (primitive->dataLength)));
+    }
+    return(ptr);
+}
+
+
+void* CsrWifiSmeCoreDumpIndDes(u8 *buffer, size_t length)
+{
+    CsrWifiSmeCoreDumpInd *primitive = kmalloc(sizeof(CsrWifiSmeCoreDumpInd), GFP_KERNEL);
+    size_t offset;
+    offset = 0;
+
+    CsrUint16Des(&primitive->common.type, buffer, &offset);
+    CsrUint32Des((u32 *) &primitive->dataLength, buffer, &offset);
+    if (primitive->dataLength)
+    {
+        primitive->data = kmalloc(primitive->dataLength, GFP_KERNEL);
+        CsrMemCpyDes(primitive->data, buffer, &offset, ((u16) (primitive->dataLength)));
+    }
+    else
+    {
+        primitive->data = NULL;
+    }
+
+    return primitive;
+}
+
+
+void CsrWifiSmeCoreDumpIndSerFree(void *voidPrimitivePointer)
+{
+    CsrWifiSmeCoreDumpInd *primitive = (CsrWifiSmeCoreDumpInd *) voidPrimitivePointer;
+    kfree(primitive->data);
+    kfree(primitive);
+}
+
+
diff --git a/drivers/staging/csr/csr_wifi_sme_serialize.h b/drivers/staging/csr/csr_wifi_sme_serialize.h
new file mode 100644
index 00000000000..4f3af0a6be7
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_serialize.h
@@ -0,0 +1,670 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2012
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_SERIALIZE_H__
+#define CSR_WIFI_SME_SERIALIZE_H__
+
+#include "csr_wifi_msgconv.h"
+#include "csr_wifi_sme_prim.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern void CsrWifiSmePfree(void *ptr);
+
+#define CsrWifiSmeActivateReqSer CsrWifiEventSer
+#define CsrWifiSmeActivateReqDes CsrWifiEventDes
+#define CsrWifiSmeActivateReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeActivateReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeAdhocConfigGetReqSer CsrWifiEventSer
+#define CsrWifiSmeAdhocConfigGetReqDes CsrWifiEventDes
+#define CsrWifiSmeAdhocConfigGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeAdhocConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeAdhocConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeAdhocConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeAdhocConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeAdhocConfigSetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeBlacklistReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeBlacklistReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeBlacklistReqSizeof(void *msg);
+extern void CsrWifiSmeBlacklistReqSerFree(void *msg);
+
+#define CsrWifiSmeCalibrationDataGetReqSer CsrWifiEventSer
+#define CsrWifiSmeCalibrationDataGetReqDes CsrWifiEventDes
+#define CsrWifiSmeCalibrationDataGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeCalibrationDataGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCalibrationDataSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCalibrationDataSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCalibrationDataSetReqSizeof(void *msg);
+extern void CsrWifiSmeCalibrationDataSetReqSerFree(void *msg);
+
+#define CsrWifiSmeCcxConfigGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeCcxConfigGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeCcxConfigGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeCcxConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCcxConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCcxConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCcxConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeCcxConfigSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeCoexConfigGetReqSer CsrWifiEventSer
+#define CsrWifiSmeCoexConfigGetReqDes CsrWifiEventDes
+#define CsrWifiSmeCoexConfigGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeCoexConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCoexConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCoexConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCoexConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeCoexConfigSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeCoexInfoGetReqSer CsrWifiEventSer
+#define CsrWifiSmeCoexInfoGetReqDes CsrWifiEventDes
+#define CsrWifiSmeCoexInfoGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeCoexInfoGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeConnectReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeConnectReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeConnectReqSizeof(void *msg);
+extern void CsrWifiSmeConnectReqSerFree(void *msg);
+
+#define CsrWifiSmeConnectionConfigGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeConnectionConfigGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeConnectionConfigGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeConnectionConfigGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeConnectionInfoGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeConnectionInfoGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeConnectionInfoGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeConnectionInfoGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeConnectionStatsGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeConnectionStatsGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeConnectionStatsGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeConnectionStatsGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeDeactivateReqSer CsrWifiEventSer
+#define CsrWifiSmeDeactivateReqDes CsrWifiEventDes
+#define CsrWifiSmeDeactivateReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeDeactivateReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeDisconnectReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeDisconnectReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeDisconnectReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeDisconnectReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeEventMaskSetReqSer CsrWifiEventCsrUint32Ser
+#define CsrWifiSmeEventMaskSetReqDes CsrWifiEventCsrUint32Des
+#define CsrWifiSmeEventMaskSetReqSizeof CsrWifiEventCsrUint32Sizeof
+#define CsrWifiSmeEventMaskSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeHostConfigGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeHostConfigGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeHostConfigGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeHostConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeHostConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeHostConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeHostConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeHostConfigSetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeKeyReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeKeyReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeKeyReqSizeof(void *msg);
+#define CsrWifiSmeKeyReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeLinkQualityGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeLinkQualityGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeLinkQualityGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeLinkQualityGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeMibConfigGetReqSer CsrWifiEventSer
+#define CsrWifiSmeMibConfigGetReqDes CsrWifiEventDes
+#define CsrWifiSmeMibConfigGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeMibConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeMibConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeMibConfigSetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeMibGetNextReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibGetNextReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibGetNextReqSizeof(void *msg);
+extern void CsrWifiSmeMibGetNextReqSerFree(void *msg);
+
+extern u8* CsrWifiSmeMibGetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibGetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibGetReqSizeof(void *msg);
+extern void CsrWifiSmeMibGetReqSerFree(void *msg);
+
+extern u8* CsrWifiSmeMibSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibSetReqSizeof(void *msg);
+extern void CsrWifiSmeMibSetReqSerFree(void *msg);
+
+extern u8* CsrWifiSmeMulticastAddressReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMulticastAddressReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMulticastAddressReqSizeof(void *msg);
+extern void CsrWifiSmeMulticastAddressReqSerFree(void *msg);
+
+extern u8* CsrWifiSmePacketFilterSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePacketFilterSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePacketFilterSetReqSizeof(void *msg);
+extern void CsrWifiSmePacketFilterSetReqSerFree(void *msg);
+
+#define CsrWifiSmePermanentMacAddressGetReqSer CsrWifiEventSer
+#define CsrWifiSmePermanentMacAddressGetReqDes CsrWifiEventDes
+#define CsrWifiSmePermanentMacAddressGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmePermanentMacAddressGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmePmkidReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePmkidReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePmkidReqSizeof(void *msg);
+extern void CsrWifiSmePmkidReqSerFree(void *msg);
+
+#define CsrWifiSmePowerConfigGetReqSer CsrWifiEventSer
+#define CsrWifiSmePowerConfigGetReqDes CsrWifiEventDes
+#define CsrWifiSmePowerConfigGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmePowerConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmePowerConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePowerConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePowerConfigSetReqSizeof(void *msg);
+#define CsrWifiSmePowerConfigSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeRegulatoryDomainInfoGetReqSer CsrWifiEventSer
+#define CsrWifiSmeRegulatoryDomainInfoGetReqDes CsrWifiEventDes
+#define CsrWifiSmeRegulatoryDomainInfoGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeRegulatoryDomainInfoGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeRoamingConfigGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeRoamingConfigGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeRoamingConfigGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeRoamingConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeRoamingConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeRoamingConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeRoamingConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeRoamingConfigSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeScanConfigGetReqSer CsrWifiEventSer
+#define CsrWifiSmeScanConfigGetReqDes CsrWifiEventDes
+#define CsrWifiSmeScanConfigGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeScanConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeScanConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeScanConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeScanConfigSetReqSizeof(void *msg);
+extern void CsrWifiSmeScanConfigSetReqSerFree(void *msg);
+
+extern u8* CsrWifiSmeScanFullReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeScanFullReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeScanFullReqSizeof(void *msg);
+extern void CsrWifiSmeScanFullReqSerFree(void *msg);
+
+#define CsrWifiSmeScanResultsFlushReqSer CsrWifiEventSer
+#define CsrWifiSmeScanResultsFlushReqDes CsrWifiEventDes
+#define CsrWifiSmeScanResultsFlushReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeScanResultsFlushReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeScanResultsGetReqSer CsrWifiEventSer
+#define CsrWifiSmeScanResultsGetReqDes CsrWifiEventDes
+#define CsrWifiSmeScanResultsGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeScanResultsGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeSmeStaConfigGetReqSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeSmeStaConfigGetReqDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeSmeStaConfigGetReqSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeSmeStaConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeSmeStaConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeSmeStaConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeSmeStaConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeSmeStaConfigSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeStationMacAddressGetReqSer CsrWifiEventSer
+#define CsrWifiSmeStationMacAddressGetReqDes CsrWifiEventDes
+#define CsrWifiSmeStationMacAddressGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeStationMacAddressGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeTspecReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeTspecReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeTspecReqSizeof(void *msg);
+extern void CsrWifiSmeTspecReqSerFree(void *msg);
+
+#define CsrWifiSmeVersionsGetReqSer CsrWifiEventSer
+#define CsrWifiSmeVersionsGetReqDes CsrWifiEventDes
+#define CsrWifiSmeVersionsGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeVersionsGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeWifiFlightmodeReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeWifiFlightmodeReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeWifiFlightmodeReqSizeof(void *msg);
+extern void CsrWifiSmeWifiFlightmodeReqSerFree(void *msg);
+
+#define CsrWifiSmeWifiOffReqSer CsrWifiEventSer
+#define CsrWifiSmeWifiOffReqDes CsrWifiEventDes
+#define CsrWifiSmeWifiOffReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeWifiOffReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeWifiOnReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeWifiOnReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeWifiOnReqSizeof(void *msg);
+extern void CsrWifiSmeWifiOnReqSerFree(void *msg);
+
+extern u8* CsrWifiSmeCloakedSsidsSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCloakedSsidsSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCloakedSsidsSetReqSizeof(void *msg);
+extern void CsrWifiSmeCloakedSsidsSetReqSerFree(void *msg);
+
+#define CsrWifiSmeCloakedSsidsGetReqSer CsrWifiEventSer
+#define CsrWifiSmeCloakedSsidsGetReqDes CsrWifiEventDes
+#define CsrWifiSmeCloakedSsidsGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeCloakedSsidsGetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeSmeCommonConfigGetReqSer CsrWifiEventSer
+#define CsrWifiSmeSmeCommonConfigGetReqDes CsrWifiEventDes
+#define CsrWifiSmeSmeCommonConfigGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeSmeCommonConfigGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeSmeCommonConfigSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeSmeCommonConfigSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeSmeCommonConfigSetReqSizeof(void *msg);
+#define CsrWifiSmeSmeCommonConfigSetReqSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeInterfaceCapabilityGetReqSer CsrWifiEventSer
+#define CsrWifiSmeInterfaceCapabilityGetReqDes CsrWifiEventDes
+#define CsrWifiSmeInterfaceCapabilityGetReqSizeof CsrWifiEventSizeof
+#define CsrWifiSmeInterfaceCapabilityGetReqSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeWpsConfigurationReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeWpsConfigurationReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeWpsConfigurationReqSizeof(void *msg);
+extern void CsrWifiSmeWpsConfigurationReqSerFree(void *msg);
+
+extern u8* CsrWifiSmeSetReqSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeSetReqDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeSetReqSizeof(void *msg);
+extern void CsrWifiSmeSetReqSerFree(void *msg);
+
+#define CsrWifiSmeActivateCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeActivateCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeActivateCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeActivateCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeAdhocConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeAdhocConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeAdhocConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeAdhocConfigGetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeAdhocConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeAdhocConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeAdhocConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeAdhocConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeAssociationCompleteIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeAssociationCompleteIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeAssociationCompleteIndSizeof(void *msg);
+extern void CsrWifiSmeAssociationCompleteIndSerFree(void *msg);
+
+extern u8* CsrWifiSmeAssociationStartIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeAssociationStartIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeAssociationStartIndSizeof(void *msg);
+#define CsrWifiSmeAssociationStartIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeBlacklistCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeBlacklistCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeBlacklistCfmSizeof(void *msg);
+extern void CsrWifiSmeBlacklistCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeCalibrationDataGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCalibrationDataGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCalibrationDataGetCfmSizeof(void *msg);
+extern void CsrWifiSmeCalibrationDataGetCfmSerFree(void *msg);
+
+#define CsrWifiSmeCalibrationDataSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeCalibrationDataSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeCalibrationDataSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeCalibrationDataSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCcxConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCcxConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCcxConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeCcxConfigGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCcxConfigSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCcxConfigSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCcxConfigSetCfmSizeof(void *msg);
+#define CsrWifiSmeCcxConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCoexConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCoexConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCoexConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeCoexConfigGetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeCoexConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeCoexConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeCoexConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeCoexConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCoexInfoGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCoexInfoGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCoexInfoGetCfmSizeof(void *msg);
+#define CsrWifiSmeCoexInfoGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeConnectCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeConnectCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeConnectCfmSizeof(void *msg);
+#define CsrWifiSmeConnectCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeConnectionConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeConnectionConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeConnectionConfigGetCfmSizeof(void *msg);
+extern void CsrWifiSmeConnectionConfigGetCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeConnectionInfoGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeConnectionInfoGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeConnectionInfoGetCfmSizeof(void *msg);
+extern void CsrWifiSmeConnectionInfoGetCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeConnectionQualityIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeConnectionQualityIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeConnectionQualityIndSizeof(void *msg);
+#define CsrWifiSmeConnectionQualityIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeConnectionStatsGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeConnectionStatsGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeConnectionStatsGetCfmSizeof(void *msg);
+#define CsrWifiSmeConnectionStatsGetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeDeactivateCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeDeactivateCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeDeactivateCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeDeactivateCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeDisconnectCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeDisconnectCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeDisconnectCfmSizeof(void *msg);
+#define CsrWifiSmeDisconnectCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeEventMaskSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeEventMaskSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeEventMaskSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeEventMaskSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeHostConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeHostConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeHostConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeHostConfigGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeHostConfigSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeHostConfigSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeHostConfigSetCfmSizeof(void *msg);
+#define CsrWifiSmeHostConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeIbssStationIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeIbssStationIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeIbssStationIndSizeof(void *msg);
+#define CsrWifiSmeIbssStationIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeKeyCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeKeyCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeKeyCfmSizeof(void *msg);
+#define CsrWifiSmeKeyCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeLinkQualityGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeLinkQualityGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeLinkQualityGetCfmSizeof(void *msg);
+#define CsrWifiSmeLinkQualityGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeMediaStatusIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMediaStatusIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMediaStatusIndSizeof(void *msg);
+extern void CsrWifiSmeMediaStatusIndSerFree(void *msg);
+
+extern u8* CsrWifiSmeMibConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeMibConfigGetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeMibConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeMibConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeMibConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeMibConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeMibGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibGetCfmSizeof(void *msg);
+extern void CsrWifiSmeMibGetCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeMibGetNextCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMibGetNextCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMibGetNextCfmSizeof(void *msg);
+extern void CsrWifiSmeMibGetNextCfmSerFree(void *msg);
+
+#define CsrWifiSmeMibSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeMibSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeMibSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeMibSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeMicFailureIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMicFailureIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMicFailureIndSizeof(void *msg);
+#define CsrWifiSmeMicFailureIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeMulticastAddressCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeMulticastAddressCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeMulticastAddressCfmSizeof(void *msg);
+extern void CsrWifiSmeMulticastAddressCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmePacketFilterSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePacketFilterSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePacketFilterSetCfmSizeof(void *msg);
+#define CsrWifiSmePacketFilterSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmePermanentMacAddressGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePermanentMacAddressGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePermanentMacAddressGetCfmSizeof(void *msg);
+#define CsrWifiSmePermanentMacAddressGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmePmkidCandidateListIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePmkidCandidateListIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePmkidCandidateListIndSizeof(void *msg);
+extern void CsrWifiSmePmkidCandidateListIndSerFree(void *msg);
+
+extern u8* CsrWifiSmePmkidCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePmkidCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePmkidCfmSizeof(void *msg);
+extern void CsrWifiSmePmkidCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmePowerConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmePowerConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmePowerConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmePowerConfigGetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmePowerConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmePowerConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmePowerConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmePowerConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeRegulatoryDomainInfoGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeRegulatoryDomainInfoGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeRegulatoryDomainInfoGetCfmSizeof(void *msg);
+#define CsrWifiSmeRegulatoryDomainInfoGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeRoamCompleteIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeRoamCompleteIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeRoamCompleteIndSizeof(void *msg);
+#define CsrWifiSmeRoamCompleteIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeRoamStartIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeRoamStartIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeRoamStartIndSizeof(void *msg);
+#define CsrWifiSmeRoamStartIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeRoamingConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeRoamingConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeRoamingConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeRoamingConfigGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeRoamingConfigSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeRoamingConfigSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeRoamingConfigSetCfmSizeof(void *msg);
+#define CsrWifiSmeRoamingConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeScanConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeScanConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeScanConfigGetCfmSizeof(void *msg);
+extern void CsrWifiSmeScanConfigGetCfmSerFree(void *msg);
+
+#define CsrWifiSmeScanConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeScanConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeScanConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeScanConfigSetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeScanFullCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeScanFullCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeScanFullCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeScanFullCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeScanResultIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeScanResultIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeScanResultIndSizeof(void *msg);
+extern void CsrWifiSmeScanResultIndSerFree(void *msg);
+
+#define CsrWifiSmeScanResultsFlushCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeScanResultsFlushCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeScanResultsFlushCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeScanResultsFlushCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeScanResultsGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeScanResultsGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeScanResultsGetCfmSizeof(void *msg);
+extern void CsrWifiSmeScanResultsGetCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeSmeStaConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeSmeStaConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeSmeStaConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeSmeStaConfigGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeSmeStaConfigSetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeSmeStaConfigSetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeSmeStaConfigSetCfmSizeof(void *msg);
+#define CsrWifiSmeSmeStaConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeStationMacAddressGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeStationMacAddressGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeStationMacAddressGetCfmSizeof(void *msg);
+#define CsrWifiSmeStationMacAddressGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeTspecIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeTspecIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeTspecIndSizeof(void *msg);
+extern void CsrWifiSmeTspecIndSerFree(void *msg);
+
+extern u8* CsrWifiSmeTspecCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeTspecCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeTspecCfmSizeof(void *msg);
+extern void CsrWifiSmeTspecCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeVersionsGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeVersionsGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeVersionsGetCfmSizeof(void *msg);
+extern void CsrWifiSmeVersionsGetCfmSerFree(void *msg);
+
+#define CsrWifiSmeWifiFlightmodeCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeWifiFlightmodeCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeWifiFlightmodeCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeWifiFlightmodeCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeWifiOffIndSer CsrWifiEventCsrUint8Ser
+#define CsrWifiSmeWifiOffIndDes CsrWifiEventCsrUint8Des
+#define CsrWifiSmeWifiOffIndSizeof CsrWifiEventCsrUint8Sizeof
+#define CsrWifiSmeWifiOffIndSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeWifiOffCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeWifiOffCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeWifiOffCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeWifiOffCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeWifiOnCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeWifiOnCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeWifiOnCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeWifiOnCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeCloakedSsidsSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeCloakedSsidsSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeCloakedSsidsSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeCloakedSsidsSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeCloakedSsidsGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCloakedSsidsGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCloakedSsidsGetCfmSizeof(void *msg);
+extern void CsrWifiSmeCloakedSsidsGetCfmSerFree(void *msg);
+
+extern u8* CsrWifiSmeWifiOnIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeWifiOnIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeWifiOnIndSizeof(void *msg);
+#define CsrWifiSmeWifiOnIndSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeSmeCommonConfigGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeSmeCommonConfigGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeSmeCommonConfigGetCfmSizeof(void *msg);
+#define CsrWifiSmeSmeCommonConfigGetCfmSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeSmeCommonConfigSetCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeSmeCommonConfigSetCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeSmeCommonConfigSetCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeSmeCommonConfigSetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeInterfaceCapabilityGetCfmSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeInterfaceCapabilityGetCfmDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeInterfaceCapabilityGetCfmSizeof(void *msg);
+#define CsrWifiSmeInterfaceCapabilityGetCfmSerFree CsrWifiSmePfree
+
+extern u8* CsrWifiSmeErrorIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeErrorIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeErrorIndSizeof(void *msg);
+extern void CsrWifiSmeErrorIndSerFree(void *msg);
+
+extern u8* CsrWifiSmeInfoIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeInfoIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeInfoIndSizeof(void *msg);
+extern void CsrWifiSmeInfoIndSerFree(void *msg);
+
+extern u8* CsrWifiSmeCoreDumpIndSer(u8 *ptr, size_t *len, void *msg);
+extern void* CsrWifiSmeCoreDumpIndDes(u8 *buffer, size_t len);
+extern size_t CsrWifiSmeCoreDumpIndSizeof(void *msg);
+extern void CsrWifiSmeCoreDumpIndSerFree(void *msg);
+
+#define CsrWifiSmeAmpStatusChangeIndSer CsrWifiEventCsrUint16CsrUint8Ser
+#define CsrWifiSmeAmpStatusChangeIndDes CsrWifiEventCsrUint16CsrUint8Des
+#define CsrWifiSmeAmpStatusChangeIndSizeof CsrWifiEventCsrUint16CsrUint8Sizeof
+#define CsrWifiSmeAmpStatusChangeIndSerFree CsrWifiSmePfree
+
+#define CsrWifiSmeWpsConfigurationCfmSer CsrWifiEventCsrUint16Ser
+#define CsrWifiSmeWpsConfigurationCfmDes CsrWifiEventCsrUint16Des
+#define CsrWifiSmeWpsConfigurationCfmSizeof CsrWifiEventCsrUint16Sizeof
+#define CsrWifiSmeWpsConfigurationCfmSerFree CsrWifiSmePfree
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* CSR_WIFI_SME_SERIALIZE_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_sme_task.h b/drivers/staging/csr/csr_wifi_sme_task.h
new file mode 100644
index 00000000000..0f725e45493
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_sme_task.h
@@ -0,0 +1,33 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+/* Note: this is an auto-generated file. */
+
+#ifndef CSR_WIFI_SME_TASK_H__
+#define CSR_WIFI_SME_TASK_H__
+
+#include "csr_sched.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CSR_WIFI_SME_LOG_ID 0x1202FFFF
+extern CsrSchedQid CSR_WIFI_SME_IFACEQUEUE;
+void CsrWifiSmeInit(void **gash);
+void CsrWifiSmeDeinit(void **gash);
+void CsrWifiSmeHandler(void **gash);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_SME_TASK_H__ */
+
diff --git a/drivers/staging/csr/csr_wifi_vif_utils.h b/drivers/staging/csr/csr_wifi_vif_utils.h
new file mode 100644
index 00000000000..523172d1ac9
--- /dev/null
+++ b/drivers/staging/csr/csr_wifi_vif_utils.h
@@ -0,0 +1,108 @@
+/*****************************************************************************
+
+            (c) Cambridge Silicon Radio Limited 2011
+            All rights reserved and confidential information of CSR
+
+            Refer to LICENSE.txt included with this source for details
+            on the license terms.
+
+*****************************************************************************/
+
+#ifndef CSR_WIFI_VIF_UTILS_H
+#define CSR_WIFI_VIF_UTILS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* STANDARD INCLUDES ********************************************************/
+
+/* PROJECT INCLUDES *********************************************************/
+/* including this file for CsrWifiInterfaceMode*/
+#include "csr_wifi_private_common.h"
+
+/* MACROS *******************************************************************/
+
+/* Common macros for NME and SME to be used temporarily until SoftMAC changes are made */
+#define CSR_WIFI_NUM_INTERFACES        (u8)0x1
+#define CSR_WIFI_INTERFACE_IN_USE      (u16)0x0
+
+/* This is used at places where interface Id isn't available*/
+#define CSR_WIFI_INTERFACE_ZERO        0
+#define CSR_WIFI_INTERFACE_STA         0
+#define CSR_WIFI_INTERFACE_AMP         0
+
+
+#define CSR_WIFI_VIF_UTILS_UNDEFINED_TAG 0xFFFF
+
+/* Extract the Interface Id from the event */
+#define CsrWifiVifUtilsGetVifTagFromEvent(msg) \
+    ((u16) * ((u16 *) ((u8 *) (msg) + sizeof(CsrWifiFsmEvent))))
+
+/* The HPI Vif combines the type and the interface id */
+#define CsrWifiVifUtilsGetVifTagFromHipEvent(msg) \
+    ((msg)->virtualInterfaceIdentifier & 0x00FF)
+
+#define CsrWifiVifUtilsPackHipEventVif(type, interfaceId) \
+    ((u16)((interfaceId) | ((type) << 8)))
+
+
+/* TYPES DEFINITIONS ********************************************************/
+
+/* GLOBAL VARIABLE DECLARATIONS *********************************************/
+
+/* PUBLIC FUNCTION PROTOTYPES ***********************************************/
+
+/**
+ * @brief
+ *     First checks if the mode is supported capability bitmap of the interface.
+ *     If this succeeds, then checks if running this mode on this interface is allowed.
+ *
+ * @param[in] u8 : interface capability bitmap
+ * @param[in] u8* : pointer to the array of current interface modes
+ * @param[in] u16 : interfaceTag
+ * @param[in] CsrWifiInterfaceMode : mode
+ *
+ * @return
+ *     u8 : returns true if the interface is allowed to operate in the mode otherwise false.
+ */
+extern u8 CsrWifiVifUtilsCheckCompatibility(u8             interfaceCapability,
+                                                 u8            *currentInterfaceModes,
+                                                 u16            interfaceTag,
+                                                 CsrWifiInterfaceMode mode);
+
+/**
+ * @brief
+ *     Checks if the specified interface is supported.
+ *     NOTE: Only checks that the interface is supported, no checks are made to
+ *     determine whether a supported interface may be made active.
+ *
+ * @param[in] u16 : interfaceTag
+ *
+ * @return
+ *     u8 : returns true if the interface is supported, otherwise false.
+ */
+extern u8 CsrWifiVifUtilsIsSupported(u16 interfaceTag);
+
+#ifdef CSR_LOG_ENABLE
+/**
+ * @brief
+ *     Registers the virtual interface utils logging details.
+ *     Should only be called once at initialisation.
+ *
+ * @param[in/out] None
+ *
+ * @return
+ *     None
+ */
+void CsrWifiVifUtilsLogTextRegister(void);
+#else
+#define CsrWifiVifUtilsLogTextRegister()
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CSR_WIFI_VIF_UTILS_H */
+
diff --git a/drivers/staging/csr/data_tx.c b/drivers/staging/csr/data_tx.c
new file mode 100644
index 00000000000..8ed7a7845cc
--- /dev/null
+++ b/drivers/staging/csr/data_tx.c
@@ -0,0 +1,57 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     data_tx.c
+ *
+ * PURPOSE:
+ *      This file provides functions to send data requests to the UniFi.
+ *
+ * Copyright (C) 2007-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "unifi_priv.h"
+
+int
+uf_verify_m4(unifi_priv_t *priv, const unsigned char *packet, unsigned int length)
+{
+    const unsigned char *p = packet;
+    u16 keyinfo;
+
+
+    if (length < (4 + 5 + 8 + 32 + 16 + 8 + 8 + 16 + 1 + 8)) {
+        return 1;
+    }
+
+    p += 8;
+    keyinfo = p[5] << 8 | p[6]; /* big-endian */
+    if (
+          (p[0] == 1 || p[0] == 2) /* protocol version 802.1X-2001 (WPA) or -2004 (WPA2) */ &&
+          p[1] == 3 /* EAPOL-Key */ &&
+          /* don't bother checking p[2] p[3] (hh ll, packet body length) */
+          (p[4] == 254 || p[4] == 2) /* descriptor type P802.1i-D3.0 (WPA) or 802.11i-2004 (WPA2) */ &&
+          ((keyinfo & 0x0007) == 1 || (keyinfo & 0x0007) == 2) /* key descriptor version */ &&
+         (keyinfo & ~0x0207U) == 0x0108 && /* key info for 4/4 or 4/2 -- ignore key desc version and sec bit (since varies in WPA 4/4) */
+          (p[4 + 5 + 8 + 32 + 16 + 8 + 8 + 16 + 0] == 0 && /* key data length (2 octets) 0 for 4/4 only */
+           p[4 + 5 + 8 + 32 + 16 + 8 + 8 + 16 + 1] == 0)
+        ) {
+        unifi_trace(priv, UDBG1, "uf_verify_m4: M4 detected \n");
+        return 0;
+    }
+    else
+    {
+        return 1;
+    }
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *      Data transport signals.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
diff --git a/drivers/staging/csr/drv.c b/drivers/staging/csr/drv.c
new file mode 100644
index 00000000000..b2c27f4f03d
--- /dev/null
+++ b/drivers/staging/csr/drv.c
@@ -0,0 +1,2262 @@
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     drv.c
+ *
+ *  PURPOSE:
+ *      Conventional device interface for debugging/monitoring of the
+ *      driver and h/w using unicli. This interface is also being used
+ *      by the SME linux implementation and the helper apps.
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+
+
+/*
+ * Porting Notes:
+ * Part of this file contains an example for how to glue the OS layer
+ * with the HIP core lib, the SDIO glue layer, and the SME.
+ *
+ * When the unifi_sdio.ko modules loads, the linux kernel calls unifi_load().
+ * unifi_load() calls uf_sdio_load() which is exported by the SDIO glue
+ * layer. uf_sdio_load() registers this driver with the underlying SDIO driver.
+ * When a card is detected, the SDIO glue layer calls register_unifi_sdio()
+ * to pass the SDIO function context and ask the OS layer to initialise
+ * the card. register_unifi_sdio() allocates all the private data of the OS
+ * layer and calls uf_run_unifihelper() to start the SME. The SME calls
+ * unifi_sys_wifi_on_req() which uses the HIP core lib to initialise the card.
+ */
+
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/poll.h>
+#include <asm/uaccess.h>
+#include <linux/jiffies.h>
+
+#include "csr_wifi_hip_unifiversion.h"
+#include "unifi_priv.h"
+#include "csr_wifi_hip_conversions.h"
+#include "unifi_native.h"
+
+/* Module parameter variables */
+int buswidth = 0;               /* 0 means use default, values 1,4 */
+int sdio_clock = 50000;         /* kHz */
+int unifi_debug = 0;
+/* fw_init prevents f/w initialisation on error. */
+int fw_init[MAX_UNIFI_DEVS] = {-1, -1};
+int use_5g = 0;
+int led_mask = 0;               /* 0x0c00 for dev-pc-1503c, dev-pc-1528a */
+int disable_hw_reset = 0;
+int disable_power_control = 0;
+int enable_wol = UNIFI_WOL_OFF; /* 0 for none, 1 for SDIO IRQ, 2 for PIO */
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+int tl_80211d = (int)CSR_WIFI_SME_80211D_TRUST_LEVEL_MIB;
+#endif
+int sdio_block_size = -1;      /* Override SDIO block size */
+int sdio_byte_mode = 0;        /* 0 for block mode + padding, 1 for byte mode */
+int coredump_max = CSR_WIFI_HIP_NUM_COREDUMP_BUFFERS;
+int run_bh_once = -1;          /* Set for scheduled interrupt mode, -1 = default */
+int bh_priority = -1;
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+#define UNIFI_LOG_HIP_SIGNALS_FILTER_BULKDATA   (1 << 1)
+#define UNIFI_LOG_HIP_SIGNALS_FILTER_TIMESTAMP  (1 << 2)
+int log_hip_signals = 0;
+#endif
+
+MODULE_DESCRIPTION("CSR UniFi (SDIO)");
+
+module_param(buswidth,    int, S_IRUGO|S_IWUSR);
+module_param(sdio_clock,  int, S_IRUGO|S_IWUSR);
+module_param(unifi_debug, int, S_IRUGO|S_IWUSR);
+module_param_array(fw_init, int, NULL, S_IRUGO|S_IWUSR);
+module_param(use_5g,      int, S_IRUGO|S_IWUSR);
+module_param(led_mask,    int, S_IRUGO|S_IWUSR);
+module_param(disable_hw_reset,  int, S_IRUGO|S_IWUSR);
+module_param(disable_power_control,  int, S_IRUGO|S_IWUSR);
+module_param(enable_wol,  int, S_IRUGO|S_IWUSR);
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+module_param(tl_80211d,   int, S_IRUGO|S_IWUSR);
+#endif
+module_param(sdio_block_size, int, S_IRUGO|S_IWUSR);
+module_param(sdio_byte_mode, int, S_IRUGO|S_IWUSR);
+module_param(coredump_max, int, S_IRUGO|S_IWUSR);
+module_param(run_bh_once, int, S_IRUGO|S_IWUSR);
+module_param(bh_priority, int, S_IRUGO|S_IWUSR);
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+module_param(log_hip_signals, int, S_IRUGO|S_IWUSR);
+#endif
+
+MODULE_PARM_DESC(buswidth, "SDIO bus width (0=default), set 1 for 1-bit or 4 for 4-bit mode");
+MODULE_PARM_DESC(sdio_clock, "SDIO bus frequency in kHz, (default = 50 MHz)");
+MODULE_PARM_DESC(unifi_debug, "Diagnostic reporting level");
+MODULE_PARM_DESC(fw_init, "Set to 0 to prevent f/w initialization on error");
+MODULE_PARM_DESC(use_5g, "Use the 5G (802.11a) radio band");
+MODULE_PARM_DESC(led_mask, "LED mask flags");
+MODULE_PARM_DESC(disable_hw_reset, "Set to 1 to disable hardware reset");
+MODULE_PARM_DESC(disable_power_control, "Set to 1 to disable SDIO power control");
+MODULE_PARM_DESC(enable_wol, "Enable wake-on-wlan function 0=off, 1=SDIO, 2=PIO");
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+MODULE_PARM_DESC(tl_80211d, "802.11d Trust Level (1-6, default = 5)");
+#endif
+MODULE_PARM_DESC(sdio_block_size, "Set to override SDIO block size");
+MODULE_PARM_DESC(sdio_byte_mode, "Set to 1 for byte mode SDIO");
+MODULE_PARM_DESC(coredump_max, "Number of chip mini-coredump buffers to allocate");
+MODULE_PARM_DESC(run_bh_once, "Run BH only when firmware interrupts");
+MODULE_PARM_DESC(bh_priority, "Modify the BH thread priority");
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+MODULE_PARM_DESC(log_hip_signals, "Set to 1 to enable HIP signal offline logging");
+#endif
+
+
+/* Callback for event logging to UDI clients */
+static void udi_log_event(ul_client_t *client,
+                          const u8 *signal, int signal_len,
+                          const bulk_data_param_t *bulkdata,
+                          int dir);
+
+static void udi_set_log_filter(ul_client_t *pcli,
+                               unifiio_filter_t *udi_filter);
+
+
+/* Mutex to protect access to  priv->sme_cli */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+DEFINE_SEMAPHORE(udi_mutex);
+#else
+DECLARE_MUTEX(udi_mutex);
+#endif
+
+s32 CsrHipResultToStatus(CsrResult csrResult)
+{
+    s32 r = -EIO;
+
+    switch (csrResult)
+    {
+    case CSR_RESULT_SUCCESS:
+        r = 0;
+        break;
+    case CSR_WIFI_HIP_RESULT_RANGE:
+        r = -ERANGE;
+        break;
+    case CSR_WIFI_HIP_RESULT_NO_DEVICE:
+        r = -ENODEV;
+        break;
+    case CSR_WIFI_HIP_RESULT_INVALID_VALUE:
+        r = -EINVAL;
+        break;
+    case CSR_WIFI_HIP_RESULT_NOT_FOUND:
+        r = -ENOENT;
+        break;
+    case CSR_WIFI_HIP_RESULT_NO_SPACE:
+        r = -ENOSPC;
+        break;
+    case CSR_WIFI_HIP_RESULT_NO_MEMORY:
+        r = -ENOMEM;
+        break;
+    case CSR_RESULT_FAILURE:
+        r = -EIO;
+        break;
+    default:
+        /*unifi_warning(card->ospriv, "CsrHipResultToStatus: Unrecognised csrResult error code: %d\n", csrResult);*/
+        r = -EIO;
+    }
+    return r;
+}
+
+
+static const char*
+trace_putest_cmdid(unifi_putest_command_t putest_cmd)
+{
+    switch (putest_cmd)
+    {
+        case UNIFI_PUTEST_START:
+            return "START";
+        case UNIFI_PUTEST_STOP:
+            return "STOP";
+        case UNIFI_PUTEST_SET_SDIO_CLOCK:
+            return "SET CLOCK";
+        case UNIFI_PUTEST_CMD52_READ:
+            return "CMD52R";
+        case UNIFI_PUTEST_CMD52_BLOCK_READ:
+            return "CMD52BR";
+        case UNIFI_PUTEST_CMD52_WRITE:
+            return "CMD52W";
+        case UNIFI_PUTEST_DL_FW:
+            return "D/L FW";
+        case UNIFI_PUTEST_DL_FW_BUFF:
+            return "D/L FW BUFFER";
+        case UNIFI_PUTEST_COREDUMP_PREPARE:
+            return "PREPARE COREDUMP";
+        case UNIFI_PUTEST_GP_READ16:
+            return "GP16R";
+        case UNIFI_PUTEST_GP_WRITE16:
+            return "GP16W";
+        default:
+            return "ERROR: unrecognised command";
+    }
+ }
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+int uf_register_hip_offline_debug(unifi_priv_t *priv)
+{
+    ul_client_t *udi_cli;
+    int i;
+
+    udi_cli = ul_register_client(priv, CLI_USING_WIRE_FORMAT, udi_log_event);
+    if (udi_cli == NULL) {
+        /* Too many clients already using this device */
+        unifi_error(priv, "Too many UDI clients already open\n");
+        return -ENOSPC;
+    }
+    unifi_trace(priv, UDBG1, "Offline HIP client is registered\n");
+
+    down(&priv->udi_logging_mutex);
+    udi_cli->event_hook = udi_log_event;
+    unifi_set_udi_hook(priv->card, logging_handler);
+    /* Log all signals by default */
+    for (i = 0; i < SIG_FILTER_SIZE; i++) {
+        udi_cli->signal_filter[i] = 0xFFFF;
+    }
+    priv->logging_client = udi_cli;
+    up(&priv->udi_logging_mutex);
+
+    return 0;
+}
+
+int uf_unregister_hip_offline_debug(unifi_priv_t *priv)
+{
+    ul_client_t *udi_cli = priv->logging_client;
+    if (udi_cli == NULL)
+    {
+        unifi_error(priv, "Unknown HIP client unregister request\n");
+        return -ERANGE;
+    }
+
+    unifi_trace(priv, UDBG1, "Offline HIP client is unregistered\n");
+
+    down(&priv->udi_logging_mutex);
+    priv->logging_client = NULL;
+    udi_cli->event_hook = NULL;
+    up(&priv->udi_logging_mutex);
+
+    ul_deregister_client(udi_cli);
+
+    return 0;
+}
+#endif
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_open
+ *  unifi_release
+ *
+ *      Open and release entry points for the UniFi debug driver.
+ *
+ *  Arguments:
+ *      Normal linux driver args.
+ *
+ *  Returns:
+ *      Linux error code.
+ * ---------------------------------------------------------------------------
+ */
+static int
+unifi_open(struct inode *inode, struct file *file)
+{
+    int devno;
+    unifi_priv_t *priv;
+    ul_client_t *udi_cli;
+
+    func_enter();
+
+    devno = MINOR(inode->i_rdev) >> 1;
+
+    /*
+     * Increase the ref_count for the char device clients.
+     * Make sure you call uf_put_instance() to decreace it if
+     * unifi_open returns an error.
+     */
+    priv = uf_get_instance(devno);
+    if (priv == NULL) {
+        unifi_error(NULL, "unifi_open: No device present\n");
+        func_exit();
+        return -ENODEV;
+    }
+
+    /* Register this instance in the client's list. */
+    /* The minor number determines the nature of the client (Unicli or SME). */
+    if (MINOR(inode->i_rdev) & 0x1) {
+        udi_cli = ul_register_client(priv, CLI_USING_WIRE_FORMAT, udi_log_event);
+        if (udi_cli == NULL) {
+            /* Too many clients already using this device */
+            unifi_error(priv, "Too many clients already open\n");
+            uf_put_instance(devno);
+            func_exit();
+            return -ENOSPC;
+        }
+        unifi_trace(priv, UDBG1, "Client is registered to /dev/unifiudi%d\n", devno);
+    } else {
+        /*
+         * Even-numbered device nodes are the control application.
+         * This is the userspace helper containing SME or
+         * unifi_manager.
+         */
+
+        down(&udi_mutex);
+
+#ifdef CSR_SME_USERSPACE
+        /* Check if a config client is already attached */
+        if (priv->sme_cli) {
+            up(&udi_mutex);
+            uf_put_instance(devno);
+
+            unifi_info(priv, "There is already a configuration client using the character device\n");
+            func_exit();
+            return -EBUSY;
+        }
+#endif /* CSR_SME_USERSPACE */
+
+#ifdef CSR_SUPPORT_SME
+        udi_cli = ul_register_client(priv,
+                                     CLI_USING_WIRE_FORMAT | CLI_SME_USERSPACE,
+                                     sme_log_event);
+#else
+        /* Config client for native driver */
+        udi_cli = ul_register_client(priv,
+                                     0,
+                                     sme_native_log_event);
+#endif
+        if (udi_cli == NULL) {
+            /* Too many clients already using this device */
+            up(&udi_mutex);
+            uf_put_instance(devno);
+
+            unifi_error(priv, "Too many clients already open\n");
+            func_exit();
+            return -ENOSPC;
+        }
+
+        /*
+         * Fill-in the pointer to the configuration client.
+         * This is the SME userspace helper or unifi_manager.
+         * Not used in the SME embedded version.
+         */
+        unifi_trace(priv, UDBG1, "SME client (id:%d s:0x%X) is registered\n",
+                    udi_cli->client_id, udi_cli->sender_id);
+        /* Store the SME UniFi Linux Client */
+        if (priv->sme_cli == NULL) {
+            priv->sme_cli = udi_cli;
+        }
+
+        up(&udi_mutex);
+    }
+
+
+    /*
+     * Store the pointer to the client.
+     * All char driver's entry points will pass this pointer.
+     */
+    file->private_data = udi_cli;
+
+    func_exit();
+    return 0;
+} /* unifi_open() */
+
+
+static int
+unifi_release(struct inode *inode, struct file *filp)
+{
+    ul_client_t *udi_cli = (void*)filp->private_data;
+    int devno;
+    unifi_priv_t *priv;
+
+    func_enter();
+
+    priv = uf_find_instance(udi_cli->instance);
+    if (!priv) {
+        unifi_error(priv, "unifi_close: instance for device not found\n");
+        return -ENODEV;
+    }
+
+    devno = MINOR(inode->i_rdev) >> 1;
+
+    /* Even device nodes are the config client (i.e. SME or unifi_manager) */
+    if ((MINOR(inode->i_rdev) & 0x1) == 0) {
+
+        if (priv->sme_cli != udi_cli) {
+            unifi_notice(priv, "Surprise closing config device: not the sme client\n");
+        }
+        unifi_notice(priv, "SME client close (unifi%d)\n", devno);
+
+        /*
+         * Clear sme_cli before calling unifi_sys_... so it doesn't try to
+         * queue a reply to the (now gone) SME.
+         */
+        down(&udi_mutex);
+        priv->sme_cli = NULL;
+        up(&udi_mutex);
+
+#ifdef CSR_SME_USERSPACE
+        /* Power-down when config client closes */
+        {
+            CsrWifiRouterCtrlWifiOffReq req = {{CSR_WIFI_ROUTER_CTRL_HIP_REQ, 0, 0, 0, NULL}};
+            CsrWifiRouterCtrlWifiOffReqHandler(priv, &req.common);
+        }
+
+        uf_sme_deinit(priv);
+
+       /* It is possible that a blocking SME request was made from another process
+        * which did not get read by the SME before the WifiOffReq.
+        * So check for a pending request which will go unanswered and cancel
+        * the wait for event. As only one blocking request can be in progress at
+        * a time, up to one event should be completed.
+        */
+       uf_sme_cancel_request(priv, 0);
+
+#endif /* CSR_SME_USERSPACE */
+    } else {
+
+        unifi_trace(priv, UDBG2, "UDI client close (unifiudi%d)\n", devno);
+
+        /* If the pointer matches the logging client, stop logging. */
+        down(&priv->udi_logging_mutex);
+        if (udi_cli == priv->logging_client) {
+            priv->logging_client = NULL;
+        }
+        up(&priv->udi_logging_mutex);
+
+        if (udi_cli == priv->amp_client) {
+            priv->amp_client = NULL;
+        }
+    }
+
+    /* Deregister this instance from the client's list. */
+    ul_deregister_client(udi_cli);
+
+    uf_put_instance(devno);
+
+    return 0;
+} /* unifi_release() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_read
+ *
+ *      The read() driver entry point.
+ *
+ *  Arguments:
+ *      filp        The file descriptor returned by unifi_open()
+ *      p           The user space buffer to copy the read data
+ *      len         The size of the p buffer
+ *      poff
+ *
+ *  Returns:
+ *      number of bytes read or an error code on failure
+ * ---------------------------------------------------------------------------
+ */
+static ssize_t
+unifi_read(struct file *filp, char *p, size_t len, loff_t *poff)
+{
+    ul_client_t *pcli = (void*)filp->private_data;
+    unifi_priv_t *priv;
+    udi_log_t *logptr = NULL;
+    udi_msg_t *msgptr;
+    struct list_head *l;
+    int msglen;
+
+    func_enter();
+
+    priv = uf_find_instance(pcli->instance);
+    if (!priv) {
+        unifi_error(priv, "invalid priv\n");
+        return -ENODEV;
+    }
+
+    if (!pcli->udi_enabled) {
+        unifi_error(priv, "unifi_read: unknown client.");
+        return -EINVAL;
+    }
+
+    if (list_empty(&pcli->udi_log)) {
+        if (filp->f_flags & O_NONBLOCK) {
+            /* Non-blocking - just return if the udi_log is empty */
+            return 0;
+        } else {
+            /* Blocking - wait on the UDI wait queue */
+            if (wait_event_interruptible(pcli->udi_wq,
+                !list_empty(&pcli->udi_log)))
+            {
+                unifi_error(priv, "unifi_read: wait_event_interruptible failed.");
+                return -ERESTARTSYS;
+            }
+        }
+    }
+
+    /* Read entry from list head and remove it from the list */
+    if (down_interruptible(&pcli->udi_sem)) {
+        return -ERESTARTSYS;
+    }
+    l = pcli->udi_log.next;
+    list_del(l);
+    up(&pcli->udi_sem);
+
+    /* Get a pointer to whole struct */
+    logptr = list_entry(l, udi_log_t, q);
+    if (logptr == NULL) {
+        unifi_error(priv, "unifi_read: failed to get event.\n");
+        return -EINVAL;
+    }
+
+    /* Get the real message */
+    msgptr = &logptr->msg;
+    msglen = msgptr->length;
+    if (msglen > len) {
+        printk(KERN_WARNING "truncated read to %d actual msg len is %lu\n", msglen, (long unsigned int)len);
+        msglen = len;
+    }
+
+    /* and pass it to the client (SME or Unicli). */
+    if (copy_to_user(p, msgptr, msglen))
+    {
+        printk(KERN_ERR "Failed to copy UDI log to user\n");
+        kfree(logptr);
+        return -EFAULT;
+    }
+
+    /* It is our resposibility to free the message buffer. */
+    kfree(logptr);
+
+    func_exit_r(msglen);
+    return msglen;
+
+} /* unifi_read() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * udi_send_signal_unpacked
+ *
+ *      Sends an unpacked signal to UniFi.
+ *
+ * Arguments:
+ *      priv            Pointer to private context struct
+ *      data            Pointer to request structure and data to send
+ *      data_len        Length of data in data pointer.
+ *
+ * Returns:
+ *      Number of bytes written, error otherwise.
+ *
+ * Notes:
+ *      All clients that use this function to send a signal to the unifi
+ *      must use the host formatted structures.
+ * ---------------------------------------------------------------------------
+ */
+static int
+udi_send_signal_unpacked(unifi_priv_t *priv, unsigned char* data, uint data_len)
+{
+    CSR_SIGNAL *sigptr = (CSR_SIGNAL*)data;
+    CSR_DATAREF *datarefptr;
+    bulk_data_param_t bulk_data;
+    uint signal_size, i;
+    uint bulk_data_offset = 0;
+    int bytecount, r;
+    CsrResult csrResult;
+
+    /* Number of bytes in the signal */
+    signal_size = SigGetSize(sigptr);
+    if (!signal_size || (signal_size > data_len)) {
+        unifi_error(priv, "unifi_sme_mlme_req - Invalid signal 0x%x size should be %d bytes\n",
+                    sigptr->SignalPrimitiveHeader.SignalId,
+                    signal_size);
+        return -EINVAL;
+    }
+    bytecount = signal_size;
+
+    /* Get a pointer to the information of the first data reference */
+    datarefptr = (CSR_DATAREF*)&sigptr->u;
+
+    /* Initialize the offset in the data buffer, bulk data is right after the signal. */
+    bulk_data_offset = signal_size;
+
+    /* store the references and the size of the bulk data to the bulkdata structure */
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+        /* the length of the bulk data is in the signal */
+        if ((datarefptr+i)->DataLength) {
+            void *dest;
+
+            csrResult = unifi_net_data_malloc(priv, &bulk_data.d[i], (datarefptr+i)->DataLength);
+            if (csrResult != CSR_RESULT_SUCCESS) {
+                unifi_error(priv, "udi_send_signal_unpacked: failed to allocate request_data.\n");
+                return -EIO;
+            }
+
+            dest = (void*)bulk_data.d[i].os_data_ptr;
+            memcpy(dest, data + bulk_data_offset, bulk_data.d[i].data_length);
+        } else {
+            bulk_data.d[i].data_length = 0;
+        }
+
+        bytecount += bulk_data.d[i].data_length;
+        /* advance the offset, to point the next bulk data */
+        bulk_data_offset += bulk_data.d[i].data_length;
+    }
+
+
+    unifi_trace(priv, UDBG3, "SME Send: signal 0x%.4X\n", sigptr->SignalPrimitiveHeader.SignalId);
+
+    /* Send the signal. */
+    r = ul_send_signal_unpacked(priv, sigptr, &bulk_data);
+    if (r < 0) {
+        unifi_error(priv, "udi_send_signal_unpacked: send failed (%d)\n", r);
+        for(i=0;i<UNIFI_MAX_DATA_REFERENCES;i++) {
+            if(bulk_data.d[i].data_length != 0) {
+                unifi_net_data_free(priv, &bulk_data.d[i]);
+            }
+        }
+        func_exit();
+        return -EIO;
+    }
+
+    return bytecount;
+} /* udi_send_signal_unpacked() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * udi_send_signal_raw
+ *
+ *      Sends a packed signal to UniFi.
+ *
+ * Arguments:
+ *      priv            Pointer to private context struct
+ *      buf             Pointer to request structure and data to send
+ *      buflen          Length of data in data pointer.
+ *
+ * Returns:
+ *      Number of bytes written, error otherwise.
+ *
+ * Notes:
+ *      All clients that use this function to send a signal to the unifi
+ *      must use the wire formatted structures.
+ * ---------------------------------------------------------------------------
+ */
+static int
+udi_send_signal_raw(unifi_priv_t *priv, unsigned char *buf, int buflen)
+{
+    int signal_size;
+    int sig_id;
+    bulk_data_param_t data_ptrs;
+    int i, r;
+    unsigned int num_data_refs;
+    int bytecount;
+    CsrResult csrResult;
+
+    func_enter();
+
+    /*
+     * The signal is the first thing in buf, the signal id is the
+     * first 16 bits of the signal.
+     */
+    /* Number of bytes in the signal */
+    sig_id = GET_SIGNAL_ID(buf);
+    signal_size = buflen;
+    signal_size -= GET_PACKED_DATAREF_LEN(buf, 0);
+    signal_size -= GET_PACKED_DATAREF_LEN(buf, 1);
+    if ((signal_size <= 0) || (signal_size > buflen)) {
+        unifi_error(priv, "udi_send_signal_raw - Couldn't find length of signal 0x%x\n",
+                    sig_id);
+        func_exit();
+        return -EINVAL;
+    }
+    unifi_trace(priv, UDBG2, "udi_send_signal_raw: signal 0x%.4X len:%d\n",
+                sig_id, signal_size);
+    /* Zero the data ref arrays */
+    memset(&data_ptrs, 0, sizeof(data_ptrs));
+
+    /*
+     * Find the number of associated bulk data packets.  Scan through
+     * the data refs to check that we have enough data and pick out
+     * pointers to appended bulk data.
+     */
+    num_data_refs = 0;
+    bytecount = signal_size;
+
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; ++i)
+    {
+        unsigned int len = GET_PACKED_DATAREF_LEN(buf, i);
+        unifi_trace(priv, UDBG3, "udi_send_signal_raw: data_ref length = %d\n", len);
+
+        if (len != 0) {
+            void *dest;
+
+            csrResult = unifi_net_data_malloc(priv, &data_ptrs.d[i], len);
+            if (csrResult != CSR_RESULT_SUCCESS) {
+                unifi_error(priv, "udi_send_signal_raw: failed to allocate request_data.\n");
+                return -EIO;
+            }
+
+            dest = (void*)data_ptrs.d[i].os_data_ptr;
+            memcpy(dest, buf + bytecount, len);
+
+            bytecount += len;
+            num_data_refs++;
+        }
+        data_ptrs.d[i].data_length = len;
+    }
+
+    unifi_trace(priv, UDBG3, "Queueing signal 0x%.4X from UDI with %u data refs\n",
+          sig_id,
+          num_data_refs);
+
+    if (bytecount > buflen) {
+        unifi_error(priv, "udi_send_signal_raw: Not enough data (%d instead of %d)\n", buflen, bytecount);
+        func_exit();
+        return -EINVAL;
+    }
+
+    /* Send the signal calling the function that uses the wire-formatted signals. */
+    r = ul_send_signal_raw(priv, buf, signal_size, &data_ptrs);
+    if (r < 0) {
+        unifi_error(priv, "udi_send_signal_raw: send failed (%d)\n", r);
+        func_exit();
+        return -EIO;
+    }
+
+#ifdef CSR_NATIVE_LINUX
+    if (sig_id == CSR_MLME_POWERMGT_REQUEST_ID) {
+        int power_mode = CSR_GET_UINT16_FROM_LITTLE_ENDIAN((buf +
+                                              SIZEOF_SIGNAL_HEADER + (UNIFI_MAX_DATA_REFERENCES*SIZEOF_DATAREF)));
+#ifdef CSR_SUPPORT_WEXT
+        /* Overide the wext power mode to the new value */
+        priv->wext_conf.power_mode = power_mode;
+#endif
+        /* Configure deep sleep signaling */
+        if (power_mode || (priv->interfacePriv[0]->connected == UnifiNotConnected)) {
+            csrResult = unifi_configure_low_power_mode(priv->card,
+                                                   UNIFI_LOW_POWER_ENABLED,
+                                                   UNIFI_PERIODIC_WAKE_HOST_DISABLED);
+        } else {
+            csrResult = unifi_configure_low_power_mode(priv->card,
+                                                   UNIFI_LOW_POWER_DISABLED,
+                                                   UNIFI_PERIODIC_WAKE_HOST_DISABLED);
+        }
+    }
+#endif
+
+    func_exit_r(bytecount);
+
+    return bytecount;
+} /* udi_send_signal_raw */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_write
+ *
+ *      The write() driver entry point.
+ *      A UniFi Debug Interface client such as unicli can write a signal
+ *      plus bulk data to the driver for sending to the UniFi chip.
+ *
+ *      Only one signal may be sent per write operation.
+ *
+ *  Arguments:
+ *      filp        The file descriptor returned by unifi_open()
+ *      p           The user space buffer to get the data from
+ *      len         The size of the p buffer
+ *      poff
+ *
+ *  Returns:
+ *      number of bytes written or an error code on failure
+ * ---------------------------------------------------------------------------
+ */
+static ssize_t
+unifi_write(struct file *filp, const char *p, size_t len, loff_t *poff)
+{
+    ul_client_t *pcli = (ul_client_t*)filp->private_data;
+    unifi_priv_t *priv;
+    unsigned char *buf;
+    unsigned char *bufptr;
+    int remaining;
+    int bytes_written;
+    int r;
+    bulk_data_param_t bulkdata;
+    CsrResult csrResult;
+
+    func_enter();
+
+    priv = uf_find_instance(pcli->instance);
+    if (!priv) {
+        unifi_error(priv, "invalid priv\n");
+        return -ENODEV;
+    }
+
+    unifi_trace(priv, UDBG5, "unifi_write: len = %d\n", len);
+
+    if (!pcli->udi_enabled) {
+        unifi_error(priv, "udi disabled\n");
+        return -EINVAL;
+    }
+
+    /*
+     * AMP client sends only one signal at a time, so we can use
+     * unifi_net_data_malloc to save the extra copy.
+     */
+    if (pcli == priv->amp_client) {
+        int signal_size;
+        int sig_id;
+        unsigned char *signal_buf;
+        char *user_data_buf;
+
+        csrResult = unifi_net_data_malloc(priv, &bulkdata.d[0], len);
+        if (csrResult != CSR_RESULT_SUCCESS) {
+            unifi_error(priv, "unifi_write: failed to allocate request_data.\n");
+            func_exit();
+            return -ENOMEM;
+        }
+
+        user_data_buf = (char*)bulkdata.d[0].os_data_ptr;
+
+        /* Get the data from the AMP client. */
+        if (copy_from_user((void*)user_data_buf, p, len)) {
+            unifi_error(priv, "unifi_write: copy from user failed\n");
+            unifi_net_data_free(priv, &bulkdata.d[0]);
+            func_exit();
+            return -EFAULT;
+        }
+
+        bulkdata.d[1].os_data_ptr = NULL;
+        bulkdata.d[1].data_length = 0;
+
+        /* Number of bytes in the signal */
+        sig_id = GET_SIGNAL_ID(bulkdata.d[0].os_data_ptr);
+        signal_size = len;
+        signal_size -= GET_PACKED_DATAREF_LEN(bulkdata.d[0].os_data_ptr, 0);
+        signal_size -= GET_PACKED_DATAREF_LEN(bulkdata.d[0].os_data_ptr, 1);
+        if ((signal_size <= 0) || (signal_size > len)) {
+            unifi_error(priv, "unifi_write - Couldn't find length of signal 0x%x\n",
+                        sig_id);
+            unifi_net_data_free(priv, &bulkdata.d[0]);
+            func_exit();
+            return -EINVAL;
+        }
+
+        unifi_trace(priv, UDBG2, "unifi_write: signal 0x%.4X len:%d\n",
+                    sig_id, signal_size);
+
+        /* Allocate a buffer for the signal */
+        signal_buf = kmalloc(signal_size, GFP_KERNEL);
+        if (!signal_buf) {
+            unifi_net_data_free(priv, &bulkdata.d[0]);
+            func_exit();
+            return -ENOMEM;
+        }
+
+        /* Get the signal from the os_data_ptr */
+        memcpy(signal_buf, bulkdata.d[0].os_data_ptr, signal_size);
+        signal_buf[5] = (pcli->sender_id >> 8) & 0xff;
+
+        if (signal_size < len) {
+            /* Remove the signal from the os_data_ptr */
+            bulkdata.d[0].data_length -= signal_size;
+            bulkdata.d[0].os_data_ptr += signal_size;
+        } else {
+            bulkdata.d[0].data_length = 0;
+            bulkdata.d[0].os_data_ptr = NULL;
+        }
+
+        /* Send the signal calling the function that uses the wire-formatted signals. */
+        r = ul_send_signal_raw(priv, signal_buf, signal_size, &bulkdata);
+        if (r < 0) {
+            unifi_error(priv, "unifi_write: send failed (%d)\n", r);
+            if (bulkdata.d[0].os_data_ptr != NULL) {
+                unifi_net_data_free(priv, &bulkdata.d[0]);
+            }
+        }
+
+        /* Free the signal buffer and return */
+        kfree(signal_buf);
+        return len;
+    }
+
+    buf = kmalloc(len, GFP_KERNEL);
+    if (!buf) {
+        return -ENOMEM;
+    }
+
+    /* Get the data from the client (SME or Unicli). */
+    if (copy_from_user((void*)buf, p, len)) {
+        unifi_error(priv, "copy from user failed\n");
+        kfree(buf);
+        return -EFAULT;
+    }
+
+    /*
+     * In SME userspace build read() contains a SYS or MGT message.
+     * Note that even though the SME sends one signal at a time, we can not
+     * use unifi_net_data_malloc because in the early stages, before having
+     * initialised the core, it will fail since the I/O block size is unknown.
+     */
+#ifdef CSR_SME_USERSPACE
+    if (pcli->configuration & CLI_SME_USERSPACE) {
+        CsrWifiRouterTransportRecv(priv, buf, len);
+        kfree(buf);
+        return len;
+    }
+#endif
+
+    /* ul_send_signal_raw will  do a sanity check of len against signal content */
+
+    /*
+     * udi_send_signal_raw() and udi_send_signal_unpacked() return the number of bytes consumed.
+     * A write call can pass multiple signal concatenated together.
+     */
+    bytes_written = 0;
+    remaining = len;
+    bufptr = buf;
+    while (remaining > 0)
+    {
+        int r;
+
+        /*
+         * Set the SenderProcessId.
+         * The SignalPrimitiveHeader is the first 3 16-bit words of the signal,
+         * the SenderProcessId is bytes 4,5.
+         * The MSB of the sender ID needs to be set to the client ID.
+         * The LSB is controlled by the SME.
+         */
+        bufptr[5] = (pcli->sender_id >> 8) & 0xff;
+
+        /* use the appropriate interface, depending on the clients' configuration */
+        if (pcli->configuration & CLI_USING_WIRE_FORMAT) {
+            unifi_trace(priv, UDBG1, "unifi_write: call udi_send_signal().\n");
+            r = udi_send_signal_raw(priv, bufptr, remaining);
+        } else {
+            r = udi_send_signal_unpacked(priv, bufptr, remaining);
+        }
+        if (r < 0) {
+            /* Set the return value to the error code */
+            unifi_error(priv, "unifi_write: (udi or sme)_send_signal() returns %d\n", r);
+            bytes_written = r;
+            break;
+        }
+        bufptr += r;
+        remaining -= r;
+        bytes_written += r;
+    }
+
+    kfree(buf);
+
+    func_exit_r(bytes_written);
+
+    return bytes_written;
+} /* unifi_write() */
+
+
+static const char* build_type_to_string(unsigned char build_type)
+{
+    switch (build_type)
+    {
+    case UNIFI_BUILD_NME: return "NME";
+    case UNIFI_BUILD_WEXT: return "WEXT";
+    case UNIFI_BUILD_AP: return "AP";
+    }
+    return "unknown";
+}
+
+
+/*
+ * ----------------------------------------------------------------
+ *  unifi_ioctl
+ *
+ *      Ioctl handler for unifi driver.
+ *
+ * Arguments:
+ *  inodep          Pointer to inode structure.
+ *  filp            Pointer to file structure.
+ *  cmd             Ioctl cmd passed by user.
+ *  arg             Ioctl arg passed by user.
+ *
+ * Returns:
+ *      0 on success, -ve error code on error.
+ * ----------------------------------------------------------------
+ */
+static long
+unifi_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+    ul_client_t *pcli = (ul_client_t*)filp->private_data;
+    unifi_priv_t *priv;
+    struct net_device *dev;
+    int r = 0;
+    int int_param, i;
+    u8* buf;
+    CsrResult csrResult;
+#if (defined CSR_SUPPORT_SME)
+    unifi_cfg_command_t cfg_cmd;
+#if (defined CSR_SUPPORT_WEXT)
+    CsrWifiSmeCoexConfig coex_config;
+    unsigned char uchar_param;
+    unsigned char varbind[MAX_VARBIND_LENGTH];
+    int vblen;
+#endif
+#endif
+    unifi_putest_command_t putest_cmd;
+
+    priv = uf_find_instance(pcli->instance);
+    if (!priv) {
+        unifi_error(priv, "ioctl error: unknown instance=%d\n", pcli->instance);
+        r = -ENODEV;
+        goto out;
+    }
+    unifi_trace(priv, UDBG5, "unifi_ioctl: cmd=0x%X, arg=0x%lX\n", cmd, arg);
+
+    switch (cmd) {
+
+      case UNIFI_GET_UDI_ENABLE:
+        unifi_trace(priv, UDBG4, "UniFi Get UDI Enable\n");
+
+        down(&priv->udi_logging_mutex);
+        int_param = (priv->logging_client == NULL) ? 0 : 1;
+        up(&priv->udi_logging_mutex);
+
+        if (put_user(int_param, (int*)arg))
+        {
+            unifi_error(priv, "UNIFI_GET_UDI_ENABLE: Failed to copy to user\n");
+            r = -EFAULT;
+            goto out;
+        }
+        break;
+
+      case UNIFI_SET_UDI_ENABLE:
+        unifi_trace(priv, UDBG4, "UniFi Set UDI Enable\n");
+        if (get_user(int_param, (int*)arg))
+        {
+            unifi_error(priv, "UNIFI_SET_UDI_ENABLE: Failed to copy from user\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+        if (log_hip_signals) {
+            unifi_error(priv, "omnicli cannot be used when log_hip_signals is used\n");
+            r = -EFAULT;
+            goto out;
+        }
+#endif
+
+        down(&priv->udi_logging_mutex);
+        if (int_param) {
+            pcli->event_hook = udi_log_event;
+            unifi_set_udi_hook(priv->card, logging_handler);
+            /* Log all signals by default */
+            for (i = 0; i < SIG_FILTER_SIZE; i++) {
+                pcli->signal_filter[i] = 0xFFFF;
+            }
+            priv->logging_client = pcli;
+
+        } else {
+            priv->logging_client = NULL;
+            pcli->event_hook = NULL;
+        }
+        up(&priv->udi_logging_mutex);
+
+        break;
+
+      case UNIFI_SET_MIB:
+        unifi_trace(priv, UDBG4, "UniFi Set MIB\n");
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+        /* Read first 2 bytes and check length */
+        if (copy_from_user((void*)varbind, (void*)arg, 2)) {
+            unifi_error(priv,
+                        "UNIFI_SET_MIB: Failed to copy in varbind header\n");
+            r = -EFAULT;
+            goto out;
+        }
+        vblen = varbind[1];
+        if ((vblen + 2) > MAX_VARBIND_LENGTH) {
+            unifi_error(priv,
+                        "UNIFI_SET_MIB: Varbind too long (%d, limit %d)\n",
+                        (vblen+2), MAX_VARBIND_LENGTH);
+            r = -EINVAL;
+            goto out;
+        }
+        /* Read rest of varbind */
+        if (copy_from_user((void*)(varbind+2), (void*)(arg+2), vblen)) {
+            unifi_error(priv, "UNIFI_SET_MIB: Failed to copy in varbind\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        /* send to SME */
+        vblen += 2;
+        r = sme_mgt_mib_set(priv, varbind, vblen);
+        if (r) {
+            goto out;
+        }
+#else
+        unifi_notice(priv, "UNIFI_SET_MIB: Unsupported.\n");
+#endif /* CSR_SUPPORT_WEXT */
+        break;
+
+      case UNIFI_GET_MIB:
+        unifi_trace(priv, UDBG4, "UniFi Get MIB\n");
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+        /* Read first 2 bytes and check length */
+        if (copy_from_user((void*)varbind, (void*)arg, 2)) {
+            unifi_error(priv, "UNIFI_GET_MIB: Failed to copy in varbind header\n");
+            r = -EFAULT;
+            goto out;
+        }
+        vblen = varbind[1];
+        if ((vblen+2) > MAX_VARBIND_LENGTH) {
+            unifi_error(priv, "UNIFI_GET_MIB: Varbind too long (%d, limit %d)\n",
+                        (vblen+2), MAX_VARBIND_LENGTH);
+            r = -EINVAL;
+            goto out;
+        }
+        /* Read rest of varbind */
+        if (copy_from_user((void*)(varbind+2), (void*)(arg+2), vblen)) {
+            unifi_error(priv, "UNIFI_GET_MIB: Failed to copy in varbind\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        vblen += 2;
+        r = sme_mgt_mib_get(priv, varbind, &vblen);
+        if (r) {
+            goto out;
+        }
+        /* copy out varbind */
+        if (vblen > MAX_VARBIND_LENGTH) {
+            unifi_error(priv,
+                        "UNIFI_GET_MIB: Varbind result too long (%d, limit %d)\n",
+                        vblen, MAX_VARBIND_LENGTH);
+            r = -EINVAL;
+            goto out;
+        }
+        if (copy_to_user((void*)arg, varbind, vblen)) {
+            r = -EFAULT;
+            goto out;
+        }
+#else
+        unifi_notice(priv, "UNIFI_GET_MIB: Unsupported.\n");
+#endif /* CSR_SUPPORT_WEXT */
+        break;
+
+      case UNIFI_CFG:
+#if (defined CSR_SUPPORT_SME)
+        if (get_user(cfg_cmd, (unifi_cfg_command_t*)arg))
+        {
+            unifi_error(priv, "UNIFI_CFG: Failed to get the command\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        unifi_trace(priv, UDBG1, "UNIFI_CFG: Command is %d (t=%u) sz=%d\n",
+                    cfg_cmd, jiffies_to_msecs(jiffies), sizeof(unifi_cfg_command_t));
+        switch (cfg_cmd) {
+          case UNIFI_CFG_POWER:
+            r = unifi_cfg_power(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_POWERSAVE:
+            r = unifi_cfg_power_save(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_POWERSUPPLY:
+            r = unifi_cfg_power_supply(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_FILTER:
+            r = unifi_cfg_packet_filters(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_GET:
+            r = unifi_cfg_get_info(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_WMM_QOSINFO:
+            r = unifi_cfg_wmm_qos_info(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_WMM_ADDTS:
+            r = unifi_cfg_wmm_addts(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_WMM_DELTS:
+            r = unifi_cfg_wmm_delts(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_STRICT_DRAFT_N:
+            r = unifi_cfg_strict_draft_n(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_CFG_ENABLE_OKC:
+            r = unifi_cfg_enable_okc(priv, (unsigned char*)arg);
+            break;
+#ifdef CSR_SUPPORT_SME
+          case UNIFI_CFG_CORE_DUMP:
+            CsrWifiRouterCtrlWifiOffIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,CSR_WIFI_SME_CONTROL_INDICATION_ERROR);
+            unifi_trace(priv, UDBG2, "UNIFI_CFG_CORE_DUMP: sent wifi off indication\n");
+            break;
+#endif
+#ifdef CSR_SUPPORT_WEXT_AP
+          case UNIFI_CFG_SET_AP_CONFIG:
+            r= unifi_cfg_set_ap_config(priv,(unsigned char*)arg);
+            break;
+#endif
+          default:
+            unifi_error(priv, "UNIFI_CFG: Unknown Command (%d)\n", cfg_cmd);
+            r = -EINVAL;
+            goto out;
+        }
+#endif
+
+        break;
+
+      case UNIFI_PUTEST:
+        if (get_user(putest_cmd, (unifi_putest_command_t*)arg))
+        {
+            unifi_error(priv, "UNIFI_PUTEST: Failed to get the command\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        unifi_trace(priv, UDBG1, "UNIFI_PUTEST: Command is %s\n",
+                    trace_putest_cmdid(putest_cmd));
+        switch (putest_cmd) {
+          case UNIFI_PUTEST_START:
+            r = unifi_putest_start(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_STOP:
+            r = unifi_putest_stop(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_SET_SDIO_CLOCK:
+            r = unifi_putest_set_sdio_clock(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_CMD52_READ:
+            r = unifi_putest_cmd52_read(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_CMD52_BLOCK_READ:
+            r = unifi_putest_cmd52_block_read(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_CMD52_WRITE:
+            r = unifi_putest_cmd52_write(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_DL_FW:
+            r = unifi_putest_dl_fw(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_DL_FW_BUFF:
+            r = unifi_putest_dl_fw_buff(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_COREDUMP_PREPARE:
+            r = unifi_putest_coredump_prepare(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_GP_READ16:
+            r = unifi_putest_gp_read16(priv, (unsigned char*)arg);
+            break;
+          case UNIFI_PUTEST_GP_WRITE16:
+            r = unifi_putest_gp_write16(priv, (unsigned char*)arg);
+            break;
+          default:
+            unifi_error(priv, "UNIFI_PUTEST: Unknown Command (%d)\n", putest_cmd);
+            r = -EINVAL;
+            goto out;
+        }
+
+        break;
+      case UNIFI_BUILD_TYPE:
+        unifi_trace(priv, UDBG2, "UNIFI_BUILD_TYPE userspace=%s\n", build_type_to_string(*(unsigned char*)arg));
+#ifndef CSR_SUPPORT_WEXT_AP
+        if (UNIFI_BUILD_AP == *(unsigned char*)arg)
+        {
+            unifi_error(priv, "Userspace has AP support, which is incompatible\n");
+        }
+#endif
+
+#ifndef CSR_SUPPORT_WEXT
+        if (UNIFI_BUILD_WEXT == *(unsigned char*)arg)
+        {
+            unifi_error(priv, "Userspace has WEXT support, which is incompatible\n");
+        }
+#endif
+        break;
+      case UNIFI_INIT_HW:
+        unifi_trace(priv, UDBG2, "UNIFI_INIT_HW.\n");
+        priv->init_progress = UNIFI_INIT_NONE;
+
+#if defined(CSR_SUPPORT_WEXT) || defined (CSR_NATIVE_LINUX)
+        /* At this point we are ready to start the SME. */
+        r = sme_mgt_wifi_on(priv);
+        if (r) {
+            goto out;
+        }
+#endif
+
+        break;
+
+      case UNIFI_INIT_NETDEV:
+        {
+            /* get the proper interfaceTagId */
+            u16 interfaceTag=0;
+            netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+            dev = priv->netdev[interfaceTag];
+            unifi_trace(priv, UDBG2, "UNIFI_INIT_NETDEV.\n");
+
+            if (copy_from_user((void*)dev->dev_addr, (void*)arg, 6)) {
+                r = -EFAULT;
+                goto out;
+            }
+
+            /* Attach the network device to the stack */
+            if (!interfacePriv->netdev_registered)
+            {
+                r = uf_register_netdev(priv,interfaceTag);
+                if (r) {
+                    unifi_error(priv, "Failed to register the network device.\n");
+                    goto out;
+                }
+            }
+
+            /* Apply scheduled interrupt mode, if requested by module param */
+            if (run_bh_once != -1) {
+                unifi_set_interrupt_mode(priv->card, (u32)run_bh_once);
+            }
+
+            priv->init_progress = UNIFI_INIT_COMPLETED;
+
+            /* Firmware initialisation is complete, so let the SDIO bus
+             * clock be raised when convienent to the core.
+             */
+            unifi_request_max_sdio_clock(priv->card);
+
+#ifdef CSR_SUPPORT_WEXT
+            /* Notify the Android wpa_supplicant that we are ready */
+            wext_send_started_event(priv);
+#endif
+
+            unifi_info(priv, "UniFi ready\n");
+
+#ifdef ANDROID_BUILD
+            /* Release the wakelock */
+            unifi_trace(priv, UDBG1, "netdev_init: release wake lock\n");
+            wake_unlock(&unifi_sdio_wake_lock);
+#endif
+#ifdef CSR_NATIVE_SOFTMAC /* For softmac dev, force-enable the network interface rather than wait for a connected-ind */
+            {
+                struct net_device *dev = priv->netdev[interfaceTag];
+#ifdef CSR_SUPPORT_WEXT
+                interfacePriv->wait_netdev_change = TRUE;
+#endif
+                netif_carrier_on(dev);
+            }
+#endif
+        }
+        break;
+      case UNIFI_GET_INIT_STATUS:
+        unifi_trace(priv, UDBG2, "UNIFI_GET_INIT_STATUS.\n");
+        if (put_user(priv->init_progress, (int*)arg))
+        {
+            printk(KERN_ERR "UNIFI_GET_INIT_STATUS: Failed to copy to user\n");
+            r = -EFAULT;
+            goto out;
+        }
+        break;
+
+      case UNIFI_KICK:
+        unifi_trace(priv, UDBG4, "Kick UniFi\n");
+        unifi_sdio_interrupt_handler(priv->card);
+        break;
+
+      case UNIFI_SET_DEBUG:
+        unifi_debug = arg;
+        unifi_trace(priv, UDBG4, "unifi_debug set to %d\n", unifi_debug);
+        break;
+
+      case UNIFI_SET_TRACE:
+        /* no longer supported */
+        r = -EINVAL;
+        break;
+
+
+      case UNIFI_SET_UDI_LOG_MASK:
+        {
+            unifiio_filter_t udi_filter;
+            uint16_t *sig_ids_addr;
+#define UF_MAX_SIG_IDS  128     /* Impose a sensible limit */
+
+            if (copy_from_user((void*)(&udi_filter), (void*)arg, sizeof(udi_filter))) {
+                r = -EFAULT;
+                goto out;
+            }
+            if ((udi_filter.action < UfSigFil_AllOn) ||
+                (udi_filter.action > UfSigFil_SelectOff))
+            {
+                printk(KERN_WARNING
+                       "UNIFI_SET_UDI_LOG_MASK: Bad action value: %d\n",
+                       udi_filter.action);
+                r = -EINVAL;
+                goto out;
+            }
+            /* No signal list for "All" actions */
+            if ((udi_filter.action == UfSigFil_AllOn) ||
+                (udi_filter.action == UfSigFil_AllOff))
+            {
+                udi_filter.num_sig_ids = 0;
+            }
+
+            if (udi_filter.num_sig_ids > UF_MAX_SIG_IDS) {
+                printk(KERN_WARNING
+                       "UNIFI_SET_UDI_LOG_MASK: too many signal ids (%d, max %d)\n",
+                       udi_filter.num_sig_ids, UF_MAX_SIG_IDS);
+                r = -EINVAL;
+                goto out;
+            }
+
+            /* Copy in signal id list if given */
+            if (udi_filter.num_sig_ids > 0) {
+                /* Preserve userspace address of sig_ids array */
+                sig_ids_addr = udi_filter.sig_ids;
+                /* Allocate kernel memory for sig_ids and copy to it */
+                udi_filter.sig_ids =
+                    kmalloc(udi_filter.num_sig_ids * sizeof(uint16_t), GFP_KERNEL);
+                if (!udi_filter.sig_ids) {
+                    r = -ENOMEM;
+                    goto out;
+                }
+                if (copy_from_user((void*)udi_filter.sig_ids,
+                                   (void*)sig_ids_addr,
+                                   udi_filter.num_sig_ids * sizeof(uint16_t)))
+                {
+                    kfree(udi_filter.sig_ids);
+                    r = -EFAULT;
+                    goto out;
+                }
+            }
+
+            udi_set_log_filter(pcli, &udi_filter);
+
+            if (udi_filter.num_sig_ids > 0) {
+                kfree(udi_filter.sig_ids);
+            }
+        }
+        break;
+
+      case UNIFI_SET_AMP_ENABLE:
+        unifi_trace(priv, UDBG4, "UniFi Set AMP Enable\n");
+        if (get_user(int_param, (int*)arg))
+        {
+            unifi_error(priv, "UNIFI_SET_AMP_ENABLE: Failed to copy from user\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        if (int_param) {
+            priv->amp_client = pcli;
+        } else {
+            priv->amp_client = NULL;
+        }
+
+        int_param = 0;
+        buf = (u8*)&int_param;
+        buf[0] = UNIFI_SOFT_COMMAND_Q_LENGTH - 1;
+        buf[1] = UNIFI_SOFT_TRAFFIC_Q_LENGTH - 1;
+        if (copy_to_user((void*)arg, &int_param, sizeof(int))) {
+            r = -EFAULT;
+            goto out;
+        }
+        break;
+
+      case UNIFI_SET_UDI_SNAP_MASK:
+        {
+            unifiio_snap_filter_t snap_filter;
+
+            if (copy_from_user((void*)(&snap_filter), (void*)arg, sizeof(snap_filter))) {
+                r = -EFAULT;
+                goto out;
+            }
+
+            if (pcli->snap_filter.count) {
+                pcli->snap_filter.count = 0;
+                kfree(pcli->snap_filter.protocols);
+            }
+
+            if (snap_filter.count == 0) {
+                break;
+            }
+
+            pcli->snap_filter.protocols = kmalloc(snap_filter.count * sizeof(u16), GFP_KERNEL);
+            if (!pcli->snap_filter.protocols) {
+                r = -ENOMEM;
+                goto out;
+            }
+            if (copy_from_user((void*)pcli->snap_filter.protocols,
+                               (void*)snap_filter.protocols,
+                               snap_filter.count * sizeof(u16)))
+            {
+                kfree(pcli->snap_filter.protocols);
+                r = -EFAULT;
+                goto out;
+            }
+
+            pcli->snap_filter.count = snap_filter.count;
+
+        }
+        break;
+
+      case UNIFI_SME_PRESENT:
+        {
+            u8 ind;
+            unifi_trace(priv, UDBG4, "UniFi SME Present IOCTL.\n");
+            if (copy_from_user((void*)(&int_param), (void*)arg, sizeof(int)))
+            {
+                printk(KERN_ERR "UNIFI_SME_PRESENT: Failed to copy from user\n");
+                r = -EFAULT;
+                goto out;
+            }
+
+            priv->sme_is_present = int_param;
+            if (priv->sme_is_present == 1) {
+                ind = CONFIG_SME_PRESENT;
+            } else {
+                ind = CONFIG_SME_NOT_PRESENT;
+            }
+            /* Send an indication to the helper app. */
+            ul_log_config_ind(priv, &ind, sizeof(u8));
+        }
+        break;
+
+      case UNIFI_CFG_PERIOD_TRAFFIC:
+      {
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+          CsrWifiSmeCoexConfig coexConfig;
+#endif /* CSR_SUPPORT_SME && CSR_SUPPORT_WEXT */
+        unifi_trace(priv, UDBG4, "UniFi Configure Periodic Traffic.\n");
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+        if (copy_from_user((void*)(&uchar_param), (void*)arg, sizeof(unsigned char))) {
+            unifi_error(priv, "UNIFI_CFG_PERIOD_TRAFFIC: Failed to copy from user\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        if (uchar_param == 0) {
+            r = sme_mgt_coex_config_get(priv, &coexConfig);
+            if (r) {
+                unifi_error(priv, "UNIFI_CFG_PERIOD_TRAFFIC: Get unifi_CoexInfoValue failed.\n");
+                goto out;
+            }
+            if (copy_to_user((void*)(arg + 1),
+                             (void*)&coexConfig,
+                             sizeof(CsrWifiSmeCoexConfig))) {
+                r = -EFAULT;
+                goto out;
+            }
+            goto out;
+        }
+
+        if (copy_from_user((void*)(&coex_config), (void*)(arg + 1), sizeof(CsrWifiSmeCoexConfig)))
+        {
+            unifi_error(priv, "UNIFI_CFG_PERIOD_TRAFFIC: Failed to copy from user\n");
+            r = -EFAULT;
+            goto out;
+        }
+
+        coexConfig = coex_config;
+        r = sme_mgt_coex_config_set(priv, &coexConfig);
+        if (r) {
+            unifi_error(priv, "UNIFI_CFG_PERIOD_TRAFFIC: Set unifi_CoexInfoValue failed.\n");
+            goto out;
+        }
+
+#endif /* CSR_SUPPORT_SME && CSR_SUPPORT_WEXT */
+        break;
+      }
+      case UNIFI_CFG_UAPSD_TRAFFIC:
+        unifi_trace(priv, UDBG4, "UniFi Configure U-APSD Mask.\n");
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+        if (copy_from_user((void*)(&uchar_param), (void*)arg, sizeof(unsigned char))) {
+            unifi_error(priv, "UNIFI_CFG_UAPSD_TRAFFIC: Failed to copy from user\n");
+            r = -EFAULT;
+            goto out;
+        }
+        unifi_trace(priv, UDBG4, "New U-APSD Mask: 0x%x\n", uchar_param);
+#endif /* CSR_SUPPORT_SME && CSR_SUPPORT_WEXT */
+        break;
+
+#ifndef UNIFI_DISABLE_COREDUMP
+      case UNIFI_COREDUMP_GET_REG:
+        unifi_trace(priv, UDBG4, "Mini-coredump data request\n");
+        {
+            unifiio_coredump_req_t dump_req;    /* Public OS layer structure */
+            unifi_coredump_req_t priv_req;      /* Private HIP structure */
+
+            if (copy_from_user((void*)(&dump_req), (void*)arg, sizeof(dump_req))) {
+                r = -EFAULT;
+                goto out;
+            }
+            memset(&priv_req, 0, sizeof(priv_req));
+            priv_req.index = dump_req.index;
+            priv_req.offset = dump_req.offset;
+
+            /* Convert OS-layer's XAP memory space ID to HIP's ID in case they differ */
+            switch (dump_req.space) {
+                case UNIFIIO_COREDUMP_MAC_REG: priv_req.space = UNIFI_COREDUMP_MAC_REG; break;
+                case UNIFIIO_COREDUMP_PHY_REG: priv_req.space = UNIFI_COREDUMP_PHY_REG; break;
+                case UNIFIIO_COREDUMP_SH_DMEM: priv_req.space = UNIFI_COREDUMP_SH_DMEM; break;
+                case UNIFIIO_COREDUMP_MAC_DMEM: priv_req.space = UNIFI_COREDUMP_MAC_DMEM; break;
+                case UNIFIIO_COREDUMP_PHY_DMEM: priv_req.space = UNIFI_COREDUMP_PHY_DMEM; break;
+                case UNIFIIO_COREDUMP_TRIGGER_MAGIC: priv_req.space = UNIFI_COREDUMP_TRIGGER_MAGIC; break;
+                default:
+                  r = -EINVAL;
+                  goto out;
+            }
+
+            if (priv_req.space == UNIFI_COREDUMP_TRIGGER_MAGIC) {
+                /* Force a coredump grab now */
+                unifi_trace(priv, UDBG2, "UNIFI_COREDUMP_GET_REG: Force capture\n");
+                csrResult = unifi_coredump_capture(priv->card, &priv_req);
+                r = CsrHipResultToStatus(csrResult);
+                unifi_trace(priv, UDBG5, "UNIFI_COREDUMP_GET_REG: status %d\n", r);
+            } else {
+                /* Retrieve the appropriate register entry */
+                csrResult = unifi_coredump_get_value(priv->card, &priv_req);
+                r = CsrHipResultToStatus(csrResult);
+                if (r) {
+                    unifi_trace(priv, UDBG5, "UNIFI_COREDUMP_GET_REG: Status %d\n", r);
+                    goto out;
+                }
+                /* Update the OS-layer structure with values returned in the private */
+                dump_req.value = priv_req.value;
+                dump_req.timestamp = priv_req.timestamp;
+                dump_req.requestor = priv_req.requestor;
+                dump_req.serial = priv_req.serial;
+                dump_req.chip_ver = priv_req.chip_ver;
+                dump_req.fw_ver = priv_req.fw_ver;
+                dump_req.drv_build = 0;
+
+                unifi_trace(priv, UDBG6,
+                            "Dump: %d (seq %d): V:0x%04x (%d) @0x%02x:%04x = 0x%04x\n",
+                            dump_req.index, dump_req.serial,
+                            dump_req.chip_ver, dump_req.drv_build,
+                            dump_req.space, dump_req.offset, dump_req.value);
+            }
+            if (copy_to_user((void*)arg, (void*)&dump_req, sizeof(dump_req))) {
+                r = -EFAULT;
+                goto out;
+            }
+        }
+        break;
+#endif
+      default:
+        r = -EINVAL;
+    }
+
+out:
+    return (long)r;
+} /* unifi_ioctl() */
+
+
+
+static unsigned int
+unifi_poll(struct file *filp, poll_table *wait)
+{
+    ul_client_t *pcli = (ul_client_t*)filp->private_data;
+    unsigned int mask = 0;
+    int ready;
+
+    func_enter();
+
+    ready = !list_empty(&pcli->udi_log);
+
+    poll_wait(filp, &pcli->udi_wq, wait);
+
+    if (ready) {
+        mask |= POLLIN | POLLRDNORM;    /* readable */
+    }
+
+    func_exit();
+
+    return mask;
+} /* unifi_poll() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  udi_set_log_filter
+ *
+ *      Configure the bit mask that determines which signal primitives are
+ *      passed to the logging process.
+ *
+ *  Arguments:
+ *      pcli            Pointer to the client to configure.
+ *      udi_filter      Pointer to a unifiio_filter_t containing instructions.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      SigGetFilterPos() returns a 32-bit value that contains an index and a
+ *      mask for accessing a signal_filter array. The top 16 bits specify an
+ *      index into a signal_filter, the bottom 16 bits specify a mask to
+ *      apply.
+ * ---------------------------------------------------------------------------
+ */
+static void
+udi_set_log_filter(ul_client_t *pcli, unifiio_filter_t *udi_filter)
+{
+    u32 filter_pos;
+    int i;
+
+    if (udi_filter->action == UfSigFil_AllOn)
+    {
+        for (i = 0; i < SIG_FILTER_SIZE; i++) {
+            pcli->signal_filter[i] = 0xFFFF;
+        }
+    }
+    else if (udi_filter->action == UfSigFil_AllOff)
+    {
+        for (i = 0; i < SIG_FILTER_SIZE; i++) {
+            pcli->signal_filter[i] = 0;
+        }
+    }
+    else if (udi_filter->action == UfSigFil_SelectOn)
+    {
+        for (i = 0; i < udi_filter->num_sig_ids; i++) {
+            filter_pos = SigGetFilterPos(udi_filter->sig_ids[i]);
+            if (filter_pos == 0xFFFFFFFF)
+            {
+                printk(KERN_WARNING
+                       "Unrecognised signal id (0x%X) specifed in logging filter\n",
+                       udi_filter->sig_ids[i]);
+            } else {
+                pcli->signal_filter[filter_pos >> 16] |= (filter_pos & 0xFFFF);
+            }
+        }
+    }
+    else if (udi_filter->action == UfSigFil_SelectOff)
+    {
+        for (i = 0; i < udi_filter->num_sig_ids; i++) {
+            filter_pos = SigGetFilterPos(udi_filter->sig_ids[i]);
+            if (filter_pos == 0xFFFFFFFF)
+            {
+                printk(KERN_WARNING
+                       "Unrecognised signal id (0x%X) specifed in logging filter\n",
+                       udi_filter->sig_ids[i]);
+            } else {
+                pcli->signal_filter[filter_pos >> 16] &= ~(filter_pos & 0xFFFF);
+            }
+        }
+    }
+
+} /* udi_set_log_filter() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  udi_log_event
+ *
+ *      Callback function to be registered as the UDI hook callback.
+ *      Copies the signal content into a new udi_log_t struct and adds
+ *      it to the read queue for this UDI client.
+ *
+ *  Arguments:
+ *      pcli            A pointer to the client instance.
+ *      signal          Pointer to the received signal.
+ *      signal_len      Size of the signal structure in bytes.
+ *      bulkdata        Pointers to any associated bulk data.
+ *      dir             Direction of the signal. Zero means from host,
+ *                      non-zero means to host.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+udi_log_event(ul_client_t *pcli,
+              const u8 *signal, int signal_len,
+              const bulk_data_param_t *bulkdata,
+              int dir)
+{
+    udi_log_t *logptr;
+    u8 *p;
+    int i;
+    int total_len;
+    udi_msg_t *msgptr;
+    u32 filter_pos;
+#ifdef OMNICLI_LINUX_EXTRA_LOG
+    static volatile unsigned int printk_cpu = UINT_MAX;
+    unsigned long long t;
+    unsigned long nanosec_rem;
+    unsigned long n_1000;
+#endif
+
+    func_enter();
+
+    /* Just a sanity check */
+    if ((signal == NULL) || (signal_len <= 0)) {
+        return;
+    }
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+    /* When HIP offline signal logging is enabled, omnicli cannot run */
+    if (log_hip_signals)
+    {
+        /* Add timestamp */
+        if (log_hip_signals & UNIFI_LOG_HIP_SIGNALS_FILTER_TIMESTAMP)
+        {
+            int timestamp = jiffies_to_msecs(jiffies);
+            unifi_debug_log_to_buf("T:");
+            unifi_debug_log_to_buf("%04X%04X ", *(((u16*)&timestamp) + 1),
+                                   *(u16*)&timestamp);
+        }
+
+        /* Add signal */
+        unifi_debug_log_to_buf("S%s:%04X R:%04X D:%04X ",
+                               dir ? "T" : "F",
+                               *(u16*)signal,
+                               *(u16*)(signal + 2),
+                               *(u16*)(signal + 4));
+        unifi_debug_hex_to_buf(signal + 6, signal_len - 6);
+
+        /* Add bulk data (assume 1 bulk data per signal) */
+        if ((log_hip_signals & UNIFI_LOG_HIP_SIGNALS_FILTER_BULKDATA) &&
+            (bulkdata->d[0].data_length > 0))
+        {
+            unifi_debug_log_to_buf("\nD:");
+            unifi_debug_hex_to_buf(bulkdata->d[0].os_data_ptr, bulkdata->d[0].data_length);
+        }
+        unifi_debug_log_to_buf("\n");
+
+        return;
+    }
+#endif
+
+#ifdef CSR_NATIVE_LINUX
+    uf_native_process_udi_signal(pcli, signal, signal_len, bulkdata, dir);
+#endif
+
+    /*
+     * Apply the logging filter - only report signals that have their
+     * bit set in the filter mask.
+     */
+    filter_pos = SigGetFilterPos(GET_SIGNAL_ID(signal));
+
+    if ((filter_pos != 0xFFFFFFFF) &&
+        ((pcli->signal_filter[filter_pos >> 16] & (filter_pos & 0xFFFF)) == 0))
+    {
+        /* Signal is not wanted by client */
+        return;
+    }
+
+
+    /* Calculate the buffer we need to store signal plus bulk data */
+    total_len = signal_len;
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+        total_len += bulkdata->d[i].data_length;
+    }
+
+    /* Allocate log structure plus actual signal. */
+    logptr = (udi_log_t *)kmalloc(sizeof(udi_log_t) + total_len, GFP_KERNEL);
+
+    if (logptr == NULL) {
+        printk(KERN_ERR
+               "Failed to allocate %lu bytes for a UDI log record\n",
+               (long unsigned int)(sizeof(udi_log_t) + total_len));
+        return;
+    }
+
+    /* Fill in udi_log struct */
+    INIT_LIST_HEAD(&logptr->q);
+    msgptr = &logptr->msg;
+    msgptr->length = sizeof(udi_msg_t) + total_len;
+#ifdef OMNICLI_LINUX_EXTRA_LOG
+    t = cpu_clock(printk_cpu);
+    nanosec_rem = do_div(t, 1000000000);
+    n_1000 = nanosec_rem/1000;
+    msgptr->timestamp = (t <<10 ) | ((unsigned long)(n_1000 >> 10) & 0x3ff);
+#else
+    msgptr->timestamp = jiffies_to_msecs(jiffies);
+#endif
+    msgptr->direction = dir;
+    msgptr->signal_length = signal_len;
+
+    /* Copy signal and bulk data to the log */
+    p = (u8 *)(msgptr + 1);
+    memcpy(p, signal, signal_len);
+    p += signal_len;
+
+    /* Append any bulk data */
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+        int len = bulkdata->d[i].data_length;
+
+        /*
+         * Len here might not be the same as the length in the bulk data slot.
+         * The slot length will always be even, but len could be odd.
+         */
+        if (len > 0) {
+            if (bulkdata->d[i].os_data_ptr) {
+                memcpy(p, bulkdata->d[i].os_data_ptr, len);
+            } else {
+                memset(p, 0, len);
+            }
+            p += len;
+        }
+    }
+
+    /* Add to tail of log queue */
+    if (down_interruptible(&pcli->udi_sem)) {
+        printk(KERN_WARNING "udi_log_event_q: Failed to get udi sem\n");
+        kfree(logptr);
+        func_exit();
+        return;
+    }
+    list_add_tail(&logptr->q, &pcli->udi_log);
+    up(&pcli->udi_sem);
+
+    /* Wake any waiting user process */
+    wake_up_interruptible(&pcli->udi_wq);
+
+    func_exit();
+} /* udi_log_event() */
+
+#ifdef CSR_SME_USERSPACE
+int
+uf_sme_queue_message(unifi_priv_t *priv, u8 *buffer, int length)
+{
+    udi_log_t *logptr;
+    udi_msg_t *msgptr;
+    u8 *p;
+
+    func_enter();
+
+    /* Just a sanity check */
+    if ((buffer == NULL) || (length <= 0)) {
+        return -EINVAL;
+    }
+
+    /* Allocate log structure plus actual signal. */
+    logptr = (udi_log_t *)kmalloc(sizeof(udi_log_t) + length, GFP_ATOMIC);
+    if (logptr == NULL) {
+        unifi_error(priv, "Failed to allocate %d bytes for an SME message\n",
+                    sizeof(udi_log_t) + length);
+                    kfree(buffer);
+                    return -ENOMEM;
+    }
+
+    /* Fill in udi_log struct */
+    INIT_LIST_HEAD(&logptr->q);
+    msgptr = &logptr->msg;
+    msgptr->length = sizeof(udi_msg_t) + length;
+    msgptr->signal_length = length;
+
+    /* Copy signal and bulk data to the log */
+    p = (u8 *)(msgptr + 1);
+    memcpy(p, buffer, length);
+
+    /* Add to tail of log queue */
+    down(&udi_mutex);
+    if (priv->sme_cli == NULL) {
+        kfree(logptr);
+        kfree(buffer);
+        up(&udi_mutex);
+        unifi_info(priv, "Message for the SME dropped, SME has gone away\n");
+        return 0;
+    }
+
+    down(&priv->sme_cli->udi_sem);
+    list_add_tail(&logptr->q, &priv->sme_cli->udi_log);
+    up(&priv->sme_cli->udi_sem);
+
+    /* Wake any waiting user process */
+    wake_up_interruptible(&priv->sme_cli->udi_wq);
+    up(&udi_mutex);
+
+    /* It is our responsibility to free the buffer allocated in build_packed_*() */
+    kfree(buffer);
+
+    func_exit();
+
+    return 0;
+
+} /* uf_sme_queue_message() */
+#endif
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
+    device_create(_class, _parent, _devno, _priv, _fmt, _args)
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
+    device_create_drvdata(_class, _parent, _devno, _priv, _fmt, _args)
+#else
+#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
+    device_create(_class, _parent, _devno, _fmt, _args)
+#endif
+
+/*
+ ****************************************************************************
+ *
+ *      Driver instantiation
+ *
+ ****************************************************************************
+ */
+static struct file_operations unifi_fops = {
+    .owner      = THIS_MODULE,
+    .open       = unifi_open,
+    .release    = unifi_release,
+    .read       = unifi_read,
+    .write      = unifi_write,
+    .unlocked_ioctl = unifi_ioctl,
+    .poll       = unifi_poll,
+};
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
+    device_create(_class, _parent, _devno, _priv, _fmt, _args)
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
+    device_create_drvdata(_class, _parent, _devno, _priv, _fmt, _args)
+#else
+#define UF_DEVICE_CREATE(_class, _parent, _devno, _priv, _fmt, _args)       \
+    device_create(_class, _parent, _devno, _fmt, _args)
+#endif
+
+static dev_t unifi_first_devno;
+static struct class *unifi_class;
+
+
+int uf_create_device_nodes(unifi_priv_t *priv, int bus_id)
+{
+    dev_t devno;
+    int r;
+
+    cdev_init(&priv->unifi_cdev, &unifi_fops);
+
+    /* cdev_init() should set the cdev owner, but it does not */
+    priv->unifi_cdev.owner = THIS_MODULE;
+
+    devno = MKDEV(MAJOR(unifi_first_devno),
+                  MINOR(unifi_first_devno) + (bus_id * 2));
+    r = cdev_add(&priv->unifi_cdev, devno, 1);
+    if (r) {
+        return r;
+    }
+
+#ifdef SDIO_EXPORTS_STRUCT_DEVICE
+    if (!UF_DEVICE_CREATE(unifi_class, priv->unifi_device,
+                          devno, priv, "unifi%d", bus_id)) {
+#else
+    priv->unifi_device = UF_DEVICE_CREATE(unifi_class, NULL,
+                                          devno, priv, "unifi%d", bus_id);
+    if (priv->unifi_device == NULL) {
+#endif /* SDIO_EXPORTS_STRUCT_DEVICE */
+
+        cdev_del(&priv->unifi_cdev);
+        return -EINVAL;
+    }
+
+    cdev_init(&priv->unifiudi_cdev, &unifi_fops);
+
+    /* cdev_init() should set the cdev owner, but it does not */
+    priv->unifiudi_cdev.owner = THIS_MODULE;
+
+    devno = MKDEV(MAJOR(unifi_first_devno),
+                  MINOR(unifi_first_devno) + (bus_id * 2) + 1);
+    r = cdev_add(&priv->unifiudi_cdev, devno, 1);
+    if (r) {
+        device_destroy(unifi_class, priv->unifi_cdev.dev);
+        cdev_del(&priv->unifi_cdev);
+        return r;
+    }
+
+    if (!UF_DEVICE_CREATE(unifi_class,
+#ifdef SDIO_EXPORTS_STRUCT_DEVICE
+                          priv->unifi_device,
+#else
+                          NULL,
+#endif /* SDIO_EXPORTS_STRUCT_DEVICE */
+                          devno, priv, "unifiudi%d", bus_id)) {
+        device_destroy(unifi_class, priv->unifi_cdev.dev);
+        cdev_del(&priv->unifiudi_cdev);
+        cdev_del(&priv->unifi_cdev);
+        return -EINVAL;
+    }
+
+    return 0;
+}
+
+
+void uf_destroy_device_nodes(unifi_priv_t *priv)
+{
+    device_destroy(unifi_class, priv->unifiudi_cdev.dev);
+    device_destroy(unifi_class, priv->unifi_cdev.dev);
+    cdev_del(&priv->unifiudi_cdev);
+    cdev_del(&priv->unifi_cdev);
+}
+
+
+
+/*
+ * ----------------------------------------------------------------
+ *  uf_create_debug_device
+ *
+ *      Allocates device numbers for unifi character device nodes
+ *      and creates a unifi class in sysfs
+ *
+ * Arguments:
+ *  fops          Pointer to the char device operations structure.
+ *
+ * Returns:
+ *      0 on success, -ve error code on error.
+ * ----------------------------------------------------------------
+ */
+static int
+uf_create_debug_device(struct file_operations *fops)
+{
+    int ret;
+
+    /* Allocate two device numbers for each device. */
+    ret = alloc_chrdev_region(&unifi_first_devno, 0, MAX_UNIFI_DEVS*2, UNIFI_NAME);
+    if (ret) {
+        unifi_error(NULL, "Failed to add alloc dev numbers: %d\n", ret);
+        return ret;
+    }
+
+    /* Create a UniFi class */
+    unifi_class = class_create(THIS_MODULE, UNIFI_NAME);
+    if (IS_ERR(unifi_class)) {
+        unifi_error(NULL, "Failed to create UniFi class\n");
+
+        /* Release device numbers */
+        unregister_chrdev_region(unifi_first_devno, MAX_UNIFI_DEVS*2);
+        unifi_first_devno = 0;
+        return -EINVAL;
+    }
+
+    return 0;
+} /* uf_create_debug_device() */
+
+
+/*
+ * ----------------------------------------------------------------
+ *  uf_remove_debug_device
+ *
+ *      Destroys the unifi class and releases the allocated
+ *      device numbers for unifi character device nodes.
+ *
+ * Arguments:
+ *
+ * Returns:
+ * ----------------------------------------------------------------
+ */
+static void
+uf_remove_debug_device(void)
+{
+    /* Destroy the UniFi class */
+    class_destroy(unifi_class);
+
+    /* Release device numbers */
+    unregister_chrdev_region(unifi_first_devno, MAX_UNIFI_DEVS*2);
+    unifi_first_devno = 0;
+
+} /* uf_remove_debug_device() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *      Module loading.
+ *
+ * ---------------------------------------------------------------------------
+ */
+int __init
+unifi_load(void)
+{
+    int r;
+
+    printk("UniFi SDIO Driver: %s %s %s\n",
+            CSR_WIFI_VERSION,
+           __DATE__, __TIME__);
+
+#ifdef CSR_SME_USERSPACE
+#ifdef CSR_SUPPORT_WEXT
+    printk("CSR SME with WEXT support\n");
+#else
+    printk("CSR SME no WEXT support\n");
+#endif /* CSR_SUPPORT_WEXT */
+#endif /* CSR_SME_USERSPACE */
+
+#ifdef CSR_NATIVE_LINUX
+#ifdef CSR_SUPPORT_WEXT
+#error WEXT unsupported in the native driver
+#endif
+    printk("CSR native no WEXT support\n");
+#endif
+#ifdef CSR_WIFI_SPLIT_PATCH
+    printk("Split patch support\n");
+#endif
+    printk("Kernel %d.%d.%d\n",
+           ((LINUX_VERSION_CODE) >> 16) & 0xff,
+           ((LINUX_VERSION_CODE) >> 8) & 0xff,
+           (LINUX_VERSION_CODE) & 0xff);
+    /*
+     * Instantiate the /dev/unifi* device nodes.
+     * We must do this before registering with the SDIO driver because it
+     * will immediately call the "insert" callback if the card is
+     * already present.
+     */
+    r = uf_create_debug_device(&unifi_fops);
+    if (r) {
+        return r;
+    }
+
+    /* Now register with the SDIO driver */
+    r = uf_sdio_load();
+    if (r) {
+        uf_remove_debug_device();
+        return r;
+    }
+
+    if (sdio_block_size > -1) {
+        unifi_info(NULL, "sdio_block_size %d\n", sdio_block_size);
+    }
+
+    if (sdio_byte_mode) {
+        unifi_info(NULL, "sdio_byte_mode\n");
+    }
+
+    if (disable_power_control) {
+        unifi_info(NULL, "disable_power_control\n");
+    }
+
+    if (disable_hw_reset) {
+        unifi_info(NULL, "disable_hw_reset\n");
+    }
+
+    if (enable_wol) {
+        unifi_info(NULL, "enable_wol %d\n", enable_wol);
+    }
+
+    if (run_bh_once != -1) {
+        unifi_info(NULL, "run_bh_once %d\n", run_bh_once);
+    }
+
+    return 0;
+} /* unifi_load() */
+
+
+void __exit
+unifi_unload(void)
+{
+    /* The SDIO remove hook will call unifi_disconnect(). */
+    uf_sdio_unload();
+
+    uf_remove_debug_device();
+
+} /* unifi_unload() */
+
+module_init(unifi_load);
+module_exit(unifi_unload);
+
+MODULE_DESCRIPTION("UniFi Device driver");
+MODULE_AUTHOR("Cambridge Silicon Radio Ltd.");
+MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/staging/csr/firmware.c b/drivers/staging/csr/firmware.c
new file mode 100644
index 00000000000..d14e1183961
--- /dev/null
+++ b/drivers/staging/csr/firmware.c
@@ -0,0 +1,413 @@
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     firmware.c
+ *
+ *  PURPOSE:
+ *      Implements the f/w related HIP core lib API.
+ *      It is part of the porting exercise in Linux.
+ *
+ *      Also, it contains example code for reading the loader and f/w files
+ *      from the userspace and starting the SME in Linux.
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/kmod.h>
+#include <linux/vmalloc.h>
+#include <linux/firmware.h>
+#include <asm/uaccess.h>
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_unifi_udi.h"
+#include "unifiio.h"
+#include "unifi_priv.h"
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *      F/W download. Part of the HIP core API
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_fw_read_start
+ *
+ *      Returns a structure to be passed in unifi_fw_read().
+ *      This structure is an OS specific description of the f/w file.
+ *      In the linux implementation it is a buffer with the f/w and its' length.
+ *      The HIP driver calls this functions to request for the loader or
+ *      the firmware file.
+ *      The structure pointer can be freed when unifi_fw_read_stop() is called.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to driver context.
+ *      is_fw           Type of firmware to retrieve
+ *      info            Versions information. Can be used to determine
+ *                      the appropriate f/w file to load.
+ *
+ *  Returns:
+ *      O on success, non-zero otherwise.
+ *
+ * ---------------------------------------------------------------------------
+ */
+void*
+unifi_fw_read_start(void *ospriv, s8 is_fw, const card_info_t *info)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    CSR_UNUSED(info);
+
+    func_enter();
+
+    if (is_fw == UNIFI_FW_STA) {
+        /* F/w may have been released after a previous successful download. */
+        if (priv->fw_sta.dl_data == NULL) {
+            unifi_trace(priv, UDBG2, "Attempt reload of sta f/w\n");
+            uf_request_firmware_files(priv, UNIFI_FW_STA);
+        }
+        /* Set up callback struct for readfunc() */
+        if (priv->fw_sta.dl_data != NULL) {
+            func_exit();
+            return &priv->fw_sta;
+        }
+
+    } else {
+        unifi_error(priv, "downloading firmware... unknown request: %d\n", is_fw);
+    }
+
+    func_exit();
+    return NULL;
+} /* unifi_fw_read_start() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_fw_read_stop
+ *
+ *      Called when the HIP driver has finished using the loader or
+ *      the firmware file.
+ *      The firmware buffer may be released now.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to driver context.
+ *      dlpriv          The pointer returned by unifi_fw_read_start()
+ *
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_fw_read_stop(void *ospriv, void *dlpriv)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    struct dlpriv *dl_struct = (struct dlpriv *)dlpriv;
+    func_enter();
+
+    if (dl_struct != NULL) {
+        if (dl_struct->dl_data != NULL) {
+            unifi_trace(priv, UDBG2, "Release f/w buffer %p, %d bytes\n",
+                        dl_struct->dl_data, dl_struct->dl_len);
+        }
+        uf_release_firmware(priv, dl_struct);
+    }
+
+    func_exit();
+} /* unifi_fw_read_stop() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_fw_open_buffer
+ *
+ *  Returns a handle for a buffer dynamically allocated by the driver,
+ *  e.g. into which a firmware file may have been converted from another format
+ *  which is the case with some production test images.
+ *
+ *  The handle may then be used by unifi_fw_read() to access the contents of
+ *  the buffer.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to driver context.
+ *      fwbuf           Buffer containing firmware image
+ *      len             Length of buffer in bytes
+ *
+ *  Returns
+ *      Handle for buffer, or NULL on error
+ * ---------------------------------------------------------------------------
+ */
+void *
+unifi_fw_open_buffer(void *ospriv, void *fwbuf, u32 len)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    func_enter();
+
+    if (fwbuf == NULL) {
+        func_exit();
+        return NULL;
+    }
+    priv->fw_conv.dl_data = fwbuf;
+    priv->fw_conv.dl_len = len;
+    priv->fw_conv.fw_desc = NULL;   /* No OS f/w resource is associated */
+
+    func_exit();
+    return &priv->fw_conv;
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_fw_close_buffer
+ *
+ *  Releases any handle for a buffer dynamically allocated by the driver,
+ *  e.g. into which a firmware file may have been converted from another format
+ *  which is the case with some production test images.
+ *
+ *
+ *  Arguments:
+ *      ospriv          Pointer to driver context.
+ *      fwbuf           Buffer containing firmware image
+ *
+ *  Returns
+ *      Handle for buffer, or NULL on error
+ * ---------------------------------------------------------------------------
+ */
+void unifi_fw_close_buffer(void *ospriv, void *fwbuf)
+{
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_fw_read
+ *
+ *      The HIP driver calls this function to ask for a part of the loader or
+ *      the firmware file.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to driver context.
+ *      arg             The pointer returned by unifi_fw_read_start().
+ *      offset          The offset in the file to return from.
+ *      buf             A buffer to store the requested data.
+ *      len             The size of the buf and the size of the requested data.
+ *
+ *  Returns
+ *      The number of bytes read from the firmware image, or -ve on error
+ * ---------------------------------------------------------------------------
+ */
+s32
+unifi_fw_read(void *ospriv, void *arg, u32 offset, void *buf, u32 len)
+{
+    const struct dlpriv *dlpriv = arg;
+
+    if (offset >= dlpriv->dl_len) {
+        /* at end of file */
+        return 0;
+    }
+
+    if ((offset + len) > dlpriv->dl_len) {
+        /* attempt to read past end of file */
+        return -1;
+    }
+
+    memcpy(buf, dlpriv->dl_data+offset, len);
+
+    return len;
+
+} /* unifi_fw_read() */
+
+
+
+
+#define UNIFIHELPER_INIT_MODE_SMEUSER   2
+#define UNIFIHELPER_INIT_MODE_NATIVE    1
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_run_unifihelper
+ *
+ *      Ask userspace to send us firmware for download by running
+ *      '/usr/sbin/unififw'.
+ *      The same script starts the SME userspace application.
+ *      Derived from net_run_sbin_hotplug().
+ *
+ *  Arguments:
+ *      priv            Pointer to OS private struct.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_run_unifihelper(unifi_priv_t *priv)
+{
+#ifdef CONFIG_HOTPLUG
+
+#ifdef ANDROID_BUILD
+    char *prog = "/system/bin/unififw";
+#else
+    char *prog = "/usr/sbin/unififw";
+#endif /* ANDROID_BUILD */
+
+    char *argv[6], *envp[4];
+    char inst_str[8];
+    char init_mode[8];
+    int i, r;
+
+#if (defined CSR_SME_USERSPACE) && (!defined CSR_SUPPORT_WEXT)
+    unifi_trace(priv, UDBG1, "SME userspace build: run unifi_helper manually\n");
+    return 0;
+#endif
+
+    unifi_trace(priv, UDBG1, "starting %s\n", prog);
+
+    snprintf(inst_str,   8, "%d", priv->instance);
+#if (defined CSR_SME_USERSPACE)
+    snprintf(init_mode, 8, "%d", UNIFIHELPER_INIT_MODE_SMEUSER);
+#else
+    snprintf(init_mode, 8, "%d", UNIFIHELPER_INIT_MODE_NATIVE);
+#endif /* CSR_SME_USERSPACE */
+
+    i = 0;
+    argv[i++] = prog;
+    argv[i++] = inst_str;
+    argv[i++] = init_mode;
+    argv[i++] = 0;
+    argv[i] = 0;
+    /* Don't add more args without making argv bigger */
+
+    /* minimal command environment */
+    i = 0;
+    envp[i++] = "HOME=/";
+    envp[i++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
+    envp[i] = 0;
+    /* Don't add more without making envp bigger */
+
+    unifi_trace(priv, UDBG2, "running %s %s %s\n", argv[0], argv[1], argv[2]);
+
+    r = call_usermodehelper(argv[0], argv, envp, 0);
+
+    return r;
+#else
+    unifi_trace(priv, UDBG1, "Can't automatically download firmware because kernel does not have HOTPLUG\n");
+    return -1;
+#endif
+} /* uf_run_unifihelper() */
+
+#ifdef CSR_WIFI_SPLIT_PATCH
+static u8 is_ap_mode(unifi_priv_t *priv)
+{
+    if (priv == NULL || priv->interfacePriv[0] == NULL)
+    {
+        return FALSE;
+    }
+
+    /* Test for mode requiring AP patch */
+    return(CSR_WIFI_HIP_IS_AP_FW(priv->interfacePriv[0]->interfaceMode));
+}
+#endif
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_request_firmware_files
+ *
+ *      Get the firmware files from userspace.
+ *
+ *  Arguments:
+ *      priv            Pointer to OS private struct.
+ *      is_fw           type of firmware to load (UNIFI_FW_STA/LOADER)
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+int uf_request_firmware_files(unifi_priv_t *priv, int is_fw)
+{
+    /* uses the default method to get the firmware */
+    const struct firmware *fw_entry;
+    int postfix;
+#define UNIFI_MAX_FW_PATH_LEN       32
+    char fw_name[UNIFI_MAX_FW_PATH_LEN];
+    int r;
+
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+    if (priv->mib_data.length) {
+        vfree(priv->mib_data.data);
+        priv->mib_data.data = NULL;
+        priv->mib_data.length = 0;
+    }
+#endif /* CSR_SUPPORT_SME && CSR_SUPPORT_WEXT*/
+
+    postfix = priv->instance;
+
+    if (is_fw == UNIFI_FW_STA) {
+        /* Free kernel buffer and reload */
+        uf_release_firmware(priv, &priv->fw_sta);
+#ifdef CSR_WIFI_SPLIT_PATCH
+        scnprintf(fw_name, UNIFI_MAX_FW_PATH_LEN, "unifi-sdio-%d/%s",
+                  postfix, (is_ap_mode(priv) ? "ap.xbv" : "staonly.xbv") );
+#else
+        scnprintf(fw_name, UNIFI_MAX_FW_PATH_LEN, "unifi-sdio-%d/%s",
+                  postfix, "sta.xbv" );
+#endif
+        r = request_firmware(&fw_entry, fw_name, priv->unifi_device);
+        if (r == 0) {
+            priv->fw_sta.dl_data = fw_entry->data;
+            priv->fw_sta.dl_len = fw_entry->size;
+            priv->fw_sta.fw_desc = (void *)fw_entry;
+        } else {
+            unifi_trace(priv, UDBG2, "Firmware file not available\n");
+        }
+    }
+
+    return 0;
+
+} /* uf_request_firmware_files() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_release_firmware_files
+ *
+ *      Release all buffers used to store firmware files
+ *
+ *  Arguments:
+ *      priv            Pointer to OS private struct.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+int uf_release_firmware_files(unifi_priv_t *priv)
+{
+    uf_release_firmware(priv, &priv->fw_sta);
+
+    return 0;
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_release_firmware
+ *
+ *      Release specific buffer used to store firmware
+ *
+ *  Arguments:
+ *      priv            Pointer to OS private struct.
+ *      to_free         Pointer to specific buffer to release
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+int uf_release_firmware(unifi_priv_t *priv, struct dlpriv *to_free)
+{
+    if (to_free != NULL) {
+        if (to_free->fw_desc != NULL) {
+            release_firmware((const struct firmware *)to_free->fw_desc);
+        }
+        to_free->fw_desc = NULL;
+        to_free->dl_data = NULL;
+        to_free->dl_len = 0;
+    }
+    return 0;
+}
diff --git a/drivers/staging/csr/inet.c b/drivers/staging/csr/inet.c
new file mode 100644
index 00000000000..b4acb54ecc6
--- /dev/null
+++ b/drivers/staging/csr/inet.c
@@ -0,0 +1,106 @@
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     inet.c
+ *
+ *  PURPOSE:
+ *      Routines related to IP address changes.
+ *      Optional part of the porting exercise. It uses system network
+ *      handlers to obtain the UniFi IP address and pass it to the SME
+ *      using the unifi_sys_ip_configured_ind().
+ *
+ * Copyright (C) 2008-2009 Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/inetdevice.h>
+#include <linux/notifier.h>
+
+#include "unifi_priv.h"
+#include "csr_wifi_hip_conversions.h"
+
+/*
+ * The inet notifier is global and not per-netdev.  To avoid having a
+ * notifier registered when there are no unifi devices present, it's
+ * registered after the first unifi network device is registered, and
+ * unregistered when the last unifi network device is unregistered.
+ */
+
+static atomic_t inet_notif_refs = ATOMIC_INIT(0);
+
+static int uf_inetaddr_event(struct notifier_block *notif, unsigned long event, void *ifa)
+{
+    struct net_device *ndev;
+    unifi_priv_t *priv;
+    struct in_ifaddr *if_addr;
+    netInterface_priv_t *InterfacePriv = (netInterface_priv_t *)NULL;
+
+    if (!ifa || !((struct in_ifaddr *)ifa)->ifa_dev) {
+        unifi_trace(NULL, UDBG1, "uf_inetaddr_event (%lu) ifa=%p\n", event, ifa);
+        return NOTIFY_DONE;
+    }
+
+    ndev = ((struct in_ifaddr *)ifa)->ifa_dev->dev;
+    InterfacePriv = (netInterface_priv_t*) netdev_priv(ndev);
+
+    /* As the notifier is global, the call may be for a non-UniFi netdev.
+     * Therefore check the netdev_priv to make sure it's a known UniFi one.
+     */
+    if (uf_find_netdev_priv(InterfacePriv) == -1) {
+        unifi_trace(NULL, UDBG1, "uf_inetaddr_event (%lu) ndev=%p, other netdev_priv=%p\n",
+                    event, ndev, InterfacePriv);
+        return NOTIFY_DONE;
+    }
+
+    if (!InterfacePriv->privPtr) {
+        unifi_error(NULL, "uf_inetaddr_event null priv (%lu) ndev=%p, InterfacePriv=%p\n",
+                    event, ndev, InterfacePriv);
+        return NOTIFY_DONE;
+    }
+
+    priv = InterfacePriv->privPtr;
+    if_addr = (struct in_ifaddr *)ifa;
+
+    /* If this event is for a UniFi device, notify the SME that an IP
+     * address has been added or removed. */
+    if (uf_find_priv(priv) != -1) {
+        switch (event) {
+            case NETDEV_UP:
+                unifi_info(priv, "IP address assigned for %s\n", priv->netdev[InterfacePriv->InterfaceTag]->name);
+                priv->sta_ip_address = if_addr->ifa_address;
+#ifdef CSR_SUPPORT_WEXT
+                sme_mgt_packet_filter_set(priv);
+#endif
+                break;
+            case NETDEV_DOWN:
+                unifi_info(priv, "IP address removed for %s\n", priv->netdev[InterfacePriv->InterfaceTag]->name);
+                priv->sta_ip_address = 0xFFFFFFFF;
+#ifdef CSR_SUPPORT_WEXT
+                sme_mgt_packet_filter_set(priv);
+#endif
+                break;
+        }
+    }
+
+    return NOTIFY_DONE;
+}
+
+static struct notifier_block uf_inetaddr_notifier = {
+    .notifier_call = uf_inetaddr_event,
+};
+
+void uf_register_inet_notifier(void)
+{
+    if (atomic_inc_return(&inet_notif_refs) == 1) {
+        register_inetaddr_notifier(&uf_inetaddr_notifier);
+    }
+}
+
+void uf_unregister_inet_notifier(void)
+{
+    if (atomic_dec_return(&inet_notif_refs) == 0) {
+        unregister_inetaddr_notifier(&uf_inetaddr_notifier);
+    }
+}
diff --git a/drivers/staging/csr/init_hw.c b/drivers/staging/csr/init_hw.c
new file mode 100644
index 00000000000..3b8a4babf9a
--- /dev/null
+++ b/drivers/staging/csr/init_hw.c
@@ -0,0 +1,108 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     init_hw.c
+ *
+ * PURPOSE:
+ *      Use the HIP core lib to initialise the UniFi chip.
+ *      It is part of the porting exercise in Linux.
+ *
+ * Copyright (C) 2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "unifi_priv.h"
+
+
+#define MAX_INIT_ATTEMPTS        4
+
+extern int led_mask;
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_init_hw
+ *
+ *      Resets hardware, downloads and initialises f/w.
+ *      This function demonstrates how to use the HIP core lib API
+ *      to implement the SME unifi_sys_wifi_on_req() part of the SYS API.
+ *
+ *      In a simple implementation, all this function needs to do is call
+ *      unifi_init_card() and then unifi_card_info().
+ *      In the Linux implementation, it will retry to initialise UniFi or
+ *      try to debug the reasons if unifi_init_card() returns an error.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to OS driver structure for the device.
+ *
+ *  Returns:
+ *      O on success, non-zero otherwise.
+ *
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_init_hw(unifi_priv_t *priv)
+{
+    int attempts = 0;
+    int priv_instance;
+    CsrResult csrResult = CSR_RESULT_FAILURE;
+
+    priv_instance = uf_find_priv(priv);
+    if (priv_instance == -1) {
+        unifi_warning(priv, "uf_init_hw: Unknown priv instance, will use fw_init[0]\n");
+        priv_instance = 0;
+    }
+
+    while (1) {
+        if (attempts > MAX_INIT_ATTEMPTS) {
+            unifi_error(priv, "Failed to initialise UniFi after %d attempts, "
+                        "giving up.\n",
+                        attempts);
+            break;
+        }
+        attempts++;
+
+        unifi_info(priv, "Initialising UniFi, attempt %d\n", attempts);
+
+        if (fw_init[priv_instance] > 0) {
+            unifi_notice(priv, "f/w init prevented by module parameter\n");
+            break;
+        } else if (fw_init[priv_instance] == 0) {
+            fw_init[priv_instance] ++;
+        }
+
+        /*
+         * Initialise driver core. This will perform a reset of UniFi
+         * internals, but not the SDIO CCCR.
+         */
+        CsrSdioClaim(priv->sdio);
+        csrResult = unifi_init_card(priv->card, led_mask);
+        CsrSdioRelease(priv->sdio);
+
+        if (csrResult == CSR_WIFI_HIP_RESULT_NO_DEVICE) {
+            return CsrHipResultToStatus(csrResult);
+        }
+        if (csrResult == CSR_WIFI_HIP_RESULT_NOT_FOUND) {
+            unifi_error(priv, "Firmware file required, but not found.\n");
+            return CsrHipResultToStatus(csrResult);
+        }
+        if (csrResult != CSR_RESULT_SUCCESS) {
+            /* failed. Reset h/w and try again */
+            unifi_error(priv, "Failed to initialise UniFi chip.\n");
+            continue;
+        }
+
+        /* Get the version information from the lib_hip */
+        unifi_card_info(priv->card, &priv->card_info);
+
+        return CsrHipResultToStatus(csrResult);
+    }
+
+    return CsrHipResultToStatus(csrResult);
+
+} /* uf_init_hw */
+
+
diff --git a/drivers/staging/csr/io.c b/drivers/staging/csr/io.c
new file mode 100644
index 00000000000..e6503d9620a
--- /dev/null
+++ b/drivers/staging/csr/io.c
@@ -0,0 +1,1166 @@
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     io.c
+ *
+ *  PURPOSE:
+ *      This file contains routines that the SDIO driver can call when a
+ *      UniFi card is first inserted (or detected) and removed.
+ *
+ *      When used with sdioemb, the udev scripts (at least on Ubuntu) don't
+ *      recognise a UniFi being added to the system. This is because sdioemb
+ *      does not register itself as a device_driver, it uses it's own code
+ *      to handle insert and remove.
+ *      To have Ubuntu recognise UniFi, edit /etc/udev/rules.d/85-ifupdown.rules
+ *      to change this line:
+ *          SUBSYSTEM=="net", DRIVERS=="?*", GOTO="net_start"
+ *      to these:
+ *          #SUBSYSTEM=="net", DRIVERS=="?*", GOTO="net_start"
+ *          SUBSYSTEM=="net", GOTO="net_start"
+ *
+ *      Then you can add a stanza to /etc/network/interfaces like this:
+ *          auto eth1
+ *          iface eth1 inet dhcp
+ *          wpa-conf /etc/wpa_supplicant.conf
+ *      This will then automatically associate when a car dis inserted.
+ *
+ * Copyright (C) 2006-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/proc_fs.h>
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_unifiversion.h"
+#include "csr_wifi_hip_unifi_udi.h"   /* for unifi_print_status() */
+#include "unifiio.h"
+#include "unifi_priv.h"
+
+
+/*
+ * Array of pointers to context structs for unifi devices that are present.
+ * The index in the array corresponds to the wlan interface number
+ * (if "wlan*" is used). If "eth*" is used, the eth* numbers are allocated
+ * after any Ethernet cards.
+ *
+ * The Arasan PCI-SDIO controller card supported by this driver has 2 slots,
+ * hence a max of 2 devices.
+ */
+static unifi_priv_t *Unifi_instances[MAX_UNIFI_DEVS];
+
+/* Array of pointers to netdev objects used by the UniFi driver, as there
+ * are now many per instance. This is used to determine which netdev events
+ * are for UniFi as opposed to other net interfaces.
+ */
+static netInterface_priv_t *Unifi_netdev_instances[MAX_UNIFI_DEVS * CSR_WIFI_NUM_INTERFACES];
+
+/*
+ * Array to hold the status of each unifi device in each slot.
+ * We only process an insert event when In_use[] for the slot is
+ * UNIFI_DEV_NOT_IN_USE. Otherwise, it means that the slot is in use or
+ * we are in the middle of a cleanup (the action on unplug).
+ */
+#define UNIFI_DEV_NOT_IN_USE    0
+#define UNIFI_DEV_IN_USE        1
+#define UNIFI_DEV_CLEANUP       2
+static int In_use[MAX_UNIFI_DEVS];
+/*
+ * Mutex to prevent UDI clients to open the character device before the priv
+ * is created and initialised.
+ */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+DEFINE_SEMAPHORE(Unifi_instance_mutex);
+#else
+DECLARE_MUTEX(Unifi_instance_mutex);
+#endif
+/*
+ * When the device is removed, unregister waits on Unifi_cleanup_wq
+ * until all the UDI clients release the character device.
+ */
+DECLARE_WAIT_QUEUE_HEAD(Unifi_cleanup_wq);
+
+
+static int uf_read_proc(char *page, char **start, off_t offset, int count,
+                        int *eof, void *data);
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+
+static CsrResult signal_buffer_init(unifi_priv_t * priv, int size)
+{
+    int i;
+    func_enter();
+
+    priv->rxSignalBuffer.writePointer =
+    priv->rxSignalBuffer.readPointer = 0;
+    priv->rxSignalBuffer.size = size;
+    /* Allocating Memory for Signal primitive pointer */
+    for(i=0; i<size; i++)
+    {
+         priv->rxSignalBuffer.rx_buff[i].sig_len=0;
+         priv->rxSignalBuffer.rx_buff[i].bufptr = kmalloc(UNIFI_PACKED_SIGBUF_SIZE, GFP_KERNEL);
+         if (priv->rxSignalBuffer.rx_buff[i].bufptr == NULL)
+         {
+             int j;
+             unifi_error(priv,"signal_buffer_init:Failed to Allocate shared memory for T-H signals \n");
+             for(j=0;j<i;j++)
+             {
+                 priv->rxSignalBuffer.rx_buff[j].sig_len=0;
+                 kfree(priv->rxSignalBuffer.rx_buff[j].bufptr);
+                 priv->rxSignalBuffer.rx_buff[j].bufptr = NULL;
+             }
+             func_exit();
+             return -1;
+         }
+    }
+    func_exit();
+    return 0;
+}
+
+
+static void signal_buffer_free(unifi_priv_t * priv, int size)
+{
+    int i;
+
+    for(i=0; i<size; i++)
+    {
+         priv->rxSignalBuffer.rx_buff[i].sig_len=0;
+         kfree(priv->rxSignalBuffer.rx_buff[i].bufptr);
+         priv->rxSignalBuffer.rx_buff[i].bufptr = NULL;
+    }
+}
+#endif
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_register_netdev
+ *
+ *      Registers the network interface, installes the qdisc,
+ *      and registers the inet handler.
+ *      In the porting exercise, register the driver to the network
+ *      stack if necessary.
+ *
+ *  Arguments:
+ *      priv          Pointer to driver context.
+ *
+ *  Returns:
+ *      O on success, non-zero otherwise.
+ *
+ *  Notes:
+ *      We will only unregister when the card is ejected, so we must
+ *      only do it once.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_register_netdev(unifi_priv_t *priv, int interfaceTag)
+{
+    int r;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_register_netdev bad interfaceTag\n");
+        return -EINVAL;
+    }
+
+    /*
+     * Allocates a device number and registers device with the network
+     * stack.
+     */
+    unifi_trace(priv, UDBG5, "uf_register_netdev: netdev %d - 0x%p\n",
+            interfaceTag, priv->netdev[interfaceTag]);
+    r = register_netdev(priv->netdev[interfaceTag]);
+    if (r) {
+        unifi_error(priv, "Failed to register net device\n");
+        return -EINVAL;
+    }
+
+    /* The device is registed */
+    interfacePriv->netdev_registered = 1;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+#ifdef CONFIG_NET_SCHED
+    /*
+     * IMPORTANT:
+     * uf_install_qdisc() holds the network device lock, we can not
+     * install the qdisk before the network device is registered.
+     */
+    r = uf_install_qdisc(priv->netdev[interfaceTag]);
+    if (r) {
+        unifi_error(priv, "Failed to install qdisc\n");
+        return r;
+    }
+#endif /* CONFIG_NET_SCHED */
+#endif /* LINUX_VERSION_CODE */
+
+#ifdef CSR_SUPPORT_SME
+    /*
+     * Register the inet handler; it notifies us for changes in the IP address.
+     */
+    uf_register_inet_notifier();
+#endif /* CSR_SUPPORT_SME */
+
+    unifi_notice(priv, "unifi%d is %s\n",
+            priv->instance, priv->netdev[interfaceTag]->name);
+
+    return 0;
+} /* uf_register_netdev */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_unregister_netdev
+ *
+ *      Unregisters the network interface and the inet handler.
+ *
+ *  Arguments:
+ *      priv          Pointer to driver context.
+ *
+ *  Returns:
+ *      None.
+ *
+ * ---------------------------------------------------------------------------
+ */
+void
+uf_unregister_netdev(unifi_priv_t *priv)
+{
+    int i=0;
+
+#ifdef CSR_SUPPORT_SME
+    /* Unregister the inet handler... */
+    uf_unregister_inet_notifier();
+#endif /* CSR_SUPPORT_SME */
+
+    for (i=0; i<CSR_WIFI_NUM_INTERFACES; i++) {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+        if (interfacePriv->netdev_registered) {
+            unifi_trace(priv, UDBG5,
+                    "uf_unregister_netdev: netdev %d - 0x%p\n",
+                    i, priv->netdev[i]);
+
+            /* ... and the netdev */
+            unregister_netdev(priv->netdev[i]);
+            interfacePriv->netdev_registered = 0;
+        }
+
+        interfacePriv->interfaceMode = 0;
+
+        /* Enable all queues by default */
+        interfacePriv->queueEnabled[0] = 1;
+        interfacePriv->queueEnabled[1] = 1;
+        interfacePriv->queueEnabled[2] = 1;
+        interfacePriv->queueEnabled[3] = 1;
+    }
+
+    priv->totalInterfaceCount = 0;
+} /* uf_unregister_netdev() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  register_unifi_sdio
+ *
+ *      This function is called from the Probe (or equivalent) method of
+ *      the SDIO driver when a UniFi card is detected.
+ *      We allocate the Linux net_device struct, initialise the HIP core
+ *      lib, create the char device nodes and start the userspace helper
+ *      to initialise the device.
+ *
+ *  Arguments:
+ *      sdio_dev        Pointer to SDIO context handle to use for all
+ *                      SDIO ops.
+ *      bus_id          A small number indicating the SDIO card position on the
+ *                      bus. Typically this is the slot number, e.g. 0, 1 etc.
+ *                      Valid values are 0 to MAX_UNIFI_DEVS-1.
+ *      dev             Pointer to kernel device manager struct.
+ *
+ *  Returns:
+ *      Pointer to the unifi instance, or NULL on error.
+ * ---------------------------------------------------------------------------
+ */
+static unifi_priv_t *
+register_unifi_sdio(CsrSdioFunction *sdio_dev, int bus_id, struct device *dev)
+{
+    unifi_priv_t *priv = NULL;
+    int r = -1;
+    CsrResult csrResult;
+
+    func_enter();
+
+    if ((bus_id < 0) || (bus_id >= MAX_UNIFI_DEVS)) {
+        unifi_error(priv, "register_unifi_sdio: invalid device %d\n",
+                bus_id);
+        return NULL;
+    }
+
+    down(&Unifi_instance_mutex);
+
+    if (In_use[bus_id] != UNIFI_DEV_NOT_IN_USE) {
+        unifi_error(priv, "register_unifi_sdio: device %d is already in use\n",
+                bus_id);
+        goto failed0;
+    }
+
+
+    /* Allocate device private and net_device structs */
+    priv = uf_alloc_netdevice(sdio_dev, bus_id);
+    if (priv == NULL) {
+        unifi_error(priv, "Failed to allocate driver private\n");
+        goto failed0;
+    }
+
+    priv->unifi_device = dev;
+
+    SET_NETDEV_DEV(priv->netdev[0], dev);
+
+    /* We are not ready to send data yet. */
+    netif_carrier_off(priv->netdev[0]);
+
+    /* Allocate driver context. */
+    priv->card = unifi_alloc_card(priv->sdio, priv);
+    if (priv->card == NULL) {
+        unifi_error(priv, "Failed to allocate UniFi driver card struct.\n");
+        goto failed1;
+    }
+
+    if (Unifi_instances[bus_id]) {
+        unifi_error(priv, "Internal error: instance for slot %d is already taken\n",
+                bus_id);
+    }
+    Unifi_instances[bus_id] = priv;
+    In_use[bus_id] = UNIFI_DEV_IN_USE;
+
+    /* Save the netdev_priv for use by the netdev event callback mechanism */
+    Unifi_netdev_instances[bus_id * CSR_WIFI_NUM_INTERFACES] = netdev_priv(priv->netdev[0]);
+
+    /* Initialise the mini-coredump capture buffers */
+    csrResult = unifi_coredump_init(priv->card, (u16)coredump_max);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Couldn't allocate mini-coredump buffers\n");
+    }
+
+    /* Create the character device nodes */
+    r = uf_create_device_nodes(priv, bus_id);
+    if (r) {
+        goto failed1;
+    }
+
+    /*
+     * We use the slot number as unifi device index.
+     */
+    snprintf(priv->proc_entry_name, 64, "driver/unifi%d", priv->instance);
+    /*
+     * The following complex casting is in place in order to eliminate 64-bit compilation warning
+     * "cast to/from pointer from/to integer of different size"
+     */
+    if (!create_proc_read_entry(priv->proc_entry_name, 0, 0,
+                uf_read_proc, (void *)(long)priv->instance))
+    {
+        unifi_error(priv, "unifi: can't create /proc/driver/unifi\n");
+    }
+
+    /* Allocate the net_device for interfaces other than 0. */
+    {
+        int i;
+        priv->totalInterfaceCount =0;
+
+        for(i=1;i<CSR_WIFI_NUM_INTERFACES;i++)
+        {
+            if( !uf_alloc_netdevice_for_other_interfaces(priv,i) )
+            {
+                /* error occured while allocating the net_device for interface[i]. The net_device are
+                 * allocated for the interfaces with id<i. Dont worry, all the allocated net_device will
+                 * be releasing chen the control goes to the label failed0.
+                 */
+                unifi_error(priv, "Failed to allocate driver private for interface[%d]\n",i);
+                goto failed0;
+            }
+            else
+            {
+                SET_NETDEV_DEV(priv->netdev[i], dev);
+
+                /* We are not ready to send data yet. */
+                netif_carrier_off(priv->netdev[i]);
+
+                /* Save the netdev_priv for use by the netdev event callback mechanism */
+                Unifi_netdev_instances[bus_id * CSR_WIFI_NUM_INTERFACES + i] = netdev_priv(priv->netdev[i]);
+            }
+        }
+
+        for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
+        {
+            netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+            interfacePriv->netdev_registered=0;
+        }
+    }
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+    if (signal_buffer_init(priv, CSR_WIFI_RX_SIGNAL_BUFFER_SIZE))
+    {
+        unifi_error(priv,"Failed to allocate shared memory for T-H signals\n");
+        goto failed2;
+    }
+    priv->rx_workqueue = create_singlethread_workqueue("rx_workq");
+    if (priv->rx_workqueue == NULL) {
+        unifi_error(priv,"create_singlethread_workqueue failed \n");
+        goto failed3;
+    }
+    INIT_WORK(&priv->rx_work_struct, rx_wq_handler);
+#endif
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+    if (log_hip_signals)
+    {
+        uf_register_hip_offline_debug(priv);
+    }
+#endif
+
+    /* Initialise the SME related threads and parameters */
+    r = uf_sme_init(priv);
+    if (r) {
+        unifi_error(priv, "SME initialisation failed.\n");
+        goto failed4;
+    }
+
+    /*
+     * Run the userspace helper program (unififw) to perform
+     * the device initialisation.
+     */
+    unifi_trace(priv, UDBG1, "run UniFi helper app...\n");
+    r = uf_run_unifihelper(priv);
+    if (r) {
+        unifi_notice(priv, "unable to run UniFi helper app\n");
+        /* Not a fatal error. */
+    }
+
+    up(&Unifi_instance_mutex);
+
+    func_exit();
+    return priv;
+
+failed4:
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+if (log_hip_signals)
+{
+    uf_unregister_hip_offline_debug(priv);
+}
+#endif
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+    flush_workqueue(priv->rx_workqueue);
+    destroy_workqueue(priv->rx_workqueue);
+failed3:
+    signal_buffer_free(priv,CSR_WIFI_RX_SIGNAL_BUFFER_SIZE);
+failed2:
+#endif
+    /* Remove the device nodes */
+    uf_destroy_device_nodes(priv);
+failed1:
+    /* Deregister priv->netdev_client */
+    ul_deregister_client(priv->netdev_client);
+
+failed0:
+    if (priv && priv->card) {
+        unifi_coredump_free(priv->card);
+        unifi_free_card(priv->card);
+    }
+    if (priv) {
+        uf_free_netdevice(priv);
+    }
+
+    up(&Unifi_instance_mutex);
+
+    func_exit();
+    return NULL;
+} /* register_unifi_sdio() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ask_unifi_sdio_cleanup
+ *
+ *      We can not free our private context, until all the char device
+ *      clients have closed the file handles. unregister_unifi_sdio() which
+ *      is called when a card is removed, waits on Unifi_cleanup_wq until
+ *      the reference count becomes zero. It is time to wake it up now.
+ *
+ *  Arguments:
+ *      priv          Pointer to driver context.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+ask_unifi_sdio_cleanup(unifi_priv_t *priv)
+{
+    func_enter();
+
+    /*
+     * Now clear the flag that says the old instance is in use.
+     * This is used to prevent a new instance being started before old
+     * one has finshed closing down, for example if bounce makes the card
+     * appear to be ejected and re-inserted quickly.
+     */
+    In_use[priv->instance] = UNIFI_DEV_CLEANUP;
+
+    unifi_trace(NULL, UDBG5, "ask_unifi_sdio_cleanup: wake up cleanup workqueue.\n");
+    wake_up(&Unifi_cleanup_wq);
+
+    func_exit();
+
+} /* ask_unifi_sdio_cleanup() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  cleanup_unifi_sdio
+ *
+ *      Release any resources owned by a unifi instance.
+ *
+ *  Arguments:
+ *      priv          Pointer to the instance to free.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+cleanup_unifi_sdio(unifi_priv_t *priv)
+{
+    int priv_instance;
+    int i;
+    static const CsrWifiMacAddress broadcast_address = {{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
+
+    func_enter();
+
+    /* Remove the device nodes */
+    uf_destroy_device_nodes(priv);
+
+    /* Mark this device as gone away by NULLing the entry in Unifi_instances */
+    Unifi_instances[priv->instance] = NULL;
+
+    unifi_trace(priv, UDBG5, "cleanup_unifi_sdio: remove_proc_entry\n");
+    /*
+     * Free the children of priv before unifi_free_netdevice() frees
+     * the priv struct
+     */
+    remove_proc_entry(priv->proc_entry_name, 0);
+
+
+    /* Unregister netdev as a client. */
+    if (priv->netdev_client) {
+        unifi_trace(priv, UDBG2, "Netdev client (id:%d s:0x%X) is unregistered\n",
+                priv->netdev_client->client_id, priv->netdev_client->sender_id);
+        ul_deregister_client(priv->netdev_client);
+    }
+
+    /* Destroy the SME related threads and parameters */
+    uf_sme_deinit(priv);
+
+#ifdef CSR_SME_USERSPACE
+    priv->smepriv = NULL;
+#endif
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+    if (log_hip_signals)
+    {
+        uf_unregister_hip_offline_debug(priv);
+    }
+#endif
+
+    /* Free any packets left in the Rx queues */
+    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
+    {
+        uf_free_pending_rx_packets(priv, UF_UNCONTROLLED_PORT_Q, broadcast_address,i);
+        uf_free_pending_rx_packets(priv, UF_CONTROLLED_PORT_Q, broadcast_address,i);
+    }
+    /*
+     * We need to free the resources held by the core, which include tx skbs,
+     * otherwise we can not call unregister_netdev().
+     */
+    if (priv->card) {
+        unifi_trace(priv, UDBG5, "cleanup_unifi_sdio: free card\n");
+        unifi_coredump_free(priv->card);
+        unifi_free_card(priv->card);
+        priv->card = NULL;
+    }
+
+    /*
+     * Unregister the network device.
+     * We can not unregister the netdev before we release
+     * all pending packets in the core.
+     */
+    uf_unregister_netdev(priv);
+    priv->totalInterfaceCount = 0;
+
+    /* Clear the table of registered netdev_priv's */
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        Unifi_netdev_instances[priv->instance * CSR_WIFI_NUM_INTERFACES + i] = NULL;
+    }
+
+    unifi_trace(priv, UDBG5, "cleanup_unifi_sdio: uf_free_netdevice\n");
+    /*
+     * When uf_free_netdevice() returns, the priv is invalid
+     * so we need to remember the instance to clear the global flag later.
+     */
+    priv_instance = priv->instance;
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+    flush_workqueue(priv->rx_workqueue);
+    destroy_workqueue(priv->rx_workqueue);
+    signal_buffer_free(priv,CSR_WIFI_RX_SIGNAL_BUFFER_SIZE);
+#endif
+
+    /* Priv is freed as part of the net_device */
+    uf_free_netdevice(priv);
+
+    /*
+     * Now clear the flag that says the old instance is in use.
+     * This is used to prevent a new instance being started before old
+     * one has finshed closing down, for example if bounce makes the card
+     * appear to be ejected and re-inserted quickly.
+     */
+    In_use[priv_instance] = UNIFI_DEV_NOT_IN_USE;
+
+    unifi_trace(NULL, UDBG5, "cleanup_unifi_sdio: DONE.\n");
+
+    func_exit();
+
+} /* cleanup_unifi_sdio() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unregister_unifi_sdio
+ *
+ *      Call from SDIO driver when it detects that UniFi has been removed.
+ *
+ *  Arguments:
+ *      bus_id          Number of the card that was ejected.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+unregister_unifi_sdio(int bus_id)
+{
+    unifi_priv_t *priv;
+    int interfaceTag=0;
+    u8 reason = CONFIG_IND_EXIT;
+
+    if ((bus_id < 0) || (bus_id >= MAX_UNIFI_DEVS)) {
+        unifi_error(NULL, "unregister_unifi_sdio: invalid device %d\n",
+                bus_id);
+        return;
+    }
+
+    priv = Unifi_instances[bus_id];
+    if (priv == NULL) {
+        unifi_error(priv, "unregister_unifi_sdio: device %d is not registered\n",
+                bus_id);
+        func_exit();
+        return;
+    }
+
+    /* Stop the network traffic before freeing the core. */
+    for(interfaceTag=0;interfaceTag<priv->totalInterfaceCount;interfaceTag++)
+    {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+        if(interfacePriv->netdev_registered)
+        {
+            netif_carrier_off(priv->netdev[interfaceTag]);
+            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[interfaceTag]);
+        }
+    }
+
+#ifdef CSR_NATIVE_LINUX
+    /*
+     * If the unifi thread was started, signal it to stop.  This
+     * should cause any userspace processes with open unifi device to
+     * close them.
+     */
+    uf_stop_thread(priv, &priv->bh_thread);
+
+    /* Unregister the interrupt handler */
+    if (csr_sdio_linux_remove_irq(priv->sdio)) {
+        unifi_notice(priv,
+                "csr_sdio_linux_remove_irq failed to talk to card.\n");
+    }
+
+    /* Ensure no MLME functions are waiting on a the mlme_event semaphore. */
+    uf_abort_mlme(priv);
+#endif /* CSR_NATIVE_LINUX */
+
+    ul_log_config_ind(priv, &reason, sizeof(u8));
+
+    /* Deregister the UDI hook from the core. */
+    unifi_remove_udi_hook(priv->card, logging_handler);
+
+    uf_put_instance(bus_id);
+
+    /*
+     * Wait until the device is cleaned up. i.e., when all userspace
+     * processes have closed any open unifi devices.
+     */
+    wait_event(Unifi_cleanup_wq, In_use[bus_id] == UNIFI_DEV_CLEANUP);
+    unifi_trace(NULL, UDBG5, "Received clean up event\n");
+
+    /* Now we can free the private context and the char device nodes */
+    cleanup_unifi_sdio(priv);
+
+} /* unregister_unifi_sdio() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_find_instance
+ *
+ *      Find the context structure for a given UniFi device instance.
+ *
+ *  Arguments:
+ *      inst            The instance number to look for.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+unifi_priv_t *
+uf_find_instance(int inst)
+{
+    if ((inst < 0) || (inst >= MAX_UNIFI_DEVS)) {
+        return NULL;
+    }
+    return Unifi_instances[inst];
+} /* uf_find_instance() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_find_priv
+ *
+ *      Find the device instance for a given context structure.
+ *
+ *  Arguments:
+ *      priv            The context structure pointer to look for.
+ *
+ *  Returns:
+ *      index of instance, -1 otherwise.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_find_priv(unifi_priv_t *priv)
+{
+    int inst;
+
+    if (!priv) {
+        return -1;
+    }
+
+    for (inst = 0; inst < MAX_UNIFI_DEVS; inst++) {
+        if (Unifi_instances[inst] == priv) {
+            return inst;
+        }
+    }
+
+    return -1;
+} /* uf_find_priv() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_find_netdev_priv
+ *
+ *      Find the device instance for a given netdev context structure.
+ *
+ *  Arguments:
+ *      priv            The context structure pointer to look for.
+ *
+ *  Returns:
+ *      index of instance, -1 otherwise.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_find_netdev_priv(netInterface_priv_t *priv)
+{
+    int inst;
+
+    if (!priv) {
+        return -1;
+    }
+
+    for (inst = 0; inst < MAX_UNIFI_DEVS * CSR_WIFI_NUM_INTERFACES; inst++) {
+        if (Unifi_netdev_instances[inst] == priv) {
+            return inst;
+        }
+    }
+
+    return -1;
+} /* uf_find_netdev_priv() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_get_instance
+ *
+ *      Find the context structure for a given UniFi device instance
+ *      and increment the reference count.
+ *
+ *  Arguments:
+ *      inst            The instance number to look for.
+ *
+ *  Returns:
+ *      Pointer to the instance or NULL if no instance exists.
+ * ---------------------------------------------------------------------------
+ */
+unifi_priv_t *
+uf_get_instance(int inst)
+{
+    unifi_priv_t *priv;
+
+    down(&Unifi_instance_mutex);
+
+    priv = uf_find_instance(inst);
+    if (priv) {
+        priv->ref_count++;
+    }
+
+    up(&Unifi_instance_mutex);
+
+    return priv;
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_put_instance
+ *
+ *      Decrement the context reference count, freeing resources and
+ *      shutting down the driver when the count reaches zero.
+ *
+ *  Arguments:
+ *      inst            The instance number to look for.
+ *
+ *  Returns:
+ *      Pointer to the instance or NULL if no instance exists.
+ * ---------------------------------------------------------------------------
+ */
+void
+uf_put_instance(int inst)
+{
+    unifi_priv_t *priv;
+
+    down(&Unifi_instance_mutex);
+
+    priv = uf_find_instance(inst);
+    if (priv) {
+        priv->ref_count--;
+        if (priv->ref_count == 0) {
+            ask_unifi_sdio_cleanup(priv);
+        }
+    }
+
+    up(&Unifi_instance_mutex);
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_read_proc
+ *
+ *      Read method for driver node in /proc/driver/unifi0
+ *
+ *  Arguments:
+ *      page
+ *      start
+ *      offset
+ *      count
+ *      eof
+ *      data
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+#ifdef CONFIG_PROC_FS
+static int
+uf_read_proc(char *page, char **start, off_t offset, int count,
+        int *eof, void *data)
+{
+#define UNIFI_DEBUG_TXT_BUFFER 8*1024
+    unifi_priv_t *priv;
+    int actual_amount_to_copy;
+    char *p, *orig_p;
+    s32 remain = UNIFI_DEBUG_TXT_BUFFER;
+    s32 written;
+    int i;
+
+    /*
+    * The following complex casting is in place in order to eliminate 64-bit compilation warning
+    * "cast to/from pointer from/to integer of different size"
+    */
+    priv = uf_find_instance((int)(long)data);
+    if (!priv) {
+        return 0;
+    }
+
+    p = kmalloc( UNIFI_DEBUG_TXT_BUFFER, GFP_KERNEL );
+
+    orig_p = p;
+
+    written = CsrSnprintf(p, remain, "UniFi SDIO Driver: %s %s %s\n",
+            CSR_WIFI_VERSION, __DATE__, __TIME__);
+    UNIFI_SNPRINTF_RET(p, remain, written);
+#ifdef CSR_SME_USERSPACE
+    written = CsrSnprintf(p, remain, "SME: CSR userspace ");
+    UNIFI_SNPRINTF_RET(p, remain, written);
+#ifdef CSR_SUPPORT_WEXT
+    written = CsrSnprintf(p, remain, "with WEXT support\n");
+#else
+    written = CsrSnprintf(p, remain, "\n");
+#endif /* CSR_SUPPORT_WEXT */
+    UNIFI_SNPRINTF_RET(p, remain, written);
+#endif /* CSR_SME_USERSPACE */
+#ifdef CSR_NATIVE_LINUX
+    written = CsrSnprintf(p, remain, "SME: native\n");
+    UNIFI_SNPRINTF_RET(p, remain, written);
+#endif
+
+#ifdef CSR_SUPPORT_SME
+    written = CsrSnprintf(p, remain,
+            "Firmware (ROM) build:%lu, Patch:%lu\n",
+            priv->card_info.fw_build,
+            priv->sme_versions.firmwarePatch);
+    UNIFI_SNPRINTF_RET(p, remain, written);
+#endif
+    p += unifi_print_status(priv->card, p, &remain);
+
+    written = CsrSnprintf(p, remain, "Last dbg str: %s\n",
+            priv->last_debug_string);
+    UNIFI_SNPRINTF_RET(p, remain, written);
+
+    written = CsrSnprintf(p, remain, "Last dbg16:");
+    UNIFI_SNPRINTF_RET(p, remain, written);
+    for (i = 0; i < 8; i++) {
+        written = CsrSnprintf(p, remain, " %04X",
+                priv->last_debug_word16[i]);
+        UNIFI_SNPRINTF_RET(p, remain, written);
+    }
+    written = CsrSnprintf(p, remain, "\n");
+    UNIFI_SNPRINTF_RET(p, remain, written);
+    written = CsrSnprintf(p, remain, "           ");
+    UNIFI_SNPRINTF_RET(p, remain, written);
+    for (; i < 16; i++) {
+        written = CsrSnprintf(p, remain, " %04X",
+                priv->last_debug_word16[i]);
+        UNIFI_SNPRINTF_RET(p, remain, written);
+    }
+    written = CsrSnprintf(p, remain, "\n");
+    UNIFI_SNPRINTF_RET(p, remain, written);
+    *start = page;
+
+    written = UNIFI_DEBUG_TXT_BUFFER - remain;
+
+    if( offset >= written )
+    {
+        *eof = 1;
+        kfree( orig_p );
+        return(0);
+    }
+
+    if( offset + count > written )
+    {
+        actual_amount_to_copy = written - offset;
+        *eof = 1;
+    }
+    else
+    {
+        actual_amount_to_copy = count;
+    }
+
+    memcpy( page, &(orig_p[offset]), actual_amount_to_copy );
+
+    kfree( orig_p );
+
+    return( actual_amount_to_copy );
+} /* uf_read_proc() */
+#endif
+
+
+
+
+static void
+uf_lx_suspend(CsrSdioFunction *sdio_ctx)
+{
+    unifi_priv_t *priv = sdio_ctx->driverData;
+    unifi_suspend(priv);
+
+    CsrSdioSuspendAcknowledge(sdio_ctx, CSR_RESULT_SUCCESS);
+}
+
+static void
+uf_lx_resume(CsrSdioFunction *sdio_ctx)
+{
+    unifi_priv_t *priv = sdio_ctx->driverData;
+    unifi_resume(priv);
+
+    CsrSdioResumeAcknowledge(sdio_ctx, CSR_RESULT_SUCCESS);
+}
+
+static int active_slot = MAX_UNIFI_DEVS;
+static struct device *os_devices[MAX_UNIFI_DEVS];
+
+void
+uf_add_os_device(int bus_id, struct device *os_device)
+{
+    if ((bus_id < 0) || (bus_id >= MAX_UNIFI_DEVS)) {
+        unifi_error(NULL, "uf_add_os_device: invalid device %d\n",
+                bus_id);
+        return;
+    }
+
+    active_slot = bus_id;
+    os_devices[bus_id] = os_device;
+} /* uf_add_os_device() */
+
+void
+uf_remove_os_device(int bus_id)
+{
+    if ((bus_id < 0) || (bus_id >= MAX_UNIFI_DEVS)) {
+        unifi_error(NULL, "uf_remove_os_device: invalid device %d\n",
+                bus_id);
+        return;
+    }
+
+    active_slot = bus_id;
+    os_devices[bus_id] = NULL;
+} /* uf_remove_os_device() */
+
+static void
+uf_sdio_inserted(CsrSdioFunction *sdio_ctx)
+{
+    unifi_priv_t *priv;
+
+    unifi_trace(NULL, UDBG5, "uf_sdio_inserted(0x%p), slot_id=%d, dev=%p\n",
+            sdio_ctx, active_slot, os_devices[active_slot]);
+
+    priv = register_unifi_sdio(sdio_ctx, active_slot, os_devices[active_slot]);
+    if (priv == NULL) {
+        CsrSdioInsertedAcknowledge(sdio_ctx, CSR_RESULT_FAILURE);
+        return;
+    }
+
+    sdio_ctx->driverData = priv;
+
+    CsrSdioInsertedAcknowledge(sdio_ctx, CSR_RESULT_SUCCESS);
+} /* uf_sdio_inserted() */
+
+
+static void
+uf_sdio_removed(CsrSdioFunction *sdio_ctx)
+{
+    unregister_unifi_sdio(active_slot);
+    CsrSdioRemovedAcknowledge(sdio_ctx);
+} /* uf_sdio_removed() */
+
+
+static void
+uf_sdio_dsr_handler(CsrSdioFunction *sdio_ctx)
+{
+    unifi_priv_t *priv = sdio_ctx->driverData;
+
+    unifi_sdio_interrupt_handler(priv->card);
+} /* uf_sdio_dsr_handler() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_sdio_int_handler
+ *
+ *      Interrupt callback function for SDIO interrupts.
+ *      This is called in kernel context (i.e. not interrupt context).
+ *      We retrieve the unifi context pointer and call the main UniFi
+ *      interrupt handler.
+ *
+ *  Arguments:
+ *      fdev      SDIO context pointer
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static CsrSdioInterruptDsrCallback
+uf_sdio_int_handler(CsrSdioFunction *sdio_ctx)
+{
+    return uf_sdio_dsr_handler;
+} /* uf_sdio_int_handler() */
+
+
+
+
+static CsrSdioFunctionId unifi_ids[] =
+{
+    {
+        .manfId = SDIO_MANF_ID_CSR,
+        .cardId = SDIO_CARD_ID_UNIFI_3,
+        .sdioFunction = SDIO_WLAN_FUNC_ID_UNIFI_3,
+        .sdioInterface = CSR_SDIO_ANY_SDIO_INTERFACE,
+    },
+    {
+        .manfId = SDIO_MANF_ID_CSR,
+        .cardId = SDIO_CARD_ID_UNIFI_4,
+        .sdioFunction = SDIO_WLAN_FUNC_ID_UNIFI_4,
+        .sdioInterface = CSR_SDIO_ANY_SDIO_INTERFACE,
+    }
+};
+
+
+/*
+ * Structure to register with the glue layer.
+ */
+static CsrSdioFunctionDriver unifi_sdioFunction_drv =
+{
+    .inserted = uf_sdio_inserted,
+    .removed = uf_sdio_removed,
+    .intr = uf_sdio_int_handler,
+    .suspend = uf_lx_suspend,
+    .resume = uf_lx_resume,
+
+    .ids = unifi_ids,
+    .idsCount = sizeof(unifi_ids) / sizeof(unifi_ids[0])
+};
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_sdio_load
+ *  uf_sdio_unload
+ *
+ *      These functions are called from the main module load and unload
+ *      functions. They perform the appropriate operations for the monolithic
+ *      driver.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+int __init
+uf_sdio_load(void)
+{
+    CsrResult csrResult;
+
+    csrResult = CsrSdioFunctionDriverRegister(&unifi_sdioFunction_drv);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(NULL, "Failed to register UniFi SDIO driver: csrResult=%d\n", csrResult);
+        return -EIO;
+    }
+
+    return 0;
+} /* uf_sdio_load() */
+
+
+
+void __exit
+uf_sdio_unload(void)
+{
+    CsrSdioFunctionDriverUnregister(&unifi_sdioFunction_drv);
+} /* uf_sdio_unload() */
+
diff --git a/drivers/staging/csr/mlme.c b/drivers/staging/csr/mlme.c
new file mode 100644
index 00000000000..ed767eccbb3
--- /dev/null
+++ b/drivers/staging/csr/mlme.c
@@ -0,0 +1,436 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     mlme.c
+ *
+ * PURPOSE:
+ *      This file provides functions to send MLME requests to the UniFi.
+ *
+ * Copyright (C) 2007-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "unifi_priv.h"
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_mlme_wait_for_reply
+ *
+ *      Wait for a reply after sending a signal.
+ *
+ * Arguments:
+ *      priv            Pointer to device private context struct
+ *      ul_client       Pointer to linux client
+ *      sig_reply_id    ID of the expected reply (defined in sigs.h).
+ *      timeout         timeout in ms
+ *
+ * Returns:
+ *      0 on success, -ve POSIX code on error.
+ *
+ * Notes:
+ *      This function waits for a specific (sig_reply_id) signal from UniFi.
+ *      It also match the sequence number of the received (cfm) signal, with
+ *      the latest sequence number of the signal (req) we have sent.
+ *      These two number match be equal.
+ *      Should only be used for waiting xxx.cfm signals and only after
+ *      we have sent the matching xxx.req signal to UniFi.
+ *      If no response is received within the expected time (timeout), we assume
+ *      that the UniFi is busy and return an error.
+ *      If the wait is aborted by a kernel signal arriving, we stop waiting.
+ *      If a response from UniFi is not what we expected, we discard it and
+ *      wait again. This could be a response from an aborted request. If we
+ *      see several bad responses we assume we have lost synchronisation with
+ *      UniFi.
+ * ---------------------------------------------------------------------------
+ */
+static int
+unifi_mlme_wait_for_reply(unifi_priv_t *priv, ul_client_t *pcli, int sig_reply_id, int timeout)
+{
+    int retries = 0;
+    long r;
+    long t = timeout;
+    unsigned int sent_seq_no;
+
+    /* Convert t in ms to jiffies */
+    t = msecs_to_jiffies(t);
+
+    do {
+        /* Wait for the confirm or timeout. */
+        r = wait_event_interruptible_timeout(pcli->udi_wq,
+                                             (pcli->wake_up_wq_id) || (priv->io_aborted == 1),
+                                             t);
+        /* Check for general i/o error */
+        if (priv->io_aborted) {
+            unifi_error(priv, "MLME operation aborted\n");
+            return -EIO;
+        }
+
+        /*
+         * If r=0 the request has timed-out.
+         * If r>0 the request has completed successfully.
+         * If r=-ERESTARTSYS an event (kill signal) has interrupted the wait_event.
+         */
+        if ((r == 0) && (pcli->wake_up_wq_id == 0)) {
+            unifi_error(priv, "mlme_wait: timed-out waiting for 0x%.4X, after %lu msec.\n",
+                        sig_reply_id,  jiffies_to_msecs(t));
+            pcli->wake_up_wq_id = 0;
+            return -ETIMEDOUT;
+        } else if (r == -ERESTARTSYS) {
+            unifi_error(priv, "mlme_wait: waiting for 0x%.4X was aborted.\n", sig_reply_id);
+            pcli->wake_up_wq_id = 0;
+            return -EINTR;
+        } else {
+            /* Get the sequence number of the signal that we previously set. */
+            if (pcli->seq_no != 0) {
+                sent_seq_no = pcli->seq_no - 1;
+            } else {
+                sent_seq_no = 0x0F;
+            }
+
+            unifi_trace(priv, UDBG5, "Received 0x%.4X, seq: (r:%d, s:%d)\n",
+                        pcli->wake_up_wq_id,
+                        pcli->wake_seq_no, sent_seq_no);
+
+            /* The two sequence ids must match. */
+            if (pcli->wake_seq_no == sent_seq_no) {
+                /* and the signal ids must match. */
+                if (sig_reply_id == pcli->wake_up_wq_id) {
+                    /* Found the expected signal */
+                    break;
+                } else {
+                    /* This should never happen ... */
+                    unifi_error(priv, "mlme_wait: mismatching signal id (0x%.4X - exp 0x%.4X) (seq %d)\n",
+                                pcli->wake_up_wq_id,
+                                sig_reply_id,
+                                pcli->wake_seq_no);
+                    pcli->wake_up_wq_id = 0;
+                    return -EIO;
+                }
+            }
+            /* Wait for the next signal. */
+            pcli->wake_up_wq_id = 0;
+
+            retries ++;
+            if (retries >= 3) {
+                unifi_error(priv, "mlme_wait: confirm wait retries exhausted (0x%.4X - exp 0x%.4X)\n",
+                            pcli->wake_up_wq_id,
+                            sig_reply_id);
+                pcli->wake_up_wq_id = 0;
+                return -EIO;
+            }
+        }
+    } while (1);
+
+    pcli->wake_up_wq_id = 0;
+
+    return 0;
+} /* unifi_mlme_wait_for_reply() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_mlme_blocking_request
+ *
+ *      Send a MLME request signal to UniFi.
+ *
+ * Arguments:
+ *      priv            Pointer to device private context struct
+ *      pcli            Pointer to context of calling process
+ *      sig             Pointer to the signal to send
+ *      data_ptrs       Pointer to the bulk data of the signal
+ *      timeout         The request's timeout.
+ *
+ * Returns:
+ *      0 on success, 802.11 result code on error.
+ * ---------------------------------------------------------------------------
+ */
+int
+unifi_mlme_blocking_request(unifi_priv_t *priv, ul_client_t *pcli,
+                            CSR_SIGNAL *sig, bulk_data_param_t *data_ptrs,
+                            int timeout)
+{
+    int r;
+
+    func_enter();
+
+    if (sig->SignalPrimitiveHeader.SignalId == 0) {
+        unifi_error(priv, "unifi_mlme_blocking_request: Invalid Signal Id (0x%x)\n",
+                    sig->SignalPrimitiveHeader.SignalId);
+        return -EINVAL;
+    }
+
+    down(&priv->mlme_blocking_mutex);
+
+    sig->SignalPrimitiveHeader.ReceiverProcessId = 0;
+    sig->SignalPrimitiveHeader.SenderProcessId = pcli->sender_id | pcli->seq_no;
+
+    unifi_trace(priv, UDBG2, "Send client=%d, S:0x%04X, sig 0x%.4X\n",
+                pcli->client_id,
+                sig->SignalPrimitiveHeader.SenderProcessId,
+                sig->SignalPrimitiveHeader.SignalId);
+    /* Send the signal to UniFi */
+    r = ul_send_signal_unpacked(priv, sig, data_ptrs);
+    if (r) {
+        up(&priv->mlme_blocking_mutex);
+        unifi_error(priv, "Error queueing MLME REQUEST signal\n");
+        return r;
+    }
+
+    unifi_trace(priv, UDBG5, "Send 0x%.4X, seq = %d\n",
+                sig->SignalPrimitiveHeader.SignalId, pcli->seq_no);
+
+    /*
+     * Advance the sequence number of the last sent signal, only
+     * if the signal has been successfully set.
+     */
+    pcli->seq_no++;
+    if (pcli->seq_no > 0x0F) {
+        pcli->seq_no = 0;
+    }
+
+    r = unifi_mlme_wait_for_reply(priv, pcli, (sig->SignalPrimitiveHeader.SignalId + 1), timeout);
+    up(&priv->mlme_blocking_mutex);
+
+    if (r) {
+        unifi_error(priv, "Error waiting for MLME CONFIRM signal\n");
+        return r;
+    }
+
+    func_exit();
+    return 0;
+} /* unifi_mlme_blocking_request() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_mlme_copy_reply_and_wakeup_client
+ *
+ *      Copy the reply signal from UniFi to the client's structure
+ *      and wake up the waiting client.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_mlme_copy_reply_and_wakeup_client(ul_client_t *pcli,
+                                        CSR_SIGNAL *signal, int signal_len,
+                                        const bulk_data_param_t *bulkdata)
+{
+    int i;
+
+    /* Copy the signal to the reply */
+    memcpy(pcli->reply_signal, signal, signal_len);
+
+    /* Get the sequence number of the signal that woke us up. */
+    pcli->wake_seq_no = pcli->reply_signal->SignalPrimitiveHeader.ReceiverProcessId & 0x0F;
+
+    /* Append any bulk data */
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+        if (bulkdata->d[i].data_length > 0) {
+            if (bulkdata->d[i].os_data_ptr) {
+                memcpy(pcli->reply_bulkdata[i]->ptr, bulkdata->d[i].os_data_ptr, bulkdata->d[i].data_length);
+                pcli->reply_bulkdata[i]->length = bulkdata->d[i].data_length;
+            } else {
+                pcli->reply_bulkdata[i]->length = 0;
+            }
+        }
+    }
+
+    /* Wake the requesting MLME function. */
+    pcli->wake_up_wq_id = pcli->reply_signal->SignalPrimitiveHeader.SignalId;
+    wake_up_interruptible(&pcli->udi_wq);
+
+} /* unifi_mlme_copy_reply_and_wakeup_client() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_abort_mlme
+ *
+ *      Abort any MLME operation in progress.
+ *      This is used in the error recovery mechanism.
+ *
+ *  Arguments:
+ *      priv          Pointer to driver context.
+ *
+ *  Returns:
+ *      0 on success.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_abort_mlme(unifi_priv_t *priv)
+{
+    ul_client_t *ul_cli;
+
+    /* Ensure no MLME functions are waiting on a the mlme_event semaphore. */
+    priv->io_aborted = 1;
+
+    ul_cli = priv->netdev_client;
+    if (ul_cli) {
+        wake_up_interruptible(&ul_cli->udi_wq);
+    }
+
+    ul_cli = priv->wext_client;
+    if (ul_cli) {
+        wake_up_interruptible(&ul_cli->udi_wq);
+    }
+
+    return 0;
+} /* uf_abort_mlme() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *      Human-readable decoding of Reason and Result codes.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+struct mlme_code {
+    const char *name;
+    int id;
+};
+
+static const struct mlme_code Result_codes[] = {
+    { "Success",                             0x0000 },
+    { "Unspecified Failure",                 0x0001 },
+    /* (Reserved)                      0x0002 - 0x0009 */
+    { "Refused Capabilities Mismatch",       0x000A },
+    /* (Reserved)                          0x000B */
+    { "Refused External Reason",             0x000C },
+    /* (Reserved)                      0x000D - 0x0010 */
+    { "Refused AP Out Of Memory",            0x0011 },
+    { "Refused Basic Rates Mismatch",        0x0012 },
+    /* (Reserved)                      0x0013 - 0x001F */
+    { "Failure",                             0x0020 },
+    /* (Reserved)                      0x0021 - 0x0024 */
+    { "Refused Reason Unspecified",          0x0025 },
+    { "Invalid Parameters",                  0x0026 },
+    { "Rejected With Suggested Changes",     0x0027 },
+    /* (Reserved)                      0x0028 - 0x002E */
+    { "Rejected For Delay Period",           0x002F },
+    { "Not Allowed",                         0x0030 },
+    { "Not Present",                         0x0031 },
+    { "Not QSTA",                            0x0032 },
+    /* (Reserved)                      0x0033 - 0x7FFF */
+    { "Timeout",                             0x8000 },
+    { "Too Many Simultaneous Requests",      0x8001 },
+    { "BSS Already Started Or Joined",       0x8002 },
+    { "Not Supported",                       0x8003 },
+    { "Transmission Failure",                0x8004 },
+    { "Refused Not Authenticated",           0x8005 },
+    { "Reset Required Before Start",         0x8006 },
+    { "LM Info Unavailable",                 0x8007 },
+    { NULL, -1 }
+};
+
+static const struct mlme_code Reason_codes[] = {
+    /* (Reserved)                      0x0000 */
+    { "Unspecified Reason",              0x0001 },
+    { "Authentication Not Valid",        0x0002 },
+    { "Deauthenticated Leave BSS",       0x0003 },
+    { "Disassociated Inactivity",        0x0004 },
+    { "AP Overload",                     0x0005 },
+    { "Class2 Frame Error",              0x0006 },
+    { "Class3 Frame Error",              0x0007 },
+    { "Disassociated Leave BSS",         0x0008 },
+    { "Association Not Authenticated",   0x0009 },
+    { "Disassociated Power Capability",  0x000A },
+    { "Disassociated Supported Channels", 0x000B },
+    /* (Reserved)                      0x000C */
+    { "Invalid Information Element",     0x000D },
+    { "Michael MIC Failure",             0x000E },
+    { "Fourway Handshake Timeout",       0x000F },
+    { "Group Key Update Timeout",        0x0010 },
+    { "Handshake Element Different",     0x0011 },
+    { "Invalid Group Cipher",            0x0012 },
+    { "Invalid Pairwise Cipher",         0x0013 },
+    { "Invalid AKMP",                    0x0014 },
+    { "Unsupported RSN IE Version",      0x0015 },
+    { "Invalid RSN IE Capabilities",     0x0016 },
+    { "Dot1X Auth Failed",               0x0017 },
+    { "Cipher Rejected By Policy",       0x0018 },
+    /* (Reserved)                  0x0019 - 0x001F */
+    { "QoS Unspecified Reason",          0x0020 },
+    { "QoS Insufficient Bandwidth",      0x0021 },
+    { "QoS Excessive Not Ack",           0x0022 },
+    { "QoS TXOP Limit Exceeded",         0x0023 },
+    { "QSTA Leaving",                    0x0024 },
+    { "End TS, End DLS, End BA",         0x0025 },
+    { "Unknown TS, Unknown DLS, Unknown BA", 0x0026 },
+    { "Timeout",                         0x0027 },
+    /* (Reserved)                  0x0028 - 0x002C */
+    { "STAKey Mismatch",                 0x002D },
+    { NULL, -1 }
+};
+
+
+static const char *
+lookup_something(const struct mlme_code *n, int id)
+{
+    for (; n->name; n++) {
+        if (n->id == id) {
+            return n->name;
+        }
+    }
+
+    /* not found */
+    return NULL;
+} /* lookup_something() */
+
+
+const char *
+lookup_result_code(int result)
+{
+    static char fallback[16];
+    const char *str;
+
+    str = lookup_something(Result_codes, result);
+
+    if (str == NULL) {
+        snprintf(fallback, 16, "%d", result);
+        str = fallback;
+    }
+
+    return str;
+} /* lookup_result_code() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  lookup_reason
+ *
+ *      Return a description string for a WiFi MLME ReasonCode.
+ *
+ *  Arguments:
+ *      reason          The ReasonCode to interpret.
+ *
+ *  Returns:
+ *      Pointer to description string.
+ * ---------------------------------------------------------------------------
+ */
+const char *
+lookup_reason_code(int reason)
+{
+    static char fallback[16];
+    const char *str;
+
+    str = lookup_something(Reason_codes, reason);
+
+    if (str == NULL) {
+        snprintf(fallback, 16, "%d", reason);
+        str = fallback;
+    }
+
+    return str;
+} /* lookup_reason_code() */
+
diff --git a/drivers/staging/csr/monitor.c b/drivers/staging/csr/monitor.c
new file mode 100644
index 00000000000..628782ad641
--- /dev/null
+++ b/drivers/staging/csr/monitor.c
@@ -0,0 +1,399 @@
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     monitor.c
+ *
+ * Copyright (C) 2006-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#include "unifi_priv.h"
+
+#ifdef UNIFI_SNIFF_ARPHRD
+
+
+#if (UNIFI_SNIFF_ARPHRD == ARPHRD_IEEE80211_RADIOTAP)
+#include <net/ieee80211_radiotap.h>
+#endif
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW ETH_P_ALL
+#endif
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_start_sniff
+ *
+ *      Start UniFi capture in SNIFF mode, i.e capture everything it hears.
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *
+ *  Returns:
+ *      0 on success or kernel error code
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_start_sniff(unifi_priv_t *priv)
+{
+    ul_client_t *pcli = priv->wext_client;
+    CSR_SIGNAL signal;
+    CSR_MLME_SNIFFJOIN_REQUEST *req = &signal.u.MlmeSniffjoinRequest;
+    int timeout = 1000;
+    int r;
+
+    req->Ifindex = priv->if_index;
+    req->Channel = priv->wext_conf.channel;
+    req->ChannelStartingFactor = 0;
+
+    signal.SignalPrimitiveHeader.SignalId = CSR_MLME_SNIFFJOIN_REQUEST_ID;
+
+    r = unifi_mlme_blocking_request(priv, pcli, &signal, NULL, timeout);
+    if (r < 0) {
+        unifi_error(priv, "failed to send SNIFFJOIN request, error %d\n", r);
+        return r;
+    }
+
+    r = pcli->reply_signal->u.MlmeSniffjoinConfirm.Resultcode;
+    if (r) {
+        unifi_notice(priv, "SNIFFJOIN request was rejected with result 0x%X (%s)\n",
+                     r, lookup_result_code(r));
+        return -EIO;
+    }
+
+    return 0;
+} /* uf_start_sniff() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * netrx_radiotap
+ *
+ *      Reformat a UniFi SNIFFDATA signal into a radiotap packet.
+ *
+ * Arguments:
+ *      priv            OS private context pointer.
+ *      ind             Pointer to a MA_UNITDATA_INDICATION or
+ *                      DS_UNITDATA_INDICATION indication structure.
+ *
+ * Notes:
+ *      Radiotap header values are all little-endian, UniFi signals will have
+ *      been converted to host-endian.
+ * ---------------------------------------------------------------------------
+ */
+#if (UNIFI_SNIFF_ARPHRD == ARPHRD_IEEE80211_RADIOTAP)
+static void
+netrx_radiotap(unifi_priv_t *priv,
+               const CSR_MA_SNIFFDATA_INDICATION *ind,
+               struct sk_buff *skb_orig)
+{
+    struct net_device *dev = priv->netdev;
+    struct sk_buff *skb = NULL;
+    unsigned char *ptr;
+    unsigned char *base;
+    int ind_data_len = skb_orig->len - 2 - ETH_HLEN;
+    struct unifi_rx_radiotap_header {
+        struct ieee80211_radiotap_header rt_hdr;
+        /* IEEE80211_RADIOTAP_TSFT */
+        u64 rt_tsft;
+        /* IEEE80211_RADIOTAP_FLAGS */
+        u8  rt_flags;
+        /* IEEE80211_RADIOTAP_RATE */
+        u8  rt_rate;
+        /* IEEE80211_RADIOTAP_CHANNEL */
+        u16 rt_chan;
+        u16 rt_chan_flags;
+        /* IEEE80211_RADIOTAP_DBM_ANTSIGNAL */
+        u8  rt_dbm_antsignal;
+        /* IEEE80211_RADIOTAP_DBM_ANTNOISE */
+        u8  rt_dbm_antnoise;
+        /* IEEE80211_RADIOTAP_ANTENNA */
+        u8  rt_antenna;
+
+        /* pad to 4-byte boundary */
+        u8 pad[3];
+    } __attribute__((__packed__));
+
+    struct unifi_rx_radiotap_header *unifi_rt;
+    int signal, noise, snr;
+
+    func_enter();
+
+    if (ind_data_len <= 0) {
+        unifi_error(priv, "Invalid length in CSR_MA_SNIFFDATA_INDICATION.\n");
+        return;
+    }
+
+    /*
+     * Allocate a SKB for the received data packet, including radiotap
+     * header.
+     */
+    skb = dev_alloc_skb(ind_data_len + sizeof(struct unifi_rx_radiotap_header) + 4);
+    if (! skb) {
+        unifi_error(priv, "alloc_skb failed.\n");
+        priv->stats.rx_errors++;
+        return;
+    }
+
+    base = skb->data;
+
+    /* Reserve the radiotap header at the front of skb */
+    unifi_rt = (struct unifi_rx_radiotap_header *)
+        skb_put(skb, sizeof(struct unifi_rx_radiotap_header));
+
+    /* Copy in the 802.11 frame */
+    ptr = skb_put(skb, ind_data_len);
+    memcpy(ptr, skb_orig->data, ind_data_len);
+
+    unifi_rt->rt_hdr.it_version = PKTHDR_RADIOTAP_VERSION;
+    unifi_rt->rt_hdr.it_pad = 0;	/* always good to zero */
+    unifi_rt->rt_hdr.it_len = sizeof(struct unifi_rx_radiotap_header);
+
+    /* Big bitfield of all the fields we provide in radiotap */
+    unifi_rt->rt_hdr.it_present = 0
+        | (1 << IEEE80211_RADIOTAP_TSFT)
+        | (1 << IEEE80211_RADIOTAP_FLAGS)
+        | (1 << IEEE80211_RADIOTAP_RATE)
+        | (1 << IEEE80211_RADIOTAP_CHANNEL)
+        | (1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL)
+        | (1 << IEEE80211_RADIOTAP_DBM_ANTNOISE)
+        | (1 << IEEE80211_RADIOTAP_ANTENNA)
+        ;
+
+
+    /* No flags to set */
+    unifi_rt->rt_tsft = (((u64)ind->Timestamp.x[7]) | (((u64)ind->Timestamp.x[6]) << 8) |
+                         (((u64)ind->Timestamp.x[5]) << 16) | (((u64)ind->Timestamp.x[4]) << 24) |
+                         (((u64)ind->Timestamp.x[3]) << 32) | (((u64)ind->Timestamp.x[2]) << 40) |
+                         (((u64)ind->Timestamp.x[1]) << 48) | (((u64)ind->Timestamp.x[0]) << 56));
+
+    unifi_rt->rt_flags = 0;
+
+    unifi_rt->rt_rate = ind->Rate;
+
+    unifi_rt->rt_chan = cpu_to_le16(ieee80211chan2mhz(priv->wext_conf.channel));
+    unifi_rt->rt_chan_flags = 0;
+
+    /* Convert signal to dBm */
+    signal = (s16)unifi2host_16(ind->Rssi);  /* in dBm */
+    snr    = (s16)unifi2host_16(ind->Snr);   /* in dB */
+    noise  = signal - snr;
+
+    unifi_rt->rt_dbm_antsignal = signal;
+    unifi_rt->rt_dbm_antnoise = noise;
+
+    unifi_rt->rt_antenna = ind->AntennaId;
+
+
+    skb->dev = dev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
+    skb->mac_header = skb->data;
+#else
+    skb->mac.raw = skb->data;
+#endif
+    skb->pkt_type = PACKET_OTHERHOST;
+    skb->protocol = __constant_htons(ETH_P_80211_RAW);
+    memset(skb->cb, 0, sizeof(skb->cb));
+
+    /* Pass up to Linux network stack */
+    netif_rx_ni(skb);
+
+    dev->last_rx = jiffies;
+
+    /* Bump the rx stats */
+    priv->stats.rx_packets++;
+    priv->stats.rx_bytes += ind_data_len;
+
+    func_exit();
+} /* netrx_radiotap() */
+#endif /* RADIOTAP */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * netrx_prism
+ *
+ *      Reformat a UniFi SNIFFDATA signal into a Prism format sniff packet.
+ *
+ * Arguments:
+ *      priv            OS private context pointer.
+ *      ind             Pointer to a MA_UNITDATA_INDICATION or
+ *                      DS_UNITDATA_INDICATION indication structure.
+ *
+ * Notes:
+ *      Radiotap header values are all little-endian, UniFi signals will have
+ *      been converted to host-endian.
+ * ---------------------------------------------------------------------------
+ */
+#if (UNIFI_SNIFF_ARPHRD == ARPHRD_IEEE80211_PRISM)
+static void
+netrx_prism(unifi_priv_t *priv,
+            const CSR_MA_SNIFFDATA_INDICATION *ind,
+            struct sk_buff *skb_orig)
+{
+    struct net_device *dev = priv->netdev;
+    struct sk_buff *skb = NULL;
+    unsigned char *ptr;
+    unsigned char *base;
+    int ind_data_len = skb_orig->len - 2 - ETH_HLEN;
+#define WLANCAP_MAGIC_COOKIE_V1 0x80211001
+    struct avs_header_v1 {
+        uint32  version;
+        uint32  length;
+        uint64  mactime;
+        uint64  hosttime;
+        uint32  phytype;
+        uint32  channel;
+        uint32  datarate;
+        uint32  antenna;
+        uint32  priority;
+        uint32  ssi_type;
+        int32   ssi_signal;
+        int32   ssi_noise;
+        uint32  preamble;
+        uint32  encoding;
+    } *avs;
+    int signal, noise, snr;
+
+    func_enter();
+
+    if (ind_data_len <= 0) {
+        unifi_error(priv, "Invalid length in CSR_MA_SNIFFDATA_INDICATION.\n");
+        return;
+    }
+
+    /*
+     * Allocate a SKB for the received data packet, including radiotap
+     * header.
+     */
+    skb = dev_alloc_skb(ind_data_len + sizeof(struct avs_header_v1) + 4);
+    if (! skb) {
+        unifi_error(priv, "alloc_skb failed.\n");
+        priv->stats.rx_errors++;
+        return;
+    }
+
+    base = skb->data;
+
+    /* Reserve the radiotap header at the front of skb */
+    avs = (struct avs_header_v1 *)skb_put(skb, sizeof(struct avs_header_v1));
+
+    /* Copy in the 802.11 frame */
+    ptr = skb_put(skb, ind_data_len);
+    memcpy(ptr, skb_orig->data, ind_data_len);
+
+    /* Convert signal to dBm */
+    signal = 0x10000 - ((s16)unifi2host_16(ind->Rssi));  /* in dBm */
+    snr    = (s16)unifi2host_16(ind->Snr);   /* in dB */
+    noise  = signal - snr;
+
+    avs->version        = htonl(WLANCAP_MAGIC_COOKIE_V1);
+    avs->length         = htonl(sizeof(struct avs_header_v1));
+    avs->mactime        = __cpu_to_be64(ind->Timestamp);
+    avs->hosttime       = __cpu_to_be64(jiffies);
+    avs->phytype        = htonl(9);             /* dss_ofdm_dot11_g */
+    avs->channel        = htonl(priv->wext_conf.channel);
+    avs->datarate       = htonl(ind->Rate * 5);
+    avs->antenna        = htonl(ind->Antenna);
+    avs->priority       = htonl(0);             /* unknown */
+    avs->ssi_type       = htonl(2);             /* dBm */
+    avs->ssi_signal     = htonl(signal);
+    avs->ssi_noise      = htonl(noise);
+    avs->preamble       = htonl(0); /* unknown */
+    avs->encoding       = htonl(0); /* unknown */
+
+
+    skb->dev = dev;
+    skb->mac.raw = skb->data;
+    skb->pkt_type = PACKET_OTHERHOST;
+    skb->protocol = __constant_htons(ETH_P_80211_RAW);
+    memset(skb->cb, 0, sizeof(skb->cb));
+
+    /* Pass up to Linux network stack */
+    netif_rx_ni(skb);
+
+    dev->last_rx = jiffies;
+
+    /* Bump the rx stats */
+    priv->stats.rx_packets++;
+    priv->stats.rx_bytes += ind_data_len;
+
+    func_exit();
+} /* netrx_prism() */
+#endif /* PRISM */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * ma_sniffdata_ind
+ *
+ *      Reformat a UniFi SNIFFDATA signal into a network
+ *
+ * Arguments:
+ *      ospriv          OS private context pointer.
+ *      ind             Pointer to a MA_UNITDATA_INDICATION or
+ *                      DS_UNITDATA_INDICATION indication structure.
+ *      bulkdata        Pointer to a bulk data structure, describing
+ *                      the data received.
+ *
+ * Notes:
+ *      Radiotap header values are all little-endian, UniFi signals will have
+ *      been converted to host-endian.
+ * ---------------------------------------------------------------------------
+ */
+void
+ma_sniffdata_ind(void *ospriv,
+                 const CSR_MA_SNIFFDATA_INDICATION *ind,
+                 const bulk_data_param_t *bulkdata)
+{
+    unifi_priv_t *priv = ospriv;
+    struct net_device *dev = priv->netdev;
+    struct sk_buff *skb = (struct sk_buff*)bulkdata->d[0].os_net_buf_ptr;
+
+    func_enter();
+
+    if (bulkdata->d[0].data_length == 0) {
+        unifi_warning(priv, "rx: MA-SNIFFDATA indication with zero bulk data\n");
+        func_exit();
+        return;
+    }
+
+    skb->len = bulkdata->d[0].data_length;
+
+    /* We only process data packets if the interface is open */
+    if (unlikely(!netif_running(dev))) {
+        priv->stats.rx_dropped++;
+        priv->wext_conf.wireless_stats.discard.misc++;
+        dev_kfree_skb(skb);
+        return;
+    }
+
+    if (ind->ReceptionStatus) {
+        priv->stats.rx_dropped++;
+        priv->wext_conf.wireless_stats.discard.misc++;
+        printk(KERN_INFO "unifi: Dropping corrupt sniff packet\n");
+        dev_kfree_skb(skb);
+        return;
+    }
+
+#if (UNIFI_SNIFF_ARPHRD == ARPHRD_IEEE80211_PRISM)
+    netrx_prism(priv, ind, skb);
+#endif /* PRISM */
+
+#if (UNIFI_SNIFF_ARPHRD == ARPHRD_IEEE80211_RADIOTAP)
+    netrx_radiotap(priv, ind, skb);
+#endif /* RADIOTAP */
+
+    dev_kfree_skb(skb);
+
+} /* ma_sniffdata_ind() */
+
+
+#endif /* UNIFI_SNIFF_ARPHRD */
+
diff --git a/drivers/staging/csr/netdev.c b/drivers/staging/csr/netdev.c
new file mode 100644
index 00000000000..1e6e111a8e1
--- /dev/null
+++ b/drivers/staging/csr/netdev.c
@@ -0,0 +1,3993 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     netdev.c
+ *
+ * PURPOSE:
+ *      This file provides the upper edge interface to the linux netdevice
+ *      and wireless extensions.
+ *      It is part of the porting exercise.
+ *
+ * Copyright (C) 2005-2010 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+
+/*
+ * Porting Notes:
+ * This file implements the data plane of the UniFi linux driver.
+ *
+ * All the Tx packets are passed to the HIP core lib, using the
+ * unifi_send_signal() API. For EAPOL packets use the MLME-EAPOL.req
+ * signal, for all other use the MLME-UNITDATA.req. The unifi_send_signal()
+ * expects the wire-formatted (packed) signal. For convenience, in the OS
+ * layer we only use the native (unpacked) signal structures. The HIP core lib
+ * provides the write_pack() helper function to convert to the packed signal.
+ * The packet is stored in the bulk data of the signal. We do not need to
+ * allocate new memory to store the packet, because unifi_net_data_malloc()
+ * is implemented to return a skb, which is the format of packet in Linux.
+ * The HIP core lib frees the bulk data buffers, so we do not need to do
+ * this in the OS layer.
+ *
+ * All the Rx packets are MLME-UNITDATA.ind signals, passed by the HIP core lib
+ * in unifi_receive_event(). We do not need to allocate an skb and copy the
+ * received packet because the HIP core lib has stored in memory allocated by
+ * unifi_net_data_malloc(). Also, we can perform the 802.11 to Ethernet
+ * translation in-place because we allocate the extra memory allocated in
+ * unifi_net_data_malloc().
+ *
+ * If possible, the porting exercise should appropriately implement
+ * unifi_net_data_malloc() and unifi_net_data_free() to save copies between
+ * network and driver buffers.
+ */
+
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <linux/mutex.h>
+#include <linux/semaphore.h>
+
+#include <linux/vmalloc.h>
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "unifi_priv.h"
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
+#include <net/iw_handler.h>
+#endif
+#include <net/pkt_sched.h>
+
+
+/* ALLOW_Q_PAUSE: Pre 2.6.28 kernels do not support multiple driver queues (required for QoS).
+ * In order to support QoS in these kernels, multiple queues are implemented in the driver. But since
+ * there is only a single queue in the kernel (leading to multiple queues in the driver) there is no possibility
+ * of stopping a particular queue in the kernel. Stopping the single kernel queue leads to undesirable starvation
+ * of driver queues. One of the proposals is to not stop the kernel queue but to prevent dequeuing from the
+ * 'stopped' driver queue. Allow q pause is an experimental implementation of this scheme for pre 2.6.28 kernels.
+ * When NOT defined, queues are paused locally in the driver and packets are dequeued for transmission only from the
+ * unpaused queues. When Allow q pause is defined the kernel queue is stopped whenever any driver queue is paused.
+ */
+#define ALLOW_Q_PAUSE
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+#ifdef UNIFI_NET_NAME
+#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
+    do {                                                                \
+        static char name[8];                                           \
+        sprintf(name, "%s%s", UNIFI_NET_NAME, _name);                   \
+        _dev = alloc_netdev_mq(_size, name, _setup, _num_of_queues);    \
+    } while (0);
+#else
+#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
+    do {                                                                \
+        _dev = alloc_etherdev_mq(_size, _num_of_queues);                \
+    } while (0);
+#endif /* UNIFI_NET_NAME */
+#else
+#ifdef UNIFI_NET_NAME
+#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
+    do {                                                                \
+        static char name[8];                                           \
+        sprintf(name, "%s%s", UNIFI_NET_NAME, _name);                   \
+        _dev = alloc_netdev(_size, name, _setup);                       \
+    } while (0);
+#else
+#define UF_ALLOC_NETDEV(_dev, _size, _name, _setup, _num_of_queues)     \
+    do {                                                                \
+        _dev = alloc_etherdev(_size);                                   \
+    } while (0);
+#endif /* UNIFI_NET_NAME */
+#endif /* LINUX_VERSION_CODE */
+
+
+/* Wext handler is suported only if CSR_SUPPORT_WEXT is defined */
+#ifdef CSR_SUPPORT_WEXT
+extern struct iw_handler_def unifi_iw_handler_def;
+#endif /* CSR_SUPPORT_WEXT */
+static void check_ba_frame_age_timeout( unifi_priv_t *priv,
+                                            netInterface_priv_t *interfacePriv,
+                                            ba_session_rx_struct *ba_session);
+static void process_ba_frame(unifi_priv_t *priv,
+                             netInterface_priv_t *interfacePriv,
+                             ba_session_rx_struct *ba_session,
+                             frame_desc_struct *frame_desc);
+static void process_ba_complete(unifi_priv_t *priv, netInterface_priv_t *interfacePriv);
+static void process_ma_packet_error_ind(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata);
+static void process_amsdu(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata);
+static int uf_net_open(struct net_device *dev);
+static int uf_net_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+static int uf_net_stop(struct net_device *dev);
+static struct net_device_stats *uf_net_get_stats(struct net_device *dev);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+static u16 uf_net_select_queue(struct net_device *dev, struct sk_buff *skb);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+static netdev_tx_t uf_net_xmit(struct sk_buff *skb, struct net_device *dev);
+#else
+static int uf_net_xmit(struct sk_buff *skb, struct net_device *dev);
+#ifndef NETDEV_TX_OK
+#define NETDEV_TX_OK        0
+#endif
+#ifndef NETDEV_TX_BUSY
+#define NETDEV_TX_BUSY      1
+#endif
+#endif
+static void uf_set_multicast_list(struct net_device *dev);
+
+
+typedef int (*tx_signal_handler)(unifi_priv_t *priv, struct sk_buff *skb, const struct ethhdr *ehdr, CSR_PRIORITY priority);
+
+#ifdef CONFIG_NET_SCHED
+/*
+ * Queueing Discipline Interface
+ * Only used if kernel is configured with CONFIG_NET_SCHED
+ */
+
+/*
+ * The driver uses the qdisc interface to buffer and control all
+ * outgoing traffic. We create a root qdisc, register our qdisc operations
+ * and later we create two subsiduary pfifo queues for the uncontrolled
+ * and controlled ports.
+ *
+ * The network stack delivers all outgoing packets in our enqueue handler.
+ * There, we classify the packet and decide whether to store it or drop it
+ * (if the controlled port state is set to "discard").
+ * If the packet is enqueued, the network stack call our dequeue handler.
+ * There, we decide whether we can send the packet, delay it or drop it
+ * (the controlled port configuration might have changed meanwhile).
+ * If a packet is dequeued, then the network stack calls our hard_start_xmit
+ * handler where finally we send the packet.
+ *
+ * If the hard_start_xmit handler fails to send the packet, we return
+ * NETDEV_TX_BUSY and the network stack call our requeue handler where
+ * we put the packet back in the same queue in came from.
+ *
+ */
+
+struct uf_sched_data
+{
+    /* Traffic Classifier TBD */
+    struct tcf_proto *filter_list;
+    /* Our two queues */
+    struct Qdisc *queues[UNIFI_TRAFFIC_Q_MAX];
+};
+
+struct uf_tx_packet_data {
+    /* Queue the packet is stored in */
+    unifi_TrafficQueue queue;
+    /* QoS Priority determined when enqueing packet */
+    CSR_PRIORITY priority;
+    /* Debug */
+    unsigned long host_tag;
+};
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+static int uf_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd);
+static int uf_qdiscop_requeue(struct sk_buff *skb, struct Qdisc* qd);
+static struct sk_buff *uf_qdiscop_dequeue(struct Qdisc* qd);
+static void uf_qdiscop_reset(struct Qdisc* qd);
+static void uf_qdiscop_destroy(struct Qdisc* qd);
+static int uf_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,25)
+static int uf_qdiscop_tune(struct Qdisc *qd, struct nlattr *opt);
+static int uf_qdiscop_init(struct Qdisc *qd, struct nlattr *opt);
+#else
+static int uf_qdiscop_tune(struct Qdisc *qd, struct rtattr *opt);
+static int uf_qdiscop_init(struct Qdisc *qd, struct rtattr *opt);
+#endif
+#endif
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+/* queueing discipline operations */
+static struct Qdisc_ops uf_qdisc_ops =
+{
+    .next = NULL,
+    .cl_ops = NULL,
+    .id = "UniFi Qdisc",
+    .priv_size = sizeof(struct uf_sched_data),
+
+    .enqueue = uf_qdiscop_enqueue,
+    .dequeue = uf_qdiscop_dequeue,
+    .requeue = uf_qdiscop_requeue,
+    .drop = NULL, /* drop not needed since we are always the root qdisc */
+
+    .init = uf_qdiscop_init,
+    .reset = uf_qdiscop_reset,
+    .destroy = uf_qdiscop_destroy,
+    .change = uf_qdiscop_tune,
+
+    .dump = uf_qdiscop_dump,
+};
+#endif /* LINUX_VERSION_CODE */
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)         \
+    qdisc_create_dflt(dev, netdev_get_tx_queue(_dev, 0), _ops, _root)
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)         \
+    qdisc_create_dflt(dev, _ops, _root)
+#else
+#define UF_QDISC_CREATE_DFLT(_dev, _ops, _root)         \
+    qdisc_create_dflt(dev, _ops)
+#endif /* LINUX_VERSION_CODE */
+
+#endif /* CONFIG_NET_SCHED */
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+static const struct net_device_ops uf_netdev_ops =
+{
+    .ndo_open = uf_net_open,
+    .ndo_stop = uf_net_stop,
+    .ndo_start_xmit = uf_net_xmit,
+    .ndo_do_ioctl = uf_net_ioctl,
+    .ndo_get_stats = uf_net_get_stats, /* called by /proc/net/dev */
+    .ndo_set_rx_mode = uf_set_multicast_list,
+    .ndo_select_queue = uf_net_select_queue,
+};
+#endif
+
+static u8 oui_rfc1042[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+static u8 oui_8021h[P80211_OUI_LEN]   = { 0x00, 0x00, 0xf8 };
+
+
+/* Callback for event logging to blocking clients */
+static void netdev_mlme_event_handler(ul_client_t  *client,
+                                      const u8 *sig_packed, int sig_len,
+                                      const bulk_data_param_t *bulkdata,
+                                      int dir);
+
+#ifdef CSR_SUPPORT_WEXT
+/* Declare netdev_notifier block which will contain the state change
+ * handler callback function
+ */
+static struct notifier_block uf_netdev_notifier;
+#endif
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_alloc_netdevice
+ *
+ *      Allocate memory for the net_device and device private structs
+ *      for this interface.
+ *      Fill in the fields, but don't register the interface yet.
+ *      We need to configure the UniFi first.
+ *
+ *  Arguments:
+ *      sdio_dev        Pointer to SDIO context handle to use for all
+ *                      SDIO ops.
+ *      bus_id          A small number indicating the SDIO card position on the
+ *                      bus. Typically this is the slot number, e.g. 0, 1 etc.
+ *                      Valid values are 0 to MAX_UNIFI_DEVS-1.
+ *
+ *  Returns:
+ *      Pointer to device private struct.
+ *
+ *  Notes:
+ *      The net_device and device private structs are allocated together
+ *      and should be freed by freeing the net_device pointer.
+ * ---------------------------------------------------------------------------
+ */
+unifi_priv_t *
+uf_alloc_netdevice(CsrSdioFunction *sdio_dev, int bus_id)
+{
+    struct net_device *dev;
+    unifi_priv_t *priv;
+    netInterface_priv_t *interfacePriv;
+#ifdef CSR_SUPPORT_WEXT
+    int rc;
+#endif
+    unsigned char i; /* loop index */
+
+    /*
+     * Allocate netdevice struct, assign name template and
+     * setup as an ethernet device.
+     * The net_device and private structs are zeroed. Ether_setup() then
+     * sets up ethernet handlers and values.
+     * The RedHat 9 redhat-config-network tool doesn't recognise wlan* devices,
+     * so use "eth*" (like other wireless extns drivers).
+     */
+    UF_ALLOC_NETDEV(dev, sizeof(unifi_priv_t)+sizeof(netInterface_priv_t), "%d", ether_setup, UNIFI_TRAFFIC_Q_MAX);
+
+    if (dev == NULL) {
+        return NULL;
+    }
+
+    /* Set up back pointer from priv to netdev */
+    interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    priv = (unifi_priv_t *)(interfacePriv + 1);
+    interfacePriv->privPtr = priv;
+    interfacePriv->InterfaceTag = 0;
+
+
+    /* Initialize all supported netdev interface to be NULL */
+    for(i=0; i<CSR_WIFI_NUM_INTERFACES; i++) {
+        priv->netdev[i] = NULL;
+        priv->interfacePriv[i] = NULL;
+    }
+    priv->netdev[0] = dev;
+    priv->interfacePriv[0] = interfacePriv;
+
+    /* Setup / override net_device fields */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    dev->netdev_ops = &uf_netdev_ops;
+#else
+    dev->open             = uf_net_open;
+    dev->stop             = uf_net_stop;
+    dev->hard_start_xmit  = uf_net_xmit;
+    dev->do_ioctl         = uf_net_ioctl;
+
+    /* called by /proc/net/dev */
+    dev->get_stats = uf_net_get_stats;
+
+    dev->set_multicast_list = uf_set_multicast_list;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+    dev->select_queue       = uf_net_select_queue;
+#endif
+#endif
+
+#ifdef CSR_SUPPORT_WEXT
+    dev->wireless_handlers = &unifi_iw_handler_def;
+#if IW_HANDLER_VERSION < 6
+    dev->get_wireless_stats = unifi_get_wireless_stats;
+#endif /* IW_HANDLER_VERSION */
+#endif /* CSR_SUPPORT_WEXT */
+
+    /* This gives us enough headroom to add the 802.11 header */
+    dev->needed_headroom = 32;
+
+    /* Use bus_id as instance number */
+    priv->instance = bus_id;
+    /* Store SDIO pointer to pass in the core */
+    priv->sdio = sdio_dev;
+
+    sdio_dev->driverData = (void*)priv;
+    /* Consider UniFi to be uninitialised */
+    priv->init_progress = UNIFI_INIT_NONE;
+
+    priv->prev_queue = 0;
+
+    /*
+     * Initialise the clients structure array.
+     * We do not need protection around ul_init_clients() because
+     * the character device can not be used until uf_alloc_netdevice()
+     * returns and Unifi_instances[bus_id]=priv is set, since unifi_open()
+     * will return -ENODEV.
+     */
+    ul_init_clients(priv);
+
+    /*
+     * Register a new ul client to send the multicast list signals.
+     * Note: priv->instance must be set before calling this.
+     */
+    priv->netdev_client = ul_register_client(priv,
+            0,
+            netdev_mlme_event_handler);
+    if (priv->netdev_client == NULL) {
+        unifi_error(priv,
+                "Failed to register a unifi client for background netdev processing\n");
+        free_netdev(priv->netdev[0]);
+        return NULL;
+    }
+    unifi_trace(priv, UDBG2, "Netdev %p client (id:%d s:0x%X) is registered\n",
+            dev, priv->netdev_client->client_id, priv->netdev_client->sender_id);
+
+    priv->sta_wmm_capabilities = 0;
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_SUPPORT_SME))
+    priv->wapi_multicast_filter = 0;
+    priv->wapi_unicast_filter = 0;
+    priv->wapi_unicast_queued_pkt_filter = 0;
+#ifdef CSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND
+    priv->isWapiConnection = FALSE;
+#endif
+#endif
+
+    /* Enable all queues by default */
+    interfacePriv->queueEnabled[0] = 1;
+    interfacePriv->queueEnabled[1] = 1;
+    interfacePriv->queueEnabled[2] = 1;
+    interfacePriv->queueEnabled[3] = 1;
+
+#ifdef CSR_SUPPORT_SME
+    priv->allPeerDozing = 0;
+#endif
+    /*
+     * Initialise the OS private struct.
+     */
+    /*
+     * Instead of deciding in advance to use 11bg or 11a, we could do a more
+     * clever scan on both radios.
+     */
+    if (use_5g) {
+        priv->if_index = CSR_INDEX_5G;
+        unifi_info(priv, "Using the 802.11a radio\n");
+    } else {
+        priv->if_index = CSR_INDEX_2G4;
+    }
+
+    /* Initialise bh thread structure */
+    priv->bh_thread.thread_task = NULL;
+    priv->bh_thread.block_thread = 1;
+    init_waitqueue_head(&priv->bh_thread.wakeup_q);
+    priv->bh_thread.wakeup_flag = 0;
+    sprintf(priv->bh_thread.name, "uf_bh_thread");
+
+    /* reset the connected state for the interface */
+    interfacePriv->connected = UnifiConnectedUnknown;  /* -1 unknown, 0 no, 1 yes */
+
+#ifdef USE_DRIVER_LOCK
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+    sema_init(&priv->lock, 1);
+#else
+    init_MUTEX(&priv->lock);
+#endif
+#endif /* USE_DRIVER_LOCK */
+
+    spin_lock_init(&priv->send_signal_lock);
+
+    spin_lock_init(&priv->m4_lock);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+    sema_init(&priv->ba_mutex, 1);
+#else
+    init_MUTEX(&priv->ba_mutex);
+#endif
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    spin_lock_init(&priv->wapi_lock);
+#endif
+
+#ifdef CSR_SUPPORT_SME
+    spin_lock_init(&priv->staRecord_lock);
+    spin_lock_init(&priv->tx_q_lock);
+#endif
+
+    /* Create the Traffic Analysis workqueue */
+    priv->unifi_workqueue = create_singlethread_workqueue("unifi_workq");
+    if (priv->unifi_workqueue == NULL) {
+        /* Deregister priv->netdev_client */
+        ul_deregister_client(priv->netdev_client);
+        free_netdev(priv->netdev[0]);
+        return NULL;
+    }
+
+#ifdef CSR_SUPPORT_SME
+    /* Create the Multicast Addresses list work structure */
+    INIT_WORK(&priv->multicast_list_task, uf_multicast_list_wq);
+
+    /* Create m4 buffering work structure */
+    INIT_WORK(&interfacePriv->send_m4_ready_task, uf_send_m4_ready_wq);
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    /* Create work structure to buffer the WAPI data packets to be sent to SME for encryption */
+    INIT_WORK(&interfacePriv->send_pkt_to_encrypt, uf_send_pkt_to_encrypt);
+#endif
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+#ifdef CONFIG_NET_SCHED
+    /* Register the qdisc operations */
+    register_qdisc(&uf_qdisc_ops);
+#endif /* CONFIG_NET_SCHED */
+#endif /* LINUX_VERSION_CODE */
+
+    priv->ref_count = 1;
+
+
+    priv->amp_client = NULL;
+    priv->coredump_mode = 0;
+    priv->ptest_mode = 0;
+    priv->wol_suspend = FALSE;
+    INIT_LIST_HEAD(&interfacePriv->rx_uncontrolled_list);
+    INIT_LIST_HEAD(&interfacePriv->rx_controlled_list);
+    sema_init(&priv->rx_q_sem, 1);
+
+#ifdef CSR_SUPPORT_WEXT
+    interfacePriv->netdev_callback_registered = FALSE;
+    interfacePriv->wait_netdev_change = FALSE;
+    /* Register callback for netdevice state changes */
+    if ((rc = register_netdevice_notifier(&uf_netdev_notifier)) == 0) {
+        interfacePriv->netdev_callback_registered = TRUE;
+    }
+    else {
+        unifi_warning(priv, "Failed to register netdevice notifier : %d %p\n", rc, dev);
+    }
+#endif /* CSR_SUPPORT_WEXT */
+
+#ifdef CSR_WIFI_SPLIT_PATCH
+    /* set it to some invalid value */
+    priv->pending_mode_set.common.destination = 0xaaaa;
+#endif
+
+    return priv;
+} /* uf_alloc_netdevice() */
+
+/*
+ *---------------------------------------------------------------------------
+ *  uf_alloc_netdevice_for_other_interfaces
+ *
+ *      Allocate memory for the net_device and device private structs
+ *      for this interface.
+ *      Fill in the fields, but don't register the interface yet.
+ *      We need to configure the UniFi first.
+ *
+ *  Arguments:
+ *      interfaceTag   Interface number.
+ *      sdio_dev        Pointer to SDIO context handle to use for all
+ *                      SDIO ops.
+ *      bus_id          A small number indicating the SDIO card position on the
+ *                      bus. Typically this is the slot number, e.g. 0, 1 etc.
+ *                      Valid values are 0 to MAX_UNIFI_DEVS-1.
+ *
+ *  Returns:
+ *      Pointer to device private struct.
+ *
+ *  Notes:
+ *      The device private structure contains the interfaceTag and pointer to the unifi_priv
+ *      structure created allocated by net_device od interface0.
+ *      The net_device and device private structs are allocated together
+ *      and should be freed by freeing the net_device pointer.
+ * ---------------------------------------------------------------------------
+ */
+u8
+uf_alloc_netdevice_for_other_interfaces(unifi_priv_t *priv, u16 interfaceTag)
+{
+    struct net_device *dev;
+    netInterface_priv_t *interfacePriv;
+
+    /*
+     * Allocate netdevice struct, assign name template and
+     * setup as an ethernet device.
+     * The net_device and private structs are zeroed. Ether_setup() then
+     * sets up ethernet handlers and values.
+     * The RedHat 9 redhat-config-network tool doesn't recognise wlan* devices,
+     * so use "eth*" (like other wireless extns drivers).
+     */
+    UF_ALLOC_NETDEV(dev, sizeof(netInterface_priv_t), "%d", ether_setup, 1);
+    if (dev == NULL) {
+        return FALSE;
+    }
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_alloc_netdevice_for_other_interfaces bad interfaceTag\n");
+        return FALSE;
+    }
+
+    /* Set up back pointer from priv to netdev */
+    interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    interfacePriv->privPtr = priv;
+    interfacePriv->InterfaceTag = interfaceTag;
+    priv->netdev[interfaceTag] = dev;
+    priv->interfacePriv[interfacePriv->InterfaceTag] = interfacePriv;
+
+    /* reset the connected state for the interface */
+    interfacePriv->connected = UnifiConnectedUnknown;  /* -1 unknown, 0 no, 1 yes */
+    INIT_LIST_HEAD(&interfacePriv->rx_uncontrolled_list);
+    INIT_LIST_HEAD(&interfacePriv->rx_controlled_list);
+
+    /* Setup / override net_device fields */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    dev->netdev_ops = &uf_netdev_ops;
+#else
+    dev->open             = uf_net_open;
+    dev->stop             = uf_net_stop;
+    dev->hard_start_xmit  = uf_net_xmit;
+    dev->do_ioctl         = uf_net_ioctl;
+
+    /* called by /proc/net/dev */
+    dev->get_stats = uf_net_get_stats;
+
+    dev->set_multicast_list = uf_set_multicast_list;
+#endif
+
+#ifdef CSR_SUPPORT_WEXT
+    dev->wireless_handlers = &unifi_iw_handler_def;
+#if IW_HANDLER_VERSION < 6
+    dev->get_wireless_stats = unifi_get_wireless_stats;
+#endif /* IW_HANDLER_VERSION */
+#endif /* CSR_SUPPORT_WEXT */
+    return TRUE;
+} /* uf_alloc_netdevice() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_free_netdevice
+ *
+ *      Unregister the network device and free the memory allocated for it.
+ *      NB This includes the memory for the priv struct.
+ *
+ *  Arguments:
+ *      priv            Device private pointer.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_free_netdevice(unifi_priv_t *priv)
+{
+    int i;
+    unsigned long flags;
+
+    func_enter();
+
+    unifi_trace(priv, UDBG1, "uf_free_netdevice\n");
+
+    if (!priv) {
+        return -EINVAL;
+    }
+
+    /*
+     * Free any buffers used for holding firmware
+     */
+    uf_release_firmware_files(priv);
+
+#if (defined CSR_SUPPORT_SME) && (defined CSR_SUPPORT_WEXT)
+    if (priv->connection_config.mlmeAssociateReqInformationElements) {
+        kfree(priv->connection_config.mlmeAssociateReqInformationElements);
+    }
+    priv->connection_config.mlmeAssociateReqInformationElements = NULL;
+    priv->connection_config.mlmeAssociateReqInformationElementsLength = 0;
+
+    if (priv->mib_data.length) {
+        vfree(priv->mib_data.data);
+    }
+    priv->mib_data.data = NULL;
+    priv->mib_data.length = 0;
+
+#endif /* CSR_SUPPORT_SME && CSR_SUPPORT_WEXT*/
+
+    /* Free any bulkdata buffers allocated for M4 caching */
+    spin_lock_irqsave(&priv->m4_lock, flags);
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+        if (interfacePriv->m4_bulk_data.data_length > 0) {
+            unifi_trace(priv, UDBG5, "uf_free_netdevice: free M4 bulkdata %d\n", i);
+            unifi_net_data_free(priv, &interfacePriv->m4_bulk_data);
+        }
+    }
+    spin_unlock_irqrestore(&priv->m4_lock, flags);
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    /* Free any bulkdata buffers allocated for M4 caching */
+    spin_lock_irqsave(&priv->wapi_lock, flags);
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+        if (interfacePriv->wapi_unicast_bulk_data.data_length > 0) {
+            unifi_trace(priv, UDBG5, "uf_free_netdevice: free WAPI PKT bulk data %d\n", i);
+            unifi_net_data_free(priv, &interfacePriv->wapi_unicast_bulk_data);
+        }
+    }
+    spin_unlock_irqrestore(&priv->wapi_lock, flags);
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+#ifdef CONFIG_NET_SCHED
+    /* Unregister the qdisc operations */
+    unregister_qdisc(&uf_qdisc_ops);
+#endif /* CONFIG_NET_SCHED */
+#endif /* LINUX_VERSION_CODE */
+
+#ifdef CSR_SUPPORT_WEXT
+    /* Unregister callback for netdevice state changes */
+    unregister_netdevice_notifier(&uf_netdev_notifier);
+#endif /* CSR_SUPPORT_WEXT */
+
+#ifdef CSR_SUPPORT_SME
+    /* Cancel work items and destroy the workqueue */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,23)
+    cancel_work_sync(&priv->multicast_list_task);
+#endif
+#endif
+/* Destroy the workqueues. */
+    flush_workqueue(priv->unifi_workqueue);
+    destroy_workqueue(priv->unifi_workqueue);
+
+    /* Free up netdev in reverse order: priv is allocated with netdev[0].
+     * So, netdev[0] should be freed after all other netdevs are freed up
+     */
+    for (i=CSR_WIFI_NUM_INTERFACES-1; i>=0; i--) {
+        /*Free the netdev struct and priv, which are all one lump*/
+        if (priv->netdev[i]) {
+            unifi_error(priv, "uf_free_netdevice: netdev %d %p\n", i, priv->netdev[i]);
+            free_netdev(priv->netdev[i]);
+        }
+    }
+
+    func_exit();
+    return 0;
+} /* uf_free_netdevice() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_net_open
+ *
+ *      Called when userland does "ifconfig wlan0 up".
+ *
+ *  Arguments:
+ *      dev             Device pointer.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static int
+uf_net_open(struct net_device *dev)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    func_enter();
+
+    /* If we haven't finished UniFi initialisation, we can't start */
+    if (priv->init_progress != UNIFI_INIT_COMPLETED) {
+        unifi_warning(priv, "%s: unifi not ready, failing net_open\n", __FUNCTION__);
+        return -EINVAL;
+    }
+
+#if (defined CSR_NATIVE_LINUX) && (defined UNIFI_SNIFF_ARPHRD) && defined(CSR_SUPPORT_WEXT)
+    /*
+     * To sniff, the user must do "iwconfig mode monitor", which sets
+     * priv->wext_conf.mode to IW_MODE_MONITOR.
+     * Then he/she must do "ifconfig ethn up", which calls this fn.
+     * There is no point in starting the sniff with SNIFFJOIN until
+     * this point.
+     */
+    if (priv->wext_conf.mode == IW_MODE_MONITOR) {
+        int err;
+        err = uf_start_sniff(priv);
+        if (err) {
+            return err;
+        }
+        netif_carrier_on(dev);
+    }
+#endif
+
+#ifdef CSR_SUPPORT_WEXT
+    if (interfacePriv->wait_netdev_change) {
+        unifi_trace(priv, UDBG1, "%s: Waiting for NETDEV_CHANGE, assume connected\n",
+                    __FUNCTION__);
+        interfacePriv->connected = UnifiConnected;
+        interfacePriv->wait_netdev_change = FALSE;
+    }
+#endif
+
+    UF_NETIF_TX_START_ALL_QUEUES(dev);
+
+    func_exit();
+    return 0;
+} /* uf_net_open() */
+
+
+static int
+uf_net_stop(struct net_device *dev)
+{
+#if defined(CSR_NATIVE_LINUX) && defined(UNIFI_SNIFF_ARPHRD) && defined(CSR_SUPPORT_WEXT)
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t*)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    func_enter();
+
+    /* Stop sniffing if in Monitor mode */
+    if (priv->wext_conf.mode == IW_MODE_MONITOR) {
+        if (priv->card) {
+            int err;
+            err = unifi_reset_state(priv, dev->dev_addr, 1);
+            if (err) {
+                return err;
+            }
+        }
+    }
+#else
+    func_enter();
+#endif
+
+    UF_NETIF_TX_STOP_ALL_QUEUES(dev);
+
+    func_exit();
+    return 0;
+} /* uf_net_stop() */
+
+
+/* This is called after the WE handlers */
+static int
+uf_net_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+    int rc;
+
+    rc = -EOPNOTSUPP;
+
+    return rc;
+} /* uf_net_ioctl() */
+
+
+
+static struct net_device_stats *
+uf_net_get_stats(struct net_device *dev)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+
+    return &interfacePriv->stats;
+} /* uf_net_get_stats() */
+
+static CSR_PRIORITY uf_get_packet_priority(unifi_priv_t *priv, netInterface_priv_t *interfacePriv, struct sk_buff *skb, const int proto)
+{
+    CSR_PRIORITY priority = CSR_CONTENTION;
+
+    func_enter();
+    priority = (CSR_PRIORITY) (skb->priority >> 5);
+
+    if (priority == CSR_QOS_UP0) { /* 0 */
+
+        unifi_trace(priv, UDBG5, "uf_get_packet_priority: proto = 0x%.4X\n", proto);
+
+        switch (proto) {
+            case 0x0800:        /* IPv4 */
+            case 0x814C:        /* SNMP */
+            case 0x880C:        /* GSMP */
+                priority = (CSR_PRIORITY) (skb->data[1 + ETH_HLEN] >> 5);
+                break;
+
+            case 0x8100:        /* VLAN */
+                priority = (CSR_PRIORITY) (skb->data[0 + ETH_HLEN] >> 5);
+                break;
+
+            case 0x86DD:        /* IPv6 */
+                priority = (CSR_PRIORITY) ((skb->data[0 + ETH_HLEN] & 0x0E) >> 1);
+                break;
+
+            default:
+                priority = CSR_QOS_UP0;
+                break;
+        }
+    }
+
+    /* Check if we are allowed to transmit on this AC. Because of ACM we may have to downgrade to a lower
+     * priority */
+    if (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_STA ||
+        interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI) {
+        unifi_TrafficQueue queue;
+
+        /* Keep trying lower priorities until we find a queue
+         * Priority to queue mapping is 1,2 - BK, 0,3 - BE, 4,5 - VI, 6,7 - VO */
+        queue = unifi_frame_priority_to_queue(priority);
+
+        while (queue > UNIFI_TRAFFIC_Q_BK && !interfacePriv->queueEnabled[queue]) {
+            queue--;
+            priority = unifi_get_default_downgrade_priority(queue);
+        }
+    }
+
+    unifi_trace(priv, UDBG5, "Packet priority = %d\n", priority);
+
+    func_exit();
+    return priority;
+}
+
+/*
+ */
+/*
+ * ---------------------------------------------------------------------------
+ *  get_packet_priority
+ *
+ *  Arguments:
+ *      priv             private data area of functional driver
+ *      skb              socket buffer
+ *      ehdr             ethernet header to fetch protocol
+ *      interfacePriv    For accessing station record database
+ *
+ *
+ *  Returns:
+ *      CSR_PRIORITY.
+ * ---------------------------------------------------------------------------
+ */
+CSR_PRIORITY
+get_packet_priority(unifi_priv_t *priv, struct sk_buff *skb, const struct ethhdr *ehdr, netInterface_priv_t *interfacePriv)
+{
+    CSR_PRIORITY priority = CSR_CONTENTION;
+    const int proto = ntohs(ehdr->h_proto);
+
+    u8 interfaceMode = interfacePriv->interfaceMode;
+
+    func_enter();
+
+    /* Priority Mapping for all the Modes */
+    switch(interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+            unifi_trace(priv, UDBG4, "mode is STA \n");
+            if ((priv->sta_wmm_capabilities & QOS_CAPABILITY_WMM_ENABLED) == 1) {
+                priority = uf_get_packet_priority(priv, interfacePriv, skb, proto);
+            } else {
+                priority = CSR_CONTENTION;
+            }
+            break;
+#ifdef CSR_SUPPORT_SME
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            {
+                CsrWifiRouterCtrlStaInfo_t * dstStaInfo =
+                    CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv,ehdr->h_dest, interfacePriv->InterfaceTag);
+                unifi_trace(priv, UDBG4, "mode is AP \n");
+                if (!(ehdr->h_dest[0] & 0x01) && dstStaInfo && dstStaInfo->wmmOrQosEnabled) {
+                    /* If packet is not Broadcast/multicast */
+                    priority = uf_get_packet_priority(priv, interfacePriv, skb, proto);
+                } else {
+                    /* Since packet destination is not QSTA, set priority to CSR_CONTENTION */
+                    unifi_trace(priv, UDBG4, "Destination is not QSTA or BroadCast/Multicast\n");
+                    priority = CSR_CONTENTION;
+                }
+            }
+            break;
+#endif
+        default:
+            unifi_trace(priv, UDBG3, " mode unknown in %s func, mode=%x\n", __FUNCTION__, interfaceMode);
+    }
+    unifi_trace(priv, UDBG5, "priority = %x\n", priority);
+
+    func_exit();
+    return priority;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_net_select_queue
+ *
+ *      Called by the kernel to select which queue to put the packet in
+ *
+ *  Arguments:
+ *      dev             Device pointer
+ *      skb             Packet
+ *
+ *  Returns:
+ *      Queue index
+ * ---------------------------------------------------------------------------
+ */
+static u16
+uf_net_select_queue(struct net_device *dev, struct sk_buff *skb)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = (unifi_priv_t *)interfacePriv->privPtr;
+    struct ethhdr ehdr;
+    unifi_TrafficQueue queue;
+    int proto;
+    CSR_PRIORITY priority;
+
+    func_enter();
+
+    memcpy(&ehdr, skb->data, ETH_HLEN);
+    proto = ntohs(ehdr.h_proto);
+
+    /* 802.1x - apply controlled/uncontrolled port rules */
+    if ((proto != ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+            && (proto != ETH_P_WAI)
+#endif
+       ) {
+        /* queues 0 - 3 */
+        priority = get_packet_priority(priv, skb, &ehdr, interfacePriv);
+        queue = unifi_frame_priority_to_queue(priority);
+    } else {
+        /* queue 4 */
+        queue = UNIFI_TRAFFIC_Q_EAPOL;
+    }
+
+
+    func_exit();
+    return (u16)queue;
+} /* uf_net_select_queue() */
+#endif
+
+int
+skb_add_llc_snap(struct net_device *dev, struct sk_buff *skb, int proto)
+{
+    llc_snap_hdr_t *snap;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int headroom;
+
+    /* get the headroom available in skb */
+    headroom = skb_headroom(skb);
+    /* step 1: classify ether frame, DIX or 802.3? */
+
+    if (proto < 0x600) {
+        /* codes <= 1500 reserved for 802.3 lengths */
+        /* it's 802.3, pass ether payload unchanged,  */
+        unifi_trace(priv, UDBG3, "802.3 len: %d\n", skb->len);
+
+        /*   leave off any PAD octets.  */
+        skb_trim(skb, proto);
+    } else if (proto == ETH_P_8021Q) {
+
+        /* Store the VLAN SNAP (should be 87-65). */
+        u16 vlan_snap = *(u16*)skb->data;
+        /* check for headroom availability before skb_push 14 = (4 + 10) */
+        if (headroom < 14) {
+            unifi_trace(priv, UDBG3, "cant append vlan snap: debug\n");
+            return -1;
+        }
+        /* Add AA-AA-03-00-00-00 */
+        snap = (llc_snap_hdr_t *)skb_push(skb, 4);
+        snap->dsap = snap->ssap = 0xAA;
+        snap->ctrl = 0x03;
+        memcpy(snap->oui, oui_rfc1042, P80211_OUI_LEN);
+
+        /* Add AA-AA-03-00-00-00 */
+        snap = (llc_snap_hdr_t *)skb_push(skb, 10);
+        snap->dsap = snap->ssap = 0xAA;
+        snap->ctrl = 0x03;
+        memcpy(snap->oui, oui_rfc1042, P80211_OUI_LEN);
+
+        /* Add the VLAN specific information */
+        snap->protocol = htons(proto);
+        *(u16*)(snap + 1) = vlan_snap;
+
+    } else
+    {
+        /* it's DIXII, time for some conversion */
+        unifi_trace(priv, UDBG3, "DIXII len: %d\n", skb->len);
+
+        /* check for headroom availability before skb_push */
+        if (headroom < sizeof(llc_snap_hdr_t)) {
+            unifi_trace(priv, UDBG3, "cant append snap: debug\n");
+            return -1;
+        }
+        /* tack on SNAP */
+        snap = (llc_snap_hdr_t *)skb_push(skb, sizeof(llc_snap_hdr_t));
+        snap->dsap = snap->ssap = 0xAA;
+        snap->ctrl = 0x03;
+        /* Use the appropriate OUI. */
+        if ((proto == ETH_P_AARP) || (proto == ETH_P_IPX)) {
+            memcpy(snap->oui, oui_8021h, P80211_OUI_LEN);
+        } else {
+            memcpy(snap->oui, oui_rfc1042, P80211_OUI_LEN);
+        }
+        snap->protocol = htons(proto);
+    }
+
+    return 0;
+} /* skb_add_llc_snap() */
+
+#ifdef CSR_SUPPORT_SME
+static int
+_identify_sme_ma_pkt_ind(unifi_priv_t *priv,
+                         const s8 *oui, u16 protocol,
+                         const CSR_SIGNAL *signal,
+                         bulk_data_param_t *bulkdata,
+                         const unsigned char *daddr,
+                         const unsigned char *saddr)
+{
+    CSR_MA_PACKET_INDICATION *pkt_ind = (CSR_MA_PACKET_INDICATION*)&signal->u.MaPacketIndication;
+    int r;
+    u8 i;
+
+    unifi_trace(priv, UDBG5,
+            "_identify_sme_ma_pkt_ind -->\n");
+    for (i = 0; i < MAX_MA_UNIDATA_IND_FILTERS; i++) {
+        if (priv->sme_unidata_ind_filters[i].in_use) {
+            if (!memcmp(oui, priv->sme_unidata_ind_filters[i].oui, 3) &&
+                    (protocol == priv->sme_unidata_ind_filters[i].protocol)) {
+
+                /* Send to client */
+                if (priv->sme_cli) {
+                    /*
+                     * Pass the packet to the SME, using unifi_sys_ma_unitdata_ind().
+                     * The frame needs to be converted according to the encapsulation.
+                     */
+                    unifi_trace(priv, UDBG1,
+                            "_identify_sme_ma_pkt_ind: handle=%d, encap=%d, proto=%x\n",
+                            i, priv->sme_unidata_ind_filters[i].encapsulation,
+                            priv->sme_unidata_ind_filters[i].protocol);
+                    if (priv->sme_unidata_ind_filters[i].encapsulation == CSR_WIFI_ROUTER_ENCAPSULATION_ETHERNET) {
+                        struct sk_buff *skb;
+                        /* The translation is performed on skb... */
+                        skb = (struct sk_buff*)bulkdata->d[0].os_net_buf_ptr;
+                        skb->len = bulkdata->d[0].data_length;
+
+                        unifi_trace(priv, UDBG1,
+                                "_identify_sme_ma_pkt_ind: skb_80211_to_ether -->\n");
+                        r = skb_80211_to_ether(priv, skb, daddr, saddr,
+                                signal, bulkdata);
+                        unifi_trace(priv, UDBG1,
+                                "_identify_sme_ma_pkt_ind: skb_80211_to_ether <--\n");
+                        if (r) {
+                            return -EINVAL;
+                        }
+
+                        /* ... but we indicate buffer and length */
+                        bulkdata->d[0].os_data_ptr = skb->data;
+                        bulkdata->d[0].data_length = skb->len;
+                    } else {
+                        /* Add the MAC addresses before the SNAP */
+                        bulkdata->d[0].os_data_ptr -= 2*ETH_ALEN;
+                        bulkdata->d[0].data_length += 2*ETH_ALEN;
+                        memcpy((void*)bulkdata->d[0].os_data_ptr, daddr, ETH_ALEN);
+                        memcpy((void*)bulkdata->d[0].os_data_ptr + ETH_ALEN, saddr, ETH_ALEN);
+                    }
+
+                    unifi_trace(priv, UDBG1,
+                            "_identify_sme_ma_pkt_ind: unifi_sys_ma_pkt_ind -->\n");
+                    CsrWifiRouterMaPacketIndSend(priv->sme_unidata_ind_filters[i].appHandle,
+                            (pkt_ind->VirtualInterfaceIdentifier & 0xff),
+                            i,
+                            pkt_ind->ReceptionStatus,
+                            bulkdata->d[0].data_length,
+                            (u8*)bulkdata->d[0].os_data_ptr,
+                            NULL,
+                            pkt_ind->Rssi,
+                            pkt_ind->Snr,
+                            pkt_ind->ReceivedRate);
+
+
+                    unifi_trace(priv, UDBG1,
+                            "_identify_sme_ma_pkt_ind: unifi_sys_ma_pkt_ind <--\n");
+                }
+
+                return 1;
+            }
+        }
+    }
+
+    return -1;
+}
+#endif /* CSR_SUPPORT_SME */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  skb_80211_to_ether
+ *
+ *      Make sure the received frame is in Ethernet (802.3) form.
+ *      De-encapsulates SNAP if necessary, adds a ethernet header.
+ *      The source buffer should not contain an 802.11 MAC header
+ *
+ *  Arguments:
+ *      payload         Pointer to packet data received from UniFi.
+ *      payload_length  Number of bytes of data received from UniFi.
+ *      daddr           Destination MAC address.
+ *      saddr           Source MAC address.
+ *
+ *  Returns:
+ *      0 on success, -1 if the packet is bad and should be dropped,
+ *      1 if the packet was forwarded to the SME or AMP client.
+ * ---------------------------------------------------------------------------
+ */
+int
+skb_80211_to_ether(unifi_priv_t *priv, struct sk_buff *skb,
+                   const unsigned char *daddr, const unsigned char *saddr,
+                   const CSR_SIGNAL *signal,
+                   bulk_data_param_t *bulkdata)
+{
+    unsigned char *payload;
+    int payload_length;
+    struct ethhdr *eth;
+    llc_snap_hdr_t *snap;
+    int headroom;
+#define UF_VLAN_LLC_HEADER_SIZE     18
+    static const u8 vlan_inner_snap[] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00 };
+#if defined(CSR_NATIVE_SOFTMAC) && defined(CSR_SUPPORT_SME)
+    const CSR_MA_PACKET_INDICATION *pkt_ind = &signal->u.MaPacketIndication;
+#endif
+
+    if(skb== NULL || daddr == NULL || saddr == NULL){
+        unifi_error(priv,"skb_80211_to_ether: PBC fail\n");
+        return 1;
+    }
+
+    payload = skb->data;
+    payload_length = skb->len;
+
+    snap = (llc_snap_hdr_t *)payload;
+    eth  = (struct ethhdr *)payload;
+
+    /* get the skb headroom size */
+    headroom = skb_headroom(skb);
+
+    /*
+     * Test for the various encodings
+     */
+    if ((payload_length >= sizeof(llc_snap_hdr_t)) &&
+            (snap->dsap == 0xAA) &&
+            (snap->ssap == 0xAA) &&
+            (snap->ctrl == 0x03) &&
+            (snap->oui[0] == 0) &&
+            (snap->oui[1] == 0) &&
+            ((snap->oui[2] == 0) || (snap->oui[2] == 0xF8)))
+    {
+        /* AppleTalk AARP (2) or IPX SNAP */
+        if ((snap->oui[2] == 0) &&
+                ((ntohs(snap->protocol) == ETH_P_AARP) || (ntohs(snap->protocol) == ETH_P_IPX)))
+        {
+            u16 len;
+
+            unifi_trace(priv, UDBG3, "%s len: %d\n",
+                    (ntohs(snap->protocol) == ETH_P_AARP) ? "ETH_P_AARP" : "ETH_P_IPX",
+                    payload_length);
+
+            /* check for headroom availability before skb_push */
+            if (headroom < (2 * ETH_ALEN + 2)) {
+                unifi_warning(priv, "headroom not available to skb_push ether header\n");
+                return -1;
+            }
+
+            /* Add 802.3 header and leave full payload */
+            len = htons(skb->len);
+            memcpy(skb_push(skb, 2), &len, 2);
+            memcpy(skb_push(skb, ETH_ALEN), saddr, ETH_ALEN);
+            memcpy(skb_push(skb, ETH_ALEN), daddr, ETH_ALEN);
+
+            return 0;
+        }
+        /* VLAN-tagged IP */
+        if ((snap->oui[2] == 0) && (ntohs(snap->protocol) == ETH_P_8021Q))
+        {
+            /*
+             * The translation doesn't change the packet length, so is done in-place.
+             *
+             * Example header (from Std 802.11-2007 Annex M):
+             * AA-AA-03-00-00-00-81-00-87-65-AA-AA-03-00-00-00-08-06
+             * -------SNAP-------p1-p1-ll-ll-------SNAP--------p2-p2
+             * dd-dd-dd-dd-dd-dd-aa-aa-aa-aa-aa-aa-p1-p1-ll-ll-p2-p2
+             * dd-dd-dd-dd-dd-dd-aa-aa-aa-aa-aa-aa-81-00-87-65-08-06
+             */
+            u16 vlan_snap;
+
+            if (payload_length < UF_VLAN_LLC_HEADER_SIZE) {
+                unifi_warning(priv, "VLAN SNAP header too short: %d bytes\n", payload_length);
+                return -1;
+            }
+
+            if (memcmp(payload + 10, vlan_inner_snap, 6)) {
+                unifi_warning(priv, "VLAN malformatted SNAP header.\n");
+                return -1;
+            }
+
+            unifi_trace(priv, UDBG3, "VLAN SNAP: %02x-%02x\n", payload[8], payload[9]);
+            unifi_trace(priv, UDBG3, "VLAN len: %d\n", payload_length);
+
+            /* Create the 802.3 header */
+
+            vlan_snap = *((u16*)(payload + 8));
+
+            /* Create LLC header without byte-swapping */
+            eth->h_proto = snap->protocol;
+
+            memcpy(eth->h_dest, daddr, ETH_ALEN);
+            memcpy(eth->h_source, saddr, ETH_ALEN);
+            *(u16*)(eth + 1) = vlan_snap;
+            return 0;
+        }
+
+        /* it's a SNAP + RFC1042 frame */
+        unifi_trace(priv, UDBG3, "SNAP+RFC1042 len: %d\n", payload_length);
+
+        /* chop SNAP+llc header from skb. */
+        skb_pull(skb, sizeof(llc_snap_hdr_t));
+
+        /* Since skb_pull called above to chop snap+llc, no need to check for headroom
+         * availability before skb_push
+         */
+        /* create 802.3 header at beginning of skb. */
+        eth = (struct ethhdr *)skb_push(skb, ETH_HLEN);
+        memcpy(eth->h_dest, daddr, ETH_ALEN);
+        memcpy(eth->h_source, saddr, ETH_ALEN);
+        /* Copy protocol field without byte-swapping */
+        eth->h_proto = snap->protocol;
+    } else {
+        u16 len;
+
+        /* check for headroom availability before skb_push */
+        if (headroom < (2 * ETH_ALEN + 2)) {
+            unifi_warning(priv, "headroom not available to skb_push ether header\n");
+            return -1;
+        }
+        /* Add 802.3 header and leave full payload */
+        len = htons(skb->len);
+        memcpy(skb_push(skb, 2), &len, 2);
+        memcpy(skb_push(skb, ETH_ALEN), saddr, ETH_ALEN);
+        memcpy(skb_push(skb, ETH_ALEN), daddr, ETH_ALEN);
+
+        return 1;
+    }
+
+    return 0;
+} /* skb_80211_to_ether() */
+
+
+static CsrWifiRouterCtrlPortAction verify_port(unifi_priv_t *priv, unsigned char *address, int queue, u16 interfaceTag)
+{
+#ifdef CSR_NATIVE_LINUX
+#ifdef CSR_SUPPORT_WEXT
+    if (queue == UF_CONTROLLED_PORT_Q) {
+        return priv->wext_conf.block_controlled_port;
+    } else {
+        return CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN;
+    }
+#else
+    return CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN; /* default to open for softmac dev */
+#endif
+#else
+    return uf_sme_port_state(priv, address, queue, interfaceTag);
+#endif
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  prepare_and_add_macheader
+ *
+ *
+ *      These functions adds mac header for packet from netdev
+ *      to UniFi for transmission.
+ *      EAP protocol packets are also appended with Mac header &
+ *      sent using send_ma_pkt_request().
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      skb             Socket buffer containing data packet to transmit
+ *      newSkb          Socket buffer containing data packet + Mac header if no sufficient headroom in skb
+ *      serviceClass    to append QOS control header in Mac header
+ *      bulkdata        if newSkb allocated then bulkdata updated to send to unifi
+ *      interfaceTag    the interfaceID on which activity going on
+ *      daddr           destination address
+ *      saddr           source address
+ *      protection      protection bit set in framce control of mac header
+ *
+ *  Returns:
+ *      Zero on success or error code.
+ * ---------------------------------------------------------------------------
+ */
+
+int prepare_and_add_macheader(unifi_priv_t *priv, struct sk_buff *skb, struct sk_buff *newSkb,
+                              CSR_PRIORITY priority,
+                              bulk_data_param_t *bulkdata,
+                              u16 interfaceTag,
+                              const u8 *daddr,
+                              const u8 *saddr,
+                              u8 protection)
+{
+    u16 fc = 0;
+    u8 qc = 0;
+    u8 macHeaderLengthInBytes = MAC_HEADER_SIZE, *bufPtr = NULL;
+    bulk_data_param_t data_ptrs;
+    CsrResult csrResult;
+    int headroom =0;
+    u8 direction = 0;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    u8 *addressOne;
+    u8 bQosNull = false;
+
+    if (skb == NULL) {
+        unifi_error(priv,"prepare_and_add_macheader: Invalid SKB reference\n");
+        return -1;
+    }
+
+    /* add a MAC header refer: 7.1.3.1 Frame Control field in P802.11REVmb.book */
+    if (priority != CSR_CONTENTION) {
+        /* EAPOL packets don't go as QOS_DATA */
+        if (priority == CSR_MANAGEMENT) {
+            fc |= cpu_to_le16(IEEE802_11_FC_TYPE_DATA);
+        } else {
+            /* Qos Control Field */
+            macHeaderLengthInBytes += QOS_CONTROL_HEADER_SIZE;
+
+            if (skb->len) {
+
+                fc |= cpu_to_le16(IEEE802_11_FC_TYPE_QOS_DATA);
+            } else {
+                fc |= cpu_to_le16(IEEE802_11_FC_TYPE_QOS_NULL);
+                bQosNull = true;
+            }
+        }
+    } else {
+        if(skb->len == 0) {
+            fc |= cpu_to_le16(IEEE802_11_FC_TYPE_NULL);
+        } else {
+            fc |= cpu_to_le16(IEEE802_11_FC_TYPE_DATA);
+        }
+    }
+
+    switch (interfacePriv->interfaceMode)
+    {
+        case  CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+            direction = 2;
+            fc |= cpu_to_le16(IEEE802_11_FC_TO_DS_MASK);
+            break;
+        case  CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            direction = 0;
+            break;
+        case  CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            direction = 1;
+            fc |= cpu_to_le16(IEEE802_11_FC_FROM_DS_MASK);
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_AMP:
+            if (priority == CSR_MANAGEMENT ) {
+
+                direction = 2;
+                fc |= cpu_to_le16(IEEE802_11_FC_TO_DS_MASK);
+            } else {
+                /* Data frames have to use WDS 4 address frames */
+                direction = 3;
+                fc |= cpu_to_le16(IEEE802_11_FC_TO_DS_MASK | IEEE802_11_FC_FROM_DS_MASK);
+                macHeaderLengthInBytes += 6;
+            }
+            break;
+        default:
+            unifi_warning(priv, "prepare_and_add_macheader: Unknown mode %d\n",
+                          interfacePriv->interfaceMode);
+    }
+
+
+    /* If Sta is QOS & HTC is supported then need to set 'order' bit */
+    /* We don't support HT Control for now */
+
+    if(protection) {
+        fc |= cpu_to_le16(IEEE802_11_FC_PROTECTED_MASK);
+    }
+
+    /* check the skb headroom before pushing mac header */
+    headroom = skb_headroom(skb);
+
+    if (headroom < macHeaderLengthInBytes) {
+        unifi_trace(priv, UDBG5,
+                    "prepare_and_add_macheader: Allocate headroom extra %d bytes\n",
+                    macHeaderLengthInBytes);
+
+        csrResult = unifi_net_data_malloc(priv, &data_ptrs.d[0], skb->len + macHeaderLengthInBytes);
+
+        if (csrResult != CSR_RESULT_SUCCESS) {
+            unifi_error(priv, " failed to allocate request_data. in %s func\n", __FUNCTION__);
+            return -1;
+        }
+        newSkb = (struct sk_buff *)(data_ptrs.d[0].os_net_buf_ptr);
+        newSkb->len = skb->len + macHeaderLengthInBytes;
+
+        memcpy((void*)data_ptrs.d[0].os_data_ptr + macHeaderLengthInBytes,
+                skb->data, skb->len);
+
+        bulkdata->d[0].os_data_ptr = newSkb->data;
+        bulkdata->d[0].os_net_buf_ptr = (unsigned char*)newSkb;
+        bulkdata->d[0].data_length = newSkb->len;
+
+        bufPtr = (u8*)data_ptrs.d[0].os_data_ptr;
+
+        /* The old skb will not be used again */
+            kfree_skb(skb);
+    } else {
+
+        /* headroom has sufficient size, so will get proper pointer */
+        bufPtr = (u8*)skb_push(skb, macHeaderLengthInBytes);
+        bulkdata->d[0].os_data_ptr = skb->data;
+        bulkdata->d[0].os_net_buf_ptr = (unsigned char*)skb;
+        bulkdata->d[0].data_length = skb->len;
+    }
+
+    /* Frame the actual MAC header */
+
+    memset(bufPtr, 0, macHeaderLengthInBytes);
+
+    /* copy frameControl field */
+    memcpy(bufPtr, &fc, sizeof(fc));
+    bufPtr += sizeof(fc);
+    macHeaderLengthInBytes -= sizeof(fc);
+
+    /* Duration/ID field which is 2 bytes */
+    bufPtr += 2;
+    macHeaderLengthInBytes -= 2;
+
+    switch(direction)
+    {
+        case 0:
+            /* Its an Ad-Hoc no need to route it through AP */
+            /* Address1: MAC address of the destination from eth header */
+            memcpy(bufPtr, daddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address2: MAC address of the source */
+            memcpy(bufPtr, saddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address3: the BSSID (locally generated in AdHoc (creators Bssid)) */
+            memcpy(bufPtr, &interfacePriv->bssid, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+            break;
+        case 1:
+           /* Address1: MAC address of the actual destination */
+            memcpy(bufPtr, daddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+            /* Address2: The MAC address of the AP */
+            memcpy(bufPtr, &interfacePriv->bssid, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address3: MAC address of the source from eth header */
+            memcpy(bufPtr, saddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+            break;
+        case  2:
+            /* Address1: To AP is the MAC address of the AP to which its associated */
+            memcpy(bufPtr, &interfacePriv->bssid, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address2: MAC address of the source from eth header */
+            memcpy(bufPtr, saddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address3: MAC address of the actual destination on the distribution system */
+            memcpy(bufPtr, daddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+            break;
+        case 3:
+            memcpy(bufPtr, &interfacePriv->bssid, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address2: MAC address of the source from eth header */
+            memcpy(bufPtr, saddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+
+            /* Address3: MAC address of the actual destination on the distribution system */
+            memcpy(bufPtr, daddr, ETH_ALEN);
+            bufPtr += ETH_ALEN;
+            macHeaderLengthInBytes -= ETH_ALEN;
+            break;
+        default:
+            unifi_error(priv,"Unknown direction =%d : Not handled now\n",direction);
+            return -1;
+    }
+    /* 2 bytes of frame control field, appended by firmware */
+    bufPtr += 2;
+    macHeaderLengthInBytes -= 2;
+
+    if (3 == direction) {
+        /* Address4: MAC address of the source */
+        memcpy(bufPtr, saddr, ETH_ALEN);
+        bufPtr += ETH_ALEN;
+        macHeaderLengthInBytes -= ETH_ALEN;
+    }
+
+    /* IF Qos Data or Qos Null Data then set QosControl field */
+    if ((priority != CSR_CONTENTION) && (macHeaderLengthInBytes >= QOS_CONTROL_HEADER_SIZE)) {
+
+        if (priority > 7) {
+            unifi_trace(priv, UDBG1, "data packets priority is more than 7, priority = %x\n", priority);
+            qc |= 7;
+        } else {
+            qc |= priority;
+        }
+        /*assigning address1
+        * Address1 offset taken fromm bufPtr(currently bufPtr pointing to Qos contorl) variable in reverse direction
+        * Address4 don't exit
+        */
+
+        addressOne = bufPtr- ADDRESS_ONE_OFFSET;
+
+        if (addressOne[0] & 0x1) {
+            /* multicast/broadcast frames, no acknowledgement needed */
+            qc |= 1 << 5;
+        }
+        /* non-AP mode only for now */
+        if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_STA ||
+           interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_IBSS ||
+           interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI) {
+           /* In case of STA and IBSS case eosp and txop limit is 0. */
+        } else {
+            if(bQosNull) {
+                qc |= 1 << 4;
+            }
+        }
+
+        /* append Qos control field to mac header */
+        bufPtr[0] = qc;
+        /* txop limit is 0 */
+        bufPtr[1] = 0;
+        macHeaderLengthInBytes -= QOS_CONTROL_HEADER_SIZE;
+    }
+    if (macHeaderLengthInBytes) {
+        unifi_warning(priv, " Mac header not appended properly\n");
+        return -1;
+    }
+    return 0;
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  send_ma_pkt_request
+ *
+ *      These functions send a data packet to UniFi for transmission.
+ *      EAP protocol packets are also sent as send_ma_pkt_request().
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      skb             Socket buffer containing data packet to transmit
+ *      ehdr            Pointer to Ethernet header within skb.
+ *
+ *  Returns:
+ *      Zero on success or error code.
+ * ---------------------------------------------------------------------------
+ */
+
+static int
+send_ma_pkt_request(unifi_priv_t *priv, struct sk_buff *skb, const struct ethhdr *ehdr, CSR_PRIORITY priority)
+{
+    int r;
+    u16 i;
+    u8 eapolStore = FALSE;
+    struct sk_buff *newSkb = NULL;
+    bulk_data_param_t bulkdata;
+    const int proto = ntohs(ehdr->h_proto);
+    u16 interfaceTag;
+    CsrWifiMacAddress peerAddress;
+    CSR_TRANSMISSION_CONTROL transmissionControl = CSR_NO_CONFIRM_REQUIRED;
+    s8 protection;
+    netInterface_priv_t *interfacePriv = NULL;
+    CSR_RATE TransmitRate = (CSR_RATE)0;
+
+    unifi_trace(priv, UDBG5, "entering send_ma_pkt_request\n");
+
+    /* Get the interface Tag by means of source Mac address */
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        if (!memcmp(priv->netdev[i]->dev_addr, ehdr->h_source, ETH_ALEN)) {
+            interfaceTag = i;
+            interfacePriv = priv->interfacePriv[interfaceTag];
+            break;
+        }
+    }
+
+    if (interfacePriv == NULL) {
+        /* No match found - error */
+        interfaceTag = 0;
+        interfacePriv = priv->interfacePriv[interfaceTag];
+        unifi_warning(priv, "Mac address not matching ... debugging needed\n");
+        interfacePriv->stats.tx_dropped++;
+        kfree_skb(skb);
+        return -1;
+    }
+
+    /* Add a SNAP header if necessary */
+    if (skb_add_llc_snap(priv->netdev[interfaceTag], skb, proto) != 0) {
+        /* convert failed */
+        unifi_error(priv, "skb_add_llc_snap failed.\n");
+        kfree_skb(skb);
+        return -1;
+    }
+
+    bulkdata.d[0].os_data_ptr = skb->data;
+    bulkdata.d[0].os_net_buf_ptr = (unsigned char*)skb;
+    bulkdata.d[0].net_buf_length = bulkdata.d[0].data_length = skb->len;
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].os_net_buf_ptr = NULL;
+    bulkdata.d[1].net_buf_length = bulkdata.d[1].data_length = 0;
+
+#ifdef CSR_SUPPORT_SME
+    /* Notify the TA module for the Tx frame  for non AP/P2PGO mode*/
+    if ((interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_AP) &&
+        (interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_P2PGO)) {
+        unifi_ta_sample(priv->card, CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_TX,
+                        &bulkdata.d[0], ehdr->h_source,
+                        priv->netdev[interfaceTag]->dev_addr,
+                        jiffies_to_msecs(jiffies),
+                        0);     /* rate is unknown on tx */
+    }
+#endif /* CSR_SUPPORT_SME */
+
+    if ((proto == ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+            || (proto == ETH_P_WAI)
+#endif
+       )
+    {
+        /* check for m4 detection */
+        if (0 == uf_verify_m4(priv, bulkdata.d[0].os_data_ptr, bulkdata.d[0].data_length)) {
+            eapolStore = TRUE;
+        }
+    }
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    if (proto == ETH_P_WAI)
+     {
+        protection = 0; /*WAI packets always sent unencrypted*/
+     }
+   else
+     {
+#endif
+#ifdef CSR_SUPPORT_SME
+    if ((protection = uf_get_protection_bit_from_interfacemode(priv, interfaceTag, ehdr->h_dest)) < 0) {
+        unifi_warning(priv, "unicast address, but destination not in station record database\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return -1;
+    }
+#else
+    protection = 0;
+#endif
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+   }
+#endif
+
+    /* append Mac header for Eapol as well as data packet */
+    if (prepare_and_add_macheader(priv, skb, newSkb, priority, &bulkdata, interfaceTag, ehdr->h_dest, ehdr->h_source, protection)) {
+        unifi_error(priv, "failed to create MAC header\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return -1;
+    }
+
+    /* RA adrress must contain the immediate destination MAC address that is similiar to
+     * the Address 1 field of 802.11 Mac header here 4 is: (sizeof(framecontrol) + sizeof (durationID))
+     * which is address 1 field
+     */
+    memcpy(peerAddress.a, ((u8 *) bulkdata.d[0].os_data_ptr) + 4, ETH_ALEN);
+
+    unifi_trace(priv, UDBG5, "RA[0]=%x, RA[1]=%x, RA[2]=%x, RA[3]=%x, RA[4]=%x, RA[5]=%x\n",
+                peerAddress.a[0],peerAddress.a[1], peerAddress.a[2], peerAddress.a[3],
+                peerAddress.a[4],peerAddress.a[5]);
+
+
+    if ((proto == ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+            || (proto == ETH_P_WAI)
+#endif
+       )
+    {
+        CSR_SIGNAL signal;
+        CSR_MA_PACKET_REQUEST *req = &signal.u.MaPacketRequest;
+
+        /* initialize signal to zero */
+        memset(&signal, 0, sizeof(CSR_SIGNAL));
+
+        /* Frame MA_PACKET request */
+        signal.SignalPrimitiveHeader.SignalId = CSR_MA_PACKET_REQUEST_ID;
+        signal.SignalPrimitiveHeader.ReceiverProcessId = 0;
+        signal.SignalPrimitiveHeader.SenderProcessId = priv->netdev_client->sender_id;
+
+        transmissionControl = req->TransmissionControl = 0;
+#ifdef CSR_SUPPORT_SME
+        if (eapolStore)
+        {
+            netInterface_priv_t *netpriv = (netInterface_priv_t *)netdev_priv(priv->netdev[interfaceTag]);
+
+            /* Fill the MA-PACKET.req */
+
+            req->Priority = priority;
+            unifi_trace(priv, UDBG3, "Tx Frame with Priority: %x\n", req->Priority);
+
+            /* rate selected by firmware */
+            req->TransmitRate = 0;
+            req->HostTag = CSR_WIFI_EAPOL_M4_HOST_TAG;
+            /* RA address matching with address 1 of Mac header */
+            memcpy(req->Ra.x, ((u8 *) bulkdata.d[0].os_data_ptr) + 4, ETH_ALEN);
+
+            spin_lock(&priv->m4_lock);
+            /* Store the M4-PACKET.req for later */
+            interfacePriv->m4_signal = signal;
+            interfacePriv->m4_bulk_data.net_buf_length = bulkdata.d[0].net_buf_length;
+            interfacePriv->m4_bulk_data.data_length = bulkdata.d[0].data_length;
+            interfacePriv->m4_bulk_data.os_data_ptr = bulkdata.d[0].os_data_ptr;
+            interfacePriv->m4_bulk_data.os_net_buf_ptr = bulkdata.d[0].os_net_buf_ptr;
+            spin_unlock(&priv->m4_lock);
+
+            /* Signal the workqueue to call CsrWifiRouterCtrlM4ReadyToSendIndSend().
+             * It cannot be called directly from the tx path because it
+             * does a non-atomic kmalloc via the framework's CsrPmemAlloc().
+             */
+            queue_work(priv->unifi_workqueue, &netpriv->send_m4_ready_task);
+
+            return 0;
+        }
+#endif
+    }/*EAPOL or WAI packet*/
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    if ((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) && \
+        (priv->wapi_unicast_filter) && \
+        (proto != ETH_P_PAE) && \
+        (proto != ETH_P_WAI) && \
+        (skb->len > 0))
+    {
+        CSR_SIGNAL signal;
+        CSR_MA_PACKET_REQUEST *req = &signal.u.MaPacketRequest;
+        netInterface_priv_t *netpriv = (netInterface_priv_t *)netdev_priv(priv->netdev[interfaceTag]);
+
+        unifi_trace(priv, UDBG4, "send_ma_pkt_request() - WAPI unicast data packet when USKID = 1 \n");
+
+        /* initialize signal to zero */
+        memset(&signal, 0, sizeof(CSR_SIGNAL));
+        /* Frame MA_PACKET request */
+        signal.SignalPrimitiveHeader.SignalId = CSR_MA_PACKET_REQUEST_ID;
+        signal.SignalPrimitiveHeader.ReceiverProcessId = 0;
+        signal.SignalPrimitiveHeader.SenderProcessId = priv->netdev_client->sender_id;
+
+        /* Fill the MA-PACKET.req */
+        req->TransmissionControl = 0;
+        req->Priority = priority;
+        unifi_trace(priv, UDBG3, "Tx Frame with Priority: %x\n", req->Priority);
+        req->TransmitRate = (CSR_RATE) 0; /* rate selected by firmware */
+        req->HostTag = 0xffffffff;        /* Ask for a new HostTag */
+        /* RA address matching with address 1 of Mac header */
+        memcpy(req->Ra.x, ((u8 *) bulkdata.d[0].os_data_ptr) + 4, ETH_ALEN);
+
+        /* Store the M4-PACKET.req for later */
+        spin_lock(&priv->wapi_lock);
+        interfacePriv->wapi_unicast_ma_pkt_sig = signal;
+        interfacePriv->wapi_unicast_bulk_data.net_buf_length = bulkdata.d[0].net_buf_length;
+        interfacePriv->wapi_unicast_bulk_data.data_length = bulkdata.d[0].data_length;
+        interfacePriv->wapi_unicast_bulk_data.os_data_ptr = bulkdata.d[0].os_data_ptr;
+        interfacePriv->wapi_unicast_bulk_data.os_net_buf_ptr = bulkdata.d[0].os_net_buf_ptr;
+        spin_unlock(&priv->wapi_lock);
+
+        /* Signal the workqueue to call CsrWifiRouterCtrlWapiUnicastTxEncryptIndSend().
+         * It cannot be called directly from the tx path because it
+         * does a non-atomic kmalloc via the framework's CsrPmemAlloc().
+         */
+        queue_work(priv->unifi_workqueue, &netpriv->send_pkt_to_encrypt);
+
+        return 0;
+    }
+#endif
+
+    if(priv->cmanrTestMode)
+    {
+        TransmitRate = priv->cmanrTestModeTransmitRate;
+        unifi_trace(priv, UDBG2, "send_ma_pkt_request: cmanrTestModeTransmitRate = %d TransmitRate=%d\n",
+                    priv->cmanrTestModeTransmitRate,
+                    TransmitRate
+                   );
+    }
+
+    /* Send UniFi msg */
+    /* Here hostTag is been sent as 0xffffffff, its been appended properly while framing MA-Packet request in pdu_processing.c file */
+    r = uf_process_ma_packet_req(priv,
+                                 peerAddress.a,
+                                 0xffffffff,  /* Ask for a new HostTag */
+                                 interfaceTag,
+                                 transmissionControl,
+                                 TransmitRate,
+                                 priority,
+                                 priv->netdev_client->sender_id,
+                                 &bulkdata);
+
+    if (r) {
+        unifi_trace(priv, UDBG1, "(HIP validation failure) r = %x\n", r);
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return -1;
+    }
+
+    unifi_trace(priv, UDBG3, "leaving send_ma_pkt_request, UNITDATA result code = %d\n", r);
+
+    return r;
+} /* send_ma_pkt_request() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_net_xmit
+ *
+ *      This function is called by the higher level stack to transmit an
+ *      ethernet packet.
+ *
+ *  Arguments:
+ *      skb     Ethernet packet to send.
+ *      dev     Pointer to the linux net device.
+ *
+ *  Returns:
+ *      0   on success (packet was consumed, not necessarily transmitted)
+ *      1   if packet was requeued
+ *     -1   on error
+ *
+ *
+ *  Notes:
+ *      The controlled port is handled in the qdisc dequeue handler.
+ * ---------------------------------------------------------------------------
+ */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+static netdev_tx_t
+#else
+static int
+#endif
+uf_net_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct ethhdr ehdr;
+    int proto, port;
+    int result;
+    static tx_signal_handler tx_handler;
+    CSR_PRIORITY priority;
+#if !defined (CONFIG_NET_SCHED) || (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28))
+    CsrWifiRouterCtrlPortAction port_action;
+#endif /* CONFIG_NET_SCHED */
+
+    func_enter();
+
+    unifi_trace(priv, UDBG5, "unifi_net_xmit: skb = %x\n", skb);
+
+    memcpy(&ehdr, skb->data, ETH_HLEN);
+    proto = ntohs(ehdr.h_proto);
+    priority = get_packet_priority(priv, skb, &ehdr, interfacePriv);
+
+    /* All frames are sent as MA-PACKET.req (EAPOL also) */
+    tx_handler = send_ma_pkt_request;
+
+    /* 802.1x - apply controlled/uncontrolled port rules */
+    if ((proto != ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+            && (proto != ETH_P_WAI)
+#endif
+       ) {
+        port = UF_CONTROLLED_PORT_Q;
+    } else {
+        /* queue 4 */
+        port = UF_UNCONTROLLED_PORT_Q;
+    }
+
+#if defined (CONFIG_NET_SCHED) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28))
+    /* Remove the ethernet header */
+    skb_pull(skb, ETH_HLEN);
+    result = tx_handler(priv, skb, &ehdr, priority);
+#else
+    /* Uncontrolled port rules apply */
+    port_action = verify_port(priv
+        , (((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode)||(CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI== interfacePriv->interfaceMode))? interfacePriv->bssid.a: ehdr.h_dest)
+        , port
+        , interfacePriv->InterfaceTag);
+
+    if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
+        unifi_trace(priv, UDBG5,
+                    "uf_net_xmit: %s controlled port open\n",
+                    port ? "" : "un");
+        /* Remove the ethernet header */
+        skb_pull(skb, ETH_HLEN);
+        result = tx_handler(priv, skb, &ehdr, priority);
+    } else {
+
+        /* Discard the packet if necessary */
+        unifi_trace(priv, UDBG2,
+                "uf_net_xmit: %s controlled port %s\n",
+                port ? "" : "un", port_action==CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_BLOCK ? "blocked" : "closed");
+        interfacePriv->stats.tx_dropped++;
+        kfree_skb(skb);
+
+        func_exit();
+        return NETDEV_TX_OK;
+    }
+#endif /* CONFIG_NET_SCHED */
+
+    if (result == NETDEV_TX_OK) {
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    	/* Don't update the tx stats when the pkt is to be sent for sw encryption*/
+    	if (!((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) &&
+              (priv->wapi_unicast_filter == 1)))
+        {
+            dev->trans_start = jiffies;
+            /* Should really count tx stats in the UNITDATA.status signal but
+             * that doesn't have the length.
+             */
+            interfacePriv->stats.tx_packets++;
+            /* count only the packet payload */
+            interfacePriv->stats.tx_bytes += skb->len;
+
+        }
+#else
+    	dev->trans_start = jiffies;
+
+        /*
+         * Should really count tx stats in the UNITDATA.status signal but
+         * that doesn't have the length.
+         */
+        interfacePriv->stats.tx_packets++;
+        /* count only the packet payload */
+        interfacePriv->stats.tx_bytes += skb->len;
+#endif
+    } else if (result < 0) {
+
+        /* Failed to send: fh queue was full, and the skb was discarded.
+         * Return OK to indicate that the buffer was consumed, to stop the
+         * kernel re-transmitting the freed buffer.
+         */
+        interfacePriv->stats.tx_dropped++;
+        unifi_trace(priv, UDBG1, "unifi_net_xmit: (Packet Drop), dropped count = %x\n", interfacePriv->stats.tx_dropped);
+        result = NETDEV_TX_OK;
+    }
+
+    /* The skb will have been freed by send_XXX_request() */
+
+    func_exit();
+    return result;
+} /* uf_net_xmit() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_pause_xmit
+ *  unifi_restart_xmit
+ *
+ *      These functions are called from the UniFi core to control the flow
+ *      of packets from the upper layers.
+ *      unifi_pause_xmit() is called when the internal queue is full and
+ *      should take action to stop unifi_ma_unitdata() being called.
+ *      When the queue has drained, unifi_restart_xmit() will be called to
+ *      re-enable the flow of packets for transmission.
+ *
+ *  Arguments:
+ *      ospriv          OS private context pointer.
+ *
+ *  Returns:
+ *      unifi_pause_xmit() is called from interrupt context.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_pause_xmit(void *ospriv, unifi_TrafficQueue queue)
+{
+    unifi_priv_t *priv = ospriv;
+    int i; /* used as a loop counter */
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "Stopping queue %d\n", queue);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
+    {
+        if (netif_running(priv->netdev[i]))
+        {
+            netif_stop_subqueue(priv->netdev[i], (u16)queue);
+        }
+    }
+#else
+#ifdef ALLOW_Q_PAUSE
+    unifi_trace(priv, UDBG2, "Stopping netif\n");
+    /* stop the traffic from all the interfaces. */
+    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
+    {
+        if (netif_running(priv->netdev[i])) {
+            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[i]);
+        }
+    }
+#else
+    if (net_is_tx_q_paused(priv, queue)) {
+        unifi_trace(priv, UDBG2, "Queue already stopped\n");
+        return;
+    }
+    net_tx_q_pause(priv, queue);
+#endif
+#endif
+
+#ifdef CSR_SUPPORT_SME
+    if(queue<=3) {
+        routerStartBuffering(priv,queue);
+        unifi_trace(priv,UDBG2,"Start buffering %d\n", queue);
+     } else {
+        routerStartBuffering(priv,0);
+        unifi_error(priv, "Start buffering %d defaulting to 0\n", queue);
+     }
+#endif
+    func_exit();
+
+} /* unifi_pause_xmit() */
+
+void
+unifi_restart_xmit(void *ospriv, unifi_TrafficQueue queue)
+{
+    unifi_priv_t *priv = ospriv;
+    int i=0; /* used as a loop counter */
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "Waking queue %d\n", queue);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
+    {
+        if (netif_running(priv->netdev[i]))
+        {
+            netif_wake_subqueue(priv->netdev[i], (u16)queue);
+        }
+    }
+#else
+#ifdef ALLOW_Q_PAUSE
+    /* Need to supply queue number depending on Kernel support */
+    /* Resume the traffic from all the interfaces */
+    for(i=0;i<CSR_WIFI_NUM_INTERFACES;i++)
+    {
+        if (netif_running(priv->netdev[i])) {
+            UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[i]);
+        }
+    }
+#else
+    if (!(net_is_tx_q_paused(priv, queue))) {
+        unifi_trace(priv, UDBG2, "Queue already running\n");
+        func_exit();
+        return;
+    }
+    net_tx_q_unpause(priv, queue);
+#endif
+#endif
+
+#ifdef CSR_SUPPORT_SME
+    if(queue <=3) {
+        routerStopBuffering(priv,queue);
+        uf_send_buffered_frames(priv,queue);
+    } else {
+        routerStopBuffering(priv,0);
+        uf_send_buffered_frames(priv,0);
+    }
+#endif
+    func_exit();
+} /* unifi_restart_xmit() */
+
+
+static void
+indicate_rx_skb(unifi_priv_t *priv, u16 ifTag, u8* dst_a, u8* src_a, struct sk_buff *skb, CSR_SIGNAL *signal,
+                bulk_data_param_t *bulkdata)
+{
+    int r, sr = 0;
+    struct net_device *dev;
+
+#ifdef CSR_SUPPORT_SME
+    llc_snap_hdr_t *snap;
+
+    snap = (llc_snap_hdr_t *)skb->data;
+
+    sr = _identify_sme_ma_pkt_ind(priv,
+                                  snap->oui, ntohs(snap->protocol),
+                                  signal,
+                                  bulkdata,
+                                  dst_a, src_a );
+#endif
+
+    /*
+     * Decapsulate any SNAP header and
+     * prepend an ethernet header so that the skb manipulation and ARP
+     * stuff works.
+     */
+    r = skb_80211_to_ether(priv, skb, dst_a, src_a,
+                           signal, bulkdata);
+    if (r == -1) {
+        /* Drop the packet and return */
+        priv->interfacePriv[ifTag]->stats.rx_errors++;
+        priv->interfacePriv[ifTag]->stats.rx_frame_errors++;
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        unifi_notice(priv, "indicate_rx_skb: Discard unknown frame.\n");
+        func_exit();
+        return;
+    }
+
+    /* Handle the case where packet is sent up through the subscription
+     * API but should not be given to the network stack (AMP PAL case)
+     * LLC header is different from WiFi and the packet has been subscribed for
+     */
+    if (r == 1 && sr == 1) {
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        unifi_trace(priv, UDBG5, "indicate_rx_skb: Data given to subscription"
+                "API, not being given to kernel\n");
+        func_exit();
+        return;
+    }
+
+    dev = priv->netdev[ifTag];
+    /* Now we look like a regular ethernet frame */
+    /* Fill in SKB meta data */
+    skb->dev = dev;
+    skb->protocol = eth_type_trans(skb, dev);
+    skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+    /* Test for an overlength frame */
+    if (skb->len > (dev->mtu + ETH_HLEN)) {
+        /* A bogus length ethfrm has been encap'd. */
+        /* Is someone trying an oflow attack? */
+        unifi_error(priv, "%s: oversize frame (%d > %d)\n",
+                    dev->name,
+                    skb->len, dev->mtu + ETH_HLEN);
+
+        /* Drop the packet and return */
+        priv->interfacePriv[ifTag]->stats.rx_errors++;
+        priv->interfacePriv[ifTag]->stats.rx_length_errors++;
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+
+    if(priv->cmanrTestMode)
+    {
+        const CSR_MA_PACKET_INDICATION *pkt_ind = &signal->u.MaPacketIndication;
+        priv->cmanrTestModeTransmitRate = pkt_ind->ReceivedRate;
+        unifi_trace(priv, UDBG2, "indicate_rx_skb: cmanrTestModeTransmitRate=%d\n", priv->cmanrTestModeTransmitRate);
+    }
+
+    /* Pass SKB up the stack */
+#ifdef CSR_WIFI_USE_NETIF_RX
+        netif_rx(skb);
+#else
+        netif_rx_ni(skb);
+#endif
+
+    if (dev != NULL) {
+        dev->last_rx = jiffies;
+    }
+
+    /* Bump rx stats */
+    priv->interfacePriv[ifTag]->stats.rx_packets++;
+    priv->interfacePriv[ifTag]->stats.rx_bytes += bulkdata->d[0].data_length;
+
+    func_exit();
+    return;
+}
+
+void
+uf_process_rx_pending_queue(unifi_priv_t *priv, int queue,
+                            CsrWifiMacAddress source_address,
+                            int indicate, u16 interfaceTag)
+{
+    rx_buffered_packets_t *rx_q_item;
+    struct list_head *rx_list;
+    struct list_head *n;
+    struct list_head *l_h;
+    static const CsrWifiMacAddress broadcast_address = {{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_process_rx_pending_queue bad interfaceTag\n");
+        return;
+    }
+
+    if (queue == UF_CONTROLLED_PORT_Q) {
+        rx_list = &interfacePriv->rx_controlled_list;
+    } else {
+        rx_list = &interfacePriv->rx_uncontrolled_list;
+    }
+
+    down(&priv->rx_q_sem);
+    list_for_each_safe(l_h, n, rx_list) {
+        rx_q_item = list_entry(l_h, rx_buffered_packets_t, q);
+
+        /* Validate against the source address */
+        if (memcmp(broadcast_address.a, source_address.a, ETH_ALEN) &&
+                memcmp(rx_q_item->sa.a, source_address.a, ETH_ALEN)) {
+
+            unifi_trace(priv, UDBG2,
+                        "uf_process_rx_pending_queue: Skipping sa=%02X%02X%02X%02X%02X%02X skb=%p, bulkdata=%p\n",
+                        rx_q_item->sa.a[0], rx_q_item->sa.a[1],
+                        rx_q_item->sa.a[2], rx_q_item->sa.a[3],
+                        rx_q_item->sa.a[4], rx_q_item->sa.a[5],
+                        rx_q_item->skb, &rx_q_item->bulkdata.d[0]);
+            continue;
+        }
+
+        list_del(l_h);
+
+
+        unifi_trace(priv, UDBG2,
+                    "uf_process_rx_pending_queue: Was Blocked skb=%p, bulkdata=%p\n",
+                    rx_q_item->skb, &rx_q_item->bulkdata);
+
+        if (indicate) {
+            indicate_rx_skb(priv, interfaceTag, rx_q_item->da.a, rx_q_item->sa.a, rx_q_item->skb, &rx_q_item->signal, &rx_q_item->bulkdata);
+        } else {
+            interfacePriv->stats.rx_dropped++;
+            unifi_net_data_free(priv, &rx_q_item->bulkdata.d[0]);
+        }
+
+        /* It is our resposibility to free the Rx structure object. */
+        kfree(rx_q_item);
+    }
+    up(&priv->rx_q_sem);
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_resume_data_plane
+ *
+ *      Is called when the (un)controlled port is set to open,
+ *      to notify the network stack to schedule for transmission
+ *      any packets queued in the qdisk while port was closed and
+ *      indicated to the stack any packets buffered in the Rx queues.
+ *
+ *  Arguments:
+ *      priv        Pointer to device private struct
+ *
+ *  Returns:
+ * ---------------------------------------------------------------------------
+ */
+void
+uf_resume_data_plane(unifi_priv_t *priv, int queue,
+                     CsrWifiMacAddress peer_address,
+                     u16 interfaceTag)
+{
+#ifdef CSR_SUPPORT_WEXT
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+#endif
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_resume_data_plane bad interfaceTag\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG2, "Resuming netif\n");
+
+    /*
+     * If we are waiting for the net device to enter the up state, don't
+     * process the rx queue yet as it will be done by the callback when
+     * the device is ready.
+     */
+#ifdef CSR_SUPPORT_WEXT
+    if (!interfacePriv->wait_netdev_change)
+#endif
+    {
+#ifdef CONFIG_NET_SCHED
+        if (netif_running(priv->netdev[interfaceTag])) {
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
+            netif_tx_schedule_all(priv->netdev[interfaceTag]);
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+            netif_schedule_queue(netdev_get_tx_queue(priv->netdev[interfaceTag], 0));
+#else
+            netif_schedule(priv->netdev[interfaceTag]);
+#endif /* LINUX_VERSION_CODE */
+        }
+#endif
+        uf_process_rx_pending_queue(priv, queue, peer_address, 1,interfaceTag);
+    }
+} /* uf_resume_data_plane() */
+
+
+void uf_free_pending_rx_packets(unifi_priv_t *priv, int queue, CsrWifiMacAddress peer_address,u16 interfaceTag)
+{
+    uf_process_rx_pending_queue(priv, queue, peer_address, 0,interfaceTag);
+
+} /* uf_free_pending_rx_packets() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_rx
+ *
+ *      Reformat a UniFi data received packet into a p80211 packet and
+ *      pass it up the protocol stack.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+unifi_rx(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata)
+{
+    u16 interfaceTag;
+    bulk_data_desc_t *pData;
+    const CSR_MA_PACKET_INDICATION *pkt_ind = &signal->u.MaPacketIndication;
+    struct sk_buff *skb;
+    CsrWifiRouterCtrlPortAction port_action;
+    u8 dataFrameType;
+    int proto;
+    int queue;
+
+    u8 da[ETH_ALEN], sa[ETH_ALEN];
+    u8 toDs, fromDs, frameType, macHeaderLengthInBytes = MAC_HEADER_SIZE;
+    u16 frameControl;
+    netInterface_priv_t *interfacePriv;
+    struct ethhdr ehdr;
+
+    func_enter();
+
+    interfaceTag = (pkt_ind->VirtualInterfaceIdentifier & 0xff);
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    /* Sanity check that the VIF refers to a sensible interface */
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "%s: MA-PACKET indication with bad interfaceTag %d\n", __FUNCTION__, interfaceTag);
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+    /* Sanity check that the VIF refers to an allocated netdev */
+    if (!interfacePriv->netdev_registered)
+    {
+        unifi_error(priv, "%s: MA-PACKET indication with unallocated interfaceTag %d\n", __FUNCTION__, interfaceTag);
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+    if (bulkdata->d[0].data_length == 0) {
+        unifi_warning(priv, "%s: MA-PACKET indication with zero bulk data\n", __FUNCTION__);
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+
+    skb = (struct sk_buff*)bulkdata->d[0].os_net_buf_ptr;
+    skb->len = bulkdata->d[0].data_length;
+
+    /* Point to the addresses */
+    toDs = (skb->data[1] & 0x01) ? 1 : 0;
+    fromDs = (skb->data[1] & 0x02) ? 1 : 0;
+
+    memcpy(da,(skb->data+4+toDs*12),ETH_ALEN);/* Address1 or 3 */
+    memcpy(sa,(skb->data+10+fromDs*(6+toDs*8)),ETH_ALEN); /* Address2, 3 or 4 */
+
+
+    pData = &bulkdata->d[0];
+    frameControl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(pData->os_data_ptr);
+    frameType = ((frameControl & 0x000C) >> 2);
+
+    dataFrameType =((frameControl & 0x00f0) >> 4);
+    unifi_trace(priv, UDBG6,
+                "%s: Receive Data Frame Type %d \n", __FUNCTION__,dataFrameType);
+
+    switch(dataFrameType)
+    {
+        case QOS_DATA:
+        case QOS_DATA_NULL:
+            /* If both are set then the Address4 exists (only for AP) */
+            if (fromDs && toDs)
+            {
+                /* 6 is the size of Address4 field */
+                macHeaderLengthInBytes += (QOS_CONTROL_HEADER_SIZE + 6);
+            }
+            else
+            {
+                macHeaderLengthInBytes += QOS_CONTROL_HEADER_SIZE;
+            }
+
+            /* If order bit set then HT control field is the part of MAC header */
+            if (frameControl & FRAME_CONTROL_ORDER_BIT)
+                macHeaderLengthInBytes += HT_CONTROL_HEADER_SIZE;
+            break;
+        default:
+            if (fromDs && toDs)
+                macHeaderLengthInBytes += 6;
+    }
+
+    /* Prepare the ethernet header from snap header of skb data */
+    switch(dataFrameType)
+    {
+        case DATA_NULL:
+        case QOS_DATA_NULL:
+            /* This is for only queue info fetching, EAPOL wont come as
+             * null data so the proto is initialized as zero
+             */
+            proto = 0x0;
+            break;
+        default:
+            {
+                llc_snap_hdr_t *snap;
+                /* Fetch a snap header to find protocol (for IPV4/IPV6 packets
+                 * the snap header fetching offset is same)
+                 */
+                snap = (llc_snap_hdr_t *) (skb->data + macHeaderLengthInBytes);
+
+                /* prepare the ethernet header from the snap header & addresses */
+                ehdr.h_proto = snap->protocol;
+                memcpy(ehdr.h_dest, da, ETH_ALEN);
+                memcpy(ehdr.h_source, sa, ETH_ALEN);
+            }
+            proto = ntohs(ehdr.h_proto);
+    }
+    unifi_trace(priv, UDBG3, "in unifi_rx protocol from snap header = 0x%x\n", proto);
+
+    if ((proto != ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+            && (proto != ETH_P_WAI)
+#endif
+       ) {
+        queue = UF_CONTROLLED_PORT_Q;
+    } else {
+        queue = UF_UNCONTROLLED_PORT_Q;
+    }
+
+    port_action = verify_port(priv, (unsigned char*)sa, queue, interfaceTag);
+    unifi_trace(priv, UDBG3, "in unifi_rx port action is = 0x%x & queue = %x\n", port_action, queue);
+
+#ifdef CSR_SUPPORT_SME
+    /* Notify the TA module for the Rx frame for non P2PGO and AP cases*/
+    if((interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_AP) &&
+            (interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_P2PGO))
+    {
+        /* Remove MAC header of length(macHeaderLengthInBytes) before sampling */
+        skb_pull(skb, macHeaderLengthInBytes);
+        pData->os_data_ptr = skb->data;
+        pData->data_length -= macHeaderLengthInBytes;
+
+        if (pData->data_length) {
+            unifi_ta_sample(priv->card, CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_RX,
+                            &bulkdata->d[0],
+                            sa, priv->netdev[interfaceTag]->dev_addr,
+                            jiffies_to_msecs(jiffies),
+                            pkt_ind->ReceivedRate);
+        }
+    } else {
+
+        /* AP/P2PGO specific handling here */
+        CsrWifiRouterCtrlStaInfo_t * srcStaInfo =
+            CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv,sa,interfaceTag);
+
+        /* Defensive check only; Source address is already checked in
+        process_ma_packet_ind and we should have a valid source address here */
+
+         if(srcStaInfo == NULL) {
+            CsrWifiMacAddress peerMacAddress;
+            /* Unknown data PDU */
+            memcpy(peerMacAddress.a,sa,ETH_ALEN);
+            unifi_trace(priv, UDBG1, "%s: Unexpected frame from peer = %x:%x:%x:%x:%x:%x\n", __FUNCTION__,
+            sa[0], sa[1],sa[2], sa[3], sa[4],sa[5]);
+            CsrWifiRouterCtrlUnexpectedFrameIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,interfaceTag,peerMacAddress);
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            func_exit();
+            return;
+        }
+
+       /* For AP GO mode, don't store the PDUs */
+        if (port_action != CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
+            /* Drop the packet and return */
+            CsrWifiMacAddress peerMacAddress;
+            memcpy(peerMacAddress.a,sa,ETH_ALEN);
+            unifi_trace(priv, UDBG3, "%s: Port is not open: unexpected frame from peer = %x:%x:%x:%x:%x:%x\n",
+                        __FUNCTION__, sa[0], sa[1],sa[2], sa[3], sa[4],sa[5]);
+
+            CsrWifiRouterCtrlUnexpectedFrameIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,interfaceTag,peerMacAddress);
+            interfacePriv->stats.rx_dropped++;
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            unifi_notice(priv, "%s: Dropping packet, proto=0x%04x, %s port\n", __FUNCTION__,
+                         proto, queue ? "Controlled" : "Un-controlled");
+            func_exit();
+            return;
+        }
+
+         /* Qos NULL/Data NULL  are freed here and not processed further */
+        if((dataFrameType == QOS_DATA_NULL) || (dataFrameType == DATA_NULL)){
+            unifi_trace(priv, UDBG5, "%s: Null Frame Received and Freed\n", __FUNCTION__);
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            func_exit();
+            return;
+        }
+
+        /* Now we have done with MAC header so proceed with the real data part*/
+        /* This function takes care of appropriate routing for AP/P2PGO case*/
+        /* the function hadnles following things
+           2. Routing the PDU to appropriate location
+           3. Error case handling
+           */
+        if(!(uf_ap_process_data_pdu(priv, skb, &ehdr, srcStaInfo,
+             signal,
+             bulkdata,
+             macHeaderLengthInBytes)))
+        {
+            func_exit();
+            return;
+        }
+        unifi_trace(priv, UDBG5, "unifi_rx: no specific AP handling process as normal frame, MAC Header len %d\n",macHeaderLengthInBytes);
+        /* Remove the MAC header for subsequent conversion */
+        skb_pull(skb, macHeaderLengthInBytes);
+        pData->os_data_ptr = skb->data;
+        pData->data_length -= macHeaderLengthInBytes;
+        pData->os_net_buf_ptr = (unsigned char*)skb;
+        pData->net_buf_length = skb->len;
+    }
+#endif /* CSR_SUPPORT_SME */
+
+
+    /* Now that the MAC header is removed, null-data frames have zero length
+     * and can be dropped
+     */
+    if (pData->data_length == 0) {
+        if (((frameControl & 0x00f0) >> 4) != QOS_DATA_NULL &&
+            ((frameControl & 0x00f0) >> 4) != DATA_NULL) {
+            unifi_trace(priv, UDBG1, "Zero length frame, but not null-data %04x\n", frameControl);
+        }
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+    if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD) {
+        /* Drop the packet and return */
+        interfacePriv->stats.rx_dropped++;
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        unifi_notice(priv, "%s: Dropping packet, proto=0x%04x, %s port\n",
+                     __FUNCTION__, proto, queue ? "controlled" : "uncontrolled");
+        func_exit();
+        return;
+    } else if ( (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_BLOCK) ||
+                   (interfacePriv->connected != UnifiConnected) ) {
+
+        /* Buffer the packet into the Rx queues */
+        rx_buffered_packets_t *rx_q_item;
+        struct list_head *rx_list;
+
+        rx_q_item = (rx_buffered_packets_t *)kmalloc(sizeof(rx_buffered_packets_t),
+                GFP_KERNEL);
+        if (rx_q_item == NULL) {
+            unifi_error(priv, "%s: Failed to allocate %d bytes for rx packet record\n",
+                        __FUNCTION__, sizeof(rx_buffered_packets_t));
+            interfacePriv->stats.rx_dropped++;
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            func_exit();
+            return;
+        }
+
+        INIT_LIST_HEAD(&rx_q_item->q);
+        rx_q_item->bulkdata = *bulkdata;
+        rx_q_item->skb = skb;
+        rx_q_item->signal = *signal;
+        memcpy(rx_q_item->sa.a, sa, ETH_ALEN);
+        memcpy(rx_q_item->da.a, da, ETH_ALEN);
+        unifi_trace(priv, UDBG2, "%s: Blocked skb=%p, bulkdata=%p\n",
+                    __FUNCTION__, rx_q_item->skb, &rx_q_item->bulkdata);
+
+        if (queue == UF_CONTROLLED_PORT_Q) {
+            rx_list = &interfacePriv->rx_controlled_list;
+        } else {
+            rx_list = &interfacePriv->rx_uncontrolled_list;
+        }
+
+        /* Add to tail of packets queue */
+        down(&priv->rx_q_sem);
+        list_add_tail(&rx_q_item->q, rx_list);
+        up(&priv->rx_q_sem);
+
+        func_exit();
+        return;
+
+    }
+
+    indicate_rx_skb(priv, interfaceTag, da, sa, skb, signal, bulkdata);
+
+    func_exit();
+
+} /* unifi_rx() */
+
+static void process_ma_packet_cfm(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata)
+{
+    u16 interfaceTag;
+    const CSR_MA_PACKET_CONFIRM *pkt_cfm = &signal->u.MaPacketConfirm;
+    netInterface_priv_t *interfacePriv;
+
+    func_enter();
+    interfaceTag = (pkt_cfm->VirtualInterfaceIdentifier & 0xff);
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    /* Sanity check that the VIF refers to a sensible interface */
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "%s: MA-PACKET confirm with bad interfaceTag %d\n", __FUNCTION__, interfaceTag);
+        func_exit();
+        return;
+    }
+#ifdef CSR_SUPPORT_SME
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+
+        uf_process_ma_pkt_cfm_for_ap(priv,interfaceTag,pkt_cfm);
+    } else if (interfacePriv->m4_sent && (pkt_cfm->HostTag == interfacePriv->m4_hostTag)) {
+        /* Check if this is a confirm for EAPOL M4 frame and we need to send transmistted ind*/
+        CsrResult result = pkt_cfm->TransmissionStatus == CSR_TX_SUCCESSFUL?CSR_RESULT_SUCCESS:CSR_RESULT_FAILURE;
+        CsrWifiMacAddress peerMacAddress;
+        memcpy(peerMacAddress.a, interfacePriv->m4_signal.u.MaPacketRequest.Ra.x, ETH_ALEN);
+
+        unifi_trace(priv, UDBG1, "%s: Sending M4 Transmit CFM\n", __FUNCTION__);
+        CsrWifiRouterCtrlM4TransmittedIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0,
+                                              interfaceTag,
+                                              peerMacAddress,
+                                              result);
+        interfacePriv->m4_sent = FALSE;
+        interfacePriv->m4_hostTag = 0xffffffff;
+    }
+#endif
+    func_exit();
+    return;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_rx
+ *
+ *      Reformat a UniFi data received packet into a p80211 packet and
+ *      pass it up the protocol stack.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void process_ma_packet_ind(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata)
+{
+    u16 interfaceTag;
+    bulk_data_desc_t *pData;
+    CSR_MA_PACKET_INDICATION *pkt_ind = (CSR_MA_PACKET_INDICATION*)&signal->u.MaPacketIndication;
+    struct sk_buff *skb;
+    u16 frameControl;
+    netInterface_priv_t *interfacePriv;
+    u8 da[ETH_ALEN], sa[ETH_ALEN];
+    u8 *bssid = NULL, *ba_addr = NULL;
+    u8 toDs, fromDs, frameType;
+    u8 i =0;
+
+#ifdef CSR_SUPPORT_SME
+    u8 dataFrameType = 0;
+    u8 powerSaveChanged = FALSE;
+    u8 pmBit = 0;
+    CsrWifiRouterCtrlStaInfo_t *srcStaInfo = NULL;
+    u16 qosControl;
+
+#endif
+
+    func_enter();
+
+    interfaceTag = (pkt_ind->VirtualInterfaceIdentifier & 0xff);
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+
+    /* Sanity check that the VIF refers to a sensible interface */
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "%s: MA-PACKET indication with bad interfaceTag %d\n", __FUNCTION__, interfaceTag);
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+    /* Sanity check that the VIF refers to an allocated netdev */
+    if (!interfacePriv->netdev_registered)
+    {
+        unifi_error(priv, "%s: MA-PACKET indication with unallocated interfaceTag %d\n", __FUNCTION__, interfaceTag);
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+    if (bulkdata->d[0].data_length == 0) {
+        unifi_warning(priv, "%s: MA-PACKET indication with zero bulk data\n", __FUNCTION__);
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+    /* For monitor mode we need to pass this indication to the registered application
+    handle this seperately*/
+    /* MIC failure is already taken care of so no need to send the PDUs which are not successfully received in non-monitor mode*/
+    if(pkt_ind->ReceptionStatus != CSR_RX_SUCCESS)
+    {
+        unifi_warning(priv, "%s: MA-PACKET indication with status = %d\n",__FUNCTION__, pkt_ind->ReceptionStatus);
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+
+    skb = (struct sk_buff*)bulkdata->d[0].os_net_buf_ptr;
+    skb->len = bulkdata->d[0].data_length;
+
+    /* Point to the addresses */
+    toDs = (skb->data[1] & 0x01) ? 1 : 0;
+    fromDs = (skb->data[1] & 0x02) ? 1 : 0;
+
+    memcpy(da,(skb->data+4+toDs*12),ETH_ALEN);/* Address1 or 3 */
+    memcpy(sa,(skb->data+10+fromDs*(6+toDs*8)),ETH_ALEN); /* Address2, 3 or 4 */
+
+    /* Find the BSSID, which will be used to match the BA session */
+    if (toDs && fromDs)
+    {
+        unifi_trace(priv, UDBG6, "4 address frame - don't try to find BSSID\n");
+        bssid = NULL;
+    }
+    else
+    {
+        bssid = (u8 *) (skb->data + 4 + 12 - (fromDs * 6) - (toDs * 12));
+    }
+
+    pData = &bulkdata->d[0];
+    frameControl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(pData->os_data_ptr);
+    frameType = ((frameControl & 0x000C) >> 2);
+
+    unifi_trace(priv, UDBG3, "Rx Frame Type: %d sn: %d\n",frameType,
+         (le16_to_cpu(*((u16*)(bulkdata->d[0].os_data_ptr + IEEE802_11_SEQUENCE_CONTROL_OFFSET))) >> 4) & 0xfff);
+    if(frameType == IEEE802_11_FRAMETYPE_CONTROL){
+#ifdef CSR_SUPPORT_SME
+        unifi_trace(priv, UDBG6, "%s: Received Control Frame\n", __FUNCTION__);
+
+        if((frameControl & 0x00f0) == 0x00A0){
+            /* This is a PS-POLL request */
+            u8 pmBit = (frameControl & 0x1000)?0x01:0x00;
+            unifi_trace(priv, UDBG6, "%s: Received PS-POLL Frame\n", __FUNCTION__);
+
+            uf_process_ps_poll(priv,sa,da,pmBit,interfaceTag);
+        }
+        else {
+            unifi_warning(priv, "%s: Non PS-POLL control frame is received\n", __FUNCTION__);
+        }
+#endif
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+    if(frameType != IEEE802_11_FRAMETYPE_DATA) {
+        unifi_warning(priv, "%s: Non control Non Data frame is received\n",__FUNCTION__);
+        unifi_net_data_free(priv,&bulkdata->d[0]);
+        func_exit();
+        return;
+    }
+
+#ifdef CSR_SUPPORT_SME
+    if((interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP) ||
+       (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO)){
+
+        srcStaInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv,sa,interfaceTag);
+
+        if(srcStaInfo == NULL) {
+            CsrWifiMacAddress peerMacAddress;
+            /* Unknown data PDU */
+            memcpy(peerMacAddress.a,sa,ETH_ALEN);
+            unifi_trace(priv, UDBG1, "%s: Unexpected frame from peer = %x:%x:%x:%x:%x:%x\n", __FUNCTION__,
+            sa[0], sa[1],sa[2], sa[3], sa[4],sa[5]);
+            CsrWifiRouterCtrlUnexpectedFrameIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,interfaceTag,peerMacAddress);
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            func_exit();
+            return;
+        }
+
+        /*
+        verify power management bit here so as to ensure host and unifi are always
+        in sync with power management status of peer.
+
+        If we do it later, it may so happen we have stored the frame in BA re-ordering
+        buffer and hence host and unifi are out of sync for power management status
+        */
+
+        pmBit = (frameControl & 0x1000)?0x01:0x00;
+        powerSaveChanged = uf_process_pm_bit_for_peer(priv,srcStaInfo,pmBit,interfaceTag);
+
+        /* Update station last activity time */
+        srcStaInfo->activity_flag = TRUE;
+
+        /* For Qos Frame if PM bit is toggled to indicate the change in power save state then it shall not be
+        considered as Trigger Frame. Enter only if WMM STA and peer is in Power save */
+
+        dataFrameType = ((frameControl & 0x00f0) >> 4);
+
+        if((powerSaveChanged == FALSE)&&(srcStaInfo->wmmOrQosEnabled == TRUE)&&
+        (srcStaInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE)){
+
+            if((dataFrameType == QOS_DATA) || (dataFrameType == QOS_DATA_NULL)){
+
+                /*
+                 * QoS control field is offset from frame control by 2 (frame control)
+                 * + 2 (duration/ID) + 2 (sequence control) + 3*ETH_ALEN or 4*ETH_ALEN
+                 */
+                if((frameControl & IEEE802_11_FC_TO_DS_MASK) && (frameControl & IEEE802_11_FC_FROM_DS_MASK)){
+                    qosControl= CSR_GET_UINT16_FROM_LITTLE_ENDIAN(pData->os_data_ptr + 30);
+                }
+                else{
+                    qosControl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(pData->os_data_ptr + 24);
+                }
+                unifi_trace(priv, UDBG5, "%s: Check if U-APSD operations are triggered for qosControl: 0x%x\n",__FUNCTION__,qosControl);
+                uf_process_wmm_deliver_ac_uapsd(priv,srcStaInfo,qosControl,interfaceTag);
+            }
+        }
+    }
+
+#endif
+
+    if( ((frameControl & 0x00f0) >> 4) == QOS_DATA) {
+        u8 *qos_control_ptr = (u8*)bulkdata->d[0].os_data_ptr + (((frameControl & IEEE802_11_FC_TO_DS_MASK) && (frameControl & IEEE802_11_FC_FROM_DS_MASK))?30: 24);
+        int tID = *qos_control_ptr & IEEE802_11_QC_TID_MASK; /* using ls octet of qos control */
+        ba_session_rx_struct *ba_session;
+        u8 ba_session_idx = 0;
+        /* Get the BA originator address */
+        if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+           interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO){
+            ba_addr = sa;
+        }else{
+            ba_addr = bssid;
+        }
+
+        down(&priv->ba_mutex);
+        for (ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX; ba_session_idx++){
+            ba_session = interfacePriv->ba_session_rx[ba_session_idx];
+            if (ba_session){
+                unifi_trace(priv, UDBG6, "found ba_session=0x%x ba_session_idx=%d", ba_session, ba_session_idx);
+                if ((!memcmp(ba_session->macAddress.a, ba_addr, ETH_ALEN)) && (ba_session->tID == tID)){
+                        frame_desc_struct frame_desc;
+                        frame_desc.bulkdata = *bulkdata;
+                        frame_desc.signal = *signal;
+                        frame_desc.sn = (le16_to_cpu(*((u16*)(bulkdata->d[0].os_data_ptr + IEEE802_11_SEQUENCE_CONTROL_OFFSET))) >> 4) & 0xfff;
+                        frame_desc.active = TRUE;
+                        unifi_trace(priv, UDBG6, "%s: calling process_ba_frame (session=%d)\n", __FUNCTION__, ba_session_idx);
+                        process_ba_frame(priv, interfacePriv, ba_session, &frame_desc);
+                        up(&priv->ba_mutex);
+                        process_ba_complete(priv, interfacePriv);
+                        break;
+                }
+            }
+        }
+        if (ba_session_idx == MAX_SUPPORTED_BA_SESSIONS_RX){
+            up(&priv->ba_mutex);
+            unifi_trace(priv, UDBG6, "%s: calling process_amsdu()", __FUNCTION__);
+            process_amsdu(priv, signal, bulkdata);
+        }
+    } else {
+        unifi_trace(priv, UDBG6, "calling unifi_rx()");
+        unifi_rx(priv, signal, bulkdata);
+    }
+
+    /* check if the frames in reorder buffer has aged, the check
+     * is done after receive processing so that if the missing frame
+     * has arrived in this receive process, then it is handled cleanly.
+     *
+     * And also this code here takes care that timeout check is made for all
+     * the receive indications
+     */
+    down(&priv->ba_mutex);
+    for (i=0; i < MAX_SUPPORTED_BA_SESSIONS_RX; i++){
+        ba_session_rx_struct *ba_session;
+        ba_session = interfacePriv->ba_session_rx[i];
+            if (ba_session){
+                check_ba_frame_age_timeout(priv, interfacePriv, ba_session);
+            }
+    }
+    up(&priv->ba_mutex);
+    process_ba_complete(priv, interfacePriv);
+
+    func_exit();
+}
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_set_multicast_list
+ *
+ *      This function is called by the higher level stack to set
+ *      a list of multicast rx addresses.
+ *
+ *  Arguments:
+ *      dev             Network Device pointer.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+
+static void
+uf_set_multicast_list(struct net_device *dev)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+#ifdef CSR_NATIVE_LINUX
+    unifi_trace(priv, UDBG3, "uf_set_multicast_list unsupported\n");
+    return;
+#else
+
+    u8 *mc_list = interfacePriv->mc_list;
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34)
+    struct netdev_hw_addr *mc_addr;
+    int mc_addr_count;
+#else
+    struct dev_mc_list *p;      /* Pointer to the addresses structure. */
+    int i;
+#endif
+
+    if (priv->init_progress != UNIFI_INIT_COMPLETED) {
+        return;
+    }
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34)
+    mc_addr_count = netdev_mc_count(dev);
+
+    unifi_trace(priv, UDBG3,
+            "uf_set_multicast_list (count=%d)\n", mc_addr_count);
+
+
+    /* Not enough space? */
+    if (mc_addr_count > UNIFI_MAX_MULTICAST_ADDRESSES) {
+        return;
+    }
+
+    /* Store the list to be processed by the work item. */
+    interfacePriv->mc_list_count = mc_addr_count;
+    netdev_hw_addr_list_for_each(mc_addr, &dev->mc) {
+        memcpy(mc_list, mc_addr->addr, ETH_ALEN);
+        mc_list += ETH_ALEN;
+    }
+
+#else
+    unifi_trace(priv, UDBG3,
+            "uf_set_multicast_list (count=%d)\n", dev->mc_count);
+
+    /* Not enough space? */
+    if (dev->mc_count > UNIFI_MAX_MULTICAST_ADDRESSES) {
+        return;
+    }
+
+    /* Store the list to be processed by the work item. */
+    interfacePriv->mc_list_count = dev->mc_count;
+    p = dev->mc_list;
+    for (i = 0; i < dev->mc_count; i++) {
+        memcpy(mc_list, p->dmi_addr, ETH_ALEN);
+        p = p->next;
+        mc_list += ETH_ALEN;
+    }
+#endif
+
+    /* Send a message to the workqueue */
+    queue_work(priv->unifi_workqueue, &priv->multicast_list_task);
+#endif
+
+} /* uf_set_multicast_list() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  netdev_mlme_event_handler
+ *
+ *      Callback function to be used as the udi_event_callback when registering
+ *      as a netdev client.
+ *      To use it, a client specifies this function as the udi_event_callback
+ *      to ul_register_client(). The signal dispatcher in
+ *      unifi_receive_event() will call this function to deliver a signal.
+ *
+ *  Arguments:
+ *      pcli            Pointer to the client instance.
+ *      signal          Pointer to the received signal.
+ *      signal_len      Size of the signal structure in bytes.
+ *      bulkdata        Pointer to structure containing any associated bulk data.
+ *      dir             Direction of the signal. Zero means from host,
+ *                      non-zero means to host.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+netdev_mlme_event_handler(ul_client_t *pcli, const u8 *sig_packed, int sig_len,
+                          const bulk_data_param_t *bulkdata_o, int dir)
+{
+    CSR_SIGNAL signal;
+    unifi_priv_t *priv = uf_find_instance(pcli->instance);
+    int id, r;
+    bulk_data_param_t bulkdata;
+
+    func_enter();
+
+    /* Just a sanity check */
+    if (sig_packed == NULL) {
+        return;
+    }
+
+    /*
+     * This copy is to silence a compiler warning about discarding the
+     * const qualifier.
+     */
+    bulkdata = *bulkdata_o;
+
+    /* Get the unpacked signal */
+    r = read_unpack_signal(sig_packed, &signal);
+    if (r) {
+        /*
+         * The CSR_MLME_CONNECTED_INDICATION_ID has a receiverID=0 so will
+         * fall through this case. It is safe to ignore this signal.
+         */
+        unifi_trace(priv, UDBG1,
+                    "Netdev - Received unknown signal 0x%.4X.\n",
+                    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sig_packed));
+        return;
+    }
+
+    id = signal.SignalPrimitiveHeader.SignalId;
+    unifi_trace(priv, UDBG3, "Netdev - Process signal 0x%.4X\n", id);
+
+    /*
+     * Take the appropriate action for the signal.
+     */
+    switch (id) {
+        case CSR_MA_PACKET_ERROR_INDICATION_ID:
+            process_ma_packet_error_ind(priv, &signal, &bulkdata);
+            break;
+        case CSR_MA_PACKET_INDICATION_ID:
+            process_ma_packet_ind(priv, &signal, &bulkdata);
+            break;
+        case  CSR_MA_PACKET_CONFIRM_ID:
+            process_ma_packet_cfm(priv, &signal, &bulkdata);
+            break;
+#ifdef CSR_SUPPORT_SME
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+            /* Handle TIM confirms from FW & set the station record's TIM state appropriately,
+             * In case of failures, tries with max_retransmit limit
+             */
+            uf_handle_tim_cfm(priv, &signal.u.MlmeSetTimConfirm, signal.SignalPrimitiveHeader.ReceiverProcessId);
+            break;
+#endif
+        case CSR_DEBUG_STRING_INDICATION_ID:
+            debug_string_indication(priv, bulkdata.d[0].os_data_ptr, bulkdata.d[0].data_length);
+            break;
+
+        case CSR_DEBUG_WORD16_INDICATION_ID:
+            debug_word16_indication(priv, &signal);
+            break;
+
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+        case CSR_DEBUG_GENERIC_INDICATION_ID:
+            debug_generic_indication(priv, &signal);
+            break;
+        default:
+            break;
+    }
+
+    func_exit();
+} /* netdev_mlme_event_handler() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_net_get_name
+ *
+ *      Retrieve the name (e.g. eth1) associated with this network device
+ *
+ *  Arguments:
+ *      dev             Pointer to the network device.
+ *      name            Buffer to write name
+ *      len             Size of buffer in bytes
+ *
+ *  Returns:
+ *      None
+ *
+ *  Notes:
+ * ---------------------------------------------------------------------------
+ */
+void uf_net_get_name(struct net_device *dev, char *name, int len)
+{
+    *name = '\0';
+    if (dev) {
+        strlcpy(name, dev->name, (len > IFNAMSIZ) ? IFNAMSIZ : len);
+    }
+
+} /* uf_net_get_name */
+
+
+
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+#ifdef CONFIG_NET_SCHED
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_install_qdisc
+ *
+ *      Creates a root qdisc, registers our qdisc handlers and
+ *      overrides the device's qdisc_sleeping to prevent the system
+ *      from creating a new one for our network device.
+ *
+ *  Arguments:
+ *      dev             Pointer to the network device.
+ *
+ *  Returns:
+ *      0 on success, Linux error code otherwise.
+ *
+ *  Notes:
+ *      This function holds the qdisk lock so it needs to be called
+ *      after registering the network device in uf_register_netdev().
+ *      Also, the qdisc_create_dflt() API has changed in 2.6.20 to
+ *      include the parentid.
+ * ---------------------------------------------------------------------------
+ */
+int uf_install_qdisc(struct net_device *dev)
+{
+    struct Qdisc *qdisc;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    struct netdev_queue *queue0;
+#endif /* LINUX_VERSION_CODE */
+
+
+    func_enter();
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,27)
+    /*
+     * check that there is no qdisc currently attached to device
+     * this ensures that we will be the root qdisc. (I can't find a better
+     * way to test this explicitly)
+     */
+    if (dev->qdisc_sleeping != &noop_qdisc) {
+        func_exit_r(-EFAULT);
+        return -EINVAL;
+    }
+#endif /* LINUX_VERSION_CODE */
+
+    qdisc = UF_QDISC_CREATE_DFLT(dev, &uf_qdisc_ops, TC_H_ROOT);
+    if (!qdisc) {
+        unifi_error(NULL, "%s: qdisc installation failed\n", dev->name);
+        func_exit_r(-EFAULT);
+        return -EFAULT;
+    }
+    unifi_trace(NULL, UDBG5, "%s: parent qdisc=0x%p\n",
+            dev->name, qdisc);
+
+    qdisc->handle = 0x80020000;
+    qdisc->flags = 0x0;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    queue0 = netdev_get_tx_queue(dev, 0);
+    if (queue0 == NULL) {
+        unifi_error(NULL, "%s: netdev_get_tx_queue returned no queue\n",
+                dev->name);
+        func_exit_r(-EFAULT);
+        return -EFAULT;
+    }
+    queue0->qdisc = qdisc;
+    queue0->qdisc_sleeping = qdisc;
+#else
+    qdisc_lock_tree(dev);
+    list_add_tail(&qdisc->list, &dev->qdisc_list);
+    dev->qdisc_sleeping = qdisc;
+    qdisc_unlock_tree(dev);
+#endif /* LINUX_VERSION_CODE */
+
+    func_exit_r(0);
+    return 0;
+
+} /* uf_install_qdisc() */
+
+static int uf_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev_queue->dev);
+#else
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev);
+#endif /* LINUX_VERSION_CODE */
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct uf_sched_data *q = qdisc_priv(qd);
+    struct uf_tx_packet_data *pkt_data = (struct uf_tx_packet_data *) skb->cb;
+    struct ethhdr ehdr;
+    struct Qdisc *qdisc;
+    int r, proto;
+
+    func_enter();
+
+    memcpy(&ehdr, skb->data, ETH_HLEN);
+    proto = ntohs(ehdr.h_proto);
+
+    /* 802.1x - apply controlled/uncontrolled port rules */
+    if ((proto != ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+            && (proto != ETH_P_WAI)
+#endif
+       ) {
+        /* queues 0 - 3 */
+        pkt_data->priority = get_packet_priority(priv, skb, &ehdr, interfacePriv);
+        pkt_data->queue = unifi_frame_priority_to_queue(pkt_data->priority);
+    } else {
+        pkt_data->queue = UNIFI_TRAFFIC_Q_EAPOL;
+    }
+
+    qdisc = q->queues[pkt_data->queue];
+    r = qdisc->enqueue(skb, qdisc);
+    if (r == NET_XMIT_SUCCESS) {
+        qd->q.qlen++;
+        qd->bstats.bytes += skb->len;
+        qd->bstats.packets++;
+        func_exit_r(NET_XMIT_SUCCESS);
+        return NET_XMIT_SUCCESS;
+    }
+
+    unifi_error(priv, "uf_qdiscop_enqueue: dropped\n");
+    qd->qstats.drops++;
+
+    func_exit_r(r);
+    return r;
+
+} /* uf_qdiscop_enqueue() */
+
+
+static int uf_qdiscop_requeue(struct sk_buff *skb, struct Qdisc* qd)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t*)netdev_priv(qd->dev_queue->dev);
+#else
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t*)netdev_priv(qd->dev);
+#endif /* LINUX_VERSION_CODE */
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct uf_sched_data *q = qdisc_priv(qd);
+    struct uf_tx_packet_data *pkt_data = (struct uf_tx_packet_data *) skb->cb;
+    struct Qdisc *qdisc;
+    int r;
+
+    func_enter();
+
+    unifi_trace(priv, UDBG5, "uf_qdiscop_requeue: (q=%d), tag=%u\n",
+            pkt_data->queue, pkt_data->host_tag);
+
+    /* we recorded which queue to use earlier! */
+    qdisc = q->queues[pkt_data->queue];
+
+    if ((r = qdisc->ops->requeue(skb, qdisc)) == 0) {
+        qd->q.qlen++;
+        func_exit_r(0);
+        return 0;
+    }
+
+    unifi_error(priv, "uf_qdiscop_requeue: dropped\n");
+    qd->qstats.drops++;
+
+    func_exit_r(r);
+    return r;
+} /* uf_qdiscop_requeue() */
+
+static struct sk_buff *uf_qdiscop_dequeue(struct Qdisc* qd)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev_queue->dev);
+#else
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(qd->dev);
+#endif /* LINUX_VERSION_CODE */
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct uf_sched_data *q = qdisc_priv(qd);
+    struct sk_buff *skb;
+    struct Qdisc *qdisc;
+    int queue, i;
+    struct ethhdr ehdr;
+    struct uf_tx_packet_data *pkt_data;
+    CsrWifiRouterCtrlPortAction port_action;
+
+    func_enter();
+
+    /* check all the queues */
+    for (i = UNIFI_TRAFFIC_Q_MAX - 1; i >= 0; i--) {
+
+        if (i != UNIFI_TRAFFIC_Q_EAPOL) {
+            queue = priv->prev_queue;
+            if (++priv->prev_queue >= UNIFI_TRAFFIC_Q_EAPOL) {
+                priv->prev_queue = 0;
+            }
+        } else {
+            queue = i;
+        }
+
+#ifndef ALLOW_Q_PAUSE
+        /* If queue is paused, do not dequeue */
+        if (net_is_tx_q_paused(priv, queue)) {
+            unifi_trace(priv, UDBG5,
+                    "uf_qdiscop_dequeue: tx queue paused (q=%d)\n", queue);
+            continue;
+        }
+#endif
+
+        qdisc = q->queues[queue];
+        skb = qdisc->dequeue(qdisc);
+        if (skb) {
+            /* A packet has been dequeued, decrease the queued packets count */
+            qd->q.qlen--;
+
+            pkt_data = (struct uf_tx_packet_data *) skb->cb;
+
+            /* Check the (un)controlled port status */
+            memcpy(&ehdr, skb->data, ETH_HLEN);
+
+            port_action = verify_port(priv
+                            , (((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) ||(CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI == interfacePriv->interfaceMode))? interfacePriv->bssid.a: ehdr.h_dest)
+                            , (UNIFI_TRAFFIC_Q_EAPOL == queue? UF_UNCONTROLLED_PORT_Q: UF_CONTROLLED_PORT_Q)
+                            , interfacePriv->InterfaceTag);
+
+            /* Dequeue packet if port is open */
+            if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
+                unifi_trace(priv, UDBG5,
+                        "uf_qdiscop_dequeue: new (q=%d), tag=%u\n",
+                        queue, pkt_data->host_tag);
+
+                func_exit();
+                return skb;
+            }
+
+            /* Discard or block the packet if necessary */
+            if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD) {
+                unifi_trace(priv, UDBG5,
+                        "uf_qdiscop_dequeue: drop (q=%d), tag=%u\n",
+                        queue, pkt_data->host_tag);
+                kfree_skb(skb);
+                break;
+            }
+
+            /* We can not send the packet now, put it back to the queue */
+            if (qdisc->ops->requeue(skb, qdisc) != 0) {
+                unifi_error(priv,
+                        "uf_qdiscop_dequeue: requeue (q=%d) failed, tag=%u, drop it\n",
+                        queue, pkt_data->host_tag);
+
+                /* Requeue failed, drop the packet */
+                kfree_skb(skb);
+                break;
+            }
+            /* We requeued the packet, increase the queued packets count */
+            qd->q.qlen++;
+
+            unifi_trace(priv, UDBG5,
+                    "uf_qdiscop_dequeue: skip (q=%d), tag=%u\n",
+                    queue, pkt_data->host_tag);
+        }
+    }
+
+    func_exit();
+    return NULL;
+} /* uf_qdiscop_dequeue() */
+
+
+static void uf_qdiscop_reset(struct Qdisc* qd)
+{
+    struct uf_sched_data *q = qdisc_priv(qd);
+    int queue;
+    func_enter();
+
+    for (queue = 0; queue < UNIFI_TRAFFIC_Q_MAX; queue++) {
+        qdisc_reset(q->queues[queue]);
+    }
+    qd->q.qlen = 0;
+
+    func_exit();
+} /* uf_qdiscop_reset() */
+
+
+static void uf_qdiscop_destroy(struct Qdisc* qd)
+{
+    struct uf_sched_data *q = qdisc_priv(qd);
+    int queue;
+
+    func_enter();
+
+    for (queue=0; queue < UNIFI_TRAFFIC_Q_MAX; queue++) {
+        qdisc_destroy(q->queues[queue]);
+        q->queues[queue] = &noop_qdisc;
+    }
+
+    func_exit();
+} /* uf_qdiscop_destroy() */
+
+
+/* called whenever parameters are updated on existing qdisc */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,25)
+static int uf_qdiscop_tune(struct Qdisc *qd, struct nlattr *opt)
+#else
+static int uf_qdiscop_tune(struct Qdisc *qd, struct rtattr *opt)
+#endif
+{
+    func_enter();
+    func_exit();
+    return 0;
+} /* uf_qdiscop_tune() */
+
+
+/* called during initial creation of qdisc on device */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,25)
+static int uf_qdiscop_init(struct Qdisc *qd, struct nlattr *opt)
+#else
+static int uf_qdiscop_init(struct Qdisc *qd, struct rtattr *opt)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    struct net_device *dev = qd->dev_queue->dev;
+#else
+    struct net_device *dev = qd->dev;
+#endif /* LINUX_VERSION_CODE */
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct uf_sched_data *q = qdisc_priv(qd);
+    int err = 0, i;
+
+    func_enter();
+
+    /* make sure we do not mess with the ingress qdisc */
+    if (qd->flags & TCQ_F_INGRESS) {
+        func_exit();
+        return -EINVAL;
+    }
+
+    /* if options were passed in, set them */
+    if (opt) {
+        err = uf_qdiscop_tune(qd, opt);
+    }
+
+    /* create child queues */
+    for (i = 0; i < UNIFI_TRAFFIC_Q_MAX; i++) {
+        q->queues[i] = UF_QDISC_CREATE_DFLT(dev, &pfifo_qdisc_ops,
+                qd->handle);
+        if (!q->queues[i]) {
+            q->queues[i] = &noop_qdisc;
+            unifi_error(priv, "%s child qdisc %i creation failed\n");
+        }
+
+        unifi_trace(priv, UDBG5, "%s: child qdisc=0x%p\n",
+                dev->name, q->queues[i]);
+    }
+
+    func_exit_r(err);
+    return err;
+} /* uf_qdiscop_init() */
+
+
+static int uf_qdiscop_dump(struct Qdisc *qd, struct sk_buff *skb)
+{
+    func_enter();
+    func_exit_r(skb->len);
+    return skb->len;
+} /* uf_qdiscop_dump() */
+
+#endif /* CONFIG_NET_SCHED */
+#endif /* LINUX_VERSION_CODE */
+
+#ifdef CSR_SUPPORT_WEXT
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_netdev_event
+ *
+ *     Callback function to handle netdev state changes
+ *
+ *  Arguments:
+ *      notif           Pointer to a notifier_block.
+ *      event           Event prompting notification
+ *      ptr             net_device pointer
+ *
+ *  Returns:
+ *      None
+ *
+ *  Notes:
+ *   The event handler is global, and may occur on non-UniFi netdevs.
+ * ---------------------------------------------------------------------------
+ */
+static int
+uf_netdev_event(struct notifier_block *notif, unsigned long event, void* ptr) {
+    struct net_device *netdev = ptr;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(netdev);
+    unifi_priv_t *priv = NULL;
+    static const CsrWifiMacAddress broadcast_address = {{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
+
+    /* Check that the event is for a UniFi netdev. If it's not, the netdev_priv
+     * structure is not safe to use.
+     */
+    if (uf_find_netdev_priv(interfacePriv) == -1) {
+        unifi_trace(NULL, UDBG1, "uf_netdev_event: ignore e=%d, ptr=%p, priv=%p %s\n",
+                    event, ptr, interfacePriv, netdev->name);
+        return 0;
+    }
+
+    switch(event) {
+    case NETDEV_CHANGE:
+        priv = interfacePriv->privPtr;
+        unifi_trace(priv, UDBG1, "NETDEV_CHANGE: %p %s %s waiting for it\n",
+                    ptr,
+                    netdev->name,
+                    interfacePriv->wait_netdev_change ? "" : "not");
+
+        if (interfacePriv->wait_netdev_change) {
+            UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[interfacePriv->InterfaceTag]);
+            interfacePriv->connected = UnifiConnected;
+            interfacePriv->wait_netdev_change = FALSE;
+            /* Note: passing the broadcast address here will allow anyone to attempt to join our adhoc network */
+            uf_process_rx_pending_queue(priv, UF_UNCONTROLLED_PORT_Q, broadcast_address, 1,interfacePriv->InterfaceTag);
+            uf_process_rx_pending_queue(priv, UF_CONTROLLED_PORT_Q, broadcast_address, 1,interfacePriv->InterfaceTag);
+        }
+        break;
+
+    default:
+        break;
+    }
+    return 0;
+}
+
+static struct notifier_block uf_netdev_notifier = {
+    .notifier_call = uf_netdev_event,
+};
+#endif /* CSR_SUPPORT_WEXT */
+
+
+static void
+        process_amsdu(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata)
+{
+    u32 offset;
+    u32 length = bulkdata->d[0].data_length;
+    u32 subframe_length, subframe_body_length, dot11_hdr_size;
+    u8 *ptr;
+    bulk_data_param_t subframe_bulkdata;
+    u8 *dot11_hdr_ptr = (u8*)bulkdata->d[0].os_data_ptr;
+    CsrResult csrResult;
+    u16 frameControl;
+    u8 *qos_control_ptr;
+
+    frameControl = le16_to_cpu(*((u16*)dot11_hdr_ptr));
+    qos_control_ptr = dot11_hdr_ptr + (((frameControl & IEEE802_11_FC_TO_DS_MASK) && (frameControl & IEEE802_11_FC_FROM_DS_MASK))?30: 24);
+    if(!(*qos_control_ptr & IEEE802_11_QC_A_MSDU_PRESENT)) {
+        unifi_trace(priv, UDBG6, "%s: calling unifi_rx()", __FUNCTION__);
+        unifi_rx(priv, signal, bulkdata);
+        return;
+    }
+    *qos_control_ptr &= ~(IEEE802_11_QC_A_MSDU_PRESENT);
+
+    ptr = qos_control_ptr + 2;
+    offset = dot11_hdr_size = ptr - dot11_hdr_ptr;
+
+    while(length > (offset + sizeof(struct ethhdr) + sizeof(llc_snap_hdr_t))) {
+        subframe_body_length = ntohs(((struct ethhdr*)ptr)->h_proto);
+        if(subframe_body_length > IEEE802_11_MAX_DATA_LEN) {
+            unifi_error(priv, "%s: bad subframe_body_length = %d\n", __FUNCTION__, subframe_body_length);
+            break;
+        }
+        subframe_length = sizeof(struct ethhdr) + subframe_body_length;
+        memset(&subframe_bulkdata, 0, sizeof(bulk_data_param_t));
+
+        csrResult = unifi_net_data_malloc(priv, &subframe_bulkdata.d[0], dot11_hdr_size + subframe_body_length);
+
+        if (csrResult != CSR_RESULT_SUCCESS) {
+            unifi_error(priv, "%s: unifi_net_data_malloc failed\n", __FUNCTION__);
+            break;
+        }
+
+        memcpy((u8*)subframe_bulkdata.d[0].os_data_ptr, dot11_hdr_ptr, dot11_hdr_size);
+
+
+        /* When to DS=0 and from DS=0, address 3 will already have BSSID so no need to re-program */
+        if ((frameControl & IEEE802_11_FC_TO_DS_MASK) && !(frameControl & IEEE802_11_FC_FROM_DS_MASK)){
+                memcpy((u8*)subframe_bulkdata.d[0].os_data_ptr + IEEE802_11_ADDR3_OFFSET, ((struct ethhdr*)ptr)->h_dest, ETH_ALEN);
+        }
+        else if (!(frameControl & IEEE802_11_FC_TO_DS_MASK) && (frameControl & IEEE802_11_FC_FROM_DS_MASK)){
+                memcpy((u8*)subframe_bulkdata.d[0].os_data_ptr + IEEE802_11_ADDR3_OFFSET,
+                         ((struct ethhdr*)ptr)->h_source,
+                           ETH_ALEN);
+        }
+
+        memcpy((u8*)subframe_bulkdata.d[0].os_data_ptr + dot11_hdr_size,
+                ptr + sizeof(struct ethhdr),
+                             subframe_body_length);
+        unifi_trace(priv, UDBG6, "%s: calling unifi_rx. length = %d subframe_length = %d\n", __FUNCTION__, length, subframe_length);
+        unifi_rx(priv, signal, &subframe_bulkdata);
+
+        subframe_length = (subframe_length + 3)&(~0x3);
+        ptr += subframe_length;
+        offset += subframe_length;
+    }
+    unifi_net_data_free(priv, &bulkdata->d[0]);
+}
+
+
+#define SN_TO_INDEX(__ba_session, __sn) (((__sn - __ba_session->start_sn) & 0xFFF) % __ba_session->wind_size)
+
+
+#define ADVANCE_EXPECTED_SN(__ba_session) \
+{ \
+    __ba_session->expected_sn++; \
+    __ba_session->expected_sn &= 0xFFF; \
+}
+
+#define FREE_BUFFER_SLOT(__ba_session, __index) \
+{ \
+    __ba_session->occupied_slots--; \
+    __ba_session->buffer[__index].active = FALSE; \
+    ADVANCE_EXPECTED_SN(__ba_session); \
+}
+
+static void add_frame_to_ba_complete(unifi_priv_t *priv,
+                          netInterface_priv_t *interfacePriv,
+                          frame_desc_struct *frame_desc)
+{
+    interfacePriv->ba_complete[interfacePriv->ba_complete_index] = *frame_desc;
+    interfacePriv->ba_complete_index++;
+}
+
+
+static void update_expected_sn(unifi_priv_t *priv,
+                          netInterface_priv_t *interfacePriv,
+                          ba_session_rx_struct *ba_session,
+                          u16 sn)
+{
+    int i, j;
+    u16 gap;
+
+    gap = (sn - ba_session->expected_sn) & 0xFFF;
+    unifi_trace(priv, UDBG6, "%s: proccess the frames up to new_expected_sn = %d gap = %d\n", __FUNCTION__, sn, gap);
+    for(j = 0; j < gap && j < ba_session->wind_size; j++) {
+        i = SN_TO_INDEX(ba_session, ba_session->expected_sn);
+        unifi_trace(priv, UDBG6, "%s: proccess the slot index = %d\n", __FUNCTION__, i);
+        if(ba_session->buffer[i].active) {
+            add_frame_to_ba_complete(priv, interfacePriv, &ba_session->buffer[i]);
+            unifi_trace(priv, UDBG6, "%s: proccess the frame at index = %d expected_sn = %d\n", __FUNCTION__, i, ba_session->expected_sn);
+            FREE_BUFFER_SLOT(ba_session, i);
+        } else {
+            unifi_trace(priv, UDBG6, "%s: empty slot at index = %d\n", __FUNCTION__, i);
+            ADVANCE_EXPECTED_SN(ba_session);
+        }
+    }
+    ba_session->expected_sn = sn;
+}
+
+
+static void complete_ready_sequence(unifi_priv_t *priv,
+                               netInterface_priv_t *interfacePriv,
+                               ba_session_rx_struct *ba_session)
+{
+    int i;
+
+    i = SN_TO_INDEX(ba_session, ba_session->expected_sn);
+    while (ba_session->buffer[i].active) {
+        add_frame_to_ba_complete(priv, interfacePriv, &ba_session->buffer[i]);
+        unifi_trace(priv, UDBG6, "%s: completed stored frame(expected_sn=%d) at i = %d\n", __FUNCTION__, ba_session->expected_sn, i);
+        FREE_BUFFER_SLOT(ba_session, i);
+        i = SN_TO_INDEX(ba_session, ba_session->expected_sn);
+    }
+}
+
+
+void scroll_ba_window(unifi_priv_t *priv,
+                                netInterface_priv_t *interfacePriv,
+                                ba_session_rx_struct *ba_session,
+                                u16 sn)
+{
+    if(((sn - ba_session->expected_sn) & 0xFFF) <= 2048) {
+        update_expected_sn(priv, interfacePriv, ba_session, sn);
+        complete_ready_sequence(priv, interfacePriv, ba_session);
+    }
+}
+
+
+static int consume_frame_or_get_buffer_index(unifi_priv_t *priv,
+                                            netInterface_priv_t *interfacePriv,
+                                            ba_session_rx_struct *ba_session,
+                                            u16 sn,
+                                            frame_desc_struct *frame_desc) {
+    int i;
+    u16 sn_temp;
+
+    if(((sn - ba_session->expected_sn) & 0xFFF) <= 2048) {
+
+        /* once we are in BA window, set the flag for BA trigger */
+        if(!ba_session->trigger_ba_after_ssn){
+            ba_session->trigger_ba_after_ssn = TRUE;
+        }
+
+        sn_temp = ba_session->expected_sn + ba_session->wind_size;
+        unifi_trace(priv, UDBG6, "%s: new frame: sn=%d\n", __FUNCTION__, sn);
+        if(!(((sn - sn_temp) & 0xFFF) > 2048)) {
+            u16 new_expected_sn;
+            unifi_trace(priv, UDBG6, "%s: frame is out of window\n", __FUNCTION__);
+            sn_temp = (sn - ba_session->wind_size) & 0xFFF;
+            new_expected_sn = (sn_temp + 1) & 0xFFF;
+            update_expected_sn(priv, interfacePriv, ba_session, new_expected_sn);
+        }
+        i = -1;
+        if (sn == ba_session->expected_sn) {
+            unifi_trace(priv, UDBG6, "%s: sn = ba_session->expected_sn = %d\n", __FUNCTION__, sn);
+            ADVANCE_EXPECTED_SN(ba_session);
+            add_frame_to_ba_complete(priv, interfacePriv, frame_desc);
+        } else {
+            i = SN_TO_INDEX(ba_session, sn);
+            unifi_trace(priv, UDBG6, "%s: sn(%d) != ba_session->expected_sn(%d), i = %d\n", __FUNCTION__, sn, ba_session->expected_sn, i);
+            if (ba_session->buffer[i].active) {
+                unifi_trace(priv, UDBG6, "%s: free frame at i = %d\n", __FUNCTION__, i);
+                i = -1;
+                unifi_net_data_free(priv, &frame_desc->bulkdata.d[0]);
+            }
+        }
+    } else {
+        i = -1;
+        if(!ba_session->trigger_ba_after_ssn){
+            unifi_trace(priv, UDBG6, "%s: frame before ssn, pass it up: sn=%d\n", __FUNCTION__, sn);
+            add_frame_to_ba_complete(priv, interfacePriv, frame_desc);
+        }else{
+            unifi_trace(priv, UDBG6, "%s: old frame, drop: sn=%d, expected_sn=%d\n", __FUNCTION__, sn, ba_session->expected_sn);
+            unifi_net_data_free(priv, &frame_desc->bulkdata.d[0]);
+        }
+    }
+    return i;
+}
+
+
+
+static void process_ba_frame(unifi_priv_t *priv,
+                                             netInterface_priv_t *interfacePriv,
+                                             ba_session_rx_struct *ba_session,
+                                             frame_desc_struct *frame_desc)
+{
+    int i;
+    u16 sn = frame_desc->sn;
+
+    if (ba_session->timeout) {
+        mod_timer(&ba_session->timer, (jiffies + usecs_to_jiffies((ba_session->timeout) * 1024)));
+    }
+    unifi_trace(priv, UDBG6, "%s: got frame(sn=%d)\n", __FUNCTION__, sn);
+
+    i = consume_frame_or_get_buffer_index(priv, interfacePriv, ba_session, sn, frame_desc);
+    if(i >= 0) {
+        unifi_trace(priv, UDBG6, "%s: store frame(sn=%d) at i = %d\n", __FUNCTION__, sn, i);
+        ba_session->buffer[i] = *frame_desc;
+        ba_session->buffer[i].recv_time = CsrTimeGet(NULL);
+        ba_session->occupied_slots++;
+    } else {
+        unifi_trace(priv, UDBG6, "%s: frame consumed - sn = %d\n", __FUNCTION__, sn);
+    }
+    complete_ready_sequence(priv, interfacePriv, ba_session);
+}
+
+
+static void process_ba_complete(unifi_priv_t *priv, netInterface_priv_t *interfacePriv)
+{
+    frame_desc_struct *frame_desc;
+    u8 i;
+
+    for(i = 0; i < interfacePriv->ba_complete_index; i++) {
+        frame_desc = &interfacePriv->ba_complete[i];
+        unifi_trace(priv, UDBG6, "%s: calling process_amsdu()\n", __FUNCTION__);
+        process_amsdu(priv, &frame_desc->signal, &frame_desc->bulkdata);
+    }
+    interfacePriv->ba_complete_index = 0;
+
+}
+
+
+/* Check if the frames in BA reoder buffer has aged and
+ * if so release the frames to upper processes and move
+ * the window
+ */
+static void check_ba_frame_age_timeout( unifi_priv_t *priv,
+                                        netInterface_priv_t *interfacePriv,
+                                        ba_session_rx_struct *ba_session)
+{
+    CsrTime now;
+    CsrTime age;
+    u8 i, j;
+    u16 sn_temp;
+
+    /* gap is started at 1 because we have buffered frames and
+     * hence a minimum gap of 1 exists
+     */
+    u8 gap=1;
+
+    now = CsrTimeGet(NULL);
+
+    if (ba_session->occupied_slots)
+    {
+        /* expected sequence has not arrived so start searching from next
+         * sequence number until a frame is available and determine the gap.
+         * Check if the frame available has timedout, if so advance the
+         * expected sequence number and release the frames
+         */
+        sn_temp = (ba_session->expected_sn + 1) & 0xFFF;
+
+        for(j = 0; j < ba_session->wind_size; j++)
+        {
+            i = SN_TO_INDEX(ba_session, sn_temp);
+
+            if(ba_session->buffer[i].active)
+            {
+                unifi_trace(priv, UDBG6, "check age at slot index = %d sn = %d recv_time = %u now = %u\n",
+                                        i,
+                                        ba_session->buffer[i].sn,
+                                        ba_session->buffer[i].recv_time,
+                                        now);
+
+                if (ba_session->buffer[i].recv_time > now)
+                {
+                    /* timer wrap */
+                    age = CsrTimeAdd((CsrTime)CsrTimeSub(CSR_SCHED_TIME_MAX, ba_session->buffer[i].recv_time), now);
+                }
+                else
+                {
+                    age = (CsrTime)CsrTimeSub(now, ba_session->buffer[i].recv_time);
+                }
+
+                if (age >= CSR_WIFI_BA_MPDU_FRAME_AGE_TIMEOUT)
+                {
+                    unifi_trace(priv, UDBG2, "release the frame at index = %d gap = %d expected_sn = %d sn = %d\n",
+                                            i,
+                                            gap,
+                                            ba_session->expected_sn,
+                                            ba_session->buffer[i].sn);
+
+                    /* if it has timedout don't wait for missing frames, move the window */
+                    while (gap--)
+                    {
+                        ADVANCE_EXPECTED_SN(ba_session);
+                    }
+                    add_frame_to_ba_complete(priv, interfacePriv, &ba_session->buffer[i]);
+                    FREE_BUFFER_SLOT(ba_session, i);
+                    complete_ready_sequence(priv, interfacePriv, ba_session);
+                }
+                break;
+
+            }
+            else
+            {
+                /* advance temp sequence number and frame gap */
+                sn_temp = (sn_temp + 1) & 0xFFF;
+                gap++;
+            }
+        }
+    }
+}
+
+
+static void process_ma_packet_error_ind(unifi_priv_t *priv, CSR_SIGNAL *signal, bulk_data_param_t *bulkdata)
+{
+    u16 interfaceTag;
+    const CSR_MA_PACKET_ERROR_INDICATION *pkt_err_ind = &signal->u.MaPacketErrorIndication;
+    netInterface_priv_t *interfacePriv;
+    ba_session_rx_struct *ba_session;
+    u8 ba_session_idx = 0;
+    CSR_PRIORITY        UserPriority;
+    CSR_SEQUENCE_NUMBER sn;
+
+    func_enter();
+
+    interfaceTag = (pkt_err_ind->VirtualInterfaceIdentifier & 0xff);
+
+
+    /* Sanity check that the VIF refers to a sensible interface */
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "%s: MaPacketErrorIndication indication with bad interfaceTag %d\n", __FUNCTION__, interfaceTag);
+        func_exit();
+        return;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+    UserPriority = pkt_err_ind->UserPriority;
+    if(UserPriority > 15) {
+        unifi_error(priv, "%s: MaPacketErrorIndication indication with bad UserPriority=%d\n", __FUNCTION__, UserPriority);
+        func_exit();
+    }
+    sn = pkt_err_ind->SequenceNumber;
+
+    down(&priv->ba_mutex);
+    /* To find the right ba_session loop through the BA sessions, compare MAC address and tID */
+    for (ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX; ba_session_idx++){
+        ba_session = interfacePriv->ba_session_rx[ba_session_idx];
+        if (ba_session){
+            if ((!memcmp(ba_session->macAddress.a, pkt_err_ind->PeerQstaAddress.x, ETH_ALEN)) && (ba_session->tID == UserPriority)){
+                if (ba_session->timeout) {
+                    mod_timer(&ba_session->timer, (jiffies + usecs_to_jiffies((ba_session->timeout) * 1024)));
+                }
+                scroll_ba_window(priv, interfacePriv, ba_session, sn);
+                break;
+            }
+        }
+    }
+
+    up(&priv->ba_mutex);
+    process_ba_complete(priv, interfacePriv);
+    func_exit();
+}
+
+
diff --git a/drivers/staging/csr/os.c b/drivers/staging/csr/os.c
new file mode 100644
index 00000000000..35dc9087d79
--- /dev/null
+++ b/drivers/staging/csr/os.c
@@ -0,0 +1,483 @@
+/*
+ * ---------------------------------------------------------------------------
+ *  FILE:     os.c
+ *
+ *  PURPOSE:
+ *      Routines to fulfil the OS-abstraction for the HIP lib.
+ *      It is part of the porting exercise.
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+/**
+ * The HIP lib OS abstraction consists of the implementation
+ * of the functions in this file. It is part of the porting exercise.
+ */
+
+#include "unifi_priv.h"
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_net_data_malloc
+ *
+ *      Allocate an OS specific net data buffer of "size" bytes.
+ *      The bulk_data_slot.os_data_ptr must be initialised to point
+ *      to the buffer allocated. The bulk_data_slot.length must be
+ *      initialised to the requested size, zero otherwise.
+ *      The bulk_data_slot.os_net_buf_ptr can be initialised to
+ *      an OS specific pointer to be used in the unifi_net_data_free().
+ *
+ *
+ *  Arguments:
+ *      ospriv              Pointer to device private context struct.
+ *      bulk_data_slot      Pointer to the bulk data structure to initialise.
+ *      size                Size of the buffer to be allocated.
+ *
+ *  Returns:
+ *      CSR_RESULT_SUCCESS on success, CSR_RESULT_FAILURE otherwise.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+unifi_net_data_malloc(void *ospriv, bulk_data_desc_t *bulk_data_slot, unsigned int size)
+{
+    struct sk_buff *skb;
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    int rounded_length;
+
+    if (priv->card_info.sdio_block_size == 0) {
+        unifi_error(priv, "unifi_net_data_malloc: Invalid SDIO block size\n");
+        return CSR_RESULT_FAILURE;
+    }
+
+    rounded_length = (size + priv->card_info.sdio_block_size - 1) & ~(priv->card_info.sdio_block_size - 1);
+
+    /*
+     * (ETH_HLEN + 2) bytes tailroom for header manipulation
+     * CSR_WIFI_ALIGN_BYTES bytes headroom for alignment manipulation
+     */
+    skb = dev_alloc_skb(rounded_length + 2 + ETH_HLEN + CSR_WIFI_ALIGN_BYTES);
+    if (! skb) {
+        unifi_error(ospriv, "alloc_skb failed.\n");
+        bulk_data_slot->os_net_buf_ptr = NULL;
+        bulk_data_slot->net_buf_length = 0;
+        bulk_data_slot->os_data_ptr = NULL;
+        bulk_data_slot->data_length = 0;
+        return CSR_RESULT_FAILURE;
+    }
+
+    bulk_data_slot->os_net_buf_ptr = (const unsigned char*)skb;
+    bulk_data_slot->net_buf_length = rounded_length + 2 + ETH_HLEN + CSR_WIFI_ALIGN_BYTES;
+    bulk_data_slot->os_data_ptr = (const void*)skb->data;
+    bulk_data_slot->data_length = size;
+
+    return CSR_RESULT_SUCCESS;
+} /* unifi_net_data_malloc() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_net_data_free
+ *
+ *      Free an OS specific net data buffer.
+ *      The bulk_data_slot.length must be initialised to 0.
+ *
+ *
+ *  Arguments:
+ *      ospriv              Pointer to device private context struct.
+ *      bulk_data_slot      Pointer to the bulk data structure that
+ *                          holds the data to be freed.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_net_data_free(void *ospriv, bulk_data_desc_t *bulk_data_slot)
+{
+    struct sk_buff *skb;
+    CSR_UNUSED(ospriv);
+
+    skb = (struct sk_buff *)bulk_data_slot->os_net_buf_ptr;
+    dev_kfree_skb(skb);
+
+    bulk_data_slot->net_buf_length = 0;
+    bulk_data_slot->data_length = 0;
+    bulk_data_slot->os_data_ptr = bulk_data_slot->os_net_buf_ptr = NULL;
+
+} /* unifi_net_data_free() */
+
+
+/*
+* ---------------------------------------------------------------------------
+*  unifi_net_dma_align
+*
+*      DMA align an OS specific net data buffer.
+*      The buffer must be empty.
+*
+*
+*  Arguments:
+*      ospriv              Pointer to device private context struct.
+*      bulk_data_slot      Pointer to the bulk data structure that
+*                          holds the data to be aligned.
+*
+*  Returns:
+*      None.
+* ---------------------------------------------------------------------------
+*/
+CsrResult
+unifi_net_dma_align(void *ospriv, bulk_data_desc_t *bulk_data_slot)
+{
+    struct sk_buff *skb;
+    unsigned long buf_address;
+    int offset;
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+
+    if ((bulk_data_slot == NULL) || (CSR_WIFI_ALIGN_BYTES == 0)) {
+        return CSR_RESULT_SUCCESS;
+    }
+
+    if ((bulk_data_slot->os_data_ptr == NULL) || (bulk_data_slot->data_length == 0)) {
+        return CSR_RESULT_SUCCESS;
+    }
+
+    buf_address = (unsigned long)(bulk_data_slot->os_data_ptr) & (CSR_WIFI_ALIGN_BYTES - 1);
+
+    unifi_trace(priv, UDBG5,
+                "unifi_net_dma_align: Allign buffer (0x%p) by %d bytes\n",
+                bulk_data_slot->os_data_ptr, buf_address);
+
+    offset = CSR_WIFI_ALIGN_BYTES - buf_address;
+    if (offset < 0) {
+        unifi_error(priv, "unifi_net_dma_align: Failed (offset=%d)\n", offset);
+        return CSR_RESULT_FAILURE;
+    }
+
+    skb = (struct sk_buff*)(bulk_data_slot->os_net_buf_ptr);
+    skb_reserve(skb, offset);
+    bulk_data_slot->os_net_buf_ptr = (const unsigned char*)skb;
+    bulk_data_slot->os_data_ptr = (const void*)(skb->data);
+
+    return CSR_RESULT_SUCCESS;
+
+} /* unifi_net_dma_align() */
+
+#ifdef ANDROID_TIMESTAMP
+static volatile unsigned int printk_cpu = UINT_MAX;
+char tbuf[30];
+
+char* print_time(void )
+{
+    unsigned long long t;
+    unsigned long nanosec_rem;
+
+    t = cpu_clock(printk_cpu);
+    nanosec_rem = do_div(t, 1000000000);
+    sprintf(tbuf, "[%5lu.%06lu] ",
+                    (unsigned long) t,
+                    nanosec_rem / 1000);
+
+    return tbuf;
+}
+#endif
+
+
+/* Module parameters */
+extern int unifi_debug;
+
+#ifdef UNIFI_DEBUG
+#define DEBUG_BUFFER_SIZE       120
+
+#define FORMAT_TRACE(_s, _len, _args, _fmt)             \
+    do {                                                \
+        va_start(_args, _fmt);                          \
+        _len += vsnprintf(&(_s)[_len],                  \
+                         (DEBUG_BUFFER_SIZE - _len),    \
+                         _fmt, _args);                  \
+        va_end(_args);                                  \
+        if (_len >= DEBUG_BUFFER_SIZE) {                \
+            (_s)[DEBUG_BUFFER_SIZE - 2] = '\n';         \
+            (_s)[DEBUG_BUFFER_SIZE - 1] = 0;            \
+        }                                               \
+    } while (0)
+
+void
+unifi_error(void* ospriv, const char *fmt, ...)
+{
+    unifi_priv_t *priv = (unifi_priv_t*) ospriv;
+    char s[DEBUG_BUFFER_SIZE];
+    va_list args;
+    unsigned int len;
+#ifdef ANDROID_TIMESTAMP
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "%s unifi%d: ", print_time(), priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "%s unifi: ", print_time());
+    }
+#else
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "unifi%d: ", priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "unifi: ");
+    }
+#endif /* ANDROID_TIMESTAMP */
+    FORMAT_TRACE(s, len, args, fmt);
+
+    printk("%s", s);
+}
+
+void
+unifi_warning(void* ospriv, const char *fmt, ...)
+{
+    unifi_priv_t *priv = (unifi_priv_t*) ospriv;
+    char s[DEBUG_BUFFER_SIZE];
+    va_list args;
+    unsigned int len;
+
+#ifdef ANDROID_TIMESTAMP
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_WARNING "%s unifi%d: ", print_time(), priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_WARNING "%s unifi: ", print_time());
+    }
+#else
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_WARNING "unifi%d: ", priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_WARNING "unifi: ");
+    }
+#endif /* ANDROID_TIMESTAMP */
+
+    FORMAT_TRACE(s, len, args, fmt);
+
+    printk("%s", s);
+}
+
+
+void
+unifi_notice(void* ospriv, const char *fmt, ...)
+{
+    unifi_priv_t *priv = (unifi_priv_t*) ospriv;
+    char s[DEBUG_BUFFER_SIZE];
+    va_list args;
+    unsigned int len;
+
+#ifdef ANDROID_TIMESTAMP
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_NOTICE "%s unifi%d: ", print_time(), priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_NOTICE "%s unifi: ", print_time());
+    }
+#else
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_NOTICE "unifi%d: ", priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_NOTICE "unifi: ");
+    }
+#endif /* ANDROID_TIMESTAMP */
+
+    FORMAT_TRACE(s, len, args, fmt);
+
+    printk("%s", s);
+}
+
+
+void
+unifi_info(void* ospriv, const char *fmt, ...)
+{
+    unifi_priv_t *priv = (unifi_priv_t*) ospriv;
+    char s[DEBUG_BUFFER_SIZE];
+    va_list args;
+    unsigned int len;
+
+#ifdef ANDROID_TIMESTAMP
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_INFO "%s unifi%d: ", print_time(), priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_INFO "%s unifi: ", print_time());
+    }
+#else
+    if (priv != NULL) {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_INFO "unifi%d: ", priv->instance);
+    } else {
+        len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_INFO "unifi: ");
+    }
+#endif /* ANDROID_TIMESTAMP */
+
+    FORMAT_TRACE(s, len, args, fmt);
+
+    printk("%s", s);
+}
+
+/* debugging */
+void
+unifi_trace(void* ospriv, int level, const char *fmt, ...)
+{
+    unifi_priv_t *priv = (unifi_priv_t*) ospriv;
+    char s[DEBUG_BUFFER_SIZE];
+    va_list args;
+    unsigned int len;
+
+    if (unifi_debug >= level) {
+#ifdef ANDROID_TIMESTAMP
+        if (priv != NULL) {
+            len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "%s unifi%d: ", print_time(), priv->instance);
+        } else {
+            len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "%s unifi: ", print_time());
+        }
+#else
+        if (priv != NULL) {
+            len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "unifi%d: ", priv->instance);
+        } else {
+            len = snprintf(s, DEBUG_BUFFER_SIZE, KERN_ERR "unifi: ");
+        }
+#endif /* ANDROID_TIMESTAMP */
+
+        FORMAT_TRACE(s, len, args, fmt);
+
+        printk("%s", s);
+    }
+}
+
+#else
+
+void
+unifi_error_nop(void* ospriv, const char *fmt, ...)
+{
+}
+
+void
+unifi_trace_nop(void* ospriv, int level, const char *fmt, ...)
+{
+}
+
+#endif /* UNIFI_DEBUG */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *      Debugging support.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#ifdef UNIFI_DEBUG
+
+/* Memory dump with level filter controlled by unifi_debug */
+void
+unifi_dump(void *ospriv, int level, const char *msg, void *mem, u16 len)
+{
+    unifi_priv_t *priv = (unifi_priv_t*) ospriv;
+
+    if (unifi_debug >= level) {
+#ifdef ANDROID_TIMESTAMP
+        if (priv != NULL) {
+            printk(KERN_ERR "%s unifi%d: --- dump: %s ---\n", print_time(), priv->instance, msg ? msg : "");
+        } else {
+            printk(KERN_ERR "%s unifi: --- dump: %s ---\n", print_time(), msg ? msg : "");
+        }
+#else
+        if (priv != NULL) {
+            printk(KERN_ERR "unifi%d: --- dump: %s ---\n", priv->instance, msg ? msg : "");
+        } else {
+            printk(KERN_ERR "unifi: --- dump: %s ---\n", msg ? msg : "");
+        }
+#endif /* ANDROID_TIMESTAMP */
+        dump(mem, len);
+
+        if (priv != NULL) {
+            printk(KERN_ERR "unifi%d: --- end of dump ---\n", priv->instance);
+        } else {
+            printk(KERN_ERR "unifi: --- end of dump ---\n");
+        }
+    }
+}
+
+/* Memory dump that appears all the time, use sparingly */
+void
+dump(void *mem, u16 len)
+{
+    int i, col = 0;
+    unsigned char *pdata = (unsigned char *)mem;
+#ifdef ANDROID_TIMESTAMP
+    printk("timestamp %s \n", print_time());
+#endif /* ANDROID_TIMESTAMP */
+    if (mem == NULL) {
+        printk("(null dump)\n");
+        return;
+    }
+    for (i = 0; i < len; i++) {
+        if (col == 0) {
+            printk("0x%02X: ", i);
+        }
+
+        printk(" %02X", pdata[i]);
+
+        if (++col == 16) {
+            printk("\n");
+            col = 0;
+        }
+    }
+    if (col) {
+        printk("\n");
+    }
+} /* dump() */
+
+
+void
+dump16(void *mem, u16 len)
+{
+    int i, col=0;
+    unsigned short *p = (unsigned short *)mem;
+#ifdef ANDROID_TIMESTAMP
+    printk("timestamp %s \n", print_time());
+#endif /* ANDROID_TIMESTAMP */
+    for (i = 0; i < len; i+=2) {
+        if (col == 0) {
+            printk("0x%02X: ", i);
+        }
+
+        printk(" %04X", *p++);
+
+        if (++col == 8) {
+            printk("\n");
+            col = 0;
+        }
+    }
+    if (col) {
+        printk("\n");
+    }
+}
+
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+void
+dump_str(void *mem, u16 len)
+{
+    int i, col = 0;
+    unsigned char *pdata = (unsigned char *)mem;
+#ifdef ANDROID_TIMESTAMP
+    printk("timestamp %s \n", print_time());
+#endif /* ANDROID_TIMESTAMP */
+    for (i = 0; i < len; i++) {
+        printk("%c", pdata[i]);
+    }
+    if (col) {
+        printk("\n");
+    }
+
+} /* dump_str() */
+#endif /* CSR_ONLY_NOTES */
+
+
+#endif /* UNIFI_DEBUG */
+
+
+/* ---------------------------------------------------------------------------
+ *                              - End -
+ * ------------------------------------------------------------------------- */
diff --git a/drivers/staging/csr/putest.c b/drivers/staging/csr/putest.c
new file mode 100644
index 00000000000..5613cf0e16b
--- /dev/null
+++ b/drivers/staging/csr/putest.c
@@ -0,0 +1,685 @@
+/*
+ * ***************************************************************************
+ *  FILE:     putest.c
+ *
+ *  PURPOSE:    putest related functions.
+ *
+ *  Copyright (C) 2008-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+
+#include <linux/vmalloc.h>
+#include <linux/firmware.h>
+
+#include "unifi_priv.h"
+#include "csr_wifi_hip_chiphelper.h"
+
+#define UNIFI_PROC_BOTH 3
+
+
+int unifi_putest_cmd52_read(unifi_priv_t *priv, unsigned char *arg)
+{
+    struct unifi_putest_cmd52 cmd52_params;
+    u8 *arg_pos;
+    unsigned int cmd_param_size;
+    int r;
+    CsrResult csrResult;
+    unsigned char ret_buffer[32];
+    u8 *ret_buffer_pos;
+    u8 retries;
+
+    arg_pos = (u8*)(((unifi_putest_command_t*)arg) + 1);
+    if (get_user(cmd_param_size, (int*)arg_pos)) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_read: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    if (cmd_param_size != sizeof(struct unifi_putest_cmd52)) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_read: cmd52 struct mismatch\n");
+        return -EINVAL;
+    }
+
+    arg_pos += sizeof(unsigned int);
+    if (copy_from_user(&cmd52_params,
+                       (void*)arg_pos,
+                       sizeof(struct unifi_putest_cmd52))) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_read: Failed to get the cmd52 params\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "cmd52r: func=%d addr=0x%x ",
+                cmd52_params.funcnum, cmd52_params.addr);
+
+    retries = 3;
+    CsrSdioClaim(priv->sdio);
+    do {
+        if (cmd52_params.funcnum == 0) {
+            csrResult = CsrSdioF0Read8(priv->sdio, cmd52_params.addr, &cmd52_params.data);
+        } else {
+            csrResult = CsrSdioRead8(priv->sdio, cmd52_params.addr, &cmd52_params.data);
+        }
+    } while (--retries && ((csrResult == CSR_SDIO_RESULT_CRC_ERROR) || (csrResult == CSR_SDIO_RESULT_TIMEOUT)));
+    CsrSdioRelease(priv->sdio);
+
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "\nunifi_putest_cmd52_read: Read8() failed (csrResult=0x%x)\n", csrResult);
+        return -EFAULT;
+    }
+    unifi_trace(priv, UDBG2, "data=%d\n", cmd52_params.data);
+
+    /* Copy the info to the out buffer */
+    *(unifi_putest_command_t*)ret_buffer = UNIFI_PUTEST_CMD52_READ;
+    ret_buffer_pos = (u8*)(((unifi_putest_command_t*)ret_buffer) + 1);
+    *(unsigned int*)ret_buffer_pos = sizeof(struct unifi_putest_cmd52);
+    ret_buffer_pos += sizeof(unsigned int);
+    memcpy(ret_buffer_pos, &cmd52_params, sizeof(struct unifi_putest_cmd52));
+    ret_buffer_pos += sizeof(struct unifi_putest_cmd52);
+
+    r = copy_to_user((void*)arg,
+                     ret_buffer,
+                     ret_buffer_pos - ret_buffer);
+    if (r) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_read: Failed to return the data\n");
+        return -EFAULT;
+    }
+
+    return 0;
+}
+
+
+int unifi_putest_cmd52_write(unifi_priv_t *priv, unsigned char *arg)
+{
+    struct unifi_putest_cmd52 cmd52_params;
+    u8 *arg_pos;
+    unsigned int cmd_param_size;
+    CsrResult csrResult;
+    u8 retries;
+
+    arg_pos = (u8*)(((unifi_putest_command_t*)arg) + 1);
+    if (get_user(cmd_param_size, (int*)arg_pos)) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_write: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    if (cmd_param_size != sizeof(struct unifi_putest_cmd52)) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_write: cmd52 struct mismatch\n");
+        return -EINVAL;
+    }
+
+    arg_pos += sizeof(unsigned int);
+    if (copy_from_user(&cmd52_params,
+                       (void*)(arg_pos),
+                       sizeof(struct unifi_putest_cmd52))) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_write: Failed to get the cmd52 params\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "cmd52w: func=%d addr=0x%x data=%d\n",
+                cmd52_params.funcnum, cmd52_params.addr, cmd52_params.data);
+
+    retries = 3;
+    CsrSdioClaim(priv->sdio);
+    do {
+        if (cmd52_params.funcnum == 0) {
+            csrResult = CsrSdioF0Write8(priv->sdio, cmd52_params.addr, cmd52_params.data);
+        } else {
+            csrResult = CsrSdioWrite8(priv->sdio, cmd52_params.addr, cmd52_params.data);
+        }
+    } while (--retries && ((csrResult == CSR_SDIO_RESULT_CRC_ERROR) || (csrResult == CSR_SDIO_RESULT_TIMEOUT)));
+    CsrSdioRelease(priv->sdio);
+
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_cmd52_write: Write8() failed (csrResult=0x%x)\n", csrResult);
+        return -EFAULT;
+    }
+
+    return 0;
+}
+
+int unifi_putest_gp_read16(unifi_priv_t *priv, unsigned char *arg)
+{
+    struct unifi_putest_gp_rw16 gp_r16_params;
+    u8 *arg_pos;
+    unsigned int cmd_param_size;
+    int r;
+    CsrResult csrResult;
+    unsigned char ret_buffer[32];
+    u8 *ret_buffer_pos;
+
+    arg_pos = (u8*)(((unifi_putest_command_t*)arg) + 1);
+    if (get_user(cmd_param_size, (int*)arg_pos)) {
+        unifi_error(priv,
+                    "unifi_putest_gp_read16: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    if (cmd_param_size != sizeof(struct unifi_putest_gp_rw16)) {
+        unifi_error(priv,
+                    "unifi_putest_gp_read16: struct mismatch\n");
+        return -EINVAL;
+    }
+
+    arg_pos += sizeof(unsigned int);
+    if (copy_from_user(&gp_r16_params,
+                       (void*)arg_pos,
+                       sizeof(struct unifi_putest_gp_rw16))) {
+        unifi_error(priv,
+                    "unifi_putest_gp_read16: Failed to get the params\n");
+        return -EFAULT;
+    }
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_card_read16(priv->card, gp_r16_params.addr, &gp_r16_params.data);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_gp_read16: unifi_card_read16() GP=0x%x failed (csrResult=0x%x)\n", gp_r16_params.addr, csrResult);
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "gp_r16: GP=0x%08x, data=0x%04x\n", gp_r16_params.addr, gp_r16_params.data);
+
+    /* Copy the info to the out buffer */
+    *(unifi_putest_command_t*)ret_buffer = UNIFI_PUTEST_GP_READ16;
+    ret_buffer_pos = (u8*)(((unifi_putest_command_t*)ret_buffer) + 1);
+    *(unsigned int*)ret_buffer_pos = sizeof(struct unifi_putest_gp_rw16);
+    ret_buffer_pos += sizeof(unsigned int);
+    memcpy(ret_buffer_pos, &gp_r16_params, sizeof(struct unifi_putest_gp_rw16));
+    ret_buffer_pos += sizeof(struct unifi_putest_gp_rw16);
+
+    r = copy_to_user((void*)arg,
+                     ret_buffer,
+                     ret_buffer_pos - ret_buffer);
+    if (r) {
+        unifi_error(priv,
+                    "unifi_putest_gp_read16: Failed to return the data\n");
+        return -EFAULT;
+    }
+
+    return 0;
+}
+
+int unifi_putest_gp_write16(unifi_priv_t *priv, unsigned char *arg)
+{
+    struct unifi_putest_gp_rw16 gp_w16_params;
+    u8 *arg_pos;
+    unsigned int cmd_param_size;
+    CsrResult csrResult;
+
+    arg_pos = (u8*)(((unifi_putest_command_t*)arg) + 1);
+    if (get_user(cmd_param_size, (int*)arg_pos)) {
+        unifi_error(priv,
+                    "unifi_putest_gp_write16: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    if (cmd_param_size != sizeof(struct unifi_putest_gp_rw16)) {
+        unifi_error(priv,
+                    "unifi_putest_gp_write16: struct mismatch\n");
+        return -EINVAL;
+    }
+
+    arg_pos += sizeof(unsigned int);
+    if (copy_from_user(&gp_w16_params,
+                       (void*)(arg_pos),
+                       sizeof(struct unifi_putest_gp_rw16))) {
+        unifi_error(priv,
+                    "unifi_putest_gp_write16: Failed to get the params\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "gp_w16: GP=0x%08x, data=0x%04x\n", gp_w16_params.addr, gp_w16_params.data);
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_card_write16(priv->card, gp_w16_params.addr, gp_w16_params.data);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_gp_write16: unifi_card_write16() GP=%x failed (csrResult=0x%x)\n", gp_w16_params.addr, csrResult);
+        return -EFAULT;
+    }
+
+    return 0;
+}
+
+int unifi_putest_set_sdio_clock(unifi_priv_t *priv, unsigned char *arg)
+{
+    int sdio_clock_speed;
+    CsrResult csrResult;
+
+    if (get_user(sdio_clock_speed, (int*)(((unifi_putest_command_t*)arg) + 1))) {
+        unifi_error(priv,
+                    "unifi_putest_set_sdio_clock: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "set sdio clock: %d KHz\n", sdio_clock_speed);
+
+    CsrSdioClaim(priv->sdio);
+    csrResult = CsrSdioMaxBusClockFrequencySet(priv->sdio, sdio_clock_speed * 1000);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_set_sdio_clock: Set clock failed (csrResult=0x%x)\n", csrResult);
+        return -EFAULT;
+    }
+
+    return 0;
+}
+
+
+int unifi_putest_start(unifi_priv_t *priv, unsigned char *arg)
+{
+    int r;
+    CsrResult csrResult;
+    int already_in_test = priv->ptest_mode;
+
+    /* Ensure that sme_sys_suspend() doesn't power down the chip because:
+     *  1) Power is needed anyway for ptest.
+     *  2) The app code uses the START ioctl as a reset, so it gets called
+     *     multiple times. If the app stops the XAPs, but the power_down/up
+     *     sequence doesn't actually power down the chip, there can be problems
+     *     resetting, because part of the power_up sequence disables function 1
+     */
+    priv->ptest_mode = 1;
+
+    /* Suspend the SME and UniFi */
+    if (priv->sme_cli) {
+        r = sme_sys_suspend(priv);
+        if (r) {
+            unifi_error(priv,
+                        "unifi_putest_start: failed to suspend UniFi\n");
+            return r;
+        }
+    }
+
+    /* Application may have stopped the XAPs, but they are needed for reset */
+    if (already_in_test) {
+        CsrSdioClaim(priv->sdio);
+        csrResult = unifi_start_processors(priv->card);
+        CsrSdioRelease(priv->sdio);
+        if (csrResult != CSR_RESULT_SUCCESS) {
+            unifi_error(priv, "Failed to start XAPs. Hard reset required.\n");
+        }
+    } else {
+        /* Ensure chip is powered for the case where there's no unifi_helper */
+        CsrSdioClaim(priv->sdio);
+        csrResult = CsrSdioPowerOn(priv->sdio);
+        CsrSdioRelease(priv->sdio);
+        if (csrResult != CSR_RESULT_SUCCESS) {
+            unifi_error(priv, "CsrSdioPowerOn csrResult = %d\n", csrResult);
+        }
+    }
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_init(priv->card);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_start: failed to init UniFi\n");
+        return CsrHipResultToStatus(csrResult);
+    }
+
+    return 0;
+}
+
+
+int unifi_putest_stop(unifi_priv_t *priv, unsigned char *arg)
+{
+    int r = 0;
+    CsrResult csrResult;
+
+    /* Application may have stopped the XAPs, but they are needed for reset */
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_start_processors(priv->card);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Failed to start XAPs. Hard reset required.\n");
+    }
+
+    /* PUTEST_STOP is also used to resume the XAPs after SME coredump.
+     * Don't power off the chip, leave that to the normal wifi-off which is
+     * about to carry on. No need to resume the SME either, as it wasn't suspended.
+     */
+    if (priv->coredump_mode) {
+        priv->coredump_mode = 0;
+        return 0;
+    }
+
+    /* At this point function 1 is enabled and the XAPs are running, so it is
+     * safe to let the card power down. Power is restored later, asynchronously,
+     * during the wifi_on requested by the SME.
+     */
+    CsrSdioClaim(priv->sdio);
+    CsrSdioPowerOff(priv->sdio);
+    CsrSdioRelease(priv->sdio);
+
+    /* Resume the SME and UniFi */
+    if (priv->sme_cli) {
+        r = sme_sys_resume(priv);
+        if (r) {
+            unifi_error(priv,
+                        "unifi_putest_stop: failed to resume SME\n");
+        }
+    }
+    priv->ptest_mode = 0;
+
+    return r;
+}
+
+
+int unifi_putest_dl_fw(unifi_priv_t *priv, unsigned char *arg)
+{
+#define UF_PUTEST_MAX_FW_FILE_NAME      16
+#define UNIFI_MAX_FW_PATH_LEN           32
+    unsigned int fw_name_length;
+    unsigned char fw_name[UF_PUTEST_MAX_FW_FILE_NAME+1];
+    unsigned char *name_buffer;
+    int postfix;
+    char fw_path[UNIFI_MAX_FW_PATH_LEN];
+    const struct firmware *fw_entry;
+    struct dlpriv temp_fw_sta;
+    int r;
+    CsrResult csrResult;
+
+    /* Get the f/w file name length */
+    if (get_user(fw_name_length, (unsigned int*)(((unifi_putest_command_t*)arg) + 1))) {
+        unifi_error(priv,
+                    "unifi_putest_dl_fw: Failed to get the length argument\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "unifi_putest_dl_fw: file name size = %d\n", fw_name_length);
+
+    /* Sanity check for the f/w file name length */
+    if (fw_name_length > UF_PUTEST_MAX_FW_FILE_NAME) {
+        unifi_error(priv,
+                    "unifi_putest_dl_fw: F/W file name is too long\n");
+        return -EINVAL;
+    }
+
+    /* Get the f/w file name */
+    name_buffer = ((unsigned char*)arg) + sizeof(unifi_putest_command_t) + sizeof(unsigned int);
+    if (copy_from_user(fw_name, (void*)name_buffer, fw_name_length)) {
+        unifi_error(priv, "unifi_putest_dl_fw: Failed to get the file name\n");
+        return -EFAULT;
+    }
+    fw_name[fw_name_length] = '\0';
+    unifi_trace(priv, UDBG2, "unifi_putest_dl_fw: file = %s\n", fw_name);
+
+    /* Keep the existing f/w to a temp, we need to restore it later */
+    temp_fw_sta = priv->fw_sta;
+
+    /* Get the putest f/w */
+    postfix = priv->instance;
+    scnprintf(fw_path, UNIFI_MAX_FW_PATH_LEN, "unifi-sdio-%d/%s",
+              postfix, fw_name);
+    r = request_firmware(&fw_entry, fw_path, priv->unifi_device);
+    if (r == 0) {
+        priv->fw_sta.fw_desc = (void *)fw_entry;
+        priv->fw_sta.dl_data = fw_entry->data;
+        priv->fw_sta.dl_len = fw_entry->size;
+    } else {
+        unifi_error(priv, "Firmware file not available\n");
+        return -EINVAL;
+    }
+
+    /* Application may have stopped the XAPs, but they are needed for reset */
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_start_processors(priv->card);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Failed to start XAPs. Hard reset required.\n");
+    }
+
+    /* Download the f/w. On UF6xxx this will cause the f/w file to convert
+     * into patch format and download via the ROM boot loader
+     */
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_download(priv->card, 0x0c00);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_dl_fw: failed to download the f/w\n");
+        goto free_fw;
+    }
+
+    /* Free the putest f/w... */
+free_fw:
+    uf_release_firmware(priv, &priv->fw_sta);
+    /* ... and restore the original f/w */
+    priv->fw_sta = temp_fw_sta;
+
+    return CsrHipResultToStatus(csrResult);
+}
+
+
+int unifi_putest_dl_fw_buff(unifi_priv_t *priv, unsigned char *arg)
+{
+    unsigned int fw_length;
+    unsigned char *fw_buf = NULL;
+    unsigned char *fw_user_ptr;
+    struct dlpriv temp_fw_sta;
+    CsrResult csrResult;
+
+    /* Get the f/w buffer length */
+    if (get_user(fw_length, (unsigned int*)(((unifi_putest_command_t*)arg) + 1))) {
+        unifi_error(priv,
+                    "unifi_putest_dl_fw_buff: Failed to get the length arg\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "unifi_putest_dl_fw_buff: size = %d\n", fw_length);
+
+    /* Sanity check for the buffer length */
+    if (fw_length == 0 || fw_length > 0xfffffff) {
+        unifi_error(priv,
+                    "unifi_putest_dl_fw_buff: buffer length bad %u\n", fw_length);
+        return -EINVAL;
+    }
+
+    /* Buffer for kernel copy of the f/w image */
+    fw_buf = kmalloc(fw_length, GFP_KERNEL);
+    if (!fw_buf) {
+        unifi_error(priv, "unifi_putest_dl_fw_buff: malloc fail\n");
+        return -ENOMEM;
+    }
+
+    /* Get the f/w image */
+    fw_user_ptr = ((unsigned char*)arg) + sizeof(unifi_putest_command_t) + sizeof(unsigned int);
+    if (copy_from_user(fw_buf, (void*)fw_user_ptr, fw_length)) {
+        unifi_error(priv, "unifi_putest_dl_fw_buff: Failed to get the buffer\n");
+        kfree(fw_buf);
+        return -EFAULT;
+    }
+
+    /* Save the existing f/w to a temp, we need to restore it later */
+    temp_fw_sta = priv->fw_sta;
+
+    /* Setting fw_desc NULL indicates to the core that no f/w file was loaded
+     * via the kernel request_firmware() mechanism. This indicates to the core
+     * that it shouldn't call release_firmware() after the download is done.
+     */
+    priv->fw_sta.fw_desc = NULL;            /* No OS f/w resource */
+    priv->fw_sta.dl_data = fw_buf;
+    priv->fw_sta.dl_len = fw_length;
+
+    /* Application may have stopped the XAPs, but they are needed for reset */
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_start_processors(priv->card);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Failed to start XAPs. Hard reset required.\n");
+    }
+
+    /* Download the f/w. On UF6xxx this will cause the f/w file to convert
+     * into patch format and download via the ROM boot loader
+     */
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_download(priv->card, 0x0c00);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv,
+                    "unifi_putest_dl_fw_buff: failed to download the f/w\n");
+        goto free_fw;
+    }
+
+free_fw:
+    /* Finished with the putest f/w, so restore the station f/w */
+    priv->fw_sta = temp_fw_sta;
+    kfree(fw_buf);
+
+    return CsrHipResultToStatus(csrResult);
+}
+
+
+int unifi_putest_coredump_prepare(unifi_priv_t *priv, unsigned char *arg)
+{
+    u16 data_u16;
+    s32 i;
+    CsrResult r;
+
+    unifi_info(priv, "Preparing for SDIO coredump\n");
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE)
+    unifi_debug_buf_dump();
+#endif
+
+    /* Sanity check that userspace hasn't called a PUTEST_START, because that
+     * would have reset UniFi, potentially power cycling it and losing context
+     */
+    if (priv->ptest_mode) {
+        unifi_error(priv, "PUTEST_START shouldn't be used before a coredump\n");
+    }
+
+    /* Flag that the userspace has requested coredump. Even if this preparation
+     * fails, the SME will call PUTEST_STOP to tidy up.
+     */
+    priv->coredump_mode = 1;
+
+    for (i = 0; i < 3; i++) {
+        CsrSdioClaim(priv->sdio);
+        r = CsrSdioRead16(priv->sdio, CHIP_HELPER_UNIFI_GBL_CHIP_VERSION*2, &data_u16);
+        CsrSdioRelease(priv->sdio);
+        if (r != CSR_RESULT_SUCCESS) {
+            unifi_info(priv, "Failed to read chip version! Try %d\n", i);
+
+            /* First try, re-enable function which may have been disabled by f/w panic */
+            if (i == 0) {
+                unifi_info(priv, "Try function enable\n");
+                CsrSdioClaim(priv->sdio);
+                r = CsrSdioFunctionEnable(priv->sdio);
+                CsrSdioRelease(priv->sdio);
+                if (r != CSR_RESULT_SUCCESS) {
+                    unifi_error(priv, "CsrSdioFunctionEnable failed %d\n", r);
+                }
+                continue;
+            }
+
+            /* Subsequent tries, reset */
+
+            /* Set clock speed low */
+            CsrSdioClaim(priv->sdio);
+            r = CsrSdioMaxBusClockFrequencySet(priv->sdio, UNIFI_SDIO_CLOCK_SAFE_HZ);
+            CsrSdioRelease(priv->sdio);
+            if (r != CSR_RESULT_SUCCESS) {
+                unifi_error(priv, "CsrSdioMaxBusClockFrequencySet() failed %d\n", r);
+            }
+
+            /* Card software reset */
+            CsrSdioClaim(priv->sdio);
+            r = unifi_card_hard_reset(priv->card);
+            CsrSdioRelease(priv->sdio);
+            if (r != CSR_RESULT_SUCCESS) {
+                unifi_error(priv, "unifi_card_hard_reset() failed %d\n", r);
+            }
+        } else {
+            unifi_info(priv, "Read chip version of 0x%04x\n", data_u16);
+            break;
+        }
+    }
+
+    if (r != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Failed to prepare chip\n");
+        return -EIO;
+    }
+
+    /* Stop the XAPs for coredump. The PUTEST_STOP must be called, e.g. at
+     * Raw SDIO deinit, to resume them.
+     */
+    CsrSdioClaim(priv->sdio);
+    r = unifi_card_stop_processor(priv->card, UNIFI_PROC_BOTH);
+    CsrSdioRelease(priv->sdio);
+    if (r != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Failed to stop processors\n");
+    }
+
+    return 0;
+}
+
+int unifi_putest_cmd52_block_read(unifi_priv_t *priv, unsigned char *arg)
+{
+    struct unifi_putest_block_cmd52_r block_cmd52;
+    u8 *arg_pos;
+    unsigned int cmd_param_size;
+    CsrResult r;
+    u8 *block_local_buffer;
+
+    arg_pos = (u8*)(((unifi_putest_command_t*)arg) + 1);
+    if (get_user(cmd_param_size, (int*)arg_pos)) {
+        unifi_error(priv,
+                    "cmd52r_block: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    if (cmd_param_size != sizeof(struct unifi_putest_block_cmd52_r)) {
+        unifi_error(priv,
+                    "cmd52r_block: cmd52 struct mismatch\n");
+        return -EINVAL;
+    }
+
+    arg_pos += sizeof(unsigned int);
+    if (copy_from_user(&block_cmd52,
+                       (void*)arg_pos,
+                       sizeof(struct unifi_putest_block_cmd52_r))) {
+        unifi_error(priv,
+                    "cmd52r_block: Failed to get the cmd52 params\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG2, "cmd52r_block: func=%d addr=0x%x len=0x%x ",
+                block_cmd52.funcnum, block_cmd52.addr, block_cmd52.length);
+
+    block_local_buffer = vmalloc(block_cmd52.length);
+    if (block_local_buffer == NULL) {
+        unifi_error(priv, "cmd52r_block: Failed to allocate buffer\n");
+        return -ENOMEM;
+    }
+
+    CsrSdioClaim(priv->sdio);
+    r = unifi_card_readn(priv->card, block_cmd52.addr, block_local_buffer, block_cmd52.length);
+    CsrSdioRelease(priv->sdio);
+    if (r != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "cmd52r_block: unifi_readn failed\n");
+        return -EIO;
+    }
+
+    if (copy_to_user((void*)block_cmd52.data,
+                     block_local_buffer,
+                     block_cmd52.length)) {
+        unifi_error(priv,
+                    "cmd52r_block: Failed to return the data\n");
+        return -EFAULT;
+    }
+
+    return 0;
+}
diff --git a/drivers/staging/csr/sdio_events.c b/drivers/staging/csr/sdio_events.c
new file mode 100644
index 00000000000..6892c2e281b
--- /dev/null
+++ b/drivers/staging/csr/sdio_events.c
@@ -0,0 +1,134 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     sdio_events.c
+ *
+ * PURPOSE:
+ *      Process the events received by the SDIO glue layer.
+ *      Optional part of the porting exercise.
+ *
+ * Copyright (C) 2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include "unifi_priv.h"
+
+
+/*
+ * Porting Notes:
+ * There are two ways to support the suspend/resume system events in a driver.
+ * In some operating systems these events are delivered to the OS driver
+ * directly from the system. In this case, the OS driver needs to pass these
+ * events to the API described in the CSR SDIO Abstration API document.
+ * In Linux, and other embedded operating systems, the suspend/resume events
+ * come from the SDIO driver. In this case, simply get these events in the
+ * SDIO glue layer and notify the OS layer.
+ *
+ * In either case, typically, the events are processed by the SME.
+ * Use the unifi_sys_suspend_ind() and unifi_sys_resume_ind() to pass
+ * the events to the SME.
+ */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_suspend
+ *
+ *      Handles a suspend request from the SDIO driver.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to OS driver context.
+ *
+ * ---------------------------------------------------------------------------
+ */
+void unifi_suspend(void *ospriv)
+{
+    unifi_priv_t *priv = ospriv;
+    int interfaceTag=0;
+
+    /* For powered suspend, tell the resume's wifi_on() not to reinit UniFi */
+    priv->wol_suspend = (enable_wol == UNIFI_WOL_OFF) ? FALSE : TRUE;
+
+    unifi_trace(priv, UDBG1, "unifi_suspend: wol_suspend %d, enable_wol %d",
+                priv->wol_suspend, enable_wol );
+
+    /* Stop network traffic. */
+    /* need to stop all the netdevices*/
+    for( interfaceTag=0;interfaceTag<CSR_WIFI_NUM_INTERFACES;interfaceTag++)
+    {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+        if (interfacePriv->netdev_registered == 1)
+        {
+            if( priv->wol_suspend ) {
+                unifi_trace(priv, UDBG1, "unifi_suspend: Don't netif_carrier_off");
+            } else {
+                unifi_trace(priv, UDBG1, "unifi_suspend: netif_carrier_off");
+                netif_carrier_off(priv->netdev[interfaceTag]);
+            }
+            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[interfaceTag]);
+        }
+    }
+
+    unifi_trace(priv, UDBG1, "unifi_suspend: suspend SME");
+
+    sme_sys_suspend(priv);
+
+} /* unifi_suspend() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_resume
+ *
+ *      Handles a resume request from the SDIO driver.
+ *
+ *  Arguments:
+ *      ospriv          Pointer to OS driver context.
+ *
+ * ---------------------------------------------------------------------------
+ */
+void unifi_resume(void *ospriv)
+{
+    unifi_priv_t *priv = ospriv;
+    int interfaceTag=0;
+    int r;
+    int wol = priv->wol_suspend;
+
+    unifi_trace(priv, UDBG1, "unifi_resume: resume SME, enable_wol=%d", enable_wol);
+
+    /* The resume causes wifi-on which will re-enable the BH and reinstall the ISR */
+    r = sme_sys_resume(priv);
+    if (r) {
+        unifi_error(priv, "Failed to resume UniFi\n");
+    }
+
+    /* Resume the network interfaces. For the cold resume case, this will
+     * happen upon reconnection.
+     */
+    if (wol) {
+        unifi_trace(priv, UDBG1, "unifi_resume: try to enable carrier");
+
+        /* need to start all the netdevices*/
+        for( interfaceTag=0;interfaceTag<CSR_WIFI_NUM_INTERFACES;interfaceTag++) {
+            netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+            unifi_trace(priv, UDBG1, "unifi_resume: interfaceTag %d netdev_registered %d mode %d\n",
+                   interfaceTag, interfacePriv->netdev_registered, interfacePriv->interfaceMode);
+
+            if (interfacePriv->netdev_registered == 1)
+            {
+                netif_carrier_on(priv->netdev[interfaceTag]);
+                UF_NETIF_TX_START_ALL_QUEUES(priv->netdev[interfaceTag]);
+            }
+        }
+
+        /* Kick the BH thread (with reason=host) to poll for data that may have
+         * arrived during a powered suspend. This caters for the case where the SME
+         * doesn't interact with the chip (e.g install autonomous scans) during resume.
+         */
+        unifi_send_signal(priv->card, NULL, 0, NULL);
+    }
+
+} /* unifi_resume() */
+
diff --git a/drivers/staging/csr/sdio_mmc.c b/drivers/staging/csr/sdio_mmc.c
new file mode 100644
index 00000000000..d3fd57cdde0
--- /dev/null
+++ b/drivers/staging/csr/sdio_mmc.c
@@ -0,0 +1,1340 @@
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * FILE: sdio_mmc.c
+ *
+ * PURPOSE: SDIO driver interface for generic MMC stack.
+ *
+ * Copyright (C) 2008-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/gfp.h>
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio.h>
+#include <linux/suspend.h>
+
+#include "unifi_priv.h"
+
+#ifdef ANDROID_BUILD
+struct wake_lock unifi_sdio_wake_lock; /* wakelock to prevent suspend while resuming */
+#endif
+
+static CsrSdioFunctionDriver *sdio_func_drv;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#ifdef CONFIG_PM
+static int uf_sdio_mmc_power_event(struct notifier_block *this, unsigned long event, void *ptr);
+#endif
+
+/*
+ * We need to keep track of the power on/off because we can not call
+ * mmc_power_restore_host() when the card is already powered.
+ * Even then, we need to patch the MMC driver to add a power_restore handler
+ * in the mmc_sdio_ops structure. If the MMC driver before 2.6.37 is not patched,
+ * mmc_power_save_host() and mmc_power_restore_host() are no-ops in the kernel,
+ * returning immediately (at least on x86).
+ */
+static int card_is_powered = 1;
+#endif /* 2.6.32 */
+
+/* MMC uses ENOMEDIUM to indicate card gone away */
+
+static CsrResult
+ConvertSdioToCsrSdioResult(int r)
+{
+    CsrResult csrResult = CSR_RESULT_FAILURE;
+
+    switch (r) {
+    case 0:
+        csrResult = CSR_RESULT_SUCCESS;
+    break;
+    case -EIO:
+    case -EILSEQ:
+        csrResult = CSR_SDIO_RESULT_CRC_ERROR;
+    break;
+    /* Timeout errors */
+    case -ETIMEDOUT:
+    case -EBUSY:
+        csrResult = CSR_SDIO_RESULT_TIMEOUT;
+    break;
+    case -ENODEV:
+    case -ENOMEDIUM:
+        csrResult = CSR_SDIO_RESULT_NO_DEVICE;
+    break;
+    case -EINVAL:
+        csrResult = CSR_SDIO_RESULT_INVALID_VALUE;
+    break;
+    case -ENOMEM:
+    case -ENOSYS:
+    case -ERANGE:
+    case -ENXIO:
+        csrResult = CSR_RESULT_FAILURE;
+    break;
+    default:
+        unifi_warning(NULL, "Unrecognised SDIO error code: %d\n", r);
+    break;
+    }
+
+    return csrResult;
+}
+
+
+static int
+csr_io_rw_direct(struct mmc_card *card, int write, uint8_t fn,
+                 uint32_t addr, uint8_t in, uint8_t* out)
+{
+    struct mmc_command cmd;
+    int err;
+
+    BUG_ON(!card);
+    BUG_ON(fn > 7);
+
+    memset(&cmd, 0, sizeof(struct mmc_command));
+
+    cmd.opcode = SD_IO_RW_DIRECT;
+    cmd.arg = write ? 0x80000000 : 0x00000000;
+    cmd.arg |= fn << 28;
+    cmd.arg |= (write && out) ? 0x08000000 : 0x00000000;
+    cmd.arg |= addr << 9;
+    cmd.arg |= in;
+    cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_AC;
+
+    err = mmc_wait_for_cmd(card->host, &cmd, 0);
+    if (err)
+        return err;
+
+    /* this function is not exported, so we will need to sort it out here
+     * for now, lets hard code it to sdio */
+    if (0) {
+        /* old arg (mmc_host_is_spi(card->host)) { */
+        /* host driver already reported errors */
+    } else {
+        if (cmd.resp[0] & R5_ERROR) {
+            printk(KERN_ERR "%s: r5 error 0x%02x\n",
+                   __FUNCTION__, cmd.resp[0]);
+            return -EIO;
+        }
+        if (cmd.resp[0] & R5_FUNCTION_NUMBER)
+            return -EINVAL;
+        if (cmd.resp[0] & R5_OUT_OF_RANGE)
+            return -ERANGE;
+    }
+
+    if (out) {
+        if (0) {    /* old argument (mmc_host_is_spi(card->host)) */
+            *out = (cmd.resp[0] >> 8) & 0xFF;
+        }
+        else {
+            *out = cmd.resp[0] & 0xFF;
+        }
+    }
+
+    return CSR_RESULT_SUCCESS;
+}
+
+
+CsrResult
+CsrSdioRead8(CsrSdioFunction *function, u32 address, u8 *data)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err = 0;
+
+    _sdio_claim_host(func);
+    *data = sdio_readb(func, address, &err);
+    _sdio_release_host(func);
+
+    if (err) {
+        func_exit_r(err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioRead8() */
+
+CsrResult
+CsrSdioWrite8(CsrSdioFunction *function, u32 address, u8 data)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err = 0;
+
+    _sdio_claim_host(func);
+    sdio_writeb(func, data, address, &err);
+    _sdio_release_host(func);
+
+    if (err) {
+        func_exit_r(err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioWrite8() */
+
+CsrResult
+CsrSdioRead16(CsrSdioFunction *function, u32 address, u16 *data)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err;
+    uint8_t b0, b1;
+
+    _sdio_claim_host(func);
+    b0 = sdio_readb(func, address, &err);
+    if (err) {
+        _sdio_release_host(func);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    b1 = sdio_readb(func, address+1, &err);
+    if (err) {
+        _sdio_release_host(func);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+    _sdio_release_host(func);
+
+    *data = ((uint16_t)b1 << 8) | b0;
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioRead16() */
+
+
+CsrResult
+CsrSdioWrite16(CsrSdioFunction *function, u32 address, u16 data)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err;
+    uint8_t b0, b1;
+
+    _sdio_claim_host(func);
+    b1 = (data >> 8) & 0xFF;
+    sdio_writeb(func, b1, address+1, &err);
+    if (err) {
+        _sdio_release_host(func);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    b0 = data & 0xFF;
+    sdio_writeb(func, b0, address, &err);
+    if (err) {
+        _sdio_release_host(func);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    _sdio_release_host(func);
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioWrite16() */
+
+
+CsrResult
+CsrSdioF0Read8(CsrSdioFunction *function, u32 address, u8 *data)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err = 0;
+
+    _sdio_claim_host(func);
+#ifdef MMC_QUIRK_LENIENT_FN0
+    *data = sdio_f0_readb(func, address, &err);
+#else
+    err = csr_io_rw_direct(func->card, 0, 0, address, 0, data);
+#endif
+    _sdio_release_host(func);
+
+    if (err) {
+        func_exit_r(err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioF0Read8() */
+
+CsrResult
+CsrSdioF0Write8(CsrSdioFunction *function, u32 address, u8 data)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err = 0;
+
+    _sdio_claim_host(func);
+#ifdef MMC_QUIRK_LENIENT_FN0
+    sdio_f0_writeb(func, data, address, &err);
+#else
+    err = csr_io_rw_direct(func->card, 1, 0, address, data, NULL);
+#endif
+    _sdio_release_host(func);
+
+    if (err) {
+        func_exit_r(err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioF0Write8() */
+
+
+CsrResult
+CsrSdioRead(CsrSdioFunction *function, u32 address, void *data, u32 length)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err;
+
+    _sdio_claim_host(func);
+    err = sdio_readsb(func, data, address, length);
+    _sdio_release_host(func);
+
+    if (err) {
+        func_exit_r(err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioRead() */
+
+CsrResult
+CsrSdioWrite(CsrSdioFunction *function, u32 address, const void *data, u32 length)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err;
+
+    _sdio_claim_host(func);
+    err = sdio_writesb(func, address, (void*)data, length);
+    _sdio_release_host(func);
+
+    if (err) {
+        func_exit_r(err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioWrite() */
+
+
+static int
+csr_sdio_enable_hs(struct mmc_card *card)
+{
+    int ret;
+    u8 speed;
+
+    if (!(card->host->caps & MMC_CAP_SD_HIGHSPEED)) {
+        /* We've asked for HS clock rates, but controller doesn't
+         * claim to support it. We should limit the clock
+         * to 25MHz via module parameter.
+         */
+        printk(KERN_INFO "unifi: request HS but not MMC_CAP_SD_HIGHSPEED");
+        return 0;
+    }
+
+    if (!card->cccr.high_speed)
+        return 0;
+
+#if 1
+    ret = csr_io_rw_direct(card, 0, 0, SDIO_CCCR_SPEED, 0, &speed);
+    if (ret)
+        return ret;
+
+    speed |= SDIO_SPEED_EHS;
+#else
+    /* Optimisation: Eliminate read by always assuming SHS and that reserved bits can be zero */
+    speed = SDIO_SPEED_EHS | SDIO_SPEED_SHS;
+#endif
+
+    ret = csr_io_rw_direct(card, 1, 0, SDIO_CCCR_SPEED, speed, NULL);
+    if (ret)
+        return ret;
+
+    mmc_card_set_highspeed(card);
+    card->host->ios.timing = MMC_TIMING_SD_HS;
+    card->host->ops->set_ios(card->host, &card->host->ios);
+
+    return 0;
+}
+
+static int
+csr_sdio_disable_hs(struct mmc_card *card)
+{
+    int ret;
+    u8 speed;
+
+    if (!(card->host->caps & MMC_CAP_SD_HIGHSPEED))
+        return 0;
+
+    if (!card->cccr.high_speed)
+        return 0;
+#if 1
+    ret = csr_io_rw_direct(card, 0, 0, SDIO_CCCR_SPEED, 0, &speed);
+    if (ret)
+        return ret;
+
+    speed &= ~SDIO_SPEED_EHS;
+#else
+    /* Optimisation: Eliminate read by always assuming SHS and that reserved bits can be zero */
+    speed = SDIO_SPEED_SHS; /* clear SDIO_SPEED_EHS */
+#endif
+
+    ret = csr_io_rw_direct(card, 1, 0, SDIO_CCCR_SPEED, speed, NULL);
+    if (ret)
+        return ret;
+
+    card->state &= ~MMC_STATE_HIGHSPEED;
+    card->host->ios.timing = MMC_TIMING_LEGACY;
+    card->host->ops->set_ios(card->host, &card->host->ios);
+
+    return 0;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioMaxBusClockFrequencySet
+ *
+ *      Set the maximum SDIO bus clock speed to use.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ *      maxFrequency         maximum clock speed in Hz
+ *
+ *  Returns:
+ *      an error code.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioMaxBusClockFrequencySet(CsrSdioFunction *function, u32 maxFrequency)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    struct mmc_host *host = func->card->host;
+    struct mmc_ios *ios = &host->ios;
+    unsigned int max_hz;
+    int err;
+	u32 max_khz = maxFrequency/1000;
+
+    if (!max_khz || max_khz > sdio_clock) {
+        max_khz = sdio_clock;
+    }
+
+    _sdio_claim_host(func);
+    max_hz = 1000 * max_khz;
+    if (max_hz > host->f_max) {
+        max_hz = host->f_max;
+    }
+
+    if (max_hz > 25000000) {
+        err = csr_sdio_enable_hs(func->card);
+    } else {
+        err = csr_sdio_disable_hs(func->card);
+    }
+    if (err) {
+        printk(KERN_ERR "SDIO warning: Failed to configure SDIO clock mode\n");
+        _sdio_release_host(func);
+		return CSR_RESULT_SUCCESS;
+    }
+
+    ios->clock = max_hz;
+    host->ops->set_ios(host, ios);
+
+    _sdio_release_host(func);
+
+	return CSR_RESULT_SUCCESS;
+} /* CsrSdioMaxBusClockFrequencySet() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioInterruptEnable
+ *  CsrSdioInterruptDisable
+ *
+ *      Enable or disable the SDIO interrupt.
+ *      The driver disables the SDIO interrupt until the i/o thread can
+ *      process it.
+ *      The SDIO interrupt can be disabled by modifying the SDIO_INT_ENABLE
+ *      register in the Card Common Control Register block, but this requires
+ *      two CMD52 operations. A better solution is to mask the interrupt at
+ *      the host controller.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ *
+ *  Returns:
+ *      Zero on success or a UniFi driver error code.
+ *
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioInterruptEnable(CsrSdioFunction *function)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err = 0;
+
+#ifdef CSR_CONFIG_MMC_INT_BYPASS_KSOFTIRQD
+    sdio_unblock_card_irq(func);
+#else
+    _sdio_claim_host(func);
+    /* Write the Int Enable in CCCR block */
+#ifdef MMC_QUIRK_LENIENT_FN0
+    sdio_f0_writeb(func, 0x3, SDIO_CCCR_IENx, &err);
+#else
+    err = csr_io_rw_direct(func->card, 1, 0, SDIO_CCCR_IENx, 0x03, NULL);
+#endif
+    _sdio_release_host(func);
+
+    func_exit();
+    if (err) {
+        printk(KERN_ERR "unifi: %s: error %d writing IENx\n", __FUNCTION__, err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+#endif
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioInterruptEnable() */
+
+CsrResult
+CsrSdioInterruptDisable(CsrSdioFunction *function)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err = 0;
+
+#ifdef CSR_CONFIG_MMC_INT_BYPASS_KSOFTIRQD
+    sdio_block_card_irq(func);
+#else
+    _sdio_claim_host(func);
+    /* Write the Int Enable in CCCR block */
+#ifdef MMC_QUIRK_LENIENT_FN0
+    sdio_f0_writeb(func, 0, SDIO_CCCR_IENx, &err);
+#else
+    err = csr_io_rw_direct(func->card, 1, 0, SDIO_CCCR_IENx, 0x00, NULL);
+#endif
+    _sdio_release_host(func);
+
+    func_exit();
+    if (err) {
+        printk(KERN_ERR "unifi: %s: error %d writing IENx\n", __FUNCTION__, err);
+        return ConvertSdioToCsrSdioResult(err);
+    }
+#endif
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioInterruptDisable() */
+
+
+void CsrSdioInterruptAcknowledge(CsrSdioFunction *function)
+{
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioFunctionEnable
+ *
+ *      Enable i/o on function 1.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ *
+ * Returns:
+ *      UniFi driver error code.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioFunctionEnable(CsrSdioFunction *function)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err;
+
+    func_enter();
+
+    /* Enable UniFi function 1 (the 802.11 part). */
+    _sdio_claim_host(func);
+    err = sdio_enable_func(func);
+    _sdio_release_host(func);
+    if (err) {
+        unifi_error(NULL, "Failed to enable SDIO function %d\n", func->num);
+    }
+
+    func_exit();
+    return ConvertSdioToCsrSdioResult(err);
+} /* CsrSdioFunctionEnable() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioFunctionDisable
+ *
+ *      Enable i/o on function 1.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ *
+ * Returns:
+ *      UniFi driver error code.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioFunctionDisable(CsrSdioFunction *function)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int err;
+
+    func_enter();
+
+    /* Disable UniFi function 1 (the 802.11 part). */
+    _sdio_claim_host(func);
+    err = sdio_disable_func(func);
+    _sdio_release_host(func);
+    if (err) {
+        unifi_error(NULL, "Failed to disable SDIO function %d\n", func->num);
+    }
+
+    func_exit();
+    return ConvertSdioToCsrSdioResult(err);
+} /* CsrSdioFunctionDisable() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioFunctionActive
+ *
+ *      No-op as the bus goes to an active state at the start of every
+ *      command.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ * ---------------------------------------------------------------------------
+ */
+void
+CsrSdioFunctionActive(CsrSdioFunction *function)
+{
+} /* CsrSdioFunctionActive() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioFunctionIdle
+ *
+ *      Set the function as idle.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ * ---------------------------------------------------------------------------
+ */
+void
+CsrSdioFunctionIdle(CsrSdioFunction *function)
+{
+} /* CsrSdioFunctionIdle() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioPowerOn
+ *
+ *      Power on UniFi.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioPowerOn(CsrSdioFunction *function)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    struct mmc_host *host = func->card->host;
+
+    _sdio_claim_host(func);
+    if (!card_is_powered) {
+        mmc_power_restore_host(host);
+        card_is_powered = 1;
+    } else {
+        printk(KERN_INFO "SDIO: Skip power on; card is already powered.\n");
+    }
+    _sdio_release_host(func);
+#endif /* 2.6.32 */
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioPowerOn() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioPowerOff
+ *
+ *      Power off UniFi.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ * ---------------------------------------------------------------------------
+ */
+void
+CsrSdioPowerOff(CsrSdioFunction *function)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    struct mmc_host *host = func->card->host;
+
+    _sdio_claim_host(func);
+    if (card_is_powered) {
+        mmc_power_save_host(host);
+        card_is_powered = 0;
+    } else {
+        printk(KERN_INFO "SDIO: Skip power off; card is already powered off.\n");
+    }
+    _sdio_release_host(func);
+#endif /* 2.6.32 */
+} /* CsrSdioPowerOff() */
+
+
+static int
+sdio_set_block_size_ignore_first_error(struct sdio_func *func, unsigned blksz)
+{
+    int ret;
+
+    if (blksz > func->card->host->max_blk_size)
+        return -EINVAL;
+
+    if (blksz == 0) {
+        blksz = min(func->max_blksize, func->card->host->max_blk_size);
+        blksz = min(blksz, 512u);
+    }
+
+    /*
+     * Ignore -ERANGE (OUT_OF_RANGE in R5) on the first byte as
+     * the block size may be invalid until both bytes are written.
+     */
+    ret = csr_io_rw_direct(func->card, 1, 0,
+                           SDIO_FBR_BASE(func->num) + SDIO_FBR_BLKSIZE,
+                           blksz & 0xff, NULL);
+    if (ret && ret != -ERANGE)
+        return ret;
+    ret = csr_io_rw_direct(func->card, 1, 0,
+                           SDIO_FBR_BASE(func->num) + SDIO_FBR_BLKSIZE + 1,
+                           (blksz >> 8) & 0xff, NULL);
+    if (ret)
+        return ret;
+    func->cur_blksize = blksz;
+
+    return 0;
+}
+
+CsrResult
+CsrSdioBlockSizeSet(CsrSdioFunction *function, u16 blockSize)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int r = 0;
+
+    /* Module parameter overrides */
+    if (sdio_block_size > -1) {
+        blockSize = sdio_block_size;
+    }
+
+    unifi_trace(NULL, UDBG1, "Set SDIO function block size to %d\n",
+                blockSize);
+
+    _sdio_claim_host(func);
+    r = sdio_set_block_size(func, blockSize);
+    _sdio_release_host(func);
+
+    /*
+     * The MMC driver for kernels prior to 2.6.32 may fail this request
+     * with -ERANGE. In this case use our workaround.
+     */
+    if (r == -ERANGE) {
+        _sdio_claim_host(func);
+        r = sdio_set_block_size_ignore_first_error(func, blockSize);
+        _sdio_release_host(func);
+    }
+    if (r) {
+        unifi_error(NULL, "Error %d setting block size\n", r);
+    }
+
+    /* Determine the achieved block size to pass to the core */
+    function->blockSize = func->cur_blksize;
+
+    return ConvertSdioToCsrSdioResult(r);
+} /* CsrSdioBlockSizeSet() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioHardReset
+ *
+ *      Hard Resets UniFi is possible.
+ *
+ *  Arguments:
+ *      sdio            SDIO context pointer
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioHardReset(CsrSdioFunction *function)
+{
+    return CSR_RESULT_FAILURE;
+} /* CsrSdioHardReset() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_glue_sdio_int_handler
+ *
+ *      Interrupt callback function for SDIO interrupts.
+ *      This is called in kernel context (i.e. not interrupt context).
+ *
+ *  Arguments:
+ *      func      SDIO context pointer
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Note: Called with host already claimed.
+ * ---------------------------------------------------------------------------
+ */
+static void
+uf_glue_sdio_int_handler(struct sdio_func *func)
+{
+    CsrSdioFunction *sdio_ctx;
+    CsrSdioInterruptDsrCallback func_dsr_callback;
+    int r;
+
+    sdio_ctx = sdio_get_drvdata(func);
+    if (!sdio_ctx) {
+        return;
+    }
+
+#ifndef CSR_CONFIG_MMC_INT_BYPASS_KSOFTIRQD
+    /*
+     * Normally, we are not allowed to do any SDIO commands here.
+     * However, this is called in a thread context and with the SDIO lock
+     * so we disable the interrupts here instead of trying to do complicated
+     * things with the SDIO lock.
+     */
+#ifdef MMC_QUIRK_LENIENT_FN0
+    sdio_f0_writeb(func, 0, SDIO_CCCR_IENx, &r);
+#else
+    r = csr_io_rw_direct(func->card, 1, 0, SDIO_CCCR_IENx, 0x00, NULL);
+#endif
+    if (r) {
+        printk(KERN_ERR "UniFi MMC Int handler: Failed to disable interrupts %d\n", r);
+    }
+#endif
+
+    /* If the function driver has registered a handler, call it */
+    if (sdio_func_drv && sdio_func_drv->intr) {
+
+        func_dsr_callback = sdio_func_drv->intr(sdio_ctx);
+
+        /* If interrupt handle returns a DSR handle, call it */
+        if (func_dsr_callback) {
+            func_dsr_callback(sdio_ctx);
+        }
+    }
+
+} /* uf_glue_sdio_int_handler() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  csr_sdio_linux_remove_irq
+ *
+ *      Unregister the interrupt handler.
+ *      This means that the linux layer can not process interrupts any more.
+ *
+ *  Arguments:
+ *      sdio      SDIO context pointer
+ *
+ *  Returns:
+ *      Status of the removal.
+ * ---------------------------------------------------------------------------
+ */
+int
+csr_sdio_linux_remove_irq(CsrSdioFunction *function)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int r;
+
+    unifi_trace(NULL, UDBG1, "csr_sdio_linux_remove_irq\n");
+
+    sdio_claim_host(func);
+    r = sdio_release_irq(func);
+    sdio_release_host(func);
+
+    return r;
+
+} /* csr_sdio_linux_remove_irq() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  csr_sdio_linux_install_irq
+ *
+ *      Register the interrupt handler.
+ *      This means that the linux layer can process interrupts.
+ *
+ *  Arguments:
+ *      sdio      SDIO context pointer
+ *
+ *  Returns:
+ *      Status of the removal.
+ * ---------------------------------------------------------------------------
+ */
+int
+csr_sdio_linux_install_irq(CsrSdioFunction *function)
+{
+    struct sdio_func *func = (struct sdio_func *)function->priv;
+    int r;
+
+    unifi_trace(NULL, UDBG1, "csr_sdio_linux_install_irq\n");
+
+    /* Register our interrupt handle */
+    sdio_claim_host(func);
+    r = sdio_claim_irq(func, uf_glue_sdio_int_handler);
+    sdio_release_host(func);
+
+    /* If the interrupt was installed earlier, is fine */
+    if (r == -EBUSY) {
+        r = 0;
+    }
+
+    return r;
+} /* csr_sdio_linux_install_irq() */
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#ifdef CONFIG_PM
+
+/*
+ * Power Management notifier
+ */
+struct uf_sdio_mmc_pm_notifier
+{
+    struct list_head list;
+
+    CsrSdioFunction *sdio_ctx;
+    struct notifier_block pm_notifier;
+};
+
+/* PM notifier list head */
+static struct uf_sdio_mmc_pm_notifier uf_sdio_mmc_pm_notifiers = {
+    .sdio_ctx = NULL,
+};
+
+/*
+ * ---------------------------------------------------------------------------
+ * uf_sdio_mmc_register_pm_notifier
+ * uf_sdio_mmc_unregister_pm_notifier
+ *
+ *      Register/unregister for power management events. A list is used to
+ *      allow multiple card instances to be supported.
+ *
+ *  Arguments:
+ *      sdio_ctx - CSR SDIO context to associate PM notifier to
+ *
+ *  Returns:
+ *      Register function returns NULL on error
+ * ---------------------------------------------------------------------------
+ */
+static struct uf_sdio_mmc_pm_notifier *
+uf_sdio_mmc_register_pm_notifier(CsrSdioFunction *sdio_ctx)
+{
+    /* Allocate notifier context for this card instance */
+    struct uf_sdio_mmc_pm_notifier *notifier_ctx = kmalloc(sizeof(struct uf_sdio_mmc_pm_notifier), GFP_KERNEL);
+
+    if (notifier_ctx)
+    {
+        notifier_ctx->sdio_ctx = sdio_ctx;
+        notifier_ctx->pm_notifier.notifier_call = uf_sdio_mmc_power_event;
+
+        list_add(&notifier_ctx->list, &uf_sdio_mmc_pm_notifiers.list);
+
+        if (register_pm_notifier(&notifier_ctx->pm_notifier)) {
+            printk(KERN_ERR "unifi: register_pm_notifier failed\n");
+        }
+    }
+
+    return notifier_ctx;
+}
+
+static void
+uf_sdio_mmc_unregister_pm_notifier(CsrSdioFunction *sdio_ctx)
+{
+    struct uf_sdio_mmc_pm_notifier *notifier_ctx;
+    struct list_head *node, *q;
+
+    list_for_each_safe(node, q, &uf_sdio_mmc_pm_notifiers.list) {
+        notifier_ctx = list_entry(node, struct uf_sdio_mmc_pm_notifier, list);
+
+        /* If it matches, unregister and free the notifier context */
+        if (notifier_ctx && notifier_ctx->sdio_ctx == sdio_ctx)
+        {
+            if (unregister_pm_notifier(&notifier_ctx->pm_notifier)) {
+                printk(KERN_ERR "unifi: unregister_pm_notifier failed\n");
+            }
+
+            /* Remove from list */
+            notifier_ctx->sdio_ctx = NULL;
+            list_del(node);
+            kfree(notifier_ctx);
+        }
+    }
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ * uf_sdio_mmc_power_event
+ *
+ *      Handler for power management events.
+ *
+ *      We need to handle suspend/resume events while the userspace is unsuspended
+ *      to allow the SME to run its suspend/resume state machines.
+ *
+ *  Arguments:
+ *      event   event ID
+ *
+ *  Returns:
+ *      Status of the event handling
+ * ---------------------------------------------------------------------------
+ */
+static int
+uf_sdio_mmc_power_event(struct notifier_block *this, unsigned long event, void *ptr)
+{
+    struct uf_sdio_mmc_pm_notifier *notifier_ctx = container_of(this,
+                                                                struct uf_sdio_mmc_pm_notifier,
+                                                                pm_notifier);
+
+    /* Call the CSR SDIO function driver's suspend/resume method
+     * while the userspace is unsuspended.
+     */
+    switch (event) {
+        case PM_POST_HIBERNATION:
+        case PM_POST_SUSPEND:
+            printk(KERN_INFO "%s:%d resume\n", __FUNCTION__, __LINE__ );
+            if (sdio_func_drv && sdio_func_drv->resume) {
+                sdio_func_drv->resume(notifier_ctx->sdio_ctx);
+            }
+            break;
+
+        case PM_HIBERNATION_PREPARE:
+        case PM_SUSPEND_PREPARE:
+            printk(KERN_INFO "%s:%d suspend\n", __FUNCTION__, __LINE__ );
+            if (sdio_func_drv && sdio_func_drv->suspend) {
+                sdio_func_drv->suspend(notifier_ctx->sdio_ctx);
+            }
+            break;
+    }
+    return NOTIFY_DONE;
+}
+
+#endif /* CONFIG_PM */
+#endif /* 2.6.32 */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_glue_sdio_probe
+ *
+ *      Card insert callback.
+ *
+ * Arguments:
+ *      func            Our (glue layer) context pointer.
+ *
+ * Returns:
+ *      UniFi driver error code.
+ * ---------------------------------------------------------------------------
+ */
+static int
+uf_glue_sdio_probe(struct sdio_func *func,
+                   const struct sdio_device_id *id)
+{
+    int instance;
+    CsrSdioFunction *sdio_ctx;
+
+    func_enter();
+
+    /* First of all claim the SDIO driver */
+    sdio_claim_host(func);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+    /* Assume that the card is already powered */
+    card_is_powered = 1;
+#endif
+
+    /* Assumes one card per host, which is true for SDIO */
+    instance = func->card->host->index;
+    printk("sdio bus_id: %16s - UniFi card 0x%X inserted\n",
+           sdio_func_id(func), instance);
+
+    /* Allocate context */
+    sdio_ctx = (CsrSdioFunction *)kmalloc(sizeof(CsrSdioFunction),
+                                          GFP_KERNEL);
+    if (sdio_ctx == NULL) {
+        sdio_release_host(func);
+        return -ENOMEM;
+    }
+
+    /* Initialise the context */
+    sdio_ctx->sdioId.manfId  = func->vendor;
+    sdio_ctx->sdioId.cardId  = func->device;
+    sdio_ctx->sdioId.sdioFunction  = func->num;
+    sdio_ctx->sdioId.sdioInterface = func->class;
+    sdio_ctx->blockSize = func->cur_blksize;
+    sdio_ctx->priv = (void *)func;
+    sdio_ctx->features = 0;
+
+    /* Module parameter enables byte mode */
+    if (sdio_byte_mode) {
+        sdio_ctx->features |= CSR_SDIO_FEATURE_BYTE_MODE;
+    }
+
+    if (func->card->host->caps & MMC_CAP_SD_HIGHSPEED) {
+        unifi_trace(NULL, UDBG1, "MMC_CAP_SD_HIGHSPEED is available\n");
+    }
+
+#ifdef MMC_QUIRK_LENIENT_FN0
+    func->card->quirks |= MMC_QUIRK_LENIENT_FN0;
+#endif
+
+    /* Pass context to the SDIO driver */
+    sdio_set_drvdata(func, sdio_ctx);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#ifdef CONFIG_PM
+    /* Register to get PM events */
+    if (uf_sdio_mmc_register_pm_notifier(sdio_ctx) == NULL) {
+        unifi_error(NULL, "%s: Failed to register for PM events\n", __FUNCTION__);
+    }
+#endif
+#endif
+
+    /* Register this device with the SDIO function driver */
+    /* Call the main UniFi driver inserted handler */
+    if (sdio_func_drv && sdio_func_drv->inserted) {
+        uf_add_os_device(instance, &func->dev);
+        sdio_func_drv->inserted(sdio_ctx);
+    }
+
+    /* We have finished, so release the SDIO driver */
+    sdio_release_host(func);
+
+#ifdef ANDROID_BUILD
+    /* Take the wakelock */
+    unifi_trace(NULL, UDBG1, "probe: take wake lock\n");
+    wake_lock(&unifi_sdio_wake_lock);
+#endif
+
+    func_exit();
+    return 0;
+} /* uf_glue_sdio_probe() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_glue_sdio_remove
+ *
+ *      Card removal callback.
+ *
+ * Arguments:
+ *      func            Our (glue layer) context pointer.
+ *
+ * Returns:
+ *      UniFi driver error code.
+ * ---------------------------------------------------------------------------
+ */
+static void
+uf_glue_sdio_remove(struct sdio_func *func)
+{
+    CsrSdioFunction *sdio_ctx;
+
+    sdio_ctx = sdio_get_drvdata(func);
+    if (!sdio_ctx) {
+        return;
+    }
+
+    func_enter();
+
+    unifi_info(NULL, "UniFi card removed\n");
+
+    /* Clean up the SDIO function driver */
+    if (sdio_func_drv && sdio_func_drv->removed) {
+        uf_remove_os_device(func->card->host->index);
+        sdio_func_drv->removed(sdio_ctx);
+    }
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#ifdef CONFIG_PM
+    /* Unregister for PM events */
+    uf_sdio_mmc_unregister_pm_notifier(sdio_ctx);
+#endif
+#endif
+
+    kfree(sdio_ctx);
+
+    func_exit();
+
+} /* uf_glue_sdio_remove */
+
+
+/*
+ * SDIO ids *must* be statically declared, so we can't take
+ * them from the list passed in csr_sdio_register_driver().
+ */
+static const struct sdio_device_id unifi_ids[] = {
+    { SDIO_DEVICE(SDIO_MANF_ID_CSR,SDIO_CARD_ID_UNIFI_3) },
+    { SDIO_DEVICE(SDIO_MANF_ID_CSR,SDIO_CARD_ID_UNIFI_4) },
+    { /* end: all zeroes */				},
+};
+
+MODULE_DEVICE_TABLE(sdio, unifi_ids);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#ifdef CONFIG_PM
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_glue_sdio_suspend
+ *
+ *      Card suspend callback. The userspace will already be suspended.
+ *
+ * Arguments:
+ *      dev            The struct device owned by the MMC driver
+ *
+ * Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+static int
+uf_glue_sdio_suspend(struct device *dev)
+{
+    func_enter();
+
+    unifi_trace(NULL, UDBG1, "uf_glue_sdio_suspend");
+
+    func_exit();
+    return 0;
+} /* uf_glue_sdio_suspend */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_glue_sdio_resume
+ *
+ *      Card resume callback. The userspace will still be suspended.
+ *
+ * Arguments:
+ *      dev            The struct device owned by the MMC driver
+ *
+ * Returns:
+ *      None
+ * ---------------------------------------------------------------------------
+ */
+static int
+uf_glue_sdio_resume(struct device *dev)
+{
+    func_enter();
+
+    unifi_trace(NULL, UDBG1, "uf_glue_sdio_resume");
+
+#ifdef ANDROID_BUILD
+    unifi_trace(NULL, UDBG1, "resume: take wakelock\n");
+    wake_lock(&unifi_sdio_wake_lock);
+#endif
+
+    func_exit();
+    return 0;
+
+} /* uf_glue_sdio_resume */
+
+static struct dev_pm_ops unifi_pm_ops = {
+    .suspend = uf_glue_sdio_suspend,
+    .resume  = uf_glue_sdio_resume,
+};
+
+#define UNIFI_PM_OPS  (&unifi_pm_ops)
+
+#else
+
+#define UNIFI_PM_OPS  NULL
+
+#endif /* CONFIG_PM */
+#endif /* 2.6.32 */
+
+static struct sdio_driver unifi_driver = {
+    .probe      = uf_glue_sdio_probe,
+    .remove     = uf_glue_sdio_remove,
+    .name       = "unifi",
+    .id_table	= unifi_ids,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+    .drv.pm     = UNIFI_PM_OPS,
+#endif /* 2.6.32 */
+};
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  CsrSdioFunctionDriverRegister
+ *  CsrSdioFunctionDriverUnregister
+ *
+ *      These functions are called from the main module load and unload
+ *      functions. They perform the appropriate operations for the
+ *      linux MMC/SDIO driver.
+ *
+ *  Arguments:
+ *      sdio_drv    Pointer to the function driver's SDIO structure.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+CsrResult
+CsrSdioFunctionDriverRegister(CsrSdioFunctionDriver *sdio_drv)
+{
+    int r;
+
+    printk("UniFi: Using native Linux MMC driver for SDIO.\n");
+
+    if (sdio_func_drv) {
+        unifi_error(NULL, "sdio_mmc: UniFi driver already registered\n");
+        return CSR_SDIO_RESULT_INVALID_VALUE;
+    }
+
+#ifdef ANDROID_BUILD
+    wake_lock_init(&unifi_sdio_wake_lock, WAKE_LOCK_SUSPEND, "unifi_sdio_work");
+#endif
+
+    /* Save the registered driver description */
+    /*
+     * FIXME:
+     * Need a table here to handle a call to register for just one function.
+     * mmc only allows us to register for the whole device
+     */
+    sdio_func_drv = sdio_drv;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#ifdef CONFIG_PM
+    /* Initialise PM notifier list */
+    INIT_LIST_HEAD(&uf_sdio_mmc_pm_notifiers.list);
+#endif
+#endif
+
+    /* Register ourself with mmc_core */
+    r = sdio_register_driver(&unifi_driver);
+    if (r) {
+        printk(KERN_ERR "unifi_sdio: Failed to register UniFi SDIO driver: %d\n", r);
+        return ConvertSdioToCsrSdioResult(r);
+    }
+
+    return CSR_RESULT_SUCCESS;
+} /* CsrSdioFunctionDriverRegister() */
+
+
+
+void
+CsrSdioFunctionDriverUnregister(CsrSdioFunctionDriver *sdio_drv)
+{
+    printk(KERN_INFO "UniFi: unregister from MMC sdio\n");
+
+#ifdef ANDROID_BUILD
+    wake_lock_destroy(&unifi_sdio_wake_lock);
+#endif
+    sdio_unregister_driver(&unifi_driver);
+
+    sdio_func_drv = NULL;
+
+} /* CsrSdioFunctionDriverUnregister() */
+
diff --git a/drivers/staging/csr/sdio_stubs.c b/drivers/staging/csr/sdio_stubs.c
new file mode 100644
index 00000000000..839fae01d96
--- /dev/null
+++ b/drivers/staging/csr/sdio_stubs.c
@@ -0,0 +1,82 @@
+/*
+ * Stubs for some of the bottom edge functions.
+ *
+ * These stubs are optional functions in the bottom edge (SDIO driver
+ * interface) API that not all platforms or SDIO drivers may support.
+ *
+ * They're declared as weak symbols so they can be overridden by
+ * simply providing a non-weak declaration.
+ *
+ * Copyright (C) 2007-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ */
+#include "csr_wifi_hip_unifi.h"
+
+void __attribute__((weak)) CsrSdioFunctionIdle(CsrSdioFunction *function)
+{
+}
+
+void __attribute__((weak)) CsrSdioFunctionActive(CsrSdioFunction *function)
+{
+}
+
+CsrResult __attribute__((weak)) CsrSdioPowerOn(CsrSdioFunction *function)
+{
+    return CSR_RESULT_SUCCESS;
+}
+
+void __attribute__((weak)) CsrSdioPowerOff(CsrSdioFunction *function)
+{
+}
+
+CsrResult __attribute__((weak)) CsrSdioHardReset(CsrSdioFunction *function)
+{
+    return CSR_SDIO_RESULT_NOT_RESET;
+}
+
+CsrResult __attribute__((weak)) CsrSdioBlockSizeSet(CsrSdioFunction *function,
+                                                   u16 blockSize)
+{
+    return CSR_RESULT_SUCCESS;
+}
+
+CsrResult __attribute__((weak)) CsrSdioSuspend(CsrSdioFunction *function)
+{
+    return CSR_RESULT_SUCCESS;
+}
+
+CsrResult __attribute__((weak)) CsrSdioResume(CsrSdioFunction *function)
+{
+    return CSR_RESULT_SUCCESS;
+}
+
+int __attribute__((weak)) csr_sdio_linux_install_irq(CsrSdioFunction *function)
+{
+    return 0;
+}
+
+int __attribute__((weak)) csr_sdio_linux_remove_irq(CsrSdioFunction *function)
+{
+    return 0;
+}
+
+void __attribute__((weak)) CsrSdioInsertedAcknowledge(CsrSdioFunction *function, CsrResult result)
+{
+}
+
+void __attribute__((weak)) CsrSdioRemovedAcknowledge(CsrSdioFunction *function)
+{
+}
+
+void __attribute__((weak)) CsrSdioSuspendAcknowledge(CsrSdioFunction *function, CsrResult result)
+{
+}
+
+void __attribute__((weak)) CsrSdioResumeAcknowledge(CsrSdioFunction *function, CsrResult result)
+{
+}
+
+
diff --git a/drivers/staging/csr/sme_blocking.c b/drivers/staging/csr/sme_blocking.c
new file mode 100644
index 00000000000..acf0f0fe3b3
--- /dev/null
+++ b/drivers/staging/csr/sme_blocking.c
@@ -0,0 +1,1535 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     sme_mgt_blocking.c
+ *
+ * PURPOSE:
+ *      This file contains the driver specific implementation of
+ *      the WEXT <==> SME MGT interface for all SME builds that support WEXT.
+ *
+ * Copyright (C) 2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#include "unifi_priv.h"
+
+
+/*
+ * This file also contains the implementation of the asyncronous
+ * requests to the SME.
+ *
+ * Before calling an asyncronous SME function, we call sme_init_request()
+ * which gets hold of the SME semaphore and updates the request status.
+ * The semaphore makes sure that there is only one pending request to
+ * the SME at a time.
+ *
+ * Now we are ready to call the SME function, but only if
+ * sme_init_request() has returned 0.
+ *
+ * When the SME function returns, we need to wait
+ * for the reply. This is done in sme_wait_for_reply().
+ * If the request times-out, the request status is set to SME_REQUEST_TIMEDOUT
+ * and the sme_wait_for_reply() returns.
+ *
+ * If the SME replies in time, we call sme_complete_request().
+ * There we change the request status to SME_REQUEST_RECEIVED. This will
+ * wake up the process waiting on sme_wait_for_reply().
+ * It is important that we copy the reply data in priv->sme_reply
+ * before calling sme_complete_request().
+ *
+ * Handling the wext requests, we need to block
+ * until the SME sends the response to our request.
+ * We use the sme_init_request() and sme_wait_for_reply()
+ * to implement this behavior in the following functions:
+ * sme_mgt_wifi_on()
+ * sme_mgt_wifi_off()
+ * sme_mgt_scan_full()
+ * sme_mgt_scan_results_get_async()
+ * sme_mgt_connect()
+ * unifi_mgt_media_status_ind()
+ * sme_mgt_disconnect()
+ * sme_mgt_pmkid()
+ * sme_mgt_key()
+ * sme_mgt_mib_get()
+ * sme_mgt_mib_set()
+ * sme_mgt_versions_get()
+ * sme_mgt_set_value()
+ * sme_mgt_get_value()
+ * sme_mgt_set_value_async()
+ * sme_mgt_get_value_async()
+ * sme_mgt_packet_filter_set()
+ * sme_mgt_tspec()
+ */
+
+
+/*
+ * Handling the suspend and resume system events, we need to block
+ * until the SME sends the response to our indication.
+ * We use the sme_init_request() and sme_wait_for_reply()
+ * to implement this behavior in the following functions:
+ * sme_sys_suspend()
+ * sme_sys_resume()
+ */
+
+#define UNIFI_SME_MGT_SHORT_TIMEOUT    10000
+#define UNIFI_SME_MGT_LONG_TIMEOUT     19000
+#define UNIFI_SME_SYS_LONG_TIMEOUT     10000
+
+#ifdef UNIFI_DEBUG
+# define sme_wait_for_reply(priv, t) _sme_wait_for_reply(priv, t, __func__)
+#else
+# define sme_wait_for_reply(priv, t) _sme_wait_for_reply(priv, t, NULL)
+#endif
+
+static int
+sme_init_request(unifi_priv_t *priv)
+{
+    if (priv == NULL) {
+        unifi_error(priv, "sme_init_request: Invalid priv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG5, "sme_init_request: wait sem\n");
+
+    /* Grab the SME semaphore until the reply comes, or timeout */
+    if (down_interruptible(&priv->sme_sem)) {
+        unifi_error(priv, "sme_init_request: Failed to get SME semaphore\n");
+        return -EIO;
+    }
+    unifi_trace(priv, UDBG5, "sme_init_request: got sem: pending\n");
+
+    priv->sme_reply.request_status = SME_REQUEST_PENDING;
+
+    return 0;
+
+} /* sme_init_request() */
+
+
+void
+uf_sme_complete_request(unifi_priv_t *priv, CsrResult reply_status, const char *func)
+{
+    if (priv == NULL) {
+        unifi_error(priv, "sme_complete_request: Invalid priv\n");
+        return;
+    }
+
+    if (priv->sme_reply.request_status != SME_REQUEST_PENDING) {
+        unifi_notice(priv,
+                    "sme_complete_request: request not pending %s (s:%d)\n",
+                    (func ? func : ""), priv->sme_reply.request_status);
+        return;
+    }
+    unifi_trace(priv, UDBG5,
+                "sme_complete_request: completed %s (s:%d)\n",
+                (func ? func : ""), priv->sme_reply.request_status);
+
+    priv->sme_reply.request_status = SME_REQUEST_RECEIVED;
+    priv->sme_reply.reply_status = reply_status;
+
+    wake_up_interruptible(&priv->sme_request_wq);
+
+    return;
+}
+
+
+void
+uf_sme_cancel_request(unifi_priv_t *priv, CsrResult reply_status)
+{
+    /* Check for a blocking SME request in progress, and cancel the wait.
+     * This should be used when the character device is closed.
+     */
+
+    if (priv == NULL) {
+        unifi_error(priv, "sme_cancel_request: Invalid priv\n");
+        return;
+    }
+
+    /* If no request is pending, nothing to wake up */
+    if (priv->sme_reply.request_status != SME_REQUEST_PENDING) {
+        unifi_trace(priv, UDBG5,
+                    "sme_cancel_request: no request was pending (s:%d)\n",
+                    priv->sme_reply.request_status);
+        /* Nothing to do */
+        return;
+    }
+    unifi_trace(priv, UDBG5,
+                "sme_cancel_request: request cancelled (s:%d)\n",
+                priv->sme_reply.request_status);
+
+    /* Wake up the wait with an error status */
+    priv->sme_reply.request_status = SME_REQUEST_CANCELLED;
+    priv->sme_reply.reply_status = reply_status; /* unimportant since the CANCELLED state will fail the ioctl */
+
+    wake_up_interruptible(&priv->sme_request_wq);
+
+    return;
+}
+
+
+static int
+_sme_wait_for_reply(unifi_priv_t *priv,
+        unsigned long timeout, const char *func)
+{
+    long r;
+
+    unifi_trace(priv, UDBG5, "sme_wait_for_reply: %s sleep\n", func ? func : "");
+    r = wait_event_interruptible_timeout(priv->sme_request_wq,
+                                         (priv->sme_reply.request_status != SME_REQUEST_PENDING),
+                                         msecs_to_jiffies(timeout));
+    unifi_trace(priv, UDBG5, "sme_wait_for_reply: %s awake (%d)\n", func ? func : "", r);
+
+    if (r == -ERESTARTSYS) {
+        /* The thread was killed */
+        unifi_info(priv, "ERESTARTSYS in _sme_wait_for_reply\n");
+        up(&priv->sme_sem);
+        return r;
+    }
+    if (priv->sme_reply.request_status == SME_REQUEST_CANCELLED) {
+        unifi_trace(priv, UDBG5, "Cancelled waiting for SME to reply (%s s:%d, t:%d, r:%d)\n",
+                    (func ? func : ""), priv->sme_reply.request_status, timeout, r);
+
+        /* Release the SME semaphore that was downed in sme_init_request() */
+        up(&priv->sme_sem);
+        return -EIO; /* fail the ioctl */
+    }
+    if ((r == 0) && (priv->sme_reply.request_status != SME_REQUEST_RECEIVED)) {
+        unifi_notice(priv, "Timeout waiting for SME to reply (%s s:%d, t:%d)\n",
+                     (func ? func : ""), priv->sme_reply.request_status, timeout);
+
+        priv->sme_reply.request_status = SME_REQUEST_TIMEDOUT;
+
+        /* Release the SME semaphore that was downed in sme_init_request() */
+        up(&priv->sme_sem);
+
+        return -ETIMEDOUT;
+    }
+
+    unifi_trace(priv, UDBG5, "sme_wait_for_reply: %s received (%d)\n",
+                func ? func : "", r);
+
+    /* Release the SME semaphore that was downed in sme_init_request() */
+    up(&priv->sme_sem);
+
+    return 0;
+} /* sme_wait_for_reply() */
+
+
+
+
+#ifdef CSR_SUPPORT_WEXT
+int sme_mgt_wifi_on(unifi_priv_t *priv)
+{
+    u16 numElements;
+    CsrWifiSmeDataBlock* dataList;
+#ifdef CSR_SUPPORT_WEXT_AP
+    int r;
+#endif
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_wifi_on: invalid smepriv\n");
+        return -EIO;
+    }
+
+    if (priv->mib_data.length) {
+        numElements = 1;
+        dataList = &priv->mib_data;
+    } else {
+        numElements = 0;
+        dataList = NULL;
+    }
+    /* Start the SME */
+#ifdef CSR_SUPPORT_WEXT_AP
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+#endif
+    CsrWifiSmeWifiOnReqSend(0, priv->sta_mac_address, numElements, dataList);
+#ifdef CSR_SUPPORT_WEXT_AP
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_LONG_TIMEOUT);
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_wifi_on: unifi_mgt_wifi_oo_req <-- (r=%d, status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    return 0;
+#endif
+} /* sme_mgt_wifi_on() */
+
+
+int sme_mgt_wifi_off(unifi_priv_t *priv)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_wifi_off: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    /* Stop the SME */
+    CsrWifiSmeWifiOffReqSend(0);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_LONG_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_wifi_off: unifi_mgt_wifi_off_req <-- (r=%d, status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+
+} /* sme_mgt_wifi_off */
+
+int sme_mgt_key(unifi_priv_t *priv, CsrWifiSmeKey *sme_key,
+        CsrWifiSmeListAction action)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_key: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeKeyReqSend(0, CSR_WIFI_INTERFACE_IN_USE, action, *sme_key);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+
+int sme_mgt_scan_full(unifi_priv_t *priv,
+        CsrWifiSsid *specific_ssid,
+        int num_channels,
+        unsigned char *channel_list)
+{
+    CsrWifiMacAddress bcastAddress = {{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }};
+    u8 is_active = (num_channels > 0) ? TRUE : FALSE;
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_scan_full: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_scan_full: -->\n");
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    /* If a channel list is provided, do an active scan */
+    if (is_active) {
+        unifi_trace(priv, UDBG1,
+                    "channel list - num_channels: %d, active scan\n",
+                    num_channels);
+    }
+
+    CsrWifiSmeScanFullReqSend(0,
+                              specific_ssid->length?1:0, /* 0 or 1 SSIDS */
+                              specific_ssid,
+                              bcastAddress,
+                              is_active,
+                              CSR_WIFI_SME_BSS_TYPE_ANY_BSS,
+                              CSR_WIFI_SME_SCAN_TYPE_ALL,
+                              (u16)num_channels, channel_list,
+                              0, NULL);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_LONG_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_scan_full: <-- (status=%d)\n", priv->sme_reply.reply_status);
+    if (priv->sme_reply.reply_status == CSR_WIFI_RESULT_UNAVAILABLE) {
+        return 0; /* initial scan already underway */
+    } else {
+        return convert_sme_error(priv->sme_reply.reply_status);
+    }
+}
+
+
+int sme_mgt_scan_results_get_async(unifi_priv_t *priv,
+        struct iw_request_info *info,
+        char *scan_results,
+        long scan_results_len)
+{
+    u16 scan_result_list_count;
+    CsrWifiSmeScanResult *scan_result_list;
+    CsrWifiSmeScanResult *scan_result;
+    int r;
+    int i;
+    char *current_ev = scan_results;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_scan_results_get_async: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeScanResultsGetReqSend(0);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_LONG_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    scan_result_list_count = priv->sme_reply.reply_scan_results_count;
+    scan_result_list = priv->sme_reply.reply_scan_results;
+    unifi_trace(priv, UDBG2,
+                "scan_results: Scan returned %d, numElements=%d\n",
+                r, scan_result_list_count);
+
+    /* OK, now we have the scan results */
+    for (i = 0; i < scan_result_list_count; ++i) {
+        scan_result = &scan_result_list[i];
+
+        unifi_trace(priv, UDBG2, "Scan Result: %.*s\n",
+                    scan_result->ssid.length,
+                    scan_result->ssid.ssid);
+
+        r = unifi_translate_scan(priv->netdev[0], info,
+                                 current_ev,
+                                 scan_results + scan_results_len,
+                                 scan_result, i+1);
+
+        if (r < 0) {
+            kfree(scan_result_list);
+            priv->sme_reply.reply_scan_results_count = 0;
+            priv->sme_reply.reply_scan_results = NULL;
+            return r;
+        }
+
+        current_ev += r;
+    }
+
+    /*
+     * Free the scan results allocated in unifi_mgt_scan_results_get_cfm()
+     * and invalidate the reply_scan_results to avoid re-using
+     * the freed pointers.
+     */
+    kfree(scan_result_list);
+    priv->sme_reply.reply_scan_results_count = 0;
+    priv->sme_reply.reply_scan_results = NULL;
+
+    unifi_trace(priv, UDBG2,
+                "scan_results: Scan translated to %d bytes\n",
+                current_ev - scan_results);
+    return (current_ev - scan_results);
+}
+
+
+int sme_mgt_connect(unifi_priv_t *priv)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_connect: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG2, "sme_mgt_connect: %.*s\n",
+                priv->connection_config.ssid.length,
+                priv->connection_config.ssid.ssid);
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeConnectReqSend(0, CSR_WIFI_INTERFACE_IN_USE, priv->connection_config);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    if (priv->sme_reply.reply_status) {
+        unifi_trace(priv, UDBG1, "sme_mgt_connect: failed with SME status %d\n",
+                    priv->sme_reply.reply_status);
+    }
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+
+int sme_mgt_disconnect(unifi_priv_t *priv)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_disconnect: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeDisconnectReqSend(0, CSR_WIFI_INTERFACE_IN_USE);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_disconnect: <-- (status=%d)\n", priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+
+int sme_mgt_pmkid(unifi_priv_t *priv,
+        CsrWifiSmeListAction action,
+        CsrWifiSmePmkidList *pmkid_list)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_pmkid: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmePmkidReqSend(0, CSR_WIFI_INTERFACE_IN_USE, action,
+                        pmkid_list->pmkidsCount, pmkid_list->pmkids);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_pmkid: <-- (status=%d)\n", priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+
+int sme_mgt_mib_get(unifi_priv_t *priv,
+        unsigned char *varbind, int *length)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_mib_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    priv->mib_cfm_buffer = varbind;
+    priv->mib_cfm_buffer_length = MAX_VARBIND_LENGTH;
+
+    CsrWifiSmeMibGetReqSend(0, *length, varbind);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        priv->mib_cfm_buffer_length = 0;
+        priv->mib_cfm_buffer = NULL;
+        return r;
+    }
+
+    *length = priv->mib_cfm_buffer_length;
+
+    priv->mib_cfm_buffer_length = 0;
+    priv->mib_cfm_buffer = NULL;
+    unifi_trace(priv, UDBG4, "sme_mgt_mib_get: <-- (status=%d)\n", priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+int sme_mgt_mib_set(unifi_priv_t *priv,
+        unsigned char *varbind, int length)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_mib_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeMibSetReqSend(0, length, varbind);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_mib_set: <-- (status=%d)\n", priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+#endif /* CSR_SUPPORT_WEXT */
+
+int sme_mgt_power_config_set(unifi_priv_t *priv, CsrWifiSmePowerConfig *powerConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_set_value_async: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmePowerConfigSetReqSend(0, *powerConfig);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_set_value_async: unifi_mgt_set_value_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_set_value: invalid smepriv\n");
+        return -EIO;
+    }
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtPowerConfigSetReq(priv->smepriv, *powerConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_sme_config_set(unifi_priv_t *priv, CsrWifiSmeStaConfig *staConfig, CsrWifiSmeDeviceConfig *deviceConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_sme_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeSmeStaConfigSetReqSend(0, CSR_WIFI_INTERFACE_IN_USE, *staConfig);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_sme_config_set: CsrWifiSmeSmeStaConfigSetReq <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeSmeCommonConfigSetReqSend(0, *deviceConfig);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_sme_config_set: CsrWifiSmeSmeCommonConfigSetReq <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_sme_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtSmeConfigSetReq(priv->smepriv, *staConfig);
+    status = CsrWifiSmeMgtDeviceConfigSetReq(priv->smepriv, *deviceConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+#ifdef CSR_SUPPORT_WEXT
+
+int sme_mgt_mib_config_set(unifi_priv_t *priv, CsrWifiSmeMibConfig *mibConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_mib_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeMibConfigSetReqSend(0, *mibConfig);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_mib_config_set: unifi_mgt_set_mib_config_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_mib_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtMibConfigSetReq(priv->smepriv, *mibConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_coex_config_set(unifi_priv_t *priv, CsrWifiSmeCoexConfig *coexConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_coex_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeCoexConfigSetReqSend(0, *coexConfig);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_coex_config_set: unifi_mgt_set_mib_config_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_coex_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtCoexConfigSetReq(priv->smepriv, *coexConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+#endif /* CSR_SUPPORT_WEXT */
+
+int sme_mgt_host_config_set(unifi_priv_t *priv, CsrWifiSmeHostConfig *hostConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_host_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeHostConfigSetReqSend(0, CSR_WIFI_INTERFACE_IN_USE, *hostConfig);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_host_config_set: unifi_mgt_set_host_config_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_host_config_set: invalid smepriv\n");
+        return -EIO;
+    }
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtHostConfigSetReq(priv->smepriv, *hostConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+#ifdef CSR_SUPPORT_WEXT
+
+int sme_mgt_versions_get(unifi_priv_t *priv, CsrWifiSmeVersions *versions)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_versions_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_versions_get: unifi_mgt_versions_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeVersionsGetReqSend(0);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (versions != NULL) {
+        memcpy((unsigned char*)versions,
+               (unsigned char*)&priv->sme_reply.versions,
+               sizeof(CsrWifiSmeVersions));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_versions_get: unifi_mgt_versions_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtVersionsGetReq(priv->smepriv, versions);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+#endif /* CSR_SUPPORT_WEXT */
+
+int sme_mgt_power_config_get(unifi_priv_t *priv, CsrWifiSmePowerConfig *powerConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_power_config_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_power_config_get: unifi_mgt_power_config_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmePowerConfigGetReqSend(0);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (powerConfig != NULL) {
+        memcpy((unsigned char*)powerConfig,
+               (unsigned char*)&priv->sme_reply.powerConfig,
+               sizeof(CsrWifiSmePowerConfig));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_get_versions: unifi_mgt_power_config_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtPowerConfigGetReq(priv->smepriv, powerConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_host_config_get(unifi_priv_t *priv, CsrWifiSmeHostConfig *hostConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_host_config_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_host_config_get: unifi_mgt_host_config_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeHostConfigGetReqSend(0, CSR_WIFI_INTERFACE_IN_USE);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (hostConfig != NULL) {
+        memcpy((unsigned char*)hostConfig,
+               (unsigned char*)&priv->sme_reply.hostConfig,
+               sizeof(CsrWifiSmeHostConfig));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_host_config_get: unifi_mgt_host_config_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtHostConfigGetReq(priv->smepriv, hostConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_sme_config_get(unifi_priv_t *priv, CsrWifiSmeStaConfig *staConfig, CsrWifiSmeDeviceConfig *deviceConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_sme_config_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_sme_config_get: unifi_mgt_sme_config_get_req -->\n");
+
+    /* Common device config */
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeSmeCommonConfigGetReqSend(0);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (deviceConfig != NULL) {
+        memcpy((unsigned char*)deviceConfig,
+               (unsigned char*)&priv->sme_reply.deviceConfig,
+               sizeof(CsrWifiSmeDeviceConfig));
+    }
+
+    /* STA config */
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeSmeStaConfigGetReqSend(0, CSR_WIFI_INTERFACE_IN_USE);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (staConfig != NULL) {
+        memcpy((unsigned char*)staConfig,
+               (unsigned char*)&priv->sme_reply.staConfig,
+               sizeof(CsrWifiSmeStaConfig));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_sme_config_get: unifi_mgt_sme_config_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtSmeConfigGetReq(priv->smepriv, staConfig);
+    status = CsrWifiSmeMgtDeviceConfigGetReq(priv->smepriv, deviceConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_coex_info_get(unifi_priv_t *priv, CsrWifiSmeCoexInfo *coexInfo)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_coex_info_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_coex_info_get: unifi_mgt_coex_info_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeCoexInfoGetReqSend(0);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (coexInfo != NULL) {
+        memcpy((unsigned char*)coexInfo,
+               (unsigned char*)&priv->sme_reply.coexInfo,
+               sizeof(CsrWifiSmeCoexInfo));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_coex_info_get: unifi_mgt_coex_info_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtCoexInfoGetReq(priv->smepriv, coexInfo);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+#ifdef CSR_SUPPORT_WEXT
+
+int sme_mgt_coex_config_get(unifi_priv_t *priv, CsrWifiSmeCoexConfig *coexConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_coex_config_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_coex_config_get: unifi_mgt_coex_config_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeCoexConfigGetReqSend(0);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (coexConfig != NULL) {
+        memcpy((unsigned char*)coexConfig,
+               (unsigned char*)&priv->sme_reply.coexConfig,
+               sizeof(CsrWifiSmeCoexConfig));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_coex_config_get: unifi_mgt_coex_config_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtCoexConfigGetReq(priv->smepriv, coexConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_mib_config_get(unifi_priv_t *priv, CsrWifiSmeMibConfig *mibConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_mib_config_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_mib_config_get: unifi_mgt_mib_config_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeMibConfigGetReqSend(0);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (mibConfig != NULL) {
+        memcpy((unsigned char*)mibConfig,
+               (unsigned char*)&priv->sme_reply.mibConfig,
+               sizeof(CsrWifiSmeMibConfig));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_mib_config_get: unifi_mgt_mib_config_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtMibConfigGetReq(priv->smepriv, mibConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_connection_info_get(unifi_priv_t *priv, CsrWifiSmeConnectionInfo *connectionInfo)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_connection_info_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_connection_info_get: unifi_mgt_connection_info_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeConnectionInfoGetReqSend(0, CSR_WIFI_INTERFACE_IN_USE);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (connectionInfo != NULL) {
+        memcpy((unsigned char*)connectionInfo,
+               (unsigned char*)&priv->sme_reply.connectionInfo,
+               sizeof(CsrWifiSmeConnectionInfo));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_connection_info_get: unifi_mgt_connection_info_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtConnectionInfoGetReq(priv->smepriv, connectionInfo);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_connection_config_get(unifi_priv_t *priv, CsrWifiSmeConnectionConfig *connectionConfig)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_connection_config_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_connection_config_get: unifi_mgt_connection_config_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeConnectionConfigGetReqSend(0, CSR_WIFI_INTERFACE_IN_USE);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (connectionConfig != NULL) {
+        memcpy((unsigned char*)connectionConfig,
+               (unsigned char*)&priv->sme_reply.connectionConfig,
+               sizeof(CsrWifiSmeConnectionConfig));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_connection_config_get: unifi_mgt_connection_config_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtConnectionConfigGetReq(priv->smepriv, connectionConfig);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+int sme_mgt_connection_stats_get(unifi_priv_t *priv, CsrWifiSmeConnectionStats *connectionStats)
+{
+#ifdef CSR_SME_USERSPACE
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_connection_stats_get: invalid smepriv\n");
+        return -EIO;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_connection_stats_get: unifi_mgt_connection_stats_get_req -->\n");
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeConnectionStatsGetReqSend(0, CSR_WIFI_INTERFACE_IN_USE);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    /* store the reply */
+    if (connectionStats != NULL) {
+        memcpy((unsigned char*)connectionStats,
+               (unsigned char*)&priv->sme_reply.connectionStats,
+               sizeof(CsrWifiSmeConnectionStats));
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_mgt_connection_stats_get: unifi_mgt_connection_stats_get_req <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+
+    return convert_sme_error(priv->sme_reply.reply_status);
+#else
+    CsrResult status;
+    CsrWifiSmeMgtClaimSyncAccess(priv->smepriv);
+    status = CsrWifiSmeMgtConnectionStatsGetReq(priv->smepriv, connectionStats);
+    CsrWifiSmeMgtReleaseSyncAccess(priv->smepriv);
+    return convert_sme_error(status);
+#endif
+}
+
+#endif /* CSR_SUPPORT_WEXT */
+
+int sme_mgt_packet_filter_set(unifi_priv_t *priv)
+{
+    CsrWifiIp4Address ipAddress = {{0xFF, 0xFF, 0xFF, 0xFF }};
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_packet_filter_set: invalid smepriv\n");
+        return -EIO;
+    }
+    if (priv->packet_filters.arp_filter) {
+        ipAddress.a[0] = (priv->sta_ip_address      ) & 0xFF;
+        ipAddress.a[1] = (priv->sta_ip_address >>  8) & 0xFF;
+        ipAddress.a[2] = (priv->sta_ip_address >> 16) & 0xFF;
+        ipAddress.a[3] = (priv->sta_ip_address >> 24) & 0xFF;
+    }
+
+    unifi_trace(priv, UDBG5,
+                "sme_mgt_packet_filter_set: IP address %d.%d.%d.%d\n",
+                ipAddress.a[0], ipAddress.a[1],
+                ipAddress.a[2], ipAddress.a[3]);
+
+    /* Doesn't block for a confirm */
+    CsrWifiSmePacketFilterSetReqSend(0, CSR_WIFI_INTERFACE_IN_USE,
+                                     priv->packet_filters.tclas_ies_length,
+                                     priv->filter_tclas_ies,
+                                     priv->packet_filters.filter_mode,
+                                     ipAddress);
+    return 0;
+}
+
+int sme_mgt_tspec(unifi_priv_t *priv, CsrWifiSmeListAction action,
+        u32 tid, CsrWifiSmeDataBlock *tspec, CsrWifiSmeDataBlock *tclas)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_mgt_tspec: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiSmeTspecReqSend(0, CSR_WIFI_INTERFACE_IN_USE,
+                           action, tid, TRUE, 0,
+                           tspec->length, tspec->data,
+                           tclas->length, tclas->data);
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4, "sme_mgt_tspec: <-- (status=%d)\n", priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+
+
+int sme_sys_suspend(unifi_priv_t *priv)
+{
+    int r;
+    CsrResult csrResult;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_sys_suspend: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    /* Suspend the SME, which MAY cause it to power down UniFi */
+    CsrWifiRouterCtrlSuspendIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0, 0, priv->wol_suspend);
+    r = sme_wait_for_reply(priv, UNIFI_SME_SYS_LONG_TIMEOUT);
+    if (r) {
+        /* No reply - forcibly power down in case the request wasn't processed */
+        unifi_notice(priv,
+                     "suspend: SME did not reply %s, ",
+                     (priv->ptest_mode | priv->wol_suspend) ? "leave powered" : "power off UniFi anyway\n");
+
+        /* Leave power on for production test, though */
+        if (!priv->ptest_mode) {
+            /* Put UniFi to deep sleep, in case we can not power it off */
+            CsrSdioClaim(priv->sdio);
+            unifi_trace(priv, UDBG1, "Force deep sleep");
+            csrResult = unifi_force_low_power_mode(priv->card);
+
+            /* For WOL, the UniFi must stay powered */
+            if (!priv->wol_suspend) {
+                unifi_trace(priv, UDBG1, "Power off\n");
+                CsrSdioPowerOff(priv->sdio);
+            }
+            CsrSdioRelease(priv->sdio);
+        }
+    }
+
+    if (priv->wol_suspend) {
+        unifi_trace(priv, UDBG1, "UniFi left powered for WOL\n");
+
+        /* Remove the IRQ, which also disables the card SDIO interrupt.
+         * Disabling the card SDIO interrupt enables the PIO WOL source.
+         * Removal of the of the handler ensures that in both SDIO and PIO cases
+         * the card interrupt only wakes the host. The card will be polled
+         * after resume to handle any pending data.
+         */
+        if (csr_sdio_linux_remove_irq(priv->sdio)) {
+            unifi_notice(priv, "WOL csr_sdio_linux_remove_irq failed\n");
+        }
+
+        if (enable_wol == UNIFI_WOL_SDIO) {
+            /* Because csr_sdio_linux_remove_irq() disabled the card SDIO interrupt,
+             * it must be left enabled to wake-on-SDIO.
+             */
+            unifi_trace(priv, UDBG1, "Enable card SDIO interrupt for SDIO WOL\n");
+
+            CsrSdioClaim(priv->sdio);
+            csrResult = CsrSdioInterruptEnable(priv->sdio);
+            CsrSdioRelease(priv->sdio);
+
+            if (csrResult != CSR_RESULT_SUCCESS) {
+                unifi_error(priv, "WOL CsrSdioInterruptEnable failed %d\n", csrResult);
+            }
+        } else {
+            unifi_trace(priv, UDBG1, "Disabled card SDIO interrupt for PIO WOL\n");
+        }
+
+        /* Prevent the BH thread from running during the suspend.
+         * Upon resume, sme_sys_resume() will trigger a wifi-on, this will cause
+         * the BH thread to be re-enabled and reinstall the ISR.
+         */
+        priv->bh_thread.block_thread = 1;
+
+        unifi_trace(priv, UDBG1, "unifi_suspend: suspended BH");
+    }
+
+    /* Consider UniFi to be uninitialised */
+    priv->init_progress = UNIFI_INIT_NONE;
+
+    unifi_trace(priv, UDBG1, "sme_sys_suspend: <-- (r=%d status=%d)\n", r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+
+int sme_sys_resume(unifi_priv_t *priv)
+{
+    int r;
+
+    unifi_trace(priv, UDBG1, "sme_sys_resume %s\n", priv->wol_suspend ? "warm" : "");
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_sys_resume: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiRouterCtrlResumeIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0, priv->wol_suspend);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_SYS_LONG_TIMEOUT);
+    if (r) {
+        unifi_notice(priv,
+                "resume: SME did not reply, return success anyway\n");
+    }
+
+    return 0;
+}
+
+#ifdef CSR_SUPPORT_WEXT_AP
+int sme_ap_stop(unifi_priv_t *priv,u16 interface_tag)
+{
+    int r;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_ap_stop: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiNmeApStopReqSend(0,interface_tag);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_ap_stop <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+
+}
+
+int sme_ap_start(unifi_priv_t *priv,u16 interface_tag,
+                 CsrWifiSmeApConfig_t * ap_config)
+{
+    int r;
+    CsrWifiSmeApP2pGoConfig p2p_go_param;
+    memset(&p2p_go_param,0,sizeof(CsrWifiSmeApP2pGoConfig));
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_ap_start: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiNmeApStartReqSend(0,interface_tag,CSR_WIFI_AP_TYPE_LEGACY,FALSE,
+                             ap_config->ssid,1,ap_config->channel,
+                             ap_config->credentials,ap_config->max_connections,
+                             p2p_go_param,FALSE);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+    if (r) {
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "sme_ap_start <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+
+int sme_ap_config(unifi_priv_t *priv,
+                  CsrWifiSmeApMacConfig *ap_mac_config,
+                  CsrWifiNmeApConfig *group_security_config)
+{
+    int r;
+    CsrWifiSmeApP2pGoConfig p2p_go_param;
+    memset(&p2p_go_param,0,sizeof(CsrWifiSmeApP2pGoConfig));
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_ap_config: invalid smepriv\n");
+        return -EIO;
+    }
+
+    r = sme_init_request(priv);
+    if (r) {
+        return -EIO;
+    }
+
+    CsrWifiNmeApConfigSetReqSend(0,*group_security_config,
+                                 *ap_mac_config);
+
+    r = sme_wait_for_reply(priv, UNIFI_SME_MGT_SHORT_TIMEOUT);
+
+    unifi_trace(priv, UDBG4,
+                "sme_ap_config <-- (r=%d status=%d)\n",
+                r, priv->sme_reply.reply_status);
+    return convert_sme_error(priv->sme_reply.reply_status);
+}
+#endif
diff --git a/drivers/staging/csr/sme_mgt.c b/drivers/staging/csr/sme_mgt.c
new file mode 100644
index 00000000000..58d1b3b7293
--- /dev/null
+++ b/drivers/staging/csr/sme_mgt.c
@@ -0,0 +1,1012 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     sme_mgt.c
+ *
+ * PURPOSE:
+ *      This file contains the driver specific implementation of
+ *      the SME MGT SAP.
+ *      It is part of the porting exercise.
+ *
+ * Copyright (C) 2008-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#include "csr_wifi_hip_unifiversion.h"
+#include "unifi_priv.h"
+#include "csr_wifi_hip_conversions.h"
+/*
+ * This file implements the SME MGT API. It contains the following functions:
+ * CsrWifiSmeWifiFlightmodeCfmSend()
+ * CsrWifiSmeWifiOnCfmSend()
+ * CsrWifiSmeWifiOffCfmSend()
+ * CsrWifiSmeWifiOffIndSend()
+ * CsrWifiSmeScanFullCfmSend()
+ * CsrWifiSmeScanResultsGetCfmSend()
+ * CsrWifiSmeScanResultIndSend()
+ * CsrWifiSmeScanResultsFlushCfmSend()
+ * CsrWifiSmeConnectCfmSend()
+ * CsrWifiSmeMediaStatusIndSend()
+ * CsrWifiSmeDisconnectCfmSend()
+ * CsrWifiSmeKeyCfmSend()
+ * CsrWifiSmeMulticastAddressCfmSend()
+ * CsrWifiSmeSetValueCfmSend()
+ * CsrWifiSmeGetValueCfmSend()
+ * CsrWifiSmeMicFailureIndSend()
+ * CsrWifiSmePmkidCfmSend()
+ * CsrWifiSmePmkidCandidateListIndSend()
+ * CsrWifiSmeMibSetCfmSend()
+ * CsrWifiSmeMibGetCfmSend()
+ * CsrWifiSmeMibGetNextCfmSend()
+ * CsrWifiSmeConnectionQualityIndSend()
+ * CsrWifiSmePacketFilterSetCfmSend()
+ * CsrWifiSmeTspecCfmSend()
+ * CsrWifiSmeTspecIndSend()
+ * CsrWifiSmeBlacklistCfmSend()
+ * CsrWifiSmeEventMaskSetCfmSend()
+ * CsrWifiSmeRoamStartIndSend()
+ * CsrWifiSmeRoamCompleteIndSend()
+ * CsrWifiSmeAssociationStartIndSend()
+ * CsrWifiSmeAssociationCompleteIndSend()
+ * CsrWifiSmeIbssStationIndSend()
+ */
+
+
+void CsrWifiSmeMicFailureIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMicFailureInd* ind = (CsrWifiSmeMicFailureInd*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeMicFailureIndSend: invalid priv\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG1,
+                "CsrWifiSmeMicFailureIndSend: count=%d, KeyType=%d\n",
+                ind->count, ind->keyType);
+
+    wext_send_michaelmicfailure_event(priv, ind->count, ind->address, ind->keyType, ind->interfaceTag);
+#endif
+}
+
+
+void CsrWifiSmePmkidCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmePmkidCfm* cfm = (CsrWifiSmePmkidCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmePmkidCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    /*
+     * WEXT never does a GET operation the PMKIDs, so we don't need
+     * handle data returned in pmkids.
+     */
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmePmkidCandidateListIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmePmkidCandidateListInd* ind = (CsrWifiSmePmkidCandidateListInd*)msg;
+    int i;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiSmePmkidCandidateListIndSend: invalid smepriv\n");
+        return;
+    }
+
+    for (i = 0; i < ind->pmkidCandidatesCount; i++)
+    {
+        wext_send_pmkid_candidate_event(priv, ind->pmkidCandidates[i].bssid, ind->pmkidCandidates[i].preAuthAllowed, ind->interfaceTag);
+    }
+#endif
+}
+
+void CsrWifiSmeScanResultsFlushCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeScanResultsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeScanResultsGetCfm* cfm = (CsrWifiSmeScanResultsGetCfm*)msg;
+    int bytesRequired = cfm->scanResultsCount * sizeof(CsrWifiSmeScanResult);
+    int i;
+    u8* current_buff;
+    CsrWifiSmeScanResult* scanCopy;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeScanResultsGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    /* Calc the size of the buffer reuired */
+    for (i = 0; i < cfm->scanResultsCount; ++i) {
+        const CsrWifiSmeScanResult *scan_result = &cfm->scanResults[i];
+        bytesRequired += scan_result->informationElementsLength;
+    }
+
+    /* Take a Copy of the scan Results :-) */
+    scanCopy = kmalloc(bytesRequired, GFP_KERNEL);
+    memcpy(scanCopy, cfm->scanResults, sizeof(CsrWifiSmeScanResult) * cfm->scanResultsCount);
+
+    /* Take a Copy of the Info Elements AND update the scan result pointers */
+    current_buff = (u8*)&scanCopy[cfm->scanResultsCount];
+    for (i = 0; i < cfm->scanResultsCount; ++i)
+    {
+        CsrWifiSmeScanResult *scan_result = &scanCopy[i];
+        memcpy(current_buff, scan_result->informationElements, scan_result->informationElementsLength);
+        scan_result->informationElements = current_buff;
+        current_buff += scan_result->informationElementsLength;
+    }
+
+    priv->sme_reply.reply_scan_results_count = cfm->scanResultsCount;
+    priv->sme_reply.reply_scan_results = scanCopy;
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeScanFullCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeScanFullCfm* cfm = (CsrWifiSmeScanFullCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeScanFullCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeScanResultIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+
+}
+
+
+void CsrWifiSmeConnectCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeConnectCfm* cfm = (CsrWifiSmeConnectCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeConnectCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeDisconnectCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeDisconnectCfm* cfm = (CsrWifiSmeDisconnectCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeDisconnectCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeKeyCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeKeyCfm* cfm = (CsrWifiSmeKeyCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeKeyCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeMulticastAddressCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMulticastAddressCfm* cfm = (CsrWifiSmeMulticastAddressCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeMulticastAddressCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeWifiFlightmodeCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeWifiFlightmodeCfm* cfm = (CsrWifiSmeWifiFlightmodeCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeWifiFlightmodeCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeWifiOnCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeWifiOnCfm* cfm = (CsrWifiSmeWifiOnCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeWifiOnCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG4,
+                "CsrWifiSmeWifiOnCfmSend: wake up status %d\n", cfm->status);
+#ifdef CSR_SUPPORT_WEXT_AP
+    sme_complete_request(priv, cfm->status);
+#endif
+
+#endif
+}
+
+void CsrWifiSmeWifiOffCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeWifiOffCfm* cfm = (CsrWifiSmeWifiOffCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeWifiOffCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeWifiOffIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeWifiOffInd* ind = (CsrWifiSmeWifiOffInd*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiRouterCtrlStoppedReqSend: Invalid ospriv.\n");
+        return;
+    }
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlStoppedReqSend: invalid smepriv\n");
+        return;
+    }
+
+    /*
+     * If the status indicates an error, the SME is in a stopped state.
+     * We need to start it again in order to reinitialise UniFi.
+     */
+    switch (ind->reason) {
+        case CSR_WIFI_SME_CONTROL_INDICATION_ERROR:
+          unifi_trace(priv, UDBG1,
+                      "CsrWifiRouterCtrlStoppedReqSend: Restarting SME (ind:%d)\n",
+                      ind->reason);
+
+          /* On error, restart the SME */
+          sme_mgt_wifi_on(priv);
+          break;
+        case CSR_WIFI_SME_CONTROL_INDICATION_EXIT:
+#ifdef CSR_SUPPORT_WEXT_AP
+          sme_complete_request(priv, 0);
+#endif
+          break;
+        default:
+          break;
+    }
+
+#endif
+}
+
+void CsrWifiSmeVersionsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeVersionsGetCfm* cfm = (CsrWifiSmeVersionsGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeVersionsGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.versions = cfm->versions;
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmePowerConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmePowerConfigGetCfm* cfm = (CsrWifiSmePowerConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmePowerConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.powerConfig = cfm->powerConfig;
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeHostConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeHostConfigGetCfm* cfm = (CsrWifiSmeHostConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeHostConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.hostConfig = cfm->hostConfig;
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeCoexInfoGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeCoexInfoGetCfm* cfm = (CsrWifiSmeCoexInfoGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeCoexInfoGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.coexInfo = cfm->coexInfo;
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeCoexConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeCoexConfigGetCfm* cfm = (CsrWifiSmeCoexConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeCoexConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.coexConfig = cfm->coexConfig;
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeMibConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMibConfigGetCfm* cfm = (CsrWifiSmeMibConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeMibConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.mibConfig = cfm->mibConfig;
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeConnectionInfoGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeConnectionInfoGetCfm* cfm = (CsrWifiSmeConnectionInfoGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeConnectionInfoGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.connectionInfo = cfm->connectionInfo;
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeConnectionConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeConnectionConfigGetCfm* cfm = (CsrWifiSmeConnectionConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeConnectionConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.connectionConfig = cfm->connectionConfig;
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeConnectionStatsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeConnectionStatsGetCfm* cfm = (CsrWifiSmeConnectionStatsGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeConnectionStatsGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.connectionStats = cfm->connectionStats;
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeMibSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMibSetCfm* cfm = (CsrWifiSmeMibSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeMibSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeMibGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMibGetCfm* cfm = (CsrWifiSmeMibGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeMibGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    if (cfm->mibAttribute == NULL) {
+        unifi_error(priv, "CsrWifiSmeMibGetCfmSend: Empty reply.\n");
+        sme_complete_request(priv, cfm->status);
+        return;
+    }
+
+    if ((priv->mib_cfm_buffer != NULL) &&
+        (priv->mib_cfm_buffer_length >= cfm->mibAttributeLength)) {
+        memcpy(priv->mib_cfm_buffer, cfm->mibAttribute, cfm->mibAttributeLength);
+        priv->mib_cfm_buffer_length = cfm->mibAttributeLength;
+    } else {
+        unifi_error(priv,
+                    "CsrWifiSmeMibGetCfmSend: No room to store MIB data (have=%d need=%d).\n",
+                    priv->mib_cfm_buffer_length, cfm->mibAttributeLength);
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeMibGetNextCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMibGetNextCfm* cfm = (CsrWifiSmeMibGetNextCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeMibGetNextCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    /* Need to copy MIB data */
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeConnectionQualityIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeConnectionQualityInd* ind = (CsrWifiSmeConnectionQualityInd*)msg;
+    int signal, noise, snr;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeConnectionQualityIndSend: Invalid ospriv.\n");
+        return;
+    }
+
+    /*
+     * level and noise below are mapped into an unsigned 8 bit number,
+     * ranging from [-192; 63]. The way this is achieved is simply to
+     * add 0x100 onto the number if it is negative,
+     * once clipped to the correct range.
+     */
+    signal = ind->linkQuality.unifiRssi;
+    /* Clip range of snr */
+    snr    = (ind->linkQuality.unifiSnr > 0) ? ind->linkQuality.unifiSnr : 0; /* In dB relative, from 0 - 255 */
+    snr    = (snr < 255) ? snr : 255;
+    noise  = signal - snr;
+
+    /* Clip range of signal */
+    signal = (signal < 63) ? signal : 63;
+    signal = (signal > -192) ? signal : -192;
+
+    /* Clip range of noise */
+    noise = (noise < 63) ? noise : 63;
+    noise = (noise > -192) ? noise : -192;
+
+    /* Make u8 */
+    signal = ( signal < 0 ) ? signal + 0x100 : signal;
+    noise = ( noise < 0 ) ? noise + 0x100 : noise;
+
+    priv->wext_wireless_stats.qual.level   = (u8)signal; /* -192 : 63 */
+    priv->wext_wireless_stats.qual.noise   = (u8)noise;  /* -192 : 63 */
+    priv->wext_wireless_stats.qual.qual    = snr;         /* 0 : 255 */
+    priv->wext_wireless_stats.qual.updated = 0;
+
+#if WIRELESS_EXT > 16
+    priv->wext_wireless_stats.qual.updated |= IW_QUAL_LEVEL_UPDATED |
+                                              IW_QUAL_NOISE_UPDATED |
+                                              IW_QUAL_QUAL_UPDATED;
+#if WIRELESS_EXT > 18
+    priv->wext_wireless_stats.qual.updated |= IW_QUAL_DBM;
+#endif
+#endif
+#endif
+}
+
+void CsrWifiSmePacketFilterSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmePacketFilterSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    /* The packet filter set request does not block for a reply */
+}
+
+void CsrWifiSmeTspecCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeTspecCfm* cfm = (CsrWifiSmeTspecCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeTspecCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeTspecIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeBlacklistCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeEventMaskSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+
+void CsrWifiSmeRoamStartIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeRoamCompleteIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    /* This is called when the association completes, before any 802.1x authentication */
+}
+
+void CsrWifiSmeAssociationStartIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeAssociationCompleteIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeIbssStationIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeWifiOnIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeRestrictedAccessEnableCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeRestrictedAccessDisableCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+
+void CsrWifiSmeAdhocConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeAdhocConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeAdhocConfigSetCfm* cfm = (CsrWifiSmeAdhocConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeCalibrationDataGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeCalibrationDataSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeCalibrationDataSetCfm* cfm = (CsrWifiSmeCalibrationDataSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeCcxConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeCcxConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeCcxConfigSetCfm* cfm = (CsrWifiSmeCcxConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeCloakedSsidsGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeCloakedSsidsSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeCloakedSsidsSetCfm* cfm = (CsrWifiSmeCloakedSsidsSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+
+void CsrWifiSmeCoexConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeCoexConfigSetCfm* cfm = (CsrWifiSmeCoexConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeHostConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeHostConfigSetCfm* cfm = (CsrWifiSmeHostConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeLinkQualityGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+
+void CsrWifiSmeMibConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMibConfigSetCfm* cfm = (CsrWifiSmeMibConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmePermanentMacAddressGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmePowerConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmePowerConfigSetCfm* cfm = (CsrWifiSmePowerConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeRegulatoryDomainInfoGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeRoamingConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeMediaStatusIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeMediaStatusInd* ind = (CsrWifiSmeMediaStatusInd*)msg;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiSmeMediaStatusIndSend: invalid smepriv\n");
+        return;
+    }
+
+    if (ind->mediaStatus == CSR_WIFI_SME_MEDIA_STATUS_CONNECTED) {
+        /*
+         * Send wireless-extension event up to userland to announce
+         * connection.
+         */
+        wext_send_assoc_event(priv,
+                              (unsigned char *)ind->connectionInfo.bssid.a,
+                              (unsigned char *)ind->connectionInfo.assocReqInfoElements,
+                              ind->connectionInfo.assocReqInfoElementsLength,
+                              (unsigned char *)ind->connectionInfo.assocRspInfoElements,
+                              ind->connectionInfo.assocRspInfoElementsLength,
+                              (unsigned char *)ind->connectionInfo.assocScanInfoElements,
+                              ind->connectionInfo.assocScanInfoElementsLength);
+
+	unifi_trace(priv, UDBG2, "CsrWifiSmeMediaStatusIndSend: IBSS=%pM\n",
+				 ind->connectionInfo.bssid.a);
+
+        sme_mgt_packet_filter_set(priv);
+
+    } else  {
+        /*
+         * Send wireless-extension event up to userland to announce
+         * connection lost to a BSS.
+         */
+        wext_send_disassoc_event(priv);
+    }
+#endif
+}
+
+void CsrWifiSmeRoamingConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeRoamingConfigSetCfm* cfm = (CsrWifiSmeRoamingConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeRoamingConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeScanConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeScanConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_SUPPORT_WEXT
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeScanConfigSetCfm* cfm = (CsrWifiSmeScanConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+#endif
+}
+
+void CsrWifiSmeStationMacAddressGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeSmeCommonConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeSmeCommonConfigGetCfm* cfm = (CsrWifiSmeSmeCommonConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeCommonConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.deviceConfig = cfm->deviceConfig;
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeSmeStaConfigGetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeSmeStaConfigGetCfm* cfm = (CsrWifiSmeSmeStaConfigGetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeStaConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    priv->sme_reply.staConfig = cfm->smeConfig;
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeSmeCommonConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeSmeCommonConfigSetCfm* cfm = (CsrWifiSmeSmeCommonConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeCommonConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeSmeStaConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiSmeSmeStaConfigSetCfm* cfm = (CsrWifiSmeSmeStaConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiSmeSmeStaConfigGetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiSmeGetInterfaceCapabilityCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeErrorIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeInfoIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeCoreDumpIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+void CsrWifiSmeAmpStatusChangeIndHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiSmeActivateCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+void CsrWifiSmeDeactivateCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+#ifdef CSR_SUPPORT_WEXT
+#ifdef CSR_SUPPORT_WEXT_AP
+void CsrWifiNmeApStartCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiNmeApStartCfm* cfm = (CsrWifiNmeApStartCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiNmeApStartCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiNmeApStopCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiNmeApStopCfm* cfm = (CsrWifiNmeApStopCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiNmeApStopCfmSend: Invalid ospriv.\n");
+        return;
+    }
+
+    sme_complete_request(priv, cfm->status);
+}
+
+void CsrWifiNmeApConfigSetCfmHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiNmeApConfigSetCfm* cfm = (CsrWifiNmeApConfigSetCfm*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiNmeApConfigSetCfmSend: Invalid ospriv.\n");
+        return;
+    }
+    sme_complete_request(priv, cfm->status);
+}
+#endif
+#endif
diff --git a/drivers/staging/csr/sme_native.c b/drivers/staging/csr/sme_native.c
new file mode 100644
index 00000000000..229268fd746
--- /dev/null
+++ b/drivers/staging/csr/sme_native.c
@@ -0,0 +1,591 @@
+/*
+ * ***************************************************************************
+ *
+ *  FILE:     sme_native.c
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+
+#include <linux/netdevice.h>
+
+#include "unifi_priv.h"
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+
+static const unsigned char wildcard_address[ETH_ALEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+int
+uf_sme_init(unifi_priv_t *priv)
+{
+    func_enter();
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+    sema_init(&priv->mlme_blocking_mutex, 1);
+#else
+    init_MUTEX(&priv->mlme_blocking_mutex);
+#endif
+
+#ifdef CSR_SUPPORT_WEXT
+    {
+        int r = uf_init_wext_interface(priv);
+        if (r != 0) {
+            func_exit();
+            return r;
+        }
+    }
+#endif
+
+
+
+    func_exit();
+    return 0;
+} /* uf_sme_init() */
+
+
+void
+uf_sme_deinit(unifi_priv_t *priv)
+{
+
+    func_enter();
+
+    /* Free memory allocated for the scan table */
+/*    unifi_clear_scan_table(priv); */
+
+    /* Cancel any pending workqueue tasks */
+    flush_workqueue(priv->unifi_workqueue);
+
+#ifdef CSR_SUPPORT_WEXT
+    uf_deinit_wext_interface(priv);
+#endif
+
+
+    func_exit();
+} /* uf_sme_deinit() */
+
+
+int sme_mgt_wifi_on(unifi_priv_t *priv)
+{
+    int r,i;
+    s32 csrResult;
+
+    if (priv == NULL) {
+        return -EINVAL;
+    }
+    /* Initialize the interface mode to None */
+    for (i=0; i<CSR_WIFI_NUM_INTERFACES; i++) {
+        priv->interfacePriv[i]->interfaceMode = 0;
+    }
+
+    /* Set up interface mode so that get_packet_priority() can
+     * select the right QOS priority when WMM is enabled.
+     */
+    priv->interfacePriv[0]->interfaceMode = CSR_WIFI_ROUTER_CTRL_MODE_STA;
+
+    r = uf_request_firmware_files(priv, UNIFI_FW_STA);
+    if (r) {
+        unifi_error(priv, "sme_mgt_wifi_on: Failed to get f/w\n");
+        return r;
+    }
+
+    /*
+     * The request to initialise UniFi might come while UniFi is running.
+     * We need to block all I/O activity until the reset completes, otherwise
+     * an SDIO error might occur resulting an indication to the SME which
+     * makes it think that the initialisation has failed.
+     */
+    priv->bh_thread.block_thread = 1;
+
+    /* Power on UniFi */
+    CsrSdioClaim(priv->sdio);
+    csrResult = CsrSdioPowerOn(priv->sdio);
+    CsrSdioRelease(priv->sdio);
+    if(csrResult != CSR_RESULT_SUCCESS && csrResult != CSR_SDIO_RESULT_NOT_RESET) {
+        return -EIO;
+    }
+
+    if (csrResult == CSR_RESULT_SUCCESS) {
+        /* Initialise UniFi hardware */
+        r = uf_init_hw(priv);
+        if (r) {
+            return r;
+        }
+    }
+
+    /* Re-enable the I/O thread */
+    priv->bh_thread.block_thread = 0;
+
+    /* Disable deep sleep signalling during the firmware initialisation, to
+     * prevent the wakeup mechanism raising the SDIO clock beyond INIT before
+     * the first MLME-RESET.ind. It gets re-enabled at the CONNECTED.ind,
+     * immediately after the MLME-RESET.ind
+     */
+    csrResult = unifi_configure_low_power_mode(priv->card,
+                                           UNIFI_LOW_POWER_DISABLED,
+                                           UNIFI_PERIODIC_WAKE_HOST_DISABLED);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_warning(priv,
+                      "sme_mgt_wifi_on: unifi_configure_low_power_mode() returned an error\n");
+    }
+
+
+    /* Start the I/O thread */
+    CsrSdioClaim(priv->sdio);
+    r = uf_init_bh(priv);
+    if (r) {
+        CsrSdioPowerOff(priv->sdio);
+        CsrSdioRelease(priv->sdio);
+        return r;
+    }
+    CsrSdioRelease(priv->sdio);
+
+    priv->init_progress = UNIFI_INIT_FW_DOWNLOADED;
+
+    return 0;
+}
+
+int
+sme_sys_suspend(unifi_priv_t *priv)
+{
+    const int interfaceNum = 0;     /* FIXME */
+    CsrResult csrResult;
+
+    /* Abort any pending requests. */
+    uf_abort_mlme(priv);
+
+    /* Allow our mlme request to go through. */
+    priv->io_aborted = 0;
+
+    /* Send MLME-RESET.req to UniFi. */
+    unifi_reset_state(priv, priv->netdev[interfaceNum]->dev_addr, 0);
+
+    /* Stop the network traffic */
+    netif_carrier_off(priv->netdev[interfaceNum]);
+
+    /* Put UniFi to deep sleep */
+    CsrSdioClaim(priv->sdio);
+    csrResult = unifi_force_low_power_mode(priv->card);
+    CsrSdioRelease(priv->sdio);
+
+    return 0;
+} /* sme_sys_suspend() */
+
+
+int
+sme_sys_resume(unifi_priv_t *priv)
+{
+#ifdef CSR_SUPPORT_WEXT
+    /* Send disconnect event so clients will re-initialise connection. */
+    memset(priv->wext_conf.current_ssid, 0, UNIFI_MAX_SSID_LEN);
+    memset((void*)priv->wext_conf.current_bssid, 0, ETH_ALEN);
+    priv->wext_conf.capability = 0;
+    wext_send_disassoc_event(priv);
+#endif
+    return 0;
+} /* sme_sys_resume() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  sme_native_log_event
+ *
+ *      Callback function to be registered as the SME event callback.
+ *      Copies the signal content into a new udi_log_t struct and adds
+ *      it to the read queue for the SME client.
+ *
+ *  Arguments:
+ *      arg             This is the value given to unifi_add_udi_hook, in
+ *                      this case a pointer to the client instance.
+ *      signal          Pointer to the received signal.
+ *      signal_len      Size of the signal structure in bytes.
+ *      bulkdata        Pointers to any associated bulk data.
+ *      dir             Direction of the signal. Zero means from host,
+ *                      non-zero means to host.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+sme_native_log_event(ul_client_t *pcli,
+                     const u8 *sig_packed, int sig_len,
+                     const bulk_data_param_t *bulkdata,
+                     int dir)
+{
+    unifi_priv_t *priv;
+    udi_log_t *logptr;
+    u8 *p;
+    int i, r;
+    int signal_len;
+    int total_len;
+    udi_msg_t *msgptr;
+    CSR_SIGNAL signal;
+    ul_client_t *client = pcli;
+
+    func_enter();
+
+    if (client == NULL) {
+        unifi_error(NULL, "sme_native_log_event: client has exited\n");
+        return;
+    }
+
+    priv = uf_find_instance(client->instance);
+    if (!priv) {
+        unifi_error(priv, "invalid priv\n");
+        return;
+    }
+
+    /* Just a sanity check */
+    if ((sig_packed == NULL) || (sig_len <= 0)) {
+        return;
+    }
+
+    /* Get the unpacked signal */
+    r = read_unpack_signal(sig_packed, &signal);
+    if (r == 0) {
+        signal_len = SigGetSize(&signal);
+    } else {
+        u16 receiver_id = CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sig_packed) + sizeof(u16)) & 0xFF00;
+
+        /* The control indications are 1 byte, pass them to client. */
+        if (sig_len == 1) {
+            unifi_trace(priv, UDBG5,
+                        "Control indication (0x%x) for native SME.\n",
+                        *sig_packed);
+
+            *(u8*)&signal = *sig_packed;
+            signal_len = sig_len;
+        } else if (receiver_id == 0) {
+            /*
+             * Also "unknown" signals with a ReceiverId of 0 are passed to the client
+             * without unpacking. (This is a code size optimisation to allow signals
+             * that the driver not interested in to be dropped from the unpack code).
+             */
+            unifi_trace(priv, UDBG5,
+                        "Signal 0x%.4X with ReceiverId 0 for native SME.\n",
+                        CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sig_packed));
+
+            *(u8*)&signal = *sig_packed;
+            signal_len = sig_len;
+        } else {
+            unifi_error(priv,
+                        "sme_native_log_event - Received unknown signal 0x%.4X.\n",
+                        CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sig_packed));
+            return;
+        }
+    }
+
+    unifi_trace(priv, UDBG3, "sme_native_log_event: signal 0x%.4X for %d\n",
+                signal.SignalPrimitiveHeader.SignalId,
+                client->client_id);
+
+    total_len = signal_len;
+    /* Calculate the buffer we need to store signal plus bulk data */
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+        total_len += bulkdata->d[i].data_length;
+    }
+
+    /* Allocate log structure plus actual signal. */
+    logptr = (udi_log_t *)kmalloc(sizeof(udi_log_t) + total_len, GFP_KERNEL);
+
+    if (logptr == NULL) {
+        unifi_error(priv,
+                    "Failed to allocate %d bytes for a UDI log record\n",
+                    sizeof(udi_log_t) + total_len);
+        return;
+    }
+
+    /* Fill in udi_log struct */
+    INIT_LIST_HEAD(&logptr->q);
+    msgptr = &logptr->msg;
+    msgptr->length = sizeof(udi_msg_t) + total_len;
+    msgptr->timestamp = jiffies_to_msecs(jiffies);
+    msgptr->direction = dir;
+    msgptr->signal_length = signal_len;
+
+    /* Copy signal and bulk data to the log */
+    p = (u8 *)(msgptr + 1);
+    memcpy(p, &signal, signal_len);
+    p += signal_len;
+
+    /* Append any bulk data */
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+        int len = bulkdata->d[i].data_length;
+
+        /*
+         * Len here might not be the same as the length in the bulk data slot.
+         * The slot length will always be even, but len could be odd.
+         */
+        if (len > 0) {
+            if (bulkdata->d[i].os_data_ptr) {
+                memcpy(p, bulkdata->d[i].os_data_ptr, len);
+            } else {
+                memset(p, 0, len);
+            }
+            p += len;
+        }
+    }
+
+    /* Add to tail of log queue */
+    down(&client->udi_sem);
+    list_add_tail(&logptr->q, &client->udi_log);
+    up(&client->udi_sem);
+
+    /* Wake any waiting user process */
+    wake_up_interruptible(&client->udi_wq);
+
+    func_exit();
+
+} /* sme_native_log_event() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_ta_indicate_protocol
+ *
+ *      Report that a packet of a particular type has been seen
+ *
+ *  Arguments:
+ *      drv_priv        The device context pointer passed to ta_init.
+ *      protocol        The protocol type enum value.
+ *      direction       Whether the packet was a tx or rx.
+ *      src_addr        The source MAC address from the data packet.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      We defer the actual sending to a background workqueue,
+ *      see uf_ta_ind_wq().
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_ta_indicate_protocol(void *ospriv,
+                           CsrWifiRouterCtrlTrafficPacketType packet_type,
+                           CsrWifiRouterCtrlProtocolDirection direction,
+                           const CsrWifiMacAddress *src_addr)
+{
+
+} /* unifi_ta_indicate_protocol */
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_ta_indicate_sampling
+ *
+ *      Send the TA sampling information to the SME.
+ *
+ *  Arguments:
+ *      drv_priv        The device context pointer passed to ta_init.
+ *      stats           The TA sampling data to send.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_ta_indicate_sampling(void *ospriv, CsrWifiRouterCtrlTrafficStats *stats)
+{
+
+} /* unifi_ta_indicate_sampling() */
+
+
+void
+unifi_ta_indicate_l4stats(void *ospriv,
+                            u32 rxTcpThroughput,
+                            u32 txTcpThroughput,
+                            u32 rxUdpThroughput,
+                            u32 txUdpThroughput)
+{
+
+} /* unifi_ta_indicate_l4stats() */
+
+/*
+ * ---------------------------------------------------------------------------
+ * uf_native_process_udi_signal
+ *
+ *      Process interesting signals from the UDI interface.
+ *
+ *  Arguments:
+ *      pcli            A pointer to the client instance.
+ *      signal          Pointer to the received signal.
+ *      signal_len      Size of the signal structure in bytes.
+ *      bulkdata        Pointers to any associated bulk data.
+ *      dir             Direction of the signal. Zero means from host,
+ *                      non-zero means to host.
+ *
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+uf_native_process_udi_signal(ul_client_t *pcli,
+                             const u8 *packed_signal, int packed_signal_len,
+                             const bulk_data_param_t *bulkdata, int dir)
+{
+
+} /* uf_native_process_udi_signal() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  sme_native_mlme_event_handler
+ *
+ *      Callback function to be used as the udi_event_callback when registering
+ *      as a client.
+ *      This function implements a blocking request-reply interface for WEXT.
+ *      To use it, a client specifies this function as the udi_event_callback
+ *      to ul_register_client(). The signal dispatcher in
+ *      unifi_receive_event() will call this function to deliver a signal.
+ *
+ *  Arguments:
+ *      pcli            Pointer to the client instance.
+ *      signal          Pointer to the received signal.
+ *      signal_len      Size of the signal structure in bytes.
+ *      bulkdata        Pointer to structure containing any associated bulk data.
+ *      dir             Direction of the signal. Zero means from host,
+ *                      non-zero means to host.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+sme_native_mlme_event_handler(ul_client_t *pcli,
+                              const u8 *sig_packed, int sig_len,
+                              const bulk_data_param_t *bulkdata,
+                              int dir)
+{
+    CSR_SIGNAL signal;
+    int signal_len;
+    unifi_priv_t *priv = uf_find_instance(pcli->instance);
+    int id, r;
+
+    func_enter();
+
+    /* Just a sanity check */
+    if ((sig_packed == NULL) || (sig_len <= 0)) {
+        return;
+    }
+
+    /* Get the unpacked signal */
+    r = read_unpack_signal(sig_packed, &signal);
+    if (r == 0) {
+        signal_len = SigGetSize(&signal);
+    } else {
+        unifi_error(priv,
+                    "sme_native_mlme_event_handler - Received unknown signal 0x%.4X.\n",
+                    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sig_packed));
+        return;
+    }
+
+    id = signal.SignalPrimitiveHeader.SignalId;
+    unifi_trace(priv, UDBG4, "wext - Process signal 0x%.4X\n", id);
+
+    /*
+     * Take the appropriate action for the signal.
+     */
+    switch (id) {
+        /*
+         * Confirm replies from UniFi.
+         * These all have zero or one CSR_DATAREF member. (FIXME: check this is still true for softmac)
+         */
+        case CSR_MA_PACKET_CONFIRM_ID:
+        case CSR_MLME_RESET_CONFIRM_ID:
+        case CSR_MLME_GET_CONFIRM_ID:
+        case CSR_MLME_SET_CONFIRM_ID:
+        case CSR_MLME_GET_NEXT_CONFIRM_ID:
+        case CSR_MLME_POWERMGT_CONFIRM_ID:
+        case CSR_MLME_SCAN_CONFIRM_ID:
+        case CSR_MLME_HL_SYNC_CONFIRM_ID:
+        case CSR_MLME_MEASURE_CONFIRM_ID:
+        case CSR_MLME_SETKEYS_CONFIRM_ID:
+        case CSR_MLME_DELETEKEYS_CONFIRM_ID:
+        case CSR_MLME_HL_SYNC_CANCEL_CONFIRM_ID:
+        case CSR_MLME_ADD_PERIODIC_CONFIRM_ID:
+        case CSR_MLME_DEL_PERIODIC_CONFIRM_ID:
+        case CSR_MLME_ADD_AUTONOMOUS_SCAN_CONFIRM_ID:
+        case CSR_MLME_DEL_AUTONOMOUS_SCAN_CONFIRM_ID:
+        case CSR_MLME_SET_PACKET_FILTER_CONFIRM_ID:
+        case CSR_MLME_STOP_MEASURE_CONFIRM_ID:
+        case CSR_MLME_PAUSE_AUTONOMOUS_SCAN_CONFIRM_ID:
+        case CSR_MLME_ADD_TRIGGERED_GET_CONFIRM_ID:
+        case CSR_MLME_DEL_TRIGGERED_GET_CONFIRM_ID:
+        case CSR_MLME_ADD_BLACKOUT_CONFIRM_ID:
+        case CSR_MLME_DEL_BLACKOUT_CONFIRM_ID:
+        case CSR_MLME_ADD_RX_TRIGGER_CONFIRM_ID:
+        case CSR_MLME_DEL_RX_TRIGGER_CONFIRM_ID:
+        case CSR_MLME_CONNECT_STATUS_CONFIRM_ID:
+        case CSR_MLME_MODIFY_BSS_PARAMETER_CONFIRM_ID:
+        case CSR_MLME_ADD_TEMPLATE_CONFIRM_ID:
+        case CSR_MLME_CONFIG_QUEUE_CONFIRM_ID:
+        case CSR_MLME_ADD_TSPEC_CONFIRM_ID:
+        case CSR_MLME_DEL_TSPEC_CONFIRM_ID:
+        case CSR_MLME_START_AGGREGATION_CONFIRM_ID:
+        case CSR_MLME_STOP_AGGREGATION_CONFIRM_ID:
+        case CSR_MLME_SM_START_CONFIRM_ID:
+        case CSR_MLME_LEAVE_CONFIRM_ID:
+        case CSR_MLME_SET_TIM_CONFIRM_ID:
+        case CSR_MLME_GET_KEY_SEQUENCE_CONFIRM_ID:
+        case CSR_MLME_SET_CHANNEL_CONFIRM_ID:
+        case CSR_MLME_ADD_MULTICAST_ADDRESS_CONFIRM_ID:
+        case CSR_DEBUG_GENERIC_CONFIRM_ID:
+            unifi_mlme_copy_reply_and_wakeup_client(pcli, &signal, signal_len, bulkdata);
+            break;
+
+        case CSR_MLME_CONNECTED_INDICATION_ID:
+            /* We currently ignore the connected-ind for softmac f/w development */
+            unifi_info(priv, "CSR_MLME_CONNECTED_INDICATION_ID ignored\n");
+            break;
+
+        default:
+            break;
+    }
+
+    func_exit();
+} /* sme_native_mlme_event_handler() */
+
+
+
+/*
+ * -------------------------------------------------------------------------
+ *  unifi_reset_state
+ *
+ *      Ensure that a MAC address has been set.
+ *      Send the MLME-RESET signal.
+ *      This must be called at least once before starting to do any
+ *      network activities (e.g. scan, join etc).
+ *
+ * Arguments:
+ *      priv            Pointer to device private context struct
+ *      macaddr         Pointer to chip MAC address.
+ *                      If this is FF:FF:FF:FF:FF:FF it will be replaced
+ *                      with the MAC address from the chip.
+ *      set_default_mib 1 if the f/w must reset the MIB to the default values
+ *                      0 otherwise
+ *
+ * Returns:
+ *      0 on success, an error code otherwise.
+ * -------------------------------------------------------------------------
+ */
+int
+unifi_reset_state(unifi_priv_t *priv, unsigned char *macaddr,
+                  unsigned char set_default_mib)
+{
+    int r = 0;
+
+    func_enter();
+
+#ifdef CSR_SUPPORT_WEXT
+    /* The reset clears any 802.11 association. */
+    priv->wext_conf.flag_associated = 0;
+#endif
+
+    func_exit();
+    return r;
+} /* unifi_reset_state() */
+
diff --git a/drivers/staging/csr/sme_sys.c b/drivers/staging/csr/sme_sys.c
new file mode 100644
index 00000000000..99de27e678d
--- /dev/null
+++ b/drivers/staging/csr/sme_sys.c
@@ -0,0 +1,3262 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     sme_sys.c
+ *
+ * PURPOSE:
+ *      Driver specific implementation of the SME SYS SAP.
+ *      It is part of the porting exercise.
+ *
+ * Copyright (C) 2008-2011 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#include "csr_wifi_hip_unifiversion.h"
+#include "unifi_priv.h"
+#include "csr_wifi_hip_conversions.h"
+#ifdef CSR_SUPPORT_WEXT_AP
+#include "csr_wifi_sme_sef.h"
+#endif
+
+
+/*
+ * This file implements the SME SYS API and contains the following functions:
+ * CsrWifiRouterCtrlMediaStatusReqHandler()
+ * CsrWifiRouterCtrlHipReqHandler()
+ * CsrWifiRouterCtrlPortConfigureReqHandler()
+ * CsrWifiRouterCtrlWifiOnReqHandler()
+ * CsrWifiRouterCtrlWifiOffReqHandler()
+ * CsrWifiRouterCtrlSuspendResHandler()
+ * CsrWifiRouterCtrlResumeResHandler()
+ * CsrWifiRouterCtrlQosControlReqHandler()
+ * CsrWifiRouterCtrlConfigurePowerModeReqHandler()
+ * CsrWifiRouterCtrlWifiOnResHandler()
+ * CsrWifiRouterCtrlWifiOffRspHandler()
+ * CsrWifiRouterCtrlMulticastAddressResHandler()
+ * CsrWifiRouterCtrlTrafficConfigReqHandler()
+ * CsrWifiRouterCtrlTrafficClassificationReqHandler()
+ * CsrWifiRouterCtrlTclasAddReqHandler()
+ * CsrWifiRouterCtrlTclasDelReqHandler()
+ * CsrWifiRouterCtrlSetModeReqHandler()
+ * CsrWifiRouterCtrlWapiMulticastFilterReqHandler()
+ * CsrWifiRouterCtrlWapiUnicastFilterReqHandler()
+ * CsrWifiRouterCtrlWapiUnicastTxPktReqHandler()
+ * CsrWifiRouterCtrlWapiRxPktReqHandler()
+ * CsrWifiRouterCtrlWapiFilterReqHandler()
+ */
+
+#ifdef CSR_SUPPORT_SME
+static void check_inactivity_timer_expire_func(unsigned long data);
+void uf_send_disconnected_ind_wq(struct work_struct *work);
+#endif
+
+void send_auto_ma_packet_confirm(unifi_priv_t *priv,
+                                 netInterface_priv_t *interfacePriv,
+                                 struct list_head *buffered_frames_list)
+{
+    tx_buffered_packets_t *buffered_frame_item = NULL;
+    struct list_head *listHead;
+    struct list_head *placeHolder;
+    int client_id;
+
+    CSR_SIGNAL unpacked_signal;
+    u8 sigbuf[UNIFI_PACKED_SIGBUF_SIZE];
+    u16 packed_siglen;
+
+
+    list_for_each_safe(listHead, placeHolder, buffered_frames_list)
+    {
+        buffered_frame_item = list_entry(listHead, tx_buffered_packets_t, q);
+
+        if(!buffered_frame_item) {
+            unifi_error(priv, "Entry should exist, otherwise it is a (BUG)\n");
+            continue;
+        }
+
+        if ((interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_NONE) &&
+            (priv->wifi_on_state == wifi_on_done))
+        {
+
+            unifi_warning(priv, "Send MA_PACKET_CONFIRM to SenderProcessId = %x for (HostTag = %x TransmissionControl = %x)\n",
+                                 (buffered_frame_item->leSenderProcessId),
+                                 buffered_frame_item->hostTag,
+                                 buffered_frame_item->transmissionControl);
+
+            client_id = buffered_frame_item->leSenderProcessId & 0xFF00;
+
+            if (client_id == priv->sme_cli->sender_id)
+            {
+                /* construct a MA-PACKET.confirm message for SME */
+                memset(&unpacked_signal, 0, sizeof(unpacked_signal));
+                unpacked_signal.SignalPrimitiveHeader.SignalId = CSR_MA_PACKET_CONFIRM_ID;
+                unpacked_signal.SignalPrimitiveHeader.ReceiverProcessId = buffered_frame_item->leSenderProcessId;
+                unpacked_signal.SignalPrimitiveHeader.SenderProcessId = CSR_WIFI_ROUTER_IFACEQUEUE;
+
+                unpacked_signal.u.MaPacketConfirm.VirtualInterfaceIdentifier = uf_get_vif_identifier(interfacePriv->interfaceMode,
+                                                                                                     interfacePriv->InterfaceTag);
+                unpacked_signal.u.MaPacketConfirm.TransmissionStatus = CSR_RESULT_FAILURE;
+                unpacked_signal.u.MaPacketConfirm.RetryCount = 0;
+                unpacked_signal.u.MaPacketConfirm.Rate = buffered_frame_item->rate;
+                unpacked_signal.u.MaPacketConfirm.HostTag = buffered_frame_item->hostTag;
+
+                write_pack(&unpacked_signal, sigbuf, &packed_siglen);
+                unifi_warning(priv, "MA_PACKET_CONFIRM for SME (0x%x, 0x%x, 0x%x, 0x%x)\n",
+                                         unpacked_signal.SignalPrimitiveHeader.ReceiverProcessId,
+                                         unpacked_signal.SignalPrimitiveHeader.SenderProcessId,
+                                         unpacked_signal.u.MaPacketConfirm.VirtualInterfaceIdentifier,
+                                         unpacked_signal.u.MaPacketConfirm.HostTag);
+
+                CsrWifiRouterCtrlHipIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,
+                                            packed_siglen,
+                                            (u8 *)sigbuf,
+                                            0, NULL,
+                                            0, NULL);
+            }
+            else if((buffered_frame_item->hostTag & 0x80000000))
+            {
+                /* construct a MA-PACKET.confirm message for NME */
+                unifi_warning(priv, "MA_PACKET_CONFIRM for NME (0x%x, 0x%x, 0x%x, 0x%x)\n",
+                                    buffered_frame_item->leSenderProcessId,
+                                    buffered_frame_item->interfaceTag,
+                                    buffered_frame_item->transmissionControl,
+                                    (buffered_frame_item->hostTag & 0x3FFFFFFF));
+
+                CsrWifiRouterMaPacketCfmSend((buffered_frame_item->leSenderProcessId & 0xFF),
+                                            buffered_frame_item->interfaceTag,
+                                            CSR_RESULT_FAILURE,
+                                            (buffered_frame_item->hostTag & 0x3FFFFFFF),
+                                            buffered_frame_item->rate);
+
+            }
+            else
+            {
+                unifi_warning(priv, "Buffered packet dropped without sending a confirm\n");
+            }
+
+        }
+
+        list_del(listHead);
+        kfree(buffered_frame_item);
+        buffered_frame_item = NULL;
+    }
+}
+
+void CsrWifiRouterCtrlMediaStatusReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlMediaStatusReq* req = (CsrWifiRouterCtrlMediaStatusReq*)msg;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+    unsigned long flags;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlMediaStatusReqHandler: invalid smepriv\n");
+        return;
+    }
+    if (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "CsrWifiRouterCtrlMediaStatusReqHandler: invalid interfaceTag\n");
+        return;
+    }
+    unifi_trace(priv, UDBG3, "CsrWifiRouterCtrlMediaStatusReqHandler: Mode = %d req->mediaStatus = %d\n",interfacePriv->interfaceMode,req->mediaStatus);
+    if (interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_AMP) {
+        bulk_data_desc_t bulk_data;
+
+        bulk_data.data_length = 0;
+
+        spin_lock_irqsave(&priv->m4_lock, flags);
+        if (interfacePriv->m4_bulk_data.data_length > 0) {
+            bulk_data = interfacePriv->m4_bulk_data;
+            interfacePriv->m4_bulk_data.net_buf_length = 0;
+            interfacePriv->m4_bulk_data.data_length = 0;
+            interfacePriv->m4_bulk_data.os_data_ptr = interfacePriv->m4_bulk_data.os_net_buf_ptr = NULL;
+        }
+        spin_unlock_irqrestore(&priv->m4_lock, flags);
+
+        if (bulk_data.data_length != 0) {
+            unifi_trace(priv, UDBG5, "CsrWifiRouterCtrlMediaStatusReqHandler: free M4\n");
+            unifi_net_data_free(priv, &bulk_data);
+        }
+
+        if ((req->mediaStatus == CSR_WIFI_SME_MEDIA_STATUS_CONNECTED) &&
+            (interfacePriv->connected != UnifiConnected)) {
+
+            switch(interfacePriv->interfaceMode){
+                case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+                case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+                    interfacePriv->connected = UnifiConnected;
+                    netif_carrier_on(priv->netdev[req->interfaceTag]);
+#ifdef CSR_SUPPORT_WEXT
+                    wext_send_started_event(priv);
+#endif
+                    unifi_trace(priv, UDBG1,
+                                "CsrWifiRouterCtrlMediaStatusReqHandler: AP/P2PGO setting netif_carrier_on\n");
+                    UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[req->interfaceTag]);
+                    break;
+
+                default:
+#ifdef CSR_SUPPORT_WEXT
+                /* In the WEXT builds (sme and native), the userspace is not ready
+                 * to process any EAPOL or WAPI packets, until it has been informed
+                 * of the NETDEV_CHANGE.
+                 */
+                if (interfacePriv->netdev_callback_registered && (interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI)) {
+                    interfacePriv->wait_netdev_change = TRUE;
+                    unifi_trace(priv, UDBG1,
+                                "CsrWifiRouterCtrlMediaStatusReqHandler: waiting for NETDEV_CHANGE\n");
+                    /*
+                     * Carrier can go to on, only after wait_netdev_change is set to TRUE.
+                     * Otherwise there can be a race in uf_netdev_event().
+                     */
+                    netif_carrier_on(priv->netdev[req->interfaceTag]);
+                    unifi_trace(priv, UDBG1,
+                                "CsrWifiRouterCtrlMediaStatusReqHandler: STA/P2PCLI setting netif_carrier_on\n");
+                }
+                else
+#endif
+                {
+                    /* In the NME build, the userspace does not wait for the NETDEV_CHANGE
+                     * so it is ready to process all the EAPOL or WAPI packets.
+                     * At this point, we enable all the Tx queues, and we indicate any packets
+                     * that are queued (and the respective port is opened).
+                     */
+                    static const CsrWifiMacAddress broadcast_address = {{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
+                    interfacePriv->connected = UnifiConnected;
+                    unifi_trace(priv, UDBG1,
+                                "CsrWifiRouterMediaStatusReqHandler: UnifiConnected && netif_carrier_on\n");
+                    netif_carrier_on(priv->netdev[req->interfaceTag]);
+                    UF_NETIF_TX_WAKE_ALL_QUEUES(priv->netdev[req->interfaceTag]);
+                    uf_process_rx_pending_queue(priv, UF_UNCONTROLLED_PORT_Q, broadcast_address, 1, interfacePriv->InterfaceTag);
+                    uf_process_rx_pending_queue(priv, UF_CONTROLLED_PORT_Q, broadcast_address, 1, interfacePriv->InterfaceTag);
+                }
+                break;
+            }
+        }
+
+        if (req->mediaStatus == CSR_WIFI_SME_MEDIA_STATUS_DISCONNECTED) {
+#ifdef CSR_SUPPORT_WEXT
+            unifi_trace(priv, UDBG1,
+                        "CsrWifiRouterMediaStatusReqHandler: cancel waiting for NETDEV_CHANGE\n");
+            interfacePriv->wait_netdev_change = FALSE;
+#endif
+            unifi_trace(priv, UDBG1,
+                        "CsrWifiRouterMediaStatusReqHandler: setting netif_carrier_off\n");
+            netif_carrier_off(priv->netdev[req->interfaceTag]);
+#ifdef CSR_SUPPORT_WEXT
+            switch(interfacePriv->interfaceMode){
+                case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+                case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+                     wext_send_started_event(priv);
+                     break;
+                default:
+                     break;
+            }
+#endif
+            interfacePriv->connected = UnifiNotConnected;
+        }
+    } else {
+        /* For AMP, just update the L2 connected flag */
+        if (req->mediaStatus == CSR_WIFI_SME_MEDIA_STATUS_CONNECTED) {
+            unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlMediaStatusReqHandler: AMP connected\n");
+            interfacePriv->connected = UnifiConnected;
+        } else {
+            unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlMediaStatusReqHandler: AMP disconnected\n");
+            interfacePriv->connected = UnifiNotConnected;
+        }
+    }
+}
+
+
+void CsrWifiRouterCtrlHipReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlHipReq* hipreq = (CsrWifiRouterCtrlHipReq*)msg;
+    bulk_data_param_t bulkdata;
+    u8 *signal_ptr;
+    int signal_length;
+    int r=0;
+    void *dest;
+    CsrResult csrResult;
+    CSR_SIGNAL *signal;
+    u16 interfaceTag = 0;
+    CSR_MA_PACKET_REQUEST *req;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (priv == NULL) {
+        return;
+    }
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlHipReqHandler: invalid smepriv\n");
+        return;
+    }
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "CsrWifiRouterCtrlHipReqHandler: invalid interfaceTag\n");
+        return;
+    }
+
+    /* Initialize bulkdata to avoid os_net_buf is garbage */
+    memset(&bulkdata, 0, sizeof(bulk_data_param_t));
+
+    signal = (CSR_SIGNAL *)hipreq->mlmeCommand;
+
+    unifi_trace(priv, UDBG4, "CsrWifiRouterCtrlHipReqHandler: 0x04%X ---->\n",
+                *((u16*)hipreq->mlmeCommand));
+
+    /* Construct the signal. */
+    signal_ptr = (u8*)hipreq->mlmeCommand;
+    signal_length = hipreq->mlmeCommandLength;
+
+    /*
+     * The MSB of the sender ID needs to be set to the client ID.
+     * The LSB is controlled by the SME.
+     */
+    signal_ptr[5] = (priv->sme_cli->sender_id >> 8) & 0xff;
+
+    /* Allocate buffers for the bulk data. */
+    if (hipreq->dataRef1Length) {
+        csrResult = unifi_net_data_malloc(priv, &bulkdata.d[0], hipreq->dataRef1Length);
+        if (csrResult == CSR_RESULT_SUCCESS) {
+            dest = (void*)bulkdata.d[0].os_data_ptr;
+            memcpy(dest, hipreq->dataRef1, hipreq->dataRef1Length);
+            bulkdata.d[0].data_length = hipreq->dataRef1Length;
+        } else {
+            unifi_warning(priv, "signal not sent down, allocation failed in CsrWifiRouterCtrlHipReqHandler\n");
+            return;
+        }
+    } else {
+        bulkdata.d[0].os_data_ptr = NULL;
+        bulkdata.d[0].data_length = 0;
+    }
+    if (hipreq->dataRef2Length) {
+        csrResult = unifi_net_data_malloc(priv, &bulkdata.d[1], hipreq->dataRef2Length);
+        if (csrResult == CSR_RESULT_SUCCESS) {
+            dest = (void*)bulkdata.d[1].os_data_ptr;
+            memcpy(dest, hipreq->dataRef2, hipreq->dataRef2Length);
+            bulkdata.d[1].data_length = hipreq->dataRef2Length;
+        } else {
+            if (bulkdata.d[0].data_length)
+            {
+                unifi_net_data_free(priv, &bulkdata.d[0]);
+            }
+            unifi_warning(priv, "signal not sent down, allocation failed in CsrWifiRouterCtrlHipReqHandler\n");
+            return;
+        }
+    } else {
+        bulkdata.d[1].os_data_ptr = NULL;
+        bulkdata.d[1].data_length = 0;
+    }
+
+    unifi_trace(priv, UDBG3, "SME SEND: Signal 0x%.4X \n",
+                *((u16*)signal_ptr));
+    if (signal->SignalPrimitiveHeader.SignalId == CSR_MA_PACKET_REQUEST_ID)
+    {
+        CSR_SIGNAL unpacked_signal;
+        read_unpack_signal((u8 *) signal, &unpacked_signal);
+        req = &unpacked_signal.u.MaPacketRequest;
+        interfaceTag = req->VirtualInterfaceIdentifier & 0xff;
+        switch(interfacePriv->interfaceMode)
+        {
+            case CSR_WIFI_ROUTER_CTRL_MODE_NONE:
+                unifi_error(priv, "CsrWifiRouterCtrlHipReqHandler: invalid mode: NONE \n");
+                break;
+            default:
+                unifi_trace(priv, UDBG5, "mode is %x\n", interfacePriv->interfaceMode);
+        }
+        /* While sending ensure that first 2 bits b31 and b30 are 00. These are used for local routing*/
+        r = uf_process_ma_packet_req(priv, req->Ra.x, (req->HostTag & 0x3FFFFFFF), interfaceTag,
+                                     req->TransmissionControl, req->TransmitRate,
+                                     req->Priority, signal->SignalPrimitiveHeader.SenderProcessId,
+                                     &bulkdata);
+        if (r)
+        {
+            if (bulkdata.d[0].data_length)
+            {
+                unifi_net_data_free(priv, &bulkdata.d[0]);
+            }
+            if (bulkdata.d[1].data_length)
+            {
+                unifi_net_data_free(priv, &bulkdata.d[1]);
+            }
+        }
+    } else {
+        /* ul_send_signal_raw frees the bulk data if it fails */
+        r = ul_send_signal_raw(priv, signal_ptr, signal_length, &bulkdata);
+    }
+
+    if (r) {
+        unifi_error(priv,
+                    "CsrWifiRouterCtrlHipReqHandler: Failed to send signal (0x%.4X - %u)\n",
+                    *((u16*)signal_ptr), r);
+        CsrWifiRouterCtrlWifiOffIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,CSR_WIFI_SME_CONTROL_INDICATION_ERROR);
+    }
+
+    unifi_trace(priv, UDBG4, "CsrWifiRouterCtrlHipReqHandler: <----\n");
+}
+
+#ifdef CSR_WIFI_SEND_GRATUITOUS_ARP
+static void
+uf_send_gratuitous_arp(unifi_priv_t *priv, u16 interfaceTag)
+{
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    CSR_PRIORITY priority;
+    CSR_SIGNAL signal;
+    bulk_data_param_t bulkdata;
+    CsrResult csrResult;
+    struct sk_buff *skb, *newSkb = NULL;
+    s8 protection;
+    int r;
+    static const u8 arp_req[36] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00,
+                                         0x08, 0x06, 0x00, 0x01, 0x08, 0x00, 0x06, 0x04, 0x00, 0x01,
+                                         0x00, 0x02, 0x5f, 0x20, 0x2f, 0x02,
+                                         0xc0, 0xa8, 0x00, 0x02,
+                                         0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+                                         0xc0, 0xa8, 0x00, 0x02};
+
+    func_enter();
+
+    csrResult = unifi_net_data_malloc(priv, &bulkdata.d[0], sizeof(arp_req));
+    if (csrResult != CSR_RESULT_SUCCESS)
+    {
+        unifi_error(priv, "Failed to allocate bulk data in CsrWifiSmeRoamCompleteIndHandler()\n");
+        return;
+    }
+    skb = (struct sk_buff *)(bulkdata.d[0].os_net_buf_ptr);
+    skb->len = bulkdata.d[0].data_length;
+
+    memcpy(skb->data, arp_req, sizeof(arp_req));
+    /* add MAC and IP address */
+    memcpy(skb->data + 16, priv->netdev[interfaceTag]->dev_addr, ETH_ALEN);
+    skb->data[22] = (priv->sta_ip_address      ) & 0xFF;
+    skb->data[23] = (priv->sta_ip_address >>  8) & 0xFF;
+    skb->data[24] = (priv->sta_ip_address >> 16) & 0xFF;
+    skb->data[25] = (priv->sta_ip_address >> 24) & 0xFF;
+    skb->data[32] = (priv->sta_ip_address      ) & 0xFF;
+    skb->data[33] = (priv->sta_ip_address >>  8) & 0xFF;
+    skb->data[34] = (priv->sta_ip_address >> 16) & 0xFF;
+    skb->data[35] = (priv->sta_ip_address >> 24) & 0xFF;
+
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].os_net_buf_ptr = NULL;
+    bulkdata.d[1].net_buf_length = bulkdata.d[1].data_length = 0;
+
+    if ((protection = uf_get_protection_bit_from_interfacemode(priv, interfaceTag, &arp_req[26])) < 0)
+    {
+        unifi_error(priv, "CsrWifiSmeRoamCompleteIndHandler: Failed to determine protection mode\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return;
+    }
+
+    if ((priv->sta_wmm_capabilities & QOS_CAPABILITY_WMM_ENABLED) == 1)
+    {
+        priority = CSR_QOS_UP0;
+    }
+    else
+    {
+        priority = CSR_CONTENTION;
+    }
+
+    if (prepare_and_add_macheader(priv, skb, newSkb, priority, &bulkdata,
+                                  interfaceTag, &arp_req[26],
+                                  priv->netdev[interfaceTag]->dev_addr, protection))
+    {
+        unifi_error(priv, "CsrWifiSmeRoamCompleteIndHandler: failed to create MAC header\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return;
+    }
+    bulkdata.d[0].os_data_ptr = skb->data;
+    bulkdata.d[0].os_net_buf_ptr = skb;
+    bulkdata.d[0].data_length = skb->len;
+
+    unifi_frame_ma_packet_req(priv, priority, 0, 0xffffffff, interfaceTag,
+                              CSR_NO_CONFIRM_REQUIRED, priv->netdev_client->sender_id,
+                              interfacePriv->bssid.a, &signal);
+
+    r = ul_send_signal_unpacked(priv, &signal, &bulkdata);
+    if (r)
+    {
+        unifi_error(priv, "CsrWifiSmeRoamCompleteIndHandler: failed to send QOS data null packet result: %d\n",r);
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return;
+    }
+
+    func_exit();
+
+}
+#endif /* CSR_WIFI_SEND_GRATUITOUS_ARP */
+
+/*
+ * ---------------------------------------------------------------------------
+ * configure_data_port
+ *
+ *      Store the new controlled port configuration.
+ *
+ * Arguments:
+ *      priv            Pointer to device private context struct
+ *      port_cfg        Pointer to the port configuration
+ *
+ * Returns:
+ *      An unifi_ControlledPortAction value.
+ * ---------------------------------------------------------------------------
+ */
+static int
+configure_data_port(unifi_priv_t *priv,
+        CsrWifiRouterCtrlPortAction port_action,
+        const CsrWifiMacAddress *macAddress,
+        const int queue,
+        u16 interfaceTag)
+{
+    const u8 broadcast_mac_address[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    unifi_port_config_t *port;
+    netInterface_priv_t *interfacePriv;
+    int i;
+    const char* controlled_string; /* cosmetic "controlled"/"uncontrolled" for trace */
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "configure_data_port: bad interfaceTag\n");
+        return -EFAULT;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (queue == UF_CONTROLLED_PORT_Q) {
+        port = &interfacePriv->controlled_data_port;
+        controlled_string = "controlled";
+    } else {
+        port = &interfacePriv->uncontrolled_data_port;
+        controlled_string = "uncontrolled";
+    }
+
+	unifi_trace(priv, UDBG2,
+		"port config request %pM %s with port_action %d.\n",
+		macAddress->a, controlled_string, port_action);
+
+    /* If the new configuration has the broadcast MAC address or if we are in infrastructure mode then clear the list first and set port overide mode */
+    if ((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode ||
+        interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI) ||
+        !memcmp(macAddress->a, broadcast_mac_address, ETH_ALEN)) {
+
+        port->port_cfg[0].port_action = port_action;
+        port->port_cfg[0].mac_address = *macAddress;
+        port->port_cfg[0].in_use = TRUE;
+        port->entries_in_use = 1;
+        port->overide_action = UF_DATA_PORT_OVERIDE;
+
+        unifi_trace(priv, UDBG2, "%s port override on\n",
+                    (queue == UF_CONTROLLED_PORT_Q) ? "Controlled" : "Uncontrolled");
+
+        /* Discard the remaining entries in the port config table */
+        for (i = 1; i < UNIFI_MAX_CONNECTIONS; i++) {
+            port->port_cfg[i].in_use = FALSE;
+        }
+
+        if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
+            unifi_trace(priv, UDBG1, "%s port broadcast set to open.\n",
+                        (queue == UF_CONTROLLED_PORT_Q) ? "Controlled" : "Uncontrolled");
+
+            /*
+             * Ask stack to schedule for transmission any packets queued
+             * while controlled port was not open.
+             * Use netif_schedule() instead of netif_wake_queue() because
+             * transmission should be already enabled at this point. If it
+             * is not, probably the interface is down and should remain as is.
+             */
+            uf_resume_data_plane(priv, queue, *macAddress, interfaceTag);
+
+#ifdef CSR_WIFI_SEND_GRATUITOUS_ARP
+            if ((CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) &&
+                (queue == UF_CONTROLLED_PORT_Q) && (priv->sta_ip_address != 0xFFFFFFFF))
+            {
+                uf_send_gratuitous_arp(priv, interfaceTag);
+            }
+#endif
+        } else {
+            unifi_trace(priv, UDBG1, "%s port broadcast set to %s.\n",
+                        (queue == UF_CONTROLLED_PORT_Q) ? "Controlled" : "Uncontrolled",
+                        (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD) ? "discard": "closed");
+
+            /* If port is closed, discard all the pending Rx packets */
+            if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD) {
+                uf_free_pending_rx_packets(priv, queue, *macAddress,interfaceTag);
+            }
+        }
+    } else {
+        /* store the new configuration, either in the entry with matching mac address (if already present),
+         * otherwise in a new entry
+         */
+
+        int found_entry_flag;
+        int first_free_slot = -1;
+
+        /* If leaving override mode, free the port entry used for override */
+        if (port->overide_action == UF_DATA_PORT_OVERIDE) {
+            port->port_cfg[0].in_use = FALSE;
+            port->entries_in_use = 0;
+            port->overide_action = UF_DATA_PORT_NOT_OVERIDE;
+
+            unifi_trace(priv, UDBG2, "%s port override off\n",
+                        (queue == UF_CONTROLLED_PORT_Q) ? "Controlled" : "Uncontrolled");
+        }
+
+        found_entry_flag = 0;
+        for (i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+            if (port->port_cfg[i].in_use) {
+                if (!memcmp(&port->port_cfg[i].mac_address.a, macAddress->a, ETH_ALEN)) {
+                    /* We've seen this address before, reconfigure it */
+                    port->port_cfg[i].port_action = port_action;
+                    found_entry_flag = 1;
+                    break;
+                }
+            } else if (first_free_slot == -1) {
+                /* Remember the first free slot on the way past so it can be claimed
+                 * if this turns out to be a new MAC address (to save walking the list again).
+                 */
+                first_free_slot = i;
+            }
+        }
+
+        /* At this point we found an existing entry and have updated it, or need to
+         * add a new entry. If all slots are allocated, give up and return an error.
+         */
+        if (!found_entry_flag) {
+            if (first_free_slot == -1) {
+                unifi_error(priv, "no free slot found in port config array (%d used)\n", port->entries_in_use);
+                return -EFAULT;
+            } else {
+                port->entries_in_use++;
+            }
+
+            unifi_trace(priv, UDBG3, "port config index assigned in config_data_port = %d\n", first_free_slot);
+            port->port_cfg[first_free_slot].in_use = TRUE;
+            port->port_cfg[first_free_slot].port_action = port_action;
+            port->port_cfg[first_free_slot].mac_address = *macAddress;
+        }
+
+        if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
+            /*
+             * Ask stack to schedule for transmission any packets queued
+             * while controlled port was not open.
+             * Use netif_schedule() instead of netif_wake_queue() because
+             * transmission should be already enabled at this point. If it
+             * is not, probably the interface is down and should remain as is.
+             */
+            uf_resume_data_plane(priv, queue, *macAddress, interfaceTag);
+        }
+
+        /*
+         * If port is closed, discard all the pending Rx packets
+         * coming from the peer station.
+         */
+        if (port_action == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD) {
+            uf_free_pending_rx_packets(priv, queue, *macAddress,interfaceTag);
+        }
+
+	unifi_trace(priv, UDBG2,
+		"port config %pM with port_action %d.\n",
+		macAddress->a, port_action);
+    }
+    return 0;
+} /* configure_data_port() */
+
+
+void CsrWifiRouterCtrlPortConfigureReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlPortConfigureReq* req = (CsrWifiRouterCtrlPortConfigureReq*)msg;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    unifi_trace(priv, UDBG3, "entering CsrWifiRouterCtrlPortConfigureReqHandler\n");
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlPortConfigureReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    /* To update the protection status of the peer/station */
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+	    case CSR_WIFI_ROUTER_CTRL_MODE_AMP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+            /* Since for Unifi as a station, the station record not maintained & interfaceID is
+             * only needed to update the peer protection status
+             */
+            interfacePriv->protect = req->setProtection;
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            {
+                u8 i;
+                CsrWifiRouterCtrlStaInfo_t *staRecord;
+                /* Ifscontrolled port is open means, The peer has been added to station record
+                 * so that the protection corresponding to the peer is valid in this req
+                 */
+                if (req->controlledPortAction == CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN) {
+                    for(i =0; i < UNIFI_MAX_CONNECTIONS; i++) {
+                        staRecord = (CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[i]);
+                        if (staRecord) {
+                                /* Find the matching station record & set the protection type */
+                                if (!memcmp(req->macAddress.a, staRecord->peerMacAddress.a, ETH_ALEN)) {
+                                        staRecord->protection = req->setProtection;
+                                        break;
+                                }
+                        }
+                    }
+                }
+            }
+            break;
+        default:
+            unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlPortConfigureReqHandler(0x%.4X) Uncaught mode %d\n",
+                        msg->source, interfacePriv->interfaceMode);
+    }
+
+    configure_data_port(priv, req->uncontrolledPortAction, (const CsrWifiMacAddress *)&req->macAddress,
+                        UF_UNCONTROLLED_PORT_Q, req->interfaceTag);
+    configure_data_port(priv, req->controlledPortAction, (const CsrWifiMacAddress *)&req->macAddress,
+                        UF_CONTROLLED_PORT_Q, req->interfaceTag);
+
+    CsrWifiRouterCtrlPortConfigureCfmSend(msg->source,req->clientData,req->interfaceTag,
+                                      CSR_RESULT_SUCCESS, req->macAddress);
+    unifi_trace(priv, UDBG3, "leaving CsrWifiRouterCtrlPortConfigureReqHandler\n");
+}
+
+
+void CsrWifiRouterCtrlWifiOnReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlVersions versions;
+    CsrWifiRouterCtrlWifiOnReq* req = (CsrWifiRouterCtrlWifiOnReq*)msg;
+    int r,i;
+    CsrResult csrResult;
+
+    if (priv == NULL) {
+        return;
+    }
+    if( priv->wol_suspend ) {
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWifiOnReqHandler: Don't reset mode\n");
+    } else {
+#ifdef ANDROID_BUILD
+        /* Take the wakelock while Wi-Fi On is in progress */
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWifiOnReqHandler: take wake lock\n");
+        wake_lock(&unifi_sdio_wake_lock);
+#endif
+        for (i=0; i<CSR_WIFI_NUM_INTERFACES; i++) {
+            unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWifiOnReqHandler: Setting interface %d to NONE\n", i );
+
+            priv->interfacePriv[i]->interfaceMode = 0;
+        }
+    }
+    unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWifiOnReqHandler(0x%.4X) req->dataLength=%d req->data=0x%x\n", msg->source, req->dataLength, req->data);
+
+    if(req->dataLength==3 && req->data && req->data[0]==0 && req->data[1]==1 && req->data[2]==1)
+    {
+        priv->cmanrTestMode = TRUE;
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWifiOnReqHandler: cmanrTestMode=%d\n", priv->cmanrTestMode);
+    }
+    else
+    {
+        priv->cmanrTestMode = FALSE;
+    }
+
+    /*
+     * The request to initialise UniFi might come while UniFi is running.
+     * We need to block all I/O activity until the reset completes, otherwise
+     * an SDIO error might occur resulting an indication to the SME which
+     * makes it think that the initialisation has failed.
+     */
+    priv->bh_thread.block_thread = 1;
+
+    /* Update the wifi_on state */
+    priv->wifi_on_state = wifi_on_in_progress;
+
+    /* If UniFi was unpowered, acquire the firmware for download to chip */
+    if (!priv->wol_suspend) {
+        r = uf_request_firmware_files(priv, UNIFI_FW_STA);
+        if (r) {
+            unifi_error(priv, "CsrWifiRouterCtrlWifiOnReqHandler: Failed to get f/w\n");
+            CsrWifiRouterCtrlWifiOnCfmSend(msg->source, req->clientData, CSR_RESULT_FAILURE);
+            return;
+        }
+    } else {
+        unifi_trace(priv, UDBG1, "Don't need firmware\n");
+    }
+
+    /* Power on UniFi (which may not necessarily have been off) */
+    CsrSdioClaim(priv->sdio);
+    csrResult = CsrSdioPowerOn(priv->sdio);
+    CsrSdioRelease(priv->sdio);
+    if (csrResult != CSR_RESULT_SUCCESS && csrResult != CSR_SDIO_RESULT_NOT_RESET) {
+        unifi_error(priv, "CsrWifiRouterCtrlWifiOnReqHandler: Failed to power on UniFi\n");
+        CsrWifiRouterCtrlWifiOnCfmSend(msg->source, req->clientData, CSR_RESULT_FAILURE);
+        return;
+    }
+
+    /* If CsrSdioPowerOn() returns CSR_RESULT_SUCCESS, it means that we need to initialise UniFi */
+    if (csrResult == CSR_RESULT_SUCCESS && !priv->wol_suspend) {
+        /* Initialise UniFi hardware */
+        r = uf_init_hw(priv);
+        if (r) {
+            unifi_error(priv, "CsrWifiRouterCtrlWifiOnReqHandler: Failed to initialise h/w, error %d\n", r);
+            CsrWifiRouterCtrlWifiOnCfmSend(msg->source, req->clientData, CSR_RESULT_FAILURE);
+            return;
+        }
+    } else {
+        unifi_trace(priv, UDBG1, "UniFi already initialised\n");
+    }
+
+    /* Completed handling of wake up from suspend with UniFi powered */
+    priv->wol_suspend = FALSE;
+
+    /* Re-enable the I/O thread */
+    priv->bh_thread.block_thread = 0;
+
+    /*
+     * Start the I/O thread. The thread might be already running.
+     * This fine, just carry on with the request.
+     */
+    r = uf_init_bh(priv);
+    if (r) {
+        CsrSdioClaim(priv->sdio);
+        CsrSdioPowerOff(priv->sdio);
+        CsrSdioRelease(priv->sdio);
+        CsrWifiRouterCtrlWifiOnCfmSend(msg->source, req->clientData, CSR_RESULT_FAILURE);
+        return;
+    }
+
+    /* Get the version information from the core */
+    unifi_card_info(priv->card, &priv->card_info);
+
+    /* Set the sme queue id */
+    priv->CSR_WIFI_SME_IFACEQUEUE = msg->source;
+    CSR_WIFI_SME_IFACEQUEUE = msg->source;
+
+
+    /* Copy to the unifiio_card_info structure. */
+    versions.chipId = priv->card_info.chip_id;
+    versions.chipVersion = priv->card_info.chip_version;
+    versions.firmwareBuild = priv->card_info.fw_build;
+    versions.firmwareHip = priv->card_info.fw_hip_version;
+    versions.routerBuild = (char*)CSR_WIFI_VERSION;
+    versions.routerHip = (UNIFI_HIP_MAJOR_VERSION << 8) | UNIFI_HIP_MINOR_VERSION;
+
+    CsrWifiRouterCtrlWifiOnIndSend(msg->source, 0, CSR_RESULT_SUCCESS, versions);
+
+    /* Update the wifi_on state */
+    priv->wifi_on_state = wifi_on_done;
+}
+
+
+/*
+ * wifi_off:
+ *      Common code for CsrWifiRouterCtrlWifiOffReqHandler() and
+ *      CsrWifiRouterCtrlWifiOffRspHandler().
+ */
+static void
+wifi_off(unifi_priv_t *priv)
+{
+    int power_off;
+    int priv_instance;
+    int i;
+    CsrResult csrResult;
+
+
+    /* Already off? */
+    if (priv->wifi_on_state == wifi_on_unspecified) {
+        unifi_trace(priv, UDBG1, "wifi_off already\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG1, "wifi_off\n");
+
+    /* Destroy the Traffic Analysis Module */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,23)
+    cancel_work_sync(&priv->ta_ind_work.task);
+    cancel_work_sync(&priv->ta_sample_ind_work.task);
+#ifdef CSR_SUPPORT_WEXT
+    cancel_work_sync(&priv->sme_config_task);
+    wext_send_disassoc_event(priv);
+#endif
+
+    /* Cancel pending M4 stuff */
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        if (priv->netdev[i]) {
+            netInterface_priv_t *netpriv = (netInterface_priv_t *) netdev_priv(priv->netdev[i]);
+            cancel_work_sync(&netpriv->send_m4_ready_task);
+        }
+    }
+#endif
+    flush_workqueue(priv->unifi_workqueue);
+
+    /* fw_init parameter can prevent power off UniFi, for debugging */
+    priv_instance = uf_find_priv(priv);
+    if (priv_instance == -1) {
+        unifi_warning(priv,
+                "CsrWifiRouterCtrlStopReqHandler: Unknown priv instance, will power off card.\n");
+        power_off = 1;
+    } else {
+        power_off = (fw_init[priv_instance] > 0) ? 0 : 1;
+    }
+
+    /* Production test mode requires power to the chip, too */
+    if (priv->ptest_mode) {
+        power_off = 0;
+    }
+
+    /* Stop the bh_thread */
+    uf_stop_thread(priv, &priv->bh_thread);
+
+    /* Read the f/w panic codes, if any. Protect against second wifi_off() call,
+     * which may happen if SME requests a wifi_off and closes the char device */
+    if (priv->init_progress != UNIFI_INIT_NONE) {
+        CsrSdioClaim(priv->sdio);
+        unifi_capture_panic(priv->card);
+        CsrSdioRelease(priv->sdio);
+    }
+
+    /* Unregister the interrupt handler */
+    if (csr_sdio_linux_remove_irq(priv->sdio)) {
+        unifi_notice(priv,
+                "csr_sdio_linux_remove_irq failed to talk to card.\n");
+    }
+
+    if (power_off) {
+        unifi_trace(priv, UDBG2,
+                    "Force low power and try to power off\n");
+        /* Put UniFi to deep sleep, in case we can not power it off */
+        CsrSdioClaim(priv->sdio);
+        csrResult = unifi_force_low_power_mode(priv->card);
+        CsrSdioRelease(priv->sdio);
+
+        CsrSdioPowerOff(priv->sdio);
+    }
+
+    /* Consider UniFi to be uninitialised */
+    priv->init_progress = UNIFI_INIT_NONE;
+    priv->wifi_on_state = wifi_on_unspecified;
+
+
+} /* wifi_off() */
+
+
+void CsrWifiRouterCtrlWifiOffReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlWifiOffReq* req = (CsrWifiRouterCtrlWifiOffReq*)msg;
+    int i = 0;
+
+    if (priv == NULL) {
+        return;
+    }
+
+    unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWifiOffReqHandler(0x%.4X)\n", msg->source);
+
+    /* Stop the network traffic on all interfaces before freeing the core. */
+    for (i=0; i<CSR_WIFI_NUM_INTERFACES; i++) {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+        if (interfacePriv->netdev_registered == 1) {
+            netif_carrier_off(priv->netdev[i]);
+            UF_NETIF_TX_STOP_ALL_QUEUES(priv->netdev[i]);
+            interfacePriv->connected = UnifiConnectedUnknown;
+        }
+        interfacePriv->interfaceMode = 0;
+
+        /* Enable all queues by default */
+        interfacePriv->queueEnabled[0] = 1;
+        interfacePriv->queueEnabled[1] = 1;
+        interfacePriv->queueEnabled[2] = 1;
+        interfacePriv->queueEnabled[3] = 1;
+    }
+    wifi_off(priv);
+
+    CsrWifiRouterCtrlWifiOffCfmSend(msg->source,req->clientData);
+
+    /* If this is called in response to closing the character device, the
+     * caller must use uf_sme_cancel_request() to terminate any pending SME
+     * blocking request or there will be a delay while the operation times out.
+     */
+}
+
+
+void CsrWifiRouterCtrlQosControlReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlQosControlReq* req = (CsrWifiRouterCtrlQosControlReq*)msg;
+    netInterface_priv_t *interfacePriv;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlQosControlReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG4, "CsrWifiRouterCtrlQosControlReqHandler:scontrol = %d", req->control);
+
+    if (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "CsrWifiRouterCtrlQosControlReqHandler: interfaceID >= CSR_WIFI_NUM_INTERFACES.\n");
+        return;
+    }
+    interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    if (req->control == CSR_WIFI_ROUTER_CTRL_QOS_CONTROL_WMM_ON) {
+        priv->sta_wmm_capabilities |= QOS_CAPABILITY_WMM_ENABLED;
+        unifi_trace(priv, UDBG1, "WMM enabled\n");
+
+        unifi_trace(priv, UDBG1, "Queue Config %x\n", req->queueConfig);
+
+        interfacePriv->queueEnabled[UNIFI_TRAFFIC_Q_BK] = (req->queueConfig & CSR_WIFI_ROUTER_CTRL_QUEUE_BK_ENABLE)?1:0;
+        interfacePriv->queueEnabled[UNIFI_TRAFFIC_Q_BE] = (req->queueConfig & CSR_WIFI_ROUTER_CTRL_QUEUE_BE_ENABLE)?1:0;
+        interfacePriv->queueEnabled[UNIFI_TRAFFIC_Q_VI] = (req->queueConfig & CSR_WIFI_ROUTER_CTRL_QUEUE_VI_ENABLE)?1:0;
+        interfacePriv->queueEnabled[UNIFI_TRAFFIC_Q_VO] = (req->queueConfig & CSR_WIFI_ROUTER_CTRL_QUEUE_VO_ENABLE)?1:0;
+
+    } else {
+        priv->sta_wmm_capabilities = 0;
+        unifi_trace(priv, UDBG1, "WMM disabled\n");
+    }
+}
+
+
+void CsrWifiRouterCtrlTclasAddReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlTclasAddReq* req = (CsrWifiRouterCtrlTclasAddReq*)msg;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlTclasAddReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    CsrWifiRouterCtrlTclasAddCfmSend(msg->source, req->clientData, req->interfaceTag , CSR_RESULT_SUCCESS);
+}
+
+void CsrWifiRouterCtrlTclasDelReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlTclasDelReq* req = (CsrWifiRouterCtrlTclasDelReq*)msg;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlTclasDelReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    CsrWifiRouterCtrlTclasDelCfmSend(msg->source, req->clientData, req->interfaceTag, CSR_RESULT_SUCCESS);
+}
+
+
+void CsrWifiRouterCtrlConfigurePowerModeReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlConfigurePowerModeReq* req = (CsrWifiRouterCtrlConfigurePowerModeReq*)msg;
+    enum unifi_low_power_mode pm;
+    CsrResult csrResult;
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlConfigurePowerModeReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    if (req->mode == CSR_WIFI_ROUTER_CTRL_LOW_POWER_MODE_DISABLED) {
+        pm = UNIFI_LOW_POWER_DISABLED;
+    } else {
+        pm = UNIFI_LOW_POWER_ENABLED;
+    }
+
+    unifi_trace(priv, UDBG2,
+                "CsrWifiRouterCtrlConfigurePowerModeReqHandler (mode=%d, wake=%d)\n",
+                req->mode, req->wakeHost);
+    csrResult = unifi_configure_low_power_mode(priv->card, pm,
+                                               (req->wakeHost ? UNIFI_PERIODIC_WAKE_HOST_ENABLED : UNIFI_PERIODIC_WAKE_HOST_DISABLED));
+}
+
+
+void CsrWifiRouterCtrlWifiOnResHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlWifiOnRes* res = (CsrWifiRouterCtrlWifiOnRes*)msg;
+
+    if (priv == NULL) {
+        unifi_error(NULL, "CsrWifiRouterCtrlWifiOnResHandler: Invalid ospriv.\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG1,
+                "CsrWifiRouterCtrlWifiOnResHandler: status %d (patch %u)\n", res->status, res->smeVersions.firmwarePatch);
+
+    if (res->smeVersions.firmwarePatch != 0) {
+        unifi_info(priv, "Firmware patch %d\n", res->smeVersions.firmwarePatch);
+    }
+
+    if (res->numInterfaceAddress > CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "WifiOnResHandler bad numInterfaceAddress %d\n", res->numInterfaceAddress);
+        return;
+    }
+
+    /* UniFi is now initialised, complete the init. */
+    if (res->status == CSR_RESULT_SUCCESS)
+    {
+        int i; /* used as a loop counter */
+        u32 intmode = CSR_WIFI_INTMODE_DEFAULT;
+#ifdef CSR_WIFI_SPLIT_PATCH
+        u8 switching_ap_fw = FALSE;
+#endif
+        /* Register the UniFi device with the OS network manager */
+        unifi_trace(priv, UDBG3, "Card Init Completed Successfully\n");
+
+        /* Store the MAC address in the netdev */
+        for(i=0;i<res->numInterfaceAddress;i++)
+        {
+            memcpy(priv->netdev[i]->dev_addr, res->stationMacAddress[i].a, ETH_ALEN);
+        }
+
+        /* Copy version structure into the private versions field */
+        priv->sme_versions = res->smeVersions;
+
+        unifi_trace(priv, UDBG2, "network interfaces count = %d\n",
+                    res->numInterfaceAddress);
+
+        /* Register the netdevs for each interface. */
+        for(i=0;i<res->numInterfaceAddress;i++)
+        {
+            netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+            if(!interfacePriv->netdev_registered)
+            {
+                int r;
+                unifi_trace(priv, UDBG3, "registering net device %d\n", i);
+                r = uf_register_netdev(priv, i);
+                if (r)
+                {
+                    /* unregister the net_device that are registered in the previous iterations */
+                    uf_unregister_netdev(priv);
+                    unifi_error(priv, "Failed to register the network device.\n");
+                    CsrWifiRouterCtrlWifiOnCfmSend(msg->source, res->clientData, CSR_RESULT_FAILURE);
+                    return;
+                }
+            }
+#ifdef CSR_WIFI_SPLIT_PATCH
+            else
+            {
+                /* If a netdev is already registered, we have received this WifiOnRes
+                 * in response to switching AP/STA firmware in a ModeSetReq.
+                 * Rememeber this in order to send a ModeSetCfm once
+                 */
+                switching_ap_fw = TRUE;
+            }
+#endif
+        }
+        priv->totalInterfaceCount = res->numInterfaceAddress;
+
+        /* If the MIB has selected f/w scheduled interrupt mode, apply it now
+         * but let module param override.
+         */
+        if (run_bh_once != -1) {
+            intmode = (u32)run_bh_once;
+        } else if (res->scheduledInterrupt) {
+            intmode = CSR_WIFI_INTMODE_RUN_BH_ONCE;
+        }
+        unifi_set_interrupt_mode(priv->card, intmode);
+
+        priv->init_progress = UNIFI_INIT_COMPLETED;
+
+        /* Acknowledge the CsrWifiRouterCtrlWifiOnReq now */
+        CsrWifiRouterCtrlWifiOnCfmSend(msg->source, res->clientData, CSR_RESULT_SUCCESS);
+
+#ifdef CSR_WIFI_SPLIT_PATCH
+        if (switching_ap_fw && (priv->pending_mode_set.common.destination != 0xaaaa)) {
+            unifi_info(priv, "Completed firmware reload with %s patch\n",
+                CSR_WIFI_HIP_IS_AP_FW(priv->interfacePriv[0]->interfaceMode) ? "AP" : "STA");
+
+            /* Confirm the ModeSetReq that requested the AP/STA patch switch */
+            CsrWifiRouterCtrlModeSetCfmSend(priv->pending_mode_set.common.source,
+                                            priv->pending_mode_set.clientData,
+                                            priv->pending_mode_set.interfaceTag,
+                                            priv->pending_mode_set.mode,
+                                            CSR_RESULT_SUCCESS);
+            priv->pending_mode_set.common.destination = 0xaaaa;
+        }
+#endif
+        unifi_info(priv, "UniFi ready\n");
+
+#ifdef ANDROID_BUILD
+        /* Release the wakelock */
+        unifi_trace(priv, UDBG1, "ready: release wake lock\n");
+        wake_unlock(&unifi_sdio_wake_lock);
+#endif
+        /* Firmware initialisation is complete, so let the SDIO bus
+         * clock be raised when convienent to the core.
+         */
+        unifi_request_max_sdio_clock(priv->card);
+
+#ifdef CSR_SUPPORT_WEXT
+        /* Notify the Android wpa_supplicant that we are ready */
+        wext_send_started_event(priv);
+
+        queue_work(priv->unifi_workqueue, &priv->sme_config_task);
+#endif
+
+    } else {
+        /* Acknowledge the CsrWifiRouterCtrlWifiOnReq now */
+        CsrWifiRouterCtrlWifiOnCfmSend(msg->source, res->clientData, CSR_RESULT_FAILURE);
+    }
+}
+
+
+void CsrWifiRouterCtrlWifiOffResHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+
+void CsrWifiRouterCtrlMulticastAddressResHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+
+void CsrWifiRouterMaPacketSubscribeReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterMaPacketSubscribeReq* req = (CsrWifiRouterMaPacketSubscribeReq*)msg;
+    u8 i;
+    CsrResult result;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterMaPacketSubscribeReqHandler: invalid priv\n");
+        return;
+    }
+
+    /* Look for an unused filter */
+
+    result = CSR_WIFI_RESULT_NO_ROOM;
+    for (i = 0; i < MAX_MA_UNIDATA_IND_FILTERS; i++) {
+
+        if (!priv->sme_unidata_ind_filters[i].in_use) {
+
+            priv->sme_unidata_ind_filters[i].in_use = 1;
+            priv->sme_unidata_ind_filters[i].appHandle = msg->source;
+            priv->sme_unidata_ind_filters[i].encapsulation = req->encapsulation;
+            priv->sme_unidata_ind_filters[i].protocol = req->protocol;
+
+            priv->sme_unidata_ind_filters[i].oui[2] = (u8)  (req->oui        & 0xFF);
+            priv->sme_unidata_ind_filters[i].oui[1] = (u8) ((req->oui >>  8) & 0xFF);
+            priv->sme_unidata_ind_filters[i].oui[0] = (u8) ((req->oui >> 16) & 0xFF);
+
+            result = CSR_RESULT_SUCCESS;
+            break;
+        }
+    }
+
+    unifi_trace(priv, UDBG1,
+                "subscribe_req: encap=%d, handle=%d, result=%d\n",
+                req->encapsulation, i, result);
+    CsrWifiRouterMaPacketSubscribeCfmSend(msg->source,req->interfaceTag, i, result, 0);
+}
+
+
+void CsrWifiRouterMaPacketUnsubscribeReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterMaPacketUnsubscribeReq* req = (CsrWifiRouterMaPacketUnsubscribeReq*)msg;
+    CsrResult result;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterMaPacketUnsubscribeReqHandler: invalid priv\n");
+        return;
+    }
+
+    result = CSR_WIFI_RESULT_NOT_FOUND;
+
+    if (req->subscriptionHandle < MAX_MA_UNIDATA_IND_FILTERS) {
+        if (priv->sme_unidata_ind_filters[req->subscriptionHandle].in_use) {
+            priv->sme_unidata_ind_filters[req->subscriptionHandle].in_use = 0;
+            result = CSR_RESULT_SUCCESS;
+        } else {
+            result = CSR_WIFI_RESULT_NOT_FOUND;
+        }
+    }
+
+    unifi_trace(priv, UDBG1,
+                "unsubscribe_req: handle=%d, result=%d\n",
+                req->subscriptionHandle, result);
+    CsrWifiRouterMaPacketUnsubscribeCfmSend(msg->source,req->interfaceTag, result);
+}
+
+
+void CsrWifiRouterCtrlCapabilitiesReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlCapabilitiesReq* req = (CsrWifiRouterCtrlCapabilitiesReq*)msg;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlCapabilitiesReqHandler: invalid priv\n");
+        return;
+    }
+
+    CsrWifiRouterCtrlCapabilitiesCfmSend(msg->source,req->clientData,
+            UNIFI_SOFT_COMMAND_Q_LENGTH - 1,
+            UNIFI_SOFT_TRAFFIC_Q_LENGTH - 1);
+}
+
+
+void CsrWifiRouterCtrlSuspendResHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlSuspendRes* res = (CsrWifiRouterCtrlSuspendRes*)msg;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlSuspendResHandler: invalid priv\n");
+        return;
+    }
+
+    sme_complete_request(priv, res->status);
+}
+
+
+void CsrWifiRouterCtrlResumeResHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlResumeRes* res = (CsrWifiRouterCtrlResumeRes*)msg;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlResumeResHandler: invalid priv\n");
+        return;
+    }
+
+    sme_complete_request(priv, res->status);
+}
+
+
+void CsrWifiRouterCtrlTrafficConfigReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlTrafficConfigReq* req = (CsrWifiRouterCtrlTrafficConfigReq*)msg;
+    CsrResult csrResult;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlTrafficConfigReqHandler: invalid smepriv\n");
+        return;
+    }
+    if (req->trafficConfigType == CSR_WIFI_ROUTER_CTRL_TRAFFIC_CONFIG_TYPE_FILTER)
+    {
+        req->config.packetFilter |= CSR_WIFI_ROUTER_CTRL_TRAFFIC_PACKET_TYPE_CUSTOM;
+    }
+    csrResult = unifi_ta_configure(priv->card, req->trafficConfigType, (const CsrWifiRouterCtrlTrafficConfig *)&req->config);
+}
+
+void CsrWifiRouterCtrlTrafficClassificationReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlTrafficClassificationReq* req = (CsrWifiRouterCtrlTrafficClassificationReq*)msg;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlTrafficClassificationReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    unifi_ta_classification(priv->card, req->trafficType, req->period);
+}
+
+static int
+_sys_packet_req(unifi_priv_t *priv, const CSR_SIGNAL *signal,
+        u8 subscriptionHandle,
+        u16 frameLength, u8 *frame,
+        int proto)
+{
+    int r;
+    const sme_ma_unidata_ind_filter_t *subs;
+    bulk_data_param_t bulkdata;
+    CSR_MA_PACKET_REQUEST req = signal->u.MaPacketRequest;
+    struct sk_buff *skb, *newSkb = NULL;
+    CsrWifiMacAddress peerMacAddress;
+    CsrResult csrResult;
+    u16 interfaceTag = req.VirtualInterfaceIdentifier & 0xff;
+    u8 eapolStore = FALSE;
+    s8 protection = 0;
+    netInterface_priv_t *interfacePriv;
+    unsigned long flags;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "_sys_packet_req: interfaceID >= CSR_WIFI_NUM_INTERFACES.\n");
+        return -EINVAL;
+    }
+    interfacePriv = priv->interfacePriv[interfaceTag];
+    if (!priv->sme_unidata_ind_filters[subscriptionHandle].in_use) {
+        unifi_error(priv, "_sys_packet_req: unknown subscription.\n");
+        return -EINVAL;
+    }
+
+    subs = &priv->sme_unidata_ind_filters[subscriptionHandle];
+    unifi_trace(priv, UDBG1,
+                "_sys_packet_req: handle=%d, subs=%p, encap=%d\n",
+                subscriptionHandle, subs, subs->encapsulation);
+
+    csrResult = unifi_net_data_malloc(priv, &bulkdata.d[0], frameLength);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "_sys_packet_req: failed to allocate bulkdata.\n");
+        return (int)CsrHipResultToStatus(csrResult);
+    }
+
+    /* get the peer Mac address */
+    memcpy(&peerMacAddress, frame, ETH_ALEN);
+
+    /* Determine if we need to add encapsulation header */
+    if (subs->encapsulation == CSR_WIFI_ROUTER_ENCAPSULATION_ETHERNET) {
+        memcpy((void*)bulkdata.d[0].os_data_ptr, frame, frameLength);
+
+        /* The translation is performed on the skb */
+        skb = (struct sk_buff*)bulkdata.d[0].os_net_buf_ptr;
+
+        unifi_trace(priv, UDBG1,
+                    "_sys_packet_req: skb_add_llc_snap -->\n");
+        r = skb_add_llc_snap(priv->netdev[interfaceTag], skb, proto);
+        unifi_trace(priv, UDBG1,
+                    "_sys_packet_req: skb_add_llc_snap <--\n");
+        if (r) {
+            unifi_error(priv,
+                        "_sys_packet_req: failed to translate eth frame.\n");
+            unifi_net_data_free(priv,&bulkdata.d[0]);
+            return r;
+        }
+
+        bulkdata.d[0].data_length = skb->len;
+    } else {
+        /* Crop the MAC addresses from the packet */
+        memcpy((void*)bulkdata.d[0].os_data_ptr, frame + 2*ETH_ALEN, frameLength - 2*ETH_ALEN);
+        bulkdata.d[0].data_length = frameLength - 2*ETH_ALEN;
+        skb = (struct sk_buff*)bulkdata.d[0].os_net_buf_ptr;
+        skb->len = bulkdata.d[0].data_length;
+
+    }
+
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].os_net_buf_ptr = NULL;
+    bulkdata.d[1].data_length = 0;
+
+    /* check for m4 detection */
+    if (0 == uf_verify_m4(priv, bulkdata.d[0].os_data_ptr, bulkdata.d[0].data_length)) {
+        eapolStore = TRUE;
+    }
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    if (proto == ETH_P_WAI)
+     {
+        protection = 0; /*WAI packets always sent unencrypted*/
+     }
+   else
+     {
+#endif
+
+#ifdef CSR_SUPPORT_SME
+    if ((protection = uf_get_protection_bit_from_interfacemode(priv, interfaceTag, peerMacAddress.a)) < 0) {
+        unifi_error(priv, "unicast address, but destination not in station record database\n");
+        unifi_net_data_free(priv,&bulkdata.d[0]);
+        return -1;
+    }
+#else
+    protection = 0;
+#endif
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    }
+#endif
+
+    /* add Mac header */
+    if (prepare_and_add_macheader(priv, skb, newSkb, req.Priority, &bulkdata, interfaceTag, frame, frame + ETH_ALEN, protection)) {
+        unifi_error(priv, "failed to create MAC header\n");
+        unifi_net_data_free(priv,&bulkdata.d[0]);
+        return -1;
+    }
+
+    if (eapolStore) {
+        spin_lock_irqsave(&priv->m4_lock, flags);
+        /* Store the EAPOL M4 packet for later */
+        interfacePriv->m4_signal = *signal;
+        interfacePriv->m4_bulk_data.net_buf_length = bulkdata.d[0].net_buf_length;
+        interfacePriv->m4_bulk_data.data_length = bulkdata.d[0].data_length;
+        interfacePriv->m4_bulk_data.os_data_ptr = bulkdata.d[0].os_data_ptr;
+        interfacePriv->m4_bulk_data.os_net_buf_ptr = bulkdata.d[0].os_net_buf_ptr;
+        spin_unlock_irqrestore(&priv->m4_lock, flags);
+        /* Send a signal to SME */
+        unifi_trace(priv, UDBG1, "_sys_packet_req: Sending CsrWifiRouterCtrlM4ReadyToSendInd\n");
+        CsrWifiRouterCtrlM4ReadyToSendIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0, interfaceTag, peerMacAddress);
+        return 0;
+    }
+
+    /* Send the signal to UniFi */
+      /* Set the B31 to 1 for local routing*/
+    r= uf_process_ma_packet_req(priv,  peerMacAddress.a, (req.HostTag | 0x80000000), interfaceTag, 0,
+                                (CSR_RATE)0, req.Priority, signal->SignalPrimitiveHeader.SenderProcessId, &bulkdata);
+    if (r) {
+        unifi_error(priv,
+                    "_sys_packet_req: failed to send signal.\n");
+        unifi_net_data_free(priv,&bulkdata.d[0]);
+        return r;
+    }
+    /* The final CsrWifiRouterMaPacketCfmSend() will called when the actual MA-PACKET.cfm is received from the chip */
+
+    return 0;
+}
+
+void CsrWifiRouterMaPacketReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    int r;
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterMaPacketReq* mareq = (CsrWifiRouterMaPacketReq*)msg;
+    llc_snap_hdr_t *snap;
+    u16 snap_protocol;
+    CSR_SIGNAL signal;
+    CSR_MA_PACKET_REQUEST *req = &signal.u.MaPacketRequest;
+    CsrWifiRouterCtrlPortAction controlPortaction;
+    u8 *daddr, *saddr;
+    u16 interfaceTag = mareq->interfaceTag & 0x00ff;
+    int queue;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (!mareq->frame || !priv || !priv->smepriv)
+    {
+        unifi_error(priv, "CsrWifiRouterMaPacketReqHandler: invalid frame/priv/priv->smepriv\n");
+        return;
+    }
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "CsrWifiRouterMaPacketReqHandler: interfaceID >= CSR_WIFI_NUM_INTERFACES.\n");
+        return;
+    }
+    /* get a pointer to dest & source Mac address */
+    daddr = mareq->frame;
+    saddr = (mareq->frame + ETH_ALEN);
+    /* point to the proper position of frame, since frame has MAC header */
+    snap = (llc_snap_hdr_t *) (mareq->frame + 2 * ETH_ALEN);
+    snap_protocol = ntohs(snap->protocol);
+    if((snap_protocol == ETH_P_PAE)
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+       || (snap_protocol == ETH_P_WAI)
+#endif
+    )
+    {
+        queue = UF_UNCONTROLLED_PORT_Q;
+    }
+    else
+    {
+        queue = UF_CONTROLLED_PORT_Q;
+    }
+
+    /* Controlled port restrictions apply to the packets */
+    controlPortaction = uf_sme_port_state(priv, daddr, queue, interfaceTag);
+    if (controlPortaction != CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN)
+    {
+        unifi_warning(priv, "CsrWifiRouterMaPacketReqHandler: (%s)controlled port is closed.\n", (queue == UF_CONTROLLED_PORT_Q)?"":"un");
+        if(mareq->cfmRequested)
+        {
+            CsrWifiRouterMaPacketCfmSend(msg->source,
+                                     interfaceTag,
+                                     CSR_RESULT_FAILURE,
+                                     mareq->hostTag, 0);
+        }
+        return;
+    }
+
+    signal.SignalPrimitiveHeader.SignalId = CSR_MA_PACKET_REQUEST_ID;
+    /* Store the appHandle in the LSB of the SenderId. */
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN(((priv->sme_cli->sender_id & 0xff00) | (unsigned int)msg->source),
+                                     (u8*)&signal.SignalPrimitiveHeader.SenderProcessId);
+    signal.SignalPrimitiveHeader.ReceiverProcessId = 0;
+
+    /* Fill in the MA-PACKET.req signal */
+    memcpy(req->Ra.x, daddr, ETH_ALEN);
+    req->Priority = mareq->priority;
+    req->TransmitRate = 0; /* Let firmware select the rate*/
+    req->VirtualInterfaceIdentifier = uf_get_vif_identifier(interfacePriv->interfaceMode,interfaceTag);
+    req->HostTag = mareq->hostTag;
+
+    if(mareq->cfmRequested)
+        req->TransmissionControl = 0;
+    else
+        req->TransmissionControl = CSR_NO_CONFIRM_REQUIRED;
+
+    r = _sys_packet_req(priv, &signal, mareq->subscriptionHandle,
+            mareq->frameLength, mareq->frame, snap_protocol);
+
+    if (r && mareq->cfmRequested)
+    {
+        CsrWifiRouterMaPacketCfmSend(msg->source,interfaceTag,
+                                     CSR_RESULT_FAILURE,
+                                     mareq->hostTag, 0);
+    }
+    return;
+}
+
+void CsrWifiRouterMaPacketCancelReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+void CsrWifiRouterCtrlM4TransmitReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlM4TransmitReq* req = (CsrWifiRouterCtrlM4TransmitReq*)msg;
+    int r;
+    bulk_data_param_t bulkdata;
+    netInterface_priv_t *interfacePriv;
+    CSR_SIGNAL m4_signal;
+    unsigned long flags;
+
+    if (priv == NULL) {
+        unifi_error(priv, "CsrWifiRouterCtrlM4TransmitReqHandler: invalid smepriv\n");
+        return;
+    }
+    if (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "M4TransmitReqHandler: interfaceTag >= CSR_WIFI_NUM_INTERFACES\n");
+        return;
+    }
+
+    interfacePriv = priv->interfacePriv[req->interfaceTag];
+    spin_lock_irqsave(&priv->m4_lock, flags);
+    if (interfacePriv->m4_bulk_data.data_length == 0) {
+        spin_unlock_irqrestore(&priv->m4_lock, flags);
+        unifi_error(priv, "CsrWifiRouterCtrlM4TransmitReqHandler: invalid buffer\n");
+        return;
+    }
+
+    memcpy(&bulkdata.d[0], &interfacePriv->m4_bulk_data, sizeof(bulk_data_desc_t));
+
+    interfacePriv->m4_bulk_data.net_buf_length = 0;
+    interfacePriv->m4_bulk_data.data_length = 0;
+    interfacePriv->m4_bulk_data.os_data_ptr = interfacePriv->m4_bulk_data.os_net_buf_ptr = NULL;
+    m4_signal = interfacePriv->m4_signal;
+    spin_unlock_irqrestore(&priv->m4_lock, flags);
+
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].data_length = 0;
+
+    interfacePriv->m4_sent = TRUE;
+    m4_signal.u.MaPacketRequest.HostTag |= 0x80000000;
+    /* Store the hostTag for later varification */
+    interfacePriv->m4_hostTag = m4_signal.u.MaPacketRequest.HostTag;
+    r = ul_send_signal_unpacked(priv, &m4_signal, &bulkdata);
+    unifi_trace(priv, UDBG1,
+                "CsrWifiRouterCtrlM4TransmitReqHandler: sent\n");
+    if (r) {
+        unifi_error(priv,
+                    "CsrWifiRouterCtrlM4TransmitReqHandler: failed to send signal.\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+    }
+}
+
+/* reset the station records when the mode is set as CSR_WIFI_ROUTER_CTRL_MODE_NONE */
+static void CsrWifiRouterCtrlResetStationRecordList(unifi_priv_t *priv, u16 interfaceTag)
+{
+    u8 i,j;
+    CsrWifiRouterCtrlStaInfo_t *staInfo=NULL;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    unsigned long lock_flags;
+
+    /* create a list for sending confirms of un-delivered packets */
+    struct list_head send_cfm_list;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "CsrWifiRouterCtrlResetStationRecordList: bad interfaceTag\n");
+        return;
+    }
+
+    INIT_LIST_HEAD(&send_cfm_list);
+
+    /* Reset the station record to NULL if mode is NONE */
+    for(i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        if ((staInfo=interfacePriv->staInfo[i]) != NULL) {
+            uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                 &send_cfm_list,
+                                                 &(staInfo->mgtFrames));
+            uf_flush_list(priv,&(staInfo->mgtFrames));
+            for(j=0;j<MAX_ACCESS_CATOGORY;j++){
+                uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                     &send_cfm_list,
+                                                     &(staInfo->dataPdu[j]));
+                uf_flush_list(priv,&(staInfo->dataPdu[j]));
+            }
+
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            /* Removing station record information from port config array */
+            memset(staInfo->peerControlledPort, 0, sizeof(unifi_port_cfg_t));
+            staInfo->peerControlledPort->port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+            staInfo->peerControlledPort->in_use = FALSE;
+            interfacePriv->controlled_data_port.entries_in_use--;
+
+            memset(staInfo->peerUnControlledPort, 0, sizeof(unifi_port_cfg_t));
+            staInfo->peerUnControlledPort->port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+            staInfo->peerUnControlledPort->in_use = FALSE;
+            interfacePriv->uncontrolled_data_port.entries_in_use--;
+
+            kfree(interfacePriv->staInfo[i]);
+            interfacePriv->staInfo[i] = NULL;
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        }
+    }
+    /* after the critical region process the list of frames that requested cfm
+     * and send cfm to requestor one by one
+     */
+    send_auto_ma_packet_confirm(priv, interfacePriv, &send_cfm_list);
+
+#ifdef CSR_SUPPORT_SME
+    /* Interface Independent, no of packet queued, incase of mode is None or AP set to 0 */
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+        case CSR_WIFI_ROUTER_CTRL_MODE_NONE:
+            if (priv->noOfPktQueuedInDriver) {
+                unifi_warning(priv, "After reset the noOfPktQueuedInDriver = %x\n", priv->noOfPktQueuedInDriver);
+                spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                priv->noOfPktQueuedInDriver = 0;
+                spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+            }
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            break;
+        default:
+            unifi_error(priv, "interfacemode is not correct in CsrWifiRouterCtrlResetStationRecordList: debug\n");
+    }
+#endif
+
+    if (((interfacePriv->controlled_data_port.entries_in_use != 0) || (interfacePriv->uncontrolled_data_port.entries_in_use != 0))
+            && (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_NONE)) {
+        /* Print in case if the value of entries goes to -ve/+ve (apart from 0)
+         * we expect the entries should be zero here if mode is set as NONE
+         */
+        unifi_trace(priv, UDBG3, "In %s controlled port entries = %d, uncontrolled port entries = %d\n",
+                   __FUNCTION__, interfacePriv->controlled_data_port.entries_in_use,
+                   interfacePriv->uncontrolled_data_port.entries_in_use);
+    }
+}
+
+void CsrWifiRouterCtrlInterfaceReset(unifi_priv_t *priv, u16 interfaceTag)
+{
+    netInterface_priv_t *interfacePriv;
+
+    /* create a list for sending confirms of un-delivered packets */
+    struct list_head send_cfm_list;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "CsrWifiRouterCtrlInterfaceReset: bad interfaceTag\n");
+        return;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    INIT_LIST_HEAD(&send_cfm_list);
+
+    /* Enable all queues by default */
+    interfacePriv->queueEnabled[0] = 1;
+    interfacePriv->queueEnabled[1] = 1;
+    interfacePriv->queueEnabled[2] = 1;
+    interfacePriv->queueEnabled[3] = 1;
+
+    uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                             &send_cfm_list,
+                                             &(interfacePriv->genericMgtFrames));
+    uf_flush_list(priv,&(interfacePriv->genericMgtFrames));
+
+    uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                             &send_cfm_list,
+                                             &(interfacePriv->genericMulticastOrBroadCastMgtFrames));
+    uf_flush_list(priv,&(interfacePriv->genericMulticastOrBroadCastMgtFrames));
+
+    uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                             &send_cfm_list,
+                                             &(interfacePriv->genericMulticastOrBroadCastFrames));
+
+    uf_flush_list(priv,&(interfacePriv->genericMulticastOrBroadCastFrames));
+
+    /*  process the list of frames that requested cfm
+    and send cfm to requestor one by one */
+    send_auto_ma_packet_confirm(priv, interfacePriv, &send_cfm_list);
+
+    /* Reset the station record to NULL if mode is tried to set as NONE */
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+        case CSR_WIFI_ROUTER_CTRL_MODE_MONITOR:
+        case CSR_WIFI_ROUTER_CTRL_MODE_AMP:
+            /* station records not available in these modes */
+            break;
+        default:
+            CsrWifiRouterCtrlResetStationRecordList(priv,interfaceTag);
+    }
+
+    interfacePriv->num_stations_joined = 0;
+    interfacePriv->sta_activity_check_enabled = FALSE;
+}
+
+
+void CsrWifiRouterCtrlModeSetReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlModeSetReq* req = (CsrWifiRouterCtrlModeSetReq*)msg;
+
+    if (priv == NULL)
+    {
+        unifi_error(priv, "CsrWifiRouterCtrlModeSetReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    if (req->interfaceTag < CSR_WIFI_NUM_INTERFACES)
+    {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+#ifdef CSR_WIFI_SPLIT_PATCH
+        u8 old_mode = interfacePriv->interfaceMode;
+#endif
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlModeSetReqHandler: interfacePriv->interfaceMode = %d\n",
+                interfacePriv->interfaceMode);
+
+        interfacePriv->interfaceMode = req->mode;
+
+#ifdef CSR_WIFI_SPLIT_PATCH
+        /* Detect a change in mode that requires a switch to/from the AP firmware patch.
+         * This should only happen when transitioning in/out of AP modes.
+         */
+        if (CSR_WIFI_HIP_IS_AP_FW(req->mode) != CSR_WIFI_HIP_IS_AP_FW(old_mode))
+        {
+            CsrWifiRouterCtrlVersions versions;
+            int r;
+
+#ifdef ANDROID_BUILD
+            /* Take the wakelock while switching patch */
+            unifi_trace(priv, UDBG1, "patch switch: take wake lock\n");
+            wake_lock(&unifi_sdio_wake_lock);
+#endif
+            unifi_info(priv, "Resetting UniFi with %s patch\n", CSR_WIFI_HIP_IS_AP_FW(req->mode) ? "AP" : "STA");
+
+            r = uf_request_firmware_files(priv, UNIFI_FW_STA);
+            if (r) {
+                unifi_error(priv, "CsrWifiRouterCtrlModeSetReqHandler: Failed to get f/w\n");
+                CsrWifiRouterCtrlModeSetCfmSend(msg->source, req->clientData, req->interfaceTag,
+                                                req->mode, CSR_RESULT_FAILURE);
+                return;
+            }
+
+            /* Block the I/O thread */
+            priv->bh_thread.block_thread = 1;
+
+            /* Reset and download the new patch */
+            r = uf_init_hw(priv);
+            if (r) {
+                unifi_error(priv, "CsrWifiRouterCtrlWifiOnReqHandler: Failed to initialise h/w, error %d\n", r);
+                CsrWifiRouterCtrlModeSetCfmSend(msg->source, req->clientData, req->interfaceTag,
+                                                req->mode, CSR_RESULT_FAILURE);
+                return;
+            }
+
+            /* Re-enable the I/O thread */
+            priv->bh_thread.block_thread = 0;
+
+            /* Get the version information from the core */
+            unifi_card_info(priv->card, &priv->card_info);
+
+            /* Copy to the unifiio_card_info structure. */
+            versions.chipId = priv->card_info.chip_id;
+            versions.chipVersion = priv->card_info.chip_version;
+            versions.firmwareBuild = priv->card_info.fw_build;
+            versions.firmwareHip = priv->card_info.fw_hip_version;
+            versions.routerBuild = (char*)CSR_WIFI_VERSION;
+            versions.routerHip = (UNIFI_HIP_MAJOR_VERSION << 8) | UNIFI_HIP_MINOR_VERSION;
+
+            /* Now that new firmware is running, send a WifiOnInd to the NME. This will
+             * cause it to retransfer the MIB.
+             */
+            CsrWifiRouterCtrlWifiOnIndSend(msg->source, 0, CSR_RESULT_SUCCESS, versions);
+
+            /* Store the request so we know where to send the ModeSetCfm */
+            priv->pending_mode_set = *req;
+        }
+        else
+#endif
+        {
+            /* No patch switch, confirm straightaway */
+            CsrWifiRouterCtrlModeSetCfmSend(msg->source, req->clientData, req->interfaceTag,
+                                            req->mode, CSR_RESULT_SUCCESS);
+        }
+
+        interfacePriv->bssid = req->bssid;
+        /* For modes other than AP/P2PGO, set below member FALSE */
+        interfacePriv->intraBssEnabled = FALSE;
+        /* Initialise the variable bcTimSet with a value
+         * other then CSR_WIFI_TIM_SET or CSR_WIFI_TIM_RESET value
+         */
+        interfacePriv->bcTimSet = 0xFF;
+        interfacePriv->bcTimSetReqPendingFlag = FALSE;
+        /* Initialise the variable bcTimSetReqQueued with a value
+         * other then CSR_WIFI_TIM_SET or CSR_WIFI_TIM_RESET value
+         */
+        interfacePriv->bcTimSetReqQueued =0xFF;
+        CsrWifiRouterCtrlInterfaceReset(priv,req->interfaceTag);
+
+        if(req->mode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+           req->mode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+            interfacePriv->protect = req->protection;
+            interfacePriv->dtimActive=FALSE;
+            interfacePriv->multicastPduHostTag = 0xffffffff;
+            /* For AP/P2PGO mode SME sending intraBssDistEnabled
+             * i.e. for AP: intraBssDistEnabled = TRUE, for P2PGO
+             * intraBssDistEnabled = TRUE/FALSE on requirement
+             */
+            interfacePriv->intraBssEnabled = req->intraBssDistEnabled;
+            unifi_trace(priv, UDBG3, "CsrWifiRouterCtrlModeSetReqHandler: IntraBssDisEnabled = %d\n",
+                        req->intraBssDistEnabled);
+        } else if (req->mode == CSR_WIFI_ROUTER_CTRL_MODE_NONE) {
+              netif_carrier_off(priv->netdev[req->interfaceTag]);
+              interfacePriv->connected = UnifiConnectedUnknown;
+        }
+    }
+    else {
+        unifi_error(priv, "CsrWifiRouterCtrlModeSetReqHandler: invalid interfaceTag :%d\n",req->interfaceTag);
+    }
+}
+
+void CsrWifiRouterMaPacketResHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+}
+
+/* delete the station record from the station record data base */
+static int peer_delete_record(unifi_priv_t *priv, CsrWifiRouterCtrlPeerDelReq *req)
+{
+    u8 j;
+    CsrWifiRouterCtrlStaInfo_t *staInfo = NULL;
+    unifi_port_config_t *controlledPort;
+    unifi_port_config_t *unControlledPort;
+    netInterface_priv_t *interfacePriv;
+
+    u8 ba_session_idx = 0;
+    ba_session_rx_struct *ba_session_rx = NULL;
+    ba_session_tx_struct *ba_session_tx = NULL;
+
+    /* create a list for sending confirms of un-delivered packets */
+    struct list_head send_cfm_list;
+
+    unsigned long lock_flags;
+
+    if ((req->peerRecordHandle >= UNIFI_MAX_CONNECTIONS) || (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES)) {
+        unifi_error(priv, "handle/interfaceTag is not proper, handle = %d, interfaceTag = %d\n", req->peerRecordHandle, req->interfaceTag);
+        return CSR_RESULT_FAILURE;
+    }
+
+    INIT_LIST_HEAD(&send_cfm_list);
+
+    interfacePriv = priv->interfacePriv[req->interfaceTag];
+    /* remove the station record & make it NULL */
+    if ((staInfo=interfacePriv->staInfo[req->peerRecordHandle])!=NULL) {
+
+        uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                 &send_cfm_list,
+                                                 &(staInfo->mgtFrames));
+
+        uf_flush_list(priv,&(staInfo->mgtFrames));
+        for(j=0;j<MAX_ACCESS_CATOGORY;j++){
+            uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                     &send_cfm_list,
+                                                     &(staInfo->dataPdu[j]));
+            uf_flush_list(priv,&(staInfo->dataPdu[j]));
+        }
+
+        spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+        /* clear the port configure array info, for the corresponding peer entry */
+        controlledPort = &interfacePriv->controlled_data_port;
+        unControlledPort = &interfacePriv->uncontrolled_data_port;
+
+        unifi_trace(priv, UDBG1, "peer_delete_record: Peer found handle = %d, port in use: cont(%d), unCont(%d)\n",
+                    req->peerRecordHandle, controlledPort->entries_in_use, unControlledPort->entries_in_use);
+
+        memset(staInfo->peerControlledPort, 0, sizeof(unifi_port_cfg_t));
+        staInfo->peerControlledPort->port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+        staInfo->peerControlledPort->in_use = FALSE;
+        if (controlledPort->entries_in_use) {
+            controlledPort->entries_in_use--;
+        } else {
+            unifi_warning(priv, "number of controlled port entries is zero, trying to decrement: debug\n");
+        }
+
+        memset(staInfo->peerUnControlledPort, 0, sizeof(unifi_port_cfg_t));
+        staInfo->peerUnControlledPort->port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+        staInfo->peerUnControlledPort->in_use = FALSE;
+        if (unControlledPort->entries_in_use) {
+            unControlledPort->entries_in_use--;
+        } else {
+            unifi_warning(priv, "number of uncontrolled port entries is zero, trying to decrement: debug\n");
+        }
+
+        spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        /* update the TIM with zero */
+        if (interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_IBSS &&
+                staInfo->timSet == CSR_WIFI_TIM_SET) {
+            unifi_trace(priv, UDBG3, "peer is deleted so TIM updated to 0, in firmware\n");
+            update_tim(priv,staInfo->aid,0,req->interfaceTag, req->peerRecordHandle);
+        }
+
+
+        /* Stop BA session if it is active, for this peer address all BA sessions
+        (per tID per role) are closed */
+
+        down(&priv->ba_mutex);
+        for(ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX; ba_session_idx++){
+            ba_session_rx = priv->interfacePriv[req->interfaceTag]->ba_session_rx[ba_session_idx];
+            if(ba_session_rx) {
+                if(!memcmp(ba_session_rx->macAddress.a, staInfo->peerMacAddress.a, ETH_ALEN)){
+                    blockack_session_stop(priv,
+                                        req->interfaceTag,
+                                        CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT,
+                                        ba_session_rx->tID,
+                                        ba_session_rx->macAddress);
+                }
+            }
+        }
+
+        for(ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_TX; ba_session_idx++){
+            ba_session_tx = priv->interfacePriv[req->interfaceTag]->ba_session_tx[ba_session_idx];
+            if(ba_session_tx) {
+                if(!memcmp(ba_session_tx->macAddress.a, staInfo->peerMacAddress.a, ETH_ALEN)){
+                    blockack_session_stop(priv,
+                                        req->interfaceTag,
+                                        CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR,
+                                        ba_session_tx->tID,
+                                        ba_session_tx->macAddress);
+                }
+            }
+        }
+
+        up(&priv->ba_mutex);
+
+#ifdef CSR_SUPPORT_SME
+        unifi_trace(priv, UDBG1, "Canceling work queue for STA with AID: %d\n", staInfo->aid);
+        cancel_work_sync(&staInfo->send_disconnected_ind_task);
+#endif
+
+        spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+#ifdef CSR_SUPPORT_SME
+        interfacePriv->num_stations_joined--;
+
+        staInfo->nullDataHostTag = INVALID_HOST_TAG;
+
+        if ((interfacePriv->sta_activity_check_enabled) &&
+            (interfacePriv->num_stations_joined < STA_INACTIVE_DETECTION_TRIGGER_THRESHOLD))
+        {
+            unifi_trace(priv, UDBG1, "STOPPING the Inactivity Timer (num of stations = %d)\n", interfacePriv->num_stations_joined);
+            interfacePriv->sta_activity_check_enabled = FALSE;
+            del_timer_sync(&interfacePriv->sta_activity_check_timer);
+        }
+#endif
+
+        /* Free the station record for corresponding peer */
+        kfree(interfacePriv->staInfo[req->peerRecordHandle]);
+        interfacePriv->staInfo[req->peerRecordHandle] = NULL;
+        spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+        /* after the critical region process the list of frames that requested cfm
+        and send cfm to requestor one by one */
+        send_auto_ma_packet_confirm(priv, interfacePriv, &send_cfm_list);
+
+
+    }
+    else
+    {
+        unifi_trace(priv, UDBG3, " peer not found: Delete request Peer handle[%d]\n", req->peerRecordHandle);
+    }
+
+    return CSR_RESULT_SUCCESS;
+}
+
+void CsrWifiRouterCtrlPeerDelReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    CsrWifiRouterCtrlPeerDelReq* req = (CsrWifiRouterCtrlPeerDelReq*)msg;
+    CsrResult status = CSR_RESULT_SUCCESS;
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    unifi_trace(priv, UDBG2, "entering CsrWifiRouterCtrlPeerDelReqHandler \n");
+    if (priv == NULL)
+    {
+        unifi_error(priv, "CsrWifiRouterCtrlPeerDelReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    if (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "CsrWifiRouterCtrlPeerDelReqHandler: bad interfaceTag\n");
+        return;
+    }
+
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            /* remove the station from station record data base */
+            status = peer_delete_record(priv, req);
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+        default:
+            /* No station record to maintain in these modes */
+            break;
+    }
+
+    CsrWifiRouterCtrlPeerDelCfmSend(msg->source,req->clientData,req->interfaceTag,status);
+    unifi_trace(priv, UDBG2, "leaving CsrWifiRouterCtrlPeerDelReqHandler \n");
+}
+
+/* Add the new station to the station record data base */
+static int peer_add_new_record(unifi_priv_t *priv,CsrWifiRouterCtrlPeerAddReq *req,u32 *handle)
+{
+    u8 i, powerModeTemp = 0;
+    u8 freeSlotFound = FALSE;
+    CsrWifiRouterCtrlStaInfo_t *newRecord = NULL;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+    CsrTime currentTime, currentTimeHi;
+    unsigned long lock_flags;
+
+    if (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "peer_add_new_record: bad interfaceTag\n");
+        return CSR_RESULT_FAILURE;
+    }
+
+    currentTime = CsrTimeGet(&currentTimeHi);
+
+    for(i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        if(interfacePriv->staInfo[i] == NULL) {
+            /* Slot is empty, so can be used for station record */
+            freeSlotFound = TRUE;
+            *handle = i;
+
+            /* Allocate for the new station record , to avoid race condition would happen between ADD_PEER &
+             * DEL_PEER the allocation made atomic memory rather than kernel memory
+             */
+            newRecord = (CsrWifiRouterCtrlStaInfo_t *) kmalloc(sizeof(CsrWifiRouterCtrlStaInfo_t), GFP_ATOMIC);
+            if (!newRecord) {
+                unifi_error(priv, "failed to allocate the %d bytes of mem for station record\n",
+                            sizeof(CsrWifiRouterCtrlStaInfo_t));
+                return CSR_RESULT_FAILURE;
+            }
+
+            unifi_trace(priv, UDBG1, "peer_add_new_record: handle = %d AID = %d addr = %x:%x:%x:%x:%x:%x LI=%u\n",
+                        *handle, req->associationId, req->peerMacAddress.a[0], req->peerMacAddress.a[1], req->peerMacAddress.a[2],
+                        req->peerMacAddress.a[3], req->peerMacAddress.a[4], req->peerMacAddress.a[5],
+                        req->staInfo.listenIntervalInTus);
+
+            /* disable the preemption until station record updated */
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+
+            interfacePriv->staInfo[i] = newRecord;
+            /* Initialize the record*/
+            memset(newRecord,0,sizeof(CsrWifiRouterCtrlStaInfo_t));
+            /* update the station record */
+            memcpy(newRecord->peerMacAddress.a, req->peerMacAddress.a, ETH_ALEN);
+            newRecord->wmmOrQosEnabled = req->staInfo.wmmOrQosEnabled;
+
+            /* maxSpLength is bit map in qosInfo field, so converting accordingly */
+            newRecord->maxSpLength = req->staInfo.maxSpLength * 2;
+
+            /*Max SP 0 mean any number of packets. since we buffer only 512
+            packets we are hard coding this to zero for the moment */
+
+            if(newRecord->maxSpLength == 0)
+                newRecord->maxSpLength=512;
+
+            newRecord->assignedHandle = i;
+
+             /* copy power save mode of all access catagory (Trigger/Delivery/both enabled/disabled) */
+            powerModeTemp = (u8) ((req->staInfo.powersaveMode >> 4) & 0xff);
+
+            if(!(req->staInfo.powersaveMode & 0x0001))
+                newRecord->powersaveMode[UNIFI_TRAFFIC_Q_BK]= CSR_WIFI_AC_LEGACY_POWER_SAVE;
+            else
+               newRecord->powersaveMode[UNIFI_TRAFFIC_Q_BK]= powerModeTemp & 0x03;
+
+            if(!(req->staInfo.powersaveMode & 0x0002))
+                newRecord->powersaveMode[UNIFI_TRAFFIC_Q_BE]= CSR_WIFI_AC_LEGACY_POWER_SAVE;
+            else
+               newRecord->powersaveMode[UNIFI_TRAFFIC_Q_BE]= ((powerModeTemp & 0x0C)>> 2);
+
+            if(!(req->staInfo.powersaveMode & 0x0004))
+                newRecord->powersaveMode[UNIFI_TRAFFIC_Q_VI]= CSR_WIFI_AC_LEGACY_POWER_SAVE;
+            else
+               newRecord->powersaveMode[UNIFI_TRAFFIC_Q_VI]= ((powerModeTemp & 0x30)>> 4);
+
+            if(!(req->staInfo.powersaveMode & 0x0008))
+                newRecord->powersaveMode[UNIFI_TRAFFIC_Q_VO]= CSR_WIFI_AC_LEGACY_POWER_SAVE;
+            else
+               newRecord->powersaveMode[UNIFI_TRAFFIC_Q_VO]= ((powerModeTemp & 0xC0)>> 6);
+
+            {
+                u8 k;
+                for(k=0; k< MAX_ACCESS_CATOGORY ;k++)
+                    unifi_trace(priv, UDBG2, "peer_add_new_record: WMM : %d ,AC %d, powersaveMode %x \n",
+                            req->staInfo.wmmOrQosEnabled,k,newRecord->powersaveMode[k]);
+            }
+
+            unifi_trace(priv, UDBG3, "newRecord->wmmOrQosEnabled : %d , MAX SP : %d\n",
+                    newRecord->wmmOrQosEnabled,newRecord->maxSpLength);
+
+            /* Initialize the mgtFrames & data Pdu list */
+            {
+                u8 j;
+                INIT_LIST_HEAD(&newRecord->mgtFrames);
+                for(j = 0; j < MAX_ACCESS_CATOGORY; j++) {
+                    INIT_LIST_HEAD(&newRecord->dataPdu[j]);
+                }
+            }
+
+            newRecord->lastActivity = currentTime;
+            newRecord->activity_flag = TRUE;
+
+            /* enable the preemption as station record updated */
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+            /* First time port actions are set for the peer with below information */
+            configure_data_port(priv, CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN, &newRecord->peerMacAddress,
+                                UF_UNCONTROLLED_PORT_Q, req->interfaceTag);
+
+            if (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_IBSS) {
+                configure_data_port(priv, CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN, &newRecord->peerMacAddress,
+                                    UF_CONTROLLED_PORT_Q, req->interfaceTag);
+            } else {
+                configure_data_port(priv, CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD, &newRecord->peerMacAddress,
+                                    UF_CONTROLLED_PORT_Q, req->interfaceTag);
+            }
+
+
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            /* Port status must be already set before calling the Add Peer request */
+            newRecord->peerControlledPort = uf_sme_port_config_handle(priv, newRecord->peerMacAddress.a,
+                                                                      UF_CONTROLLED_PORT_Q, req->interfaceTag);
+            newRecord->peerUnControlledPort = uf_sme_port_config_handle(priv, newRecord->peerMacAddress.a,
+                                                                        UF_UNCONTROLLED_PORT_Q, req->interfaceTag);
+
+            if (!newRecord->peerControlledPort || !newRecord->peerUnControlledPort) {
+                /* enable the preemption as station record failed to update */
+                unifi_warning(priv, "Un/ControlledPort record not found in port configuration array index = %d\n", i);
+                kfree(interfacePriv->staInfo[i]);
+                interfacePriv->staInfo[i] = NULL;
+                spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+                return CSR_RESULT_FAILURE;
+            }
+
+            newRecord->currentPeerState = CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE;
+
+            /* changes done during block ack handling */
+            newRecord->txSuspend = FALSE;
+
+            /*U-APSD related data structure*/
+            newRecord->timRequestPendingFlag = FALSE;
+
+            /* Initialise the variable updateTimReqQueued with a value
+             * other then CSR_WIFI_TIM_SET or CSR_WIFI_TIM_RESET value
+             */
+            newRecord->updateTimReqQueued = 0xFF;
+            newRecord->timSet = CSR_WIFI_TIM_RESET;
+            newRecord->uapsdActive = FALSE;
+            newRecord->noOfSpFramesSent =0;
+            newRecord->triggerFramePriority = CSR_QOS_UP0;
+
+            /* The protection bit is updated once the port opens for corresponding peer in
+             * routerPortConfigure request */
+
+            /* update the association ID */
+            newRecord->aid = req->associationId;
+
+#ifdef CSR_SUPPORT_SME
+            interfacePriv->num_stations_joined++;
+            newRecord->interfacePriv = interfacePriv;
+            newRecord->listenIntervalInTus = req->staInfo.listenIntervalInTus;
+            newRecord->nullDataHostTag = INVALID_HOST_TAG;
+
+            INIT_WORK(&newRecord->send_disconnected_ind_task, uf_send_disconnected_ind_wq);
+
+            if(!(interfacePriv->sta_activity_check_enabled) &&
+               (interfacePriv->num_stations_joined >= STA_INACTIVE_DETECTION_TRIGGER_THRESHOLD)){
+                unifi_trace(priv, UDBG1,
+                            "peer_add_new_record: STARTING the Inactivity Timer (num of stations = %d)",
+                            interfacePriv->num_stations_joined);
+
+                interfacePriv->sta_activity_check_enabled = TRUE;
+                interfacePriv->sta_activity_check_timer.function = check_inactivity_timer_expire_func;
+                interfacePriv->sta_activity_check_timer.data = (unsigned long)interfacePriv;
+
+                init_timer(&interfacePriv->sta_activity_check_timer);
+                mod_timer(&interfacePriv->sta_activity_check_timer,
+                          (jiffies + usecs_to_jiffies(STA_INACTIVE_DETECTION_TIMER_INTERVAL * 1000 * 1000)));
+
+            }
+#endif
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+            break;
+        }
+    }
+
+    if(!freeSlotFound) {
+        unifi_error(priv, "Limited connectivity, Free slot not found for station record addition\n");
+        return CSR_RESULT_FAILURE;
+    }
+    return CSR_RESULT_SUCCESS;
+}
+
+#ifdef CSR_SUPPORT_SME
+static void check_inactivity_timer_expire_func(unsigned long data)
+{
+    struct unifi_priv *priv;
+    CsrWifiRouterCtrlStaInfo_t *sta_record = NULL;
+    u8 i = 0;
+    CsrTime now;
+    CsrTime inactive_time;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *) data;
+
+    if (!interfacePriv)
+    {
+        return;
+    }
+
+    priv = interfacePriv->privPtr;
+
+    if (interfacePriv->InterfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "check_inactivity_timer_expire_func: Invalid interfaceTag\n");
+        return;
+    }
+
+    /* RUN Algorithm to check inactivity for each connected station */
+    now = CsrTimeGet(NULL);
+
+    for(i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        if(interfacePriv->staInfo[i] != NULL) {
+            sta_record = interfacePriv->staInfo[i];
+
+            if (sta_record->activity_flag == TRUE){
+                sta_record->activity_flag = FALSE;
+                sta_record->lastActivity = now;
+                continue;
+            }
+
+            if (sta_record->lastActivity > now)
+            {
+                /* simple timer wrap (for 1 wrap) */
+                inactive_time = CsrTimeAdd((CsrTime)CsrTimeSub(CSR_SCHED_TIME_MAX, sta_record->lastActivity), now);
+            }
+            else
+            {
+                inactive_time = (CsrTime)CsrTimeSub(now, sta_record->lastActivity);
+            }
+
+            if (inactive_time >= STA_INACTIVE_TIMEOUT_VAL)
+            {
+                unifi_trace(priv, UDBG1, "STA is Inactive - AID = %d inactive_time = %d\n",
+                                        sta_record->aid,
+                                        inactive_time);
+
+                /* station is in-active, if it is in active mode send a null frame
+                 * and the station should acknowledge the null frame, if acknowledgement
+                 * is not received throw out the station.
+                 * If the station is in Power Save, update TIM for the station so
+                 * that it wakes up and register some activity through PS-Poll or
+                 * trigger frame.
+                 */
+                 if (sta_record->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE)
+                 {
+                    unifi_trace(priv, UDBG1, "STA power save state - Active, send a NULL frame to check if it is ALIVE\n");
+                    uf_send_nulldata ( priv,
+                                       sta_record->interfacePriv->InterfaceTag,
+                                       sta_record->peerMacAddress.a,
+                                       CSR_CONTENTION,
+                                       sta_record);
+                 }
+                 else if (sta_record->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE)
+                 {
+                    if((sta_record->timSet == CSR_WIFI_TIM_SET) ||
+                       (sta_record->timSet == CSR_WIFI_TIM_SETTING))
+                    {
+                        unifi_trace(priv, UDBG1, "STA power save state - PS, TIM is already SET\n");
+
+                        /* If TIM is set and we do not have any activity for
+                         * more than 3 listen intervals then send a disconnected
+                         * indication to SME, to delete the station from station
+                         * record list.
+                         * The inactivity is already more than STA_INACTIVE_TIMEOUT_VAL
+                         * and this check ensures if the listen interval is a larger
+                         * value than STA_INACTIVE_TIMEOUT_VAL.
+                         */
+                         if (inactive_time > (3 * (sta_record->listenIntervalInTus * 1024)))
+                         {
+                            unifi_trace(priv, UDBG1, "STA is inactive for more than 3 listen intervals\n");
+                            queue_work( priv->unifi_workqueue,
+                                        &sta_record->send_disconnected_ind_task);
+                         }
+
+                    }
+                    else
+                    {
+                        unifi_trace(priv, UDBG1, "STA power save state - PS, update TIM to see if it is ALIVE\n");
+                        update_tim(priv,
+                                   sta_record->aid,
+                                   CSR_WIFI_TIM_SET,
+                                   interfacePriv->InterfaceTag,
+                                   sta_record->assignedHandle);
+                    }
+                 }
+            }
+        }
+    }
+
+    /* re-run the timer interrupt */
+    mod_timer(&interfacePriv->sta_activity_check_timer,
+              (jiffies + usecs_to_jiffies(STA_INACTIVE_DETECTION_TIMER_INTERVAL * 1000 * 1000)));
+
+}
+
+
+void uf_send_disconnected_ind_wq(struct work_struct *work)
+{
+
+    CsrWifiRouterCtrlStaInfo_t *staInfo = container_of(work, CsrWifiRouterCtrlStaInfo_t, send_disconnected_ind_task);
+    unifi_priv_t *priv;
+    u16 interfaceTag;
+    struct list_head send_cfm_list;
+    u8 j;
+
+    func_enter();
+
+    if(!staInfo) {
+        return;
+    }
+
+    if(!staInfo->interfacePriv) {
+        return;
+    }
+
+    priv = staInfo->interfacePriv->privPtr;
+    interfaceTag =  staInfo->interfacePriv->InterfaceTag;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_send_disconnected_ind_wq: invalid interfaceTag\n");
+        return;
+    }
+
+    /* The SME/NME may be waiting for confirmation for requested frames to this station.
+     * So loop through buffered frames for this station and if confirmation is
+     * requested, send auto confirmation with failure status. Also flush the frames so
+     * that these are not processed again in PEER_DEL_REQ handler.
+     */
+    INIT_LIST_HEAD(&send_cfm_list);
+
+    uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                             &send_cfm_list,
+                                             &(staInfo->mgtFrames));
+
+    uf_flush_list(priv, &(staInfo->mgtFrames));
+
+    for(j = 0; j < MAX_ACCESS_CATOGORY; j++){
+        uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                 &send_cfm_list,
+                                                 &(staInfo->dataPdu[j]));
+
+        uf_flush_list(priv,&(staInfo->dataPdu[j]));
+    }
+
+    send_auto_ma_packet_confirm(priv, staInfo->interfacePriv, &send_cfm_list);
+
+    unifi_warning(priv, "uf_send_disconnected_ind_wq: Router Disconnected IND Peer (%x-%x-%x-%x-%x-%x)\n",
+                staInfo->peerMacAddress.a[0],
+                staInfo->peerMacAddress.a[1],
+                staInfo->peerMacAddress.a[2],
+                staInfo->peerMacAddress.a[3],
+                staInfo->peerMacAddress.a[4],
+                staInfo->peerMacAddress.a[5]);
+
+    CsrWifiRouterCtrlConnectedIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,
+                                      0,
+                                      staInfo->interfacePriv->InterfaceTag,
+                                      staInfo->peerMacAddress,
+                                      CSR_WIFI_ROUTER_CTRL_PEER_DISCONNECTED);
+
+
+    return;
+}
+
+
+#endif
+void CsrWifiRouterCtrlPeerAddReqHandler(void* drvpriv,CsrWifiFsmEvent* msg)
+{
+    CsrWifiRouterCtrlPeerAddReq* req = (CsrWifiRouterCtrlPeerAddReq*)msg;
+    CsrResult status = CSR_RESULT_SUCCESS;
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    u32 handle = 0;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    unifi_trace(priv, UDBG2, "entering CsrWifiRouterCtrlPeerAddReqHandler \n");
+    if (priv == NULL)
+    {
+        unifi_error(priv, "CsrWifiRouterCtrlPeerAddReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    if (req->interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+    {
+        unifi_error(priv, "CsrWifiRouterCtrlPeerAddReqHandler: bad interfaceTag\n");
+        return;
+    }
+
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            /* Add station record */
+            status = peer_add_new_record(priv,req,&handle);
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+        default:
+            /* No station record to maintain in these modes */
+            break;
+    }
+
+    CsrWifiRouterCtrlPeerAddCfmSend(msg->source,req->clientData,req->interfaceTag,req->peerMacAddress,handle,status);
+    unifi_trace(priv, UDBG2, "leaving CsrWifiRouterCtrlPeerAddReqHandler \n");
+}
+
+void CsrWifiRouterCtrlPeerUpdateReqHandler(void* drvpriv,CsrWifiFsmEvent* msg)
+{
+    CsrWifiRouterCtrlPeerUpdateReq* req = (CsrWifiRouterCtrlPeerUpdateReq*)msg;
+    CsrResult status = CSR_RESULT_SUCCESS;
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+
+    unifi_trace(priv, UDBG2, "entering CsrWifiRouterCtrlPeerUpdateReqHandler \n");
+    if (priv == NULL)
+    {
+        unifi_error(priv, "CsrWifiRouterCtrlPeerUpdateReqHandler: invalid smepriv\n");
+        return;
+    }
+
+    CsrWifiRouterCtrlPeerUpdateCfmSend(msg->source,req->clientData,req->interfaceTag,status);
+    unifi_trace(priv, UDBG2, "leaving CsrWifiRouterCtrlPeerUpdateReqHandler \n");
+}
+
+
+ void CsrWifiRouterCtrlRawSdioDeinitialiseReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    /* This will never be called as it is intercepted in the Userspace */
+}
+
+void CsrWifiRouterCtrlRawSdioInitialiseReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    /* This will never be called as it is intercepted in the Userspace */
+}
+
+void
+uf_send_ba_err_wq(struct work_struct *work)
+{
+    ba_session_rx_struct *ba_session = container_of(work, ba_session_rx_struct, send_ba_err_task);
+    unifi_priv_t *priv;
+
+    if(!ba_session) {
+        return;
+    }
+
+    if(!ba_session->interfacePriv) {
+        return;
+    }
+
+    priv = ba_session->interfacePriv->privPtr;
+
+    if (ba_session->interfacePriv->InterfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "%s: invalid interfaceTag\n", __FUNCTION__);
+        return;
+    }
+
+    unifi_warning(priv, "%s: Calling CsrWifiRouterCtrlBlockAckErrorIndSend(%d, %d, %d, %d, %x:%x:%x:%x:%x:%x, %d)\n",
+                    __FUNCTION__,
+                    priv->CSR_WIFI_SME_IFACEQUEUE,
+                    0,
+                    ba_session->interfacePriv->InterfaceTag,
+                    ba_session->tID,
+                    ba_session->macAddress.a[0],
+                    ba_session->macAddress.a[1],
+                    ba_session->macAddress.a[2],
+                    ba_session->macAddress.a[3],
+                    ba_session->macAddress.a[4],
+                    ba_session->macAddress.a[5],
+                    CSR_RESULT_SUCCESS
+                 );
+    CsrWifiRouterCtrlBlockAckErrorIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,
+                    0,
+                    ba_session->interfacePriv->InterfaceTag,
+                    ba_session->tID,
+                    ba_session->macAddress,
+                    CSR_RESULT_SUCCESS);
+}
+
+
+static void ba_session_terminate_timer_func(unsigned long data)
+{
+    ba_session_rx_struct *ba_session = (ba_session_rx_struct*)data;
+    struct unifi_priv *priv;
+
+    if(!ba_session) {
+        return;
+    }
+
+    if(!ba_session->interfacePriv) {
+        return;
+    }
+
+    priv = ba_session->interfacePriv->privPtr;
+
+    if (ba_session->interfacePriv->InterfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "%s: invalid interfaceTag\n", __FUNCTION__);
+        return;
+    }
+
+    queue_work(priv->unifi_workqueue, &ba_session->send_ba_err_task);
+}
+
+
+u8 blockack_session_stop(unifi_priv_t *priv,
+                                     u16 interfaceTag,
+                                     CsrWifiRouterCtrlBlockAckRole role,
+                                     u16 tID,
+                                     CsrWifiMacAddress macAddress)
+{
+    netInterface_priv_t *interfacePriv;
+    ba_session_rx_struct *ba_session_rx = NULL;
+    ba_session_tx_struct *ba_session_tx = NULL;
+    u8 ba_session_idx = 0;
+    int i;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "%s: bad interfaceTag = %d\n", __FUNCTION__, interfaceTag);
+        return FALSE;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if(!interfacePriv) {
+        unifi_error(priv, "%s: bad interfacePriv\n", __FUNCTION__);
+        return FALSE;
+    }
+
+    if(tID > 15) {
+        unifi_error(priv, "%s: bad tID = %d\n", __FUNCTION__, tID);
+        return FALSE;
+    }
+
+    if((role != CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR) &&
+        (role != CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT)) {
+        unifi_error(priv, "%s: bad role = %d\n", __FUNCTION__, role);
+        return FALSE;
+        }
+
+	unifi_warning(priv,
+		"%s: stopping ba_session for peer = %pM role = %d tID = %d\n",
+		__func__, macAddress.a, role, tID);
+
+    /* find out the appropriate ba session (/station /tid /role) for which stop is requested */
+    if (role == CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT){
+        for (ba_session_idx =0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX; ba_session_idx++){
+
+            ba_session_rx = interfacePriv->ba_session_rx[ba_session_idx];
+
+            if(ba_session_rx){
+                if ((!memcmp(ba_session_rx->macAddress.a, macAddress.a, ETH_ALEN)) && (ba_session_rx->tID == tID)){
+                    break;
+                }
+            }
+        }
+
+        if (!ba_session_rx || (ba_session_idx == MAX_SUPPORTED_BA_SESSIONS_RX)) {
+            unifi_error(priv, "%s: bad ba_session for Rx [tID=%d]\n", __FUNCTION__, tID);
+            return FALSE;
+        }
+
+
+        if(ba_session_rx->timeout) {
+            del_timer_sync(&ba_session_rx->timer);
+        }
+        cancel_work_sync(&ba_session_rx->send_ba_err_task);
+        for (i = 0; i < ba_session_rx->wind_size; i++) {
+            if(ba_session_rx->buffer[i].active) {
+                frame_desc_struct *frame_desc = &ba_session_rx->buffer[i];
+                unifi_net_data_free(priv, &frame_desc->bulkdata.d[0]);
+            }
+        }
+        kfree(ba_session_rx->buffer);
+
+        interfacePriv->ba_session_rx[ba_session_idx] = NULL;
+        kfree(ba_session_rx);
+    }else if (role == CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR){
+        for (ba_session_idx =0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_TX; ba_session_idx++){
+        ba_session_tx = interfacePriv->ba_session_tx[ba_session_idx];
+            if(ba_session_tx){
+                if ((!memcmp(ba_session_tx->macAddress.a, macAddress.a, ETH_ALEN)) && (ba_session_tx->tID == tID)){
+                    break;
+                }
+            }
+        }
+
+        if (!ba_session_tx || (ba_session_idx == MAX_SUPPORTED_BA_SESSIONS_TX)) {
+            unifi_error(priv, "%s: bad ba_session for Tx [tID=%d]\n", __FUNCTION__, tID);
+            return FALSE;
+        }
+        interfacePriv->ba_session_tx[ba_session_idx] = NULL;
+        kfree(ba_session_tx);
+
+    }
+
+    return TRUE;
+}
+
+
+void CsrWifiRouterCtrlBlockAckDisableReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    CsrWifiRouterCtrlBlockAckDisableReq* req = (CsrWifiRouterCtrlBlockAckDisableReq*)msg;
+    u8 r;
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+
+    unifi_trace(priv, UDBG6, "%s: in ok\n", __FUNCTION__);
+
+    down(&priv->ba_mutex);
+    r = blockack_session_stop(priv,
+                              req->interfaceTag,
+                              req->role,
+                              req->trafficStreamID,
+                              req->macAddress);
+    up(&priv->ba_mutex);
+
+    CsrWifiRouterCtrlBlockAckDisableCfmSend(msg->source,
+                                            req->clientData,
+                                            req->interfaceTag,
+                                            r?CSR_RESULT_SUCCESS:CSR_RESULT_FAILURE);
+
+    unifi_trace(priv, UDBG6, "%s: out ok\n", __FUNCTION__);
+}
+
+
+u8 blockack_session_start(unifi_priv_t *priv,
+                               u16 interfaceTag,
+                               u16 tID,
+                               u16 timeout,
+                               CsrWifiRouterCtrlBlockAckRole role,
+                               u16 wind_size,
+                               u16 start_sn,
+                               CsrWifiMacAddress macAddress
+                              )
+{
+    netInterface_priv_t *interfacePriv;
+    ba_session_rx_struct *ba_session_rx = NULL;
+    ba_session_tx_struct *ba_session_tx = NULL;
+    u8 ba_session_idx = 0;
+
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "%s: bad interfaceTag = %d\n", __FUNCTION__, interfaceTag);
+        return FALSE;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if(!interfacePriv) {
+        unifi_error(priv, "%s: bad interfacePriv\n", __FUNCTION__);
+        return FALSE;
+    }
+
+    if(tID > 15)
+    {
+        unifi_error(priv, "%s: bad tID=%d\n", __FUNCTION__, tID);
+        return FALSE;
+    }
+
+    if(wind_size > MAX_BA_WIND_SIZE) {
+        unifi_error(priv, "%s: bad wind_size = %d\n", __FUNCTION__, wind_size);
+        return FALSE;
+    }
+
+    if(role != CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR &&
+       role != CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT) {
+        unifi_error(priv, "%s: bad role = %d\n", __FUNCTION__, role);
+        return FALSE;
+    }
+
+	unifi_warning(priv,
+		"%s: ba session with peer= (%pM)\n", __func__,
+		macAddress.a);
+
+    unifi_warning(priv, "%s: ba session for tID=%d timeout=%d role=%d wind_size=%d start_sn=%d\n", __FUNCTION__,
+                  tID,
+                  timeout,
+                  role,
+                  wind_size,
+                  start_sn);
+
+    /* Check if BA session exists for per station, per TID, per role or not.
+    if BA session exists update parameters and if it does not exist
+    create a new BA session */
+    if (role == CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR){
+        for (ba_session_idx =0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_TX; ba_session_idx++){
+            ba_session_tx = interfacePriv->ba_session_tx[ba_session_idx];
+            if (ba_session_tx) {
+                if ((!memcmp(ba_session_tx->macAddress.a, macAddress.a, ETH_ALEN)) && (ba_session_tx->tID == tID)){
+                    unifi_warning(priv, "%s: ba_session for Tx already exists\n", __FUNCTION__);
+                    return TRUE;
+                }
+            }
+        }
+
+        /* we have to create new ba_session_tx struct */
+         ba_session_tx = NULL;
+
+        /* loop through until an empty BA session slot is there and save the session there */
+        for (ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_TX ; ba_session_idx++){
+            if (!(interfacePriv->ba_session_tx[ba_session_idx])){
+                break;
+            }
+        }
+        if (ba_session_idx == MAX_SUPPORTED_BA_SESSIONS_TX){
+            unifi_error(priv, "%s: All ba_session used for Tx, NO free session available\n", __FUNCTION__);
+            return FALSE;
+        }
+
+        /* create and populate the new BA session structure */
+        ba_session_tx = kmalloc(sizeof(ba_session_tx_struct), GFP_KERNEL);
+        if (!ba_session_tx) {
+            unifi_error(priv, "%s: kmalloc failed for ba_session_tx\n", __FUNCTION__);
+            return FALSE;
+        }
+        memset(ba_session_tx, 0, sizeof(ba_session_tx_struct));
+
+        ba_session_tx->interfacePriv = interfacePriv;
+        ba_session_tx->tID = tID;
+        ba_session_tx->macAddress = macAddress;
+
+        interfacePriv->ba_session_tx[ba_session_idx] = ba_session_tx;
+
+    } else if (role == CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT){
+
+        for (ba_session_idx =0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX; ba_session_idx++){
+            ba_session_rx = interfacePriv->ba_session_rx[ba_session_idx];
+            if (ba_session_rx) {
+                if ((!memcmp(ba_session_rx->macAddress.a, macAddress.a, ETH_ALEN)) && (ba_session_rx->tID == tID)){
+                    unifi_warning(priv, "%s: ba_session for Rx[tID = %d] already exists\n", __FUNCTION__, tID);
+
+                    if(ba_session_rx->wind_size == wind_size &&
+                        ba_session_rx->timeout == timeout &&
+                        ba_session_rx->expected_sn == start_sn) {
+                        return TRUE;
+                    }
+
+                    if(ba_session_rx->timeout) {
+                        del_timer_sync(&ba_session_rx->timer);
+                        ba_session_rx->timeout = 0;
+                    }
+
+                    if(ba_session_rx->wind_size != wind_size) {
+                        blockack_session_stop(priv, interfaceTag, role, tID, macAddress);
+                    } else {
+                        if (timeout) {
+                            ba_session_rx->timeout = timeout;
+                            ba_session_rx->timer.function = ba_session_terminate_timer_func;
+                            ba_session_rx->timer.data = (unsigned long)ba_session_rx;
+                            init_timer(&ba_session_rx->timer);
+                            mod_timer(&ba_session_rx->timer, (jiffies + usecs_to_jiffies((ba_session_rx->timeout) * 1024)));
+                        }
+                        /*
+                         * The starting sequence number shall remain same if the BA
+                         * enable request is issued to update BA parameters only. If
+                         * it is not same, then we scroll our window to the new starting
+                         * sequence number. This could happen if the DELBA frame from
+                         * originator is lost and then we receive ADDBA frame with new SSN.
+                        */
+                        if(ba_session_rx->start_sn != start_sn) {
+                            scroll_ba_window(priv, interfacePriv, ba_session_rx, start_sn);
+                        }
+                        return TRUE;
+                    }
+                }
+            }
+        }
+
+        /* we could have a valid BA session pointer here or un-initialized
+        ba session pointer. but in any case we have to create a new session.
+        so re-initialize the ba_session pointer */
+        ba_session_rx = NULL;
+
+        /* loop through until an empty BA session slot is there and save the session there */
+        for (ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX ; ba_session_idx++){
+            if (!(interfacePriv->ba_session_rx[ba_session_idx])){
+                break;
+            }
+        }
+        if (ba_session_idx == MAX_SUPPORTED_BA_SESSIONS_RX){
+            unifi_error(priv, "%s: All ba_session used for Rx, NO free session available\n", __FUNCTION__);
+            return FALSE;
+        }
+
+        /* It is observed that with some devices there is a race between
+         * EAPOL exchanges and BA session establishment. This results in
+         * some EAPOL authentication packets getting stuck in BA reorder
+         * buffer and hence the conection cannot be established. To avoid
+         * this we check here if the EAPOL authentication is complete and
+         * if so then only allow the BA session to establish.
+         *
+         * It is verified that the peers normally re-establish
+         * the BA session after the initial rejection.
+         */
+        if (CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN != uf_sme_port_state(priv, macAddress.a, UF_CONTROLLED_PORT_Q, interfacePriv->InterfaceTag))
+        {
+            unifi_warning(priv, "blockack_session_start: Controlled port not opened, Reject BA request\n");
+            return FALSE;
+        }
+
+        ba_session_rx = kmalloc(sizeof(ba_session_rx_struct), GFP_KERNEL);
+        if (!ba_session_rx) {
+            unifi_error(priv, "%s: kmalloc failed for ba_session_rx\n", __FUNCTION__);
+            return FALSE;
+        }
+        memset(ba_session_rx, 0, sizeof(ba_session_rx_struct));
+
+        ba_session_rx->wind_size = wind_size;
+        ba_session_rx->start_sn = ba_session_rx->expected_sn = start_sn;
+        ba_session_rx->trigger_ba_after_ssn = FALSE;
+
+        ba_session_rx->buffer = kmalloc(ba_session_rx->wind_size*sizeof(frame_desc_struct), GFP_KERNEL);
+        if (!ba_session_rx->buffer) {
+            kfree(ba_session_rx);
+            unifi_error(priv, "%s: kmalloc failed for buffer\n", __FUNCTION__);
+            return FALSE;
+        }
+
+        memset(ba_session_rx->buffer, 0, ba_session_rx->wind_size*sizeof(frame_desc_struct));
+
+        INIT_WORK(&ba_session_rx->send_ba_err_task, uf_send_ba_err_wq);
+        if (timeout) {
+            ba_session_rx->timeout = timeout;
+            ba_session_rx->timer.function = ba_session_terminate_timer_func;
+            ba_session_rx->timer.data = (unsigned long)ba_session_rx;
+            init_timer(&ba_session_rx->timer);
+            mod_timer(&ba_session_rx->timer, (jiffies + usecs_to_jiffies((ba_session_rx->timeout) * 1024)));
+        }
+
+        ba_session_rx->interfacePriv = interfacePriv;
+        ba_session_rx->tID = tID;
+        ba_session_rx->macAddress = macAddress;
+
+        interfacePriv->ba_session_rx[ba_session_idx] = ba_session_rx;
+    }
+    return TRUE;
+}
+
+void CsrWifiRouterCtrlBlockAckEnableReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+    CsrWifiRouterCtrlBlockAckEnableReq* req = (CsrWifiRouterCtrlBlockAckEnableReq*)msg;
+    u8 r;
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+
+    unifi_trace(priv, UDBG6, ">>%s\n", __FUNCTION__);
+    down(&priv->ba_mutex);
+    r = blockack_session_start(priv,
+                               req->interfaceTag,
+                               req->trafficStreamID,
+                               req->timeout,
+                               req->role,
+                               req->bufferSize,
+                               req->ssn,
+                               req->macAddress
+                              );
+    up(&priv->ba_mutex);
+
+    CsrWifiRouterCtrlBlockAckEnableCfmSend(msg->source,
+                                           req->clientData,
+                                           req->interfaceTag,
+                                           r?CSR_RESULT_SUCCESS:CSR_RESULT_FAILURE);
+    unifi_trace(priv, UDBG6, "<<%s: r=%d\n", __FUNCTION__, r);
+
+}
+
+void CsrWifiRouterCtrlWapiMulticastFilterReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlWapiMulticastFilterReq* req = (CsrWifiRouterCtrlWapiMulticastFilterReq*)msg;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    if (CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) {
+
+        unifi_trace(priv, UDBG6, ">>%s\n", __FUNCTION__);
+
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWapiMulticastFilterReq: req->status = %d\n", req->status);
+
+        /* status 1 - Filter on
+        * status 0 - Filter off */
+        priv->wapi_multicast_filter = req->status;
+
+        unifi_trace(priv, UDBG6, "<<%s\n", __FUNCTION__);
+    } else {
+
+    	unifi_warning(priv, "%s is NOT applicable for interface mode - %d\n", __FUNCTION__,interfacePriv->interfaceMode);
+
+    }
+#elif defined(UNIFI_DEBUG)
+    /*WAPI Disabled*/
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    unifi_error(priv,"CsrWifiRouterCtrlWapiMulticastFilterReqHandler: called when WAPI isn't enabled\n");
+#endif
+}
+
+void CsrWifiRouterCtrlWapiUnicastFilterReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlWapiUnicastFilterReq* req = (CsrWifiRouterCtrlWapiUnicastFilterReq*)msg;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    if (CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) {
+
+        unifi_trace(priv, UDBG6, ">>%s\n", __FUNCTION__);
+
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWapiUnicastFilterReq: req->status= %d\n", req->status);
+
+        if ((priv->wapi_unicast_filter == 1) && (req->status == 0)) {
+            /* When we have successfully re-associated and obtained a new unicast key with keyid = 0 */
+            priv->wapi_unicast_queued_pkt_filter = 1;
+        }
+
+        /* status 1 - Filter ON
+         * status 0 - Filter OFF */
+        priv->wapi_unicast_filter = req->status;
+
+        unifi_trace(priv, UDBG6, "<<%s\n", __FUNCTION__);
+    } else {
+
+    	 unifi_warning(priv, "%s is NOT applicable for interface mode - %d\n", __FUNCTION__,interfacePriv->interfaceMode);
+
+    }
+#elif defined(UNIFI_DEBUG)
+    /*WAPI Disabled*/
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    unifi_error(priv,"CsrWifiRouterCtrlWapiUnicastFilterReqHandler: called when WAPI isn't enabled\n");
+#endif
+}
+
+void CsrWifiRouterCtrlWapiRxPktReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlWapiRxPktReq* req =  (CsrWifiRouterCtrlWapiRxPktReq*)msg;
+    int client_id, receiver_id;
+    bulk_data_param_t bulkdata;
+    CsrResult res;
+    ul_client_t *client;
+    CSR_SIGNAL signal;
+    CSR_MA_PACKET_INDICATION *pkt_ind;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    if (CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) {
+
+    	unifi_trace(priv, UDBG6, ">>%s\n", __FUNCTION__);
+
+        if (priv == NULL) {
+            unifi_error(priv, "CsrWifiRouterCtrlWapiRxPktReq : invalid priv\n",__FUNCTION__);
+            return;
+        }
+
+        if (priv->smepriv == NULL) {
+             unifi_error(priv, "CsrWifiRouterCtrlWapiRxPktReq : invalid sme priv\n",__FUNCTION__);
+             return;
+        }
+
+        if (req->dataLength == 0 || req->data == NULL) {
+             unifi_error(priv, "CsrWifiRouterCtrlWapiRxPktReq: invalid request\n",__FUNCTION__);
+             return;
+        }
+
+        res = unifi_net_data_malloc(priv, &bulkdata.d[0], req->dataLength);
+        if (res != CSR_RESULT_SUCCESS) {
+             unifi_error(priv, "CsrWifiRouterCtrlWapiRxPktReq: Could not allocate net data\n",__FUNCTION__);
+             return;
+        }
+
+        /* This function is expected to be called only when the MIC has been verified by SME to be correct
+         * So reset the reception status to rx_success */
+        res = read_unpack_signal(req->signal, &signal);
+        if (res) {
+	          unifi_error(priv,"CsrWifiRouterCtrlWapiRxPktReqHandler: Received unknown or corrupted signal.\n");
+	          return;
+        }
+        pkt_ind = (CSR_MA_PACKET_INDICATION*) (&((&signal)->u).MaPacketIndication);
+        if (pkt_ind->ReceptionStatus != CSR_MICHAEL_MIC_ERROR) {
+	          unifi_error(priv,"CsrWifiRouterCtrlWapiRxPktReqHandler: Unknown signal with reception status = %d\n",pkt_ind->ReceptionStatus);
+	          return;
+        } else {
+	          unifi_trace(priv, UDBG4,"CsrWifiRouterCtrlWapiRxPktReqHandler: MIC verified , RX_SUCCESS \n",__FUNCTION__);
+	          pkt_ind->ReceptionStatus = CSR_RX_SUCCESS;
+	          write_pack(&signal, req->signal, &(req->signalLength));
+        }
+
+        memcpy((void*)bulkdata.d[0].os_data_ptr, req->data, req->dataLength);
+
+        receiver_id = CSR_GET_UINT16_FROM_LITTLE_ENDIAN((req->signal) + sizeof(s16)) & 0xFFF0;
+        client_id = (receiver_id & 0x0F00) >> UDI_SENDER_ID_SHIFT;
+
+        client = &priv->ul_clients[client_id];
+
+        if (client && client->event_hook) {
+              unifi_trace(priv, UDBG3,
+                          "CsrWifiRouterCtrlWapiRxPktReq: "
+                          "Sending signal to client %d, (s:0x%X, r:0x%X) - Signal 0x%X \n",
+                          client->client_id, client->sender_id, receiver_id,
+                          CSR_GET_UINT16_FROM_LITTLE_ENDIAN(req->signal));
+
+              client->event_hook(client, req->signal, req->signalLength, &bulkdata, UDI_TO_HOST);
+        } else {
+              unifi_trace(priv, UDBG4, "No client to give the packet to\n");
+              unifi_net_data_free(priv, &bulkdata.d[0]);
+        }
+
+        unifi_trace(priv, UDBG6, "<<%s\n", __FUNCTION__);
+    } else {
+    	unifi_warning(priv, "%s is NOT applicable for interface mode - %d\n", __FUNCTION__,interfacePriv->interfaceMode);
+    }
+#elif defined(UNIFI_DEBUG)
+    /*WAPI Disabled*/
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    unifi_error(priv,"CsrWifiRouterCtrlWapiRxPktReqHandler: called when WAPI isn't enabled\n");
+#endif
+}
+
+void CsrWifiRouterCtrlWapiUnicastTxPktReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+
+	unifi_priv_t *priv = (unifi_priv_t*) drvpriv;
+    CsrWifiRouterCtrlWapiUnicastTxPktReq *req 	= (CsrWifiRouterCtrlWapiUnicastTxPktReq*) msg;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+    bulk_data_param_t bulkdata;
+    u8 macHeaderLengthInBytes = MAC_HEADER_SIZE;
+    /*KeyID, Reserved, PN, MIC*/
+    u8 appendedCryptoFields = 1 + 1 + 16 + 16;
+    CsrResult result;
+    /* Retrieve the MA PACKET REQ fields from the Signal retained from send_ma_pkt_request() */
+    CSR_MA_PACKET_REQUEST *storedSignalMAPktReq = &interfacePriv->wapi_unicast_ma_pkt_sig.u.MaPacketRequest;
+
+    if (CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) {
+
+        unifi_trace(priv, UDBG6, ">>%s\n", __FUNCTION__);
+
+        if (priv == NULL) {
+            unifi_error(priv, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler : invalid priv\n",__FUNCTION__);
+            return;
+        }
+        if (priv->smepriv == NULL) {
+            unifi_error(priv, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler : invalid sme priv\n",__FUNCTION__);
+            return;
+        }
+        if (req->data == NULL) {
+            unifi_error(priv, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler: invalid request\n",__FUNCTION__);
+            return;
+        } else {
+            /* If it is QoS data (type = data subtype = QoS), frame header contains QoS control field */
+            if ((req->data[0] & 0x88) == 0x88) {
+      	        macHeaderLengthInBytes  = macHeaderLengthInBytes + QOS_CONTROL_HEADER_SIZE;
+            }
+        }
+        if ( !(req->dataLength>(macHeaderLengthInBytes+appendedCryptoFields)) ) {
+            unifi_error(priv, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler: invalid dataLength\n",__FUNCTION__);
+            return;
+        }
+
+	    /* Encrypted DATA Packet contained in (req->data)
+         * -------------------------------------------------------------------
+         * |MAC Header|  KeyId   | Reserved |    PN    | xxDataxx | xxMICxxx |
+         * -------------------------------------------------------------------
+         *                                             (<-----Encrypted----->)
+         * -------------------------------------------------------------------
+         * |24/26(QoS)|    1     |    1     |    16    |    x     |    16    |
+         * -------------------------------------------------------------------
+         */
+        result = unifi_net_data_malloc(priv, &bulkdata.d[0], req->dataLength);
+        if (result != CSR_RESULT_SUCCESS) {
+             unifi_error(priv, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler: Could not allocate net data\n",__FUNCTION__);
+             return;
+        }
+        memcpy((void*)bulkdata.d[0].os_data_ptr, req->data, req->dataLength);
+        bulkdata.d[0].data_length = req->dataLength;
+        bulkdata.d[1].os_data_ptr = NULL;
+        bulkdata.d[1].data_length = 0;
+
+        /* Send UniFi msg */
+        /* Here hostTag is been sent as 0xffffffff, its been appended properly while framing MA-Packet request in pdu_processing.c file */
+        result = uf_process_ma_packet_req(priv,
+    	                                  storedSignalMAPktReq->Ra.x,
+                                          storedSignalMAPktReq->HostTag,/* Ask for a new HostTag */
+                                          req->interfaceTag,
+                                          storedSignalMAPktReq->TransmissionControl,
+                                          storedSignalMAPktReq->TransmitRate,
+                                          storedSignalMAPktReq->Priority, /* Retained value */
+                                          interfacePriv->wapi_unicast_ma_pkt_sig.SignalPrimitiveHeader.SenderProcessId, /*FIXME AP: VALIDATE ???*/
+                                          &bulkdata);
+
+        if (result == NETDEV_TX_OK) {
+             (priv->netdev[req->interfaceTag])->trans_start = jiffies;
+             /* Should really count tx stats in the UNITDATA.status signal but
+              * that doesn't have the length.
+              */
+             interfacePriv->stats.tx_packets++;
+
+             /* count only the packet payload */
+             interfacePriv->stats.tx_bytes += req->dataLength - macHeaderLengthInBytes - appendedCryptoFields;
+             unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler: (Packet Sent), sent count = %x\n", interfacePriv->stats.tx_packets);
+        } else {
+             /* Failed to send: fh queue was full, and the skb was discarded*/
+             unifi_trace(priv, UDBG1, "(HIP validation failure) Result = %d\n", result);
+             unifi_net_data_free(priv, &bulkdata.d[0]);
+
+             interfacePriv->stats.tx_dropped++;
+             unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWapiUnicastTxPktReqHandler: (Packet Drop), dropped count = %x\n", interfacePriv->stats.tx_dropped);
+        }
+
+        unifi_trace(priv, UDBG6, "<<%s\n", __FUNCTION__);
+
+    } else {
+
+    	unifi_warning(priv, "%s is NOT applicable for interface mode - %d\n", __FUNCTION__,interfacePriv->interfaceMode);
+
+    }
+#elif defined(UNIFI_DEBUG)
+    /*WAPI Disabled*/
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    unifi_error(priv,"CsrWifiRouterCtrlWapiUnicastTxPktReqHandler: called when WAPI SW ENCRYPTION isn't enabled\n");
+#endif
+}
+
+void CsrWifiRouterCtrlWapiFilterReqHandler(void* drvpriv, CsrWifiFsmEvent* msg)
+{
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+
+#ifdef CSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    CsrWifiRouterCtrlWapiFilterReq* req = (CsrWifiRouterCtrlWapiFilterReq*)msg;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[req->interfaceTag];
+
+    if (CSR_WIFI_ROUTER_CTRL_MODE_STA == interfacePriv->interfaceMode) {
+
+        unifi_trace(priv, UDBG6, ">>%s\n", __FUNCTION__);
+
+        unifi_trace(priv, UDBG1, "CsrWifiRouterCtrlWapiFilterReq: req->isWapiConnected [0/1] = %d \n",req->isWapiConnected);
+
+        priv->isWapiConnection = req->isWapiConnected;
+
+        unifi_trace(priv, UDBG6, "<<%s\n", __FUNCTION__);
+    } else {
+
+    	unifi_warning(priv, "%s is NOT applicable for interface mode - %d\n", __FUNCTION__,interfacePriv->interfaceMode);
+
+    }
+#endif
+
+#elif defined(UNIFI_DEBUG)
+    /*WAPI Disabled*/
+    unifi_priv_t *priv = (unifi_priv_t*)drvpriv;
+    unifi_error(priv,"CsrWifiRouterCtrlWapiFilterReq: called when WAPI isn't enabled\n");
+#endif
+}
diff --git a/drivers/staging/csr/sme_userspace.c b/drivers/staging/csr/sme_userspace.c
new file mode 100644
index 00000000000..abcb446fb8c
--- /dev/null
+++ b/drivers/staging/csr/sme_userspace.c
@@ -0,0 +1,315 @@
+/*
+ *****************************************************************************
+ *
+ * FILE : sme_userspace.c
+ *
+ * PURPOSE : Support functions for userspace SME helper application.
+ *
+ *
+ * Copyright (C) 2008-2011 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ *****************************************************************************
+ */
+
+#include "unifi_priv.h"
+
+/*
+ * Fix Me..... These need to be the correct values...
+ * Dynamic from the user space.
+ */
+CsrSchedQid CSR_WIFI_ROUTER_IFACEQUEUE   = 0xFFFF;
+CsrSchedQid CSR_WIFI_SME_IFACEQUEUE      = 0xFFFF;
+#ifdef CSR_SUPPORT_WEXT_AP
+CsrSchedQid CSR_WIFI_NME_IFACEQUEUE      = 0xFFFF;
+#endif
+int
+uf_sme_init(unifi_priv_t *priv)
+{
+    int i, j;
+
+    CsrWifiRouterTransportInit(priv);
+
+    priv->smepriv = priv;
+
+    init_waitqueue_head(&priv->sme_request_wq);
+
+    priv->filter_tclas_ies = NULL;
+    memset(&priv->packet_filters, 0, sizeof(uf_cfg_bcast_packet_filter_t));
+
+#ifdef CSR_SUPPORT_WEXT
+    priv->ignore_bssid_join = FALSE;
+    priv->mib_data.length = 0;
+
+    uf_sme_wext_set_defaults(priv);
+#endif /* CSR_SUPPORT_WEXT*/
+
+    priv->sta_ip_address = 0xFFFFFFFF;
+
+    priv->wifi_on_state = wifi_on_unspecified;
+
+    sema_init(&priv->sme_sem, 1);
+    memset(&priv->sme_reply, 0, sizeof(sme_reply_t));
+
+    priv->ta_ind_work.in_use = 0;
+    priv->ta_sample_ind_work.in_use = 0;
+
+    priv->CSR_WIFI_SME_IFACEQUEUE = 0xFFFF;
+
+    for (i = 0; i < MAX_MA_UNIDATA_IND_FILTERS; i++) {
+        priv->sme_unidata_ind_filters[i].in_use = 0;
+    }
+
+    /* Create a work queue item for Traffic Analysis indications to SME */
+    INIT_WORK(&priv->ta_ind_work.task, uf_ta_ind_wq);
+    INIT_WORK(&priv->ta_sample_ind_work.task, uf_ta_sample_ind_wq);
+#ifdef CSR_SUPPORT_WEXT
+    INIT_WORK(&priv->sme_config_task, uf_sme_config_wq);
+#endif
+
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        netInterface_priv_t *interfacePriv = priv->interfacePriv[i];
+        interfacePriv->m4_sent = FALSE;
+        interfacePriv->m4_bulk_data.net_buf_length = 0;
+        interfacePriv->m4_bulk_data.data_length = 0;
+        interfacePriv->m4_bulk_data.os_data_ptr = interfacePriv->m4_bulk_data.os_net_buf_ptr = NULL;
+
+        memset(&interfacePriv->controlled_data_port, 0, sizeof(unifi_port_config_t));
+        interfacePriv->controlled_data_port.entries_in_use = 1;
+        interfacePriv->controlled_data_port.port_cfg[0].in_use = TRUE;
+        interfacePriv->controlled_data_port.port_cfg[0].port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+        interfacePriv->controlled_data_port.overide_action = UF_DATA_PORT_OVERIDE;
+
+        memset(&interfacePriv->uncontrolled_data_port, 0, sizeof(unifi_port_config_t));
+        interfacePriv->uncontrolled_data_port.entries_in_use = 1;
+        interfacePriv->uncontrolled_data_port.port_cfg[0].in_use = TRUE;
+        interfacePriv->uncontrolled_data_port.port_cfg[0].port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+        interfacePriv->uncontrolled_data_port.overide_action = UF_DATA_PORT_OVERIDE;
+
+        /* Mark the remainder of the port config table as unallocated */
+        for(j = 1; j < UNIFI_MAX_CONNECTIONS; j++) {
+            interfacePriv->controlled_data_port.port_cfg[j].in_use = FALSE;
+            interfacePriv->controlled_data_port.port_cfg[j].port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+
+            interfacePriv->uncontrolled_data_port.port_cfg[j].in_use = FALSE;
+            interfacePriv->uncontrolled_data_port.port_cfg[j].port_action = CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+        }
+
+        /* intializing the lists */
+        INIT_LIST_HEAD(&interfacePriv->genericMgtFrames);
+        INIT_LIST_HEAD(&interfacePriv->genericMulticastOrBroadCastMgtFrames);
+        INIT_LIST_HEAD(&interfacePriv->genericMulticastOrBroadCastFrames);
+
+        for(j = 0; j < UNIFI_MAX_CONNECTIONS; j++) {
+            interfacePriv->staInfo[j] = NULL;
+        }
+
+        interfacePriv->num_stations_joined = 0;
+        interfacePriv->sta_activity_check_enabled = FALSE;
+    }
+
+
+    return 0;
+} /* uf_sme_init() */
+
+
+void
+uf_sme_deinit(unifi_priv_t *priv)
+{
+    int i,j;
+    u8 ba_session_idx;
+    ba_session_rx_struct *ba_session_rx = NULL;
+    ba_session_tx_struct *ba_session_tx = NULL;
+    CsrWifiRouterCtrlStaInfo_t *staInfo = NULL;
+    netInterface_priv_t *interfacePriv = NULL;
+
+    /* Free any TCLASs previously allocated */
+    if (priv->packet_filters.tclas_ies_length) {
+        priv->packet_filters.tclas_ies_length = 0;
+        kfree(priv->filter_tclas_ies);
+        priv->filter_tclas_ies = NULL;
+    }
+
+    for (i = 0; i < MAX_MA_UNIDATA_IND_FILTERS; i++) {
+        priv->sme_unidata_ind_filters[i].in_use = 0;
+    }
+
+    /* Remove all the Peer database, before going down */
+    for (i = 0; i < CSR_WIFI_NUM_INTERFACES; i++) {
+        down(&priv->ba_mutex);
+        for(ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_RX; ba_session_idx++){
+            ba_session_rx = priv->interfacePriv[i]->ba_session_rx[ba_session_idx];
+            if(ba_session_rx) {
+                blockack_session_stop(priv,
+                                    i,
+                                    CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_RECIPIENT,
+                                    ba_session_rx->tID,
+                                    ba_session_rx->macAddress);
+            }
+        }
+        for(ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_TX; ba_session_idx++){
+            ba_session_tx = priv->interfacePriv[i]->ba_session_tx[ba_session_idx];
+            if(ba_session_tx) {
+                blockack_session_stop(priv,
+                                    i,
+                                    CSR_WIFI_ROUTER_CTRL_BLOCK_ACK_ORIGINATOR,
+                                    ba_session_tx->tID,
+                                    ba_session_tx->macAddress);
+            }
+        }
+
+        up(&priv->ba_mutex);
+        interfacePriv = priv->interfacePriv[i];
+        if(interfacePriv){
+            for(j = 0; j < UNIFI_MAX_CONNECTIONS; j++) {
+                if ((staInfo=interfacePriv->staInfo[j]) != NULL) {
+                    /* Clear the STA activity parameters before freeing station Record */
+                    unifi_trace(priv, UDBG1, "uf_sme_deinit: Canceling work queue for STA with AID: %d\n", staInfo->aid);
+                    cancel_work_sync(&staInfo->send_disconnected_ind_task);
+                    staInfo->nullDataHostTag = INVALID_HOST_TAG;
+                }
+            }
+            if (interfacePriv->sta_activity_check_enabled){
+                interfacePriv->sta_activity_check_enabled = FALSE;
+                del_timer_sync(&interfacePriv->sta_activity_check_timer);
+            }
+        }
+        CsrWifiRouterCtrlInterfaceReset(priv, i);
+        priv->interfacePriv[i]->interfaceMode = CSR_WIFI_ROUTER_CTRL_MODE_NONE;
+    }
+
+
+} /* uf_sme_deinit() */
+
+
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_ta_indicate_protocol
+ *
+ *      Report that a packet of a particular type has been seen
+ *
+ *  Arguments:
+ *      drv_priv        The device context pointer passed to ta_init.
+ *      protocol        The protocol type enum value.
+ *      direction       Whether the packet was a tx or rx.
+ *      src_addr        The source MAC address from the data packet.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      We defer the actual sending to a background workqueue,
+ *      see uf_ta_ind_wq().
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_ta_indicate_protocol(void *ospriv,
+                           CsrWifiRouterCtrlTrafficPacketType packet_type,
+                           CsrWifiRouterCtrlProtocolDirection direction,
+                           const CsrWifiMacAddress *src_addr)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+
+    if (priv->ta_ind_work.in_use) {
+        unifi_warning(priv,
+                "unifi_ta_indicate_protocol: workqueue item still in use, not sending\n");
+        return;
+    }
+
+    if (CSR_WIFI_ROUTER_CTRL_PROTOCOL_DIRECTION_RX == direction)
+    {
+        u16 interfaceTag = 0;
+        CsrWifiRouterCtrlTrafficProtocolIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,
+                interfaceTag,
+                packet_type,
+                direction,
+                *src_addr);
+    }
+    else
+    {
+        priv->ta_ind_work.packet_type = packet_type;
+        priv->ta_ind_work.direction = direction;
+        priv->ta_ind_work.src_addr = *src_addr;
+
+        queue_work(priv->unifi_workqueue, &priv->ta_ind_work.task);
+    }
+
+} /* unifi_ta_indicate_protocol() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_ta_indicate_sampling
+ *
+ *      Send the TA sampling information to the SME.
+ *
+ *  Arguments:
+ *      drv_priv        The device context pointer passed to ta_init.
+ *      stats   The TA sampling data to send.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_ta_indicate_sampling(void *ospriv, CsrWifiRouterCtrlTrafficStats *stats)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+
+    if (!priv) {
+        return;
+    }
+
+    if (priv->ta_sample_ind_work.in_use) {
+        unifi_warning(priv,
+                     "unifi_ta_indicate_sampling: workqueue item still in use, not sending\n");
+        return;
+    }
+
+    priv->ta_sample_ind_work.stats = *stats;
+
+    queue_work(priv->unifi_workqueue, &priv->ta_sample_ind_work.task);
+
+} /* unifi_ta_indicate_sampling() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * unifi_ta_indicate_l4stats
+ *
+ *      Send the TA TCP/UDP throughput information to the driver.
+ *
+ *  Arguments:
+ *    drv_priv        The device context pointer passed to ta_init.
+ *    rxTcpThroughput TCP RX throughput in KiloBytes
+ *    txTcpThroughput TCP TX throughput in KiloBytes
+ *    rxUdpThroughput UDP RX throughput in KiloBytes
+ *    txUdpThroughput UDP TX throughput in KiloBytes
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_ta_indicate_l4stats(void *ospriv,
+                          u32 rxTcpThroughput,
+                          u32 txTcpThroughput,
+                          u32 rxUdpThroughput,
+                          u32 txUdpThroughput)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+
+    if (!priv) {
+        return;
+    }
+    /* Save the info. The actual action will be taken in unifi_ta_indicate_sampling() */
+    priv->rxTcpThroughput = rxTcpThroughput;
+    priv->txTcpThroughput = txTcpThroughput;
+    priv->rxUdpThroughput = rxUdpThroughput;
+    priv->txUdpThroughput = txUdpThroughput;
+} /* unifi_ta_indicate_l4stats() */
diff --git a/drivers/staging/csr/sme_userspace.h b/drivers/staging/csr/sme_userspace.h
new file mode 100644
index 00000000000..7816b15b4b5
--- /dev/null
+++ b/drivers/staging/csr/sme_userspace.h
@@ -0,0 +1,38 @@
+/*
+ * ***************************************************************************
+ *  FILE:     sme_userspace.h
+ *
+ *  PURPOSE:    SME related definitions.
+ *
+ *  Copyright (C) 2007-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+#ifndef __LINUX_SME_USERSPACE_H__
+#define __LINUX_SME_USERSPACE_H__ 1
+
+#include <linux/kernel.h>
+
+int uf_sme_init(unifi_priv_t *priv);
+void uf_sme_deinit(unifi_priv_t *priv);
+int uf_sme_queue_message(unifi_priv_t *priv, u8 *buffer, int length);
+
+
+#include "csr_wifi_router_lib.h"
+#include "csr_wifi_router_sef.h"
+#include "csr_wifi_router_ctrl_lib.h"
+#include "csr_wifi_router_ctrl_sef.h"
+#include "csr_wifi_sme_task.h"
+#ifdef CSR_SUPPORT_WEXT_AP
+#include "csr_wifi_nme_ap_lib.h"
+#endif
+#include "csr_wifi_sme_lib.h"
+
+void CsrWifiRouterTransportInit(unifi_priv_t *priv);
+void CsrWifiRouterTransportRecv(unifi_priv_t *priv, u8* buffer, size_t bufferLength);
+void CsrWifiRouterTransportDeInit(unifi_priv_t *priv);
+
+#endif /* __LINUX_SME_USERSPACE_H__ */
diff --git a/drivers/staging/csr/sme_wext.c b/drivers/staging/csr/sme_wext.c
new file mode 100644
index 00000000000..7e85907e29a
--- /dev/null
+++ b/drivers/staging/csr/sme_wext.c
@@ -0,0 +1,3381 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     sme_wext.c
+ *
+ * PURPOSE:
+ *      Handlers for ioctls from iwconfig.
+ *      These provide the control plane operations.
+ *
+ * Copyright (C) 2007-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+#include <linux/ctype.h>
+#include "unifi_priv.h"
+#include <linux/rtnetlink.h>
+
+#define CHECK_INITED(_priv)                             \
+    do {                                                    \
+        if (_priv->init_progress != UNIFI_INIT_COMPLETED) { \
+            unifi_trace(_priv, UDBG2, "%s unifi not ready, failing wext call\n", __FUNCTION__); \
+            return -ENODEV;                                 \
+        }                                                   \
+    } while (0)
+
+/* Workaround for the wpa_supplicant hanging issue - disabled on Android */
+#ifndef ANDROID_BUILD
+#define CSR_WIFI_WEXT_HANG_WORKAROUND
+#endif
+
+#ifdef CSR_WIFI_WEXT_HANG_WORKAROUND
+# define UF_RTNL_LOCK()    rtnl_lock()
+# define UF_RTNL_UNLOCK()  rtnl_unlock()
+#else
+# define UF_RTNL_LOCK()
+# define UF_RTNL_UNLOCK()
+#endif
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *      Helper functions
+ * ---------------------------------------------------------------------------
+ */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  wext_freq_to_channel
+ *  channel_to_mhz
+ *
+ *      These functions convert between channel number and frequency.
+ *
+ *  Arguments:
+ *      ch      Channel number, as defined in 802.11 specs
+ *      m, e    Mantissa and exponent as provided by wireless extension.
+ *
+ *  Returns:
+ *      channel or frequency (in MHz) value
+ * ---------------------------------------------------------------------------
+ */
+static int
+wext_freq_to_channel(int m, int e)
+{
+    int mhz;
+
+    mhz = m;
+    while (e < 6) {
+        mhz /= 10;
+        e++;
+    }
+    while (e > 6) {
+        mhz *= 10;
+        e--;
+    }
+
+    if (mhz >= 5000) {
+        return ((mhz - 5000) / 5);
+    }
+
+    if (mhz == 2482) {
+        return 14;
+    }
+
+    if (mhz >= 2407) {
+        return ((mhz - 2407) / 5);
+    }
+
+    return 0;
+} /* wext_freq_to_channel() */
+
+static int
+channel_to_mhz(int ch, int dot11a)
+{
+
+    if (ch == 0) return 0;
+    if (ch > 200) return 0;
+
+    /* 5G */
+    if (dot11a) {
+        return (5000 + (5 * ch));
+    }
+
+    /* 2.4G */
+    if (ch == 14) {
+        return 2484;
+    }
+
+    if ((ch < 14) && (ch > 0)) {
+        return (2407 + (5 * ch));
+    }
+
+    return 0;
+}
+#ifdef CSR_SUPPORT_WEXT_AP
+void uf_sme_wext_ap_set_defaults(unifi_priv_t *priv)
+{
+    memcpy(priv->ap_config.ssid.ssid,"defaultssid",sizeof("defaultssid"));
+
+    priv->ap_config.ssid.length = 8;
+    priv->ap_config.channel = 6;
+    priv->ap_config.if_index = 1;
+    priv->ap_config.credentials.authType = 0;
+    priv->ap_config.max_connections=8;
+
+    priv->group_sec_config.apGroupkeyTimeout = 0;
+    priv->group_sec_config.apStrictGtkRekey = 0;
+    priv->group_sec_config.apGmkTimeout = 0;
+    priv->group_sec_config.apResponseTimeout = 100; /* Default*/
+    priv->group_sec_config.apRetransLimit = 3; /* Default*/
+    /* Set default params even if they may not be used*/
+    /* Until Here*/
+
+    priv->ap_mac_config.preamble = CSR_WIFI_SME_USE_LONG_PREAMBLE;
+    priv->ap_mac_config.shortSlotTimeEnabled = FALSE;
+    priv->ap_mac_config.ctsProtectionType=CSR_WIFI_SME_CTS_PROTECTION_AUTOMATIC;
+
+    priv->ap_mac_config.wmmEnabled = TRUE;
+    priv->ap_mac_config.wmmApParams[0].cwMin=4;
+    priv->ap_mac_config.wmmApParams[0].cwMax=10;
+    priv->ap_mac_config.wmmApParams[0].aifs=3;
+    priv->ap_mac_config.wmmApParams[0].txopLimit=0;
+    priv->ap_mac_config.wmmApParams[0].admissionControlMandatory=FALSE;
+    priv->ap_mac_config.wmmApParams[1].cwMin=4;
+    priv->ap_mac_config.wmmApParams[1].cwMax=10;
+    priv->ap_mac_config.wmmApParams[1].aifs=7;
+    priv->ap_mac_config.wmmApParams[1].txopLimit=0;
+    priv->ap_mac_config.wmmApParams[1].admissionControlMandatory=FALSE;
+    priv->ap_mac_config.wmmApParams[2].cwMin=3;
+    priv->ap_mac_config.wmmApParams[2].cwMax=4;
+    priv->ap_mac_config.wmmApParams[2].aifs=1;
+    priv->ap_mac_config.wmmApParams[2].txopLimit=94;
+    priv->ap_mac_config.wmmApParams[2].admissionControlMandatory=FALSE;
+    priv->ap_mac_config.wmmApParams[3].cwMin=2;
+    priv->ap_mac_config.wmmApParams[3].cwMax=3;
+    priv->ap_mac_config.wmmApParams[3].aifs=1;
+    priv->ap_mac_config.wmmApParams[3].txopLimit=47;
+    priv->ap_mac_config.wmmApParams[3].admissionControlMandatory=FALSE;
+
+    priv->ap_mac_config.wmmApBcParams[0].cwMin=4;
+    priv->ap_mac_config.wmmApBcParams[0].cwMax=10;
+    priv->ap_mac_config.wmmApBcParams[0].aifs=3;
+    priv->ap_mac_config.wmmApBcParams[0].txopLimit=0;
+    priv->ap_mac_config.wmmApBcParams[0].admissionControlMandatory=FALSE;
+    priv->ap_mac_config.wmmApBcParams[1].cwMin=4;
+    priv->ap_mac_config.wmmApBcParams[1].cwMax=10;
+    priv->ap_mac_config.wmmApBcParams[1].aifs=7;
+    priv->ap_mac_config.wmmApBcParams[1].txopLimit=0;
+    priv->ap_mac_config.wmmApBcParams[1].admissionControlMandatory=FALSE;
+    priv->ap_mac_config.wmmApBcParams[2].cwMin=3;
+    priv->ap_mac_config.wmmApBcParams[2].cwMax=4;
+    priv->ap_mac_config.wmmApBcParams[2].aifs=2;
+    priv->ap_mac_config.wmmApBcParams[2].txopLimit=94;
+    priv->ap_mac_config.wmmApBcParams[2].admissionControlMandatory=FALSE;
+    priv->ap_mac_config.wmmApBcParams[3].cwMin=2;
+    priv->ap_mac_config.wmmApBcParams[3].cwMax=3;
+    priv->ap_mac_config.wmmApBcParams[3].aifs=2;
+    priv->ap_mac_config.wmmApBcParams[3].txopLimit=47;
+    priv->ap_mac_config.wmmApBcParams[3].admissionControlMandatory=FALSE;
+
+    priv->ap_mac_config.accessType=CSR_WIFI_AP_ACCESS_TYPE_NONE;
+    priv->ap_mac_config.macAddressListCount=0;
+    priv->ap_mac_config.macAddressList=NULL;
+
+    priv->ap_mac_config.apHtParams.rxStbc=1;
+    priv->ap_mac_config.apHtParams.rifsModeAllowed=TRUE;
+    priv->ap_mac_config.apHtParams.greenfieldSupported=FALSE;
+    priv->ap_mac_config.apHtParams.shortGi20MHz=TRUE;
+    priv->ap_mac_config.apHtParams.htProtection=0;
+    priv->ap_mac_config.apHtParams.dualCtsProtection=FALSE;
+
+    priv->ap_mac_config.phySupportedBitmap =
+            (CSR_WIFI_SME_AP_PHY_SUPPORT_B|CSR_WIFI_SME_AP_PHY_SUPPORT_G|CSR_WIFI_SME_AP_PHY_SUPPORT_N);
+    priv->ap_mac_config.beaconInterval= 100;
+    priv->ap_mac_config.dtimPeriod=3;
+    priv->ap_mac_config.maxListenInterval=0x00ff;/* Set it to a large value
+                                                    to enable different types of
+                                                    devices to join us */
+    priv->ap_mac_config.supportedRatesCount =
+           uf_configure_supported_rates(priv->ap_mac_config.supportedRates,priv->ap_mac_config.phySupportedBitmap);
+}
+#endif
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_sme_wext_set_defaults
+ *
+ *      Set up power-on defaults for driver config.
+ *
+ *      Note: The SME Management API *cannot* be used in this function.
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+uf_sme_wext_set_defaults(unifi_priv_t *priv)
+{
+    memset(&priv->connection_config, 0, sizeof(CsrWifiSmeConnectionConfig));
+
+    priv->connection_config.bssType = CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE;
+    priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+    priv->connection_config.encryptionModeMask = CSR_WIFI_SME_ENCRYPTION_CIPHER_NONE;
+    priv->connection_config.privacyMode = CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED;
+    priv->connection_config.wmmQosInfo = 0xFF;
+    priv->connection_config.ifIndex = CSR_WIFI_SME_RADIO_IF_BOTH;
+    priv->connection_config.adhocJoinOnly = FALSE;
+    priv->connection_config.adhocChannel = 6;
+
+    priv->wep_tx_key_index = 0;
+
+    priv->wext_wireless_stats.qual.qual = 0;
+    priv->wext_wireless_stats.qual.level = 0;
+    priv->wext_wireless_stats.qual.noise = 0;
+    priv->wext_wireless_stats.qual.updated = 0x70;
+#ifdef CSR_SUPPORT_WEXT_AP
+    /* Initialize the default configuration for AP */
+    uf_sme_wext_ap_set_defaults(priv);
+#endif
+
+
+} /* uf_sme_wext_set_defaults() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *      WEXT methods
+ * ---------------------------------------------------------------------------
+ */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_giwname   - handler for SIOCGIWNAME
+ *  unifi_siwfreq   - handler for SIOCSIWFREQ
+ *  unifi_giwfreq   - handler for SIOCGIWFREQ
+ *  unifi_siwmode   - handler for SIOCSIWMODE
+ *  unifi_giwmode   - handler for SIOCGIWMODE
+ *  unifi_giwrange  - handler for SIOCGIWRANGE
+ *  unifi_siwap     - handler for SIOCSIWAP
+ *  unifi_giwap     - handler for SIOCGIWAP
+ *  unifi_siwscan   - handler for SIOCSIWSCAN
+ *  unifi_giwscan   - handler for SIOCGIWSCAN
+ *  unifi_siwessid  - handler for SIOCSIWESSID
+ *  unifi_giwessid  - handler for SIOCGIWESSID
+ *  unifi_siwencode - handler for SIOCSIWENCODE
+ *  unifi_giwencode - handler for SIOCGIWENCODE
+ *
+ *      Handler functions for IW extensions.
+ *      These are registered via the unifi_iw_handler_def struct below
+ *      and called by the generic IW driver support code.
+ *      See include/net/iw_handler.h.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static int
+iwprivsdefs(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    int r;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    CsrWifiSmeMibConfig mibConfig;
+    CsrWifiSmePowerConfig powerConfig;
+
+    unifi_trace(priv, UDBG1, "iwprivs80211defaults: reload defaults\n");
+
+    uf_sme_wext_set_defaults(priv);
+
+    /* Get, modify and set the MIB data */
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    if (r) {
+        unifi_error(priv, "iwprivs80211defaults: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+    mibConfig.dot11RtsThreshold = 2347;
+    mibConfig.dot11FragmentationThreshold = 2346;
+    r = sme_mgt_mib_config_set(priv, &mibConfig);
+    if (r) {
+        unifi_error(priv, "iwprivs80211defaults: Set CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW;
+    powerConfig.listenIntervalTu = 100;
+    powerConfig.rxDtims = 1;
+
+    r = sme_mgt_power_config_set(priv, &powerConfig);
+    if (r) {
+        unifi_error(priv, "iwprivs80211defaults: Set unifi_PowerConfigValue failed.\n");
+        return r;
+    }
+
+    return 0;
+} /* iwprivsdefs() */
+
+static int
+iwprivs80211ps(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    int r = 0;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    int ps_mode = (int)(*extra);
+    CsrWifiSmePowerConfig powerConfig;
+
+    unifi_trace(priv, UDBG1, "iwprivs80211ps: power save mode = %d\n", ps_mode);
+
+    r = sme_mgt_power_config_get(priv, &powerConfig);
+    if (r) {
+        unifi_error(priv, "iwprivs80211ps: Get unifi_PowerConfigValue failed.\n");
+        return r;
+    }
+
+    switch (ps_mode) {
+        case CSR_PMM_ACTIVE_MODE:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW;
+            break;
+        case CSR_PMM_POWER_SAVE:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_HIGH;
+            break;
+        case CSR_PMM_FAST_POWER_SAVE:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_MED;
+            break;
+        default:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_AUTO;
+            break;
+    }
+
+    r = sme_mgt_power_config_set(priv, &powerConfig);
+    if (r) {
+        unifi_error(priv, "iwprivs80211ps: Set unifi_PowerConfigValue failed.\n");
+    }
+
+    return r;
+}
+
+static int
+iwprivg80211ps(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    CsrWifiSmePowerConfig powerConfig;
+    int r;
+
+    r = sme_mgt_power_config_get(priv, &powerConfig);
+    if (r) {
+        unifi_error(priv, "iwprivg80211ps: Get 802.11 power mode failed.\n");
+        return r;
+    }
+
+    switch (powerConfig.powerSaveLevel) {
+        case CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW:
+            snprintf(extra, IWPRIV_POWER_SAVE_MAX_STRING,
+                     "Power save mode: %d (Active)",
+                     powerConfig.powerSaveLevel);
+            break;
+        case CSR_WIFI_SME_POWER_SAVE_LEVEL_MED:
+            snprintf(extra, IWPRIV_POWER_SAVE_MAX_STRING,
+                     "Power save mode: %d (Fast)",
+                     powerConfig.powerSaveLevel);
+            break;
+        case CSR_WIFI_SME_POWER_SAVE_LEVEL_HIGH:
+            snprintf(extra, IWPRIV_POWER_SAVE_MAX_STRING,
+                     "Power save mode: %d (Full)",
+                     powerConfig.powerSaveLevel);
+            break;
+        case CSR_WIFI_SME_POWER_SAVE_LEVEL_AUTO:
+            snprintf(extra, IWPRIV_POWER_SAVE_MAX_STRING,
+                     "Power save mode: %d (Auto)",
+                     powerConfig.powerSaveLevel);
+            break;
+        default:
+            snprintf(extra, IWPRIV_POWER_SAVE_MAX_STRING,
+                     "Power save mode: %d (Unknown)",
+                     powerConfig.powerSaveLevel);
+            break;
+    }
+
+    wrqu->data.length = strlen(extra) + 1;
+
+    return 0;
+}
+
+static int
+iwprivssmedebug(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    /* No longer supported on the API */
+#if defined (CSR_WIFI_HIP_DEBUG_OFFLINE)
+    unifi_debug_buf_dump();
+#endif
+
+    return 0;
+}
+
+#ifdef CSR_SUPPORT_WEXT_AP
+#define PARAM_TYPE_INT 0
+#define PARAM_TYPE_STRING 1
+#define CSR_WIFI_MAX_SSID_LEN 32
+#define CSR_WIFI_MAX_SEC_LEN 16
+#define CSR_WIFI_MAX_KEY_LEN 65
+
+static int hex_look_up(char x)
+{
+    if(x>='0' && x<='9')
+        return (x-48);
+    if(x>= 'a' && x <= 'f')
+        return (x-87);
+    return -1;
+}
+
+static int power (int a, int b)
+{
+    int i;
+    int num =1;
+    for(i=0;i<b;i++)
+       num *=a;
+    return num;
+}
+
+static int decode_parameter_from_string(unifi_priv_t* priv, char **str_ptr,
+                                        const char *token, int param_type,
+                                        void  *dst, int param_max_len)
+{
+    u8 int_str[7] = "0";
+    u32 param_str_len;
+    u8  *param_str_begin,*param_str_end;
+    u8  *orig_str = *str_ptr;
+
+    if (!strncmp(*str_ptr, token, strlen(token))) {
+        strsep(str_ptr, "=,");
+        param_str_begin = *str_ptr;
+        strsep(str_ptr, "=,");
+        if (*str_ptr == NULL) {
+            param_str_len = strlen(param_str_begin);
+        } else {
+            param_str_end = *str_ptr-1;
+            param_str_len = param_str_end - param_str_begin;
+        }
+        unifi_trace(priv,UDBG2,"'token:%s', len:%d, ", token, param_str_len);
+        if (param_str_len > param_max_len) {
+            unifi_notice(priv,"extracted param len:%d is > MAX:%d\n",param_str_len, param_max_len);
+            param_str_len = param_max_len;
+        }
+        switch (param_type) {
+            case PARAM_TYPE_INT:
+            {
+                u32 *pdst_int = dst,num =0;
+                int i,j=0;
+                if (param_str_len > sizeof(int_str)) {
+                    param_str_len = sizeof(int_str);
+                }
+                memcpy(int_str, param_str_begin, param_str_len);
+                for(i = param_str_len; i>0;i--) {
+                    if(int_str[i-1] >= '0' && int_str[i-1] <='9') {
+                        num += ((int_str[i-1]-'0')*power(10,j));
+                        j++;
+                    } else {
+                        unifi_error(priv,"decode_parameter_from_string:not a number %c\n",(int_str[i-1]));
+                        return -1;
+                    }
+                }
+                *pdst_int = num;
+                unifi_trace(priv,UDBG2,"decode_parameter_from_string:decoded int = %d\n",*pdst_int);
+            }
+            break;
+            default:
+                memcpy(dst, param_str_begin, param_str_len);
+                *((char *)dst + param_str_len) = 0;
+                unifi_trace(priv,UDBG2,"decode_parameter_from_string:decoded string = %s\n",(char *)dst);
+            break;
+        }
+    } else {
+        unifi_error(priv,"decode_parameter_from_string: Token:%s not found in %s \n",token,orig_str);
+        return -1;
+    }
+    return 0;
+}
+static int store_ap_advanced_config_from_string(unifi_priv_t *priv, char *param_str)
+{
+    char * str_ptr=param_str;
+    int ret = 0,tmp_var;
+    char phy_mode[6];
+    CsrWifiSmeApMacConfig * ap_mac_config = &priv->ap_mac_config;
+
+    /* Check for BI */
+    ret = decode_parameter_from_string(priv, &str_ptr, "BI=",
+                                       PARAM_TYPE_INT, &tmp_var, 5);
+    if(ret) {
+        unifi_error(priv,"store_ap_advanced_config_from_string: BI not found\n");
+        return -1;
+    }
+    ap_mac_config->beaconInterval = tmp_var;
+    ret = decode_parameter_from_string(priv, &str_ptr, "DTIM_PER=",
+                                        PARAM_TYPE_INT, &tmp_var, 5);
+    if(ret) {
+        unifi_error(priv,"store_ap_advanced_config_from_string: DTIM_PER not found\n");
+        return -1;
+    }
+    ap_mac_config->dtimPeriod = tmp_var;
+    ret = decode_parameter_from_string(priv, &str_ptr, "WMM=",
+                                        PARAM_TYPE_INT, &tmp_var, 5);
+    if(ret) {
+        unifi_error(priv,"store_ap_advanced_config_from_string: WMM not found\n");
+        return -1;
+    }
+    ap_mac_config->wmmEnabled = tmp_var;
+    ret = decode_parameter_from_string(priv, &str_ptr, "PHY=",
+                                        PARAM_TYPE_STRING, phy_mode, 5);
+    if(ret) {
+        unifi_error(priv,"store_ap_advanced_config_from_string: PHY not found\n");
+    } else {
+       if(strstr(phy_mode,"b")){
+           ap_mac_config->phySupportedBitmap = CSR_WIFI_SME_AP_PHY_SUPPORT_B;
+       }
+       if(strstr(phy_mode,"g")) {
+           ap_mac_config->phySupportedBitmap |= CSR_WIFI_SME_AP_PHY_SUPPORT_G;
+       }
+       if(strstr(phy_mode,"n")) {
+           ap_mac_config->phySupportedBitmap |= CSR_WIFI_SME_AP_PHY_SUPPORT_N;
+       }
+       ap_mac_config->supportedRatesCount =
+       uf_configure_supported_rates(ap_mac_config->supportedRates, ap_mac_config->phySupportedBitmap);
+    }
+    return ret;
+}
+
+static int store_ap_config_from_string( unifi_priv_t * priv,char *param_str)
+
+{
+    char *str_ptr = param_str;
+    char sub_cmd[16];
+    char sec[CSR_WIFI_MAX_SEC_LEN];
+    char key[CSR_WIFI_MAX_KEY_LEN];
+    int ret = 0,tmp_var;
+    CsrWifiSmeApConfig_t *ap_config = &priv->ap_config;
+    CsrWifiSmeApMacConfig * ap_mac_config = &priv->ap_mac_config;
+    memset(sub_cmd, 0, sizeof(sub_cmd));
+    if(!strstr(param_str,"END")) {
+        unifi_error(priv,"store_ap_config_from_string:Invalid config string:%s\n",param_str);
+        return -1;
+    }
+    if (decode_parameter_from_string(priv,&str_ptr, "ASCII_CMD=",
+        PARAM_TYPE_STRING, sub_cmd, 6) != 0) {
+         return -1;
+    }
+    if (strncmp(sub_cmd, "AP_CFG", 6)) {
+
+        if(!strncmp(sub_cmd ,"ADVCFG", 6)) {
+           return store_ap_advanced_config_from_string(priv, str_ptr);
+        }
+        unifi_error(priv,"store_ap_config_from_string: sub_cmd:%s != 'AP_CFG or ADVCFG'!\n", sub_cmd);
+        return -1;
+    }
+    memset(ap_config, 0, sizeof(CsrWifiSmeApConfig_t));
+    ret = decode_parameter_from_string(priv,&str_ptr, "SSID=",
+                                       PARAM_TYPE_STRING, ap_config->ssid.ssid,
+                                       CSR_WIFI_MAX_SSID_LEN);
+    if(ret) {
+        unifi_error(priv,"store_ap_config_from_string: SSID not found\n");
+        return -1;
+    }
+    ap_config->ssid.length = strlen(ap_config->ssid.ssid);
+
+    ret = decode_parameter_from_string(priv, &str_ptr, "SEC=",
+                                       PARAM_TYPE_STRING, sec, CSR_WIFI_MAX_SEC_LEN);
+    if(ret) {
+        unifi_error(priv,"store_ap_config_from_string: SEC not found\n");
+        return -1;
+    }
+    ret = decode_parameter_from_string(priv,&str_ptr, "KEY=",
+                         PARAM_TYPE_STRING,  key, CSR_WIFI_MAX_KEY_LEN);
+    if(!strcasecmp(sec,"open")) {
+        unifi_trace(priv,UDBG2,"store_ap_config_from_string: security open");
+        ap_config->credentials.authType = CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM;
+        if(ret) {
+            unifi_notice(priv,"store_ap_config_from_string: KEY not found:fine with Open\n");
+        }
+    }
+    else if(!strcasecmp(sec,"wpa2-psk")) {
+        int i,j=0;
+        CsrWifiNmeApAuthPers *pers =
+                            ((CsrWifiNmeApAuthPers *)&(ap_config->credentials.nmeAuthType.authTypePersonal));
+        u8 *psk = pers->authPers_credentials.psk.psk;
+
+        unifi_trace(priv,UDBG2,"store_ap_config_from_string: security WPA2");
+        if(ret) {
+            unifi_error(priv,"store_ap_config_from_string: KEY not found for WPA2\n");
+            return -1;
+        }
+        ap_config->credentials.authType = CSR_WIFI_SME_AP_AUTH_TYPE_PERSONAL;
+        pers->authSupport = CSR_WIFI_SME_RSN_AUTH_WPA2PSK;
+        pers->rsnCapabilities =0;
+        pers->wapiCapabilities =0;
+        pers->pskOrPassphrase=CSR_WIFI_NME_AP_CREDENTIAL_TYPE_PSK;
+        pers->authPers_credentials.psk.encryptionMode =
+                 (CSR_WIFI_NME_ENCRYPTION_CIPHER_PAIRWISE_CCMP |CSR_WIFI_NME_ENCRYPTION_CIPHER_GROUP_CCMP) ;
+        for(i=0;i<32;i++){
+           psk[i] = (16*hex_look_up(key[j]))+hex_look_up(key[j+1]);
+           j+=2;
+        }
+
+    } else {
+       unifi_notice(priv,"store_ap_config_from_string: Unknown security: Assuming Open");
+       ap_config->credentials.authType = CSR_WIFI_SME_AP_AUTH_TYPE_OPEN_SYSTEM;
+       return -1;
+    }
+   /* Get the decoded value in a temp int variable to ensure that other fields within the struct
+      which are of type other than int are not over written */
+    ret = decode_parameter_from_string(priv,&str_ptr, "CHANNEL=", PARAM_TYPE_INT, &tmp_var, 5);
+    if(ret)
+        return -1;
+    ap_config->channel = tmp_var;
+    ret = decode_parameter_from_string(priv,&str_ptr, "PREAMBLE=", PARAM_TYPE_INT, &tmp_var, 5);
+    if(ret)
+        return -1;
+    ap_mac_config->preamble = tmp_var;
+    ret = decode_parameter_from_string(priv,&str_ptr, "MAX_SCB=", PARAM_TYPE_INT,  &tmp_var, 5);
+    ap_config->max_connections = tmp_var;
+    return ret;
+}
+
+static int
+iwprivsapstart(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int r;
+
+    unifi_trace(priv, UDBG1, "iwprivsapstart\n" );
+    r = sme_ap_start(priv,interfacePriv->InterfaceTag,&priv->ap_config);
+    if(r) {
+        unifi_error(priv,"iwprivsapstart AP START failed : %d\n",-r);
+    }
+    return r;
+}
+
+static int
+iwprivsapconfig(struct net_device *dev, struct iw_request_info *info,
+                union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    char  *cfg_str = NULL;
+    int r;
+
+    unifi_trace(priv, UDBG1, "iwprivsapconfig\n" );
+    if (wrqu->data.length != 0) {
+        char *str;
+        if (!(cfg_str = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+        {
+            return -ENOMEM;
+        }
+        if (copy_from_user(cfg_str, wrqu->data.pointer, wrqu->data.length)) {
+            kfree(cfg_str);
+            return -EFAULT;
+        }
+        cfg_str[wrqu->data.length] = 0;
+        unifi_trace(priv,UDBG2,"length:%d\n",wrqu->data.length);
+        unifi_trace(priv,UDBG2,"AP configuration string:%s\n",cfg_str);
+        str = cfg_str;
+       if ((r = store_ap_config_from_string(priv,str))) {
+           unifi_error(priv, "iwprivsapconfig:Failed  to decode the string %d\n",r);
+           kfree(cfg_str);
+           return -EIO;
+
+       }
+    } else {
+        unifi_error(priv,"iwprivsapconfig argument length = 0 \n");
+        return -EIO;
+    }
+    r = sme_ap_config(priv, &priv->ap_mac_config, &priv->group_sec_config);
+    if(r) {
+        unifi_error(priv,"iwprivsapstop AP Config failed : %d\n",-r);
+    } else if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+        interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+        unifi_trace(priv, UDBG1, "iwprivsapconfig: Starting the AP");
+        r = sme_ap_start(priv,interfacePriv->InterfaceTag,&priv->ap_config);
+        if(r) {
+            unifi_error(priv,"iwprivsapstart AP START failed : %d\n",-r);
+        }
+    }
+    kfree(cfg_str);
+    return r;
+}
+
+static int
+iwprivsapstop(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int r;
+    u16 interface_tag = interfacePriv->InterfaceTag;
+
+    unifi_trace(priv, UDBG1, "iwprivsapstop\n" );
+    r = sme_ap_stop(priv,interface_tag);
+    if(r) {
+        unifi_error(priv,"iwprivsapstop AP STOP failed : %d\n",-r);
+    }
+    return r;
+}
+
+#ifdef ANDROID_BUILD
+static int
+iwprivsapfwreload(struct net_device *dev, struct iw_request_info *info,
+                  union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    unifi_trace(priv, UDBG1, "iwprivsapfwreload\n" );
+    return 0;
+}
+
+static int
+iwprivsstackstart(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    unifi_trace(priv, UDBG1, "iwprivsstackstart\n" );
+    return 0;
+}
+
+static int
+iwprivsstackstop(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int r = 0;
+    u16 interface_tag = interfacePriv->InterfaceTag;
+
+    unifi_trace(priv, UDBG1, "iwprivsstackstop\n" );
+
+    switch(interfacePriv->interfaceMode) {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            r = sme_mgt_disconnect(priv);
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            r = sme_ap_stop(priv,interface_tag);
+            break;
+        default :
+            break;
+    }
+
+    if(r) {
+        unifi_error(priv,"iwprivsstackstop Stack stop failed : %d\n",-r);
+    }
+    return 0;
+}
+#endif /* ANDROID_BUILD */
+#endif /* CSR_SUPPORT_WEXT_AP */
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+static int
+iwprivsconfwapi(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    u8 enable;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    func_enter();
+
+    unifi_trace(priv, UDBG1, "iwprivsconfwapi\n" );
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "iwprivsconfwapi: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+    enable = *(u8*)(extra);
+
+    if (enable) {
+        priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+        priv->connection_config.authModeMask |= (CSR_WIFI_SME_AUTH_MODE_WAPI_WAIPSK | CSR_WIFI_SME_AUTH_MODE_WAPI_WAI);
+        priv->connection_config.encryptionModeMask |=
+            CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_SMS4 | CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_SMS4;
+    } else {
+        priv->connection_config.authModeMask &= ~(CSR_WIFI_SME_AUTH_MODE_WAPI_WAIPSK | CSR_WIFI_SME_AUTH_MODE_WAPI_WAI);
+        priv->connection_config.encryptionModeMask &=
+            ~(CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_SMS4 | CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_SMS4);
+    }
+
+    func_exit();
+    return 0;
+}
+
+static int
+iwprivswpikey(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    int r = 0, i;
+    CsrWifiSmeKey key;
+    unifiio_wapi_key_t inKey;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    func_enter();
+
+    unifi_trace(priv, UDBG1, "iwprivswpikey\n" );
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "iwprivswpikey: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+    inKey = *(unifiio_wapi_key_t*)(extra);
+
+    if (inKey.unicastKey) {
+        key.keyType   = CSR_WIFI_SME_KEY_TYPE_PAIRWISE;
+    } else {
+        key.keyType   = CSR_WIFI_SME_KEY_TYPE_GROUP;
+    }
+
+    key.keyIndex  = inKey.keyIndex;
+
+    /* memcpy(key.keyRsc, inKey.keyRsc, 16); */
+    for (i = 0; i < 16; i+= 2)
+    {
+        key.keyRsc[i/2] = inKey.keyRsc[i+1] << 8 | inKey.keyRsc[i];
+    }
+
+    memcpy(key.address.a, inKey.address, 6);
+    key.keyLength = 32;
+    memcpy(key.key, inKey.key, 32);
+    key.authenticator = 0;
+    key.wepTxKey = 0;
+
+    unifi_trace(priv, UDBG1, "keyType = %d, keyIndex = %d, wepTxKey = %d, keyRsc = %x:%x, auth = %d, address = %x:%x, "
+                "keylength = %d, key = %x:%x\n", key.keyType, key.keyIndex, key.wepTxKey,
+                key.keyRsc[0], key.keyRsc[7], key.authenticator,
+                key.address.a[0], key.address.a[5], key.keyLength, key.key[0],
+                key.key[15]);
+
+    r = sme_mgt_key(priv, &key, CSR_WIFI_SME_LIST_ACTION_ADD);
+    if (r) {
+        unifi_error(priv, "SETKEYS request was rejected with result %d\n", r);
+        return convert_sme_error(r);
+    }
+
+    func_exit();
+    return r;
+}
+#endif
+
+
+static int
+unifi_giwname(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    char *name = wrqu->name;
+    unifi_trace(priv, UDBG2, "unifi_giwname\n");
+
+    if (priv->if_index == CSR_INDEX_5G) {
+        strcpy(name, "IEEE 802.11-a");
+    } else {
+        strcpy(name, "IEEE 802.11-bgn");
+    }
+    return 0;
+} /* unifi_giwname() */
+
+
+static int
+unifi_siwfreq(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_freq *freq = (struct iw_freq *)wrqu;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_siwfreq\n");
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwfreq: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    /*
+     * Channel is stored in the connection configuration,
+     * and set later when ask for a connection.
+     */
+    if ((freq->e == 0) && (freq->m <= 1000)) {
+        priv->connection_config.adhocChannel = freq->m;
+    } else {
+        priv->connection_config.adhocChannel = wext_freq_to_channel(freq->m, freq->e);
+    }
+
+    func_exit();
+    return 0;
+} /* unifi_siwfreq() */
+
+
+static int
+unifi_giwfreq(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_freq *freq = (struct iw_freq *)wrqu;
+    int err = 0;
+    CsrWifiSmeConnectionInfo connectionInfo;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_giwfreq\n");
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_giwfreq: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    UF_RTNL_UNLOCK();
+    err = sme_mgt_connection_info_get(priv, &connectionInfo);
+    UF_RTNL_LOCK();
+
+    freq->m = channel_to_mhz(connectionInfo.channelNumber,
+            (connectionInfo.networkType80211 == CSR_WIFI_SME_RADIO_IF_GHZ_5_0));
+    freq->e = 6;
+
+    func_exit();
+    return convert_sme_error(err);
+} /* unifi_giwfreq() */
+
+
+static int
+unifi_siwmode(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_siwmode\n");
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwmode: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    switch(wrqu->mode) {
+        case IW_MODE_ADHOC:
+            priv->connection_config.bssType = CSR_WIFI_SME_BSS_TYPE_ADHOC;
+            break;
+        case IW_MODE_INFRA:
+            priv->connection_config.bssType = CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE;
+            break;
+        case IW_MODE_AUTO:
+            priv->connection_config.bssType = CSR_WIFI_SME_BSS_TYPE_ANY_BSS;
+            break;
+        default:
+            unifi_notice(priv, "Unknown IW MODE value.\n");
+    }
+
+    /* Clear the SSID and BSSID configuration */
+    priv->connection_config.ssid.length = 0;
+    memset(priv->connection_config.bssid.a, 0xFF, ETH_ALEN);
+
+    func_exit();
+    return 0;
+} /* unifi_siwmode() */
+
+
+
+static int
+unifi_giwmode(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    int r = 0;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    CsrWifiSmeConnectionConfig connectionConfig;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_giwmode\n");
+    CHECK_INITED(priv);
+
+    unifi_trace(priv, UDBG2, "unifi_giwmode: Exisitng mode = 0x%x\n",
+                interfacePriv->interfaceMode);
+    switch(interfacePriv->interfaceMode) {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+           wrqu->mode = IW_MODE_INFRA;
+           break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            wrqu->mode = IW_MODE_MASTER;
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            wrqu->mode = IW_MODE_ADHOC;
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2P:
+        case CSR_WIFI_ROUTER_CTRL_MODE_NONE:
+            UF_RTNL_UNLOCK();
+            r = sme_mgt_connection_config_get(priv, &connectionConfig);
+            UF_RTNL_LOCK();
+            if (r == 0) {
+                switch(connectionConfig.bssType) {
+                    case CSR_WIFI_SME_BSS_TYPE_ADHOC:
+                        wrqu->mode = IW_MODE_ADHOC;
+                        break;
+                    case CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE:
+                        wrqu->mode = IW_MODE_INFRA;
+                        break;
+                    default:
+                        wrqu->mode = IW_MODE_AUTO;
+                        unifi_notice(priv, "Unknown IW MODE value.\n");
+                }
+            }
+            break;
+        default:
+            wrqu->mode = IW_MODE_AUTO;
+            unifi_notice(priv, "Unknown IW MODE value.\n");
+
+    }
+    unifi_trace(priv, UDBG4, "unifi_giwmode: mode = 0x%x\n", wrqu->mode);
+    func_exit();
+    return r;
+} /* unifi_giwmode() */
+
+
+
+static int
+unifi_giwrange(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    struct iw_point *dwrq = &wrqu->data;
+    struct iw_range *range = (struct iw_range *) extra;
+    int i;
+
+    unifi_trace(NULL, UDBG2, "unifi_giwrange\n");
+
+    dwrq->length = sizeof(struct iw_range);
+    memset(range, 0, sizeof(*range));
+    range->min_nwid = 0x0000;
+    range->max_nwid = 0x0000;
+
+    /*
+     * Don't report the frequency/channel table, then the channel
+     * number returned elsewhere will be printed as a channel number.
+     */
+
+    /* Ranges of values reported in quality structs */
+    range->max_qual.qual  = 40;         /* Max expected qual value */
+    range->max_qual.level = -120;       /* Noise floor in dBm */
+    range->max_qual.noise = -120;       /* Noise floor in dBm */
+
+
+    /* space for IW_MAX_BITRATES (8 up to WE15, 32 later) */
+    i = 0;
+#if WIRELESS_EXT > 15
+    range->bitrate[i++] =   2 * 500000;
+    range->bitrate[i++] =   4 * 500000;
+    range->bitrate[i++] =  11 * 500000;
+    range->bitrate[i++] =  22 * 500000;
+    range->bitrate[i++] =  12 * 500000;
+    range->bitrate[i++] =  18 * 500000;
+    range->bitrate[i++] =  24 * 500000;
+    range->bitrate[i++] =  36 * 500000;
+    range->bitrate[i++] =  48 * 500000;
+    range->bitrate[i++] =  72 * 500000;
+    range->bitrate[i++] =  96 * 500000;
+    range->bitrate[i++] = 108 * 500000;
+#else
+    range->bitrate[i++] =   2 * 500000;
+    range->bitrate[i++] =   4 * 500000;
+    range->bitrate[i++] =  11 * 500000;
+    range->bitrate[i++] =  22 * 500000;
+    range->bitrate[i++] =  24 * 500000;
+    range->bitrate[i++] =  48 * 500000;
+    range->bitrate[i++] =  96 * 500000;
+    range->bitrate[i++] = 108 * 500000;
+#endif /* WIRELESS_EXT < 16 */
+    range->num_bitrates = i;
+
+    range->max_encoding_tokens = NUM_WEPKEYS;
+    range->num_encoding_sizes = 2;
+    range->encoding_size[0] = 5;
+    range->encoding_size[1] = 13;
+
+    range->we_version_source = 20;
+    range->we_version_compiled = WIRELESS_EXT;
+
+    /* Number of channels available in h/w */
+    range->num_channels = 14;
+    /* Number of entries in freq[] array */
+    range->num_frequency = 14;
+    for (i = 0; (i < range->num_frequency) && (i < IW_MAX_FREQUENCIES); i++) {
+        int chan = i + 1;
+        range->freq[i].i = chan;
+        range->freq[i].m = channel_to_mhz(chan, 0);
+        range->freq[i].e = 6;
+    }
+    if ((i+3) < IW_MAX_FREQUENCIES) {
+        range->freq[i].i = 36;
+        range->freq[i].m = channel_to_mhz(36, 1);
+        range->freq[i].e = 6;
+        range->freq[i+1].i = 40;
+        range->freq[i+1].m = channel_to_mhz(40, 1);
+        range->freq[i+1].e = 6;
+        range->freq[i+2].i = 44;
+        range->freq[i+2].m = channel_to_mhz(44, 1);
+        range->freq[i+2].e = 6;
+        range->freq[i+3].i = 48;
+        range->freq[i+3].m = channel_to_mhz(48, 1);
+        range->freq[i+3].e = 6;
+    }
+
+#if WIRELESS_EXT > 16
+    /* Event capability (kernel + driver) */
+    range->event_capa[0] = (IW_EVENT_CAPA_K_0 |
+            IW_EVENT_CAPA_MASK(SIOCGIWTHRSPY) |
+            IW_EVENT_CAPA_MASK(SIOCGIWAP) |
+            IW_EVENT_CAPA_MASK(SIOCGIWSCAN));
+    range->event_capa[1] = IW_EVENT_CAPA_K_1;
+    range->event_capa[4] = (IW_EVENT_CAPA_MASK(IWEVTXDROP) |
+            IW_EVENT_CAPA_MASK(IWEVCUSTOM) |
+            IW_EVENT_CAPA_MASK(IWEVREGISTERED) |
+            IW_EVENT_CAPA_MASK(IWEVEXPIRED));
+#endif /* WIRELESS_EXT > 16 */
+
+#if WIRELESS_EXT > 17
+    range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+        IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif /* WIRELESS_EXT > 17 */
+
+
+    return 0;
+} /* unifi_giwrange() */
+
+
+static int
+unifi_siwap(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int err = 0;
+    const unsigned char zero_bssid[ETH_ALEN] = {0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00};
+
+    func_enter();
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwap: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    if (wrqu->ap_addr.sa_family != ARPHRD_ETHER) {
+        return -EINVAL;
+    }
+
+	unifi_trace(priv, UDBG1, "unifi_siwap: asked for %pM\n",
+		wrqu->ap_addr.sa_data);
+
+    if (!memcmp(wrqu->ap_addr.sa_data, zero_bssid, ETH_ALEN)) {
+        priv->ignore_bssid_join = FALSE;
+        err = sme_mgt_disconnect(priv);
+        if (err) {
+            unifi_trace(priv, UDBG4, "unifi_siwap: Disconnect failed, status %d\n", err);
+        }
+        return 0;
+    }
+
+    if (priv->ignore_bssid_join) {
+        unifi_trace(priv, UDBG4, "unifi_siwap: ignoring second join\n");
+        priv->ignore_bssid_join = FALSE;
+    } else {
+        memcpy(priv->connection_config.bssid.a, wrqu->ap_addr.sa_data, ETH_ALEN);
+        unifi_trace(priv, UDBG1, "unifi_siwap: Joining %X:%X:%X:%X:%X:%X\n",
+                    priv->connection_config.bssid.a[0],
+                    priv->connection_config.bssid.a[1],
+                    priv->connection_config.bssid.a[2],
+                    priv->connection_config.bssid.a[3],
+                    priv->connection_config.bssid.a[4],
+                    priv->connection_config.bssid.a[5]);
+        err = sme_mgt_connect(priv);
+        if (err) {
+            unifi_error(priv, "unifi_siwap: Join failed, status %d\n", err);
+            func_exit();
+            return convert_sme_error(err);
+        }
+    }
+    func_exit();
+
+    return 0;
+} /* unifi_siwap() */
+
+
+static int
+unifi_giwap(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    CsrWifiSmeConnectionInfo connectionInfo;
+    int r = 0;
+    u8 *bssid;
+
+    func_enter();
+
+    CHECK_INITED(priv);
+    unifi_trace(priv, UDBG2, "unifi_giwap\n");
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "iwprivswpikey: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_connection_info_get(priv, &connectionInfo);
+    UF_RTNL_LOCK();
+
+    if (r == 0) {
+        bssid = connectionInfo.bssid.a;
+        wrqu->ap_addr.sa_family = ARPHRD_ETHER;
+		unifi_trace(priv, UDBG4, "unifi_giwap: BSSID = %pM\n", bssid);
+
+        memcpy(wrqu->ap_addr.sa_data, bssid, ETH_ALEN);
+    } else {
+        memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+    }
+
+    func_exit();
+    return 0;
+} /* unifi_giwap() */
+
+
+static int
+unifi_siwscan(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int scantype;
+    int r;
+    CsrWifiSsid scan_ssid;
+    unsigned char *channel_list = NULL;
+    int chans_good = 0;
+#if WIRELESS_EXT > 17
+    struct iw_point *data = &wrqu->data;
+    struct iw_scan_req *req = (struct iw_scan_req *) extra;
+#endif
+
+    func_enter();
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwscan: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    scantype = UNIFI_SCAN_ACTIVE;
+
+#if WIRELESS_EXT > 17
+    /* Providing a valid channel list will force an active scan */
+    if (req) {
+        if ((req->num_channels > 0) && (req->num_channels < IW_MAX_FREQUENCIES)) {
+            channel_list = kmalloc(req->num_channels, GFP_KERNEL);
+            if (channel_list) {
+                int i;
+                for (i = 0; i < req->num_channels; i++) {
+                    /* Convert frequency to channel number */
+                    int ch = wext_freq_to_channel(req->channel_list[i].m,
+                            req->channel_list[i].e);
+                    if (ch) {
+                        channel_list[chans_good++] = ch;
+                    }
+                }
+                unifi_trace(priv, UDBG1,
+                            "SIWSCAN: Scanning %d channels\n", chans_good);
+            } else {
+                /* Fall back to scanning all */
+                unifi_error(priv, "SIWSCAN: Can't alloc channel_list (%d)\n",
+                        req->num_channels);
+            }
+        }
+    }
+
+    if (req && (data->flags & IW_SCAN_THIS_ESSID)) {
+        memcpy(scan_ssid.ssid, req->essid, req->essid_len);
+        scan_ssid.length = req->essid_len;
+        unifi_trace(priv, UDBG1,
+                    "SIWSCAN: Scanning for %.*s\n",
+                    scan_ssid.length, scan_ssid.ssid);
+    } else
+#endif
+    {
+        unifi_trace(priv, UDBG1, "SIWSCAN: Scanning for all APs\n");
+        scan_ssid.length = 0;
+    }
+
+    r = sme_mgt_scan_full(priv, &scan_ssid, chans_good, channel_list);
+    if (r) {
+        unifi_error(priv, "SIWSCAN: Scan returned error %d\n", r);
+    } else {
+        unifi_trace(priv, UDBG1, "SIWSCAN: Scan done\n");
+        wext_send_scan_results_event(priv);
+    }
+
+    if (channel_list) {
+        kfree(channel_list);
+    }
+
+    func_exit();
+    return r;
+
+} /* unifi_siwscan() */
+
+
+static const unsigned char *
+unifi_find_info_element(int id, const unsigned char *info, int len)
+{
+    const unsigned char *ie = info;
+
+    while (len > 1)
+    {
+        int e_id, e_len;
+        e_id = ie[0];
+        e_len = ie[1];
+
+        /* Return if we find a match */
+        if (e_id == id)
+        {
+            return ie;
+        }
+
+        len -= (e_len + 2);
+        ie  += (e_len + 2);
+    }
+
+    return NULL;
+} /* unifi_find_info_element() */
+
+
+/*
+ * Translate scan data returned from the card to a card independent
+ * format that the Wireless Tools will understand - Jean II
+ */
+int
+unifi_translate_scan(struct net_device *dev,
+                     struct iw_request_info *info,
+                     char *current_ev, char *end_buf,
+                     CsrWifiSmeScanResult *scan_data,
+                     int scan_index)
+{
+    struct iw_event iwe;                /* Temporary buffer */
+    unsigned char *info_elems;
+    int info_elem_len;
+    const unsigned char *elem;
+    u16 capabilities;
+    int signal, noise, snr;
+    char *start_buf = current_ev;
+    char *current_val;  /* For rates */
+    int i, r;
+
+    info_elems    = scan_data->informationElements;
+    info_elem_len = scan_data->informationElementsLength;
+
+    if (!scan_data->informationElementsLength || !scan_data->informationElements) {
+        unifi_error(NULL, "*** NULL SCAN IEs ***\n");
+        return -EIO;
+    }
+
+    /* get capinfo bits */
+    capabilities = scan_data->capabilityInformation;
+
+    unifi_trace(NULL, UDBG5, "Capabilities: 0x%x\n", capabilities);
+
+    /* First entry *MUST* be the AP MAC address */
+    memset(&iwe, 0, sizeof(iwe));
+    iwe.cmd = SIOCGIWAP;
+    iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+    memcpy(iwe.u.ap_addr.sa_data, scan_data->bssid.a, ETH_ALEN);
+    iwe.len = IW_EV_ADDR_LEN;
+    r = uf_iwe_stream_add_event(info, start_buf, end_buf, &iwe, IW_EV_ADDR_LEN);
+    if (r < 0) {
+        return r;
+    }
+    start_buf += r;
+
+    /* Other entries will be displayed in the order we give them */
+
+    /* Add the ESSID */
+    /* find SSID in Info Elems */
+    elem = unifi_find_info_element(IE_SSID_ID, info_elems, info_elem_len);
+    if (elem) {
+        int e_len = elem[1];
+        const unsigned char *e_ptr = elem + 2;
+        unsigned char buf[33];
+
+        memset(&iwe, 0, sizeof(iwe));
+        iwe.cmd = SIOCGIWESSID;
+        iwe.u.essid.length = e_len;
+        if (iwe.u.essid.length > 32) {
+            iwe.u.essid.length = 32;
+        }
+        iwe.u.essid.flags = scan_index;
+        memcpy(buf, e_ptr, iwe.u.essid.length);
+        buf[iwe.u.essid.length] = '\0';
+        r = uf_iwe_stream_add_point(info, start_buf, end_buf, &iwe, buf);
+        if (r < 0) {
+            return r;
+        }
+        start_buf += r;
+
+    }
+
+    /* Add mode */
+    memset(&iwe, 0, sizeof(iwe));
+    iwe.cmd = SIOCGIWMODE;
+    if (scan_data->bssType == CSR_WIFI_SME_BSS_TYPE_INFRASTRUCTURE) {
+        iwe.u.mode = IW_MODE_INFRA;
+    } else {
+        iwe.u.mode = IW_MODE_ADHOC;
+    }
+    iwe.len = IW_EV_UINT_LEN;
+    r = uf_iwe_stream_add_event(info, start_buf, end_buf, &iwe, IW_EV_UINT_LEN);
+    if (r < 0) {
+        return r;
+    }
+    start_buf += r;
+
+    /* Add frequency. iwlist will convert to channel using table given in giwrange */
+    memset(&iwe, 0, sizeof(iwe));
+    iwe.cmd = SIOCGIWFREQ;
+    iwe.u.freq.m = scan_data->channelFrequency;
+    iwe.u.freq.e = 6;
+    r = uf_iwe_stream_add_event(info, start_buf, end_buf, &iwe, IW_EV_FREQ_LEN);
+    if (r < 0) {
+        return r;
+    }
+    start_buf += r;
+
+
+    /* Add quality statistics */
+    iwe.cmd = IWEVQUAL;
+    /*
+     * level and noise below are mapped into an unsigned 8 bit number,
+     * ranging from [-192; 63]. The way this is achieved is simply to
+     * add 0x100 onto the number if it is negative,
+     * once clipped to the correct range.
+     */
+    signal = scan_data->rssi; /* This value is in dBm */
+    /* Clip range of snr */
+    snr    = (scan_data->snr > 0) ? scan_data->snr : 0; /* In dB relative, from 0 - 255 */
+    snr    = (snr < 255) ? snr : 255;
+    noise  = signal - snr;
+
+    /* Clip range of signal */
+    signal = (signal < 63) ? signal : 63;
+    signal = (signal > -192) ? signal : -192;
+
+    /* Clip range of noise */
+    noise = (noise < 63) ? noise : 63;
+    noise = (noise > -192) ? noise : -192;
+
+    /* Make u8 */
+    signal = ( signal < 0 ) ? signal + 0x100 : signal;
+    noise = ( noise < 0 ) ? noise + 0x100 : noise;
+
+    iwe.u.qual.level = (u8)signal; /* -192 : 63 */
+    iwe.u.qual.noise = (u8)noise;  /* -192 : 63 */
+    iwe.u.qual.qual = snr;         /* 0 : 255 */
+    iwe.u.qual.updated = 0;
+#if WIRELESS_EXT > 16
+    iwe.u.qual.updated |= IW_QUAL_LEVEL_UPDATED | IW_QUAL_NOISE_UPDATED |
+        IW_QUAL_QUAL_UPDATED;
+#if WIRELESS_EXT > 18
+    iwe.u.qual.updated |= IW_QUAL_DBM;
+#endif
+#endif
+    r = uf_iwe_stream_add_event(info, start_buf, end_buf, &iwe, IW_EV_QUAL_LEN);
+    if (r < 0) {
+        return r;
+    }
+    start_buf += r;
+
+    /* Add encryption capability */
+    iwe.cmd = SIOCGIWENCODE;
+    if (capabilities & SIG_CAP_PRIVACY) {
+        iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+    } else {
+        iwe.u.data.flags = IW_ENCODE_DISABLED;
+    }
+    iwe.u.data.length = 0;
+    iwe.len = IW_EV_POINT_LEN + iwe.u.data.length;
+    r = uf_iwe_stream_add_point(info, start_buf, end_buf, &iwe, "");
+    if (r < 0) {
+        return r;
+    }
+    start_buf += r;
+
+
+    /*
+     * Rate : stuffing multiple values in a single event require a bit
+     * more of magic - Jean II
+     */
+    current_val = start_buf + IW_EV_LCP_LEN;
+
+    iwe.cmd = SIOCGIWRATE;
+    /* Those two flags are ignored... */
+    iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+
+    elem = unifi_find_info_element(IE_SUPPORTED_RATES_ID,
+            info_elems, info_elem_len);
+    if (elem) {
+        int e_len = elem[1];
+        const unsigned char *e_ptr = elem + 2;
+
+        /*
+         * Count how many rates we have.
+         * Zero marks the end of the list, if the list is not truncated.
+         */
+        /* Max 8 values */
+        for (i = 0; i < e_len; i++) {
+            if (e_ptr[i] == 0) {
+                break;
+            }
+            /* Bit rate given in 500 kb/s units (+ 0x80) */
+            iwe.u.bitrate.value = ((e_ptr[i] & 0x7f) * 500000);
+            /* Add new value to event */
+            r = uf_iwe_stream_add_value(info, start_buf, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+            if (r < 0) {
+                return r;
+            }
+            current_val +=r;
+
+        }
+    }
+    elem = unifi_find_info_element(IE_EXTENDED_SUPPORTED_RATES_ID,
+            info_elems, info_elem_len);
+    if (elem) {
+        int e_len = elem[1];
+        const unsigned char *e_ptr = elem + 2;
+
+        /*
+         * Count how many rates we have.
+         * Zero marks the end of the list, if the list is not truncated.
+         */
+        /* Max 8 values */
+        for (i = 0; i < e_len; i++) {
+            if (e_ptr[i] == 0) {
+                break;
+            }
+            /* Bit rate given in 500 kb/s units (+ 0x80) */
+            iwe.u.bitrate.value = ((e_ptr[i] & 0x7f) * 500000);
+            /* Add new value to event */
+            r = uf_iwe_stream_add_value(info, start_buf, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+            if (r < 0) {
+                return r;
+            }
+            current_val +=r;
+        }
+    }
+    /* Check if we added any rates event */
+    if ((current_val - start_buf) > IW_EV_LCP_LEN) {
+        start_buf = current_val;
+    }
+
+
+#if WIRELESS_EXT > 17
+    memset(&iwe, 0, sizeof(iwe));
+    iwe.cmd = IWEVGENIE;
+    iwe.u.data.length = info_elem_len;
+
+    r = uf_iwe_stream_add_point(info, start_buf, end_buf, &iwe, info_elems);
+    if (r < 0) {
+        return r;
+    }
+
+    start_buf += r;
+#endif /* WE > 17 */
+
+    return (start_buf - current_ev);
+} /* unifi_translate_scan() */
+
+
+
+static int
+unifi_giwscan(struct net_device *dev, struct iw_request_info *info,
+              union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_point *dwrq = &wrqu->data;
+    int r;
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_giwscan: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    unifi_trace(priv, UDBG1,
+            "unifi_giwscan: buffer (%d bytes) \n",
+            dwrq->length);
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_scan_results_get_async(priv, info, extra, dwrq->length);
+    UF_RTNL_LOCK();
+    if (r < 0) {
+        unifi_trace(priv, UDBG1,
+                "unifi_giwscan: buffer (%d bytes) not big enough.\n",
+                dwrq->length);
+        return r;
+    }
+
+    dwrq->length = r;
+    dwrq->flags = 0;
+
+    return 0;
+} /* unifi_giwscan() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_siwessid
+ *
+ *      Request to join a network or start and AdHoc.
+ *
+ *  Arguments:
+ *      dev             Pointer to network device struct.
+ *      info            Pointer to broken-out ioctl request.
+ *      data            Pointer to argument data.
+ *      essid           Pointer to string giving name of network to join
+ *                      or start
+ *
+ *  Returns:
+ *      0 on success and everything complete
+ *      -EINPROGRESS to have the higher level call the commit method.
+ * ---------------------------------------------------------------------------
+ */
+static int
+unifi_siwessid(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *data, char *essid)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int len;
+    int err = 0;
+
+    func_enter();
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwessid: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    len = 0;
+    if (data->flags & 1) {
+        /* Limit length  */
+        len = data->length;
+        if (len > UNIFI_MAX_SSID_LEN) {
+            len = UNIFI_MAX_SSID_LEN;
+        }
+    }
+
+#ifdef UNIFI_DEBUG
+    {
+        char essid_str[UNIFI_MAX_SSID_LEN+1];
+        int i;
+
+        for (i = 0; i < len; i++) {
+            essid_str[i] = (isprint(essid[i]) ? essid[i] : '?');
+        }
+        essid_str[i] = '\0';
+
+        unifi_trace(priv, UDBG1, "unifi_siwessid: asked for '%*s' (%d)\n", len, essid_str, len);
+        unifi_trace(priv, UDBG2, " with authModeMask = %d", priv->connection_config.authModeMask);
+    }
+#endif
+
+    memset(priv->connection_config.bssid.a, 0xFF, ETH_ALEN);
+    if (len) {
+        if (essid[len - 1] == 0) {
+            len --;
+        }
+
+        memcpy(priv->connection_config.ssid.ssid, essid, len);
+        priv->connection_config.ssid.length = len;
+
+    } else {
+        priv->connection_config.ssid.length = 0;
+    }
+
+    UF_RTNL_UNLOCK();
+    err = sme_mgt_connect(priv);
+    UF_RTNL_LOCK();
+    if (err) {
+        unifi_error(priv, "unifi_siwessid: Join failed, status %d\n", err);
+        func_exit();
+        return convert_sme_error(err);
+    }
+
+    func_exit();
+    return 0;
+} /* unifi_siwessid() */
+
+
+static int
+unifi_giwessid(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *essid)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_point *data = &wrqu->essid;
+    CsrWifiSmeConnectionInfo connectionInfo;
+    int r = 0;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_giwessid\n");
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_giwessid: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_connection_info_get(priv, &connectionInfo);
+    UF_RTNL_LOCK();
+
+    if (r == 0) {
+        data->length = connectionInfo.ssid.length;
+        strncpy(essid,
+                connectionInfo.ssid.ssid,
+                data->length);
+        data->flags = 1;            /* active */
+
+        unifi_trace(priv, UDBG2, "unifi_giwessid: %.*s\n",
+                data->length, essid);
+    }
+
+    func_exit();
+
+    return 0;
+} /* unifi_giwessid() */
+
+
+static int
+unifi_siwrate(struct net_device *dev, struct iw_request_info *info,
+              union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_param *args = &wrqu->bitrate;
+    CsrWifiSmeMibConfig mibConfig;
+    int r;
+
+    func_enter();
+
+    CHECK_INITED(priv);
+    unifi_trace(priv, UDBG2, "unifi_siwrate\n");
+
+    /*
+     * If args->fixed == 0, value is max rate or -1 for best
+     * If args->fixed == 1, value is rate to set or -1 for best
+     * args->disabled and args->flags are not used in SIOCSIWRATE
+     */
+
+    /* Get, modify and set the MIB data */
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwrate: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    /* Default to auto rate algorithm */
+    /* in 500Kbit/s, 0 means auto */
+    mibConfig.unifiFixTxDataRate = 0;
+
+    if (args->value != -1) {
+        mibConfig.unifiFixTxDataRate = args->value / 500000;
+    }
+
+    /* 1 means rate is a maximum, 2 means rate is a set value */
+    if (args->fixed == 1) {
+        mibConfig.unifiFixMaxTxDataRate = 0;
+    } else {
+        mibConfig.unifiFixMaxTxDataRate = 1;
+    }
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_set(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwrate: Set CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    func_exit();
+
+    return 0;
+} /* unifi_siwrate() */
+
+
+
+static int
+unifi_giwrate(struct net_device *dev, struct iw_request_info *info,
+              union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_param *args = &wrqu->bitrate;
+    int r;
+    int bitrate, flag;
+    CsrWifiSmeMibConfig mibConfig;
+    CsrWifiSmeConnectionStats connectionStats;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_giwrate\n");
+    CHECK_INITED(priv);
+
+    flag = 0;
+    bitrate = 0;
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_giwrate: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    bitrate = mibConfig.unifiFixTxDataRate;
+    flag = mibConfig.unifiFixMaxTxDataRate;
+
+    /* Used the value returned by the SME if MIB returns 0 */
+    if (bitrate == 0) {
+        UF_RTNL_UNLOCK();
+        r = sme_mgt_connection_stats_get(priv, &connectionStats);
+        UF_RTNL_LOCK();
+        /* Ignore errors, we may be disconnected */
+        if (r == 0) {
+            bitrate = connectionStats.unifiTxDataRate;
+        }
+    }
+
+    args->value = bitrate * 500000;
+    args->fixed = !flag;
+
+    func_exit();
+
+    return 0;
+} /* unifi_giwrate() */
+
+
+static int
+unifi_siwrts(struct net_device *dev, struct iw_request_info *info,
+             union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int val = wrqu->rts.value;
+    int r = 0;
+    CsrWifiSmeMibConfig mibConfig;
+
+    unifi_trace(priv, UDBG2, "unifi_siwrts\n");
+    CHECK_INITED(priv);
+
+    if (wrqu->rts.disabled) {
+        val = 2347;
+    }
+
+    if ( (val < 0) || (val > 2347) )
+    {
+        return -EINVAL;
+    }
+
+    /* Get, modify and set the MIB data */
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwrts: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+    mibConfig.dot11RtsThreshold = val;
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_set(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwrts: Set CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    return 0;
+}
+
+
+static int
+unifi_giwrts(struct net_device *dev, struct iw_request_info *info,
+             union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int r;
+    int rts_thresh;
+    CsrWifiSmeMibConfig mibConfig;
+
+    unifi_trace(priv, UDBG2, "unifi_giwrts\n");
+    CHECK_INITED(priv);
+
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_giwrts: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    rts_thresh = mibConfig.dot11RtsThreshold;
+    if (rts_thresh > 2347) {
+        rts_thresh = 2347;
+    }
+
+    wrqu->rts.value = rts_thresh;
+    wrqu->rts.disabled = (rts_thresh == 2347);
+    wrqu->rts.fixed = 1;
+
+    return 0;
+}
+
+
+static int
+unifi_siwfrag(struct net_device *dev, struct iw_request_info *info,
+              union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int val = wrqu->frag.value;
+    int r = 0;
+    CsrWifiSmeMibConfig mibConfig;
+
+    unifi_trace(priv, UDBG2, "unifi_siwfrag\n");
+    CHECK_INITED(priv);
+
+    if (wrqu->frag.disabled)
+        val = 2346;
+
+    if ( (val < 256) || (val > 2347) )
+        return -EINVAL;
+
+    /* Get, modify and set the MIB data */
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwfrag: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+    /* Fragmentation Threashold must be even */
+    mibConfig.dot11FragmentationThreshold = (val & ~0x1);
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_set(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwfrag: Set CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    return 0;
+}
+
+
+static int
+unifi_giwfrag(struct net_device *dev, struct iw_request_info *info,
+              union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int r;
+    int frag_thresh;
+    CsrWifiSmeMibConfig mibConfig;
+
+    unifi_trace(priv, UDBG2, "unifi_giwfrag\n");
+    CHECK_INITED(priv);
+
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_mib_config_get(priv, &mibConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_giwfrag: Get CsrWifiSmeMibConfigValue failed.\n");
+        return r;
+    }
+
+    frag_thresh = mibConfig.dot11FragmentationThreshold;
+
+    /* Build the return structure */
+    wrqu->frag.value = frag_thresh;
+    wrqu->frag.disabled = (frag_thresh >= 2346);
+    wrqu->frag.fixed = 1;
+
+    return 0;
+}
+
+
+static int
+unifi_siwencode(struct net_device *dev, struct iw_request_info *info,
+                union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_point *erq = &wrqu->encoding;
+    int index;
+    int rc = 0;
+    int privacy = -1;
+    CsrWifiSmeKey sme_key;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_siwencode\n");
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwencode: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    /*
+     * Key index is encoded in the flags.
+     * 0 - use current default,
+     * 1-4 - if a key value is given set that key
+     *       if not use that key
+     */
+    index = (erq->flags & IW_ENCODE_INDEX);  /* key number, 1-4 */
+    if ((index < 0) || (index > 4)) {
+        unifi_error(priv, "unifi_siwencode: Request to set an invalid key (index:%d)", index);
+        return -EINVAL;
+    }
+
+    /*
+     * Basic checking: do we have a key to set ?
+     * The IW_ENCODE_NOKEY flag is set when no key is present (only change flags),
+     * but older versions rely on sending a key id 1-4.
+     */
+    if (erq->length > 0) {
+
+        /* Check the size of the key */
+        if ((erq->length > LARGE_KEY_SIZE) || (erq->length < SMALL_KEY_SIZE)) {
+            unifi_error(priv, "unifi_siwencode: Request to set an invalid key (length:%d)",
+                        erq->length);
+            return -EINVAL;
+        }
+
+        /* Check the index (none (i.e. 0) means use current) */
+        if ((index < 1) || (index > 4)) {
+            /* If we do not have a previous key, use 1 as default */
+            if (!priv->wep_tx_key_index) {
+                priv->wep_tx_key_index = 1;
+            }
+            index = priv->wep_tx_key_index;
+        }
+
+        /* If we didn't have a key and a valid index is set, we want to remember it*/
+        if (!priv->wep_tx_key_index) {
+            priv->wep_tx_key_index = index;
+        }
+
+        unifi_trace(priv, UDBG1, "Tx key Index is %d\n", priv->wep_tx_key_index);
+
+        privacy = 1;
+
+        /* Check if the key is not marked as invalid */
+        if ((erq->flags & IW_ENCODE_NOKEY) == 0) {
+
+            unifi_trace(priv, UDBG1, "New %s key (len=%d, index=%d)\n",
+                        (priv->wep_tx_key_index == index) ? "tx" : "",
+                        erq->length, index);
+
+            sme_key.wepTxKey = (priv->wep_tx_key_index == index);
+            if (priv->wep_tx_key_index == index) {
+                sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_PAIRWISE;
+            } else {
+                sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_GROUP;
+            }
+            /* Key index is zero based in SME but 1 based in wext */
+            sme_key.keyIndex = (index - 1);
+            sme_key.keyLength = erq->length;
+            sme_key.authenticator = 0;
+            memset(sme_key.address.a, 0xFF, ETH_ALEN);
+            memcpy(sme_key.key, extra, erq->length);
+
+            UF_RTNL_UNLOCK();
+            rc = sme_mgt_key(priv, &sme_key, CSR_WIFI_SME_LIST_ACTION_ADD);
+            UF_RTNL_LOCK();
+            if (rc) {
+                unifi_error(priv, "unifi_siwencode: Set key failed (%d)", rc);
+                return convert_sme_error(rc);
+            }
+
+            /* Store the key to be reported by the SIOCGIWENCODE handler */
+            priv->wep_keys[index - 1].len = erq->length;
+            memcpy(priv->wep_keys[index - 1].key, extra, erq->length);
+        }
+    } else {
+        /*
+         * No additional key data, so it must be a request to change the
+         * active key.
+         */
+        if (index != 0) {
+            unifi_trace(priv, UDBG1, "Tx key Index is %d\n", index - 1);
+
+            /* Store the index to be reported by the SIOCGIWENCODE handler */
+            priv->wep_tx_key_index = index;
+
+            sme_key.wepTxKey = 1;
+            sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_PAIRWISE;
+
+            /* Key index is zero based in SME but 1 based in wext */
+            sme_key.keyIndex = (index - 1);
+            sme_key.keyLength = 0;
+            sme_key.authenticator = 0;
+            UF_RTNL_UNLOCK();
+            rc = sme_mgt_key(priv, &sme_key, CSR_WIFI_SME_LIST_ACTION_ADD);
+            UF_RTNL_LOCK();
+            if (rc) {
+                unifi_error(priv, "unifi_siwencode: Set key failed (%d)", rc);
+                return convert_sme_error(rc);
+            }
+
+            /* Turn on encryption */
+            privacy = 1;
+        }
+    }
+
+    /* Read the flags */
+    if (erq->flags & IW_ENCODE_DISABLED) {
+        /* disable encryption */
+        unifi_trace(priv, UDBG1, "disable WEP encryption\n");
+        privacy = 0;
+
+        priv->wep_tx_key_index = 0;
+
+        unifi_trace(priv, UDBG1, "IW_ENCODE_DISABLED: CSR_WIFI_SME_AUTH_MODE_80211_OPEN\n");
+        priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+    }
+
+    if (erq->flags & IW_ENCODE_RESTRICTED) {
+        /* Use shared key auth */
+        unifi_trace(priv, UDBG1, "IW_ENCODE_RESTRICTED: CSR_WIFI_SME_AUTH_MODE_80211_SHARED\n");
+        priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_SHARED;
+
+        /* Turn on encryption */
+        privacy = 1;
+    }
+    if (erq->flags & IW_ENCODE_OPEN) {
+        unifi_trace(priv, UDBG1, "IW_ENCODE_OPEN: CSR_WIFI_SME_AUTH_MODE_80211_OPEN\n");
+        priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+    }
+
+    /* Commit the changes to flags if needed */
+    if (privacy != -1) {
+        priv->connection_config.privacyMode = privacy ? CSR_WIFI_SME_80211_PRIVACY_MODE_ENABLED : CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED;
+        priv->connection_config.encryptionModeMask = privacy ? (CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP40 |
+                CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP104 |
+                CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP40 |
+                CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP104) :
+            CSR_WIFI_SME_ENCRYPTION_CIPHER_NONE;
+    }
+
+    func_exit_r(rc);
+    return convert_sme_error(rc);
+
+} /* unifi_siwencode() */
+
+
+
+static int
+unifi_giwencode(struct net_device *dev, struct iw_request_info *info,
+                union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_point *erq = &wrqu->encoding;
+
+    unifi_trace(priv, UDBG2, "unifi_giwencode\n");
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_giwencode: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    if (priv->connection_config.authModeMask == CSR_WIFI_SME_AUTH_MODE_80211_SHARED) {
+        erq->flags = IW_ENCODE_RESTRICTED;
+    }
+    else {
+        if (priv->connection_config.privacyMode == CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED) {
+            erq->flags = IW_ENCODE_DISABLED;
+        } else {
+            erq->flags = IW_ENCODE_OPEN;
+        }
+    }
+
+    erq->length = 0;
+
+    if (erq->flags != IW_ENCODE_DISABLED) {
+        int index = priv->wep_tx_key_index;
+
+        if ((index > 0) && (index <= NUM_WEPKEYS)) {
+            erq->flags |= (index & IW_ENCODE_INDEX);
+            erq->length = priv->wep_keys[index - 1].len;
+            memcpy(extra, priv->wep_keys[index - 1].key, erq->length);
+        } else {
+            unifi_notice(priv, "unifi_giwencode: Surprise, do not have a valid key index (%d)\n",
+                         index);
+        }
+    }
+
+    return 0;
+} /* unifi_giwencode() */
+
+
+static int
+unifi_siwpower(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *extra)
+{
+    struct iw_param *args = &wrqu->power;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int listen_interval, wake_for_dtim;
+    int r = 0;
+    CsrWifiSmePowerConfig powerConfig;
+
+    unifi_trace(priv, UDBG2, "unifi_siwpower\n");
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwpower: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_power_config_get(priv, &powerConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwpower: Get unifi_PowerConfigValue failed.\n");
+        return r;
+    }
+
+    listen_interval = -1;
+    wake_for_dtim = -1;
+    if (args->disabled) {
+        powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW;
+    }
+    else
+    {
+        powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_HIGH;
+
+        switch (args->flags & IW_POWER_TYPE) {
+            case 0:
+                /* not specified */
+                break;
+            case IW_POWER_PERIOD:
+                listen_interval = args->value / 1000;
+                break;
+            default:
+                return -EINVAL;
+        }
+
+        switch (args->flags & IW_POWER_MODE) {
+            case 0:
+                /* not specified */
+                break;
+            case IW_POWER_UNICAST_R:
+                /* not interested in broadcast packets */
+                wake_for_dtim = 0;
+                break;
+            case IW_POWER_ALL_R:
+                /* yes, we are interested in broadcast packets */
+                wake_for_dtim = 1;
+                break;
+            default:
+                return -EINVAL;
+        }
+    }
+
+    if (listen_interval > 0) {
+        powerConfig.listenIntervalTu = listen_interval;
+        unifi_trace(priv, UDBG4, "unifi_siwpower: new Listen Interval = %d.\n",
+                    powerConfig.listenIntervalTu);
+    }
+
+    if (wake_for_dtim >= 0) {
+        powerConfig.rxDtims = wake_for_dtim;
+    }
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_power_config_set(priv, &powerConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_siwpower: Set unifi_PowerConfigValue failed.\n");
+        return r;
+    }
+
+    return 0;
+} /* unifi_siwpower() */
+
+
+static int
+unifi_giwpower(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *extra)
+{
+    struct iw_param *args = &wrqu->power;
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    CsrWifiSmePowerConfig powerConfig;
+    int r;
+
+    unifi_trace(priv, UDBG2, "unifi_giwpower\n");
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_giwpower: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    args->flags = 0;
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_power_config_get(priv, &powerConfig);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "unifi_giwpower: Get unifi_PowerConfigValue failed.\n");
+        return r;
+    }
+
+    unifi_trace(priv, UDBG4, "unifi_giwpower: mode=%d\n",
+                powerConfig.powerSaveLevel);
+
+    args->disabled = (powerConfig.powerSaveLevel == CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW);
+    if (args->disabled) {
+        args->flags = 0;
+        return 0;
+    }
+
+    args->value = powerConfig.listenIntervalTu * 1000;
+    args->flags |= IW_POWER_PERIOD;
+
+    if (powerConfig.rxDtims) {
+        args->flags |= IW_POWER_ALL_R;
+    } else {
+        args->flags |= IW_POWER_UNICAST_R;
+    }
+
+    return 0;
+} /* unifi_giwpower() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_siwcommit - handler for SIOCSIWCOMMIT
+ *
+ *      Apply all the parameters that have been set.
+ *      In practice this means:
+ *       - do a scan
+ *       - join a network or start an AdHoc
+ *       - authenticate and associate.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static int
+unifi_siwcommit(struct net_device *dev, struct iw_request_info *info,
+                union iwreq_data *wrqu, char *extra)
+{
+    return 0;
+} /* unifi_siwcommit() */
+
+
+
+static int
+unifi_siwmlme(struct net_device *dev, struct iw_request_info *info,
+              union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_mlme *mlme = (struct iw_mlme *)extra;
+    func_enter();
+
+    unifi_trace(priv, UDBG2, "unifi_siwmlme\n");
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwmlme: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    switch (mlme->cmd) {
+        case IW_MLME_DEAUTH:
+        case IW_MLME_DISASSOC:
+            UF_RTNL_UNLOCK();
+            sme_mgt_disconnect(priv);
+            UF_RTNL_LOCK();
+            break;
+        default:
+            func_exit_r(-EOPNOTSUPP);
+            return -EOPNOTSUPP;
+    }
+
+    func_exit();
+    return 0;
+} /* unifi_siwmlme() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_siwgenie
+ *  unifi_giwgenie
+ *
+ *      WPA : Generic IEEE 802.11 information element (e.g., for WPA/RSN/WMM).
+ *      Handlers for SIOCSIWGENIE, SIOCGIWGENIE - set/get generic IE
+ *
+ *      The host program (e.g. wpa_supplicant) uses this call to set the
+ *      additional IEs to accompany the next (Associate?) request.
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ *  Notes:
+ *      From wireless.h:
+ *        This ioctl uses struct iw_point and data buffer that includes IE id
+ *        and len fields. More than one IE may be included in the
+ *        request. Setting the generic IE to empty buffer (len=0) removes the
+ *        generic IE from the driver.
+ * ---------------------------------------------------------------------------
+ */
+static int
+unifi_siwgenie(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int len;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_siwgenie\n");
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwgenie: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    if ( priv->connection_config.mlmeAssociateReqInformationElements) {
+        kfree( priv->connection_config.mlmeAssociateReqInformationElements);
+    }
+    priv->connection_config.mlmeAssociateReqInformationElementsLength = 0;
+    priv->connection_config.mlmeAssociateReqInformationElements = NULL;
+
+    len = wrqu->data.length;
+    if (len == 0) {
+        func_exit();
+        return 0;
+    }
+
+    priv->connection_config.mlmeAssociateReqInformationElements = kmalloc(len, GFP_KERNEL);
+    if (priv->connection_config.mlmeAssociateReqInformationElements == NULL) {
+        func_exit();
+        return -ENOMEM;
+    }
+
+    priv->connection_config.mlmeAssociateReqInformationElementsLength = len;
+    memcpy( priv->connection_config.mlmeAssociateReqInformationElements, extra, len);
+
+    func_exit();
+    return 0;
+} /* unifi_siwgenie() */
+
+
+static int
+unifi_giwgenie(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    int len;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_giwgenie\n");
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_giwgenie: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    len = priv->connection_config.mlmeAssociateReqInformationElementsLength;
+
+    if (len == 0) {
+        wrqu->data.length = 0;
+        return 0;
+    }
+
+    if (wrqu->data.length < len) {
+        return -E2BIG;
+    }
+
+    wrqu->data.length = len;
+    memcpy(extra, priv->connection_config.mlmeAssociateReqInformationElements, len);
+
+    func_exit();
+    return 0;
+} /* unifi_giwgenie() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_siwauth
+ *  unifi_giwauth
+ *
+ *      Handlers for SIOCSIWAUTH, SIOCGIWAUTH
+ *      Set/get various authentication parameters.
+ *
+ *  Arguments:
+ *
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static int
+_unifi_siwauth(struct net_device *dev, struct iw_request_info *info,
+               union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    CsrWifiSmeAuthModeMask new_auth;
+
+    func_enter();
+    unifi_trace(priv, UDBG2, "unifi_siwauth\n");
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwauth: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    /*
+     * This ioctl is safe to call even when UniFi is powered off.
+     * wpa_supplicant calls it to test whether we support WPA.
+     */
+
+    switch (wrqu->param.flags & IW_AUTH_INDEX) {
+
+        case IW_AUTH_WPA_ENABLED:
+            unifi_trace(priv, UDBG1, "IW_AUTH_WPA_ENABLED: %d\n", wrqu->param.value);
+
+            if (wrqu->param.value == 0) {
+                unifi_trace(priv, UDBG5, "IW_AUTH_WPA_ENABLED: CSR_WIFI_SME_AUTH_MODE_80211_OPEN\n");
+                priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+            }
+            break;
+
+        case IW_AUTH_PRIVACY_INVOKED:
+            unifi_trace(priv, UDBG1, "IW_AUTH_PRIVACY_INVOKED: %d\n", wrqu->param.value);
+
+            priv->connection_config.privacyMode = wrqu->param.value ? CSR_WIFI_SME_80211_PRIVACY_MODE_ENABLED : CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED;
+            if (wrqu->param.value == CSR_WIFI_SME_80211_PRIVACY_MODE_DISABLED)
+            {
+                priv->connection_config.encryptionModeMask = CSR_WIFI_SME_ENCRYPTION_CIPHER_NONE;
+            }
+            break;
+
+        case IW_AUTH_80211_AUTH_ALG:
+            /*
+               IW_AUTH_ALG_OPEN_SYSTEM      0x00000001
+               IW_AUTH_ALG_SHARED_KEY       0x00000002
+               IW_AUTH_ALG_LEAP             0x00000004
+               */
+            new_auth = 0;
+            if (wrqu->param.value & IW_AUTH_ALG_OPEN_SYSTEM) {
+                unifi_trace(priv, UDBG1, "IW_AUTH_80211_AUTH_ALG: %d (IW_AUTH_ALG_OPEN_SYSTEM)\n", wrqu->param.value);
+                new_auth |= CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+            }
+            if (wrqu->param.value & IW_AUTH_ALG_SHARED_KEY) {
+                unifi_trace(priv, UDBG1, "IW_AUTH_80211_AUTH_ALG: %d (IW_AUTH_ALG_SHARED_KEY)\n", wrqu->param.value);
+                new_auth |= CSR_WIFI_SME_AUTH_MODE_80211_SHARED;
+            }
+            if (wrqu->param.value & IW_AUTH_ALG_LEAP) {
+                /* Initial exchanges using open-system to set EAP */
+                unifi_trace(priv, UDBG1, "IW_AUTH_80211_AUTH_ALG: %d (IW_AUTH_ALG_LEAP)\n", wrqu->param.value);
+                new_auth |= CSR_WIFI_SME_AUTH_MODE_8021X_OTHER1X;
+            }
+            if (new_auth == 0) {
+                unifi_trace(priv, UDBG1, "IW_AUTH_80211_AUTH_ALG: invalid value %d\n",
+                        wrqu->param.value);
+                return -EINVAL;
+            } else {
+                priv->connection_config.authModeMask = new_auth;
+            }
+            break;
+
+        case IW_AUTH_WPA_VERSION:
+            unifi_trace(priv, UDBG1, "IW_AUTH_WPA_VERSION: %d\n", wrqu->param.value);
+            priv->ignore_bssid_join = TRUE;
+            /*
+               IW_AUTH_WPA_VERSION_DISABLED 0x00000001
+               IW_AUTH_WPA_VERSION_WPA      0x00000002
+               IW_AUTH_WPA_VERSION_WPA2     0x00000004
+               */
+
+            if (!(wrqu->param.value & IW_AUTH_WPA_VERSION_DISABLED)) {
+
+                priv->connection_config.authModeMask = CSR_WIFI_SME_AUTH_MODE_80211_OPEN;
+
+                if (wrqu->param.value & IW_AUTH_WPA_VERSION_WPA) {
+                    unifi_trace(priv, UDBG4, "IW_AUTH_WPA_VERSION: WPA, WPA-PSK\n");
+                    priv->connection_config.authModeMask |= (CSR_WIFI_SME_AUTH_MODE_8021X_WPA | CSR_WIFI_SME_AUTH_MODE_8021X_WPAPSK);
+                }
+                if (wrqu->param.value & IW_AUTH_WPA_VERSION_WPA2) {
+                    unifi_trace(priv, UDBG4, "IW_AUTH_WPA_VERSION: WPA2, WPA2-PSK\n");
+                    priv->connection_config.authModeMask |= (CSR_WIFI_SME_AUTH_MODE_8021X_WPA2 | CSR_WIFI_SME_AUTH_MODE_8021X_WPA2PSK);
+                }
+            }
+            break;
+
+        case IW_AUTH_CIPHER_PAIRWISE:
+            unifi_trace(priv, UDBG1, "IW_AUTH_CIPHER_PAIRWISE: %d\n", wrqu->param.value);
+            /*
+             * one of:
+             IW_AUTH_CIPHER_NONE	0x00000001
+             IW_AUTH_CIPHER_WEP40	0x00000002
+             IW_AUTH_CIPHER_TKIP	0x00000004
+             IW_AUTH_CIPHER_CCMP	0x00000008
+             IW_AUTH_CIPHER_WEP104	0x00000010
+             */
+
+            priv->connection_config.encryptionModeMask = CSR_WIFI_SME_ENCRYPTION_CIPHER_NONE;
+
+            if (wrqu->param.value & IW_AUTH_CIPHER_WEP40) {
+                priv->connection_config.encryptionModeMask |=
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP40 | CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP40;
+            }
+            if (wrqu->param.value & IW_AUTH_CIPHER_WEP104) {
+                priv->connection_config.encryptionModeMask |=
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_WEP104 | CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP104;
+            }
+            if (wrqu->param.value & IW_AUTH_CIPHER_TKIP) {
+                priv->connection_config.encryptionModeMask |=
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_TKIP | CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_TKIP;
+            }
+            if (wrqu->param.value & IW_AUTH_CIPHER_CCMP) {
+                priv->connection_config.encryptionModeMask |=
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_PAIRWISE_CCMP | CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_CCMP;
+            }
+
+            break;
+
+        case IW_AUTH_CIPHER_GROUP:
+            unifi_trace(priv, UDBG1, "IW_AUTH_CIPHER_GROUP: %d\n", wrqu->param.value);
+            /*
+             * Use the WPA version and the group cipher suite to set the permitted
+             * group key in the MIB. f/w uses this value to validate WPA and RSN IEs
+             * in the probe responses from the desired BSS(ID)
+             */
+
+            priv->connection_config.encryptionModeMask &= ~(CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP40 |
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP104 |
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_TKIP |
+                    CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_CCMP);
+            if (wrqu->param.value & IW_AUTH_CIPHER_WEP40) {
+                priv->connection_config.encryptionModeMask |= CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP40;
+            }
+            if (wrqu->param.value & IW_AUTH_CIPHER_WEP104) {
+                priv->connection_config.encryptionModeMask |= CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_WEP104;
+            }
+            if (wrqu->param.value & IW_AUTH_CIPHER_TKIP) {
+                priv->connection_config.encryptionModeMask |= CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_TKIP;
+            }
+            if (wrqu->param.value & IW_AUTH_CIPHER_CCMP) {
+                priv->connection_config.encryptionModeMask |= CSR_WIFI_SME_ENCRYPTION_CIPHER_GROUP_CCMP;
+            }
+
+            break;
+
+        case IW_AUTH_KEY_MGMT:
+            unifi_trace(priv, UDBG1, "IW_AUTH_KEY_MGMT: %d\n", wrqu->param.value);
+            /*
+               IW_AUTH_KEY_MGMT_802_1X 1
+               IW_AUTH_KEY_MGMT_PSK    2
+               */
+            if (priv->connection_config.authModeMask & (CSR_WIFI_SME_AUTH_MODE_8021X_WPA | CSR_WIFI_SME_AUTH_MODE_8021X_WPAPSK)) {
+                /* Check for explicitly set mode. */
+                if (wrqu->param.value == IW_AUTH_KEY_MGMT_802_1X) {
+                    priv->connection_config.authModeMask &= ~CSR_WIFI_SME_AUTH_MODE_8021X_WPAPSK;
+                }
+                if (wrqu->param.value == IW_AUTH_KEY_MGMT_PSK) {
+                    priv->connection_config.authModeMask &= ~CSR_WIFI_SME_AUTH_MODE_8021X_WPA;
+                }
+                unifi_trace(priv, UDBG5, "IW_AUTH_KEY_MGMT: WPA: %d\n",
+                            priv->connection_config.authModeMask);
+            }
+            if (priv->connection_config.authModeMask & (CSR_WIFI_SME_AUTH_MODE_8021X_WPA2 | CSR_WIFI_SME_AUTH_MODE_8021X_WPA2PSK)) {
+                /* Check for explicitly set mode. */
+                if (wrqu->param.value == IW_AUTH_KEY_MGMT_802_1X) {
+                    priv->connection_config.authModeMask &= ~CSR_WIFI_SME_AUTH_MODE_8021X_WPA2PSK;
+                }
+                if (wrqu->param.value == IW_AUTH_KEY_MGMT_PSK) {
+                    priv->connection_config.authModeMask &= ~CSR_WIFI_SME_AUTH_MODE_8021X_WPA2;
+                }
+                unifi_trace(priv, UDBG5, "IW_AUTH_KEY_MGMT: WPA2: %d\n",
+                            priv->connection_config.authModeMask);
+            }
+
+            break;
+        case IW_AUTH_TKIP_COUNTERMEASURES:
+            /*
+             * Set to true at the start of the 60 second backup-off period
+             * following 2 MichaelMIC failures within 60s.
+             */
+            unifi_trace(priv, UDBG1, "IW_AUTH_TKIP_COUNTERMEASURES: %d\n", wrqu->param.value);
+            break;
+
+        case IW_AUTH_DROP_UNENCRYPTED:
+            /*
+             * Set to true on init.
+             * Set to false just before associate if encryption will not be
+             * required.
+             *
+             * Note this is not the same as the 802.1X controlled port
+             */
+            unifi_trace(priv, UDBG1, "IW_AUTH_DROP_UNENCRYPTED: %d\n", wrqu->param.value);
+            break;
+
+        case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            /*
+             * This is set by wpa_supplicant to allow unencrypted EAPOL messages
+             * even if pairwise keys are set when not using WPA. IEEE 802.1X
+             * specifies that these frames are not encrypted, but WPA encrypts
+             * them when pairwise keys are in use.
+             * I think the UniFi f/w handles this decision for us.
+             */
+            unifi_trace(priv, UDBG1, "IW_AUTH_RX_UNENCRYPTED_EAPOL: %d\n", wrqu->param.value);
+            break;
+
+        case IW_AUTH_ROAMING_CONTROL:
+            unifi_trace(priv, UDBG1, "IW_AUTH_ROAMING_CONTROL: %d\n", wrqu->param.value);
+            break;
+
+        default:
+            unifi_trace(priv, UDBG1, "Unsupported auth param %d to 0x%X\n",
+                        wrqu->param.flags & IW_AUTH_INDEX,
+                        wrqu->param.value);
+            return -EOPNOTSUPP;
+    }
+
+    unifi_trace(priv, UDBG2, "authModeMask = %d", priv->connection_config.authModeMask);
+    func_exit();
+
+    return 0;
+} /* _unifi_siwauth() */
+
+
+static int
+unifi_siwauth(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    int err = 0;
+
+    UF_RTNL_UNLOCK();
+    err = _unifi_siwauth(dev, info, wrqu, extra);
+    UF_RTNL_LOCK();
+
+    return err;
+} /* unifi_siwauth() */
+
+
+static int
+unifi_giwauth(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    unifi_trace(NULL, UDBG2, "unifi_giwauth\n");
+    return -EOPNOTSUPP;
+} /* unifi_giwauth() */
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_siwencodeext
+ *  unifi_giwencodeext
+ *
+ *      Handlers for SIOCSIWENCODEEXT, SIOCGIWENCODEEXT - set/get
+ *      encoding token & mode
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      For WPA/WPA2 we don't take note of the IW_ENCODE_EXT_SET_TX_KEY flag.
+ *      This flag means "use this key to encode transmissions"; we just
+ *      assume only one key will be set and that is the one to use.
+ * ---------------------------------------------------------------------------
+ */
+static int
+_unifi_siwencodeext(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int r = 0;
+    unsigned char *keydata;
+    unsigned char tkip_key[32];
+    int keyid;
+    unsigned char *a = (unsigned char *)ext->addr.sa_data;
+    CsrWifiSmeKey sme_key;
+    CsrWifiSmeKeyType key_type;
+
+    func_enter();
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwencodeext: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+    unifi_trace(priv, UDBG1, "siwencodeext: flags=0x%X, alg=%d, ext_flags=0x%X, len=%d, index=%d,\n",
+                wrqu->encoding.flags, ext->alg, ext->ext_flags,
+                ext->key_len, (wrqu->encoding.flags & IW_ENCODE_INDEX));
+	unifi_trace(priv, UDBG3, "              addr=%pM\n", a);
+
+    if ((ext->key_len == 0) && (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)) {
+        /* This means use a different key (given by key_idx) for Tx. */
+        /* NYI */
+        unifi_trace(priv, UDBG1, KERN_ERR "unifi_siwencodeext: NYI should change tx key id here!!\n");
+        return -ENOTSUPP;
+    }
+
+    memset(&sme_key, 0, sizeof(sme_key));
+
+    keydata = (unsigned char *)(ext + 1);
+    keyid = (wrqu->encoding.flags & IW_ENCODE_INDEX);
+
+    /*
+     * Check for request to delete keys for an address.
+     */
+    /* Pick out request for no privacy. */
+    if (ext->alg == IW_ENCODE_ALG_NONE) {
+
+        unifi_trace(priv, UDBG1, "Deleting %s key %d\n",
+                    (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) ? "GROUP" : "PAIRWISE",
+                    keyid);
+
+        if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
+            sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_GROUP;
+        } else {
+            sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_PAIRWISE;
+        }
+        sme_key.keyIndex = (keyid - 1);
+        sme_key.keyLength = 0;
+        sme_key.authenticator = 0;
+        memcpy(sme_key.address.a, a, ETH_ALEN);
+        UF_RTNL_UNLOCK();
+        r = sme_mgt_key(priv, &sme_key, CSR_WIFI_SME_LIST_ACTION_REMOVE);
+        UF_RTNL_LOCK();
+        if (r) {
+            unifi_error(priv, "Delete key request was rejected with result %d\n", r);
+            return convert_sme_error(r);
+        }
+
+        return 0;
+    }
+
+    /*
+     * Request is to set a key, not delete
+     */
+
+    /* Pick out WEP and use set_wep_key(). */
+    if (ext->alg == IW_ENCODE_ALG_WEP) {
+        /* WEP-40, WEP-104 */
+
+        /* Check for valid key length */
+        if (!((ext->key_len == 5) || (ext->key_len == 13))) {
+            unifi_trace(priv, UDBG1, KERN_ERR "Invalid length for WEP key: %d\n", ext->key_len);
+            return -EINVAL;
+        }
+
+        unifi_trace(priv, UDBG1, "Setting WEP key %d tx:%d\n",
+                    keyid, ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY);
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+            sme_key.wepTxKey = TRUE;
+            sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_PAIRWISE;
+        } else {
+            sme_key.wepTxKey = FALSE;
+            sme_key.keyType = CSR_WIFI_SME_KEY_TYPE_GROUP;
+        }
+        sme_key.keyIndex = (keyid - 1);
+        sme_key.keyLength = ext->key_len;
+        sme_key.authenticator = 0;
+        memset(sme_key.address.a, 0xFF, ETH_ALEN);
+        memcpy(sme_key.key, keydata, ext->key_len);
+        UF_RTNL_UNLOCK();
+        r = sme_mgt_key(priv, &sme_key, CSR_WIFI_SME_LIST_ACTION_ADD);
+        UF_RTNL_LOCK();
+        if (r) {
+            unifi_error(priv, "siwencodeext: Set key failed (%d)", r);
+            return convert_sme_error(r);
+        }
+
+        return 0;
+    }
+
+    /*
+     *
+     * If we reach here, we are dealing with a WPA/WPA2 key
+     *
+     */
+    if (ext->key_len > 32) {
+        return -EINVAL;
+    }
+
+    /*
+     * TKIP keys from wpa_supplicant need swapping.
+     * What about other supplicants (when they come along)?
+     */
+    if ((ext->alg == IW_ENCODE_ALG_TKIP) && (ext->key_len == 32)) {
+        memcpy(tkip_key, keydata, 16);
+        memcpy(tkip_key + 16, keydata + 24, 8);
+        memcpy(tkip_key + 24, keydata + 16, 8);
+        keydata = tkip_key;
+    }
+
+    key_type = (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) ?
+        CSR_WIFI_SME_KEY_TYPE_GROUP : /* Group Key */
+        CSR_WIFI_SME_KEY_TYPE_PAIRWISE; /* Pairwise Key */
+
+    sme_key.keyType = key_type;
+    sme_key.keyIndex = (keyid - 1);
+    sme_key.keyLength = ext->key_len;
+    sme_key.authenticator = 0;
+    memcpy(sme_key.address.a, ext->addr.sa_data, ETH_ALEN);
+    if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+
+        unifi_trace(priv, UDBG5, "RSC first 6 bytes = %02X:%02X:%02X:%02X:%02X:%02X\n",
+                    ext->rx_seq[0], ext->rx_seq[1], ext->rx_seq[2], ext->rx_seq[3], ext->rx_seq[4], ext->rx_seq[5]);
+
+        /* memcpy((u8*)(&sme_key.keyRsc), ext->rx_seq, 8); */
+        sme_key.keyRsc[0] = ext->rx_seq[1] << 8 | ext->rx_seq[0];
+        sme_key.keyRsc[1] = ext->rx_seq[3] << 8 | ext->rx_seq[2];
+        sme_key.keyRsc[2] = ext->rx_seq[5] << 8 | ext->rx_seq[4];
+        sme_key.keyRsc[3] = ext->rx_seq[7] << 8 | ext->rx_seq[6];
+
+    }
+
+    memcpy(sme_key.key, keydata, ext->key_len);
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_key(priv, &sme_key, CSR_WIFI_SME_LIST_ACTION_ADD);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "SETKEYS request was rejected with result %d\n", r);
+        return convert_sme_error(r);
+    }
+
+    func_exit();
+    return r;
+} /* _unifi_siwencodeext() */
+
+
+static int
+unifi_siwencodeext(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    int err = 0;
+
+    err = _unifi_siwencodeext(dev, info, wrqu, extra);
+
+    return err;
+} /* unifi_siwencodeext() */
+
+
+static int
+unifi_giwencodeext(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    return -EOPNOTSUPP;
+} /* unifi_giwencodeext() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_siwpmksa
+ *
+ *      SIOCSIWPMKSA - PMKSA cache operation
+ *      The caller passes a pmksa structure:
+ *        - cmd         one of ADD, REMOVE, FLUSH
+ *        - bssid       MAC address
+ *        - pmkid       ID string (16 bytes)
+ *
+ *  Arguments:
+ *      None.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      This is not needed since we provide a siwgenie method.
+ * ---------------------------------------------------------------------------
+ */
+#define UNIFI_PMKID_KEY_SIZE 16
+static int
+unifi_siwpmksa(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrqu, char *extra)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+    struct iw_pmksa *pmksa = (struct iw_pmksa *)extra;
+    CsrResult r = 0;
+    CsrWifiSmePmkidList pmkid_list;
+    CsrWifiSmePmkid pmkid;
+    CsrWifiSmeListAction action;
+
+    CHECK_INITED(priv);
+
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+       unifi_error(priv, "unifi_siwpmksa: not permitted in Mode %d\n",
+                                      interfacePriv->interfaceMode);
+       return -EPERM;
+    }
+
+
+	unifi_trace(priv, UDBG1, "SIWPMKSA: cmd %d, %pM\n", pmksa->cmd,
+		pmksa->bssid.sa_data);
+
+    pmkid_list.pmkids = NULL;
+    switch (pmksa->cmd) {
+      case IW_PMKSA_ADD:
+        pmkid_list.pmkids = &pmkid;
+        action = CSR_WIFI_SME_LIST_ACTION_ADD;
+        pmkid_list.pmkidsCount = 1;
+        memcpy(pmkid.bssid.a, pmksa->bssid.sa_data, ETH_ALEN);
+        memcpy(pmkid.pmkid, pmksa->pmkid, UNIFI_PMKID_KEY_SIZE);
+        break;
+      case IW_PMKSA_REMOVE:
+        pmkid_list.pmkids = &pmkid;
+        action = CSR_WIFI_SME_LIST_ACTION_REMOVE;
+        pmkid_list.pmkidsCount = 1;
+        memcpy(pmkid.bssid.a, pmksa->bssid.sa_data, ETH_ALEN);
+        memcpy(pmkid.pmkid, pmksa->pmkid, UNIFI_PMKID_KEY_SIZE);
+        break;
+      case IW_PMKSA_FLUSH:
+        /* Replace current PMKID's with an empty list */
+        pmkid_list.pmkidsCount = 0;
+        action = CSR_WIFI_SME_LIST_ACTION_FLUSH;
+        break;
+      default:
+        unifi_notice(priv, "SIWPMKSA: Unknown command (0x%x)\n", pmksa->cmd);
+        return -EINVAL;
+    }
+
+    /* Set the Value the pmkid's will have 1 added OR 1 removed OR be cleared at this point */
+    UF_RTNL_UNLOCK();
+    r = sme_mgt_pmkid(priv, action, &pmkid_list);
+    UF_RTNL_LOCK();
+    if (r) {
+        unifi_error(priv, "SIWPMKSA: Set PMKID's Failed.\n");
+    }
+
+    return r;
+
+} /* unifi_siwpmksa() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_get_wireless_stats
+ *
+ *      get_wireless_stats method for Linux wireless extensions.
+ *
+ *  Arguments:
+ *      dev             Pointer to associated netdevice.
+ *
+ *  Returns:
+ *      Pointer to iw_statistics struct.
+ * ---------------------------------------------------------------------------
+ */
+struct iw_statistics *
+unifi_get_wireless_stats(struct net_device *dev)
+{
+    netInterface_priv_t *interfacePriv = (netInterface_priv_t *)netdev_priv(dev);
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    if (priv->init_progress != UNIFI_INIT_COMPLETED) {
+        return NULL;
+    }
+
+    return &priv->wext_wireless_stats;
+} /* unifi_get_wireless_stats() */
+
+
+/*
+ * Structures to export the Wireless Handlers
+ */
+
+static const struct iw_priv_args unifi_private_args[] = {
+    /*{ cmd,         set_args,                            get_args, name } */
+    { SIOCIWS80211POWERSAVEPRIV, IW_PRIV_TYPE_BYTE | IW_PRIV_SIZE_FIXED | 1,
+        IW_PRIV_TYPE_NONE, "iwprivs80211ps" },
+    { SIOCIWG80211POWERSAVEPRIV, IW_PRIV_TYPE_NONE,
+        IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | IWPRIV_POWER_SAVE_MAX_STRING, "iwprivg80211ps" },
+    { SIOCIWS80211RELOADDEFAULTSPRIV, IW_PRIV_TYPE_NONE,
+        IW_PRIV_TYPE_NONE, "iwprivsdefs" },
+    { SIOCIWSSMEDEBUGPRIV, IW_PRIV_TYPE_CHAR | IWPRIV_SME_DEBUG_MAX_STRING, IW_PRIV_TYPE_NONE, "iwprivssmedebug" },
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    { SIOCIWSCONFWAPIPRIV, IW_PRIV_TYPE_BYTE | IW_PRIV_SIZE_FIXED | 1,
+        IW_PRIV_TYPE_NONE, "iwprivsconfwapi" },
+    { SIOCIWSWAPIKEYPRIV, IW_PRIV_TYPE_BYTE | IW_PRIV_SIZE_FIXED | sizeof(unifiio_wapi_key_t),
+        IW_PRIV_TYPE_NONE, "iwprivswpikey" },
+#endif
+#ifdef CSR_SUPPORT_WEXT_AP
+    { SIOCIWSAPCFGPRIV, IW_PRIV_TYPE_CHAR | 256, IW_PRIV_TYPE_NONE, "AP_SET_CFG" },
+    { SIOCIWSAPSTARTPRIV, 0,IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED|IWPRIV_SME_MAX_STRING,"AP_BSS_START" },
+    { SIOCIWSAPSTOPPRIV, IW_PRIV_TYPE_CHAR |IW_PRIV_SIZE_FIXED|0,
+      IW_PRIV_TYPE_CHAR |IW_PRIV_SIZE_FIXED|0, "AP_BSS_STOP" },
+#ifdef ANDROID_BUILD
+    { SIOCIWSFWRELOADPRIV, IW_PRIV_TYPE_CHAR |256,
+      IW_PRIV_TYPE_CHAR |IW_PRIV_SIZE_FIXED|0, "WL_FW_RELOAD" },
+    { SIOCIWSSTACKSTART, 0,
+      IW_PRIV_TYPE_CHAR |IW_PRIV_SIZE_FIXED|IWPRIV_SME_MAX_STRING, "START" },
+    { SIOCIWSSTACKSTOP, 0,
+      IW_PRIV_TYPE_CHAR |IW_PRIV_SIZE_FIXED|IWPRIV_SME_MAX_STRING, "STOP" },
+#endif /* ANDROID_BUILD */
+#endif /* CSR_SUPPORT_WEXT_AP */
+};
+
+static const iw_handler unifi_handler[] =
+{
+    (iw_handler) unifi_siwcommit,           /* SIOCSIWCOMMIT */
+    (iw_handler) unifi_giwname,             /* SIOCGIWNAME */
+    (iw_handler) NULL,                      /* SIOCSIWNWID */
+    (iw_handler) NULL,                      /* SIOCGIWNWID */
+    (iw_handler) unifi_siwfreq,             /* SIOCSIWFREQ */
+    (iw_handler) unifi_giwfreq,             /* SIOCGIWFREQ */
+    (iw_handler) unifi_siwmode,             /* SIOCSIWMODE */
+    (iw_handler) unifi_giwmode,             /* SIOCGIWMODE */
+    (iw_handler) NULL,                      /* SIOCSIWSENS */
+    (iw_handler) NULL,                      /* SIOCGIWSENS */
+    (iw_handler) NULL,                      /* SIOCSIWRANGE */
+    (iw_handler) unifi_giwrange,            /* SIOCGIWRANGE */
+    (iw_handler) NULL,                      /* SIOCSIWPRIV */
+    (iw_handler) NULL,                      /* SIOCGIWPRIV */
+    (iw_handler) NULL,                      /* SIOCSIWSTATS */
+    (iw_handler) NULL,                      /* SIOCGIWSTATS */
+    (iw_handler) NULL,                      /* SIOCSIWSPY */
+    (iw_handler) NULL,                      /* SIOCGIWSPY */
+    (iw_handler) NULL,                      /* SIOCSIWTHRSPY */
+    (iw_handler) NULL,                      /* SIOCGIWTHRSPY */
+    (iw_handler) unifi_siwap,               /* SIOCSIWAP */
+    (iw_handler) unifi_giwap,               /* SIOCGIWAP */
+#if WIRELESS_EXT > 17
+    /* WPA : IEEE 802.11 MLME requests */
+    unifi_siwmlme,              /* SIOCSIWMLME, request MLME operation */
+#else
+    (iw_handler) NULL,                      /* -- hole -- */
+#endif
+    (iw_handler) NULL,                      /* SIOCGIWAPLIST */
+    (iw_handler) unifi_siwscan,             /* SIOCSIWSCAN */
+    (iw_handler) unifi_giwscan,             /* SIOCGIWSCAN */
+    (iw_handler) unifi_siwessid,            /* SIOCSIWESSID */
+    (iw_handler) unifi_giwessid,            /* SIOCGIWESSID */
+    (iw_handler) NULL,                      /* SIOCSIWNICKN */
+    (iw_handler) NULL,                      /* SIOCGIWNICKN */
+    (iw_handler) NULL,                      /* -- hole -- */
+    (iw_handler) NULL,                      /* -- hole -- */
+    unifi_siwrate,                          /* SIOCSIWRATE */
+    unifi_giwrate,                          /* SIOCGIWRATE */
+    unifi_siwrts,                           /* SIOCSIWRTS */
+    unifi_giwrts,                           /* SIOCGIWRTS */
+    unifi_siwfrag,                          /* SIOCSIWFRAG */
+    unifi_giwfrag,                          /* SIOCGIWFRAG */
+    (iw_handler) NULL,                      /* SIOCSIWTXPOW */
+    (iw_handler) NULL,                      /* SIOCGIWTXPOW */
+    (iw_handler) NULL,                      /* SIOCSIWRETRY */
+    (iw_handler) NULL,                      /* SIOCGIWRETRY */
+    unifi_siwencode,                        /* SIOCSIWENCODE */
+    unifi_giwencode,                        /* SIOCGIWENCODE */
+    unifi_siwpower,                         /* SIOCSIWPOWER */
+    unifi_giwpower,                         /* SIOCGIWPOWER */
+#if WIRELESS_EXT > 17
+    (iw_handler) NULL,                      /* -- hole -- */
+    (iw_handler) NULL,                      /* -- hole -- */
+
+    /* WPA : Generic IEEE 802.11 informatiom element (e.g., for WPA/RSN/WMM). */
+    unifi_siwgenie,             /* SIOCSIWGENIE */      /* set generic IE */
+    unifi_giwgenie,             /* SIOCGIWGENIE */      /* get generic IE */
+
+    /* WPA : Authentication mode parameters */
+    unifi_siwauth,              /* SIOCSIWAUTH */       /* set authentication mode params */
+    unifi_giwauth,              /* SIOCGIWAUTH */       /* get authentication mode params */
+
+    /* WPA : Extended version of encoding configuration */
+    unifi_siwencodeext,         /* SIOCSIWENCODEEXT */  /* set encoding token & mode */
+    unifi_giwencodeext,         /* SIOCGIWENCODEEXT */  /* get encoding token & mode */
+
+    /* WPA2 : PMKSA cache management */
+    unifi_siwpmksa,             /* SIOCSIWPMKSA */      /* PMKSA cache operation */
+    (iw_handler) NULL,          /* -- hole -- */
+#endif /* WIRELESS_EXT > 17 */
+};
+
+
+static const iw_handler unifi_private_handler[] =
+{
+    iwprivs80211ps,                 /* SIOCIWFIRSTPRIV */
+    iwprivg80211ps,                 /* SIOCIWFIRSTPRIV + 1 */
+    iwprivsdefs,                    /* SIOCIWFIRSTPRIV + 2 */
+    (iw_handler) NULL,
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    iwprivsconfwapi,                /* SIOCIWFIRSTPRIV + 4 */
+    (iw_handler) NULL,              /* SIOCIWFIRSTPRIV + 5 */
+    iwprivswpikey,                  /* SIOCIWFIRSTPRIV + 6 */
+#else
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+#endif
+    (iw_handler) NULL,
+    iwprivssmedebug,                /* SIOCIWFIRSTPRIV + 8 */
+#ifdef CSR_SUPPORT_WEXT_AP
+    (iw_handler) NULL,
+    iwprivsapconfig,
+    (iw_handler) NULL,
+    iwprivsapstart,
+    (iw_handler) NULL,
+    iwprivsapstop,
+    (iw_handler) NULL,
+#ifdef ANDROID_BUILD
+    iwprivsapfwreload,
+    (iw_handler) NULL,
+    iwprivsstackstart,
+    (iw_handler) NULL,
+    iwprivsstackstop,
+#else
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+#endif /* ANDROID_BUILD */
+#else
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+    (iw_handler) NULL,
+#endif /* CSR_SUPPORT_WEXT_AP */
+};
+
+struct iw_handler_def unifi_iw_handler_def =
+{
+    .num_standard       = sizeof(unifi_handler) / sizeof(iw_handler),
+    .num_private        = sizeof(unifi_private_handler) / sizeof(iw_handler),
+    .num_private_args   = sizeof(unifi_private_args) / sizeof(struct iw_priv_args),
+    .standard           = (iw_handler *) unifi_handler,
+    .private            = (iw_handler *) unifi_private_handler,
+    .private_args       = (struct iw_priv_args *) unifi_private_args,
+#if IW_HANDLER_VERSION >= 6
+    .get_wireless_stats = unifi_get_wireless_stats,
+#endif
+};
+
+
diff --git a/drivers/staging/csr/ul_int.c b/drivers/staging/csr/ul_int.c
new file mode 100644
index 00000000000..46d3507fd8f
--- /dev/null
+++ b/drivers/staging/csr/ul_int.c
@@ -0,0 +1,532 @@
+/*
+ * ***************************************************************************
+ *  FILE:     ul_int.c
+ *
+ *  PURPOSE:
+ *      Manage list of client applications using UniFi.
+ *
+ * Copyright (C) 2006-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "unifi_priv.h"
+#include "unifiio.h"
+#include "unifi_os.h"
+
+static void free_bulkdata_buffers(unifi_priv_t *priv, bulk_data_param_t *bulkdata);
+static void reset_driver_status(unifi_priv_t *priv);
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ul_init_clients
+ *
+ *      Initialise the clients array to empty.
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      This function needs to be called before priv is stored in
+ *      Unifi_instances[].
+ * ---------------------------------------------------------------------------
+ */
+void
+ul_init_clients(unifi_priv_t *priv)
+{
+    int id;
+    ul_client_t *ul_clients;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+    sema_init(&priv->udi_logging_mutex, 1);
+#else
+    init_MUTEX(&priv->udi_logging_mutex);
+#endif
+    priv->logging_client = NULL;
+
+    ul_clients = priv->ul_clients;
+
+    for (id = 0; id < MAX_UDI_CLIENTS; id++) {
+        memset(&ul_clients[id], 0, sizeof(ul_client_t));
+
+        ul_clients[id].client_id = id;
+        ul_clients[id].sender_id = UDI_SENDER_ID_BASE + (id << UDI_SENDER_ID_SHIFT);
+        ul_clients[id].instance = -1;
+        ul_clients[id].event_hook = NULL;
+
+        INIT_LIST_HEAD(&ul_clients[id].udi_log);
+        init_waitqueue_head(&ul_clients[id].udi_wq);
+        sema_init(&ul_clients[id].udi_sem, 1);
+
+        ul_clients[id].wake_up_wq_id = 0;
+        ul_clients[id].seq_no = 0;
+        ul_clients[id].wake_seq_no = 0;
+        ul_clients[id].snap_filter.count = 0;
+    }
+} /* ul_init_clients() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ul_register_client
+ *
+ *      This function registers a new ul client.
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      configuration   Special configuration for the client.
+ *      udi_event_clbk  Callback for receiving event from unifi.
+ *
+ *  Returns:
+ *      0 if a new clients is registered, -1 otherwise.
+ * ---------------------------------------------------------------------------
+ */
+ul_client_t *
+ul_register_client(unifi_priv_t *priv, unsigned int configuration,
+                   udi_event_t udi_event_clbk)
+{
+    unsigned char id, ref;
+    ul_client_t *ul_clients;
+
+    ul_clients = priv->ul_clients;
+
+    /* check for an unused entry */
+    for (id = 0; id < MAX_UDI_CLIENTS; id++) {
+        if (ul_clients[id].udi_enabled == 0) {
+            ul_clients[id].instance = priv->instance;
+            ul_clients[id].udi_enabled = 1;
+            ul_clients[id].configuration = configuration;
+
+            /* Allocate memory for the reply signal.. */
+            ul_clients[id].reply_signal = kmalloc(sizeof(CSR_SIGNAL), GFP_KERNEL);
+            if (ul_clients[id].reply_signal == NULL) {
+                unifi_error(priv, "Failed to allocate reply signal for client.\n");
+                return NULL;
+            }
+            /* .. and the bulk data of the reply signal. */
+            for (ref = 0; ref < UNIFI_MAX_DATA_REFERENCES; ref ++) {
+                ul_clients[id].reply_bulkdata[ref] = kmalloc(sizeof(bulk_data_t), GFP_KERNEL);
+                /* If allocation fails, free allocated memory. */
+                if (ul_clients[id].reply_bulkdata[ref] == NULL) {
+                    for (; ref > 0; ref --) {
+                        kfree(ul_clients[id].reply_bulkdata[ref - 1]);
+                    }
+                    kfree(ul_clients[id].reply_signal);
+                    unifi_error(priv, "Failed to allocate bulk data buffers for client.\n");
+                    return NULL;
+                }
+            }
+
+            /* Set the event callback. */
+            ul_clients[id].event_hook = udi_event_clbk;
+
+            unifi_trace(priv, UDBG2, "UDI %d (0x%x) registered. configuration = 0x%x\n",
+                        id, &ul_clients[id], configuration);
+            return &ul_clients[id];
+        }
+    }
+    return NULL;
+} /* ul_register_client() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ul_deregister_client
+ *
+ *      This function deregisters a blocking UDI client.
+ *
+ *  Arguments:
+ *      client      Pointer to the client we deregister.
+ *
+ *  Returns:
+ *      0 if a new clients is deregistered.
+ * ---------------------------------------------------------------------------
+ */
+int
+ul_deregister_client(ul_client_t *ul_client)
+{
+    struct list_head *pos, *n;
+    udi_log_t *logptr;
+    unifi_priv_t *priv = uf_find_instance(ul_client->instance);
+    int ref;
+
+    ul_client->instance = -1;
+    ul_client->event_hook = NULL;
+    ul_client->udi_enabled = 0;
+    unifi_trace(priv, UDBG5, "UDI (0x%x) deregistered.\n", ul_client);
+
+    /* Free memory allocated for the reply signal and its bulk data. */
+    kfree(ul_client->reply_signal);
+    for (ref = 0; ref < UNIFI_MAX_DATA_REFERENCES; ref ++) {
+        kfree(ul_client->reply_bulkdata[ref]);
+    }
+
+    if (ul_client->snap_filter.count) {
+        ul_client->snap_filter.count = 0;
+        kfree(ul_client->snap_filter.protocols);
+    }
+
+    /* Free anything pending on the udi_log list */
+    down(&ul_client->udi_sem);
+    list_for_each_safe(pos, n, &ul_client->udi_log)
+    {
+        logptr = list_entry(pos, udi_log_t, q);
+        list_del(pos);
+        kfree(logptr);
+    }
+    up(&ul_client->udi_sem);
+
+    return 0;
+} /* ul_deregister_client() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  logging_handler
+ *
+ *      This function is registered with the driver core.
+ *      It is called every time a UniFi HIP Signal is sent. It iterates over
+ *      the list of processes interested in receiving log events and
+ *      delivers the events to them.
+ *
+ *  Arguments:
+ *      ospriv      Pointer to driver's private data.
+ *      sigdata     Pointer to the packed signal buffer.
+ *      signal_len  Length of the packed signal.
+ *      bulkdata    Pointer to the signal's bulk data.
+ *      dir         Direction of the signal
+ *                  0 = from-host
+ *                  1 = to-host
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+logging_handler(void *ospriv,
+                u8 *sigdata, u32 signal_len,
+                const bulk_data_param_t *bulkdata,
+                enum udi_log_direction direction)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    ul_client_t *client;
+    int dir;
+
+    dir = (direction == UDI_LOG_FROM_HOST) ? UDI_FROM_HOST : UDI_TO_HOST;
+
+    down(&priv->udi_logging_mutex);
+    client = priv->logging_client;
+    if (client != NULL) {
+        client->event_hook(client, sigdata, signal_len,
+                           bulkdata, dir);
+    }
+    up(&priv->udi_logging_mutex);
+
+} /* logging_handler() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ul_log_config_ind
+ *
+ *      This function uses the client's register callback
+ *      to indicate configuration information e.g core errors.
+ *
+ *  Arguments:
+ *      priv        Pointer to driver's private data.
+ *      conf_param  Pointer to the configuration data.
+ *      len         Length of the configuration data.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+ul_log_config_ind(unifi_priv_t *priv, u8 *conf_param, int len)
+{
+#ifdef CSR_SUPPORT_SME
+    if (priv->smepriv == NULL)
+    {
+        return;
+    }
+    if ((CONFIG_IND_ERROR == (*conf_param)) && (priv->wifi_on_state == wifi_on_in_progress)) {
+        unifi_notice(priv, "ul_log_config_ind: wifi on in progress, suppress error\n");
+    } else {
+        /* wifi_off_ind (error or exit) */
+        CsrWifiRouterCtrlWifiOffIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0, (CsrWifiRouterCtrlControlIndication)(*conf_param));
+    }
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+    unifi_debug_buf_dump();
+#endif
+#else
+    bulk_data_param_t bulkdata;
+
+    /*
+     * If someone killed unifi_managed before the driver was unloaded
+     * the g_drvpriv pointer is going to be NULL. In this case it is
+     * safe to assume that there is no client to get the indication.
+     */
+    if (!priv) {
+        unifi_notice(NULL, "uf_sme_event_ind: NULL priv\n");
+        return;
+    }
+
+    /* Create a null bulkdata structure. */
+    bulkdata.d[0].data_length = 0;
+    bulkdata.d[1].data_length = 0;
+
+    sme_native_log_event(priv->sme_cli, conf_param, sizeof(u8),
+                         &bulkdata, UDI_CONFIG_IND);
+
+#endif /* CSR_SUPPORT_SME */
+
+} /* ul_log_config_ind */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  free_bulkdata_buffers
+ *
+ *      Free the bulkdata buffers e.g. after a failed unifi_send_signal().
+ *
+ *  Arguments:
+ *      priv        Pointer to device private struct
+ *      bulkdata    Pointer to bulkdata parameter table
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+free_bulkdata_buffers(unifi_priv_t *priv, bulk_data_param_t *bulkdata)
+{
+    int i;
+
+    if (bulkdata) {
+        for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; ++i) {
+            if (bulkdata->d[i].data_length != 0) {
+                unifi_net_data_free(priv, (bulk_data_desc_t *)(&bulkdata->d[i]));
+                /* data_length is now 0 */
+            }
+        }
+    }
+
+} /* free_bulkdata_buffers */
+
+static int
+_align_bulk_data_buffers(unifi_priv_t *priv, u8 *signal,
+                         bulk_data_param_t *bulkdata)
+{
+    unsigned int i;
+
+    if ((bulkdata == NULL) || (CSR_WIFI_ALIGN_BYTES == 0)) {
+        return 0;
+    }
+
+    for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++)
+    {
+        struct sk_buff *skb;
+        /*
+        * The following complex casting is in place in order to eliminate 64-bit compilation warning
+        * "cast to/from pointer from/to integer of different size"
+        */
+        u32 align_offset = (u32)(long)(bulkdata->d[i].os_data_ptr) & (CSR_WIFI_ALIGN_BYTES-1);
+        if (align_offset)
+        {
+            skb = (struct sk_buff*)bulkdata->d[i].os_net_buf_ptr;
+            if (skb == NULL) {
+                unifi_warning(priv,
+                              "_align_bulk_data_buffers: Align offset found (%d) but skb is NULL!\n",
+                              align_offset);
+                return -EINVAL;
+            }
+            if (bulkdata->d[i].data_length == 0) {
+                unifi_warning(priv,
+                              "_align_bulk_data_buffers: Align offset found (%d) but length is zero\n",
+                              align_offset);
+                return CSR_RESULT_SUCCESS;
+            }
+            unifi_trace(priv, UDBG5,
+                        "Align f-h buffer (0x%p) by %d bytes (skb->data: 0x%p)\n",
+                        bulkdata->d[i].os_data_ptr, align_offset, skb->data);
+
+
+            /* Check if there is enough headroom... */
+            if (unlikely(skb_headroom(skb) < align_offset))
+            {
+                struct sk_buff *tmp = skb;
+
+                unifi_trace(priv, UDBG5, "Headroom not enough - realloc it\n");
+                skb = skb_realloc_headroom(skb, align_offset);
+                if (skb == NULL) {
+                    unifi_error(priv,
+                                "_align_bulk_data_buffers: skb_realloc_headroom failed - signal is dropped\n");
+                    return -EFAULT;
+                }
+                /* Free the old bulk data only if allocation succeeds */
+                kfree_skb(tmp);
+                /* Bulkdata needs to point to the new skb */
+                bulkdata->d[i].os_net_buf_ptr = (const unsigned char*)skb;
+                bulkdata->d[i].os_data_ptr = (const void*)skb->data;
+            }
+            /* ... before pushing the data to the right alignment offset */
+            skb_push(skb, align_offset);
+
+        }
+        /* The direction bit is zero for the from-host */
+        signal[SIZEOF_SIGNAL_HEADER + (i * SIZEOF_DATAREF) + 1] = align_offset;
+
+    }
+    return 0;
+} /* _align_bulk_data_buffers() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ul_send_signal_unpacked
+ *
+ *      This function sends a host formatted signal to unifi.
+ *
+ *  Arguments:
+ *      priv        Pointer to driver's private data.
+ *      sigptr      Pointer to the signal.
+ *      bulkdata    Pointer to the signal's bulk data.
+ *
+ *  Returns:
+ *      O on success, error code otherwise.
+ *
+ *  Notes:
+ *  The signals have to be sent in the format described in the host interface
+ *  specification, i.e wire formatted. Certain clients use the host formatted
+ *  structures. The write_pack() transforms the host formatted signal
+ *  into the wired formatted signal. The code is in the core, since the signals
+ *  are defined therefore binded to the host interface specification.
+ * ---------------------------------------------------------------------------
+ */
+int
+ul_send_signal_unpacked(unifi_priv_t *priv, CSR_SIGNAL *sigptr,
+                        bulk_data_param_t *bulkdata)
+{
+    u8 sigbuf[UNIFI_PACKED_SIGBUF_SIZE];
+    u16 packed_siglen;
+    CsrResult csrResult;
+    unsigned long lock_flags;
+    int r;
+
+
+    csrResult = write_pack(sigptr, sigbuf, &packed_siglen);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "Malformed HIP signal in ul_send_signal_unpacked()\n");
+        return CsrHipResultToStatus(csrResult);
+    }
+    r = _align_bulk_data_buffers(priv, sigbuf, (bulk_data_param_t*)bulkdata);
+    if (r) {
+        return r;
+    }
+
+    spin_lock_irqsave(&priv->send_signal_lock, lock_flags);
+    csrResult = unifi_send_signal(priv->card, sigbuf, packed_siglen, bulkdata);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+  /*      free_bulkdata_buffers(priv, (bulk_data_param_t *)bulkdata); */
+        spin_unlock_irqrestore(&priv->send_signal_lock, lock_flags);
+        return CsrHipResultToStatus(csrResult);
+    }
+    spin_unlock_irqrestore(&priv->send_signal_lock, lock_flags);
+
+    return 0;
+} /* ul_send_signal_unpacked() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  reset_driver_status
+ *
+ *      This function is called from ul_send_signal_raw() when it detects
+ *      that the SME has sent a MLME-RESET request.
+ *
+ *  Arguments:
+ *      priv        Pointer to device private struct
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+static void
+reset_driver_status(unifi_priv_t *priv)
+{
+    priv->sta_wmm_capabilities = 0;
+#ifdef CSR_NATIVE_LINUX
+#ifdef CSR_SUPPORT_WEXT
+    priv->wext_conf.flag_associated = 0;
+    priv->wext_conf.block_controlled_port = CSR_WIFI_ROUTER_PORT_ACTION_8021X_PORT_OPEN;
+    priv->wext_conf.bss_wmm_capabilities = 0;
+    priv->wext_conf.disable_join_on_ssid_set = 0;
+#endif
+#endif
+} /* reset_driver_status() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  ul_send_signal_raw
+ *
+ *      This function sends a wire formatted data signal to unifi.
+ *
+ *  Arguments:
+ *      priv        Pointer to driver's private data.
+ *      sigptr      Pointer to the signal.
+ *      siglen      Length of the signal.
+ *      bulkdata    Pointer to the signal's bulk data.
+ *
+ *  Returns:
+ *      O on success, error code otherwise.
+ * ---------------------------------------------------------------------------
+ */
+int
+ul_send_signal_raw(unifi_priv_t *priv, unsigned char *sigptr, int siglen,
+                   bulk_data_param_t *bulkdata)
+{
+    CsrResult csrResult;
+    unsigned long lock_flags;
+    int r;
+
+    /*
+     * Make sure that the signal is updated with the bulk data
+     * alignment for DMA.
+     */
+    r = _align_bulk_data_buffers(priv, (u8*)sigptr, bulkdata);
+    if (r) {
+        return r;
+    }
+
+    spin_lock_irqsave(&priv->send_signal_lock, lock_flags);
+    csrResult = unifi_send_signal(priv->card, sigptr, siglen, bulkdata);
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        free_bulkdata_buffers(priv, bulkdata);
+        spin_unlock_irqrestore(&priv->send_signal_lock, lock_flags);
+        return CsrHipResultToStatus(csrResult);
+    }
+    spin_unlock_irqrestore(&priv->send_signal_lock, lock_flags);
+
+    /*
+     * Since this is use by unicli, if we get an MLME reset request
+     * we need to initialize a few status parameters
+     * that the driver uses to make decisions.
+     */
+    if (GET_SIGNAL_ID(sigptr) == CSR_MLME_RESET_REQUEST_ID) {
+        reset_driver_status(priv);
+    }
+
+    return 0;
+} /* ul_send_signal_raw() */
+
+
diff --git a/drivers/staging/csr/unifi_clients.h b/drivers/staging/csr/unifi_clients.h
new file mode 100644
index 00000000000..df853e160ea
--- /dev/null
+++ b/drivers/staging/csr/unifi_clients.h
@@ -0,0 +1,129 @@
+/*
+ *****************************************************************************
+ *
+ * FILE : unifi_clients.h
+ *
+ * PURPOSE : Private header file for unifi clients.
+ *
+ *           UDI = UniFi Debug Interface
+ *
+ * Copyright (C) 2005-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ *****************************************************************************
+ */
+#ifndef __LINUX_UNIFI_CLIENTS_H__
+#define __LINUX_UNIFI_CLIENTS_H__ 1
+
+#include <linux/kernel.h>
+
+#define MAX_UDI_CLIENTS 8
+
+/* The start of the range of process ids allocated for ul clients */
+#define UDI_SENDER_ID_BASE      0xC000
+#define UDI_SENDER_ID_SHIFT     8
+
+
+/* Structure to hold a UDI logged signal */
+typedef struct {
+
+    /* List link structure */
+    struct list_head q;
+
+    /* The message that will be passed to the user app */
+    udi_msg_t msg;
+
+    /* Signal body and data follow */
+
+} udi_log_t;
+
+
+
+typedef struct ul_client ul_client_t;
+
+typedef void (*udi_event_t)(ul_client_t *client,
+                            const u8 *sigdata, int signal_len,
+                            const bulk_data_param_t *bulkdata,
+                            int dir);
+
+void logging_handler(void *ospriv,
+                     u8 *sigdata, u32 signal_len,
+                     const bulk_data_param_t *bulkdata,
+                     enum udi_log_direction direction);
+
+
+/*
+ * Structure describing a bulk data slot.
+ * The length field is used to indicate empty/occupied state.
+ */
+typedef struct _bulk_data
+{
+    unsigned char ptr[2000];
+    unsigned int length;
+} bulk_data_t;
+
+
+struct ul_client {
+    /* Index of this client in the ul_clients array. */
+    int client_id;
+
+    /* Index of UniFi device to which this client is attached. */
+    int instance;
+
+    /* Flag to say whether this client has been enabled. */
+    int udi_enabled;
+
+    /* Value to use in signal->SenderProcessId */
+    int sender_id;
+
+    /* Configuration flags, e.g blocking, logging, etc. */
+    unsigned int configuration;
+
+    udi_event_t event_hook;
+
+    /* A list to hold signals received from UniFi for reading by read() */
+    struct list_head udi_log;
+
+    /* Semaphore to protect the udi_log list */
+    struct semaphore udi_sem;
+
+    /*
+     * Linux waitqueue to support blocking read and poll.
+     * Logging clients should wait on udi_log. while
+     * blocking clients should wait on wake_up_wq.
+     */
+    wait_queue_head_t udi_wq;
+    CSR_SIGNAL* reply_signal;
+    bulk_data_t* reply_bulkdata[UNIFI_MAX_DATA_REFERENCES];
+
+    u16 signal_filter[SIG_FILTER_SIZE];
+
+
+    /* ------------------------------------------------------------------- */
+    /* Code below here is used by the sme_native configuration only */
+
+    /* Flag to wake up blocking clients waiting on udi_wq. */
+    int wake_up_wq_id;
+
+    /*
+     * A 0x00 - 0x0F mask to apply in signal->SenderProcessId.
+     * Every time we do a blocking mlme request we increase this value.
+     * The mlme_wait_for_reply() will wait for this sequence number.
+     * Only the MLME blocking functions update this field.
+     */
+    unsigned char seq_no;
+
+    /*
+     * A 0x00 - 0x0F counter, containing the sequence number of
+     * the signal that this client has last received.
+     * Only the MLME blocking functions update this field.
+     */
+    unsigned char wake_seq_no;
+
+    unifiio_snap_filter_t snap_filter;
+}; /* struct ul_client */
+
+
+#endif /* __LINUX_UNIFI_CLIENTS_H__ */
diff --git a/drivers/staging/csr/unifi_config.h b/drivers/staging/csr/unifi_config.h
new file mode 100644
index 00000000000..fe119707844
--- /dev/null
+++ b/drivers/staging/csr/unifi_config.h
@@ -0,0 +1,34 @@
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * FILE: unifi_config.h
+ *
+ * PURPOSE:
+ *      This header file provides parameters that configure the operation
+ *      of the driver.
+ *
+ * Copyright (C) 2006-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __UNIFI_CONFIG_H__
+#define __UNIFI_CONFIG_H__ 1
+
+/*
+ * Override for the SDIO function block size on this host. When byte mode CMD53s
+ * are not used/supported by the SD host controller, transfers are padded up to
+ * the next block boundary. The 512-byte default on UF6xxx wastes too much space
+ * on the chip, so the block size is reduced to support this configuration.
+ */
+#define CSR_WIFI_HIP_SDIO_BLOCK_SIZE  64
+
+/* Define the number of mini-coredump buffers to allocate at startup. These are
+ * used to record chip status for the last n unexpected resets.
+ */
+#define CSR_WIFI_HIP_NUM_COREDUMP_BUFFERS  5
+
+
+#endif /* __UNIFI_CONFIG_H__ */
diff --git a/drivers/staging/csr/unifi_dbg.c b/drivers/staging/csr/unifi_dbg.c
new file mode 100644
index 00000000000..38d57085a26
--- /dev/null
+++ b/drivers/staging/csr/unifi_dbg.c
@@ -0,0 +1,110 @@
+/*
+ * ***************************************************************************
+ *  FILE:     unifi_dbg.c
+ *
+ *  PURPOSE:
+ *      Handle debug signals received from UniFi.
+ *
+ * Copyright (C) 2007-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+#include "unifi_priv.h"
+
+/*
+ * ---------------------------------------------------------------------------
+ *  debug_string_indication
+ *  debug_word16_indication
+ *
+ *      Handlers for debug indications.
+ *
+ *  Arguments:
+ *      priv            Pointer to private context structure.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+debug_string_indication(unifi_priv_t *priv, const unsigned char *extra, unsigned int extralen)
+{
+    const unsigned int maxlen = sizeof(priv->last_debug_string) - 1;
+
+    if (extralen > maxlen) {
+        extralen = maxlen;
+    }
+
+    strncpy(priv->last_debug_string, extra, extralen);
+
+    /* Make sure the string is terminated */
+    priv->last_debug_string[extralen] = '\0';
+
+    unifi_info(priv, "unifi debug: %s\n", priv->last_debug_string);
+
+} /* debug_string_indication() */
+
+
+
+void
+debug_word16_indication(unifi_priv_t *priv, const CSR_SIGNAL *sigptr)
+{
+    int i;
+
+    if (priv == NULL) {
+        unifi_info(priv, "Priv is NULL\n");
+        return;
+    }
+
+    for (i = 0; i < 16; i++) {
+        priv->last_debug_word16[i] =
+                sigptr->u.DebugWord16Indication.DebugWords[i];
+    }
+
+    if (priv->last_debug_word16[0] == 0xFA11) {
+        unsigned long ts;
+        ts = (priv->last_debug_word16[6] << 16) | priv->last_debug_word16[5];
+        unifi_info(priv, " %10lu: %s fault %04x, arg %04x (x%d)\n",
+                   ts,
+                   priv->last_debug_word16[3] == 0x8000 ? "MAC" :
+                   priv->last_debug_word16[3] == 0x4000 ? "PHY" :
+                   "???",
+                   priv->last_debug_word16[1],
+                   priv->last_debug_word16[2],
+                   priv->last_debug_word16[4]);
+    }
+    else if (priv->last_debug_word16[0] != 0xDBAC)
+        /* suppress SDL Trace output (note: still available to unicli). */
+    {
+        unifi_info(priv, "unifi debug: %04X %04X %04X %04X %04X %04X %04X %04X\n",
+                   priv->last_debug_word16[0], priv->last_debug_word16[1],
+                   priv->last_debug_word16[2], priv->last_debug_word16[3],
+                   priv->last_debug_word16[4], priv->last_debug_word16[5],
+                   priv->last_debug_word16[6], priv->last_debug_word16[7]);
+        unifi_info(priv, "             %04X %04X %04X %04X %04X %04X %04X %04X\n",
+                   priv->last_debug_word16[8], priv->last_debug_word16[9],
+                   priv->last_debug_word16[10], priv->last_debug_word16[11],
+                   priv->last_debug_word16[12], priv->last_debug_word16[13],
+                   priv->last_debug_word16[14], priv->last_debug_word16[15]);
+    }
+
+} /* debug_word16_indication() */
+
+
+void
+debug_generic_indication(unifi_priv_t *priv, const CSR_SIGNAL *sigptr)
+{
+    unifi_info(priv, "debug: %04X %04X %04X %04X %04X %04X %04X %04X\n",
+               sigptr->u.DebugGenericIndication.DebugWords[0],
+               sigptr->u.DebugGenericIndication.DebugWords[1],
+               sigptr->u.DebugGenericIndication.DebugWords[2],
+               sigptr->u.DebugGenericIndication.DebugWords[3],
+               sigptr->u.DebugGenericIndication.DebugWords[4],
+               sigptr->u.DebugGenericIndication.DebugWords[5],
+               sigptr->u.DebugGenericIndication.DebugWords[6],
+               sigptr->u.DebugGenericIndication.DebugWords[7]);
+
+} /* debug_generic_indication() */
+
diff --git a/drivers/staging/csr/unifi_event.c b/drivers/staging/csr/unifi_event.c
new file mode 100644
index 00000000000..c27b23daf39
--- /dev/null
+++ b/drivers/staging/csr/unifi_event.c
@@ -0,0 +1,700 @@
+/*
+ * ***************************************************************************
+ *  FILE:     unifi_event.c
+ *
+ *  PURPOSE:
+ *      Process the signals received by UniFi.
+ *      It is part of the porting exercise.
+ *
+ * Copyright (C) 2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+
+
+/*
+ * Porting notes:
+ * The implementation of unifi_receive_event() in Linux is fairly complicated.
+ * The linux driver support multiple userspace applications and several
+ * build configurations, so the received signals are processed by different
+ * processes and multiple times.
+ * In a simple implementation, this function needs to deliver:
+ * - The MLME-UNITDATA.ind signals to the Rx data plane and to the Traffic
+ *   Analysis using unifi_ta_sample().
+ * - The MLME-UNITDATA-STATUS.ind signals to the Tx data plane.
+ * - All the other signals to the SME using unifi_sys_hip_ind().
+ */
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "unifi_priv.h"
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  send_to_client
+ *
+ *      Helper for unifi_receive_event.
+ *
+ *      This function forwards a signal to one client.
+ *
+ *  Arguments:
+ *      priv        Pointer to driver's private data.
+ *      client      Pointer to the client structure.
+ *      receiver_id The reciever id of the signal.
+ *      sigdata     Pointer to the packed signal buffer.
+ *      siglen      Length of the packed signal.
+ *      bulkdata    Pointer to the signal's bulk data.
+ *
+ *  Returns:
+ *      None.
+ *
+ * ---------------------------------------------------------------------------
+ */
+static void send_to_client(unifi_priv_t *priv, ul_client_t *client,
+        int receiver_id,
+        unsigned char *sigdata, int siglen,
+        const bulk_data_param_t *bulkdata)
+{
+    if (client && client->event_hook) {
+        /*unifi_trace(priv, UDBG3,
+                "Receive: client %d, (s:0x%X, r:0x%X) - Signal 0x%.4X \n",
+                client->client_id, client->sender_id, receiver_id,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata));*/
+
+        client->event_hook(client, sigdata, siglen, bulkdata, UDI_TO_HOST);
+    }
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  process_pkt_data_ind
+ *
+ *      Dispatcher for received signals.
+ *
+ *      This function receives the 'to host' signals and forwards
+ *      them to the unifi linux clients.
+ *
+ *  Arguments:
+ *      priv         Context
+ *      sigdata      Pointer to the packed signal buffer(Its in form of MA-PACKET.ind).
+ *      bulkdata     Pointer to signal's bulkdata
+ *      freeBulkData Pointer to a flag which gets set if the bulkdata needs to
+ *                   be freed after calling the logging handlers. If it is not
+ *                   set the bulkdata must be freed by the MLME handler or
+ *                   passed to the network stack.
+ *  Returns:
+ *      TRUE if the packet should be routed to the SME etc.
+ *      FALSE if the packet is for the driver or network stack
+ * ---------------------------------------------------------------------------
+ */
+static u8 check_routing_pkt_data_ind(unifi_priv_t *priv,
+        u8 *sigdata,
+        const bulk_data_param_t* bulkdata,
+        u8 *freeBulkData,
+        netInterface_priv_t *interfacePriv)
+{
+    u16  frmCtrl, receptionStatus, frmCtrlSubType;
+    u8 *macHdrLocation;
+    u8 interfaceTag;
+    u8 isDataFrame;
+    u8 isProtocolVerInvalid = FALSE;
+    u8 isDataFrameSubTypeNoData = FALSE;
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    static const u8 wapiProtocolIdSNAPHeader[] = {0x88,0xb4};
+    static const u8 wapiProtocolIdSNAPHeaderOffset = 6;
+    u8 *destAddr;
+    u8 *srcAddr;
+    u8 isWapiUnicastPkt = FALSE;
+
+#ifdef CSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND
+    u16 qosControl;
+#endif
+
+    u8 llcSnapHeaderOffset = 0;
+
+    destAddr = (u8 *) bulkdata->d[0].os_data_ptr + MAC_HEADER_ADDR1_OFFSET;
+    srcAddr  = (u8 *) bulkdata->d[0].os_data_ptr + MAC_HEADER_ADDR2_OFFSET;
+
+    /*Individual/Group bit - Bit 0 of first byte*/
+    isWapiUnicastPkt = (!(destAddr[0] & 0x01)) ? TRUE : FALSE;
+#endif
+
+#define CSR_WIFI_MA_PKT_IND_RECEPTION_STATUS_OFFSET    sizeof(CSR_SIGNAL_PRIMITIVE_HEADER) + 22
+
+    *freeBulkData = FALSE;
+
+    /* Fetch the MAC header location from  MA_PKT_IND packet */
+    macHdrLocation = (u8 *) bulkdata->d[0].os_data_ptr;
+    /* Fetch the Frame Control value from  MAC header */
+    frmCtrl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(macHdrLocation);
+
+    /* Pull out interface tag from virtual interface identifier */
+    interfaceTag = (CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata + 14)) & 0xff;
+
+    /* check for MIC failure before processing the signal */
+    receptionStatus = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata + CSR_WIFI_MA_PKT_IND_RECEPTION_STATUS_OFFSET);
+
+    /* To discard any spurious MIC failures that could be reported by the firmware */
+    isDataFrame = ((frmCtrl & IEEE80211_FC_TYPE_MASK) == (IEEE802_11_FC_TYPE_DATA & IEEE80211_FC_TYPE_MASK)) ? TRUE : FALSE;
+    /* 0x00 is the only valid protocol version*/
+    isProtocolVerInvalid = (frmCtrl & IEEE80211_FC_PROTO_VERSION_MASK) ? TRUE : FALSE;
+    frmCtrlSubType = (frmCtrl & IEEE80211_FC_SUBTYPE_MASK) >> FRAME_CONTROL_SUBTYPE_FIELD_OFFSET;
+    /*Exclude the no data & reserved sub-types from MIC failure processing*/
+    isDataFrameSubTypeNoData = (((frmCtrlSubType>0x03)&&(frmCtrlSubType<0x08)) || (frmCtrlSubType>0x0B)) ? TRUE : FALSE;
+    if ((receptionStatus == CSR_MICHAEL_MIC_ERROR) &&
+        ((!isDataFrame) || isProtocolVerInvalid || (isDataFrame && isDataFrameSubTypeNoData))) {
+        /* Currently MIC errors are discarded for frames other than data frames. This might need changing when we start
+         * supporting 802.11w (Protected Management frames)
+         */
+        *freeBulkData = TRUE;
+        unifi_trace(priv, UDBG4, "Discarding this frame and ignoring the MIC failure as this is a garbage/non-data/no data frame\n");
+        return FALSE;
+     }
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+
+    if (receptionStatus == CSR_MICHAEL_MIC_ERROR) {
+
+        if (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_STA) {
+
+#ifdef CSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND
+            if ((isDataFrame) &&
+                ((IEEE802_11_FC_TYPE_QOS_DATA & IEEE80211_FC_SUBTYPE_MASK) == (frmCtrl & IEEE80211_FC_SUBTYPE_MASK)) &&
+                (priv->isWapiConnection))
+            {
+            	qosControl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(macHdrLocation + (((frmCtrl & IEEE802_11_FC_TO_DS_MASK) && (frmCtrl & IEEE802_11_FC_FROM_DS_MASK)) ? 30 : 24) );
+
+            	unifi_trace(priv, UDBG4, "check_routing_pkt_data_ind() :: Value of the QoS control field - 0x%04x \n", qosControl);
+
+                if (qosControl & IEEE802_11_QC_NON_TID_BITS_MASK)
+                {
+                	unifi_trace(priv, UDBG4, "Ignore the MIC failure and pass the MPDU to the stack when any of bits [4-15] is set in the QoS control field\n");
+
+            		/*Exclude the MIC [16] and the PN [16] that are appended by the firmware*/
+            		((bulk_data_param_t*)bulkdata)->d[0].data_length = bulkdata->d[0].data_length - 32;
+
+            		/*Clear the reception status of the signal (CSR_RX_SUCCESS)*/
+            		*(sigdata + CSR_WIFI_MA_PKT_IND_RECEPTION_STATUS_OFFSET)     = 0x00;
+            		*(sigdata + CSR_WIFI_MA_PKT_IND_RECEPTION_STATUS_OFFSET+1)   = 0x00;
+
+            		*freeBulkData = FALSE;
+
+            		return FALSE;
+                }
+            }
+#endif
+            /* If this MIC ERROR reported by the firmware is either for
+             *    [1] a WAPI Multicast MPDU and the Multicast filter has NOT been set (It is set only when group key index (MSKID) = 1 in Group Rekeying)   OR
+             *    [2] a WAPI Unicast MPDU and either the CONTROL PORT is open or the WAPI Unicast filter or filter(s) is NOT set
+             * then report a MIC FAILURE indication to the SME.
+             */
+#ifndef CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION
+    	if ((priv->wapi_multicast_filter == 0) || isWapiUnicastPkt) {
+#else
+        /*When SW encryption is enabled and USKID=1 (wapi_unicast_filter = 1), we are expected
+		 *to receive MIC failure INDs for unicast MPDUs*/
+    	if ( ((priv->wapi_multicast_filter == 0) && !isWapiUnicastPkt) ||
+             ((priv->wapi_unicast_filter   == 0) &&  isWapiUnicastPkt) ) {
+#endif
+                /*Discard the frame*/
+                *freeBulkData = TRUE;
+                unifi_trace(priv, UDBG4, "Discarding the contents of the frame with MIC failure \n");
+
+                if (isWapiUnicastPkt &&
+                    ((uf_sme_port_state(priv,srcAddr,UF_CONTROLLED_PORT_Q,interfaceTag) != CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN)||
+#ifndef CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION
+                    (priv->wapi_unicast_filter) ||
+#endif
+                    (priv->wapi_unicast_queued_pkt_filter))) {
+
+                    /* Workaround to handle MIC failures reported by the firmware for encrypted packets from the AP
+                     * while we are in the process of re-association induced by unsupported WAPI Unicast key index
+                     *             - Discard the packets with MIC failures "until" we have
+                     *               a. negotiated a key,
+                     *               b. opened the CONTROL PORT and
+                     *               c. the AP has started using the new key
+                     */
+                    unifi_trace(priv, UDBG4, "Ignoring the MIC failure as either a. CONTROL PORT isn't OPEN or b. Unicast filter is set or c. WAPI AP using old key for buffered pkts\n");
+
+                    /*Ignore this MIC failure*/
+                    return FALSE;
+
+                }/*WAPI re-key specific workaround*/
+
+                unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind - MIC FAILURE : interfaceTag %x Src Addr %x:%x:%x:%x:%x:%x\n",
+                            interfaceTag, srcAddr[0], srcAddr[1], srcAddr[2], srcAddr[3], srcAddr[4], srcAddr[5]);
+                unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind - MIC FAILURE : Dest Addr %x:%x:%x:%x:%x:%x\n",
+                            destAddr[0], destAddr[1], destAddr[2], destAddr[3], destAddr[4], destAddr[5]);
+                unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind - MIC FAILURE : Control Port State - 0x%.4X \n",
+                            uf_sme_port_state(priv,srcAddr,UF_CONTROLLED_PORT_Q,interfaceTag));
+
+                unifi_error(priv, "MIC failure in %s\n", __FUNCTION__);
+
+                /*Report the MIC failure to the SME*/
+                return TRUE;
+            }
+        }/* STA mode */
+        else {
+            /* Its AP Mode . Just Return */
+            *freeBulkData = TRUE;
+            unifi_error(priv, "MIC failure in %s\n", __FUNCTION__);
+            return TRUE;
+         } /* AP mode */
+    }/* MIC error */
+#else
+    if (receptionStatus == CSR_MICHAEL_MIC_ERROR) {
+        *freeBulkData = TRUE;
+        unifi_error(priv, "MIC failure in %s\n", __FUNCTION__);
+        return TRUE;
+    }
+#endif /*CSR_WIFI_SECURITY_WAPI_ENABLE*/
+
+    unifi_trace(priv, UDBG4, "frmCtrl = 0x%04x %s\n",
+                frmCtrl,
+                (((frmCtrl & 0x000c)>>FRAME_CONTROL_TYPE_FIELD_OFFSET) == IEEE802_11_FRAMETYPE_MANAGEMENT) ?
+                    "Mgt" : "Ctrl/Data");
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    /* To ignore MIC failures reported due to the WAPI AP using the old key for queued packets before
+     * starting to use the new key negotiated as part of unicast re-keying
+     */
+    if ((interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_STA)&&
+        isWapiUnicastPkt &&
+        (receptionStatus == CSR_RX_SUCCESS) &&
+        (priv->wapi_unicast_queued_pkt_filter==1)) {
+
+        unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind(): WAPI unicast pkt received when the (wapi_unicast_queued_pkt_filter) is set\n");
+
+        if (isDataFrame) {
+            switch(frmCtrl & IEEE80211_FC_SUBTYPE_MASK) {
+                case IEEE802_11_FC_TYPE_QOS_DATA & IEEE80211_FC_SUBTYPE_MASK:
+                    llcSnapHeaderOffset = MAC_HEADER_SIZE + 2;
+                    break;
+                case IEEE802_11_FC_TYPE_QOS_NULL & IEEE80211_FC_SUBTYPE_MASK:
+                case IEEE802_11_FC_TYPE_NULL & IEEE80211_FC_SUBTYPE_MASK:
+                    break;
+                default:
+                    llcSnapHeaderOffset = MAC_HEADER_SIZE;
+            }
+        }
+
+        if (llcSnapHeaderOffset > 0) {
+        	/* QoS data or Data */
+            unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind(): SNAP header found & its offset %d\n",llcSnapHeaderOffset);
+            if (memcmp((u8 *)(bulkdata->d[0].os_data_ptr+llcSnapHeaderOffset+wapiProtocolIdSNAPHeaderOffset),
+                       wapiProtocolIdSNAPHeader,sizeof(wapiProtocolIdSNAPHeader))) {
+
+            	unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind(): This is a data & NOT a WAI protocol packet\n");
+                /* On the first unicast data pkt that is decrypted successfully after re-keying, reset the filter */
+                priv->wapi_unicast_queued_pkt_filter = 0;
+                unifi_trace(priv, UDBG4, "check_routing_pkt_data_ind(): WAPI AP has started using the new unicast key, no more MIC failures expected (reset filter)\n");
+            }
+            else {
+                unifi_trace(priv, UDBG6, "check_routing_pkt_data_ind(): WAPI - This is a WAI protocol packet\n");
+            }
+        }
+	}
+#endif
+
+
+    switch ((frmCtrl & 0x000c)>>FRAME_CONTROL_TYPE_FIELD_OFFSET) {
+        case IEEE802_11_FRAMETYPE_MANAGEMENT:
+            *freeBulkData = TRUE;       /* Free (after SME handler copies it) */
+
+            /* In P2P device mode, filter the legacy AP beacons here */
+            if((interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2P)&&\
+               ((CSR_WIFI_80211_GET_FRAME_SUBTYPE(macHdrLocation)) == CSR_WIFI_80211_FRAME_SUBTYPE_BEACON)){
+
+                u8 *pSsid, *pSsidLen;
+                static u8 P2PWildCardSsid[CSR_WIFI_P2P_WILDCARD_SSID_LENGTH] = {'D', 'I', 'R', 'E', 'C', 'T', '-'};
+
+                pSsidLen = macHdrLocation + MAC_HEADER_SIZE + CSR_WIFI_BEACON_FIXED_LENGTH;
+                pSsid = pSsidLen + 2;
+
+                if(*(pSsidLen + 1) >= CSR_WIFI_P2P_WILDCARD_SSID_LENGTH){
+                    if(memcmp(pSsid, P2PWildCardSsid, CSR_WIFI_P2P_WILDCARD_SSID_LENGTH) == 0){
+                        unifi_trace(priv, UDBG6, "Received a P2P Beacon, pass it to SME\n");
+                        return TRUE;
+                    }
+                }
+                unifi_trace(priv, UDBG6, "Received a Legacy AP beacon in P2P mode, drop it\n");
+                return FALSE;
+            }
+            return TRUE;                /* Route to SME */
+        case IEEE802_11_FRAMETYPE_DATA:
+        case IEEE802_11_FRAMETYPE_CONTROL:
+            *freeBulkData = FALSE;      /* Network stack or MLME handler frees */
+            return FALSE;
+        default:
+            unifi_error(priv, "Unhandled frame type %04x\n", frmCtrl);
+            *freeBulkData = TRUE;       /* Not interested, but must free it */
+            return FALSE;
+    }
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_process_receive_event
+ *
+ *      Dispatcher for received signals.
+ *
+ *      This function receives the 'to host' signals and forwards
+ *      them to the unifi linux clients.
+ *
+ *  Arguments:
+ *      ospriv      Pointer to driver's private data.
+ *      sigdata     Pointer to the packed signal buffer.
+ *      siglen      Length of the packed signal.
+ *      bulkdata    Pointer to the signal's bulk data.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *  The signals are received in the format described in the host interface
+ *  specification, i.e wire formatted. Certain clients use the same format
+ *  to interpret them and other clients use the host formatted structures.
+ *  Each client has to call read_unpack_signal() to transform the wire
+ *  formatted signal into the host formatted signal, if necessary.
+ *  The code is in the core, since the signals are defined therefore
+ *  binded to the host interface specification.
+ * ---------------------------------------------------------------------------
+ */
+static void
+unifi_process_receive_event(void *ospriv,
+                            u8 *sigdata, u32 siglen,
+                            const bulk_data_param_t *bulkdata)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    int i, receiver_id;
+    int client_id;
+    s16 signal_id;
+    u8 pktIndToSme = FALSE, freeBulkData = FALSE;
+
+    func_enter();
+
+    unifi_trace(priv, UDBG5, "unifi_process_receive_event: "
+                "%04x %04x %04x %04x %04x %04x %04x %04x (%d)\n",
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*0) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*1) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*2) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*3) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*4) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*5) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*6) & 0xFFFF,
+                CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*7) & 0xFFFF,
+                siglen);
+
+    receiver_id = CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)) & 0xFF00;
+    client_id = (receiver_id & 0x0F00) >> UDI_SENDER_ID_SHIFT;
+    signal_id = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata);
+
+
+
+    /* check for the type of frame received (checks for 802.11 management frames) */
+    if (signal_id == CSR_MA_PACKET_INDICATION_ID)
+    {
+#define CSR_MA_PACKET_INDICATION_INTERFACETAG_OFFSET    14
+        u8 interfaceTag;
+        netInterface_priv_t *interfacePriv;
+
+        /* Pull out interface tag from virtual interface identifier */
+        interfaceTag = (CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata + CSR_MA_PACKET_INDICATION_INTERFACETAG_OFFSET)) & 0xff;
+        interfacePriv = priv->interfacePriv[interfaceTag];
+
+        /* Update activity for this station in case of IBSS */
+#ifdef CSR_SUPPORT_SME
+        if (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_IBSS)
+        {
+            u8 *saddr;
+            /* Fetch the source address from  mac header */
+            saddr = (u8 *) bulkdata->d[0].os_data_ptr + MAC_HEADER_ADDR2_OFFSET;
+            unifi_trace(priv, UDBG5,
+                                    "Updating sta activity in IBSS interfaceTag %x Src Addr %x:%x:%x:%x:%x:%x\n",
+                                    interfaceTag, saddr[0], saddr[1], saddr[2], saddr[3], saddr[4], saddr[5]);
+
+            uf_update_sta_activity(priv, interfaceTag, saddr);
+        }
+#endif
+
+        pktIndToSme = check_routing_pkt_data_ind(priv, sigdata, bulkdata, &freeBulkData, interfacePriv);
+
+        unifi_trace(priv, UDBG6, "RX: packet entry point to driver from HIP,pkt to SME ?(%s) \n", (pktIndToSme)? "YES":"NO");
+
+    }
+
+    if (pktIndToSme)
+    {
+        /* Management MA_PACKET_IND for SME */
+        if(sigdata != NULL && bulkdata != NULL){
+            send_to_client(priv, priv->sme_cli, receiver_id, sigdata, siglen, bulkdata);
+        }
+        else{
+            unifi_error(priv, "unifi_receive_event2: sigdata or Bulkdata is NULL \n");
+        }
+#ifdef CSR_NATIVE_LINUX
+        send_to_client(priv, priv->wext_client,
+                receiver_id,
+                sigdata, siglen, bulkdata);
+#endif
+    }
+    else
+    {
+        /* Signals with ReceiverId==0 are also reported to SME / WEXT,
+         * unless they are data/control MA_PACKET_INDs or VIF_AVAILABILITY_INDs
+         */
+        if (!receiver_id) {
+               if(signal_id == CSR_MA_VIF_AVAILABILITY_INDICATION_ID) {
+                      uf_process_ma_vif_availibility_ind(priv, sigdata, siglen);
+               }
+               else if (signal_id != CSR_MA_PACKET_INDICATION_ID) {
+                      send_to_client(priv, priv->sme_cli, receiver_id, sigdata, siglen, bulkdata);
+#ifdef CSR_NATIVE_LINUX
+                      send_to_client(priv, priv->wext_client,
+                                     receiver_id,
+                                     sigdata, siglen, bulkdata);
+#endif
+               }
+               else
+               {
+
+#if (defined(CSR_SUPPORT_SME) && defined(CSR_WIFI_SECURITY_WAPI_ENABLE))
+                   #define CSR_MA_PACKET_INDICATION_RECEPTION_STATUS_OFFSET    sizeof(CSR_SIGNAL_PRIMITIVE_HEADER) + 22
+                   netInterface_priv_t *interfacePriv;
+                   u8 interfaceTag;
+                   u16 receptionStatus = CSR_RX_SUCCESS;
+
+                   /* Pull out interface tag from virtual interface identifier */
+                   interfaceTag = (CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata + CSR_MA_PACKET_INDICATION_INTERFACETAG_OFFSET)) & 0xff;
+                   interfacePriv = priv->interfacePriv[interfaceTag];
+
+                   /* check for MIC failure */
+                   receptionStatus = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata + CSR_MA_PACKET_INDICATION_RECEPTION_STATUS_OFFSET);
+
+                   /* Send a WAPI MPDU to SME for re-check MIC if the respective filter has been set*/
+                   if ((!freeBulkData) &&
+                       (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_STA) &&
+                       (receptionStatus == CSR_MICHAEL_MIC_ERROR) &&
+                       ((priv->wapi_multicast_filter == 1)
+#ifdef CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION
+                         || (priv->wapi_unicast_filter == 1)
+#endif
+                       ))
+                   {
+                       CSR_SIGNAL signal;
+                       u8 *destAddr;
+                       CsrResult res;
+                       u16 interfaceTag = 0;
+                       u8 isMcastPkt = TRUE;
+
+                       unifi_trace(priv, UDBG6, "Received a WAPI data packet when the Unicast/Multicast filter is set\n");
+                       res = read_unpack_signal(sigdata, &signal);
+                       if (res) {
+                           unifi_error(priv, "Received unknown or corrupted signal (0x%x).\n",
+                                       CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata));
+                           return;
+                       }
+
+                       /* Check if the type of MPDU and the respective filter status*/
+                       destAddr = (u8 *) bulkdata->d[0].os_data_ptr + MAC_HEADER_ADDR1_OFFSET;
+                       isMcastPkt = (destAddr[0] & 0x01) ? TRUE : FALSE;
+                       unifi_trace(priv, UDBG6,
+                                   "1.MPDU type: (%s), 2.Multicast filter: (%s), 3. Unicast filter: (%s)\n",
+                                   ((isMcastPkt) ? "Multiast":"Unicast"),
+                                   ((priv->wapi_multicast_filter) ? "Enabled":"Disabled"),
+                                   ((priv->wapi_unicast_filter)  ? "Enabled":"Disabled"));
+
+                       if (((isMcastPkt) && (priv->wapi_multicast_filter == 1))
+#ifdef CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION
+                           || ((!isMcastPkt) && (priv->wapi_unicast_filter == 1))
+#endif
+                          )
+                        {
+                            unifi_trace(priv, UDBG4, "Sending the WAPI MPDU for MIC check\n");
+                            CsrWifiRouterCtrlWapiRxMicCheckIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0, interfaceTag, siglen, sigdata, bulkdata->d[0].data_length, (u8*)bulkdata->d[0].os_data_ptr);
+
+                            for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+                                if (bulkdata->d[i].data_length != 0) {
+                                    unifi_net_data_free(priv, (void *)&bulkdata->d[i]);
+                                }
+                           }
+                           func_exit();
+                           return;
+                       }
+                   } /* CSR_MA_PACKET_INDICATION_ID */
+#endif /*CSR_SUPPORT_SME && CSR_WIFI_SECURITY_WAPI_ENABLE*/
+               }
+        }
+
+        /* calls the registered clients handler callback func.
+         * netdev_mlme_event_handler is one of the registered handler used to route
+         * data packet to network stack or AMP/EAPOL related data to SME
+         *
+         * The freeBulkData check ensures that, it has received a management frame and
+         * the frame needs to be freed here. So not to be passed to netdev handler
+         */
+        if(!freeBulkData){
+            if ((client_id < MAX_UDI_CLIENTS) &&
+                    (&priv->ul_clients[client_id] != priv->logging_client)) {
+            	unifi_trace(priv, UDBG6, "Call the registered clients handler callback func\n");
+                send_to_client(priv, &priv->ul_clients[client_id],
+                        receiver_id,
+                        sigdata, siglen, bulkdata);
+            }
+        }
+    }
+
+    /*
+     * Free bulk data buffers here unless it is a CSR_MA_PACKET_INDICATION
+     */
+    switch (signal_id)
+    {
+#ifdef UNIFI_SNIFF_ARPHRD
+        case CSR_MA_SNIFFDATA_INDICATION_ID:
+#endif
+            break;
+
+        case CSR_MA_PACKET_INDICATION_ID:
+            if (!freeBulkData)
+            {
+                break;
+            }
+            /* FALLS THROUGH... */
+        default:
+            for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+                if (bulkdata->d[i].data_length != 0) {
+                    unifi_net_data_free(priv, (void *)&bulkdata->d[i]);
+                }
+            }
+    }
+
+    func_exit();
+} /* unifi_process_receive_event() */
+
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+static u8 signal_buffer_is_full(unifi_priv_t* priv)
+{
+    return (((priv->rxSignalBuffer.writePointer + 1)% priv->rxSignalBuffer.size) == (priv->rxSignalBuffer.readPointer));
+}
+
+void unifi_rx_queue_flush(void *ospriv)
+{
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+
+    func_enter();
+    unifi_trace(priv, UDBG4, "rx_wq_handler: RdPtr = %d WritePtr =  %d\n",
+                priv->rxSignalBuffer.readPointer,priv->rxSignalBuffer.writePointer);
+    if(priv != NULL) {
+        u8 readPointer = priv->rxSignalBuffer.readPointer;
+        while (readPointer != priv->rxSignalBuffer.writePointer)
+        {
+             rx_buff_struct_t *buf = &priv->rxSignalBuffer.rx_buff[readPointer];
+             unifi_trace(priv, UDBG6, "rx_wq_handler: RdPtr = %d WritePtr =  %d\n",
+                         readPointer,priv->rxSignalBuffer.writePointer);
+             unifi_process_receive_event(priv, buf->bufptr, buf->sig_len, &buf->data_ptrs);
+             readPointer ++;
+             if(readPointer >= priv->rxSignalBuffer.size) {
+                    readPointer = 0;
+             }
+        }
+        priv->rxSignalBuffer.readPointer = readPointer;
+    }
+    func_exit();
+}
+
+void rx_wq_handler(struct work_struct *work)
+{
+    unifi_priv_t *priv = container_of(work, unifi_priv_t, rx_work_struct);
+    unifi_rx_queue_flush(priv);
+}
+#endif
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  unifi_receive_event
+ *
+ *      Dispatcher for received signals.
+ *
+ *      This function receives the 'to host' signals and forwards
+ *      them to the unifi linux clients.
+ *
+ *  Arguments:
+ *      ospriv      Pointer to driver's private data.
+ *      sigdata     Pointer to the packed signal buffer.
+ *      siglen      Length of the packed signal.
+ *      bulkdata    Pointer to the signal's bulk data.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *  The signals are received in the format described in the host interface
+ *  specification, i.e wire formatted. Certain clients use the same format
+ *  to interpret them and other clients use the host formatted structures.
+ *  Each client has to call read_unpack_signal() to transform the wire
+ *  formatted signal into the host formatted signal, if necessary.
+ *  The code is in the core, since the signals are defined therefore
+ *  binded to the host interface specification.
+ * ---------------------------------------------------------------------------
+ */
+void
+unifi_receive_event(void *ospriv,
+                    u8 *sigdata, u32 siglen,
+                    const bulk_data_param_t *bulkdata)
+{
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    u8 writePointer;
+    int i;
+    rx_buff_struct_t * rx_buff;
+    func_enter();
+
+    unifi_trace(priv, UDBG5, "unifi_receive_event: "
+            "%04x %04x %04x %04x %04x %04x %04x %04x (%d)\n",
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*0) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*1) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*2) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*3) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*4) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*5) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*6) & 0xFFFF,
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN((sigdata) + sizeof(s16)*7) & 0xFFFF, siglen);
+    if(signal_buffer_is_full(priv)) {
+        unifi_error(priv,"TO HOST signal queue FULL dropping the PDU\n");
+        for (i = 0; i < UNIFI_MAX_DATA_REFERENCES; i++) {
+            if (bulkdata->d[i].data_length != 0) {
+                unifi_net_data_free(priv, (void *)&bulkdata->d[i]);
+            }
+        }
+        return;
+    }
+    writePointer = priv->rxSignalBuffer.writePointer;
+    rx_buff = &priv->rxSignalBuffer.rx_buff[writePointer];
+    memcpy(rx_buff->bufptr,sigdata,siglen);
+    rx_buff->sig_len = siglen;
+    rx_buff->data_ptrs = *bulkdata;
+    writePointer++;
+    if(writePointer >= priv->rxSignalBuffer.size) {
+        writePointer =0;
+    }
+    unifi_trace(priv, UDBG4, "unifi_receive_event:writePtr = %d\n",priv->rxSignalBuffer.writePointer);
+    priv->rxSignalBuffer.writePointer = writePointer;
+
+#ifndef CSR_WIFI_RX_PATH_SPLIT_DONT_USE_WQ
+    queue_work(priv->rx_workqueue, &priv->rx_work_struct);
+#endif
+
+#else
+    unifi_process_receive_event(ospriv, sigdata, siglen, bulkdata);
+#endif
+    func_exit();
+} /* unifi_receive_event() */
+
diff --git a/drivers/staging/csr/unifi_native.h b/drivers/staging/csr/unifi_native.h
new file mode 100644
index 00000000000..a73b38e5e48
--- /dev/null
+++ b/drivers/staging/csr/unifi_native.h
@@ -0,0 +1,257 @@
+/*
+ *****************************************************************************
+ *
+ * FILE : unifi_native.h
+ *
+ * PURPOSE : Private header file for unifi driver support to wireless extensions.
+ *
+ *           UDI = UniFi Debug Interface
+ *
+ * Copyright (C) 2005-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ *****************************************************************************
+ */
+#ifndef __LINUX_UNIFI_NATIVE_H__
+#define __LINUX_UNIFI_NATIVE_H__ 1
+
+#include <linux/kernel.h>
+#include <linux/if_arp.h>
+
+
+/*
+ *      scan.c wext.c autojoin.c
+ */
+/* Structure to hold results of a scan */
+typedef struct scan_info {
+
+/*    CSR_MLME_SCAN_INDICATION msi; */
+
+    unsigned char *info_elems;
+    int info_elem_length;
+
+} scan_info_t;
+
+
+#define IE_VECTOR_MAXLEN 1024
+
+#ifdef CSR_SUPPORT_WEXT
+/*
+ * Structre to hold the wireless network configuration info.
+ */
+struct wext_config {
+
+    /* Requested channel when setting up an adhoc network */
+    int channel;
+
+    /* wireless extns mode: IW_MODE_AUTO, ADHOC, INFRA, MASTER ... MONITOR */
+    int mode;
+
+    /* The capabilities of the currently joined network */
+    int capability;
+
+    /* The interval between beacons if we create an IBSS */
+    int beacon_period;
+
+    /*
+    * Power-save parameters
+    */
+    /* The listen interval to ask for in Associate req. */
+    int assoc_listen_interval;
+    /* Power-mode to put UniFi into */
+
+    unsigned char desired_ssid[UNIFI_MAX_SSID_LEN];     /* the last ESSID set by SIOCSIWESSID */
+    int power_mode;
+    /* Whether to wake for broadcast packets (using DTIM interval) */
+    int wakeup_for_dtims;
+
+    /* Currently selected WEP Key ID (0..3) */
+    int wep_key_id;
+
+    wep_key_t wep_keys[NUM_WEPKEYS];
+
+/*    CSR_AUTHENTICATION_TYPE auth_type; */
+    int privacy;
+
+    u32 join_failure_timeout;
+    u32 auth_failure_timeout;
+    u32 assoc_failure_timeout;
+
+    unsigned char generic_ie[IE_VECTOR_MAXLEN];
+    int generic_ie_len;
+
+    struct iw_statistics wireless_stats;
+
+
+    /* the ESSID we are currently associated to */
+    unsigned char current_ssid[UNIFI_MAX_SSID_LEN];
+    /* the BSSID we are currently associated to */
+    unsigned char current_bssid[6];
+
+    /*
+    * IW_AUTH_WPA_VERSION_DISABLED 0x00000001
+    * IW_AUTH_WPA_VERSION_WPA      0x00000002
+    * IW_AUTH_WPA_VERSION_WPA2     0x00000004
+    */
+    unsigned char wpa_version;
+
+    /*
+     * cipher selection:
+    * IW_AUTH_CIPHER_NONE	0x00000001
+    * IW_AUTH_CIPHER_WEP40	0x00000002
+    * IW_AUTH_CIPHER_TKIP	0x00000004
+    * IW_AUTH_CIPHER_CCMP	0x00000008
+    * IW_AUTH_CIPHER_WEP104	0x00000010
+    */
+    unsigned char pairwise_cipher_used;
+    unsigned char group_cipher_used;
+
+    unsigned int frag_thresh;
+    unsigned int rts_thresh;
+
+    /* U-APSD value, send with Association Request to WMM Enabled APs */
+    unsigned char wmm_bss_uapsd_mask;
+    /* The WMM capabilities of the selected BSS */
+    unsigned int bss_wmm_capabilities;
+
+    /* Flag to prevent a join when the ssid is set */
+    int disable_join_on_ssid_set;
+
+    /* Scan info */
+#define UNIFI_MAX_SCANS 32
+    scan_info_t scan_list[UNIFI_MAX_SCANS];
+    int num_scan_info;
+
+    /* Flag on whether non-802.1x packets are allowed out */
+/*    CsrWifiRouterPortAction block_controlled_port;*/
+
+    /* Flag on whether we have completed an authenticate/associate process */
+    unsigned int flag_associated        : 1;
+}; /* struct wext_config */
+
+#endif /* CSR_SUPPORT_WEXT */
+
+
+/*
+ *      wext.c
+ */
+/*int mlme_set_protection(unifi_priv_t *priv, unsigned char *addr,
+                        CSR_PROTECT_TYPE prot, CSR_KEY_TYPE key_type);
+*/
+
+/*
+ * scan.c
+ */
+/*
+void unifi_scan_indication_handler(unifi_priv_t *priv,
+                                   const CSR_MLME_SCAN_INDICATION *msg,
+                                   const unsigned char *extra,
+                                   unsigned int len);
+*/
+void unifi_clear_scan_table(unifi_priv_t *priv);
+scan_info_t *unifi_get_scan_report(unifi_priv_t *priv, int index);
+
+
+/*
+ * Utility functions
+ */
+const unsigned char *unifi_find_info_element(int id,
+                                             const unsigned char *info,
+                                             int len);
+int unifi_add_info_element(unsigned char *info,
+                           int ie_id,
+                           const unsigned char *ie_data,
+                           int ie_len);
+
+/*
+ *      autojoin.c
+ */
+/* Higher level fns */
+int unifi_autojoin(unifi_priv_t *priv, const char *ssid);
+/*
+int unifi_do_scan(unifi_priv_t *priv, int scantype, CSR_BSS_TYPE bsstype,
+                  const char *ssid, int ssid_len);
+*/
+int unifi_set_powermode(unifi_priv_t *priv);
+int unifi_join_ap(unifi_priv_t *priv, scan_info_t *si);
+int unifi_join_bss(unifi_priv_t *priv, unsigned char *macaddr);
+int unifi_leave(unifi_priv_t *priv);
+unsigned int unifi_get_wmm_bss_capabilities(unifi_priv_t *priv,
+                                            unsigned char *ie_vector,
+                                            int ie_len, int *ap_capabilities);
+
+/*
+ * Status and management.
+ */
+int uf_init_wext_interface(unifi_priv_t *priv);
+void uf_deinit_wext_interface(unifi_priv_t *priv);
+
+/*
+ * Function to reset UniFi's 802.11 state by sending MLME-RESET.req
+ */
+int unifi_reset_state(unifi_priv_t *priv, unsigned char *macaddr, unsigned char set_default_mib);
+
+
+/*
+ *      mlme.c
+ */
+/* Abort an MLME operation - useful in error recovery */
+int uf_abort_mlme(unifi_priv_t *priv);
+
+int unifi_mlme_blocking_request(unifi_priv_t *priv, ul_client_t *pcli,
+                                CSR_SIGNAL *sig, bulk_data_param_t *data_ptrs,
+                                int timeout);
+void unifi_mlme_copy_reply_and_wakeup_client(ul_client_t *pcli,
+                                             CSR_SIGNAL *signal, int signal_len,
+                                             const bulk_data_param_t *bulkdata);
+
+/*
+ * Utility functions
+ */
+const char *lookup_reason_code(int reason);
+const char *lookup_result_code(int result);
+
+
+/*
+ *      sme_native.c
+ */
+int uf_sme_init(unifi_priv_t *priv);
+void uf_sme_deinit(unifi_priv_t *priv);
+int sme_sys_suspend(unifi_priv_t *priv);
+int sme_sys_resume(unifi_priv_t *priv);
+int sme_mgt_wifi_on(unifi_priv_t *priv);
+
+/* Callback for event logging to SME clients (unifi_manager) */
+void sme_native_log_event(ul_client_t *client,
+                          const u8 *sig_packed, int sig_len,
+                          const bulk_data_param_t *bulkdata,
+                          int dir);
+
+void sme_native_mlme_event_handler(ul_client_t *pcli,
+                                   const u8 *sig_packed, int sig_len,
+                                   const bulk_data_param_t *bulkdata,
+                                   int dir);
+
+/* Task to query statistics from the MIB */
+#define UF_SME_STATS_WQ_TIMEOUT     2000    /* in msecs */
+void uf_sme_stats_wq(struct work_struct *work);
+
+void uf_native_process_udi_signal(ul_client_t *pcli,
+                                  const u8 *packed_signal,
+                                  int packed_signal_len,
+                                  const bulk_data_param_t *bulkdata, int dir);
+#ifdef UNIFI_SNIFF_ARPHRD
+/*
+ * monitor.c
+ */
+int uf_start_sniff(unifi_priv_t *priv);
+/*
+void ma_sniffdata_ind(void *ospriv,
+                      const CSR_MA_SNIFFDATA_INDICATION *ind,
+                      const bulk_data_param_t *bulkdata);
+*/
+#endif /* ARPHRD_IEEE80211_PRISM */
+
+#endif /* __LINUX_UNIFI_NATIVE_H__ */
diff --git a/drivers/staging/csr/unifi_os.h b/drivers/staging/csr/unifi_os.h
new file mode 100644
index 00000000000..4e63a942f1a
--- /dev/null
+++ b/drivers/staging/csr/unifi_os.h
@@ -0,0 +1,145 @@
+/*
+ * ---------------------------------------------------------------------------
+ *
+ * FILE: os_linux/unifi_os.h
+ *
+ * PURPOSE:
+ *      This header file provides the OS-dependent facilities for a linux
+ *      environment.
+ *
+ * Copyright (C) 2005-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __UNIFI_OS_LINUX_H__
+#define __UNIFI_OS_LINUX_H__ 1
+
+#include <linux/kernel.h>
+#include <linux/time.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+
+/*
+ * Needed for core/signals.c
+ */
+#include <stddef.h>
+
+
+/* Define INLINE directive*/
+#define INLINE      inline
+
+/* Malloc and free */
+CsrResult unifi_net_data_malloc(void *ospriv, bulk_data_desc_t *bulk_data_slot, unsigned int size);
+void unifi_net_data_free(void *ospriv, bulk_data_desc_t *bulk_data_slot);
+#define CSR_WIFI_ALIGN_BYTES    4
+CsrResult unifi_net_dma_align(void *ospriv, bulk_data_desc_t *bulk_data_slot);
+
+/*
+ * Byte Order
+ * Note that __le*_to_cpu and __cpu_to_le* return an unsigned value!
+ */
+#ifdef __KERNEL__
+#define unifi2host_16(n)	(__le16_to_cpu((n)))
+#define unifi2host_32(n)	(__le32_to_cpu((n)))
+#define host2unifi_16(n)	(__cpu_to_le16((n)))
+#define host2unifi_32(n)	(__cpu_to_le32((n)))
+#endif
+
+/* Module parameters */
+extern int unifi_debug;
+
+/* debugging */
+#ifdef UNIFI_DEBUG
+/*
+ * unifi_debug is a verbosity level for debug messages
+ * UDBG0 msgs are always printed if UNIFI_DEBUG is defined
+ * UDBG1 msgs are printed if UNIFI_DEBUG is defined and unifi_debug > 0
+ * etc.
+ */
+
+#define func_enter()                                    \
+    do {                                                \
+        if (unifi_debug >= 5) {                         \
+            printk("unifi: => %s\n", __FUNCTION__);     \
+        }                                               \
+    } while (0)
+#define func_exit()                                     \
+    do {                                                \
+        if (unifi_debug >= 5) {                         \
+            printk("unifi: <= %s\n", __FUNCTION__);     \
+        }                                               \
+    } while (0)
+#define func_exit_r(_rc)                                        \
+    do {                                                        \
+        if (unifi_debug >= 5) {                                 \
+            printk("unifi: <= %s %d\n", __FUNCTION__, (int)(_rc));   \
+        }                                                       \
+    } while (0)
+
+
+#define ASSERT(cond)                                            \
+    do {                                                            \
+        if (!(cond)) {                                              \
+            printk("Assertion failed in %s at %s:%d: %s\n",         \
+                    __FUNCTION__, __FILE__, __LINE__, #cond);        \
+        }                                                           \
+    } while (0)
+
+
+void unifi_dump(void *ospriv, int lvl, const char *msg, void *mem, u16 len);
+void dump(void *mem, u16 len);
+void dump16(void *mem, u16 len);
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+void dump_str(void *mem, u16 len);
+#endif /* CSR_WIFI_HIP_DEBUG_OFFLINE */
+
+void unifi_error(void* ospriv, const char *fmt, ...);
+void unifi_warning(void* ospriv, const char *fmt, ...);
+void unifi_notice(void* ospriv, const char *fmt, ...);
+void unifi_info(void* ospriv, const char *fmt, ...);
+
+void unifi_trace(void* ospriv, int level, const char *fmt, ...);
+
+#else
+
+/* Stubs */
+#define func_enter()
+#define func_exit()
+#define func_exit_r(_rc)
+
+#define ASSERT(cond)
+
+static inline void unifi_dump(void *ospriv, int lvl, const char *msg, void *mem, u16 len) {}
+static inline void dump(void *mem, u16 len) {}
+static inline void dump16(void *mem, u16 len) {}
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+static inline void dump_str(void *mem, u16 len) {}
+#endif /* CSR_WIFI_HIP_DEBUG_OFFLINE */
+
+void unifi_error_nop(void* ospriv, const char *fmt, ...);
+void unifi_trace_nop(void* ospriv, int level, const char *fmt, ...);
+#define unifi_error if(1);else unifi_error_nop
+#define unifi_warning if(1);else unifi_error_nop
+#define unifi_notice if(1);else unifi_error_nop
+#define unifi_info if(1);else unifi_error_nop
+#define unifi_trace if(1);else unifi_trace_nop
+
+#endif /* UNIFI_DEBUG */
+
+
+/* Different levels of diagnostic detail... */
+#define UDBG0       0   /* always prints in debug build */
+#define UDBG1       1
+#define UDBG2       2
+#define UDBG3       3
+#define UDBG4       4
+#define UDBG5       5
+#define UDBG6       6
+#define UDBG7       7
+
+
+#endif /* __UNIFI_OS_LINUX_H__ */
diff --git a/drivers/staging/csr/unifi_pdu_processing.c b/drivers/staging/csr/unifi_pdu_processing.c
new file mode 100644
index 00000000000..7c7e8d49ae4
--- /dev/null
+++ b/drivers/staging/csr/unifi_pdu_processing.c
@@ -0,0 +1,3755 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     unifi_pdu_processing.c
+ *
+ * PURPOSE:
+ *      This file provides the PDU handling functionality before it gets sent to unfi and after
+ *      receiving a PDU from unifi
+ *
+ * Copyright (C) 2010 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <linux/vmalloc.h>
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+#include "csr_time.h"
+#include "unifi_priv.h"
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13)
+#include <net/iw_handler.h>
+#endif
+#include <net/pkt_sched.h>
+
+#ifdef CSR_SUPPORT_SME
+static void _update_buffered_pkt_params_after_alignment(unifi_priv_t *priv, bulk_data_param_t *bulkdata,
+                                                        tx_buffered_packets_t* buffered_pkt)
+{
+    struct sk_buff *skb ;
+    u32 align_offset;
+
+    if (priv == NULL || bulkdata == NULL || buffered_pkt == NULL){
+        return;
+    }
+
+    skb = (struct sk_buff*)bulkdata->d[0].os_net_buf_ptr;
+    align_offset = (u32)(long)(bulkdata->d[0].os_data_ptr) & (CSR_WIFI_ALIGN_BYTES-1);
+    if(align_offset){
+        skb_pull(skb,align_offset);
+    }
+
+    buffered_pkt->bulkdata.os_data_ptr = bulkdata->d[0].os_data_ptr;
+    buffered_pkt->bulkdata.data_length = bulkdata->d[0].data_length;
+    buffered_pkt->bulkdata.os_net_buf_ptr = bulkdata->d[0].os_net_buf_ptr;
+    buffered_pkt->bulkdata.net_buf_length = bulkdata->d[0].net_buf_length;
+}
+#endif
+
+void
+unifi_frame_ma_packet_req(unifi_priv_t *priv, CSR_PRIORITY priority,
+                          CSR_RATE TransmitRate, CSR_CLIENT_TAG hostTag,
+                          u16 interfaceTag, CSR_TRANSMISSION_CONTROL transmissionControl,
+                          CSR_PROCESS_ID leSenderProcessId, u8 *peerMacAddress,
+                          CSR_SIGNAL *signal)
+{
+
+    CSR_MA_PACKET_REQUEST *req = &signal->u.MaPacketRequest;
+    netInterface_priv_t *interfacePriv;
+    u8 ba_session_idx = 0;
+    ba_session_tx_struct *ba_session = NULL;
+    u8 *ba_addr = NULL;
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+	unifi_trace(priv, UDBG5,
+		"In unifi_frame_ma_packet_req, Frame for Peer: %pMF\n",
+		peerMacAddress);
+    signal->SignalPrimitiveHeader.SignalId = CSR_MA_PACKET_REQUEST_ID;
+    signal->SignalPrimitiveHeader.ReceiverProcessId = 0;
+    signal->SignalPrimitiveHeader.SenderProcessId = leSenderProcessId;
+
+    /* Fill the MA-PACKET.req */
+    req->Priority = priority;
+    unifi_trace(priv, UDBG3, "Tx Frame with Priority: 0x%x\n", req->Priority);
+
+    /* A value of 0 is used for auto selection of rates. But for P2P GO case
+     * for action frames the rate is governed by SME. Hence instead of 0,
+     * the rate is filled in with the value passed here
+     */
+    req->TransmitRate = TransmitRate;
+
+    /* packets from netdev then no confirm required but packets from
+     * Nme/Sme eapol data frames requires the confirmation
+     */
+    req->TransmissionControl = transmissionControl;
+    req->VirtualInterfaceIdentifier =
+           uf_get_vif_identifier(interfacePriv->interfaceMode,interfaceTag);
+    memcpy(req->Ra.x, peerMacAddress, ETH_ALEN);
+
+    if (hostTag == 0xffffffff) {
+        req->HostTag = interfacePriv->tag++;
+        req->HostTag |= 0x40000000;
+        unifi_trace(priv, UDBG3, "new host tag assigned = 0x%x\n", req->HostTag);
+        interfacePriv->tag &= 0x0fffffff;
+    } else {
+        req->HostTag = hostTag;
+        unifi_trace(priv, UDBG3, "host tag got from SME  = 0x%x\n", req->HostTag);
+    }
+    /* check if BA session exists for the peer MAC address on same tID */
+    if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+       interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO){
+        ba_addr = peerMacAddress;
+    }else{
+        ba_addr = interfacePriv->bssid.a;
+    }
+    for (ba_session_idx=0; ba_session_idx < MAX_SUPPORTED_BA_SESSIONS_TX; ba_session_idx++){
+        ba_session = interfacePriv->ba_session_tx[ba_session_idx];
+        if (ba_session){
+           if ((!memcmp(ba_session->macAddress.a, ba_addr, ETH_ALEN)) && (ba_session->tID == priority)){
+                req->TransmissionControl |= CSR_ALLOW_BA;
+                break;
+            }
+        }
+    }
+
+    unifi_trace(priv, UDBG5, "leaving unifi_frame_ma_packet_req\n");
+}
+
+#ifdef CSR_SUPPORT_SME
+
+#define TRANSMISSION_CONTROL_TRIGGER_MASK 0x0001
+#define TRANSMISSION_CONTROL_EOSP_MASK 0x0002
+
+static
+int frame_and_send_queued_pdu(unifi_priv_t* priv,tx_buffered_packets_t* buffered_pkt,
+            CsrWifiRouterCtrlStaInfo_t *staRecord,u8 moreData , u8 eosp)
+{
+
+    CSR_SIGNAL signal;
+    bulk_data_param_t bulkdata;
+    int result;
+    u8 toDs, fromDs, macHeaderLengthInBytes = MAC_HEADER_SIZE;
+    u8 *qc;
+    u16 *fc = (u16*)(buffered_pkt->bulkdata.os_data_ptr);
+    unsigned long lock_flags;
+    unifi_trace(priv, UDBG3, "frame_and_send_queued_pdu with moreData: %d , EOSP: %d\n",moreData,eosp);
+    unifi_frame_ma_packet_req(priv, buffered_pkt->priority, buffered_pkt->rate, buffered_pkt->hostTag,
+               buffered_pkt->interfaceTag, buffered_pkt->transmissionControl,
+               buffered_pkt->leSenderProcessId, buffered_pkt->peerMacAddress.a, &signal);
+    bulkdata.d[0].os_data_ptr = buffered_pkt->bulkdata.os_data_ptr;
+    bulkdata.d[0].data_length = buffered_pkt->bulkdata.data_length;
+    bulkdata.d[0].os_net_buf_ptr = buffered_pkt->bulkdata.os_net_buf_ptr;
+    bulkdata.d[0].net_buf_length = buffered_pkt->bulkdata.net_buf_length;
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].data_length = 0;
+    bulkdata.d[1].os_net_buf_ptr =0;
+    bulkdata.d[1].net_buf_length =0;
+
+    if(moreData) {
+        *fc |= cpu_to_le16(IEEE802_11_FC_MOREDATA_MASK);
+    } else {
+        *fc &= cpu_to_le16(~IEEE802_11_FC_MOREDATA_MASK);
+    }
+
+    if((staRecord != NULL)&& (staRecord->wmmOrQosEnabled == TRUE))
+    {
+        unifi_trace(priv, UDBG3, "frame_and_send_queued_pdu WMM Enabled: %d \n",staRecord->wmmOrQosEnabled);
+
+        toDs = (*fc & cpu_to_le16(IEEE802_11_FC_TO_DS_MASK))?1 : 0;
+        fromDs = (*fc & cpu_to_le16(IEEE802_11_FC_FROM_DS_MASK))? 1: 0;
+
+        switch(le16_to_cpu(*fc) & IEEE80211_FC_SUBTYPE_MASK)
+        {
+            case IEEE802_11_FC_TYPE_QOS_DATA & IEEE80211_FC_SUBTYPE_MASK:
+            case IEEE802_11_FC_TYPE_QOS_NULL & IEEE80211_FC_SUBTYPE_MASK:
+                /* If both are set then the Address4 exists (only for AP) */
+                if (fromDs && toDs) {
+                    /* 6 is the size of Address4 field */
+                    macHeaderLengthInBytes += (QOS_CONTROL_HEADER_SIZE + 6);
+                } else {
+                    macHeaderLengthInBytes += QOS_CONTROL_HEADER_SIZE;
+                }
+
+                /* If order bit set then HT control field is the part of MAC header */
+                if (*fc & cpu_to_le16(IEEE80211_FC_ORDER_MASK)) {
+                    macHeaderLengthInBytes += HT_CONTROL_HEADER_SIZE;
+                    qc = (u8*)(buffered_pkt->bulkdata.os_data_ptr + (macHeaderLengthInBytes-6));
+                } else {
+                    qc = (u8*)(buffered_pkt->bulkdata.os_data_ptr + (macHeaderLengthInBytes-2));
+                }
+                *qc = eosp ? *qc | (1 << 4) : *qc & (~(1 << 4));
+                break;
+            default:
+                if (fromDs && toDs)
+                    macHeaderLengthInBytes += 6;
+        }
+
+    }
+    result = ul_send_signal_unpacked(priv, &signal, &bulkdata);
+    if(result){
+        _update_buffered_pkt_params_after_alignment(priv, &bulkdata,buffered_pkt);
+    }
+
+ /* Decrement the packet counts queued in driver */
+    if (result != -ENOSPC) {
+        /* protect entire counter updation by disabling preemption */
+        if (!priv->noOfPktQueuedInDriver) {
+            unifi_error(priv, "packets queued in driver 0 still decrementing\n");
+        } else {
+            spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+            priv->noOfPktQueuedInDriver--;
+            spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+        }
+        /* Sta Record is available for all unicast (except genericMgt Frames) & in other case its NULL */
+        if (staRecord) {
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            if (!staRecord->noOfPktQueued) {
+                unifi_error(priv, "packets queued in driver per station is 0 still decrementing\n");
+            } else {
+                staRecord->noOfPktQueued--;
+            }
+            /* if the STA alive probe frame has failed then reset the saved host tag */
+            if (result){
+                if (staRecord->nullDataHostTag == buffered_pkt->hostTag){
+                    staRecord->nullDataHostTag = INVALID_HOST_TAG;
+                }
+            }
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        }
+
+    }
+    return result;
+}
+#ifdef CSR_SUPPORT_SME
+static
+void set_eosp_transmit_ctrl(unifi_priv_t *priv, struct list_head *txList)
+{
+    /* dequeue the tx data packets from the appropriate queue */
+    tx_buffered_packets_t *tx_q_item = NULL;
+    struct list_head *listHead;
+    struct list_head *placeHolder;
+    unsigned long lock_flags;
+
+
+    unifi_trace(priv, UDBG5, "entering set_eosp_transmit_ctrl\n");
+    /* check for list empty */
+    if (list_empty(txList)) {
+        unifi_warning(priv, "In set_eosp_transmit_ctrl, the list is empty\n");
+        return;
+    }
+
+    /* return the last node , and modify it. */
+
+    spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+    list_for_each_prev_safe(listHead, placeHolder, txList) {
+        tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+        tx_q_item->transmissionControl |= TRANSMISSION_CONTROL_EOSP_MASK;
+        tx_q_item->transmissionControl = (tx_q_item->transmissionControl & ~(CSR_NO_CONFIRM_REQUIRED));
+        unifi_trace(priv, UDBG1,
+                "set_eosp_transmit_ctrl Transmission Control = 0x%x hostTag = 0x%x \n",tx_q_item->transmissionControl,tx_q_item->hostTag);
+        unifi_trace(priv,UDBG3,"in set_eosp_transmit_ctrl no.of buffered frames %d\n",priv->noOfPktQueuedInDriver);
+        break;
+    }
+    spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+    unifi_trace(priv, UDBG1,"List Empty %d\n",list_empty(txList));
+    unifi_trace(priv, UDBG5, "leaving set_eosp_transmit_ctrl\n");
+    return;
+}
+
+static
+void send_vif_availibility_rsp(unifi_priv_t *priv,CSR_VIF_IDENTIFIER vif,CSR_RESULT_CODE resultCode)
+{
+    CSR_SIGNAL signal;
+    CSR_MA_VIF_AVAILABILITY_RESPONSE *rsp;
+    bulk_data_param_t *bulkdata = NULL;
+    int r;
+
+    unifi_trace(priv, UDBG3, "send_vif_availibility_rsp : invoked with resultCode = %d \n", resultCode);
+
+    memset(&signal,0,sizeof(CSR_SIGNAL));
+    rsp = &signal.u.MaVifAvailabilityResponse;
+    rsp->VirtualInterfaceIdentifier = vif;
+    rsp->ResultCode = resultCode;
+    signal.SignalPrimitiveHeader.SignalId = CSR_MA_VIF_AVAILABILITY_RESPONSE_ID;
+    signal.SignalPrimitiveHeader.ReceiverProcessId = 0;
+    signal.SignalPrimitiveHeader.SenderProcessId = priv->netdev_client->sender_id;
+
+    /* Send the signal to UniFi */
+    r = ul_send_signal_unpacked(priv, &signal, bulkdata);
+    if(r) {
+        unifi_error(priv,"Availibility response sending failed %x status %d\n",vif,r);
+    }
+    else {
+        unifi_trace(priv, UDBG3, "send_vif_availibility_rsp : status = %d \n", r);
+    }
+}
+#endif
+
+static
+void verify_and_accomodate_tx_packet(unifi_priv_t *priv)
+{
+    tx_buffered_packets_t *tx_q_item;
+    unsigned long lock_flags;
+    struct list_head *listHead, *list;
+    struct list_head *placeHolder;
+    u8 i, j,eospFramedeleted=0;
+    u8 thresholdExcedeDueToBroadcast = TRUE;
+    /* it will be made it interface Specific in the future when multi interfaces are supported ,
+    right now interface 0 is considered */
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[0];
+    CsrWifiRouterCtrlStaInfo_t *staInfo = NULL;
+
+    unifi_trace(priv, UDBG3, "entering verify_and_accomodate_tx_packet\n");
+
+    for(i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        staInfo = interfacePriv->staInfo[i];
+            if (staInfo && (staInfo->noOfPktQueued >= CSR_WIFI_DRIVER_MAX_PKT_QUEUING_THRESHOLD_PER_PEER)) {
+            /* remove the first(oldest) packet from the all the access catogory, since data
+             * packets for station record crossed the threshold limit (64 for AP supporting
+             * 8 peers)
+             */
+            unifi_trace(priv,UDBG3,"number of station pkts queued=  %d for sta id = %d\n", staInfo->noOfPktQueued, staInfo->aid);
+            for(j = 0; j < MAX_ACCESS_CATOGORY; j++) {
+                list = &staInfo->dataPdu[j];
+                spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                list_for_each_safe(listHead, placeHolder, list) {
+                    tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+                    list_del(listHead);
+                    thresholdExcedeDueToBroadcast = FALSE;
+                    unifi_net_data_free(priv, &tx_q_item->bulkdata);
+                    kfree(tx_q_item);
+                    tx_q_item = NULL;
+                    if (!priv->noOfPktQueuedInDriver) {
+                        unifi_error(priv, "packets queued in driver 0 still decrementing in %s\n", __FUNCTION__);
+                    } else {
+                        /* protection provided by spinlock */
+                        priv->noOfPktQueuedInDriver--;
+
+                    }
+                    /* Sta Record is available for all unicast (except genericMgt Frames) & in other case its NULL */
+                    if (!staInfo->noOfPktQueued) {
+                        unifi_error(priv, "packets queued in driver per station is 0 still decrementing in %s\n", __FUNCTION__);
+                    } else {
+                        spin_lock(&priv->staRecord_lock);
+                        staInfo->noOfPktQueued--;
+                        spin_unlock(&priv->staRecord_lock);
+                    }
+                    break;
+                }
+                spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+            }
+        }
+    }
+    if (thresholdExcedeDueToBroadcast &&  interfacePriv->noOfbroadcastPktQueued > CSR_WIFI_DRIVER_MINIMUM_BROADCAST_PKT_THRESHOLD ) {
+        /* Remove the packets from genericMulticastOrBroadCastFrames queue
+         * (the max packets in driver is reached due to broadcast/multicast frames)
+         */
+        spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+        list_for_each_safe(listHead, placeHolder, &interfacePriv->genericMulticastOrBroadCastFrames) {
+            tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+            if(eospFramedeleted){
+                tx_q_item->transmissionControl |= TRANSMISSION_CONTROL_EOSP_MASK;
+                tx_q_item->transmissionControl = (tx_q_item->transmissionControl & ~(CSR_NO_CONFIRM_REQUIRED));
+                unifi_trace(priv, UDBG1,"updating eosp for next packet hostTag:= 0x%x ",tx_q_item->hostTag);
+                eospFramedeleted =0;
+                break;
+            }
+
+            if(tx_q_item->transmissionControl & TRANSMISSION_CONTROL_EOSP_MASK ){
+               eospFramedeleted = 1;
+            }
+            unifi_trace(priv,UDBG1, "freeing of multicast packets ToC = 0x%x hostTag = 0x%x \n",tx_q_item->transmissionControl,tx_q_item->hostTag);
+            list_del(listHead);
+            unifi_net_data_free(priv, &tx_q_item->bulkdata);
+            kfree(tx_q_item);
+            priv->noOfPktQueuedInDriver--;
+            spin_lock(&priv->staRecord_lock);
+            interfacePriv->noOfbroadcastPktQueued--;
+            spin_unlock(&priv->staRecord_lock);
+            if(!eospFramedeleted){
+                break;
+            }
+        }
+        spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+    }
+    unifi_trace(priv, UDBG3, "leaving verify_and_accomodate_tx_packet\n");
+}
+
+static
+CsrResult enque_tx_data_pdu(unifi_priv_t *priv, bulk_data_param_t *bulkdata,
+                            struct list_head *list, CSR_SIGNAL *signal,
+                            u8 requeueOnSamePos)
+{
+
+    /* queue the tx data packets on to appropriate queue */
+    CSR_MA_PACKET_REQUEST *req = &signal->u.MaPacketRequest;
+    tx_buffered_packets_t *tx_q_item;
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG5, "entering enque_tx_data_pdu\n");
+    if(!list) {
+       unifi_error(priv,"List is not specified\n");
+       return CSR_RESULT_FAILURE;
+    }
+
+    /* Removes aged packets & adds the incoming packet */
+    if (priv->noOfPktQueuedInDriver >= CSR_WIFI_DRIVER_SUPPORT_FOR_MAX_PKT_QUEUEING) {
+        unifi_trace(priv,UDBG3,"number of pkts queued=  %d \n", priv->noOfPktQueuedInDriver);
+        verify_and_accomodate_tx_packet(priv);
+    }
+
+
+
+    tx_q_item = (tx_buffered_packets_t *)kmalloc(sizeof(tx_buffered_packets_t), GFP_ATOMIC);
+    if (tx_q_item == NULL) {
+        unifi_error(priv,
+                "Failed to allocate %d bytes for tx packet record\n",
+                sizeof(tx_buffered_packets_t));
+        func_exit();
+        return CSR_RESULT_FAILURE;
+    }
+
+    /* disable the preemption */
+    spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+    INIT_LIST_HEAD(&tx_q_item->q);
+    /* fill the tx_q structure members */
+    tx_q_item->bulkdata.os_data_ptr = bulkdata->d[0].os_data_ptr;
+    tx_q_item->bulkdata.data_length = bulkdata->d[0].data_length;
+    tx_q_item->bulkdata.os_net_buf_ptr = bulkdata->d[0].os_net_buf_ptr;
+    tx_q_item->bulkdata.net_buf_length = bulkdata->d[0].net_buf_length;
+    tx_q_item->interfaceTag = req->VirtualInterfaceIdentifier & 0xff;
+    tx_q_item->hostTag = req->HostTag;
+    tx_q_item->leSenderProcessId = signal->SignalPrimitiveHeader.SenderProcessId;
+    tx_q_item->transmissionControl = req->TransmissionControl;
+    tx_q_item->priority = req->Priority;
+    tx_q_item->rate = req->TransmitRate;
+    memcpy(tx_q_item->peerMacAddress.a, req->Ra.x, ETH_ALEN);
+
+
+
+    if (requeueOnSamePos) {
+        list_add(&tx_q_item->q, list);
+    } else {
+        list_add_tail(&tx_q_item->q, list);
+    }
+
+    /* Count of packet queued in driver */
+    priv->noOfPktQueuedInDriver++;
+    spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+    unifi_trace(priv, UDBG5, "leaving enque_tx_data_pdu\n");
+    return CSR_RESULT_SUCCESS;
+}
+
+#ifdef CSR_WIFI_REQUEUE_PACKET_TO_HAL
+CsrResult unifi_reque_ma_packet_request (void *ospriv, u32 host_tag,
+                                         u16 txStatus, bulk_data_desc_t *bulkDataDesc)
+{
+    CsrResult status = CSR_RESULT_SUCCESS;
+    unifi_priv_t *priv = (unifi_priv_t*)ospriv;
+    netInterface_priv_t *interfacePriv;
+    struct list_head *list = NULL;
+    CsrWifiRouterCtrlStaInfo_t *staRecord = NULL;
+    bulk_data_param_t bulkData;
+    CSR_SIGNAL signal;
+    CSR_PRIORITY priority = 0;
+    u16 interfaceTag = 0;
+    unifi_TrafficQueue priority_q;
+    u16 frameControl = 0, frameType = 0;
+    unsigned long lock_flags;
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    /* If the current mode is not AP or P2PGO then just return failure
+     * to clear the hip slot
+     */
+    if(!((interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP) ||
+        (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO))) {
+        return CSR_RESULT_FAILURE;
+    }
+
+    unifi_trace(priv, UDBG6, "unifi_reque_ma_packet_request: host_tag = 0x%x\n", host_tag);
+
+    staRecord = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv,
+                                                                    (((u8 *) bulkDataDesc->os_data_ptr) + 4),
+                                                                    interfaceTag);
+    if (NULL == staRecord) {
+        unifi_trace(priv, UDBG5, "unifi_reque_ma_packet_request: Invalid STA record \n");
+        return CSR_RESULT_FAILURE;
+    }
+
+    /* Update TIM if MA-PACKET.cfm fails with status as Tx-retry-limit or No-BSS and then just return failure
+     * to clear the hip slot associated with the Packet
+     */
+    if (CSR_TX_RETRY_LIMIT == txStatus || CSR_TX_NO_BSS == txStatus) {
+        if (staRecord->timSet == CSR_WIFI_TIM_RESET || staRecord->timSet == CSR_WIFI_TIM_RESETTING)
+        {
+            unifi_trace(priv, UDBG2, "unifi_reque_ma_packet_request: CFM failed with Retry Limit or No BSS-->update TIM\n");
+            if (!staRecord->timRequestPendingFlag) {
+                update_tim(priv, staRecord->aid, 1, interfaceTag, staRecord->assignedHandle);
+            }
+            else {
+                /* Cache the TimSet value so that it will processed immidiatly after
+                 * completing the current setTim Request
+                 */
+                staRecord->updateTimReqQueued = 1;
+                unifi_trace(priv, UDBG6, "unifi_reque_ma_packet_request: One more UpdateTim Request(:%d)Queued for AID %x\n",
+                                         staRecord->updateTimReqQueued, staRecord->aid);
+            }
+        }
+        return CSR_RESULT_FAILURE;
+    }
+    else if ((CSR_TX_LIFETIME == txStatus) ||  (CSR_TX_BLOCK_ACK_TIMEOUT == txStatus) ||
+             (CSR_TX_FAIL_TRANSMISSION_VIF_INTERRUPTED == txStatus) ||
+             (CSR_TX_REJECTED_PEER_STATION_SLEEPING == txStatus)    ||
+             (CSR_TX_REJECTED_DTIM_STARTED == txStatus)) {
+        /* Extract the Frame control and the frame type */
+        frameControl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(bulkDataDesc->os_data_ptr);
+        frameType =  ((frameControl & IEEE80211_FC_TYPE_MASK) >> FRAME_CONTROL_TYPE_FIELD_OFFSET);
+
+        /* Mgmt frames will not be re-queued for Tx
+         * so just return failure to clear the hip slot
+         */
+        if (IEEE802_11_FRAMETYPE_MANAGEMENT == frameType) {
+            return CSR_RESULT_FAILURE;
+        }
+        else if (IEEE802_11_FRAMETYPE_DATA == frameType) {
+            /* QOS NULL and DATA NULL frames will not be re-queued for Tx
+             * so just return failure to clear the hip slot
+             */
+            if ((((frameControl & IEEE80211_FC_SUBTYPE_MASK) >> FRAME_CONTROL_SUBTYPE_FIELD_OFFSET) == QOS_DATA_NULL) ||
+                (((frameControl & IEEE80211_FC_SUBTYPE_MASK) >> FRAME_CONTROL_SUBTYPE_FIELD_OFFSET)== DATA_NULL )) {
+                return CSR_RESULT_FAILURE;
+            }
+        }
+
+        /* Extract the Packet priority */
+        if (TRUE == staRecord->wmmOrQosEnabled) {
+            u16 qosControl = 0;
+            u8  dataFrameType = 0;
+
+            dataFrameType =((frameControl & IEEE80211_FC_SUBTYPE_MASK) >> 4);
+
+            if (dataFrameType == QOS_DATA) {
+                /* QoS control field is offset from frame control by 2 (frame control)
+                 * + 2 (duration/ID) + 2 (sequence control) + 3*ETH_ALEN or 4*ETH_ALEN
+                 */
+                if((frameControl & IEEE802_11_FC_TO_DS_MASK) && (frameControl & IEEE802_11_FC_FROM_DS_MASK)) {
+                    qosControl= CSR_GET_UINT16_FROM_LITTLE_ENDIAN(bulkDataDesc->os_data_ptr + 30);
+                }
+                else {
+                    qosControl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(bulkDataDesc->os_data_ptr + 24);
+                }
+            }
+
+            priority = (CSR_PRIORITY)(qosControl & IEEE802_11_QC_TID_MASK);
+
+            if (priority < CSR_QOS_UP0 || priority > CSR_QOS_UP7) {
+                unifi_trace(priv, UDBG5, "unifi_reque_ma_packet_request: Invalid priority:%x \n", priority);
+                return CSR_RESULT_FAILURE;
+            }
+        }
+        else {
+            priority = CSR_CONTENTION;
+        }
+
+        /* Frame Bulk data to requeue it back to HAL Queues */
+        bulkData.d[0].os_data_ptr    = bulkDataDesc->os_data_ptr;
+        bulkData.d[0].data_length    = bulkDataDesc->data_length;
+        bulkData.d[0].os_net_buf_ptr = bulkDataDesc->os_net_buf_ptr;
+        bulkData.d[0].net_buf_length = bulkDataDesc->net_buf_length;
+
+        bulkData.d[1].os_data_ptr    = NULL;
+        bulkData.d[1].os_net_buf_ptr = NULL;
+        bulkData.d[1].data_length    = bulkData.d[1].net_buf_length = 0;
+
+        /* Initialize signal to zero */
+        memset(&signal, 0, sizeof(CSR_SIGNAL));
+
+        /* Frame MA Packet Req */
+        unifi_frame_ma_packet_req(priv, priority, 0, host_tag,
+                              interfaceTag, CSR_NO_CONFIRM_REQUIRED,
+                              priv->netdev_client->sender_id,
+                              staRecord->peerMacAddress.a, &signal);
+
+        /* Find the Q-Priority */
+        priority_q = unifi_frame_priority_to_queue(priority);
+        list = &staRecord->dataPdu[priority_q];
+
+        /* Place the Packet on to HAL Queue */
+        status = enque_tx_data_pdu(priv, &bulkData, list, &signal, TRUE);
+
+        /* Update the Per-station queued packet counter */
+        if (!status) {
+            spin_lock_irqsave(&priv->staRecord_lock, lock_flags);
+            staRecord->noOfPktQueued++;
+            spin_unlock_irqrestore(&priv->staRecord_lock, lock_flags);
+        }
+    }
+    else {
+        /* Packet will not be re-queued for any of the other MA Packet Tx failure
+         * reasons so just return failure to clear the hip slot
+         */
+        return CSR_RESULT_FAILURE;
+    }
+
+    return status;
+}
+#endif
+
+static void is_all_ac_deliver_enabled_and_moredata(CsrWifiRouterCtrlStaInfo_t *staRecord, u8 *allDeliveryEnabled, u8 *dataAvailable)
+{
+    u8 i;
+    *allDeliveryEnabled = TRUE;
+    for (i = 0 ;i < MAX_ACCESS_CATOGORY; i++) {
+        if (!IS_DELIVERY_ENABLED(staRecord->powersaveMode[i])) {
+            /* One is is not Delivery Enabled */
+            *allDeliveryEnabled = FALSE;
+            break;
+        }
+    }
+    if (*allDeliveryEnabled) {
+        *dataAvailable = (!list_empty(&staRecord->dataPdu[0]) || !list_empty(&staRecord->dataPdu[1])
+                          ||!list_empty(&staRecord->dataPdu[2]) ||!list_empty(&staRecord->dataPdu[3])
+                          ||!list_empty(&staRecord->mgtFrames));
+    }
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_handle_tim_cfm
+ *
+ *
+ *      This function updates tim status in host depending confirm status from firmware
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      cfm             CSR_MLME_SET_TIM_CONFIRM
+ *      receiverProcessId SenderProcessID to fetch handle & timSet status
+ *
+ * ---------------------------------------------------------------------------
+ */
+void uf_handle_tim_cfm(unifi_priv_t *priv, CSR_MLME_SET_TIM_CONFIRM *cfm, u16 receiverProcessId)
+{
+    u8 handle = CSR_WIFI_GET_STATION_HANDLE_FROM_RECEIVER_ID(receiverProcessId);
+    u8 timSetStatus = CSR_WIFI_GET_TIMSET_STATE_FROM_RECEIVER_ID(receiverProcessId);
+    u16 interfaceTag = (cfm->VirtualInterfaceIdentifier & 0xff);
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    CsrWifiRouterCtrlStaInfo_t *staRecord = NULL;
+    /* This variable holds what TIM value we wanted to set in firmware */
+    u16 timSetValue = 0;
+    /* Irrespective of interface the count maintained */
+    static u8 retryCount = 0;
+    unsigned long lock_flags;
+    unifi_trace(priv, UDBG3, "entering %s, handle = %x, timSetStatus = %x\n", __FUNCTION__, handle, timSetStatus);
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_warning(priv, "bad interfaceTag = %x\n", interfaceTag);
+        return;
+    }
+
+    if ((handle != CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE) && (handle >= UNIFI_MAX_CONNECTIONS)) {
+        unifi_warning(priv, "bad station Handle = %x\n", handle);
+        return;
+    }
+
+    if (handle != CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE) {
+        spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+        if ((staRecord = ((CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[handle]))) == NULL) {
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+            unifi_warning(priv, "uf_handle_tim_cfm: station record is NULL  handle = %x\n", handle);
+            return;
+        }
+       spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+    }
+    switch(timSetStatus)
+    {
+        case CSR_WIFI_TIM_SETTING:
+            timSetValue = CSR_WIFI_TIM_SET;
+            break;
+        case CSR_WIFI_TIM_RESETTING:
+            timSetValue = CSR_WIFI_TIM_RESET;
+            break;
+        default:
+            unifi_warning(priv, "timSet state is %x: Debug\n", timSetStatus);
+            return;
+    }
+
+    /* check TIM confirm for success/failures */
+    switch(cfm->ResultCode)
+    {
+        case CSR_RC_SUCCESS:
+            if (handle != CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE) {
+                /* Unicast frame & station record available */
+                if (timSetStatus == staRecord->timSet) {
+                    staRecord->timSet = timSetValue;
+                    /* fh_cmd_q can also be full at some point of time!,
+                     * resetting count as queue is cleaned by firmware at this point
+                     */
+                    retryCount = 0;
+                    unifi_trace(priv, UDBG2, "tim (%s) successfully in firmware\n", (timSetValue)?"SET":"RESET");
+                } else {
+                    unifi_trace(priv, UDBG3, "receiver processID = %x, success: request & confirm states are not matching in TIM cfm: Debug status = %x, staRecord->timSet = %x, handle = %x\n",
+                                 receiverProcessId, timSetStatus, staRecord->timSet, handle);
+                }
+
+                /* Reset TIM pending flag to send next TIM request */
+                staRecord->timRequestPendingFlag = FALSE;
+
+                /* Make sure that one more UpdateTim request is queued, if Queued its value
+                 * should be CSR_WIFI_TIM_SET or CSR_WIFI_TIM_RESET
+                 */
+                if (0xFF != staRecord->updateTimReqQueued)
+                {
+                    /* Process the UpdateTim Request which is queued while previous UpdateTim was in progress */
+                    if (staRecord->timSet != staRecord->updateTimReqQueued)
+                    {
+                       unifi_trace(priv, UDBG2, "uf_handle_tim_cfm : Processing Queued UpdateTimReq \n");
+
+                       update_tim(priv, staRecord->aid, staRecord->updateTimReqQueued, interfaceTag, handle);
+
+                       staRecord->updateTimReqQueued = 0xFF;
+                    }
+                }
+            } else {
+
+                interfacePriv->bcTimSet = timSetValue;
+                /* fh_cmd_q can also be full at some point of time!,
+                 * resetting count as queue is cleaned by firmware at this point
+                 */
+                retryCount = 0;
+                unifi_trace(priv, UDBG3, "tim (%s) successfully for broadcast frame in firmware\n", (timSetValue)?"SET":"RESET");
+
+                /* Reset DTIM pending flag to send next DTIM request */
+                interfacePriv->bcTimSetReqPendingFlag = FALSE;
+
+                /* Make sure that one more UpdateDTim request is queued, if Queued its value
+                 * should be CSR_WIFI_TIM_SET or CSR_WIFI_TIM_RESET
+                 */
+                if (0xFF != interfacePriv->bcTimSetReqQueued)
+                {
+                    /* Process the UpdateTim Request which is queued while previous UpdateTim was in progress */
+                    if (interfacePriv->bcTimSet != interfacePriv->bcTimSetReqQueued)
+                    {
+                        unifi_trace(priv, UDBG2, "uf_handle_tim_cfm : Processing Queued UpdateDTimReq \n");
+
+                        update_tim(priv, 0, interfacePriv->bcTimSetReqQueued, interfaceTag, 0xFFFFFFFF);
+
+                        interfacePriv->bcTimSetReqQueued = 0xFF;
+                    }
+                }
+
+            }
+            break;
+        case CSR_RC_INVALID_PARAMETERS:
+        case CSR_RC_INSUFFICIENT_RESOURCE:
+            /* check for max retry limit & send again
+             * MAX_RETRY_LIMIT is not maintained for each set of transactions..Its generic
+             * If failure crosses this Limit, we have to take a call to FIX
+             */
+            if (retryCount > UNIFI_MAX_RETRY_LIMIT) {
+                u8 moreData = FALSE;
+                retryCount = 0;
+                /* Because of continuos traffic in fh_cmd_q the tim set request is failing (exceeding retry limit)
+                 * but if we didn't synchronize our timSet varible state with firmware then it can cause below issues
+                 * cond 1. We want to SET tim in firmware if its fails & max retry limit reached
+                 *   -> If host set's the timSet to 1, we wont try to send(as max retry reached) update tim but
+                 *   firmware is not updated with queue(TIM) status so it wont set TIM in beacon finally host start piling
+                 *    up data & wont try to set tim in firmware (This can cause worser performance)
+                 * cond 2. We want to reset tim in firmware it fails & reaches max retry limit
+                 *   -> If host sets the timSet to Zero, it wont try to set a TIM request unless we wont have any packets
+                 *   to be queued, so beacon unnecessarily advertizes the TIM
+                 */
+
+                if(staRecord) {
+                    if(!staRecord->wmmOrQosEnabled) {
+                        moreData = (!list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]) ||
+                                !list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_VO]) ||
+                                !list_empty(&staRecord->mgtFrames));
+                    } else {
+                        /* Peer is QSTA */
+                        u8 allDeliveryEnabled = 0, dataAvailable = 0;
+                        /* Check if all AC's are Delivery Enabled */
+                        is_all_ac_deliver_enabled_and_moredata(staRecord, &allDeliveryEnabled, &dataAvailable);
+                        /*check for more data in non-delivery enabled queues*/
+                        moreData = (uf_is_more_data_for_non_delivery_ac(staRecord) || (allDeliveryEnabled && dataAvailable));
+
+                    }
+                    /* To avoid cond 1 & 2, check internal Queues status, if we have more Data then set RESET the timSet(0),
+                     *  so we are trying to be in sync with firmware & next packets before queuing atleast try to
+                     *  set TIM in firmware otherwise it SET timSet(1)
+                     */
+                    if (moreData) {
+                        staRecord->timSet = CSR_WIFI_TIM_RESET;
+                    } else {
+                        staRecord->timSet = CSR_WIFI_TIM_SET;
+                    }
+                } else {
+                    /* Its a broadcast frames */
+                    moreData = (!list_empty(&interfacePriv->genericMulticastOrBroadCastMgtFrames) ||
+                               !list_empty(&interfacePriv->genericMulticastOrBroadCastFrames));
+                    if (moreData) {
+                        update_tim(priv, 0, CSR_WIFI_TIM_SET, interfaceTag, 0xFFFFFFFF);
+                    } else {
+                        update_tim(priv, 0, CSR_WIFI_TIM_RESET, interfaceTag, 0xFFFFFFFF);
+                    }
+                }
+
+                unifi_error(priv, "no of error's for TIM setting crossed the Limit: verify\n");
+                return;
+            }
+            retryCount++;
+
+            if (handle != CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE) {
+                if (timSetStatus == staRecord->timSet) {
+                    unifi_warning(priv, "tim request failed, retry for AID = %x\n", staRecord->aid);
+                    update_tim(priv, staRecord->aid, timSetValue, interfaceTag, handle);
+                } else {
+                    unifi_trace(priv, UDBG1, "failure: request & confirm states are not matching in TIM cfm: Debug status = %x, staRecord->timSet = %x\n",
+                                  timSetStatus, staRecord->timSet);
+                }
+            } else {
+                unifi_warning(priv, "tim request failed, retry for broadcast frames\n");
+                update_tim(priv, 0, timSetValue, interfaceTag, 0xFFFFFFFF);
+            }
+            break;
+        default:
+            unifi_warning(priv, "tim update request failed resultcode = %x\n", cfm->ResultCode);
+    }
+
+    unifi_trace(priv, UDBG2, "leaving %s\n", __FUNCTION__);
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  update_tim
+ *
+ *
+ *      This function updates tim status in firmware for AID[1 to UNIFI_MAX_CONNECTIONS] or
+ *       AID[0] for broadcast/multicast packets.
+ *
+ *      NOTE: The LSB (least significant BYTE) of senderId while sending this MLME premitive
+ *       has been modified(utilized) as below
+ *
+ *       SenderID in signal's SignalPrimitiveHeader is 2 byte the lowe byte bitmap is below
+ *
+ *       station handle(6 bits)      timSet Status (2 bits)
+ *       ---------------------       ----------------------
+ *       0  0  0  0  0  0        |       0  0
+ *
+ * timSet Status can be one of below:
+ *
+ * CSR_WIFI_TIM_RESET
+ * CSR_WIFI_TIM_RESETTING
+ * CSR_WIFI_TIM_SET
+ * CSR_WIFI_TIM_SETTING
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      aid             can be 1 t0 UNIFI_MAX_CONNECTIONS & 0 means multicast/broadcast
+ *      setTim          value SET(1) / RESET(0)
+ *      interfaceTag    the interfaceID on which activity going on
+ *      handle          from  (0 <= handle < UNIFI_MAX_CONNECTIONS)
+ *
+ * ---------------------------------------------------------------------------
+ */
+void update_tim(unifi_priv_t * priv, u16 aid, u8 setTim, u16 interfaceTag, u32 handle)
+{
+    CSR_SIGNAL signal;
+    s32 r;
+    CSR_MLME_SET_TIM_REQUEST *req = &signal.u.MlmeSetTimRequest;
+    bulk_data_param_t *bulkdata = NULL;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    u8 senderIdLsb = 0;
+    CsrWifiRouterCtrlStaInfo_t *staRecord = NULL;
+    u32 oldTimSetStatus = 0, timSetStatus = 0;
+
+    unifi_trace(priv, UDBG5, "entering the update_tim routine\n");
+
+
+    if (handle == 0xFFFFFFFF) {
+        handle &= CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE;
+        if (setTim == interfacePriv->bcTimSet)
+        {
+            unifi_trace(priv, UDBG3, "update_tim, Drop:Hdl=%x, timval=%d, globalTim=%d\n", handle, setTim, interfacePriv->bcTimSet);
+            return;
+        }
+    } else if ((handle != 0xFFFFFFFF) && (handle >= UNIFI_MAX_CONNECTIONS)) {
+        unifi_warning(priv, "bad station Handle = %x\n", handle);
+        return;
+    }
+
+    if (setTim) {
+        timSetStatus =  CSR_WIFI_TIM_SETTING;
+    } else {
+        timSetStatus =  CSR_WIFI_TIM_RESETTING;
+    }
+
+    if (handle != CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE) {
+        if ((staRecord = ((CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[handle]))) == NULL) {
+            unifi_warning(priv, "station record is NULL in  update_tim: handle = %x :debug\n", handle);
+            return;
+        }
+        /* In case of signal sending failed, revert back to old state */
+        oldTimSetStatus = staRecord->timSet;
+        staRecord->timSet = timSetStatus;
+    }
+
+    /* pack senderID LSB */
+    senderIdLsb = CSR_WIFI_PACK_SENDER_ID_LSB_FOR_TIM_REQ(handle,  timSetStatus);
+
+    /* initialize signal to zero */
+    memset(&signal, 0, sizeof(CSR_SIGNAL));
+
+    /* Frame the MLME-SET-TIM request */
+    signal.SignalPrimitiveHeader.SignalId = CSR_MLME_SET_TIM_REQUEST_ID;
+    signal.SignalPrimitiveHeader.ReceiverProcessId = 0;
+    CSR_COPY_UINT16_TO_LITTLE_ENDIAN(((priv->netdev_client->sender_id & 0xff00) | senderIdLsb),
+                   (u8*)&signal.SignalPrimitiveHeader.SenderProcessId);
+
+    /* set The virtual interfaceIdentifier, aid, tim value */
+    req->VirtualInterfaceIdentifier = uf_get_vif_identifier(interfacePriv->interfaceMode,interfaceTag);
+    req->AssociationId = aid;
+    req->TimValue = setTim;
+
+
+    unifi_trace(priv, UDBG2, "update_tim:AID %x,senderIdLsb = 0x%x, handle = 0x%x, timSetStatus = %x, sender proceesID = %x \n",
+                aid,senderIdLsb, handle, timSetStatus, signal.SignalPrimitiveHeader.SenderProcessId);
+
+    /* Send the signal to UniFi */
+    r = ul_send_signal_unpacked(priv, &signal, bulkdata);
+    if (r) {
+        /* No need to free bulk data, as TIM request doesn't carries any data */
+        unifi_error(priv, "Error queueing CSR_MLME_SET_TIM_REQUEST signal\n");
+        if (staRecord) {
+            staRecord->timSet = oldTimSetStatus ;
+        }
+        else
+        {
+            /* MLME_SET_TIM.req sending failed here for AID0, so revert back our bcTimSet status */
+            interfacePriv->bcTimSet = !setTim;
+        }
+    }
+    else {
+        /* Update tim request pending flag and ensure no more TIM set requests are send
+           for the same station until TIM confirm is received */
+        if (staRecord) {
+            staRecord->timRequestPendingFlag = TRUE;
+        }
+        else
+        {
+            /* Update tim request (for AID 0) pending flag and ensure no more DTIM set requests are send
+             * for the same station until TIM confirm is received
+             */
+            interfacePriv->bcTimSetReqPendingFlag = TRUE;
+        }
+    }
+    unifi_trace(priv, UDBG5, "leaving the update_tim routine\n");
+}
+
+static
+void process_peer_active_transition(unifi_priv_t * priv,
+                                    CsrWifiRouterCtrlStaInfo_t *staRecord,
+                                    u16 interfaceTag)
+{
+    int r,i;
+    u8 spaceAvail[4] = {TRUE,TRUE,TRUE,TRUE};
+    tx_buffered_packets_t * buffered_pkt = NULL;
+    unsigned long lock_flags;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    unifi_trace(priv, UDBG5, "entering process_peer_active_transition\n");
+
+    if(IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag)) {
+        /* giving more priority to multicast packets so delaying unicast packets*/
+        unifi_trace(priv,UDBG2, "Multicast transmission is going on so resume unicast transmission after DTIM over\n");
+
+        /* As station is active now, even though AP is not able to send frames to it
+         * because of DTIM, it needs to reset the TIM here
+         */
+        if (!staRecord->timRequestPendingFlag){
+            if((staRecord->timSet == CSR_WIFI_TIM_SET) || (staRecord->timSet == CSR_WIFI_TIM_SETTING)){
+                update_tim(priv, staRecord->aid, 0, interfaceTag, staRecord->assignedHandle);
+            }
+        }
+        else
+        {
+            /* Cache the TimSet value so that it will processed immidiatly after
+             * completing the current setTim Request
+             */
+            staRecord->updateTimReqQueued = 0;
+            unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                        staRecord->aid);
+        }
+        return;
+    }
+    while((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->mgtFrames))) {
+        buffered_pkt->transmissionControl &=
+                     ~(TRANSMISSION_CONTROL_TRIGGER_MASK|TRANSMISSION_CONTROL_EOSP_MASK);
+        if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,0,FALSE)) == -ENOSPC) {
+            unifi_trace(priv, UDBG2, "p_p_a_t:(ENOSPC) Mgt Frame queueing \n");
+            /* Enqueue at the head of the queue */
+            spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+            list_add(&buffered_pkt->q, &staRecord->mgtFrames);
+            spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+            priv->pausedStaHandle[3]=(u8)(staRecord->assignedHandle);
+            spaceAvail[3] = FALSE;
+            break;
+        } else {
+            if(r){
+                unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                /* the PDU failed where we can't do any thing so free the storage */
+                unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+            }
+            kfree(buffered_pkt);
+        }
+    }
+    if (!staRecord->timRequestPendingFlag) {
+        if (staRecord->txSuspend) {
+            if(staRecord->timSet == CSR_WIFI_TIM_SET) {
+                update_tim(priv,staRecord->aid,0,interfaceTag, staRecord->assignedHandle);
+            }
+            return;
+        }
+    }
+    else
+    {
+        /* Cache the TimSet value so that it will processed immidiatly after
+         * completing the current setTim Request
+         */
+        staRecord->updateTimReqQueued = 0;
+        unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                    staRecord->aid);
+    }
+    for(i=3;i>=0;i--) {
+        if(!spaceAvail[i])
+            continue;
+        unifi_trace(priv, UDBG6, "p_p_a_t:data pkt sending for AC %d \n",i);
+        while((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->dataPdu[i]))) {
+           buffered_pkt->transmissionControl &=
+                      ~(TRANSMISSION_CONTROL_TRIGGER_MASK|TRANSMISSION_CONTROL_EOSP_MASK);
+           if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,0,FALSE)) == -ENOSPC) {
+               /* Clear the trigger bit transmission control*/
+               /* Enqueue at the head of the queue */
+               spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+               list_add(&buffered_pkt->q, &staRecord->dataPdu[i]);
+               spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+               priv->pausedStaHandle[i]=(u8)(staRecord->assignedHandle);
+               break;
+           } else {
+              if(r){
+                  unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                  /* the PDU failed where we can't do any thing so free the storage */
+                  unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+               }
+              kfree(buffered_pkt);
+           }
+        }
+    }
+    if (!staRecord->timRequestPendingFlag){
+        if((staRecord->timSet  == CSR_WIFI_TIM_SET) || (staRecord->timSet  == CSR_WIFI_TIM_SETTING)) {
+            unifi_trace(priv, UDBG3, "p_p_a_t:resetting tim .....\n");
+            update_tim(priv,staRecord->aid,0,interfaceTag, staRecord->assignedHandle);
+        }
+    }
+    else
+    {
+        /* Cache the TimSet value so that it will processed immidiatly after
+         * completing the current setTim Request
+         */
+        staRecord->updateTimReqQueued = 0;
+        unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                    staRecord->aid);
+    }
+    unifi_trace(priv, UDBG5, "leaving process_peer_active_transition\n");
+}
+
+
+
+void uf_process_ma_pkt_cfm_for_ap(unifi_priv_t *priv,u16 interfaceTag, const CSR_MA_PACKET_CONFIRM *pkt_cfm)
+{
+    netInterface_priv_t *interfacePriv;
+    u8 i;
+    CsrWifiRouterCtrlStaInfo_t *staRecord = NULL;
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+
+    if(pkt_cfm->HostTag == interfacePriv->multicastPduHostTag) {
+         unifi_trace(priv,UDBG2,"CFM for marked Multicast Tag = %x\n",interfacePriv->multicastPduHostTag);
+         interfacePriv->multicastPduHostTag = 0xffffffff;
+         resume_suspended_uapsd(priv,interfaceTag);
+         resume_unicast_buffered_frames(priv,interfaceTag);
+         if(list_empty(&interfacePriv->genericMulticastOrBroadCastMgtFrames) &&
+              list_empty(&interfacePriv->genericMulticastOrBroadCastFrames)) {
+            unifi_trace(priv,UDBG1,"Resetting multicastTIM");
+            if (!interfacePriv->bcTimSetReqPendingFlag)
+            {
+                update_tim(priv,0,CSR_WIFI_TIM_RESET,interfaceTag, 0xFFFFFFFF);
+            }
+            else
+            {
+                /* Cache the DTimSet value so that it will processed immidiatly after
+                 * completing the current setDTim Request
+                 */
+                 interfacePriv->bcTimSetReqQueued = CSR_WIFI_TIM_RESET;
+                 unifi_trace(priv, UDBG2, "uf_process_ma_pkt_cfm_for_ap : One more UpdateDTim Request(%d) Queued \n",
+                             interfacePriv->bcTimSetReqQueued);
+            }
+
+        }
+        return;
+    }
+
+    /* Check if it is a Confirm for null data frame used
+     * for probing station activity
+     */
+    for(i =0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        staRecord = (CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[i]);
+        if (staRecord && (staRecord->nullDataHostTag == pkt_cfm->HostTag)) {
+
+            unifi_trace(priv, UDBG1, "CFM for Inactive probe Null frame (tag = %x, status = %d)\n",
+                                    pkt_cfm->HostTag,
+                                    pkt_cfm->TransmissionStatus
+                                    );
+            staRecord->nullDataHostTag = INVALID_HOST_TAG;
+
+            if(pkt_cfm->TransmissionStatus == CSR_TX_RETRY_LIMIT){
+                CsrTime now;
+                CsrTime inactive_time;
+
+                unifi_trace(priv, UDBG1, "Nulldata to probe STA ALIVE Failed with retry limit\n");
+                /* Recheck if there is some activity after null data is sent.
+                *
+                * If still there is no activity then send a disconnected indication
+                * to SME to delete the station record.
+                */
+                if (staRecord->activity_flag){
+                    return;
+                }
+                now = CsrTimeGet(NULL);
+
+                if (staRecord->lastActivity > now)
+                {
+                    /* simple timer wrap (for 1 wrap) */
+                    inactive_time = CsrTimeAdd((CsrTime)CsrTimeSub(CSR_SCHED_TIME_MAX, staRecord->lastActivity),
+                                               now);
+                }
+                else
+                {
+                    inactive_time = (CsrTime)CsrTimeSub(now, staRecord->lastActivity);
+                }
+
+                if (inactive_time >= STA_INACTIVE_TIMEOUT_VAL)
+                {
+                    struct list_head send_cfm_list;
+                    u8 j;
+
+                    /* The SME/NME may be waiting for confirmation for requested frames to this station.
+                     * Though this is --VERY UNLIKELY-- in case of station in active mode. But still as a
+                     * a defensive check, it loops through buffered frames for this station and if confirmation
+                     * is requested, send auto confirmation with failure status. Also flush the frames so
+                     * that these are not processed again in PEER_DEL_REQ handler.
+                     */
+                    INIT_LIST_HEAD(&send_cfm_list);
+
+                    uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                             &send_cfm_list,
+                                                             &(staRecord->mgtFrames));
+
+                    uf_flush_list(priv, &(staRecord->mgtFrames));
+
+                    for(j = 0; j < MAX_ACCESS_CATOGORY; j++){
+                        uf_prepare_send_cfm_list_for_queued_pkts(priv,
+                                                                 &send_cfm_list,
+                                                                 &(staRecord->dataPdu[j]));
+
+                        uf_flush_list(priv,&(staRecord->dataPdu[j]));
+                    }
+
+                    send_auto_ma_packet_confirm(priv, staRecord->interfacePriv, &send_cfm_list);
+
+
+
+                    unifi_warning(priv, "uf_process_ma_pkt_cfm_for_ap: Router Disconnected IND Peer (%x-%x-%x-%x-%x-%x)\n",
+                                             staRecord->peerMacAddress.a[0],
+                                             staRecord->peerMacAddress.a[1],
+                                             staRecord->peerMacAddress.a[2],
+                                             staRecord->peerMacAddress.a[3],
+                                             staRecord->peerMacAddress.a[4],
+                                             staRecord->peerMacAddress.a[5]);
+
+                    CsrWifiRouterCtrlConnectedIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,
+                                                      0,
+                                                      staRecord->interfacePriv->InterfaceTag,
+                                                      staRecord->peerMacAddress,
+                                                      CSR_WIFI_ROUTER_CTRL_PEER_DISCONNECTED);
+                }
+
+            }
+            else if (pkt_cfm->TransmissionStatus == CSR_TX_SUCCESSFUL)
+            {
+                 staRecord->activity_flag = TRUE;
+            }
+        }
+    }
+}
+
+#endif
+u16 uf_get_vif_identifier (CsrWifiRouterCtrlMode mode, u16 tag)
+{
+    switch(mode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+            return (0x02<<8|tag);
+
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            return (0x03<<8|tag);
+
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            return (0x01<<8|tag);
+
+        case CSR_WIFI_ROUTER_CTRL_MODE_MONITOR:
+            return (0x04<<8|tag);
+        case CSR_WIFI_ROUTER_CTRL_MODE_AMP:
+            return (0x05<<8|tag);
+        default:
+            return tag;
+    }
+}
+
+#ifdef CSR_SUPPORT_SME
+
+/*
+ * ---------------------------------------------------------------------------
+ *  update_macheader
+ *
+ *
+ *      These functions updates mac header for intra BSS packet
+ *      routing.
+ *      NOTE: This function always has to be called in rx context which
+ *      is in bh thread context since GFP_KERNEL is used. In soft IRQ/ Interrupt
+ *      context shouldn't be used
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      skb             Socket buffer containing data packet to transmit
+ *      newSkb          Socket buffer containing data packet + Mac header if no sufficient headroom in skb
+ *      priority        to append QOS control header in Mac header
+ *      bulkdata        if newSkb allocated then bulkdata updated to send to unifi
+ *      interfaceTag    the interfaceID on which activity going on
+ *      macHeaderLengthInBytes no. of bytes of mac header in received frame
+ *      qosDestination  used to append Qos control field
+ *
+ *  Returns:
+ *      Zero on success or -1 on error.
+ * ---------------------------------------------------------------------------
+ */
+
+static int update_macheader(unifi_priv_t *priv, struct sk_buff *skb,
+                            struct sk_buff *newSkb, CSR_PRIORITY *priority,
+                            bulk_data_param_t *bulkdata, u16 interfaceTag,
+                            u8 macHeaderLengthInBytes,
+                            u8 qosDestination)
+{
+
+    u16 *fc = NULL;
+    u8 direction = 0, toDs, fromDs;
+    u8 *bufPtr = NULL;
+    u8 sa[ETH_ALEN], da[ETH_ALEN];
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    int headroom;
+    u8 macHeaderBuf[IEEE802_11_DATA_FRAME_MAC_HEADER_SIZE] = {0};
+
+    unifi_trace(priv, UDBG5, "entering the update_macheader function\n");
+
+    /* temporary buffer for the Mac header storage */
+    memcpy(macHeaderBuf, skb->data, macHeaderLengthInBytes);
+
+    /* remove the Macheader from the skb */
+    skb_pull(skb, macHeaderLengthInBytes);
+
+    /* get the skb headroom for skb_push check */
+    headroom = skb_headroom(skb);
+
+    /*  pointer to frame control field */
+    fc = (u16*) macHeaderBuf;
+
+    toDs = (*fc & cpu_to_le16(IEEE802_11_FC_TO_DS_MASK))?1 : 0;
+    fromDs = (*fc & cpu_to_le16(IEEE802_11_FC_FROM_DS_MASK))? 1: 0;
+    unifi_trace(priv, UDBG5, "In update_macheader function, fromDs = %x, toDs = %x\n", fromDs, toDs);
+    direction = ((fromDs | (toDs << 1)) & 0x3);
+
+    /* Address1 or 3 from the macheader */
+    memcpy(da, macHeaderBuf+4+toDs*12, ETH_ALEN);
+    /* Address2, 3 or 4 from the mac header */
+    memcpy(sa, macHeaderBuf+10+fromDs*(6+toDs*8), ETH_ALEN);
+
+    unifi_trace(priv, UDBG3, "update_macheader:direction = %x\n", direction);
+    /* update the toDs, fromDs & address fields in Mac header */
+    switch(direction)
+    {
+        case 2:
+            /* toDs = 1 & fromDs = 0 , toAp when frames received from peer
+             * while sending this packet to Destination the Mac header changed
+             * as fromDs = 1 & toDs = 0, fromAp
+             */
+            *fc &= cpu_to_le16(~IEEE802_11_FC_TO_DS_MASK);
+            *fc |= cpu_to_le16(IEEE802_11_FC_FROM_DS_MASK);
+            /* Address1: MAC address of the actual destination (4 = 2+2) */
+            memcpy(macHeaderBuf + 4, da, ETH_ALEN);
+            /* Address2: The MAC address of the AP (10 = 2+2+6) */
+            memcpy(macHeaderBuf + 10, &interfacePriv->bssid, ETH_ALEN);
+            /* Address3: MAC address of the actual source from mac header (16 = 2+2+6+6) */
+            memcpy(macHeaderBuf + 16, sa, ETH_ALEN);
+            break;
+        case 3:
+            unifi_trace(priv, UDBG3, "when both the toDs & fromDS set, NOT SUPPORTED\n");
+            break;
+        default:
+            unifi_trace(priv, UDBG3, "problem in decoding packet in update_macheader \n");
+            return -1;
+    }
+
+    /* frameType is Data always, Validation is done before calling this function */
+
+    /* check for the souce station type */
+    switch(le16_to_cpu(*fc) & IEEE80211_FC_SUBTYPE_MASK)
+    {
+        case IEEE802_11_FC_TYPE_QOS_DATA & IEEE80211_FC_SUBTYPE_MASK:
+            /* No need to modify the qos control field */
+            if (!qosDestination) {
+
+                /* If source Sta is QOS enabled & if this bit set, then HTC is supported by
+                 * peer station & htc field present in macHeader
+                 */
+                if (*fc & cpu_to_le16(IEEE80211_FC_ORDER_MASK)) {
+                    /* HT control field present in Mac header
+                     * 6 = sizeof(qosControl) + sizeof(htc)
+                     */
+                    macHeaderLengthInBytes -= 6;
+                } else {
+                    macHeaderLengthInBytes -= 2;
+                }
+                /* Destination STA is non qos so change subtype to DATA */
+                *fc &= cpu_to_le16(~IEEE80211_FC_SUBTYPE_MASK);
+                *fc |= cpu_to_le16(IEEE802_11_FC_TYPE_DATA);
+                /* remove the qos control field & HTC(if present). new macHeaderLengthInBytes is less than old
+                 * macHeaderLengthInBytes so no need to verify skb headroom
+                 */
+                if (headroom < macHeaderLengthInBytes) {
+                    unifi_trace(priv, UDBG1, " sufficient headroom not there to push updated mac header \n");
+                    return -1;
+                }
+                bufPtr = (u8 *) skb_push(skb, macHeaderLengthInBytes);
+
+                /*  update bulk data os_data_ptr */
+                bulkdata->d[0].os_data_ptr = skb->data;
+                bulkdata->d[0].os_net_buf_ptr = (unsigned char*)skb;
+                bulkdata->d[0].data_length = skb->len;
+
+            } else {
+                /* pointing to QOS control field */
+                u8 qc;
+                if (*fc & cpu_to_le16(IEEE80211_FC_ORDER_MASK)) {
+                    qc = *((u8*)(macHeaderBuf + (macHeaderLengthInBytes - 4 - 2)));
+                } else {
+                    qc = *((u8*)(macHeaderBuf + (macHeaderLengthInBytes - 2)));
+                }
+
+                if ((qc & IEEE802_11_QC_TID_MASK) > 7) {
+                    *priority = 7;
+                } else {
+                    *priority = qc & IEEE802_11_QC_TID_MASK;
+                }
+
+                unifi_trace(priv, UDBG1, "Incoming packet priority from QSTA is %x\n", *priority);
+
+                if (headroom < macHeaderLengthInBytes) {
+                    unifi_trace(priv, UDBG3, " sufficient headroom not there to push updated mac header \n");
+                    return -1;
+                }
+                bufPtr = (u8 *) skb_push(skb, macHeaderLengthInBytes);
+            }
+            break;
+        default:
+            {
+                bulk_data_param_t data_ptrs;
+                CsrResult csrResult;
+                unifi_trace(priv, UDBG5, "normal Data packet, NO QOS \n");
+
+                if (qosDestination) {
+                    u8 qc = 0;
+                    unifi_trace(priv, UDBG3, "destination is QOS station \n");
+
+                    /* Set Ma-Packet.req UP to UP0 */
+                    *priority = CSR_QOS_UP0;
+
+                    /* prepare the qos control field */
+                    qc |= CSR_QOS_UP0;
+                    /* no Amsdu is in ap buffer so eosp is left 0 */
+                    if (da[0] & 0x1) {
+                        /* multicast/broadcast frames, no acknowledgement needed */
+                        qc |= 1 << 5;
+                    }
+
+                    /* update new Mac header Length with 2 = sizeof(qos control) */
+                    macHeaderLengthInBytes += 2;
+
+                    /* received DATA frame but destiantion is QOS station so update subtype to QOS*/
+                    *fc &= cpu_to_le16(~IEEE80211_FC_SUBTYPE_MASK);
+                    *fc |= cpu_to_le16(IEEE802_11_FC_TYPE_QOS_DATA);
+
+                    /* appendQosControlOffset = macHeaderLengthInBytes - 2, since source sta is not QOS */
+                    macHeaderBuf[macHeaderLengthInBytes - 2] = qc;
+                    /* txopLimit is 0 */
+                    macHeaderBuf[macHeaderLengthInBytes - 1] = 0;
+                    if (headroom < macHeaderLengthInBytes) {
+                        csrResult = unifi_net_data_malloc(priv, &data_ptrs.d[0], skb->len + macHeaderLengthInBytes);
+
+                        if (csrResult != CSR_RESULT_SUCCESS) {
+                            unifi_error(priv, " failed to allocate request_data. in update_macheader func\n");
+                            return -1;
+                        }
+                        newSkb = (struct sk_buff *)(data_ptrs.d[0].os_net_buf_ptr);
+                        newSkb->len = skb->len + macHeaderLengthInBytes;
+
+                        memcpy((void*)data_ptrs.d[0].os_data_ptr + macHeaderLengthInBytes,
+                                skb->data, skb->len);
+
+                        bulkdata->d[0].os_data_ptr = newSkb->data;
+                        bulkdata->d[0].os_net_buf_ptr = (unsigned char*)newSkb;
+                        bulkdata->d[0].data_length = newSkb->len;
+
+                        bufPtr = (u8*)data_ptrs.d[0].os_data_ptr;
+
+                        /* The old skb will not be used again */
+                        kfree_skb(skb);
+                    } else {
+                        /* skb headroom is sufficient to append Macheader */
+                        bufPtr = (u8*)skb_push(skb, macHeaderLengthInBytes);
+                        bulkdata->d[0].os_data_ptr = skb->data;
+                        bulkdata->d[0].os_net_buf_ptr = (unsigned char*)skb;
+                        bulkdata->d[0].data_length = skb->len;
+                    }
+                } else {
+                    unifi_trace(priv, UDBG3, "destination is not a QSTA\n");
+                    if (headroom < macHeaderLengthInBytes) {
+                        csrResult = unifi_net_data_malloc(priv, &data_ptrs.d[0], skb->len + macHeaderLengthInBytes);
+
+                        if (csrResult != CSR_RESULT_SUCCESS) {
+                            unifi_error(priv, " failed to allocate request_data. in update_macheader func\n");
+                            return -1;
+                        }
+                        newSkb = (struct sk_buff *)(data_ptrs.d[0].os_net_buf_ptr);
+                        newSkb->len = skb->len + macHeaderLengthInBytes;
+
+                        memcpy((void*)data_ptrs.d[0].os_data_ptr + macHeaderLengthInBytes,
+                                skb->data, skb->len);
+
+                        bulkdata->d[0].os_data_ptr = newSkb->data;
+                        bulkdata->d[0].os_net_buf_ptr = (unsigned char*)newSkb;
+                        bulkdata->d[0].data_length = newSkb->len;
+
+                        bufPtr = (u8*)data_ptrs.d[0].os_data_ptr;
+
+                        /* The old skb will not be used again */
+                        kfree_skb(skb);
+                    } else {
+                        /* skb headroom is sufficient to append Macheader */
+                        bufPtr = (u8*)skb_push(skb, macHeaderLengthInBytes);
+                        bulkdata->d[0].os_data_ptr = skb->data;
+                        bulkdata->d[0].os_net_buf_ptr = (unsigned char*)skb;
+                        bulkdata->d[0].data_length = skb->len;
+                    }
+                }
+            }
+    }
+
+    /* prepare the complete skb, by pushing the MAC header to the begining of the skb->data */
+    unifi_trace(priv, UDBG5, "updated Mac Header: %d \n",macHeaderLengthInBytes);
+    memcpy(bufPtr, macHeaderBuf, macHeaderLengthInBytes);
+
+    unifi_trace(priv, UDBG5, "leaving the update_macheader function\n");
+    return 0;
+}
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_ap_process_data_pdu
+ *
+ *
+ *      Takes care of intra BSS admission control & routing packets within BSS
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      skb             Socket buffer containing data packet to transmit
+ *      ehdr            ethernet header to fetch priority of packet
+ *      srcStaInfo      source stations record for connection verification
+ *      packed_signal
+ *      signal_len
+ *      signal          MA-PACKET.indication signal
+ *      bulkdata        if newSkb allocated then bulkdata updated to send to unifi
+ *      macHeaderLengthInBytes no. of bytes of mac header in received frame
+ *
+ *  Returns:
+ *      Zero on success(ap processing complete) or -1 if packet also have to be sent to NETDEV.
+ * ---------------------------------------------------------------------------
+ */
+int
+uf_ap_process_data_pdu(unifi_priv_t *priv, struct sk_buff *skb,
+                       struct ethhdr *ehdr, CsrWifiRouterCtrlStaInfo_t * srcStaInfo,
+                       const CSR_SIGNAL *signal,
+                       bulk_data_param_t *bulkdata,
+                       u8 macHeaderLengthInBytes)
+{
+    const CSR_MA_PACKET_INDICATION *ind = &(signal->u.MaPacketIndication);
+    u16 interfaceTag = (ind->VirtualInterfaceIdentifier & 0x00ff);
+    struct sk_buff *newSkb = NULL;
+    /* pointer to skb or private skb created using skb_copy() */
+    struct sk_buff *skbPtr = skb;
+    u8 sendToNetdev = FALSE;
+    u8 qosDestination = FALSE;
+    CSR_PRIORITY priority = CSR_CONTENTION;
+    CsrWifiRouterCtrlStaInfo_t *dstStaInfo = NULL;
+    netInterface_priv_t *interfacePriv;
+
+    unifi_trace(priv, UDBG5, "entering  uf_ap_process_data_pdu %d\n",macHeaderLengthInBytes);
+    /* InterfaceTag validation from MA_PACKET.indication */
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_trace(priv, UDBG1, "Interface Tag is Invalid in uf_ap_process_data_pdu\n");
+        unifi_net_data_free(priv, &bulkdata->d[0]);
+        return 0;
+    }
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if((interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) &&
+       (interfacePriv->intraBssEnabled == FALSE)) {
+        unifi_trace(priv, UDBG2, "uf_ap_process_data_pdu:P2P GO intrabssEnabled?= %d\n", interfacePriv->intraBssEnabled);
+
+        /*In P2P GO case, if intraBSS distribution Disabled then don't do IntraBSS routing */
+        /* If destination in our BSS then drop otherwise give packet to netdev */
+        dstStaInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, ehdr->h_dest, interfaceTag);
+        if (dstStaInfo) {
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            return 0;
+        }
+        /* May be associated P2PCLI trying to send the packets on backbone (Netdev) */
+        return -1;
+    }
+
+    if(!memcmp(ehdr->h_dest, interfacePriv->bssid.a, ETH_ALEN)) {
+        /* This packet will be given to the TCP/IP stack since this packet is for us(AP)
+         * No routing needed */
+        unifi_trace(priv, UDBG4, "destination address is csr_ap\n");
+        return -1;
+    }
+
+    /* fetch the destination record from staion record database */
+    dstStaInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, ehdr->h_dest, interfaceTag);
+
+    /* AP mode processing, & if packet is unicast */
+    if(!dstStaInfo) {
+        if (!(ehdr->h_dest[0] & 0x1)) {
+            /* destination not in station record & its a unicast packet, so pass the packet to network stack */
+            unifi_trace(priv, UDBG3, "unicast frame & destination record not exist, send to netdev proto = %x\n", htons(skb->protocol));
+            return -1;
+        } else {
+            /* packet is multicast/broadcast */
+            /* copy the skb to skbPtr, send skb to netdev & skbPtr to multicast/broad cast list */
+            unifi_trace(priv, UDBG5, "skb_copy, in  uf_ap_process_data_pdu, protocol = %x\n", htons(skb->protocol));
+            skbPtr = skb_copy(skb, GFP_KERNEL);
+            if(skbPtr == NULL) {
+                /* We don't have memory to don't send the frame in BSS*/
+                unifi_notice(priv, "broacast/multicast frame can't be sent in BSS No memeory: proto = %x\n", htons(skb->protocol));
+                return -1;
+            }
+            sendToNetdev = TRUE;
+        }
+    } else {
+
+        /* validate the Peer & Destination Station record */
+        if (uf_process_station_records_for_sending_data(priv, interfaceTag, srcStaInfo, dstStaInfo)) {
+            unifi_notice(priv, "uf_ap_process_data_pdu: station record validation failed \n");
+            interfacePriv->stats.rx_errors++;
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+            return 0;
+        }
+    }
+
+    /* BroadCast packet received and it's been sent as non QOS packets.
+     * Since WMM spec not mandates broadcast/multicast to be sent as QOS data only,
+     * if all Peers are QSTA
+     */
+    if(sendToNetdev) {
+       /* BroadCast packet and it's been sent as non QOS packets */
+        qosDestination = FALSE;
+    } else if(dstStaInfo && (dstStaInfo->wmmOrQosEnabled == TRUE)) {
+          qosDestination = TRUE;
+    }
+
+    unifi_trace(priv, UDBG3, "uf_ap_process_data_pdu QoS destination  = %s\n", (qosDestination)? "TRUE": "FALSE");
+
+    /* packet is allowed to send to unifi, update the Mac header */
+    if (update_macheader(priv, skbPtr, newSkb, &priority, bulkdata, interfaceTag, macHeaderLengthInBytes, qosDestination)) {
+        interfacePriv->stats.rx_errors++;
+        unifi_notice(priv, "(Packet Drop) failed to update the Mac header in uf_ap_process_data_pdu\n");
+        if (sendToNetdev) {
+            /*  Free's the skb_copy(skbPtr) data since packet processing failed */
+            bulkdata->d[0].os_data_ptr = skbPtr->data;
+            bulkdata->d[0].os_net_buf_ptr = (unsigned char*)skbPtr;
+            bulkdata->d[0].data_length = skbPtr->len;
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+        }
+        return -1;
+    }
+
+    unifi_trace(priv, UDBG3, "Mac Header updated...calling uf_process_ma_packet_req \n");
+
+    /* Packet is ready to send to unifi ,transmissionControl = 0x0004, confirmation is not needed for data packets */
+    if (uf_process_ma_packet_req(priv,  ehdr->h_dest, 0xffffffff, interfaceTag, CSR_NO_CONFIRM_REQUIRED, (CSR_RATE)0,priority, priv->netdev_client->sender_id, bulkdata)) {
+        if (sendToNetdev) {
+            unifi_trace(priv, UDBG1, "In uf_ap_process_data_pdu, (Packet Drop) uf_process_ma_packet_req failed. freeing skb_copy data (original data sent to Netdev)\n");
+            /*  Free's the skb_copy(skbPtr) data since packet processing failed */
+            bulkdata->d[0].os_data_ptr = skbPtr->data;
+            bulkdata->d[0].os_net_buf_ptr = (unsigned char*)skbPtr;
+            bulkdata->d[0].data_length = skbPtr->len;
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+        } else {
+            /* This free's the skb data */
+            unifi_trace(priv, UDBG1, "In uf_ap_process_data_pdu, (Packet Drop). Unicast data so freeing original skb \n");
+            unifi_net_data_free(priv, &bulkdata->d[0]);
+        }
+    }
+    unifi_trace(priv, UDBG5, "leaving  uf_ap_process_data_pdu\n");
+
+    if (sendToNetdev) {
+        /* The packet is multicast/broadcast, so after AP processing packet has to
+         * be sent to netdev, if peer port state is open
+        */
+        unifi_trace(priv, UDBG4, "Packet will be routed to NetDev\n");
+        return -1;
+    }
+    /* Ap handled the packet & its a unicast packet, no need to send to netdev */
+    return 0;
+}
+
+#endif
+
+CsrResult uf_process_ma_packet_req(unifi_priv_t *priv,
+                                   u8 *peerMacAddress,
+                                   CSR_CLIENT_TAG hostTag,
+                                   u16 interfaceTag,
+                                   CSR_TRANSMISSION_CONTROL transmissionControl,
+                                   CSR_RATE TransmitRate,
+                                   CSR_PRIORITY priority,
+                                   CSR_PROCESS_ID leSenderProcessId,
+                                   bulk_data_param_t *bulkdata)
+{
+    CsrResult status = CSR_RESULT_SUCCESS;
+    CSR_SIGNAL signal;
+    int result;
+#ifdef CSR_SUPPORT_SME
+   CsrWifiRouterCtrlStaInfo_t *staRecord = NULL;
+    const u8 *macHdrLocation =  bulkdata->d[0].os_data_ptr;
+    CsrWifiPacketType pktType;
+    int frameType = 0;
+    u8 queuePacketDozing = FALSE;
+    u32 priority_q;
+    u16 frmCtrl;
+    struct list_head * list = NULL; /* List to which buffered PDUs are to be enqueued*/
+    u8 setBcTim=FALSE;
+    netInterface_priv_t *interfacePriv;
+    u8 requeueOnSamePos = FALSE;
+    u32 handle = 0xFFFFFFFF;
+    unsigned long lock_flags;
+
+	unifi_trace(priv, UDBG5,
+		"entering uf_process_ma_packet_req, peer: %pMF\n",
+		peerMacAddress);
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "interfaceTag >= CSR_WIFI_NUM_INTERFACES, interfacetag = %d\n", interfaceTag);
+        return CSR_RESULT_FAILURE;
+    }
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+
+    /* fetch the station record for corresponding peer mac address */
+    if ((staRecord = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, peerMacAddress, interfaceTag))) {
+        handle = staRecord->assignedHandle;
+    }
+
+    /* Frame ma-packet.req, this is saved/transmitted depend on queue state */
+    unifi_frame_ma_packet_req(priv, priority, TransmitRate, hostTag,
+                              interfaceTag, transmissionControl, leSenderProcessId,
+                              peerMacAddress, &signal);
+
+   /* Since it's common path between STA & AP mode, in case of STA packet
+     * need not to be queued but in AP case we have to queue PDU's in
+     * different scenarios
+     */
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            /* For this mode processing done below */
+            break;
+        default:
+            /* In case of STA/IBSS/P2PCLI/AMP, no checks needed send the packet down & return */
+            unifi_trace(priv, UDBG5, "In %s, interface mode is %x \n", __FUNCTION__, interfacePriv->interfaceMode);
+            if (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_NONE) {
+                unifi_warning(priv, "In %s, interface mode NONE \n", __FUNCTION__);
+            }
+            if ((result = ul_send_signal_unpacked(priv, &signal, bulkdata))) {
+                status = CSR_RESULT_FAILURE;
+            }
+            return status;
+    }
+
+    /* -----Only AP/P2pGO mode handling falls below----- */
+
+    /* convert priority to queue */
+    priority_q = unifi_frame_priority_to_queue((CSR_PRIORITY) priority);
+
+    /* check the powersave status of the peer */
+    if (staRecord && (staRecord->currentPeerState ==
+                     CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE)) {
+        /* Peer is dozing & packet have to be delivered, so buffer the packet &
+         * update the TIM
+         */
+        queuePacketDozing = TRUE;
+    }
+
+    /* find the type of frame unicast or mulicast/broadcast */
+    if (*peerMacAddress & 0x1) {
+        /* Multicast/broadCast data are always triggered by vif_availability.ind
+         * at the DTIM
+         */
+        pktType = CSR_WIFI_MULTICAST_PDU;
+    } else {
+        pktType = CSR_WIFI_UNICAST_PDU;
+    }
+
+    /* Fetch the frame control field from mac header & check for frame type */
+    frmCtrl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(macHdrLocation);
+
+    /* Processing done according to Frame/Packet type */
+    frameType =  ((frmCtrl & 0x000c) >> FRAME_CONTROL_TYPE_FIELD_OFFSET);
+    switch(frameType)
+    {
+        case IEEE802_11_FRAMETYPE_MANAGEMENT:
+
+            switch(pktType)
+            {
+                case CSR_WIFI_UNICAST_PDU:
+                    unifi_trace(priv, UDBG5, "management unicast PDU in uf_process_ma_packet_req \n");
+                    /* push the packet in to the queue with appropriate mgt list */
+                    if (!staRecord) {
+                        /* push the packet to the unifi if list is empty (if packet lost how to re-enque) */
+                        if (list_empty(&interfacePriv->genericMgtFrames)) {
+#ifdef CSR_SUPPORT_SME
+                            if(!(IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag))) {
+#endif
+
+                            unifi_trace(priv, UDBG3, "genericMgtFrames list is empty uf_process_ma_packet_req \n");
+                            result = ul_send_signal_unpacked(priv, &signal, bulkdata);
+                            /*  reque only on ENOSPC */
+                            if(result == -ENOSPC) {
+                                /* requeue the failed packet to genericMgtFrame with same position */
+                                unifi_trace(priv, UDBG1, "(ENOSPC) Sending genericMgtFrames Failed so buffering\n");
+                                list = &interfacePriv->genericMgtFrames;
+                                requeueOnSamePos = TRUE;
+                            }
+#ifdef CSR_SUPPORT_SME
+                            }else{
+                                list = &interfacePriv->genericMgtFrames;
+                                unifi_trace(priv, UDBG3, "genericMgtFrames queue empty and dtim started\n hosttag is 0x%x,\n",signal.u.MaPacketRequest.HostTag);
+                                update_eosp_to_head_of_broadcast_list_head(priv,interfaceTag);
+                           }
+#endif
+                        } else {
+                            /* Queue the packet to genericMgtFrame of unifi_priv_t data structure */
+                            list = &interfacePriv->genericMgtFrames;
+                            unifi_trace(priv, UDBG2, "genericMgtFrames queue not empty\n");
+                        }
+                    } else {
+                        /* check peer power state */
+                        if (queuePacketDozing || !list_empty(&staRecord->mgtFrames) || IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag)) {
+                            /* peer is in dozing mode, so queue packet in mgt frame list of station record */
+                           /*if multicast traffic is going on, buffer the unicast packets*/
+                            list = &staRecord->mgtFrames;
+
+                            unifi_trace(priv, UDBG1, "staRecord->MgtFrames list empty? = %s, handle = %d, queuePacketDozing = %d\n",
+                                        (list_empty(&staRecord->mgtFrames))? "YES": "NO", staRecord->assignedHandle, queuePacketDozing);
+                            if(IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag)){
+                                update_eosp_to_head_of_broadcast_list_head(priv,interfaceTag);
+                            }
+
+                        } else {
+                            unifi_trace(priv, UDBG5, "staRecord->mgtFrames list is empty uf_process_ma_packet_req \n");
+                            result = ul_send_signal_unpacked(priv, &signal, bulkdata);
+                            if(result == -ENOSPC) {
+                                /* requeue the failed packet to staRecord->mgtFrames with same position */
+                                list = &staRecord->mgtFrames;
+                                requeueOnSamePos = TRUE;
+                                unifi_trace(priv, UDBG1, "(ENOSPC) Sending MgtFrames Failed handle = %d so buffering\n",staRecord->assignedHandle);
+                                priv->pausedStaHandle[0]=(u8)(staRecord->assignedHandle);
+                            } else if (result) {
+                                status = CSR_RESULT_FAILURE;
+                            }
+                        }
+                    }
+                    break;
+                case CSR_WIFI_MULTICAST_PDU:
+                    unifi_trace(priv, UDBG5, "management multicast/broadcast PDU in uf_process_ma_packet_req 'QUEUE it' \n");
+                    /* Queue the packet to genericMulticastOrBroadCastMgtFrames of unifi_priv_t data structure
+                     * will be sent when we receive VIF AVAILABILITY from firmware as part of DTIM
+                     */
+
+                    list = &interfacePriv->genericMulticastOrBroadCastMgtFrames;
+                    if((interfacePriv->interfaceMode != CSR_WIFI_ROUTER_CTRL_MODE_IBSS) &&
+                            (list_empty(&interfacePriv->genericMulticastOrBroadCastMgtFrames))) {
+                        setBcTim=TRUE;
+                    }
+                    break;
+                default:
+                    unifi_error(priv, "condition never meets: packet type unrecognized\n");
+            }
+            break;
+        case IEEE802_11_FRAMETYPE_DATA:
+            switch(pktType)
+            {
+                case CSR_WIFI_UNICAST_PDU:
+                    unifi_trace(priv, UDBG5, "data unicast PDU in uf_process_ma_packet_req \n");
+                    /* check peer power state, list status & peer port status */
+                    if(!staRecord) {
+                        unifi_error(priv, "In %s unicast but staRecord = NULL\n", __FUNCTION__);
+                        return CSR_RESULT_FAILURE;
+                    } else if (queuePacketDozing || isRouterBufferEnabled(priv,priority_q)|| !list_empty(&staRecord->dataPdu[priority_q]) || IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag)) {
+                        /* peer is in dozing mode, so queue packet in mgt frame list of station record */
+                        /* if multicast traffic is going on, buffet the unicast packets */
+                        unifi_trace(priv, UDBG2, "Enqueued to staRecord->dataPdu[%d] queuePacketDozing=%d,\
+                                Buffering enabled = %d \n", priority_q,queuePacketDozing,isRouterBufferEnabled(priv,priority_q));
+                        list = &staRecord->dataPdu[priority_q];
+                    } else {
+                        unifi_trace(priv, UDBG5, "staRecord->dataPdu[%d] list is empty uf_process_ma_packet_req \n", priority_q);
+                        /* Pdu allowed to send to unifi */
+                        result = ul_send_signal_unpacked(priv, &signal, bulkdata);
+                        if(result == -ENOSPC) {
+                            /* requeue the failed packet to staRecord->dataPdu[priority_q] with same position */
+                            unifi_trace(priv, UDBG1, "(ENOSPC) Sending Unicast DataPDU to queue %d Failed so buffering\n",priority_q);
+                            requeueOnSamePos = TRUE;
+                            list = &staRecord->dataPdu[priority_q];
+                            priv->pausedStaHandle[priority_q]=(u8)(staRecord->assignedHandle);
+                            if(!isRouterBufferEnabled(priv,priority_q)) {
+                                unifi_error(priv,"Buffering Not enabled for queue %d \n",priority_q);
+                            }
+                        } else if (result) {
+                            status = CSR_RESULT_FAILURE;
+                        }
+                    }
+                    break;
+                case CSR_WIFI_MULTICAST_PDU:
+                    unifi_trace(priv, UDBG5, "data multicast/broadcast PDU in uf_process_ma_packet_req \n");
+                    /* Queue the packet to genericMulticastOrBroadCastFrames list of unifi_priv_t data structure
+                     * will be sent when we receive VIF AVAILABILITY from firmware as part of DTIM
+                     */
+                    list = &interfacePriv->genericMulticastOrBroadCastFrames;
+                    if(list_empty(&interfacePriv->genericMulticastOrBroadCastFrames)) {
+                        setBcTim = TRUE;
+                    }
+                    break;
+                default:
+                    unifi_error(priv, "condition never meets: packet type un recognized\n");
+            }
+            break;
+        default:
+            unifi_error(priv, "unrecognized frame type\n");
+    }
+    if(list) {
+        status = enque_tx_data_pdu(priv, bulkdata,list, &signal,requeueOnSamePos);
+        /* Record no. of packet queued for each peer */
+        if (staRecord && (pktType == CSR_WIFI_UNICAST_PDU) && (!status)) {
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            staRecord->noOfPktQueued++;
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        }
+        else if ((pktType == CSR_WIFI_MULTICAST_PDU) && (!status))
+        {
+            /* If broadcast Tim is set && queuing is successfull, then only update TIM */
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            interfacePriv->noOfbroadcastPktQueued++;
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        }
+    }
+    /* If broadcast Tim is set && queuing is successfull, then only update TIM */
+    if(setBcTim && !status) {
+        unifi_trace(priv, UDBG3, "tim set due to broadcast pkt\n");
+        if (!interfacePriv->bcTimSetReqPendingFlag)
+        {
+            update_tim(priv,0,CSR_WIFI_TIM_SET,interfaceTag, handle);
+        }
+        else
+        {
+            /* Cache the TimSet value so that it will processed immidiatly after
+            * completing the current setTim Request
+            */
+            interfacePriv->bcTimSetReqQueued = CSR_WIFI_TIM_SET;
+            unifi_trace(priv, UDBG2, "uf_process_ma_packet_req : One more UpdateDTim Request(:%d) Queued \n",
+                        interfacePriv->bcTimSetReqQueued);
+        }
+    } else if(staRecord && staRecord->currentPeerState ==
+                            CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE) {
+        if(staRecord->timSet == CSR_WIFI_TIM_RESET || staRecord->timSet == CSR_WIFI_TIM_RESETTING) {
+            if(!staRecord->wmmOrQosEnabled) {
+                if(!list_empty(&staRecord->mgtFrames) ||
+                   !list_empty(&staRecord->dataPdu[3]) ||
+                   !list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION])) {
+                    unifi_trace(priv, UDBG3, "tim set due to unicast pkt & peer in powersave\n");
+                    if (!staRecord->timRequestPendingFlag){
+                        update_tim(priv,staRecord->aid,1,interfaceTag, handle);
+                    }
+                    else
+                    {
+                        /* Cache the TimSet value so that it will processed immidiatly after
+                         * completing the current setTim Request
+                         */
+                        staRecord->updateTimReqQueued = 1;
+                        unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                                    staRecord->aid);
+                    }
+                }
+            } else {
+                /* Check for non delivery enable(i.e trigger enable), all delivery enable & legacy AC for TIM update in firmware */
+                u8 allDeliveryEnabled = 0, dataAvailable = 0;
+                /* Check if all AC's are Delivery Enabled */
+                is_all_ac_deliver_enabled_and_moredata(staRecord, &allDeliveryEnabled, &dataAvailable);
+                if (uf_is_more_data_for_non_delivery_ac(staRecord) || (allDeliveryEnabled && dataAvailable)
+                    || (!list_empty(&staRecord->mgtFrames))) {
+                    if (!staRecord->timRequestPendingFlag) {
+                        update_tim(priv,staRecord->aid,1,interfaceTag, handle);
+                    }
+                    else
+                    {
+                        /* Cache the TimSet value so that it will processed immidiatly after
+                         * completing the current setTim Request
+                         */
+                        staRecord->updateTimReqQueued = 1;
+                        unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                                    staRecord->aid);
+                    }
+                }
+            }
+        }
+    }
+
+    if((list) && (pktType == CSR_WIFI_UNICAST_PDU && !queuePacketDozing) && !(isRouterBufferEnabled(priv,priority_q)) && !(IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag))) {
+        unifi_trace(priv, UDBG2, "buffering cleared for queue = %d So resending buffered frames\n",priority_q);
+        uf_send_buffered_frames(priv, priority_q);
+    }
+    unifi_trace(priv, UDBG5, "leaving uf_process_ma_packet_req \n");
+    return status;
+#else
+#ifdef CSR_NATIVE_LINUX
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "interfaceTag >= CSR_WIFI_NUM_INTERFACES, interfacetag = %d\n", interfaceTag);
+        return CSR_RESULT_FAILURE;
+    }
+    /* Frame ma-packet.req, this is saved/transmitted depend on queue state */
+    unifi_frame_ma_packet_req(priv, priority, TransmitRate, hostTag, interfaceTag,
+            transmissionControl, leSenderProcessId,
+            peerMacAddress, &signal);
+    result = ul_send_signal_unpacked(priv, &signal, bulkdata);
+    if (result) {
+        return CSR_RESULT_FAILURE;
+    }
+#endif
+    return status;
+#endif
+}
+
+#ifdef CSR_SUPPORT_SME
+s8 uf_get_protection_bit_from_interfacemode(unifi_priv_t *priv, u16 interfaceTag, const u8 *daddr)
+{
+    s8 protection = 0;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    switch(interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_STA:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PCLI:
+        case CSR_WIFI_ROUTER_CTRL_MODE_AMP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_IBSS:
+            protection = interfacePriv->protect;
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            {
+                CsrWifiRouterCtrlStaInfo_t *dstStaInfo = NULL;
+                if (daddr[0] & 0x1) {
+                    unifi_trace(priv, UDBG3, "broadcast/multicast packet in send_ma_pkt_request\n");
+                    /* In this mode, the protect member of priv structure has an information of how
+                     * AP/P2PGO has started, & the member updated in set mode request for AP/P2PGO
+                     */
+                    protection = interfacePriv->protect;
+                } else {
+                    /* fetch the destination record from staion record database */
+                    dstStaInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, daddr, interfaceTag);
+                    if (!dstStaInfo) {
+                        unifi_trace(priv, UDBG3, "peer not found in station record in send_ma_pkt_request\n");
+                        return -1;
+                    }
+                    protection = dstStaInfo->protection;
+                }
+            }
+            break;
+        default:
+            unifi_trace(priv, UDBG2, "mode unknown in send_ma_pkt_request\n");
+    }
+    return protection;
+}
+#endif
+#ifdef CSR_SUPPORT_SME
+u8 send_multicast_frames(unifi_priv_t *priv, u16 interfaceTag)
+{
+    int r;
+    tx_buffered_packets_t * buffered_pkt = NULL;
+    u8 moreData = FALSE;
+    u8 pduSent =0;
+    unsigned long lock_flags;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    u32 hostTag = 0xffffffff;
+
+    func_enter();
+    if(!isRouterBufferEnabled(priv,UNIFI_TRAFFIC_Q_VO)) {
+        while((interfacePriv->dtimActive)&& (buffered_pkt=dequeue_tx_data_pdu(priv,&interfacePriv->genericMulticastOrBroadCastMgtFrames))) {
+            buffered_pkt->transmissionControl |= (TRANSMISSION_CONTROL_TRIGGER_MASK);
+            moreData = (buffered_pkt->transmissionControl & TRANSMISSION_CONTROL_EOSP_MASK)?FALSE:TRUE;
+
+
+            unifi_trace(priv,UDBG2,"DTIM Occurred for interface:sending Mgt packet %d\n",interfaceTag);
+
+            if((r=frame_and_send_queued_pdu(priv,buffered_pkt,NULL,moreData,FALSE)) == -ENOSPC) {
+               unifi_trace(priv,UDBG1,"frame_and_send_queued_pdu failed with ENOSPC for host tag = %x\n", buffered_pkt->hostTag);
+               /* Enqueue at the head of the queue */
+               spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+               list_add(&buffered_pkt->q, &interfacePriv->genericMulticastOrBroadCastMgtFrames);
+               spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+               break;
+            } else {
+                unifi_trace(priv,UDBG1,"send_multicast_frames: Send genericMulticastOrBroadCastMgtFrames (%x, %x)\n",
+                                        buffered_pkt->hostTag,
+                                        r);
+                if(r) {
+                   unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                }
+                if(!moreData) {
+
+                    interfacePriv->dtimActive = FALSE;
+                    if(!r) {
+                        hostTag = buffered_pkt->hostTag;
+                        pduSent++;
+                    } else {
+                        send_vif_availibility_rsp(priv,uf_get_vif_identifier(interfacePriv->interfaceMode,interfaceTag),CSR_RC_UNSPECIFIED_FAILURE);
+                    }
+                }
+                /* Buffered frame sent successfully */
+                spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+                interfacePriv->noOfbroadcastPktQueued--;
+                spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+                kfree(buffered_pkt);
+           }
+
+        }
+    }
+    if(!isRouterBufferEnabled(priv,UNIFI_TRAFFIC_Q_CONTENTION)) {
+        while((interfacePriv->dtimActive)&& (buffered_pkt=dequeue_tx_data_pdu(priv,&interfacePriv->genericMulticastOrBroadCastFrames))) {
+            buffered_pkt->transmissionControl |= TRANSMISSION_CONTROL_TRIGGER_MASK;
+            moreData = (buffered_pkt->transmissionControl & TRANSMISSION_CONTROL_EOSP_MASK)?FALSE:TRUE;
+
+
+            if((r=frame_and_send_queued_pdu(priv,buffered_pkt,NULL,moreData,FALSE)) == -ENOSPC) {
+                /* Clear the trigger bit transmission control*/
+                buffered_pkt->transmissionControl &= ~(TRANSMISSION_CONTROL_TRIGGER_MASK);
+                /* Enqueue at the head of the queue */
+                spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                list_add(&buffered_pkt->q, &interfacePriv->genericMulticastOrBroadCastFrames);
+                spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                break;
+            } else {
+                if(r) {
+                    unifi_trace(priv,UDBG1,"send_multicast_frames: Send genericMulticastOrBroadCastFrame failed (%x, %x)\n",
+                                            buffered_pkt->hostTag,
+                                            r);
+                    unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                }
+                if(!moreData) {
+                    interfacePriv->dtimActive = FALSE;
+                    if(!r) {
+                        pduSent ++;
+                        hostTag = buffered_pkt->hostTag;
+                    } else {
+                        send_vif_availibility_rsp(priv,uf_get_vif_identifier(interfacePriv->interfaceMode,interfaceTag),CSR_RC_UNSPECIFIED_FAILURE);
+                    }
+                }
+                /* Buffered frame sent successfully */
+                spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+                interfacePriv->noOfbroadcastPktQueued--;
+                spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+                kfree(buffered_pkt);
+            }
+        }
+    }
+    if((interfacePriv->dtimActive == FALSE)) {
+        /* Record the host Tag*/
+        unifi_trace(priv,UDBG2,"send_multicast_frames: Recorded hostTag of EOSP packet: = 0x%x\n",hostTag);
+        interfacePriv->multicastPduHostTag = hostTag;
+    }
+    return pduSent;
+}
+#endif
+void uf_process_ma_vif_availibility_ind(unifi_priv_t *priv,u8 *sigdata,
+                                        u32 siglen)
+{
+#ifdef CSR_SUPPORT_SME
+    CSR_SIGNAL signal;
+    CSR_MA_VIF_AVAILABILITY_INDICATION *ind;
+    int r;
+    u16 interfaceTag;
+    u8 pduSent =0;
+    CSR_RESULT_CODE resultCode = CSR_RC_SUCCESS;
+    netInterface_priv_t *interfacePriv;
+
+    func_enter();
+    unifi_trace(priv, UDBG3,
+            "uf_process_ma_vif_availibility_ind: Process signal 0x%.4X\n",
+            *((u16*)sigdata));
+
+    r = read_unpack_signal(sigdata, &signal);
+    if (r) {
+        unifi_error(priv,
+                    "uf_process_ma_vif_availibility_ind: Received unknown signal 0x%.4X.\n",
+                    CSR_GET_UINT16_FROM_LITTLE_ENDIAN(sigdata));
+        func_exit();
+        return;
+    }
+    ind = &signal.u.MaVifAvailabilityIndication;
+    interfaceTag=ind->VirtualInterfaceIdentifier & 0xff;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "in vif_availability_ind interfaceTag is wrong\n");
+        return;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if(ind->Multicast) {
+        if(list_empty(&interfacePriv->genericMulticastOrBroadCastFrames) &&
+            list_empty(&interfacePriv->genericMulticastOrBroadCastMgtFrames)) {
+            /* This condition can occur because of a potential race where the
+               TIM is not yet reset as host is waiting for confirm but it is sent
+               by firmware and DTIM occurs*/
+            unifi_notice(priv,"ma_vif_availibility_ind recevied for multicast but queues are empty%d\n",interfaceTag);
+            send_vif_availibility_rsp(priv,ind->VirtualInterfaceIdentifier,CSR_RC_NO_BUFFERED_BROADCAST_MULTICAST_FRAMES);
+            interfacePriv->dtimActive = FALSE;
+            if(interfacePriv->multicastPduHostTag == 0xffffffff) {
+                unifi_notice(priv,"ma_vif_availibility_ind recevied for multicast but queues are empty%d\n",interfaceTag);
+                /* This may be an extra request in very rare race conditions but it is fine as it would atleast remove the potential lock up */
+                if (!interfacePriv->bcTimSetReqPendingFlag)
+                {
+                    update_tim(priv,0,CSR_WIFI_TIM_RESET,interfaceTag, 0xFFFFFFFF);
+                }
+                else
+                {
+                    /* Cache the TimSet value so that it will processed immidiatly after
+                     * completing the current setTim Request
+                     */
+                    interfacePriv->bcTimSetReqQueued = CSR_WIFI_TIM_RESET;
+                    unifi_trace(priv, UDBG2, "uf_process_ma_vif_availibility_ind : One more UpdateDTim Request(%d) Queued \n",
+                                interfacePriv->bcTimSetReqQueued);
+                }
+            }
+            return;
+        }
+        if(interfacePriv->dtimActive) {
+            unifi_trace(priv,UDBG2,"DTIM Occurred for already active DTIM interface %d\n",interfaceTag);
+            return;
+        } else {
+            unifi_trace(priv,UDBG2,"DTIM Occurred for interface %d\n",interfaceTag);
+            if(list_empty(&interfacePriv->genericMulticastOrBroadCastFrames)) {
+                set_eosp_transmit_ctrl(priv,&interfacePriv->genericMulticastOrBroadCastMgtFrames);
+            } else {
+                set_eosp_transmit_ctrl(priv,&interfacePriv->genericMulticastOrBroadCastFrames);
+            }
+        }
+        interfacePriv->dtimActive = TRUE;
+        pduSent = send_multicast_frames(priv,interfaceTag);
+    }
+    else {
+        unifi_error(priv,"Interface switching is not supported %d\n",interfaceTag);
+        resultCode = CSR_RC_NOT_SUPPORTED;
+        send_vif_availibility_rsp(priv,ind->VirtualInterfaceIdentifier,CSR_RC_NOT_SUPPORTED);
+    }
+#endif
+}
+#ifdef CSR_SUPPORT_SME
+
+#define  GET_ACTIVE_INTERFACE_TAG(priv) 0
+
+static u8 uf_is_more_data_for_delivery_ac(unifi_priv_t *priv, CsrWifiRouterCtrlStaInfo_t *staRecord)
+{
+    s8 i;
+
+    for(i=UNIFI_TRAFFIC_Q_VO; i >= UNIFI_TRAFFIC_Q_BK; i--)
+    {
+        if(((staRecord->powersaveMode[i]==CSR_WIFI_AC_DELIVERY_ONLY_ENABLE)
+             ||(staRecord->powersaveMode[i]==CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED))
+             &&(!list_empty(&staRecord->dataPdu[i]))) {
+            unifi_trace(priv,UDBG2,"uf_is_more_data_for_delivery_ac: Data Available AC = %d\n", i);
+            return TRUE;
+        }
+    }
+
+    unifi_trace(priv,UDBG2,"uf_is_more_data_for_delivery_ac: Data NOT Available \n");
+    return FALSE;
+}
+
+static u8 uf_is_more_data_for_usp_delivery(unifi_priv_t *priv, CsrWifiRouterCtrlStaInfo_t *staRecord, unifi_TrafficQueue queue)
+{
+    s8 i;
+
+    for(i = queue; i >= UNIFI_TRAFFIC_Q_BK; i--)
+    {
+        if(((staRecord->powersaveMode[i]==CSR_WIFI_AC_DELIVERY_ONLY_ENABLE)
+             ||(staRecord->powersaveMode[i]==CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED))
+             &&(!list_empty(&staRecord->dataPdu[i]))) {
+            unifi_trace(priv,UDBG2,"uf_is_more_data_for_usp_delivery: Data Available AC = %d\n", i);
+            return TRUE;
+        }
+    }
+
+    unifi_trace(priv,UDBG2,"uf_is_more_data_for_usp_delivery: Data NOT Available \n");
+    return FALSE;
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_send_buffered_data_from_delivery_ac
+ *
+ *      This function takes care of
+ *      -> Parsing the delivery enabled queue & sending frame down to HIP
+ *      -> Setting EOSP=1 when USP to be terminated
+ *      -> Depending on MAX SP length services the USP
+ *
+ * NOTE:This function always called from uf_handle_uspframes_delivery(), Dont
+ *      call this function from any other location in code
+ *
+ *  Arguments:
+ *      priv        Pointer to device private context struct
+ *      vif         interface specific HIP vif instance
+ *      staInfo     peer for which UAPSD to be scheduled
+ *      queue       AC from which Data to be sent in USP
+ *      txList      access category for processing list
+ * ---------------------------------------------------------------------------
+ */
+void uf_send_buffered_data_from_delivery_ac(unifi_priv_t *priv,
+                                            CsrWifiRouterCtrlStaInfo_t * staInfo,
+                                            u8 queue,
+                                            struct list_head *txList)
+{
+
+    u16 interfaceTag = GET_ACTIVE_INTERFACE_TAG(priv);
+    tx_buffered_packets_t * buffered_pkt = NULL;
+    unsigned long lock_flags;
+    u8 eosp=FALSE;
+    s8 r =0;
+    u8 moreData = FALSE;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    unifi_trace(priv, UDBG2, "++uf_send_buffered_data_from_delivery_ac, active=%x\n", staInfo->uapsdActive);
+
+    if (queue > UNIFI_TRAFFIC_Q_VO)
+    {
+        return;
+    }
+    while((buffered_pkt=dequeue_tx_data_pdu(priv, txList))) {
+        if((IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag))) {
+            unifi_trace(priv, UDBG2, "uf_send_buffered_data_from_delivery_ac: DTIM Active, suspend UAPSD, staId: 0x%x\n",
+                        staInfo->aid);
+
+            /* Once resume called, the U-APSD delivery operation will resume */
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            staInfo->uspSuspend = TRUE;
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+            /* re-queueing the packet as DTIM started */
+            spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+            list_add(&buffered_pkt->q,txList);
+            spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+            break;
+        }
+
+        buffered_pkt->transmissionControl &=
+                 ~(TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+
+
+        if((staInfo->wmmOrQosEnabled == TRUE)&&(staInfo->uapsdActive == TRUE)) {
+
+             buffered_pkt->transmissionControl = TRANSMISSION_CONTROL_TRIGGER_MASK;
+
+             /* Check All delivery enables Ac for more data, because caller of this
+              * function not aware about last packet
+              * (First check in moreData fetching helps in draining out Mgt frames Q)
+              */
+              moreData = (!list_empty(txList) || uf_is_more_data_for_usp_delivery(priv, staInfo, queue));
+
+              if(staInfo->noOfSpFramesSent == (staInfo->maxSpLength - 1)) {
+                  moreData = FALSE;
+              }
+
+              if(moreData == FALSE) {
+                  eosp = TRUE;
+                  buffered_pkt->transmissionControl =
+                      (TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+              }
+        } else {
+            /* Non QoS and non U-APSD */
+            unifi_warning(priv, "uf_send_buffered_data_from_delivery_ac: non U-APSD !!! \n");
+        }
+
+        unifi_trace(priv,UDBG2,"uf_send_buffered_data_from_delivery_ac : MoreData:%d, EOSP:%d\n",moreData,eosp);
+
+        if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staInfo,moreData,eosp)) == -ENOSPC) {
+
+            unifi_trace(priv, UDBG2, "uf_send_buffered_data_from_delivery_ac: UASPD suspended, ENOSPC in hipQ=%x\n", queue);
+
+            /* Once resume called, the U-APSD delivery operation will resume */
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            staInfo->uspSuspend = TRUE;
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+            spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+            list_add(&buffered_pkt->q,txList);
+            spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+            priv->pausedStaHandle[queue]=(u8)(staInfo->assignedHandle);
+            break;
+        } else {
+            if(r){
+                /* the PDU failed where we can't do any thing so free the storage */
+                unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+            }
+            kfree(buffered_pkt);
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            staInfo->noOfSpFramesSent++;
+            if((!moreData) || (staInfo->noOfSpFramesSent == staInfo->maxSpLength)) {
+                unifi_trace(priv, UDBG2, "uf_send_buffered_data_from_delivery_ac: Terminating USP\n");
+                staInfo->uapsdActive = FALSE;
+                staInfo->uspSuspend = FALSE;
+                staInfo->noOfSpFramesSent = 0;
+                spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+                break;
+            }
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        }
+    }
+    unifi_trace(priv, UDBG2, "--uf_send_buffered_data_from_delivery_ac, active=%x\n", staInfo->uapsdActive);
+}
+
+void uf_send_buffered_data_from_ac(unifi_priv_t *priv,
+                                   CsrWifiRouterCtrlStaInfo_t * staInfo,
+                                   u8 queue,
+                                   struct list_head *txList)
+{
+    tx_buffered_packets_t * buffered_pkt = NULL;
+    unsigned long lock_flags;
+    u8 eosp=FALSE;
+    u8 moreData = FALSE;
+    s8 r =0;
+
+    func_enter();
+
+    unifi_trace(priv,UDBG2,"uf_send_buffered_data_from_ac :\n");
+
+    while(!isRouterBufferEnabled(priv,queue) &&
+                    ((buffered_pkt=dequeue_tx_data_pdu(priv, txList))!=NULL)){
+
+        buffered_pkt->transmissionControl &=
+                 ~(TRANSMISSION_CONTROL_TRIGGER_MASK|TRANSMISSION_CONTROL_EOSP_MASK);
+
+        unifi_trace(priv,UDBG3,"uf_send_buffered_data_from_ac : MoreData:%d, EOSP:%d\n",moreData,eosp);
+
+        if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staInfo,moreData,eosp)) == -ENOSPC) {
+           /* Enqueue at the head of the queue */
+           spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+           list_add(&buffered_pkt->q,txList);
+           spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+           if(staInfo != NULL){
+              priv->pausedStaHandle[queue]=(u8)(staInfo->assignedHandle);
+           }
+           unifi_trace(priv,UDBG3," uf_send_buffered_data_from_ac: PDU sending failed .. no space for queue %d \n",queue);
+           } else {
+            if(r){
+                /* the PDU failed where we can't do any thing so free the storage */
+                unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+            }
+            kfree(buffered_pkt);
+      }
+  }
+
+  func_exit();
+
+}
+
+void uf_send_buffered_frames(unifi_priv_t *priv,unifi_TrafficQueue q)
+{
+    u16 interfaceTag = GET_ACTIVE_INTERFACE_TAG(priv);
+    u32 startIndex=0,endIndex=0;
+    CsrWifiRouterCtrlStaInfo_t * staInfo = NULL;
+    u8 queue;
+    u8 moreData = FALSE;
+
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if(!((interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP) ||
+        (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO)))
+        return;
+    func_enter();
+
+    queue = (q<=3)?q:0;
+
+    if(interfacePriv->dtimActive) {
+        /* this function updates dtimActive*/
+        send_multicast_frames(priv,interfaceTag);
+        if(!interfacePriv->dtimActive) {
+            moreData = (!list_empty(&interfacePriv->genericMulticastOrBroadCastMgtFrames) ||
+             !list_empty(&interfacePriv->genericMulticastOrBroadCastFrames));
+            if(!moreData) {
+                if (!interfacePriv->bcTimSetReqPendingFlag)
+                {
+                    update_tim(priv,0,CSR_WIFI_TIM_RESET,interfaceTag, 0XFFFFFFFF);
+                }
+                else
+                {
+                    /* Cache the TimSet value so that it will processed immidiatly after
+                     * completing the current setTim Request
+                     */
+                    interfacePriv->bcTimSetReqQueued = CSR_WIFI_TIM_RESET;
+                    unifi_trace(priv, UDBG2, "uf_send_buffered_frames : One more UpdateDTim Request(%d) Queued \n",
+                                interfacePriv->bcTimSetReqQueued);
+                }
+            }
+        } else {
+            moreData = (!list_empty(&interfacePriv->genericMulticastOrBroadCastMgtFrames) ||
+                        !list_empty(&interfacePriv->genericMulticastOrBroadCastFrames));
+           if(!moreData) {
+               /* This should never happen but if it happens, we need a way out */
+               unifi_error(priv,"ERROR: No More Data but DTIM is active sending Response\n");
+               send_vif_availibility_rsp(priv,uf_get_vif_identifier(interfacePriv->interfaceMode,interfaceTag),CSR_RC_NO_BUFFERED_BROADCAST_MULTICAST_FRAMES);
+               interfacePriv->dtimActive = FALSE;
+           }
+        }
+        func_exit();
+        return;
+    }
+    if(priv->pausedStaHandle[queue] > 7) {
+        priv->pausedStaHandle[queue] = 0;
+    }
+
+    if(queue == UNIFI_TRAFFIC_Q_VO) {
+
+
+        unifi_trace(priv,UDBG2,"uf_send_buffered_frames : trying mgt from queue=%d\n",queue);
+        for(startIndex= 0; startIndex < UNIFI_MAX_CONNECTIONS;startIndex++) {
+            staInfo =  CsrWifiRouterCtrlGetStationRecordFromHandle(priv,startIndex,interfaceTag);
+            if(!staInfo ) {
+                continue;
+            } else if((staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE)
+                       &&(staInfo->uapsdActive == FALSE) ) {
+                continue;
+            }
+
+            if((staInfo != NULL)&&(staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE)
+                               &&(staInfo->uapsdActive == FALSE)){
+                            /*Non-UAPSD case push the management frames out*/
+               if(!list_empty(&staInfo->mgtFrames)){
+                    uf_send_buffered_data_from_ac(priv,staInfo, UNIFI_TRAFFIC_Q_VO, &staInfo->mgtFrames);
+                }
+            }
+
+            if(isRouterBufferEnabled(priv,queue)) {
+                unifi_notice(priv,"uf_send_buffered_frames : No space Left for queue = %d\n",queue);
+                break;
+            }
+        }
+        /*push generic management frames out*/
+        if(!list_empty(&interfacePriv->genericMgtFrames)) {
+            unifi_trace(priv,UDBG2,"uf_send_buffered_frames : trying generic mgt from queue=%d\n",queue);
+            uf_send_buffered_data_from_ac(priv,staInfo, UNIFI_TRAFFIC_Q_VO, &interfacePriv->genericMgtFrames);
+        }
+    }
+
+
+    unifi_trace(priv,UDBG2,"uf_send_buffered_frames : Resume called for Queue=%d\n",queue);
+    unifi_trace(priv,UDBG2,"uf_send_buffered_frames : start=%d end=%d\n",startIndex,endIndex);
+
+    startIndex = priv->pausedStaHandle[queue];
+    endIndex = (startIndex + UNIFI_MAX_CONNECTIONS -1) % UNIFI_MAX_CONNECTIONS;
+
+    while(startIndex != endIndex) {
+        staInfo =  CsrWifiRouterCtrlGetStationRecordFromHandle(priv,startIndex,interfaceTag);
+        if(!staInfo) {
+            startIndex ++;
+            if(startIndex >= UNIFI_MAX_CONNECTIONS) {
+                startIndex = 0;
+            }
+            continue;
+        } else if((staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE)
+                   &&(staInfo->uapsdActive == FALSE)) {
+            startIndex ++;
+            if(startIndex >= UNIFI_MAX_CONNECTIONS) {
+                startIndex = 0;
+            }
+            continue;
+        }
+        /* Peer is active or U-APSD is active so send PDUs to the peer */
+        unifi_trace(priv,UDBG2,"uf_send_buffered_frames : trying data from queue=%d\n",queue);
+
+
+        if((staInfo != NULL)&&(staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE)
+                           &&(staInfo->uapsdActive == FALSE)) {
+           if(!list_empty(&staInfo->dataPdu[queue])) {
+
+               /*Non-UAPSD case push the AC frames out*/
+               uf_send_buffered_data_from_ac(priv, staInfo, queue, (&staInfo->dataPdu[queue]));
+           }
+        }
+        startIndex ++;
+        if(startIndex >= UNIFI_MAX_CONNECTIONS) {
+           startIndex = 0;
+        }
+    }
+    if(isRouterBufferEnabled(priv,queue)) {
+        priv->pausedStaHandle[queue] = endIndex;
+    } else {
+        priv->pausedStaHandle[queue] = 0;
+    }
+
+    /* U-APSD might have stopped because of ENOSPC in lib_hip (pause activity).
+     * So restart it if U-APSD was active with any of the station
+     */
+    unifi_trace(priv, UDBG4, "csrWifiHipSendBufferedFrames: UAPSD Resume Q=%x\n", queue);
+    resume_suspended_uapsd(priv, interfaceTag);
+    func_exit();
+}
+
+
+u8 uf_is_more_data_for_non_delivery_ac(CsrWifiRouterCtrlStaInfo_t *staRecord)
+{
+    u8 i;
+
+    for(i=0;i<=3;i++)
+    {
+        if(((staRecord->powersaveMode[i]==CSR_WIFI_AC_TRIGGER_ONLY_ENABLED)
+                ||(staRecord->powersaveMode[i]==CSR_WIFI_AC_LEGACY_POWER_SAVE))
+                &&(!list_empty(&staRecord->dataPdu[i]))){
+
+         return TRUE;
+        }
+    }
+
+    if(((staRecord->powersaveMode[UNIFI_TRAFFIC_Q_VO]==CSR_WIFI_AC_TRIGGER_ONLY_ENABLED)
+            ||(staRecord->powersaveMode[UNIFI_TRAFFIC_Q_VO]==CSR_WIFI_AC_LEGACY_POWER_SAVE))
+            &&(!list_empty(&staRecord->mgtFrames))){
+
+     return TRUE;
+    }
+
+
+
+    return FALSE;
+}
+
+
+int uf_process_station_records_for_sending_data(unifi_priv_t *priv,u16 interfaceTag,
+                                                 CsrWifiRouterCtrlStaInfo_t *srcStaInfo,
+                                                 CsrWifiRouterCtrlStaInfo_t *dstStaInfo)
+{
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    unifi_trace(priv, UDBG5, "entering uf_process_station_records_for_sending_data\n");
+
+    if (srcStaInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_DISCONNECTED) {
+        unifi_error(priv, "Peer State not connected AID = %x, handle = %x, control port state = %x\n",
+                    srcStaInfo->aid, srcStaInfo->assignedHandle, srcStaInfo->peerControlledPort->port_action);
+        return -1;
+    }
+    switch (interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+            unifi_trace(priv, UDBG5, "mode is AP/P2PGO\n");
+            break;
+        default:
+            unifi_warning(priv, "mode is nor AP neither P2PGO, packet cant be xmit\n");
+            return -1;
+    }
+
+    switch(dstStaInfo->peerControlledPort->port_action)
+    {
+        case CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD:
+        case CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_BLOCK:
+            unifi_trace(priv, UDBG5, "destination port is closed/blocked, discarding the packet\n");
+            return -1;
+        default:
+            unifi_trace(priv, UDBG5, "destination port state is open\n");
+    }
+
+    /* port state is open, destination station record is valid, Power save state is
+     * validated in uf_process_ma_packet_req function
+     */
+    unifi_trace(priv, UDBG5, "leaving uf_process_station_records_for_sending_data\n");
+    return 0;
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_handle_uspframes_delivery
+ *
+ *      This function takes care of handling USP session for peer, when
+ *      -> trigger frame from peer
+ *      -> suspended USP to be processed (resumed)
+ *
+ *      NOTE: uf_send_buffered_data_from_delivery_ac() always called from this function, Dont
+ *      make a direct call to uf_send_buffered_data_from_delivery_ac() from any other part of
+ *      code
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      staInfo         peer for which UAPSD to be scheduled
+ *      interfaceTag    virtual interface tag
+ * ---------------------------------------------------------------------------
+ */
+static void uf_handle_uspframes_delivery(unifi_priv_t * priv, CsrWifiRouterCtrlStaInfo_t *staInfo, u16 interfaceTag)
+{
+
+    s8 i;
+    u8 allDeliveryEnabled = 0, dataAvailable = 0;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG2, " ++ uf_handle_uspframes_delivery, uapsd active=%x, suspended?=%x\n",
+                staInfo->uapsdActive, staInfo->uspSuspend);
+
+    /* Check for Buffered frames according to priority order & deliver it
+     *  1. AC_VO delivery enable & Mgt frames available
+     *  2. Process remaining Ac's from order AC_VO to AC_BK
+     */
+
+    /* USP initiated by WMMPS enabled peer  & SET the status flag to TRUE */
+    if (!staInfo->uspSuspend && staInfo->uapsdActive)
+    {
+        unifi_notice(priv, "uf_handle_uspframes_delivery: U-APSD already active! STA=%x:%x:%x:%x:%x:%x\n",
+                staInfo->peerMacAddress.a[0], staInfo->peerMacAddress.a[1],
+                staInfo->peerMacAddress.a[2], staInfo->peerMacAddress.a[3],
+                staInfo->peerMacAddress.a[4], staInfo->peerMacAddress.a[5]);
+        return;
+    }
+
+    spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+    staInfo->uapsdActive = TRUE;
+    staInfo->uspSuspend = FALSE;
+    spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+    if(((staInfo->powersaveMode[UNIFI_TRAFFIC_Q_VO]==CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED)||
+        (staInfo->powersaveMode[UNIFI_TRAFFIC_Q_VO]==CSR_WIFI_AC_DELIVERY_ONLY_ENABLE))
+        && (!list_empty(&staInfo->mgtFrames))) {
+
+         /* Management queue has data &&  UNIFI_TRAFFIC_Q_VO is delivery enable */
+        unifi_trace(priv, UDBG4, "uf_handle_uspframes_delivery: Sending buffered management frames\n");
+        uf_send_buffered_data_from_delivery_ac(priv, staInfo, UNIFI_TRAFFIC_Q_VO, &staInfo->mgtFrames);
+    }
+
+    if (!uf_is_more_data_for_delivery_ac(priv, staInfo)) {
+        /* All delivery enable AC's are empty, so QNULL to be sent to terminate the USP
+         * NOTE: If we have sent Mgt frame also, we must send QNULL followed to terminate USP
+         */
+        if (!staInfo->uspSuspend) {
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            staInfo->uapsdActive = FALSE;
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+            unifi_trace(priv, UDBG2, "uf_handle_uspframes_delivery: sending QNull for trigger\n");
+            uf_send_qos_null(priv, interfaceTag, staInfo->peerMacAddress.a, (CSR_PRIORITY) staInfo->triggerFramePriority, staInfo);
+            staInfo->triggerFramePriority = CSR_QOS_UP0;
+        } else {
+            unifi_trace(priv, UDBG2, "uf_handle_uspframes_delivery: MgtQ xfer suspended\n");
+        }
+    } else {
+        for(i = UNIFI_TRAFFIC_Q_VO; i >= UNIFI_TRAFFIC_Q_BK; i--) {
+            if(((staInfo->powersaveMode[i]==CSR_WIFI_AC_DELIVERY_ONLY_ENABLE)
+                ||(staInfo->powersaveMode[i]==CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED))
+                && (!list_empty(&staInfo->dataPdu[i]))) {
+                /* Deliver Data according to AC priority (from VO to BK) as part of USP */
+                unifi_trace(priv, UDBG4, "uf_handle_uspframes_delivery: Buffered data frames from Queue (%d) for USP\n", i);
+                uf_send_buffered_data_from_delivery_ac(priv, staInfo, i, &staInfo->dataPdu[i]);
+            }
+
+            if ((!staInfo->uapsdActive) ||
+                    (staInfo->uspSuspend && IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag))) {
+                /* If DTIM active found on one AC, No need to parse the remaining AC's
+                 * as USP suspended. Break out of loop
+                 */
+                unifi_trace(priv, UDBG2, "uf_handle_uspframes_delivery: suspend=%x,  DTIM=%x, USP terminated=%s\n",
+                           staInfo->uspSuspend, IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag),
+                           staInfo->uapsdActive?"NO":"YES");
+                break;
+            }
+        }
+    }
+
+    /* Depending on the USP status, update the TIM accordingly for delivery enabled AC only
+     * (since we are not manipulating any Non-delivery list(AC))
+     */
+    is_all_ac_deliver_enabled_and_moredata(staInfo, &allDeliveryEnabled, &dataAvailable);
+    if ((allDeliveryEnabled && !dataAvailable)) {
+        if ((staInfo->timSet != CSR_WIFI_TIM_RESET) || (staInfo->timSet != CSR_WIFI_TIM_RESETTING)) {
+            staInfo->updateTimReqQueued = (u8) CSR_WIFI_TIM_RESET;
+            unifi_trace(priv, UDBG4, " --uf_handle_uspframes_delivery, UAPSD timset\n");
+            if (!staInfo->timRequestPendingFlag) {
+                update_tim(priv, staInfo->aid, 0, interfaceTag, staInfo->assignedHandle);
+            }
+        }
+    }
+    unifi_trace(priv, UDBG2, " --uf_handle_uspframes_delivery, uapsd active=%x, suspend?=%x\n",
+                staInfo->uapsdActive, staInfo->uspSuspend);
+}
+
+void uf_process_wmm_deliver_ac_uapsd(unifi_priv_t * priv,
+                                     CsrWifiRouterCtrlStaInfo_t * srcStaInfo,
+                                     u16 qosControl,
+                                     u16 interfaceTag)
+{
+    CSR_PRIORITY priority;
+    unifi_TrafficQueue priority_q;
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG2, "++uf_process_wmm_deliver_ac_uapsd: uapsdactive?=%x\n", srcStaInfo->uapsdActive);
+    /* If recceived Frames trigger Frame and Devlivery enabled AC has data
+     * then transmit from High priorty delivery enabled AC
+     */
+    priority = (CSR_PRIORITY)(qosControl & IEEE802_11_QC_TID_MASK);
+    priority_q = unifi_frame_priority_to_queue((CSR_PRIORITY) priority);
+
+    if((srcStaInfo->powersaveMode[priority_q]==CSR_WIFI_AC_TRIGGER_ONLY_ENABLED)
+        ||(srcStaInfo->powersaveMode[priority_q]==CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED)) {
+        spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+        srcStaInfo->triggerFramePriority = priority;
+        spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        unifi_trace(priv, UDBG2, "uf_process_wmm_deliver_ac_uapsd: trigger frame, Begin U-APSD, triggerQ=%x\n", priority_q);
+        uf_handle_uspframes_delivery(priv, srcStaInfo, interfaceTag);
+    }
+    unifi_trace(priv, UDBG2, "--uf_process_wmm_deliver_ac_uapsd: uapsdactive?=%x\n", srcStaInfo->uapsdActive);
+}
+
+
+void uf_send_qos_null(unifi_priv_t * priv,u16 interfaceTag, const u8 *da,CSR_PRIORITY priority,CsrWifiRouterCtrlStaInfo_t * srcStaInfo)
+{
+    bulk_data_param_t bulkdata;
+    CsrResult csrResult;
+    struct sk_buff *skb, *newSkb = NULL;
+    CsrWifiMacAddress peerAddress;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    CSR_TRANSMISSION_CONTROL transmissionControl = (TRANSMISSION_CONTROL_EOSP_MASK | TRANSMISSION_CONTROL_TRIGGER_MASK);
+    int r;
+    CSR_SIGNAL signal;
+    u32 priority_q;
+    CSR_RATE transmitRate = 0;
+
+
+    func_enter();
+    /* Send a Null Frame to Peer,
+     * 32= size of mac header  */
+    csrResult = unifi_net_data_malloc(priv, &bulkdata.d[0], MAC_HEADER_SIZE + QOS_CONTROL_HEADER_SIZE);
+
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, " failed to allocate request_data. in uf_send_qos_null func\n");
+        return ;
+    }
+    skb = (struct sk_buff *)(bulkdata.d[0].os_net_buf_ptr);
+    skb->len = 0;
+    bulkdata.d[0].os_data_ptr = skb->data;
+    bulkdata.d[0].os_net_buf_ptr = (unsigned char*)skb;
+    bulkdata.d[0].net_buf_length = bulkdata.d[0].data_length = skb->len;
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].os_net_buf_ptr = NULL;
+    bulkdata.d[1].net_buf_length = bulkdata.d[1].data_length = 0;
+
+    /* For null frames protection bit should not be set in MAC header, so passing value 0 below for protection field */
+
+    if (prepare_and_add_macheader(priv, skb, newSkb, priority, &bulkdata, interfaceTag, da, interfacePriv->bssid.a, 0)) {
+        unifi_error(priv, "failed to create MAC header\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return;
+    }
+    memcpy(peerAddress.a, ((u8 *) bulkdata.d[0].os_data_ptr) + 4, ETH_ALEN);
+    /* convert priority to queue */
+    priority_q = unifi_frame_priority_to_queue((CSR_PRIORITY) priority);
+
+    /* Frame ma-packet.req, this is saved/transmitted depend on queue state
+     * send the null frame at data rate of 1 Mb/s for AP or 6 Mb/s for P2PGO
+     */
+    switch (interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+            transmitRate = 2;
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            transmitRate = 12;
+            break;
+        default:
+            transmitRate = 0;
+    }
+    unifi_frame_ma_packet_req(priv, priority, transmitRate, 0xffffffff, interfaceTag,
+                              transmissionControl, priv->netdev_client->sender_id,
+                              peerAddress.a, &signal);
+
+    r = ul_send_signal_unpacked(priv, &signal, &bulkdata);
+    if(r) {
+        unifi_error(priv, "failed to send QOS data null packet result: %d\n",r);
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+    }
+
+    func_exit();
+    return;
+
+}
+void uf_send_nulldata(unifi_priv_t * priv,u16 interfaceTag, const u8 *da,CSR_PRIORITY priority,CsrWifiRouterCtrlStaInfo_t * srcStaInfo)
+{
+    bulk_data_param_t bulkdata;
+    CsrResult csrResult;
+    struct sk_buff *skb, *newSkb = NULL;
+    CsrWifiMacAddress peerAddress;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    CSR_TRANSMISSION_CONTROL transmissionControl = 0;
+    int r;
+    CSR_SIGNAL signal;
+    u32 priority_q;
+    CSR_RATE transmitRate = 0;
+    CSR_MA_PACKET_REQUEST *req = &signal.u.MaPacketRequest;
+    unsigned long lock_flags;
+
+    func_enter();
+    /* Send a Null Frame to Peer, size = 24 for MAC header */
+    csrResult = unifi_net_data_malloc(priv, &bulkdata.d[0], MAC_HEADER_SIZE);
+
+    if (csrResult != CSR_RESULT_SUCCESS) {
+        unifi_error(priv, "uf_send_nulldata: Failed to allocate memory for NULL frame\n");
+        return ;
+    }
+    skb = (struct sk_buff *)(bulkdata.d[0].os_net_buf_ptr);
+    skb->len = 0;
+    bulkdata.d[0].os_data_ptr = skb->data;
+    bulkdata.d[0].os_net_buf_ptr = (unsigned char*)skb;
+    bulkdata.d[0].net_buf_length = bulkdata.d[0].data_length = skb->len;
+    bulkdata.d[1].os_data_ptr = NULL;
+    bulkdata.d[1].os_net_buf_ptr = NULL;
+    bulkdata.d[1].net_buf_length = bulkdata.d[1].data_length = 0;
+
+    /* For null frames protection bit should not be set in MAC header, so passing value 0 below for protection field */
+    if (prepare_and_add_macheader(priv, skb, newSkb, priority, &bulkdata, interfaceTag, da, interfacePriv->bssid.a, 0)) {
+        unifi_error(priv, "uf_send_nulldata: Failed to create MAC header\n");
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        return;
+    }
+    memcpy(peerAddress.a, ((u8 *) bulkdata.d[0].os_data_ptr) + 4, ETH_ALEN);
+    /* convert priority to queue */
+    priority_q = unifi_frame_priority_to_queue((CSR_PRIORITY) priority);
+    transmissionControl &= ~(CSR_NO_CONFIRM_REQUIRED);
+
+    /* Frame ma-packet.req, this is saved/transmitted depend on queue state
+     * send the null frame at data rate of 1 Mb/s for AP or 6 Mb/s for P2PGO
+     */
+    switch (interfacePriv->interfaceMode)
+    {
+        case CSR_WIFI_ROUTER_CTRL_MODE_AP:
+            transmitRate = 2;
+            break;
+        case CSR_WIFI_ROUTER_CTRL_MODE_P2PGO:
+            transmitRate = 12;
+            break;
+        default:
+            transmitRate = 0;
+    }
+    unifi_frame_ma_packet_req(priv, priority, transmitRate, INVALID_HOST_TAG, interfaceTag,
+                              transmissionControl, priv->netdev_client->sender_id,
+                              peerAddress.a, &signal);
+
+    /* Save host tag to check the status on reception of MA packet confirm */
+    srcStaInfo->nullDataHostTag = req->HostTag;
+    unifi_trace(priv, UDBG1, "uf_send_nulldata: STA AID = %d hostTag = %x\n", srcStaInfo->aid, req->HostTag);
+
+    r = ul_send_signal_unpacked(priv, &signal, &bulkdata);
+
+    if(r == -ENOSPC) {
+        unifi_trace(priv, UDBG1, "uf_send_nulldata: ENOSPC Requeue the Null frame\n");
+        enque_tx_data_pdu(priv, &bulkdata, &srcStaInfo->dataPdu[priority_q], &signal, 1);
+        spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+        srcStaInfo->noOfPktQueued++;
+        spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+
+    }
+    if(r && r != -ENOSPC){
+        unifi_error(priv, "uf_send_nulldata: Failed to send Null frame Error = %d\n",r);
+        unifi_net_data_free(priv, &bulkdata.d[0]);
+        srcStaInfo->nullDataHostTag = INVALID_HOST_TAG;
+    }
+
+    func_exit();
+    return;
+}
+
+u8 uf_check_broadcast_bssid(unifi_priv_t *priv, const bulk_data_param_t *bulkdata)
+{
+    u8 *bssid = NULL;
+    static const CsrWifiMacAddress broadcast_address = {{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}};
+    u8 toDs, fromDs;
+
+    toDs = (((bulkdata->d[0].os_data_ptr)[1]) & 0x01) ? 1 : 0;
+    fromDs =(((bulkdata->d[0].os_data_ptr)[1]) & 0x02) ? 1 : 0;
+
+     if (toDs && fromDs)
+    {
+        unifi_trace(priv, UDBG6, "Address 4 present, Don't try to find BSSID\n");
+        bssid = NULL;
+    }
+    else if((toDs == 0) && (fromDs ==0))
+    {
+        /* BSSID is Address 3 */
+        bssid = (u8 *) (bulkdata->d[0].os_data_ptr + 4 + (2 * ETH_ALEN));
+    }
+    else if(toDs)
+    {
+        /* BSSID is Address 1 */
+        bssid = (u8 *) (bulkdata->d[0].os_data_ptr + 4);
+    }
+    else if(fromDs)
+    {
+        /* BSSID is Address 2 */
+        bssid = (u8 *) (bulkdata->d[0].os_data_ptr + 4 + ETH_ALEN);
+    }
+
+    if (memcmp(broadcast_address.a, bssid, ETH_ALEN)== 0)
+    {
+        return TRUE;
+    }
+    else
+    {
+        return FALSE;
+    }
+}
+
+
+u8 uf_process_pm_bit_for_peer(unifi_priv_t * priv, CsrWifiRouterCtrlStaInfo_t * srcStaInfo,
+                                u8 pmBit,u16 interfaceTag)
+{
+    u8 moreData = FALSE;
+    u8 powerSaveChanged = FALSE;
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG3, "entering uf_process_pm_bit_for_peer\n");
+    if (pmBit) {
+        priv->allPeerDozing |= (0x01 << (srcStaInfo->assignedHandle));
+    } else {
+        priv->allPeerDozing &= ~(0x01 << (srcStaInfo->assignedHandle));
+    }
+    if(pmBit) {
+        if(srcStaInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE) {
+
+            /* disable the preemption */
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            srcStaInfo->currentPeerState =CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE;
+            powerSaveChanged = TRUE;
+            /* enable the preemption */
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        } else {
+            return powerSaveChanged;
+        }
+    } else {
+        if(srcStaInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE) {
+            /* disable the preemption */
+            spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+            srcStaInfo->currentPeerState = CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE;
+            powerSaveChanged = TRUE;
+            /* enable the preemption */
+            spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+        }else {
+            return powerSaveChanged;
+        }
+    }
+
+
+    if(srcStaInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE) {
+        unifi_trace(priv,UDBG3, "Peer with AID = %d is active now\n",srcStaInfo->aid);
+        process_peer_active_transition(priv,srcStaInfo,interfaceTag);
+    } else {
+        unifi_trace(priv,UDBG3, "Peer with AID = %d is in PS Now\n",srcStaInfo->aid);
+        /* Set TIM if needed */
+        if(!srcStaInfo->wmmOrQosEnabled) {
+            moreData = (!list_empty(&srcStaInfo->mgtFrames) ||
+                        !list_empty(&srcStaInfo->dataPdu[UNIFI_TRAFFIC_Q_VO])||
+                        !list_empty(&srcStaInfo->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]));
+            if(moreData && (srcStaInfo->timSet == CSR_WIFI_TIM_RESET)) {
+                unifi_trace(priv, UDBG3, "This condition should not occur\n");
+                if (!srcStaInfo->timRequestPendingFlag){
+                    update_tim(priv,srcStaInfo->aid,1,interfaceTag, srcStaInfo->assignedHandle);
+                }
+                else
+                {
+                    /* Cache the TimSet value so that it will processed immidiatly after
+                     * completing the current setTim Request
+                     */
+                    srcStaInfo->updateTimReqQueued = 1;
+                    unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", srcStaInfo->updateTimReqQueued,
+                                srcStaInfo->aid);
+                }
+
+            }
+        } else {
+            u8 allDeliveryEnabled = 0, dataAvailable = 0;
+            unifi_trace(priv, UDBG5, "Qos in AP Mode\n");
+            /* Check if all AC's are Delivery Enabled */
+            is_all_ac_deliver_enabled_and_moredata(srcStaInfo, &allDeliveryEnabled, &dataAvailable);
+            /*check for more data in non-delivery enabled queues*/
+            moreData = (uf_is_more_data_for_non_delivery_ac(srcStaInfo) || (allDeliveryEnabled && dataAvailable));
+
+            if(moreData && (srcStaInfo->timSet == CSR_WIFI_TIM_RESET)) {
+                if (!srcStaInfo->timRequestPendingFlag){
+                    update_tim(priv,srcStaInfo->aid,1,interfaceTag, srcStaInfo->assignedHandle);
+                }
+                else
+                {
+                    /* Cache the TimSet value so that it will processed immidiatly after
+                     * completing the current setTim Request
+                     */
+                    srcStaInfo->updateTimReqQueued = 1;
+                    unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", srcStaInfo->updateTimReqQueued,
+                                srcStaInfo->aid);
+                }
+            }
+        }
+    }
+    unifi_trace(priv, UDBG3, "leaving uf_process_pm_bit_for_peer\n");
+    return powerSaveChanged;
+}
+
+
+
+void uf_process_ps_poll(unifi_priv_t *priv,u8* sa,u8* da,u8 pmBit,u16 interfaceTag)
+{
+    CsrWifiRouterCtrlStaInfo_t *staRecord =
+    CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, sa, interfaceTag);
+    tx_buffered_packets_t * buffered_pkt = NULL;
+    CsrWifiMacAddress peerMacAddress;
+    unsigned long lock_flags;
+    s8 r =0;
+    u8 moreData = FALSE;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    unifi_trace(priv, UDBG3, "entering uf_process_ps_poll\n");
+    if(!staRecord) {
+        memcpy(peerMacAddress.a,sa,ETH_ALEN);
+        unifi_trace(priv, UDBG3, "In uf_process_ps_poll, sta record not found:unexpected frame addr = %x:%x:%x:%x:%x:%x\n",
+                sa[0], sa[1],sa[2], sa[3], sa[4],sa[5]);
+        CsrWifiRouterCtrlUnexpectedFrameIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,interfaceTag,peerMacAddress);
+        return;
+    }
+
+    uf_process_pm_bit_for_peer(priv,staRecord,pmBit,interfaceTag);
+
+    /* Update station last activity time */
+    staRecord->activity_flag = TRUE;
+
+    /* This should not change the PM bit as PS-POLL has PM bit always set */
+    if(!pmBit) {
+        unifi_notice (priv," PM bit reset in PS-POLL\n");
+        return;
+    }
+
+    if(IS_DTIM_ACTIVE(interfacePriv->dtimActive,interfacePriv->multicastPduHostTag)) {
+        /* giving more priority to multicast packets so dropping ps-poll*/
+        unifi_notice (priv," multicast transmission is going on so don't take action on PS-POLL\n");
+        return;
+    }
+
+    if(!staRecord->wmmOrQosEnabled) {
+        if((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->mgtFrames))) {
+            buffered_pkt->transmissionControl |= TRANSMISSION_CONTROL_TRIGGER_MASK;
+            moreData = (!list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]) ||
+                        !list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_VO]) ||
+                        !list_empty(&staRecord->mgtFrames));
+
+            buffered_pkt->transmissionControl |= (TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+            if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,moreData,FALSE)) == -ENOSPC) {
+                /* Clear the trigger bit transmission control*/
+                buffered_pkt->transmissionControl &= ~(TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+                /* Enqueue at the head of the queue */
+                spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                list_add(&buffered_pkt->q, &staRecord->mgtFrames);
+                spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                unifi_trace(priv, UDBG1, "(ENOSPC) PS-POLL received : PDU sending failed \n");
+                priv->pausedStaHandle[3]=(u8)(staRecord->assignedHandle);
+            } else {
+                if(r){
+                    unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                    /* the PDU failed where we can't do any thing so free the storage */
+                    unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                }
+                kfree(buffered_pkt);
+            }
+        } else if((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->dataPdu[UNIFI_TRAFFIC_Q_VO]))) {
+            buffered_pkt->transmissionControl |= TRANSMISSION_CONTROL_TRIGGER_MASK;
+            moreData = (!list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]) ||
+                        !list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_VO]));
+
+            buffered_pkt->transmissionControl |= (TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+            if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,moreData,FALSE)) == -ENOSPC) {
+                /* Clear the trigger bit transmission control*/
+                buffered_pkt->transmissionControl &= ~(TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+                /* Enqueue at the head of the queue */
+                spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                list_add(&buffered_pkt->q, &staRecord->dataPdu[UNIFI_TRAFFIC_Q_VO]);
+                spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                priv->pausedStaHandle[3]=(u8)(staRecord->assignedHandle);
+                unifi_trace(priv, UDBG1, "(ENOSPC) PS-POLL received : PDU sending failed \n");
+            } else {
+                if(r){
+                    unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                    /* the PDU failed where we can't do any thing so free the storage */
+                    unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                }
+                kfree(buffered_pkt);
+            }
+        } else  if((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]))) {
+            buffered_pkt->transmissionControl |= TRANSMISSION_CONTROL_TRIGGER_MASK;
+            moreData = !list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]);
+
+            buffered_pkt->transmissionControl |= (TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+            if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,moreData,FALSE)) == -ENOSPC) {
+                /* Clear the trigger bit transmission control*/
+                buffered_pkt->transmissionControl &= ~(TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+                /* Enqueue at the head of the queue */
+                spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                list_add(&buffered_pkt->q, &staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]);
+                spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                priv->pausedStaHandle[0]=(u8)(staRecord->assignedHandle);
+                unifi_trace(priv, UDBG1, "(ENOSPC) PS-POLL received : PDU sending failed \n");
+            } else {
+                if(r){
+                    unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                    /* the PDU failed where we can't do any thing so free the storage */
+                    unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                }
+                kfree(buffered_pkt);
+            }
+        } else {
+         /* Actually since we have sent an ACK, there
+         * there is no need to send a NULL frame*/
+        }
+        moreData = (!list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_VO]) ||
+           !list_empty(&staRecord->dataPdu[UNIFI_TRAFFIC_Q_CONTENTION]) ||
+            !list_empty(&staRecord->mgtFrames));
+        if(!moreData && (staRecord->timSet == CSR_WIFI_TIM_SET)) {
+            unifi_trace(priv, UDBG3, "more data = NULL, set tim to 0 in uf_process_ps_poll\n");
+            if (!staRecord->timRequestPendingFlag){
+                update_tim(priv,staRecord->aid,0,interfaceTag, staRecord->assignedHandle);
+            }
+            else
+            {
+                /* Cache the TimSet value so that it will processed immidiatly after
+                 * completing the current setTim Request
+                 */
+                staRecord->updateTimReqQueued = 0;
+                unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                            staRecord->aid);
+            }
+        }
+    } else {
+
+        u8 allDeliveryEnabled = 0, dataAvailable = 0;
+        unifi_trace(priv, UDBG3,"Qos Support station.Processing PS-Poll\n");
+
+        /*Send Data From Management Frames*/
+        /* Priority orders for delivering the buffered packets are
+         * 1. Deliver the Management frames if there
+         * 2. Other access catagory frames which are non deliver enable including UNIFI_TRAFFIC_Q_VO
+         * priority is from VO->BK
+         */
+
+        /* Check if all AC's are Delivery Enabled */
+        is_all_ac_deliver_enabled_and_moredata(staRecord, &allDeliveryEnabled, &dataAvailable);
+
+        if (allDeliveryEnabled) {
+            unifi_trace(priv, UDBG3, "uf_process_ps_poll: All ACs are delivery enable so Sending QOS Null in response of Ps-poll\n");
+            uf_send_qos_null(priv,interfaceTag,sa,CSR_QOS_UP0,staRecord);
+            return;
+        }
+
+        if (!list_empty(&staRecord->mgtFrames)) {
+             if ((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->mgtFrames))) {
+                    /* We dont have packets in non delivery enabled UNIFI_TRAFFIC_Q_VO, So we are looking in management
+                     * queue of the station record
+                     */
+                    moreData = uf_is_more_data_for_non_delivery_ac(staRecord);
+                    buffered_pkt->transmissionControl |= (TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+
+                    /* Last parameter is EOSP & its false always for PS-POLL processing */
+                    if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,moreData,FALSE)) == -ENOSPC) {
+                        /* Clear the trigger bit transmission control*/
+                        buffered_pkt->transmissionControl &= ~(TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+                        /* Enqueue at the head of the queue */
+                        spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                        list_add(&buffered_pkt->q, &staRecord->mgtFrames);
+                        spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                        priv->pausedStaHandle[0]=(u8)(staRecord->assignedHandle);
+                        unifi_trace(priv, UDBG1, "(ENOSPC) PS-POLL received : PDU sending failed \n");
+                    } else {
+                        if(r){
+                            unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                            /* the PDU failed where we can't do any thing so free the storage */
+                            unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                        }
+                        kfree(buffered_pkt);
+                    }
+                } else {
+                    unifi_error(priv, "uf_process_ps_poll: Mgt frame list empty!! \n");
+                }
+
+        } else {
+            s8 i;
+            /* We dont have buffered packet in mangement frame queue (1 failed), So proceed with condition 2
+             * UNIFI_TRAFFIC_Q_VO -> VI -> BE -> BK
+             */
+            for(i= 3; i>=0; i--) {
+                if (!IS_DELIVERY_ENABLED(staRecord->powersaveMode[i])) {
+                    /* Send One packet, if queue is NULL then continue */
+                    if((buffered_pkt=dequeue_tx_data_pdu(priv, &staRecord->dataPdu[i]))) {
+                        moreData = uf_is_more_data_for_non_delivery_ac(staRecord);
+
+                        buffered_pkt->transmissionControl |= (TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+
+                        /* Last parameter is EOSP & its false always for PS-POLL processing */
+                        if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staRecord,moreData,FALSE)) == -ENOSPC) {
+                            /* Clear the trigger bit transmission control*/
+                            buffered_pkt->transmissionControl &= ~(TRANSMISSION_CONTROL_TRIGGER_MASK | TRANSMISSION_CONTROL_EOSP_MASK);
+                            /* Enqueue at the head of the queue */
+                            spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                            list_add(&buffered_pkt->q, &staRecord->dataPdu[i]);
+                            spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                            priv->pausedStaHandle[0]=(u8)(staRecord->assignedHandle);
+                            unifi_trace(priv, UDBG1, "(ENOSPC) PS-POLL received : PDU sending failed \n");
+                        } else {
+                            if(r) {
+                                unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                                /* the PDU failed where we can't do any thing so free the storage */
+                                unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                            }
+                            kfree(buffered_pkt);
+                        }
+                        break;
+                    }
+                }
+            }
+        }
+        /* Check if all AC's are Delivery Enabled */
+        is_all_ac_deliver_enabled_and_moredata(staRecord, &allDeliveryEnabled, &dataAvailable);
+        /*check for more data in non-delivery enabled queues*/
+        moreData = (uf_is_more_data_for_non_delivery_ac(staRecord) || (allDeliveryEnabled && dataAvailable));
+        if(!moreData && (staRecord->timSet == CSR_WIFI_TIM_SET)) {
+            unifi_trace(priv, UDBG3, "more data = NULL, set tim to 0 in uf_process_ps_poll\n");
+            if (!staRecord->timRequestPendingFlag){
+                update_tim(priv,staRecord->aid,0,interfaceTag, staRecord->assignedHandle);
+            }
+            else
+            {
+                /* Cache the TimSet value so that it will processed immidiatly after
+                 * completing the current setTim Request
+                 */
+                staRecord->updateTimReqQueued = 0;
+                unifi_trace(priv, UDBG6, "update_tim : One more UpdateTim Request (Tim value:%d) Queued for AID %x\n", staRecord->updateTimReqQueued,
+                            staRecord->aid);
+            }
+
+        }
+    }
+
+        unifi_trace(priv, UDBG3, "leaving uf_process_ps_poll\n");
+}
+
+
+
+void add_to_send_cfm_list(unifi_priv_t * priv,
+                          tx_buffered_packets_t *tx_q_item,
+                          struct list_head *frames_need_cfm_list)
+{
+    tx_buffered_packets_t *send_cfm_list_item = NULL;
+
+    send_cfm_list_item = (tx_buffered_packets_t *) kmalloc(sizeof(tx_buffered_packets_t), GFP_ATOMIC);
+
+    if(send_cfm_list_item == NULL){
+        unifi_warning(priv, "%s: Failed to allocate memory for new list item \n");
+        return;
+    }
+
+    INIT_LIST_HEAD(&send_cfm_list_item->q);
+
+    send_cfm_list_item->hostTag = tx_q_item->hostTag;
+    send_cfm_list_item->interfaceTag = tx_q_item->interfaceTag;
+    send_cfm_list_item->transmissionControl = tx_q_item->transmissionControl;
+    send_cfm_list_item->leSenderProcessId = tx_q_item->leSenderProcessId;
+    send_cfm_list_item->rate = tx_q_item->rate;
+    memcpy(send_cfm_list_item->peerMacAddress.a, tx_q_item->peerMacAddress.a, ETH_ALEN);
+    send_cfm_list_item->priority = tx_q_item->priority;
+
+    list_add_tail(&send_cfm_list_item->q, frames_need_cfm_list);
+}
+
+void uf_prepare_send_cfm_list_for_queued_pkts(unifi_priv_t * priv,
+                                                 struct list_head *frames_need_cfm_list,
+                                                 struct list_head * list)
+{
+    tx_buffered_packets_t *tx_q_item = NULL;
+    struct list_head *listHead;
+    struct list_head *placeHolder;
+    unsigned long lock_flags;
+
+    func_enter();
+
+    spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+
+    /* Search through the list and if confirmation required for any frames,
+    add it to the send_cfm list */
+    list_for_each_safe(listHead, placeHolder, list) {
+        tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+
+        if(!tx_q_item) {
+            unifi_error(priv, "Entry should exist, otherwise it is a (BUG)\n");
+            continue;
+        }
+
+        /* check if confirmation is requested and if the sender ID
+        is not netdevice client then save the entry in the list for need cfms */
+        if (!(tx_q_item->transmissionControl & CSR_NO_CONFIRM_REQUIRED) &&
+            (tx_q_item->leSenderProcessId != priv->netdev_client->sender_id)){
+             unifi_trace(priv, UDBG1, "%s: SenderProcessID=%x host tag=%x transmission control=%x\n",
+                __FUNCTION__,
+                tx_q_item->leSenderProcessId,
+                tx_q_item->hostTag,
+                tx_q_item->transmissionControl);
+
+             add_to_send_cfm_list(priv, tx_q_item, frames_need_cfm_list);
+        }
+    }
+
+    spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+
+    func_exit();
+}
+
+
+
+void uf_flush_list(unifi_priv_t * priv, struct list_head * list)
+{
+    tx_buffered_packets_t *tx_q_item;
+    struct list_head *listHead;
+    struct list_head *placeHolder;
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG5, "entering the uf_flush_list \n");
+
+    spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+    /* go through list, delete & free memory */
+    list_for_each_safe(listHead, placeHolder, list) {
+        tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+
+        if(!tx_q_item) {
+            unifi_error(priv, "entry should exists, otherwise crashes (bug)\n");
+        }
+        unifi_trace(priv, UDBG5,
+                "proccess_tx:  in uf_flush_list peerMacAddress=%02X%02X%02X%02X%02X%02X senderProcessId=%x\n",
+                tx_q_item->peerMacAddress.a[0], tx_q_item->peerMacAddress.a[1],
+                tx_q_item->peerMacAddress.a[2], tx_q_item->peerMacAddress.a[3],
+                tx_q_item->peerMacAddress.a[4], tx_q_item->peerMacAddress.a[5],
+                tx_q_item->leSenderProcessId);
+
+        list_del(listHead);
+        /* free the allocated memory */
+        unifi_net_data_free(priv, &tx_q_item->bulkdata);
+        kfree(tx_q_item);
+        tx_q_item = NULL;
+        if (!priv->noOfPktQueuedInDriver) {
+            unifi_error(priv, "packets queued in driver 0 still decrementing in %s\n", __FUNCTION__);
+        } else {
+            priv->noOfPktQueuedInDriver--;
+        }
+    }
+    spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+}
+
+tx_buffered_packets_t *dequeue_tx_data_pdu(unifi_priv_t *priv, struct list_head *txList)
+{
+    /* dequeue the tx data packets from the appropriate queue */
+    tx_buffered_packets_t *tx_q_item = NULL;
+    struct list_head *listHead;
+    struct list_head *placeHolder;
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG5, "entering dequeue_tx_data_pdu\n");
+    /* check for list empty */
+    if (list_empty(txList)) {
+        unifi_trace(priv, UDBG5, "In dequeue_tx_data_pdu, the list is empty\n");
+        return NULL;
+    }
+
+    /* Verification, if packet count is negetive */
+    if (priv->noOfPktQueuedInDriver == 0xFFFF) {
+        unifi_warning(priv, "no packet available in queue: debug");
+        return NULL;
+    }
+
+    /* return first node after header, & delete from the list  && atleast one item exist */
+    spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+    list_for_each_safe(listHead, placeHolder, txList) {
+        tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+        list_del(listHead);
+        break;
+    }
+    spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+
+    if (tx_q_item) {
+        unifi_trace(priv, UDBG5,
+                "proccess_tx:  In dequeue_tx_data_pdu peerMacAddress=%02X%02X%02X%02X%02X%02X senderProcessId=%x\n",
+                tx_q_item->peerMacAddress.a[0], tx_q_item->peerMacAddress.a[1],
+                tx_q_item->peerMacAddress.a[2], tx_q_item->peerMacAddress.a[3],
+                tx_q_item->peerMacAddress.a[4], tx_q_item->peerMacAddress.a[5],
+                tx_q_item->leSenderProcessId);
+    }
+
+    unifi_trace(priv, UDBG5, "leaving dequeue_tx_data_pdu\n");
+    return tx_q_item;
+}
+/* generic function to get the station record handler */
+CsrWifiRouterCtrlStaInfo_t *CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(unifi_priv_t *priv,
+        const u8 *peerMacAddress,
+        u16 interfaceTag)
+{
+    u8 i;
+    netInterface_priv_t *interfacePriv;
+    unsigned long lock_flags;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "interfaceTag is not proper, interfaceTag = %d\n", interfaceTag);
+        return NULL;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    /* disable the preemption untill station record is fetched */
+    spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+
+    for (i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        if (interfacePriv->staInfo[i]!= NULL) {
+            if (!memcmp(((CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[i]))->peerMacAddress.a, peerMacAddress, ETH_ALEN)) {
+                /* enable the preemption as station record is fetched */
+                spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+                unifi_trace(priv, UDBG5, "peer entry found in station record\n");
+                return ((CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[i]));
+            }
+        }
+    }
+    /* enable the preemption as station record is fetched */
+    spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+    unifi_trace(priv, UDBG5, "peer entry not found in station record\n");
+    return NULL;
+}
+/* generic function to get the station record handler from the handle */
+CsrWifiRouterCtrlStaInfo_t * CsrWifiRouterCtrlGetStationRecordFromHandle(unifi_priv_t *priv,
+                                                                 u32 handle,
+                                                                 u16 interfaceTag)
+{
+    netInterface_priv_t *interfacePriv;
+
+    if ((handle >= UNIFI_MAX_CONNECTIONS) || (interfaceTag >= CSR_WIFI_NUM_INTERFACES)) {
+        unifi_error(priv, "handle/interfaceTag is not proper, handle = %d, interfaceTag = %d\n", handle, interfaceTag);
+        return NULL;
+    }
+    interfacePriv = priv->interfacePriv[interfaceTag];
+    return ((CsrWifiRouterCtrlStaInfo_t *) (interfacePriv->staInfo[handle]));
+}
+
+/* Function to do inactivity */
+void uf_check_inactivity(unifi_priv_t *priv, u16 interfaceTag, CsrTime currentTime)
+{
+    u32 i;
+    CsrWifiRouterCtrlStaInfo_t *staInfo;
+    CsrTime elapsedTime;    /* Time in microseconds */
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    CsrWifiMacAddress peerMacAddress;
+    unsigned long lock_flags;
+
+    if (interfacePriv == NULL) {
+        unifi_trace(priv, UDBG3, "uf_check_inactivity: Interface priv is NULL \n");
+        return;
+    }
+
+    spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+    /* Go through the list of stations to check for inactivity */
+    for(i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        staInfo =  CsrWifiRouterCtrlGetStationRecordFromHandle(priv, i, interfaceTag);
+        if(!staInfo ) {
+            continue;
+        }
+
+        unifi_trace(priv, UDBG3, "Running Inactivity handler Time %xus station's last activity %xus\n",
+                currentTime, staInfo->lastActivity);
+
+
+        elapsedTime = (currentTime >= staInfo->lastActivity)?
+                (currentTime - staInfo->lastActivity):
+                (~((u32)0) - staInfo->lastActivity + currentTime);
+        spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+        if (elapsedTime > MAX_INACTIVITY_INTERVAL) {
+            memcpy((u8*)&peerMacAddress, (u8*)&staInfo->peerMacAddress, sizeof(CsrWifiMacAddress));
+
+            /* Indicate inactivity for the station */
+            unifi_trace(priv, UDBG3, "Station %x:%x:%x:%x:%x:%x inactive since %xus\n sending Inactive Ind\n",
+                        peerMacAddress.a[0], peerMacAddress.a[1],
+                        peerMacAddress.a[2], peerMacAddress.a[3],
+                        peerMacAddress.a[4], peerMacAddress.a[5],
+                        elapsedTime);
+
+            CsrWifiRouterCtrlStaInactiveIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0, interfaceTag, peerMacAddress);
+        }
+    }
+
+    interfacePriv->last_inactivity_check = currentTime;
+}
+
+/* Function to update activity of a station */
+void uf_update_sta_activity(unifi_priv_t *priv, u16 interfaceTag, const u8 *peerMacAddress)
+{
+    CsrTime elapsedTime, currentTime;    /* Time in microseconds */
+    CsrTime timeHi;         /* Not used - Time in microseconds */
+    CsrWifiRouterCtrlStaInfo_t *staInfo;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    unsigned long lock_flags;
+
+    if (interfacePriv == NULL) {
+        unifi_trace(priv, UDBG3, "uf_check_inactivity: Interface priv is NULL \n");
+        return;
+    }
+
+    currentTime = CsrTimeGet(&timeHi);
+
+
+    staInfo = CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv, peerMacAddress, interfaceTag);
+
+    if (staInfo == NULL) {
+        unifi_trace(priv, UDBG4, "Sta does not exist yet");
+        return;
+    }
+
+    spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+    /* Update activity */
+    staInfo->lastActivity = currentTime;
+
+    /* See if inactivity handler needs to be run
+     * Here it is theoretically possible that the counter may have wrapped around. But
+     * since we just want to know when to run the inactivity handler it does not really matter.
+     * Especially since this is data path it makes sense in keeping it simple and avoiding
+     * 64 bit handling */
+    elapsedTime = (currentTime >= interfacePriv->last_inactivity_check)?
+                    (currentTime - interfacePriv->last_inactivity_check):
+                    (~((u32)0) - interfacePriv->last_inactivity_check + currentTime);
+
+    spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+
+    /* Check if it is time to run the inactivity handler */
+    if (elapsedTime > INACTIVITY_CHECK_INTERVAL) {
+        uf_check_inactivity(priv, interfaceTag, currentTime);
+    }
+}
+void resume_unicast_buffered_frames(unifi_priv_t *priv, u16 interfaceTag)
+{
+
+   netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+   u8 i;
+   int j;
+   tx_buffered_packets_t * buffered_pkt = NULL;
+   u8 hipslotFree[4] = {TRUE,TRUE,TRUE,TRUE};
+   int r;
+   unsigned long lock_flags;
+
+   func_enter();
+   while(!isRouterBufferEnabled(priv,3) &&
+                            ((buffered_pkt=dequeue_tx_data_pdu(priv,&interfacePriv->genericMgtFrames))!=NULL)) {
+        buffered_pkt->transmissionControl &=
+                     ~(TRANSMISSION_CONTROL_TRIGGER_MASK|TRANSMISSION_CONTROL_EOSP_MASK);
+        if((r=frame_and_send_queued_pdu(priv,buffered_pkt,NULL,0,FALSE)) == -ENOSPC) {
+            /* Enqueue at the head of the queue */
+            spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+            list_add(&buffered_pkt->q, &interfacePriv->genericMgtFrames);
+            spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+            hipslotFree[3]=FALSE;
+            break;
+        }else {
+            if(r){
+                unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                /* the PDU failed where we can't do any thing so free the storage */
+                unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+            }
+            kfree(buffered_pkt);
+        }
+   }
+   for(i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        CsrWifiRouterCtrlStaInfo_t *staInfo = interfacePriv->staInfo[i];
+        if(!hipslotFree[0] && !hipslotFree[1] && !hipslotFree[2] && !hipslotFree[3]) {
+            unifi_trace(priv, UDBG3, "(ENOSPC) in resume_unicast_buffered_frames:: hip slots are full \n");
+            break;
+        }
+        if (staInfo && (staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE)) {
+          while((( TRUE == hipslotFree[3] ) && (buffered_pkt=dequeue_tx_data_pdu(priv, &staInfo->mgtFrames)))) {
+              buffered_pkt->transmissionControl &=
+                           ~(TRANSMISSION_CONTROL_TRIGGER_MASK|TRANSMISSION_CONTROL_EOSP_MASK);
+              if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staInfo,0,FALSE)) == -ENOSPC) {
+                  unifi_trace(priv, UDBG3, "(ENOSPC) in resume_unicast_buffered_frames:: hip slots are full for voice queue\n");
+                  /* Enqueue at the head of the queue */
+                  spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                  list_add(&buffered_pkt->q, &staInfo->mgtFrames);
+                  spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                  priv->pausedStaHandle[3]=(u8)(staInfo->assignedHandle);
+                  hipslotFree[3] = FALSE;
+                  break;
+              } else {
+                  if(r){
+                      unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                      /* the PDU failed where we can't do any thing so free the storage */
+                      unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                  }
+                  kfree(buffered_pkt);
+              }
+          }
+
+          for(j=3;j>=0;j--) {
+              if(!hipslotFree[j])
+                  continue;
+
+              while((buffered_pkt=dequeue_tx_data_pdu(priv, &staInfo->dataPdu[j]))) {
+                 buffered_pkt->transmissionControl &=
+                            ~(TRANSMISSION_CONTROL_TRIGGER_MASK|TRANSMISSION_CONTROL_EOSP_MASK);
+                 if((r=frame_and_send_queued_pdu(priv,buffered_pkt,staInfo,0,FALSE)) == -ENOSPC) {
+                     /* Enqueue at the head of the queue */
+                     spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+                     list_add(&buffered_pkt->q, &staInfo->dataPdu[j]);
+                     spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+                     priv->pausedStaHandle[j]=(u8)(staInfo->assignedHandle);
+                     hipslotFree[j]=FALSE;
+                     break;
+                 } else {
+                    if(r){
+                        unifi_trace (priv, UDBG1, " HIP validation failure : PDU sending failed \n");
+                        /* the PDU failed where we can't do any thing so free the storage */
+                        unifi_net_data_free(priv, &buffered_pkt->bulkdata);
+                     }
+                    kfree(buffered_pkt);
+                 }
+              }
+          }
+       }
+    }
+    func_exit();
+}
+void update_eosp_to_head_of_broadcast_list_head(unifi_priv_t *priv,u16 interfaceTag)
+{
+
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+    unsigned long lock_flags;
+    struct list_head *listHead;
+    struct list_head *placeHolder;
+    tx_buffered_packets_t *tx_q_item;
+
+    func_enter();
+    if (interfacePriv->noOfbroadcastPktQueued) {
+
+        /* Update the EOSP to the HEAD of b/c list
+         * beacuse we have received any mgmt packet so it should not hold for long time
+         * peer may time out.
+         */
+        spin_lock_irqsave(&priv->tx_q_lock,lock_flags);
+        list_for_each_safe(listHead, placeHolder, &interfacePriv->genericMulticastOrBroadCastFrames) {
+            tx_q_item = list_entry(listHead, tx_buffered_packets_t, q);
+            tx_q_item->transmissionControl |= TRANSMISSION_CONTROL_EOSP_MASK;
+            tx_q_item->transmissionControl = (tx_q_item->transmissionControl & ~(CSR_NO_CONFIRM_REQUIRED));
+            unifi_trace(priv, UDBG1,"updating eosp for list Head hostTag:= 0x%x ",tx_q_item->hostTag);
+            break;
+        }
+        spin_unlock_irqrestore(&priv->tx_q_lock,lock_flags);
+    }
+    func_exit();
+}
+
+/*
+ * ---------------------------------------------------------------------------
+ *  resume_suspended_uapsd
+ *
+ *      This function takes care processing packets of Unscheduled Service Period,
+ *      which been suspended earlier due to DTIM/HIP ENOSPC scenarios
+ *
+ *  Arguments:
+ *      priv            Pointer to device private context struct
+ *      interfaceTag    For which resume should happen
+ * ---------------------------------------------------------------------------
+ */
+void resume_suspended_uapsd(unifi_priv_t* priv,u16 interfaceTag)
+{
+
+   u8 startIndex;
+   CsrWifiRouterCtrlStaInfo_t * staInfo = NULL;
+    unsigned long lock_flags;
+
+    unifi_trace(priv, UDBG2, "++resume_suspended_uapsd: \n");
+    for(startIndex= 0; startIndex < UNIFI_MAX_CONNECTIONS;startIndex++) {
+        staInfo =  CsrWifiRouterCtrlGetStationRecordFromHandle(priv,startIndex,interfaceTag);
+
+        if(!staInfo || !staInfo->wmmOrQosEnabled) {
+            continue;
+        } else if((staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_POWER_SAVE)
+                   &&staInfo->uapsdActive && staInfo->uspSuspend) {
+            /* U-APSD Still active & previously suspended either ENOSPC of FH queues OR
+             * due to DTIM activity
+             */
+            uf_handle_uspframes_delivery(priv, staInfo, interfaceTag);
+        } else {
+            unifi_trace(priv, UDBG2, "resume_suspended_uapsd: PS state=%x, uapsdActive?=%x, suspend?=%x\n",
+                        staInfo->currentPeerState, staInfo->uapsdActive, staInfo->uspSuspend);
+            if (staInfo->currentPeerState == CSR_WIFI_ROUTER_CTRL_PEER_CONNECTED_ACTIVE)
+            {
+                spin_lock_irqsave(&priv->staRecord_lock,lock_flags);
+                staInfo->uapsdActive = FALSE;
+                staInfo->uspSuspend = FALSE;
+                spin_unlock_irqrestore(&priv->staRecord_lock,lock_flags);
+            }
+        }
+    }
+    unifi_trace(priv, UDBG2, "--resume_suspended_uapsd:\n");
+}
+
+#endif
diff --git a/drivers/staging/csr/unifi_priv.h b/drivers/staging/csr/unifi_priv.h
new file mode 100644
index 00000000000..6d6b46191a1
--- /dev/null
+++ b/drivers/staging/csr/unifi_priv.h
@@ -0,0 +1,1177 @@
+/*
+ *****************************************************************************
+ *
+ * FILE : unifi_priv.h
+ *
+ * PURPOSE : Private header file for unifi driver.
+ *
+ *           UDI = UniFi Debug Interface
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ *****************************************************************************
+ */
+#ifndef __LINUX_UNIFI_PRIV_H__
+#define __LINUX_UNIFI_PRIV_H__ 1
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/cdev.h>
+#include <linux/kthread.h>
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,19)
+#include <linux/freezer.h>
+#endif
+
+#ifdef CSR_WIFI_SUPPORT_MMC_DRIVER
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio.h>
+#endif /* CSR_WIFI_SUPPORT_MMC_DRIVER */
+
+#include <linux/fs.h>
+
+#ifdef ANDROID_BUILD
+#include <linux/wakelock.h>
+#endif
+
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_unifi_udi.h"
+#include "csr_wifi_router_lib.h"
+#include "unifiio.h"
+#ifndef CSR_WIFI_HIP_TA_DISABLE
+#include "csr_wifi_vif_utils.h"
+#endif
+
+/* Define the unifi_priv_t before include the unifi_native.h */
+struct unifi_priv;
+typedef struct unifi_priv unifi_priv_t;
+#ifdef CSR_SUPPORT_WEXT_AP
+struct CsrWifiSmeApConfig;
+typedef struct CsrWifiSmeApConfig CsrWifiSmeApConfig_t;
+#endif
+#ifdef CSR_SUPPORT_WEXT
+#include "unifi_wext.h"
+#endif
+
+#ifdef ANDROID_BUILD
+extern struct wake_lock unifi_sdio_wake_lock;
+#endif
+
+#include "unifi_clients.h"
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+#include <linux/workqueue.h>
+
+#undef INIT_WORK
+#define INIT_WORK(_work, _func)                                         \
+    do {                                                                \
+        INIT_LIST_HEAD(&(_work)->entry);                                \
+        (_work)->pending = 0;                                           \
+        PREPARE_WORK((_work), (_func), (_work));                        \
+        init_timer(&(_work)->timer);                                    \
+    } while(0)
+
+#endif  /* Linux kernel < 2.6.20 */
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define UF_NETIF_TX_WAKE_ALL_QUEUES(_netdev)    netif_tx_wake_all_queues(_netdev)
+#define UF_NETIF_TX_START_ALL_QUEUES(_netdev)   netif_tx_start_all_queues(_netdev)
+#define UF_NETIF_TX_STOP_ALL_QUEUES(_netdev)    netif_tx_stop_all_queues(_netdev)
+#else
+#define UF_NETIF_TX_WAKE_ALL_QUEUES(_netdev)    netif_wake_queue(_netdev)
+#define UF_NETIF_TX_START_ALL_QUEUES(_netdev)   netif_start_queue(_netdev)
+#define UF_NETIF_TX_STOP_ALL_QUEUES(_netdev)    netif_stop_queue(_netdev)
+#endif  /* Linux kernel >= 2.6.27 */
+
+
+#ifdef CSR_NATIVE_LINUX
+#include "sme_native/unifi_native.h"
+#else
+#include "unifi_sme.h"
+#endif
+
+/* The device major number to use when registering the udi driver */
+#define UNIFI_NAME      "unifi"
+/*
+ * MAX_UNIFI_DEVS defines the maximum number of UniFi devices that can be present.
+ * This number should be set to the number of SDIO slots supported by the SDIO
+ * host controller on the platform.
+ * Note: If MAX_UNIFI_DEVS value changes, fw_init[] needs to be corrected in drv.c
+ */
+#define MAX_UNIFI_DEVS  2
+
+/* 802.11 Mac header offsets */
+#define MAC_HEADER_SIZE  24
+#define QOS_CONTROL_HEADER_SIZE 2
+#define HT_CONTROL_HEADER_SIZE  4
+#define QOS_DATA 0x8
+#define QOS_DATA_NULL 0xc
+#define DATA_NULL 0x04
+#define FRAME_CONTROL_ORDER_BIT 0x8000
+#define FRAME_CONTROL_TYPE_FIELD_OFFSET  2
+#define FRAME_CONTROL_SUBTYPE_FIELD_OFFSET  4
+#define IEEE802_11_FRAMETYPE_DATA 0x02
+#define IEEE802_11_FRAMETYPE_CONTROL 0x01
+#define IEEE802_11_FRAMETYPE_MANAGEMENT 0x00
+#define IEEE802_11_FRAMETYPE_RESERVED 0x03
+
+/* octet offset from start of mac header for certain fields */
+#define IEEE802_11_ADDR3_OFFSET 16
+#define IEEE802_11_SEQUENCE_CONTROL_OFFSET 22
+#define IEEE802_11_MAX_DATA_LEN 2304
+
+/* frame control (FC) masks, for frame control as 16 bit integer */
+#define IEEE802_11_FC_TO_DS_MASK 0x100
+#define IEEE802_11_FC_FROM_DS_MASK 0x200
+#define IEEE802_11_FC_MOREDATA_MASK 0x2000
+#define IEEE802_11_FC_PROTECTED_MASK 0x4000
+#define IEEE80211_FC_ORDER_MASK 0x8000
+#define IEEE80211_FC_SUBTYPE_MASK 0x00f0
+#define IEEE80211_FC_TYPE_MASK 0x000c
+#define IEEE80211_FC_PROTO_VERSION_MASK 0x0003
+
+/*  selected type and subtype combinations as in 7.1.3.1 table 1
+    For frame control as 16 bit integer, or for ls octet
+*/
+#define IEEE802_11_FC_TYPE_DATA 0x08
+#define IEEE802_11_FC_TYPE_NULL 0x48
+#define IEEE802_11_FC_TYPE_QOS_NULL 0xc8
+#define IEEE802_11_FC_TYPE_QOS_DATA 0x88
+
+#define IEEE802_11_FC_TYPE_DATA_SUBTYPE_RESERVED 0x0D
+
+/* qos control (QC) masks for qos control as 16 bit integer, or for ls octet */
+#define IEEE802_11_QC_TID_MASK 0x0f
+#define IEEE802_11_QC_A_MSDU_PRESENT 0x80
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND))
+#define IEEE802_11_QC_NON_TID_BITS_MASK 0xFFF0
+#endif
+
+#define CSR_WIFI_EAPOL_M4_HOST_TAG 0x50000000
+#define IEEE802_11_DATA_FRAME_MAC_HEADER_SIZE 36
+#define MAX_ACCESS_CATOGORY 4
+
+/* Time in us to check for inactivity of stations 5 mins */
+#define INACTIVITY_CHECK_INTERVAL   300000000
+/* Time in us before a station is flagged as inactive */
+#define MAX_INACTIVITY_INTERVAL     300000000
+
+
+/* Define for maximum BA session */
+#define MAX_SUPPORTED_BA_SESSIONS_TX   1
+#define MAX_SUPPORTED_BA_SESSIONS_RX   4
+
+#define MAX_BA_WIND_SIZE 64
+#define MAC_HEADER_ADDR1_OFFSET     4
+#define MAC_HEADER_ADDR2_OFFSET     10
+
+/* Define for age (in us) value for frames in MPDU reorder buffer */
+#define CSR_WIFI_BA_MPDU_FRAME_AGE_TIMEOUT  30000 /* 30 milli seconds */
+
+/* This macro used in prepare_and_add_macheader*/
+#define ADDRESS_ONE_OFFSET 20
+
+/* Defines for STA inactivity detection */
+#define     STA_INACTIVE_DETECTION_TRIGGER_THRESHOLD           1                /* in number of stations */
+#define     STA_INACTIVE_DETECTION_TIMER_INTERVAL              30               /* in seconds */
+#define     STA_INACTIVE_TIMEOUT_VAL                           120*1000*1000    /* 120 seconds */
+
+/* Test for modes requiring AP firmware patch */
+#define CSR_WIFI_HIP_IS_AP_FW(mode) ((((mode) == CSR_WIFI_ROUTER_CTRL_MODE_AP) || \
+                                      ((mode) == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO)) ? TRUE : FALSE)
+
+/* Defines used in beacon filtering in case of P2P */
+#define CSR_WIFI_P2P_WILDCARD_SSID_LENGTH        0x7
+#define CSR_WIFI_80211_FRAME_SUBTYPE_BEACON      0x8
+#define CSR_WIFI_BEACON_FIXED_LENGTH             12
+#define CSR_WIFI_FRAME_SUBTYPE_BIT_OFFSET        4
+#define CSR_WIFI_80211_FRAME_SUBTYPE_BIT_MASK    ((u8)(0xF << CSR_WIFI_FRAME_SUBTYPE_BIT_OFFSET))
+
+#define CSR_WIFI_80211_GET_FRAME_SUBTYPE(frameBuffer) \
+    ((u8)(((u8 *)frameBuffer)[0] & CSR_WIFI_80211_FRAME_SUBTYPE_BIT_MASK) >> CSR_WIFI_FRAME_SUBTYPE_BIT_OFFSET)
+
+/* For M4 request received via netdev*/
+
+typedef u8 CsrWifiPacketType;
+#define CSR_WIFI_UNICAST_PDU   ((CsrWifiPacketType) 0x00)
+#define CSR_WIFI_MULTICAST_PDU ((CsrWifiPacketType) 0x1)
+#define CSR_WIFI_BROADCAST_PDU ((CsrWifiPacketType) 0x2)
+
+#define PRIO_TO_NICE(prio)  ((prio) - MAX_RT_PRIO - 20)
+
+/* Module parameter variables */
+extern int buswidth;
+extern int sdio_clock;
+extern int use_5g;
+extern int disable_hw_reset;
+extern int disable_power_control;
+extern int enable_wol;
+extern int sme_debug;
+extern int fw_init[MAX_UNIFI_DEVS];
+extern int tl_80211d;
+extern int sdio_byte_mode;
+extern int sdio_block_size;
+extern int coredump_max;
+extern int run_bh_once;
+extern int bh_priority;
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+extern int log_hip_signals;
+#endif
+
+struct dlpriv {
+    const unsigned char *dl_data;
+    int dl_len;
+    void *fw_desc;
+};
+
+
+struct uf_thread {
+
+    struct task_struct *thread_task;
+
+    /* wait_queue for waking the unifi_thread kernel thread */
+    wait_queue_head_t wakeup_q;
+    unsigned int wakeup_flag;
+
+    /*
+     * Use it to block the I/O thread when
+     * an error occurs or UniFi is reinitialised.
+     */
+    int block_thread;
+
+    char name[16];
+    int prio;
+};
+
+/*
+ * Link list to hold the received packets for the period the port
+ * remains closed.
+ */
+typedef struct rx_buffered_packets {
+    /* List link structure */
+    struct list_head q;
+    /* Packet to indicate when the port reopens */
+    struct sk_buff *skb;
+    /* Bulkdata to free in case the port closes and need to discard the packet */
+    bulk_data_param_t bulkdata;
+    /* The source address of the packet */
+    CsrWifiMacAddress sa;
+    /* The destination address of the packet */
+    CsrWifiMacAddress da;
+    /* Corresponding signal */
+    CSR_SIGNAL signal;
+} rx_buffered_packets_t;
+
+
+typedef u8 CsrWifiAcPowersaveMode;
+#define CSR_WIFI_AC_TRIGGER_ONLY_ENABLED 0x00
+#define CSR_WIFI_AC_DELIVERY_ONLY_ENABLE 0X01
+#define CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED 0X03
+#define CSR_WIFI_AC_LEGACY_POWER_SAVE 0X02
+
+
+#define IS_DELIVERY_ENABLED(mode) (mode & CSR_WIFI_AC_DELIVERY_ONLY_ENABLE)? 1: 0
+#define IS_DELIVERY_AND_TRIGGER_ENABLED(mode) ((mode & CSR_WIFI_AC_DELIVERY_ONLY_ENABLE)||(mode & CSR_WIFI_AC_TRIGGER_AND_DELIVERY_ENABLED))? 1: 0
+#define IS_DTIM_ACTIVE(flag,hostTag) ((flag == TRUE || hostTag != INVALID_HOST_TAG))
+#define INVALID_HOST_TAG 0xFFFFFFFF
+#define UNIFI_TRAFFIC_Q_CONTENTION UNIFI_TRAFFIC_Q_BE
+
+
+
+
+/* Queue to be used for contention priority */
+
+/*
+ * Link list to hold the tx packets for the period the peer
+ * powersave/free slots in unifi
+ */
+typedef struct tx_buffered_packets {
+    /* List link structure */
+    struct list_head q;
+    u16 interfaceTag;
+    CSR_CLIENT_TAG hostTag;
+    CSR_PROCESS_ID leSenderProcessId;
+    CSR_TRANSMISSION_CONTROL transmissionControl;
+    CSR_RATE rate;
+    /* Bulkdata to free in case the port closes and need to discard the packet */
+    bulk_data_desc_t bulkdata;
+    /* The source address of the packet */
+    CsrWifiMacAddress peerMacAddress;
+    CSR_PRIORITY priority;
+} tx_buffered_packets_t;
+
+/* station record has this data structure */
+typedef struct CsrWifiRouterCtrlStaInfo_t {
+
+    /* Sme sends these parameters */
+    CsrWifiMacAddress peerMacAddress;
+    u32 assignedHandle;
+    u8 wmmOrQosEnabled;
+    CsrWifiAcPowersaveMode powersaveMode[MAX_ACCESS_CATOGORY];
+    u16 maxSpLength;
+    u8 uapsdActive;
+    u16 noOfSpFramesSent;
+
+    /* Router/Driver database */
+#ifdef CSR_SUPPORT_SME
+    unifi_port_cfg_t *peerControlledPort;
+    unifi_port_cfg_t *peerUnControlledPort;
+
+    /* Inactivity feature parameters */
+    struct netInterface_priv *interfacePriv;
+    struct work_struct send_disconnected_ind_task;
+    u8 activity_flag;
+    u16 listenIntervalInTus;
+    CSR_CLIENT_TAG nullDataHostTag;
+
+    /* Activity timestamps for the station */
+    CsrTime lastActivity;
+
+    /* during m/c transmission sp suspended */
+    u8 uspSuspend;
+    CSR_PRIORITY triggerFramePriority;
+#endif
+    CsrWifiRouterCtrlPeerStatus currentPeerState;
+    struct list_head dataPdu[MAX_ACCESS_CATOGORY];
+    struct list_head mgtFrames;
+    u8 spStatus;
+    u8 prevFrmType;
+    u8 prevFrmAccessCatogory;
+    u8 protection;
+    u16 aid;
+    u8 txSuspend;
+    u8 timSet;
+    /* Dont change the value of below macro for SET & RESET */
+#define CSR_WIFI_TIM_RESET       0
+#define CSR_WIFI_TIM_SET         1
+#define CSR_WIFI_TIM_RESETTING   2
+#define CSR_WIFI_TIM_SETTING     3
+
+    u8 timRequestPendingFlag;
+    u8 updateTimReqQueued;
+    u16 noOfPktQueued;
+}CsrWifiRouterCtrlStaInfo_t;
+
+#ifdef CSR_SUPPORT_WEXT_AP
+struct CsrWifiSmeApConfig {
+    CsrWifiSsid ssid;
+    u16 channel;
+    CsrWifiNmeApCredentials credentials;
+    u8 max_connections;
+    u8 if_index;
+};
+#endif
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+/* This is a test code and may be removed later*/
+#define CSR_WIFI_RX_SIGNAL_BUFFER_SIZE    (60+1)
+
+typedef struct
+{
+    u8 *bufptr; /* Signal Primitive */
+    bulk_data_param_t data_ptrs; /* Bulk Data pointers */
+    u16 sig_len;
+}rx_buff_struct_t;
+
+typedef struct
+{
+    u8 writePointer; /**< write pointer */
+    u8 readPointer;  /**< read pointer */
+    u8 size;         /**< size of circular buffer */
+    rx_buff_struct_t rx_buff[CSR_WIFI_RX_SIGNAL_BUFFER_SIZE];    /**< Element of ciruclar buffer */
+} rxCircularBuffer_t;
+
+void rx_wq_handler(struct work_struct *work);
+#endif
+
+struct unifi_priv {
+
+    card_t *card;
+    CsrSdioFunction *sdio;
+
+    /* Index into Unifi_instances[] for this device. */
+    int instance;
+    /* Reference count for this instance */
+    int ref_count;
+
+    /* Firmware images */
+    struct dlpriv fw_sta;
+    struct dlpriv fw_conv;  /* used for conversion of production test image */
+
+    /* Char device related structures */
+    struct cdev unifi_cdev;
+    struct cdev unifiudi_cdev;
+    struct device *unifi_device;
+
+    /* Which wireless interface to use (1 - 2.4GHz, 2 - 5GHz) */
+    CSR_IFINTERFACE if_index;
+
+    /* For multiple interface support */
+    struct net_device *netdev[CSR_WIFI_NUM_INTERFACES];
+    struct netInterface_priv *interfacePriv[CSR_WIFI_NUM_INTERFACES];
+
+    u8 totalInterfaceCount;
+
+    int prev_queue;
+
+    /* Name of node under /proc */
+    char proc_entry_name[64];
+
+    /*
+     * Flags:
+     *  drop_unencrypted
+     *                - Not used?
+     *  netdev_registered
+     *                - whether the netdev has been registered.
+     */
+    unsigned int drop_unencrypted       : 1;
+
+    /* Our list of unifi linux clients. */
+    ul_client_t ul_clients[MAX_UDI_CLIENTS];
+
+    /* Mutex to protect using the logging hook after UDI client is gone */
+    struct semaphore udi_logging_mutex;
+    /* Pointer to the ul_clients[] array */
+    ul_client_t *logging_client;
+
+    /* A ul_client_t* used to send the netdev related MIB requests. */
+    ul_client_t *netdev_client;
+
+    /* The SME ul_client_t pointer. */
+    ul_client_t *sme_cli;
+
+    /* The AMP ul_client_t pointer. */
+    ul_client_t *amp_client;
+
+    /*
+     * Semaphore for locking the top-half to one user process.
+     * This is necessary to prevent multiple processes calling driver
+     * operations. This can happen because the network driver entry points
+     * can be called from multiple processes.
+     */
+#ifdef USE_DRIVER_LOCK
+    struct semaphore lock;
+#endif /* USE_DRIVER_LOCK */
+
+    /* Flag to say that an operation was aborted */
+    int io_aborted;
+
+    struct uf_thread bh_thread;
+
+#define UNIFI_INIT_NONE         0x00
+#define UNIFI_INIT_IN_PROGRESS  0x01
+#define UNIFI_INIT_FW_DOWNLOADED 0x02
+#define UNIFI_INIT_COMPLETED    0x04
+    unsigned char init_progress;
+
+    int sme_is_present;
+
+    /* The WMM features that UniFi uses in the current BSS */
+    unsigned int sta_wmm_capabilities;
+
+    /* Debug only */
+    char last_debug_string[256];
+    unsigned short last_debug_word16[16];
+
+#ifdef CSR_SUPPORT_SME
+  /* lock to protect the tx queues list */
+    spinlock_t tx_q_lock;
+    u8 allPeerDozing;
+    u8 pausedStaHandle[MAX_ACCESS_CATOGORY];
+    /* Max packet the driver can queue, irrespective of interface number */
+    u16 noOfPktQueuedInDriver;
+#define CSR_WIFI_DRIVER_SUPPORT_FOR_MAX_PKT_QUEUEING 512
+#define CSR_WIFI_DRIVER_MAX_PKT_QUEUING_THRESHOLD_PER_PEER 64
+#define CSR_WIFI_DRIVER_MINIMUM_BROADCAST_PKT_THRESHOLD 3
+
+    u8 routerBufferEnable[MAX_ACCESS_CATOGORY];
+    /* lock to protect stainfo members and priv members*/
+    spinlock_t staRecord_lock;
+#endif
+#ifdef CSR_NATIVE_LINUX
+#ifdef CSR_SUPPORT_WEXT
+    /* wireless config */
+    struct wext_config wext_conf;
+#endif
+
+    /* Mutex to protect the MLME blocking requests */
+    struct semaphore mlme_blocking_mutex;
+
+    /* The ul_client that provides the blocking API for WEXT calls */
+    ul_client_t *wext_client;
+
+#endif /* CSR_NATIVE_LINUX */
+
+#ifdef CSR_SUPPORT_SME
+    wait_queue_head_t sme_request_wq;
+    /* Semaphore to protect the SME blocking requests */
+    struct semaphore sme_sem;
+    /* Structure to hold the SME blocking requests data*/
+    sme_reply_t sme_reply;
+
+    /* Structure to hold a traffic protocol indication */
+    struct ta_ind {
+        struct work_struct task;
+        CsrWifiRouterCtrlTrafficPacketType packet_type;
+        CsrWifiRouterCtrlProtocolDirection direction;
+        CsrWifiMacAddress src_addr;
+        int in_use;
+    } ta_ind_work;
+
+    struct ta_sample_ind {
+        struct work_struct task;
+        CsrWifiRouterCtrlTrafficStats stats;
+        int in_use;
+    } ta_sample_ind_work;
+
+    __be32 sta_ip_address;
+    CsrWifiRouterCtrlSmeVersions    sme_versions;
+
+    /*
+     * Flag to reflect state of unifi_sys_wifi_on_*() progress.
+     * This indicates whether we are in an "wifi on" state when we are
+     * allowed to indication errors with unifi_mgt_wifi_off_ind()
+     */
+    enum {
+        wifi_on_unspecified = -1,
+        wifi_on_in_progress = 0,
+        wifi_on_done = 1,
+    } wifi_on_state;
+
+    /* Userspace TaskId for the SME Set when a wifi on req is received */
+    CsrSchedQid CSR_WIFI_SME_IFACEQUEUE;
+
+    struct work_struct multicast_list_task;
+    /*
+     * The SME installs filters to ask for specific MA-UNITDATA.req
+     * to be passed to different SME components.
+     */
+#define MAX_MA_UNIDATA_IND_FILTERS      8
+    sme_ma_unidata_ind_filter_t sme_unidata_ind_filters[MAX_MA_UNIDATA_IND_FILTERS];
+
+/* UNIFI_CFG related parameters */
+    uf_cfg_bcast_packet_filter_t packet_filters;
+    unsigned char *filter_tclas_ies;
+ /* The structure that holds all the connection configuration. */
+    CsrWifiSmeConnectionConfig connection_config;
+#ifdef CSR_SUPPORT_WEXT
+
+    int ignore_bssid_join;
+    struct iw_statistics wext_wireless_stats;
+
+    /* The MIB and MAC address files contents, read from userspace */
+    CsrWifiSmeDataBlock mib_data;
+    CsrWifiMacAddress sta_mac_address;
+
+    int wep_tx_key_index;
+    wep_key_t wep_keys[NUM_WEPKEYS];
+
+
+#ifdef CSR_SUPPORT_WEXT_AP
+    CsrWifiSmeApMacConfig ap_mac_config;
+    CsrWifiNmeApConfig group_sec_config;
+    CsrWifiSmeApConfig_t ap_config;
+#endif
+    struct work_struct sme_config_task;
+
+#endif /* CSR_SUPPORT_WEXT */
+
+#endif /* CSR_SUPPORT_SME */
+
+#ifdef CSR_SME_USERSPACE
+    void *smepriv;
+#endif /* CSR_SME_USERSPACE */
+
+    card_info_t card_info;
+
+    /* Mutex to protect unifi_send_signal() */
+    spinlock_t send_signal_lock;
+
+
+    /*
+     * The workqueue to offload the TA run
+     * and the multicast addresses list set
+     */
+    struct workqueue_struct *unifi_workqueue;
+
+    unsigned char *mib_cfm_buffer;
+    unsigned int mib_cfm_buffer_length;
+
+    int ptest_mode;     /* Set when in production test mode */
+    int coredump_mode;  /* Set when SME has requested a coredump */
+    u8 wol_suspend; /* Set when suspending with UniFi powered */
+
+#define UF_UNCONTROLLED_PORT_Q      0
+#define UF_CONTROLLED_PORT_Q        1
+
+    /* Semaphore to protect the rx queues list */
+    struct semaphore rx_q_sem;
+
+    /* Spinlock to protect M4 data */
+    spinlock_t m4_lock;
+    /* Mutex to protect BA RX data */
+    struct semaphore ba_mutex;
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    /* Spinlock to protect the WAPI data */
+    spinlock_t wapi_lock;
+#endif
+
+#ifndef ALLOW_Q_PAUSE
+    /* Array to indicate if a particular Tx queue is paused, this may not be
+     * required in a multiqueue implementation since we can directly stop kernel
+     * queues */
+    u8 tx_q_paused_flag[UNIFI_TRAFFIC_Q_MAX];
+#endif
+
+#ifdef CSR_WIFI_RX_PATH_SPLIT
+    struct workqueue_struct *rx_workqueue;
+    struct work_struct rx_work_struct;
+    rxCircularBuffer_t rxSignalBuffer;
+
+#endif
+
+    u32 rxTcpThroughput;
+    u32 txTcpThroughput;
+    u32 rxUdpThroughput;
+    u32 txUdpThroughput;
+
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    /*Set if multicast KeyID = 1*/
+    u8 wapi_multicast_filter;
+    /*Set if unicast KeyID = 1*/
+    u8 wapi_unicast_filter;
+    u8 wapi_unicast_queued_pkt_filter;
+#ifdef CSR_WIFI_SECURITY_WAPI_QOSCTRL_MIC_WORKAROUND
+    u8  isWapiConnection;
+#endif
+#endif
+
+#ifdef CSR_WIFI_SPLIT_PATCH
+    CsrWifiRouterCtrlModeSetReq pending_mode_set;
+#endif
+
+    u8 cmanrTestMode;
+    CSR_RATE cmanrTestModeTransmitRate;
+
+};
+
+typedef struct {
+    u16 queue_length[4];
+    u8 os_queue_paused;
+} unifi_OsQosInfo;
+
+
+typedef struct {
+    u8 active;
+    bulk_data_param_t bulkdata;
+    CSR_SIGNAL signal;
+    u16 sn;
+    CsrTime recv_time;
+} frame_desc_struct;
+
+typedef struct {
+    frame_desc_struct *buffer;
+    u16 wind_size;
+    u16 occupied_slots;
+    struct timer_list timer;
+    u16 timeout;
+    u16 expected_sn;
+    u16 start_sn;
+    u8   trigger_ba_after_ssn;
+    struct netInterface_priv *interfacePriv;
+    u16 tID;
+    CsrWifiMacAddress macAddress;
+    struct work_struct send_ba_err_task;
+} ba_session_rx_struct;
+
+
+typedef struct {
+    struct netInterface_priv *interfacePriv;
+    u16 tID;
+    CsrWifiMacAddress macAddress;
+} ba_session_tx_struct;
+
+typedef struct netInterface_priv
+{
+    u16 InterfaceTag;
+    struct unifi_priv *privPtr;
+    ba_session_tx_struct *ba_session_tx[MAX_SUPPORTED_BA_SESSIONS_TX];
+    ba_session_rx_struct *ba_session_rx[MAX_SUPPORTED_BA_SESSIONS_RX];
+    frame_desc_struct ba_complete[MAX_BA_WIND_SIZE];
+    u8 ba_complete_index;
+    u8 queueEnabled[UNIFI_NO_OF_TX_QS];
+    struct work_struct send_m4_ready_task;
+#ifdef CSR_WIFI_SECURITY_WAPI_ENABLE
+    struct work_struct send_pkt_to_encrypt;
+#endif
+    struct net_device_stats stats;
+    u8 interfaceMode;
+    u8 protect;
+    CsrWifiMacAddress bssid;
+    /*
+    * Flag to reflect state of CONNECTED indication signal.
+    * This indicates whether we are "joined" an Access Point (i.e. have
+    * nominated an AP and are receiving beacons) but give no indication
+    * of whether we are authenticated and/or associated.
+    */
+    enum {
+        UnifiConnectedUnknown = -1,
+        UnifiNotConnected = 0,
+        UnifiConnected = 1,
+    } connected;
+#ifdef CSR_SUPPORT_WEXT
+    /* Tracks when we are waiting for a netdevice state change callback */
+    u8 wait_netdev_change;
+    /* True if we have successfully registered for netdev callbacks */
+    u8 netdev_callback_registered;
+#endif /* CSR_SUPPORT_WEXT */
+    unsigned int netdev_registered;
+#define UNIFI_MAX_MULTICAST_ADDRESSES 10
+    /* The multicast addresses list that the thread needs to set. */
+    u8 mc_list[UNIFI_MAX_MULTICAST_ADDRESSES*ETH_ALEN];
+    /* The multicast addresses count that the thread needs to set. */
+    int mc_list_count;
+    u32 tag;
+#ifdef CSR_SUPPORT_SME
+    /* (un)controlled port configuration */
+    unifi_port_config_t controlled_data_port;
+    unifi_port_config_t uncontrolled_data_port;
+
+    /* station record maintenance related data structures */
+    u8 num_stations_joined;
+    CsrWifiRouterCtrlStaInfo_t *(staInfo)[UNIFI_MAX_CONNECTIONS];
+    struct list_head genericMgtFrames;
+    struct list_head genericMulticastOrBroadCastFrames;
+    struct list_head genericMulticastOrBroadCastMgtFrames;
+
+    /* Timer for detecting station inactivity */
+    struct timer_list sta_activity_check_timer;
+    u8 sta_activity_check_enabled;
+
+    /* Timestamp when the last inactivity check was done */
+    CsrTime last_inactivity_check;
+
+    /*number of multicast or borad cast packets  queued*/
+    u16 noOfbroadcastPktQueued;
+#endif
+    /* A list to hold the buffered uncontrolled port packets */
+    struct list_head rx_uncontrolled_list;
+    /* A list to hold the buffered controlled port packets */
+    struct list_head rx_controlled_list;
+    /* Buffered M4 signal to take care of WPA race condition */
+    CSR_SIGNAL m4_signal;
+    bulk_data_desc_t m4_bulk_data;
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+    /* Buffered WAPI Unicast MA Packet Request for encryption in Sme */
+    CSR_SIGNAL wapi_unicast_ma_pkt_sig;
+    bulk_data_desc_t wapi_unicast_bulk_data;
+#endif
+
+    /* This should be removed and m4_hostTag should be used for checking*/
+    u8 m4_sent;
+    CSR_CLIENT_TAG m4_hostTag;
+    u8 dtimActive;
+    u8 intraBssEnabled;
+    u32 multicastPduHostTag; /* Used to set the tim after getting
+       a confirm for it */
+    u8 bcTimSet;
+    u8 bcTimSetReqPendingFlag;
+    u8 bcTimSetReqQueued;
+} netInterface_priv_t;
+
+#ifndef ALLOW_Q_PAUSE
+#define net_is_tx_q_paused(priv, q)   (priv->tx_q_paused_flag[q])
+#define net_tx_q_unpause(priv, q)   (priv->tx_q_paused_flag[q] = 0)
+#define net_tx_q_pause(priv, q)   (priv->tx_q_paused_flag[q] = 1)
+#endif
+
+#ifdef CSR_SUPPORT_SME
+#define routerStartBuffering(priv,queue) priv->routerBufferEnable[(queue)] = TRUE;
+#define routerStopBuffering(priv,queue) priv->routerBufferEnable[(queue)]  = FALSE;
+#define isRouterBufferEnabled(priv,queue) priv->routerBufferEnable[(queue)]
+#endif
+
+#ifdef USE_DRIVER_LOCK
+#define LOCK_DRIVER(_p)         down_interruptible(&(_p)->lock)
+#define UNLOCK_DRIVER(_p)       up(&(_p)->lock)
+#else
+#define LOCK_DRIVER(_p)         (void)(_p); /* as nothing */
+#define UNLOCK_DRIVER(_p)       (void)(_p); /* as nothing */
+#endif /* USE_DRIVER_LOCK */
+
+s32 CsrHipResultToStatus(CsrResult csrResult);
+
+
+/*
+ * SDIO related functions and callbacks
+ */
+int  uf_sdio_load(void);
+void uf_sdio_unload(void);
+unifi_priv_t *uf_find_instance(int inst);
+int uf_find_priv(unifi_priv_t *priv);
+int uf_find_netdev_priv(netInterface_priv_t *priv);
+unifi_priv_t *uf_get_instance(int inst);
+void uf_put_instance(int inst);
+int csr_sdio_linux_install_irq(CsrSdioFunction *sdio);
+int csr_sdio_linux_remove_irq(CsrSdioFunction *sdio);
+
+void uf_add_os_device(int bus_id, struct device *os_device);
+void uf_remove_os_device(int bus_id);
+
+
+
+/*
+ * Claim/release SDIO
+ *
+ * For multifunction cards, we cannot grub the SDIO lock around the unifi_bh()
+ * as this prevents other functions using SDIO.
+ * Since some of CSR SDIO API is used regardless of trying to lock unifi_bh()
+ * we have followed this scheme:
+ * 1. If a function needs protection only when CSR_WIFI_SINGLE_FUNCTION is defined
+ *    then we call CsrSdioClaim/CsrSdioRelease().
+ * 2. If a function needs protection only when CSR_WIFI_SINGLE_FUNCTION is not defined
+ *    then we call _sdio_claim_host/_sdio_claim_host(). Use of this should be restricted
+ *    to the SDIO glue layer only (e.g. sdio_mmc.c).
+ * 3. If a function needs protection, regardless of the CSR_WIFI_SINGLE_FUNCTION
+ *    then we call directly the sdio_claim_host/sdio_release_host().
+ *    Use of this must be restricted to the SDIO glue layer only (e.g. sdio_mmc.c).
+ *
+ * Note: The _func and function pointers are _not_ the same.
+ * The former is the (struct sdio_func*) context, which restricts the use to the SDIO glue layer.
+ * The latter is the (CsrSdioFunction*) context, which allows calls from all layers.
+ */
+
+#ifdef CSR_WIFI_SUPPORT_MMC_DRIVER
+
+#ifdef CSR_WIFI_SINGLE_FUNCTION
+#define CsrSdioClaim(function)    sdio_claim_host((function)->priv);
+#define CsrSdioRelease(function)  sdio_release_host((function)->priv);
+
+#define _sdio_claim_host(_func)
+#define _sdio_release_host(_func)
+
+#else
+#define CsrSdioClaim(function)
+#define CsrSdioRelease(function)
+
+#define _sdio_claim_host(_func)     sdio_claim_host(_func)
+#define _sdio_release_host(_func)   sdio_release_host(_func)
+
+#endif /* CSR_WIFI_SINGLE_FUNCTION */
+
+#else
+#define _sdio_claim_host(_func)
+#define _sdio_release_host(_func)
+
+#define CsrSdioClaim(function)
+#define CsrSdioRelease(function)
+
+#endif /* CSR_WIFI_SUPPORT_MMC_DRIVER */
+
+
+/*
+ * Functions to allocate and free an ethernet device.
+ */
+unifi_priv_t *uf_alloc_netdevice(CsrSdioFunction *sdio_dev, int bus_id);
+int uf_free_netdevice(unifi_priv_t *priv);
+
+/* Allocating function for other interfaces */
+u8 uf_alloc_netdevice_for_other_interfaces(unifi_priv_t *priv, u16 interfaceTag);
+
+/*
+ * Firmware download related functions.
+ */
+int uf_run_unifihelper(unifi_priv_t *priv);
+int uf_request_firmware_files(unifi_priv_t *priv, int is_fw);
+int uf_release_firmware_files(unifi_priv_t *priv);
+int uf_release_firmware(unifi_priv_t *priv, struct dlpriv *to_free);
+
+/*
+ * Functions to create and delete the device nodes.
+ */
+int uf_create_device_nodes(unifi_priv_t *priv, int bus_id);
+void uf_destroy_device_nodes(unifi_priv_t *priv);
+
+/*
+ * Upper Edge Initialisation functions
+ */
+int uf_init_bh(unifi_priv_t *priv);
+int uf_init_hw(unifi_priv_t *priv);
+
+/* Thread related helper functions */
+int uf_start_thread(unifi_priv_t *priv, struct uf_thread *thread, int (*func)(void *));
+void uf_stop_thread(unifi_priv_t *priv, struct uf_thread *thread);
+void uf_wait_for_thread_to_stop(unifi_priv_t *priv, struct uf_thread *thread);
+
+
+/*
+ * Unifi Linux functions
+ */
+void ul_init_clients(unifi_priv_t *priv);
+
+/* Configuration flags */
+#define CLI_USING_WIRE_FORMAT   0x0002
+#define CLI_SME_USERSPACE       0x0020
+ul_client_t *ul_register_client(unifi_priv_t *priv,
+        unsigned int configuration,
+        udi_event_t udi_event_clbk);
+int ul_deregister_client(ul_client_t *pcli);
+
+int ul_send_signal_unpacked(unifi_priv_t *priv,
+                            CSR_SIGNAL *sigptr,
+                            bulk_data_param_t *bulkdata);
+int ul_send_signal_raw(unifi_priv_t *priv,
+                       unsigned char *sigptr, int siglen,
+                       bulk_data_param_t *bulkdata);
+
+void ul_log_config_ind(unifi_priv_t *priv, u8 *conf_param, int len);
+
+
+/*
+ * Data plane operations
+ */
+/*
+ *      data_tx.c
+ */
+int uf_verify_m4(unifi_priv_t *priv, const unsigned char *packet,
+        unsigned int length);
+
+#ifdef CSR_SUPPORT_SME
+u8 uf_check_broadcast_bssid(unifi_priv_t *priv, const bulk_data_param_t *bulkdata);
+u8 uf_process_pm_bit_for_peer(unifi_priv_t * priv, CsrWifiRouterCtrlStaInfo_t * srcStaInfo,u8 pmBit,u16 interfaceTag);
+void uf_process_ps_poll(unifi_priv_t *priv,u8* sa,u8* da,u8 pmBit,u16 interfaceTag);
+int uf_ap_process_data_pdu(unifi_priv_t *priv, struct sk_buff *skb,
+                   struct ethhdr *ehdr, CsrWifiRouterCtrlStaInfo_t * srcStaInfo,
+                   const CSR_SIGNAL *signal,
+                   bulk_data_param_t *bulkdata,
+                   u8 macHeaderLengthInBytes);
+u8 uf_is_more_data_for_non_delivery_ac(CsrWifiRouterCtrlStaInfo_t *staRecord);
+void uf_process_wmm_deliver_ac_uapsd (  unifi_priv_t * priv,
+                                        CsrWifiRouterCtrlStaInfo_t * srcStaInfo,
+                                        u16 qosControl,
+                                        u16 interfaceTag);
+
+void uf_send_buffered_data_from_ac(unifi_priv_t *priv, CsrWifiRouterCtrlStaInfo_t * staInfo, u8 queue, struct list_head *txList);
+void uf_send_buffered_data_from_delivery_ac(unifi_priv_t *priv, CsrWifiRouterCtrlStaInfo_t * staInfo, u8 queue, struct list_head *txList);
+
+void uf_continue_uapsd(unifi_priv_t *priv, CsrWifiRouterCtrlStaInfo_t * staInfo);
+void uf_send_qos_null(unifi_priv_t * priv,u16 interfaceTag, const u8 *da,CSR_PRIORITY priority,CsrWifiRouterCtrlStaInfo_t * srcStaInfo);
+void uf_send_nulldata(unifi_priv_t * priv,u16 interfaceTag, const u8 *da,CSR_PRIORITY priority,CsrWifiRouterCtrlStaInfo_t * srcStaInfo);
+
+
+
+#endif
+CsrResult uf_process_ma_packet_req(unifi_priv_t *priv,  u8 *peerMacAddress, CSR_CLIENT_TAG hostTag, u16 interfaceTag, CSR_TRANSMISSION_CONTROL transmissionControl, CSR_RATE TransmitRate, CSR_PRIORITY priority, CSR_PROCESS_ID senderId, bulk_data_param_t *bulkdata);
+void uf_process_ma_vif_availibility_ind(unifi_priv_t *priv,u8 *sigdata, u32 siglen);
+#ifdef CSR_SUPPORT_SME
+void uf_send_buffered_frames(unifi_priv_t *priv,unifi_TrafficQueue queue);
+int uf_process_station_records_for_sending_data(unifi_priv_t *priv,u16 interfaceTag,
+                                                 CsrWifiRouterCtrlStaInfo_t *srcStaInfo,
+                                                 CsrWifiRouterCtrlStaInfo_t *dstStaInfo);
+void uf_prepare_send_cfm_list_for_queued_pkts(unifi_priv_t * priv,
+                                                 struct list_head *frames_need_cfm_list,
+                                                 struct list_head * list);
+void send_auto_ma_packet_confirm(unifi_priv_t *priv,
+                                 netInterface_priv_t *interfacePriv,
+                                 struct list_head *buffered_frames_list);
+void uf_flush_list(unifi_priv_t * priv, struct list_head * list);
+tx_buffered_packets_t *dequeue_tx_data_pdu(unifi_priv_t *priv, struct list_head *txList);
+void resume_unicast_buffered_frames(unifi_priv_t *priv, u16 interfaceTag);
+void update_eosp_to_head_of_broadcast_list_head(unifi_priv_t *priv,u16 interfaceTag);
+void resume_suspended_uapsd(unifi_priv_t* priv,u16 interfaceTag);
+#endif
+/*
+ *      netdev.c
+ */
+
+#ifndef P80211_OUI_LEN
+#define P80211_OUI_LEN  3
+#endif
+typedef struct {
+    u8    dsap;   /* always 0xAA */
+    u8    ssap;   /* always 0xAA */
+    u8    ctrl;   /* always 0x03 */
+    u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+    u16 protocol;
+} __attribute__ ((packed)) llc_snap_hdr_t;
+int skb_add_llc_snap(struct net_device *dev, struct sk_buff *skb, int proto);
+int skb_80211_to_ether(unifi_priv_t *priv, struct sk_buff *skb,
+        const unsigned char *daddr, const unsigned char *saddr,
+        const CSR_SIGNAL *signal,
+        bulk_data_param_t *bulkdata);
+
+const char *result_code_str(int result);
+
+
+/* prepares & appends the Mac header for the payload */
+int prepare_and_add_macheader(unifi_priv_t *priv,
+                              struct sk_buff *skb,
+                              struct sk_buff *newSkb,
+                              CSR_PRIORITY priority,
+                              bulk_data_param_t *bulkdata,
+                              u16 interfaceTag,
+                              const u8 *daddr,
+                              const u8 *saddr,
+                              u8 protection);
+CSR_PRIORITY
+get_packet_priority(unifi_priv_t *priv, struct sk_buff *skb, const struct ethhdr *ehdr, netInterface_priv_t *interfacePriv);
+
+void
+unifi_frame_ma_packet_req(unifi_priv_t *priv, CSR_PRIORITY priority,
+                          CSR_RATE TransmitRate, CSR_CLIENT_TAG hostTag,
+                          u16 interfaceTag, CSR_TRANSMISSION_CONTROL transmissionControl,
+                          CSR_PROCESS_ID leSenderProcessId, u8 *peerMacAddress,
+                          CSR_SIGNAL *signal);
+
+
+/* Pack the LSB to include station handle & status of tim set */
+#define CSR_WIFI_PACK_SENDER_ID_LSB_FOR_TIM_REQ(handle, timState)  ((handle << 2) | timState)
+/* get the station record handle from the sender ID */
+#define CSR_WIFI_GET_STATION_HANDLE_FROM_RECEIVER_ID(receiverProcessId) (u8) ((receiverProcessId & 0xff) >> 2)
+/* get the timSet status from the sender ID */
+#define CSR_WIFI_GET_TIMSET_STATE_FROM_RECEIVER_ID(receiverProcessId)  (u8) (receiverProcessId & 0x03)
+
+/* handle is 6 bits to accomodate in senderId LSB (only 64 station can be associated) */
+#define CSR_WIFI_BROADCAST_OR_MULTICAST_HANDLE 0x3F
+
+void update_tim(unifi_priv_t * priv, u16 aid, u8 setTim, u16 interfaceTag, u32 handle);
+void uf_handle_tim_cfm(unifi_priv_t *priv, CSR_MLME_SET_TIM_CONFIRM *cfm, u16 senderProcessId);
+
+/* Clear the Peer station Record, in case of wifioff/unexpected card removal */
+void CsrWifiRouterCtrlInterfaceReset(unifi_priv_t *priv, u16 interfaceTag);
+
+void scroll_ba_window(unifi_priv_t *priv,
+                      netInterface_priv_t *interfacePriv,
+                      ba_session_rx_struct *ba_session,
+                      u16 sn);
+
+u8 blockack_session_stop(unifi_priv_t *priv,
+                              u16 interfaceTag,
+                              CsrWifiRouterCtrlBlockAckRole role,
+                              u16 tID,
+                              CsrWifiMacAddress macAddress);
+#ifdef CSR_SUPPORT_SME
+/* Fetch the protection information from interface Mode */
+s8 uf_get_protection_bit_from_interfacemode(unifi_priv_t *priv, u16 interfaceTag, const u8 *daddr);
+#endif
+
+/* Fetch the station record handler from data base for matching Mac address */
+#ifdef CSR_SUPPORT_SME
+CsrWifiRouterCtrlStaInfo_t *CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(unifi_priv_t *priv,
+                                                                                const u8 *peerMacAddress,
+                                                                                u16 interfaceTag);
+
+/* Fetch the station record handler from data base for matching handle */
+CsrWifiRouterCtrlStaInfo_t * CsrWifiRouterCtrlGetStationRecordFromHandle(unifi_priv_t *priv,
+                                                                 u32 handle,
+                                                                 u16 interfaceTag);
+
+void uf_update_sta_activity(unifi_priv_t *priv, u16 interfaceTag, const u8 *peerMacAddress);
+void uf_process_ma_pkt_cfm_for_ap(unifi_priv_t *priv,u16 interfaceTag, const CSR_MA_PACKET_CONFIRM *pkt_cfm);
+#endif
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28)
+int uf_install_qdisc(struct net_device *dev);
+#endif
+
+void uf_resume_data_plane(unifi_priv_t *priv, int queue,
+                          CsrWifiMacAddress peer_address,
+                          u16 interfaceTag);
+void uf_free_pending_rx_packets(unifi_priv_t *priv, int queue,
+        CsrWifiMacAddress peer_address,u16 interfaceTag);
+
+int uf_register_netdev(unifi_priv_t *priv, int numOfInterface);
+void uf_unregister_netdev(unifi_priv_t *priv);
+
+void uf_net_get_name(struct net_device *dev, char *name, int len);
+
+void uf_send_queue_info(unifi_priv_t *priv);
+u16 uf_get_vif_identifier(CsrWifiRouterCtrlMode mode, u16 tag);
+
+void uf_process_rx_pending_queue(unifi_priv_t *priv, int queue,
+                                 CsrWifiMacAddress source_address,
+                                 int indicate, u16 interfaceTag);
+
+#ifdef CSR_WIFI_HIP_DEBUG_OFFLINE
+int uf_register_hip_offline_debug(unifi_priv_t *priv);
+int uf_unregister_hip_offline_debug(unifi_priv_t *priv);
+#endif
+
+/*
+ *      inet.c
+ */
+void uf_register_inet_notifier(void);
+void uf_unregister_inet_notifier(void);
+
+
+/*
+ * Suspend / Resume handlers
+ */
+void unifi_resume(void *ospriv);
+void unifi_suspend(void *ospriv);
+
+
+#define QOS_CAPABILITY_WMM_ENABLED      0x0001
+#define QOS_CAPABILITY_WMM_UAPSD        0x0002
+#define QOS_CAPABILITY_ACM_BE_ENABLED   0x0010
+#define QOS_CAPABILITY_ACM_BK_ENABLED   0x0020
+#define QOS_CAPABILITY_ACM_VI_ENABLED   0x0040
+#define QOS_CAPABILITY_ACM_VO_ENABLED   0x0080
+#define QOS_CAPABILITY_TS_BE_ENABLED    0x0100
+#define QOS_CAPABILITY_TS_BK_ENABLED    0x0200
+#define QOS_CAPABILITY_TS_VI_ENABLED    0x0400
+#define QOS_CAPABILITY_TS_VO_ENABLED    0x0800
+
+
+/* EAPOL PDUS */
+#ifndef ETH_P_PAE
+#define ETH_P_PAE 0x888e
+#endif
+#ifndef ETH_P_WAI
+#define ETH_P_WAI 0x88b4
+#endif
+/*
+ * unifi_dbg.c
+ */
+void debug_string_indication(unifi_priv_t *priv,
+        const unsigned char *extra,
+        unsigned int extralen);
+void debug_word16_indication(unifi_priv_t *priv, const CSR_SIGNAL *sigptr);
+void debug_generic_indication(unifi_priv_t *priv, const CSR_SIGNAL *sigptr);
+
+
+/*
+ * putest.c
+ */
+int unifi_putest_start(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_cmd52_block_read(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_stop(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_set_sdio_clock(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_cmd52_read(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_coredump_prepare(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_cmd52_write(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_gp_read16(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_gp_write16(unifi_priv_t *priv, unsigned char *arg);
+
+int unifi_putest_dl_fw(unifi_priv_t *priv, unsigned char *arg);
+int unifi_putest_dl_fw_buff(unifi_priv_t *priv, unsigned char *arg);
+
+#endif /* __LINUX_UNIFI_PRIV_H__ */
diff --git a/drivers/staging/csr/unifi_sme.c b/drivers/staging/csr/unifi_sme.c
new file mode 100644
index 00000000000..ff639d47d07
--- /dev/null
+++ b/drivers/staging/csr/unifi_sme.c
@@ -0,0 +1,1239 @@
+/*
+ * ***************************************************************************
+ *  FILE:     unifi_sme.c
+ *
+ *  PURPOSE:    SME related functions.
+ *
+ *  Copyright (C) 2007-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ***************************************************************************
+ */
+
+#include "unifi_priv.h"
+#include "csr_wifi_hip_unifi.h"
+#include "csr_wifi_hip_conversions.h"
+
+
+
+
+    int
+convert_sme_error(CsrResult error)
+{
+    switch (error) {
+        case CSR_RESULT_SUCCESS:
+            return 0;
+        case CSR_RESULT_FAILURE:
+        case CSR_WIFI_RESULT_NOT_FOUND:
+        case CSR_WIFI_RESULT_TIMED_OUT:
+        case CSR_WIFI_RESULT_CANCELLED:
+        case CSR_WIFI_RESULT_UNAVAILABLE:
+            return -EIO;
+        case CSR_WIFI_RESULT_NO_ROOM:
+            return -EBUSY;
+        case CSR_WIFI_RESULT_INVALID_PARAMETER:
+            return -EINVAL;
+        case CSR_WIFI_RESULT_UNSUPPORTED:
+            return -EOPNOTSUPP;
+        default:
+            return -EIO;
+    }
+}
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  sme_log_event
+ *
+ *      Callback function to be registered as the SME event callback.
+ *      Copies the signal content into a new udi_log_t struct and adds
+ *      it to the read queue for the SME client.
+ *
+ *  Arguments:
+ *      arg             This is the value given to unifi_add_udi_hook, in
+ *                      this case a pointer to the client instance.
+ *      signal          Pointer to the received signal.
+ *      signal_len      Size of the signal structure in bytes.
+ *      bulkdata        Pointers to any associated bulk data.
+ *      dir             Direction of the signal. Zero means from host,
+ *                      non-zero means to host.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+    void
+sme_log_event(ul_client_t *pcli,
+        const u8 *signal, int signal_len,
+        const bulk_data_param_t *bulkdata,
+        int dir)
+{
+    unifi_priv_t *priv;
+    CSR_SIGNAL unpacked_signal;
+    CsrWifiSmeDataBlock mlmeCommand;
+    CsrWifiSmeDataBlock dataref1;
+    CsrWifiSmeDataBlock dataref2;
+    CsrResult result = CSR_RESULT_SUCCESS;
+    int r;
+
+    func_enter();
+    /* Just a sanity check */
+    if ((signal == NULL) || (signal_len <= 0)) {
+        func_exit();
+        return;
+    }
+
+    priv = uf_find_instance(pcli->instance);
+    if (!priv) {
+        unifi_error(priv, "sme_log_event: invalid priv\n");
+        func_exit();
+        return;
+    }
+
+    if (priv->smepriv == NULL) {
+        unifi_error(priv, "sme_log_event: invalid smepriv\n");
+        func_exit();
+        return;
+    }
+
+    unifi_trace(priv, UDBG3,
+            "sme_log_event: Process signal 0x%.4X\n",
+            CSR_GET_UINT16_FROM_LITTLE_ENDIAN(signal));
+
+
+    /* If the signal is known, then do any filtering required, otherwise it pass it to the SME. */
+    r = read_unpack_signal(signal, &unpacked_signal);
+    if (r == CSR_RESULT_SUCCESS) {
+        if ((unpacked_signal.SignalPrimitiveHeader.SignalId == CSR_DEBUG_STRING_INDICATION_ID) ||
+            (unpacked_signal.SignalPrimitiveHeader.SignalId == CSR_DEBUG_WORD16_INDICATION_ID))
+        {
+            func_exit();
+            return;
+        }
+        if (unpacked_signal.SignalPrimitiveHeader.SignalId == CSR_MA_PACKET_INDICATION_ID)
+        {
+            u16 frmCtrl;
+            u8 unicastPdu = TRUE;
+            u8 *macHdrLocation;
+            u8 *raddr = NULL, *taddr = NULL;
+            CsrWifiMacAddress peerMacAddress;
+            /* Check if we need to send CsrWifiRouterCtrlMicFailureInd*/
+            CSR_MA_PACKET_INDICATION *ind = &unpacked_signal.u.MaPacketIndication;
+
+            macHdrLocation = (u8 *) bulkdata->d[0].os_data_ptr;
+            /* Fetch the frame control value from  mac header */
+            frmCtrl = CSR_GET_UINT16_FROM_LITTLE_ENDIAN(macHdrLocation);
+
+            /* Point to the addresses */
+            raddr = macHdrLocation + MAC_HEADER_ADDR1_OFFSET;
+            taddr = macHdrLocation + MAC_HEADER_ADDR2_OFFSET;
+
+            memcpy(peerMacAddress.a, taddr, ETH_ALEN);
+
+            if(ind->ReceptionStatus == CSR_MICHAEL_MIC_ERROR)
+            {
+                if (*raddr & 0x1)
+                    unicastPdu = FALSE;
+
+                CsrWifiRouterCtrlMicFailureIndSend (priv->CSR_WIFI_SME_IFACEQUEUE, 0,
+                        (ind->VirtualInterfaceIdentifier & 0xff),peerMacAddress,
+                        unicastPdu);
+                return;
+            }
+            else
+            {
+                if(ind->ReceptionStatus == CSR_RX_SUCCESS)
+                {
+                    u8 pmBit = (frmCtrl & 0x1000)?0x01:0x00;
+                    u16 interfaceTag = (ind->VirtualInterfaceIdentifier & 0xff);
+                    CsrWifiRouterCtrlStaInfo_t *srcStaInfo =  CsrWifiRouterCtrlGetStationRecordFromPeerMacAddress(priv,taddr,interfaceTag);
+                    if((srcStaInfo != NULL) && (uf_check_broadcast_bssid(priv, bulkdata)== FALSE))
+                    {
+                        uf_process_pm_bit_for_peer(priv,srcStaInfo,pmBit,interfaceTag);
+
+                        /* Update station last activity flag */
+                        srcStaInfo->activity_flag = TRUE;
+                    }
+                }
+            }
+        }
+
+        if (unpacked_signal.SignalPrimitiveHeader.SignalId == CSR_MA_PACKET_CONFIRM_ID)
+        {
+            CSR_MA_PACKET_CONFIRM *cfm = &unpacked_signal.u.MaPacketConfirm;
+            u16 interfaceTag = (cfm->VirtualInterfaceIdentifier & 0xff);
+            netInterface_priv_t *interfacePriv;
+            CSR_MA_PACKET_REQUEST *req;
+            CsrWifiMacAddress peerMacAddress;
+
+            if (interfaceTag >= CSR_WIFI_NUM_INTERFACES)
+            {
+                unifi_error(priv, "Bad MA_PACKET_CONFIRM interfaceTag %d\n", interfaceTag);
+                func_exit();
+                return;
+            }
+
+            unifi_trace(priv,UDBG1,"MA-PACKET Confirm (%x, %x)\n", cfm->HostTag, cfm->TransmissionStatus);
+
+            interfacePriv = priv->interfacePriv[interfaceTag];
+#ifdef CSR_SUPPORT_SME
+            if(interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_AP ||
+                 interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_P2PGO) {
+
+                if(cfm->HostTag == interfacePriv->multicastPduHostTag){
+                    uf_process_ma_pkt_cfm_for_ap(priv ,interfaceTag, cfm);
+                }
+            }
+#endif
+
+            req = &interfacePriv->m4_signal.u.MaPacketRequest;
+
+            if(cfm->HostTag & 0x80000000)
+            {
+                if (cfm->TransmissionStatus != CSR_TX_SUCCESSFUL)
+                {
+                    result = CSR_RESULT_FAILURE;
+                }
+#ifdef CSR_SUPPORT_SME
+                memcpy(peerMacAddress.a, req->Ra.x, ETH_ALEN);
+                /* Check if this is a confirm for EAPOL M4 frame and we need to send transmistted ind*/
+                if (interfacePriv->m4_sent && (cfm->HostTag == interfacePriv->m4_hostTag))
+                {
+                    unifi_trace(priv, UDBG1, "%s: Sending M4 Transmit CFM\n", __FUNCTION__);
+                    CsrWifiRouterCtrlM4TransmittedIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0,
+                            interfaceTag,
+                            peerMacAddress,
+                            result);
+                    interfacePriv->m4_sent = FALSE;
+                    interfacePriv->m4_hostTag = 0xffffffff;
+                }
+#endif
+                /* If EAPOL was requested via router APIs then send cfm else ignore*/
+                if((cfm->HostTag & 0x80000000) != CSR_WIFI_EAPOL_M4_HOST_TAG) {
+                    CsrWifiRouterMaPacketCfmSend((u16)signal[2],
+                        cfm->VirtualInterfaceIdentifier,
+                        result,
+                        (cfm->HostTag & 0x3fffffff), cfm->Rate);
+                } else {
+                    unifi_trace(priv, UDBG1, "%s: M4 received from netdevice\n", __FUNCTION__);
+                }
+                func_exit();
+                return;
+            }
+        }
+    }
+
+    mlmeCommand.length = signal_len;
+    mlmeCommand.data = (u8*)signal;
+
+    dataref1.length = bulkdata->d[0].data_length;
+    if (dataref1.length > 0) {
+        dataref1.data = (u8 *) bulkdata->d[0].os_data_ptr;
+    } else
+    {
+        dataref1.data = NULL;
+    }
+
+    dataref2.length = bulkdata->d[1].data_length;
+    if (dataref2.length > 0) {
+        dataref2.data = (u8 *) bulkdata->d[1].os_data_ptr;
+    } else
+    {
+        dataref2.data = NULL;
+    }
+
+    CsrWifiRouterCtrlHipIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, mlmeCommand.length, mlmeCommand.data,
+            dataref1.length, dataref1.data,
+            dataref2.length, dataref2.data);
+
+    func_exit();
+} /* sme_log_event() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ * uf_sme_port_state
+ *
+ *      Return the state of the controlled port.
+ *
+ * Arguments:
+ *      priv            Pointer to device private context struct
+ *      address    Pointer to the destination for tx or sender for rx address
+ *      queue           Controlled or uncontrolled queue
+ *
+ * Returns:
+ *      An unifi_ControlledPortAction value.
+ * ---------------------------------------------------------------------------
+ */
+CsrWifiRouterCtrlPortAction
+uf_sme_port_state(unifi_priv_t *priv, unsigned char *address, int queue, u16 interfaceTag)
+{
+    int i;
+    unifi_port_config_t *port;
+    netInterface_priv_t *interfacePriv;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_sme_port_state: bad interfaceTag\n");
+        return CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+    }
+
+    interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (queue == UF_CONTROLLED_PORT_Q) {
+        port = &interfacePriv->controlled_data_port;
+    } else {
+        port = &interfacePriv->uncontrolled_data_port;
+    }
+
+    if (!port->entries_in_use) {
+        unifi_trace(priv, UDBG5, "No port configurations, return Discard.\n");
+        return CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_CLOSED_DISCARD;
+    }
+
+    /* If the port configuration is common for all destinations, return it. */
+    if (port->overide_action == UF_DATA_PORT_OVERIDE) {
+        unifi_trace(priv, UDBG5, "Single port configuration (%d).\n",
+                port->port_cfg[0].port_action);
+        return port->port_cfg[0].port_action;
+    }
+
+    unifi_trace(priv, UDBG5, "Multiple (%d) port configurations.\n", port->entries_in_use);
+
+    /* If multiple configurations exist.. */
+    for (i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        /* .. go through the list and match the destination address. */
+        if (port->port_cfg[i].in_use &&
+            memcmp(address, port->port_cfg[i].mac_address.a, ETH_ALEN) == 0) {
+            /* Return the desired action. */
+            return port->port_cfg[i].port_action;
+        }
+    }
+
+    /* Could not find any information, return Open. */
+    unifi_trace(priv, UDBG5, "port configuration not found, return Open.\n");
+    return CSR_WIFI_ROUTER_CTRL_PORT_ACTION_8021X_PORT_OPEN;
+} /* uf_sme_port_state() */
+
+/*
+ * ---------------------------------------------------------------------------
+ * uf_sme_port_config_handle
+ *
+ *      Return the port config handle of the controlled/uncontrolled port.
+ *
+ * Arguments:
+ *      priv            Pointer to device private context struct
+ *      address    Pointer to the destination for tx or sender for rx address
+ *      queue           Controlled or uncontrolled queue
+ *
+ * Returns:
+ *      An  unifi_port_cfg_t* .
+ * ---------------------------------------------------------------------------
+ */
+unifi_port_cfg_t*
+uf_sme_port_config_handle(unifi_priv_t *priv, unsigned char *address, int queue, u16 interfaceTag)
+{
+    int i;
+    unifi_port_config_t *port;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_sme_port_config_handle: bad interfaceTag\n");
+        return NULL;
+    }
+
+    if (queue == UF_CONTROLLED_PORT_Q) {
+        port = &interfacePriv->controlled_data_port;
+    } else {
+        port = &interfacePriv->uncontrolled_data_port;
+    }
+
+    if (!port->entries_in_use) {
+        unifi_trace(priv, UDBG5, "No port configurations, return Discard.\n");
+        return NULL;
+    }
+
+    /* If the port configuration is common for all destinations, return it. */
+    if (port->overide_action == UF_DATA_PORT_OVERIDE) {
+        unifi_trace(priv, UDBG5, "Single port configuration (%d).\n",
+                port->port_cfg[0].port_action);
+        if (address) {
+            unifi_trace(priv, UDBG5, "addr[0] = %x, addr[1] = %x, addr[2] = %x, addr[3] = %x\n", address[0], address[1], address[2], address[3]);
+        }
+        return &port->port_cfg[0];
+    }
+
+    unifi_trace(priv, UDBG5, "Multiple port configurations.\n");
+
+    /* If multiple configurations exist.. */
+    for (i = 0; i < UNIFI_MAX_CONNECTIONS; i++) {
+        /* .. go through the list and match the destination address. */
+        if (port->port_cfg[i].in_use &&
+            memcmp(address, port->port_cfg[i].mac_address.a, ETH_ALEN) == 0) {
+            /* Return the desired action. */
+            return &port->port_cfg[i];
+        }
+    }
+
+    /* Could not find any information, return Open. */
+    unifi_trace(priv, UDBG5, "port configuration not found, returning NULL (debug).\n");
+    return NULL;
+} /* uf_sme_port_config_handle */
+
+void
+uf_multicast_list_wq(struct work_struct *work)
+{
+    unifi_priv_t *priv = container_of(work, unifi_priv_t,
+            multicast_list_task);
+    int i;
+    u16 interfaceTag = 0;
+    CsrWifiMacAddress* multicast_address_list = NULL;
+    int mc_count;
+    u8 *mc_list;
+    netInterface_priv_t *interfacePriv = priv->interfacePriv[interfaceTag];
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "uf_multicast_list_wq: bad interfaceTag\n");
+        return;
+    }
+
+    unifi_trace(priv, UDBG5,
+            "uf_multicast_list_wq: list count = %d\n",
+            interfacePriv->mc_list_count);
+
+    /* Flush the current list */
+    CsrWifiRouterCtrlMulticastAddressIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0, interfaceTag, CSR_WIFI_SME_LIST_ACTION_FLUSH, 0, NULL);
+
+    mc_count = interfacePriv->mc_list_count;
+    mc_list = interfacePriv->mc_list;
+    /*
+     * Allocate a new list, need to free it later
+     * in unifi_mgt_multicast_address_cfm().
+     */
+    multicast_address_list = kmalloc(mc_count * sizeof(CsrWifiMacAddress), GFP_KERNEL);
+
+    if (multicast_address_list == NULL) {
+        return;
+    }
+
+    for (i = 0; i < mc_count; i++) {
+        memcpy(multicast_address_list[i].a, mc_list, ETH_ALEN);
+        mc_list += ETH_ALEN;
+    }
+
+    if (priv->smepriv == NULL) {
+        kfree(multicast_address_list);
+        return;
+    }
+
+    CsrWifiRouterCtrlMulticastAddressIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,
+            interfaceTag,
+            CSR_WIFI_SME_LIST_ACTION_ADD,
+            mc_count, multicast_address_list);
+
+    /* The SME will take a copy of the addreses*/
+    kfree(multicast_address_list);
+}
+
+
+int unifi_cfg_power(unifi_priv_t *priv, unsigned char *arg)
+{
+    unifi_cfg_power_t cfg_power;
+    int rc;
+    int wol;
+
+    if (get_user(cfg_power, (unifi_cfg_power_t*)(((unifi_cfg_command_t*)arg) + 1))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    switch (cfg_power) {
+        case UNIFI_CFG_POWER_OFF:
+            priv->wol_suspend = (enable_wol == UNIFI_WOL_OFF) ? FALSE : TRUE;
+            rc = sme_sys_suspend(priv);
+            if (rc) {
+                return rc;
+            }
+            break;
+        case UNIFI_CFG_POWER_ON:
+            wol = priv->wol_suspend;
+            rc = sme_sys_resume(priv);
+            if (rc) {
+                return rc;
+            }
+            if (wol) {
+                /* Kick the BH to ensure pending transfers are handled when
+                 * a suspend happened with card powered.
+                 */
+                unifi_send_signal(priv->card, NULL, 0, NULL);
+            }
+            break;
+        default:
+            unifi_error(priv, "WIFI POWER: Unknown value.\n");
+            return -EINVAL;
+    }
+
+    return 0;
+}
+
+
+int unifi_cfg_power_save(unifi_priv_t *priv, unsigned char *arg)
+{
+    unifi_cfg_powersave_t cfg_power_save;
+    CsrWifiSmePowerConfig powerConfig;
+    int rc;
+
+    if (get_user(cfg_power_save, (unifi_cfg_powersave_t*)(((unifi_cfg_command_t*)arg) + 1))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    /* Get the coex info from the SME */
+    rc = sme_mgt_power_config_get(priv, &powerConfig);
+    if (rc) {
+        unifi_error(priv, "UNIFI_CFG: Get unifi_PowerConfigValue failed.\n");
+        return rc;
+    }
+
+    switch (cfg_power_save) {
+        case UNIFI_CFG_POWERSAVE_NONE:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_LOW;
+            break;
+        case UNIFI_CFG_POWERSAVE_FAST:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_MED;
+            break;
+        case UNIFI_CFG_POWERSAVE_FULL:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_HIGH;
+            break;
+        case UNIFI_CFG_POWERSAVE_AUTO:
+            powerConfig.powerSaveLevel = CSR_WIFI_SME_POWER_SAVE_LEVEL_AUTO;
+            break;
+        default:
+            unifi_error(priv, "POWERSAVE: Unknown value.\n");
+            return -EINVAL;
+    }
+
+    rc = sme_mgt_power_config_set(priv, &powerConfig);
+
+    if (rc) {
+        unifi_error(priv, "UNIFI_CFG: Set unifi_PowerConfigValue failed.\n");
+    }
+
+    return rc;
+}
+
+
+int unifi_cfg_power_supply(unifi_priv_t *priv, unsigned char *arg)
+{
+    unifi_cfg_powersupply_t cfg_power_supply;
+    CsrWifiSmeHostConfig hostConfig;
+    int rc;
+
+    if (get_user(cfg_power_supply, (unifi_cfg_powersupply_t*)(((unifi_cfg_command_t*)arg) + 1))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    /* Get the coex info from the SME */
+    rc = sme_mgt_host_config_get(priv, &hostConfig);
+    if (rc) {
+        unifi_error(priv, "UNIFI_CFG: Get unifi_HostConfigValue failed.\n");
+        return rc;
+    }
+
+    switch (cfg_power_supply) {
+        case UNIFI_CFG_POWERSUPPLY_MAINS:
+            hostConfig.powerMode = CSR_WIFI_SME_HOST_POWER_MODE_ACTIVE;
+            break;
+        case UNIFI_CFG_POWERSUPPLY_BATTERIES:
+            hostConfig.powerMode = CSR_WIFI_SME_HOST_POWER_MODE_POWER_SAVE;
+            break;
+        default:
+            unifi_error(priv, "POWERSUPPLY: Unknown value.\n");
+            return -EINVAL;
+    }
+
+    rc = sme_mgt_host_config_set(priv, &hostConfig);
+    if (rc) {
+        unifi_error(priv, "UNIFI_CFG: Set unifi_HostConfigValue failed.\n");
+    }
+
+    return rc;
+}
+
+
+int unifi_cfg_packet_filters(unifi_priv_t *priv, unsigned char *arg)
+{
+    unsigned char *tclas_buffer;
+    unsigned int tclas_buffer_length;
+    tclas_t *dhcp_tclas;
+    int rc;
+
+    /* Free any TCLASs previously allocated */
+    if (priv->packet_filters.tclas_ies_length) {
+        kfree(priv->filter_tclas_ies);
+        priv->filter_tclas_ies = NULL;
+    }
+
+    tclas_buffer = ((unsigned char*)arg) + sizeof(unifi_cfg_command_t) + sizeof(unsigned int);
+    if (copy_from_user(&priv->packet_filters, (void*)tclas_buffer,
+                sizeof(uf_cfg_bcast_packet_filter_t))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the filter struct\n");
+        return -EFAULT;
+    }
+
+    tclas_buffer_length = priv->packet_filters.tclas_ies_length;
+
+    /* Allocate TCLASs if necessary */
+    if (priv->packet_filters.dhcp_filter) {
+        priv->packet_filters.tclas_ies_length += sizeof(tclas_t);
+    }
+    if (priv->packet_filters.tclas_ies_length > 0) {
+        priv->filter_tclas_ies = kmalloc(priv->packet_filters.tclas_ies_length, GFP_KERNEL);
+        if (priv->filter_tclas_ies == NULL) {
+            return -ENOMEM;
+        }
+        if (tclas_buffer_length) {
+            tclas_buffer += sizeof(uf_cfg_bcast_packet_filter_t) - sizeof(unsigned char*);
+            if (copy_from_user(priv->filter_tclas_ies,
+                        tclas_buffer,
+                        tclas_buffer_length)) {
+                unifi_error(priv, "UNIFI_CFG: Failed to get the TCLAS buffer\n");
+                return -EFAULT;
+            }
+        }
+    }
+
+    if(priv->packet_filters.dhcp_filter)
+    {
+        /* Append the DHCP tclas IE */
+        dhcp_tclas = (tclas_t*)(priv->filter_tclas_ies + tclas_buffer_length);
+        memset(dhcp_tclas, 0, sizeof(tclas_t));
+        dhcp_tclas->element_id = 14;
+        dhcp_tclas->length = sizeof(tcpip_clsfr_t) + 1;
+        dhcp_tclas->user_priority = 0;
+        dhcp_tclas->tcp_ip_cls_fr.cls_fr_type = 1;
+        dhcp_tclas->tcp_ip_cls_fr.version = 4;
+        ((u8*)(&dhcp_tclas->tcp_ip_cls_fr.source_port))[0] = 0x00;
+        ((u8*)(&dhcp_tclas->tcp_ip_cls_fr.source_port))[1] = 0x44;
+        ((u8*)(&dhcp_tclas->tcp_ip_cls_fr.dest_port))[0] = 0x00;
+        ((u8*)(&dhcp_tclas->tcp_ip_cls_fr.dest_port))[1] = 0x43;
+        dhcp_tclas->tcp_ip_cls_fr.protocol = 0x11;
+        dhcp_tclas->tcp_ip_cls_fr.cls_fr_mask = 0x58; //bits: 3,4,6
+    }
+
+    rc = sme_mgt_packet_filter_set(priv);
+
+    return rc;
+}
+
+
+int unifi_cfg_wmm_qos_info(unifi_priv_t *priv, unsigned char *arg)
+{
+    u8 wmm_qos_info;
+    int rc = 0;
+
+    if (get_user(wmm_qos_info, (u8*)(((unifi_cfg_command_t*)arg) + 1))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    /* Store the value in the connection info */
+    priv->connection_config.wmmQosInfo = wmm_qos_info;
+
+    return rc;
+}
+
+
+int unifi_cfg_wmm_addts(unifi_priv_t *priv, unsigned char *arg)
+{
+    u32 addts_tid;
+    u8 addts_ie_length;
+    u8 *addts_ie;
+    u8 *addts_params;
+    CsrWifiSmeDataBlock tspec;
+    CsrWifiSmeDataBlock tclas;
+    int rc;
+
+    addts_params = (u8*)(((unifi_cfg_command_t*)arg) + 1);
+    if (get_user(addts_tid, (u32*)addts_params)) {
+        unifi_error(priv, "unifi_cfg_wmm_addts: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    addts_params += sizeof(u32);
+    if (get_user(addts_ie_length, (u8*)addts_params)) {
+        unifi_error(priv, "unifi_cfg_wmm_addts: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG4, "addts: tid = 0x%x ie_length = %d\n",
+            addts_tid, addts_ie_length);
+
+    addts_ie = kmalloc(addts_ie_length, GFP_KERNEL);
+    if (addts_ie == NULL) {
+        unifi_error(priv,
+                "unifi_cfg_wmm_addts: Failed to malloc %d bytes for addts_ie buffer\n",
+                addts_ie_length);
+        return -ENOMEM;
+    }
+
+    addts_params += sizeof(u8);
+    rc = copy_from_user(addts_ie, addts_params, addts_ie_length);
+    if (rc) {
+        unifi_error(priv, "unifi_cfg_wmm_addts: Failed to get the addts buffer\n");
+        kfree(addts_ie);
+        return -EFAULT;
+    }
+
+    tspec.data = addts_ie;
+    tspec.length = addts_ie_length;
+    tclas.data = NULL;
+    tclas.length = 0;
+
+    rc = sme_mgt_tspec(priv, CSR_WIFI_SME_LIST_ACTION_ADD, addts_tid,
+            &tspec, &tclas);
+
+    kfree(addts_ie);
+    return rc;
+}
+
+
+int unifi_cfg_wmm_delts(unifi_priv_t *priv, unsigned char *arg)
+{
+    u32 delts_tid;
+    u8 *delts_params;
+    CsrWifiSmeDataBlock tspec;
+    CsrWifiSmeDataBlock tclas;
+    int rc;
+
+    delts_params = (u8*)(((unifi_cfg_command_t*)arg) + 1);
+    if (get_user(delts_tid, (u32*)delts_params)) {
+        unifi_error(priv, "unifi_cfg_wmm_delts: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG4, "delts: tid = 0x%x\n", delts_tid);
+
+    tspec.data = tclas.data = NULL;
+    tspec.length = tclas.length = 0;
+
+    rc = sme_mgt_tspec(priv, CSR_WIFI_SME_LIST_ACTION_REMOVE, delts_tid,
+            &tspec, &tclas);
+
+    return rc;
+}
+
+int unifi_cfg_strict_draft_n(unifi_priv_t *priv, unsigned char *arg)
+{
+    u8 strict_draft_n;
+    u8 *strict_draft_n_params;
+    int rc;
+
+    CsrWifiSmeStaConfig  staConfig;
+    CsrWifiSmeDeviceConfig  deviceConfig;
+
+    strict_draft_n_params = (u8*)(((unifi_cfg_command_t*)arg) + 1);
+    if (get_user(strict_draft_n, (u8*)strict_draft_n_params)) {
+        unifi_error(priv, "unifi_cfg_strict_draft_n: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG4, "strict_draft_n: = %s\n", ((strict_draft_n) ? "yes":"no"));
+
+    rc = sme_mgt_sme_config_get(priv, &staConfig, &deviceConfig);
+
+    if (rc) {
+        unifi_warning(priv, "unifi_cfg_strict_draft_n: Get unifi_SMEConfigValue failed.\n");
+        return -EFAULT;
+    }
+
+    deviceConfig.enableStrictDraftN = strict_draft_n;
+
+    rc = sme_mgt_sme_config_set(priv, &staConfig, &deviceConfig);
+    if (rc) {
+        unifi_warning(priv, "unifi_cfg_strict_draft_n: Set unifi_SMEConfigValue failed.\n");
+        rc = -EFAULT;
+    }
+
+    return rc;
+}
+
+
+int unifi_cfg_enable_okc(unifi_priv_t *priv, unsigned char *arg)
+{
+    u8 enable_okc;
+    u8 *enable_okc_params;
+    int rc;
+
+    CsrWifiSmeStaConfig staConfig;
+    CsrWifiSmeDeviceConfig deviceConfig;
+
+    enable_okc_params = (u8*)(((unifi_cfg_command_t*)arg) + 1);
+    if (get_user(enable_okc, (u8*)enable_okc_params)) {
+        unifi_error(priv, "unifi_cfg_enable_okc: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    unifi_trace(priv, UDBG4, "enable_okc: = %s\n", ((enable_okc) ? "yes":"no"));
+
+    rc = sme_mgt_sme_config_get(priv, &staConfig, &deviceConfig);
+    if (rc) {
+        unifi_warning(priv, "unifi_cfg_enable_okc: Get unifi_SMEConfigValue failed.\n");
+        return -EFAULT;
+    }
+
+    staConfig.enableOpportunisticKeyCaching = enable_okc;
+
+    rc = sme_mgt_sme_config_set(priv, &staConfig, &deviceConfig);
+    if (rc) {
+        unifi_warning(priv, "unifi_cfg_enable_okc: Set unifi_SMEConfigValue failed.\n");
+        rc = -EFAULT;
+    }
+
+    return rc;
+}
+
+
+int unifi_cfg_get_info(unifi_priv_t *priv, unsigned char *arg)
+{
+    unifi_cfg_get_t get_cmd;
+    char inst_name[IFNAMSIZ];
+    int rc;
+
+    if (get_user(get_cmd, (unifi_cfg_get_t*)(((unifi_cfg_command_t*)arg) + 1))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the argument\n");
+        return -EFAULT;
+    }
+
+    switch (get_cmd) {
+        case UNIFI_CFG_GET_COEX:
+            {
+                CsrWifiSmeCoexInfo coexInfo;
+                /* Get the coex info from the SME */
+                rc = sme_mgt_coex_info_get(priv, &coexInfo);
+                if (rc) {
+                    unifi_error(priv, "UNIFI_CFG: Get unifi_CoexInfoValue failed.\n");
+                    return rc;
+                }
+
+                /* Copy the info to the out buffer */
+                if (copy_to_user((void*)arg,
+                            &coexInfo,
+                            sizeof(CsrWifiSmeCoexInfo))) {
+                    unifi_error(priv, "UNIFI_CFG: Failed to copy the coex info\n");
+                    return -EFAULT;
+                }
+                break;
+            }
+        case UNIFI_CFG_GET_POWER_MODE:
+            {
+                CsrWifiSmePowerConfig powerConfig;
+                rc = sme_mgt_power_config_get(priv, &powerConfig);
+                if (rc) {
+                    unifi_error(priv, "UNIFI_CFG: Get unifi_PowerConfigValue failed.\n");
+                    return rc;
+                }
+
+                /* Copy the info to the out buffer */
+                if (copy_to_user((void*)arg,
+                            &powerConfig.powerSaveLevel,
+                            sizeof(CsrWifiSmePowerSaveLevel))) {
+                    unifi_error(priv, "UNIFI_CFG: Failed to copy the power save info\n");
+                    return -EFAULT;
+                }
+                break;
+            }
+        case UNIFI_CFG_GET_POWER_SUPPLY:
+            {
+                CsrWifiSmeHostConfig hostConfig;
+                rc = sme_mgt_host_config_get(priv, &hostConfig);
+                if (rc) {
+                    unifi_error(priv, "UNIFI_CFG: Get unifi_HostConfigValue failed.\n");
+                    return rc;
+                }
+
+                /* Copy the info to the out buffer */
+                if (copy_to_user((void*)arg,
+                            &hostConfig.powerMode,
+                            sizeof(CsrWifiSmeHostPowerMode))) {
+                    unifi_error(priv, "UNIFI_CFG: Failed to copy the host power mode\n");
+                    return -EFAULT;
+                }
+                break;
+            }
+        case UNIFI_CFG_GET_VERSIONS:
+            break;
+        case UNIFI_CFG_GET_INSTANCE:
+            {
+                u16 InterfaceId=0;
+                uf_net_get_name(priv->netdev[InterfaceId], &inst_name[0], sizeof(inst_name));
+
+                /* Copy the info to the out buffer */
+                if (copy_to_user((void*)arg,
+                            &inst_name[0],
+                            sizeof(inst_name))) {
+                    unifi_error(priv, "UNIFI_CFG: Failed to copy the instance name\n");
+                    return -EFAULT;
+                }
+            }
+            break;
+
+        case UNIFI_CFG_GET_AP_CONFIG:
+            {
+#ifdef CSR_SUPPORT_WEXT_AP
+                uf_cfg_ap_config_t cfg_ap_config;
+                cfg_ap_config.channel = priv->ap_config.channel;
+                cfg_ap_config.beaconInterval = priv->ap_mac_config.beaconInterval;
+                cfg_ap_config.wmmEnabled = priv->ap_mac_config.wmmEnabled;
+                cfg_ap_config.dtimPeriod = priv->ap_mac_config.dtimPeriod;
+                cfg_ap_config.phySupportedBitmap = priv->ap_mac_config.phySupportedBitmap;
+                if (copy_to_user((void*)arg,
+                            &cfg_ap_config,
+                            sizeof(uf_cfg_ap_config_t))) {
+                    unifi_error(priv, "UNIFI_CFG: Failed to copy the AP configuration\n");
+                    return -EFAULT;
+                }
+#else
+                   return -EPERM;
+#endif
+            }
+            break;
+
+
+        default:
+            unifi_error(priv, "unifi_cfg_get_info: Unknown value.\n");
+            return -EINVAL;
+    }
+
+    return 0;
+}
+#ifdef CSR_SUPPORT_WEXT_AP
+int
+ uf_configure_supported_rates(u8 * supportedRates, u8 phySupportedBitmap)
+{
+    int i=0;
+    u8 b=FALSE, g = FALSE, n = FALSE;
+    b = phySupportedBitmap & CSR_WIFI_SME_AP_PHY_SUPPORT_B;
+    n = phySupportedBitmap & CSR_WIFI_SME_AP_PHY_SUPPORT_N;
+    g = phySupportedBitmap & CSR_WIFI_SME_AP_PHY_SUPPORT_G;
+    if(b || g) {
+        supportedRates[i++]=0x82;
+        supportedRates[i++]=0x84;
+        supportedRates[i++]=0x8b;
+        supportedRates[i++]=0x96;
+    } else if(n) {
+        /* For some strange reasons WiFi stack needs both b and g rates*/
+        supportedRates[i++]=0x02;
+        supportedRates[i++]=0x04;
+        supportedRates[i++]=0x0b;
+        supportedRates[i++]=0x16;
+        supportedRates[i++]=0x0c;
+        supportedRates[i++]=0x12;
+        supportedRates[i++]=0x18;
+	supportedRates[i++]=0x24;
+        supportedRates[i++]=0x30;
+        supportedRates[i++]=0x48;
+        supportedRates[i++]=0x60;
+        supportedRates[i++]=0x6c;
+    }
+    if(g) {
+        if(!b) {
+            supportedRates[i++]=0x8c;
+            supportedRates[i++]=0x98;
+            supportedRates[i++]=0xb0;
+        } else {
+            supportedRates[i++]=0x0c;
+            supportedRates[i++]=0x18;
+            supportedRates[i++]=0x30;
+        }
+        supportedRates[i++]=0x48;
+        supportedRates[i++]=0x12;
+        supportedRates[i++]=0x24;
+        supportedRates[i++]=0x60;
+        supportedRates[i++]=0x6c;
+    }
+    return i;
+}
+int unifi_cfg_set_ap_config(unifi_priv_t * priv,unsigned char* arg)
+{
+    uf_cfg_ap_config_t cfg_ap_config;
+    char *buffer;
+
+    buffer = ((unsigned char*)arg) + sizeof(unifi_cfg_command_t) + sizeof(unsigned int);
+    if (copy_from_user(&cfg_ap_config, (void*)buffer,
+                sizeof(uf_cfg_ap_config_t))) {
+        unifi_error(priv, "UNIFI_CFG: Failed to get the ap config struct\n");
+        return -EFAULT;
+    }
+    priv->ap_config.channel = cfg_ap_config.channel;
+    priv->ap_mac_config.dtimPeriod = cfg_ap_config.dtimPeriod;
+    priv->ap_mac_config.beaconInterval = cfg_ap_config.beaconInterval;
+    priv->group_sec_config.apGroupkeyTimeout = cfg_ap_config.groupkeyTimeout;
+    priv->group_sec_config.apStrictGtkRekey = cfg_ap_config.strictGtkRekeyEnabled;
+    priv->group_sec_config.apGmkTimeout = cfg_ap_config.gmkTimeout;
+    priv->group_sec_config.apResponseTimeout = cfg_ap_config.responseTimeout;
+    priv->group_sec_config.apRetransLimit = cfg_ap_config.retransLimit;
+
+    priv->ap_mac_config.shortSlotTimeEnabled = cfg_ap_config.shortSlotTimeEnabled;
+    priv->ap_mac_config.ctsProtectionType=cfg_ap_config.ctsProtectionType;
+
+    priv->ap_mac_config.wmmEnabled = cfg_ap_config.wmmEnabled;
+
+    priv->ap_mac_config.apHtParams.rxStbc=cfg_ap_config.rxStbc;
+    priv->ap_mac_config.apHtParams.rifsModeAllowed=cfg_ap_config.rifsModeAllowed;
+
+    priv->ap_mac_config.phySupportedBitmap = cfg_ap_config.phySupportedBitmap;
+    priv->ap_mac_config.maxListenInterval=cfg_ap_config.maxListenInterval;
+
+    priv->ap_mac_config.supportedRatesCount=     uf_configure_supported_rates(priv->ap_mac_config.supportedRates,priv->ap_mac_config.phySupportedBitmap);
+
+    return 0;
+}
+
+#endif
+#ifdef CSR_SUPPORT_WEXT
+
+    void
+uf_sme_config_wq(struct work_struct *work)
+{
+    CsrWifiSmeStaConfig  staConfig;
+    CsrWifiSmeDeviceConfig  deviceConfig;
+    unifi_priv_t *priv = container_of(work, unifi_priv_t, sme_config_task);
+
+    /* Register to receive indications from the SME */
+    CsrWifiSmeEventMaskSetReqSend(0,
+            CSR_WIFI_SME_INDICATIONS_WIFIOFF | CSR_WIFI_SME_INDICATIONS_CONNECTIONQUALITY |
+            CSR_WIFI_SME_INDICATIONS_MEDIASTATUS | CSR_WIFI_SME_INDICATIONS_MICFAILURE);
+
+    if (sme_mgt_sme_config_get(priv, &staConfig, &deviceConfig)) {
+        unifi_warning(priv, "uf_sme_config_wq: Get unifi_SMEConfigValue failed.\n");
+        return;
+    }
+
+    if (priv->if_index == CSR_INDEX_5G) {
+        staConfig.ifIndex = CSR_WIFI_SME_RADIO_IF_GHZ_5_0;
+    } else {
+        staConfig.ifIndex = CSR_WIFI_SME_RADIO_IF_GHZ_2_4;
+    }
+
+    deviceConfig.trustLevel = (CsrWifiSme80211dTrustLevel)tl_80211d;
+    if (sme_mgt_sme_config_set(priv, &staConfig, &deviceConfig)) {
+        unifi_warning(priv,
+                "SME config for 802.11d Trust Level and Radio Band failed.\n");
+        return;
+    }
+
+} /* uf_sme_config_wq() */
+
+#endif /* CSR_SUPPORT_WEXT */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_ta_ind_wq
+ *
+ *      Deferred work queue function to send Traffic Analysis protocols
+ *      indications to the SME.
+ *      These are done in a deferred work queue for two reasons:
+ *       - the CsrWifiRouterCtrl...Send() functions are not safe for atomic context
+ *       - we want to load the main driver data path as lightly as possible
+ *
+ *      The TA classifications already come from a workqueue.
+ *
+ *  Arguments:
+ *      work    Pointer to work queue item.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+    void
+uf_ta_ind_wq(struct work_struct *work)
+{
+    struct ta_ind *ind = container_of(work, struct ta_ind, task);
+    unifi_priv_t *priv = container_of(ind, unifi_priv_t, ta_ind_work);
+    u16 interfaceTag = 0;
+
+
+    CsrWifiRouterCtrlTrafficProtocolIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0,
+            interfaceTag,
+            ind->packet_type,
+            ind->direction,
+            ind->src_addr);
+    ind->in_use = 0;
+
+} /* uf_ta_ind_wq() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_ta_sample_ind_wq
+ *
+ *      Deferred work queue function to send Traffic Analysis sample
+ *      indications to the SME.
+ *      These are done in a deferred work queue for two reasons:
+ *       - the CsrWifiRouterCtrl...Send() functions are not safe for atomic context
+ *       - we want to load the main driver data path as lightly as possible
+ *
+ *      The TA classifications already come from a workqueue.
+ *
+ *  Arguments:
+ *      work    Pointer to work queue item.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+    void
+uf_ta_sample_ind_wq(struct work_struct *work)
+{
+    struct ta_sample_ind *ind = container_of(work, struct ta_sample_ind, task);
+    unifi_priv_t *priv = container_of(ind, unifi_priv_t, ta_sample_ind_work);
+    u16 interfaceTag = 0;
+
+     unifi_trace(priv, UDBG5, "rxtcp %d txtcp %d rxudp %d txudp %d prio %d\n",
+        priv->rxTcpThroughput,
+        priv->txTcpThroughput,
+        priv->rxUdpThroughput,
+        priv->txUdpThroughput,
+        priv->bh_thread.prio);
+
+    if(priv->rxTcpThroughput > 1000)
+    {
+        if (bh_priority == -1 && priv->bh_thread.prio != 1)
+        {
+            struct sched_param param;
+            priv->bh_thread.prio = 1;
+            unifi_trace(priv, UDBG1, "%s new thread (RT) priority = %d\n",
+                        priv->bh_thread.name, priv->bh_thread.prio);
+            param.sched_priority = priv->bh_thread.prio;
+            sched_setscheduler(priv->bh_thread.thread_task, SCHED_FIFO, &param);
+        }
+    } else
+    {
+        if (bh_priority == -1 && priv->bh_thread.prio != DEFAULT_PRIO)
+        {
+            struct sched_param param;
+            param.sched_priority = 0;
+            sched_setscheduler(priv->bh_thread.thread_task, SCHED_NORMAL, &param);
+            priv->bh_thread.prio = DEFAULT_PRIO;
+            unifi_trace(priv, UDBG1, "%s new thread priority = %d\n",
+                        priv->bh_thread.name, priv->bh_thread.prio);
+            set_user_nice(priv->bh_thread.thread_task, PRIO_TO_NICE(priv->bh_thread.prio));
+        }
+    }
+
+    CsrWifiRouterCtrlTrafficSampleIndSend(priv->CSR_WIFI_SME_IFACEQUEUE,0, interfaceTag, ind->stats);
+
+    ind->in_use = 0;
+
+} /* uf_ta_sample_ind_wq() */
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_send_m4_ready_wq
+ *
+ *      Deferred work queue function to send M4 ReadyToSend inds to the SME.
+ *      These are done in a deferred work queue for two reasons:
+ *       - the CsrWifiRouterCtrl...Send() functions are not safe for atomic context
+ *       - we want to load the main driver data path as lightly as possible
+ *
+ *  Arguments:
+ *      work    Pointer to work queue item.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void
+uf_send_m4_ready_wq(struct work_struct *work)
+{
+    netInterface_priv_t *InterfacePriv = container_of(work, netInterface_priv_t, send_m4_ready_task);
+    u16 iface = InterfacePriv->InterfaceTag;
+    unifi_priv_t *priv = InterfacePriv->privPtr;
+    CSR_MA_PACKET_REQUEST *req = &InterfacePriv->m4_signal.u.MaPacketRequest;
+    CsrWifiMacAddress peer;
+    unsigned long flags;
+
+    func_enter();
+
+    /* The peer address was stored in the signal */
+    spin_lock_irqsave(&priv->m4_lock, flags);
+    memcpy(peer.a, req->Ra.x, sizeof(peer.a));
+    spin_unlock_irqrestore(&priv->m4_lock, flags);
+
+    /* Send a signal to SME */
+    CsrWifiRouterCtrlM4ReadyToSendIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0, iface, peer);
+
+	unifi_trace(priv, UDBG1, "M4ReadyToSendInd sent for peer %pMF\n",
+		peer.a);
+
+    func_exit();
+
+} /* uf_send_m4_ready_wq() */
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+/*
+ * ---------------------------------------------------------------------------
+ *  uf_send_pkt_to_encrypt
+ *
+ *      Deferred work queue function to send the WAPI data pkts to SME when unicast KeyId = 1
+ *      These are done in a deferred work queue for two reasons:
+ *       - the CsrWifiRouterCtrl...Send() functions are not safe for atomic context
+ *       - we want to load the main driver data path as lightly as possible
+ *
+ *  Arguments:
+ *      work    Pointer to work queue item.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+void uf_send_pkt_to_encrypt(struct work_struct *work)
+{
+    netInterface_priv_t *interfacePriv = container_of(work, netInterface_priv_t, send_pkt_to_encrypt);
+    u16 interfaceTag = interfacePriv->InterfaceTag;
+    unifi_priv_t *priv = interfacePriv->privPtr;
+
+    u32 pktBulkDataLength;
+    u8 *pktBulkData;
+    unsigned long flags;
+
+    if (interfacePriv->interfaceMode == CSR_WIFI_ROUTER_CTRL_MODE_STA) {
+
+        func_enter();
+
+        pktBulkDataLength = interfacePriv->wapi_unicast_bulk_data.data_length;
+
+        if (pktBulkDataLength > 0) {
+		    pktBulkData = kmalloc(pktBulkDataLength, GFP_KERNEL);
+		    memset(pktBulkData, 0, pktBulkDataLength);
+	    } else {
+		    unifi_error(priv, "uf_send_pkt_to_encrypt() : invalid buffer\n");
+		    return;
+	    }
+
+        spin_lock_irqsave(&priv->wapi_lock, flags);
+        /* Copy over the MA PKT REQ bulk data */
+        memcpy(pktBulkData, (u8*)interfacePriv->wapi_unicast_bulk_data.os_data_ptr, pktBulkDataLength);
+        /* Free any bulk data buffers allocated for the WAPI Data pkt */
+        unifi_net_data_free(priv, &interfacePriv->wapi_unicast_bulk_data);
+        interfacePriv->wapi_unicast_bulk_data.net_buf_length = 0;
+        interfacePriv->wapi_unicast_bulk_data.data_length = 0;
+        interfacePriv->wapi_unicast_bulk_data.os_data_ptr = interfacePriv->wapi_unicast_bulk_data.os_net_buf_ptr = NULL;
+        spin_unlock_irqrestore(&priv->wapi_lock, flags);
+
+        CsrWifiRouterCtrlWapiUnicastTxEncryptIndSend(priv->CSR_WIFI_SME_IFACEQUEUE, 0, interfaceTag, pktBulkDataLength, pktBulkData);
+        unifi_trace(priv, UDBG1, "WapiUnicastTxEncryptInd sent to SME\n");
+
+        kfree(pktBulkData); /* Would have been copied over by the SME Handler */
+
+        func_exit();
+    } else {
+	    unifi_warning(priv, "uf_send_pkt_to_encrypt() is NOT applicable for interface mode - %d\n",interfacePriv->interfaceMode);
+    }
+}/* uf_send_pkt_to_encrypt() */
+#endif
diff --git a/drivers/staging/csr/unifi_sme.h b/drivers/staging/csr/unifi_sme.h
new file mode 100644
index 00000000000..b689cfe2b10
--- /dev/null
+++ b/drivers/staging/csr/unifi_sme.h
@@ -0,0 +1,245 @@
+/*
+ * ***************************************************************************
+ *  FILE:     unifi_sme.h
+ *
+ *  PURPOSE:    SME related definitions.
+ *
+ *  Copyright (C) 2007-2011 by Cambridge Silicon Radio Ltd.
+ *
+ *  Refer to LICENSE.txt included with this source code for details on
+ *  the license terms.
+ *
+ * ***************************************************************************
+ */
+#ifndef __LINUX_UNIFI_SME_H__
+#define __LINUX_UNIFI_SME_H__ 1
+
+#include <linux/kernel.h>
+
+#ifdef CSR_SME_USERSPACE
+#include "sme_userspace.h"
+#endif
+
+#include "csr_wifi_sme_lib.h"
+
+typedef int unifi_data_port_action;
+
+typedef struct unifi_port_cfg
+{
+    /* TRUE if this port entry is allocated */
+    u8 in_use;
+    CsrWifiRouterCtrlPortAction port_action;
+    CsrWifiMacAddress mac_address;
+} unifi_port_cfg_t;
+
+#define UNIFI_MAX_CONNECTIONS           8
+#define UNIFI_MAX_RETRY_LIMIT           5
+#define UF_DATA_PORT_NOT_OVERIDE        0
+#define UF_DATA_PORT_OVERIDE            1
+
+typedef struct unifi_port_config
+{
+    int entries_in_use;
+    int overide_action;
+    unifi_port_cfg_t port_cfg[UNIFI_MAX_CONNECTIONS];
+} unifi_port_config_t;
+
+
+enum sme_request_status {
+    SME_REQUEST_EMPTY,
+    SME_REQUEST_PENDING,
+    SME_REQUEST_RECEIVED,
+    SME_REQUEST_TIMEDOUT,
+    SME_REQUEST_CANCELLED,
+};
+
+/* Structure to hold a UDI logged signal */
+typedef struct {
+
+    /* The current status of the request */
+    enum sme_request_status request_status;
+
+    /* The status the SME has passed to us */
+    CsrResult reply_status;
+
+    /* SME's reply to a get request */
+    CsrWifiSmeVersions versions;
+    CsrWifiSmePowerConfig powerConfig;
+    CsrWifiSmeHostConfig hostConfig;
+    CsrWifiSmeStaConfig staConfig;
+    CsrWifiSmeDeviceConfig deviceConfig;
+    CsrWifiSmeCoexInfo coexInfo;
+    CsrWifiSmeCoexConfig coexConfig;
+    CsrWifiSmeMibConfig mibConfig;
+    CsrWifiSmeConnectionInfo connectionInfo;
+    CsrWifiSmeConnectionConfig connectionConfig;
+    CsrWifiSmeConnectionStats connectionStats;
+
+
+    /* SME's reply to a scan request */
+    u16 reply_scan_results_count;
+    CsrWifiSmeScanResult* reply_scan_results;
+
+} sme_reply_t;
+
+
+typedef struct {
+    u16 appHandle;
+    CsrWifiRouterEncapsulation encapsulation;
+    u16 protocol;
+    u8 oui[3];
+    u8 in_use;
+} sme_ma_unidata_ind_filter_t;
+
+
+CsrWifiRouterCtrlPortAction uf_sme_port_state(unifi_priv_t *priv,
+                                          unsigned char *address,
+                                          int queue,
+                                          u16 interfaceTag);
+unifi_port_cfg_t *uf_sme_port_config_handle(unifi_priv_t *priv,
+                                            unsigned char *address,
+                                            int queue,
+                                            u16 interfaceTag);
+
+
+
+/* Callback for event logging to SME clients */
+void sme_log_event(ul_client_t *client, const u8 *signal, int signal_len,
+                   const bulk_data_param_t *bulkdata, int dir);
+
+/* The workqueue task to the set the multicast addresses list */
+void uf_multicast_list_wq(struct work_struct *work);
+
+/* The workqueue task to execute the TA module */
+void uf_ta_wq(struct work_struct *work);
+
+
+/*
+ * SME blocking helper functions
+ */
+#ifdef UNIFI_DEBUG
+# define sme_complete_request(priv, status)   uf_sme_complete_request(priv, status, __func__)
+#else
+# define sme_complete_request(priv, status)   uf_sme_complete_request(priv, status, NULL)
+#endif
+
+void uf_sme_complete_request(unifi_priv_t *priv, CsrResult reply_status, const char *func);
+void uf_sme_cancel_request(unifi_priv_t *priv, CsrResult reply_status);
+
+
+/*
+ * Blocking functions using the SME SYS API.
+ */
+int sme_sys_suspend(unifi_priv_t *priv);
+int sme_sys_resume(unifi_priv_t *priv);
+
+
+/*
+ * Traffic Analysis workqueue jobs
+ */
+void uf_ta_ind_wq(struct work_struct *work);
+void uf_ta_sample_ind_wq(struct work_struct *work);
+
+/*
+ * SME config workqueue job
+ */
+void uf_sme_config_wq(struct work_struct *work);
+
+/*
+ * To send M4 read to send IND
+ */
+void uf_send_m4_ready_wq(struct work_struct *work);
+
+#if (defined(CSR_WIFI_SECURITY_WAPI_ENABLE) && defined(CSR_WIFI_SECURITY_WAPI_SW_ENCRYPTION))
+/*
+ * To send data pkt to Sme for encryption
+ */
+void uf_send_pkt_to_encrypt(struct work_struct *work);
+#endif
+
+int sme_mgt_power_config_set(unifi_priv_t *priv, CsrWifiSmePowerConfig *powerConfig);
+int sme_mgt_power_config_get(unifi_priv_t *priv, CsrWifiSmePowerConfig *powerConfig);
+int sme_mgt_host_config_set(unifi_priv_t *priv, CsrWifiSmeHostConfig *hostConfig);
+int sme_mgt_host_config_get(unifi_priv_t *priv, CsrWifiSmeHostConfig *hostConfig);
+int sme_mgt_sme_config_set(unifi_priv_t *priv, CsrWifiSmeStaConfig *staConfig, CsrWifiSmeDeviceConfig *deviceConfig);
+int sme_mgt_sme_config_get(unifi_priv_t *priv, CsrWifiSmeStaConfig *staConfig, CsrWifiSmeDeviceConfig *deviceConfig);
+int sme_mgt_coex_info_get(unifi_priv_t *priv, CsrWifiSmeCoexInfo *coexInfo);
+int sme_mgt_packet_filter_set(unifi_priv_t *priv);
+int sme_mgt_tspec(unifi_priv_t *priv, CsrWifiSmeListAction action,
+                  u32 tid, CsrWifiSmeDataBlock *tspec, CsrWifiSmeDataBlock *tclas);
+
+#ifdef CSR_SUPPORT_WEXT
+/*
+ * Blocking functions using the SME MGT API.
+ */
+int sme_mgt_wifi_on(unifi_priv_t *priv);
+int sme_mgt_wifi_off(unifi_priv_t *priv);
+/*int sme_mgt_set_value_async(unifi_priv_t *priv, unifi_AppValue *app_value);
+int sme_mgt_get_value_async(unifi_priv_t *priv, unifi_AppValue *app_value);
+int sme_mgt_get_value(unifi_priv_t *priv, unifi_AppValue *app_value);
+int sme_mgt_set_value(unifi_priv_t *priv, unifi_AppValue *app_value);
+*/
+int sme_mgt_coex_config_set(unifi_priv_t *priv, CsrWifiSmeCoexConfig *coexConfig);
+int sme_mgt_coex_config_get(unifi_priv_t *priv, CsrWifiSmeCoexConfig *coexConfig);
+int sme_mgt_mib_config_set(unifi_priv_t *priv, CsrWifiSmeMibConfig *mibConfig);
+int sme_mgt_mib_config_get(unifi_priv_t *priv, CsrWifiSmeMibConfig *mibConfig);
+
+int sme_mgt_connection_info_set(unifi_priv_t *priv, CsrWifiSmeConnectionInfo *connectionInfo);
+int sme_mgt_connection_info_get(unifi_priv_t *priv, CsrWifiSmeConnectionInfo *connectionInfo);
+int sme_mgt_connection_config_set(unifi_priv_t *priv, CsrWifiSmeConnectionConfig *connectionConfig);
+int sme_mgt_connection_config_get(unifi_priv_t *priv, CsrWifiSmeConnectionConfig *connectionConfig);
+int sme_mgt_connection_stats_get(unifi_priv_t *priv, CsrWifiSmeConnectionStats *connectionStats);
+
+int sme_mgt_versions_get(unifi_priv_t *priv, CsrWifiSmeVersions *versions);
+
+
+int sme_mgt_scan_full(unifi_priv_t *priv, CsrWifiSsid *specific_ssid,
+                      int num_channels, unsigned char *channel_list);
+int sme_mgt_scan_results_get_async(unifi_priv_t *priv,
+                                   struct iw_request_info *info,
+                                   char *scan_results,
+                                   long scan_results_len);
+int sme_mgt_disconnect(unifi_priv_t *priv);
+int sme_mgt_connect(unifi_priv_t *priv);
+int sme_mgt_key(unifi_priv_t *priv, CsrWifiSmeKey *sme_key,
+                CsrWifiSmeListAction action);
+int sme_mgt_pmkid(unifi_priv_t *priv, CsrWifiSmeListAction action,
+                  CsrWifiSmePmkidList *pmkid_list);
+int sme_mgt_mib_get(unifi_priv_t *priv,
+                    unsigned char *varbind, int *length);
+int sme_mgt_mib_set(unifi_priv_t *priv,
+                    unsigned char *varbind, int length);
+#ifdef CSR_SUPPORT_WEXT_AP
+int sme_ap_start(unifi_priv_t *priv,u16 interface_tag,CsrWifiSmeApConfig_t *ap_config);
+int sme_ap_stop(unifi_priv_t *priv,u16 interface_tag);
+int sme_ap_config(unifi_priv_t *priv,CsrWifiSmeApMacConfig *ap_mac_config, CsrWifiNmeApConfig *group_security_config);
+int uf_configure_supported_rates(u8 * supportedRates, u8 phySupportedBitmap);
+#endif
+int unifi_translate_scan(struct net_device *dev,
+                         struct iw_request_info *info,
+                         char *current_ev, char *end_buf,
+                         CsrWifiSmeScanResult *scan_data,
+                         int scan_index);
+
+#endif /* CSR_SUPPORT_WEXT */
+
+int unifi_cfg_power(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_power_save(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_power_supply(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_packet_filters(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_wmm_qos_info(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_wmm_addts(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_wmm_delts(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_get_info(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_strict_draft_n(unifi_priv_t *priv, unsigned char *arg);
+int unifi_cfg_enable_okc(unifi_priv_t *priv, unsigned char *arg);
+#ifdef CSR_SUPPORT_WEXT_AP
+int unifi_cfg_set_ap_config(unifi_priv_t * priv,unsigned char* arg);
+#endif
+
+
+
+int convert_sme_error(CsrResult error);
+
+
+#endif /* __LINUX_UNIFI_SME_H__ */
diff --git a/drivers/staging/csr/unifi_wext.h b/drivers/staging/csr/unifi_wext.h
new file mode 100644
index 00000000000..6d7a9959508
--- /dev/null
+++ b/drivers/staging/csr/unifi_wext.h
@@ -0,0 +1,124 @@
+/*
+ *****************************************************************************
+ *
+ * FILE : unifi_wext.h
+ *
+ * PURPOSE : Private header file for unifi driver support to wireless extensions.
+ *
+ * Copyright (C) 2005-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+*****************************************************************************
+ */
+#ifndef __LINUX_UNIFI_WEXT_H__
+#define __LINUX_UNIFI_WEXT_H__ 1
+
+#include <linux/kernel.h>
+#include <net/iw_handler.h>
+#include "csr_wifi_sme_prim.h"
+
+/*
+ *      wext.c
+ */
+/* A few details needed for WEP (Wireless Equivalent Privacy) */
+#define UNIFI_MAX_KEY_SIZE      16
+#define NUM_WEPKEYS              4
+#define SMALL_KEY_SIZE           5
+#define LARGE_KEY_SIZE          13
+typedef struct wep_key_t {
+    int len;
+    unsigned char key[UNIFI_MAX_KEY_SIZE];  /* 40-bit and 104-bit keys */
+} wep_key_t;
+
+#define UNIFI_SCAN_ACTIVE       0
+#define UNIFI_SCAN_PASSIVE      1
+#define UNIFI_MAX_SSID_LEN      32
+
+#define MAX_WPA_IE_LEN 64
+#define MAX_RSN_IE_LEN 255
+
+/*
+ * Function to register in the netdev to report wireless stats.
+ */
+struct iw_statistics *unifi_get_wireless_stats(struct net_device *dev);
+
+void uf_sme_wext_set_defaults(unifi_priv_t *priv);
+
+
+/*
+ *      wext_events.c
+ */
+/* Functions to generate Wireless Extension events */
+void wext_send_scan_results_event(unifi_priv_t *priv);
+void wext_send_assoc_event(unifi_priv_t *priv, unsigned char *bssid,
+                           unsigned char *req_ie, int req_ie_len,
+                           unsigned char *resp_ie, int resp_ie_len,
+                           unsigned char *scan_ie, unsigned int scan_ie_len);
+void wext_send_disassoc_event(unifi_priv_t *priv);
+void wext_send_michaelmicfailure_event(unifi_priv_t *priv,
+                                       u16 count, CsrWifiMacAddress address,
+                                       CsrWifiSmeKeyType keyType, u16 interfaceTag);
+void wext_send_pmkid_candidate_event(unifi_priv_t *priv, CsrWifiMacAddress bssid, u8 preauth_allowed, u16 interfaceTag);
+void wext_send_started_event(unifi_priv_t *priv);
+
+
+static inline int
+uf_iwe_stream_add_point(struct iw_request_info *info, char *start, char *stop,
+                        struct iw_event *piwe, char *extra)
+{
+    char *new_start;
+
+    new_start = iwe_stream_add_point(
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) || defined (IW_REQUEST_FLAG_COMPAT)
+                                     info,
+#endif
+                                     start, stop, piwe, extra);
+    if (unlikely(new_start == start))
+    {
+        return -E2BIG;
+    }
+
+    return (new_start - start);
+}
+
+
+static inline int
+uf_iwe_stream_add_event(struct iw_request_info *info, char *start, char *stop,
+                        struct iw_event *piwe, int len)
+{
+    char *new_start;
+
+    new_start = iwe_stream_add_event(
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) || defined(IW_REQUEST_FLAG_COMPAT)
+                                     info,
+#endif
+                                     start, stop, piwe, len);
+    if (unlikely(new_start == start)) {
+        return -E2BIG;
+    }
+
+    return (new_start - start);
+}
+
+static inline int
+uf_iwe_stream_add_value(struct iw_request_info *info, char *stream, char *start,
+                        char *stop, struct iw_event *piwe, int len)
+{
+    char *new_start;
+
+    new_start = iwe_stream_add_value(
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27) || defined(IW_REQUEST_FLAG_COMPAT)
+                                     info,
+#endif
+                                     stream, start, stop, piwe, len);
+    if (unlikely(new_start == start)) {
+        return -E2BIG;
+    }
+
+    return (new_start - start);
+}
+
+
+#endif /* __LINUX_UNIFI_WEXT_H__ */
diff --git a/drivers/staging/csr/unifiio.h b/drivers/staging/csr/unifiio.h
new file mode 100644
index 00000000000..b9de0cb94e9
--- /dev/null
+++ b/drivers/staging/csr/unifiio.h
@@ -0,0 +1,398 @@
+/*
+ * ---------------------------------------------------------------------------
+ *
+ *  FILE: unifiio.h
+ *
+ *      Public definitions for the UniFi linux driver.
+ *      This is mostly ioctl command values and structs.
+ *
+ *      Include <sys/ioctl.h> or similar before this file
+ *
+ * Copyright (C) 2005-2009 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#ifndef __UNIFIIO_H__
+#define __UNIFIIO_H__
+
+#include <linux/types.h>
+
+#define UNIFI_GET_UDI_ENABLE    _IOR('u',  1, int)
+#define UNIFI_SET_UDI_ENABLE    _IOW('u',  2, int)
+/* Values for UDI_ENABLE */
+#define UDI_ENABLE_DATA         0x1
+#define UDI_ENABLE_CONTROL      0x2
+
+/* MIB set/get. Arg is a pointer to a varbind */
+#define UNIFI_GET_MIB           _IOWR('u',  3, unsigned char *)
+#define UNIFI_SET_MIB           _IOW ('u',  4, unsigned char *)
+#define MAX_VARBIND_LENGTH 127
+
+/* Private IOCTLs */
+#define SIOCIWS80211POWERSAVEPRIV           SIOCIWFIRSTPRIV
+#define SIOCIWG80211POWERSAVEPRIV           SIOCIWFIRSTPRIV + 1
+#define SIOCIWS80211RELOADDEFAULTSPRIV      SIOCIWFIRSTPRIV + 2
+#define SIOCIWSCONFWAPIPRIV                 SIOCIWFIRSTPRIV + 4
+#define SIOCIWSWAPIKEYPRIV                  SIOCIWFIRSTPRIV + 6
+#define SIOCIWSSMEDEBUGPRIV                 SIOCIWFIRSTPRIV + 8
+#define SIOCIWSAPCFGPRIV                    SIOCIWFIRSTPRIV + 10
+#define SIOCIWSAPSTARTPRIV                  SIOCIWFIRSTPRIV + 12
+#define SIOCIWSAPSTOPPRIV                   SIOCIWFIRSTPRIV + 14
+#define SIOCIWSFWRELOADPRIV                 SIOCIWFIRSTPRIV + 16
+#define SIOCIWSSTACKSTART                   SIOCIWFIRSTPRIV + 18
+#define SIOCIWSSTACKSTOP                    SIOCIWFIRSTPRIV + 20
+
+
+
+#define IWPRIV_POWER_SAVE_MAX_STRING 32
+#define IWPRIV_SME_DEBUG_MAX_STRING 32
+#define IWPRIV_SME_MAX_STRING 120
+
+
+/* Private configuration commands */
+#define UNIFI_CFG               _IOWR('u',  5, unsigned char *)
+/*
+ * <------------------  Read/Write Buffer  -------------------->
+ * _____________________________________________________________
+ * |    Cmd    |    Arg    |   ...  Buffer (opt)  ...          |
+ * -------------------------------------------------------------
+ * <-- uint --><-- uint --><-----  unsigned char buffer  ------>
+ *
+ * Cmd:    A unifi_cfg_command_t command.
+ * Arg:    Out:Length     if Cmd==UNIFI_CFG_GET
+ *         In:PowerOnOff  if Cmd==UNIFI_CFG_POWER
+ *         In:PowerMode   if Cmd==UNIFI_CFG_POWERSAVE
+ *         In:Length      if Cmd==UNIFI_CFG_FILTER
+ *         In:WMM Qos Info if Cmd==UNIFI_CFG_WMM_QOS_INFO
+ * Buffer: Out:Data       if Cmd==UNIFI_CFG_GET
+ *         NULL           if Cmd==UNIFI_CFG_POWER
+ *         NULL           if Cmd==UNIFI_CFG_POWERSAVE
+ *         In:Filters     if Cmd==UNIFI_CFG_FILTER
+ *
+ * where Filters is a uf_cfg_bcast_packet_filter_t structure
+ * followed by 0 - n tclas_t structures. The length of the tclas_t
+ * structures is obtained by uf_cfg_bcast_packet_filter_t::tclas_ies_length.
+ */
+
+
+#define UNIFI_PUTEST            _IOWR('u',  6, unsigned char *)
+/*
+ * <------------------  Read/Write Buffer  -------------------->
+ * _____________________________________________________________
+ * |    Cmd    |    Arg    |   ...  Buffer (opt)  ...          |
+ * -------------------------------------------------------------
+ * <-- uint --><-- uint --><-----  unsigned char buffer  ------>
+ *
+ * Cmd:    A unifi_putest_command_t command.
+ * Arg:    N/A                           if Cmd==UNIFI_PUTEST_START
+ *         N/A                           if Cmd==UNIFI_PUTEST_STOP
+ *         In:int (Clock Speed)          if Cmd==UNIFI_PUTEST_SET_SDIO_CLOCK
+ *         In/Out:sizeof(unifi_putest_cmd52) if Cmd==UNIFI_PUTEST_CMD52_READ
+ *         In:sizeof(unifi_putest_cmd52) if Cmd==UNIFI_PUTEST_CMD52_WRITE
+ *         In:uint (f/w file name length) if Cmd==UNIFI_PUTEST_DL_FW
+ * Buffer: NULL                          if Cmd==UNIFI_PUTEST_START
+ *         NULL                          if Cmd==UNIFI_PUTEST_STOP
+ *         NULL                          if Cmd==UNIFI_PUTEST_SET_SDIO_CLOCK
+ *         In/Out:unifi_putest_cmd52     if Cmd==UNIFI_PUTEST_CMD52_READ
+ *         In:unifi_putest_cmd52         if Cmd==UNIFI_PUTEST_CMD52_WRITE
+ *         In:f/w file name              if Cmd==UNIFI_PUTEST_DL_FW
+ */
+
+#define UNIFI_BUILD_TYPE _IOWR('u', 7, unsigned char)
+#define UNIFI_BUILD_NME 1
+#define UNIFI_BUILD_WEXT 2
+#define UNIFI_BUILD_AP 3
+
+/* debugging */
+#define UNIFI_KICK              _IO ('u',  0x10)
+#define UNIFI_SET_DEBUG         _IO ('u',  0x11)
+#define UNIFI_SET_TRACE         _IO ('u',  0x12)
+
+#define UNIFI_GET_INIT_STATUS   _IOR ('u', 0x15, int)
+#define UNIFI_SET_UDI_LOG_MASK  _IOR('u',  0x18, unifiio_filter_t)
+#define UNIFI_SET_UDI_SNAP_MASK _IOW('u',  0x1a, unifiio_snap_filter_t)
+#define UNIFI_SET_AMP_ENABLE    _IOWR('u',  0x1b, int)
+
+#define UNIFI_INIT_HW           _IOR ('u', 0x13, unsigned char)
+#define UNIFI_INIT_NETDEV       _IOW ('u', 0x14, unsigned char[6])
+#define UNIFI_SME_PRESENT       _IOW ('u', 0x19, int)
+
+#define UNIFI_CFG_PERIOD_TRAFFIC _IOW ('u', 0x21, unsigned char *)
+#define UNIFI_CFG_UAPSD_TRAFFIC _IOW ('u', 0x22, unsigned char)
+
+#define UNIFI_COREDUMP_GET_REG  _IOWR('u', 0x23, unifiio_coredump_req_t)
+
+
+/*
+ * Following reset, f/w may only be downloaded using CMD52.
+ * This is slow, so there is a facility to download a secondary
+ * loader first which supports CMD53.
+ * If loader_len is > 0, then loader_data is assumed to point to
+ * a suitable secondary loader that can be used to download the
+ * main image.
+ *
+ * The driver will run the host protocol initialisation sequence
+ * after downloading the image.
+ *
+ * If both lengths are zero, then the f/w is assumed to have been
+ * booted from Flash and the host protocol initialisation sequence
+ * is run.
+ */
+typedef struct {
+
+    /* Number of bytes in the image */
+    int img_len;
+
+    /* Pointer to image data. */
+    unsigned char *img_data;
+
+
+    /* Number of bytes in the loader image */
+    int loader_len;
+
+    /* Pointer to loader image data. */
+    unsigned char *loader_data;
+
+} unifiio_img_t;
+
+
+/* Structure of data read from the unifi device. */
+typedef struct
+{
+    /* Length (in bytes) of entire structure including appended bulk data */
+    int length;
+
+    /* System time (in milliseconds) that signal was transferred */
+    int timestamp;
+
+    /* Direction in which signal was transferred. */
+    int direction;
+#define UDI_FROM_HOST   0
+#define UDI_TO_HOST     1
+#define UDI_CONFIG_IND  2
+
+    /* The length of the signal (in bytes) not including bulk data */
+    int signal_length;
+
+    /* Signal body follows, then any bulk data */
+
+} udi_msg_t;
+
+
+typedef enum
+{
+    UfSigFil_AllOn = 0,         /* Log all signal IDs */
+    UfSigFil_AllOff = 1,        /* Don't log any signal IDs */
+    UfSigFil_SelectOn = 2,      /* Log these signal IDs */
+    UfSigFil_SelectOff = 3      /* Don't log these signal IDs */
+} uf_sigfilter_action_t;
+
+typedef struct {
+
+    /* Number of 16-bit ints in the sig_ids array */
+    int num_sig_ids;
+    /* The action to perform */
+    uf_sigfilter_action_t action;
+    /* List of signal IDs to pass or block */
+    unsigned short *sig_ids;
+
+} unifiio_filter_t;
+
+
+typedef struct {
+    /* Number of 16-bit ints in the protocols array */
+    u16 count;
+    /* List of protocol ids to pass */
+    u16 *protocols;
+} unifiio_snap_filter_t;
+
+
+
+typedef u8 unifi_putest_command_t;
+
+#define UNIFI_PUTEST_START 0
+#define UNIFI_PUTEST_STOP 1
+#define UNIFI_PUTEST_SET_SDIO_CLOCK 2
+#define UNIFI_PUTEST_CMD52_READ 3
+#define UNIFI_PUTEST_CMD52_WRITE 4
+#define UNIFI_PUTEST_DL_FW 5
+#define UNIFI_PUTEST_DL_FW_BUFF 6
+#define UNIFI_PUTEST_CMD52_BLOCK_READ 7
+#define UNIFI_PUTEST_COREDUMP_PREPARE 8
+#define UNIFI_PUTEST_GP_READ16 9
+#define UNIFI_PUTEST_GP_WRITE16 10
+
+
+struct unifi_putest_cmd52 {
+    int funcnum;
+    unsigned long addr;
+    unsigned char data;
+};
+
+
+struct unifi_putest_block_cmd52_r {
+    int           funcnum;
+    unsigned long addr;
+    unsigned int  length;
+    unsigned char *data;
+};
+
+struct unifi_putest_gp_rw16 {
+    unsigned long addr;        /* generic address */
+    unsigned short data;
+};
+
+typedef enum unifi_cfg_command {
+    UNIFI_CFG_GET,
+    UNIFI_CFG_POWER,
+    UNIFI_CFG_POWERSAVE,
+    UNIFI_CFG_FILTER,
+    UNIFI_CFG_POWERSUPPLY,
+    UNIFI_CFG_WMM_QOSINFO,
+    UNIFI_CFG_WMM_ADDTS,
+    UNIFI_CFG_WMM_DELTS,
+    UNIFI_CFG_STRICT_DRAFT_N,
+    UNIFI_CFG_ENABLE_OKC,
+    UNIFI_CFG_SET_AP_CONFIG,
+    UNIFI_CFG_CORE_DUMP /* request to take a fw core dump */
+} unifi_cfg_command_t;
+
+typedef enum unifi_cfg_power {
+    UNIFI_CFG_POWER_UNSPECIFIED,
+    UNIFI_CFG_POWER_OFF,
+    UNIFI_CFG_POWER_ON
+} unifi_cfg_power_t;
+
+typedef enum unifi_cfg_powersupply {
+    UNIFI_CFG_POWERSUPPLY_UNSPECIFIED,
+    UNIFI_CFG_POWERSUPPLY_MAINS,
+    UNIFI_CFG_POWERSUPPLY_BATTERIES
+} unifi_cfg_powersupply_t;
+
+typedef enum unifi_cfg_powersave {
+    UNIFI_CFG_POWERSAVE_UNSPECIFIED,
+    UNIFI_CFG_POWERSAVE_NONE,
+    UNIFI_CFG_POWERSAVE_FAST,
+    UNIFI_CFG_POWERSAVE_FULL,
+    UNIFI_CFG_POWERSAVE_AUTO
+} unifi_cfg_powersave_t;
+
+typedef enum unifi_cfg_get {
+    UNIFI_CFG_GET_COEX,
+    UNIFI_CFG_GET_POWER_MODE,
+    UNIFI_CFG_GET_VERSIONS,
+    UNIFI_CFG_GET_POWER_SUPPLY,
+    UNIFI_CFG_GET_INSTANCE,
+    UNIFI_CFG_GET_AP_CONFIG
+} unifi_cfg_get_t;
+
+#define UNIFI_CFG_FILTER_NONE            0x0000
+#define UNIFI_CFG_FILTER_DHCP            0x0001
+#define UNIFI_CFG_FILTER_ARP             0x0002
+#define UNIFI_CFG_FILTER_NBNS            0x0004
+#define UNIFI_CFG_FILTER_NBDS            0x0008
+#define UNIFI_CFG_FILTER_CUPS            0x0010
+#define UNIFI_CFG_FILTER_ALL             0xFFFF
+
+
+typedef struct uf_cfg_bcast_packet_filter
+{
+    unsigned long filter_mode;     //as defined by HIP protocol
+    unsigned char arp_filter;
+    unsigned char dhcp_filter;
+    unsigned long tclas_ies_length; // length of tclas_ies in bytes
+    unsigned char tclas_ies[1];    // variable length depending on above field
+} uf_cfg_bcast_packet_filter_t;
+
+typedef struct uf_cfg_ap_config
+{
+    u8    phySupportedBitmap;
+    u8    channel;
+    u16   beaconInterval;
+    u8    dtimPeriod;
+    u8     wmmEnabled;
+    u8    shortSlotTimeEnabled;
+    u16   groupkeyTimeout;
+    u8     strictGtkRekeyEnabled;
+    u16   gmkTimeout;
+    u16   responseTimeout;
+    u8    retransLimit;
+    u8    rxStbc;
+    u8     rifsModeAllowed;
+    u8    dualCtsProtection;
+    u8    ctsProtectionType;
+    u16   maxListenInterval;
+}uf_cfg_ap_config_t;
+
+typedef struct tcpic_clsfr
+{
+    __u8 cls_fr_type;
+    __u8 cls_fr_mask;
+    __u8 version;
+    __u8 source_ip_addr[4];
+    __u8 dest_ip_addr[4];
+    __u16 source_port;
+    __u16 dest_port;
+    __u8 dscp;
+    __u8 protocol;
+    __u8 reserved;
+} __attribute__ ((packed)) tcpip_clsfr_t;
+
+typedef struct tclas {
+    __u8 element_id;
+    __u8 length;
+    __u8 user_priority;
+    tcpip_clsfr_t tcp_ip_cls_fr;
+} __attribute__ ((packed)) tclas_t;
+
+
+#define CONFIG_IND_ERROR            0x01
+#define CONFIG_IND_EXIT             0x02
+#define CONFIG_SME_NOT_PRESENT      0x10
+#define CONFIG_SME_PRESENT          0x20
+
+/* WAPI Key */
+typedef struct
+{
+    u8                          unicastKey;
+    /* If non zero, then unicast key otherwise group key */
+    u8                          keyIndex;
+    u8                          keyRsc[16];
+    u8                          authenticator;
+    /* If non zero, then authenticator otherwise supplicant */
+    u8                          address[6];
+    u8                          key[32];
+} unifiio_wapi_key_t;
+
+/* Values describing XAP memory regions captured by the mini-coredump system */
+typedef enum unifiio_coredump_space {
+    UNIFIIO_COREDUMP_MAC_REG,
+    UNIFIIO_COREDUMP_PHY_REG,
+    UNIFIIO_COREDUMP_SH_DMEM,
+    UNIFIIO_COREDUMP_MAC_DMEM,
+    UNIFIIO_COREDUMP_PHY_DMEM,
+    UNIFIIO_COREDUMP_TRIGGER_MAGIC = 0xFEED
+} unifiio_coredump_space_t;
+
+/* Userspace tool uses this structure to retrieve a register value from a
+ * mini-coredump buffer previously saved by the HIP
+ */
+typedef struct unifiio_coredump_req {
+    /* From user */
+    int index;                      /* 0=newest, -1=oldest */
+    unsigned int offset;            /* register offset in space */
+    unifiio_coredump_space_t space; /* memory space */
+    /* Filled by driver */
+    unsigned int drv_build;         /* driver build id */
+    unsigned int chip_ver;          /* chip version */
+    unsigned int fw_ver;            /* firmware version */
+    int requestor;                  /* requestor: 0=auto dump, 1=manual */
+    unsigned int timestamp;         /* time of capture by driver */
+    unsigned int serial;            /* capture serial number */
+    int value;                      /* 16 bit register value, -ve for error */
+} unifiio_coredump_req_t;           /* Core-dumped register value request */
+
+#endif /* __UNIFIIO_H__ */
diff --git a/drivers/staging/csr/wext_events.c b/drivers/staging/csr/wext_events.c
new file mode 100644
index 00000000000..d356887ac4c
--- /dev/null
+++ b/drivers/staging/csr/wext_events.c
@@ -0,0 +1,285 @@
+/*
+ * ---------------------------------------------------------------------------
+ * FILE:     wext_events.c
+ *
+ * PURPOSE:
+ *      Code to generate iwevents.
+ *
+ * Copyright (C) 2006-2008 by Cambridge Silicon Radio Ltd.
+ *
+ * Refer to LICENSE.txt included with this source code for details on
+ * the license terms.
+ *
+ * ---------------------------------------------------------------------------
+ */
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+#include "csr_wifi_hip_unifi.h"
+#include "unifi_priv.h"
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  wext_send_assoc_event
+ *
+ *      Send wireless-extension events up to userland to announce
+ *      successful association with an AP.
+ *
+ *  Arguments:
+ *      priv                    Pointer to driver context.
+ *      bssid                   MAC address of AP we associated with
+ *      req_ie, req_ie_len      IEs in the original request
+ *      resp_ie, resp_ie_len    IEs in the response
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      This is sent on first successful association, and again if we
+ *      roam to another AP.
+ * ---------------------------------------------------------------------------
+ */
+void
+wext_send_assoc_event(unifi_priv_t *priv, unsigned char *bssid,
+                      unsigned char *req_ie, int req_ie_len,
+                      unsigned char *resp_ie, int resp_ie_len,
+                      unsigned char *scan_ie, unsigned int scan_ie_len)
+{
+#if WIRELESS_EXT > 17
+    union iwreq_data wrqu;
+
+    if (req_ie_len == 0) req_ie = NULL;
+    wrqu.data.length = req_ie_len;
+    wrqu.data.flags = 0;
+    wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], IWEVASSOCREQIE, &wrqu, req_ie);
+
+    if (resp_ie_len == 0) resp_ie = NULL;
+    wrqu.data.length = resp_ie_len;
+    wrqu.data.flags = 0;
+    wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], IWEVASSOCRESPIE, &wrqu, resp_ie);
+
+    if (scan_ie_len > 0) {
+        wrqu.data.length = scan_ie_len;
+        wrqu.data.flags = 0;
+        wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], IWEVGENIE, &wrqu, scan_ie);
+    }
+
+    memcpy(&wrqu.ap_addr.sa_data, bssid, ETH_ALEN);
+    wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], SIOCGIWAP, &wrqu, NULL);
+#endif
+} /* wext_send_assoc_event() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  wext_send_disassoc_event
+ *
+ *      Send a wireless-extension event up to userland to announce
+ *      that we disassociated from an AP.
+ *
+ *  Arguments:
+ *      priv                    Pointer to driver context.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      The semantics of wpa_supplicant (the userland SME application) are
+ *      that a SIOCGIWAP event with MAC address of all zero means
+ *      disassociate.
+ * ---------------------------------------------------------------------------
+ */
+void
+wext_send_disassoc_event(unifi_priv_t *priv)
+{
+#if WIRELESS_EXT > 17
+    union iwreq_data wrqu;
+
+    memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+    wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], SIOCGIWAP, &wrqu, NULL);
+#endif
+} /* wext_send_disassoc_event() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  wext_send_scan_results_event
+ *
+ *      Send wireless-extension events up to userland to announce
+ *      completion of a scan.
+ *
+ *  Arguments:
+ *      priv                    Pointer to driver context.
+ *
+ *  Returns:
+ *      None.
+ *
+ *  Notes:
+ *      This doesn't actually report the results, they are retrieved
+ *      using the SIOCGIWSCAN ioctl command.
+ * ---------------------------------------------------------------------------
+ */
+void
+wext_send_scan_results_event(unifi_priv_t *priv)
+{
+#if WIRELESS_EXT > 17
+    union iwreq_data wrqu;
+
+    wrqu.data.length = 0;
+    wrqu.data.flags = 0;
+    wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], SIOCGIWSCAN, &wrqu, NULL);
+
+#endif
+} /* wext_send_scan_results_event() */
+
+
+
+/*
+ * ---------------------------------------------------------------------------
+ *  wext_send_michaelmicfailure_event
+ *
+ *      Send wireless-extension events up to userland to announce
+ *      completion of a scan.
+ *
+ *  Arguments:
+ *      priv            Pointer to driver context.
+ *      count, macaddr, key_type, key_idx, tsc
+ *                      Parameters from report from UniFi.
+ *
+ *  Returns:
+ *      None.
+ * ---------------------------------------------------------------------------
+ */
+#if WIRELESS_EXT >= 18
+static inline void
+_send_michaelmicfailure_event(struct net_device *dev,
+                              int count, const unsigned char *macaddr,
+                              int key_type, int key_idx,
+                              unsigned char *tsc)
+{
+    union iwreq_data wrqu;
+    struct iw_michaelmicfailure mmf;
+
+    memset(&mmf, 0, sizeof(mmf));
+
+    mmf.flags = key_idx & IW_MICFAILURE_KEY_ID;
+    if (key_type == CSR_GROUP) {
+        mmf.flags |= IW_MICFAILURE_GROUP;
+    } else {
+        mmf.flags |= IW_MICFAILURE_PAIRWISE;
+    }
+    mmf.flags |= ((count << 5) & IW_MICFAILURE_COUNT);
+
+    mmf.src_addr.sa_family = ARPHRD_ETHER;
+    memcpy(mmf.src_addr.sa_data, macaddr, ETH_ALEN);
+
+    memcpy(mmf.tsc, tsc, IW_ENCODE_SEQ_MAX_SIZE);
+
+    memset(&wrqu, 0, sizeof(wrqu));
+    wrqu.data.length = sizeof(mmf);
+
+    wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&mmf);
+}
+#elif WIRELESS_EXT >= 15
+static inline void
+_send_michaelmicfailure_event(struct net_device *dev,
+                              int count, const unsigned char *macaddr,
+                              int key_type, int key_idx,
+                              unsigned char *tsc)
+{
+    union iwreq_data wrqu;
+    char buf[128];
+
+    sprintf(buf,
+            "MLME-MICHAELMICFAILURE.indication(keyid=%d %scast addr=%02x:%02x:%02x:%02x:%02x:%02x)",
+            key_idx, (key_type == CSR_GROUP) ? "broad" : "uni",
+            macaddr[0], macaddr[1], macaddr[2],
+            macaddr[3], macaddr[4], macaddr[5]);
+    memset(&wrqu, 0, sizeof(wrqu));
+    wrqu.data.length = strlen(buf);
+    wireless_send_event(dev, IWEVCUSTOM, &wrqu, buf);
+}
+#else /* WIRELESS_EXT >= 15 */
+static inline void
+_send_michaelmicfailure_event(struct net_device *dev,
+                              int count, const unsigned char *macaddr,
+                              int key_type, int key_idx,
+                              unsigned char *tsc)
+{
+    /* Not supported before WEXT 15 */
+}
+#endif /* WIRELESS_EXT >= 15 */
+
+
+void
+wext_send_michaelmicfailure_event(unifi_priv_t *priv,
+                                  u16 count,
+                                  CsrWifiMacAddress address,
+                                  CsrWifiSmeKeyType keyType,
+                                  u16 interfaceTag)
+{
+    unsigned char tsc[8] = {0};
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "wext_send_michaelmicfailure_event bad interfaceTag\n");
+        return;
+    }
+
+    _send_michaelmicfailure_event(priv->netdev[interfaceTag],
+                                  count,
+                                  address.a,
+                                  keyType,
+                                  0,
+                                  tsc);
+} /* wext_send_michaelmicfailure_event() */
+
+void
+wext_send_pmkid_candidate_event(unifi_priv_t *priv, CsrWifiMacAddress bssid, u8 preauth_allowed, u16 interfaceTag)
+{
+#if WIRELESS_EXT > 17
+    union iwreq_data wrqu;
+    struct iw_pmkid_cand pmkid_cand;
+
+    if (interfaceTag >= CSR_WIFI_NUM_INTERFACES) {
+        unifi_error(priv, "wext_send_pmkid_candidate_event bad interfaceTag\n");
+        return;
+    }
+
+    memset(&pmkid_cand, 0, sizeof(pmkid_cand));
+
+    if (preauth_allowed) {
+        pmkid_cand.flags |= IW_PMKID_CAND_PREAUTH;
+    }
+    pmkid_cand.bssid.sa_family = ARPHRD_ETHER;
+    memcpy(pmkid_cand.bssid.sa_data, bssid.a, ETH_ALEN);
+    /* Used as priority, smaller the number higher the priority, not really used in our case */
+    pmkid_cand.index = 1;
+
+    memset(&wrqu, 0, sizeof(wrqu));
+    wrqu.data.length = sizeof(pmkid_cand);
+
+    wireless_send_event(priv->netdev[interfaceTag], IWEVPMKIDCAND, &wrqu, (char *)&pmkid_cand);
+#endif
+} /* wext_send_pmkid_candidate_event() */
+
+/*
+ * Send a custom WEXT event to say we have completed initialisation
+ * and are now ready for WEXT ioctls. Used by Android wpa_supplicant.
+ */
+void
+wext_send_started_event(unifi_priv_t *priv)
+{
+#if WIRELESS_EXT > 17
+    union iwreq_data wrqu;
+    char data[] = "STARTED";
+
+    wrqu.data.length = sizeof(data);
+    wrqu.data.flags = 0;
+    wireless_send_event(priv->netdev[CSR_WIFI_INTERFACE_IN_USE], IWEVCUSTOM, &wrqu, data);
+#endif
+} /* wext_send_started_event() */
+
diff --git a/drivers/staging/echo/echo.c b/drivers/staging/echo/echo.c
index afbf5442b42..ca87ce9874b 100644
--- a/drivers/staging/echo/echo.c
+++ b/drivers/staging/echo/echo.c
@@ -118,7 +118,8 @@
 #ifdef __bfin__
 static inline void lms_adapt_bg(struct oslec_state *ec, int clean, int shift)
 {
-	int i, j;
+	int i;
+	int j;
 	int offset1;
 	int offset2;
 	int factor;
@@ -335,7 +336,8 @@ int16_t oslec_update(struct oslec_state *ec, int16_t tx, int16_t rx)
 {
 	int32_t echo_value;
 	int clean_bg;
-	int tmp, tmp1;
+	int tmp;
+	int tmp1;
 
 	/*
 	 * Input scaling was found be required to prevent problems when tx
@@ -624,7 +626,8 @@ EXPORT_SYMBOL_GPL(oslec_update);
 
 int16_t oslec_hpf_tx(struct oslec_state *ec, int16_t tx)
 {
-	int tmp, tmp1;
+	int tmp;
+	int tmp1;
 
 	if (ec->adaption_mode & ECHO_CAN_USE_TX_HPF) {
 		tmp = tx << 15;
diff --git a/drivers/staging/echo/echo.h b/drivers/staging/echo/echo.h
index 754e66d30c1..32ca9dedeca 100644
--- a/drivers/staging/echo/echo.h
+++ b/drivers/staging/echo/echo.h
@@ -36,7 +36,6 @@ What does it do?
 This module aims to provide G.168-2002 compliant echo cancellation, to remove
 electrical echoes (e.g. from 2-4 wire hybrids) from voice calls.
 
-
 How does it work?
 
 The heart of the echo cancellor is FIR filter. This is adapted to match the
@@ -128,7 +127,8 @@ a minor burden.
     echo canceller.
 */
 struct oslec_state {
-	int16_t tx, rx;
+	int16_t tx;
+	int16_t rx;
 	int16_t clean;
 	int16_t clean_nlp;
 
@@ -145,11 +145,18 @@ struct oslec_state {
 	int16_t shift;
 
 	/* Average levels and averaging filter states */
-	int Ltxacc, Lrxacc, Lcleanacc, Lclean_bgacc;
-	int Ltx, Lrx;
+	int Ltxacc;
+	int Lrxacc;
+	int Lcleanacc;
+	int Lclean_bgacc;
+	int Ltx;
+	int Lrx;
 	int Lclean;
 	int Lclean_bg;
-	int Lbgn, Lbgn_acc, Lbgn_upper, Lbgn_upper_acc;
+	int Lbgn;
+	int Lbgn_acc;
+	int Lbgn_upper;
+	int Lbgn_upper_acc;
 
 	/* foreground and background filter states */
 	struct fir16_state_t fir_state;
@@ -157,11 +164,16 @@ struct oslec_state {
 	int16_t *fir_taps16[2];
 
 	/* DC blocking filter states */
-	int tx_1, tx_2, rx_1, rx_2;
+	int tx_1;
+	int tx_2;
+	int rx_1;
+	int rx_2;
 
 	/* optional High Pass Filter states */
-	int32_t xvtx[5], yvtx[5];
-	int32_t xvrx[5], yvrx[5];
+	int32_t xvtx[5];
+	int32_t yvtx[5];
+	int32_t xvrx[5];
+	int32_t yvrx[5];
 
 	/* Parameters for the optional Hoth noise generator */
 	int cng_level;
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index 5b11c5e3622..029725c89e5 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -53,6 +53,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/pci.h>
 #include <linux/init.h>
 #include <linux/module.h>
@@ -85,8 +87,7 @@
 MODULE_AUTHOR("Victor Soriano <vjsoriano@agere.com>");
 MODULE_AUTHOR("Mark Einon <mark.einon@gmail.com>");
 MODULE_LICENSE("Dual BSD/GPL");
-MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver "
-		   "for the ET1310 by Agere Systems");
+MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver for the ET1310 by Agere Systems");
 
 /* EEPROM defines */
 #define MAX_NUM_REGISTER_POLLS          1000
@@ -1767,8 +1768,8 @@ static void et131x_xcvr_init(struct et131x_adapter *adapter)
 	/* Set the link status interrupt only.  Bad behavior when link status
 	 * and auto neg are set, we run into a nested interrupt problem
 	 */
-	imr |= (ET_PHY_INT_MASK_AUTONEGSTAT &
-		ET_PHY_INT_MASK_LINKSTAT &
+	imr |= (ET_PHY_INT_MASK_AUTONEGSTAT |
+		ET_PHY_INT_MASK_LINKSTAT |
 		ET_PHY_INT_MASK_ENABLE);
 
 	et131x_mii_write(adapter, PHY_INTERRUPT_MASK, imr);
@@ -1784,7 +1785,7 @@ static void et131x_xcvr_init(struct et131x_adapter *adapter)
 	if ((adapter->eeprom_data[1] & 0x4) == 0) {
 		et131x_mii_read(adapter, PHY_LED_2, &lcr2);
 
-		lcr2 &= (ET_LED2_LED_100TX & ET_LED2_LED_1000T);
+		lcr2 &= (ET_LED2_LED_100TX | ET_LED2_LED_1000T);
 		lcr2 |= (LED_VAL_LINKON_ACTIVE << LED_LINK_SHIFT);
 
 		if ((adapter->eeprom_data[1] & 0x8) == 0)
@@ -2554,8 +2555,8 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 			  "Cannot alloc memory for Packet Status Ring\n");
 		return -ENOMEM;
 	}
-	printk(KERN_INFO "Packet Status Ring %lx\n",
-	    (unsigned long) rx_ring->ps_ring_physaddr);
+	pr_info("Packet Status Ring %llx\n",
+		(unsigned long long) rx_ring->ps_ring_physaddr);
 
 	/*
 	 * NOTE : dma_alloc_coherent(), used above to alloc DMA regions,
@@ -2575,7 +2576,7 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 		return -ENOMEM;
 	}
 	rx_ring->num_rfd = NIC_DEFAULT_NUM_RFD;
-	printk(KERN_INFO "PRS %lx\n", (unsigned long)rx_ring->rx_status_bus);
+	pr_info("PRS %llx\n", (unsigned long long)rx_ring->rx_status_bus);
 
 	/* Recv
 	 * kmem_cache_create initializes a lookaside list. After successful
@@ -2967,11 +2968,10 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 		(ring_index == 0 &&
 		buff_index > rx_local->fbr[1]->num_entries - 1) ||
 		(ring_index == 1 &&
-		buff_index > rx_local->fbr[0]->num_entries - 1))
+		buff_index > rx_local->fbr[0]->num_entries - 1)) {
 #else
-	if (ring_index != 1 || buff_index > rx_local->fbr[0]->num_entries - 1)
+	if (ring_index != 1 || buff_index > rx_local->fbr[0]->num_entries - 1) {
 #endif
-	{
 		/* Illegal buffer or ring index cannot be used by S/W*/
 		dev_err(&adapter->pdev->dev,
 			  "NICRxPkts PSR Entry %d indicates "
@@ -3945,12 +3945,6 @@ static struct ethtool_ops et131x_ethtool_ops = {
 	.get_regs	= et131x_get_regs,
 	.get_link = ethtool_op_get_link,
 };
-
-static void et131x_set_ethtool_ops(struct net_device *netdev)
-{
-	SET_ETHTOOL_OPS(netdev, &et131x_ethtool_ops);
-}
-
 /**
  * et131x_hwaddr_init - set up the MAC Address on the ET1310
  * @adapter: pointer to our private adapter structure
@@ -4326,8 +4320,7 @@ static int et131x_mii_probe(struct net_device *netdev)
 	phydev->advertising = phydev->supported;
 	adapter->phydev = phydev;
 
-	dev_info(&adapter->pdev->dev, "attached PHY driver [%s] "
-		 "(mii_bus:phy_addr=%s)\n",
+	dev_info(&adapter->pdev->dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s)\n",
 		 phydev->drv->name, dev_name(&phydev->dev));
 
 	return 0;
@@ -5189,8 +5182,8 @@ static int et131x_set_mac_addr(struct net_device *netdev, void *new_mac)
 
 	memcpy(netdev->dev_addr, address->sa_data, netdev->addr_len);
 
-	printk(KERN_INFO "%s: Setting MAC address to %pM\n",
-			netdev->name, netdev->dev_addr);
+	netdev_info(netdev, "Setting MAC address to %pM\n",
+		    netdev->dev_addr);
 
 	/* Free Rx DMA memory */
 	et131x_adapter_memory_free(adapter);
@@ -5304,7 +5297,7 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 	netdev->netdev_ops     = &et131x_netdev_ops;
 
 	SET_NETDEV_DEV(netdev, &pdev->dev);
-	et131x_set_ethtool_ops(netdev);
+	SET_ETHTOOL_OPS(netdev, &et131x_ethtool_ops);
 
 	adapter = et131x_adapter_init(netdev, pdev);
 
@@ -5448,24 +5441,4 @@ static struct pci_driver et131x_driver = {
 	.driver.pm	= ET131X_PM_OPS,
 };
 
-/**
- * et131x_init_module - The "main" entry point called on driver initialization
- *
- * Returns 0 on success, errno on failure (as defined in errno.h)
- */
-static int __init et131x_init_module(void)
-{
-	return pci_register_driver(&et131x_driver);
-}
-
-/**
- * et131x_cleanup_module - The entry point called on driver cleanup
- */
-static void __exit et131x_cleanup_module(void)
-{
-	pci_unregister_driver(&et131x_driver);
-}
-
-module_init(et131x_init_module);
-module_exit(et131x_cleanup_module);
-
+module_pci_driver(et131x_driver);
diff --git a/drivers/staging/frontier/alphatrack.c b/drivers/staging/frontier/alphatrack.c
index acbb2cc510f..33085782689 100644
--- a/drivers/staging/frontier/alphatrack.c
+++ b/drivers/staging/frontier/alphatrack.c
@@ -333,7 +333,7 @@ static int usb_alphatrack_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&usb_alphatrack_driver, subminor);
 
 	if (!interface) {
-		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		pr_err("%s - error, can't find device for minor %d\n",
 		       __func__, subminor);
 		retval = -ENODEV;
 		goto unlock_disconnect_exit;
@@ -494,7 +494,7 @@ static ssize_t usb_alphatrack_read(struct file *file, char __user *buffer,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		printk(KERN_ERR "%s: No device or device unplugged %d\n",
+		pr_err("%s: No device or device unplugged %d\n",
 		       __func__, retval);
 		goto unlock_exit;
 	}
@@ -565,7 +565,7 @@ static ssize_t usb_alphatrack_write(struct file *file,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		printk(KERN_ERR "%s: No device or device unplugged %d\n",
+		pr_err("%s: No device or device unplugged %d\n",
 		       __func__, retval);
 		goto unlock_exit;
 	}
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
index 376706f1c71..5196a4e053e 100644
--- a/drivers/staging/frontier/tranzport.c
+++ b/drivers/staging/frontier/tranzport.c
@@ -353,8 +353,8 @@ static int usb_tranzport_open(struct inode *inode, struct file *file)
 	interface = usb_find_interface(&usb_tranzport_driver, subminor);
 
 	if (!interface) {
-		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
-			__func__, subminor);
+		pr_err("%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
 		retval = -ENODEV;
 		goto unlock_disconnect_exit;
 	}
@@ -520,8 +520,8 @@ static ssize_t usb_tranzport_read(struct file *file, char __user *buffer,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		printk(KERN_ERR "%s: No device or device unplugged %d\n",
-			__func__, retval);
+		pr_err("%s: No device or device unplugged %d\n",
+		       __func__, retval);
 		goto unlock_exit;
 	}
 
@@ -693,8 +693,8 @@ static ssize_t usb_tranzport_write(struct file *file,
 	/* verify that the device wasn't unplugged */
 	if (dev->intf == NULL) {
 		retval = -ENODEV;
-		printk(KERN_ERR "%s: No device or device unplugged %d\n",
-			__func__, retval);
+		pr_err("%s: No device or device unplugged %d\n",
+		       __func__, retval);
 		goto unlock_exit;
 	}
 
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
index c4a8a0a26eb..86a680c09ba 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
@@ -97,11 +97,10 @@ static inline u16 ft1000_read_fifo_len(struct net_device *dev)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 
-	if (info->AsicID == ELECTRABUZZ_ID) {
+	if (info->AsicID == ELECTRABUZZ_ID)
 		return (ft1000_read_reg(dev, FT1000_REG_UFIFO_STAT) - 16);
-	} else {
+	else
 		return (ft1000_read_reg(dev, FT1000_REG_MAG_UFSR) - 16);
-	}
 }
 
 //---------------------------------------------------------------------------
@@ -116,7 +115,7 @@ static inline u16 ft1000_read_fifo_len(struct net_device *dev)
 //     value  - value of dpram
 //
 //---------------------------------------------------------------------------
-u16 ft1000_read_dpram(struct net_device * dev, int offset)
+u16 ft1000_read_dpram(struct net_device *dev, int offset)
 {
 	struct ft1000_info *info = netdev_priv(dev);
 	unsigned long flags;
@@ -1997,42 +1996,43 @@ static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 	inttype = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
 
     // Make sure we process all interrupt before leaving the ISR due to the edge trigger interrupt type
-    while (inttype) {
-	if (inttype & ISR_DOORBELL_PEND) {
-		ft1000_parse_dpram_msg(dev);
-	}
+	while (inttype) {
+		if (inttype & ISR_DOORBELL_PEND)
+			ft1000_parse_dpram_msg(dev);
 
-	if (inttype & ISR_RCV) {
-		DEBUG(1, "Data in FIFO\n");
+		if (inttype & ISR_RCV) {
+			DEBUG(1, "Data in FIFO\n");
 
-		cnt = 0;
-		do {
-			// Check if we have packets in the Downlink FIFO
-			if (info->AsicID == ELECTRABUZZ_ID) {
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_DFIFO_STAT);
-			} else {
-				tempword =
-					ft1000_read_reg(dev, FT1000_REG_MAG_DFSR);
-			}
-			if (tempword & 0x1f) {
-				ft1000_copy_up_pkt(dev);
-			} else {
-				break;
-			}
-			cnt++;
-		} while (cnt < MAX_RCV_LOOP);
+			cnt = 0;
+			do {
+				// Check if we have packets in the Downlink FIFO
+				if (info->AsicID == ELECTRABUZZ_ID) {
+					tempword =
+					ft1000_read_reg(dev,
+							FT1000_REG_DFIFO_STAT);
+				} else {
+					tempword =
+					ft1000_read_reg(dev,
+							FT1000_REG_MAG_DFSR);
+				}
+				if (tempword & 0x1f) {
+					ft1000_copy_up_pkt(dev);
+				} else {
+					break;
+				}
+				cnt++;
+			} while (cnt < MAX_RCV_LOOP);
 
+		}
+		// clear interrupts
+		tempword = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
+		DEBUG(1, "ft1000_hw: interrupt status register = 0x%x\n", tempword);
+		ft1000_write_reg(dev, FT1000_REG_SUP_ISR, tempword);
+
+		// Read interrupt type
+		inttype = ft1000_read_reg (dev, FT1000_REG_SUP_ISR);
+		DEBUG(1,"ft1000_hw: interrupt status register after clear = 0x%x\n",inttype);
 	}
-	// clear interrupts
-	tempword = ft1000_read_reg(dev, FT1000_REG_SUP_ISR);
-	DEBUG(1, "ft1000_hw: interrupt status register = 0x%x\n", tempword);
-	ft1000_write_reg(dev, FT1000_REG_SUP_ISR, tempword);
-
-        // Read interrupt type
-        inttype = ft1000_read_reg (dev, FT1000_REG_SUP_ISR);
-        DEBUG(1,"ft1000_hw: interrupt status register after clear = 0x%x\n",inttype);
-    }
 	ft1000_enable_interrupts(dev);
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c
index 71aaad31270..72727c6b9e2 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_proc.c
@@ -34,16 +34,14 @@
 
 #define PUTX_TO_PAGE(len, page, message, size, var) \
 	len += snprintf(page+len, PAGE_SIZE - len, message); \
-	for(i = 0; i < (size - 1); i++) \
-	{ \
+	for(i = 0; i < (size - 1); i++) { \
 		len += snprintf(page+len, PAGE_SIZE - len, "%02x:", var[i]); \
 	} \
 	len += snprintf(page+len, PAGE_SIZE - len, "%02x\n", var[i])
 
 #define PUTD_TO_PAGE(len, page, message, size, var) \
 	len += snprintf(page+len, PAGE_SIZE - len, message); \
-	for(i = 0; i < (size - 1); i++) \
-	{ \
+	for(i = 0; i < (size - 1); i++) { \
 		len += snprintf(page+len, PAGE_SIZE - len, "%d.", var[i]); \
 	} \
 	len += snprintf(page+len, PAGE_SIZE - len, "%d\n", var[i])
@@ -55,8 +53,8 @@ static int ft1000ReadProc(char *page, char **start, off_t off,
 	int len;
 	int i;
 	struct ft1000_info *info;
-	char *status[] =
-		{ "Idle (Disconnect)", "Searching", "Active (Connected)",
+	char *status[] = {
+		"Idle (Disconnect)", "Searching", "Active (Connected)",
 		"Waiting for L2", "Sleep", "No Coverage", "", ""
 	};
 	char *signal[] = { "", "*", "**", "***", "****" };
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index 19db23fe73c..6d911fda47f 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -167,7 +167,7 @@ int ft1000_create_dev(struct ft1000_device *dev)
 		goto fail;
 	}
 
-	dir = debugfs_create_dir(info->DeviceName, 0);
+	dir = debugfs_create_dir(info->DeviceName, NULL);
 	if (IS_ERR(dir)) {
 		result = PTR_ERR(dir);
 		goto debug_dir_fail;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
index 43b1d363107..31929ef5332 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
@@ -1178,7 +1178,6 @@ static int ft1000_open(struct net_device *dev)
 {
 	struct ft1000_info *pInfo = netdev_priv(dev);
 	struct timeval tv;
-	int ret;
 
 	DEBUG("ft1000_open is called for card %d\n", pInfo->CardNumber);
 
@@ -1194,9 +1193,7 @@ static int ft1000_open(struct net_device *dev)
 
 	netif_carrier_on(dev);
 
-	ret = ft1000_submit_rx_urb(pInfo);
-
-	return ret;
+	return ft1000_submit_rx_urb(pInfo);
 }
 
 //---------------------------------------------------------------------------
@@ -1754,8 +1751,8 @@ out:
 	return status;
 }
 
-int ft1000_poll(void* dev_id) {
-
+int ft1000_poll(void* dev_id)
+{
     struct ft1000_device *dev = (struct ft1000_device *)dev_id;
 	struct ft1000_info *info = netdev_priv(dev->net);
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index 84c38d5c939..b2ecd0e6780 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -37,9 +37,9 @@ static struct usb_device_id id_table[] = {
 MODULE_DEVICE_TABLE(usb, id_table);
 
 static bool gPollingfailed = FALSE;
-int ft1000_poll_thread(void *arg)
+static int ft1000_poll_thread(void *arg)
 {
-	int ret = STATUS_SUCCESS;
+	int ret;
 
 	while (!kthread_should_stop()) {
 		msleep(10);
@@ -67,15 +67,13 @@ static int ft1000_probe(struct usb_interface *interface,
 	struct ft1000_info *pft1000info = NULL;
 	const struct firmware *dsp_fw;
 
-	ft1000dev = kmalloc(sizeof(struct ft1000_device), GFP_KERNEL);
+	ft1000dev = kzalloc(sizeof(struct ft1000_device), GFP_KERNEL);
 
 	if (!ft1000dev) {
-		printk(KERN_ERR "out of memory allocating device structure\n");
-		return 0;
+		pr_err("out of memory allocating device structure\n");
+		return -ENOMEM;
 	}
 
-	memset(ft1000dev, 0, sizeof(*ft1000dev));
-
 	dev = interface_to_usbdev(interface);
 	DEBUG("ft1000_probe: usb device descriptor info:\n");
 	DEBUG("ft1000_probe: number of configuration is %d\n",
@@ -140,7 +138,7 @@ static int ft1000_probe(struct usb_interface *interface,
 
 	ret = request_firmware(&dsp_fw, "ft3000.img", &dev->dev);
 	if (ret < 0) {
-		printk(KERN_ERR "Error request_firmware().\n");
+		pr_err("Error request_firmware().\n");
 		goto err_fw;
 	}
 
@@ -168,7 +166,7 @@ static int ft1000_probe(struct usb_interface *interface,
 	DEBUG("In probe: pft1000info=%p\n", pft1000info);
 	ret = dsp_reload(ft1000dev);
 	if (ret) {
-		printk(KERN_ERR "Problem with DSP image loading\n");
+		pr_err("Problem with DSP image loading\n");
 		goto err_load;
 	}
 
@@ -239,7 +237,7 @@ static void ft1000_disconnect(struct usb_interface *interface)
 			ft1000_destroy_dev(pft1000info->pFt1000Dev->net);
 			unregister_netdev(pft1000info->pFt1000Dev->net);
 			DEBUG
-			    ("ft1000_disconnect: network device unregisterd\n");
+			    ("ft1000_disconnect: network device unregistered\n");
 			free_netdev(pft1000info->pFt1000Dev->net);
 
 		}
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
index 51c084756b4..642bb89942f 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
@@ -1,7 +1,6 @@
 #ifndef _FT1000_USB_H_
 #define _FT1000_USB_H_
 
-/*Jim*/
 #include "../ft1000.h"
 #include "ft1000_ioctl.h"
 #define FT1000_DRV_VER      0x01010403
@@ -15,19 +14,18 @@
 #define  SUCCESS             0x00
 
 struct app_info_block {
-	u32 nTxMsg;                    // DPRAM msg sent to DSP with app_id
-	u32 nRxMsg;                    // DPRAM msg rcv from dsp with app_id
-	u32 nTxMsgReject;              // DPRAM msg rejected due to DSP doorbell set
-	u32 nRxMsgMiss;                // DPRAM msg dropped due to overflow
-	struct fown_struct *fileobject;// Application's file object
-	u16 app_id;                    // Application id
+	u32 nTxMsg;                    /* DPRAM msg sent to DSP with app_id */
+	u32 nRxMsg;                    /* DPRAM msg rcv from dsp with app_id */
+	u32 nTxMsgReject;              /* DPRAM msg rejected due to DSP doorbell set */
+	u32 nRxMsgMiss;                /* DPRAM msg dropped due to overflow */
+	struct fown_struct *fileobject;/* Application's file object */
+	u16 app_id;                    /* Application id */
 	int DspBCMsgFlag;
-	int NumOfMsg;                   // number of messages queued up
+	int NumOfMsg;                   /* number of messages queued up */
 	wait_queue_head_t wait_dpram_msg;
-	struct list_head app_sqlist;   // link list of msgs for applicaton on slow queue
-} __attribute__((packed));
+	struct list_head app_sqlist;   /* link list of msgs for applicaton on slow queue */
+} __packed;
 
-/*end of Jim*/
 #define DEBUG(args...) printk(KERN_INFO args)
 
 #define FALSE           0
@@ -47,20 +45,19 @@ struct app_info_block {
 #undef FT1000_DPRAM_RX_BASE
 #define FT1000_DPRAM_RX_BASE	0x1800	/* RX AREA (SlowQ) */
 
-// MEMORY MAP FOR MAGNEMITE
+/* MEMORY MAP FOR MAGNEMITE */
 /* the indexes are swapped comparing to PCMCIA - is it OK or a bug? */
 #undef FT1000_MAG_DSP_LED_INDX
 #define FT1000_MAG_DSP_LED_INDX		0x1	/* dsp led status for PAD device */
 #undef FT1000_MAG_DSP_CON_STATE_INDX
 #define FT1000_MAG_DSP_CON_STATE_INDX	0x0	/* DSP Connection Status Info */
 
-// Maximum times trying to get ASIC out of reset
+/* Maximum times trying to get ASIC out of reset */
 #define MAX_ASIC_RESET_CNT      20
 
 #define MAX_BUF_SIZE            4096
 
-struct ft1000_device
-{
+struct ft1000_device {
 	struct usb_device *dev;
 	struct net_device *net;
 
@@ -74,11 +71,7 @@ struct ft1000_device
 
 	u8 bulk_in_endpointAddr;
 	u8 bulk_out_endpointAddr;
-
-	//struct ft1000_ethernet_configuration configuration;
-
-//	struct net_device_stats stats; //mbelian
-} __attribute__ ((packed));
+} __packed;
 
 struct ft1000_debug_dirs {
 	struct list_head list;
@@ -88,90 +81,101 @@ struct ft1000_debug_dirs {
 };
 
 struct ft1000_info {
-    struct ft1000_device *pFt1000Dev;
-    struct net_device_stats stats;
+	struct ft1000_device *pFt1000Dev;
+	struct net_device_stats stats;
 
-    struct task_struct *pPollThread;
+	struct task_struct *pPollThread;
 
-    unsigned char fcodeldr;
-    unsigned char bootmode;
+	unsigned char fcodeldr;
+	unsigned char bootmode;
 	unsigned char usbboot;
-    unsigned short dspalive;
-    u16 ASIC_ID;
-    bool fProvComplete;
-    bool fCondResetPend;
-    bool fAppMsgPend;
-    u16 DrvErrNum;
-    u16 AsicID;
-    int DspAsicReset;
-    int DeviceCreated;
-    int CardReady;
-    int NetDevRegDone;
-    u8 CardNumber;
-    u8 DeviceName[15];
-    struct ft1000_debug_dirs nodes;
-    int registered;
-    int mediastate;
-    u8 squeseqnum;                 // sequence number on slow queue
-    spinlock_t dpram_lock;
-    spinlock_t fifo_lock;
-    u16 fifo_cnt;
-    u8 DspVer[DSPVERSZ];        // DSP version number
-    u8 HwSerNum[HWSERNUMSZ];    // Hardware Serial Number
-    u8 Sku[SKUSZ];              // SKU
-    u8 eui64[EUISZ];            // EUI64
-    time_t ConTm;               // Connection Time
-    u8 ProductMode[MODESZ];
-    u8 RfCalVer[CALVERSZ];
-    u8 RfCalDate[CALDATESZ];
-    u16 DSP_TIME[4];
-    u16 LedStat;	//mbelian
-    u16 ConStat;	//mbelian
-    u16 ProgConStat;
-    struct list_head prov_list;
-    int appcnt;
+	unsigned short dspalive;
+	u16 ASIC_ID;
+	bool fProvComplete;
+	bool fCondResetPend;
+	bool fAppMsgPend;
+	u16 DrvErrNum;
+	u16 AsicID;
+	int DspAsicReset;
+	int DeviceCreated;
+	int CardReady;
+	int NetDevRegDone;
+	u8 CardNumber;
+	u8 DeviceName[15];
+	struct ft1000_debug_dirs nodes;
+	int registered;
+	int mediastate;
+	u8 squeseqnum;                 /* sequence number on slow queue */
+	spinlock_t dpram_lock;
+	spinlock_t fifo_lock;
+	u16 fifo_cnt;
+	u8 DspVer[DSPVERSZ];        /* DSP version number */
+	u8 HwSerNum[HWSERNUMSZ];    /* Hardware Serial Number */
+	u8 Sku[SKUSZ];              /* SKU */
+	u8 eui64[EUISZ];            /* EUI64 */
+	time_t ConTm;               /* Connection Time */
+	u8 ProductMode[MODESZ];
+	u8 RfCalVer[CALVERSZ];
+	u8 RfCalDate[CALDATESZ];
+	u16 DSP_TIME[4];
+	u16 LedStat;
+	u16 ConStat;
+	u16 ProgConStat;
+	struct list_head prov_list;
+	int appcnt;
 	struct app_info_block app_info[MAX_NUM_APP];
-    u16 DSPInfoBlklen;
-    u16 DrvMsgPend;
-    int (*ft1000_reset)(struct net_device *dev);
-    u16 DSPInfoBlk[MAX_DSP_SESS_REC];
-    union {
-        u16 Rec[MAX_DSP_SESS_REC];
-        u32 MagRec[MAX_DSP_SESS_REC/2];
-    } DSPSess;
+	u16 DSPInfoBlklen;
+	u16 DrvMsgPend;
+	int (*ft1000_reset)(struct net_device *dev);
+	u16 DSPInfoBlk[MAX_DSP_SESS_REC];
+	union {
+		u16 Rec[MAX_DSP_SESS_REC];
+		u32 MagRec[MAX_DSP_SESS_REC/2];
+	} DSPSess;
 	unsigned short tempbuf[32];
 	char netdevname[IFNAMSIZ];
-	struct proc_dir_entry *ft1000_proc_dir; //mbelian
+	struct proc_dir_entry *ft1000_proc_dir;
 };
 
 
 struct dpram_blk {
-    struct list_head list;
-    u16 *pbuffer;
-} __attribute__ ((packed));
-
-int ft1000_read_register(struct ft1000_device *ft1000dev, u16* Data, u16 nRegIndx);
-int ft1000_write_register(struct ft1000_device *ft1000dev, u16 value, u16 nRegIndx);
-int ft1000_read_dpram32(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer, u16 cnt);
-int ft1000_write_dpram32(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer, u16 cnt);
-int ft1000_read_dpram16(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer, u8 highlow);
-int ft1000_write_dpram16(struct ft1000_device *ft1000dev, u16 indx, u16 value, u8 highlow);
-int fix_ft1000_read_dpram32(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer);
-int fix_ft1000_write_dpram32(struct ft1000_device *ft1000dev, u16 indx, u8 *buffer);
+	struct list_head list;
+	u16 *pbuffer;
+} __packed;
+
+int ft1000_read_register(struct ft1000_device *ft1000dev,
+			u16 *Data, u16 nRegIndx);
+int ft1000_write_register(struct ft1000_device *ft1000dev,
+			u16 value, u16 nRegIndx);
+int ft1000_read_dpram32(struct ft1000_device *ft1000dev,
+			u16 indx, u8 *buffer, u16 cnt);
+int ft1000_write_dpram32(struct ft1000_device *ft1000dev,
+			u16 indx, u8 *buffer, u16 cnt);
+int ft1000_read_dpram16(struct ft1000_device *ft1000dev,
+			u16 indx, u8 *buffer, u8 highlow);
+int ft1000_write_dpram16(struct ft1000_device *ft1000dev,
+			u16 indx, u16 value, u8 highlow);
+int fix_ft1000_read_dpram32(struct ft1000_device *ft1000dev,
+			u16 indx, u8 *buffer);
+int fix_ft1000_write_dpram32(struct ft1000_device *ft1000dev,
+			u16 indx, u8 *buffer);
 
 extern void *pFileStart;
 extern size_t FileLength;
 extern int numofmsgbuf;
 
-int ft1000_close (struct net_device *dev);
-u16 scram_dnldr(struct ft1000_device *ft1000dev, void *pFileStart, u32  FileLength);
+int ft1000_close(struct net_device *dev);
+u16 scram_dnldr(struct ft1000_device *ft1000dev, void *pFileStart,
+		u32  FileLength);
 
 extern struct list_head freercvpool;
-extern spinlock_t free_buff_lock;   // lock to arbitrate free buffer list for receive command data
+
+extern spinlock_t free_buff_lock;   /* lock to arbitrate free buffer list for receive command data */
 
 int ft1000_create_dev(struct ft1000_device *dev);
 void ft1000_destroy_dev(struct net_device *dev);
-extern void card_send_command(struct ft1000_device *ft1000dev, void *ptempbuffer, int size);
+extern void card_send_command(struct ft1000_device *ft1000dev,
+				void *ptempbuffer, int size);
 
 struct dpram_blk *ft1000_get_buffer(struct list_head *bufflist);
 void ft1000_free_buffer(struct dpram_blk *pdpram_blk, struct list_head *plist);
@@ -179,8 +183,9 @@ void ft1000_free_buffer(struct dpram_blk *pdpram_blk, struct list_head *plist);
 int dsp_reload(struct ft1000_device *ft1000dev);
 int init_ft1000_netdev(struct ft1000_device *ft1000dev);
 struct usb_interface;
-int reg_ft1000_netdev(struct ft1000_device *ft1000dev, struct usb_interface *intf);
-int ft1000_poll(void* dev_id);
+int reg_ft1000_netdev(struct ft1000_device *ft1000dev,
+			struct usb_interface *intf);
+int ft1000_poll(void *dev_id);
 
 int ft1000_init_proc(struct net_device *dev);
 void ft1000_cleanup_proc(struct ft1000_info *info);
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index 0217680ec54..80bde053fbc 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -11,7 +11,6 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/version.h>
 #include <linux/etherdevice.h>
 #include <asm/byteorder.h>
 
@@ -196,7 +195,7 @@ static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
 	return 0;
 }
 
-static u32 get_qos_index(struct nic *nic, u8* iph, u8* tcpudph)
+static u32 get_qos_index(struct nic *nic, u8 *iph, u8 *tcpudph)
 {
 	u32	IP_Ver, Header_Len, i;
 	struct qos_cb_s *qcb = &nic->qos;
@@ -241,8 +240,7 @@ static u32 extract_qos_list(struct nic *nic, struct list_head *head)
 					qcb->csr[i].QoSBufCount++;
 
 					if (!list_empty(&qcb->qos_list[i]))
-						wprintk("QoS Index(%d) "
-							"is piled!!\n", i);
+						wprintk("QoS Index(%d) is piled!!\n", i);
 				}
 			}
 		}
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index e1a3dd2fc4a..3e43c012ef2 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -14,7 +14,6 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
-#include <linux/version.h>
 
 #include <linux/mmc/core.h>
 #include <linux/mmc/card.h>
@@ -665,20 +664,17 @@ static int sdio_wimax_probe(struct sdio_func *func,
 	if (ret)
 		return ret;
 
-	phy_dev = kmalloc(sizeof(*phy_dev), GFP_KERNEL);
+	phy_dev = kzalloc(sizeof(*phy_dev), GFP_KERNEL);
 	if (phy_dev == NULL) {
 		ret = -ENOMEM;
 		goto out;
 	}
-	sdev = kmalloc(sizeof(*sdev), GFP_KERNEL);
+	sdev = kzalloc(sizeof(*sdev), GFP_KERNEL);
 	if (sdev == NULL) {
 		ret = -ENOMEM;
 		goto out;
 	}
 
-	memset(phy_dev, 0, sizeof(*phy_dev));
-	memset(sdev, 0, sizeof(*sdev));
-
 	phy_dev->priv_dev = (void *)sdev;
 	phy_dev->send_func = gdm_sdio_send;
 	phy_dev->rcv_func = gdm_sdio_receive;
diff --git a/drivers/staging/gdm72xx/gdm_usb.c b/drivers/staging/gdm72xx/gdm_usb.c
index 1e9dc0d9036..d48d49c145e 100644
--- a/drivers/staging/gdm72xx/gdm_usb.c
+++ b/drivers/staging/gdm72xx/gdm_usb.c
@@ -12,10 +12,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/kernel.h>
 #include <linux/usb.h>
 #include <asm/byteorder.h>
+#include <linux/kthread.h>
 
 #include "gdm_usb.h"
 #include "gdm_wimax.h"
@@ -270,20 +270,17 @@ static void release_usb(struct usbwm_dev *udev)
 	}
 }
 
-static void gdm_usb_send_complete(struct urb *urb)
+static void __gdm_usb_send_complete(struct urb *urb)
 {
 	struct usb_tx *t = urb->context;
 	struct tx_cxt *tx = t->tx_cxt;
 	u8 *pkt = t->buf;
 	u16 cmd_evt;
-	unsigned long flags;
 
 	/* Completion by usb_unlink_urb */
 	if (urb->status == -ECONNRESET)
 		return;
 
-	spin_lock_irqsave(&tx->lock, flags);
-
 	if (t->callback)
 		t->callback(t->cb_data);
 
@@ -295,7 +292,16 @@ static void gdm_usb_send_complete(struct urb *urb)
 		put_tx_struct(tx, t);
 	else
 		free_tx_struct(t);
+}
 
+static void gdm_usb_send_complete(struct urb *urb)
+{
+	struct usb_tx *t = urb->context;
+	struct tx_cxt *tx = t->tx_cxt;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tx->lock, flags);
+	__gdm_usb_send_complete(urb);
 	spin_unlock_irqrestore(&tx->lock, flags);
 }
 
@@ -411,7 +417,7 @@ out:
 
 send_fail:
 	t->callback = NULL;
-	gdm_usb_send_complete(t->urb);
+	__gdm_usb_send_complete(t->urb);
 	spin_unlock_irqrestore(&tx->lock, flags);
 	return ret;
 }
@@ -540,7 +546,7 @@ static void do_pm_control(struct work_struct *work)
 
 			if (ret) {
 				t->callback = NULL;
-				gdm_usb_send_complete(t->urb);
+				__gdm_usb_send_complete(t->urb);
 			}
 		}
 	}
@@ -584,20 +590,17 @@ static int gdm_usb_probe(struct usb_interface *intf,
 		goto out;
 	}
 
-	phy_dev = kmalloc(sizeof(*phy_dev), GFP_KERNEL);
+	phy_dev = kzalloc(sizeof(*phy_dev), GFP_KERNEL);
 	if (phy_dev == NULL) {
 		ret = -ENOMEM;
 		goto out;
 	}
-	udev = kmalloc(sizeof(*udev), GFP_KERNEL);
+	udev = kzalloc(sizeof(*udev), GFP_KERNEL);
 	if (udev == NULL) {
 		ret = -ENOMEM;
 		goto out;
 	}
 
-	memset(phy_dev, 0, sizeof(*phy_dev));
-	memset(udev, 0, sizeof(*udev));
-
 	if (idProduct == 0x7205 || idProduct == 0x7206)
 		udev->padding = GDM7205_PADDING;
 	else
@@ -742,7 +745,7 @@ static int k_mode_thread(void *arg)
 
 				if (ret) {
 					t->callback = NULL;
-					gdm_usb_send_complete(t->urb);
+					__gdm_usb_send_complete(t->urb);
 				}
 			}
 
@@ -775,7 +778,7 @@ static struct usb_driver gdm_usb_driver = {
 static int __init usb_gdm_wimax_init(void)
 {
 #ifdef CONFIG_WIMAX_GDM72XX_K_MODE
-	kernel_thread(k_mode_thread, NULL, CLONE_KERNEL);
+	kthread_run(k_mode_thread, NULL, "WiMax_thread");
 #endif /* CONFIG_WIMAX_GDM72XX_K_MODE */
 	return usb_register(&gdm_usb_driver);
 }
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index f1936b92533..0716efc1817 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -11,7 +11,6 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/version.h>
 #include <linux/etherdevice.h>
 #include <asm/byteorder.h>
 #include <linux/ip.h>
@@ -256,17 +255,15 @@ static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
 
 static int gdm_wimax_event_init(void)
 {
-	if (wm_event.ref_cnt == 0) {
+	if (!wm_event.ref_cnt) {
 		wm_event.sock = netlink_init(NETLINK_WIMAX,
 						gdm_wimax_event_rcv);
+		if (wm_event.sock)
+			wm_event.ref_cnt++;
 		INIT_LIST_HEAD(&wm_event.evtq);
 		INIT_LIST_HEAD(&wm_event.freeq);
 		INIT_WORK(&wm_event.ws, __gdm_wimax_event_send);
 		spin_lock_init(&wm_event.evt_lock);
-	}
-
-	if (wm_event.sock) {
-		wm_event.ref_cnt++;
 		return 0;
 	}
 
@@ -745,13 +742,8 @@ static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
 					"[%x/%d]\n", __func__, T, L);
 				return -1;
 			}
-			printk(KERN_INFO
-				"MAC change [%02x:%02x:%02x:%02x:%02x:%02x]"
-				"->[%02x:%02x:%02x:%02x:%02x:%02x]\n",
-				dev->dev_addr[0], dev->dev_addr[1],
-				dev->dev_addr[2], dev->dev_addr[3],
-				dev->dev_addr[4], dev->dev_addr[5],
-				V[0], V[1], V[2], V[3], V[4], V[5]);
+			printk(KERN_INFO "MAC change [%pM]->[%pM]\n",
+				dev->dev_addr, V);
 			memcpy(dev->dev_addr, V, dev->addr_len);
 			return 1;
 		}
diff --git a/drivers/staging/gdm72xx/gdm_wimax.h b/drivers/staging/gdm72xx/gdm_wimax.h
index 023e6492e33..6ec0ab43e9c 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.h
+++ b/drivers/staging/gdm72xx/gdm_wimax.h
@@ -15,7 +15,6 @@
 #define __GDM_WIMAX_H__
 
 #include <linux/netdevice.h>
-#include <linux/version.h>
 #include <linux/types.h>
 #include "wm_ioctl.h"
 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index 51665132c61..3abb31df8f2 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -11,7 +11,6 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/etherdevice.h>
 #include <linux/netlink.h>
@@ -88,13 +87,15 @@ struct sock *netlink_init(int unit, void (*cb)(struct net_device *dev, u16 type,
 						void *msg, int len))
 {
 	struct sock *sock;
+	struct netlink_kernel_cfg cfg = {
+		.input  = netlink_rcv,
+	};
 
 #if !defined(DEFINE_MUTEX)
 	init_MUTEX(&netlink_mutex);
 #endif
 
-	sock = netlink_kernel_create(&init_net, unit, 0, netlink_rcv, NULL,
-					THIS_MODULE);
+	sock = netlink_kernel_create(&init_net, unit, THIS_MODULE, &cfg);
 
 	if (sock)
 		rcv_cb = cb;
@@ -127,8 +128,12 @@ int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 	}
 
 	seq++;
-	nlh = NLMSG_PUT(skb, 0, seq, type, len);
-	memcpy(NLMSG_DATA(nlh), msg, len);
+	nlh = nlmsg_put(skb, 0, seq, type, len, 0);
+	if (!nlh) {
+		kfree_skb(skb);
+		return -EMSGSIZE;
+	}
+	memcpy(nlmsg_data(nlh), msg, len);
 
 	NETLINK_CB(skb).pid = 0;
 	NETLINK_CB(skb).dst_group = 0;
@@ -144,7 +149,5 @@ int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 		}
 		ret = 0;
 	}
-
-nlmsg_failure:
 	return ret;
 }
diff --git a/drivers/staging/gdm72xx/sdio_boot.c b/drivers/staging/gdm72xx/sdio_boot.c
index 6ff4dc37252..65624bca8b3 100644
--- a/drivers/staging/gdm72xx/sdio_boot.c
+++ b/drivers/staging/gdm72xx/sdio_boot.c
@@ -12,7 +12,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/mm.h>
@@ -67,9 +66,8 @@ static int download_image(struct sdio_func *func, char *img_name)
 		return -ENOENT;
 	}
 
-	if (filp->f_dentry)
-		inode = filp->f_dentry->d_inode;
-	if (!inode || !S_ISREG(inode->i_mode)) {
+	inode = filp->f_dentry->d_inode;
+	if (!S_ISREG(inode->i_mode)) {
 		printk(KERN_ERR "Invalid file type: %s\n", img_name);
 		ret = -EINVAL;
 		goto out;
@@ -124,7 +122,7 @@ static int download_image(struct sdio_func *func, char *img_name)
 		pno++;
 	}
 out:
-	filp_close(filp, current->files);
+	filp_close(filp, NULL);
 	return ret;
 }
 
diff --git a/drivers/staging/gdm72xx/usb_boot.c b/drivers/staging/gdm72xx/usb_boot.c
index 5a0e030220d..e3dbd5a552c 100644
--- a/drivers/staging/gdm72xx/usb_boot.c
+++ b/drivers/staging/gdm72xx/usb_boot.c
@@ -13,7 +13,6 @@
 
 #include <linux/uaccess.h>
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/usb.h>
@@ -174,14 +173,12 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 	filp = filp_open(img_name, O_RDONLY | O_LARGEFILE, 0);
 	if (IS_ERR(filp)) {
 		printk(KERN_ERR "Can't find %s.\n", img_name);
-		set_fs(fs);
-		ret = -ENOENT;
+		ret = PTR_ERR(filp);
 		goto restore_fs;
 	}
 
-	if (filp->f_dentry)
-		inode = filp->f_dentry->d_inode;
-	if (!inode || !S_ISREG(inode->i_mode)) {
+	inode = filp->f_dentry->d_inode;
+	if (!S_ISREG(inode->i_mode)) {
 		printk(KERN_ERR "Invalid file type: %s\n", img_name);
 		ret = -EINVAL;
 		goto out;
@@ -263,7 +260,7 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 		ret = -EINVAL;
 	}
 out:
-	filp_close(filp, current->files);
+	filp_close(filp, NULL);
 
 restore_fs:
 	set_fs(fs);
@@ -323,13 +320,11 @@ static int em_download_image(struct usb_device *usbdev, char *path,
 		goto restore_fs;
 	}
 
-	if (filp->f_dentry) {
-		inode = filp->f_dentry->d_inode;
-		if (!inode || !S_ISREG(inode->i_mode)) {
-			printk(KERN_ERR "Invalid file type: %s\n", path);
-			ret = -EINVAL;
-			goto out;
-		}
+	inode = filp->f_dentry->d_inode;
+	if (!S_ISREG(inode->i_mode)) {
+		printk(KERN_ERR "Invalid file type: %s\n", path);
+		ret = -EINVAL;
+		goto out;
 	}
 
 	buf = kmalloc(DOWNLOAD_CHUCK + pad_size, GFP_KERNEL);
@@ -365,7 +360,7 @@ static int em_download_image(struct usb_device *usbdev, char *path,
 		goto out;
 
 out:
-	filp_close(filp, current->files);
+	filp_close(filp, NULL);
 
 restore_fs:
 	set_fs(fs);
diff --git a/drivers/staging/iio/Documentation/device.txt b/drivers/staging/iio/Documentation/device.txt
index f03fbd3bb45..ea08d621337 100644
--- a/drivers/staging/iio/Documentation/device.txt
+++ b/drivers/staging/iio/Documentation/device.txt
@@ -12,7 +12,7 @@ struct iio_dev *indio_dev = iio_device_alloc(sizeof(struct chip_state));
 where chip_state is a structure of local state data for this instance of
 the chip.
 
-That data can be accessed using iio_priv(struct iio_dev *)
+That data can be accessed using iio_priv(struct iio_dev *).
 
 Then fill in the following:
 
@@ -39,7 +39,7 @@ Then fill in the following:
 		and for associate parameters such as offsets and scales.
 	* info->write_raw:
 		Raw value writing function. Used for writable device values such
-		as DAC values and caliboffset.
+		as DAC values and calibbias.
 	* info->read_event_config:
 		Typically only set if there are some interrupt lines.  This
 		is used to read if an on sensor event detector is enabled.
@@ -62,8 +62,8 @@ Then fill in the following:
 	Poll function related elements. This controls what occurs when a trigger
 	to which this device is attached sends an event.
 - indio_dev->channels:
-	Specification of device channels. Most attributes etc are built
-	form this spec.
+	Specification of device channels. Most attributes etc. are built
+	from this spec.
 - indio_dev->num_channels:
 	How many channels are there?
 
@@ -76,4 +76,4 @@ be registered afterwards (otherwise the whole parentage of devices
 gets confused)
 
 On remove, iio_device_unregister(indio_dev) will remove the device from
-the core, and iio_device_free will clean up.
+the core, and iio_device_free(indio_dev) will clean up.
diff --git a/drivers/staging/iio/Documentation/generic_buffer.c b/drivers/staging/iio/Documentation/generic_buffer.c
index bf553356fda..827e92de8e3 100644
--- a/drivers/staging/iio/Documentation/generic_buffer.c
+++ b/drivers/staging/iio/Documentation/generic_buffer.c
@@ -18,6 +18,8 @@
  *
  */
 
+#define _GNU_SOURCE
+
 #include <unistd.h>
 #include <dirent.h>
 #include <fcntl.h>
@@ -29,12 +31,14 @@
 #include <string.h>
 #include <poll.h>
 #include <endian.h>
+#include <getopt.h>
+#include <inttypes.h>
 #include "iio_utils.h"
 
 /**
  * size_from_channelarray() - calculate the storage size of a scan
- * @channels: the channel info array
- * @num_channels: size of the channel info array
+ * @channels:		the channel info array
+ * @num_channels:	number of channels
  *
  * Has the side effect of filling the channels[i].location values used
  * in processing the buffer output.
@@ -58,22 +62,22 @@ int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
 void print2byte(int input, struct iio_channel_info *info)
 {
 	/* First swap if incorrect endian */
-
 	if (info->be)
 		input = be16toh((uint16_t)input);
 	else
 		input = le16toh((uint16_t)input);
 
-	/* shift before conversion to avoid sign extension
-	   of left aligned data */
+	/*
+	 * Shift before conversion to avoid sign extension
+	 * of left aligned data
+	 */
 	input = input >> info->shift;
 	if (info->is_signed) {
 		int16_t val = input;
 		val &= (1 << info->bits_used) - 1;
 		val = (int16_t)(val << (16 - info->bits_used)) >>
 			(16 - info->bits_used);
-		printf("%05f  ", val,
-		       (float)(val + info->offset)*info->scale);
+		printf("%05f ", ((float)val + info->offset)*info->scale);
 	} else {
 		uint16_t val = input;
 		val &= (1 << info->bits_used) - 1;
@@ -83,39 +87,39 @@ void print2byte(int input, struct iio_channel_info *info)
 /**
  * process_scan() - print out the values in SI units
  * @data:		pointer to the start of the scan
- * @infoarray:		information about the channels. Note
+ * @channels:		information about the channels. Note
  *  size_from_channelarray must have been called first to fill the
  *  location offsets.
- * @num_channels:	the number of active channels
+ * @num_channels:	number of channels
  **/
 void process_scan(char *data,
-		  struct iio_channel_info *infoarray,
+		  struct iio_channel_info *channels,
 		  int num_channels)
 {
 	int k;
 	for (k = 0; k < num_channels; k++)
-		switch (infoarray[k].bytes) {
+		switch (channels[k].bytes) {
 			/* only a few cases implemented so far */
 		case 2:
-			print2byte(*(uint16_t *)(data + infoarray[k].location),
-				   &infoarray[k]);
+			print2byte(*(uint16_t *)(data + channels[k].location),
+				   &channels[k]);
 			break;
 		case 8:
-			if (infoarray[k].is_signed) {
+			if (channels[k].is_signed) {
 				int64_t val = *(int64_t *)
 					(data +
-					 infoarray[k].location);
-				if ((val >> infoarray[k].bits_used) & 1)
-					val = (val & infoarray[k].mask) |
-						~infoarray[k].mask;
+					 channels[k].location);
+				if ((val >> channels[k].bits_used) & 1)
+					val = (val & channels[k].mask) |
+						~channels[k].mask;
 				/* special case for timestamp */
-				if (infoarray[k].scale == 1.0f &&
-				    infoarray[k].offset == 0.0f)
-					printf(" %lld", val);
+				if (channels[k].scale == 1.0f &&
+				    channels[k].offset == 0.0f)
+					printf("%" PRId64 " ", val);
 				else
 					printf("%05f ", ((float)val +
-							 infoarray[k].offset)*
-					       infoarray[k].scale);
+							 channels[k].offset)*
+					       channels[k].scale);
 			}
 			break;
 		default:
@@ -130,10 +134,7 @@ int main(int argc, char **argv)
 	unsigned long timedelay = 1000000;
 	unsigned long buf_len = 128;
 
-
 	int ret, c, i, j, toread;
-
-	FILE *fp_ev;
 	int fp;
 
 	int num_channels;
@@ -149,7 +150,7 @@ int main(int argc, char **argv)
 	int noevents = 0;
 	char *dummy;
 
-	struct iio_channel_info *infoarray;
+	struct iio_channel_info *channels;
 
 	while ((c = getopt(argc, argv, "l:w:c:et:n:")) != -1) {
 		switch (c) {
@@ -192,7 +193,7 @@ int main(int argc, char **argv)
 	asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num);
 	if (trigger_name == NULL) {
 		/*
-		 * Build the trigger name. If it is device associated it's
+		 * Build the trigger name. If it is device associated its
 		 * name is <device_name>_dev[n] where n matches the device
 		 * number found above
 		 */
@@ -217,7 +218,7 @@ int main(int argc, char **argv)
 	 * Parse the files in scan_elements to identify what channels are
 	 * present
 	 */
-	ret = build_channel_array(dev_dir_name, &infoarray, &num_channels);
+	ret = build_channel_array(dev_dir_name, &channels, &num_channels);
 	if (ret) {
 		printf("Problem reading scan element information\n");
 		printf("diag %s\n", dev_dir_name);
@@ -236,7 +237,7 @@ int main(int argc, char **argv)
 		goto error_free_triggername;
 	}
 	printf("%s %s\n", dev_dir_name, trigger_name);
-	/* Set the device trigger to be the data rdy trigger found above */
+	/* Set the device trigger to be the data ready trigger found above */
 	ret = write_sysfs_string_and_verify("trigger/current_trigger",
 					dev_dir_name,
 					trigger_name);
@@ -254,7 +255,7 @@ int main(int argc, char **argv)
 	ret = write_sysfs_int("enable", buf_dir_name, 1);
 	if (ret < 0)
 		goto error_free_buf_dir_name;
-	scan_size = size_from_channelarray(infoarray, num_channels);
+	scan_size = size_from_channelarray(channels, num_channels);
 	data = malloc(scan_size*buf_len);
 	if (!data) {
 		ret = -ENOMEM;
@@ -269,7 +270,7 @@ int main(int argc, char **argv)
 
 	/* Attempt to open non blocking the access dev */
 	fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
-	if (fp == -1) { /*If it isn't there make the node */
+	if (fp == -1) { /* If it isn't there make the node */
 		printf("Failed to open %s\n", buffer_access);
 		ret = -errno;
 		goto error_free_buffer_access;
@@ -300,16 +301,16 @@ int main(int argc, char **argv)
 		}
 		for (i = 0; i < read_size/scan_size; i++)
 			process_scan(data + scan_size*i,
-				     infoarray,
+				     channels,
 				     num_channels);
 	}
 
-	/* Stop the ring buffer */
+	/* Stop the buffer */
 	ret = write_sysfs_int("enable", buf_dir_name, 0);
 	if (ret < 0)
 		goto error_close_buffer_access;
 
-	/* Disconnect from the trigger - just write a dummy name.*/
+	/* Disconnect the trigger - just write a dummy name. */
 	write_sysfs_string("trigger/current_trigger",
 			dev_dir_name, "NULL");
 
diff --git a/drivers/staging/iio/Documentation/iio_event_monitor.c b/drivers/staging/iio/Documentation/iio_event_monitor.c
index 22275845fb1..3a9b0008740 100644
--- a/drivers/staging/iio/Documentation/iio_event_monitor.c
+++ b/drivers/staging/iio/Documentation/iio_event_monitor.c
@@ -45,6 +45,7 @@ static const char * const iio_chan_type_name_spec[] = {
 	[IIO_ANGL] = "angl",
 	[IIO_TIMESTAMP] = "timestamp",
 	[IIO_CAPACITANCE] = "capacitance",
+	[IIO_ALTVOLTAGE] = "altvoltage",
 };
 
 static const char * const iio_ev_type_text[] = {
@@ -67,6 +68,12 @@ static const char * const iio_modifier_names[] = {
 	[IIO_MOD_Z] = "z",
 	[IIO_MOD_LIGHT_BOTH] = "both",
 	[IIO_MOD_LIGHT_IR] = "ir",
+	[IIO_MOD_ROOT_SUM_SQUARED_X_Y] = "sqrt(x^2+y^2)",
+	[IIO_MOD_SUM_SQUARED_X_Y_Z] = "x^2+y^2+z^2",
+	[IIO_MOD_LIGHT_CLEAR] = "clear",
+	[IIO_MOD_LIGHT_RED] = "red",
+	[IIO_MOD_LIGHT_GREEN] = "green",
+	[IIO_MOD_LIGHT_BLUE] = "blue",
 };
 
 static bool event_is_known(struct iio_event_data *event)
@@ -92,6 +99,7 @@ static bool event_is_known(struct iio_event_data *event)
 	case IIO_ANGL:
 	case IIO_TIMESTAMP:
 	case IIO_CAPACITANCE:
+	case IIO_ALTVOLTAGE:
 		break;
 	default:
 		return false;
@@ -104,6 +112,12 @@ static bool event_is_known(struct iio_event_data *event)
 	case IIO_MOD_Z:
 	case IIO_MOD_LIGHT_BOTH:
 	case IIO_MOD_LIGHT_IR:
+	case IIO_MOD_ROOT_SUM_SQUARED_X_Y:
+	case IIO_MOD_SUM_SQUARED_X_Y_Z:
+	case IIO_MOD_LIGHT_CLEAR:
+	case IIO_MOD_LIGHT_RED:
+	case IIO_MOD_LIGHT_GREEN:
+	case IIO_MOD_LIGHT_BLUE:
 		break;
 	default:
 		return false;
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
index 6f3a392297e..cf32ae099cd 100644
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ b/drivers/staging/iio/Documentation/iio_utils.h
@@ -7,14 +7,15 @@
  * the Free Software Foundation.
  */
 
-/* Made up value to limit allocation sizes */
 #include <string.h>
 #include <stdlib.h>
 #include <ctype.h>
 #include <stdio.h>
 #include <stdint.h>
 #include <dirent.h>
+#include <errno.h>
 
+/* Made up value to limit allocation sizes */
 #define IIO_MAX_NAME_LENGTH 30
 
 #define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements"
@@ -27,7 +28,7 @@ const char *iio_dir = "/sys/bus/iio/devices/";
  * @full_name: the full channel name
  * @generic_name: the output generic channel name
  **/
-static int iioutils_break_up_name(const char *full_name,
+inline int iioutils_break_up_name(const char *full_name,
 				  char **generic_name)
 {
 	char *current;
@@ -157,7 +158,8 @@ inline int iioutils_get_type(unsigned *is_signed,
 				     &padint, shift);
 			if (ret < 0) {
 				printf("failed to pass scan type description\n");
-				return ret;
+				ret = -errno;
+				goto error_close_sysfsfp;
 			}
 			*be = (endianchar == 'b');
 			*bytes = padint / 8;
@@ -173,7 +175,11 @@ inline int iioutils_get_type(unsigned *is_signed,
 			free(filename);
 
 			filename = 0;
+			sysfsfp = 0;
 		}
+error_close_sysfsfp:
+	if (sysfsfp)
+		fclose(sysfsfp);
 error_free_filename:
 	if (filename)
 		free(filename);
@@ -280,7 +286,7 @@ inline int build_channel_array(const char *device_dir,
 {
 	DIR *dp;
 	FILE *sysfsfp;
-	int count, temp, i;
+	int count, i;
 	struct iio_channel_info *current;
 	int ret;
 	const struct dirent *ent;
@@ -447,7 +453,7 @@ inline int find_type_by_name(const char *name, const char *type)
 
 	dp = opendir(iio_dir);
 	if (dp == NULL) {
-		printf("No industrialio devices available");
+		printf("No industrialio devices available\n");
 		return -ENODEV;
 	}
 
@@ -467,23 +473,30 @@ inline int find_type_by_name(const char *name, const char *type)
 						+ strlen(type)
 						+ numstrlen
 						+ 6);
-				if (filename == NULL)
+				if (filename == NULL) {
+					closedir(dp);
 					return -ENOMEM;
+				}
 				sprintf(filename, "%s%s%d/name",
 					iio_dir,
 					type,
 					number);
 				nameFile = fopen(filename, "r");
-				if (!nameFile)
+				if (!nameFile) {
+					free(filename);
 					continue;
+				}
 				free(filename);
 				fscanf(nameFile, "%s", thisname);
-				if (strcmp(name, thisname) == 0)
-					return number;
 				fclose(nameFile);
+				if (strcmp(name, thisname) == 0) {
+					closedir(dp);
+					return number;
+				}
 			}
 		}
 	}
+	closedir(dp);
 	return -ENODEV;
 }
 
@@ -512,6 +525,7 @@ inline int _write_sysfs_int(char *filename, char *basedir, int val, int verify)
 			goto error_free;
 		}
 		fscanf(sysfsfp, "%d", &test);
+		fclose(sysfsfp);
 		if (test != val) {
 			printf("Possible failure in int write %d to %s%s\n",
 				val,
@@ -561,6 +575,7 @@ int _write_sysfs_string(char *filename, char *basedir, char *val, int verify)
 			goto error_free;
 		}
 		fscanf(sysfsfp, "%s", temp);
+		fclose(sysfsfp);
 		if (strcmp(temp, val) != 0) {
 			printf("Possible failure in string write of %s "
 				"Should be %s "
diff --git a/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583 b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583
index 470f7ad9c07..470f7ad9c07 100755..100644
--- a/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583
+++ b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2583
diff --git a/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x
index b2798b258bf..b2798b258bf 100755..100644
--- a/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x
+++ b/drivers/staging/iio/Documentation/light/sysfs-bus-iio-light-tsl2x7x
diff --git a/drivers/staging/iio/Documentation/overview.txt b/drivers/staging/iio/Documentation/overview.txt
index afc39ecde9c..43f92b06bc3 100644
--- a/drivers/staging/iio/Documentation/overview.txt
+++ b/drivers/staging/iio/Documentation/overview.txt
@@ -8,7 +8,7 @@ actual devices combine some ADCs with digital to analog converters
 The aim is to fill the gap between the somewhat similar hwmon and
 input subsystems.  Hwmon is very much directed at low sample rate
 sensors used in applications such as fan speed control and temperature
-measurement.  Input is, as it's name suggests focused on input
+measurement.  Input is, as its name suggests focused on input
 devices. In some cases there is considerable overlap between these and
 IIO.
 
diff --git a/drivers/staging/iio/Documentation/ring.txt b/drivers/staging/iio/Documentation/ring.txt
index e33807761cd..e1da43381d0 100644
--- a/drivers/staging/iio/Documentation/ring.txt
+++ b/drivers/staging/iio/Documentation/ring.txt
@@ -15,8 +15,8 @@ struct iio_ring_buffer contains a struct iio_ring_setup_ops *setup_ops
 which in turn contains the 4 function pointers
 (preenable, postenable, predisable and postdisable).
 These are used to perform device specific steps on either side
-of the core changing it's current mode to indicate that the buffer
-is enabled or disabled (along with enabling triggering etc as appropriate).
+of the core changing its current mode to indicate that the buffer
+is enabled or disabled (along with enabling triggering etc. as appropriate).
 
 Also in struct iio_ring_buffer is a struct iio_ring_access_funcs.
 The function pointers within here are used to allow the core to handle
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio-light b/drivers/staging/iio/Documentation/sysfs-bus-iio-light
index 715c74dcb53..17e5c9c515d 100644
--- a/drivers/staging/iio/Documentation/sysfs-bus-iio-light
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio-light
@@ -34,7 +34,7 @@ Description:
 		it comes back in SI units, it should also include _input else it
 		should include _raw to signify it is not in SI units.
 
-What:		/sys/.../device[n]/proximity_on_chip_ambient_infrared_supression
+What:		/sys/.../device[n]/proximity_on_chip_ambient_infrared_suppression
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -82,3 +82,26 @@ Contact:	linux-iio@vger.kernel.org
 Description:
 		This property gets/sets the table of coefficients
 		used in calculating illuminance in lux.
+
+What:		/sys/bus/iio/devices/device[n]/in_intensity_clear[_input|_raw]
+What:		/sys/bus/iio/devices/device[n]/in_intensity_red[_input|_raw]
+What:		/sys/bus/iio/devices/device[n]/in_intensity_green[_input|_raw]
+What:		/sys/bus/iio/devices/device[n]/in_intensity_blue[_input|_raw]
+KernelVersion:	3.6.0
+Contact:	linux-iio@vger.kernel.org
+Description:
+		This property is supported by sensors that have a RGBC
+		sensing mode. This value should be the output from a reading
+		and if expressed in SI units, should include _input. If this
+		value is not in SI units (irradiance, uW/mm^2), then it should
+		include _raw.
+
+What:		/sys/bus/iio/devices/device[n]/in_cct0[_input|_raw]
+KernelVersion:	3.6.0
+Contact:	linux-iio@vger.kernel.org
+Description:
+		This should return the correlated color temperature from the
+		light sensor. If it comes back in SI units, it should also
+		include _input else it should include _raw to signify it is not
+		in SI units.
+
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index 3c8e5ec26ac..04cd6ec1f70 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -29,7 +29,6 @@ source "drivers/staging/iio/accel/Kconfig"
 source "drivers/staging/iio/adc/Kconfig"
 source "drivers/staging/iio/addac/Kconfig"
 source "drivers/staging/iio/cdc/Kconfig"
-source "drivers/staging/iio/dac/Kconfig"
 source "drivers/staging/iio/frequency/Kconfig"
 source "drivers/staging/iio/gyro/Kconfig"
 source "drivers/staging/iio/impedance-analyzer/Kconfig"
diff --git a/drivers/staging/iio/Makefile b/drivers/staging/iio/Makefile
index 6a46d5afb38..fa6937d92ee 100644
--- a/drivers/staging/iio/Makefile
+++ b/drivers/staging/iio/Makefile
@@ -17,7 +17,6 @@ obj-y += accel/
 obj-y += adc/
 obj-y += addac/
 obj-y += cdc/
-obj-y += dac/
 obj-y += frequency/
 obj-y += gyro/
 obj-y += impedance-analyzer/
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 02b340919c0..204106b72d2 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -159,21 +159,6 @@ static int adis16201_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16201_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	int ret;
-	bool res;
-
-	if (len < 1)
-		return -EINVAL;
-	ret = strtobool(buf, &res);
-	if (ret || !res)
-		return ret;
-	return adis16201_reset(dev_to_iio_dev(dev));
-}
-
 int adis16201_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret = 0;
@@ -507,19 +492,7 @@ static struct iio_chan_spec adis16201_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(7)
 };
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16201_write_reset, 0);
-
-static struct attribute *adis16201_attributes[] = {
-	&iio_dev_attr_reset.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group adis16201_attribute_group = {
-	.attrs = adis16201_attributes,
-};
-
 static const struct iio_info adis16201_info = {
-	.attrs = &adis16201_attribute_group,
 	.read_raw = &adis16201_read_raw,
 	.write_raw = &adis16201_write_raw,
 	.driver_module = THIS_MODULE,
diff --git a/drivers/staging/iio/accel/adis16201_ring.c b/drivers/staging/iio/accel/adis16201_ring.c
index 247602a8e54..03fcf6e319d 100644
--- a/drivers/staging/iio/accel/adis16201_ring.c
+++ b/drivers/staging/iio/accel/adis16201_ring.c
@@ -70,7 +70,7 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)
@@ -85,8 +85,9 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index 15d46bfd1b4..22085e9dfd1 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -178,22 +178,6 @@ static int adis16203_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16203_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	if (len < 1)
-		return -EINVAL;
-	switch (buf[0]) {
-	case '1':
-	case 'y':
-	case 'Y':
-		return adis16203_reset(indio_dev);
-	}
-	return -EINVAL;
-}
-
 int adis16203_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret = 0;
@@ -444,19 +428,7 @@ static struct iio_chan_spec adis16203_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(5),
 };
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16203_write_reset, 0);
-
-static struct attribute *adis16203_attributes[] = {
-	&iio_dev_attr_reset.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group adis16203_attribute_group = {
-	.attrs = adis16203_attributes,
-};
-
 static const struct iio_info adis16203_info = {
-	.attrs = &adis16203_attribute_group,
 	.read_raw = &adis16203_read_raw,
 	.write_raw = &adis16203_write_raw,
 	.driver_module = THIS_MODULE,
diff --git a/drivers/staging/iio/accel/adis16203_ring.c b/drivers/staging/iio/accel/adis16203_ring.c
index 7bbd2c2bbd1..c16b2b7323a 100644
--- a/drivers/staging/iio/accel/adis16203_ring.c
+++ b/drivers/staging/iio/accel/adis16203_ring.c
@@ -69,7 +69,7 @@ static irqreturn_t adis16203_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
@@ -86,8 +86,9 @@ static irqreturn_t adis16203_trigger_handler(int irq, void *p)
 			      (u8 *)data,
 			      pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index ac9d95e4ea4..5f2e5f11c54 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -169,32 +169,6 @@ error_ret:
 	return ret;
 }
 
-static ssize_t adis16204_read_14bit_signed(struct device *dev,
-		struct device_attribute *attr,
-		char *buf)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	s16 val = 0;
-	ssize_t ret;
-
-	mutex_lock(&indio_dev->mlock);
-
-	ret = adis16204_spi_read_reg_16(indio_dev,
-					this_attr->address, (u16 *)&val);
-	if (!ret) {
-		if (val & ADIS16204_ERROR_ACTIVE)
-			adis16204_check_status(indio_dev);
-
-		val = ((s16)(val << 2) >> 2);
-		ret = sprintf(buf, "%d\n", val);
-	}
-
-	mutex_unlock(&indio_dev->mlock);
-
-	return ret;
-}
-
 static int adis16204_reset(struct iio_dev *indio_dev)
 {
 	int ret;
@@ -207,23 +181,6 @@ static int adis16204_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16204_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-
-	if (len < 1)
-		return -EINVAL;
-	switch (buf[0]) {
-	case '1':
-	case 'y':
-	case 'Y':
-		return adis16204_reset(indio_dev);
-	}
-	return -EINVAL;
-}
-
 int adis16204_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret = 0;
@@ -299,18 +256,6 @@ err_ret:
 }
 
 /* Unique to this driver currently */
-#define IIO_DEV_ATTR_ACCEL_XY(_show, _addr)			\
-	IIO_DEVICE_ATTR(in_accel_xy, S_IRUGO, _show, NULL, _addr)
-#define IIO_DEV_ATTR_ACCEL_XYPEAK(_show, _addr)		\
-	IIO_DEVICE_ATTR(in_accel_xypeak, S_IRUGO, _show, NULL, _addr)
-
-static IIO_DEV_ATTR_ACCEL_XY(adis16204_read_14bit_signed,
-		ADIS16204_XY_RSS_OUT);
-static IIO_DEV_ATTR_ACCEL_XYPEAK(adis16204_read_14bit_signed,
-		ADIS16204_XY_PEAK_OUT);
-static IIO_CONST_ATTR(in_accel_xy_scale, "0.017125");
-
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16204_write_reset, 0);
 
 enum adis16204_channel {
 	in_supply,
@@ -318,9 +263,10 @@ enum adis16204_channel {
 	temp,
 	accel_x,
 	accel_y,
+	accel_xy,
 };
 
-static u8 adis16204_addresses[5][3] = {
+static u8 adis16204_addresses[6][3] = {
 	[in_supply] = { ADIS16204_SUPPLY_OUT },
 	[in_aux] = { ADIS16204_AUX_ADC },
 	[temp] = { ADIS16204_TEMP_OUT },
@@ -328,6 +274,8 @@ static u8 adis16204_addresses[5][3] = {
 		      ADIS16204_X_PEAK_OUT },
 	[accel_y] = { ADIS16204_XACCL_OUT, ADIS16204_YACCL_NULL,
 		      ADIS16204_Y_PEAK_OUT },
+	[accel_xy] = { ADIS16204_XY_RSS_OUT, 0,
+		       ADIS16204_XY_PEAK_OUT },
 };
 
 static int adis16204_read_raw(struct iio_dev *indio_dev,
@@ -381,10 +329,16 @@ static int adis16204_read_raw(struct iio_dev *indio_dev,
 			return IIO_VAL_INT_PLUS_MICRO;
 		case IIO_ACCEL:
 			*val = 0;
-			if (chan->channel == 'x')
+			switch (chan->channel2) {
+			case IIO_MOD_X:
+			case IIO_MOD_ROOT_SUM_SQUARED_X_Y:
 				*val2 = 17125;
-			else
+				break;
+			case IIO_MOD_Y:
+			case IIO_MOD_Z:
 				*val2 = 8407;
+				break;
+			}
 			return IIO_VAL_INT_PLUS_MICRO;
 		default:
 			return -EINVAL;
@@ -517,22 +471,23 @@ static struct iio_chan_spec adis16204_channels[] = {
 		},
 	},
 	IIO_CHAN_SOFT_TIMESTAMP(5),
-};
-
-static struct attribute *adis16204_attributes[] = {
-	&iio_dev_attr_reset.dev_attr.attr,
-	&iio_dev_attr_in_accel_xy.dev_attr.attr,
-	&iio_dev_attr_in_accel_xypeak.dev_attr.attr,
-	&iio_const_attr_in_accel_xy_scale.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group adis16204_attribute_group = {
-	.attrs = adis16204_attributes,
+	{
+		.type = IIO_ACCEL,
+		.modified = 1,
+		.channel2 = IIO_MOD_ROOT_SUM_SQUARED_X_Y,
+		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
+		.address = accel_xy,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 14,
+			.storagebits = 16,
+		},
+	}
 };
 
 static const struct iio_info adis16204_info = {
-	.attrs = &adis16204_attribute_group,
 	.read_raw = &adis16204_read_raw,
 	.write_raw = &adis16204_write_raw,
 	.driver_module = THIS_MODULE,
@@ -569,7 +524,7 @@ static int __devinit adis16204_probe(struct spi_device *spi)
 
 	ret = iio_buffer_register(indio_dev,
 				  adis16204_channels,
-				  ARRAY_SIZE(adis16204_channels));
+				  6);
 	if (ret) {
 		printk(KERN_ERR "failed to initialize the ring\n");
 		goto error_unreg_ring_funcs;
diff --git a/drivers/staging/iio/accel/adis16204_ring.c b/drivers/staging/iio/accel/adis16204_ring.c
index f73518bc658..1d2b31cc849 100644
--- a/drivers/staging/iio/accel/adis16204_ring.c
+++ b/drivers/staging/iio/accel/adis16204_ring.c
@@ -66,7 +66,7 @@ static irqreturn_t adis16204_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
@@ -81,8 +81,9 @@ static irqreturn_t adis16204_trigger_handler(int irq, void *p)
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index f6fd0d31d4f..494570508c3 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -153,23 +153,6 @@ static int adis16209_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16209_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-
-	if (len < 1)
-		return -EINVAL;
-	switch (buf[0]) {
-	case '1':
-	case 'y':
-	case 'Y':
-		return adis16209_reset(indio_dev);
-	}
-	return -EINVAL;
-}
-
 int adis16209_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret = 0;
@@ -519,19 +502,7 @@ static struct iio_chan_spec adis16209_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(8)
 };
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16209_write_reset, 0);
-
-static struct attribute *adis16209_attributes[] = {
-	&iio_dev_attr_reset.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group adis16209_attribute_group = {
-	.attrs = adis16209_attributes,
-};
-
 static const struct iio_info adis16209_info = {
-	.attrs = &adis16209_attribute_group,
 	.read_raw = &adis16209_read_raw,
 	.write_raw = &adis16209_write_raw,
 	.driver_module = THIS_MODULE,
@@ -606,8 +577,6 @@ static int adis16209_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
-	flush_scheduled_work();
-
 	iio_device_unregister(indio_dev);
 	adis16209_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
diff --git a/drivers/staging/iio/accel/adis16209_ring.c b/drivers/staging/iio/accel/adis16209_ring.c
index 090607504c9..1a4a55c27c7 100644
--- a/drivers/staging/iio/accel/adis16209_ring.c
+++ b/drivers/staging/iio/accel/adis16209_ring.c
@@ -66,7 +66,7 @@ static irqreturn_t adis16209_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
@@ -81,8 +81,9 @@ static irqreturn_t adis16209_trigger_handler(int irq, void *p)
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index 6a9ac898cb0..575f1af25d5 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -204,26 +204,6 @@ static int adis16220_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16220_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	bool val;
-	int ret;
-
-	ret = strtobool(buf, &val);
-	if (ret)
-		return ret;
-	if (!val)
-		return -EINVAL;
-
-	ret = adis16220_reset(indio_dev);
-	if (ret)
-		return ret;
-	return len;
-}
-
 static ssize_t adis16220_write_capture(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf, size_t len)
@@ -454,9 +434,6 @@ static struct bin_attribute adc2_bin = {
 	.size = ADIS16220_CAPTURE_SIZE,
 };
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL,
-		adis16220_write_reset, 0);
-
 #define IIO_DEV_ATTR_CAPTURE(_store)				\
 	IIO_DEVICE_ATTR(capture, S_IWUSR, NULL, _store, 0)
 
@@ -611,7 +588,6 @@ static const struct iio_chan_spec adis16220_channels[] = {
 };
 
 static struct attribute *adis16220_attributes[] = {
-	&iio_dev_attr_reset.dev_attr.attr,
 	&iio_dev_attr_capture.dev_attr.attr,
 	&iio_dev_attr_capture_count.dev_attr.attr,
 	NULL
@@ -694,8 +670,6 @@ static int adis16220_remove(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
-	flush_scheduled_work();
-
 	sysfs_remove_bin_file(&indio_dev->dev.kobj, &adc2_bin);
 	sysfs_remove_bin_file(&indio_dev->dev.kobj, &adc1_bin);
 	sysfs_remove_bin_file(&indio_dev->dev.kobj, &accel_bin);
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 8b15eaea338..b30b7874ffb 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -199,23 +199,6 @@ static int adis16240_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16240_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-
-	if (len < 1)
-		return -EINVAL;
-	switch (buf[0]) {
-	case '1':
-	case 'y':
-	case 'Y':
-		return adis16240_reset(indio_dev);
-	}
-	return -EINVAL;
-}
-
 int adis16240_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret = 0;
@@ -329,8 +312,6 @@ static IIO_DEVICE_ATTR(in_accel_xyz_squared_peak_raw, S_IRUGO,
 		       adis16240_read_12bit_signed, NULL,
 		       ADIS16240_XYZPEAK_OUT);
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16240_write_reset, 0);
-
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("4096");
 
 enum adis16240_chan {
@@ -500,7 +481,8 @@ static struct iio_chan_spec adis16240_channels[] = {
 		.channel2 = IIO_MOD_X,
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		.address = accel_x,
 		.scan_index = ADIS16240_SCAN_ACC_X,
 		.scan_type = {
@@ -514,7 +496,8 @@ static struct iio_chan_spec adis16240_channels[] = {
 		.channel2 = IIO_MOD_Y,
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		.address = accel_y,
 		.scan_index = ADIS16240_SCAN_ACC_Y,
 		.scan_type = {
@@ -528,7 +511,8 @@ static struct iio_chan_spec adis16240_channels[] = {
 		.channel2 = IIO_MOD_Z,
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT |
-		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16240_SCAN_ACC_Z,
 		.scan_type = {
@@ -556,7 +540,6 @@ static struct iio_chan_spec adis16240_channels[] = {
 static struct attribute *adis16240_attributes[] = {
 	&iio_dev_attr_in_accel_xyz_squared_peak_raw.dev_attr.attr,
 	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
-	&iio_dev_attr_reset.dev_attr.attr,
 	NULL
 };
 
@@ -641,8 +624,6 @@ static int adis16240_remove(struct spi_device *spi)
 
 	struct iio_dev *indio_dev = spi_get_drvdata(spi);
 
-	flush_scheduled_work();
-
 	iio_device_unregister(indio_dev);
 	adis16240_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
diff --git a/drivers/staging/iio/accel/adis16240_ring.c b/drivers/staging/iio/accel/adis16240_ring.c
index 86a2a4757ea..360dfed6d4d 100644
--- a/drivers/staging/iio/accel/adis16240_ring.c
+++ b/drivers/staging/iio/accel/adis16240_ring.c
@@ -64,7 +64,7 @@ static irqreturn_t adis16240_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
@@ -79,8 +79,9 @@ static irqreturn_t adis16240_trigger_handler(int irq, void *p)
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index 51b00dfc046..18d108fd967 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -143,7 +143,7 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 	if (data == NULL) {
 		dev_err(indio_dev->dev.parent,
 			"memory alloc failed in buffer bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
@@ -151,13 +151,13 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 
 	  /* Guaranteed to be aligned with 8 byte boundary */
 	if (indio_dev->scan_timestamp)
-		*(s64 *)(((phys_addr_t)data + len
-				+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
+		*(s64 *)((u8 *)data + ALIGN(len, sizeof(s64)))
 			= pf->timestamp;
 	buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index 6ec5c204ff1..c218d71abf1 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -1189,7 +1189,7 @@ static int __devinit sca3000_probe(struct spi_device *spi)
 		ret = request_threaded_irq(spi->irq,
 					   NULL,
 					   &sca3000_event_handler,
-					   IRQF_TRIGGER_FALLING,
+					   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
 					   "sca3000",
 					   indio_dev);
 		if (ret)
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 8f1b3af02f2..67711b7d718 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -13,7 +13,7 @@ config AD7291
 config AD7298
 	tristate "Analog Devices AD7298 ADC driver"
 	depends on SPI
-	select IIO_KFIFO_BUF if IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER if IIO_BUFFER
 	help
 	  Say yes here to build support for Analog Devices AD7298
 	  8 Channel ADC with temperature sensor.
@@ -25,8 +25,7 @@ config AD7606
 	tristate "Analog Devices AD7606 ADC driver"
 	depends on GPIOLIB
 	select IIO_BUFFER
-	select IIO_TRIGGER
-	select IIO_KFIFO_BUF
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to build support for Analog Devices:
 	  ad7606, ad7606-6, ad7606-4 analog to digital converters (ADC).
@@ -64,7 +63,7 @@ config AD799X_RING_BUFFER
 	bool "Analog Devices AD799x: use ring buffer"
 	depends on AD799X
 	select IIO_BUFFER
-	select IIO_KFIFO_BUF
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to include ring buffer support in the AD799X
 	  ADC driver.
@@ -73,8 +72,7 @@ config AD7476
 	tristate "Analog Devices AD7475/6/7/8 AD7466/7/8 and AD7495 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_KFIFO_BUF
-	select IIO_TRIGGER
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to build support for Analog Devices
 	  AD7475, AD7476, AD7477, AD7478, AD7466, AD7467, AD7468, AD7495
@@ -88,8 +86,7 @@ config AD7887
 	tristate "Analog Devices AD7887 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_KFIFO_BUF
-	select IIO_TRIGGER
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to build support for Analog Devices
 	  AD7887 SPI analog to digital converter (ADC).
@@ -114,8 +111,7 @@ config AD7793
 	tristate "Analog Devices AD7792 AD7793 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_KFIFO_BUF
-	select IIO_TRIGGER
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to build support for Analog Devices
 	  AD7792 and AD7793 SPI analog to digital converters (ADC).
@@ -136,8 +132,7 @@ config AD7192
 	tristate "Analog Devices AD7190 AD7192 AD7195 ADC driver"
 	depends on SPI
 	select IIO_BUFFER
-	select IIO_KFIFO_BUF
-	select IIO_TRIGGER
+	select IIO_TRIGGERED_BUFFER
 	help
 	  Say yes here to build support for Analog Devices AD7190,
 	  AD7192 or AD7195 SPI analog to digital converters (ADC).
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 5eaeaf1f0ae..22c3923d55e 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -20,9 +20,9 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad7192.h"
 
@@ -146,7 +146,6 @@ struct ad7192_state {
 	u32				mode;
 	u32				conf;
 	u32				scale_avail[8][2];
-	long				available_scan_masks[9];
 	u8				gpocon;
 	u8				devid;
 	/*
@@ -538,45 +537,18 @@ static const struct iio_buffer_setup_ops ad7192_ring_setup_ops = {
 	.postenable = &iio_triggered_buffer_postenable,
 	.predisable = &iio_triggered_buffer_predisable,
 	.postdisable = &ad7192_ring_postdisable,
+	.validate_scan_mask = &iio_validate_scan_mask_onehot,
 };
 
 static int ad7192_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
-	int ret;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
-						 &ad7192_trigger_handler,
-						 IRQF_ONESHOT,
-						 indio_dev,
-						 "ad7192_consumer%d",
-						 indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-
-	/* Ring buffer functions - here trigger setup related */
-	indio_dev->setup_ops = &ad7192_ring_setup_ops;
-
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+			&ad7192_trigger_handler, &ad7192_ring_setup_ops);
 }
 
 static void ad7192_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
 
 /**
@@ -1012,7 +984,7 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 	struct ad7192_platform_data *pdata = spi->dev.platform_data;
 	struct ad7192_state *st;
 	struct iio_dev *indio_dev;
-	int ret, i , voltage_uv = 0;
+	int ret , voltage_uv = 0;
 
 	if (!pdata) {
 		dev_err(&spi->dev, "no platform data?\n");
@@ -1056,17 +1028,11 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = ad7192_channels;
 	indio_dev->num_channels = ARRAY_SIZE(ad7192_channels);
-	indio_dev->available_scan_masks = st->available_scan_masks;
 	if (st->devid == ID_AD7195)
 		indio_dev->info = &ad7195_info;
 	else
 		indio_dev->info = &ad7192_info;
 
-	for (i = 0; i < indio_dev->num_channels; i++)
-		st->available_scan_masks[i] = (1 << i) | (1 <<
-			indio_dev->channels[indio_dev->num_channels - 1].
-			scan_index);
-
 	init_waitqueue_head(&st->wq_data_avail);
 
 	ret = ad7192_register_ring_funcs_and_init(indio_dev);
@@ -1077,23 +1043,15 @@ static int __devinit ad7192_probe(struct spi_device *spi)
 	if (ret)
 		goto error_ring_cleanup;
 
-	ret = iio_buffer_register(indio_dev,
-				  indio_dev->channels,
-				  indio_dev->num_channels);
-	if (ret)
-		goto error_remove_trigger;
-
 	ret = ad7192_setup(st);
 	if (ret)
-		goto error_unreg_ring;
+		goto error_remove_trigger;
 
 	ret = iio_device_register(indio_dev);
 	if (ret < 0)
-		goto error_unreg_ring;
+		goto error_remove_trigger;
 	return 0;
 
-error_unreg_ring:
-	iio_buffer_unregister(indio_dev);
 error_remove_trigger:
 	ad7192_remove_trigger(indio_dev);
 error_ring_cleanup:
@@ -1116,7 +1074,6 @@ static int ad7192_remove(struct spi_device *spi)
 	struct ad7192_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	iio_buffer_unregister(indio_dev);
 	ad7192_remove_trigger(indio_dev);
 	ad7192_ring_cleanup(indio_dev);
 
diff --git a/drivers/staging/iio/adc/ad7298.h b/drivers/staging/iio/adc/ad7298.h
index 5051a7e4d4f..18f27872300 100644
--- a/drivers/staging/iio/adc/ad7298.h
+++ b/drivers/staging/iio/adc/ad7298.h
@@ -55,6 +55,8 @@ struct ad7298_state {
 #ifdef CONFIG_IIO_BUFFER
 int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad7298_ring_cleanup(struct iio_dev *indio_dev);
+int ad7298_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *active_scan_mask);
 #else /* CONFIG_IIO_BUFFER */
 
 static inline int
@@ -66,5 +68,8 @@ ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 static inline void ad7298_ring_cleanup(struct iio_dev *indio_dev)
 {
 }
+
+#define ad7298_update_scan_mode NULL
+
 #endif /* CONFIG_IIO_BUFFER */
 #endif /* IIO_ADC_AD7298_H_ */
diff --git a/drivers/staging/iio/adc/ad7298_core.c b/drivers/staging/iio/adc/ad7298_core.c
index c90f2b3e661..6141f4a70cf 100644
--- a/drivers/staging/iio/adc/ad7298_core.c
+++ b/drivers/staging/iio/adc/ad7298_core.c
@@ -45,8 +45,8 @@ static struct iio_chan_spec ad7298_channels[] = {
 		.channel = 0,
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		.address = 9,
-		.scan_index = AD7298_CH_TEMP,
+		.address = AD7298_CH_TEMP,
+		.scan_index = -1,
 		.scan_type = {
 			.sign = 's',
 			.realbits = 32,
@@ -171,6 +171,7 @@ static int ad7298_read_raw(struct iio_dev *indio_dev,
 
 static const struct iio_info ad7298_info = {
 	.read_raw = &ad7298_read_raw,
+	.update_scan_mode = ad7298_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
 
@@ -231,19 +232,12 @@ static int __devinit ad7298_probe(struct spi_device *spi)
 	if (ret)
 		goto error_disable_reg;
 
-	ret = iio_buffer_register(indio_dev,
-				  &ad7298_channels[1], /* skip temp0 */
-				  ARRAY_SIZE(ad7298_channels) - 1);
-	if (ret)
-		goto error_cleanup_ring;
 	ret = iio_device_register(indio_dev);
 	if (ret)
-		goto error_unregister_ring;
+		goto error_cleanup_ring;
 
 	return 0;
 
-error_unregister_ring:
-	iio_buffer_unregister(indio_dev);
 error_cleanup_ring:
 	ad7298_ring_cleanup(indio_dev);
 error_disable_reg:
@@ -263,7 +257,6 @@ static int __devexit ad7298_remove(struct spi_device *spi)
 	struct ad7298_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	iio_buffer_unregister(indio_dev);
 	ad7298_ring_cleanup(indio_dev);
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
diff --git a/drivers/staging/iio/adc/ad7298_ring.c b/drivers/staging/iio/adc/ad7298_ring.c
index 908a3e5609d..fd1d855ff57 100644
--- a/drivers/staging/iio/adc/ad7298_ring.c
+++ b/drivers/staging/iio/adc/ad7298_ring.c
@@ -13,37 +13,29 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad7298.h"
 
 /**
- * ad7298_ring_preenable() setup the parameters of the ring before enabling
- *
- * The complex nature of the setting of the number of bytes per datum is due
- * to this driver currently ensuring that the timestamp is stored at an 8
- * byte boundary.
+ * ad7298_update_scan_mode() setup the spi transfer buffer for the new scan mask
  **/
-static int ad7298_ring_preenable(struct iio_dev *indio_dev)
+int ad7298_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *active_scan_mask)
 {
 	struct ad7298_state *st = iio_priv(indio_dev);
 	int i, m;
 	unsigned short command;
-	int scan_count, ret;
-
-	ret = iio_sw_buffer_preenable(indio_dev);
-	if (ret < 0)
-		return ret;
+	int scan_count;
 
 	/* Now compute overall size */
-	scan_count = bitmap_weight(indio_dev->active_scan_mask,
-				   indio_dev->masklength);
+	scan_count = bitmap_weight(active_scan_mask, indio_dev->masklength);
 
 	command = AD7298_WRITE | st->ext_ref;
 
 	for (i = 0, m = AD7298_CH(0); i < AD7298_MAX_CHAN; i++, m >>= 1)
-		if (test_bit(i, indio_dev->active_scan_mask))
+		if (test_bit(i, active_scan_mask))
 			command |= m;
 
 	st->tx_buf[0] = cpu_to_be16(command);
@@ -90,7 +82,7 @@ static irqreturn_t ad7298_trigger_handler(int irq, void *p)
 
 	b_sent = spi_sync(st->spi, &st->ring_msg);
 	if (b_sent)
-		return b_sent;
+		goto done;
 
 	if (indio_dev->scan_timestamp) {
 		time_ns = iio_get_time_ns();
@@ -103,54 +95,20 @@ static irqreturn_t ad7298_trigger_handler(int irq, void *p)
 		buf[i] = be16_to_cpu(st->rx_buf[i]);
 
 	indio_dev->buffer->access->store_to(ring, (u8 *)buf, time_ns);
+
+done:
 	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
 
-static const struct iio_buffer_setup_ops ad7298_ring_setup_ops = {
-	.preenable = &ad7298_ring_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-};
-
 int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
-	int ret;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	indio_dev->pollfunc = iio_alloc_pollfunc(NULL,
-						 &ad7298_trigger_handler,
-						 IRQF_ONESHOT,
-						 indio_dev,
-						 "ad7298_consumer%d",
-						 indio_dev->id);
-
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-
-	/* Ring buffer functions - here trigger setup related */
-	indio_dev->setup_ops = &ad7298_ring_setup_ops;
-	indio_dev->buffer->scan_timestamp = true;
-
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev, NULL,
+			&ad7298_trigger_handler, NULL);
 }
 
 void ad7298_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
diff --git a/drivers/staging/iio/adc/ad7476_core.c b/drivers/staging/iio/adc/ad7476_core.c
index be1c260cf16..4d30a798ba0 100644
--- a/drivers/staging/iio/adc/ad7476_core.c
+++ b/drivers/staging/iio/adc/ad7476_core.c
@@ -177,20 +177,12 @@ static int __devinit ad7476_probe(struct spi_device *spi)
 	if (ret)
 		goto error_disable_reg;
 
-	ret = iio_buffer_register(indio_dev,
-				  st->chip_info->channel,
-				  ARRAY_SIZE(st->chip_info->channel));
-	if (ret)
-		goto error_cleanup_ring;
-
 	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto error_ring_unregister;
 	return 0;
 
 error_ring_unregister:
-	iio_buffer_unregister(indio_dev);
-error_cleanup_ring:
 	ad7476_ring_cleanup(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(st->reg))
@@ -210,7 +202,6 @@ static int ad7476_remove(struct spi_device *spi)
 	struct ad7476_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	iio_buffer_unregister(indio_dev);
 	ad7476_ring_cleanup(indio_dev);
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
diff --git a/drivers/staging/iio/adc/ad7476_ring.c b/drivers/staging/iio/adc/ad7476_ring.c
index 383611b0576..d087b21c51f 100644
--- a/drivers/staging/iio/adc/ad7476_ring.c
+++ b/drivers/staging/iio/adc/ad7476_ring.c
@@ -15,8 +15,8 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad7476.h"
 
@@ -31,7 +31,7 @@ static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
 
 	rxbuf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (rxbuf == NULL)
-		return -ENOMEM;
+		goto done;
 
 	b_sent = spi_read(st->spi, rxbuf,
 			  st->chip_info->channel[0].scan_type.storagebits / 8);
@@ -52,51 +52,13 @@ done:
 	return IRQ_HANDLED;
 }
 
-static const struct iio_buffer_setup_ops ad7476_ring_setup_ops = {
-	.preenable = &iio_sw_buffer_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-};
-
 int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
-	struct ad7476_state *st = iio_priv(indio_dev);
-	int ret = 0;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	indio_dev->pollfunc
-		= iio_alloc_pollfunc(NULL,
-				     &ad7476_trigger_handler,
-				     IRQF_ONESHOT,
-				     indio_dev,
-				     "%s_consumer%d",
-				     spi_get_device_id(st->spi)->name,
-				     indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-
-	/* Ring buffer functions - here trigger setup related */
-	indio_dev->setup_ops = &ad7476_ring_setup_ops;
-	indio_dev->buffer->scan_timestamp = true;
-
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev, NULL,
+			&ad7476_trigger_handler, NULL);
 }
 
 void ad7476_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index a13afff2dfe..ccb97fecdea 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -533,20 +533,12 @@ struct iio_dev *ad7606_probe(struct device *dev, int irq,
 	if (ret)
 		goto error_free_irq;
 
-	ret = iio_buffer_register(indio_dev,
-				  indio_dev->channels,
-				  indio_dev->num_channels);
-	if (ret)
-		goto error_cleanup_ring;
 	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto error_unregister_ring;
 
 	return indio_dev;
 error_unregister_ring:
-	iio_buffer_unregister(indio_dev);
-
-error_cleanup_ring:
 	ad7606_ring_cleanup(indio_dev);
 
 error_free_irq:
@@ -571,7 +563,6 @@ int ad7606_remove(struct iio_dev *indio_dev, int irq)
 	struct ad7606_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	iio_buffer_unregister(indio_dev);
 	ad7606_ring_cleanup(indio_dev);
 
 	free_irq(irq, indio_dev);
diff --git a/drivers/staging/iio/adc/ad7606_ring.c b/drivers/staging/iio/adc/ad7606_ring.c
index 24ce8fc7164..f15afe47c20 100644
--- a/drivers/staging/iio/adc/ad7606_ring.c
+++ b/drivers/staging/iio/adc/ad7606_ring.c
@@ -13,8 +13,8 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad7606.h"
 
@@ -91,54 +91,18 @@ done:
 	kfree(buf);
 }
 
-static const struct iio_buffer_setup_ops ad7606_ring_setup_ops = {
-	.preenable = &iio_sw_buffer_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-};
-
 int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
 	struct ad7606_state *st = iio_priv(indio_dev);
-	int ret;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-
-	indio_dev->pollfunc = iio_alloc_pollfunc(&ad7606_trigger_handler_th_bh,
-						 &ad7606_trigger_handler_th_bh,
-						 0,
-						 indio_dev,
-						 "%s_consumer%d",
-						 indio_dev->name,
-						 indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-
-	/* Ring buffer functions - here trigger setup related */
-
-	indio_dev->setup_ops = &ad7606_ring_setup_ops;
-	indio_dev->buffer->scan_timestamp = true;
 
 	INIT_WORK(&st->poll_work, &ad7606_poll_bh_to_ring);
 
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev,
+		&ad7606_trigger_handler_th_bh, &ad7606_trigger_handler_th_bh,
+		NULL);
 }
 
 void ad7606_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
diff --git a/drivers/staging/iio/adc/ad7793.c b/drivers/staging/iio/adc/ad7793.c
index b36556fa295..76fdd7145fc 100644
--- a/drivers/staging/iio/adc/ad7793.c
+++ b/drivers/staging/iio/adc/ad7793.c
@@ -21,9 +21,9 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad7793.h"
 
@@ -52,8 +52,7 @@ struct ad7793_state {
 	u16				mode;
 	u16				conf;
 	u32				scale_avail[8][2];
-	/* Note this uses fact that 8 the mask always fits in a long */
-	unsigned long			available_scan_masks[7];
+
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
 	 * transfer buffers to live in their own cache lines.
@@ -403,45 +402,18 @@ static const struct iio_buffer_setup_ops ad7793_ring_setup_ops = {
 	.postenable = &iio_triggered_buffer_postenable,
 	.predisable = &iio_triggered_buffer_predisable,
 	.postdisable = &ad7793_ring_postdisable,
+	.validate_scan_mask = &iio_validate_scan_mask_onehot,
 };
 
 static int ad7793_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
-	int ret;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
-						 &ad7793_trigger_handler,
-						 IRQF_ONESHOT,
-						 indio_dev,
-						 "ad7793_consumer%d",
-						 indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-
-	/* Ring buffer functions - here trigger setup related */
-	indio_dev->setup_ops = &ad7793_ring_setup_ops;
-
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+			&ad7793_trigger_handler, &ad7793_ring_setup_ops);
 }
 
 static void ad7793_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
 
 /**
@@ -786,7 +758,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.address = AD7793_CH_AIN1M_AIN1M,
 			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 			IIO_CHAN_INFO_SCALE_SHARED_BIT,
-			.scan_index = 2,
+			.scan_index = 3,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
 		.channel[4] = {
@@ -859,7 +831,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.address = AD7793_CH_AIN1M_AIN1M,
 			.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 			IIO_CHAN_INFO_SCALE_SHARED_BIT,
-			.scan_index = 2,
+			.scan_index = 3,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
 		.channel[4] = {
@@ -892,7 +864,7 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 	struct ad7793_platform_data *pdata = spi->dev.platform_data;
 	struct ad7793_state *st;
 	struct iio_dev *indio_dev;
-	int ret, i, voltage_uv = 0;
+	int ret, voltage_uv = 0;
 
 	if (!pdata) {
 		dev_err(&spi->dev, "no platform data?\n");
@@ -938,17 +910,9 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 	indio_dev->name = spi_get_device_id(spi)->name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->channels = st->chip_info->channel;
-	indio_dev->available_scan_masks = st->available_scan_masks;
 	indio_dev->num_channels = 7;
 	indio_dev->info = &ad7793_info;
 
-	for (i = 0; i < indio_dev->num_channels; i++) {
-		set_bit(i, &st->available_scan_masks[i]);
-		set_bit(indio_dev->
-			channels[indio_dev->num_channels - 1].scan_index,
-			&st->available_scan_masks[i]);
-	}
-
 	init_waitqueue_head(&st->wq_data_avail);
 
 	ret = ad7793_register_ring_funcs_and_init(indio_dev);
@@ -959,24 +923,16 @@ static int __devinit ad7793_probe(struct spi_device *spi)
 	if (ret)
 		goto error_unreg_ring;
 
-	ret = iio_buffer_register(indio_dev,
-				  indio_dev->channels,
-				  indio_dev->num_channels);
-	if (ret)
-		goto error_remove_trigger;
-
 	ret = ad7793_setup(st);
 	if (ret)
-		goto error_uninitialize_ring;
+		goto error_remove_trigger;
 
 	ret = iio_device_register(indio_dev);
 	if (ret)
-		goto error_uninitialize_ring;
+		goto error_remove_trigger;
 
 	return 0;
 
-error_uninitialize_ring:
-	iio_buffer_unregister(indio_dev);
 error_remove_trigger:
 	ad7793_remove_trigger(indio_dev);
 error_unreg_ring:
@@ -999,7 +955,6 @@ static int ad7793_remove(struct spi_device *spi)
 	struct ad7793_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	iio_buffer_unregister(indio_dev);
 	ad7793_remove_trigger(indio_dev);
 	ad7793_ring_cleanup(indio_dev);
 
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index 5356b091b08..c5fb9476a2d 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -402,7 +402,7 @@ static int __devinit ad7816_probe(struct spi_device *spi_dev)
 		ret = request_threaded_irq(spi_dev->irq,
 					   NULL,
 					   &ad7816_event_handler,
-					   IRQF_TRIGGER_LOW,
+					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 					   indio_dev->name,
 					   indio_dev);
 		if (ret)
diff --git a/drivers/staging/iio/adc/ad7887_core.c b/drivers/staging/iio/adc/ad7887_core.c
index 7186074deeb..397b8494715 100644
--- a/drivers/staging/iio/adc/ad7887_core.c
+++ b/drivers/staging/iio/adc/ad7887_core.c
@@ -201,20 +201,12 @@ static int __devinit ad7887_probe(struct spi_device *spi)
 	if (ret)
 		goto error_disable_reg;
 
-	ret = iio_buffer_register(indio_dev,
-				  indio_dev->channels,
-				  indio_dev->num_channels);
-	if (ret)
-		goto error_cleanup_ring;
-
 	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto error_unregister_ring;
 
 	return 0;
 error_unregister_ring:
-	iio_buffer_unregister(indio_dev);
-error_cleanup_ring:
 	ad7887_ring_cleanup(indio_dev);
 error_disable_reg:
 	if (!IS_ERR(st->reg))
@@ -233,7 +225,6 @@ static int ad7887_remove(struct spi_device *spi)
 	struct ad7887_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
-	iio_buffer_unregister(indio_dev);
 	ad7887_ring_cleanup(indio_dev);
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
diff --git a/drivers/staging/iio/adc/ad7887_ring.c b/drivers/staging/iio/adc/ad7887_ring.c
index fd91384db89..c76fdb5081c 100644
--- a/drivers/staging/iio/adc/ad7887_ring.c
+++ b/drivers/staging/iio/adc/ad7887_ring.c
@@ -14,8 +14,8 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad7887.h"
 
@@ -82,7 +82,7 @@ static irqreturn_t ad7887_trigger_handler(int irq, void *p)
 
 	buf = kzalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (buf == NULL)
-		return -ENOMEM;
+		goto done;
 
 	b_sent = spi_sync(st->spi, st->ring_msg);
 	if (b_sent)
@@ -112,38 +112,11 @@ static const struct iio_buffer_setup_ops ad7887_ring_setup_ops = {
 
 int ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
-	int ret;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
-						 &ad7887_trigger_handler,
-						 IRQF_ONESHOT,
-						 indio_dev,
-						 "ad7887_consumer%d",
-						 indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-	/* Ring buffer functions - here trigger setup related */
-	indio_dev->setup_ops = &ad7887_ring_setup_ops;
-
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+			&ad7887_trigger_handler, &ad7887_ring_setup_ops);
 }
 
 void ad7887_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
diff --git a/drivers/staging/iio/adc/ad799x.h b/drivers/staging/iio/adc/ad799x.h
index 99f8abe9731..3e363c4ba21 100644
--- a/drivers/staging/iio/adc/ad799x.h
+++ b/drivers/staging/iio/adc/ad799x.h
@@ -120,8 +120,6 @@ struct ad799x_platform_data {
 	u16				vref_mv;
 };
 
-int ad7997_8_set_scan_mode(struct ad799x_state *st, unsigned mask);
-
 #ifdef CONFIG_AD799X_RING_BUFFER
 int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad799x_ring_cleanup(struct iio_dev *indio_dev);
diff --git a/drivers/staging/iio/adc/ad799x_core.c b/drivers/staging/iio/adc/ad799x_core.c
index 80e0c6e25a9..990050700af 100644
--- a/drivers/staging/iio/adc/ad799x_core.c
+++ b/drivers/staging/iio/adc/ad799x_core.c
@@ -99,10 +99,21 @@ static int ad799x_i2c_write8(struct ad799x_state *st, u8 reg, u8 data)
 	return ret;
 }
 
-int ad7997_8_set_scan_mode(struct ad799x_state *st, unsigned mask)
+static int ad7997_8_update_scan_mode(struct iio_dev *indio_dev,
+	const unsigned long *scan_mask)
 {
-	return ad799x_i2c_write16(st, AD7998_CONF_REG,
-		st->config | (mask << AD799X_CHANNEL_SHIFT));
+	struct ad799x_state *st = iio_priv(indio_dev);
+
+	switch (st->id) {
+	case ad7997:
+	case ad7998:
+		return ad799x_i2c_write16(st, AD7998_CONF_REG,
+			st->config | (*scan_mask << AD799X_CHANNEL_SHIFT));
+	default:
+		break;
+	}
+
+	return 0;
 }
 
 static int ad799x_scan_direct(struct ad799x_state *st, unsigned ch)
@@ -339,10 +350,10 @@ static irqreturn_t ad799x_event_handler(int irq, void *private)
 
 	ret = ad799x_i2c_read8(st, AD7998_ALERT_STAT_REG, &status);
 	if (ret)
-		return ret;
+		goto done;
 
 	if (!status)
-		return -EIO;
+		goto done;
 
 	ad799x_i2c_write8(st, AD7998_ALERT_STAT_REG, AD7998_ALERT_STAT_CLEAR);
 
@@ -361,6 +372,7 @@ static irqreturn_t ad799x_event_handler(int irq, void *private)
 				       iio_get_time_ns());
 	}
 
+done:
 	return IRQ_HANDLED;
 }
 
@@ -442,6 +454,7 @@ static const struct iio_info ad7993_4_7_8_info = {
 	.read_event_value = &ad799x_read_event_value,
 	.write_event_value = &ad799x_write_event_value,
 	.driver_module = THIS_MODULE,
+	.update_scan_mode = ad7997_8_update_scan_mode,
 };
 
 #define AD799X_EV_MASK (IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) | \
@@ -887,12 +900,6 @@ static int __devinit ad799x_probe(struct i2c_client *client,
 	if (ret)
 		goto error_disable_reg;
 
-	ret = iio_buffer_register(indio_dev,
-				  indio_dev->channels,
-				  indio_dev->num_channels);
-	if (ret)
-		goto error_cleanup_ring;
-
 	if (client->irq > 0) {
 		ret = request_threaded_irq(client->irq,
 					   NULL,
@@ -934,7 +941,6 @@ static __devexit int ad799x_remove(struct i2c_client *client)
 	if (client->irq > 0)
 		free_irq(client->irq, indio_dev);
 
-	iio_buffer_unregister(indio_dev);
 	ad799x_ring_cleanup(indio_dev);
 	if (!IS_ERR(st->reg)) {
 		regulator_disable(st->reg);
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
index 1c7ff4423db..858a685e388 100644
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ b/drivers/staging/iio/adc/ad799x_ring.c
@@ -18,33 +18,12 @@
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/kfifo_buf.h>
 #include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #include "ad799x.h"
 
 /**
- * ad799x_ring_preenable() setup the parameters of the ring before enabling
- *
- * The complex nature of the setting of the number of bytes per datum is due
- * to this driver currently ensuring that the timestamp is stored at an 8
- * byte boundary.
- **/
-static int ad799x_ring_preenable(struct iio_dev *indio_dev)
-{
-	struct ad799x_state *st = iio_priv(indio_dev);
-	/*
-	 * Need to figure out the current mode based upon the requested
-	 * scan mask in iio_dev
-	 */
-
-	if (st->id == ad7997 || st->id == ad7998)
-		ad7997_8_set_scan_mode(st, *indio_dev->active_scan_mask);
-
-	return iio_sw_buffer_preenable(indio_dev);
-}
-
-/**
  * ad799x_trigger_handler() bh of trigger launched polling to ring buffer
  *
  * Currently there is no option in this driver to disable the saving of
@@ -102,57 +81,19 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	ring->access->store_to(indio_dev->buffer, rxbuf, time_ns);
 done:
 	kfree(rxbuf);
-	if (b_sent < 0)
-		return b_sent;
 out:
 	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
 
-static const struct iio_buffer_setup_ops ad799x_buf_setup_ops = {
-	.preenable = &ad799x_ring_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
-	.predisable = &iio_triggered_buffer_predisable,
-};
-
 int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 {
-	int ret = 0;
-
-	indio_dev->buffer = iio_kfifo_allocate(indio_dev);
-	if (!indio_dev->buffer) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	indio_dev->pollfunc = iio_alloc_pollfunc(NULL,
-						 &ad799x_trigger_handler,
-						 IRQF_ONESHOT,
-						 indio_dev,
-						 "%s_consumer%d",
-						 indio_dev->name,
-						 indio_dev->id);
-	if (indio_dev->pollfunc == NULL) {
-		ret = -ENOMEM;
-		goto error_deallocate_kfifo;
-	}
-
-	/* Ring buffer functions - here trigger setup related */
-	indio_dev->setup_ops = &ad799x_buf_setup_ops;
-	indio_dev->buffer->scan_timestamp = true;
-
-	/* Flag that polled ring buffering is possible */
-	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
-	return 0;
-
-error_deallocate_kfifo:
-	iio_kfifo_free(indio_dev->buffer);
-error_ret:
-	return ret;
+	return iio_triggered_buffer_setup(indio_dev, NULL,
+		&ad799x_trigger_handler, NULL);
 }
 
 void ad799x_ring_cleanup(struct iio_dev *indio_dev)
 {
-	iio_dealloc_pollfunc(indio_dev->pollfunc);
-	iio_kfifo_free(indio_dev->buffer);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
diff --git a/drivers/staging/iio/adc/adt7310.c b/drivers/staging/iio/adc/adt7310.c
index e5f1ed7f869..72460b6dc2f 100644
--- a/drivers/staging/iio/adc/adt7310.c
+++ b/drivers/staging/iio/adc/adt7310.c
@@ -397,7 +397,7 @@ static irqreturn_t adt7310_event_handler(int irq, void *private)
 
 	ret = adt7310_spi_read_byte(chip, ADT7310_STATUS, &status);
 	if (ret)
-		return ret;
+		goto done;
 
 	if (status & ADT7310_STAT_T_HIGH)
 		iio_push_event(indio_dev,
@@ -417,6 +417,8 @@ static irqreturn_t adt7310_event_handler(int irq, void *private)
 						    IIO_EV_TYPE_THRESH,
 						    IIO_EV_DIR_RISING),
 			timestamp);
+
+done:
 	return IRQ_HANDLED;
 }
 
@@ -778,7 +780,7 @@ static int __devinit adt7310_probe(struct spi_device *spi_dev)
 		ret = request_threaded_irq(spi_dev->irq,
 					   NULL,
 					   &adt7310_event_handler,
-					   irq_flags,
+					   irq_flags | IRQF_ONESHOT,
 					   indio_dev->name,
 					   indio_dev);
 		if (ret)
@@ -790,7 +792,8 @@ static int __devinit adt7310_probe(struct spi_device *spi_dev)
 		ret = request_threaded_irq(adt7310_platform_data[0],
 					   NULL,
 					   &adt7310_event_handler,
-					   adt7310_platform_data[1],
+					   adt7310_platform_data[1] |
+					   IRQF_ONESHOT,
 					   indio_dev->name,
 					   indio_dev);
 		if (ret)
diff --git a/drivers/staging/iio/adc/adt7410.c b/drivers/staging/iio/adc/adt7410.c
index 917b6921e24..4157596ea3b 100644
--- a/drivers/staging/iio/adc/adt7410.c
+++ b/drivers/staging/iio/adc/adt7410.c
@@ -257,7 +257,7 @@ static ssize_t adt7410_store_resolution(struct device *dev,
 
 	chip->config = config;
 
-	return ret;
+	return len;
 }
 
 static IIO_DEVICE_ATTR(resolution, S_IRUGO | S_IWUSR,
@@ -293,26 +293,17 @@ static ssize_t adt7410_convert_temperature(struct adt7410_chip_info *chip,
 {
 	char sign = ' ';
 
-	if (chip->config & ADT7410_RESOLUTION) {
-		if (data & ADT7410_T16_VALUE_SIGN) {
-			/* convert supplement to positive value */
-			data = (u16)((ADT7410_T16_VALUE_SIGN << 1) - (u32)data);
-			sign = '-';
-		}
-		return sprintf(buf, "%c%d.%.7d\n", sign,
-				(data >> ADT7410_T16_VALUE_FLOAT_OFFSET),
-				(data & ADT7410_T16_VALUE_FLOAT_MASK) * 78125);
-	} else {
-		if (data & ADT7410_T13_VALUE_SIGN) {
-			/* convert supplement to positive value */
-			data >>= ADT7410_T13_VALUE_OFFSET;
-			data = (ADT7410_T13_VALUE_SIGN << 1) - data;
-			sign = '-';
-		}
-		return sprintf(buf, "%c%d.%.4d\n", sign,
-				(data >> ADT7410_T13_VALUE_FLOAT_OFFSET),
-				(data & ADT7410_T13_VALUE_FLOAT_MASK) * 625);
+	if (!(chip->config & ADT7410_RESOLUTION))
+		data &= ~0x7;
+
+	if (data & ADT7410_T16_VALUE_SIGN) {
+		/* convert supplement to positive value */
+		data = (u16)((ADT7410_T16_VALUE_SIGN << 1) - (u32)data);
+		sign = '-';
 	}
+	return sprintf(buf, "%c%d.%.7d\n", sign,
+			(data >> ADT7410_T16_VALUE_FLOAT_OFFSET),
+			(data & ADT7410_T16_VALUE_FLOAT_MASK) * 78125);
 }
 
 static ssize_t adt7410_show_value(struct device *dev,
@@ -720,6 +711,7 @@ static int __devinit adt7410_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int ret = 0;
 	unsigned long *adt7410_platform_data = client->dev.platform_data;
+	unsigned long local_pdata[] = {0, 0};
 
 	indio_dev = iio_device_alloc(sizeof(*chip));
 	if (indio_dev == NULL) {
@@ -737,12 +729,15 @@ static int __devinit adt7410_probe(struct i2c_client *client,
 	indio_dev->info = &adt7410_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
+	if (!adt7410_platform_data)
+		adt7410_platform_data = local_pdata;
+
 	/* CT critcal temperature event. line 0 */
 	if (client->irq) {
 		ret = request_threaded_irq(client->irq,
 					   NULL,
 					   &adt7410_event_handler,
-					   IRQF_TRIGGER_LOW,
+					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 					   id->name,
 					   indio_dev);
 		if (ret)
@@ -754,20 +749,23 @@ static int __devinit adt7410_probe(struct i2c_client *client,
 		ret = request_threaded_irq(adt7410_platform_data[0],
 					   NULL,
 					   &adt7410_event_handler,
-					   adt7410_platform_data[1],
+					   adt7410_platform_data[1] |
+					   IRQF_ONESHOT,
 					   id->name,
 					   indio_dev);
 		if (ret)
 			goto error_unreg_ct_irq;
 	}
 
-	if (client->irq && adt7410_platform_data[0]) {
+	ret = adt7410_i2c_read_byte(chip, ADT7410_CONFIG, &chip->config);
+	if (ret) {
+		ret = -EIO;
+		goto error_unreg_int_irq;
+	}
 
-		ret = adt7410_i2c_read_byte(chip, ADT7410_CONFIG, &chip->config);
-		if (ret) {
-			ret = -EIO;
-			goto error_unreg_int_irq;
-		}
+	chip->config |= ADT7410_RESOLUTION;
+
+	if (client->irq && adt7410_platform_data[0]) {
 
 		/* set irq polarity low level */
 		chip->config &= ~ADT7410_CT_POLARITY;
@@ -776,12 +774,12 @@ static int __devinit adt7410_probe(struct i2c_client *client,
 			chip->config |= ADT7410_INT_POLARITY;
 		else
 			chip->config &= ~ADT7410_INT_POLARITY;
+	}
 
-		ret = adt7410_i2c_write_byte(chip, ADT7410_CONFIG, chip->config);
-		if (ret) {
-			ret = -EIO;
-			goto error_unreg_int_irq;
-		}
+	ret = adt7410_i2c_write_byte(chip, ADT7410_CONFIG, chip->config);
+	if (ret) {
+		ret = -EIO;
+		goto error_unreg_int_irq;
 	}
 	ret = iio_device_register(indio_dev);
 	if (ret)
diff --git a/drivers/staging/iio/adc/lpc32xx_adc.c b/drivers/staging/iio/adc/lpc32xx_adc.c
index 9690306d1f8..348d051fc2f 100644
--- a/drivers/staging/iio/adc/lpc32xx_adc.c
+++ b/drivers/staging/iio/adc/lpc32xx_adc.c
@@ -196,7 +196,7 @@ static int __devinit lpc32xx_adc_probe(struct platform_device *pdev)
 	return 0;
 
 errout5:
-	free_irq(irq, iodev);
+	free_irq(irq, info);
 errout4:
 	clk_put(info->clk);
 errout3:
@@ -214,7 +214,7 @@ static int __devexit lpc32xx_adc_remove(struct platform_device *pdev)
 	int irq = platform_get_irq(pdev, 0);
 
 	iio_device_unregister(iodev);
-	free_irq(irq, iodev);
+	free_irq(irq, info);
 	platform_set_drvdata(pdev, NULL);
 	clk_put(info->clk);
 	iounmap(info->adc_base);
diff --git a/drivers/staging/iio/adc/max1363_ring.c b/drivers/staging/iio/adc/max1363_ring.c
index b3020130012..774ae1b6355 100644
--- a/drivers/staging/iio/adc/max1363_ring.c
+++ b/drivers/staging/iio/adc/max1363_ring.c
@@ -64,17 +64,17 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 	 * no harm.
 	 */
 	if (numvals == 0)
-		return IRQ_HANDLED;
+		goto done;
 
 	rxbuf = kmalloc(d_size,	GFP_KERNEL);
 	if (rxbuf == NULL)
-		return -ENOMEM;
+		goto done;
 	if (st->chip_info->bits != 8)
 		b_sent = i2c_master_recv(st->client, rxbuf, numvals*2);
 	else
 		b_sent = i2c_master_recv(st->client, rxbuf, numvals);
 	if (b_sent < 0)
-		goto done;
+		goto done_free;
 
 	time_ns = iio_get_time_ns();
 
@@ -82,9 +82,10 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 		memcpy(rxbuf + d_size - sizeof(s64), &time_ns, sizeof(time_ns));
 	iio_push_to_buffer(indio_dev->buffer, rxbuf, time_ns);
 
+done_free:
+	kfree(rxbuf);
 done:
 	iio_trigger_notify_done(indio_dev->trig);
-	kfree(rxbuf);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/cdc/ad7150.c b/drivers/staging/iio/cdc/ad7150.c
index a16d1a22db0..6a4041417d4 100644
--- a/drivers/staging/iio/cdc/ad7150.c
+++ b/drivers/staging/iio/cdc/ad7150.c
@@ -585,7 +585,8 @@ static int __devinit ad7150_probe(struct i2c_client *client,
 					   NULL,
 					   &ad7150_event_handler,
 					   IRQF_TRIGGER_RISING |
-					   IRQF_TRIGGER_FALLING,
+					   IRQF_TRIGGER_FALLING |
+					   IRQF_ONESHOT,
 					   "ad7150_irq1",
 					   indio_dev);
 		if (ret)
@@ -598,7 +599,8 @@ static int __devinit ad7150_probe(struct i2c_client *client,
 					   NULL,
 					   &ad7150_event_handler,
 					   IRQF_TRIGGER_RISING |
-					   IRQF_TRIGGER_FALLING,
+					   IRQF_TRIGGER_FALLING |
+					   IRQF_ONESHOT,
 					   "ad7150_irq2",
 					   indio_dev);
 		if (ret)
diff --git a/drivers/staging/iio/dac/ad5421.h b/drivers/staging/iio/dac/ad5421.h
deleted file mode 100644
index cd2bb84ff1b..00000000000
--- a/drivers/staging/iio/dac/ad5421.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef __IIO_DAC_AD5421_H__
-#define __IIO_DAC_AD5421_H__
-
-/*
- * TODO: This file needs to go into include/linux/iio
- */
-
-/**
- * enum ad5421_current_range - Current range the AD5421 is configured for.
- * @AD5421_CURRENT_RANGE_4mA_20mA: 4 mA to 20 mA (RANGE1,0 pins = 00)
- * @AD5421_CURRENT_RANGE_3mA8_21mA: 3.8 mA to 21 mA (RANGE1,0 pins = x1)
- * @AD5421_CURRENT_RANGE_3mA2_24mA: 3.2 mA to 24 mA (RANGE1,0 pins = 10)
- */
-
-enum ad5421_current_range {
-	AD5421_CURRENT_RANGE_4mA_20mA,
-	AD5421_CURRENT_RANGE_3mA8_21mA,
-	AD5421_CURRENT_RANGE_3mA2_24mA,
-};
-
-/**
- * struct ad5421_platform_data - AD5421 DAC driver platform data
- * @external_vref: whether an external reference voltage is used or not
- * @current_range: Current range the AD5421 is configured for
- */
-
-struct ad5421_platform_data {
-	bool external_vref;
-	enum ad5421_current_range current_range;
-};
-
-#endif
diff --git a/drivers/staging/iio/dac/ad5504.h b/drivers/staging/iio/dac/ad5504.h
deleted file mode 100644
index afe09522f53..00000000000
--- a/drivers/staging/iio/dac/ad5504.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * AD5504 SPI DAC driver
- *
- * Copyright 2011 Analog Devices Inc.
- *
- * Licensed under the GPL-2.
- */
-
-#ifndef SPI_AD5504_H_
-#define SPI_AD5504_H_
-
-#define AD5505_BITS			12
-#define AD5504_RES_MASK			((1 << (AD5505_BITS)) - 1)
-
-#define AD5504_CMD_READ			(1 << 15)
-#define AD5504_CMD_WRITE		(0 << 15)
-#define AD5504_ADDR(addr)		((addr) << 12)
-
-/* Registers */
-#define AD5504_ADDR_NOOP		0
-#define AD5504_ADDR_DAC(x)		((x) + 1)
-#define AD5504_ADDR_ALL_DAC		5
-#define AD5504_ADDR_CTRL		7
-
-/* Control Register */
-#define AD5504_DAC_PWR(ch)		((ch) << 2)
-#define AD5504_DAC_PWRDWN_MODE(mode)	((mode) << 6)
-#define AD5504_DAC_PWRDN_20K		0
-#define AD5504_DAC_PWRDN_3STATE		1
-
-/*
- * TODO: struct ad5504_platform_data needs to go into include/linux/iio
- */
-
-struct ad5504_platform_data {
-	u16				vref_mv;
-};
-
-/**
- * struct ad5446_state - driver instance specific data
- * @us:			spi_device
- * @reg:		supply regulator
- * @vref_mv:		actual reference voltage used
- * @pwr_down_mask	power down mask
- * @pwr_down_mode	current power down mode
- */
-
-struct ad5504_state {
-	struct spi_device		*spi;
-	struct regulator		*reg;
-	unsigned short			vref_mv;
-	unsigned			pwr_down_mask;
-	unsigned			pwr_down_mode;
-};
-
-/**
- * ad5504_supported_device_ids:
- */
-
-enum ad5504_supported_device_ids {
-	ID_AD5504,
-	ID_AD5501,
-};
-
-#endif /* SPI_AD5504_H_ */
diff --git a/drivers/staging/iio/dac/ad5791.h b/drivers/staging/iio/dac/ad5791.h
deleted file mode 100644
index fd7edbdb4ec..00000000000
--- a/drivers/staging/iio/dac/ad5791.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * AD5791 SPI DAC driver
- *
- * Copyright 2011 Analog Devices Inc.
- *
- * Licensed under the GPL-2.
- */
-
-#ifndef SPI_AD5791_H_
-#define SPI_AD5791_H_
-
-#define AD5791_RES_MASK(x)		((1 << (x)) - 1)
-#define AD5791_DAC_MASK			AD5791_RES_MASK(20)
-#define AD5791_DAC_MSB			(1 << 19)
-
-#define AD5791_CMD_READ			(1 << 23)
-#define AD5791_CMD_WRITE		(0 << 23)
-#define AD5791_ADDR(addr)		((addr) << 20)
-
-/* Registers */
-#define AD5791_ADDR_NOOP		0
-#define AD5791_ADDR_DAC0		1
-#define AD5791_ADDR_CTRL		2
-#define AD5791_ADDR_CLRCODE		3
-#define AD5791_ADDR_SW_CTRL		4
-
-/* Control Register */
-#define AD5791_CTRL_RBUF		(1 << 1)
-#define AD5791_CTRL_OPGND		(1 << 2)
-#define AD5791_CTRL_DACTRI		(1 << 3)
-#define AD5791_CTRL_BIN2SC		(1 << 4)
-#define AD5791_CTRL_SDODIS		(1 << 5)
-#define AD5761_CTRL_LINCOMP(x)		((x) << 6)
-
-#define AD5791_LINCOMP_0_10		0
-#define AD5791_LINCOMP_10_12		1
-#define AD5791_LINCOMP_12_16		2
-#define AD5791_LINCOMP_16_19		3
-#define AD5791_LINCOMP_19_20		12
-
-#define AD5780_LINCOMP_0_10		0
-#define AD5780_LINCOMP_10_20		12
-
-/* Software Control Register */
-#define AD5791_SWCTRL_LDAC		(1 << 0)
-#define AD5791_SWCTRL_CLR		(1 << 1)
-#define AD5791_SWCTRL_RESET		(1 << 2)
-
-#define AD5791_DAC_PWRDN_6K		0
-#define AD5791_DAC_PWRDN_3STATE		1
-
-/*
- * TODO: struct ad5791_platform_data needs to go into include/linux/iio
- */
-
-/**
- * struct ad5791_platform_data - platform specific information
- * @vref_pos_mv:	Vdd Positive Analog Supply Volatge (mV)
- * @vref_neg_mv:	Vdd Negative Analog Supply Volatge (mV)
- * @use_rbuf_gain2:	ext. amplifier connected in gain of two configuration
- */
-
-struct ad5791_platform_data {
-	u16				vref_pos_mv;
-	u16				vref_neg_mv;
-	bool				use_rbuf_gain2;
-};
-
-/**
- * struct ad5791_chip_info - chip specific information
- * @get_lin_comp:	function pointer to the device specific function
- */
-
-struct ad5791_chip_info {
-	int (*get_lin_comp)	(unsigned int span);
-};
-
-/**
- * struct ad5791_state - driver instance specific data
- * @us:			spi_device
- * @reg_vdd:		positive supply regulator
- * @reg_vss:		negative supply regulator
- * @chip_info:		chip model specific constants
- * @vref_mv:		actual reference voltage used
- * @vref_neg_mv:	voltage of the negative supply
- * @pwr_down_mode	current power down mode
- */
-
-struct ad5791_state {
-	struct spi_device		*spi;
-	struct regulator		*reg_vdd;
-	struct regulator		*reg_vss;
-	const struct ad5791_chip_info	*chip_info;
-	unsigned short			vref_mv;
-	unsigned int			vref_neg_mv;
-	unsigned			ctrl;
-	unsigned			pwr_down_mode;
-	bool				pwr_down;
-};
-
-/**
- * ad5791_supported_device_ids:
- */
-
-enum ad5791_supported_device_ids {
-	ID_AD5760,
-	ID_AD5780,
-	ID_AD5781,
-	ID_AD5791,
-};
-
-#endif /* SPI_AD5791_H_ */
diff --git a/drivers/staging/iio/dac/dac.h b/drivers/staging/iio/dac/dac.h
deleted file mode 100644
index 0754d715cf9..00000000000
--- a/drivers/staging/iio/dac/dac.h
+++ /dev/null
@@ -1,6 +0,0 @@
-/*
- * dac.h - sysfs attributes associated with DACs
- */
-
-#define IIO_DEV_ATTR_OUT_RAW(_num, _store, _addr)				\
-	IIO_DEVICE_ATTR(out_voltage##_num##_raw, S_IWUSR, NULL, _store, _addr)
diff --git a/drivers/staging/iio/dac/max517.h b/drivers/staging/iio/dac/max517.h
deleted file mode 100644
index 8106cf24642..00000000000
--- a/drivers/staging/iio/dac/max517.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * MAX517 DAC driver
- *
- * Copyright 2011 Roland Stigge <stigge@antcom.de>
- *
- * Licensed under the GPL-2 or later.
- */
-#ifndef IIO_DAC_MAX517_H_
-#define IIO_DAC_MAX517_H_
-
-/*
- * TODO: struct max517_platform_data needs to go into include/linux/iio
- */
-
-struct max517_platform_data {
-	u16				vref_mv[2];
-};
-
-#endif /* IIO_DAC_MAX517_H_ */
diff --git a/drivers/staging/iio/gyro/adis16080_core.c b/drivers/staging/iio/gyro/adis16080_core.c
index 11f1dccd7a0..345e4fa778b 100644
--- a/drivers/staging/iio/gyro/adis16080_core.c
+++ b/drivers/staging/iio/gyro/adis16080_core.c
@@ -82,7 +82,7 @@ static int adis16080_read_raw(struct iio_dev *indio_dev,
 			     long mask)
 {
 	int ret = -EINVAL;
-	u16 ut;
+	u16 ut = 0;
 	/* Take the iio_dev status lock */
 
 	mutex_lock(&indio_dev->mlock);
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index ec765f955f8..93aa431287a 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -233,22 +233,6 @@ static int adis16260_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16260_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	if (len < 1)
-		return -EINVAL;
-	switch (buf[0]) {
-	case '1':
-	case 'y':
-	case 'Y':
-		return adis16260_reset(indio_dev);
-	}
-	return -EINVAL;
-}
-
 int adis16260_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret;
@@ -375,8 +359,6 @@ static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
 		adis16260_read_frequency,
 		adis16260_write_frequency);
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16260_write_reset, 0);
-
 static IIO_DEVICE_ATTR(sampling_frequency_available,
 		       S_IRUGO, adis16260_read_frequency_available, NULL, 0);
 
@@ -604,7 +586,6 @@ static int adis16260_write_raw(struct iio_dev *indio_dev,
 static struct attribute *adis16260_attributes[] = {
 	&iio_dev_attr_sampling_frequency.dev_attr.attr,
 	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
-	&iio_dev_attr_reset.dev_attr.attr,
 	NULL
 };
 
@@ -636,7 +617,7 @@ static int __devinit adis16260_probe(struct spi_device *spi)
 	if (pd)
 		st->negate = pd->negate;
 	/* this is only used for removal purposes */
-	spi_set_drvdata(spi, st);
+	spi_set_drvdata(spi, indio_dev);
 
 	st->us = spi;
 	mutex_init(&st->buf_lock);
@@ -728,8 +709,6 @@ static int adis16260_remove(struct spi_device *spi)
 	if (ret)
 		goto err_ret;
 
-	flush_scheduled_work();
-
 	adis16260_remove_trigger(indio_dev);
 	iio_buffer_unregister(indio_dev);
 	adis16260_unconfigure_ring(indio_dev);
diff --git a/drivers/staging/iio/gyro/adis16260_ring.c b/drivers/staging/iio/gyro/adis16260_ring.c
index 0fe2d9dfb6c..eeee8e760e6 100644
--- a/drivers/staging/iio/gyro/adis16260_ring.c
+++ b/drivers/staging/iio/gyro/adis16260_ring.c
@@ -69,7 +69,7 @@ static irqreturn_t adis16260_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
@@ -84,8 +84,9 @@ static irqreturn_t adis16260_trigger_handler(int irq, void *p)
 
 	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
 
-	iio_trigger_notify_done(indio_dev->trig);
 	kfree(data);
+done:
+	iio_trigger_notify_done(indio_dev->trig);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/staging/iio/iio_hwmon.c b/drivers/staging/iio/iio_hwmon.c
index b03554fee44..27d27ec9521 100644
--- a/drivers/staging/iio/iio_hwmon.c
+++ b/drivers/staging/iio/iio_hwmon.c
@@ -51,12 +51,12 @@ static ssize_t iio_hwmon_read_val(struct device *dev,
 	 * No locking between this pair, so theoretically possible
 	 * the scale has changed.
 	 */
-	ret = iio_st_read_channel_raw(&state->channels[sattr->index],
+	ret = iio_read_channel_raw(&state->channels[sattr->index],
 				      &val);
 	if (ret < 0)
 		return ret;
 
-	ret = iio_st_read_channel_scale(&state->channels[sattr->index],
+	ret = iio_read_channel_scale(&state->channels[sattr->index],
 					&scaleint, &scalepart);
 	if (ret < 0)
 		return ret;
@@ -106,7 +106,7 @@ static int __devinit iio_hwmon_probe(struct platform_device *pdev)
 		goto error_ret;
 	}
 
-	st->channels = iio_st_channel_get_all(dev_name(&pdev->dev));
+	st->channels = iio_channel_get_all(dev_name(&pdev->dev));
 	if (IS_ERR(st->channels)) {
 		ret = PTR_ERR(st->channels);
 		goto error_free_state;
@@ -130,7 +130,7 @@ static int __devinit iio_hwmon_probe(struct platform_device *pdev)
 		}
 
 		sysfs_attr_init(&a->dev_attr.attr);
-		ret = iio_st_get_channel_type(&st->channels[i], &type);
+		ret = iio_get_channel_type(&st->channels[i], &type);
 		if (ret < 0) {
 			kfree(a);
 			goto error_free_attrs;
@@ -186,7 +186,7 @@ error_free_attrs:
 	iio_hwmon_free_attrs(st);
 	kfree(st->attrs);
 error_release_channels:
-	iio_st_channel_release_all(st->channels);
+	iio_channel_release_all(st->channels);
 error_free_state:
 	kfree(st);
 error_ret:
@@ -201,7 +201,7 @@ static int __devexit iio_hwmon_remove(struct platform_device *pdev)
 	sysfs_remove_group(&pdev->dev.kobj, &st->attr_group);
 	iio_hwmon_free_attrs(st);
 	kfree(st->attrs);
-	iio_st_channel_release_all(st->channels);
+	iio_channel_release_all(st->channels);
 
 	return 0;
 }
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index 310411911ed..155a49a9da7 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -27,7 +27,7 @@
 
 /*
  * A few elements needed to fake a bus for this driver
- * Note instances parmeter controls how many of these
+ * Note instances parameter controls how many of these
  * dummy devices are registered.
  */
 static unsigned instances = 1;
@@ -178,7 +178,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		.scan_index = accelx,
 		.scan_type = { /* Description of storage in buffer */
 			.sign = 's', /* signed */
-			.realbits = 16, /* 12 bits */
+			.realbits = 16, /* 16 bits */
 			.storagebits = 16, /* 16 bits used for storage */
 			.shift = 0, /* zero shift */
 		},
@@ -285,9 +285,9 @@ static int iio_dummy_read_raw(struct iio_dev *indio_dev,
  * iio_dummy_write_raw() - data write function.
  * @indio_dev:	the struct iio_dev associated with this device instance
  * @chan:	the channel whose data is to be read
- * @val:	first element of returned value (typically INT)
- * @val2:	second element of returned value (typically MICRO)
- * @mask:	what we actually want to read. 0 is the channel, everything else
+ * @val:	first element of value to set (typically INT)
+ * @val2:	second element of value to set (typically MICRO)
+ * @mask:	what we actually want to write. 0 is the channel, everything else
  *		is as per the info_mask in iio_chan_spec.
  *
  * Note that all raw writes are assumed IIO_VAL_INT and info mask elements
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/staging/iio/iio_simple_dummy_buffer.c
index fdfc8739095..bd628de472a 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/staging/iio/iio_simple_dummy_buffer.c
@@ -52,7 +52,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL)
-		return -ENOMEM;
+		goto done;
 
 	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
 		/*
@@ -67,31 +67,31 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 		 * software culled hardware scans:
 		 *   occasionally a driver may process the nearest hardware
 		 *   scan to avoid storing elements that are not desired. This
-		 *   is the fidliest option by far.
-		 * Here lets pretend we have random access. And the values are
+		 *   is the fiddliest option by far.
+		 * Here let's pretend we have random access. And the values are
 		 * in the constant table fakedata.
 		 */
 		int i, j;
 		for (i = 0, j = 0;
 		     i < bitmap_weight(indio_dev->active_scan_mask,
 				       indio_dev->masklength);
-		     i++) {
+		     i++, j++) {
 			j = find_next_bit(buffer->scan_mask,
-					  indio_dev->masklength, j + 1);
-			/* random access read form the 'device' */
+					  indio_dev->masklength, j);
+			/* random access read from the 'device' */
 			data[i] = fakedata[j];
 			len += 2;
 		}
 	}
-	/* Store a timestampe at an 8 byte boundary */
+	/* Store the timestamp at an 8 byte aligned offset */
 	if (indio_dev->scan_timestamp)
-		*(s64 *)(((phys_addr_t)data + len
-				+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
+		*(s64 *)((u8 *)data + ALIGN(len, sizeof(s64)))
 			= iio_get_time_ns();
 	buffer->access->store_to(buffer, (u8 *)data, pf->timestamp);
 
 	kfree(data);
 
+done:
 	/*
 	 * Tell the core we are done with this trigger and ready for the
 	 * next one.
diff --git a/drivers/staging/iio/imu/adis16400.h b/drivers/staging/iio/imu/adis16400.h
index 83d133efaac..9dd9f1459a4 100644
--- a/drivers/staging/iio/imu/adis16400.h
+++ b/drivers/staging/iio/imu/adis16400.h
@@ -42,7 +42,8 @@
 #define ADIS16350_ZTEMP_OUT 0x14 /* Z-axis gyroscope temperature measurement */
 
 #define ADIS16300_PITCH_OUT 0x12 /* X axis inclinometer output measurement */
-#define ADIS16300_ROLL_OUT  0x12 /* Y axis inclinometer output measurement */
+#define ADIS16300_ROLL_OUT  0x14 /* Y axis inclinometer output measurement */
+#define ADIS16300_AUX_ADC   0x16 /* Auxiliary ADC measurement */
 
 /* Calibration parameters */
 #define ADIS16400_XGYRO_OFF 0x1A /* X-axis gyroscope bias offset factor */
diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c
index 1f6bd854e95..1f4c17779b5 100644
--- a/drivers/staging/iio/imu/adis16400_core.c
+++ b/drivers/staging/iio/imu/adis16400_core.c
@@ -268,25 +268,6 @@ static int adis16400_reset(struct iio_dev *indio_dev)
 	return ret;
 }
 
-static ssize_t adis16400_write_reset(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf, size_t len)
-{
-	bool val;
-	int ret;
-
-	ret = strtobool(buf, &val);
-	if (ret < 0)
-		return ret;
-	if (val) {
-		ret = adis16400_reset(dev_to_iio_dev(dev));
-		if (ret < 0)
-			return ret;
-	}
-
-	return len;
-}
-
 int adis16400_set_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int ret;
@@ -454,8 +435,6 @@ static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
 			      adis16400_read_frequency,
 			      adis16400_write_frequency);
 
-static IIO_DEVICE_ATTR(reset, S_IWUSR, NULL, adis16400_write_reset, 0);
-
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("409 546 819 1638");
 
 enum adis16400_chan {
@@ -472,11 +451,12 @@ enum adis16400_chan {
 	temp,
 	temp0, temp1, temp2,
 	in1,
+	in2,
 	incli_x,
 	incli_y,
 };
 
-static u8 adis16400_addresses[17][2] = {
+static u8 adis16400_addresses[18][2] = {
 	[in_supply] = { ADIS16400_SUPPLY_OUT },
 	[gyro_x] = { ADIS16400_XGYRO_OUT, ADIS16400_XGYRO_OFF },
 	[gyro_y] = { ADIS16400_YGYRO_OUT, ADIS16400_YGYRO_OFF },
@@ -491,7 +471,8 @@ static u8 adis16400_addresses[17][2] = {
 	[temp0] = { ADIS16350_XTEMP_OUT },
 	[temp1] = { ADIS16350_YTEMP_OUT },
 	[temp2] = { ADIS16350_ZTEMP_OUT },
-	[in1] = { ADIS16400_AUX_ADC },
+	[in1] = { ADIS16300_AUX_ADC },
+	[in2] = { ADIS16400_AUX_ADC },
 	[incli_x] = { ADIS16300_PITCH_OUT },
 	[incli_y] = { ADIS16300_ROLL_OUT }
 };
@@ -752,7 +733,7 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.channel = 1,
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		.address = in1,
+		.address = in2,
 		.scan_index = ADIS16400_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
 	},
@@ -946,7 +927,7 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
-		.address = temp,
+		.address = temp0,
 		.scan_index = ADIS16400_SCAN_TEMP,
 		.scan_type = IIO_ST('s', 12, 16, 0),
 	}, {
@@ -1054,8 +1035,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.info_mask = IIO_CHAN_INFO_RAW_SEPARATE_BIT |
 		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
 		IIO_CHAN_INFO_SCALE_SHARED_BIT,
-		.address = accel_z,
-		.scan_index = ADIS16400_SCAN_ACC_Z,
+		.address = temp0,
+		.scan_index = ADIS16400_SCAN_TEMP,
 		.scan_type = IIO_ST('s', 14, 16, 0),
 	},
 	IIO_CHAN_SOFT_TIMESTAMP(12)
@@ -1064,7 +1045,6 @@ static const struct iio_chan_spec adis16334_channels[] = {
 static struct attribute *adis16400_attributes[] = {
 	&iio_dev_attr_sampling_frequency.dev_attr.attr,
 	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
-	&iio_dev_attr_reset.dev_attr.attr,
 	NULL
 };
 
diff --git a/drivers/staging/iio/imu/adis16400_ring.c b/drivers/staging/iio/imu/adis16400_ring.c
index 809e2c4270d..beec650ddbd 100644
--- a/drivers/staging/iio/imu/adis16400_ring.c
+++ b/drivers/staging/iio/imu/adis16400_ring.c
@@ -125,20 +125,20 @@ static irqreturn_t adis16400_trigger_handler(int irq, void *p)
 	data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
-		return -ENOMEM;
+		goto done;
 	}
 
 	if (scan_count) {
 		if (st->variant->flags & ADIS16400_NO_BURST) {
 			ret = adis16350_spi_read_all(indio_dev, st->rx);
 			if (ret < 0)
-				goto err;
+				goto done;
 			for (; i < scan_count; i++)
 				data[i]	= *(s16 *)(st->rx + i*2);
 		} else {
 			ret = adis16400_spi_read_burst(indio_dev, st->rx);
 			if (ret < 0)
-				goto err;
+				goto done;
 			for (; i < scan_count; i++) {
 				j = __ffs(mask);
 				mask &= ~(1 << j);
@@ -152,14 +152,11 @@ static irqreturn_t adis16400_trigger_handler(int irq, void *p)
 		*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
 	ring->access->store_to(indio_dev->buffer, (u8 *) data, pf->timestamp);
 
+done:
+	kfree(data);
 	iio_trigger_notify_done(indio_dev->trig);
 
-	kfree(data);
 	return IRQ_HANDLED;
-
-err:
-	kfree(data);
-	return ret;
 }
 
 void adis16400_unconfigure_ring(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index 0abbf18d610..31d22f5591c 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -292,18 +292,18 @@ static ssize_t store_resolution(struct device *dev,
 }
 
 /* proximity scheme */
-static ssize_t show_prox_infrared_supression(struct device *dev,
+static ssize_t show_prox_infrared_suppression(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
 
 	/* return the "proximity scheme" i.e. if the chip does on chip
-	infrared supression (1 means perform on chip supression) */
+	infrared suppression (1 means perform on chip suppression) */
 	return sprintf(buf, "%d\n", chip->prox_scheme);
 }
 
-static ssize_t store_prox_infrared_supression(struct device *dev,
+static ssize_t store_prox_infrared_suppression(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -318,7 +318,7 @@ static ssize_t store_prox_infrared_supression(struct device *dev,
 	}
 
 	/* get the  "proximity scheme" i.e. if the chip does on chip
-	infrared supression (1 means perform on chip supression) */
+	infrared suppression (1 means perform on chip suppression) */
 	mutex_lock(&chip->lock);
 	chip->prox_scheme = (int)lval;
 	mutex_unlock(&chip->lock);
@@ -413,10 +413,10 @@ static IIO_CONST_ATTR(range_available, "1000 4000 16000 64000");
 static IIO_CONST_ATTR(adc_resolution_available, "4 8 12 16");
 static IIO_DEVICE_ATTR(adc_resolution, S_IRUGO | S_IWUSR,
 					show_resolution, store_resolution, 0);
-static IIO_DEVICE_ATTR(proximity_on_chip_ambient_infrared_supression,
+static IIO_DEVICE_ATTR(proximity_on_chip_ambient_infrared_suppression,
 					S_IRUGO | S_IWUSR,
-					show_prox_infrared_supression,
-					store_prox_infrared_supression, 0);
+					show_prox_infrared_suppression,
+					store_prox_infrared_suppression, 0);
 
 #define ISL29018_DEV_ATTR(name) (&iio_dev_attr_##name.dev_attr.attr)
 #define ISL29018_CONST_ATTR(name) (&iio_const_attr_##name.dev_attr.attr)
@@ -425,7 +425,7 @@ static struct attribute *isl29018_attributes[] = {
 	ISL29018_CONST_ATTR(range_available),
 	ISL29018_DEV_ATTR(adc_resolution),
 	ISL29018_CONST_ATTR(adc_resolution_available),
-	ISL29018_DEV_ATTR(proximity_on_chip_ambient_infrared_supression),
+	ISL29018_DEV_ATTR(proximity_on_chip_ambient_infrared_suppression),
 	NULL
 };
 
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index 5e23ad5a30d..6d2f4c659e5 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -410,7 +410,7 @@ static int taos_chip_on(struct iio_dev *indio_dev)
 		return -EINVAL;
 	}
 
-	/* determine als integration regster */
+	/* determine als integration register */
 	als_count = (chip->taos_settings.als_time * 100 + 135) / 270;
 	if (als_count == 0)
 		als_count = 1; /* ensure at least one cycle */
diff --git a/drivers/staging/iio/light/tsl2x7x.h b/drivers/staging/iio/light/tsl2x7x.h
index c4acf5ff179..c4acf5ff179 100755..100644
--- a/drivers/staging/iio/light/tsl2x7x.h
+++ b/drivers/staging/iio/light/tsl2x7x.h
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index c3b05a1b3ea..497a977ae41 100755..100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -27,7 +27,6 @@
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/iio/events.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -737,7 +736,7 @@ static int tsl2x7x_chip_on(struct iio_dev *indio_dev)
 		return -EINVAL;
 	}
 
-	/* determine als integration regster */
+	/* determine als integration register */
 	als_count = (chip->tsl2x7x_settings.als_time * 100 + 135) / 270;
 	if (als_count == 0)
 		als_count = 1; /* ensure at least one cycle */
@@ -2029,14 +2028,13 @@ static int tsl2x7x_resume(struct device *dev)
 
 static int __devexit tsl2x7x_remove(struct i2c_client *client)
 {
-	struct tsl2X7X_chip *chip = i2c_get_clientdata(client);
-	struct iio_dev *indio_dev = iio_priv_to_dev(chip);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 
 	tsl2x7x_chip_off(indio_dev);
 
 	iio_device_unregister(indio_dev);
 	if (client->irq)
-		free_irq(client->irq, chip->client->name);
+		free_irq(client->irq, indio_dev);
 
 	iio_device_free(indio_dev);
 
diff --git a/drivers/staging/iio/magnetometer/Kconfig b/drivers/staging/iio/magnetometer/Kconfig
index b9d932595ba..df5e0d4ea29 100644
--- a/drivers/staging/iio/magnetometer/Kconfig
+++ b/drivers/staging/iio/magnetometer/Kconfig
@@ -6,7 +6,7 @@ menu "Magnetometer sensors"
 config SENSORS_AK8975
 	tristate "Asahi Kasei AK8975 3-Axis Magnetometer"
 	depends on I2C
-	depends on GENERIC_GPIO
+	depends on GPIOLIB
 	help
 	  Say yes here to build support for Asahi Kasei AK8975 3-Axis
 	  Magnetometer.
diff --git a/drivers/staging/iio/magnetometer/ak8975.c b/drivers/staging/iio/magnetometer/ak8975.c
index 5834e4a70f8..01b4b07c227 100644
--- a/drivers/staging/iio/magnetometer/ak8975.c
+++ b/drivers/staging/iio/magnetometer/ak8975.c
@@ -92,7 +92,6 @@ struct ak8975_data {
 	struct mutex		lock;
 	u8			asa[3];
 	long			raw_to_gauss[3];
-	bool			mode;
 	u8			reg_cache[AK8975_MAX_REGS];
 	int			eoc_gpio;
 	int			eoc_irq;
@@ -194,6 +193,17 @@ static int ak8975_setup(struct i2c_client *client)
 		return ret;
 	}
 
+	/* After reading fuse ROM data set power-down mode */
+	ret = ak8975_write_data(client,
+				AK8975_REG_CNTL,
+				AK8975_REG_CNTL_MODE_POWER_DOWN,
+				AK8975_REG_CNTL_MODE_MASK,
+				AK8975_REG_CNTL_MODE_SHIFT);
+	if (ret < 0) {
+		dev_err(&client->dev, "Error in setting power-down mode\n");
+		return ret;
+	}
+
 /*
  * Precalculate scale factor (in Gauss units) for each axis and
  * store in the device data.
@@ -236,60 +246,6 @@ static int ak8975_setup(struct i2c_client *client)
 	return 0;
 }
 
-/*
- * Shows the device's mode.  0 = off, 1 = on.
- */
-static ssize_t show_mode(struct device *dev, struct device_attribute *devattr,
-			 char *buf)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct ak8975_data *data = iio_priv(indio_dev);
-
-	return sprintf(buf, "%u\n", data->mode);
-}
-
-/*
- * Sets the device's mode.  0 = off, 1 = on.  The device's mode must be on
- * for the magn raw attributes to be available.
- */
-static ssize_t store_mode(struct device *dev, struct device_attribute *devattr,
-			  const char *buf, size_t count)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct ak8975_data *data = iio_priv(indio_dev);
-	struct i2c_client *client = data->client;
-	bool value;
-	int ret;
-
-	/* Convert mode string and do some basic sanity checking on it.
-	   only 0 or 1 are valid. */
-	ret = strtobool(buf, &value);
-	if (ret < 0)
-		return ret;
-
-	mutex_lock(&data->lock);
-
-	/* Write the mode to the device. */
-	if (data->mode != value) {
-		ret = ak8975_write_data(client,
-					AK8975_REG_CNTL,
-					(u8)value,
-					AK8975_REG_CNTL_MODE_MASK,
-					AK8975_REG_CNTL_MODE_SHIFT);
-
-		if (ret < 0) {
-			dev_err(&client->dev, "Error in setting mode\n");
-			mutex_unlock(&data->lock);
-			return ret;
-		}
-		data->mode = value;
-	}
-
-	mutex_unlock(&data->lock);
-
-	return count;
-}
-
 static int wait_conversion_complete_gpio(struct ak8975_data *data)
 {
 	struct i2c_client *client = data->client;
@@ -357,12 +313,6 @@ static int ak8975_read_axis(struct iio_dev *indio_dev, int index, int *val)
 
 	mutex_lock(&data->lock);
 
-	if (data->mode == 0) {
-		dev_err(&client->dev, "Operating mode is in power down mode\n");
-		ret = -EBUSY;
-		goto exit;
-	}
-
 	/* Set up the device for taking a sample. */
 	ret = ak8975_write_data(client,
 				AK8975_REG_CNTL,
@@ -454,24 +404,12 @@ static const struct iio_chan_spec ak8975_channels[] = {
 	AK8975_CHANNEL(X, 0), AK8975_CHANNEL(Y, 1), AK8975_CHANNEL(Z, 2),
 };
 
-static IIO_DEVICE_ATTR(mode, S_IRUGO | S_IWUSR, show_mode, store_mode, 0);
-
-static struct attribute *ak8975_attr[] = {
-	&iio_dev_attr_mode.dev_attr.attr,
-	NULL
-};
-
-static struct attribute_group ak8975_attr_group = {
-	.attrs = ak8975_attr,
-};
-
 static const struct iio_info ak8975_info = {
-	.attrs = &ak8975_attr_group,
 	.read_raw = &ak8975_read_raw,
 	.driver_module = THIS_MODULE,
 };
 
-static int ak8975_probe(struct i2c_client *client,
+static int __devinit ak8975_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct ak8975_data *data;
@@ -488,21 +426,13 @@ static int ak8975_probe(struct i2c_client *client,
 	/* We may not have a GPIO based IRQ to scan, that is fine, we will
 	   poll if so */
 	if (gpio_is_valid(eoc_gpio)) {
-		err = gpio_request(eoc_gpio, "ak_8975");
+		err = gpio_request_one(eoc_gpio, GPIOF_IN, "ak_8975");
 		if (err < 0) {
 			dev_err(&client->dev,
 				"failed to request GPIO %d, error %d\n",
 							eoc_gpio, err);
 			goto exit;
 		}
-
-		err = gpio_direction_input(eoc_gpio);
-		if (err < 0) {
-			dev_err(&client->dev,
-				"Failed to configure input direction for GPIO %d, error %d\n",
-						eoc_gpio, err);
-			goto exit_gpio;
-		}
 	}
 
 	/* Register with IIO */
@@ -545,7 +475,7 @@ exit:
 	return err;
 }
 
-static int ak8975_remove(struct i2c_client *client)
+static int __devexit ak8975_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct ak8975_data *data = iio_priv(indio_dev);
diff --git a/drivers/staging/iio/magnetometer/hmc5843.c b/drivers/staging/iio/magnetometer/hmc5843.c
index c1fa09f0762..6c3e50f7c0d 100644
--- a/drivers/staging/iio/magnetometer/hmc5843.c
+++ b/drivers/staging/iio/magnetometer/hmc5843.c
@@ -665,7 +665,7 @@ static const struct iio_info hmc5843_info = {
 	.driver_module = THIS_MODULE,
 };
 
-static int hmc5843_probe(struct i2c_client *client,
+static int __devinit hmc5843_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct hmc5843_data *data;
@@ -704,7 +704,7 @@ exit:
 	return err;
 }
 
-static int hmc5843_remove(struct i2c_client *client)
+static int __devexit hmc5843_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 
@@ -755,7 +755,7 @@ static struct i2c_driver hmc5843_driver = {
 	},
 	.id_table	= hmc5843_id,
 	.probe		= hmc5843_probe,
-	.remove		= hmc5843_remove,
+	.remove		= __devexit_p(hmc5843_remove),
 	.detect		= hmc5843_detect,
 	.address_list	= normal_i2c,
 };
diff --git a/drivers/staging/iio/meter/ade7758.h b/drivers/staging/iio/meter/ade7758.h
index bdd1b05bf7a..ec202b4ecfb 100644
--- a/drivers/staging/iio/meter/ade7758.h
+++ b/drivers/staging/iio/meter/ade7758.h
@@ -122,7 +122,6 @@ struct ade7758_state {
 	u8			*tx;
 	u8			*rx;
 	struct mutex		buf_lock;
-	unsigned long		available_scan_masks[AD7758_NUM_WAVESRC];
 	struct iio_chan_spec	*ade7758_ring_channels;
 	struct spi_transfer	ring_xfer[4];
 	struct spi_message	ring_msg;
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index 96d6114a31a..7014a007844 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -883,7 +883,7 @@ static const struct iio_info ade7758_info = {
 
 static int __devinit ade7758_probe(struct spi_device *spi)
 {
-	int i, ret;
+	int ret;
 	struct ade7758_state *st;
 	struct iio_dev *indio_dev = iio_device_alloc(sizeof(*st));
 
@@ -916,11 +916,6 @@ static int __devinit ade7758_probe(struct spi_device *spi)
 	indio_dev->info = &ade7758_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	for (i = 0; i < AD7758_NUM_WAVESRC; i++)
-		set_bit(i, &st->available_scan_masks[i]);
-
-	indio_dev->available_scan_masks = st->available_scan_masks;
-
 	ret = ade7758_configure_ring(indio_dev);
 	if (ret)
 		goto error_free_tx;
diff --git a/drivers/staging/iio/meter/ade7758_ring.c b/drivers/staging/iio/meter/ade7758_ring.c
index 92159f208d1..1ce10b21f4d 100644
--- a/drivers/staging/iio/meter/ade7758_ring.c
+++ b/drivers/staging/iio/meter/ade7758_ring.c
@@ -114,6 +114,7 @@ static const struct iio_buffer_setup_ops ade7758_ring_setup_ops = {
 	.preenable = &ade7758_ring_preenable,
 	.postenable = &iio_triggered_buffer_postenable,
 	.predisable = &iio_triggered_buffer_predisable,
+	.validate_scan_mask = &iio_validate_scan_mask_onehot,
 };
 
 void ade7758_unconfigure_ring(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/ring_sw.c b/drivers/staging/iio/ring_sw.c
index 9358c6cb1c7..f61c8fdaab0 100644
--- a/drivers/staging/iio/ring_sw.c
+++ b/drivers/staging/iio/ring_sw.c
@@ -24,7 +24,7 @@
  * @read_p:		read pointer (oldest available)
  * @write_p:		write pointer
  * @half_p:		half buffer length behind write_p (event generation)
- * @update_needed:	flag to indicated change in size requested
+ * @update_needed:	flag to indicate change in size requested
  *
  * Note that the first element of all ring buffers must be a
  * struct iio_buffer.
@@ -363,5 +363,5 @@ void iio_sw_rb_free(struct iio_buffer *r)
 }
 EXPORT_SYMBOL(iio_sw_rb_free);
 
-MODULE_DESCRIPTION("Industrialio I/O software ring buffer");
+MODULE_DESCRIPTION("Industrial I/O software ring buffer");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
index f85734d212b..ce6a7b1b886 100644
--- a/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
+++ b/drivers/staging/iio/trigger/iio-trig-bfin-timer.c
@@ -60,7 +60,7 @@ struct bfin_tmr_state {
 static ssize_t iio_bfin_tmr_frequency_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_trigger *trig = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct bfin_tmr_state *st = trig->private_data;
 	long val;
 	int ret;
@@ -97,7 +97,7 @@ static ssize_t iio_bfin_tmr_frequency_show(struct device *dev,
 				 struct device_attribute *attr,
 				 char *buf)
 {
-	struct iio_trigger *trig = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct bfin_tmr_state *st = trig->private_data;
 
 	return sprintf(buf, "%lu\n",
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index 9f2d055524a..4ceaa18ef9f 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -41,7 +41,7 @@ static ssize_t iio_trig_periodic_read_freq(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	struct iio_trigger *trig = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct iio_prtc_trigger_info *trig_info = trig->private_data;
 	return sprintf(buf, "%u\n", trig_info->frequency);
 }
@@ -51,7 +51,7 @@ static ssize_t iio_trig_periodic_write_freq(struct device *dev,
 					    const char *buf,
 					    size_t len)
 {
-	struct iio_trigger *trig = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct iio_prtc_trigger_info *trig_info = trig->private_data;
 	unsigned long val;
 	int ret;
diff --git a/drivers/staging/iio/trigger/iio-trig-sysfs.c b/drivers/staging/iio/trigger/iio-trig-sysfs.c
index 552763bb3d4..fee47464810 100644
--- a/drivers/staging/iio/trigger/iio-trig-sysfs.c
+++ b/drivers/staging/iio/trigger/iio-trig-sysfs.c
@@ -92,7 +92,7 @@ static struct device iio_sysfs_trig_dev = {
 static ssize_t iio_sysfs_trigger_poll(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct iio_trigger *trig = dev_get_drvdata(dev);
+	struct iio_trigger *trig = to_iio_trigger(dev);
 	iio_trigger_poll_chained(trig, 0);
 
 	return count;
diff --git a/drivers/staging/ipack/TODO b/drivers/staging/ipack/TODO
index 3a45a53afd8..b21d33ab144 100644
--- a/drivers/staging/ipack/TODO
+++ b/drivers/staging/ipack/TODO
@@ -3,7 +3,7 @@
 Introduction
 ============
 
-These drivers add support for IndustryPack devices: carrier and mezzanine
+These drivers add support for IndustryPack devices: carrier and IP module
 boards.
 
 The ipack driver is just an abstraction of the bus providing the common
@@ -15,10 +15,6 @@ TODO
 TPCI-200
 --------
 
-* It receives the name of the mezzanine plugged in each slot by SYSFS.
-  No autodetection supported yet, because the mezzanine driver could not be
-  loaded at the time that the tpci200 driver loads.
-
 * It has a linked list with the tpci200 devices it is managing. Get rid of it
   and use driver_for_each_device() instead.
 
@@ -35,7 +31,7 @@ Ipack
 -----
 
 * The structures and API exported can be improved a lot. For example, the
-  way to unregistering mezzanine devices, doing the mezzanine driver a call to
+  way to unregistering IP module devices, doing the IP module driver a call to
   remove_device() to notify the carrier driver, or the opposite with the call to
   the ipack_driver_ops' remove() function could be improved.
 
@@ -43,4 +39,5 @@ Ipack
 Contact
 =======
 
-Contact: Samuel Iglesias Gonsalvez <siglesias@igalia.com>
\ No newline at end of file
+Contact: Samuel Iglesias Gonsalvez <siglesias@igalia.com>
+Mailing List: industrypack-devel@lists.sourceforge.net
diff --git a/drivers/staging/ipack/bridges/tpci200.c b/drivers/staging/ipack/bridges/tpci200.c
index ad287075023..2b83fa8e550 100644
--- a/drivers/staging/ipack/bridges/tpci200.c
+++ b/drivers/staging/ipack/bridges/tpci200.c
@@ -11,8 +11,6 @@
  * Software Foundation; version 2 of the License.
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/module.h>
 #include "tpci200.h"
 
@@ -36,10 +34,8 @@ static struct tpci200_board *check_slot(struct ipack_device *dev)
 	struct tpci200_board *tpci200;
 	int found = 0;
 
-	if (dev == NULL) {
-		pr_info("Slot doesn't exist.\n");
+	if (dev == NULL)
 		return NULL;
-	}
 
 	list_for_each_entry(tpci200, &tpci200_list, list) {
 		if (tpci200->number == dev->bus_nr) {
@@ -49,20 +45,14 @@ static struct tpci200_board *check_slot(struct ipack_device *dev)
 	}
 
 	if (!found) {
-		pr_err("Carrier not found\n");
+		dev_err(&dev->dev, "Carrier not found\n");
 		return NULL;
 	}
 
 	if (dev->slot >= TPCI200_NB_SLOT) {
-		pr_info("Slot [%d:%d] doesn't exist! Last tpci200 slot is %d.\n",
-			dev->bus_nr, dev->slot, TPCI200_NB_SLOT-1);
-		return NULL;
-	}
-
-	BUG_ON(tpci200->slots == NULL);
-	if (tpci200->slots[dev->slot].dev == NULL) {
-		pr_info("Slot [%d:%d] is not registered !\n", dev->bus_nr,
-			dev->slot);
+		dev_info(&dev->dev,
+			 "Slot [%d:%d] doesn't exist! Last tpci200 slot is %d.\n",
+			 dev->bus_nr, dev->slot, TPCI200_NB_SLOT-1);
 		return NULL;
 	}
 
@@ -123,14 +113,15 @@ static struct ipack_addr_space *get_slot_address_space(struct ipack_device *dev,
 		addr = &dev->mem_space;
 		break;
 	default:
-		pr_err("Slot [%d:%d] space number %d doesn't exist !\n",
-		       dev->bus_nr, dev->slot, space);
+		dev_err(&dev->dev,
+			"Slot [%d:%d] space number %d doesn't exist !\n",
+			dev->bus_nr, dev->slot, space);
 		return NULL;
 		break;
 	}
 
 	if ((addr->size == 0) || (addr->address == NULL)) {
-		pr_err("Error, slot space not mapped !\n");
+		dev_err(&dev->dev, "Error, slot space not mapped !\n");
 		return NULL;
 	}
 
@@ -152,7 +143,7 @@ static int tpci200_read8(struct ipack_device *dev, int space,
 		return -EINVAL;
 
 	if (offset >= addr->size) {
-		pr_err("Error, slot space offset error !\n");
+		dev_err(&dev->dev, "Error, slot space offset error !\n");
 		return -EFAULT;
 	}
 
@@ -176,7 +167,7 @@ static int tpci200_read16(struct ipack_device *dev, int space,
 		return -EINVAL;
 
 	if ((offset+2) >= addr->size) {
-		pr_err("Error, slot space offset error !\n");
+		dev_err(&dev->dev, "Error, slot space offset error !\n");
 		return -EFAULT;
 	}
 	*value = __tpci200_read16(addr->address, offset);
@@ -199,7 +190,7 @@ static int tpci200_read32(struct ipack_device *dev, int space,
 		return -EINVAL;
 
 	if ((offset+4) >= addr->size) {
-		pr_err("Error, slot space offset error !\n");
+		dev_err(&dev->dev, "Error, slot space offset error !\n");
 		return -EFAULT;
 	}
 
@@ -223,7 +214,7 @@ static int tpci200_write8(struct ipack_device *dev, int space,
 		return -EINVAL;
 
 	if (offset >= addr->size) {
-		pr_err("Error, slot space offset error !\n");
+		dev_err(&dev->dev, "Error, slot space offset error !\n");
 		return -EFAULT;
 	}
 
@@ -247,7 +238,7 @@ static int tpci200_write16(struct ipack_device *dev, int space,
 		return -EINVAL;
 
 	if ((offset+2) >= addr->size) {
-		pr_err("Error, slot space offset error !\n");
+		dev_err(&dev->dev, "Error, slot space offset error !\n");
 		return -EFAULT;
 	}
 
@@ -271,7 +262,7 @@ static int tpci200_write32(struct ipack_device *dev, int space,
 		return -EINVAL;
 
 	if ((offset+4) >= addr->size) {
-		pr_err("Error, slot space offset error !\n");
+		dev_err(&dev->dev, "Error, slot space offset error !\n");
 		return -EFAULT;
 	}
 
@@ -297,8 +288,6 @@ static void tpci200_unregister(struct tpci200_board *tpci200)
 	pci_disable_device(tpci200->info->pdev);
 	pci_dev_put(tpci200->info->pdev);
 
-	kfree(tpci200->info);
-
 	for (i = 0; i < TPCI200_NB_SLOT; i++) {
 		tpci200->slots[i].io_phys.address = NULL;
 		tpci200->slots[i].io_phys.size = 0;
@@ -313,13 +302,10 @@ static irqreturn_t tpci200_interrupt(int irq, void *dev_id)
 {
 	struct tpci200_board *tpci200 = (struct tpci200_board *) dev_id;
 	int i;
-	unsigned long flags;
 	unsigned short status_reg, reg_value;
 	unsigned short unhandled_ints = 0;
 	irqreturn_t ret = IRQ_NONE;
 
-	spin_lock_irqsave(&tpci200->info->access_lock, flags);
-
 	/* Read status register */
 	status_reg = readw(tpci200->info->interface_regs +
 			   TPCI200_STATUS_REG);
@@ -347,8 +333,9 @@ static irqreturn_t tpci200_interrupt(int irq, void *dev_id)
 	if (unhandled_ints) {
 		for (i = 0; i < TPCI200_NB_SLOT; i++) {
 			if (unhandled_ints & ((TPCI200_INT0_EN | TPCI200_INT1_EN) << (2*i))) {
-				pr_info("No registered ISR for slot [%d:%d]!. IRQ will be disabled.\n",
-					tpci200->number, i);
+				dev_info(&tpci200->slots[i].dev->dev,
+					 "No registered ISR for slot [%d:%d]!. IRQ will be disabled.\n",
+					 tpci200->number, i);
 				reg_value = readw(
 					tpci200->info->interface_regs +
 					control_reg[i]);
@@ -361,211 +348,12 @@ static irqreturn_t tpci200_interrupt(int irq, void *dev_id)
 		}
 	}
 
-	spin_unlock_irqrestore(&tpci200->info->access_lock, flags);
 	return ret;
 }
 
-#ifdef CONFIG_SYSFS
-
-static struct ipack_device *tpci200_slot_register(unsigned int tpci200_number,
-						  unsigned int slot_position)
-{
-	int found = 0;
-	struct ipack_device  *dev;
-	struct tpci200_board *tpci200;
-
-	list_for_each_entry(tpci200, &tpci200_list, list) {
-		if (tpci200->number == tpci200_number) {
-			found = 1;
-			break;
-		}
-	}
-
-	if (!found) {
-		pr_err("carrier board not found for the device\n");
-		return NULL;
-	}
-
-	if (slot_position >= TPCI200_NB_SLOT) {
-		pr_info("Slot [%d:%d] doesn't exist!\n", tpci200_number,
-			slot_position);
-		return NULL;
-	}
-
-	if (mutex_lock_interruptible(&tpci200->mutex))
-		return NULL;
-
-	if (tpci200->slots[slot_position].dev != NULL) {
-		pr_err("Slot [%d:%d] already installed !\n", tpci200_number,
-		       slot_position);
-		goto err_unlock;
-	}
-
-	/*
-	 * Give the same IRQ number as the slot number.
-	 * The TPCI200 has assigned his own two IRQ by PCI bus driver
-	 */
-	dev = ipack_device_register(tpci200->info->ipack_bus,
-				    slot_position, slot_position);
-	if (dev == NULL) {
-		pr_info("Slot [%d:%d] Unable to register an ipack device\n",
-			tpci200_number, slot_position);
-		goto err_unlock;
-	}
-
-	tpci200->slots[slot_position].dev = dev;
-	mutex_unlock(&tpci200->mutex);
-	return dev;
-
-err_unlock:
-	mutex_unlock(&tpci200->mutex);
-	return NULL;
-}
-
-static ssize_t tpci200_store_board(struct device *pdev, const char *buf,
-				   size_t count, int slot)
-{
-	struct tpci200_board *card = dev_get_drvdata(pdev);
-	struct ipack_device *dev = card->slots[slot].dev;
-
-	if (dev != NULL)
-		return -EBUSY;
-
-	dev = tpci200_slot_register(card->number, slot);
-	if (dev == NULL)
-		return -ENODEV;
-
-	return count;
-}
-
-static ssize_t tpci200_show_board(struct device *pdev, char *buf, int slot)
-{
-	struct tpci200_board *card = dev_get_drvdata(pdev);
-	struct ipack_device *dev = card->slots[slot].dev;
-
-	if (dev != NULL)
-		return snprintf(buf, PAGE_SIZE, "%s\n", dev_name(&dev->dev));
-	else
-		return snprintf(buf, PAGE_SIZE, "none\n");
-}
-
-static ssize_t tpci200_show_description(struct device *pdev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	return snprintf(buf, PAGE_SIZE,
-			"TEWS tpci200 carrier PCI for Industry-pack mezzanines.\n");
-}
-
-static ssize_t tpci200_show_board_slot0(struct device *pdev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	return tpci200_show_board(pdev, buf, 0);
-}
-
-static ssize_t tpci200_store_board_slot0(struct device *pdev,
-					 struct device_attribute *attr,
-					 const char *buf, size_t count)
-{
-	return tpci200_store_board(pdev, buf, count, 0);
-}
-
-static ssize_t tpci200_show_board_slot1(struct device *pdev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	return tpci200_show_board(pdev, buf, 1);
-}
-
-static ssize_t tpci200_store_board_slot1(struct device *pdev,
-					 struct device_attribute *attr,
-					 const char *buf, size_t count)
-{
-	return tpci200_store_board(pdev, buf, count, 1);
-}
-
-static ssize_t tpci200_show_board_slot2(struct device *pdev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	return tpci200_show_board(pdev, buf, 2);
-}
-
-static ssize_t tpci200_store_board_slot2(struct device *pdev,
-					 struct device_attribute *attr,
-					 const char *buf, size_t count)
-{
-	return tpci200_store_board(pdev, buf, count, 2);
-}
-
-
-static ssize_t tpci200_show_board_slot3(struct device *pdev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	return tpci200_show_board(pdev, buf, 3);
-}
-
-static ssize_t tpci200_store_board_slot3(struct device *pdev,
-					 struct device_attribute *attr,
-					 const char *buf, size_t count)
-{
-	return tpci200_store_board(pdev, buf, count, 3);
-}
-
-/* Declaration of the device attributes for the TPCI200 */
-static DEVICE_ATTR(description, S_IRUGO,
-		   tpci200_show_description, NULL);
-static DEVICE_ATTR(board_slot0, S_IRUGO | S_IWUSR,
-		   tpci200_show_board_slot0, tpci200_store_board_slot0);
-static DEVICE_ATTR(board_slot1, S_IRUGO | S_IWUSR,
-		   tpci200_show_board_slot1, tpci200_store_board_slot1);
-static DEVICE_ATTR(board_slot2, S_IRUGO | S_IWUSR,
-		   tpci200_show_board_slot2, tpci200_store_board_slot2);
-static DEVICE_ATTR(board_slot3, S_IRUGO | S_IWUSR,
-		   tpci200_show_board_slot3, tpci200_store_board_slot3);
-
-static struct attribute *tpci200_attrs[] = {
-	&dev_attr_description.attr,
-	&dev_attr_board_slot0.attr,
-	&dev_attr_board_slot1.attr,
-	&dev_attr_board_slot2.attr,
-	&dev_attr_board_slot3.attr,
-	NULL,
-};
-
-static struct attribute_group tpci200_attr_group = {
-	.attrs = tpci200_attrs,
-};
-
-static int tpci200_create_sysfs_files(struct tpci200_board *card)
-{
-	return sysfs_create_group(&card->info->pdev->dev.kobj,
-				  &tpci200_attr_group);
-}
-
-static void tpci200_remove_sysfs_files(struct tpci200_board *card)
-{
-	sysfs_remove_group(&card->info->pdev->dev.kobj, &tpci200_attr_group);
-}
-
-#else
-
-static int tpci200_create_sysfs_files(struct tpci200_board *card)
-{
-	return 0;
-}
-
-static void tpci200_remove_sysfs_files(struct tpci200_board *card)
-{
-}
-
-#endif /* CONFIG_SYSFS */
-
 static int tpci200_register(struct tpci200_board *tpci200)
 {
-	int  i;
+	int i;
 	int res;
 	unsigned long ioidint_base;
 	unsigned long mem_base;
@@ -574,20 +362,15 @@ static int tpci200_register(struct tpci200_board *tpci200)
 	if (pci_enable_device(tpci200->info->pdev) < 0)
 		return -ENODEV;
 
-	if (tpci200_create_sysfs_files(tpci200) < 0) {
-		pr_err("failed creating sysfs files\n");
-		res = -EFAULT;
-		goto out_disable_pci;
-	}
-
 	/* Request IP interface register (Bar 2) */
 	res = pci_request_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR,
 				 "Carrier IP interface registers");
 	if (res) {
-		pr_err("(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 2 !",
-		       tpci200->info->pdev->bus->number,
-		       tpci200->info->pdev->devfn);
-		goto out_remove_sysfs;
+		dev_err(&tpci200->info->pdev->dev,
+			"(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 2 !",
+			tpci200->info->pdev->bus->number,
+			tpci200->info->pdev->devfn);
+		goto out_disable_pci;
 	}
 
 	/* Request IO ID INT space (Bar 3) */
@@ -595,9 +378,10 @@ static int tpci200_register(struct tpci200_board *tpci200)
 				 TPCI200_IO_ID_INT_SPACES_BAR,
 				 "Carrier IO ID INT space");
 	if (res) {
-		pr_err("(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 3 !",
-		       tpci200->info->pdev->bus->number,
-		       tpci200->info->pdev->devfn);
+		dev_err(&tpci200->info->pdev->dev,
+			"(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 3 !",
+			tpci200->info->pdev->bus->number,
+			tpci200->info->pdev->devfn);
 		goto out_release_ip_space;
 	}
 
@@ -605,9 +389,10 @@ static int tpci200_register(struct tpci200_board *tpci200)
 	res = pci_request_region(tpci200->info->pdev, TPCI200_MEM8_SPACE_BAR,
 				 "Carrier MEM space");
 	if (res) {
-		pr_err("(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 4!",
-		       tpci200->info->pdev->bus->number,
-		       tpci200->info->pdev->devfn);
+		dev_err(&tpci200->info->pdev->dev,
+			"(bn 0x%X, sn 0x%X) failed to allocate PCI resource for BAR 4!",
+			tpci200->info->pdev->bus->number,
+			tpci200->info->pdev->devfn);
 		goto out_release_ioid_int_space;
 	}
 
@@ -625,7 +410,6 @@ static int tpci200_register(struct tpci200_board *tpci200)
 					   TPCI200_MEM8_SPACE_BAR),
 			TPCI200_MEM8_SIZE);
 
-	spin_lock_init(&tpci200->info->access_lock);
 	ioidint_base = pci_resource_start(tpci200->info->pdev,
 					  TPCI200_IO_ID_INT_SPACES_BAR);
 	mem_base = pci_resource_start(tpci200->info->pdev,
@@ -665,11 +449,11 @@ static int tpci200_register(struct tpci200_board *tpci200)
 			  tpci200_interrupt, IRQF_SHARED,
 			  KBUILD_MODNAME, (void *) tpci200);
 	if (res) {
-		pr_err("(bn 0x%X, sn 0x%X) unable to register IRQ !",
-		       tpci200->info->pdev->bus->number,
-		       tpci200->info->pdev->devfn);
-		tpci200_unregister(tpci200);
-		goto out_err;
+		dev_err(&tpci200->info->pdev->dev,
+			"(bn 0x%X, sn 0x%X) unable to register IRQ !",
+			tpci200->info->pdev->bus->number,
+			tpci200->info->pdev->devfn);
+		goto out_release_ioid_int_space;
 	}
 
 	return 0;
@@ -678,11 +462,8 @@ out_release_ioid_int_space:
 	pci_release_region(tpci200->info->pdev, TPCI200_IO_ID_INT_SPACES_BAR);
 out_release_ip_space:
 	pci_release_region(tpci200->info->pdev, TPCI200_IP_INTERFACE_BAR);
-out_remove_sysfs:
-	tpci200_remove_sysfs_files(tpci200);
 out_disable_pci:
 	pci_disable_device(tpci200->info->pdev);
-out_err:
 	return res;
 }
 
@@ -726,24 +507,19 @@ static void __tpci200_free_irq(struct tpci200_board *tpci200,
 
 static int tpci200_free_irq(struct ipack_device *dev)
 {
-	int res;
 	struct slot_irq *slot_irq;
 	struct tpci200_board *tpci200;
 
 	tpci200 = check_slot(dev);
-	if (tpci200 == NULL) {
-		res = -EINVAL;
-		goto out;
-	}
+	if (tpci200 == NULL)
+		return -EINVAL;
 
-	if (mutex_lock_interruptible(&tpci200->mutex)) {
-		res = -ERESTARTSYS;
-		goto out;
-	}
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
 
 	if (tpci200->slots[dev->slot].irq == NULL) {
-		res = -EINVAL;
-		goto out_unlock;
+		mutex_unlock(&tpci200->mutex);
+		return -EINVAL;
 	}
 
 	__tpci200_free_irq(tpci200, dev);
@@ -751,60 +527,56 @@ static int tpci200_free_irq(struct ipack_device *dev)
 	tpci200->slots[dev->slot].irq = NULL;
 	kfree(slot_irq);
 
-out_unlock:
 	mutex_unlock(&tpci200->mutex);
-out:
-	return res;
+	return 0;
 }
 
 static int tpci200_slot_unmap_space(struct ipack_device *dev, int space)
 {
-	int res;
 	struct ipack_addr_space *virt_addr_space;
 	struct tpci200_board *tpci200;
 
 	tpci200 = check_slot(dev);
-	if (tpci200 == NULL) {
-		res = -EINVAL;
-		goto out;
-	}
+	if (tpci200 == NULL)
+		return -EINVAL;
 
-	if (mutex_lock_interruptible(&tpci200->mutex)) {
-		res = -ERESTARTSYS;
-		goto out;
-	}
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
 
 	switch (space) {
 	case IPACK_IO_SPACE:
 		if (dev->io_space.address == NULL) {
-			pr_info("Slot [%d:%d] IO space not mapped !\n",
-				dev->bus_nr, dev->slot);
+			dev_info(&dev->dev,
+				 "Slot [%d:%d] IO space not mapped !\n",
+				 dev->bus_nr, dev->slot);
 			goto out_unlock;
 		}
 		virt_addr_space = &dev->io_space;
 		break;
 	case IPACK_ID_SPACE:
 		if (dev->id_space.address == NULL) {
-			pr_info("Slot [%d:%d] ID space not mapped !\n",
-				dev->bus_nr, dev->slot);
+			dev_info(&dev->dev,
+				 "Slot [%d:%d] ID space not mapped !\n",
+				 dev->bus_nr, dev->slot);
 			goto out_unlock;
 		}
 		virt_addr_space = &dev->id_space;
 		break;
 	case IPACK_MEM_SPACE:
 		if (dev->mem_space.address == NULL) {
-			pr_info("Slot [%d:%d] MEM space not mapped !\n",
-				dev->bus_nr, dev->slot);
-		goto out_unlock;
+			dev_info(&dev->dev,
+				 "Slot [%d:%d] MEM space not mapped !\n",
+				 dev->bus_nr, dev->slot);
+			goto out_unlock;
 		}
 		virt_addr_space = &dev->mem_space;
 		break;
 	default:
-		pr_err("Slot [%d:%d] space number %d doesn't exist !\n",
-		       dev->bus_nr, dev->slot, space);
-		res = -EINVAL;
-		goto out_unlock;
-		break;
+		dev_err(&dev->dev,
+			"Slot [%d:%d] space number %d doesn't exist !\n",
+			dev->bus_nr, dev->slot, space);
+		mutex_unlock(&tpci200->mutex);
+		return -EINVAL;
 	}
 
 	iounmap(virt_addr_space->address);
@@ -813,8 +585,7 @@ static int tpci200_slot_unmap_space(struct ipack_device *dev, int space)
 	virt_addr_space->size = 0;
 out_unlock:
 	mutex_unlock(&tpci200->mutex);
-out:
-	return res;
+	return 0;
 }
 
 static int tpci200_slot_unregister(struct ipack_device *dev)
@@ -843,28 +614,25 @@ static int tpci200_slot_unregister(struct ipack_device *dev)
 static int tpci200_slot_map_space(struct ipack_device *dev,
 				  unsigned int memory_size, int space)
 {
-	int res;
+	int res = 0;
 	unsigned int size_to_map;
 	void __iomem *phys_address;
 	struct ipack_addr_space *virt_addr_space;
 	struct tpci200_board *tpci200;
 
 	tpci200 = check_slot(dev);
-	if (tpci200 == NULL) {
-		res = -EINVAL;
-		goto out;
-	}
+	if (tpci200 == NULL)
+		return -EINVAL;
 
-	if (mutex_lock_interruptible(&tpci200->mutex)) {
-		res = -ERESTARTSYS;
-		goto out;
-	}
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
 
 	switch (space) {
 	case IPACK_IO_SPACE:
 		if (dev->io_space.address != NULL) {
-			pr_err("Slot [%d:%d] IO space already mapped !\n",
-			       tpci200->number, dev->slot);
+			dev_err(&dev->dev,
+				"Slot [%d:%d] IO space already mapped !\n",
+				tpci200->number, dev->slot);
 			res = -EINVAL;
 			goto out_unlock;
 		}
@@ -875,8 +643,9 @@ static int tpci200_slot_map_space(struct ipack_device *dev,
 		break;
 	case IPACK_ID_SPACE:
 		if (dev->id_space.address != NULL) {
-			pr_err("Slot [%d:%d] ID space already mapped !\n",
-			       tpci200->number, dev->slot);
+			dev_err(&dev->dev,
+				"Slot [%d:%d] ID space already mapped !\n",
+				tpci200->number, dev->slot);
 			res = -EINVAL;
 			goto out_unlock;
 		}
@@ -887,17 +656,19 @@ static int tpci200_slot_map_space(struct ipack_device *dev,
 		break;
 	case IPACK_MEM_SPACE:
 		if (dev->mem_space.address != NULL) {
-			pr_err("Slot [%d:%d] MEM space already mapped !\n",
-			       tpci200->number, dev->slot);
+			dev_err(&dev->dev,
+				"Slot [%d:%d] MEM space already mapped !\n",
+				tpci200->number, dev->slot);
 			res = -EINVAL;
 			goto out_unlock;
 		}
 		virt_addr_space = &dev->mem_space;
 
 		if (memory_size > tpci200->slots[dev->slot].mem_phys.size) {
-			pr_err("Slot [%d:%d] request is 0x%X memory, only 0x%X available !\n",
-			       dev->bus_nr, dev->slot, memory_size,
-			       tpci200->slots[dev->slot].mem_phys.size);
+			dev_err(&dev->dev,
+				"Slot [%d:%d] request is 0x%X memory, only 0x%X available !\n",
+				dev->bus_nr, dev->slot, memory_size,
+				tpci200->slots[dev->slot].mem_phys.size);
 			res = -EINVAL;
 			goto out_unlock;
 		}
@@ -906,11 +677,10 @@ static int tpci200_slot_map_space(struct ipack_device *dev,
 		size_to_map = memory_size;
 		break;
 	default:
-		pr_err("Slot [%d:%d] space %d doesn't exist !\n",
-		       tpci200->number, dev->slot, space);
+		dev_err(&dev->dev, "Slot [%d:%d] space %d doesn't exist !\n",
+			tpci200->number, dev->slot, space);
 		res = -EINVAL;
 		goto out_unlock;
-		break;
 	}
 
 	virt_addr_space->size = size_to_map;
@@ -919,7 +689,6 @@ static int tpci200_slot_map_space(struct ipack_device *dev,
 
 out_unlock:
 	mutex_unlock(&tpci200->mutex);
-out:
 	return res;
 }
 
@@ -931,27 +700,25 @@ static int tpci200_request_irq(struct ipack_device *dev, int vector,
 	struct tpci200_board *tpci200;
 
 	tpci200 = check_slot(dev);
-	if (tpci200 == NULL) {
-		res = -EINVAL;
-		goto out;
-	}
+	if (tpci200 == NULL)
+		return -EINVAL;
 
-	if (mutex_lock_interruptible(&tpci200->mutex)) {
-		res = -ERESTARTSYS;
-		goto out;
-	}
+	if (mutex_lock_interruptible(&tpci200->mutex))
+		return -ERESTARTSYS;
 
 	if (tpci200->slots[dev->slot].irq != NULL) {
-		pr_err("Slot [%d:%d] IRQ already registered !\n", dev->bus_nr,
-		       dev->slot);
+		dev_err(&dev->dev,
+			"Slot [%d:%d] IRQ already registered !\n", dev->bus_nr,
+			dev->slot);
 		res = -EINVAL;
 		goto out_unlock;
 	}
 
 	slot_irq = kzalloc(sizeof(struct slot_irq), GFP_KERNEL);
 	if (slot_irq == NULL) {
-		pr_err("Slot [%d:%d] unable to allocate memory for IRQ !\n",
-		       dev->bus_nr, dev->slot);
+		dev_err(&dev->dev,
+			"Slot [%d:%d] unable to allocate memory for IRQ !\n",
+			dev->bus_nr, dev->slot);
 		res = -ENOMEM;
 		goto out_unlock;
 	}
@@ -965,32 +732,21 @@ static int tpci200_request_irq(struct ipack_device *dev, int vector,
 	slot_irq->vector = vector;
 	slot_irq->handler = handler;
 	slot_irq->arg = arg;
-	slot_irq->name = dev_name(&dev->dev);
 
 	tpci200->slots[dev->slot].irq = slot_irq;
 	res = __tpci200_request_irq(tpci200, dev);
 
 out_unlock:
 	mutex_unlock(&tpci200->mutex);
-out:
 	return res;
 }
 
-static void tpci200_slot_remove(struct tpci200_slot *slot)
-{
-	if ((slot->dev == NULL) ||
-	    (slot->dev->driver->ops->remove == NULL))
-		return;
-
-	slot->dev->driver->ops->remove(slot->dev);
-}
-
 static void tpci200_uninstall(struct tpci200_board *tpci200)
 {
 	int i;
 
 	for (i = 0; i < TPCI200_NB_SLOT; i++)
-		tpci200_slot_remove(&tpci200->slots[i]);
+		tpci200_slot_unregister(tpci200->slots[i].dev);
 
 	tpci200_unregister(tpci200);
 	kfree(tpci200->slots);
@@ -1016,29 +772,24 @@ static int tpci200_install(struct tpci200_board *tpci200)
 
 	tpci200->slots = kzalloc(
 		TPCI200_NB_SLOT * sizeof(struct tpci200_slot), GFP_KERNEL);
-	if (tpci200->slots == NULL) {
-		res = -ENOMEM;
-		goto out_err;
-	}
+	if (tpci200->slots == NULL)
+		return -ENOMEM;
 
 	res = tpci200_register(tpci200);
-	if (res)
-		goto out_free;
+	if (res) {
+		kfree(tpci200->slots);
+		tpci200->slots = NULL;
+		return res;
+	}
 
 	mutex_init(&tpci200->mutex);
 	return 0;
-
-out_free:
-	kfree(tpci200->slots);
-	tpci200->slots = NULL;
-out_err:
-	return res;
 }
 
 static int tpci200_pciprobe(struct pci_dev *pdev,
 			    const struct pci_device_id *id)
 {
-	int ret;
+	int ret, i;
 	struct tpci200_board *tpci200;
 
 	tpci200 = kzalloc(sizeof(struct tpci200_board), GFP_KERNEL);
@@ -1058,7 +809,7 @@ static int tpci200_pciprobe(struct pci_dev *pdev,
 	/* register the device and initialize it */
 	ret = tpci200_install(tpci200);
 	if (ret) {
-		pr_err("Error during tpci200 install !\n");
+		dev_err(&pdev->dev, "Error during tpci200 install !\n");
 		kfree(tpci200->info);
 		kfree(tpci200);
 		return -ENODEV;
@@ -1069,7 +820,8 @@ static int tpci200_pciprobe(struct pci_dev *pdev,
 						      TPCI200_NB_SLOT,
 						      &tpci200_bus_ops);
 	if (!tpci200->info->ipack_bus) {
-		pr_err("error registering the carrier on ipack driver\n");
+		dev_err(&pdev->dev,
+			"error registering the carrier on ipack driver\n");
 		tpci200_uninstall(tpci200);
 		kfree(tpci200->info);
 		kfree(tpci200);
@@ -1081,13 +833,20 @@ static int tpci200_pciprobe(struct pci_dev *pdev,
 	dev_set_drvdata(&pdev->dev, tpci200);
 	/* add the registered device in an internal linked list */
 	list_add_tail(&tpci200->list, &tpci200_list);
+
+	/*
+	 * Give the same IRQ number as the slot number.
+	 * The TPCI200 has assigned his own two IRQ by PCI bus driver
+	 */
+	for (i = 0; i < TPCI200_NB_SLOT; i++)
+		tpci200->slots[i].dev =
+			ipack_device_register(tpci200->info->ipack_bus, i, i);
 	return ret;
 }
 
 static void __tpci200_pci_remove(struct tpci200_board *tpci200)
 {
 	tpci200_uninstall(tpci200);
-	tpci200_remove_sysfs_files(tpci200);
 	list_del(&tpci200->list);
 	ipack_bus_unregister(tpci200->info->ipack_bus);
 	kfree(tpci200->info);
@@ -1107,12 +866,14 @@ static void __devexit tpci200_pci_remove(struct pci_dev *dev)
 	}
 }
 
-static struct pci_device_id tpci200_idtable[2] = {
+static DEFINE_PCI_DEVICE_TABLE(tpci200_idtable) = {
 	{ TPCI200_VENDOR_ID, TPCI200_DEVICE_ID, TPCI200_SUBVENDOR_ID,
 	  TPCI200_SUBDEVICE_ID },
 	{ 0, },
 };
 
+MODULE_DEVICE_TABLE(pci, tpci200_idtable);
+
 static struct pci_driver tpci200_pci_drv = {
 	.name = "tpci200",
 	.id_table = tpci200_idtable,
diff --git a/drivers/staging/ipack/bridges/tpci200.h b/drivers/staging/ipack/bridges/tpci200.h
index 0b547eedddc..d04510a89be 100644
--- a/drivers/staging/ipack/bridges/tpci200.h
+++ b/drivers/staging/ipack/bridges/tpci200.h
@@ -14,7 +14,6 @@
 #ifndef _TPCI200_H_
 #define _TPCI200_H_
 
-#include <linux/version.h>
 #include <linux/limits.h>
 #include <linux/pci.h>
 #include <linux/spinlock.h>
@@ -106,14 +105,12 @@
  * @vector	Vector number
  * @handler	Handler called when IRQ arrives
  * @arg		Handler argument
- * @name	IRQ name
  *
  */
 struct slot_irq {
 	int		vector;
 	int		(*handler)(void *);
 	void		*arg;
-	const char	*name;
 };
 
 /**
@@ -139,7 +136,6 @@ struct tpci200_slot {
  * @interface_regs	Pointer to IP interface space (Bar 2)
  * @ioidint_space	Pointer to IP ID, IO and INT space (Bar 3)
  * @mem8_space		Pointer to MEM space (Bar 4)
- * @access_lock		Mutex lock for simultaneous access
  *
  */
 struct tpci200_infos {
@@ -148,7 +144,6 @@ struct tpci200_infos {
 	void __iomem			*interface_regs;
 	void __iomem			*ioidint_space;
 	void __iomem			*mem8_space;
-	spinlock_t			access_lock;
 	struct ipack_bus_device		*ipack_bus;
 };
 struct tpci200_board {
diff --git a/drivers/staging/ipack/devices/ipoctal.c b/drivers/staging/ipack/devices/ipoctal.c
index 29f6fa841d2..fd0e30132ca 100644
--- a/drivers/staging/ipack/devices/ipoctal.c
+++ b/drivers/staging/ipack/devices/ipoctal.c
@@ -11,20 +11,14 @@
  * Software Foundation; version 2 of the License.
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/device.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/interrupt.h>
-#include <linux/fs.h>
-#include <linux/delay.h>
 #include <linux/sched.h>
 #include <linux/tty.h>
 #include <linux/serial.h>
 #include <linux/tty_flip.h>
 #include <linux/slab.h>
-#include <linux/uaccess.h>
 #include <linux/atomic.h>
 #include "../ipack.h"
 #include "ipoctal.h"
@@ -48,12 +42,9 @@ struct ipoctal {
 	struct scc2698_channel		*chan_regs;
 	struct scc2698_block		*block_regs;
 	struct ipoctal_stats		chan_stats[NR_CHANNELS];
-	char				*buffer[NR_CHANNELS];
 	unsigned int			nb_bytes[NR_CHANNELS];
 	unsigned int			count_wr[NR_CHANNELS];
-	struct ipoctal_config		chan_config[NR_CHANNELS];
 	wait_queue_head_t		queue[NR_CHANNELS];
-	unsigned short			error_flag[NR_CHANNELS];
 	spinlock_t			lock[NR_CHANNELS];
 	unsigned int			pointer_read[NR_CHANNELS];
 	unsigned int			pointer_write[NR_CHANNELS];
@@ -116,14 +107,13 @@ static int ipoctal_port_activate(struct tty_port *port, struct tty_struct *tty)
 	ipoctal = ipoctal_find_board(tty);
 
 	if (ipoctal == NULL) {
-		pr_err("Device not found. Major %d\n", tty->driver->major);
+		dev_err(tty->dev, "Device not found. Major %d\n",
+			tty->driver->major);
 		return -ENODEV;
 	}
 
 	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
 			     CR_ENABLE_RX);
-	tty->driver_data = ipoctal;
-
 	return 0;
 }
 
@@ -136,13 +126,16 @@ static int ipoctal_open(struct tty_struct *tty, struct file *file)
 	ipoctal = ipoctal_find_board(tty);
 
 	if (ipoctal == NULL) {
-		pr_err("Device not found. Major %d\n", tty->driver->major);
+		dev_err(tty->dev, "Device not found. Major %d\n",
+			tty->driver->major);
 		return -ENODEV;
 	}
 
 	if (atomic_read(&ipoctal->open[channel]))
 		return -EBUSY;
 
+	tty->driver_data = ipoctal;
+
 	res = tty_port_open(&ipoctal->tty_port[channel], tty, file);
 	if (res)
 		return res;
@@ -276,23 +269,19 @@ static int ipoctal_irq_handler(void *arg)
 						     CR_CMD_RESET_ERR_STATUS);
 
 				if (sr & SR_OVERRUN_ERROR) {
-					ipoctal->error_flag[channel] |= UART_OVERRUN;
 					ipoctal->chan_stats[channel].overrun_err++;
 					/* Overrun doesn't affect the current character*/
 					tty_insert_flip_char(tty, 0, TTY_OVERRUN);
 				}
 				if (sr & SR_PARITY_ERROR) {
-					ipoctal->error_flag[channel] |= UART_PARITY;
 					ipoctal->chan_stats[channel].parity_err++;
 					flag = TTY_PARITY;
 				}
 				if (sr & SR_FRAMING_ERROR) {
-					ipoctal->error_flag[channel] |= UART_FRAMING;
 					ipoctal->chan_stats[channel].framing_err++;
 					flag = TTY_FRAME;
 				}
 				if (sr & SR_RECEIVED_BREAK) {
-					ipoctal->error_flag[channel] |= UART_BREAK;
 					ipoctal->chan_stats[channel].rcv_break++;
 					flag = TTY_BREAK;
 				}
@@ -310,8 +299,8 @@ static int ipoctal_irq_handler(void *arg)
 				ipoctal->nb_bytes[channel] = 0;
 				continue;
 			}
-			spin_lock(&ipoctal->lock[channel]);
-			value = ipoctal->buffer[channel][*pointer_write];
+
+			value = ipoctal->tty_port[channel].xmit_buf[*pointer_write];
 			ipoctal_write_io_reg(ipoctal,
 					     &ipoctal->chan_regs[channel].u.w.thr,
 					     value);
@@ -320,7 +309,6 @@ static int ipoctal_irq_handler(void *arg)
 			(*pointer_write)++;
 			*pointer_write = *pointer_write % PAGE_SIZE;
 			ipoctal->nb_bytes[channel]--;
-			spin_unlock(&ipoctal->lock[channel]);
 
 			if ((ipoctal->nb_bytes[channel] == 0) &&
 			    (waitqueue_active(&ipoctal->queue[channel]))) {
@@ -381,7 +369,9 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev, 0,
 						IPACK_ID_SPACE);
 	if (res) {
-		pr_err("Unable to map slot [%d:%d] ID space!\n", bus_nr, slot);
+		dev_err(&ipoctal->dev->dev,
+			"Unable to map slot [%d:%d] ID space!\n",
+			bus_nr, slot);
 		return res;
 	}
 
@@ -396,14 +386,18 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev, 0,
 						IPACK_IO_SPACE);
 	if (res) {
-		pr_err("Unable to map slot [%d:%d] IO space!\n", bus_nr, slot);
+		dev_err(&ipoctal->dev->dev,
+			"Unable to map slot [%d:%d] IO space!\n",
+			bus_nr, slot);
 		goto out_unregister_id_space;
 	}
 
 	res = ipoctal->dev->bus->ops->map_space(ipoctal->dev,
 					   0x8000, IPACK_MEM_SPACE);
 	if (res) {
-		pr_err("Unable to map slot [%d:%d] MEM space!\n", bus_nr, slot);
+		dev_err(&ipoctal->dev->dev,
+			"Unable to map slot [%d:%d] MEM space!\n",
+			bus_nr, slot);
 		goto out_unregister_io_space;
 	}
 
@@ -417,6 +411,20 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	for (i = 0; i < NR_CHANNELS ; i++) {
 		ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
 				     CR_DISABLE_RX | CR_DISABLE_TX);
+		ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
+				     CR_CMD_RESET_RX);
+		ipoctal_write_cr_cmd(ipoctal, &ipoctal->chan_regs[i].u.w.cr,
+				     CR_CMD_RESET_TX);
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->chan_regs[i].u.w.mr,
+				     MR1_CHRL_8_BITS | MR1_ERROR_CHAR |
+				     MR1_RxINT_RxRDY); /* mr1 */
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->chan_regs[i].u.w.mr,
+				     0); /* mr2 */
+		ipoctal_write_io_reg(ipoctal,
+				     &ipoctal->chan_regs[i].u.w.csr,
+				     TX_CLK_9600  | RX_CLK_9600);
 	}
 
 	for (i = 0; i < IP_OCTAL_NB_BLOCKS; i++) {
@@ -441,7 +449,8 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	 */
 	ipoctal->dev->bus->ops->request_irq(ipoctal->dev, vector,
 				       ipoctal_irq_handler, ipoctal);
-	ipoctal->dev->bus->ops->write8(ipoctal->dev, IPACK_ID_SPACE, 0, vector);
+	ipoctal->dev->bus->ops->write8(ipoctal->dev, IPACK_MEM_SPACE, 0,
+				       vector);
 
 	/* Register the TTY device */
 
@@ -472,7 +481,7 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 	tty_set_operations(tty, &ipoctal_fops);
 	res = tty_register_driver(tty);
 	if (res) {
-		pr_err("Can't register tty driver.\n");
+		dev_err(&ipoctal->dev->dev, "Can't register tty driver.\n");
 		put_tty_driver(tty);
 		goto out_unregister_slot_unmap;
 	}
@@ -488,7 +497,6 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 		ipoctal_reset_stats(&ipoctal->chan_stats[i]);
 		ipoctal->nb_bytes[i] = 0;
 		init_waitqueue_head(&ipoctal->queue[i]);
-		ipoctal->error_flag[i] = UART_NOERROR;
 
 		spin_lock_init(&ipoctal->lock[i]);
 		ipoctal->pointer_read[i] = 0;
@@ -547,8 +555,6 @@ static int ipoctal_write(struct ipoctal *ipoctal, unsigned int channel,
 
 	ipoctal_copy_write_buffer(ipoctal, channel, buf, count);
 
-	ipoctal->error_flag[channel] = UART_NOERROR;
-
 	/* As the IP-OCTAL 485 only supports half duplex, do it manually */
 	if (ipoctal->board_id == IP_OCTAL_485_ID) {
 		ipoctal_write_io_reg(ipoctal,
@@ -665,22 +671,18 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 		if (cflag & CRTSCTS) {
 			mr1 |= MR1_RxRTS_CONTROL_ON;
 			mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_ON;
-			ipoctal->chan_config[channel].flow_control = 1;
 		} else {
 			mr1 |= MR1_RxRTS_CONTROL_OFF;
 			mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_OFF;
-			ipoctal->chan_config[channel].flow_control = 0;
 		}
 		break;
 	case IP_OCTAL_422_ID:
 		mr1 |= MR1_RxRTS_CONTROL_OFF;
 		mr2 |= MR2_TxRTS_CONTROL_OFF | MR2_CTS_ENABLE_TX_OFF;
-		ipoctal->chan_config[channel].flow_control = 0;
 		break;
 	case IP_OCTAL_485_ID:
 		mr1 |= MR1_RxRTS_CONTROL_OFF;
 		mr2 |= MR2_TxRTS_CONTROL_ON | MR2_CTS_ENABLE_TX_OFF;
-		ipoctal->chan_config[channel].flow_control = 0;
 		break;
 	default:
 		return;
@@ -744,12 +746,6 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.mr, mr2);
 	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.csr, csr);
 
-	/* save the setup in the structure */
-	ipoctal->chan_config[channel].baud = tty_get_baud_rate(tty);
-	ipoctal->chan_config[channel].bits_per_char = cflag & CSIZE;
-	ipoctal->chan_config[channel].parity = cflag & PARENB;
-	ipoctal->chan_config[channel].stop_bits = cflag & CSTOPB;
-
 	/* Enable again the RX */
 	ipoctal_write_io_reg(ipoctal, &ipoctal->chan_regs[channel].u.w.cr,
 			     CR_ENABLE_RX);
@@ -853,11 +849,6 @@ static void __ipoctal_remove(struct ipoctal *ipoctal)
 
 	tty_unregister_driver(ipoctal->tty_drv);
 	put_tty_driver(ipoctal->tty_drv);
-
-	/* Tell the carrier board to free all the resources for this device */
-	if (ipoctal->dev->bus->ops->remove_device != NULL)
-		ipoctal->dev->bus->ops->remove_device(ipoctal->dev);
-
 	list_del(&ipoctal->list);
 	kfree(ipoctal);
 }
@@ -889,7 +880,7 @@ static void __exit ipoctal_exit(void)
 	struct ipoctal *p, *next;
 
 	list_for_each_entry_safe(p, next, &ipoctal_list, list)
-		__ipoctal_remove(p);
+		p->dev->bus->ops->remove_device(p->dev);
 
 	ipack_driver_unregister(&driver);
 }
diff --git a/drivers/staging/ipack/devices/ipoctal.h b/drivers/staging/ipack/devices/ipoctal.h
index 266f3617069..c5b4ed46516 100644
--- a/drivers/staging/ipack/devices/ipoctal.h
+++ b/drivers/staging/ipack/devices/ipoctal.h
@@ -20,45 +20,6 @@
 #define RELEVANT_IFLAG(iflag) ((iflag) & (IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK))
 
 /**
- * enum uart_parity_e - UART supported parity.
- */
-enum uart_parity_e {
-	UART_NONE  = 0,
-	UART_ODD   = 1,
-	UART_EVEN  = 2,
-};
-
-/**
- * enum uart_error - UART error type
- *
- */
-enum uart_error	{
-	UART_NOERROR = 0,      /* No error during transmission */
-	UART_TIMEOUT = 1 << 0, /* Timeout error */
-	UART_OVERRUN = 1 << 1, /* Overrun error */
-	UART_PARITY  = 1 << 2, /* Parity error */
-	UART_FRAMING = 1 << 3, /* Framing error */
-	UART_BREAK   = 1 << 4, /* Received break */
-};
-
-/**
- * struct ipoctal_config - Serial configuration
- *
- * @baud: Baud rate
- * @stop_bits: Stop bits (1 or 2)
- * @bits_per_char: data size in bits
- * @parity
- * @flow_control: Flow control management (RTS/CTS) (0 disabled, 1 enabled)
- */
-struct ipoctal_config {
-	unsigned int baud;
-	unsigned int stop_bits;
-	unsigned int bits_per_char;
-	unsigned short parity;
-	unsigned int flow_control;
-};
-
-/**
  * struct ipoctal_stats -- Stats since last reset
  *
  * @tx: Number of transmitted bytes
diff --git a/drivers/staging/ipack/ipack.c b/drivers/staging/ipack/ipack.c
index 2b4fa51bf16..c1cd97a4d9c 100644
--- a/drivers/staging/ipack/ipack.c
+++ b/drivers/staging/ipack/ipack.c
@@ -9,25 +9,15 @@
  * Software Foundation; version 2 of the License.
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
 #include <linux/module.h>
-#include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/idr.h>
 #include "ipack.h"
 
 #define to_ipack_dev(device) container_of(device, struct ipack_device, dev)
 #define to_ipack_driver(drv) container_of(drv, struct ipack_driver, driver)
 
-/* used when allocating bus numbers */
-#define IPACK_MAXBUS              64
-
-static DEFINE_MUTEX(ipack_mutex);
-
-struct ipack_busmap {
-	unsigned long busmap[IPACK_MAXBUS / (8*sizeof(unsigned long))];
-};
-static struct ipack_busmap busmap;
+static DEFINE_IDA(ipack_ida);
 
 static void ipack_device_release(struct device *dev)
 {
@@ -48,7 +38,7 @@ static int ipack_bus_match(struct device *device, struct device_driver *driver)
 	if (ret)
 		dev->driver = drv;
 
-	return 0;
+	return ret;
 }
 
 static int ipack_bus_probe(struct device *device)
@@ -79,26 +69,6 @@ static struct bus_type ipack_bus_type = {
 	.remove = ipack_bus_remove,
 };
 
-static int ipack_assign_bus_number(void)
-{
-	int busnum;
-
-	mutex_lock(&ipack_mutex);
-	busnum = find_next_zero_bit(busmap.busmap, IPACK_MAXBUS, 1);
-
-	if (busnum >= IPACK_MAXBUS) {
-		pr_err("too many buses\n");
-		busnum = -1;
-		goto error_find_busnum;
-	}
-
-	set_bit(busnum, busmap.busmap);
-
-error_find_busnum:
-	mutex_unlock(&ipack_mutex);
-	return busnum;
-}
-
 struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
 					    struct ipack_bus_ops *ops)
 {
@@ -109,7 +79,7 @@ struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
 	if (!bus)
 		return NULL;
 
-	bus_nr = ipack_assign_bus_number();
+	bus_nr = ida_simple_get(&ipack_ida, 0, 0, GFP_KERNEL);
 	if (bus_nr < 0) {
 		kfree(bus);
 		return NULL;
@@ -125,9 +95,7 @@ EXPORT_SYMBOL_GPL(ipack_bus_register);
 
 int ipack_bus_unregister(struct ipack_bus_device *bus)
 {
-	mutex_lock(&ipack_mutex);
-	clear_bit(bus->bus_nr, busmap.busmap);
-	mutex_unlock(&ipack_mutex);
+	ida_simple_remove(&ipack_ida, bus->bus_nr);
 	kfree(bus);
 	return 0;
 }
@@ -171,8 +139,6 @@ struct ipack_device *ipack_device_register(struct ipack_bus_device *bus,
 
 	ret = device_register(&dev->dev);
 	if (ret < 0) {
-		pr_err("error registering the device.\n");
-		dev->driver->ops->remove(dev);
 		kfree(dev);
 		return NULL;
 	}
@@ -189,12 +155,14 @@ EXPORT_SYMBOL_GPL(ipack_device_unregister);
 
 static int __init ipack_init(void)
 {
+	ida_init(&ipack_ida);
 	return bus_register(&ipack_bus_type);
 }
 
 static void __exit ipack_exit(void)
 {
 	bus_unregister(&ipack_bus_type);
+	ida_destroy(&ipack_ida);
 }
 
 module_init(ipack_init);
diff --git a/drivers/staging/keucr/init.c b/drivers/staging/keucr/init.c
index 071bdc23878..231611dc0f7 100644
--- a/drivers/staging/keucr/init.c
+++ b/drivers/staging/keucr/init.c
@@ -30,9 +30,8 @@ int ENE_InitMedia(struct us_data *us)
 	if (MiscReg03 & 0x02) {
 		if (!us->SM_Status.Ready && !us->MS_Status.Ready) {
 			result = ENE_SMInit(us);
-			if (result != USB_STOR_XFER_GOOD) {
+			if (result != USB_STOR_XFER_GOOD)
 				return USB_STOR_TRANSPORT_ERROR;
-			}
 		}
 
 	}
diff --git a/drivers/staging/keucr/scsiglue.c b/drivers/staging/keucr/scsiglue.c
index e1f3931d41e..083b20e6253 100644
--- a/drivers/staging/keucr/scsiglue.c
+++ b/drivers/staging/keucr/scsiglue.c
@@ -230,7 +230,10 @@ void usb_stor_report_bus_reset(struct us_data *us)
 /* we use this macro to help us write into the buffer */
 #undef SPRINTF
 #define SPRINTF(args...) \
-	do { if (pos < buffer+length) pos += sprintf(pos, ## args); } while (0)
+	do { \
+		if (pos < buffer+length) \
+			pos += sprintf(pos, ## args); \
+	} while (0)
 
 /*
  * proc_info()
@@ -279,8 +282,10 @@ static int proc_info(struct Scsi_Host *host, char *buffer, char **start,
 		pos += sprintf(pos, "       Quirks:");
 
 #define US_FLAG(name, value) \
-	if (us->fflags & value)\
-		pos += sprintf(pos, " " #name);
+	do { \
+		if (us->fflags & value) \
+			pos += sprintf(pos, " " #name); \
+	} while (0);
 US_DO_ALL_FLAGS
 #undef US_FLAG
 
diff --git a/drivers/staging/line6/control.c b/drivers/staging/line6/control.c
index 67e23b6e2d3..f8326f587e3 100644
--- a/drivers/staging/line6/control.c
+++ b/drivers/staging/line6/control.c
@@ -55,10 +55,10 @@ static ssize_t pod_set_param_int(struct device *dev, const char *buf,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
-	unsigned long value;
+	u8 value;
 	int retval;
 
-	retval = strict_strtoul(buf, 10, &value);
+	retval = kstrtou8(buf, 10, &value);
 	if (retval)
 		return retval;
 
diff --git a/drivers/staging/line6/driver.c b/drivers/staging/line6/driver.c
index 4513f78f112..b8358ca71bd 100644
--- a/drivers/staging/line6/driver.c
+++ b/drivers/staging/line6/driver.c
@@ -490,7 +490,7 @@ static void line6_data_received(struct urb *urb)
 /*
 	Send channel number (i.e., switch to a different sound).
 */
-int line6_send_program(struct usb_line6 *line6, int value)
+int line6_send_program(struct usb_line6 *line6, u8 value)
 {
 	int retval;
 	unsigned char *buffer;
@@ -526,7 +526,7 @@ int line6_send_program(struct usb_line6 *line6, int value)
 /*
 	Transmit Line6 control parameter.
 */
-int line6_transmit_parameter(struct usb_line6 *line6, int param, int value)
+int line6_transmit_parameter(struct usb_line6 *line6, int param, u8 value)
 {
 	int retval;
 	unsigned char *buffer;
diff --git a/drivers/staging/line6/driver.h b/drivers/staging/line6/driver.h
index 117bf994356..a3029eb223d 100644
--- a/drivers/staging/line6/driver.h
+++ b/drivers/staging/line6/driver.h
@@ -209,7 +209,7 @@ extern int line6_read_data(struct usb_line6 *line6, int address, void *data,
 			   size_t datalen);
 extern int line6_read_serial_number(struct usb_line6 *line6,
 				    int *serial_number);
-extern int line6_send_program(struct usb_line6 *line6, int value);
+extern int line6_send_program(struct usb_line6 *line6, u8 value);
 extern int line6_send_raw_message(struct usb_line6 *line6, const char *buffer,
 				  int size);
 extern int line6_send_raw_message_async(struct usb_line6 *line6,
@@ -224,7 +224,7 @@ extern void line6_start_timer(struct timer_list *timer, unsigned int msecs,
 			      void (*function) (unsigned long),
 			      unsigned long data);
 extern int line6_transmit_parameter(struct usb_line6 *line6, int param,
-				    int value);
+				    u8 value);
 extern int line6_version_request_async(struct usb_line6 *line6);
 extern int line6_write_data(struct usb_line6 *line6, int address, void *data,
 			    size_t datalen);
diff --git a/drivers/staging/line6/pod.c b/drivers/staging/line6/pod.c
index 4dadc571d96..9edd053fb9a 100644
--- a/drivers/staging/line6/pod.c
+++ b/drivers/staging/line6/pod.c
@@ -405,7 +405,7 @@ void line6_pod_midi_postprocess(struct usb_line6_pod *pod, unsigned char *data,
 /*
 	Send channel number (i.e., switch to a different sound).
 */
-static void pod_send_channel(struct usb_line6_pod *pod, int value)
+static void pod_send_channel(struct usb_line6_pod *pod, u8 value)
 {
 	line6_invalidate_current(&pod->dumpreq);
 
@@ -419,7 +419,7 @@ static void pod_send_channel(struct usb_line6_pod *pod, int value)
 	Transmit PODxt Pro control parameter.
 */
 void line6_pod_transmit_parameter(struct usb_line6_pod *pod, int param,
-				  int value)
+				  u8 value)
 {
 	if (line6_transmit_parameter(&pod->line6, param, value) == 0)
 		pod_store_parameter(pod, param, value);
@@ -434,11 +434,11 @@ void line6_pod_transmit_parameter(struct usb_line6_pod *pod, int param,
 static int pod_resolve(const char *buf, short block0, short block1,
 		       unsigned char *location)
 {
-	unsigned long value;
+	u8 value;
 	short block;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
@@ -560,10 +560,10 @@ static ssize_t pod_set_channel(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
-	unsigned long value;
+	u8 value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
@@ -892,10 +892,10 @@ static ssize_t pod_set_midi_postprocess(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
-	unsigned long value;
+	u8 value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/line6/pod.h b/drivers/staging/line6/pod.h
index 18b9d08c328..47e0d1a1c4b 100644
--- a/drivers/staging/line6/pod.h
+++ b/drivers/staging/line6/pod.h
@@ -200,6 +200,6 @@ extern void line6_pod_midi_postprocess(struct usb_line6_pod *pod,
 				       unsigned char *data, int length);
 extern void line6_pod_process_message(struct usb_line6_pod *pod);
 extern void line6_pod_transmit_parameter(struct usb_line6_pod *pod, int param,
-					 int value);
+					 u8 value);
 
 #endif
diff --git a/drivers/staging/line6/variax.c b/drivers/staging/line6/variax.c
index d36622228b2..bb99ee4919e 100644
--- a/drivers/staging/line6/variax.c
+++ b/drivers/staging/line6/variax.c
@@ -319,10 +319,10 @@ static ssize_t variax_set_volume(struct device *dev,
 {
 	struct usb_line6_variax *variax =
 	    usb_get_intfdata(to_usb_interface(dev));
-	unsigned long value;
+	u8 value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
@@ -418,10 +418,10 @@ static ssize_t variax_set_tone(struct device *dev,
 {
 	struct usb_line6_variax *variax =
 	    usb_get_intfdata(to_usb_interface(dev));
-	unsigned long value;
+	u8 value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou8(buf, 10, &value);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
index c365cdf714e..ebe5a27c06f 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ b/drivers/staging/media/dt3155v4l/dt3155v4l.c
@@ -971,20 +971,7 @@ static struct pci_driver pci_driver = {
 	.remove = __devexit_p(dt3155_remove),
 };
 
-static int __init
-dt3155_init_module(void)
-{
-	return pci_register_driver(&pci_driver);
-}
-
-static void __exit
-dt3155_exit_module(void)
-{
-	pci_unregister_driver(&pci_driver);
-}
-
-module_init(dt3155_init_module);
-module_exit(dt3155_exit_module);
+module_pci_driver(pci_driver);
 
 MODULE_DESCRIPTION("video4linux pci-driver for dt3155 frame grabber");
 MODULE_AUTHOR("Marin Mitov <mitov@issp.bas.bg>");
diff --git a/drivers/staging/media/easycap/easycap_main.c b/drivers/staging/media/easycap/easycap_main.c
index a1c45e4dcdc..8269c77dbf7 100644
--- a/drivers/staging/media/easycap/easycap_main.c
+++ b/drivers/staging/media/easycap/easycap_main.c
@@ -3083,6 +3083,7 @@ static int create_video_urbs(struct easycap *peasycap)
 		peasycap->allocation_video_urb += 1;
 		pdata_urb = kzalloc(sizeof(struct data_urb), GFP_KERNEL);
 		if (!pdata_urb) {
+			usb_free_urb(purb);
 			SAM("ERROR: Could not allocate struct data_urb.\n");
 			return -ENOMEM;
 		}
diff --git a/drivers/staging/media/go7007/wis-i2c.h b/drivers/staging/media/go7007/wis-i2c.h
index 3c2b9be455d..6d09c06c856 100644
--- a/drivers/staging/media/go7007/wis-i2c.h
+++ b/drivers/staging/media/go7007/wis-i2c.h
@@ -25,11 +25,6 @@
 #define	I2C_DRIVERID_WIS_TW2804		0xf0f6
 #define	I2C_DRIVERID_S2250		0xf0f7
 
-/* Flag to indicate that the client needs to be accessed with SCCB semantics */
-/* We re-use the I2C_M_TEN value so the flag passes through the masks in the
- * core I2C code.  Major kludge, but the I2C layer ain't exactly flexible. */
-#define	I2C_CLIENT_SCCB			0x10
-
 /* Definitions for new video decoder commands */
 
 struct video_decoder_resolution {
diff --git a/drivers/staging/media/lirc/lirc_bt829.c b/drivers/staging/media/lirc/lirc_bt829.c
index 4d20e9f7411..951007a3fc9 100644
--- a/drivers/staging/media/lirc/lirc_bt829.c
+++ b/drivers/staging/media/lirc/lirc_bt829.c
@@ -171,7 +171,7 @@ static void cycle_delay(int cycle)
 }
 
 
-static int poll_main()
+static int poll_main(void)
 {
 	unsigned char status_high, status_low;
 
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 945d9623550..4afc3b41973 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -52,6 +52,7 @@
 #include <linux/io.h>
 #include <asm/irq.h>
 #include <linux/fcntl.h>
+#include <linux/platform_device.h>
 #ifdef LIRC_ON_SA1100
 #include <asm/hardware.h>
 #ifdef CONFIG_SA1100_COLLIE
@@ -487,9 +488,11 @@ static struct lirc_driver driver = {
 	.owner		= THIS_MODULE,
 };
 
+static struct platform_device *lirc_sir_dev;
 
 static int init_chrdev(void)
 {
+	driver.dev = &lirc_sir_dev->dev;
 	driver.minor = lirc_register_driver(&driver);
 	if (driver.minor < 0) {
 		printk(KERN_ERR LIRC_DRIVER_NAME ": init_chrdev() failed.\n");
@@ -1215,20 +1218,71 @@ static int init_lirc_sir(void)
 	return 0;
 }
 
+static int __devinit lirc_sir_probe(struct platform_device *dev)
+{
+	return 0;
+}
+
+static int __devexit lirc_sir_remove(struct platform_device *dev)
+{
+	return 0;
+}
+
+static struct platform_driver lirc_sir_driver = {
+	.probe		= lirc_sir_probe,
+	.remove		= __devexit_p(lirc_sir_remove),
+	.driver		= {
+		.name	= "lirc_sir",
+		.owner	= THIS_MODULE,
+	},
+};
 
 static int __init lirc_sir_init(void)
 {
 	int retval;
 
+	retval = platform_driver_register(&lirc_sir_driver);
+	if (retval) {
+		printk(KERN_ERR LIRC_DRIVER_NAME ": Platform driver register "
+		       "failed!\n");
+		return -ENODEV;
+	}
+
+	lirc_sir_dev = platform_device_alloc("lirc_dev", 0);
+	if (!lirc_sir_dev) {
+		printk(KERN_ERR LIRC_DRIVER_NAME ": Platform device alloc "
+		       "failed!\n");
+		retval = -ENOMEM;
+		goto pdev_alloc_fail;
+	}
+
+	retval = platform_device_add(lirc_sir_dev);
+	if (retval) {
+		printk(KERN_ERR LIRC_DRIVER_NAME ": Platform device add "
+		       "failed!\n");
+		retval = -ENODEV;
+		goto pdev_add_fail;
+	}
+
 	retval = init_chrdev();
 	if (retval < 0)
-		return retval;
+		goto fail;
+
 	retval = init_lirc_sir();
 	if (retval) {
 		drop_chrdev();
-		return retval;
+		goto fail;
 	}
+
 	return 0;
+
+fail:
+	platform_device_del(lirc_sir_dev);
+pdev_add_fail:
+	platform_device_put(lirc_sir_dev);
+pdev_alloc_fail:
+	platform_driver_unregister(&lirc_sir_driver);
+	return retval;
 }
 
 static void __exit lirc_sir_exit(void)
@@ -1236,6 +1290,8 @@ static void __exit lirc_sir_exit(void)
 	drop_hardware();
 	drop_chrdev();
 	drop_port();
+	platform_device_unregister(lirc_sir_dev);
+	platform_driver_unregister(&lirc_sir_driver);
 	printk(KERN_INFO LIRC_DRIVER_NAME ": Uninstalled.\n");
 }
 
diff --git a/drivers/staging/media/solo6x10/TODO b/drivers/staging/media/solo6x10/TODO
index 7e6c4fa130d..539f739fe9e 100644
--- a/drivers/staging/media/solo6x10/TODO
+++ b/drivers/staging/media/solo6x10/TODO
@@ -20,5 +20,5 @@ TODO (general):
 	  - implement loopback of external sound jack with incoming audio?
 	  - implement pause/resume
 
-Plase send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc Ben Collins
+Plase send patches to Mauro Carvalho Chehab <mchehab@redhat.com> and Cc Ben Collins
 <bcollins@bluecherry.net>
diff --git a/drivers/staging/media/solo6x10/core.c b/drivers/staging/media/solo6x10/core.c
index d2fd842e37c..3ee9b125797 100644
--- a/drivers/staging/media/solo6x10/core.c
+++ b/drivers/staging/media/solo6x10/core.c
@@ -318,15 +318,4 @@ static struct pci_driver solo_pci_driver = {
 	.remove = solo_pci_remove,
 };
 
-static int __init solo_module_init(void)
-{
-	return pci_register_driver(&solo_pci_driver);
-}
-
-static void __exit solo_module_exit(void)
-{
-	pci_unregister_driver(&solo_pci_driver);
-}
-
-module_init(solo_module_init);
-module_exit(solo_module_exit);
+module_pci_driver(solo_pci_driver);
diff --git a/drivers/staging/media/solo6x10/i2c.c b/drivers/staging/media/solo6x10/i2c.c
index ef95a500b4d..398070a3d29 100644
--- a/drivers/staging/media/solo6x10/i2c.c
+++ b/drivers/staging/media/solo6x10/i2c.c
@@ -175,7 +175,7 @@ int solo_i2c_isr(struct solo_dev *solo_dev)
 
 	solo_reg_write(solo_dev, SOLO_IRQ_STAT, SOLO_IRQ_IIC);
 
-	if (status & (SOLO_IIC_STATE_TRNS & SOLO_IIC_STATE_SIG_ERR) ||
+	if (status & (SOLO_IIC_STATE_TRNS | SOLO_IIC_STATE_SIG_ERR) ||
 	    solo_dev->i2c_id < 0) {
 		solo_i2c_stop(solo_dev);
 		return -ENXIO;
diff --git a/drivers/staging/nvec/Kconfig b/drivers/staging/nvec/Kconfig
index 731301f524a..43048e9ba0d 100644
--- a/drivers/staging/nvec/Kconfig
+++ b/drivers/staging/nvec/Kconfig
@@ -26,8 +26,9 @@ config NVEC_POWER
 	  Say Y to enable support for battery and charger interface for
 	  nVidia compliant embedded controllers.
 
-config NVEC_LEDS
-	bool "NVEC leds"
-	depends on MFD_NVEC && LEDS_CLASS
+config NVEC_PAZ00
+	bool "Support for OEM specific functions on Compal PAZ00 based devices"
+	depends on MFD_NVEC && LEDS_CLASS && MACH_PAZ00
 	help
-	  Say Y to enable yellow side leds on AC100 or other nVidia tegra nvec leds
+	  Say Y to enable control of the yellow side leds on Compal PAZ00 based
+	  devices, e.g. Toshbia AC100 and Dynabooks AZ netbooks.
diff --git a/drivers/staging/nvec/Makefile b/drivers/staging/nvec/Makefile
index b844d604e3a..0db0e1f4333 100644
--- a/drivers/staging/nvec/Makefile
+++ b/drivers/staging/nvec/Makefile
@@ -2,4 +2,4 @@ obj-$(CONFIG_SERIO_NVEC_PS2)	+= nvec_ps2.o
 obj-$(CONFIG_MFD_NVEC)		+= nvec.o
 obj-$(CONFIG_NVEC_POWER) 	+= nvec_power.o
 obj-$(CONFIG_KEYBOARD_NVEC) 	+= nvec_kbd.o
-obj-$(CONFIG_NVEC_LEDS) 	+= nvec_leds.o
+obj-$(CONFIG_NVEC_PAZ00) 	+= nvec_paz00.o
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index 3c60088871e..695ea35f75b 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -97,7 +97,7 @@ static struct mfd_cell nvec_devices[] = {
 		.id = 2,
 	},
 	{
-		.name = "nvec-leds",
+		.name = "nvec-paz00",
 		.id = 1,
 	},
 };
@@ -127,12 +127,14 @@ EXPORT_SYMBOL_GPL(nvec_register_notifier);
 static int nvec_status_notifier(struct notifier_block *nb,
 				unsigned long event_type, void *data)
 {
+	struct nvec_chip *nvec = container_of(nb, struct nvec_chip,
+						nvec_status_notifier);
 	unsigned char *msg = (unsigned char *)data;
 
 	if (event_type != NVEC_CNTL)
 		return NOTIFY_DONE;
 
-	printk(KERN_WARNING "unhandled msg type %ld\n", event_type);
+	dev_warn(nvec->dev, "unhandled msg type %ld\n", event_type);
 	print_hex_dump(KERN_WARNING, "payload: ", DUMP_PREFIX_NONE, 16, 1,
 		msg, msg[1] + 2, true);
 
@@ -675,7 +677,7 @@ static void tegra_init_i2c_slave(struct nvec_chip *nvec)
 {
 	u32 val;
 
-	clk_enable(nvec->i2c_clk);
+	clk_prepare_enable(nvec->i2c_clk);
 
 	tegra_periph_reset_assert(nvec->i2c_clk);
 	udelay(2);
@@ -695,15 +697,17 @@ static void tegra_init_i2c_slave(struct nvec_chip *nvec)
 
 	enable_irq(nvec->irq);
 
-	clk_disable(nvec->i2c_clk);
+	clk_disable_unprepare(nvec->i2c_clk);
 }
 
+#ifdef CONFIG_PM_SLEEP
 static void nvec_disable_i2c_slave(struct nvec_chip *nvec)
 {
 	disable_irq(nvec->irq);
 	writel(I2C_SL_NEWSL | I2C_SL_NACK, nvec->base + I2C_SL_CNFG);
-	clk_disable(nvec->i2c_clk);
+	clk_disable_unprepare(nvec->i2c_clk);
 }
+#endif
 
 static void nvec_power_off(void)
 {
@@ -719,10 +723,9 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	struct nvec_chip *nvec;
 	struct nvec_msg *msg;
 	struct resource *res;
-	struct resource *iomem;
 	void __iomem *base;
 
-	nvec = kzalloc(sizeof(struct nvec_chip), GFP_KERNEL);
+	nvec = devm_kzalloc(&pdev->dev, sizeof(struct nvec_chip), GFP_KERNEL);
 	if (nvec == NULL) {
 		dev_err(&pdev->dev, "failed to reserve memory\n");
 		return -ENOMEM;
@@ -737,15 +740,15 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 		nvec->gpio = of_get_named_gpio(nvec->dev->of_node, "request-gpios", 0);
 		if (nvec->gpio < 0) {
 			dev_err(&pdev->dev, "no gpio specified");
-			goto failed;
+			return -ENODEV;
 		}
 		if (of_property_read_u32(nvec->dev->of_node, "slave-addr", &nvec->i2c_addr)) {
 			dev_err(&pdev->dev, "no i2c address specified");
-			goto failed;
+			return -ENODEV;
 		}
 	} else {
 		dev_err(&pdev->dev, "no platform data\n");
-		goto failed;
+		return -ENODEV;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -754,13 +757,7 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	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));
+	base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!base) {
 		dev_err(&pdev->dev, "Can't ioremap I2C region\n");
 		return -ENOMEM;
@@ -769,14 +766,13 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no irq resource?\n");
-		ret = -ENODEV;
-		goto err_iounmap;
+		return -ENODEV;
 	}
 
 	i2c_clk = clk_get_sys("tegra-i2c.2", NULL);
 	if (IS_ERR(i2c_clk)) {
 		dev_err(nvec->dev, "failed to get controller clock\n");
-		goto err_iounmap;
+		return -ENODEV;
 	}
 
 	nvec->base = base;
@@ -797,22 +793,26 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	INIT_WORK(&nvec->tx_work, nvec_request_master);
 	nvec->wq = alloc_workqueue("nvec", WQ_NON_REENTRANT, 2);
 
-	err = gpio_request_one(nvec->gpio, GPIOF_OUT_INIT_HIGH, "nvec gpio");
+	err = devm_gpio_request_one(&pdev->dev, nvec->gpio, GPIOF_OUT_INIT_HIGH,
+					"nvec gpio");
 	if (err < 0) {
 		dev_err(nvec->dev, "couldn't request gpio\n");
-		goto failed;
+		destroy_workqueue(nvec->wq);
+		return -ENODEV;
 	}
 
-	err = request_irq(nvec->irq, nvec_interrupt, 0, "nvec", nvec);
+	err = devm_request_irq(&pdev->dev, nvec->irq, nvec_interrupt, 0,
+				"nvec", nvec);
 	if (err) {
 		dev_err(nvec->dev, "couldn't request irq\n");
-		goto failed;
+		destroy_workqueue(nvec->wq);
+		return -ENODEV;
 	}
 	disable_irq(nvec->irq);
 
 	tegra_init_i2c_slave(nvec);
 
-	clk_enable(i2c_clk);
+	clk_prepare_enable(i2c_clk);
 
 
 	/* enable event reporting */
@@ -851,12 +851,6 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	nvec_write_async(nvec, "\x01\x01\x01\x00\x00\x80\x00", 7);
 
 	return 0;
-
-err_iounmap:
-	iounmap(base);
-failed:
-	kfree(nvec);
-	return -ENOMEM;
 }
 
 static int __devexit tegra_nvec_remove(struct platform_device *pdev)
@@ -865,19 +859,15 @@ static int __devexit tegra_nvec_remove(struct platform_device *pdev)
 
 	nvec_write_async(nvec, EC_DISABLE_EVENT_REPORTING, 3);
 	mfd_remove_devices(nvec->dev);
-	free_irq(nvec->irq, &nvec_interrupt);
-	iounmap(nvec->base);
-	gpio_free(nvec->gpio);
 	destroy_workqueue(nvec->wq);
-	kfree(nvec);
 
 	return 0;
 }
 
-#ifdef CONFIG_PM
-
-static int tegra_nvec_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int nvec_suspend(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct nvec_chip *nvec = platform_get_drvdata(pdev);
 	struct nvec_msg *msg;
 
@@ -894,8 +884,9 @@ static int tegra_nvec_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int tegra_nvec_resume(struct platform_device *pdev)
+static int nvec_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct nvec_chip *nvec = platform_get_drvdata(pdev);
 
 	dev_dbg(nvec->dev, "resuming\n");
@@ -904,12 +895,10 @@ static int tegra_nvec_resume(struct platform_device *pdev)
 
 	return 0;
 }
-
-#else
-#define tegra_nvec_suspend NULL
-#define tegra_nvec_resume NULL
 #endif
 
+static const SIMPLE_DEV_PM_OPS(nvec_pm_ops, nvec_suspend, nvec_resume);
+
 /* Match table for of_platform binding */
 static const struct of_device_id nvidia_nvec_of_match[] __devinitconst = {
 	{ .compatible = "nvidia,nvec", },
@@ -920,21 +909,15 @@ MODULE_DEVICE_TABLE(of, nvidia_nvec_of_match);
 static struct platform_driver nvec_device_driver = {
 	.probe   = tegra_nvec_probe,
 	.remove  = __devexit_p(tegra_nvec_remove),
-	.suspend = tegra_nvec_suspend,
-	.resume  = tegra_nvec_resume,
 	.driver  = {
 		.name = "nvec",
 		.owner = THIS_MODULE,
+		.pm = &nvec_pm_ops,
 		.of_match_table = nvidia_nvec_of_match,
 	}
 };
 
-static int __init tegra_nvec_init(void)
-{
-	return platform_driver_register(&nvec_device_driver);
-}
-
-module_init(tegra_nvec_init);
+module_platform_driver(nvec_device_driver);
 
 MODULE_ALIAS("platform:nvec");
 MODULE_DESCRIPTION("NVIDIA compliant embedded controller interface");
diff --git a/drivers/staging/nvec/nvec_kbd.c b/drivers/staging/nvec/nvec_kbd.c
index a4ce5a740e2..6cc30dcd830 100644
--- a/drivers/staging/nvec/nvec_kbd.c
+++ b/drivers/staging/nvec/nvec_kbd.c
@@ -159,20 +159,24 @@ fail:
 	return err;
 }
 
+static int __devexit nvec_kbd_remove(struct platform_device *pdev)
+{
+	input_unregister_device(keys_dev.input);
+	input_free_device(keys_dev.input);
+
+	return 0;
+}
+
 static struct platform_driver nvec_kbd_driver = {
 	.probe  = nvec_kbd_probe,
+	.remove = __devexit_p(nvec_kbd_remove),
 	.driver = {
 		.name = "nvec-kbd",
 		.owner = THIS_MODULE,
 	},
 };
 
-static int __init nvec_kbd_init(void)
-{
-	return platform_driver_register(&nvec_kbd_driver);
-}
-
-module_init(nvec_kbd_init);
+module_platform_driver(nvec_kbd_driver);
 
 MODULE_AUTHOR("Marc Dietrich <marvin24@gmx.de>");
 MODULE_DESCRIPTION("NVEC keyboard driver");
diff --git a/drivers/staging/nvec/nvec_leds.c b/drivers/staging/nvec/nvec_paz00.c
index f4cbcd62500..b747e39ff94 100644
--- a/drivers/staging/nvec/nvec_leds.c
+++ b/drivers/staging/nvec/nvec_paz00.c
@@ -1,5 +1,5 @@
 /*
- * nvec_leds: LED driver for a NVIDIA compliant embedded controller
+ * nvec_paz00: OEM specific driver for Compal PAZ00 based devices
  *
  * Copyright (C) 2011 The AC100 Kernel Team <ac100@lists.launchpad.net>
  *
@@ -43,20 +43,20 @@ static void nvec_led_brightness_set(struct led_classdev *led_cdev,
 
 }
 
-static int __devinit nvec_led_probe(struct platform_device *pdev)
+static int __devinit nvec_paz00_probe(struct platform_device *pdev)
 {
 	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
 	struct nvec_led *led;
 	int ret = 0;
 
-	led = kzalloc(sizeof(*led), GFP_KERNEL);
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
 	if (led == NULL)
 		return -ENOMEM;
 
 	led->cdev.max_brightness = NVEC_LED_MAX;
 
 	led->cdev.brightness_set = nvec_led_brightness_set;
-	led->cdev.name = "nvec-led";
+	led->cdev.name = "paz00-led";
 	led->cdev.flags |= LED_CORE_SUSPENDRESUME;
 	led->nvec = nvec;
 
@@ -64,51 +64,35 @@ static int __devinit nvec_led_probe(struct platform_device *pdev)
 
 	ret = led_classdev_register(&pdev->dev, &led->cdev);
 	if (ret < 0)
-		goto err_led;
+		return ret;
 
 	/* to expose the default value to userspace */
 	led->cdev.brightness = 0;
 
 	return 0;
-
-err_led:
-	kfree(led);
-	return ret;
 }
 
-static int __devexit nvec_led_remove(struct platform_device *pdev)
+static int __devexit nvec_paz00_remove(struct platform_device *pdev)
 {
 	struct nvec_led *led = platform_get_drvdata(pdev);
 
 	led_classdev_unregister(&led->cdev);
-	kfree(led);
+
 	return 0;
 }
 
-static struct platform_driver nvec_led_driver = {
-	.probe  = nvec_led_probe,
-	.remove = __devexit_p(nvec_led_remove),
+static struct platform_driver nvec_paz00_driver = {
+	.probe  = nvec_paz00_probe,
+	.remove = __devexit_p(nvec_paz00_remove),
 	.driver = {
-		   .name  = "nvec-leds",
-		   .owner = THIS_MODULE,
+		.name  = "nvec-paz00",
+		.owner = THIS_MODULE,
 	},
 };
 
-static int __init nvec_led_init(void)
-{
-	return platform_driver_register(&nvec_led_driver);
-}
-
-module_init(nvec_led_init);
-
-static void __exit nvec_led_exit(void)
-{
-	platform_driver_unregister(&nvec_led_driver);
-}
-
-module_exit(nvec_led_exit);
+module_platform_driver(nvec_paz00_driver);
 
 MODULE_AUTHOR("Ilya Petrov <ilya.muromec@gmail.com>");
-MODULE_DESCRIPTION("Tegra NVEC LED driver");
+MODULE_DESCRIPTION("Tegra NVEC PAZ00 driver");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:nvec-leds");
+MODULE_ALIAS("platform:nvec-paz00");
diff --git a/drivers/staging/nvec/nvec_power.c b/drivers/staging/nvec/nvec_power.c
index dfa966f6189..cc8ccd75e7f 100644
--- a/drivers/staging/nvec/nvec_power.c
+++ b/drivers/staging/nvec/nvec_power.c
@@ -371,10 +371,13 @@ static void nvec_power_poll(struct work_struct *work)
 static int __devinit nvec_power_probe(struct platform_device *pdev)
 {
 	struct power_supply *psy;
-	struct nvec_power *power =
-	    kzalloc(sizeof(struct nvec_power), GFP_NOWAIT);
+	struct nvec_power *power;
 	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
 
+	power = devm_kzalloc(&pdev->dev, sizeof(struct nvec_power), GFP_NOWAIT);
+	if (power == NULL)
+		return -ENOMEM;
+
 	dev_set_drvdata(&pdev->dev, power);
 	power->nvec = nvec;
 
@@ -393,7 +396,6 @@ static int __devinit nvec_power_probe(struct platform_device *pdev)
 		power->notifier.notifier_call = nvec_power_bat_notifier;
 		break;
 	default:
-		kfree(power);
 		return -ENODEV;
 	}
 
@@ -405,20 +407,32 @@ static int __devinit nvec_power_probe(struct platform_device *pdev)
 	return power_supply_register(&pdev->dev, psy);
 }
 
+static int __devexit nvec_power_remove(struct platform_device *pdev)
+{
+	struct nvec_power *power = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&power->poller);
+	switch (pdev->id) {
+	case AC:
+		power_supply_unregister(&nvec_psy);
+		break;
+	case BAT:
+		power_supply_unregister(&nvec_bat_psy);
+	}
+
+	return 0;
+}
+
 static struct platform_driver nvec_power_driver = {
 	.probe = nvec_power_probe,
+	.remove = __devexit_p(nvec_power_remove),
 	.driver = {
 		   .name = "nvec-power",
 		   .owner = THIS_MODULE,
 		   }
 };
 
-static int __init nvec_power_init(void)
-{
-	return platform_driver_register(&nvec_power_driver);
-}
-
-module_init(nvec_power_init);
+module_platform_driver(nvec_power_driver);
 
 MODULE_AUTHOR("Ilya Petrov <ilya.muromec@gmail.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/nvec/nvec_ps2.c b/drivers/staging/nvec/nvec_ps2.c
index 14a6f687cf7..d7c65110213 100644
--- a/drivers/staging/nvec/nvec_ps2.c
+++ b/drivers/staging/nvec/nvec_ps2.c
@@ -96,7 +96,11 @@ static int nvec_ps2_notifier(struct notifier_block *nb,
 static int __devinit nvec_mouse_probe(struct platform_device *pdev)
 {
 	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
-	struct serio *ser_dev = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	struct serio *ser_dev;
+
+	ser_dev = devm_kzalloc(&pdev->dev, sizeof(struct serio), GFP_KERNEL);
+	if (ser_dev == NULL)
+		return -ENOMEM;
 
 	ser_dev->id.type = SERIO_PS_PSTHRU;
 	ser_dev->write = ps2_sendcommand;
@@ -119,8 +123,17 @@ static int __devinit nvec_mouse_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int nvec_mouse_suspend(struct platform_device *pdev, pm_message_t state)
+static int __devexit nvec_mouse_remove(struct platform_device *pdev)
+{
+	serio_unregister_port(ps2_dev.ser_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int nvec_mouse_suspend(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
 
 	/* disable mouse */
@@ -132,8 +145,9 @@ static int nvec_mouse_suspend(struct platform_device *pdev, pm_message_t state)
 	return 0;
 }
 
-static int nvec_mouse_resume(struct platform_device *pdev)
+static int nvec_mouse_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct nvec_chip *nvec = dev_get_drvdata(pdev->dev.parent);
 
 	ps2_startstreaming(ps2_dev.ser_dev);
@@ -143,23 +157,22 @@ static int nvec_mouse_resume(struct platform_device *pdev)
 
 	return 0;
 }
+#endif
+
+static const SIMPLE_DEV_PM_OPS(nvec_mouse_pm_ops, nvec_mouse_suspend,
+				nvec_mouse_resume);
 
 static struct platform_driver nvec_mouse_driver = {
 	.probe  = nvec_mouse_probe,
-	.suspend = nvec_mouse_suspend,
-	.resume = nvec_mouse_resume,
+	.remove = __devexit_p(nvec_mouse_remove),
 	.driver = {
 		.name = "nvec-mouse",
 		.owner = THIS_MODULE,
+		.pm = &nvec_mouse_pm_ops,
 	},
 };
 
-static int __init nvec_mouse_init(void)
-{
-	return platform_driver_register(&nvec_mouse_driver);
-}
-
-module_init(nvec_mouse_init);
+module_platform_driver(nvec_mouse_driver);
 
 MODULE_DESCRIPTION("NVEC mouse driver");
 MODULE_AUTHOR("Marc Dietrich <marvin24@gmx.de>");
diff --git a/drivers/staging/octeon/ethernet-mdio.c b/drivers/staging/octeon/ethernet-mdio.c
index e31949c9c87..f15b31b37ca 100644
--- a/drivers/staging/octeon/ethernet-mdio.c
+++ b/drivers/staging/octeon/ethernet-mdio.c
@@ -28,6 +28,7 @@
 #include <linux/ethtool.h>
 #include <linux/phy.h>
 #include <linux/ratelimit.h>
+#include <linux/of_mdio.h>
 
 #include <net/dst.h>
 
@@ -161,22 +162,23 @@ static void cvm_oct_adjust_link(struct net_device *dev)
 int cvm_oct_phy_setup_device(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
+	struct device_node *phy_node;
 
-	int phy_addr = cvmx_helper_board_get_mii_address(priv->port);
-	if (phy_addr != -1) {
-		char phy_id[MII_BUS_ID_SIZE + 3];
+	if (!priv->of_node)
+		return 0;
 
-		snprintf(phy_id, sizeof(phy_id), PHY_ID_FMT, "mdio-octeon-0", phy_addr);
+	phy_node = of_parse_phandle(priv->of_node, "phy-handle", 0);
+	if (!phy_node)
+		return 0;
 
-		priv->phydev = phy_connect(dev, phy_id, cvm_oct_adjust_link, 0,
-					PHY_INTERFACE_MODE_GMII);
+	priv->phydev = of_phy_connect(dev, phy_node, cvm_oct_adjust_link, 0,
+				      PHY_INTERFACE_MODE_GMII);
+
+	if (priv->phydev == NULL)
+		return -ENODEV;
+
+	priv->last_link = 0;
+	phy_start_aneg(priv->phydev);
 
-		if (IS_ERR(priv->phydev)) {
-			priv->phydev = NULL;
-			return -1;
-		}
-		priv->last_link = 0;
-		phy_start_aneg(priv->phydev);
-	}
 	return 0;
 }
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
index 18f7a790f73..683bedc74dd 100644
--- a/drivers/staging/octeon/ethernet.c
+++ b/drivers/staging/octeon/ethernet.c
@@ -24,6 +24,7 @@
  * This file may also be available under a different license from Cavium.
  * Contact Cavium Networks for more information
 **********************************************************************/
+#include <linux/platform_device.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/module.h>
@@ -32,6 +33,7 @@
 #include <linux/phy.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
+#include <linux/of_net.h>
 
 #include <net/dst.h>
 
@@ -113,15 +115,6 @@ int rx_napi_weight = 32;
 module_param(rx_napi_weight, int, 0444);
 MODULE_PARM_DESC(rx_napi_weight, "The NAPI WEIGHT parameter.");
 
-/*
- * The offset from mac_addr_base that should be used for the next port
- * that is configured.  By convention, if any mgmt ports exist on the
- * chip, they get the first mac addresses, The ports controlled by
- * this driver are numbered sequencially following any mgmt addresses
- * that may exist.
- */
-static unsigned int cvm_oct_mac_addr_offset;
-
 /**
  * cvm_oct_poll_queue - Workqueue for polling operations.
  */
@@ -176,7 +169,7 @@ static void cvm_oct_periodic_worker(struct work_struct *work)
 		queue_delayed_work(cvm_oct_poll_queue, &priv->port_periodic_work, HZ);
  }
 
-static __init void cvm_oct_configure_common_hw(void)
+static __devinit void cvm_oct_configure_common_hw(void)
 {
 	/* Setup the FPA */
 	cvmx_fpa_enable();
@@ -396,23 +389,21 @@ static void cvm_oct_common_set_multicast_list(struct net_device *dev)
 
  * Returns Zero on success
  */
-static int cvm_oct_common_set_mac_address(struct net_device *dev, void *addr)
+static int cvm_oct_set_mac_filter(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
 	union cvmx_gmxx_prtx_cfg gmx_cfg;
 	int interface = INTERFACE(priv->port);
 	int index = INDEX(priv->port);
 
-	memcpy(dev->dev_addr, addr + 2, 6);
-
 	if ((interface < 2)
 	    && (cvmx_helper_interface_get_mode(interface) !=
 		CVMX_HELPER_INTERFACE_MODE_SPI)) {
 		int i;
-		uint8_t *ptr = addr;
+		uint8_t *ptr = dev->dev_addr;
 		uint64_t mac = 0;
 		for (i = 0; i < 6; i++)
-			mac = (mac << 8) | (uint64_t) (ptr[i + 2]);
+			mac = (mac << 8) | (uint64_t)ptr[i];
 
 		gmx_cfg.u64 =
 		    cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
@@ -421,17 +412,17 @@ static int cvm_oct_common_set_mac_address(struct net_device *dev, void *addr)
 
 		cvmx_write_csr(CVMX_GMXX_SMACX(index, interface), mac);
 		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM0(index, interface),
-			       ptr[2]);
+			       ptr[0]);
 		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM1(index, interface),
-			       ptr[3]);
+			       ptr[1]);
 		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM2(index, interface),
-			       ptr[4]);
+			       ptr[2]);
 		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM3(index, interface),
-			       ptr[5]);
+			       ptr[3]);
 		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM4(index, interface),
-			       ptr[6]);
+			       ptr[4]);
 		cvmx_write_csr(CVMX_GMXX_RXX_ADR_CAM5(index, interface),
-			       ptr[7]);
+			       ptr[5]);
 		cvm_oct_common_set_multicast_list(dev);
 		cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface),
 			       gmx_cfg.u64);
@@ -439,6 +430,15 @@ static int cvm_oct_common_set_mac_address(struct net_device *dev, void *addr)
 	return 0;
 }
 
+static int cvm_oct_common_set_mac_address(struct net_device *dev, void *addr)
+{
+	int r = eth_mac_addr(dev, addr);
+
+	if (r)
+		return r;
+	return cvm_oct_set_mac_filter(dev);
+}
+
 /**
  * cvm_oct_common_init - per network device initialization
  * @dev:    Device to initialize
@@ -448,26 +448,17 @@ static int cvm_oct_common_set_mac_address(struct net_device *dev, void *addr)
 int cvm_oct_common_init(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
-	struct sockaddr sa;
-	u64 mac = ((u64)(octeon_bootinfo->mac_addr_base[0] & 0xff) << 40) |
-		((u64)(octeon_bootinfo->mac_addr_base[1] & 0xff) << 32) |
-		((u64)(octeon_bootinfo->mac_addr_base[2] & 0xff) << 24) |
-		((u64)(octeon_bootinfo->mac_addr_base[3] & 0xff) << 16) |
-		((u64)(octeon_bootinfo->mac_addr_base[4] & 0xff) << 8) |
-		(u64)(octeon_bootinfo->mac_addr_base[5] & 0xff);
-
-	mac += cvm_oct_mac_addr_offset;
-	sa.sa_data[0] = (mac >> 40) & 0xff;
-	sa.sa_data[1] = (mac >> 32) & 0xff;
-	sa.sa_data[2] = (mac >> 24) & 0xff;
-	sa.sa_data[3] = (mac >> 16) & 0xff;
-	sa.sa_data[4] = (mac >> 8) & 0xff;
-	sa.sa_data[5] = mac & 0xff;
-
-	if (cvm_oct_mac_addr_offset >= octeon_bootinfo->mac_addr_count)
-		printk(KERN_DEBUG "%s: Using MAC outside of the assigned range:"
-			" %pM\n", dev->name, sa.sa_data);
-	cvm_oct_mac_addr_offset++;
+	const u8 *mac = NULL;
+
+	if (priv->of_node)
+		mac = of_get_mac_address(priv->of_node);
+
+	if (mac && is_valid_ether_addr(mac)) {
+		memcpy(dev->dev_addr, mac, ETH_ALEN);
+		dev->addr_assign_type &= ~NET_ADDR_RANDOM;
+	} else {
+		eth_hw_addr_random(dev);
+	}
 
 	/*
 	 * Force the interface to use the POW send if always_use_pow
@@ -488,7 +479,7 @@ int cvm_oct_common_init(struct net_device *dev)
 	SET_ETHTOOL_OPS(dev, &cvm_oct_ethtool_ops);
 
 	cvm_oct_phy_setup_device(dev);
-	dev->netdev_ops->ndo_set_mac_address(dev, &sa);
+	cvm_oct_set_mac_filter(dev);
 	dev->netdev_ops->ndo_change_mtu(dev, dev->mtu);
 
 	/*
@@ -595,22 +586,55 @@ static const struct net_device_ops cvm_oct_pow_netdev_ops = {
 
 extern void octeon_mdiobus_force_mod_depencency(void);
 
-static int __init cvm_oct_init_module(void)
+static struct device_node * __devinit cvm_oct_of_get_child(const struct device_node *parent,
+							   int reg_val)
+{
+	struct device_node *node = NULL;
+	int size;
+	const __be32 *addr;
+
+	for (;;) {
+		node = of_get_next_child(parent, node);
+		if (!node)
+			break;
+		addr = of_get_property(node, "reg", &size);
+		if (addr && (be32_to_cpu(*addr) == reg_val))
+			break;
+	}
+	return node;
+}
+
+static struct device_node * __devinit cvm_oct_node_for_port(struct device_node *pip,
+							    int interface, int port)
+{
+	struct device_node *ni, *np;
+
+	ni = cvm_oct_of_get_child(pip, interface);
+	if (!ni)
+		return NULL;
+
+	np = cvm_oct_of_get_child(ni, port);
+	of_node_put(ni);
+
+	return np;
+}
+
+static int __devinit cvm_oct_probe(struct platform_device *pdev)
 {
 	int num_interfaces;
 	int interface;
 	int fau = FAU_NUM_PACKET_BUFFERS_TO_FREE;
 	int qos;
+	struct device_node *pip;
 
 	octeon_mdiobus_force_mod_depencency();
 	pr_notice("cavium-ethernet %s\n", OCTEON_ETHERNET_VERSION);
 
-	if (OCTEON_IS_MODEL(OCTEON_CN52XX))
-		cvm_oct_mac_addr_offset = 2; /* First two are the mgmt ports. */
-	else if (OCTEON_IS_MODEL(OCTEON_CN56XX))
-		cvm_oct_mac_addr_offset = 1; /* First one is the mgmt port. */
-	else
-		cvm_oct_mac_addr_offset = 0;
+	pip = pdev->dev.of_node;
+	if (!pip) {
+		pr_err("Error: No 'pip' in /aliases\n");
+		return -EINVAL;
+	}
 
 	cvm_oct_poll_queue = create_singlethread_workqueue("octeon-ethernet");
 	if (cvm_oct_poll_queue == NULL) {
@@ -689,10 +713,11 @@ static int __init cvm_oct_init_module(void)
 		    cvmx_helper_interface_get_mode(interface);
 		int num_ports = cvmx_helper_ports_on_interface(interface);
 		int port;
+		int port_index;
 
-		for (port = cvmx_helper_get_ipd_port(interface, 0);
+		for (port_index = 0, port = cvmx_helper_get_ipd_port(interface, 0);
 		     port < cvmx_helper_get_ipd_port(interface, num_ports);
-		     port++) {
+		     port_index++, port++) {
 			struct octeon_ethernet *priv;
 			struct net_device *dev =
 			    alloc_etherdev(sizeof(struct octeon_ethernet));
@@ -703,6 +728,7 @@ static int __init cvm_oct_init_module(void)
 
 			/* Initialize the device private structure. */
 			priv = netdev_priv(dev);
+			priv->of_node = cvm_oct_node_for_port(pip, interface, port_index);
 
 			INIT_DELAYED_WORK(&priv->port_periodic_work,
 					  cvm_oct_periodic_worker);
@@ -787,7 +813,7 @@ static int __init cvm_oct_init_module(void)
 	return 0;
 }
 
-static void __exit cvm_oct_cleanup_module(void)
+static int __devexit cvm_oct_remove(struct platform_device *pdev)
 {
 	int port;
 
@@ -835,10 +861,29 @@ static void __exit cvm_oct_cleanup_module(void)
 	if (CVMX_FPA_OUTPUT_BUFFER_POOL != CVMX_FPA_PACKET_POOL)
 		cvm_oct_mem_empty_fpa(CVMX_FPA_OUTPUT_BUFFER_POOL,
 				      CVMX_FPA_OUTPUT_BUFFER_POOL_SIZE, 128);
+	return 0;
 }
 
+static struct of_device_id cvm_oct_match[] = {
+	{
+		.compatible = "cavium,octeon-3860-pip",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, cvm_oct_match);
+
+static struct platform_driver cvm_oct_driver = {
+	.probe		= cvm_oct_probe,
+	.remove		= __devexit_p(cvm_oct_remove),
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= KBUILD_MODNAME,
+		.of_match_table = cvm_oct_match,
+	},
+};
+
+module_platform_driver(cvm_oct_driver);
+
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Cavium Networks <support@caviumnetworks.com>");
 MODULE_DESCRIPTION("Cavium Networks Octeon ethernet driver.");
-module_init(cvm_oct_init_module);
-module_exit(cvm_oct_cleanup_module);
diff --git a/drivers/staging/octeon/octeon-ethernet.h b/drivers/staging/octeon/octeon-ethernet.h
index d5819256355..9360e22e073 100644
--- a/drivers/staging/octeon/octeon-ethernet.h
+++ b/drivers/staging/octeon/octeon-ethernet.h
@@ -31,6 +31,8 @@
 #ifndef OCTEON_ETHERNET_H
 #define OCTEON_ETHERNET_H
 
+#include <linux/of.h>
+
 /**
  * This is the definition of the Ethernet driver's private
  * driver state stored in netdev_priv(dev).
@@ -59,6 +61,7 @@ struct octeon_ethernet {
 	void (*poll) (struct net_device *dev);
 	struct delayed_work	port_periodic_work;
 	struct work_struct	port_work;	/* may be unused. */
+	struct device_node	*of_node;
 };
 
 int cvm_oct_free_work(void *work_queue_entry);
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 992275c0d87..2c4bd746715 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -27,6 +27,7 @@
 #include <linux/uaccess.h>
 #include <linux/ctype.h>
 #include <linux/reboot.h>
+#include <linux/olpc-ec.h>
 #include <asm/tsc.h>
 #include <asm/olpc.h>
 
diff --git a/drivers/staging/omap-thermal/Kconfig b/drivers/staging/omap-thermal/Kconfig
new file mode 100644
index 00000000000..30cbc3bc8df
--- /dev/null
+++ b/drivers/staging/omap-thermal/Kconfig
@@ -0,0 +1,46 @@
+config OMAP_BANDGAP
+	tristate "Texas Instruments OMAP4+ temperature sensor driver"
+	depends on THERMAL
+	depends on ARCH_OMAP4 || SOC_OMAP5
+	help
+	  If you say yes here you get support for the Texas Instruments
+	  OMAP4460+ on die bandgap temperature sensor support. The register
+	  set is part of system control module.
+
+	  This includes alert interrupts generation and also the TSHUT
+	  support.
+
+config OMAP_THERMAL
+	bool "Texas Instruments OMAP4+ thermal framework support"
+	depends on OMAP_BANDGAP
+	depends on CPU_THERMAL
+	help
+	  If you say yes here you want to get support for generic thermal
+	  framework for the Texas Instruments OMAP4460+ on die bandgap
+	  temperature sensor.
+
+config OMAP4_THERMAL
+	bool "Texas Instruments OMAP4 thermal support"
+	depends on OMAP_BANDGAP
+	depends on ARCH_OMAP4
+	help
+	  If you say yes here you get thermal support for the Texas Instruments
+	  OMAP4 SoC family. The current chip supported are:
+	   - OMAP4430
+	   - OMAP4460
+	   - OMAP4470
+
+	  This includes alert interrupts generation and also the TSHUT
+	  support.
+
+config OMAP5_THERMAL
+	bool "Texas Instruments OMAP5 thermal support"
+	depends on OMAP_BANDGAP
+	depends on SOC_OMAP5
+	help
+	  If you say yes here you get thermal support for the Texas Instruments
+	  OMAP5 SoC family. The current chip supported are:
+	   - OMAP5430
+
+	  This includes alert interrupts generation and also the TSHUT
+	  support.
diff --git a/drivers/staging/omap-thermal/Makefile b/drivers/staging/omap-thermal/Makefile
new file mode 100644
index 00000000000..091c4d20b14
--- /dev/null
+++ b/drivers/staging/omap-thermal/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_OMAP_BANDGAP)	+= omap-thermal.o
+omap-thermal-y			:= omap-bandgap.o
+omap-thermal-$(CONFIG_OMAP_THERMAL)	+= omap-thermal-common.o
+omap-thermal-$(CONFIG_OMAP4_THERMAL)	+= omap4-thermal.o
+omap-thermal-$(CONFIG_OMAP5_THERMAL)	+= omap5-thermal.o
diff --git a/drivers/staging/omap-thermal/TODO b/drivers/staging/omap-thermal/TODO
new file mode 100644
index 00000000000..9e23cc4d551
--- /dev/null
+++ b/drivers/staging/omap-thermal/TODO
@@ -0,0 +1,28 @@
+List of TODOs (by Eduardo Valentin)
+
+on omap-bandgap.c:
+- Rework locking
+- Improve driver code by adding usage of regmap-mmio
+- Test every exposed API to userland
+- Add support to hwmon
+- Review and revisit all API exposed
+- Revisit PM support
+- Revisit data structures and simplify them
+- Once SCM-core api settles, update this driver accordingly
+
+on omap-thermal-common.c/omap-thermal.h:
+- Revisit extrapolation constants for O4/O5
+- Revisit need for locking
+- Revisit trips and its definitions
+- Revisit trending
+
+on omap5-thermal.c
+- Add support for GPU cooling
+
+generally:
+- write Kconfig dependencies so that omap variants are covered
+- make checkpatch.pl and sparse happy
+- make sure this code works on OMAP4430, OMAP4460 and OMAP5430
+- update documentation
+
+Copy patches to Eduardo Valentin <eduardo.valentin@ti.com>
diff --git a/drivers/staging/omap-thermal/omap-bandgap.c b/drivers/staging/omap-thermal/omap-bandgap.c
new file mode 100644
index 00000000000..c556abb63a1
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap-bandgap.c
@@ -0,0 +1,1187 @@
+/*
+ * OMAP4 Bandgap temperature sensor driver
+ *
+ * Copyright (C) 2011-2012 Texas Instruments Incorporated - http://www.ti.com/
+ * Author: J Keerthy <j-keerthy@ti.com>
+ * Author: Moiz Sonasath <m-sonasath@ti.com>
+ * Couple of fixes, DT and MFD adaptation:
+ *   Eduardo Valentin <eduardo.valentin@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/module.h>
+#include <linux/export.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/types.h>
+#include <linux/mutex.h>
+#include <linux/reboot.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+
+#include "omap-bandgap.h"
+
+static u32 omap_bandgap_readl(struct omap_bandgap *bg_ptr, u32 reg)
+{
+	return readl(bg_ptr->base + reg);
+}
+
+static void omap_bandgap_writel(struct omap_bandgap *bg_ptr, u32 val, u32 reg)
+{
+	writel(val, bg_ptr->base + reg);
+}
+
+static int omap_bandgap_power(struct omap_bandgap *bg_ptr, bool on)
+{
+	struct temp_sensor_registers *tsr;
+	int i;
+	u32 ctrl;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, POWER_SWITCH))
+		return 0;
+
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		tsr = bg_ptr->conf->sensors[i].registers;
+		ctrl = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+		ctrl &= ~tsr->bgap_tempsoff_mask;
+		/* active on 0 */
+		ctrl |= !on << __ffs(tsr->bgap_tempsoff_mask);
+
+		/* write BGAP_TEMPSOFF should be reset to 0 */
+		omap_bandgap_writel(bg_ptr, ctrl, tsr->temp_sensor_ctrl);
+	}
+
+	return 0;
+}
+
+/* This is the Talert handler. Call it only if HAS(TALERT) is set */
+static irqreturn_t talert_irq_handler(int irq, void *data)
+{
+	struct omap_bandgap *bg_ptr = data;
+	struct temp_sensor_registers *tsr;
+	u32 t_hot = 0, t_cold = 0, temp, ctrl;
+	int i;
+
+	bg_ptr = data;
+	/* Read the status of t_hot */
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		tsr = bg_ptr->conf->sensors[i].registers;
+		t_hot = omap_bandgap_readl(bg_ptr, tsr->bgap_status);
+		t_hot &= tsr->status_hot_mask;
+
+		/* Read the status of t_cold */
+		t_cold = omap_bandgap_readl(bg_ptr, tsr->bgap_status);
+		t_cold &= tsr->status_cold_mask;
+
+		if (!t_cold && !t_hot)
+			continue;
+
+		ctrl = omap_bandgap_readl(bg_ptr, tsr->bgap_mask_ctrl);
+		/*
+		 * One TALERT interrupt: Two sources
+		 * If the interrupt is due to t_hot then mask t_hot and
+		 * and unmask t_cold else mask t_cold and unmask t_hot
+		 */
+		if (t_hot) {
+			ctrl &= ~tsr->mask_hot_mask;
+			ctrl |= tsr->mask_cold_mask;
+		} else if (t_cold) {
+			ctrl &= ~tsr->mask_cold_mask;
+			ctrl |= tsr->mask_hot_mask;
+		}
+
+		omap_bandgap_writel(bg_ptr, ctrl, tsr->bgap_mask_ctrl);
+
+		/* read temperature */
+		temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+		temp &= tsr->bgap_dtemp_mask;
+
+		/* report temperature to whom may concern */
+		if (bg_ptr->conf->report_temperature)
+			bg_ptr->conf->report_temperature(bg_ptr, i);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/* This is the Tshut handler. Call it only if HAS(TSHUT) is set */
+static irqreturn_t omap_bandgap_tshut_irq_handler(int irq, void *data)
+{
+	orderly_poweroff(true);
+
+	return IRQ_HANDLED;
+}
+
+static
+int adc_to_temp_conversion(struct omap_bandgap *bg_ptr, int id, int adc_val,
+			   int *t)
+{
+	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[id].ts_data;
+
+	/* look up for temperature in the table and return the temperature */
+	if (adc_val < ts_data->adc_start_val || adc_val > ts_data->adc_end_val)
+		return -ERANGE;
+
+	*t = bg_ptr->conv_table[adc_val - ts_data->adc_start_val];
+
+	return 0;
+}
+
+static int temp_to_adc_conversion(long temp, struct omap_bandgap *bg_ptr, int i,
+				  int *adc)
+{
+	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[i].ts_data;
+	int high, low, mid;
+
+	low = 0;
+	high = ts_data->adc_end_val - ts_data->adc_start_val;
+	mid = (high + low) / 2;
+
+	if (temp < bg_ptr->conv_table[high] || temp > bg_ptr->conv_table[high])
+		return -EINVAL;
+
+	while (low < high) {
+		if (temp < bg_ptr->conv_table[mid])
+			high = mid - 1;
+		else
+			low = mid + 1;
+		mid = (low + high) / 2;
+	}
+
+	*adc = ts_data->adc_start_val + low;
+
+	return 0;
+}
+
+/* Talert masks. Call it only if HAS(TALERT) is set */
+static int temp_sensor_unmask_interrupts(struct omap_bandgap *bg_ptr, int id,
+					 u32 t_hot, u32 t_cold)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp, reg_val;
+
+	/* Read the current on die temperature */
+	tsr = bg_ptr->conf->sensors[id].registers;
+	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+	temp &= tsr->bgap_dtemp_mask;
+
+	reg_val = omap_bandgap_readl(bg_ptr, tsr->bgap_mask_ctrl);
+	if (temp < t_hot)
+		reg_val |= tsr->mask_hot_mask;
+	else
+		reg_val &= ~tsr->mask_hot_mask;
+
+	if (t_cold < temp)
+		reg_val |= tsr->mask_cold_mask;
+	else
+		reg_val &= ~tsr->mask_cold_mask;
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_mask_ctrl);
+
+	return 0;
+}
+
+static
+int add_hyst(int adc_val, int hyst_val, struct omap_bandgap *bg_ptr, int i,
+	     u32 *sum)
+{
+	int temp, ret;
+
+	ret = adc_to_temp_conversion(bg_ptr, i, adc_val, &temp);
+	if (ret < 0)
+		return ret;
+
+	temp += hyst_val;
+
+	return temp_to_adc_conversion(temp, bg_ptr, i, sum);
+}
+
+/* Talert Thot threshold. Call it only if HAS(TALERT) is set */
+static
+int temp_sensor_configure_thot(struct omap_bandgap *bg_ptr, int id, int t_hot)
+{
+	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[id].ts_data;
+	struct temp_sensor_registers *tsr;
+	u32 thresh_val, reg_val;
+	int cold, err = 0;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+
+	/* obtain the T cold value */
+	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
+	cold = (thresh_val & tsr->threshold_tcold_mask) >>
+	    __ffs(tsr->threshold_tcold_mask);
+	if (t_hot <= cold) {
+		/* change the t_cold to t_hot - 5000 millidegrees */
+		err |= add_hyst(t_hot, -ts_data->hyst_val, bg_ptr, id, &cold);
+		/* write the new t_cold value */
+		reg_val = thresh_val & (~tsr->threshold_tcold_mask);
+		reg_val |= cold << __ffs(tsr->threshold_tcold_mask);
+		omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
+		thresh_val = reg_val;
+	}
+
+	/* write the new t_hot value */
+	reg_val = thresh_val & ~tsr->threshold_thot_mask;
+	reg_val |= (t_hot << __ffs(tsr->threshold_thot_mask));
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
+	if (err) {
+		dev_err(bg_ptr->dev, "failed to reprogram thot threshold\n");
+		return -EIO;
+	}
+
+	return temp_sensor_unmask_interrupts(bg_ptr, id, t_hot, cold);
+}
+
+/* Talert Thot and Tcold thresholds. Call it only if HAS(TALERT) is set */
+static
+int temp_sensor_init_talert_thresholds(struct omap_bandgap *bg_ptr, int id,
+				       int t_hot, int t_cold)
+{
+	struct temp_sensor_registers *tsr;
+	u32 reg_val, thresh_val;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
+
+	/* write the new t_cold value */
+	reg_val = thresh_val & ~tsr->threshold_tcold_mask;
+	reg_val |= (t_cold << __ffs(tsr->threshold_tcold_mask));
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
+
+	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
+
+	/* write the new t_hot value */
+	reg_val = thresh_val & ~tsr->threshold_thot_mask;
+	reg_val |= (t_hot << __ffs(tsr->threshold_thot_mask));
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
+
+	reg_val = omap_bandgap_readl(bg_ptr, tsr->bgap_mask_ctrl);
+	reg_val |= tsr->mask_hot_mask;
+	reg_val |= tsr->mask_cold_mask;
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_mask_ctrl);
+
+	return 0;
+}
+
+/* Talert Tcold threshold. Call it only if HAS(TALERT) is set */
+static
+int temp_sensor_configure_tcold(struct omap_bandgap *bg_ptr, int id,
+				int t_cold)
+{
+	struct temp_sensor_data *ts_data = bg_ptr->conf->sensors[id].ts_data;
+	struct temp_sensor_registers *tsr;
+	u32 thresh_val, reg_val;
+	int hot, err = 0;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	/* obtain the T cold value */
+	thresh_val = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
+	hot = (thresh_val & tsr->threshold_thot_mask) >>
+	    __ffs(tsr->threshold_thot_mask);
+
+	if (t_cold >= hot) {
+		/* change the t_hot to t_cold + 5000 millidegrees */
+		err |= add_hyst(t_cold, ts_data->hyst_val, bg_ptr, id, &hot);
+		/* write the new t_hot value */
+		reg_val = thresh_val & (~tsr->threshold_thot_mask);
+		reg_val |= hot << __ffs(tsr->threshold_thot_mask);
+		omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
+		thresh_val = reg_val;
+	}
+
+	/* write the new t_cold value */
+	reg_val = thresh_val & ~tsr->threshold_tcold_mask;
+	reg_val |= (t_cold << __ffs(tsr->threshold_tcold_mask));
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->bgap_threshold);
+	if (err) {
+		dev_err(bg_ptr->dev, "failed to reprogram tcold threshold\n");
+		return -EIO;
+	}
+
+	return temp_sensor_unmask_interrupts(bg_ptr, id, hot, t_cold);
+}
+
+/* This is Tshut Thot config. Call it only if HAS(TSHUT_CONFIG) is set */
+static int temp_sensor_configure_tshut_hot(struct omap_bandgap *bg_ptr,
+					   int id, int tshut_hot)
+{
+	struct temp_sensor_registers *tsr;
+	u32 reg_val;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	reg_val = omap_bandgap_readl(bg_ptr, tsr->tshut_threshold);
+	reg_val &= ~tsr->tshut_hot_mask;
+	reg_val |= tshut_hot << __ffs(tsr->tshut_hot_mask);
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->tshut_threshold);
+
+	return 0;
+}
+
+/* This is Tshut Tcold config. Call it only if HAS(TSHUT_CONFIG) is set */
+static int temp_sensor_configure_tshut_cold(struct omap_bandgap *bg_ptr,
+					    int id, int tshut_cold)
+{
+	struct temp_sensor_registers *tsr;
+	u32 reg_val;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	reg_val = omap_bandgap_readl(bg_ptr, tsr->tshut_threshold);
+	reg_val &= ~tsr->tshut_cold_mask;
+	reg_val |= tshut_cold << __ffs(tsr->tshut_cold_mask);
+	omap_bandgap_writel(bg_ptr, reg_val, tsr->tshut_threshold);
+
+	return 0;
+}
+
+/* This is counter config. Call it only if HAS(COUNTER) is set */
+static int configure_temp_sensor_counter(struct omap_bandgap *bg_ptr, int id,
+					 u32 counter)
+{
+	struct temp_sensor_registers *tsr;
+	u32 val;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	val = omap_bandgap_readl(bg_ptr, tsr->bgap_counter);
+	val &= ~tsr->counter_mask;
+	val |= counter << __ffs(tsr->counter_mask);
+	omap_bandgap_writel(bg_ptr, val, tsr->bgap_counter);
+
+	return 0;
+}
+
+#define bandgap_is_valid(b)						\
+			(!IS_ERR_OR_NULL(b))
+#define bandgap_is_valid_sensor_id(b, i)				\
+			((i) >= 0 && (i) < (b)->conf->sensor_count)
+static inline int omap_bandgap_validate(struct omap_bandgap *bg_ptr, int id)
+{
+	if (!bandgap_is_valid(bg_ptr)) {
+		pr_err("%s: invalid bandgap pointer\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!bandgap_is_valid_sensor_id(bg_ptr, id)) {
+		dev_err(bg_ptr->dev, "%s: sensor id out of range (%d)\n",
+			__func__, id);
+		return -ERANGE;
+	}
+
+	return 0;
+}
+
+/* Exposed APIs */
+/**
+ * omap_bandgap_read_thot() - reads sensor current thot
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @thot - resulting current thot value
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_read_thot(struct omap_bandgap *bg_ptr, int id,
+			      int *thot)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp;
+	int ret;
+
+	ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
+		return -ENOTSUPP;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	temp = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
+	temp = (temp & tsr->threshold_thot_mask) >>
+		__ffs(tsr->threshold_thot_mask);
+	ret |= adc_to_temp_conversion(bg_ptr, id, temp, &temp);
+	if (ret) {
+		dev_err(bg_ptr->dev, "failed to read thot\n");
+		return -EIO;
+	}
+
+	*thot = temp;
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_write_thot() - sets sensor current thot
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @val - desired thot value
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_write_thot(struct omap_bandgap *bg_ptr, int id, int val)
+{
+	struct temp_sensor_data *ts_data;
+	struct temp_sensor_registers *tsr;
+	u32 t_hot;
+	int ret;
+
+	ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
+		return -ENOTSUPP;
+
+	ts_data = bg_ptr->conf->sensors[id].ts_data;
+	tsr = bg_ptr->conf->sensors[id].registers;
+
+	if (val < ts_data->min_temp + ts_data->hyst_val)
+		return -EINVAL;
+	ret = temp_to_adc_conversion(val, bg_ptr, id, &t_hot);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&bg_ptr->bg_mutex);
+	temp_sensor_configure_thot(bg_ptr, id, t_hot);
+	mutex_unlock(&bg_ptr->bg_mutex);
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_read_tcold() - reads sensor current tcold
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @tcold - resulting current tcold value
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_read_tcold(struct omap_bandgap *bg_ptr, int id,
+			       int *tcold)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp;
+	int ret;
+
+	ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
+		return -ENOTSUPP;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	temp = omap_bandgap_readl(bg_ptr, tsr->bgap_threshold);
+	temp = (temp & tsr->threshold_tcold_mask)
+	    >> __ffs(tsr->threshold_tcold_mask);
+	ret |= adc_to_temp_conversion(bg_ptr, id, temp, &temp);
+	if (ret)
+		return -EIO;
+
+	*tcold = temp;
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_write_tcold() - sets the sensor tcold
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @val - desired tcold value
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_write_tcold(struct omap_bandgap *bg_ptr, int id, int val)
+{
+	struct temp_sensor_data *ts_data;
+	struct temp_sensor_registers *tsr;
+	u32 t_cold;
+	int ret;
+
+	ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, TALERT))
+		return -ENOTSUPP;
+
+	ts_data = bg_ptr->conf->sensors[id].ts_data;
+	tsr = bg_ptr->conf->sensors[id].registers;
+	if (val > ts_data->max_temp + ts_data->hyst_val)
+		return -EINVAL;
+
+	ret = temp_to_adc_conversion(val, bg_ptr, id, &t_cold);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&bg_ptr->bg_mutex);
+	temp_sensor_configure_tcold(bg_ptr, id, t_cold);
+	mutex_unlock(&bg_ptr->bg_mutex);
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_read_update_interval() - read the sensor update interval
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @interval - resulting update interval in miliseconds
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_read_update_interval(struct omap_bandgap *bg_ptr, int id,
+					 int *interval)
+{
+	struct temp_sensor_registers *tsr;
+	u32 time;
+	int ret;
+
+	ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+		return -ENOTSUPP;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	time = omap_bandgap_readl(bg_ptr, tsr->bgap_counter);
+	if (ret)
+		return ret;
+	time = (time & tsr->counter_mask) >> __ffs(tsr->counter_mask);
+	time = time * 1000 / bg_ptr->clk_rate;
+
+	*interval = time;
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_write_update_interval() - set the update interval
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @interval - desired update interval in miliseconds
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_write_update_interval(struct omap_bandgap *bg_ptr,
+					  int id, u32 interval)
+{
+	int ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	if (!OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+		return -ENOTSUPP;
+
+	interval = interval * bg_ptr->clk_rate / 1000;
+	mutex_lock(&bg_ptr->bg_mutex);
+	configure_temp_sensor_counter(bg_ptr, id, interval);
+	mutex_unlock(&bg_ptr->bg_mutex);
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_read_temperature() - report current temperature
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @temperature - resulting temperature
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_read_temperature(struct omap_bandgap *bg_ptr, int id,
+				     int *temperature)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp;
+	int ret;
+
+	ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+	temp &= tsr->bgap_dtemp_mask;
+
+	ret |= adc_to_temp_conversion(bg_ptr, id, temp, &temp);
+	if (ret)
+		return -EIO;
+
+	*temperature = temp;
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_set_sensor_data() - helper function to store thermal
+ * framework related data.
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ * @data - thermal framework related data to be stored
+ *
+ * returns 0 on success or the proper error code
+ */
+int omap_bandgap_set_sensor_data(struct omap_bandgap *bg_ptr, int id,
+				void *data)
+{
+	int ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ret;
+
+	bg_ptr->conf->sensors[id].data = data;
+
+	return 0;
+}
+
+/**
+ * omap_bandgap_get_sensor_data() - helper function to get thermal
+ * framework related data.
+ * @bg_ptr - pointer to bandgap instance
+ * @id - sensor id
+ *
+ * returns data stored by set function with sensor id on success or NULL
+ */
+void *omap_bandgap_get_sensor_data(struct omap_bandgap *bg_ptr, int id)
+{
+	int ret = omap_bandgap_validate(bg_ptr, id);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return bg_ptr->conf->sensors[id].data;
+}
+
+static int
+omap_bandgap_force_single_read(struct omap_bandgap *bg_ptr, int id)
+{
+	struct temp_sensor_registers *tsr;
+	u32 temp = 0, counter = 1000;
+
+	tsr = bg_ptr->conf->sensors[id].registers;
+	/* Select single conversion mode */
+	if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG)) {
+		temp = omap_bandgap_readl(bg_ptr, tsr->bgap_mode_ctrl);
+		temp &= ~(1 << __ffs(tsr->mode_ctrl_mask));
+		omap_bandgap_writel(bg_ptr, temp, tsr->bgap_mode_ctrl);
+	}
+
+	/* Start of Conversion = 1 */
+	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+	temp |= 1 << __ffs(tsr->bgap_soc_mask);
+	omap_bandgap_writel(bg_ptr, temp, tsr->temp_sensor_ctrl);
+	/* Wait until DTEMP is updated */
+	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+	temp &= (tsr->bgap_dtemp_mask);
+	while ((temp == 0) && --counter) {
+		temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+		temp &= (tsr->bgap_dtemp_mask);
+	}
+	/* Start of Conversion = 0 */
+	temp = omap_bandgap_readl(bg_ptr, tsr->temp_sensor_ctrl);
+	temp &= ~(1 << __ffs(tsr->bgap_soc_mask));
+	omap_bandgap_writel(bg_ptr, temp, tsr->temp_sensor_ctrl);
+
+	return 0;
+}
+
+/**
+ * enable_continuous_mode() - One time enabling of continuous conversion mode
+ * @bg_ptr - pointer to scm instance
+ *
+ * Call this function only if HAS(MODE_CONFIG) is set
+ */
+static int enable_continuous_mode(struct omap_bandgap *bg_ptr)
+{
+	struct temp_sensor_registers *tsr;
+	int i;
+	u32 val;
+
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		/* Perform a single read just before enabling continuous */
+		omap_bandgap_force_single_read(bg_ptr, i);
+		tsr = bg_ptr->conf->sensors[i].registers;
+		val = omap_bandgap_readl(bg_ptr, tsr->bgap_mode_ctrl);
+		val |= 1 << __ffs(tsr->mode_ctrl_mask);
+		omap_bandgap_writel(bg_ptr, val, tsr->bgap_mode_ctrl);
+	}
+
+	return 0;
+}
+
+static int omap_bandgap_tshut_init(struct omap_bandgap *bg_ptr,
+				      struct platform_device *pdev)
+{
+	int gpio_nr = bg_ptr->tshut_gpio;
+	int status;
+
+	/* Request for gpio_86 line */
+	status = gpio_request(gpio_nr, "tshut");
+	if (status < 0) {
+		dev_err(bg_ptr->dev,
+			"Could not request for TSHUT GPIO:%i\n", 86);
+		return status;
+	}
+	status = gpio_direction_input(gpio_nr);
+	if (status) {
+		dev_err(bg_ptr->dev,
+			"Cannot set input TSHUT GPIO %d\n", gpio_nr);
+		return status;
+	}
+
+	status = request_irq(gpio_to_irq(gpio_nr),
+			     omap_bandgap_tshut_irq_handler,
+			     IRQF_TRIGGER_RISING, "tshut",
+			     NULL);
+	if (status) {
+		gpio_free(gpio_nr);
+		dev_err(bg_ptr->dev, "request irq failed for TSHUT");
+	}
+
+	return 0;
+}
+
+/* Initialization of Talert. Call it only if HAS(TALERT) is set */
+static int omap_bandgap_talert_init(struct omap_bandgap *bg_ptr,
+				       struct platform_device *pdev)
+{
+	int ret;
+
+	bg_ptr->irq = platform_get_irq(pdev, 0);
+	if (bg_ptr->irq < 0) {
+		dev_err(&pdev->dev, "get_irq failed\n");
+		return bg_ptr->irq;
+	}
+	ret = request_threaded_irq(bg_ptr->irq, NULL,
+				   talert_irq_handler,
+				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+				   "talert", bg_ptr);
+	if (ret) {
+		dev_err(&pdev->dev, "Request threaded irq failed.\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id of_omap_bandgap_match[];
+static struct omap_bandgap *omap_bandgap_build(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	const struct of_device_id *of_id;
+	struct omap_bandgap *bg_ptr;
+	struct resource *res;
+	u32 prop;
+	int i;
+
+	/* just for the sake */
+	if (!node) {
+		dev_err(&pdev->dev, "no platform information available\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	bg_ptr = devm_kzalloc(&pdev->dev, sizeof(struct omap_bandgap),
+				    GFP_KERNEL);
+	if (!bg_ptr) {
+		dev_err(&pdev->dev, "Unable to allocate mem for driver ref\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	of_id = of_match_device(of_omap_bandgap_match, &pdev->dev);
+	if (of_id)
+		bg_ptr->conf = of_id->data;
+
+	i = 0;
+	do {
+		void __iomem *chunk;
+
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res)
+			break;
+		chunk = devm_request_and_ioremap(&pdev->dev, res);
+		if (i == 0)
+			bg_ptr->base = chunk;
+		if (!chunk) {
+			dev_err(&pdev->dev,
+				"failed to request the IO (%d:%pR).\n",
+				i, res);
+			return ERR_PTR(-EADDRNOTAVAIL);
+		}
+		i++;
+	} while (res);
+
+	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
+		if (of_property_read_u32(node, "ti,tshut-gpio", &prop) < 0) {
+			dev_err(&pdev->dev, "missing tshut gpio in device tree\n");
+			return ERR_PTR(-EINVAL);
+		}
+		bg_ptr->tshut_gpio = prop;
+		if (!gpio_is_valid(bg_ptr->tshut_gpio)) {
+			dev_err(&pdev->dev, "invalid gpio for tshut (%d)\n",
+				bg_ptr->tshut_gpio);
+			return ERR_PTR(-EINVAL);
+		}
+	}
+
+	return bg_ptr;
+}
+
+static
+int __devinit omap_bandgap_probe(struct platform_device *pdev)
+{
+	struct omap_bandgap *bg_ptr;
+	int clk_rate, ret = 0, i;
+
+	bg_ptr = omap_bandgap_build(pdev);
+	if (IS_ERR_OR_NULL(bg_ptr)) {
+		dev_err(&pdev->dev, "failed to fetch platform data\n");
+		return PTR_ERR(bg_ptr);
+	}
+	bg_ptr->dev = &pdev->dev;
+
+	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
+		ret = omap_bandgap_tshut_init(bg_ptr, pdev);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed to initialize system tshut IRQ\n");
+			return ret;
+		}
+	}
+
+	bg_ptr->fclock = clk_get(NULL, bg_ptr->conf->fclock_name);
+	ret = IS_ERR_OR_NULL(bg_ptr->fclock);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request fclock reference\n");
+		goto free_irqs;
+	}
+
+	bg_ptr->div_clk = clk_get(NULL,  bg_ptr->conf->div_ck_name);
+	ret = IS_ERR_OR_NULL(bg_ptr->div_clk);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to request div_ts_ck clock ref\n");
+		goto free_irqs;
+	}
+
+	bg_ptr->conv_table = bg_ptr->conf->conv_table;
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		struct temp_sensor_registers *tsr;
+		u32 val;
+
+		tsr = bg_ptr->conf->sensors[i].registers;
+		/*
+		 * check if the efuse has a non-zero value if not
+		 * it is an untrimmed sample and the temperatures
+		 * may not be accurate
+		 */
+		val = omap_bandgap_readl(bg_ptr, tsr->bgap_efuse);
+		if (ret || !val)
+			dev_info(&pdev->dev,
+				 "Non-trimmed BGAP, Temp not accurate\n");
+	}
+
+	clk_rate = clk_round_rate(bg_ptr->div_clk,
+				  bg_ptr->conf->sensors[0].ts_data->max_freq);
+	if (clk_rate < bg_ptr->conf->sensors[0].ts_data->min_freq ||
+	    clk_rate == 0xffffffff) {
+		ret = -ENODEV;
+		dev_err(&pdev->dev, "wrong clock rate (%d)\n", clk_rate);
+		goto put_clks;
+	}
+
+	ret = clk_set_rate(bg_ptr->div_clk, clk_rate);
+	if (ret)
+		dev_err(&pdev->dev, "Cannot re-set clock rate. Continuing\n");
+
+	bg_ptr->clk_rate = clk_rate;
+	clk_enable(bg_ptr->fclock);
+
+	mutex_init(&bg_ptr->bg_mutex);
+	bg_ptr->dev = &pdev->dev;
+	platform_set_drvdata(pdev, bg_ptr);
+
+	omap_bandgap_power(bg_ptr, true);
+
+	/* Set default counter to 1 for now */
+	if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+		for (i = 0; i < bg_ptr->conf->sensor_count; i++)
+			configure_temp_sensor_counter(bg_ptr, i, 1);
+
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		struct temp_sensor_data *ts_data;
+
+		ts_data = bg_ptr->conf->sensors[i].ts_data;
+
+		if (OMAP_BANDGAP_HAS(bg_ptr, TALERT))
+			temp_sensor_init_talert_thresholds(bg_ptr, i,
+							   ts_data->t_hot,
+							   ts_data->t_cold);
+		if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG)) {
+			temp_sensor_configure_tshut_hot(bg_ptr, i,
+							ts_data->tshut_hot);
+			temp_sensor_configure_tshut_cold(bg_ptr, i,
+							 ts_data->tshut_cold);
+		}
+	}
+
+	if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
+		enable_continuous_mode(bg_ptr);
+
+	/* Set .250 seconds time as default counter */
+	if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+		for (i = 0; i < bg_ptr->conf->sensor_count; i++)
+			configure_temp_sensor_counter(bg_ptr, i,
+						      bg_ptr->clk_rate / 4);
+
+	/* Every thing is good? Then expose the sensors */
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		char *domain;
+
+		domain = bg_ptr->conf->sensors[i].domain;
+		if (bg_ptr->conf->expose_sensor)
+			bg_ptr->conf->expose_sensor(bg_ptr, i, domain);
+
+		if (bg_ptr->conf->sensors[i].register_cooling)
+			bg_ptr->conf->sensors[i].register_cooling(bg_ptr, i);
+	}
+
+	/*
+	 * Enable the Interrupts once everything is set. Otherwise irq handler
+	 * might be called as soon as it is enabled where as rest of framework
+	 * is still getting initialised.
+	 */
+	if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
+		ret = omap_bandgap_talert_init(bg_ptr, pdev);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to initialize Talert IRQ\n");
+			i = bg_ptr->conf->sensor_count;
+			goto disable_clk;
+		}
+	}
+
+	return 0;
+
+disable_clk:
+	clk_disable(bg_ptr->fclock);
+put_clks:
+	clk_put(bg_ptr->fclock);
+	clk_put(bg_ptr->div_clk);
+free_irqs:
+	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
+		free_irq(gpio_to_irq(bg_ptr->tshut_gpio), NULL);
+		gpio_free(bg_ptr->tshut_gpio);
+	}
+
+	return ret;
+}
+
+static
+int __devexit omap_bandgap_remove(struct platform_device *pdev)
+{
+	struct omap_bandgap *bg_ptr = platform_get_drvdata(pdev);
+	int i;
+
+	/* First thing is to remove sensor interfaces */
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		if (bg_ptr->conf->sensors[i].register_cooling)
+			bg_ptr->conf->sensors[i].unregister_cooling(bg_ptr, i);
+
+		if (bg_ptr->conf->remove_sensor)
+			bg_ptr->conf->remove_sensor(bg_ptr, i);
+	}
+
+	omap_bandgap_power(bg_ptr, false);
+
+	clk_disable(bg_ptr->fclock);
+	clk_put(bg_ptr->fclock);
+	clk_put(bg_ptr->div_clk);
+
+	if (OMAP_BANDGAP_HAS(bg_ptr, TALERT))
+		free_irq(bg_ptr->irq, bg_ptr);
+
+	if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT)) {
+		free_irq(gpio_to_irq(bg_ptr->tshut_gpio), NULL);
+		gpio_free(bg_ptr->tshut_gpio);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int omap_bandgap_save_ctxt(struct omap_bandgap *bg_ptr)
+{
+	int i;
+
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		struct temp_sensor_registers *tsr;
+		struct temp_sensor_regval *rval;
+
+		rval = &bg_ptr->conf->sensors[i].regval;
+		tsr = bg_ptr->conf->sensors[i].registers;
+
+		if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
+			rval->bg_mode_ctrl = omap_bandgap_readl(bg_ptr,
+								tsr->bgap_mode_ctrl);
+		if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+			rval->bg_counter = omap_bandgap_readl(bg_ptr,
+							      tsr->bgap_counter);
+		if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
+			rval->bg_threshold = omap_bandgap_readl(bg_ptr,
+								tsr->bgap_threshold);
+			rval->bg_ctrl = omap_bandgap_readl(bg_ptr,
+							   tsr->bgap_mask_ctrl);
+		}
+
+		if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG))
+			rval->tshut_threshold = omap_bandgap_readl(bg_ptr,
+								   tsr->tshut_threshold);
+	}
+
+	return 0;
+}
+
+static int omap_bandgap_restore_ctxt(struct omap_bandgap *bg_ptr)
+{
+	int i;
+	u32 temp = 0;
+
+	for (i = 0; i < bg_ptr->conf->sensor_count; i++) {
+		struct temp_sensor_registers *tsr;
+		struct temp_sensor_regval *rval;
+		u32 val = 0;
+
+		rval = &bg_ptr->conf->sensors[i].regval;
+		tsr = bg_ptr->conf->sensors[i].registers;
+
+		if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+			val = omap_bandgap_readl(bg_ptr, tsr->bgap_counter);
+
+		if (val == 0) {
+			if (OMAP_BANDGAP_HAS(bg_ptr, TSHUT_CONFIG))
+				omap_bandgap_writel(bg_ptr, rval->tshut_threshold,
+							   tsr->tshut_threshold);
+			/* Force immediate temperature measurement and update
+			 * of the DTEMP field
+			 */
+			omap_bandgap_force_single_read(bg_ptr, i);
+
+			if (OMAP_BANDGAP_HAS(bg_ptr, COUNTER))
+				omap_bandgap_writel(bg_ptr, rval->bg_counter,
+							   tsr->bgap_counter);
+			if (OMAP_BANDGAP_HAS(bg_ptr, MODE_CONFIG))
+				omap_bandgap_writel(bg_ptr, rval->bg_mode_ctrl,
+							   tsr->bgap_mode_ctrl);
+			if (OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
+				omap_bandgap_writel(bg_ptr,
+							   rval->bg_threshold,
+							   tsr->bgap_threshold);
+				omap_bandgap_writel(bg_ptr, rval->bg_ctrl,
+							   tsr->bgap_mask_ctrl);
+			}
+		} else {
+			temp = omap_bandgap_readl(bg_ptr,
+						  tsr->temp_sensor_ctrl);
+			temp &= (tsr->bgap_dtemp_mask);
+			omap_bandgap_force_single_read(bg_ptr, i);
+			if (temp == 0 && OMAP_BANDGAP_HAS(bg_ptr, TALERT)) {
+				temp = omap_bandgap_readl(bg_ptr,
+							  tsr->bgap_mask_ctrl);
+				temp |= 1 << __ffs(tsr->mode_ctrl_mask);
+				omap_bandgap_writel(bg_ptr, temp,
+							   tsr->bgap_mask_ctrl);
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int omap_bandgap_suspend(struct device *dev)
+{
+	struct omap_bandgap *bg_ptr = dev_get_drvdata(dev);
+	int err;
+
+	err = omap_bandgap_save_ctxt(bg_ptr);
+	omap_bandgap_power(bg_ptr, false);
+	clk_disable(bg_ptr->fclock);
+
+	return err;
+}
+
+static int omap_bandgap_resume(struct device *dev)
+{
+	struct omap_bandgap *bg_ptr = dev_get_drvdata(dev);
+
+	clk_enable(bg_ptr->fclock);
+	omap_bandgap_power(bg_ptr, true);
+
+	return omap_bandgap_restore_ctxt(bg_ptr);
+}
+static const struct dev_pm_ops omap_bandgap_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(omap_bandgap_suspend,
+				omap_bandgap_resume)
+};
+
+#define DEV_PM_OPS	(&omap_bandgap_dev_pm_ops)
+#else
+#define DEV_PM_OPS	NULL
+#endif
+
+static const struct of_device_id of_omap_bandgap_match[] = {
+#ifdef CONFIG_OMAP4_THERMAL
+	{
+		.compatible = "ti,omap4430-bandgap",
+		.data = (void *)&omap4430_data,
+	},
+	{
+		.compatible = "ti,omap4460-bandgap",
+		.data = (void *)&omap4460_data,
+	},
+	{
+		.compatible = "ti,omap4470-bandgap",
+		.data = (void *)&omap4470_data,
+	},
+#endif
+#ifdef CONFIG_OMAP5_THERMAL
+	{
+		.compatible = "ti,omap5430-bandgap",
+		.data = (void *)&omap5430_data,
+	},
+#endif
+	/* Sentinel */
+	{ },
+};
+MODULE_DEVICE_TABLE(of, of_omap_bandgap_match);
+
+static struct platform_driver omap_bandgap_sensor_driver = {
+	.probe = omap_bandgap_probe,
+	.remove = omap_bandgap_remove,
+	.driver = {
+			.name = "omap-bandgap",
+			.pm = DEV_PM_OPS,
+			.of_match_table	= of_omap_bandgap_match,
+	},
+};
+
+module_platform_driver(omap_bandgap_sensor_driver);
+
+MODULE_DESCRIPTION("OMAP4+ bandgap temperature sensor driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:omap-bandgap");
+MODULE_AUTHOR("Texas Instrument Inc.");
diff --git a/drivers/staging/omap-thermal/omap-bandgap.h b/drivers/staging/omap-thermal/omap-bandgap.h
new file mode 100644
index 00000000000..78aed7535f4
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap-bandgap.h
@@ -0,0 +1,441 @@
+/*
+ * OMAP4 Bandgap temperature sensor driver
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@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
+ *
+ */
+#ifndef __OMAP_BANDGAP_H
+#define __OMAP_BANDGAP_H
+
+#include <linux/mutex.h>
+#include <linux/types.h>
+#include <linux/err.h>
+
+/* TEMP_SENSOR OMAP4430 */
+#define OMAP4430_BGAP_TSHUT_SHIFT			11
+#define OMAP4430_BGAP_TSHUT_MASK			(1 << 11)
+
+/* TEMP_SENSOR OMAP4430 */
+#define OMAP4430_BGAP_TEMPSOFF_SHIFT			12
+#define OMAP4430_BGAP_TEMPSOFF_MASK			(1 << 12)
+#define OMAP4430_SINGLE_MODE_SHIFT			10
+#define OMAP4430_SINGLE_MODE_MASK			(1 << 10)
+#define OMAP4430_BGAP_TEMP_SENSOR_SOC_SHIFT		9
+#define OMAP4430_BGAP_TEMP_SENSOR_SOC_MASK		(1 << 9)
+#define OMAP4430_BGAP_TEMP_SENSOR_EOCZ_SHIFT		8
+#define OMAP4430_BGAP_TEMP_SENSOR_EOCZ_MASK		(1 << 8)
+#define OMAP4430_BGAP_TEMP_SENSOR_DTEMP_SHIFT		0
+#define OMAP4430_BGAP_TEMP_SENSOR_DTEMP_MASK		(0xff << 0)
+
+#define OMAP4430_ADC_START_VALUE			0
+#define OMAP4430_ADC_END_VALUE				127
+#define OMAP4430_MAX_FREQ				32768
+#define OMAP4430_MIN_FREQ				32768
+#define OMAP4430_MIN_TEMP				-40000
+#define OMAP4430_MAX_TEMP				125000
+#define OMAP4430_HYST_VAL				5000
+
+/* TEMP_SENSOR OMAP4460 */
+#define OMAP4460_BGAP_TEMPSOFF_SHIFT			13
+#define OMAP4460_BGAP_TEMPSOFF_MASK			(1 << 13)
+#define OMAP4460_BGAP_TEMP_SENSOR_SOC_SHIFT		11
+#define OMAP4460_BGAP_TEMP_SENSOR_SOC_MASK		(1 << 11)
+#define OMAP4460_BGAP_TEMP_SENSOR_EOCZ_SHIFT		10
+#define OMAP4460_BGAP_TEMP_SENSOR_EOCZ_MASK		(1 << 10)
+#define OMAP4460_BGAP_TEMP_SENSOR_DTEMP_SHIFT		0
+#define OMAP4460_BGAP_TEMP_SENSOR_DTEMP_MASK		(0x3ff << 0)
+
+/* BANDGAP_CTRL */
+#define OMAP4460_SINGLE_MODE_SHIFT			31
+#define OMAP4460_SINGLE_MODE_MASK			(1 << 31)
+#define OMAP4460_MASK_HOT_SHIFT				1
+#define OMAP4460_MASK_HOT_MASK				(1 << 1)
+#define OMAP4460_MASK_COLD_SHIFT			0
+#define OMAP4460_MASK_COLD_MASK				(1 << 0)
+
+/* BANDGAP_COUNTER */
+#define OMAP4460_COUNTER_SHIFT				0
+#define OMAP4460_COUNTER_MASK				(0xffffff << 0)
+
+/* BANDGAP_THRESHOLD */
+#define OMAP4460_T_HOT_SHIFT				16
+#define OMAP4460_T_HOT_MASK				(0x3ff << 16)
+#define OMAP4460_T_COLD_SHIFT				0
+#define OMAP4460_T_COLD_MASK				(0x3ff << 0)
+
+/* TSHUT_THRESHOLD */
+#define OMAP4460_TSHUT_HOT_SHIFT			16
+#define OMAP4460_TSHUT_HOT_MASK				(0x3ff << 16)
+#define OMAP4460_TSHUT_COLD_SHIFT			0
+#define OMAP4460_TSHUT_COLD_MASK			(0x3ff << 0)
+
+/* BANDGAP_STATUS */
+#define OMAP4460_CLEAN_STOP_SHIFT			3
+#define OMAP4460_CLEAN_STOP_MASK			(1 << 3)
+#define OMAP4460_BGAP_ALERT_SHIFT			2
+#define OMAP4460_BGAP_ALERT_MASK			(1 << 2)
+#define OMAP4460_HOT_FLAG_SHIFT				1
+#define OMAP4460_HOT_FLAG_MASK				(1 << 1)
+#define OMAP4460_COLD_FLAG_SHIFT			0
+#define OMAP4460_COLD_FLAG_MASK				(1 << 0)
+
+/* TEMP_SENSOR OMAP5430 */
+#define OMAP5430_BGAP_TEMP_SENSOR_SOC_SHIFT		12
+#define OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK		(1 << 12)
+#define OMAP5430_BGAP_TEMPSOFF_SHIFT			11
+#define OMAP5430_BGAP_TEMPSOFF_MASK			(1 << 11)
+#define OMAP5430_BGAP_TEMP_SENSOR_EOCZ_SHIFT		10
+#define OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK		(1 << 10)
+#define OMAP5430_BGAP_TEMP_SENSOR_DTEMP_SHIFT		0
+#define OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK		(0x3ff << 0)
+
+/* BANDGAP_CTRL */
+#define OMAP5430_MASK_HOT_CORE_SHIFT			5
+#define OMAP5430_MASK_HOT_CORE_MASK			(1 << 5)
+#define OMAP5430_MASK_COLD_CORE_SHIFT			4
+#define OMAP5430_MASK_COLD_CORE_MASK			(1 << 4)
+#define OMAP5430_MASK_HOT_MM_SHIFT			3
+#define OMAP5430_MASK_HOT_MM_MASK			(1 << 3)
+#define OMAP5430_MASK_COLD_MM_SHIFT			2
+#define OMAP5430_MASK_COLD_MM_MASK			(1 << 2)
+#define OMAP5430_MASK_HOT_MPU_SHIFT			1
+#define OMAP5430_MASK_HOT_MPU_MASK			(1 << 1)
+#define OMAP5430_MASK_COLD_MPU_SHIFT			0
+#define OMAP5430_MASK_COLD_MPU_MASK			(1 << 0)
+
+/* BANDGAP_COUNTER */
+#define OMAP5430_REPEAT_MODE_SHIFT			31
+#define OMAP5430_REPEAT_MODE_MASK			(1 << 31)
+#define OMAP5430_COUNTER_SHIFT				0
+#define OMAP5430_COUNTER_MASK				(0xffffff << 0)
+
+/* BANDGAP_THRESHOLD */
+#define OMAP5430_T_HOT_SHIFT				16
+#define OMAP5430_T_HOT_MASK				(0x3ff << 16)
+#define OMAP5430_T_COLD_SHIFT				0
+#define OMAP5430_T_COLD_MASK				(0x3ff << 0)
+
+/* TSHUT_THRESHOLD */
+#define OMAP5430_TSHUT_HOT_SHIFT			16
+#define OMAP5430_TSHUT_HOT_MASK				(0x3ff << 16)
+#define OMAP5430_TSHUT_COLD_SHIFT			0
+#define OMAP5430_TSHUT_COLD_MASK			(0x3ff << 0)
+
+/* BANDGAP_STATUS */
+#define OMAP5430_BGAP_ALERT_SHIFT			31
+#define OMAP5430_BGAP_ALERT_MASK			(1 << 31)
+#define OMAP5430_HOT_CORE_FLAG_SHIFT			5
+#define OMAP5430_HOT_CORE_FLAG_MASK			(1 << 5)
+#define OMAP5430_COLD_CORE_FLAG_SHIFT			4
+#define OMAP5430_COLD_CORE_FLAG_MASK			(1 << 4)
+#define OMAP5430_HOT_MM_FLAG_SHIFT			3
+#define OMAP5430_HOT_MM_FLAG_MASK			(1 << 3)
+#define OMAP5430_COLD_MM_FLAG_SHIFT			2
+#define OMAP5430_COLD_MM_FLAG_MASK			(1 << 2)
+#define OMAP5430_HOT_MPU_FLAG_SHIFT			1
+#define OMAP5430_HOT_MPU_FLAG_MASK			(1 << 1)
+#define OMAP5430_COLD_MPU_FLAG_SHIFT			0
+#define OMAP5430_COLD_MPU_FLAG_MASK			(1 << 0)
+
+/* Offsets from the base of temperature sensor registers */
+
+/* 4430 - All goes relative to OPP_BGAP */
+#define OMAP4430_FUSE_OPP_BGAP				0x0
+#define OMAP4430_TEMP_SENSOR_CTRL_OFFSET		0xCC
+
+/* 4460 - All goes relative to OPP_BGAP */
+#define OMAP4460_FUSE_OPP_BGAP				0x0
+#define OMAP4460_TEMP_SENSOR_CTRL_OFFSET		0xCC
+#define OMAP4460_BGAP_CTRL_OFFSET			0x118
+#define OMAP4460_BGAP_COUNTER_OFFSET			0x11C
+#define OMAP4460_BGAP_THRESHOLD_OFFSET			0x120
+#define OMAP4460_BGAP_TSHUT_OFFSET			0x124
+#define OMAP4460_BGAP_STATUS_OFFSET			0x128
+
+/* 5430 - All goes relative to OPP_BGAP_GPU */
+#define OMAP5430_FUSE_OPP_BGAP_GPU			0x0
+#define OMAP5430_TEMP_SENSOR_GPU_OFFSET			0x150
+#define OMAP5430_BGAP_COUNTER_GPU_OFFSET		0x1C0
+#define OMAP5430_BGAP_THRESHOLD_GPU_OFFSET		0x1A8
+#define OMAP5430_BGAP_TSHUT_GPU_OFFSET			0x1B4
+
+#define OMAP5430_FUSE_OPP_BGAP_MPU			0x4
+#define OMAP5430_TEMP_SENSOR_MPU_OFFSET			0x14C
+#define OMAP5430_BGAP_CTRL_OFFSET			0x1A0
+#define OMAP5430_BGAP_COUNTER_MPU_OFFSET		0x1BC
+#define OMAP5430_BGAP_THRESHOLD_MPU_OFFSET		0x1A4
+#define OMAP5430_BGAP_TSHUT_MPU_OFFSET			0x1B0
+#define OMAP5430_BGAP_STATUS_OFFSET			0x1C8
+
+#define OMAP5430_FUSE_OPP_BGAP_CORE			0x8
+#define OMAP5430_TEMP_SENSOR_CORE_OFFSET		0x154
+#define OMAP5430_BGAP_COUNTER_CORE_OFFSET		0x1C4
+#define OMAP5430_BGAP_THRESHOLD_CORE_OFFSET		0x1AC
+#define OMAP5430_BGAP_TSHUT_CORE_OFFSET			0x1B8
+
+#define OMAP4460_TSHUT_HOT				900	/* 122 deg C */
+#define OMAP4460_TSHUT_COLD				895	/* 100 deg C */
+#define OMAP4460_T_HOT					800	/* 73 deg C */
+#define OMAP4460_T_COLD					795	/* 71 deg C */
+#define OMAP4460_MAX_FREQ				1500000
+#define OMAP4460_MIN_FREQ				1000000
+#define OMAP4460_MIN_TEMP				-40000
+#define OMAP4460_MAX_TEMP				123000
+#define OMAP4460_HYST_VAL				5000
+#define OMAP4460_ADC_START_VALUE			530
+#define OMAP4460_ADC_END_VALUE				932
+
+#define OMAP5430_MPU_TSHUT_HOT				915
+#define OMAP5430_MPU_TSHUT_COLD				900
+#define OMAP5430_MPU_T_HOT				800
+#define OMAP5430_MPU_T_COLD				795
+#define OMAP5430_MPU_MAX_FREQ				1500000
+#define OMAP5430_MPU_MIN_FREQ				1000000
+#define OMAP5430_MPU_MIN_TEMP				-40000
+#define OMAP5430_MPU_MAX_TEMP				125000
+#define OMAP5430_MPU_HYST_VAL				5000
+#define OMAP5430_ADC_START_VALUE			532
+#define OMAP5430_ADC_END_VALUE				934
+
+
+#define OMAP5430_GPU_TSHUT_HOT				915
+#define OMAP5430_GPU_TSHUT_COLD				900
+#define OMAP5430_GPU_T_HOT				800
+#define OMAP5430_GPU_T_COLD				795
+#define OMAP5430_GPU_MAX_FREQ				1500000
+#define OMAP5430_GPU_MIN_FREQ				1000000
+#define OMAP5430_GPU_MIN_TEMP				-40000
+#define OMAP5430_GPU_MAX_TEMP				125000
+#define OMAP5430_GPU_HYST_VAL				5000
+
+#define OMAP5430_CORE_TSHUT_HOT				915
+#define OMAP5430_CORE_TSHUT_COLD			900
+#define OMAP5430_CORE_T_HOT				800
+#define OMAP5430_CORE_T_COLD				795
+#define OMAP5430_CORE_MAX_FREQ				1500000
+#define OMAP5430_CORE_MIN_FREQ				1000000
+#define OMAP5430_CORE_MIN_TEMP				-40000
+#define OMAP5430_CORE_MAX_TEMP				125000
+#define OMAP5430_CORE_HYST_VAL				5000
+
+/**
+ * The register offsets and bit fields might change across
+ * OMAP versions hence populating them in this structure.
+ */
+
+struct temp_sensor_registers {
+	u32	temp_sensor_ctrl;
+	u32	bgap_tempsoff_mask;
+	u32	bgap_soc_mask;
+	u32	bgap_eocz_mask;
+	u32	bgap_dtemp_mask;
+
+	u32	bgap_mask_ctrl;
+	u32	mask_hot_mask;
+	u32	mask_cold_mask;
+
+	u32	bgap_mode_ctrl;
+	u32	mode_ctrl_mask;
+
+	u32	bgap_counter;
+	u32	counter_mask;
+
+	u32	bgap_threshold;
+	u32	threshold_thot_mask;
+	u32	threshold_tcold_mask;
+
+	u32	tshut_threshold;
+	u32	tshut_hot_mask;
+	u32	tshut_cold_mask;
+
+	u32	bgap_status;
+	u32	status_clean_stop_mask;
+	u32	status_bgap_alert_mask;
+	u32	status_hot_mask;
+	u32	status_cold_mask;
+
+	u32	bgap_efuse;
+};
+
+/**
+ * The thresholds and limits for temperature sensors.
+ */
+struct temp_sensor_data {
+	u32	tshut_hot;
+	u32	tshut_cold;
+	u32	t_hot;
+	u32	t_cold;
+	u32	min_freq;
+	u32	max_freq;
+	int     max_temp;
+	int     min_temp;
+	int     hyst_val;
+	u32     adc_start_val;
+	u32     adc_end_val;
+	u32     update_int1;
+	u32     update_int2;
+};
+
+struct omap_bandgap_data;
+
+/**
+ * struct omap_bandgap - bandgap device structure
+ * @dev: device pointer
+ * @conf: platform data with sensor data
+ * @fclock: pointer to functional clock of temperature sensor
+ * @div_clk: pointer to parent clock of temperature sensor fclk
+ * @conv_table: Pointer to adc to temperature conversion table
+ * @bg_mutex: Mutex for sysfs, irq and PM
+ * @irq: MPU Irq number for thermal alert
+ * @tshut_gpio: GPIO where Tshut signal is routed
+ * @clk_rate: Holds current clock rate
+ */
+struct omap_bandgap {
+	struct device			*dev;
+	void __iomem			*base;
+	struct omap_bandgap_data	*conf;
+	struct clk			*fclock;
+	struct clk			*div_clk;
+	const int			*conv_table;
+	struct mutex			bg_mutex; /* Mutex for irq and PM */
+	int				irq;
+	int				tshut_gpio;
+	u32				clk_rate;
+};
+
+/**
+ * struct temp_sensor_regval - temperature sensor register values
+ * @bg_mode_ctrl: temp sensor control register value
+ * @bg_ctrl: bandgap ctrl register value
+ * @bg_counter: bandgap counter value
+ * @bg_threshold: bandgap threshold register value
+ * @tshut_threshold: bandgap tshut register value
+ */
+struct temp_sensor_regval {
+	u32			bg_mode_ctrl;
+	u32			bg_ctrl;
+	u32			bg_counter;
+	u32			bg_threshold;
+	u32			tshut_threshold;
+};
+
+/**
+ * struct thermal_cooling_conf - description on how to cool a thermal zone
+ * @freq_clip_count: size of freq_data
+ */
+struct thermal_cooling_conf {
+	int freq_clip_count;
+};
+
+/**
+ * struct omap_temp_sensor - bandgap temperature sensor platform data
+ * @ts_data: pointer to struct with thresholds, limits of temperature sensor
+ * @registers: pointer to the list of register offsets and bitfields
+ * @regval: temperature sensor register values
+ * @domain: the name of the domain where the sensor is located
+ * @cooling_data: description on how the zone should be cooled off.
+ * @slope: sensor gradient slope info for hotspot extrapolation
+ * @const: sensor gradient const info for hotspot extrapolation
+ * @slope_pcb: sensor gradient slope info for hotspot extrapolation
+ *             with no external influence
+ * @const_pcb: sensor gradient const info for hotspot extrapolation
+ *             with no external influence
+ * @data: private data
+ * @register_cooling: function to describe how this sensor is going to be cooled
+ * @unregister_cooling: function to release cooling data
+ */
+struct omap_temp_sensor {
+	struct temp_sensor_data		*ts_data;
+	struct temp_sensor_registers	*registers;
+	struct temp_sensor_regval	regval;
+	char				*domain;
+	struct thermal_cooling_conf	cooling_data;
+	/* for hotspot extrapolation */
+	const int			slope;
+	const int			constant;
+	const int			slope_pcb;
+	const int			constant_pcb;
+	void				*data;
+	int (*register_cooling)(struct omap_bandgap *bg_ptr, int id);
+	int (*unregister_cooling)(struct omap_bandgap *bg_ptr, int id);
+};
+
+/**
+ * struct omap_bandgap_data - bandgap platform data structure
+ * @features: a bitwise flag set to describe the device features
+ * @conv_table: Pointer to adc to temperature conversion table
+ * @fclock_name: clock name of the functional clock
+ * @div_ck_nme: clock name of the clock divisor
+ * @sensor_count: count of temperature sensor device in scm
+ * @sensors: array of sensors present in this bandgap instance
+ * @expose_sensor: callback to export sensor to thermal API
+ */
+struct omap_bandgap_data {
+#define OMAP_BANDGAP_FEATURE_TSHUT		(1 << 0)
+#define OMAP_BANDGAP_FEATURE_TSHUT_CONFIG	(1 << 1)
+#define OMAP_BANDGAP_FEATURE_TALERT		(1 << 2)
+#define OMAP_BANDGAP_FEATURE_MODE_CONFIG	(1 << 3)
+#define OMAP_BANDGAP_FEATURE_COUNTER		(1 << 4)
+#define OMAP_BANDGAP_FEATURE_POWER_SWITCH	(1 << 5)
+#define OMAP_BANDGAP_HAS(b, f)			\
+			((b)->conf->features & OMAP_BANDGAP_FEATURE_ ## f)
+	unsigned int			features;
+	const int			*conv_table;
+	char				*fclock_name;
+	char				*div_ck_name;
+	int				sensor_count;
+	int (*report_temperature)(struct omap_bandgap *bg_ptr, int id);
+	int (*expose_sensor)(struct omap_bandgap *bg_ptr, int id, char *domain);
+	int (*remove_sensor)(struct omap_bandgap *bg_ptr, int id);
+
+	/* this needs to be at the end */
+	struct omap_temp_sensor		sensors[];
+};
+
+int omap_bandgap_read_thot(struct omap_bandgap *bg_ptr, int id, int *thot);
+int omap_bandgap_write_thot(struct omap_bandgap *bg_ptr, int id, int val);
+int omap_bandgap_read_tcold(struct omap_bandgap *bg_ptr, int id, int *tcold);
+int omap_bandgap_write_tcold(struct omap_bandgap *bg_ptr, int id, int val);
+int omap_bandgap_read_update_interval(struct omap_bandgap *bg_ptr, int id,
+				      int *interval);
+int omap_bandgap_write_update_interval(struct omap_bandgap *bg_ptr, int id,
+				       u32 interval);
+int omap_bandgap_read_temperature(struct omap_bandgap *bg_ptr, int id,
+				  int *temperature);
+int omap_bandgap_set_sensor_data(struct omap_bandgap *bg_ptr, int id,
+				 void *data);
+void *omap_bandgap_get_sensor_data(struct omap_bandgap *bg_ptr, int id);
+
+#ifdef CONFIG_OMAP4_THERMAL
+extern const struct omap_bandgap_data omap4430_data;
+extern const struct omap_bandgap_data omap4460_data;
+extern const struct omap_bandgap_data omap4470_data;
+#else
+#define omap4430_data					NULL
+#define omap4460_data					NULL
+#define omap4470_data					NULL
+#endif
+
+#ifdef CONFIG_OMAP5_THERMAL
+extern const struct omap_bandgap_data omap5430_data;
+#else
+#define omap5430_data					NULL
+#endif
+
+#endif
diff --git a/drivers/staging/omap-thermal/omap-thermal-common.c b/drivers/staging/omap-thermal/omap-thermal-common.c
new file mode 100644
index 00000000000..0675a5e2f7c
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap-thermal-common.c
@@ -0,0 +1,364 @@
+/*
+ * OMAP thermal driver interface
+ *
+ * Copyright (C) 2012 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@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/device.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/gfp.h>
+#include <linux/kernel.h>
+#include <linux/workqueue.h>
+#include <linux/thermal.h>
+#include <linux/cpufreq.h>
+#include <linux/cpu_cooling.h>
+
+#include "omap-thermal.h"
+#include "omap-bandgap.h"
+
+/* common data structures */
+struct omap_thermal_data {
+	struct thermal_zone_device *omap_thermal;
+	struct thermal_cooling_device *cool_dev;
+	struct omap_bandgap *bg_ptr;
+	enum thermal_device_mode mode;
+	struct work_struct thermal_wq;
+	int sensor_id;
+};
+
+static void omap_thermal_work(struct work_struct *work)
+{
+	struct omap_thermal_data *data = container_of(work,
+					struct omap_thermal_data, thermal_wq);
+
+	thermal_zone_device_update(data->omap_thermal);
+
+	dev_dbg(&data->omap_thermal->device, "updated thermal zone %s\n",
+		data->omap_thermal->type);
+}
+
+/**
+ * omap_thermal_hotspot_temperature - returns sensor extrapolated temperature
+ * @t:	omap sensor temperature
+ * @s:	omap sensor slope value
+ * @c:	omap sensor const value
+ */
+static inline int omap_thermal_hotspot_temperature(int t, int s, int c)
+{
+	int delta = t * s / 1000 + c;
+
+	if (delta < 0)
+		delta = 0;
+
+	return t + delta;
+}
+
+/* thermal zone ops */
+/* Get temperature callback function for thermal zone*/
+static inline int omap_thermal_get_temp(struct thermal_zone_device *thermal,
+					 unsigned long *temp)
+{
+	struct omap_thermal_data *data = thermal->devdata;
+	struct omap_bandgap *bg_ptr = data->bg_ptr;
+	struct omap_temp_sensor *s = &bg_ptr->conf->sensors[data->sensor_id];
+	int ret, tmp, pcb_temp, slope, constant;
+
+	ret = omap_bandgap_read_temperature(bg_ptr, data->sensor_id, &tmp);
+	if (ret)
+		return ret;
+
+	pcb_temp = 0;
+	/* TODO: Introduce pcb temperature lookup */
+	/* In case pcb zone is available, use the extrapolation rule with it */
+	if (pcb_temp) {
+		tmp -= pcb_temp;
+		slope = s->slope_pcb;
+		constant = s->constant_pcb;
+	} else {
+		slope = s->slope;
+		constant = s->constant;
+	}
+	*temp = omap_thermal_hotspot_temperature(tmp, slope, constant);
+
+	return ret;
+}
+
+/* Bind callback functions for thermal zone */
+static int omap_thermal_bind(struct thermal_zone_device *thermal,
+			      struct thermal_cooling_device *cdev)
+{
+	struct omap_thermal_data *data = thermal->devdata;
+	int max, id;
+
+	if (IS_ERR_OR_NULL(data))
+		return -ENODEV;
+
+	/* check if this is the cooling device we registered */
+	if (data->cool_dev != cdev)
+		return 0;
+
+	id = data->sensor_id;
+	max = data->bg_ptr->conf->sensors[id].cooling_data.freq_clip_count;
+
+	/* TODO: bind with min and max states */
+	/* Simple thing, two trips, one passive another critical */
+	return thermal_zone_bind_cooling_device(thermal, 0, cdev);
+}
+
+/* Unbind callback functions for thermal zone */
+static int omap_thermal_unbind(struct thermal_zone_device *thermal,
+				struct thermal_cooling_device *cdev)
+{
+	struct omap_thermal_data *data = thermal->devdata;
+
+	if (IS_ERR_OR_NULL(data))
+		return -ENODEV;
+
+	/* check if this is the cooling device we registered */
+	if (data->cool_dev != cdev)
+		return 0;
+
+	/* Simple thing, two trips, one passive another critical */
+	return thermal_zone_unbind_cooling_device(thermal, 0, cdev);
+}
+
+/* Get mode callback functions for thermal zone */
+static int omap_thermal_get_mode(struct thermal_zone_device *thermal,
+				  enum thermal_device_mode *mode)
+{
+	struct omap_thermal_data *data = thermal->devdata;
+
+	if (data)
+		*mode = data->mode;
+
+	return 0;
+}
+
+/* Set mode callback functions for thermal zone */
+static int omap_thermal_set_mode(struct thermal_zone_device *thermal,
+				  enum thermal_device_mode mode)
+{
+	struct omap_thermal_data *data = thermal->devdata;
+
+	if (!data->omap_thermal) {
+		dev_notice(&thermal->device, "thermal zone not registered\n");
+		return 0;
+	}
+
+	mutex_lock(&data->omap_thermal->lock);
+
+	if (mode == THERMAL_DEVICE_ENABLED)
+		data->omap_thermal->polling_delay = FAST_TEMP_MONITORING_RATE;
+	else
+		data->omap_thermal->polling_delay = 0;
+
+	mutex_unlock(&data->omap_thermal->lock);
+
+	data->mode = mode;
+	thermal_zone_device_update(data->omap_thermal);
+	dev_dbg(&thermal->device, "thermal polling set for duration=%d msec\n",
+		data->omap_thermal->polling_delay);
+
+	return 0;
+}
+
+/* Get trip type callback functions for thermal zone */
+static int omap_thermal_get_trip_type(struct thermal_zone_device *thermal,
+				       int trip, enum thermal_trip_type *type)
+{
+	if (!omap_thermal_is_valid_trip(trip))
+		return -EINVAL;
+
+	if (trip + 1 == OMAP_TRIP_NUMBER)
+		*type = THERMAL_TRIP_CRITICAL;
+	else
+		*type = THERMAL_TRIP_PASSIVE;
+
+	return 0;
+}
+
+/* Get trip temperature callback functions for thermal zone */
+static int omap_thermal_get_trip_temp(struct thermal_zone_device *thermal,
+				       int trip, unsigned long *temp)
+{
+	if (!omap_thermal_is_valid_trip(trip))
+		return -EINVAL;
+
+	*temp = omap_thermal_get_trip_value(trip);
+
+	return 0;
+}
+
+/* Get critical temperature callback functions for thermal zone */
+static int omap_thermal_get_crit_temp(struct thermal_zone_device *thermal,
+				       unsigned long *temp)
+{
+	/* shutdown zone */
+	return omap_thermal_get_trip_temp(thermal, OMAP_TRIP_NUMBER - 1, temp);
+}
+
+static struct thermal_zone_device_ops omap_thermal_ops = {
+	.get_temp = omap_thermal_get_temp,
+	/* TODO: add .get_trend */
+	.bind = omap_thermal_bind,
+	.unbind = omap_thermal_unbind,
+	.get_mode = omap_thermal_get_mode,
+	.set_mode = omap_thermal_set_mode,
+	.get_trip_type = omap_thermal_get_trip_type,
+	.get_trip_temp = omap_thermal_get_trip_temp,
+	.get_crit_temp = omap_thermal_get_crit_temp,
+};
+
+int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
+			       char *domain)
+{
+	struct omap_thermal_data *data;
+
+	data = devm_kzalloc(bg_ptr->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(bg_ptr->dev, "kzalloc fail\n");
+		return -ENOMEM;
+	}
+	data->sensor_id = id;
+	data->bg_ptr = bg_ptr;
+	data->mode = THERMAL_DEVICE_ENABLED;
+	INIT_WORK(&data->thermal_wq, omap_thermal_work);
+
+	/* TODO: remove TC1 TC2 */
+	/* Create thermal zone */
+	data->omap_thermal = thermal_zone_device_register(domain,
+				OMAP_TRIP_NUMBER, 0, data, &omap_thermal_ops,
+				0, FAST_TEMP_MONITORING_RATE, 0, 0);
+	if (IS_ERR_OR_NULL(data->omap_thermal)) {
+		dev_err(bg_ptr->dev, "thermal zone device is NULL\n");
+		return PTR_ERR(data->omap_thermal);
+	}
+	data->omap_thermal->polling_delay = FAST_TEMP_MONITORING_RATE;
+	omap_bandgap_set_sensor_data(bg_ptr, id, data);
+
+	return 0;
+}
+
+int omap_thermal_remove_sensor(struct omap_bandgap *bg_ptr, int id)
+{
+	struct omap_thermal_data *data;
+
+	data = omap_bandgap_get_sensor_data(bg_ptr, id);
+
+	thermal_zone_device_unregister(data->omap_thermal);
+
+	return 0;
+}
+
+int omap_thermal_report_sensor_temperature(struct omap_bandgap *bg_ptr, int id)
+{
+	struct omap_thermal_data *data;
+
+	data = omap_bandgap_get_sensor_data(bg_ptr, id);
+
+	schedule_work(&data->thermal_wq);
+
+	return 0;
+}
+
+static int omap_thermal_build_cpufreq_clip(struct omap_bandgap *bg_ptr,
+					   struct freq_clip_table **tab_ptr,
+					   int *tab_size)
+{
+	struct cpufreq_frequency_table *freq_table;
+	struct freq_clip_table *tab;
+	int i, count = 0;
+
+	freq_table = cpufreq_frequency_get_table(0);
+	if (IS_ERR_OR_NULL(freq_table)) {
+		dev_err(bg_ptr->dev,
+			"%s: failed to get cpufreq table (%p)\n",
+			__func__, freq_table);
+		return -EINVAL;
+	}
+
+	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
+		unsigned int freq = freq_table[i].frequency;
+		if (freq == CPUFREQ_ENTRY_INVALID)
+			continue;
+		count++;
+	}
+
+	tab = devm_kzalloc(bg_ptr->dev, sizeof(*tab) * count, GFP_KERNEL);
+	if (!tab) {
+		dev_err(bg_ptr->dev,
+			"%s: no memory available\n", __func__);
+		return -ENOMEM;
+	}
+
+	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
+		unsigned int freq = freq_table[i].frequency;
+
+		if (freq == CPUFREQ_ENTRY_INVALID)
+			continue;
+
+		tab[count - i - 1].freq_clip_max = freq;
+		tab[count - i - 1].temp_level = OMAP_TRIP_HOT;
+		tab[count - i - 1].mask_val = cpumask_of(0);
+	}
+
+	*tab_ptr = tab;
+	*tab_size = count;
+
+	return 0;
+}
+
+int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
+{
+	struct omap_thermal_data *data;
+	struct freq_clip_table *tab_ptr;
+	int tab_size, ret;
+
+	data = omap_bandgap_get_sensor_data(bg_ptr, id);
+
+	ret = omap_thermal_build_cpufreq_clip(bg_ptr, &tab_ptr, &tab_size);
+	if (ret < 0) {
+		dev_err(bg_ptr->dev,
+			"%s: failed to build cpufreq clip table\n", __func__);
+		return ret;
+	}
+
+	/* Register cooling device */
+	data->cool_dev = cpufreq_cooling_register(tab_ptr, tab_size);
+	if (IS_ERR_OR_NULL(data->cool_dev)) {
+		dev_err(bg_ptr->dev,
+			"Failed to register cpufreq cooling device\n");
+		return PTR_ERR(data->cool_dev);
+	}
+	bg_ptr->conf->sensors[id].cooling_data.freq_clip_count = tab_size;
+
+	return 0;
+}
+
+int omap_thermal_unregister_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
+{
+	struct omap_thermal_data *data;
+
+	data = omap_bandgap_get_sensor_data(bg_ptr, id);
+	cpufreq_cooling_unregister(data->cool_dev);
+
+	return 0;
+}
diff --git a/drivers/staging/omap-thermal/omap-thermal.h b/drivers/staging/omap-thermal/omap-thermal.h
new file mode 100644
index 00000000000..0dd2184b966
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap-thermal.h
@@ -0,0 +1,108 @@
+/*
+ * OMAP thermal definitions
+ *
+ * Copyright (C) 2012 Texas Instruments Incorporated - http://www.ti.com/
+ * Contact:
+ *   Eduardo Valentin <eduardo.valentin@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
+ *
+ */
+#ifndef __OMAP_THERMAL_H
+#define __OMAP_THERMAL_H
+
+#include "omap-bandgap.h"
+
+/* sensors gradient and offsets */
+#define OMAP_GRADIENT_SLOPE_4460				348
+#define OMAP_GRADIENT_CONST_4460				-9301
+#define OMAP_GRADIENT_SLOPE_4470				308
+#define OMAP_GRADIENT_CONST_4470				-7896
+
+#define OMAP_GRADIENT_SLOPE_5430_CPU				196
+#define OMAP_GRADIENT_CONST_5430_CPU				-6822
+#define OMAP_GRADIENT_SLOPE_5430_GPU				64
+#define OMAP_GRADIENT_CONST_5430_GPU				978
+
+/* PCB sensor calculation constants */
+#define OMAP_GRADIENT_SLOPE_W_PCB_4460				1142
+#define OMAP_GRADIENT_CONST_W_PCB_4460				-393
+#define OMAP_GRADIENT_SLOPE_W_PCB_4470				1063
+#define OMAP_GRADIENT_CONST_W_PCB_4470				-477
+
+#define OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU			469
+#define OMAP_GRADIENT_CONST_W_PCB_5430_CPU			-1272
+#define OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU			378
+#define OMAP_GRADIENT_CONST_W_PCB_5430_GPU			154
+
+/* trip points of interest in milicelsius (at hotspot level) */
+#define OMAP_TRIP_COLD						100000
+#define OMAP_TRIP_HOT						110000
+#define OMAP_TRIP_SHUTDOWN					125000
+#define OMAP_TRIP_NUMBER					2
+#define OMAP_TRIP_STEP							\
+	((OMAP_TRIP_SHUTDOWN - OMAP_TRIP_HOT) / (OMAP_TRIP_NUMBER - 1))
+
+/* Update rates */
+#define FAST_TEMP_MONITORING_RATE				250
+
+/* helper macros */
+/**
+ * omap_thermal_get_trip_value - returns trip temperature based on index
+ * @i:	trip index
+ */
+#define omap_thermal_get_trip_value(i)					\
+	(OMAP_TRIP_HOT + ((i) * OMAP_TRIP_STEP))
+
+/**
+ * omap_thermal_is_valid_trip - check for trip index
+ * @i:	trip index
+ */
+#define omap_thermal_is_valid_trip(trip)				\
+	((trip) >= 0 && (trip) < OMAP_TRIP_NUMBER)
+
+#ifdef CONFIG_OMAP_THERMAL
+int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
+			       char *domain);
+int omap_thermal_remove_sensor(struct omap_bandgap *bg_ptr, int id);
+int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id);
+int omap_thermal_unregister_cpu_cooling(struct omap_bandgap *bg_ptr, int id);
+#else
+static inline
+int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
+			       char *domain)
+{
+	return 0;
+}
+
+static inline
+int omap_thermal_remove_sensor(struct omap_bandgap *bg_ptr, int id)
+{
+	return 0;
+}
+
+static inline
+int omap_thermal_register_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
+{
+	return 0;
+}
+
+static inline
+int omap_thermal_unregister_cpu_cooling(struct omap_bandgap *bg_ptr, int id)
+{
+	return 0;
+}
+#endif
+#endif
diff --git a/drivers/staging/omap-thermal/omap4-thermal.c b/drivers/staging/omap-thermal/omap4-thermal.c
new file mode 100644
index 00000000000..fa9dbcd7183
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap4-thermal.c
@@ -0,0 +1,259 @@
+/*
+ * OMAP4 thermal driver.
+ *
+ * Copyright (C) 2011-2012 Texas Instruments Inc.
+ * Contact:
+ *	Eduardo Valentin <eduardo.valentin@ti.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 "omap-thermal.h"
+#include "omap-bandgap.h"
+
+/*
+ * OMAP4430 has one instance of thermal sensor for MPU
+ * need to describe the individual bit fields
+ */
+static struct temp_sensor_registers
+omap4430_mpu_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP4430_TEMP_SENSOR_CTRL_OFFSET,
+	.bgap_tempsoff_mask = OMAP4430_BGAP_TEMPSOFF_MASK,
+	.bgap_soc_mask = OMAP4430_BGAP_TEMP_SENSOR_SOC_MASK,
+	.bgap_eocz_mask = OMAP4430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP4430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mode_ctrl = OMAP4430_TEMP_SENSOR_CTRL_OFFSET,
+	.mode_ctrl_mask = OMAP4430_SINGLE_MODE_MASK,
+
+	.bgap_efuse = OMAP4430_FUSE_OPP_BGAP,
+};
+
+/* Thresholds and limits for OMAP4430 MPU temperature sensor */
+static struct temp_sensor_data omap4430_mpu_temp_sensor_data = {
+	.min_freq = OMAP4430_MIN_FREQ,
+	.max_freq = OMAP4430_MAX_FREQ,
+	.max_temp = OMAP4430_MAX_TEMP,
+	.min_temp = OMAP4430_MIN_TEMP,
+	.hyst_val = OMAP4430_HYST_VAL,
+	.adc_start_val = OMAP4430_ADC_START_VALUE,
+	.adc_end_val = OMAP4430_ADC_END_VALUE,
+};
+
+/*
+ * Temperature values in milli degree celsius
+ * ADC code values from 530 to 923
+ */
+static const int
+omap4430_adc_to_temp[OMAP4430_ADC_END_VALUE - OMAP4430_ADC_START_VALUE + 1] = {
+	-38000, -35000, -34000, -32000, -30000, -28000, -26000, -24000, -22000,
+	-20000, -18000, -17000, -15000, -13000, -12000, -10000, -8000, -6000,
+	-5000, -3000, -1000, 0, 2000, 3000, 5000, 6000, 8000, 10000, 12000,
+	13000, 15000, 17000, 19000, 21000, 23000, 25000, 27000, 28000, 30000,
+	32000, 33000, 35000, 37000, 38000, 40000, 42000, 43000, 45000, 47000,
+	48000, 50000, 52000, 53000, 55000, 57000, 58000, 60000, 62000, 64000,
+	66000, 68000, 70000, 71000, 73000, 75000, 77000, 78000, 80000, 82000,
+	83000, 85000, 87000, 88000, 90000, 92000, 93000, 95000, 97000, 98000,
+	100000, 102000, 103000, 105000, 107000, 109000, 111000, 113000, 115000,
+	117000, 118000, 120000, 122000, 123000,
+};
+
+/* OMAP4430 data */
+const struct omap_bandgap_data omap4430_data = {
+	.features = OMAP_BANDGAP_FEATURE_MODE_CONFIG |
+			OMAP_BANDGAP_FEATURE_POWER_SWITCH,
+	.fclock_name = "bandgap_fclk",
+	.div_ck_name = "bandgap_fclk",
+	.conv_table = omap4430_adc_to_temp,
+	.expose_sensor = omap_thermal_expose_sensor,
+	.remove_sensor = omap_thermal_remove_sensor,
+	.sensors = {
+		{
+			.registers = &omap4430_mpu_temp_sensor_registers,
+			.ts_data = &omap4430_mpu_temp_sensor_data,
+			.domain = "cpu",
+			.slope = 0,
+			.constant = 20000,
+			.slope_pcb = 0,
+			.constant_pcb = 20000,
+			.register_cooling = omap_thermal_register_cpu_cooling,
+			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		},
+	},
+	.sensor_count = 1,
+};
+/*
+ * OMAP4460 has one instance of thermal sensor for MPU
+ * need to describe the individual bit fields
+ */
+static struct temp_sensor_registers
+omap4460_mpu_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP4460_TEMP_SENSOR_CTRL_OFFSET,
+	.bgap_tempsoff_mask = OMAP4460_BGAP_TEMPSOFF_MASK,
+	.bgap_soc_mask = OMAP4460_BGAP_TEMP_SENSOR_SOC_MASK,
+	.bgap_eocz_mask = OMAP4460_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP4460_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP4460_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP4460_MASK_HOT_MASK,
+	.mask_cold_mask = OMAP4460_MASK_COLD_MASK,
+
+	.bgap_mode_ctrl = OMAP4460_BGAP_CTRL_OFFSET,
+	.mode_ctrl_mask = OMAP4460_SINGLE_MODE_MASK,
+
+	.bgap_counter = OMAP4460_BGAP_COUNTER_OFFSET,
+	.counter_mask = OMAP4460_COUNTER_MASK,
+
+	.bgap_threshold = OMAP4460_BGAP_THRESHOLD_OFFSET,
+	.threshold_thot_mask = OMAP4460_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP4460_T_COLD_MASK,
+
+	.tshut_threshold = OMAP4460_BGAP_TSHUT_OFFSET,
+	.tshut_hot_mask = OMAP4460_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP4460_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP4460_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = OMAP4460_CLEAN_STOP_MASK,
+	.status_bgap_alert_mask = OMAP4460_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP4460_HOT_FLAG_MASK,
+	.status_cold_mask = OMAP4460_COLD_FLAG_MASK,
+
+	.bgap_efuse = OMAP4460_FUSE_OPP_BGAP,
+};
+
+/* Thresholds and limits for OMAP4460 MPU temperature sensor */
+static struct temp_sensor_data omap4460_mpu_temp_sensor_data = {
+	.tshut_hot = OMAP4460_TSHUT_HOT,
+	.tshut_cold = OMAP4460_TSHUT_COLD,
+	.t_hot = OMAP4460_T_HOT,
+	.t_cold = OMAP4460_T_COLD,
+	.min_freq = OMAP4460_MIN_FREQ,
+	.max_freq = OMAP4460_MAX_FREQ,
+	.max_temp = OMAP4460_MAX_TEMP,
+	.min_temp = OMAP4460_MIN_TEMP,
+	.hyst_val = OMAP4460_HYST_VAL,
+	.adc_start_val = OMAP4460_ADC_START_VALUE,
+	.adc_end_val = OMAP4460_ADC_END_VALUE,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+/*
+ * Temperature values in milli degree celsius
+ * ADC code values from 530 to 923
+ */
+static const int
+omap4460_adc_to_temp[OMAP4460_ADC_END_VALUE - OMAP4460_ADC_START_VALUE + 1] = {
+	-40000, -40000, -40000, -40000, -39800, -39400, -39000, -38600, -38200,
+	-37800, -37300, -36800, -36400, -36000, -35600, -35200, -34800,
+	-34300, -33800, -33400, -33000, -32600, -32200, -31800, -31300,
+	-30800, -30400, -30000, -29600, -29200, -28700, -28200, -27800,
+	-27400, -27000, -26600, -26200, -25700, -25200, -24800, -24400,
+	-24000, -23600, -23200, -22700, -22200, -21800, -21400, -21000,
+	-20600, -20200, -19700, -19200, -18800, -18400, -18000, -17600,
+	-17200, -16700, -16200, -15800, -15400, -15000, -14600, -14200,
+	-13700, -13200, -12800, -12400, -12000, -11600, -11200, -10700,
+	-10200, -9800, -9400, -9000, -8600, -8200, -7700, -7200, -6800,
+	-6400, -6000, -5600, -5200, -4800, -4300, -3800, -3400, -3000,
+	-2600, -2200, -1800, -1300, -800, -400, 0, 400, 800, 1200, 1600,
+	2100, 2600, 3000, 3400, 3800, 4200, 4600, 5100, 5600, 6000, 6400,
+	6800, 7200, 7600, 8000, 8500, 9000, 9400, 9800, 10200, 10600, 11000,
+	11400, 11900, 12400, 12800, 13200, 13600, 14000, 14400, 14800,
+	15300, 15800, 16200, 16600, 17000, 17400, 17800, 18200, 18700,
+	19200, 19600, 20000, 20400, 20800, 21200, 21600, 22100, 22600,
+	23000, 23400, 23800, 24200, 24600, 25000, 25400, 25900, 26400,
+	26800, 27200, 27600, 28000, 28400, 28800, 29300, 29800, 30200,
+	30600, 31000, 31400, 31800, 32200, 32600, 33100, 33600, 34000,
+	34400, 34800, 35200, 35600, 36000, 36400, 36800, 37300, 37800,
+	38200, 38600, 39000, 39400, 39800, 40200, 40600, 41100, 41600,
+	42000, 42400, 42800, 43200, 43600, 44000, 44400, 44800, 45300,
+	45800, 46200, 46600, 47000, 47400, 47800, 48200, 48600, 49000,
+	49500, 50000, 50400, 50800, 51200, 51600, 52000, 52400, 52800,
+	53200, 53700, 54200, 54600, 55000, 55400, 55800, 56200, 56600,
+	57000, 57400, 57800, 58200, 58700, 59200, 59600, 60000, 60400,
+	60800, 61200, 61600, 62000, 62400, 62800, 63300, 63800, 64200,
+	64600, 65000, 65400, 65800, 66200, 66600, 67000, 67400, 67800,
+	68200, 68700, 69200, 69600, 70000, 70400, 70800, 71200, 71600,
+	72000, 72400, 72800, 73200, 73600, 74100, 74600, 75000, 75400,
+	75800, 76200, 76600, 77000, 77400, 77800, 78200, 78600, 79000,
+	79400, 79800, 80300, 80800, 81200, 81600, 82000, 82400, 82800,
+	83200, 83600, 84000, 84400, 84800, 85200, 85600, 86000, 86400,
+	86800, 87300, 87800, 88200, 88600, 89000, 89400, 89800, 90200,
+	90600, 91000, 91400, 91800, 92200, 92600, 93000, 93400, 93800,
+	94200, 94600, 95000, 95500, 96000, 96400, 96800, 97200, 97600,
+	98000, 98400, 98800, 99200, 99600, 100000, 100400, 100800, 101200,
+	101600, 102000, 102400, 102800, 103200, 103600, 104000, 104400,
+	104800, 105200, 105600, 106100, 106600, 107000, 107400, 107800,
+	108200, 108600, 109000, 109400, 109800, 110200, 110600, 111000,
+	111400, 111800, 112200, 112600, 113000, 113400, 113800, 114200,
+	114600, 115000, 115400, 115800, 116200, 116600, 117000, 117400,
+	117800, 118200, 118600, 119000, 119400, 119800, 120200, 120600,
+	121000, 121400, 121800, 122200, 122600, 123000, 123400, 123800, 124200,
+	124600, 124900, 125000, 125000, 125000, 125000
+};
+
+/* OMAP4460 data */
+const struct omap_bandgap_data omap4460_data = {
+	.features = OMAP_BANDGAP_FEATURE_TSHUT |
+			OMAP_BANDGAP_FEATURE_TSHUT_CONFIG |
+			OMAP_BANDGAP_FEATURE_TALERT |
+			OMAP_BANDGAP_FEATURE_MODE_CONFIG |
+			OMAP_BANDGAP_FEATURE_POWER_SWITCH |
+			OMAP_BANDGAP_FEATURE_COUNTER,
+	.fclock_name = "bandgap_ts_fclk",
+	.div_ck_name = "div_ts_ck",
+	.conv_table = omap4460_adc_to_temp,
+	.expose_sensor = omap_thermal_expose_sensor,
+	.remove_sensor = omap_thermal_remove_sensor,
+	.sensors = {
+		{
+			.registers = &omap4460_mpu_temp_sensor_registers,
+			.ts_data = &omap4460_mpu_temp_sensor_data,
+			.domain = "cpu",
+			.slope = OMAP_GRADIENT_SLOPE_4460,
+			.constant = OMAP_GRADIENT_CONST_4460,
+			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4460,
+			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4460,
+			.register_cooling = omap_thermal_register_cpu_cooling,
+			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		},
+	},
+	.sensor_count = 1,
+};
+
+/* OMAP4470 data */
+const struct omap_bandgap_data omap4470_data = {
+	.features = OMAP_BANDGAP_FEATURE_TSHUT |
+			OMAP_BANDGAP_FEATURE_TSHUT_CONFIG |
+			OMAP_BANDGAP_FEATURE_TALERT |
+			OMAP_BANDGAP_FEATURE_MODE_CONFIG |
+			OMAP_BANDGAP_FEATURE_POWER_SWITCH |
+			OMAP_BANDGAP_FEATURE_COUNTER,
+	.fclock_name = "bandgap_ts_fclk",
+	.div_ck_name = "div_ts_ck",
+	.conv_table = omap4460_adc_to_temp,
+	.expose_sensor = omap_thermal_expose_sensor,
+	.remove_sensor = omap_thermal_remove_sensor,
+	.sensors = {
+		{
+			.registers = &omap4460_mpu_temp_sensor_registers,
+			.ts_data = &omap4460_mpu_temp_sensor_data,
+			.domain = "cpu",
+			.slope = OMAP_GRADIENT_SLOPE_4470,
+			.constant = OMAP_GRADIENT_CONST_4470,
+			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_4470,
+			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_4470,
+			.register_cooling = omap_thermal_register_cpu_cooling,
+			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+		},
+	},
+	.sensor_count = 1,
+};
diff --git a/drivers/staging/omap-thermal/omap5-thermal.c b/drivers/staging/omap-thermal/omap5-thermal.c
new file mode 100644
index 00000000000..0658af24a5c
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap5-thermal.c
@@ -0,0 +1,297 @@
+/*
+ * OMAP5 thermal driver.
+ *
+ * Copyright (C) 2011-2012 Texas Instruments Inc.
+ * Contact:
+ *	Eduardo Valentin <eduardo.valentin@ti.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 "omap-bandgap.h"
+#include "omap-thermal.h"
+
+/*
+ * omap5430 has one instance of thermal sensor for MPU
+ * need to describe the individual bit fields
+ */
+static struct temp_sensor_registers
+omap5430_mpu_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_MPU_OFFSET,
+	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
+	.bgap_soc_mask = OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK,
+	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP5430_MASK_HOT_MPU_MASK,
+	.mask_cold_mask = OMAP5430_MASK_COLD_MPU_MASK,
+
+	.bgap_mode_ctrl = OMAP5430_BGAP_COUNTER_MPU_OFFSET,
+	.mode_ctrl_mask = OMAP5430_REPEAT_MODE_MASK,
+
+	.bgap_counter = OMAP5430_BGAP_COUNTER_MPU_OFFSET,
+	.counter_mask = OMAP5430_COUNTER_MASK,
+
+	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_MPU_OFFSET,
+	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
+
+	.tshut_threshold = OMAP5430_BGAP_TSHUT_MPU_OFFSET,
+	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = 0x0,
+	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP5430_HOT_MPU_FLAG_MASK,
+	.status_cold_mask = OMAP5430_COLD_MPU_FLAG_MASK,
+
+	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_MPU,
+};
+
+/*
+ * omap5430 has one instance of thermal sensor for GPU
+ * need to describe the individual bit fields
+ */
+static struct temp_sensor_registers
+omap5430_gpu_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_GPU_OFFSET,
+	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
+	.bgap_soc_mask = OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK,
+	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP5430_MASK_HOT_MM_MASK,
+	.mask_cold_mask = OMAP5430_MASK_COLD_MM_MASK,
+
+	.bgap_mode_ctrl = OMAP5430_BGAP_COUNTER_GPU_OFFSET,
+	.mode_ctrl_mask = OMAP5430_REPEAT_MODE_MASK,
+
+	.bgap_counter = OMAP5430_BGAP_COUNTER_GPU_OFFSET,
+	.counter_mask = OMAP5430_COUNTER_MASK,
+
+	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_GPU_OFFSET,
+	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
+
+	.tshut_threshold = OMAP5430_BGAP_TSHUT_GPU_OFFSET,
+	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = 0x0,
+	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP5430_HOT_MM_FLAG_MASK,
+	.status_cold_mask = OMAP5430_COLD_MM_FLAG_MASK,
+
+	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_GPU,
+};
+
+/*
+ * omap5430 has one instance of thermal sensor for CORE
+ * need to describe the individual bit fields
+ */
+static struct temp_sensor_registers
+omap5430_core_temp_sensor_registers = {
+	.temp_sensor_ctrl = OMAP5430_TEMP_SENSOR_CORE_OFFSET,
+	.bgap_tempsoff_mask = OMAP5430_BGAP_TEMPSOFF_MASK,
+	.bgap_soc_mask = OMAP5430_BGAP_TEMP_SENSOR_SOC_MASK,
+	.bgap_eocz_mask = OMAP5430_BGAP_TEMP_SENSOR_EOCZ_MASK,
+	.bgap_dtemp_mask = OMAP5430_BGAP_TEMP_SENSOR_DTEMP_MASK,
+
+	.bgap_mask_ctrl = OMAP5430_BGAP_CTRL_OFFSET,
+	.mask_hot_mask = OMAP5430_MASK_HOT_CORE_MASK,
+	.mask_cold_mask = OMAP5430_MASK_COLD_CORE_MASK,
+
+	.bgap_mode_ctrl = OMAP5430_BGAP_COUNTER_CORE_OFFSET,
+	.mode_ctrl_mask = OMAP5430_REPEAT_MODE_MASK,
+
+	.bgap_counter = OMAP5430_BGAP_COUNTER_CORE_OFFSET,
+	.counter_mask = OMAP5430_COUNTER_MASK,
+
+	.bgap_threshold = OMAP5430_BGAP_THRESHOLD_CORE_OFFSET,
+	.threshold_thot_mask = OMAP5430_T_HOT_MASK,
+	.threshold_tcold_mask = OMAP5430_T_COLD_MASK,
+
+	.tshut_threshold = OMAP5430_BGAP_TSHUT_CORE_OFFSET,
+	.tshut_hot_mask = OMAP5430_TSHUT_HOT_MASK,
+	.tshut_cold_mask = OMAP5430_TSHUT_COLD_MASK,
+
+	.bgap_status = OMAP5430_BGAP_STATUS_OFFSET,
+	.status_clean_stop_mask = 0x0,
+	.status_bgap_alert_mask = OMAP5430_BGAP_ALERT_MASK,
+	.status_hot_mask = OMAP5430_HOT_CORE_FLAG_MASK,
+	.status_cold_mask = OMAP5430_COLD_CORE_FLAG_MASK,
+
+	.bgap_efuse = OMAP5430_FUSE_OPP_BGAP_CORE,
+};
+
+/* Thresholds and limits for OMAP5430 MPU temperature sensor */
+static struct temp_sensor_data omap5430_mpu_temp_sensor_data = {
+	.tshut_hot = OMAP5430_MPU_TSHUT_HOT,
+	.tshut_cold = OMAP5430_MPU_TSHUT_COLD,
+	.t_hot = OMAP5430_MPU_T_HOT,
+	.t_cold = OMAP5430_MPU_T_COLD,
+	.min_freq = OMAP5430_MPU_MIN_FREQ,
+	.max_freq = OMAP5430_MPU_MAX_FREQ,
+	.max_temp = OMAP5430_MPU_MAX_TEMP,
+	.min_temp = OMAP5430_MPU_MIN_TEMP,
+	.hyst_val = OMAP5430_MPU_HYST_VAL,
+	.adc_start_val = OMAP5430_ADC_START_VALUE,
+	.adc_end_val = OMAP5430_ADC_END_VALUE,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+/* Thresholds and limits for OMAP5430 GPU temperature sensor */
+static struct temp_sensor_data omap5430_gpu_temp_sensor_data = {
+	.tshut_hot = OMAP5430_GPU_TSHUT_HOT,
+	.tshut_cold = OMAP5430_GPU_TSHUT_COLD,
+	.t_hot = OMAP5430_GPU_T_HOT,
+	.t_cold = OMAP5430_GPU_T_COLD,
+	.min_freq = OMAP5430_GPU_MIN_FREQ,
+	.max_freq = OMAP5430_GPU_MAX_FREQ,
+	.max_temp = OMAP5430_GPU_MAX_TEMP,
+	.min_temp = OMAP5430_GPU_MIN_TEMP,
+	.hyst_val = OMAP5430_GPU_HYST_VAL,
+	.adc_start_val = OMAP5430_ADC_START_VALUE,
+	.adc_end_val = OMAP5430_ADC_END_VALUE,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+/* Thresholds and limits for OMAP5430 CORE temperature sensor */
+static struct temp_sensor_data omap5430_core_temp_sensor_data = {
+	.tshut_hot = OMAP5430_CORE_TSHUT_HOT,
+	.tshut_cold = OMAP5430_CORE_TSHUT_COLD,
+	.t_hot = OMAP5430_CORE_T_HOT,
+	.t_cold = OMAP5430_CORE_T_COLD,
+	.min_freq = OMAP5430_CORE_MIN_FREQ,
+	.max_freq = OMAP5430_CORE_MAX_FREQ,
+	.max_temp = OMAP5430_CORE_MAX_TEMP,
+	.min_temp = OMAP5430_CORE_MIN_TEMP,
+	.hyst_val = OMAP5430_CORE_HYST_VAL,
+	.adc_start_val = OMAP5430_ADC_START_VALUE,
+	.adc_end_val = OMAP5430_ADC_END_VALUE,
+	.update_int1 = 1000,
+	.update_int2 = 2000,
+};
+
+static const int
+omap5430_adc_to_temp[OMAP5430_ADC_END_VALUE - OMAP5430_ADC_START_VALUE + 1] = {
+	-40000, -40000, -40000, -40000, -39800, -39400, -39000, -38600,
+	-38200, -37800, -37300, -36800,
+	-36400, -36000, -35600, -35200, -34800, -34300, -33800, -33400, -33000,
+	-32600,
+	-32200, -31800, -31300, -30800, -30400, -30000, -29600, -29200, -28700,
+	-28200, -27800, -27400, -27000, -26600, -26200, -25700, -25200, -24800,
+	-24400, -24000, -23600, -23200, -22700, -22200, -21800, -21400, -21000,
+	-20600, -20200, -19700, -19200, -9300, -18400, -18000, -17600, -17200,
+	-16700, -16200, -15800, -15400, -15000, -14600, -14200, -13700, -13200,
+	-12800, -12400, -12000, -11600, -11200, -10700, -10200, -9800, -9400,
+	-9000,
+	-8600, -8200, -7700, -7200, -6800, -6400, -6000, -5600, -5200, -4800,
+	-4300,
+	-3800, -3400, -3000, -2600, -2200, -1800, -1300, -800, -400, 0, 400,
+	800,
+	1200, 1600, 2100, 2600, 3000, 3400, 3800, 4200, 4600, 5100, 5600, 6000,
+	6400, 6800, 7200, 7600, 8000, 8500, 9000, 9400, 9800, 10200, 10800,
+	11100,
+	11400, 11900, 12400, 12800, 13200, 13600, 14000, 14400, 14800, 15300,
+	15800,
+	16200, 16600, 17000, 17400, 17800, 18200, 18700, 19200, 19600, 20000,
+	20400,
+	20800, 21200, 21600, 22100, 22600, 23000, 23400, 23800, 24200, 24600,
+	25000,
+	25400, 25900, 26400, 26800, 27200, 27600, 28000, 28400, 28800, 29300,
+	29800,
+	30200, 30600, 31000, 31400, 31800, 32200, 32600, 33100, 33600, 34000,
+	34400,
+	34800, 35200, 35600, 36000, 36400, 36800, 37300, 37800, 38200, 38600,
+	39000,
+	39400, 39800, 40200, 40600, 41100, 41600, 42000, 42400, 42800, 43200,
+	43600,
+	44000, 44400, 44800, 45300, 45800, 46200, 46600, 47000, 47400, 47800,
+	48200,
+	48600, 49000, 49500, 50000, 50400, 50800, 51200, 51600, 52000, 52400,
+	52800,
+	53200, 53700, 54200, 54600, 55000, 55400, 55800, 56200, 56600, 57000,
+	57400,
+	57800, 58200, 58700, 59200, 59600, 60000, 60400, 60800, 61200, 61600,
+	62000,
+	62400, 62800, 63300, 63800, 64200, 64600, 65000, 65400, 65800, 66200,
+	66600,
+	67000, 67400, 67800, 68200, 68700, 69200, 69600, 70000, 70400, 70800,
+	71200,
+	71600, 72000, 72400, 72800, 73200, 73600, 74100, 74600, 75000, 75400,
+	75800,
+	76200, 76600, 77000, 77400, 77800, 78200, 78600, 79000, 79400, 79800,
+	80300,
+	80800, 81200, 81600, 82000, 82400, 82800, 83200, 83600, 84000, 84400,
+	84800,
+	85200, 85600, 86000, 86400, 86800, 87300, 87800, 88200, 88600, 89000,
+	89400,
+	89800, 90200, 90600, 91000, 91400, 91800, 92200, 92600, 93000, 93400,
+	93800,
+	94200, 94600, 95000, 95500, 96000, 96400, 96800, 97200, 97600, 98000,
+	98400,
+	98800, 99200, 99600, 100000, 100400, 100800, 101200, 101600, 102000,
+	102400,
+	102800, 103200, 103600, 104000, 104400, 104800, 105200, 105600, 106100,
+	106600, 107000, 107400, 107800, 108200, 108600, 109000, 109400, 109800,
+	110200, 110600, 111000, 111400, 111800, 112200, 112600, 113000, 113400,
+	113800, 114200, 114600, 115000, 115400, 115800, 116200, 116600, 117000,
+	117400, 117800, 118200, 118600, 119000, 119400, 119800, 120200, 120600,
+	121000, 121400, 121800, 122200, 122600, 123000, 123400, 123800, 124200,
+	124600, 124900, 125000, 125000, 125000, 125000,
+};
+
+const struct omap_bandgap_data omap5430_data = {
+	.features = OMAP_BANDGAP_FEATURE_TSHUT_CONFIG |
+			OMAP_BANDGAP_FEATURE_TALERT |
+			OMAP_BANDGAP_FEATURE_MODE_CONFIG |
+			OMAP_BANDGAP_FEATURE_COUNTER,
+	.fclock_name = "ts_clk_div_ck",
+	.div_ck_name = "ts_clk_div_ck",
+	.conv_table = omap5430_adc_to_temp,
+	.expose_sensor = omap_thermal_expose_sensor,
+	.remove_sensor = omap_thermal_remove_sensor,
+	.sensors = {
+		{
+			.registers = &omap5430_mpu_temp_sensor_registers,
+			.ts_data = &omap5430_mpu_temp_sensor_data,
+			.domain = "cpu",
+			.register_cooling = omap_thermal_register_cpu_cooling,
+			.unregister_cooling = omap_thermal_unregister_cpu_cooling,
+			.slope = OMAP_GRADIENT_SLOPE_5430_CPU,
+			.constant = OMAP_GRADIENT_CONST_5430_CPU,
+			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_CPU,
+			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_CPU,
+		},
+		{
+			.registers = &omap5430_gpu_temp_sensor_registers,
+			.ts_data = &omap5430_gpu_temp_sensor_data,
+			.domain = "gpu",
+			.slope = OMAP_GRADIENT_SLOPE_5430_GPU,
+			.constant = OMAP_GRADIENT_CONST_5430_GPU,
+			.slope_pcb = OMAP_GRADIENT_SLOPE_W_PCB_5430_GPU,
+			.constant_pcb = OMAP_GRADIENT_CONST_W_PCB_5430_GPU,
+		},
+		{
+			.registers = &omap5430_core_temp_sensor_registers,
+			.ts_data = &omap5430_core_temp_sensor_data,
+			.domain = "core",
+		},
+	},
+	.sensor_count = 3,
+};
diff --git a/drivers/staging/omap-thermal/omap_bandgap.txt b/drivers/staging/omap-thermal/omap_bandgap.txt
new file mode 100644
index 00000000000..6008a1452fd
--- /dev/null
+++ b/drivers/staging/omap-thermal/omap_bandgap.txt
@@ -0,0 +1,30 @@
+* Texas Instrument OMAP SCM bandgap bindings
+
+In the System Control Module, OMAP supplies a voltage reference
+and a temperature sensor feature that are gathered in the band
+gap voltage and temperature sensor (VBGAPTS) module. The band
+gap provides current and voltage reference for its internal
+circuits and other analog IP blocks. The analog-to-digital
+converter (ADC) produces an output value that is proportional
+to the silicon temperature.
+
+Required properties:
+- compatible : Should be:
+  - "ti,omap4460-control-bandgap" : for OMAP4460 bandgap
+  - "ti,omap5430-control-bandgap" : for OMAP5430 bandgap
+- interrupts : this entry should indicate which interrupt line
+the talert signal is routed to;
+Specific:
+- ti,tshut-gpio : this entry should be used to inform which GPIO
+line the tshut signal is routed to;
+
+Example:
+
+bandgap {
+	reg = <0x4a002260 0x4
+		0x4a00232C 0x4
+		0x4a002378 0x18>;
+	compatible = "ti,omap4460-control-bandgap";
+	interrupts = <0 126 4>; /* talert */
+	ti,tshut-gpio = <86>;
+};
diff --git a/drivers/staging/omapdrm/TODO b/drivers/staging/omapdrm/TODO
index 55b18377ac4..938c7888ca3 100644
--- a/drivers/staging/omapdrm/TODO
+++ b/drivers/staging/omapdrm/TODO
@@ -1,9 +1,7 @@
 TODO
-. check error handling/cleanup paths
-. add drm_plane / overlay support
 . add video decode/encode support (via syslink3 + codec-engine)
-. still some rough edges with flipping.. event back to userspace should
-  really come after VSYNC interrupt
+  . NOTE: with dmabuf this probably could be split into different driver
+    so perhaps this TODO doesn't belong here
 . where should we do eviction (detatch_pages())?  We aren't necessarily
   accessing the pages via a GART, so maybe we need some other threshold
   to put a cap on the # of pages that can be pin'd.  (It is mostly only
@@ -27,7 +25,6 @@ TODO
   CRTC's should be disabled, and on resume the LUT should be reprogrammed
   before CRTC's are re-enabled, to prevent DSS from trying to DMA from a
   buffer mapped in DMM/TILER before LUT is reloaded.
-. Add debugfs information for DMM/TILER
 
 Userspace:
 . git://github.com/robclark/xf86-video-omap.git
diff --git a/drivers/staging/omapdrm/omap_crtc.c b/drivers/staging/omapdrm/omap_crtc.c
index 8b864afb40b..62e0022561b 100644
--- a/drivers/staging/omapdrm/omap_crtc.c
+++ b/drivers/staging/omapdrm/omap_crtc.c
@@ -60,7 +60,7 @@ static void omap_crtc_dpms(struct drm_crtc *crtc, int mode)
 }
 
 static bool omap_crtc_mode_fixup(struct drm_crtc *crtc,
-		struct drm_display_mode *mode,
+		const struct drm_display_mode *mode,
 		struct drm_display_mode *adjusted_mode)
 {
 	return true;
diff --git a/drivers/staging/omapdrm/omap_dmm_priv.h b/drivers/staging/omapdrm/omap_dmm_priv.h
index 2f529ab4b7c..08b22e9f0ed 100644
--- a/drivers/staging/omapdrm/omap_dmm_priv.h
+++ b/drivers/staging/omapdrm/omap_dmm_priv.h
@@ -181,7 +181,6 @@ struct dmm {
 
 	/* allocation list and lock */
 	struct list_head alloc_head;
-	spinlock_t list_lock;
 };
 
 #endif
diff --git a/drivers/staging/omapdrm/omap_dmm_tiler.c b/drivers/staging/omapdrm/omap_dmm_tiler.c
index 9d83060e753..86197831f63 100644
--- a/drivers/staging/omapdrm/omap_dmm_tiler.c
+++ b/drivers/staging/omapdrm/omap_dmm_tiler.c
@@ -40,6 +40,9 @@
 static struct tcm *containers[TILFMT_NFORMATS];
 static struct dmm *omap_dmm;
 
+/* global spinlock for protecting lists */
+static DEFINE_SPINLOCK(list_lock);
+
 /* Geometry table */
 #define GEOM(xshift, yshift, bytes_per_pixel) { \
 		.x_shft = (xshift), \
@@ -147,13 +150,13 @@ static struct dmm_txn *dmm_txn_init(struct dmm *dmm, struct tcm *tcm)
 	down(&dmm->engine_sem);
 
 	/* grab an idle engine */
-	spin_lock(&dmm->list_lock);
+	spin_lock(&list_lock);
 	if (!list_empty(&dmm->idle_head)) {
 		engine = list_entry(dmm->idle_head.next, struct refill_engine,
 					idle_node);
 		list_del(&engine->idle_node);
 	}
-	spin_unlock(&dmm->list_lock);
+	spin_unlock(&list_lock);
 
 	BUG_ON(!engine);
 
@@ -256,9 +259,9 @@ static int dmm_txn_commit(struct dmm_txn *txn, bool wait)
 	}
 
 cleanup:
-	spin_lock(&dmm->list_lock);
+	spin_lock(&list_lock);
 	list_add(&engine->idle_node, &dmm->idle_head);
-	spin_unlock(&dmm->list_lock);
+	spin_unlock(&list_lock);
 
 	up(&omap_dmm->engine_sem);
 	return ret;
@@ -351,9 +354,9 @@ struct tiler_block *tiler_reserve_2d(enum tiler_fmt fmt, uint16_t w,
 	}
 
 	/* add to allocation list */
-	spin_lock(&omap_dmm->list_lock);
+	spin_lock(&list_lock);
 	list_add(&block->alloc_node, &omap_dmm->alloc_head);
-	spin_unlock(&omap_dmm->list_lock);
+	spin_unlock(&list_lock);
 
 	return block;
 }
@@ -374,9 +377,9 @@ struct tiler_block *tiler_reserve_1d(size_t size)
 		return ERR_PTR(-ENOMEM);
 	}
 
-	spin_lock(&omap_dmm->list_lock);
+	spin_lock(&list_lock);
 	list_add(&block->alloc_node, &omap_dmm->alloc_head);
-	spin_unlock(&omap_dmm->list_lock);
+	spin_unlock(&list_lock);
 
 	return block;
 }
@@ -389,9 +392,9 @@ int tiler_release(struct tiler_block *block)
 	if (block->area.tcm)
 		dev_err(omap_dmm->dev, "failed to release block\n");
 
-	spin_lock(&omap_dmm->list_lock);
+	spin_lock(&list_lock);
 	list_del(&block->alloc_node);
-	spin_unlock(&omap_dmm->list_lock);
+	spin_unlock(&list_lock);
 
 	kfree(block);
 	return ret;
@@ -479,13 +482,13 @@ static int omap_dmm_remove(struct platform_device *dev)
 
 	if (omap_dmm) {
 		/* free all area regions */
-		spin_lock(&omap_dmm->list_lock);
+		spin_lock(&list_lock);
 		list_for_each_entry_safe(block, _block, &omap_dmm->alloc_head,
 					alloc_node) {
 			list_del(&block->alloc_node);
 			kfree(block);
 		}
-		spin_unlock(&omap_dmm->list_lock);
+		spin_unlock(&list_lock);
 
 		for (i = 0; i < omap_dmm->num_lut; i++)
 			if (omap_dmm->tcm && omap_dmm->tcm[i])
@@ -503,7 +506,7 @@ static int omap_dmm_remove(struct platform_device *dev)
 
 		vfree(omap_dmm->lut);
 
-		if (omap_dmm->irq != -1)
+		if (omap_dmm->irq > 0)
 			free_irq(omap_dmm->irq, omap_dmm);
 
 		iounmap(omap_dmm->base);
@@ -527,6 +530,10 @@ static int omap_dmm_probe(struct platform_device *dev)
 		goto fail;
 	}
 
+	/* initialize lists */
+	INIT_LIST_HEAD(&omap_dmm->alloc_head);
+	INIT_LIST_HEAD(&omap_dmm->idle_head);
+
 	/* lookup hwmod data - base address and irq */
 	mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	if (!mem) {
@@ -629,7 +636,6 @@ static int omap_dmm_probe(struct platform_device *dev)
 	}
 
 	sema_init(&omap_dmm->engine_sem, omap_dmm->num_engines);
-	INIT_LIST_HEAD(&omap_dmm->idle_head);
 	for (i = 0; i < omap_dmm->num_engines; i++) {
 		omap_dmm->engines[i].id = i;
 		omap_dmm->engines[i].dmm = omap_dmm;
@@ -672,9 +678,6 @@ static int omap_dmm_probe(struct platform_device *dev)
 	containers[TILFMT_32BIT] = omap_dmm->tcm[0];
 	containers[TILFMT_PAGE] = omap_dmm->tcm[0];
 
-	INIT_LIST_HEAD(&omap_dmm->alloc_head);
-	spin_lock_init(&omap_dmm->list_lock);
-
 	area = (struct tcm_area) {
 		.is2d = true,
 		.tcm = NULL,
@@ -697,7 +700,8 @@ static int omap_dmm_probe(struct platform_device *dev)
 	return 0;
 
 fail:
-	omap_dmm_remove(dev);
+	if (omap_dmm_remove(dev))
+		dev_err(&dev->dev, "cleanup failed\n");
 	return ret;
 }
 
@@ -810,7 +814,7 @@ int tiler_map_show(struct seq_file *s, void *arg)
 		map[i] = global_map + i * (w_adj + 1);
 		map[i][w_adj] = 0;
 	}
-	spin_lock_irqsave(&omap_dmm->list_lock, flags);
+	spin_lock_irqsave(&list_lock, flags);
 
 	list_for_each_entry(block, &omap_dmm->alloc_head, alloc_node) {
 		if (block->fmt != TILFMT_PAGE) {
@@ -836,7 +840,7 @@ int tiler_map_show(struct seq_file *s, void *arg)
 		}
 	}
 
-	spin_unlock_irqrestore(&omap_dmm->list_lock, flags);
+	spin_unlock_irqrestore(&list_lock, flags);
 
 	if (s) {
 		seq_printf(s, "BEGIN DMM TILER MAP\n");
diff --git a/drivers/staging/omapdrm/omap_drv.h b/drivers/staging/omapdrm/omap_drv.h
index f238d574da0..2092a9167d2 100644
--- a/drivers/staging/omapdrm/omap_drv.h
+++ b/drivers/staging/omapdrm/omap_drv.h
@@ -25,8 +25,8 @@
 #include <linux/types.h>
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <linux/platform_data/omap_drm.h>
 #include "omap_drm.h"
-#include "omap_priv.h"
 
 #define DBG(fmt, ...) DRM_DEBUG(fmt"\n", ##__VA_ARGS__)
 #define VERB(fmt, ...) if (0) DRM_DEBUG(fmt, ##__VA_ARGS__) /* verbose debug */
diff --git a/drivers/staging/omapdrm/omap_encoder.c b/drivers/staging/omapdrm/omap_encoder.c
index 06c52cb62d2..31c735d3921 100644
--- a/drivers/staging/omapdrm/omap_encoder.c
+++ b/drivers/staging/omapdrm/omap_encoder.c
@@ -48,7 +48,7 @@ static void omap_encoder_dpms(struct drm_encoder *encoder, int mode)
 }
 
 static bool omap_encoder_mode_fixup(struct drm_encoder *encoder,
-				  struct drm_display_mode *mode,
+				  const struct drm_display_mode *mode,
 				  struct drm_display_mode *adjusted_mode)
 {
 	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
diff --git a/drivers/staging/omapdrm/omap_priv.h b/drivers/staging/omapdrm/omap_priv.h
deleted file mode 100644
index ef644144714..00000000000
--- a/drivers/staging/omapdrm/omap_priv.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * include/drm/omap_priv.h
- *
- * Copyright (C) 2011 Texas Instruments
- * Author: Rob Clark <rob@ti.com>
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __OMAP_PRIV_H__
-#define __OMAP_PRIV_H__
-
-/* Non-userspace facing APIs
- */
-
-/* optional platform data to configure the default configuration of which
- * pipes/overlays/CRTCs are used.. if this is not provided, then instead the
- * first CONFIG_DRM_OMAP_NUM_CRTCS are used, and they are each connected to
- * one manager, with priority given to managers that are connected to
- * detected devices.  Remaining overlays are used as video planes.  This
- * should be a good default behavior for most cases, but yet there still
- * might be times when you wish to do something different.
- */
-struct omap_kms_platform_data {
-	/* overlays to use as CRTCs: */
-	int ovl_cnt;
-	const int *ovl_ids;
-
-	/* overlays to use as video planes: */
-	int pln_cnt;
-	const int *pln_ids;
-
-	int mgr_cnt;
-	const int *mgr_ids;
-
-	int dev_cnt;
-	const char **dev_names;
-};
-
-struct omap_drm_platform_data {
-	struct omap_kms_platform_data *kms_pdata;
-	struct omap_dmm_platform_data *dmm_pdata;
-};
-
-#endif /* __OMAP_DRM_H__ */
diff --git a/drivers/staging/ozwpan/TODO b/drivers/staging/ozwpan/TODO
index c2d30a7112f..b5db2456bff 100644
--- a/drivers/staging/ozwpan/TODO
+++ b/drivers/staging/ozwpan/TODO
@@ -1,10 +1,11 @@
 TODO:
-	- review user mode interface and determine if ioctls can be replaced
-	  with something better. correctly export data structures to user mode
-	  if ioctls are still required and allocate ioctl numbers from
-	  ioctl-number.txt.
+	- Convert event tracing code to in-kernel tracing infrastructure
+	- Check for remaining ioctl & check if that can be converted into
+	  sysfs entries
+	- Convert debug prints to appropriate dev_debug or something better
+	- Modify Kconfig to add CONFIG option for enabling/disabling event
+	  tracing.
 	- check USB HCD implementation is complete and correct.
-	- remove any debug and trace code.
 	- code review by USB developer community.
 	- testing with as many devices as possible.
 
diff --git a/drivers/staging/ozwpan/ozappif.h b/drivers/staging/ozwpan/ozappif.h
index 1b59b0748c6..449a6ba8233 100644
--- a/drivers/staging/ozwpan/ozappif.h
+++ b/drivers/staging/ozwpan/ozappif.h
@@ -30,9 +30,9 @@ struct oz_binding_info {
 #define OZ_IOCTL_GET_PD_LIST	_IOR(OZ_IOCTL_MAGIC, 0, struct oz_pd_list)
 #define OZ_IOCTL_SET_ACTIVE_PD	_IOW(OZ_IOCTL_MAGIC, 1, struct oz_mac_addr)
 #define OZ_IOCTL_GET_ACTIVE_PD	_IOR(OZ_IOCTL_MAGIC, 2, struct oz_mac_addr)
-#define OZ_IOCTL_ADD_BINDING	_IOW(OZ_IOCTL_MAGIC, 5, struct oz_binding_info)
-#define OZ_IOCTL_REMOVE_BINDING	_IOW(OZ_IOCTL_MAGIC, 8, struct oz_binding_info)
-#define OZ_IOCTL_MAX		9
+#define OZ_IOCTL_ADD_BINDING	_IOW(OZ_IOCTL_MAGIC, 3, struct oz_binding_info)
+#define OZ_IOCTL_REMOVE_BINDING	_IOW(OZ_IOCTL_MAGIC, 4, struct oz_binding_info)
+#define OZ_IOCTL_MAX		5
 
 
 #endif /* _OZAPPIF_H */
diff --git a/drivers/staging/ozwpan/ozcdev.c b/drivers/staging/ozwpan/ozcdev.c
index 27325f74ecd..d9832194580 100644
--- a/drivers/staging/ozwpan/ozcdev.c
+++ b/drivers/staging/ozwpan/ozcdev.c
@@ -42,6 +42,7 @@ struct oz_serial_ctx {
 /*------------------------------------------------------------------------------
  */
 static struct oz_cdev g_cdev;
+struct class *g_oz_class;
 /*------------------------------------------------------------------------------
  * Context: process and softirq
  */
@@ -330,10 +331,11 @@ const struct file_operations oz_fops = {
 int oz_cdev_register(void)
 {
 	int err;
+	struct device *dev;
 	memset(&g_cdev, 0, sizeof(g_cdev));
 	err = alloc_chrdev_region(&g_cdev.devnum, 0, 1, "ozwpan");
 	if (err < 0)
-		return err;
+		goto out3;
 	oz_trace("Alloc dev number %d:%d\n", MAJOR(g_cdev.devnum),
 			MINOR(g_cdev.devnum));
 	cdev_init(&g_cdev.cdev, &oz_fops);
@@ -342,7 +344,27 @@ int oz_cdev_register(void)
 	spin_lock_init(&g_cdev.lock);
 	init_waitqueue_head(&g_cdev.rdq);
 	err = cdev_add(&g_cdev.cdev, g_cdev.devnum, 1);
+	if (err < 0) {
+		oz_trace("Failed to add cdev\n");
+		goto out2;
+	}
+	g_oz_class = class_create(THIS_MODULE, "ozmo_wpan");
+	if (IS_ERR(g_oz_class)) {
+		oz_trace("Failed to register ozmo_wpan class\n");
+		goto out1;
+	}
+	dev = device_create(g_oz_class, NULL, g_cdev.devnum, NULL, "ozwpan");
+	if (IS_ERR(dev)) {
+		oz_trace("Failed to create sysfs entry for cdev\n");
+		goto out1;
+	}
 	return 0;
+out1:
+	cdev_del(&g_cdev.cdev);
+out2:
+	unregister_chrdev_region(g_cdev.devnum, 1);
+out3:
+	return err;
 }
 /*------------------------------------------------------------------------------
  * Context: process
@@ -351,6 +373,10 @@ int oz_cdev_deregister(void)
 {
 	cdev_del(&g_cdev.cdev);
 	unregister_chrdev_region(g_cdev.devnum, 1);
+	if (g_oz_class) {
+		device_destroy(g_oz_class, g_cdev.devnum);
+		class_destroy(g_oz_class);
+	}
 	return 0;
 }
 /*------------------------------------------------------------------------------
diff --git a/drivers/staging/ozwpan/ozmain.c b/drivers/staging/ozwpan/ozmain.c
index 7579645d642..c1ed6b2522e 100644
--- a/drivers/staging/ozwpan/ozmain.c
+++ b/drivers/staging/ozwpan/ozmain.c
@@ -59,6 +59,6 @@ module_exit(ozwpan_exit);
 
 MODULE_AUTHOR("Chris Kelly");
 MODULE_DESCRIPTION("Ozmo Devices USB over WiFi hcd driver");
-MODULE_VERSION("1.0.9");
+MODULE_VERSION("1.0.10");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/ozwpan/ozpd.c b/drivers/staging/ozwpan/ozpd.c
index 04cd57f2a6d..6c287ac6eae 100644
--- a/drivers/staging/ozwpan/ozpd.c
+++ b/drivers/staging/ozwpan/ozpd.c
@@ -24,18 +24,22 @@
 /*------------------------------------------------------------------------------
  */
 #define OZ_MAX_TX_POOL_SIZE	6
-/* Maximum number of uncompleted isoc frames that can be pending.
+/* Maximum number of uncompleted isoc frames that can be pending in network.
  */
 #define OZ_MAX_SUBMITTED_ISOC	16
+/* Maximum number of uncompleted isoc frames that can be pending in Tx Queue.
+ */
+#define OZ_MAX_TX_QUEUE_ISOC	32
 /*------------------------------------------------------------------------------
  */
 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd);
 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f);
+static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f);
 static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f);
 static int oz_send_isoc_frame(struct oz_pd *pd);
 static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f);
 static void oz_isoc_stream_free(struct oz_isoc_stream *st);
-static int oz_send_next_queued_frame(struct oz_pd *pd, int *more_data);
+static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data);
 static void oz_isoc_destructor(struct sk_buff *skb);
 static int oz_def_app_init(void);
 static void oz_def_app_term(void);
@@ -208,6 +212,8 @@ void oz_pd_destroy(struct oz_pd *pd)
 	while (e != &pd->tx_queue) {
 		f = container_of(e, struct oz_tx_frame, link);
 		e = e->next;
+		if (f->skb != NULL)
+			kfree_skb(f->skb);
 		oz_retire_frame(pd, f);
 	}
 	oz_elt_buf_term(&pd->elt_buff);
@@ -375,6 +381,23 @@ static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd)
 /*------------------------------------------------------------------------------
  * Context: softirq or process
  */
+static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f)
+{
+	pd->nb_queued_isoc_frames--;
+	list_del_init(&f->link);
+	if (pd->tx_pool_count < OZ_MAX_TX_POOL_SIZE) {
+		f->link.next = pd->tx_pool;
+		pd->tx_pool = &f->link;
+		pd->tx_pool_count++;
+	} else {
+		kfree(f);
+	}
+	oz_trace2(OZ_TRACE_TX_FRAMES, "Releasing ISOC Frame isoc_nb= %d\n",
+						pd->nb_queued_isoc_frames);
+}
+/*------------------------------------------------------------------------------
+ * Context: softirq or process
+ */
 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f)
 {
 	spin_lock_bh(&pd->tx_frame_lock);
@@ -389,6 +412,22 @@ static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f)
 		kfree(f);
 }
 /*------------------------------------------------------------------------------
+ * Context: softirq-serialized
+ */
+void oz_set_more_bit(struct sk_buff *skb)
+{
+	struct oz_hdr *oz_hdr = (struct oz_hdr *)skb_network_header(skb);
+	oz_hdr->control |= OZ_F_MORE_DATA;
+}
+/*------------------------------------------------------------------------------
+ * Context: softirq-serialized
+ */
+void oz_set_last_pkt_nb(struct oz_pd *pd, struct sk_buff *skb)
+{
+	struct oz_hdr *oz_hdr = (struct oz_hdr *)skb_network_header(skb);
+	oz_hdr->last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
+}
+/*------------------------------------------------------------------------------
  * Context: softirq
  */
 int oz_prepare_frame(struct oz_pd *pd, int empty)
@@ -403,6 +442,7 @@ int oz_prepare_frame(struct oz_pd *pd, int empty)
 	f = oz_tx_frame_alloc(pd);
 	if (f == 0)
 		return -1;
+	f->skb = NULL;
 	f->hdr.control =
 		(OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ACK_REQUESTED;
 	++pd->last_tx_pkt_num;
@@ -486,24 +526,52 @@ static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 /*------------------------------------------------------------------------------
  * Context: softirq-serialized
  */
-static int oz_send_next_queued_frame(struct oz_pd *pd, int *more_data)
+static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data)
 {
 	struct sk_buff *skb;
 	struct oz_tx_frame *f;
 	struct list_head *e;
-	*more_data = 0;
 	spin_lock(&pd->tx_frame_lock);
 	e = pd->last_sent_frame->next;
 	if (e == &pd->tx_queue) {
 		spin_unlock(&pd->tx_frame_lock);
 		return -1;
 	}
-	pd->last_sent_frame = e;
-	if (e->next != &pd->tx_queue)
-		*more_data = 1;
 	f = container_of(e, struct oz_tx_frame, link);
+
+	if (f->skb != NULL) {
+		skb = f->skb;
+		oz_tx_isoc_free(pd, f);
+		spin_unlock(&pd->tx_frame_lock);
+		if (more_data)
+			oz_set_more_bit(skb);
+		oz_set_last_pkt_nb(pd, skb);
+		if ((int)atomic_read(&g_submitted_isoc) <
+							OZ_MAX_SUBMITTED_ISOC) {
+			if (dev_queue_xmit(skb) < 0) {
+				oz_trace2(OZ_TRACE_TX_FRAMES,
+						"Dropping ISOC Frame\n");
+				oz_event_log(OZ_EVT_TX_ISOC_DROP, 0, 0, 0, 0);
+				return -1;
+			}
+			atomic_inc(&g_submitted_isoc);
+			oz_trace2(OZ_TRACE_TX_FRAMES,
+					"Sending ISOC Frame, nb_isoc= %d\n",
+						pd->nb_queued_isoc_frames);
+			return 0;
+		} else {
+			kfree_skb(skb);
+			oz_trace2(OZ_TRACE_TX_FRAMES, "Dropping ISOC Frame>\n");
+			oz_event_log(OZ_EVT_TX_ISOC_DROP, 0, 0, 0, 0);
+			return -1;
+		}
+	}
+
+	pd->last_sent_frame = e;
 	skb = oz_build_frame(pd, f);
 	spin_unlock(&pd->tx_frame_lock);
+	if (more_data)
+		oz_set_more_bit(skb);
 	oz_trace2(OZ_TRACE_TX_FRAMES, "TX frame PN=0x%x\n", f->hdr.pkt_num);
 	if (skb) {
 		oz_event_log(OZ_EVT_TX_FRAME,
@@ -512,6 +580,7 @@ static int oz_send_next_queued_frame(struct oz_pd *pd, int *more_data)
 			0, f->hdr.pkt_num);
 		if (dev_queue_xmit(skb) < 0)
 			return -1;
+
 	}
 	return 0;
 }
@@ -520,21 +589,38 @@ static int oz_send_next_queued_frame(struct oz_pd *pd, int *more_data)
  */
 void oz_send_queued_frames(struct oz_pd *pd, int backlog)
 {
-	int more;
-	if (backlog <  OZ_MAX_QUEUED_FRAMES) {
-		if (oz_send_next_queued_frame(pd, &more) >= 0) {
-			while (more && oz_send_next_queued_frame(pd, &more))
-				;
-		} else {
-			if (((pd->mode & OZ_F_ISOC_ANYTIME) == 0)
-				|| (pd->isoc_sent == 0)) {
-				if (oz_prepare_frame(pd, 1) >= 0)
-					oz_send_next_queued_frame(pd, &more);
-			}
+	while (oz_prepare_frame(pd, 0) >= 0)
+		backlog++;
+
+	switch (pd->mode & (OZ_F_ISOC_NO_ELTS | OZ_F_ISOC_ANYTIME)) {
+
+		case OZ_F_ISOC_NO_ELTS: {
+			backlog += pd->nb_queued_isoc_frames;
+			if (backlog <= 0)
+				goto out;
+			if (backlog > OZ_MAX_SUBMITTED_ISOC)
+				backlog = OZ_MAX_SUBMITTED_ISOC;
+			break;
+		}
+		case OZ_NO_ELTS_ANYTIME: {
+			if ((backlog <= 0) && (pd->isoc_sent == 0))
+				goto out;
+			break;
+		}
+		default: {
+			if (backlog <= 0)
+				goto out;
+			break;
 		}
-	} else {
-		oz_send_next_queued_frame(pd, &more);
 	}
+	while (backlog--) {
+		if (oz_send_next_queued_frame(pd, backlog) < 0)
+			break;
+	}
+	return;
+
+out:	oz_prepare_frame(pd, 1);
+	oz_send_next_queued_frame(pd, 0);
 }
 /*------------------------------------------------------------------------------
  * Context: softirq
@@ -603,8 +689,10 @@ void oz_retire_tx_frames(struct oz_pd *pd, u8 lpn)
 		f = container_of(e, struct oz_tx_frame, link);
 		pkt_num = le32_to_cpu(get_unaligned(&f->hdr.pkt_num));
 		diff = (lpn - (pkt_num & OZ_LAST_PN_MASK)) & OZ_LAST_PN_MASK;
-		if (diff > OZ_LAST_PN_HALF_CYCLE)
+		if ((diff > OZ_LAST_PN_HALF_CYCLE) || (pkt_num == 0))
 			break;
+		oz_trace2(OZ_TRACE_TX_FRAMES, "Releasing pkt_num= %u, nb= %d\n",
+						 pkt_num, pd->nb_queued_frames);
 		if (first == 0)
 			first = e;
 		last = e;
@@ -727,6 +815,8 @@ int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, u8 *data, int len)
 		skb_reset_network_header(skb);
 		skb->dev = dev;
 		skb->protocol = htons(OZ_ETHERTYPE);
+		/* For audio packet set priority to AC_VO */
+		skb->priority = 0x7;
 		size = sizeof(struct oz_hdr) + sizeof(struct oz_isoc_large);
 		oz_hdr = (struct oz_hdr *)skb_put(skb, size);
 	}
@@ -756,21 +846,53 @@ int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, u8 *data, int len)
 		memcpy(oz_hdr, &oz, sizeof(oz));
 		memcpy(oz_hdr+1, &iso, sizeof(iso));
 		if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
-				dev->dev_addr, skb->len) < 0) {
-			kfree_skb(skb);
-			return -1;
+				dev->dev_addr, skb->len) < 0)
+			goto out;
+
+		skb->destructor = oz_isoc_destructor;
+		/*Queue for Xmit if mode is not ANYTIME*/
+		if (!(pd->mode & OZ_F_ISOC_ANYTIME)) {
+			struct oz_tx_frame *isoc_unit = NULL;
+			int nb = pd->nb_queued_isoc_frames;
+			if (nb >= OZ_MAX_TX_QUEUE_ISOC) {
+				oz_trace2(OZ_TRACE_TX_FRAMES,
+						"Dropping ISOC Unit nb= %d\n",
+									nb);
+				goto out;
+			}
+			isoc_unit = oz_tx_frame_alloc(pd);
+			if (isoc_unit == NULL)
+				goto out;
+			isoc_unit->hdr = oz;
+			isoc_unit->skb = skb;
+			spin_lock_bh(&pd->tx_frame_lock);
+			list_add_tail(&isoc_unit->link, &pd->tx_queue);
+			pd->nb_queued_isoc_frames++;
+			spin_unlock_bh(&pd->tx_frame_lock);
+			oz_trace2(OZ_TRACE_TX_FRAMES,
+			"Added ISOC Frame to Tx Queue isoc_nb= %d, nb= %d\n",
+			pd->nb_queued_isoc_frames, pd->nb_queued_frames);
+			oz_event_log(OZ_EVT_TX_ISOC, nb_units, iso.frame_number,
+					skb, atomic_read(&g_submitted_isoc));
+			return 0;
 		}
+
+		/*In ANYTIME mode Xmit unit immediately*/
 		if (atomic_read(&g_submitted_isoc) < OZ_MAX_SUBMITTED_ISOC) {
-			skb->destructor = oz_isoc_destructor;
 			atomic_inc(&g_submitted_isoc);
 			oz_event_log(OZ_EVT_TX_ISOC, nb_units, iso.frame_number,
-				skb, atomic_read(&g_submitted_isoc));
-			if (dev_queue_xmit(skb) < 0)
+					skb, atomic_read(&g_submitted_isoc));
+			if (dev_queue_xmit(skb) < 0) {
+				oz_event_log(OZ_EVT_TX_ISOC_DROP, 0, 0, 0, 0);
 				return -1;
-		} else {
-			oz_event_log(OZ_EVT_TX_ISOC_DROP, 0, 0, 0, 0);
-			kfree_skb(skb);
+			} else
+				return 0;
 		}
+
+out:	oz_event_log(OZ_EVT_TX_ISOC_DROP, 0, 0, 0, 0);
+	kfree_skb(skb);
+	return -1;
+
 	}
 	return 0;
 }
diff --git a/drivers/staging/ozwpan/ozpd.h b/drivers/staging/ozwpan/ozpd.h
index afc77f0260f..ddf1341b4e6 100644
--- a/drivers/staging/ozwpan/ozpd.h
+++ b/drivers/staging/ozwpan/ozpd.h
@@ -29,6 +29,7 @@ struct oz_tx_frame {
 	struct list_head link;
 	struct list_head elt_list;
 	struct oz_hdr hdr;
+	struct sk_buff *skb;
 	int total_size;
 };
 
@@ -83,6 +84,7 @@ struct oz_pd {
 	u8		ms_per_isoc;
 	unsigned	max_stream_buffering;
 	int		nb_queued_frames;
+	int		nb_queued_isoc_frames;
 	struct list_head *tx_pool;
 	int		tx_pool_count;
 	spinlock_t	tx_frame_lock;
@@ -118,4 +120,3 @@ void oz_apps_init(void);
 void oz_apps_term(void);
 
 #endif /* Sentry */
-
diff --git a/drivers/staging/ozwpan/ozproto.c b/drivers/staging/ozwpan/ozproto.c
index ad857eeabbb..a50ab18a598 100644
--- a/drivers/staging/ozwpan/ozproto.c
+++ b/drivers/staging/ozwpan/ozproto.c
@@ -217,7 +217,6 @@ static struct oz_pd *oz_connect_req(struct oz_pd *cur_pd, struct oz_elt *elt,
 	pd->mode = body->mode;
 	pd->pd_info = body->pd_info;
 	if (pd->mode & OZ_F_ISOC_NO_ELTS) {
-		pd->mode |= OZ_F_ISOC_ANYTIME;
 		pd->ms_per_isoc = body->ms_per_isoc;
 		if (!pd->ms_per_isoc)
 			pd->ms_per_isoc = 4;
@@ -366,6 +365,7 @@ static void oz_rx_frame(struct sk_buff *skb)
 	}
 
 	if (pd && !dup && ((pd->mode & OZ_MODE_MASK) == OZ_MODE_TRIGGERED)) {
+		oz_trace2(OZ_TRACE_RX_FRAMES, "Received TRIGGER Frame\n");
 		pd->last_sent_frame = &pd->tx_queue;
 		if (oz_hdr->control & OZ_F_ACK) {
 			/* Retire completed frames */
@@ -376,8 +376,6 @@ static void oz_rx_frame(struct sk_buff *skb)
 			int backlog = pd->nb_queued_frames;
 			pd->trigger_pkt_num = pkt_num;
 			/* Send queued frames */
-			while (oz_prepare_frame(pd, 0) >= 0)
-				;
 			oz_send_queued_frames(pd, backlog);
 		}
 	}
@@ -796,7 +794,7 @@ void oz_binding_add(char *net_dev)
 {
 	struct oz_binding *binding;
 
-	binding = kmalloc(sizeof(struct oz_binding), GFP_ATOMIC);
+	binding = kmalloc(sizeof(struct oz_binding), GFP_KERNEL);
 	if (binding) {
 		binding->ptype.type = __constant_htons(OZ_ETHERTYPE);
 		binding->ptype.func = oz_pkt_recv;
diff --git a/drivers/staging/ozwpan/ozprotocol.h b/drivers/staging/ozwpan/ozprotocol.h
index b3e7d77f3ff..1e4edbeb61c 100644
--- a/drivers/staging/ozwpan/ozprotocol.h
+++ b/drivers/staging/ozwpan/ozprotocol.h
@@ -89,6 +89,7 @@ struct oz_elt_connect_req {
 #define OZ_MODE_MASK		0xf
 #define OZ_F_ISOC_NO_ELTS	0x40
 #define OZ_F_ISOC_ANYTIME	0x80
+#define OZ_NO_ELTS_ANYTIME	0xc0
 
 /* Keep alive field.
  */
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 7365089a33e..39f9982c270 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -34,6 +34,8 @@
  *
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 
 #include <linux/types.h>
@@ -1837,12 +1839,6 @@ static void panel_process_inputs(void)
 	struct list_head *item;
 	struct logical_input *input;
 
-#if 0
-	printk(KERN_DEBUG
-	       "entering panel_process_inputs with pp=%016Lx & pc=%016Lx\n",
-	       phys_prev, phys_curr);
-#endif
-
 	keypressed = 0;
 	inputs_stable = 1;
 	list_for_each(item, &logical_inputs) {
@@ -1987,10 +1983,9 @@ static struct logical_input *panel_bind_key(char *name, char *press,
 	struct logical_input *key;
 
 	key = kzalloc(sizeof(struct logical_input), GFP_KERNEL);
-	if (!key) {
-		printk(KERN_ERR "panel: not enough memory\n");
+	if (!key)
 		return NULL;
-	}
+
 	if (!input_name2mask(name, &key->mask, &key->value, &scan_mask_i,
 			     &scan_mask_o)) {
 		kfree(key);
@@ -2002,10 +1997,6 @@ static struct logical_input *panel_bind_key(char *name, char *press,
 	key->rise_time = 1;
 	key->fall_time = 1;
 
-#if 0
-	printk(KERN_DEBUG "bind: <%s> : m=%016Lx v=%016Lx\n", name, key->mask,
-	       key->value);
-#endif
 	strncpy(key->u.kbd.press_str, press, sizeof(key->u.kbd.press_str));
 	strncpy(key->u.kbd.repeat_str, repeat, sizeof(key->u.kbd.repeat_str));
 	strncpy(key->u.kbd.release_str, release,
@@ -2030,10 +2021,9 @@ static struct logical_input *panel_bind_callback(char *name,
 	struct logical_input *callback;
 
 	callback = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
-	if (!callback) {
-		printk(KERN_ERR "panel: not enough memory\n");
+	if (!callback)
 		return NULL;
-	}
+
 	memset(callback, 0, sizeof(struct logical_input));
 	if (!input_name2mask(name, &callback->mask, &callback->value,
 			     &scan_mask_i, &scan_mask_o))
@@ -2110,10 +2100,8 @@ static void panel_attach(struct parport *port)
 		return;
 
 	if (pprt) {
-		printk(KERN_ERR
-		       "panel_attach(): port->number=%d parport=%d, "
-		       "already registered !\n",
-		       port->number, parport);
+		pr_err("%s: port->number=%d parport=%d, already registered!\n",
+		       __func__, port->number, parport);
 		return;
 	}
 
@@ -2122,16 +2110,14 @@ static void panel_attach(struct parport *port)
 				       /*PARPORT_DEV_EXCL */
 				       0, (void *)&pprt);
 	if (pprt == NULL) {
-		pr_err("panel_attach(): port->number=%d parport=%d, "
-		       "parport_register_device() failed\n",
-		       port->number, parport);
+		pr_err("%s: port->number=%d parport=%d, parport_register_device() failed\n",
+		       __func__, port->number, parport);
 		return;
 	}
 
 	if (parport_claim(pprt)) {
-		printk(KERN_ERR
-		       "Panel: could not claim access to parport%d. "
-		       "Aborting.\n", parport);
+		pr_err("could not claim access to parport%d. Aborting.\n",
+		       parport);
 		goto err_unreg_device;
 	}
 
@@ -2165,10 +2151,8 @@ static void panel_detach(struct parport *port)
 		return;
 
 	if (!pprt) {
-		printk(KERN_ERR
-		       "panel_detach(): port->number=%d parport=%d, "
-		       "nothing to unregister.\n",
-		       port->number, parport);
+		pr_err("%s: port->number=%d parport=%d, nothing to unregister.\n",
+		       __func__, port->number, parport);
 		return;
 	}
 
@@ -2278,8 +2262,7 @@ int panel_init(void)
 	init_in_progress = 1;
 
 	if (parport_register_driver(&panel_driver)) {
-		printk(KERN_ERR
-		       "Panel: could not register with parport. Aborting.\n");
+		pr_err("could not register with parport. Aborting.\n");
 		return -EIO;
 	}
 
@@ -2291,20 +2274,19 @@ int panel_init(void)
 			pprt = NULL;
 		}
 		parport_unregister_driver(&panel_driver);
-		printk(KERN_ERR "Panel driver version " PANEL_VERSION
-		       " disabled.\n");
+		pr_err("driver version " PANEL_VERSION " disabled.\n");
 		return -ENODEV;
 	}
 
 	register_reboot_notifier(&panel_notifier);
 
 	if (pprt)
-		printk(KERN_INFO "Panel driver version " PANEL_VERSION
-		       " registered on parport%d (io=0x%lx).\n", parport,
-		       pprt->port->base);
+		pr_info("driver version " PANEL_VERSION
+			" registered on parport%d (io=0x%lx).\n", parport,
+			pprt->port->base);
 	else
-		printk(KERN_INFO "Panel driver version " PANEL_VERSION
-		       " not yet registered\n");
+		pr_info("driver version " PANEL_VERSION
+			" not yet registered\n");
 	/* tells various subsystems about the fact that initialization
 	   is finished */
 	init_in_progress = 0;
diff --git a/drivers/staging/phison/phison.c b/drivers/staging/phison/phison.c
index d77b21f1eb2..919cb95236f 100644
--- a/drivers/staging/phison/phison.c
+++ b/drivers/staging/phison/phison.c
@@ -87,18 +87,7 @@ static struct pci_driver phison_pci_driver = {
 #endif
 };
 
-static int __init phison_ide_init(void)
-{
-	return pci_register_driver(&phison_pci_driver);
-}
-
-static void __exit phison_ide_exit(void)
-{
-	pci_unregister_driver(&phison_pci_driver);
-}
-
-module_init(phison_ide_init);
-module_exit(phison_ide_exit);
+module_pci_driver(phison_pci_driver);
 
 MODULE_AUTHOR("Evan Ko");
 MODULE_DESCRIPTION("PCIE driver module for PHISON PS5000 E-BOX");
diff --git a/drivers/staging/ramster/r2net.c b/drivers/staging/ramster/r2net.c
index 2ee02204c43..fc830c3ac74 100644
--- a/drivers/staging/ramster/r2net.c
+++ b/drivers/staging/ramster/r2net.c
@@ -266,8 +266,8 @@ int ramster_remote_put(struct tmem_xhandle *xh, char *data, size_t size,
 		static unsigned long cnt;
 		cnt++;
 		if (!(cnt&(cnt-1)))
-			pr_err("ramster_remote_put: message failed, "
-				"ret=%d, cnt=%lu\n", ret, cnt);
+			pr_err("ramster_remote_put: message failed, ret=%d, cnt=%lu\n",
+				ret, cnt);
 		ret = -1;
 	}
 #endif
diff --git a/drivers/staging/rtl8187se/r8180_wx.c b/drivers/staging/rtl8187se/r8180_wx.c
index 46ee6f47f52..52f63d75d24 100644
--- a/drivers/staging/rtl8187se/r8180_wx.c
+++ b/drivers/staging/rtl8187se/r8180_wx.c
@@ -615,10 +615,10 @@ static int r8180_wx_get_retry(struct net_device *dev,
 		return -EINVAL;
 
 	if (wrqu->retry.flags & IW_RETRY_MAX) {
-		wrqu->retry.flags = IW_RETRY_LIMIT & IW_RETRY_MAX;
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
 		wrqu->retry.value = priv->retry_rts;
 	} else {
-		wrqu->retry.flags = IW_RETRY_LIMIT & IW_RETRY_MIN;
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MIN;
 		wrqu->retry.value = priv->retry_data;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 778d7baf8e0..6202358c298 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -919,10 +919,10 @@ static int r8192_wx_get_retry(struct net_device *dev,
 		return -EINVAL;
 
 	if (wrqu->retry.flags & IW_RETRY_MAX) {
-		wrqu->retry.flags = IW_RETRY_LIMIT & IW_RETRY_MAX;
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
 		wrqu->retry.value = priv->retry_rts;
 	} else {
-		wrqu->retry.flags = IW_RETRY_LIMIT & IW_RETRY_MIN;
+		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MIN;
 		wrqu->retry.value = priv->retry_data;
 	}
 	return 0;
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index 69f616c6964..c758c40e0c8 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -41,8 +41,6 @@
 #include "usb_ops.h"
 #include "usb_osintf.h"
 
-#define DRVER  "v7_0.20100831"
-
 static struct usb_interface *pintf;
 
 static int r871xu_drv_init(struct usb_interface *pusb_intf,
@@ -374,7 +372,7 @@ static int r871xu_drv_init(struct usb_interface *pusb_intf,
 	struct net_device *pnetdev;
 	struct usb_device *udev;
 
-	printk(KERN_INFO "r8712u: DriverVersion: %s\n", DRVER);
+	printk(KERN_INFO "r8712u: Staging version\n");
 	/* In this probe function, O.S. will provide the usb interface pointer
 	 * to driver. We have to increase the reference count of the usb device
 	 * structure by using the usb_get_dev function.
diff --git a/drivers/staging/rts5139/rts51x_card.c b/drivers/staging/rts5139/rts51x_card.c
index 4192c3bac12..50be42ac592 100644
--- a/drivers/staging/rts5139/rts51x_card.c
+++ b/drivers/staging/rts5139/rts51x_card.c
@@ -211,13 +211,12 @@ static void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
 				release_map |= MS_CARD;
 		}
 	} else {
-		if (chip->card_status & XD_CD) {
+		if (chip->card_status & XD_CD)
 			reset_map |= XD_CARD;
-		} else if (chip->card_status & SD_CD) {
+		else if (chip->card_status & SD_CD)
 			reset_map |= SD_CARD;
-		} else if (chip->card_status & MS_CD) {
+		else if (chip->card_status & MS_CD)
 			reset_map |= MS_CARD;
-		}
 	}
 
 	if (CHECK_PKG(chip, QFN24) && reset_map) {
@@ -827,8 +826,7 @@ int card_power_on(struct rts51x_chip *chip, u8 card)
 	if ((card == SD_CARD) || (card == XD_CARD)) {
 		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask | LDO3318_PWR_MASK,
 				 val1 | LDO_SUSPEND);
-	}
-	else {
+	} else {
 #endif
 		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val1);
 #ifdef SD_XD_IO_FOLLOW_PWR
diff --git a/drivers/staging/rts5139/rts51x_chip.c b/drivers/staging/rts5139/rts51x_chip.c
index db88d7a194b..08dcae8db63 100644
--- a/drivers/staging/rts5139/rts51x_chip.c
+++ b/drivers/staging/rts5139/rts51x_chip.c
@@ -363,11 +363,10 @@ void rts51x_polling_func(struct rts51x_chip *chip)
 		break;
 	}
 
-	if (chip->option.auto_delink_en && !chip->card_ready) {
+	if (chip->option.auto_delink_en && !chip->card_ready)
 		rts51x_auto_delink(chip);
-	} else {
+	else
 		chip->auto_delink_counter = 0;
-	}
 }
 
 void rts51x_add_cmd(struct rts51x_chip *chip,
diff --git a/drivers/staging/rts5139/rts51x_chip.h b/drivers/staging/rts5139/rts51x_chip.h
index 6d395b6533a..64257caf2f3 100644
--- a/drivers/staging/rts5139/rts51x_chip.h
+++ b/drivers/staging/rts5139/rts51x_chip.h
@@ -204,7 +204,7 @@ struct trace_msg_t {
 /*---- error code ----*/
 #define CUR_ERR                 0x70	/* current error                    */
 
-/*---- sense key Infomation ----*/
+/*---- sense key Information ----*/
 
 #define SKSV                    0x80
 #define CDB_ILLEGAL             0x40
diff --git a/drivers/staging/rts5139/rts51x_fop.c b/drivers/staging/rts5139/rts51x_fop.c
index ef893c8cdec..e1200fe8957 100644
--- a/drivers/staging/rts5139/rts51x_fop.c
+++ b/drivers/staging/rts5139/rts51x_fop.c
@@ -36,7 +36,6 @@
 #include "rts51x_card.h"
 #include "rts51x_fop.h"
 #include "sd_cprm.h"
-#include "rts51x.h"
 
 #define RTS5139_IOC_MAGIC		0x39
 
diff --git a/drivers/staging/rts5139/rts51x_scsi.h b/drivers/staging/rts5139/rts51x_scsi.h
index 060d2c2e77e..9042bc98a9a 100644
--- a/drivers/staging/rts5139/rts51x_scsi.h
+++ b/drivers/staging/rts5139/rts51x_scsi.h
@@ -73,7 +73,7 @@
 #define GET_BATCHRSP		0x44
 
 #ifdef SUPPORT_CPRM
-/* SD Pass Through Command Extention */
+/* SD Pass Through Command Extension */
 #define SD_PASS_THRU_MODE	0xD0
 #define SD_EXECUTE_NO_DATA	0xD1
 #define SD_EXECUTE_READ		0xD2
diff --git a/drivers/staging/rts5139/sd_cprm.c b/drivers/staging/rts5139/sd_cprm.c
index f8c60711f71..0167f7f35c2 100644
--- a/drivers/staging/rts5139/sd_cprm.c
+++ b/drivers/staging/rts5139/sd_cprm.c
@@ -201,9 +201,8 @@ RTY_SEND_CMD:
 			if (buf[1] & 0x80)
 				TRACE_RET(chip, STATUS_FAIL);
 		}
-		if (buf[1] & 0x7F) {
+		if (buf[1] & 0x7F)
 			TRACE_RET(chip, STATUS_FAIL);
-		}
 		if (buf[2] & 0xF8)
 			TRACE_RET(chip, STATUS_FAIL);
 
@@ -224,7 +223,8 @@ RTY_SEND_CMD:
 	return STATUS_SUCCESS;
 }
 
-static int ext_sd_get_rsp(struct rts51x_chip *chip, int len, u8 *rsp, u8 rsp_type)
+static int ext_sd_get_rsp(struct rts51x_chip *chip, int len,
+			u8 *rsp, u8 rsp_type)
 {
 	int retval, rsp_len;
 	u16 reg_addr;
@@ -844,7 +844,7 @@ int sd_pass_thru_mode(struct scsi_cmnd *srb, struct rts51x_chip *chip)
 
 	buf[15] = chip->max_lun;
 
-	len = min(18, (int)scsi_bufflen(srb));
+	len = min_t(unsigned, 18, scsi_bufflen(srb));
 	rts51x_set_xfer_buf(buf, len, srb);
 
 	return TRANSPORT_GOOD;
diff --git a/drivers/staging/rts_pstor/ms.c b/drivers/staging/rts_pstor/ms.c
index 0bf6d95b3fa..7cc2b53f20d 100644
--- a/drivers/staging/rts_pstor/ms.c
+++ b/drivers/staging/rts_pstor/ms.c
@@ -2488,7 +2488,7 @@ int reset_ms_card(struct rtsx_chip *chip)
 
 	if (!CHK_MSPRO(ms_card)) {
 		/* Build table for the last segment,
-		 * to check if L2P talbe block exist,erasing it
+		 * to check if L2P table block exists, erasing it
 		 */
 		retval = ms_build_l2p_tbl(chip, ms_card->total_block / 512 - 1);
 		if (retval != STATUS_SUCCESS) {
diff --git a/drivers/staging/rts_pstor/rtsx.c b/drivers/staging/rts_pstor/rtsx.c
index 1dccd933a7e..5fb05a2edeb 100644
--- a/drivers/staging/rts_pstor/rtsx.c
+++ b/drivers/staging/rts_pstor/rtsx.c
@@ -1021,7 +1021,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 	}
 
 	/* Start up the thread for delayed SCSI-device scanning */
-	th = kthread_create(rtsx_scan_thread, dev, "rtsx-scan");
+	th = kthread_run(rtsx_scan_thread, dev, "rtsx-scan");
 	if (IS_ERR(th)) {
 		printk(KERN_ERR "Unable to start the device-scanning thread\n");
 		complete(&dev->scanning_done);
@@ -1030,8 +1030,6 @@ static int __devinit rtsx_probe(struct pci_dev *pci,
 		goto errout;
 	}
 
-	wake_up_process(th);
-
 	/* Start up the thread for polling thread */
 	th = kthread_run(rtsx_polling_thread, dev, "rtsx-polling");
 	if (IS_ERR(th)) {
diff --git a/drivers/staging/rts_pstor/sd.c b/drivers/staging/rts_pstor/sd.c
index aab690932ea..3cc9a489e4e 100644
--- a/drivers/staging/rts_pstor/sd.c
+++ b/drivers/staging/rts_pstor/sd.c
@@ -137,11 +137,10 @@ static int sd_check_data0_status(struct rtsx_chip *chip)
 {
 	u8 stat;
 
-	if (CHECK_PID(chip, 0x5209)) {
+	if (CHECK_PID(chip, 0x5209))
 		RTSX_READ_REG(chip, REG_SD_BUS_STAT, &stat);
-	} else {
+	else
 		RTSX_READ_REG(chip, REG_SD_STAT1, &stat);
-	}
 
 	if (!(stat & SD_DAT0_STATUS)) {
 		sd_set_err_code(chip, SD_BUSY);
@@ -188,9 +187,9 @@ RTY_SEND_CMD:
 		     SD_TRANSFER_END | SD_STAT_IDLE, SD_TRANSFER_END | SD_STAT_IDLE);
 
 	if (rsp_type == SD_RSP_TYPE_R2) {
-		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
+		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16; reg_addr++)
 			rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-		}
+
 		stat_idx = 16;
 	} else if (rsp_type != SD_RSP_TYPE_R0) {
 		for (reg_addr = REG_SD_CMD0; reg_addr <= REG_SD_CMD4; reg_addr++) {
@@ -1169,7 +1168,7 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 		RTSX_DEBUGP("func_group4_mask = 0x%02x\n", buf[0x07]);
 	} else {
 		/* Maximum current consumption, check whether current is acceptable;
-		 * bit[511:496] = 0x0000 means some error happaned.
+		 * bit[511:496] = 0x0000 means some error happened.
 		 */
 		u16 cc = ((u16)buf[0] << 8) | buf[1];
 		RTSX_DEBUGP("Maximum current consumption: %dmA\n", cc);
diff --git a/drivers/staging/sbe-2t3e3/2t3e3.h b/drivers/staging/sbe-2t3e3/2t3e3.h
index fe9f086b6e7..383f2cfc1ad 100644
--- a/drivers/staging/sbe-2t3e3/2t3e3.h
+++ b/drivers/staging/sbe-2t3e3/2t3e3.h
@@ -801,9 +801,6 @@ u32 cpld_read(struct channel *sc, u32 reg);
 void cpld_set_crc(struct channel *, u32);
 void cpld_start_intr(struct channel *);
 void cpld_stop_intr(struct channel *);
-#if 0
-void cpld_led_onoff(struct channel *, u32, u32, u32, u32);
-#endif
 void cpld_set_clock(struct channel *sc, u32 mode);
 void cpld_set_scrambler(struct channel *, u32);
 void cpld_select_panel(struct channel *, u32);
diff --git a/drivers/staging/sbe-2t3e3/cpld.c b/drivers/staging/sbe-2t3e3/cpld.c
index b0fc2ddad32..cc2b54d52b1 100644
--- a/drivers/staging/sbe-2t3e3/cpld.c
+++ b/drivers/staging/sbe-2t3e3/cpld.c
@@ -41,14 +41,6 @@ static inline void cpld_clear_bit(struct channel *channel, unsigned reg, u32 bit
 void cpld_init(struct channel *sc)
 {
 	u32 val;
-#if 0
-	/* reset LIU and Framer */
-	val = cpld_val_map[SBE_2T3E3_CPLD_VAL_LIU_FRAMER_RESET][sc->h.slot];
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_STATIC_RESET, val);
-	udelay(10000); /* TODO - how long? */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_STATIC_RESET, val);
-#endif
 
 	/* PCRA */
 	val = SBE_2T3E3_CPLD_VAL_CRC32 |
@@ -109,13 +101,6 @@ void cpld_start_intr(struct channel *sc)
 	val = SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ENABLE |
 		SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ENABLE;
 	cpld_write(sc, SBE_2T3E3_CPLD_REG_PIER, val);
-#if 0
-	/*
-	  do you want to hang up your computer?
-	  ENABLE REST OF INTERRUPTS !!!
-	  you have been warned :).
-	*/
-#endif
 }
 
 void cpld_stop_intr(struct channel *sc)
diff --git a/drivers/staging/sbe-2t3e3/ctrl.c b/drivers/staging/sbe-2t3e3/ctrl.c
index d9dd216e9ae..a5825d7f1bb 100644
--- a/drivers/staging/sbe-2t3e3/ctrl.c
+++ b/drivers/staging/sbe-2t3e3/ctrl.c
@@ -230,11 +230,9 @@ void t3e3_port_get_stats(struct channel *sc,
 		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
 		sc->s.LOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_LOF ? 1 : 0;
 		sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-#if 0
-		sc->s.LOS = result & SBE_2T3E3_FRAMER_VAL_E3_RX_LOS ? 1 : 0;
-#else
+
 		cpld_LOS_update(sc);
-#endif
+
 		sc->s.AIS = result & SBE_2T3E3_FRAMER_VAL_E3_RX_AIS ? 1 : 0;
 		sc->s.FERF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_FERF ? 1 : 0;
 		break;
@@ -243,11 +241,9 @@ void t3e3_port_get_stats(struct channel *sc,
 	case SBE_2T3E3_FRAME_TYPE_T3_M13:
 		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
 		sc->s.AIS = result & SBE_2T3E3_FRAMER_VAL_T3_RX_AIS ? 1 : 0;
-#if 0
-		sc->s.LOS = result & SBE_2T3E3_FRAMER_VAL_T3_RX_LOS ? 1 : 0;
-#else
+
 		cpld_LOS_update(sc);
-#endif
+
 		sc->s.IDLE = result & SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE ? 1 : 0;
 		sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
 
@@ -322,10 +318,6 @@ void t3e3_if_config(struct channel *sc, u32 cmd, char *set,
 		*rlen = sizeof(ret->u.data);
 		break;
 	case SBE_2T3E3_PORT_WRITE_REGS:
-#if 0
-		printk(KERN_DEBUG "SBE_2T3E3_PORT_WRITE_REGS, 0x%x, 0x%x, 0x%x\n",
-		       ((int*)data)[0], ((int*)data)[1], ((int*)data)[2]);
-#endif
 		t3e3_reg_write(sc, data);
 		*rlen = 0;
 		break;
@@ -336,9 +328,6 @@ void t3e3_if_config(struct channel *sc, u32 cmd, char *set,
 		*rlen = 0;
 		break;
 	}
-
-	/* turn on interrupt */
-	/* cpld_start_intr(sc); */
 }
 
 void t3e3_sc_init(struct channel *sc)
diff --git a/drivers/staging/sbe-2t3e3/dc.c b/drivers/staging/sbe-2t3e3/dc.c
index 9dc4ec2109e..9e81d9036a3 100644
--- a/drivers/staging/sbe-2t3e3/dc.c
+++ b/drivers/staging/sbe-2t3e3/dc.c
@@ -63,14 +63,6 @@ void dc_init(struct channel *sc)
 	if (sc->p.loopback == SBE_2T3E3_LOOPBACK_ETHERNET)
 		sc->p.loopback = SBE_2T3E3_LOOPBACK_NONE;
 
-#if 0 /* No need to clear this register - and it may be in use */
-	/*
-	 * BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT (CSR9)
-	 */
-	val = 0;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, val);
-#endif
-
 	/*
 	 * GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL (CSR11)
 	 */
@@ -81,7 +73,7 @@ void dc_init(struct channel *sc)
 		SBE_2T3E3_21143_VAL_NUMBER_OF_RECEIVE_PACKETS;
 	dc_write(sc->addr, SBE_2T3E3_21143_REG_GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL, val);
 
-	/* prepare descriptors and data for receive and transmit procecsses */
+	/* prepare descriptors and data for receive and transmit processes */
 	if (dc_init_descriptor_list(sc) != 0)
 		return;
 
@@ -301,15 +293,6 @@ void dc_set_loopback(struct channel *sc, u32 mode)
 		return;
 	}
 
-#if 0
-	/* restart SIA */
-	dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_SIA_CONNECTIVITY,
-		      SBE_2T3E3_21143_VAL_SIA_RESET);
-	udelay(1000);
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_SIA_CONNECTIVITY,
-		    SBE_2T3E3_21143_VAL_SIA_RESET);
-#endif
-
 	/* select loopback mode */
 	val = dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
 		~SBE_2T3E3_21143_VAL_OPERATING_MODE;
diff --git a/drivers/staging/sbe-2t3e3/exar7250.c b/drivers/staging/sbe-2t3e3/exar7250.c
index 809f446bdc3..e3ddd140207 100644
--- a/drivers/staging/sbe-2t3e3/exar7250.c
+++ b/drivers/staging/sbe-2t3e3/exar7250.c
@@ -78,64 +78,32 @@ void exar7250_start_intr(struct channel *sc, u32 type)
 	case SBE_2T3E3_FRAME_TYPE_E3_G751:
 	case SBE_2T3E3_FRAME_TYPE_E3_G832:
 		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-#if 0
-		sc->s.LOS = val & SBE_2T3E3_FRAMER_VAL_E3_RX_LOS ? 1 : 0;
-#else
+
 		cpld_LOS_update(sc);
-#endif
+
 		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
 		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
 		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1,
 			       SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
 			       SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE);
-#if 0
-		/*SBE_2T3E3_FRAMER_VAL_E3_RX_COFA_INTERRUPT_ENABLE |
-		  SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
-		  SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_INTERRUPT_ENABLE |
-		  SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE |
-		  SBE_2T3E3_FRAMER_VAL_E3_RX_AIS_INTERRUPT_ENABLE);*/
-#endif
 
 		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-#if 0
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_2,
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_FEBE_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_FRAMING_BYTE_ERROR_INTERRUPT_ENABLE);
-#endif
 		break;
 
 	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
 	case SBE_2T3E3_FRAME_TYPE_T3_M13:
 		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-#if 0
-		sc->s.LOS = val & SBE_2T3E3_FRAMER_VAL_T3_RX_LOS ? 1 : 0;
-#else
+
 		cpld_LOS_update(sc);
-#endif
+
 		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
 
 		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
 		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE,
 			       SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
 			       SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE);
-#if 0
-		/* SBE_2T3E3_FRAMER_VAL_T3_RX_CP_BIT_ERROR_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_AIS_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_FERF_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_AIC_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE |
-		   SBE_2T3E3_FRAMER_VAL_T3_RX_P_BIT_INTERRUPT_ENABLE);*/
-#endif
 
 		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-#if 0
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS,
-			       SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_REMOVE_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID_INTERRUPT_ENABLE);
-#endif
 
 		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL, 0);
 		break;
diff --git a/drivers/staging/sbe-2t3e3/exar7300.c b/drivers/staging/sbe-2t3e3/exar7300.c
index d10d696cf6f..cd229998a62 100644
--- a/drivers/staging/sbe-2t3e3/exar7300.c
+++ b/drivers/staging/sbe-2t3e3/exar7300.c
@@ -43,23 +43,6 @@ void exar7300_set_loopback(struct channel *sc, u32 mode)
 	val &= ~(SBE_2T3E3_LIU_VAL_LOCAL_LOOPBACK | SBE_2T3E3_LIU_VAL_REMOTE_LOOPBACK);
 	val |= mode;
 	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4, val);
-
-#if 0
-	/* TODO - is it necessary? idea from 2T3E3_HW_Test_code */
-	switch (mode) {
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_OFF:
-		break;
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE:
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_ON);
-		break;
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG:
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL:
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_ON);
-		break;
-	}
-#endif
 }
 
 void exar7300_set_frame_type(struct channel *sc, u32 type)
diff --git a/drivers/staging/sbe-2t3e3/intr.c b/drivers/staging/sbe-2t3e3/intr.c
index 1336aab11bd..efdeb751004 100644
--- a/drivers/staging/sbe-2t3e3/intr.c
+++ b/drivers/staging/sbe-2t3e3/intr.c
@@ -434,11 +434,6 @@ void exar7250_intr(struct channel *sc)
 {
 	u32 status, old_OOF;
 
-#if 0
-	/* disable interrupts */
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE, 0);
-#endif
-
 	old_OOF = sc->s.OOF;
 
 	status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
@@ -479,13 +474,6 @@ void exar7250_intr(struct channel *sc)
 			dc_start_intr(sc);
 		}
 	}
-#if 0
-	/* reenable interrupts */
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE,
-		       SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_ENABLE |
-		       SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_ENABLE
-		);
-#endif
 }
 
 
@@ -503,16 +491,8 @@ void exar7250_T3_intr(struct channel *sc, u32 block_status)
 
 			result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
 
-#if 0
-			if (status & SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_STATUS) {
-				dev_dbg(&sc->pdev->dev,
-					"Framer interrupt T3: LOS\n");
-				sc->s.LOS = result & SBE_2T3E3_FRAMER_VAL_T3_RX_LOS ? 1 : 0;
-
-			}
-#else
 			cpld_LOS_update(sc);
-#endif
+
 			if (status & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_STATUS) {
 				sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
 				dev_dbg(&sc->pdev->dev,
@@ -523,16 +503,6 @@ void exar7250_T3_intr(struct channel *sc, u32 block_status)
 			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE,
 				       SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
 				       SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE);
-#if 0
-			SBE_2T3E3_FRAMER_VAL_T3_RX_CP_BIT_ERROR_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_AIS_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_FERF_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_AIC_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_T3_RX_P_BIT_INTERRUPT_ENABLE
-#endif
 				}
 
 		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
@@ -540,12 +510,6 @@ void exar7250_T3_intr(struct channel *sc, u32 block_status)
 			dev_dbg(&sc->pdev->dev,
 				"Framer interrupt T3 RX (REG[0x17] = %02X)\n",
 				status);
-#if 0
-			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS,
-				       SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_REMOVE_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID_INTERRUPT_ENABLE
-				);
-#endif
 		}
 
 		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL);
@@ -582,15 +546,8 @@ void exar7250_E3_intr(struct channel *sc, u32 block_status)
 
 			result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
 
-#if 0
-			if (status & SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_STATUS) {
-				dev_dbg(&sc->pdev->dev,
-					"Framer interrupt E3: LOS\n");
-				sc->s.LOS = result & SBE_2T3E3_FRAMER_VAL_E3_RX_LOS ? 1 : 0;
-			}
-#else
 			cpld_LOS_update(sc);
-#endif
+
 			if (status & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_STATUS) {
 				sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
 				dev_dbg(&sc->pdev->dev,
@@ -602,13 +559,6 @@ void exar7250_E3_intr(struct channel *sc, u32 block_status)
 				       SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
 				       SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE
 				);
-#if 0
-			SBE_2T3E3_FRAMER_VAL_E3_RX_COFA_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE |
-				SBE_2T3E3_FRAMER_VAL_E3_RX_AIS_INTERRUPT_ENABLE
-#endif
 				}
 
 		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
@@ -617,12 +567,6 @@ void exar7250_E3_intr(struct channel *sc, u32 block_status)
 				"Framer interrupt E3 RX (REG[0x15] = %02X)\n",
 				status);
 
-#if 0
-			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_2,
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_FEBE_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_FRAMING_BYTE_ERROR_INTERRUPT_ENABLE);
-#endif
 		}
 
 	}
diff --git a/drivers/staging/sbe-2t3e3/io.c b/drivers/staging/sbe-2t3e3/io.c
index 9a50bcc5959..c9947b165b3 100644
--- a/drivers/staging/sbe-2t3e3/io.c
+++ b/drivers/staging/sbe-2t3e3/io.c
@@ -17,7 +17,7 @@
 /* All access to registers done via the 21143 on port 0 must be
  * protected via the card->bootrom_lock. */
 
-/* priviate define to be used here only - must be protected by card->bootrom_lock */
+/* private define to be used here only - must be protected by card->bootrom_lock */
 #define cpld_write_nolock(channel, reg, val)			\
 	bootrom_write((channel), CPLD_MAP_REG(reg, channel), val)
 
@@ -199,15 +199,6 @@ u32 exar7250_read(struct channel *channel, u32 reg)
 	u32 result;
 	unsigned long flags;
 
-#if 0
-	switch (reg) {
-	case SBE_2T3E3_FRAMER_REG_OPERATING_MODE:
-		return channel->framer_regs[reg];
-		break;
-	default:
-	}
-#endif
-
 	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
 
 	result = bootrom_read(channel, cpld_reg_map[SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS]
@@ -243,18 +234,6 @@ u32 exar7300_read(struct channel *channel, u32 reg)
 	unsigned long addr = channel->card->bootrom_addr, flags;
 	u32 i, val;
 
-#if 0
-	switch (reg) {
-	case SBE_2T3E3_LIU_REG_REG1:
-	case SBE_2T3E3_LIU_REG_REG2:
-	case SBE_2T3E3_LIU_REG_REG3:
-	case SBE_2T3E3_LIU_REG_REG4:
-		return channel->liu_regs[reg];
-		break;
-	default:
-	}
-#endif
-
 	/* select correct Serial Chip */
 
 	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
diff --git a/drivers/staging/sbe-2t3e3/module.c b/drivers/staging/sbe-2t3e3/module.c
index e87fe81f6bb..cd778b3a02b 100644
--- a/drivers/staging/sbe-2t3e3/module.c
+++ b/drivers/staging/sbe-2t3e3/module.c
@@ -194,17 +194,6 @@ static struct pci_driver t3e3_pci_driver = {
 	.remove   = t3e3_remove_card,
 };
 
-static int __init t3e3_init_module(void)
-{
-	return pci_register_driver(&t3e3_pci_driver);
-}
-
-static void __exit t3e3_cleanup_module(void)
-{
-	pci_unregister_driver(&t3e3_pci_driver);
-}
-
-module_init(t3e3_init_module);
-module_exit(t3e3_cleanup_module);
+module_pci_driver(t3e3_pci_driver);
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(pci, t3e3_pci_tbl);
diff --git a/drivers/staging/sep/sep_crypto.c b/drivers/staging/sep/sep_crypto.c
index 1cc790e9fa0..34710ce5600 100644
--- a/drivers/staging/sep/sep_crypto.c
+++ b/drivers/staging/sep/sep_crypto.c
@@ -44,7 +44,6 @@
 #include <linux/poll.h>
 #include <linux/wait.h>
 #include <linux/pci.h>
-#include <linux/pci.h>
 #include <linux/pm_runtime.h>
 #include <linux/err.h>
 #include <linux/device.h>
@@ -85,28 +84,6 @@ static void sep_dequeuer(void *data);
 
 /* TESTING */
 /**
- * crypto_sep_dump_message - dump the message that is pending
- * @sep: SEP device
- * This will only print dump if DEBUG is set; it does
- * follow kernel debug print enabling
- */
-static void crypto_sep_dump_message(struct sep_device *sep, void *msg)
-{
-#if 0
-	u32 *p;
-	u32 *i;
-	int count;
-
-	p = sep->shared_addr;
-	i = (u32 *)msg;
-	for (count = 0; count < 10 * 4; count += 4)
-		dev_dbg(&sep->pdev->dev,
-			"[PID%d] Word %d of the message is %x (local)%x\n",
-				current->pid, count/4, *p++, *i++);
-#endif
-}
-
-/**
  *	sep_do_callback
  *	@work: pointer to work_struct
  *	This is what is called by the queue; it is generic so that it
@@ -487,55 +464,6 @@ static int partial_overlap(void *src_ptr, void *dst_ptr, u32 nbytes)
 	return 0;
 }
 
-/* Debug - prints only if DEBUG is defined; follows kernel debug model */
-static void sep_dump(struct sep_device *sep, char *stg, void *start, int len)
-{
-#if 0
-	int ct1;
-	u8 *ptt;
-
-	dev_dbg(&sep->pdev->dev,
-		"Dump of %s starting at %08lx for %08x bytes\n",
-		stg, (unsigned long)start, len);
-	for (ct1 = 0; ct1 < len; ct1 += 1) {
-		ptt = (u8 *)(start + ct1);
-		dev_dbg(&sep->pdev->dev, "%02x ", *ptt);
-		if (ct1 % 16 == 15)
-			dev_dbg(&sep->pdev->dev, "\n");
-	}
-	dev_dbg(&sep->pdev->dev, "\n");
-#endif
-}
-
-/* Debug - prints only if DEBUG is defined; follows kernel debug model */
-static void sep_dump_sg(struct sep_device *sep, char *stg,
-			struct scatterlist *sg)
-{
-#if 0
-	int ct1, ct2;
-	u8 *ptt;
-
-	dev_dbg(&sep->pdev->dev, "Dump of scatterlist %s\n", stg);
-
-	ct1 = 0;
-	while (sg) {
-		dev_dbg(&sep->pdev->dev, "page %x\n size %x", ct1,
-			sg->length);
-		dev_dbg(&sep->pdev->dev, "phys addr is %lx",
-			(unsigned long)sg_phys(sg));
-		ptt = sg_virt(sg);
-		for (ct2 = 0; ct2 < sg->length; ct2 += 1) {
-			dev_dbg(&sep->pdev->dev, "byte %x is %02x\n",
-				ct2, (unsigned char)*(ptt + ct2));
-		}
-
-		ct1 += 1;
-		sg = sg_next(sg);
-	}
-	dev_dbg(&sep->pdev->dev, "\n");
-#endif
-}
-
 /* Debug - prints only if DEBUG is defined */
 static void sep_dump_ivs(struct ablkcipher_request *req, char *reason)
 
@@ -807,7 +735,7 @@ end_function:
  *	@size: size of parameter to copy (in bytes)
  *	@max_size: size to move up offset; SEP mesg is in word sizes
  *	@msg_offset: pointer to current offset (is updated)
- *	@byte_array: flag ti indicate wheter endian must be changed
+ *	@byte_array: flag ti indicate whether endian must be changed
  *	Copies data into the message area from caller
  */
 static void sep_write_msg(struct this_task_ctx *ta_ctx, void *in_addr,
@@ -855,7 +783,7 @@ static void sep_make_header(struct this_task_ctx *ta_ctx, u32 *msg_offset,
  *	@size: size of parameter to copy (in bytes)
  *	@max_size: size to move up offset; SEP mesg is in word sizes
  *	@msg_offset: pointer to current offset (is updated)
- *	@byte_array: flag ti indicate wheter endian must be changed
+ *	@byte_array: flag ti indicate whether endian must be changed
  *	Copies data out of the message area to caller
  */
 static void sep_read_msg(struct this_task_ctx *ta_ctx, void *in_addr,
@@ -990,7 +918,7 @@ static void sep_clear_out(struct this_task_ctx *ta_ctx)
 		/**
 		 * The following unlocks the sep and makes it available
 		 * to any other application
-		 * First, null out crypto entries in sep before relesing it
+		 * First, null out crypto entries in sep before releasing it
 		 */
 		ta_ctx->sep_used->current_hash_req = NULL;
 		ta_ctx->sep_used->current_cypher_req = NULL;
@@ -1001,7 +929,7 @@ static void sep_clear_out(struct this_task_ctx *ta_ctx)
 
 		ta_ctx->call_status.status = 0;
 
-		/* Remove anything confidentail */
+		/* Remove anything confidential */
 		memset(ta_ctx->sep_used->shared_addr, 0,
 			SEP_DRIVER_MESSAGE_SHARED_AREA_SIZE_IN_BYTES);
 
@@ -1095,8 +1023,8 @@ static int sep_crypto_take_sep(struct this_task_ctx *ta_ctx)
 		current->comm, sizeof(current->comm));
 
 	if (!ta_ctx->queue_elem) {
-		dev_dbg(&sep->pdev->dev, "[PID%d] updating queue"
-			" status error\n", current->pid);
+		dev_dbg(&sep->pdev->dev,
+			"[PID%d] updating queue status error\n", current->pid);
 		return -EINVAL;
 	}
 
@@ -1207,7 +1135,7 @@ static int sep_crypto_block_data(struct ablkcipher_request *req)
 		req->nbytes, ta_ctx->walk.blocksize, &new_sg, 1);
 
 	if (int_error < 0) {
-		dev_warn(&ta_ctx->sep_used->pdev->dev, "oddball page eerror\n");
+		dev_warn(&ta_ctx->sep_used->pdev->dev, "oddball page error\n");
 		return -ENOMEM;
 	} else if (int_error == 1) {
 		ta_ctx->src_sg = new_sg;
@@ -1238,9 +1166,6 @@ static int sep_crypto_block_data(struct ablkcipher_request *req)
 	/* Key already done; this is for data */
 	dev_dbg(&ta_ctx->sep_used->pdev->dev, "sending data\n");
 
-	sep_dump_sg(ta_ctx->sep_used,
-		"block sg in", ta_ctx->src_sg);
-
 	/* check for valid data and proper spacing */
 	src_ptr = sg_virt(ta_ctx->src_sg);
 	dst_ptr = sg_virt(ta_ctx->dst_sg);
@@ -1362,14 +1287,10 @@ static int sep_crypto_block_data(struct ablkcipher_request *req)
 		sep_write_context(ta_ctx, &msg_offset,
 			&sctx->des_private_ctx,
 			sizeof(struct sep_des_private_context));
-		sep_dump(ta_ctx->sep_used, "ctx to block des",
-			&sctx->des_private_ctx, 40);
 	} else {
 		sep_write_context(ta_ctx, &msg_offset,
 			&sctx->aes_private_ctx,
 			sizeof(struct sep_aes_private_context));
-		sep_dump(ta_ctx->sep_used, "ctx to block aes",
-			&sctx->aes_private_ctx, 20);
 	}
 
 	/* conclude message */
@@ -1426,8 +1347,6 @@ static int sep_crypto_send_key(struct ablkcipher_request *req)
 		}
 
 		memcpy(ta_ctx->iv, ta_ctx->walk.iv, SEP_DES_IV_SIZE_BYTES);
-		sep_dump(ta_ctx->sep_used, "iv",
-			ta_ctx->iv, SEP_DES_IV_SIZE_BYTES);
 	}
 
 	if ((ta_ctx->current_request == AES_CBC) &&
@@ -1438,8 +1357,6 @@ static int sep_crypto_send_key(struct ablkcipher_request *req)
 		}
 
 		memcpy(ta_ctx->iv, ta_ctx->walk.iv, SEP_AES_IV_SIZE_BYTES);
-		sep_dump(ta_ctx->sep_used, "iv",
-			ta_ctx->iv, SEP_AES_IV_SIZE_BYTES);
 	}
 
 	/* put together message to SEP */
@@ -1452,8 +1369,6 @@ static int sep_crypto_send_key(struct ablkcipher_request *req)
 			sep_write_msg(ta_ctx, ta_ctx->iv,
 				SEP_DES_IV_SIZE_BYTES, sizeof(u32) * 4,
 				&msg_offset, 1);
-			sep_dump(ta_ctx->sep_used, "initial IV",
-				ta_ctx->walk.iv, SEP_DES_IV_SIZE_BYTES);
 		} else {
 			/* Skip if ECB */
 			msg_offset += 4 * sizeof(u32);
@@ -1465,8 +1380,6 @@ static int sep_crypto_send_key(struct ablkcipher_request *req)
 			sep_write_msg(ta_ctx, ta_ctx->iv,
 				SEP_AES_IV_SIZE_BYTES, max_length,
 				&msg_offset, 1);
-			sep_dump(ta_ctx->sep_used, "initial IV",
-				ta_ctx->walk.iv, SEP_AES_IV_SIZE_BYTES);
 		} else {
 				/* Skip if ECB */
 				msg_offset += max_length;
@@ -1646,7 +1559,6 @@ static u32 crypto_post_op(struct sep_device *sep)
 
 	dev_dbg(&ta_ctx->sep_used->pdev->dev, "crypto post_op\n");
 	dev_dbg(&ta_ctx->sep_used->pdev->dev, "crypto post_op message dump\n");
-	crypto_sep_dump_message(ta_ctx->sep_used, ta_ctx->msg);
 
 	/* first bring msg from shared area to local area */
 	memcpy(ta_ctx->msg, sep->shared_addr,
@@ -1670,16 +1582,10 @@ static u32 crypto_post_op(struct sep_device *sep)
 			sep_read_context(ta_ctx, &msg_offset,
 			&sctx->des_private_ctx,
 			sizeof(struct sep_des_private_context));
-
-			sep_dump(ta_ctx->sep_used, "ctx init des",
-				&sctx->des_private_ctx, 40);
 		} else {
 			sep_read_context(ta_ctx, &msg_offset,
 			&sctx->aes_private_ctx,
 			sizeof(struct sep_aes_private_context));
-
-			sep_dump(ta_ctx->sep_used, "ctx init aes",
-				&sctx->aes_private_ctx, 20);
 		}
 
 		sep_dump_ivs(req, "after sending key to sep\n");
@@ -1758,9 +1664,6 @@ static u32 crypto_post_op(struct sep_device *sep)
 				sizeof(struct sep_aes_private_context));
 		}
 
-		sep_dump_sg(ta_ctx->sep_used,
-			"block sg out", ta_ctx->dst_sg);
-
 		/* Copy to correct sg if this block had oddball pages */
 		if (ta_ctx->dst_sg_hold)
 			sep_copy_sg(ta_ctx->sep_used,
@@ -1870,7 +1773,7 @@ static u32 hash_update_post_op(struct sep_device *sep)
 		sizeof(struct sep_hash_private_context));
 
 	/**
-	 * Following is only for finup; if we just completd the
+	 * Following is only for finup; if we just completed the
 	 * data portion of finup, we now need to kick off the
 	 * finish portion of finup.
 	 */
@@ -2011,7 +1914,7 @@ static u32 hash_digest_post_op(struct sep_device *sep)
 }
 
 /**
- * The sep_finish function is the function that is schedule (via tasket)
+ * The sep_finish function is the function that is scheduled (via tasklet)
  * by the interrupt service routine when the SEP sends and interrupt
  * This is only called by the interrupt handler as a tasklet.
  */
@@ -2249,7 +2152,7 @@ static void sep_hash_update(void *data)
 	head_len = (block_size - int_ctx->prev_update_bytes) % block_size;
 	tail_len = (req->nbytes - head_len) % block_size;
 
-	/* Make sure all pages are even block */
+	/* Make sure all pages are an even block */
 	int_error = sep_oddball_pages(ta_ctx->sep_used, req->src,
 		req->nbytes,
 		block_size, &new_sg, 1);
@@ -2274,8 +2177,6 @@ static void sep_hash_update(void *data)
 		src_ptr = NULL;
 	}
 
-	sep_dump_sg(ta_ctx->sep_used, "hash block sg in", ta_ctx->src_sg);
-
 	ta_ctx->dcb_input_data.app_in_address = src_ptr;
 	ta_ctx->dcb_input_data.data_in_size =
 		req->nbytes - (head_len + tail_len);
@@ -2482,7 +2383,7 @@ static void sep_hash_digest(void *data)
 	dev_dbg(&ta_ctx->sep_used->pdev->dev, "block_size is %x\n", block_size);
 	dev_dbg(&ta_ctx->sep_used->pdev->dev, "tail len is %x\n", tail_len);
 
-	/* Make sure all pages are even block */
+	/* Make sure all pages are an even block */
 	int_error = sep_oddball_pages(ta_ctx->sep_used, req->src,
 		req->nbytes,
 		block_size, &new_sg, 1);
@@ -2507,8 +2408,6 @@ static void sep_hash_digest(void *data)
 		src_ptr = NULL;
 	}
 
-	sep_dump_sg(ta_ctx->sep_used, "hash block sg in", ta_ctx->src_sg);
-
 	ta_ctx->dcb_input_data.app_in_address = src_ptr;
 	ta_ctx->dcb_input_data.data_in_size = req->nbytes - tail_len;
 	ta_ctx->dcb_input_data.app_out_address = NULL;
diff --git a/drivers/staging/sep/sep_driver_api.h b/drivers/staging/sep/sep_driver_api.h
index 8b797d5388b..7ee1c3bf17d 100644
--- a/drivers/staging/sep/sep_driver_api.h
+++ b/drivers/staging/sep/sep_driver_api.h
@@ -91,7 +91,7 @@ struct sep_dcblock {
 };
 
 /*
-	command structure for building dcb block (currently for ext app only
+	command structure for building dcb block (currently for ext app only)
 */
 struct build_dcb_struct {
 	/* address value of the data in */
@@ -234,7 +234,7 @@ struct sep_dma_context {
 	u32 dmatables_len;
 	/* size of input data */
 	u32 input_data_len;
-	/* secure dma use (for imr memory restriced area in output */
+	/* secure dma use (for imr memory restricted area in output) */
 	bool secure_dma;
 	struct sep_dma_resource dma_res_arr[SEP_MAX_NUM_SYNC_DMA_OPS];
 	/* Scatter gather for kernel crypto */
@@ -347,10 +347,10 @@ int sep_prepare_input_output_dma_table_in_dcb(struct sep_device *sep,
 
 /**
  * sep_free_dma_table_data_handler - free DMA table
- * @sep: pointere to struct sep_device
+ * @sep: pointer to struct sep_device
  * @dma_ctx: dma context
  *
- * Handles the request to  free DMA table for synchronic actions
+ * Handles the request to free DMA table for synchronic actions
  */
 int sep_free_dma_table_data_handler(struct sep_device *sep,
 					   struct sep_dma_context **dma_ctx);
diff --git a/drivers/staging/sep/sep_driver_config.h b/drivers/staging/sep/sep_driver_config.h
index 9d9fc7c94a6..7d7c7ab610b 100644
--- a/drivers/staging/sep/sep_driver_config.h
+++ b/drivers/staging/sep/sep_driver_config.h
@@ -43,7 +43,7 @@
 #define SEP_DRIVER_POLLING_MODE                         0
 
 /* flag which defines if the shared area address should be
-	reconfiged (send to SEP anew) during init of the driver */
+	reconfigured (send to SEP anew) during init of the driver */
 #define SEP_DRIVER_RECONFIG_MESSAGE_AREA                0
 
 /* the mode for running on the ARM1172 Evaluation platform (flag is 1) */
@@ -166,7 +166,7 @@ held by the process (struct file) */
 	(SEP_DRIVER_STATIC_AREA_OFFSET_IN_BYTES + \
 	SEP_DRIVER_STATIC_AREA_SIZE_IN_BYTES)
 
-/* synhronic dma tables area offset */
+/* synchronic dma tables area offset */
 #define SYNCHRONIC_DMA_TABLES_AREA_OFFSET_BYTES \
 	(SEP_DRIVER_DATA_POOL_AREA_OFFSET_IN_BYTES + \
 	SEP_DRIVER_DATA_POOL_SHARED_AREA_SIZE_IN_BYTES)
diff --git a/drivers/staging/sep/sep_main.c b/drivers/staging/sep/sep_main.c
index df1d13e96fc..ca8946acba6 100644
--- a/drivers/staging/sep/sep_main.c
+++ b/drivers/staging/sep/sep_main.c
@@ -61,7 +61,6 @@
 #include <linux/interrupt.h>
 #include <linux/pagemap.h>
 #include <asm/cacheflush.h>
-#include <linux/sched.h>
 #include <linux/delay.h>
 #include <linux/jiffies.h>
 #include <linux/async.h>
@@ -94,7 +93,7 @@
 #endif
 
 /**
- * Currenlty, there is only one SEP device per platform;
+ * Currently, there is only one SEP device per platform;
  * In event platforms in the future have more than one SEP
  * device, this will be a linked list
  */
@@ -106,7 +105,7 @@ struct sep_device *sep_dev;
  * @sep: SEP device
  * @sep_queue_info: pointer to status queue
  *
- * This function will removes information about transaction from the queue.
+ * This function will remove information about transaction from the queue.
  */
 void sep_queue_status_remove(struct sep_device *sep,
 				      struct sep_queue_info **queue_elem)
@@ -294,7 +293,7 @@ int sep_wait_transaction(struct sep_device *sep)
 end_function_setpid:
 	/*
 	 * The pid_doing_transaction indicates that this process
-	 * now owns the facilities to performa a transaction with
+	 * now owns the facilities to perform a transaction with
 	 * the SEP. While this process is performing a transaction,
 	 * no other process who has the SEP device open can perform
 	 * any transactions. This method allows more than one process
@@ -447,10 +446,10 @@ static int sep_open(struct inode *inode, struct file *filp)
 
 /**
  * sep_free_dma_table_data_handler - free DMA table
- * @sep: pointere to struct sep_device
+ * @sep: pointer to struct sep_device
  * @dma_ctx: dma context
  *
- * Handles the request to  free DMA table for synchronic actions
+ * Handles the request to free DMA table for synchronic actions
  */
 int sep_free_dma_table_data_handler(struct sep_device *sep,
 					   struct sep_dma_context **dma_ctx)
@@ -540,7 +539,7 @@ int sep_free_dma_table_data_handler(struct sep_device *sep,
 		 * don't have a page array; the page array is generated
 		 * only in the lock_user_pages, which is not called
 		 * for kernel crypto, which is what the sg (scatter gather
-		 * is used for exclusively
+		 * is used for exclusively)
 		 */
 		if (dma->src_sg) {
 			dma_unmap_sg(&sep->pdev->dev, dma->src_sg,
@@ -1227,7 +1226,7 @@ static int sep_lock_user_pages(struct sep_device *sep,
 	/* Map array */
 	struct sep_dma_map *map_array;
 
-	/* Set start and end pages  and num pages */
+	/* Set start and end pages and num pages */
 	end_page = (app_virt_addr + data_size - 1) >> PAGE_SHIFT;
 	start_page = app_virt_addr >> PAGE_SHIFT;
 	num_pages = end_page - start_page + 1;
@@ -1431,13 +1430,14 @@ static int sep_lli_table_secure_dma(struct sep_device *sep,
 	/* Array of lli */
 	struct sep_lli_entry *lli_array;
 
-	/* Set start and end pages  and num pages */
+	/* Set start and end pages and num pages */
 	end_page = (app_virt_addr + data_size - 1) >> PAGE_SHIFT;
 	start_page = app_virt_addr >> PAGE_SHIFT;
 	num_pages = end_page - start_page + 1;
 
-	dev_dbg(&sep->pdev->dev, "[PID%d] lock user pages"
-		" app_virt_addr is %x\n", current->pid, app_virt_addr);
+	dev_dbg(&sep->pdev->dev,
+		"[PID%d] lock user pages  app_virt_addr is %x\n",
+		current->pid, app_virt_addr);
 
 	dev_dbg(&sep->pdev->dev, "[PID%d] data_size is (hex) %x\n",
 		current->pid, data_size);
@@ -1601,7 +1601,7 @@ end_function:
  * @num_table_entries_ptr: pointer to number of tables
  * @table_data_size: total data size
  *
- * Builds ant lli table from the lli_array according to
+ * Builds an lli table from the lli_array according to
  * the given size of data
  */
 static void sep_build_lli_table(struct sep_device *sep,
@@ -1700,7 +1700,7 @@ static void sep_build_lli_table(struct sep_device *sep,
  * @virt_address: virtual address to convert
  *
  * This functions returns the physical address inside shared area according
- * to the virtual address. It can be either on the externa RAM device
+ * to the virtual address. It can be either on the external RAM device
  * (ioremapped), or on the system RAM
  * This implementation is for the external RAM
  */
@@ -1724,7 +1724,7 @@ static dma_addr_t sep_shared_area_virt_to_bus(struct sep_device *sep,
  *
  * This functions returns the virtual address inside shared area
  * according to the physical address. It can be either on the
- * externa RAM device (ioremapped), or on the system RAM
+ * external RAM device (ioremapped), or on the system RAM
  * This implementation is for the external RAM
  */
 static void *sep_shared_area_bus_to_virt(struct sep_device *sep,
@@ -1890,9 +1890,9 @@ static void sep_prepare_empty_lli_table(struct sep_device *sep,
  * @lli_table_ptr:
  * @num_entries_ptr:
  * @table_data_size_ptr:
- * @is_kva: set for kernel data (kernel cryptio call)
+ * @is_kva: set for kernel data (kernel crypt io call)
  *
- * This function prepares only input DMA table for synhronic symmetric
+ * This function prepares only input DMA table for synchronic symmetric
  * operations (HASH)
  * Note that all bus addresses that are passed to the SEP
  * are in 32 bit format; the SEP is a 32 bit device
@@ -1931,9 +1931,9 @@ static int sep_prepare_input_dma_table(struct sep_device *sep,
 	void *dma_lli_table_alloc_addr = NULL;
 	void *dma_in_lli_table_ptr = NULL;
 
-	dev_dbg(&sep->pdev->dev, "[PID%d] prepare intput dma "
-				 "tbl data size: (hex) %x\n",
-					current->pid, data_size);
+	dev_dbg(&sep->pdev->dev,
+		"[PID%d] prepare intput dma tbl data size: (hex) %x\n",
+		current->pid, data_size);
 
 	dev_dbg(&sep->pdev->dev, "[PID%d] block_size is (hex) %x\n",
 					current->pid, block_size);
@@ -2173,9 +2173,9 @@ static int sep_construct_dma_tables_from_lli(
 	u32 last_table_flag = 0;
 	/* The data size that should be in table */
 	u32 table_data_size = 0;
-	/* Number of etnries in the input table */
+	/* Number of entries in the input table */
 	u32 num_entries_in_table = 0;
-	/* Number of etnries in the output table */
+	/* Number of entries in the output table */
 	u32 num_entries_out_table = 0;
 
 	if (!dma_ctx) {
@@ -2400,7 +2400,7 @@ static int sep_construct_dma_tables_from_lli(
  * @table_data_size_ptr:
  * @is_kva: set for kernel data; used only for kernel crypto module
  *
- * This function builds input and output DMA tables for synhronic
+ * This function builds input and output DMA tables for synchronic
  * symmetric operations (AES, DES, HASH). It also checks that each table
  * is of the modular block size
  * Note that all bus addresses that are passed to the SEP
@@ -2542,19 +2542,20 @@ static int sep_prepare_input_output_dma_table(struct sep_device *sep,
 		}
 	}
 
-	dev_dbg(&sep->pdev->dev, "[PID%d] After lock; prep input output dma "
-		"table sep_in_num_pages is (hex) %x\n", current->pid,
+	dev_dbg(&sep->pdev->dev,
+		"[PID%d] After lock; prep input output dma table sep_in_num_pages is (hex) %x\n",
+		current->pid,
 		dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].in_num_pages);
 
 	dev_dbg(&sep->pdev->dev, "[PID%d] sep_out_num_pages is (hex) %x\n",
 		current->pid,
 		dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].out_num_pages);
 
-	dev_dbg(&sep->pdev->dev, "[PID%d] SEP_DRIVER_ENTRIES_PER_TABLE_IN_SEP"
-		" is (hex) %x\n", current->pid,
-		SEP_DRIVER_ENTRIES_PER_TABLE_IN_SEP);
+	dev_dbg(&sep->pdev->dev,
+		"[PID%d] SEP_DRIVER_ENTRIES_PER_TABLE_IN_SEP is (hex) %x\n",
+		current->pid, SEP_DRIVER_ENTRIES_PER_TABLE_IN_SEP);
 
-	/* Call the fucntion that creates table from the lli arrays */
+	/* Call the function that creates table from the lli arrays */
 	dev_dbg(&sep->pdev->dev, "[PID%d] calling create table from lli\n",
 					current->pid);
 	error = sep_construct_dma_tables_from_lli(
@@ -3661,7 +3662,7 @@ static ssize_t sep_read(struct file *filp,
 		goto end_function;
 	}
 
-	/* Checks that user has called necessarry apis */
+	/* Checks that user has called necessary apis */
 	if (0 == test_bit(SEP_FASTCALL_WRITE_DONE_OFFSET,
 			&call_status->status)) {
 		dev_warn(&sep->pdev->dev,
@@ -3844,8 +3845,9 @@ static ssize_t sep_write(struct file *filp,
 	 * buffers created. Only SEP_DOUBLEBUF_USERS_LIMIT number
 	 * of threads can progress further at a time
 	 */
-	dev_dbg(&sep->pdev->dev, "[PID%d] waiting for double buffering "
-				 "region access\n", current->pid);
+	dev_dbg(&sep->pdev->dev,
+		"[PID%d] waiting for double buffering region access\n",
+		current->pid);
 	error = down_interruptible(&sep->sep_doublebuf);
 	dev_dbg(&sep->pdev->dev, "[PID%d] double buffering region start\n",
 					current->pid);
@@ -3889,8 +3891,8 @@ static ssize_t sep_write(struct file *filp,
 				     current->comm, sizeof(current->comm));
 
 	if (!my_queue_elem) {
-		dev_dbg(&sep->pdev->dev, "[PID%d] updating queue"
-					"status error\n", current->pid);
+		dev_dbg(&sep->pdev->dev,
+			"[PID%d] updating queue status error\n", current->pid);
 		error = -ENOMEM;
 		goto end_function_error_doublebuf;
 	}
@@ -4155,8 +4157,8 @@ static int __devinit sep_probe(struct pci_dev *pdev,
 
 	INIT_LIST_HEAD(&sep->sep_queue_status);
 
-	dev_dbg(&sep->pdev->dev, "sep probe: PCI obtained, "
-		"device being prepared\n");
+	dev_dbg(&sep->pdev->dev,
+		"sep probe: PCI obtained, device being prepared\n");
 
 	/* Set up our register area */
 	sep->reg_physical_addr = pci_resource_start(sep->pdev, 0);
@@ -4318,7 +4320,7 @@ static void sep_remove(struct pci_dev *pdev)
 static DEFINE_PCI_DEVICE_TABLE(sep_pci_id_tbl) = {
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0826)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x08e9)},
- 	{0}
+	{0}
 };
 
 /* Export our pci_device_id structure to user space */
@@ -4489,30 +4491,5 @@ static struct pci_driver sep_pci_driver = {
 	.remove = sep_remove
 };
 
-/**
- * sep_init - init function
- *
- * Module load time. Register the PCI device driver.
- */
-
-static int __init sep_init(void)
-{
-	return pci_register_driver(&sep_pci_driver);
-}
-
-
-/**
- * sep_exit - called to unload driver
- *
- * Unregister the driver The device will perform all the cleanup required.
- */
-static void __exit sep_exit(void)
-{
-	pci_unregister_driver(&sep_pci_driver);
-}
-
-
-module_init(sep_init);
-module_exit(sep_exit);
-
+module_pci_driver(sep_pci_driver);
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index 43045db982d..8a362f7af37 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -609,7 +609,7 @@ static int BoxSetRegister(struct usb_serial *serial, unsigned short Uart_Number,
 
 /*
  * qt_setuart
- * issuse a SET_UART vendor-spcific request on the default control pipe
+ * issues a SET_UART vendor-specific request on the default control pipe
  * If successful sets baud rate divisor and LCR value
  */
 static int qt_setuart(struct usb_serial *serial, unsigned short Uart_Number,
@@ -1388,7 +1388,7 @@ static inline int qt_real_tiocmset(struct tty_struct *tty,
 		return -ESPIPE;
 
 	/*
-	 * Turn off the RTS and DTR and loopbcck and then only turn on what was
+	 * Turn off the RTS and DTR and loopback and then only turn on what was
 	 * asked for
 	 */
 	mcr &= ~(SERIAL_MCR_RTS | SERIAL_MCR_DTR | SERIAL_MCR_LOOP);
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 77a0751a31a..56829fc032f 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -47,7 +47,7 @@
  *      Oasis cards (single and dual port PCI-x Gigabit) copper and fiber
  *      Kalahari cards (dual and quad port PCI-e Gigabit) copper and fiber
  *
- * The driver was acutally tested on Oasis and Kalahari cards.
+ * The driver was actually tested on Oasis and Kalahari cards.
  *
  *
  * NOTE: This is the standard, non-accelerated version of Alacritech's
@@ -3196,7 +3196,6 @@ static void __devexit slic_entry_remove(struct pci_dev *pcidev)
 	struct sliccard *card;
 	struct mcast_address *mcaddr, *mlist;
 
-	ASSERT(adapter);
 	slic_adapter_freeresources(adapter);
 	slic_unmap_mmio_space(adapter);
 	unregister_netdev(dev);
@@ -3235,6 +3234,7 @@ static void __devexit slic_entry_remove(struct pci_dev *pcidev)
 	}
 	free_netdev(dev);
 	pci_release_regions(pcidev);
+	pci_disable_device(pcidev);
 }
 
 static int slic_entry_halt(struct net_device *dev)
@@ -3746,8 +3746,7 @@ static u32 slic_card_locate(struct adapter *adapter)
 		rdhostid_offset = SLIC_RDHOSTID_1GB;
 		break;
 	default:
-		ASSERT(0);
-		break;
+		return -ENODEV;
 	}
 
 	hostid_reg =
diff --git a/drivers/staging/sm7xx/Kconfig b/drivers/staging/sm7xx/Kconfig
deleted file mode 100644
index 315102c7fed..00000000000
--- a/drivers/staging/sm7xx/Kconfig
+++ /dev/null
@@ -1,8 +0,0 @@
-config FB_SM7XX
-	tristate "Silicon Motion SM7XX Frame Buffer Support"
-	depends on FB
-	select FB_CFB_FILLRECT
-	select FB_CFB_COPYAREA
-	select FB_CFB_IMAGEBLIT
-	help
-	  Frame Buffer driver for the Silicon Motion SM7XX serial graphic card.
diff --git a/drivers/staging/sm7xx/Makefile b/drivers/staging/sm7xx/Makefile
deleted file mode 100644
index f43cb910630..00000000000
--- a/drivers/staging/sm7xx/Makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-obj-$(CONFIG_FB_SM7XX) += sm7xx.o
-
-sm7xx-y := smtcfb.o
diff --git a/drivers/staging/sm7xxfb/Kconfig b/drivers/staging/sm7xxfb/Kconfig
new file mode 100644
index 00000000000..e2922ae3a3e
--- /dev/null
+++ b/drivers/staging/sm7xxfb/Kconfig
@@ -0,0 +1,13 @@
+config FB_SM7XX
+	tristate "Silicon Motion SM7XX framebuffer support"
+	depends on FB && PCI
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	help
+	  Frame buffer driver for the Silicon Motion SM710, SM712, SM721
+	  and SM722 chips.
+
+	  This driver is also available as a module. The module will be
+	  called sm7xxfb. If you want to compile it as a module, say M
+	  here and read <file:Documentation/kbuild/modules.txt>.
diff --git a/drivers/staging/sm7xxfb/Makefile b/drivers/staging/sm7xxfb/Makefile
new file mode 100644
index 00000000000..48f471cf9f3
--- /dev/null
+++ b/drivers/staging/sm7xxfb/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_FB_SM7XX) += sm7xxfb.o
diff --git a/drivers/staging/sm7xx/TODO b/drivers/staging/sm7xxfb/TODO
index 7304021368c..1fcead591c1 100644
--- a/drivers/staging/sm7xx/TODO
+++ b/drivers/staging/sm7xxfb/TODO
@@ -3,7 +3,7 @@ TODO:
 - 2D acceleration support
 - use kernel coding style
 - refine the code and remove unused code
-- move it to drivers/video/sm7xx/ or make it be drivers/video/sm7xxfb.c
+- move it to drivers/video/sm7xxfb.c
 
 Please send any patches to Greg Kroah-Hartman <greg@kroah.com> and
 Teddy Wang <teddy.wang@siliconmotion.com.cn>.
diff --git a/drivers/staging/sm7xx/smtcfb.h b/drivers/staging/sm7xxfb/sm7xx.h
index 43d86f87341..333f33c3dc6 100644
--- a/drivers/staging/sm7xx/smtcfb.h
+++ b/drivers/staging/sm7xxfb/sm7xx.h
@@ -14,7 +14,6 @@
  */
 
 #define NR_PALETTE        256
-#define NR_RGB            2
 
 #define FB_ACCEL_SMI_LYNX 88
 
diff --git a/drivers/staging/sm7xx/smtcfb.c b/drivers/staging/sm7xxfb/sm7xxfb.c
index 746c4cd5d30..1c1780c70fb 100644
--- a/drivers/staging/sm7xx/smtcfb.c
+++ b/drivers/staging/sm7xxfb/sm7xxfb.c
@@ -15,6 +15,7 @@
  * License. See the file COPYING in the main directory of this archive for
  * more details.
  *
+ * Framebuffer driver for Silicon Motion SM710, SM712, SM721 and SM722 chips
  */
 
 #include <linux/io.h>
@@ -31,22 +32,17 @@
 #include <linux/pm.h>
 #endif
 
-#include "smtcfb.h"
-
-struct screen_info smtc_screen_info;
+#include "sm7xx.h"
 
 /*
 * Private structure
 */
 struct smtcfb_info {
-	struct fb_info fb;
 	struct pci_dev *pdev;
-	struct {
-		u8 red, green, blue;
-	} palette[NR_RGB];
-	u_int palette_size;
+	struct fb_info fb;
+	u16 chip_id;
+	u8  chip_rev_id;
 
-	u16 chipID;
 	unsigned char __iomem *m_pMMIO;
 	char __iomem *m_pLFB;
 	char *m_pDPR;
@@ -56,39 +52,13 @@ struct smtcfb_info {
 	u_int width;
 	u_int height;
 	u_int hz;
-	u_long BaseAddressInVRAM;
-	u8 chipRevID;
-};
 
-struct vesa_mode_table	{
-	char mode_index[6];
-	u16 lfb_width;
-	u16 lfb_height;
-	u16 lfb_depth;
-};
-
-static struct vesa_mode_table vesa_mode[] = {
-	{"0x301", 640,  480,  8},
-	{"0x303", 800,  600,  8},
-	{"0x305", 1024, 768,  8},
-	{"0x307", 1280, 1024, 8},
-
-	{"0x311", 640,  480,  16},
-	{"0x314", 800,  600,  16},
-	{"0x317", 1024, 768,  16},
-	{"0x31A", 1280, 1024, 16},
-
-	{"0x312", 640,  480,  24},
-	{"0x315", 800,  600,  24},
-	{"0x318", 1024, 768,  24},
-	{"0x31B", 1280, 1024, 24},
+	u32 colreg[17];
 };
 
 char __iomem *smtc_RegBaseAddress;	/* Memory Map IO starting address */
 char __iomem *smtc_VRAMBaseAddress;	/* video memory starting address */
 
-static u32 colreg[17];
-
 static struct fb_var_screeninfo smtcfb_var = {
 	.xres           = 1024,
 	.yres           = 600,
@@ -112,145 +82,71 @@ static struct fb_fix_screeninfo smtcfb_fix = {
 	.accel          = FB_ACCEL_SMI_LYNX,
 };
 
-static void sm712_set_timing(struct smtcfb_info *sfb)
-{
-	int i = 0, j = 0;
-	u32 m_nScreenStride;
-
-	dev_dbg(&sfb->pdev->dev,
-		"sfb->width=%d sfb->height=%d "
-		"sfb->fb.var.bits_per_pixel=%d sfb->hz=%d\n",
-		sfb->width, sfb->height, sfb->fb.var.bits_per_pixel, sfb->hz);
-
-	for (j = 0; j < numVGAModes; j++) {
-		if (VGAMode[j].mmSizeX == sfb->width &&
-		    VGAMode[j].mmSizeY == sfb->height &&
-		    VGAMode[j].bpp == sfb->fb.var.bits_per_pixel &&
-		    VGAMode[j].hz == sfb->hz) {
-
-			dev_dbg(&sfb->pdev->dev,
-				"VGAMode[j].mmSizeX=%d VGAMode[j].mmSizeY=%d "
-				"VGAMode[j].bpp=%d VGAMode[j].hz=%d\n",
-				VGAMode[j].mmSizeX, VGAMode[j].mmSizeY,
-				VGAMode[j].bpp, VGAMode[j].hz);
-
-			dev_dbg(&sfb->pdev->dev, "VGAMode index=%d\n", j);
-
-			smtc_mmiowb(0x0, 0x3c6);
-
-			smtc_seqw(0, 0x1);
-
-			smtc_mmiowb(VGAMode[j].Init_MISC, 0x3c2);
-
-			/* init SEQ register SR00 - SR04 */
-			for (i = 0; i < SIZE_SR00_SR04; i++)
-				smtc_seqw(i, VGAMode[j].Init_SR00_SR04[i]);
+struct vesa_mode {
+	char index[6];
+	u16  lfb_width;
+	u16  lfb_height;
+	u16  lfb_depth;
+};
 
-			/* init SEQ register SR10 - SR24 */
-			for (i = 0; i < SIZE_SR10_SR24; i++)
-				smtc_seqw(i + 0x10,
-					  VGAMode[j].Init_SR10_SR24[i]);
+static struct vesa_mode vesa_mode_table[] = {
+	{"0x301", 640,  480,  8},
+	{"0x303", 800,  600,  8},
+	{"0x305", 1024, 768,  8},
+	{"0x307", 1280, 1024, 8},
 
-			/* init SEQ register SR30 - SR75 */
-			for (i = 0; i < SIZE_SR30_SR75; i++)
-				if (((i + 0x30) != 0x62) \
-					&& ((i + 0x30) != 0x6a) \
-					&& ((i + 0x30) != 0x6b))
-					smtc_seqw(i + 0x30,
-						VGAMode[j].Init_SR30_SR75[i]);
+	{"0x311", 640,  480,  16},
+	{"0x314", 800,  600,  16},
+	{"0x317", 1024, 768,  16},
+	{"0x31A", 1280, 1024, 16},
 
-			/* init SEQ register SR80 - SR93 */
-			for (i = 0; i < SIZE_SR80_SR93; i++)
-				smtc_seqw(i + 0x80,
-					  VGAMode[j].Init_SR80_SR93[i]);
+	{"0x312", 640,  480,  24},
+	{"0x315", 800,  600,  24},
+	{"0x318", 1024, 768,  24},
+	{"0x31B", 1280, 1024, 24},
+};
 
-			/* init SEQ register SRA0 - SRAF */
-			for (i = 0; i < SIZE_SRA0_SRAF; i++)
-				smtc_seqw(i + 0xa0,
-					  VGAMode[j].Init_SRA0_SRAF[i]);
+struct screen_info smtc_scr_info;
 
-			/* init Graphic register GR00 - GR08 */
-			for (i = 0; i < SIZE_GR00_GR08; i++)
-				smtc_grphw(i, VGAMode[j].Init_GR00_GR08[i]);
+/* process command line options, get vga parameter */
+static int __init sm7xx_vga_setup(char *options)
+{
+	int i;
 
-			/* init Attribute register AR00 - AR14 */
-			for (i = 0; i < SIZE_AR00_AR14; i++)
-				smtc_attrw(i, VGAMode[j].Init_AR00_AR14[i]);
+	if (!options || !*options)
+		return -EINVAL;
 
-			/* init CRTC register CR00 - CR18 */
-			for (i = 0; i < SIZE_CR00_CR18; i++)
-				smtc_crtcw(i, VGAMode[j].Init_CR00_CR18[i]);
+	smtc_scr_info.lfb_width = 0;
+	smtc_scr_info.lfb_height = 0;
+	smtc_scr_info.lfb_depth = 0;
 
-			/* init CRTC register CR30 - CR4D */
-			for (i = 0; i < SIZE_CR30_CR4D; i++)
-				smtc_crtcw(i + 0x30,
-					   VGAMode[j].Init_CR30_CR4D[i]);
+	pr_debug("sm7xx_vga_setup = %s\n", options);
 
-			/* init CRTC register CR90 - CRA7 */
-			for (i = 0; i < SIZE_CR90_CRA7; i++)
-				smtc_crtcw(i + 0x90,
-					   VGAMode[j].Init_CR90_CRA7[i]);
+	for (i = 0; i < ARRAY_SIZE(vesa_mode_table); i++) {
+		if (strstr(options, vesa_mode_table[i].index)) {
+			smtc_scr_info.lfb_width  = vesa_mode_table[i].lfb_width;
+			smtc_scr_info.lfb_height = vesa_mode_table[i].lfb_height;
+			smtc_scr_info.lfb_depth  = vesa_mode_table[i].lfb_depth;
+			return 0;
 		}
 	}
-	smtc_mmiowb(0x67, 0x3c2);
-
-	/* set VPR registers */
-	writel(0x0, sfb->m_pVPR + 0x0C);
-	writel(0x0, sfb->m_pVPR + 0x40);
-
-	/* set data width */
-	m_nScreenStride =
-		(sfb->width * sfb->fb.var.bits_per_pixel) / 64;
-	switch (sfb->fb.var.bits_per_pixel) {
-	case 8:
-		writel(0x0, sfb->m_pVPR + 0x0);
-		break;
-	case 16:
-		writel(0x00020000, sfb->m_pVPR + 0x0);
-		break;
-	case 24:
-		writel(0x00040000, sfb->m_pVPR + 0x0);
-		break;
-	case 32:
-		writel(0x00030000, sfb->m_pVPR + 0x0);
-		break;
-	}
-	writel((u32) (((m_nScreenStride + 2) << 16) | m_nScreenStride),
-	       sfb->m_pVPR + 0x10);
 
+	return -1;
 }
+__setup("vga=", sm7xx_vga_setup);
 
 static void sm712_setpalette(int regno, unsigned red, unsigned green,
 			     unsigned blue, struct fb_info *info)
 {
-	struct smtcfb_info *sfb = info->par;
+	/* set bit 5:4 = 01 (write LCD RAM only) */
+	smtc_seqw(0x66, (smtc_seqr(0x66) & 0xC3) | 0x10);
 
-	if (sfb->BaseAddressInVRAM)
-		/*
-		 * second display palette for dual head. Enable CRT RAM, 6-bit
-		 * RAM
-		 */
-		smtc_seqw(0x66, (smtc_seqr(0x66) & 0xC3) | 0x20);
-	else
-		/* primary display palette. Enable LCD RAM only, 6-bit RAM */
-		smtc_seqw(0x66, (smtc_seqr(0x66) & 0xC3) | 0x10);
 	smtc_mmiowb(regno, dac_reg);
 	smtc_mmiowb(red >> 10, dac_val);
 	smtc_mmiowb(green >> 10, dac_val);
 	smtc_mmiowb(blue >> 10, dac_val);
 }
 
-static void smtc_set_timing(struct smtcfb_info *sfb)
-{
-	switch (sfb->chipID) {
-	case 0x710:
-	case 0x712:
-	case 0x720:
-		sm712_set_timing(sfb);
-		break;
-	}
-}
-
 /* chan_to_field
  *
  * convert a colour value into a field position
@@ -340,9 +236,11 @@ static int smtc_blank(int blank_mode, struct fb_info *info)
 static int smtc_setcolreg(unsigned regno, unsigned red, unsigned green,
 			  unsigned blue, unsigned trans, struct fb_info *info)
 {
-	struct smtcfb_info *sfb = (struct smtcfb_info *)info;
+	struct smtcfb_info *sfb;
 	u32 val;
 
+	sfb = info->par;
+
 	if (regno > 255)
 		return 1;
 
@@ -350,7 +248,7 @@ static int smtc_setcolreg(unsigned regno, unsigned red, unsigned green,
 	case FB_VISUAL_DIRECTCOLOR:
 	case FB_VISUAL_TRUECOLOR:
 		/*
-		 * 16/32 bit true-colour, use pseuo-palette for 16 base color
+		 * 16/32 bit true-colour, use pseudo-palette for 16 base color
 		 */
 		if (regno < 16) {
 			if (sfb->fb.var.bits_per_pixel == 16) {
@@ -564,59 +462,172 @@ smtcfb_write(struct fb_info *info, const char __user *buf, size_t count,
 }
 #endif	/* ! __BIG_ENDIAN */
 
+static void sm7xx_set_timing(struct smtcfb_info *sfb)
+{
+	int i = 0, j = 0;
+	u32 m_nScreenStride;
+
+	dev_dbg(&sfb->pdev->dev,
+		"sfb->width=%d sfb->height=%d "
+		"sfb->fb.var.bits_per_pixel=%d sfb->hz=%d\n",
+		sfb->width, sfb->height, sfb->fb.var.bits_per_pixel, sfb->hz);
+
+	for (j = 0; j < numVGAModes; j++) {
+		if (VGAMode[j].mmSizeX == sfb->width &&
+		    VGAMode[j].mmSizeY == sfb->height &&
+		    VGAMode[j].bpp == sfb->fb.var.bits_per_pixel &&
+		    VGAMode[j].hz == sfb->hz) {
+
+			dev_dbg(&sfb->pdev->dev,
+				"VGAMode[j].mmSizeX=%d VGAMode[j].mmSizeY=%d "
+				"VGAMode[j].bpp=%d VGAMode[j].hz=%d\n",
+				VGAMode[j].mmSizeX, VGAMode[j].mmSizeY,
+				VGAMode[j].bpp, VGAMode[j].hz);
+
+			dev_dbg(&sfb->pdev->dev, "VGAMode index=%d\n", j);
+
+			smtc_mmiowb(0x0, 0x3c6);
+
+			smtc_seqw(0, 0x1);
+
+			smtc_mmiowb(VGAMode[j].Init_MISC, 0x3c2);
+
+			/* init SEQ register SR00 - SR04 */
+			for (i = 0; i < SIZE_SR00_SR04; i++)
+				smtc_seqw(i, VGAMode[j].Init_SR00_SR04[i]);
+
+			/* init SEQ register SR10 - SR24 */
+			for (i = 0; i < SIZE_SR10_SR24; i++)
+				smtc_seqw(i + 0x10,
+					  VGAMode[j].Init_SR10_SR24[i]);
+
+			/* init SEQ register SR30 - SR75 */
+			for (i = 0; i < SIZE_SR30_SR75; i++)
+				if (((i + 0x30) != 0x62) \
+					&& ((i + 0x30) != 0x6a) \
+					&& ((i + 0x30) != 0x6b))
+					smtc_seqw(i + 0x30,
+						VGAMode[j].Init_SR30_SR75[i]);
+
+			/* init SEQ register SR80 - SR93 */
+			for (i = 0; i < SIZE_SR80_SR93; i++)
+				smtc_seqw(i + 0x80,
+					  VGAMode[j].Init_SR80_SR93[i]);
+
+			/* init SEQ register SRA0 - SRAF */
+			for (i = 0; i < SIZE_SRA0_SRAF; i++)
+				smtc_seqw(i + 0xa0,
+					  VGAMode[j].Init_SRA0_SRAF[i]);
+
+			/* init Graphic register GR00 - GR08 */
+			for (i = 0; i < SIZE_GR00_GR08; i++)
+				smtc_grphw(i, VGAMode[j].Init_GR00_GR08[i]);
+
+			/* init Attribute register AR00 - AR14 */
+			for (i = 0; i < SIZE_AR00_AR14; i++)
+				smtc_attrw(i, VGAMode[j].Init_AR00_AR14[i]);
+
+			/* init CRTC register CR00 - CR18 */
+			for (i = 0; i < SIZE_CR00_CR18; i++)
+				smtc_crtcw(i, VGAMode[j].Init_CR00_CR18[i]);
+
+			/* init CRTC register CR30 - CR4D */
+			for (i = 0; i < SIZE_CR30_CR4D; i++)
+				smtc_crtcw(i + 0x30,
+					   VGAMode[j].Init_CR30_CR4D[i]);
+
+			/* init CRTC register CR90 - CRA7 */
+			for (i = 0; i < SIZE_CR90_CRA7; i++)
+				smtc_crtcw(i + 0x90,
+					   VGAMode[j].Init_CR90_CRA7[i]);
+		}
+	}
+	smtc_mmiowb(0x67, 0x3c2);
+
+	/* set VPR registers */
+	writel(0x0, sfb->m_pVPR + 0x0C);
+	writel(0x0, sfb->m_pVPR + 0x40);
+
+	/* set data width */
+	m_nScreenStride =
+		(sfb->width * sfb->fb.var.bits_per_pixel) / 64;
+	switch (sfb->fb.var.bits_per_pixel) {
+	case 8:
+		writel(0x0, sfb->m_pVPR + 0x0);
+		break;
+	case 16:
+		writel(0x00020000, sfb->m_pVPR + 0x0);
+		break;
+	case 24:
+		writel(0x00040000, sfb->m_pVPR + 0x0);
+		break;
+	case 32:
+		writel(0x00030000, sfb->m_pVPR + 0x0);
+		break;
+	}
+	writel((u32) (((m_nScreenStride + 2) << 16) | m_nScreenStride),
+	       sfb->m_pVPR + 0x10);
+
+}
+
+static void smtc_set_timing(struct smtcfb_info *sfb)
+{
+	switch (sfb->chip_id) {
+	case 0x710:
+	case 0x712:
+	case 0x720:
+		sm7xx_set_timing(sfb);
+		break;
+	}
+}
+
 void smtcfb_setmode(struct smtcfb_info *sfb)
 {
 	switch (sfb->fb.var.bits_per_pixel) {
 	case 32:
-		sfb->fb.fix.visual = FB_VISUAL_TRUECOLOR;
-		sfb->fb.fix.line_length = sfb->fb.var.xres * 4;
-		sfb->fb.var.red.length = 8;
+		sfb->fb.fix.visual       = FB_VISUAL_TRUECOLOR;
+		sfb->fb.fix.line_length  = sfb->fb.var.xres * 4;
+		sfb->fb.var.red.length   = 8;
 		sfb->fb.var.green.length = 8;
-		sfb->fb.var.blue.length = 8;
-		sfb->fb.var.red.offset = 16;
+		sfb->fb.var.blue.length  = 8;
+		sfb->fb.var.red.offset   = 16;
 		sfb->fb.var.green.offset = 8;
-		sfb->fb.var.blue.offset = 0;
-
-		break;
-	case 8:
-		sfb->fb.fix.visual = FB_VISUAL_PSEUDOCOLOR;
-		sfb->fb.fix.line_length = sfb->fb.var.xres;
-		sfb->fb.var.red.offset = 5;
-		sfb->fb.var.red.length = 3;
-		sfb->fb.var.green.offset = 2;
-		sfb->fb.var.green.length = 3;
-		sfb->fb.var.blue.offset = 0;
-		sfb->fb.var.blue.length = 2;
+		sfb->fb.var.blue.offset  = 0;
 		break;
 	case 24:
-		sfb->fb.fix.visual = FB_VISUAL_TRUECOLOR;
-		sfb->fb.fix.line_length = sfb->fb.var.xres * 3;
-		sfb->fb.var.red.length = 8;
+		sfb->fb.fix.visual       = FB_VISUAL_TRUECOLOR;
+		sfb->fb.fix.line_length  = sfb->fb.var.xres * 3;
+		sfb->fb.var.red.length   = 8;
 		sfb->fb.var.green.length = 8;
-		sfb->fb.var.blue.length = 8;
-
-		sfb->fb.var.red.offset = 16;
+		sfb->fb.var.blue.length  = 8;
+		sfb->fb.var.red.offset   = 16;
 		sfb->fb.var.green.offset = 8;
-		sfb->fb.var.blue.offset = 0;
-
+		sfb->fb.var.blue.offset  = 0;
+		break;
+	case 8:
+		sfb->fb.fix.visual       = FB_VISUAL_PSEUDOCOLOR;
+		sfb->fb.fix.line_length  = sfb->fb.var.xres;
+		sfb->fb.var.red.length   = 3;
+		sfb->fb.var.green.length = 3;
+		sfb->fb.var.blue.length  = 2;
+		sfb->fb.var.red.offset   = 5;
+		sfb->fb.var.green.offset = 2;
+		sfb->fb.var.blue.offset  = 0;
 		break;
 	case 16:
 	default:
-		sfb->fb.fix.visual = FB_VISUAL_TRUECOLOR;
-		sfb->fb.fix.line_length = sfb->fb.var.xres * 2;
-
-		sfb->fb.var.red.length = 5;
+		sfb->fb.fix.visual       = FB_VISUAL_TRUECOLOR;
+		sfb->fb.fix.line_length  = sfb->fb.var.xres * 2;
+		sfb->fb.var.red.length   = 5;
 		sfb->fb.var.green.length = 6;
-		sfb->fb.var.blue.length = 5;
-
-		sfb->fb.var.red.offset = 11;
+		sfb->fb.var.blue.length  = 5;
+		sfb->fb.var.red.offset   = 11;
 		sfb->fb.var.green.offset = 5;
-		sfb->fb.var.blue.offset = 0;
-
+		sfb->fb.var.blue.offset  = 0;
 		break;
 	}
 
-	sfb->width = sfb->fb.var.xres;
+	sfb->width  = sfb->fb.var.xres;
 	sfb->height = sfb->fb.var.yres;
 	sfb->hz = 60;
 	smtc_set_timing(sfb);
@@ -641,9 +652,7 @@ static int smtc_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 
 static int smtc_set_par(struct fb_info *info)
 {
-	struct smtcfb_info *sfb = (struct smtcfb_info *)info;
-
-	smtcfb_setmode(sfb);
+	smtcfb_setmode(info->par);
 
 	return 0;
 }
@@ -664,7 +673,7 @@ static struct fb_ops smtcfb_ops = {
 };
 
 /*
- * Alloc struct smtcfb_info and assign the default value
+ * alloc struct smtcfb_info and assign the default value
  */
 static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev, char *name)
 {
@@ -677,37 +686,45 @@ static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev, char *name)
 
 	sfb->pdev = pdev;
 
-	/*** Init sfb->fb with default value ***/
+	/* init sfb->fb with default value */
+
 	sfb->fb.flags = FBINFO_FLAG_DEFAULT;
 	sfb->fb.fbops = &smtcfb_ops;
-	sfb->fb.var = smtcfb_var;
-	sfb->fb.fix = smtcfb_fix;
 
+	sfb->fb.fix = smtcfb_fix;
 	strcpy(sfb->fb.fix.id, name);
 
-	sfb->fb.fix.type = FB_TYPE_PACKED_PIXELS;
-	sfb->fb.fix.type_aux = 0;
-	sfb->fb.fix.xpanstep = 0;
-	sfb->fb.fix.ypanstep = 0;
-	sfb->fb.fix.ywrapstep = 0;
-	sfb->fb.fix.accel = FB_ACCEL_SMI_LYNX;
-
-	sfb->fb.var.nonstd = 0;
-	sfb->fb.var.activate = FB_ACTIVATE_NOW;
-	sfb->fb.var.height = -1;
-	sfb->fb.var.width = -1;
-	/* text mode acceleration */
+	sfb->fb.fix.type        = FB_TYPE_PACKED_PIXELS;
+	sfb->fb.fix.type_aux    = 0;
+	sfb->fb.fix.xpanstep    = 0;
+	sfb->fb.fix.ypanstep    = 0;
+	sfb->fb.fix.ywrapstep   = 0;
+	sfb->fb.fix.accel       = FB_ACCEL_SMI_LYNX;
+
+	sfb->fb.var             = smtcfb_var;
+	sfb->fb.var.nonstd      = 0;
+	sfb->fb.var.activate    = FB_ACTIVATE_NOW;
+	sfb->fb.var.height      = -1;
+	sfb->fb.var.width       = -1;
 	sfb->fb.var.accel_flags = FB_ACCELF_TEXT;
-	sfb->fb.var.vmode = FB_VMODE_NONINTERLACED;
+	sfb->fb.var.vmode       = FB_VMODE_NONINTERLACED;
 
-	sfb->fb.par = sfb;
+	sfb->fb.pseudo_palette  = sfb->colreg;
 
-	sfb->fb.pseudo_palette = colreg;
+	sfb->fb.par = sfb;
 
 	return sfb;
 }
 
 /*
+ * free struct smtcfb_info
+ */
+static void smtc_free_fb_info(struct smtcfb_info *sfb)
+{
+	kfree(sfb);
+}
+
+/*
  * Unmap in the memory mapped IO registers
  */
 
@@ -724,16 +741,13 @@ static void smtc_unmap_mmio(struct smtcfb_info *sfb)
 static int smtc_map_smem(struct smtcfb_info *sfb,
 		struct pci_dev *pdev, u_long smem_len)
 {
-	if (sfb->fb.var.bits_per_pixel == 32) {
+
+	sfb->fb.fix.smem_start = pci_resource_start(pdev, 0);
+
 #ifdef __BIG_ENDIAN
-		sfb->fb.fix.smem_start = pci_resource_start(pdev, 0)
-			+ 0x800000;
-#else
-		sfb->fb.fix.smem_start = pci_resource_start(pdev, 0);
+	if (sfb->fb.var.bits_per_pixel == 32)
+		sfb->fb.fix.smem_start += 0x800000;
 #endif
-	} else {
-		sfb->fb.fix.smem_start = pci_resource_start(pdev, 0);
-	}
 
 	sfb->fb.fix.smem_len = smem_len;
 
@@ -761,7 +775,7 @@ static void smtc_unmap_smem(struct smtcfb_info *sfb)
 }
 
 /*
- * We need to wake up the LynxEM+, and make sure its in linear memory mode.
+ * We need to wake up the device and make sure its in linear memory mode.
  */
 static inline void sm7xx_init_hw(void)
 {
@@ -769,49 +783,6 @@ static inline void sm7xx_init_hw(void)
 	outb_p(0x11, 0x3c5);
 }
 
-static void smtc_free_fb_info(struct smtcfb_info *sfb)
-{
-	if (sfb) {
-		fb_alloc_cmap(&sfb->fb.cmap, 0, 0);
-		kfree(sfb);
-	}
-}
-
-/*
- *	sm712vga_setup - process command line options, get vga parameter
- *	@options: string of options
- *	Returns zero.
- *
- */
-static int __init sm712vga_setup(char *options)
-{
-	int index;
-
-	if (!options || !*options)
-		return -EINVAL;
-
-	smtc_screen_info.lfb_width = 0;
-	smtc_screen_info.lfb_height = 0;
-	smtc_screen_info.lfb_depth = 0;
-
-	pr_debug("sm712vga_setup = %s\n", options);
-
-	for (index = 0;
-	     index < ARRAY_SIZE(vesa_mode);
-	     index++) {
-		if (strstr(options, vesa_mode[index].mode_index)) {
-			smtc_screen_info.lfb_width = vesa_mode[index].lfb_width;
-			smtc_screen_info.lfb_height =
-					vesa_mode[index].lfb_height;
-			smtc_screen_info.lfb_depth = vesa_mode[index].lfb_depth;
-			return 0;
-		}
-	}
-
-	return -1;
-}
-__setup("vga=", sm712vga_setup);
-
 static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 				   const struct pci_device_id *ent)
 {
@@ -829,21 +800,23 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 
 	sfb = smtc_alloc_fb_info(pdev, name);
 
-	if (!sfb)
+	if (!sfb) {
+		err = -ENOMEM;
 		goto failed_free;
+	}
 
-	sfb->chipID = ent->device;
-	sprintf(name, "sm%Xfb", sfb->chipID);
+	sfb->chip_id = ent->device;
+	sprintf(name, "sm%Xfb", sfb->chip_id);
 
 	pci_set_drvdata(pdev, sfb);
 
 	sm7xx_init_hw();
 
-	/*get mode parameter from smtc_screen_info */
-	if (smtc_screen_info.lfb_width != 0) {
-		sfb->fb.var.xres = smtc_screen_info.lfb_width;
-		sfb->fb.var.yres = smtc_screen_info.lfb_height;
-		sfb->fb.var.bits_per_pixel = smtc_screen_info.lfb_depth;
+	/* get mode parameter from smtc_scr_info */
+	if (smtc_scr_info.lfb_width != 0) {
+		sfb->fb.var.xres = smtc_scr_info.lfb_width;
+		sfb->fb.var.yres = smtc_scr_info.lfb_height;
+		sfb->fb.var.bits_per_pixel = smtc_scr_info.lfb_depth;
 	} else {
 		/* default resolution 1024x600 16bit mode */
 		sfb->fb.var.xres = SCREEN_X_RES;
@@ -853,13 +826,13 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 
 #ifdef __BIG_ENDIAN
 	if (sfb->fb.var.bits_per_pixel == 24)
-		sfb->fb.var.bits_per_pixel = (smtc_screen_info.lfb_depth = 32);
+		sfb->fb.var.bits_per_pixel = (smtc_scr_info.lfb_depth = 32);
 #endif
 	/* Map address and memory detection */
 	pFramebufferPhysical = pci_resource_start(pdev, 0);
-	pci_read_config_byte(pdev, PCI_REVISION_ID, &sfb->chipRevID);
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &sfb->chip_rev_id);
 
-	switch (sfb->chipID) {
+	switch (sfb->chip_id) {
 	case 0x710:
 	case 0x712:
 		sfb->fb.fix.mmio_start = pFramebufferPhysical + 0x00400000;
@@ -938,8 +911,6 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 		goto failed;
 
 	smtcfb_setmode(sfb);
-	/* Primary display starting from 0 position */
-	sfb->BaseAddressInVRAM = 0;
 
 	err = register_framebuffer(&sfb->fb);
 	if (err < 0)
@@ -947,7 +918,7 @@ static int __devinit smtcfb_pci_probe(struct pci_dev *pdev,
 
 	dev_info(&pdev->dev,
 		 "Silicon Motion SM%X Rev%X primary display mode %dx%d-%d Init Complete.",
-		 sfb->chipID, sfb->chipRevID, sfb->fb.var.xres,
+		 sfb->chip_id, sfb->chip_rev_id, sfb->fb.var.xres,
 		 sfb->fb.var.yres, sfb->fb.var.bits_per_pixel);
 
 	return 0;
@@ -966,7 +937,11 @@ failed_free:
 	return err;
 }
 
-
+/*
+ * 0x710 (LynxEM)
+ * 0x712 (LynxEM+)
+ * 0x720 (Lynx3DM, Lynx3DM+)
+ */
 static DEFINE_PCI_DEVICE_TABLE(smtcfb_pci_table) = {
 	{ PCI_DEVICE(0x126f, 0x710), },
 	{ PCI_DEVICE(0x126f, 0x712), },
@@ -974,7 +949,6 @@ static DEFINE_PCI_DEVICE_TABLE(smtcfb_pci_table) = {
 	{0,}
 };
 
-
 static void __devexit smtcfb_pci_remove(struct pci_dev *pdev)
 {
 	struct smtcfb_info *sfb;
@@ -995,7 +969,7 @@ static int smtcfb_pci_suspend(struct device *device)
 
 	sfb = pci_get_drvdata(pdev);
 
-	/* set the hw in sleep mode use externel clock and self memory refresh
+	/* set the hw in sleep mode use external clock and self memory refresh
 	 * so that we can turn off internal PLLs later on
 	 */
 	smtc_seqw(0x20, (smtc_seqr(0x20) | 0xc0));
@@ -1020,7 +994,7 @@ static int smtcfb_pci_resume(struct device *device)
 
 	/* reinit hardware */
 	sm7xx_init_hw();
-	switch (sfb->chipID) {
+	switch (sfb->chip_id) {
 	case 0x710:
 	case 0x712:
 		/* set MCLK = 14.31818 *  (0x16 / 0x2) */
@@ -1078,18 +1052,7 @@ static struct pci_driver smtcfb_driver = {
 	.driver.pm  = SM7XX_PM_OPS,
 };
 
-static int __init smtcfb_init(void)
-{
-	return pci_register_driver(&smtcfb_driver);
-}
-
-static void __exit smtcfb_exit(void)
-{
-	pci_unregister_driver(&smtcfb_driver);
-}
-
-module_init(smtcfb_init);
-module_exit(smtcfb_exit);
+module_pci_driver(smtcfb_driver);
 
 MODULE_AUTHOR("Siliconmotion ");
 MODULE_DESCRIPTION("Framebuffer driver for SMI Graphic Cards");
diff --git a/drivers/staging/speakup/i18n.c b/drivers/staging/speakup/i18n.c
index c2119433f33..ca01734d13c 100644
--- a/drivers/staging/speakup/i18n.c
+++ b/drivers/staging/speakup/i18n.c
@@ -71,7 +71,7 @@ static char *speakup_default_msgs[MSG_LAST_INDEX] = {
 	[MSG_CTL_SHIFT] = "shift",
 	[MSG_CTL_ALTGR] = "altgr",
 	[MSG_CTL_CONTROL] = "control",
-	[MSG_CTL_ALT] = "ault",
+	[MSG_CTL_ALT] = "alt",
 	[MSG_CTL_LSHIFT] = "l shift",
 	[MSG_CTL_SPEAKUP] = "speakup",
 	[MSG_CTL_LCONTROL] = "l control",
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 92b34e29ad0..40e2488b967 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -1854,7 +1854,7 @@ static void speakup_bits(struct vc_data *vc)
 
 static int handle_goto(struct vc_data *vc, u_char type, u_char ch, u_short key)
 {
-	static u_char *goto_buf = "\0\0\0\0\0\0";
+	static u_char goto_buf[8];
 	static int num;
 	int maxlen, go_pos;
 	char *cp;
diff --git a/drivers/staging/speakup/selection.c b/drivers/staging/speakup/selection.c
index fe1f405d5d7..0612df06a4b 100644
--- a/drivers/staging/speakup/selection.c
+++ b/drivers/staging/speakup/selection.c
@@ -68,7 +68,7 @@ int speakup_set_selection(struct tty_struct *tty)
 	if (spk_sel_cons != vc_cons[fg_console].d) {
 		speakup_clear_selection();
 		spk_sel_cons = vc_cons[fg_console].d;
-		printk(KERN_WARNING
+		dev_warn(tty->dev,
 			"Selection: mark console not the same as cut\n");
 		return -EINVAL;
 	}
@@ -95,7 +95,7 @@ int speakup_set_selection(struct tty_struct *tty)
 	/* Allocate a new buffer before freeing the old one ... */
 	bp = kmalloc((sel_end-sel_start)/2+1, GFP_ATOMIC);
 	if (!bp) {
-		printk(KERN_WARNING "selection: kmalloc() failed\n");
+		dev_warn(tty->dev, "selection: kmalloc() failed\n");
 		speakup_clear_selection();
 		return -ENOMEM;
 	}
@@ -141,7 +141,7 @@ int speakup_paste_selection(struct tty_struct *tty)
 		count = sel_buffer_lth - pasted;
 		count = min_t(int, count, tty->receive_room);
 		tty->ldisc->ops->receive_buf(tty, sel_buffer + pasted,
-			0, count);
+			NULL, count);
 		pasted += count;
 	}
 	remove_wait_queue(&vc->paste_wait, &wait);
diff --git a/drivers/staging/speakup/speakup_acnt.h b/drivers/staging/speakup/speakup_acnt.h
index 2d883654ffc..6376fca9e0e 100644
--- a/drivers/staging/speakup/speakup_acnt.h
+++ b/drivers/staging/speakup/speakup_acnt.h
@@ -12,5 +12,5 @@
 				   to accept a byte of data. */
 #define SYNTH_QUIET	'S' /* synth is not speaking */
 #define SYNTH_FULL	'F' /* synth is full. */
-#define SYNTH_ALMOST_EMPTY 'M' /* synth has les than 2 seconds of text left */
+#define SYNTH_ALMOST_EMPTY 'M' /* synth has less than 2 seconds of text left */
 #define SYNTH_SPEAKING	's' /* synth is speaking and has a fare way to go */
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index de25527decf..a09a0c9975d 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -66,7 +66,7 @@
 #define	CMD_null		0x0000	/* post status */
 #define	CMD_control		0x1000	/* hard control command */
 #define	CTRL_mask		0x0F00	/*   mask off control nibble */
-#define	CTRL_data		0x00FF	/*   madk to get data byte */
+#define	CTRL_data		0x00FF	/*   mask to get data byte */
 #define	CTRL_null		0x0000	/*   null control */
 #define	CTRL_vol_up		0x0100	/*   increase volume */
 #define	CTRL_vol_down		0x0200	/*   decrease volume */
diff --git a/drivers/staging/speakup/synth.c b/drivers/staging/speakup/synth.c
index 331eae78870..df953379809 100644
--- a/drivers/staging/speakup/synth.c
+++ b/drivers/staging/speakup/synth.c
@@ -64,7 +64,7 @@ EXPORT_SYMBOL_GPL(serial_synth_probe);
 /* Main loop of the progression thread: keep eating from the buffer
  * and push to the serial port, waiting as needed
  *
- * For devices that have a "full" notification mecanism, the driver can
+ * For devices that have a "full" notification mechanism, the driver can
  * adapt the loop the way they prefer.
  */
 void spk_do_catch_up(struct spk_synth *synth)
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index 11728a03f8a..277491a877e 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -655,13 +655,13 @@ static int synpatics_rmi4_touchpad_detect(struct synaptics_rmi4_data *pdata,
 }
 
 /**
- * synpatics_rmi4_touchpad_config() - confiures the rmi4 touchpad device
+ * synaptics_rmi4_touchpad_config() - configures the rmi4 touchpad device
  * @pdata: pointer to synaptics_rmi4_data structure
  * @rfi: pointer to synaptics_rmi4_fn structure
  *
- * This function calls to confiures the rmi4 touchpad device
+ * This function calls to configures the rmi4 touchpad device
  */
-int synpatics_rmi4_touchpad_config(struct synaptics_rmi4_data *pdata,
+int synaptics_rmi4_touchpad_config(struct synaptics_rmi4_data *pdata,
 						struct synaptics_rmi4_fn *rfi)
 {
 	/*
@@ -855,7 +855,7 @@ static int synaptics_rmi4_i2c_query_device(struct synaptics_rmi4_data *pdata)
 			if (rfi->num_of_data_sources) {
 				if (rfi->fn_number ==
 					SYNAPTICS_RMI4_TOUCHPAD_FUNC_NUM) {
-					retval = synpatics_rmi4_touchpad_config
+					retval = synaptics_rmi4_touchpad_config
 								(pdata, rfi);
 					if (retval < 0)
 						return retval;
diff --git a/drivers/staging/telephony/ixj.c b/drivers/staging/telephony/ixj.c
index fd7757ad7fa..b303c9192e1 100644
--- a/drivers/staging/telephony/ixj.c
+++ b/drivers/staging/telephony/ixj.c
@@ -7094,7 +7094,7 @@ static int ixj_selfprobe(IXJ *j)
 	for (cnt = 0; cnt < 35; cnt++)
 		j->ixj_signals[cnt] = SIGIO;
 
-	/* Set the excetion signal enable flags */
+	/* Set the exception signal enable flags */
 	j->ex_sig.bits.dtmf_ready = j->ex_sig.bits.hookstate = j->ex_sig.bits.flash = j->ex_sig.bits.pstn_ring =
 	j->ex_sig.bits.caller_id = j->ex_sig.bits.pstn_wink = j->ex_sig.bits.f0 = j->ex_sig.bits.f1 = j->ex_sig.bits.f2 =
 	j->ex_sig.bits.f3 = j->ex_sig.bits.fc0 = j->ex_sig.bits.fc1 = j->ex_sig.bits.fc2 = j->ex_sig.bits.fc3 = 1;
@@ -7132,7 +7132,7 @@ IXJ *ixj_pcmcia_probe(unsigned long dsp, unsigned long xilinx)
 	return j;
 }
 
-EXPORT_SYMBOL(ixj_pcmcia_probe);		/* Fpr PCMCIA */
+EXPORT_SYMBOL(ixj_pcmcia_probe);		/* For PCMCIA */
 
 static int ixj_get_status_proc(char *buf)
 {
diff --git a/drivers/staging/tidspbridge/core/io_sm.c b/drivers/staging/tidspbridge/core/io_sm.c
index c51f651dfd1..480a3845a24 100644
--- a/drivers/staging/tidspbridge/core/io_sm.c
+++ b/drivers/staging/tidspbridge/core/io_sm.c
@@ -128,6 +128,16 @@ struct io_mgr {
 
 };
 
+struct shm_symbol_val {
+	u32 shm_base;
+	u32 shm_lim;
+	u32 msg_base;
+	u32 msg_lim;
+	u32 shm0_end;
+	u32 dyn_ext;
+	u32 ext_end;
+};
+
 /* Function Prototypes */
 static void io_dispatch_pm(struct io_mgr *pio_mgr);
 static void notify_chnl_complete(struct chnl_object *pchnl,
@@ -256,6 +266,75 @@ int bridge_io_destroy(struct io_mgr *hio_mgr)
 	return status;
 }
 
+struct shm_symbol_val *_get_shm_symbol_values(struct io_mgr *hio_mgr)
+{
+	struct shm_symbol_val *s;
+	struct cod_manager *cod_man;
+	int status;
+
+	s = kzalloc(sizeof(*s), GFP_KERNEL);
+	if (!s)
+		return ERR_PTR(-ENOMEM);
+
+	status = dev_get_cod_mgr(hio_mgr->dev_obj, &cod_man);
+	if (status)
+		goto free_symbol;
+
+	/* Get start and length of channel part of shared memory */
+	status = cod_get_sym_value(cod_man, CHNL_SHARED_BUFFER_BASE_SYM,
+				   &s->shm_base);
+	if (status)
+		goto free_symbol;
+
+	status = cod_get_sym_value(cod_man, CHNL_SHARED_BUFFER_LIMIT_SYM,
+				   &s->shm_lim);
+	if (status)
+		goto free_symbol;
+
+	if (s->shm_lim <= s->shm_base) {
+		status = -EINVAL;
+		goto free_symbol;
+	}
+
+	/* Get start and length of message part of shared memory */
+	status = cod_get_sym_value(cod_man, MSG_SHARED_BUFFER_BASE_SYM,
+				   &s->msg_base);
+	if (status)
+		goto free_symbol;
+
+	status = cod_get_sym_value(cod_man, MSG_SHARED_BUFFER_LIMIT_SYM,
+				   &s->msg_lim);
+	if (status)
+		goto free_symbol;
+
+	if (s->msg_lim <= s->msg_base) {
+		status = -EINVAL;
+		goto free_symbol;
+	}
+
+#ifdef CONFIG_TIDSPBRIDGE_BACKTRACE
+	status = cod_get_sym_value(cod_man, DSP_TRACESEC_END, &s->shm0_end);
+#else
+	status = cod_get_sym_value(cod_man, SHM0_SHARED_END_SYM, &s->shm0_end);
+#endif
+	if (status)
+		goto free_symbol;
+
+	status = cod_get_sym_value(cod_man, DYNEXTBASE, &s->dyn_ext);
+	if (status)
+		goto free_symbol;
+
+	status = cod_get_sym_value(cod_man, EXTEND, &s->ext_end);
+	if (status)
+		goto free_symbol;
+
+	return s;
+
+free_symbol:
+	kfree(s);
+	return ERR_PTR(status);
+}
+
 /*
  *  ======== bridge_io_on_loaded ========
  *  Purpose:
@@ -265,193 +344,112 @@ int bridge_io_destroy(struct io_mgr *hio_mgr)
  */
 int bridge_io_on_loaded(struct io_mgr *hio_mgr)
 {
+	struct bridge_dev_context *dc = hio_mgr->bridge_context;
+	struct cfg_hostres *cfg_res = dc->resources;
+	struct bridge_ioctl_extproc *eproc;
 	struct cod_manager *cod_man;
 	struct chnl_mgr *hchnl_mgr;
 	struct msg_mgr *hmsg_mgr;
-	u32 ul_shm_base;
-	u32 ul_shm_base_offset;
-	u32 ul_shm_limit;
-	u32 ul_shm_length = -1;
-	u32 ul_mem_length = -1;
-	u32 ul_msg_base;
-	u32 ul_msg_limit;
-	u32 ul_msg_length = -1;
-	u32 ul_ext_end;
-	u32 ul_gpp_pa = 0;
-	u32 ul_gpp_va = 0;
-	u32 ul_dsp_va = 0;
-	u32 ul_seg_size = 0;
-	u32 ul_pad_size = 0;
+	struct shm_symbol_val *s;
+	int status;
+	u8 num_procs;
+	s32 ndx;
 	u32 i;
-	int status = 0;
-	u8 num_procs = 0;
-	s32 ndx = 0;
-	/* DSP MMU setup table */
-	struct bridge_ioctl_extproc ae_proc[BRDIOCTL_NUMOFMMUTLB];
-	struct cfg_hostres *host_res;
-	struct bridge_dev_context *pbridge_context;
-	u32 map_attrs;
-	u32 shm0_end;
-	u32 ul_dyn_ext_base;
-	u32 ul_seg1_size = 0;
-	u32 pa_curr = 0;
-	u32 va_curr = 0;
-	u32 gpp_va_curr = 0;
-	u32 num_bytes = 0;
+	u32 mem_sz, msg_sz, pad_sz, shm_sz, shm_base_offs;
+	u32 seg0_sz, seg1_sz;
+	u32 pa, va, da;
+	u32 pa_curr, va_curr, da_curr;
+	u32 bytes;
 	u32 all_bits = 0;
-	u32 page_size[] = { HW_PAGE_SIZE16MB, HW_PAGE_SIZE1MB,
+	u32 page_size[] = {
+		HW_PAGE_SIZE16MB, HW_PAGE_SIZE1MB,
 		HW_PAGE_SIZE64KB, HW_PAGE_SIZE4KB
 	};
+	u32 map_attrs = DSP_MAPLITTLEENDIAN | DSP_MAPPHYSICALADDR |
+			DSP_MAPELEMSIZE32 | DSP_MAPDONOTLOCK;
 
-	status = dev_get_bridge_context(hio_mgr->dev_obj, &pbridge_context);
-	if (!pbridge_context) {
-		status = -EFAULT;
-		goto func_end;
-	}
-
-	host_res = pbridge_context->resources;
-	if (!host_res) {
-		status = -EFAULT;
-		goto func_end;
-	}
 	status = dev_get_cod_mgr(hio_mgr->dev_obj, &cod_man);
-	if (!cod_man) {
-		status = -EFAULT;
-		goto func_end;
-	}
+	if (status)
+		return status;
+
 	hchnl_mgr = hio_mgr->chnl_mgr;
-	/* The message manager is destroyed when the board is stopped. */
+
+	/* The message manager is destroyed when the board is stopped */
 	dev_get_msg_mgr(hio_mgr->dev_obj, &hio_mgr->msg_mgr);
 	hmsg_mgr = hio_mgr->msg_mgr;
-	if (!hchnl_mgr || !hmsg_mgr) {
-		status = -EFAULT;
-		goto func_end;
-	}
+	if (!hchnl_mgr || !hmsg_mgr)
+		return -EFAULT;
+
 	if (hio_mgr->shared_mem)
 		hio_mgr->shared_mem = NULL;
 
-	/* Get start and length of channel part of shared memory */
-	status = cod_get_sym_value(cod_man, CHNL_SHARED_BUFFER_BASE_SYM,
-				   &ul_shm_base);
-	if (status) {
-		status = -EFAULT;
-		goto func_end;
-	}
-	status = cod_get_sym_value(cod_man, CHNL_SHARED_BUFFER_LIMIT_SYM,
-				   &ul_shm_limit);
-	if (status) {
-		status = -EFAULT;
-		goto func_end;
-	}
-	if (ul_shm_limit <= ul_shm_base) {
-		status = -EINVAL;
-		goto func_end;
-	}
+	s = _get_shm_symbol_values(hio_mgr);
+	if (IS_ERR(s))
+		return PTR_ERR(s);
+
 	/* Get total length in bytes */
-	ul_shm_length = (ul_shm_limit - ul_shm_base + 1) * hio_mgr->word_size;
+	shm_sz = (s->shm_lim - s->shm_base + 1) * hio_mgr->word_size;
+
 	/* Calculate size of a PROCCOPY shared memory region */
 	dev_dbg(bridge, "%s: (proc)proccopy shmmem size: 0x%x bytes\n",
-		__func__, (ul_shm_length - sizeof(struct shm)));
+		__func__, shm_sz - sizeof(struct shm));
 
-	/* Get start and length of message part of shared memory */
-	status = cod_get_sym_value(cod_man, MSG_SHARED_BUFFER_BASE_SYM,
-					   &ul_msg_base);
-	if (!status) {
-		status = cod_get_sym_value(cod_man, MSG_SHARED_BUFFER_LIMIT_SYM,
-					   &ul_msg_limit);
-		if (!status) {
-			if (ul_msg_limit <= ul_msg_base) {
-				status = -EINVAL;
-			} else {
-				/*
-				 * Length (bytes) of messaging part of shared
-				 * memory.
-				 */
-				ul_msg_length =
-				    (ul_msg_limit - ul_msg_base +
-				     1) * hio_mgr->word_size;
-				/*
-				 * Total length (bytes) of shared memory:
-				 * chnl + msg.
-				 */
-				ul_mem_length = ul_shm_length + ul_msg_length;
-			}
-		} else {
-			status = -EFAULT;
-		}
-	} else {
-		status = -EFAULT;
-	}
-	if (!status) {
-#if defined(CONFIG_TIDSPBRIDGE_BACKTRACE)
-		status =
-		    cod_get_sym_value(cod_man, DSP_TRACESEC_END, &shm0_end);
-#else
-		status = cod_get_sym_value(cod_man, SHM0_SHARED_END_SYM,
-					   &shm0_end);
-#endif
-		if (status)
-			status = -EFAULT;
-	}
-	if (!status) {
-		status =
-		    cod_get_sym_value(cod_man, DYNEXTBASE, &ul_dyn_ext_base);
-		if (status)
-			status = -EFAULT;
-	}
-	if (!status) {
-		status = cod_get_sym_value(cod_man, EXTEND, &ul_ext_end);
-		if (status)
-			status = -EFAULT;
+	/* Length (bytes) of messaging part of shared memory */
+	msg_sz = (s->msg_lim - s->msg_base + 1) * hio_mgr->word_size;
+
+	/* Total length (bytes) of shared memory: chnl + msg */
+	mem_sz = shm_sz + msg_sz;
+
+	/* Get memory reserved in host resources */
+	(void)mgr_enum_processor_info(0,
+				      (struct dsp_processorinfo *)
+						&hio_mgr->ext_proc_info,
+				      sizeof(struct mgr_processorextinfo),
+				      &num_procs);
+
+	/* IO supports only one DSP for now */
+	if (num_procs != 1) {
+		status = -EINVAL;
+		goto free_symbol;
 	}
-	if (!status) {
-		/* Get memory reserved in host resources */
-		(void)mgr_enum_processor_info(0, (struct dsp_processorinfo *)
-					      &hio_mgr->ext_proc_info,
-					      sizeof(struct
-						     mgr_processorextinfo),
-					      &num_procs);
-
-		/* The first MMU TLB entry(TLB_0) in DCD is ShmBase. */
-		ndx = 0;
-		ul_gpp_pa = host_res->mem_phys[1];
-		ul_gpp_va = host_res->mem_base[1];
-		/* This is the virtual uncached ioremapped address!!! */
-		/* Why can't we directly take the DSPVA from the symbols? */
-		ul_dsp_va = hio_mgr->ext_proc_info.ty_tlb[0].dsp_virt;
-		ul_seg_size = (shm0_end - ul_dsp_va) * hio_mgr->word_size;
-		ul_seg1_size =
-		    (ul_ext_end - ul_dyn_ext_base) * hio_mgr->word_size;
-		/* 4K align */
-		ul_seg1_size = (ul_seg1_size + 0xFFF) & (~0xFFFUL);
-		/* 64K align */
-		ul_seg_size = (ul_seg_size + 0xFFFF) & (~0xFFFFUL);
-		ul_pad_size = UL_PAGE_ALIGN_SIZE - ((ul_gpp_pa + ul_seg1_size) %
-						    UL_PAGE_ALIGN_SIZE);
-		if (ul_pad_size == UL_PAGE_ALIGN_SIZE)
-			ul_pad_size = 0x0;
-
-		dev_dbg(bridge, "%s: ul_gpp_pa %x, ul_gpp_va %x, ul_dsp_va %x, "
-			"shm0_end %x, ul_dyn_ext_base %x, ul_ext_end %x, "
-			"ul_seg_size %x ul_seg1_size %x \n", __func__,
-			ul_gpp_pa, ul_gpp_va, ul_dsp_va, shm0_end,
-			ul_dyn_ext_base, ul_ext_end, ul_seg_size, ul_seg1_size);
-
-		if ((ul_seg_size + ul_seg1_size + ul_pad_size) >
-		    host_res->mem_length[1]) {
-			pr_err("%s: shm Error, reserved 0x%x required 0x%x\n",
-			       __func__, host_res->mem_length[1],
-			       ul_seg_size + ul_seg1_size + ul_pad_size);
-			status = -ENOMEM;
-		}
+
+	/* The first MMU TLB entry(TLB_0) in DCD is ShmBase */
+	pa = cfg_res->mem_phys[1];
+	va = cfg_res->mem_base[1];
+
+	/* This is the virtual uncached ioremapped address!!! */
+	/* Why can't we directly take the DSPVA from the symbols? */
+	da = hio_mgr->ext_proc_info.ty_tlb[0].dsp_virt;
+	seg0_sz = (s->shm0_end - da) * hio_mgr->word_size;
+	seg1_sz = (s->ext_end - s->dyn_ext) * hio_mgr->word_size;
+
+	/* 4K align */
+	seg1_sz = (seg1_sz + 0xFFF) & (~0xFFFUL);
+
+	/* 64K align */
+	seg0_sz = (seg0_sz + 0xFFFF) & (~0xFFFFUL);
+
+	pad_sz = UL_PAGE_ALIGN_SIZE - ((pa + seg1_sz) % UL_PAGE_ALIGN_SIZE);
+	if (pad_sz == UL_PAGE_ALIGN_SIZE)
+		pad_sz = 0x0;
+
+	dev_dbg(bridge, "%s: pa %x, va %x, da %x\n", __func__, pa, va, da);
+	dev_dbg(bridge,
+		"shm0_end %x, dyn_ext %x, ext_end %x, seg0_sz %x seg1_sz %x\n",
+		s->shm0_end, s->dyn_ext, s->ext_end, seg0_sz, seg1_sz);
+
+	if ((seg0_sz + seg1_sz + pad_sz) > cfg_res->mem_length[1]) {
+		pr_err("%s: shm Error, reserved 0x%x required 0x%x\n",
+		       __func__, cfg_res->mem_length[1],
+		       seg0_sz + seg1_sz + pad_sz);
+		status = -ENOMEM;
+		goto free_symbol;
 	}
-	if (status)
-		goto func_end;
 
-	pa_curr = ul_gpp_pa;
-	va_curr = ul_dyn_ext_base * hio_mgr->word_size;
-	gpp_va_curr = ul_gpp_va;
-	num_bytes = ul_seg1_size;
+	pa_curr = pa;
+	va_curr = s->dyn_ext * hio_mgr->word_size;
+	da_curr = va;
+	bytes = seg1_sz;
 
 	/*
 	 * Try to fit into TLB entries. If not possible, push them to page
@@ -459,37 +457,30 @@ int bridge_io_on_loaded(struct io_mgr *hio_mgr)
 	 * bigger page boundary, we may end up making several small pages.
 	 * So, push them onto page tables, if that is the case.
 	 */
-	map_attrs = 0x00000000;
-	map_attrs = DSP_MAPLITTLEENDIAN;
-	map_attrs |= DSP_MAPPHYSICALADDR;
-	map_attrs |= DSP_MAPELEMSIZE32;
-	map_attrs |= DSP_MAPDONOTLOCK;
-
-	while (num_bytes) {
+	while (bytes) {
 		/*
 		 * To find the max. page size with which both PA & VA are
 		 * aligned.
 		 */
 		all_bits = pa_curr | va_curr;
-		dev_dbg(bridge, "all_bits %x, pa_curr %x, va_curr %x, "
-			"num_bytes %x\n", all_bits, pa_curr, va_curr,
-			num_bytes);
+		dev_dbg(bridge,
+			"seg all_bits %x, pa_curr %x, va_curr %x, bytes %x\n",
+			all_bits, pa_curr, va_curr, bytes);
+
 		for (i = 0; i < 4; i++) {
-			if ((num_bytes >= page_size[i]) && ((all_bits &
-							     (page_size[i] -
-							      1)) == 0)) {
-				status =
-				    hio_mgr->intf_fxns->
-				    brd_mem_map(hio_mgr->bridge_context,
-						    pa_curr, va_curr,
-						    page_size[i], map_attrs,
-						    NULL);
+			if ((bytes >= page_size[i]) &&
+			    ((all_bits & (page_size[i] - 1)) == 0)) {
+				status = hio_mgr->intf_fxns->brd_mem_map(dc,
+							pa_curr, va_curr,
+							page_size[i], map_attrs,
+							NULL);
 				if (status)
-					goto func_end;
+					goto free_symbol;
+
 				pa_curr += page_size[i];
 				va_curr += page_size[i];
-				gpp_va_curr += page_size[i];
-				num_bytes -= page_size[i];
+				da_curr += page_size[i];
+				bytes -= page_size[i];
 				/*
 				 * Don't try smaller sizes. Hopefully we have
 				 * reached an address aligned to a bigger page
@@ -499,71 +490,75 @@ int bridge_io_on_loaded(struct io_mgr *hio_mgr)
 			}
 		}
 	}
-	pa_curr += ul_pad_size;
-	va_curr += ul_pad_size;
-	gpp_va_curr += ul_pad_size;
 
+	pa_curr += pad_sz;
+	va_curr += pad_sz;
+	da_curr += pad_sz;
+	bytes = seg0_sz;
+	va_curr = da * hio_mgr->word_size;
+
+	eproc = kzalloc(sizeof(*eproc) * BRDIOCTL_NUMOFMMUTLB, GFP_KERNEL);
+	if (!eproc) {
+		status = -ENOMEM;
+		goto free_symbol;
+	}
+
+	ndx = 0;
 	/* Configure the TLB entries for the next cacheable segment */
-	num_bytes = ul_seg_size;
-	va_curr = ul_dsp_va * hio_mgr->word_size;
-	while (num_bytes) {
+	while (bytes) {
 		/*
 		 * To find the max. page size with which both PA & VA are
 		 * aligned.
 		 */
 		all_bits = pa_curr | va_curr;
-		dev_dbg(bridge, "all_bits for Seg1 %x, pa_curr %x, "
-			"va_curr %x, num_bytes %x\n", all_bits, pa_curr,
-			va_curr, num_bytes);
+		dev_dbg(bridge,
+			"seg1 all_bits %x, pa_curr %x, va_curr %x, bytes %x\n",
+			all_bits, pa_curr, va_curr, bytes);
+
 		for (i = 0; i < 4; i++) {
-			if (!(num_bytes >= page_size[i]) ||
+			if (!(bytes >= page_size[i]) ||
 			    !((all_bits & (page_size[i] - 1)) == 0))
 				continue;
-			if (ndx < MAX_LOCK_TLB_ENTRIES) {
-				/*
-				 * This is the physical address written to
-				 * DSP MMU.
-				 */
-				ae_proc[ndx].gpp_pa = pa_curr;
-				/*
-				 * This is the virtual uncached ioremapped
-				 * address!!!
-				 */
-				ae_proc[ndx].gpp_va = gpp_va_curr;
-				ae_proc[ndx].dsp_va =
-				    va_curr / hio_mgr->word_size;
-				ae_proc[ndx].size = page_size[i];
-				ae_proc[ndx].endianism = HW_LITTLE_ENDIAN;
-				ae_proc[ndx].elem_size = HW_ELEM_SIZE16BIT;
-				ae_proc[ndx].mixed_mode = HW_MMU_CPUES;
-				dev_dbg(bridge, "shm MMU TLB entry PA %x"
-					" VA %x DSP_VA %x Size %x\n",
-					ae_proc[ndx].gpp_pa,
-					ae_proc[ndx].gpp_va,
-					ae_proc[ndx].dsp_va *
-					hio_mgr->word_size, page_size[i]);
-				ndx++;
-			} else {
-				status =
-				    hio_mgr->intf_fxns->
-				    brd_mem_map(hio_mgr->bridge_context,
-						    pa_curr, va_curr,
-						    page_size[i], map_attrs,
-						    NULL);
+
+			if (ndx >= MAX_LOCK_TLB_ENTRIES) {
+				status = hio_mgr->intf_fxns->brd_mem_map(dc,
+							pa_curr, va_curr,
+							page_size[i], map_attrs,
+							NULL);
 				dev_dbg(bridge,
-					"shm MMU PTE entry PA %x"
-					" VA %x DSP_VA %x Size %x\n",
-					ae_proc[ndx].gpp_pa,
-					ae_proc[ndx].gpp_va,
-					ae_proc[ndx].dsp_va *
+					"PTE pa %x va %x dsp_va %x sz %x\n",
+					eproc[ndx].gpp_pa,
+					eproc[ndx].gpp_va,
+					eproc[ndx].dsp_va *
 					hio_mgr->word_size, page_size[i]);
 				if (status)
-					goto func_end;
+					goto free_eproc;
 			}
+
+			/* This is the physical address written to DSP MMU */
+			eproc[ndx].gpp_pa = pa_curr;
+
+			/*
+			 * This is the virtual uncached ioremapped
+			 * address!!!
+			 */
+			eproc[ndx].gpp_va = da_curr;
+			eproc[ndx].dsp_va = va_curr / hio_mgr->word_size;
+			eproc[ndx].size = page_size[i];
+			eproc[ndx].endianism = HW_LITTLE_ENDIAN;
+			eproc[ndx].elem_size = HW_ELEM_SIZE16BIT;
+			eproc[ndx].mixed_mode = HW_MMU_CPUES;
+			dev_dbg(bridge, "%s: tlb pa %x va %x dsp_va %x sz %x\n",
+				__func__, eproc[ndx].gpp_pa,
+				eproc[ndx].gpp_va,
+				eproc[ndx].dsp_va * hio_mgr->word_size,
+				page_size[i]);
+			ndx++;
+
 			pa_curr += page_size[i];
 			va_curr += page_size[i];
-			gpp_va_curr += page_size[i];
-			num_bytes -= page_size[i];
+			da_curr += page_size[i];
+			bytes -= page_size[i];
 			/*
 			 * Don't try smaller sizes. Hopefully we have reached
 			 * an address aligned to a bigger page size.
@@ -577,146 +572,127 @@ int bridge_io_on_loaded(struct io_mgr *hio_mgr)
 	 * should not conflict with shm entries on MPU or DSP side.
 	 */
 	for (i = 3; i < 7 && ndx < BRDIOCTL_NUMOFMMUTLB; i++) {
-		if (hio_mgr->ext_proc_info.ty_tlb[i].gpp_phys == 0)
+		struct mgr_processorextinfo *ep = &hio_mgr->ext_proc_info;
+		u32 word_sz = hio_mgr->word_size;
+
+		if (ep->ty_tlb[i].gpp_phys == 0)
 			continue;
 
-		if ((hio_mgr->ext_proc_info.ty_tlb[i].gpp_phys >
-		     ul_gpp_pa - 0x100000
-		     && hio_mgr->ext_proc_info.ty_tlb[i].gpp_phys <=
-		     ul_gpp_pa + ul_seg_size)
-		    || (hio_mgr->ext_proc_info.ty_tlb[i].dsp_virt >
-			ul_dsp_va - 0x100000 / hio_mgr->word_size
-			&& hio_mgr->ext_proc_info.ty_tlb[i].dsp_virt <=
-			ul_dsp_va + ul_seg_size / hio_mgr->word_size)) {
+		if ((ep->ty_tlb[i].gpp_phys > pa - 0x100000 &&
+		     ep->ty_tlb[i].gpp_phys <= pa + seg0_sz) ||
+		    (ep->ty_tlb[i].dsp_virt > da - 0x100000 / word_sz &&
+		     ep->ty_tlb[i].dsp_virt <= da + seg0_sz / word_sz)) {
 			dev_dbg(bridge,
-				"CDB MMU entry %d conflicts with "
-				"shm.\n\tCDB: GppPa %x, DspVa %x.\n\tSHM: "
-				"GppPa %x, DspVa %x, Bytes %x.\n", i,
-				hio_mgr->ext_proc_info.ty_tlb[i].gpp_phys,
-				hio_mgr->ext_proc_info.ty_tlb[i].dsp_virt,
-				ul_gpp_pa, ul_dsp_va, ul_seg_size);
+				"err cdb%d pa %x da %x shm pa %x da %x sz %x\n",
+				i, ep->ty_tlb[i].gpp_phys,
+				ep->ty_tlb[i].dsp_virt, pa, da, seg0_sz);
 			status = -EPERM;
-		} else {
-			if (ndx < MAX_LOCK_TLB_ENTRIES) {
-				ae_proc[ndx].dsp_va =
-				    hio_mgr->ext_proc_info.ty_tlb[i].
-				    dsp_virt;
-				ae_proc[ndx].gpp_pa =
-				    hio_mgr->ext_proc_info.ty_tlb[i].
-				    gpp_phys;
-				ae_proc[ndx].gpp_va = 0;
-				/* 1 MB */
-				ae_proc[ndx].size = 0x100000;
-				dev_dbg(bridge, "shm MMU entry PA %x "
-					"DSP_VA 0x%x\n", ae_proc[ndx].gpp_pa,
-					ae_proc[ndx].dsp_va);
-				ndx++;
-			} else {
-				status = hio_mgr->intf_fxns->brd_mem_map
-				    (hio_mgr->bridge_context,
-				     hio_mgr->ext_proc_info.ty_tlb[i].
-				     gpp_phys,
-				     hio_mgr->ext_proc_info.ty_tlb[i].
-				     dsp_virt, 0x100000, map_attrs,
-				     NULL);
-			}
+			goto free_eproc;
+		}
+
+		if (ndx >= MAX_LOCK_TLB_ENTRIES) {
+			status = hio_mgr->intf_fxns->brd_mem_map(dc,
+						ep->ty_tlb[i].gpp_phys,
+						ep->ty_tlb[i].dsp_virt,
+						0x100000, map_attrs, NULL);
+			if (status)
+				goto free_eproc;
 		}
-		if (status)
-			goto func_end;
-	}
 
-	map_attrs = 0x00000000;
-	map_attrs = DSP_MAPLITTLEENDIAN;
-	map_attrs |= DSP_MAPPHYSICALADDR;
-	map_attrs |= DSP_MAPELEMSIZE32;
-	map_attrs |= DSP_MAPDONOTLOCK;
+		eproc[ndx].dsp_va = ep->ty_tlb[i].dsp_virt;
+		eproc[ndx].gpp_pa = ep->ty_tlb[i].gpp_phys;
+		eproc[ndx].gpp_va = 0;
+
+		/* 1 MB */
+		eproc[ndx].size = 0x100000;
+		dev_dbg(bridge, "shm MMU entry pa %x da 0x%x\n",
+			eproc[ndx].gpp_pa, eproc[ndx].dsp_va);
+		ndx++;
+	}
 
 	/* Map the L4 peripherals */
 	i = 0;
 	while (l4_peripheral_table[i].phys_addr) {
-		status = hio_mgr->intf_fxns->brd_mem_map
-		    (hio_mgr->bridge_context, l4_peripheral_table[i].phys_addr,
-		     l4_peripheral_table[i].dsp_virt_addr, HW_PAGE_SIZE4KB,
-		     map_attrs, NULL);
+		status = hio_mgr->intf_fxns->brd_mem_map(dc,
+					l4_peripheral_table[i].phys_addr,
+					l4_peripheral_table[i].dsp_virt_addr,
+					HW_PAGE_SIZE4KB, map_attrs, NULL);
 		if (status)
-			goto func_end;
+			goto free_eproc;
 		i++;
 	}
 
 	for (i = ndx; i < BRDIOCTL_NUMOFMMUTLB; i++) {
-		ae_proc[i].dsp_va = 0;
-		ae_proc[i].gpp_pa = 0;
-		ae_proc[i].gpp_va = 0;
-		ae_proc[i].size = 0;
+		eproc[i].dsp_va = 0;
+		eproc[i].gpp_pa = 0;
+		eproc[i].gpp_va = 0;
+		eproc[i].size = 0;
 	}
+
 	/*
 	 * Set the shm physical address entry (grayed out in CDB file)
 	 * to the virtual uncached ioremapped address of shm reserved
 	 * on MPU.
 	 */
 	hio_mgr->ext_proc_info.ty_tlb[0].gpp_phys =
-	    (ul_gpp_va + ul_seg1_size + ul_pad_size);
+		(va + seg1_sz + pad_sz);
 
 	/*
 	 * Need shm Phys addr. IO supports only one DSP for now:
 	 * num_procs = 1.
 	 */
-	if (!hio_mgr->ext_proc_info.ty_tlb[0].gpp_phys || num_procs != 1) {
-		status = -EFAULT;
-		goto func_end;
-	} else {
-		if (ae_proc[0].dsp_va > ul_shm_base) {
-			status = -EPERM;
-			goto func_end;
-		}
-		/* ul_shm_base may not be at ul_dsp_va address */
-		ul_shm_base_offset = (ul_shm_base - ae_proc[0].dsp_va) *
+	if (!hio_mgr->ext_proc_info.ty_tlb[0].gpp_phys)
+		return -EFAULT;
+
+	if (eproc[0].dsp_va > s->shm_base)
+		return -EPERM;
+
+	/* shm_base may not be at ul_dsp_va address */
+	shm_base_offs = (s->shm_base - eproc[0].dsp_va) *
 		    hio_mgr->word_size;
-		/*
-		 * bridge_dev_ctrl() will set dev context dsp-mmu info. In
-		 * bridge_brd_start() the MMU will be re-programed with MMU
-		 * DSPVa-GPPPa pair info while DSP is in a known
-		 * (reset) state.
-		 */
+	/*
+	 * bridge_dev_ctrl() will set dev context dsp-mmu info. In
+	 * bridge_brd_start() the MMU will be re-programed with MMU
+	 * DSPVa-GPPPa pair info while DSP is in a known
+	 * (reset) state.
+	 */
+	status = hio_mgr->intf_fxns->dev_cntrl(hio_mgr->bridge_context,
+					       BRDIOCTL_SETMMUCONFIG, eproc);
+	if (status)
+		goto free_eproc;
 
-		status =
-		    hio_mgr->intf_fxns->dev_cntrl(hio_mgr->bridge_context,
-						      BRDIOCTL_SETMMUCONFIG,
-						      ae_proc);
-		if (status)
-			goto func_end;
-		ul_shm_base = hio_mgr->ext_proc_info.ty_tlb[0].gpp_phys;
-		ul_shm_base += ul_shm_base_offset;
-		ul_shm_base = (u32) MEM_LINEAR_ADDRESS((void *)ul_shm_base,
-						       ul_mem_length);
-		if (ul_shm_base == 0) {
-			status = -EFAULT;
-			goto func_end;
-		}
-		/* Register SM */
-		status =
-		    register_shm_segs(hio_mgr, cod_man, ae_proc[0].gpp_pa);
+	s->shm_base = hio_mgr->ext_proc_info.ty_tlb[0].gpp_phys;
+	s->shm_base += shm_base_offs;
+	s->shm_base = (u32) MEM_LINEAR_ADDRESS((void *)s->shm_base,
+					       mem_sz);
+	if (!s->shm_base) {
+		status = -EFAULT;
+		goto free_eproc;
 	}
 
-	hio_mgr->shared_mem = (struct shm *)ul_shm_base;
+	/* Register SM */
+	status = register_shm_segs(hio_mgr, cod_man, eproc[0].gpp_pa);
+
+	hio_mgr->shared_mem = (struct shm *)s->shm_base;
 	hio_mgr->input = (u8 *) hio_mgr->shared_mem + sizeof(struct shm);
-	hio_mgr->output = hio_mgr->input + (ul_shm_length -
+	hio_mgr->output = hio_mgr->input + (shm_sz -
 					    sizeof(struct shm)) / 2;
 	hio_mgr->sm_buf_size = hio_mgr->output - hio_mgr->input;
 
-	/*  Set up Shared memory addresses for messaging. */
-	hio_mgr->msg_input_ctrl = (struct msg_ctrl *)((u8 *) hio_mgr->shared_mem
-						      + ul_shm_length);
+	/*  Set up Shared memory addresses for messaging */
+	hio_mgr->msg_input_ctrl =
+		(struct msg_ctrl *)((u8 *) hio_mgr->shared_mem + shm_sz);
 	hio_mgr->msg_input =
-	    (u8 *) hio_mgr->msg_input_ctrl + sizeof(struct msg_ctrl);
+		(u8 *) hio_mgr->msg_input_ctrl + sizeof(struct msg_ctrl);
 	hio_mgr->msg_output_ctrl =
-	    (struct msg_ctrl *)((u8 *) hio_mgr->msg_input_ctrl +
-				ul_msg_length / 2);
+		(struct msg_ctrl *)((u8 *) hio_mgr->msg_input_ctrl +
+					   msg_sz / 2);
 	hio_mgr->msg_output =
-	    (u8 *) hio_mgr->msg_output_ctrl + sizeof(struct msg_ctrl);
+		(u8 *) hio_mgr->msg_output_ctrl + sizeof(struct msg_ctrl);
 	hmsg_mgr->max_msgs =
-	    ((u8 *) hio_mgr->msg_output_ctrl - hio_mgr->msg_input)
-	    / sizeof(struct msg_dspmsg);
+		((u8 *) hio_mgr->msg_output_ctrl - hio_mgr->msg_input) /
+		sizeof(struct msg_dspmsg);
+
 	dev_dbg(bridge, "IO MGR shm details: shared_mem %p, input %p, "
 		"output %p, msg_input_ctrl %p, msg_input %p, "
 		"msg_output_ctrl %p, msg_output %p\n",
@@ -732,47 +708,53 @@ int bridge_io_on_loaded(struct io_mgr *hio_mgr)
 	/* Get the start address of trace buffer */
 	status = cod_get_sym_value(cod_man, SYS_PUTCBEG,
 				   &hio_mgr->trace_buffer_begin);
-	if (status) {
-		status = -EFAULT;
-		goto func_end;
-	}
+	if (status)
+		goto free_eproc;
+
+	hio_mgr->gpp_read_pointer =
+		hio_mgr->trace_buffer_begin =
+			(va + seg1_sz + pad_sz) +
+			(hio_mgr->trace_buffer_begin - da);
 
-	hio_mgr->gpp_read_pointer = hio_mgr->trace_buffer_begin =
-	    (ul_gpp_va + ul_seg1_size + ul_pad_size) +
-	    (hio_mgr->trace_buffer_begin - ul_dsp_va);
 	/* Get the end address of trace buffer */
 	status = cod_get_sym_value(cod_man, SYS_PUTCEND,
 				   &hio_mgr->trace_buffer_end);
-	if (status) {
-		status = -EFAULT;
-		goto func_end;
-	}
+	if (status)
+		goto free_eproc;
+
 	hio_mgr->trace_buffer_end =
-	    (ul_gpp_va + ul_seg1_size + ul_pad_size) +
-	    (hio_mgr->trace_buffer_end - ul_dsp_va);
+		(va + seg1_sz + pad_sz) +
+		(hio_mgr->trace_buffer_end - da);
+
 	/* Get the current address of DSP write pointer */
 	status = cod_get_sym_value(cod_man, BRIDGE_SYS_PUTC_CURRENT,
 				   &hio_mgr->trace_buffer_current);
-	if (status) {
-		status = -EFAULT;
-		goto func_end;
-	}
+	if (status)
+		goto free_eproc;
+
 	hio_mgr->trace_buffer_current =
-	    (ul_gpp_va + ul_seg1_size + ul_pad_size) +
-	    (hio_mgr->trace_buffer_current - ul_dsp_va);
+		(va + seg1_sz + pad_sz) +
+		(hio_mgr->trace_buffer_current - da);
+
 	/* Calculate the size of trace buffer */
 	kfree(hio_mgr->msg);
 	hio_mgr->msg = kmalloc(((hio_mgr->trace_buffer_end -
 				hio_mgr->trace_buffer_begin) *
 				hio_mgr->word_size) + 2, GFP_KERNEL);
-	if (!hio_mgr->msg)
+	if (!hio_mgr->msg) {
 		status = -ENOMEM;
+		goto free_eproc;
+	}
 
-	hio_mgr->dsp_va = ul_dsp_va;
-	hio_mgr->gpp_va = (ul_gpp_va + ul_seg1_size + ul_pad_size);
-
+	hio_mgr->dsp_va = da;
+	hio_mgr->gpp_va = (va + seg1_sz + pad_sz);
 #endif
-func_end:
+
+free_eproc:
+	kfree(eproc);
+free_symbol:
+	kfree(s);
+
 	return status;
 }
 
diff --git a/drivers/staging/tidspbridge/core/tiomap3430.c b/drivers/staging/tidspbridge/core/tiomap3430.c
index 9cf29fcea11..f9609ce2c16 100644
--- a/drivers/staging/tidspbridge/core/tiomap3430.c
+++ b/drivers/staging/tidspbridge/core/tiomap3430.c
@@ -1547,20 +1547,27 @@ EXIT_LOOP:
 static u32 user_va2_pa(struct mm_struct *mm, u32 address)
 {
 	pgd_t *pgd;
+	pud_t *pud;
 	pmd_t *pmd;
 	pte_t *ptep, pte;
 
 	pgd = pgd_offset(mm, address);
-	if (!(pgd_none(*pgd) || pgd_bad(*pgd))) {
-		pmd = pmd_offset(pgd, address);
-		if (!(pmd_none(*pmd) || pmd_bad(*pmd))) {
-			ptep = pte_offset_map(pmd, address);
-			if (ptep) {
-				pte = *ptep;
-				if (pte_present(pte))
-					return pte & PAGE_MASK;
-			}
-		}
+	if (pgd_none(*pgd) || pgd_bad(*pgd))
+		return 0;
+
+	pud = pud_offset(pgd, address);
+	if (pud_none(*pud) || pud_bad(*pud))
+		return 0;
+
+	pmd = pmd_offset(pud, address);
+	if (pmd_none(*pmd) || pmd_bad(*pmd))
+		return 0;
+
+	ptep = pte_offset_map(pmd, address);
+	if (ptep) {
+		pte = *ptep;
+		if (pte_present(pte))
+			return pte & PAGE_MASK;
 	}
 
 	return 0;
diff --git a/drivers/staging/tidspbridge/dynload/cload.c b/drivers/staging/tidspbridge/dynload/cload.c
index fe1ef0addb0..9d54744805b 100644
--- a/drivers/staging/tidspbridge/dynload/cload.c
+++ b/drivers/staging/tidspbridge/dynload/cload.c
@@ -14,6 +14,8 @@
  * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  */
 
+#include <linux/slab.h>
+
 #include "header.h"
 
 #include "module_list.h"
@@ -706,6 +708,7 @@ static void dload_symbols(struct dload_state *dlthis)
 	struct local_symbol *sp;
 	struct dynload_symbol *symp;
 	struct dynload_symbol *newsym;
+	struct doff_syment_t *my_sym_buf;
 
 	sym_count = dlthis->dfile_hdr.df_no_syms;
 	if (sym_count == 0)
@@ -739,13 +742,18 @@ static void dload_symbols(struct dload_state *dlthis)
 	 become defined from the global symbol table */
 	checks = dlthis->verify.dv_sym_tab_checksum;
 	symbols_left = sym_count;
+
+	my_sym_buf = kzalloc(sizeof(*my_sym_buf) * MY_SYM_BUF_SIZ, GFP_KERNEL);
+	if (!my_sym_buf)
+		return;
+
 	do {			/* read all symbols */
 		char *sname;
 		u32 val;
 		s32 delta;
 		struct doff_syment_t *input_sym;
 		unsigned syms_in_buf;
-		struct doff_syment_t my_sym_buf[MY_SYM_BUF_SIZ];
+
 		input_sym = my_sym_buf;
 		syms_in_buf = symbols_left > MY_SYM_BUF_SIZ ?
 		    MY_SYM_BUF_SIZ : symbols_left;
@@ -753,7 +761,7 @@ static void dload_symbols(struct dload_state *dlthis)
 		if (dlthis->strm->read_buffer(dlthis->strm, input_sym, siz) !=
 		    siz) {
 			DL_ERROR(readstrm, sym_errid);
-			return;
+			goto free_sym_buf;
 		}
 		if (dlthis->reorder_map)
 			dload_reorder(input_sym, siz, dlthis->reorder_map);
@@ -856,7 +864,7 @@ static void dload_symbols(struct dload_state *dlthis)
 					DL_ERROR("Absolute symbol %s is "
 						 "defined multiple times with "
 						 "different values", sname);
-					return;
+					goto free_sym_buf;
 				}
 			}
 loop_itr:
@@ -887,6 +895,9 @@ loop_cont:
 	if (~checks)
 		dload_error(dlthis, "Checksum of symbols failed");
 
+free_sym_buf:
+	kfree(my_sym_buf);
+	return;
 }				/* dload_symbols */
 
 /*****************************************************************************
@@ -1116,6 +1127,11 @@ static int relocate_packet(struct dload_state *dlthis,
 /* VERY dangerous */
 static const char imagepak[] = { "image packet" };
 
+struct img_buffer {
+	struct image_packet_t ipacket;
+	u8 bufr[BYTE_TO_HOST(IMAGE_PACKET_SIZE)];
+};
+
 /*************************************************************************
  * Procedure dload_data
  *
@@ -1131,16 +1147,16 @@ static void dload_data(struct dload_state *dlthis)
 	u16 curr_sect;
 	struct doff_scnhdr_t *sptr = dlthis->sect_hdrs;
 	struct ldr_section_info *lptr = dlthis->ldr_sections;
+	struct img_buffer *ibuf;
 	u8 *dest;
 
-	struct {
-		struct image_packet_t ipacket;
-		u8 bufr[BYTE_TO_HOST(IMAGE_PACKET_SIZE)];
-	} ibuf;
-
 	/* Indicates whether CINIT processing has occurred */
 	bool cinit_processed = false;
 
+	ibuf = kzalloc(sizeof(*ibuf), GFP_KERNEL);
+	if (!ibuf)
+		return;
+
 	/* Loop through the sections and load them one at a time.
 	 */
 	for (curr_sect = 0; curr_sect < dlthis->dfile_hdr.df_no_scns;
@@ -1168,37 +1184,37 @@ static void dload_data(struct dload_state *dlthis)
 
 				/* get the fixed header bits */
 				if (dlthis->strm->read_buffer(dlthis->strm,
-							      &ibuf.ipacket,
+							      &ibuf->ipacket,
 							      IPH_SIZE) !=
 				    IPH_SIZE) {
 					DL_ERROR(readstrm, imagepak);
-					return;
+					goto free_ibuf;
 				}
 				/* reorder the header if need be */
 				if (dlthis->reorder_map) {
-					dload_reorder(&ibuf.ipacket, IPH_SIZE,
+					dload_reorder(&ibuf->ipacket, IPH_SIZE,
 						      dlthis->reorder_map);
 				}
 				/* now read the rest of the packet */
 				ipsize =
 				    BYTE_TO_HOST(DOFF_ALIGN
-						 (ibuf.ipacket.packet_size));
+						 (ibuf->ipacket.packet_size));
 				if (ipsize > BYTE_TO_HOST(IMAGE_PACKET_SIZE)) {
 					DL_ERROR("Bad image packet size %d",
 						 ipsize);
-					return;
+					goto free_ibuf;
 				}
-				dest = ibuf.bufr;
+				dest = ibuf->bufr;
 				/* End of determination */
 
 				if (dlthis->strm->read_buffer(dlthis->strm,
-							      ibuf.bufr,
+							      ibuf->bufr,
 							      ipsize) !=
 				    ipsize) {
 					DL_ERROR(readstrm, imagepak);
-					return;
+					goto free_ibuf;
 				}
-				ibuf.ipacket.img_data = dest;
+				ibuf->ipacket.img_data = dest;
 
 				/* reorder the bytes if need be */
 #if !defined(_BIG_ENDIAN) || (TARGET_AU_BITS > 16)
@@ -1225,16 +1241,16 @@ static void dload_data(struct dload_state *dlthis)
 #endif
 #endif
 
-				checks += dload_checksum(&ibuf.ipacket,
+				checks += dload_checksum(&ibuf->ipacket,
 							 IPH_SIZE);
 				/* relocate the image bits as needed */
-				if (ibuf.ipacket.num_relocs) {
+				if (ibuf->ipacket.num_relocs) {
 					dlthis->image_offset = image_offset;
 					if (!relocate_packet(dlthis,
-							     &ibuf.ipacket,
+							     &ibuf->ipacket,
 							     &checks,
 							     &tramp_generated))
-						return;	/* serious error */
+						goto free_ibuf;	/* error */
 				}
 				if (~checks)
 					DL_ERROR(err_checksum, imagepak);
@@ -1249,20 +1265,20 @@ static void dload_data(struct dload_state *dlthis)
 					if (dload_check_type(sptr,
 						DLOAD_CINIT)) {
 						cload_cinit(dlthis,
-							    &ibuf.ipacket);
+							    &ibuf->ipacket);
 						cinit_processed = true;
 					} else {
 						/* FIXME */
 						if (!dlthis->myio->
 						    writemem(dlthis->
 							myio,
-							ibuf.bufr,
+							ibuf->bufr,
 							lptr->
 							load_addr +
 							image_offset,
 							lptr,
 							BYTE_TO_HOST
-							(ibuf.
+							(ibuf->
 							ipacket.
 							packet_size))) {
 							DL_ERROR
@@ -1276,7 +1292,7 @@ static void dload_data(struct dload_state *dlthis)
 					}
 				}
 				image_offset +=
-				    BYTE_TO_TADDR(ibuf.ipacket.packet_size);
+				    BYTE_TO_TADDR(ibuf->ipacket.packet_size);
 			}	/* process packets */
 			/* if this is a BSS section, we may want to fill it */
 			if (!dload_check_type(sptr, DLOAD_BSS))
@@ -1334,6 +1350,9 @@ loop_cont:
 		DL_ERROR("Finalization of auto-trampolines (size = " FMT_UI32
 			 ") failed", dlthis->tramp.tramp_sect_next_addr);
 	}
+free_ibuf:
+	kfree(ibuf);
+	return;
 }				/* dload_data */
 
 /*************************************************************************
diff --git a/drivers/staging/usbip/userspace/src/Makefile.am b/drivers/staging/usbip/userspace/src/Makefile.am
index 3f09f6ad39f..c365a3fada9 100644
--- a/drivers/staging/usbip/userspace/src/Makefile.am
+++ b/drivers/staging/usbip/userspace/src/Makefile.am
@@ -4,8 +4,9 @@ LDADD       = $(top_builddir)/libsrc/libusbip.la @PACKAGE_LIBS@
 
 sbin_PROGRAMS := usbip usbipd
 
-usbip_SOURCES := usbip.c utils.c usbip_network.c \
+usbip_SOURCES := usbip.h utils.h usbip.c utils.c usbip_network.c \
 		 usbip_attach.c usbip_detach.c usbip_list.c \
 		 usbip_bind.c usbip_unbind.c
 
-usbipd_SOURCES := usbipd.c usbip_network.c
+
+usbipd_SOURCES := usbip_network.h usbipd.c usbip_network.c
diff --git a/drivers/staging/usbip/vhci.h b/drivers/staging/usbip/vhci.h
index 88b32981cf1..c66b8b3f97b 100644
--- a/drivers/staging/usbip/vhci.h
+++ b/drivers/staging/usbip/vhci.h
@@ -71,12 +71,7 @@ struct vhci_unlink {
 	unsigned long unlink_seqnum;
 };
 
-/*
- * The number of ports is less than 16 ?
- * USB_MAXCHILDREN is statically defined to 16 in usb.h.  Its maximum value
- * would be 31 because the event_bits[1] of struct usb_hub is defined as
- * unsigned long in hub.h
- */
+/* Number of supported ports. Value has an upperbound of USB_MAXCHILDREN */
 #define VHCI_NPORTS 8
 
 /* for usb_bus.hcpriv */
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index f708cbaee16..12a9a5fbc79 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -178,44 +178,32 @@ void rh_port_disconnect(int rhport)
 	  | USB_PORT_STAT_C_RESET) << 16)
 
 /*
- * This function is almostly the same as dummy_hcd.c:dummy_hub_status() without
- * suspend/resume support. But, it is modified to provide multiple ports.
+ * Returns 0 if the status hasn't changed, or the number of bytes in buf.
+ * Ports are 0-indexed from the HCD point of view,
+ * and 1-indexed from the USB core pointer of view.
  *
  * @buf: a bitmap to show which port status has been changed.
- *  bit  0: reserved or used for another purpose?
+ *  bit  0: reserved
  *  bit  1: the status of port 0 has been changed.
  *  bit  2: the status of port 1 has been changed.
  *  ...
- *  bit  7: the status of port 6 has been changed.
- *  bit  8: the status of port 7 has been changed.
- *  ...
- *  bit 15: the status of port 14 has been changed.
- *
- * So, the maximum number of ports is 31 ( port 0 to port 30) ?
- *
- * The return value is the actual transferred length in byte. If nothing has
- * been changed, return 0. In the case that the number of ports is less than or
- * equal to 6 (VHCI_NPORTS==7), return 1.
- *
  */
 static int vhci_hub_status(struct usb_hcd *hcd, char *buf)
 {
 	struct vhci_hcd	*vhci;
 	unsigned long	flags;
-	int		retval = 0;
-
-	/* the enough buffer is allocated according to USB_MAXCHILDREN */
-	unsigned long	*event_bits = (unsigned long *) buf;
+	int		retval;
 	int		rhport;
 	int		changed = 0;
 
-	*event_bits = 0;
+	retval = DIV_ROUND_UP(VHCI_NPORTS + 1, 8);
+	memset(buf, 0, retval);
 
 	vhci = hcd_to_vhci(hcd);
 
 	spin_lock_irqsave(&vhci->lock, flags);
 	if (!HCD_HW_ACCESSIBLE(hcd)) {
-		usbip_dbg_vhci_rh("hw accessible flag in on?\n");
+		usbip_dbg_vhci_rh("hw accessible flag not on?\n");
 		goto done;
 	}
 
@@ -223,9 +211,9 @@ static int vhci_hub_status(struct usb_hcd *hcd, char *buf)
 	for (rhport = 0; rhport < VHCI_NPORTS; rhport++) {
 		if ((vhci->port_status[rhport] & PORT_C_MASK)) {
 			/* The status of a port has been changed, */
-			usbip_dbg_vhci_rh("port %d is changed\n", rhport);
+			usbip_dbg_vhci_rh("port %d status changed\n", rhport);
 
-			*event_bits |= 1 << (rhport + 1);
+			buf[(rhport + 1) / 8] |= 1 << (rhport + 1) % 8;
 			changed = 1;
 		}
 	}
@@ -235,14 +223,9 @@ static int vhci_hub_status(struct usb_hcd *hcd, char *buf)
 	if (hcd->state == HC_STATE_SUSPENDED)
 		usb_hcd_resume_root_hub(hcd);
 
-	if (changed)
-		retval = 1 + (VHCI_NPORTS / 8);
-	else
-		retval = 0;
-
 done:
 	spin_unlock_irqrestore(&vhci->lock, flags);
-	return retval;
+	return changed ? retval : 0;
 }
 
 /* See hub_configure in hub.c */
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index e24a6f95db1..e25645e226e 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -31,7 +31,6 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/syscalls.h>
-#include <linux/mutex.h>
 #include <linux/types.h>
 
 #include <linux/io.h>
@@ -474,7 +473,7 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 	case CONTROL_MINOR:
 		switch (cmd) {
 		case VME_IRQ_GEN:
-			copied = copy_from_user(&irq_req, (char *)arg,
+			copied = copy_from_user(&irq_req, argp,
 						sizeof(struct vme_irq_id));
 			if (copied != 0) {
 				printk(KERN_WARNING "Partial copy from userspace\n");
diff --git a/drivers/staging/vt6655/80211hdr.h b/drivers/staging/vt6655/80211hdr.h
index f55283b8641..c4d2349260e 100644
--- a/drivers/staging/vt6655/80211hdr.h
+++ b/drivers/staging/vt6655/80211hdr.h
@@ -80,7 +80,6 @@
 #define WLAN_HDR_ADDR4_LEN          30
 #define WLAN_IEHDR_LEN              2
 #define WLAN_SSID_MAXLEN            32
-/*#define WLAN_RATES_MAXLEN           255*/
 #define WLAN_RATES_MAXLEN           16
 #define WLAN_RATES_MAXLEN_11B       4
 #define WLAN_RSN_MAXLEN             32
@@ -106,7 +105,6 @@
 #define WLAN_WEP40_KEYLEN           5
 #define WLAN_WEP104_KEYLEN          13
 #define WLAN_WEP232_KEYLEN          29
-/*#define WLAN_WEPMAX_KEYLEN          29*/
 #define WLAN_WEPMAX_KEYLEN          32
 #define WLAN_CHALLENGE_IE_MAXLEN    255
 #define WLAN_CHALLENGE_IE_LEN       130
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index 1e1c6e34f78..e7b93a21e3b 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -60,8 +60,6 @@
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
 
-//#define	PLICE_DEBUG
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index 8294bdbb7b5..be2d6890949 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -46,36 +46,6 @@
 // Baseband RF pair definition in eeprom (Bits 6..0)
 //
 
-/*
-#define RATE_1M         0
-#define RATE_2M         1
-#define RATE_5M         2
-#define RATE_11M        3
-#define RATE_6M         4
-#define RATE_9M         5
-#define RATE_12M        6
-#define RATE_18M        7
-#define RATE_24M        8
-#define RATE_36M        9
-#define RATE_48M       10
-#define RATE_54M       11
-#define RATE_AUTO      12
-#define MAX_RATE       12
-
-
-//0:11A 1:11B 2:11G
-#define BB_TYPE_11A    0
-#define BB_TYPE_11B    1
-#define BB_TYPE_11G    2
-
-//0:11a,1:11b,2:11gb(only CCK in BasicRate),3:11ga(OFDM in Basic Rate)
-#define PK_TYPE_11A     0
-#define PK_TYPE_11B     1
-#define PK_TYPE_11GB    2
-#define PK_TYPE_11GA    3
-*/
-
-
 #define PREAMBLE_LONG   0
 #define PREAMBLE_SHORT  1
 
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
index 1368e8cc9ad..fcffa4f0f4e 100644
--- a/drivers/staging/vt6655/bssdb.c
+++ b/drivers/staging/vt6655/bssdb.c
@@ -58,7 +58,6 @@
 #include "wpa2.h"
 #include "iowpa.h"
 
-//#define	PLICE_DEBUG
 /*---------------------  Static Definitions -------------------------*/
 
 
@@ -142,9 +141,8 @@ BSSpSearchBSSList(
     unsigned int ii = 0;
 
     if (pbyDesireBSSID != NULL) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSpSearchBSSList BSSID[%02X %02X %02X-%02X %02X %02X]\n",
-                            *pbyDesireBSSID,*(pbyDesireBSSID+1),*(pbyDesireBSSID+2),
-                            *(pbyDesireBSSID+3),*(pbyDesireBSSID+4),*(pbyDesireBSSID+5));
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
+			"BSSpSearchBSSList BSSID[%pM]\n", pbyDesireBSSID);
         if ((!is_broadcast_ether_addr(pbyDesireBSSID)) &&
 	     (memcmp(pbyDesireBSSID, ZeroBSSID, 6)!= 0)){
             pbyBSSID = pbyDesireBSSID;
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
index 138897a7932..084a1a5566a 100644
--- a/drivers/staging/vt6655/desc.h
+++ b/drivers/staging/vt6655/desc.h
@@ -152,10 +152,6 @@
 #define FRAGCTL_TKIP        0x0002 // 0000 0010 0000 0000
 #define FRAGCTL_LEGACY      0x0001 // 0000 0001 0000 0000
 #define FRAGCTL_NONENCRYPT  0x0000 // 0000 0000 0000 0000
-//#define FRAGCTL_AC3         0x0C00 // 0000 0000 0000 1100
-//#define FRAGCTL_AC2         0x0800 // 0000 0000 0000 1000
-//#define FRAGCTL_AC1         0x0400 // 0000 0000 0000 0100
-//#define FRAGCTL_AC0         0x0000 // 0000 0000 0000 0000
 #define FRAGCTL_ENDFRAG     0x0300 // 0000 0000 0000 0011
 #define FRAGCTL_MIDFRAG     0x0200 // 0000 0000 0000 0010
 #define FRAGCTL_STAFRAG     0x0100 // 0000 0000 0000 0001
@@ -184,10 +180,6 @@
 #define FRAGCTL_TKIP        0x0200 // 0000 0010 0000 0000
 #define FRAGCTL_LEGACY      0x0100 // 0000 0001 0000 0000
 #define FRAGCTL_NONENCRYPT  0x0000 // 0000 0000 0000 0000
-//#define FRAGCTL_AC3         0x000C // 0000 0000 0000 1100
-//#define FRAGCTL_AC2         0x0008 // 0000 0000 0000 1000
-//#define FRAGCTL_AC1         0x0004 // 0000 0000 0000 0100
-//#define FRAGCTL_AC0         0x0000 // 0000 0000 0000 0000
 #define FRAGCTL_ENDFRAG     0x0003 // 0000 0000 0000 0011
 #define FRAGCTL_MIDFRAG     0x0002 // 0000 0000 0000 0010
 #define FRAGCTL_STAFRAG     0x0001 // 0000 0000 0000 0001
@@ -195,8 +187,6 @@
 
 #endif // #ifdef __BIG_ENDIAN
 
-//#define TYPE_AC0DMA     0
-//#define TYPE_TXDMA0     1
 #define TYPE_TXDMA0     0
 #define TYPE_AC0DMA     1
 #define TYPE_ATIMDMA    2
@@ -215,7 +205,6 @@
 #define TD_FLAGS_NETIF_SKB               0x01       // check if need release skb
 #define TD_FLAGS_PRIV_SKB                0x02       // check if called from private skb(hostap)
 #define TD_FLAGS_PS_RETRY                0x04       // check if PS STA frame re-transmit
-//#define TD_FLAGS_NETIF_SKB                0x04
 
 /*---------------------  Export Types  ------------------------------*/
 
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index 780205c0a88..c5e6b98d3e4 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -69,10 +69,6 @@
 #ifndef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 #define WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 #endif
-//2008-4-14<add> by chester for led issue
-//#define FOR_LED_ON_NOTEBOOK
-//
-
 
 //
 // device specific
@@ -91,12 +87,6 @@
 #include "key.h"
 #include "mac.h"
 
-//PLICE_DEBUG->
-//#define		THREAD
-
-//#define	TASK_LET
-//PLICE_DEBUG<-
-
 
 /*---------------------  Export Definitions -------------------------*/
 
@@ -105,11 +95,6 @@
 #define MAX_MULTICAST_ADDRESS_NUM       32
 #define MULTICAST_ADDRESS_LIST_SIZE     (MAX_MULTICAST_ADDRESS_NUM * ETH_ALEN)
 
-
-//#define OP_MODE_INFRASTRUCTURE  0
-//#define OP_MODE_ADHOC           1
-//#define OP_MODE_AP              2
-
 #define DUPLICATE_RX_CACHE_LENGTH       5
 
 #define NUM_KEY_ENTRY                   11
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 3e8283c2dc7..89d1c22695a 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -87,12 +87,10 @@
 #include <linux/kthread.h>
 #include <linux/slab.h>
 
-//#define	DEBUG
 /*---------------------  Static Definitions -------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =   MSG_LEVEL_INFO;
 
-//#define	PLICE_DEBUG
 //
 // Define module options
 //
@@ -100,10 +98,8 @@ MODULE_AUTHOR("VIA Networking Technologies, Inc., <lyndonchen@vntek.com.tw>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("VIA Networking Solomon-A/B/G Wireless LAN Adapter Driver");
 
-//PLICE_DEBUG ->
 	static int mlme_kill;
 	//static  struct task_struct * mlme_task;
-//PLICE_DEBUG <-
 
 #define DEVICE_PARAM(N,D)
 /*
@@ -1086,15 +1082,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
         device_free_info(pDevice);
         return -ENODEV;
     }
-//2008-07-21-01<Add>by MikeLiu
-//register wpadev
-#if 0
-   if(wpa_set_wpadev(pDevice, 1)!=0) {
-     printk("Fail to Register WPADEV?\n");
-        unregister_netdev(pDevice->dev);
-        free_netdev(dev);
-   }
-#endif
     device_print_info(pDevice);
     pci_set_drvdata(pcid, pDevice);
     return 0;
@@ -1948,15 +1935,6 @@ device_init_rd0_ring(pDevice);
 
 
 
-#if 0
-	pDevice->MLMEThr_pid = kernel_thread(MlmeThread, pDevice, CLONE_VM);
-	if (pDevice->MLMEThr_pid <0 )
-	{
-		printk("unable start thread MlmeThread\n");
-		return -1;
-	}
-#endif
-
 	//printk("thread id is %d\n",pDevice->MLMEThr_pid);
 	//printk("Create thread time is %x\n",jiffies);
 	//wait_for_completion(&pDevice->notify);
@@ -2493,21 +2471,6 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
                                 &(pDevice->byTopCCKBasicRate),
                                 &(pDevice->byTopOFDMBasicRate));
 
-#if 0
-printk("auto rate:Rate : %d,AckRate:%d,TopCCKRate:%d,TopOFDMRate:%d\n",
-pDevice->wCurrentRate,pDevice->byACKRate,
-pDevice->byTopCCKBasicRate,pDevice->byTopOFDMBasicRate);
-
-#endif
-
-#if 0
-
-	pDevice->wCurrentRate = 11;
-	pDevice->byACKRate = 8;
-	pDevice->byTopCCKBasicRate = 3;
-	pDevice->byTopOFDMBasicRate = 8;
-#endif
-
 
 		}
     }
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index c0fab4bc870..e8a71ba4b92 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -55,7 +55,6 @@
 #include "iowpa.h"
 #include "aes_ccmp.h"
 
-//#define	PLICE_DEBUG
 
 
 /*---------------------  Static Definitions -------------------------*/
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
index 77350270220..6ac6f452b26 100644
--- a/drivers/staging/vt6655/hostap.c
+++ b/drivers/staging/vt6655/hostap.c
@@ -40,14 +40,8 @@
 
 #define VIAWGET_HOSTAPD_MAX_BUF_SIZE 1024
 #define HOSTAP_CRYPT_FLAG_SET_TX_KEY BIT0
-#define HOSTAP_CRYPT_FLAG_PERMANENT BIT1
-#define HOSTAP_CRYPT_ERR_UNKNOWN_ALG 2
 #define HOSTAP_CRYPT_ERR_UNKNOWN_ADDR 3
-#define HOSTAP_CRYPT_ERR_CRYPT_INIT_FAILED 4
 #define HOSTAP_CRYPT_ERR_KEY_SET_FAILED 5
-#define HOSTAP_CRYPT_ERR_TX_KEY_SET_FAILED 6
-#define HOSTAP_CRYPT_ERR_CARD_CONF_FAILED 7
-
 
 /*---------------------  Static Definitions -------------------------*/
 
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index 87288db2178..77aad7f5ae7 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -84,24 +84,6 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 
 	pDevice->wstats.status = pDevice->eOPMode;
 	#ifdef Calcu_LinkQual
-	 #if 0
-	  if(pDevice->byBBType == BB_TYPE_11B) {
-	     if(pDevice->byCurrSQ > 120)
-                  pDevice->scStatistic.LinkQuality = 100;
-	     else
-		 pDevice->scStatistic.LinkQuality = pDevice->byCurrSQ*100/120;
-	    }
-	  else if(pDevice->byBBType == BB_TYPE_11G) {
-                if(pDevice->byCurrSQ < 20)
-		   pDevice->scStatistic.LinkQuality = 100;
-	       else if(pDevice->byCurrSQ >96)
-		   pDevice->scStatistic.LinkQuality  = 0;
-	       else
-		   pDevice->scStatistic.LinkQuality = (96-pDevice->byCurrSQ)*100/76;
-	   }
-	   if(pDevice->bLinkPass !=true)
-	       pDevice->scStatistic.LinkQuality = 0;
-	  #endif
 	   if(pDevice->scStatistic.LinkQuality > 100)
    	       pDevice->scStatistic.LinkQuality = 100;
                pDevice->wstats.qual.qual =(unsigned char) pDevice->scStatistic.LinkQuality;
@@ -2004,24 +1986,6 @@ param->u.wpa_key.key = (u8 *)key_array;
 param->u.wpa_key.seq = (u8 *)seq;
 param->u.wpa_key.seq_len = seq_len;
 
-#if 0
-printk("param->u.wpa_key.alg_name =%d\n",param->u.wpa_key.alg_name);
-printk(KERN_DEBUG "param->addr=%pM\n", param->addr);
-printk("param->u.wpa_key.set_tx =%d\n",param->u.wpa_key.set_tx);
-printk("param->u.wpa_key.key_index =%d\n",param->u.wpa_key.key_index);
-printk("param->u.wpa_key.key_len =%d\n",param->u.wpa_key.key_len);
-printk("param->u.wpa_key.key =");
-for(ii=0;ii<param->u.wpa_key.key_len;ii++)
-	printk("%02x:",param->u.wpa_key.key[ii]);
-         printk("\n");
-printk("param->u.wpa_key.seq_len =%d\n",param->u.wpa_key.seq_len);
-printk("param->u.wpa_key.seq =");
-for(ii=0;ii<param->u.wpa_key.seq_len;ii++)
-	printk("%02x:",param->u.wpa_key.seq[ii]);
-         printk("\n");
-
-printk("...........\n");
-#endif
 //****set if current action is Network Manager count??
 //****this method is so foolish,but there is no other way???
 if(param->u.wpa_key.alg_name == WPA_ALG_NONE) {
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index b96d27ee254..e3ccfee9026 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -292,19 +292,11 @@
 //
 #define EnCFG_BarkerPream   0x00020000
 #define EnCFG_NXTBTTCFPSTR  0x00010000
-//#define EnCFG_TXLMT3UPDATE  0x00008000
-//#define EnCFG_TXLMT2UPDATE  0x00004000
-//#define EnCFG_TXLMT1UPDATE  0x00002000
-//#define EnCFG_TXLMT3EN      0x00001000
-//#define EnCFG_TXLMT2EN      0x00000800
-//#define EnCFG_TXLMT1EN      0x00000400
 #define EnCFG_BcnSusClr     0x00000200
 #define EnCFG_BcnSusInd     0x00000100
-//#define EnCFG_CWOFF1        0x00000080
 #define EnCFG_CFP_ProtectEn 0x00000040
 #define EnCFG_ProtectMd     0x00000020
 #define EnCFG_HwParCFP      0x00000010
-//#define EnCFG_QOS           0x00000008
 #define EnCFG_CFNULRSP      0x00000004
 #define EnCFG_BBType_MASK   0x00000003
 #define EnCFG_BBType_g      0x00000002
@@ -392,14 +384,6 @@
 #define IMR_RADARDETECT     0x10000000      //
 #define IMR_MEASUREEND      0x08000000      //
 #define IMR_SOFTTIMER1      0x00200000      //
-//#define IMR_SYNCFLUSHOK     0x00100000      //
-//#define IMR_ATIMEND         0x00080000      //0000 1000 0000 0000 0000 0000
-//#define IMR_CFPEND          0x00040000      //0000 0100 0000 0000 0000 0000
-//#define IMR_AC3DMA          0x00020000      //0000 0010 0000 0000 0000 0000
-//#define IMR_AC2DMA          0x00010000      //0000 0001 0000 0000 0000 0000
-//#define IMR_AC1DMA          0x00008000      //0000 0000 1000 0000 0000 0000
-//#define IMR_SYNCTX          0x00004000      //0000 0000 0100 0000 0000 0000
-//#define IMR_ATIMTX          0x00002000      //0000 0000 0010 0000 0000 0000
 #define IMR_RXDMA1          0x00001000      //0000 0000 0001 0000 0000 0000
 #define IMR_RXNOBUF         0x00000800      //
 #define IMR_MIBNEARFULL     0x00000400      //
@@ -424,14 +408,6 @@
 #define ISR_RADARDETECT     0x10000000      //
 #define ISR_MEASUREEND      0x08000000      //
 #define ISR_SOFTTIMER1      0x00200000      //
-//#define ISR_SYNCFLUSHOK     0x00100000      //0001 0000 0000 0000 0000 0000
-//#define ISR_ATIMEND         0x00080000      //0000 1000 0000 0000 0000 0000
-//#define ISR_CFPEND          0x00040000      //0000 0100 0000 0000 0000 0000
-//#define ISR_AC3DMA          0x00020000      //0000 0010 0000 0000 0000 0000
-//#define ISR_AC2DMA          0x00010000      //0000 0001 0000 0000 0000 0000
-//#define ISR_AC1DMA          0x00008000      //0000 0000 1000 0000 0000 0000
-//#define ISR_SYNCTX          0x00004000      //0000 0000 0100 0000 0000 0000
-//#define ISR_ATIMTX          0x00002000      //0000 0000 0010 0000 0000 0000
 #define ISR_RXDMA1          0x00001000      //0000 0000 0001 0000 0000 0000
 #define ISR_RXNOBUF         0x00000800      //0000 0000 0000 1000 0000 0000
 #define ISR_MIBNEARFULL     0x00000400      //0000 0000 0000 0100 0000 0000
diff --git a/drivers/staging/vt6655/mib.h b/drivers/staging/vt6655/mib.h
index 009f3a4d29f..5cd697a2bc7 100644
--- a/drivers/staging/vt6655/mib.h
+++ b/drivers/staging/vt6655/mib.h
@@ -98,7 +98,6 @@ typedef struct tagSMib2Counter {
 } SMib2Counter, *PSMib2Counter;
 
 // Value in the ifType entry
-//#define ETHERNETCSMACD      6           //
 #define WIRELESSLANIEEE80211b      6           //
 
 // Value in the ifAdminStatus/ifOperStatus entry
@@ -210,7 +209,6 @@ typedef struct tagSISRCounters {
 #define UNDER_CREATION      3           //
 #define INVALID             4           //
 
-//#define MAX_RATE            12
 //
 // statistic counter
 //
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index b8ec783e55e..aa696650b86 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -41,52 +41,16 @@
 
 //static int          msglevel                =MSG_LEVEL_INFO;
 
-#define BY_RF2959_REG_LEN     23 //24bits
-#define CB_RF2959_INIT_SEQ    15
-#define SWITCH_CHANNEL_DELAY_RF2959 200 //us
-#define RF2959_PWR_IDX_LEN    32
-
-#define BY_MA2825_REG_LEN     23 //24bit
-#define CB_MA2825_INIT_SEQ    13
-#define SWITCH_CHANNEL_DELAY_MA2825 200 //us
-#define MA2825_PWR_IDX_LEN    31
-
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define SWITCH_CHANNEL_DELAY_AL2230 200 //us
 #define AL2230_PWR_IDX_LEN    64
 
 
-#define BY_UW2451_REG_LEN     23
-#define CB_UW2451_INIT_SEQ    6
-#define SWITCH_CHANNEL_DELAY_UW2451 200 //us
-#define UW2451_PWR_IDX_LEN    25
-
-//{{ RobertYu: 20041118
-#define BY_MA2829_REG_LEN     23 //24bit
-#define CB_MA2829_INIT_SEQ    13
-#define SWITCH_CHANNEL_DELAY_MA2829 200 //us
-#define MA2829_PWR_IDX_LEN    64
-//}} RobertYu
-
-//{{ RobertYu:20050103
 #define BY_AL7230_REG_LEN     23 //24bit
 #define CB_AL7230_INIT_SEQ    16
 #define SWITCH_CHANNEL_DELAY_AL7230 200 //us
 #define AL7230_PWR_IDX_LEN    64
-//}} RobertYu
-
-//{{ RobertYu: 20041210
-#define BY_UW2452_REG_LEN     23
-#define CB_UW2452_INIT_SEQ    5 //RoberYu:20050113, Rev0.2 Programming Guide(remove R3, so 6-->5)
-#define SWITCH_CHANNEL_DELAY_UW2452 100 //us
-#define UW2452_PWR_IDX_LEN    64
-//}} RobertYu
-
-#define BY_VT3226_REG_LEN     23
-#define CB_VT3226_INIT_SEQ    12
-#define SWITCH_CHANNEL_DELAY_VT3226 200 //us
-#define VT3226_PWR_IDX_LEN    16
 
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -1067,48 +1031,6 @@ unsigned char byPwrdBm = 0;
 		break;
     }
 
-#if 0
-
-    // 802.11h TPC
-    if (pDevice->bLinkPass == true) {
-        // do not over local constraint
-        if (byPwrdBm > pDevice->abyLocalPwr[uCH]) {
-            pDevice->byCurPwrdBm = pDevice->abyLocalPwr[uCH];
-            byDec = byPwrdBm - pDevice->abyLocalPwr[uCH];
-            if (pDevice->byRFType == RF_UW2452) {
-                byDec *= 3;
-            } else {
-                byDec <<= 1;
-            }
-            if (byPwr > byDec) {
-                byPwr -= byDec;
-            } else {
-                byPwr = 0;
-            }
-        } else {
-            pDevice->byCurPwrdBm = byPwrdBm;
-        }
-    } else {
-        // do not over regulatory constraint
-        if (byPwrdBm > pDevice->abyRegPwr[uCH]) {
-            pDevice->byCurPwrdBm = pDevice->abyRegPwr[uCH];
-            byDec = byPwrdBm - pDevice->abyRegPwr[uCH];
-            if (pDevice->byRFType == RF_UW2452) {
-                byDec *= 3;
-            } else {
-                byDec <<= 1;
-            }
-            if (byPwr > byDec) {
-                byPwr -= byDec;
-            } else {
-                byPwr = 0;
-            }
-        } else {
-            pDevice->byCurPwrdBm = byPwrdBm;
-        }
-    }
-#endif
-
 //    if (pDevice->byLocalID <= REV_ID_VT3253_B1) {
     if (pDevice->byCurPwr == byPwr) {
         return true;
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index 1f8d82e1304..73f09693ee7 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -41,7 +41,6 @@
 #define RF_MAXIMAG              0x02
 #define RF_AIROHA               0x03
 
-//#define RF_GCT5103              0x04
 #define RF_UW2451               0x05
 #define RF_MAXIMG               0x06
 #define RF_MAXIM2829            0x07 // RobertYu: 20041118
diff --git a/drivers/staging/vt6655/tether.h b/drivers/staging/vt6655/tether.h
index 787d885deee..6a68f97d9a3 100644
--- a/drivers/staging/vt6655/tether.h
+++ b/drivers/staging/vt6655/tether.h
@@ -90,8 +90,6 @@
 #define TYPE_CTL_ACK        0xd400
 
 
-//#define WEP_IV_MASK         0xFFFFFF00
-
 #else //if LITTLE_ENDIAN
 //
 // wType field in the SEthernetHeader
@@ -143,7 +141,6 @@
 #define TYPE_CTL_ACK        0x00d4
 
 
-//#define WEP_IV_MASK         0x00FFFFFF
 
 #endif //#ifdef __BIG_ENDIAN
 
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
index 0491d0b52c8..d645ecd8941 100644
--- a/drivers/staging/vt6655/vntwifi.c
+++ b/drivers/staging/vt6655/vntwifi.c
@@ -38,8 +38,6 @@
 #include "wmgr.h"
 #include "datarate.h"
 
-//#define	PLICE_DEBUG
-
 /*---------------------  Static Definitions -------------------------*/
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 //static int          msglevel                =MSG_LEVEL_INFO;
diff --git a/drivers/staging/vt6655/wcmd.c b/drivers/staging/vt6655/wcmd.c
index bab3b0116cc..7b5b99c8cf1 100644
--- a/drivers/staging/vt6655/wcmd.c
+++ b/drivers/staging/vt6655/wcmd.c
@@ -684,18 +684,6 @@ printk("chester-abyDesireSSID=%s\n",((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySS
 	       return;
 	   }
 	          pDevice->byLinkWaitCount = 0;
-		 #if 0
-                     #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-                    // if(pDevice->bWPASuppWextEnabled == true)
-                        {
-                  	union iwreq_data  wrqu;
-                  	memset(&wrqu, 0, sizeof (wrqu));
-                          wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-                  	printk("wireless_send_event--->SIOCGIWAP(disassociated:AUTHENTICATE_WAIT_timeout)\n");
-                  	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-                       }
-                    #endif
-	         #endif
             s_bCommandComplete(pDevice);
             break;
 
@@ -748,18 +736,6 @@ printk("chester-abyDesireSSID=%s\n",((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySS
 	       return;
 	   }
 	          pDevice->byLinkWaitCount = 0;
-		#if 0
-                     #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-                    // if(pDevice->bWPASuppWextEnabled == true)
-                        {
-                  	union iwreq_data  wrqu;
-                  	memset(&wrqu, 0, sizeof (wrqu));
-                          wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-                  	printk("wireless_send_event--->SIOCGIWAP(disassociated:ASSOCIATE_WAIT_timeout)\n");
-                  	wireless_send_event(pDevice->dev, SIOCGIWAP, &wrqu, NULL);
-                       }
-                    #endif
-		#endif
 
             s_bCommandComplete(pDevice);
             break;
diff --git a/drivers/staging/vt6656/80211hdr.h b/drivers/staging/vt6656/80211hdr.h
index 15c6ef1a635..b87d5434077 100644
--- a/drivers/staging/vt6656/80211hdr.h
+++ b/drivers/staging/vt6656/80211hdr.h
@@ -78,7 +78,6 @@
 #define WLAN_HDR_ADDR4_LEN          30
 #define WLAN_IEHDR_LEN              2
 #define WLAN_SSID_MAXLEN            32
-/* #define WLAN_RATES_MAXLEN           255 */
 #define WLAN_RATES_MAXLEN           16
 #define WLAN_RATES_MAXLEN_11B       4
 #define WLAN_RSN_MAXLEN             32
@@ -104,7 +103,6 @@
 #define WLAN_WEP40_KEYLEN           5
 #define WLAN_WEP104_KEYLEN          13
 #define WLAN_WEP232_KEYLEN          29
-/* #define WLAN_WEPMAX_KEYLEN          29 */
 #define WLAN_WEPMAX_KEYLEN          32
 #define WLAN_CHALLENGE_IE_MAXLEN    255
 #define WLAN_CHALLENGE_IE_LEN       130
diff --git a/drivers/staging/vt6656/bssdb.c b/drivers/staging/vt6656/bssdb.c
index 619c257e877..099936771e6 100644
--- a/drivers/staging/vt6656/bssdb.c
+++ b/drivers/staging/vt6656/bssdb.c
@@ -129,9 +129,8 @@ PKnownBSS BSSpSearchBSSList(void *hDeviceContext,
     unsigned int ii = 0;
     unsigned int jj = 0;
     if (pbyDesireBSSID != NULL) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSpSearchBSSList BSSID[%02X %02X %02X-%02X %02X %02X]\n",
-                            *pbyDesireBSSID,*(pbyDesireBSSID+1),*(pbyDesireBSSID+2),
-                            *(pbyDesireBSSID+3),*(pbyDesireBSSID+4),*(pbyDesireBSSID+5));
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
+			"BSSpSearchBSSList BSSID[%pM]\n", pbyDesireBSSID);
 	if ((!is_broadcast_ether_addr(pbyDesireBSSID)) &&
 	     (memcmp(pbyDesireBSSID, ZeroBSSID, 6)!= 0)){
             pbyBSSID = pbyDesireBSSID;
@@ -218,7 +217,9 @@ PKnownBSS BSSpSearchBSSList(void *hDeviceContext,
                 }
 
         pMgmt->pSameBSS[jj].uChannel = pCurrBSS->uChannel;
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSpSearchBSSList pSelect1[%02X %02X %02X-%02X %02X %02X]\n",*pCurrBSS->abyBSSID,*(pCurrBSS->abyBSSID+1),*(pCurrBSS->abyBSSID+2),*(pCurrBSS->abyBSSID+3),*(pCurrBSS->abyBSSID+4),*(pCurrBSS->abyBSSID+5));
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
+			"BSSpSearchBSSList pSelect1[%pM]\n",
+			pCurrBSS->abyBSSID);
         jj++;
 
 
diff --git a/drivers/staging/vt6656/bssdb.h b/drivers/staging/vt6656/bssdb.h
index a8f97ebb659..6b2ec390e77 100644
--- a/drivers/staging/vt6656/bssdb.h
+++ b/drivers/staging/vt6656/bssdb.h
@@ -64,8 +64,6 @@
 // send and receive non-IEEE 802.1X frames
 #define WLAN_STA_AUTHORIZED      BIT5
 
-//#define MAX_RATE            12
-
 #define MAX_WPA_IE_LEN      64
 
 
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 9d09e9fd8e1..e3ddc0b5531 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -66,7 +66,6 @@ static int          msglevel                =MSG_LEVEL_INFO;
 
 
 /*---------------------  Static Definitions -------------------------*/
-#define CB_TXPOWER_LEVEL            6
 
 /*---------------------  Static Classes  ----------------------------*/
 
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index 9cf71a3d880..55962b19883 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -52,7 +52,6 @@ typedef enum _CARD_OP_MODE {
 } CARD_OP_MODE, *PCARD_OP_MODE;
 
 #define CB_MAX_CHANNEL_24G  14
-/* #define CB_MAX_CHANNEL_5G   24 */
 #define CB_MAX_CHANNEL_5G       42 /* add channel9(5045MHz), 41==>42 */
 #define CB_MAX_CHANNEL      (CB_MAX_CHANNEL_24G+CB_MAX_CHANNEL_5G)
 
diff --git a/drivers/staging/vt6656/channel.c b/drivers/staging/vt6656/channel.c
index 99e054d2d60..65021760785 100644
--- a/drivers/staging/vt6656/channel.c
+++ b/drivers/staging/vt6656/channel.c
@@ -368,8 +368,6 @@ static  struct
 /*                                           1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  48  49  50  51  52  53  54  55  56  */
 };
 
-#define NUM_RULES	ARRAY_SIZE(ChannelRuleTab)
-
 /*---------------------  Export function  -------------------------*/
 /************************************************************************
  * Country Channel Valid
diff --git a/drivers/staging/vt6656/desc.h b/drivers/staging/vt6656/desc.h
index 767112b3c4a..b68b2ec96ea 100644
--- a/drivers/staging/vt6656/desc.h
+++ b/drivers/staging/vt6656/desc.h
@@ -36,8 +36,6 @@
 #include "ttype.h"
 #include "tether.h"
 
-/*---------------------  Export Definitions -------------------------*/
-
 // max transmit or receive buffer size
 #define CB_MAX_BUF_SIZE     2900U       // max buffer size
                                         // NOTE: must be multiple of 4
@@ -120,17 +118,11 @@
 #define FRAGCTL_TKIP        0x0200 // 0000 0010 0000 0000
 #define FRAGCTL_LEGACY      0x0100 // 0000 0001 0000 0000
 #define FRAGCTL_NONENCRYPT  0x0000 // 0000 0000 0000 0000
-//#define FRAGCTL_AC3         0x000C // 0000 0000 0000 1100
-//#define FRAGCTL_AC2         0x0008 // 0000 0000 0000 1000
-//#define FRAGCTL_AC1         0x0004 // 0000 0000 0000 0100
-//#define FRAGCTL_AC0         0x0000 // 0000 0000 0000 0000
 #define FRAGCTL_ENDFRAG     0x0003 // 0000 0000 0000 0011
 #define FRAGCTL_MIDFRAG     0x0002 // 0000 0000 0000 0010
 #define FRAGCTL_STAFRAG     0x0001 // 0000 0000 0000 0001
 #define FRAGCTL_NONFRAG     0x0000 // 0000 0000 0000 0000
 
-//#define TYPE_AC0DMA     0
-//#define TYPE_TXDMA0     1
 #define TYPE_TXDMA0     0
 #define TYPE_AC0DMA     1
 #define TYPE_ATIMDMA    2
@@ -147,9 +139,6 @@
 #define TD_FLAGS_NETIF_SKB               0x01       // check if need release skb
 #define TD_FLAGS_PRIV_SKB                0x02       // check if called from private skb(hostap)
 #define TD_FLAGS_PS_RETRY                0x04       // check if PS STA frame re-transmit
-//#define TD_FLAGS_NETIF_SKB                0x04
-
-/*---------------------  Export Types  ------------------------------*/
 
 //
 // RsvTime buffer header
@@ -268,7 +257,6 @@ SRTS_a_FB, *PSRTS_a_FB;
 
 typedef const SRTS_a_FB *PCSRTS_a_FB;
 
-
 //
 // CTS buffer header
 //
@@ -421,12 +409,5 @@ typedef struct tagSKeyEntry {
     DWORD dwKey4[4];
 } __attribute__ ((__packed__))
 SKeyEntry;
-/*---------------------  Export Macros ------------------------------*/
-
-/*---------------------  Export Classes  ----------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
 
 #endif /* __DESC_H__ */
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index f1496ec5dc7..171dd68cf5b 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -106,10 +106,6 @@
 #define MAX_MULTICAST_ADDRESS_NUM       32
 #define MULTICAST_ADDRESS_LIST_SIZE     (MAX_MULTICAST_ADDRESS_NUM * ETH_ALEN)
 
-//#define OP_MODE_INFRASTRUCTURE  0
-//#define OP_MODE_ADHOC           1
-//#define OP_MODE_AP              2
-
 #define DUPLICATE_RX_CACHE_LENGTH       5
 
 #define NUM_KEY_ENTRY                   11
diff --git a/drivers/staging/vt6656/hostap.c b/drivers/staging/vt6656/hostap.c
index 51b5adf3657..682002a5b8d 100644
--- a/drivers/staging/vt6656/hostap.c
+++ b/drivers/staging/vt6656/hostap.c
@@ -41,31 +41,11 @@
 
 #define VIAWGET_HOSTAPD_MAX_BUF_SIZE 1024
 #define HOSTAP_CRYPT_FLAG_SET_TX_KEY BIT0
-#define HOSTAP_CRYPT_FLAG_PERMANENT BIT1
-#define HOSTAP_CRYPT_ERR_UNKNOWN_ALG 2
 #define HOSTAP_CRYPT_ERR_UNKNOWN_ADDR 3
-#define HOSTAP_CRYPT_ERR_CRYPT_INIT_FAILED 4
 #define HOSTAP_CRYPT_ERR_KEY_SET_FAILED 5
-#define HOSTAP_CRYPT_ERR_TX_KEY_SET_FAILED 6
-#define HOSTAP_CRYPT_ERR_CARD_CONF_FAILED 7
 
-
-/*---------------------  Static Definitions -------------------------*/
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
 
-/*---------------------  Static Functions  --------------------------*/
-
-
-
-
-/*---------------------  Export Variables  --------------------------*/
-
-
 /*
  * Description:
  *      register net_device (AP) for hostap deamon
@@ -314,37 +294,6 @@ static int hostap_get_info_sta(PSDevice pDevice,
 	return 0;
 }
 
-/*
- * Description:
- *      reset txexec
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *      TURE, FALSE
- *
- * Return Value:
- *
- */
-/*
-static int hostap_reset_txexc_sta(PSDevice pDevice,
-					  struct viawget_hostapd_param *param)
-{
-    PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &uNodeIndex)) {
-        pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts = 0;
-	}
-	else {
-	    return -ENOENT;
-	}
-
-	return 0;
-}
-*/
 
 /*
  * Description:
@@ -479,12 +428,6 @@ static int hostap_set_encryption(PSDevice pDevice,
 
 
 	param->u.crypt.err = 0;
-/*
-	if (param_len !=
-	    (int) ((char *) param->u.crypt.key - (char *) param) +
-	    param->u.crypt.key_len)
-		return -EINVAL;
-*/
 
 	if (param->u.crypt.alg > WPA_ALG_CCMP)
 		return -EINVAL;
@@ -814,12 +757,6 @@ int vt6656_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 		 ret = hostap_get_info_sta(pDevice, param);
 		 ap_ioctl = 1;
 		break;
-/*
-	case VIAWGET_HOSTAPD_RESET_TXEXC_STA:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_RESET_TXEXC_STA \n");
-		 ret = hostap_reset_txexc_sta(pDevice, param);
-		break;
-*/
 	case VIAWGET_HOSTAPD_SET_FLAGS_STA:
 	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_FLAGS_STA \n");
 		 ret = hostap_set_flags_sta(pDevice, param);
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index 0a114231145..eba4b5061cf 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -41,9 +41,7 @@
 #include "usbpipe.h"
 
 /*---------------------  Static Definitions -------------------------*/
-/* static int msglevel = MSG_LEVEL_DEBUG; */
-static int msglevel = MSG_LEVEL_INFO;
-
+static int msglevel = MSG_LEVEL_INFO; /* MSG_LEVEL_DEBUG */
 
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -53,10 +51,8 @@ static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Export Variables  --------------------------*/
 
-
 /*---------------------  Export Functions  --------------------------*/
 
-
 /*+
  *
  *  Function:   InterruptPollingThread
@@ -81,7 +77,7 @@ static int msglevel = MSG_LEVEL_INFO;
 -*/
 void INTvWorkItem(void *Context)
 {
-	PSDevice pDevice = (PSDevice) Context;
+	PSDevice pDevice = Context;
 	int ntStatus;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Interrupt Polling Thread\n");
@@ -94,8 +90,8 @@ void INTvWorkItem(void *Context)
 
 void INTnsProcessData(PSDevice pDevice)
 {
-	PSINTData	pINTData;
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
+	PSINTData pINTData;
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 	struct net_device_stats *pStats = &pDevice->stats;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsInterruptProcessData\n");
@@ -103,8 +99,8 @@ void INTnsProcessData(PSDevice pDevice)
 	pINTData = (PSINTData) pDevice->intBuf.pDataBuf;
 	if (pINTData->byTSR0 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE) (pINTData->byPkt0 & 0x0F),
-					(BYTE) (pINTData->byPkt0>>4),
+					(BYTE)(pINTData->byPkt0 & 0x0F),
+					(BYTE)(pINTData->byPkt0>>4),
 					pINTData->byTSR0);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -114,8 +110,8 @@ void INTnsProcessData(PSDevice pDevice)
 	}
 	if (pINTData->byTSR1 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE) (pINTData->byPkt1 & 0x0F),
-					(BYTE) (pINTData->byPkt1>>4),
+					(BYTE)(pINTData->byPkt1 & 0x0F),
+					(BYTE)(pINTData->byPkt1>>4),
 					pINTData->byTSR1);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -125,8 +121,8 @@ void INTnsProcessData(PSDevice pDevice)
 	}
 	if (pINTData->byTSR2 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE) (pINTData->byPkt2 & 0x0F),
-					(BYTE) (pINTData->byPkt2>>4),
+					(BYTE)(pINTData->byPkt2 & 0x0F),
+					(BYTE)(pINTData->byPkt2>>4),
 					pINTData->byTSR2);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -136,8 +132,8 @@ void INTnsProcessData(PSDevice pDevice)
 	}
 	if (pINTData->byTSR3 & TSR_VALID) {
 		STAvUpdateTDStatCounter(&(pDevice->scStatistic),
-					(BYTE) (pINTData->byPkt3 & 0x0F),
-					(BYTE) (pINTData->byPkt3>>4),
+					(BYTE)(pINTData->byPkt3 & 0x0F),
+					(BYTE)(pINTData->byPkt3>>4),
 					pINTData->byTSR3);
 		BSSvUpdateNodeTxCounter(pDevice,
 					&(pDevice->scStatistic),
@@ -186,11 +182,11 @@ void INTnsProcessData(PSDevice pDevice)
 		LODWORD(pDevice->qwCurrTSF) = pINTData->dwLoTSF;
 		HIDWORD(pDevice->qwCurrTSF) = pINTData->dwHiTSF;
 		/*DBG_PRN_GRP01(("ISR0 = %02x ,
-				LoTsf =  %08x,
-				HiTsf =  %08x\n",
-				pINTData->byISR0,
-				pINTData->dwLoTSF,
-				pINTData->dwHiTSF)); */
+		  LoTsf =  %08x,
+		  HiTsf =  %08x\n",
+		  pINTData->byISR0,
+		  pINTData->dwLoTSF,
+		  pINTData->dwHiTSF)); */
 
 		STAvUpdate802_11Counter(&pDevice->s802_11Counter,
 					&pDevice->scStatistic,
@@ -202,7 +198,6 @@ void INTnsProcessData(PSDevice pDevice)
 					pINTData->byISR0,
 					pINTData->byISR1);
 	}
-
 	if (pINTData->byISR1 != 0)
 		if (pINTData->byISR1 & ISR_GPIO3)
 			bScheduleCommand((void *) pDevice,
@@ -213,8 +208,8 @@ void INTnsProcessData(PSDevice pDevice)
 
 	pStats->tx_packets = pDevice->scStatistic.ullTsrOK;
 	pStats->tx_bytes = pDevice->scStatistic.ullTxDirectedBytes +
-			pDevice->scStatistic.ullTxMulticastBytes +
-			pDevice->scStatistic.ullTxBroadcastBytes;
+		pDevice->scStatistic.ullTxMulticastBytes +
+		pDevice->scStatistic.ullTxBroadcastBytes;
 	pStats->tx_errors = pDevice->scStatistic.dwTsrErr;
 	pStats->tx_dropped = pDevice->scStatistic.dwTsrErr;
 }
diff --git a/drivers/staging/vt6656/int.h b/drivers/staging/vt6656/int.h
index a5d96b96817..3734e2c953d 100644
--- a/drivers/staging/vt6656/int.h
+++ b/drivers/staging/vt6656/int.h
@@ -36,31 +36,30 @@
 /*---------------------  Export Definitions -------------------------*/
 #pragma pack(1)
 typedef struct tagSINTData {
-    BYTE    byTSR0;
-    BYTE    byPkt0;
-    WORD    wTime0;
-    BYTE    byTSR1;
-    BYTE    byPkt1;
-    WORD    wTime1;
-    BYTE    byTSR2;
-    BYTE    byPkt2;
-    WORD    wTime2;
-    BYTE    byTSR3;
-    BYTE    byPkt3;
-    WORD    wTime3;
-    DWORD   dwLoTSF;
-    DWORD   dwHiTSF;
-    BYTE    byISR0;
-    BYTE    byISR1;
-    BYTE    byRTSSuccess;
-    BYTE    byRTSFail;
-    BYTE    byACKFail;
-    BYTE    byFCSErr;
-    BYTE    abySW[2];
+	BYTE byTSR0;
+	BYTE byPkt0;
+	WORD wTime0;
+	BYTE byTSR1;
+	BYTE byPkt1;
+	WORD wTime1;
+	BYTE byTSR2;
+	BYTE byPkt2;
+	WORD wTime2;
+	BYTE byTSR3;
+	BYTE byPkt3;
+	WORD wTime3;
+	DWORD dwLoTSF;
+	DWORD dwHiTSF;
+	BYTE byISR0;
+	BYTE byISR1;
+	BYTE byRTSSuccess;
+	BYTE byRTSFail;
+	BYTE byACKFail;
+	BYTE byFCSErr;
+	BYTE abySW[2];
 } __attribute__ ((__packed__))
 SINTData, *PSINTData;
 
-
 /*---------------------  Export Classes  ----------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
diff --git a/drivers/staging/vt6656/iwctl.c b/drivers/staging/vt6656/iwctl.c
index b24e5314a6a..8b9894b1146 100644
--- a/drivers/staging/vt6656/iwctl.c
+++ b/drivers/staging/vt6656/iwctl.c
@@ -47,20 +47,20 @@
 #include <net/iw_handler.h>
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-#define SUPPORTED_WIRELESS_EXT                  18
+#define SUPPORTED_WIRELESS_EXT 18
 #else
-#define SUPPORTED_WIRELESS_EXT                  17
+#define SUPPORTED_WIRELESS_EXT 17
 #endif
 
 static const long frequency_list[] = {
-    2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472, 2484,
-    4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
-    5035, 5040, 5045, 5055, 5060, 5080, 5170, 5180, 5190, 5200, 5210, 5220, 5230, 5240,
-    5260, 5280, 5300, 5320, 5500, 5520, 5540, 5560, 5580, 5600, 5620, 5640, 5660, 5680,
-    5700, 5745, 5765, 5785, 5805, 5825
-	};
+	2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472, 2484,
+	4915, 4920, 4925, 4935, 4940, 4945, 4960, 4980,
+	5035, 5040, 5045, 5055, 5060, 5080, 5170, 5180, 5190, 5200, 5210, 5220, 5230, 5240,
+	5260, 5280, 5300, 5320, 5500, 5520, 5540, 5560, 5580, 5600, 5620, 5640, 5660, 5680,
+	5700, 5745, 5765, 5785, 5805, 5825
+};
 
-static int          msglevel                =MSG_LEVEL_INFO;
+static int msglevel = MSG_LEVEL_INFO;
 
 struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 {
@@ -68,9 +68,9 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	long ldBm;
 
 	pDevice->wstats.status = pDevice->eOPMode;
-	   if(pDevice->scStatistic.LinkQuality > 100)
-   	       pDevice->scStatistic.LinkQuality = 100;
-               pDevice->wstats.qual.qual =(BYTE) pDevice->scStatistic.LinkQuality;
+	if (pDevice->scStatistic.LinkQuality > 100)
+		pDevice->scStatistic.LinkQuality = 100;
+	pDevice->wstats.qual.qual =(BYTE)pDevice->scStatistic.LinkQuality;
 	RFvRSSITodBm(pDevice, (BYTE)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
 	pDevice->wstats.qual.noise = 0;
@@ -81,93 +81,84 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	pDevice->wstats.discard.retries = pDevice->scStatistic.dwTsrErr;
 	pDevice->wstats.discard.misc = 0;
 	pDevice->wstats.miss.beacon = 0;
-
 	return &pDevice->wstats;
 }
 
 /*
- * Wireless Handler : get protocol name
+ * Wireless Handler: get protocol name
  */
-
-int iwctl_giwname(struct net_device *dev,
-			 struct iw_request_info *info,
-			 char *wrq,
-			 char *extra)
+int iwctl_giwname(struct net_device *dev, struct iw_request_info *info,
+		char *wrq, char *extra)
 {
 	strcpy(wrq, "802.11-a/b/g");
 	return 0;
 }
 
 /*
- * Wireless Handler : set scan
+ * Wireless Handler: set scan
  */
-
-int iwctl_siwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_point *wrq,
-             char *extra)
+int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-	 PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	struct iw_scan_req  *req = (struct iw_scan_req *)extra;
-	BYTE                abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	PWLAN_IE_SSID       pItemSSID=NULL;
-
-  if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
-        return -EINVAL;
-
-    PRINT_K(" SIOCSIWSCAN \n");
-
-if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-        // In scanning..
-     PRINT_K("SIOCSIWSCAN(overlap??)-->In scanning...\n");
-     return -EAGAIN;
-  }
-
-if(pDevice->byReAssocCount > 0) {   //reject scan when re-associating!
-//send scan event to wpa_Supplicant
-  union iwreq_data wrqu;
- PRINT_K("wireless_send_event--->SIOCGIWSCAN(scan done)\n");
- memset(&wrqu, 0, sizeof(wrqu));
- wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
-  return 0;
-}
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	struct iw_scan_req *req = (struct iw_scan_req *)extra;
+	BYTE abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+	PWLAN_IE_SSID pItemSSID = NULL;
+
+	if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
+		return -EINVAL;
+
+	PRINT_K(" SIOCSIWSCAN \n");
+
+	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
+		// In scanning..
+		PRINT_K("SIOCSIWSCAN(overlap??)-->In scanning...\n");
+		return -EAGAIN;
+	}
+
+	if (pDevice->byReAssocCount > 0) { // reject scan when re-associating!
+		// send scan event to wpa_Supplicant
+		union iwreq_data wrqu;
+		PRINT_K("wireless_send_event--->SIOCGIWSCAN(scan done)\n");
+		memset(&wrqu, 0, sizeof(wrqu));
+		wireless_send_event(pDevice->dev, SIOCGIWSCAN, &wrqu, NULL);
+		return 0;
+	}
 
 	spin_lock_irq(&pDevice->lock);
 
-	BSSvClearBSSList((void *) pDevice, pDevice->bLinkPass);
-
-//mike add: active scan OR passive scan OR desire_ssid scan
- if(wrq->length == sizeof(struct iw_scan_req)) {
-   if (wrq->flags & IW_SCAN_THIS_ESSID)  {                               //desire_ssid scan
-       memset(abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-       pItemSSID = (PWLAN_IE_SSID)abyScanSSID;
-       pItemSSID->byElementID = WLAN_EID_SSID;
-       memcpy(pItemSSID->abySSID, req->essid, (int)req->essid_len);
-         if (pItemSSID->abySSID[req->essid_len - 1] == '\0') {
-           if(req->essid_len>0)
-		pItemSSID->len = req->essid_len - 1;
-         }
-	else
-	  pItemSSID->len = req->essid_len;
-	  pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-         PRINT_K("SIOCSIWSCAN:[desired_ssid=%s,len=%d]\n",((PWLAN_IE_SSID)abyScanSSID)->abySSID,
-		 	                                                                                ((PWLAN_IE_SSID)abyScanSSID)->len);
-	bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
-	spin_unlock_irq(&pDevice->lock);
+	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
+
+	// mike add: active scan OR passive scan OR desire_ssid scan
+	if (wrq->length == sizeof(struct iw_scan_req)) {
+		if (wrq->flags & IW_SCAN_THIS_ESSID) { // desire_ssid scan
+			memset(abyScanSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
+			pItemSSID = (PWLAN_IE_SSID)abyScanSSID;
+			pItemSSID->byElementID = WLAN_EID_SSID;
+			memcpy(pItemSSID->abySSID, req->essid, (int)req->essid_len);
+			if (pItemSSID->abySSID[req->essid_len - 1] == '\0') {
+				if (req->essid_len > 0)
+					pItemSSID->len = req->essid_len - 1;
+			} else {
+				pItemSSID->len = req->essid_len;
+			}
+			pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+			PRINT_K("SIOCSIWSCAN:[desired_ssid=%s,len=%d]\n", ((PWLAN_IE_SSID)abyScanSSID)->abySSID,
+				((PWLAN_IE_SSID)abyScanSSID)->len);
+			bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, abyScanSSID);
+			spin_unlock_irq(&pDevice->lock);
+
+			return 0;
+		} else if (req->scan_type == IW_SCAN_TYPE_PASSIVE) { // passive scan
+			pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+		}
+	} else { // active scan
+		pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+	}
 
-	return 0;
-   }
-   else if(req->scan_type == IW_SCAN_TYPE_PASSIVE) {          //passive scan
-       pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-   }
- }
- else {           //active scan
-     pMgmt->eScanType = WMAC_SCAN_ACTIVE;
- }
-
-	 pMgmt->eScanType = WMAC_SCAN_PASSIVE;
-	bScheduleCommand((void *) pDevice, WLAN_CMD_BSSID_SCAN, NULL);
+	pMgmt->eScanType = WMAC_SCAN_PASSIVE;
+	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 	spin_unlock_irq(&pDevice->lock);
 
 	return 0;
@@ -176,18 +167,18 @@ if(pDevice->byReAssocCount > 0) {   //reject scan when re-associating!
 /*
  * Wireless Handler : get scan results
  */
-
-int iwctl_giwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_point *wrq,
-             char *extra)
+int iwctl_giwscan(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-    int ii, jj, kk;
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    PKnownBSS           pBSS;
-    PWLAN_IE_SSID       pItemSSID;
-    PWLAN_IE_SUPP_RATES pSuppRates, pExtSuppRates;
+	int ii;
+	int jj;
+	int kk;
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	PKnownBSS pBSS;
+	PWLAN_IE_SSID pItemSSID;
+	PWLAN_IE_SUPP_RATES pSuppRates;
+	PWLAN_IE_SUPP_RATES pExtSuppRates;
 	char *current_ev = extra;
 	char *end_buf = extra + IW_SCAN_MAX_DATA;
 	char *current_val = NULL;
@@ -195,194 +186,179 @@ int iwctl_giwscan(struct net_device *dev,
 	long ldBm;
 	char buf[MAX_WPA_IE_LEN * 2 + 30];
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSCAN\n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSCAN\n");
 
-    if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
-        // In scanning..
+	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
+		// In scanning..
 		return -EAGAIN;
 	}
 	pBSS = &(pMgmt->sBSSList[0]);
-    for (ii = 0, jj = 0; jj < MAX_BSS_NUM ; jj++) {
+	for (ii = 0, jj = 0; jj < MAX_BSS_NUM ; jj++) {
 		if (current_ev >= end_buf)
 			break;
-        pBSS = &(pMgmt->sBSSList[jj]);
-        if (pBSS->bActive) {
-		//ADD mac address
-		    memset(&iwe, 0, sizeof(iwe));
-		    iwe.cmd = SIOCGIWAP;
-		    iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		pBSS = &(pMgmt->sBSSList[jj]);
+		if (pBSS->bActive) {
+			// ADD mac address
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWAP;
+			iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 			memcpy(iwe.u.ap_addr.sa_data, pBSS->abyBSSID, WLAN_BSSID_LEN);
-                           current_ev = iwe_stream_add_event(info,current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
-                 //ADD ssid
-	             memset(&iwe, 0, sizeof(iwe));
-                      iwe.cmd = SIOCGIWESSID;
-                      pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
-                       iwe.u.data.length = pItemSSID->len;
-                       iwe.u.data.flags = 1;
-                      current_ev = iwe_stream_add_point(info,current_ev,end_buf, &iwe, pItemSSID->abySSID);
-		//ADD mode
-		    memset(&iwe, 0, sizeof(iwe));
-		    iwe.cmd = SIOCGIWMODE;
-            if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo)) {
-		        iwe.u.mode = IW_MODE_INFRA;
-            }
-            else {
-                iwe.u.mode = IW_MODE_ADHOC;
-		    }
-	        iwe.len = IW_EV_UINT_LEN;
-                      current_ev = iwe_stream_add_event(info,current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
-           //ADD frequency
-            pSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abySuppRates;
-            pExtSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abyExtSuppRates;
-            memset(&iwe, 0, sizeof(iwe));
-           	iwe.cmd = SIOCGIWFREQ;
-           	iwe.u.freq.m = pBSS->uChannel;
-           	iwe.u.freq.e = 0;
-           	iwe.u.freq.i = 0;
-                  current_ev = iwe_stream_add_event(info,current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_ADDR_LEN);
+			// ADD ssid
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWESSID;
+			pItemSSID = (PWLAN_IE_SSID)pBSS->abySSID;
+			iwe.u.data.length = pItemSSID->len;
+			iwe.u.data.flags = 1;
+			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
+			// ADD mode
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWMODE;
+			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo))
+				iwe.u.mode = IW_MODE_INFRA;
+			else
+				iwe.u.mode = IW_MODE_ADHOC;
+			iwe.len = IW_EV_UINT_LEN;
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
+			// ADD frequency
+			pSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abySuppRates;
+			pExtSuppRates = (PWLAN_IE_SUPP_RATES)pBSS->abyExtSuppRates;
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWFREQ;
+			iwe.u.freq.m = pBSS->uChannel;
+			iwe.u.freq.e = 0;
+			iwe.u.freq.i = 0;
+			current_ev = iwe_stream_add_event(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
 			{
-			int f = (int)pBSS->uChannel - 1;
-			if(f < 0)f = 0;
-			iwe.u.freq.m = frequency_list[f] * 100000;
-			iwe.u.freq.e = 1;
+				int f = (int)pBSS->uChannel - 1;
+				if (f < 0)
+					f = 0;
+				iwe.u.freq.m = frequency_list[f] * 100000;
+				iwe.u.freq.e = 1;
 			}
-                  current_ev = iwe_stream_add_event(info,current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
-       		//ADD quality
-            memset(&iwe, 0, sizeof(iwe));
-	        iwe.cmd = IWEVQUAL;
-	        RFvRSSITodBm(pDevice, (BYTE)(pBSS->uRSSI), &ldBm);
-		    iwe.u.qual.level = ldBm;
-	        iwe.u.qual.noise = 0;
-
-			if(-ldBm<50){
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+			// ADD quality
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVQUAL;
+			RFvRSSITodBm(pDevice, (BYTE)(pBSS->uRSSI), &ldBm);
+			iwe.u.qual.level = ldBm;
+			iwe.u.qual.noise = 0;
+
+			if (-ldBm < 50)
 				iwe.u.qual.qual = 100;
-			}else  if(-ldBm > 90) {
-				 iwe.u.qual.qual = 0;
-			}else {
-				iwe.u.qual.qual=(40-(-ldBm-50))*100/40;
+			else  if (-ldBm > 90)
+				iwe.u.qual.qual = 0;
+			else
+				iwe.u.qual.qual = (40 - (-ldBm - 50)) * 100 / 40;
+			iwe.u.qual.updated = 7;
+
+			current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+			// ADD encryption
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWENCODE;
+			iwe.u.data.length = 0;
+			if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo))
+				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+			else
+				iwe.u.data.flags = IW_ENCODE_DISABLED;
+			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pItemSSID->abySSID);
+
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+			iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+			current_val = current_ev + IW_EV_LCP_LEN;
+
+			for (kk = 0; kk < 12; kk++) {
+				if (pSuppRates->abyRates[kk] == 0)
+					break;
+				// Bit rate given in 500 kb/s units (+ 0x80)
+				iwe.u.bitrate.value = ((pSuppRates->abyRates[kk] & 0x7f) * 500000);
+				current_val = iwe_stream_add_value(info, current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
 			}
-			iwe.u.qual.updated=7;
-
-                 current_ev = iwe_stream_add_event(info,current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
-       	//ADD encryption
-            memset(&iwe, 0, sizeof(iwe));
-            iwe.cmd = SIOCGIWENCODE;
-            iwe.u.data.length = 0;
-            if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo)) {
-                iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-            }else {
-                iwe.u.data.flags = IW_ENCODE_DISABLED;
-            }
-            current_ev = iwe_stream_add_point(info,current_ev,end_buf, &iwe, pItemSSID->abySSID);
-
-            memset(&iwe, 0, sizeof(iwe));
-            iwe.cmd = SIOCGIWRATE;
-           	iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
-      		current_val = current_ev + IW_EV_LCP_LEN;
-
-       		for (kk = 0 ; kk < 12 ; kk++) {
-		        if (pSuppRates->abyRates[kk] == 0)
-			        break;
-		        // Bit rate given in 500 kb/s units (+ 0x80)
-		        iwe.u.bitrate.value = ((pSuppRates->abyRates[kk] & 0x7f) * 500000);
-                          current_val = iwe_stream_add_value(info,current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
-	        }
-       		for (kk = 0 ; kk < 8 ; kk++) {
-		        if (pExtSuppRates->abyRates[kk] == 0)
-			        break;
-		        // Bit rate given in 500 kb/s units (+ 0x80)
-		        iwe.u.bitrate.value = ((pExtSuppRates->abyRates[kk] & 0x7f) * 500000);
-                         current_val = iwe_stream_add_value(info,current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
-	        }
-
-	        if((current_val - current_ev) > IW_EV_LCP_LEN)
-		        current_ev = current_val;
-
-            memset(&iwe, 0, sizeof(iwe));
-            iwe.cmd = IWEVCUSTOM;
-            sprintf(buf, "bcn_int=%d", pBSS->wBeaconInterval);
-            iwe.u.data.length = strlen(buf);
-             current_ev = iwe_stream_add_point(info,current_ev, end_buf, &iwe, buf);
-
-            if ((pBSS->wWPALen > 0) && (pBSS->wWPALen <= MAX_WPA_IE_LEN)) {
-                memset(&iwe, 0, sizeof(iwe));
-                iwe.cmd = IWEVGENIE;
-                iwe.u.data.length = pBSS->wWPALen;
-                current_ev = iwe_stream_add_point(info,current_ev, end_buf, &iwe, pBSS->byWPAIE);
-            }
-
-            if ((pBSS->wRSNLen > 0) && (pBSS->wRSNLen <= MAX_WPA_IE_LEN)) {
-                memset(&iwe, 0, sizeof(iwe));
-                iwe.cmd = IWEVGENIE;
-                iwe.u.data.length = pBSS->wRSNLen;
-                current_ev = iwe_stream_add_point(info,current_ev, end_buf, &iwe, pBSS->byRSNIE);
-            }
-
-        }
-    }// for
+			for (kk = 0; kk < 8; kk++) {
+				if (pExtSuppRates->abyRates[kk] == 0)
+					break;
+				// Bit rate given in 500 kb/s units (+ 0x80)
+				iwe.u.bitrate.value = ((pExtSuppRates->abyRates[kk] & 0x7f) * 500000);
+				current_val = iwe_stream_add_value(info, current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+			}
+
+			if ((current_val - current_ev) > IW_EV_LCP_LEN)
+				current_ev = current_val;
 
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = IWEVCUSTOM;
+			sprintf(buf, "bcn_int=%d", pBSS->wBeaconInterval);
+			iwe.u.data.length = strlen(buf);
+			current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, buf);
+
+			if ((pBSS->wWPALen > 0) && (pBSS->wWPALen <= MAX_WPA_IE_LEN)) {
+				memset(&iwe, 0, sizeof(iwe));
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = pBSS->wWPALen;
+				current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pBSS->byWPAIE);
+			}
+
+			if ((pBSS->wRSNLen > 0) && (pBSS->wRSNLen <= MAX_WPA_IE_LEN)) {
+				memset(&iwe, 0, sizeof(iwe));
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = pBSS->wRSNLen;
+				current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, pBSS->byRSNIE);
+			}
+		}
+	} // for
 	wrq->length = current_ev - extra;
 	return 0;
-
 }
 
-
 /*
- * Wireless Handler : set frequence or channel
+ * Wireless Handler: set frequence or channel
  */
-
-int iwctl_siwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra)
+int iwctl_siwfreq(struct net_device *dev, struct iw_request_info *info,
+		struct iw_freq *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	PSDevice pDevice = netdev_priv(dev);
 	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFREQ \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFREQ \n");
 
 	// If setting by frequency, convert to a channel
-	if((wrq->e == 1) &&
-	   (wrq->m >= (int) 2.412e8) &&
-	   (wrq->m <= (int) 2.487e8)) {
+	if ((wrq->e == 1) && (wrq->m >= (int)2.412e8) &&
+		(wrq->m <= (int)2.487e8)) {
 		int f = wrq->m / 100000;
 		int c = 0;
-		while((c < 14) && (f != frequency_list[c]))
+		while ((c < 14) && (f != frequency_list[c]))
 			c++;
 		wrq->e = 0;
 		wrq->m = c + 1;
 	}
 	// Setting by channel number
-	if((wrq->m > 14) || (wrq->e > 0))
+	if ((wrq->m > 14) || (wrq->e > 0)) {
 		rc = -EOPNOTSUPP;
-	else {
+	} else {
 		int channel = wrq->m;
-		if((channel < 1) || (channel > 14)) {
+		if ((channel < 1) || (channel > 14)) {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: New channel value of %d is invalid!\n", dev->name, wrq->m);
 			rc = -EINVAL;
 		} else {
-			  // Yes ! We can set it !!!
-              DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set to channel = %d\n", channel);
-			  pDevice->uChannel = channel;
+			// Yes ! We can set it !!!
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set to channel = %d\n", channel);
+			pDevice->uChannel = channel;
 		}
 	}
-
 	return rc;
 }
 
 /*
- * Wireless Handler : get frequence or channel
+ * Wireless Handler: get frequence or channel
  */
-
-int iwctl_giwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra)
+int iwctl_giwfreq(struct net_device *dev, struct iw_request_info *info,
+		struct iw_freq *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFREQ \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFREQ \n");
 
 #ifdef WEXT_USECHANNELS
 	wrq->m = (int)pMgmt->uCurrChannel;
@@ -390,74 +366,66 @@ int iwctl_giwfreq(struct net_device *dev,
 #else
 	{
 		int f = (int)pMgmt->uCurrChannel - 1;
-		if(f < 0)
-		   f = 0;
+		if (f < 0)
+			f = 0;
 		wrq->m = frequency_list[f] * 100000;
 		wrq->e = 1;
 	}
 #endif
-
 	return 0;
 }
 
 /*
- * Wireless Handler : set operation mode
+ * Wireless Handler: set operation mode
  */
-
-int iwctl_siwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra)
+int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
+		__u32 *wmode, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE \n");
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int rc = 0;
 
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP && pDevice->bEnableHostapd) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Can't set operation mode, hostapd is running \n");
-        return rc;
-    }
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE \n");
 
-	switch(*wmode) {
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP && pDevice->bEnableHostapd) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Can't set operation mode, hostapd is running \n");
+		return rc;
+	}
 
+	switch (*wmode) {
 	case IW_MODE_ADHOC:
-	    if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
-            pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-            if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		        pDevice->bCommit = TRUE;
-   		    }
+		if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
+			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
+				pDevice->bCommit = TRUE;
 		}
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc \n");
 		break;
 	case IW_MODE_AUTO:
 	case IW_MODE_INFRA:
-	    if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
-            pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-            if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		        pDevice->bCommit = TRUE;
-   		    }
+		if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
+			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
+				pDevice->bCommit = TRUE;
 		}
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure \n");
 		break;
 	case IW_MODE_MASTER:
 
-        pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
 		break;
 
-	    if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
-            pMgmt->eConfigMode = WMAC_CONFIG_AP;
-            if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		        pDevice->bCommit = TRUE;
-   		    }
+		if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
+			pMgmt->eConfigMode = WMAC_CONFIG_AP;
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
+				pDevice->bCommit = TRUE;
 		}
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point \n");
 		break;
 
 	case IW_MODE_REPEAT:
-        pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
+		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
 		break;
 	default:
@@ -468,26 +436,22 @@ int iwctl_siwmode(struct net_device *dev,
 }
 
 /*
- * Wireless Handler : get operation mode
+ * Wireless Handler: get operation mode
  */
-
-void iwctl_giwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra)
+void iwctl_giwmode(struct net_device *dev, struct iw_request_info *info,
+		__u32 *wmode, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWMODE \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWMODE \n");
 	// If not managed, assume it's ad-hoc
 	switch (pMgmt->eConfigMode) {
 	case WMAC_CONFIG_ESS_STA:
 		*wmode = IW_MODE_INFRA;
 		break;
 	case WMAC_CONFIG_IBSS_STA:
-        *wmode = IW_MODE_ADHOC;
+		*wmode = IW_MODE_ADHOC;
 		break;
 	case WMAC_CONFIG_AUTO:
 		*wmode = IW_MODE_INFRA;
@@ -500,21 +464,21 @@ void iwctl_giwmode(struct net_device *dev,
 	}
 }
 
-
 /*
- * Wireless Handler : get capability range
+ * Wireless Handler: get capability range
  */
-
-void iwctl_giwrange(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+void iwctl_giwrange(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	struct iw_range *range = (struct iw_range *) extra;
-	int		i,k;
-    BYTE abySupportedRates[13]= {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
+	struct iw_range *range = (struct iw_range *)extra;
+	int i;
+	int k;
+	BYTE abySupportedRates[13] = {
+		0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48,
+		0x60, 0x6C, 0x90
+	};
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRANGE\n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRANGE\n");
 	if (wrq->pointer) {
 		wrq->length = sizeof(struct iw_range);
 		memset(range, 0, sizeof(struct iw_range));
@@ -522,7 +486,7 @@ void iwctl_giwrange(struct net_device *dev,
 		range->max_nwid = 0x0000;
 		range->num_channels = 14;
 		// Should be based on cap_rid.country to give only
-		//  what the current card support
+		// what the current card support
 		k = 0;
 		for (i = 0; i < 14; i++) {
 			range->freq[k].i = i + 1; // List index
@@ -531,14 +495,14 @@ void iwctl_giwrange(struct net_device *dev,
 		}
 		range->num_frequency = k;
 		// Hum... Should put the right values there
-                 range->max_qual.qual = 100;
+		range->max_qual.qual = 100;
 		range->max_qual.level = 0;
 		range->max_qual.noise = 0;
 		range->sensitivity = 255;
 
-		for (i = 0 ; i < 13 ; i++) {
+		for (i = 0; i < 13; i++) {
 			range->bitrate[i] = abySupportedRates[i] * 500000;
-			if(range->bitrate[i] == 0)
+			if (range->bitrate[i] == 0)
 				break;
 		}
 		range->num_bitrates = i;
@@ -546,7 +510,7 @@ void iwctl_giwrange(struct net_device *dev,
 		// Set an indication of the max TCP throughput
 		// in bit/s that we can expect using this interface.
 		//  May be use for QoS stuff... Jean II
-		if(i > 2)
+		if (i > 2)
 			range->throughput = 5 * 1000 * 1000;
 		else
 			range->throughput = 1.5 * 1000 * 1000;
@@ -556,32 +520,30 @@ void iwctl_giwrange(struct net_device *dev,
 		range->min_frag = 256;
 		range->max_frag = 2312;
 
+		// the encoding capabilities
+		range->num_encoding_sizes = 3;
+		// 64(40) bits WEP
+		range->encoding_size[0] = 5;
+		// 128(104) bits WEP
+		range->encoding_size[1] = 13;
+		// 256 bits for WPA-PSK
+		range->encoding_size[2] = 32;
+		// 4 keys are allowed
+		range->max_encoding_tokens = 4;
 
-	    // the encoding capabilities
-	    range->num_encoding_sizes = 3;
-	    // 64(40) bits WEP
-	    range->encoding_size[0] = 5;
-	    // 128(104) bits WEP
-	    range->encoding_size[1] = 13;
-	    // 256 bits for WPA-PSK
-	    range->encoding_size[2] = 32;
-	    // 4 keys are allowed
-	    range->max_encoding_tokens = 4;
-
-	    range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
-		    IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+		range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+			IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
 
 		range->min_pmp = 0;
-		range->max_pmp = 1000000;// 1 secs
+		range->max_pmp = 1000000; // 1 secs
 		range->min_pmt = 0;
-		range->max_pmt = 1000000;// 1 secs
+		range->max_pmt = 1000000; // 1 secs
 		range->pmp_flags = IW_POWER_PERIOD;
 		range->pmt_flags = IW_POWER_TIMEOUT;
 		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT | IW_POWER_ALL_R;
 
 		// Transmit Power - values are in mW
-
-        range->txpower[0] = 100;
+		range->txpower[0] = 100;
 		range->num_txpower = 1;
 		range->txpower_capa = IW_TXPOW_MWATT;
 		range->we_version_source = SUPPORTED_WIRELESS_EXT;
@@ -597,111 +559,97 @@ void iwctl_giwrange(struct net_device *dev,
 		// Note : with or without the (local->rssi), results
 		//  are somewhat different. - Jean II
 		range->avg_qual.qual = 6;
-		range->avg_qual.level = 176;	// -80 dBm
+		range->avg_qual.level = 176; // -80 dBm
 		range->avg_qual.noise = 0;
 	}
 }
 
-
 /*
  * Wireless Handler : set ap mac address
  */
-
-int iwctl_siwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra)
+int iwctl_siwap(struct net_device *dev, struct iw_request_info *info,
+		struct sockaddr *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
-    BYTE                 ZeroBSSID[WLAN_BSSID_LEN]={0x00,0x00,0x00,0x00,0x00,0x00};
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int rc = 0;
+	BYTE ZeroBSSID[WLAN_BSSID_LEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
-   PRINT_K(" SIOCSIWAP \n");
+	PRINT_K(" SIOCSIWAP \n");
 
-	if (wrq->sa_family != ARPHRD_ETHER)
+	if (wrq->sa_family != ARPHRD_ETHER) {
 		rc = -EINVAL;
-	else {
+	} else {
 		memcpy(pMgmt->abyDesireBSSID, wrq->sa_data, 6);
+		// mike: add
+		if ((is_broadcast_ether_addr(pMgmt->abyDesireBSSID)) ||
+			(memcmp(pMgmt->abyDesireBSSID, ZeroBSSID, 6) == 0)) {
+			PRINT_K("SIOCSIWAP:invalid desired BSSID return!\n");
+			return rc;
+		}
+		// mike add: if desired AP is hidden ssid(there are
+		// two same BSSID in list), then ignore,because you
+		// don't known which one to be connect with??
+		{
+			unsigned ii;
+			unsigned uSameBssidNum = 0;
+			for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+				if (pMgmt->sBSSList[ii].bActive &&
+					!compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID,
+							pMgmt->abyDesireBSSID)) {
+					uSameBssidNum++;
+				}
+			}
+			if (uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
+				PRINT_K("SIOCSIWAP:ignore for desired AP in hidden mode\n");
+				return rc;
+			}
+		}
 
-	//mike :add
-	 if ((is_broadcast_ether_addr(pMgmt->abyDesireBSSID)) ||
-	     (memcmp(pMgmt->abyDesireBSSID, ZeroBSSID, 6) == 0)){
-	      PRINT_K("SIOCSIWAP:invalid desired BSSID return!\n");
-               return rc;
-         }
-       //mike add: if desired AP is hidden ssid(there are two same BSSID in list),
-       //                  then ignore,because you don't known which one to be connect with??
-       	{
-		unsigned int ii, uSameBssidNum = 0;
-                  for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-                     if (pMgmt->sBSSList[ii].bActive &&
-			 !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID,
-					     pMgmt->abyDesireBSSID)) {
-                        uSameBssidNum++;
-                     }
-                  }
-	     if(uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
-                 PRINT_K("SIOCSIWAP:ignore for desired AP in hidden mode\n");
-	        return rc;
-	     }
-       	}
-
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-		    pDevice->bCommit = TRUE;
-   		}
+		if (pDevice->flags & DEVICE_FLAGS_OPENED)
+			pDevice->bCommit = TRUE;
 	}
 	return rc;
 }
 
 /*
- * Wireless Handler : get ap mac address
+ * Wireless Handler: get ap mac address
  */
-
-int iwctl_giwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra)
+int iwctl_giwap(struct net_device *dev, struct iw_request_info *info,
+		struct sockaddr *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAP \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAP \n");
 
-    memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
+	memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
 
- if ((pDevice->bLinkPass == FALSE) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
-        memset(wrq->sa_data, 0, 6);
+	if ((pDevice->bLinkPass == FALSE) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
+		memset(wrq->sa_data, 0, 6);
 
-    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-        memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-    }
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
+		memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
 
 	wrq->sa_family = ARPHRD_ETHER;
-
 	return 0;
-
 }
 
-
 /*
- * Wireless Handler : get ap list
+ * Wireless Handler: get ap list
  */
-
-int iwctl_giwaplist(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+int iwctl_giwaplist(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	int ii,jj, rc = 0;
+	int ii;
+	int jj;
+	int rc = 0;
 	struct sockaddr sock[IW_MAX_AP];
 	struct iw_quality qual[IW_MAX_AP];
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST \n");
 	// Only super-user can see AP list
 
 	if (!capable(CAP_NET_ADMIN)) {
@@ -710,15 +658,14 @@ int iwctl_giwaplist(struct net_device *dev,
 	}
 
 	if (wrq->pointer) {
-
 		PKnownBSS pBSS = &(pMgmt->sBSSList[0]);
 
 		for (ii = 0, jj= 0; ii < MAX_BSS_NUM; ii++) {
-		    pBSS = &(pMgmt->sBSSList[ii]);
-            if (!pBSS->bActive)
-                continue;
-            if ( jj >= IW_MAX_AP)
-                break;
+			pBSS = &(pMgmt->sBSSList[ii]);
+			if (!pBSS->bActive)
+				continue;
+			if (jj >= IW_MAX_AP)
+				break;
 			memcpy(sock[jj].sa_data, pBSS->abyBSSID, 6);
 			sock[jj].sa_family = ARPHRD_ETHER;
 			qual[jj].level = pBSS->uRSSI;
@@ -729,151 +676,137 @@ int iwctl_giwaplist(struct net_device *dev,
 
 		wrq->flags = 1; // Should be define'd
 		wrq->length = jj;
-		memcpy(extra, sock, sizeof(struct sockaddr)*jj);
-		memcpy(extra + sizeof(struct sockaddr)*jj, qual, sizeof(struct iw_quality)*jj);
+		memcpy(extra, sock, sizeof(struct sockaddr) * jj);
+		memcpy(extra + sizeof(struct sockaddr) * jj, qual, sizeof(struct iw_quality) * jj);
 	}
-
 	return rc;
 }
 
-
 /*
- * Wireless Handler : set essid
+ * Wireless Handler: set essid
  */
-
-int iwctl_siwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+int iwctl_siwessid(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    PWLAN_IE_SSID       pItemSSID;
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	PWLAN_IE_SSID pItemSSID;
 
-  if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
-        return -EINVAL;
+	if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
+		return -EINVAL;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWESSID :\n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWESSID :\n");
 
-         pDevice->fWPA_Authened = FALSE;
+	pDevice->fWPA_Authened = FALSE;
 	// Check if we asked for `any'
-	if(wrq->flags == 0) {
+	if (wrq->flags == 0) {
 		// Just send an empty SSID list
 		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-                  memset(pMgmt->abyDesireBSSID, 0xFF,6);
-	    PRINT_K("set essid to 'any' \n");
-           #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-             //Unknown desired AP,so here need not associate??
-                  return 0;
-            #endif
+		memset(pMgmt->abyDesireBSSID, 0xFF,6);
+		PRINT_K("set essid to 'any' \n");
+#ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
+		// Unknown desired AP, so here need not associate??
+		return 0;
+#endif
 	} else {
 		// Set the SSID
 		memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-        pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-        pItemSSID->byElementID = WLAN_EID_SSID;
+		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
+		pItemSSID->byElementID = WLAN_EID_SSID;
 
 		memcpy(pItemSSID->abySSID, extra, wrq->length);
-         if (pItemSSID->abySSID[wrq->length - 1] == '\0') {
-           if(wrq->length>0)
-		pItemSSID->len = wrq->length - 1;
-         }
-	else
-	  pItemSSID->len = wrq->length;
-	PRINT_K("set essid to %s \n",pItemSSID->abySSID);
+		if (pItemSSID->abySSID[wrq->length - 1] == '\0') {
+			if (wrq->length>0)
+				pItemSSID->len = wrq->length - 1;
+		} else {
+			pItemSSID->len = wrq->length;
+		}
+		PRINT_K("set essid to %s \n", pItemSSID->abySSID);
 
-     //mike:need clear desiredBSSID
-     if(pItemSSID->len==0) {
-        memset(pMgmt->abyDesireBSSID, 0xFF,6);
-        return 0;
-     }
+		// mike: need clear desiredBSSID
+		if (pItemSSID->len==0) {
+			memset(pMgmt->abyDesireBSSID, 0xFF, 6);
+			return 0;
+		}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
- //Wext wil order another command of siwap to link with desired AP,
- //so here need not associate??
-  if(pDevice->bWPASuppWextEnabled == TRUE)  {
-        /*******search if  in hidden ssid mode ****/
-        {
-           PKnownBSS       pCurr = NULL;
-           BYTE                   abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-	  unsigned int ii, uSameBssidNum = 0;
-
-	  memcpy(abyTmpDesireSSID,pMgmt->abyDesireSSID,sizeof(abyTmpDesireSSID));
-            pCurr = BSSpSearchBSSList(pDevice,
-                                      NULL,
-                                      abyTmpDesireSSID,
-                                      pDevice->eConfigPHYMode
-                                      );
-
-            if (pCurr == NULL){
-               PRINT_K("SIOCSIWESSID:hidden ssid site survey before associate.......\n");
-	      vResetCommandTimer((void *) pDevice);
-	      pMgmt->eScanType = WMAC_SCAN_ACTIVE;
-	      bScheduleCommand((void *) pDevice,
-			       WLAN_CMD_BSSID_SCAN,
-			       pMgmt->abyDesireSSID);
-	      bScheduleCommand((void *) pDevice,
-			       WLAN_CMD_SSID,
-			       pMgmt->abyDesireSSID);
-          }
-	 else {  //mike:to find out if that desired SSID is a hidden-ssid AP ,
-                     //         by means of judging if there are two same BSSID exist in list ?
-                  for (ii = 0; ii < MAX_BSS_NUM; ii++) {
-                     if (pMgmt->sBSSList[ii].bActive &&
-			 !compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID,
-					     pCurr->abyBSSID)) {
-                        uSameBssidNum++;
-                     }
-                  }
-	     if(uSameBssidNum >= 2) {  //hit: desired AP is in hidden ssid mode!!!
-                 PRINT_K("SIOCSIWESSID:hidden ssid directly associate.......\n");
-		 vResetCommandTimer((void *) pDevice);
-	        pMgmt->eScanType = WMAC_SCAN_PASSIVE;          //this scan type,you'll submit scan result!
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_BSSID_SCAN,
-				 pMgmt->abyDesireSSID);
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_SSID,
-				 pMgmt->abyDesireSSID);
-	     }
-	 }
-        }
-     return 0;
-  }
-	     #endif
-
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s \n", pItemSSID->abySSID);
-	}
+		// Wext wil order another command of siwap to link
+		// with desired AP, so here need not associate??
+		if (pDevice->bWPASuppWextEnabled == TRUE)  {
+			/*******search if  in hidden ssid mode ****/
+			PKnownBSS pCurr = NULL;
+			BYTE abyTmpDesireSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
+			unsigned ii;
+			unsigned uSameBssidNum = 0;
+
+			memcpy(abyTmpDesireSSID, pMgmt->abyDesireSSID, sizeof(abyTmpDesireSSID));
+			pCurr = BSSpSearchBSSList(pDevice, NULL,
+						abyTmpDesireSSID,
+						pDevice->eConfigPHYMode);
+
+			if (pCurr == NULL) {
+				PRINT_K("SIOCSIWESSID:hidden ssid site survey before associate.......\n");
+				vResetCommandTimer((void *)pDevice);
+				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
+				bScheduleCommand((void *)pDevice,
+						WLAN_CMD_BSSID_SCAN,
+						pMgmt->abyDesireSSID);
+				bScheduleCommand((void *)pDevice,
+						WLAN_CMD_SSID,
+						pMgmt->abyDesireSSID);
+			} else {  // mike: to find out if that desired SSID is a
+				// hidden-ssid AP, by means of judging if there
+				// are two same BSSID exist in list ?
+				for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+					if (pMgmt->sBSSList[ii].bActive &&
+						!compare_ether_addr(pMgmt->sBSSList[ii].abyBSSID,
+								pCurr->abyBSSID)) {
+						uSameBssidNum++;
+					}
+				}
+				if (uSameBssidNum >= 2) { // hit: desired AP is in hidden ssid mode!!!
+					PRINT_K("SIOCSIWESSID:hidden ssid directly associate.......\n");
+					vResetCommandTimer((void *)pDevice);
+					pMgmt->eScanType = WMAC_SCAN_PASSIVE; // this scan type, you'll submit scan result!
+					bScheduleCommand((void *)pDevice,
+							WLAN_CMD_BSSID_SCAN,
+							pMgmt->abyDesireSSID);
+					bScheduleCommand((void *)pDevice,
+							WLAN_CMD_SSID,
+							pMgmt->abyDesireSSID);
+				}
+			}
+			return 0;
+		}
+#endif
 
-    if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-	    pDevice->bCommit = TRUE;
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s \n", pItemSSID->abySSID);
 	}
 
+	if (pDevice->flags & DEVICE_FLAGS_OPENED)
+		pDevice->bCommit = TRUE;
 
 	return 0;
 }
 
-
 /*
- * Wireless Handler : get essid
+ * Wireless Handler: get essid
  */
-void iwctl_giwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+void iwctl_giwessid(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	PWLAN_IE_SSID pItemSSID;
 
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-	PWLAN_IE_SSID       pItemSSID;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWESSID \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWESSID \n");
 
-	// Note : if wrq->u.data.flags != 0, we should
-	// get the relevant SSID from the SSID list...
+	// Note: if wrq->u.data.flags != 0, we should get the relevant
+	// SSID from the SSID list...
 
 	// Get the current SSID
-    pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-	memcpy(extra, pItemSSID->abySSID , pItemSSID->len);
+	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
+	memcpy(extra, pItemSSID->abySSID, pItemSSID->len);
 	extra[pItemSSID->len] = '\0';
 
         wrq->length = pItemSSID->len;
@@ -881,146 +814,138 @@ void iwctl_giwessid(struct net_device *dev,
 }
 
 /*
- * Wireless Handler : set data rate
+ * Wireless Handler: set data rate
  */
-
-int iwctl_siwrate(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_siwrate(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    int rc = 0;
-	u8	brate = 0;
-	int	i;
-	BYTE abySupportedRates[13]= {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-
+	PSDevice pDevice = netdev_priv(dev);
+	int rc = 0;
+	u8 brate = 0;
+	int i;
+	BYTE abySupportedRates[13] = {
+		0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48,
+		0x60, 0x6C, 0x90
+	};
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRATE \n");
-    if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-        rc = -EINVAL;
-        return rc;
-    }
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRATE \n");
+	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
+		rc = -EINVAL;
+		return rc;
+	}
 
-	// First : get a valid bit rate value
+	// First: get a valid bit rate value
 
 	// Which type of value
-	if((wrq->value < 13) &&
-	   (wrq->value >= 0)) {
+	if ((wrq->value < 13) && (wrq->value >= 0)) {
 		// Setting by rate index
 		// Find value in the magic rate table
 		brate = wrq->value;
 	} else {
 		// Setting by frequency value
-		u8	normvalue = (u8) (wrq->value/500000);
+		u8 normvalue = (u8)(wrq->value/500000);
 
 		// Check if rate is valid
-		for (i = 0 ; i < 13 ; i++) {
-			if(normvalue == abySupportedRates[i]) {
+		for (i = 0; i < 13; i++) {
+			if (normvalue == abySupportedRates[i]) {
 				brate = i;
 				break;
 			}
 		}
 	}
 	// -1 designed the max rate (mostly auto mode)
-	if(wrq->value == -1) {
+	if (wrq->value == -1) {
 		// Get the highest available rate
-		for (i = 0 ; i < 13 ; i++) {
-			if(abySupportedRates[i] == 0)
+		for (i = 0; i < 13; i++) {
+			if (abySupportedRates[i] == 0)
 				break;
 		}
-		if(i != 0)
+		if (i != 0)
 			brate = i - 1;
 
 	}
 	// Check that it is valid
 	// brate is index of abySupportedRates[]
-	if(brate > 13 ) {
+	if (brate > 13 ) {
 		rc = -EINVAL;
 		return rc;
 	}
 
 	// Now, check if we want a fixed or auto value
-	if(wrq->fixed != 0) {
+	if (wrq->fixed != 0) {
 		// Fixed mode
 		// One rate, fixed
 		pDevice->bFixRate = TRUE;
-        if ((pDevice->byBBType == BB_TYPE_11B)&& (brate > 3)) {
-            pDevice->uConnectionRate = 3;
-        }
-        else {
-            pDevice->uConnectionRate = brate;
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fixed to Rate %d \n", pDevice->uConnectionRate);
-        }
-
+		if ((pDevice->byBBType == BB_TYPE_11B) && (brate > 3)) {
+			pDevice->uConnectionRate = 3;
+		} else {
+			pDevice->uConnectionRate = brate;
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fixed to Rate %d \n", pDevice->uConnectionRate);
+		}
+	} else {
+		pDevice->bFixRate = FALSE;
+		pDevice->uConnectionRate = 13;
 	}
-	else {
-        pDevice->bFixRate = FALSE;
-        pDevice->uConnectionRate = 13;
-    }
 
 	return rc;
 }
 
 /*
- * Wireless Handler : get data rate
+ * Wireless Handler: get data rate
  */
-void iwctl_giwrate(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_param *wrq,
-             char *extra)
+void iwctl_giwrate(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRATE \n");
+	{
+		BYTE abySupportedRates[13] = {
+			0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30,
+			0x48, 0x60, 0x6C, 0x90
+		};
+		int brate = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRATE \n");
-    {
-        BYTE abySupportedRates[13]= {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
-	    int brate = 0;
 		if (pDevice->uConnectionRate < 13) {
-	        brate = abySupportedRates[pDevice->uConnectionRate];
-	    }else {
-            if (pDevice->byBBType == BB_TYPE_11B)
-	            brate = 0x16;
-            if (pDevice->byBBType == BB_TYPE_11G)
-	            brate = 0x6C;
-            if (pDevice->byBBType == BB_TYPE_11A)
-	            brate = 0x6C;
-	    }
-
-	    if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-            if (pDevice->byBBType == BB_TYPE_11B)
-	            brate = 0x16;
-            if (pDevice->byBBType == BB_TYPE_11G)
-	            brate = 0x6C;
-            if (pDevice->byBBType == BB_TYPE_11A)
-	            brate = 0x6C;
-	    }
+			brate = abySupportedRates[pDevice->uConnectionRate];
+		} else {
+			if (pDevice->byBBType == BB_TYPE_11B)
+				brate = 0x16;
+			if (pDevice->byBBType == BB_TYPE_11G)
+				brate = 0x6C;
+			if (pDevice->byBBType == BB_TYPE_11A)
+				brate = 0x6C;
+		}
+		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+			if (pDevice->byBBType == BB_TYPE_11B)
+				brate = 0x16;
+			if (pDevice->byBBType == BB_TYPE_11G)
+				brate = 0x6C;
+			if (pDevice->byBBType == BB_TYPE_11A)
+				brate = 0x6C;
+		}
     		if (pDevice->uConnectionRate == 13)
-                brate = abySupportedRates[pDevice->wCurrentRate];
-	    wrq->value = brate * 500000;
-	    // If more than one rate, set auto
-	    if (pDevice->bFixRate == TRUE)
-	        wrq->fixed = TRUE;
-    }
+			brate = abySupportedRates[pDevice->wCurrentRate];
+		wrq->value = brate * 500000;
+		// If more than one rate, set auto
+		if (pDevice->bFixRate == TRUE)
+			wrq->fixed = TRUE;
+	}
 }
 
-
-
 /*
- * Wireless Handler : set rts threshold
+ * Wireless Handler: set rts threshold
  */
-int iwctl_siwrts(struct net_device *dev,
-		 struct iw_param *wrq)
+int iwctl_siwrts(struct net_device *dev, struct iw_param *wrq)
 {
-	PSDevice pDevice = (PSDevice)netdev_priv(dev);
+	PSDevice pDevice = netdev_priv(dev);
 
 	if ((wrq->value < 0 || wrq->value > 2312) && !wrq->disabled)
 		return -EINVAL;
 
 	else if (wrq->disabled)
 		pDevice->wRTSThreshold = 2312;
-
 	else
 		pDevice->wRTSThreshold = wrq->value;
 
@@ -1028,87 +953,68 @@ int iwctl_siwrts(struct net_device *dev,
 }
 
 /*
- * Wireless Handler : get rts
+ * Wireless Handler: get rts
  */
-
-int iwctl_giwrts(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_giwrts(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	PSDevice pDevice = netdev_priv(dev);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRTS \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRTS \n");
 	wrq->value = pDevice->wRTSThreshold;
 	wrq->disabled = (wrq->value >= 2312);
 	wrq->fixed = 1;
-
 	return 0;
 }
 
 /*
- * Wireless Handler : set fragment threshold
+ * Wireless Handler: set fragment threshold
  */
-
-int iwctl_siwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_siwfrag(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    int rc = 0;
-    int fthr = wrq->value;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFRAG \n");
+	PSDevice pDevice = netdev_priv(dev);
+	int rc = 0;
+	int fthr = wrq->value;
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFRAG \n");
 
-    if (wrq->disabled)
+	if (wrq->disabled)
 		fthr = 2312;
-    if((fthr < 256) || (fthr > 2312)) {
+	if ((fthr < 256) || (fthr > 2312)) {
 		rc = -EINVAL;
-    }else {
-		 fthr &= ~0x1;	// Get an even value
-	     pDevice->wFragmentationThreshold = (u16)fthr;
-    }
-
+	} else {
+		fthr &= ~0x1; // Get an even value
+		pDevice->wFragmentationThreshold = (u16)fthr;
+	}
 	return rc;
 }
 
 /*
- * Wireless Handler : get fragment threshold
+ * Wireless Handler: get fragment threshold
  */
-
-int iwctl_giwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_giwfrag(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	PSDevice pDevice = netdev_priv(dev);
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFRAG \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFRAG \n");
 	wrq->value = pDevice->wFragmentationThreshold;
 	wrq->disabled = (wrq->value >= 2312);
 	wrq->fixed = 1;
-
 	return 0;
 }
 
-
-
 /*
- * Wireless Handler : set retry threshold
+ * Wireless Handler: set retry threshold
  */
-int iwctl_siwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_siwretry(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    int rc = 0;
-
+	PSDevice pDevice = netdev_priv(dev);
+	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRETRY \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRETRY \n");
 
 	if (wrq->disabled) {
 		rc = -EINVAL;
@@ -1116,238 +1022,215 @@ int iwctl_siwretry(struct net_device *dev,
 	}
 
 	if (wrq->flags & IW_RETRY_LIMIT) {
-		if(wrq->flags & IW_RETRY_MAX)
+		if (wrq->flags & IW_RETRY_MAX) {
 			pDevice->byLongRetryLimit = wrq->value;
-		else if (wrq->flags & IW_RETRY_MIN)
+		} else if (wrq->flags & IW_RETRY_MIN) {
 			pDevice->byShortRetryLimit = wrq->value;
-		else {
+		} else {
 			// No modifier : set both
 			pDevice->byShortRetryLimit = wrq->value;
 			pDevice->byLongRetryLimit = wrq->value;
 		}
 	}
-	if (wrq->flags & IW_RETRY_LIFETIME) {
+	if (wrq->flags & IW_RETRY_LIFETIME)
 		pDevice->wMaxTransmitMSDULifetime = wrq->value;
-	}
-
-
 	return rc;
 }
 
 /*
- * Wireless Handler : get retry threshold
+ * Wireless Handler: get retry threshold
  */
-int iwctl_giwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_giwretry(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRETRY \n");
-	wrq->disabled = 0;      // Can't be disabled
+	PSDevice pDevice = netdev_priv(dev);
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRETRY \n");
+	wrq->disabled = 0; // Can't be disabled
 
-	// Note : by default, display the min retry number
-	if((wrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME) {
+	// Note: by default, display the min retry number
+	if ((wrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME) {
 		wrq->flags = IW_RETRY_LIFETIME;
-		wrq->value = (int)pDevice->wMaxTransmitMSDULifetime; //ms
-	} else if((wrq->flags & IW_RETRY_MAX)) {
+		wrq->value = (int)pDevice->wMaxTransmitMSDULifetime; // ms
+	} else if ((wrq->flags & IW_RETRY_MAX)) {
 		wrq->flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
 		wrq->value = (int)pDevice->byLongRetryLimit;
 	} else {
 		wrq->flags = IW_RETRY_LIMIT;
 		wrq->value = (int)pDevice->byShortRetryLimit;
-		if((int)pDevice->byShortRetryLimit != (int)pDevice->byLongRetryLimit)
+		if ((int)pDevice->byShortRetryLimit != (int)pDevice->byLongRetryLimit)
 			wrq->flags |= IW_RETRY_MIN;
 	}
-
-
 	return 0;
 }
 
-
 /*
- * Wireless Handler : set encode mode
+ * Wireless Handler: set encode mode
  */
-int iwctl_siwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 	DWORD dwKeyIndex = (DWORD)(wrq->flags & IW_ENCODE_INDEX);
-	int ii,uu, rc = 0;
+	int ii;
+	int uu;
+	int rc = 0;
 	int index = (wrq->flags & IW_ENCODE_INDEX);
 
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE \n");
 
 	// Check the size of the key
 	if (wrq->length > WLAN_WEP232_KEYLEN) {
 		rc = -EINVAL;
-        return rc;
+		return rc;
 	}
 
 	if (dwKeyIndex > WLAN_WEP_NKEYS) {
 		rc = -EINVAL;
-        return rc;
-    }
+		return rc;
+	}
 
-    if (dwKeyIndex > 0)
+	if (dwKeyIndex > 0)
 		dwKeyIndex--;
 
 	// Send the key to the card
 	if (wrq->length > 0) {
-
-        if (wrq->length ==  WLAN_WEP232_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
-        }
-        else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
-        }
-        else if (wrq->length == WLAN_WEP40_KEYLEN) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
-        }
-        memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
-        memcpy(pDevice->abyKey, extra, wrq->length);
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"abyKey: ");
-        for (ii = 0; ii < wrq->length; ii++) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
-        }
-
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-            spin_lock_irq(&pDevice->lock);
-            KeybSetDefaultKey(  pDevice,
-                                &(pDevice->sKey),
-                                dwKeyIndex | (1 << 31),
-                                wrq->length,
-                                NULL,
-                                pDevice->abyKey,
-                                KEY_CTL_WEP
-                              );
-            spin_unlock_irq(&pDevice->lock);
-        }
-        pDevice->byKeyIndex = (BYTE)dwKeyIndex;
-        pDevice->uKeyLength = wrq->length;
-        pDevice->bTransmitKey = TRUE;
-        pDevice->bEncryptionEnable = TRUE;
-        pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-
-		// Do we want to just set the transmit key index ?
-		if ( index < 4 ) {
-		    pDevice->byKeyIndex = index;
+		if (wrq->length == WLAN_WEP232_KEYLEN) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
+		} else if (wrq->length == WLAN_WEP104_KEYLEN) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
+		} else if (wrq->length == WLAN_WEP40_KEYLEN) {
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
+		}
+		memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
+		memcpy(pDevice->abyKey, extra, wrq->length);
+
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"abyKey: ");
+		for (ii = 0; ii < wrq->length; ii++)
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
+
+		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			spin_lock_irq(&pDevice->lock);
+			KeybSetDefaultKey(pDevice,
+					&(pDevice->sKey),
+					dwKeyIndex | (1 << 31),
+					wrq->length, NULL,
+					pDevice->abyKey,
+					KEY_CTL_WEP);
+			spin_unlock_irq(&pDevice->lock);
+		}
+		pDevice->byKeyIndex = (BYTE)dwKeyIndex;
+		pDevice->uKeyLength = wrq->length;
+		pDevice->bTransmitKey = TRUE;
+		pDevice->bEncryptionEnable = TRUE;
+		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
+
+		// Do we want to just set the transmit key index?
+		if (index < 4) {
+			pDevice->byKeyIndex = index;
 		} else if (!(wrq->flags & IW_ENCODE_MODE)) {
-				rc = -EINVAL;
-				return rc;
-	    }
+			rc = -EINVAL;
+			return rc;
+		}
 	}
 	// Read the flags
-	if(wrq->flags & IW_ENCODE_DISABLED){
-
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
+	if (wrq->flags & IW_ENCODE_DISABLED) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
 		pMgmt->bShareKeyAlgorithm = FALSE;
-        pDevice->bEncryptionEnable = FALSE;
-        pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-        if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-            spin_lock_irq(&pDevice->lock);
-	    for (uu = 0; uu < MAX_KEY_TABLE; uu++)
-		MACvDisableKeyEntry(pDevice, uu);
-            spin_unlock_irq(&pDevice->lock);
-        }
+		pDevice->bEncryptionEnable = FALSE;
+		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			spin_lock_irq(&pDevice->lock);
+			for (uu = 0; uu < MAX_KEY_TABLE; uu++)
+				MACvDisableKeyEntry(pDevice, uu);
+			spin_unlock_irq(&pDevice->lock);
+		}
 	}
-	if(wrq->flags & IW_ENCODE_RESTRICTED) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & ShareKey System\n");
+	if (wrq->flags & IW_ENCODE_RESTRICTED) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & ShareKey System\n");
 		pMgmt->bShareKeyAlgorithm = TRUE;
 	}
-	if(wrq->flags & IW_ENCODE_OPEN) {
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & Open System\n");
+	if (wrq->flags & IW_ENCODE_OPEN) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & Open System\n");
 		pMgmt->bShareKeyAlgorithm = FALSE;
 	}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	   memset(pMgmt->abyDesireBSSID, 0xFF,6);
+	memset(pMgmt->abyDesireBSSID, 0xFF, 6);
 #endif
-
 	return rc;
 }
 
-int iwctl_giwencode(struct net_device *dev,
-			struct iw_request_info *info,
-			struct iw_point *wrq,
-			char *extra)
+int iwctl_giwencode(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
-	PSMgmtObject		pMgmt = &(pDevice->sMgmtObj);
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 	char abyKey[WLAN_WEP232_KEYLEN];
 
-	unsigned int index = (unsigned int)(wrq->flags & IW_ENCODE_INDEX);
-	PSKeyItem	pKey = NULL;
+	unsigned index = (unsigned)(wrq->flags & IW_ENCODE_INDEX);
+	PSKeyItem pKey = NULL;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODE\n");
 
-	if (index > WLAN_WEP_NKEYS) {
+	if (index > WLAN_WEP_NKEYS)
 		return	-EINVAL;
+	if (index < 1) { // get default key
+		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
+			index = pDevice->byKeyIndex;
+         	else
+			index = 0;
+	} else {
+		index--;
 	}
-	if(index<1){//get default key
-		if(pDevice->byKeyIndex<WLAN_WEP_NKEYS){
-			index=pDevice->byKeyIndex;
-         	} else
-                      index=0;
-	}else
-             index--;
 
 	memset(abyKey, 0, WLAN_WEP232_KEYLEN);
 	// Check encryption mode
 	wrq->flags = IW_ENCODE_NOKEY;
 	// Is WEP enabled ???
 	if (pDevice->bEncryptionEnable)
-		wrq->flags |=  IW_ENCODE_ENABLED;
+		wrq->flags |= IW_ENCODE_ENABLED;
 	else
-		wrq->flags |=  IW_ENCODE_DISABLED;
+		wrq->flags |= IW_ENCODE_DISABLED;
 
 	if (pMgmt->bShareKeyAlgorithm)
-		wrq->flags |=  IW_ENCODE_RESTRICTED;
+		wrq->flags |= IW_ENCODE_RESTRICTED;
 	else
-		wrq->flags |=  IW_ENCODE_OPEN;
-		wrq->length=0;
-
-	if((index==0)&&(pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled||
-		pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)){//get wpa pairwise  key
-			if (KeybGetKey(&(pDevice->sKey),pMgmt->abyCurrBSSID, 0xffffffff, &pKey)){
-			   wrq->length = pKey->uKeyLength;
-				  memcpy(abyKey, pKey->abyKey,	pKey->uKeyLength);
-				  memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
-			   }
-	}else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (BYTE)index , &pKey)){
+		wrq->flags |= IW_ENCODE_OPEN;
+	wrq->length = 0;
+
+	if ((index == 0) && (pDevice->eEncryptionStatus == Ndis802_11Encryption2Enabled ||
+				pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled)) { // get wpa pairwise  key
+		if (KeybGetKey(&(pDevice->sKey), pMgmt->abyCurrBSSID, 0xffffffff, &pKey)) {
 			wrq->length = pKey->uKeyLength;
-			memcpy(abyKey, pKey->abyKey,  pKey->uKeyLength);
-		memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
+			memcpy(abyKey, pKey->abyKey,	pKey->uKeyLength);
+			memcpy(extra,  abyKey, WLAN_WEP232_KEYLEN);
+		}
+	} else if (KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, (BYTE)index, &pKey)) {
+		wrq->length = pKey->uKeyLength;
+		memcpy(abyKey, pKey->abyKey, pKey->uKeyLength);
+		memcpy(extra, abyKey, WLAN_WEP232_KEYLEN);
 	}
 
-	wrq->flags |= index+1;
-
+	wrq->flags |= index + 1;
 	return 0;
 }
 
-
 /*
- * Wireless Handler : set power mode
+ * Wireless Handler: set power mode
  */
-int iwctl_siwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_siwpower(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice            pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int rc = 0;
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int rc = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER \n");
 
-    if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
-		 rc = -EINVAL;
-		 return rc;
+	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
+		rc = -EINVAL;
+		return rc;
 	}
 
 	if (wrq->disabled) {
@@ -1356,23 +1239,23 @@ int iwctl_siwpower(struct net_device *dev,
 		return rc;
 	}
 	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
-         pDevice->ePSMode = WMAC_POWER_FAST;
-	 PSvEnablePowerSaving((void *) pDevice, pMgmt->wListenInterval);
+		pDevice->ePSMode = WMAC_POWER_FAST;
+		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 
 	} else if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_PERIOD) {
-	     pDevice->ePSMode = WMAC_POWER_FAST;
-	     PSvEnablePowerSaving((void *) pDevice, pMgmt->wListenInterval);
+		pDevice->ePSMode = WMAC_POWER_FAST;
+		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 	}
 	switch (wrq->flags & IW_POWER_MODE) {
 	case IW_POWER_UNICAST_R:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R \n");
 		rc = -EINVAL;
 		break;
 	case IW_POWER_ALL_R:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ALL_R \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ALL_R \n");
 		rc = -EINVAL;
 	case IW_POWER_ON:
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ON \n");
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ON \n");
 		break;
 	default:
 		rc = -EINVAL;
@@ -1382,23 +1265,19 @@ int iwctl_siwpower(struct net_device *dev,
 }
 
 /*
- * Wireless Handler : get power mode
+ * Wireless Handler: get power mode
  */
-int iwctl_giwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra)
+int iwctl_giwpower(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice            pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
-    int mode = pDevice->ePSMode;
-
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPOWER \n");
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int mode = pDevice->ePSMode;
 
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPOWER \n");
 
 	if ((wrq->disabled = (mode == WMAC_POWER_CAM)))
-	    return 0;
+		return 0;
 
 	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
 		wrq->value = (int)((pMgmt->wListenInterval * pMgmt->wCurrBeaconPeriod) << 10);
@@ -1408,117 +1287,105 @@ int iwctl_giwpower(struct net_device *dev,
 		wrq->flags = IW_POWER_PERIOD;
 	}
 	wrq->flags |= IW_POWER_ALL_R;
-
 	return 0;
 }
 
-
 /*
- * Wireless Handler : get Sensitivity
+ * Wireless Handler: get Sensitivity
  */
-int iwctl_giwsens(struct net_device *dev,
-			 struct iw_request_info *info,
-			 struct iw_param *wrq,
-			 char *extra)
+int iwctl_giwsens(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    long ldBm;
+	PSDevice pDevice = netdev_priv(dev);
+	long ldBm;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS \n");
-    if (pDevice->bLinkPass == TRUE) {
-        RFvRSSITodBm(pDevice, (BYTE)(pDevice->uCurrRSSI), &ldBm);
-	    wrq->value = ldBm;
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS \n");
+	if (pDevice->bLinkPass == TRUE) {
+		RFvRSSITodBm(pDevice, (BYTE)(pDevice->uCurrRSSI), &ldBm);
+		wrq->value = ldBm;
+	} else {
+		wrq->value = 0;
 	}
-	else {
-	    wrq->value = 0;
-    };
 	wrq->disabled = (wrq->value == 0);
 	wrq->fixed = 1;
-
-
 	return 0;
 }
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 
-int iwctl_siwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra)
+int iwctl_siwauth(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret=0;
-	static int wpa_version=0;  //must be static to save the last value,einsn liu
-	static int pairwise=0;
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int ret = 0;
+	static int wpa_version = 0; // must be static to save the last value, einsn liu
+	static int pairwise = 0;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH \n");
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH \n");
 	switch (wrq->flags & IW_AUTH_INDEX) {
 	case IW_AUTH_WPA_VERSION:
 		wpa_version = wrq->value;
-		if(wrq->value == IW_AUTH_WPA_VERSION_DISABLED) {
-		       PRINT_K("iwctl_siwauth:set WPADEV to disable at 1??????\n");
-		}
-		else if(wrq->value == IW_AUTH_WPA_VERSION_WPA) {
-                          PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
-		}
-		else {
-                          PRINT_K("iwctl_siwauth:set WPADEV to WPA2******\n");
+		if (wrq->value == IW_AUTH_WPA_VERSION_DISABLED) {
+			PRINT_K("iwctl_siwauth:set WPADEV to disable at 1??????\n");
+		} else if (wrq->value == IW_AUTH_WPA_VERSION_WPA) {
+			PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
+		} else {
+			PRINT_K("iwctl_siwauth:set WPADEV to WPA2******\n");
 		}
 		break;
 	case IW_AUTH_CIPHER_PAIRWISE:
 		pairwise = wrq->value;
-                   PRINT_K("iwctl_siwauth:set pairwise=%d\n",pairwise);
-		if(pairwise == IW_AUTH_CIPHER_CCMP){
+		PRINT_K("iwctl_siwauth:set pairwise=%d\n", pairwise);
+		if (pairwise == IW_AUTH_CIPHER_CCMP){
 			pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-		}else if(pairwise == IW_AUTH_CIPHER_TKIP){
+		} else if (pairwise == IW_AUTH_CIPHER_TKIP) {
 			pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
 		} else if (pairwise == IW_AUTH_CIPHER_WEP40 ||
-			   pairwise == IW_AUTH_CIPHER_WEP104) {
+			pairwise == IW_AUTH_CIPHER_WEP104) {
 			pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
-		}else if(pairwise == IW_AUTH_CIPHER_NONE){
-			//do nothing,einsn liu
-		}else pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-
+		} else if (pairwise == IW_AUTH_CIPHER_NONE) {
+			// do nothing, einsn liu
+		} else {
+			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		}
 		break;
 	case IW_AUTH_CIPHER_GROUP:
-		 PRINT_K("iwctl_siwauth:set GROUP=%d\n",wrq->value);
-		if(wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
+		PRINT_K("iwctl_siwauth:set GROUP=%d\n", wrq->value);
+		if (wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
 			break;
-		if(pairwise == IW_AUTH_CIPHER_NONE){
-			if(wrq->value == IW_AUTH_CIPHER_CCMP){
+		if (pairwise == IW_AUTH_CIPHER_NONE) {
+			if (wrq->value == IW_AUTH_CIPHER_CCMP)
 				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-			}else {
+			else
 				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-			}
 		}
 		break;
 	case IW_AUTH_KEY_MGMT:
-                    PRINT_K("iwctl_siwauth(wpa_version=%d):set KEY_MGMT=%d\n",wpa_version,wrq->value);
-		if(wpa_version == IW_AUTH_WPA_VERSION_WPA2){
-			if(wrq->value == IW_AUTH_KEY_MGMT_PSK)
+		PRINT_K("iwctl_siwauth(wpa_version=%d):set KEY_MGMT=%d\n", wpa_version,wrq->value);
+		if (wpa_version == IW_AUTH_WPA_VERSION_WPA2){
+			if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPA2PSK;
 			else pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
-		}else if(wpa_version == IW_AUTH_WPA_VERSION_WPA){
-			if(wrq->value == 0){
+		} else if (wpa_version == IW_AUTH_WPA_VERSION_WPA) {
+			if (wrq->value == 0){
 				pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-			}else if(wrq->value == IW_AUTH_KEY_MGMT_PSK)
+			} else if (wrq->value == IW_AUTH_KEY_MGMT_PSK)
 				pMgmt->eAuthenMode = WMAC_AUTH_WPAPSK;
-			else pMgmt->eAuthenMode = WMAC_AUTH_WPA;
+		} else {
+			pMgmt->eAuthenMode = WMAC_AUTH_WPA;
 		}
-
 		break;
 	case IW_AUTH_TKIP_COUNTERMEASURES:
-		break;		/* FIXME */
+		break; /* FIXME */
 	case IW_AUTH_DROP_UNENCRYPTED:
 		break;
 	case IW_AUTH_80211_AUTH_ALG:
-		 PRINT_K("iwctl_siwauth:set AUTH_ALG=%d\n",wrq->value);
-		if(wrq->value==IW_AUTH_ALG_OPEN_SYSTEM){
-			pMgmt->bShareKeyAlgorithm=FALSE;
-		}else if(wrq->value==IW_AUTH_ALG_SHARED_KEY){
-			pMgmt->bShareKeyAlgorithm=TRUE;
-		}
+		PRINT_K("iwctl_siwauth:set AUTH_ALG=%d\n", wrq->value);
+		if (wrq->value == IW_AUTH_ALG_OPEN_SYSTEM)
+			pMgmt->bShareKeyAlgorithm = FALSE;
+		else if (wrq->value == IW_AUTH_ALG_SHARED_KEY)
+			pMgmt->bShareKeyAlgorithm = TRUE;
 		break;
 	case IW_AUTH_WPA_ENABLED:
 		break;
@@ -1529,345 +1396,314 @@ int iwctl_siwauth(struct net_device *dev,
 		break;
 	case IW_AUTH_PRIVACY_INVOKED:
 		pDevice->bEncryptionEnable = !!wrq->value;
-		if(pDevice->bEncryptionEnable == FALSE){
+		if (pDevice->bEncryptionEnable == FALSE) {
 			wpa_version = 0;
 			pairwise = 0;
 			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 			pMgmt->bShareKeyAlgorithm = FALSE;
 			pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-			 PRINT_K("iwctl_siwauth:set WPADEV to disaable at 2?????\n");
+			PRINT_K("iwctl_siwauth:set WPADEV to disaable at 2?????\n");
 		}
-
 		break;
 	default:
 		ret = -EOPNOTSUPP;
 		break;
 	}
-   return ret;
+	return ret;
 }
 
-
-int iwctl_giwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra)
+int iwctl_giwauth(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra)
 {
 	return -EOPNOTSUPP;
 }
 
-
-
-int iwctl_siwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra)
+int iwctl_siwgenie(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret=0;
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int ret = 0;
 
-	if(wrq->length){
-		if ((wrq->length < 2) || (extra[1]+2 != wrq->length)) {
+	if (wrq->length){
+		if ((wrq->length < 2) || (extra[1] + 2 != wrq->length)) {
 			ret = -EINVAL;
 			goto out;
 		}
-		if(wrq->length > MAX_WPA_IE_LEN){
+		if (wrq->length > MAX_WPA_IE_LEN){
 			ret = -ENOMEM;
 			goto out;
 		}
 		memset(pMgmt->abyWPAIE, 0, MAX_WPA_IE_LEN);
-		if(copy_from_user(pMgmt->abyWPAIE, extra, wrq->length)){
+		if (copy_from_user(pMgmt->abyWPAIE, extra, wrq->length)){
 			ret = -EFAULT;
 			goto out;
 		}
 		pMgmt->wWPAIELen = wrq->length;
-	}else {
+	} else {
 		memset(pMgmt->abyWPAIE, 0, MAX_WPA_IE_LEN);
 		pMgmt->wWPAIELen = 0;
 	}
 
-	out://not completely ...not necessary in wpa_supplicant 0.5.8
+out: // not completely ...not necessary in wpa_supplicant 0.5.8
 	return ret;
 }
 
-int iwctl_giwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra)
+int iwctl_giwgenie(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
-	int ret=0;
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+	int ret = 0;
 	int space = wrq->length;
 
 	wrq->length = 0;
-	if(pMgmt->wWPAIELen > 0){
+	if (pMgmt->wWPAIELen > 0) {
 		wrq->length = pMgmt->wWPAIELen;
-		if(pMgmt->wWPAIELen <= space){
-			if(copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen)){
+		if (pMgmt->wWPAIELen <= space) {
+			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen)) {
 				ret = -EFAULT;
 			}
-		}else
+		} else {
 			ret = -E2BIG;
+		}
 	}
-
 	return ret;
 }
 
-
-int iwctl_siwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+int iwctl_siwencodeext(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-    PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-    PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 	struct iw_encode_ext *ext = (struct iw_encode_ext*)extra;
-    struct viawget_wpa_param *param=NULL;
-//original member
-    wpa_alg alg_name;
-    u8  addr[6];
-    int key_idx, set_tx=0;
-    u8  seq[IW_ENCODE_SEQ_MAX_SIZE];
-    u8 key[64];
-    size_t seq_len=0,key_len=0;
-    u8 *buf;
-    size_t blen;
-    u8 key_array[64];
-    int ret=0;
-
-PRINT_K("SIOCSIWENCODEEXT...... \n");
-
-blen = sizeof(*param);
-buf = kmalloc((int)blen, (int)GFP_KERNEL);
-if (buf == NULL)
-    return -ENOMEM;
-memset(buf, 0, blen);
-param = (struct viawget_wpa_param *) buf;
-
-//recover alg_name
-switch (ext->alg) {
-    case IW_ENCODE_ALG_NONE:
-                  alg_name = WPA_ALG_NONE;
+	struct viawget_wpa_param *param=NULL;
+// original member
+	wpa_alg alg_name;
+	u8 addr[6];
+	int key_idx;
+	int set_tx = 0;
+	u8 seq[IW_ENCODE_SEQ_MAX_SIZE];
+	u8 key[64];
+	size_t seq_len = 0;
+	size_t key_len = 0;
+	u8 *buf;
+	size_t blen;
+	u8 key_array[64];
+	int ret = 0;
+
+	PRINT_K("SIOCSIWENCODEEXT...... \n");
+
+	blen = sizeof(*param);
+	buf = kmalloc((int)blen, (int)GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+	memset(buf, 0, blen);
+	param = (struct viawget_wpa_param *)buf;
+
+// recover alg_name
+	switch (ext->alg) {
+	case IW_ENCODE_ALG_NONE:
+		alg_name = WPA_ALG_NONE;
 		break;
-    case IW_ENCODE_ALG_WEP:
-                  alg_name = WPA_ALG_WEP;
+	case IW_ENCODE_ALG_WEP:
+		alg_name = WPA_ALG_WEP;
 		break;
-    case IW_ENCODE_ALG_TKIP:
-                  alg_name = WPA_ALG_TKIP;
+	case IW_ENCODE_ALG_TKIP:
+		alg_name = WPA_ALG_TKIP;
 		break;
-    case IW_ENCODE_ALG_CCMP:
-                  alg_name = WPA_ALG_CCMP;
+	case IW_ENCODE_ALG_CCMP:
+		alg_name = WPA_ALG_CCMP;
 		break;
-    default:
+	default:
 		PRINT_K("Unknown alg = %d\n",ext->alg);
 		ret= -ENOMEM;
 		goto error;
-		}
-//recover addr
- memcpy(addr, ext->addr.sa_data, ETH_ALEN);
-//recover key_idx
-  key_idx = (wrq->flags&IW_ENCODE_INDEX) - 1;
-//recover set_tx
-if(ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
-   set_tx = 1;
-//recover seq,seq_len
-	if(ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
-   seq_len=IW_ENCODE_SEQ_MAX_SIZE;
-   memcpy(seq, ext->rx_seq, seq_len);
-		}
-//recover key,key_len
-if(ext->key_len) {
-  key_len=ext->key_len;
-  memcpy(key, &ext->key[0], key_len);
 	}
-
-memset(key_array, 0, 64);
-if ( key_len > 0) {
-     memcpy(key_array, key, key_len);
-    if (key_len == 32) {
-          // notice ! the oder
-	  memcpy(&key_array[16], &key[24], 8);
-	  memcpy(&key_array[24], &key[16], 8);
+// recover addr
+	memcpy(addr, ext->addr.sa_data, ETH_ALEN);
+// recover key_idx
+	key_idx = (wrq->flags&IW_ENCODE_INDEX) - 1;
+// recover set_tx
+	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		set_tx = 1;
+// recover seq,seq_len
+	if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+		seq_len=IW_ENCODE_SEQ_MAX_SIZE;
+		memcpy(seq, ext->rx_seq, seq_len);
 	}
+// recover key,key_len
+	if (ext->key_len) {
+		key_len = ext->key_len;
+		memcpy(key, &ext->key[0], key_len);
+	}
+	memset(key_array, 0, 64);
+	if (key_len > 0) {
+		memcpy(key_array, key, key_len);
+		if (key_len == 32) {
+			// notice ! the oder
+			memcpy(&key_array[16], &key[24], 8);
+			memcpy(&key_array[24], &key[16], 8);
+		}
 	}
 
 /**************Translate iw_encode_ext to viawget_wpa_param****************/
-memcpy(param->addr, addr, ETH_ALEN);
-param->u.wpa_key.alg_name = (int)alg_name;
-param->u.wpa_key.set_tx = set_tx;
-param->u.wpa_key.key_index = key_idx;
-param->u.wpa_key.key_len = key_len;
-param->u.wpa_key.key = (u8 *)key_array;
-param->u.wpa_key.seq = (u8 *)seq;
-param->u.wpa_key.seq_len = seq_len;
-
-//****set if current action is Network Manager count??
-//****this method is so foolish,but there is no other way???
-if(param->u.wpa_key.alg_name == WPA_ALG_NONE) {
-   if(param->u.wpa_key.key_index ==0) {
-     pDevice->bwextstep0 = TRUE;
-    }
-   if((pDevice->bwextstep0 = TRUE)&&(param->u.wpa_key.key_index ==1)) {
-     pDevice->bwextstep0 = FALSE;
-     pDevice->bwextstep1 = TRUE;
-    }
-   if((pDevice->bwextstep1 = TRUE)&&(param->u.wpa_key.key_index ==2)) {
-     pDevice->bwextstep1 = FALSE;
-     pDevice->bwextstep2 = TRUE;
+	memcpy(param->addr, addr, ETH_ALEN);
+	param->u.wpa_key.alg_name = (int)alg_name;
+	param->u.wpa_key.set_tx = set_tx;
+	param->u.wpa_key.key_index = key_idx;
+	param->u.wpa_key.key_len = key_len;
+	param->u.wpa_key.key = (u8 *)key_array;
+	param->u.wpa_key.seq = (u8 *)seq;
+	param->u.wpa_key.seq_len = seq_len;
+
+/****set if current action is Network Manager count?? */
+/****this method is so foolish,but there is no other way??? */
+	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
+		if (param->u.wpa_key.key_index ==0) {
+			pDevice->bwextstep0 = TRUE;
+		}
+		if ((pDevice->bwextstep0 == TRUE) && (param->u.wpa_key.key_index == 1)) {
+			pDevice->bwextstep0 = FALSE;
+			pDevice->bwextstep1 = TRUE;
+		}
+		if ((pDevice->bwextstep1 == TRUE) && (param->u.wpa_key.key_index == 2)) {
+			pDevice->bwextstep1 = FALSE;
+			pDevice->bwextstep2 = TRUE;
+		}
+		if ((pDevice->bwextstep2 == TRUE) && (param->u.wpa_key.key_index == 3)) {
+			pDevice->bwextstep2 = FALSE;
+			pDevice->bwextstep3 = TRUE;
+		}
+	}
+	if (pDevice->bwextstep3 == TRUE) {
+		PRINT_K("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
+		pDevice->bwextstep0 = FALSE;
+		pDevice->bwextstep1 = FALSE;
+		pDevice->bwextstep2 = FALSE;
+		pDevice->bwextstep3 = FALSE;
+		pDevice->bWPASuppWextEnabled = TRUE;
+		memset(pMgmt->abyDesireBSSID, 0xFF, 6);
+		KeyvInitTable(pDevice, &pDevice->sKey);
 	}
-   if((pDevice->bwextstep2 = TRUE)&&(param->u.wpa_key.key_index ==3)) {
-     pDevice->bwextstep2 = FALSE;
-     pDevice->bwextstep3 = TRUE;
-        }
-		 }
-if(pDevice->bwextstep3 == TRUE) {
-    PRINT_K("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
-     pDevice->bwextstep0 = FALSE;
-     pDevice->bwextstep1 = FALSE;
-     pDevice->bwextstep2 = FALSE;
-     pDevice->bwextstep3 = FALSE;
-     pDevice->bWPASuppWextEnabled = TRUE;
-     memset(pMgmt->abyDesireBSSID, 0xFF,6);
-     KeyvInitTable(pDevice,&pDevice->sKey);
-		 }
-//******
-
-		spin_lock_irq(&pDevice->lock);
- ret = wpa_set_keys(pDevice, param, TRUE);
-		spin_unlock_irq(&pDevice->lock);
+/*******/
+	spin_lock_irq(&pDevice->lock);
+	ret = wpa_set_keys(pDevice, param, TRUE);
+	spin_unlock_irq(&pDevice->lock);
 
 error:
-kfree(param);
+	kfree(param);
 	return ret;
 }
 
-
-
-int iwctl_giwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra)
+int iwctl_giwencodeext(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-		return -EOPNOTSUPP;
+	return -EOPNOTSUPP;
 }
 
-int iwctl_siwmlme(struct net_device *dev,
-				struct iw_request_info * info,
-				struct iw_point *wrq,
-				char *extra)
+int iwctl_siwmlme(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
-	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
+	PSDevice pDevice = netdev_priv(dev);
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 	struct iw_mlme *mlme = (struct iw_mlme *)extra;
 	int ret = 0;
 
-	if(memcmp(pMgmt->abyCurrBSSID, mlme->addr.sa_data, ETH_ALEN)){
+	if (memcmp(pMgmt->abyCurrBSSID, mlme->addr.sa_data, ETH_ALEN)) {
 		ret = -EINVAL;
 		return ret;
 	}
-	switch(mlme->cmd){
+	switch (mlme->cmd){
 	case IW_MLME_DEAUTH:
 	case IW_MLME_DISASSOC:
-		if(pDevice->bLinkPass == TRUE){
-		  PRINT_K("iwctl_siwmlme--->send DISASSOCIATE\n");
-		  bScheduleCommand((void *) pDevice,
-				   WLAN_CMD_DISASSOCIATE,
-				   NULL);
+		if (pDevice->bLinkPass == TRUE) {
+			PRINT_K("iwctl_siwmlme--->send DISASSOCIATE\n");
+			bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE,
+					NULL);
 		}
 		break;
 	default:
 		ret = -EOPNOTSUPP;
 	}
-
 	return ret;
-
 }
 
 #endif
 
-static const iw_handler		iwctl_handler[] =
-{
-	(iw_handler) NULL,      /* SIOCSIWCOMMIT */
-	(iw_handler) NULL,      // SIOCGIWNAME
-	(iw_handler) NULL,				// SIOCSIWNWID
-	(iw_handler) NULL,				// SIOCGIWNWID
-	(iw_handler) NULL,		// SIOCSIWFREQ
-	(iw_handler) NULL,		// SIOCGIWFREQ
-	(iw_handler) NULL,		// SIOCSIWMODE
-	(iw_handler) NULL,		// SIOCGIWMODE
-	(iw_handler) NULL,		        // SIOCSIWSENS
-	(iw_handler) NULL,		        // SIOCGIWSENS
-	(iw_handler) NULL, 		        // SIOCSIWRANGE
-	(iw_handler) iwctl_giwrange,		// SIOCGIWRANGE
-	(iw_handler) NULL,         		    // SIOCSIWPRIV
-	(iw_handler) NULL,             		// SIOCGIWPRIV
-	(iw_handler) NULL,             		// SIOCSIWSTATS
-	(iw_handler) NULL,                  // SIOCGIWSTATS
-    (iw_handler) NULL,                  // SIOCSIWSPY
-	(iw_handler) NULL,		            // SIOCGIWSPY
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) NULL,				    // -- hole --
-	(iw_handler) NULL,		    // SIOCSIWAP
-	(iw_handler) NULL,		    // SIOCGIWAP
-	(iw_handler) NULL,				    // -- hole -- 0x16
-	(iw_handler) NULL,       // SIOCGIWAPLIST
-	(iw_handler) iwctl_siwscan,         // SIOCSIWSCAN
-	(iw_handler) iwctl_giwscan,         // SIOCGIWSCAN
-	(iw_handler) NULL,		// SIOCSIWESSID
-	(iw_handler) NULL,		// SIOCGIWESSID
-	(iw_handler) NULL,		// SIOCSIWNICKN
-	(iw_handler) NULL,		// SIOCGIWNICKN
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// -- hole --
-	(iw_handler) NULL,		// SIOCSIWRATE 0x20
-	(iw_handler) NULL,		// SIOCGIWRATE
-	(iw_handler) NULL,		// SIOCSIWRTS
-	(iw_handler) NULL,		// SIOCGIWRTS
-	(iw_handler) NULL,		// SIOCSIWFRAG
-	(iw_handler) NULL,		// SIOCGIWFRAG
-	(iw_handler) NULL,		// SIOCSIWTXPOW
-	(iw_handler) NULL,		// SIOCGIWTXPOW
-	(iw_handler) NULL,		// SIOCSIWRETRY
-	(iw_handler) NULL,		// SIOCGIWRETRY
-	(iw_handler) NULL,		// SIOCSIWENCODE
-	(iw_handler) NULL,		// SIOCGIWENCODE
-	(iw_handler) NULL,		// SIOCSIWPOWER
-	(iw_handler) NULL,		// SIOCGIWPOWER
-	(iw_handler) NULL,			// -- hole --
-	(iw_handler) NULL,			// -- hole --
-	(iw_handler) NULL,    // SIOCSIWGENIE
-	(iw_handler) NULL,    // SIOCGIWGENIE
-	(iw_handler) NULL,		// SIOCSIWAUTH
-	(iw_handler) NULL,		// SIOCGIWAUTH
-	(iw_handler) NULL,		// SIOCSIWENCODEEXT
-	(iw_handler) NULL,		// SIOCGIWENCODEEXT
-	(iw_handler) NULL,				// SIOCSIWPMKSA
-	(iw_handler) NULL,				// -- hole --
+static const iw_handler iwctl_handler[] = {
+	(iw_handler)NULL, // SIOCSIWCOMMIT
+	(iw_handler)NULL, // SIOCGIWNAME
+	(iw_handler)NULL, // SIOCSIWNWID
+	(iw_handler)NULL, // SIOCGIWNWID
+	(iw_handler)NULL, // SIOCSIWFREQ
+	(iw_handler)NULL, // SIOCGIWFREQ
+	(iw_handler)NULL, // SIOCSIWMODE
+	(iw_handler)NULL, // SIOCGIWMODE
+	(iw_handler)NULL, // SIOCSIWSENS
+	(iw_handler)NULL, // SIOCGIWSENS
+	(iw_handler)NULL, // SIOCSIWRANGE
+	(iw_handler)iwctl_giwrange, // SIOCGIWRANGE
+	(iw_handler)NULL, // SIOCSIWPRIV
+	(iw_handler)NULL, // SIOCGIWPRIV
+	(iw_handler)NULL, // SIOCSIWSTATS
+	(iw_handler)NULL, // SIOCGIWSTATS
+	(iw_handler)NULL, // SIOCSIWSPY
+	(iw_handler)NULL, // SIOCGIWSPY
+	(iw_handler)NULL, // -- hole --
+	(iw_handler)NULL, // -- hole --
+	(iw_handler)NULL, // SIOCSIWAP
+	(iw_handler)NULL, // SIOCGIWAP
+	(iw_handler)NULL, // -- hole -- 0x16
+	(iw_handler)NULL, // SIOCGIWAPLIST
+	(iw_handler)iwctl_siwscan, // SIOCSIWSCAN
+	(iw_handler)iwctl_giwscan, // SIOCGIWSCAN
+	(iw_handler)NULL, // SIOCSIWESSID
+	(iw_handler)NULL, // SIOCGIWESSID
+	(iw_handler)NULL, // SIOCSIWNICKN
+	(iw_handler)NULL, // SIOCGIWNICKN
+	(iw_handler)NULL, // -- hole --
+	(iw_handler)NULL, // -- hole --
+	(iw_handler)NULL, // SIOCSIWRATE 0x20
+	(iw_handler)NULL, // SIOCGIWRATE
+	(iw_handler)NULL, // SIOCSIWRTS
+	(iw_handler)NULL, // SIOCGIWRTS
+	(iw_handler)NULL, // SIOCSIWFRAG
+	(iw_handler)NULL, // SIOCGIWFRAG
+	(iw_handler)NULL, // SIOCSIWTXPOW
+	(iw_handler)NULL, // SIOCGIWTXPOW
+	(iw_handler)NULL, // SIOCSIWRETRY
+	(iw_handler)NULL, // SIOCGIWRETRY
+	(iw_handler)NULL, // SIOCSIWENCODE
+	(iw_handler)NULL, // SIOCGIWENCODE
+	(iw_handler)NULL, // SIOCSIWPOWER
+	(iw_handler)NULL, // SIOCGIWPOWER
+	(iw_handler)NULL, // -- hole --
+	(iw_handler)NULL, // -- hole --
+	(iw_handler)NULL, // SIOCSIWGENIE
+	(iw_handler)NULL, // SIOCGIWGENIE
+	(iw_handler)NULL, // SIOCSIWAUTH
+	(iw_handler)NULL, // SIOCGIWAUTH
+	(iw_handler)NULL, // SIOCSIWENCODEEXT
+	(iw_handler)NULL, // SIOCGIWENCODEEXT
+	(iw_handler)NULL, // SIOCSIWPMKSA
+	(iw_handler)NULL, // -- hole --
 };
 
-
-static const iw_handler		iwctl_private_handler[] =
-{
-	NULL,				// SIOCIWFIRSTPRIV
+static const iw_handler iwctl_private_handler[] = {
+	NULL, // SIOCIWFIRSTPRIV
 };
 
-
 struct iw_priv_args iwctl_private_args[] = {
-{ IOCTL_CMD_SET,
-  IW_PRIV_TYPE_CHAR | 1024, 0,
-  "set"},
+	{ IOCTL_CMD_SET, IW_PRIV_TYPE_CHAR | 1024, 0, "set" },
 };
 
-
-
-const struct iw_handler_def	iwctl_handler_def =
-{
-	.get_wireless_stats = &iwctl_get_wireless_stats,
-	.num_standard	= sizeof(iwctl_handler)/sizeof(iw_handler),
-	.num_private	= 0,
-	.num_private_args = 0,
-	.standard	= (iw_handler *) iwctl_handler,
-	.private	= NULL,
-	.private_args	= NULL,
+const struct iw_handler_def iwctl_handler_def = {
+	.get_wireless_stats	= &iwctl_get_wireless_stats,
+	.num_standard		= sizeof(iwctl_handler) / sizeof(iw_handler),
+	.num_private		= 0,
+	.num_private_args	= 0,
+	.standard		= (iw_handler *)iwctl_handler,
+	.private		= NULL,
+	.private_args		= NULL,
 };
diff --git a/drivers/staging/vt6656/iwctl.h b/drivers/staging/vt6656/iwctl.h
index 0c6e0496779..d056f83a915 100644
--- a/drivers/staging/vt6656/iwctl.h
+++ b/drivers/staging/vt6656/iwctl.h
@@ -41,173 +41,107 @@
 
 struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev);
 
-int iwctl_siwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra);
-
-void iwctl_giwrange(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-
-void iwctl_giwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra);
-
-int iwctl_siwmode(struct net_device *dev,
-             struct iw_request_info *info,
-             __u32 *wmode,
-             char *extra);
-
-int iwctl_giwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra);
-
-int iwctl_siwfreq(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_freq *wrq,
-             char *extra);
-
-int iwctl_giwname(struct net_device *dev,
-			 struct iw_request_info *info,
-			 char *wrq,
-			 char *extra);
-
-int iwctl_giwsens(struct net_device *dev,
-			 struct iw_request_info *info,
-			 struct iw_param *wrq,
-			 char *extra);
-
-int iwctl_giwap(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct sockaddr *wrq,
-             char *extra);
-
-int iwctl_giwaplist(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-int iwctl_siwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-void iwctl_giwessid(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-int iwctl_siwrate(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-void iwctl_giwrate(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_param *wrq,
-             char *extra);
-
-int iwctl_siwrts(struct net_device *dev,
-		 struct iw_param *wrq);
-
-int iwctl_giwrts(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_siwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_giwfrag(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_siwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_giwretry(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_siwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-int iwctl_giwencode(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-int iwctl_siwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_giwpower(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
-
-int iwctl_giwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_point *wrq,
-             char *extra);
-
-int iwctl_siwscan(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-             char *extra);
+int iwctl_siwap(struct net_device *dev, struct iw_request_info *info,
+		struct sockaddr *wrq, char *extra);
+
+void iwctl_giwrange(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+void iwctl_giwmode(struct net_device *dev, struct iw_request_info *info,
+		__u32 *wmode, char *extra);
+
+int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
+		__u32 *wmode, char *extra);
+
+int iwctl_giwfreq(struct net_device *dev, struct iw_request_info *info,
+		struct iw_freq *wrq, char *extra);
+
+int iwctl_siwfreq(struct net_device *dev, struct iw_request_info *info,
+		struct iw_freq *wrq, char *extra);
+
+int iwctl_giwname(struct net_device *dev, struct iw_request_info *info,
+		char *wrq, char *extra);
+
+int iwctl_giwsens(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_giwap(struct net_device *dev, struct iw_request_info *info,
+		struct sockaddr *wrq, char *extra);
+
+int iwctl_giwaplist(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_siwessid(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+void iwctl_giwessid(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_siwrate(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+void iwctl_giwrate(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_siwrts(struct net_device *dev, struct iw_param *wrq);
+
+int iwctl_giwrts(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_siwfrag(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_giwfrag(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_siwretry(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_giwretry(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_giwencode(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_siwpower(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_giwpower(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_giwscan(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-int iwctl_siwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra);
-
-int iwctl_giwauth(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_param *wrq,
-			  char *extra);
-
-int iwctl_siwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra);
-
-int iwctl_giwgenie(struct net_device *dev,
-			  struct iw_request_info *info,
-			  struct iw_point *wrq,
-			  char *extra);
-
-int iwctl_siwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-int iwctl_giwencodeext(struct net_device *dev,
-             struct iw_request_info *info,
-             struct iw_point *wrq,
-             char *extra);
-
-int iwctl_siwmlme(struct net_device *dev,
-			struct iw_request_info * info,
-			struct iw_point *wrq,
-			char *extra);
+int iwctl_siwauth(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_giwauth(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *wrq, char *extra);
+
+int iwctl_siwgenie(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_giwgenie(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_siwencodeext(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_giwencodeext(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
+
+int iwctl_siwmlme(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra);
 #endif // #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 
-extern const struct iw_handler_def	iwctl_handler_def;
-extern const struct iw_priv_args	iwctl_private_args;
+extern const struct iw_handler_def iwctl_handler_def;
+extern const struct iw_priv_args iwctl_private_args;
 
 #endif /* __IWCTL_H__ */
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index ee5261a3688..b06fd5b723f 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -92,22 +92,14 @@ MODULE_DESCRIPTION(DEVICE_FULL_DRV_NAM);
         module_param_array(N, int, NULL, 0);\
         MODULE_PARM_DESC(N, D);
 
-#define RX_DESC_MIN0     16
-#define RX_DESC_MAX0     128
 #define RX_DESC_DEF0     64
 DEVICE_PARAM(RxDescriptors0,"Number of receive usb desc buffer");
 
 
-#define TX_DESC_MIN0     16
-#define TX_DESC_MAX0     128
 #define TX_DESC_DEF0     64
 DEVICE_PARAM(TxDescriptors0,"Number of transmit usb desc buffer");
 
-
-#define CHANNEL_MIN     1
-#define CHANNEL_MAX     14
 #define CHANNEL_DEF     6
-
 DEVICE_PARAM(Channel, "Channel number");
 
 
@@ -120,23 +112,13 @@ DEVICE_PARAM(Channel, "Channel number");
 
 DEVICE_PARAM(PreambleType, "Preamble Type");
 
-
-#define RTS_THRESH_MIN     512
-#define RTS_THRESH_MAX     2347
 #define RTS_THRESH_DEF     2347
-
 DEVICE_PARAM(RTSThreshold, "RTS threshold");
 
-
-#define FRAG_THRESH_MIN     256
-#define FRAG_THRESH_MAX     2346
 #define FRAG_THRESH_DEF     2346
-
 DEVICE_PARAM(FragThreshold, "Fragmentation threshold");
 
 
-#define DATA_RATE_MIN     0
-#define DATA_RATE_MAX     13
 #define DATA_RATE_DEF     13
 /* datarate[] index
    0: indicate 1 Mbps   0x02
@@ -157,10 +139,7 @@ DEVICE_PARAM(FragThreshold, "Fragmentation threshold");
 
 DEVICE_PARAM(ConnectionRate, "Connection data rate");
 
-#define OP_MODE_MAX     2
 #define OP_MODE_DEF     0
-#define OP_MODE_MIN     0
-
 DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
 
 /* OpMode[] is used for transmit.
@@ -176,34 +155,22 @@ DEVICE_PARAM(OPMode, "Infrastruct, adhoc, AP mode ");
 */
 
 #define PS_MODE_DEF     0
-
 DEVICE_PARAM(PSMode, "Power saving mode");
 
 
-#define SHORT_RETRY_MIN     0
-#define SHORT_RETRY_MAX     31
 #define SHORT_RETRY_DEF     8
-
-
 DEVICE_PARAM(ShortRetryLimit, "Short frame retry limits");
 
-#define LONG_RETRY_MIN     0
-#define LONG_RETRY_MAX     15
 #define LONG_RETRY_DEF     4
-
-
 DEVICE_PARAM(LongRetryLimit, "long frame retry limits");
 
-
 /* BasebandType[] baseband type selected
    0: indicate 802.11a type
    1: indicate 802.11b type
    2: indicate 802.11g type
 */
-#define BBP_TYPE_MIN     0
-#define BBP_TYPE_MAX     2
-#define BBP_TYPE_DEF     2
 
+#define BBP_TYPE_DEF     2
 DEVICE_PARAM(BasebandType, "baseband type");
 
 
@@ -238,11 +205,6 @@ static const long frequency_list[] = {
 	};
 
 
-#ifndef IW_ENCODE_NOKEY
-#define IW_ENCODE_NOKEY         0x0800
-#define IW_ENCODE_MODE  (IW_ENCODE_DISABLED | IW_ENCODE_RESTRICTED | IW_ENCODE_OPEN)
-#endif
-
 static const struct iw_handler_def	iwctl_handler_def;
 */
 
diff --git a/drivers/staging/vt6656/mib.h b/drivers/staging/vt6656/mib.h
index a89cca0c5ec..82d69a9cc20 100644
--- a/drivers/staging/vt6656/mib.h
+++ b/drivers/staging/vt6656/mib.h
@@ -113,7 +113,6 @@ typedef struct tagSMib2Counter {
 } SMib2Counter, *PSMib2Counter;
 
 // Value in the ifType entry
-//#define ETHERNETCSMACD      6           //
 #define WIRELESSLANIEEE80211b      6           //
 
 // Value in the ifAdminStatus/ifOperStatus entry
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 9b64b102f55..bb464527fc1 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -73,8 +73,6 @@ static int          msglevel                = MSG_LEVEL_INFO;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Static Definitions -------------------------*/
-#define CRITICAL_PACKET_LEN      256    // if packet size < 256 -> in-direct send
-                                        //    packet size >= 256 -> direct send
 
 const WORD wTimeStampOff[2][MAX_RATE] = {
         {384, 288, 226, 209, 54, 43, 37, 31, 28, 25, 24, 23}, // Long Preamble
diff --git a/drivers/staging/vt6656/tether.h b/drivers/staging/vt6656/tether.h
index 4ec05237469..8c1f5d253f8 100644
--- a/drivers/staging/vt6656/tether.h
+++ b/drivers/staging/vt6656/tether.h
@@ -72,7 +72,6 @@
 #define TYPE_CTL_ACK        0xd400
 
 
-//#define WEP_IV_MASK         0xFFFFFF00
 
 #else //if LITTLE_ENDIAN
 //
@@ -111,7 +110,6 @@
 #define TYPE_CTL_ACK        0x00d4
 
 
-//#define WEP_IV_MASK         0x00FFFFFF
 
 #endif //#ifdef __BIG_ENDIAN
 
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index c612ab58f38..609e8fa10b9 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -52,9 +52,6 @@
 //endpoint 2: read bulk
 //endpoint 3: write bulk
 
-//RequestType:
-//#define REQUEST_OUT       (USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE) // 0x40
-//#define REQUEST_IN        (USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE )  //0xc0
 //static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =MSG_LEVEL_INFO;
 
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 78ea121b7e2..9d2caa819f4 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -1254,51 +1254,50 @@ static BOOL s_bClearBSSID_SCAN(void *hDeviceContext)
 //mike add:reset command timer
 void vResetCommandTimer(void *hDeviceContext)
 {
-  PSDevice        pDevice = (PSDevice)hDeviceContext;
-
-  //delete timer
-      del_timer(&pDevice->sTimerCommand);
-  //init timer
-      init_timer(&pDevice->sTimerCommand);
-    pDevice->sTimerCommand.data = (unsigned long)pDevice;
-    pDevice->sTimerCommand.function = (TimerFunction)vRunCommand;
-    pDevice->sTimerCommand.expires = RUN_AT(HZ);
-    pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
-    pDevice->uCmdDequeueIdx = 0;
-    pDevice->uCmdEnqueueIdx = 0;
-    pDevice->eCommandState = WLAN_CMD_IDLE;
-    pDevice->bCmdRunning = FALSE;
-    pDevice->bCmdClear = FALSE;
+	PSDevice pDevice = (PSDevice)hDeviceContext;
+
+	//delete timer
+	del_timer(&pDevice->sTimerCommand);
+	//init timer
+	init_timer(&pDevice->sTimerCommand);
+	pDevice->sTimerCommand.data = (unsigned long)pDevice;
+	pDevice->sTimerCommand.function = (TimerFunction)vRunCommand;
+	pDevice->sTimerCommand.expires = RUN_AT(HZ);
+	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
+	pDevice->uCmdDequeueIdx = 0;
+	pDevice->uCmdEnqueueIdx = 0;
+	pDevice->eCommandState = WLAN_CMD_IDLE;
+	pDevice->bCmdRunning = FALSE;
+	pDevice->bCmdClear = FALSE;
 }
 
 void BSSvSecondTxData(void *hDeviceContext)
 {
-  PSDevice        pDevice = (PSDevice)hDeviceContext;
-  PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
-
-  pDevice->nTxDataTimeCout++;
-
-  if(pDevice->nTxDataTimeCout<4)     //don't tx data if timer less than 40s
-    {
-     // printk("mike:%s-->no data Tx not exceed the desired Time as %d\n",__FUNCTION__,
-	//  	(int)pDevice->nTxDataTimeCout);
-     pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-     add_timer(&pDevice->sTimerTxData);
-      return;
-    }
-
-  spin_lock_irq(&pDevice->lock);
-  //is wap_supplicant running successful OR only open && sharekey mode!
-  if(((pDevice->bLinkPass ==TRUE)&&(pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||  //open && sharekey linking
-      (pDevice->fWPA_Authened == TRUE)) {   //wpa linking
-        //   printk("mike:%s-->InSleep Tx Data Procedure\n",__FUNCTION__);
-	  pDevice->fTxDataInSleep = TRUE;
-	  PSbSendNullPacket(pDevice);      //send null packet
-	  pDevice->fTxDataInSleep = FALSE;
-  	}
-  spin_unlock_irq(&pDevice->lock);
-
-  pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-  add_timer(&pDevice->sTimerTxData);
-  return;
+	PSDevice pDevice = (PSDevice)hDeviceContext;
+	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
+
+	pDevice->nTxDataTimeCout++;
+
+	if (pDevice->nTxDataTimeCout < 4) {   //don't tx data if timer less than 40s
+		// printk("mike:%s-->no data Tx not exceed the desired Time as %d\n",__FUNCTION__,
+		//  	(int)pDevice->nTxDataTimeCout);
+		pDevice->sTimerTxData.expires = RUN_AT(10 * HZ);      //10s callback
+		add_timer(&pDevice->sTimerTxData);
+		return;
+	}
+
+	spin_lock_irq(&pDevice->lock);
+	//is wap_supplicant running successful OR only open && sharekey mode!
+	if (((pDevice->bLinkPass == TRUE) &&
+		(pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||  //open && sharekey linking
+		(pDevice->fWPA_Authened == TRUE)) {   //wpa linking
+		//   printk("mike:%s-->InSleep Tx Data Procedure\n",__FUNCTION__);
+		pDevice->fTxDataInSleep = TRUE;
+		PSbSendNullPacket(pDevice);      //send null packet
+		pDevice->fTxDataInSleep = FALSE;
+	}
+	spin_unlock_irq(&pDevice->lock);
+
+	pDevice->sTimerTxData.expires = RUN_AT(10 * HZ);      //10s callback
+	add_timer(&pDevice->sTimerTxData);
 }
diff --git a/drivers/staging/winbond/mds_s.h b/drivers/staging/winbond/mds_s.h
index 07d835b3b70..2972d66c943 100644
--- a/drivers/staging/winbond/mds_s.h
+++ b/drivers/staging/winbond/mds_s.h
@@ -40,7 +40,7 @@ enum {
 
 /*
  * ================================================================
- * Configration default value
+ * Configuration default value
  * ================================================================
  */
 #define DEFAULT_MULTICASTLISTMAX	32	/* standard */
diff --git a/drivers/staging/winbond/mto.c b/drivers/staging/winbond/mto.c
index 5250217156a..1b52ebd4b01 100644
--- a/drivers/staging/winbond/mto.c
+++ b/drivers/staging/winbond/mto.c
@@ -33,7 +33,7 @@ u16 MTO_Frag_Th_Tbl[MTO_MAX_FRAG_TH_LEVELS] = {
 
 /*
  * Declare data rate table:
- * The following table will be changed at anytime if the opration rate
+ * The following table will be changed at anytime if the operation rate
  * supported by AP don't match the table
  */
 static u8 MTO_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] = {
diff --git a/drivers/staging/winbond/wbusb.c b/drivers/staging/winbond/wbusb.c
index c3751a71838..ef360547ece 100644
--- a/drivers/staging/winbond/wbusb.c
+++ b/drivers/staging/winbond/wbusb.c
@@ -747,20 +747,18 @@ static int wb35_probe(struct usb_interface *intf,
 	struct usb_host_interface *interface;
 	struct ieee80211_hw *dev;
 	struct wbsoft_priv *priv;
-	int nr, err;
+	int err;
 	u32 ltmp;
 
 	usb_get_dev(udev);
 
 	/* Check the device if it already be opened */
-	nr = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
+	err = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
 			     0x01,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
 			     0x0, 0x400, &ltmp, 4, HZ * 100);
-	if (nr < 0) {
-		err = nr;
+	if (err < 0)
 		goto error;
-	}
 
 	/* Is already initialized? */
 	ltmp = cpu_to_le32(ltmp);
diff --git a/drivers/staging/wlags49_h2/dhf.c b/drivers/staging/wlags49_h2/dhf.c
index bb80b547cc1..13d360fa58e 100644
--- a/drivers/staging/wlags49_h2/dhf.c
+++ b/drivers/staging/wlags49_h2/dhf.c
@@ -1,5 +1,4 @@
 
-/*   vim:tw=110:ts=4: */
 /**************************************************************************************************************
 *
 * FILE   :	DHF.C
diff --git a/drivers/staging/wlags49_h2/dhf.h b/drivers/staging/wlags49_h2/dhf.h
index dbe0611fd03..1299b825646 100644
--- a/drivers/staging/wlags49_h2/dhf.h
+++ b/drivers/staging/wlags49_h2/dhf.h
@@ -1,5 +1,4 @@
 
-/*   vim:tw=110:ts=4: */
 #ifndef DHF_H
 #define DHF_H
 
diff --git a/drivers/staging/wlags49_h2/hcf.c b/drivers/staging/wlags49_h2/hcf.c
index 366e4a4b75c..423544634ae 100644
--- a/drivers/staging/wlags49_h2/hcf.c
+++ b/drivers/staging/wlags49_h2/hcf.c
@@ -705,7 +705,7 @@ hcf_action( IFBP ifbp, hcf_16 action )
 			// 800 us latency before FW switches to high power
 			MSF_WAIT(800);                              // MSF-defined function to wait n microseconds.
 //OOR			if ( ifbp->IFB_DSLinkStat & CFG_LINK_STAT_DS_OOR ) { // OutOfRange
-//				printk( "<5>ACT_INT_OFF: Deepsleep phase terminated, enable and go to AwaitConnection\n" );     //;?remove me 1 day
+//				printk(KERN_NOTICE "ACT_INT_OFF: Deepsleep phase terminated, enable and go to AwaitConnection\n" );     //;?remove me 1 day
 //				hcf_cntl( ifbp, HCF_CNTL_ENABLE );
 //			}
 //			ifbp->IFB_DSLinkStat &= ~( CFG_LINK_STAT_DS_IR | CFG_LINK_STAT_DS_OOR); //clear IR/OOR state
@@ -2979,7 +2979,7 @@ hcf_service_nic( IFBP ifbp, wci_bufp bufp, unsigned int len )
 			ltv.typ = CFG_DDS_TICK_TIME;
 			ltv.tick_time = ( ( ifbp->IFB_DSLinkStat & CFG_LINK_STAT_TIMER ) + 0x10 ) *64; //78 is more right
 			hcf_put_info( ifbp, (LTVP)&ltv );
-			printk( "<5>Preparing for sleep, link_status: %04X, timer : %d\n",
+			printk(KERN_NOTICE "Preparing for sleep, link_status: %04X, timer : %d\n",
 				ifbp->IFB_DSLinkStat, ltv.tick_time );//;?remove me 1 day
 			ifbp->IFB_TickCnt++; //;?just to make sure we do not keep on printing above message
 			if ( ltv.tick_time < 300 * 125 ) ifbp->IFB_DSLinkStat += 0x0010;
@@ -4221,11 +4221,11 @@ isr_info( IFBP ifbp )
 // /*4*/    if ( info[1] == CFG_LINK_STAT ) {
 //          ifbp->IFB_DSLinkStat = IPW( HREG_DATA_1 ) | CFG_LINK_STAT_CHANGE;   //corrupts BAP !! ;?
 //          ifbp->IFB_LinkStat = ifbp->IFB_DSLinkStat & CFG_LINK_STAT_FW; //;? to be obsoleted
-//          printk( "<4>linkstatus: %04x\n", ifbp->IFB_DSLinkStat );        //;?remove me 1 day
+//          printk(KERN_ERR "linkstatus: %04x\n", ifbp->IFB_DSLinkStat );        //;?remove me 1 day
 // #if (HCF_SLEEP) & HCF_DDS
 //          if ( ( ifbp->IFB_DSLinkStat & CFG_LINK_STAT_CONNECTED ) == 0 ) {    //even values are disconnected etc.
 //              ifbp->IFB_TickCnt = 0;              //start 2 second period (with 1 tick uncertanty)
-//              printk( "<5>isr_info: AwaitConnection phase started, IFB_TickCnt = 0\n" );      //;?remove me 1 day
+//              printk(KERN_NOTICE "isr_info: AwaitConnection phase started, IFB_TickCnt = 0\n" );      //;?remove me 1 day
 //          }
 // #endif // HCF_DDS
 //      }
diff --git a/drivers/staging/wlags49_h2/hcf.h b/drivers/staging/wlags49_h2/hcf.h
index 68e23303b5e..2abeaa11d8c 100644
--- a/drivers/staging/wlags49_h2/hcf.h
+++ b/drivers/staging/wlags49_h2/hcf.h
@@ -1,5 +1,4 @@
 
-//   vim:tw=110:ts=4:
 #ifndef HCF_H
 #define HCF_H 1
 
diff --git a/drivers/staging/wlags49_h2/hcfcfg.h b/drivers/staging/wlags49_h2/hcfcfg.h
index ef60da8c3eb..39fb4d326f6 100644
--- a/drivers/staging/wlags49_h2/hcfcfg.h
+++ b/drivers/staging/wlags49_h2/hcfcfg.h
@@ -1,5 +1,4 @@
 
-//   vim:tw=110:ts=4:
 #ifndef HCFCFG_H
 #define HCFCFG_H 1
 
diff --git a/drivers/staging/wlags49_h2/mdd.h b/drivers/staging/wlags49_h2/mdd.h
index 5f951efb9c0..5c3515f31a1 100644
--- a/drivers/staging/wlags49_h2/mdd.h
+++ b/drivers/staging/wlags49_h2/mdd.h
@@ -1,5 +1,4 @@
 
-//   vim:tw=110:ts=4:
 #ifndef MDD_H
 #define MDD_H 1
 
diff --git a/drivers/staging/wlags49_h2/mmd.c b/drivers/staging/wlags49_h2/mmd.c
index 7204a373bc5..3312348c347 100644
--- a/drivers/staging/wlags49_h2/mmd.c
+++ b/drivers/staging/wlags49_h2/mmd.c
@@ -1,5 +1,4 @@
 
-//   vim:tw=110:ts=4:
 /************************************************************************************************************
 *
 * FILE	  : mmd.c
diff --git a/drivers/staging/wlags49_h2/mmd.h b/drivers/staging/wlags49_h2/mmd.h
index 91495251300..14458035d9e 100644
--- a/drivers/staging/wlags49_h2/mmd.h
+++ b/drivers/staging/wlags49_h2/mmd.h
@@ -1,5 +1,4 @@
 
-//   vim:tw=110:ts=4:
 #ifndef MMD_H
 #define MMD_H 1
 
diff --git a/drivers/staging/wlags49_h2/wl_main.c b/drivers/staging/wlags49_h2/wl_main.c
index d5bf0a7012f..20410782957 100644
--- a/drivers/staging/wlags49_h2/wl_main.c
+++ b/drivers/staging/wlags49_h2/wl_main.c
@@ -3822,7 +3822,7 @@ static int write_int(struct file *file, const char *buffer, unsigned long count,
 		lp->timer_oor.data = (unsigned long)lp;
 		lp->timer_oor.expires = RUN_AT( 3 * HZ );
 		add_timer( &lp->timer_oor );
-		printk( "<5>wl_enable: %ld\n", jiffies );		//;?remove me 1 day
+		printk(KERN_NOTICE "wl_enable: %ld\n", jiffies );		//;?remove me 1 day
 #endif //DN554
 #ifdef DN554
 /*******************************************************************************
@@ -3852,7 +3852,7 @@ void timer_oor( u_long arg )
     DBG_ENTER( DbgInfo );
     DBG_PARAM( DbgInfo, "arg", "0x%08lx", arg );
 
-	printk( "<5>timer_oor: %ld 0x%04X\n", jiffies, lp->timer_oor_cnt );		//;?remove me 1 day
+	printk(KERN_NOTICE "timer_oor: %ld 0x%04X\n", jiffies, lp->timer_oor_cnt );		//;?remove me 1 day
 	lp->timer_oor_cnt += 10;
     if ( (lp->timer_oor_cnt & ~DS_OOR) > 300 ) {
 		lp->timer_oor_cnt = 300;
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
index 8bc562b8c4d..fabff4d650e 100644
--- a/drivers/staging/wlan-ng/cfg80211.c
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -127,7 +127,9 @@ int prism2_change_virtual_intf(struct wiphy *wiphy,
 	}
 
 	/* Set Operation mode to the PORT TYPE RID */
-	result = prism2_domibset_uint32(wlandev, DIDmib_p2_p2Static_p2CnfPortType, data);
+	result = prism2_domibset_uint32(wlandev,
+					DIDmib_p2_p2Static_p2CnfPortType,
+					data);
 
 	if (result)
 		err = -EFAULT;
@@ -363,7 +365,8 @@ int prism2_scan(struct wiphy *wiphy, struct net_device *dev,
 	if (request->n_ssids > 0) {
 		msg1.scantype.data = P80211ENUM_scantype_active;
 		msg1.ssid.data.len = request->ssids->ssid_len;
-		memcpy(msg1.ssid.data.data, request->ssids->ssid, request->ssids->ssid_len);
+		memcpy(msg1.ssid.data.data,
+			request->ssids->ssid, request->ssids->ssid_len);
 	} else {
 		msg1.scantype.data = 0;
 	}
@@ -540,7 +543,9 @@ int prism2_connect(struct wiphy *wiphy, struct net_device *dev,
 				goto exit;
 			}
 
-			result = prism2_domibset_pstr32(wlandev, did, sme->key_len, (u8 *) sme->key);
+			result = prism2_domibset_pstr32(wlandev,
+							did, sme->key_len,
+							(u8 *)sme->key);
 			if (result)
 				goto exit;
 
@@ -662,10 +667,11 @@ int prism2_get_tx_power(struct wiphy *wiphy, int *dbm)
 	struct prism2_wiphy_private *priv = wiphy_priv(wiphy);
 	wlandevice_t *wlandev = priv->wlandev;
 	struct p80211msg_dot11req_mibget msg;
-	p80211item_uint32_t *mibitem = (p80211item_uint32_t *) &msg.mibattribute.data;
+	p80211item_uint32_t *mibitem;
 	int result;
 	int err = 0;
 
+	mibitem = (p80211item_uint32_t *) &msg.mibattribute.data;
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem->did =
 	    DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel;
@@ -689,7 +695,8 @@ exit:
 /* Interface callback functions, passing data back up to the cfg80211 layer */
 void prism2_connect_result(wlandevice_t *wlandev, u8 failed)
 {
-	u16 status = failed ? WLAN_STATUS_UNSPECIFIED_FAILURE : WLAN_STATUS_SUCCESS;
+	u16 status = failed ?
+		     WLAN_STATUS_UNSPECIFIED_FAILURE : WLAN_STATUS_SUCCESS;
 
 	cfg80211_connect_result(wlandev->netdev, wlandev->bssid,
 				NULL, 0, NULL, 0, status, GFP_KERNEL);
@@ -732,7 +739,8 @@ struct wiphy *wlan_create_wiphy(struct device *dev, wlandevice_t *wlandev)
 {
 	struct wiphy *wiphy;
 	struct prism2_wiphy_private *priv;
-	wiphy = wiphy_new(&prism2_usb_cfg_ops, sizeof(struct prism2_wiphy_private));
+
+	wiphy = wiphy_new(&prism2_usb_cfg_ops, sizeof(*priv));
 	if (!wiphy)
 		return NULL;
 
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 3c40096f0c0..66c9aa97231 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -172,7 +172,7 @@ static int read_cardpda(struct pda *pda, wlandevice_t *wlandev);
 static int mkpdrlist(struct pda *pda);
 
 static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
-	      struct s3plugrec *s3plug, unsigned int ns3plug, struct pda * pda);
+	      struct s3plugrec *s3plug, unsigned int ns3plug, struct pda *pda);
 
 static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
 	     struct s3crcrec *s3crc, unsigned int ns3crc);
@@ -207,7 +207,8 @@ int prism2_fwtry(struct usb_device *udev, wlandevice_t *wlandev)
 
 	printk(KERN_INFO "prism2_usb: Checking for firmware %s\n",
 	       PRISM2_USB_FWFILE);
-	if (request_ihex_firmware(&fw_entry, PRISM2_USB_FWFILE, &udev->dev) != 0) {
+	if (request_ihex_firmware(&fw_entry,
+				  PRISM2_USB_FWFILE, &udev->dev) != 0) {
 		printk(KERN_INFO
 		       "prism2_usb: Firmware not available, but not essential\n");
 		printk(KERN_INFO
@@ -593,7 +594,8 @@ int mkpdrlist(struct pda *pda)
 	       le16_to_cpu(pda16[curroff + 1]) != HFA384x_PDR_END_OF_PDA) {
 		pda->rec[pda->nrec] = (hfa384x_pdrec_t *) &(pda16[curroff]);
 
-		if (le16_to_cpu(pda->rec[pda->nrec]->code) == HFA384x_PDR_NICID) {
+		if (le16_to_cpu(pda->rec[pda->nrec]->code) ==
+		    HFA384x_PDR_NICID) {
 			memcpy(&nicid, &pda->rec[pda->nrec]->data.nicid,
 			       sizeof(nicid));
 			nicid.id = le16_to_cpu(nicid.id);
@@ -655,7 +657,7 @@ int mkpdrlist(struct pda *pda)
 *	~0	failure
 ----------------------------------------------------------------*/
 int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
-	      struct s3plugrec *s3plug, unsigned int ns3plug, struct pda * pda)
+	      struct s3plugrec *s3plug, unsigned int ns3plug, struct pda *pda)
 {
 	int result = 0;
 	int i;			/* plug index */
@@ -980,9 +982,8 @@ int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	       unsigned int nfchunks)
 {
 	int result = 0;
-	struct p80211msg_p2req_ramdl_state rstatemsg;
-	struct p80211msg_p2req_ramdl_write rwritemsg;
-	struct p80211msg *msgp;
+	struct p80211msg_p2req_ramdl_state *rstmsg;
+	struct p80211msg_p2req_ramdl_write *rwrmsg;
 	u32 resultcode;
 	int i;
 	int j;
@@ -991,57 +992,68 @@ int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	u32 currlen;
 	u32 currdaddr;
 
+	rstmsg = kmalloc(sizeof(*rstmsg), GFP_KERNEL);
+	rwrmsg = kmalloc(sizeof(*rwrmsg), GFP_KERNEL);
+	if (!rstmsg || !rwrmsg) {
+		kfree(rstmsg);
+		kfree(rwrmsg);
+		printk(KERN_ERR
+		       "writeimage: no memory for firmware download, "
+		       "aborting download\n");
+		return -ENOMEM;
+	}
+
 	/* Initialize the messages */
-	memset(&rstatemsg, 0, sizeof(rstatemsg));
-	strcpy(rstatemsg.devname, wlandev->name);
-	rstatemsg.msgcode = DIDmsg_p2req_ramdl_state;
-	rstatemsg.msglen = sizeof(rstatemsg);
-	rstatemsg.enable.did = DIDmsg_p2req_ramdl_state_enable;
-	rstatemsg.exeaddr.did = DIDmsg_p2req_ramdl_state_exeaddr;
-	rstatemsg.resultcode.did = DIDmsg_p2req_ramdl_state_resultcode;
-	rstatemsg.enable.status = P80211ENUM_msgitem_status_data_ok;
-	rstatemsg.exeaddr.status = P80211ENUM_msgitem_status_data_ok;
-	rstatemsg.resultcode.status = P80211ENUM_msgitem_status_no_value;
-	rstatemsg.enable.len = sizeof(u32);
-	rstatemsg.exeaddr.len = sizeof(u32);
-	rstatemsg.resultcode.len = sizeof(u32);
-
-	memset(&rwritemsg, 0, sizeof(rwritemsg));
-	strcpy(rwritemsg.devname, wlandev->name);
-	rwritemsg.msgcode = DIDmsg_p2req_ramdl_write;
-	rwritemsg.msglen = sizeof(rwritemsg);
-	rwritemsg.addr.did = DIDmsg_p2req_ramdl_write_addr;
-	rwritemsg.len.did = DIDmsg_p2req_ramdl_write_len;
-	rwritemsg.data.did = DIDmsg_p2req_ramdl_write_data;
-	rwritemsg.resultcode.did = DIDmsg_p2req_ramdl_write_resultcode;
-	rwritemsg.addr.status = P80211ENUM_msgitem_status_data_ok;
-	rwritemsg.len.status = P80211ENUM_msgitem_status_data_ok;
-	rwritemsg.data.status = P80211ENUM_msgitem_status_data_ok;
-	rwritemsg.resultcode.status = P80211ENUM_msgitem_status_no_value;
-	rwritemsg.addr.len = sizeof(u32);
-	rwritemsg.len.len = sizeof(u32);
-	rwritemsg.data.len = WRITESIZE_MAX;
-	rwritemsg.resultcode.len = sizeof(u32);
+	memset(rstmsg, 0, sizeof(*rstmsg));
+	strcpy(rstmsg->devname, wlandev->name);
+	rstmsg->msgcode = DIDmsg_p2req_ramdl_state;
+	rstmsg->msglen = sizeof(*rstmsg);
+	rstmsg->enable.did = DIDmsg_p2req_ramdl_state_enable;
+	rstmsg->exeaddr.did = DIDmsg_p2req_ramdl_state_exeaddr;
+	rstmsg->resultcode.did = DIDmsg_p2req_ramdl_state_resultcode;
+	rstmsg->enable.status = P80211ENUM_msgitem_status_data_ok;
+	rstmsg->exeaddr.status = P80211ENUM_msgitem_status_data_ok;
+	rstmsg->resultcode.status = P80211ENUM_msgitem_status_no_value;
+	rstmsg->enable.len = sizeof(u32);
+	rstmsg->exeaddr.len = sizeof(u32);
+	rstmsg->resultcode.len = sizeof(u32);
+
+	memset(rwrmsg, 0, sizeof(*rwrmsg));
+	strcpy(rwrmsg->devname, wlandev->name);
+	rwrmsg->msgcode = DIDmsg_p2req_ramdl_write;
+	rwrmsg->msglen = sizeof(*rwrmsg);
+	rwrmsg->addr.did = DIDmsg_p2req_ramdl_write_addr;
+	rwrmsg->len.did = DIDmsg_p2req_ramdl_write_len;
+	rwrmsg->data.did = DIDmsg_p2req_ramdl_write_data;
+	rwrmsg->resultcode.did = DIDmsg_p2req_ramdl_write_resultcode;
+	rwrmsg->addr.status = P80211ENUM_msgitem_status_data_ok;
+	rwrmsg->len.status = P80211ENUM_msgitem_status_data_ok;
+	rwrmsg->data.status = P80211ENUM_msgitem_status_data_ok;
+	rwrmsg->resultcode.status = P80211ENUM_msgitem_status_no_value;
+	rwrmsg->addr.len = sizeof(u32);
+	rwrmsg->len.len = sizeof(u32);
+	rwrmsg->data.len = WRITESIZE_MAX;
+	rwrmsg->resultcode.len = sizeof(u32);
 
 	/* Send xxx_state(enable) */
 	pr_debug("Sending dl_state(enable) message.\n");
-	rstatemsg.enable.data = P80211ENUM_truth_true;
-	rstatemsg.exeaddr.data = startaddr;
+	rstmsg->enable.data = P80211ENUM_truth_true;
+	rstmsg->exeaddr.data = startaddr;
 
-	msgp = (struct p80211msg *) &rstatemsg;
-	result = prism2mgmt_ramdl_state(wlandev, msgp);
+	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
 		printk(KERN_ERR
 		       "writeimage state enable failed w/ result=%d, "
 		       "aborting download\n", result);
-		return result;
+		goto free_result;
 	}
-	resultcode = rstatemsg.resultcode.data;
+	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
 		printk(KERN_ERR
 		       "writeimage()->xxxdl_state msg indicates failure, "
 		       "w/ resultcode=%d, aborting download.\n", resultcode);
-		return 1;
+		result = 1;
+		goto free_result;
 	}
 
 	/* Now, loop through the data chunks and send WRITESIZE_MAX data */
@@ -1059,9 +1071,9 @@ int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 			curroff = j * WRITESIZE_MAX;
 			currdaddr = fchunk[i].addr + curroff;
 			/* Setup the message */
-			rwritemsg.addr.data = currdaddr;
-			rwritemsg.len.data = currlen;
-			memcpy(rwritemsg.data.data,
+			rwrmsg->addr.data = currdaddr;
+			rwrmsg->len.data = currlen;
+			memcpy(rwrmsg->data.data,
 			       fchunk[i].data + curroff, currlen);
 
 			/* Send flashdl_write(pda) */
@@ -1069,23 +1081,23 @@ int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 			    ("Sending xxxdl_write message addr=%06x len=%d.\n",
 			     currdaddr, currlen);
 
-			msgp = (struct p80211msg *) &rwritemsg;
-			result = prism2mgmt_ramdl_write(wlandev, msgp);
+			result = prism2mgmt_ramdl_write(wlandev, rwrmsg);
 
 			/* Check the results */
 			if (result) {
 				printk(KERN_ERR
 				       "writeimage chunk write failed w/ result=%d, "
 				       "aborting download\n", result);
-				return result;
+				goto free_result;
 			}
-			resultcode = rstatemsg.resultcode.data;
+			resultcode = rstmsg->resultcode.data;
 			if (resultcode != P80211ENUM_resultcode_success) {
 				printk(KERN_ERR
 				       "writeimage()->xxxdl_write msg indicates failure, "
 				       "w/ resultcode=%d, aborting download.\n",
 				       resultcode);
-				return 1;
+				result = 1;
+				goto free_result;
 			}
 
 		}
@@ -1093,24 +1105,28 @@ int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 	/* Send xxx_state(disable) */
 	pr_debug("Sending dl_state(disable) message.\n");
-	rstatemsg.enable.data = P80211ENUM_truth_false;
-	rstatemsg.exeaddr.data = 0;
+	rstmsg->enable.data = P80211ENUM_truth_false;
+	rstmsg->exeaddr.data = 0;
 
-	msgp = (struct p80211msg *) &rstatemsg;
-	result = prism2mgmt_ramdl_state(wlandev, msgp);
+	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
 		printk(KERN_ERR
 		       "writeimage state disable failed w/ result=%d, "
 		       "aborting download\n", result);
-		return result;
+		goto free_result;
 	}
-	resultcode = rstatemsg.resultcode.data;
+	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
 		printk(KERN_ERR
 		       "writeimage()->xxxdl_state msg indicates failure, "
 		       "w/ resultcode=%d, aborting download.\n", resultcode);
-		return 1;
+		result = 1;
+		goto free_result;
 	}
+
+free_result:
+	kfree(rstmsg);
+	kfree(rwrmsg);
 	return result;
 }
 
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 417aea5e01c..1dfd9aa5e9f 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -1947,8 +1947,8 @@ static wlandevice_t *create_wlan(void)
 	hfa384x_t *hw = NULL;
 
 	/* Alloc our structures */
-	wlandev = kmalloc(sizeof(wlandevice_t), GFP_KERNEL);
-	hw = kmalloc(sizeof(hfa384x_t), GFP_KERNEL);
+	wlandev = kzalloc(sizeof(wlandevice_t), GFP_KERNEL);
+	hw = kzalloc(sizeof(hfa384x_t), GFP_KERNEL);
 
 	if (!wlandev || !hw) {
 		printk(KERN_ERR "%s: Memory allocation failure.\n", dev_info);
@@ -1957,10 +1957,6 @@ static wlandevice_t *create_wlan(void)
 		return NULL;
 	}
 
-	/* Clear all the structs */
-	memset(wlandev, 0, sizeof(wlandevice_t));
-	memset(hw, 0, sizeof(hfa384x_t));
-
 	/* Initialize the network device object. */
 	wlandev->nsdname = dev_info;
 	wlandev->msdstate = WLAN_MSD_HWPRESENT_PENDING;
diff --git a/drivers/staging/xgifb/XGI_main.h b/drivers/staging/xgifb/XGI_main.h
index 9c62aeb9ede..c033da408a5 100644
--- a/drivers/staging/xgifb/XGI_main.h
+++ b/drivers/staging/xgifb/XGI_main.h
@@ -1,17 +1,9 @@
 #ifndef _XGIFB_MAIN
 #define _XGIFB_MAIN
-
-
 /* ------------------- Constant Definitions ------------------------- */
-
-
 #include "XGIfb.h"
-#include "vb_struct.h"
-#include "../../video/sis/sis.h"
 #include "vb_def.h"
 
-#define XGIFAIL(x) do { printk(x "\n"); return -EINVAL; } while (0)
-
 #ifndef PCI_DEVICE_ID_XGI_42
 #define PCI_DEVICE_ID_XGI_42      0x042
 #endif
@@ -76,9 +68,6 @@ static int XGIfb_crt2type = -1;
 /* PR: Tv plug type (for overriding autodetection) */
 static int XGIfb_tvplug = -1;
 
-/* TW: For ioctl XGIFB_GET_INFO */
-/* XGIfb_info XGIfbinfo; */
-
 #define MD_XGI315 1
 
 /* mode table */
@@ -238,17 +227,6 @@ static const struct _XGI_vrate {
 	{0, 0, 0, 0}
 };
 
-static const struct _chswtable {
-	int subsysVendor;
-	int subsysCard;
-	char *vendorName;
-	char *cardName;
-} mychswtable[] = {
-	{ 0x1631, 0x1002, "Mitachi", "0x1002" },
-	{ 0,      0,      ""       , ""       }
-};
-
-/* Eden Chen */
 static const struct _XGI_TV_filter {
 	u8 filter[9][4];
 } XGI_TV_filter[] = {
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 85dbf32b1f6..64ffd70eb7d 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -6,36 +6,12 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-/* #include <linux/config.h> */
 #include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/tty.h>
-#include <linux/slab.h>
-#include <linux/delay.h>
-#include <linux/fb.h>
-#include <linux/console.h>
-#include <linux/selection.h>
-#include <linux/ioport.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/vt_kern.h>
-#include <linux/capability.h>
-#include <linux/fs.h>
-#include <linux/types.h>
-#include <linux/proc_fs.h>
-
-#include <linux/io.h>
+
 #ifdef CONFIG_MTRR
 #include <asm/mtrr.h>
 #endif
 
-#include "XGIfb.h"
-#include "vgatypes.h"
 #include "XGI_main.h"
 #include "vb_init.h"
 #include "vb_util.h"
@@ -54,86 +30,24 @@ static unsigned int refresh_rate;
 
 /* -------------------- Macro definitions ---------------------------- */
 
-#undef XGIFBDEBUG
-
-#ifdef XGIFBDEBUG
-#define DPRINTK(fmt, args...) pr_debug("%s: " fmt, __func__ , ## args)
-#else
-#define DPRINTK(fmt, args...)
-#endif
-
-#ifdef XGIFBDEBUG
+#ifdef DEBUG
 static void dumpVGAReg(void)
 {
 	u8 i, reg;
 
 	xgifb_reg_set(XGISR, 0x05, 0x86);
-	/*
-	xgifb_reg_set(XGISR, 0x08, 0x4f);
-	xgifb_reg_set(XGISR, 0x0f, 0x20);
-	xgifb_reg_set(XGISR, 0x11, 0x4f);
-	xgifb_reg_set(XGISR, 0x13, 0x45);
-	xgifb_reg_set(XGISR, 0x14, 0x51);
-	xgifb_reg_set(XGISR, 0x1e, 0x41);
-	xgifb_reg_set(XGISR, 0x1f, 0x0);
-	xgifb_reg_set(XGISR, 0x20, 0xa1);
-	xgifb_reg_set(XGISR, 0x22, 0xfb);
-	xgifb_reg_set(XGISR, 0x26, 0x22);
-	xgifb_reg_set(XGISR, 0x3e, 0x07);
-	*/
-
-	/* xgifb_reg_set(XGICR, 0x19, 0x00); */
-	/* xgifb_reg_set(XGICR, 0x1a, 0x3C); */
-	/* xgifb_reg_set(XGICR, 0x22, 0xff); */
-	/* xgifb_reg_set(XGICR, 0x3D, 0x10); */
-
-	/* xgifb_reg_set(XGICR, 0x4a, 0xf3); */
-
-	/* xgifb_reg_set(XGICR, 0x57, 0x0); */
-	/* xgifb_reg_set(XGICR, 0x7a, 0x2c); */
-
-	/* xgifb_reg_set(XGICR, 0x82, 0xcc); */
-	/* xgifb_reg_set(XGICR, 0x8c, 0x0); */
-	/*
-	xgifb_reg_set(XGICR, 0x99, 0x1);
-	xgifb_reg_set(XGICR, 0x41, 0x40);
-	*/
 
 	for (i = 0; i < 0x4f; i++) {
 		reg = xgifb_reg_get(XGISR, i);
-		printk("\no 3c4 %x", i);
-		printk("\ni 3c5 => %x", reg);
+		pr_debug("o 3c4 %x\n", i);
+		pr_debug("i 3c5 => %x\n", reg);
 	}
 
 	for (i = 0; i < 0xF0; i++) {
 		reg = xgifb_reg_get(XGICR, i);
-		printk("\no 3d4 %x", i);
-		printk("\ni 3d5 => %x", reg);
-	}
-	/*
-	xgifb_reg_set(XGIPART1,0x2F,1);
-	for (i=1; i < 0x50; i++) {
-		reg = xgifb_reg_get(XGIPART1, i);
-		printk("\no d004 %x", i);
-		printk("\ni d005 => %x", reg);
-	}
-
-	for (i=0; i < 0x50; i++) {
-		 reg = xgifb_reg_get(XGIPART2, i);
-		 printk("\no d010 %x", i);
-		 printk("\ni d011 => %x", reg);
-	}
-	for (i=0; i < 0x50; i++) {
-		reg = xgifb_reg_get(XGIPART3, i);
-		printk("\no d012 %x",i);
-		printk("\ni d013 => %x",reg);
+		pr_debug("o 3d4 %x\n", i);
+		pr_debug("i 3d5 => %x\n", reg);
 	}
-	for (i=0; i < 0x50; i++) {
-		reg = xgifb_reg_get(XGIPART4, i);
-		printk("\no d014 %x",i);
-		printk("\ni d015 => %x",reg);
-	}
-	*/
 }
 #else
 static inline void dumpVGAReg(void)
@@ -141,12 +55,6 @@ static inline void dumpVGAReg(void)
 }
 #endif
 
-#if 1
-#define DEBUGPRN(x)
-#else
-#define DEBUGPRN(x) pr_info(x "\n");
-#endif
-
 /* --------------- Hardware Access Routines -------------------------- */
 
 static int XGIfb_mode_rate_to_dclock(struct vb_device_info *XGI_Pr,
@@ -253,15 +161,6 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 			| ((unsigned short) (sr_data & 0x01) << 10);
 	A = VT + 2;
 
-	/* cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[10]; */
-
-	/* Vertical display enable end */
-	/*
-	VDE = (cr_data & 0xff) |
-		((unsigned short) (cr_data2 & 0x02) << 7) |
-		((unsigned short) (cr_data2 & 0x40) << 3) |
-		((unsigned short) (sr_data & 0x02) << 9);
-	*/
 	VDE = XGI_Pr->RefIndex[RefreshRateTableIndex].YRes - 1;
 	E = VDE + 1;
 
@@ -363,28 +262,19 @@ static void XGIRegInit(struct vb_device_info *XGI_Pr, unsigned long BaseAddr)
 
 static int XGIfb_GetXG21DefaultLVDSModeIdx(struct xgifb_video_info *xgifb_info)
 {
+	int i = 0;
 
-	int found_mode = 0;
-	int XGIfb_mode_idx = 0;
-
-	found_mode = 0;
-	while ((XGIbios_mode[XGIfb_mode_idx].mode_no != 0)
-			&& (XGIbios_mode[XGIfb_mode_idx].xres
-					<= xgifb_info->lvds_data.LVDSHDE)) {
-		if ((XGIbios_mode[XGIfb_mode_idx].xres
-				== xgifb_info->lvds_data.LVDSHDE)
-				&& (XGIbios_mode[XGIfb_mode_idx].yres
-					== xgifb_info->lvds_data.LVDSVDE)
-				&& (XGIbios_mode[XGIfb_mode_idx].bpp == 8)) {
-			found_mode = 1;
-			break;
+	while ((XGIbios_mode[i].mode_no != 0)
+	       && (XGIbios_mode[i].xres <= xgifb_info->lvds_data.LVDSHDE)) {
+		if ((XGIbios_mode[i].xres == xgifb_info->lvds_data.LVDSHDE)
+		    && (XGIbios_mode[i].yres == xgifb_info->lvds_data.LVDSVDE)
+		    && (XGIbios_mode[i].bpp == 8)) {
+			return i;
 		}
-		XGIfb_mode_idx++;
+		i++;
 	}
-	if (!found_mode)
-		XGIfb_mode_idx = -1;
 
-	return XGIfb_mode_idx;
+	return -1;
 }
 
 static void XGIfb_search_mode(struct xgifb_video_info *xgifb_info,
@@ -415,7 +305,7 @@ invalid_mode:
 static void XGIfb_search_vesamode(struct xgifb_video_info *xgifb_info,
 				  unsigned int vesamode)
 {
-	int i = 0, j = 0;
+	int i = 0;
 
 	if (vesamode == 0)
 		goto invalid;
@@ -426,15 +316,13 @@ static void XGIfb_search_vesamode(struct xgifb_video_info *xgifb_info,
 		if ((XGIbios_mode[i].vesa_mode_no_1 == vesamode) ||
 		    (XGIbios_mode[i].vesa_mode_no_2 == vesamode)) {
 			xgifb_info->mode_idx = i;
-			j = 1;
-			break;
+			return;
 		}
 		i++;
 	}
 
 invalid:
-	if (!j)
-		pr_info("Invalid VESA mode 0x%x'\n", vesamode);
+	pr_info("Invalid VESA mode 0x%x'\n", vesamode);
 }
 
 static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
@@ -628,7 +516,7 @@ static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
 				if (XGIbios_mode[myindex].yres != 576)
 					return -1;
 			}
-			/*  TW: LVDS/CHRONTEL does not support 720 */
+			/* LVDS/CHRONTEL does not support 720 */
 			if (xgifb_info->hasVB == HASVB_LVDS_CHRONTEL ||
 			    xgifb_info->hasVB == HASVB_CHRONTEL) {
 				return -1;
@@ -692,8 +580,8 @@ static u8 XGIfb_search_refresh_rate(struct xgifb_video_info *xgifb_info,
 				break;
 			} else if (XGIfb_vrate[i].refresh > rate) {
 				if ((XGIfb_vrate[i].refresh - rate) <= 3) {
-					DPRINTK("XGIfb: Adjusting rate from %d up to %d\n",
-						rate, XGIfb_vrate[i].refresh);
+					pr_debug("Adjusting rate from %d up to %d\n",
+						 rate, XGIfb_vrate[i].refresh);
 					xgifb_info->rate_idx =
 						XGIfb_vrate[i].idx;
 					xgifb_info->refresh_rate =
@@ -701,8 +589,9 @@ static u8 XGIfb_search_refresh_rate(struct xgifb_video_info *xgifb_info,
 				} else if (((rate - XGIfb_vrate[i - 1].refresh)
 						<= 2) && (XGIfb_vrate[i].idx
 						!= 1)) {
-					DPRINTK("XGIfb: Adjusting rate from %d down to %d\n",
-						rate, XGIfb_vrate[i-1].refresh);
+					pr_debug("Adjusting rate from %d down to %d\n",
+						 rate,
+						 XGIfb_vrate[i-1].refresh);
 					xgifb_info->rate_idx =
 						XGIfb_vrate[i - 1].idx;
 					xgifb_info->refresh_rate =
@@ -710,8 +599,8 @@ static u8 XGIfb_search_refresh_rate(struct xgifb_video_info *xgifb_info,
 				}
 				break;
 			} else if ((rate - XGIfb_vrate[i].refresh) <= 2) {
-				DPRINTK("XGIfb: Adjusting rate from %d down to %d\n",
-					rate, XGIfb_vrate[i].refresh);
+				pr_debug("Adjusting rate from %d down to %d\n",
+					 rate, XGIfb_vrate[i].refresh);
 				xgifb_info->rate_idx = XGIfb_vrate[i].idx;
 				break;
 			}
@@ -832,26 +721,25 @@ static void XGIfb_post_setmode(struct xgifb_video_info *xgifb_info)
 {
 	u8 reg;
 	unsigned char doit = 1;
-	/*
-	xgifb_reg_set(XGISR,IND_SIS_PASSWORD,SIS_PASSWORD);
-	xgifb_reg_set(XGICR, 0x13, 0x00);
-	xgifb_reg_and_or(XGISR,0x0E, 0xF0, 0x01);
-	*test*
-	*/
+
 	if (xgifb_info->video_bpp == 8) {
-		/* TW: We can't switch off CRT1 on LVDS/Chrontel
-		 * in 8bpp Modes */
+		/*
+		 * We can't switch off CRT1 on LVDS/Chrontel
+		 * in 8bpp Modes
+		 */
 		if ((xgifb_info->hasVB == HASVB_LVDS) ||
 		    (xgifb_info->hasVB == HASVB_LVDS_CHRONTEL)) {
 			doit = 0;
 		}
-		/* TW: We can't switch off CRT1 on 301B-DH
-		 * in 8bpp Modes if using LCD */
+		/*
+		 * We can't switch off CRT1 on 301B-DH
+		 * in 8bpp Modes if using LCD
+		 */
 		if (xgifb_info->display2 == XGIFB_DISP_LCD)
 			doit = 0;
 	}
 
-	/* TW: We can't switch off CRT1 if bridge is in slave mode */
+	/* We can't switch off CRT1 if bridge is in slave mode */
 	if (xgifb_info->hasVB != HASVB_NONE) {
 		reg = xgifb_reg_get(XGIPART1, 0x00);
 
@@ -1025,15 +913,15 @@ static void XGIfb_post_setmode(struct xgifb_video_info *xgifb_info)
 			}
 
 			if ((filter >= 0) && (filter <= 7)) {
-				DPRINTK("FilterTable[%d]-%d: %02x %02x %02x %02x\n",
-					filter_tb, filter,
-					XGI_TV_filter[filter_tb].
+				pr_debug("FilterTable[%d]-%d: %02x %02x %02x %02x\n",
+					 filter_tb, filter,
+					 XGI_TV_filter[filter_tb].
 						filter[filter][0],
-					XGI_TV_filter[filter_tb].
+					 XGI_TV_filter[filter_tb].
 						filter[filter][1],
-					XGI_TV_filter[filter_tb].
+					 XGI_TV_filter[filter_tb].
 						filter[filter][2],
-					XGI_TV_filter[filter_tb].
+					 XGI_TV_filter[filter_tb].
 						filter[filter][3]
 				);
 				xgifb_reg_set(
@@ -1076,10 +964,6 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 	unsigned int drate = 0, hrate = 0;
 	int found_mode = 0;
 	int old_mode;
-	/* unsigned char reg, reg1; */
-
-	DEBUGPRN("Inside do_set_var");
-	/* printk(KERN_DEBUG "XGIfb:var->yres=%d, var->upper_margin=%d, var->lower_margin=%d, var->vsync_len=%d\n", var->yres, var->upper_margin, var->lower_margin, var->vsync_len); */
 
 	info->var.xres_virtual = var->xres_virtual;
 	info->var.yres_virtual = var->yres_virtual;
@@ -1089,13 +973,9 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 		vtotal <<= 1;
 	else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE)
 		vtotal <<= 2;
-	else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED) {
-		/* vtotal <<= 1; */
-		/* var->yres <<= 1; */
-	}
 
 	if (!htotal || !vtotal) {
-		DPRINTK("XGIfb: Invalid 'var' information\n");
+		pr_debug("Invalid 'var' information\n");
 		return -EINVAL;
 	} pr_debug("var->pixclock=%d, htotal=%d, vtotal=%d\n",
 			var->pixclock, htotal, vtotal);
@@ -1171,11 +1051,11 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 
 		XGIfb_post_setmode(xgifb_info);
 
-		DPRINTK("XGIfb: Set new mode: %dx%dx%d-%d\n",
-				XGIbios_mode[xgifb_info->mode_idx].xres,
-				XGIbios_mode[xgifb_info->mode_idx].yres,
-				XGIbios_mode[xgifb_info->mode_idx].bpp,
-				xgifb_info->refresh_rate);
+		pr_debug("Set new mode: %dx%dx%d-%d\n",
+			 XGIbios_mode[xgifb_info->mode_idx].xres,
+			 XGIbios_mode[xgifb_info->mode_idx].yres,
+			 XGIbios_mode[xgifb_info->mode_idx].bpp,
+			 xgifb_info->refresh_rate);
 
 		xgifb_info->video_bpp = XGIbios_mode[xgifb_info->mode_idx].bpp;
 		xgifb_info->video_vwidth = info->var.xres_virtual;
@@ -1217,13 +1097,12 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 			break;
 		default:
 			xgifb_info->video_cmap_len = 16;
-			pr_err("Unsupported depth %d",
+			pr_err("Unsupported depth %d\n",
 			       xgifb_info->video_bpp);
 			break;
 		}
 	}
 	XGIfb_bpp_to_var(xgifb_info, var); /*update ARGB info*/
-	DEBUGPRN("End of do_set_var");
 
 	dumpVGAReg();
 	return 0;
@@ -1234,8 +1113,6 @@ static int XGIfb_pan_var(struct fb_var_screeninfo *var, struct fb_info *info)
 	struct xgifb_video_info *xgifb_info = info->par;
 	unsigned int base;
 
-	/* printk("Inside pan_var"); */
-
 	base = var->yoffset * info->var.xres_virtual + var->xoffset;
 
 	/* calculate base bpp dep. */
@@ -1269,7 +1146,6 @@ static int XGIfb_pan_var(struct fb_var_screeninfo *var, struct fb_info *info)
 				 0x7F,
 				 ((base >> 24) & 0x01) << 7);
 	}
-	/* printk("End of pan_var"); */
 	return 0;
 }
 
@@ -1345,13 +1221,21 @@ static int XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
 {
 	struct xgifb_video_info *xgifb_info = info->par;
 
-	DEBUGPRN("inside get_fix");
 	memset(fix, 0, sizeof(struct fb_fix_screeninfo));
 
-	fix->smem_start = xgifb_info->video_base;
+	strncpy(fix->id, "XGI", sizeof(fix->id) - 1);
 
+	/* if register_framebuffer has been called, we must lock */
+	if (atomic_read(&info->count))
+		mutex_lock(&info->mm_lock);
+
+	fix->smem_start = xgifb_info->video_base;
 	fix->smem_len = xgifb_info->video_size;
 
+	/* if register_framebuffer has been called, we can unlock */
+	if (atomic_read(&info->count))
+		mutex_unlock(&info->mm_lock);
+
 	fix->type = FB_TYPE_PACKED_PIXELS;
 	fix->type_aux = 0;
 	if (xgifb_info->video_bpp == 8)
@@ -1367,7 +1251,6 @@ static int XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
 	fix->mmio_len = xgifb_info->mmio_size;
 	fix->accel = FB_ACCEL_SIS_XABRE;
 
-	DEBUGPRN("end of get_fix");
 	return 0;
 }
 
@@ -1375,12 +1258,10 @@ static int XGIfb_set_par(struct fb_info *info)
 {
 	int err;
 
-	/* printk("XGIfb: inside set_par\n"); */
 	err = XGIfb_do_set_var(&info->var, 1, info);
 	if (err)
 		return err;
 	XGIfb_get_fix(&info->fix, -1, info);
-	/* printk("XGIfb: end of set_par\n"); */
 	return 0;
 }
 
@@ -1394,8 +1275,6 @@ static int XGIfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 	int found_mode = 0;
 	int refresh_rate, search_idx;
 
-	DEBUGPRN("Inside check_var");
-
 	if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_NONINTERLACED) {
 		vtotal = var->upper_margin + var->yres + var->lower_margin
 				+ var->vsync_len;
@@ -1411,8 +1290,10 @@ static int XGIfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 		vtotal = var->upper_margin + var->yres + var->lower_margin
 				+ var->vsync_len;
 
-	if (!(htotal) || !(vtotal))
-		XGIFAIL("XGIfb: no valid timing data");
+	if (!(htotal) || !(vtotal)) {
+		pr_debug("No valid timing data\n");
+		return -EINVAL;
+	}
 
 	if (var->pixclock && htotal && vtotal) {
 		drate = 1000000000 / var->pixclock;
@@ -1428,16 +1309,7 @@ static int XGIfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 		xgifb_info->refresh_rate = 60;
 	}
 
-	/*
-	if ((var->pixclock) && (htotal)) {
-		drate = 1E12 / var->pixclock;
-		hrate = drate / htotal;
-		refresh_rate = (unsigned int) (hrate / vtotal * 2 + 0.5);
-	} else {
-		refresh_rate = 60;
-	}
-	*/
-	/* TW: Calculation wrong for 1024x600 - force it to 60Hz */
+	/* Calculation wrong for 1024x600 - force it to 60Hz */
 	if ((var->xres == 1024) && (var->yres == 600))
 		refresh_rate = 60;
 
@@ -1486,8 +1358,6 @@ static int XGIfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 		}
 	}
 
-	/* TW: TODO: Check the refresh rate */
-
 	/* Adapt RGB settings */
 	XGIfb_bpp_to_var(xgifb_info, var);
 
@@ -1502,16 +1372,7 @@ static int XGIfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 			var->xres_virtual = var->xres;
 		if (var->yres != var->yres_virtual)
 			var->yres_virtual = var->yres;
-	} /* else { */
-		/* TW: Now patch yres_virtual if we use panning */
-		/* May I do this? */
-		/* var->yres_virtual = xgifb_info->heapstart /
-			(var->xres * (var->bits_per_pixel >> 3)); */
-		/* if (var->yres_virtual <= var->yres) { */
-		/* TW: Paranoia check */
-		/* var->yres_virtual = var->yres; */
-		/* } */
-	/* } */
+	}
 
 	/* Truncate offsets to maximum if too high */
 	if (var->xoffset > var->xres_virtual - var->xres)
@@ -1526,7 +1387,6 @@ static int XGIfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 	var->blue.msb_right =
 	var->transp.offset = var->transp.length = var->transp.msb_right = 0;
 
-	DEBUGPRN("end of check_var");
 	return 0;
 }
 
@@ -1535,8 +1395,6 @@ static int XGIfb_pan_display(struct fb_var_screeninfo *var,
 {
 	int err;
 
-	/* printk("\nInside pan_display:\n"); */
-
 	if (var->xoffset > (info->var.xres_virtual - info->var.xres))
 		return -EINVAL;
 	if (var->yoffset > (info->var.yres_virtual - info->var.yres))
@@ -1563,7 +1421,6 @@ static int XGIfb_pan_display(struct fb_var_screeninfo *var,
 	else
 		info->var.vmode &= ~FB_VMODE_YWRAP;
 
-	/* printk("End of pan_display\n"); */
 	return 0;
 }
 
@@ -1597,7 +1454,6 @@ static struct fb_ops XGIfb_ops = {
 	.fb_fillrect = cfb_fillrect,
 	.fb_copyarea = cfb_copyarea,
 	.fb_imageblit = cfb_imageblit,
-	/* .fb_mmap = XGIfb_mmap, */
 };
 
 /* ---------------- Chip generation dependent routines ---------------- */
@@ -1674,9 +1530,6 @@ static int XGIfb_get_dram_size(struct xgifb_video_info *xgifb_info)
 	}
 
 	xgifb_info->video_size = xgifb_info->video_size * ChannelNum;
-	/* PLiad fixed for benchmarking and fb set */
-	/* xgifb_info->video_size = 0x200000; */ /* 1024x768x16 */
-	/* xgifb_info->video_size = 0x1000000; */ /* benchmark */
 
 	pr_info("SR14=%x DramSzie %x ChannelNum %x\n",
 	       reg,
@@ -1691,16 +1544,6 @@ static void XGIfb_detect_VB(struct xgifb_video_info *xgifb_info)
 
 	xgifb_info->TV_plug = xgifb_info->TV_type = 0;
 
-	switch (xgifb_info->hasVB) {
-	case HASVB_LVDS_CHRONTEL:
-	case HASVB_CHRONTEL:
-		break;
-	case HASVB_301:
-	case HASVB_302:
-		/* XGI_Sense30x(); */ /* Yi-Lin TV Sense? */
-		break;
-	}
-
 	cr32 = xgifb_reg_get(XGICR, IND_XGI_SCRATCH_REG_CR32);
 
 	if ((cr32 & SIS_CRT1) && !XGIfb_crt1off)
@@ -1724,7 +1567,7 @@ static void XGIfb_detect_VB(struct xgifb_video_info *xgifb_info)
 	}
 
 	if (XGIfb_tvplug != -1)
-		/* PR/TW: Override with option */
+		/* Override with option */
 		xgifb_info->TV_plug = XGIfb_tvplug;
 	else if (cr32 & SIS_VB_HIVISION) {
 		xgifb_info->TV_type = TVMODE_HIVISION;
@@ -1744,7 +1587,7 @@ static void XGIfb_detect_VB(struct xgifb_video_info *xgifb_info)
 			xgifb_info->TV_type = TVMODE_NTSC;
 	}
 
-	/* TW: Copy forceCRT1 option to CRT1off if option is given */
+	/* Copy forceCRT1 option to CRT1off if option is given */
 	if (XGIfb_forcecrt1 != -1) {
 		if (XGIfb_forcecrt1)
 			XGIfb_crt1off = 0;
@@ -1796,7 +1639,7 @@ static int __init xgifb_optval(char *fullopt, int validx)
 	unsigned long lres;
 
 	if (kstrtoul(fullopt + validx, 0, &lres) < 0 || lres > INT_MAX) {
-		pr_err("xgifb: invalid value for option: %s\n", fullopt);
+		pr_err("Invalid value for option: %s\n", fullopt);
 		return 0;
 	}
 	return lres;
@@ -1809,7 +1652,7 @@ static int __init XGIfb_setup(char *options)
 	if (!options || !*options)
 		return 0;
 
-	pr_info("xgifb: options: %s\n", options);
+	pr_info("Options: %s\n", options);
 
 	while ((this_opt = strsep(&options, ",")) != NULL) {
 
@@ -1838,7 +1681,7 @@ static int __init XGIfb_setup(char *options)
 			XGIfb_search_tvstd(this_opt + 7);
 		} else if (!strncmp(this_opt, "dstn", 4)) {
 			enable_dstn = 1;
-			/* TW: DSTN overrules forcecrt2type */
+			/* DSTN overrules forcecrt2type */
 			XGIfb_crt2type = XGIFB_DISP_LCD;
 		} else if (!strncmp(this_opt, "noypan", 6)) {
 			XGIfb_ypan = 0;
@@ -1882,9 +1725,9 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	xgifb_info->mmio_base = pci_resource_start(pdev, 1);
 	xgifb_info->mmio_size = pci_resource_len(pdev, 1);
 	xgifb_info->vga_base = pci_resource_start(pdev, 2) + 0x30;
-	pr_info("Relocate IO address: %Lx [%08lx]\n",
-	       (u64) pci_resource_start(pdev, 2),
-	       xgifb_info->vga_base);
+	dev_info(&pdev->dev, "Relocate IO address: %Lx [%08lx]\n",
+		 (u64) pci_resource_start(pdev, 2),
+		 xgifb_info->vga_base);
 
 	if (pci_enable_device(pdev)) {
 		ret = -EIO;
@@ -1902,9 +1745,9 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	reg1 = xgifb_reg_get(XGISR, IND_SIS_PASSWORD);
 
 	if (reg1 != 0xa1) { /*I/O error */
-		pr_err("I/O error!!!");
+		dev_err(&pdev->dev, "I/O error\n");
 		ret = -EIO;
-		goto error;
+		goto error_disable;
 	}
 
 	switch (xgifb_info->chip_id) {
@@ -1927,16 +1770,17 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		break;
 	default:
 		ret = -ENODEV;
-		goto error;
+		goto error_disable;
 	}
 
-	pr_info("chipid = %x\n", xgifb_info->chip);
+	dev_info(&pdev->dev, "chipid = %x\n", xgifb_info->chip);
 	hw_info->jChipType = xgifb_info->chip;
 
 	if (XGIfb_get_dram_size(xgifb_info)) {
-		pr_err("Fatal error: Unable to determine RAM size.\n");
+		dev_err(&pdev->dev,
+			"Fatal error: Unable to determine RAM size.\n");
 		ret = -ENODEV;
-		goto error;
+		goto error_disable;
 	}
 
 	/* Enable PCI_LINEAR_ADDRESSING and MMIO_ENABLE  */
@@ -1951,18 +1795,20 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	if (!request_mem_region(xgifb_info->video_base,
 				xgifb_info->video_size,
 				"XGIfb FB")) {
-		pr_err("unable request memory size %x\n",
+		dev_err(&pdev->dev, "Unable request memory size %x\n",
 		       xgifb_info->video_size);
-		pr_err("Fatal error: Unable to reserve frame buffer memory\n");
-		pr_err("Is there another framebuffer driver active?\n");
+		dev_err(&pdev->dev,
+			"Fatal error: Unable to reserve frame buffer memory. "
+			"Is there another framebuffer driver active?\n");
 		ret = -ENODEV;
-		goto error;
+		goto error_disable;
 	}
 
 	if (!request_mem_region(xgifb_info->mmio_base,
 				xgifb_info->mmio_size,
 				"XGIfb MMIO")) {
-		pr_err("Fatal error: Unable to reserve MMIO region\n");
+		dev_err(&pdev->dev,
+			"Fatal error: Unable to reserve MMIO region\n");
 		ret = -ENODEV;
 		goto error_0;
 	}
@@ -1972,18 +1818,20 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	xgifb_info->mmio_vbase = ioremap(xgifb_info->mmio_base,
 					    xgifb_info->mmio_size);
 
-	pr_info("Framebuffer at 0x%Lx, mapped to 0x%p, size %dk\n",
-	       (u64) xgifb_info->video_base,
-	       xgifb_info->video_vbase,
-	       xgifb_info->video_size / 1024);
+	dev_info(&pdev->dev,
+		 "Framebuffer at 0x%Lx, mapped to 0x%p, size %dk\n",
+		 (u64) xgifb_info->video_base,
+		 xgifb_info->video_vbase,
+		 xgifb_info->video_size / 1024);
 
-	pr_info("MMIO at 0x%Lx, mapped to 0x%p, size %ldk\n",
-	       (u64) xgifb_info->mmio_base, xgifb_info->mmio_vbase,
-	       xgifb_info->mmio_size / 1024);
+	dev_info(&pdev->dev,
+		 "MMIO at 0x%Lx, mapped to 0x%p, size %ldk\n",
+		 (u64) xgifb_info->mmio_base, xgifb_info->mmio_vbase,
+		 xgifb_info->mmio_size / 1024);
 
 	pci_set_drvdata(pdev, xgifb_info);
 	if (!XGIInitNew(pdev))
-		pr_err("XGIInitNew() failed!\n");
+		dev_err(&pdev->dev, "XGIInitNew() failed!\n");
 
 	xgifb_info->mtrr = (unsigned int) 0;
 
@@ -2012,29 +1860,31 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		reg = xgifb_reg_get(XGIPART4, 0x01);
 		if (reg >= 0xE0) {
 			hw_info->ujVBChipID = VB_CHIP_302LV;
-			pr_info("XGI302LV bridge detected (revision 0x%02x)\n", reg);
+			dev_info(&pdev->dev,
+				 "XGI302LV bridge detected (revision 0x%02x)\n",
+				 reg);
 		} else if (reg >= 0xD0) {
 			hw_info->ujVBChipID = VB_CHIP_301LV;
-			pr_info("XGI301LV bridge detected (revision 0x%02x)\n", reg);
-		}
-		/* else if (reg >= 0xB0) {
-			hw_info->ujVBChipID = VB_CHIP_301B;
-			reg1 = xgifb_reg_get(XGIPART4, 0x23);
-			printk("XGIfb: XGI301B bridge detected\n");
-		} */
-		else {
+			dev_info(&pdev->dev,
+				 "XGI301LV bridge detected (revision 0x%02x)\n",
+				 reg);
+		} else {
 			hw_info->ujVBChipID = VB_CHIP_301;
-			pr_info("XGI301 bridge detected\n");
+			dev_info(&pdev->dev, "XGI301 bridge detected\n");
 		}
 		break;
 	case HASVB_302:
 		reg = xgifb_reg_get(XGIPART4, 0x01);
 		if (reg >= 0xE0) {
 			hw_info->ujVBChipID = VB_CHIP_302LV;
-			pr_info("XGI302LV bridge detected (revision 0x%02x)\n", reg);
+			dev_info(&pdev->dev,
+				 "XGI302LV bridge detected (revision 0x%02x)\n",
+				 reg);
 		} else if (reg >= 0xD0) {
 			hw_info->ujVBChipID = VB_CHIP_301LV;
-			pr_info("XGI302LV bridge detected (revision 0x%02x)\n", reg);
+			dev_info(&pdev->dev,
+				 "XGI302LV bridge detected (revision 0x%02x)\n",
+				 reg);
 		} else if (reg >= 0xB0) {
 			reg1 = xgifb_reg_get(XGIPART4, 0x23);
 
@@ -2042,27 +1892,28 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 
 		} else {
 			hw_info->ujVBChipID = VB_CHIP_302;
-			pr_info("XGI302 bridge detected\n");
+			dev_info(&pdev->dev, "XGI302 bridge detected\n");
 		}
 		break;
 	case HASVB_LVDS:
 		hw_info->ulExternalChip = 0x1;
-		pr_info("LVDS transmitter detected\n");
+		dev_info(&pdev->dev, "LVDS transmitter detected\n");
 		break;
 	case HASVB_TRUMPION:
 		hw_info->ulExternalChip = 0x2;
-		pr_info("Trumpion Zurac LVDS scaler detected\n");
+		dev_info(&pdev->dev, "Trumpion Zurac LVDS scaler detected\n");
 		break;
 	case HASVB_CHRONTEL:
 		hw_info->ulExternalChip = 0x4;
-		pr_info("Chrontel TV encoder detected\n");
+		dev_info(&pdev->dev, "Chrontel TV encoder detected\n");
 		break;
 	case HASVB_LVDS_CHRONTEL:
 		hw_info->ulExternalChip = 0x5;
-		pr_info("LVDS transmitter and Chrontel TV encoder detected\n");
+		dev_info(&pdev->dev,
+			 "LVDS transmitter and Chrontel TV encoder detected\n");
 		break;
 	default:
-		pr_info("No or unknown bridge type detected\n");
+		dev_info(&pdev->dev, "No or unknown bridge type detected\n");
 		break;
 	}
 
@@ -2079,36 +1930,6 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		}
 	}
 
-	if ((hw_info->ujVBChipID == VB_CHIP_302B) ||
-			(hw_info->ujVBChipID == VB_CHIP_301LV) ||
-			(hw_info->ujVBChipID == VB_CHIP_302LV)) {
-		int tmp;
-		tmp = xgifb_reg_get(XGICR, 0x34);
-		if (tmp <= 0x13) {
-			/* Currently on LCDA?
-			 *(Some BIOSes leave CR38) */
-			tmp = xgifb_reg_get(XGICR, 0x38);
-			if ((tmp & 0x03) == 0x03) {
-				/* XGI_Pr.XGI_UseLCDA = 1; */
-			} else {
-				/* Currently on LCDA?
-				 *(Some newer BIOSes set D0 in CR35) */
-				tmp = xgifb_reg_get(XGICR, 0x35);
-				if (tmp & 0x01) {
-					/* XGI_Pr.XGI_UseLCDA = 1; */
-				} else {
-					tmp = xgifb_reg_get(XGICR,
-							    0x30);
-					if (tmp & 0x20) {
-						tmp = xgifb_reg_get(
-							XGIPART1, 0x13);
-					}
-				}
-			}
-		}
-
-	}
-
 	xgifb_info->mode_idx = -1;
 
 	if (mode)
@@ -2130,11 +1951,11 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	}
 
 	if (xgifb_info->mode_idx < 0) {
-		dev_err(&pdev->dev, "no supported video mode found\n");
+		dev_err(&pdev->dev, "No supported video mode found\n");
 		goto error_1;
 	}
 
-	/* yilin set default refresh rate */
+	/* set default refresh rate */
 	xgifb_info->refresh_rate = refresh_rate;
 	if (xgifb_info->refresh_rate == 0)
 		xgifb_info->refresh_rate = 60;
@@ -2230,11 +2051,6 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 
 	}
 
-	strncpy(fb_info->fix.id, "XGI", sizeof(fb_info->fix.id) - 1);
-	fb_info->fix.type	= FB_TYPE_PACKED_PIXELS;
-	fb_info->fix.xpanstep	= 1;
-	fb_info->fix.ypanstep	= 1;
-
 	fb_info->flags = FBINFO_FLAG_DEFAULT;
 	fb_info->screen_base = xgifb_info->video_vbase;
 	fb_info->fbops = &XGIfb_ops;
@@ -2247,7 +2063,7 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	xgifb_info->mtrr = mtrr_add(xgifb_info->video_base,
 		xgifb_info->video_size, MTRR_TYPE_WRCOMB, 1);
 	if (xgifb_info->mtrr >= 0)
-		dev_info(&pdev->dev, "added MTRR\n");
+		dev_info(&pdev->dev, "Added MTRR\n");
 #endif
 
 	if (register_framebuffer(fb_info) < 0) {
@@ -2271,6 +2087,8 @@ error_1:
 	release_mem_region(xgifb_info->mmio_base, xgifb_info->mmio_size);
 error_0:
 	release_mem_region(xgifb_info->video_base, xgifb_info->video_size);
+error_disable:
+	pci_disable_device(pdev);
 error:
 	framebuffer_release(fb_info);
 	return ret;
@@ -2295,6 +2113,7 @@ static void __devexit xgifb_remove(struct pci_dev *pdev)
 	iounmap(xgifb_info->video_vbase);
 	release_mem_region(xgifb_info->mmio_base, xgifb_info->mmio_size);
 	release_mem_region(xgifb_info->video_base, xgifb_info->video_size);
+	pci_disable_device(pdev);
 	framebuffer_release(fb_info);
 	pci_set_drvdata(pdev, NULL);
 }
@@ -2306,51 +2125,44 @@ static struct pci_driver xgifb_driver = {
 	.remove = __devexit_p(xgifb_remove)
 };
 
-static int __init xgifb_init(void)
-{
-	char *option = NULL;
 
-	if (forcecrt2type != NULL)
-		XGIfb_search_crt2type(forcecrt2type);
-	if (fb_get_options("xgifb", &option))
-		return -ENODEV;
-	XGIfb_setup(option);
-
-	return pci_register_driver(&xgifb_driver);
-}
-
-module_init(xgifb_init);
 
 /*****************************************************/
 /*                      MODULE                       */
 /*****************************************************/
 
-#ifdef MODULE
-
-MODULE_DESCRIPTION("Z7 Z9 Z9S Z11 framebuffer device driver");
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("XGITECH , Others");
-
 module_param(mode, charp, 0);
-module_param(vesa, int, 0);
-module_param(filter, int, 0);
-module_param(forcecrt2type, charp, 0);
+MODULE_PARM_DESC(mode,
+	"Selects the desired default display mode in the format XxYxDepth "
+	"(eg. 1024x768x16).");
 
+module_param(forcecrt2type, charp, 0);
 MODULE_PARM_DESC(forcecrt2type,
-	"\nForce the second display output type. Possible values are NONE,\n"
-	"LCD, TV, VGA, SVIDEO or COMPOSITE.\n");
-
-MODULE_PARM_DESC(mode,
-	"\nSelects the desired default display mode in the format XxYxDepth,\n"
-	"eg. 1024x768x16.\n");
+	"Force the second display output type. Possible values are NONE, "
+	"LCD, TV, VGA, SVIDEO or COMPOSITE.");
 
+module_param(vesa, int, 0);
 MODULE_PARM_DESC(vesa,
-	"\nSelects the desired default display mode by VESA mode number, eg.\n"
-	"0x117.\n");
+	"Selects the desired default display mode by VESA mode number "
+	"(eg. 0x117).");
 
+module_param(filter, int, 0);
 MODULE_PARM_DESC(filter,
-		"\nSelects TV flicker filter type (only for systems with a SiS301 video bridge).\n"
-		"(Possible values 0-7, default: [no filter])\n");
+	"Selects TV flicker filter type (only for systems with a SiS301 video bridge). "
+	"Possible values 0-7. Default: [no filter]).");
+
+static int __init xgifb_init(void)
+{
+	char *option = NULL;
+
+	if (forcecrt2type != NULL)
+		XGIfb_search_crt2type(forcecrt2type);
+	if (fb_get_options("xgifb", &option))
+		return -ENODEV;
+	XGIfb_setup(option);
+
+	return pci_register_driver(&xgifb_driver);
+}
 
 static void __exit xgifb_remove_module(void)
 {
@@ -2358,6 +2170,8 @@ static void __exit xgifb_remove_module(void)
 	pr_debug("Module unloaded\n");
 }
 
+MODULE_DESCRIPTION("Z7 Z9 Z9S Z11 framebuffer device driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("XGITECH , Others");
+module_init(xgifb_init);
 module_exit(xgifb_remove_module);
-
-#endif	/*  /MODULE  */
diff --git a/drivers/staging/xgifb/XGIfb.h b/drivers/staging/xgifb/XGIfb.h
index 9068c5ad76e..80547983759 100644
--- a/drivers/staging/xgifb/XGIfb.h
+++ b/drivers/staging/xgifb/XGIfb.h
@@ -1,8 +1,5 @@
 #ifndef _LINUX_XGIFB
 #define _LINUX_XGIFB
-#include <linux/ioctl.h>
-#include <linux/types.h>
-
 #include "vgatypes.h"
 #include "vb_struct.h"
 
@@ -33,13 +30,13 @@ enum xgi_tvtype {
 	TVMODE_NTSC = 0,
 	TVMODE_PAL,
 	TVMODE_HIVISION,
-	TVTYPE_PALM,	/* vicki@030226 */
-	TVTYPE_PALN,	/* vicki@030226 */
-	TVTYPE_NTSCJ,	/* vicki@030226 */
+	TVTYPE_PALM,
+	TVTYPE_PALN,
+	TVTYPE_NTSCJ,
 	TVMODE_TOTAL
 };
 
-enum xgi_tv_plug { /* vicki@030226 */
+enum xgi_tv_plug {
 	TVPLUG_UNKNOWN = 0,
 	TVPLUG_COMPOSITE = 1,
 	TVPLUG_SVIDEO = 2,
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
index c7317931f67..69078d933a4 100644
--- a/drivers/staging/xgifb/vb_def.h
+++ b/drivers/staging/xgifb/vb_def.h
@@ -1,5 +1,3 @@
-/* $XFree86: xc/programs/Xserver/hw/xfree86/drivers/xgi/initdef.h
- * ,v 1.4 2000/12/02 01:16:17 dawes Exp $*/
 #ifndef _VB_DEF_
 #define _VB_DEF_
 #include "../../video/sis/initdef.h"
@@ -12,7 +10,6 @@
 #define SetCHTVOverScan         0x8000
 
 #define Panel_320x480            0x07 /*fstn*/
-/* [ycchen] 02/12/03 Modify for Multi-Sync. LCD Support */
 #define PanelResInfo            0x1F /* CR36 Panel Type/LCDResInfo */
 #define Panel_1024x768x75        0x22
 #define Panel_1280x1024x75       0x23
@@ -264,4 +261,23 @@
 #define RES1280x960x85       0x46
 #define RES1280x960x120      0x47
 
+
+#define XG27_CR8F 0x0C
+#define XG27_SR36 0x30
+#define XG27_SR40 0x04
+#define XG27_SR41 0x00
+#define XG40_CRCF 0x13
+#define XGI330_CRT2Data_1_2 0
+#define XGI330_CRT2Data_4_D 0
+#define XGI330_CRT2Data_4_E 0
+#define XGI330_CRT2Data_4_10 0x80
+#define XGI330_SR07 0x18
+#define XGI330_SR1F 0
+#define XGI330_SR23 0xf6
+#define XGI330_SR24 0x0d
+#define XGI330_SR25 0
+#define XGI330_SR31 0xc0
+#define XGI330_SR32 0x11
+#define XGI330_SR33 0
+
 #endif
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index c222d611431..80dba6a425b 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -1,39 +1,30 @@
-#include <linux/types.h>
-#include <linux/delay.h> /* udelay */
-#include <linux/pci.h>
+#include <linux/delay.h>
 #include <linux/vmalloc.h>
 
 #include "XGIfb.h"
-#include "vgatypes.h"
-
 #include "vb_def.h"
-#include "vb_struct.h"
 #include "vb_util.h"
 #include "vb_setmode.h"
 #include "vb_init.h"
-
-
-#include <linux/io.h>
-
-static const unsigned short XGINew_DDRDRAM_TYPE340[4][5] = {
-	{ 2, 13, 9, 64, 0x45},
-	{ 2, 12, 9, 32, 0x35},
-	{ 2, 12, 8, 16, 0x31},
-	{ 2, 11, 8,  8, 0x21} };
-
-static const unsigned short XGINew_DDRDRAM_TYPE20[12][5] = {
-	{ 2, 14, 11, 128, 0x5D},
-	{ 2, 14, 10, 64, 0x59},
-	{ 2, 13, 11, 64, 0x4D},
-	{ 2, 14,  9, 32, 0x55},
-	{ 2, 13, 10, 32, 0x49},
-	{ 2, 12, 11, 32, 0x3D},
-	{ 2, 14,  8, 16, 0x51},
-	{ 2, 13,  9, 16, 0x45},
-	{ 2, 12, 10, 16, 0x39},
-	{ 2, 13,  8,  8, 0x41},
-	{ 2, 12,  9,  8, 0x35},
-	{ 2, 12,  8,  4, 0x31} };
+static const unsigned short XGINew_DDRDRAM_TYPE340[4][2] = {
+	{ 16, 0x45},
+	{  8, 0x35},
+	{  4, 0x31},
+	{  2, 0x21} };
+
+static const unsigned short XGINew_DDRDRAM_TYPE20[12][2] = {
+	{ 128, 0x5D},
+	{ 64, 0x59},
+	{ 64, 0x4D},
+	{ 32, 0x55},
+	{ 32, 0x49},
+	{ 32, 0x3D},
+	{ 16, 0x51},
+	{ 16, 0x45},
+	{ 16, 0x39},
+	{  8, 0x41},
+	{  8, 0x35},
+	{  4, 0x31} };
 
 #define XGIFB_ROM_SIZE	65536
 
@@ -44,21 +35,12 @@ XGINew_GetXG20DRAMType(struct xgi_hw_device_info *HwDeviceExtension,
 	unsigned char data, temp;
 
 	if (HwDeviceExtension->jChipType < XG20) {
-		if (*pVBInfo->pSoftSetting & SoftDRAMType) {
-			data = *pVBInfo->pSoftSetting & 0x07;
-			return data;
-		} else {
-			data = xgifb_reg_get(pVBInfo->P3c4, 0x39) & 0x02;
-			if (data == 0)
-				data = (xgifb_reg_get(pVBInfo->P3c4, 0x3A) &
-				       0x02) >> 1;
-			return data;
-		}
+		data = xgifb_reg_get(pVBInfo->P3c4, 0x39) & 0x02;
+		if (data == 0)
+			data = (xgifb_reg_get(pVBInfo->P3c4, 0x3A) &
+				   0x02) >> 1;
+		return data;
 	} else if (HwDeviceExtension->jChipType == XG27) {
-		if (*pVBInfo->pSoftSetting & SoftDRAMType) {
-			data = *pVBInfo->pSoftSetting & 0x07;
-			return data;
-		}
 		temp = xgifb_reg_get(pVBInfo->P3c4, 0x3B);
 		/* SR3B[7][3]MAA15 MAA11 (Power on Trapping) */
 		if (((temp & 0x88) == 0x80) || ((temp & 0x88) == 0x08))
@@ -101,13 +83,11 @@ static void XGINew_DDR1x_MRS_340(unsigned long P3c4,
 	xgifb_reg_set(P3c4, 0x16, 0x00);
 	xgifb_reg_set(P3c4, 0x16, 0x80);
 
-	if (*pVBInfo->pXGINew_DRAMTypeDefinition != 0x0C) { /* Samsung F Die */
-		mdelay(3);
-		xgifb_reg_set(P3c4, 0x18, 0x00);
-		xgifb_reg_set(P3c4, 0x19, 0x20);
-		xgifb_reg_set(P3c4, 0x16, 0x00);
-		xgifb_reg_set(P3c4, 0x16, 0x80);
-	}
+	mdelay(3);
+	xgifb_reg_set(P3c4, 0x18, 0x00);
+	xgifb_reg_set(P3c4, 0x19, 0x20);
+	xgifb_reg_set(P3c4, 0x16, 0x00);
+	xgifb_reg_set(P3c4, 0x16, 0x80);
 
 	udelay(60);
 	xgifb_reg_set(P3c4,
@@ -152,10 +132,8 @@ static void XGINew_SetMemoryClock(struct xgi_hw_device_info *HwDeviceExtension,
 		      0x30,
 		      pVBInfo->ECLKData[pVBInfo->ram_type].SR30);
 
-	/* [Vicent] 2004/07/07,
-	 * When XG42 ECLK = MCLK = 207MHz, Set SR32 D[1:0] = 10b */
-	/* [Hsuan] 2004/08/20,
-	 * Modify SR32 value, when MCLK=207MHZ, ELCK=250MHz,
+	/* When XG42 ECLK = MCLK = 207MHz, Set SR32 D[1:0] = 10b */
+	/* Modify SR32 value, when MCLK=207MHZ, ELCK=250MHz,
 	 * Set SR32 D[1:0] = 10b */
 	if (HwDeviceExtension->jChipType == XG42) {
 		if ((pVBInfo->MCLKData[pVBInfo->ram_type].SR28 == 0x1C) &&
@@ -180,8 +158,7 @@ static void XGINew_DDRII_Bootup_XG27(
 	XGINew_SetMemoryClock(HwDeviceExtension, pVBInfo);
 
 	/* Set Double Frequency */
-	/* xgifb_reg_set(P3d4, 0x97, 0x11); *//* CR97 */
-	xgifb_reg_set(P3d4, 0x97, *pVBInfo->pXGINew_CR97); /* CR97 */
+	xgifb_reg_set(P3d4, 0x97, pVBInfo->XGINew_CR97); /* CR97 */
 
 	udelay(200);
 
@@ -212,7 +189,6 @@ static void XGINew_DDRII_Bootup_XG27(
 	udelay(30);
 	xgifb_reg_set(P3c4, 0x16, 0x00); /* Set SR16 */
 	xgifb_reg_set(P3c4, 0x16, 0x80); /* Set SR16 */
-	/* udelay(15); */
 
 	xgifb_reg_set(P3c4, 0x1B, 0x04); /* Set SR1B */
 	udelay(60);
@@ -272,7 +248,6 @@ static void XGINew_DDR2_MRS_XG20(struct xgi_hw_device_info *HwDeviceExtension,
 	xgifb_reg_set(P3c4, 0x16, 0x05);
 	xgifb_reg_set(P3c4, 0x16, 0x85);
 
-	/* xgifb_reg_set(P3c4, 0x18, 0x52); */ /* MRS1 */
 	xgifb_reg_set(P3c4, 0x18, 0x42); /* MRS1 */
 	xgifb_reg_set(P3c4, 0x19, 0x02);
 	xgifb_reg_set(P3c4, 0x16, 0x05);
@@ -284,7 +259,6 @@ static void XGINew_DDR2_MRS_XG20(struct xgi_hw_device_info *HwDeviceExtension,
 	xgifb_reg_set(P3c4, 0x1B, 0x00); /* SR1B */
 	udelay(100);
 
-	/* xgifb_reg_set(P3c4 ,0x18, 0x52); */ /* MRS2 */
 	xgifb_reg_set(P3c4, 0x18, 0x42); /* MRS1 */
 	xgifb_reg_set(P3c4, 0x19, 0x00);
 	xgifb_reg_set(P3c4, 0x16, 0x05);
@@ -310,14 +284,12 @@ static void XGINew_DDR1x_MRS_XG20(unsigned long P3c4,
 	xgifb_reg_set(P3c4,
 		      0x18,
 		      pVBInfo->SR15[2][pVBInfo->ram_type]); /* SR18 */
-	/* xgifb_reg_set(P3c4, 0x18, 0x31); */
 	xgifb_reg_set(P3c4, 0x19, 0x01);
 	xgifb_reg_set(P3c4, 0x16, 0x03);
 	xgifb_reg_set(P3c4, 0x16, 0x83);
 	mdelay(1);
 	xgifb_reg_set(P3c4, 0x1B, 0x03);
 	udelay(500);
-	/* xgifb_reg_set(P3c4, 0x18, 0x31); */
 	xgifb_reg_set(P3c4,
 		      0x18,
 		      pVBInfo->SR15[2][pVBInfo->ram_type]); /* SR18 */
@@ -541,7 +513,7 @@ static void XGINew_SetDRAMDefaultRegister340(
 		      pVBInfo->CR40[0][pVBInfo->ram_type]); /* CR41 */
 
 	if (HwDeviceExtension->jChipType == XG27)
-		xgifb_reg_set(P3d4, 0x8F, *pVBInfo->pCR8F); /* CR8F */
+		xgifb_reg_set(P3d4, 0x8F, XG27_CR8F); /* CR8F */
 
 	for (j = 0; j <= 6; j++) /* CR90 - CR96 */
 		xgifb_reg_set(P3d4, (0x90 + j),
@@ -564,9 +536,8 @@ static void XGINew_SetDRAMDefaultRegister340(
 
 	xgifb_reg_set(P3d4, 0x83, 0x09); /* CR83 */
 	xgifb_reg_set(P3d4, 0x87, 0x00); /* CR87 */
-	xgifb_reg_set(P3d4, 0xCF, *pVBInfo->pCRCF); /* CRCF */
+	xgifb_reg_set(P3d4, 0xCF, XG40_CRCF); /* CRCF */
 	if (pVBInfo->ram_type) {
-		/* xgifb_reg_set(P3c4, 0x17, 0xC0); */ /* SR17 DDRII */
 		xgifb_reg_set(P3c4, 0x17, 0x80); /* SR17 DDRII */
 		if (HwDeviceExtension->jChipType == XG27)
 			xgifb_reg_set(P3c4, 0x17, 0x02); /* SR17 DDRII */
@@ -588,71 +559,16 @@ static void XGINew_SetDRAMDefaultRegister340(
 		      pVBInfo->SR15[3][pVBInfo->ram_type]); /* SR1B */
 }
 
-static void XGINew_SetDRAMSizingType(int index,
-		const unsigned short DRAMTYPE_TABLE[][5],
-		struct vb_device_info *pVBInfo)
-{
-	unsigned short data;
-
-	data = DRAMTYPE_TABLE[index][4];
-	xgifb_reg_and_or(pVBInfo->P3c4, 0x13, 0x80, data);
-	udelay(15);
-	/* should delay 50 ns */
-}
-
-static unsigned short XGINew_SetDRAMSizeReg(int index,
-		const unsigned short DRAMTYPE_TABLE[][5],
-		struct vb_device_info *pVBInfo)
-{
-	unsigned short data = 0, memsize = 0;
-	int RankSize;
-	unsigned char ChannelNo;
-
-	RankSize = DRAMTYPE_TABLE[index][3] * pVBInfo->ram_bus / 32;
-	data = xgifb_reg_get(pVBInfo->P3c4, 0x13);
-	data &= 0x80;
-
-	if (data == 0x80)
-		RankSize *= 2;
-
-	data = 0;
-
-	if (pVBInfo->ram_channel == 3)
-		ChannelNo = 4;
-	else
-		ChannelNo = pVBInfo->ram_channel;
-
-	if (ChannelNo * RankSize <= 256) {
-		while ((RankSize >>= 1) > 0)
-			data += 0x10;
-
-		memsize = data >> 4;
-
-		/* [2004/03/25] Vicent, Fix DRAM Sizing Error */
-		xgifb_reg_set(pVBInfo->P3c4,
-			      0x14,
-			      (xgifb_reg_get(pVBInfo->P3c4, 0x14) & 0x0F) |
-			       (data & 0xF0));
-
-		/* data |= pVBInfo->ram_channel << 2; */
-		/* data |= (pVBInfo->ram_bus / 64) << 1; */
-		/* xgifb_reg_set(pVBInfo->P3c4, 0x14, data); */
-
-		/* should delay */
-		/* XGINew_SetDRAMModeRegister340(pVBInfo); */
-	}
-	return memsize;
-}
 
-static unsigned short XGINew_SetDRAMSize20Reg(int index,
-		const unsigned short DRAMTYPE_TABLE[][5],
+static unsigned short XGINew_SetDRAMSize20Reg(
+		unsigned short dram_size,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short data = 0, memsize = 0;
 	int RankSize;
 	unsigned char ChannelNo;
 
-	RankSize = DRAMTYPE_TABLE[index][3] * pVBInfo->ram_bus / 8;
+	RankSize = dram_size * pVBInfo->ram_bus / 8;
 	data = xgifb_reg_get(pVBInfo->P3c4, 0x13);
 	data &= 0x80;
 
@@ -672,19 +588,12 @@ static unsigned short XGINew_SetDRAMSize20Reg(int index,
 
 		memsize = data >> 4;
 
-		/* [2004/03/25] Vicent, Fix DRAM Sizing Error */
+		/* Fix DRAM Sizing Error */
 		xgifb_reg_set(pVBInfo->P3c4,
 			      0x14,
 			      (xgifb_reg_get(pVBInfo->P3c4, 0x14) & 0x0F) |
 				(data & 0xF0));
 		udelay(15);
-
-		/* data |= pVBInfo->ram_channel << 2; */
-		/* data |= (pVBInfo->ram_bus / 64) << 1; */
-		/* xgifb_reg_set(pVBInfo->P3c4, 0x14, data); */
-
-		/* should delay */
-		/* XGINew_SetDRAMModeRegister340(pVBInfo); */
 	}
 	return memsize;
 }
@@ -703,8 +612,7 @@ static int XGINew_ReadWriteRest(unsigned short StopAddr,
 		writel(Position, fbaddr + Position);
 	}
 
-	udelay(500); /* [Vicent] 2004/04/16.
-			Fix #1759 Memory Size error in Multi-Adapter. */
+	udelay(500); /* Fix #1759 Memory Size error in Multi-Adapter. */
 
 	Position = 0;
 
@@ -946,52 +854,41 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 static int XGINew_DDRSizing340(struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
-	int i;
-	unsigned short memsize, addr;
+	u8 i, size;
+	unsigned short memsize, start_addr;
+	const unsigned short (*dram_table)[2];
 
 	xgifb_reg_set(pVBInfo->P3c4, 0x15, 0x00); /* noninterleaving */
 	xgifb_reg_set(pVBInfo->P3c4, 0x1C, 0x00); /* nontiling */
 	XGINew_CheckChannel(HwDeviceExtension, pVBInfo);
 
 	if (HwDeviceExtension->jChipType >= XG20) {
-		for (i = 0; i < 12; i++) {
-			XGINew_SetDRAMSizingType(i,
-						 XGINew_DDRDRAM_TYPE20,
-						 pVBInfo);
-			memsize = XGINew_SetDRAMSize20Reg(i,
-							  XGINew_DDRDRAM_TYPE20,
-							  pVBInfo);
-			if (memsize == 0)
-				continue;
-
-			addr = memsize + (pVBInfo->ram_channel - 2) + 20;
-			if ((HwDeviceExtension->ulVideoMemorySize - 1) <
-			    (unsigned long) (1 << addr))
-				continue;
-
-			if (XGINew_ReadWriteRest(addr, 5, pVBInfo) == 1)
-				return 1;
-		}
+		dram_table = XGINew_DDRDRAM_TYPE20;
+		size = ARRAY_SIZE(XGINew_DDRDRAM_TYPE20);
+		start_addr = 5;
 	} else {
-		for (i = 0; i < 4; i++) {
-			XGINew_SetDRAMSizingType(i,
-						 XGINew_DDRDRAM_TYPE340,
-						 pVBInfo);
-			memsize = XGINew_SetDRAMSizeReg(i,
-							XGINew_DDRDRAM_TYPE340,
-							pVBInfo);
-
-			if (memsize == 0)
-				continue;
-
-			addr = memsize + (pVBInfo->ram_channel - 2) + 20;
-			if ((HwDeviceExtension->ulVideoMemorySize - 1) <
-			    (unsigned long) (1 << addr))
-				continue;
-
-			if (XGINew_ReadWriteRest(addr, 9, pVBInfo) == 1)
-				return 1;
-		}
+		dram_table = XGINew_DDRDRAM_TYPE340;
+		size = ARRAY_SIZE(XGINew_DDRDRAM_TYPE340);
+		start_addr = 9;
+	}
+
+	for (i = 0; i < size; i++) {
+		/* SetDRAMSizingType */
+		xgifb_reg_and_or(pVBInfo->P3c4, 0x13, 0x80, dram_table[i][1]);
+		udelay(15); /* should delay 50 ns */
+
+		memsize = XGINew_SetDRAMSize20Reg(dram_table[i][0], pVBInfo);
+
+		if (memsize == 0)
+			continue;
+
+		memsize += (pVBInfo->ram_channel - 2) + 20;
+		if ((HwDeviceExtension->ulVideoMemorySize - 1) <
+			(unsigned long) (1 << memsize))
+			continue;
+
+		if (XGINew_ReadWriteRest(memsize, start_addr, pVBInfo) == 1)
+			return 1;
 	}
 	return 0;
 }
@@ -1011,9 +908,6 @@ static void XGINew_SetDRAMSize_340(struct xgifb_video_info *xgifb_info,
 	xgifb_reg_set(pVBInfo->P3c4, 0x21, (unsigned short) (data & 0xDF));
 	XGI_DisplayOff(xgifb_info, HwDeviceExtension, pVBInfo);
 
-	/* data = xgifb_reg_get(pVBInfo->P3c4, 0x1); */
-	/* data |= 0x20 ; */
-	/* xgifb_reg_set(pVBInfo->P3c4, 0x01, data); *//* Turn OFF Display */
 	XGINew_DDRSizing340(HwDeviceExtension, pVBInfo);
 	data = xgifb_reg_get(pVBInfo->P3c4, 0x21);
 	/* enable read cache */
@@ -1057,7 +951,7 @@ static void xgifb_read_vbios(struct pci_dev *pdev,
 	pVBInfo->IF_DEF_LVDS = 0;
 	vbios = xgifb_copy_rom(pdev, &vbios_size);
 	if (vbios == NULL) {
-		dev_err(&pdev->dev, "video BIOS not available\n");
+		dev_err(&pdev->dev, "Video BIOS not available\n");
 		return;
 	}
 	if (vbios_size <= 0x65)
@@ -1112,7 +1006,7 @@ static void xgifb_read_vbios(struct pci_dev *pdev,
 	pVBInfo->IF_DEF_LVDS = 1;
 	return;
 error:
-	dev_err(&pdev->dev, "video BIOS corrupted\n");
+	dev_err(&pdev->dev, "Video BIOS corrupted\n");
 	vfree(vbios);
 }
 
@@ -1148,15 +1042,10 @@ static void XGINew_ChkSenseStatus(struct xgi_hw_device_info *HwDeviceExtension,
 
 	if (tempbx & tempcx) {
 		CR3CData = xgifb_reg_get(pVBInfo->P3d4, 0x3c);
-		if (!(CR3CData & DisplayDeviceFromCMOS)) {
+		if (!(CR3CData & DisplayDeviceFromCMOS))
 			tempcx = 0x1FF0;
-			if (*pVBInfo->pSoftSetting & ModeSoftSetting)
-				tempbx = 0x1FF0;
-		}
 	} else {
 		tempcx = 0x1FF0;
-		if (*pVBInfo->pSoftSetting & ModeSoftSetting)
-			tempbx = 0x1FF0;
 	}
 
 	tempbx &= tempcx;
@@ -1295,13 +1184,11 @@ static void XGINew_GetXG21Sense(struct xgi_hw_device_info *HwDeviceExtension,
 {
 	unsigned char Temp;
 
-#if 1
 	if (pVBInfo->IF_DEF_LVDS) { /* For XG21 LVDS */
 		xgifb_reg_or(pVBInfo->P3d4, 0x32, LCDSense);
 		/* LVDS on chip */
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x38, ~0xE0, 0xC0);
 	} else {
-#endif
 		/* Enable GPIOA/B read  */
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x4A, ~0x03, 0x03);
 		Temp = xgifb_reg_get(pVBInfo->P3d4, 0x48) & 0xC0;
@@ -1324,9 +1211,7 @@ static void XGINew_GetXG21Sense(struct xgi_hw_device_info *HwDeviceExtension,
 			/* Disable read GPIOF */
 			xgifb_reg_and(pVBInfo->P3d4, 0x4A, ~0x20);
 		}
-#if 1
 	}
-#endif
 }
 
 static void XGINew_GetXG27Sense(struct xgi_hw_device_info *HwDeviceExtension,
@@ -1398,37 +1283,23 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 	struct vb_device_info VBINF;
 	struct vb_device_info *pVBInfo = &VBINF;
 	unsigned char i, temp = 0, temp1;
-	/* VBIOSVersion[5]; */
-
-	/* unsigned long j, k; */
 
 	pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress;
 
 	pVBInfo->BaseAddr = xgifb_info->vga_base;
 
-	/* Newdebugcode(0x99); */
-
 	if (pVBInfo->FBAddr == NULL) {
-		printk("\n pVBInfo->FBAddr == 0 ");
+		dev_dbg(&pdev->dev, "pVBInfo->FBAddr == 0\n");
 		return 0;
 	}
-	printk("1");
 	if (pVBInfo->BaseAddr == 0) {
-		printk("\npVBInfo->BaseAddr == 0 ");
+		dev_dbg(&pdev->dev, "pVBInfo->BaseAddr == 0\n");
 		return 0;
 	}
-	printk("2");
 
 	outb(0x67, (pVBInfo->BaseAddr + 0x12)); /* 3c2 <- 67 ,ynlai */
 
 	pVBInfo->ISXPDOS = 0;
-	printk("3");
-
-	printk("4");
-
-	/* VBIOSVersion[4] = 0x0; */
-
-	/* 09/07/99 modify by domao */
 
 	pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14;
 	pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24;
@@ -1447,9 +1318,8 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 	pVBInfo->Part3Port = pVBInfo->BaseAddr + SIS_CRT2_PORT_12;
 	pVBInfo->Part4Port = pVBInfo->BaseAddr + SIS_CRT2_PORT_14;
 	pVBInfo->Part5Port = pVBInfo->BaseAddr + SIS_CRT2_PORT_14 + 2;
-	printk("5");
 
-	if (HwDeviceExtension->jChipType < XG20) /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20)
 		/* Run XGI_GetVBType before InitTo330Pointer */
 		XGI_GetVBType(pVBInfo);
 
@@ -1457,9 +1327,8 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 	xgifb_read_vbios(pdev, pVBInfo);
 
-	/* 1.Openkey */
+	/* Openkey */
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
-	printk("6");
 
 	/* GetXG21Sense (GPIO) */
 	if (HwDeviceExtension->jChipType == XG21)
@@ -1468,9 +1337,7 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 	if (HwDeviceExtension->jChipType == XG27)
 		XGINew_GetXG27Sense(HwDeviceExtension, pVBInfo);
 
-	printk("7");
-
-	/* 2.Reset Extended register */
+	/* Reset Extended register */
 
 	for (i = 0x06; i < 0x20; i++)
 		xgifb_reg_set(pVBInfo->P3c4, i, 0);
@@ -1478,105 +1345,37 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 	for (i = 0x21; i <= 0x27; i++)
 		xgifb_reg_set(pVBInfo->P3c4, i, 0);
 
-	/* for(i = 0x06; i <= 0x27; i++) */
-	/* xgifb_reg_set(pVBInfo->P3c4, i, 0); */
-
-	printk("8");
-
 	for (i = 0x31; i <= 0x3B; i++)
 		xgifb_reg_set(pVBInfo->P3c4, i, 0);
-	printk("9");
 
-	/* [Hsuan] 2004/08/20 Auto over driver for XG42 */
+	/* Auto over driver for XG42 */
 	if (HwDeviceExtension->jChipType == XG42)
 		xgifb_reg_set(pVBInfo->P3c4, 0x3B, 0xC0);
 
-	/* for (i = 0x30; i <= 0x3F; i++) */
-	/* xgifb_reg_set(pVBInfo->P3d4, i, 0); */
-
 	for (i = 0x79; i <= 0x7C; i++)
-		xgifb_reg_set(pVBInfo->P3d4, i, 0); /* shampoo 0208 */
-
-	printk("10");
+		xgifb_reg_set(pVBInfo->P3d4, i, 0);
 
 	if (HwDeviceExtension->jChipType >= XG20)
-		xgifb_reg_set(pVBInfo->P3d4, 0x97, *pVBInfo->pXGINew_CR97);
-
-	/* 3.SetMemoryClock
+		xgifb_reg_set(pVBInfo->P3d4, 0x97, pVBInfo->XGINew_CR97);
 
-	pVBInfo->ram_type = XGINew_GetXG20DRAMType(HwDeviceExtension, pVBInfo);
-	*/
-
-	printk("11");
-
-	/* 4.SetDefExt1Regs begin */
-	xgifb_reg_set(pVBInfo->P3c4, 0x07, *pVBInfo->pSR07);
+	/* SetDefExt1Regs begin */
+	xgifb_reg_set(pVBInfo->P3c4, 0x07, XGI330_SR07);
 	if (HwDeviceExtension->jChipType == XG27) {
-		xgifb_reg_set(pVBInfo->P3c4, 0x40, *pVBInfo->pSR40);
-		xgifb_reg_set(pVBInfo->P3c4, 0x41, *pVBInfo->pSR41);
+		xgifb_reg_set(pVBInfo->P3c4, 0x40, XG27_SR40);
+		xgifb_reg_set(pVBInfo->P3c4, 0x41, XG27_SR41);
 	}
 	xgifb_reg_set(pVBInfo->P3c4, 0x11, 0x0F);
-	xgifb_reg_set(pVBInfo->P3c4, 0x1F, *pVBInfo->pSR1F);
-	/* xgifb_reg_set(pVBInfo->P3c4, 0x20, 0x20); */
-	/* alan, 2001/6/26 Frame buffer can read/write SR20 */
+	xgifb_reg_set(pVBInfo->P3c4, 0x1F, XGI330_SR1F);
+	/* Frame buffer can read/write SR20 */
 	xgifb_reg_set(pVBInfo->P3c4, 0x20, 0xA0);
-	/* Hsuan, 2006/01/01 H/W request for slow corner chip */
+	/* H/W request for slow corner chip */
 	xgifb_reg_set(pVBInfo->P3c4, 0x36, 0x70);
-	if (HwDeviceExtension->jChipType == XG27) /* Alan 12/07/2006 */
-		xgifb_reg_set(pVBInfo->P3c4, 0x36, *pVBInfo->pSR36);
-
-	/* SR11 = 0x0F; */
-	/* xgifb_reg_set(pVBInfo->P3c4, 0x11, SR11); */
-
-	printk("12");
+	if (HwDeviceExtension->jChipType == XG27)
+		xgifb_reg_set(pVBInfo->P3c4, 0x36, XG27_SR36);
 
-	if (HwDeviceExtension->jChipType < XG20) { /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20) {
 		u32 Temp;
 
-		/* Set AGP Rate */
-		/*
-		temp1 = xgifb_reg_get(pVBInfo->P3c4, 0x3B);
-		temp1 &= 0x02;
-		if (temp1 == 0x02) {
-			outl(0x80000000, 0xcf8);
-			ChipsetID = inl(0x0cfc);
-			outl(0x8000002C, 0xcf8);
-			VendorID = inl(0x0cfc);
-			VendorID &= 0x0000FFFF;
-			outl(0x8001002C, 0xcf8);
-			GraphicVendorID = inl(0x0cfc);
-			GraphicVendorID &= 0x0000FFFF;
-
-			if (ChipsetID == 0x7301039)
-				xgifb_reg_set(pVBInfo->P3d4, 0x5F, 0x09);
-
-			ChipsetID &= 0x0000FFFF;
-
-			if ((ChipsetID == 0x700E) ||
-			    (ChipsetID == 0x1022) ||
-			    (ChipsetID == 0x1106) ||
-			    (ChipsetID == 0x10DE)) {
-				if (ChipsetID == 0x1106) {
-					if ((VendorID == 0x1019) &&
-					    (GraphicVendorID == 0x1019))
-						xgifb_reg_set(pVBInfo->P3d4,
-							      0x5F,
-							      0x0D);
-					else
-						xgifb_reg_set(pVBInfo->P3d4,
-							      0x5F,
-							      0x0B);
-				} else {
-					xgifb_reg_set(pVBInfo->P3d4,
-						      0x5F,
-						      0x0B);
-				}
-			}
-		}
-		*/
-
-		printk("13");
-
 		/* Set AGP customize registers (in SetDefAGPRegs) Start */
 		for (i = 0x47; i <= 0x4C; i++)
 			xgifb_reg_set(pVBInfo->P3d4,
@@ -1592,12 +1391,6 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 			xgifb_reg_set(pVBInfo->P3d4,
 				      i,
 				      pVBInfo->AGPReg[8 + i - 0x74]);
-		/* Set AGP customize registers (in SetDefAGPRegs) End */
-		/* [Hsuan]2004/12/14 AGP Input Delay Adjustment on 850 */
-		/*        outl(0x80000000, 0xcf8); */
-		/*        ChipsetID = inl(0x0cfc); */
-		/*        if (ChipsetID == 0x25308086) */
-		/*            xgifb_reg_set(pVBInfo->P3d4, 0x77, 0xF0); */
 
 		pci_read_config_dword(pdev, 0x50, &Temp);
 		Temp >>= 20;
@@ -1605,19 +1398,17 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 		if (Temp == 1)
 			xgifb_reg_set(pVBInfo->P3d4, 0x48, 0x20); /* CR48 */
-		printk("14");
 	} /* != XG20 */
 
 	/* Set PCI */
-	xgifb_reg_set(pVBInfo->P3c4, 0x23, *pVBInfo->pSR23);
-	xgifb_reg_set(pVBInfo->P3c4, 0x24, *pVBInfo->pSR24);
-	xgifb_reg_set(pVBInfo->P3c4, 0x25, pVBInfo->SR25[0]);
-	printk("15");
+	xgifb_reg_set(pVBInfo->P3c4, 0x23, XGI330_SR23);
+	xgifb_reg_set(pVBInfo->P3c4, 0x24, XGI330_SR24);
+	xgifb_reg_set(pVBInfo->P3c4, 0x25, XGI330_SR25);
 
-	if (HwDeviceExtension->jChipType < XG20) { /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20) {
 		/* Set VB */
 		XGI_UnLockCRT2(HwDeviceExtension, pVBInfo);
-		/* alan, disable VideoCapture */
+		/* disable VideoCapture */
 		xgifb_reg_and_or(pVBInfo->Part0Port, 0x3F, 0xEF, 0x00);
 		xgifb_reg_set(pVBInfo->Part1Port, 0x00, 0x00);
 		/* chk if BCLK>=100MHz */
@@ -1625,10 +1416,7 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 		temp = (unsigned char) ((temp1 >> 4) & 0x0F);
 
 		xgifb_reg_set(pVBInfo->Part1Port,
-			      0x02,
-			      (*pVBInfo->pCRT2Data_1_2));
-
-		printk("16");
+			      0x02, XGI330_CRT2Data_1_2);
 
 		xgifb_reg_set(pVBInfo->Part1Port, 0x2E, 0x08); /* use VB */
 	} /* != XG20 */
@@ -1640,51 +1428,36 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 		/* Not DDR */
 		xgifb_reg_set(pVBInfo->P3c4,
 			      0x31,
-			      (*pVBInfo->pSR31 & 0x3F) | 0x40);
+			      (XGI330_SR31 & 0x3F) | 0x40);
 		xgifb_reg_set(pVBInfo->P3c4,
 			      0x32,
-			      (*pVBInfo->pSR32 & 0xFC) | 0x01);
+			      (XGI330_SR32 & 0xFC) | 0x01);
 	} else {
-		xgifb_reg_set(pVBInfo->P3c4, 0x31, *pVBInfo->pSR31);
-		xgifb_reg_set(pVBInfo->P3c4, 0x32, *pVBInfo->pSR32);
+		xgifb_reg_set(pVBInfo->P3c4, 0x31, XGI330_SR31);
+		xgifb_reg_set(pVBInfo->P3c4, 0x32, XGI330_SR32);
 	}
-	xgifb_reg_set(pVBInfo->P3c4, 0x33, *pVBInfo->pSR33);
-	printk("17");
+	xgifb_reg_set(pVBInfo->P3c4, 0x33, XGI330_SR33);
 
-	/*
-	 SetPowerConsume (HwDeviceExtension, pVBInfo->P3c4);	*/
-
-	if (HwDeviceExtension->jChipType < XG20) { /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20) {
 		if (XGI_BridgeIsOn(pVBInfo) == 1) {
 			if (pVBInfo->IF_DEF_LVDS == 0) {
 				xgifb_reg_set(pVBInfo->Part2Port, 0x00, 0x1C);
 				xgifb_reg_set(pVBInfo->Part4Port,
-					      0x0D,
-					      *pVBInfo->pCRT2Data_4_D);
+					      0x0D, XGI330_CRT2Data_4_D);
 				xgifb_reg_set(pVBInfo->Part4Port,
-					      0x0E,
-					      *pVBInfo->pCRT2Data_4_E);
+					      0x0E, XGI330_CRT2Data_4_E);
 				xgifb_reg_set(pVBInfo->Part4Port,
-					      0x10,
-					      *pVBInfo->pCRT2Data_4_10);
+					      0x10, XGI330_CRT2Data_4_10);
 				xgifb_reg_set(pVBInfo->Part4Port, 0x0F, 0x3F);
 			}
 
 			XGI_LockCRT2(HwDeviceExtension, pVBInfo);
 		}
 	} /* != XG20 */
-	printk("18");
-
-	printk("181");
-
-	printk("182");
 
 	XGI_SenseCRT1(pVBInfo);
 
-	printk("183");
-	/* XGINew_DetectMonitor(HwDeviceExtension); */
 	if (HwDeviceExtension->jChipType == XG21) {
-		printk("186");
 
 		xgifb_reg_and_or(pVBInfo->P3d4,
 				 0x32,
@@ -1692,7 +1465,6 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 				 Monitor1Sense); /* Z9 default has CRT */
 		temp = GetXG21FPBits(pVBInfo);
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x37, ~0x01, temp);
-		printk("187");
 
 	}
 	if (HwDeviceExtension->jChipType == XG27) {
@@ -1703,7 +1475,6 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 		temp = GetXG27FPBits(pVBInfo);
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x37, ~0x03, temp);
 	}
-	printk("19");
 
 	pVBInfo->ram_type = XGINew_GetXG20DRAMType(HwDeviceExtension, pVBInfo);
 
@@ -1711,51 +1482,19 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 					 pVBInfo->P3d4,
 					 pVBInfo);
 
-	printk("20");
 	XGINew_SetDRAMSize_340(xgifb_info, HwDeviceExtension, pVBInfo);
-	printk("21");
-
-	printk("22");
 
-	/* SetDefExt2Regs begin */
-	/*
-	AGP = 1;
-	temp = (unsigned char) xgifb_reg_get(pVBInfo->P3c4, 0x3A);
-	temp &= 0x30;
-	if (temp == 0x30)
-		AGP = 0;
-
-	if (AGP == 0)
-		*pVBInfo->pSR21 &= 0xEF;
-
-	xgifb_reg_set(pVBInfo->P3c4, 0x21, *pVBInfo->pSR21);
-	if (AGP == 1)
-		*pVBInfo->pSR22 &= 0x20;
-	xgifb_reg_set(pVBInfo->P3c4, 0x22, *pVBInfo->pSR22);
-	*/
-	/* base = 0x80000000; */
-	/* OutPortLong(0xcf8, base); */
-	/* Temp = (InPortLong(0xcfc) & 0xFFFF); */
-	/* if (Temp == 0x1039) { */
 	xgifb_reg_set(pVBInfo->P3c4,
 		      0x22,
-		      (unsigned char) ((*pVBInfo->pSR22) & 0xFE));
-	/* } else { */
-	/*	xgifb_reg_set(pVBInfo->P3c4, 0x22, *pVBInfo->pSR22); */
-	/* } */
-
-	xgifb_reg_set(pVBInfo->P3c4, 0x21, *pVBInfo->pSR21);
+		      (unsigned char) ((pVBInfo->SR22) & 0xFE));
 
-	printk("23");
+	xgifb_reg_set(pVBInfo->P3c4, 0x21, pVBInfo->SR21);
 
 	XGINew_ChkSenseStatus(HwDeviceExtension, pVBInfo);
 	XGINew_SetModeScratch(HwDeviceExtension, pVBInfo);
 
-	printk("24");
-
 	xgifb_reg_set(pVBInfo->P3d4, 0x8c, 0x87);
 	xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x31);
-	printk("25");
 
 	return 1;
 } /* end of init */
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index b2f4338b110..e81149fc66e 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -1,20 +1,13 @@
-
-#include <linux/io.h>
 #include <linux/delay.h>
-#include <linux/types.h>
 #include "XGIfb.h"
 
-
 #include "vb_def.h"
-#include "vgatypes.h"
-#include "vb_struct.h"
-#include "vb_init.h"
 #include "vb_util.h"
 #include "vb_table.h"
 #include "vb_setmode.h"
 
-
 #define  IndexMask 0xff
+#define TVCLKBASE_315_25 (TVCLKBASE_315 + 25)
 
 static const unsigned short XGINew_VGA_DAC[] = {
 	0x00, 0x10, 0x04, 0x14, 0x01, 0x11, 0x09, 0x15,
@@ -45,9 +38,6 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 	pVBInfo->ModeResInfo
 			= (struct SiS_ModeResInfo_S *) XGI330_ModeResInfo;
 
-	pVBInfo->pOutputSelect = &XGI330_OutputSelect;
-	pVBInfo->pSoftSetting = &XGI330_SoftSetting;
-	pVBInfo->pSR07 = &XGI330_SR07;
 	pVBInfo->LCDResInfo = 0;
 	pVBInfo->LCDTypeInfo = 0;
 	pVBInfo->LCDInfo = 0;
@@ -56,36 +46,15 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 
 	pVBInfo->SR15 = XGI340_SR13;
 	pVBInfo->CR40 = XGI340_cr41;
-	pVBInfo->SR25 = XGI330_sr25;
-	pVBInfo->pSR31 = &XGI330_sr31;
-	pVBInfo->pSR32 = &XGI330_sr32;
 	pVBInfo->CR6B = XGI340_CR6B;
 	pVBInfo->CR6E = XGI340_CR6E;
 	pVBInfo->CR6F = XGI340_CR6F;
 	pVBInfo->CR89 = XGI340_CR89;
 	pVBInfo->AGPReg = XGI340_AGPReg;
 	pVBInfo->SR16 = XGI340_SR16;
-	pVBInfo->pCRCF = &XG40_CRCF;
-	pVBInfo->pXGINew_DRAMTypeDefinition = &XG40_DRAMTypeDefinition;
-
-	pVBInfo->CR49 = XGI330_CR49;
-	pVBInfo->pSR1F = &XGI330_SR1F;
-	pVBInfo->pSR21 = &XGI330_SR21;
-	pVBInfo->pSR22 = &XGI330_SR22;
-	pVBInfo->pSR23 = &XGI330_SR23;
-	pVBInfo->pSR24 = &XGI330_SR24;
-	pVBInfo->pSR33 = &XGI330_SR33;
-
-	pVBInfo->pCRT2Data_1_2 = &XGI330_CRT2Data_1_2;
-	pVBInfo->pCRT2Data_4_D = &XGI330_CRT2Data_4_D;
-	pVBInfo->pCRT2Data_4_E = &XGI330_CRT2Data_4_E;
-	pVBInfo->pCRT2Data_4_10 = &XGI330_CRT2Data_4_10;
-	pVBInfo->pRGBSenseData = &XGI330_RGBSenseData;
-	pVBInfo->pVideoSenseData = &XGI330_VideoSenseData;
-	pVBInfo->pYCSenseData = &XGI330_YCSenseData;
-	pVBInfo->pRGBSenseData2 = &XGI330_RGBSenseData2;
-	pVBInfo->pVideoSenseData2 = &XGI330_VideoSenseData2;
-	pVBInfo->pYCSenseData2 = &XGI330_YCSenseData2;
+
+	pVBInfo->SR21 = 0xa3;
+	pVBInfo->SR22 = 0xfb;
 
 	pVBInfo->NTSCTiming = XGI330_NTSCTiming;
 	pVBInfo->PALTiming = XGI330_PALTiming;
@@ -112,41 +81,22 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 	else
 		pVBInfo->LCDCapList = XGI_LCDCapList;
 
-	pVBInfo->XGI_TVDelayList = XGI301TVDelayList;
-	pVBInfo->XGI_TVDelayList2 = XGI301TVDelayList2;
-
-	pVBInfo->pXGINew_I2CDefinition = &XG40_I2CDefinition;
-
 	if (ChipType >= XG20)
-		pVBInfo->pXGINew_CR97 = &XG20_CR97;
+		pVBInfo->XGINew_CR97 = 0x10;
 
 	if (ChipType == XG27) {
 		unsigned char temp;
 		pVBInfo->MCLKData
 			= (struct SiS_MCLKData *) XGI27New_MCLKData;
 		pVBInfo->CR40 = XGI27_cr41;
-		pVBInfo->pXGINew_CR97 = &XG27_CR97;
-		pVBInfo->pSR36 = &XG27_SR36;
-		pVBInfo->pCR8F = &XG27_CR8F;
-		pVBInfo->pCRD0 = XG27_CRD0;
-		pVBInfo->pCRDE = XG27_CRDE;
-		pVBInfo->pSR40 = &XG27_SR40;
-		pVBInfo->pSR41 = &XG27_SR41;
+		pVBInfo->XGINew_CR97 = 0xc1;
 		pVBInfo->SR15 = XG27_SR13;
 
 		/*Z11m DDR*/
 		temp = xgifb_reg_get(pVBInfo->P3c4, 0x3B);
 		/* SR3B[7][3]MAA15 MAA11 (Power on Trapping) */
 		if (((temp & 0x88) == 0x80) || ((temp & 0x88) == 0x08))
-			pVBInfo->pXGINew_CR97 = &Z11m_CR97;
-	}
-
-	if (ChipType >= XG20) {
-		pVBInfo->pDVOSetting = &XG21_DVOSetting;
-		pVBInfo->pCR2E = &XG21_CR2E;
-		pVBInfo->pCR2F = &XG21_CR2F;
-		pVBInfo->pCR46 = &XG21_CR46;
-		pVBInfo->pCR47 = &XG21_CR47;
+			pVBInfo->XGINew_CR97 = 0x80;
 	}
 
 }
@@ -211,18 +161,15 @@ static void XGI_SetATTRegs(unsigned short ModeNo,
 
 	for (i = 0; i <= 0x13; i++) {
 		ARdata = pVBInfo->StandTable->ATTR[i];
-		if (modeflag & Charx8Dot) { /* ifndef Dot9 */
-			if (i == 0x13) {
-				if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
+
+		if ((modeflag & Charx8Dot) && i == 0x13) { /* ifndef Dot9 */
+			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
+				ARdata = 0;
+			} else {
+				if ((pVBInfo->VBInfo &
+				     (SetCRT2ToTV | SetCRT2ToLCD)) &&
+				    (pVBInfo->VBInfo & SetInSlaveMode))
 					ARdata = 0;
-				} else {
-					if (pVBInfo->VBInfo & (SetCRT2ToTV
-							| SetCRT2ToLCD)) {
-						if (pVBInfo->VBInfo &
-						    SetInSlaveMode)
-							ARdata = 0;
-					}
-				}
 			}
 		}
 
@@ -303,48 +250,30 @@ static unsigned char XGI_AjustCRT2Rate(unsigned short ModeNo,
 		if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
 			tempax |= SupportLCD;
 
-			if (pVBInfo->LCDResInfo != Panel_1280x1024) {
-				if (pVBInfo->LCDResInfo != Panel_1280x960) {
-					if (pVBInfo->LCDInfo &
-					    LCDNonExpanding) {
-						if (resinfo >= 9) {
-							tempax = 0;
-							return 0;
-						}
-					}
-				}
-			}
+			if (pVBInfo->LCDResInfo != Panel_1280x1024 &&
+			    pVBInfo->LCDResInfo != Panel_1280x960 &&
+			    (pVBInfo->LCDInfo & LCDNonExpanding) &&
+			    resinfo >= 9)
+				return 0;
 		}
 
 		if (pVBInfo->VBInfo & SetCRT2ToHiVision) { /* for HiTV */
 			if ((pVBInfo->VBType & VB_SIS301LV) &&
 			    (pVBInfo->VBExtInfo == VB_YPbPr1080i)) {
 				tempax |= SupportYPbPr750p;
-				if (pVBInfo->VBInfo & SetInSlaveMode) {
-					if (resinfo == 4)
-						return 0;
-
-					if (resinfo == 3)
-						return 0;
-
-					if (resinfo > 7)
-						return 0;
-				}
+				if ((pVBInfo->VBInfo & SetInSlaveMode) &&
+				    ((resinfo == 3) ||
+				     (resinfo == 4) ||
+				     (resinfo > 7)))
+					return 0;
 			} else {
 				tempax |= SupportHiVision;
-				if (pVBInfo->VBInfo & SetInSlaveMode) {
-					if (resinfo == 4)
+				if ((pVBInfo->VBInfo & SetInSlaveMode) &&
+				    ((resinfo == 4) ||
+				     (resinfo == 3 &&
+				      (pVBInfo->SetFlag & TVSimuMode)) ||
+				     (resinfo > 7)))
 						return 0;
-
-					if (resinfo == 3) {
-						if (pVBInfo->SetFlag
-								& TVSimuMode)
-							return 0;
-					}
-
-					if (resinfo > 7)
-						return 0;
-				}
 			}
 		} else {
 			if (pVBInfo->VBInfo & (SetCRT2ToAVIDEO |
@@ -354,23 +283,18 @@ static unsigned char XGI_AjustCRT2Rate(unsigned short ModeNo,
 					       SetCRT2ToHiVision)) {
 				tempax |= SupportTV;
 
-				if (pVBInfo->VBType & (VB_SIS301B | VB_SIS302B
-						| VB_SIS301LV | VB_SIS302LV
-						| VB_XGI301C)) {
+				if (pVBInfo->VBType & (VB_SIS301B |
+						       VB_SIS302B |
+						       VB_SIS301LV |
+						       VB_SIS302LV |
+						       VB_XGI301C))
 					tempax |= SupportTV1024;
-				}
 
-				if (!(pVBInfo->VBInfo & TVSetPAL)) {
-					if (modeflag & NoSupportSimuTV) {
-						if (pVBInfo->VBInfo &
-						    SetInSlaveMode) {
-							if (!(pVBInfo->VBInfo &
-							      SetNotSimuMode)) {
-								return 0;
-							}
-						}
-					}
-				}
+				if (!(pVBInfo->VBInfo & TVSetPAL) &&
+				    (modeflag & NoSupportSimuTV) &&
+				    (pVBInfo->VBInfo & SetInSlaveMode) &&
+				    (!(pVBInfo->VBInfo & SetNotSimuMode)))
+					return 0;
 			}
 		}
 	} else { /* for LVDS */
@@ -793,13 +717,6 @@ static void xgifb_set_lcd(int chip_id,
 		}
 	}
 
-	if (((*pVBInfo->pDVOSetting) & 0xC0) == 0xC0) {
-		xgifb_reg_set(pVBInfo->P3d4, 0x2E, *pVBInfo->pCR2E);
-		xgifb_reg_set(pVBInfo->P3d4, 0x2F, *pVBInfo->pCR2F);
-		xgifb_reg_set(pVBInfo->P3d4, 0x46, *pVBInfo->pCR46);
-		xgifb_reg_set(pVBInfo->P3d4, 0x47, *pVBInfo->pCR47);
-	}
-
 	if (chip_id == XG27) {
 		XGI_SetXG27FPBits(pVBInfo);
 	} else {
@@ -1018,24 +935,6 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned short LCDXlat1VCLK[4] = { VCLK65_315 + 2,
-					   VCLK65_315 + 2,
-					   VCLK65_315 + 2,
-					   VCLK65_315 + 2 };
-	unsigned short LCDXlat2VCLK[4] = { VCLK108_2_315 + 5,
-					   VCLK108_2_315 + 5,
-					   VCLK108_2_315 + 5,
-					   VCLK108_2_315 + 5 };
-	unsigned short LVDSXlat1VCLK[4] = { VCLK40, VCLK40, VCLK40, VCLK40 };
-	unsigned short LVDSXlat2VCLK[4] = { VCLK65_315 + 2,
-					    VCLK65_315 + 2,
-					    VCLK65_315 + 2,
-					    VCLK65_315 + 2 };
-	unsigned short LVDSXlat3VCLK[4] = { VCLK65_315 + 2,
-					    VCLK65_315 + 2,
-					    VCLK65_315 + 2,
-					    VCLK65_315 + 2 };
-
 	unsigned short CRT2Index, VCLKIndex;
 	unsigned short modeflag, resinfo;
 
@@ -1046,27 +945,26 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 
 	if (pVBInfo->IF_DEF_LVDS == 0) {
 		CRT2Index = CRT2Index >> 6; /*  for LCD */
-		if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) { /*301b*/
+		if (pVBInfo->VBInfo &
+		    (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) { /*301b*/
 			if (pVBInfo->LCDResInfo != Panel_1024x768)
-				VCLKIndex = LCDXlat2VCLK[CRT2Index];
+				/* LCDXlat2VCLK */
+				VCLKIndex = VCLK108_2_315 + 5;
 			else
-				VCLKIndex = LCDXlat1VCLK[CRT2Index];
+				VCLKIndex = VCLK65_315 + 2; /* LCDXlat1VCLK */
 		} else if (pVBInfo->VBInfo & SetCRT2ToHiVision) {
-			if (pVBInfo->SetFlag & RPLLDIV2XO) {
-				VCLKIndex = TVCLKBASE_315 + HiTVVCLKDIV2;
-				VCLKIndex += 25;
-			} else {
-				VCLKIndex = TVCLKBASE_315 + HiTVVCLK;
-				VCLKIndex += 25;
-			}
+			if (pVBInfo->SetFlag & RPLLDIV2XO)
+				VCLKIndex = TVCLKBASE_315_25 + HiTVVCLKDIV2;
+			else
+				VCLKIndex = TVCLKBASE_315_25 + HiTVVCLK;
 
 			if (pVBInfo->SetFlag & TVSimuMode) {
 				if (modeflag & Charx8Dot) {
-					VCLKIndex = TVCLKBASE_315 + HiTVSimuVCLK;
-					VCLKIndex += 25;
+					VCLKIndex = TVCLKBASE_315_25 +
+							HiTVSimuVCLK;
 				} else {
-					VCLKIndex = TVCLKBASE_315 + HiTVTextVCLK;
-					VCLKIndex += 25;
+					VCLKIndex = TVCLKBASE_315_25 +
+							HiTVTextVCLK;
 				}
 			}
 
@@ -1083,13 +981,10 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 					VCLKIndex = YPbPr525iVCLK;
 			}
 		} else if (pVBInfo->VBInfo & SetCRT2ToTV) {
-			if (pVBInfo->SetFlag & RPLLDIV2XO) {
-				VCLKIndex = TVCLKBASE_315 + TVVCLKDIV2;
-				VCLKIndex += 25;
-			} else {
-				VCLKIndex = TVCLKBASE_315 + TVVCLK;
-				VCLKIndex += 25;
-			}
+			if (pVBInfo->SetFlag & RPLLDIV2XO)
+				VCLKIndex = TVCLKBASE_315_25 + TVVCLKDIV2;
+			else
+				VCLKIndex = TVCLKBASE_315_25 + TVVCLK;
 		} else { /* for CRT2 */
 			/* di+Ext_CRTVCLK */
 			VCLKIndex = pVBInfo->RefIndex[RefreshRateTableIndex].
@@ -1097,16 +992,12 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 			VCLKIndex &= IndexMask;
 		}
 	} else { /* LVDS */
-		VCLKIndex = CRT2Index;
-		VCLKIndex = VCLKIndex >> 6;
 		if ((pVBInfo->LCDResInfo == Panel_800x600) ||
 		    (pVBInfo->LCDResInfo == Panel_320x480))
-			VCLKIndex = LVDSXlat1VCLK[VCLKIndex];
-		else if ((pVBInfo->LCDResInfo == Panel_1024x768) ||
-			 (pVBInfo->LCDResInfo == Panel_1024x768x75))
-			VCLKIndex = LVDSXlat2VCLK[VCLKIndex];
+			VCLKIndex = VCLK40; /* LVDSXlat1VCLK */
 		else
-			VCLKIndex = LVDSXlat3VCLK[VCLKIndex];
+			VCLKIndex = VCLK65_315 + 2; /* LVDSXlat2VCLK,
+						       LVDSXlat3VCLK  */
 	}
 
 	return VCLKIndex;
@@ -1945,7 +1836,6 @@ static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
 		i++;
 	}
 
-	/* 07/05/22 */
 	if (table == 0x04) {
 		switch (tempdi[i].DATAPTR) {
 		case 0:
@@ -2019,12 +1909,12 @@ static void XGI_GetLVDSData(unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short tempbx;
-	struct XGI330_LVDSDataStruct *LCDPtr = NULL;
+	struct SiS_LVDSData *LCDPtr = NULL;
 
 	tempbx = 2;
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-		LCDPtr = (struct XGI330_LVDSDataStruct *) XGI_GetLcdPtr(tempbx,
+		LCDPtr = (struct SiS_LVDSData *)XGI_GetLcdPtr(tempbx,
 				ModeNo, ModeIdIndex, RefreshRateTableIndex,
 				pVBInfo);
 		pVBInfo->VGAHT = LCDPtr->VGAHT;
@@ -2041,7 +1931,8 @@ static void XGI_GetLVDSData(unsigned short ModeNo, unsigned short ModeIdIndex,
 				pVBInfo->HDE = 1024;
 				pVBInfo->VDE = 768;
 			} else if ((pVBInfo->LCDResInfo == Panel_1280x1024) ||
-				   (pVBInfo->LCDResInfo == Panel_1280x1024x75)) {
+				   (pVBInfo->LCDResInfo ==
+					Panel_1280x1024x75)) {
 				pVBInfo->HDE = 1280;
 				pVBInfo->VDE = 1024;
 			} else if (pVBInfo->LCDResInfo == Panel_1400x1050) {
@@ -2498,7 +2389,8 @@ static unsigned char XGI_GetVCLKPtr(unsigned short RefreshRateTableIndex,
 				if (pVBInfo->TVInfo & TVSimuMode) {
 					tempal = TVCLKBASE_315 + HiTVSimuVCLK;
 					if (!(modeflag & Charx8Dot))
-						tempal = TVCLKBASE_315 + HiTVTextVCLK;
+						tempal = TVCLKBASE_315 +
+								HiTVTextVCLK;
 
 				}
 				return tempal;
@@ -2544,8 +2436,8 @@ static void XGI_GetVCLKLen(unsigned char tempal, unsigned char *di_0,
 {
 	if (pVBInfo->VBType & (VB_SIS301 | VB_SIS301B | VB_SIS302B
 			| VB_SIS301LV | VB_SIS302LV | VB_XGI301C)) {
-		if ((!(pVBInfo->VBInfo & XGI_SetCRT2ToLCDA)) && (pVBInfo->SetFlag
-				& ProgrammingCRT2)) {
+		if ((!(pVBInfo->VBInfo & XGI_SetCRT2ToLCDA)) &&
+		    (pVBInfo->SetFlag & ProgrammingCRT2)) {
 			*di_0 = (unsigned char) XGI_VBVCLKData[tempal].SR2B;
 			*di_1 = XGI_VBVCLKData[tempal].SR2C;
 		}
@@ -2605,7 +2497,7 @@ static void XGI_UpdateModeInfo(struct xgi_hw_device_info *HwDeviceExtension,
 		temp &= 0x0f;
 
 		if (!(temp == 0x08)) {
-			/* Check ChannelA by Part1_13 [2003/10/03] */
+			/* Check ChannelA */
 			tempax = xgifb_reg_get(pVBInfo->Part1Port, 0x13);
 			if (tempax & 0x04)
 				tempcl = tempcl | ActiveLCD;
@@ -2672,42 +2564,45 @@ void XGI_GetVBType(struct vb_device_info *pVBInfo)
 {
 	unsigned short flag, tempbx, tempah;
 
-	if (pVBInfo->IF_DEF_LVDS == 0) {
-		tempbx = VB_SIS302B;
-		flag = xgifb_reg_get(pVBInfo->Part4Port, 0x00);
-		if (flag != 0x02) {
-			tempbx = VB_SIS301;
-			flag = xgifb_reg_get(pVBInfo->Part4Port, 0x01);
-			if (flag >= 0xB0) {
-				tempbx = VB_SIS301B;
-				if (flag >= 0xC0) {
-					tempbx = VB_XGI301C;
-					if (flag >= 0xD0) {
-						tempbx = VB_SIS301LV;
-						if (flag >= 0xE0) {
-							tempbx = VB_SIS302LV;
-							tempah = xgifb_reg_get(
-							    pVBInfo->Part4Port,
-							    0x39);
-							if (tempah != 0xFF)
-								tempbx =
-								    VB_XGI301C;
-						}
-					}
-				}
+	if (pVBInfo->IF_DEF_LVDS != 0)
+		return;
 
-				if (tempbx & (VB_SIS301B | VB_SIS302B)) {
-					flag = xgifb_reg_get(
-							pVBInfo->Part4Port,
-							0x23);
+	tempbx = VB_SIS302B;
+	flag = xgifb_reg_get(pVBInfo->Part4Port, 0x00);
+	if (flag == 0x02)
+		goto finish;
 
-					if (!(flag & 0x02))
-						tempbx = tempbx | VB_NoLCD;
-				}
-			}
-		}
-		pVBInfo->VBType = tempbx;
+	tempbx = VB_SIS301;
+	flag = xgifb_reg_get(pVBInfo->Part4Port, 0x01);
+	if (flag < 0xB0)
+		goto finish;
+
+	tempbx = VB_SIS301B;
+	if (flag < 0xC0)
+		goto bigger_than_0xB0;
+
+	tempbx = VB_XGI301C;
+	if (flag < 0xD0)
+		goto bigger_than_0xB0;
+
+	tempbx = VB_SIS301LV;
+	if (flag < 0xE0)
+		goto bigger_than_0xB0;
+
+	tempbx = VB_SIS302LV;
+	tempah = xgifb_reg_get(pVBInfo->Part4Port, 0x39);
+	if (tempah != 0xFF)
+		tempbx = VB_XGI301C;
+
+bigger_than_0xB0:
+	if (tempbx & (VB_SIS301B | VB_SIS302B)) {
+		flag = xgifb_reg_get(pVBInfo->Part4Port, 0x23);
+		if (!(flag & 0x02))
+			tempbx = tempbx | VB_NoLCD;
 	}
+
+finish:
+	pVBInfo->VBType = tempbx;
 }
 
 static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
@@ -2721,187 +2616,183 @@ static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 	pVBInfo->ModeType = modeflag & ModeTypeMask;
 	tempbx = 0;
 
-	if (pVBInfo->VBType & 0xFFFF) {
-		/* Check Display Device */
-		temp = xgifb_reg_get(pVBInfo->P3d4, 0x30);
-		tempbx = tempbx | temp;
-		temp = xgifb_reg_get(pVBInfo->P3d4, 0x31);
-		push = temp;
-		push = push << 8;
-		tempax = temp << 8;
-		tempbx = tempbx | tempax;
-		temp = (SetCRT2ToDualEdge | SetCRT2ToYPbPr525750 | XGI_SetCRT2ToLCDA
-				| SetInSlaveMode | DisableCRT2Display);
-		temp = 0xFFFF ^ temp;
-		tempbx &= temp;
-
-		temp = xgifb_reg_get(pVBInfo->P3d4, 0x38);
+	if (!(pVBInfo->VBType & 0xFFFF))
+		return;
 
-		if (pVBInfo->IF_DEF_LCDA == 1) {
-
-			if ((HwDeviceExtension->jChipType >= XG20) ||
-			    (HwDeviceExtension->jChipType >= XG40)) {
-				if (pVBInfo->IF_DEF_LVDS == 0) {
-					if (pVBInfo->VBType &
-					    (VB_SIS302B |
-					     VB_SIS301LV |
-					     VB_SIS302LV |
-					     VB_XGI301C)) {
-						if (temp & EnableDualEdge) {
-							tempbx |=
-							    SetCRT2ToDualEdge;
-
-							if (temp & SetToLCDA)
-								tempbx |=
-								  XGI_SetCRT2ToLCDA;
-						}
-					}
+	/* Check Display Device */
+	temp = xgifb_reg_get(pVBInfo->P3d4, 0x30);
+	tempbx = tempbx | temp;
+	temp = xgifb_reg_get(pVBInfo->P3d4, 0x31);
+	push = temp;
+	push = push << 8;
+	tempax = temp << 8;
+	tempbx = tempbx | tempax;
+	temp = (SetCRT2ToDualEdge | SetCRT2ToYPbPr525750 | XGI_SetCRT2ToLCDA
+		| SetInSlaveMode | DisableCRT2Display);
+	temp = 0xFFFF ^ temp;
+	tempbx &= temp;
+
+	temp = xgifb_reg_get(pVBInfo->P3d4, 0x38);
+
+	if (pVBInfo->IF_DEF_LCDA == 1) {
+
+		if (((HwDeviceExtension->jChipType >= XG20) ||
+		     (HwDeviceExtension->jChipType >= XG40)) &&
+		    (pVBInfo->IF_DEF_LVDS == 0)) {
+			if (pVBInfo->VBType &
+			    (VB_SIS302B |
+			     VB_SIS301LV |
+			     VB_SIS302LV |
+			     VB_XGI301C)) {
+				if (temp & EnableDualEdge) {
+					tempbx |= SetCRT2ToDualEdge;
+					if (temp & SetToLCDA)
+						tempbx |= XGI_SetCRT2ToLCDA;
 				}
 			}
 		}
+	}
 
-		if (pVBInfo->IF_DEF_YPbPr == 1) {
-			/* [Billy] 07/05/04 */
-			if (((pVBInfo->IF_DEF_LVDS == 0) &&
-			    ((pVBInfo->VBType & VB_SIS301LV) ||
-			    (pVBInfo->VBType & VB_SIS302LV) ||
-			    (pVBInfo->VBType & VB_XGI301C)))) {
-				if (temp & SetYPbPr) {
-					if (pVBInfo->IF_DEF_HiVision == 1) {
-						/* shampoo add for new
-						 * scratch */
-						temp = xgifb_reg_get(
-								pVBInfo->P3d4,
-								0x35);
-						temp &= YPbPrMode;
-						tempbx |= SetCRT2ToHiVision;
-
-						if (temp != YPbPrMode1080i) {
-							tempbx &=
-							 (~SetCRT2ToHiVision);
-							tempbx |=
-							 SetCRT2ToYPbPr525750;
-						}
+	if (pVBInfo->IF_DEF_YPbPr == 1) {
+		if (((pVBInfo->IF_DEF_LVDS == 0) &&
+		     ((pVBInfo->VBType & VB_SIS301LV) ||
+		      (pVBInfo->VBType & VB_SIS302LV) ||
+		      (pVBInfo->VBType & VB_XGI301C)))) {
+			if (temp & SetYPbPr) {
+				if (pVBInfo->IF_DEF_HiVision == 1) {
+					/* shampoo add for new
+					 * scratch */
+					temp = xgifb_reg_get(
+						pVBInfo->P3d4,
+						0x35);
+					temp &= YPbPrMode;
+					tempbx |= SetCRT2ToHiVision;
+
+					if (temp != YPbPrMode1080i) {
+						tempbx &=
+							(~SetCRT2ToHiVision);
+						tempbx |=
+							SetCRT2ToYPbPr525750;
 					}
 				}
 			}
 		}
+	}
 
-		tempax = push; /* restore CR31 */
+	tempax = push; /* restore CR31 */
 
-		if (pVBInfo->IF_DEF_LVDS == 0) {
-			if (pVBInfo->IF_DEF_YPbPr == 1) {
-				if (pVBInfo->IF_DEF_HiVision == 1)
-					temp = 0x09FC;
-				else
-					temp = 0x097C;
-			} else {
-				if (pVBInfo->IF_DEF_HiVision == 1)
-					temp = 0x01FC;
-				else
-					temp = 0x017C;
-			}
-		} else { /* 3nd party chip */
-			temp = SetCRT2ToLCD;
-		}
-
-		if (!(tempbx & temp)) {
-			tempax |= DisableCRT2Display;
-			tempbx = 0;
-		}
-
-		if (pVBInfo->IF_DEF_LCDA == 1) { /* Select Display Device */
-			if (!(pVBInfo->VBType & VB_NoLCD)) {
-				if (tempbx & XGI_SetCRT2ToLCDA) {
-					if (tempbx & SetSimuScanMode)
-						tempbx &= (~(SetCRT2ToLCD |
-							   SetCRT2ToRAMDAC |
-							   SwitchCRT2));
-					else
-						tempbx &= (~(SetCRT2ToLCD |
-							     SetCRT2ToRAMDAC |
-							     SetCRT2ToTV |
-							     SwitchCRT2));
-				}
-			}
+	if (pVBInfo->IF_DEF_LVDS == 0) {
+		if (pVBInfo->IF_DEF_YPbPr == 1) {
+			if (pVBInfo->IF_DEF_HiVision == 1)
+				temp = 0x09FC;
+			else
+				temp = 0x097C;
+		} else {
+			if (pVBInfo->IF_DEF_HiVision == 1)
+				temp = 0x01FC;
+			else
+				temp = 0x017C;
 		}
+	} else { /* 3nd party chip */
+		temp = SetCRT2ToLCD;
+	}
 
-		/* shampoo add */
-		/* for driver abnormal */
-		if (!(tempbx & (SwitchCRT2 | SetSimuScanMode))) {
-			if (pVBInfo->IF_DEF_CRT2Monitor == 1) {
-				if (tempbx & SetCRT2ToRAMDAC) {
-					tempbx &= (0xFF00 |
-						   SetCRT2ToRAMDAC |
-						   SwitchCRT2 |
-						   SetSimuScanMode);
-					tempbx &= (0x00FF | (~SetCRT2ToYPbPr525750));
-				}
-			} else {
-				tempbx &= (~(SetCRT2ToRAMDAC |
-					   SetCRT2ToLCD |
-					   SetCRT2ToTV));
+	if (!(tempbx & temp)) {
+		tempax |= DisableCRT2Display;
+		tempbx = 0;
+	}
+
+	if (pVBInfo->IF_DEF_LCDA == 1) { /* Select Display Device */
+		if (!(pVBInfo->VBType & VB_NoLCD)) {
+			if (tempbx & XGI_SetCRT2ToLCDA) {
+				if (tempbx & SetSimuScanMode)
+					tempbx &= (~(SetCRT2ToLCD |
+						     SetCRT2ToRAMDAC |
+						     SwitchCRT2));
+				else
+					tempbx &= (~(SetCRT2ToLCD |
+						     SetCRT2ToRAMDAC |
+						     SetCRT2ToTV |
+						     SwitchCRT2));
 			}
 		}
+	}
 
-		if (!(pVBInfo->VBType & VB_NoLCD)) {
-			if (tempbx & SetCRT2ToLCD) {
+	/* shampoo add */
+	/* for driver abnormal */
+	if (!(tempbx & (SwitchCRT2 | SetSimuScanMode))) {
+		if (pVBInfo->IF_DEF_CRT2Monitor == 1) {
+			if (tempbx & SetCRT2ToRAMDAC) {
 				tempbx &= (0xFF00 |
-					   SetCRT2ToLCD |
+					   SetCRT2ToRAMDAC |
 					   SwitchCRT2 |
 					   SetSimuScanMode);
 				tempbx &= (0x00FF | (~SetCRT2ToYPbPr525750));
 			}
+		} else {
+			tempbx &= (~(SetCRT2ToRAMDAC |
+				     SetCRT2ToLCD |
+				     SetCRT2ToTV));
 		}
+	}
 
-		if (tempbx & SetCRT2ToSCART) {
+	if (!(pVBInfo->VBType & VB_NoLCD)) {
+		if (tempbx & SetCRT2ToLCD) {
 			tempbx &= (0xFF00 |
-				   SetCRT2ToSCART |
+				   SetCRT2ToLCD |
 				   SwitchCRT2 |
 				   SetSimuScanMode);
 			tempbx &= (0x00FF | (~SetCRT2ToYPbPr525750));
 		}
+	}
 
-		if (pVBInfo->IF_DEF_YPbPr == 1) {
-			if (tempbx & SetCRT2ToYPbPr525750)
-				tempbx &= (0xFF00 |
-					   SwitchCRT2 |
-					   SetSimuScanMode);
-		}
+	if (tempbx & SetCRT2ToSCART) {
+		tempbx &= (0xFF00 |
+			   SetCRT2ToSCART |
+			   SwitchCRT2 |
+			   SetSimuScanMode);
+		tempbx &= (0x00FF | (~SetCRT2ToYPbPr525750));
+	}
 
-		if (pVBInfo->IF_DEF_HiVision == 1) {
-			if (tempbx & SetCRT2ToHiVision)
-				tempbx &= (0xFF00 |
-					   SetCRT2ToHiVision |
-					   SwitchCRT2 |
-					   SetSimuScanMode);
-		}
+	if (pVBInfo->IF_DEF_YPbPr == 1) {
+		if (tempbx & SetCRT2ToYPbPr525750)
+			tempbx &= (0xFF00 |
+				   SwitchCRT2 |
+				   SetSimuScanMode);
+	}
 
-		if (tempax & DisableCRT2Display) { /* Set Display Device Info */
-			if (!(tempbx & (SwitchCRT2 | SetSimuScanMode)))
-				tempbx = DisableCRT2Display;
-		}
+	if (pVBInfo->IF_DEF_HiVision == 1) {
+		if (tempbx & SetCRT2ToHiVision)
+			tempbx &= (0xFF00 |
+				   SetCRT2ToHiVision |
+				   SwitchCRT2 |
+				   SetSimuScanMode);
+	}
 
-		if (!(tempbx & DisableCRT2Display)) {
-			if ((!(tempbx & DriverMode)) ||
-			    (!(modeflag & CRT2Mode))) {
-				if (pVBInfo->IF_DEF_LCDA == 1) {
-					if (!(tempbx & XGI_SetCRT2ToLCDA))
-						tempbx |= (SetInSlaveMode |
-							   SetSimuScanMode);
-				}
-			}
+	if (tempax & DisableCRT2Display) { /* Set Display Device Info */
+		if (!(tempbx & (SwitchCRT2 | SetSimuScanMode)))
+			tempbx = DisableCRT2Display;
+	}
 
-			/* LCD+TV can't support in slave mode
-			 * (Force LCDA+TV->LCDB) */
-			if ((tempbx & SetInSlaveMode) &&
-			    (tempbx & XGI_SetCRT2ToLCDA)) {
-				tempbx ^= (SetCRT2ToLCD |
-					  XGI_SetCRT2ToLCDA |
-					  SetCRT2ToDualEdge);
-				pVBInfo->SetFlag |= ReserveTVOption;
+	if (!(tempbx & DisableCRT2Display)) {
+		if ((!(tempbx & DriverMode)) ||
+		    (!(modeflag & CRT2Mode))) {
+			if (pVBInfo->IF_DEF_LCDA == 1) {
+				if (!(tempbx & XGI_SetCRT2ToLCDA))
+					tempbx |= (SetInSlaveMode |
+						   SetSimuScanMode);
 			}
 		}
+
+		/* LCD+TV can't support in slave mode
+		 * (Force LCDA+TV->LCDB) */
+		if ((tempbx & SetInSlaveMode) &&
+		    (tempbx & XGI_SetCRT2ToLCDA)) {
+			tempbx ^= (SetCRT2ToLCD |
+				   XGI_SetCRT2ToLCDA |
+				   SetCRT2ToDualEdge);
+			pVBInfo->SetFlag |= ReserveTVOption;
+		}
 	}
 
 	pVBInfo->VBInfo = tempbx;
@@ -3013,7 +2904,7 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 	if (tempbx == 0)
 		tempbx = Panel_1024x768; /* default */
 
-	/* LCD75 [2003/8/22] Vicent */
+	/* LCD75 */
 	if ((tempbx == Panel_1024x768) || (tempbx == Panel_1280x1024)) {
 		if (pVBInfo->VBInfo & DriverMode) {
 			tempax = xgifb_reg_get(pVBInfo->P3d4, 0x33);
@@ -3062,7 +2953,10 @@ static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
 		if ((pVBInfo->LCDResInfo == Panel_1400x1050) && (pVBInfo->VBInfo
 				& SetCRT2ToLCD) && (resinfo == 9) &&
 				(!(tempbx & EnableScalingLCD)))
-			/* set to center in 1280x1024 LCDB for Panel_1400x1050 */
+			/*
+			 * set to center in 1280x1024 LCDB
+			 * for Panel_1400x1050
+			 */
 			tempbx |= SetLCDtoNonExpanding;
 	}
 
@@ -3377,46 +3271,47 @@ static void XGI_GetCRT2ResInfo(unsigned short ModeNo,
 	if (modeflag & DoubleScanMode)
 		yres *= 2;
 
-	if (pVBInfo->VBInfo & SetCRT2ToLCD) {
-		if (pVBInfo->IF_DEF_LVDS == 0) {
-			if (pVBInfo->LCDResInfo == Panel_1600x1200) {
-				if (!(pVBInfo->LCDInfo & XGI_LCDVESATiming)) {
-					if (yres == 1024)
-						yres = 1056;
-				}
+	if (!(pVBInfo->VBInfo & SetCRT2ToLCD))
+		goto exit;
+
+	if (pVBInfo->IF_DEF_LVDS == 0) {
+		if (pVBInfo->LCDResInfo == Panel_1600x1200) {
+			if (!(pVBInfo->LCDInfo & XGI_LCDVESATiming)) {
+				if (yres == 1024)
+					yres = 1056;
 			}
+		}
 
-			if (pVBInfo->LCDResInfo == Panel_1280x1024) {
-				if (yres == 400)
-					yres = 405;
-				else if (yres == 350)
-					yres = 360;
+		if (pVBInfo->LCDResInfo == Panel_1280x1024) {
+			if (yres == 400)
+				yres = 405;
+			else if (yres == 350)
+				yres = 360;
 
-				if (pVBInfo->LCDInfo & XGI_LCDVESATiming) {
-					if (yres == 360)
-						yres = 375;
-				}
+			if (pVBInfo->LCDInfo & XGI_LCDVESATiming) {
+				if (yres == 360)
+					yres = 375;
 			}
+		}
 
-			if (pVBInfo->LCDResInfo == Panel_1024x768) {
-				if (!(pVBInfo->LCDInfo & XGI_LCDVESATiming)) {
-					if (!(pVBInfo->LCDInfo
-							& LCDNonExpanding)) {
-						if (yres == 350)
-							yres = 357;
-						else if (yres == 400)
-							yres = 420;
-						else if (yres == 480)
-							yres = 525;
-					}
+		if (pVBInfo->LCDResInfo == Panel_1024x768) {
+			if (!(pVBInfo->LCDInfo & XGI_LCDVESATiming)) {
+				if (!(pVBInfo->LCDInfo & LCDNonExpanding)) {
+					if (yres == 350)
+						yres = 357;
+					else if (yres == 400)
+						yres = 420;
+					else if (yres == 480)
+						yres = 525;
 				}
 			}
 		}
-
-		if (xres == 720)
-			xres = 640;
 	}
 
+	if (xres == 720)
+		xres = 640;
+
+exit:
 	pVBInfo->VGAHDE = xres;
 	pVBInfo->HDE = xres;
 	pVBInfo->VGAVDE = yres;
@@ -4152,24 +4047,16 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	}
 
 	if (pVBInfo->VBInfo & SetCRT2ToHiVision) {
-		if (pVBInfo->VBType & VB_SIS301LV) {
-			if (pVBInfo->TVInfo & TVSetHiVision) {
-				tempbx -= 10;
-			} else {
-				if (pVBInfo->TVInfo & TVSimuMode) {
-					if (pVBInfo->TVInfo & TVSetPAL) {
-						if (pVBInfo->VBType &
-						    VB_SIS301LV) {
-							if (!(pVBInfo->TVInfo &
-							    (TVSetYPbPr525p |
-							    TVSetYPbPr750p |
-							    TVSetHiVision)))
-								tempbx += 40;
-						} else {
-							tempbx += 40;
-						}
-					}
-				}
+		if ((pVBInfo->VBType & VB_SIS301LV) &&
+		    !(pVBInfo->TVInfo & TVSetHiVision)) {
+			if ((pVBInfo->TVInfo & TVSimuMode) &&
+			    (pVBInfo->TVInfo & TVSetPAL)) {
+				if (!(pVBInfo->VBType & VB_SIS301LV) ||
+				    !(pVBInfo->TVInfo &
+				      (TVSetYPbPr525p |
+				       TVSetYPbPr750p |
+				       TVSetHiVision)))
+					tempbx += 40;
 			}
 		} else {
 			tempbx -= 10;
@@ -4677,7 +4564,7 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 		}
 	}
 
-	/* [ycchen] 01/14/03 Modify for 301C PALM Support */
+	/* Modify for 301C PALM Support */
 	if (pVBInfo->VBType & VB_XGI301C) {
 		if (pVBInfo->TVInfo & TVSetPALM)
 			xgifb_reg_and_or(pVBInfo->Part2Port, 0x4E, ~0x08,
@@ -5572,7 +5459,8 @@ static void XGI_DisableBridge(struct xgifb_video_info *xgifb_info,
 			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
 				if (pVBInfo->VBInfo & SetCRT2ToDualEdge) {
 					tempah = 0x7F; /* Disable Channel A */
-					if (!(pVBInfo->VBInfo & XGI_SetCRT2ToLCDA))
+					if (!(pVBInfo->VBInfo &
+					      XGI_SetCRT2ToLCDA))
 						/* Disable Channel B */
 						tempah = 0xBF;
 
@@ -5591,9 +5479,10 @@ static void XGI_DisableBridge(struct xgifb_video_info *xgifb_info,
 		xgifb_reg_and(pVBInfo->Part4Port, 0x1F, tempah);
 
 		if (pVBInfo->VBType & (VB_SIS302LV | VB_XGI301C)) {
-			if (((pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)))
-					|| (XGI_DisableChISLCD(pVBInfo))
-					|| (XGI_IsLCDON(pVBInfo)))
+			if (((pVBInfo->VBInfo &
+			      (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)))
+			    || (XGI_DisableChISLCD(pVBInfo))
+			    || (XGI_IsLCDON(pVBInfo)))
 				/* LVDS Driver power down */
 				xgifb_reg_or(pVBInfo->Part4Port, 0x30, 0x80);
 		}
@@ -5748,32 +5637,20 @@ static void XGI_GetTVPtrIndex2(unsigned short *tempbx, unsigned char *tempcl,
 
 static void XGI_SetDelayComp(struct vb_device_info *pVBInfo)
 {
-	unsigned short index;
-
 	unsigned char tempah, tempbl, tempbh;
 
 	if (pVBInfo->VBType & (VB_SIS301B | VB_SIS302B | VB_SIS301LV
 			| VB_SIS302LV | VB_XGI301C)) {
 		if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA
 				| SetCRT2ToTV | SetCRT2ToRAMDAC)) {
-			tempbl = 0;
 			tempbh = 0;
-
-			index = XGI_GetTVPtrIndex(pVBInfo); /* Get TV Delay */
-			tempbl = pVBInfo->XGI_TVDelayList[index];
-
-			if (pVBInfo->VBType & (VB_SIS301B | VB_SIS302B
-					| VB_SIS301LV | VB_SIS302LV
-					| VB_XGI301C))
-				tempbl = pVBInfo->XGI_TVDelayList2[index];
+			tempbl = XGI301TVDelay;
 
 			if (pVBInfo->VBInfo & SetCRT2ToDualEdge)
 				tempbl = tempbl >> 4;
-			if (pVBInfo->VBInfo & (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-				/* Get LCD Delay */
-				index = XGI_GetLCDCapPtr(pVBInfo);
-				tempbh = pVBInfo->LCDCapList[index].
-						LCD_DelayCompensation;
+			if (pVBInfo->VBInfo &
+			    (SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
+				tempbh = XGI301LCDDelay;
 
 				if (!(pVBInfo->VBInfo & XGI_SetCRT2ToLCDA))
 					tempbl = tempbh;
@@ -5789,7 +5666,8 @@ static void XGI_SetDelayComp(struct vb_device_info *pVBInfo)
 				tempah |= tempbl;
 			}
 
-			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) { /* Channel A */
+			if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
+				/* Channel A */
 				tempah &= 0x0F;
 				tempah |= tempbh;
 			}
@@ -5799,10 +5677,7 @@ static void XGI_SetDelayComp(struct vb_device_info *pVBInfo)
 		tempbl = 0;
 		tempbh = 0;
 		if (pVBInfo->VBInfo & SetCRT2ToLCD) {
-			/* / Get LCD Delay */
-			tempah = pVBInfo->LCDCapList[
-					XGI_GetLCDCapPtr(pVBInfo)].
-						LCD_DelayCompensation;
+			tempah = XGI301LCDDelay;
 			tempah &= 0x0f;
 			tempah = tempah << 4;
 			xgifb_reg_and_or(pVBInfo->Part1Port, 0x2D, 0x0f,
@@ -6127,60 +6002,49 @@ static void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 	tempah = 0x08;
 	tempbl = 0xf0;
 
-	if (pVBInfo->VBInfo & DisableCRT2Display) {
-		xgifb_reg_and_or(pVBInfo->Part1Port, 0x2e, tempbl, tempah);
-	} else {
-		tempah = 0x00;
-		tempbl = 0xff;
+	if (pVBInfo->VBInfo & DisableCRT2Display)
+		goto reg_and_or;
 
-		if (pVBInfo->VBInfo & (SetCRT2ToRAMDAC | SetCRT2ToTV
-				| SetCRT2ToLCD | XGI_SetCRT2ToLCDA)) {
-			if ((pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) &&
-			    (!(pVBInfo->VBInfo & SetSimuScanMode))) {
-				tempbl &= 0xf7;
-				tempah |= 0x01;
-				xgifb_reg_and_or(pVBInfo->Part1Port, 0x2e,
-						tempbl, tempah);
-			} else {
-				if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
-					tempbl &= 0xf7;
-					tempah |= 0x01;
-				}
+	tempah = 0x00;
+	tempbl = 0xff;
 
-				if (pVBInfo->VBInfo &
-				    (SetCRT2ToRAMDAC |
-				     SetCRT2ToTV |
-				     SetCRT2ToLCD)) {
-					tempbl &= 0xf8;
-					tempah = 0x01;
+	if (!(pVBInfo->VBInfo & (SetCRT2ToRAMDAC | SetCRT2ToTV |
+				 SetCRT2ToLCD | XGI_SetCRT2ToLCDA)))
+		goto reg_and_or;
 
-					if (!(pVBInfo->VBInfo & SetInSlaveMode))
-						tempah |= 0x02;
+	if ((pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) &&
+	    (!(pVBInfo->VBInfo & SetSimuScanMode))) {
+		tempbl &= 0xf7;
+		tempah |= 0x01;
+		goto reg_and_or;
+	}
 
-					if (!(pVBInfo->VBInfo &
-					      SetCRT2ToRAMDAC)) {
-						tempah = tempah ^ 0x05;
-						if (!(pVBInfo->VBInfo &
-						      SetCRT2ToLCD))
-							tempah = tempah ^ 0x01;
-					}
+	if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
+		tempbl &= 0xf7;
+		tempah |= 0x01;
+	}
 
-					if (!(pVBInfo->VBInfo &
-					      SetCRT2ToDualEdge))
-						tempah |= 0x08;
-					xgifb_reg_and_or(pVBInfo->Part1Port,
-							0x2e, tempbl, tempah);
-				} else {
-					xgifb_reg_and_or(pVBInfo->Part1Port,
-							0x2e, tempbl, tempah);
-				}
-			}
-		} else {
-			xgifb_reg_and_or(pVBInfo->Part1Port, 0x2e, tempbl,
-					tempah);
-		}
+	if (!(pVBInfo->VBInfo & (SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD)))
+		goto reg_and_or;
+
+	tempbl &= 0xf8;
+	tempah = 0x01;
+
+	if (!(pVBInfo->VBInfo & SetInSlaveMode))
+		tempah |= 0x02;
+
+	if (!(pVBInfo->VBInfo & SetCRT2ToRAMDAC)) {
+		tempah = tempah ^ 0x05;
+		if (!(pVBInfo->VBInfo & SetCRT2ToLCD))
+			tempah = tempah ^ 0x01;
 	}
 
+	if (!(pVBInfo->VBInfo & SetCRT2ToDualEdge))
+		tempah |= 0x08;
+
+reg_and_or:
+	xgifb_reg_and_or(pVBInfo->Part1Port, 0x2e, tempbl, tempah);
+
 	if (pVBInfo->VBInfo & (SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD
 			| XGI_SetCRT2ToLCDA)) {
 		tempah &= (~0x08);
@@ -6265,17 +6129,6 @@ static void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 	}
 }
 
-static void XGI_CloseCRTC(struct xgi_hw_device_info *HwDeviceExtension,
-		struct vb_device_info *pVBInfo)
-{
-	unsigned short tempbx;
-
-	tempbx = 0;
-
-	if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA)
-		tempbx = 0x08A0;
-
-}
 
 void XGI_UnLockCRT2(struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
@@ -6358,8 +6211,7 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 		    (pVBInfo->RefIndex[RefreshRateTableIndex].YRes == 600)) {
 			index++;
 		}
-		/* Alan 10/19/2007;
-		 * do the similar adjustment like XGISearchCRT1Rate() */
+		/* do the similar adjustment like XGISearchCRT1Rate() */
 		if ((pVBInfo->RefIndex[RefreshRateTableIndex].XRes == 1024) &&
 		    (pVBInfo->RefIndex[RefreshRateTableIndex].YRes == 768)) {
 			index++;
@@ -6467,7 +6319,7 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 	int i;
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
 
-	/* [2004/05/06] Vicent to fix XG42 single LCD sense to CRT+LCD */
+	/* to fix XG42 single LCD sense to CRT+LCD */
 	xgifb_reg_set(pVBInfo->P3d4, 0x57, 0x4A);
 	xgifb_reg_set(pVBInfo->P3d4, 0x53, (unsigned char) (xgifb_reg_get(
 			pVBInfo->P3d4, 0x53) | 0x02));
@@ -6532,7 +6384,7 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 	else
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x32, 0xDF, 0x00);
 
-	/* alan, avoid display something, set BLACK DAC if not restore DAC */
+	/* avoid display something, set BLACK DAC if not restore DAC */
 	outb(0x00, pVBInfo->P3c8);
 
 	for (i = 0; i < 256; i++) {
@@ -6545,7 +6397,6 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 	xgifb_reg_set(pVBInfo->P3d4, 0x63, CR63);
 	xgifb_reg_set(pVBInfo->P3c4, 0x31, SR31);
 
-	/* [2004/05/11] Vicent */
 	xgifb_reg_set(pVBInfo->P3d4, 0x53, (unsigned char) (xgifb_reg_get(
 			pVBInfo->P3d4, 0x53) & 0xFD));
 	xgifb_reg_set(pVBInfo->P3c4, 0x1F, (unsigned char) SR1F);
@@ -6622,32 +6473,24 @@ static void XGI_EnableBridge(struct xgifb_video_info *xgifb_info,
 		if (!(pVBInfo->VBInfo & DisableCRT2Display)) {
 			tempah = 0xc0;
 
-			if (!(pVBInfo->VBInfo & SetSimuScanMode)) {
-				if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) {
-					if (pVBInfo->VBInfo &
-					    SetCRT2ToDualEdge) {
-						tempah = tempah & 0x40;
-						if (pVBInfo->VBInfo &
-						    XGI_SetCRT2ToLCDA)
-							tempah = tempah ^ 0xC0;
-
-						if (pVBInfo->SetFlag &
-						    DisableChB)
-							tempah &= 0xBF;
-
-						if (pVBInfo->SetFlag &
-						    DisableChA)
-							tempah &= 0x7F;
-
-						if (pVBInfo->SetFlag &
-						    EnableChB)
-							tempah |= 0x40;
-
-						if (pVBInfo->SetFlag &
-						    EnableChA)
-							tempah |= 0x80;
-					}
-				}
+			if (!(pVBInfo->VBInfo & SetSimuScanMode) &&
+			    (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA) &&
+			    (pVBInfo->VBInfo & SetCRT2ToDualEdge)) {
+				tempah = tempah & 0x40;
+				if (pVBInfo->VBInfo & XGI_SetCRT2ToLCDA)
+					tempah = tempah ^ 0xC0;
+
+				if (pVBInfo->SetFlag & DisableChB)
+					tempah &= 0xBF;
+
+				if (pVBInfo->SetFlag &  DisableChA)
+					tempah &= 0x7F;
+
+				if (pVBInfo->SetFlag &  EnableChB)
+					tempah |= 0x40;
+
+				if (pVBInfo->SetFlag &  EnableChA)
+					tempah |= 0x80;
 			}
 		}
 
@@ -6766,7 +6609,7 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 	pVBInfo->IF_DEF_LVDS = 0;
 	pVBInfo->IF_DEF_LCDA = 1;
 
-	if (HwDeviceExtension->jChipType >= XG20) { /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType >= XG20) {
 		pVBInfo->IF_DEF_YPbPr = 0;
 		pVBInfo->IF_DEF_HiVision = 0;
 		pVBInfo->IF_DEF_CRT2Monitor = 0;
@@ -6808,7 +6651,7 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 		}
 	}
 
-	if (HwDeviceExtension->jChipType < XG20) /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20)
 		XGI_GetVBType(pVBInfo);
 
 	InitTo330Pointer(HwDeviceExtension->jChipType, pVBInfo);
@@ -6816,12 +6659,12 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 		ModeNo = ModeNo & 0x7F;
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
 
-	if (HwDeviceExtension->jChipType < XG20) /* kuku 2004/06/25 1.Openkey */
+	if (HwDeviceExtension->jChipType < XG20)
 		XGI_UnLockCRT2(HwDeviceExtension, pVBInfo);
 
 	XGI_SearchModeID(ModeNo, &ModeIdIndex, pVBInfo);
 
-	if (HwDeviceExtension->jChipType < XG20) { /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20) {
 		XGI_GetVBInfo(ModeNo, ModeIdIndex, HwDeviceExtension, pVBInfo);
 		XGI_GetTVInfo(ModeNo, ModeIdIndex, pVBInfo);
 		XGI_GetLCDInfo(ModeNo, ModeIdIndex, pVBInfo);
@@ -6867,7 +6710,6 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 
 		XGI_SetCRT2ModeRegs(ModeNo, HwDeviceExtension, pVBInfo);
 		XGI_OEM310Setting(ModeNo, ModeIdIndex, pVBInfo); /*0212*/
-		XGI_CloseCRTC(HwDeviceExtension, pVBInfo);
 		XGI_EnableBridge(xgifb_info, HwDeviceExtension, pVBInfo);
 	} /* !XG20 */
 	else {
@@ -6893,9 +6735,8 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 
 	XGI_UpdateModeInfo(HwDeviceExtension, pVBInfo);
 
-	if (HwDeviceExtension->jChipType < XG20) { /* kuku 2004/06/25 */
+	if (HwDeviceExtension->jChipType < XG20)
 		XGI_LockCRT2(HwDeviceExtension, pVBInfo);
-	}
 
 	return 1;
 }
diff --git a/drivers/staging/xgifb/vb_struct.h b/drivers/staging/xgifb/vb_struct.h
index 38f47ffc69c..22c8eb9810d 100644
--- a/drivers/staging/xgifb/vb_struct.h
+++ b/drivers/staging/xgifb/vb_struct.h
@@ -28,7 +28,6 @@ struct XGI_Ext2Struct {
 	unsigned char  ModeID;
 	unsigned short XRes;
 	unsigned short YRes;
-	/* unsigned short ROM_OFFSET; */
 };
 
 struct XGI_ECLKDataStruct {
@@ -51,13 +50,6 @@ struct XGI_LCDDataTablStruct {
 	unsigned short DATAPTR;
 };
 
-struct XGI330_LVDSDataStruct {
-	unsigned short VGAHT;
-	unsigned short VGAVT;
-	unsigned short LCDHT;
-	unsigned short LCDVT;
-};
-
 struct XGI330_LCDDataDesStruct2 {
 	unsigned short LCDHDES;
 	unsigned short LCDHRS;
@@ -67,15 +59,6 @@ struct XGI330_LCDDataDesStruct2 {
 	unsigned short LCDVSync;
 };
 
-struct XGI330_LCDDataStruct {
-	unsigned short RVBHCMAX;
-	unsigned short RVBHCFACT;
-	unsigned short VGAHT;
-	unsigned short VGAVT;
-	unsigned short LCDHT;
-	unsigned short LCDVT;
-};
-
 
 struct XGI330_TVDataStruct {
 	unsigned short RVBHCMAX;
@@ -103,13 +86,6 @@ struct XGI330_TVDataTablStruct {
 };
 
 
-struct XGI330_CHTVDataStruct {
-	unsigned short VGAHT;
-	unsigned short VGAVT;
-	unsigned short LCDHT;
-	unsigned short LCDVT;
-};
-
 struct XGI_TimingHStruct {
 	unsigned char data[8];
 };
@@ -126,7 +102,6 @@ struct XGI330_LCDCapStruct {
 	unsigned char	LCD_ID;
 	unsigned short	LCD_Capability;
 	unsigned char	LCD_SetFlag;
-	unsigned char	LCD_DelayCompensation;
 	unsigned char	LCD_HSyncWidth;
 	unsigned char	LCD_VSyncWidth;
 	unsigned char	LCD_VCLK;
@@ -174,11 +149,6 @@ struct XGI_CRT1TableStruct {
 };
 
 
-struct XGI330_VCLKDataStruct {
-	unsigned char SR2B, SR2C;
-	unsigned short CLOCK;
-};
-
 struct XGI301C_Tap4TimingStruct {
 	unsigned short DE;
 	unsigned char  Reg[64];   /* C0-FF */
@@ -196,7 +166,6 @@ struct vb_device_info {
 	unsigned short   LCDHRS, LCDVRS, LCDHDES, LCDVDES;
 
 	unsigned short   ModeType;
-	/* ,IF_DEF_FSTN; add for dstn */
 	unsigned short   IF_DEF_LVDS, IF_DEF_TRUMPION, IF_DEF_DSTN;
 	unsigned short   IF_DEF_CRT2Monitor;
 	unsigned short   IF_DEF_LCDA, IF_DEF_YPbPr;
@@ -220,50 +189,14 @@ struct vb_device_info {
 	unsigned char (*SR15)[8];
 	unsigned char (*CR40)[8];
 
-	unsigned char  *pSoftSetting;
-	unsigned char  *pOutputSelect;
-
-	unsigned short *pRGBSenseData;
-	unsigned short *pRGBSenseData2; /*301b*/
-	unsigned short *pVideoSenseData;
-	unsigned short *pVideoSenseData2;
-	unsigned short *pYCSenseData;
-	unsigned short *pYCSenseData2;
-
-	unsigned char  *pSR07;
-	unsigned char  *CR49;
-	unsigned char  *pSR1F;
 	unsigned char  *AGPReg;
 	unsigned char  *SR16;
-	unsigned char  *pSR21;
-	unsigned char  *pSR22;
-	unsigned char  *pSR23;
-	unsigned char  *pSR24;
-	unsigned char  *SR25;
-	unsigned char  *pSR31;
-	unsigned char  *pSR32;
-	unsigned char  *pSR33;
-	unsigned char  *pSR36;      /* alan 12/07/2006 */
-	unsigned char  *pCRCF;
-	unsigned char  *pCRD0;      /* alan 12/07/2006 */
-	unsigned char  *pCRDE;      /* alan 12/07/2006 */
-	unsigned char  *pCR8F;      /* alan 12/07/2006 */
-	unsigned char  *pSR40;      /* alan 12/07/2006 */
-	unsigned char  *pSR41;      /* alan 12/07/2006 */
-	unsigned char  *pDVOSetting;
-	unsigned char  *pCR2E;
-	unsigned char  *pCR2F;
-	unsigned char  *pCR46;
-	unsigned char  *pCR47;
-	unsigned char  *pCRT2Data_1_2;
-	unsigned char  *pCRT2Data_4_D;
-	unsigned char  *pCRT2Data_4_E;
-	unsigned char  *pCRT2Data_4_10;
+	unsigned char  SR21;
+	unsigned char  SR22;
+	unsigned char  SR25;
 	struct SiS_MCLKData  *MCLKData;
 	struct XGI_ECLKDataStruct  *ECLKData;
 
-	unsigned char   *XGI_TVDelayList;
-	unsigned char   *XGI_TVDelayList2;
 	unsigned char   *NTSCTiming;
 	unsigned char   *PALTiming;
 	unsigned char   *HiTVExtTiming;
@@ -280,8 +213,7 @@ struct vb_device_info {
 	unsigned char   *Ren750pGroup3;
 	unsigned char   *ScreenOffset;
 	unsigned char   *pXGINew_DRAMTypeDefinition;
-	unsigned char   *pXGINew_I2CDefinition ;
-	unsigned char   *pXGINew_CR97 ;
+	unsigned char   XGINew_CR97;
 
 	struct XGI330_LCDCapStruct  *LCDCapList;
 
@@ -291,7 +223,6 @@ struct vb_device_info {
 	struct SiS_StandTable_S  *StandTable;
 	struct XGI_ExtStruct         *EModeIDTable;
 	struct XGI_Ext2Struct        *RefIndex;
-	/* XGINew_CRT1TableStruct *CRT1Table; */
 	struct XGI_CRT1TableStruct    *XGINEWUB_CRT1Table;
 	struct SiS_VCLKData    *VCLKData;
 	struct SiS_VBVCLKData  *VBVCLKData;
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
index d22e599cb30..1c168461411 100644
--- a/drivers/staging/xgifb/vb_table.h
+++ b/drivers/staging/xgifb/vb_table.h
@@ -1,4 +1,5 @@
-/* yilin modify for xgi20 */
+#ifndef _VB_TABLE_
+#define _VB_TABLE_
 static struct SiS_MCLKData XGI340New_MCLKData[] = {
 	{0x16, 0x01, 0x01, 166},
 	{0x19, 0x02, 0x01, 124},
@@ -21,7 +22,6 @@ static struct SiS_MCLKData XGI27New_MCLKData[] = {
 	{0x5c, 0x23, 0x01, 166}
 };
 
-/* yilin modify for xgi20 */
 static struct XGI_ECLKDataStruct XGI340_ECLKData[] = {
 	{0x5c, 0x23, 0x01, 166},
 	{0x55, 0x84, 0x01, 123},
@@ -128,13 +128,6 @@ static unsigned char XGI340_AGPReg[12] = {
 
 static unsigned char XGI340_SR16[4] = {0x03, 0x83, 0x03, 0x83};
 
-static unsigned char XGI330_sr25[2];
-static unsigned char XGI330_sr31 = 0xc0;
-static unsigned char XGI330_sr32 = 0x11;
-static unsigned char XGI330_SR33;
-static unsigned char XG40_CRCF = 0x13;
-static unsigned char XG40_DRAMTypeDefinition = 0xFF ;
-
 static struct XGI_ExtStruct XGI330_EModeIDTable[] = {
 	{0x2e, 0x0a1b, 0x0306, 0x06, 0x05, 0x06},
 	{0x2f, 0x0a1b, 0x0305, 0x05, 0x05, 0x05},
@@ -418,7 +411,7 @@ static unsigned char XGI_CH7017LV1400x1050[] = {
 	0xAD, 0xDB, 0xF6, 0xAC, 0xE0, 0x02};
 
 /*add for new UNIVGABIOS*/
-static struct XGI330_LCDDataStruct  XGI_StLCD1024x768Data[] = {
+static struct SiS_LCDData  XGI_StLCD1024x768Data[] = {
 	{62,  25, 800,  546, 1344, 806},
 	{32,  15, 930,  546, 1344, 806},
 	{62,  25, 800,  546, 1344, 806}, /*chiawenfordot9->dot8*/
@@ -428,14 +421,10 @@ static struct XGI330_LCDDataStruct  XGI_StLCD1024x768Data[] = {
 	{1,   1,  1344, 806, 1344, 806}
 };
 
-static struct XGI330_LCDDataStruct  XGI_ExtLCD1024x768Data[] = {
-	/* { 12, 5, 896, 512,1344, 806}, // alan 09/12/2003 */
+static struct SiS_LCDData  XGI_ExtLCD1024x768Data[] = {
 	{42, 25, 1536, 419, 1344, 806},
-	/* { 12, 5, 896, 510,1344, 806}, // alan 09/12/2003 */
 	{48, 25, 1536, 369, 1344, 806},
-	/* { 32, 15,1008, 505,1344, 806}, // alan 09/12/2003 */
 	{42, 25, 1536, 419, 1344, 806},
-	/* { 32, 15,1008, 514,1344, 806}, // alan 09/12/2003 */
 	{48, 25, 1536, 369, 1344, 806},
 	{12, 5,  896,  500, 1344, 806},
 	{42, 25, 1024, 625, 1344, 806},
@@ -448,7 +437,7 @@ static struct XGI330_LCDDataStruct  XGI_ExtLCD1024x768Data[] = {
 	{1,  1,  1344, 806, 1344, 806}
 };
 
-static struct XGI330_LCDDataStruct  XGI_CetLCD1024x768Data[] = {
+static struct SiS_LCDData  XGI_CetLCD1024x768Data[] = {
 	{1, 1, 1344, 806, 1344, 806}, /* ; 00 (320x200,320x400,
 					       640x200,640x400) */
 	{1, 1, 1344, 806, 1344, 806}, /* 01 (320x350,640x350) */
@@ -459,7 +448,7 @@ static struct XGI330_LCDDataStruct  XGI_CetLCD1024x768Data[] = {
 	{1, 1, 1344, 806, 1344, 806}  /* 06 (1024x768x60Hz) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_StLCD1280x1024Data[] = {
+static struct SiS_LCDData  XGI_StLCD1280x1024Data[] = {
 	{22,  5,  800,  510,  1650, 1088},
 	{22,  5,  800,  510,  1650, 1088},
 	{176, 45, 900,  510,  1650, 1088},
@@ -470,7 +459,7 @@ static struct XGI330_LCDDataStruct  XGI_StLCD1280x1024Data[] = {
 	{1,   1,  1688, 1066, 1688, 1066}
 };
 
-static struct XGI330_LCDDataStruct  XGI_ExtLCD1280x1024Data[] = {
+static struct SiS_LCDData  XGI_ExtLCD1280x1024Data[] = {
 	{211, 60,  1024, 501,  1688, 1066},
 	{211, 60,  1024, 508,  1688, 1066},
 	{211, 60,  1024, 501,  1688, 1066},
@@ -481,7 +470,7 @@ static struct XGI330_LCDDataStruct  XGI_ExtLCD1280x1024Data[] = {
 	{1,   1,   1688, 1066, 1688, 1066}
 };
 
-static struct XGI330_LCDDataStruct  XGI_CetLCD1280x1024Data[] = {
+static struct SiS_LCDData  XGI_CetLCD1280x1024Data[] = {
 	{1, 1, 1688, 1066, 1688, 1066}, /* 00 (320x200,320x400,
 					       640x200,640x400) */
 	{1, 1, 1688, 1066, 1688, 1066}, /* 01 (320x350,640x350) */
@@ -494,7 +483,7 @@ static struct XGI330_LCDDataStruct  XGI_CetLCD1280x1024Data[] = {
 	{1, 1, 1688, 1066, 1688, 1066}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LCDDataStruct xgifb_lcd_1400x1050[] = {
+static struct SiS_LCDData xgifb_lcd_1400x1050[] = {
 	{211, 100, 2100, 408,  1688, 1066}, /* 00 (320x200,320x400,
 						   640x200,640x400) */
 	{211, 64,  1536, 358,  1688, 1066}, /* 01 (320x350,640x350) */
@@ -508,15 +497,11 @@ static struct XGI330_LCDDataStruct xgifb_lcd_1400x1050[] = {
 	{1,   1,   1688, 1066, 1688, 1066}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_ExtLCD1600x1200Data[] = {
-	{4,  1,  1620, 420,  2160, 1250}, /* { 3,1,2160,425,2160,1250 },
-					  // 00 (320x200,320x400,
-					  //	 640x200,640x400)
-					  //	 // alan 10/14/2003 */
+static struct SiS_LCDData  XGI_ExtLCD1600x1200Data[] = {
+	{4,  1,  1620, 420,  2160, 1250}, /* 00 (320x200,320x400,
+						 640x200,640x400)*/
 	{27, 7,  1920, 375,  2160, 1250}, /* 01 (320x350,640x350) */
-	{4,  1,  1620, 420,  2160, 1250}, /* { 3,1,2160,425,2160,1250 },
-					  // 02 (360x400,720x400)
-					  // // alan 10/14/2003 */
+	{4,  1,  1620, 420,  2160, 1250}, /* 02 (360x400,720x400)*/
 	{27, 7,  1920, 375,  2160, 1250}, /* 03 (720x350) */
 	{27, 4,  800,  500,  2160, 1250}, /* 04 (640x480x60Hz) */
 	{4,  1,  1080, 625,  2160, 1250}, /* 05 (800x600x60Hz) */
@@ -526,7 +511,7 @@ static struct XGI330_LCDDataStruct  XGI_ExtLCD1600x1200Data[] = {
 	{1,  1,  2160, 1250, 2160, 1250}  /* 09 (1600x1200x60Hz) ;302lv */
 };
 
-static struct XGI330_LCDDataStruct  XGI_StLCD1600x1200Data[] = {
+static struct SiS_LCDData  XGI_StLCD1600x1200Data[] = {
 	{27,  4,  800,  500,  2160, 1250}, /* 00 (320x200,320x400,
 						  640x200,640x400) */
 	{27,  4,  800,  500,  2160, 1250}, /* 01 (320x350,640x350) */
@@ -540,7 +525,7 @@ static struct XGI330_LCDDataStruct  XGI_StLCD1600x1200Data[] = {
 	{1,   1,  2160, 1250, 2160, 1250}  /* 09 (1600x1200) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_CetLCD1400x1050Data[] = {
+static struct SiS_LCDData  XGI_CetLCD1400x1050Data[] = {
 	{1, 1, 1688, 1066, 1688, 1066}, /* 00 (320x200,320x400,
 					       640x200,640x400) */
 	{1, 1, 1688, 1066, 1688, 1066}, /* 01 (320x350,640x350) */
@@ -553,7 +538,7 @@ static struct XGI330_LCDDataStruct  XGI_CetLCD1400x1050Data[] = {
 	{1, 1, 1688, 1066, 1688, 1066}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_NoScalingData[] = {
+static struct SiS_LCDData  XGI_NoScalingData[] = {
 	{1, 1, 800,  449,  800,  449},
 	{1, 1, 800,  449,  800,  449},
 	{1, 1, 900,  449,  900,  449},
@@ -564,7 +549,7 @@ static struct XGI330_LCDDataStruct  XGI_NoScalingData[] = {
 	{1, 1, 1688, 1066, 1688, 1066}
 };
 
-static struct XGI330_LCDDataStruct  XGI_ExtLCD1024x768x75Data[] = {
+static struct SiS_LCDData  XGI_ExtLCD1024x768x75Data[] = {
 	{42, 25, 1536, 419, 1344, 806}, /* ; 00 (320x200,320x400,
 						 640x200,640x400) */
 	{48, 25, 1536, 369, 1344, 806}, /* ; 01 (320x350,640x350) */
@@ -575,7 +560,7 @@ static struct XGI330_LCDDataStruct  XGI_ExtLCD1024x768x75Data[] = {
 	{1,  1,  1312, 800, 1312, 800}  /* ; 06 (1024x768x75Hz) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_CetLCD1024x768x75Data[] = {
+static struct SiS_LCDData  XGI_CetLCD1024x768x75Data[] = {
 	{1, 1, 1312, 800, 1312, 800}, /* ; 00 (320x200,320x400,
 					       640x200,640x400) */
 	{1, 1, 1312, 800, 1312, 800}, /* ; 01 (320x350,640x350) */
@@ -586,7 +571,7 @@ static struct XGI330_LCDDataStruct  XGI_CetLCD1024x768x75Data[] = {
 	{1, 1, 1312, 800, 1312, 800}  /* ; 06 (1024x768x75Hz) */
 };
 
-static struct XGI330_LCDDataStruct xgifb_lcd_1280x1024x75[] = {
+static struct SiS_LCDData xgifb_lcd_1280x1024x75[] = {
 	{211, 60,  1024, 501,  1688, 1066}, /* ; 00 (320x200,320x400,
 						     640x200,640x400) */
 	{211, 60,  1024, 508,  1688, 1066}, /* ; 01 (320x350,640x350) */
@@ -598,7 +583,7 @@ static struct XGI330_LCDDataStruct xgifb_lcd_1280x1024x75[] = {
 	{1,   1,   1688, 1066, 1688, 1066}  /* ; 07 (1280x1024x75Hz) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_CetLCD1280x1024x75Data[] = {
+static struct SiS_LCDData  XGI_CetLCD1280x1024x75Data[] = {
 	{1, 1, 1688, 1066, 1688, 1066}, /* ; 00 (320x200,320x400,
 						 640x200,640x400) */
 	{1, 1, 1688, 1066, 1688, 1066}, /* ; 01 (320x350,640x350) */
@@ -610,7 +595,7 @@ static struct XGI330_LCDDataStruct  XGI_CetLCD1280x1024x75Data[] = {
 	{1, 1, 1688, 1066, 1688, 1066}  /* ; 07 (1280x1024x75Hz) */
 };
 
-static struct XGI330_LCDDataStruct  XGI_NoScalingDatax75[] = {
+static struct SiS_LCDData  XGI_NoScalingDatax75[] = {
 	{1, 1, 800,  449,  800,  449},  /* ; 00 (320x200, 320x400,
 						 640x200, 640x400) */
 	{1, 1, 800,  449,  800,  449},  /* ; 01 (320x350, 640x350) */
@@ -620,8 +605,7 @@ static struct XGI330_LCDDataStruct  XGI_NoScalingDatax75[] = {
 	{1, 1, 1056, 625,  1056, 625},  /* ; 05 (800x600x75Hz) */
 	{1, 1, 1312, 800,  1312, 800},  /* ; 06 (1024x768x75Hz) */
 	{1, 1, 1688, 1066, 1688, 1066}, /* ; 07 (1280x1024x75Hz) */
-	{1, 1, 1688, 1066, 1688, 1066}, /* ; 08 (1400x1050x75Hz)
-					   ;;[ycchen] 12/19/02 */
+	{1, 1, 1688, 1066, 1688, 1066}, /* ; 08 (1400x1050x75Hz)*/
 	{1, 1, 2160, 1250, 2160, 1250}, /* ; 09 (1600x1200x75Hz) */
 	{1, 1, 1688, 806,  1688, 806}   /* ; 0A (1280x768x75Hz) */
 };
@@ -828,8 +812,7 @@ static struct XGI330_LCDDataDesStruct2  XGI_NoScalingDesData[] = {
 	{9, 849,  627, 600,  128,  4}, /* 05 (800x600x60Hz) */
 	{9, 1057, 805, 770,  0136, 6}, /* 06 (1024x768x60Hz) */
 	{9, 1337, 0,   1025, 112,  3}, /* 07 (1280x1024x60Hz) */
-	{9, 1457, 0,   1051, 112,  3}, /* 08 (1400x1050x60Hz) },
-					//;[ycchen] 12/19/02 */
+	{9, 1457, 0,   1051, 112,  3}, /* 08 (1400x1050x60Hz)*/
 	{9, 1673, 0,   1201, 192,  3}, /* 09 (1600x1200x60Hz) */
 	{9, 1337, 0,   771,  112,  6}  /* 0A (1280x768x60Hz) */
 };
@@ -915,8 +898,7 @@ static struct XGI330_LCDDataDesStruct2 XGI_NoScalingDesDatax75[] =  {
 	{9, 825,  0,   601,  80,  3}, /* ; 05 (800x600x75Hz) */
 	{9, 1049, 0,   769,  96,  3}, /* ; 06 (1024x768x75Hz) */
 	{9, 1305, 0,   1025, 144, 3}, /* ; 07 (1280x1024x75Hz) */
-	{9, 1457, 0,   1051, 112, 3}, /* ; 08 (1400x1050x60Hz)
-					 ;;[ycchen] 12/19/02 */
+	{9, 1457, 0,   1051, 112, 3}, /* ; 08 (1400x1050x60Hz)*/
 	{9, 1673, 0,   1201, 192, 3}, /* ; 09 (1600x1200x75Hz) */
 	{9, 1337, 0,   771,  112, 6}  /* ; 0A (1280x768x60Hz) */
 };
@@ -1220,7 +1202,7 @@ static unsigned char XGI330_Ren750pGroup3[] = {
 	0x18, 0x1D, 0x23, 0x28, 0x4C, 0xAA, 0x01
 };
 
-static struct XGI330_LVDSDataStruct  XGI_LVDS1024x768Data_1[] = {
+static struct SiS_LVDSData  XGI_LVDS1024x768Data_1[] = {
 	{ 960, 438, 1344, 806},	/* 00 (320x200,320x400,640x200,640x400) */
 	{ 960, 388, 1344, 806},	/* 01 (320x350,640x350) */
 	{1040, 438, 1344, 806},	/* 02 (360x400,720x400) */
@@ -1231,7 +1213,7 @@ static struct XGI330_LVDSDataStruct  XGI_LVDS1024x768Data_1[] = {
 };
 
 
-static struct XGI330_LVDSDataStruct  XGI_LVDS1024x768Data_2[] = {
+static struct SiS_LVDSData  XGI_LVDS1024x768Data_2[] = {
 	{1344, 806, 1344, 806},
 	{1344, 806, 1344, 806},
 	{1344, 806, 1344, 806},
@@ -1243,7 +1225,7 @@ static struct XGI330_LVDSDataStruct  XGI_LVDS1024x768Data_2[] = {
 	{800,  525, 1280, 813}
 };
 
-static struct XGI330_LVDSDataStruct  XGI_LVDS1280x1024Data_1[] = {
+static struct SiS_LVDSData  XGI_LVDS1280x1024Data_1[] = {
 	{1048, 442,  1688, 1066},
 	{1048, 392,  1688, 1066},
 	{1048, 442,  1688, 1066},
@@ -1254,7 +1236,7 @@ static struct XGI330_LVDSDataStruct  XGI_LVDS1280x1024Data_1[] = {
 	{1688, 1066, 1688, 1066}
 };
 
-static struct XGI330_LVDSDataStruct  XGI_LVDS1280x1024Data_2[] = {
+static struct SiS_LVDSData  XGI_LVDS1280x1024Data_2[] = {
 	{1344, 806, 1344, 806},
 	{1344, 806, 1344, 806},
 	{1344, 806, 1344, 806},
@@ -1266,7 +1248,7 @@ static struct XGI330_LVDSDataStruct  XGI_LVDS1280x1024Data_2[] = {
 	{800,  525, 1280, 813}
 };
 
-static struct XGI330_LVDSDataStruct  XGI_LVDS1400x1050Data_1[] = {
+static struct SiS_LVDSData  XGI_LVDS1400x1050Data_1[] = {
 	{928,   416, 1688, 1066},
 	{928,   366, 1688, 1066},
 	{928,   416, 1688, 1066},
@@ -1278,7 +1260,7 @@ static struct XGI330_LVDSDataStruct  XGI_LVDS1400x1050Data_1[] = {
 	{1688, 1066, 1688, 1066}
 };
 
-static struct XGI330_LVDSDataStruct  XGI_LVDS1400x1050Data_2[] = {
+static struct SiS_LVDSData  XGI_LVDS1400x1050Data_2[] = {
 	{1688, 1066, 1688, 1066},
 	{1688, 1066, 1688, 1066},
 	{1688, 1066, 1688, 1066},
@@ -1291,7 +1273,7 @@ static struct XGI330_LVDSDataStruct  XGI_LVDS1400x1050Data_2[] = {
 };
 
 /* ;;[ycchen] 12/05/02 LCDHTxLCDVT=2048x1320 */
-static struct XGI330_LVDSDataStruct XGI_LVDS1600x1200Data_1[] = {
+static struct SiS_LVDSData XGI_LVDS1600x1200Data_1[] = {
 	{1088, 520,  2048, 1320}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1088, 470,  2048, 1320}, /* 01 (320x350,640x350) */
 	{1088, 520,  2048, 1320}, /* 02 (360x400,720x400) */
@@ -1304,7 +1286,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1600x1200Data_1[] = {
 	{2048, 1320, 2048, 1320}  /* 09 (1600x1200) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDSNoScalingData[] = {
+static struct SiS_LVDSData XGI_LVDSNoScalingData[] = {
 	{ 800,  449,  800,  449}, /* 00 (320x200,320x400,640x200,640x400) */
 	{ 800,  449,  800,  449}, /* 01 (320x350,640x350) */
 	{ 800,  449,  800,  449}, /* 02 (360x400,720x400) */
@@ -1313,12 +1295,12 @@ static struct XGI330_LVDSDataStruct XGI_LVDSNoScalingData[] = {
 	{1056,  628, 1056,  628}, /* 05 (800x600x60Hz) */
 	{1344,  806, 1344,  806}, /* 06 (1024x768x60Hz) */
 	{1688, 1066, 1688, 1066}, /* 07 (1280x1024x60Hz) */
-	{1688, 1066, 1688, 1066}, /* 08 (1400x1050x60Hz) ;;[ycchen] 12/19/02 */
+	{1688, 1066, 1688, 1066}, /* 08 (1400x1050x60Hz) */
 	{2160, 1250, 2160, 1250}, /* 09 (1600x1200x60Hz) */
 	{1688,  806, 1688,  806}  /* 0A (1280x768x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Data_1x75[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Data_1x75[] = {
 	{ 960, 438, 1312, 800}, /* 00 (320x200,320x400,640x200,640x400) */
 	{ 960, 388, 1312, 800}, /* 01 (320x350,640x350) */
 	{1040, 438, 1312, 800}, /* 02 (360x400,720x400) */
@@ -1329,7 +1311,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Data_1x75[] = {
 };
 
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Data_2x75[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Data_2x75[] = {
 	{1312, 800, 1312, 800}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1312, 800, 1312, 800}, /* ; 01 (320x350,640x350) */
 	{1312, 800, 1312, 800}, /* ; 02 (360x400,720x400) */
@@ -1339,7 +1321,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Data_2x75[] = {
 	{1312, 800, 1312, 800}, /* ; 06 (512x384,1024x768) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Data_1x75[] = {
+static struct SiS_LVDSData XGI_LVDS1280x1024Data_1x75[] = {
 	{1048,  442, 1688, 1066  }, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1048,  392, 1688, 1066  }, /* ; 01 (320x350,640x350) */
 	{1128,  442, 1688, 1066  }, /* ; 02 (360x400,720x400) */
@@ -1350,7 +1332,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Data_1x75[] = {
 	{1688, 1066, 1688, 1066 },  /* ; 06; 07 (640x512,1280x1024) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Data_2x75[] = {
+static struct SiS_LVDSData XGI_LVDS1280x1024Data_2x75[] = {
 	{1688, 1066, 1688, 1066 }, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1688, 1066, 1688, 1066 }, /* ; 01 (320x350,640x350) */
 	{1688, 1066, 1688, 1066 }, /* ; 02 (360x400,720x400) */
@@ -1361,7 +1343,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Data_2x75[] = {
 	{1688, 1066, 1688, 1066 }, /* ; 06; 07 (640x512,1280x1024) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDSNoScalingDatax75[] = {
+static struct SiS_LVDSData XGI_LVDSNoScalingDatax75[] = {
 	{ 800,  449,  800, 449},  /* ; 00 (320x200,320x400,640x200,640x400) */
 	{ 800,  449,  800, 449},  /* ; 01 (320x350,640x350) */
 	{ 900,  449,  900, 449},  /* ; 02 (360x400,720x400) */
@@ -1376,7 +1358,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDSNoScalingDatax75[] = {
 	{1688,  806, 1688, 806},  /* ; 0A (1280x768x75Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_1[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Des_1[] = {
 	{0, 1048,   0, 771}, /* 00 (320x200,320x400,640x200,640x400) */
 	{0, 1048,   0, 771}, /* 01 (320x350,640x350) */
 	{0, 1048,   0, 771}, /* 02 (360x400,720x400) */
@@ -1386,7 +1368,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_1[] = {
 	{0, 1048, 805, 770}  /* 06 (1024x768x60Hz) */
 } ;
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_2[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Des_2[] = {
 	{1142,  856, 622, 587}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1142,  856, 597, 562}, /* 01 (320x350,640x350) */
 	{1142,  856, 622, 587}, /* 02 (360x400,720x400) */
@@ -1396,7 +1378,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_2[] = {
 	{   0, 1048, 805, 771}  /* 06 (1024x768x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_3[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Des_3[] = {
 	{320, 24, 622, 587}, /* 00 (320x200,320x400,640x200,640x400) */
 	{320, 24, 597, 562}, /* 01 (320x350,640x350) */
 	{320, 24, 622, 587}, /* 02 (360x400,720x400) */
@@ -1404,7 +1386,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_3[] = {
 	{320, 24, 722, 687}  /* 04 (640x480x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_1[] = {
+static struct SiS_LVDSData XGI_LVDS1280x1024Des_1[] = {
 	{0, 1328,    0, 1025}, /* 00 (320x200,320x400,640x200,640x400) */
 	{0, 1328,    0, 1025}, /* 01 (320x350,640x350) */
 	{0, 1328,    0, 1025}, /* 02 (360x400,720x400) */
@@ -1416,7 +1398,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_1[] = {
 };
 
  /* The Display setting for DE Mode Panel */
-static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_2[] = {
+static struct SiS_LVDSData XGI_LVDS1280x1024Des_2[] = {
 	{1368, 1008, 752, 711}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1368, 1008, 729, 688}, /* 01 (320x350,640x350) */
 	{1408, 1048, 752, 711}, /* 02 (360x400,720x400) */
@@ -1427,7 +1409,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_2[] = {
 	{0000, 1328,   0, 1025} /* 07 (1280x1024x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1400x1050Des_1[] = {
+static struct SiS_LVDSData XGI_LVDS1400x1050Des_1[] = {
 	{0, 1448, 0, 1051}, /* 00 (320x200,320x400,640x200,640x400) */
 	{0, 1448, 0, 1051}, /* 01 (320x350,640x350) */
 	{0, 1448, 0, 1051}, /* 02 (360x400,720x400) */
@@ -1439,7 +1421,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1400x1050Des_1[] = {
 	{0, 1448, 0, 1051}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1400x1050Des_2[] = {
+static struct SiS_LVDSData XGI_LVDS1400x1050Des_2[] = {
 	{1308, 1068,  781,  766}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1308, 1068,  781,  766}, /* 01 (320x350,640x350) */
 	{1308, 1068,  781,  766}, /* 02 (360x400,720x400) */
@@ -1451,7 +1433,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1400x1050Des_2[] = {
 	{   0, 1448,    0, 1051}  /* 08 (1400x1050x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1600x1200Des_1[] = {
+static struct SiS_LVDSData XGI_LVDS1600x1200Des_1[] = {
 	{0, 1664, 0, 1201}, /* 00 (320x200,320x400,640x200,640x400) */
 	{0, 1664, 0, 1201}, /* 01 (320x350,640x350) */
 	{0, 1664, 0, 1201}, /* 02 (360x400,720x400) */
@@ -1474,14 +1456,13 @@ static struct XGI330_LCDDataDesStruct2  XGI_LVDSNoScalingDesData[] = {
 	{0,  840,  627,  600, 128, 4}, /* 05 (800x600x60Hz) */
 	{0, 1048,  805,  770, 136, 6}, /* 06 (1024x768x60Hz) */
 	{0, 1328,    0, 1025, 112, 3}, /* 07 (1280x1024x60Hz) */
-	{0, 1438,    0, 1051, 112, 3}, /* 08 (1400x1050x60Hz)
-					;;[ycchen] 12/19/02 */
+	{0, 1438,    0, 1051, 112, 3}, /* 08 (1400x1050x60Hz)*/
 	{0, 1664,    0, 1201, 192, 3}, /* 09 (1600x1200x60Hz) */
 	{0, 1328,    0, 0771, 112, 6}  /* 0A (1280x768x60Hz) */
 };
 
 /* ; 1024x768 Full-screen */
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_1x75[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Des_1x75[] = {
 	{0, 1040, 0, 769}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{0, 1040, 0, 769}, /* ; 01 (320x350,640x350) */
 	{0, 1040, 0, 769}, /* ; 02 (360x400,720x400) */
@@ -1492,7 +1473,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_1x75[] = {
 };
 
 /* ; 1024x768 center-screen (Enh. Mode) */
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_2x75[] = {
+static struct SiS_LVDSData XGI_LVDS1024x768Des_2x75[] = {
 	{1142,  856, 622, 587}, /* 00 (320x200,320x400,640x200,640x400) */
 	{1142,  856, 597, 562}, /* 01 (320x350,640x350) */
 	{1142,  856, 622, 587}, /* 02 (360x400,720x400) */
@@ -1503,7 +1484,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_2x75[] = {
 };
 
 /* ; 1024x768 center-screen (St.Mode) */
-static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_3x75[] =  {
+static struct SiS_LVDSData XGI_LVDS1024x768Des_3x75[] =  {
 	{320, 24, 622, 587}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{320, 24, 597, 562}, /* ; 01 (320x350,640x350) */
 	{320, 24, 622, 587}, /* ; 02 (360x400,720x400) */
@@ -1511,7 +1492,7 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1024x768Des_3x75[] =  {
 	{320, 24, 722, 687}  /* ; 04 (640x480x60Hz) */
 };
 
-static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_1x75[] = {
+static struct SiS_LVDSData XGI_LVDS1280x1024Des_1x75[] = {
 	{0, 1296, 0, 1025}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{0, 1296, 0, 1025}, /* ; 01 (320x350,640x350) */
 	{0, 1296, 0, 1025}, /* ; 02 (360x400,720x400) */
@@ -1523,8 +1504,8 @@ static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_1x75[] = {
 };
 
 /* The Display setting for DE Mode Panel */
-/* [ycchen] 02/18/03 Set DE as default */
-static struct XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_2x75[] = {
+/* Set DE as default */
+static struct SiS_LVDSData XGI_LVDS1280x1024Des_2x75[] = {
 	{1368,  976, 752,  711}, /* ; 00 (320x200,320x400,640x200,640x400) */
 	{1368,  976, 729,  688}, /* ; 01 (320x350,640x350) */
 	{1408,  976, 752,  711}, /* ; 02 (360x400,720x400) */
@@ -1546,13 +1527,12 @@ static struct XGI330_LCDDataDesStruct2 XGI_LVDSNoScalingDesDatax75[] = {
 	{0,  816,   0,  601,  80, 3}, /* ; 05 (800x600x75Hz) */
 	{0, 1040,   0,  769,  96, 3}, /* ; 06 (1024x768x75Hz) */
 	{0, 1296,   0, 1025, 144, 3}, /* ; 07 (1280x1024x75Hz) */
-	{0, 1448,   0, 1051, 112, 3}, /* ; 08 (1400x1050x75Hz)
-					 ;;[ycchen] 12/19/02 */
+	{0, 1448,   0, 1051, 112, 3}, /* ; 08 (1400x1050x75Hz) */
 	{0, 1664,   0, 1201, 192, 3}, /* ; 09 (1600x1200x75Hz) */
 	{0, 1328,   0,  771, 112, 6}  /* ; 0A (1280x768x75Hz) */
 };
 
-static struct XGI330_CHTVDataStruct  XGI_CHTVUNTSCData[] = {
+static struct SiS_LVDSData  XGI_CHTVUNTSCData[] = {
 	{ 840, 600,  840, 600},
 	{ 840, 600,  840, 600},
 	{ 840, 600,  840, 600},
@@ -1561,7 +1541,7 @@ static struct XGI330_CHTVDataStruct  XGI_CHTVUNTSCData[] = {
 	{1064, 750, 1064, 750}
 };
 
-static struct XGI330_CHTVDataStruct  XGI_CHTVONTSCData[] = {
+static struct SiS_LVDSData  XGI_CHTVONTSCData[] = {
 	{ 840, 525,  840, 525},
 	{ 840, 525,  840, 525},
 	{ 840, 525,  840, 525},
@@ -1570,7 +1550,7 @@ static struct XGI330_CHTVDataStruct  XGI_CHTVONTSCData[] = {
 	{1040, 700, 1040, 700}
 };
 
-static struct XGI330_CHTVDataStruct  XGI_CHTVUPALData[] = {
+static struct SiS_LVDSData  XGI_CHTVUPALData[] = {
 	{1008, 625, 1008, 625},
 	{1008, 625, 1008, 625},
 	{1008, 625, 1008, 625},
@@ -1579,7 +1559,7 @@ static struct XGI330_CHTVDataStruct  XGI_CHTVUPALData[] = {
 	{ 936, 836,  936, 836}
 };
 
-static struct XGI330_CHTVDataStruct  XGI_CHTVOPALData[] = {
+static struct SiS_LVDSData  XGI_CHTVOPALData[] = {
 	{1008, 625, 1008, 625},
 	{1008, 625, 1008, 625},
 	{1008, 625, 1008, 625},
@@ -1858,9 +1838,12 @@ static struct XGI330_LCDDataTablStruct XGI_LCDDataTable[] = {
 	{Panel_1024x768x75, 0x0019, 0x0001, 12}, /* XGI_ExtLCD1024x768x75Data */
 	{Panel_1024x768x75, 0x0019, 0x0000, 13}, /* XGI_StLCD1024x768x75Data */
 	{Panel_1024x768x75, 0x0018, 0x0010, 14}, /* XGI_CetLCD1024x768x75Data */
-	{Panel_1280x1024x75, 0x0019, 0x0001, 15}, /* XGI_ExtLCD1280x1024x75Data*/
-	{Panel_1280x1024x75, 0x0019, 0x0000, 16}, /* XGI_StLCD1280x1024x75Data */
-	{Panel_1280x1024x75, 0x0018, 0x0010, 17}, /* XGI_CetLCD1280x1024x75Data*/
+	/* XGI_ExtLCD1280x1024x75Data */
+	{Panel_1280x1024x75, 0x0019, 0x0001, 15},
+	/* XGI_StLCD1280x1024x75Data */
+	{Panel_1280x1024x75, 0x0019, 0x0000, 16},
+	/* XGI_CetLCD1280x1024x75Data */
+	{Panel_1280x1024x75, 0x0018, 0x0010, 17},
 	{PanelRef75Hz, 0x0008, 0x0008, 18}, /* XGI_NoScalingDatax75 */
 	{0xFF, 0x0000, 0x0000, 0} /* End of table */
 };
@@ -1879,9 +1862,12 @@ static struct XGI330_LCDDataTablStruct XGI_LCDDesDataTable[] = {
 	{Panel_1600x1200, 0x0019, 0x0001, 10}, /* XGI_ExtLCDDes1600x1200Data */
 	{Panel_1600x1200, 0x0019, 0x0000, 11}, /* XGI_StLCDDes1600x1200Data */
 	{PanelRef60Hz, 0x0008, 0x0008, 12}, /* XGI_NoScalingDesData */
-	{Panel_1024x768x75, 0x0019, 0x0001, 13}, /*XGI_ExtLCDDes1024x768x75Data*/
-	{Panel_1024x768x75, 0x0019, 0x0000, 14}, /* XGI_StLCDDes1024x768x75Data*/
-	{Panel_1024x768x75, 0x0018, 0x0010, 15}, /*XGI_CetLCDDes1024x768x75Data*/
+	/* XGI_ExtLCDDes1024x768x75Data */
+	{Panel_1024x768x75, 0x0019, 0x0001, 13},
+	/* XGI_StLCDDes1024x768x75Data */
+	{Panel_1024x768x75, 0x0019, 0x0000, 14},
+	/* XGI_CetLCDDes1024x768x75Data */
+	{Panel_1024x768x75, 0x0018, 0x0010, 15},
 	/* XGI_ExtLCDDes1280x1024x75Data */
 	{Panel_1280x1024x75, 0x0019, 0x0001, 16},
 	/* XGI_StLCDDes1280x1024x75Data */
@@ -1918,8 +1904,10 @@ static struct XGI330_LCDDataTablStruct XGI_EPLLCDDataPtr[] = {
 	{PanelRef60Hz, 0x0008, 0x0008, 7}, /* XGI_LVDSNoScalingData */
 	{Panel_1024x768x75, 0x0018, 0x0000, 8}, /* XGI_LVDS1024x768Data_1x75 */
 	{Panel_1024x768x75, 0x0018, 0x0010, 9}, /* XGI_LVDS1024x768Data_2x75 */
-	{Panel_1280x1024x75, 0x0018, 0x0000, 10}, /* XGI_LVDS1280x1024Data_1x75*/
-	{Panel_1280x1024x75, 0x0018, 0x0010, 11},  /*XGI_LVDS1280x1024Data_2x75*/
+	/* XGI_LVDS1280x1024Data_1x75 */
+	{Panel_1280x1024x75, 0x0018, 0x0000, 10},
+	/* XGI_LVDS1280x1024Data_2x75 */
+	{Panel_1280x1024x75, 0x0018, 0x0010, 11},
 	{PanelRef75Hz, 0x0008, 0x0008, 12}, /* XGI_LVDSNoScalingDatax75 */
 	{0xFF, 0x0000, 0x0000, 0}
 };
@@ -1937,8 +1925,10 @@ static struct XGI330_LCDDataTablStruct XGI_EPLLCDDesDataPtr[] = {
 	{Panel_1024x768x75, 0x0018, 0x0000, 9}, /* XGI_LVDS1024x768Des_1x75 */
 	{Panel_1024x768x75, 0x0618, 0x0410, 10}, /* XGI_LVDS1024x768Des_3x75 */
 	{Panel_1024x768x75, 0x0018, 0x0010, 11}, /* XGI_LVDS1024x768Des_2x75 */
-	{Panel_1280x1024x75, 0x0018, 0x0000, 12}, /* XGI_LVDS1280x1024Des_1x75 */
-	{Panel_1280x1024x75, 0x0018, 0x0010, 13}, /* XGI_LVDS1280x1024Des_2x75 */
+	/* XGI_LVDS1280x1024Des_1x75 */
+	{Panel_1280x1024x75, 0x0018, 0x0000, 12},
+	/* XGI_LVDS1280x1024Des_2x75 */
+	{Panel_1280x1024x75, 0x0018, 0x0010, 13},
 	{PanelRef75Hz, 0x0008, 0x0008, 14}, /* XGI_LVDSNoScalingDesDatax75 */
 	{0xFF, 0x0000, 0x0000, 0}
 };
@@ -1991,70 +1981,70 @@ static unsigned short LCDLenList[] = {
 /* Dual link only */
 static struct XGI330_LCDCapStruct  XGI_LCDDLCapList[] = {
 /* LCDCap1024x768 */
-	{Panel_1024x768, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65_315,
+	{Panel_1024x768, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
 	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024 */
 	{Panel_1280x1024, XGI_LCDDualLink+DefaultLCDCap, StLCDBToA,
-	0x012, 0x70, 0x03, VCLK108_2_315,
+	0x70, 0x03, VCLK108_2_315,
 	0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1400x1050 */
 	{Panel_1400x1050, XGI_LCDDualLink+DefaultLCDCap, StLCDBToA,
-	0x012, 0x70, 0x03, VCLK108_2_315,
+	0x70, 0x03, VCLK108_2_315,
 	 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1600x1200 */
 	{Panel_1600x1200, XGI_LCDDualLink+DefaultLCDCap, LCDToFull,
-	0x012, 0xC0, 0x03, VCLK162,
+	0xC0, 0x03, VCLK162,
 	 0x43, 0x22, 0x70, 0x24, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1024x768x75 */
-	{Panel_1024x768x75, DefaultLCDCap, 0, 0x012, 0x60, 0, VCLK78_75,
+	{Panel_1024x768x75, DefaultLCDCap, 0, 0x60, 0, VCLK78_75,
 	 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024x75 */
 	{Panel_1280x1024x75, XGI_LCDDualLink+DefaultLCDCap, StLCDBToA,
-	0x012, 0x90, 0x03, VCLK135_5,
+	 0x90, 0x03, VCLK135_5,
 	 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCapDefault */
-	{0xFF, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65_315,
+	{0xFF, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
 	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
 };
 
 static struct XGI330_LCDCapStruct  XGI_LCDCapList[] = {
 /* LCDCap1024x768 */
-	{Panel_1024x768, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65_315,
+	{Panel_1024x768, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
 	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024 */
 	{Panel_1280x1024, DefaultLCDCap, StLCDBToA,
-	0x012, 0x70, 0x03, VCLK108_2_315,
+	0x70, 0x03, VCLK108_2_315,
 	0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1400x1050 */
 	{Panel_1400x1050, DefaultLCDCap, StLCDBToA,
-	0x012, 0x70, 0x03, VCLK108_2_315,
+	 0x70, 0x03, VCLK108_2_315,
 	 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1600x1200 */
 	{Panel_1600x1200, DefaultLCDCap, LCDToFull,
-	0x012, 0xC0, 0x03, VCLK162,
+	 0xC0, 0x03, VCLK162,
 	 0x5A, 0x23, 0x5A, 0x23, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCap1024x768x75 */
-	{Panel_1024x768x75, DefaultLCDCap, 0, 0x012, 0x60, 0, VCLK78_75,
+	{Panel_1024x768x75, DefaultLCDCap, 0, 0x60, 0, VCLK78_75,
 	 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
 /* LCDCap1280x1024x75 */
 	{Panel_1280x1024x75, DefaultLCDCap, StLCDBToA,
-	0x012, 0x90, 0x03, VCLK135_5,
+	 0x90, 0x03, VCLK135_5,
 	 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
 /* LCDCapDefault */
-	{0xFF, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65_315,
+	{0xFF, DefaultLCDCap, 0, 0x88, 0x06, VCLK65_315,
 	0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
 };
@@ -2253,49 +2243,7 @@ static struct SiS_ModeResInfo_S XGI330_ModeResInfo[] = {
 	{1152,  864, 8, 16}
 };
 
-static unsigned char XGI330_OutputSelect = 0x40;
-static unsigned char XGI330_SoftSetting = 0x30;
-static unsigned char XGI330_SR07 = 0x18;
-
-static unsigned char XGI330_CR49[] = {0xaa, 0x88};
-static unsigned char XGI330_SR1F;
-static unsigned char XGI330_SR21 = 0xa3;
-static unsigned char XGI330_SR22 = 0xfb;
-static unsigned char XGI330_SR23 = 0xf6;
-static unsigned char XGI330_SR24 = 0xd;
-
-static unsigned char XGI330_CRT2Data_1_2;
-static unsigned char XGI330_CRT2Data_4_D;
-static unsigned char XGI330_CRT2Data_4_E;
-static unsigned char XGI330_CRT2Data_4_10 = 0x80;
-static unsigned short XGI330_RGBSenseData = 0xd1;
-static unsigned short XGI330_VideoSenseData = 0xb9;
-static unsigned short XGI330_YCSenseData = 0xb3;
-static unsigned short XGI330_RGBSenseData2 = 0x0190;     /*301b*/
-static unsigned short XGI330_VideoSenseData2 = 0x0110;
-static unsigned short XGI330_YCSenseData2 = 0x016B;
-static unsigned char XG40_I2CDefinition;
-static unsigned char XG20_CR97 = 0x10 ;
-
-static unsigned char XG21_DVOSetting;
-static unsigned char XG21_CR2E;
-static unsigned char XG21_CR2F;
-static unsigned char XG21_CR46;
-static unsigned char XG21_CR47;
-
-static unsigned char XG27_CR97 = 0xC1 ;
-static unsigned char XG27_SR36 = 0x30 ;
-static unsigned char XG27_CR8F = 0x0C ;
-static unsigned char XG27_CRD0[] = {
-	0, 0, 0, 0, 0, 0, 0, 0x82, 0x00, 0x66, 0x01, 0x00
-};
-static unsigned char XG27_CRDE[2];
-static unsigned char XG27_SR40 = 0x04 ;
-static unsigned char XG27_SR41 = 0x00 ;
-
-static unsigned char Z11m_CR97 = 0x80 ;
-
-static struct XGI330_VCLKDataStruct XGI_VCLKData[] = {
+static struct SiS_VCLKData XGI_VCLKData[] = {
 	/* SR2B,SR2C,SR2D */
 	{0x1B, 0xE1,  25}, /* 00 (25.175MHz) */
 	{0x4E, 0xE4,  28}, /* 01 (28.322MHz) */
@@ -2388,7 +2336,7 @@ static struct XGI330_VCLKDataStruct XGI_VCLKData[] = {
 	{0xFF, 0x00,   0}  /* End mark */
 };
 
-static struct XGI330_VCLKDataStruct XGI_VBVCLKData[] = {
+static struct SiS_VCLKData XGI_VBVCLKData[] = {
 	{0x1B, 0xE1,  25}, /* 00 (25.175MHz) */
 	{0x4E, 0xE4,  28}, /* 01 (28.322MHz) */
 	{0x57, 0xE4,  31}, /* 02 (31.500MHz) */
@@ -2480,36 +2428,8 @@ static struct XGI330_VCLKDataStruct XGI_VBVCLKData[] = {
 	{0xFF, 0x00,   0}  /* End mark */
 };
 
-static unsigned char XGI301TVDelayList[] = {
-	0x22, /* ; 0 ExtNTSCDelay */
-	0x22, /* ; 1 StNTSCDelay */
-	0x22, /* ; 2 ExtPALDelay */
-	0x22, /* ; 3 StPALDelay */
-	0x88, /* ; 4 ExtHiTVDelay(1080i) */
-	0xBB, /* ; 5 StHiTVDelay(1080i) */
-	0x22, /* ; 6 ExtYPbPrDelay(525i) */
-	0x22, /* ; 7 StYPbPrDealy(525i) */
-	0x22, /* ; 8 ExtYPbPrDelay(525p) */
-	0x22, /* ; 9 StYPbPrDealy(525p) */
-	0x22, /* ; A ExtYPbPrDelay(750p) */
-	0x22  /* B StYPbPrDealy(750p) */
-};
-
-static unsigned char XGI301TVDelayList2[] = {
-	0x22, /* ; 0 ExtNTSCDelay */
-	0x22, /* ; 1 StNTSCDelay */
-	0x22, /* ; 2 ExtPALDelay */
-	0x22, /* ; 3 StPALDelay */
-	0x22, /* ; 4 ExtHiTVDelay */
-	0x22, /* ; 5 StHiTVDelay */
-	0x22, /* ; 6 ExtYPbPrDelay(525i) */
-	0x22, /* ; 7 StYPbPrDealy(525i) */
-	0x22, /* ; 8 ExtYPbPrDelay(525p) */
-	0x22, /* ; 9 StYPbPrDealy(525p) */
-	0x22, /* ; A ExtYPbPrDelay(750p) */
-	0x22  /* ; B StYPbPrDealy(750p) */
-};
-
+#define XGI301TVDelay 0x22
+#define XGI301LCDDelay 0x12
 
 static unsigned char TVAntiFlickList[] = {/* NTSCAntiFlicker */
 	0x04, /* ; 0 Adaptive */
@@ -2696,3 +2616,4 @@ static struct XGI301C_Tap4TimingStruct YPbPr750pTap4Timing[] = {
 		 }
 	}
 };
+#endif
diff --git a/drivers/staging/xgifb/vb_util.c b/drivers/staging/xgifb/vb_util.c
index b5c99891ead..1b452f8b627 100644
--- a/drivers/staging/xgifb/vb_util.c
+++ b/drivers/staging/xgifb/vb_util.c
@@ -1,12 +1,4 @@
-#include <linux/io.h>
-#include <linux/types.h>
-
-#include "vb_def.h"
 #include "vgatypes.h"
-#include "vb_struct.h"
-
-#include "XGIfb.h"
-
 #include "vb_util.h"
 
 void xgifb_reg_set(unsigned long port, u8 index, u8 data)
diff --git a/drivers/staging/xgifb/vgatypes.h b/drivers/staging/xgifb/vgatypes.h
index 30cdd1af81f..ddf7776c295 100644
--- a/drivers/staging/xgifb/vgatypes.h
+++ b/drivers/staging/xgifb/vgatypes.h
@@ -1,7 +1,6 @@
 #ifndef _VGATYPES_
 #define _VGATYPES_
 
-#include <linux/ioctl.h>
 #include <linux/fb.h>	/* for struct fb_var_screeninfo for sis.h */
 #include "../../video/sis/vgatypes.h"
 #include "../../video/sis/sis.h"		/* for LCD_TYPE */
@@ -22,40 +21,6 @@ enum XGI_VB_CHIP_TYPE {
 };
 #endif
 
-
-#define XGI_LCD_TYPE
-/* Since the merge with video/sis the LCD_TYPEs are used from
- drivers/video/sis/sis.h . Nevertheless we keep this (for the moment) for
- future reference until the code is merged completely and we are sure
- nothing of this should be added to the sis.h header */
-#ifndef XGI_LCD_TYPE
-enum XGI_LCD_TYPE {
-	LCD_INVALID = 0,
-	LCD_320x480,       /* FSTN, DSTN */
-	LCD_640x480,
-	LCD_640x480_2,     /* FSTN, DSTN */
-	LCD_640x480_3,     /* FSTN, DSTN */
-	LCD_800x600,
-	LCD_848x480,
-	LCD_1024x600,
-	LCD_1024x768,
-	LCD_1152x768,
-	LCD_1152x864,
-	LCD_1280x720,
-	LCD_1280x768,
-	LCD_1280x800,
-	LCD_1280x960,
-	LCD_1280x1024,
-	LCD_1400x1050,
-	LCD_1600x1200,
-	LCD_1680x1050,
-	LCD_1920x1440,
-	LCD_2048x1536,
-	LCD_CUSTOM,
-	LCD_UNKNOWN
-};
-#endif
-
 struct xgi_hw_device_info {
 	unsigned long ulExternalChip; /* NO VB or other video bridge*/
 				      /* if ujVBChipID = VB_CHIP_UNKNOWN, */
diff --git a/drivers/staging/zcache/Kconfig b/drivers/staging/zcache/Kconfig
index 7048e01f081..4881839be62 100644
--- a/drivers/staging/zcache/Kconfig
+++ b/drivers/staging/zcache/Kconfig
@@ -1,9 +1,6 @@
 config ZCACHE
 	bool "Dynamic compression of swap pages and clean pagecache pages"
-	# X86 dependency is because zsmalloc uses non-portable pte/tlb
-	# functions
-	depends on (CLEANCACHE || FRONTSWAP) && CRYPTO=y && X86
-	select ZSMALLOC
+	depends on (CLEANCACHE || FRONTSWAP) && CRYPTO=y && ZSMALLOC=y
 	select CRYPTO_LZO
 	default n
 	help
diff --git a/drivers/staging/zcache/tmem.c b/drivers/staging/zcache/tmem.c
index 1ca66ea9b28..eaa90213457 100644
--- a/drivers/staging/zcache/tmem.c
+++ b/drivers/staging/zcache/tmem.c
@@ -72,33 +72,49 @@ void tmem_register_pamops(struct tmem_pamops *m)
  * the hashbucket lock must be held.
  */
 
-/* searches for object==oid in pool, returns locked object if found */
-static struct tmem_obj *tmem_obj_find(struct tmem_hashbucket *hb,
-					struct tmem_oid *oidp)
+static struct tmem_obj
+*__tmem_obj_find(struct tmem_hashbucket*hb, struct tmem_oid *oidp,
+		 struct rb_node **parent, struct rb_node ***link)
 {
-	struct rb_node *rbnode;
-	struct tmem_obj *obj;
-
-	rbnode = hb->obj_rb_root.rb_node;
-	while (rbnode) {
-		BUG_ON(RB_EMPTY_NODE(rbnode));
-		obj = rb_entry(rbnode, struct tmem_obj, rb_tree_node);
+	struct rb_node *_parent = NULL, **rbnode;
+	struct tmem_obj *obj = NULL;
+
+	rbnode = &hb->obj_rb_root.rb_node;
+	while (*rbnode) {
+		BUG_ON(RB_EMPTY_NODE(*rbnode));
+		_parent = *rbnode;
+		obj = rb_entry(*rbnode, struct tmem_obj,
+			       rb_tree_node);
 		switch (tmem_oid_compare(oidp, &obj->oid)) {
 		case 0: /* equal */
 			goto out;
 		case -1:
-			rbnode = rbnode->rb_left;
+			rbnode = &(*rbnode)->rb_left;
 			break;
 		case 1:
-			rbnode = rbnode->rb_right;
+			rbnode = &(*rbnode)->rb_right;
 			break;
 		}
 	}
+
+	if (parent)
+		*parent = _parent;
+	if (link)
+		*link = rbnode;
+
 	obj = NULL;
 out:
 	return obj;
 }
 
+
+/* searches for object==oid in pool, returns locked object if found */
+static struct tmem_obj *tmem_obj_find(struct tmem_hashbucket *hb,
+					struct tmem_oid *oidp)
+{
+	return __tmem_obj_find(hb, oidp, NULL, NULL);
+}
+
 static void tmem_pampd_destroy_all_in_obj(struct tmem_obj *);
 
 /* free an object that has no more pampds in it */
@@ -131,8 +147,7 @@ static void tmem_obj_init(struct tmem_obj *obj, struct tmem_hashbucket *hb,
 					struct tmem_oid *oidp)
 {
 	struct rb_root *root = &hb->obj_rb_root;
-	struct rb_node **new = &(root->rb_node), *parent = NULL;
-	struct tmem_obj *this;
+	struct rb_node **new = NULL, *parent = NULL;
 
 	BUG_ON(pool == NULL);
 	atomic_inc(&pool->obj_count);
@@ -144,22 +159,10 @@ static void tmem_obj_init(struct tmem_obj *obj, struct tmem_hashbucket *hb,
 	obj->pampd_count = 0;
 	(*tmem_pamops.new_obj)(obj);
 	SET_SENTINEL(obj, OBJ);
-	while (*new) {
-		BUG_ON(RB_EMPTY_NODE(*new));
-		this = rb_entry(*new, struct tmem_obj, rb_tree_node);
-		parent = *new;
-		switch (tmem_oid_compare(oidp, &this->oid)) {
-		case 0:
-			BUG(); /* already present; should never happen! */
-			break;
-		case -1:
-			new = &(*new)->rb_left;
-			break;
-		case 1:
-			new = &(*new)->rb_right;
-			break;
-		}
-	}
+
+	if (__tmem_obj_find(hb, oidp, &parent, &new))
+		BUG();
+
 	rb_link_node(&obj->rb_tree_node, parent, new);
 	rb_insert_color(&obj->rb_tree_node, root);
 }
diff --git a/drivers/staging/zcache/zcache-main.c b/drivers/staging/zcache/zcache-main.c
index 784c796b984..c214977b4ab 100644
--- a/drivers/staging/zcache/zcache-main.c
+++ b/drivers/staging/zcache/zcache-main.c
@@ -31,13 +31,11 @@
 #include <linux/math64.h>
 #include <linux/crypto.h>
 #include <linux/string.h>
+#include <linux/idr.h>
 #include "tmem.h"
 
 #include "../zsmalloc/zsmalloc.h"
 
-#if (!defined(CONFIG_CLEANCACHE) && !defined(CONFIG_FRONTSWAP))
-#error "zcache is useless without CONFIG_CLEANCACHE or CONFIG_FRONTSWAP"
-#endif
 #ifdef CONFIG_CLEANCACHE
 #include <linux/cleancache.h>
 #endif
@@ -53,15 +51,13 @@
 	(__GFP_FS | __GFP_NORETRY | __GFP_NOWARN | __GFP_NOMEMALLOC)
 #endif
 
-#define MAX_POOLS_PER_CLIENT 16
-
 #define MAX_CLIENTS 16
 #define LOCAL_CLIENT ((uint16_t)-1)
 
 MODULE_LICENSE("GPL");
 
 struct zcache_client {
-	struct tmem_pool *tmem_pools[MAX_POOLS_PER_CLIENT];
+	struct idr tmem_pools;
 	struct zs_pool *zspool;
 	bool allocated;
 	atomic_t refcount;
@@ -78,6 +74,17 @@ static inline uint16_t get_client_id_from_client(struct zcache_client *cli)
 	return cli - &zcache_clients[0];
 }
 
+static struct zcache_client *get_zcache_client(uint16_t cli_id)
+{
+	if (cli_id == LOCAL_CLIENT)
+		return &zcache_host;
+
+	if ((unsigned int)cli_id < MAX_CLIENTS)
+		return &zcache_clients[cli_id];
+
+	return NULL;
+}
+
 static inline bool is_local_client(struct zcache_client *cli)
 {
 	return cli == &zcache_host;
@@ -110,6 +117,8 @@ static inline int zcache_comp_op(enum comp_op op,
 	case ZCACHE_COMPOP_DECOMPRESS:
 		ret = crypto_comp_decompress(tfm, src, slen, dst, dlen);
 		break;
+	default:
+		ret = -EINVAL;
 	}
 	put_cpu();
 	return ret;
@@ -602,16 +611,14 @@ out:
 	return;
 }
 
-static void zbud_init(void)
+static void __init zbud_init(void)
 {
 	int i;
 
 	INIT_LIST_HEAD(&zbud_buddied_list);
-	zcache_zbud_buddied_count = 0;
-	for (i = 0; i < NCHUNKS; i++) {
+
+	for (i = 0; i < NCHUNKS; i++)
 		INIT_LIST_HEAD(&zbud_unbuddied[i].list);
-		zbud_unbuddied[i].count = 0;
-	}
 }
 
 #ifdef CONFIG_SYSFS
@@ -693,14 +700,14 @@ static unsigned int zv_max_mean_zsize = (PAGE_SIZE / 8) * 5;
 static atomic_t zv_curr_dist_counts[NCHUNKS];
 static atomic_t zv_cumul_dist_counts[NCHUNKS];
 
-static struct zv_hdr *zv_create(struct zs_pool *pool, uint32_t pool_id,
+static unsigned long zv_create(struct zs_pool *pool, uint32_t pool_id,
 				struct tmem_oid *oid, uint32_t index,
 				void *cdata, unsigned clen)
 {
 	struct zv_hdr *zv;
 	u32 size = clen + sizeof(struct zv_hdr);
 	int chunks = (size + (CHUNK_SIZE - 1)) >> CHUNK_SHIFT;
-	void *handle = NULL;
+	unsigned long handle = 0;
 
 	BUG_ON(!irqs_disabled());
 	BUG_ON(chunks >= NCHUNKS);
@@ -709,7 +716,7 @@ static struct zv_hdr *zv_create(struct zs_pool *pool, uint32_t pool_id,
 		goto out;
 	atomic_inc(&zv_curr_dist_counts[chunks]);
 	atomic_inc(&zv_cumul_dist_counts[chunks]);
-	zv = zs_map_object(pool, handle);
+	zv = zs_map_object(pool, handle, ZS_MM_WO);
 	zv->index = index;
 	zv->oid = *oid;
 	zv->pool_id = pool_id;
@@ -721,14 +728,14 @@ out:
 	return handle;
 }
 
-static void zv_free(struct zs_pool *pool, void *handle)
+static void zv_free(struct zs_pool *pool, unsigned long handle)
 {
 	unsigned long flags;
 	struct zv_hdr *zv;
 	uint16_t size;
 	int chunks;
 
-	zv = zs_map_object(pool, handle);
+	zv = zs_map_object(pool, handle, ZS_MM_RW);
 	ASSERT_SENTINEL(zv, ZVH);
 	size = zv->size + sizeof(struct zv_hdr);
 	INVERT_SENTINEL(zv, ZVH);
@@ -743,14 +750,14 @@ static void zv_free(struct zs_pool *pool, void *handle)
 	local_irq_restore(flags);
 }
 
-static void zv_decompress(struct page *page, void *handle)
+static void zv_decompress(struct page *page, unsigned long handle)
 {
 	unsigned int clen = PAGE_SIZE;
 	char *to_va;
 	int ret;
 	struct zv_hdr *zv;
 
-	zv = zs_map_object(zcache_host.zspool, handle);
+	zv = zs_map_object(zcache_host.zspool, handle, ZS_MM_RO);
 	BUG_ON(zv->size == 0);
 	ASSERT_SENTINEL(zv, ZVH);
 	to_va = kmap_atomic(page);
@@ -939,21 +946,14 @@ static struct tmem_pool *zcache_get_pool_by_id(uint16_t cli_id, uint16_t poolid)
 	struct tmem_pool *pool = NULL;
 	struct zcache_client *cli = NULL;
 
-	if (cli_id == LOCAL_CLIENT)
-		cli = &zcache_host;
-	else {
-		if (cli_id >= MAX_CLIENTS)
-			goto out;
-		cli = &zcache_clients[cli_id];
-		if (cli == NULL)
-			goto out;
-		atomic_inc(&cli->refcount);
-	}
-	if (poolid < MAX_POOLS_PER_CLIENT) {
-		pool = cli->tmem_pools[poolid];
-		if (pool != NULL)
-			atomic_inc(&pool->refcount);
-	}
+	cli = get_zcache_client(cli_id);
+	if (!cli)
+		goto out;
+
+	atomic_inc(&cli->refcount);
+	pool = idr_find(&cli->tmem_pools, poolid);
+	if (pool != NULL)
+		atomic_inc(&pool->refcount);
 out:
 	return pool;
 }
@@ -971,13 +971,11 @@ static void zcache_put_pool(struct tmem_pool *pool)
 
 int zcache_new_client(uint16_t cli_id)
 {
-	struct zcache_client *cli = NULL;
+	struct zcache_client *cli;
 	int ret = -1;
 
-	if (cli_id == LOCAL_CLIENT)
-		cli = &zcache_host;
-	else if ((unsigned int)cli_id < MAX_CLIENTS)
-		cli = &zcache_clients[cli_id];
+	cli = get_zcache_client(cli_id);
+
 	if (cli == NULL)
 		goto out;
 	if (cli->allocated)
@@ -987,6 +985,7 @@ int zcache_new_client(uint16_t cli_id)
 	cli->zspool = zs_create_pool("zcache", ZCACHE_GFP_MASK);
 	if (cli->zspool == NULL)
 		goto out;
+	idr_init(&cli->tmem_pools);
 #endif
 	ret = 0;
 out:
@@ -1035,45 +1034,38 @@ static int zcache_do_preload(struct tmem_pool *pool)
 		goto out;
 	if (unlikely(zcache_obj_cache == NULL))
 		goto out;
-	preempt_disable();
+
+	/* IRQ has already been disabled. */
 	kp = &__get_cpu_var(zcache_preloads);
 	while (kp->nr < ARRAY_SIZE(kp->objnodes)) {
-		preempt_enable_no_resched();
 		objnode = kmem_cache_alloc(zcache_objnode_cache,
 				ZCACHE_GFP_MASK);
 		if (unlikely(objnode == NULL)) {
 			zcache_failed_alloc++;
 			goto out;
 		}
-		preempt_disable();
-		kp = &__get_cpu_var(zcache_preloads);
-		if (kp->nr < ARRAY_SIZE(kp->objnodes))
-			kp->objnodes[kp->nr++] = objnode;
-		else
-			kmem_cache_free(zcache_objnode_cache, objnode);
+
+		kp->objnodes[kp->nr++] = objnode;
 	}
-	preempt_enable_no_resched();
-	obj = kmem_cache_alloc(zcache_obj_cache, ZCACHE_GFP_MASK);
-	if (unlikely(obj == NULL)) {
-		zcache_failed_alloc++;
-		goto out;
+
+	if (!kp->obj) {
+		obj = kmem_cache_alloc(zcache_obj_cache, ZCACHE_GFP_MASK);
+		if (unlikely(obj == NULL)) {
+			zcache_failed_alloc++;
+			goto out;
+		}
+		kp->obj = obj;
 	}
-	page = (void *)__get_free_page(ZCACHE_GFP_MASK);
-	if (unlikely(page == NULL)) {
-		zcache_failed_get_free_pages++;
-		kmem_cache_free(zcache_obj_cache, obj);
-		goto out;
+
+	if (!kp->page) {
+		page = (void *)__get_free_page(ZCACHE_GFP_MASK);
+		if (unlikely(page == NULL)) {
+			zcache_failed_get_free_pages++;
+			goto out;
+		}
+		kp->page =  page;
 	}
-	preempt_disable();
-	kp = &__get_cpu_var(zcache_preloads);
-	if (kp->obj == NULL)
-		kp->obj = obj;
-	else
-		kmem_cache_free(zcache_obj_cache, obj);
-	if (kp->page == NULL)
-		kp->page = page;
-	else
-		free_page((unsigned long)page);
+
 	ret = 0;
 out:
 	return ret;
@@ -1247,7 +1239,7 @@ static int zcache_pampd_get_data(char *data, size_t *bufsize, bool raw,
 	int ret = 0;
 
 	BUG_ON(is_ephemeral(pool));
-	zv_decompress((struct page *)(data), pampd);
+	zv_decompress((struct page *)(data), (unsigned long)pampd);
 	return ret;
 }
 
@@ -1282,7 +1274,7 @@ static void zcache_pampd_free(void *pampd, struct tmem_pool *pool,
 		atomic_dec(&zcache_curr_eph_pampd_count);
 		BUG_ON(atomic_read(&zcache_curr_eph_pampd_count) < 0);
 	} else {
-		zv_free(cli->zspool, pampd);
+		zv_free(cli->zspool, (unsigned long)pampd);
 		atomic_dec(&zcache_curr_pers_pampd_count);
 		BUG_ON(atomic_read(&zcache_curr_pers_pampd_count) < 0);
 	}
@@ -1583,15 +1575,14 @@ static int zcache_put_page(int cli_id, int pool_id, struct tmem_oid *oidp,
 			else
 				zcache_failed_pers_puts++;
 		}
-		zcache_put_pool(pool);
-		preempt_enable_no_resched();
 	} else {
 		zcache_put_to_flush++;
 		if (atomic_read(&pool->obj_count) > 0)
 			/* the put fails whether the flush succeeds or not */
 			(void)tmem_flush_page(pool, oidp, index);
-		zcache_put_pool(pool);
 	}
+
+	zcache_put_pool(pool);
 out:
 	return ret;
 }
@@ -1661,22 +1652,21 @@ static int zcache_flush_object(int cli_id, int pool_id,
 static int zcache_destroy_pool(int cli_id, int pool_id)
 {
 	struct tmem_pool *pool = NULL;
-	struct zcache_client *cli = NULL;
+	struct zcache_client *cli;
 	int ret = -1;
 
 	if (pool_id < 0)
 		goto out;
-	if (cli_id == LOCAL_CLIENT)
-		cli = &zcache_host;
-	else if ((unsigned int)cli_id < MAX_CLIENTS)
-		cli = &zcache_clients[cli_id];
+
+	cli = get_zcache_client(cli_id);
 	if (cli == NULL)
 		goto out;
+
 	atomic_inc(&cli->refcount);
-	pool = cli->tmem_pools[pool_id];
+	pool = idr_find(&cli->tmem_pools, pool_id);
 	if (pool == NULL)
 		goto out;
-	cli->tmem_pools[pool_id] = NULL;
+	idr_remove(&cli->tmem_pools, pool_id);
 	/* wait for pool activity on other cpus to quiesce */
 	while (atomic_read(&pool->refcount) != 0)
 		;
@@ -1696,13 +1686,12 @@ static int zcache_new_pool(uint16_t cli_id, uint32_t flags)
 	int poolid = -1;
 	struct tmem_pool *pool;
 	struct zcache_client *cli = NULL;
+	int r;
 
-	if (cli_id == LOCAL_CLIENT)
-		cli = &zcache_host;
-	else if ((unsigned int)cli_id < MAX_CLIENTS)
-		cli = &zcache_clients[cli_id];
+	cli = get_zcache_client(cli_id);
 	if (cli == NULL)
 		goto out;
+
 	atomic_inc(&cli->refcount);
 	pool = kmalloc(sizeof(struct tmem_pool), GFP_ATOMIC);
 	if (pool == NULL) {
@@ -1710,20 +1699,25 @@ static int zcache_new_pool(uint16_t cli_id, uint32_t flags)
 		goto out;
 	}
 
-	for (poolid = 0; poolid < MAX_POOLS_PER_CLIENT; poolid++)
-		if (cli->tmem_pools[poolid] == NULL)
-			break;
-	if (poolid >= MAX_POOLS_PER_CLIENT) {
-		pr_info("zcache: pool creation failed: max exceeded\n");
+	do {
+		r = idr_pre_get(&cli->tmem_pools, GFP_ATOMIC);
+		if (r != 1) {
+			kfree(pool);
+			pr_info("zcache: pool creation failed: out of memory\n");
+			goto out;
+		}
+		r = idr_get_new(&cli->tmem_pools, pool, &poolid);
+	} while (r == -EAGAIN);
+	if (r) {
+		pr_info("zcache: pool creation failed: error %d\n", r);
 		kfree(pool);
-		poolid = -1;
 		goto out;
 	}
+
 	atomic_set(&pool->refcount, 0);
 	pool->client = cli;
 	pool->pool_id = poolid;
 	tmem_new_pool(pool, flags);
-	cli->tmem_pools[poolid] = pool;
 	pr_info("zcache: created %s tmem pool, id=%d, client=%d\n",
 		flags & TMEM_POOL_PERSIST ? "persistent" : "ephemeral",
 		poolid, cli_id);
@@ -1981,7 +1975,7 @@ static int __init enable_zcache_compressor(char *s)
 __setup("zcache=", enable_zcache_compressor);
 
 
-static int zcache_comp_init(void)
+static int __init zcache_comp_init(void)
 {
 	int ret = 0;
 
@@ -2021,7 +2015,7 @@ static int __init zcache_init(void)
 		goto out;
 	}
 #endif /* CONFIG_SYSFS */
-#if defined(CONFIG_CLEANCACHE) || defined(CONFIG_FRONTSWAP)
+
 	if (zcache_enabled) {
 		unsigned int cpu;
 
@@ -2052,7 +2046,7 @@ static int __init zcache_init(void)
 		pr_err("zcache: can't create client\n");
 		goto out;
 	}
-#endif
+
 #ifdef CONFIG_CLEANCACHE
 	if (zcache_enabled && use_cleancache) {
 		struct cleancache_ops old_ops;
diff --git a/drivers/staging/zram/Kconfig b/drivers/staging/zram/Kconfig
index 9d11a4cb99b..be5abe8e794 100644
--- a/drivers/staging/zram/Kconfig
+++ b/drivers/staging/zram/Kconfig
@@ -1,9 +1,6 @@
 config ZRAM
 	tristate "Compressed RAM block device support"
-	# X86 dependency is because zsmalloc uses non-portable pte/tlb
-	# functions
-	depends on BLOCK && SYSFS && X86
-	select ZSMALLOC
+	depends on BLOCK && SYSFS && ZSMALLOC
 	select LZO_COMPRESS
 	select LZO_DECOMPRESS
 	default n
diff --git a/drivers/staging/zram/zram_drv.c b/drivers/staging/zram/zram_drv.c
index 685d612a627..653b074035f 100644
--- a/drivers/staging/zram/zram_drv.c
+++ b/drivers/staging/zram/zram_drv.c
@@ -135,7 +135,8 @@ static void zram_set_disksize(struct zram *zram, size_t totalram_bytes)
 
 static void zram_free_page(struct zram *zram, size_t index)
 {
-	void *handle = zram->table[index].handle;
+	unsigned long handle = zram->table[index].handle;
+	u16 size = zram->table[index].size;
 
 	if (unlikely(!handle)) {
 		/*
@@ -149,24 +150,19 @@ static void zram_free_page(struct zram *zram, size_t index)
 		return;
 	}
 
-	if (unlikely(zram_test_flag(zram, index, ZRAM_UNCOMPRESSED))) {
-		__free_page(handle);
-		zram_clear_flag(zram, index, ZRAM_UNCOMPRESSED);
-		zram_stat_dec(&zram->stats.pages_expand);
-		goto out;
-	}
+	if (unlikely(size > max_zpage_size))
+		zram_stat_dec(&zram->stats.bad_compress);
 
 	zs_free(zram->mem_pool, handle);
 
-	if (zram->table[index].size <= PAGE_SIZE / 2)
+	if (size <= PAGE_SIZE / 2)
 		zram_stat_dec(&zram->stats.good_compress);
 
-out:
 	zram_stat64_sub(zram, &zram->stats.compr_size,
 			zram->table[index].size);
 	zram_stat_dec(&zram->stats.pages_stored);
 
-	zram->table[index].handle = NULL;
+	zram->table[index].handle = 0;
 	zram->table[index].size = 0;
 }
 
@@ -182,22 +178,6 @@ static void handle_zero_page(struct bio_vec *bvec)
 	flush_dcache_page(page);
 }
 
-static void handle_uncompressed_page(struct zram *zram, struct bio_vec *bvec,
-				     u32 index, int offset)
-{
-	struct page *page = bvec->bv_page;
-	unsigned char *user_mem, *cmem;
-
-	user_mem = kmap_atomic(page);
-	cmem = kmap_atomic(zram->table[index].handle);
-
-	memcpy(user_mem + bvec->bv_offset, cmem + offset, bvec->bv_len);
-	kunmap_atomic(cmem);
-	kunmap_atomic(user_mem);
-
-	flush_dcache_page(page);
-}
-
 static inline int is_partial_io(struct bio_vec *bvec)
 {
 	return bvec->bv_len != PAGE_SIZE;
@@ -209,7 +189,6 @@ static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
 	int ret;
 	size_t clen;
 	struct page *page;
-	struct zobj_header *zheader;
 	unsigned char *user_mem, *cmem, *uncmem = NULL;
 
 	page = bvec->bv_page;
@@ -227,12 +206,6 @@ static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
 		return 0;
 	}
 
-	/* Page is stored uncompressed since it's incompressible */
-	if (unlikely(zram_test_flag(zram, index, ZRAM_UNCOMPRESSED))) {
-		handle_uncompressed_page(zram, bvec, index, offset);
-		return 0;
-	}
-
 	if (is_partial_io(bvec)) {
 		/* Use  a temporary buffer to decompress the page */
 		uncmem = kmalloc(PAGE_SIZE, GFP_KERNEL);
@@ -247,10 +220,10 @@ static int zram_bvec_read(struct zram *zram, struct bio_vec *bvec,
 		uncmem = user_mem;
 	clen = PAGE_SIZE;
 
-	cmem = zs_map_object(zram->mem_pool, zram->table[index].handle);
+	cmem = zs_map_object(zram->mem_pool, zram->table[index].handle,
+				ZS_MM_RO);
 
-	ret = lzo1x_decompress_safe(cmem + sizeof(*zheader),
-				    zram->table[index].size,
+	ret = lzo1x_decompress_safe(cmem, zram->table[index].size,
 				    uncmem, &clen);
 
 	if (is_partial_io(bvec)) {
@@ -278,28 +251,18 @@ static int zram_read_before_write(struct zram *zram, char *mem, u32 index)
 {
 	int ret;
 	size_t clen = PAGE_SIZE;
-	struct zobj_header *zheader;
 	unsigned char *cmem;
+	unsigned long handle = zram->table[index].handle;
 
-	if (zram_test_flag(zram, index, ZRAM_ZERO) ||
-	    !zram->table[index].handle) {
+	if (zram_test_flag(zram, index, ZRAM_ZERO) || !handle) {
 		memset(mem, 0, PAGE_SIZE);
 		return 0;
 	}
 
-	cmem = zs_map_object(zram->mem_pool, zram->table[index].handle);
-
-	/* Page is stored uncompressed since it's incompressible */
-	if (unlikely(zram_test_flag(zram, index, ZRAM_UNCOMPRESSED))) {
-		memcpy(mem, cmem, PAGE_SIZE);
-		kunmap_atomic(cmem);
-		return 0;
-	}
-
-	ret = lzo1x_decompress_safe(cmem + sizeof(*zheader),
-				    zram->table[index].size,
+	cmem = zs_map_object(zram->mem_pool, handle, ZS_MM_RO);
+	ret = lzo1x_decompress_safe(cmem, zram->table[index].size,
 				    mem, &clen);
-	zs_unmap_object(zram->mem_pool, zram->table[index].handle);
+	zs_unmap_object(zram->mem_pool, handle);
 
 	/* Should NEVER happen. Return bio error if it does. */
 	if (unlikely(ret != LZO_E_OK)) {
@@ -315,11 +278,9 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 			   int offset)
 {
 	int ret;
-	u32 store_offset;
 	size_t clen;
-	void *handle;
-	struct zobj_header *zheader;
-	struct page *page, *page_store;
+	unsigned long handle;
+	struct page *page;
 	unsigned char *user_mem, *cmem, *src, *uncmem = NULL;
 
 	page = bvec->bv_page;
@@ -381,57 +342,21 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 		goto out;
 	}
 
-	/*
-	 * Page is incompressible. Store it as-is (uncompressed)
-	 * since we do not want to return too many disk write
-	 * errors which has side effect of hanging the system.
-	 */
-	if (unlikely(clen > max_zpage_size)) {
-		clen = PAGE_SIZE;
-		page_store = alloc_page(GFP_NOIO | __GFP_HIGHMEM);
-		if (unlikely(!page_store)) {
-			pr_info("Error allocating memory for "
-				"incompressible page: %u\n", index);
-			ret = -ENOMEM;
-			goto out;
-		}
-
-		store_offset = 0;
-		zram_set_flag(zram, index, ZRAM_UNCOMPRESSED);
-		zram_stat_inc(&zram->stats.pages_expand);
-		handle = page_store;
-		src = kmap_atomic(page);
-		cmem = kmap_atomic(page_store);
-		goto memstore;
-	}
+	if (unlikely(clen > max_zpage_size))
+		zram_stat_inc(&zram->stats.bad_compress);
 
-	handle = zs_malloc(zram->mem_pool, clen + sizeof(*zheader));
+	handle = zs_malloc(zram->mem_pool, clen);
 	if (!handle) {
 		pr_info("Error allocating memory for compressed "
 			"page: %u, size=%zu\n", index, clen);
 		ret = -ENOMEM;
 		goto out;
 	}
-	cmem = zs_map_object(zram->mem_pool, handle);
-
-memstore:
-#if 0
-	/* Back-reference needed for memory defragmentation */
-	if (!zram_test_flag(zram, index, ZRAM_UNCOMPRESSED)) {
-		zheader = (struct zobj_header *)cmem;
-		zheader->table_idx = index;
-		cmem += sizeof(*zheader);
-	}
-#endif
+	cmem = zs_map_object(zram->mem_pool, handle, ZS_MM_WO);
 
 	memcpy(cmem, src, clen);
 
-	if (unlikely(zram_test_flag(zram, index, ZRAM_UNCOMPRESSED))) {
-		kunmap_atomic(cmem);
-		kunmap_atomic(src);
-	} else {
-		zs_unmap_object(zram->mem_pool, handle);
-	}
+	zs_unmap_object(zram->mem_pool, handle);
 
 	zram->table[index].handle = handle;
 	zram->table[index].size = clen;
@@ -592,14 +517,11 @@ void __zram_reset_device(struct zram *zram)
 
 	/* Free all pages that are still in this zram device */
 	for (index = 0; index < zram->disksize >> PAGE_SHIFT; index++) {
-		void *handle = zram->table[index].handle;
+		unsigned long handle = zram->table[index].handle;
 		if (!handle)
 			continue;
 
-		if (unlikely(zram_test_flag(zram, index, ZRAM_UNCOMPRESSED)))
-			__free_page(handle);
-		else
-			zs_free(zram->mem_pool, handle);
+		zs_free(zram->mem_pool, handle);
 	}
 
 	vfree(zram->table);
@@ -724,7 +646,7 @@ static int create_device(struct zram *zram, int device_id)
 	zram->disk = alloc_disk(1);
 	if (!zram->disk) {
 		blk_cleanup_queue(zram->queue);
-		pr_warning("Error allocating disk structure for device %d\n",
+		pr_warn("Error allocating disk structure for device %d\n",
 			device_id);
 		ret = -ENOMEM;
 		goto out;
@@ -755,7 +677,7 @@ static int create_device(struct zram *zram, int device_id)
 	ret = sysfs_create_group(&disk_to_dev(zram->disk)->kobj,
 				&zram_disk_attr_group);
 	if (ret < 0) {
-		pr_warning("Error creating sysfs group");
+		pr_warn("Error creating sysfs group");
 		goto out;
 	}
 
@@ -789,7 +711,7 @@ static int __init zram_init(void)
 	int ret, dev_id;
 
 	if (num_devices > max_num_devices) {
-		pr_warning("Invalid value for num_devices: %u\n",
+		pr_warn("Invalid value for num_devices: %u\n",
 				num_devices);
 		ret = -EINVAL;
 		goto out;
@@ -797,7 +719,7 @@ static int __init zram_init(void)
 
 	zram_major = register_blkdev(0, "zram");
 	if (zram_major <= 0) {
-		pr_warning("Unable to get major number\n");
+		pr_warn("Unable to get major number\n");
 		ret = -EBUSY;
 		goto out;
 	}
diff --git a/drivers/staging/zram/zram_drv.h b/drivers/staging/zram/zram_drv.h
index fbe8ac98704..572c0b1551d 100644
--- a/drivers/staging/zram/zram_drv.h
+++ b/drivers/staging/zram/zram_drv.h
@@ -26,18 +26,6 @@
  */
 static const unsigned max_num_devices = 32;
 
-/*
- * Stored at beginning of each compressed object.
- *
- * It stores back-reference to table entry which points to this
- * object. This is required to support memory defragmentation.
- */
-struct zobj_header {
-#if 0
-	u32 table_idx;
-#endif
-};
-
 /*-- Configurable parameters */
 
 /* Default zram disk size: 25% of total RAM */
@@ -68,9 +56,6 @@ static const size_t max_zpage_size = PAGE_SIZE / 4 * 3;
 
 /* Flags for zram pages (table[page_no].flags) */
 enum zram_pageflags {
-	/* Page is stored uncompressed */
-	ZRAM_UNCOMPRESSED,
-
 	/* Page consists entirely of zeros */
 	ZRAM_ZERO,
 
@@ -81,11 +66,11 @@ enum zram_pageflags {
 
 /* Allocated for each disk page */
 struct table {
-	void *handle;
+	unsigned long handle;
 	u16 size;	/* object size (excluding header) */
 	u8 count;	/* object ref count (not yet used) */
 	u8 flags;
-} __attribute__((aligned(4)));
+} __aligned(4);
 
 struct zram_stats {
 	u64 compr_size;		/* compressed size of pages stored */
@@ -98,7 +83,7 @@ struct zram_stats {
 	u32 pages_zero;		/* no. of zero filled pages */
 	u32 pages_stored;	/* no. of pages currently stored */
 	u32 good_compress;	/* % of pages with compression ratio<=50% */
-	u32 pages_expand;	/* % of incompressible pages */
+	u32 bad_compress;	/* % of pages with compression ratio>=75% */
 };
 
 struct zram {
diff --git a/drivers/staging/zram/zram_sysfs.c b/drivers/staging/zram/zram_sysfs.c
index a7f37717552..edb0ed4125d 100644
--- a/drivers/staging/zram/zram_sysfs.c
+++ b/drivers/staging/zram/zram_sysfs.c
@@ -186,10 +186,8 @@ static ssize_t mem_used_total_show(struct device *dev,
 	u64 val = 0;
 	struct zram *zram = dev_to_zram(dev);
 
-	if (zram->init_done) {
-		val = zs_get_total_size_bytes(zram->mem_pool) +
-			((u64)(zram->stats.pages_expand) << PAGE_SHIFT);
-	}
+	if (zram->init_done)
+		val = zs_get_total_size_bytes(zram->mem_pool);
 
 	return sprintf(buf, "%llu\n", val);
 }
diff --git a/drivers/staging/zsmalloc/Kconfig b/drivers/staging/zsmalloc/Kconfig
index a5ab7200626..90845656579 100644
--- a/drivers/staging/zsmalloc/Kconfig
+++ b/drivers/staging/zsmalloc/Kconfig
@@ -1,9 +1,5 @@
 config ZSMALLOC
 	tristate "Memory allocator for compressed pages"
-	# X86 dependency is because of the use of __flush_tlb_one and set_pte
-	# in zsmalloc-main.c.
-	# TODO: convert these to portable functions
-	depends on X86
 	default n
 	help
 	  zsmalloc is a slab-based memory allocator designed to store
diff --git a/drivers/staging/zsmalloc/zsmalloc-main.c b/drivers/staging/zsmalloc/zsmalloc-main.c
index 44967377328..8b0bcb626a7 100644
--- a/drivers/staging/zsmalloc/zsmalloc-main.c
+++ b/drivers/staging/zsmalloc/zsmalloc-main.c
@@ -10,6 +10,54 @@
  * Released under the terms of GNU General Public License Version 2.0
  */
 
+
+/*
+ * This allocator is designed for use with zcache and zram. Thus, the
+ * allocator is supposed to work well under low memory conditions. In
+ * particular, it never attempts higher order page allocation which is
+ * very likely to fail under memory pressure. On the other hand, if we
+ * just use single (0-order) pages, it would suffer from very high
+ * fragmentation -- any object of size PAGE_SIZE/2 or larger would occupy
+ * an entire page. This was one of the major issues with its predecessor
+ * (xvmalloc).
+ *
+ * To overcome these issues, zsmalloc allocates a bunch of 0-order pages
+ * and links them together using various 'struct page' fields. These linked
+ * pages act as a single higher-order page i.e. an object can span 0-order
+ * page boundaries. The code refers to these linked pages as a single entity
+ * called zspage.
+ *
+ * Following is how we use various fields and flags of underlying
+ * struct page(s) to form a zspage.
+ *
+ * Usage of struct page fields:
+ *	page->first_page: points to the first component (0-order) page
+ *	page->index (union with page->freelist): offset of the first object
+ *		starting in this page. For the first page, this is
+ *		always 0, so we use this field (aka freelist) to point
+ *		to the first free object in zspage.
+ *	page->lru: links together all component pages (except the first page)
+ *		of a zspage
+ *
+ *	For _first_ page only:
+ *
+ *	page->private (union with page->first_page): refers to the
+ *		component page after the first page
+ *	page->freelist: points to the first free object in zspage.
+ *		Free objects are linked together using in-place
+ *		metadata.
+ *	page->objects: maximum number of objects we can store in this
+ *		zspage (class->zspage_order * PAGE_SIZE / class->size)
+ *	page->lru: links together first pages of various zspages.
+ *		Basically forming list of zspages in a fullness group.
+ *	page->mapping: class index and fullness group of the zspage
+ *
+ * Usage of struct page flags:
+ *	PG_private: identifies the first component page
+ *	PG_private2: identifies the last component page
+ *
+ */
+
 #ifdef CONFIG_ZSMALLOC_DEBUG
 #define DEBUG
 #endif
@@ -247,13 +295,11 @@ static void *obj_location_to_handle(struct page *page, unsigned long obj_idx)
 }
 
 /* Decode <page, obj_idx> pair from the given object handle */
-static void obj_handle_to_location(void *handle, struct page **page,
+static void obj_handle_to_location(unsigned long handle, struct page **page,
 				unsigned long *obj_idx)
 {
-	unsigned long hval = (unsigned long)handle;
-
-	*page = pfn_to_page(hval >> OBJ_INDEX_BITS);
-	*obj_idx = hval & OBJ_INDEX_MASK;
+	*page = pfn_to_page(handle >> OBJ_INDEX_BITS);
+	*obj_idx = handle & OBJ_INDEX_MASK;
 }
 
 static unsigned long obj_idx_to_offset(struct page *page,
@@ -354,7 +400,7 @@ static void init_zspage(struct page *first_page, struct size_class *class)
 static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
 {
 	int i, error;
-	struct page *first_page = NULL;
+	struct page *first_page = NULL, *uninitialized_var(prev_page);
 
 	/*
 	 * Allocate individual pages and link them together as:
@@ -369,7 +415,7 @@ static struct page *alloc_zspage(struct size_class *class, gfp_t flags)
 	 */
 	error = -ENOMEM;
 	for (i = 0; i < class->pages_per_zspage; i++) {
-		struct page *page, *prev_page;
+		struct page *page;
 
 		page = alloc_page(flags);
 		if (!page)
@@ -424,12 +470,51 @@ static struct page *find_get_zspage(struct size_class *class)
 	return page;
 }
 
+static void zs_copy_map_object(char *buf, struct page *firstpage,
+				int off, int size)
+{
+	struct page *pages[2];
+	int sizes[2];
+	void *addr;
+
+	pages[0] = firstpage;
+	pages[1] = get_next_page(firstpage);
+	BUG_ON(!pages[1]);
+
+	sizes[0] = PAGE_SIZE - off;
+	sizes[1] = size - sizes[0];
+
+	/* copy object to per-cpu buffer */
+	addr = kmap_atomic(pages[0]);
+	memcpy(buf, addr + off, sizes[0]);
+	kunmap_atomic(addr);
+	addr = kmap_atomic(pages[1]);
+	memcpy(buf + sizes[0], addr, sizes[1]);
+	kunmap_atomic(addr);
+}
 
-/*
- * If this becomes a separate module, register zs_init() with
- * module_init(), zs_exit with module_exit(), and remove zs_initialized
-*/
-static int zs_initialized;
+static void zs_copy_unmap_object(char *buf, struct page *firstpage,
+				int off, int size)
+{
+	struct page *pages[2];
+	int sizes[2];
+	void *addr;
+
+	pages[0] = firstpage;
+	pages[1] = get_next_page(firstpage);
+	BUG_ON(!pages[1]);
+
+	sizes[0] = PAGE_SIZE - off;
+	sizes[1] = size - sizes[0];
+
+	/* copy per-cpu buffer to object */
+	addr = kmap_atomic(pages[0]);
+	memcpy(addr + off, buf, sizes[0]);
+	kunmap_atomic(addr);
+	addr = kmap_atomic(pages[1]);
+	memcpy(addr, buf + sizes[0], sizes[1]);
+	kunmap_atomic(addr);
+}
 
 static int zs_cpu_notifier(struct notifier_block *nb, unsigned long action,
 				void *pcpu)
@@ -440,18 +525,23 @@ static int zs_cpu_notifier(struct notifier_block *nb, unsigned long action,
 	switch (action) {
 	case CPU_UP_PREPARE:
 		area = &per_cpu(zs_map_area, cpu);
-		if (area->vm)
-			break;
-		area->vm = alloc_vm_area(2 * PAGE_SIZE, area->vm_ptes);
-		if (!area->vm)
-			return notifier_from_errno(-ENOMEM);
+		/*
+		 * Make sure we don't leak memory if a cpu UP notification
+		 * and zs_init() race and both call zs_cpu_up() on the same cpu
+		 */
+		if (area->vm_buf)
+			return 0;
+		area->vm_buf = (char *)__get_free_page(GFP_KERNEL);
+		if (!area->vm_buf)
+			return -ENOMEM;
+		return 0;
 		break;
 	case CPU_DEAD:
 	case CPU_UP_CANCELED:
 		area = &per_cpu(zs_map_area, cpu);
-		if (area->vm)
-			free_vm_area(area->vm);
-		area->vm = NULL;
+		if (area->vm_buf)
+			free_page((unsigned long)area->vm_buf);
+		area->vm_buf = NULL;
 		break;
 	}
 
@@ -489,7 +579,7 @@ fail:
 
 struct zs_pool *zs_create_pool(const char *name, gfp_t flags)
 {
-	int i, error, ovhd_size;
+	int i, ovhd_size;
 	struct zs_pool *pool;
 
 	if (!name)
@@ -516,28 +606,9 @@ struct zs_pool *zs_create_pool(const char *name, gfp_t flags)
 
 	}
 
-	/*
-	 * If this becomes a separate module, register zs_init with
-	 * module_init, and remove this block
-	*/
-	if (!zs_initialized) {
-		error = zs_init();
-		if (error)
-			goto cleanup;
-		zs_initialized = 1;
-	}
-
 	pool->flags = flags;
 	pool->name = name;
 
-	error = 0; /* Success */
-
-cleanup:
-	if (error) {
-		zs_destroy_pool(pool);
-		pool = NULL;
-	}
-
 	return pool;
 }
 EXPORT_SYMBOL_GPL(zs_create_pool);
@@ -568,12 +639,12 @@ EXPORT_SYMBOL_GPL(zs_destroy_pool);
  * @size: size of block to allocate
  *
  * On success, handle to the allocated object is returned,
- * otherwise NULL.
+ * otherwise 0.
  * Allocation requests with size > ZS_MAX_ALLOC_SIZE will fail.
  */
-void *zs_malloc(struct zs_pool *pool, size_t size)
+unsigned long zs_malloc(struct zs_pool *pool, size_t size)
 {
-	void *obj;
+	unsigned long obj;
 	struct link_free *link;
 	int class_idx;
 	struct size_class *class;
@@ -582,7 +653,7 @@ void *zs_malloc(struct zs_pool *pool, size_t size)
 	unsigned long m_objidx, m_offset;
 
 	if (unlikely(!size || size > ZS_MAX_ALLOC_SIZE))
-		return NULL;
+		return 0;
 
 	class_idx = get_size_class_index(size);
 	class = &pool->size_class[class_idx];
@@ -595,14 +666,14 @@ void *zs_malloc(struct zs_pool *pool, size_t size)
 		spin_unlock(&class->lock);
 		first_page = alloc_zspage(class, pool->flags);
 		if (unlikely(!first_page))
-			return NULL;
+			return 0;
 
 		set_zspage_mapping(first_page, class->index, ZS_EMPTY);
 		spin_lock(&class->lock);
 		class->pages_allocated += class->pages_per_zspage;
 	}
 
-	obj = first_page->freelist;
+	obj = (unsigned long)first_page->freelist;
 	obj_handle_to_location(obj, &m_page, &m_objidx);
 	m_offset = obj_idx_to_offset(m_page, m_objidx, class->size);
 
@@ -621,7 +692,7 @@ void *zs_malloc(struct zs_pool *pool, size_t size)
 }
 EXPORT_SYMBOL_GPL(zs_malloc);
 
-void zs_free(struct zs_pool *pool, void *obj)
+void zs_free(struct zs_pool *pool, unsigned long obj)
 {
 	struct link_free *link;
 	struct page *first_page, *f_page;
@@ -648,7 +719,7 @@ void zs_free(struct zs_pool *pool, void *obj)
 							+ f_offset);
 	link->next = first_page->freelist;
 	kunmap_atomic(link);
-	first_page->freelist = obj;
+	first_page->freelist = (void *)obj;
 
 	first_page->inuse--;
 	fullness = fix_fullness_group(pool, first_page);
@@ -670,9 +741,15 @@ EXPORT_SYMBOL_GPL(zs_free);
  *
  * Before using an object allocated from zs_malloc, it must be mapped using
  * this function. When done with the object, it must be unmapped using
- * zs_unmap_object
+ * zs_unmap_object.
+ *
+ * Only one object can be mapped per cpu at a time. There is no protection
+ * against nested mappings.
+ *
+ * This function returns with preemption and page faults disabled.
 */
-void *zs_map_object(struct zs_pool *pool, void *handle)
+void *zs_map_object(struct zs_pool *pool, unsigned long handle,
+			enum zs_mapmode mm)
 {
 	struct page *page;
 	unsigned long obj_idx, off;
@@ -693,26 +770,20 @@ void *zs_map_object(struct zs_pool *pool, void *handle)
 	if (off + class->size <= PAGE_SIZE) {
 		/* this object is contained entirely within a page */
 		area->vm_addr = kmap_atomic(page);
-	} else {
-		/* this object spans two pages */
-		struct page *nextp;
-
-		nextp = get_next_page(page);
-		BUG_ON(!nextp);
-
-
-		set_pte(area->vm_ptes[0], mk_pte(page, PAGE_KERNEL));
-		set_pte(area->vm_ptes[1], mk_pte(nextp, PAGE_KERNEL));
-
-		/* We pre-allocated VM area so mapping can never fail */
-		area->vm_addr = area->vm->addr;
+		return area->vm_addr + off;
 	}
 
-	return area->vm_addr + off;
+	/* disable page faults to match kmap_atomic() return conditions */
+	pagefault_disable();
+
+	if (mm != ZS_MM_WO)
+		zs_copy_map_object(area->vm_buf, page, off, class->size);
+	area->vm_addr = NULL;
+	return area->vm_buf;
 }
 EXPORT_SYMBOL_GPL(zs_map_object);
 
-void zs_unmap_object(struct zs_pool *pool, void *handle)
+void zs_unmap_object(struct zs_pool *pool, unsigned long handle)
 {
 	struct page *page;
 	unsigned long obj_idx, off;
@@ -722,6 +793,17 @@ void zs_unmap_object(struct zs_pool *pool, void *handle)
 	struct size_class *class;
 	struct mapping_area *area;
 
+	area = &__get_cpu_var(zs_map_area);
+	/* single-page object fastpath */
+	if (area->vm_addr) {
+		kunmap_atomic(area->vm_addr);
+		goto out;
+	}
+
+	/* no write fastpath */
+	if (area->vm_mm == ZS_MM_RO)
+		goto pfenable;
+
 	BUG_ON(!handle);
 
 	obj_handle_to_location(handle, &page, &obj_idx);
@@ -729,15 +811,12 @@ void zs_unmap_object(struct zs_pool *pool, void *handle)
 	class = &pool->size_class[class_idx];
 	off = obj_idx_to_offset(page, obj_idx, class->size);
 
-	area = &__get_cpu_var(zs_map_area);
-	if (off + class->size <= PAGE_SIZE) {
-		kunmap_atomic(area->vm_addr);
-	} else {
-		set_pte(area->vm_ptes[0], __pte(0));
-		set_pte(area->vm_ptes[1], __pte(0));
-		__flush_tlb_one((unsigned long)area->vm_addr);
-		__flush_tlb_one((unsigned long)area->vm_addr + PAGE_SIZE);
-	}
+	zs_copy_unmap_object(area->vm_buf, page, off, class->size);
+
+pfenable:
+	/* enable page faults to match kunmap_atomic() return conditions */
+	pagefault_enable();
+out:
 	put_cpu_var(zs_map_area);
 }
 EXPORT_SYMBOL_GPL(zs_unmap_object);
@@ -753,3 +832,9 @@ u64 zs_get_total_size_bytes(struct zs_pool *pool)
 	return npages << PAGE_SHIFT;
 }
 EXPORT_SYMBOL_GPL(zs_get_total_size_bytes);
+
+module_init(zs_init);
+module_exit(zs_exit);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Nitin Gupta <ngupta@vflare.org>");
diff --git a/drivers/staging/zsmalloc/zsmalloc.h b/drivers/staging/zsmalloc/zsmalloc.h
index 949384ee749..de2e8bfbcc0 100644
--- a/drivers/staging/zsmalloc/zsmalloc.h
+++ b/drivers/staging/zsmalloc/zsmalloc.h
@@ -15,16 +15,28 @@
 
 #include <linux/types.h>
 
+/*
+ * zsmalloc mapping modes
+ *
+ * NOTE: These only make a difference when a mapped object spans pages
+*/
+enum zs_mapmode {
+	ZS_MM_RW, /* normal read-write mapping */
+	ZS_MM_RO, /* read-only (no copy-out at unmap time) */
+	ZS_MM_WO /* write-only (no copy-in at map time) */
+};
+
 struct zs_pool;
 
 struct zs_pool *zs_create_pool(const char *name, gfp_t flags);
 void zs_destroy_pool(struct zs_pool *pool);
 
-void *zs_malloc(struct zs_pool *pool, size_t size);
-void zs_free(struct zs_pool *pool, void *obj);
+unsigned long zs_malloc(struct zs_pool *pool, size_t size);
+void zs_free(struct zs_pool *pool, unsigned long obj);
 
-void *zs_map_object(struct zs_pool *pool, void *handle);
-void zs_unmap_object(struct zs_pool *pool, void *handle);
+void *zs_map_object(struct zs_pool *pool, unsigned long handle,
+			enum zs_mapmode mm);
+void zs_unmap_object(struct zs_pool *pool, unsigned long handle);
 
 u64 zs_get_total_size_bytes(struct zs_pool *pool);
 
diff --git a/drivers/staging/zsmalloc/zsmalloc_int.h b/drivers/staging/zsmalloc/zsmalloc_int.h
index 6fd32a9e031..52805176773 100644
--- a/drivers/staging/zsmalloc/zsmalloc_int.h
+++ b/drivers/staging/zsmalloc/zsmalloc_int.h
@@ -110,9 +110,9 @@ enum fullness_group {
 static const int fullness_threshold_frac = 4;
 
 struct mapping_area {
-	struct vm_struct *vm;
-	pte_t *vm_ptes[2];
-	char *vm_addr;
+	char *vm_buf; /* copy buffer for objects that span pages */
+	char *vm_addr; /* address of kmap_atomic()'ed pages */
+	enum zs_mapmode vm_mm; /* mapping mode */
 };
 
 struct size_class {
diff --git a/drivers/target/Makefile b/drivers/target/Makefile
index 61648d84fbb..9fdcb561422 100644
--- a/drivers/target/Makefile
+++ b/drivers/target/Makefile
@@ -9,7 +9,8 @@ target_core_mod-y		:= target_core_configfs.o \
 				   target_core_tmr.o \
 				   target_core_tpg.o \
 				   target_core_transport.o \
-				   target_core_cdb.o \
+				   target_core_sbc.o \
+				   target_core_spc.o \
 				   target_core_ua.o \
 				   target_core_rd.o \
 				   target_core_stat.o
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index d57d10cb2e4..97c0f78c3c9 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -429,19 +429,8 @@ int iscsit_reset_np_thread(
 
 int iscsit_del_np_comm(struct iscsi_np *np)
 {
-	if (!np->np_socket)
-		return 0;
-
-	/*
-	 * Some network transports allocate their own struct sock->file,
-	 * see  if we need to free any additional allocated resources.
-	 */
-	if (np->np_flags & NPF_SCTP_STRUCT_FILE) {
-		kfree(np->np_socket->file);
-		np->np_socket->file = NULL;
-	}
-
-	sock_release(np->np_socket);
+	if (np->np_socket)
+		sock_release(np->np_socket);
 	return 0;
 }
 
@@ -1413,8 +1402,10 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 		spin_unlock_bh(&cmd->istate_lock);
 
 		iscsit_stop_dataout_timer(cmd);
-		return (!ooo_cmdsn) ? transport_generic_handle_data(
-					&cmd->se_cmd) : 0;
+		if (ooo_cmdsn)
+			return 0;
+		target_execute_cmd(&cmd->se_cmd);
+		return 0;
 	} else /* DATAOUT_CANNOT_RECOVER */
 		return -1;
 
@@ -2683,7 +2674,7 @@ static int iscsit_send_logout_response(
 		 */
 		logout_conn = iscsit_get_conn_from_cid_rcfr(sess,
 				cmd->logout_cid);
-		if ((logout_conn)) {
+		if (logout_conn) {
 			iscsit_connection_reinstatement_rcfr(logout_conn);
 			iscsit_dec_conn_usage_count(logout_conn);
 		}
@@ -4077,13 +4068,8 @@ int iscsit_close_connection(
 	kfree(conn->conn_ops);
 	conn->conn_ops = NULL;
 
-	if (conn->sock) {
-		if (conn->conn_flags & CONNFLAG_SCTP_STRUCT_FILE) {
-			kfree(conn->sock->file);
-			conn->sock->file = NULL;
-		}
+	if (conn->sock)
 		sock_release(conn->sock);
-	}
 	conn->thread_set = NULL;
 
 	pr_debug("Moving to TARG_CONN_STATE_FREE.\n");
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index 69dc8e35c03..a7b25e783b5 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -47,28 +47,6 @@ struct lio_target_configfs_attribute {
 	ssize_t (*store)(void *, const char *, size_t);
 };
 
-struct iscsi_portal_group *lio_get_tpg_from_tpg_item(
-	struct config_item *item,
-	struct iscsi_tiqn **tiqn_out)
-{
-	struct se_portal_group *se_tpg = container_of(to_config_group(item),
-					struct se_portal_group, tpg_group);
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-	int ret;
-
-	if (!tpg) {
-		pr_err("Unable to locate struct iscsi_portal_group "
-			"pointer\n");
-		return NULL;
-	}
-	ret = iscsit_get_tpg(tpg);
-	if (ret < 0)
-		return NULL;
-
-	*tiqn_out = tpg->tpg_tiqn;
-	return tpg;
-}
-
 /* Start items for lio_target_portal_cit */
 
 static ssize_t lio_target_np_show_sctp(
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 1c70144cdaf..8a908b28d8b 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -224,7 +224,6 @@ enum iscsi_timer_flags_table {
 /* Used for struct iscsi_np->np_flags */
 enum np_flags_table {
 	NPF_IP_NETWORK		= 0x00,
-	NPF_SCTP_STRUCT_FILE	= 0x01 /* Bugfix */
 };
 
 /* Used for struct iscsi_np->np_thread_state */
@@ -481,6 +480,7 @@ struct iscsi_tmr_req {
 	bool			task_reassign:1;
 	u32			ref_cmd_sn;
 	u32			exp_data_sn;
+	struct iscsi_cmd	*ref_cmd;
 	struct iscsi_conn_recovery *conn_recovery;
 	struct se_tmr_req	*se_tmr_req;
 };
@@ -503,7 +503,6 @@ struct iscsi_conn {
 	u16			local_port;
 	int			net_size;
 	u32			auth_id;
-#define CONNFLAG_SCTP_STRUCT_FILE			0x01
 	u32			conn_flags;
 	/* Used for iscsi_tx_login_rsp() */
 	u32			login_itt;
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index ecdd46deedd..3df8a2cef86 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -965,8 +965,8 @@ int iscsit_execute_cmd(struct iscsi_cmd *cmd, int ooo)
 		if (cmd->immediate_data) {
 			if (cmd->cmd_flags & ICF_GOT_LAST_DATAOUT) {
 				spin_unlock_bh(&cmd->istate_lock);
-				return transport_generic_handle_data(
-						&cmd->se_cmd);
+				target_execute_cmd(&cmd->se_cmd);
+				return 0;
 			}
 			spin_unlock_bh(&cmd->istate_lock);
 
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index a3656c9903a..0694d9b1bce 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -518,7 +518,7 @@ int iscsi_login_post_auth_non_zero_tsih(
 	 * initiator and release the new connection.
 	 */
 	conn_ptr = iscsit_get_conn_from_cid_rcfr(sess, cid);
-	if ((conn_ptr)) {
+	if (conn_ptr) {
 		pr_err("Connection exists with CID %hu for %s,"
 			" performing connection reinstatement.\n",
 			conn_ptr->cid, sess->sess_ops->InitiatorName);
@@ -539,7 +539,7 @@ int iscsi_login_post_auth_non_zero_tsih(
 	if (sess->sess_ops->ErrorRecoveryLevel == 2) {
 		cr = iscsit_get_inactive_connection_recovery_entry(
 				sess, cid);
-		if ((cr)) {
+		if (cr) {
 			pr_debug("Performing implicit logout"
 				" for connection recovery on CID: %hu\n",
 					conn->cid);
@@ -795,22 +795,6 @@ int iscsi_target_setup_login_socket(
 	}
 	np->np_socket = sock;
 	/*
-	 * The SCTP stack needs struct socket->file.
-	 */
-	if ((np->np_network_transport == ISCSI_SCTP_TCP) ||
-	    (np->np_network_transport == ISCSI_SCTP_UDP)) {
-		if (!sock->file) {
-			sock->file = kzalloc(sizeof(struct file), GFP_KERNEL);
-			if (!sock->file) {
-				pr_err("Unable to allocate struct"
-						" file for SCTP\n");
-				ret = -ENOMEM;
-				goto fail;
-			}
-			np->np_flags |= NPF_SCTP_STRUCT_FILE;
-		}
-	}
-	/*
 	 * Setup the np->np_sockaddr from the passed sockaddr setup
 	 * in iscsi_target_configfs.c code..
 	 */
@@ -869,21 +853,15 @@ int iscsi_target_setup_login_socket(
 
 fail:
 	np->np_socket = NULL;
-	if (sock) {
-		if (np->np_flags & NPF_SCTP_STRUCT_FILE) {
-			kfree(sock->file);
-			sock->file = NULL;
-		}
-
+	if (sock)
 		sock_release(sock);
-	}
 	return ret;
 }
 
 static int __iscsi_target_login_thread(struct iscsi_np *np)
 {
 	u8 buffer[ISCSI_HDR_LEN], iscsi_opcode, zero_tsih = 0;
-	int err, ret = 0, set_sctp_conn_flag, stop;
+	int err, ret = 0, stop;
 	struct iscsi_conn *conn = NULL;
 	struct iscsi_login *login;
 	struct iscsi_portal_group *tpg = NULL;
@@ -894,7 +872,6 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	struct sockaddr_in6 sock_in6;
 
 	flush_signals(current);
-	set_sctp_conn_flag = 0;
 	sock = np->np_socket;
 
 	spin_lock_bh(&np->np_thread_lock);
@@ -917,35 +894,12 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 		spin_unlock_bh(&np->np_thread_lock);
 		goto out;
 	}
-	/*
-	 * The SCTP stack needs struct socket->file.
-	 */
-	if ((np->np_network_transport == ISCSI_SCTP_TCP) ||
-	    (np->np_network_transport == ISCSI_SCTP_UDP)) {
-		if (!new_sock->file) {
-			new_sock->file = kzalloc(
-					sizeof(struct file), GFP_KERNEL);
-			if (!new_sock->file) {
-				pr_err("Unable to allocate struct"
-						" file for SCTP\n");
-				sock_release(new_sock);
-				/* Get another socket */
-				return 1;
-			}
-			set_sctp_conn_flag = 1;
-		}
-	}
-
 	iscsi_start_login_thread_timer(np);
 
 	conn = kzalloc(sizeof(struct iscsi_conn), GFP_KERNEL);
 	if (!conn) {
 		pr_err("Could not allocate memory for"
 			" new connection\n");
-		if (set_sctp_conn_flag) {
-			kfree(new_sock->file);
-			new_sock->file = NULL;
-		}
 		sock_release(new_sock);
 		/* Get another socket */
 		return 1;
@@ -955,9 +909,6 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	conn->conn_state = TARG_CONN_STATE_FREE;
 	conn->sock = new_sock;
 
-	if (set_sctp_conn_flag)
-		conn->conn_flags |= CONNFLAG_SCTP_STRUCT_FILE;
-
 	pr_debug("Moving to TARG_CONN_STATE_XPT_UP.\n");
 	conn->conn_state = TARG_CONN_STATE_XPT_UP;
 
@@ -1081,7 +1032,7 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 		goto new_sess_out;
 
 	zero_tsih = (pdu->tsih == 0x0000);
-	if ((zero_tsih)) {
+	if (zero_tsih) {
 		/*
 		 * This is the leading connection of a new session.
 		 * We wait until after authentication to check for
@@ -1205,13 +1156,8 @@ old_sess_out:
 		iscsi_release_param_list(conn->param_list);
 		conn->param_list = NULL;
 	}
-	if (conn->sock) {
-		if (conn->conn_flags & CONNFLAG_SCTP_STRUCT_FILE) {
-			kfree(conn->sock->file);
-			conn->sock->file = NULL;
-		}
+	if (conn->sock)
 		sock_release(conn->sock);
-	}
 	kfree(conn);
 
 	if (tpg) {
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index ed5241e7f12..0c4760fabfc 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -681,7 +681,7 @@ int iscsi_update_param_value(struct iscsi_param *param, char *value)
 	param->value = kzalloc(strlen(value) + 1, GFP_KERNEL);
 	if (!param->value) {
 		pr_err("Unable to allocate memory for value.\n");
-		return -1;
+		return -ENOMEM;
 	}
 
 	memcpy(param->value, value, strlen(value));
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index f4e640b51fd..f62fe123d90 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -19,6 +19,7 @@
  ******************************************************************************/
 
 #include <asm/unaligned.h>
+#include <scsi/scsi_device.h>
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
@@ -61,7 +62,7 @@ u8 iscsit_tmr_abort_task(
 	}
 
 	se_tmr->ref_task_tag		= hdr->rtt;
-	se_tmr->ref_cmd			= &ref_cmd->se_cmd;
+	tmr_req->ref_cmd		= ref_cmd;
 	tmr_req->ref_cmd_sn		= hdr->refcmdsn;
 	tmr_req->exp_data_sn		= hdr->exp_datasn;
 
@@ -121,7 +122,7 @@ u8 iscsit_tmr_task_reassign(
 	struct iscsi_tmr_req *tmr_req = cmd->tmr_req;
 	struct se_tmr_req *se_tmr = cmd->se_cmd.se_tmr_req;
 	struct iscsi_tm *hdr = (struct iscsi_tm *) buf;
-	int ret;
+	int ret, ref_lun;
 
 	pr_debug("Got TASK_REASSIGN TMR ITT: 0x%08x,"
 		" RefTaskTag: 0x%08x, ExpDataSN: 0x%08x, CID: %hu\n",
@@ -155,9 +156,16 @@ u8 iscsit_tmr_task_reassign(
 		return ISCSI_TMF_RSP_REJECTED;
 	}
 
+	ref_lun = scsilun_to_int(&hdr->lun);
+	if (ref_lun != ref_cmd->se_cmd.orig_fe_lun) {
+		pr_err("Unable to perform connection recovery for"
+			" differing ref_lun: %d ref_cmd orig_fe_lun: %u\n",
+			ref_lun, ref_cmd->se_cmd.orig_fe_lun);
+		return ISCSI_TMF_RSP_REJECTED;
+	}
+
 	se_tmr->ref_task_tag		= hdr->rtt;
-	se_tmr->ref_cmd			= &ref_cmd->se_cmd;
-	se_tmr->ref_task_lun		= get_unaligned_le64(&hdr->lun);
+	tmr_req->ref_cmd		= ref_cmd;
 	tmr_req->ref_cmd_sn		= hdr->refcmdsn;
 	tmr_req->exp_data_sn		= hdr->exp_datasn;
 	tmr_req->conn_recovery		= cr;
@@ -191,9 +199,7 @@ static int iscsit_task_reassign_complete_nop_out(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
-	struct se_tmr_req *se_tmr = tmr_req->se_tmr_req;
-	struct se_cmd *se_cmd = se_tmr->ref_cmd;
-	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	struct iscsi_cmd *cmd = tmr_req->ref_cmd;
 	struct iscsi_conn_recovery *cr;
 
 	if (!cmd->cr) {
@@ -251,7 +257,8 @@ static int iscsit_task_reassign_complete_write(
 			pr_debug("WRITE ITT: 0x%08x: t_state: %d"
 				" never sent to transport\n",
 				cmd->init_task_tag, cmd->se_cmd.t_state);
-			return transport_generic_handle_data(se_cmd);
+			target_execute_cmd(se_cmd);
+			return 0;
 		}
 
 		cmd->i_state = ISTATE_SEND_STATUS;
@@ -360,9 +367,7 @@ static int iscsit_task_reassign_complete_scsi_cmnd(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
-	struct se_tmr_req *se_tmr = tmr_req->se_tmr_req;
-	struct se_cmd *se_cmd = se_tmr->ref_cmd;
-	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	struct iscsi_cmd *cmd = tmr_req->ref_cmd;
 	struct iscsi_conn_recovery *cr;
 
 	if (!cmd->cr) {
@@ -385,7 +390,7 @@ static int iscsit_task_reassign_complete_scsi_cmnd(
 	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
-	if (se_cmd->se_cmd_flags & SCF_SENT_CHECK_CONDITION) {
+	if (cmd->se_cmd.se_cmd_flags & SCF_SENT_CHECK_CONDITION) {
 		cmd->i_state = ISTATE_SEND_STATUS;
 		iscsit_add_cmd_to_response_queue(cmd, conn, cmd->i_state);
 		return 0;
@@ -411,17 +416,14 @@ static int iscsit_task_reassign_complete(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
-	struct se_tmr_req *se_tmr = tmr_req->se_tmr_req;
-	struct se_cmd *se_cmd;
 	struct iscsi_cmd *cmd;
 	int ret = 0;
 
-	if (!se_tmr->ref_cmd) {
+	if (!tmr_req->ref_cmd) {
 		pr_err("TMR Request is missing a RefCmd struct iscsi_cmd.\n");
 		return -1;
 	}
-	se_cmd = se_tmr->ref_cmd;
-	cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	cmd = tmr_req->ref_cmd;
 
 	cmd->conn = conn;
 
@@ -547,9 +549,7 @@ int iscsit_task_reassign_prepare_write(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
-	struct se_tmr_req *se_tmr = tmr_req->se_tmr_req;
-	struct se_cmd *se_cmd = se_tmr->ref_cmd;
-	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	struct iscsi_cmd *cmd = tmr_req->ref_cmd;
 	struct iscsi_pdu *pdu = NULL;
 	struct iscsi_r2t *r2t = NULL, *r2t_tmp;
 	int first_incomplete_r2t = 1, i = 0;
@@ -782,14 +782,12 @@ int iscsit_check_task_reassign_expdatasn(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
-	struct se_tmr_req *se_tmr = tmr_req->se_tmr_req;
-	struct se_cmd *se_cmd = se_tmr->ref_cmd;
-	struct iscsi_cmd *ref_cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
+	struct iscsi_cmd *ref_cmd = tmr_req->ref_cmd;
 
 	if (ref_cmd->iscsi_opcode != ISCSI_OP_SCSI_CMD)
 		return 0;
 
-	if (se_cmd->se_cmd_flags & SCF_SENT_CHECK_CONDITION)
+	if (ref_cmd->se_cmd.se_cmd_flags & SCF_SENT_CHECK_CONDITION)
 		return 0;
 
 	if (ref_cmd->data_direction == DMA_NONE)
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index 879d8d0fa3f..a38a3f8ab0d 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -303,6 +303,7 @@ int iscsit_tpg_enable_portal_group(struct iscsi_portal_group *tpg)
 {
 	struct iscsi_param *param;
 	struct iscsi_tiqn *tiqn = tpg->tpg_tiqn;
+	int ret;
 
 	spin_lock(&tpg->tpg_state_lock);
 	if (tpg->tpg_state == TPG_STATE_ACTIVE) {
@@ -319,19 +320,19 @@ int iscsit_tpg_enable_portal_group(struct iscsi_portal_group *tpg)
 	param = iscsi_find_param_from_key(AUTHMETHOD, tpg->param_list);
 	if (!param) {
 		spin_unlock(&tpg->tpg_state_lock);
-		return -ENOMEM;
+		return -EINVAL;
 	}
 
 	if (ISCSI_TPG_ATTRIB(tpg)->authentication) {
-		if (!strcmp(param->value, NONE))
-			if (iscsi_update_param_value(param, CHAP) < 0) {
-				spin_unlock(&tpg->tpg_state_lock);
-				return -ENOMEM;
-			}
-		if (iscsit_ta_authentication(tpg, 1) < 0) {
-			spin_unlock(&tpg->tpg_state_lock);
-			return -ENOMEM;
+		if (!strcmp(param->value, NONE)) {
+			ret = iscsi_update_param_value(param, CHAP);
+			if (ret)
+				goto err;
 		}
+
+		ret = iscsit_ta_authentication(tpg, 1);
+		if (ret < 0)
+			goto err;
 	}
 
 	tpg->tpg_state = TPG_STATE_ACTIVE;
@@ -344,6 +345,10 @@ int iscsit_tpg_enable_portal_group(struct iscsi_portal_group *tpg)
 	spin_unlock(&tiqn->tiqn_tpg_lock);
 
 	return 0;
+
+err:
+	spin_unlock(&tpg->tpg_state_lock);
+	return ret;
 }
 
 int iscsit_tpg_disable_portal_group(struct iscsi_portal_group *tpg, int force)
@@ -558,7 +563,7 @@ int iscsit_ta_authentication(struct iscsi_portal_group *tpg, u32 authentication)
 	if ((authentication != 1) && (authentication != 0)) {
 		pr_err("Illegal value for authentication parameter:"
 			" %u, ignoring request.\n", authentication);
-		return -1;
+		return -EINVAL;
 	}
 
 	memset(buf1, 0, sizeof(buf1));
@@ -593,7 +598,7 @@ int iscsit_ta_authentication(struct iscsi_portal_group *tpg, u32 authentication)
 	} else {
 		snprintf(buf1, sizeof(buf1), "%s", param->value);
 		none = strstr(buf1, NONE);
-		if ((none))
+		if (none)
 			goto out;
 		strncat(buf1, ",", strlen(","));
 		strncat(buf1, NONE, strlen(NONE));
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 38dfac2b0a1..5491c632a15 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -211,12 +211,11 @@ static void tcm_loop_submission_work(struct work_struct *work)
 	/*
 	 * Because some userspace code via scsi-generic do not memset their
 	 * associated read buffers, go ahead and do that here for type
-	 * SCF_SCSI_CONTROL_SG_IO_CDB.  Also note that this is currently
-	 * guaranteed to be a single SGL for SCF_SCSI_CONTROL_SG_IO_CDB
-	 * by target core in target_setup_cmd_from_cdb() ->
-	 * transport_generic_cmd_sequencer().
+	 * non-data CDBs.  Also note that this is currently guaranteed to be a
+	 * single SGL for this case by target core in
+	 * target_setup_cmd_from_cdb() -> transport_generic_cmd_sequencer().
 	 */
-	if (se_cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB &&
+	if (!(se_cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) &&
 	    se_cmd->data_direction == DMA_FROM_DEVICE) {
 		struct scatterlist *sg = scsi_sglist(sc);
 		unsigned char *buf = kmap(sg_page(sg)) + sg->offset;
@@ -779,7 +778,7 @@ static int tcm_loop_write_pending(struct se_cmd *se_cmd)
 	 * We now tell TCM to add this WRITE CDB directly into the TCM storage
 	 * object execution queue.
 	 */
-	transport_generic_process_write(se_cmd);
+	target_execute_cmd(se_cmd);
 	return 0;
 }
 
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
index 7e6136e2ce8..39ddba584b3 100644
--- a/drivers/target/sbp/sbp_target.c
+++ b/drivers/target/sbp/sbp_target.c
@@ -1219,28 +1219,14 @@ static void sbp_handle_command(struct sbp_target_request *req)
 	ret = sbp_fetch_command(req);
 	if (ret) {
 		pr_debug("sbp_handle_command: fetch command failed: %d\n", ret);
-		req->status.status |= cpu_to_be32(
-			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
-			STATUS_BLOCK_DEAD(0) |
-			STATUS_BLOCK_LEN(1) |
-			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
-		sbp_send_status(req);
-		sbp_free_request(req);
-		return;
+		goto err;
 	}
 
 	ret = sbp_fetch_page_table(req);
 	if (ret) {
 		pr_debug("sbp_handle_command: fetch page table failed: %d\n",
 			ret);
-		req->status.status |= cpu_to_be32(
-			STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
-			STATUS_BLOCK_DEAD(0) |
-			STATUS_BLOCK_LEN(1) |
-			STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
-		sbp_send_status(req);
-		sbp_free_request(req);
-		return;
+		goto err;
 	}
 
 	unpacked_lun = req->login->lun->unpacked_lun;
@@ -1249,9 +1235,21 @@ static void sbp_handle_command(struct sbp_target_request *req)
 	pr_debug("sbp_handle_command ORB:0x%llx unpacked_lun:%d data_len:%d data_dir:%d\n",
 			req->orb_pointer, unpacked_lun, data_length, data_dir);
 
-	target_submit_cmd(&req->se_cmd, sess->se_sess, req->cmd_buf,
-			req->sense_buf, unpacked_lun, data_length,
-			MSG_SIMPLE_TAG, data_dir, 0);
+	if (target_submit_cmd(&req->se_cmd, sess->se_sess, req->cmd_buf,
+			      req->sense_buf, unpacked_lun, data_length,
+			      MSG_SIMPLE_TAG, data_dir, 0))
+		goto err;
+
+	return;
+
+err:
+	req->status.status |= cpu_to_be32(
+		STATUS_BLOCK_RESP(STATUS_RESP_TRANSPORT_FAILURE) |
+		STATUS_BLOCK_DEAD(0) |
+		STATUS_BLOCK_LEN(1) |
+		STATUS_BLOCK_SBP_STATUS(SBP_STATUS_UNSPECIFIED_ERROR));
+	sbp_send_status(req);
+	sbp_free_request(req);
 }
 
 /*
@@ -1784,8 +1782,7 @@ static int sbp_write_pending(struct se_cmd *se_cmd)
 		return ret;
 	}
 
-	transport_generic_process_write(se_cmd);
-
+	target_execute_cmd(se_cmd);
 	return 0;
 }
 
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 5ad972856a8..cf2c66f3c11 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -300,8 +300,8 @@ int core_free_device_list_for_node(
 		lun = deve->se_lun;
 
 		spin_unlock_irq(&nacl->device_list_lock);
-		core_update_device_list_for_node(lun, NULL, deve->mapped_lun,
-			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg, 0);
+		core_disable_device_list_for_node(lun, NULL, deve->mapped_lun,
+			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg);
 		spin_lock_irq(&nacl->device_list_lock);
 	}
 	spin_unlock_irq(&nacl->device_list_lock);
@@ -342,72 +342,46 @@ void core_update_device_list_access(
 	spin_unlock_irq(&nacl->device_list_lock);
 }
 
-/*      core_update_device_list_for_node():
+/*      core_enable_device_list_for_node():
  *
  *
  */
-int core_update_device_list_for_node(
+int core_enable_device_list_for_node(
 	struct se_lun *lun,
 	struct se_lun_acl *lun_acl,
 	u32 mapped_lun,
 	u32 lun_access,
 	struct se_node_acl *nacl,
-	struct se_portal_group *tpg,
-	int enable)
+	struct se_portal_group *tpg)
 {
 	struct se_port *port = lun->lun_sep;
-	struct se_dev_entry *deve = nacl->device_list[mapped_lun];
-	int trans = 0;
-	/*
-	 * If the MappedLUN entry is being disabled, the entry in
-	 * port->sep_alua_list must be removed now before clearing the
-	 * struct se_dev_entry pointers below as logic in
-	 * core_alua_do_transition_tg_pt() depends on these being present.
-	 */
-	if (!enable) {
-		/*
-		 * deve->se_lun_acl will be NULL for demo-mode created LUNs
-		 * that have not been explicitly concerted to MappedLUNs ->
-		 * struct se_lun_acl, but we remove deve->alua_port_list from
-		 * port->sep_alua_list. This also means that active UAs and
-		 * NodeACL context specific PR metadata for demo-mode
-		 * MappedLUN *deve will be released below..
-		 */
-		spin_lock_bh(&port->sep_alua_lock);
-		list_del(&deve->alua_port_list);
-		spin_unlock_bh(&port->sep_alua_lock);
-	}
+	struct se_dev_entry *deve;
 
 	spin_lock_irq(&nacl->device_list_lock);
-	if (enable) {
-		/*
-		 * Check if the call is handling demo mode -> explict LUN ACL
-		 * transition.  This transition must be for the same struct se_lun
-		 * + mapped_lun that was setup in demo mode..
-		 */
-		if (deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS) {
-			if (deve->se_lun_acl != NULL) {
-				pr_err("struct se_dev_entry->se_lun_acl"
-					" already set for demo mode -> explict"
-					" LUN ACL transition\n");
-				spin_unlock_irq(&nacl->device_list_lock);
-				return -EINVAL;
-			}
-			if (deve->se_lun != lun) {
-				pr_err("struct se_dev_entry->se_lun does"
-					" match passed struct se_lun for demo mode"
-					" -> explict LUN ACL transition\n");
-				spin_unlock_irq(&nacl->device_list_lock);
-				return -EINVAL;
-			}
-			deve->se_lun_acl = lun_acl;
-			trans = 1;
-		} else {
-			deve->se_lun = lun;
-			deve->se_lun_acl = lun_acl;
-			deve->mapped_lun = mapped_lun;
-			deve->lun_flags |= TRANSPORT_LUNFLAGS_INITIATOR_ACCESS;
+
+	deve = nacl->device_list[mapped_lun];
+
+	/*
+	 * Check if the call is handling demo mode -> explict LUN ACL
+	 * transition.  This transition must be for the same struct se_lun
+	 * + mapped_lun that was setup in demo mode..
+	 */
+	if (deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS) {
+		if (deve->se_lun_acl != NULL) {
+			pr_err("struct se_dev_entry->se_lun_acl"
+			       " already set for demo mode -> explict"
+			       " LUN ACL transition\n");
+			spin_unlock_irq(&nacl->device_list_lock);
+			return -EINVAL;
 		}
+		if (deve->se_lun != lun) {
+			pr_err("struct se_dev_entry->se_lun does"
+			       " match passed struct se_lun for demo mode"
+			       " -> explict LUN ACL transition\n");
+			spin_unlock_irq(&nacl->device_list_lock);
+			return -EINVAL;
+		}
+		deve->se_lun_acl = lun_acl;
 
 		if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) {
 			deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_ONLY;
@@ -417,27 +391,72 @@ int core_update_device_list_for_node(
 			deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
 		}
 
-		if (trans) {
-			spin_unlock_irq(&nacl->device_list_lock);
-			return 0;
-		}
-		deve->creation_time = get_jiffies_64();
-		deve->attach_count++;
 		spin_unlock_irq(&nacl->device_list_lock);
+		return 0;
+	}
 
-		spin_lock_bh(&port->sep_alua_lock);
-		list_add_tail(&deve->alua_port_list, &port->sep_alua_list);
-		spin_unlock_bh(&port->sep_alua_lock);
+	deve->se_lun = lun;
+	deve->se_lun_acl = lun_acl;
+	deve->mapped_lun = mapped_lun;
+	deve->lun_flags |= TRANSPORT_LUNFLAGS_INITIATOR_ACCESS;
 
-		return 0;
+	if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) {
+		deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_ONLY;
+		deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_WRITE;
+	} else {
+		deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_WRITE;
+		deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
 	}
+
+	deve->creation_time = get_jiffies_64();
+	deve->attach_count++;
+	spin_unlock_irq(&nacl->device_list_lock);
+
+	spin_lock_bh(&port->sep_alua_lock);
+	list_add_tail(&deve->alua_port_list, &port->sep_alua_list);
+	spin_unlock_bh(&port->sep_alua_lock);
+
+	return 0;
+}
+
+/*      core_disable_device_list_for_node():
+ *
+ *
+ */
+int core_disable_device_list_for_node(
+	struct se_lun *lun,
+	struct se_lun_acl *lun_acl,
+	u32 mapped_lun,
+	u32 lun_access,
+	struct se_node_acl *nacl,
+	struct se_portal_group *tpg)
+{
+	struct se_port *port = lun->lun_sep;
+	struct se_dev_entry *deve = nacl->device_list[mapped_lun];
+
+	/*
+	 * If the MappedLUN entry is being disabled, the entry in
+	 * port->sep_alua_list must be removed now before clearing the
+	 * struct se_dev_entry pointers below as logic in
+	 * core_alua_do_transition_tg_pt() depends on these being present.
+	 *
+	 * deve->se_lun_acl will be NULL for demo-mode created LUNs
+	 * that have not been explicitly converted to MappedLUNs ->
+	 * struct se_lun_acl, but we remove deve->alua_port_list from
+	 * port->sep_alua_list. This also means that active UAs and
+	 * NodeACL context specific PR metadata for demo-mode
+	 * MappedLUN *deve will be released below..
+	 */
+	spin_lock_bh(&port->sep_alua_lock);
+	list_del(&deve->alua_port_list);
+	spin_unlock_bh(&port->sep_alua_lock);
 	/*
 	 * Wait for any in process SPEC_I_PT=1 or REGISTER_AND_MOVE
 	 * PR operation to complete.
 	 */
-	spin_unlock_irq(&nacl->device_list_lock);
 	while (atomic_read(&deve->pr_ref_count) != 0)
 		cpu_relax();
+
 	spin_lock_irq(&nacl->device_list_lock);
 	/*
 	 * Disable struct se_dev_entry LUN ACL mapping
@@ -475,9 +494,9 @@ void core_clear_lun_from_tpg(struct se_lun *lun, struct se_portal_group *tpg)
 				continue;
 			spin_unlock_irq(&nacl->device_list_lock);
 
-			core_update_device_list_for_node(lun, NULL,
+			core_disable_device_list_for_node(lun, NULL,
 				deve->mapped_lun, TRANSPORT_LUNFLAGS_NO_ACCESS,
-				nacl, tpg, 0);
+				nacl, tpg);
 
 			spin_lock_irq(&nacl->device_list_lock);
 		}
@@ -715,7 +734,7 @@ void se_release_device_for_hba(struct se_device *dev)
 		se_dev_stop(dev);
 
 	if (dev->dev_ptr) {
-		kthread_stop(dev->process_thread);
+		destroy_workqueue(dev->tmr_wq);
 		if (dev->transport->free_device)
 			dev->transport->free_device(dev->dev_ptr);
 	}
@@ -822,7 +841,7 @@ int se_dev_check_shutdown(struct se_device *dev)
 	return ret;
 }
 
-u32 se_dev_align_max_sectors(u32 max_sectors, u32 block_size)
+static u32 se_dev_align_max_sectors(u32 max_sectors, u32 block_size)
 {
 	u32 tmp, aligned_max_sectors;
 	/*
@@ -1273,7 +1292,6 @@ int se_dev_set_block_size(struct se_device *dev, u32 block_size)
 
 struct se_lun *core_dev_add_lun(
 	struct se_portal_group *tpg,
-	struct se_hba *hba,
 	struct se_device *dev,
 	u32 lun)
 {
@@ -1298,7 +1316,7 @@ struct se_lun *core_dev_add_lun(
 	pr_debug("%s_TPG[%u]_LUN[%u] - Activated %s Logical Unit from"
 		" CORE HBA: %u\n", tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun_p->unpacked_lun,
-		tpg->se_tpg_tfo->get_fabric_name(), hba->hba_id);
+		tpg->se_tpg_tfo->get_fabric_name(), dev->se_hba->hba_id);
 	/*
 	 * Update LUN maps for dynamically added initiators when
 	 * generate_node_acl is enabled.
@@ -1470,8 +1488,8 @@ int core_dev_add_initiator_node_lun_acl(
 
 	lacl->se_lun = lun;
 
-	if (core_update_device_list_for_node(lun, lacl, lacl->mapped_lun,
-			lun_access, nacl, tpg, 1) < 0)
+	if (core_enable_device_list_for_node(lun, lacl, lacl->mapped_lun,
+			lun_access, nacl, tpg) < 0)
 		return -EINVAL;
 
 	spin_lock(&lun->lun_acl_lock);
@@ -1514,8 +1532,8 @@ int core_dev_del_initiator_node_lun_acl(
 	smp_mb__after_atomic_dec();
 	spin_unlock(&lun->lun_acl_lock);
 
-	core_update_device_list_for_node(lun, NULL, lacl->mapped_lun,
-		TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg, 0);
+	core_disable_device_list_for_node(lun, NULL, lacl->mapped_lun,
+		TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg);
 
 	lacl->se_lun = NULL;
 
diff --git a/drivers/target/target_core_fabric_configfs.c b/drivers/target/target_core_fabric_configfs.c
index 405cc98eaed..ea479e54f5f 100644
--- a/drivers/target/target_core_fabric_configfs.c
+++ b/drivers/target/target_core_fabric_configfs.c
@@ -764,8 +764,7 @@ static int target_fabric_port_link(
 		goto out;
 	}
 
-	lun_p = core_dev_add_lun(se_tpg, dev->se_hba, dev,
-				lun->unpacked_lun);
+	lun_p = core_dev_add_lun(se_tpg, dev, lun->unpacked_lun);
 	if (IS_ERR(lun_p)) {
 		pr_err("core_dev_add_lun() failed\n");
 		ret = PTR_ERR(lun_p);
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 9f99d040490..cbb5aaf3e56 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -109,46 +109,29 @@ static struct se_device *fd_create_virtdevice(
 	struct se_subsystem_dev *se_dev,
 	void *p)
 {
-	char *dev_p = NULL;
 	struct se_device *dev;
 	struct se_dev_limits dev_limits;
 	struct queue_limits *limits;
 	struct fd_dev *fd_dev = p;
 	struct fd_host *fd_host = hba->hba_ptr;
-	mm_segment_t old_fs;
 	struct file *file;
 	struct inode *inode = NULL;
 	int dev_flags = 0, flags, ret = -EINVAL;
 
 	memset(&dev_limits, 0, sizeof(struct se_dev_limits));
 
-	old_fs = get_fs();
-	set_fs(get_ds());
-	dev_p = getname(fd_dev->fd_dev_name);
-	set_fs(old_fs);
-
-	if (IS_ERR(dev_p)) {
-		pr_err("getname(%s) failed: %lu\n",
-			fd_dev->fd_dev_name, IS_ERR(dev_p));
-		ret = PTR_ERR(dev_p);
-		goto fail;
-	}
 	/*
 	 * Use O_DSYNC by default instead of O_SYNC to forgo syncing
 	 * of pure timestamp updates.
 	 */
 	flags = O_RDWR | O_CREAT | O_LARGEFILE | O_DSYNC;
 
-	file = filp_open(dev_p, flags, 0600);
+	file = filp_open(fd_dev->fd_dev_name, flags, 0600);
 	if (IS_ERR(file)) {
-		pr_err("filp_open(%s) failed\n", dev_p);
+		pr_err("filp_open(%s) failed\n", fd_dev->fd_dev_name);
 		ret = PTR_ERR(file);
 		goto fail;
 	}
-	if (!file || !file->f_dentry) {
-		pr_err("filp_open(%s) failed\n", dev_p);
-		goto fail;
-	}
 	fd_dev->fd_file = file;
 	/*
 	 * If using a block backend with this struct file, we extract
@@ -212,14 +195,12 @@ static struct se_device *fd_create_virtdevice(
 		" %llu total bytes\n", fd_host->fd_host_id, fd_dev->fd_dev_id,
 			fd_dev->fd_dev_name, fd_dev->fd_dev_size);
 
-	putname(dev_p);
 	return dev;
 fail:
 	if (fd_dev->fd_file) {
 		filp_close(fd_dev->fd_file, NULL);
 		fd_dev->fd_file = NULL;
 	}
-	putname(dev_p);
 	return ERR_PTR(ret);
 }
 
@@ -331,7 +312,7 @@ static int fd_do_writev(struct se_cmd *cmd, struct scatterlist *sgl,
 	return 1;
 }
 
-static void fd_emulate_sync_cache(struct se_cmd *cmd)
+static int fd_execute_sync_cache(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct fd_dev *fd_dev = dev->dev_ptr;
@@ -365,7 +346,7 @@ static void fd_emulate_sync_cache(struct se_cmd *cmd)
 		pr_err("FILEIO: vfs_fsync_range() failed: %d\n", ret);
 
 	if (immed)
-		return;
+		return 0;
 
 	if (ret) {
 		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
@@ -373,11 +354,15 @@ static void fd_emulate_sync_cache(struct se_cmd *cmd)
 	} else {
 		target_complete_cmd(cmd, SAM_STAT_GOOD);
 	}
+
+	return 0;
 }
 
-static int fd_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
-		u32 sgl_nents, enum dma_data_direction data_direction)
+static int fd_execute_rw(struct se_cmd *cmd)
 {
+	struct scatterlist *sgl = cmd->t_data_sg;
+	u32 sgl_nents = cmd->t_data_nents;
+	enum dma_data_direction data_direction = cmd->data_direction;
 	struct se_device *dev = cmd->se_dev;
 	int ret = 0;
 
@@ -448,14 +433,11 @@ static ssize_t fd_set_configfs_dev_params(
 		token = match_token(ptr, tokens, args);
 		switch (token) {
 		case Opt_fd_dev_name:
-			arg_p = match_strdup(&args[0]);
-			if (!arg_p) {
-				ret = -ENOMEM;
+			if (match_strlcpy(fd_dev->fd_dev_name, &args[0],
+				FD_MAX_DEV_NAME) == 0) {
+				ret = -EINVAL;
 				break;
 			}
-			snprintf(fd_dev->fd_dev_name, FD_MAX_DEV_NAME,
-					"%s", arg_p);
-			kfree(arg_p);
 			pr_debug("FILEIO: Referencing Path: %s\n",
 					fd_dev->fd_dev_name);
 			fd_dev->fbd_flags |= FBDF_HAS_PATH;
@@ -550,6 +532,16 @@ static sector_t fd_get_blocks(struct se_device *dev)
 	return div_u64(dev_size, dev->se_sub_dev->se_dev_attrib.block_size);
 }
 
+static struct spc_ops fd_spc_ops = {
+	.execute_rw		= fd_execute_rw,
+	.execute_sync_cache	= fd_execute_sync_cache,
+};
+
+static int fd_parse_cdb(struct se_cmd *cmd)
+{
+	return sbc_parse_cdb(cmd, &fd_spc_ops);
+}
+
 static struct se_subsystem_api fileio_template = {
 	.name			= "fileio",
 	.owner			= THIS_MODULE,
@@ -561,8 +553,7 @@ static struct se_subsystem_api fileio_template = {
 	.allocate_virtdevice	= fd_allocate_virtdevice,
 	.create_virtdevice	= fd_create_virtdevice,
 	.free_device		= fd_free_device,
-	.execute_cmd		= fd_execute_cmd,
-	.do_sync_cache		= fd_emulate_sync_cache,
+	.parse_cdb		= fd_parse_cdb,
 	.check_configfs_dev_params = fd_check_configfs_dev_params,
 	.set_configfs_dev_params = fd_set_configfs_dev_params,
 	.show_configfs_dev_params = fd_show_configfs_dev_params,
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index fd47950727b..76db75e836e 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -40,6 +40,7 @@
 #include <linux/module.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
+#include <asm/unaligned.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
@@ -96,6 +97,7 @@ static struct se_device *iblock_create_virtdevice(
 	struct request_queue *q;
 	struct queue_limits *limits;
 	u32 dev_flags = 0;
+	fmode_t mode;
 	int ret = -EINVAL;
 
 	if (!ib_dev) {
@@ -117,8 +119,11 @@ static struct se_device *iblock_create_virtdevice(
 	pr_debug( "IBLOCK: Claiming struct block_device: %s\n",
 			ib_dev->ibd_udev_path);
 
-	bd = blkdev_get_by_path(ib_dev->ibd_udev_path,
-				FMODE_WRITE|FMODE_READ|FMODE_EXCL, ib_dev);
+	mode = FMODE_READ|FMODE_EXCL;
+	if (!ib_dev->ibd_readonly)
+		mode |= FMODE_WRITE;
+
+	bd = blkdev_get_by_path(ib_dev->ibd_udev_path, mode, ib_dev);
 	if (IS_ERR(bd)) {
 		ret = PTR_ERR(bd);
 		goto failed;
@@ -292,7 +297,7 @@ static void iblock_end_io_flush(struct bio *bio, int err)
  * Implement SYCHRONIZE CACHE.  Note that we can't handle lba ranges and must
  * always flush the whole cache.
  */
-static void iblock_emulate_sync_cache(struct se_cmd *cmd)
+static int iblock_execute_sync_cache(struct se_cmd *cmd)
 {
 	struct iblock_dev *ib_dev = cmd->se_dev->dev_ptr;
 	int immed = (cmd->t_task_cdb[1] & 0x2);
@@ -311,23 +316,98 @@ static void iblock_emulate_sync_cache(struct se_cmd *cmd)
 	if (!immed)
 		bio->bi_private = cmd;
 	submit_bio(WRITE_FLUSH, bio);
+	return 0;
 }
 
-static int iblock_do_discard(struct se_device *dev, sector_t lba, u32 range)
+static int iblock_execute_unmap(struct se_cmd *cmd)
 {
+	struct se_device *dev = cmd->se_dev;
 	struct iblock_dev *ibd = dev->dev_ptr;
-	struct block_device *bd = ibd->ibd_bd;
-	int barrier = 0;
+	unsigned char *buf, *ptr = NULL;
+	sector_t lba;
+	int size = cmd->data_length;
+	u32 range;
+	int ret = 0;
+	int dl, bd_dl;
+
+	buf = transport_kmap_data_sg(cmd);
+
+	dl = get_unaligned_be16(&buf[0]);
+	bd_dl = get_unaligned_be16(&buf[2]);
+
+	size = min(size - 8, bd_dl);
+	if (size / 16 > dev->se_sub_dev->se_dev_attrib.max_unmap_block_desc_count) {
+		cmd->scsi_sense_reason = TCM_INVALID_PARAMETER_LIST;
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* First UNMAP block descriptor starts at 8 byte offset */
+	ptr = &buf[8];
+	pr_debug("UNMAP: Sub: %s Using dl: %u bd_dl: %u size: %u"
+		" ptr: %p\n", dev->transport->name, dl, bd_dl, size, ptr);
+
+	while (size >= 16) {
+		lba = get_unaligned_be64(&ptr[0]);
+		range = get_unaligned_be32(&ptr[8]);
+		pr_debug("UNMAP: Using lba: %llu and range: %u\n",
+				 (unsigned long long)lba, range);
+
+		if (range > dev->se_sub_dev->se_dev_attrib.max_unmap_lba_count) {
+			cmd->scsi_sense_reason = TCM_INVALID_PARAMETER_LIST;
+			ret = -EINVAL;
+			goto err;
+		}
+
+		if (lba + range > dev->transport->get_blocks(dev) + 1) {
+			cmd->scsi_sense_reason = TCM_ADDRESS_OUT_OF_RANGE;
+			ret = -EINVAL;
+			goto err;
+		}
 
-	return blkdev_issue_discard(bd, lba, range, GFP_KERNEL, barrier);
+		ret = blkdev_issue_discard(ibd->ibd_bd, lba, range,
+					   GFP_KERNEL, 0);
+		if (ret < 0) {
+			pr_err("blkdev_issue_discard() failed: %d\n",
+					ret);
+			goto err;
+		}
+
+		ptr += 16;
+		size -= 16;
+	}
+
+err:
+	transport_kunmap_data_sg(cmd);
+	if (!ret)
+		target_complete_cmd(cmd, GOOD);
+	return ret;
+}
+
+static int iblock_execute_write_same(struct se_cmd *cmd)
+{
+	struct iblock_dev *ibd = cmd->se_dev->dev_ptr;
+	int ret;
+
+	ret = blkdev_issue_discard(ibd->ibd_bd, cmd->t_task_lba,
+				   spc_get_write_same_sectors(cmd), GFP_KERNEL,
+				   0);
+	if (ret < 0) {
+		pr_debug("blkdev_issue_discard() failed for WRITE_SAME\n");
+		return ret;
+	}
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
 }
 
 enum {
-	Opt_udev_path, Opt_force, Opt_err
+	Opt_udev_path, Opt_readonly, Opt_force, Opt_err
 };
 
 static match_table_t tokens = {
 	{Opt_udev_path, "udev_path=%s"},
+	{Opt_readonly, "readonly=%d"},
 	{Opt_force, "force=%d"},
 	{Opt_err, NULL}
 };
@@ -340,6 +420,7 @@ static ssize_t iblock_set_configfs_dev_params(struct se_hba *hba,
 	char *orig, *ptr, *arg_p, *opts;
 	substring_t args[MAX_OPT_ARGS];
 	int ret = 0, token;
+	unsigned long tmp_readonly;
 
 	opts = kstrdup(page, GFP_KERNEL);
 	if (!opts)
@@ -372,6 +453,22 @@ static ssize_t iblock_set_configfs_dev_params(struct se_hba *hba,
 					ib_dev->ibd_udev_path);
 			ib_dev->ibd_flags |= IBDF_HAS_UDEV_PATH;
 			break;
+		case Opt_readonly:
+			arg_p = match_strdup(&args[0]);
+			if (!arg_p) {
+				ret = -ENOMEM;
+				break;
+			}
+			ret = strict_strtoul(arg_p, 0, &tmp_readonly);
+			kfree(arg_p);
+			if (ret < 0) {
+				pr_err("strict_strtoul() failed for"
+						" readonly=\n");
+				goto out;
+			}
+			ib_dev->ibd_readonly = tmp_readonly;
+			pr_debug("IBLOCK: readonly: %d\n", ib_dev->ibd_readonly);
+			break;
 		case Opt_force:
 			break;
 		default:
@@ -411,11 +508,10 @@ static ssize_t iblock_show_configfs_dev_params(
 	if (bd)
 		bl += sprintf(b + bl, "iBlock device: %s",
 				bdevname(bd, buf));
-	if (ibd->ibd_flags & IBDF_HAS_UDEV_PATH) {
-		bl += sprintf(b + bl, "  UDEV PATH: %s\n",
+	if (ibd->ibd_flags & IBDF_HAS_UDEV_PATH)
+		bl += sprintf(b + bl, "  UDEV PATH: %s",
 				ibd->ibd_udev_path);
-	} else
-		bl += sprintf(b + bl, "\n");
+	bl += sprintf(b + bl, "  readonly: %d\n", ibd->ibd_readonly);
 
 	bl += sprintf(b + bl, "        ");
 	if (bd) {
@@ -493,9 +589,11 @@ static void iblock_submit_bios(struct bio_list *list, int rw)
 	blk_finish_plug(&plug);
 }
 
-static int iblock_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
-		u32 sgl_nents, enum dma_data_direction data_direction)
+static int iblock_execute_rw(struct se_cmd *cmd)
 {
+	struct scatterlist *sgl = cmd->t_data_sg;
+	u32 sgl_nents = cmd->t_data_nents;
+	enum dma_data_direction data_direction = cmd->data_direction;
 	struct se_device *dev = cmd->se_dev;
 	struct iblock_req *ibr;
 	struct bio *bio;
@@ -642,6 +740,18 @@ static void iblock_bio_done(struct bio *bio, int err)
 	iblock_complete_cmd(cmd);
 }
 
+static struct spc_ops iblock_spc_ops = {
+	.execute_rw		= iblock_execute_rw,
+	.execute_sync_cache	= iblock_execute_sync_cache,
+	.execute_write_same	= iblock_execute_write_same,
+	.execute_unmap		= iblock_execute_unmap,
+};
+
+static int iblock_parse_cdb(struct se_cmd *cmd)
+{
+	return sbc_parse_cdb(cmd, &iblock_spc_ops);
+}
+
 static struct se_subsystem_api iblock_template = {
 	.name			= "iblock",
 	.owner			= THIS_MODULE,
@@ -653,9 +763,7 @@ static struct se_subsystem_api iblock_template = {
 	.allocate_virtdevice	= iblock_allocate_virtdevice,
 	.create_virtdevice	= iblock_create_virtdevice,
 	.free_device		= iblock_free_device,
-	.execute_cmd		= iblock_execute_cmd,
-	.do_discard		= iblock_do_discard,
-	.do_sync_cache		= iblock_emulate_sync_cache,
+	.parse_cdb		= iblock_parse_cdb,
 	.check_configfs_dev_params = iblock_check_configfs_dev_params,
 	.set_configfs_dev_params = iblock_set_configfs_dev_params,
 	.show_configfs_dev_params = iblock_show_configfs_dev_params,
diff --git a/drivers/target/target_core_iblock.h b/drivers/target/target_core_iblock.h
index 66cf7b9e205..533627ae79e 100644
--- a/drivers/target/target_core_iblock.h
+++ b/drivers/target/target_core_iblock.h
@@ -18,6 +18,7 @@ struct iblock_dev {
 	u32	ibd_flags;
 	struct bio_set	*ibd_bio_set;
 	struct block_device *ibd_bd;
+	bool ibd_readonly;
 } ____cacheline_aligned;
 
 #endif /* TARGET_CORE_IBLOCK_H */
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index 165e8242968..0fd428225d1 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -4,25 +4,16 @@
 /* target_core_alua.c */
 extern struct t10_alua_lu_gp *default_lu_gp;
 
-/* target_core_cdb.c */
-int	target_emulate_inquiry(struct se_cmd *cmd);
-int	target_emulate_readcapacity(struct se_cmd *cmd);
-int	target_emulate_readcapacity_16(struct se_cmd *cmd);
-int	target_emulate_modesense(struct se_cmd *cmd);
-int	target_emulate_request_sense(struct se_cmd *cmd);
-int	target_emulate_unmap(struct se_cmd *cmd);
-int	target_emulate_write_same(struct se_cmd *cmd);
-int	target_emulate_synchronize_cache(struct se_cmd *cmd);
-int	target_emulate_noop(struct se_cmd *cmd);
-
 /* target_core_device.c */
 struct se_dev_entry *core_get_se_deve_from_rtpi(struct se_node_acl *, u16);
 int	core_free_device_list_for_node(struct se_node_acl *,
 		struct se_portal_group *);
 void	core_dec_lacl_count(struct se_node_acl *, struct se_cmd *);
 void	core_update_device_list_access(u32, u32, struct se_node_acl *);
-int	core_update_device_list_for_node(struct se_lun *, struct se_lun_acl *,
-		u32, u32, struct se_node_acl *, struct se_portal_group *, int);
+int	core_enable_device_list_for_node(struct se_lun *, struct se_lun_acl *,
+		u32, u32, struct se_node_acl *, struct se_portal_group *);
+int	core_disable_device_list_for_node(struct se_lun *, struct se_lun_acl *,
+		u32, u32, struct se_node_acl *, struct se_portal_group *);
 void	core_clear_lun_from_tpg(struct se_lun *, struct se_portal_group *);
 int	core_dev_export(struct se_device *, struct se_portal_group *,
 		struct se_lun *);
@@ -56,8 +47,7 @@ int	se_dev_set_max_sectors(struct se_device *, u32);
 int	se_dev_set_fabric_max_sectors(struct se_device *, u32);
 int	se_dev_set_optimal_sectors(struct se_device *, u32);
 int	se_dev_set_block_size(struct se_device *, u32);
-struct se_lun *core_dev_add_lun(struct se_portal_group *, struct se_hba *,
-		struct se_device *, u32);
+struct se_lun *core_dev_add_lun(struct se_portal_group *, struct se_device *, u32);
 int	core_dev_del_lun(struct se_portal_group *, u32);
 struct se_lun *core_get_lun_from_tpg(struct se_portal_group *, u32);
 struct se_lun_acl *core_dev_init_initiator_node_lun_acl(struct se_portal_group *,
@@ -104,7 +94,6 @@ void	release_se_kmem_caches(void);
 u32	scsi_get_new_index(scsi_index_t);
 void	transport_subsystem_check_init(void);
 void	transport_cmd_finish_abort(struct se_cmd *, int);
-void	__target_remove_from_execute_list(struct se_cmd *);
 unsigned char *transport_dump_cmd_direction(struct se_cmd *);
 void	transport_dump_dev_state(struct se_device *, char *, int *);
 void	transport_dump_dev_info(struct se_device *, struct se_lun *,
@@ -116,6 +105,7 @@ int	transport_dump_vpd_ident(struct t10_vpd *, unsigned char *, int);
 bool	target_stop_cmd(struct se_cmd *cmd, unsigned long *flags);
 int	transport_clear_lun_from_sessions(struct se_lun *);
 void	transport_send_task_abort(struct se_cmd *);
+int	target_cmd_size_check(struct se_cmd *cmd, unsigned int size);
 
 /* target_core_stat.c */
 void	target_stat_setup_dev_default_groups(struct se_subsystem_dev *);
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 85564998500..1e946502c37 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -507,7 +507,7 @@ static int core_scsi3_pr_seq_non_holder(
 	 * Check if write exclusive initiator ports *NOT* holding the
 	 * WRITE_EXCLUSIVE_* reservation.
 	 */
-	if ((we) && !(registered_nexus)) {
+	if (we && !registered_nexus) {
 		if (cmd->data_direction == DMA_TO_DEVICE) {
 			/*
 			 * Conflict for write exclusive
@@ -2031,7 +2031,7 @@ static int __core_scsi3_write_aptpl_to_file(
 	if (IS_ERR(file) || !file || !file->f_dentry) {
 		pr_err("filp_open(%s) for APTPL metadata"
 			" failed\n", path);
-		return (PTR_ERR(file) < 0 ? PTR_ERR(file) : -ENOENT);
+		return IS_ERR(file) ? PTR_ERR(file) : -ENOENT;
 	}
 
 	iov[0].iov_base = &buf[0];
@@ -2486,7 +2486,7 @@ static int core_scsi3_pro_reserve(
 	 */
 	spin_lock(&dev->dev_reservation_lock);
 	pr_res_holder = dev->dev_pr_res_holder;
-	if ((pr_res_holder)) {
+	if (pr_res_holder) {
 		/*
 		 * From spc4r17 Section 5.7.9: Reserving:
 		 *
@@ -3818,7 +3818,7 @@ int target_scsi3_emulate_pr_out(struct se_cmd *cmd)
 			" SPC-2 reservation is held, returning"
 			" RESERVATION_CONFLICT\n");
 		cmd->scsi_sense_reason = TCM_RESERVATION_CONFLICT;
-		ret = EINVAL;
+		ret = -EINVAL;
 		goto out;
 	}
 
@@ -3828,7 +3828,8 @@ int target_scsi3_emulate_pr_out(struct se_cmd *cmd)
 	 */
 	if (!cmd->se_sess) {
 		cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	if (cmd->data_length < 24) {
@@ -4029,7 +4030,7 @@ static int core_scsi3_pri_read_reservation(struct se_cmd *cmd)
 
 	spin_lock(&se_dev->dev_reservation_lock);
 	pr_reg = se_dev->dev_pr_res_holder;
-	if ((pr_reg)) {
+	if (pr_reg) {
 		/*
 		 * Set the hardcoded Additional Length
 		 */
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 4ce2cf642fc..6e32ff6f2fa 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -35,8 +35,10 @@
 #include <linux/spinlock.h>
 #include <linux/genhd.h>
 #include <linux/cdrom.h>
-#include <linux/file.h>
+#include <linux/ratelimit.h>
 #include <linux/module.h>
+#include <asm/unaligned.h>
+
 #include <scsi/scsi.h>
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_cmnd.h>
@@ -46,12 +48,14 @@
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
 
+#include "target_core_alua.h"
 #include "target_core_pscsi.h"
 
 #define ISPRINT(a)  ((a >= ' ') && (a <= '~'))
 
 static struct se_subsystem_api pscsi_template;
 
+static int pscsi_execute_cmd(struct se_cmd *cmd);
 static void pscsi_req_done(struct request *, int);
 
 /*	pscsi_attach_hba():
@@ -1019,9 +1023,79 @@ fail:
 	return -ENOMEM;
 }
 
-static int pscsi_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
-		u32 sgl_nents, enum dma_data_direction data_direction)
+/*
+ * Clear a lun set in the cdb if the initiator talking to use spoke
+ * and old standards version, as we can't assume the underlying device
+ * won't choke up on it.
+ */
+static inline void pscsi_clear_cdb_lun(unsigned char *cdb)
+{
+	switch (cdb[0]) {
+	case READ_10: /* SBC - RDProtect */
+	case READ_12: /* SBC - RDProtect */
+	case READ_16: /* SBC - RDProtect */
+	case SEND_DIAGNOSTIC: /* SPC - SELF-TEST Code */
+	case VERIFY: /* SBC - VRProtect */
+	case VERIFY_16: /* SBC - VRProtect */
+	case WRITE_VERIFY: /* SBC - VRProtect */
+	case WRITE_VERIFY_12: /* SBC - VRProtect */
+	case MAINTENANCE_IN: /* SPC - Parameter Data Format for SA RTPG */
+		break;
+	default:
+		cdb[1] &= 0x1f; /* clear logical unit number */
+		break;
+	}
+}
+
+static int pscsi_parse_cdb(struct se_cmd *cmd)
+{
+	unsigned char *cdb = cmd->t_task_cdb;
+	unsigned int dummy_size;
+	int ret;
+
+	if (cmd->se_cmd_flags & SCF_BIDI) {
+		cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
+		return -EINVAL;
+	}
+
+	pscsi_clear_cdb_lun(cdb);
+
+	/*
+	 * For REPORT LUNS we always need to emulate the response, for everything
+	 * else the default for pSCSI is to pass the command to the underlying
+	 * LLD / physical hardware.
+	 */
+	switch (cdb[0]) {
+	case REPORT_LUNS:
+		ret = spc_parse_cdb(cmd, &dummy_size);
+		if (ret)
+			return ret;
+		break;
+	case READ_6:
+	case READ_10:
+	case READ_12:
+	case READ_16:
+	case WRITE_6:
+	case WRITE_10:
+	case WRITE_12:
+	case WRITE_16:
+	case WRITE_VERIFY:
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		/* FALLTHROUGH*/
+	default:
+		cmd->execute_cmd = pscsi_execute_cmd;
+		break;
+	}
+
+	return 0;
+}
+
+static int pscsi_execute_cmd(struct se_cmd *cmd)
 {
+	struct scatterlist *sgl = cmd->t_data_sg;
+	u32 sgl_nents = cmd->t_data_nents;
+	enum dma_data_direction data_direction = cmd->data_direction;
 	struct pscsi_dev_virt *pdv = cmd->se_dev->dev_ptr;
 	struct pscsi_plugin_task *pt;
 	struct request *req;
@@ -1042,7 +1116,7 @@ static int pscsi_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
 	memcpy(pt->pscsi_cdb, cmd->t_task_cdb,
 		scsi_command_size(cmd->t_task_cdb));
 
-	if (cmd->se_cmd_flags & SCF_SCSI_NON_DATA_CDB) {
+	if (!sgl) {
 		req = blk_get_request(pdv->pdv_sd->request_queue,
 				(data_direction == DMA_TO_DEVICE),
 				GFP_KERNEL);
@@ -1188,7 +1262,7 @@ static struct se_subsystem_api pscsi_template = {
 	.create_virtdevice	= pscsi_create_virtdevice,
 	.free_device		= pscsi_free_device,
 	.transport_complete	= pscsi_transport_complete,
-	.execute_cmd		= pscsi_execute_cmd,
+	.parse_cdb		= pscsi_parse_cdb,
 	.check_configfs_dev_params = pscsi_check_configfs_dev_params,
 	.set_configfs_dev_params = pscsi_set_configfs_dev_params,
 	.show_configfs_dev_params = pscsi_show_configfs_dev_params,
diff --git a/drivers/target/target_core_rd.c b/drivers/target/target_core_rd.c
index d0ceb873c0e..d00bbe33ff8 100644
--- a/drivers/target/target_core_rd.c
+++ b/drivers/target/target_core_rd.c
@@ -284,9 +284,11 @@ static struct rd_dev_sg_table *rd_get_sg_table(struct rd_dev *rd_dev, u32 page)
 	return NULL;
 }
 
-static int rd_execute_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
-		u32 sgl_nents, enum dma_data_direction data_direction)
+static int rd_execute_rw(struct se_cmd *cmd)
 {
+	struct scatterlist *sgl = cmd->t_data_sg;
+	u32 sgl_nents = cmd->t_data_nents;
+	enum dma_data_direction data_direction = cmd->data_direction;
 	struct se_device *se_dev = cmd->se_dev;
 	struct rd_dev *dev = se_dev->dev_ptr;
 	struct rd_dev_sg_table *table;
@@ -460,6 +462,15 @@ static sector_t rd_get_blocks(struct se_device *dev)
 	return blocks_long;
 }
 
+static struct spc_ops rd_spc_ops = {
+	.execute_rw		= rd_execute_rw,
+};
+
+static int rd_parse_cdb(struct se_cmd *cmd)
+{
+	return sbc_parse_cdb(cmd, &rd_spc_ops);
+}
+
 static struct se_subsystem_api rd_mcp_template = {
 	.name			= "rd_mcp",
 	.transport_type		= TRANSPORT_PLUGIN_VHBA_VDEV,
@@ -468,7 +479,7 @@ static struct se_subsystem_api rd_mcp_template = {
 	.allocate_virtdevice	= rd_allocate_virtdevice,
 	.create_virtdevice	= rd_create_virtdevice,
 	.free_device		= rd_free_device,
-	.execute_cmd		= rd_execute_cmd,
+	.parse_cdb		= rd_parse_cdb,
 	.check_configfs_dev_params = rd_check_configfs_dev_params,
 	.set_configfs_dev_params = rd_set_configfs_dev_params,
 	.show_configfs_dev_params = rd_show_configfs_dev_params,
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
new file mode 100644
index 00000000000..a9dd9469e3b
--- /dev/null
+++ b/drivers/target/target_core_sbc.c
@@ -0,0 +1,581 @@
+/*
+ * SCSI Block Commands (SBC) parsing and emulation.
+ *
+ * Copyright (c) 2002, 2003, 2004, 2005 PyX Technologies, Inc.
+ * Copyright (c) 2005, 2006, 2007 SBE, Inc.
+ * Copyright (c) 2007-2010 Rising Tide Systems
+ * Copyright (c) 2008-2010 Linux-iSCSI.org
+ *
+ * Nicholas A. Bellinger <nab@kernel.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/ratelimit.h>
+#include <asm/unaligned.h>
+#include <scsi/scsi.h>
+
+#include <target/target_core_base.h>
+#include <target/target_core_backend.h>
+#include <target/target_core_fabric.h>
+
+#include "target_core_internal.h"
+#include "target_core_ua.h"
+
+
+static int sbc_emulate_readcapacity(struct se_cmd *cmd)
+{
+	struct se_device *dev = cmd->se_dev;
+	unsigned char *buf;
+	unsigned long long blocks_long = dev->transport->get_blocks(dev);
+	u32 blocks;
+
+	if (blocks_long >= 0x00000000ffffffff)
+		blocks = 0xffffffff;
+	else
+		blocks = (u32)blocks_long;
+
+	buf = transport_kmap_data_sg(cmd);
+
+	buf[0] = (blocks >> 24) & 0xff;
+	buf[1] = (blocks >> 16) & 0xff;
+	buf[2] = (blocks >> 8) & 0xff;
+	buf[3] = blocks & 0xff;
+	buf[4] = (dev->se_sub_dev->se_dev_attrib.block_size >> 24) & 0xff;
+	buf[5] = (dev->se_sub_dev->se_dev_attrib.block_size >> 16) & 0xff;
+	buf[6] = (dev->se_sub_dev->se_dev_attrib.block_size >> 8) & 0xff;
+	buf[7] = dev->se_sub_dev->se_dev_attrib.block_size & 0xff;
+
+	transport_kunmap_data_sg(cmd);
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+static int sbc_emulate_readcapacity_16(struct se_cmd *cmd)
+{
+	struct se_device *dev = cmd->se_dev;
+	unsigned char *buf;
+	unsigned long long blocks = dev->transport->get_blocks(dev);
+
+	buf = transport_kmap_data_sg(cmd);
+
+	buf[0] = (blocks >> 56) & 0xff;
+	buf[1] = (blocks >> 48) & 0xff;
+	buf[2] = (blocks >> 40) & 0xff;
+	buf[3] = (blocks >> 32) & 0xff;
+	buf[4] = (blocks >> 24) & 0xff;
+	buf[5] = (blocks >> 16) & 0xff;
+	buf[6] = (blocks >> 8) & 0xff;
+	buf[7] = blocks & 0xff;
+	buf[8] = (dev->se_sub_dev->se_dev_attrib.block_size >> 24) & 0xff;
+	buf[9] = (dev->se_sub_dev->se_dev_attrib.block_size >> 16) & 0xff;
+	buf[10] = (dev->se_sub_dev->se_dev_attrib.block_size >> 8) & 0xff;
+	buf[11] = dev->se_sub_dev->se_dev_attrib.block_size & 0xff;
+	/*
+	 * Set Thin Provisioning Enable bit following sbc3r22 in section
+	 * READ CAPACITY (16) byte 14 if emulate_tpu or emulate_tpws is enabled.
+	 */
+	if (dev->se_sub_dev->se_dev_attrib.emulate_tpu || dev->se_sub_dev->se_dev_attrib.emulate_tpws)
+		buf[14] = 0x80;
+
+	transport_kunmap_data_sg(cmd);
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+int spc_get_write_same_sectors(struct se_cmd *cmd)
+{
+	u32 num_blocks;
+
+	if (cmd->t_task_cdb[0] == WRITE_SAME)
+		num_blocks = get_unaligned_be16(&cmd->t_task_cdb[7]);
+	else if (cmd->t_task_cdb[0] == WRITE_SAME_16)
+		num_blocks = get_unaligned_be32(&cmd->t_task_cdb[10]);
+	else /* WRITE_SAME_32 via VARIABLE_LENGTH_CMD */
+		num_blocks = get_unaligned_be32(&cmd->t_task_cdb[28]);
+
+	/*
+	 * Use the explicit range when non zero is supplied, otherwise calculate
+	 * the remaining range based on ->get_blocks() - starting LBA.
+	 */
+	if (num_blocks)
+		return num_blocks;
+
+	return cmd->se_dev->transport->get_blocks(cmd->se_dev) -
+		cmd->t_task_lba + 1;
+}
+EXPORT_SYMBOL(spc_get_write_same_sectors);
+
+static int sbc_emulate_verify(struct se_cmd *cmd)
+{
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+static inline u32 sbc_get_size(struct se_cmd *cmd, u32 sectors)
+{
+	return cmd->se_dev->se_sub_dev->se_dev_attrib.block_size * sectors;
+}
+
+static int sbc_check_valid_sectors(struct se_cmd *cmd)
+{
+	struct se_device *dev = cmd->se_dev;
+	unsigned long long end_lba;
+	u32 sectors;
+
+	sectors = cmd->data_length / dev->se_sub_dev->se_dev_attrib.block_size;
+	end_lba = dev->transport->get_blocks(dev) + 1;
+
+	if (cmd->t_task_lba + sectors > end_lba) {
+		pr_err("target: lba %llu, sectors %u exceeds end lba %llu\n",
+			cmd->t_task_lba, sectors, end_lba);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static inline u32 transport_get_sectors_6(unsigned char *cdb)
+{
+	/*
+	 * Use 8-bit sector value.  SBC-3 says:
+	 *
+	 *   A TRANSFER LENGTH field set to zero specifies that 256
+	 *   logical blocks shall be written.  Any other value
+	 *   specifies the number of logical blocks that shall be
+	 *   written.
+	 */
+	return cdb[4] ? : 256;
+}
+
+static inline u32 transport_get_sectors_10(unsigned char *cdb)
+{
+	return (u32)(cdb[7] << 8) + cdb[8];
+}
+
+static inline u32 transport_get_sectors_12(unsigned char *cdb)
+{
+	return (u32)(cdb[6] << 24) + (cdb[7] << 16) + (cdb[8] << 8) + cdb[9];
+}
+
+static inline u32 transport_get_sectors_16(unsigned char *cdb)
+{
+	return (u32)(cdb[10] << 24) + (cdb[11] << 16) +
+		    (cdb[12] << 8) + cdb[13];
+}
+
+/*
+ * Used for VARIABLE_LENGTH_CDB WRITE_32 and READ_32 variants
+ */
+static inline u32 transport_get_sectors_32(unsigned char *cdb)
+{
+	return (u32)(cdb[28] << 24) + (cdb[29] << 16) +
+		    (cdb[30] << 8) + cdb[31];
+
+}
+
+static inline u32 transport_lba_21(unsigned char *cdb)
+{
+	return ((cdb[1] & 0x1f) << 16) | (cdb[2] << 8) | cdb[3];
+}
+
+static inline u32 transport_lba_32(unsigned char *cdb)
+{
+	return (cdb[2] << 24) | (cdb[3] << 16) | (cdb[4] << 8) | cdb[5];
+}
+
+static inline unsigned long long transport_lba_64(unsigned char *cdb)
+{
+	unsigned int __v1, __v2;
+
+	__v1 = (cdb[2] << 24) | (cdb[3] << 16) | (cdb[4] << 8) | cdb[5];
+	__v2 = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
+
+	return ((unsigned long long)__v2) | (unsigned long long)__v1 << 32;
+}
+
+/*
+ * For VARIABLE_LENGTH_CDB w/ 32 byte extended CDBs
+ */
+static inline unsigned long long transport_lba_64_ext(unsigned char *cdb)
+{
+	unsigned int __v1, __v2;
+
+	__v1 = (cdb[12] << 24) | (cdb[13] << 16) | (cdb[14] << 8) | cdb[15];
+	__v2 = (cdb[16] << 24) | (cdb[17] << 16) | (cdb[18] << 8) | cdb[19];
+
+	return ((unsigned long long)__v2) | (unsigned long long)__v1 << 32;
+}
+
+static int sbc_write_same_supported(struct se_device *dev,
+		unsigned char *flags)
+{
+	if ((flags[0] & 0x04) || (flags[0] & 0x02)) {
+		pr_err("WRITE_SAME PBDATA and LBDATA"
+			" bits not supported for Block Discard"
+			" Emulation\n");
+		return -ENOSYS;
+	}
+
+	/*
+	 * Currently for the emulated case we only accept
+	 * tpws with the UNMAP=1 bit set.
+	 */
+	if (!(flags[0] & 0x08)) {
+		pr_err("WRITE_SAME w/o UNMAP bit not"
+			" supported for Block Discard Emulation\n");
+		return -ENOSYS;
+	}
+
+	return 0;
+}
+
+static void xdreadwrite_callback(struct se_cmd *cmd)
+{
+	unsigned char *buf, *addr;
+	struct scatterlist *sg;
+	unsigned int offset;
+	int i;
+	int count;
+	/*
+	 * From sbc3r22.pdf section 5.48 XDWRITEREAD (10) command
+	 *
+	 * 1) read the specified logical block(s);
+	 * 2) transfer logical blocks from the data-out buffer;
+	 * 3) XOR the logical blocks transferred from the data-out buffer with
+	 *    the logical blocks read, storing the resulting XOR data in a buffer;
+	 * 4) if the DISABLE WRITE bit is set to zero, then write the logical
+	 *    blocks transferred from the data-out buffer; and
+	 * 5) transfer the resulting XOR data to the data-in buffer.
+	 */
+	buf = kmalloc(cmd->data_length, GFP_KERNEL);
+	if (!buf) {
+		pr_err("Unable to allocate xor_callback buf\n");
+		return;
+	}
+	/*
+	 * Copy the scatterlist WRITE buffer located at cmd->t_data_sg
+	 * into the locally allocated *buf
+	 */
+	sg_copy_to_buffer(cmd->t_data_sg,
+			  cmd->t_data_nents,
+			  buf,
+			  cmd->data_length);
+
+	/*
+	 * Now perform the XOR against the BIDI read memory located at
+	 * cmd->t_mem_bidi_list
+	 */
+
+	offset = 0;
+	for_each_sg(cmd->t_bidi_data_sg, sg, cmd->t_bidi_data_nents, count) {
+		addr = kmap_atomic(sg_page(sg));
+		if (!addr)
+			goto out;
+
+		for (i = 0; i < sg->length; i++)
+			*(addr + sg->offset + i) ^= *(buf + offset + i);
+
+		offset += sg->length;
+		kunmap_atomic(addr);
+	}
+
+out:
+	kfree(buf);
+}
+
+int sbc_parse_cdb(struct se_cmd *cmd, struct spc_ops *ops)
+{
+	struct se_subsystem_dev *su_dev = cmd->se_dev->se_sub_dev;
+	struct se_device *dev = cmd->se_dev;
+	unsigned char *cdb = cmd->t_task_cdb;
+	unsigned int size;
+	u32 sectors = 0;
+	int ret;
+
+	switch (cdb[0]) {
+	case READ_6:
+		sectors = transport_get_sectors_6(cdb);
+		cmd->t_task_lba = transport_lba_21(cdb);
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case READ_10:
+		sectors = transport_get_sectors_10(cdb);
+		cmd->t_task_lba = transport_lba_32(cdb);
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case READ_12:
+		sectors = transport_get_sectors_12(cdb);
+		cmd->t_task_lba = transport_lba_32(cdb);
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case READ_16:
+		sectors = transport_get_sectors_16(cdb);
+		cmd->t_task_lba = transport_lba_64(cdb);
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case WRITE_6:
+		sectors = transport_get_sectors_6(cdb);
+		cmd->t_task_lba = transport_lba_21(cdb);
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case WRITE_10:
+	case WRITE_VERIFY:
+		sectors = transport_get_sectors_10(cdb);
+		cmd->t_task_lba = transport_lba_32(cdb);
+		if (cdb[1] & 0x8)
+			cmd->se_cmd_flags |= SCF_FUA;
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case WRITE_12:
+		sectors = transport_get_sectors_12(cdb);
+		cmd->t_task_lba = transport_lba_32(cdb);
+		if (cdb[1] & 0x8)
+			cmd->se_cmd_flags |= SCF_FUA;
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case WRITE_16:
+		sectors = transport_get_sectors_16(cdb);
+		cmd->t_task_lba = transport_lba_64(cdb);
+		if (cdb[1] & 0x8)
+			cmd->se_cmd_flags |= SCF_FUA;
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+		cmd->execute_cmd = ops->execute_rw;
+		break;
+	case XDWRITEREAD_10:
+		if ((cmd->data_direction != DMA_TO_DEVICE) ||
+		    !(cmd->se_cmd_flags & SCF_BIDI))
+			goto out_invalid_cdb_field;
+		sectors = transport_get_sectors_10(cdb);
+
+		cmd->t_task_lba = transport_lba_32(cdb);
+		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+
+		/*
+		 * Setup BIDI XOR callback to be run after I/O completion.
+		 */
+		cmd->execute_cmd = ops->execute_rw;
+		cmd->transport_complete_callback = &xdreadwrite_callback;
+		if (cdb[1] & 0x8)
+			cmd->se_cmd_flags |= SCF_FUA;
+		break;
+	case VARIABLE_LENGTH_CMD:
+	{
+		u16 service_action = get_unaligned_be16(&cdb[8]);
+		switch (service_action) {
+		case XDWRITEREAD_32:
+			sectors = transport_get_sectors_32(cdb);
+
+			/*
+			 * Use WRITE_32 and READ_32 opcodes for the emulated
+			 * XDWRITE_READ_32 logic.
+			 */
+			cmd->t_task_lba = transport_lba_64_ext(cdb);
+			cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+
+			/*
+			 * Setup BIDI XOR callback to be run during after I/O
+			 * completion.
+			 */
+			cmd->execute_cmd = ops->execute_rw;
+			cmd->transport_complete_callback = &xdreadwrite_callback;
+			if (cdb[1] & 0x8)
+				cmd->se_cmd_flags |= SCF_FUA;
+			break;
+		case WRITE_SAME_32:
+			if (!ops->execute_write_same)
+				goto out_unsupported_cdb;
+
+			sectors = transport_get_sectors_32(cdb);
+			if (!sectors) {
+				pr_err("WSNZ=1, WRITE_SAME w/sectors=0 not"
+				       " supported\n");
+				goto out_invalid_cdb_field;
+			}
+
+			size = sbc_get_size(cmd, 1);
+			cmd->t_task_lba = get_unaligned_be64(&cdb[12]);
+
+			if (sbc_write_same_supported(dev, &cdb[10]) < 0)
+				goto out_unsupported_cdb;
+			cmd->execute_cmd = ops->execute_write_same;
+			break;
+		default:
+			pr_err("VARIABLE_LENGTH_CMD service action"
+				" 0x%04x not supported\n", service_action);
+			goto out_unsupported_cdb;
+		}
+		break;
+	}
+	case READ_CAPACITY:
+		size = READ_CAP_LEN;
+		cmd->execute_cmd = sbc_emulate_readcapacity;
+		break;
+	case SERVICE_ACTION_IN:
+		switch (cmd->t_task_cdb[1] & 0x1f) {
+		case SAI_READ_CAPACITY_16:
+			cmd->execute_cmd = sbc_emulate_readcapacity_16;
+			break;
+		default:
+			pr_err("Unsupported SA: 0x%02x\n",
+				cmd->t_task_cdb[1] & 0x1f);
+			goto out_invalid_cdb_field;
+		}
+		size = (cdb[10] << 24) | (cdb[11] << 16) |
+		       (cdb[12] << 8) | cdb[13];
+		break;
+	case SYNCHRONIZE_CACHE:
+	case SYNCHRONIZE_CACHE_16:
+		if (!ops->execute_sync_cache)
+			goto out_unsupported_cdb;
+
+		/*
+		 * Extract LBA and range to be flushed for emulated SYNCHRONIZE_CACHE
+		 */
+		if (cdb[0] == SYNCHRONIZE_CACHE) {
+			sectors = transport_get_sectors_10(cdb);
+			cmd->t_task_lba = transport_lba_32(cdb);
+		} else {
+			sectors = transport_get_sectors_16(cdb);
+			cmd->t_task_lba = transport_lba_64(cdb);
+		}
+
+		size = sbc_get_size(cmd, sectors);
+
+		/*
+		 * Check to ensure that LBA + Range does not exceed past end of
+		 * device for IBLOCK and FILEIO ->do_sync_cache() backend calls
+		 */
+		if (cmd->t_task_lba || sectors) {
+			if (sbc_check_valid_sectors(cmd) < 0)
+				goto out_invalid_cdb_field;
+		}
+		cmd->execute_cmd = ops->execute_sync_cache;
+		break;
+	case UNMAP:
+		if (!ops->execute_unmap)
+			goto out_unsupported_cdb;
+
+		size = get_unaligned_be16(&cdb[7]);
+		cmd->execute_cmd = ops->execute_unmap;
+		break;
+	case WRITE_SAME_16:
+		if (!ops->execute_write_same)
+			goto out_unsupported_cdb;
+
+		sectors = transport_get_sectors_16(cdb);
+		if (!sectors) {
+			pr_err("WSNZ=1, WRITE_SAME w/sectors=0 not supported\n");
+			goto out_invalid_cdb_field;
+		}
+
+		size = sbc_get_size(cmd, 1);
+		cmd->t_task_lba = get_unaligned_be64(&cdb[2]);
+
+		if (sbc_write_same_supported(dev, &cdb[1]) < 0)
+			goto out_unsupported_cdb;
+		cmd->execute_cmd = ops->execute_write_same;
+		break;
+	case WRITE_SAME:
+		if (!ops->execute_write_same)
+			goto out_unsupported_cdb;
+
+		sectors = transport_get_sectors_10(cdb);
+		if (!sectors) {
+			pr_err("WSNZ=1, WRITE_SAME w/sectors=0 not supported\n");
+			goto out_invalid_cdb_field;
+		}
+
+		size = sbc_get_size(cmd, 1);
+		cmd->t_task_lba = get_unaligned_be32(&cdb[2]);
+
+		/*
+		 * Follow sbcr26 with WRITE_SAME (10) and check for the existence
+		 * of byte 1 bit 3 UNMAP instead of original reserved field
+		 */
+		if (sbc_write_same_supported(dev, &cdb[1]) < 0)
+			goto out_unsupported_cdb;
+		cmd->execute_cmd = ops->execute_write_same;
+		break;
+	case VERIFY:
+		size = 0;
+		cmd->execute_cmd = sbc_emulate_verify;
+		break;
+	default:
+		ret = spc_parse_cdb(cmd, &size);
+		if (ret)
+			return ret;
+	}
+
+	/* reject any command that we don't have a handler for */
+	if (!(cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) && !cmd->execute_cmd)
+		goto out_unsupported_cdb;
+
+	if (cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) {
+		unsigned long long end_lba;
+
+		if (sectors > su_dev->se_dev_attrib.fabric_max_sectors) {
+			printk_ratelimited(KERN_ERR "SCSI OP %02xh with too"
+				" big sectors %u exceeds fabric_max_sectors:"
+				" %u\n", cdb[0], sectors,
+				su_dev->se_dev_attrib.fabric_max_sectors);
+			goto out_invalid_cdb_field;
+		}
+		if (sectors > su_dev->se_dev_attrib.hw_max_sectors) {
+			printk_ratelimited(KERN_ERR "SCSI OP %02xh with too"
+				" big sectors %u exceeds backend hw_max_sectors:"
+				" %u\n", cdb[0], sectors,
+				su_dev->se_dev_attrib.hw_max_sectors);
+			goto out_invalid_cdb_field;
+		}
+
+		end_lba = dev->transport->get_blocks(dev) + 1;
+		if (cmd->t_task_lba + sectors > end_lba) {
+			pr_err("cmd exceeds last lba %llu "
+				"(lba %llu, sectors %u)\n",
+				end_lba, cmd->t_task_lba, sectors);
+			goto out_invalid_cdb_field;
+		}
+
+		size = sbc_get_size(cmd, sectors);
+	}
+
+	ret = target_cmd_size_check(cmd, size);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+
+out_unsupported_cdb:
+	cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+	cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
+	return -EINVAL;
+out_invalid_cdb_field:
+	cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+	cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
+	return -EINVAL;
+}
+EXPORT_SYMBOL(sbc_parse_cdb);
diff --git a/drivers/target/target_core_cdb.c b/drivers/target/target_core_spc.c
index 9888693a18f..4c861de538c 100644
--- a/drivers/target/target_core_cdb.c
+++ b/drivers/target/target_core_spc.c
@@ -1,5 +1,5 @@
 /*
- * CDB emulation for non-READ/WRITE commands.
+ * SCSI Primary Commands (SPC) parsing and emulation.
  *
  * Copyright (c) 2002, 2003, 2004, 2005 PyX Technologies, Inc.
  * Copyright (c) 2005, 2006, 2007 SBE, Inc.
@@ -26,17 +26,21 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <asm/unaligned.h>
+
 #include <scsi/scsi.h>
+#include <scsi/scsi_tcq.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
 #include <target/target_core_fabric.h>
 
 #include "target_core_internal.h"
+#include "target_core_alua.h"
+#include "target_core_pr.h"
 #include "target_core_ua.h"
 
-static void
-target_fill_alua_data(struct se_port *port, unsigned char *buf)
+
+static void spc_fill_alua_data(struct se_port *port, unsigned char *buf)
 {
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
 	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
@@ -65,8 +69,7 @@ target_fill_alua_data(struct se_port *port, unsigned char *buf)
 	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
 }
 
-static int
-target_emulate_inquiry_std(struct se_cmd *cmd, char *buf)
+static int spc_emulate_inquiry_std(struct se_cmd *cmd, char *buf)
 {
 	struct se_lun *lun = cmd->se_lun;
 	struct se_device *dev = cmd->se_dev;
@@ -93,7 +96,7 @@ target_emulate_inquiry_std(struct se_cmd *cmd, char *buf)
 	 * Enable SCCS and TPGS fields for Emulated ALUA
 	 */
 	if (dev->se_sub_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED)
-		target_fill_alua_data(lun->lun_sep, buf);
+		spc_fill_alua_data(lun->lun_sep, buf);
 
 	buf[7] = 0x2; /* CmdQue=1 */
 
@@ -106,8 +109,7 @@ target_emulate_inquiry_std(struct se_cmd *cmd, char *buf)
 }
 
 /* unit serial number */
-static int
-target_emulate_evpd_80(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_80(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
 	u16 len = 0;
@@ -127,8 +129,8 @@ target_emulate_evpd_80(struct se_cmd *cmd, unsigned char *buf)
 	return 0;
 }
 
-static void
-target_parse_naa_6h_vendor_specific(struct se_device *dev, unsigned char *buf)
+static void spc_parse_naa_6h_vendor_specific(struct se_device *dev,
+		unsigned char *buf)
 {
 	unsigned char *p = &dev->se_sub_dev->t10_wwn.unit_serial[0];
 	int cnt;
@@ -162,8 +164,7 @@ target_parse_naa_6h_vendor_specific(struct se_device *dev, unsigned char *buf)
  * Device identification VPD, for a complete list of
  * DESIGNATOR TYPEs see spc4r17 Table 459.
  */
-static int
-target_emulate_evpd_83(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_83(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_lun *lun = cmd->se_lun;
@@ -220,7 +221,7 @@ target_emulate_evpd_83(struct se_cmd *cmd, unsigned char *buf)
 	 * VENDOR_SPECIFIC_IDENTIFIER and
 	 * VENDOR_SPECIFIC_IDENTIFIER_EXTENTION
 	 */
-	target_parse_naa_6h_vendor_specific(dev, &buf[off]);
+	spc_parse_naa_6h_vendor_specific(dev, &buf[off]);
 
 	len = 20;
 	off = (len + 4);
@@ -414,8 +415,7 @@ check_scsi_name:
 }
 
 /* Extended INQUIRY Data VPD Page */
-static int
-target_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
 {
 	buf[3] = 0x3c;
 	/* Set HEADSUP, ORDSUP, SIMPSUP */
@@ -428,15 +428,14 @@ target_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
 }
 
 /* Block Limits VPD page */
-static int
-target_emulate_evpd_b0(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_b0(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
 	u32 max_sectors;
 	int have_tp = 0;
 
 	/*
-	 * Following sbc3r22 section 6.5.3 Block Limits VPD page, when
+	 * Following spc3r22 section 6.5.3 Block Limits VPD page, when
 	 * emulate_tpu=1 or emulate_tpws=1 we will be expect a
 	 * different page length for Thin Provisioning.
 	 */
@@ -500,8 +499,7 @@ target_emulate_evpd_b0(struct se_cmd *cmd, unsigned char *buf)
 }
 
 /* Block Device Characteristics VPD page */
-static int
-target_emulate_evpd_b1(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_b1(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
 
@@ -513,13 +511,12 @@ target_emulate_evpd_b1(struct se_cmd *cmd, unsigned char *buf)
 }
 
 /* Thin Provisioning VPD */
-static int
-target_emulate_evpd_b2(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_b2(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
 
 	/*
-	 * From sbc3r22 section 6.5.4 Thin Provisioning VPD page:
+	 * From spc3r22 section 6.5.4 Thin Provisioning VPD page:
 	 *
 	 * The PAGE LENGTH field is defined in SPC-4. If the DP bit is set to
 	 * zero, then the page length shall be set to 0004h.  If the DP bit
@@ -564,25 +561,23 @@ target_emulate_evpd_b2(struct se_cmd *cmd, unsigned char *buf)
 	return 0;
 }
 
-static int
-target_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf);
+static int spc_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf);
 
 static struct {
 	uint8_t		page;
 	int		(*emulate)(struct se_cmd *, unsigned char *);
 } evpd_handlers[] = {
-	{ .page = 0x00, .emulate = target_emulate_evpd_00 },
-	{ .page = 0x80, .emulate = target_emulate_evpd_80 },
-	{ .page = 0x83, .emulate = target_emulate_evpd_83 },
-	{ .page = 0x86, .emulate = target_emulate_evpd_86 },
-	{ .page = 0xb0, .emulate = target_emulate_evpd_b0 },
-	{ .page = 0xb1, .emulate = target_emulate_evpd_b1 },
-	{ .page = 0xb2, .emulate = target_emulate_evpd_b2 },
+	{ .page = 0x00, .emulate = spc_emulate_evpd_00 },
+	{ .page = 0x80, .emulate = spc_emulate_evpd_80 },
+	{ .page = 0x83, .emulate = spc_emulate_evpd_83 },
+	{ .page = 0x86, .emulate = spc_emulate_evpd_86 },
+	{ .page = 0xb0, .emulate = spc_emulate_evpd_b0 },
+	{ .page = 0xb1, .emulate = spc_emulate_evpd_b1 },
+	{ .page = 0xb2, .emulate = spc_emulate_evpd_b2 },
 };
 
 /* supported vital product data pages */
-static int
-target_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf)
+static int spc_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf)
 {
 	int p;
 
@@ -601,7 +596,7 @@ target_emulate_evpd_00(struct se_cmd *cmd, unsigned char *buf)
 	return 0;
 }
 
-int target_emulate_inquiry(struct se_cmd *cmd)
+static int spc_emulate_inquiry(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_portal_group *tpg = cmd->se_lun->lun_sep->sep_tpg;
@@ -643,7 +638,7 @@ int target_emulate_inquiry(struct se_cmd *cmd)
 			goto out;
 		}
 
-		ret = target_emulate_inquiry_std(cmd, buf);
+		ret = spc_emulate_inquiry_std(cmd, buf);
 		goto out;
 	}
 
@@ -671,70 +666,7 @@ out:
 	return ret;
 }
 
-int target_emulate_readcapacity(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	unsigned char *buf;
-	unsigned long long blocks_long = dev->transport->get_blocks(dev);
-	u32 blocks;
-
-	if (blocks_long >= 0x00000000ffffffff)
-		blocks = 0xffffffff;
-	else
-		blocks = (u32)blocks_long;
-
-	buf = transport_kmap_data_sg(cmd);
-
-	buf[0] = (blocks >> 24) & 0xff;
-	buf[1] = (blocks >> 16) & 0xff;
-	buf[2] = (blocks >> 8) & 0xff;
-	buf[3] = blocks & 0xff;
-	buf[4] = (dev->se_sub_dev->se_dev_attrib.block_size >> 24) & 0xff;
-	buf[5] = (dev->se_sub_dev->se_dev_attrib.block_size >> 16) & 0xff;
-	buf[6] = (dev->se_sub_dev->se_dev_attrib.block_size >> 8) & 0xff;
-	buf[7] = dev->se_sub_dev->se_dev_attrib.block_size & 0xff;
-
-	transport_kunmap_data_sg(cmd);
-
-	target_complete_cmd(cmd, GOOD);
-	return 0;
-}
-
-int target_emulate_readcapacity_16(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	unsigned char *buf;
-	unsigned long long blocks = dev->transport->get_blocks(dev);
-
-	buf = transport_kmap_data_sg(cmd);
-
-	buf[0] = (blocks >> 56) & 0xff;
-	buf[1] = (blocks >> 48) & 0xff;
-	buf[2] = (blocks >> 40) & 0xff;
-	buf[3] = (blocks >> 32) & 0xff;
-	buf[4] = (blocks >> 24) & 0xff;
-	buf[5] = (blocks >> 16) & 0xff;
-	buf[6] = (blocks >> 8) & 0xff;
-	buf[7] = blocks & 0xff;
-	buf[8] = (dev->se_sub_dev->se_dev_attrib.block_size >> 24) & 0xff;
-	buf[9] = (dev->se_sub_dev->se_dev_attrib.block_size >> 16) & 0xff;
-	buf[10] = (dev->se_sub_dev->se_dev_attrib.block_size >> 8) & 0xff;
-	buf[11] = dev->se_sub_dev->se_dev_attrib.block_size & 0xff;
-	/*
-	 * Set Thin Provisioning Enable bit following sbc3r22 in section
-	 * READ CAPACITY (16) byte 14 if emulate_tpu or emulate_tpws is enabled.
-	 */
-	if (dev->se_sub_dev->se_dev_attrib.emulate_tpu || dev->se_sub_dev->se_dev_attrib.emulate_tpws)
-		buf[14] = 0x80;
-
-	transport_kunmap_data_sg(cmd);
-
-	target_complete_cmd(cmd, GOOD);
-	return 0;
-}
-
-static int
-target_modesense_rwrecovery(unsigned char *p)
+static int spc_modesense_rwrecovery(unsigned char *p)
 {
 	p[0] = 0x01;
 	p[1] = 0x0a;
@@ -742,8 +674,7 @@ target_modesense_rwrecovery(unsigned char *p)
 	return 12;
 }
 
-static int
-target_modesense_control(struct se_device *dev, unsigned char *p)
+static int spc_modesense_control(struct se_device *dev, unsigned char *p)
 {
 	p[0] = 0x0a;
 	p[1] = 0x0a;
@@ -828,8 +759,7 @@ target_modesense_control(struct se_device *dev, unsigned char *p)
 	return 12;
 }
 
-static int
-target_modesense_caching(struct se_device *dev, unsigned char *p)
+static int spc_modesense_caching(struct se_device *dev, unsigned char *p)
 {
 	p[0] = 0x08;
 	p[1] = 0x12;
@@ -840,8 +770,7 @@ target_modesense_caching(struct se_device *dev, unsigned char *p)
 	return 20;
 }
 
-static void
-target_modesense_write_protect(unsigned char *buf, int type)
+static void spc_modesense_write_protect(unsigned char *buf, int type)
 {
 	/*
 	 * I believe that the WP bit (bit 7) in the mode header is the same for
@@ -856,8 +785,7 @@ target_modesense_write_protect(unsigned char *buf, int type)
 	}
 }
 
-static void
-target_modesense_dpofua(unsigned char *buf, int type)
+static void spc_modesense_dpofua(unsigned char *buf, int type)
 {
 	switch (type) {
 	case TYPE_DISK:
@@ -868,7 +796,7 @@ target_modesense_dpofua(unsigned char *buf, int type)
 	}
 }
 
-int target_emulate_modesense(struct se_cmd *cmd)
+static int spc_emulate_modesense(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	char *cdb = cmd->t_task_cdb;
@@ -883,18 +811,18 @@ int target_emulate_modesense(struct se_cmd *cmd)
 
 	switch (cdb[2] & 0x3f) {
 	case 0x01:
-		length = target_modesense_rwrecovery(&buf[offset]);
+		length = spc_modesense_rwrecovery(&buf[offset]);
 		break;
 	case 0x08:
-		length = target_modesense_caching(dev, &buf[offset]);
+		length = spc_modesense_caching(dev, &buf[offset]);
 		break;
 	case 0x0a:
-		length = target_modesense_control(dev, &buf[offset]);
+		length = spc_modesense_control(dev, &buf[offset]);
 		break;
 	case 0x3f:
-		length = target_modesense_rwrecovery(&buf[offset]);
-		length += target_modesense_caching(dev, &buf[offset+length]);
-		length += target_modesense_control(dev, &buf[offset+length]);
+		length = spc_modesense_rwrecovery(&buf[offset]);
+		length += spc_modesense_caching(dev, &buf[offset+length]);
+		length += spc_modesense_control(dev, &buf[offset+length]);
 		break;
 	default:
 		pr_err("MODE SENSE: unimplemented page/subpage: 0x%02x/0x%02x\n",
@@ -912,11 +840,11 @@ int target_emulate_modesense(struct se_cmd *cmd)
 		if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
 		    (cmd->se_deve &&
 		    (cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY)))
-			target_modesense_write_protect(&buf[3], type);
+			spc_modesense_write_protect(&buf[3], type);
 
 		if ((dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0) &&
 		    (dev->se_sub_dev->se_dev_attrib.emulate_fua_write > 0))
-			target_modesense_dpofua(&buf[3], type);
+			spc_modesense_dpofua(&buf[3], type);
 
 		if ((offset + 2) > cmd->data_length)
 			offset = cmd->data_length;
@@ -928,11 +856,11 @@ int target_emulate_modesense(struct se_cmd *cmd)
 		if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
 		    (cmd->se_deve &&
 		    (cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY)))
-			target_modesense_write_protect(&buf[2], type);
+			spc_modesense_write_protect(&buf[2], type);
 
 		if ((dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0) &&
 		    (dev->se_sub_dev->se_dev_attrib.emulate_fua_write > 0))
-			target_modesense_dpofua(&buf[2], type);
+			spc_modesense_dpofua(&buf[2], type);
 
 		if ((offset + 1) > cmd->data_length)
 			offset = cmd->data_length;
@@ -946,7 +874,7 @@ int target_emulate_modesense(struct se_cmd *cmd)
 	return 0;
 }
 
-int target_emulate_request_sense(struct se_cmd *cmd)
+static int spc_emulate_request_sense(struct se_cmd *cmd)
 {
 	unsigned char *cdb = cmd->t_task_cdb;
 	unsigned char *buf;
@@ -1005,126 +933,172 @@ end:
 	return 0;
 }
 
-/*
- * Used for TCM/IBLOCK and TCM/FILEIO for block/blk-lib.c level discard support.
- * Note this is not used for TCM/pSCSI passthrough
- */
-int target_emulate_unmap(struct se_cmd *cmd)
+static int spc_emulate_testunitready(struct se_cmd *cmd)
 {
-	struct se_device *dev = cmd->se_dev;
-	unsigned char *buf, *ptr = NULL;
-	unsigned char *cdb = &cmd->t_task_cdb[0];
-	sector_t lba;
-	unsigned int size = cmd->data_length, range;
-	int ret = 0, offset;
-	unsigned short dl, bd_dl;
-
-	if (!dev->transport->do_discard) {
-		pr_err("UNMAP emulation not supported for: %s\n",
-				dev->transport->name);
-		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
-		return -ENOSYS;
-	}
-
-	/* First UNMAP block descriptor starts at 8 byte offset */
-	offset = 8;
-	size -= 8;
-	dl = get_unaligned_be16(&cdb[0]);
-	bd_dl = get_unaligned_be16(&cdb[2]);
-
-	buf = transport_kmap_data_sg(cmd);
-
-	ptr = &buf[offset];
-	pr_debug("UNMAP: Sub: %s Using dl: %hu bd_dl: %hu size: %hu"
-		" ptr: %p\n", dev->transport->name, dl, bd_dl, size, ptr);
-
-	while (size) {
-		lba = get_unaligned_be64(&ptr[0]);
-		range = get_unaligned_be32(&ptr[8]);
-		pr_debug("UNMAP: Using lba: %llu and range: %u\n",
-				 (unsigned long long)lba, range);
-
-		ret = dev->transport->do_discard(dev, lba, range);
-		if (ret < 0) {
-			pr_err("blkdev_issue_discard() failed: %d\n",
-					ret);
-			goto err;
-		}
-
-		ptr += 16;
-		size -= 16;
-	}
-
-err:
-	transport_kunmap_data_sg(cmd);
-	if (!ret)
-		target_complete_cmd(cmd, GOOD);
-	return ret;
+	target_complete_cmd(cmd, GOOD);
+	return 0;
 }
 
-/*
- * Used for TCM/IBLOCK and TCM/FILEIO for block/blk-lib.c level discard support.
- * Note this is not used for TCM/pSCSI passthrough
- */
-int target_emulate_write_same(struct se_cmd *cmd)
+int spc_parse_cdb(struct se_cmd *cmd, unsigned int *size)
 {
 	struct se_device *dev = cmd->se_dev;
-	sector_t range;
-	sector_t lba = cmd->t_task_lba;
-	u32 num_blocks;
-	int ret;
-
-	if (!dev->transport->do_discard) {
-		pr_err("WRITE_SAME emulation not supported"
-				" for: %s\n", dev->transport->name);
-		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
-		return -ENOSYS;
-	}
-
-	if (cmd->t_task_cdb[0] == WRITE_SAME)
-		num_blocks = get_unaligned_be16(&cmd->t_task_cdb[7]);
-	else if (cmd->t_task_cdb[0] == WRITE_SAME_16)
-		num_blocks = get_unaligned_be32(&cmd->t_task_cdb[10]);
-	else /* WRITE_SAME_32 via VARIABLE_LENGTH_CMD */
-		num_blocks = get_unaligned_be32(&cmd->t_task_cdb[28]);
-
-	/*
-	 * Use the explicit range when non zero is supplied, otherwise calculate
-	 * the remaining range based on ->get_blocks() - starting LBA.
-	 */
-	if (num_blocks != 0)
-		range = num_blocks;
-	else
-		range = (dev->transport->get_blocks(dev) - lba);
-
-	pr_debug("WRITE_SAME UNMAP: LBA: %llu Range: %llu\n",
-		 (unsigned long long)lba, (unsigned long long)range);
+	struct se_subsystem_dev *su_dev = dev->se_sub_dev;
+	unsigned char *cdb = cmd->t_task_cdb;
 
-	ret = dev->transport->do_discard(dev, lba, range);
-	if (ret < 0) {
-		pr_debug("blkdev_issue_discard() failed for WRITE_SAME\n");
-		return ret;
-	}
+	switch (cdb[0]) {
+	case MODE_SELECT:
+		*size = cdb[4];
+		break;
+	case MODE_SELECT_10:
+		*size = (cdb[7] << 8) + cdb[8];
+		break;
+	case MODE_SENSE:
+		*size = cdb[4];
+		cmd->execute_cmd = spc_emulate_modesense;
+		break;
+	case MODE_SENSE_10:
+		*size = (cdb[7] << 8) + cdb[8];
+		cmd->execute_cmd = spc_emulate_modesense;
+		break;
+	case LOG_SELECT:
+	case LOG_SENSE:
+		*size = (cdb[7] << 8) + cdb[8];
+		break;
+	case PERSISTENT_RESERVE_IN:
+		if (su_dev->t10_pr.res_type == SPC3_PERSISTENT_RESERVATIONS)
+			cmd->execute_cmd = target_scsi3_emulate_pr_in;
+		*size = (cdb[7] << 8) + cdb[8];
+		break;
+	case PERSISTENT_RESERVE_OUT:
+		if (su_dev->t10_pr.res_type == SPC3_PERSISTENT_RESERVATIONS)
+			cmd->execute_cmd = target_scsi3_emulate_pr_out;
+		*size = (cdb[7] << 8) + cdb[8];
+		break;
+	case RELEASE:
+	case RELEASE_10:
+		if (cdb[0] == RELEASE_10)
+			*size = (cdb[7] << 8) | cdb[8];
+		else
+			*size = cmd->data_length;
+
+		if (su_dev->t10_pr.res_type != SPC_PASSTHROUGH)
+			cmd->execute_cmd = target_scsi2_reservation_release;
+		break;
+	case RESERVE:
+	case RESERVE_10:
+		/*
+		 * The SPC-2 RESERVE does not contain a size in the SCSI CDB.
+		 * Assume the passthrough or $FABRIC_MOD will tell us about it.
+		 */
+		if (cdb[0] == RESERVE_10)
+			*size = (cdb[7] << 8) | cdb[8];
+		else
+			*size = cmd->data_length;
 
-	target_complete_cmd(cmd, GOOD);
-	return 0;
-}
+		/*
+		 * Setup the legacy emulated handler for SPC-2 and
+		 * >= SPC-3 compatible reservation handling (CRH=1)
+		 * Otherwise, we assume the underlying SCSI logic is
+		 * is running in SPC_PASSTHROUGH, and wants reservations
+		 * emulation disabled.
+		 */
+		if (su_dev->t10_pr.res_type != SPC_PASSTHROUGH)
+			cmd->execute_cmd = target_scsi2_reservation_reserve;
+		break;
+	case REQUEST_SENSE:
+		*size = cdb[4];
+		cmd->execute_cmd = spc_emulate_request_sense;
+		break;
+	case INQUIRY:
+		*size = (cdb[3] << 8) + cdb[4];
 
-int target_emulate_synchronize_cache(struct se_cmd *cmd)
-{
-	if (!cmd->se_dev->transport->do_sync_cache) {
-		pr_err("SYNCHRONIZE_CACHE emulation not supported"
-			" for: %s\n", cmd->se_dev->transport->name);
+		/*
+		 * Do implict HEAD_OF_QUEUE processing for INQUIRY.
+		 * See spc4r17 section 5.3
+		 */
+		if (cmd->se_dev->dev_task_attr_type == SAM_TASK_ATTR_EMULATED)
+			cmd->sam_task_attr = MSG_HEAD_TAG;
+		cmd->execute_cmd = spc_emulate_inquiry;
+		break;
+	case SECURITY_PROTOCOL_IN:
+	case SECURITY_PROTOCOL_OUT:
+		*size = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
+		break;
+	case EXTENDED_COPY:
+	case READ_ATTRIBUTE:
+	case RECEIVE_COPY_RESULTS:
+	case WRITE_ATTRIBUTE:
+		*size = (cdb[10] << 24) | (cdb[11] << 16) |
+		       (cdb[12] << 8) | cdb[13];
+		break;
+	case RECEIVE_DIAGNOSTIC:
+	case SEND_DIAGNOSTIC:
+		*size = (cdb[3] << 8) | cdb[4];
+		break;
+	case WRITE_BUFFER:
+		*size = (cdb[6] << 16) + (cdb[7] << 8) + cdb[8];
+		break;
+	case REPORT_LUNS:
+		cmd->execute_cmd = target_report_luns;
+		*size = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
+		/*
+		 * Do implict HEAD_OF_QUEUE processing for REPORT_LUNS
+		 * See spc4r17 section 5.3
+		 */
+		if (cmd->se_dev->dev_task_attr_type == SAM_TASK_ATTR_EMULATED)
+			cmd->sam_task_attr = MSG_HEAD_TAG;
+		break;
+	case TEST_UNIT_READY:
+		cmd->execute_cmd = spc_emulate_testunitready;
+		*size = 0;
+		break;
+	case MAINTENANCE_IN:
+		if (dev->transport->get_device_type(dev) != TYPE_ROM) {
+			/*
+			 * MAINTENANCE_IN from SCC-2
+			 * Check for emulated MI_REPORT_TARGET_PGS
+			 */
+			if ((cdb[1] & 0x1f) == MI_REPORT_TARGET_PGS &&
+			    su_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED) {
+				cmd->execute_cmd =
+					target_emulate_report_target_port_groups;
+			}
+			*size = get_unaligned_be32(&cdb[6]);
+		} else {
+			/*
+			 * GPCMD_SEND_KEY from multi media commands
+			 */
+			*size = get_unaligned_be16(&cdb[8]);
+		}
+		break;
+	case MAINTENANCE_OUT:
+		if (dev->transport->get_device_type(dev) != TYPE_ROM) {
+			/*
+			 * MAINTENANCE_OUT from SCC-2
+			 * Check for emulated MO_SET_TARGET_PGS.
+			 */
+			if (cdb[1] == MO_SET_TARGET_PGS &&
+			    su_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED) {
+				cmd->execute_cmd =
+					target_emulate_set_target_port_groups;
+			}
+			*size = get_unaligned_be32(&cdb[6]);
+		} else {
+			/*
+			 * GPCMD_SEND_KEY from multi media commands
+			 */
+			*size = get_unaligned_be16(&cdb[8]);
+		}
+		break;
+	default:
+		pr_warn("TARGET_CORE[%s]: Unsupported SCSI Opcode"
+			" 0x%02x, sending CHECK_CONDITION.\n",
+			cmd->se_tfo->get_fabric_name(), cdb[0]);
+		cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
 		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
-		return -ENOSYS;
+		return -EINVAL;
 	}
 
-	cmd->se_dev->transport->do_sync_cache(cmd);
-	return 0;
-}
-
-int target_emulate_noop(struct se_cmd *cmd)
-{
-	target_complete_cmd(cmd, GOOD);
 	return 0;
 }
+EXPORT_SYMBOL(spc_parse_cdb);
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index 84caf1bed9a..1c59a3c23b2 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -295,9 +295,6 @@ static void core_tmr_drain_state_list(
 
 		list_move_tail(&cmd->state_list, &drain_task_list);
 		cmd->state_active = false;
-
-		if (!list_empty(&cmd->execute_list))
-			__target_remove_from_execute_list(cmd);
 	}
 	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 
@@ -354,57 +351,6 @@ static void core_tmr_drain_state_list(
 	}
 }
 
-static void core_tmr_drain_cmd_list(
-	struct se_device *dev,
-	struct se_cmd *prout_cmd,
-	struct se_node_acl *tmr_nacl,
-	int tas,
-	struct list_head *preempt_and_abort_list)
-{
-	LIST_HEAD(drain_cmd_list);
-	struct se_queue_obj *qobj = &dev->dev_queue_obj;
-	struct se_cmd *cmd, *tcmd;
-	unsigned long flags;
-
-	/*
-	 * Release all commands remaining in the per-device command queue.
-	 *
-	 * This follows the same logic as above for the state list.
-	 */
-	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
-	list_for_each_entry_safe(cmd, tcmd, &qobj->qobj_list, se_queue_node) {
-		/*
-		 * For PREEMPT_AND_ABORT usage, only process commands
-		 * with a matching reservation key.
-		 */
-		if (target_check_cdb_and_preempt(preempt_and_abort_list, cmd))
-			continue;
-		/*
-		 * Not aborting PROUT PREEMPT_AND_ABORT CDB..
-		 */
-		if (prout_cmd == cmd)
-			continue;
-
-		cmd->transport_state &= ~CMD_T_QUEUED;
-		atomic_dec(&qobj->queue_cnt);
-		list_move_tail(&cmd->se_queue_node, &drain_cmd_list);
-	}
-	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
-
-	while (!list_empty(&drain_cmd_list)) {
-		cmd = list_entry(drain_cmd_list.next, struct se_cmd, se_queue_node);
-		list_del_init(&cmd->se_queue_node);
-
-		pr_debug("LUN_RESET: %s from Device Queue: cmd: %p t_state:"
-			" %d t_fe_count: %d\n", (preempt_and_abort_list) ?
-			"Preempt" : "", cmd, cmd->t_state,
-			atomic_read(&cmd->t_fe_count));
-
-		core_tmr_handle_tas_abort(tmr_nacl, cmd, tas,
-				atomic_read(&cmd->t_fe_count));
-	}
-}
-
 int core_tmr_lun_reset(
         struct se_device *dev,
         struct se_tmr_req *tmr,
@@ -447,8 +393,7 @@ int core_tmr_lun_reset(
 	core_tmr_drain_tmr_list(dev, tmr, preempt_and_abort_list);
 	core_tmr_drain_state_list(dev, prout_cmd, tmr_nacl, tas,
 				preempt_and_abort_list);
-	core_tmr_drain_cmd_list(dev, prout_cmd, tmr_nacl, tas,
-				preempt_and_abort_list);
+
 	/*
 	 * Clear any legacy SPC-2 reservation when called during
 	 * LOGICAL UNIT RESET
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index 8bd58e28418..b8628a5014b 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -77,8 +77,8 @@ static void core_clear_initiator_node_from_tpg(
 
 		lun = deve->se_lun;
 		spin_unlock_irq(&nacl->device_list_lock);
-		core_update_device_list_for_node(lun, NULL, deve->mapped_lun,
-			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg, 0);
+		core_disable_device_list_for_node(lun, NULL, deve->mapped_lun,
+			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg);
 
 		spin_lock_irq(&nacl->device_list_lock);
 	}
@@ -172,8 +172,8 @@ void core_tpg_add_node_to_devs(
 			(lun_access == TRANSPORT_LUNFLAGS_READ_WRITE) ?
 			"READ-WRITE" : "READ-ONLY");
 
-		core_update_device_list_for_node(lun, NULL, lun->unpacked_lun,
-				lun_access, acl, tpg, 1);
+		core_enable_device_list_for_node(lun, NULL, lun->unpacked_lun,
+				lun_access, acl, tpg);
 		spin_lock(&tpg->tpg_lun_lock);
 	}
 	spin_unlock(&tpg->tpg_lun_lock);
@@ -306,10 +306,8 @@ struct se_node_acl *core_tpg_check_initiator_node_acl(
 	 * TPG LUNs if the fabric is not explictly asking for
 	 * tpg_check_demo_mode_login_only() == 1.
 	 */
-	if ((tpg->se_tpg_tfo->tpg_check_demo_mode_login_only != NULL) &&
-	    (tpg->se_tpg_tfo->tpg_check_demo_mode_login_only(tpg) == 1))
-		do { ; } while (0);
-	else
+	if ((tpg->se_tpg_tfo->tpg_check_demo_mode_login_only == NULL) ||
+	    (tpg->se_tpg_tfo->tpg_check_demo_mode_login_only(tpg) != 1))
 		core_tpg_add_node_to_devs(acl, tpg);
 
 	spin_lock_irq(&tpg->acl_node_lock);
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 634d0f31a28..0eaae23d12b 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -66,15 +66,12 @@ struct kmem_cache *t10_alua_lu_gp_mem_cache;
 struct kmem_cache *t10_alua_tg_pt_gp_cache;
 struct kmem_cache *t10_alua_tg_pt_gp_mem_cache;
 
-static int transport_generic_write_pending(struct se_cmd *);
-static int transport_processing_thread(void *param);
-static int __transport_execute_tasks(struct se_device *dev, struct se_cmd *);
 static void transport_complete_task_attr(struct se_cmd *cmd);
 static void transport_handle_queue_full(struct se_cmd *cmd,
 		struct se_device *dev);
 static int transport_generic_get_mem(struct se_cmd *cmd);
+static int target_get_sess_cmd(struct se_session *, struct se_cmd *, bool);
 static void transport_put_cmd(struct se_cmd *cmd);
-static void transport_remove_cmd_from_queue(struct se_cmd *cmd);
 static int transport_set_sense_codes(struct se_cmd *cmd, u8 asc, u8 ascq);
 static void target_complete_ok_work(struct work_struct *work);
 
@@ -195,14 +192,6 @@ u32 scsi_get_new_index(scsi_index_t type)
 	return new_index;
 }
 
-static void transport_init_queue_obj(struct se_queue_obj *qobj)
-{
-	atomic_set(&qobj->queue_cnt, 0);
-	INIT_LIST_HEAD(&qobj->qobj_list);
-	init_waitqueue_head(&qobj->thread_wq);
-	spin_lock_init(&qobj->cmd_queue_lock);
-}
-
 void transport_subsystem_check_init(void)
 {
 	int ret;
@@ -243,7 +232,6 @@ struct se_session *transport_init_session(void)
 	INIT_LIST_HEAD(&se_sess->sess_list);
 	INIT_LIST_HEAD(&se_sess->sess_acl_list);
 	INIT_LIST_HEAD(&se_sess->sess_cmd_list);
-	INIT_LIST_HEAD(&se_sess->sess_wait_list);
 	spin_lock_init(&se_sess->sess_cmd_lock);
 	kref_init(&se_sess->sess_kref);
 
@@ -468,18 +456,7 @@ static void target_remove_from_state_list(struct se_cmd *cmd)
 	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 }
 
-/*	transport_cmd_check_stop():
- *
- *	'transport_off = 1' determines if CMD_T_ACTIVE should be cleared.
- *	'transport_off = 2' determines if task_dev_state should be removed.
- *
- *	A non-zero u8 t_state sets cmd->t_state.
- *	Returns 1 when command is stopped, else 0.
- */
-static int transport_cmd_check_stop(
-	struct se_cmd *cmd,
-	int transport_off,
-	u8 t_state)
+static int transport_cmd_check_stop(struct se_cmd *cmd, bool remove_from_lists)
 {
 	unsigned long flags;
 
@@ -493,13 +470,23 @@ static int transport_cmd_check_stop(
 			__func__, __LINE__, cmd->se_tfo->get_task_tag(cmd));
 
 		cmd->transport_state &= ~CMD_T_ACTIVE;
-		if (transport_off == 2)
+		if (remove_from_lists)
 			target_remove_from_state_list(cmd);
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
 		complete(&cmd->transport_lun_stop_comp);
 		return 1;
 	}
+
+	if (remove_from_lists) {
+		target_remove_from_state_list(cmd);
+
+		/*
+		 * Clear struct se_cmd->se_lun before the handoff to FE.
+		 */
+		cmd->se_lun = NULL;
+	}
+
 	/*
 	 * Determine if frontend context caller is requesting the stopping of
 	 * this command for frontend exceptions.
@@ -509,58 +496,36 @@ static int transport_cmd_check_stop(
 			__func__, __LINE__,
 			cmd->se_tfo->get_task_tag(cmd));
 
-		if (transport_off == 2)
-			target_remove_from_state_list(cmd);
-
-		/*
-		 * Clear struct se_cmd->se_lun before the transport_off == 2 handoff
-		 * to FE.
-		 */
-		if (transport_off == 2)
-			cmd->se_lun = NULL;
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
 		complete(&cmd->t_transport_stop_comp);
 		return 1;
 	}
-	if (transport_off) {
-		cmd->transport_state &= ~CMD_T_ACTIVE;
-		if (transport_off == 2) {
-			target_remove_from_state_list(cmd);
-			/*
-			 * Clear struct se_cmd->se_lun before the transport_off == 2
-			 * handoff to fabric module.
-			 */
-			cmd->se_lun = NULL;
-			/*
-			 * Some fabric modules like tcm_loop can release
-			 * their internally allocated I/O reference now and
-			 * struct se_cmd now.
-			 *
-			 * Fabric modules are expected to return '1' here if the
-			 * se_cmd being passed is released at this point,
-			 * or zero if not being released.
-			 */
-			if (cmd->se_tfo->check_stop_free != NULL) {
-				spin_unlock_irqrestore(
-					&cmd->t_state_lock, flags);
-
-				return cmd->se_tfo->check_stop_free(cmd);
-			}
+
+	cmd->transport_state &= ~CMD_T_ACTIVE;
+	if (remove_from_lists) {
+		/*
+		 * Some fabric modules like tcm_loop can release
+		 * their internally allocated I/O reference now and
+		 * struct se_cmd now.
+		 *
+		 * Fabric modules are expected to return '1' here if the
+		 * se_cmd being passed is released at this point,
+		 * or zero if not being released.
+		 */
+		if (cmd->se_tfo->check_stop_free != NULL) {
+			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+			return cmd->se_tfo->check_stop_free(cmd);
 		}
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	}
 
-		return 0;
-	} else if (t_state)
-		cmd->t_state = t_state;
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-
 	return 0;
 }
 
 static int transport_cmd_check_stop_to_fabric(struct se_cmd *cmd)
 {
-	return transport_cmd_check_stop(cmd, 2, 0);
+	return transport_cmd_check_stop(cmd, true);
 }
 
 static void transport_lun_remove_cmd(struct se_cmd *cmd)
@@ -591,79 +556,8 @@ void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
 
 	if (transport_cmd_check_stop_to_fabric(cmd))
 		return;
-	if (remove) {
-		transport_remove_cmd_from_queue(cmd);
+	if (remove)
 		transport_put_cmd(cmd);
-	}
-}
-
-static void transport_add_cmd_to_queue(struct se_cmd *cmd, int t_state,
-		bool at_head)
-{
-	struct se_device *dev = cmd->se_dev;
-	struct se_queue_obj *qobj = &dev->dev_queue_obj;
-	unsigned long flags;
-
-	if (t_state) {
-		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		cmd->t_state = t_state;
-		cmd->transport_state |= CMD_T_ACTIVE;
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-	}
-
-	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
-
-	/* If the cmd is already on the list, remove it before we add it */
-	if (!list_empty(&cmd->se_queue_node))
-		list_del(&cmd->se_queue_node);
-	else
-		atomic_inc(&qobj->queue_cnt);
-
-	if (at_head)
-		list_add(&cmd->se_queue_node, &qobj->qobj_list);
-	else
-		list_add_tail(&cmd->se_queue_node, &qobj->qobj_list);
-	cmd->transport_state |= CMD_T_QUEUED;
-	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
-
-	wake_up_interruptible(&qobj->thread_wq);
-}
-
-static struct se_cmd *
-transport_get_cmd_from_queue(struct se_queue_obj *qobj)
-{
-	struct se_cmd *cmd;
-	unsigned long flags;
-
-	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
-	if (list_empty(&qobj->qobj_list)) {
-		spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
-		return NULL;
-	}
-	cmd = list_first_entry(&qobj->qobj_list, struct se_cmd, se_queue_node);
-
-	cmd->transport_state &= ~CMD_T_QUEUED;
-	list_del_init(&cmd->se_queue_node);
-	atomic_dec(&qobj->queue_cnt);
-	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
-
-	return cmd;
-}
-
-static void transport_remove_cmd_from_queue(struct se_cmd *cmd)
-{
-	struct se_queue_obj *qobj = &cmd->se_dev->dev_queue_obj;
-	unsigned long flags;
-
-	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
-	if (!(cmd->transport_state & CMD_T_QUEUED)) {
-		spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
-		return;
-	}
-	cmd->transport_state &= ~CMD_T_QUEUED;
-	atomic_dec(&qobj->queue_cnt);
-	list_del_init(&cmd->se_queue_node);
-	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
 }
 
 static void target_complete_failure_work(struct work_struct *work)
@@ -742,68 +636,11 @@ static void target_add_to_state_list(struct se_cmd *cmd)
 	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
 }
 
-static void __target_add_to_execute_list(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	bool head_of_queue = false;
-
-	if (!list_empty(&cmd->execute_list))
-		return;
-
-	if (dev->dev_task_attr_type == SAM_TASK_ATTR_EMULATED &&
-	    cmd->sam_task_attr == MSG_HEAD_TAG)
-		head_of_queue = true;
-
-	if (head_of_queue)
-		list_add(&cmd->execute_list, &dev->execute_list);
-	else
-		list_add_tail(&cmd->execute_list, &dev->execute_list);
-
-	atomic_inc(&dev->execute_tasks);
-
-	if (cmd->state_active)
-		return;
-
-	if (head_of_queue)
-		list_add(&cmd->state_list, &dev->state_list);
-	else
-		list_add_tail(&cmd->state_list, &dev->state_list);
-
-	cmd->state_active = true;
-}
-
-static void target_add_to_execute_list(struct se_cmd *cmd)
-{
-	unsigned long flags;
-	struct se_device *dev = cmd->se_dev;
-
-	spin_lock_irqsave(&dev->execute_task_lock, flags);
-	__target_add_to_execute_list(cmd);
-	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
-}
-
-void __target_remove_from_execute_list(struct se_cmd *cmd)
-{
-	list_del_init(&cmd->execute_list);
-	atomic_dec(&cmd->se_dev->execute_tasks);
-}
-
-static void target_remove_from_execute_list(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	unsigned long flags;
-
-	if (WARN_ON(list_empty(&cmd->execute_list)))
-		return;
-
-	spin_lock_irqsave(&dev->execute_task_lock, flags);
-	__target_remove_from_execute_list(cmd);
-	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
-}
-
 /*
  * Handle QUEUE_FULL / -EAGAIN and -ENOMEM status
  */
+static void transport_write_pending_qf(struct se_cmd *cmd);
+static void transport_complete_qf(struct se_cmd *cmd);
 
 static void target_qf_do_work(struct work_struct *work)
 {
@@ -827,7 +664,10 @@ static void target_qf_do_work(struct work_struct *work)
 			(cmd->t_state == TRANSPORT_COMPLETE_QF_WP) ? "WRITE_PENDING"
 			: "UNKNOWN");
 
-		transport_add_cmd_to_queue(cmd, cmd->t_state, true);
+		if (cmd->t_state == TRANSPORT_COMPLETE_QF_WP)
+			transport_write_pending_qf(cmd);
+		else if (cmd->t_state == TRANSPORT_COMPLETE_QF_OK)
+			transport_complete_qf(cmd);
 	}
 }
 
@@ -874,8 +714,7 @@ void transport_dump_dev_state(
 		break;
 	}
 
-	*bl += sprintf(b + *bl, "  Execute/Max Queue Depth: %d/%d",
-		atomic_read(&dev->execute_tasks), dev->queue_depth);
+	*bl += sprintf(b + *bl, "  Max Queue Depth: %d", dev->queue_depth);
 	*bl += sprintf(b + *bl, "  SectorSize: %u  HwMaxSectors: %u\n",
 		dev->se_sub_dev->se_dev_attrib.block_size,
 		dev->se_sub_dev->se_dev_attrib.hw_max_sectors);
@@ -1212,7 +1051,6 @@ struct se_device *transport_add_device_to_core_hba(
 		return NULL;
 	}
 
-	transport_init_queue_obj(&dev->dev_queue_obj);
 	dev->dev_flags		= device_flags;
 	dev->dev_status		|= TRANSPORT_DEVICE_DEACTIVATED;
 	dev->dev_ptr		= transport_dev;
@@ -1222,7 +1060,6 @@ struct se_device *transport_add_device_to_core_hba(
 	INIT_LIST_HEAD(&dev->dev_list);
 	INIT_LIST_HEAD(&dev->dev_sep_list);
 	INIT_LIST_HEAD(&dev->dev_tmr_list);
-	INIT_LIST_HEAD(&dev->execute_list);
 	INIT_LIST_HEAD(&dev->delayed_cmd_list);
 	INIT_LIST_HEAD(&dev->state_list);
 	INIT_LIST_HEAD(&dev->qf_cmd_list);
@@ -1261,17 +1098,17 @@ struct se_device *transport_add_device_to_core_hba(
 	 * Setup the Asymmetric Logical Unit Assignment for struct se_device
 	 */
 	if (core_setup_alua(dev, force_pt) < 0)
-		goto out;
+		goto err_dev_list;
 
 	/*
 	 * Startup the struct se_device processing thread
 	 */
-	dev->process_thread = kthread_run(transport_processing_thread, dev,
-					  "LIO_%s", dev->transport->name);
-	if (IS_ERR(dev->process_thread)) {
-		pr_err("Unable to create kthread: LIO_%s\n",
+	dev->tmr_wq = alloc_workqueue("tmr-%s", WQ_MEM_RECLAIM | WQ_UNBOUND, 1,
+				      dev->transport->name);
+	if (!dev->tmr_wq) {
+		pr_err("Unable to create tmr workqueue for %s\n",
 			dev->transport->name);
-		goto out;
+		goto err_dev_list;
 	}
 	/*
 	 * Setup work_queue for QUEUE_FULL
@@ -1289,7 +1126,7 @@ struct se_device *transport_add_device_to_core_hba(
 		if (!inquiry_prod || !inquiry_rev) {
 			pr_err("All non TCM/pSCSI plugins require"
 				" INQUIRY consts\n");
-			goto out;
+			goto err_wq;
 		}
 
 		strncpy(&dev->se_sub_dev->t10_wwn.vendor[0], "LIO-ORG", 8);
@@ -1299,9 +1136,10 @@ struct se_device *transport_add_device_to_core_hba(
 	scsi_dump_inquiry(dev);
 
 	return dev;
-out:
-	kthread_stop(dev->process_thread);
 
+err_wq:
+	destroy_workqueue(dev->tmr_wq);
+err_dev_list:
 	spin_lock(&hba->device_lock);
 	list_del(&dev->dev_list);
 	hba->dev_count--;
@@ -1315,35 +1153,54 @@ out:
 }
 EXPORT_SYMBOL(transport_add_device_to_core_hba);
 
-/*	transport_generic_prepare_cdb():
- *
- *	Since the Initiator sees iSCSI devices as LUNs,  the SCSI CDB will
- *	contain the iSCSI LUN in bits 7-5 of byte 1 as per SAM-2.
- *	The point of this is since we are mapping iSCSI LUNs to
- *	SCSI Target IDs having a non-zero LUN in the CDB will throw the
- *	devices and HBAs for a loop.
- */
-static inline void transport_generic_prepare_cdb(
-	unsigned char *cdb)
+int target_cmd_size_check(struct se_cmd *cmd, unsigned int size)
 {
-	switch (cdb[0]) {
-	case READ_10: /* SBC - RDProtect */
-	case READ_12: /* SBC - RDProtect */
-	case READ_16: /* SBC - RDProtect */
-	case SEND_DIAGNOSTIC: /* SPC - SELF-TEST Code */
-	case VERIFY: /* SBC - VRProtect */
-	case VERIFY_16: /* SBC - VRProtect */
-	case WRITE_VERIFY: /* SBC - VRProtect */
-	case WRITE_VERIFY_12: /* SBC - VRProtect */
-	case MAINTENANCE_IN: /* SPC - Parameter Data Format for SA RTPG */
-		break;
-	default:
-		cdb[1] &= 0x1f; /* clear logical unit number */
-		break;
+	struct se_device *dev = cmd->se_dev;
+
+	if (cmd->unknown_data_length) {
+		cmd->data_length = size;
+	} else if (size != cmd->data_length) {
+		pr_warn("TARGET_CORE[%s]: Expected Transfer Length:"
+			" %u does not match SCSI CDB Length: %u for SAM Opcode:"
+			" 0x%02x\n", cmd->se_tfo->get_fabric_name(),
+				cmd->data_length, size, cmd->t_task_cdb[0]);
+
+		cmd->cmd_spdtl = size;
+
+		if (cmd->data_direction == DMA_TO_DEVICE) {
+			pr_err("Rejecting underflow/overflow"
+					" WRITE data\n");
+			goto out_invalid_cdb_field;
+		}
+		/*
+		 * Reject READ_* or WRITE_* with overflow/underflow for
+		 * type SCF_SCSI_DATA_CDB.
+		 */
+		if (dev->se_sub_dev->se_dev_attrib.block_size != 512)  {
+			pr_err("Failing OVERFLOW/UNDERFLOW for LBA op"
+				" CDB on non 512-byte sector setup subsystem"
+				" plugin: %s\n", dev->transport->name);
+			/* Returns CHECK_CONDITION + INVALID_CDB_FIELD */
+			goto out_invalid_cdb_field;
+		}
+
+		if (size > cmd->data_length) {
+			cmd->se_cmd_flags |= SCF_OVERFLOW_BIT;
+			cmd->residual_count = (size - cmd->data_length);
+		} else {
+			cmd->se_cmd_flags |= SCF_UNDERFLOW_BIT;
+			cmd->residual_count = (cmd->data_length - size);
+		}
+		cmd->data_length = size;
 	}
-}
 
-static int transport_generic_cmd_sequencer(struct se_cmd *, unsigned char *);
+	return 0;
+
+out_invalid_cdb_field:
+	cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+	cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
+	return -EINVAL;
+}
 
 /*
  * Used by fabric modules containing a local struct se_cmd within their
@@ -1361,9 +1218,7 @@ void transport_init_se_cmd(
 	INIT_LIST_HEAD(&cmd->se_lun_node);
 	INIT_LIST_HEAD(&cmd->se_delayed_node);
 	INIT_LIST_HEAD(&cmd->se_qf_node);
-	INIT_LIST_HEAD(&cmd->se_queue_node);
 	INIT_LIST_HEAD(&cmd->se_cmd_list);
-	INIT_LIST_HEAD(&cmd->execute_list);
 	INIT_LIST_HEAD(&cmd->state_list);
 	init_completion(&cmd->transport_lun_fe_stop_comp);
 	init_completion(&cmd->transport_lun_stop_comp);
@@ -1418,9 +1273,12 @@ int target_setup_cmd_from_cdb(
 	struct se_cmd *cmd,
 	unsigned char *cdb)
 {
+	struct se_subsystem_dev *su_dev = cmd->se_dev->se_sub_dev;
+	u32 pr_reg_type = 0;
+	u8 alua_ascq = 0;
+	unsigned long flags;
 	int ret;
 
-	transport_generic_prepare_cdb(cdb);
 	/*
 	 * Ensure that the received CDB is less than the max (252 + 8) bytes
 	 * for VARIABLE_LENGTH_CMD
@@ -1457,15 +1315,66 @@ int target_setup_cmd_from_cdb(
 	 * Copy the original CDB into cmd->
 	 */
 	memcpy(cmd->t_task_cdb, cdb, scsi_command_size(cdb));
+
 	/*
-	 * Setup the received CDB based on SCSI defined opcodes and
-	 * perform unit attention, persistent reservations and ALUA
-	 * checks for virtual device backends.  The cmd->t_task_cdb
-	 * pointer is expected to be setup before we reach this point.
+	 * Check for an existing UNIT ATTENTION condition
 	 */
-	ret = transport_generic_cmd_sequencer(cmd, cdb);
+	if (core_scsi3_ua_check(cmd, cdb) < 0) {
+		cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+		cmd->scsi_sense_reason = TCM_CHECK_CONDITION_UNIT_ATTENTION;
+		return -EINVAL;
+	}
+
+	ret = su_dev->t10_alua.alua_state_check(cmd, cdb, &alua_ascq);
+	if (ret != 0) {
+		/*
+		 * Set SCSI additional sense code (ASC) to 'LUN Not Accessible';
+		 * The ALUA additional sense code qualifier (ASCQ) is determined
+		 * by the ALUA primary or secondary access state..
+		 */
+		if (ret > 0) {
+			pr_debug("[%s]: ALUA TG Port not available, "
+				"SenseKey: NOT_READY, ASC/ASCQ: "
+				"0x04/0x%02x\n",
+				cmd->se_tfo->get_fabric_name(), alua_ascq);
+
+			transport_set_sense_codes(cmd, 0x04, alua_ascq);
+			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+			cmd->scsi_sense_reason = TCM_CHECK_CONDITION_NOT_READY;
+			return -EINVAL;
+		}
+		cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+		cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
+		return -EINVAL;
+	}
+
+	/*
+	 * Check status for SPC-3 Persistent Reservations
+	 */
+	if (su_dev->t10_pr.pr_ops.t10_reservation_check(cmd, &pr_reg_type)) {
+		if (su_dev->t10_pr.pr_ops.t10_seq_non_holder(
+					cmd, cdb, pr_reg_type) != 0) {
+			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+			cmd->se_cmd_flags |= SCF_SCSI_RESERVATION_CONFLICT;
+			cmd->scsi_status = SAM_STAT_RESERVATION_CONFLICT;
+			cmd->scsi_sense_reason = TCM_RESERVATION_CONFLICT;
+			return -EBUSY;
+		}
+		/*
+		 * This means the CDB is allowed for the SCSI Initiator port
+		 * when said port is *NOT* holding the legacy SPC-2 or
+		 * SPC-3 Persistent Reservation.
+		 */
+	}
+
+	ret = cmd->se_dev->transport->parse_cdb(cmd);
 	if (ret < 0)
 		return ret;
+
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	cmd->se_cmd_flags |= SCF_SUPPORTED_SAM_OPCODE;
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+
 	/*
 	 * Check for SAM Task Attribute Emulation
 	 */
@@ -1503,10 +1412,9 @@ int transport_handle_cdb_direct(
 		return -EINVAL;
 	}
 	/*
-	 * Set TRANSPORT_NEW_CMD state and CMD_T_ACTIVE following
-	 * transport_generic_handle_cdb*() -> transport_add_cmd_to_queue()
-	 * in existing usage to ensure that outstanding descriptors are handled
-	 * correctly during shutdown via transport_wait_for_tasks()
+	 * Set TRANSPORT_NEW_CMD state and CMD_T_ACTIVE to ensure that
+	 * outstanding descriptors are handled correctly during shutdown via
+	 * transport_wait_for_tasks()
 	 *
 	 * Also, we don't take cmd->t_state_lock here as we only expect
 	 * this to be called for initial descriptor submission.
@@ -1540,10 +1448,14 @@ EXPORT_SYMBOL(transport_handle_cdb_direct);
  * @data_dir: DMA data direction
  * @flags: flags for command submission from target_sc_flags_tables
  *
+ * Returns non zero to signal active I/O shutdown failure.  All other
+ * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
+ * but still return zero here.
+ *
  * This may only be called from process context, and also currently
  * assumes internal allocation of fabric payload buffer by target-core.
  **/
-void target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
+int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 		unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
 		u32 data_length, int task_attr, int data_dir, int flags)
 {
@@ -1569,7 +1481,9 @@ void target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 	 * for fabrics using TARGET_SCF_ACK_KREF that expect a second
 	 * kref_put() to happen during fabric packet acknowledgement.
 	 */
-	target_get_sess_cmd(se_sess, se_cmd, (flags & TARGET_SCF_ACK_KREF));
+	rc = target_get_sess_cmd(se_sess, se_cmd, (flags & TARGET_SCF_ACK_KREF));
+	if (rc)
+		return rc;
 	/*
 	 * Signal bidirectional data payloads to target-core
 	 */
@@ -1582,16 +1496,13 @@ void target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 		transport_send_check_condition_and_sense(se_cmd,
 				se_cmd->scsi_sense_reason, 0);
 		target_put_sess_cmd(se_sess, se_cmd);
-		return;
+		return 0;
 	}
-	/*
-	 * Sanitize CDBs via transport_generic_cmd_sequencer() and
-	 * allocate the necessary tasks to complete the received CDB+data
-	 */
+
 	rc = target_setup_cmd_from_cdb(se_cmd, cdb);
 	if (rc != 0) {
 		transport_generic_request_failure(se_cmd);
-		return;
+		return 0;
 	}
 
 	/*
@@ -1600,14 +1511,8 @@ void target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 	 */
 	core_alua_check_nonop_delay(se_cmd);
 
-	/*
-	 * Dispatch se_cmd descriptor to se_lun->lun_se_dev backend
-	 * for immediate execution of READs, otherwise wait for
-	 * transport_generic_handle_data() to be called for WRITEs
-	 * when fabric has filled the incoming buffer.
-	 */
 	transport_handle_cdb_direct(se_cmd);
-	return;
+	return 0;
 }
 EXPORT_SYMBOL(target_submit_cmd);
 
@@ -1662,7 +1567,11 @@ int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
 		se_cmd->se_tmr_req->ref_task_tag = tag;
 
 	/* See target_submit_cmd for commentary */
-	target_get_sess_cmd(se_sess, se_cmd, (flags & TARGET_SCF_ACK_KREF));
+	ret = target_get_sess_cmd(se_sess, se_cmd, (flags & TARGET_SCF_ACK_KREF));
+	if (ret) {
+		core_tmr_release_req(se_cmd->se_tmr_req);
+		return ret;
+	}
 
 	ret = transport_lookup_tmr_lun(se_cmd, unpacked_lun);
 	if (ret) {
@@ -1680,67 +1589,6 @@ int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
 EXPORT_SYMBOL(target_submit_tmr);
 
 /*
- * Used by fabric module frontends defining a TFO->new_cmd_map() caller
- * to  queue up a newly setup se_cmd w/ TRANSPORT_NEW_CMD_MAP in order to
- * complete setup in TCM process context w/ TFO->new_cmd_map().
- */
-int transport_generic_handle_cdb_map(
-	struct se_cmd *cmd)
-{
-	if (!cmd->se_lun) {
-		dump_stack();
-		pr_err("cmd->se_lun is NULL\n");
-		return -EINVAL;
-	}
-
-	transport_add_cmd_to_queue(cmd, TRANSPORT_NEW_CMD_MAP, false);
-	return 0;
-}
-EXPORT_SYMBOL(transport_generic_handle_cdb_map);
-
-/*	transport_generic_handle_data():
- *
- *
- */
-int transport_generic_handle_data(
-	struct se_cmd *cmd)
-{
-	/*
-	 * For the software fabric case, then we assume the nexus is being
-	 * failed/shutdown when signals are pending from the kthread context
-	 * caller, so we return a failure.  For the HW target mode case running
-	 * in interrupt code, the signal_pending() check is skipped.
-	 */
-	if (!in_interrupt() && signal_pending(current))
-		return -EPERM;
-	/*
-	 * If the received CDB has aleady been ABORTED by the generic
-	 * target engine, we now call transport_check_aborted_status()
-	 * to queue any delated TASK_ABORTED status for the received CDB to the
-	 * fabric module as we are expecting no further incoming DATA OUT
-	 * sequences at this point.
-	 */
-	if (transport_check_aborted_status(cmd, 1) != 0)
-		return 0;
-
-	transport_add_cmd_to_queue(cmd, TRANSPORT_PROCESS_WRITE, false);
-	return 0;
-}
-EXPORT_SYMBOL(transport_generic_handle_data);
-
-/*	transport_generic_handle_tmr():
- *
- *
- */
-int transport_generic_handle_tmr(
-	struct se_cmd *cmd)
-{
-	transport_add_cmd_to_queue(cmd, TRANSPORT_PROCESS_TMR, false);
-	return 0;
-}
-EXPORT_SYMBOL(transport_generic_handle_tmr);
-
-/*
  * If the cmd is active, request it to be stopped and sleep until it
  * has completed.
  */
@@ -1797,6 +1645,7 @@ void transport_generic_request_failure(struct se_cmd *cmd)
 	case TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE:
 	case TCM_UNKNOWN_MODE_PAGE:
 	case TCM_WRITE_PROTECTED:
+	case TCM_ADDRESS_OUT_OF_RANGE:
 	case TCM_CHECK_CONDITION_ABORT_CMD:
 	case TCM_CHECK_CONDITION_UNIT_ATTENTION:
 	case TCM_CHECK_CONDITION_NOT_READY:
@@ -1832,13 +1681,7 @@ void transport_generic_request_failure(struct se_cmd *cmd)
 		cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
 		break;
 	}
-	/*
-	 * If a fabric does not define a cmd->se_tfo->new_cmd_map caller,
-	 * make the call to transport_send_check_condition_and_sense()
-	 * directly.  Otherwise expect the fabric to make the call to
-	 * transport_send_check_condition_and_sense() after handling
-	 * possible unsoliticied write data payloads.
-	 */
+
 	ret = transport_send_check_condition_and_sense(cmd,
 			cmd->scsi_sense_reason, 0);
 	if (ret == -EAGAIN || ret == -ENOMEM)
@@ -1856,406 +1699,123 @@ queue_full:
 }
 EXPORT_SYMBOL(transport_generic_request_failure);
 
-static inline u32 transport_lba_21(unsigned char *cdb)
+static void __target_execute_cmd(struct se_cmd *cmd)
 {
-	return ((cdb[1] & 0x1f) << 16) | (cdb[2] << 8) | cdb[3];
-}
+	int error = 0;
 
-static inline u32 transport_lba_32(unsigned char *cdb)
-{
-	return (cdb[2] << 24) | (cdb[3] << 16) | (cdb[4] << 8) | cdb[5];
-}
-
-static inline unsigned long long transport_lba_64(unsigned char *cdb)
-{
-	unsigned int __v1, __v2;
-
-	__v1 = (cdb[2] << 24) | (cdb[3] << 16) | (cdb[4] << 8) | cdb[5];
-	__v2 = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
-
-	return ((unsigned long long)__v2) | (unsigned long long)__v1 << 32;
-}
-
-/*
- * For VARIABLE_LENGTH_CDB w/ 32 byte extended CDBs
- */
-static inline unsigned long long transport_lba_64_ext(unsigned char *cdb)
-{
-	unsigned int __v1, __v2;
-
-	__v1 = (cdb[12] << 24) | (cdb[13] << 16) | (cdb[14] << 8) | cdb[15];
-	__v2 = (cdb[16] << 24) | (cdb[17] << 16) | (cdb[18] << 8) | cdb[19];
-
-	return ((unsigned long long)__v2) | (unsigned long long)__v1 << 32;
-}
-
-static void transport_set_supported_SAM_opcode(struct se_cmd *se_cmd)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&se_cmd->t_state_lock, flags);
-	se_cmd->se_cmd_flags |= SCF_SUPPORTED_SAM_OPCODE;
-	spin_unlock_irqrestore(&se_cmd->t_state_lock, flags);
-}
-
-/*
- * Called from Fabric Module context from transport_execute_tasks()
- *
- * The return of this function determins if the tasks from struct se_cmd
- * get added to the execution queue in transport_execute_tasks(),
- * or are added to the delayed or ordered lists here.
- */
-static inline int transport_execute_task_attr(struct se_cmd *cmd)
-{
-	if (cmd->se_dev->dev_task_attr_type != SAM_TASK_ATTR_EMULATED)
-		return 1;
-	/*
-	 * Check for the existence of HEAD_OF_QUEUE, and if true return 1
-	 * to allow the passed struct se_cmd list of tasks to the front of the list.
-	 */
-	 if (cmd->sam_task_attr == MSG_HEAD_TAG) {
-		pr_debug("Added HEAD_OF_QUEUE for CDB:"
-			" 0x%02x, se_ordered_id: %u\n",
-			cmd->t_task_cdb[0],
-			cmd->se_ordered_id);
-		return 1;
-	} else if (cmd->sam_task_attr == MSG_ORDERED_TAG) {
-		atomic_inc(&cmd->se_dev->dev_ordered_sync);
-		smp_mb__after_atomic_inc();
-
-		pr_debug("Added ORDERED for CDB: 0x%02x to ordered"
-				" list, se_ordered_id: %u\n",
-				cmd->t_task_cdb[0],
-				cmd->se_ordered_id);
-		/*
-		 * Add ORDERED command to tail of execution queue if
-		 * no other older commands exist that need to be
-		 * completed first.
-		 */
-		if (!atomic_read(&cmd->se_dev->simple_cmds))
-			return 1;
-	} else {
-		/*
-		 * For SIMPLE and UNTAGGED Task Attribute commands
-		 */
-		atomic_inc(&cmd->se_dev->simple_cmds);
-		smp_mb__after_atomic_inc();
-	}
-	/*
-	 * Otherwise if one or more outstanding ORDERED task attribute exist,
-	 * add the dormant task(s) built for the passed struct se_cmd to the
-	 * execution queue and become in Active state for this struct se_device.
-	 */
-	if (atomic_read(&cmd->se_dev->dev_ordered_sync) != 0) {
-		/*
-		 * Otherwise, add cmd w/ tasks to delayed cmd queue that
-		 * will be drained upon completion of HEAD_OF_QUEUE task.
-		 */
-		spin_lock(&cmd->se_dev->delayed_cmd_lock);
-		cmd->se_cmd_flags |= SCF_DELAYED_CMD_FROM_SAM_ATTR;
-		list_add_tail(&cmd->se_delayed_node,
-				&cmd->se_dev->delayed_cmd_list);
-		spin_unlock(&cmd->se_dev->delayed_cmd_lock);
-
-		pr_debug("Added CDB: 0x%02x Task Attr: 0x%02x to"
-			" delayed CMD list, se_ordered_id: %u\n",
-			cmd->t_task_cdb[0], cmd->sam_task_attr,
-			cmd->se_ordered_id);
-		/*
-		 * Return zero to let transport_execute_tasks() know
-		 * not to add the delayed tasks to the execution list.
-		 */
-		return 0;
-	}
-	/*
-	 * Otherwise, no ORDERED task attributes exist..
-	 */
-	return 1;
-}
-
-/*
- * Called from fabric module context in transport_generic_new_cmd() and
- * transport_generic_process_write()
- */
-static void transport_execute_tasks(struct se_cmd *cmd)
-{
-	int add_tasks;
-	struct se_device *se_dev = cmd->se_dev;
-	/*
-	 * Call transport_cmd_check_stop() to see if a fabric exception
-	 * has occurred that prevents execution.
-	 */
-	if (!transport_cmd_check_stop(cmd, 0, TRANSPORT_PROCESSING)) {
-		/*
-		 * Check for SAM Task Attribute emulation and HEAD_OF_QUEUE
-		 * attribute for the tasks of the received struct se_cmd CDB
-		 */
-		add_tasks = transport_execute_task_attr(cmd);
-		if (add_tasks) {
-			__transport_execute_tasks(se_dev, cmd);
-			return;
-		}
-	}
-	__transport_execute_tasks(se_dev, NULL);
-}
-
-static int __transport_execute_tasks(struct se_device *dev, struct se_cmd *new_cmd)
-{
-	int error;
-	struct se_cmd *cmd = NULL;
-	unsigned long flags;
-
-check_depth:
-	spin_lock_irq(&dev->execute_task_lock);
-	if (new_cmd != NULL)
-		__target_add_to_execute_list(new_cmd);
-
-	if (list_empty(&dev->execute_list)) {
-		spin_unlock_irq(&dev->execute_task_lock);
-		return 0;
-	}
-	cmd = list_first_entry(&dev->execute_list, struct se_cmd, execute_list);
-	__target_remove_from_execute_list(cmd);
-	spin_unlock_irq(&dev->execute_task_lock);
-
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	cmd->transport_state |= CMD_T_BUSY;
-	cmd->transport_state |= CMD_T_SENT;
-
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	spin_lock_irq(&cmd->t_state_lock);
+	cmd->transport_state |= (CMD_T_BUSY|CMD_T_SENT);
+	spin_unlock_irq(&cmd->t_state_lock);
 
 	if (cmd->execute_cmd)
 		error = cmd->execute_cmd(cmd);
-	else {
-		error = dev->transport->execute_cmd(cmd, cmd->t_data_sg,
-				cmd->t_data_nents, cmd->data_direction);
-	}
 
-	if (error != 0) {
-		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		cmd->transport_state &= ~CMD_T_BUSY;
-		cmd->transport_state &= ~CMD_T_SENT;
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	if (error) {
+		spin_lock_irq(&cmd->t_state_lock);
+		cmd->transport_state &= ~(CMD_T_BUSY|CMD_T_SENT);
+		spin_unlock_irq(&cmd->t_state_lock);
 
 		transport_generic_request_failure(cmd);
 	}
-
-	new_cmd = NULL;
-	goto check_depth;
-
-	return 0;
 }
 
-static inline u32 transport_get_sectors_6(
-	unsigned char *cdb,
-	struct se_cmd *cmd,
-	int *ret)
+void target_execute_cmd(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 
 	/*
-	 * Assume TYPE_DISK for non struct se_device objects.
-	 * Use 8-bit sector value.
-	 */
-	if (!dev)
-		goto type_disk;
-
-	/*
-	 * Use 24-bit allocation length for TYPE_TAPE.
+	 * If the received CDB has aleady been aborted stop processing it here.
 	 */
-	if (dev->transport->get_device_type(dev) == TYPE_TAPE)
-		return (u32)(cdb[2] << 16) + (cdb[3] << 8) + cdb[4];
-
-	/*
-	 * Everything else assume TYPE_DISK Sector CDB location.
-	 * Use 8-bit sector value.  SBC-3 says:
-	 *
-	 *   A TRANSFER LENGTH field set to zero specifies that 256
-	 *   logical blocks shall be written.  Any other value
-	 *   specifies the number of logical blocks that shall be
-	 *   written.
-	 */
-type_disk:
-	return cdb[4] ? : 256;
-}
-
-static inline u32 transport_get_sectors_10(
-	unsigned char *cdb,
-	struct se_cmd *cmd,
-	int *ret)
-{
-	struct se_device *dev = cmd->se_dev;
+	if (transport_check_aborted_status(cmd, 1))
+		return;
 
 	/*
-	 * Assume TYPE_DISK for non struct se_device objects.
-	 * Use 16-bit sector value.
+	 * Determine if IOCTL context caller in requesting the stopping of this
+	 * command for LUN shutdown purposes.
 	 */
-	if (!dev)
-		goto type_disk;
+	spin_lock_irq(&cmd->t_state_lock);
+	if (cmd->transport_state & CMD_T_LUN_STOP) {
+		pr_debug("%s:%d CMD_T_LUN_STOP for ITT: 0x%08x\n",
+			__func__, __LINE__, cmd->se_tfo->get_task_tag(cmd));
 
-	/*
-	 * XXX_10 is not defined in SSC, throw an exception
-	 */
-	if (dev->transport->get_device_type(dev) == TYPE_TAPE) {
-		*ret = -EINVAL;
-		return 0;
+		cmd->transport_state &= ~CMD_T_ACTIVE;
+		spin_unlock_irq(&cmd->t_state_lock);
+		complete(&cmd->transport_lun_stop_comp);
+		return;
 	}
-
 	/*
-	 * Everything else assume TYPE_DISK Sector CDB location.
-	 * Use 16-bit sector value.
-	 */
-type_disk:
-	return (u32)(cdb[7] << 8) + cdb[8];
-}
-
-static inline u32 transport_get_sectors_12(
-	unsigned char *cdb,
-	struct se_cmd *cmd,
-	int *ret)
-{
-	struct se_device *dev = cmd->se_dev;
-
-	/*
-	 * Assume TYPE_DISK for non struct se_device objects.
-	 * Use 32-bit sector value.
+	 * Determine if frontend context caller is requesting the stopping of
+	 * this command for frontend exceptions.
 	 */
-	if (!dev)
-		goto type_disk;
+	if (cmd->transport_state & CMD_T_STOP) {
+		pr_debug("%s:%d CMD_T_STOP for ITT: 0x%08x\n",
+			__func__, __LINE__,
+			cmd->se_tfo->get_task_tag(cmd));
 
-	/*
-	 * XXX_12 is not defined in SSC, throw an exception
-	 */
-	if (dev->transport->get_device_type(dev) == TYPE_TAPE) {
-		*ret = -EINVAL;
-		return 0;
+		spin_unlock_irq(&cmd->t_state_lock);
+		complete(&cmd->t_transport_stop_comp);
+		return;
 	}
 
-	/*
-	 * Everything else assume TYPE_DISK Sector CDB location.
-	 * Use 32-bit sector value.
-	 */
-type_disk:
-	return (u32)(cdb[6] << 24) + (cdb[7] << 16) + (cdb[8] << 8) + cdb[9];
-}
-
-static inline u32 transport_get_sectors_16(
-	unsigned char *cdb,
-	struct se_cmd *cmd,
-	int *ret)
-{
-	struct se_device *dev = cmd->se_dev;
-
-	/*
-	 * Assume TYPE_DISK for non struct se_device objects.
-	 * Use 32-bit sector value.
-	 */
-	if (!dev)
-		goto type_disk;
+	cmd->t_state = TRANSPORT_PROCESSING;
+	spin_unlock_irq(&cmd->t_state_lock);
 
-	/*
-	 * Use 24-bit allocation length for TYPE_TAPE.
-	 */
-	if (dev->transport->get_device_type(dev) == TYPE_TAPE)
-		return (u32)(cdb[12] << 16) + (cdb[13] << 8) + cdb[14];
-
-type_disk:
-	return (u32)(cdb[10] << 24) + (cdb[11] << 16) +
-		    (cdb[12] << 8) + cdb[13];
-}
+	if (dev->dev_task_attr_type != SAM_TASK_ATTR_EMULATED)
+		goto execute;
 
-/*
- * Used for VARIABLE_LENGTH_CDB WRITE_32 and READ_32 variants
- */
-static inline u32 transport_get_sectors_32(
-	unsigned char *cdb,
-	struct se_cmd *cmd,
-	int *ret)
-{
 	/*
-	 * Assume TYPE_DISK for non struct se_device objects.
-	 * Use 32-bit sector value.
+	 * Check for the existence of HEAD_OF_QUEUE, and if true return 1
+	 * to allow the passed struct se_cmd list of tasks to the front of the list.
 	 */
-	return (u32)(cdb[28] << 24) + (cdb[29] << 16) +
-		    (cdb[30] << 8) + cdb[31];
+	switch (cmd->sam_task_attr) {
+	case MSG_HEAD_TAG:
+		pr_debug("Added HEAD_OF_QUEUE for CDB: 0x%02x, "
+			 "se_ordered_id: %u\n",
+			 cmd->t_task_cdb[0], cmd->se_ordered_id);
+		goto execute;
+	case MSG_ORDERED_TAG:
+		atomic_inc(&dev->dev_ordered_sync);
+		smp_mb__after_atomic_inc();
 
-}
+		pr_debug("Added ORDERED for CDB: 0x%02x to ordered list, "
+			 " se_ordered_id: %u\n",
+			 cmd->t_task_cdb[0], cmd->se_ordered_id);
 
-static inline u32 transport_get_size(
-	u32 sectors,
-	unsigned char *cdb,
-	struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-
-	if (dev->transport->get_device_type(dev) == TYPE_TAPE) {
-		if (cdb[1] & 1) { /* sectors */
-			return dev->se_sub_dev->se_dev_attrib.block_size * sectors;
-		} else /* bytes */
-			return sectors;
+		/*
+		 * Execute an ORDERED command if no other older commands
+		 * exist that need to be completed first.
+		 */
+		if (!atomic_read(&dev->simple_cmds))
+			goto execute;
+		break;
+	default:
+		/*
+		 * For SIMPLE and UNTAGGED Task Attribute commands
+		 */
+		atomic_inc(&dev->simple_cmds);
+		smp_mb__after_atomic_inc();
+		break;
 	}
 
-	pr_debug("Returning block_size: %u, sectors: %u == %u for"
-		" %s object\n", dev->se_sub_dev->se_dev_attrib.block_size,
-		sectors, dev->se_sub_dev->se_dev_attrib.block_size * sectors,
-		dev->transport->name);
-
-	return dev->se_sub_dev->se_dev_attrib.block_size * sectors;
-}
+	if (atomic_read(&dev->dev_ordered_sync) != 0) {
+		spin_lock(&dev->delayed_cmd_lock);
+		list_add_tail(&cmd->se_delayed_node, &dev->delayed_cmd_list);
+		spin_unlock(&dev->delayed_cmd_lock);
 
-static void transport_xor_callback(struct se_cmd *cmd)
-{
-	unsigned char *buf, *addr;
-	struct scatterlist *sg;
-	unsigned int offset;
-	int i;
-	int count;
-	/*
-	 * From sbc3r22.pdf section 5.48 XDWRITEREAD (10) command
-	 *
-	 * 1) read the specified logical block(s);
-	 * 2) transfer logical blocks from the data-out buffer;
-	 * 3) XOR the logical blocks transferred from the data-out buffer with
-	 *    the logical blocks read, storing the resulting XOR data in a buffer;
-	 * 4) if the DISABLE WRITE bit is set to zero, then write the logical
-	 *    blocks transferred from the data-out buffer; and
-	 * 5) transfer the resulting XOR data to the data-in buffer.
-	 */
-	buf = kmalloc(cmd->data_length, GFP_KERNEL);
-	if (!buf) {
-		pr_err("Unable to allocate xor_callback buf\n");
+		pr_debug("Added CDB: 0x%02x Task Attr: 0x%02x to"
+			" delayed CMD list, se_ordered_id: %u\n",
+			cmd->t_task_cdb[0], cmd->sam_task_attr,
+			cmd->se_ordered_id);
 		return;
 	}
-	/*
-	 * Copy the scatterlist WRITE buffer located at cmd->t_data_sg
-	 * into the locally allocated *buf
-	 */
-	sg_copy_to_buffer(cmd->t_data_sg,
-			  cmd->t_data_nents,
-			  buf,
-			  cmd->data_length);
 
+execute:
 	/*
-	 * Now perform the XOR against the BIDI read memory located at
-	 * cmd->t_mem_bidi_list
+	 * Otherwise, no ORDERED task attributes exist..
 	 */
-
-	offset = 0;
-	for_each_sg(cmd->t_bidi_data_sg, sg, cmd->t_bidi_data_nents, count) {
-		addr = kmap_atomic(sg_page(sg));
-		if (!addr)
-			goto out;
-
-		for (i = 0; i < sg->length; i++)
-			*(addr + sg->offset + i) ^= *(buf + offset + i);
-
-		offset += sg->length;
-		kunmap_atomic(addr);
-	}
-
-out:
-	kfree(buf);
+	__target_execute_cmd(cmd);
 }
+EXPORT_SYMBOL(target_execute_cmd);
 
 /*
  * Used to obtain Sense Data from underlying Linux/SCSI struct scsi_cmnd
@@ -2312,737 +1872,31 @@ out:
 	return -1;
 }
 
-static inline long long transport_dev_end_lba(struct se_device *dev)
-{
-	return dev->transport->get_blocks(dev) + 1;
-}
-
-static int transport_cmd_get_valid_sectors(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	u32 sectors;
-
-	if (dev->transport->get_device_type(dev) != TYPE_DISK)
-		return 0;
-
-	sectors = (cmd->data_length / dev->se_sub_dev->se_dev_attrib.block_size);
-
-	if ((cmd->t_task_lba + sectors) > transport_dev_end_lba(dev)) {
-		pr_err("LBA: %llu Sectors: %u exceeds"
-			" transport_dev_end_lba(): %llu\n",
-			cmd->t_task_lba, sectors,
-			transport_dev_end_lba(dev));
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int target_check_write_same_discard(unsigned char *flags, struct se_device *dev)
-{
-	/*
-	 * Determine if the received WRITE_SAME is used to for direct
-	 * passthrough into Linux/SCSI with struct request via TCM/pSCSI
-	 * or we are signaling the use of internal WRITE_SAME + UNMAP=1
-	 * emulation for -> Linux/BLOCK disbard with TCM/IBLOCK code.
-	 */
-	int passthrough = (dev->transport->transport_type ==
-				TRANSPORT_PLUGIN_PHBA_PDEV);
-
-	if (!passthrough) {
-		if ((flags[0] & 0x04) || (flags[0] & 0x02)) {
-			pr_err("WRITE_SAME PBDATA and LBDATA"
-				" bits not supported for Block Discard"
-				" Emulation\n");
-			return -ENOSYS;
-		}
-		/*
-		 * Currently for the emulated case we only accept
-		 * tpws with the UNMAP=1 bit set.
-		 */
-		if (!(flags[0] & 0x08)) {
-			pr_err("WRITE_SAME w/o UNMAP bit not"
-				" supported for Block Discard Emulation\n");
-			return -ENOSYS;
-		}
-	}
-
-	return 0;
-}
-
-/*	transport_generic_cmd_sequencer():
- *
- *	Generic Command Sequencer that should work for most DAS transport
- *	drivers.
- *
- *	Called from target_setup_cmd_from_cdb() in the $FABRIC_MOD
- *	RX Thread.
- *
- *	FIXME: Need to support other SCSI OPCODES where as well.
+/*
+ * Process all commands up to the last received ORDERED task attribute which
+ * requires another blocking boundary
  */
-static int transport_generic_cmd_sequencer(
-	struct se_cmd *cmd,
-	unsigned char *cdb)
+static void target_restart_delayed_cmds(struct se_device *dev)
 {
-	struct se_device *dev = cmd->se_dev;
-	struct se_subsystem_dev *su_dev = dev->se_sub_dev;
-	int ret = 0, sector_ret = 0, passthrough;
-	u32 sectors = 0, size = 0, pr_reg_type = 0;
-	u16 service_action;
-	u8 alua_ascq = 0;
-	/*
-	 * Check for an existing UNIT ATTENTION condition
-	 */
-	if (core_scsi3_ua_check(cmd, cdb) < 0) {
-		cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
-		cmd->scsi_sense_reason = TCM_CHECK_CONDITION_UNIT_ATTENTION;
-		return -EINVAL;
-	}
-	/*
-	 * Check status of Asymmetric Logical Unit Assignment port
-	 */
-	ret = su_dev->t10_alua.alua_state_check(cmd, cdb, &alua_ascq);
-	if (ret != 0) {
-		/*
-		 * Set SCSI additional sense code (ASC) to 'LUN Not Accessible';
-		 * The ALUA additional sense code qualifier (ASCQ) is determined
-		 * by the ALUA primary or secondary access state..
-		 */
-		if (ret > 0) {
-			pr_debug("[%s]: ALUA TG Port not available,"
-				" SenseKey: NOT_READY, ASC/ASCQ: 0x04/0x%02x\n",
-				cmd->se_tfo->get_fabric_name(), alua_ascq);
+	for (;;) {
+		struct se_cmd *cmd;
 
-			transport_set_sense_codes(cmd, 0x04, alua_ascq);
-			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
-			cmd->scsi_sense_reason = TCM_CHECK_CONDITION_NOT_READY;
-			return -EINVAL;
-		}
-		goto out_invalid_cdb_field;
-	}
-	/*
-	 * Check status for SPC-3 Persistent Reservations
-	 */
-	if (su_dev->t10_pr.pr_ops.t10_reservation_check(cmd, &pr_reg_type) != 0) {
-		if (su_dev->t10_pr.pr_ops.t10_seq_non_holder(
-					cmd, cdb, pr_reg_type) != 0) {
-			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
-			cmd->se_cmd_flags |= SCF_SCSI_RESERVATION_CONFLICT;
-			cmd->scsi_status = SAM_STAT_RESERVATION_CONFLICT;
-			cmd->scsi_sense_reason = TCM_RESERVATION_CONFLICT;
-			return -EBUSY;
-		}
-		/*
-		 * This means the CDB is allowed for the SCSI Initiator port
-		 * when said port is *NOT* holding the legacy SPC-2 or
-		 * SPC-3 Persistent Reservation.
-		 */
-	}
-
-	/*
-	 * If we operate in passthrough mode we skip most CDB emulation and
-	 * instead hand the commands down to the physical SCSI device.
-	 */
-	passthrough =
-		(dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV);
-
-	switch (cdb[0]) {
-	case READ_6:
-		sectors = transport_get_sectors_6(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_21(cdb);
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case READ_10:
-		sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_32(cdb);
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case READ_12:
-		sectors = transport_get_sectors_12(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_32(cdb);
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case READ_16:
-		sectors = transport_get_sectors_16(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_64(cdb);
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case WRITE_6:
-		sectors = transport_get_sectors_6(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_21(cdb);
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case WRITE_10:
-	case WRITE_VERIFY:
-		sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_32(cdb);
-		if (cdb[1] & 0x8)
-			cmd->se_cmd_flags |= SCF_FUA;
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case WRITE_12:
-		sectors = transport_get_sectors_12(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_32(cdb);
-		if (cdb[1] & 0x8)
-			cmd->se_cmd_flags |= SCF_FUA;
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case WRITE_16:
-		sectors = transport_get_sectors_16(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_64(cdb);
-		if (cdb[1] & 0x8)
-			cmd->se_cmd_flags |= SCF_FUA;
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-		break;
-	case XDWRITEREAD_10:
-		if ((cmd->data_direction != DMA_TO_DEVICE) ||
-		    !(cmd->se_cmd_flags & SCF_BIDI))
-			goto out_invalid_cdb_field;
-		sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->t_task_lba = transport_lba_32(cdb);
-		cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-
-		/*
-		 * Do now allow BIDI commands for passthrough mode.
-		 */
-		if (passthrough)
-			goto out_unsupported_cdb;
-
-		/*
-		 * Setup BIDI XOR callback to be run after I/O completion.
-		 */
-		cmd->transport_complete_callback = &transport_xor_callback;
-		if (cdb[1] & 0x8)
-			cmd->se_cmd_flags |= SCF_FUA;
-		break;
-	case VARIABLE_LENGTH_CMD:
-		service_action = get_unaligned_be16(&cdb[8]);
-		switch (service_action) {
-		case XDWRITEREAD_32:
-			sectors = transport_get_sectors_32(cdb, cmd, &sector_ret);
-			if (sector_ret)
-				goto out_unsupported_cdb;
-			size = transport_get_size(sectors, cdb, cmd);
-			/*
-			 * Use WRITE_32 and READ_32 opcodes for the emulated
-			 * XDWRITE_READ_32 logic.
-			 */
-			cmd->t_task_lba = transport_lba_64_ext(cdb);
-			cmd->se_cmd_flags |= SCF_SCSI_DATA_SG_IO_CDB;
-
-			/*
-			 * Do now allow BIDI commands for passthrough mode.
-			 */
-			if (passthrough)
-				goto out_unsupported_cdb;
-
-			/*
-			 * Setup BIDI XOR callback to be run during after I/O
-			 * completion.
-			 */
-			cmd->transport_complete_callback = &transport_xor_callback;
-			if (cdb[1] & 0x8)
-				cmd->se_cmd_flags |= SCF_FUA;
-			break;
-		case WRITE_SAME_32:
-			sectors = transport_get_sectors_32(cdb, cmd, &sector_ret);
-			if (sector_ret)
-				goto out_unsupported_cdb;
-
-			if (sectors)
-				size = transport_get_size(1, cdb, cmd);
-			else {
-				pr_err("WSNZ=1, WRITE_SAME w/sectors=0 not"
-				       " supported\n");
-				goto out_invalid_cdb_field;
-			}
-
-			cmd->t_task_lba = get_unaligned_be64(&cdb[12]);
-			cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-
-			if (target_check_write_same_discard(&cdb[10], dev) < 0)
-				goto out_unsupported_cdb;
-			if (!passthrough)
-				cmd->execute_cmd = target_emulate_write_same;
-			break;
-		default:
-			pr_err("VARIABLE_LENGTH_CMD service action"
-				" 0x%04x not supported\n", service_action);
-			goto out_unsupported_cdb;
-		}
-		break;
-	case MAINTENANCE_IN:
-		if (dev->transport->get_device_type(dev) != TYPE_ROM) {
-			/* MAINTENANCE_IN from SCC-2 */
-			/*
-			 * Check for emulated MI_REPORT_TARGET_PGS.
-			 */
-			if ((cdb[1] & 0x1f) == MI_REPORT_TARGET_PGS &&
-			    su_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED) {
-				cmd->execute_cmd =
-					target_emulate_report_target_port_groups;
-			}
-			size = (cdb[6] << 24) | (cdb[7] << 16) |
-			       (cdb[8] << 8) | cdb[9];
-		} else {
-			/* GPCMD_SEND_KEY from multi media commands */
-			size = (cdb[8] << 8) + cdb[9];
-		}
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case MODE_SELECT:
-		size = cdb[4];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case MODE_SELECT_10:
-		size = (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case MODE_SENSE:
-		size = cdb[4];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_modesense;
-		break;
-	case MODE_SENSE_10:
-		size = (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_modesense;
-		break;
-	case GPCMD_READ_BUFFER_CAPACITY:
-	case GPCMD_SEND_OPC:
-	case LOG_SELECT:
-	case LOG_SENSE:
-		size = (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case READ_BLOCK_LIMITS:
-		size = READ_BLOCK_LEN;
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case GPCMD_GET_CONFIGURATION:
-	case GPCMD_READ_FORMAT_CAPACITIES:
-	case GPCMD_READ_DISC_INFO:
-	case GPCMD_READ_TRACK_RZONE_INFO:
-		size = (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case PERSISTENT_RESERVE_IN:
-		if (su_dev->t10_pr.res_type == SPC3_PERSISTENT_RESERVATIONS)
-			cmd->execute_cmd = target_scsi3_emulate_pr_in;
-		size = (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case PERSISTENT_RESERVE_OUT:
-		if (su_dev->t10_pr.res_type == SPC3_PERSISTENT_RESERVATIONS)
-			cmd->execute_cmd = target_scsi3_emulate_pr_out;
-		size = (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case GPCMD_MECHANISM_STATUS:
-	case GPCMD_READ_DVD_STRUCTURE:
-		size = (cdb[8] << 8) + cdb[9];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case READ_POSITION:
-		size = READ_POSITION_LEN;
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case MAINTENANCE_OUT:
-		if (dev->transport->get_device_type(dev) != TYPE_ROM) {
-			/* MAINTENANCE_OUT from SCC-2
-			 *
-			 * Check for emulated MO_SET_TARGET_PGS.
-			 */
-			if (cdb[1] == MO_SET_TARGET_PGS &&
-			    su_dev->t10_alua.alua_type == SPC3_ALUA_EMULATED) {
-				cmd->execute_cmd =
-					target_emulate_set_target_port_groups;
-			}
-
-			size = (cdb[6] << 24) | (cdb[7] << 16) |
-			       (cdb[8] << 8) | cdb[9];
-		} else  {
-			/* GPCMD_REPORT_KEY from multi media commands */
-			size = (cdb[8] << 8) + cdb[9];
-		}
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case INQUIRY:
-		size = (cdb[3] << 8) + cdb[4];
-		/*
-		 * Do implict HEAD_OF_QUEUE processing for INQUIRY.
-		 * See spc4r17 section 5.3
-		 */
-		if (cmd->se_dev->dev_task_attr_type == SAM_TASK_ATTR_EMULATED)
-			cmd->sam_task_attr = MSG_HEAD_TAG;
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_inquiry;
-		break;
-	case READ_BUFFER:
-		size = (cdb[6] << 16) + (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case READ_CAPACITY:
-		size = READ_CAP_LEN;
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_readcapacity;
-		break;
-	case READ_MEDIA_SERIAL_NUMBER:
-	case SECURITY_PROTOCOL_IN:
-	case SECURITY_PROTOCOL_OUT:
-		size = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case SERVICE_ACTION_IN:
-		switch (cmd->t_task_cdb[1] & 0x1f) {
-		case SAI_READ_CAPACITY_16:
-			if (!passthrough)
-				cmd->execute_cmd =
-					target_emulate_readcapacity_16;
-			break;
-		default:
-			if (passthrough)
-				break;
-
-			pr_err("Unsupported SA: 0x%02x\n",
-				cmd->t_task_cdb[1] & 0x1f);
-			goto out_invalid_cdb_field;
-		}
-		/*FALLTHROUGH*/
-	case ACCESS_CONTROL_IN:
-	case ACCESS_CONTROL_OUT:
-	case EXTENDED_COPY:
-	case READ_ATTRIBUTE:
-	case RECEIVE_COPY_RESULTS:
-	case WRITE_ATTRIBUTE:
-		size = (cdb[10] << 24) | (cdb[11] << 16) |
-		       (cdb[12] << 8) | cdb[13];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case RECEIVE_DIAGNOSTIC:
-	case SEND_DIAGNOSTIC:
-		size = (cdb[3] << 8) | cdb[4];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-/* #warning FIXME: Figure out correct GPCMD_READ_CD blocksize. */
-#if 0
-	case GPCMD_READ_CD:
-		sectors = (cdb[6] << 16) + (cdb[7] << 8) + cdb[8];
-		size = (2336 * sectors);
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-#endif
-	case READ_TOC:
-		size = cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case REQUEST_SENSE:
-		size = cdb[4];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_request_sense;
-		break;
-	case READ_ELEMENT_STATUS:
-		size = 65536 * cdb[7] + 256 * cdb[8] + cdb[9];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case WRITE_BUFFER:
-		size = (cdb[6] << 16) + (cdb[7] << 8) + cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case RESERVE:
-	case RESERVE_10:
-		/*
-		 * The SPC-2 RESERVE does not contain a size in the SCSI CDB.
-		 * Assume the passthrough or $FABRIC_MOD will tell us about it.
-		 */
-		if (cdb[0] == RESERVE_10)
-			size = (cdb[7] << 8) | cdb[8];
-		else
-			size = cmd->data_length;
-
-		/*
-		 * Setup the legacy emulated handler for SPC-2 and
-		 * >= SPC-3 compatible reservation handling (CRH=1)
-		 * Otherwise, we assume the underlying SCSI logic is
-		 * is running in SPC_PASSTHROUGH, and wants reservations
-		 * emulation disabled.
-		 */
-		if (su_dev->t10_pr.res_type != SPC_PASSTHROUGH)
-			cmd->execute_cmd = target_scsi2_reservation_reserve;
-		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
-		break;
-	case RELEASE:
-	case RELEASE_10:
-		/*
-		 * The SPC-2 RELEASE does not contain a size in the SCSI CDB.
-		 * Assume the passthrough or $FABRIC_MOD will tell us about it.
-		*/
-		if (cdb[0] == RELEASE_10)
-			size = (cdb[7] << 8) | cdb[8];
-		else
-			size = cmd->data_length;
-
-		if (su_dev->t10_pr.res_type != SPC_PASSTHROUGH)
-			cmd->execute_cmd = target_scsi2_reservation_release;
-		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
-		break;
-	case SYNCHRONIZE_CACHE:
-	case SYNCHRONIZE_CACHE_16:
-		/*
-		 * Extract LBA and range to be flushed for emulated SYNCHRONIZE_CACHE
-		 */
-		if (cdb[0] == SYNCHRONIZE_CACHE) {
-			sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
-			cmd->t_task_lba = transport_lba_32(cdb);
-		} else {
-			sectors = transport_get_sectors_16(cdb, cmd, &sector_ret);
-			cmd->t_task_lba = transport_lba_64(cdb);
-		}
-		if (sector_ret)
-			goto out_unsupported_cdb;
-
-		size = transport_get_size(sectors, cdb, cmd);
-		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
-
-		if (passthrough)
+		spin_lock(&dev->delayed_cmd_lock);
+		if (list_empty(&dev->delayed_cmd_list)) {
+			spin_unlock(&dev->delayed_cmd_lock);
 			break;
-
-		/*
-		 * Check to ensure that LBA + Range does not exceed past end of
-		 * device for IBLOCK and FILEIO ->do_sync_cache() backend calls
-		 */
-		if ((cmd->t_task_lba != 0) || (sectors != 0)) {
-			if (transport_cmd_get_valid_sectors(cmd) < 0)
-				goto out_invalid_cdb_field;
-		}
-		cmd->execute_cmd = target_emulate_synchronize_cache;
-		break;
-	case UNMAP:
-		size = get_unaligned_be16(&cdb[7]);
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_unmap;
-		break;
-	case WRITE_SAME_16:
-		sectors = transport_get_sectors_16(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
-
-		if (sectors)
-			size = transport_get_size(1, cdb, cmd);
-		else {
-			pr_err("WSNZ=1, WRITE_SAME w/sectors=0 not supported\n");
-			goto out_invalid_cdb_field;
 		}
 
-		cmd->t_task_lba = get_unaligned_be64(&cdb[2]);
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-
-		if (target_check_write_same_discard(&cdb[1], dev) < 0)
-			goto out_unsupported_cdb;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_write_same;
-		break;
-	case WRITE_SAME:
-		sectors = transport_get_sectors_10(cdb, cmd, &sector_ret);
-		if (sector_ret)
-			goto out_unsupported_cdb;
+		cmd = list_entry(dev->delayed_cmd_list.next,
+				 struct se_cmd, se_delayed_node);
+		list_del(&cmd->se_delayed_node);
+		spin_unlock(&dev->delayed_cmd_lock);
 
-		if (sectors)
-			size = transport_get_size(1, cdb, cmd);
-		else {
-			pr_err("WSNZ=1, WRITE_SAME w/sectors=0 not supported\n");
-			goto out_invalid_cdb_field;
-		}
+		__target_execute_cmd(cmd);
 
-		cmd->t_task_lba = get_unaligned_be32(&cdb[2]);
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		/*
-		 * Follow sbcr26 with WRITE_SAME (10) and check for the existence
-		 * of byte 1 bit 3 UNMAP instead of original reserved field
-		 */
-		if (target_check_write_same_discard(&cdb[1], dev) < 0)
-			goto out_unsupported_cdb;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_write_same;
-		break;
-	case ALLOW_MEDIUM_REMOVAL:
-	case ERASE:
-	case REZERO_UNIT:
-	case SEEK_10:
-	case SPACE:
-	case START_STOP:
-	case TEST_UNIT_READY:
-	case VERIFY:
-	case WRITE_FILEMARKS:
-		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
-		if (!passthrough)
-			cmd->execute_cmd = target_emulate_noop;
-		break;
-	case GPCMD_CLOSE_TRACK:
-	case INITIALIZE_ELEMENT_STATUS:
-	case GPCMD_LOAD_UNLOAD:
-	case GPCMD_SET_SPEED:
-	case MOVE_MEDIUM:
-		cmd->se_cmd_flags |= SCF_SCSI_NON_DATA_CDB;
-		break;
-	case REPORT_LUNS:
-		cmd->execute_cmd = target_report_luns;
-		size = (cdb[6] << 24) | (cdb[7] << 16) | (cdb[8] << 8) | cdb[9];
-		/*
-		 * Do implict HEAD_OF_QUEUE processing for REPORT_LUNS
-		 * See spc4r17 section 5.3
-		 */
-		if (cmd->se_dev->dev_task_attr_type == SAM_TASK_ATTR_EMULATED)
-			cmd->sam_task_attr = MSG_HEAD_TAG;
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case GET_EVENT_STATUS_NOTIFICATION:
-		size = (cdb[7] << 8) | cdb[8];
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	case ATA_16:
-		/* Only support ATA passthrough to pSCSI backends.. */
-		if (!passthrough)
-			goto out_unsupported_cdb;
-
-		/* T_LENGTH */
-		switch (cdb[2] & 0x3) {
-		case 0x0:
-			sectors = 0;
-			break;
-		case 0x1:
-			sectors = (((cdb[1] & 0x1) ? cdb[3] : 0) << 8) | cdb[4];
-			break;
-		case 0x2:
-			sectors = (((cdb[1] & 0x1) ? cdb[5] : 0) << 8) | cdb[6];
+		if (cmd->sam_task_attr == MSG_ORDERED_TAG)
 			break;
-		case 0x3:
-			pr_err("T_LENGTH=0x3 not supported for ATA_16\n");
-			goto out_invalid_cdb_field;
-		}
-
-		/* BYTE_BLOCK */
-		if (cdb[2] & 0x4) {
-			/* BLOCK T_TYPE: 512 or sector */
-			size = sectors * ((cdb[2] & 0x10) ?
-				dev->se_sub_dev->se_dev_attrib.block_size : 512);
-		} else {
-			/* BYTE */
-			size = sectors;
-		}
-		cmd->se_cmd_flags |= SCF_SCSI_CONTROL_SG_IO_CDB;
-		break;
-	default:
-		pr_warn("TARGET_CORE[%s]: Unsupported SCSI Opcode"
-			" 0x%02x, sending CHECK_CONDITION.\n",
-			cmd->se_tfo->get_fabric_name(), cdb[0]);
-		goto out_unsupported_cdb;
-	}
-
-	if (cmd->unknown_data_length)
-		cmd->data_length = size;
-
-	if (size != cmd->data_length) {
-		pr_warn("TARGET_CORE[%s]: Expected Transfer Length:"
-			" %u does not match SCSI CDB Length: %u for SAM Opcode:"
-			" 0x%02x\n", cmd->se_tfo->get_fabric_name(),
-				cmd->data_length, size, cdb[0]);
-
-		cmd->cmd_spdtl = size;
-
-		if (cmd->data_direction == DMA_TO_DEVICE) {
-			pr_err("Rejecting underflow/overflow"
-					" WRITE data\n");
-			goto out_invalid_cdb_field;
-		}
-		/*
-		 * Reject READ_* or WRITE_* with overflow/underflow for
-		 * type SCF_SCSI_DATA_SG_IO_CDB.
-		 */
-		if (!ret && (dev->se_sub_dev->se_dev_attrib.block_size != 512))  {
-			pr_err("Failing OVERFLOW/UNDERFLOW for LBA op"
-				" CDB on non 512-byte sector setup subsystem"
-				" plugin: %s\n", dev->transport->name);
-			/* Returns CHECK_CONDITION + INVALID_CDB_FIELD */
-			goto out_invalid_cdb_field;
-		}
-
-		if (size > cmd->data_length) {
-			cmd->se_cmd_flags |= SCF_OVERFLOW_BIT;
-			cmd->residual_count = (size - cmd->data_length);
-		} else {
-			cmd->se_cmd_flags |= SCF_UNDERFLOW_BIT;
-			cmd->residual_count = (cmd->data_length - size);
-		}
-		cmd->data_length = size;
 	}
-
-	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
-		if (sectors > su_dev->se_dev_attrib.fabric_max_sectors) {
-			printk_ratelimited(KERN_ERR "SCSI OP %02xh with too"
-				" big sectors %u exceeds fabric_max_sectors:"
-				" %u\n", cdb[0], sectors,
-				su_dev->se_dev_attrib.fabric_max_sectors);
-			goto out_invalid_cdb_field;
-		}
-		if (sectors > su_dev->se_dev_attrib.hw_max_sectors) {
-			printk_ratelimited(KERN_ERR "SCSI OP %02xh with too"
-				" big sectors %u exceeds backend hw_max_sectors:"
-				" %u\n", cdb[0], sectors,
-				su_dev->se_dev_attrib.hw_max_sectors);
-			goto out_invalid_cdb_field;
-		}
-	}
-
-	/* reject any command that we don't have a handler for */
-	if (!(passthrough || cmd->execute_cmd ||
-	     (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB)))
-		goto out_unsupported_cdb;
-
-	transport_set_supported_SAM_opcode(cmd);
-	return ret;
-
-out_unsupported_cdb:
-	cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
-	cmd->scsi_sense_reason = TCM_UNSUPPORTED_SCSI_OPCODE;
-	return -EINVAL;
-out_invalid_cdb_field:
-	cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
-	cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
-	return -EINVAL;
 }
 
 /*
@@ -3052,8 +1906,6 @@ out_invalid_cdb_field:
 static void transport_complete_task_attr(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct se_cmd *cmd_p, *cmd_tmp;
-	int new_active_tasks = 0;
 
 	if (cmd->sam_task_attr == MSG_SIMPLE_TAG) {
 		atomic_dec(&dev->simple_cmds);
@@ -3075,38 +1927,8 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 		pr_debug("Incremented dev_cur_ordered_id: %u for ORDERED:"
 			" %u\n", dev->dev_cur_ordered_id, cmd->se_ordered_id);
 	}
-	/*
-	 * Process all commands up to the last received
-	 * ORDERED task attribute which requires another blocking
-	 * boundary
-	 */
-	spin_lock(&dev->delayed_cmd_lock);
-	list_for_each_entry_safe(cmd_p, cmd_tmp,
-			&dev->delayed_cmd_list, se_delayed_node) {
-
-		list_del(&cmd_p->se_delayed_node);
-		spin_unlock(&dev->delayed_cmd_lock);
-
-		pr_debug("Calling add_tasks() for"
-			" cmd_p: 0x%02x Task Attr: 0x%02x"
-			" Dormant -> Active, se_ordered_id: %u\n",
-			cmd_p->t_task_cdb[0],
-			cmd_p->sam_task_attr, cmd_p->se_ordered_id);
 
-		target_add_to_execute_list(cmd_p);
-		new_active_tasks++;
-
-		spin_lock(&dev->delayed_cmd_lock);
-		if (cmd_p->sam_task_attr == MSG_ORDERED_TAG)
-			break;
-	}
-	spin_unlock(&dev->delayed_cmd_lock);
-	/*
-	 * If new tasks have become active, wake up the transport thread
-	 * to do the processing of the Active tasks.
-	 */
-	if (new_active_tasks != 0)
-		wake_up_interruptible(&dev->dev_queue_obj.thread_wq);
+	target_restart_delayed_cmds(dev);
 }
 
 static void transport_complete_qf(struct se_cmd *cmd)
@@ -3365,31 +2187,27 @@ int transport_generic_map_mem_to_cmd(
 	if (!sgl || !sgl_count)
 		return 0;
 
-	if ((cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) ||
-	    (cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB)) {
-		/*
-		 * Reject SCSI data overflow with map_mem_to_cmd() as incoming
-		 * scatterlists already have been set to follow what the fabric
-		 * passes for the original expected data transfer length.
-		 */
-		if (cmd->se_cmd_flags & SCF_OVERFLOW_BIT) {
-			pr_warn("Rejecting SCSI DATA overflow for fabric using"
-				" SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC\n");
-			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
-			cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
-			return -EINVAL;
-		}
+	/*
+	 * Reject SCSI data overflow with map_mem_to_cmd() as incoming
+	 * scatterlists already have been set to follow what the fabric
+	 * passes for the original expected data transfer length.
+	 */
+	if (cmd->se_cmd_flags & SCF_OVERFLOW_BIT) {
+		pr_warn("Rejecting SCSI DATA overflow for fabric using"
+			" SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC\n");
+		cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+		cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
+		return -EINVAL;
+	}
 
-		cmd->t_data_sg = sgl;
-		cmd->t_data_nents = sgl_count;
+	cmd->t_data_sg = sgl;
+	cmd->t_data_nents = sgl_count;
 
-		if (sgl_bidi && sgl_bidi_count) {
-			cmd->t_bidi_data_sg = sgl_bidi;
-			cmd->t_bidi_data_nents = sgl_bidi_count;
-		}
-		cmd->se_cmd_flags |= SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC;
+	if (sgl_bidi && sgl_bidi_count) {
+		cmd->t_bidi_data_sg = sgl_bidi;
+		cmd->t_bidi_data_nents = sgl_bidi_count;
 	}
-
+	cmd->se_cmd_flags |= SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC;
 	return 0;
 }
 EXPORT_SYMBOL(transport_generic_map_mem_to_cmd);
@@ -3461,7 +2279,7 @@ transport_generic_get_mem(struct se_cmd *cmd)
 	cmd->t_data_nents = nents;
 	sg_init_table(cmd->t_data_sg, nents);
 
-	zero_flag = cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB ? 0 : __GFP_ZERO;
+	zero_flag = cmd->se_cmd_flags & SCF_SCSI_DATA_CDB ? 0 : __GFP_ZERO;
 
 	while (length) {
 		u32 page_len = min_t(u32, length, PAGE_SIZE);
@@ -3492,7 +2310,6 @@ out:
  */
 int transport_generic_new_cmd(struct se_cmd *cmd)
 {
-	struct se_device *dev = cmd->se_dev;
 	int ret = 0;
 
 	/*
@@ -3508,8 +2325,7 @@ int transport_generic_new_cmd(struct se_cmd *cmd)
 	}
 
 	/* Workaround for handling zero-length control CDBs */
-	if ((cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB) &&
-	    !cmd->data_length) {
+	if (!(cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) && !cmd->data_length) {
 		spin_lock_irq(&cmd->t_state_lock);
 		cmd->t_state = TRANSPORT_COMPLETE;
 		cmd->transport_state |= CMD_T_ACTIVE;
@@ -3527,52 +2343,45 @@ int transport_generic_new_cmd(struct se_cmd *cmd)
 		return 0;
 	}
 
-	if (cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) {
-		struct se_dev_attrib *attr = &dev->se_sub_dev->se_dev_attrib;
-
-		if (transport_cmd_get_valid_sectors(cmd) < 0)
-			return -EINVAL;
-
-		BUG_ON(cmd->data_length % attr->block_size);
-		BUG_ON(DIV_ROUND_UP(cmd->data_length, attr->block_size) >
-			attr->hw_max_sectors);
-	}
-
 	atomic_inc(&cmd->t_fe_count);
 
 	/*
-	 * For WRITEs, let the fabric know its buffer is ready.
-	 *
-	 * The command will be added to the execution queue after its write
-	 * data has arrived.
+	 * If this command is not a write we can execute it right here,
+	 * for write buffers we need to notify the fabric driver first
+	 * and let it call back once the write buffers are ready.
 	 */
-	if (cmd->data_direction == DMA_TO_DEVICE) {
-		target_add_to_state_list(cmd);
-		return transport_generic_write_pending(cmd);
+	target_add_to_state_list(cmd);
+	if (cmd->data_direction != DMA_TO_DEVICE) {
+		target_execute_cmd(cmd);
+		return 0;
 	}
-	/*
-	 * Everything else but a WRITE, add the command to the execution queue.
-	 */
-	transport_execute_tasks(cmd);
-	return 0;
+
+	spin_lock_irq(&cmd->t_state_lock);
+	cmd->t_state = TRANSPORT_WRITE_PENDING;
+	spin_unlock_irq(&cmd->t_state_lock);
+
+	transport_cmd_check_stop(cmd, false);
+
+	ret = cmd->se_tfo->write_pending(cmd);
+	if (ret == -EAGAIN || ret == -ENOMEM)
+		goto queue_full;
+
+	if (ret < 0)
+		return ret;
+	return 1;
 
 out_fail:
 	cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
 	cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 	return -EINVAL;
+queue_full:
+	pr_debug("Handling write_pending QUEUE__FULL: se_cmd: %p\n", cmd);
+	cmd->t_state = TRANSPORT_COMPLETE_QF_WP;
+	transport_handle_queue_full(cmd, cmd->se_dev);
+	return 0;
 }
 EXPORT_SYMBOL(transport_generic_new_cmd);
 
-/*	transport_generic_process_write():
- *
- *
- */
-void transport_generic_process_write(struct se_cmd *cmd)
-{
-	transport_execute_tasks(cmd);
-}
-EXPORT_SYMBOL(transport_generic_process_write);
-
 static void transport_write_pending_qf(struct se_cmd *cmd)
 {
 	int ret;
@@ -3585,43 +2394,6 @@ static void transport_write_pending_qf(struct se_cmd *cmd)
 	}
 }
 
-static int transport_generic_write_pending(struct se_cmd *cmd)
-{
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	cmd->t_state = TRANSPORT_WRITE_PENDING;
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-
-	/*
-	 * Clear the se_cmd for WRITE_PENDING status in order to set
-	 * CMD_T_ACTIVE so that transport_generic_handle_data can be called
-	 * from HW target mode interrupt code.  This is safe to be called
-	 * with transport_off=1 before the cmd->se_tfo->write_pending
-	 * because the se_cmd->se_lun pointer is not being cleared.
-	 */
-	transport_cmd_check_stop(cmd, 1, 0);
-
-	/*
-	 * Call the fabric write_pending function here to let the
-	 * frontend know that WRITE buffers are ready.
-	 */
-	ret = cmd->se_tfo->write_pending(cmd);
-	if (ret == -EAGAIN || ret == -ENOMEM)
-		goto queue_full;
-	else if (ret < 0)
-		return ret;
-
-	return 1;
-
-queue_full:
-	pr_debug("Handling write_pending QUEUE__FULL: se_cmd: %p\n", cmd);
-	cmd->t_state = TRANSPORT_COMPLETE_QF_WP;
-	transport_handle_queue_full(cmd, cmd->se_dev);
-	return 0;
-}
-
 void transport_generic_free_cmd(struct se_cmd *cmd, int wait_for_tasks)
 {
 	if (!(cmd->se_cmd_flags & SCF_SE_LUN_CMD)) {
@@ -3648,10 +2420,11 @@ EXPORT_SYMBOL(transport_generic_free_cmd);
  * @se_cmd:	command descriptor to add
  * @ack_kref:	Signal that fabric will perform an ack target_put_sess_cmd()
  */
-void target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
-			bool ack_kref)
+static int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
+			       bool ack_kref)
 {
 	unsigned long flags;
+	int ret = 0;
 
 	kref_init(&se_cmd->cmd_kref);
 	/*
@@ -3665,11 +2438,17 @@ void target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
 	}
 
 	spin_lock_irqsave(&se_sess->sess_cmd_lock, flags);
+	if (se_sess->sess_tearing_down) {
+		ret = -ESHUTDOWN;
+		goto out;
+	}
 	list_add_tail(&se_cmd->se_cmd_list, &se_sess->sess_cmd_list);
 	se_cmd->check_release = 1;
+
+out:
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
+	return ret;
 }
-EXPORT_SYMBOL(target_get_sess_cmd);
 
 static void target_release_cmd_kref(struct kref *kref)
 {
@@ -3704,28 +2483,27 @@ int target_put_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd)
 }
 EXPORT_SYMBOL(target_put_sess_cmd);
 
-/* target_splice_sess_cmd_list - Split active cmds into sess_wait_list
- * @se_sess:	session to split
+/* target_sess_cmd_list_set_waiting - Flag all commands in
+ *         sess_cmd_list to complete cmd_wait_comp.  Set
+ *         sess_tearing_down so no more commands are queued.
+ * @se_sess:	session to flag
  */
-void target_splice_sess_cmd_list(struct se_session *se_sess)
+void target_sess_cmd_list_set_waiting(struct se_session *se_sess)
 {
 	struct se_cmd *se_cmd;
 	unsigned long flags;
 
-	WARN_ON(!list_empty(&se_sess->sess_wait_list));
-	INIT_LIST_HEAD(&se_sess->sess_wait_list);
-
 	spin_lock_irqsave(&se_sess->sess_cmd_lock, flags);
-	se_sess->sess_tearing_down = 1;
 
-	list_splice_init(&se_sess->sess_cmd_list, &se_sess->sess_wait_list);
+	WARN_ON(se_sess->sess_tearing_down);
+	se_sess->sess_tearing_down = 1;
 
-	list_for_each_entry(se_cmd, &se_sess->sess_wait_list, se_cmd_list)
+	list_for_each_entry(se_cmd, &se_sess->sess_cmd_list, se_cmd_list)
 		se_cmd->cmd_wait_set = 1;
 
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 }
-EXPORT_SYMBOL(target_splice_sess_cmd_list);
+EXPORT_SYMBOL(target_sess_cmd_list_set_waiting);
 
 /* target_wait_for_sess_cmds - Wait for outstanding descriptors
  * @se_sess:    session to wait for active I/O
@@ -3739,7 +2517,7 @@ void target_wait_for_sess_cmds(
 	bool rc = false;
 
 	list_for_each_entry_safe(se_cmd, tmp_cmd,
-				&se_sess->sess_wait_list, se_cmd_list) {
+				&se_sess->sess_cmd_list, se_cmd_list) {
 		list_del(&se_cmd->se_cmd_list);
 
 		pr_debug("Waiting for se_cmd: %p t_state: %d, fabric state:"
@@ -3791,26 +2569,20 @@ static int transport_lun_wait_for_tasks(struct se_cmd *cmd, struct se_lun *lun)
 		pr_debug("ConfigFS ITT[0x%08x] - CMD_T_STOP, skipping\n",
 			 cmd->se_tfo->get_task_tag(cmd));
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-		transport_cmd_check_stop(cmd, 1, 0);
+		transport_cmd_check_stop(cmd, false);
 		return -EPERM;
 	}
 	cmd->transport_state |= CMD_T_LUN_FE_STOP;
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-	wake_up_interruptible(&cmd->se_dev->dev_queue_obj.thread_wq);
-
 	// XXX: audit task_flags checks.
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
 	if ((cmd->transport_state & CMD_T_BUSY) &&
 	    (cmd->transport_state & CMD_T_SENT)) {
 		if (!target_stop_cmd(cmd, &flags))
 			ret++;
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-	} else {
-		spin_unlock_irqrestore(&cmd->t_state_lock,
-				flags);
-		target_remove_from_execute_list(cmd);
 	}
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
 	pr_debug("ConfigFS: cmd: %p stop tasks ret:"
 			" %d\n", cmd, ret);
@@ -3821,7 +2593,6 @@ static int transport_lun_wait_for_tasks(struct se_cmd *cmd, struct se_lun *lun)
 		pr_debug("ConfigFS: ITT[0x%08x] - stopped cmd....\n",
 				cmd->se_tfo->get_task_tag(cmd));
 	}
-	transport_remove_cmd_from_queue(cmd);
 
 	return 0;
 }
@@ -3840,11 +2611,6 @@ static void __transport_clear_lun_from_sessions(struct se_lun *lun)
 		       struct se_cmd, se_lun_node);
 		list_del_init(&cmd->se_lun_node);
 
-		/*
-		 * This will notify iscsi_target_transport.c:
-		 * transport_cmd_check_stop() that a LUN shutdown is in
-		 * progress for the iscsi_cmd_t.
-		 */
 		spin_lock(&cmd->t_state_lock);
 		pr_debug("SE_LUN[%d] - Setting cmd->transport"
 			"_lun_stop for  ITT: 0x%08x\n",
@@ -3911,7 +2677,7 @@ check_cond:
 
 			spin_unlock_irqrestore(&cmd->t_state_lock,
 					cmd_flags);
-			transport_cmd_check_stop(cmd, 1, 0);
+			transport_cmd_check_stop(cmd, false);
 			complete(&cmd->transport_lun_fe_stop_comp);
 			spin_lock_irqsave(&lun->lun_cmd_lock, lun_flags);
 			continue;
@@ -3967,10 +2733,7 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 		return false;
 	}
-	/*
-	 * Only perform a possible wait_for_tasks if SCF_SUPPORTED_SAM_OPCODE
-	 * has been set in transport_set_supported_SAM_opcode().
-	 */
+
 	if (!(cmd->se_cmd_flags & SCF_SUPPORTED_SAM_OPCODE) &&
 	    !(cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)) {
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
@@ -4028,8 +2791,6 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
 
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-	wake_up_interruptible(&cmd->se_dev->dev_queue_obj.thread_wq);
-
 	wait_for_completion(&cmd->t_transport_stop_comp);
 
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
@@ -4212,6 +2973,15 @@ int transport_send_check_condition_and_sense(
 		/* WRITE PROTECTED */
 		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x27;
 		break;
+	case TCM_ADDRESS_OUT_OF_RANGE:
+		/* CURRENT ERROR */
+		buffer[offset] = 0x70;
+		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		/* ILLEGAL REQUEST */
+		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		/* LOGICAL BLOCK ADDRESS OUT OF RANGE */
+		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x21;
+		break;
 	case TCM_CHECK_CONDITION_UNIT_ATTENTION:
 		/* CURRENT ERROR */
 		buffer[offset] = 0x70;
@@ -4312,8 +3082,9 @@ void transport_send_task_abort(struct se_cmd *cmd)
 	cmd->se_tfo->queue_status(cmd);
 }
 
-static int transport_generic_do_tmr(struct se_cmd *cmd)
+static void target_tmr_work(struct work_struct *work)
 {
+	struct se_cmd *cmd = container_of(work, struct se_cmd, work);
 	struct se_device *dev = cmd->se_dev;
 	struct se_tmr_req *tmr = cmd->se_tmr_req;
 	int ret;
@@ -4349,80 +3120,13 @@ static int transport_generic_do_tmr(struct se_cmd *cmd)
 	cmd->se_tfo->queue_tm_rsp(cmd);
 
 	transport_cmd_check_stop_to_fabric(cmd);
-	return 0;
 }
 
-/*	transport_processing_thread():
- *
- *
- */
-static int transport_processing_thread(void *param)
+int transport_generic_handle_tmr(
+	struct se_cmd *cmd)
 {
-	int ret;
-	struct se_cmd *cmd;
-	struct se_device *dev = param;
-
-	while (!kthread_should_stop()) {
-		ret = wait_event_interruptible(dev->dev_queue_obj.thread_wq,
-				atomic_read(&dev->dev_queue_obj.queue_cnt) ||
-				kthread_should_stop());
-		if (ret < 0)
-			goto out;
-
-get_cmd:
-		cmd = transport_get_cmd_from_queue(&dev->dev_queue_obj);
-		if (!cmd)
-			continue;
-
-		switch (cmd->t_state) {
-		case TRANSPORT_NEW_CMD:
-			BUG();
-			break;
-		case TRANSPORT_NEW_CMD_MAP:
-			if (!cmd->se_tfo->new_cmd_map) {
-				pr_err("cmd->se_tfo->new_cmd_map is"
-					" NULL for TRANSPORT_NEW_CMD_MAP\n");
-				BUG();
-			}
-			ret = cmd->se_tfo->new_cmd_map(cmd);
-			if (ret < 0) {
-				transport_generic_request_failure(cmd);
-				break;
-			}
-			ret = transport_generic_new_cmd(cmd);
-			if (ret < 0) {
-				transport_generic_request_failure(cmd);
-				break;
-			}
-			break;
-		case TRANSPORT_PROCESS_WRITE:
-			transport_generic_process_write(cmd);
-			break;
-		case TRANSPORT_PROCESS_TMR:
-			transport_generic_do_tmr(cmd);
-			break;
-		case TRANSPORT_COMPLETE_QF_WP:
-			transport_write_pending_qf(cmd);
-			break;
-		case TRANSPORT_COMPLETE_QF_OK:
-			transport_complete_qf(cmd);
-			break;
-		default:
-			pr_err("Unknown t_state: %d  for ITT: 0x%08x "
-				"i_state: %d on SE LUN: %u\n",
-				cmd->t_state,
-				cmd->se_tfo->get_task_tag(cmd),
-				cmd->se_tfo->get_cmd_state(cmd),
-				cmd->se_lun->unpacked_lun);
-			BUG();
-		}
-
-		goto get_cmd;
-	}
-
-out:
-	WARN_ON(!list_empty(&dev->state_list));
-	WARN_ON(!list_empty(&dev->dev_queue_obj.qobj_list));
-	dev->process_thread = NULL;
+	INIT_WORK(&cmd->work, target_tmr_work);
+	queue_work(cmd->se_dev->tmr_wq, &cmd->work);
 	return 0;
 }
+EXPORT_SYMBOL(transport_generic_handle_tmr);
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index f03fb9730f5..b9cb5006177 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -215,7 +215,7 @@ int ft_write_pending(struct se_cmd *se_cmd)
 		 */
 		if ((ep->xid <= lport->lro_xid) &&
 		    (fh->fh_r_ctl == FC_RCTL_DD_DATA_DESC)) {
-			if ((se_cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) &&
+			if ((se_cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) &&
 			    lport->tt.ddp_target(lport, ep->xid,
 						 se_cmd->t_data_sg,
 						 se_cmd->t_data_nents))
@@ -230,6 +230,8 @@ u32 ft_get_task_tag(struct se_cmd *se_cmd)
 {
 	struct ft_cmd *cmd = container_of(se_cmd, struct ft_cmd, se_cmd);
 
+	if (cmd->aborted)
+		return ~0;
 	return fc_seq_exch(cmd->seq)->rxid;
 }
 
@@ -541,9 +543,11 @@ static void ft_send_work(struct work_struct *work)
 	 * Use a single se_cmd->cmd_kref as we expect to release se_cmd
 	 * directly from ft_check_stop_free callback in response path.
 	 */
-	target_submit_cmd(&cmd->se_cmd, cmd->sess->se_sess, fcp->fc_cdb,
-			&cmd->ft_sense_buffer[0], scsilun_to_int(&fcp->fc_lun),
-			ntohl(fcp->fc_dl), task_attr, data_dir, 0);
+	if (target_submit_cmd(&cmd->se_cmd, cmd->sess->se_sess, fcp->fc_cdb,
+			      &cmd->ft_sense_buffer[0], scsilun_to_int(&fcp->fc_lun),
+			      ntohl(fcp->fc_dl), task_attr, data_dir, 0))
+		goto err;
+
 	pr_debug("r_ctl %x alloc target_submit_cmd\n", fh->fh_r_ctl);
 	return;
 
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index 071a505f98f..ad36ede1a1e 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -183,6 +183,13 @@ int ft_queue_data_in(struct se_cmd *se_cmd)
 	return ft_queue_status(se_cmd);
 }
 
+static void ft_execute_work(struct work_struct *work)
+{
+	struct ft_cmd *cmd = container_of(work, struct ft_cmd, work);
+
+	target_execute_cmd(&cmd->se_cmd);
+}
+
 /*
  * Receive write data frame.
  */
@@ -307,8 +314,10 @@ void ft_recv_write_data(struct ft_cmd *cmd, struct fc_frame *fp)
 		cmd->write_data_len += tlen;
 	}
 last_frame:
-	if (cmd->write_data_len == se_cmd->data_length)
-		transport_generic_handle_data(se_cmd);
+	if (cmd->write_data_len == se_cmd->data_length) {
+		INIT_WORK(&cmd->work, ft_execute_work);
+		queue_work(cmd->sess->tport->tpg->workqueue, &cmd->work);
+	}
 drop:
 	fc_frame_free(fp);
 }
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index 514a691abea..3ab2bd540b5 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -23,6 +23,7 @@ config SPEAR_THERMAL
 	bool "SPEAr thermal sensor driver"
 	depends on THERMAL
 	depends on PLAT_SPEAR
+	depends on OF
 	help
 	  Enable this to plug the SPEAr thermal sensor driver into the Linux
 	  thermal framework
diff --git a/drivers/thermal/spear_thermal.c b/drivers/thermal/spear_thermal.c
index c2e32df3b16..5f8ee39f200 100644
--- a/drivers/thermal/spear_thermal.c
+++ b/drivers/thermal/spear_thermal.c
@@ -20,9 +20,9 @@
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/of.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/platform_data/spear_thermal.h>
 #include <linux/thermal.h>
 
 #define MD_FACTOR	1000
@@ -103,21 +103,20 @@ static int spear_thermal_probe(struct platform_device *pdev)
 {
 	struct thermal_zone_device *spear_thermal = NULL;
 	struct spear_thermal_dev *stdev;
-	struct spear_thermal_pdata *pdata;
-	int ret = 0;
+	struct device_node *np = pdev->dev.of_node;
 	struct resource *stres = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	int ret = 0, val;
+
+	if (!np || !of_property_read_u32(np, "st,thermal-flags", &val)) {
+		dev_err(&pdev->dev, "Failed: DT Pdata not passed\n");
+		return -EINVAL;
+	}
 
 	if (!stres) {
 		dev_err(&pdev->dev, "memory resource missing\n");
 		return -ENODEV;
 	}
 
-	pdata = dev_get_platdata(&pdev->dev);
-	if (!pdata) {
-		dev_err(&pdev->dev, "platform data is NULL\n");
-		return -EINVAL;
-	}
-
 	stdev = devm_kzalloc(&pdev->dev, sizeof(*stdev), GFP_KERNEL);
 	if (!stdev) {
 		dev_err(&pdev->dev, "kzalloc fail\n");
@@ -144,10 +143,10 @@ static int spear_thermal_probe(struct platform_device *pdev)
 		goto put_clk;
 	}
 
-	stdev->flags = pdata->thermal_flags;
+	stdev->flags = val;
 	writel_relaxed(stdev->flags, stdev->thermal_base);
 
-	spear_thermal = thermal_zone_device_register("spear_thermal", 0,
+	spear_thermal = thermal_zone_device_register("spear_thermal", 0, 0,
 				stdev, &ops, 0, 0, 0, 0);
 	if (IS_ERR(spear_thermal)) {
 		dev_err(&pdev->dev, "thermal zone device is NULL\n");
@@ -189,6 +188,12 @@ static int spear_thermal_exit(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id spear_thermal_id_table[] = {
+	{ .compatible = "st,thermal-spear1340" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, spear_thermal_id_table);
+
 static struct platform_driver spear_thermal_driver = {
 	.probe = spear_thermal_probe,
 	.remove = spear_thermal_exit,
@@ -196,6 +201,7 @@ static struct platform_driver spear_thermal_driver = {
 		.name = "spear_thermal",
 		.owner = THIS_MODULE,
 		.pm = &spear_thermal_pm_ops,
+		.of_match_table = of_match_ptr(spear_thermal_id_table),
 	},
 };
 
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_sys.c
index 022bacb71a7..2ab31e4f02c 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_sys.c
@@ -196,6 +196,28 @@ trip_point_type_show(struct device *dev, struct device_attribute *attr,
 }
 
 static ssize_t
+trip_point_temp_store(struct device *dev, struct device_attribute *attr,
+		     const char *buf, size_t count)
+{
+	struct thermal_zone_device *tz = to_thermal_zone(dev);
+	int trip, ret;
+	unsigned long temperature;
+
+	if (!tz->ops->set_trip_temp)
+		return -EPERM;
+
+	if (!sscanf(attr->attr.name, "trip_point_%d_temp", &trip))
+		return -EINVAL;
+
+	if (kstrtoul(buf, 10, &temperature))
+		return -EINVAL;
+
+	ret = tz->ops->set_trip_temp(tz, trip, temperature);
+
+	return ret ? ret : count;
+}
+
+static ssize_t
 trip_point_temp_show(struct device *dev, struct device_attribute *attr,
 		     char *buf)
 {
@@ -218,6 +240,52 @@ trip_point_temp_show(struct device *dev, struct device_attribute *attr,
 }
 
 static ssize_t
+trip_point_hyst_store(struct device *dev, struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct thermal_zone_device *tz = to_thermal_zone(dev);
+	int trip, ret;
+	unsigned long temperature;
+
+	if (!tz->ops->set_trip_hyst)
+		return -EPERM;
+
+	if (!sscanf(attr->attr.name, "trip_point_%d_hyst", &trip))
+		return -EINVAL;
+
+	if (kstrtoul(buf, 10, &temperature))
+		return -EINVAL;
+
+	/*
+	 * We are not doing any check on the 'temperature' value
+	 * here. The driver implementing 'set_trip_hyst' has to
+	 * take care of this.
+	 */
+	ret = tz->ops->set_trip_hyst(tz, trip, temperature);
+
+	return ret ? ret : count;
+}
+
+static ssize_t
+trip_point_hyst_show(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	struct thermal_zone_device *tz = to_thermal_zone(dev);
+	int trip, ret;
+	unsigned long temperature;
+
+	if (!tz->ops->get_trip_hyst)
+		return -EPERM;
+
+	if (!sscanf(attr->attr.name, "trip_point_%d_hyst", &trip))
+		return -EINVAL;
+
+	ret = tz->ops->get_trip_hyst(tz, trip, &temperature);
+
+	return ret ? ret : sprintf(buf, "%ld\n", temperature);
+}
+
+static ssize_t
 passive_store(struct device *dev, struct device_attribute *attr,
 		    const char *buf, size_t count)
 {
@@ -283,33 +351,6 @@ static DEVICE_ATTR(temp, 0444, temp_show, NULL);
 static DEVICE_ATTR(mode, 0644, mode_show, mode_store);
 static DEVICE_ATTR(passive, S_IRUGO | S_IWUSR, passive_show, passive_store);
 
-static struct device_attribute trip_point_attrs[] = {
-	__ATTR(trip_point_0_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_0_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_1_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_1_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_2_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_2_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_3_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_3_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_4_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_4_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_5_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_5_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_6_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_6_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_7_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_7_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_8_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_8_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_9_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_9_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_10_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_10_temp, 0444, trip_point_temp_show, NULL),
-	__ATTR(trip_point_11_type, 0444, trip_point_type_show, NULL),
-	__ATTR(trip_point_11_temp, 0444, trip_point_temp_show, NULL),
-};
-
 /* sys I/F for cooling device */
 #define to_cooling_device(_dev)	\
 	container_of(_dev, struct thermal_cooling_device, device)
@@ -1089,9 +1130,113 @@ leave:
 EXPORT_SYMBOL(thermal_zone_device_update);
 
 /**
+ * create_trip_attrs - create attributes for trip points
+ * @tz:		the thermal zone device
+ * @mask:	Writeable trip point bitmap.
+ */
+static int create_trip_attrs(struct thermal_zone_device *tz, int mask)
+{
+	int indx;
+	int size = sizeof(struct thermal_attr) * tz->trips;
+
+	tz->trip_type_attrs = kzalloc(size, GFP_KERNEL);
+	if (!tz->trip_type_attrs)
+		return -ENOMEM;
+
+	tz->trip_temp_attrs = kzalloc(size, GFP_KERNEL);
+	if (!tz->trip_temp_attrs) {
+		kfree(tz->trip_type_attrs);
+		return -ENOMEM;
+	}
+
+	if (tz->ops->get_trip_hyst) {
+		tz->trip_hyst_attrs = kzalloc(size, GFP_KERNEL);
+		if (!tz->trip_hyst_attrs) {
+			kfree(tz->trip_type_attrs);
+			kfree(tz->trip_temp_attrs);
+			return -ENOMEM;
+		}
+	}
+
+
+	for (indx = 0; indx < tz->trips; indx++) {
+		/* create trip type attribute */
+		snprintf(tz->trip_type_attrs[indx].name, THERMAL_NAME_LENGTH,
+			 "trip_point_%d_type", indx);
+
+		sysfs_attr_init(&tz->trip_type_attrs[indx].attr.attr);
+		tz->trip_type_attrs[indx].attr.attr.name =
+						tz->trip_type_attrs[indx].name;
+		tz->trip_type_attrs[indx].attr.attr.mode = S_IRUGO;
+		tz->trip_type_attrs[indx].attr.show = trip_point_type_show;
+
+		device_create_file(&tz->device,
+				   &tz->trip_type_attrs[indx].attr);
+
+		/* create trip temp attribute */
+		snprintf(tz->trip_temp_attrs[indx].name, THERMAL_NAME_LENGTH,
+			 "trip_point_%d_temp", indx);
+
+		sysfs_attr_init(&tz->trip_temp_attrs[indx].attr.attr);
+		tz->trip_temp_attrs[indx].attr.attr.name =
+						tz->trip_temp_attrs[indx].name;
+		tz->trip_temp_attrs[indx].attr.attr.mode = S_IRUGO;
+		tz->trip_temp_attrs[indx].attr.show = trip_point_temp_show;
+		if (mask & (1 << indx)) {
+			tz->trip_temp_attrs[indx].attr.attr.mode |= S_IWUSR;
+			tz->trip_temp_attrs[indx].attr.store =
+							trip_point_temp_store;
+		}
+
+		device_create_file(&tz->device,
+				   &tz->trip_temp_attrs[indx].attr);
+
+		/* create Optional trip hyst attribute */
+		if (!tz->ops->get_trip_hyst)
+			continue;
+		snprintf(tz->trip_hyst_attrs[indx].name, THERMAL_NAME_LENGTH,
+			 "trip_point_%d_hyst", indx);
+
+		sysfs_attr_init(&tz->trip_hyst_attrs[indx].attr.attr);
+		tz->trip_hyst_attrs[indx].attr.attr.name =
+					tz->trip_hyst_attrs[indx].name;
+		tz->trip_hyst_attrs[indx].attr.attr.mode = S_IRUGO;
+		tz->trip_hyst_attrs[indx].attr.show = trip_point_hyst_show;
+		if (tz->ops->set_trip_hyst) {
+			tz->trip_hyst_attrs[indx].attr.attr.mode |= S_IWUSR;
+			tz->trip_hyst_attrs[indx].attr.store =
+					trip_point_hyst_store;
+		}
+
+		device_create_file(&tz->device,
+				   &tz->trip_hyst_attrs[indx].attr);
+	}
+	return 0;
+}
+
+static void remove_trip_attrs(struct thermal_zone_device *tz)
+{
+	int indx;
+
+	for (indx = 0; indx < tz->trips; indx++) {
+		device_remove_file(&tz->device,
+				   &tz->trip_type_attrs[indx].attr);
+		device_remove_file(&tz->device,
+				   &tz->trip_temp_attrs[indx].attr);
+		if (tz->ops->get_trip_hyst)
+			device_remove_file(&tz->device,
+				  &tz->trip_hyst_attrs[indx].attr);
+	}
+	kfree(tz->trip_type_attrs);
+	kfree(tz->trip_temp_attrs);
+	kfree(tz->trip_hyst_attrs);
+}
+
+/**
  * thermal_zone_device_register - register a new thermal zone device
  * @type:	the thermal zone device type
  * @trips:	the number of trip points the thermal zone support
+ * @mask:	a bit string indicating the writeablility of trip points
  * @devdata:	private device data
  * @ops:	standard thermal zone device callbacks
  * @tc1:	thermal coefficient 1 for passive calculations
@@ -1106,8 +1251,8 @@ EXPORT_SYMBOL(thermal_zone_device_update);
  * longer needed. The passive cooling formula uses tc1 and tc2 as described in
  * section 11.1.5.1 of the ACPI specification 3.0.
  */
-struct thermal_zone_device *thermal_zone_device_register(char *type,
-	int trips, void *devdata,
+struct thermal_zone_device *thermal_zone_device_register(const char *type,
+	int trips, int mask, void *devdata,
 	const struct thermal_zone_device_ops *ops,
 	int tc1, int tc2, int passive_delay, int polling_delay)
 {
@@ -1121,7 +1266,7 @@ struct thermal_zone_device *thermal_zone_device_register(char *type,
 	if (strlen(type) >= THERMAL_NAME_LENGTH)
 		return ERR_PTR(-EINVAL);
 
-	if (trips > THERMAL_MAX_TRIPS || trips < 0)
+	if (trips > THERMAL_MAX_TRIPS || trips < 0 || mask >> trips)
 		return ERR_PTR(-EINVAL);
 
 	if (!ops || !ops->get_temp)
@@ -1175,15 +1320,11 @@ struct thermal_zone_device *thermal_zone_device_register(char *type,
 			goto unregister;
 	}
 
+	result = create_trip_attrs(tz, mask);
+	if (result)
+		goto unregister;
+
 	for (count = 0; count < trips; count++) {
-		result = device_create_file(&tz->device,
-					    &trip_point_attrs[count * 2]);
-		if (result)
-			break;
-		result = device_create_file(&tz->device,
-					    &trip_point_attrs[count * 2 + 1]);
-		if (result)
-			goto unregister;
 		tz->ops->get_trip_type(tz, count, &trip_type);
 		if (trip_type == THERMAL_TRIP_PASSIVE)
 			passive = 1;
@@ -1232,7 +1373,6 @@ void thermal_zone_device_unregister(struct thermal_zone_device *tz)
 {
 	struct thermal_cooling_device *cdev;
 	struct thermal_zone_device *pos = NULL;
-	int count;
 
 	if (!tz)
 		return;
@@ -1259,13 +1399,8 @@ void thermal_zone_device_unregister(struct thermal_zone_device *tz)
 	device_remove_file(&tz->device, &dev_attr_temp);
 	if (tz->ops->get_mode)
 		device_remove_file(&tz->device, &dev_attr_mode);
+	remove_trip_attrs(tz);
 
-	for (count = 0; count < tz->trips; count++) {
-		device_remove_file(&tz->device,
-				   &trip_point_attrs[count * 2]);
-		device_remove_file(&tz->device,
-				   &trip_point_attrs[count * 2 + 1]);
-	}
 	thermal_remove_hwmon_sysfs(tz);
 	release_idr(&thermal_tz_idr, &thermal_idr_lock, tz->id);
 	idr_destroy(&tz->idr);
diff --git a/drivers/tty/hvc/hvc_xen.c b/drivers/tty/hvc/hvc_xen.c
index 944eaeb8e0c..1e456dca4f6 100644
--- a/drivers/tty/hvc/hvc_xen.c
+++ b/drivers/tty/hvc/hvc_xen.c
@@ -209,11 +209,10 @@ static int xen_hvm_console_init(void)
 		info = kzalloc(sizeof(struct xencons_info), GFP_KERNEL | __GFP_ZERO);
 		if (!info)
 			return -ENOMEM;
-	}
-
-	/* already configured */
-	if (info->intf != NULL)
+	} else if (info->intf != NULL) {
+		/* already configured */
 		return 0;
+	}
 	/*
 	 * If the toolstack (or the hypervisor) hasn't set these values, the
 	 * default value is 0. Even though mfn = 0 and evtchn = 0 are
@@ -259,12 +258,10 @@ static int xen_pv_console_init(void)
 		info = kzalloc(sizeof(struct xencons_info), GFP_KERNEL | __GFP_ZERO);
 		if (!info)
 			return -ENOMEM;
-	}
-
-	/* already configured */
-	if (info->intf != NULL)
+	} else if (info->intf != NULL) {
+		/* already configured */
 		return 0;
-
+	}
 	info->evtchn = xen_start_info->console.domU.evtchn;
 	info->intf = mfn_to_virt(xen_start_info->console.domU.mfn);
 	info->vtermno = HVC_COOKIE;
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index b81cc31f265..92b1ac8db63 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -53,9 +53,9 @@
 #include <linux/delay.h>
 #include <linux/types.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/sizes.h>
 
 #include <asm/io.h>
-#include <asm/sizes.h>
 
 #define UART_NR			14
 
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 0af4eec8c7b..20e91172402 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -169,7 +169,6 @@
 #define SERIAL_IMX_MAJOR        207
 #define MINOR_START	        16
 #define DEV_NAME		"ttymxc"
-#define MAX_INTERNAL_IRQ	MXC_INTERNAL_IRQS
 
 /*
  * This determines how often we check the modem status signals
@@ -741,10 +740,7 @@ static int imx_startup(struct uart_port *port)
 
 		/* do not use RTS IRQ on IrDA */
 		if (!USE_IRDA(sport)) {
-			retval = request_irq(sport->rtsirq, imx_rtsint,
-				     (sport->rtsirq < MAX_INTERNAL_IRQ) ? 0 :
-				       IRQF_TRIGGER_FALLING |
-				       IRQF_TRIGGER_RISING,
+			retval = request_irq(sport->rtsirq, imx_rtsint, 0,
 					DRIVER_NAME, sport);
 			if (retval)
 				goto error_out3;
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index ec56d8397aa..2e341b81ff8 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -33,6 +33,7 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/of_device.h>
 
 #include <asm/cacheflush.h>
 
@@ -675,6 +676,30 @@ static struct uart_driver auart_driver = {
 #endif
 };
 
+/*
+ * This function returns 1 if pdev isn't a device instatiated by dt, 0 if it
+ * could successfully get all information from dt or a negative errno.
+ */
+static int serial_mxs_probe_dt(struct mxs_auart_port *s,
+		struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	int ret;
+
+	if (!np)
+		/* no device tree device */
+		return 1;
+
+	ret = of_alias_get_id(np, "serial");
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to get alias id: %d\n", ret);
+		return ret;
+	}
+	s->port.line = ret;
+
+	return 0;
+}
+
 static int __devinit mxs_auart_probe(struct platform_device *pdev)
 {
 	struct mxs_auart_port *s;
@@ -689,6 +714,12 @@ static int __devinit mxs_auart_probe(struct platform_device *pdev)
 		goto out;
 	}
 
+	ret = serial_mxs_probe_dt(s, pdev);
+	if (ret > 0)
+		s->port.line = pdev->id < 0 ? 0 : pdev->id;
+	else if (ret < 0)
+		goto out_free;
+
 	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
 	if (IS_ERR(pinctrl)) {
 		ret = PTR_ERR(pinctrl);
@@ -711,7 +742,6 @@ static int __devinit mxs_auart_probe(struct platform_device *pdev)
 	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;
@@ -728,7 +758,7 @@ static int __devinit mxs_auart_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, s);
 
-	auart_port[pdev->id] = s;
+	auart_port[s->port.line] = s;
 
 	mxs_auart_reset(&s->port);
 
@@ -769,12 +799,19 @@ static int __devexit mxs_auart_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static struct of_device_id mxs_auart_dt_ids[] = {
+	{ .compatible = "fsl,imx23-auart", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_auart_dt_ids);
+
 static struct platform_driver mxs_auart_driver = {
 	.probe = mxs_auart_probe,
 	.remove = __devexit_p(mxs_auart_remove),
 	.driver = {
 		.name = "mxs-auart",
 		.owner = THIS_MODULE,
+		.of_match_table = mxs_auart_dt_ids,
 	},
 };
 
@@ -807,3 +844,4 @@ module_init(mxs_auart_init);
 module_exit(mxs_auart_exit);
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Freescale MXS application uart driver");
+MODULE_ALIAS("platform:mxs-auart");
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 1bd9163bc11..9be296cf729 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -25,6 +25,7 @@
 
 #include <linux/module.h>
 #include <linux/errno.h>
+#include <linux/sh_dma.h>
 #include <linux/timer.h>
 #include <linux/interrupt.h>
 #include <linux/tty.h>
@@ -1410,8 +1411,8 @@ static void work_fn_rx(struct work_struct *work)
 		/* Handle incomplete DMA receive */
 		struct tty_struct *tty = port->state->port.tty;
 		struct dma_chan *chan = s->chan_rx;
-		struct sh_desc *sh_desc = container_of(desc, struct sh_desc,
-						       async_tx);
+		struct shdma_desc *sh_desc = container_of(desc,
+					struct shdma_desc, async_tx);
 		unsigned long flags;
 		int count;
 
@@ -1615,9 +1616,9 @@ static bool filter(struct dma_chan *chan, void *slave)
 	struct sh_dmae_slave *param = slave;
 
 	dev_dbg(chan->device->dev, "%s: slave ID %d\n", __func__,
-		param->slave_id);
+		param->shdma_slave.slave_id);
 
-	chan->private = param;
+	chan->private = &param->shdma_slave;
 	return true;
 }
 
@@ -1656,7 +1657,7 @@ 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->cfg->dma_slave_tx;
+	param->shdma_slave.slave_id = s->cfg->dma_slave_tx;
 
 	s->cookie_tx = -EINVAL;
 	chan = dma_request_channel(mask, filter, param);
@@ -1684,7 +1685,7 @@ 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->cfg->dma_slave_rx;
+	param->shdma_slave.slave_id = s->cfg->dma_slave_rx;
 
 	chan = dma_request_channel(mask, filter, param);
 	dev_dbg(port->dev, "%s: RX: got channel %p\n", __func__, chan);
diff --git a/drivers/tty/serial/uartlite.c b/drivers/tty/serial/uartlite.c
index 6cd414341d5..6579ffdd8e9 100644
--- a/drivers/tty/serial/uartlite.c
+++ b/drivers/tty/serial/uartlite.c
@@ -216,8 +216,7 @@ static int ulite_startup(struct uart_port *port)
 {
 	int ret;
 
-	ret = request_irq(port->irq, ulite_isr,
-			  IRQF_SHARED | IRQF_SAMPLE_RANDOM, "uartlite", port);
+	ret = request_irq(port->irq, ulite_isr, IRQF_SHARED, "uartlite", port);
 	if (ret)
 		return ret;
 
diff --git a/drivers/usb/atm/xusbatm.c b/drivers/usb/atm/xusbatm.c
index 14ec9f0c592..b3b1bb78b2e 100644
--- a/drivers/usb/atm/xusbatm.c
+++ b/drivers/usb/atm/xusbatm.c
@@ -20,7 +20,7 @@
  ******************************************************************************/
 
 #include <linux/module.h>
-#include <linux/etherdevice.h>		/* for random_ether_addr() */
+#include <linux/etherdevice.h>		/* for eth_random_addr() */
 
 #include "usbatm.h"
 
@@ -163,7 +163,7 @@ static int xusbatm_atm_start(struct usbatm_data *usbatm,
 	atm_dbg(usbatm, "%s entered\n", __func__);
 
 	/* use random MAC as we've no way to get it from the device */
-	random_ether_addr(atm_dev->esi);
+	eth_random_addr(atm_dev->esi);
 
 	return 0;
 }
diff --git a/drivers/usb/chipidea/Kconfig b/drivers/usb/chipidea/Kconfig
index fd36dc8b889..8337fb5d988 100644
--- a/drivers/usb/chipidea/Kconfig
+++ b/drivers/usb/chipidea/Kconfig
@@ -20,6 +20,7 @@ config USB_CHIPIDEA_UDC
 
 config USB_CHIPIDEA_HOST
 	bool "ChipIdea host controller"
+	select USB_EHCI_ROOT_HUB_TT
 	help
 	  Say Y here to enable host controller functionality of the
 	  ChipIdea driver.
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index cc349376972..5c66d9c330c 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -5,10 +5,15 @@ ci_hdrc-$(CONFIG_USB_CHIPIDEA_UDC)	+= udc.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_HOST)	+= host.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_DEBUG)	+= debug.o
 
+# Glue/Bridge layers go here
+
+obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_msm.o
+
+# PCI doesn't provide stubs, need to check
 ifneq ($(CONFIG_PCI),)
 	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_pci.o
 endif
 
-ifneq ($(CONFIG_ARCH_MSM),)
-	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_msm.o
+ifneq ($(CONFIG_OF_DEVICE),)
+	obj-$(CONFIG_USB_CHIPIDEA)	+= ci13xxx_imx.o
 endif
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index 50911f8490d..d738603a275 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -36,7 +36,7 @@
  * @name: string description of the endpoint
  * @qh: queue head for this endpoint
  * @wedge: is the endpoint wedged
- * @udc: pointer to the controller
+ * @ci: pointer to the controller
  * @lock: pointer to controller's spinlock
  * @td_pool: pointer to controller's TD pool
  */
@@ -54,7 +54,7 @@ struct ci13xxx_ep {
 	int					wedge;
 
 	/* global resources */
-	struct ci13xxx				*udc;
+	struct ci13xxx				*ci;
 	spinlock_t				*lock;
 	struct dma_pool				*td_pool;
 };
@@ -125,7 +125,7 @@ struct hw_bank {
  * @remote_wakeup: host-enabled remote wakeup
  * @suspended: suspended by host
  * @test_mode: the selected test mode
- * @udc_driver: platform specific information supplied by parent device
+ * @platdata: platform specific information supplied by parent device
  * @vbus_active: is VBUS active
  * @transceiver: pointer to USB PHY, if any
  * @hcd: pointer to usb_hcd for ehci host driver
@@ -158,8 +158,10 @@ struct ci13xxx {
 	u8				suspended;
 	u8				test_mode;
 
-	struct ci13xxx_udc_driver	*udc_driver;
+	struct ci13xxx_platform_data	*platdata;
 	int				vbus_active;
+	/* FIXME: some day, we'll not use global phy */
+	bool				global_phy;
 	struct usb_phy			*transceiver;
 	struct usb_hcd			*hcd;
 };
@@ -250,9 +252,9 @@ static inline int ffs_nr(u32 x)
  *
  * This function returns register contents
  */
-static inline u32 hw_read(struct ci13xxx *udc, enum ci13xxx_regs reg, u32 mask)
+static inline u32 hw_read(struct ci13xxx *ci, enum ci13xxx_regs reg, u32 mask)
 {
-	return ioread32(udc->hw_bank.regmap[reg]) & mask;
+	return ioread32(ci->hw_bank.regmap[reg]) & mask;
 }
 
 /**
@@ -261,14 +263,14 @@ static inline u32 hw_read(struct ci13xxx *udc, enum ci13xxx_regs reg, u32 mask)
  * @mask: bitfield mask
  * @data: new value
  */
-static inline void hw_write(struct ci13xxx *udc, enum ci13xxx_regs reg,
+static inline void hw_write(struct ci13xxx *ci, enum ci13xxx_regs reg,
 			    u32 mask, u32 data)
 {
 	if (~mask)
-		data = (ioread32(udc->hw_bank.regmap[reg]) & ~mask)
+		data = (ioread32(ci->hw_bank.regmap[reg]) & ~mask)
 			| (data & mask);
 
-	iowrite32(data, udc->hw_bank.regmap[reg]);
+	iowrite32(data, ci->hw_bank.regmap[reg]);
 }
 
 /**
@@ -278,12 +280,12 @@ static inline void hw_write(struct ci13xxx *udc, enum ci13xxx_regs reg,
  *
  * This function returns register contents
  */
-static inline u32 hw_test_and_clear(struct ci13xxx *udc, enum ci13xxx_regs reg,
+static inline u32 hw_test_and_clear(struct ci13xxx *ci, enum ci13xxx_regs reg,
 				    u32 mask)
 {
-	u32 val = ioread32(udc->hw_bank.regmap[reg]) & mask;
+	u32 val = ioread32(ci->hw_bank.regmap[reg]) & mask;
 
-	iowrite32(val, udc->hw_bank.regmap[reg]);
+	iowrite32(val, ci->hw_bank.regmap[reg]);
 	return val;
 }
 
@@ -295,12 +297,12 @@ static inline u32 hw_test_and_clear(struct ci13xxx *udc, enum ci13xxx_regs reg,
  *
  * This function returns register contents
  */
-static inline u32 hw_test_and_write(struct ci13xxx *udc, enum ci13xxx_regs reg,
+static inline u32 hw_test_and_write(struct ci13xxx *ci, enum ci13xxx_regs reg,
 				    u32 mask, u32 data)
 {
-	u32 val = hw_read(udc, reg, ~0);
+	u32 val = hw_read(ci, reg, ~0);
 
-	hw_write(udc, reg, mask, data);
+	hw_write(ci, reg, mask, data);
 	return (val & mask) >> ffs_nr(mask);
 }
 
diff --git a/drivers/usb/chipidea/ci13xxx_imx.c b/drivers/usb/chipidea/ci13xxx_imx.c
new file mode 100644
index 00000000000..ef60d06835d
--- /dev/null
+++ b/drivers/usb/chipidea/ci13xxx_imx.c
@@ -0,0 +1,198 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Marek Vasut <marex@denx.de>
+ * on behalf of DENX Software Engineering GmbH
+ *
+ * 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/module.h>
+#include <linux/of_platform.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/dma-mapping.h>
+#include <linux/usb/chipidea.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include "ci.h"
+
+#define pdev_to_phy(pdev) \
+	((struct usb_phy *)platform_get_drvdata(pdev))
+
+struct ci13xxx_imx_data {
+	struct device_node *phy_np;
+	struct usb_phy *phy;
+	struct platform_device *ci_pdev;
+	struct clk *clk;
+	struct regulator *reg_vbus;
+};
+
+static struct ci13xxx_platform_data ci13xxx_imx_platdata __devinitdata  = {
+	.name			= "ci13xxx_imx",
+	.flags			= CI13XXX_REQUIRE_TRANSCEIVER |
+				  CI13XXX_PULLUP_ON_VBUS |
+				  CI13XXX_DISABLE_STREAMING,
+	.capoffset		= DEF_CAPOFFSET,
+};
+
+static int __devinit ci13xxx_imx_probe(struct platform_device *pdev)
+{
+	struct ci13xxx_imx_data *data;
+	struct platform_device *plat_ci, *phy_pdev;
+	struct device_node *phy_np;
+	struct resource *res;
+	struct regulator *reg_vbus;
+	int ret;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "Failed to allocate CI13xxx-IMX data!\n");
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Can't get device resources!\n");
+		return -ENOENT;
+	}
+
+	data->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(data->clk)) {
+		dev_err(&pdev->dev,
+			"Failed to get clock, err=%ld\n", PTR_ERR(data->clk));
+		return PTR_ERR(data->clk);
+	}
+
+	ret = clk_prepare_enable(data->clk);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Failed to prepare or enable clock, err=%d\n", ret);
+		return ret;
+	}
+
+	phy_np = of_parse_phandle(pdev->dev.of_node, "fsl,usbphy", 0);
+	if (phy_np) {
+		data->phy_np = phy_np;
+		phy_pdev = of_find_device_by_node(phy_np);
+		if (phy_pdev) {
+			struct usb_phy *phy;
+			phy = pdev_to_phy(phy_pdev);
+			if (phy &&
+			    try_module_get(phy_pdev->dev.driver->owner)) {
+				usb_phy_init(phy);
+				data->phy = phy;
+			}
+		}
+	}
+
+	/* we only support host now, so enable vbus here */
+	reg_vbus = devm_regulator_get(&pdev->dev, "vbus");
+	if (!IS_ERR(reg_vbus)) {
+		ret = regulator_enable(reg_vbus);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Failed to enable vbus regulator, err=%d\n",
+				ret);
+			goto put_np;
+		}
+		data->reg_vbus = reg_vbus;
+	} else {
+		reg_vbus = NULL;
+	}
+
+	ci13xxx_imx_platdata.phy = data->phy;
+
+	if (!pdev->dev.dma_mask) {
+		pdev->dev.dma_mask = devm_kzalloc(&pdev->dev,
+				      sizeof(*pdev->dev.dma_mask), GFP_KERNEL);
+		if (!pdev->dev.dma_mask) {
+			ret = -ENOMEM;
+			dev_err(&pdev->dev, "Failed to alloc dma_mask!\n");
+			goto err;
+		}
+		*pdev->dev.dma_mask = DMA_BIT_MASK(32);
+		dma_set_coherent_mask(&pdev->dev, *pdev->dev.dma_mask);
+	}
+	plat_ci = ci13xxx_add_device(&pdev->dev,
+				pdev->resource, pdev->num_resources,
+				&ci13xxx_imx_platdata);
+	if (IS_ERR(plat_ci)) {
+		ret = PTR_ERR(plat_ci);
+		dev_err(&pdev->dev,
+			"Can't register ci_hdrc platform device, err=%d\n",
+			ret);
+		goto err;
+	}
+
+	data->ci_pdev = plat_ci;
+	platform_set_drvdata(pdev, data);
+
+	pm_runtime_no_callbacks(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	return 0;
+
+err:
+	if (reg_vbus)
+		regulator_disable(reg_vbus);
+put_np:
+	if (phy_np)
+		of_node_put(phy_np);
+	clk_disable_unprepare(data->clk);
+	return ret;
+}
+
+static int __devexit ci13xxx_imx_remove(struct platform_device *pdev)
+{
+	struct ci13xxx_imx_data *data = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+	ci13xxx_remove_device(data->ci_pdev);
+
+	if (data->reg_vbus)
+		regulator_disable(data->reg_vbus);
+
+	if (data->phy) {
+		usb_phy_shutdown(data->phy);
+		module_put(data->phy->dev->driver->owner);
+	}
+
+	of_node_put(data->phy_np);
+
+	clk_disable_unprepare(data->clk);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id ci13xxx_imx_dt_ids[] = {
+	{ .compatible = "fsl,imx27-usb", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ci13xxx_imx_dt_ids);
+
+static struct platform_driver ci13xxx_imx_driver = {
+	.probe = ci13xxx_imx_probe,
+	.remove = __devexit_p(ci13xxx_imx_remove),
+	.driver = {
+		.name = "imx_usb",
+		.owner = THIS_MODULE,
+		.of_match_table = ci13xxx_imx_dt_ids,
+	 },
+};
+
+module_platform_driver(ci13xxx_imx_driver);
+
+MODULE_ALIAS("platform:imx-usb");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CI13xxx i.MX USB binding");
+MODULE_AUTHOR("Marek Vasut <marex@denx.de>");
+MODULE_AUTHOR("Richard Zhao <richard.zhao@freescale.com>");
diff --git a/drivers/usb/chipidea/ci13xxx_msm.c b/drivers/usb/chipidea/ci13xxx_msm.c
index 958069ef95e..b01feb3be92 100644
--- a/drivers/usb/chipidea/ci13xxx_msm.c
+++ b/drivers/usb/chipidea/ci13xxx_msm.c
@@ -15,11 +15,11 @@
 
 #include "ci.h"
 
-#define MSM_USB_BASE	(udc->hw_bank.abs)
+#define MSM_USB_BASE	(ci->hw_bank.abs)
 
-static void ci13xxx_msm_notify_event(struct ci13xxx *udc, unsigned event)
+static void ci13xxx_msm_notify_event(struct ci13xxx *ci, unsigned event)
 {
-	struct device *dev = udc->gadget.dev.parent;
+	struct device *dev = ci->gadget.dev.parent;
 	int val;
 
 	switch (event) {
@@ -34,18 +34,18 @@ static void ci13xxx_msm_notify_event(struct ci13xxx *udc, unsigned event)
 		 * Put the transceiver in non-driving mode. Otherwise host
 		 * may not detect soft-disconnection.
 		 */
-		val = usb_phy_io_read(udc->transceiver, ULPI_FUNC_CTRL);
+		val = usb_phy_io_read(ci->transceiver, ULPI_FUNC_CTRL);
 		val &= ~ULPI_FUNC_CTRL_OPMODE_MASK;
 		val |= ULPI_FUNC_CTRL_OPMODE_NONDRIVING;
-		usb_phy_io_write(udc->transceiver, val, ULPI_FUNC_CTRL);
+		usb_phy_io_write(ci->transceiver, val, ULPI_FUNC_CTRL);
 		break;
 	default:
-		dev_dbg(dev, "unknown ci13xxx_udc event\n");
+		dev_dbg(dev, "unknown ci13xxx event\n");
 		break;
 	}
 }
 
-static struct ci13xxx_udc_driver ci13xxx_msm_udc_driver = {
+static struct ci13xxx_platform_data ci13xxx_msm_platdata = {
 	.name			= "ci13xxx_msm",
 	.flags			= CI13XXX_REGS_SHARED |
 				  CI13XXX_REQUIRE_TRANSCEIVER |
@@ -55,56 +55,45 @@ static struct ci13xxx_udc_driver ci13xxx_msm_udc_driver = {
 	.notify_event		= ci13xxx_msm_notify_event,
 };
 
-static int ci13xxx_msm_probe(struct platform_device *pdev)
+static int __devinit ci13xxx_msm_probe(struct platform_device *pdev)
 {
 	struct platform_device *plat_ci;
-	int ret;
 
 	dev_dbg(&pdev->dev, "ci13xxx_msm_probe\n");
 
-	plat_ci = platform_device_alloc("ci_hdrc", -1);
-	if (!plat_ci) {
-		dev_err(&pdev->dev, "can't allocate ci_hdrc platform device\n");
-		return -ENOMEM;
+	plat_ci = ci13xxx_add_device(&pdev->dev,
+				pdev->resource, pdev->num_resources,
+				&ci13xxx_msm_platdata);
+	if (IS_ERR(plat_ci)) {
+		dev_err(&pdev->dev, "ci13xxx_add_device failed!\n");
+		return PTR_ERR(plat_ci);
 	}
 
-	ret = platform_device_add_resources(plat_ci, pdev->resource,
-					    pdev->num_resources);
-	if (ret) {
-		dev_err(&pdev->dev, "can't add resources to platform device\n");
-		goto put_platform;
-	}
-
-	ret = platform_device_add_data(plat_ci, &ci13xxx_msm_udc_driver,
-				       sizeof(ci13xxx_msm_udc_driver));
-	if (ret)
-		goto put_platform;
-
-	ret = platform_device_add(plat_ci);
-	if (ret)
-		goto put_platform;
+	platform_set_drvdata(pdev, plat_ci);
 
 	pm_runtime_no_callbacks(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
 	return 0;
+}
+
+static int __devexit ci13xxx_msm_remove(struct platform_device *pdev)
+{
+	struct platform_device *plat_ci = platform_get_drvdata(pdev);
 
-put_platform:
-	platform_device_put(plat_ci);
+	pm_runtime_disable(&pdev->dev);
+	ci13xxx_remove_device(plat_ci);
 
-	return ret;
+	return 0;
 }
 
 static struct platform_driver ci13xxx_msm_driver = {
 	.probe = ci13xxx_msm_probe,
+	.remove = __devexit_p(ci13xxx_msm_remove),
 	.driver = { .name = "msm_hsusb", },
 };
-MODULE_ALIAS("platform:msm_hsusb");
 
-static int __init ci13xxx_msm_init(void)
-{
-	return platform_driver_register(&ci13xxx_msm_driver);
-}
-module_init(ci13xxx_msm_init);
+module_platform_driver(ci13xxx_msm_driver);
 
+MODULE_ALIAS("platform:msm_hsusb");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/chipidea/ci13xxx_pci.c b/drivers/usb/chipidea/ci13xxx_pci.c
index e3dab27f5c7..918e14971f2 100644
--- a/drivers/usb/chipidea/ci13xxx_pci.c
+++ b/drivers/usb/chipidea/ci13xxx_pci.c
@@ -23,17 +23,17 @@
 /******************************************************************************
  * PCI block
  *****************************************************************************/
-struct ci13xxx_udc_driver pci_driver = {
+struct ci13xxx_platform_data pci_platdata = {
 	.name		= UDC_DRIVER_NAME,
 	.capoffset	= DEF_CAPOFFSET,
 };
 
-struct ci13xxx_udc_driver langwell_pci_driver = {
+struct ci13xxx_platform_data langwell_pci_platdata = {
 	.name		= UDC_DRIVER_NAME,
 	.capoffset	= 0,
 };
 
-struct ci13xxx_udc_driver penwell_pci_driver = {
+struct ci13xxx_platform_data penwell_pci_platdata = {
 	.name		= UDC_DRIVER_NAME,
 	.capoffset	= 0,
 	.power_budget	= 200,
@@ -51,12 +51,12 @@ struct ci13xxx_udc_driver penwell_pci_driver = {
 static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
 				       const struct pci_device_id *id)
 {
-	struct ci13xxx_udc_driver *driver = (void *)id->driver_data;
+	struct ci13xxx_platform_data *platdata = (void *)id->driver_data;
 	struct platform_device *plat_ci;
 	struct resource res[3];
 	int retval = 0, nres = 2;
 
-	if (!driver) {
+	if (!platdata) {
 		dev_err(&pdev->dev, "device doesn't provide driver data\n");
 		return -ENODEV;
 	}
@@ -75,13 +75,6 @@ static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
 	pci_set_master(pdev);
 	pci_try_set_mwi(pdev);
 
-	plat_ci = platform_device_alloc("ci_hdrc", -1);
-	if (!plat_ci) {
-		dev_err(&pdev->dev, "can't allocate ci_hdrc platform device\n");
-		retval = -ENOMEM;
-		goto disable_device;
-	}
-
 	memset(res, 0, sizeof(res));
 	res[0].start	= pci_resource_start(pdev, 0);
 	res[0].end	= pci_resource_end(pdev, 0);
@@ -89,32 +82,17 @@ static int __devinit ci13xxx_pci_probe(struct pci_dev *pdev,
 	res[1].start	= pdev->irq;
 	res[1].flags	= IORESOURCE_IRQ;
 
-	retval = platform_device_add_resources(plat_ci, res, nres);
-	if (retval) {
-		dev_err(&pdev->dev, "can't add resources to platform device\n");
-		goto put_platform;
+	plat_ci = ci13xxx_add_device(&pdev->dev, res, nres, platdata);
+	if (IS_ERR(plat_ci)) {
+		dev_err(&pdev->dev, "ci13xxx_add_device failed!\n");
+		retval = PTR_ERR(plat_ci);
+		goto disable_device;
 	}
 
-	retval = platform_device_add_data(plat_ci, driver, sizeof(*driver));
-	if (retval)
-		goto put_platform;
-
-	dma_set_coherent_mask(&plat_ci->dev, pdev->dev.coherent_dma_mask);
-	plat_ci->dev.dma_mask = pdev->dev.dma_mask;
-	plat_ci->dev.dma_parms = pdev->dev.dma_parms;
-	plat_ci->dev.parent = &pdev->dev;
-
 	pci_set_drvdata(pdev, plat_ci);
 
-	retval = platform_device_add(plat_ci);
-	if (retval)
-		goto put_platform;
-
 	return 0;
 
- put_platform:
-	pci_set_drvdata(pdev, NULL);
-	platform_device_put(plat_ci);
  disable_device:
 	pci_disable_device(pdev);
  done:
@@ -133,7 +111,7 @@ static void __devexit ci13xxx_pci_remove(struct pci_dev *pdev)
 {
 	struct platform_device *plat_ci = pci_get_drvdata(pdev);
 
-	platform_device_unregister(plat_ci);
+	ci13xxx_remove_device(plat_ci);
 	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
 }
@@ -147,19 +125,19 @@ static void __devexit ci13xxx_pci_remove(struct pci_dev *pdev)
 static DEFINE_PCI_DEVICE_TABLE(ci13xxx_pci_id_table) = {
 	{
 		PCI_DEVICE(0x153F, 0x1004),
-		.driver_data = (kernel_ulong_t)&pci_driver,
+		.driver_data = (kernel_ulong_t)&pci_platdata,
 	},
 	{
 		PCI_DEVICE(0x153F, 0x1006),
-		.driver_data = (kernel_ulong_t)&pci_driver,
+		.driver_data = (kernel_ulong_t)&pci_platdata,
 	},
 	{
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0811),
-		.driver_data = (kernel_ulong_t)&langwell_pci_driver,
+		.driver_data = (kernel_ulong_t)&langwell_pci_platdata,
 	},
 	{
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0829),
-		.driver_data = (kernel_ulong_t)&penwell_pci_driver,
+		.driver_data = (kernel_ulong_t)&penwell_pci_platdata,
 	},
 	{ 0, 0, 0, 0, 0, 0, 0 /* end: all zeroes */ }
 };
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 15e03b308f8..1083585fad0 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -56,6 +56,7 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
+#include <linux/idr.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
@@ -179,7 +180,7 @@ static int hw_device_init(struct ci13xxx *ci, void __iomem *base)
 	ci->hw_bank.abs = base;
 
 	ci->hw_bank.cap = ci->hw_bank.abs;
-	ci->hw_bank.cap += ci->udc_driver->capoffset;
+	ci->hw_bank.cap += ci->platdata->capoffset;
 	ci->hw_bank.op = ci->hw_bank.cap + ioread8(ci->hw_bank.cap);
 
 	hw_alloc_regmap(ci, false);
@@ -227,11 +228,11 @@ int hw_device_reset(struct ci13xxx *ci, u32 mode)
 		udelay(10);		/* not RTOS friendly */
 
 
-	if (ci->udc_driver->notify_event)
-		ci->udc_driver->notify_event(ci,
+	if (ci->platdata->notify_event)
+		ci->platdata->notify_event(ci,
 			CI13XXX_CONTROLLER_RESET_EVENT);
 
-	if (ci->udc_driver->flags & CI13XXX_DISABLE_STREAMING)
+	if (ci->platdata->flags & CI13XXX_DISABLE_STREAMING)
 		hw_write(ci, OP_USBMODE, USBMODE_CI_SDIS, USBMODE_CI_SDIS);
 
 	/* USBMODE should be configured step by step */
@@ -332,6 +333,59 @@ static irqreturn_t ci_irq(int irq, void *data)
 	return ci->role == CI_ROLE_END ? ret : ci_role(ci)->irq(ci);
 }
 
+static DEFINE_IDA(ci_ida);
+
+struct platform_device *ci13xxx_add_device(struct device *dev,
+			struct resource *res, int nres,
+			struct ci13xxx_platform_data *platdata)
+{
+	struct platform_device *pdev;
+	int id, ret;
+
+	id = ida_simple_get(&ci_ida, 0, 0, GFP_KERNEL);
+	if (id < 0)
+		return ERR_PTR(id);
+
+	pdev = platform_device_alloc("ci_hdrc", id);
+	if (!pdev) {
+		ret = -ENOMEM;
+		goto put_id;
+	}
+
+	pdev->dev.parent = dev;
+	pdev->dev.dma_mask = dev->dma_mask;
+	pdev->dev.dma_parms = dev->dma_parms;
+	dma_set_coherent_mask(&pdev->dev, dev->coherent_dma_mask);
+
+	ret = platform_device_add_resources(pdev, res, nres);
+	if (ret)
+		goto err;
+
+	ret = platform_device_add_data(pdev, platdata, sizeof(*platdata));
+	if (ret)
+		goto err;
+
+	ret = platform_device_add(pdev);
+	if (ret)
+		goto err;
+
+	return pdev;
+
+err:
+	platform_device_put(pdev);
+put_id:
+	ida_simple_remove(&ci_ida, id);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(ci13xxx_add_device);
+
+void ci13xxx_remove_device(struct platform_device *pdev)
+{
+	platform_device_unregister(pdev);
+	ida_simple_remove(&ci_ida, pdev->id);
+}
+EXPORT_SYMBOL_GPL(ci13xxx_remove_device);
+
 static int __devinit ci_hdrc_probe(struct platform_device *pdev)
 {
 	struct device	*dev = &pdev->dev;
@@ -364,7 +418,11 @@ static int __devinit ci_hdrc_probe(struct platform_device *pdev)
 	}
 
 	ci->dev = dev;
-	ci->udc_driver = dev->platform_data;
+	ci->platdata = dev->platform_data;
+	if (ci->platdata->phy)
+		ci->transceiver = ci->platdata->phy;
+	else
+		ci->global_phy = true;
 
 	ret = hw_device_init(ci, base);
 	if (ret < 0) {
@@ -419,7 +477,7 @@ static int __devinit ci_hdrc_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, ci);
-	ret = request_irq(ci->irq, ci_irq, IRQF_SHARED, ci->udc_driver->name,
+	ret = request_irq(ci->irq, ci_irq, IRQF_SHARED, ci->platdata->name,
 			  ci);
 	if (ret)
 		goto stop;
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index c4b3e15532d..c6f50a25756 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -68,15 +68,15 @@ void dbg_interrupt(u32 intmask)
  *
  * This function returns number of registers read
  */
-static size_t hw_register_read(struct ci13xxx *udc, u32 *buf, size_t size)
+static size_t hw_register_read(struct ci13xxx *ci, u32 *buf, size_t size)
 {
 	unsigned i;
 
-	if (size > udc->hw_bank.size)
-		size = udc->hw_bank.size;
+	if (size > ci->hw_bank.size)
+		size = ci->hw_bank.size;
 
 	for (i = 0; i < size; i++)
-		buf[i] = hw_read(udc, i * sizeof(u32), ~0);
+		buf[i] = hw_read(ci, i * sizeof(u32), ~0);
 
 	return size;
 }
@@ -88,18 +88,18 @@ static size_t hw_register_read(struct ci13xxx *udc, u32 *buf, size_t size)
  *
  * This function returns an error code
  */
-static int hw_register_write(struct ci13xxx *udc, u16 addr, u32 data)
+static int hw_register_write(struct ci13xxx *ci, u16 addr, u32 data)
 {
 	/* align */
 	addr /= sizeof(u32);
 
-	if (addr >= udc->hw_bank.size)
+	if (addr >= ci->hw_bank.size)
 		return -EINVAL;
 
 	/* align */
 	addr *= sizeof(u32);
 
-	hw_write(udc, addr, ~0, data);
+	hw_write(ci, addr, ~0, data);
 	return 0;
 }
 
@@ -110,13 +110,13 @@ static int hw_register_write(struct ci13xxx *udc, u16 addr, u32 data)
  *
  * This function returns an error code
  */
-static int hw_intr_clear(struct ci13xxx *udc, int n)
+static int hw_intr_clear(struct ci13xxx *ci, int n)
 {
 	if (n >= REG_BITS)
 		return -EINVAL;
 
-	hw_write(udc, OP_USBINTR, BIT(n), 0);
-	hw_write(udc, OP_USBSTS,  BIT(n), BIT(n));
+	hw_write(ci, OP_USBINTR, BIT(n), 0);
+	hw_write(ci, OP_USBSTS,  BIT(n), BIT(n));
 	return 0;
 }
 
@@ -127,15 +127,15 @@ static int hw_intr_clear(struct ci13xxx *udc, int n)
  *
  * This function returns an error code
  */
-static int hw_intr_force(struct ci13xxx *udc, int n)
+static int hw_intr_force(struct ci13xxx *ci, int n)
 {
 	if (n >= REG_BITS)
 		return -EINVAL;
 
-	hw_write(udc, CAP_TESTMODE, TESTMODE_FORCE, TESTMODE_FORCE);
-	hw_write(udc, OP_USBINTR,  BIT(n), BIT(n));
-	hw_write(udc, OP_USBSTS,   BIT(n), BIT(n));
-	hw_write(udc, CAP_TESTMODE, TESTMODE_FORCE, 0);
+	hw_write(ci, CAP_TESTMODE, TESTMODE_FORCE, TESTMODE_FORCE);
+	hw_write(ci, OP_USBINTR,  BIT(n), BIT(n));
+	hw_write(ci, OP_USBSTS,   BIT(n), BIT(n));
+	hw_write(ci, CAP_TESTMODE, TESTMODE_FORCE, 0);
 	return 0;
 }
 
@@ -147,12 +147,12 @@ static int hw_intr_force(struct ci13xxx *udc, int n)
 static ssize_t show_device(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	struct usb_gadget *gadget = &udc->gadget;
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct usb_gadget *gadget = &ci->gadget;
 	int n = 0;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		return 0;
 	}
 
@@ -188,8 +188,8 @@ static DEVICE_ATTR(device, S_IRUSR, show_device, NULL);
 static ssize_t show_driver(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
-	struct usb_gadget_driver *driver = udc->driver;
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
+	struct usb_gadget_driver *driver = ci->driver;
 	int n = 0;
 
 	if (attr == NULL || buf == NULL) {
@@ -412,22 +412,22 @@ static DEVICE_ATTR(events, S_IRUSR | S_IWUSR, show_events, store_events);
 static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	u32 intr;
 	unsigned i, j, n = 0;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		return 0;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
 
 	/*n += scnprintf(buf + n, PAGE_SIZE - n,
-		       "status = %08x\n", hw_read_intr_status(udc));
+		       "status = %08x\n", hw_read_intr_status(ci));
 	n += scnprintf(buf + n, PAGE_SIZE - n,
-	"enable = %08x\n", hw_read_intr_enable(udc));*/
+	"enable = %08x\n", hw_read_intr_enable(ci));*/
 
 	n += scnprintf(buf + n, PAGE_SIZE - n, "*test = %d\n",
 		       isr_statistics.test);
@@ -471,7 +471,7 @@ static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
 			n += scnprintf(buf + n, PAGE_SIZE - n, "\n");
 	}
 
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	return n;
 }
@@ -485,31 +485,31 @@ static ssize_t show_inters(struct device *dev, struct device_attribute *attr,
 static ssize_t store_inters(struct device *dev, struct device_attribute *attr,
 			    const char *buf, size_t count)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	unsigned en, bit;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "EINVAL\n");
+		dev_err(ci->dev, "EINVAL\n");
 		goto done;
 	}
 
 	if (sscanf(buf, "%u %u", &en, &bit) != 2 || en > 1) {
-		dev_err(udc->dev, "<1|0> <bit>: enable|disable interrupt\n");
+		dev_err(ci->dev, "<1|0> <bit>: enable|disable interrupt\n");
 		goto done;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
 	if (en) {
-		if (hw_intr_force(udc, bit))
+		if (hw_intr_force(ci, bit))
 			dev_err(dev, "invalid bit number\n");
 		else
 			isr_statistics.test++;
 	} else {
-		if (hw_intr_clear(udc, bit))
+		if (hw_intr_clear(ci, bit))
 			dev_err(dev, "invalid bit number\n");
 	}
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
  done:
 	return count;
@@ -524,18 +524,18 @@ static DEVICE_ATTR(inters, S_IRUSR | S_IWUSR, show_inters, store_inters);
 static ssize_t show_port_test(struct device *dev,
 			      struct device_attribute *attr, char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	unsigned mode;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "EINVAL\n");
+		dev_err(ci->dev, "EINVAL\n");
 		return 0;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	mode = hw_port_test_get(udc);
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
+	mode = hw_port_test_get(ci);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	return scnprintf(buf, PAGE_SIZE, "mode = %u\n", mode);
 }
@@ -549,24 +549,24 @@ static ssize_t store_port_test(struct device *dev,
 			       struct device_attribute *attr,
 			       const char *buf, size_t count)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	unsigned mode;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		goto done;
 	}
 
 	if (sscanf(buf, "%u", &mode) != 1) {
-		dev_err(udc->dev, "<mode>: set port test mode");
+		dev_err(ci->dev, "<mode>: set port test mode");
 		goto done;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	if (hw_port_test_set(udc, mode))
-		dev_err(udc->dev, "invalid mode\n");
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
+	if (hw_port_test_set(ci, mode))
+		dev_err(ci->dev, "invalid mode\n");
+	spin_unlock_irqrestore(&ci->lock, flags);
 
  done:
 	return count;
@@ -582,20 +582,20 @@ static DEVICE_ATTR(port_test, S_IRUSR | S_IWUSR,
 static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	unsigned i, j, n = 0;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		return 0;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	for (i = 0; i < udc->hw_ep_max/2; i++) {
-		struct ci13xxx_ep *mEpRx = &udc->ci13xxx_ep[i];
+	spin_lock_irqsave(&ci->lock, flags);
+	for (i = 0; i < ci->hw_ep_max/2; i++) {
+		struct ci13xxx_ep *mEpRx = &ci->ci13xxx_ep[i];
 		struct ci13xxx_ep *mEpTx =
-			&udc->ci13xxx_ep[i + udc->hw_ep_max/2];
+			&ci->ci13xxx_ep[i + ci->hw_ep_max/2];
 		n += scnprintf(buf + n, PAGE_SIZE - n,
 			       "EP=%02i: RX=%08X TX=%08X\n",
 			       i, (u32)mEpRx->qh.dma, (u32)mEpTx->qh.dma);
@@ -606,7 +606,7 @@ static ssize_t show_qheads(struct device *dev, struct device_attribute *attr,
 				       *((u32 *)mEpTx->qh.ptr + j));
 		}
 	}
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	return n;
 }
@@ -621,25 +621,25 @@ static DEVICE_ATTR(qheads, S_IRUSR, show_qheads, NULL);
 static ssize_t show_registers(struct device *dev,
 			      struct device_attribute *attr, char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	u32 *dump;
 	unsigned i, k, n = 0;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		return 0;
 	}
 
 	dump = kmalloc(sizeof(u32) * DUMP_ENTRIES, GFP_KERNEL);
 	if (!dump) {
-		dev_err(udc->dev, "%s: out of memory\n", __func__);
+		dev_err(ci->dev, "%s: out of memory\n", __func__);
 		return 0;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	k = hw_register_read(udc, dump, DUMP_ENTRIES);
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
+	k = hw_register_read(ci, dump, DUMP_ENTRIES);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	for (i = 0; i < k; i++) {
 		n += scnprintf(buf + n, PAGE_SIZE - n,
@@ -660,24 +660,24 @@ static ssize_t store_registers(struct device *dev,
 			       struct device_attribute *attr,
 			       const char *buf, size_t count)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long addr, data, flags;
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		goto done;
 	}
 
 	if (sscanf(buf, "%li %li", &addr, &data) != 2) {
-		dev_err(udc->dev,
+		dev_err(ci->dev,
 			"<addr> <data>: write data to register address\n");
 		goto done;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	if (hw_register_write(udc, addr, data))
-		dev_err(udc->dev, "invalid address range\n");
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
+	if (hw_register_write(ci, addr, data))
+		dev_err(ci->dev, "invalid address range\n");
+	spin_unlock_irqrestore(&ci->lock, flags);
 
  done:
 	return count;
@@ -693,34 +693,34 @@ static DEVICE_ATTR(registers, S_IRUSR | S_IWUSR,
 static ssize_t show_requests(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
-	struct ci13xxx *udc = container_of(dev, struct ci13xxx, gadget.dev);
+	struct ci13xxx *ci = container_of(dev, struct ci13xxx, gadget.dev);
 	unsigned long flags;
 	struct list_head   *ptr = NULL;
 	struct ci13xxx_req *req = NULL;
 	unsigned i, j, n = 0, qSize = sizeof(struct ci13xxx_td)/sizeof(u32);
 
 	if (attr == NULL || buf == NULL) {
-		dev_err(udc->dev, "[%s] EINVAL\n", __func__);
+		dev_err(ci->dev, "[%s] EINVAL\n", __func__);
 		return 0;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	for (i = 0; i < udc->hw_ep_max; i++)
-		list_for_each(ptr, &udc->ci13xxx_ep[i].qh.queue)
+	spin_lock_irqsave(&ci->lock, flags);
+	for (i = 0; i < ci->hw_ep_max; i++)
+		list_for_each(ptr, &ci->ci13xxx_ep[i].qh.queue)
 		{
 			req = list_entry(ptr, struct ci13xxx_req, queue);
 
 			n += scnprintf(buf + n, PAGE_SIZE - n,
 					"EP=%02i: TD=%08X %s\n",
-					i % udc->hw_ep_max/2, (u32)req->dma,
-					((i < udc->hw_ep_max/2) ? "RX" : "TX"));
+					i % ci->hw_ep_max/2, (u32)req->dma,
+					((i < ci->hw_ep_max/2) ? "RX" : "TX"));
 
 			for (j = 0; j < qSize; j++)
 				n += scnprintf(buf + n, PAGE_SIZE - n,
 						" %04X:    %08X\n", j,
 						*((u32 *)req->ptr + j));
 		}
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	return n;
 }
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index 9eacd21c0cd..ebff9f4f56e 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -116,7 +116,8 @@ static int host_start(struct ci13xxx *ci)
 	hcd->regs = ci->hw_bank.abs;
 	hcd->has_tt = 1;
 
-	hcd->power_budget = ci->udc_driver->power_budget;
+	hcd->power_budget = ci->platdata->power_budget;
+	hcd->phy = ci->transceiver;
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = ci->hw_bank.cap;
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 51f96942dc5..c7a032a4f0c 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -14,6 +14,7 @@
 #include <linux/device.h>
 #include <linux/dmapool.h>
 #include <linux/dma-mapping.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
@@ -66,11 +67,11 @@ static inline int hw_ep_bit(int num, int dir)
 	return num + (dir ? 16 : 0);
 }
 
-static inline int ep_to_bit(struct ci13xxx *udc, int n)
+static inline int ep_to_bit(struct ci13xxx *ci, int n)
 {
-	int fill = 16 - udc->hw_ep_max / 2;
+	int fill = 16 - ci->hw_ep_max / 2;
 
-	if (n >= udc->hw_ep_max / 2)
+	if (n >= ci->hw_ep_max / 2)
 		n += fill;
 
 	return n;
@@ -83,17 +84,17 @@ static inline int ep_to_bit(struct ci13xxx *udc, int n)
  *
  * This function returns an error code
  */
-static int hw_device_state(struct ci13xxx *udc, u32 dma)
+static int hw_device_state(struct ci13xxx *ci, u32 dma)
 {
 	if (dma) {
-		hw_write(udc, OP_ENDPTLISTADDR, ~0, dma);
+		hw_write(ci, OP_ENDPTLISTADDR, ~0, dma);
 		/* interrupt, error, port change, reset, sleep/suspend */
-		hw_write(udc, OP_USBINTR, ~0,
+		hw_write(ci, OP_USBINTR, ~0,
 			     USBi_UI|USBi_UEI|USBi_PCI|USBi_URI|USBi_SLI);
-		hw_write(udc, OP_USBCMD, USBCMD_RS, USBCMD_RS);
+		hw_write(ci, OP_USBCMD, USBCMD_RS, USBCMD_RS);
 	} else {
-		hw_write(udc, OP_USBCMD, USBCMD_RS, 0);
-		hw_write(udc, OP_USBINTR, ~0, 0);
+		hw_write(ci, OP_USBCMD, USBCMD_RS, 0);
+		hw_write(ci, OP_USBINTR, ~0, 0);
 	}
 	return 0;
 }
@@ -105,16 +106,16 @@ static int hw_device_state(struct ci13xxx *udc, u32 dma)
  *
  * This function returns an error code
  */
-static int hw_ep_flush(struct ci13xxx *udc, int num, int dir)
+static int hw_ep_flush(struct ci13xxx *ci, int num, int dir)
 {
 	int n = hw_ep_bit(num, dir);
 
 	do {
 		/* flush any pending transfer */
-		hw_write(udc, OP_ENDPTFLUSH, BIT(n), BIT(n));
-		while (hw_read(udc, OP_ENDPTFLUSH, BIT(n)))
+		hw_write(ci, OP_ENDPTFLUSH, BIT(n), BIT(n));
+		while (hw_read(ci, OP_ENDPTFLUSH, BIT(n)))
 			cpu_relax();
-	} while (hw_read(udc, OP_ENDPTSTAT, BIT(n)));
+	} while (hw_read(ci, OP_ENDPTSTAT, BIT(n)));
 
 	return 0;
 }
@@ -126,10 +127,10 @@ static int hw_ep_flush(struct ci13xxx *udc, int num, int dir)
  *
  * This function returns an error code
  */
-static int hw_ep_disable(struct ci13xxx *udc, int num, int dir)
+static int hw_ep_disable(struct ci13xxx *ci, int num, int dir)
 {
-	hw_ep_flush(udc, num, dir);
-	hw_write(udc, OP_ENDPTCTRL + num,
+	hw_ep_flush(ci, num, dir);
+	hw_write(ci, OP_ENDPTCTRL + num,
 		 dir ? ENDPTCTRL_TXE : ENDPTCTRL_RXE, 0);
 	return 0;
 }
@@ -142,7 +143,7 @@ static int hw_ep_disable(struct ci13xxx *udc, int num, int dir)
  *
  * This function returns an error code
  */
-static int hw_ep_enable(struct ci13xxx *udc, int num, int dir, int type)
+static int hw_ep_enable(struct ci13xxx *ci, int num, int dir, int type)
 {
 	u32 mask, data;
 
@@ -165,7 +166,7 @@ static int hw_ep_enable(struct ci13xxx *udc, int num, int dir, int type)
 		mask |= ENDPTCTRL_RXE;  /* enable  */
 		data |= ENDPTCTRL_RXE;
 	}
-	hw_write(udc, OP_ENDPTCTRL + num, mask, data);
+	hw_write(ci, OP_ENDPTCTRL + num, mask, data);
 	return 0;
 }
 
@@ -176,11 +177,11 @@ static int hw_ep_enable(struct ci13xxx *udc, int num, int dir, int type)
  *
  * This function returns 1 if endpoint halted
  */
-static int hw_ep_get_halt(struct ci13xxx *udc, int num, int dir)
+static int hw_ep_get_halt(struct ci13xxx *ci, int num, int dir)
 {
 	u32 mask = dir ? ENDPTCTRL_TXS : ENDPTCTRL_RXS;
 
-	return hw_read(udc, OP_ENDPTCTRL + num, mask) ? 1 : 0;
+	return hw_read(ci, OP_ENDPTCTRL + num, mask) ? 1 : 0;
 }
 
 /**
@@ -190,10 +191,10 @@ static int hw_ep_get_halt(struct ci13xxx *udc, int num, int dir)
  *
  * This function returns setup status
  */
-static int hw_test_and_clear_setup_status(struct ci13xxx *udc, int n)
+static int hw_test_and_clear_setup_status(struct ci13xxx *ci, int n)
 {
-	n = ep_to_bit(udc, n);
-	return hw_test_and_clear(udc, OP_ENDPTSETUPSTAT, BIT(n));
+	n = ep_to_bit(ci, n);
+	return hw_test_and_clear(ci, OP_ENDPTSETUPSTAT, BIT(n));
 }
 
 /**
@@ -204,18 +205,18 @@ static int hw_test_and_clear_setup_status(struct ci13xxx *udc, int n)
  *
  * This function returns an error code
  */
-static int hw_ep_prime(struct ci13xxx *udc, int num, int dir, int is_ctrl)
+static int hw_ep_prime(struct ci13xxx *ci, int num, int dir, int is_ctrl)
 {
 	int n = hw_ep_bit(num, dir);
 
-	if (is_ctrl && dir == RX && hw_read(udc, OP_ENDPTSETUPSTAT, BIT(num)))
+	if (is_ctrl && dir == RX && hw_read(ci, OP_ENDPTSETUPSTAT, BIT(num)))
 		return -EAGAIN;
 
-	hw_write(udc, OP_ENDPTPRIME, BIT(n), BIT(n));
+	hw_write(ci, OP_ENDPTPRIME, BIT(n), BIT(n));
 
-	while (hw_read(udc, OP_ENDPTPRIME, BIT(n)))
+	while (hw_read(ci, OP_ENDPTPRIME, BIT(n)))
 		cpu_relax();
-	if (is_ctrl && dir == RX && hw_read(udc, OP_ENDPTSETUPSTAT, BIT(num)))
+	if (is_ctrl && dir == RX && hw_read(ci, OP_ENDPTSETUPSTAT, BIT(num)))
 		return -EAGAIN;
 
 	/* status shoult be tested according with manual but it doesn't work */
@@ -231,7 +232,7 @@ static int hw_ep_prime(struct ci13xxx *udc, int num, int dir, int is_ctrl)
  *
  * This function returns an error code
  */
-static int hw_ep_set_halt(struct ci13xxx *udc, int num, int dir, int value)
+static int hw_ep_set_halt(struct ci13xxx *ci, int num, int dir, int value)
 {
 	if (value != 0 && value != 1)
 		return -EINVAL;
@@ -242,9 +243,9 @@ static int hw_ep_set_halt(struct ci13xxx *udc, int num, int dir, int value)
 		u32 mask_xr = dir ? ENDPTCTRL_TXR : ENDPTCTRL_RXR;
 
 		/* data toggle - reserved for EP0 but it's in ESS */
-		hw_write(udc, reg, mask_xs|mask_xr,
+		hw_write(ci, reg, mask_xs|mask_xr,
 			  value ? mask_xs : mask_xr);
-	} while (value != hw_ep_get_halt(udc, num, dir));
+	} while (value != hw_ep_get_halt(ci, num, dir));
 
 	return 0;
 }
@@ -254,10 +255,10 @@ static int hw_ep_set_halt(struct ci13xxx *udc, int num, int dir, int value)
  *
  * This function returns true if high speed port
  */
-static int hw_port_is_high_speed(struct ci13xxx *udc)
+static int hw_port_is_high_speed(struct ci13xxx *ci)
 {
-	return udc->hw_bank.lpm ? hw_read(udc, OP_DEVLC, DEVLC_PSPD) :
-		hw_read(udc, OP_PORTSC, PORTSC_HSP);
+	return ci->hw_bank.lpm ? hw_read(ci, OP_DEVLC, DEVLC_PSPD) :
+		hw_read(ci, OP_PORTSC, PORTSC_HSP);
 }
 
 /**
@@ -265,9 +266,9 @@ static int hw_port_is_high_speed(struct ci13xxx *udc)
  *
  * This function returns register data
  */
-static u32 hw_read_intr_enable(struct ci13xxx *udc)
+static u32 hw_read_intr_enable(struct ci13xxx *ci)
 {
-	return hw_read(udc, OP_USBINTR, ~0);
+	return hw_read(ci, OP_USBINTR, ~0);
 }
 
 /**
@@ -275,9 +276,9 @@ static u32 hw_read_intr_enable(struct ci13xxx *udc)
  *
  * This function returns register data
  */
-static u32 hw_read_intr_status(struct ci13xxx *udc)
+static u32 hw_read_intr_status(struct ci13xxx *ci)
 {
-	return hw_read(udc, OP_USBSTS, ~0);
+	return hw_read(ci, OP_USBSTS, ~0);
 }
 
 /**
@@ -287,10 +288,10 @@ static u32 hw_read_intr_status(struct ci13xxx *udc)
  *
  * This function returns complete status
  */
-static int hw_test_and_clear_complete(struct ci13xxx *udc, int n)
+static int hw_test_and_clear_complete(struct ci13xxx *ci, int n)
 {
-	n = ep_to_bit(udc, n);
-	return hw_test_and_clear(udc, OP_ENDPTCOMPLETE, BIT(n));
+	n = ep_to_bit(ci, n);
+	return hw_test_and_clear(ci, OP_ENDPTCOMPLETE, BIT(n));
 }
 
 /**
@@ -299,11 +300,11 @@ static int hw_test_and_clear_complete(struct ci13xxx *udc, int n)
  *
  * This function returns active interrutps
  */
-static u32 hw_test_and_clear_intr_active(struct ci13xxx *udc)
+static u32 hw_test_and_clear_intr_active(struct ci13xxx *ci)
 {
-	u32 reg = hw_read_intr_status(udc) & hw_read_intr_enable(udc);
+	u32 reg = hw_read_intr_status(ci) & hw_read_intr_enable(ci);
 
-	hw_write(udc, OP_USBSTS, ~0, reg);
+	hw_write(ci, OP_USBSTS, ~0, reg);
 	return reg;
 }
 
@@ -313,9 +314,9 @@ static u32 hw_test_and_clear_intr_active(struct ci13xxx *udc)
  *
  * This function returns guard value
  */
-static int hw_test_and_clear_setup_guard(struct ci13xxx *udc)
+static int hw_test_and_clear_setup_guard(struct ci13xxx *ci)
 {
-	return hw_test_and_write(udc, OP_USBCMD, USBCMD_SUTW, 0);
+	return hw_test_and_write(ci, OP_USBCMD, USBCMD_SUTW, 0);
 }
 
 /**
@@ -324,9 +325,9 @@ static int hw_test_and_clear_setup_guard(struct ci13xxx *udc)
  *
  * This function returns guard value
  */
-static int hw_test_and_set_setup_guard(struct ci13xxx *udc)
+static int hw_test_and_set_setup_guard(struct ci13xxx *ci)
 {
-	return hw_test_and_write(udc, OP_USBCMD, USBCMD_SUTW, USBCMD_SUTW);
+	return hw_test_and_write(ci, OP_USBCMD, USBCMD_SUTW, USBCMD_SUTW);
 }
 
 /**
@@ -336,9 +337,9 @@ static int hw_test_and_set_setup_guard(struct ci13xxx *udc)
  * This function explicitly sets the address, without the "USBADRA" (advance)
  * feature, which is not supported by older versions of the controller.
  */
-static void hw_usb_set_address(struct ci13xxx *udc, u8 value)
+static void hw_usb_set_address(struct ci13xxx *ci, u8 value)
 {
-	hw_write(udc, OP_DEVICEADDR, DEVICEADDR_USBADR,
+	hw_write(ci, OP_DEVICEADDR, DEVICEADDR_USBADR,
 		 value << ffs_nr(DEVICEADDR_USBADR));
 }
 
@@ -348,21 +349,21 @@ static void hw_usb_set_address(struct ci13xxx *udc, u8 value)
  *
  * This function returns an error code
  */
-static int hw_usb_reset(struct ci13xxx *udc)
+static int hw_usb_reset(struct ci13xxx *ci)
 {
-	hw_usb_set_address(udc, 0);
+	hw_usb_set_address(ci, 0);
 
 	/* ESS flushes only at end?!? */
-	hw_write(udc, OP_ENDPTFLUSH,    ~0, ~0);
+	hw_write(ci, OP_ENDPTFLUSH,    ~0, ~0);
 
 	/* clear setup token semaphores */
-	hw_write(udc, OP_ENDPTSETUPSTAT, 0,  0);
+	hw_write(ci, OP_ENDPTSETUPSTAT, 0,  0);
 
 	/* clear complete status */
-	hw_write(udc, OP_ENDPTCOMPLETE,  0,  0);
+	hw_write(ci, OP_ENDPTCOMPLETE,  0,  0);
 
 	/* wait until all bits cleared */
-	while (hw_read(udc, OP_ENDPTPRIME, ~0))
+	while (hw_read(ci, OP_ENDPTPRIME, ~0))
 		udelay(10);             /* not RTOS friendly */
 
 	/* reset all endpoints ? */
@@ -394,7 +395,7 @@ static inline u8 _usb_addr(struct ci13xxx_ep *ep)
  */
 static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 {
-	struct ci13xxx *udc = mEp->udc;
+	struct ci13xxx *ci = mEp->ci;
 	unsigned i;
 	int ret = 0;
 	unsigned length = mReq->req.length;
@@ -417,7 +418,7 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 		if (!mReq->req.no_interrupt)
 			mReq->zptr->token   |= TD_IOC;
 	}
-	ret = usb_gadget_map_request(&udc->gadget, &mReq->req, mEp->dir);
+	ret = usb_gadget_map_request(&ci->gadget, &mReq->req, mEp->dir);
 	if (ret)
 		return ret;
 
@@ -453,13 +454,13 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 		else
 			mReqPrev->ptr->next = mReq->dma & TD_ADDR_MASK;
 		wmb();
-		if (hw_read(udc, OP_ENDPTPRIME, BIT(n)))
+		if (hw_read(ci, OP_ENDPTPRIME, BIT(n)))
 			goto done;
 		do {
-			hw_write(udc, OP_USBCMD, USBCMD_ATDTW, USBCMD_ATDTW);
-			tmp_stat = hw_read(udc, OP_ENDPTSTAT, BIT(n));
-		} while (!hw_read(udc, OP_USBCMD, USBCMD_ATDTW));
-		hw_write(udc, OP_USBCMD, USBCMD_ATDTW, 0);
+			hw_write(ci, OP_USBCMD, USBCMD_ATDTW, USBCMD_ATDTW);
+			tmp_stat = hw_read(ci, OP_ENDPTSTAT, BIT(n));
+		} while (!hw_read(ci, OP_USBCMD, USBCMD_ATDTW));
+		hw_write(ci, OP_USBCMD, USBCMD_ATDTW, 0);
 		if (tmp_stat)
 			goto done;
 	}
@@ -471,7 +472,7 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 
 	wmb();   /* synchronize before ep prime */
 
-	ret = hw_ep_prime(udc, mEp->num, mEp->dir,
+	ret = hw_ep_prime(ci, mEp->num, mEp->dir,
 			   mEp->type == USB_ENDPOINT_XFER_CONTROL);
 done:
 	return ret;
@@ -501,7 +502,7 @@ static int _hardware_dequeue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
 
 	mReq->req.status = 0;
 
-	usb_gadget_unmap_request(&mEp->udc->gadget, &mReq->req, mEp->dir);
+	usb_gadget_unmap_request(&mEp->ci->gadget, &mReq->req, mEp->dir);
 
 	mReq->req.status = mReq->ptr->token & TD_STATUS;
 	if ((TD_STATUS_HALTED & mReq->req.status) != 0)
@@ -533,7 +534,7 @@ __acquires(mEp->lock)
 	if (mEp == NULL)
 		return -EINVAL;
 
-	hw_ep_flush(mEp->udc, mEp->num, mEp->dir);
+	hw_ep_flush(mEp->ci, mEp->num, mEp->dir);
 
 	while (!list_empty(&mEp->qh.queue)) {
 
@@ -562,33 +563,33 @@ __acquires(mEp->lock)
 static int _gadget_stop_activity(struct usb_gadget *gadget)
 {
 	struct usb_ep *ep;
-	struct ci13xxx    *udc = container_of(gadget, struct ci13xxx, gadget);
+	struct ci13xxx    *ci = container_of(gadget, struct ci13xxx, gadget);
 	unsigned long flags;
 
-	spin_lock_irqsave(&udc->lock, flags);
-	udc->gadget.speed = USB_SPEED_UNKNOWN;
-	udc->remote_wakeup = 0;
-	udc->suspended = 0;
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
+	ci->gadget.speed = USB_SPEED_UNKNOWN;
+	ci->remote_wakeup = 0;
+	ci->suspended = 0;
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	/* flush all endpoints */
 	gadget_for_each_ep(ep, gadget) {
 		usb_ep_fifo_flush(ep);
 	}
-	usb_ep_fifo_flush(&udc->ep0out->ep);
-	usb_ep_fifo_flush(&udc->ep0in->ep);
+	usb_ep_fifo_flush(&ci->ep0out->ep);
+	usb_ep_fifo_flush(&ci->ep0in->ep);
 
-	if (udc->driver)
-		udc->driver->disconnect(gadget);
+	if (ci->driver)
+		ci->driver->disconnect(gadget);
 
 	/* make sure to disable all endpoints */
 	gadget_for_each_ep(ep, gadget) {
 		usb_ep_disable(ep);
 	}
 
-	if (udc->status != NULL) {
-		usb_ep_free_request(&udc->ep0in->ep, udc->status);
-		udc->status = NULL;
+	if (ci->status != NULL) {
+		usb_ep_free_request(&ci->ep0in->ep, ci->status);
+		ci->status = NULL;
 	}
 
 	return 0;
@@ -599,36 +600,36 @@ static int _gadget_stop_activity(struct usb_gadget *gadget)
  *****************************************************************************/
 /**
  * isr_reset_handler: USB reset interrupt handler
- * @udc: UDC device
+ * @ci: UDC device
  *
  * This function resets USB engine after a bus reset occurred
  */
-static void isr_reset_handler(struct ci13xxx *udc)
-__releases(udc->lock)
-__acquires(udc->lock)
+static void isr_reset_handler(struct ci13xxx *ci)
+__releases(ci->lock)
+__acquires(ci->lock)
 {
 	int retval;
 
 	dbg_event(0xFF, "BUS RST", 0);
 
-	spin_unlock(&udc->lock);
-	retval = _gadget_stop_activity(&udc->gadget);
+	spin_unlock(&ci->lock);
+	retval = _gadget_stop_activity(&ci->gadget);
 	if (retval)
 		goto done;
 
-	retval = hw_usb_reset(udc);
+	retval = hw_usb_reset(ci);
 	if (retval)
 		goto done;
 
-	udc->status = usb_ep_alloc_request(&udc->ep0in->ep, GFP_ATOMIC);
-	if (udc->status == NULL)
+	ci->status = usb_ep_alloc_request(&ci->ep0in->ep, GFP_ATOMIC);
+	if (ci->status == NULL)
 		retval = -ENOMEM;
 
 done:
-	spin_lock(&udc->lock);
+	spin_lock(&ci->lock);
 
 	if (retval)
-		dev_err(udc->dev, "error: %i\n", retval);
+		dev_err(ci->dev, "error: %i\n", retval);
 }
 
 /**
@@ -649,17 +650,17 @@ static void isr_get_status_complete(struct usb_ep *ep, struct usb_request *req)
 
 /**
  * isr_get_status_response: get_status request response
- * @udc: udc struct
+ * @ci: ci struct
  * @setup: setup request packet
  *
  * This function returns an error code
  */
-static int isr_get_status_response(struct ci13xxx *udc,
+static int isr_get_status_response(struct ci13xxx *ci,
 				   struct usb_ctrlrequest *setup)
 __releases(mEp->lock)
 __acquires(mEp->lock)
 {
-	struct ci13xxx_ep *mEp = udc->ep0in;
+	struct ci13xxx_ep *mEp = ci->ep0in;
 	struct usb_request *req = NULL;
 	gfp_t gfp_flags = GFP_ATOMIC;
 	int dir, num, retval;
@@ -683,14 +684,14 @@ __acquires(mEp->lock)
 
 	if ((setup->bRequestType & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
 		/* Assume that device is bus powered for now. */
-		*(u16 *)req->buf = udc->remote_wakeup << 1;
+		*(u16 *)req->buf = ci->remote_wakeup << 1;
 		retval = 0;
 	} else if ((setup->bRequestType & USB_RECIP_MASK) \
 		   == USB_RECIP_ENDPOINT) {
 		dir = (le16_to_cpu(setup->wIndex) & USB_ENDPOINT_DIR_MASK) ?
 			TX : RX;
 		num =  le16_to_cpu(setup->wIndex) & USB_ENDPOINT_NUMBER_MASK;
-		*(u16 *)req->buf = hw_ep_get_halt(udc, num, dir);
+		*(u16 *)req->buf = hw_ep_get_halt(ci, num, dir);
 	}
 	/* else do nothing; reserved for future use */
 
@@ -722,39 +723,39 @@ __acquires(mEp->lock)
 static void
 isr_setup_status_complete(struct usb_ep *ep, struct usb_request *req)
 {
-	struct ci13xxx *udc = req->context;
+	struct ci13xxx *ci = req->context;
 	unsigned long flags;
 
-	if (udc->setaddr) {
-		hw_usb_set_address(udc, udc->address);
-		udc->setaddr = false;
+	if (ci->setaddr) {
+		hw_usb_set_address(ci, ci->address);
+		ci->setaddr = false;
 	}
 
-	spin_lock_irqsave(&udc->lock, flags);
-	if (udc->test_mode)
-		hw_port_test_set(udc, udc->test_mode);
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
+	if (ci->test_mode)
+		hw_port_test_set(ci, ci->test_mode);
+	spin_unlock_irqrestore(&ci->lock, flags);
 }
 
 /**
  * isr_setup_status_phase: queues the status phase of a setup transation
- * @udc: udc struct
+ * @ci: ci struct
  *
  * This function returns an error code
  */
-static int isr_setup_status_phase(struct ci13xxx *udc)
+static int isr_setup_status_phase(struct ci13xxx *ci)
 __releases(mEp->lock)
 __acquires(mEp->lock)
 {
 	int retval;
 	struct ci13xxx_ep *mEp;
 
-	mEp = (udc->ep0_dir == TX) ? udc->ep0out : udc->ep0in;
-	udc->status->context = udc;
-	udc->status->complete = isr_setup_status_complete;
+	mEp = (ci->ep0_dir == TX) ? ci->ep0out : ci->ep0in;
+	ci->status->context = ci;
+	ci->status->complete = isr_setup_status_complete;
 
 	spin_unlock(mEp->lock);
-	retval = usb_ep_queue(&mEp->ep, udc->status, GFP_ATOMIC);
+	retval = usb_ep_queue(&mEp->ep, ci->status, GFP_ATOMIC);
 	spin_lock(mEp->lock);
 
 	return retval;
@@ -789,7 +790,7 @@ __acquires(mEp->lock)
 			spin_unlock(mEp->lock);
 			if ((mEp->type == USB_ENDPOINT_XFER_CONTROL) &&
 					mReq->req.length)
-				mEpTemp = mEp->udc->ep0in;
+				mEpTemp = mEp->ci->ep0in;
 			mReq->req.complete(&mEpTemp->ep, &mReq->req);
 			spin_lock(mEp->lock);
 		}
@@ -805,48 +806,48 @@ __acquires(mEp->lock)
 
 /**
  * isr_tr_complete_handler: transaction complete interrupt handler
- * @udc: UDC descriptor
+ * @ci: UDC descriptor
  *
  * This function handles traffic events
  */
-static void isr_tr_complete_handler(struct ci13xxx *udc)
-__releases(udc->lock)
-__acquires(udc->lock)
+static void isr_tr_complete_handler(struct ci13xxx *ci)
+__releases(ci->lock)
+__acquires(ci->lock)
 {
 	unsigned i;
 	u8 tmode = 0;
 
-	for (i = 0; i < udc->hw_ep_max; i++) {
-		struct ci13xxx_ep *mEp  = &udc->ci13xxx_ep[i];
+	for (i = 0; i < ci->hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp  = &ci->ci13xxx_ep[i];
 		int type, num, dir, err = -EINVAL;
 		struct usb_ctrlrequest req;
 
 		if (mEp->ep.desc == NULL)
 			continue;   /* not configured */
 
-		if (hw_test_and_clear_complete(udc, i)) {
+		if (hw_test_and_clear_complete(ci, i)) {
 			err = isr_tr_complete_low(mEp);
 			if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
 				if (err > 0)   /* needs status phase */
-					err = isr_setup_status_phase(udc);
+					err = isr_setup_status_phase(ci);
 				if (err < 0) {
 					dbg_event(_usb_addr(mEp),
 						  "ERROR", err);
-					spin_unlock(&udc->lock);
+					spin_unlock(&ci->lock);
 					if (usb_ep_set_halt(&mEp->ep))
-						dev_err(udc->dev,
+						dev_err(ci->dev,
 							"error: ep_set_halt\n");
-					spin_lock(&udc->lock);
+					spin_lock(&ci->lock);
 				}
 			}
 		}
 
 		if (mEp->type != USB_ENDPOINT_XFER_CONTROL ||
-		    !hw_test_and_clear_setup_status(udc, i))
+		    !hw_test_and_clear_setup_status(ci, i))
 			continue;
 
 		if (i != 0) {
-			dev_warn(udc->dev, "ctrl traffic at endpoint %d\n", i);
+			dev_warn(ci->dev, "ctrl traffic at endpoint %d\n", i);
 			continue;
 		}
 
@@ -854,18 +855,18 @@ __acquires(udc->lock)
 		 * Flush data and handshake transactions of previous
 		 * setup packet.
 		 */
-		_ep_nuke(udc->ep0out);
-		_ep_nuke(udc->ep0in);
+		_ep_nuke(ci->ep0out);
+		_ep_nuke(ci->ep0in);
 
 		/* read_setup_packet */
 		do {
-			hw_test_and_set_setup_guard(udc);
+			hw_test_and_set_setup_guard(ci);
 			memcpy(&req, &mEp->qh.ptr->setup, sizeof(req));
-		} while (!hw_test_and_clear_setup_guard(udc));
+		} while (!hw_test_and_clear_setup_guard(ci));
 
 		type = req.bRequestType;
 
-		udc->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
+		ci->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
 
 		dbg_setup(_usb_addr(mEp), &req);
 
@@ -880,23 +881,23 @@ __acquires(udc->lock)
 				dir = num & USB_ENDPOINT_DIR_MASK;
 				num &= USB_ENDPOINT_NUMBER_MASK;
 				if (dir) /* TX */
-					num += udc->hw_ep_max/2;
-				if (!udc->ci13xxx_ep[num].wedge) {
-					spin_unlock(&udc->lock);
+					num += ci->hw_ep_max/2;
+				if (!ci->ci13xxx_ep[num].wedge) {
+					spin_unlock(&ci->lock);
 					err = usb_ep_clear_halt(
-						&udc->ci13xxx_ep[num].ep);
-					spin_lock(&udc->lock);
+						&ci->ci13xxx_ep[num].ep);
+					spin_lock(&ci->lock);
 					if (err)
 						break;
 				}
-				err = isr_setup_status_phase(udc);
+				err = isr_setup_status_phase(ci);
 			} else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE) &&
 					le16_to_cpu(req.wValue) ==
 					USB_DEVICE_REMOTE_WAKEUP) {
 				if (req.wLength != 0)
 					break;
-				udc->remote_wakeup = 0;
-				err = isr_setup_status_phase(udc);
+				ci->remote_wakeup = 0;
+				err = isr_setup_status_phase(ci);
 			} else {
 				goto delegate;
 			}
@@ -909,7 +910,7 @@ __acquires(udc->lock)
 			if (le16_to_cpu(req.wLength) != 2 ||
 			    le16_to_cpu(req.wValue)  != 0)
 				break;
-			err = isr_get_status_response(udc, &req);
+			err = isr_get_status_response(ci, &req);
 			break;
 		case USB_REQ_SET_ADDRESS:
 			if (type != (USB_DIR_OUT|USB_RECIP_DEVICE))
@@ -917,9 +918,9 @@ __acquires(udc->lock)
 			if (le16_to_cpu(req.wLength) != 0 ||
 			    le16_to_cpu(req.wIndex)  != 0)
 				break;
-			udc->address = (u8)le16_to_cpu(req.wValue);
-			udc->setaddr = true;
-			err = isr_setup_status_phase(udc);
+			ci->address = (u8)le16_to_cpu(req.wValue);
+			ci->setaddr = true;
+			err = isr_setup_status_phase(ci);
 			break;
 		case USB_REQ_SET_FEATURE:
 			if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
@@ -931,20 +932,20 @@ __acquires(udc->lock)
 				dir = num & USB_ENDPOINT_DIR_MASK;
 				num &= USB_ENDPOINT_NUMBER_MASK;
 				if (dir) /* TX */
-					num += udc->hw_ep_max/2;
+					num += ci->hw_ep_max/2;
 
-				spin_unlock(&udc->lock);
-				err = usb_ep_set_halt(&udc->ci13xxx_ep[num].ep);
-				spin_lock(&udc->lock);
+				spin_unlock(&ci->lock);
+				err = usb_ep_set_halt(&ci->ci13xxx_ep[num].ep);
+				spin_lock(&ci->lock);
 				if (!err)
-					isr_setup_status_phase(udc);
+					isr_setup_status_phase(ci);
 			} else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE)) {
 				if (req.wLength != 0)
 					break;
 				switch (le16_to_cpu(req.wValue)) {
 				case USB_DEVICE_REMOTE_WAKEUP:
-					udc->remote_wakeup = 1;
-					err = isr_setup_status_phase(udc);
+					ci->remote_wakeup = 1;
+					err = isr_setup_status_phase(ci);
 					break;
 				case USB_DEVICE_TEST_MODE:
 					tmode = le16_to_cpu(req.wIndex) >> 8;
@@ -954,9 +955,9 @@ __acquires(udc->lock)
 					case TEST_SE0_NAK:
 					case TEST_PACKET:
 					case TEST_FORCE_EN:
-						udc->test_mode = tmode;
+						ci->test_mode = tmode;
 						err = isr_setup_status_phase(
-								udc);
+								ci);
 						break;
 					default:
 						break;
@@ -971,21 +972,21 @@ __acquires(udc->lock)
 		default:
 delegate:
 			if (req.wLength == 0)   /* no data phase */
-				udc->ep0_dir = TX;
+				ci->ep0_dir = TX;
 
-			spin_unlock(&udc->lock);
-			err = udc->driver->setup(&udc->gadget, &req);
-			spin_lock(&udc->lock);
+			spin_unlock(&ci->lock);
+			err = ci->driver->setup(&ci->gadget, &req);
+			spin_lock(&ci->lock);
 			break;
 		}
 
 		if (err < 0) {
 			dbg_event(_usb_addr(mEp), "ERROR", err);
 
-			spin_unlock(&udc->lock);
+			spin_unlock(&ci->lock);
 			if (usb_ep_set_halt(&mEp->ep))
-				dev_err(udc->dev, "error: ep_set_halt\n");
-			spin_lock(&udc->lock);
+				dev_err(ci->dev, "error: ep_set_halt\n");
+			spin_lock(&ci->lock);
 		}
 	}
 }
@@ -1015,7 +1016,7 @@ static int ep_enable(struct usb_ep *ep,
 	mEp->ep.desc = desc;
 
 	if (!list_empty(&mEp->qh.queue))
-		dev_warn(mEp->udc->dev, "enabling a non-empty endpoint!\n");
+		dev_warn(mEp->ci->dev, "enabling a non-empty endpoint!\n");
 
 	mEp->dir  = usb_endpoint_dir_in(desc) ? TX : RX;
 	mEp->num  = usb_endpoint_num(desc);
@@ -1043,7 +1044,7 @@ static int ep_enable(struct usb_ep *ep,
 	 * is always enabled
 	 */
 	if (mEp->num)
-		retval |= hw_ep_enable(mEp->udc, mEp->num, mEp->dir, mEp->type);
+		retval |= hw_ep_enable(mEp->ci, mEp->num, mEp->dir, mEp->type);
 
 	spin_unlock_irqrestore(mEp->lock, flags);
 	return retval;
@@ -1074,7 +1075,7 @@ static int ep_disable(struct usb_ep *ep)
 		dbg_event(_usb_addr(mEp), "DISABLE", 0);
 
 		retval |= _ep_nuke(mEp);
-		retval |= hw_ep_disable(mEp->udc, mEp->num, mEp->dir);
+		retval |= hw_ep_disable(mEp->ci, mEp->num, mEp->dir);
 
 		if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
 			mEp->dir = (mEp->dir == TX) ? RX : TX;
@@ -1131,7 +1132,7 @@ static void ep_free_request(struct usb_ep *ep, struct usb_request *req)
 	if (ep == NULL || req == NULL) {
 		return;
 	} else if (!list_empty(&mReq->queue)) {
-		dev_err(mEp->udc->dev, "freeing queued request\n");
+		dev_err(mEp->ci->dev, "freeing queued request\n");
 		return;
 	}
 
@@ -1156,7 +1157,7 @@ static int ep_queue(struct usb_ep *ep, struct usb_request *req,
 {
 	struct ci13xxx_ep  *mEp  = container_of(ep,  struct ci13xxx_ep, ep);
 	struct ci13xxx_req *mReq = container_of(req, struct ci13xxx_req, req);
-	struct ci13xxx *udc = mEp->udc;
+	struct ci13xxx *ci = mEp->ci;
 	int retval = 0;
 	unsigned long flags;
 
@@ -1167,12 +1168,12 @@ static int ep_queue(struct usb_ep *ep, struct usb_request *req,
 
 	if (mEp->type == USB_ENDPOINT_XFER_CONTROL) {
 		if (req->length)
-			mEp = (udc->ep0_dir == RX) ?
-			       udc->ep0out : udc->ep0in;
+			mEp = (ci->ep0_dir == RX) ?
+			       ci->ep0out : ci->ep0in;
 		if (!list_empty(&mEp->qh.queue)) {
 			_ep_nuke(mEp);
 			retval = -EOVERFLOW;
-			dev_warn(mEp->udc->dev, "endpoint ctrl %X nuked\n",
+			dev_warn(mEp->ci->dev, "endpoint ctrl %X nuked\n",
 				 _usb_addr(mEp));
 		}
 	}
@@ -1180,14 +1181,14 @@ static int ep_queue(struct usb_ep *ep, struct usb_request *req,
 	/* first nuke then test link, e.g. previous status has not sent */
 	if (!list_empty(&mReq->queue)) {
 		retval = -EBUSY;
-		dev_err(mEp->udc->dev, "request already in queue\n");
+		dev_err(mEp->ci->dev, "request already in queue\n");
 		goto done;
 	}
 
 	if (req->length > 4 * CI13XXX_PAGE_SIZE) {
 		req->length = 4 * CI13XXX_PAGE_SIZE;
 		retval = -EMSGSIZE;
-		dev_warn(mEp->udc->dev, "request length truncated\n");
+		dev_warn(mEp->ci->dev, "request length truncated\n");
 	}
 
 	dbg_queue(_usb_addr(mEp), req, retval);
@@ -1230,12 +1231,12 @@ static int ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 
 	dbg_event(_usb_addr(mEp), "DEQUEUE", 0);
 
-	hw_ep_flush(mEp->udc, mEp->num, mEp->dir);
+	hw_ep_flush(mEp->ci, mEp->num, mEp->dir);
 
 	/* pop request */
 	list_del_init(&mReq->queue);
 
-	usb_gadget_unmap_request(&mEp->udc->gadget, req, mEp->dir);
+	usb_gadget_unmap_request(&mEp->ci->gadget, req, mEp->dir);
 
 	req->status = -ECONNRESET;
 
@@ -1277,7 +1278,7 @@ static int ep_set_halt(struct usb_ep *ep, int value)
 	direction = mEp->dir;
 	do {
 		dbg_event(_usb_addr(mEp), "HALT", value);
-		retval |= hw_ep_set_halt(mEp->udc, mEp->num, mEp->dir, value);
+		retval |= hw_ep_set_halt(mEp->ci, mEp->num, mEp->dir, value);
 
 		if (!value)
 			mEp->wedge = 0;
@@ -1325,14 +1326,14 @@ static void ep_fifo_flush(struct usb_ep *ep)
 	unsigned long flags;
 
 	if (ep == NULL) {
-		dev_err(mEp->udc->dev, "%02X: -EINVAL\n", _usb_addr(mEp));
+		dev_err(mEp->ci->dev, "%02X: -EINVAL\n", _usb_addr(mEp));
 		return;
 	}
 
 	spin_lock_irqsave(mEp->lock, flags);
 
 	dbg_event(_usb_addr(mEp), "FFLUSH", 0);
-	hw_ep_flush(mEp->udc, mEp->num, mEp->dir);
+	hw_ep_flush(mEp->ci, mEp->num, mEp->dir);
 
 	spin_unlock_irqrestore(mEp->lock, flags);
 }
@@ -1358,30 +1359,30 @@ static const struct usb_ep_ops usb_ep_ops = {
  *****************************************************************************/
 static int ci13xxx_vbus_session(struct usb_gadget *_gadget, int is_active)
 {
-	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
+	struct ci13xxx *ci = container_of(_gadget, struct ci13xxx, gadget);
 	unsigned long flags;
 	int gadget_ready = 0;
 
-	if (!(udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS))
+	if (!(ci->platdata->flags & CI13XXX_PULLUP_ON_VBUS))
 		return -EOPNOTSUPP;
 
-	spin_lock_irqsave(&udc->lock, flags);
-	udc->vbus_active = is_active;
-	if (udc->driver)
+	spin_lock_irqsave(&ci->lock, flags);
+	ci->vbus_active = is_active;
+	if (ci->driver)
 		gadget_ready = 1;
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	if (gadget_ready) {
 		if (is_active) {
 			pm_runtime_get_sync(&_gadget->dev);
-			hw_device_reset(udc, USBMODE_CM_DC);
-			hw_device_state(udc, udc->ep0out->qh.dma);
+			hw_device_reset(ci, USBMODE_CM_DC);
+			hw_device_state(ci, ci->ep0out->qh.dma);
 		} else {
-			hw_device_state(udc, 0);
-			if (udc->udc_driver->notify_event)
-				udc->udc_driver->notify_event(udc,
+			hw_device_state(ci, 0);
+			if (ci->platdata->notify_event)
+				ci->platdata->notify_event(ci,
 				CI13XXX_CONTROLLER_STOPPED_EVENT);
-			_gadget_stop_activity(&udc->gadget);
+			_gadget_stop_activity(&ci->gadget);
 			pm_runtime_put_sync(&_gadget->dev);
 		}
 	}
@@ -1391,31 +1392,31 @@ static int ci13xxx_vbus_session(struct usb_gadget *_gadget, int is_active)
 
 static int ci13xxx_wakeup(struct usb_gadget *_gadget)
 {
-	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
+	struct ci13xxx *ci = container_of(_gadget, struct ci13xxx, gadget);
 	unsigned long flags;
 	int ret = 0;
 
-	spin_lock_irqsave(&udc->lock, flags);
-	if (!udc->remote_wakeup) {
+	spin_lock_irqsave(&ci->lock, flags);
+	if (!ci->remote_wakeup) {
 		ret = -EOPNOTSUPP;
 		goto out;
 	}
-	if (!hw_read(udc, OP_PORTSC, PORTSC_SUSP)) {
+	if (!hw_read(ci, OP_PORTSC, PORTSC_SUSP)) {
 		ret = -EINVAL;
 		goto out;
 	}
-	hw_write(udc, OP_PORTSC, PORTSC_FPR, PORTSC_FPR);
+	hw_write(ci, OP_PORTSC, PORTSC_FPR, PORTSC_FPR);
 out:
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 	return ret;
 }
 
 static int ci13xxx_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
 {
-	struct ci13xxx *udc = container_of(_gadget, struct ci13xxx, gadget);
+	struct ci13xxx *ci = container_of(_gadget, struct ci13xxx, gadget);
 
-	if (udc->transceiver)
-		return usb_phy_set_power(udc->transceiver, mA);
+	if (ci->transceiver)
+		return usb_phy_set_power(ci->transceiver, mA);
 	return -ENOTSUPP;
 }
 
@@ -1436,28 +1437,28 @@ static const struct usb_gadget_ops usb_gadget_ops = {
 	.udc_stop	= ci13xxx_stop,
 };
 
-static int init_eps(struct ci13xxx *udc)
+static int init_eps(struct ci13xxx *ci)
 {
 	int retval = 0, i, j;
 
-	for (i = 0; i < udc->hw_ep_max/2; i++)
+	for (i = 0; i < ci->hw_ep_max/2; i++)
 		for (j = RX; j <= TX; j++) {
-			int k = i + j * udc->hw_ep_max/2;
-			struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[k];
+			int k = i + j * ci->hw_ep_max/2;
+			struct ci13xxx_ep *mEp = &ci->ci13xxx_ep[k];
 
 			scnprintf(mEp->name, sizeof(mEp->name), "ep%i%s", i,
 					(j == TX)  ? "in" : "out");
 
-			mEp->udc          = udc;
-			mEp->lock         = &udc->lock;
-			mEp->td_pool      = udc->td_pool;
+			mEp->ci          = ci;
+			mEp->lock         = &ci->lock;
+			mEp->td_pool      = ci->td_pool;
 
 			mEp->ep.name      = mEp->name;
 			mEp->ep.ops       = &usb_ep_ops;
 			mEp->ep.maxpacket = CTRL_PAYLOAD_MAX;
 
 			INIT_LIST_HEAD(&mEp->qh.queue);
-			mEp->qh.ptr = dma_pool_alloc(udc->qh_pool, GFP_KERNEL,
+			mEp->qh.ptr = dma_pool_alloc(ci->qh_pool, GFP_KERNEL,
 						     &mEp->qh.dma);
 			if (mEp->qh.ptr == NULL)
 				retval = -ENOMEM;
@@ -1470,14 +1471,14 @@ static int init_eps(struct ci13xxx *udc)
 			 */
 			if (i == 0) {
 				if (j == RX)
-					udc->ep0out = mEp;
+					ci->ep0out = mEp;
 				else
-					udc->ep0in = mEp;
+					ci->ep0in = mEp;
 
 				continue;
 			}
 
-			list_add_tail(&mEp->ep.ep_list, &udc->gadget.ep_list);
+			list_add_tail(&mEp->ep.ep_list, &ci->gadget.ep_list);
 		}
 
 	return retval;
@@ -1493,7 +1494,7 @@ static int init_eps(struct ci13xxx *udc)
 static int ci13xxx_start(struct usb_gadget *gadget,
 			 struct usb_gadget_driver *driver)
 {
-	struct ci13xxx *udc = container_of(gadget, struct ci13xxx, gadget);
+	struct ci13xxx *ci = container_of(gadget, struct ci13xxx, gadget);
 	unsigned long flags;
 	int retval = -ENOMEM;
 
@@ -1501,35 +1502,35 @@ static int ci13xxx_start(struct usb_gadget *gadget,
 		return -EINVAL;
 
 
-	udc->ep0out->ep.desc = &ctrl_endpt_out_desc;
-	retval = usb_ep_enable(&udc->ep0out->ep);
+	ci->ep0out->ep.desc = &ctrl_endpt_out_desc;
+	retval = usb_ep_enable(&ci->ep0out->ep);
 	if (retval)
 		return retval;
 
-	udc->ep0in->ep.desc = &ctrl_endpt_in_desc;
-	retval = usb_ep_enable(&udc->ep0in->ep);
+	ci->ep0in->ep.desc = &ctrl_endpt_in_desc;
+	retval = usb_ep_enable(&ci->ep0in->ep);
 	if (retval)
 		return retval;
-	spin_lock_irqsave(&udc->lock, flags);
-
-	udc->driver = driver;
-	pm_runtime_get_sync(&udc->gadget.dev);
-	if (udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS) {
-		if (udc->vbus_active) {
-			if (udc->udc_driver->flags & CI13XXX_REGS_SHARED)
-				hw_device_reset(udc, USBMODE_CM_DC);
+	spin_lock_irqsave(&ci->lock, flags);
+
+	ci->driver = driver;
+	pm_runtime_get_sync(&ci->gadget.dev);
+	if (ci->platdata->flags & CI13XXX_PULLUP_ON_VBUS) {
+		if (ci->vbus_active) {
+			if (ci->platdata->flags & CI13XXX_REGS_SHARED)
+				hw_device_reset(ci, USBMODE_CM_DC);
 		} else {
-			pm_runtime_put_sync(&udc->gadget.dev);
+			pm_runtime_put_sync(&ci->gadget.dev);
 			goto done;
 		}
 	}
 
-	retval = hw_device_state(udc, udc->ep0out->qh.dma);
+	retval = hw_device_state(ci, ci->ep0out->qh.dma);
 	if (retval)
-		pm_runtime_put_sync(&udc->gadget.dev);
+		pm_runtime_put_sync(&ci->gadget.dev);
 
  done:
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 	return retval;
 }
 
@@ -1539,25 +1540,25 @@ static int ci13xxx_start(struct usb_gadget *gadget,
 static int ci13xxx_stop(struct usb_gadget *gadget,
 			struct usb_gadget_driver *driver)
 {
-	struct ci13xxx *udc = container_of(gadget, struct ci13xxx, gadget);
+	struct ci13xxx *ci = container_of(gadget, struct ci13xxx, gadget);
 	unsigned long flags;
 
-	spin_lock_irqsave(&udc->lock, flags);
+	spin_lock_irqsave(&ci->lock, flags);
 
-	if (!(udc->udc_driver->flags & CI13XXX_PULLUP_ON_VBUS) ||
-			udc->vbus_active) {
-		hw_device_state(udc, 0);
-		if (udc->udc_driver->notify_event)
-			udc->udc_driver->notify_event(udc,
+	if (!(ci->platdata->flags & CI13XXX_PULLUP_ON_VBUS) ||
+			ci->vbus_active) {
+		hw_device_state(ci, 0);
+		if (ci->platdata->notify_event)
+			ci->platdata->notify_event(ci,
 			CI13XXX_CONTROLLER_STOPPED_EVENT);
-		udc->driver = NULL;
-		spin_unlock_irqrestore(&udc->lock, flags);
-		_gadget_stop_activity(&udc->gadget);
-		spin_lock_irqsave(&udc->lock, flags);
-		pm_runtime_put(&udc->gadget.dev);
+		ci->driver = NULL;
+		spin_unlock_irqrestore(&ci->lock, flags);
+		_gadget_stop_activity(&ci->gadget);
+		spin_lock_irqsave(&ci->lock, flags);
+		pm_runtime_put(&ci->gadget.dev);
 	}
 
-	spin_unlock_irqrestore(&udc->lock, flags);
+	spin_unlock_irqrestore(&ci->lock, flags);
 
 	return 0;
 }
@@ -1566,64 +1567,64 @@ static int ci13xxx_stop(struct usb_gadget *gadget,
  * BUS block
  *****************************************************************************/
 /**
- * udc_irq: udc interrupt handler
+ * udc_irq: ci interrupt handler
  *
  * This function returns IRQ_HANDLED if the IRQ has been handled
  * It locks access to registers
  */
-static irqreturn_t udc_irq(struct ci13xxx *udc)
+static irqreturn_t udc_irq(struct ci13xxx *ci)
 {
 	irqreturn_t retval;
 	u32 intr;
 
-	if (udc == NULL)
+	if (ci == NULL)
 		return IRQ_HANDLED;
 
-	spin_lock(&udc->lock);
+	spin_lock(&ci->lock);
 
-	if (udc->udc_driver->flags & CI13XXX_REGS_SHARED) {
-		if (hw_read(udc, OP_USBMODE, USBMODE_CM) !=
+	if (ci->platdata->flags & CI13XXX_REGS_SHARED) {
+		if (hw_read(ci, OP_USBMODE, USBMODE_CM) !=
 				USBMODE_CM_DC) {
-			spin_unlock(&udc->lock);
+			spin_unlock(&ci->lock);
 			return IRQ_NONE;
 		}
 	}
-	intr = hw_test_and_clear_intr_active(udc);
+	intr = hw_test_and_clear_intr_active(ci);
 	dbg_interrupt(intr);
 
 	if (intr) {
 		/* order defines priority - do NOT change it */
 		if (USBi_URI & intr)
-			isr_reset_handler(udc);
+			isr_reset_handler(ci);
 
 		if (USBi_PCI & intr) {
-			udc->gadget.speed = hw_port_is_high_speed(udc) ?
+			ci->gadget.speed = hw_port_is_high_speed(ci) ?
 				USB_SPEED_HIGH : USB_SPEED_FULL;
-			if (udc->suspended && udc->driver->resume) {
-				spin_unlock(&udc->lock);
-				udc->driver->resume(&udc->gadget);
-				spin_lock(&udc->lock);
-				udc->suspended = 0;
+			if (ci->suspended && ci->driver->resume) {
+				spin_unlock(&ci->lock);
+				ci->driver->resume(&ci->gadget);
+				spin_lock(&ci->lock);
+				ci->suspended = 0;
 			}
 		}
 
 		if (USBi_UI  & intr)
-			isr_tr_complete_handler(udc);
+			isr_tr_complete_handler(ci);
 
 		if (USBi_SLI & intr) {
-			if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
-			    udc->driver->suspend) {
-				udc->suspended = 1;
-				spin_unlock(&udc->lock);
-				udc->driver->suspend(&udc->gadget);
-				spin_lock(&udc->lock);
+			if (ci->gadget.speed != USB_SPEED_UNKNOWN &&
+			    ci->driver->suspend) {
+				ci->suspended = 1;
+				spin_unlock(&ci->lock);
+				ci->driver->suspend(&ci->gadget);
+				spin_lock(&ci->lock);
 			}
 		}
 		retval = IRQ_HANDLED;
 	} else {
 		retval = IRQ_NONE;
 	}
-	spin_unlock(&udc->lock);
+	spin_unlock(&ci->lock);
 
 	return retval;
 }
@@ -1640,112 +1641,111 @@ static void udc_release(struct device *dev)
 
 /**
  * udc_start: initialize gadget role
- * @udc: chipidea controller
+ * @ci: chipidea controller
  */
-static int udc_start(struct ci13xxx *udc)
+static int udc_start(struct ci13xxx *ci)
 {
-	struct device *dev = udc->dev;
+	struct device *dev = ci->dev;
 	int retval = 0;
 
-	if (!udc)
-		return -EINVAL;
-
-	spin_lock_init(&udc->lock);
+	spin_lock_init(&ci->lock);
 
-	udc->gadget.ops          = &usb_gadget_ops;
-	udc->gadget.speed        = USB_SPEED_UNKNOWN;
-	udc->gadget.max_speed    = USB_SPEED_HIGH;
-	udc->gadget.is_otg       = 0;
-	udc->gadget.name         = udc->udc_driver->name;
+	ci->gadget.ops          = &usb_gadget_ops;
+	ci->gadget.speed        = USB_SPEED_UNKNOWN;
+	ci->gadget.max_speed    = USB_SPEED_HIGH;
+	ci->gadget.is_otg       = 0;
+	ci->gadget.name         = ci->platdata->name;
 
-	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	INIT_LIST_HEAD(&ci->gadget.ep_list);
 
-	dev_set_name(&udc->gadget.dev, "gadget");
-	udc->gadget.dev.dma_mask = dev->dma_mask;
-	udc->gadget.dev.coherent_dma_mask = dev->coherent_dma_mask;
-	udc->gadget.dev.parent   = dev;
-	udc->gadget.dev.release  = udc_release;
+	dev_set_name(&ci->gadget.dev, "gadget");
+	ci->gadget.dev.dma_mask = dev->dma_mask;
+	ci->gadget.dev.coherent_dma_mask = dev->coherent_dma_mask;
+	ci->gadget.dev.parent   = dev;
+	ci->gadget.dev.release  = udc_release;
 
 	/* alloc resources */
-	udc->qh_pool = dma_pool_create("ci13xxx_qh", dev,
+	ci->qh_pool = dma_pool_create("ci13xxx_qh", dev,
 				       sizeof(struct ci13xxx_qh),
 				       64, CI13XXX_PAGE_SIZE);
-	if (udc->qh_pool == NULL)
+	if (ci->qh_pool == NULL)
 		return -ENOMEM;
 
-	udc->td_pool = dma_pool_create("ci13xxx_td", dev,
+	ci->td_pool = dma_pool_create("ci13xxx_td", dev,
 				       sizeof(struct ci13xxx_td),
 				       64, CI13XXX_PAGE_SIZE);
-	if (udc->td_pool == NULL) {
+	if (ci->td_pool == NULL) {
 		retval = -ENOMEM;
 		goto free_qh_pool;
 	}
 
-	retval = init_eps(udc);
+	retval = init_eps(ci);
 	if (retval)
 		goto free_pools;
 
-	udc->gadget.ep0 = &udc->ep0in->ep;
+	ci->gadget.ep0 = &ci->ep0in->ep;
 
-	udc->transceiver = usb_get_transceiver();
+	if (ci->global_phy)
+		ci->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 
-	if (udc->udc_driver->flags & CI13XXX_REQUIRE_TRANSCEIVER) {
-		if (udc->transceiver == NULL) {
+	if (ci->platdata->flags & CI13XXX_REQUIRE_TRANSCEIVER) {
+		if (ci->transceiver == NULL) {
 			retval = -ENODEV;
 			goto free_pools;
 		}
 	}
 
-	if (!(udc->udc_driver->flags & CI13XXX_REGS_SHARED)) {
-		retval = hw_device_reset(udc, USBMODE_CM_DC);
+	if (!(ci->platdata->flags & CI13XXX_REGS_SHARED)) {
+		retval = hw_device_reset(ci, USBMODE_CM_DC);
 		if (retval)
 			goto put_transceiver;
 	}
 
-	retval = device_register(&udc->gadget.dev);
+	retval = device_register(&ci->gadget.dev);
 	if (retval) {
-		put_device(&udc->gadget.dev);
+		put_device(&ci->gadget.dev);
 		goto put_transceiver;
 	}
 
-	retval = dbg_create_files(&udc->gadget.dev);
+	retval = dbg_create_files(&ci->gadget.dev);
 	if (retval)
 		goto unreg_device;
 
-	if (udc->transceiver) {
-		retval = otg_set_peripheral(udc->transceiver->otg,
-						&udc->gadget);
+	if (!IS_ERR_OR_NULL(ci->transceiver)) {
+		retval = otg_set_peripheral(ci->transceiver->otg,
+						&ci->gadget);
 		if (retval)
 			goto remove_dbg;
 	}
 
-	retval = usb_add_gadget_udc(dev, &udc->gadget);
+	retval = usb_add_gadget_udc(dev, &ci->gadget);
 	if (retval)
 		goto remove_trans;
 
-	pm_runtime_no_callbacks(&udc->gadget.dev);
-	pm_runtime_enable(&udc->gadget.dev);
+	pm_runtime_no_callbacks(&ci->gadget.dev);
+	pm_runtime_enable(&ci->gadget.dev);
 
 	return retval;
 
 remove_trans:
-	if (udc->transceiver) {
-		otg_set_peripheral(udc->transceiver->otg, &udc->gadget);
-		usb_put_transceiver(udc->transceiver);
+	if (!IS_ERR_OR_NULL(ci->transceiver)) {
+		otg_set_peripheral(ci->transceiver->otg, &ci->gadget);
+		if (ci->global_phy)
+			usb_put_phy(ci->transceiver);
 	}
 
 	dev_err(dev, "error = %i\n", retval);
 remove_dbg:
-	dbg_remove_files(&udc->gadget.dev);
+	dbg_remove_files(&ci->gadget.dev);
 unreg_device:
-	device_unregister(&udc->gadget.dev);
+	device_unregister(&ci->gadget.dev);
 put_transceiver:
-	if (udc->transceiver)
-		usb_put_transceiver(udc->transceiver);
+	if (!IS_ERR_OR_NULL(ci->transceiver) && ci->global_phy)
+		usb_put_phy(ci->transceiver);
 free_pools:
-	dma_pool_destroy(udc->td_pool);
+	dma_pool_destroy(ci->td_pool);
 free_qh_pool:
-	dma_pool_destroy(udc->qh_pool);
+	dma_pool_destroy(ci->qh_pool);
 	return retval;
 }
 
@@ -1754,32 +1754,33 @@ free_qh_pool:
  *
  * No interrupts active, the IRQ has been released
  */
-static void udc_stop(struct ci13xxx *udc)
+static void udc_stop(struct ci13xxx *ci)
 {
 	int i;
 
-	if (udc == NULL)
+	if (ci == NULL)
 		return;
 
-	usb_del_gadget_udc(&udc->gadget);
+	usb_del_gadget_udc(&ci->gadget);
 
-	for (i = 0; i < udc->hw_ep_max; i++) {
-		struct ci13xxx_ep *mEp = &udc->ci13xxx_ep[i];
+	for (i = 0; i < ci->hw_ep_max; i++) {
+		struct ci13xxx_ep *mEp = &ci->ci13xxx_ep[i];
 
-		dma_pool_free(udc->qh_pool, mEp->qh.ptr, mEp->qh.dma);
+		dma_pool_free(ci->qh_pool, mEp->qh.ptr, mEp->qh.dma);
 	}
 
-	dma_pool_destroy(udc->td_pool);
-	dma_pool_destroy(udc->qh_pool);
+	dma_pool_destroy(ci->td_pool);
+	dma_pool_destroy(ci->qh_pool);
 
-	if (udc->transceiver) {
-		otg_set_peripheral(udc->transceiver->otg, NULL);
-		usb_put_transceiver(udc->transceiver);
+	if (!IS_ERR_OR_NULL(ci->transceiver)) {
+		otg_set_peripheral(ci->transceiver->otg, NULL);
+		if (ci->global_phy)
+			usb_put_phy(ci->transceiver);
 	}
-	dbg_remove_files(&udc->gadget.dev);
-	device_unregister(&udc->gadget.dev);
+	dbg_remove_files(&ci->gadget.dev);
+	device_unregister(&ci->gadget.dev);
 	/* my kobject is dynamic, I swear! */
-	memset(&udc->gadget, 0, sizeof(udc->gadget));
+	memset(&ci->gadget, 0, sizeof(ci->gadget));
 }
 
 /**
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index bb2e37f7db2..18f4e62aaa7 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -996,7 +996,7 @@ static int acm_probe(struct usb_interface *intf,
 		case USB_CDC_CALL_MANAGEMENT_TYPE:
 			call_management_function = buffer[3];
 			call_interface_num = buffer[4];
-			if ( (quirks & NOT_A_MODEM) == 0 && (call_management_function & 3) != 3)
+			if ((quirks & NOT_A_MODEM) == 0 && (call_management_function & 3) != 3)
 				dev_err(&intf->dev, "This device cannot do calls on its own. It is not a modem.\n");
 			break;
 		default:
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index ee469274a3f..65a55abb791 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -32,8 +32,6 @@
 #define DRIVER_AUTHOR "Oliver Neukum"
 #define DRIVER_DESC "USB Abstract Control Model driver for USB WCM Device Management"
 
-#define HUAWEI_VENDOR_ID	0x12D1
-
 static const struct usb_device_id wdm_ids[] = {
 	{
 		.match_flags = USB_DEVICE_ID_MATCH_INT_CLASS |
@@ -41,29 +39,6 @@ static const struct usb_device_id wdm_ids[] = {
 		.bInterfaceClass = USB_CLASS_COMM,
 		.bInterfaceSubClass = USB_CDC_SUBCLASS_DMM
 	},
-	{
-		/* 
-		 * Huawei E392, E398 and possibly other Qualcomm based modems
-		 * embed the Qualcomm QMI protocol inside CDC on CDC ECM like
-		 * control interfaces.  Userspace access to this is required
-		 * to configure the accompanying data interface
-		 */
-		.match_flags        = USB_DEVICE_ID_MATCH_VENDOR |
-					USB_DEVICE_ID_MATCH_INT_INFO,
-		.idVendor           = HUAWEI_VENDOR_ID,
-		.bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
-		.bInterfaceSubClass = 1,
-		.bInterfaceProtocol = 9, /* NOTE: CDC ECM control interface! */
-	},
-	{
-		 /* Vodafone/Huawei K5005 (12d1:14c8) and similar modems */
-		.match_flags        = USB_DEVICE_ID_MATCH_VENDOR |
-				      USB_DEVICE_ID_MATCH_INT_INFO,
-		.idVendor           = HUAWEI_VENDOR_ID,
-		.bInterfaceClass    = USB_CLASS_VENDOR_SPEC,
-		.bInterfaceSubClass = 1,
-		.bInterfaceProtocol = 57, /* NOTE: CDC ECM control interface! */
-	},
 	{ }
 };
 
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index e0f107948eb..ebb8a9de8b5 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -47,6 +47,7 @@
 #include <linux/notifier.h>
 #include <linux/security.h>
 #include <linux/user_namespace.h>
+#include <linux/scatterlist.h>
 #include <asm/uaccess.h>
 #include <asm/byteorder.h>
 #include <linux/moduleparam.h>
@@ -55,6 +56,7 @@
 
 #define USB_MAXBUS			64
 #define USB_DEVICE_MAX			USB_MAXBUS * 128
+#define USB_SG_SIZE			16384 /* split-size for large txs */
 
 /* Mutual exclusion for removal, open, and release */
 DEFINE_MUTEX(usbfs_mutex);
@@ -285,9 +287,16 @@ static struct async *alloc_async(unsigned int numisoframes)
 
 static void free_async(struct async *as)
 {
+	int i;
+
 	put_pid(as->pid);
 	if (as->cred)
 		put_cred(as->cred);
+	for (i = 0; i < as->urb->num_sgs; i++) {
+		if (sg_page(&as->urb->sg[i]))
+			kfree(sg_virt(&as->urb->sg[i]));
+	}
+	kfree(as->urb->sg);
 	kfree(as->urb->transfer_buffer);
 	kfree(as->urb->setup_packet);
 	usb_free_urb(as->urb);
@@ -388,6 +397,53 @@ static void snoop_urb(struct usb_device *udev,
 	}
 }
 
+static void snoop_urb_data(struct urb *urb, unsigned len)
+{
+	int i, size;
+
+	if (!usbfs_snoop)
+		return;
+
+	if (urb->num_sgs == 0) {
+		print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_NONE, 32, 1,
+			urb->transfer_buffer, len, 1);
+		return;
+	}
+
+	for (i = 0; i < urb->num_sgs && len; i++) {
+		size = (len > USB_SG_SIZE) ? USB_SG_SIZE : len;
+		print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_NONE, 32, 1,
+			sg_virt(&urb->sg[i]), size, 1);
+		len -= size;
+	}
+}
+
+static int copy_urb_data_to_user(u8 __user *userbuffer, struct urb *urb)
+{
+	unsigned i, len, size;
+
+	if (urb->number_of_packets > 0)		/* Isochronous */
+		len = urb->transfer_buffer_length;
+	else					/* Non-Isoc */
+		len = urb->actual_length;
+
+	if (urb->num_sgs == 0) {
+		if (copy_to_user(userbuffer, urb->transfer_buffer, len))
+			return -EFAULT;
+		return 0;
+	}
+
+	for (i = 0; i < urb->num_sgs && len; i++) {
+		size = (len > USB_SG_SIZE) ? USB_SG_SIZE : len;
+		if (copy_to_user(userbuffer, sg_virt(&urb->sg[i]), size))
+			return -EFAULT;
+		userbuffer += size;
+		len -= size;
+	}
+
+	return 0;
+}
+
 #define AS_CONTINUATION	1
 #define AS_UNLINK	2
 
@@ -454,9 +510,10 @@ static void async_completed(struct urb *urb)
 	}
 	snoop(&urb->dev->dev, "urb complete\n");
 	snoop_urb(urb->dev, as->userurb, urb->pipe, urb->actual_length,
-			as->status, COMPLETE,
-			((urb->transfer_flags & URB_DIR_MASK) == USB_DIR_OUT) ?
-				NULL : urb->transfer_buffer, urb->actual_length);
+			as->status, COMPLETE, NULL, 0);
+	if ((urb->transfer_flags & URB_DIR_MASK) == USB_DIR_IN)
+		snoop_urb_data(urb, urb->actual_length);
+
 	if (as->status < 0 && as->bulk_addr && as->status != -ECONNRESET &&
 			as->status != -ENOENT)
 		cancel_bulk_urbs(ps, as->bulk_addr);
@@ -1114,8 +1171,8 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 	struct async *as = NULL;
 	struct usb_ctrlrequest *dr = NULL;
 	unsigned int u, totlen, isofrmlen;
-	int ret, ifnum = -1;
-	int is_in;
+	int i, ret, is_in, num_sgs = 0, ifnum = -1;
+	void *buf;
 
 	if (uurb->flags & ~(USBDEVFS_URB_ISO_ASAP |
 				USBDEVFS_URB_SHORT_NOT_OK |
@@ -1199,6 +1256,9 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 			goto interrupt_urb;
 		}
 		uurb->number_of_packets = 0;
+		num_sgs = DIV_ROUND_UP(uurb->buffer_length, USB_SG_SIZE);
+		if (num_sgs == 1 || num_sgs > ps->dev->bus->sg_tablesize)
+			num_sgs = 0;
 		break;
 
 	case USBDEVFS_URB_TYPE_INTERRUPT:
@@ -1255,26 +1315,67 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 		ret = -ENOMEM;
 		goto error;
 	}
-	u += sizeof(struct async) + sizeof(struct urb) + uurb->buffer_length;
+
+	u += sizeof(struct async) + sizeof(struct urb) + uurb->buffer_length +
+	     num_sgs * sizeof(struct scatterlist);
 	ret = usbfs_increase_memory_usage(u);
 	if (ret)
 		goto error;
 	as->mem_usage = u;
 
-	if (uurb->buffer_length > 0) {
+	if (num_sgs) {
+		as->urb->sg = kmalloc(num_sgs * sizeof(struct scatterlist),
+				      GFP_KERNEL);
+		if (!as->urb->sg) {
+			ret = -ENOMEM;
+			goto error;
+		}
+		as->urb->num_sgs = num_sgs;
+		sg_init_table(as->urb->sg, as->urb->num_sgs);
+
+		totlen = uurb->buffer_length;
+		for (i = 0; i < as->urb->num_sgs; i++) {
+			u = (totlen > USB_SG_SIZE) ? USB_SG_SIZE : totlen;
+			buf = kmalloc(u, GFP_KERNEL);
+			if (!buf) {
+				ret = -ENOMEM;
+				goto error;
+			}
+			sg_set_buf(&as->urb->sg[i], buf, u);
+
+			if (!is_in) {
+				if (copy_from_user(buf, uurb->buffer, u)) {
+					ret = -EFAULT;
+					goto error;
+				}
+			}
+			totlen -= u;
+		}
+	} else if (uurb->buffer_length > 0) {
 		as->urb->transfer_buffer = kmalloc(uurb->buffer_length,
 				GFP_KERNEL);
 		if (!as->urb->transfer_buffer) {
 			ret = -ENOMEM;
 			goto error;
 		}
-		/* Isochronous input data may end up being discontiguous
-		 * if some of the packets are short.  Clear the buffer so
-		 * that the gaps don't leak kernel data to userspace.
-		 */
-		if (is_in && uurb->type == USBDEVFS_URB_TYPE_ISO)
+
+		if (!is_in) {
+			if (copy_from_user(as->urb->transfer_buffer,
+					   uurb->buffer,
+					   uurb->buffer_length)) {
+				ret = -EFAULT;
+				goto error;
+			}
+		} else if (uurb->type == USBDEVFS_URB_TYPE_ISO) {
+			/*
+			 * Isochronous input data may end up being
+			 * discontiguous if some of the packets are short.
+			 * Clear the buffer so that the gaps don't leak
+			 * kernel data to userspace.
+			 */
 			memset(as->urb->transfer_buffer, 0,
 					uurb->buffer_length);
+		}
 	}
 	as->urb->dev = ps->dev;
 	as->urb->pipe = (uurb->type << 30) |
@@ -1328,17 +1429,12 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 	as->pid = get_pid(task_pid(current));
 	as->cred = get_current_cred();
 	security_task_getsecid(current, &as->secid);
-	if (!is_in && uurb->buffer_length > 0) {
-		if (copy_from_user(as->urb->transfer_buffer, uurb->buffer,
-				uurb->buffer_length)) {
-			ret = -EFAULT;
-			goto error;
-		}
-	}
 	snoop_urb(ps->dev, as->userurb, as->urb->pipe,
 			as->urb->transfer_buffer_length, 0, SUBMIT,
-			is_in ? NULL : as->urb->transfer_buffer,
-				uurb->buffer_length);
+			NULL, 0);
+	if (!is_in)
+		snoop_urb_data(as->urb, as->urb->transfer_buffer_length);
+
 	async_newpending(as);
 
 	if (usb_endpoint_xfer_bulk(&ep->desc)) {
@@ -1433,11 +1529,7 @@ static int processcompl(struct async *as, void __user * __user *arg)
 	unsigned int i;
 
 	if (as->userbuffer && urb->actual_length) {
-		if (urb->number_of_packets > 0)		/* Isochronous */
-			i = urb->transfer_buffer_length;
-		else					/* Non-Isoc */
-			i = urb->actual_length;
-		if (copy_to_user(as->userbuffer, urb->transfer_buffer, i))
+		if (copy_urb_data_to_user(as->userbuffer, urb))
 			goto err_out;
 	}
 	if (put_user(as->status, &userurb->status))
@@ -1604,10 +1696,10 @@ static int processcompl_compat(struct async *as, void __user * __user *arg)
 	void __user *addr = as->userurb;
 	unsigned int i;
 
-	if (as->userbuffer && urb->actual_length)
-		if (copy_to_user(as->userbuffer, urb->transfer_buffer,
-				 urb->actual_length))
+	if (as->userbuffer && urb->actual_length) {
+		if (copy_urb_data_to_user(as->userbuffer, urb))
 			return -EFAULT;
+	}
 	if (put_user(as->status, &userurb->status))
 		return -EFAULT;
 	if (put_user(urb->actual_length, &userurb->actual_length))
@@ -1820,6 +1912,22 @@ static int proc_release_port(struct dev_state *ps, void __user *arg)
 	return usb_hub_release_port(ps->dev, portnum, ps);
 }
 
+static int proc_get_capabilities(struct dev_state *ps, void __user *arg)
+{
+	__u32 caps;
+
+	caps = USBDEVFS_CAP_ZERO_PACKET | USBDEVFS_CAP_NO_PACKET_SIZE_LIM;
+	if (!ps->dev->bus->no_stop_on_short)
+		caps |= USBDEVFS_CAP_BULK_CONTINUATION;
+	if (ps->dev->bus->sg_tablesize)
+		caps |= USBDEVFS_CAP_BULK_SCATTER_GATHER;
+
+	if (put_user(caps, (__u32 __user *)arg))
+		return -EFAULT;
+
+	return 0;
+}
+
 /*
  * NOTE:  All requests here that have interface numbers as parameters
  * are assuming that somehow the configuration has been prevented from
@@ -1990,6 +2098,9 @@ static long usbdev_do_ioctl(struct file *file, unsigned int cmd,
 		snoop(&dev->dev, "%s: RELEASE_PORT\n", __func__);
 		ret = proc_release_port(ps, p);
 		break;
+	case USBDEVFS_GET_CAPABILITIES:
+		ret = proc_get_capabilities(ps, p);
+		break;
 	}
 	usb_unlock_device(dev);
 	if (ret >= 0)
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index f536aebc958..445455a4429 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -367,6 +367,7 @@ static int usb_probe_interface(struct device *dev)
 	return error;
 
  err:
+	usb_set_intfdata(intf, NULL);
 	intf->needs_remote_wakeup = 0;
 	intf->condition = USB_INTERFACE_UNBOUND;
 	usb_cancel_queued_reset(intf);
@@ -606,30 +607,19 @@ int usb_match_device(struct usb_device *dev, const struct usb_device_id *id)
 }
 
 /* returns 0 if no match, 1 if match */
-int usb_match_one_id(struct usb_interface *interface,
-		     const struct usb_device_id *id)
+int usb_match_one_id_intf(struct usb_device *dev,
+			  struct usb_host_interface *intf,
+			  const struct usb_device_id *id)
 {
-	struct usb_host_interface *intf;
-	struct usb_device *dev;
-
-	/* proc_connectinfo in devio.c may call us with id == NULL. */
-	if (id == NULL)
-		return 0;
-
-	intf = interface->cur_altsetting;
-	dev = interface_to_usbdev(interface);
-
-	if (!usb_match_device(dev, id))
-		return 0;
-
-	/* The interface class, subclass, and protocol should never be
+	/* The interface class, subclass, protocol and number should never be
 	 * checked for a match if the device class is Vendor Specific,
 	 * unless the match record specifies the Vendor ID. */
 	if (dev->descriptor.bDeviceClass == USB_CLASS_VENDOR_SPEC &&
 			!(id->match_flags & USB_DEVICE_ID_MATCH_VENDOR) &&
 			(id->match_flags & (USB_DEVICE_ID_MATCH_INT_CLASS |
 				USB_DEVICE_ID_MATCH_INT_SUBCLASS |
-				USB_DEVICE_ID_MATCH_INT_PROTOCOL)))
+				USB_DEVICE_ID_MATCH_INT_PROTOCOL |
+				USB_DEVICE_ID_MATCH_INT_NUMBER)))
 		return 0;
 
 	if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_CLASS) &&
@@ -644,8 +634,32 @@ int usb_match_one_id(struct usb_interface *interface,
 	    (id->bInterfaceProtocol != intf->desc.bInterfaceProtocol))
 		return 0;
 
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_NUMBER) &&
+	    (id->bInterfaceNumber != intf->desc.bInterfaceNumber))
+		return 0;
+
 	return 1;
 }
+
+/* returns 0 if no match, 1 if match */
+int usb_match_one_id(struct usb_interface *interface,
+		     const struct usb_device_id *id)
+{
+	struct usb_host_interface *intf;
+	struct usb_device *dev;
+
+	/* proc_connectinfo in devio.c may call us with id == NULL. */
+	if (id == NULL)
+		return 0;
+
+	intf = interface->cur_altsetting;
+	dev = interface_to_usbdev(interface);
+
+	if (!usb_match_device(dev, id))
+		return 0;
+
+	return usb_match_one_id_intf(dev, intf, id);
+}
 EXPORT_SYMBOL_GPL(usb_match_one_id);
 
 /**
diff --git a/drivers/usb/core/file.c b/drivers/usb/core/file.c
index e673b26e598..e5387a47ef6 100644
--- a/drivers/usb/core/file.c
+++ b/drivers/usb/core/file.c
@@ -92,7 +92,7 @@ static int init_usb_class(void)
 	}
 
 	kref_init(&usb_class->kref);
-	usb_class->class = class_create(THIS_MODULE, "usb");
+	usb_class->class = class_create(THIS_MODULE, "usbmisc");
 	if (IS_ERR(usb_class->class)) {
 		result = IS_ERR(usb_class->class);
 		printk(KERN_ERR "class_create failed for usb devices\n");
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 190b1ec7bdc..bc84106ac05 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -1398,7 +1398,15 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 	    && !(urb->transfer_flags & URB_NO_TRANSFER_DMA_MAP)) {
 		if (hcd->self.uses_dma) {
 			if (urb->num_sgs) {
-				int n = dma_map_sg(
+				int n;
+
+				/* We don't support sg for isoc transfers ! */
+				if (usb_endpoint_xfer_isoc(&urb->ep->desc)) {
+					WARN_ON(1);
+					return -EINVAL;
+				}
+
+				n = dma_map_sg(
 						hcd->self.controller,
 						urb->sg,
 						urb->num_sgs,
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 8fb484984c8..128a804c42f 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -20,10 +20,12 @@
 #include <linux/usb.h>
 #include <linux/usbdevice_fs.h>
 #include <linux/usb/hcd.h>
+#include <linux/usb/otg.h>
 #include <linux/usb/quirks.h>
 #include <linux/kthread.h>
 #include <linux/mutex.h>
 #include <linux/freezer.h>
+#include <linux/random.h>
 
 #include <asm/uaccess.h>
 #include <asm/byteorder.h>
@@ -81,7 +83,7 @@ struct usb_hub {
 	u8			indicator[USB_MAXCHILDREN];
 	struct delayed_work	leds;
 	struct delayed_work	init_work;
-	void			**port_owners;
+	struct dev_state	**port_owners;
 };
 
 static inline int hub_is_superspeed(struct usb_device *hdev)
@@ -1271,7 +1273,8 @@ static int hub_configure(struct usb_hub *hub,
 
 	hdev->children = kzalloc(hdev->maxchild *
 				sizeof(struct usb_device *), GFP_KERNEL);
-	hub->port_owners = kzalloc(hdev->maxchild * sizeof(void *), GFP_KERNEL);
+	hub->port_owners = kzalloc(hdev->maxchild * sizeof(struct dev_state *),
+				GFP_KERNEL);
 	if (!hdev->children || !hub->port_owners) {
 		ret = -ENOMEM;
 		goto fail;
@@ -1649,7 +1652,7 @@ hub_ioctl(struct usb_interface *intf, unsigned int code, void *user_data)
  * to one of these "claimed" ports, the program will "own" the device.
  */
 static int find_port_owner(struct usb_device *hdev, unsigned port1,
-		void ***ppowner)
+		struct dev_state ***ppowner)
 {
 	if (hdev->state == USB_STATE_NOTATTACHED)
 		return -ENODEV;
@@ -1664,10 +1667,11 @@ static int find_port_owner(struct usb_device *hdev, unsigned port1,
 }
 
 /* In the following three functions, the caller must hold hdev's lock */
-int usb_hub_claim_port(struct usb_device *hdev, unsigned port1, void *owner)
+int usb_hub_claim_port(struct usb_device *hdev, unsigned port1,
+		       struct dev_state *owner)
 {
 	int rc;
-	void **powner;
+	struct dev_state **powner;
 
 	rc = find_port_owner(hdev, port1, &powner);
 	if (rc)
@@ -1678,10 +1682,11 @@ int usb_hub_claim_port(struct usb_device *hdev, unsigned port1, void *owner)
 	return rc;
 }
 
-int usb_hub_release_port(struct usb_device *hdev, unsigned port1, void *owner)
+int usb_hub_release_port(struct usb_device *hdev, unsigned port1,
+			 struct dev_state *owner)
 {
 	int rc;
-	void **powner;
+	struct dev_state **powner;
 
 	rc = find_port_owner(hdev, port1, &powner);
 	if (rc)
@@ -1692,10 +1697,10 @@ int usb_hub_release_port(struct usb_device *hdev, unsigned port1, void *owner)
 	return rc;
 }
 
-void usb_hub_release_all_ports(struct usb_device *hdev, void *owner)
+void usb_hub_release_all_ports(struct usb_device *hdev, struct dev_state *owner)
 {
 	int n;
-	void **powner;
+	struct dev_state **powner;
 
 	n = find_port_owner(hdev, 1, &powner);
 	if (n == 0) {
@@ -2065,7 +2070,7 @@ static int usb_enumerate_device(struct usb_device *udev)
 		if (err < 0) {
 			dev_err(&udev->dev, "can't read configurations, error %d\n",
 				err);
-			goto fail;
+			return err;
 		}
 	}
 	if (udev->wusb == 1 && udev->authorized == 0) {
@@ -2081,8 +2086,12 @@ static int usb_enumerate_device(struct usb_device *udev)
 		udev->serial = usb_cache_string(udev, udev->descriptor.iSerialNumber);
 	}
 	err = usb_enumerate_device_otg(udev);
-fail:
-	return err;
+	if (err < 0)
+		return err;
+
+	usb_detect_interface_quirks(udev);
+
+	return 0;
 }
 
 static void set_usb_port_removable(struct usb_device *udev)
@@ -2173,6 +2182,14 @@ int usb_new_device(struct usb_device *udev)
 	/* Tell the world! */
 	announce_device(udev);
 
+	if (udev->serial)
+		add_device_randomness(udev->serial, strlen(udev->serial));
+	if (udev->product)
+		add_device_randomness(udev->product, strlen(udev->product));
+	if (udev->manufacturer)
+		add_device_randomness(udev->manufacturer,
+				      strlen(udev->manufacturer));
+
 	device_enable_async_suspend(&udev->dev);
 
 	/*
@@ -2611,6 +2628,50 @@ static int check_port_resume_type(struct usb_device *udev,
 	return status;
 }
 
+int usb_disable_ltm(struct usb_device *udev)
+{
+	struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+
+	/* Check if the roothub and device supports LTM. */
+	if (!usb_device_supports_ltm(hcd->self.root_hub) ||
+			!usb_device_supports_ltm(udev))
+		return 0;
+
+	/* Clear Feature LTM Enable can only be sent if the device is
+	 * configured.
+	 */
+	if (!udev->actconfig)
+		return 0;
+
+	return usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			USB_REQ_CLEAR_FEATURE, USB_RECIP_DEVICE,
+			USB_DEVICE_LTM_ENABLE, 0, NULL, 0,
+			USB_CTRL_SET_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(usb_disable_ltm);
+
+void usb_enable_ltm(struct usb_device *udev)
+{
+	struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+
+	/* Check if the roothub and device supports LTM. */
+	if (!usb_device_supports_ltm(hcd->self.root_hub) ||
+			!usb_device_supports_ltm(udev))
+		return;
+
+	/* Set Feature LTM Enable can only be sent if the device is
+	 * configured.
+	 */
+	if (!udev->actconfig)
+		return;
+
+	usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+			USB_REQ_SET_FEATURE, USB_RECIP_DEVICE,
+			USB_DEVICE_LTM_ENABLE, 0, NULL, 0,
+			USB_CTRL_SET_TIMEOUT);
+}
+EXPORT_SYMBOL_GPL(usb_enable_ltm);
+
 #ifdef	CONFIG_USB_SUSPEND
 
 /*
@@ -2706,6 +2767,11 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	if (udev->usb2_hw_lpm_enabled == 1)
 		usb_set_usb2_hardware_lpm(udev, 0);
 
+	if (usb_disable_ltm(udev)) {
+		dev_err(&udev->dev, "%s Failed to disable LTM before suspend\n.",
+				__func__);
+		return -ENOMEM;
+	}
 	if (usb_unlocked_disable_lpm(udev)) {
 		dev_err(&udev->dev, "%s Failed to disable LPM before suspend\n.",
 				__func__);
@@ -2735,7 +2801,8 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 		if (udev->usb2_hw_lpm_capable == 1)
 			usb_set_usb2_hardware_lpm(udev, 1);
 
-		/* Try to enable USB3 LPM again */
+		/* Try to enable USB3 LTM and LPM again */
+		usb_enable_ltm(udev);
 		usb_unlocked_enable_lpm(udev);
 
 		/* System sleep transitions should never fail */
@@ -2936,7 +3003,8 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		if (udev->usb2_hw_lpm_capable == 1)
 			usb_set_usb2_hardware_lpm(udev, 1);
 
-		/* Try to enable USB3 LPM */
+		/* Try to enable USB3 LTM and LPM */
+		usb_enable_ltm(udev);
 		usb_unlocked_enable_lpm(udev);
 	}
 
@@ -3489,6 +3557,15 @@ EXPORT_SYMBOL_GPL(usb_unlocked_disable_lpm);
 
 void usb_unlocked_enable_lpm(struct usb_device *udev) { }
 EXPORT_SYMBOL_GPL(usb_unlocked_enable_lpm);
+
+int usb_disable_ltm(struct usb_device *udev)
+{
+	return 0;
+}
+EXPORT_SYMBOL_GPL(usb_disable_ltm);
+
+void usb_enable_ltm(struct usb_device *udev) { }
+EXPORT_SYMBOL_GPL(usb_enable_ltm);
 #endif
 
 
@@ -4038,6 +4115,13 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		}
 	}
 
+	if (hcd->phy && !hdev->parent) {
+		if (portstatus & USB_PORT_STAT_CONNECTION)
+			usb_phy_notify_connect(hcd->phy, port1);
+		else
+			usb_phy_notify_disconnect(hcd->phy, port1);
+	}
+
 	/* Return now if debouncing failed or nothing is connected or
 	 * the device was "removed".
 	 */
@@ -4672,6 +4756,23 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 	}
 	parent_hub = hdev_to_hub(parent_hdev);
 
+	/* Disable LPM and LTM while we reset the device and reinstall the alt
+	 * settings.  Device-initiated LPM settings, and system exit latency
+	 * settings are cleared when the device is reset, so we have to set
+	 * them up again.
+	 */
+	ret = usb_unlocked_disable_lpm(udev);
+	if (ret) {
+		dev_err(&udev->dev, "%s Failed to disable LPM\n.", __func__);
+		goto re_enumerate;
+	}
+	ret = usb_disable_ltm(udev);
+	if (ret) {
+		dev_err(&udev->dev, "%s Failed to disable LTM\n.",
+				__func__);
+		goto re_enumerate;
+	}
+
 	set_bit(port1, parent_hub->busy_bits);
 	for (i = 0; i < SET_CONFIG_TRIES; ++i) {
 
@@ -4699,22 +4800,11 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		goto done;
 
 	mutex_lock(hcd->bandwidth_mutex);
-	/* Disable LPM while we reset the device and reinstall the alt settings.
-	 * Device-initiated LPM settings, and system exit latency settings are
-	 * cleared when the device is reset, so we have to set them up again.
-	 */
-	ret = usb_disable_lpm(udev);
-	if (ret) {
-		dev_err(&udev->dev, "%s Failed to disable LPM\n.", __func__);
-		mutex_unlock(hcd->bandwidth_mutex);
-		goto done;
-	}
 	ret = usb_hcd_alloc_bandwidth(udev, udev->actconfig, NULL, NULL);
 	if (ret < 0) {
 		dev_warn(&udev->dev,
 				"Busted HC?  Not enough HCD resources for "
 				"old configuration.\n");
-		usb_enable_lpm(udev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		goto re_enumerate;
 	}
@@ -4726,7 +4816,6 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		dev_err(&udev->dev,
 			"can't restore configuration #%d (error=%d)\n",
 			udev->actconfig->desc.bConfigurationValue, ret);
-		usb_enable_lpm(udev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		goto re_enumerate;
   	}
@@ -4765,17 +4854,18 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 				desc->bInterfaceNumber,
 				desc->bAlternateSetting,
 				ret);
-			usb_unlocked_enable_lpm(udev);
 			goto re_enumerate;
 		}
 	}
 
-	/* Now that the alt settings are re-installed, enable LPM. */
-	usb_unlocked_enable_lpm(udev);
 done:
+	/* Now that the alt settings are re-installed, enable LTM and LPM. */
+	usb_unlocked_enable_lpm(udev);
+	usb_enable_ltm(udev);
 	return 0;
  
 re_enumerate:
+	/* LPM state doesn't matter when we're about to destroy the device. */
 	hub_port_logical_disconnect(parent_hub, port1);
 	return -ENODEV;
 }
diff --git a/drivers/usb/core/message.c b/drivers/usb/core/message.c
index bdd1c6749d8..0ab7da2283e 100644
--- a/drivers/usb/core/message.c
+++ b/drivers/usb/core/message.c
@@ -1174,6 +1174,8 @@ void usb_disable_device(struct usb_device *dev, int skip_ep0)
 			put_device(&dev->actconfig->interface[i]->dev);
 			dev->actconfig->interface[i] = NULL;
 		}
+		usb_unlocked_disable_lpm(dev);
+		usb_disable_ltm(dev);
 		dev->actconfig = NULL;
 		if (dev->state == USB_STATE_CONFIGURED)
 			usb_set_device_state(dev, USB_STATE_ADDRESS);
@@ -1559,7 +1561,7 @@ static int usb_if_uevent(struct device *dev, struct kobj_uevent_env *env)
 
 	if (add_uevent_var(env,
 		   "MODALIAS=usb:"
-		   "v%04Xp%04Xd%04Xdc%02Xdsc%02Xdp%02Xic%02Xisc%02Xip%02X",
+		   "v%04Xp%04Xd%04Xdc%02Xdsc%02Xdp%02Xic%02Xisc%02Xip%02Xin%02X",
 		   le16_to_cpu(usb_dev->descriptor.idVendor),
 		   le16_to_cpu(usb_dev->descriptor.idProduct),
 		   le16_to_cpu(usb_dev->descriptor.bcdDevice),
@@ -1568,7 +1570,8 @@ static int usb_if_uevent(struct device *dev, struct kobj_uevent_env *env)
 		   usb_dev->descriptor.bDeviceProtocol,
 		   alt->desc.bInterfaceClass,
 		   alt->desc.bInterfaceSubClass,
-		   alt->desc.bInterfaceProtocol))
+		   alt->desc.bInterfaceProtocol,
+		   alt->desc.bInterfaceNumber))
 		return -ENOMEM;
 
 	return 0;
@@ -1791,14 +1794,15 @@ free_interfaces:
 	 * installed, so that the xHCI driver can recalculate the U1/U2
 	 * timeouts.
 	 */
-	if (usb_disable_lpm(dev)) {
+	if (dev->actconfig && usb_disable_lpm(dev)) {
 		dev_err(&dev->dev, "%s Failed to disable LPM\n.", __func__);
 		mutex_unlock(hcd->bandwidth_mutex);
 		return -ENOMEM;
 	}
 	ret = usb_hcd_alloc_bandwidth(dev, cp, NULL, NULL);
 	if (ret < 0) {
-		usb_enable_lpm(dev);
+		if (dev->actconfig)
+			usb_enable_lpm(dev);
 		mutex_unlock(hcd->bandwidth_mutex);
 		usb_autosuspend_device(dev);
 		goto free_interfaces;
@@ -1818,7 +1822,7 @@ free_interfaces:
 	if (!cp) {
 		usb_set_device_state(dev, USB_STATE_ADDRESS);
 		usb_hcd_alloc_bandwidth(dev, NULL, NULL, NULL);
-		usb_enable_lpm(dev);
+		/* Leave LPM disabled while the device is unconfigured. */
 		mutex_unlock(hcd->bandwidth_mutex);
 		usb_autosuspend_device(dev);
 		goto free_interfaces;
@@ -1876,6 +1880,8 @@ free_interfaces:
 
 	/* Now that the interfaces are installed, re-enable LPM. */
 	usb_unlocked_enable_lpm(dev);
+	/* Enable LTM if it was turned off by usb_disable_device. */
+	usb_enable_ltm(dev);
 
 	/* Now that all the interfaces are set up, register them
 	 * to trigger binding of drivers to interfaces.  probe()
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 32d3adc315f..f15501f4c58 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -15,17 +15,22 @@
 #include <linux/usb/quirks.h>
 #include "usb.h"
 
-/* List of quirky USB devices.  Please keep this list ordered by:
+/* Lists of quirky USB devices, split in device quirks and interface quirks.
+ * Device quirks are applied at the very beginning of the enumeration process,
+ * right after reading the device descriptor. They can thus only match on device
+ * information.
+ *
+ * Interface quirks are applied after reading all the configuration descriptors.
+ * They can match on both device and interface information.
+ *
+ * Note that the DELAY_INIT and HONOR_BNUMINTERFACES quirks do not make sense as
+ * interface quirks, as they only influence the enumeration process which is run
+ * before processing the interface quirks.
+ *
+ * Please keep the lists ordered by:
  * 	1) Vendor ID
  * 	2) Product ID
  * 	3) Class ID
- *
- * as we want specific devices to be overridden first, and only after that, any
- * class specific quirks.
- *
- * Right now the logic aborts if it finds a valid device in the table, we might
- * want to change that in the future if it turns out that a whole class of
- * devices is broken...
  */
 static const struct usb_device_id usb_quirk_list[] = {
 	/* CBM - Flash disk */
@@ -38,53 +43,23 @@ static const struct usb_device_id usb_quirk_list[] = {
 	/* Creative SB Audigy 2 NX */
 	{ USB_DEVICE(0x041e, 0x3020), .driver_info = USB_QUIRK_RESET_RESUME },
 
-	/* Logitech Webcam C200 */
-	{ USB_DEVICE(0x046d, 0x0802), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam C250 */
-	{ USB_DEVICE(0x046d, 0x0804), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam C300 */
-	{ USB_DEVICE(0x046d, 0x0805), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam B/C500 */
-	{ USB_DEVICE(0x046d, 0x0807), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam C600 */
-	{ USB_DEVICE(0x046d, 0x0808), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam Pro 9000 */
-	{ USB_DEVICE(0x046d, 0x0809), .driver_info = USB_QUIRK_RESET_RESUME },
+	/* Logitech Quickcam Fusion */
+	{ USB_DEVICE(0x046d, 0x08c1), .driver_info = USB_QUIRK_RESET_RESUME },
 
-	/* Logitech Webcam C905 */
-	{ USB_DEVICE(0x046d, 0x080a), .driver_info = USB_QUIRK_RESET_RESUME },
+	/* Logitech Quickcam Orbit MP */
+	{ USB_DEVICE(0x046d, 0x08c2), .driver_info = USB_QUIRK_RESET_RESUME },
 
-	/* Logitech Webcam C210 */
-	{ USB_DEVICE(0x046d, 0x0819), .driver_info = USB_QUIRK_RESET_RESUME },
+	/* Logitech Quickcam Pro for Notebook */
+	{ USB_DEVICE(0x046d, 0x08c3), .driver_info = USB_QUIRK_RESET_RESUME },
 
-	/* Logitech Webcam C260 */
-	{ USB_DEVICE(0x046d, 0x081a), .driver_info = USB_QUIRK_RESET_RESUME },
+	/* Logitech Quickcam Pro 5000 */
+	{ USB_DEVICE(0x046d, 0x08c5), .driver_info = USB_QUIRK_RESET_RESUME },
 
-	/* Logitech Webcam C310 */
-	{ USB_DEVICE(0x046d, 0x081b), .driver_info = USB_QUIRK_RESET_RESUME },
+	/* Logitech Quickcam OEM Dell Notebook */
+	{ USB_DEVICE(0x046d, 0x08c6), .driver_info = USB_QUIRK_RESET_RESUME },
 
-	/* Logitech Webcam C910 */
-	{ USB_DEVICE(0x046d, 0x0821), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam C160 */
-	{ USB_DEVICE(0x046d, 0x0824), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Webcam C270 */
-	{ USB_DEVICE(0x046d, 0x0825), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Quickcam Pro 9000 */
-	{ USB_DEVICE(0x046d, 0x0990), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Quickcam E3500 */
-	{ USB_DEVICE(0x046d, 0x09a4), .driver_info = USB_QUIRK_RESET_RESUME },
-
-	/* Logitech Quickcam Vision Pro */
-	{ USB_DEVICE(0x046d, 0x09a6), .driver_info = USB_QUIRK_RESET_RESUME },
+	/* Logitech Quickcam OEM Cisco VT Camera II */
+	{ USB_DEVICE(0x046d, 0x08c7), .driver_info = USB_QUIRK_RESET_RESUME },
 
 	/* Logitech Harmony 700-series */
 	{ USB_DEVICE(0x046d, 0xc122), .driver_info = USB_QUIRK_DELAY_INIT },
@@ -156,16 +131,57 @@ static const struct usb_device_id usb_quirk_list[] = {
 	{ }  /* terminating entry must be last */
 };
 
-static const struct usb_device_id *find_id(struct usb_device *udev)
+static const struct usb_device_id usb_interface_quirk_list[] = {
+	/* Logitech UVC Cameras */
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x046d, USB_CLASS_VIDEO, 1, 0),
+	  .driver_info = USB_QUIRK_RESET_RESUME },
+
+	{ }  /* terminating entry must be last */
+};
+
+static bool usb_match_any_interface(struct usb_device *udev,
+				    const struct usb_device_id *id)
 {
-	const struct usb_device_id *id = usb_quirk_list;
+	unsigned int i;
 
-	for (; id->idVendor || id->bDeviceClass || id->bInterfaceClass ||
-			id->driver_info; id++) {
-		if (usb_match_device(udev, id))
-			return id;
+	for (i = 0; i < udev->descriptor.bNumConfigurations; ++i) {
+		struct usb_host_config *cfg = &udev->config[i];
+		unsigned int j;
+
+		for (j = 0; j < cfg->desc.bNumInterfaces; ++j) {
+			struct usb_interface_cache *cache;
+			struct usb_host_interface *intf;
+
+			cache = cfg->intf_cache[j];
+			if (cache->num_altsetting == 0)
+				continue;
+
+			intf = &cache->altsetting[0];
+			if (usb_match_one_id_intf(udev, intf, id))
+				return true;
+		}
+	}
+
+	return false;
+}
+
+static u32 __usb_detect_quirks(struct usb_device *udev,
+			       const struct usb_device_id *id)
+{
+	u32 quirks = 0;
+
+	for (; id->match_flags; id++) {
+		if (!usb_match_device(udev, id))
+			continue;
+
+		if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_INFO) &&
+		    !usb_match_any_interface(udev, id))
+			continue;
+
+		quirks |= (u32)(id->driver_info);
 	}
-	return NULL;
+
+	return quirks;
 }
 
 /*
@@ -173,14 +189,10 @@ static const struct usb_device_id *find_id(struct usb_device *udev)
  */
 void usb_detect_quirks(struct usb_device *udev)
 {
-	const struct usb_device_id *id = usb_quirk_list;
-
-	id = find_id(udev);
-	if (id)
-		udev->quirks = (u32)(id->driver_info);
+	udev->quirks = __usb_detect_quirks(udev, usb_quirk_list);
 	if (udev->quirks)
 		dev_dbg(&udev->dev, "USB quirks for this device: %x\n",
-				udev->quirks);
+			udev->quirks);
 
 	/* For the present, all devices default to USB-PERSIST enabled */
 #if 0		/* was: #ifdef CONFIG_PM */
@@ -197,3 +209,16 @@ void usb_detect_quirks(struct usb_device *udev)
 		udev->persist_enabled = 1;
 #endif	/* CONFIG_PM */
 }
+
+void usb_detect_interface_quirks(struct usb_device *udev)
+{
+	u32 quirks;
+
+	quirks = __usb_detect_quirks(udev, usb_interface_quirk_list);
+	if (quirks == 0)
+		return;
+
+	dev_dbg(&udev->dev, "USB interface quirks for this device: %x\n",
+		quirks);
+	udev->quirks |= quirks;
+}
diff --git a/drivers/usb/core/sysfs.c b/drivers/usb/core/sysfs.c
index 9a56e3adf47..682e8256b95 100644
--- a/drivers/usb/core/sysfs.c
+++ b/drivers/usb/core/sysfs.c
@@ -253,6 +253,15 @@ show_removable(struct device *dev, struct device_attribute *attr, char *buf)
 }
 static DEVICE_ATTR(removable, S_IRUGO, show_removable, NULL);
 
+static ssize_t
+show_ltm_capable(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	if (usb_device_supports_ltm(to_usb_device(dev)))
+		return sprintf(buf, "%s\n", "yes");
+	return sprintf(buf, "%s\n", "no");
+}
+static DEVICE_ATTR(ltm_capable, S_IRUGO, show_ltm_capable, NULL);
+
 #ifdef	CONFIG_PM
 
 static ssize_t
@@ -649,6 +658,7 @@ static struct attribute *dev_attrs[] = {
 	&dev_attr_authorized.attr,
 	&dev_attr_remove.attr,
 	&dev_attr_removable.attr,
+	&dev_attr_ltm_capable.attr,
 	NULL,
 };
 static struct attribute_group dev_attr_grp = {
@@ -840,7 +850,7 @@ static ssize_t show_modalias(struct device *dev,
 	alt = intf->cur_altsetting;
 
 	return sprintf(buf, "usb:v%04Xp%04Xd%04Xdc%02Xdsc%02Xdp%02X"
-			"ic%02Xisc%02Xip%02X\n",
+			"ic%02Xisc%02Xip%02Xin%02X\n",
 			le16_to_cpu(udev->descriptor.idVendor),
 			le16_to_cpu(udev->descriptor.idProduct),
 			le16_to_cpu(udev->descriptor.bcdDevice),
@@ -849,7 +859,8 @@ static ssize_t show_modalias(struct device *dev,
 			udev->descriptor.bDeviceProtocol,
 			alt->desc.bInterfaceClass,
 			alt->desc.bInterfaceSubClass,
-			alt->desc.bInterfaceProtocol);
+			alt->desc.bInterfaceProtocol,
+			alt->desc.bInterfaceNumber);
 }
 static DEVICE_ATTR(modalias, S_IRUGO, show_modalias, NULL);
 
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index 25d0c61c3f8..cd8fb44a3e1 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -396,6 +396,7 @@ struct usb_device *usb_alloc_dev(struct usb_device *parent,
 	dev->dev.dma_mask = bus->controller->dma_mask;
 	set_dev_node(&dev->dev, dev_to_node(bus->controller));
 	dev->state = USB_STATE_ATTACHED;
+	dev->lpm_disable_count = 1;
 	atomic_set(&dev->urbnum, 0);
 
 	INIT_LIST_HEAD(&dev->ep0.urb_list);
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index 5c5c538ea73..acb103c5c39 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -1,5 +1,7 @@
 #include <linux/pm.h>
 
+struct dev_state;
+
 /* Functions local to drivers/usb/core/ */
 
 extern int usb_create_sysfs_dev_files(struct usb_device *dev);
@@ -24,6 +26,7 @@ extern void usb_disable_device(struct usb_device *dev, int skip_ep0);
 extern int usb_deauthorize_device(struct usb_device *);
 extern int usb_authorize_device(struct usb_device *);
 extern void usb_detect_quirks(struct usb_device *udev);
+extern void usb_detect_interface_quirks(struct usb_device *udev);
 extern int usb_remove_device(struct usb_device *udev);
 
 extern int usb_get_device_descriptor(struct usb_device *dev,
@@ -35,16 +38,20 @@ extern int usb_set_configuration(struct usb_device *dev, int configuration);
 extern int usb_choose_configuration(struct usb_device *udev);
 
 extern void usb_kick_khubd(struct usb_device *dev);
+extern int usb_match_one_id_intf(struct usb_device *dev,
+				 struct usb_host_interface *intf,
+				 const struct usb_device_id *id);
 extern int usb_match_device(struct usb_device *dev,
 			    const struct usb_device_id *id);
 extern void usb_forced_unbind_intf(struct usb_interface *intf);
 extern void usb_rebind_intf(struct usb_interface *intf);
 
 extern int usb_hub_claim_port(struct usb_device *hdev, unsigned port,
-		void *owner);
+		struct dev_state *owner);
 extern int usb_hub_release_port(struct usb_device *hdev, unsigned port,
-		void *owner);
-extern void usb_hub_release_all_ports(struct usb_device *hdev, void *owner);
+		struct dev_state *owner);
+extern void usb_hub_release_all_ports(struct usb_device *hdev,
+		struct dev_state *owner);
 extern bool usb_device_is_owned(struct usb_device *udev);
 
 extern int  usb_hub_init(void);
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 1040bdb8dc8..c34452a7304 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -148,6 +148,8 @@ static void dwc3_core_soft_reset(struct dwc3 *dwc)
 	reg &= ~DWC3_GUSB2PHYCFG_PHYSOFTRST;
 	dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
 
+	mdelay(100);
+
 	/* After PHYs are stable we can take Core out of reset state */
 	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
 	reg &= ~DWC3_GCTL_CORESOFTRESET;
@@ -255,7 +257,7 @@ static int __devinit dwc3_alloc_event_buffers(struct dwc3 *dwc, unsigned length)
  *
  * Returns 0 on success otherwise negative errno.
  */
-static int __devinit dwc3_event_buffers_setup(struct dwc3 *dwc)
+static int dwc3_event_buffers_setup(struct dwc3 *dwc)
 {
 	struct dwc3_event_buffer	*evt;
 	int				n;
@@ -266,6 +268,8 @@ static int __devinit dwc3_event_buffers_setup(struct dwc3 *dwc)
 				evt->buf, (unsigned long long) evt->dma,
 				evt->length);
 
+		evt->lpos = 0;
+
 		dwc3_writel(dwc->regs, DWC3_GEVNTADRLO(n),
 				lower_32_bits(evt->dma));
 		dwc3_writel(dwc->regs, DWC3_GEVNTADRHI(n),
@@ -285,6 +289,9 @@ static void dwc3_event_buffers_cleanup(struct dwc3 *dwc)
 
 	for (n = 0; n < dwc->num_event_buffers; n++) {
 		evt = dwc->ev_buffs[n];
+
+		evt->lpos = 0;
+
 		dwc3_writel(dwc->regs, DWC3_GEVNTADRLO(n), 0);
 		dwc3_writel(dwc->regs, DWC3_GEVNTADRHI(n), 0);
 		dwc3_writel(dwc->regs, DWC3_GEVNTSIZ(n), 0);
@@ -328,8 +335,6 @@ static int __devinit dwc3_core_init(struct dwc3 *dwc)
 	}
 	dwc->revision = reg;
 
-	dwc3_core_soft_reset(dwc);
-
 	/* issue device SoftReset too */
 	timeout = jiffies + msecs_to_jiffies(500);
 	dwc3_writel(dwc->regs, DWC3_DCTL, DWC3_DCTL_CSFTRST);
@@ -347,6 +352,8 @@ static int __devinit dwc3_core_init(struct dwc3 *dwc)
 		cpu_relax();
 	} while (true);
 
+	dwc3_core_soft_reset(dwc);
+
 	dwc3_cache_hwparams(dwc);
 
 	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index f69c877add0..151eca876df 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -67,6 +67,7 @@
 #define DWC3_DEVICE_EVENT_CONNECT_DONE		2
 #define DWC3_DEVICE_EVENT_LINK_STATUS_CHANGE	3
 #define DWC3_DEVICE_EVENT_WAKEUP		4
+#define DWC3_DEVICE_EVENT_HIBER_REQ		5
 #define DWC3_DEVICE_EVENT_EOPF			6
 #define DWC3_DEVICE_EVENT_SOF			7
 #define DWC3_DEVICE_EVENT_ERRATIC_ERROR		9
@@ -171,28 +172,36 @@
 #define DWC3_GCTL_PRTCAP_DEVICE	2
 #define DWC3_GCTL_PRTCAP_OTG	3
 
-#define DWC3_GCTL_CORESOFTRESET	(1 << 11)
-#define DWC3_GCTL_SCALEDOWN(n)	((n) << 4)
-#define DWC3_GCTL_SCALEDOWN_MASK DWC3_GCTL_SCALEDOWN(3)
-#define DWC3_GCTL_DISSCRAMBLE	(1 << 3)
-#define DWC3_GCTL_DSBLCLKGTNG	(1 << 0)
+#define DWC3_GCTL_CORESOFTRESET		(1 << 11)
+#define DWC3_GCTL_SCALEDOWN(n)		((n) << 4)
+#define DWC3_GCTL_SCALEDOWN_MASK	DWC3_GCTL_SCALEDOWN(3)
+#define DWC3_GCTL_DISSCRAMBLE		(1 << 3)
+#define DWC3_GCTL_GBLHIBERNATIONEN	(1 << 1)
+#define DWC3_GCTL_DSBLCLKGTNG		(1 << 0)
 
 /* Global USB2 PHY Configuration Register */
-#define DWC3_GUSB2PHYCFG_PHYSOFTRST (1 << 31)
-#define DWC3_GUSB2PHYCFG_SUSPHY	(1 << 6)
+#define DWC3_GUSB2PHYCFG_PHYSOFTRST	(1 << 31)
+#define DWC3_GUSB2PHYCFG_SUSPHY		(1 << 6)
 
 /* Global USB3 PIPE Control Register */
-#define DWC3_GUSB3PIPECTL_PHYSOFTRST (1 << 31)
-#define DWC3_GUSB3PIPECTL_SUSPHY (1 << 17)
+#define DWC3_GUSB3PIPECTL_PHYSOFTRST	(1 << 31)
+#define DWC3_GUSB3PIPECTL_SUSPHY	(1 << 17)
 
 /* Global TX Fifo Size Register */
-#define DWC3_GTXFIFOSIZ_TXFDEF(n) ((n) & 0xffff)
-#define DWC3_GTXFIFOSIZ_TXFSTADDR(n) ((n) & 0xffff0000)
+#define DWC3_GTXFIFOSIZ_TXFDEF(n)	((n) & 0xffff)
+#define DWC3_GTXFIFOSIZ_TXFSTADDR(n)	((n) & 0xffff0000)
 
 /* Global HWPARAMS1 Register */
 #define DWC3_GHWPARAMS1_EN_PWROPT(n)	(((n) & (3 << 24)) >> 24)
 #define DWC3_GHWPARAMS1_EN_PWROPT_NO	0
 #define DWC3_GHWPARAMS1_EN_PWROPT_CLK	1
+#define DWC3_GHWPARAMS1_EN_PWROPT_HIB	2
+#define DWC3_GHWPARAMS1_PWROPT(n)	((n) << 24)
+#define DWC3_GHWPARAMS1_PWROPT_MASK	DWC3_GHWPARAMS1_PWROPT(3)
+
+/* Global HWPARAMS4 Register */
+#define DWC3_GHWPARAMS4_HIBER_SCRATCHBUFS(n)	(((n) & (0x0f << 13)) >> 13)
+#define DWC3_MAX_HIBER_SCRATCHBUFS		15
 
 /* Device Configuration Register */
 #define DWC3_DCFG_LPM_CAP	(1 << 22)
@@ -206,24 +215,32 @@
 #define DWC3_DCFG_LOWSPEED	(2 << 0)
 #define DWC3_DCFG_FULLSPEED1	(3 << 0)
 
+#define DWC3_DCFG_LPM_CAP	(1 << 22)
+
 /* Device Control Register */
 #define DWC3_DCTL_RUN_STOP	(1 << 31)
 #define DWC3_DCTL_CSFTRST	(1 << 30)
 #define DWC3_DCTL_LSFTRST	(1 << 29)
 
 #define DWC3_DCTL_HIRD_THRES_MASK	(0x1f << 24)
-#define DWC3_DCTL_HIRD_THRES(n)	(((n) & DWC3_DCTL_HIRD_THRES_MASK) >> 24)
+#define DWC3_DCTL_HIRD_THRES(n)	((n) << 24)
 
 #define DWC3_DCTL_APPL1RES	(1 << 23)
 
-#define DWC3_DCTL_TRGTULST_MASK	(0x0f << 17)
-#define DWC3_DCTL_TRGTULST(n)	((n) << 17)
-
-#define DWC3_DCTL_TRGTULST_U2	(DWC3_DCTL_TRGTULST(2))
-#define DWC3_DCTL_TRGTULST_U3	(DWC3_DCTL_TRGTULST(3))
-#define DWC3_DCTL_TRGTULST_SS_DIS (DWC3_DCTL_TRGTULST(4))
-#define DWC3_DCTL_TRGTULST_RX_DET (DWC3_DCTL_TRGTULST(5))
-#define DWC3_DCTL_TRGTULST_SS_INACT (DWC3_DCTL_TRGTULST(6))
+/* These apply for core versions 1.87a and earlier */
+#define DWC3_DCTL_TRGTULST_MASK		(0x0f << 17)
+#define DWC3_DCTL_TRGTULST(n)		((n) << 17)
+#define DWC3_DCTL_TRGTULST_U2		(DWC3_DCTL_TRGTULST(2))
+#define DWC3_DCTL_TRGTULST_U3		(DWC3_DCTL_TRGTULST(3))
+#define DWC3_DCTL_TRGTULST_SS_DIS	(DWC3_DCTL_TRGTULST(4))
+#define DWC3_DCTL_TRGTULST_RX_DET	(DWC3_DCTL_TRGTULST(5))
+#define DWC3_DCTL_TRGTULST_SS_INACT	(DWC3_DCTL_TRGTULST(6))
+
+/* These apply for core versions 1.94a and later */
+#define DWC3_DCTL_KEEP_CONNECT	(1 << 19)
+#define DWC3_DCTL_L1_HIBER_EN	(1 << 18)
+#define DWC3_DCTL_CRS		(1 << 17)
+#define DWC3_DCTL_CSS		(1 << 16)
 
 #define DWC3_DCTL_INITU2ENA	(1 << 12)
 #define DWC3_DCTL_ACCEPTU2ENA	(1 << 11)
@@ -249,6 +266,7 @@
 #define DWC3_DEVTEN_ERRTICERREN		(1 << 9)
 #define DWC3_DEVTEN_SOFEN		(1 << 7)
 #define DWC3_DEVTEN_EOPFEN		(1 << 6)
+#define DWC3_DEVTEN_HIBERNATIONREQEVTEN	(1 << 5)
 #define DWC3_DEVTEN_WKUPEVTEN		(1 << 4)
 #define DWC3_DEVTEN_ULSTCNGEN		(1 << 3)
 #define DWC3_DEVTEN_CONNECTDONEEN	(1 << 2)
@@ -256,7 +274,15 @@
 #define DWC3_DEVTEN_DISCONNEVTEN	(1 << 0)
 
 /* Device Status Register */
+#define DWC3_DSTS_DCNRD			(1 << 29)
+
+/* This applies for core versions 1.87a and earlier */
 #define DWC3_DSTS_PWRUPREQ		(1 << 24)
+
+/* These apply for core versions 1.94a and later */
+#define DWC3_DSTS_RSS			(1 << 25)
+#define DWC3_DSTS_SSS			(1 << 24)
+
 #define DWC3_DSTS_COREIDLE		(1 << 23)
 #define DWC3_DSTS_DEVCTRLHLT		(1 << 22)
 
@@ -265,7 +291,7 @@
 
 #define DWC3_DSTS_RXFIFOEMPTY		(1 << 17)
 
-#define DWC3_DSTS_SOFFN_MASK		(0x3ff << 3)
+#define DWC3_DSTS_SOFFN_MASK		(0x3fff << 3)
 #define DWC3_DSTS_SOFFN(n)		(((n) & DWC3_DSTS_SOFFN_MASK) >> 3)
 
 #define DWC3_DSTS_CONNECTSPD		(7 << 0)
@@ -280,6 +306,11 @@
 #define DWC3_DGCMD_SET_LMP		0x01
 #define DWC3_DGCMD_SET_PERIODIC_PAR	0x02
 #define DWC3_DGCMD_XMIT_FUNCTION	0x03
+
+/* These apply for core versions 1.94a and later */
+#define DWC3_DGCMD_SET_SCRATCHPAD_ADDR_LO	0x04
+#define DWC3_DGCMD_SET_SCRATCHPAD_ADDR_HI	0x05
+
 #define DWC3_DGCMD_SELECTED_FIFO_FLUSH	0x09
 #define DWC3_DGCMD_ALL_FIFO_FLUSH	0x0a
 #define DWC3_DGCMD_SET_ENDPOINT_NRDY	0x0c
@@ -287,6 +318,15 @@
 
 #define DWC3_DGCMD_STATUS(n)		(((n) >> 15) & 1)
 #define DWC3_DGCMD_CMDACT		(1 << 10)
+#define DWC3_DGCMD_CMDIOC		(1 << 8)
+
+/* Device Generic Command Parameter Register */
+#define DWC3_DGCMDPAR_FORCE_LINKPM_ACCEPT	(1 << 0)
+#define DWC3_DGCMDPAR_FIFO_NUM(n)		((n) << 0)
+#define DWC3_DGCMDPAR_RX_FIFO			(0 << 5)
+#define DWC3_DGCMDPAR_TX_FIFO			(1 << 5)
+#define DWC3_DGCMDPAR_LOOPBACK_DIS		(0 << 0)
+#define DWC3_DGCMDPAR_LOOPBACK_ENA		(1 << 0)
 
 /* Device Endpoint Command Register */
 #define DWC3_DEPCMD_PARAM_SHIFT		16
@@ -303,7 +343,10 @@
 #define DWC3_DEPCMD_STARTTRANSFER	(0x06 << 0)
 #define DWC3_DEPCMD_CLEARSTALL		(0x05 << 0)
 #define DWC3_DEPCMD_SETSTALL		(0x04 << 0)
+/* This applies for core versions 1.90a and earlier */
 #define DWC3_DEPCMD_GETSEQNUMBER	(0x03 << 0)
+/* This applies for core versions 1.94a and later */
+#define DWC3_DEPCMD_GETEPSTATE		(0x03 << 0)
 #define DWC3_DEPCMD_SETTRANSFRESOURCE	(0x02 << 0)
 #define DWC3_DEPCMD_SETEPCONFIG		(0x01 << 0)
 
@@ -361,7 +404,8 @@ struct dwc3_event_buffer {
  * @current_trb: index of current used trb
  * @number: endpoint number (1 - 15)
  * @type: set to bmAttributes & USB_ENDPOINT_XFERTYPE_MASK
- * @res_trans_idx: Resource transfer index
+ * @resource_index: Resource transfer index
+ * @current_uf: Current uf received through last event parameter
  * @interval: the intervall on which the ISOC transfer is started
  * @name: a human readable name e.g. ep1out-bulk
  * @direction: true for TX, false for RX
@@ -385,6 +429,7 @@ struct dwc3_ep {
 #define DWC3_EP_WEDGE		(1 << 2)
 #define DWC3_EP_BUSY		(1 << 4)
 #define DWC3_EP_PENDING_REQUEST	(1 << 5)
+#define DWC3_EP_MISSED_ISOC	(1 << 6)
 
 	/* This last one is specific to EP0 */
 #define DWC3_EP0_DIR_IN		(1 << 31)
@@ -393,7 +438,8 @@ struct dwc3_ep {
 
 	u8			number;
 	u8			type;
-	u8			res_trans_idx;
+	u8			resource_index;
+	u16			current_uf;
 	u32			interval;
 
 	char			name[20];
@@ -437,6 +483,8 @@ enum dwc3_link_state {
 	DWC3_LINK_STATE_HRESET		= 0x09,
 	DWC3_LINK_STATE_CMPLY		= 0x0a,
 	DWC3_LINK_STATE_LPBK		= 0x0b,
+	DWC3_LINK_STATE_RESET		= 0x0e,
+	DWC3_LINK_STATE_RESUME		= 0x0f,
 	DWC3_LINK_STATE_MASK		= 0x0f,
 };
 
@@ -450,11 +498,12 @@ enum dwc3_device_state {
 #define DWC3_TRB_SIZE_MASK	(0x00ffffff)
 #define DWC3_TRB_SIZE_LENGTH(n)	((n) & DWC3_TRB_SIZE_MASK)
 #define DWC3_TRB_SIZE_PCM1(n)	(((n) & 0x03) << 24)
-#define DWC3_TRB_SIZE_TRBSTS(n)	(((n) & (0x0f << 28) >> 28))
+#define DWC3_TRB_SIZE_TRBSTS(n)	(((n) & (0x0f << 28)) >> 28)
 
 #define DWC3_TRBSTS_OK			0
 #define DWC3_TRBSTS_MISSED_ISOC		1
 #define DWC3_TRBSTS_SETUP_PENDING	2
+#define DWC3_TRB_STS_XFER_IN_PROG	4
 
 /* TRB Control */
 #define DWC3_TRB_CTRL_HWO		(1 << 0)
@@ -543,6 +592,14 @@ struct dwc3_request {
 	unsigned		queued:1;
 };
 
+/*
+ * struct dwc3_scratchpad_array - hibernation scratchpad array
+ * (format defined by hw)
+ */
+struct dwc3_scratchpad_array {
+	__le64	dma_adr[DWC3_MAX_HIBER_SCRATCHBUFS];
+};
+
 /**
  * struct dwc3 - representation of our controller
  * @ctrl_req: usb control request which is used for ep0
@@ -624,8 +681,10 @@ struct dwc3 {
 #define DWC3_REVISION_180A	0x5533180a
 #define DWC3_REVISION_183A	0x5533183a
 #define DWC3_REVISION_185A	0x5533185a
+#define DWC3_REVISION_187A	0x5533187a
 #define DWC3_REVISION_188A	0x5533188a
 #define DWC3_REVISION_190A	0x5533190a
+#define DWC3_REVISION_194A	0x5533194a
 #define DWC3_REVISION_200A	0x5533200a
 #define DWC3_REVISION_202A	0x5533202a
 #define DWC3_REVISION_210A	0x5533210a
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index d1903019808..b8f00389fa3 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -18,7 +18,6 @@
 #include <linux/platform_device.h>
 #include <linux/platform_data/dwc3-exynos.h>
 #include <linux/dma-mapping.h>
-#include <linux/module.h>
 #include <linux/clk.h>
 
 #include "core.h"
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 9e8a3dce69f..9b94886b66e 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -54,7 +54,9 @@
 #include "gadget.h"
 #include "io.h"
 
-static void dwc3_ep0_do_control_status(struct dwc3 *dwc, u32 epnum);
+static void __dwc3_ep0_do_control_status(struct dwc3 *dwc, struct dwc3_ep *dep);
+static void __dwc3_ep0_do_control_data(struct dwc3 *dwc,
+		struct dwc3_ep *dep, struct dwc3_request *req);
 
 static const char *dwc3_ep0_state_string(enum dwc3_ep0_state state)
 {
@@ -111,7 +113,7 @@ static int dwc3_ep0_start_trans(struct dwc3 *dwc, u8 epnum, dma_addr_t buf_dma,
 	}
 
 	dep->flags |= DWC3_EP_BUSY;
-	dep->res_trans_idx = dwc3_gadget_ep_get_transfer_index(dwc,
+	dep->resource_index = dwc3_gadget_ep_get_transfer_index(dwc,
 			dep->number);
 
 	dwc->ep0_next_event = DWC3_EP0_COMPLETE;
@@ -150,16 +152,15 @@ static int __dwc3_gadget_ep0_queue(struct dwc3_ep *dep,
 			return 0;
 		}
 
-		ret = dwc3_ep0_start_trans(dwc, direction,
-				req->request.dma, req->request.length,
-				DWC3_TRBCTL_CONTROL_DATA);
+		__dwc3_ep0_do_control_data(dwc, dwc->eps[direction], req);
+
 		dep->flags &= ~(DWC3_EP_PENDING_REQUEST |
 				DWC3_EP0_DIR_IN);
 	} else if (dwc->delayed_status) {
 		dwc->delayed_status = false;
 
 		if (dwc->ep0state == EP0_STATUS_PHASE)
-			dwc3_ep0_do_control_status(dwc, 1);
+			__dwc3_ep0_do_control_status(dwc, dwc->eps[1]);
 		else
 			dev_dbg(dwc->dev, "too early for delayed status\n");
 	}
@@ -224,6 +225,16 @@ static void dwc3_ep0_stall_and_restart(struct dwc3 *dwc)
 	dwc3_ep0_out_start(dwc);
 }
 
+int dwc3_gadget_ep0_set_halt(struct usb_ep *ep, int value)
+{
+	struct dwc3_ep			*dep = to_dwc3_ep(ep);
+	struct dwc3			*dwc = dep->dwc;
+
+	dwc3_ep0_stall_and_restart(dwc);
+
+	return 0;
+}
+
 void dwc3_ep0_out_start(struct dwc3 *dwc)
 {
 	int				ret;
@@ -463,6 +474,7 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 {
 	u32 cfg;
 	int ret;
+	u32 reg;
 
 	dwc->start_config_issued = false;
 	cfg = le16_to_cpu(ctrl->wValue);
@@ -477,6 +489,14 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 		/* if the cfg matches and the cfg is non zero */
 		if (cfg && (!ret || (ret == USB_GADGET_DELAYED_STATUS))) {
 			dwc->dev_state = DWC3_CONFIGURED_STATE;
+			/*
+			 * Enable transition to U1/U2 state when
+			 * nothing is pending from application.
+			 */
+			reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+			reg |= (DWC3_DCTL_ACCEPTU1ENA | DWC3_DCTL_ACCEPTU2ENA);
+			dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+
 			dwc->resize_fifos = true;
 			dev_dbg(dwc->dev, "resize fifos flag SET\n");
 		}
@@ -514,8 +534,8 @@ static void dwc3_ep0_set_sel_cmpl(struct usb_ep *ep, struct usb_request *req)
 
 	dwc->u1sel = timing.u1sel;
 	dwc->u1pel = timing.u1pel;
-	dwc->u2sel = timing.u2sel;
-	dwc->u2pel = timing.u2pel;
+	dwc->u2sel = le16_to_cpu(timing.u2sel);
+	dwc->u2pel = le16_to_cpu(timing.u2pel);
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	if (reg & DWC3_DCTL_INITU2ENA)
@@ -640,11 +660,11 @@ static void dwc3_ep0_inspect_setup(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
 	struct usb_ctrlrequest *ctrl = dwc->ctrl_req;
-	int ret;
+	int ret = -EINVAL;
 	u32 len;
 
 	if (!dwc->gadget_driver)
-		goto err;
+		goto out;
 
 	len = le16_to_cpu(ctrl->wLength);
 	if (!len) {
@@ -665,11 +685,9 @@ static void dwc3_ep0_inspect_setup(struct dwc3 *dwc,
 	if (ret == USB_GADGET_DELAYED_STATUS)
 		dwc->delayed_status = true;
 
-	if (ret >= 0)
-		return;
-
-err:
-	dwc3_ep0_stall_and_restart(dwc);
+out:
+	if (ret < 0)
+		dwc3_ep0_stall_and_restart(dwc);
 }
 
 static void dwc3_ep0_complete_data(struct dwc3 *dwc,
@@ -723,7 +741,7 @@ static void dwc3_ep0_complete_data(struct dwc3 *dwc,
 	}
 }
 
-static void dwc3_ep0_complete_req(struct dwc3 *dwc,
+static void dwc3_ep0_complete_status(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
 	struct dwc3_request	*r;
@@ -745,6 +763,7 @@ static void dwc3_ep0_complete_req(struct dwc3 *dwc,
 			dev_dbg(dwc->dev, "Invalid Test #%d\n",
 					dwc->test_mode_nr);
 			dwc3_ep0_stall_and_restart(dwc);
+			return;
 		}
 	}
 
@@ -758,7 +777,7 @@ static void dwc3_ep0_xfer_complete(struct dwc3 *dwc,
 	struct dwc3_ep		*dep = dwc->eps[event->endpoint_number];
 
 	dep->flags &= ~DWC3_EP_BUSY;
-	dep->res_trans_idx = 0;
+	dep->resource_index = 0;
 	dwc->setup_packet_pending = false;
 
 	switch (dwc->ep0state) {
@@ -774,7 +793,7 @@ static void dwc3_ep0_xfer_complete(struct dwc3 *dwc,
 
 	case EP0_STATUS_PHASE:
 		dev_vdbg(dwc->dev, "Status Phase\n");
-		dwc3_ep0_complete_req(dwc, event);
+		dwc3_ep0_complete_status(dwc, event);
 		break;
 	default:
 		WARN(true, "UNKNOWN ep0state %d\n", dwc->ep0state);
@@ -787,68 +806,81 @@ static void dwc3_ep0_do_control_setup(struct dwc3 *dwc,
 	dwc3_ep0_out_start(dwc);
 }
 
-static void dwc3_ep0_do_control_data(struct dwc3 *dwc,
-		const struct dwc3_event_depevt *event)
+static void __dwc3_ep0_do_control_data(struct dwc3 *dwc,
+		struct dwc3_ep *dep, struct dwc3_request *req)
 {
-	struct dwc3_ep		*dep;
-	struct dwc3_request	*req;
 	int			ret;
 
-	dep = dwc->eps[0];
-
-	if (list_empty(&dep->request_list)) {
-		dev_vdbg(dwc->dev, "pending request for EP0 Data phase\n");
-		dep->flags |= DWC3_EP_PENDING_REQUEST;
-
-		if (event->endpoint_number)
-			dep->flags |= DWC3_EP0_DIR_IN;
-		return;
-	}
-
-	req = next_request(&dep->request_list);
-	req->direction = !!event->endpoint_number;
+	req->direction = !!dep->number;
 
 	if (req->request.length == 0) {
-		ret = dwc3_ep0_start_trans(dwc, event->endpoint_number,
+		ret = dwc3_ep0_start_trans(dwc, dep->number,
 				dwc->ctrl_req_addr, 0,
 				DWC3_TRBCTL_CONTROL_DATA);
-	} else if ((req->request.length % dep->endpoint.maxpacket)
-			&& (event->endpoint_number == 0)) {
+	} else if (!IS_ALIGNED(req->request.length, dep->endpoint.maxpacket)
+			&& (dep->number == 0)) {
+		u32		transfer_size;
+
 		ret = usb_gadget_map_request(&dwc->gadget, &req->request,
-				event->endpoint_number);
+				dep->number);
 		if (ret) {
 			dev_dbg(dwc->dev, "failed to map request\n");
 			return;
 		}
 
-		WARN_ON(req->request.length > dep->endpoint.maxpacket);
+		WARN_ON(req->request.length > DWC3_EP0_BOUNCE_SIZE);
+
+		transfer_size = roundup(req->request.length,
+				(u32) dep->endpoint.maxpacket);
 
 		dwc->ep0_bounced = true;
 
 		/*
-		 * REVISIT in case request length is bigger than EP0
-		 * wMaxPacketSize, we will need two chained TRBs to handle
-		 * the transfer.
+		 * REVISIT in case request length is bigger than
+		 * DWC3_EP0_BOUNCE_SIZE we will need two chained
+		 * TRBs to handle the transfer.
 		 */
-		ret = dwc3_ep0_start_trans(dwc, event->endpoint_number,
-				dwc->ep0_bounce_addr, dep->endpoint.maxpacket,
+		ret = dwc3_ep0_start_trans(dwc, dep->number,
+				dwc->ep0_bounce_addr, transfer_size,
 				DWC3_TRBCTL_CONTROL_DATA);
 	} else {
 		ret = usb_gadget_map_request(&dwc->gadget, &req->request,
-				event->endpoint_number);
+				dep->number);
 		if (ret) {
 			dev_dbg(dwc->dev, "failed to map request\n");
 			return;
 		}
 
-		ret = dwc3_ep0_start_trans(dwc, event->endpoint_number,
-				req->request.dma, req->request.length,
-				DWC3_TRBCTL_CONTROL_DATA);
+		ret = dwc3_ep0_start_trans(dwc, dep->number, req->request.dma,
+				req->request.length, DWC3_TRBCTL_CONTROL_DATA);
 	}
 
 	WARN_ON(ret < 0);
 }
 
+static void dwc3_ep0_do_control_data(struct dwc3 *dwc,
+		const struct dwc3_event_depevt *event)
+{
+	struct dwc3_ep		*dep;
+	struct dwc3_request	*req;
+
+	dep = dwc->eps[0];
+
+	if (list_empty(&dep->request_list)) {
+		dev_vdbg(dwc->dev, "pending request for EP0 Data phase\n");
+		dep->flags |= DWC3_EP_PENDING_REQUEST;
+
+		if (event->endpoint_number)
+			dep->flags |= DWC3_EP0_DIR_IN;
+		return;
+	}
+
+	req = next_request(&dep->request_list);
+	dep = dwc->eps[event->endpoint_number];
+
+	__dwc3_ep0_do_control_data(dwc, dep, req);
+}
+
 static int dwc3_ep0_start_control_status(struct dwc3_ep *dep)
 {
 	struct dwc3		*dwc = dep->dwc;
@@ -861,10 +893,8 @@ static int dwc3_ep0_start_control_status(struct dwc3_ep *dep)
 			dwc->ctrl_req_addr, 0, type);
 }
 
-static void dwc3_ep0_do_control_status(struct dwc3 *dwc, u32 epnum)
+static void __dwc3_ep0_do_control_status(struct dwc3 *dwc, struct dwc3_ep *dep)
 {
-	struct dwc3_ep		*dep = dwc->eps[epnum];
-
 	if (dwc->resize_fifos) {
 		dev_dbg(dwc->dev, "starting to resize fifos\n");
 		dwc3_gadget_resize_tx_fifos(dwc);
@@ -874,13 +904,21 @@ static void dwc3_ep0_do_control_status(struct dwc3 *dwc, u32 epnum)
 	WARN_ON(dwc3_ep0_start_control_status(dep));
 }
 
+static void dwc3_ep0_do_control_status(struct dwc3 *dwc,
+		const struct dwc3_event_depevt *event)
+{
+	struct dwc3_ep		*dep = dwc->eps[event->endpoint_number];
+
+	__dwc3_ep0_do_control_status(dwc, dep);
+}
+
 static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
 	dwc->setup_packet_pending = true;
 
 	/*
-	 * This part is very tricky: If we has just handled
+	 * This part is very tricky: If we have just handled
 	 * XferNotReady(Setup) and we're now expecting a
 	 * XferComplete but, instead, we receive another
 	 * XferNotReady(Setup), we should STALL and restart
@@ -974,7 +1012,7 @@ static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 			return;
 		}
 
-		dwc3_ep0_do_control_status(dwc, event->endpoint_number);
+		dwc3_ep0_do_control_status(dwc, event);
 	}
 }
 
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index ec70df7aba1..58fdfad96b4 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -100,6 +100,23 @@ int dwc3_gadget_set_link_state(struct dwc3 *dwc, enum dwc3_link_state state)
 	int		retries = 10000;
 	u32		reg;
 
+	/*
+	 * Wait until device controller is ready. Only applies to 1.94a and
+	 * later RTL.
+	 */
+	if (dwc->revision >= DWC3_REVISION_194A) {
+		while (--retries) {
+			reg = dwc3_readl(dwc->regs, DWC3_DSTS);
+			if (reg & DWC3_DSTS_DCNRD)
+				udelay(5);
+			else
+				break;
+		}
+
+		if (retries <= 0)
+			return -ETIMEDOUT;
+	}
+
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_ULSTCHNGREQ_MASK;
 
@@ -107,7 +124,15 @@ int dwc3_gadget_set_link_state(struct dwc3 *dwc, enum dwc3_link_state state)
 	reg |= DWC3_DCTL_ULSTCHNGREQ(state);
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 
+	/*
+	 * The following code is racy when called from dwc3_gadget_wakeup,
+	 * and is not needed, at least on newer versions
+	 */
+	if (dwc->revision >= DWC3_REVISION_194A)
+		return 0;
+
 	/* wait for a change in DSTS */
+	retries = 10000;
 	while (--retries) {
 		reg = dwc3_readl(dwc->regs, DWC3_DSTS);
 
@@ -265,8 +290,8 @@ static const char *dwc3_gadget_ep_cmd_string(u8 cmd)
 		return "Clear Stall";
 	case DWC3_DEPCMD_SETSTALL:
 		return "Set Stall";
-	case DWC3_DEPCMD_GETSEQNUMBER:
-		return "Get Data Sequence Number";
+	case DWC3_DEPCMD_GETEPSTATE:
+		return "Get Endpoint State";
 	case DWC3_DEPCMD_SETTRANSFRESOURCE:
 		return "Set Endpoint Transfer Resource";
 	case DWC3_DEPCMD_SETEPCONFIG:
@@ -414,7 +439,7 @@ static int dwc3_gadget_set_ep_config(struct dwc3 *dwc, struct dwc3_ep *dep,
 
 	params.param0 = DWC3_DEPCFG_EP_TYPE(usb_endpoint_type(desc))
 		| DWC3_DEPCFG_MAX_PACKET_SIZE(usb_endpoint_maxp(desc))
-		| DWC3_DEPCFG_BURST_SIZE(dep->endpoint.maxburst);
+		| DWC3_DEPCFG_BURST_SIZE(dep->endpoint.maxburst - 1);
 
 	params.param1 = DWC3_DEPCFG_XFER_COMPLETE_EN
 		| DWC3_DEPCFG_XFER_NOT_READY_EN;
@@ -530,9 +555,37 @@ static void dwc3_remove_requests(struct dwc3 *dwc, struct dwc3_ep *dep)
 {
 	struct dwc3_request		*req;
 
-	if (!list_empty(&dep->req_queued))
+	if (!list_empty(&dep->req_queued)) {
 		dwc3_stop_active_transfer(dwc, dep->number);
 
+		/*
+		 * NOTICE: We are violating what the Databook says about the
+		 * EndTransfer command. Ideally we would _always_ wait for the
+		 * EndTransfer Command Completion IRQ, but that's causing too
+		 * much trouble synchronizing between us and gadget driver.
+		 *
+		 * We have discussed this with the IP Provider and it was
+		 * suggested to giveback all requests here, but give HW some
+		 * extra time to synchronize with the interconnect. We're using
+		 * an arbitraty 100us delay for that.
+		 *
+		 * Note also that a similar handling was tested by Synopsys
+		 * (thanks a lot Paul) and nothing bad has come out of it.
+		 * In short, what we're doing is:
+		 *
+		 * - Issue EndTransfer WITH CMDIOC bit set
+		 * - Wait 100us
+		 * - giveback all requests to gadget driver
+		 */
+		udelay(100);
+
+		while (!list_empty(&dep->req_queued)) {
+			req = next_request(&dep->req_queued);
+
+			dwc3_gadget_giveback(dep, req, -ESHUTDOWN);
+		}
+	}
+
 	while (!list_empty(&dep->request_list)) {
 		req = next_request(&dep->request_list);
 
@@ -741,8 +794,7 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 	case USB_ENDPOINT_XFER_ISOC:
 		trb->ctrl = DWC3_TRBCTL_ISOCHRONOUS_FIRST;
 
-		/* IOC every DWC3_TRB_NUM / 4 so we can refill */
-		if (!(cur_slot % (DWC3_TRB_NUM / 4)))
+		if (!req->request.no_interrupt)
 			trb->ctrl |= DWC3_TRB_CTRL_IOC;
 		break;
 
@@ -958,14 +1010,42 @@ static int __dwc3_gadget_kick_transfer(struct dwc3_ep *dep, u16 cmd_param,
 	dep->flags |= DWC3_EP_BUSY;
 
 	if (start_new) {
-		dep->res_trans_idx = dwc3_gadget_ep_get_transfer_index(dwc,
+		dep->resource_index = dwc3_gadget_ep_get_transfer_index(dwc,
 				dep->number);
-		WARN_ON_ONCE(!dep->res_trans_idx);
+		WARN_ON_ONCE(!dep->resource_index);
 	}
 
 	return 0;
 }
 
+static void __dwc3_gadget_start_isoc(struct dwc3 *dwc,
+		struct dwc3_ep *dep, u32 cur_uf)
+{
+	u32 uf;
+
+	if (list_empty(&dep->request_list)) {
+		dev_vdbg(dwc->dev, "ISOC ep %s run out for requests.\n",
+			dep->name);
+		return;
+	}
+
+	/* 4 micro frames in the future */
+	uf = cur_uf + dep->interval * 4;
+
+	__dwc3_gadget_kick_transfer(dep, uf, 1);
+}
+
+static void dwc3_gadget_start_isoc(struct dwc3 *dwc,
+		struct dwc3_ep *dep, const struct dwc3_event_depevt *event)
+{
+	u32 cur_uf, mask;
+
+	mask = ~(dep->interval - 1);
+	cur_uf = event->parameters & mask;
+
+	__dwc3_gadget_start_isoc(dwc, dep, cur_uf);
+}
+
 static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 {
 	struct dwc3		*dwc = dep->dwc;
@@ -995,11 +1075,8 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 
 	list_add_tail(&req->list, &dep->request_list);
 
-	if (usb_endpoint_xfer_isoc(dep->endpoint.desc) && (dep->flags & DWC3_EP_BUSY))
-		dep->flags |= DWC3_EP_PENDING_REQUEST;
-
 	/*
-	 * There are two special cases:
+	 * There are a few special cases:
 	 *
 	 * 1. XferNotReady with empty list of requests. We need to kick the
 	 *    transfer here in that situation, otherwise we will be NAKing
@@ -1008,31 +1085,46 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 	 *    able to receive the data until the next request is queued.
 	 *    The following code is handling exactly that.
 	 *
-	 * 2. XferInProgress on Isoc EP with an active transfer. We need to
-	 *    kick the transfer here after queuing a request, otherwise the
-	 *    core may not see the modified TRB(s).
 	 */
 	if (dep->flags & DWC3_EP_PENDING_REQUEST) {
 		int	ret;
-		int	start_trans = 1;
-		u8	trans_idx = dep->res_trans_idx;
 
-		if (usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
-				(dep->flags & DWC3_EP_BUSY)) {
-			start_trans = 0;
-			WARN_ON_ONCE(!trans_idx);
-		} else {
-			trans_idx = 0;
+		ret = __dwc3_gadget_kick_transfer(dep, 0, true);
+		if (ret && ret != -EBUSY) {
+			struct dwc3	*dwc = dep->dwc;
+
+			dev_dbg(dwc->dev, "%s: failed to kick transfers\n",
+					dep->name);
 		}
+	}
 
-		ret = __dwc3_gadget_kick_transfer(dep, trans_idx, start_trans);
+	/*
+	 * 2. XferInProgress on Isoc EP with an active transfer. We need to
+	 *    kick the transfer here after queuing a request, otherwise the
+	 *    core may not see the modified TRB(s).
+	 */
+	if (usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
+			(dep->flags & DWC3_EP_BUSY)) {
+		WARN_ON_ONCE(!dep->resource_index);
+		ret = __dwc3_gadget_kick_transfer(dep, dep->resource_index,
+				false);
 		if (ret && ret != -EBUSY) {
 			struct dwc3	*dwc = dep->dwc;
 
 			dev_dbg(dwc->dev, "%s: failed to kick transfers\n",
 					dep->name);
 		}
-	};
+	}
+
+	/*
+	 * 3. Missed ISOC Handling. We need to start isoc transfer on the saved
+	 * uframe number.
+	 */
+	if (usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
+		(dep->flags & DWC3_EP_MISSED_ISOC)) {
+			__dwc3_gadget_start_isoc(dwc, dep, dep->current_uf);
+			dep->flags &= ~DWC3_EP_MISSED_ISOC;
+	}
 
 	return 0;
 }
@@ -1118,15 +1210,6 @@ int __dwc3_gadget_ep_set_halt(struct dwc3_ep *dep, int value)
 	memset(&params, 0x00, sizeof(params));
 
 	if (value) {
-		if (dep->number == 0 || dep->number == 1) {
-			/*
-			 * Whenever EP0 is stalled, we will restart
-			 * the state machine, thus moving back to
-			 * Setup Phase
-			 */
-			dwc->ep0state = EP0_SETUP_PHASE;
-		}
-
 		ret = dwc3_send_gadget_ep_cmd(dwc, dep->number,
 			DWC3_DEPCMD_SETSTALL, &params);
 		if (ret)
@@ -1186,7 +1269,10 @@ static int dwc3_gadget_ep_set_wedge(struct usb_ep *ep)
 	dep->flags |= DWC3_EP_WEDGE;
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
-	return dwc3_gadget_ep_set_halt(ep, 1);
+	if (dep->number == 0 || dep->number == 1)
+		return dwc3_gadget_ep0_set_halt(ep, 1);
+	else
+		return dwc3_gadget_ep_set_halt(ep, 1);
 }
 
 /* -------------------------------------------------------------------------- */
@@ -1204,7 +1290,7 @@ static const struct usb_ep_ops dwc3_gadget_ep0_ops = {
 	.free_request	= dwc3_gadget_ep_free_request,
 	.queue		= dwc3_gadget_ep0_queue,
 	.dequeue	= dwc3_gadget_ep_dequeue,
-	.set_halt	= dwc3_gadget_ep_set_halt,
+	.set_halt	= dwc3_gadget_ep0_set_halt,
 	.set_wedge	= dwc3_gadget_ep_set_wedge,
 };
 
@@ -1280,9 +1366,13 @@ static int dwc3_gadget_wakeup(struct usb_gadget *g)
 		goto out;
 	}
 
-	/* write zeroes to Link Change Request */
-	reg &= ~DWC3_DCTL_ULSTCHNGREQ_MASK;
-	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+	/* Recent versions do this automatically */
+	if (dwc->revision < DWC3_REVISION_194A) {
+		/* write zeroes to Link Change Request */
+		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+		reg &= ~DWC3_DCTL_ULSTCHNGREQ_MASK;
+		dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+	}
 
 	/* poll until Link State changes to ON */
 	timeout = jiffies + msecs_to_jiffies(100);
@@ -1319,16 +1409,21 @@ static int dwc3_gadget_set_selfpowered(struct usb_gadget *g,
 	return 0;
 }
 
-static void dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on)
+static int dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on)
 {
 	u32			reg;
 	u32			timeout = 500;
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	if (is_on) {
-		reg &= ~DWC3_DCTL_TRGTULST_MASK;
-		reg |= (DWC3_DCTL_RUN_STOP
-				| DWC3_DCTL_TRGTULST_RX_DET);
+		if (dwc->revision <= DWC3_REVISION_187A) {
+			reg &= ~DWC3_DCTL_TRGTULST_MASK;
+			reg |= DWC3_DCTL_TRGTULST_RX_DET;
+		}
+
+		if (dwc->revision >= DWC3_REVISION_194A)
+			reg &= ~DWC3_DCTL_KEEP_CONNECT;
+		reg |= DWC3_DCTL_RUN_STOP;
 	} else {
 		reg &= ~DWC3_DCTL_RUN_STOP;
 	}
@@ -1346,7 +1441,7 @@ static void dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on)
 		}
 		timeout--;
 		if (!timeout)
-			break;
+			return -ETIMEDOUT;
 		udelay(1);
 	} while (1);
 
@@ -1354,20 +1449,23 @@ static void dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on)
 			dwc->gadget_driver
 			? dwc->gadget_driver->function : "no-function",
 			is_on ? "connect" : "disconnect");
+
+	return 0;
 }
 
 static int dwc3_gadget_pullup(struct usb_gadget *g, int is_on)
 {
 	struct dwc3		*dwc = gadget_to_dwc(g);
 	unsigned long		flags;
+	int			ret;
 
 	is_on = !!is_on;
 
 	spin_lock_irqsave(&dwc->lock, flags);
-	dwc3_gadget_run_stop(dwc, is_on);
+	ret = dwc3_gadget_run_stop(dwc, is_on);
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
-	return 0;
+	return ret;
 }
 
 static int dwc3_gadget_start(struct usb_gadget *g,
@@ -1468,6 +1566,7 @@ static int dwc3_gadget_stop(struct usb_gadget *g,
 
 	return 0;
 }
+
 static const struct usb_gadget_ops dwc3_gadget_ops = {
 	.get_frame		= dwc3_gadget_get_frame,
 	.wakeup			= dwc3_gadget_wakeup,
@@ -1558,6 +1657,7 @@ static int dwc3_cleanup_done_reqs(struct dwc3 *dwc, struct dwc3_ep *dep,
 	struct dwc3_trb		*trb;
 	unsigned int		count;
 	unsigned int		s_pkt = 0;
+	unsigned int		trb_status;
 
 	do {
 		req = next_request(&dep->req_queued);
@@ -1583,9 +1683,18 @@ static int dwc3_cleanup_done_reqs(struct dwc3 *dwc, struct dwc3_ep *dep,
 
 		if (dep->direction) {
 			if (count) {
-				dev_err(dwc->dev, "incomplete IN transfer %s\n",
-						dep->name);
-				status = -ECONNRESET;
+				trb_status = DWC3_TRB_SIZE_TRBSTS(trb->size);
+				if (trb_status == DWC3_TRBSTS_MISSED_ISOC) {
+					dev_dbg(dwc->dev, "incomplete IN transfer %s\n",
+							dep->name);
+					dep->current_uf = event->parameters &
+						~(dep->interval - 1);
+					dep->flags |= DWC3_EP_MISSED_ISOC;
+				} else {
+					dev_err(dwc->dev, "incomplete IN transfer %s\n",
+							dep->name);
+					status = -ECONNRESET;
+				}
 			}
 		} else {
 			if (count && (event->status & DEPEVT_STATUS_SHORT))
@@ -1604,7 +1713,8 @@ static int dwc3_cleanup_done_reqs(struct dwc3 *dwc, struct dwc3_ep *dep,
 		if (s_pkt)
 			break;
 		if ((event->status & DEPEVT_STATUS_LST) &&
-				(trb->ctrl & DWC3_TRB_CTRL_LST))
+				(trb->ctrl & (DWC3_TRB_CTRL_LST |
+						DWC3_TRB_CTRL_HWO)))
 			break;
 		if ((event->status & DEPEVT_STATUS_IOC) &&
 				(trb->ctrl & DWC3_TRB_CTRL_IOC))
@@ -1657,65 +1767,6 @@ static void dwc3_endpoint_transfer_complete(struct dwc3 *dwc,
 	}
 }
 
-static void dwc3_gadget_start_isoc(struct dwc3 *dwc,
-		struct dwc3_ep *dep, const struct dwc3_event_depevt *event)
-{
-	u32 uf, mask;
-
-	if (list_empty(&dep->request_list)) {
-		dev_vdbg(dwc->dev, "ISOC ep %s run out for requests.\n",
-			dep->name);
-		return;
-	}
-
-	mask = ~(dep->interval - 1);
-	uf = event->parameters & mask;
-	/* 4 micro frames in the future */
-	uf += dep->interval * 4;
-
-	__dwc3_gadget_kick_transfer(dep, uf, 1);
-}
-
-static void dwc3_process_ep_cmd_complete(struct dwc3_ep *dep,
-		const struct dwc3_event_depevt *event)
-{
-	struct dwc3 *dwc = dep->dwc;
-	struct dwc3_event_depevt mod_ev = *event;
-
-	/*
-	 * We were asked to remove one request. It is possible that this
-	 * request and a few others were started together and have the same
-	 * transfer index. Since we stopped the complete endpoint we don't
-	 * know how many requests were already completed (and not yet)
-	 * reported and how could be done (later). We purge them all until
-	 * the end of the list.
-	 */
-	mod_ev.status = DEPEVT_STATUS_LST;
-	dwc3_cleanup_done_reqs(dwc, dep, &mod_ev, -ESHUTDOWN);
-	dep->flags &= ~DWC3_EP_BUSY;
-	/* pending requests are ignored and are queued on XferNotReady */
-}
-
-static void dwc3_ep_cmd_compl(struct dwc3_ep *dep,
-		const struct dwc3_event_depevt *event)
-{
-	u32 param = event->parameters;
-	u32 cmd_type = (param >> 8) & ((1 << 5) - 1);
-
-	switch (cmd_type) {
-	case DWC3_DEPCMD_ENDTRANSFER:
-		dwc3_process_ep_cmd_complete(dep, event);
-		break;
-	case DWC3_DEPCMD_STARTTRANSFER:
-		dep->res_trans_idx = param & 0x7f;
-		break;
-	default:
-		printk(KERN_ERR "%s() unknown /unexpected type: %d\n",
-				__func__, cmd_type);
-		break;
-	};
-}
-
 static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
@@ -1724,6 +1775,9 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 
 	dep = dwc->eps[epnum];
 
+	if (!(dep->flags & DWC3_EP_ENABLED))
+		return;
+
 	dev_vdbg(dwc->dev, "%s: %s\n", dep->name,
 			dwc3_ep_event_string(event->endpoint_event));
 
@@ -1734,7 +1788,7 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 
 	switch (event->endpoint_event) {
 	case DWC3_DEPEVT_XFERCOMPLETE:
-		dep->res_trans_idx = 0;
+		dep->resource_index = 0;
 
 		if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
 			dev_dbg(dwc->dev, "%s is an Isochronous endpoint\n",
@@ -1797,7 +1851,7 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		dev_dbg(dwc->dev, "%s FIFO Overrun\n", dep->name);
 		break;
 	case DWC3_DEPEVT_EPCMDCMPLT:
-		dwc3_ep_cmd_compl(dep, event);
+		dev_vdbg(dwc->dev, "Endpoint Command Complete\n");
 		break;
 	}
 }
@@ -1820,16 +1874,16 @@ static void dwc3_stop_active_transfer(struct dwc3 *dwc, u32 epnum)
 
 	dep = dwc->eps[epnum];
 
-	WARN_ON(!dep->res_trans_idx);
-	if (dep->res_trans_idx) {
-		cmd = DWC3_DEPCMD_ENDTRANSFER;
-		cmd |= DWC3_DEPCMD_HIPRI_FORCERM | DWC3_DEPCMD_CMDIOC;
-		cmd |= DWC3_DEPCMD_PARAM(dep->res_trans_idx);
-		memset(&params, 0, sizeof(params));
-		ret = dwc3_send_gadget_ep_cmd(dwc, dep->number, cmd, &params);
-		WARN_ON_ONCE(ret);
-		dep->res_trans_idx = 0;
-	}
+	if (!dep->resource_index)
+		return;
+
+	cmd = DWC3_DEPCMD_ENDTRANSFER;
+	cmd |= DWC3_DEPCMD_HIPRI_FORCERM | DWC3_DEPCMD_CMDIOC;
+	cmd |= DWC3_DEPCMD_PARAM(dep->resource_index);
+	memset(&params, 0, sizeof(params));
+	ret = dwc3_send_gadget_ep_cmd(dwc, dep->number, cmd, &params);
+	WARN_ON_ONCE(ret);
+	dep->resource_index = 0;
 }
 
 static void dwc3_stop_active_transfers(struct dwc3 *dwc)
@@ -1872,11 +1926,9 @@ static void dwc3_clear_stall_all_ep(struct dwc3 *dwc)
 
 static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 {
+	int			reg;
+
 	dev_vdbg(dwc->dev, "%s\n", __func__);
-#if 0
-	XXX
-	U1/U2 is powersave optimization. Skip it for now. Anyway we need to
-	enable it before we can disable it.
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_INITU1ENA;
@@ -1884,9 +1936,7 @@ static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 
 	reg &= ~DWC3_DCTL_INITU2ENA;
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
-#endif
 
-	dwc3_stop_active_transfers(dwc);
 	dwc3_disconnect_gadget(dwc);
 	dwc->start_config_issued = false;
 
@@ -1894,30 +1944,30 @@ static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 	dwc->setup_packet_pending = false;
 }
 
-static void dwc3_gadget_usb3_phy_power(struct dwc3 *dwc, int on)
+static void dwc3_gadget_usb3_phy_suspend(struct dwc3 *dwc, int suspend)
 {
 	u32			reg;
 
 	reg = dwc3_readl(dwc->regs, DWC3_GUSB3PIPECTL(0));
 
-	if (on)
-		reg &= ~DWC3_GUSB3PIPECTL_SUSPHY;
-	else
+	if (suspend)
 		reg |= DWC3_GUSB3PIPECTL_SUSPHY;
+	else
+		reg &= ~DWC3_GUSB3PIPECTL_SUSPHY;
 
 	dwc3_writel(dwc->regs, DWC3_GUSB3PIPECTL(0), reg);
 }
 
-static void dwc3_gadget_usb2_phy_power(struct dwc3 *dwc, int on)
+static void dwc3_gadget_usb2_phy_suspend(struct dwc3 *dwc, int suspend)
 {
 	u32			reg;
 
 	reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYCFG(0));
 
-	if (on)
-		reg &= ~DWC3_GUSB2PHYCFG_SUSPHY;
-	else
+	if (suspend)
 		reg |= DWC3_GUSB2PHYCFG_SUSPHY;
+	else
+		reg &= ~DWC3_GUSB2PHYCFG_SUSPHY;
 
 	dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
 }
@@ -1962,16 +2012,18 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 	/* after reset -> Default State */
 	dwc->dev_state = DWC3_DEFAULT_STATE;
 
-	/* Enable PHYs */
-	dwc3_gadget_usb2_phy_power(dwc, true);
-	dwc3_gadget_usb3_phy_power(dwc, true);
+	/* Recent versions support automatic phy suspend and don't need this */
+	if (dwc->revision < DWC3_REVISION_194A) {
+		/* Resume PHYs */
+		dwc3_gadget_usb2_phy_suspend(dwc, false);
+		dwc3_gadget_usb3_phy_suspend(dwc, false);
+	}
 
 	if (dwc->gadget.speed != USB_SPEED_UNKNOWN)
 		dwc3_disconnect_gadget(dwc);
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_TSTCTRL_MASK;
-	reg &= ~(DWC3_DCTL_INITU1ENA | DWC3_DCTL_INITU2ENA);
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 	dwc->test_mode = false;
 
@@ -2010,16 +2062,16 @@ static void dwc3_update_ram_clk_sel(struct dwc3 *dwc, u32 speed)
 	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
 }
 
-static void dwc3_gadget_disable_phy(struct dwc3 *dwc, u8 speed)
+static void dwc3_gadget_phy_suspend(struct dwc3 *dwc, u8 speed)
 {
 	switch (speed) {
 	case USB_SPEED_SUPER:
-		dwc3_gadget_usb2_phy_power(dwc, false);
+		dwc3_gadget_usb2_phy_suspend(dwc, true);
 		break;
 	case USB_SPEED_HIGH:
 	case USB_SPEED_FULL:
 	case USB_SPEED_LOW:
-		dwc3_gadget_usb3_phy_power(dwc, false);
+		dwc3_gadget_usb3_phy_suspend(dwc, true);
 		break;
 	}
 }
@@ -2082,8 +2134,11 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
 		break;
 	}
 
-	/* Disable unneded PHY */
-	dwc3_gadget_disable_phy(dwc, dwc->gadget.speed);
+	/* Recent versions support automatic phy suspend and don't need this */
+	if (dwc->revision < DWC3_REVISION_194A) {
+		/* Suspend unneeded PHY */
+		dwc3_gadget_phy_suspend(dwc, dwc->gadget.speed);
+	}
 
 	dep = dwc->eps[0];
 	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL);
@@ -2373,10 +2428,6 @@ int __devinit dwc3_gadget_init(struct dwc3 *dwc)
 	reg |= DWC3_DCFG_LPM_CAP;
 	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
 
-	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
-	reg |= DWC3_DCTL_ACCEPTU1ENA | DWC3_DCTL_ACCEPTU2ENA;
-	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
-
 	/* Enable all but Start and End of Frame IRQs */
 	reg = (DWC3_DEVTEN_VNDRDEVTSTRCVEDEN |
 			DWC3_DEVTEN_EVNTOVERFLOWEN |
@@ -2389,6 +2440,24 @@ int __devinit dwc3_gadget_init(struct dwc3 *dwc)
 			DWC3_DEVTEN_DISCONNEVTEN);
 	dwc3_writel(dwc->regs, DWC3_DEVTEN, reg);
 
+	/* Enable USB2 LPM and automatic phy suspend only on recent versions */
+	if (dwc->revision >= DWC3_REVISION_194A) {
+		reg = dwc3_readl(dwc->regs, DWC3_DCFG);
+		reg |= DWC3_DCFG_LPM_CAP;
+		dwc3_writel(dwc->regs, DWC3_DCFG, reg);
+
+		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+		reg &= ~(DWC3_DCTL_HIRD_THRES_MASK | DWC3_DCTL_L1_HIBER_EN);
+
+		/* TODO: This should be configurable */
+		reg |= DWC3_DCTL_HIRD_THRES(28);
+
+		dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+
+		dwc3_gadget_usb2_phy_suspend(dwc, false);
+		dwc3_gadget_usb3_phy_suspend(dwc, false);
+	}
+
 	ret = device_register(&dwc->gadget.dev);
 	if (ret) {
 		dev_err(dwc->dev, "failed to register gadget device\n");
diff --git a/drivers/usb/dwc3/gadget.h b/drivers/usb/dwc3/gadget.h
index 95ef6a2f776..99e6d724882 100644
--- a/drivers/usb/dwc3/gadget.h
+++ b/drivers/usb/dwc3/gadget.h
@@ -66,7 +66,12 @@ struct dwc3;
 #define DWC3_DEPCFG_FIFO_NUMBER(n)	((n) << 17)
 #define DWC3_DEPCFG_BURST_SIZE(n)	((n) << 22)
 #define DWC3_DEPCFG_DATA_SEQ_NUM(n)	((n) << 26)
+/* This applies for core versions earlier than 1.94a */
 #define DWC3_DEPCFG_IGN_SEQ_NUM		(1 << 31)
+/* These apply for core versions 1.94a and later */
+#define DWC3_DEPCFG_ACTION_INIT		(0 << 30)
+#define DWC3_DEPCFG_ACTION_RESTORE	(1 << 30)
+#define DWC3_DEPCFG_ACTION_MODIFY	(2 << 30)
 
 /* DEPXFERCFG parameter 0 */
 #define DWC3_DEPXFERCFG_NUM_XFER_RES(n)	((n) & 0xffff)
@@ -106,6 +111,7 @@ int dwc3_gadget_set_link_state(struct dwc3 *dwc, enum dwc3_link_state state);
 void dwc3_ep0_interrupt(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event);
 void dwc3_ep0_out_start(struct dwc3 *dwc);
+int dwc3_gadget_ep0_set_halt(struct usb_ep *ep, int value);
 int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 		gfp_t gfp_flags);
 int __dwc3_gadget_ep_set_halt(struct dwc3_ep *dep, int value);
diff --git a/drivers/usb/early/ehci-dbgp.c b/drivers/usb/early/ehci-dbgp.c
index 1fc8f124980..ee0ebacf822 100644
--- a/drivers/usb/early/ehci-dbgp.c
+++ b/drivers/usb/early/ehci-dbgp.c
@@ -334,7 +334,7 @@ static int dbgp_control_msg(unsigned devnum, int requesttype,
 	int ret;
 
 	read = (requesttype & USB_DIR_IN) != 0;
-	if (size > (read ? DBGP_MAX_PACKET:0))
+	if (size > (read ? DBGP_MAX_PACKET : 0))
 		return -1;
 
 	/* Compute the control message */
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index bddc8fd9a7b..51ab5fd5d46 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -185,7 +185,7 @@ config USB_FUSB300
 
 config USB_OMAP
 	tristate "OMAP USB Device Controller"
-	depends on ARCH_OMAP
+	depends on ARCH_OMAP1
 	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3 || MACH_OMAP_H4_OTG
 	select USB_OTG_UTILS if ARCH_OMAP
 	help
@@ -321,6 +321,15 @@ config USB_MV_UDC
 	  USB2.0 OTG controller, which can be configured as high speed or
 	  full speed USB peripheral.
 
+config USB_MV_U3D
+	tristate "MARVELL PXA2128 USB 3.0 controller"
+	depends on CPU_MMP3
+	select USB_GADGET_DUALSPEED
+	select USB_GADGET_SUPERSPEED
+	help
+	  MARVELL PXA2128 Processor series include a super speed USB3.0 device
+	  controller, which support super speed USB peripheral.
+
 #
 # Controllers available in both integrated and discrete versions
 #
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 1811513f1c2..3fd8cd09d2c 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_USB_EG20T)		+= pch_udc.o
 obj-$(CONFIG_USB_MV_UDC)	+= mv_udc.o
 mv_udc-y			:= mv_udc_core.o
 obj-$(CONFIG_USB_FUSB300)	+= fusb300_udc.o
+obj-$(CONFIG_USB_MV_U3D)	+= mv_u3d_core.o
 
 #
 # USB gadget drivers
diff --git a/drivers/usb/gadget/acm_ms.c b/drivers/usb/gadget/acm_ms.c
index fdb7aec3bd0..75b8a691fd0 100644
--- a/drivers/usb/gadget/acm_ms.c
+++ b/drivers/usb/gadget/acm_ms.c
@@ -235,6 +235,7 @@ static int __exit acm_ms_unbind(struct usb_composite_dev *cdev)
 static struct usb_composite_driver acm_ms_driver = {
 	.name		= "g_acm_ms",
 	.dev		= &device_desc,
+	.max_speed	= USB_SPEED_SUPER,
 	.strings	= dev_strings,
 	.unbind		= __exit_p(acm_ms_unbind),
 };
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index 1a4430f315c..c9e66dfb02e 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -1634,6 +1634,7 @@ static int at91_start(struct usb_gadget *gadget,
 	udc = container_of(gadget, struct at91_udc, gadget);
 	udc->driver = driver;
 	udc->gadget.dev.driver = &driver->driver;
+	udc->gadget.dev.of_node = udc->pdev->dev.of_node;
 	dev_set_drvdata(&udc->gadget.dev, &driver->driver);
 	udc->enabled = 1;
 	udc->selfpowered = 1;
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 390749bbb0c..3f72110da1b 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -117,6 +117,7 @@ int config_ep_by_speed(struct usb_gadget *g,
 			struct usb_function *f,
 			struct usb_ep *_ep)
 {
+	struct usb_composite_dev	*cdev = get_gadget_data(g);
 	struct usb_endpoint_descriptor *chosen_desc = NULL;
 	struct usb_descriptor_header **speed_desc = NULL;
 
@@ -180,10 +181,12 @@ ep_found:
 			_ep->mult = comp_desc->bmAttributes & 0x3;
 		case USB_ENDPOINT_XFER_BULK:
 		case USB_ENDPOINT_XFER_INT:
-			_ep->maxburst = comp_desc->bMaxBurst;
+			_ep->maxburst = comp_desc->bMaxBurst + 1;
 			break;
 		default:
-			/* Do nothing for control endpoints */
+			if (comp_desc->bMaxBurst != 0)
+				ERROR(cdev, "ep0 bMaxBurst must be 0\n");
+			_ep->maxburst = 1;
 			break;
 		}
 	}
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index dcd1c7fbb01..8adc79d1b40 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -21,6 +21,7 @@
 #include <linux/blkdev.h>
 #include <linux/pagemap.h>
 #include <linux/export.h>
+#include <linux/hid.h>
 #include <asm/unaligned.h>
 
 #include <linux/usb/composite.h>
@@ -1671,6 +1672,12 @@ static int __must_check ffs_do_desc(char *data, unsigned len,
 	}
 		break;
 
+	case HID_DT_HID:
+		pr_vdebug("hid descriptor\n");
+		if (length != sizeof(struct hid_descriptor))
+			goto inv_length;
+		break;
+
 	case USB_DT_OTG:
 		if (length != sizeof(struct usb_otg_descriptor))
 			goto inv_length;
diff --git a/drivers/usb/gadget/f_hid.c b/drivers/usb/gadget/f_hid.c
index 3b3932c5536..16a8b1c15c6 100644
--- a/drivers/usb/gadget/f_hid.c
+++ b/drivers/usb/gadget/f_hid.c
@@ -26,6 +26,12 @@ static struct class *hidg_class;
 /*-------------------------------------------------------------------------*/
 /*                            HID gadget struct                            */
 
+struct f_hidg_req_list {
+	struct usb_request	*req;
+	unsigned int		pos;
+	struct list_head 	list;
+};
+
 struct f_hidg {
 	/* configuration */
 	unsigned char			bInterfaceSubClass;
@@ -35,10 +41,10 @@ struct f_hidg {
 	unsigned short			report_length;
 
 	/* recv report */
-	char				*set_report_buff;
-	unsigned short			set_report_length;
+	struct list_head		completed_out_req;
 	spinlock_t			spinlock;
 	wait_queue_head_t		read_queue;
+	unsigned int			qlen;
 
 	/* send report */
 	struct mutex			lock;
@@ -49,7 +55,9 @@ struct f_hidg {
 	int				minor;
 	struct cdev			cdev;
 	struct usb_function		func;
+
 	struct usb_ep			*in_ep;
+	struct usb_ep			*out_ep;
 };
 
 static inline struct f_hidg *func_to_hidg(struct usb_function *f)
@@ -65,7 +73,7 @@ static struct usb_interface_descriptor hidg_interface_desc = {
 	.bDescriptorType	= USB_DT_INTERFACE,
 	/* .bInterfaceNumber	= DYNAMIC */
 	.bAlternateSetting	= 0,
-	.bNumEndpoints		= 1,
+	.bNumEndpoints		= 2,
 	.bInterfaceClass	= USB_CLASS_HID,
 	/* .bInterfaceSubClass	= DYNAMIC */
 	/* .bInterfaceProtocol	= DYNAMIC */
@@ -96,10 +104,23 @@ static struct usb_endpoint_descriptor hidg_hs_in_ep_desc = {
 				      */
 };
 
+static struct usb_endpoint_descriptor hidg_hs_out_ep_desc = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bEndpointAddress	= USB_DIR_OUT,
+	.bmAttributes		= USB_ENDPOINT_XFER_INT,
+	/*.wMaxPacketSize	= DYNAMIC */
+	.bInterval		= 4, /* FIXME: Add this field in the
+				      * HID gadget configuration?
+				      * (struct hidg_func_descriptor)
+				      */
+};
+
 static struct usb_descriptor_header *hidg_hs_descriptors[] = {
 	(struct usb_descriptor_header *)&hidg_interface_desc,
 	(struct usb_descriptor_header *)&hidg_desc,
 	(struct usb_descriptor_header *)&hidg_hs_in_ep_desc,
+	(struct usb_descriptor_header *)&hidg_hs_out_ep_desc,
 	NULL,
 };
 
@@ -117,10 +138,23 @@ static struct usb_endpoint_descriptor hidg_fs_in_ep_desc = {
 				       */
 };
 
+static struct usb_endpoint_descriptor hidg_fs_out_ep_desc = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bEndpointAddress	= USB_DIR_OUT,
+	.bmAttributes		= USB_ENDPOINT_XFER_INT,
+	/*.wMaxPacketSize	= DYNAMIC */
+	.bInterval		= 10, /* FIXME: Add this field in the
+				       * HID gadget configuration?
+				       * (struct hidg_func_descriptor)
+				       */
+};
+
 static struct usb_descriptor_header *hidg_fs_descriptors[] = {
 	(struct usb_descriptor_header *)&hidg_interface_desc,
 	(struct usb_descriptor_header *)&hidg_desc,
 	(struct usb_descriptor_header *)&hidg_fs_in_ep_desc,
+	(struct usb_descriptor_header *)&hidg_fs_out_ep_desc,
 	NULL,
 };
 
@@ -130,9 +164,11 @@ static struct usb_descriptor_header *hidg_fs_descriptors[] = {
 static ssize_t f_hidg_read(struct file *file, char __user *buffer,
 			size_t count, loff_t *ptr)
 {
-	struct f_hidg	*hidg     = file->private_data;
-	char		*tmp_buff = NULL;
-	unsigned long	flags;
+	struct f_hidg *hidg = file->private_data;
+	struct f_hidg_req_list *list;
+	struct usb_request *req;
+	unsigned long flags;
+	int ret;
 
 	if (!count)
 		return 0;
@@ -142,8 +178,9 @@ static ssize_t f_hidg_read(struct file *file, char __user *buffer,
 
 	spin_lock_irqsave(&hidg->spinlock, flags);
 
-#define READ_COND (hidg->set_report_buff != NULL)
+#define READ_COND (!list_empty(&hidg->completed_out_req))
 
+	/* wait for at least one buffer to complete */
 	while (!READ_COND) {
 		spin_unlock_irqrestore(&hidg->spinlock, flags);
 		if (file->f_flags & O_NONBLOCK)
@@ -155,19 +192,34 @@ static ssize_t f_hidg_read(struct file *file, char __user *buffer,
 		spin_lock_irqsave(&hidg->spinlock, flags);
 	}
 
-
-	count = min_t(unsigned, count, hidg->set_report_length);
-	tmp_buff = hidg->set_report_buff;
-	hidg->set_report_buff = NULL;
-
+	/* pick the first one */
+	list = list_first_entry(&hidg->completed_out_req,
+				struct f_hidg_req_list, list);
+	req = list->req;
+	count = min_t(unsigned int, count, req->actual - list->pos);
 	spin_unlock_irqrestore(&hidg->spinlock, flags);
 
-	if (tmp_buff != NULL) {
-		/* copy to user outside spinlock */
-		count -= copy_to_user(buffer, tmp_buff, count);
-		kfree(tmp_buff);
-	} else
-		count = -ENOMEM;
+	/* copy to user outside spinlock */
+	count -= copy_to_user(buffer, req->buf + list->pos, count);
+	list->pos += count;
+
+	/*
+	 * if this request is completely handled and transfered to
+	 * userspace, remove its entry from the list and requeue it
+	 * again. Otherwise, we will revisit it again upon the next
+	 * call, taking into account its current read position.
+	 */
+	if (list->pos == req->actual) {
+		spin_lock_irqsave(&hidg->spinlock, flags);
+		list_del(&list->list);
+		kfree(list);
+		spin_unlock_irqrestore(&hidg->spinlock, flags);
+
+		req->length = hidg->report_length;
+		ret = usb_ep_queue(hidg->out_ep, req, GFP_KERNEL);
+		if (ret < 0)
+			return ret;
+	}
 
 	return count;
 }
@@ -282,28 +334,37 @@ static int f_hidg_open(struct inode *inode, struct file *fd)
 /*-------------------------------------------------------------------------*/
 /*                                usb_function                             */
 
-static void hidg_set_report_complete(struct usb_ep *ep, struct usb_request *req)
+static struct usb_request *hidg_alloc_ep_req(struct usb_ep *ep, unsigned length)
 {
-	struct f_hidg *hidg = (struct f_hidg *)req->context;
-
-	if (req->status != 0 || req->buf == NULL || req->actual == 0) {
-		ERROR(hidg->func.config->cdev, "%s FAILED\n", __func__);
-		return;
+	struct usb_request *req;
+
+	req = usb_ep_alloc_request(ep, GFP_ATOMIC);
+	if (req) {
+		req->length = length;
+		req->buf = kmalloc(length, GFP_ATOMIC);
+		if (!req->buf) {
+			usb_ep_free_request(ep, req);
+			req = NULL;
+		}
 	}
+	return req;
+}
 
-	spin_lock(&hidg->spinlock);
-
-	hidg->set_report_buff = krealloc(hidg->set_report_buff,
-					 req->actual, GFP_ATOMIC);
+static void hidg_set_report_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_hidg *hidg = (struct f_hidg *) req->context;
+	struct f_hidg_req_list *req_list;
+	unsigned long flags;
 
-	if (hidg->set_report_buff == NULL) {
-		spin_unlock(&hidg->spinlock);
+	req_list = kzalloc(sizeof(*req_list), GFP_ATOMIC);
+	if (!req_list)
 		return;
-	}
-	hidg->set_report_length = req->actual;
-	memcpy(hidg->set_report_buff, req->buf, req->actual);
 
-	spin_unlock(&hidg->spinlock);
+	req_list->req = req;
+
+	spin_lock_irqsave(&hidg->spinlock, flags);
+	list_add_tail(&req_list->list, &hidg->completed_out_req);
+	spin_unlock_irqrestore(&hidg->spinlock, flags);
 
 	wake_up(&hidg->read_queue);
 }
@@ -344,9 +405,7 @@ static int hidg_setup(struct usb_function *f,
 	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
 		  | HID_REQ_SET_REPORT):
 		VDBG(cdev, "set_report | wLenght=%d\n", ctrl->wLength);
-		req->context  = hidg;
-		req->complete = hidg_set_report_complete;
-		goto respond;
+		goto stall;
 		break;
 
 	case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
@@ -403,16 +462,25 @@ respond:
 static void hidg_disable(struct usb_function *f)
 {
 	struct f_hidg *hidg = func_to_hidg(f);
+	struct f_hidg_req_list *list, *next;
 
 	usb_ep_disable(hidg->in_ep);
 	hidg->in_ep->driver_data = NULL;
+
+	usb_ep_disable(hidg->out_ep);
+	hidg->out_ep->driver_data = NULL;
+
+	list_for_each_entry_safe(list, next, &hidg->completed_out_req, list) {
+		list_del(&list->list);
+		kfree(list);
+	}
 }
 
 static int hidg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 {
 	struct usb_composite_dev		*cdev = f->config->cdev;
 	struct f_hidg				*hidg = func_to_hidg(f);
-	int status = 0;
+	int i, status = 0;
 
 	VDBG(cdev, "hidg_set_alt intf:%d alt:%d\n", intf, alt);
 
@@ -429,11 +497,55 @@ static int hidg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 		}
 		status = usb_ep_enable(hidg->in_ep);
 		if (status < 0) {
-			ERROR(cdev, "Enable endpoint FAILED!\n");
+			ERROR(cdev, "Enable IN endpoint FAILED!\n");
 			goto fail;
 		}
 		hidg->in_ep->driver_data = hidg;
 	}
+
+
+	if (hidg->out_ep != NULL) {
+		/* restart endpoint */
+		if (hidg->out_ep->driver_data != NULL)
+			usb_ep_disable(hidg->out_ep);
+
+		status = config_ep_by_speed(f->config->cdev->gadget, f,
+					    hidg->out_ep);
+		if (status) {
+			ERROR(cdev, "config_ep_by_speed FAILED!\n");
+			goto fail;
+		}
+		status = usb_ep_enable(hidg->out_ep);
+		if (status < 0) {
+			ERROR(cdev, "Enable IN endpoint FAILED!\n");
+			goto fail;
+		}
+		hidg->out_ep->driver_data = hidg;
+
+		/*
+		 * allocate a bunch of read buffers and queue them all at once.
+		 */
+		for (i = 0; i < hidg->qlen && status == 0; i++) {
+			struct usb_request *req =
+					hidg_alloc_ep_req(hidg->out_ep,
+							  hidg->report_length);
+			if (req) {
+				req->complete = hidg_set_report_complete;
+				req->context  = hidg;
+				status = usb_ep_queue(hidg->out_ep, req,
+						      GFP_ATOMIC);
+				if (status)
+					ERROR(cdev, "%s queue req --> %d\n",
+						hidg->out_ep->name, status);
+			} else {
+				usb_ep_disable(hidg->out_ep);
+				hidg->out_ep->driver_data = NULL;
+				status = -ENOMEM;
+				goto fail;
+			}
+		}
+	}
+
 fail:
 	return status;
 }
@@ -470,13 +582,18 @@ static int __init hidg_bind(struct usb_configuration *c, struct usb_function *f)
 	ep->driver_data = c->cdev;	/* claim */
 	hidg->in_ep = ep;
 
+	ep = usb_ep_autoconfig(c->cdev->gadget, &hidg_fs_out_ep_desc);
+	if (!ep)
+		goto fail;
+	ep->driver_data = c->cdev;	/* claim */
+	hidg->out_ep = ep;
+
 	/* preallocate request and buffer */
 	status = -ENOMEM;
 	hidg->req = usb_ep_alloc_request(hidg->in_ep, GFP_KERNEL);
 	if (!hidg->req)
 		goto fail;
 
-
 	hidg->req->buf = kmalloc(hidg->report_length, GFP_KERNEL);
 	if (!hidg->req->buf)
 		goto fail;
@@ -486,12 +603,12 @@ static int __init hidg_bind(struct usb_configuration *c, struct usb_function *f)
 	hidg_interface_desc.bInterfaceProtocol = hidg->bInterfaceProtocol;
 	hidg_hs_in_ep_desc.wMaxPacketSize = cpu_to_le16(hidg->report_length);
 	hidg_fs_in_ep_desc.wMaxPacketSize = cpu_to_le16(hidg->report_length);
+	hidg_hs_out_ep_desc.wMaxPacketSize = cpu_to_le16(hidg->report_length);
+	hidg_fs_out_ep_desc.wMaxPacketSize = cpu_to_le16(hidg->report_length);
 	hidg_desc.desc[0].bDescriptorType = HID_DT_REPORT;
 	hidg_desc.desc[0].wDescriptorLength =
 		cpu_to_le16(hidg->report_desc_length);
 
-	hidg->set_report_buff = NULL;
-
 	/* copy descriptors */
 	f->descriptors = usb_copy_descriptors(hidg_fs_descriptors);
 	if (!f->descriptors)
@@ -500,6 +617,8 @@ static int __init hidg_bind(struct usb_configuration *c, struct usb_function *f)
 	if (gadget_is_dualspeed(c->cdev->gadget)) {
 		hidg_hs_in_ep_desc.bEndpointAddress =
 			hidg_fs_in_ep_desc.bEndpointAddress;
+		hidg_hs_out_ep_desc.bEndpointAddress =
+			hidg_fs_out_ep_desc.bEndpointAddress;
 		f->hs_descriptors = usb_copy_descriptors(hidg_hs_descriptors);
 		if (!f->hs_descriptors)
 			goto fail;
@@ -509,6 +628,7 @@ static int __init hidg_bind(struct usb_configuration *c, struct usb_function *f)
 	spin_lock_init(&hidg->spinlock);
 	init_waitqueue_head(&hidg->write_queue);
 	init_waitqueue_head(&hidg->read_queue);
+	INIT_LIST_HEAD(&hidg->completed_out_req);
 
 	/* create char device */
 	cdev_init(&hidg->cdev, &f_hidg_fops);
@@ -553,7 +673,6 @@ static void hidg_unbind(struct usb_configuration *c, struct usb_function *f)
 	usb_free_descriptors(f->descriptors);
 
 	kfree(hidg->report_desc);
-	kfree(hidg->set_report_buff);
 	kfree(hidg);
 }
 
@@ -624,6 +743,9 @@ int __init hidg_bind_config(struct usb_configuration *c,
 	hidg->func.disable = hidg_disable;
 	hidg->func.setup   = hidg_setup;
 
+	/* this could me made configurable at some point */
+	hidg->qlen	   = 4;
+
 	status = usb_add_function(c, &hidg->func);
 	if (status)
 		kfree(hidg);
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index f67b453740b..4f1142efa6d 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -44,12 +44,12 @@
  * function for a USB device, it also illustrates a technique of
  * double-buffering for increased throughput.
  *
- * Function supports multiple logical units (LUNs).  Backing storage
- * for each LUN is provided by a regular file or a block device.
- * Access for each LUN can be limited to read-only.  Moreover, the
- * function can indicate that LUN is removable and/or CD-ROM.  (The
- * later implies read-only access.)
- *
+ * For more information about MSF and in particular its module
+ * parameters and sysfs interface read the
+ * <Documentation/usb/mass-storage.txt> file.
+ */
+
+/*
  * MSF is configured by specifying a fsg_config structure.  It has the
  * following fields:
  *
@@ -75,25 +75,6 @@
  *	->nofua		Flag specifying that FUA flag in SCSI WRITE(10,12)
  *				commands for this LUN shall be ignored.
  *
- *	lun_name_format	A printf-like format for names of the LUN
- *				devices.  This determines how the
- *				directory in sysfs will be named.
- *				Unless you are using several MSFs in
- *				a single gadget (as opposed to single
- *				MSF in many configurations) you may
- *				leave it as NULL (in which case
- *				"lun%d" will be used).  In the format
- *				you can use "%d" to index LUNs for
- *				MSF's with more than one LUN.  (Beware
- *				that there is only one integer given
- *				as an argument for the format and
- *				specifying invalid format may cause
- *				unspecified behaviour.)
- *	thread_name	Name of the kernel thread process used by the
- *				MSF.  You can safely set it to NULL
- *				(in which case default "file-storage"
- *				will be used).
- *
  *	vendor_name
  *	product_name
  *	release		Information used as a reply to INQUIRY
@@ -114,62 +95,6 @@
  * data track and no audio tracks; hence there need be only one
  * backing file per LUN.
  *
- *
- * MSF includes support for module parameters.  If gadget using it
- * decides to use it, the following module parameters will be
- * available:
- *
- *	file=filename[,filename...]
- *			Names of the files or block devices used for
- *				backing storage.
- *	ro=b[,b...]	Default false, boolean for read-only access.
- *	removable=b[,b...]
- *			Default true, boolean for removable media.
- *	cdrom=b[,b...]	Default false, boolean for whether to emulate
- *				a CD-ROM drive.
- *	nofua=b[,b...]	Default false, booleans for ignore FUA flag
- *				in SCSI WRITE(10,12) commands
- *	luns=N		Default N = number of filenames, number of
- *				LUNs to support.
- *	stall		Default determined according to the type of
- *				USB device controller (usually true),
- *				boolean to permit the driver to halt
- *				bulk endpoints.
- *
- * The module parameters may be prefixed with some string.  You need
- * to consult gadget's documentation or source to verify whether it is
- * using those module parameters and if it does what are the prefixes
- * (look for FSG_MODULE_PARAMETERS() macro usage, what's inside it is
- * the prefix).
- *
- *
- * Requirements are modest; only a bulk-in and a bulk-out endpoint are
- * needed.  The memory requirement amounts to two 16K buffers, size
- * configurable by a parameter.  Support is included for both
- * full-speed and high-speed operation.
- *
- * Note that the driver is slightly non-portable in that it assumes a
- * single memory/DMA buffer will be useable for bulk-in, bulk-out, and
- * interrupt-in endpoints.  With most device controllers this isn't an
- * issue, but there may be some with hardware restrictions that prevent
- * a buffer from being used by more than one endpoint.
- *
- *
- * The pathnames of the backing files and the ro settings are
- * available in the attribute files "file" and "ro" in the lun<n> (or
- * to be more precise in a directory which name comes from
- * "lun_name_format" option!) subdirectory of the gadget's sysfs
- * directory.  If the "removable" option is set, writing to these
- * files will simulate ejecting/loading the medium (writing an empty
- * line means eject) and adjusting a write-enable tab.  Changes to the
- * ro setting are not allowed when the medium is loaded or if CD-ROM
- * emulation is being used.
- *
- * When a LUN receive an "eject" SCSI request (Start/Stop Unit),
- * if the LUN is removable, the backing file is released to simulate
- * ejection.
- *
- *
  * This function is heavily based on "File-backed Storage Gadget" by
  * Alan Stern which in turn is heavily based on "Gadget Zero" by David
  * Brownell.  The driver's SCSI command interface was based on the
@@ -211,7 +136,7 @@
  * In normal operation the main thread is started during the gadget's
  * fsg_bind() callback and stopped during fsg_unbind().  But it can
  * also exit when it receives a signal, and there's no point leaving
- * the gadget running when the thread is dead.  At of this moment, MSF
+ * the gadget running when the thread is dead.  As of this moment, MSF
  * provides no way to deregister the gadget when thread dies -- maybe
  * a callback functions is needed.
  *
@@ -417,9 +342,6 @@ struct fsg_config {
 		char nofua;
 	} luns[FSG_MAX_LUNS];
 
-	const char		*lun_name_format;
-	const char		*thread_name;
-
 	/* Callback functions. */
 	const struct fsg_operations	*ops;
 	/* Gadget's private data. */
@@ -2687,11 +2609,15 @@ static int fsg_main_thread(void *common_)
 
 /*************************** DEVICE ATTRIBUTES ***************************/
 
-/* Write permission is checked per LUN in store_*() functions. */
 static DEVICE_ATTR(ro, 0644, fsg_show_ro, fsg_store_ro);
 static DEVICE_ATTR(nofua, 0644, fsg_show_nofua, fsg_store_nofua);
 static DEVICE_ATTR(file, 0644, fsg_show_file, fsg_store_file);
 
+static struct device_attribute dev_attr_ro_cdrom =
+	__ATTR(ro, 0444, fsg_show_ro, NULL);
+static struct device_attribute dev_attr_file_nonremovable =
+	__ATTR(file, 0444, fsg_show_file, NULL);
+
 
 /****************************** FSG COMMON ******************************/
 
@@ -2792,11 +2718,7 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
 		curlun->dev.parent = &gadget->dev;
 		/* curlun->dev.driver = &fsg_driver.driver; XXX */
 		dev_set_drvdata(&curlun->dev, &common->filesem);
-		dev_set_name(&curlun->dev,
-			     cfg->lun_name_format
-			   ? cfg->lun_name_format
-			   : "lun%d",
-			     i);
+		dev_set_name(&curlun->dev, "lun%d", i);
 
 		rc = device_register(&curlun->dev);
 		if (rc) {
@@ -2806,10 +2728,16 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
 			goto error_release;
 		}
 
-		rc = device_create_file(&curlun->dev, &dev_attr_ro);
+		rc = device_create_file(&curlun->dev,
+					curlun->cdrom
+				      ? &dev_attr_ro_cdrom
+				      : &dev_attr_ro);
 		if (rc)
 			goto error_luns;
-		rc = device_create_file(&curlun->dev, &dev_attr_file);
+		rc = device_create_file(&curlun->dev,
+					curlun->removable
+				      ? &dev_attr_file
+				      : &dev_attr_file_nonremovable);
 		if (rc)
 			goto error_luns;
 		rc = device_create_file(&curlun->dev, &dev_attr_nofua);
@@ -2878,8 +2806,7 @@ buffhds_first_it:
 
 	/* Tell the thread to start working */
 	common->thread_task =
-		kthread_create(fsg_main_thread, common,
-			       cfg->thread_name ?: "file-storage");
+		kthread_create(fsg_main_thread, common, "file-storage");
 	if (IS_ERR(common->thread_task)) {
 		rc = PTR_ERR(common->thread_task);
 		goto error_release;
@@ -2945,8 +2872,14 @@ static void fsg_common_release(struct kref *ref)
 		/* In error recovery common->nluns may be zero. */
 		for (; i; --i, ++lun) {
 			device_remove_file(&lun->dev, &dev_attr_nofua);
-			device_remove_file(&lun->dev, &dev_attr_ro);
-			device_remove_file(&lun->dev, &dev_attr_file);
+			device_remove_file(&lun->dev,
+					   lun->cdrom
+					 ? &dev_attr_ro_cdrom
+					 : &dev_attr_ro);
+			device_remove_file(&lun->dev,
+					   lun->removable
+					 ? &dev_attr_file
+					 : &dev_attr_file_nonremovable);
 			fsg_lun_close(lun);
 			device_unregister(&lun->dev);
 		}
@@ -3167,8 +3100,7 @@ fsg_config_from_params(struct fsg_config *cfg,
 	for (i = 0, lun = cfg->luns; i < cfg->nluns; ++i, ++lun) {
 		lun->ro = !!params->ro[i];
 		lun->cdrom = !!params->cdrom[i];
-		lun->removable = /* Removable by default */
-			params->removable_count <= i || params->removable[i];
+		lun->removable = !!params->removable[i];
 		lun->filename =
 			params->file_count > i && params->file[i][0]
 			? params->file[i]
@@ -3176,8 +3108,6 @@ fsg_config_from_params(struct fsg_config *cfg,
 	}
 
 	/* Let MSF use defaults */
-	cfg->lun_name_format = 0;
-	cfg->thread_name = 0;
 	cfg->vendor_name = 0;
 	cfg->product_name = 0;
 	cfg->release = 0xffff;
@@ -3203,4 +3133,3 @@ fsg_common_from_params(struct fsg_common *common,
 	fsg_config_from_params(&cfg, params);
 	return fsg_common_init(common, cdev, &cfg);
 }
-
diff --git a/drivers/usb/gadget/f_phonet.c b/drivers/usb/gadget/f_phonet.c
index 965a6293206..8ee9268fe25 100644
--- a/drivers/usb/gadget/f_phonet.c
+++ b/drivers/usb/gadget/f_phonet.c
@@ -301,7 +301,7 @@ pn_rx_submit(struct f_phonet *fp, struct usb_request *req, gfp_t gfp_flags)
 	struct page *page;
 	int err;
 
-	page = alloc_page(gfp_flags);
+	page = __skb_alloc_page(gfp_flags | __GFP_NOMEMALLOC, NULL);
 	if (!page)
 		return -ENOMEM;
 
diff --git a/drivers/usb/gadget/f_uvc.c b/drivers/usb/gadget/f_uvc.c
index 2022fe49214..2a8bf0655c6 100644
--- a/drivers/usb/gadget/f_uvc.c
+++ b/drivers/usb/gadget/f_uvc.c
@@ -29,6 +29,25 @@
 
 unsigned int uvc_gadget_trace_param;
 
+/*-------------------------------------------------------------------------*/
+
+/* module parameters specific to the Video streaming endpoint */
+static unsigned streaming_interval = 1;
+module_param(streaming_interval, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_interval, "1 - 16");
+
+static unsigned streaming_maxpacket = 1024;
+module_param(streaming_maxpacket, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_maxpacket, "0 - 1023 (fs), 0 - 1024 (hs/ss)");
+
+static unsigned streaming_mult;
+module_param(streaming_mult, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_mult, "0 - 2 (hs/ss only)");
+
+static unsigned streaming_maxburst;
+module_param(streaming_maxburst, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_maxburst, "0 - 15 (ss only)");
+
 /* --------------------------------------------------------------------------
  * Function descriptors
  */
@@ -59,6 +78,8 @@ static struct usb_gadget_strings *uvc_function_strings[] = {
 #define UVC_INTF_VIDEO_CONTROL			0
 #define UVC_INTF_VIDEO_STREAMING		1
 
+#define STATUS_BYTECOUNT			16	/* 16 bytes status */
+
 static struct usb_interface_assoc_descriptor uvc_iad __initdata = {
 	.bLength		= sizeof(uvc_iad),
 	.bDescriptorType	= USB_DT_INTERFACE_ASSOCIATION,
@@ -82,12 +103,12 @@ static struct usb_interface_descriptor uvc_control_intf __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_control_ep __initdata = {
+static struct usb_endpoint_descriptor uvc_fs_control_ep __initdata = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
 	.bmAttributes		= USB_ENDPOINT_XFER_INT,
-	.wMaxPacketSize		= cpu_to_le16(16),
+	.wMaxPacketSize		= cpu_to_le16(STATUS_BYTECOUNT),
 	.bInterval		= 8,
 };
 
@@ -95,7 +116,7 @@ static struct uvc_control_endpoint_descriptor uvc_control_cs_ep __initdata = {
 	.bLength		= UVC_DT_CONTROL_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_CS_ENDPOINT,
 	.bDescriptorSubType	= UVC_EP_INTERRUPT,
-	.wMaxTransferSize	= cpu_to_le16(16),
+	.wMaxTransferSize	= cpu_to_le16(STATUS_BYTECOUNT),
 };
 
 static struct usb_interface_descriptor uvc_streaming_intf_alt0 __initdata = {
@@ -122,7 +143,7 @@ static struct usb_interface_descriptor uvc_streaming_intf_alt1 __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_streaming_ep = {
+static struct usb_endpoint_descriptor uvc_fs_streaming_ep = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
@@ -131,15 +152,72 @@ static struct usb_endpoint_descriptor uvc_streaming_ep = {
 	.bInterval		= 1,
 };
 
+static struct usb_endpoint_descriptor uvc_hs_streaming_ep = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bEndpointAddress	= USB_DIR_IN,
+	.bmAttributes		= USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize		= cpu_to_le16(1024),
+	.bInterval		= 1,
+};
+
+/* super speed support */
+static struct usb_endpoint_descriptor uvc_ss_control_ep __initdata = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(STATUS_BYTECOUNT),
+	.bInterval =		8,
+};
+
+static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp __initdata = {
+	.bLength =		sizeof uvc_ss_control_comp,
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
+	/* the following 3 values can be tweaked if necessary */
+	/* .bMaxBurst =		0, */
+	/* .bmAttributes =	0, */
+	.wBytesPerInterval =	cpu_to_le16(STATUS_BYTECOUNT),
+};
+
+static struct usb_endpoint_descriptor uvc_ss_streaming_ep __initdata = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		4,
+};
+
+static struct usb_ss_ep_comp_descriptor uvc_ss_streaming_comp = {
+	.bLength =		sizeof uvc_ss_streaming_comp,
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
+	/* the following 3 values can be tweaked if necessary */
+	.bMaxBurst =		0,
+	.bmAttributes =	0,
+	.wBytesPerInterval =	cpu_to_le16(1024),
+};
+
 static const struct usb_descriptor_header * const uvc_fs_streaming[] = {
 	(struct usb_descriptor_header *) &uvc_streaming_intf_alt1,
-	(struct usb_descriptor_header *) &uvc_streaming_ep,
+	(struct usb_descriptor_header *) &uvc_fs_streaming_ep,
 	NULL,
 };
 
 static const struct usb_descriptor_header * const uvc_hs_streaming[] = {
 	(struct usb_descriptor_header *) &uvc_streaming_intf_alt1,
-	(struct usb_descriptor_header *) &uvc_streaming_ep,
+	(struct usb_descriptor_header *) &uvc_hs_streaming_ep,
+	NULL,
+};
+
+static const struct usb_descriptor_header * const uvc_ss_streaming[] = {
+	(struct usb_descriptor_header *) &uvc_streaming_intf_alt1,
+	(struct usb_descriptor_header *) &uvc_ss_streaming_ep,
+	(struct usb_descriptor_header *) &uvc_ss_streaming_comp,
 	NULL,
 };
 
@@ -215,6 +293,7 @@ uvc_function_set_alt(struct usb_function *f, unsigned interface, unsigned alt)
 	struct uvc_device *uvc = to_uvc(f);
 	struct v4l2_event v4l2_event;
 	struct uvc_event *uvc_event = (void *)&v4l2_event.u.data;
+	int ret;
 
 	INFO(f->config->cdev, "uvc_function_set_alt(%u, %u)\n", interface, alt);
 
@@ -262,7 +341,10 @@ uvc_function_set_alt(struct usb_function *f, unsigned interface, unsigned alt)
 			return 0;
 
 		if (uvc->video.ep) {
-			uvc->video.ep->desc = &uvc_streaming_ep;
+			ret = config_ep_by_speed(f->config->cdev->gadget,
+					&(uvc->func), uvc->video.ep);
+			if (ret)
+				return ret;
 			usb_ep_enable(uvc->video.ep);
 		}
 
@@ -368,9 +450,11 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 {
 	struct uvc_input_header_descriptor *uvc_streaming_header;
 	struct uvc_header_descriptor *uvc_control_header;
+	const struct uvc_descriptor_header * const *uvc_control_desc;
 	const struct uvc_descriptor_header * const *uvc_streaming_cls;
 	const struct usb_descriptor_header * const *uvc_streaming_std;
 	const struct usb_descriptor_header * const *src;
+	static struct usb_endpoint_descriptor *uvc_control_ep;
 	struct usb_descriptor_header **dst;
 	struct usb_descriptor_header **hdr;
 	unsigned int control_size;
@@ -379,10 +463,29 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	unsigned int bytes;
 	void *mem;
 
-	uvc_streaming_cls = (speed == USB_SPEED_FULL)
-			  ? uvc->desc.fs_streaming : uvc->desc.hs_streaming;
-	uvc_streaming_std = (speed == USB_SPEED_FULL)
-			  ? uvc_fs_streaming : uvc_hs_streaming;
+	switch (speed) {
+	case USB_SPEED_SUPER:
+		uvc_control_desc = uvc->desc.ss_control;
+		uvc_streaming_cls = uvc->desc.ss_streaming;
+		uvc_streaming_std = uvc_ss_streaming;
+		uvc_control_ep = &uvc_ss_control_ep;
+		break;
+
+	case USB_SPEED_HIGH:
+		uvc_control_desc = uvc->desc.fs_control;
+		uvc_streaming_cls = uvc->desc.hs_streaming;
+		uvc_streaming_std = uvc_hs_streaming;
+		uvc_control_ep = &uvc_fs_control_ep;
+		break;
+
+	case USB_SPEED_FULL:
+	default:
+		uvc_control_desc = uvc->desc.fs_control;
+		uvc_streaming_cls = uvc->desc.fs_streaming;
+		uvc_streaming_std = uvc_fs_streaming;
+		uvc_control_ep = &uvc_fs_control_ep;
+		break;
+	}
 
 	/* Descriptors layout
 	 *
@@ -400,16 +503,24 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	control_size = 0;
 	streaming_size = 0;
 	bytes = uvc_iad.bLength + uvc_control_intf.bLength
-	      + uvc_control_ep.bLength + uvc_control_cs_ep.bLength
+	      + uvc_control_ep->bLength + uvc_control_cs_ep.bLength
 	      + uvc_streaming_intf_alt0.bLength;
-	n_desc = 5;
 
-	for (src = (const struct usb_descriptor_header**)uvc->desc.control; *src; ++src) {
+	if (speed == USB_SPEED_SUPER) {
+		bytes += uvc_ss_control_comp.bLength;
+		n_desc = 6;
+	} else {
+		n_desc = 5;
+	}
+
+	for (src = (const struct usb_descriptor_header **)uvc_control_desc;
+			*src; ++src) {
 		control_size += (*src)->bLength;
 		bytes += (*src)->bLength;
 		n_desc++;
 	}
-	for (src = (const struct usb_descriptor_header**)uvc_streaming_cls; *src; ++src) {
+	for (src = (const struct usb_descriptor_header **)uvc_streaming_cls;
+			*src; ++src) {
 		streaming_size += (*src)->bLength;
 		bytes += (*src)->bLength;
 		n_desc++;
@@ -433,12 +544,15 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 
 	uvc_control_header = mem;
 	UVC_COPY_DESCRIPTORS(mem, dst,
-		(const struct usb_descriptor_header**)uvc->desc.control);
+		(const struct usb_descriptor_header **)uvc_control_desc);
 	uvc_control_header->wTotalLength = cpu_to_le16(control_size);
 	uvc_control_header->bInCollection = 1;
 	uvc_control_header->baInterfaceNr[0] = uvc->streaming_intf;
 
-	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_control_ep);
+	UVC_COPY_DESCRIPTOR(mem, dst, uvc_control_ep);
+	if (speed == USB_SPEED_SUPER)
+		UVC_COPY_DESCRIPTOR(mem, dst, &uvc_ss_control_comp);
+
 	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_control_cs_ep);
 	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_streaming_intf_alt0);
 
@@ -446,7 +560,8 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	UVC_COPY_DESCRIPTORS(mem, dst,
 		(const struct usb_descriptor_header**)uvc_streaming_cls);
 	uvc_streaming_header->wTotalLength = cpu_to_le16(streaming_size);
-	uvc_streaming_header->bEndpointAddress = uvc_streaming_ep.bEndpointAddress;
+	uvc_streaming_header->bEndpointAddress =
+		uvc_fs_streaming_ep.bEndpointAddress;
 
 	UVC_COPY_DESCRIPTORS(mem, dst, uvc_streaming_std);
 
@@ -482,6 +597,7 @@ uvc_function_unbind(struct usb_configuration *c, struct usb_function *f)
 
 	kfree(f->descriptors);
 	kfree(f->hs_descriptors);
+	kfree(f->ss_descriptors);
 
 	kfree(uvc);
 }
@@ -496,8 +612,26 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 
 	INFO(cdev, "uvc_function_bind\n");
 
+	/* sanity check the streaming endpoint module parameters */
+	if (streaming_interval < 1)
+		streaming_interval = 1;
+	if (streaming_interval > 16)
+		streaming_interval = 16;
+	if (streaming_mult > 2)
+		streaming_mult = 2;
+	if (streaming_maxburst > 15)
+		streaming_maxburst = 15;
+
+	/*
+	 * fill in the FS video streaming specific descriptors from the
+	 * module parameters
+	 */
+	uvc_fs_streaming_ep.wMaxPacketSize = streaming_maxpacket > 1023 ?
+						1023 : streaming_maxpacket;
+	uvc_fs_streaming_ep.bInterval = streaming_interval;
+
 	/* Allocate endpoints. */
-	ep = usb_ep_autoconfig(cdev->gadget, &uvc_control_ep);
+	ep = usb_ep_autoconfig(cdev->gadget, &uvc_fs_control_ep);
 	if (!ep) {
 		INFO(cdev, "Unable to allocate control EP\n");
 		goto error;
@@ -505,7 +639,7 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc->control_ep = ep;
 	ep->driver_data = uvc;
 
-	ep = usb_ep_autoconfig(cdev->gadget, &uvc_streaming_ep);
+	ep = usb_ep_autoconfig(cdev->gadget, &uvc_fs_streaming_ep);
 	if (!ep) {
 		INFO(cdev, "Unable to allocate streaming EP\n");
 		goto error;
@@ -526,9 +660,52 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc_streaming_intf_alt1.bInterfaceNumber = ret;
 	uvc->streaming_intf = ret;
 
-	/* Copy descriptors. */
+	/* sanity check the streaming endpoint module parameters */
+	if (streaming_maxpacket > 1024)
+		streaming_maxpacket = 1024;
+
+	/* Copy descriptors for FS. */
 	f->descriptors = uvc_copy_descriptors(uvc, USB_SPEED_FULL);
-	f->hs_descriptors = uvc_copy_descriptors(uvc, USB_SPEED_HIGH);
+
+	/* support high speed hardware */
+	if (gadget_is_dualspeed(cdev->gadget)) {
+		/*
+		 * Fill in the HS descriptors from the module parameters for the
+		 * Video Streaming endpoint.
+		 * NOTE: We assume that the user knows what they are doing and
+		 * won't give parameters that their UDC doesn't support.
+		 */
+		uvc_hs_streaming_ep.wMaxPacketSize = streaming_maxpacket;
+		uvc_hs_streaming_ep.wMaxPacketSize |= streaming_mult << 11;
+		uvc_hs_streaming_ep.bInterval = streaming_interval;
+		uvc_hs_streaming_ep.bEndpointAddress =
+				uvc_fs_streaming_ep.bEndpointAddress;
+
+		/* Copy descriptors. */
+		f->hs_descriptors = uvc_copy_descriptors(uvc, USB_SPEED_HIGH);
+	}
+
+	/* support super speed hardware */
+	if (gadget_is_superspeed(c->cdev->gadget)) {
+		/*
+		 * Fill in the SS descriptors from the module parameters for the
+		 * Video Streaming endpoint.
+		 * NOTE: We assume that the user knows what they are doing and
+		 * won't give parameters that their UDC doesn't support.
+		 */
+		uvc_ss_streaming_ep.wMaxPacketSize = streaming_maxpacket;
+		uvc_ss_streaming_ep.bInterval = streaming_interval;
+		uvc_ss_streaming_comp.bmAttributes = streaming_mult;
+		uvc_ss_streaming_comp.bMaxBurst = streaming_maxburst;
+		uvc_ss_streaming_comp.wBytesPerInterval =
+			streaming_maxpacket * (streaming_mult + 1) *
+			(streaming_maxburst + 1);
+		uvc_ss_streaming_ep.bEndpointAddress =
+				uvc_fs_streaming_ep.bEndpointAddress;
+
+		/* Copy descriptors. */
+		f->ss_descriptors = uvc_copy_descriptors(uvc, USB_SPEED_SUPER);
+	}
 
 	/* Preallocate control endpoint request. */
 	uvc->control_req = usb_ep_alloc_request(cdev->gadget->ep0, GFP_KERNEL);
@@ -583,9 +760,11 @@ error:
  */
 int __init
 uvc_bind_config(struct usb_configuration *c,
-		const struct uvc_descriptor_header * const *control,
+		const struct uvc_descriptor_header * const *fs_control,
+		const struct uvc_descriptor_header * const *ss_control,
 		const struct uvc_descriptor_header * const *fs_streaming,
-		const struct uvc_descriptor_header * const *hs_streaming)
+		const struct uvc_descriptor_header * const *hs_streaming,
+		const struct uvc_descriptor_header * const *ss_streaming)
 {
 	struct uvc_device *uvc;
 	int ret = 0;
@@ -603,38 +782,54 @@ uvc_bind_config(struct usb_configuration *c,
 	uvc->state = UVC_STATE_DISCONNECTED;
 
 	/* Validate the descriptors. */
-	if (control == NULL || control[0] == NULL ||
-	    control[0]->bDescriptorSubType != UVC_VC_HEADER)
+	if (fs_control == NULL || fs_control[0] == NULL ||
+		fs_control[0]->bDescriptorSubType != UVC_VC_HEADER)
 		goto error;
 
-	if (fs_streaming == NULL || fs_streaming[0] == NULL ||
-	    fs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
+	if (ss_control == NULL || ss_control[0] == NULL ||
+		ss_control[0]->bDescriptorSubType != UVC_VC_HEADER)
 		goto error;
 
-	if (hs_streaming == NULL || hs_streaming[0] == NULL ||
-	    hs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
+	if (fs_streaming == NULL || fs_streaming[0] == NULL ||
+		fs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
 		goto error;
 
-	uvc->desc.control = control;
-	uvc->desc.fs_streaming = fs_streaming;
-	uvc->desc.hs_streaming = hs_streaming;
-
-	/* Allocate string descriptor numbers. */
-	if ((ret = usb_string_id(c->cdev)) < 0)
+	if (hs_streaming == NULL || hs_streaming[0] == NULL ||
+		hs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
 		goto error;
-	uvc_en_us_strings[UVC_STRING_ASSOCIATION_IDX].id = ret;
-	uvc_iad.iFunction = ret;
 
-	if ((ret = usb_string_id(c->cdev)) < 0)
+	if (ss_streaming == NULL || ss_streaming[0] == NULL ||
+		ss_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
 		goto error;
-	uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id = ret;
-	uvc_control_intf.iInterface = ret;
 
-	if ((ret = usb_string_id(c->cdev)) < 0)
-		goto error;
-	uvc_en_us_strings[UVC_STRING_STREAMING_IDX].id = ret;
-	uvc_streaming_intf_alt0.iInterface = ret;
-	uvc_streaming_intf_alt1.iInterface = ret;
+	uvc->desc.fs_control = fs_control;
+	uvc->desc.ss_control = ss_control;
+	uvc->desc.fs_streaming = fs_streaming;
+	uvc->desc.hs_streaming = hs_streaming;
+	uvc->desc.ss_streaming = ss_streaming;
+
+	/* maybe allocate device-global string IDs, and patch descriptors */
+	if (uvc_en_us_strings[UVC_STRING_ASSOCIATION_IDX].id == 0) {
+		/* Allocate string descriptor numbers. */
+		ret = usb_string_id(c->cdev);
+		if (ret < 0)
+			goto error;
+		uvc_en_us_strings[UVC_STRING_ASSOCIATION_IDX].id = ret;
+		uvc_iad.iFunction = ret;
+
+		ret = usb_string_id(c->cdev);
+		if (ret < 0)
+			goto error;
+		uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id = ret;
+		uvc_control_intf.iInterface = ret;
+
+		ret = usb_string_id(c->cdev);
+		if (ret < 0)
+			goto error;
+		uvc_en_us_strings[UVC_STRING_STREAMING_IDX].id = ret;
+		uvc_streaming_intf_alt0.iInterface = ret;
+		uvc_streaming_intf_alt1.iInterface = ret;
+	}
 
 	/* Register the function. */
 	uvc->func.name = "uvc";
diff --git a/drivers/usb/gadget/f_uvc.h b/drivers/usb/gadget/f_uvc.h
index abf83293513..c3d258d3018 100644
--- a/drivers/usb/gadget/f_uvc.h
+++ b/drivers/usb/gadget/f_uvc.h
@@ -17,9 +17,11 @@
 #include <linux/usb/video.h>
 
 extern int uvc_bind_config(struct usb_configuration *c,
-			   const struct uvc_descriptor_header * const *control,
-			   const struct uvc_descriptor_header * const *fs_streaming,
-			   const struct uvc_descriptor_header * const *hs_streaming);
+		   const struct uvc_descriptor_header * const *fs_control,
+		   const struct uvc_descriptor_header * const *hs_control,
+		   const struct uvc_descriptor_header * const *fs_streaming,
+		   const struct uvc_descriptor_header * const *hs_streaming,
+		   const struct uvc_descriptor_header * const *ss_streaming);
 
 #endif /* _F_UVC_H_ */
 
diff --git a/drivers/usb/gadget/fsl_mxc_udc.c b/drivers/usb/gadget/fsl_mxc_udc.c
index dcbc0a2e48d..1b0f086426b 100644
--- a/drivers/usb/gadget/fsl_mxc_udc.c
+++ b/drivers/usb/gadget/fsl_mxc_udc.c
@@ -21,7 +21,8 @@
 #include <mach/hardware.h>
 
 static struct clk *mxc_ahb_clk;
-static struct clk *mxc_usb_clk;
+static struct clk *mxc_per_clk;
+static struct clk *mxc_ipg_clk;
 
 /* workaround ENGcm09152 for i.MX35 */
 #define USBPHYCTRL_OTGBASE_OFFSET	0x608
@@ -35,28 +36,31 @@ int fsl_udc_clk_init(struct platform_device *pdev)
 
 	pdata = pdev->dev.platform_data;
 
-	if (!cpu_is_mx35() && !cpu_is_mx25()) {
-		mxc_ahb_clk = clk_get(&pdev->dev, "usb_ahb");
-		if (IS_ERR(mxc_ahb_clk))
-			return PTR_ERR(mxc_ahb_clk);
+	mxc_ipg_clk = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(mxc_ipg_clk)) {
+		dev_err(&pdev->dev, "clk_get(\"ipg\") failed\n");
+		return PTR_ERR(mxc_ipg_clk);
+	}
 
-		ret = clk_enable(mxc_ahb_clk);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "clk_enable(\"usb_ahb\") failed\n");
-			goto eenahb;
-		}
+	mxc_ahb_clk = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(mxc_ahb_clk)) {
+		dev_err(&pdev->dev, "clk_get(\"ahb\") failed\n");
+		return PTR_ERR(mxc_ahb_clk);
 	}
 
-	/* make sure USB_CLK is running at 60 MHz +/- 1000 Hz */
-	mxc_usb_clk = clk_get(&pdev->dev, "usb");
-	if (IS_ERR(mxc_usb_clk)) {
-		dev_err(&pdev->dev, "clk_get(\"usb\") failed\n");
-		ret = PTR_ERR(mxc_usb_clk);
-		goto egusb;
+	mxc_per_clk = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(mxc_per_clk)) {
+		dev_err(&pdev->dev, "clk_get(\"per\") failed\n");
+		return PTR_ERR(mxc_per_clk);
 	}
 
+	clk_prepare_enable(mxc_ipg_clk);
+	clk_prepare_enable(mxc_ahb_clk);
+	clk_prepare_enable(mxc_per_clk);
+
+	/* make sure USB_CLK is running at 60 MHz +/- 1000 Hz */
 	if (!cpu_is_mx51()) {
-		freq = clk_get_rate(mxc_usb_clk);
+		freq = clk_get_rate(mxc_per_clk);
 		if (pdata->phy_mode != FSL_USB2_PHY_ULPI &&
 		    (freq < 59999000 || freq > 60001000)) {
 			dev_err(&pdev->dev, "USB_CLK=%lu, should be 60MHz\n", freq);
@@ -65,24 +69,13 @@ int fsl_udc_clk_init(struct platform_device *pdev)
 		}
 	}
 
-	ret = clk_enable(mxc_usb_clk);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "clk_enable(\"usb_clk\") failed\n");
-		goto eenusb;
-	}
-
 	return 0;
 
-eenusb:
 eclkrate:
-	clk_put(mxc_usb_clk);
-	mxc_usb_clk = NULL;
-egusb:
-	if (!cpu_is_mx35())
-		clk_disable(mxc_ahb_clk);
-eenahb:
-	if (!cpu_is_mx35())
-		clk_put(mxc_ahb_clk);
+	clk_disable_unprepare(mxc_ipg_clk);
+	clk_disable_unprepare(mxc_ahb_clk);
+	clk_disable_unprepare(mxc_per_clk);
+	mxc_per_clk = NULL;
 	return ret;
 }
 
@@ -104,20 +97,15 @@ void fsl_udc_clk_finalize(struct platform_device *pdev)
 
 	/* ULPI transceivers don't need usbpll */
 	if (pdata->phy_mode == FSL_USB2_PHY_ULPI) {
-		clk_disable(mxc_usb_clk);
-		clk_put(mxc_usb_clk);
-		mxc_usb_clk = NULL;
+		clk_disable_unprepare(mxc_per_clk);
+		mxc_per_clk = NULL;
 	}
 }
 
 void fsl_udc_clk_release(void)
 {
-	if (mxc_usb_clk) {
-		clk_disable(mxc_usb_clk);
-		clk_put(mxc_usb_clk);
-	}
-	if (!cpu_is_mx35()) {
-		clk_disable(mxc_ahb_clk);
-		clk_put(mxc_ahb_clk);
-	}
+	if (mxc_per_clk)
+		clk_disable_unprepare(mxc_per_clk);
+	clk_disable_unprepare(mxc_ahb_clk);
+	clk_disable_unprepare(mxc_ipg_clk);
 }
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index bc6f9bb9994..3def828f85e 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -24,6 +24,7 @@
 #include <linux/ioport.h>
 #include <linux/types.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/list.h>
@@ -1229,7 +1230,7 @@ static int fsl_vbus_draw(struct usb_gadget *gadget, unsigned mA)
 	struct fsl_udc *udc;
 
 	udc = container_of(gadget, struct fsl_udc, gadget);
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return usb_phy_set_power(udc->transceiver, mA);
 	return -ENOTSUPP;
 }
@@ -1983,13 +1984,13 @@ static int fsl_start(struct usb_gadget_driver *driver,
 		goto out;
 	}
 
-	if (udc_controller->transceiver) {
+	if (!IS_ERR_OR_NULL(udc_controller->transceiver)) {
 		/* Suspend the controller until OTG enable it */
 		udc_controller->stopped = 1;
 		printk(KERN_INFO "Suspend udc for OTG auto detect\n");
 
 		/* connect to bus through transceiver */
-		if (udc_controller->transceiver) {
+		if (!IS_ERR_OR_NULL(udc_controller->transceiver)) {
 			retval = otg_set_peripheral(
 					udc_controller->transceiver->otg,
 						    &udc_controller->gadget);
@@ -2030,7 +2031,7 @@ static int fsl_stop(struct usb_gadget_driver *driver)
 	if (!driver || driver != udc_controller->driver || !driver->unbind)
 		return -EINVAL;
 
-	if (udc_controller->transceiver)
+	if (!IS_ERR_OR_NULL(udc_controller->transceiver))
 		otg_set_peripheral(udc_controller->transceiver->otg, NULL);
 
 	/* stop DR, disable intr */
@@ -2455,8 +2456,8 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 
 #ifdef CONFIG_USB_OTG
 	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
-		udc_controller->transceiver = usb_get_transceiver();
-		if (!udc_controller->transceiver) {
+		udc_controller->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
+		if (IS_ERR_OR_NULL(udc_controller->transceiver)) {
 			ERR("Can't find OTG driver!\n");
 			ret = -ENODEV;
 			goto err_kfree;
@@ -2540,7 +2541,7 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 		goto err_free_irq;
 	}
 
-	if (!udc_controller->transceiver) {
+	if (IS_ERR_OR_NULL(udc_controller->transceiver)) {
 		/* initialize usb hw reg except for regs for EP,
 		 * leave usbintr reg untouched */
 		dr_controller_setup(udc_controller);
@@ -2560,11 +2561,12 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	dev_set_name(&udc_controller->gadget.dev, "gadget");
 	udc_controller->gadget.dev.release = fsl_udc_release;
 	udc_controller->gadget.dev.parent = &pdev->dev;
+	udc_controller->gadget.dev.of_node = pdev->dev.of_node;
 	ret = device_register(&udc_controller->gadget.dev);
 	if (ret < 0)
 		goto err_free_irq;
 
-	if (udc_controller->transceiver)
+	if (!IS_ERR_OR_NULL(udc_controller->transceiver))
 		udc_controller->gadget.is_otg = 1;
 
 	/* setup QH and epctrl for ep0 */
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index 3d28fb976c7..9fd7886cfa9 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -1836,7 +1836,7 @@ static int goku_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	/* init to known state, then setup irqs */
 	udc_reset(dev);
 	udc_reinit (dev);
-	if (request_irq(pdev->irq, goku_irq, IRQF_SHARED/*|IRQF_SAMPLE_RANDOM*/,
+	if (request_irq(pdev->irq, goku_irq, IRQF_SHARED,
 			driver_name, dev) != 0) {
 		DBG(dev, "request interrupt %d failed\n", pdev->irq);
 		retval = -EBUSY;
diff --git a/drivers/usb/gadget/imx_udc.c b/drivers/usb/gadget/imx_udc.c
index 54034f84f99..dc5334856af 100644
--- a/drivers/usb/gadget/imx_udc.c
+++ b/drivers/usb/gadget/imx_udc.c
@@ -1432,7 +1432,7 @@ static int __init imx_udc_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "can't get USB clock\n");
 		goto fail2;
 	}
-	clk_enable(clk);
+	clk_prepare_enable(clk);
 
 	if (clk_get_rate(clk) != 48000000) {
 		D_INI(&pdev->dev,
@@ -1496,7 +1496,7 @@ fail4:
 		free_irq(imx_usb->usbd_int[i], imx_usb);
 fail3:
 	clk_put(clk);
-	clk_disable(clk);
+	clk_disable_unprepare(clk);
 fail2:
 	iounmap(base);
 fail1:
@@ -1521,7 +1521,7 @@ static int __exit imx_udc_remove(struct platform_device *pdev)
 		free_irq(imx_usb->usbd_int[i], imx_usb);
 
 	clk_put(imx_usb->clk);
-	clk_disable(imx_usb->clk);
+	clk_disable_unprepare(imx_usb->clk);
 	iounmap(imx_usb->base);
 
 	release_mem_region(imx_usb->res->start, resource_size(imx_usb->res));
diff --git a/drivers/usb/gadget/lpc32xx_udc.c b/drivers/usb/gadget/lpc32xx_udc.c
index 2ab0388d93e..f1ec99e69cb 100644
--- a/drivers/usb/gadget/lpc32xx_udc.c
+++ b/drivers/usb/gadget/lpc32xx_udc.c
@@ -165,6 +165,7 @@ struct lpc32xx_udc {
 	int			udp_irq[4];
 	struct clk		*usb_pll_clk;
 	struct clk		*usb_slv_clk;
+	struct clk		*usb_otg_clk;
 
 	/* DMA support */
 	u32			*udca_v_base;
@@ -227,33 +228,15 @@ static inline struct lpc32xx_udc *to_udc(struct usb_gadget *g)
 #define UDCA_BUFF_SIZE (128)
 
 /* TODO: When the clock framework is introduced in LPC32xx, IO_ADDRESS will
- * be replaced with an inremap()ed pointer, see USB_OTG_CLK_CTRL()
+ * be replaced with an inremap()ed pointer
  * */
 #define USB_CTRL		IO_ADDRESS(LPC32XX_CLK_PM_BASE + 0x64)
-#define USB_CLOCK_MASK		(AHB_M_CLOCK_ON | OTG_CLOCK_ON | \
-				 DEV_CLOCK_ON | I2C_CLOCK_ON)
 
 /* USB_CTRL bit defines */
 #define USB_SLAVE_HCLK_EN	(1 << 24)
 #define USB_HOST_NEED_CLK_EN	(1 << 21)
 #define USB_DEV_NEED_CLK_EN	(1 << 22)
 
-#define USB_OTG_CLK_CTRL(udc)	((udc)->udp_baseaddr + 0xFF4)
-#define USB_OTG_CLK_STAT(udc)	((udc)->udp_baseaddr + 0xFF8)
-
-/* USB_OTG_CLK_CTRL bit defines */
-#define AHB_M_CLOCK_ON		(1 << 4)
-#define OTG_CLOCK_ON		(1 << 3)
-#define I2C_CLOCK_ON		(1 << 2)
-#define DEV_CLOCK_ON		(1 << 1)
-#define HOST_CLOCK_ON		(1 << 0)
-
-#define USB_OTG_STAT_CONTROL(udc) (udc->udp_baseaddr + 0x110)
-
-/* USB_OTG_STAT_CONTROL bit defines */
-#define TRANSPARENT_I2C_EN	(1 << 7)
-#define HOST_EN			(1 << 0)
-
 /**********************************************************************
  * USB device controller register offsets
  **********************************************************************/
@@ -677,7 +660,7 @@ static void isp1301_udc_configure(struct lpc32xx_udc *udc)
 		ISP1301_I2C_INTERRUPT_RISING, INT_VBUS_VLD);
 
 	/* Enable usb_need_clk clock after transceiver is initialized */
-	writel((readl(USB_CTRL) | (1 << 22)), USB_CTRL);
+	writel((readl(USB_CTRL) | USB_DEV_NEED_CLK_EN), USB_CTRL);
 
 	dev_info(udc->dev, "ISP1301 Vendor ID  : 0x%04x\n",
 		 i2c_smbus_read_word_data(udc->isp1301_i2c_client, 0x00));
@@ -1010,11 +993,8 @@ static void udc_dd_free(struct lpc32xx_udc *udc, struct lpc32xx_usbd_dd_gad *dd)
 /* Enables or disables most of the USB system clocks when low power mode is
  * needed. Clocks are typically started on a connection event, and disabled
  * when a cable is disconnected */
-#define OTGOFF_CLK_MASK (AHB_M_CLOCK_ON | I2C_CLOCK_ON)
 static void udc_clk_set(struct lpc32xx_udc *udc, int enable)
 {
-	int to = 1000;
-
 	if (enable != 0) {
 		if (udc->clocked)
 			return;
@@ -1028,14 +1008,7 @@ static void udc_clk_set(struct lpc32xx_udc *udc, int enable)
 		writel(readl(USB_CTRL) | USB_DEV_NEED_CLK_EN,
 			     USB_CTRL);
 
-		/* Set to enable all needed USB OTG clocks */
-		writel(USB_CLOCK_MASK, USB_OTG_CLK_CTRL(udc));
-
-		while (((readl(USB_OTG_CLK_STAT(udc)) & USB_CLOCK_MASK) !=
-			USB_CLOCK_MASK) && (to > 0))
-			to--;
-		if (!to)
-			dev_dbg(udc->dev, "Cannot enable USB OTG clocking\n");
+		clk_enable(udc->usb_otg_clk);
 	} else {
 		if (!udc->clocked)
 			return;
@@ -1047,19 +1020,11 @@ static void udc_clk_set(struct lpc32xx_udc *udc, int enable)
 		/* 48MHz PLL dpwn */
 		clk_disable(udc->usb_pll_clk);
 
-		/* Enable the USB device clock */
+		/* Disable the USB device clock */
 		writel(readl(USB_CTRL) & ~USB_DEV_NEED_CLK_EN,
 			     USB_CTRL);
 
-		/* Set to enable all needed USB OTG clocks */
-		writel(OTGOFF_CLK_MASK, USB_OTG_CLK_CTRL(udc));
-
-		while (((readl(USB_OTG_CLK_STAT(udc)) &
-			 OTGOFF_CLK_MASK) !=
-			OTGOFF_CLK_MASK) && (to > 0))
-			to--;
-		if (!to)
-			dev_dbg(udc->dev, "Cannot disable USB OTG clocking\n");
+		clk_disable(udc->usb_otg_clk);
 	}
 }
 
@@ -3041,6 +3006,7 @@ static int lpc32xx_start(struct usb_gadget_driver *driver,
 
 	udc->driver = driver;
 	udc->gadget.dev.driver = &driver->driver;
+	udc->gadget.dev.of_node = udc->dev->of_node;
 	udc->enabled = 1;
 	udc->selfpowered = 1;
 	udc->vbus = 0;
@@ -3239,6 +3205,12 @@ static int __init lpc32xx_udc_probe(struct platform_device *pdev)
 		retval = PTR_ERR(udc->usb_slv_clk);
 		goto usb_clk_get_fail;
 	}
+	udc->usb_otg_clk = clk_get(&pdev->dev, "ck_usb_otg");
+	if (IS_ERR(udc->usb_otg_clk)) {
+		dev_err(udc->dev, "failed to acquire USB otg clock\n");
+		retval = PTR_ERR(udc->usb_slv_clk);
+		goto usb_otg_clk_get_fail;
+	}
 
 	/* Setup PLL clock to 48MHz */
 	retval = clk_enable(udc->usb_pll_clk);
@@ -3262,15 +3234,12 @@ static int __init lpc32xx_udc_probe(struct platform_device *pdev)
 		goto usb_clk_enable_fail;
 	}
 
-	/* Set to enable all needed USB OTG clocks */
-	writel(USB_CLOCK_MASK, USB_OTG_CLK_CTRL(udc));
-
-	i = 1000;
-	while (((readl(USB_OTG_CLK_STAT(udc)) & USB_CLOCK_MASK) !=
-		USB_CLOCK_MASK) && (i > 0))
-		i--;
-	if (!i)
-		dev_dbg(udc->dev, "USB OTG clocks not correctly enabled\n");
+	/* Enable USB OTG clock */
+	retval = clk_enable(udc->usb_otg_clk);
+	if (retval < 0) {
+		dev_err(udc->dev, "failed to start USB otg clock\n");
+		goto usb_otg_clk_enable_fail;
+	}
 
 	/* Setup deferred workqueue data */
 	udc->poweron = udc->pullup = 0;
@@ -3390,12 +3359,16 @@ dma_alloc_fail:
 	dma_free_coherent(&pdev->dev, UDCA_BUFF_SIZE,
 			  udc->udca_v_base, udc->udca_p_base);
 i2c_fail:
+	clk_disable(udc->usb_otg_clk);
+usb_otg_clk_enable_fail:
 	clk_disable(udc->usb_slv_clk);
 usb_clk_enable_fail:
 pll_set_fail:
 	clk_disable(udc->usb_pll_clk);
 pll_enable_fail:
 	clk_put(udc->usb_slv_clk);
+usb_otg_clk_get_fail:
+	clk_put(udc->usb_otg_clk);
 usb_clk_get_fail:
 	clk_put(udc->usb_pll_clk);
 pll_get_fail:
@@ -3433,6 +3406,8 @@ static int __devexit lpc32xx_udc_remove(struct platform_device *pdev)
 
 	device_unregister(&udc->gadget.dev);
 
+	clk_disable(udc->usb_otg_clk);
+	clk_put(udc->usb_otg_clk);
 	clk_disable(udc->usb_slv_clk);
 	clk_put(udc->usb_slv_clk);
 	clk_disable(udc->usb_pll_clk);
@@ -3446,7 +3421,6 @@ static int __devexit lpc32xx_udc_remove(struct platform_device *pdev)
 #ifdef CONFIG_PM
 static int lpc32xx_udc_suspend(struct platform_device *pdev, pm_message_t mesg)
 {
-	int to = 1000;
 	struct lpc32xx_udc *udc = platform_get_drvdata(pdev);
 
 	if (udc->clocked) {
@@ -3461,15 +3435,6 @@ static int lpc32xx_udc_suspend(struct platform_device *pdev, pm_message_t mesg)
 		   on resume */
 		udc->clocked = 1;
 
-		/* Kill OTG and I2C clocks */
-		writel(0, USB_OTG_CLK_CTRL(udc));
-		while (((readl(USB_OTG_CLK_STAT(udc)) & OTGOFF_CLK_MASK) !=
-			OTGOFF_CLK_MASK) && (to > 0))
-			to--;
-		if (!to)
-			dev_dbg(udc->dev,
-				"USB OTG clocks not correctly enabled\n");
-
 		/* Kill global USB clock */
 		clk_disable(udc->usb_slv_clk);
 	}
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index 8981fbb5748..cf6bd626f3f 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -1583,12 +1583,10 @@ static int __exit m66592_remove(struct platform_device *pdev)
 	iounmap(m66592->reg);
 	free_irq(platform_get_irq(pdev, 0), m66592);
 	m66592_free_request(&m66592->ep[0].ep, m66592->ep0_req);
-#ifdef CONFIG_HAVE_CLK
 	if (m66592->pdata->on_chip) {
 		clk_disable(m66592->clk);
 		clk_put(m66592->clk);
 	}
-#endif
 	kfree(m66592);
 	return 0;
 }
@@ -1602,9 +1600,7 @@ static int __init m66592_probe(struct platform_device *pdev)
 	struct resource *res, *ires;
 	void __iomem *reg = NULL;
 	struct m66592 *m66592 = NULL;
-#ifdef CONFIG_HAVE_CLK
 	char clk_name[8];
-#endif
 	int ret = 0;
 	int i;
 
@@ -1671,7 +1667,6 @@ static int __init m66592_probe(struct platform_device *pdev)
 		goto clean_up;
 	}
 
-#ifdef CONFIG_HAVE_CLK
 	if (m66592->pdata->on_chip) {
 		snprintf(clk_name, sizeof(clk_name), "usbf%d", pdev->id);
 		m66592->clk = clk_get(&pdev->dev, clk_name);
@@ -1683,7 +1678,7 @@ static int __init m66592_probe(struct platform_device *pdev)
 		}
 		clk_enable(m66592->clk);
 	}
-#endif
+
 	INIT_LIST_HEAD(&m66592->gadget.ep_list);
 	m66592->gadget.ep0 = &m66592->ep[0].ep;
 	INIT_LIST_HEAD(&m66592->gadget.ep0->ep_list);
@@ -1731,13 +1726,11 @@ err_add_udc:
 	m66592_free_request(&m66592->ep[0].ep, m66592->ep0_req);
 
 clean_up3:
-#ifdef CONFIG_HAVE_CLK
 	if (m66592->pdata->on_chip) {
 		clk_disable(m66592->clk);
 		clk_put(m66592->clk);
 	}
 clean_up2:
-#endif
 	free_irq(ires->start, m66592);
 clean_up:
 	if (m66592) {
diff --git a/drivers/usb/gadget/m66592-udc.h b/drivers/usb/gadget/m66592-udc.h
index 88c85b4116a..16c7e14678b 100644
--- a/drivers/usb/gadget/m66592-udc.h
+++ b/drivers/usb/gadget/m66592-udc.h
@@ -13,10 +13,7 @@
 #ifndef __M66592_UDC_H__
 #define __M66592_UDC_H__
 
-#ifdef CONFIG_HAVE_CLK
 #include <linux/clk.h>
-#endif
-
 #include <linux/usb/m66592.h>
 
 #define M66592_SYSCFG		0x00
@@ -468,9 +465,7 @@ struct m66592_ep {
 struct m66592 {
 	spinlock_t		lock;
 	void __iomem		*reg;
-#ifdef CONFIG_HAVE_CLK
 	struct clk *clk;
-#endif
 	struct m66592_platdata	*pdata;
 	unsigned long		irq_trigger;
 
diff --git a/drivers/usb/gadget/mv_u3d.h b/drivers/usb/gadget/mv_u3d.h
new file mode 100644
index 00000000000..e32a787ac37
--- /dev/null
+++ b/drivers/usb/gadget/mv_u3d.h
@@ -0,0 +1,320 @@
+/*
+ * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#ifndef __MV_U3D_H
+#define __MV_U3D_H
+
+#define MV_U3D_EP_CONTEXT_ALIGNMENT	32
+#define MV_U3D_TRB_ALIGNMENT	16
+#define MV_U3D_DMA_BOUNDARY	4096
+#define MV_U3D_EP0_MAX_PKT_SIZE	512
+
+/* ep0 transfer state */
+#define MV_U3D_WAIT_FOR_SETUP		0
+#define MV_U3D_DATA_STATE_XMIT		1
+#define MV_U3D_DATA_STATE_NEED_ZLP	2
+#define MV_U3D_WAIT_FOR_OUT_STATUS	3
+#define MV_U3D_DATA_STATE_RECV		4
+#define MV_U3D_STATUS_STAGE		5
+
+#define MV_U3D_EP_MAX_LENGTH_TRANSFER	0x10000
+
+/* USB3 Interrupt Status */
+#define MV_U3D_USBINT_SETUP		0x00000001
+#define MV_U3D_USBINT_RX_COMPLETE	0x00000002
+#define MV_U3D_USBINT_TX_COMPLETE	0x00000004
+#define MV_U3D_USBINT_UNDER_RUN	0x00000008
+#define MV_U3D_USBINT_RXDESC_ERR	0x00000010
+#define MV_U3D_USBINT_TXDESC_ERR	0x00000020
+#define MV_U3D_USBINT_RX_TRB_COMPLETE	0x00000040
+#define MV_U3D_USBINT_TX_TRB_COMPLETE	0x00000080
+#define MV_U3D_USBINT_VBUS_VALID	0x00010000
+#define MV_U3D_USBINT_STORAGE_CMD_FULL	0x00020000
+#define MV_U3D_USBINT_LINK_CHG		0x01000000
+
+/* USB3 Interrupt Enable */
+#define MV_U3D_INTR_ENABLE_SETUP		0x00000001
+#define MV_U3D_INTR_ENABLE_RX_COMPLETE		0x00000002
+#define MV_U3D_INTR_ENABLE_TX_COMPLETE		0x00000004
+#define MV_U3D_INTR_ENABLE_UNDER_RUN		0x00000008
+#define MV_U3D_INTR_ENABLE_RXDESC_ERR		0x00000010
+#define MV_U3D_INTR_ENABLE_TXDESC_ERR		0x00000020
+#define MV_U3D_INTR_ENABLE_RX_TRB_COMPLETE	0x00000040
+#define MV_U3D_INTR_ENABLE_TX_TRB_COMPLETE	0x00000080
+#define MV_U3D_INTR_ENABLE_RX_BUFFER_ERR	0x00000100
+#define MV_U3D_INTR_ENABLE_VBUS_VALID		0x00010000
+#define MV_U3D_INTR_ENABLE_STORAGE_CMD_FULL	0x00020000
+#define MV_U3D_INTR_ENABLE_LINK_CHG		0x01000000
+#define MV_U3D_INTR_ENABLE_PRIME_STATUS	0x02000000
+
+/* USB3 Link Change */
+#define MV_U3D_LINK_CHANGE_LINK_UP		0x00000001
+#define MV_U3D_LINK_CHANGE_SUSPEND		0x00000002
+#define MV_U3D_LINK_CHANGE_RESUME		0x00000004
+#define MV_U3D_LINK_CHANGE_WRESET		0x00000008
+#define MV_U3D_LINK_CHANGE_HRESET		0x00000010
+#define MV_U3D_LINK_CHANGE_VBUS_INVALID	0x00000020
+#define MV_U3D_LINK_CHANGE_INACT		0x00000040
+#define MV_U3D_LINK_CHANGE_DISABLE_AFTER_U0	0x00000080
+#define MV_U3D_LINK_CHANGE_U1			0x00000100
+#define MV_U3D_LINK_CHANGE_U2			0x00000200
+#define MV_U3D_LINK_CHANGE_U3			0x00000400
+
+/* bridge setting */
+#define MV_U3D_BRIDGE_SETTING_VBUS_VALID	(1 << 16)
+
+/* Command Register Bit Masks */
+#define MV_U3D_CMD_RUN_STOP		0x00000001
+#define MV_U3D_CMD_CTRL_RESET		0x00000002
+
+/* ep control register */
+#define MV_U3D_EPXCR_EP_TYPE_CONTROL		0
+#define MV_U3D_EPXCR_EP_TYPE_ISOC		1
+#define MV_U3D_EPXCR_EP_TYPE_BULK		2
+#define MV_U3D_EPXCR_EP_TYPE_INT		3
+#define MV_U3D_EPXCR_EP_ENABLE_SHIFT		4
+#define MV_U3D_EPXCR_MAX_BURST_SIZE_SHIFT	12
+#define MV_U3D_EPXCR_MAX_PACKET_SIZE_SHIFT	16
+#define MV_U3D_USB_BULK_BURST_OUT		6
+#define MV_U3D_USB_BULK_BURST_IN		14
+
+#define MV_U3D_EPXCR_EP_FLUSH		(1 << 7)
+#define MV_U3D_EPXCR_EP_HALT		(1 << 1)
+#define MV_U3D_EPXCR_EP_INIT		(1)
+
+/* TX/RX Status Register */
+#define MV_U3D_XFERSTATUS_COMPLETE_SHIFT	24
+#define MV_U3D_COMPLETE_INVALID	0
+#define MV_U3D_COMPLETE_SUCCESS	1
+#define MV_U3D_COMPLETE_BUFF_ERR	2
+#define MV_U3D_COMPLETE_SHORT_PACKET	3
+#define MV_U3D_COMPLETE_TRB_ERR	5
+#define MV_U3D_XFERSTATUS_TRB_LENGTH_MASK	(0xFFFFFF)
+
+#define MV_U3D_USB_LINK_BYPASS_VBUS	0x8
+
+#define MV_U3D_LTSSM_PHY_INIT_DONE		0x80000000
+#define MV_U3D_LTSSM_NEVER_GO_COMPLIANCE	0x40000000
+
+#define MV_U3D_USB3_OP_REGS_OFFSET	0x100
+#define MV_U3D_USB3_PHY_OFFSET		0xB800
+
+#define DCS_ENABLE	0x1
+
+/* timeout */
+#define MV_U3D_RESET_TIMEOUT		10000
+#define MV_U3D_FLUSH_TIMEOUT		100000
+#define MV_U3D_OWN_TIMEOUT		10000
+#define LOOPS_USEC_SHIFT	4
+#define LOOPS_USEC		(1 << LOOPS_USEC_SHIFT)
+#define LOOPS(timeout)		((timeout) >> LOOPS_USEC_SHIFT)
+
+/* ep direction */
+#define MV_U3D_EP_DIR_IN		1
+#define MV_U3D_EP_DIR_OUT		0
+#define mv_u3d_ep_dir(ep)	(((ep)->ep_num == 0) ? \
+				((ep)->u3d->ep0_dir) : ((ep)->direction))
+
+/* usb capability registers */
+struct mv_u3d_cap_regs {
+	u32	rsvd[5];
+	u32	dboff;	/* doorbell register offset */
+	u32	rtsoff;	/* runtime register offset */
+	u32	vuoff;	/* vendor unique register offset */
+};
+
+/* operation registers */
+struct mv_u3d_op_regs {
+	u32	usbcmd;		/* Command register */
+	u32	rsvd1[11];
+	u32	dcbaapl;	/* Device Context Base Address low register */
+	u32	dcbaaph;	/* Device Context Base Address high register */
+	u32	rsvd2[243];
+	u32	portsc;		/* port status and control register*/
+	u32	portlinkinfo;	/* port link info register*/
+	u32	rsvd3[9917];
+	u32	doorbell;	/* doorbell register */
+};
+
+/* control enpoint enable registers */
+struct epxcr {
+	u32	epxoutcr0;	/* ep out control 0 register */
+	u32	epxoutcr1;	/* ep out control 1 register */
+	u32	epxincr0;	/* ep in control 0 register */
+	u32	epxincr1;	/* ep in control 1 register */
+};
+
+/* transfer status registers */
+struct xferstatus {
+	u32	curdeqlo;	/* current TRB pointer low */
+	u32	curdeqhi;	/* current TRB pointer high */
+	u32	statuslo;	/* transfer status low */
+	u32	statushi;	/* transfer status high */
+};
+
+/* vendor unique control registers */
+struct mv_u3d_vuc_regs {
+	u32	ctrlepenable;	/* control endpoint enable register */
+	u32	setuplock;	/* setup lock register */
+	u32	endcomplete;	/* endpoint transfer complete register */
+	u32	intrcause;	/* interrupt cause register */
+	u32	intrenable;	/* interrupt enable register */
+	u32	trbcomplete;	/* TRB complete register */
+	u32	linkchange;	/* link change register */
+	u32	rsvd1[5];
+	u32	trbunderrun;	/* TRB underrun register */
+	u32	rsvd2[43];
+	u32	bridgesetting;	/* bridge setting register */
+	u32	rsvd3[7];
+	struct xferstatus	txst[16];	/* TX status register */
+	struct xferstatus	rxst[16];	/* RX status register */
+	u32	ltssm;		/* LTSSM control register */
+	u32	pipe;		/* PIPE control register */
+	u32	linkcr0;	/* link control 0 register */
+	u32	linkcr1;	/* link control 1 register */
+	u32	rsvd6[60];
+	u32	mib0;		/* MIB0 counter register */
+	u32	usblink;	/* usb link control register */
+	u32	ltssmstate;	/* LTSSM state register */
+	u32	linkerrorcause;	/* link error cause register */
+	u32	rsvd7[60];
+	u32	devaddrtiebrkr;	/* device address and tie breaker */
+	u32	itpinfo0;	/* ITP info 0 register */
+	u32	itpinfo1;	/* ITP info 1 register */
+	u32	rsvd8[61];
+	struct epxcr	epcr[16];	/* ep control register */
+	u32	rsvd9[64];
+	u32	phyaddr;	/* PHY address register */
+	u32	phydata;	/* PHY data register */
+};
+
+/* Endpoint context structure */
+struct mv_u3d_ep_context {
+	u32	rsvd0;
+	u32	rsvd1;
+	u32	trb_addr_lo;		/* TRB address low 32 bit */
+	u32	trb_addr_hi;		/* TRB address high 32 bit */
+	u32	rsvd2;
+	u32	rsvd3;
+	struct usb_ctrlrequest setup_buffer;	/* setup data buffer */
+};
+
+/* TRB control data structure */
+struct mv_u3d_trb_ctrl {
+	u32	own:1;		/* owner of TRB */
+	u32	rsvd1:3;
+	u32	chain:1;	/* associate this TRB with the
+				next TRB on the Ring */
+	u32	ioc:1;		/* interrupt on complete */
+	u32	rsvd2:4;
+	u32	type:6;		/* TRB type */
+#define TYPE_NORMAL	1
+#define TYPE_DATA	3
+#define TYPE_LINK	6
+	u32	dir:1;		/* Working at data stage of control endpoint
+				operation. 0 is OUT and 1 is IN. */
+	u32	rsvd3:15;
+};
+
+/* TRB data structure
+ * For multiple TRB, all the TRBs' physical address should be continuous.
+ */
+struct mv_u3d_trb_hw {
+	u32	buf_addr_lo;	/* data buffer address low 32 bit */
+	u32	buf_addr_hi;	/* data buffer address high 32 bit */
+	u32	trb_len;	/* transfer length */
+	struct mv_u3d_trb_ctrl	ctrl;	/* TRB control data */
+};
+
+/* TRB structure */
+struct mv_u3d_trb {
+	struct mv_u3d_trb_hw *trb_hw;	/* point to the trb_hw structure */
+	dma_addr_t trb_dma;		/* dma address for this trb_hw */
+	struct list_head trb_list;	/* trb list */
+};
+
+/* device data structure */
+struct mv_u3d {
+	struct usb_gadget		gadget;
+	struct usb_gadget_driver	*driver;
+	spinlock_t			lock;	/* device lock */
+	struct completion		*done;
+	struct device			*dev;
+	int				irq;
+
+	/* usb controller registers */
+	struct mv_u3d_cap_regs __iomem	*cap_regs;
+	struct mv_u3d_op_regs __iomem	*op_regs;
+	struct mv_u3d_vuc_regs __iomem	*vuc_regs;
+	void __iomem			*phy_regs;
+
+	unsigned int			max_eps;
+	struct mv_u3d_ep_context	*ep_context;
+	size_t				ep_context_size;
+	dma_addr_t			ep_context_dma;
+
+	struct dma_pool			*trb_pool; /* for TRB data structure */
+	struct mv_u3d_ep		*eps;
+
+	struct mv_u3d_req		*status_req; /* ep0 status request */
+	struct usb_ctrlrequest		local_setup_buff; /* store setup data*/
+
+	unsigned int		resume_state;	/* USB state to resume */
+	unsigned int		usb_state;	/* USB current state */
+	unsigned int		ep0_state;	/* Endpoint zero state */
+	unsigned int		ep0_dir;
+
+	unsigned int		dev_addr;	/* device address */
+
+	unsigned int		errors;
+
+	unsigned		softconnect:1;
+	unsigned		vbus_active:1;	/* vbus is active or not */
+	unsigned		remote_wakeup:1; /* support remote wakeup */
+	unsigned		clock_gating:1;	/* clock gating or not */
+	unsigned		active:1;	/* udc is active or not */
+	unsigned		vbus_valid_detect:1; /* udc vbus detection */
+
+	struct mv_usb_addon_irq *vbus;
+	unsigned int		power;
+
+	struct clk		*clk;
+};
+
+/* endpoint data structure */
+struct mv_u3d_ep {
+	struct usb_ep		ep;
+	struct mv_u3d		*u3d;
+	struct list_head	queue;	/* ep request queued hardware */
+	struct list_head	req_list; /* list of ep request */
+	struct mv_u3d_ep_context	*ep_context; /* ep context */
+	u32			direction;
+	char			name[14];
+	u32			processing; /* there is ep request
+						queued on haredware */
+	spinlock_t		req_lock; /* ep lock */
+	unsigned		wedge:1;
+	unsigned		enabled:1;
+	unsigned		ep_type:2;
+	unsigned		ep_num:8;
+};
+
+/* request data structure */
+struct mv_u3d_req {
+	struct usb_request	req;
+	struct mv_u3d_ep	*ep;
+	struct list_head	queue;	/* ep requst queued on hardware */
+	struct list_head	list;	/* ep request list */
+	struct list_head	trb_list; /* trb list of a request */
+
+	struct mv_u3d_trb	*trb_head; /* point to first trb of a request */
+	unsigned		trb_count; /* TRB number in the chain */
+	unsigned		chain;	   /* TRB chain or not */
+};
+
+#endif
diff --git a/drivers/usb/gadget/mv_u3d_core.c b/drivers/usb/gadget/mv_u3d_core.c
new file mode 100644
index 00000000000..8cfd5b028db
--- /dev/null
+++ b/drivers/usb/gadget/mv_u3d_core.c
@@ -0,0 +1,2098 @@
+/*
+ * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/notifier.h>
+#include <linux/interrupt.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/pm.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/mv_usb.h>
+#include <linux/clk.h>
+#include <asm/system.h>
+#include <asm/unaligned.h>
+#include <asm/byteorder.h>
+
+#include "mv_u3d.h"
+
+#define DRIVER_DESC		"Marvell PXA USB3.0 Device Controller driver"
+
+static const char driver_name[] = "mv_u3d";
+static const char driver_desc[] = DRIVER_DESC;
+
+static void mv_u3d_nuke(struct mv_u3d_ep *ep, int status);
+static void mv_u3d_stop_activity(struct mv_u3d *u3d,
+			struct usb_gadget_driver *driver);
+
+/* for endpoint 0 operations */
+static const struct usb_endpoint_descriptor mv_u3d_ep0_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	0,
+	.bmAttributes =		USB_ENDPOINT_XFER_CONTROL,
+	.wMaxPacketSize =	MV_U3D_EP0_MAX_PKT_SIZE,
+};
+
+static void mv_u3d_ep0_reset(struct mv_u3d *u3d)
+{
+	struct mv_u3d_ep *ep;
+	u32 epxcr;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		ep = &u3d->eps[i];
+		ep->u3d = u3d;
+
+		/* ep0 ep context, ep0 in and out share the same ep context */
+		ep->ep_context = &u3d->ep_context[1];
+	}
+
+	/* reset ep state machine */
+	/* reset ep0 out */
+	epxcr = ioread32(&u3d->vuc_regs->epcr[0].epxoutcr0);
+	epxcr |= MV_U3D_EPXCR_EP_INIT;
+	iowrite32(epxcr, &u3d->vuc_regs->epcr[0].epxoutcr0);
+	udelay(5);
+	epxcr &= ~MV_U3D_EPXCR_EP_INIT;
+	iowrite32(epxcr, &u3d->vuc_regs->epcr[0].epxoutcr0);
+
+	epxcr = ((MV_U3D_EP0_MAX_PKT_SIZE
+		<< MV_U3D_EPXCR_MAX_PACKET_SIZE_SHIFT)
+		| (1 << MV_U3D_EPXCR_MAX_BURST_SIZE_SHIFT)
+		| (1 << MV_U3D_EPXCR_EP_ENABLE_SHIFT)
+		| MV_U3D_EPXCR_EP_TYPE_CONTROL);
+	iowrite32(epxcr, &u3d->vuc_regs->epcr[0].epxoutcr1);
+
+	/* reset ep0 in */
+	epxcr = ioread32(&u3d->vuc_regs->epcr[0].epxincr0);
+	epxcr |= MV_U3D_EPXCR_EP_INIT;
+	iowrite32(epxcr, &u3d->vuc_regs->epcr[0].epxincr0);
+	udelay(5);
+	epxcr &= ~MV_U3D_EPXCR_EP_INIT;
+	iowrite32(epxcr, &u3d->vuc_regs->epcr[0].epxincr0);
+
+	epxcr = ((MV_U3D_EP0_MAX_PKT_SIZE
+		<< MV_U3D_EPXCR_MAX_PACKET_SIZE_SHIFT)
+		| (1 << MV_U3D_EPXCR_MAX_BURST_SIZE_SHIFT)
+		| (1 << MV_U3D_EPXCR_EP_ENABLE_SHIFT)
+		| MV_U3D_EPXCR_EP_TYPE_CONTROL);
+	iowrite32(epxcr, &u3d->vuc_regs->epcr[0].epxincr1);
+}
+
+static void mv_u3d_ep0_stall(struct mv_u3d *u3d)
+{
+	u32 tmp;
+	dev_dbg(u3d->dev, "%s\n", __func__);
+
+	/* set TX and RX to stall */
+	tmp = ioread32(&u3d->vuc_regs->epcr[0].epxoutcr0);
+	tmp |= MV_U3D_EPXCR_EP_HALT;
+	iowrite32(tmp, &u3d->vuc_regs->epcr[0].epxoutcr0);
+
+	tmp = ioread32(&u3d->vuc_regs->epcr[0].epxincr0);
+	tmp |= MV_U3D_EPXCR_EP_HALT;
+	iowrite32(tmp, &u3d->vuc_regs->epcr[0].epxincr0);
+
+	/* update ep0 state */
+	u3d->ep0_state = MV_U3D_WAIT_FOR_SETUP;
+	u3d->ep0_dir = MV_U3D_EP_DIR_OUT;
+}
+
+static int mv_u3d_process_ep_req(struct mv_u3d *u3d, int index,
+	struct mv_u3d_req *curr_req)
+{
+	struct mv_u3d_trb	*curr_trb;
+	dma_addr_t cur_deq_lo;
+	struct mv_u3d_ep_context	*curr_ep_context;
+	int trb_complete, actual, remaining_length;
+	int direction, ep_num;
+	int retval = 0;
+	u32 tmp, status, length;
+
+	curr_ep_context = &u3d->ep_context[index];
+	direction = index % 2;
+	ep_num = index / 2;
+
+	trb_complete = 0;
+	actual = curr_req->req.length;
+
+	while (!list_empty(&curr_req->trb_list)) {
+		curr_trb = list_entry(curr_req->trb_list.next,
+					struct mv_u3d_trb, trb_list);
+		if (!curr_trb->trb_hw->ctrl.own) {
+			dev_err(u3d->dev, "%s, TRB own error!\n",
+				u3d->eps[index].name);
+			return 1;
+		}
+
+		curr_trb->trb_hw->ctrl.own = 0;
+		if (direction == MV_U3D_EP_DIR_OUT) {
+			tmp = ioread32(&u3d->vuc_regs->rxst[ep_num].statuslo);
+			cur_deq_lo =
+				ioread32(&u3d->vuc_regs->rxst[ep_num].curdeqlo);
+		} else {
+			tmp = ioread32(&u3d->vuc_regs->txst[ep_num].statuslo);
+			cur_deq_lo =
+				ioread32(&u3d->vuc_regs->txst[ep_num].curdeqlo);
+		}
+
+		status = tmp >> MV_U3D_XFERSTATUS_COMPLETE_SHIFT;
+		length = tmp & MV_U3D_XFERSTATUS_TRB_LENGTH_MASK;
+
+		if (status == MV_U3D_COMPLETE_SUCCESS ||
+			(status == MV_U3D_COMPLETE_SHORT_PACKET &&
+			direction == MV_U3D_EP_DIR_OUT)) {
+			remaining_length += length;
+			actual -= remaining_length;
+		} else {
+			dev_err(u3d->dev,
+				"complete_tr error: ep=%d %s: error = 0x%x\n",
+				index >> 1, direction ? "SEND" : "RECV",
+				status);
+			retval = -EPROTO;
+		}
+
+		list_del_init(&curr_trb->trb_list);
+	}
+	if (retval)
+		return retval;
+
+	curr_req->req.actual = actual;
+	return 0;
+}
+
+/*
+ * mv_u3d_done() - retire a request; caller blocked irqs
+ * @status : request status to be set, only works when
+ * request is still in progress.
+ */
+static
+void mv_u3d_done(struct mv_u3d_ep *ep, struct mv_u3d_req *req, int status)
+{
+	struct mv_u3d *u3d = (struct mv_u3d *)ep->u3d;
+
+	dev_dbg(u3d->dev, "mv_u3d_done: remove req->queue\n");
+	/* Removed the req from ep queue */
+	list_del_init(&req->queue);
+
+	/* req.status should be set as -EINPROGRESS in ep_queue() */
+	if (req->req.status == -EINPROGRESS)
+		req->req.status = status;
+	else
+		status = req->req.status;
+
+	/* Free trb for the request */
+	if (!req->chain)
+		dma_pool_free(u3d->trb_pool,
+			req->trb_head->trb_hw, req->trb_head->trb_dma);
+	else {
+		dma_unmap_single(ep->u3d->gadget.dev.parent,
+			(dma_addr_t)req->trb_head->trb_dma,
+			req->trb_count * sizeof(struct mv_u3d_trb_hw),
+			DMA_BIDIRECTIONAL);
+		kfree(req->trb_head->trb_hw);
+	}
+	kfree(req->trb_head);
+
+	usb_gadget_unmap_request(&u3d->gadget, &req->req, mv_u3d_ep_dir(ep));
+
+	if (status && (status != -ESHUTDOWN)) {
+		dev_dbg(u3d->dev, "complete %s req %p stat %d len %u/%u",
+			ep->ep.name, &req->req, status,
+			req->req.actual, req->req.length);
+	}
+
+	spin_unlock(&ep->u3d->lock);
+	/*
+	 * complete() is from gadget layer,
+	 * eg fsg->bulk_in_complete()
+	 */
+	if (req->req.complete)
+		req->req.complete(&ep->ep, &req->req);
+
+	spin_lock(&ep->u3d->lock);
+}
+
+static int mv_u3d_queue_trb(struct mv_u3d_ep *ep, struct mv_u3d_req *req)
+{
+	u32 tmp, direction;
+	struct mv_u3d *u3d;
+	struct mv_u3d_ep_context *ep_context;
+	int retval = 0;
+
+	u3d = ep->u3d;
+	direction = mv_u3d_ep_dir(ep);
+
+	/* ep0 in and out share the same ep context slot 1*/
+	if (ep->ep_num == 0)
+		ep_context = &(u3d->ep_context[1]);
+	else
+		ep_context = &(u3d->ep_context[ep->ep_num * 2 + direction]);
+
+	/* check if the pipe is empty or not */
+	if (!list_empty(&ep->queue)) {
+		dev_err(u3d->dev, "add trb to non-empty queue!\n");
+		retval = -ENOMEM;
+		WARN_ON(1);
+	} else {
+		ep_context->rsvd0 = cpu_to_le32(1);
+		ep_context->rsvd1 = 0;
+
+		/* Configure the trb address and set the DCS bit.
+		 * Both DCS bit and own bit in trb should be set.
+		 */
+		ep_context->trb_addr_lo =
+			cpu_to_le32(req->trb_head->trb_dma | DCS_ENABLE);
+		ep_context->trb_addr_hi = 0;
+
+		/* Ensure that updates to the EP Context will
+		 * occure before Ring Bell.
+		 */
+		wmb();
+
+		/* ring bell the ep */
+		if (ep->ep_num == 0)
+			tmp = 0x1;
+		else
+			tmp = ep->ep_num * 2
+				+ ((direction == MV_U3D_EP_DIR_OUT) ? 0 : 1);
+
+		iowrite32(tmp, &u3d->op_regs->doorbell);
+	}
+	return retval;
+}
+
+static struct mv_u3d_trb *mv_u3d_build_trb_one(struct mv_u3d_req *req,
+				unsigned *length, dma_addr_t *dma)
+{
+	u32 temp;
+	unsigned int direction;
+	struct mv_u3d_trb *trb;
+	struct mv_u3d_trb_hw *trb_hw;
+	struct mv_u3d *u3d;
+
+	/* how big will this transfer be? */
+	*length = req->req.length - req->req.actual;
+	BUG_ON(*length > (unsigned)MV_U3D_EP_MAX_LENGTH_TRANSFER);
+
+	u3d = req->ep->u3d;
+
+	trb = kzalloc(sizeof(*trb), GFP_ATOMIC);
+	if (!trb) {
+		dev_err(u3d->dev, "%s, trb alloc fail\n", __func__);
+		return NULL;
+	}
+
+	/*
+	 * Be careful that no _GFP_HIGHMEM is set,
+	 * or we can not use dma_to_virt
+	 * cannot use GFP_KERNEL in spin lock
+	 */
+	trb_hw = dma_pool_alloc(u3d->trb_pool, GFP_ATOMIC, dma);
+	if (!trb_hw) {
+		dev_err(u3d->dev,
+			"%s, dma_pool_alloc fail\n", __func__);
+		return NULL;
+	}
+	trb->trb_dma = *dma;
+	trb->trb_hw = trb_hw;
+
+	/* initialize buffer page pointers */
+	temp = (u32)(req->req.dma + req->req.actual);
+
+	trb_hw->buf_addr_lo = cpu_to_le32(temp);
+	trb_hw->buf_addr_hi = 0;
+	trb_hw->trb_len = cpu_to_le32(*length);
+	trb_hw->ctrl.own = 1;
+
+	if (req->ep->ep_num == 0)
+		trb_hw->ctrl.type = TYPE_DATA;
+	else
+		trb_hw->ctrl.type = TYPE_NORMAL;
+
+	req->req.actual += *length;
+
+	direction = mv_u3d_ep_dir(req->ep);
+	if (direction == MV_U3D_EP_DIR_IN)
+		trb_hw->ctrl.dir = 1;
+	else
+		trb_hw->ctrl.dir = 0;
+
+	/* Enable interrupt for the last trb of a request */
+	if (!req->req.no_interrupt)
+		trb_hw->ctrl.ioc = 1;
+
+	trb_hw->ctrl.chain = 0;
+
+	wmb();
+	return trb;
+}
+
+static int mv_u3d_build_trb_chain(struct mv_u3d_req *req, unsigned *length,
+		struct mv_u3d_trb *trb, int *is_last)
+{
+	u32 temp;
+	unsigned int direction;
+	struct mv_u3d *u3d;
+
+	/* how big will this transfer be? */
+	*length = min(req->req.length - req->req.actual,
+			(unsigned)MV_U3D_EP_MAX_LENGTH_TRANSFER);
+
+	u3d = req->ep->u3d;
+
+	trb->trb_dma = 0;
+
+	/* initialize buffer page pointers */
+	temp = (u32)(req->req.dma + req->req.actual);
+
+	trb->trb_hw->buf_addr_lo = cpu_to_le32(temp);
+	trb->trb_hw->buf_addr_hi = 0;
+	trb->trb_hw->trb_len = cpu_to_le32(*length);
+	trb->trb_hw->ctrl.own = 1;
+
+	if (req->ep->ep_num == 0)
+		trb->trb_hw->ctrl.type = TYPE_DATA;
+	else
+		trb->trb_hw->ctrl.type = TYPE_NORMAL;
+
+	req->req.actual += *length;
+
+	direction = mv_u3d_ep_dir(req->ep);
+	if (direction == MV_U3D_EP_DIR_IN)
+		trb->trb_hw->ctrl.dir = 1;
+	else
+		trb->trb_hw->ctrl.dir = 0;
+
+	/* zlp is needed if req->req.zero is set */
+	if (req->req.zero) {
+		if (*length == 0 || (*length % req->ep->ep.maxpacket) != 0)
+			*is_last = 1;
+		else
+			*is_last = 0;
+	} else if (req->req.length == req->req.actual)
+		*is_last = 1;
+	else
+		*is_last = 0;
+
+	/* Enable interrupt for the last trb of a request */
+	if (*is_last && !req->req.no_interrupt)
+		trb->trb_hw->ctrl.ioc = 1;
+
+	if (*is_last)
+		trb->trb_hw->ctrl.chain = 0;
+	else {
+		trb->trb_hw->ctrl.chain = 1;
+		dev_dbg(u3d->dev, "chain trb\n");
+	}
+
+	wmb();
+
+	return 0;
+}
+
+/* generate TRB linked list for a request
+ * usb controller only supports continous trb chain,
+ * that trb structure physical address should be continous.
+ */
+static int mv_u3d_req_to_trb(struct mv_u3d_req *req)
+{
+	unsigned count;
+	int is_last;
+	struct mv_u3d_trb *trb;
+	struct mv_u3d_trb_hw *trb_hw;
+	struct mv_u3d *u3d;
+	dma_addr_t dma;
+	unsigned length;
+	unsigned trb_num;
+
+	u3d = req->ep->u3d;
+
+	INIT_LIST_HEAD(&req->trb_list);
+
+	length = req->req.length - req->req.actual;
+	/* normally the request transfer length is less than 16KB.
+	 * we use buil_trb_one() to optimize it.
+	 */
+	if (length <= (unsigned)MV_U3D_EP_MAX_LENGTH_TRANSFER) {
+		trb = mv_u3d_build_trb_one(req, &count, &dma);
+		list_add_tail(&trb->trb_list, &req->trb_list);
+		req->trb_head = trb;
+		req->trb_count = 1;
+		req->chain = 0;
+	} else {
+		trb_num = length / MV_U3D_EP_MAX_LENGTH_TRANSFER;
+		if (length % MV_U3D_EP_MAX_LENGTH_TRANSFER)
+			trb_num++;
+
+		trb = kcalloc(trb_num, sizeof(*trb), GFP_ATOMIC);
+		if (!trb) {
+			dev_err(u3d->dev,
+					"%s, trb alloc fail\n", __func__);
+			return -ENOMEM;
+		}
+
+		trb_hw = kcalloc(trb_num, sizeof(*trb_hw), GFP_ATOMIC);
+		if (!trb_hw) {
+			dev_err(u3d->dev,
+					"%s, trb_hw alloc fail\n", __func__);
+			return -ENOMEM;
+		}
+
+		do {
+			trb->trb_hw = trb_hw;
+			if (mv_u3d_build_trb_chain(req, &count,
+						trb, &is_last)) {
+				dev_err(u3d->dev,
+					"%s, mv_u3d_build_trb_chain fail\n",
+					__func__);
+				return -EIO;
+			}
+
+			list_add_tail(&trb->trb_list, &req->trb_list);
+			req->trb_count++;
+			trb++;
+			trb_hw++;
+		} while (!is_last);
+
+		req->trb_head = list_entry(req->trb_list.next,
+					struct mv_u3d_trb, trb_list);
+		req->trb_head->trb_dma = dma_map_single(u3d->gadget.dev.parent,
+					req->trb_head->trb_hw,
+					trb_num * sizeof(*trb_hw),
+					DMA_BIDIRECTIONAL);
+
+		req->chain = 1;
+	}
+
+	return 0;
+}
+
+static int
+mv_u3d_start_queue(struct mv_u3d_ep *ep)
+{
+	struct mv_u3d *u3d = ep->u3d;
+	struct mv_u3d_req *req;
+	int ret;
+
+	if (!list_empty(&ep->req_list) && !ep->processing)
+		req = list_entry(ep->req_list.next, struct mv_u3d_req, list);
+	else
+		return 0;
+
+	ep->processing = 1;
+
+	/* set up dma mapping */
+	ret = usb_gadget_map_request(&u3d->gadget, &req->req,
+					mv_u3d_ep_dir(ep));
+	if (ret)
+		return ret;
+
+	req->req.status = -EINPROGRESS;
+	req->req.actual = 0;
+	req->trb_count = 0;
+
+	/* build trbs and push them to device queue */
+	if (!mv_u3d_req_to_trb(req)) {
+		ret = mv_u3d_queue_trb(ep, req);
+		if (ret) {
+			ep->processing = 0;
+			return ret;
+		}
+	} else {
+		ep->processing = 0;
+		dev_err(u3d->dev, "%s, mv_u3d_req_to_trb fail\n", __func__);
+		return -ENOMEM;
+	}
+
+	/* irq handler advances the queue */
+	if (req)
+		list_add_tail(&req->queue, &ep->queue);
+
+	return 0;
+}
+
+static int mv_u3d_ep_enable(struct usb_ep *_ep,
+		const struct usb_endpoint_descriptor *desc)
+{
+	struct mv_u3d *u3d;
+	struct mv_u3d_ep *ep;
+	struct mv_u3d_ep_context *ep_context;
+	u16 max = 0;
+	unsigned maxburst = 0;
+	u32 epxcr, direction;
+
+	if (!_ep || !desc || desc->bDescriptorType != USB_DT_ENDPOINT)
+		return -EINVAL;
+
+	ep = container_of(_ep, struct mv_u3d_ep, ep);
+	u3d = ep->u3d;
+
+	if (!u3d->driver || u3d->gadget.speed == USB_SPEED_UNKNOWN)
+		return -ESHUTDOWN;
+
+	direction = mv_u3d_ep_dir(ep);
+	max = le16_to_cpu(desc->wMaxPacketSize);
+
+	if (!_ep->maxburst)
+		_ep->maxburst = 1;
+	maxburst = _ep->maxburst;
+
+	/* Get the endpoint context address */
+	ep_context = (struct mv_u3d_ep_context *)ep->ep_context;
+
+	/* Set the max burst size */
+	switch (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) {
+	case USB_ENDPOINT_XFER_BULK:
+		if (maxburst > 16) {
+			dev_dbg(u3d->dev,
+				"max burst should not be greater "
+				"than 16 on bulk ep\n");
+			maxburst = 1;
+			_ep->maxburst = maxburst;
+		}
+		dev_dbg(u3d->dev,
+			"maxburst: %d on bulk %s\n", maxburst, ep->name);
+		break;
+	case USB_ENDPOINT_XFER_CONTROL:
+		/* control transfer only supports maxburst as one */
+		maxburst = 1;
+		_ep->maxburst = maxburst;
+		break;
+	case USB_ENDPOINT_XFER_INT:
+		if (maxburst != 1) {
+			dev_dbg(u3d->dev,
+				"max burst should be 1 on int ep "
+				"if transfer size is not 1024\n");
+			maxburst = 1;
+			_ep->maxburst = maxburst;
+		}
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		if (maxburst != 1) {
+			dev_dbg(u3d->dev,
+				"max burst should be 1 on isoc ep "
+				"if transfer size is not 1024\n");
+			maxburst = 1;
+			_ep->maxburst = maxburst;
+		}
+		break;
+	default:
+		goto en_done;
+	}
+
+	ep->ep.maxpacket = max;
+	ep->ep.desc = desc;
+	ep->enabled = 1;
+
+	/* Enable the endpoint for Rx or Tx and set the endpoint type */
+	if (direction == MV_U3D_EP_DIR_OUT) {
+		epxcr = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+		epxcr |= MV_U3D_EPXCR_EP_INIT;
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+		udelay(5);
+		epxcr &= ~MV_U3D_EPXCR_EP_INIT;
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+
+		epxcr = ((max << MV_U3D_EPXCR_MAX_PACKET_SIZE_SHIFT)
+		      | ((maxburst - 1) << MV_U3D_EPXCR_MAX_BURST_SIZE_SHIFT)
+		      | (1 << MV_U3D_EPXCR_EP_ENABLE_SHIFT)
+		      | (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK));
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxoutcr1);
+	} else {
+		epxcr = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+		epxcr |= MV_U3D_EPXCR_EP_INIT;
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+		udelay(5);
+		epxcr &= ~MV_U3D_EPXCR_EP_INIT;
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+
+		epxcr = ((max << MV_U3D_EPXCR_MAX_PACKET_SIZE_SHIFT)
+		      | ((maxburst - 1) << MV_U3D_EPXCR_MAX_BURST_SIZE_SHIFT)
+		      | (1 << MV_U3D_EPXCR_EP_ENABLE_SHIFT)
+		      | (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK));
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxincr1);
+	}
+
+	return 0;
+en_done:
+	return -EINVAL;
+}
+
+static int  mv_u3d_ep_disable(struct usb_ep *_ep)
+{
+	struct mv_u3d *u3d;
+	struct mv_u3d_ep *ep;
+	struct mv_u3d_ep_context *ep_context;
+	u32 epxcr, direction;
+
+	if (!_ep)
+		return -EINVAL;
+
+	ep = container_of(_ep, struct mv_u3d_ep, ep);
+	if (!ep->ep.desc)
+		return -EINVAL;
+
+	u3d = ep->u3d;
+
+	/* Get the endpoint context address */
+	ep_context = ep->ep_context;
+
+	direction = mv_u3d_ep_dir(ep);
+
+	/* nuke all pending requests (does flush) */
+	mv_u3d_nuke(ep, -ESHUTDOWN);
+
+	/* Disable the endpoint for Rx or Tx and reset the endpoint type */
+	if (direction == MV_U3D_EP_DIR_OUT) {
+		epxcr = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxoutcr1);
+		epxcr &= ~((1 << MV_U3D_EPXCR_EP_ENABLE_SHIFT)
+		      | USB_ENDPOINT_XFERTYPE_MASK);
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxoutcr1);
+	} else {
+		epxcr = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxincr1);
+		epxcr &= ~((1 << MV_U3D_EPXCR_EP_ENABLE_SHIFT)
+		      | USB_ENDPOINT_XFERTYPE_MASK);
+		iowrite32(epxcr, &u3d->vuc_regs->epcr[ep->ep_num].epxincr1);
+	}
+
+	ep->enabled = 0;
+
+	ep->ep.desc = NULL;
+	return 0;
+}
+
+static struct usb_request *
+mv_u3d_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
+{
+	struct mv_u3d_req *req = NULL;
+
+	req = kzalloc(sizeof *req, gfp_flags);
+	if (!req)
+		return NULL;
+
+	INIT_LIST_HEAD(&req->queue);
+
+	return &req->req;
+}
+
+static void mv_u3d_free_request(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct mv_u3d_req *req = container_of(_req, struct mv_u3d_req, req);
+
+	kfree(req);
+}
+
+static void mv_u3d_ep_fifo_flush(struct usb_ep *_ep)
+{
+	struct mv_u3d *u3d;
+	u32 direction;
+	struct mv_u3d_ep *ep = container_of(_ep, struct mv_u3d_ep, ep);
+	unsigned int loops;
+	u32 tmp;
+
+	/* if endpoint is not enabled, cannot flush endpoint */
+	if (!ep->enabled)
+		return;
+
+	u3d = ep->u3d;
+	direction = mv_u3d_ep_dir(ep);
+
+	/* ep0 need clear bit after flushing fifo. */
+	if (!ep->ep_num) {
+		if (direction == MV_U3D_EP_DIR_OUT) {
+			tmp = ioread32(&u3d->vuc_regs->epcr[0].epxoutcr0);
+			tmp |= MV_U3D_EPXCR_EP_FLUSH;
+			iowrite32(tmp, &u3d->vuc_regs->epcr[0].epxoutcr0);
+			udelay(10);
+			tmp &= ~MV_U3D_EPXCR_EP_FLUSH;
+			iowrite32(tmp, &u3d->vuc_regs->epcr[0].epxoutcr0);
+		} else {
+			tmp = ioread32(&u3d->vuc_regs->epcr[0].epxincr0);
+			tmp |= MV_U3D_EPXCR_EP_FLUSH;
+			iowrite32(tmp, &u3d->vuc_regs->epcr[0].epxincr0);
+			udelay(10);
+			tmp &= ~MV_U3D_EPXCR_EP_FLUSH;
+			iowrite32(tmp, &u3d->vuc_regs->epcr[0].epxincr0);
+		}
+		return;
+	}
+
+	if (direction == MV_U3D_EP_DIR_OUT) {
+		tmp = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+		tmp |= MV_U3D_EPXCR_EP_FLUSH;
+		iowrite32(tmp, &u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+
+		/* Wait until flushing completed */
+		loops = LOOPS(MV_U3D_FLUSH_TIMEOUT);
+		while (ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0) &
+			MV_U3D_EPXCR_EP_FLUSH) {
+			/*
+			 * EP_FLUSH bit should be cleared to indicate this
+			 * operation is complete
+			 */
+			if (loops == 0) {
+				dev_dbg(u3d->dev,
+				    "EP FLUSH TIMEOUT for ep%d%s\n", ep->ep_num,
+				    direction ? "in" : "out");
+				return;
+			}
+			loops--;
+			udelay(LOOPS_USEC);
+		}
+	} else {	/* EP_DIR_IN */
+		tmp = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+		tmp |= MV_U3D_EPXCR_EP_FLUSH;
+		iowrite32(tmp, &u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+
+		/* Wait until flushing completed */
+		loops = LOOPS(MV_U3D_FLUSH_TIMEOUT);
+		while (ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxincr0) &
+			MV_U3D_EPXCR_EP_FLUSH) {
+			/*
+			* EP_FLUSH bit should be cleared to indicate this
+			* operation is complete
+			*/
+			if (loops == 0) {
+				dev_dbg(u3d->dev,
+				    "EP FLUSH TIMEOUT for ep%d%s\n", ep->ep_num,
+				    direction ? "in" : "out");
+				return;
+			}
+			loops--;
+			udelay(LOOPS_USEC);
+		}
+	}
+}
+
+/* queues (submits) an I/O request to an endpoint */
+static int
+mv_u3d_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
+{
+	struct mv_u3d_ep *ep;
+	struct mv_u3d_req *req;
+	struct mv_u3d *u3d;
+	unsigned long flags;
+	int is_first_req = 0;
+
+	if (unlikely(!_ep || !_req))
+		return -EINVAL;
+
+	ep = container_of(_ep, struct mv_u3d_ep, ep);
+	u3d = ep->u3d;
+
+	req = container_of(_req, struct mv_u3d_req, req);
+
+	if (!ep->ep_num
+		&& u3d->ep0_state == MV_U3D_STATUS_STAGE
+		&& !_req->length) {
+		dev_dbg(u3d->dev, "ep0 status stage\n");
+		u3d->ep0_state = MV_U3D_WAIT_FOR_SETUP;
+		return 0;
+	}
+
+	dev_dbg(u3d->dev, "%s: %s, req: 0x%x\n",
+			__func__, _ep->name, (u32)req);
+
+	/* catch various bogus parameters */
+	if (!req->req.complete || !req->req.buf
+			|| !list_empty(&req->queue)) {
+		dev_err(u3d->dev,
+			"%s, bad params, _req: 0x%x,"
+			"req->req.complete: 0x%x, req->req.buf: 0x%x,"
+			"list_empty: 0x%x\n",
+			__func__, (u32)_req,
+			(u32)req->req.complete, (u32)req->req.buf,
+			(u32)list_empty(&req->queue));
+		return -EINVAL;
+	}
+	if (unlikely(!ep->ep.desc)) {
+		dev_err(u3d->dev, "%s, bad ep\n", __func__);
+		return -EINVAL;
+	}
+	if (ep->ep.desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+		if (req->req.length > ep->ep.maxpacket)
+			return -EMSGSIZE;
+	}
+
+	if (!u3d->driver || u3d->gadget.speed == USB_SPEED_UNKNOWN) {
+		dev_err(u3d->dev,
+			"bad params of driver/speed\n");
+		return -ESHUTDOWN;
+	}
+
+	req->ep = ep;
+
+	/* Software list handles usb request. */
+	spin_lock_irqsave(&ep->req_lock, flags);
+	is_first_req = list_empty(&ep->req_list);
+	list_add_tail(&req->list, &ep->req_list);
+	spin_unlock_irqrestore(&ep->req_lock, flags);
+	if (!is_first_req) {
+		dev_dbg(u3d->dev, "list is not empty\n");
+		return 0;
+	}
+
+	dev_dbg(u3d->dev, "call mv_u3d_start_queue from usb_ep_queue\n");
+	spin_lock_irqsave(&u3d->lock, flags);
+	mv_u3d_start_queue(ep);
+	spin_unlock_irqrestore(&u3d->lock, flags);
+	return 0;
+}
+
+/* dequeues (cancels, unlinks) an I/O request from an endpoint */
+static int mv_u3d_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct mv_u3d_ep *ep;
+	struct mv_u3d_req *req;
+	struct mv_u3d *u3d;
+	struct mv_u3d_ep_context *ep_context;
+	struct mv_u3d_req *next_req;
+
+	unsigned long flags;
+	int ret = 0;
+
+	if (!_ep || !_req)
+		return -EINVAL;
+
+	ep = container_of(_ep, struct mv_u3d_ep, ep);
+	u3d = ep->u3d;
+
+	spin_lock_irqsave(&ep->u3d->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) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* The request is in progress, or completed but not dequeued */
+	if (ep->queue.next == &req->queue) {
+		_req->status = -ECONNRESET;
+		mv_u3d_ep_fifo_flush(_ep);
+
+		/* The request isn't the last request in this ep queue */
+		if (req->queue.next != &ep->queue) {
+			dev_dbg(u3d->dev,
+				"it is the last request in this ep queue\n");
+			ep_context = ep->ep_context;
+			next_req = list_entry(req->queue.next,
+					struct mv_u3d_req, queue);
+
+			/* Point first TRB of next request to the EP context. */
+			iowrite32((u32) next_req->trb_head,
+					&ep_context->trb_addr_lo);
+		} else {
+			struct mv_u3d_ep_context *ep_context;
+			ep_context = ep->ep_context;
+			ep_context->trb_addr_lo = 0;
+			ep_context->trb_addr_hi = 0;
+		}
+
+	} else
+		WARN_ON(1);
+
+	mv_u3d_done(ep, req, -ECONNRESET);
+
+	/* remove the req from the ep req list */
+	if (!list_empty(&ep->req_list)) {
+		struct mv_u3d_req *curr_req;
+		curr_req = list_entry(ep->req_list.next,
+					struct mv_u3d_req, list);
+		if (curr_req == req) {
+			list_del_init(&req->list);
+			ep->processing = 0;
+		}
+	}
+
+out:
+	spin_unlock_irqrestore(&ep->u3d->lock, flags);
+	return ret;
+}
+
+static void
+mv_u3d_ep_set_stall(struct mv_u3d *u3d, u8 ep_num, u8 direction, int stall)
+{
+	u32 tmp;
+	struct mv_u3d_ep *ep = u3d->eps;
+
+	dev_dbg(u3d->dev, "%s\n", __func__);
+	if (direction == MV_U3D_EP_DIR_OUT) {
+		tmp = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+		if (stall)
+			tmp |= MV_U3D_EPXCR_EP_HALT;
+		else
+			tmp &= ~MV_U3D_EPXCR_EP_HALT;
+		iowrite32(tmp, &u3d->vuc_regs->epcr[ep->ep_num].epxoutcr0);
+	} else {
+		tmp = ioread32(&u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+		if (stall)
+			tmp |= MV_U3D_EPXCR_EP_HALT;
+		else
+			tmp &= ~MV_U3D_EPXCR_EP_HALT;
+		iowrite32(tmp, &u3d->vuc_regs->epcr[ep->ep_num].epxincr0);
+	}
+}
+
+static int mv_u3d_ep_set_halt_wedge(struct usb_ep *_ep, int halt, int wedge)
+{
+	struct mv_u3d_ep *ep;
+	unsigned long flags = 0;
+	int status = 0;
+	struct mv_u3d *u3d;
+
+	ep = container_of(_ep, struct mv_u3d_ep, ep);
+	u3d = ep->u3d;
+	if (!ep->ep.desc) {
+		status = -EINVAL;
+		goto out;
+	}
+
+	if (ep->ep.desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+		status = -EOPNOTSUPP;
+		goto out;
+	}
+
+	/*
+	 * Attempt to halt IN ep will fail if any transfer requests
+	 * are still queue
+	 */
+	if (halt && (mv_u3d_ep_dir(ep) == MV_U3D_EP_DIR_IN)
+			&& !list_empty(&ep->queue)) {
+		status = -EAGAIN;
+		goto out;
+	}
+
+	spin_lock_irqsave(&ep->u3d->lock, flags);
+	mv_u3d_ep_set_stall(u3d, ep->ep_num, mv_u3d_ep_dir(ep), halt);
+	if (halt && wedge)
+		ep->wedge = 1;
+	else if (!halt)
+		ep->wedge = 0;
+	spin_unlock_irqrestore(&ep->u3d->lock, flags);
+
+	if (ep->ep_num == 0)
+		u3d->ep0_dir = MV_U3D_EP_DIR_OUT;
+out:
+	return status;
+}
+
+static int mv_u3d_ep_set_halt(struct usb_ep *_ep, int halt)
+{
+	return mv_u3d_ep_set_halt_wedge(_ep, halt, 0);
+}
+
+static int mv_u3d_ep_set_wedge(struct usb_ep *_ep)
+{
+	return mv_u3d_ep_set_halt_wedge(_ep, 1, 1);
+}
+
+static struct usb_ep_ops mv_u3d_ep_ops = {
+	.enable		= mv_u3d_ep_enable,
+	.disable	= mv_u3d_ep_disable,
+
+	.alloc_request	= mv_u3d_alloc_request,
+	.free_request	= mv_u3d_free_request,
+
+	.queue		= mv_u3d_ep_queue,
+	.dequeue	= mv_u3d_ep_dequeue,
+
+	.set_wedge	= mv_u3d_ep_set_wedge,
+	.set_halt	= mv_u3d_ep_set_halt,
+	.fifo_flush	= mv_u3d_ep_fifo_flush,
+};
+
+static void mv_u3d_controller_stop(struct mv_u3d *u3d)
+{
+	u32 tmp;
+
+	if (!u3d->clock_gating && u3d->vbus_valid_detect)
+		iowrite32(MV_U3D_INTR_ENABLE_VBUS_VALID,
+				&u3d->vuc_regs->intrenable);
+	else
+		iowrite32(0, &u3d->vuc_regs->intrenable);
+	iowrite32(~0x0, &u3d->vuc_regs->endcomplete);
+	iowrite32(~0x0, &u3d->vuc_regs->trbunderrun);
+	iowrite32(~0x0, &u3d->vuc_regs->trbcomplete);
+	iowrite32(~0x0, &u3d->vuc_regs->linkchange);
+	iowrite32(0x1, &u3d->vuc_regs->setuplock);
+
+	/* Reset the RUN bit in the command register to stop USB */
+	tmp = ioread32(&u3d->op_regs->usbcmd);
+	tmp &= ~MV_U3D_CMD_RUN_STOP;
+	iowrite32(tmp, &u3d->op_regs->usbcmd);
+	dev_dbg(u3d->dev, "after u3d_stop, USBCMD 0x%x\n",
+		ioread32(&u3d->op_regs->usbcmd));
+}
+
+static void mv_u3d_controller_start(struct mv_u3d *u3d)
+{
+	u32 usbintr;
+	u32 temp;
+
+	/* enable link LTSSM state machine */
+	temp = ioread32(&u3d->vuc_regs->ltssm);
+	temp |= MV_U3D_LTSSM_PHY_INIT_DONE;
+	iowrite32(temp, &u3d->vuc_regs->ltssm);
+
+	/* Enable interrupts */
+	usbintr = MV_U3D_INTR_ENABLE_LINK_CHG | MV_U3D_INTR_ENABLE_TXDESC_ERR |
+		MV_U3D_INTR_ENABLE_RXDESC_ERR | MV_U3D_INTR_ENABLE_TX_COMPLETE |
+		MV_U3D_INTR_ENABLE_RX_COMPLETE | MV_U3D_INTR_ENABLE_SETUP |
+		(u3d->vbus_valid_detect ? MV_U3D_INTR_ENABLE_VBUS_VALID : 0);
+	iowrite32(usbintr, &u3d->vuc_regs->intrenable);
+
+	/* Enable ctrl ep */
+	iowrite32(0x1, &u3d->vuc_regs->ctrlepenable);
+
+	/* Set the Run bit in the command register */
+	iowrite32(MV_U3D_CMD_RUN_STOP, &u3d->op_regs->usbcmd);
+	dev_dbg(u3d->dev, "after u3d_start, USBCMD 0x%x\n",
+		ioread32(&u3d->op_regs->usbcmd));
+}
+
+static int mv_u3d_controller_reset(struct mv_u3d *u3d)
+{
+	unsigned int loops;
+	u32 tmp;
+
+	/* Stop the controller */
+	tmp = ioread32(&u3d->op_regs->usbcmd);
+	tmp &= ~MV_U3D_CMD_RUN_STOP;
+	iowrite32(tmp, &u3d->op_regs->usbcmd);
+
+	/* Reset the controller to get default values */
+	iowrite32(MV_U3D_CMD_CTRL_RESET, &u3d->op_regs->usbcmd);
+
+	/* wait for reset to complete */
+	loops = LOOPS(MV_U3D_RESET_TIMEOUT);
+	while (ioread32(&u3d->op_regs->usbcmd) & MV_U3D_CMD_CTRL_RESET) {
+		if (loops == 0) {
+			dev_err(u3d->dev,
+				"Wait for RESET completed TIMEOUT\n");
+			return -ETIMEDOUT;
+		}
+		loops--;
+		udelay(LOOPS_USEC);
+	}
+
+	/* Configure the Endpoint Context Address */
+	iowrite32(u3d->ep_context_dma, &u3d->op_regs->dcbaapl);
+	iowrite32(0, &u3d->op_regs->dcbaaph);
+
+	return 0;
+}
+
+static int mv_u3d_enable(struct mv_u3d *u3d)
+{
+	struct mv_usb_platform_data *pdata = u3d->dev->platform_data;
+	int retval;
+
+	if (u3d->active)
+		return 0;
+
+	if (!u3d->clock_gating) {
+		u3d->active = 1;
+		return 0;
+	}
+
+	dev_dbg(u3d->dev, "enable u3d\n");
+	clk_enable(u3d->clk);
+	if (pdata->phy_init) {
+		retval = pdata->phy_init(u3d->phy_regs);
+		if (retval) {
+			dev_err(u3d->dev,
+				"init phy error %d\n", retval);
+			clk_disable(u3d->clk);
+			return retval;
+		}
+	}
+	u3d->active = 1;
+
+	return 0;
+}
+
+static void mv_u3d_disable(struct mv_u3d *u3d)
+{
+	struct mv_usb_platform_data *pdata = u3d->dev->platform_data;
+	if (u3d->clock_gating && u3d->active) {
+		dev_dbg(u3d->dev, "disable u3d\n");
+		if (pdata->phy_deinit)
+			pdata->phy_deinit(u3d->phy_regs);
+		clk_disable(u3d->clk);
+		u3d->active = 0;
+	}
+}
+
+static int mv_u3d_vbus_session(struct usb_gadget *gadget, int is_active)
+{
+	struct mv_u3d *u3d;
+	unsigned long flags;
+	int retval = 0;
+
+	u3d = container_of(gadget, struct mv_u3d, gadget);
+
+	spin_lock_irqsave(&u3d->lock, flags);
+
+	u3d->vbus_active = (is_active != 0);
+	dev_dbg(u3d->dev, "%s: softconnect %d, vbus_active %d\n",
+		__func__, u3d->softconnect, u3d->vbus_active);
+	/*
+	 * 1. external VBUS detect: we can disable/enable clock on demand.
+	 * 2. UDC VBUS detect: we have to enable clock all the time.
+	 * 3. No VBUS detect: we have to enable clock all the time.
+	 */
+	if (u3d->driver && u3d->softconnect && u3d->vbus_active) {
+		retval = mv_u3d_enable(u3d);
+		if (retval == 0) {
+			/*
+			 * after clock is disabled, we lost all the register
+			 *  context. We have to re-init registers
+			 */
+			mv_u3d_controller_reset(u3d);
+			mv_u3d_ep0_reset(u3d);
+			mv_u3d_controller_start(u3d);
+		}
+	} else if (u3d->driver && u3d->softconnect) {
+		if (!u3d->active)
+			goto out;
+
+		/* stop all the transfer in queue*/
+		mv_u3d_stop_activity(u3d, u3d->driver);
+		mv_u3d_controller_stop(u3d);
+		mv_u3d_disable(u3d);
+	}
+
+out:
+	spin_unlock_irqrestore(&u3d->lock, flags);
+	return retval;
+}
+
+/* constrain controller's VBUS power usage
+ * This call is used by gadget drivers during SET_CONFIGURATION calls,
+ * reporting how much power the device may consume.  For example, this
+ * could affect how quickly batteries are recharged.
+ *
+ * Returns zero on success, else negative errno.
+ */
+static int mv_u3d_vbus_draw(struct usb_gadget *gadget, unsigned mA)
+{
+	struct mv_u3d *u3d = container_of(gadget, struct mv_u3d, gadget);
+
+	u3d->power = mA;
+
+	return 0;
+}
+
+static int mv_u3d_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct mv_u3d *u3d = container_of(gadget, struct mv_u3d, gadget);
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&u3d->lock, flags);
+
+	dev_dbg(u3d->dev, "%s: softconnect %d, vbus_active %d\n",
+		__func__, u3d->softconnect, u3d->vbus_active);
+	u3d->softconnect = (is_on != 0);
+	if (u3d->driver && u3d->softconnect && u3d->vbus_active) {
+		retval = mv_u3d_enable(u3d);
+		if (retval == 0) {
+			/*
+			 * after clock is disabled, we lost all the register
+			 *  context. We have to re-init registers
+			 */
+			mv_u3d_controller_reset(u3d);
+			mv_u3d_ep0_reset(u3d);
+			mv_u3d_controller_start(u3d);
+		}
+	} else if (u3d->driver && u3d->vbus_active) {
+		/* stop all the transfer in queue*/
+		mv_u3d_stop_activity(u3d, u3d->driver);
+		mv_u3d_controller_stop(u3d);
+		mv_u3d_disable(u3d);
+	}
+
+	spin_unlock_irqrestore(&u3d->lock, flags);
+
+	return retval;
+}
+
+static int mv_u3d_start(struct usb_gadget *g,
+		struct usb_gadget_driver *driver)
+{
+	struct mv_u3d *u3d = container_of(g, struct mv_u3d, gadget);
+	struct mv_usb_platform_data *pdata = u3d->dev->platform_data;
+	unsigned long flags;
+
+	if (u3d->driver)
+		return -EBUSY;
+
+	spin_lock_irqsave(&u3d->lock, flags);
+
+	if (!u3d->clock_gating) {
+		clk_enable(u3d->clk);
+		if (pdata->phy_init)
+			pdata->phy_init(u3d->phy_regs);
+	}
+
+	/* hook up the driver ... */
+	driver->driver.bus = NULL;
+	u3d->driver = driver;
+	u3d->gadget.dev.driver = &driver->driver;
+
+	u3d->ep0_dir = USB_DIR_OUT;
+
+	spin_unlock_irqrestore(&u3d->lock, flags);
+
+	u3d->vbus_valid_detect = 1;
+
+	return 0;
+}
+
+static int mv_u3d_stop(struct usb_gadget *g,
+		struct usb_gadget_driver *driver)
+{
+	struct mv_u3d *u3d = container_of(g, struct mv_u3d, gadget);
+	struct mv_usb_platform_data *pdata = u3d->dev->platform_data;
+	unsigned long flags;
+
+	u3d->vbus_valid_detect = 0;
+	spin_lock_irqsave(&u3d->lock, flags);
+
+	/* enable clock to access controller register */
+	clk_enable(u3d->clk);
+	if (pdata->phy_init)
+		pdata->phy_init(u3d->phy_regs);
+
+	mv_u3d_controller_stop(u3d);
+	/* stop all usb activities */
+	u3d->gadget.speed = USB_SPEED_UNKNOWN;
+	mv_u3d_stop_activity(u3d, driver);
+	mv_u3d_disable(u3d);
+
+	if (pdata->phy_deinit)
+		pdata->phy_deinit(u3d->phy_regs);
+	clk_disable(u3d->clk);
+
+	spin_unlock_irqrestore(&u3d->lock, flags);
+
+	u3d->gadget.dev.driver = NULL;
+	u3d->driver = NULL;
+
+	return 0;
+}
+
+/* device controller usb_gadget_ops structure */
+static const struct usb_gadget_ops mv_u3d_ops = {
+	/* notify controller that VBUS is powered or not */
+	.vbus_session	= mv_u3d_vbus_session,
+
+	/* constrain controller's VBUS power usage */
+	.vbus_draw	= mv_u3d_vbus_draw,
+
+	.pullup		= mv_u3d_pullup,
+	.udc_start	= mv_u3d_start,
+	.udc_stop	= mv_u3d_stop,
+};
+
+static int mv_u3d_eps_init(struct mv_u3d *u3d)
+{
+	struct mv_u3d_ep	*ep;
+	char name[14];
+	int i;
+
+	/* initialize ep0, ep0 in/out use eps[1] */
+	ep = &u3d->eps[1];
+	ep->u3d = u3d;
+	strncpy(ep->name, "ep0", sizeof(ep->name));
+	ep->ep.name = ep->name;
+	ep->ep.ops = &mv_u3d_ep_ops;
+	ep->wedge = 0;
+	ep->ep.maxpacket = MV_U3D_EP0_MAX_PKT_SIZE;
+	ep->ep_num = 0;
+	ep->ep.desc = &mv_u3d_ep0_desc;
+	INIT_LIST_HEAD(&ep->queue);
+	INIT_LIST_HEAD(&ep->req_list);
+	ep->ep_type = USB_ENDPOINT_XFER_CONTROL;
+
+	/* add ep0 ep_context */
+	ep->ep_context = &u3d->ep_context[1];
+
+	/* initialize other endpoints */
+	for (i = 2; i < u3d->max_eps * 2; i++) {
+		ep = &u3d->eps[i];
+		if (i & 1) {
+			snprintf(name, sizeof(name), "ep%din", i >> 1);
+			ep->direction = MV_U3D_EP_DIR_IN;
+		} else {
+			snprintf(name, sizeof(name), "ep%dout", i >> 1);
+			ep->direction = MV_U3D_EP_DIR_OUT;
+		}
+		ep->u3d = u3d;
+		strncpy(ep->name, name, sizeof(ep->name));
+		ep->ep.name = ep->name;
+
+		ep->ep.ops = &mv_u3d_ep_ops;
+		ep->ep.maxpacket = (unsigned short) ~0;
+		ep->ep_num = i / 2;
+
+		INIT_LIST_HEAD(&ep->queue);
+		list_add_tail(&ep->ep.ep_list, &u3d->gadget.ep_list);
+
+		INIT_LIST_HEAD(&ep->req_list);
+		spin_lock_init(&ep->req_lock);
+		ep->ep_context = &u3d->ep_context[i];
+	}
+
+	return 0;
+}
+
+/* delete all endpoint requests, called with spinlock held */
+static void mv_u3d_nuke(struct mv_u3d_ep *ep, int status)
+{
+	/* endpoint fifo flush */
+	mv_u3d_ep_fifo_flush(&ep->ep);
+
+	while (!list_empty(&ep->queue)) {
+		struct mv_u3d_req *req = NULL;
+		req = list_entry(ep->queue.next, struct mv_u3d_req, queue);
+		mv_u3d_done(ep, req, status);
+	}
+}
+
+/* stop all USB activities */
+static
+void mv_u3d_stop_activity(struct mv_u3d *u3d, struct usb_gadget_driver *driver)
+{
+	struct mv_u3d_ep	*ep;
+
+	mv_u3d_nuke(&u3d->eps[1], -ESHUTDOWN);
+
+	list_for_each_entry(ep, &u3d->gadget.ep_list, ep.ep_list) {
+		mv_u3d_nuke(ep, -ESHUTDOWN);
+	}
+
+	/* report disconnect; the driver is already quiesced */
+	if (driver) {
+		spin_unlock(&u3d->lock);
+		driver->disconnect(&u3d->gadget);
+		spin_lock(&u3d->lock);
+	}
+}
+
+static void mv_u3d_irq_process_error(struct mv_u3d *u3d)
+{
+	/* Increment the error count */
+	u3d->errors++;
+	dev_err(u3d->dev, "%s\n", __func__);
+}
+
+static void mv_u3d_irq_process_link_change(struct mv_u3d *u3d)
+{
+	u32 linkchange;
+
+	linkchange = ioread32(&u3d->vuc_regs->linkchange);
+	iowrite32(linkchange, &u3d->vuc_regs->linkchange);
+
+	dev_dbg(u3d->dev, "linkchange: 0x%x\n", linkchange);
+
+	if (linkchange & MV_U3D_LINK_CHANGE_LINK_UP) {
+		dev_dbg(u3d->dev, "link up: ltssm state: 0x%x\n",
+			ioread32(&u3d->vuc_regs->ltssmstate));
+
+		u3d->usb_state = USB_STATE_DEFAULT;
+		u3d->ep0_dir = MV_U3D_EP_DIR_OUT;
+		u3d->ep0_state = MV_U3D_WAIT_FOR_SETUP;
+
+		/* set speed */
+		u3d->gadget.speed = USB_SPEED_SUPER;
+	}
+
+	if (linkchange & MV_U3D_LINK_CHANGE_SUSPEND) {
+		dev_dbg(u3d->dev, "link suspend\n");
+		u3d->resume_state = u3d->usb_state;
+		u3d->usb_state = USB_STATE_SUSPENDED;
+	}
+
+	if (linkchange & MV_U3D_LINK_CHANGE_RESUME) {
+		dev_dbg(u3d->dev, "link resume\n");
+		u3d->usb_state = u3d->resume_state;
+		u3d->resume_state = 0;
+	}
+
+	if (linkchange & MV_U3D_LINK_CHANGE_WRESET) {
+		dev_dbg(u3d->dev, "warm reset\n");
+		u3d->usb_state = USB_STATE_POWERED;
+	}
+
+	if (linkchange & MV_U3D_LINK_CHANGE_HRESET) {
+		dev_dbg(u3d->dev, "hot reset\n");
+		u3d->usb_state = USB_STATE_DEFAULT;
+	}
+
+	if (linkchange & MV_U3D_LINK_CHANGE_INACT)
+		dev_dbg(u3d->dev, "inactive\n");
+
+	if (linkchange & MV_U3D_LINK_CHANGE_DISABLE_AFTER_U0)
+		dev_dbg(u3d->dev, "ss.disabled\n");
+
+	if (linkchange & MV_U3D_LINK_CHANGE_VBUS_INVALID) {
+		dev_dbg(u3d->dev, "vbus invalid\n");
+		u3d->usb_state = USB_STATE_ATTACHED;
+		u3d->vbus_valid_detect = 1;
+		/* if external vbus detect is not supported,
+		 * we handle it here.
+		 */
+		if (!u3d->vbus) {
+			spin_unlock(&u3d->lock);
+			mv_u3d_vbus_session(&u3d->gadget, 0);
+			spin_lock(&u3d->lock);
+		}
+	}
+}
+
+static void mv_u3d_ch9setaddress(struct mv_u3d *u3d,
+				struct usb_ctrlrequest *setup)
+{
+	u32 tmp;
+
+	if (u3d->usb_state != USB_STATE_DEFAULT) {
+		dev_err(u3d->dev,
+			"%s, cannot setaddr in this state (%d)\n",
+			__func__, u3d->usb_state);
+		goto err;
+	}
+
+	u3d->dev_addr = (u8)setup->wValue;
+
+	dev_dbg(u3d->dev, "%s: 0x%x\n", __func__, u3d->dev_addr);
+
+	if (u3d->dev_addr > 127) {
+		dev_err(u3d->dev,
+			"%s, u3d address is wrong (out of range)\n", __func__);
+		u3d->dev_addr = 0;
+		goto err;
+	}
+
+	/* update usb state */
+	u3d->usb_state = USB_STATE_ADDRESS;
+
+	/* set the new address */
+	tmp = ioread32(&u3d->vuc_regs->devaddrtiebrkr);
+	tmp &= ~0x7F;
+	tmp |= (u32)u3d->dev_addr;
+	iowrite32(tmp, &u3d->vuc_regs->devaddrtiebrkr);
+
+	return;
+err:
+	mv_u3d_ep0_stall(u3d);
+}
+
+static int mv_u3d_is_set_configuration(struct usb_ctrlrequest *setup)
+{
+	if ((setup->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD)
+		if (setup->bRequest == USB_REQ_SET_CONFIGURATION)
+			return 1;
+
+	return 0;
+}
+
+static void mv_u3d_handle_setup_packet(struct mv_u3d *u3d, u8 ep_num,
+	struct usb_ctrlrequest *setup)
+{
+	bool delegate = false;
+
+	mv_u3d_nuke(&u3d->eps[ep_num * 2 + MV_U3D_EP_DIR_IN], -ESHUTDOWN);
+
+	dev_dbg(u3d->dev, "SETUP %02x.%02x v%04x i%04x l%04x\n",
+			setup->bRequestType, setup->bRequest,
+			setup->wValue, setup->wIndex, setup->wLength);
+
+	/* We process some stardard setup requests here */
+	if ((setup->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
+		switch (setup->bRequest) {
+		case USB_REQ_GET_STATUS:
+			delegate = true;
+			break;
+
+		case USB_REQ_SET_ADDRESS:
+			mv_u3d_ch9setaddress(u3d, setup);
+			break;
+
+		case USB_REQ_CLEAR_FEATURE:
+			delegate = true;
+			break;
+
+		case USB_REQ_SET_FEATURE:
+			delegate = true;
+			break;
+
+		default:
+			delegate = true;
+		}
+	} else
+		delegate = true;
+
+	/* delegate USB standard requests to the gadget driver */
+	if (delegate == true) {
+		/* USB requests handled by gadget */
+		if (setup->wLength) {
+			/* DATA phase from gadget, STATUS phase from u3d */
+			u3d->ep0_dir = (setup->bRequestType & USB_DIR_IN)
+					? MV_U3D_EP_DIR_IN : MV_U3D_EP_DIR_OUT;
+			spin_unlock(&u3d->lock);
+			if (u3d->driver->setup(&u3d->gadget,
+				&u3d->local_setup_buff) < 0) {
+				dev_err(u3d->dev, "setup error!\n");
+				mv_u3d_ep0_stall(u3d);
+			}
+			spin_lock(&u3d->lock);
+		} else {
+			/* no DATA phase, STATUS phase from gadget */
+			u3d->ep0_dir = MV_U3D_EP_DIR_IN;
+			u3d->ep0_state = MV_U3D_STATUS_STAGE;
+			spin_unlock(&u3d->lock);
+			if (u3d->driver->setup(&u3d->gadget,
+				&u3d->local_setup_buff) < 0)
+				mv_u3d_ep0_stall(u3d);
+			spin_lock(&u3d->lock);
+		}
+
+		if (mv_u3d_is_set_configuration(setup)) {
+			dev_dbg(u3d->dev, "u3d configured\n");
+			u3d->usb_state = USB_STATE_CONFIGURED;
+		}
+	}
+}
+
+static void mv_u3d_get_setup_data(struct mv_u3d *u3d, u8 ep_num, u8 *buffer_ptr)
+{
+	struct mv_u3d_ep_context *epcontext;
+
+	epcontext = &u3d->ep_context[ep_num * 2 + MV_U3D_EP_DIR_IN];
+
+	/* Copy the setup packet to local buffer */
+	memcpy(buffer_ptr, (u8 *) &epcontext->setup_buffer, 8);
+}
+
+static void mv_u3d_irq_process_setup(struct mv_u3d *u3d)
+{
+	u32 tmp, i;
+	/* Process all Setup packet received interrupts */
+	tmp = ioread32(&u3d->vuc_regs->setuplock);
+	if (tmp) {
+		for (i = 0; i < u3d->max_eps; i++) {
+			if (tmp & (1 << i)) {
+				mv_u3d_get_setup_data(u3d, i,
+					(u8 *)(&u3d->local_setup_buff));
+				mv_u3d_handle_setup_packet(u3d, i,
+					&u3d->local_setup_buff);
+			}
+		}
+	}
+
+	iowrite32(tmp, &u3d->vuc_regs->setuplock);
+}
+
+static void mv_u3d_irq_process_tr_complete(struct mv_u3d *u3d)
+{
+	u32 tmp, bit_pos;
+	int i, ep_num = 0, direction = 0;
+	struct mv_u3d_ep	*curr_ep;
+	struct mv_u3d_req *curr_req, *temp_req;
+	int status;
+
+	tmp = ioread32(&u3d->vuc_regs->endcomplete);
+
+	dev_dbg(u3d->dev, "tr_complete: ep: 0x%x\n", tmp);
+	if (!tmp)
+		return;
+	iowrite32(tmp, &u3d->vuc_regs->endcomplete);
+
+	for (i = 0; i < u3d->max_eps * 2; i++) {
+		ep_num = i >> 1;
+		direction = i % 2;
+
+		bit_pos = 1 << (ep_num + 16 * direction);
+
+		if (!(bit_pos & tmp))
+			continue;
+
+		if (i == 0)
+			curr_ep = &u3d->eps[1];
+		else
+			curr_ep = &u3d->eps[i];
+
+		/* remove req out of ep request list after completion */
+		dev_dbg(u3d->dev, "tr comp: check req_list\n");
+		spin_lock(&curr_ep->req_lock);
+		if (!list_empty(&curr_ep->req_list)) {
+			struct mv_u3d_req *req;
+			req = list_entry(curr_ep->req_list.next,
+						struct mv_u3d_req, list);
+			list_del_init(&req->list);
+			curr_ep->processing = 0;
+		}
+		spin_unlock(&curr_ep->req_lock);
+
+		/* process the req queue until an uncomplete request */
+		list_for_each_entry_safe(curr_req, temp_req,
+			&curr_ep->queue, queue) {
+			status = mv_u3d_process_ep_req(u3d, i, curr_req);
+			if (status)
+				break;
+			/* write back status to req */
+			curr_req->req.status = status;
+
+			/* ep0 request completion */
+			if (ep_num == 0) {
+				mv_u3d_done(curr_ep, curr_req, 0);
+				break;
+			} else {
+				mv_u3d_done(curr_ep, curr_req, status);
+			}
+		}
+
+		dev_dbg(u3d->dev, "call mv_u3d_start_queue from ep complete\n");
+		mv_u3d_start_queue(curr_ep);
+	}
+}
+
+static irqreturn_t mv_u3d_irq(int irq, void *dev)
+{
+	struct mv_u3d *u3d = (struct mv_u3d *)dev;
+	u32 status, intr;
+	u32 bridgesetting;
+	u32 trbunderrun;
+
+	spin_lock(&u3d->lock);
+
+	status = ioread32(&u3d->vuc_regs->intrcause);
+	intr = ioread32(&u3d->vuc_regs->intrenable);
+	status &= intr;
+
+	if (status == 0) {
+		spin_unlock(&u3d->lock);
+		dev_err(u3d->dev, "irq error!\n");
+		return IRQ_NONE;
+	}
+
+	if (status & MV_U3D_USBINT_VBUS_VALID) {
+		bridgesetting = ioread32(&u3d->vuc_regs->bridgesetting);
+		if (bridgesetting & MV_U3D_BRIDGE_SETTING_VBUS_VALID) {
+			/* write vbus valid bit of bridge setting to clear */
+			bridgesetting = MV_U3D_BRIDGE_SETTING_VBUS_VALID;
+			iowrite32(bridgesetting, &u3d->vuc_regs->bridgesetting);
+			dev_dbg(u3d->dev, "vbus valid\n");
+
+			u3d->usb_state = USB_STATE_POWERED;
+			u3d->vbus_valid_detect = 0;
+			/* if external vbus detect is not supported,
+			 * we handle it here.
+			 */
+			if (!u3d->vbus) {
+				spin_unlock(&u3d->lock);
+				mv_u3d_vbus_session(&u3d->gadget, 1);
+				spin_lock(&u3d->lock);
+			}
+		} else
+			dev_err(u3d->dev, "vbus bit is not set\n");
+	}
+
+	/* RX data is already in the 16KB FIFO.*/
+	if (status & MV_U3D_USBINT_UNDER_RUN) {
+		trbunderrun = ioread32(&u3d->vuc_regs->trbunderrun);
+		dev_err(u3d->dev, "under run, ep%d\n", trbunderrun);
+		iowrite32(trbunderrun, &u3d->vuc_regs->trbunderrun);
+		mv_u3d_irq_process_error(u3d);
+	}
+
+	if (status & (MV_U3D_USBINT_RXDESC_ERR | MV_U3D_USBINT_TXDESC_ERR)) {
+		/* write one to clear */
+		iowrite32(status & (MV_U3D_USBINT_RXDESC_ERR
+			| MV_U3D_USBINT_TXDESC_ERR),
+			&u3d->vuc_regs->intrcause);
+		dev_err(u3d->dev, "desc err 0x%x\n", status);
+		mv_u3d_irq_process_error(u3d);
+	}
+
+	if (status & MV_U3D_USBINT_LINK_CHG)
+		mv_u3d_irq_process_link_change(u3d);
+
+	if (status & MV_U3D_USBINT_TX_COMPLETE)
+		mv_u3d_irq_process_tr_complete(u3d);
+
+	if (status & MV_U3D_USBINT_RX_COMPLETE)
+		mv_u3d_irq_process_tr_complete(u3d);
+
+	if (status & MV_U3D_USBINT_SETUP)
+		mv_u3d_irq_process_setup(u3d);
+
+	spin_unlock(&u3d->lock);
+	return IRQ_HANDLED;
+}
+
+static void mv_u3d_gadget_release(struct device *dev)
+{
+	dev_dbg(dev, "%s\n", __func__);
+}
+
+static __devexit int mv_u3d_remove(struct platform_device *dev)
+{
+	struct mv_u3d *u3d = platform_get_drvdata(dev);
+
+	BUG_ON(u3d == NULL);
+
+	usb_del_gadget_udc(&u3d->gadget);
+
+	/* free memory allocated in probe */
+	if (u3d->trb_pool)
+		dma_pool_destroy(u3d->trb_pool);
+
+	if (u3d->ep_context)
+		dma_free_coherent(&dev->dev, u3d->ep_context_size,
+			u3d->ep_context, u3d->ep_context_dma);
+
+	kfree(u3d->eps);
+
+	if (u3d->irq)
+		free_irq(u3d->irq, &dev->dev);
+
+	if (u3d->cap_regs)
+		iounmap(u3d->cap_regs);
+	u3d->cap_regs = NULL;
+
+	kfree(u3d->status_req);
+
+	clk_put(u3d->clk);
+
+	device_unregister(&u3d->gadget.dev);
+
+	platform_set_drvdata(dev, NULL);
+
+	kfree(u3d);
+
+	return 0;
+}
+
+static int mv_u3d_probe(struct platform_device *dev)
+{
+	struct mv_u3d *u3d = NULL;
+	struct mv_usb_platform_data *pdata = dev->dev.platform_data;
+	int retval = 0;
+	struct resource *r;
+	size_t size;
+
+	if (!dev->dev.platform_data) {
+		dev_err(&dev->dev, "missing platform_data\n");
+		retval = -ENODEV;
+		goto err_pdata;
+	}
+
+	u3d = kzalloc(sizeof(*u3d), GFP_KERNEL);
+	if (!u3d) {
+		dev_err(&dev->dev, "failed to allocate memory for u3d\n");
+		retval = -ENOMEM;
+		goto err_alloc_private;
+	}
+
+	spin_lock_init(&u3d->lock);
+
+	platform_set_drvdata(dev, u3d);
+
+	u3d->dev = &dev->dev;
+	u3d->vbus = pdata->vbus;
+
+	u3d->clk = clk_get(&dev->dev, pdata->clkname[0]);
+	if (IS_ERR(u3d->clk)) {
+		retval = PTR_ERR(u3d->clk);
+		goto err_get_clk;
+	}
+
+	r = platform_get_resource_byname(dev, IORESOURCE_MEM, "capregs");
+	if (!r) {
+		dev_err(&dev->dev, "no I/O memory resource defined\n");
+		retval = -ENODEV;
+		goto err_get_cap_regs;
+	}
+
+	u3d->cap_regs = (struct mv_u3d_cap_regs __iomem *)
+		ioremap(r->start, resource_size(r));
+	if (!u3d->cap_regs) {
+		dev_err(&dev->dev, "failed to map I/O memory\n");
+		retval = -EBUSY;
+		goto err_map_cap_regs;
+	} else {
+		dev_dbg(&dev->dev, "cap_regs address: 0x%x/0x%x\n",
+			(unsigned int)r->start, (unsigned int)u3d->cap_regs);
+	}
+
+	/* we will access controller register, so enable the u3d controller */
+	clk_enable(u3d->clk);
+
+	if (pdata->phy_init) {
+		retval = pdata->phy_init(u3d->phy_regs);
+		if (retval) {
+			dev_err(&dev->dev, "init phy error %d\n", retval);
+			goto err_u3d_enable;
+		}
+	}
+
+	u3d->op_regs = (struct mv_u3d_op_regs __iomem *)((u32)u3d->cap_regs
+		+ MV_U3D_USB3_OP_REGS_OFFSET);
+
+	u3d->vuc_regs = (struct mv_u3d_vuc_regs __iomem *)((u32)u3d->cap_regs
+		+ ioread32(&u3d->cap_regs->vuoff));
+
+	u3d->max_eps = 16;
+
+	/*
+	 * some platform will use usb to download image, it may not disconnect
+	 * usb gadget before loading kernel. So first stop u3d here.
+	 */
+	mv_u3d_controller_stop(u3d);
+	iowrite32(0xFFFFFFFF, &u3d->vuc_regs->intrcause);
+
+	if (pdata->phy_deinit)
+		pdata->phy_deinit(u3d->phy_regs);
+	clk_disable(u3d->clk);
+
+	size = u3d->max_eps * sizeof(struct mv_u3d_ep_context) * 2;
+	size = (size + MV_U3D_EP_CONTEXT_ALIGNMENT - 1)
+		& ~(MV_U3D_EP_CONTEXT_ALIGNMENT - 1);
+	u3d->ep_context = dma_alloc_coherent(&dev->dev, size,
+					&u3d->ep_context_dma, GFP_KERNEL);
+	if (!u3d->ep_context) {
+		dev_err(&dev->dev, "allocate ep context memory failed\n");
+		retval = -ENOMEM;
+		goto err_alloc_ep_context;
+	}
+	u3d->ep_context_size = size;
+
+	/* create TRB dma_pool resource */
+	u3d->trb_pool = dma_pool_create("u3d_trb",
+			&dev->dev,
+			sizeof(struct mv_u3d_trb_hw),
+			MV_U3D_TRB_ALIGNMENT,
+			MV_U3D_DMA_BOUNDARY);
+
+	if (!u3d->trb_pool) {
+		retval = -ENOMEM;
+		goto err_alloc_trb_pool;
+	}
+
+	size = u3d->max_eps * sizeof(struct mv_u3d_ep) * 2;
+	u3d->eps = kzalloc(size, GFP_KERNEL);
+	if (!u3d->eps) {
+		dev_err(&dev->dev, "allocate ep memory failed\n");
+		retval = -ENOMEM;
+		goto err_alloc_eps;
+	}
+
+	/* initialize ep0 status request structure */
+	u3d->status_req = kzalloc(sizeof(struct mv_u3d_req) + 8, GFP_KERNEL);
+	if (!u3d->status_req) {
+		dev_err(&dev->dev, "allocate status_req memory failed\n");
+		retval = -ENOMEM;
+		goto err_alloc_status_req;
+	}
+	INIT_LIST_HEAD(&u3d->status_req->queue);
+
+	/* allocate a small amount of memory to get valid address */
+	u3d->status_req->req.buf = (char *)u3d->status_req
+					+ sizeof(struct mv_u3d_req);
+	u3d->status_req->req.dma = virt_to_phys(u3d->status_req->req.buf);
+
+	u3d->resume_state = USB_STATE_NOTATTACHED;
+	u3d->usb_state = USB_STATE_ATTACHED;
+	u3d->ep0_dir = MV_U3D_EP_DIR_OUT;
+	u3d->remote_wakeup = 0;
+
+	r = platform_get_resource(dev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		dev_err(&dev->dev, "no IRQ resource defined\n");
+		retval = -ENODEV;
+		goto err_get_irq;
+	}
+	u3d->irq = r->start;
+	if (request_irq(u3d->irq, mv_u3d_irq,
+		IRQF_DISABLED | IRQF_SHARED, driver_name, u3d)) {
+		u3d->irq = 0;
+		dev_err(&dev->dev, "Request irq %d for u3d failed\n",
+			u3d->irq);
+		retval = -ENODEV;
+		goto err_request_irq;
+	}
+
+	/* initialize gadget structure */
+	u3d->gadget.ops = &mv_u3d_ops;	/* usb_gadget_ops */
+	u3d->gadget.ep0 = &u3d->eps[1].ep;	/* gadget ep0 */
+	INIT_LIST_HEAD(&u3d->gadget.ep_list);	/* ep_list */
+	u3d->gadget.speed = USB_SPEED_UNKNOWN;	/* speed */
+
+	/* the "gadget" abstracts/virtualizes the controller */
+	dev_set_name(&u3d->gadget.dev, "gadget");
+	u3d->gadget.dev.parent = &dev->dev;
+	u3d->gadget.dev.dma_mask = dev->dev.dma_mask;
+	u3d->gadget.dev.release = mv_u3d_gadget_release;
+	u3d->gadget.name = driver_name;		/* gadget name */
+
+	retval = device_register(&u3d->gadget.dev);
+	if (retval)
+		goto err_register_gadget_device;
+
+	mv_u3d_eps_init(u3d);
+
+	/* external vbus detection */
+	if (u3d->vbus) {
+		u3d->clock_gating = 1;
+		dev_err(&dev->dev, "external vbus detection\n");
+	}
+
+	if (!u3d->clock_gating)
+		u3d->vbus_active = 1;
+
+	/* enable usb3 controller vbus detection */
+	u3d->vbus_valid_detect = 1;
+
+	retval = usb_add_gadget_udc(&dev->dev, &u3d->gadget);
+	if (retval)
+		goto err_unregister;
+
+	dev_dbg(&dev->dev, "successful probe usb3 device %s clock gating.\n",
+		u3d->clock_gating ? "with" : "without");
+
+	return 0;
+
+err_unregister:
+	device_unregister(&u3d->gadget.dev);
+err_register_gadget_device:
+	free_irq(u3d->irq, &dev->dev);
+err_request_irq:
+err_get_irq:
+	kfree(u3d->status_req);
+err_alloc_status_req:
+	kfree(u3d->eps);
+err_alloc_eps:
+	dma_pool_destroy(u3d->trb_pool);
+err_alloc_trb_pool:
+	dma_free_coherent(&dev->dev, u3d->ep_context_size,
+		u3d->ep_context, u3d->ep_context_dma);
+err_alloc_ep_context:
+	if (pdata->phy_deinit)
+		pdata->phy_deinit(u3d->phy_regs);
+	clk_disable(u3d->clk);
+err_u3d_enable:
+	iounmap(u3d->cap_regs);
+err_map_cap_regs:
+err_get_cap_regs:
+err_get_clk:
+	clk_put(u3d->clk);
+	platform_set_drvdata(dev, NULL);
+	kfree(u3d);
+err_alloc_private:
+err_pdata:
+	return retval;
+}
+
+#ifdef CONFIG_PM
+static int mv_u3d_suspend(struct device *dev)
+{
+	struct mv_u3d *u3d = dev_get_drvdata(dev);
+
+	/*
+	 * only cable is unplugged, usb can suspend.
+	 * So do not care about clock_gating == 1, it is handled by
+	 * vbus session.
+	 */
+	if (!u3d->clock_gating) {
+		mv_u3d_controller_stop(u3d);
+
+		spin_lock_irq(&u3d->lock);
+		/* stop all usb activities */
+		mv_u3d_stop_activity(u3d, u3d->driver);
+		spin_unlock_irq(&u3d->lock);
+
+		mv_u3d_disable(u3d);
+	}
+
+	return 0;
+}
+
+static int mv_u3d_resume(struct device *dev)
+{
+	struct mv_u3d *u3d = dev_get_drvdata(dev);
+	int retval;
+
+	if (!u3d->clock_gating) {
+		retval = mv_u3d_enable(u3d);
+		if (retval)
+			return retval;
+
+		if (u3d->driver && u3d->softconnect) {
+			mv_u3d_controller_reset(u3d);
+			mv_u3d_ep0_reset(u3d);
+			mv_u3d_controller_start(u3d);
+		}
+	}
+
+	return 0;
+}
+
+SIMPLE_DEV_PM_OPS(mv_u3d_pm_ops, mv_u3d_suspend, mv_u3d_resume);
+#endif
+
+static void mv_u3d_shutdown(struct platform_device *dev)
+{
+	struct mv_u3d *u3d = dev_get_drvdata(&dev->dev);
+	u32 tmp;
+
+	tmp = ioread32(&u3d->op_regs->usbcmd);
+	tmp &= ~MV_U3D_CMD_RUN_STOP;
+	iowrite32(tmp, &u3d->op_regs->usbcmd);
+}
+
+static struct platform_driver mv_u3d_driver = {
+	.probe		= mv_u3d_probe,
+	.remove		= __exit_p(mv_u3d_remove),
+	.shutdown	= mv_u3d_shutdown,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "mv-u3d",
+#ifdef CONFIG_PM
+		.pm	= &mv_u3d_pm_ops,
+#endif
+	},
+};
+
+module_platform_driver(mv_u3d_driver);
+MODULE_ALIAS("platform:mv-u3d");
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Yu Xu <yuxu@marvell.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/mv_udc_core.c b/drivers/usb/gadget/mv_udc_core.c
index 117a4bba1b8..75db2c306ce 100644
--- a/drivers/usb/gadget/mv_udc_core.c
+++ b/drivers/usb/gadget/mv_udc_core.c
@@ -19,6 +19,7 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/timer.h>
 #include <linux/list.h>
@@ -1381,7 +1382,7 @@ static int mv_udc_start(struct usb_gadget_driver *driver,
 		return retval;
 	}
 
-	if (udc->transceiver) {
+	if (!IS_ERR_OR_NULL(udc->transceiver)) {
 		retval = otg_set_peripheral(udc->transceiver->otg,
 					&udc->gadget);
 		if (retval) {
@@ -2107,7 +2108,7 @@ static int __devexit mv_udc_remove(struct platform_device *dev)
 	 * then vbus irq will not be requested in udc driver.
 	 */
 	if (udc->pdata && udc->pdata->vbus
-		&& udc->clock_gating && udc->transceiver == NULL)
+		&& udc->clock_gating && IS_ERR_OR_NULL(udc->transceiver))
 		free_irq(udc->pdata->vbus->irq, &dev->dev);
 
 	/* free memory allocated in probe */
@@ -2180,7 +2181,7 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 
 #ifdef CONFIG_USB_OTG_UTILS
 	if (pdata->mode == MV_USB_MODE_OTG)
-		udc->transceiver = usb_get_transceiver();
+		udc->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 #endif
 
 	udc->clknum = pdata->clknum;
@@ -2325,7 +2326,7 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 	eps_init(udc);
 
 	/* VBUS detect: we can disable/enable clock on demand.*/
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		udc->clock_gating = 1;
 	else if (pdata->vbus) {
 		udc->clock_gating = 1;
@@ -2369,7 +2370,7 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 
 err_unregister:
 	if (udc->pdata && udc->pdata->vbus
-		&& udc->clock_gating && udc->transceiver == NULL)
+		&& udc->clock_gating && IS_ERR_OR_NULL(udc->transceiver))
 		free_irq(pdata->vbus->irq, &dev->dev);
 	device_unregister(&udc->gadget.dev);
 err_free_irq:
@@ -2404,7 +2405,7 @@ static int mv_udc_suspend(struct device *_dev)
 	struct mv_udc *udc = the_controller;
 
 	/* if OTG is enabled, the following will be done in OTG driver*/
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return 0;
 
 	if (udc->pdata->vbus && udc->pdata->vbus->poll)
@@ -2437,7 +2438,7 @@ static int mv_udc_resume(struct device *_dev)
 	int retval;
 
 	/* if OTG is enabled, the following will be done in OTG driver*/
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return 0;
 
 	if (!udc->clock_gating) {
diff --git a/drivers/usb/gadget/omap_udc.c b/drivers/usb/gadget/omap_udc.c
index a460e8c204f..f9132ada53b 100644
--- a/drivers/usb/gadget/omap_udc.c
+++ b/drivers/usb/gadget/omap_udc.c
@@ -35,16 +35,18 @@
 #include <linux/usb/otg.h>
 #include <linux/dma-mapping.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/prefetch.h>
+#include <linux/io.h>
 
 #include <asm/byteorder.h>
-#include <asm/io.h>
 #include <asm/irq.h>
 #include <asm/unaligned.h>
 #include <asm/mach-types.h>
 
 #include <plat/dma.h>
-#include <plat/usb.h>
+
+#include <mach/usb.h>
 
 #include "omap_udc.h"
 
@@ -59,11 +61,6 @@
 #define	DRIVER_DESC	"OMAP UDC driver"
 #define	DRIVER_VERSION	"4 October 2004"
 
-#define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
-
-#define OMAP2_DMA_CH(ch)	(((ch) - 1) << 1)
-#define OMAP24XX_DMA(name, ch)	(OMAP24XX_DMA_##name + OMAP2_DMA_CH(ch))
-
 /*
  * The OMAP UDC needs _very_ early endpoint setup:  before enabling the
  * D+ pullup to allow enumeration.  That's too early for the gadget
@@ -87,14 +84,14 @@
 #ifdef	USE_ISO
 static unsigned fifo_mode = 3;
 #else
-static unsigned fifo_mode = 0;
+static unsigned fifo_mode;
 #endif
 
 /* "modprobe omap_udc fifo_mode=42", or else as a kernel
  * boot parameter "omap_udc:fifo_mode=42"
  */
-module_param (fifo_mode, uint, 0);
-MODULE_PARM_DESC (fifo_mode, "endpoint configuration");
+module_param(fifo_mode, uint, 0);
+MODULE_PARM_DESC(fifo_mode, "endpoint configuration");
 
 #ifdef	USE_DMA
 static bool use_dma = 1;
@@ -102,8 +99,8 @@ static bool use_dma = 1;
 /* "modprobe omap_udc use_dma=y", or else as a kernel
  * boot parameter "omap_udc:use_dma=y"
  */
-module_param (use_dma, bool, 0);
-MODULE_PARM_DESC (use_dma, "enable/disable DMA");
+module_param(use_dma, bool, 0);
+MODULE_PARM_DESC(use_dma, "enable/disable DMA");
 #else	/* !USE_DMA */
 
 /* save a bit of code */
@@ -111,8 +108,8 @@ MODULE_PARM_DESC (use_dma, "enable/disable DMA");
 #endif	/* !USE_DMA */
 
 
-static const char driver_name [] = "omap_udc";
-static const char driver_desc [] = DRIVER_DESC;
+static const char driver_name[] = "omap_udc";
+static const char driver_desc[] = DRIVER_DESC;
 
 /*-------------------------------------------------------------------------*/
 
@@ -250,7 +247,7 @@ static int omap_ep_disable(struct usb_ep *_ep)
 
 	spin_lock_irqsave(&ep->udc->lock, flags);
 	ep->ep.desc = NULL;
-	nuke (ep, -ESHUTDOWN);
+	nuke(ep, -ESHUTDOWN);
 	ep->ep.maxpacket = ep->maxpacket;
 	ep->has_dma = 0;
 	omap_writew(UDC_SET_HALT, UDC_CTRL);
@@ -271,10 +268,11 @@ omap_alloc_request(struct usb_ep *ep, gfp_t gfp_flags)
 	struct omap_req	*req;
 
 	req = kzalloc(sizeof(*req), gfp_flags);
-	if (req) {
-		req->req.dma = DMA_ADDR_INVALID;
-		INIT_LIST_HEAD (&req->queue);
-	}
+	if (!req)
+		return NULL;
+
+	INIT_LIST_HEAD(&req->queue);
+
 	return &req->req;
 }
 
@@ -283,8 +281,7 @@ omap_free_request(struct usb_ep *ep, struct usb_request *_req)
 {
 	struct omap_req	*req = container_of(_req, struct omap_req, req);
 
-	if (_req)
-		kfree (req);
+	kfree(req);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -292,6 +289,7 @@ omap_free_request(struct usb_ep *ep, struct usb_request *_req)
 static void
 done(struct omap_ep *ep, struct omap_req *req, int status)
 {
+	struct omap_udc		*udc = ep->udc;
 	unsigned		stopped = ep->stopped;
 
 	list_del_init(&req->queue);
@@ -301,22 +299,9 @@ done(struct omap_ep *ep, struct omap_req *req, int status)
 	else
 		status = req->req.status;
 
-	if (use_dma && ep->has_dma) {
-		if (req->mapped) {
-			dma_unmap_single(ep->udc->gadget.dev.parent,
-				req->req.dma, req->req.length,
-				(ep->bEndpointAddress & USB_DIR_IN)
-					? DMA_TO_DEVICE
-					: DMA_FROM_DEVICE);
-			req->req.dma = DMA_ADDR_INVALID;
-			req->mapped = 0;
-		} else
-			dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
-				req->req.dma, req->req.length,
-				(ep->bEndpointAddress & USB_DIR_IN)
-					? DMA_TO_DEVICE
-					: DMA_FROM_DEVICE);
-	}
+	if (use_dma && ep->has_dma)
+		usb_gadget_unmap_request(&udc->gadget, &req->req,
+				(ep->bEndpointAddress & USB_DIR_IN));
 
 #ifndef	USB_TRACE
 	if (status && status != -ESHUTDOWN)
@@ -364,10 +349,10 @@ write_packet(u8 *buf, struct omap_req *req, unsigned max)
 	return len;
 }
 
-// FIXME change r/w fifo calling convention
+/* FIXME change r/w fifo calling convention */
 
 
-// return:  0 = still running, 1 = completed, negative = errno
+/* return:  0 = still running, 1 = completed, negative = errno */
 static int write_fifo(struct omap_ep *ep, struct omap_req *req)
 {
 	u8		*buf;
@@ -429,7 +414,7 @@ read_packet(u8 *buf, struct omap_req *req, unsigned avail)
 	return len;
 }
 
-// return:  0 = still running, 1 = queue empty, negative = errno
+/* return:  0 = still running, 1 = queue empty, negative = errno */
 static int read_fifo(struct omap_ep *ep, struct omap_req *req)
 {
 	u8		*buf;
@@ -536,12 +521,8 @@ static void next_in_dma(struct omap_ep *ep, struct omap_req *req)
 				: OMAP_DMA_SYNC_ELEMENT;
 	int		dma_trigger = 0;
 
-	if (cpu_is_omap24xx())
-		dma_trigger = OMAP24XX_DMA(USB_W2FC_TX0, ep->dma_channel);
-
 	/* measure length in either bytes or packets */
 	if ((cpu_is_omap16xx() && length <= UDC_TXN_TSC)
-			|| (cpu_is_omap24xx() && length < ep->maxpacket)
 			|| (cpu_is_omap15xx() && length < ep->maxpacket)) {
 		txdma_ctrl = UDC_TXN_EOT | length;
 		omap_set_dma_transfer_params(ep->lch, OMAP_DMA_DATA_TYPE_S8,
@@ -600,28 +581,14 @@ static void next_out_dma(struct omap_ep *ep, struct omap_req *req)
 	int dma_trigger = 0;
 	u16 w;
 
-	if (cpu_is_omap24xx())
-		dma_trigger = OMAP24XX_DMA(USB_W2FC_RX0, ep->dma_channel);
-
-	/* NOTE:  we filtered out "short reads" before, so we know
-	 * the buffer has only whole numbers of packets.
-	 * except MODE SELECT(6) sent the 24 bytes data in OMAP24XX DMA mode
-	 */
-	if (cpu_is_omap24xx() && packets < ep->maxpacket) {
-		omap_set_dma_transfer_params(ep->lch, OMAP_DMA_DATA_TYPE_S8,
-				packets, 1, OMAP_DMA_SYNC_ELEMENT,
-				dma_trigger, 0);
-		req->dma_bytes = packets;
-	} else {
-		/* set up this DMA transfer, enable the fifo, start */
-		packets /= ep->ep.maxpacket;
-		packets = min(packets, (unsigned)UDC_RXN_TC + 1);
-		req->dma_bytes = packets * ep->ep.maxpacket;
-		omap_set_dma_transfer_params(ep->lch, OMAP_DMA_DATA_TYPE_S16,
-				ep->ep.maxpacket >> 1, packets,
-				OMAP_DMA_SYNC_ELEMENT,
-				dma_trigger, 0);
-	}
+	/* set up this DMA transfer, enable the fifo, start */
+	packets /= ep->ep.maxpacket;
+	packets = min(packets, (unsigned)UDC_RXN_TC + 1);
+	req->dma_bytes = packets * ep->ep.maxpacket;
+	omap_set_dma_transfer_params(ep->lch, OMAP_DMA_DATA_TYPE_S16,
+			ep->ep.maxpacket >> 1, packets,
+			OMAP_DMA_SYNC_ELEMENT,
+			dma_trigger, 0);
 	omap_set_dma_dest_params(ep->lch, OMAP_DMA_PORT_EMIFF,
 		OMAP_DMA_AMODE_POST_INC, req->req.dma + req->req.actual,
 		0, 0);
@@ -683,7 +650,7 @@ static void dma_irq(struct omap_udc *udc, u16 irq_src)
 		}
 		omap_writew(UDC_TXN_DONE, UDC_IRQ_SRC);
 
-		if (!list_empty (&ep->queue)) {
+		if (!list_empty(&ep->queue)) {
 			req = container_of(ep->queue.next,
 					struct omap_req, queue);
 			next_in_dma(ep, req);
@@ -702,7 +669,7 @@ static void dma_irq(struct omap_udc *udc, u16 irq_src)
 		}
 		omap_writew(UDC_RXN_EOT, UDC_IRQ_SRC);
 
-		if (!list_empty (&ep->queue)) {
+		if (!list_empty(&ep->queue)) {
 			req = container_of(ep->queue.next,
 					struct omap_req, queue);
 			next_out_dma(ep, req);
@@ -760,10 +727,7 @@ static void dma_channel_claim(struct omap_ep *ep, unsigned channel)
 	ep->dma_channel = channel;
 
 	if (is_in) {
-		if (cpu_is_omap24xx())
-			dma_channel = OMAP24XX_DMA(USB_W2FC_TX0, channel);
-		else
-			dma_channel = OMAP_DMA_USB_W2FC_TX0 - 1 + channel;
+		dma_channel = OMAP_DMA_USB_W2FC_TX0 - 1 + channel;
 		status = omap_request_dma(dma_channel,
 			ep->ep.name, dma_error, ep, &ep->lch);
 		if (status == 0) {
@@ -780,11 +744,7 @@ static void dma_channel_claim(struct omap_ep *ep, unsigned channel)
 				0, 0);
 		}
 	} else {
-		if (cpu_is_omap24xx())
-			dma_channel = OMAP24XX_DMA(USB_W2FC_RX0, channel);
-		else
-			dma_channel = OMAP_DMA_USB_W2FC_RX0 - 1 + channel;
-
+		dma_channel = OMAP_DMA_USB_W2FC_RX0 - 1 + channel;
 		status = omap_request_dma(dma_channel,
 			ep->ep.name, dma_error, ep, &ep->lch);
 		if (status == 0) {
@@ -808,7 +768,7 @@ static void dma_channel_claim(struct omap_ep *ep, unsigned channel)
 		omap_disable_dma_irq(ep->lch, OMAP_DMA_BLOCK_IRQ);
 
 		/* channel type P: hw synch (fifo) */
-		if (cpu_class_is_omap1() && !cpu_is_omap15xx())
+		if (!cpu_is_omap15xx())
 			omap_set_dma_channel_mode(ep->lch, OMAP_DMA_LCH_P);
 	}
 
@@ -928,13 +888,11 @@ omap_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 
 	/* this isn't bogus, but OMAP DMA isn't the only hardware to
 	 * have a hard time with partial packet reads...  reject it.
-	 * Except OMAP2 can handle the small packets.
 	 */
 	if (use_dma
 			&& ep->has_dma
 			&& ep->bEndpointAddress != 0
 			&& (ep->bEndpointAddress & USB_DIR_IN) == 0
-			&& !cpu_class_is_omap2()
 			&& (req->req.length % ep->ep.maxpacket) != 0) {
 		DBG("%s, no partial packet OUT reads\n", __func__);
 		return -EMSGSIZE;
@@ -944,26 +902,9 @@ omap_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)
 		return -ESHUTDOWN;
 
-	if (use_dma && ep->has_dma) {
-		if (req->req.dma == DMA_ADDR_INVALID) {
-			req->req.dma = dma_map_single(
-				ep->udc->gadget.dev.parent,
-				req->req.buf,
-				req->req.length,
-				(ep->bEndpointAddress & USB_DIR_IN)
-					? DMA_TO_DEVICE
-					: DMA_FROM_DEVICE);
-			req->mapped = 1;
-		} else {
-			dma_sync_single_for_device(
-				ep->udc->gadget.dev.parent,
-				req->req.dma, req->req.length,
-				(ep->bEndpointAddress & USB_DIR_IN)
-					? DMA_TO_DEVICE
-					: DMA_FROM_DEVICE);
-			req->mapped = 0;
-		}
-	}
+	if (use_dma && ep->has_dma)
+		usb_gadget_map_request(&udc->gadget, &req->req,
+				(ep->bEndpointAddress & USB_DIR_IN));
 
 	VDBG("%s queue req %p, len %d buf %p\n",
 		ep->ep.name, _req, _req->length, _req->buf);
@@ -984,7 +925,7 @@ omap_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 		int	is_in;
 
 		if (ep->bEndpointAddress == 0) {
-			if (!udc->ep0_pending || !list_empty (&ep->queue)) {
+			if (!udc->ep0_pending || !list_empty(&ep->queue)) {
 				spin_unlock_irqrestore(&udc->lock, flags);
 				return -EL2HLT;
 			}
@@ -1011,7 +952,8 @@ omap_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 				 * always an IN ... even for IN transfers,
 				 * a weird case which seem to stall OMAP.
 				 */
-				omap_writew(UDC_EP_SEL | UDC_EP_DIR, UDC_EP_NUM);
+				omap_writew(UDC_EP_SEL | UDC_EP_DIR,
+						UDC_EP_NUM);
 				omap_writew(UDC_CLR_EP, UDC_CTRL);
 				omap_writew(UDC_SET_FIFO_EN, UDC_CTRL);
 				omap_writew(UDC_EP_DIR, UDC_EP_NUM);
@@ -1023,7 +965,8 @@ omap_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 
 			/* non-empty DATA stage */
 			} else if (is_in) {
-				omap_writew(UDC_EP_SEL | UDC_EP_DIR, UDC_EP_NUM);
+				omap_writew(UDC_EP_SEL | UDC_EP_DIR,
+						UDC_EP_NUM);
 			} else {
 				if (udc->ep0_setup)
 					goto irq_wait;
@@ -1071,7 +1014,7 @@ static int omap_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	spin_lock_irqsave(&ep->udc->lock, flags);
 
 	/* make sure it's actually queued on this endpoint */
-	list_for_each_entry (req, &ep->queue, queue) {
+	list_for_each_entry(req, &ep->queue, queue) {
 		if (&req->req == _req)
 			break;
 	}
@@ -1178,8 +1121,8 @@ static struct usb_ep_ops omap_ep_ops = {
 	.dequeue	= omap_ep_dequeue,
 
 	.set_halt	= omap_ep_set_halt,
-	// fifo_status ... report bytes in fifo
-	// fifo_flush ... flush fifo
+	/* fifo_status ... report bytes in fifo */
+	/* fifo_flush ... flush fifo */
 };
 
 /*-------------------------------------------------------------------------*/
@@ -1211,7 +1154,7 @@ static int omap_wakeup(struct usb_gadget *gadget)
 
 	/* NOTE:  non-OTG systems may use SRP TOO... */
 	} else if (!(udc->devstat & UDC_ATT)) {
-		if (udc->transceiver)
+		if (!IS_ERR_OR_NULL(udc->transceiver))
 			retval = otg_start_srp(udc->transceiver->otg);
 	}
 	spin_unlock_irqrestore(&udc->lock, flags);
@@ -1343,7 +1286,7 @@ static int omap_vbus_draw(struct usb_gadget *gadget, unsigned mA)
 	struct omap_udc	*udc;
 
 	udc = container_of(gadget, struct omap_udc, gadget);
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return usb_phy_set_power(udc->transceiver, mA);
 	return -EOPNOTSUPP;
 }
@@ -1409,7 +1352,7 @@ static void udc_quiesce(struct omap_udc *udc)
 
 	udc->gadget.speed = USB_SPEED_UNKNOWN;
 	nuke(&udc->ep[0], -ESHUTDOWN);
-	list_for_each_entry (ep, &udc->gadget.ep_list, ep.ep_list)
+	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list)
 		nuke(ep, -ESHUTDOWN);
 }
 
@@ -1525,7 +1468,8 @@ static void ep0_irq(struct omap_udc *udc, u16 irq_src)
 				/* read next OUT packet of request, maybe
 				 * reactiviting the fifo; stall on errors.
 				 */
-				if (!req || (stat = read_fifo(ep0, req)) < 0) {
+				stat = read_fifo(ep0, req);
+				if (!req || stat < 0) {
 					omap_writew(UDC_STALL_CMD, UDC_SYSCON2);
 					udc->ep0_pending = 0;
 					stat = 0;
@@ -1658,7 +1602,7 @@ static void ep0_irq(struct omap_udc *udc, u16 irq_src)
 				/* this has rude side-effects (aborts) and
 				 * can't really work if DMA-IN is active
 				 */
-				DBG("%s host set_halt, NYET \n", ep->name);
+				DBG("%s host set_halt, NYET\n", ep->name);
 				goto do_stall;
 			}
 			use_ep(ep, 0);
@@ -1749,7 +1693,7 @@ delegate:
 			 */
 			udc->ep0_setup = 1;
 			spin_unlock(&udc->lock);
-			status = udc->driver->setup (&udc->gadget, &u.r);
+			status = udc->driver->setup(&udc->gadget, &u.r);
 			spin_lock(&udc->lock);
 			udc->ep0_setup = 0;
 		}
@@ -1792,12 +1736,12 @@ static void devstate_irq(struct omap_udc *udc, u16 irq_src)
 			if (devstat & UDC_ATT) {
 				udc->gadget.speed = USB_SPEED_FULL;
 				VDBG("connect\n");
-				if (!udc->transceiver)
+				if (IS_ERR_OR_NULL(udc->transceiver))
 					pullup_enable(udc);
-				// if (driver->connect) call it
+				/* if (driver->connect) call it */
 			} else if (udc->gadget.speed != USB_SPEED_UNKNOWN) {
 				udc->gadget.speed = USB_SPEED_UNKNOWN;
-				if (!udc->transceiver)
+				if (IS_ERR_OR_NULL(udc->transceiver))
 					pullup_disable(udc);
 				DBG("disconnect, gadget %s\n",
 					udc->driver->driver.name);
@@ -1826,7 +1770,7 @@ static void devstate_irq(struct omap_udc *udc, u16 irq_src)
 	}
 	if (change & UDC_SUS) {
 		if (udc->gadget.speed != USB_SPEED_UNKNOWN) {
-			// FIXME tell isp1301 to suspend/resume (?)
+			/* FIXME tell isp1301 to suspend/resume (?) */
 			if (devstat & UDC_SUS) {
 				VDBG("suspend\n");
 				update_otg(udc);
@@ -1837,12 +1781,12 @@ static void devstate_irq(struct omap_udc *udc, u16 irq_src)
 					udc->driver->suspend(&udc->gadget);
 					spin_lock(&udc->lock);
 				}
-				if (udc->transceiver)
+				if (!IS_ERR_OR_NULL(udc->transceiver))
 					usb_phy_set_suspend(
 							udc->transceiver, 1);
 			} else {
 				VDBG("resume\n");
-				if (udc->transceiver)
+				if (!IS_ERR_OR_NULL(udc->transceiver))
 					usb_phy_set_suspend(
 							udc->transceiver, 0);
 				if (udc->gadget.speed == USB_SPEED_FULL
@@ -2029,7 +1973,7 @@ static irqreturn_t omap_udc_iso_irq(int irq, void *_dev)
 	spin_lock_irqsave(&udc->lock, flags);
 
 	/* handle all non-DMA ISO transfers */
-	list_for_each_entry (ep, &udc->iso, iso) {
+	list_for_each_entry(ep, &udc->iso, iso) {
 		u16		stat;
 		struct omap_req	*req;
 
@@ -2088,15 +2032,11 @@ static irqreturn_t omap_udc_iso_irq(int irq, void *_dev)
 
 static inline int machine_without_vbus_sense(void)
 {
-	return (machine_is_omap_innovator()
+	return machine_is_omap_innovator()
 		|| machine_is_omap_osk()
-		|| machine_is_omap_apollon()
-#ifndef CONFIG_MACH_OMAP_H4_OTG
-		|| machine_is_omap_h4()
-#endif
 		|| machine_is_sx1()
-		|| cpu_is_omap7xx() /* No known omap7xx boards with vbus sense */
-		);
+		/* No known omap7xx boards with vbus sense */
+		|| cpu_is_omap7xx();
 }
 
 static int omap_udc_start(struct usb_gadget_driver *driver,
@@ -2110,7 +2050,7 @@ static int omap_udc_start(struct usb_gadget_driver *driver,
 	if (!udc)
 		return -ENODEV;
 	if (!driver
-			// FIXME if otg, check:  driver->is_otg
+			/* FIXME if otg, check:  driver->is_otg */
 			|| driver->max_speed < USB_SPEED_FULL
 			|| !bind || !driver->setup)
 		return -EINVAL;
@@ -2122,7 +2062,7 @@ static int omap_udc_start(struct usb_gadget_driver *driver,
 	}
 
 	/* reset state */
-	list_for_each_entry (ep, &udc->gadget.ep_list, ep.ep_list) {
+	list_for_each_entry(ep, &udc->gadget.ep_list, ep.ep_list) {
 		ep->irqs = 0;
 		if (ep->bmAttributes == USB_ENDPOINT_XFER_ISOC)
 			continue;
@@ -2154,13 +2094,13 @@ static int omap_udc_start(struct usb_gadget_driver *driver,
 	omap_writew(UDC_IRQ_SRC_MASK, UDC_IRQ_SRC);
 
 	/* connect to bus through transceiver */
-	if (udc->transceiver) {
+	if (!IS_ERR_OR_NULL(udc->transceiver)) {
 		status = otg_set_peripheral(udc->transceiver->otg,
 						&udc->gadget);
 		if (status < 0) {
 			ERR("can't bind to transceiver\n");
 			if (driver->unbind) {
-				driver->unbind (&udc->gadget);
+				driver->unbind(&udc->gadget);
 				udc->gadget.dev.driver = NULL;
 				udc->driver = NULL;
 			}
@@ -2168,9 +2108,9 @@ static int omap_udc_start(struct usb_gadget_driver *driver,
 		}
 	} else {
 		if (can_pullup(udc))
-			pullup_enable (udc);
+			pullup_enable(udc);
 		else
-			pullup_disable (udc);
+			pullup_disable(udc);
 	}
 
 	/* boards that don't have VBUS sensing can't autogate 48MHz;
@@ -2201,7 +2141,7 @@ static int omap_udc_stop(struct usb_gadget_driver *driver)
 	if (machine_without_vbus_sense())
 		omap_vbus_session(&udc->gadget, 0);
 
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		(void) otg_set_peripheral(udc->transceiver->otg, NULL);
 	else
 		pullup_disable(udc);
@@ -2229,7 +2169,7 @@ static int omap_udc_stop(struct usb_gadget_driver *driver)
 static const char proc_filename[] = "driver/udc";
 
 #define FOURBITS "%s%s%s%s"
-#define EIGHTBITS FOURBITS FOURBITS
+#define EIGHTBITS "%s%s%s%s%s%s%s%s"
 
 static void proc_ep_show(struct seq_file *s, struct omap_ep *ep)
 {
@@ -2251,12 +2191,21 @@ static void proc_ep_show(struct seq_file *s, struct omap_ep *ep)
 		"\n%s %s%s%sirqs %ld stat %04x " EIGHTBITS FOURBITS "%s\n",
 		ep->name, buf,
 		ep->double_buf ? "dbuf " : "",
-		({char *s; switch(ep->ackwait){
-		case 0: s = ""; break;
-		case 1: s = "(ackw) "; break;
-		case 2: s = "(ackw2) "; break;
-		default: s = "(?) "; break;
-		} s;}),
+		({ char *s;
+		switch (ep->ackwait) {
+		case 0:
+			s = "";
+			break;
+		case 1:
+			s = "(ackw) ";
+			break;
+		case 2:
+			s = "(ackw2) ";
+			break;
+		default:
+			s = "(?) ";
+			break;
+		} s; }),
 		ep->irqs, stat_flg,
 		(stat_flg & UDC_NO_RXPACKET) ? "no_rxpacket " : "",
 		(stat_flg & UDC_MISS_IN) ? "miss_in " : "",
@@ -2272,10 +2221,10 @@ static void proc_ep_show(struct seq_file *s, struct omap_ep *ep)
 		(stat_flg & UDC_NON_ISO_FIFO_EMPTY) ? "fifo_empty " : "",
 		(stat_flg & UDC_NON_ISO_FIFO_FULL) ? "fifo_full " : "");
 
-	if (list_empty (&ep->queue))
+	if (list_empty(&ep->queue))
 		seq_printf(s, "\t(queue empty)\n");
 	else
-		list_for_each_entry (req, &ep->queue, queue) {
+		list_for_each_entry(req, &ep->queue, queue) {
 			unsigned	length = req->req.actual;
 
 			if (use_dma && buf[0]) {
@@ -2293,11 +2242,16 @@ static void proc_ep_show(struct seq_file *s, struct omap_ep *ep)
 static char *trx_mode(unsigned m, int enabled)
 {
 	switch (m) {
-	case 0:		return enabled ? "*6wire" : "unused";
-	case 1:		return "4wire";
-	case 2:		return "3wire";
-	case 3:		return "6wire";
-	default:	return "unknown";
+	case 0:
+		return enabled ? "*6wire" : "unused";
+	case 1:
+		return "4wire";
+	case 2:
+		return "3wire";
+	case 3:
+		return "6wire";
+	default:
+		return "unknown";
 	}
 }
 
@@ -2307,12 +2261,9 @@ static int proc_otg_show(struct seq_file *s)
 	u32		trans = 0;
 	char		*ctrl_name = "(UNKNOWN)";
 
-	/* XXX This needs major revision for OMAP2+ */
 	tmp = omap_readl(OTG_REV);
-	if (cpu_class_is_omap1()) {
-		ctrl_name = "tranceiver_ctrl";
-		trans = omap_readw(USB_TRANSCEIVER_CTRL);
-	}
+	ctrl_name = "tranceiver_ctrl";
+	trans = omap_readw(USB_TRANSCEIVER_CTRL);
 	seq_printf(s, "\nOTG rev %d.%d, %s %05x\n",
 		tmp >> 4, tmp & 0xf, ctrl_name, trans);
 	tmp = omap_readw(OTG_SYSCON_1);
@@ -2332,7 +2283,7 @@ static int proc_otg_show(struct seq_file *s)
 			" b_ase_brst=%d hmc=%d\n", tmp,
 		(tmp & OTG_EN) ? " otg_en" : "",
 		(tmp & USBX_SYNCHRO) ? " synchro" : "",
-		// much more SRP stuff
+		/* much more SRP stuff */
 		(tmp & SRP_DATA) ? " srp_data" : "",
 		(tmp & SRP_VBUS) ? " srp_vbus" : "",
 		(tmp & OTG_PADEN) ? " otg_paden" : "",
@@ -2399,14 +2350,12 @@ static int proc_udc_show(struct seq_file *s, void *_)
 		HMC,
 		udc->transceiver
 			? udc->transceiver->label
-			: ((cpu_is_omap1710() || cpu_is_omap24xx())
+			: (cpu_is_omap1710()
 				? "external" : "(none)"));
-	if (cpu_class_is_omap1()) {
-		seq_printf(s, "ULPD control %04x req %04x status %04x\n",
-			omap_readw(ULPD_CLOCK_CTRL),
-			omap_readw(ULPD_SOFT_REQ),
-			omap_readw(ULPD_STATUS_REQ));
-	}
+	seq_printf(s, "ULPD control %04x req %04x status %04x\n",
+		omap_readw(ULPD_CLOCK_CTRL),
+		omap_readw(ULPD_SOFT_REQ),
+		omap_readw(ULPD_STATUS_REQ));
 
 	/* OTG controller registers */
 	if (!cpu_is_omap15xx())
@@ -2422,7 +2371,7 @@ static int proc_udc_show(struct seq_file *s, void *_)
 		(tmp & UDC_SELF_PWR) ? " self_pwr" : "",
 		(tmp & UDC_SOFF_DIS) ? " soff_dis" : "",
 		(tmp & UDC_PULLUP_EN) ? " PULLUP" : "");
-	// syscon2 is write-only
+	/* syscon2 is write-only */
 
 	/* UDC controller registers */
 	if (!(tmp & UDC_PULLUP_EN)) {
@@ -2506,7 +2455,7 @@ static int proc_udc_show(struct seq_file *s, void *_)
 	if (tmp & UDC_ATT) {
 		proc_ep_show(s, &udc->ep[0]);
 		if (tmp & UDC_ADD) {
-			list_for_each_entry (ep, &udc->gadget.ep_list,
+			list_for_each_entry(ep, &udc->gadget.ep_list,
 					ep.ep_list) {
 				if (ep->ep.desc)
 					proc_ep_show(s, ep);
@@ -2557,7 +2506,7 @@ static inline void remove_proc_file(void) {}
  * UDC_SYSCON_1.CFG_LOCK is set can now work.  We won't use that
  * capability yet though.
  */
-static unsigned __init
+static unsigned __devinit
 omap_ep_setup(char *name, u8 addr, u8 type,
 		unsigned buf, unsigned maxp, int dbuf)
 {
@@ -2575,14 +2524,29 @@ omap_ep_setup(char *name, u8 addr, u8 type,
 	/* chip setup ... bit values are same for IN, OUT */
 	if (type == USB_ENDPOINT_XFER_ISOC) {
 		switch (maxp) {
-		case 8:		epn_rxtx = 0 << 12; break;
-		case 16:	epn_rxtx = 1 << 12; break;
-		case 32:	epn_rxtx = 2 << 12; break;
-		case 64:	epn_rxtx = 3 << 12; break;
-		case 128:	epn_rxtx = 4 << 12; break;
-		case 256:	epn_rxtx = 5 << 12; break;
-		case 512:	epn_rxtx = 6 << 12; break;
-		default:	BUG();
+		case 8:
+			epn_rxtx = 0 << 12;
+			break;
+		case 16:
+			epn_rxtx = 1 << 12;
+			break;
+		case 32:
+			epn_rxtx = 2 << 12;
+			break;
+		case 64:
+			epn_rxtx = 3 << 12;
+			break;
+		case 128:
+			epn_rxtx = 4 << 12;
+			break;
+		case 256:
+			epn_rxtx = 5 << 12;
+			break;
+		case 512:
+			epn_rxtx = 6 << 12;
+			break;
+		default:
+			BUG();
 		}
 		epn_rxtx |= UDC_EPN_RX_ISO;
 		dbuf = 1;
@@ -2591,15 +2555,24 @@ omap_ep_setup(char *name, u8 addr, u8 type,
 		 * and ignored for PIO-IN on newer chips
 		 * (for more reliable behavior)
 		 */
-		if (!use_dma || cpu_is_omap15xx() || cpu_is_omap24xx())
+		if (!use_dma || cpu_is_omap15xx())
 			dbuf = 0;
 
 		switch (maxp) {
-		case 8:		epn_rxtx = 0 << 12; break;
-		case 16:	epn_rxtx = 1 << 12; break;
-		case 32:	epn_rxtx = 2 << 12; break;
-		case 64:	epn_rxtx = 3 << 12; break;
-		default:	BUG();
+		case 8:
+			epn_rxtx = 0 << 12;
+			break;
+		case 16:
+			epn_rxtx = 1 << 12;
+			break;
+		case 32:
+			epn_rxtx = 2 << 12;
+			break;
+		case 64:
+			epn_rxtx = 3 << 12;
+			break;
+		default:
+			BUG();
 		}
 		if (dbuf && addr)
 			epn_rxtx |= UDC_EPN_RX_DB;
@@ -2639,7 +2612,7 @@ omap_ep_setup(char *name, u8 addr, u8 type,
 	ep->ep.name = ep->name;
 	ep->ep.ops = &omap_ep_ops;
 	ep->ep.maxpacket = ep->maxpacket = maxp;
-	list_add_tail (&ep->ep.ep_list, &udc->gadget.ep_list);
+	list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
 
 	return buf;
 }
@@ -2647,11 +2620,11 @@ omap_ep_setup(char *name, u8 addr, u8 type,
 static void omap_udc_release(struct device *dev)
 {
 	complete(udc->done);
-	kfree (udc);
+	kfree(udc);
 	udc = NULL;
 }
 
-static int __init
+static int __devinit
 omap_udc_setup(struct platform_device *odev, struct usb_phy *xceiv)
 {
 	unsigned	tmp, buf;
@@ -2665,13 +2638,13 @@ omap_udc_setup(struct platform_device *odev, struct usb_phy *xceiv)
 	omap_writew(0, UDC_TXDMA_CFG);
 
 	/* UDC_PULLUP_EN gates the chip clock */
-	// OTG_SYSCON_1 |= DEV_IDLE_EN;
+	/* OTG_SYSCON_1 |= DEV_IDLE_EN; */
 
 	udc = kzalloc(sizeof(*udc), GFP_KERNEL);
 	if (!udc)
 		return -ENOMEM;
 
-	spin_lock_init (&udc->lock);
+	spin_lock_init(&udc->lock);
 
 	udc->gadget.ops = &omap_gadget_ops;
 	udc->gadget.ep0 = &udc->ep[0].ep;
@@ -2701,13 +2674,13 @@ omap_udc_setup(struct platform_device *odev, struct usb_phy *xceiv)
 		omap_writew(0, UDC_EP_TX(tmp));
 	}
 
-#define OMAP_BULK_EP(name,addr) \
+#define OMAP_BULK_EP(name, addr) \
 	buf = omap_ep_setup(name "-bulk", addr, \
 			USB_ENDPOINT_XFER_BULK, buf, 64, 1);
-#define OMAP_INT_EP(name,addr, maxp) \
+#define OMAP_INT_EP(name, addr, maxp) \
 	buf = omap_ep_setup(name "-int", addr, \
 			USB_ENDPOINT_XFER_INT, buf, maxp, 0);
-#define OMAP_ISO_EP(name,addr, maxp) \
+#define OMAP_ISO_EP(name, addr, maxp) \
 	buf = omap_ep_setup(name "-iso", addr, \
 			USB_ENDPOINT_XFER_ISOC, buf, maxp, 1);
 
@@ -2788,15 +2761,18 @@ omap_udc_setup(struct platform_device *odev, struct usb_phy *xceiv)
 	return 0;
 }
 
-static int __init omap_udc_probe(struct platform_device *pdev)
+static int __devinit omap_udc_probe(struct platform_device *pdev)
 {
 	int			status = -ENODEV;
 	int			hmc;
 	struct usb_phy		*xceiv = NULL;
 	const char		*type = NULL;
 	struct omap_usb_config	*config = pdev->dev.platform_data;
-	struct clk		*dc_clk;
-	struct clk		*hhc_clk;
+	struct clk		*dc_clk = NULL;
+	struct clk		*hhc_clk = NULL;
+
+	if (cpu_is_omap7xx())
+		use_dma = 0;
 
 	/* NOTE:  "knows" the order of the resources! */
 	if (!request_mem_region(pdev->resource[0].start,
@@ -2816,16 +2792,6 @@ static int __init omap_udc_probe(struct platform_device *pdev)
 		udelay(100);
 	}
 
-	if (cpu_is_omap24xx()) {
-		dc_clk = clk_get(&pdev->dev, "usb_fck");
-		hhc_clk = clk_get(&pdev->dev, "usb_l4_ick");
-		BUG_ON(IS_ERR(dc_clk) || IS_ERR(hhc_clk));
-		/* can't use omap_udc_enable_clock yet */
-		clk_enable(dc_clk);
-		clk_enable(hhc_clk);
-		udelay(100);
-	}
-
 	if (cpu_is_omap7xx()) {
 		dc_clk = clk_get(&pdev->dev, "usb_dc_ck");
 		hhc_clk = clk_get(&pdev->dev, "l3_ocpi_ck");
@@ -2865,8 +2831,8 @@ static int __init omap_udc_probe(struct platform_device *pdev)
 		 * use it.  Except for OTG, we don't _need_ to talk to one;
 		 * but not having one probably means no VBUS detection.
 		 */
-		xceiv = usb_get_transceiver();
-		if (xceiv)
+		xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+		if (!IS_ERR_OR_NULL(xceiv))
 			type = xceiv->label;
 		else if (config->otg) {
 			DBG("OTG requires external transceiver!\n");
@@ -2875,14 +2841,6 @@ static int __init omap_udc_probe(struct platform_device *pdev)
 
 		hmc = HMC_1610;
 
-		if (cpu_is_omap24xx()) {
-			/* this could be transceiverless in one of the
-			 * "we don't need to know" modes.
-			 */
-			type = "external";
-			goto known;
-		}
-
 		switch (hmc) {
 		case 0:			/* POWERUP DEFAULT == 0 */
 		case 4:
@@ -2898,7 +2856,7 @@ static int __init omap_udc_probe(struct platform_device *pdev)
 		case 16:
 		case 19:
 		case 25:
-			if (!xceiv) {
+			if (IS_ERR_OR_NULL(xceiv)) {
 				DBG("external transceiver not registered!\n");
 				type = "unknown";
 			}
@@ -2921,16 +2879,16 @@ bad_on_1710:
 			goto cleanup0;
 		}
 	}
-known:
+
 	INFO("hmc mode %d, %s transceiver\n", hmc, type);
 
 	/* a "gadget" abstracts/virtualizes the controller */
 	status = omap_udc_setup(pdev, xceiv);
-	if (status) {
+	if (status)
 		goto cleanup0;
-	}
+
 	xceiv = NULL;
-	// "udc" is now valid
+	/* "udc" is now valid */
 	pullup_disable(udc);
 #if	defined(CONFIG_USB_OHCI_HCD) || defined(CONFIG_USB_OHCI_HCD_MODULE)
 	udc->gadget.is_otg = (config->otg != 0);
@@ -2944,7 +2902,7 @@ known:
 
 	/* USB general purpose IRQ:  ep0, state changes, dma, etc */
 	status = request_irq(pdev->resource[1].start, omap_udc_irq,
-			IRQF_SAMPLE_RANDOM, driver_name, udc);
+			0, driver_name, udc);
 	if (status != 0) {
 		ERR("can't get irq %d, err %d\n",
 			(int) pdev->resource[1].start, status);
@@ -2953,7 +2911,7 @@ known:
 
 	/* USB "non-iso" IRQ (PIO for all but ep0) */
 	status = request_irq(pdev->resource[2].start, omap_udc_pio_irq,
-			IRQF_SAMPLE_RANDOM, "omap_udc pio", udc);
+			0, "omap_udc pio", udc);
 	if (status != 0) {
 		ERR("can't get irq %d, err %d\n",
 			(int) pdev->resource[2].start, status);
@@ -2975,16 +2933,6 @@ known:
 		clk_disable(dc_clk);
 	}
 
-	if (cpu_is_omap24xx()) {
-		udc->dc_clk = dc_clk;
-		udc->hhc_clk = hhc_clk;
-		/* FIXME OMAP2 don't release hhc & dc clock */
-#if 0
-		clk_disable(hhc_clk);
-		clk_disable(dc_clk);
-#endif
-	}
-
 	create_proc_file();
 	status = device_add(&udc->gadget.dev);
 	if (status)
@@ -3006,14 +2954,14 @@ cleanup2:
 	free_irq(pdev->resource[1].start, udc);
 
 cleanup1:
-	kfree (udc);
+	kfree(udc);
 	udc = NULL;
 
 cleanup0:
-	if (xceiv)
-		usb_put_transceiver(xceiv);
+	if (!IS_ERR_OR_NULL(xceiv))
+		usb_put_phy(xceiv);
 
-	if (cpu_is_omap16xx() || cpu_is_omap24xx() || cpu_is_omap7xx()) {
+	if (cpu_is_omap16xx() || cpu_is_omap7xx()) {
 		clk_disable(hhc_clk);
 		clk_disable(dc_clk);
 		clk_put(hhc_clk);
@@ -3026,7 +2974,7 @@ cleanup0:
 	return status;
 }
 
-static int __exit omap_udc_remove(struct platform_device *pdev)
+static int __devexit omap_udc_remove(struct platform_device *pdev)
 {
 	DECLARE_COMPLETION_ONSTACK(done);
 
@@ -3040,8 +2988,8 @@ static int __exit omap_udc_remove(struct platform_device *pdev)
 	udc->done = &done;
 
 	pullup_disable(udc);
-	if (udc->transceiver) {
-		usb_put_transceiver(udc->transceiver);
+	if (!IS_ERR_OR_NULL(udc->transceiver)) {
+		usb_put_phy(udc->transceiver);
 		udc->transceiver = NULL;
 	}
 	omap_writew(0, UDC_SYSCON1);
@@ -3111,7 +3059,8 @@ static int omap_udc_resume(struct platform_device *dev)
 /*-------------------------------------------------------------------------*/
 
 static struct platform_driver udc_driver = {
-	.remove		= __exit_p(omap_udc_remove),
+	.probe		= omap_udc_probe,
+	.remove		= __devexit_p(omap_udc_remove),
 	.suspend	= omap_udc_suspend,
 	.resume		= omap_udc_resume,
 	.driver		= {
@@ -3120,27 +3069,7 @@ static struct platform_driver udc_driver = {
 	},
 };
 
-static int __init udc_init(void)
-{
-	/* Disable DMA for omap7xx -- it doesn't work right. */
-	if (cpu_is_omap7xx())
-		use_dma = 0;
-
-	INFO("%s, version: " DRIVER_VERSION
-#ifdef	USE_ISO
-		" (iso)"
-#endif
-		"%s\n", driver_desc,
-		use_dma ?  " (dma)" : "");
-	return platform_driver_probe(&udc_driver, omap_udc_probe);
-}
-module_init(udc_init);
-
-static void __exit udc_exit(void)
-{
-	platform_driver_unregister(&udc_driver);
-}
-module_exit(udc_exit);
+module_platform_driver(udc_driver);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/pch_udc.c b/drivers/usb/gadget/pch_udc.c
index 1cfcc9ecbfb..f4fb71c9ae0 100644
--- a/drivers/usb/gadget/pch_udc.c
+++ b/drivers/usb/gadget/pch_udc.c
@@ -2208,7 +2208,7 @@ static void pch_udc_complete_receiver(struct pch_udc_ep *ep)
 			return;
 		}
 		if ((td->status & PCH_UDC_BUFF_STS) == PCH_UDC_BS_DMA_DONE)
-			if (td->status | PCH_UDC_DMA_LAST) {
+			if (td->status & PCH_UDC_DMA_LAST) {
 				count = td->status & PCH_UDC_RXTX_BYTES;
 				break;
 			}
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index f7ff9e8e746..907ad3ecb34 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -21,6 +21,7 @@
 #include <linux/ioport.h>
 #include <linux/types.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/init.h>
@@ -993,7 +994,7 @@ static int pxa25x_udc_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
 
 	udc = container_of(_gadget, struct pxa25x_udc, gadget);
 
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return usb_phy_set_power(udc->transceiver, mA);
 	return -EOPNOTSUPP;
 }
@@ -1299,7 +1300,7 @@ fail:
 	DMSG("registered gadget driver '%s'\n", driver->driver.name);
 
 	/* connect to bus through transceiver */
-	if (dev->transceiver) {
+	if (!IS_ERR_OR_NULL(dev->transceiver)) {
 		retval = otg_set_peripheral(dev->transceiver->otg,
 						&dev->gadget);
 		if (retval) {
@@ -1359,7 +1360,7 @@ static int pxa25x_stop(struct usb_gadget_driver *driver)
 	stop_activity(dev, driver);
 	local_irq_enable();
 
-	if (dev->transceiver)
+	if (!IS_ERR_OR_NULL(dev->transceiver))
 		(void) otg_set_peripheral(dev->transceiver->otg, NULL);
 
 	driver->unbind(&dev->gadget);
@@ -2159,7 +2160,7 @@ static int __init pxa25x_udc_probe(struct platform_device *pdev)
 	dev->dev = &pdev->dev;
 	dev->mach = pdev->dev.platform_data;
 
-	dev->transceiver = usb_get_transceiver();
+	dev->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 
 	if (gpio_is_valid(dev->mach->gpio_pullup)) {
 		if ((retval = gpio_request(dev->mach->gpio_pullup,
@@ -2200,19 +2201,15 @@ static int __init pxa25x_udc_probe(struct platform_device *pdev)
 
 #ifdef CONFIG_ARCH_LUBBOCK
 	if (machine_is_lubbock()) {
-		retval = request_irq(LUBBOCK_USB_DISC_IRQ,
-				lubbock_vbus_irq,
-				IRQF_SAMPLE_RANDOM,
-				driver_name, dev);
+		retval = request_irq(LUBBOCK_USB_DISC_IRQ, lubbock_vbus_irq,
+				     0, driver_name, dev);
 		if (retval != 0) {
 			pr_err("%s: can't get irq %i, err %d\n",
 				driver_name, LUBBOCK_USB_DISC_IRQ, retval);
 			goto err_irq_lub;
 		}
-		retval = request_irq(LUBBOCK_USB_IRQ,
-				lubbock_vbus_irq,
-				IRQF_SAMPLE_RANDOM,
-				driver_name, dev);
+		retval = request_irq(LUBBOCK_USB_IRQ, lubbock_vbus_irq,
+				     0, driver_name, dev);
 		if (retval != 0) {
 			pr_err("%s: can't get irq %i, err %d\n",
 				driver_name, LUBBOCK_USB_IRQ, retval);
@@ -2237,8 +2234,8 @@ lubbock_fail0:
 	if (gpio_is_valid(dev->mach->gpio_pullup))
 		gpio_free(dev->mach->gpio_pullup);
  err_gpio_pullup:
-	if (dev->transceiver) {
-		usb_put_transceiver(dev->transceiver);
+	if (!IS_ERR_OR_NULL(dev->transceiver)) {
+		usb_put_phy(dev->transceiver);
 		dev->transceiver = NULL;
 	}
 	clk_put(dev->clk);
@@ -2279,8 +2276,8 @@ static int __exit pxa25x_udc_remove(struct platform_device *pdev)
 
 	clk_put(dev->clk);
 
-	if (dev->transceiver) {
-		usb_put_transceiver(dev->transceiver);
+	if (!IS_ERR_OR_NULL(dev->transceiver)) {
+		usb_put_phy(dev->transceiver);
 		dev->transceiver = NULL;
 	}
 
diff --git a/drivers/usb/gadget/pxa27x_udc.c b/drivers/usb/gadget/pxa27x_udc.c
index 98acb3ab9e1..644b4305cb9 100644
--- a/drivers/usb/gadget/pxa27x_udc.c
+++ b/drivers/usb/gadget/pxa27x_udc.c
@@ -13,6 +13,7 @@
 #include <linux/kernel.h>
 #include <linux/types.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/list.h>
@@ -1573,7 +1574,7 @@ static int should_enable_udc(struct pxa_udc *udc)
 	int put_on;
 
 	put_on = ((udc->pullup_on) && (udc->driver));
-	put_on &= ((udc->vbus_sensed) || (!udc->transceiver));
+	put_on &= ((udc->vbus_sensed) || (IS_ERR_OR_NULL(udc->transceiver)));
 	return put_on;
 }
 
@@ -1594,7 +1595,7 @@ static int should_disable_udc(struct pxa_udc *udc)
 	int put_off;
 
 	put_off = ((!udc->pullup_on) || (!udc->driver));
-	put_off |= ((!udc->vbus_sensed) && (udc->transceiver));
+	put_off |= ((!udc->vbus_sensed) && (!IS_ERR_OR_NULL(udc->transceiver)));
 	return put_off;
 }
 
@@ -1665,7 +1666,7 @@ static int pxa_udc_vbus_draw(struct usb_gadget *_gadget, unsigned mA)
 	struct pxa_udc *udc;
 
 	udc = to_gadget_udc(_gadget);
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return usb_phy_set_power(udc->transceiver, mA);
 	return -EOPNOTSUPP;
 }
@@ -1834,7 +1835,7 @@ static int pxa27x_udc_start(struct usb_gadget_driver *driver,
 	dev_dbg(udc->dev, "registered gadget driver '%s'\n",
 		driver->driver.name);
 
-	if (udc->transceiver) {
+	if (!IS_ERR_OR_NULL(udc->transceiver)) {
 		retval = otg_set_peripheral(udc->transceiver->otg,
 						&udc->gadget);
 		if (retval) {
@@ -1908,7 +1909,7 @@ static int pxa27x_udc_stop(struct usb_gadget_driver *driver)
 	dev_info(udc->dev, "unregistered gadget driver '%s'\n",
 		 driver->driver.name);
 
-	if (udc->transceiver)
+	if (!IS_ERR_OR_NULL(udc->transceiver))
 		return otg_set_peripheral(udc->transceiver->otg, NULL);
 	return 0;
 }
@@ -2464,7 +2465,7 @@ static int __init pxa_udc_probe(struct platform_device *pdev)
 
 	udc->dev = &pdev->dev;
 	udc->mach = pdev->dev.platform_data;
-	udc->transceiver = usb_get_transceiver();
+	udc->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 
 	gpio = udc->mach->gpio_pullup;
 	if (gpio_is_valid(gpio)) {
@@ -2543,7 +2544,7 @@ static int __exit pxa_udc_remove(struct platform_device *_dev)
 	if (gpio_is_valid(gpio))
 		gpio_free(gpio);
 
-	usb_put_transceiver(udc->transceiver);
+	usb_put_phy(udc->transceiver);
 
 	udc->transceiver = NULL;
 	platform_set_drvdata(_dev, NULL);
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index f3ac2a20c27..5a80751accb 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -1831,12 +1831,12 @@ static int __exit r8a66597_remove(struct platform_device *pdev)
 		iounmap(r8a66597->sudmac_reg);
 	free_irq(platform_get_irq(pdev, 0), r8a66597);
 	r8a66597_free_request(&r8a66597->ep[0].ep, r8a66597->ep0_req);
-#ifdef CONFIG_HAVE_CLK
+
 	if (r8a66597->pdata->on_chip) {
 		clk_disable(r8a66597->clk);
 		clk_put(r8a66597->clk);
 	}
-#endif
+
 	device_unregister(&r8a66597->gadget.dev);
 	kfree(r8a66597);
 	return 0;
@@ -1868,9 +1868,7 @@ static int __init r8a66597_sudmac_ioremap(struct r8a66597 *r8a66597,
 
 static int __init r8a66597_probe(struct platform_device *pdev)
 {
-#ifdef CONFIG_HAVE_CLK
 	char clk_name[8];
-#endif
 	struct resource *res, *ires;
 	int irq;
 	void __iomem *reg = NULL;
@@ -1934,7 +1932,6 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 	r8a66597->timer.data = (unsigned long)r8a66597;
 	r8a66597->reg = reg;
 
-#ifdef CONFIG_HAVE_CLK
 	if (r8a66597->pdata->on_chip) {
 		snprintf(clk_name, sizeof(clk_name), "usb%d", pdev->id);
 		r8a66597->clk = clk_get(&pdev->dev, clk_name);
@@ -1946,7 +1943,7 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 		}
 		clk_enable(r8a66597->clk);
 	}
-#endif
+
 	if (r8a66597->pdata->sudmac) {
 		ret = r8a66597_sudmac_ioremap(r8a66597, pdev);
 		if (ret < 0)
@@ -2006,13 +2003,11 @@ err_add_udc:
 clean_up3:
 	free_irq(irq, r8a66597);
 clean_up2:
-#ifdef CONFIG_HAVE_CLK
 	if (r8a66597->pdata->on_chip) {
 		clk_disable(r8a66597->clk);
 		clk_put(r8a66597->clk);
 	}
 clean_up_dev:
-#endif
 	device_unregister(&r8a66597->gadget.dev);
 clean_up:
 	if (r8a66597) {
diff --git a/drivers/usb/gadget/r8a66597-udc.h b/drivers/usb/gadget/r8a66597-udc.h
index 99908c76ccd..45c4b2df178 100644
--- a/drivers/usb/gadget/r8a66597-udc.h
+++ b/drivers/usb/gadget/r8a66597-udc.h
@@ -13,10 +13,7 @@
 #ifndef __R8A66597_H__
 #define __R8A66597_H__
 
-#ifdef CONFIG_HAVE_CLK
 #include <linux/clk.h>
-#endif
-
 #include <linux/usb/r8a66597.h>
 
 #define R8A66597_MAX_SAMPLING	10
@@ -92,9 +89,7 @@ struct r8a66597 {
 	void __iomem		*reg;
 	void __iomem		*sudmac_reg;
 
-#ifdef CONFIG_HAVE_CLK
 	struct clk *clk;
-#endif
 	struct r8a66597_platdata	*pdata;
 
 	struct usb_gadget		gadget;
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/gadget/s3c-hsotg.c
index f4abb0ed987..b13e0bb5f5b 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/gadget/s3c-hsotg.c
@@ -112,7 +112,6 @@ struct s3c_hsotg_ep {
 	struct s3c_hsotg_req	*req;
 	struct dentry		*debugfs;
 
-	spinlock_t		lock;
 
 	unsigned long		total_data;
 	unsigned int		size_loaded;
@@ -136,7 +135,6 @@ struct s3c_hsotg_ep {
  * @driver: USB gadget driver
  * @plat: The platform specific configuration data.
  * @regs: The memory area mapped for accessing registers.
- * @regs_res: The resource that was allocated when claiming register space.
  * @irq: The IRQ number we are using
  * @supplies: Definition of USB power supplies
  * @dedicated_fifos: Set if the hardware has dedicated IN-EP fifos.
@@ -157,8 +155,9 @@ struct s3c_hsotg {
 	struct usb_gadget_driver *driver;
 	struct s3c_hsotg_plat	 *plat;
 
+	spinlock_t              lock;
+
 	void __iomem		*regs;
-	struct resource		*regs_res;
 	int			irq;
 	struct clk		*clk;
 
@@ -896,7 +895,6 @@ static int s3c_hsotg_ep_queue(struct usb_ep *ep, struct usb_request *req,
 	struct s3c_hsotg_req *hs_req = our_req(req);
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
 	struct s3c_hsotg *hs = hs_ep->parent;
-	unsigned long irqflags;
 	bool first;
 
 	dev_dbg(hs->dev, "%s: req %p: %d@%p, noi=%d, zero=%d, snok=%d\n",
@@ -915,19 +913,30 @@ static int s3c_hsotg_ep_queue(struct usb_ep *ep, struct usb_request *req,
 			return ret;
 	}
 
-	spin_lock_irqsave(&hs_ep->lock, irqflags);
-
 	first = list_empty(&hs_ep->queue);
 	list_add_tail(&hs_req->queue, &hs_ep->queue);
 
 	if (first)
 		s3c_hsotg_start_req(hs, hs_ep, hs_req, false);
 
-	spin_unlock_irqrestore(&hs_ep->lock, irqflags);
-
 	return 0;
 }
 
+static int s3c_hsotg_ep_queue_lock(struct usb_ep *ep, struct usb_request *req,
+			      gfp_t gfp_flags)
+{
+	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
+	struct s3c_hsotg *hs = hs_ep->parent;
+	unsigned long flags = 0;
+	int ret = 0;
+
+	spin_lock_irqsave(&hs->lock, flags);
+	ret = s3c_hsotg_ep_queue(ep, req, gfp_flags);
+	spin_unlock_irqrestore(&hs->lock, flags);
+
+	return ret;
+}
+
 static void s3c_hsotg_ep_free_request(struct usb_ep *ep,
 				      struct usb_request *req)
 {
@@ -1383,9 +1392,9 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 	 */
 
 	if (hs_req->req.complete) {
-		spin_unlock(&hs_ep->lock);
+		spin_unlock(&hsotg->lock);
 		hs_req->req.complete(&hs_ep->ep, &hs_req->req);
-		spin_lock(&hs_ep->lock);
+		spin_lock(&hsotg->lock);
 	}
 
 	/*
@@ -1404,28 +1413,6 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 }
 
 /**
- * s3c_hsotg_complete_request_lock - complete a request given to us (locked)
- * @hsotg: The device state.
- * @hs_ep: The endpoint the request was on.
- * @hs_req: The request to complete.
- * @result: The result code (0 => Ok, otherwise errno)
- *
- * See s3c_hsotg_complete_request(), but called with the endpoint's
- * lock held.
- */
-static void s3c_hsotg_complete_request_lock(struct s3c_hsotg *hsotg,
-					    struct s3c_hsotg_ep *hs_ep,
-					    struct s3c_hsotg_req *hs_req,
-					    int result)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&hs_ep->lock, flags);
-	s3c_hsotg_complete_request(hsotg, hs_ep, hs_req, result);
-	spin_unlock_irqrestore(&hs_ep->lock, flags);
-}
-
-/**
  * s3c_hsotg_rx_data - receive data from the FIFO for an endpoint
  * @hsotg: The device state.
  * @ep_idx: The endpoint index for the data
@@ -1444,6 +1431,7 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 	int max_req;
 	int read_ptr;
 
+
 	if (!hs_req) {
 		u32 epctl = readl(hsotg->regs + DOEPCTL(ep_idx));
 		int ptr;
@@ -1459,8 +1447,6 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 		return;
 	}
 
-	spin_lock(&hs_ep->lock);
-
 	to_read = size;
 	read_ptr = hs_req->req.actual;
 	max_req = hs_req->req.length - read_ptr;
@@ -1487,8 +1473,6 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 	 * alignment of the data.
 	 */
 	readsl(fifo, hs_req->req.buf + read_ptr, to_read);
-
-	spin_unlock(&hs_ep->lock);
 }
 
 /**
@@ -1609,7 +1593,7 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
 			s3c_hsotg_send_zlp(hsotg, hs_req);
 	}
 
-	s3c_hsotg_complete_request_lock(hsotg, hs_ep, hs_req, result);
+	s3c_hsotg_complete_request(hsotg, hs_ep, hs_req, result);
 }
 
 /**
@@ -1864,7 +1848,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 	/* Finish ZLP handling for IN EP0 transactions */
 	if (hsotg->eps[0].sent_zlp) {
 		dev_dbg(hsotg->dev, "zlp packet received\n");
-		s3c_hsotg_complete_request_lock(hsotg, hs_ep, hs_req, 0);
+		s3c_hsotg_complete_request(hsotg, hs_ep, hs_req, 0);
 		return;
 	}
 
@@ -1915,7 +1899,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 		dev_dbg(hsotg->dev, "%s trying more for req...\n", __func__);
 		s3c_hsotg_start_req(hsotg, hs_ep, hs_req, true);
 	} else
-		s3c_hsotg_complete_request_lock(hsotg, hs_ep, hs_req, 0);
+		s3c_hsotg_complete_request(hsotg, hs_ep, hs_req, 0);
 }
 
 /**
@@ -2123,9 +2107,6 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 			      int result, bool force)
 {
 	struct s3c_hsotg_req *req, *treq;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ep->lock, flags);
 
 	list_for_each_entry_safe(req, treq, &ep->queue, queue) {
 		/*
@@ -2139,14 +2120,15 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 		s3c_hsotg_complete_request(hsotg, ep, req,
 					   result);
 	}
-
-	spin_unlock_irqrestore(&ep->lock, flags);
 }
 
 #define call_gadget(_hs, _entry) \
 	if ((_hs)->gadget.speed != USB_SPEED_UNKNOWN &&	\
-	    (_hs)->driver && (_hs)->driver->_entry)	\
-		(_hs)->driver->_entry(&(_hs)->gadget);
+	    (_hs)->driver && (_hs)->driver->_entry) { \
+		spin_unlock(&_hs->lock); \
+		(_hs)->driver->_entry(&(_hs)->gadget); \
+		spin_lock(&_hs->lock); \
+		}
 
 /**
  * s3c_hsotg_disconnect - disconnect service
@@ -2388,6 +2370,7 @@ static irqreturn_t s3c_hsotg_irq(int irq, void *pw)
 	u32 gintsts;
 	u32 gintmsk;
 
+	spin_lock(&hsotg->lock);
 irq_retry:
 	gintsts = readl(hsotg->regs + GINTSTS);
 	gintmsk = readl(hsotg->regs + GINTMSK);
@@ -2557,6 +2540,8 @@ irq_retry:
 	if (gintsts & IRQ_RETRY_MASK && --retry_count > 0)
 			goto irq_retry;
 
+	spin_unlock(&hsotg->lock);
+
 	return IRQ_HANDLED;
 }
 
@@ -2604,7 +2589,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	dev_dbg(hsotg->dev, "%s: read DxEPCTL=0x%08x from 0x%08x\n",
 		__func__, epctrl, epctrl_reg);
 
-	spin_lock_irqsave(&hs_ep->lock, flags);
+	spin_lock_irqsave(&hsotg->lock, flags);
 
 	epctrl &= ~(DxEPCTL_EPType_MASK | DxEPCTL_MPS_MASK);
 	epctrl |= DxEPCTL_MPS(mps);
@@ -2683,7 +2668,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	s3c_hsotg_ctrl_epint(hsotg, index, dir_in, 1);
 
 out:
-	spin_unlock_irqrestore(&hs_ep->lock, flags);
+	spin_unlock_irqrestore(&hsotg->lock, flags);
 	return ret;
 }
 
@@ -2710,10 +2695,10 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 
 	epctrl_reg = dir_in ? DIEPCTL(index) : DOEPCTL(index);
 
+	spin_lock_irqsave(&hsotg->lock, flags);
 	/* terminate all requests with shutdown */
 	kill_all_requests(hsotg, hs_ep, -ESHUTDOWN, false);
 
-	spin_lock_irqsave(&hs_ep->lock, flags);
 
 	ctrl = readl(hsotg->regs + epctrl_reg);
 	ctrl &= ~DxEPCTL_EPEna;
@@ -2726,7 +2711,7 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 	/* disable endpoint interrupts */
 	s3c_hsotg_ctrl_epint(hsotg, hs_ep->index, hs_ep->dir_in, 0);
 
-	spin_unlock_irqrestore(&hs_ep->lock, flags);
+	spin_unlock_irqrestore(&hsotg->lock, flags);
 	return 0;
 }
 
@@ -2761,15 +2746,15 @@ static int s3c_hsotg_ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 
 	dev_info(hs->dev, "ep_dequeue(%p,%p)\n", ep, req);
 
-	spin_lock_irqsave(&hs_ep->lock, flags);
+	spin_lock_irqsave(&hs->lock, flags);
 
 	if (!on_list(hs_ep, hs_req)) {
-		spin_unlock_irqrestore(&hs_ep->lock, flags);
+		spin_unlock_irqrestore(&hs->lock, flags);
 		return -EINVAL;
 	}
 
 	s3c_hsotg_complete_request(hs, hs_ep, hs_req, -ECONNRESET);
-	spin_unlock_irqrestore(&hs_ep->lock, flags);
+	spin_unlock_irqrestore(&hs->lock, flags);
 
 	return 0;
 }
@@ -2784,15 +2769,12 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
 	struct s3c_hsotg *hs = hs_ep->parent;
 	int index = hs_ep->index;
-	unsigned long irqflags;
 	u32 epreg;
 	u32 epctl;
 	u32 xfertype;
 
 	dev_info(hs->dev, "%s(ep %p %s, %d)\n", __func__, ep, ep->name, value);
 
-	spin_lock_irqsave(&hs_ep->lock, irqflags);
-
 	/* write both IN and OUT control registers */
 
 	epreg = DIEPCTL(index);
@@ -2827,19 +2809,36 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 
 	writel(epctl, hs->regs + epreg);
 
-	spin_unlock_irqrestore(&hs_ep->lock, irqflags);
-
 	return 0;
 }
 
+/**
+ * s3c_hsotg_ep_sethalt_lock - set halt on a given endpoint with lock held
+ * @ep: The endpoint to set halt.
+ * @value: Set or unset the halt.
+ */
+static int s3c_hsotg_ep_sethalt_lock(struct usb_ep *ep, int value)
+{
+	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
+	struct s3c_hsotg *hs = hs_ep->parent;
+	unsigned long flags = 0;
+	int ret = 0;
+
+	spin_lock_irqsave(&hs->lock, flags);
+	ret = s3c_hsotg_ep_sethalt(ep, value);
+	spin_unlock_irqrestore(&hs->lock, flags);
+
+	return ret;
+}
+
 static struct usb_ep_ops s3c_hsotg_ep_ops = {
 	.enable		= s3c_hsotg_ep_enable,
 	.disable	= s3c_hsotg_ep_disable,
 	.alloc_request	= s3c_hsotg_ep_alloc_request,
 	.free_request	= s3c_hsotg_ep_free_request,
-	.queue		= s3c_hsotg_ep_queue,
+	.queue		= s3c_hsotg_ep_queue_lock,
 	.dequeue	= s3c_hsotg_ep_dequeue,
-	.set_halt	= s3c_hsotg_ep_sethalt,
+	.set_halt	= s3c_hsotg_ep_sethalt_lock,
 	/* note, don't believe we have any call for the fifo routines */
 };
 
@@ -2954,6 +2953,7 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 	driver->driver.bus = NULL;
 	hsotg->driver = driver;
 	hsotg->gadget.dev.driver = &driver->driver;
+	hsotg->gadget.dev.of_node = hsotg->dev->of_node;
 	hsotg->gadget.dev.dma_mask = hsotg->dev->dma_mask;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
 
@@ -2964,9 +2964,6 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 		goto err;
 	}
 
-	s3c_hsotg_phy_enable(hsotg);
-
-	s3c_hsotg_core_init(hsotg);
 	hsotg->last_rst = jiffies;
 	dev_info(hsotg->dev, "bound driver %s\n", driver->driver.name);
 	return 0;
@@ -2988,6 +2985,7 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 			  struct usb_gadget_driver *driver)
 {
 	struct s3c_hsotg *hsotg = to_hsotg(gadget);
+	unsigned long flags = 0;
 	int ep;
 
 	if (!hsotg)
@@ -3000,6 +2998,8 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 	for (ep = 0; ep < hsotg->num_of_eps; ep++)
 		s3c_hsotg_ep_disable(&hsotg->eps[ep].ep);
 
+	spin_lock_irqsave(&hsotg->lock, flags);
+
 	s3c_hsotg_phy_disable(hsotg);
 	regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
 
@@ -3007,6 +3007,8 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
 	hsotg->gadget.dev.driver = NULL;
 
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
 	dev_info(hsotg->dev, "unregistered gadget driver '%s'\n",
 		 driver->driver.name);
 
@@ -3024,10 +3026,40 @@ static int s3c_hsotg_gadget_getframe(struct usb_gadget *gadget)
 	return s3c_hsotg_read_frameno(to_hsotg(gadget));
 }
 
+/**
+ * s3c_hsotg_pullup - connect/disconnect the USB PHY
+ * @gadget: The usb gadget state
+ * @is_on: Current state of the USB PHY
+ *
+ * Connect/Disconnect the USB PHY pullup
+ */
+static int s3c_hsotg_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct s3c_hsotg *hsotg = to_hsotg(gadget);
+	unsigned long flags = 0;
+
+	dev_dbg(hsotg->dev, "%s: is_in: %d\n", __func__, is_on);
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	if (is_on) {
+		s3c_hsotg_phy_enable(hsotg);
+		s3c_hsotg_core_init(hsotg);
+	} else {
+		s3c_hsotg_disconnect(hsotg);
+		s3c_hsotg_phy_disable(hsotg);
+	}
+
+	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	return 0;
+}
+
 static struct usb_gadget_ops s3c_hsotg_gadget_ops = {
 	.get_frame	= s3c_hsotg_gadget_getframe,
 	.udc_start		= s3c_hsotg_udc_start,
 	.udc_stop		= s3c_hsotg_udc_stop,
+	.pullup                 = s3c_hsotg_pullup,
 };
 
 /**
@@ -3063,8 +3095,6 @@ static void __devinit s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	INIT_LIST_HEAD(&hs_ep->queue);
 	INIT_LIST_HEAD(&hs_ep->ep.ep_list);
 
-	spin_lock_init(&hs_ep->lock);
-
 	/* add to the list of endpoints known by the gadget driver */
 	if (epnum)
 		list_add_tail(&hs_ep->ep.ep_list, &hsotg->gadget.ep_list);
@@ -3342,7 +3372,7 @@ static int ep_show(struct seq_file *seq, void *v)
 	seq_printf(seq, "request list (%p,%p):\n",
 		   ep->queue.next, ep->queue.prev);
 
-	spin_lock_irqsave(&ep->lock, flags);
+	spin_lock_irqsave(&hsotg->lock, flags);
 
 	list_for_each_entry(req, &ep->queue, queue) {
 		if (--show_limit < 0) {
@@ -3357,7 +3387,7 @@ static int ep_show(struct seq_file *seq, void *v)
 			   req->req.actual, req->req.status);
 	}
 
-	spin_unlock_irqrestore(&ep->lock, flags);
+	spin_unlock_irqrestore(&hsotg->lock, flags);
 
 	return 0;
 }
@@ -3477,7 +3507,7 @@ static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	hsotg = kzalloc(sizeof(struct s3c_hsotg), GFP_KERNEL);
+	hsotg = devm_kzalloc(&pdev->dev, sizeof(struct s3c_hsotg), GFP_KERNEL);
 	if (!hsotg) {
 		dev_err(dev, "cannot get memory\n");
 		return -ENOMEM;
@@ -3489,46 +3519,35 @@ static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 	hsotg->clk = clk_get(&pdev->dev, "otg");
 	if (IS_ERR(hsotg->clk)) {
 		dev_err(dev, "cannot get otg clock\n");
-		ret = PTR_ERR(hsotg->clk);
-		goto err_mem;
+		return PTR_ERR(hsotg->clk);
 	}
 
 	platform_set_drvdata(pdev, hsotg);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "cannot find register resource 0\n");
-		ret = -EINVAL;
-		goto err_clk;
-	}
-
-	hsotg->regs_res = request_mem_region(res->start, resource_size(res),
-					     dev_name(dev));
-	if (!hsotg->regs_res) {
-		dev_err(dev, "cannot reserve registers\n");
-		ret = -ENOENT;
-		goto err_clk;
-	}
 
-	hsotg->regs = ioremap(res->start, resource_size(res));
+	hsotg->regs = devm_request_and_ioremap(&pdev->dev, res);
 	if (!hsotg->regs) {
 		dev_err(dev, "cannot map registers\n");
 		ret = -ENXIO;
-		goto err_regs_res;
+		goto err_clk;
 	}
 
 	ret = platform_get_irq(pdev, 0);
 	if (ret < 0) {
 		dev_err(dev, "cannot find IRQ\n");
-		goto err_regs;
+		goto err_clk;
 	}
 
+	spin_lock_init(&hsotg->lock);
+
 	hsotg->irq = ret;
 
-	ret = request_irq(ret, s3c_hsotg_irq, 0, dev_name(dev), hsotg);
+	ret = devm_request_irq(&pdev->dev, hsotg->irq, s3c_hsotg_irq, 0,
+				dev_name(dev), hsotg);
 	if (ret < 0) {
 		dev_err(dev, "cannot claim IRQ\n");
-		goto err_regs;
+		goto err_clk;
 	}
 
 	dev_info(dev, "regs %p, irq %d\n", hsotg->regs, hsotg->irq);
@@ -3558,7 +3577,7 @@ static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 				 hsotg->supplies);
 	if (ret) {
 		dev_err(dev, "failed to request supplies: %d\n", ret);
-		goto err_irq;
+		goto err_clk;
 	}
 
 	ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
@@ -3642,19 +3661,11 @@ err_ep_mem:
 err_supplies:
 	s3c_hsotg_phy_disable(hsotg);
 	regulator_bulk_free(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
-err_irq:
-	free_irq(hsotg->irq, hsotg);
-err_regs:
-	iounmap(hsotg->regs);
-
-err_regs_res:
-	release_resource(hsotg->regs_res);
-	kfree(hsotg->regs_res);
+
 err_clk:
 	clk_disable_unprepare(hsotg->clk);
 	clk_put(hsotg->clk);
-err_mem:
-	kfree(hsotg);
+
 	return ret;
 }
 
@@ -3675,12 +3686,6 @@ static int __devexit s3c_hsotg_remove(struct platform_device *pdev)
 		usb_gadget_unregister_driver(hsotg->driver);
 	}
 
-	free_irq(hsotg->irq, hsotg);
-	iounmap(hsotg->regs);
-
-	release_resource(hsotg->regs_res);
-	kfree(hsotg->regs_res);
-
 	s3c_hsotg_phy_disable(hsotg);
 	regulator_bulk_free(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
 
diff --git a/drivers/usb/gadget/s3c-hsudc.c b/drivers/usb/gadget/s3c-hsudc.c
index 236b271871a..e26a4e7ed2b 100644
--- a/drivers/usb/gadget/s3c-hsudc.c
+++ b/drivers/usb/gadget/s3c-hsudc.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/otg.h>
@@ -1165,7 +1166,7 @@ static int s3c_hsudc_start(struct usb_gadget *gadget,
 	}
 
 	/* connect to bus through transceiver */
-	if (hsudc->transceiver) {
+	if (!IS_ERR_OR_NULL(hsudc->transceiver)) {
 		ret = otg_set_peripheral(hsudc->transceiver->otg,
 					&hsudc->gadget);
 		if (ret) {
@@ -1220,7 +1221,7 @@ static int s3c_hsudc_stop(struct usb_gadget *gadget,
 	s3c_hsudc_stop_activity(hsudc);
 	spin_unlock_irqrestore(&hsudc->lock, flags);
 
-	if (hsudc->transceiver)
+	if (!IS_ERR_OR_NULL(hsudc->transceiver))
 		(void) otg_set_peripheral(hsudc->transceiver->otg, NULL);
 
 	disable_irq(hsudc->irq);
@@ -1249,7 +1250,7 @@ static int s3c_hsudc_vbus_draw(struct usb_gadget *gadget, unsigned mA)
 	if (!hsudc)
 		return -ENODEV;
 
-	if (hsudc->transceiver)
+	if (!IS_ERR_OR_NULL(hsudc->transceiver))
 		return usb_phy_set_power(hsudc->transceiver, mA);
 
 	return -EOPNOTSUPP;
@@ -1282,7 +1283,7 @@ static int __devinit s3c_hsudc_probe(struct platform_device *pdev)
 	hsudc->dev = dev;
 	hsudc->pd = pdev->dev.platform_data;
 
-	hsudc->transceiver = usb_get_transceiver();
+	hsudc->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
 
 	for (i = 0; i < ARRAY_SIZE(hsudc->supplies); i++)
 		hsudc->supplies[i].supply = s3c_hsudc_supply_names[i];
@@ -1385,8 +1386,8 @@ err_irq:
 err_remap:
 	release_mem_region(res->start, resource_size(res));
 err_res:
-	if (hsudc->transceiver)
-		usb_put_transceiver(hsudc->transceiver);
+	if (!IS_ERR_OR_NULL(hsudc->transceiver))
+		usb_put_phy(hsudc->transceiver);
 
 	regulator_bulk_free(ARRAY_SIZE(hsudc->supplies), hsudc->supplies);
 err_supplies:
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index 8081ca3a70a..8d9bcd8207c 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -38,12 +38,6 @@
  */
 
 /*
- * When FSG_BUFFHD_STATIC_BUFFER is defined when this file is included
- * the fsg_buffhd structure's buf field will be an array of FSG_BUFLEN
- * characters rather then a pointer to void.
- */
-
-/*
  * When USB_GADGET_DEBUG_FILES is defined the module param num_buffers
  * sets the number of pipeline buffers (length of the fsg_buffhd array).
  * The valid range of num_buffers is: num >= 2 && num <= 4.
@@ -260,11 +254,7 @@ enum fsg_buffer_state {
 };
 
 struct fsg_buffhd {
-#ifdef FSG_BUFFHD_STATIC_BUFFER
-	char				buf[FSG_BUFLEN];
-#else
 	void				*buf;
-#endif
 	enum fsg_buffer_state		state;
 	struct fsg_buffhd		*next;
 
@@ -627,6 +617,16 @@ static struct usb_gadget_strings	fsg_stringtab = {
  * the caller must own fsg->filesem for writing.
  */
 
+static void fsg_lun_close(struct fsg_lun *curlun)
+{
+	if (curlun->filp) {
+		LDBG(curlun, "close backing file\n");
+		fput(curlun->filp);
+		curlun->filp = NULL;
+	}
+}
+
+
 static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 {
 	int				ro;
@@ -636,6 +636,8 @@ static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 	loff_t				size;
 	loff_t				num_sectors;
 	loff_t				min_sectors;
+	unsigned int			blkbits;
+	unsigned int			blksize;
 
 	/* R/W if we can, R/O if we must */
 	ro = curlun->initially_ro;
@@ -654,9 +656,8 @@ static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 	if (!(filp->f_mode & FMODE_WRITE))
 		ro = 1;
 
-	if (filp->f_path.dentry)
-		inode = filp->f_path.dentry->d_inode;
-	if (!inode || (!S_ISREG(inode->i_mode) && !S_ISBLK(inode->i_mode))) {
+	inode = filp->f_path.dentry->d_inode;
+	if ((!S_ISREG(inode->i_mode) && !S_ISBLK(inode->i_mode))) {
 		LINFO(curlun, "invalid file type: %s\n", filename);
 		goto out;
 	}
@@ -665,7 +666,7 @@ static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 	 * If we can't read the file, it's no good.
 	 * If we can't write the file, use it read-only.
 	 */
-	if (!filp->f_op || !(filp->f_op->read || filp->f_op->aio_read)) {
+	if (!(filp->f_op->read || filp->f_op->aio_read)) {
 		LINFO(curlun, "file not readable: %s\n", filename);
 		goto out;
 	}
@@ -680,17 +681,17 @@ static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 	}
 
 	if (curlun->cdrom) {
-		curlun->blksize = 2048;
-		curlun->blkbits = 11;
+		blksize = 2048;
+		blkbits = 11;
 	} else if (inode->i_bdev) {
-		curlun->blksize = bdev_logical_block_size(inode->i_bdev);
-		curlun->blkbits = blksize_bits(curlun->blksize);
+		blksize = bdev_logical_block_size(inode->i_bdev);
+		blkbits = blksize_bits(blksize);
 	} else {
-		curlun->blksize = 512;
-		curlun->blkbits = 9;
+		blksize = 512;
+		blkbits = 9;
 	}
 
-	num_sectors = size >> curlun->blkbits; /* File size in logic-block-size blocks */
+	num_sectors = size >> blkbits; /* File size in logic-block-size blocks */
 	min_sectors = 1;
 	if (curlun->cdrom) {
 		min_sectors = 300;	/* Smallest track is 300 frames */
@@ -707,30 +708,24 @@ static int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 		goto out;
 	}
 
-	get_file(filp);
+	if (fsg_lun_is_open(curlun))
+		fsg_lun_close(curlun);
+
+	curlun->blksize = blksize;
+	curlun->blkbits = blkbits;
 	curlun->ro = ro;
 	curlun->filp = filp;
 	curlun->file_length = size;
 	curlun->num_sectors = num_sectors;
 	LDBG(curlun, "open backing file: %s\n", filename);
-	rc = 0;
+	return 0;
 
 out:
-	filp_close(filp, current->files);
+	fput(filp);
 	return rc;
 }
 
 
-static void fsg_lun_close(struct fsg_lun *curlun)
-{
-	if (curlun->filp) {
-		LDBG(curlun, "close backing file\n");
-		fput(curlun->filp);
-		curlun->filp = NULL;
-	}
-}
-
-
 /*-------------------------------------------------------------------------*/
 
 /*
@@ -881,19 +876,17 @@ static ssize_t fsg_store_file(struct device *dev, struct device_attribute *attr,
 	if (count > 0 && buf[count-1] == '\n')
 		((char *) buf)[count-1] = 0;		/* Ugh! */
 
-	/* Eject current medium */
-	down_write(filesem);
-	if (fsg_lun_is_open(curlun)) {
-		fsg_lun_close(curlun);
-		curlun->unit_attention_data = SS_MEDIUM_NOT_PRESENT;
-	}
-
 	/* Load new medium */
+	down_write(filesem);
 	if (count > 0 && buf[0]) {
+		/* fsg_lun_open() will close existing file if any. */
 		rc = fsg_lun_open(curlun, buf);
 		if (rc == 0)
 			curlun->unit_attention_data =
 					SS_NOT_READY_TO_READY_TRANSITION;
+	} else if (fsg_lun_is_open(curlun)) {
+		fsg_lun_close(curlun);
+		curlun->unit_attention_data = SS_MEDIUM_NOT_PRESENT;
 	}
 	up_write(filesem);
 	return (rc < 0 ? rc : count);
diff --git a/drivers/usb/gadget/tcm_usb_gadget.c b/drivers/usb/gadget/tcm_usb_gadget.c
index c46439c8dd7..5444866e13e 100644
--- a/drivers/usb/gadget/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/tcm_usb_gadget.c
@@ -294,7 +294,7 @@ static int bot_send_write_request(struct usbg_cmd *cmd)
 		pr_err("%s(%d)\n", __func__, __LINE__);
 
 	wait_for_completion(&cmd->write_complete);
-	transport_generic_process_write(se_cmd);
+	target_execute_cmd(se_cmd);
 cleanup:
 	return ret;
 }
@@ -725,7 +725,7 @@ static int uasp_send_write_request(struct usbg_cmd *cmd)
 	}
 
 	wait_for_completion(&cmd->write_complete);
-	transport_generic_process_write(se_cmd);
+	target_execute_cmd(se_cmd);
 cleanup:
 	return ret;
 }
@@ -1065,16 +1065,20 @@ static void usbg_cmd_work(struct work_struct *work)
 				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
 				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
 				cmd->prio_attr, cmd->sense_iu.sense);
-
-		transport_send_check_condition_and_sense(se_cmd,
-				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
-		usbg_cleanup_cmd(cmd);
-		return;
+		goto out;
 	}
 
-	target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
+	if (target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
 			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
-			0, cmd->prio_attr, dir, TARGET_SCF_UNKNOWN_SIZE);
+			0, cmd->prio_attr, dir, TARGET_SCF_UNKNOWN_SIZE) < 0)
+		goto out;
+
+	return;
+
+out:
+	transport_send_check_condition_and_sense(se_cmd,
+			TCM_UNSUPPORTED_SCSI_OPCODE, 1);
+	usbg_cleanup_cmd(cmd);
 }
 
 static int usbg_submit_command(struct f_uas *fu,
@@ -1177,16 +1181,20 @@ static void bot_cmd_work(struct work_struct *work)
 				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
 				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
 				cmd->prio_attr, cmd->sense_iu.sense);
-
-		transport_send_check_condition_and_sense(se_cmd,
-				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
-		usbg_cleanup_cmd(cmd);
-		return;
+		goto out;
 	}
 
-	target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
+	if (target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
 			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
-			cmd->data_len, cmd->prio_attr, dir, 0);
+			cmd->data_len, cmd->prio_attr, dir, 0) < 0)
+		goto out;
+
+	return;
+
+out:
+	transport_send_check_condition_and_sense(se_cmd,
+				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
+	usbg_cleanup_cmd(cmd);
 }
 
 static int bot_submit_command(struct f_uas *fu,
@@ -1400,19 +1408,6 @@ static u32 usbg_tpg_get_inst_index(struct se_portal_group *se_tpg)
 	return 1;
 }
 
-static int usbg_new_cmd(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-	int ret;
-
-	ret = target_setup_cmd_from_cdb(se_cmd, cmd->cmd_buf);
-	if (ret)
-		return ret;
-
-	return transport_generic_map_mem_to_cmd(se_cmd, NULL, 0, NULL, 0);
-}
-
 static void usbg_cmd_release(struct kref *ref)
 {
 	struct usbg_cmd *cmd = container_of(ref, struct usbg_cmd,
@@ -1902,7 +1897,6 @@ static struct target_core_fabric_ops usbg_ops = {
 	.tpg_alloc_fabric_acl		= usbg_alloc_fabric_acl,
 	.tpg_release_fabric_acl		= usbg_release_fabric_acl,
 	.tpg_get_inst_index		= usbg_tpg_get_inst_index,
-	.new_cmd_map			= usbg_new_cmd,
 	.release_cmd			= usbg_release_cmd,
 	.shutdown_session		= usbg_shutdown_session,
 	.close_session			= usbg_close_session,
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index 47cf48b51c9..90e82e288eb 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -724,7 +724,7 @@ static int get_ether_addr(const char *str, u8 *dev_addr)
 		if (is_valid_ether_addr(dev_addr))
 			return 0;
 	}
-	random_ether_addr(dev_addr);
+	eth_random_addr(dev_addr);
 	return 1;
 }
 
@@ -798,12 +798,6 @@ int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
 
 	SET_ETHTOOL_OPS(net, &ops);
 
-	/* two kinds of host-initiated state changes:
-	 *  - iff DATA transfer is active, carrier is "on"
-	 *  - tx queueing enabled if open *and* carrier is "on"
-	 */
-	netif_carrier_off(net);
-
 	dev->gadget = g;
 	SET_NETDEV_DEV(net, &g->dev);
 	SET_NETDEV_DEVTYPE(net, &gadget_type);
@@ -817,6 +811,12 @@ int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
 		INFO(dev, "HOST MAC %pM\n", dev->host_mac);
 
 		the_dev = dev;
+
+		/* two kinds of host-initiated state changes:
+		 *  - iff DATA transfer is active, carrier is "on"
+		 *  - tx queueing enabled if open *and* carrier is "on"
+		 */
+		netif_carrier_off(net);
 	}
 
 	return status;
diff --git a/drivers/usb/gadget/u_uac1.c b/drivers/usb/gadget/u_uac1.c
index af989898205..e0c5e88e03e 100644
--- a/drivers/usb/gadget/u_uac1.c
+++ b/drivers/usb/gadget/u_uac1.c
@@ -275,17 +275,17 @@ static int gaudio_close_snd_dev(struct gaudio *gau)
 	/* Close control device */
 	snd = &gau->control;
 	if (snd->filp)
-		filp_close(snd->filp, current->files);
+		filp_close(snd->filp, NULL);
 
 	/* Close PCM playback device and setup substream */
 	snd = &gau->playback;
 	if (snd->filp)
-		filp_close(snd->filp, current->files);
+		filp_close(snd->filp, NULL);
 
 	/* Close PCM capture device and setup substream */
 	snd = &gau->capture;
 	if (snd->filp)
-		filp_close(snd->filp, current->files);
+		filp_close(snd->filp, NULL);
 
 	return 0;
 }
diff --git a/drivers/usb/gadget/uvc.h b/drivers/usb/gadget/uvc.h
index ca4e03a1c73..93b0c119111 100644
--- a/drivers/usb/gadget/uvc.h
+++ b/drivers/usb/gadget/uvc.h
@@ -153,9 +153,11 @@ struct uvc_device
 
 	/* Descriptors */
 	struct {
-		const struct uvc_descriptor_header * const *control;
+		const struct uvc_descriptor_header * const *fs_control;
+		const struct uvc_descriptor_header * const *ss_control;
 		const struct uvc_descriptor_header * const *fs_streaming;
 		const struct uvc_descriptor_header * const *hs_streaming;
+		const struct uvc_descriptor_header * const *ss_streaming;
 	} desc;
 
 	unsigned int control_intf;
diff --git a/drivers/usb/gadget/webcam.c b/drivers/usb/gadget/webcam.c
index 668fe128f2e..120e134e805 100644
--- a/drivers/usb/gadget/webcam.c
+++ b/drivers/usb/gadget/webcam.c
@@ -272,7 +272,15 @@ static const struct uvc_color_matching_descriptor uvc_color_matching = {
 	.bMatrixCoefficients	= 4,
 };
 
-static const struct uvc_descriptor_header * const uvc_control_cls[] = {
+static const struct uvc_descriptor_header * const uvc_fs_control_cls[] = {
+	(const struct uvc_descriptor_header *) &uvc_control_header,
+	(const struct uvc_descriptor_header *) &uvc_camera_terminal,
+	(const struct uvc_descriptor_header *) &uvc_processing,
+	(const struct uvc_descriptor_header *) &uvc_output_terminal,
+	NULL,
+};
+
+static const struct uvc_descriptor_header * const uvc_ss_control_cls[] = {
 	(const struct uvc_descriptor_header *) &uvc_control_header,
 	(const struct uvc_descriptor_header *) &uvc_camera_terminal,
 	(const struct uvc_descriptor_header *) &uvc_processing,
@@ -304,6 +312,18 @@ static const struct uvc_descriptor_header * const uvc_hs_streaming_cls[] = {
 	NULL,
 };
 
+static const struct uvc_descriptor_header * const uvc_ss_streaming_cls[] = {
+	(const struct uvc_descriptor_header *) &uvc_input_header,
+	(const struct uvc_descriptor_header *) &uvc_format_yuv,
+	(const struct uvc_descriptor_header *) &uvc_frame_yuv_360p,
+	(const struct uvc_descriptor_header *) &uvc_frame_yuv_720p,
+	(const struct uvc_descriptor_header *) &uvc_format_mjpg,
+	(const struct uvc_descriptor_header *) &uvc_frame_mjpg_360p,
+	(const struct uvc_descriptor_header *) &uvc_frame_mjpg_720p,
+	(const struct uvc_descriptor_header *) &uvc_color_matching,
+	NULL,
+};
+
 /* --------------------------------------------------------------------------
  * USB configuration
  */
@@ -311,8 +331,9 @@ static const struct uvc_descriptor_header * const uvc_hs_streaming_cls[] = {
 static int __init
 webcam_config_bind(struct usb_configuration *c)
 {
-	return uvc_bind_config(c, uvc_control_cls, uvc_fs_streaming_cls,
-			       uvc_hs_streaming_cls);
+	return uvc_bind_config(c, uvc_fs_control_cls, uvc_ss_control_cls,
+		uvc_fs_streaming_cls, uvc_hs_streaming_cls,
+		uvc_ss_streaming_cls);
 }
 
 static struct usb_configuration webcam_config_driver = {
@@ -373,7 +394,7 @@ static struct usb_composite_driver webcam_driver = {
 	.name		= "g_webcam",
 	.dev		= &webcam_device_descriptor,
 	.strings	= webcam_device_strings,
-	.max_speed	= USB_SPEED_HIGH,
+	.max_speed	= USB_SPEED_SUPER,
 	.unbind		= webcam_unbind,
 };
 
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 83e58df29fe..075d2eca810 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -308,7 +308,7 @@ config USB_OHCI_HCD
 
 config USB_OHCI_HCD_OMAP1
 	bool "OHCI support for OMAP1/2 chips"
-	depends on USB_OHCI_HCD && (ARCH_OMAP1 || ARCH_OMAP2)
+	depends on USB_OHCI_HCD && ARCH_OMAP1
 	default y
 	---help---
 	  Enables support for the OHCI controller on OMAP1/2 chips.
@@ -652,7 +652,7 @@ config USB_HCD_BCMA
 	select USB_OHCI_HCD_PLATFORM if USB_OHCI_HCD
 	select USB_EHCI_HCD_PLATFORM if USB_EHCI_HCD
 	help
-	  Enbale support for the EHCI and OCHI host controller on an bcma bus.
+	  Enable support for the EHCI and OCHI host controller on an bcma bus.
 	  It converts the bcma driver into two platform device drivers
 	  for ehci and ohci.
 
@@ -664,7 +664,7 @@ config USB_HCD_SSB
 	select USB_OHCI_HCD_PLATFORM if USB_OHCI_HCD
 	select USB_EHCI_HCD_PLATFORM if USB_EHCI_HCD
 	help
-	  Enbale support for the EHCI and OCHI host controller on an bcma bus.
+	  Enable support for the EHCI and OCHI host controller on an bcma bus.
 	  It converts the bcma driver into two platform device drivers
 	  for ehci and ohci.
 
diff --git a/drivers/usb/host/ehci-atmel.c b/drivers/usb/host/ehci-atmel.c
index cf14c95a670..a47e2cffaaf 100644
--- a/drivers/usb/host/ehci-atmel.c
+++ b/drivers/usb/host/ehci-atmel.c
@@ -53,30 +53,15 @@ static void atmel_stop_ehci(struct platform_device *pdev)
 static int ehci_atmel_setup(struct usb_hcd *hcd)
 {
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	int retval = 0;
+	int retval;
 
 	/* registers start at offset 0x0 */
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
 
-	/* data structure init */
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	ehci->sbrn = 0x20;
-
-	ehci_reset(ehci);
 	ehci_port_power(ehci, 0);
 
 	return retval;
diff --git a/drivers/usb/host/ehci-au1xxx.c b/drivers/usb/host/ehci-au1xxx.c
index bf7441afed1..cba10d625a5 100644
--- a/drivers/usb/host/ehci-au1xxx.c
+++ b/drivers/usb/host/ehci-au1xxx.c
@@ -20,10 +20,12 @@ extern int usb_disabled(void);
 static int au1xxx_ehci_setup(struct usb_hcd *hcd)
 {
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	int ret = ehci_init(hcd);
+	int ret;
+
+	ehci->caps = hcd->regs;
+	ret = ehci_setup(hcd);
 
 	ehci->need_io_watchdog = 0;
-	ehci_reset(ehci);
 	return ret;
 }
 
@@ -78,7 +80,6 @@ static const struct hc_driver ehci_au1xxx_hc_driver = {
 static int ehci_hcd_au1xxx_drv_probe(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd;
-	struct ehci_hcd *ehci;
 	struct resource *res;
 	int ret;
 
@@ -116,13 +117,6 @@ static int ehci_hcd_au1xxx_drv_probe(struct platform_device *pdev)
 		goto err3;
 	}
 
-	ehci = hcd_to_ehci(hcd);
-	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = readl(&ehci->caps->hcs_params);
-
 	ret = usb_add_hcd(hcd, pdev->resource[1].start,
 			  IRQF_SHARED);
 	if (ret == 0) {
@@ -158,28 +152,10 @@ static int ehci_hcd_au1xxx_drv_remove(struct platform_device *pdev)
 static int ehci_hcd_au1xxx_drv_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	unsigned long flags;
-	int rc = 0;
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(10);
-
-	/* Root hub was already suspended. Disable irq emission and
-	 * mark HW unaccessible.  The PM and USB cores make sure that
-	 * the root hub is either suspended or stopped.
-	 */
-	ehci_prepare_ports_for_controller_suspend(ehci, device_may_wakeup(dev));
-	spin_lock_irqsave(&ehci->lock, flags);
-	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
-	(void)ehci_readl(ehci, &ehci->regs->intr_enable);
-
-	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	spin_unlock_irqrestore(&ehci->lock, flags);
-
-	// could save FLADJ in case of Vaux power loss
-	// ... we'd only use it to handle clock skew
+	bool do_wakeup = device_may_wakeup(dev);
+	int rc;
 
+	rc = ehci_suspend(hcd, do_wakeup);
 	alchemy_usb_control(ALCHEMY_USB_EHCI0, 0);
 
 	return rc;
@@ -188,56 +164,9 @@ static int ehci_hcd_au1xxx_drv_suspend(struct device *dev)
 static int ehci_hcd_au1xxx_drv_resume(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 
 	alchemy_usb_control(ALCHEMY_USB_EHCI0, 1);
-
-	// maybe restore FLADJ
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(100);
-
-	/* Mark hardware accessible again as we are out of D3 state by now */
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
-	/* If CF is still set, we maintained PCI Vaux power.
-	 * Just undo the effect of ehci_pci_suspend().
-	 */
-	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF) {
-		int	mask = INTR_MASK;
-
-		ehci_prepare_ports_for_controller_resume(ehci);
-		if (!hcd->self.root_hub->do_remote_wakeup)
-			mask &= ~STS_PCD;
-		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
-		ehci_readl(ehci, &ehci->regs->intr_enable);
-		return 0;
-	}
-
-	ehci_dbg(ehci, "lost power, restarting\n");
-	usb_root_hub_lost_power(hcd->self.root_hub);
-
-	/* Else reset, to cope with power loss or flush-to-storage
-	 * style "resume" having let BIOS kick in during reboot.
-	 */
-	(void) ehci_halt(ehci);
-	(void) ehci_reset(ehci);
-
-	/* emptying the schedule aborts any urbs */
-	spin_lock_irq(&ehci->lock);
-	if (ehci->reclaim)
-		end_unlink_async(ehci);
-	ehci_work(ehci);
-	spin_unlock_irq(&ehci->lock);
-
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
-	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
-
-	/* here we "know" root ports should always stay powered */
-	ehci_port_power(ehci, 1);
-
-	ehci->rh_state = EHCI_RH_SUSPENDED;
+	ehci_resume(hcd, false);
 
 	return 0;
 }
diff --git a/drivers/usb/host/ehci-cns3xxx.c b/drivers/usb/host/ehci-cns3xxx.c
index 6536abdea6e..caaa3e5be33 100644
--- a/drivers/usb/host/ehci-cns3xxx.c
+++ b/drivers/usb/host/ehci-cns3xxx.c
@@ -33,14 +33,10 @@ static int cns3xxx_ehci_init(struct usb_hcd *hcd)
 	}
 
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs
-		+ HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
 	hcd->has_tt = 0;
-	ehci_reset(ehci);
 
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
diff --git a/drivers/usb/host/ehci-dbg.c b/drivers/usb/host/ehci-dbg.c
index 7561966fbdc..f0c00de035e 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -404,9 +404,9 @@ struct debug_buffer {
 
 #define speed_char(info1) ({ char tmp; \
 		switch (info1 & (3 << 12)) { \
-		case 0 << 12: tmp = 'f'; break; \
-		case 1 << 12: tmp = 'l'; break; \
-		case 2 << 12: tmp = 'h'; break; \
+		case QH_FULL_SPEED: tmp = 'f'; break; \
+		case QH_LOW_SPEED:  tmp = 'l'; break; \
+		case QH_HIGH_SPEED: tmp = 'h'; break; \
 		default: tmp = '?'; break; \
 		}; tmp; })
 
@@ -538,12 +538,13 @@ static ssize_t fill_async_buffer(struct debug_buffer *buf)
 	spin_lock_irqsave (&ehci->lock, flags);
 	for (qh = ehci->async->qh_next.qh; size > 0 && qh; qh = qh->qh_next.qh)
 		qh_lines (ehci, qh, &next, &size);
-	if (ehci->reclaim && size > 0) {
-		temp = scnprintf (next, size, "\nreclaim =\n");
+	if (ehci->async_unlink && size > 0) {
+		temp = scnprintf(next, size, "\nunlink =\n");
 		size -= temp;
 		next += temp;
 
-		for (qh = ehci->reclaim; size > 0 && qh; qh = qh->reclaim)
+		for (qh = ehci->async_unlink; size > 0 && qh;
+				qh = qh->unlink_next)
 			qh_lines (ehci, qh, &next, &size);
 	}
 	spin_unlock_irqrestore (&ehci->lock, flags);
@@ -705,6 +706,8 @@ static const char *rh_state_string(struct ehci_hcd *ehci)
 		return "suspended";
 	case EHCI_RH_RUNNING:
 		return "running";
+	case EHCI_RH_STOPPING:
+		return "stopping";
 	}
 	return "?";
 }
@@ -841,16 +844,17 @@ static ssize_t fill_registers_buffer(struct debug_buffer *buf)
 		}
 	}
 
-	if (ehci->reclaim) {
-		temp = scnprintf(next, size, "reclaim qh %p\n", ehci->reclaim);
+	if (ehci->async_unlink) {
+		temp = scnprintf(next, size, "async unlink qh %p\n",
+				ehci->async_unlink);
 		size -= temp;
 		next += temp;
 	}
 
 #ifdef EHCI_STATS
 	temp = scnprintf (next, size,
-		"irq normal %ld err %ld reclaim %ld (lost %ld)\n",
-		ehci->stats.normal, ehci->stats.error, ehci->stats.reclaim,
+		"irq normal %ld err %ld iaa %ld (lost %ld)\n",
+		ehci->stats.normal, ehci->stats.error, ehci->stats.iaa,
 		ehci->stats.lost_iaa);
 	size -= temp;
 	next += temp;
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 43362577b54..b7451b29c5a 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -27,6 +27,7 @@
 #include <linux/types.h>
 #include <linux/delay.h>
 #include <linux/pm.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/fsl_devices.h>
 
@@ -142,19 +143,19 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
 		struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 
-		ehci->transceiver = usb_get_transceiver();
-		dev_dbg(&pdev->dev, "hcd=0x%p  ehci=0x%p, transceiver=0x%p\n",
-			hcd, ehci, ehci->transceiver);
+		hcd->phy = usb_get_phy(USB_PHY_TYPE_USB2);
+		dev_dbg(&pdev->dev, "hcd=0x%p  ehci=0x%p, phy=0x%p\n",
+			hcd, ehci, hcd->phy);
 
-		if (ehci->transceiver) {
-			retval = otg_set_host(ehci->transceiver->otg,
+		if (!IS_ERR_OR_NULL(hcd->phy)) {
+			retval = otg_set_host(hcd->phy->otg,
 					      &ehci_to_hcd(ehci)->self);
 			if (retval) {
-				usb_put_transceiver(ehci->transceiver);
+				usb_put_phy(hcd->phy);
 				goto err4;
 			}
 		} else {
-			dev_err(&pdev->dev, "can't find transceiver\n");
+			dev_err(&pdev->dev, "can't find phy\n");
 			retval = -ENODEV;
 			goto err4;
 		}
@@ -190,11 +191,10 @@ static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
 			       struct platform_device *pdev)
 {
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 
-	if (ehci->transceiver) {
-		otg_set_host(ehci->transceiver->otg, NULL);
-		usb_put_transceiver(ehci->transceiver);
+	if (!IS_ERR_OR_NULL(hcd->phy)) {
+		otg_set_host(hcd->phy->otg, NULL);
+		usb_put_phy(hcd->phy);
 	}
 
 	usb_remove_hcd(hcd);
@@ -313,7 +313,7 @@ static void ehci_fsl_usb_setup(struct ehci_hcd *ehci)
 	}
 
 	if (pdata->have_sysif_regs) {
-#ifdef CONFIG_PPC_85xx
+#ifdef CONFIG_FSL_SOC_BOOKE
 		out_be32(non_ehci + FSL_SOC_USB_PRICTRL, 0x00000008);
 		out_be32(non_ehci + FSL_SOC_USB_AGECNTTHRSH, 0x00000080);
 #else
@@ -348,29 +348,13 @@ static int ehci_fsl_setup(struct usb_hcd *hcd)
 
 	/* EHCI registers start at offset 0x100 */
 	ehci->caps = hcd->regs + 0x100;
-	ehci->regs = hcd->regs + 0x100 +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
 	hcd->has_tt = 1;
 
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
-
-	/* data structure init */
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	ehci->sbrn = 0x20;
-
-	ehci_reset(ehci);
-
 	if (of_device_is_compatible(dev->parent->of_node,
 				    "fsl,mpc5121-usb2-dr")) {
 		/*
diff --git a/drivers/usb/host/ehci-grlib.c b/drivers/usb/host/ehci-grlib.c
index fdfd8c5b639..22ca45c079a 100644
--- a/drivers/usb/host/ehci-grlib.c
+++ b/drivers/usb/host/ehci-grlib.c
@@ -40,18 +40,13 @@ static int ehci_grlib_setup(struct usb_hcd *hcd)
 	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
 	int		retval;
 
-	retval = ehci_halt(ehci);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	retval = ehci_init(hcd);
-	if (retval)
-		return retval;
-
-	ehci->sbrn = 0x20;
 	ehci_port_power(ehci, 1);
 
-	return ehci_reset(ehci);
+	return retval;
 }
 
 
@@ -164,12 +159,6 @@ static int __devinit ehci_hcd_grlib_probe(struct platform_device *op)
 		ehci->big_endian_capbase = 1;
 	}
 
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
 	rv = usb_add_hcd(hcd, irq, 0);
 	if (rv)
 		goto err_ehci;
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 800be38c78b..b05c6865b61 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -30,8 +30,7 @@
 #include <linux/vmalloc.h>
 #include <linux/errno.h>
 #include <linux/init.h>
-#include <linux/timer.h>
-#include <linux/ktime.h>
+#include <linux/hrtimer.h>
 #include <linux/list.h>
 #include <linux/interrupt.h>
 #include <linux/usb.h>
@@ -94,12 +93,6 @@ static const char	hcd_name [] = "ehci_hcd";
  */
 #define	EHCI_TUNE_FLS		1	/* (medium) 512-frame schedule */
 
-#define EHCI_IAA_MSECS		10		/* arbitrary */
-#define EHCI_IO_JIFFIES		(HZ/10)		/* io watchdog > irq_thresh */
-#define EHCI_ASYNC_JIFFIES	(HZ/20)		/* async idle timeout */
-#define EHCI_SHRINK_JIFFIES	(DIV_ROUND_UP(HZ, 200) + 1)
-						/* 5-ms async qh unlink delay */
-
 /* Initial IRQ latency:  faster than hw default */
 static int log2_irq_thresh = 0;		// 0 to 6
 module_param (log2_irq_thresh, int, S_IRUGO);
@@ -130,41 +123,6 @@ MODULE_PARM_DESC(hird, "host initiated resume duration, +1 for each 75us");
 
 /*-------------------------------------------------------------------------*/
 
-static void
-timer_action(struct ehci_hcd *ehci, enum ehci_timer_action action)
-{
-	/* Don't override timeouts which shrink or (later) disable
-	 * the async ring; just the I/O watchdog.  Note that if a
-	 * SHRINK were pending, OFF would never be requested.
-	 */
-	if (timer_pending(&ehci->watchdog)
-			&& ((BIT(TIMER_ASYNC_SHRINK) | BIT(TIMER_ASYNC_OFF))
-				& ehci->actions))
-		return;
-
-	if (!test_and_set_bit(action, &ehci->actions)) {
-		unsigned long t;
-
-		switch (action) {
-		case TIMER_IO_WATCHDOG:
-			if (!ehci->need_io_watchdog)
-				return;
-			t = EHCI_IO_JIFFIES;
-			break;
-		case TIMER_ASYNC_OFF:
-			t = EHCI_ASYNC_JIFFIES;
-			break;
-		/* case TIMER_ASYNC_SHRINK: */
-		default:
-			t = EHCI_SHRINK_JIFFIES;
-			break;
-		}
-		mod_timer(&ehci->watchdog, t + jiffies);
-	}
-}
-
-/*-------------------------------------------------------------------------*/
-
 /*
  * handshake - spin reading hc until handshake completes or fails
  * @ptr: address of hc register to be read
@@ -203,29 +161,30 @@ static int handshake (struct ehci_hcd *ehci, void __iomem *ptr,
 /* check TDI/ARC silicon is in host mode */
 static int tdi_in_host_mode (struct ehci_hcd *ehci)
 {
-	u32 __iomem	*reg_ptr;
 	u32		tmp;
 
-	reg_ptr = (u32 __iomem *)(((u8 __iomem *)ehci->regs) + USBMODE);
-	tmp = ehci_readl(ehci, reg_ptr);
+	tmp = ehci_readl(ehci, &ehci->regs->usbmode);
 	return (tmp & 3) == USBMODE_CM_HC;
 }
 
-/* force HC to halt state from unknown (EHCI spec section 2.3) */
+/*
+ * Force HC to halt state from unknown (EHCI spec section 2.3).
+ * Must be called with interrupts enabled and the lock not held.
+ */
 static int ehci_halt (struct ehci_hcd *ehci)
 {
-	u32	temp = ehci_readl(ehci, &ehci->regs->status);
+	u32	temp;
+
+	spin_lock_irq(&ehci->lock);
 
 	/* disable any irqs left enabled by previous code */
 	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
 
-	if (ehci_is_TDI(ehci) && tdi_in_host_mode(ehci) == 0) {
+	if (ehci_is_TDI(ehci) && !tdi_in_host_mode(ehci)) {
+		spin_unlock_irq(&ehci->lock);
 		return 0;
 	}
 
-	if ((temp & STS_HALT) != 0)
-		return 0;
-
 	/*
 	 * This routine gets called during probe before ehci->command
 	 * has been initialized, so we can't rely on its value.
@@ -234,80 +193,20 @@ static int ehci_halt (struct ehci_hcd *ehci)
 	temp = ehci_readl(ehci, &ehci->regs->command);
 	temp &= ~(CMD_RUN | CMD_IAAD);
 	ehci_writel(ehci, temp, &ehci->regs->command);
-	return handshake (ehci, &ehci->regs->status,
-			  STS_HALT, STS_HALT, 16 * 125);
-}
-
-#if defined(CONFIG_USB_SUSPEND) && defined(CONFIG_PPC_PS3)
-
-/*
- * The EHCI controller of the Cell Super Companion Chip used in the
- * PS3 will stop the root hub after all root hub ports are suspended.
- * When in this condition handshake will return -ETIMEDOUT.  The
- * STS_HLT bit will not be set, so inspection of the frame index is
- * used here to test for the condition.  If the condition is found
- * return success to allow the USB suspend to complete.
- */
 
-static int handshake_for_broken_root_hub(struct ehci_hcd *ehci,
-					 void __iomem *ptr, u32 mask, u32 done,
-					 int usec)
-{
-	unsigned int old_index;
-	int error;
-
-	if (!firmware_has_feature(FW_FEATURE_PS3_LV1))
-		return -ETIMEDOUT;
-
-	old_index = ehci_read_frame_index(ehci);
-
-	error = handshake(ehci, ptr, mask, done, usec);
-
-	if (error == -ETIMEDOUT && ehci_read_frame_index(ehci) == old_index)
-		return 0;
-
-	return error;
-}
-
-#else
-
-static int handshake_for_broken_root_hub(struct ehci_hcd *ehci,
-					 void __iomem *ptr, u32 mask, u32 done,
-					 int usec)
-{
-	return -ETIMEDOUT;
-}
-
-#endif
-
-static int handshake_on_error_set_halt(struct ehci_hcd *ehci, void __iomem *ptr,
-				       u32 mask, u32 done, int usec)
-{
-	int error;
-
-	error = handshake(ehci, ptr, mask, done, usec);
-	if (error == -ETIMEDOUT)
-		error = handshake_for_broken_root_hub(ehci, ptr, mask, done,
-						      usec);
-
-	if (error) {
-		ehci_halt(ehci);
-		ehci->rh_state = EHCI_RH_HALTED;
-		ehci_err(ehci, "force halt; handshake %p %08x %08x -> %d\n",
-			ptr, mask, done, error);
-	}
+	spin_unlock_irq(&ehci->lock);
+	synchronize_irq(ehci_to_hcd(ehci)->irq);
 
-	return error;
+	return handshake(ehci, &ehci->regs->status,
+			  STS_HALT, STS_HALT, 16 * 125);
 }
 
 /* put TDI/ARC silicon into EHCI mode */
 static void tdi_reset (struct ehci_hcd *ehci)
 {
-	u32 __iomem	*reg_ptr;
 	u32		tmp;
 
-	reg_ptr = (u32 __iomem *)(((u8 __iomem *)ehci->regs) + USBMODE);
-	tmp = ehci_readl(ehci, reg_ptr);
+	tmp = ehci_readl(ehci, &ehci->regs->usbmode);
 	tmp |= USBMODE_CM_HC;
 	/* The default byte access to MMR space is LE after
 	 * controller reset. Set the required endian mode
@@ -315,10 +214,13 @@ static void tdi_reset (struct ehci_hcd *ehci)
 	 */
 	if (ehci_big_endian_mmio(ehci))
 		tmp |= USBMODE_BE;
-	ehci_writel(ehci, tmp, reg_ptr);
+	ehci_writel(ehci, tmp, &ehci->regs->usbmode);
 }
 
-/* reset a non-running (STS_HALT == 1) controller */
+/*
+ * Reset a non-running (STS_HALT == 1) controller.
+ * Must be called with interrupts enabled and the lock not held.
+ */
 static int ehci_reset (struct ehci_hcd *ehci)
 {
 	int	retval;
@@ -339,9 +241,8 @@ static int ehci_reset (struct ehci_hcd *ehci)
 
 	if (ehci->has_hostpc) {
 		ehci_writel(ehci, USBMODE_EX_HC | USBMODE_EX_VBPS,
-			(u32 __iomem *)(((u8 *)ehci->regs) + USBMODE_EX));
-		ehci_writel(ehci, TXFIFO_DEFAULT,
-			(u32 __iomem *)(((u8 *)ehci->regs) + TXFILLTUNING));
+				&ehci->regs->usbmode_ex);
+		ehci_writel(ehci, TXFIFO_DEFAULT, &ehci->regs->txfill_tuning);
 	}
 	if (retval)
 		return retval;
@@ -357,36 +258,40 @@ static int ehci_reset (struct ehci_hcd *ehci)
 	return retval;
 }
 
-/* idle the controller (from running) */
+/*
+ * Idle the controller (turn off the schedules).
+ * Must be called with interrupts enabled and the lock not held.
+ */
 static void ehci_quiesce (struct ehci_hcd *ehci)
 {
 	u32	temp;
 
-#ifdef DEBUG
 	if (ehci->rh_state != EHCI_RH_RUNNING)
-		BUG ();
-#endif
+		return;
 
 	/* wait for any schedule enables/disables to take effect */
 	temp = (ehci->command << 10) & (STS_ASS | STS_PSS);
-	if (handshake_on_error_set_halt(ehci, &ehci->regs->status,
-					STS_ASS | STS_PSS, temp, 16 * 125))
-		return;
+	handshake(ehci, &ehci->regs->status, STS_ASS | STS_PSS, temp, 16 * 125);
 
 	/* then disable anything that's still active */
+	spin_lock_irq(&ehci->lock);
 	ehci->command &= ~(CMD_ASE | CMD_PSE);
 	ehci_writel(ehci, ehci->command, &ehci->regs->command);
+	spin_unlock_irq(&ehci->lock);
 
 	/* hardware can take 16 microframes to turn off ... */
-	handshake_on_error_set_halt(ehci, &ehci->regs->status,
-				    STS_ASS | STS_PSS, 0, 16 * 125);
+	handshake(ehci, &ehci->regs->status, STS_ASS | STS_PSS, 0, 16 * 125);
 }
 
 /*-------------------------------------------------------------------------*/
 
 static void end_unlink_async(struct ehci_hcd *ehci);
+static void unlink_empty_async(struct ehci_hcd *ehci);
 static void ehci_work(struct ehci_hcd *ehci);
+static void start_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh);
+static void end_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh);
 
+#include "ehci-timer.c"
 #include "ehci-hub.c"
 #include "ehci-lpm.c"
 #include "ehci-mem.c"
@@ -396,68 +301,6 @@ static void ehci_work(struct ehci_hcd *ehci);
 
 /*-------------------------------------------------------------------------*/
 
-static void ehci_iaa_watchdog(unsigned long param)
-{
-	struct ehci_hcd		*ehci = (struct ehci_hcd *) param;
-	unsigned long		flags;
-
-	spin_lock_irqsave (&ehci->lock, flags);
-
-	/* Lost IAA irqs wedge things badly; seen first with a vt8235.
-	 * So we need this watchdog, but must protect it against both
-	 * (a) SMP races against real IAA firing and retriggering, and
-	 * (b) clean HC shutdown, when IAA watchdog was pending.
-	 */
-	if (ehci->reclaim
-			&& !timer_pending(&ehci->iaa_watchdog)
-			&& ehci->rh_state == EHCI_RH_RUNNING) {
-		u32 cmd, status;
-
-		/* If we get here, IAA is *REALLY* late.  It's barely
-		 * conceivable that the system is so busy that CMD_IAAD
-		 * is still legitimately set, so let's be sure it's
-		 * clear before we read STS_IAA.  (The HC should clear
-		 * CMD_IAAD when it sets STS_IAA.)
-		 */
-		cmd = ehci_readl(ehci, &ehci->regs->command);
-
-		/* If IAA is set here it either legitimately triggered
-		 * before we cleared IAAD above (but _way_ late, so we'll
-		 * still count it as lost) ... or a silicon erratum:
-		 * - VIA seems to set IAA without triggering the IRQ;
-		 * - IAAD potentially cleared without setting IAA.
-		 */
-		status = ehci_readl(ehci, &ehci->regs->status);
-		if ((status & STS_IAA) || !(cmd & CMD_IAAD)) {
-			COUNT (ehci->stats.lost_iaa);
-			ehci_writel(ehci, STS_IAA, &ehci->regs->status);
-		}
-
-		ehci_vdbg(ehci, "IAA watchdog: status %x cmd %x\n",
-				status, cmd);
-		end_unlink_async(ehci);
-	}
-
-	spin_unlock_irqrestore(&ehci->lock, flags);
-}
-
-static void ehci_watchdog(unsigned long param)
-{
-	struct ehci_hcd		*ehci = (struct ehci_hcd *) param;
-	unsigned long		flags;
-
-	spin_lock_irqsave(&ehci->lock, flags);
-
-	/* stop async processing after it's idled a bit */
-	if (test_bit (TIMER_ASYNC_OFF, &ehci->actions))
-		start_unlink_async (ehci, ehci->async);
-
-	/* ehci could run by timer, without IRQs ... */
-	ehci_work (ehci);
-
-	spin_unlock_irqrestore (&ehci->lock, flags);
-}
-
 /* On some systems, leaving remote wakeup enabled prevents system shutdown.
  * The firmware seems to think that powering off is a wakeup event!
  * This routine turns off remote wakeup and everything else, on all ports.
@@ -473,11 +316,14 @@ static void ehci_turn_off_all_ports(struct ehci_hcd *ehci)
 
 /*
  * Halt HC, turn off all ports, and let the BIOS use the companion controllers.
- * Should be called with ehci->lock held.
+ * Must be called with interrupts enabled and the lock not held.
  */
 static void ehci_silence_controller(struct ehci_hcd *ehci)
 {
 	ehci_halt(ehci);
+
+	spin_lock_irq(&ehci->lock);
+	ehci->rh_state = EHCI_RH_HALTED;
 	ehci_turn_off_all_ports(ehci);
 
 	/* make BIOS/etc use companion controller during reboot */
@@ -485,6 +331,7 @@ static void ehci_silence_controller(struct ehci_hcd *ehci)
 
 	/* unblock posted writes */
 	ehci_readl(ehci, &ehci->regs->configured_flag);
+	spin_unlock_irq(&ehci->lock);
 }
 
 /* ehci_shutdown kick in for silicon on any bus (not just pci, etc).
@@ -495,12 +342,15 @@ static void ehci_shutdown(struct usb_hcd *hcd)
 {
 	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
 
-	del_timer_sync(&ehci->watchdog);
-	del_timer_sync(&ehci->iaa_watchdog);
-
 	spin_lock_irq(&ehci->lock);
-	ehci_silence_controller(ehci);
+	ehci->shutdown = true;
+	ehci->rh_state = EHCI_RH_STOPPING;
+	ehci->enabled_hrtimer_events = 0;
 	spin_unlock_irq(&ehci->lock);
+
+	ehci_silence_controller(ehci);
+
+	hrtimer_cancel(&ehci->hrtimer);
 }
 
 static void ehci_port_power (struct ehci_hcd *ehci, int is_on)
@@ -529,28 +379,33 @@ static void ehci_port_power (struct ehci_hcd *ehci, int is_on)
  */
 static void ehci_work (struct ehci_hcd *ehci)
 {
-	timer_action_done (ehci, TIMER_IO_WATCHDOG);
-
 	/* another CPU may drop ehci->lock during a schedule scan while
 	 * it reports urb completions.  this flag guards against bogus
 	 * attempts at re-entrant schedule scanning.
 	 */
-	if (ehci->scanning)
+	if (ehci->scanning) {
+		ehci->need_rescan = true;
 		return;
-	ehci->scanning = 1;
-	scan_async (ehci);
-	if (ehci->next_uframe != -1)
-		scan_periodic (ehci);
-	ehci->scanning = 0;
+	}
+	ehci->scanning = true;
+
+ rescan:
+	ehci->need_rescan = false;
+	if (ehci->async_count)
+		scan_async(ehci);
+	if (ehci->intr_count > 0)
+		scan_intr(ehci);
+	if (ehci->isoc_count > 0)
+		scan_isoc(ehci);
+	if (ehci->need_rescan)
+		goto rescan;
+	ehci->scanning = false;
 
 	/* the IO watchdog guards against hardware or driver bugs that
 	 * misplace IRQs, and should let us run completely without IRQs.
 	 * such lossage has been observed on both VT6202 and VT8235.
 	 */
-	if (ehci->rh_state == EHCI_RH_RUNNING &&
-			(ehci->async->qh_next.ptr != NULL ||
-			 ehci->periodic_sched != 0))
-		timer_action (ehci, TIMER_IO_WATCHDOG);
+	turn_on_io_watchdog(ehci);
 }
 
 /*
@@ -563,24 +418,22 @@ static void ehci_stop (struct usb_hcd *hcd)
 	ehci_dbg (ehci, "stop\n");
 
 	/* no more interrupts ... */
-	del_timer_sync (&ehci->watchdog);
-	del_timer_sync(&ehci->iaa_watchdog);
 
 	spin_lock_irq(&ehci->lock);
-	if (ehci->rh_state == EHCI_RH_RUNNING)
-		ehci_quiesce (ehci);
+	ehci->enabled_hrtimer_events = 0;
+	spin_unlock_irq(&ehci->lock);
 
+	ehci_quiesce(ehci);
 	ehci_silence_controller(ehci);
 	ehci_reset (ehci);
-	spin_unlock_irq(&ehci->lock);
 
+	hrtimer_cancel(&ehci->hrtimer);
 	remove_sysfs_files(ehci);
 	remove_debug_files (ehci);
 
 	/* root hub is shut down separately (first, when possible) */
 	spin_lock_irq (&ehci->lock);
-	if (ehci->async)
-		ehci_work (ehci);
+	end_free_itds(ehci);
 	spin_unlock_irq (&ehci->lock);
 	ehci_mem_cleanup (ehci);
 
@@ -588,8 +441,8 @@ static void ehci_stop (struct usb_hcd *hcd)
 		usb_amd_dev_put();
 
 #ifdef	EHCI_STATS
-	ehci_dbg (ehci, "irq normal %ld err %ld reclaim %ld (lost %ld)\n",
-		ehci->stats.normal, ehci->stats.error, ehci->stats.reclaim,
+	ehci_dbg(ehci, "irq normal %ld err %ld iaa %ld (lost %ld)\n",
+		ehci->stats.normal, ehci->stats.error, ehci->stats.iaa,
 		ehci->stats.lost_iaa);
 	ehci_dbg (ehci, "complete %ld unlink %ld\n",
 		ehci->stats.complete, ehci->stats.unlink);
@@ -614,13 +467,10 @@ static int ehci_init(struct usb_hcd *hcd)
 	 * keep io watchdog by default, those good HCDs could turn off it later
 	 */
 	ehci->need_io_watchdog = 1;
-	init_timer(&ehci->watchdog);
-	ehci->watchdog.function = ehci_watchdog;
-	ehci->watchdog.data = (unsigned long) ehci;
 
-	init_timer(&ehci->iaa_watchdog);
-	ehci->iaa_watchdog.function = ehci_iaa_watchdog;
-	ehci->iaa_watchdog.data = (unsigned long) ehci;
+	hrtimer_init(&ehci->hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+	ehci->hrtimer.function = ehci_hrtimer_func;
+	ehci->next_hrtimer_event = EHCI_HRTIMER_NO_EVENT;
 
 	hcc_params = ehci_readl(ehci, &ehci->caps->hcc_params);
 
@@ -635,6 +485,7 @@ static int ehci_init(struct usb_hcd *hcd)
 	 * periodic_size can shrink by USBCMD update if hcc_params allows.
 	 */
 	ehci->periodic_size = DEFAULT_I_TDPS;
+	INIT_LIST_HEAD(&ehci->intr_qh_list);
 	INIT_LIST_HEAD(&ehci->cached_itd_list);
 	INIT_LIST_HEAD(&ehci->cached_sitd_list);
 
@@ -656,10 +507,6 @@ static int ehci_init(struct usb_hcd *hcd)
 	else					// N microframes cached
 		ehci->i_thresh = 2 + HCC_ISOC_THRES(hcc_params);
 
-	ehci->reclaim = NULL;
-	ehci->next_uframe = -1;
-	ehci->clock_frame = -1;
-
 	/*
 	 * dedicate a qh for the async ring head, since we couldn't unlink
 	 * a 'real' qh without stopping the async schedule [4.8].  use it
@@ -672,7 +519,7 @@ static int ehci_init(struct usb_hcd *hcd)
 	hw->hw_next = QH_NEXT(ehci, ehci->async->qh_dma);
 	hw->hw_info1 = cpu_to_hc32(ehci, QH_HEAD);
 #if defined(CONFIG_PPC_PS3)
-	hw->hw_info1 |= cpu_to_hc32(ehci, (1 << 7));	/* I = 1 */
+	hw->hw_info1 |= cpu_to_hc32(ehci, QH_INACTIVATE);
 #endif
 	hw->hw_token = cpu_to_hc32(ehci, QTD_STS_HALT);
 	hw->hw_qtd_next = EHCI_LIST_END(ehci);
@@ -813,7 +660,7 @@ static int ehci_run (struct usb_hcd *hcd)
 	return 0;
 }
 
-static int __maybe_unused ehci_setup (struct usb_hcd *hcd)
+static int ehci_setup(struct usb_hcd *hcd)
 {
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	int retval;
@@ -828,15 +675,18 @@ static int __maybe_unused ehci_setup (struct usb_hcd *hcd)
 
 	ehci->sbrn = HCD_USB2;
 
-	retval = ehci_halt(ehci);
+	/* data structure init */
+	retval = ehci_init(hcd);
 	if (retval)
 		return retval;
 
-	/* data structure init */
-	retval = ehci_init(hcd);
+	retval = ehci_halt(ehci);
 	if (retval)
 		return retval;
 
+	if (ehci_is_TDI(ehci))
+		tdi_reset(ehci);
+
 	ehci_reset(ehci);
 
 	return 0;
@@ -895,14 +745,28 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 
 	/* complete the unlinking of some qh [4.15.2.3] */
 	if (status & STS_IAA) {
+
+		/* Turn off the IAA watchdog */
+		ehci->enabled_hrtimer_events &= ~BIT(EHCI_HRTIMER_IAA_WATCHDOG);
+
+		/*
+		 * Mild optimization: Allow another IAAD to reset the
+		 * hrtimer, if one occurs before the next expiration.
+		 * In theory we could always cancel the hrtimer, but
+		 * tests show that about half the time it will be reset
+		 * for some other event anyway.
+		 */
+		if (ehci->next_hrtimer_event == EHCI_HRTIMER_IAA_WATCHDOG)
+			++ehci->next_hrtimer_event;
+
 		/* guard against (alleged) silicon errata */
 		if (cmd & CMD_IAAD)
 			ehci_dbg(ehci, "IAA with IAAD still set?\n");
-		if (ehci->reclaim) {
-			COUNT(ehci->stats.reclaim);
+		if (ehci->async_iaa) {
+			COUNT(ehci->stats.iaa);
 			end_unlink_async(ehci);
 		} else
-			ehci_dbg(ehci, "IAA with nothing to reclaim?\n");
+			ehci_dbg(ehci, "IAA with nothing unlinked?\n");
 	}
 
 	/* remote wakeup [4.3.1] */
@@ -956,15 +820,19 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 		ehci_err(ehci, "fatal error\n");
 		dbg_cmd(ehci, "fatal", cmd);
 		dbg_status(ehci, "fatal", status);
-		ehci_halt(ehci);
 dead:
-		ehci_reset(ehci);
-		ehci_writel(ehci, 0, &ehci->regs->configured_flag);
 		usb_hc_died(hcd);
-		/* generic layer kills/unlinks all urbs, then
-		 * uses ehci_stop to clean up the rest
-		 */
-		bh = 1;
+
+		/* Don't let the controller do anything more */
+		ehci->shutdown = true;
+		ehci->rh_state = EHCI_RH_STOPPING;
+		ehci->command &= ~(CMD_RUN | CMD_ASE | CMD_PSE);
+		ehci_writel(ehci, ehci->command, &ehci->regs->command);
+		ehci_writel(ehci, 0, &ehci->regs->intr_enable);
+		ehci_handle_controller_death(ehci);
+
+		/* Handle completions when the controller stops */
+		bh = 0;
 	}
 
 	if (bh)
@@ -1026,38 +894,6 @@ static int ehci_urb_enqueue (
 	}
 }
 
-static void unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
-{
-	/* failfast */
-	if (ehci->rh_state != EHCI_RH_RUNNING && ehci->reclaim)
-		end_unlink_async(ehci);
-
-	/* If the QH isn't linked then there's nothing we can do
-	 * unless we were called during a giveback, in which case
-	 * qh_completions() has to deal with it.
-	 */
-	if (qh->qh_state != QH_STATE_LINKED) {
-		if (qh->qh_state == QH_STATE_COMPLETING)
-			qh->needs_rescan = 1;
-		return;
-	}
-
-	/* defer till later if busy */
-	if (ehci->reclaim) {
-		struct ehci_qh		*last;
-
-		for (last = ehci->reclaim;
-				last->reclaim;
-				last = last->reclaim)
-			continue;
-		qh->qh_state = QH_STATE_UNLINK_WAIT;
-		last->reclaim = qh;
-
-	/* start IAA cycle */
-	} else
-		start_unlink_async (ehci, qh);
-}
-
 /* remove from hardware lists
  * completions normally happen asynchronously
  */
@@ -1084,7 +920,7 @@ static int ehci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 		switch (qh->qh_state) {
 		case QH_STATE_LINKED:
 		case QH_STATE_COMPLETING:
-			unlink_async(ehci, qh);
+			start_unlink_async(ehci, qh);
 			break;
 		case QH_STATE_UNLINK:
 		case QH_STATE_UNLINK_WAIT:
@@ -1104,7 +940,7 @@ static int ehci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 		switch (qh->qh_state) {
 		case QH_STATE_LINKED:
 		case QH_STATE_COMPLETING:
-			intr_deschedule (ehci, qh);
+			start_unlink_intr(ehci, qh);
 			break;
 		case QH_STATE_IDLE:
 			qh_completions (ehci, qh);
@@ -1152,11 +988,17 @@ rescan:
 	 * accelerate iso completions ... so spin a while.
 	 */
 	if (qh->hw == NULL) {
-		ehci_vdbg (ehci, "iso delay\n");
-		goto idle_timeout;
+		struct ehci_iso_stream	*stream = ep->hcpriv;
+
+		if (!list_empty(&stream->td_list))
+			goto idle_timeout;
+
+		/* BUG_ON(!list_empty(&stream->free_list)); */
+		kfree(stream);
+		goto done;
 	}
 
-	if (ehci->rh_state != EHCI_RH_RUNNING)
+	if (ehci->rh_state < EHCI_RH_RUNNING)
 		qh->qh_state = QH_STATE_IDLE;
 	switch (qh->qh_state) {
 	case QH_STATE_LINKED:
@@ -1169,7 +1011,7 @@ rescan:
 		 * may already be unlinked.
 		 */
 		if (tmp)
-			unlink_async(ehci, qh);
+			start_unlink_async(ehci, qh);
 		/* FALL THROUGH */
 	case QH_STATE_UNLINK:		/* wait for hw to finish? */
 	case QH_STATE_UNLINK_WAIT:
@@ -1181,7 +1023,7 @@ idle_timeout:
 		if (qh->clearing_tt)
 			goto idle_timeout;
 		if (list_empty (&qh->qtd_list)) {
-			qh_put (qh);
+			qh_destroy(ehci, qh);
 			break;
 		}
 		/* else FALL THROUGH */
@@ -1194,8 +1036,8 @@ idle_timeout:
 			list_empty (&qh->qtd_list) ? "" : "(has tds)");
 		break;
 	}
+ done:
 	ep->hcpriv = NULL;
-done:
 	spin_unlock_irqrestore (&ehci->lock, flags);
 }
 
@@ -1232,9 +1074,9 @@ ehci_endpoint_reset(struct usb_hcd *hcd, struct usb_host_endpoint *ep)
 			 * re-linking will call qh_refresh().
 			 */
 			if (eptype == USB_ENDPOINT_XFER_BULK)
-				unlink_async(ehci, qh);
+				start_unlink_async(ehci, qh);
 			else
-				intr_deschedule(ehci, qh);
+				start_unlink_intr(ehci, qh);
 		}
 	}
 	spin_unlock_irqrestore(&ehci->lock, flags);
@@ -1247,6 +1089,104 @@ static int ehci_get_frame (struct usb_hcd *hcd)
 }
 
 /*-------------------------------------------------------------------------*/
+
+#ifdef	CONFIG_PM
+
+/* suspend/resume, section 4.3 */
+
+/* These routines handle the generic parts of controller suspend/resume */
+
+static int __maybe_unused ehci_suspend(struct usb_hcd *hcd, bool do_wakeup)
+{
+	struct ehci_hcd		*ehci = hcd_to_ehci(hcd);
+
+	if (time_before(jiffies, ehci->next_statechange))
+		msleep(10);
+
+	/*
+	 * Root hub was already suspended.  Disable IRQ emission and
+	 * mark HW unaccessible.  The PM and USB cores make sure that
+	 * the root hub is either suspended or stopped.
+	 */
+	ehci_prepare_ports_for_controller_suspend(ehci, do_wakeup);
+
+	spin_lock_irq(&ehci->lock);
+	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
+	(void) ehci_readl(ehci, &ehci->regs->intr_enable);
+
+	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+	spin_unlock_irq(&ehci->lock);
+
+	return 0;
+}
+
+/* Returns 0 if power was preserved, 1 if power was lost */
+static int __maybe_unused ehci_resume(struct usb_hcd *hcd, bool hibernated)
+{
+	struct ehci_hcd		*ehci = hcd_to_ehci(hcd);
+
+	if (time_before(jiffies, ehci->next_statechange))
+		msleep(100);
+
+	/* Mark hardware accessible again as we are back to full power by now */
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	if (ehci->shutdown)
+		return 0;		/* Controller is dead */
+
+	/*
+	 * If CF is still set and we aren't resuming from hibernation
+	 * then we maintained suspend power.
+	 * Just undo the effect of ehci_suspend().
+	 */
+	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF &&
+			!hibernated) {
+		int	mask = INTR_MASK;
+
+		ehci_prepare_ports_for_controller_resume(ehci);
+
+		spin_lock_irq(&ehci->lock);
+		if (ehci->shutdown)
+			goto skip;
+
+		if (!hcd->self.root_hub->do_remote_wakeup)
+			mask &= ~STS_PCD;
+		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
+		ehci_readl(ehci, &ehci->regs->intr_enable);
+ skip:
+		spin_unlock_irq(&ehci->lock);
+		return 0;
+	}
+
+	/*
+	 * Else reset, to cope with power loss or resume from hibernation
+	 * having let the firmware kick in during reboot.
+	 */
+	usb_root_hub_lost_power(hcd->self.root_hub);
+	(void) ehci_halt(ehci);
+	(void) ehci_reset(ehci);
+
+	spin_lock_irq(&ehci->lock);
+	if (ehci->shutdown)
+		goto skip;
+
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
+	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
+	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
+
+	ehci->rh_state = EHCI_RH_SUSPENDED;
+	spin_unlock_irq(&ehci->lock);
+
+	/* here we "know" root ports should always stay powered */
+	ehci_port_power(ehci, 1);
+
+	return 1;
+}
+
+#endif
+
+/*-------------------------------------------------------------------------*/
+
 /*
  * The EHCI in ChipIdea HDRC cannot be a separate module or device,
  * because its registers (and irq) are shared between host/gadget/otg
@@ -1349,6 +1289,11 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ehci_msm_driver
 #endif
 
+#ifdef CONFIG_TILE_USB
+#include "ehci-tilegx.c"
+#define	PLATFORM_DRIVER		ehci_hcd_tilegx_driver
+#endif
+
 #ifdef CONFIG_USB_EHCI_HCD_PMC_MSP
 #include "ehci-pmcmsp.c"
 #define	PLATFORM_DRIVER		ehci_hcd_msp_driver
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index fc9e7cc6ac9..c7880223738 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -59,6 +59,7 @@ static void ehci_handover_companion_ports(struct ehci_hcd *ehci)
 	/* Give the connections some time to appear */
 	msleep(20);
 
+	spin_lock_irq(&ehci->lock);
 	port = HCS_N_PORTS(ehci->hcs_params);
 	while (port--) {
 		if (test_bit(port, &ehci->owned_ports)) {
@@ -70,23 +71,30 @@ static void ehci_handover_companion_ports(struct ehci_hcd *ehci)
 				clear_bit(port, &ehci->owned_ports);
 			else if (test_bit(port, &ehci->companion_ports))
 				ehci_writel(ehci, status & ~PORT_PE, reg);
-			else
+			else {
+				spin_unlock_irq(&ehci->lock);
 				ehci_hub_control(hcd, SetPortFeature,
 						USB_PORT_FEAT_RESET, port + 1,
 						NULL, 0);
+				spin_lock_irq(&ehci->lock);
+			}
 		}
 	}
+	spin_unlock_irq(&ehci->lock);
 
 	if (!ehci->owned_ports)
 		return;
 	msleep(90);		/* Wait for resets to complete */
 
+	spin_lock_irq(&ehci->lock);
 	port = HCS_N_PORTS(ehci->hcs_params);
 	while (port--) {
 		if (test_bit(port, &ehci->owned_ports)) {
+			spin_unlock_irq(&ehci->lock);
 			ehci_hub_control(hcd, GetPortStatus,
 					0, port + 1,
 					(char *) &buf, sizeof(buf));
+			spin_lock_irq(&ehci->lock);
 
 			/* The companion should now own the port,
 			 * but if something went wrong the port must not
@@ -105,9 +113,10 @@ static void ehci_handover_companion_ports(struct ehci_hcd *ehci)
 	}
 
 	ehci->owned_ports = 0;
+	spin_unlock_irq(&ehci->lock);
 }
 
-static int __maybe_unused ehci_port_change(struct ehci_hcd *ehci)
+static int ehci_port_change(struct ehci_hcd *ehci)
 {
 	int i = HCS_N_PORTS(ehci->hcs_params);
 
@@ -128,12 +137,11 @@ static int __maybe_unused ehci_port_change(struct ehci_hcd *ehci)
 	return 0;
 }
 
-static __maybe_unused void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
+static void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 		bool suspending, bool do_wakeup)
 {
 	int		port;
 	u32		temp;
-	unsigned long	flags;
 
 	/* If remote wakeup is enabled for the root hub but disabled
 	 * for the controller, we must adjust all the port wakeup flags
@@ -143,22 +151,20 @@ static __maybe_unused void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 	if (!ehci_to_hcd(ehci)->self.root_hub->do_remote_wakeup || do_wakeup)
 		return;
 
-	spin_lock_irqsave(&ehci->lock, flags);
+	spin_lock_irq(&ehci->lock);
 
 	/* clear phy low-power mode before changing wakeup flags */
 	if (ehci->has_hostpc) {
 		port = HCS_N_PORTS(ehci->hcs_params);
 		while (port--) {
-			u32 __iomem	*hostpc_reg;
+			u32 __iomem	*hostpc_reg = &ehci->regs->hostpc[port];
 
-			hostpc_reg = (u32 __iomem *)((u8 *) ehci->regs
-					+ HOSTPC0 + 4 * port);
 			temp = ehci_readl(ehci, hostpc_reg);
 			ehci_writel(ehci, temp & ~HOSTPC_PHCD, hostpc_reg);
 		}
-		spin_unlock_irqrestore(&ehci->lock, flags);
+		spin_unlock_irq(&ehci->lock);
 		msleep(5);
-		spin_lock_irqsave(&ehci->lock, flags);
+		spin_lock_irq(&ehci->lock);
 	}
 
 	port = HCS_N_PORTS(ehci->hcs_params);
@@ -185,10 +191,8 @@ static __maybe_unused void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 	if (ehci->has_hostpc) {
 		port = HCS_N_PORTS(ehci->hcs_params);
 		while (port--) {
-			u32 __iomem	*hostpc_reg;
+			u32 __iomem	*hostpc_reg = &ehci->regs->hostpc[port];
 
-			hostpc_reg = (u32 __iomem *)((u8 *) ehci->regs
-					+ HOSTPC0 + 4 * port);
 			temp = ehci_readl(ehci, hostpc_reg);
 			ehci_writel(ehci, temp | HOSTPC_PHCD, hostpc_reg);
 		}
@@ -198,7 +202,7 @@ static __maybe_unused void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 	if (!suspending && ehci_port_change(ehci))
 		usb_hcd_resume_root_hub(ehci_to_hcd(ehci));
 
-	spin_unlock_irqrestore(&ehci->lock, flags);
+	spin_unlock_irq(&ehci->lock);
 }
 
 static int ehci_bus_suspend (struct usb_hcd *hcd)
@@ -212,10 +216,13 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 
 	if (time_before (jiffies, ehci->next_statechange))
 		msleep(5);
-	del_timer_sync(&ehci->watchdog);
-	del_timer_sync(&ehci->iaa_watchdog);
+
+	/* stop the schedules */
+	ehci_quiesce(ehci);
 
 	spin_lock_irq (&ehci->lock);
+	if (ehci->rh_state < EHCI_RH_RUNNING)
+		goto done;
 
 	/* Once the controller is stopped, port resumes that are already
 	 * in progress won't complete.  Hence if remote wakeup is enabled
@@ -230,11 +237,6 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 		}
 	}
 
-	/* stop schedules, clean any completed work */
-	if (ehci->rh_state == EHCI_RH_RUNNING)
-		ehci_quiesce (ehci);
-	ehci_work(ehci);
-
 	/* Unlike other USB host controller types, EHCI doesn't have
 	 * any notion of "global" or bus-wide suspend.  The driver has
 	 * to manually suspend all the active unsuspended ports, and
@@ -285,11 +287,9 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 
 		port = HCS_N_PORTS(ehci->hcs_params);
 		while (port--) {
-			u32 __iomem	*hostpc_reg;
+			u32 __iomem	*hostpc_reg = &ehci->regs->hostpc[port];
 			u32		t3;
 
-			hostpc_reg = (u32 __iomem *)((u8 *) ehci->regs
-					+ HOSTPC0 + 4 * port);
 			t3 = ehci_readl(ehci, hostpc_reg);
 			ehci_writel(ehci, t3 | HOSTPC_PHCD, hostpc_reg);
 			t3 = ehci_readl(ehci, hostpc_reg);
@@ -298,6 +298,7 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 					"succeeded" : "failed");
 		}
 	}
+	spin_unlock_irq(&ehci->lock);
 
 	/* Apparently some devices need a >= 1-uframe delay here */
 	if (ehci->bus_suspended)
@@ -305,10 +306,18 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 
 	/* turn off now-idle HC */
 	ehci_halt (ehci);
+
+	spin_lock_irq(&ehci->lock);
+	if (ehci->enabled_hrtimer_events & BIT(EHCI_HRTIMER_POLL_DEAD))
+		ehci_handle_controller_death(ehci);
+	if (ehci->rh_state != EHCI_RH_RUNNING)
+		goto done;
 	ehci->rh_state = EHCI_RH_SUSPENDED;
 
-	if (ehci->reclaim)
-		end_unlink_async(ehci);
+	end_unlink_async(ehci);
+	unlink_empty_async(ehci);
+	ehci_handle_intr_unlinks(ehci);
+	end_free_itds(ehci);
 
 	/* allow remote wakeup */
 	mask = INTR_MASK;
@@ -317,13 +326,13 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 	ehci_writel(ehci, mask, &ehci->regs->intr_enable);
 	ehci_readl(ehci, &ehci->regs->intr_enable);
 
+ done:
 	ehci->next_statechange = jiffies + msecs_to_jiffies(10);
+	ehci->enabled_hrtimer_events = 0;
+	ehci->next_hrtimer_event = EHCI_HRTIMER_NO_EVENT;
 	spin_unlock_irq (&ehci->lock);
 
-	/* ehci_work() may have re-enabled the watchdog timer, which we do not
-	 * want, and so we must delete any pending watchdog timer events.
-	 */
-	del_timer_sync(&ehci->watchdog);
+	hrtimer_cancel(&ehci->hrtimer);
 	return 0;
 }
 
@@ -340,10 +349,8 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 	if (time_before (jiffies, ehci->next_statechange))
 		msleep(5);
 	spin_lock_irq (&ehci->lock);
-	if (!HCD_HW_ACCESSIBLE(hcd)) {
-		spin_unlock_irq(&ehci->lock);
-		return -ESHUTDOWN;
-	}
+	if (!HCD_HW_ACCESSIBLE(hcd) || ehci->shutdown)
+		goto shutdown;
 
 	if (unlikely(ehci->debug)) {
 		if (!dbgp_reset_prep())
@@ -382,16 +389,17 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 	spin_unlock_irq(&ehci->lock);
 	msleep(8);
 	spin_lock_irq(&ehci->lock);
+	if (ehci->shutdown)
+		goto shutdown;
 
 	/* clear phy low-power mode before resume */
 	if (ehci->bus_suspended && ehci->has_hostpc) {
 		i = HCS_N_PORTS(ehci->hcs_params);
 		while (i--) {
 			if (test_bit(i, &ehci->bus_suspended)) {
-				u32 __iomem	*hostpc_reg;
+				u32 __iomem	*hostpc_reg =
+							&ehci->regs->hostpc[i];
 
-				hostpc_reg = (u32 __iomem *)((u8 *) ehci->regs
-						+ HOSTPC0 + 4 * i);
 				temp = ehci_readl(ehci, hostpc_reg);
 				ehci_writel(ehci, temp & ~HOSTPC_PHCD,
 						hostpc_reg);
@@ -400,6 +408,8 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 		spin_unlock_irq(&ehci->lock);
 		msleep(5);
 		spin_lock_irq(&ehci->lock);
+		if (ehci->shutdown)
+			goto shutdown;
 	}
 
 	/* manually resume the ports we suspended during bus_suspend() */
@@ -420,6 +430,8 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 		spin_unlock_irq(&ehci->lock);
 		msleep(20);
 		spin_lock_irq(&ehci->lock);
+		if (ehci->shutdown)
+			goto shutdown;
 	}
 
 	i = HCS_N_PORTS (ehci->hcs_params);
@@ -431,27 +443,25 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 			ehci_vdbg (ehci, "resumed port %d\n", i + 1);
 		}
 	}
-	(void) ehci_readl(ehci, &ehci->regs->command);
-
-	/* maybe re-activate the schedule(s) */
-	temp = 0;
-	if (ehci->async->qh_next.qh)
-		temp |= CMD_ASE;
-	if (ehci->periodic_sched)
-		temp |= CMD_PSE;
-	if (temp) {
-		ehci->command |= temp;
-		ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	}
 
 	ehci->next_statechange = jiffies + msecs_to_jiffies(5);
+	spin_unlock_irq(&ehci->lock);
+
+	ehci_handover_companion_ports(ehci);
 
 	/* Now we can safely re-enable irqs */
+	spin_lock_irq(&ehci->lock);
+	if (ehci->shutdown)
+		goto shutdown;
 	ehci_writel(ehci, INTR_MASK, &ehci->regs->intr_enable);
+	(void) ehci_readl(ehci, &ehci->regs->intr_enable);
+	spin_unlock_irq(&ehci->lock);
 
-	spin_unlock_irq (&ehci->lock);
-	ehci_handover_companion_ports(ehci);
 	return 0;
+
+ shutdown:
+	spin_unlock_irq(&ehci->lock);
+	return -ESHUTDOWN;
 }
 
 #else
@@ -667,7 +677,7 @@ static int ehci_hub_control (
 	int		ports = HCS_N_PORTS (ehci->hcs_params);
 	u32 __iomem	*status_reg = &ehci->regs->port_status[
 				(wIndex & 0xff) - 1];
-	u32 __iomem	*hostpc_reg = NULL;
+	u32 __iomem	*hostpc_reg = &ehci->regs->hostpc[(wIndex & 0xff) - 1];
 	u32		temp, temp1, status;
 	unsigned long	flags;
 	int		retval = 0;
@@ -680,9 +690,6 @@ static int ehci_hub_control (
 	 * power, "this is the one", etc.  EHCI spec supports this.
 	 */
 
-	if (ehci->has_hostpc)
-		hostpc_reg = (u32 __iomem *)((u8 *)ehci->regs
-				+ HOSTPC0 + 4 * ((wIndex & 0xff) - 1));
 	spin_lock_irqsave (&ehci->lock, flags);
 	switch (typeReq) {
 	case ClearHubFeature:
@@ -724,7 +731,7 @@ static int ehci_hub_control (
 #ifdef CONFIG_USB_OTG
 			if ((hcd->self.otg_port == (wIndex + 1))
 			    && hcd->self.b_hnp_enable) {
-				otg_start_hnp(ehci->transceiver->otg);
+				otg_start_hnp(hcd->phy->otg);
 				break;
 			}
 #endif
@@ -734,7 +741,7 @@ static int ehci_hub_control (
 				goto error;
 
 			/* clear phy low-power mode before resume */
-			if (hostpc_reg) {
+			if (ehci->has_hostpc) {
 				temp1 = ehci_readl(ehci, hostpc_reg);
 				ehci_writel(ehci, temp1 & ~HOSTPC_PHCD,
 						hostpc_reg);
@@ -984,7 +991,7 @@ static int ehci_hub_control (
 			temp &= ~PORT_WKCONN_E;
 			temp |= PORT_WKDISC_E | PORT_WKOC_E;
 			ehci_writel(ehci, temp | PORT_SUSPEND, status_reg);
-			if (hostpc_reg) {
+			if (ehci->has_hostpc) {
 				spin_unlock_irqrestore(&ehci->lock, flags);
 				msleep(5);/* 5ms for HCD enter low pwr mode */
 				spin_lock_irqsave(&ehci->lock, flags);
@@ -1041,7 +1048,9 @@ static int ehci_hub_control (
 		case USB_PORT_FEAT_TEST:
 			if (!selector || selector > 5)
 				goto error;
+			spin_unlock_irqrestore(&ehci->lock, flags);
 			ehci_quiesce(ehci);
+			spin_lock_irqsave(&ehci->lock, flags);
 
 			/* Put all enabled ports into suspend */
 			while (ports--) {
@@ -1053,7 +1062,11 @@ static int ehci_hub_control (
 					ehci_writel(ehci, temp | PORT_SUSPEND,
 							sreg);
 			}
+
+			spin_unlock_irqrestore(&ehci->lock, flags);
 			ehci_halt(ehci);
+			spin_lock_irqsave(&ehci->lock, flags);
+
 			temp = ehci_readl(ehci, status_reg);
 			temp |= selector << 16;
 			ehci_writel(ehci, temp, status_reg);
diff --git a/drivers/usb/host/ehci-ixp4xx.c b/drivers/usb/host/ehci-ixp4xx.c
index c4460f3d009..488d401942e 100644
--- a/drivers/usb/host/ehci-ixp4xx.c
+++ b/drivers/usb/host/ehci-ixp4xx.c
@@ -22,14 +22,10 @@ static int ixp4xx_ehci_init(struct usb_hcd *hcd)
 	ehci->big_endian_mmio = 1;
 
 	ehci->caps = hcd->regs + 0x100;
-	ehci->regs = hcd->regs + 0x100
-		+ HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
 	hcd->has_tt = 1;
-	ehci_reset(ehci);
 
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
diff --git a/drivers/usb/host/ehci-mem.c b/drivers/usb/host/ehci-mem.c
index 12f70c302b0..ef2c3a1eca4 100644
--- a/drivers/usb/host/ehci-mem.c
+++ b/drivers/usb/host/ehci-mem.c
@@ -64,10 +64,8 @@ static inline void ehci_qtd_free (struct ehci_hcd *ehci, struct ehci_qtd *qtd)
 }
 
 
-static void qh_destroy(struct ehci_qh *qh)
+static void qh_destroy(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
-	struct ehci_hcd *ehci = qh->ehci;
-
 	/* clean qtds first, and know this is not linked */
 	if (!list_empty (&qh->qtd_list) || qh->qh_next.ptr) {
 		ehci_dbg (ehci, "unused qh not empty!\n");
@@ -92,8 +90,6 @@ static struct ehci_qh *ehci_qh_alloc (struct ehci_hcd *ehci, gfp_t flags)
 	if (!qh->hw)
 		goto fail;
 	memset(qh->hw, 0, sizeof *qh->hw);
-	qh->refcount = 1;
-	qh->ehci = ehci;
 	qh->qh_dma = dma;
 	// INIT_LIST_HEAD (&qh->qh_list);
 	INIT_LIST_HEAD (&qh->qtd_list);
@@ -113,20 +109,6 @@ fail:
 	return NULL;
 }
 
-/* to share a qh (cpu threads, or hc) */
-static inline struct ehci_qh *qh_get (struct ehci_qh *qh)
-{
-	WARN_ON(!qh->refcount);
-	qh->refcount++;
-	return qh;
-}
-
-static inline void qh_put (struct ehci_qh *qh)
-{
-	if (!--qh->refcount)
-		qh_destroy(qh);
-}
-
 /*-------------------------------------------------------------------------*/
 
 /* The queue heads and transfer descriptors are managed from pools tied
@@ -136,13 +118,12 @@ static inline void qh_put (struct ehci_qh *qh)
 
 static void ehci_mem_cleanup (struct ehci_hcd *ehci)
 {
-	free_cached_lists(ehci);
 	if (ehci->async)
-		qh_put (ehci->async);
+		qh_destroy(ehci, ehci->async);
 	ehci->async = NULL;
 
 	if (ehci->dummy)
-		qh_put(ehci->dummy);
+		qh_destroy(ehci, ehci->dummy);
 	ehci->dummy = NULL;
 
 	/* DMA consistent memory and pools */
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index 9803a55fd5f..17dd9e94001 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -145,8 +145,8 @@ static int ehci_msm_probe(struct platform_device *pdev)
 	 * powering up VBUS, mapping of registers address space and power
 	 * management.
 	 */
-	phy = usb_get_transceiver();
-	if (!phy) {
+	phy = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(phy)) {
 		dev_err(&pdev->dev, "unable to find transceiver\n");
 		ret = -ENODEV;
 		goto unmap;
@@ -169,7 +169,7 @@ static int ehci_msm_probe(struct platform_device *pdev)
 	return 0;
 
 put_transceiver:
-	usb_put_transceiver(phy);
+	usb_put_phy(phy);
 unmap:
 	iounmap(hcd->regs);
 put_hcd:
@@ -187,7 +187,7 @@ static int __devexit ehci_msm_remove(struct platform_device *pdev)
 	pm_runtime_set_suspended(&pdev->dev);
 
 	otg_set_host(phy->otg, NULL);
-	usb_put_transceiver(phy);
+	usb_put_phy(phy);
 
 	usb_put_hcd(hcd);
 
@@ -198,24 +198,11 @@ static int __devexit ehci_msm_remove(struct platform_device *pdev)
 static int ehci_msm_pm_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	bool wakeup = device_may_wakeup(dev);
+	bool do_wakeup = device_may_wakeup(dev);
 
 	dev_dbg(dev, "ehci-msm PM suspend\n");
 
-	/*
-	 * EHCI helper function has also the same check before manipulating
-	 * port wakeup flags.  We do check here the same condition before
-	 * calling the same helper function to avoid bringing hardware
-	 * from Low power mode when there is no need for adjusting port
-	 * wakeup flags.
-	 */
-	if (hcd->self.root_hub->do_remote_wakeup && !wakeup) {
-		pm_runtime_resume(dev);
-		ehci_prepare_ports_for_controller_suspend(hcd_to_ehci(hcd),
-				wakeup);
-	}
-
-	return 0;
+	return ehci_suspend(hcd, do_wakeup);
 }
 
 static int ehci_msm_pm_resume(struct device *dev)
@@ -223,7 +210,7 @@ static int ehci_msm_pm_resume(struct device *dev)
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 
 	dev_dbg(dev, "ehci-msm PM resume\n");
-	ehci_prepare_ports_for_controller_resume(hcd_to_ehci(hcd));
+	ehci_resume(hcd, false);
 
 	return 0;
 }
diff --git a/drivers/usb/host/ehci-mv.c b/drivers/usb/host/ehci-mv.c
index a936bbcff8f..f6df1ccc961 100644
--- a/drivers/usb/host/ehci-mv.c
+++ b/drivers/usb/host/ehci-mv.c
@@ -13,6 +13,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/usb/otg.h>
 #include <linux/platform_data/mv_usb.h>
 
@@ -76,7 +77,6 @@ static void mv_ehci_disable(struct ehci_hcd_mv *ehci_mv)
 
 static int mv_ehci_reset(struct usb_hcd *hcd)
 {
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	struct device *dev = hcd->self.controller;
 	struct ehci_hcd_mv *ehci_mv = dev_get_drvdata(dev);
 	int retval;
@@ -86,25 +86,13 @@ static int mv_ehci_reset(struct usb_hcd *hcd)
 		return -ENODEV;
 	}
 
-	/*
-	 * data structure init
-	 */
-	retval = ehci_init(hcd);
-	if (retval) {
-		dev_err(dev, "ehci_init failed %d\n", retval);
-		return retval;
-	}
-
 	hcd->has_tt = 1;
-	ehci->sbrn = 0x20;
 
-	retval = ehci_reset(ehci);
-	if (retval) {
-		dev_err(dev, "ehci_reset failed %d\n", retval);
-		return retval;
-	}
+	retval = ehci_setup(hcd);
+	if (retval)
+		dev_err(dev, "ehci_setup failed %d\n", retval);
 
-	return 0;
+	return retval;
 }
 
 static const struct hc_driver mv_ehci_hc_driver = {
@@ -247,14 +235,12 @@ static int mv_ehci_probe(struct platform_device *pdev)
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = (struct ehci_caps *) ehci_mv->cap_regs;
-	ehci->regs = (struct ehci_regs *) ehci_mv->op_regs;
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
 	ehci_mv->mode = pdata->mode;
 	if (ehci_mv->mode == MV_USB_MODE_OTG) {
 #ifdef CONFIG_USB_OTG_UTILS
-		ehci_mv->otg = usb_get_transceiver();
-		if (!ehci_mv->otg) {
+		ehci_mv->otg = usb_get_phy(USB_PHY_TYPE_USB2);
+		if (IS_ERR_OR_NULL(ehci_mv->otg)) {
 			dev_err(&pdev->dev,
 				"unable to find transceiver\n");
 			retval = -ENODEV;
@@ -302,8 +288,8 @@ err_set_vbus:
 		pdata->set_vbus(0);
 #ifdef CONFIG_USB_OTG_UTILS
 err_put_transceiver:
-	if (ehci_mv->otg)
-		usb_put_transceiver(ehci_mv->otg);
+	if (!IS_ERR_OR_NULL(ehci_mv->otg))
+		usb_put_phy(ehci_mv->otg);
 #endif
 err_disable_clk:
 	mv_ehci_disable(ehci_mv);
@@ -331,9 +317,9 @@ static int mv_ehci_remove(struct platform_device *pdev)
 	if (hcd->rh_registered)
 		usb_remove_hcd(hcd);
 
-	if (ehci_mv->otg) {
+	if (!IS_ERR_OR_NULL(ehci_mv->otg)) {
 		otg_set_host(ehci_mv->otg->otg, NULL);
-		usb_put_transceiver(ehci_mv->otg);
+		usb_put_phy(ehci_mv->otg);
 	}
 
 	if (ehci_mv->mode == MV_USB_MODE_HOST) {
diff --git a/drivers/usb/host/ehci-mxc.c b/drivers/usb/host/ehci-mxc.c
index c778ffe4e4e..34201372c85 100644
--- a/drivers/usb/host/ehci-mxc.c
+++ b/drivers/usb/host/ehci-mxc.c
@@ -42,27 +42,12 @@ static int ehci_mxc_setup(struct usb_hcd *hcd)
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	int retval;
 
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
 	hcd->has_tt = 1;
 
-	retval = ehci_halt(ehci);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	/* data structure init */
-	retval = ehci_init(hcd);
-	if (retval)
-		return retval;
-
-	ehci->sbrn = 0x20;
-
-	ehci_reset(ehci);
-
 	ehci_port_power(ehci, 0);
 	return 0;
 }
diff --git a/drivers/usb/host/ehci-octeon.c b/drivers/usb/host/ehci-octeon.c
index c0104882c72..ba26957abf4 100644
--- a/drivers/usb/host/ehci-octeon.c
+++ b/drivers/usb/host/ehci-octeon.c
@@ -56,7 +56,7 @@ static const struct hc_driver ehci_octeon_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
-	.reset			= ehci_init,
+	.reset			= ehci_setup,
 	.start			= ehci_run,
 	.stop			= ehci_stop,
 	.shutdown		= ehci_shutdown,
@@ -150,12 +150,6 @@ static int ehci_octeon_drv_probe(struct platform_device *pdev)
 #endif
 
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
-	ehci_reset(ehci);
 
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret) {
diff --git a/drivers/usb/host/ehci-omap.c b/drivers/usb/host/ehci-omap.c
index c30435499a0..bb55eb4a7d4 100644
--- a/drivers/usb/host/ehci-omap.c
+++ b/drivers/usb/host/ehci-omap.c
@@ -115,9 +115,8 @@ static void omap_ehci_erratum_i693(struct ehci_hcd *ehci)
 	clk_disable(usbhost_p2_fck);
 }
 
-static void omap_ehci_soft_phy_reset(struct platform_device *pdev, u8 port)
+static void omap_ehci_soft_phy_reset(struct usb_hcd *hcd, u8 port)
 {
-	struct usb_hcd	*hcd = dev_get_drvdata(&pdev->dev);
 	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
 	unsigned reg = 0;
 
@@ -139,12 +138,63 @@ static void omap_ehci_soft_phy_reset(struct platform_device *pdev, u8 port)
 		cpu_relax();
 
 		if (time_after(jiffies, timeout)) {
-			dev_dbg(&pdev->dev, "phy reset operation timed out\n");
+			dev_dbg(hcd->self.controller,
+					"phy reset operation timed out\n");
 			break;
 		}
 	}
 }
 
+static int omap_ehci_init(struct usb_hcd *hcd)
+{
+	struct ehci_hcd		*ehci = hcd_to_ehci(hcd);
+	int			rc;
+	struct ehci_hcd_omap_platform_data	*pdata;
+
+	pdata = hcd->self.controller->platform_data;
+
+	/* Hold PHYs in reset while initializing EHCI controller */
+	if (pdata->phy_reset) {
+		if (gpio_is_valid(pdata->reset_gpio_port[0]))
+			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 0);
+
+		if (gpio_is_valid(pdata->reset_gpio_port[1]))
+			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 0);
+
+		/* Hold the PHY in RESET for enough time till DIR is high */
+		udelay(10);
+	}
+
+	/* Soft reset the PHY using PHY reset command over ULPI */
+	if (pdata->port_mode[0] == OMAP_EHCI_PORT_MODE_PHY)
+		omap_ehci_soft_phy_reset(hcd, 0);
+	if (pdata->port_mode[1] == OMAP_EHCI_PORT_MODE_PHY)
+		omap_ehci_soft_phy_reset(hcd, 1);
+
+	/* we know this is the memory we want, no need to ioremap again */
+	ehci->caps = hcd->regs;
+
+	rc = ehci_setup(hcd);
+
+	if (pdata->phy_reset) {
+		/* Hold the PHY in RESET for enough time till
+		 * PHY is settled and ready
+		 */
+		udelay(10);
+
+		if (gpio_is_valid(pdata->reset_gpio_port[0]))
+			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 1);
+
+		if (gpio_is_valid(pdata->reset_gpio_port[1]))
+			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 1);
+	}
+
+	/* root ports should always stay powered */
+	ehci_port_power(ehci, 1);
+
+	return rc;
+}
+
 static int omap_ehci_hub_control(
 	struct usb_hcd	*hcd,
 	u16		typeReq,
@@ -219,7 +269,6 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 	struct resource				*res;
 	struct usb_hcd				*hcd;
 	void __iomem				*regs;
-	struct ehci_hcd				*omap_ehci;
 	int					ret = -ENODEV;
 	int					irq;
 	int					i;
@@ -281,18 +330,6 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 		}
 	}
 
-	/* Hold PHYs in reset while initializing EHCI controller */
-	if (pdata->phy_reset) {
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 0);
-
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 0);
-
-		/* Hold the PHY in RESET for enough time till DIR is high */
-		udelay(10);
-	}
-
 	pm_runtime_enable(dev);
 	pm_runtime_get_sync(dev);
 
@@ -308,49 +345,12 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 	ehci_write(regs, EHCI_INSNREG04,
 				EHCI_INSNREG04_DISABLE_UNSUSPEND);
 
-	/* Soft reset the PHY using PHY reset command over ULPI */
-	if (pdata->port_mode[0] == OMAP_EHCI_PORT_MODE_PHY)
-		omap_ehci_soft_phy_reset(pdev, 0);
-	if (pdata->port_mode[1] == OMAP_EHCI_PORT_MODE_PHY)
-		omap_ehci_soft_phy_reset(pdev, 1);
-
-	omap_ehci = hcd_to_ehci(hcd);
-	omap_ehci->sbrn = 0x20;
-
-	/* we know this is the memory we want, no need to ioremap again */
-	omap_ehci->caps = hcd->regs;
-	omap_ehci->regs = hcd->regs
-		+ HC_LENGTH(ehci, readl(&omap_ehci->caps->hc_capbase));
-
-	dbg_hcs_params(omap_ehci, "reset");
-	dbg_hcc_params(omap_ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	omap_ehci->hcs_params = readl(&omap_ehci->caps->hcs_params);
-
-	ehci_reset(omap_ehci);
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret) {
 		dev_err(dev, "failed to add hcd with err %d\n", ret);
-		goto err_add_hcd;
+		goto err_pm_runtime;
 	}
 
-	if (pdata->phy_reset) {
-		/* Hold the PHY in RESET for enough time till
-		 * PHY is settled and ready
-		 */
-		udelay(10);
-
-		if (gpio_is_valid(pdata->reset_gpio_port[0]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[0], 1);
-
-		if (gpio_is_valid(pdata->reset_gpio_port[1]))
-			gpio_set_value_cansleep(pdata->reset_gpio_port[1], 1);
-	}
-
-	/* root ports should always stay powered */
-	ehci_port_power(omap_ehci, 1);
-
 	/* get clocks */
 	utmi_p1_fck = clk_get(dev, "utmi_p1_gfclk");
 	if (IS_ERR(utmi_p1_fck)) {
@@ -422,8 +422,12 @@ err_utmi_p1_fck:
 	clk_put(utmi_p1_fck);
 
 err_add_hcd:
+	usb_remove_hcd(hcd);
+
+err_pm_runtime:
 	disable_put_regulator(pdata);
 	pm_runtime_put_sync(dev);
+	usb_put_hcd(hcd);
 
 err_io:
 	iounmap(regs);
@@ -506,7 +510,7 @@ static const struct hc_driver ehci_omap_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
-	.reset			= ehci_init,
+	.reset			= omap_ehci_init,
 	.start			= ehci_run,
 	.stop			= ehci_stop,
 	.shutdown		= ehci_shutdown,
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 82de1073aa5..8892d3642ce 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -106,21 +106,10 @@ static int ehci_orion_setup(struct usb_hcd *hcd)
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	int retval;
 
-	hcd->has_tt = 1;
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
-
-	/*
-	 * data structure init
-	 */
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	ehci_reset(ehci);
-
 	ehci_port_power(ehci, 0);
 
 	return retval;
@@ -261,11 +250,7 @@ static int __devinit ehci_orion_drv_probe(struct platform_device *pdev)
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs + 0x100;
-	ehci->regs = hcd->regs + 0x100 +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 	hcd->has_tt = 1;
-	ehci->sbrn = 0x20;
 
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
@@ -298,6 +283,10 @@ static int __devinit ehci_orion_drv_probe(struct platform_device *pdev)
 err4:
 	usb_put_hcd(hcd);
 err3:
+	if (!IS_ERR(clk)) {
+		clk_disable_unprepare(clk);
+		clk_put(clk);
+	}
 	iounmap(regs);
 err2:
 	release_mem_region(res->start, resource_size(res));
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index 123481793a4..2cb7d370c4e 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -54,6 +54,17 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 	u32			temp;
 	int			retval;
 
+	ehci->caps = hcd->regs;
+
+	/*
+	 * ehci_init() causes memory for DMA transfers to be
+	 * allocated.  Thus, any vendor-specific workarounds based on
+	 * limiting the type of memory used for DMA transfers must
+	 * happen before ehci_setup() is called.
+	 *
+	 * Most other workarounds can be done either before or after
+	 * init and reset; they are located here too.
+	 */
 	switch (pdev->vendor) {
 	case PCI_VENDOR_ID_TOSHIBA_2:
 		/* celleb's companion chip */
@@ -66,20 +77,6 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 #endif
 		}
 		break;
-	}
-
-	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-        /* ehci_init() causes memory for DMA transfers to be
-         * allocated.  Thus, any vendor-specific workarounds based on
-         * limiting the type of memory used for DMA transfers must
-         * happen before ehci_init() is called. */
-	switch (pdev->vendor) {
 	case PCI_VENDOR_ID_NVIDIA:
 		/* NVidia reports that certain chips don't handle
 		 * QH, ITD, or SITD addresses above 2GB.  (But TD,
@@ -95,61 +92,24 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 				ehci_warn(ehci, "can't enable NVidia "
 					"workaround for >2GB RAM\n");
 			break;
-		}
-		break;
-	}
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
 
-	if ((pdev->vendor == PCI_VENDOR_ID_AMD && pdev->device == 0x7808) ||
-	    (pdev->vendor == PCI_VENDOR_ID_ATI && pdev->device == 0x4396)) {
-		/* EHCI controller on AMD SB700/SB800/Hudson-2/3 platforms may
-		 * read/write memory space which does not belong to it when
-		 * there is NULL pointer with T-bit set to 1 in the frame list
-		 * table. To avoid the issue, the frame list link pointer
-		 * should always contain a valid pointer to a inactive qh.
+		/* Some NForce2 chips have problems with selective suspend;
+		 * fixed in newer silicon.
 		 */
-		ehci->use_dummy_qh = 1;
-		ehci_info(ehci, "applying AMD SB700/SB800/Hudson-2/3 EHCI "
-				"dummy qh workaround\n");
-	}
-
-	/* data structure init */
-	retval = ehci_init(hcd);
-	if (retval)
-		return retval;
-
-	switch (pdev->vendor) {
-	case PCI_VENDOR_ID_NEC:
-		ehci->need_io_watchdog = 0;
+		case 0x0068:
+			if (pdev->revision < 0xa4)
+				ehci->no_selective_suspend = 1;
+			break;
+		}
 		break;
 	case PCI_VENDOR_ID_INTEL:
-		ehci->need_io_watchdog = 0;
 		ehci->fs_i_thresh = 1;
-		if (pdev->device == 0x27cc) {
-			ehci->broken_periodic = 1;
-			ehci_info(ehci, "using broken periodic workaround\n");
-		}
-		if (pdev->device == 0x0806 || pdev->device == 0x0811
-				|| pdev->device == 0x0829) {
-			ehci_info(ehci, "disable lpm for langwell/penwell\n");
-			ehci->has_lpm = 0;
-		}
-		if (pdev->device == PCI_DEVICE_ID_INTEL_CE4100_USB) {
+		if (pdev->device == PCI_DEVICE_ID_INTEL_CE4100_USB)
 			hcd->has_tt = 1;
-			tdi_reset(ehci);
-		}
 		break;
 	case PCI_VENDOR_ID_TDI:
-		if (pdev->device == PCI_DEVICE_ID_TDI_EHCI) {
+		if (pdev->device == PCI_DEVICE_ID_TDI_EHCI)
 			hcd->has_tt = 1;
-			tdi_reset(ehci);
-		}
 		break;
 	case PCI_VENDOR_ID_AMD:
 		/* AMD PLL quirk */
@@ -161,28 +121,17 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 			retval = -EIO;
 			goto done;
 		}
-		break;
-	case PCI_VENDOR_ID_NVIDIA:
-		switch (pdev->device) {
-		/* Some NForce2 chips have problems with selective suspend;
-		 * fixed in newer silicon.
-		 */
-		case 0x0068:
-			if (pdev->revision < 0xa4)
-				ehci->no_selective_suspend = 1;
-			break;
 
-		/* MCP89 chips on the MacBookAir3,1 give EPROTO when
-		 * fetching device descriptors unless LPM is disabled.
-		 * There are also intermittent problems enumerating
-		 * devices with PPCD enabled.
+		/*
+		 * EHCI controller on AMD SB700/SB800/Hudson-2/3 platforms may
+		 * read/write memory space which does not belong to it when
+		 * there is NULL pointer with T-bit set to 1 in the frame list
+		 * table. To avoid the issue, the frame list link pointer
+		 * should always contain a valid pointer to a inactive qh.
 		 */
-		case 0x0d9d:
-			ehci_info(ehci, "disable lpm/ppcd for nvidia mcp89");
-			ehci->has_lpm = 0;
-			ehci->has_ppcd = 0;
-			ehci->command &= ~CMD_PPCEE;
-			break;
+		if (pdev->device == 0x7808) {
+			ehci->use_dummy_qh = 1;
+			ehci_info(ehci, "applying AMD SB700/SB800/Hudson-2/3 EHCI dummy qh workaround\n");
 		}
 		break;
 	case PCI_VENDOR_ID_VIA:
@@ -203,6 +152,18 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 		/* AMD PLL quirk */
 		if (usb_amd_find_chipset_info())
 			ehci->amd_pll_fix = 1;
+
+		/*
+		 * EHCI controller on AMD SB700/SB800/Hudson-2/3 platforms may
+		 * read/write memory space which does not belong to it when
+		 * there is NULL pointer with T-bit set to 1 in the frame list
+		 * table. To avoid the issue, the frame list link pointer
+		 * should always contain a valid pointer to a inactive qh.
+		 */
+		if (pdev->device == 0x4396) {
+			ehci->use_dummy_qh = 1;
+			ehci_info(ehci, "applying AMD SB700/SB800/Hudson-2/3 EHCI dummy qh workaround\n");
+		}
 		/* SB600 and old version of SB700 have a bug in EHCI controller,
 		 * which causes usb devices lose response in some cases.
 		 */
@@ -231,6 +192,40 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 		break;
 	}
 
+	retval = ehci_setup(hcd);
+	if (retval)
+		return retval;
+
+	/* These workarounds need to be applied after ehci_setup() */
+	switch (pdev->vendor) {
+	case PCI_VENDOR_ID_NEC:
+		ehci->need_io_watchdog = 0;
+		break;
+	case PCI_VENDOR_ID_INTEL:
+		ehci->need_io_watchdog = 0;
+		if (pdev->device == 0x0806 || pdev->device == 0x0811
+				|| pdev->device == 0x0829) {
+			ehci_info(ehci, "disable lpm for langwell/penwell\n");
+			ehci->has_lpm = 0;
+		}
+		break;
+	case PCI_VENDOR_ID_NVIDIA:
+		switch (pdev->device) {
+		/* MCP89 chips on the MacBookAir3,1 give EPROTO when
+		 * fetching device descriptors unless LPM is disabled.
+		 * There are also intermittent problems enumerating
+		 * devices with PPCD enabled.
+		 */
+		case 0x0d9d:
+			ehci_info(ehci, "disable lpm/ppcd for nvidia mcp89");
+			ehci->has_lpm = 0;
+			ehci->has_ppcd = 0;
+			ehci->command &= ~CMD_PPCEE;
+			break;
+		}
+		break;
+	}
+
 	/* optional debug port, normally in the first BAR */
 	temp = pci_find_capability(pdev, 0x0a);
 	if (temp) {
@@ -238,7 +233,7 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 		temp >>= 16;
 		if ((temp & (3 << 13)) == (1 << 13)) {
 			temp &= 0x1fff;
-			ehci->debug = ehci_to_hcd(ehci)->regs + temp;
+			ehci->debug = hcd->regs + temp;
 			temp = ehci_readl(ehci, &ehci->debug->control);
 			ehci_info(ehci, "debug port %d%s\n",
 				HCS_DEBUG_PORT(ehci->hcs_params),
@@ -250,8 +245,6 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 		}
 	}
 
-	ehci_reset(ehci);
-
 	/* at least the Genesys GL880S needs fixup here */
 	temp = HCS_N_CC(ehci->hcs_params) * HCS_N_PCC(ehci->hcs_params);
 	temp &= 0x0f;
@@ -275,10 +268,11 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 	}
 
 	/* Serial Bus Release Number is at PCI 0x60 offset */
-	pci_read_config_byte(pdev, 0x60, &ehci->sbrn);
 	if (pdev->vendor == PCI_VENDOR_ID_STMICRO
 	    && pdev->device == PCI_DEVICE_ID_STMICRO_USB_HOST)
-		ehci->sbrn = 0x20; /* ConneXT has no sbrn register */
+		;	/* ConneXT has no sbrn register */
+	else
+		pci_read_config_byte(pdev, 0x60, &ehci->sbrn);
 
 	/* Keep this around for a while just in case some EHCI
 	 * implementation uses legacy PCI PM support.  This test
@@ -331,29 +325,7 @@ done:
 
 static int ehci_pci_suspend(struct usb_hcd *hcd, bool do_wakeup)
 {
-	struct ehci_hcd		*ehci = hcd_to_ehci(hcd);
-	unsigned long		flags;
-	int			rc = 0;
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(10);
-
-	/* Root hub was already suspended. Disable irq emission and
-	 * mark HW unaccessible.  The PM and USB cores make sure that
-	 * the root hub is either suspended or stopped.
-	 */
-	ehci_prepare_ports_for_controller_suspend(ehci, do_wakeup);
-	spin_lock_irqsave (&ehci->lock, flags);
-	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
-	(void)ehci_readl(ehci, &ehci->regs->intr_enable);
-
-	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	spin_unlock_irqrestore (&ehci->lock, flags);
-
-	// could save FLADJ in case of Vaux power loss
-	// ... we'd only use it to handle clock skew
-
-	return rc;
+	return ehci_suspend(hcd, do_wakeup);
 }
 
 static bool usb_is_intel_switchable_ehci(struct pci_dev *pdev)
@@ -402,54 +374,8 @@ static int ehci_pci_resume(struct usb_hcd *hcd, bool hibernated)
 	if (usb_is_intel_switchable_ehci(pdev))
 		ehci_enable_xhci_companion();
 
-	// maybe restore FLADJ
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(100);
-
-	/* Mark hardware accessible again as we are out of D3 state by now */
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
-	/* If CF is still set and we aren't resuming from hibernation
-	 * then we maintained PCI Vaux power.
-	 * Just undo the effect of ehci_pci_suspend().
-	 */
-	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF &&
-				!hibernated) {
-		int	mask = INTR_MASK;
-
-		ehci_prepare_ports_for_controller_resume(ehci);
-		if (!hcd->self.root_hub->do_remote_wakeup)
-			mask &= ~STS_PCD;
-		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
-		ehci_readl(ehci, &ehci->regs->intr_enable);
-		return 0;
-	}
-
-	usb_root_hub_lost_power(hcd->self.root_hub);
-
-	/* Else reset, to cope with power loss or flush-to-storage
-	 * style "resume" having let BIOS kick in during reboot.
-	 */
-	(void) ehci_halt(ehci);
-	(void) ehci_reset(ehci);
-	(void) ehci_pci_reinit(ehci, pdev);
-
-	/* emptying the schedule aborts any urbs */
-	spin_lock_irq(&ehci->lock);
-	if (ehci->reclaim)
-		end_unlink_async(ehci);
-	ehci_work(ehci);
-	spin_unlock_irq(&ehci->lock);
-
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
-	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
-
-	/* here we "know" root ports should always stay powered */
-	ehci_port_power(ehci, 1);
-
-	ehci->rh_state = EHCI_RH_SUSPENDED;
+	if (ehci_resume(hcd, hibernated) != 0)
+		(void) ehci_pci_reinit(ehci, pdev);
 	return 0;
 }
 #endif
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index dfe881a34ae..4b1d896d5a2 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -153,17 +153,16 @@ static int __devexit ehci_platform_remove(struct platform_device *dev)
 static int ehci_platform_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	bool wakeup = device_may_wakeup(dev);
+	bool do_wakeup = device_may_wakeup(dev);
 
-	ehci_prepare_ports_for_controller_suspend(hcd_to_ehci(hcd), wakeup);
-	return 0;
+	return ehci_suspend(hcd, do_wakeup);
 }
 
 static int ehci_platform_resume(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 
-	ehci_prepare_ports_for_controller_resume(hcd_to_ehci(hcd));
+	ehci_resume(hcd, false);
 	return 0;
 }
 
diff --git a/drivers/usb/host/ehci-pmcmsp.c b/drivers/usb/host/ehci-pmcmsp.c
index e8d54de44ac..087aee2a904 100644
--- a/drivers/usb/host/ehci-pmcmsp.c
+++ b/drivers/usb/host/ehci-pmcmsp.c
@@ -78,27 +78,14 @@ static int ehci_msp_setup(struct usb_hcd *hcd)
 {
 	struct ehci_hcd		*ehci = hcd_to_ehci(hcd);
 	int			retval;
+
 	ehci->big_endian_mmio = 1;
 	ehci->big_endian_desc = 1;
 
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 	hcd->has_tt = 1;
 
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
-
-	ehci_reset(ehci);
-
-	/* data structure init */
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
diff --git a/drivers/usb/host/ehci-ppc-of.c b/drivers/usb/host/ehci-ppc-of.c
index 41d11fe1425..bbbe89dfd88 100644
--- a/drivers/usb/host/ehci-ppc-of.c
+++ b/drivers/usb/host/ehci-ppc-of.c
@@ -17,24 +17,6 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 
-/* called during probe() after chip reset completes */
-static int ehci_ppc_of_setup(struct usb_hcd *hcd)
-{
-	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
-	int		retval;
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
-
-	retval = ehci_init(hcd);
-	if (retval)
-		return retval;
-
-	ehci->sbrn = 0x20;
-	return ehci_reset(ehci);
-}
-
 
 static const struct hc_driver ehci_ppc_of_hc_driver = {
 	.description		= hcd_name,
@@ -50,7 +32,7 @@ static const struct hc_driver ehci_ppc_of_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
-	.reset			= ehci_ppc_of_setup,
+	.reset			= ehci_setup,
 	.start			= ehci_run,
 	.stop			= ehci_stop,
 	.shutdown		= ehci_shutdown,
@@ -178,11 +160,6 @@ static int __devinit ehci_hcd_ppc_of_probe(struct platform_device *op)
 		ehci->big_endian_desc = 1;
 
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
 	if (of_device_is_compatible(dn, "ibm,usb-ehci-440epx")) {
 		rv = ppc44x_enable_bmt(dn);
diff --git a/drivers/usb/host/ehci-ps3.c b/drivers/usb/host/ehci-ps3.c
index a20e496eb47..45a356e9f13 100644
--- a/drivers/usb/host/ehci-ps3.c
+++ b/drivers/usb/host/ehci-ps3.c
@@ -55,28 +55,12 @@ static int ps3_ehci_hc_reset(struct usb_hcd *hcd)
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 
 	ehci->big_endian_mmio = 1;
-
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs + HC_LENGTH(ehci, ehci_readl(ehci,
-		&ehci->caps->hc_capbase));
-
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
-	result = ehci_halt(ehci);
 
+	result = ehci_setup(hcd);
 	if (result)
 		return result;
 
-	result = ehci_init(hcd);
-
-	if (result)
-		return result;
-
-	ehci_reset(ehci);
-
 	ps3_ehci_setup_insnreg(ehci);
 
 	return result;
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index 4378bf72bba..9bc39ca460c 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -100,7 +100,7 @@ qh_update (struct ehci_hcd *ehci, struct ehci_qh *qh, struct ehci_qtd *qtd)
 	 * and set the pseudo-toggle in udev. Only usb_clear_halt() will
 	 * ever clear it.
 	 */
-	if (!(hw->hw_info1 & cpu_to_hc32(ehci, 1 << 14))) {
+	if (!(hw->hw_info1 & cpu_to_hc32(ehci, QH_TOGGLE_CTL))) {
 		unsigned	is_out, epnum;
 
 		is_out = qh->is_out;
@@ -265,7 +265,6 @@ __acquires(ehci->lock)
 			/* ... update hc-wide periodic stats (for usbfs) */
 			ehci_to_hcd(ehci)->self.bandwidth_int_reqs--;
 		}
-		qh_put (qh);
 	}
 
 	if (unlikely(urb->unlinked)) {
@@ -294,9 +293,6 @@ __acquires(ehci->lock)
 	spin_lock (&ehci->lock);
 }
 
-static void start_unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh);
-static void unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh);
-
 static int qh_schedule (struct ehci_hcd *ehci, struct ehci_qh *qh);
 
 /*
@@ -326,7 +322,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	 *
 	 * It's a bug for qh->qh_state to be anything other than
 	 * QH_STATE_IDLE, unless our caller is scan_async() or
-	 * scan_periodic().
+	 * scan_intr().
 	 */
 	state = qh->qh_state;
 	qh->qh_state = QH_STATE_COMPLETING;
@@ -434,7 +430,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 
 		/* stop scanning when we reach qtds the hc is using */
 		} else if (likely (!stopped
-				&& ehci->rh_state == EHCI_RH_RUNNING)) {
+				&& ehci->rh_state >= EHCI_RH_RUNNING)) {
 			break;
 
 		/* scan the whole queue for unlinks whenever it stops */
@@ -442,7 +438,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 			stopped = 1;
 
 			/* cancel everything if we halt, suspend, etc */
-			if (ehci->rh_state != EHCI_RH_RUNNING)
+			if (ehci->rh_state < EHCI_RH_RUNNING)
 				last_status = -ESHUTDOWN;
 
 			/* this qtd is active; skip it unless a previous qtd
@@ -836,7 +832,6 @@ qh_make (
 				is_input, 0,
 				hb_mult(maxp) * max_packet(maxp)));
 		qh->start = NO_FRAME;
-		qh->stamp = ehci->periodic_stamp;
 
 		if (urb->dev->speed == USB_SPEED_HIGH) {
 			qh->c_usecs = 0;
@@ -887,7 +882,7 @@ qh_make (
 	/* using TT? */
 	switch (urb->dev->speed) {
 	case USB_SPEED_LOW:
-		info1 |= (1 << 12);	/* EPS "low" */
+		info1 |= QH_LOW_SPEED;
 		/* FALL THROUGH */
 
 	case USB_SPEED_FULL:
@@ -895,8 +890,8 @@ qh_make (
 		if (type != PIPE_INTERRUPT)
 			info1 |= (EHCI_TUNE_RL_TT << 28);
 		if (type == PIPE_CONTROL) {
-			info1 |= (1 << 27);	/* for TT */
-			info1 |= 1 << 14;	/* toggle from qtd */
+			info1 |= QH_CONTROL_EP;		/* for TT */
+			info1 |= QH_TOGGLE_CTL;		/* toggle from qtd */
 		}
 		info1 |= maxp << 16;
 
@@ -921,11 +916,11 @@ qh_make (
 		break;
 
 	case USB_SPEED_HIGH:		/* no TT involved */
-		info1 |= (2 << 12);	/* EPS "high" */
+		info1 |= QH_HIGH_SPEED;
 		if (type == PIPE_CONTROL) {
 			info1 |= (EHCI_TUNE_RL_HS << 28);
 			info1 |= 64 << 16;	/* usb2 fixed maxpacket */
-			info1 |= 1 << 14;	/* toggle from qtd */
+			info1 |= QH_TOGGLE_CTL;	/* toggle from qtd */
 			info2 |= (EHCI_TUNE_MULT_HS << 30);
 		} else if (type == PIPE_BULK) {
 			info1 |= (EHCI_TUNE_RL_HS << 28);
@@ -946,7 +941,7 @@ qh_make (
 		ehci_dbg(ehci, "bogus dev %p speed %d\n", urb->dev,
 			urb->dev->speed);
 done:
-		qh_put (qh);
+		qh_destroy(ehci, qh);
 		return NULL;
 	}
 
@@ -965,6 +960,31 @@ done:
 
 /*-------------------------------------------------------------------------*/
 
+static void enable_async(struct ehci_hcd *ehci)
+{
+	if (ehci->async_count++)
+		return;
+
+	/* Stop waiting to turn off the async schedule */
+	ehci->enabled_hrtimer_events &= ~BIT(EHCI_HRTIMER_DISABLE_ASYNC);
+
+	/* Don't start the schedule until ASS is 0 */
+	ehci_poll_ASS(ehci);
+	turn_on_io_watchdog(ehci);
+}
+
+static void disable_async(struct ehci_hcd *ehci)
+{
+	if (--ehci->async_count)
+		return;
+
+	/* The async schedule and async_unlink list are supposed to be empty */
+	WARN_ON(ehci->async->qh_next.qh || ehci->async_unlink);
+
+	/* Don't turn off the schedule until ASS is 1 */
+	ehci_poll_ASS(ehci);
+}
+
 /* move qh (and its qtds) onto async queue; maybe enable queue.  */
 
 static void qh_link_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
@@ -978,24 +998,11 @@ static void qh_link_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 
 	WARN_ON(qh->qh_state != QH_STATE_IDLE);
 
-	/* (re)start the async schedule? */
-	head = ehci->async;
-	timer_action_done (ehci, TIMER_ASYNC_OFF);
-	if (!head->qh_next.qh) {
-		if (!(ehci->command & CMD_ASE)) {
-			/* in case a clear of CMD_ASE didn't take yet */
-			(void)handshake(ehci, &ehci->regs->status,
-					STS_ASS, 0, 150);
-			ehci->command |= CMD_ASE;
-			ehci_writel(ehci, ehci->command, &ehci->regs->command);
-			/* posted write need not be known to HC yet ... */
-		}
-	}
-
 	/* clear halt and/or toggle; and maybe recover from silicon quirk */
 	qh_refresh(ehci, qh);
 
 	/* splice right after start */
+	head = ehci->async;
 	qh->qh_next = head->qh_next;
 	qh->hw->hw_next = head->hw->hw_next;
 	wmb ();
@@ -1003,10 +1010,11 @@ static void qh_link_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	head->qh_next.qh = qh;
 	head->hw->hw_next = dma;
 
-	qh_get(qh);
 	qh->xacterrs = 0;
 	qh->qh_state = QH_STATE_LINKED;
 	/* qtd completions reported later by interrupt */
+
+	enable_async(ehci);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -1090,7 +1098,7 @@ static struct ehci_qh *qh_append_tds (
 			wmb ();
 			dummy->hw_token = token;
 
-			urb->hcpriv = qh_get (qh);
+			urb->hcpriv = qh;
 		}
 	}
 	return qh;
@@ -1155,117 +1163,155 @@ submit_async (
 
 /*-------------------------------------------------------------------------*/
 
-/* the async qh for the qtds being reclaimed are now unlinked from the HC */
-
-static void end_unlink_async (struct ehci_hcd *ehci)
+static void single_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
-	struct ehci_qh		*qh = ehci->reclaim;
-	struct ehci_qh		*next;
+	struct ehci_qh		*prev;
 
-	iaa_watchdog_done(ehci);
+	/* Add to the end of the list of QHs waiting for the next IAAD */
+	qh->qh_state = QH_STATE_UNLINK;
+	if (ehci->async_unlink)
+		ehci->async_unlink_last->unlink_next = qh;
+	else
+		ehci->async_unlink = qh;
+	ehci->async_unlink_last = qh;
 
-	// qh->hw_next = cpu_to_hc32(qh->qh_dma);
-	qh->qh_state = QH_STATE_IDLE;
-	qh->qh_next.qh = NULL;
-	qh_put (qh);			// refcount from reclaim
+	/* Unlink it from the schedule */
+	prev = ehci->async;
+	while (prev->qh_next.qh != qh)
+		prev = prev->qh_next.qh;
 
-	/* other unlink(s) may be pending (in QH_STATE_UNLINK_WAIT) */
-	next = qh->reclaim;
-	ehci->reclaim = next;
-	qh->reclaim = NULL;
+	prev->hw->hw_next = qh->hw->hw_next;
+	prev->qh_next = qh->qh_next;
+	if (ehci->qh_scan_next == qh)
+		ehci->qh_scan_next = qh->qh_next.qh;
+}
 
-	qh_completions (ehci, qh);
+static void start_iaa_cycle(struct ehci_hcd *ehci, bool nested)
+{
+	/*
+	 * Do nothing if an IAA cycle is already running or
+	 * if one will be started shortly.
+	 */
+	if (ehci->async_iaa || ehci->async_unlinking)
+		return;
 
-	if (!list_empty(&qh->qtd_list) && ehci->rh_state == EHCI_RH_RUNNING) {
-		qh_link_async (ehci, qh);
-	} else {
-		/* it's not free to turn the async schedule on/off; leave it
-		 * active but idle for a while once it empties.
-		 */
-		if (ehci->rh_state == EHCI_RH_RUNNING
-				&& ehci->async->qh_next.qh == NULL)
-			timer_action (ehci, TIMER_ASYNC_OFF);
-	}
-	qh_put(qh);			/* refcount from async list */
+	/* Do all the waiting QHs at once */
+	ehci->async_iaa = ehci->async_unlink;
+	ehci->async_unlink = NULL;
 
-	if (next) {
-		ehci->reclaim = NULL;
-		start_unlink_async (ehci, next);
+	/* If the controller isn't running, we don't have to wait for it */
+	if (unlikely(ehci->rh_state < EHCI_RH_RUNNING)) {
+		if (!nested)		/* Avoid recursion */
+			end_unlink_async(ehci);
+
+	/* Otherwise start a new IAA cycle */
+	} else if (likely(ehci->rh_state == EHCI_RH_RUNNING)) {
+		/* Make sure the unlinks are all visible to the hardware */
+		wmb();
+
+		ehci_writel(ehci, ehci->command | CMD_IAAD,
+				&ehci->regs->command);
+		ehci_readl(ehci, &ehci->regs->command);
+		ehci_enable_event(ehci, EHCI_HRTIMER_IAA_WATCHDOG, true);
 	}
+}
+
+/* the async qh for the qtds being unlinked are now gone from the HC */
+
+static void end_unlink_async(struct ehci_hcd *ehci)
+{
+	struct ehci_qh		*qh;
 
 	if (ehci->has_synopsys_hc_bug)
 		ehci_writel(ehci, (u32) ehci->async->qh_dma,
 			    &ehci->regs->async_next);
-}
-
-/* makes sure the async qh will become idle */
-/* caller must own ehci->lock */
 
-static void start_unlink_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
-{
-	struct ehci_qh	*prev;
+	/* Process the idle QHs */
+ restart:
+	ehci->async_unlinking = true;
+	while (ehci->async_iaa) {
+		qh = ehci->async_iaa;
+		ehci->async_iaa = qh->unlink_next;
+		qh->unlink_next = NULL;
+
+		qh->qh_state = QH_STATE_IDLE;
+		qh->qh_next.qh = NULL;
+
+		qh_completions(ehci, qh);
+		if (!list_empty(&qh->qtd_list) &&
+				ehci->rh_state == EHCI_RH_RUNNING)
+			qh_link_async(ehci, qh);
+		disable_async(ehci);
+	}
+	ehci->async_unlinking = false;
 
-#ifdef DEBUG
-	assert_spin_locked(&ehci->lock);
-	if (ehci->reclaim
-			|| (qh->qh_state != QH_STATE_LINKED
-				&& qh->qh_state != QH_STATE_UNLINK_WAIT)
-			)
-		BUG ();
-#endif
+	/* Start a new IAA cycle if any QHs are waiting for it */
+	if (ehci->async_unlink) {
+		start_iaa_cycle(ehci, true);
+		if (unlikely(ehci->rh_state < EHCI_RH_RUNNING))
+			goto restart;
+	}
+}
 
-	/* stop async schedule right now? */
-	if (unlikely (qh == ehci->async)) {
-		/* can't get here without STS_ASS set */
-		if (ehci->rh_state != EHCI_RH_HALTED
-				&& !ehci->reclaim) {
-			/* ... and CMD_IAAD clear */
-			ehci->command &= ~CMD_ASE;
-			ehci_writel(ehci, ehci->command, &ehci->regs->command);
-			wmb ();
-			// handshake later, if we need to
-			timer_action_done (ehci, TIMER_ASYNC_OFF);
+static void unlink_empty_async(struct ehci_hcd *ehci)
+{
+	struct ehci_qh		*qh, *next;
+	bool			stopped = (ehci->rh_state < EHCI_RH_RUNNING);
+	bool			check_unlinks_later = false;
+
+	/* Unlink all the async QHs that have been empty for a timer cycle */
+	next = ehci->async->qh_next.qh;
+	while (next) {
+		qh = next;
+		next = qh->qh_next.qh;
+
+		if (list_empty(&qh->qtd_list) &&
+				qh->qh_state == QH_STATE_LINKED) {
+			if (!stopped && qh->unlink_cycle ==
+					ehci->async_unlink_cycle)
+				check_unlinks_later = true;
+			else
+				single_unlink_async(ehci, qh);
 		}
-		return;
 	}
 
-	qh->qh_state = QH_STATE_UNLINK;
-	ehci->reclaim = qh = qh_get (qh);
+	/* Start a new IAA cycle if any QHs are waiting for it */
+	if (ehci->async_unlink)
+		start_iaa_cycle(ehci, false);
 
-	prev = ehci->async;
-	while (prev->qh_next.qh != qh)
-		prev = prev->qh_next.qh;
+	/* QHs that haven't been empty for long enough will be handled later */
+	if (check_unlinks_later) {
+		ehci_enable_event(ehci, EHCI_HRTIMER_ASYNC_UNLINKS, true);
+		++ehci->async_unlink_cycle;
+	}
+}
 
-	prev->hw->hw_next = qh->hw->hw_next;
-	prev->qh_next = qh->qh_next;
-	if (ehci->qh_scan_next == qh)
-		ehci->qh_scan_next = qh->qh_next.qh;
-	wmb ();
+/* makes sure the async qh will become idle */
+/* caller must own ehci->lock */
 
-	/* If the controller isn't running, we don't have to wait for it */
-	if (unlikely(ehci->rh_state != EHCI_RH_RUNNING)) {
-		/* if (unlikely (qh->reclaim != 0))
-		 *	this will recurse, probably not much
-		 */
-		end_unlink_async (ehci);
+static void start_unlink_async(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	/*
+	 * If the QH isn't linked then there's nothing we can do
+	 * unless we were called during a giveback, in which case
+	 * qh_completions() has to deal with it.
+	 */
+	if (qh->qh_state != QH_STATE_LINKED) {
+		if (qh->qh_state == QH_STATE_COMPLETING)
+			qh->needs_rescan = 1;
 		return;
 	}
 
-	ehci_writel(ehci, ehci->command | CMD_IAAD, &ehci->regs->command);
-	(void)ehci_readl(ehci, &ehci->regs->command);
-	iaa_watchdog_start(ehci);
+	single_unlink_async(ehci, qh);
+	start_iaa_cycle(ehci, false);
 }
 
 /*-------------------------------------------------------------------------*/
 
 static void scan_async (struct ehci_hcd *ehci)
 {
-	bool			stopped;
 	struct ehci_qh		*qh;
-	enum ehci_timer_action	action = TIMER_IO_WATCHDOG;
-
-	timer_action_done (ehci, TIMER_ASYNC_SHRINK);
-	stopped = (ehci->rh_state != EHCI_RH_RUNNING);
+	bool			check_unlinks_later = false;
 
 	ehci->qh_scan_next = ehci->async->qh_next.qh;
 	while (ehci->qh_scan_next) {
@@ -1281,33 +1327,30 @@ static void scan_async (struct ehci_hcd *ehci)
 			 * drops the lock.  That's why ehci->qh_scan_next
 			 * always holds the next qh to scan; if the next qh
 			 * gets unlinked then ehci->qh_scan_next is adjusted
-			 * in start_unlink_async().
+			 * in single_unlink_async().
 			 */
-			qh = qh_get(qh);
 			temp = qh_completions(ehci, qh);
-			if (qh->needs_rescan)
-				unlink_async(ehci, qh);
-			qh->unlink_time = jiffies + EHCI_SHRINK_JIFFIES;
-			qh_put(qh);
-			if (temp != 0)
+			if (qh->needs_rescan) {
+				start_unlink_async(ehci, qh);
+			} else if (list_empty(&qh->qtd_list)
+					&& qh->qh_state == QH_STATE_LINKED) {
+				qh->unlink_cycle = ehci->async_unlink_cycle;
+				check_unlinks_later = true;
+			} else if (temp != 0)
 				goto rescan;
 		}
+	}
 
-		/* unlink idle entries, reducing DMA usage as well
-		 * as HCD schedule-scanning costs.  delay for any qh
-		 * we just scanned, there's a not-unusual case that it
-		 * doesn't stay idle for long.
-		 * (plus, avoids some kind of re-activation race.)
-		 */
-		if (list_empty(&qh->qtd_list)
-				&& qh->qh_state == QH_STATE_LINKED) {
-			if (!ehci->reclaim && (stopped ||
-					time_after_eq(jiffies, qh->unlink_time)))
-				start_unlink_async(ehci, qh);
-			else
-				action = TIMER_ASYNC_SHRINK;
-		}
+	/*
+	 * Unlink empty entries, reducing DMA usage as well
+	 * as HCD schedule-scanning costs.  Delay for any qh
+	 * we just scanned, there's a not-unusual case that it
+	 * doesn't stay idle for long.
+	 */
+	if (check_unlinks_later && ehci->rh_state == EHCI_RH_RUNNING &&
+			!(ehci->enabled_hrtimer_events &
+				BIT(EHCI_HRTIMER_ASYNC_UNLINKS))) {
+		ehci_enable_event(ehci, EHCI_HRTIMER_ASYNC_UNLINKS, true);
+		++ehci->async_unlink_cycle;
 	}
-	if (action == TIMER_ASYNC_SHRINK)
-		timer_action (ehci, TIMER_ASYNC_SHRINK);
 }
diff --git a/drivers/usb/host/ehci-s5p.c b/drivers/usb/host/ehci-s5p.c
index c474cec064e..9d8f1dd57cb 100644
--- a/drivers/usb/host/ehci-s5p.c
+++ b/drivers/usb/host/ehci-s5p.c
@@ -13,7 +13,9 @@
  */
 
 #include <linux/clk.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/of_gpio.h>
 #include <plat/ehci.h>
 #include <plat/usb-phy.h>
 
@@ -40,7 +42,7 @@ static const struct hc_driver s5p_ehci_hc_driver = {
 	.irq			= ehci_irq,
 	.flags			= HCD_MEMORY | HCD_USB2,
 
-	.reset			= ehci_init,
+	.reset			= ehci_setup,
 	.start			= ehci_run,
 	.stop			= ehci_stop,
 	.shutdown		= ehci_shutdown,
@@ -63,6 +65,26 @@ static const struct hc_driver s5p_ehci_hc_driver = {
 	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
 };
 
+static void s5p_setup_vbus_gpio(struct platform_device *pdev)
+{
+	int err;
+	int gpio;
+
+	if (!pdev->dev.of_node)
+		return;
+
+	gpio = of_get_named_gpio(pdev->dev.of_node,
+			"samsung,vbus-gpio", 0);
+	if (!gpio_is_valid(gpio))
+		return;
+
+	err = gpio_request_one(gpio, GPIOF_OUT_INIT_HIGH, "ehci_vbus_gpio");
+	if (err)
+		dev_err(&pdev->dev, "can't request ehci vbus gpio %d", gpio);
+}
+
+static u64 ehci_s5p_dma_mask = DMA_BIT_MASK(32);
+
 static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 {
 	struct s5p_ehci_platdata *pdata;
@@ -79,7 +101,20 @@ static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	s5p_ehci = kzalloc(sizeof(struct s5p_ehci_hcd), GFP_KERNEL);
+	/*
+	 * Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we move to full device tree support this will vanish off.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &ehci_s5p_dma_mask;
+	if (!pdev->dev.coherent_dma_mask)
+		pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+
+	s5p_setup_vbus_gpio(pdev);
+
+	s5p_ehci = devm_kzalloc(&pdev->dev, sizeof(struct s5p_ehci_hcd),
+				GFP_KERNEL);
 	if (!s5p_ehci)
 		return -ENOMEM;
 
@@ -89,8 +124,7 @@ static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 					dev_name(&pdev->dev));
 	if (!hcd) {
 		dev_err(&pdev->dev, "Unable to create HCD\n");
-		err = -ENOMEM;
-		goto fail_hcd;
+		return -ENOMEM;
 	}
 
 	s5p_ehci->hcd = hcd;
@@ -115,7 +149,7 @@ static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = ioremap(res->start, resource_size(res));
+	hcd->regs = devm_ioremap(&pdev->dev, res->start, hcd->rsrc_len);
 	if (!hcd->regs) {
 		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
 		err = -ENOMEM;
@@ -126,7 +160,7 @@ static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 	if (!irq) {
 		dev_err(&pdev->dev, "Failed to get IRQ\n");
 		err = -ENODEV;
-		goto fail;
+		goto fail_io;
 	}
 
 	if (pdata->phy_init)
@@ -134,40 +168,26 @@ static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
 
 	/* DMA burst Enable */
 	writel(EHCI_INSNREG00_ENABLE_DMA_BURST, EHCI_INSNREG00(hcd->regs));
 
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = readl(&ehci->caps->hcs_params);
-
-	ehci_reset(ehci);
-
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to add USB HCD\n");
-		goto fail;
+		goto fail_io;
 	}
 
 	platform_set_drvdata(pdev, s5p_ehci);
 
 	return 0;
 
-fail:
-	iounmap(hcd->regs);
 fail_io:
 	clk_disable(s5p_ehci->clk);
 fail_clken:
 	clk_put(s5p_ehci->clk);
 fail_clk:
 	usb_put_hcd(hcd);
-fail_hcd:
-	kfree(s5p_ehci);
 	return err;
 }
 
@@ -182,13 +202,10 @@ static int __devexit s5p_ehci_remove(struct platform_device *pdev)
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
 
-	iounmap(hcd->regs);
-
 	clk_disable(s5p_ehci->clk);
 	clk_put(s5p_ehci->clk);
 
 	usb_put_hcd(hcd);
-	kfree(s5p_ehci);
 
 	return 0;
 }
@@ -207,27 +224,12 @@ static int s5p_ehci_suspend(struct device *dev)
 {
 	struct s5p_ehci_hcd *s5p_ehci = dev_get_drvdata(dev);
 	struct usb_hcd *hcd = s5p_ehci->hcd;
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	bool do_wakeup = device_may_wakeup(dev);
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s5p_ehci_platdata *pdata = pdev->dev.platform_data;
-	unsigned long flags;
-	int rc = 0;
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(20);
-
-	/*
-	 * Root hub was already suspended. Disable irq emission and
-	 * mark HW unaccessible.  The PM and USB cores make sure that
-	 * the root hub is either suspended or stopped.
-	 */
-	ehci_prepare_ports_for_controller_suspend(ehci, device_may_wakeup(dev));
-	spin_lock_irqsave(&ehci->lock, flags);
-	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
-	(void)ehci_readl(ehci, &ehci->regs->intr_enable);
+	int rc;
 
-	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	spin_unlock_irqrestore(&ehci->lock, flags);
+	rc = ehci_suspend(hcd, do_wakeup);
 
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
@@ -241,7 +243,6 @@ static int s5p_ehci_resume(struct device *dev)
 {
 	struct s5p_ehci_hcd *s5p_ehci = dev_get_drvdata(dev);
 	struct usb_hcd *hcd = s5p_ehci->hcd;
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s5p_ehci_platdata *pdata = pdev->dev.platform_data;
 
@@ -253,44 +254,7 @@ static int s5p_ehci_resume(struct device *dev)
 	/* DMA burst Enable */
 	writel(EHCI_INSNREG00_ENABLE_DMA_BURST, EHCI_INSNREG00(hcd->regs));
 
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(100);
-
-	/* Mark hardware accessible again as we are out of D3 state by now */
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
-	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF) {
-		int	mask = INTR_MASK;
-
-		ehci_prepare_ports_for_controller_resume(ehci);
-		if (!hcd->self.root_hub->do_remote_wakeup)
-			mask &= ~STS_PCD;
-		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
-		ehci_readl(ehci, &ehci->regs->intr_enable);
-		return 0;
-	}
-
-	usb_root_hub_lost_power(hcd->self.root_hub);
-
-	(void) ehci_halt(ehci);
-	(void) ehci_reset(ehci);
-
-	/* emptying the schedule aborts any urbs */
-	spin_lock_irq(&ehci->lock);
-	if (ehci->reclaim)
-		end_unlink_async(ehci);
-	ehci_work(ehci);
-	spin_unlock_irq(&ehci->lock);
-
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
-	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
-
-	/* here we "know" root ports should always stay powered */
-	ehci_port_power(ehci, 1);
-
-	ehci->rh_state = EHCI_RH_SUSPENDED;
-
+	ehci_resume(hcd, false);
 	return 0;
 }
 #else
@@ -303,6 +267,14 @@ static const struct dev_pm_ops s5p_ehci_pm_ops = {
 	.resume		= s5p_ehci_resume,
 };
 
+#ifdef CONFIG_OF
+static const struct of_device_id exynos_ehci_match[] = {
+	{ .compatible = "samsung,exynos-ehci" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_ehci_match);
+#endif
+
 static struct platform_driver s5p_ehci_driver = {
 	.probe		= s5p_ehci_probe,
 	.remove		= __devexit_p(s5p_ehci_remove),
@@ -311,6 +283,7 @@ static struct platform_driver s5p_ehci_driver = {
 		.name	= "s5p-ehci",
 		.owner	= THIS_MODULE,
 		.pm	= &s5p_ehci_pm_ops,
+		.of_match_table = of_match_ptr(exynos_ehci_match),
 	}
 };
 
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index 33182c6d1ff..7cf3da7babf 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -479,70 +479,26 @@ static int tt_no_collision (
 
 /*-------------------------------------------------------------------------*/
 
-static int enable_periodic (struct ehci_hcd *ehci)
+static void enable_periodic(struct ehci_hcd *ehci)
 {
-	int	status;
-
-	if (ehci->periodic_sched++)
-		return 0;
-
-	/* did clearing PSE did take effect yet?
-	 * takes effect only at frame boundaries...
-	 */
-	status = handshake_on_error_set_halt(ehci, &ehci->regs->status,
-					     STS_PSS, 0, 9 * 125);
-	if (status) {
-		usb_hc_died(ehci_to_hcd(ehci));
-		return status;
-	}
+	if (ehci->periodic_count++)
+		return;
 
-	ehci->command |= CMD_PSE;
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	/* posted write ... PSS happens later */
+	/* Stop waiting to turn off the periodic schedule */
+	ehci->enabled_hrtimer_events &= ~BIT(EHCI_HRTIMER_DISABLE_PERIODIC);
 
-	/* make sure ehci_work scans these */
-	ehci->next_uframe = ehci_read_frame_index(ehci)
-		% (ehci->periodic_size << 3);
-	if (unlikely(ehci->broken_periodic))
-		ehci->last_periodic_enable = ktime_get_real();
-	return 0;
+	/* Don't start the schedule until PSS is 0 */
+	ehci_poll_PSS(ehci);
+	turn_on_io_watchdog(ehci);
 }
 
-static int disable_periodic (struct ehci_hcd *ehci)
+static void disable_periodic(struct ehci_hcd *ehci)
 {
-	int	status;
-
-	if (--ehci->periodic_sched)
-		return 0;
-
-	if (unlikely(ehci->broken_periodic)) {
-		/* delay experimentally determined */
-		ktime_t safe = ktime_add_us(ehci->last_periodic_enable, 1000);
-		ktime_t now = ktime_get_real();
-		s64 delay = ktime_us_delta(safe, now);
-
-		if (unlikely(delay > 0))
-			udelay(delay);
-	}
-
-	/* did setting PSE not take effect yet?
-	 * takes effect only at frame boundaries...
-	 */
-	status = handshake_on_error_set_halt(ehci, &ehci->regs->status,
-					     STS_PSS, STS_PSS, 9 * 125);
-	if (status) {
-		usb_hc_died(ehci_to_hcd(ehci));
-		return status;
-	}
-
-	ehci->command &= ~CMD_PSE;
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	/* posted write ... */
-
-	free_cached_lists(ehci);
+	if (--ehci->periodic_count)
+		return;
 
-	ehci->next_uframe = -1;
-	return 0;
+	/* Don't turn off the schedule until PSS is 1 */
+	ehci_poll_PSS(ehci);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -553,7 +509,7 @@ static int disable_periodic (struct ehci_hcd *ehci)
  * this just links in a qh; caller guarantees uframe masks are set right.
  * no FSTN support (yet; ehci 0.96+)
  */
-static int qh_link_periodic (struct ehci_hcd *ehci, struct ehci_qh *qh)
+static void qh_link_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
 	unsigned	i;
 	unsigned	period = qh->period;
@@ -606,28 +562,38 @@ static int qh_link_periodic (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	}
 	qh->qh_state = QH_STATE_LINKED;
 	qh->xacterrs = 0;
-	qh_get (qh);
 
 	/* update per-qh bandwidth for usbfs */
 	ehci_to_hcd(ehci)->self.bandwidth_allocated += qh->period
 		? ((qh->usecs + qh->c_usecs) / qh->period)
 		: (qh->usecs * 8);
 
+	list_add(&qh->intr_node, &ehci->intr_qh_list);
+
 	/* maybe enable periodic schedule processing */
-	return enable_periodic(ehci);
+	++ehci->intr_count;
+	enable_periodic(ehci);
 }
 
-static int qh_unlink_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
+static void qh_unlink_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
 	unsigned	i;
 	unsigned	period;
 
-	// FIXME:
-	// IF this isn't high speed
-	//   and this qh is active in the current uframe
-	//   (and overlay token SplitXstate is false?)
-	// THEN
-	//   qh->hw_info1 |= cpu_to_hc32(1 << 7 /* "ignore" */);
+	/*
+	 * If qh is for a low/full-speed device, simply unlinking it
+	 * could interfere with an ongoing split transaction.  To unlink
+	 * it safely would require setting the QH_INACTIVATE bit and
+	 * waiting at least one frame, as described in EHCI 4.12.2.5.
+	 *
+	 * We won't bother with any of this.  Instead, we assume that the
+	 * only reason for unlinking an interrupt QH while the current URB
+	 * is still active is to dequeue all the URBs (flush the whole
+	 * endpoint queue).
+	 *
+	 * If rebalancing the periodic schedule is ever implemented, this
+	 * approach will no longer be valid.
+	 */
 
 	/* high bandwidth, or otherwise part of every microframe */
 	if ((period = qh->period) == 0)
@@ -650,18 +616,15 @@ static int qh_unlink_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
 	/* qh->qh_next still "live" to HC */
 	qh->qh_state = QH_STATE_UNLINK;
 	qh->qh_next.ptr = NULL;
-	qh_put (qh);
 
-	/* maybe turn off periodic schedule */
-	return disable_periodic(ehci);
+	if (ehci->qh_scan_next == qh)
+		ehci->qh_scan_next = list_entry(qh->intr_node.next,
+				struct ehci_qh, intr_node);
+	list_del(&qh->intr_node);
 }
 
-static void intr_deschedule (struct ehci_hcd *ehci, struct ehci_qh *qh)
+static void start_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
 {
-	unsigned		wait;
-	struct ehci_qh_hw	*hw = qh->hw;
-	int			rc;
-
 	/* If the QH isn't linked then there's nothing we can do
 	 * unless we were called during a giveback, in which case
 	 * qh_completions() has to deal with it.
@@ -674,28 +637,45 @@ static void intr_deschedule (struct ehci_hcd *ehci, struct ehci_qh *qh)
 
 	qh_unlink_periodic (ehci, qh);
 
-	/* simple/paranoid:  always delay, expecting the HC needs to read
-	 * qh->hw_next or finish a writeback after SPLIT/CSPLIT ... and
-	 * expect khubd to clean up after any CSPLITs we won't issue.
-	 * active high speed queues may need bigger delays...
+	/* Make sure the unlinks are visible before starting the timer */
+	wmb();
+
+	/*
+	 * The EHCI spec doesn't say how long it takes the controller to
+	 * stop accessing an unlinked interrupt QH.  The timer delay is
+	 * 9 uframes; presumably that will be long enough.
 	 */
-	if (list_empty (&qh->qtd_list)
-			|| (cpu_to_hc32(ehci, QH_CMASK)
-					& hw->hw_info2) != 0)
-		wait = 2;
+	qh->unlink_cycle = ehci->intr_unlink_cycle;
+
+	/* New entries go at the end of the intr_unlink list */
+	if (ehci->intr_unlink)
+		ehci->intr_unlink_last->unlink_next = qh;
 	else
-		wait = 55;	/* worst case: 3 * 1024 */
+		ehci->intr_unlink = qh;
+	ehci->intr_unlink_last = qh;
+
+	if (ehci->intr_unlinking)
+		;	/* Avoid recursive calls */
+	else if (ehci->rh_state < EHCI_RH_RUNNING)
+		ehci_handle_intr_unlinks(ehci);
+	else if (ehci->intr_unlink == qh) {
+		ehci_enable_event(ehci, EHCI_HRTIMER_UNLINK_INTR, true);
+		++ehci->intr_unlink_cycle;
+	}
+}
+
+static void end_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	struct ehci_qh_hw	*hw = qh->hw;
+	int			rc;
 
-	udelay (wait);
 	qh->qh_state = QH_STATE_IDLE;
 	hw->hw_next = EHCI_LIST_END(ehci);
-	wmb ();
 
 	qh_completions(ehci, qh);
 
 	/* reschedule QH iff another request is queued */
-	if (!list_empty(&qh->qtd_list) &&
-			ehci->rh_state == EHCI_RH_RUNNING) {
+	if (!list_empty(&qh->qtd_list) && ehci->rh_state == EHCI_RH_RUNNING) {
 		rc = qh_schedule(ehci, qh);
 
 		/* An error here likely indicates handshake failure
@@ -708,6 +688,10 @@ static void intr_deschedule (struct ehci_hcd *ehci, struct ehci_qh *qh)
 			ehci_err(ehci, "can't reschedule qh %p, err %d\n",
 					qh, rc);
 	}
+
+	/* maybe turn off periodic schedule */
+	--ehci->intr_count;
+	disable_periodic(ehci);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -884,7 +868,7 @@ static int qh_schedule(struct ehci_hcd *ehci, struct ehci_qh *qh)
 		ehci_dbg (ehci, "reused qh %p schedule\n", qh);
 
 	/* stuff into the periodic schedule */
-	status = qh_link_periodic (ehci, qh);
+	qh_link_periodic(ehci, qh);
 done:
 	return status;
 }
@@ -944,6 +928,35 @@ done_not_linked:
 	return status;
 }
 
+static void scan_intr(struct ehci_hcd *ehci)
+{
+	struct ehci_qh		*qh;
+
+	list_for_each_entry_safe(qh, ehci->qh_scan_next, &ehci->intr_qh_list,
+			intr_node) {
+ rescan:
+		/* clean any finished work for this qh */
+		if (!list_empty(&qh->qtd_list)) {
+			int temp;
+
+			/*
+			 * Unlinks could happen here; completion reporting
+			 * drops the lock.  That's why ehci->qh_scan_next
+			 * always holds the next qh to scan; if the next qh
+			 * gets unlinked then ehci->qh_scan_next is adjusted
+			 * in qh_unlink_periodic().
+			 */
+			temp = qh_completions(ehci, qh);
+			if (unlikely(qh->needs_rescan ||
+					(list_empty(&qh->qtd_list) &&
+						qh->qh_state == QH_STATE_LINKED)))
+				start_unlink_intr(ehci, qh);
+			else if (temp != 0)
+				goto rescan;
+		}
+	}
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* ehci_iso_stream ops work with both ITD and SITD */
@@ -958,7 +971,6 @@ iso_stream_alloc (gfp_t mem_flags)
 		INIT_LIST_HEAD(&stream->td_list);
 		INIT_LIST_HEAD(&stream->free_list);
 		stream->next_uframe = -1;
-		stream->refcount = 1;
 	}
 	return stream;
 }
@@ -1058,57 +1070,6 @@ iso_stream_init (
 	stream->maxp = maxp;
 }
 
-static void
-iso_stream_put(struct ehci_hcd *ehci, struct ehci_iso_stream *stream)
-{
-	stream->refcount--;
-
-	/* free whenever just a dev->ep reference remains.
-	 * not like a QH -- no persistent state (toggle, halt)
-	 */
-	if (stream->refcount == 1) {
-		// BUG_ON (!list_empty(&stream->td_list));
-
-		while (!list_empty (&stream->free_list)) {
-			struct list_head	*entry;
-
-			entry = stream->free_list.next;
-			list_del (entry);
-
-			/* knows about ITD vs SITD */
-			if (stream->highspeed) {
-				struct ehci_itd		*itd;
-
-				itd = list_entry (entry, struct ehci_itd,
-						itd_list);
-				dma_pool_free (ehci->itd_pool, itd,
-						itd->itd_dma);
-			} else {
-				struct ehci_sitd	*sitd;
-
-				sitd = list_entry (entry, struct ehci_sitd,
-						sitd_list);
-				dma_pool_free (ehci->sitd_pool, sitd,
-						sitd->sitd_dma);
-			}
-		}
-
-		stream->bEndpointAddress &= 0x0f;
-		if (stream->ep)
-			stream->ep->hcpriv = NULL;
-
-		kfree(stream);
-	}
-}
-
-static inline struct ehci_iso_stream *
-iso_stream_get (struct ehci_iso_stream *stream)
-{
-	if (likely (stream != NULL))
-		stream->refcount++;
-	return stream;
-}
-
 static struct ehci_iso_stream *
 iso_stream_find (struct ehci_hcd *ehci, struct urb *urb)
 {
@@ -1129,7 +1090,6 @@ iso_stream_find (struct ehci_hcd *ehci, struct urb *urb)
 	if (unlikely (stream == NULL)) {
 		stream = iso_stream_alloc(GFP_ATOMIC);
 		if (likely (stream != NULL)) {
-			/* dev->ep owns the initial refcount */
 			ep->hcpriv = stream;
 			stream->ep = ep;
 			iso_stream_init(ehci, stream, urb->dev, urb->pipe,
@@ -1144,9 +1104,6 @@ iso_stream_find (struct ehci_hcd *ehci, struct urb *urb)
 		stream = NULL;
 	}
 
-	/* caller guarantees an eventual matching iso_stream_put */
-	stream = iso_stream_get (stream);
-
 	spin_unlock_irqrestore (&ehci->lock, flags);
 	return stream;
 }
@@ -1254,17 +1211,19 @@ itd_urb_transaction (
 	spin_lock_irqsave (&ehci->lock, flags);
 	for (i = 0; i < num_itds; i++) {
 
-		/* free_list.next might be cache-hot ... but maybe
-		 * the HC caches it too. avoid that issue for now.
+		/*
+		 * Use iTDs from the free list, but not iTDs that may
+		 * still be in use by the hardware.
 		 */
-
-		/* prefer previously-allocated itds */
-		if (likely (!list_empty(&stream->free_list))) {
-			itd = list_entry (stream->free_list.prev,
+		if (likely(!list_empty(&stream->free_list))) {
+			itd = list_first_entry(&stream->free_list,
 					struct ehci_itd, itd_list);
+			if (itd->frame == ehci->now_frame)
+				goto alloc_itd;
 			list_del (&itd->itd_list);
 			itd_dma = itd->itd_dma;
 		} else {
+ alloc_itd:
 			spin_unlock_irqrestore (&ehci->lock, flags);
 			itd = dma_pool_alloc (ehci->itd_pool, mem_flags,
 					&itd_dma);
@@ -1528,6 +1487,10 @@ iso_stream_schedule (
 	urb->start_frame = stream->next_uframe;
 	if (!stream->highspeed)
 		urb->start_frame >>= 3;
+
+	/* Make sure scan_isoc() sees these */
+	if (ehci->isoc_count == 0)
+		ehci->next_frame = now >> 3;
 	return 0;
 
  fail:
@@ -1615,8 +1578,7 @@ itd_link (struct ehci_hcd *ehci, unsigned frame, struct ehci_itd *itd)
 }
 
 /* fit urb's itds into the selected schedule slot; activate as needed */
-static int
-itd_link_urb (
+static void itd_link_urb(
 	struct ehci_hcd		*ehci,
 	struct urb		*urb,
 	unsigned		mod,
@@ -1659,7 +1621,7 @@ itd_link_urb (
 			itd = list_entry (iso_sched->td_list.next,
 					struct ehci_itd, itd_list);
 			list_move_tail (&itd->itd_list, &stream->td_list);
-			itd->stream = iso_stream_get (stream);
+			itd->stream = stream;
 			itd->urb = urb;
 			itd_init (ehci, stream, itd);
 		}
@@ -1686,8 +1648,8 @@ itd_link_urb (
 	iso_sched_free (stream, iso_sched);
 	urb->hcpriv = NULL;
 
-	timer_action (ehci, TIMER_IO_WATCHDOG);
-	return enable_periodic(ehci);
+	++ehci->isoc_count;
+	enable_periodic(ehci);
 }
 
 #define	ISO_ERRS (EHCI_ISOC_BUF_ERR | EHCI_ISOC_BABBLE | EHCI_ISOC_XACTERR)
@@ -1702,11 +1664,8 @@ itd_link_urb (
  * (b) only this endpoint's completions submit URBs.  It seems some silicon
  * corrupts things if you reuse completed descriptors very quickly...
  */
-static unsigned
-itd_complete (
-	struct ehci_hcd	*ehci,
-	struct ehci_itd	*itd
-) {
+static bool itd_complete(struct ehci_hcd *ehci, struct ehci_itd *itd)
+{
 	struct urb				*urb = itd->urb;
 	struct usb_iso_packet_descriptor	*desc;
 	u32					t;
@@ -1714,7 +1673,7 @@ itd_complete (
 	int					urb_index = -1;
 	struct ehci_iso_stream			*stream = itd->stream;
 	struct usb_device			*dev;
-	unsigned				retval = false;
+	bool					retval = false;
 
 	/* for each uframe with a packet */
 	for (uframe = 0; uframe < 8; uframe++) {
@@ -1767,9 +1726,11 @@ itd_complete (
 	ehci_urb_done(ehci, urb, 0);
 	retval = true;
 	urb = NULL;
-	(void) disable_periodic(ehci);
-	ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
 
+	--ehci->isoc_count;
+	disable_periodic(ehci);
+
+	ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
 	if (ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs == 0) {
 		if (ehci->amd_pll_fix == 1)
 			usb_amd_quirk_pll_enable();
@@ -1783,28 +1744,20 @@ itd_complete (
 			dev->devpath, stream->bEndpointAddress & 0x0f,
 			(stream->bEndpointAddress & USB_DIR_IN) ? "in" : "out");
 	}
-	iso_stream_put (ehci, stream);
 
 done:
 	itd->urb = NULL;
-	if (ehci->clock_frame != itd->frame || itd->index[7] != -1) {
-		/* OK to recycle this ITD now. */
-		itd->stream = NULL;
-		list_move(&itd->itd_list, &stream->free_list);
-		iso_stream_put(ehci, stream);
-	} else {
-		/* HW might remember this ITD, so we can't recycle it yet.
-		 * Move it to a safe place until a new frame starts.
-		 */
-		list_move(&itd->itd_list, &ehci->cached_itd_list);
-		if (stream->refcount == 2) {
-			/* If iso_stream_put() were called here, stream
-			 * would be freed.  Instead, just prevent reuse.
-			 */
-			stream->ep->hcpriv = NULL;
-			stream->ep = NULL;
-		}
+
+	/* Add to the end of the free list for later reuse */
+	list_move_tail(&itd->itd_list, &stream->free_list);
+
+	/* Recycle the iTDs when the pipeline is empty (ep no longer in use) */
+	if (list_empty(&stream->td_list)) {
+		list_splice_tail_init(&stream->free_list,
+				&ehci->cached_itd_list);
+		start_free_itds(ehci);
 	}
+
 	return retval;
 }
 
@@ -1861,12 +1814,9 @@ static int itd_submit (struct ehci_hcd *ehci, struct urb *urb,
 		itd_link_urb (ehci, urb, ehci->periodic_size << 3, stream);
 	else
 		usb_hcd_unlink_urb_from_ep(ehci_to_hcd(ehci), urb);
-done_not_linked:
+ done_not_linked:
 	spin_unlock_irqrestore (&ehci->lock, flags);
-
-done:
-	if (unlikely (status < 0))
-		iso_stream_put (ehci, stream);
+ done:
 	return status;
 }
 
@@ -1955,17 +1905,19 @@ sitd_urb_transaction (
 		 * means we never need two sitds for full speed packets.
 		 */
 
-		/* free_list.next might be cache-hot ... but maybe
-		 * the HC caches it too. avoid that issue for now.
+		/*
+		 * Use siTDs from the free list, but not siTDs that may
+		 * still be in use by the hardware.
 		 */
-
-		/* prefer previously-allocated sitds */
-		if (!list_empty(&stream->free_list)) {
-			sitd = list_entry (stream->free_list.prev,
+		if (likely(!list_empty(&stream->free_list))) {
+			sitd = list_first_entry(&stream->free_list,
 					 struct ehci_sitd, sitd_list);
+			if (sitd->frame == ehci->now_frame)
+				goto alloc_sitd;
 			list_del (&sitd->sitd_list);
 			sitd_dma = sitd->sitd_dma;
 		} else {
+ alloc_sitd:
 			spin_unlock_irqrestore (&ehci->lock, flags);
 			sitd = dma_pool_alloc (ehci->sitd_pool, mem_flags,
 					&sitd_dma);
@@ -2034,8 +1986,7 @@ sitd_link (struct ehci_hcd *ehci, unsigned frame, struct ehci_sitd *sitd)
 }
 
 /* fit urb's sitds into the selected schedule slot; activate as needed */
-static int
-sitd_link_urb (
+static void sitd_link_urb(
 	struct ehci_hcd		*ehci,
 	struct urb		*urb,
 	unsigned		mod,
@@ -2081,7 +2032,7 @@ sitd_link_urb (
 		sitd = list_entry (sched->td_list.next,
 				struct ehci_sitd, sitd_list);
 		list_move_tail (&sitd->sitd_list, &stream->td_list);
-		sitd->stream = iso_stream_get (stream);
+		sitd->stream = stream;
 		sitd->urb = urb;
 
 		sitd_patch(ehci, stream, sitd, sched, packet);
@@ -2096,8 +2047,8 @@ sitd_link_urb (
 	iso_sched_free (stream, sched);
 	urb->hcpriv = NULL;
 
-	timer_action (ehci, TIMER_IO_WATCHDOG);
-	return enable_periodic(ehci);
+	++ehci->isoc_count;
+	enable_periodic(ehci);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -2115,18 +2066,15 @@ sitd_link_urb (
  * (b) only this endpoint's completions submit URBs.  It seems some silicon
  * corrupts things if you reuse completed descriptors very quickly...
  */
-static unsigned
-sitd_complete (
-	struct ehci_hcd		*ehci,
-	struct ehci_sitd	*sitd
-) {
+static bool sitd_complete(struct ehci_hcd *ehci, struct ehci_sitd *sitd)
+{
 	struct urb				*urb = sitd->urb;
 	struct usb_iso_packet_descriptor	*desc;
 	u32					t;
 	int					urb_index = -1;
 	struct ehci_iso_stream			*stream = sitd->stream;
 	struct usb_device			*dev;
-	unsigned				retval = false;
+	bool					retval = false;
 
 	urb_index = sitd->index;
 	desc = &urb->iso_frame_desc [urb_index];
@@ -2163,9 +2111,11 @@ sitd_complete (
 	ehci_urb_done(ehci, urb, 0);
 	retval = true;
 	urb = NULL;
-	(void) disable_periodic(ehci);
-	ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
 
+	--ehci->isoc_count;
+	disable_periodic(ehci);
+
+	ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
 	if (ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs == 0) {
 		if (ehci->amd_pll_fix == 1)
 			usb_amd_quirk_pll_enable();
@@ -2179,28 +2129,20 @@ sitd_complete (
 			dev->devpath, stream->bEndpointAddress & 0x0f,
 			(stream->bEndpointAddress & USB_DIR_IN) ? "in" : "out");
 	}
-	iso_stream_put (ehci, stream);
 
 done:
 	sitd->urb = NULL;
-	if (ehci->clock_frame != sitd->frame) {
-		/* OK to recycle this SITD now. */
-		sitd->stream = NULL;
-		list_move(&sitd->sitd_list, &stream->free_list);
-		iso_stream_put(ehci, stream);
-	} else {
-		/* HW might remember this SITD, so we can't recycle it yet.
-		 * Move it to a safe place until a new frame starts.
-		 */
-		list_move(&sitd->sitd_list, &ehci->cached_sitd_list);
-		if (stream->refcount == 2) {
-			/* If iso_stream_put() were called here, stream
-			 * would be freed.  Instead, just prevent reuse.
-			 */
-			stream->ep->hcpriv = NULL;
-			stream->ep = NULL;
-		}
+
+	/* Add to the end of the free list for later reuse */
+	list_move_tail(&sitd->sitd_list, &stream->free_list);
+
+	/* Recycle the siTDs when the pipeline is empty (ep no longer in use) */
+	if (list_empty(&stream->td_list)) {
+		list_splice_tail_init(&stream->free_list,
+				&ehci->cached_sitd_list);
+		start_free_itds(ehci);
 	}
+
 	return retval;
 }
 
@@ -2254,74 +2196,39 @@ static int sitd_submit (struct ehci_hcd *ehci, struct urb *urb,
 		sitd_link_urb (ehci, urb, ehci->periodic_size << 3, stream);
 	else
 		usb_hcd_unlink_urb_from_ep(ehci_to_hcd(ehci), urb);
-done_not_linked:
+ done_not_linked:
 	spin_unlock_irqrestore (&ehci->lock, flags);
-
-done:
-	if (status < 0)
-		iso_stream_put (ehci, stream);
+ done:
 	return status;
 }
 
 /*-------------------------------------------------------------------------*/
 
-static void free_cached_lists(struct ehci_hcd *ehci)
+static void scan_isoc(struct ehci_hcd *ehci)
 {
-	struct ehci_itd *itd, *n;
-	struct ehci_sitd *sitd, *sn;
-
-	list_for_each_entry_safe(itd, n, &ehci->cached_itd_list, itd_list) {
-		struct ehci_iso_stream	*stream = itd->stream;
-		itd->stream = NULL;
-		list_move(&itd->itd_list, &stream->free_list);
-		iso_stream_put(ehci, stream);
-	}
-
-	list_for_each_entry_safe(sitd, sn, &ehci->cached_sitd_list, sitd_list) {
-		struct ehci_iso_stream	*stream = sitd->stream;
-		sitd->stream = NULL;
-		list_move(&sitd->sitd_list, &stream->free_list);
-		iso_stream_put(ehci, stream);
-	}
-}
-
-/*-------------------------------------------------------------------------*/
-
-static void
-scan_periodic (struct ehci_hcd *ehci)
-{
-	unsigned	now_uframe, frame, clock, clock_frame, mod;
-	unsigned	modified;
-
-	mod = ehci->periodic_size << 3;
+	unsigned	uf, now_frame, frame;
+	unsigned	fmask = ehci->periodic_size - 1;
+	bool		modified, live;
 
 	/*
 	 * When running, scan from last scan point up to "now"
 	 * else clean up by scanning everything that's left.
 	 * Touches as few pages as possible:  cache-friendly.
 	 */
-	now_uframe = ehci->next_uframe;
-	if (ehci->rh_state == EHCI_RH_RUNNING) {
-		clock = ehci_read_frame_index(ehci);
-		clock_frame = (clock >> 3) & (ehci->periodic_size - 1);
+	if (ehci->rh_state >= EHCI_RH_RUNNING) {
+		uf = ehci_read_frame_index(ehci);
+		now_frame = (uf >> 3) & fmask;
+		live = true;
 	} else  {
-		clock = now_uframe + mod - 1;
-		clock_frame = -1;
+		now_frame = (ehci->next_frame - 1) & fmask;
+		live = false;
 	}
-	if (ehci->clock_frame != clock_frame) {
-		free_cached_lists(ehci);
-		ehci->clock_frame = clock_frame;
-	}
-	clock &= mod - 1;
-	clock_frame = clock >> 3;
-	++ehci->periodic_stamp;
+	ehci->now_frame = now_frame;
 
+	frame = ehci->next_frame;
 	for (;;) {
 		union ehci_shadow	q, *q_p;
 		__hc32			type, *hw_p;
-		unsigned		incomplete = false;
-
-		frame = now_uframe >> 3;
 
 restart:
 		/* scan each element in frame's queue for completions */
@@ -2329,48 +2236,17 @@ restart:
 		hw_p = &ehci->periodic [frame];
 		q.ptr = q_p->ptr;
 		type = Q_NEXT_TYPE(ehci, *hw_p);
-		modified = 0;
+		modified = false;
 
 		while (q.ptr != NULL) {
-			unsigned		uf;
-			union ehci_shadow	temp;
-			int			live;
-
-			live = (ehci->rh_state == EHCI_RH_RUNNING);
 			switch (hc32_to_cpu(ehci, type)) {
-			case Q_TYPE_QH:
-				/* handle any completions */
-				temp.qh = qh_get (q.qh);
-				type = Q_NEXT_TYPE(ehci, q.qh->hw->hw_next);
-				q = q.qh->qh_next;
-				if (temp.qh->stamp != ehci->periodic_stamp) {
-					modified = qh_completions(ehci, temp.qh);
-					if (!modified)
-						temp.qh->stamp = ehci->periodic_stamp;
-					if (unlikely(list_empty(&temp.qh->qtd_list) ||
-							temp.qh->needs_rescan))
-						intr_deschedule(ehci, temp.qh);
-				}
-				qh_put (temp.qh);
-				break;
-			case Q_TYPE_FSTN:
-				/* for "save place" FSTNs, look at QH entries
-				 * in the previous frame for completions.
-				 */
-				if (q.fstn->hw_prev != EHCI_LIST_END(ehci)) {
-					ehci_dbg(ehci,
-						"ignoring completions from FSTNs\n");
-				}
-				type = Q_NEXT_TYPE(ehci, q.fstn->hw_next);
-				q = q.fstn->fstn_next;
-				break;
 			case Q_TYPE_ITD:
 				/* If this ITD is still active, leave it for
 				 * later processing ... check the next entry.
 				 * No need to check for activity unless the
 				 * frame is current.
 				 */
-				if (frame == clock_frame && live) {
+				if (frame == now_frame && live) {
 					rmb();
 					for (uf = 0; uf < 8; uf++) {
 						if (q.itd->hw_transaction[uf] &
@@ -2378,7 +2254,6 @@ restart:
 							break;
 					}
 					if (uf < 8) {
-						incomplete = true;
 						q_p = &q.itd->itd_next;
 						hw_p = &q.itd->hw_next;
 						type = Q_NEXT_TYPE(ehci,
@@ -2410,14 +2285,12 @@ restart:
 				 * No need to check for activity unless the
 				 * frame is current.
 				 */
-				if (((frame == clock_frame) ||
-				     (((frame + 1) & (ehci->periodic_size - 1))
-				      == clock_frame))
+				if (((frame == now_frame) ||
+				     (((frame + 1) & fmask) == now_frame))
 				    && live
 				    && (q.sitd->hw_results &
 					SITD_ACTIVE(ehci))) {
 
-					incomplete = true;
 					q_p = &q.sitd->sitd_next;
 					hw_p = &q.sitd->hw_next;
 					type = Q_NEXT_TYPE(ehci,
@@ -2445,58 +2318,23 @@ restart:
 				ehci_dbg(ehci, "corrupt type %d frame %d shadow %p\n",
 					type, frame, q.ptr);
 				// BUG ();
+				/* FALL THROUGH */
+			case Q_TYPE_QH:
+			case Q_TYPE_FSTN:
+				/* End of the iTDs and siTDs */
 				q.ptr = NULL;
+				break;
 			}
 
 			/* assume completion callbacks modify the queue */
-			if (unlikely (modified)) {
-				if (likely(ehci->periodic_sched > 0))
-					goto restart;
-				/* short-circuit this scan */
-				now_uframe = clock;
-				break;
-			}
+			if (unlikely(modified && ehci->isoc_count > 0))
+				goto restart;
 		}
 
-		/* If we can tell we caught up to the hardware, stop now.
-		 * We can't advance our scan without collecting the ISO
-		 * transfers that are still pending in this frame.
-		 */
-		if (incomplete && ehci->rh_state == EHCI_RH_RUNNING) {
-			ehci->next_uframe = now_uframe;
+		/* Stop when we have reached the current frame */
+		if (frame == now_frame)
 			break;
-		}
-
-		// FIXME:  this assumes we won't get lapped when
-		// latencies climb; that should be rare, but...
-		// detect it, and just go all the way around.
-		// FLR might help detect this case, so long as latencies
-		// don't exceed periodic_size msec (default 1.024 sec).
-
-		// FIXME:  likewise assumes HC doesn't halt mid-scan
-
-		if (now_uframe == clock) {
-			unsigned	now;
-
-			if (ehci->rh_state != EHCI_RH_RUNNING
-					|| ehci->periodic_sched == 0)
-				break;
-			ehci->next_uframe = now_uframe;
-			now = ehci_read_frame_index(ehci) & (mod - 1);
-			if (now_uframe == now)
-				break;
-
-			/* rescan the rest of this frame, then ... */
-			clock = now;
-			clock_frame = clock >> 3;
-			if (ehci->clock_frame != clock_frame) {
-				free_cached_lists(ehci);
-				ehci->clock_frame = clock_frame;
-				++ehci->periodic_stamp;
-			}
-		} else {
-			now_uframe++;
-			now_uframe &= mod - 1;
-		}
+		frame = (frame + 1) & fmask;
 	}
+	ehci->next_frame = now_frame;
 }
diff --git a/drivers/usb/host/ehci-sead3.c b/drivers/usb/host/ehci-sead3.c
index cc199e87a7a..58c96bd50d2 100644
--- a/drivers/usb/host/ehci-sead3.c
+++ b/drivers/usb/host/ehci-sead3.c
@@ -160,84 +160,16 @@ static int ehci_hcd_sead3_drv_remove(struct platform_device *pdev)
 static int ehci_hcd_sead3_drv_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	unsigned long flags;
-	int rc = 0;
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(20);
-
-	/* Root hub was already suspended. Disable irq emission and
-	 * mark HW unaccessible.  The PM and USB cores make sure that
-	 * the root hub is either suspended or stopped.
-	 */
-	ehci_prepare_ports_for_controller_suspend(ehci, device_may_wakeup(dev));
-	spin_lock_irqsave(&ehci->lock, flags);
-	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
-	(void)ehci_readl(ehci, &ehci->regs->intr_enable);
-
-	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	spin_unlock_irqrestore(&ehci->lock, flags);
+	bool do_wakeup = device_may_wakeup(dev);
 
-	/* could save FLADJ in case of Vaux power loss
-	 * ... we'd only use it to handle clock skew
-	 */
-
-	return rc;
+	return ehci_suspend(hcd, do_wakeup);
 }
 
 static int ehci_hcd_sead3_drv_resume(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-	/* maybe restore FLADJ. */
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(100);
-
-	/* Mark hardware accessible again as we are out of D3 state by now */
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
-	/* If CF is still set, we maintained PCI Vaux power.
-	 * Just undo the effect of ehci_pci_suspend().
-	 */
-	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF) {
-		int	mask = INTR_MASK;
-
-		ehci_prepare_ports_for_controller_resume(ehci);
-		if (!hcd->self.root_hub->do_remote_wakeup)
-			mask &= ~STS_PCD;
-		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
-		ehci_readl(ehci, &ehci->regs->intr_enable);
-		return 0;
-	}
-
-	ehci_dbg(ehci, "lost power, restarting\n");
-	usb_root_hub_lost_power(hcd->self.root_hub);
-
-	/* Else reset, to cope with power loss or flush-to-storage
-	 * style "resume" having let BIOS kick in during reboot.
-	 */
-	(void) ehci_halt(ehci);
-	(void) ehci_reset(ehci);
-
-	/* emptying the schedule aborts any urbs */
-	spin_lock_irq(&ehci->lock);
-	if (ehci->reclaim)
-		end_unlink_async(ehci);
-	ehci_work(ehci);
-	spin_unlock_irq(&ehci->lock);
-
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
-	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
-
-	/* here we "know" root ports should always stay powered */
-	ehci_port_power(ehci, 1);
-
-	ehci->rh_state = EHCI_RH_SUSPENDED;
 
+	ehci_resume(hcd, false);
 	return 0;
 }
 
diff --git a/drivers/usb/host/ehci-sh.c b/drivers/usb/host/ehci-sh.c
index e7cb3925abf..b3f1e3650da 100644
--- a/drivers/usb/host/ehci-sh.c
+++ b/drivers/usb/host/ehci-sh.c
@@ -24,25 +24,11 @@ static int ehci_sh_reset(struct usb_hcd *hcd)
 	int ret;
 
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs + HC_LENGTH(ehci, ehci_readl(ehci,
-		&ehci->caps->hc_capbase));
 
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
-	ret = ehci_halt(ehci);
-	if (unlikely(ret))
-		return ret;
-
-	ret = ehci_init(hcd);
+	ret = ehci_setup(hcd);
 	if (unlikely(ret))
 		return ret;
 
-	ehci->sbrn = 0x20;
-
-	ehci_reset(ehci);
 	ehci_port_power(ehci, 0);
 
 	return ret;
diff --git a/drivers/usb/host/ehci-spear.c b/drivers/usb/host/ehci-spear.c
index 37ba8c8d2fd..c718a065e15 100644
--- a/drivers/usb/host/ehci-spear.c
+++ b/drivers/usb/host/ehci-spear.c
@@ -41,19 +41,11 @@ static int ehci_spear_setup(struct usb_hcd *hcd)
 
 	/* registers start at offset 0x0 */
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs + HC_LENGTH(ehci, ehci_readl(ehci,
-				&ehci->caps->hc_capbase));
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
 
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	ehci_reset(ehci);
 	ehci_port_power(ehci, 0);
 
 	return retval;
@@ -97,71 +89,16 @@ static const struct hc_driver ehci_spear_hc_driver = {
 static int ehci_spear_drv_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	unsigned long flags;
-	int rc = 0;
+	bool do_wakeup = device_may_wakeup(dev);
 
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(10);
-
-	/*
-	 * Root hub was already suspended. Disable irq emission and mark HW
-	 * unaccessible. The PM and USB cores make sure that the root hub is
-	 * either suspended or stopped.
-	 */
-	spin_lock_irqsave(&ehci->lock, flags);
-	ehci_prepare_ports_for_controller_suspend(ehci, device_may_wakeup(dev));
-	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
-	ehci_readl(ehci, &ehci->regs->intr_enable);
-	spin_unlock_irqrestore(&ehci->lock, flags);
-
-	return rc;
+	return ehci_suspend(hcd, do_wakeup);
 }
 
 static int ehci_spear_drv_resume(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-	if (time_before(jiffies, ehci->next_statechange))
-		msleep(100);
-
-	if (ehci_readl(ehci, &ehci->regs->configured_flag) == FLAG_CF) {
-		int mask = INTR_MASK;
-
-		ehci_prepare_ports_for_controller_resume(ehci);
-
-		if (!hcd->self.root_hub->do_remote_wakeup)
-			mask &= ~STS_PCD;
-
-		ehci_writel(ehci, mask, &ehci->regs->intr_enable);
-		ehci_readl(ehci, &ehci->regs->intr_enable);
-		return 0;
-	}
-
-	usb_root_hub_lost_power(hcd->self.root_hub);
-
-	/*
-	 * Else reset, to cope with power loss or flush-to-storage style
-	 * "resume" having let BIOS kick in during reboot.
-	 */
-	ehci_halt(ehci);
-	ehci_reset(ehci);
-
-	/* emptying the schedule aborts any urbs */
-	spin_lock_irq(&ehci->lock);
-	if (ehci->reclaim)
-		end_unlink_async(ehci);
-
-	ehci_work(ehci);
-	spin_unlock_irq(&ehci->lock);
-
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
-	ehci_readl(ehci, &ehci->regs->command);	/* unblock posted writes */
 
-	/* here we "know" root ports should always stay powered */
-	ehci_port_power(ehci, 1);
+	ehci_resume(hcd, false);
 	return 0;
 }
 #endif /* CONFIG_PM */
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 68548236ec4..950e95efa38 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/platform_data/tegra_usb.h>
 #include <linux/irq.h>
@@ -46,8 +47,8 @@ static void tegra_ehci_power_up(struct usb_hcd *hcd)
 {
 	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
 
-	clk_enable(tegra->emc_clk);
-	clk_enable(tegra->clk);
+	clk_prepare_enable(tegra->emc_clk);
+	clk_prepare_enable(tegra->clk);
 	tegra_usb_phy_power_on(tegra->phy);
 	tegra->host_resumed = 1;
 }
@@ -58,8 +59,8 @@ static void tegra_ehci_power_down(struct usb_hcd *hcd)
 
 	tegra->host_resumed = 0;
 	tegra_usb_phy_power_off(tegra->phy);
-	clk_disable(tegra->clk);
-	clk_disable(tegra->emc_clk);
+	clk_disable_unprepare(tegra->clk);
+	clk_disable_unprepare(tegra->emc_clk);
 }
 
 static int tegra_ehci_internal_port_reset(
@@ -280,30 +281,14 @@ static int tegra_ehci_setup(struct usb_hcd *hcd)
 
 	/* EHCI registers start at offset 0x100 */
 	ehci->caps = hcd->regs + 0x100;
-	ehci->regs = hcd->regs + 0x100 +
-		HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
-
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = readl(&ehci->caps->hcs_params);
 
 	/* switch to host mode */
 	hcd->has_tt = 1;
-	ehci_reset(ehci);
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
 
-	/* data structure init */
-	retval = ehci_init(hcd);
+	retval = ehci_setup(hcd);
 	if (retval)
 		return retval;
 
-	ehci->sbrn = 0x20;
-
 	ehci_port_power(ehci, 1);
 	return retval;
 }
@@ -460,12 +445,11 @@ static int controller_suspend(struct device *dev)
 	if (time_before(jiffies, ehci->next_statechange))
 		msleep(10);
 
-	spin_lock_irqsave(&ehci->lock, flags);
+	ehci_halt(ehci);
 
+	spin_lock_irqsave(&ehci->lock, flags);
 	tegra->port_speed = (readl(&hw->port_status[0]) >> 26) & 0x3;
-	ehci_halt(ehci);
 	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
 	spin_unlock_irqrestore(&ehci->lock, flags);
 
 	tegra_ehci_power_down(hcd);
@@ -671,7 +655,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		goto fail_clk;
 	}
 
-	err = clk_enable(tegra->clk);
+	err = clk_prepare_enable(tegra->clk);
 	if (err)
 		goto fail_clken;
 
@@ -682,7 +666,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		goto fail_emc_clk;
 	}
 
-	clk_enable(tegra->emc_clk);
+	clk_prepare_enable(tegra->emc_clk);
 	clk_set_rate(tegra->emc_clk, 400000000);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -749,8 +733,8 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 
 #ifdef CONFIG_USB_OTG_UTILS
 	if (pdata->operating_mode == TEGRA_USB_OTG) {
-		tegra->transceiver = usb_get_transceiver();
-		if (tegra->transceiver)
+		tegra->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
+		if (!IS_ERR_OR_NULL(tegra->transceiver))
 			otg_set_host(tegra->transceiver->otg, &hcd->self);
 	}
 #endif
@@ -773,19 +757,19 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 
 fail:
 #ifdef CONFIG_USB_OTG_UTILS
-	if (tegra->transceiver) {
+	if (!IS_ERR_OR_NULL(tegra->transceiver)) {
 		otg_set_host(tegra->transceiver->otg, NULL);
-		usb_put_transceiver(tegra->transceiver);
+		usb_put_phy(tegra->transceiver);
 	}
 #endif
 	tegra_usb_phy_close(tegra->phy);
 fail_phy:
 	iounmap(hcd->regs);
 fail_io:
-	clk_disable(tegra->emc_clk);
+	clk_disable_unprepare(tegra->emc_clk);
 	clk_put(tegra->emc_clk);
 fail_emc_clk:
-	clk_disable(tegra->clk);
+	clk_disable_unprepare(tegra->clk);
 fail_clken:
 	clk_put(tegra->clk);
 fail_clk:
@@ -808,9 +792,9 @@ static int tegra_ehci_remove(struct platform_device *pdev)
 	pm_runtime_put_noidle(&pdev->dev);
 
 #ifdef CONFIG_USB_OTG_UTILS
-	if (tegra->transceiver) {
+	if (!IS_ERR_OR_NULL(tegra->transceiver)) {
 		otg_set_host(tegra->transceiver->otg, NULL);
-		usb_put_transceiver(tegra->transceiver);
+		usb_put_phy(tegra->transceiver);
 	}
 #endif
 
@@ -820,10 +804,10 @@ static int tegra_ehci_remove(struct platform_device *pdev)
 	tegra_usb_phy_close(tegra->phy);
 	iounmap(hcd->regs);
 
-	clk_disable(tegra->clk);
+	clk_disable_unprepare(tegra->clk);
 	clk_put(tegra->clk);
 
-	clk_disable(tegra->emc_clk);
+	clk_disable_unprepare(tegra->emc_clk);
 	clk_put(tegra->emc_clk);
 
 	kfree(tegra);
diff --git a/drivers/usb/host/ehci-tilegx.c b/drivers/usb/host/ehci-tilegx.c
new file mode 100644
index 00000000000..1d215cdb9de
--- /dev/null
+++ b/drivers/usb/host/ehci-tilegx.c
@@ -0,0 +1,214 @@
+/*
+ * Copyright 2012 Tilera Corporation. All Rights Reserved.
+ *
+ *   This program is free software; you can redistribute it and/or
+ *   modify it under the terms of the GNU General Public License
+ *   as published by the Free Software Foundation, version 2.
+ *
+ *   This program is distributed in the hope that it will be useful, but
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ *   NON INFRINGEMENT.  See the GNU General Public License for
+ *   more details.
+ */
+
+/*
+ * Tilera TILE-Gx USB EHCI host controller driver.
+ */
+
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/usb/tilegx.h>
+#include <linux/usb.h>
+
+#include <asm/homecache.h>
+
+#include <gxio/iorpc_usb_host.h>
+#include <gxio/usb_host.h>
+
+static void tilegx_start_ehc(void)
+{
+}
+
+static void tilegx_stop_ehc(void)
+{
+}
+
+static int tilegx_ehci_setup(struct usb_hcd *hcd)
+{
+	int ret = ehci_init(hcd);
+
+	/*
+	 * Some drivers do:
+	 *
+	 *   struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	 *   ehci->need_io_watchdog = 0;
+	 *
+	 * here, but since this is a new driver we're going to leave the
+	 * watchdog enabled.  Later we may try to turn it off and see
+	 * whether we run into any problems.
+	 */
+
+	return ret;
+}
+
+static const struct hc_driver ehci_tilegx_hc_driver = {
+	.description		= hcd_name,
+	.product_desc		= "Tile-Gx EHCI",
+	.hcd_priv_size		= sizeof(struct ehci_hcd),
+
+	/*
+	 * Generic hardware linkage.
+	 */
+	.irq			= ehci_irq,
+	.flags			= HCD_MEMORY | HCD_USB2,
+
+	/*
+	 * Basic lifecycle operations.
+	 */
+	.reset			= tilegx_ehci_setup,
+	.start			= ehci_run,
+	.stop			= ehci_stop,
+	.shutdown		= ehci_shutdown,
+
+	/*
+	 * Managing I/O requests and associated device resources.
+	 */
+	.urb_enqueue		= ehci_urb_enqueue,
+	.urb_dequeue		= ehci_urb_dequeue,
+	.endpoint_disable	= ehci_endpoint_disable,
+	.endpoint_reset		= ehci_endpoint_reset,
+
+	/*
+	 * Scheduling support.
+	 */
+	.get_frame_number	= ehci_get_frame,
+
+	/*
+	 * Root hub support.
+	 */
+	.hub_status_data	= ehci_hub_status_data,
+	.hub_control		= ehci_hub_control,
+	.bus_suspend		= ehci_bus_suspend,
+	.bus_resume		= ehci_bus_resume,
+	.relinquish_port	= ehci_relinquish_port,
+	.port_handed_over	= ehci_port_handed_over,
+
+	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
+};
+
+static int ehci_hcd_tilegx_drv_probe(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd;
+	struct ehci_hcd *ehci;
+	struct tilegx_usb_platform_data *pdata = pdev->dev.platform_data;
+	pte_t pte = { 0 };
+	int my_cpu = smp_processor_id();
+	int ret;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	/*
+	 * Try to initialize our GXIO context; if we can't, the device
+	 * doesn't exist.
+	 */
+	if (gxio_usb_host_init(&pdata->usb_ctx, pdata->dev_index, 1) != 0)
+		return -ENXIO;
+
+	hcd = usb_create_hcd(&ehci_tilegx_hc_driver, &pdev->dev,
+			     dev_name(&pdev->dev));
+	if (!hcd)
+		return -ENOMEM;
+
+	/*
+	 * We don't use rsrc_start to map in our registers, but seems like
+	 * we ought to set it to something, so we use the register VA.
+	 */
+	hcd->rsrc_start =
+		(ulong) gxio_usb_host_get_reg_start(&pdata->usb_ctx);
+	hcd->rsrc_len = gxio_usb_host_get_reg_len(&pdata->usb_ctx);
+	hcd->regs = gxio_usb_host_get_reg_start(&pdata->usb_ctx);
+
+	tilegx_start_ehc();
+
+	ehci = hcd_to_ehci(hcd);
+	ehci->caps = hcd->regs;
+	ehci->regs =
+		hcd->regs + HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
+	/* cache this readonly data; minimize chip reads */
+	ehci->hcs_params = readl(&ehci->caps->hcs_params);
+
+	/* Create our IRQs and register them. */
+	pdata->irq = create_irq();
+	if (pdata->irq < 0) {
+		ret = -ENXIO;
+		goto err_no_irq;
+	}
+
+	tile_irq_activate(pdata->irq, TILE_IRQ_PERCPU);
+
+	/* Configure interrupts. */
+	ret = gxio_usb_host_cfg_interrupt(&pdata->usb_ctx,
+					  cpu_x(my_cpu), cpu_y(my_cpu),
+					  KERNEL_PL, pdata->irq);
+	if (ret) {
+		ret = -ENXIO;
+		goto err_have_irq;
+	}
+
+	/* Register all of our memory. */
+	pte = pte_set_home(pte, PAGE_HOME_HASH);
+	ret = gxio_usb_host_register_client_memory(&pdata->usb_ctx, pte, 0);
+	if (ret) {
+		ret = -ENXIO;
+		goto err_have_irq;
+	}
+
+	ret = usb_add_hcd(hcd, pdata->irq, IRQF_SHARED);
+	if (ret == 0) {
+		platform_set_drvdata(pdev, hcd);
+		return ret;
+	}
+
+err_have_irq:
+	destroy_irq(pdata->irq);
+err_no_irq:
+	tilegx_stop_ehc();
+	usb_put_hcd(hcd);
+	gxio_usb_host_destroy(&pdata->usb_ctx);
+	return ret;
+}
+
+static int ehci_hcd_tilegx_drv_remove(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct tilegx_usb_platform_data *pdata = pdev->dev.platform_data;
+
+	usb_remove_hcd(hcd);
+	usb_put_hcd(hcd);
+	tilegx_stop_ehc();
+	gxio_usb_host_destroy(&pdata->usb_ctx);
+	destroy_irq(pdata->irq);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static void ehci_hcd_tilegx_drv_shutdown(struct platform_device *pdev)
+{
+	usb_hcd_platform_shutdown(pdev);
+	ehci_hcd_tilegx_drv_remove(pdev);
+}
+
+static struct platform_driver ehci_hcd_tilegx_driver = {
+	.probe		= ehci_hcd_tilegx_drv_probe,
+	.remove		= ehci_hcd_tilegx_drv_remove,
+	.shutdown	= ehci_hcd_tilegx_drv_shutdown,
+	.driver = {
+		.name	= "tilegx-ehci",
+		.owner	= THIS_MODULE,
+	}
+};
+
+MODULE_ALIAS("platform:tilegx-ehci");
diff --git a/drivers/usb/host/ehci-timer.c b/drivers/usb/host/ehci-timer.c
new file mode 100644
index 00000000000..eb896a2c8f2
--- /dev/null
+++ b/drivers/usb/host/ehci-timer.c
@@ -0,0 +1,401 @@
+/*
+ * Copyright (C) 2012 by Alan Stern
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+
+/* This file is part of ehci-hcd.c */
+
+/*-------------------------------------------------------------------------*/
+
+/* Set a bit in the USBCMD register */
+static void ehci_set_command_bit(struct ehci_hcd *ehci, u32 bit)
+{
+	ehci->command |= bit;
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
+
+	/* unblock posted write */
+	ehci_readl(ehci, &ehci->regs->command);
+}
+
+/* Clear a bit in the USBCMD register */
+static void ehci_clear_command_bit(struct ehci_hcd *ehci, u32 bit)
+{
+	ehci->command &= ~bit;
+	ehci_writel(ehci, ehci->command, &ehci->regs->command);
+
+	/* unblock posted write */
+	ehci_readl(ehci, &ehci->regs->command);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * EHCI timer support...  Now using hrtimers.
+ *
+ * Lots of different events are triggered from ehci->hrtimer.  Whenever
+ * the timer routine runs, it checks each possible event; events that are
+ * currently enabled and whose expiration time has passed get handled.
+ * The set of enabled events is stored as a collection of bitflags in
+ * ehci->enabled_hrtimer_events, and they are numbered in order of
+ * increasing delay values (ranging between 1 ms and 100 ms).
+ *
+ * Rather than implementing a sorted list or tree of all pending events,
+ * we keep track only of the lowest-numbered pending event, in
+ * ehci->next_hrtimer_event.  Whenever ehci->hrtimer gets restarted, its
+ * expiration time is set to the timeout value for this event.
+ *
+ * As a result, events might not get handled right away; the actual delay
+ * could be anywhere up to twice the requested delay.  This doesn't
+ * matter, because none of the events are especially time-critical.  The
+ * ones that matter most all have a delay of 1 ms, so they will be
+ * handled after 2 ms at most, which is okay.  In addition to this, we
+ * allow for an expiration range of 1 ms.
+ */
+
+/*
+ * Delay lengths for the hrtimer event types.
+ * Keep this list sorted by delay length, in the same order as
+ * the event types indexed by enum ehci_hrtimer_event in ehci.h.
+ */
+static unsigned event_delays_ns[] = {
+	1 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_POLL_ASS */
+	1 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_POLL_PSS */
+	1 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_POLL_DEAD */
+	1125 * NSEC_PER_USEC,	/* EHCI_HRTIMER_UNLINK_INTR */
+	2 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_FREE_ITDS */
+	6 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_ASYNC_UNLINKS */
+	10 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_IAA_WATCHDOG */
+	10 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_DISABLE_PERIODIC */
+	15 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_DISABLE_ASYNC */
+	100 * NSEC_PER_MSEC,	/* EHCI_HRTIMER_IO_WATCHDOG */
+};
+
+/* Enable a pending hrtimer event */
+static void ehci_enable_event(struct ehci_hcd *ehci, unsigned event,
+		bool resched)
+{
+	ktime_t		*timeout = &ehci->hr_timeouts[event];
+
+	if (resched)
+		*timeout = ktime_add(ktime_get(),
+				ktime_set(0, event_delays_ns[event]));
+	ehci->enabled_hrtimer_events |= (1 << event);
+
+	/* Track only the lowest-numbered pending event */
+	if (event < ehci->next_hrtimer_event) {
+		ehci->next_hrtimer_event = event;
+		hrtimer_start_range_ns(&ehci->hrtimer, *timeout,
+				NSEC_PER_MSEC, HRTIMER_MODE_ABS);
+	}
+}
+
+
+/* Poll the STS_ASS status bit; see when it agrees with CMD_ASE */
+static void ehci_poll_ASS(struct ehci_hcd *ehci)
+{
+	unsigned	actual, want;
+
+	/* Don't enable anything if the controller isn't running (e.g., died) */
+	if (ehci->rh_state != EHCI_RH_RUNNING)
+		return;
+
+	want = (ehci->command & CMD_ASE) ? STS_ASS : 0;
+	actual = ehci_readl(ehci, &ehci->regs->status) & STS_ASS;
+
+	if (want != actual) {
+
+		/* Poll again later, but give up after about 20 ms */
+		if (ehci->ASS_poll_count++ < 20) {
+			ehci_enable_event(ehci, EHCI_HRTIMER_POLL_ASS, true);
+			return;
+		}
+		ehci_warn(ehci, "Waited too long for the async schedule status, giving up\n");
+	}
+	ehci->ASS_poll_count = 0;
+
+	/* The status is up-to-date; restart or stop the schedule as needed */
+	if (want == 0) {	/* Stopped */
+		if (ehci->async_count > 0)
+			ehci_set_command_bit(ehci, CMD_ASE);
+
+	} else {		/* Running */
+		if (ehci->async_count == 0) {
+
+			/* Turn off the schedule after a while */
+			ehci_enable_event(ehci, EHCI_HRTIMER_DISABLE_ASYNC,
+					true);
+		}
+	}
+}
+
+/* Turn off the async schedule after a brief delay */
+static void ehci_disable_ASE(struct ehci_hcd *ehci)
+{
+	ehci_clear_command_bit(ehci, CMD_ASE);
+}
+
+
+/* Poll the STS_PSS status bit; see when it agrees with CMD_PSE */
+static void ehci_poll_PSS(struct ehci_hcd *ehci)
+{
+	unsigned	actual, want;
+
+	/* Don't do anything if the controller isn't running (e.g., died) */
+	if (ehci->rh_state != EHCI_RH_RUNNING)
+		return;
+
+	want = (ehci->command & CMD_PSE) ? STS_PSS : 0;
+	actual = ehci_readl(ehci, &ehci->regs->status) & STS_PSS;
+
+	if (want != actual) {
+
+		/* Poll again later, but give up after about 20 ms */
+		if (ehci->PSS_poll_count++ < 20) {
+			ehci_enable_event(ehci, EHCI_HRTIMER_POLL_PSS, true);
+			return;
+		}
+		ehci_warn(ehci, "Waited too long for the periodic schedule status, giving up\n");
+	}
+	ehci->PSS_poll_count = 0;
+
+	/* The status is up-to-date; restart or stop the schedule as needed */
+	if (want == 0) {	/* Stopped */
+		if (ehci->periodic_count > 0)
+			ehci_set_command_bit(ehci, CMD_PSE);
+
+	} else {		/* Running */
+		if (ehci->periodic_count == 0) {
+
+			/* Turn off the schedule after a while */
+			ehci_enable_event(ehci, EHCI_HRTIMER_DISABLE_PERIODIC,
+					true);
+		}
+	}
+}
+
+/* Turn off the periodic schedule after a brief delay */
+static void ehci_disable_PSE(struct ehci_hcd *ehci)
+{
+	ehci_clear_command_bit(ehci, CMD_PSE);
+}
+
+
+/* Poll the STS_HALT status bit; see when a dead controller stops */
+static void ehci_handle_controller_death(struct ehci_hcd *ehci)
+{
+	if (!(ehci_readl(ehci, &ehci->regs->status) & STS_HALT)) {
+
+		/* Give up after a few milliseconds */
+		if (ehci->died_poll_count++ < 5) {
+			/* Try again later */
+			ehci_enable_event(ehci, EHCI_HRTIMER_POLL_DEAD, true);
+			return;
+		}
+		ehci_warn(ehci, "Waited too long for the controller to stop, giving up\n");
+	}
+
+	/* Clean up the mess */
+	ehci->rh_state = EHCI_RH_HALTED;
+	ehci_writel(ehci, 0, &ehci->regs->configured_flag);
+	ehci_writel(ehci, 0, &ehci->regs->intr_enable);
+	ehci_work(ehci);
+	end_unlink_async(ehci);
+
+	/* Not in process context, so don't try to reset the controller */
+}
+
+
+/* Handle unlinked interrupt QHs once they are gone from the hardware */
+static void ehci_handle_intr_unlinks(struct ehci_hcd *ehci)
+{
+	bool		stopped = (ehci->rh_state < EHCI_RH_RUNNING);
+
+	/*
+	 * Process all the QHs on the intr_unlink list that were added
+	 * before the current unlink cycle began.  The list is in
+	 * temporal order, so stop when we reach the first entry in the
+	 * current cycle.  But if the root hub isn't running then
+	 * process all the QHs on the list.
+	 */
+	ehci->intr_unlinking = true;
+	while (ehci->intr_unlink) {
+		struct ehci_qh	*qh = ehci->intr_unlink;
+
+		if (!stopped && qh->unlink_cycle == ehci->intr_unlink_cycle)
+			break;
+		ehci->intr_unlink = qh->unlink_next;
+		qh->unlink_next = NULL;
+		end_unlink_intr(ehci, qh);
+	}
+
+	/* Handle remaining entries later */
+	if (ehci->intr_unlink) {
+		ehci_enable_event(ehci, EHCI_HRTIMER_UNLINK_INTR, true);
+		++ehci->intr_unlink_cycle;
+	}
+	ehci->intr_unlinking = false;
+}
+
+
+/* Start another free-iTDs/siTDs cycle */
+static void start_free_itds(struct ehci_hcd *ehci)
+{
+	if (!(ehci->enabled_hrtimer_events & BIT(EHCI_HRTIMER_FREE_ITDS))) {
+		ehci->last_itd_to_free = list_entry(
+				ehci->cached_itd_list.prev,
+				struct ehci_itd, itd_list);
+		ehci->last_sitd_to_free = list_entry(
+				ehci->cached_sitd_list.prev,
+				struct ehci_sitd, sitd_list);
+		ehci_enable_event(ehci, EHCI_HRTIMER_FREE_ITDS, true);
+	}
+}
+
+/* Wait for controller to stop using old iTDs and siTDs */
+static void end_free_itds(struct ehci_hcd *ehci)
+{
+	struct ehci_itd		*itd, *n;
+	struct ehci_sitd	*sitd, *sn;
+
+	if (ehci->rh_state < EHCI_RH_RUNNING) {
+		ehci->last_itd_to_free = NULL;
+		ehci->last_sitd_to_free = NULL;
+	}
+
+	list_for_each_entry_safe(itd, n, &ehci->cached_itd_list, itd_list) {
+		list_del(&itd->itd_list);
+		dma_pool_free(ehci->itd_pool, itd, itd->itd_dma);
+		if (itd == ehci->last_itd_to_free)
+			break;
+	}
+	list_for_each_entry_safe(sitd, sn, &ehci->cached_sitd_list, sitd_list) {
+		list_del(&sitd->sitd_list);
+		dma_pool_free(ehci->sitd_pool, sitd, sitd->sitd_dma);
+		if (sitd == ehci->last_sitd_to_free)
+			break;
+	}
+
+	if (!list_empty(&ehci->cached_itd_list) ||
+			!list_empty(&ehci->cached_sitd_list))
+		start_free_itds(ehci);
+}
+
+
+/* Handle lost (or very late) IAA interrupts */
+static void ehci_iaa_watchdog(struct ehci_hcd *ehci)
+{
+	if (ehci->rh_state != EHCI_RH_RUNNING)
+		return;
+
+	/*
+	 * Lost IAA irqs wedge things badly; seen first with a vt8235.
+	 * So we need this watchdog, but must protect it against both
+	 * (a) SMP races against real IAA firing and retriggering, and
+	 * (b) clean HC shutdown, when IAA watchdog was pending.
+	 */
+	if (ehci->async_iaa) {
+		u32 cmd, status;
+
+		/* If we get here, IAA is *REALLY* late.  It's barely
+		 * conceivable that the system is so busy that CMD_IAAD
+		 * is still legitimately set, so let's be sure it's
+		 * clear before we read STS_IAA.  (The HC should clear
+		 * CMD_IAAD when it sets STS_IAA.)
+		 */
+		cmd = ehci_readl(ehci, &ehci->regs->command);
+
+		/*
+		 * If IAA is set here it either legitimately triggered
+		 * after the watchdog timer expired (_way_ late, so we'll
+		 * still count it as lost) ... or a silicon erratum:
+		 * - VIA seems to set IAA without triggering the IRQ;
+		 * - IAAD potentially cleared without setting IAA.
+		 */
+		status = ehci_readl(ehci, &ehci->regs->status);
+		if ((status & STS_IAA) || !(cmd & CMD_IAAD)) {
+			COUNT(ehci->stats.lost_iaa);
+			ehci_writel(ehci, STS_IAA, &ehci->regs->status);
+		}
+
+		ehci_vdbg(ehci, "IAA watchdog: status %x cmd %x\n",
+				status, cmd);
+		end_unlink_async(ehci);
+	}
+}
+
+
+/* Enable the I/O watchdog, if appropriate */
+static void turn_on_io_watchdog(struct ehci_hcd *ehci)
+{
+	/* Not needed if the controller isn't running or it's already enabled */
+	if (ehci->rh_state != EHCI_RH_RUNNING ||
+			(ehci->enabled_hrtimer_events &
+				BIT(EHCI_HRTIMER_IO_WATCHDOG)))
+		return;
+
+	/*
+	 * Isochronous transfers always need the watchdog.
+	 * For other sorts we use it only if the flag is set.
+	 */
+	if (ehci->isoc_count > 0 || (ehci->need_io_watchdog &&
+			ehci->async_count + ehci->intr_count > 0))
+		ehci_enable_event(ehci, EHCI_HRTIMER_IO_WATCHDOG, true);
+}
+
+
+/*
+ * Handler functions for the hrtimer event types.
+ * Keep this array in the same order as the event types indexed by
+ * enum ehci_hrtimer_event in ehci.h.
+ */
+static void (*event_handlers[])(struct ehci_hcd *) = {
+	ehci_poll_ASS,			/* EHCI_HRTIMER_POLL_ASS */
+	ehci_poll_PSS,			/* EHCI_HRTIMER_POLL_PSS */
+	ehci_handle_controller_death,	/* EHCI_HRTIMER_POLL_DEAD */
+	ehci_handle_intr_unlinks,	/* EHCI_HRTIMER_UNLINK_INTR */
+	end_free_itds,			/* EHCI_HRTIMER_FREE_ITDS */
+	unlink_empty_async,		/* EHCI_HRTIMER_ASYNC_UNLINKS */
+	ehci_iaa_watchdog,		/* EHCI_HRTIMER_IAA_WATCHDOG */
+	ehci_disable_PSE,		/* EHCI_HRTIMER_DISABLE_PERIODIC */
+	ehci_disable_ASE,		/* EHCI_HRTIMER_DISABLE_ASYNC */
+	ehci_work,			/* EHCI_HRTIMER_IO_WATCHDOG */
+};
+
+static enum hrtimer_restart ehci_hrtimer_func(struct hrtimer *t)
+{
+	struct ehci_hcd	*ehci = container_of(t, struct ehci_hcd, hrtimer);
+	ktime_t		now;
+	unsigned long	events;
+	unsigned long	flags;
+	unsigned	e;
+
+	spin_lock_irqsave(&ehci->lock, flags);
+
+	events = ehci->enabled_hrtimer_events;
+	ehci->enabled_hrtimer_events = 0;
+	ehci->next_hrtimer_event = EHCI_HRTIMER_NO_EVENT;
+
+	/*
+	 * Check each pending event.  If its time has expired, handle
+	 * the event; otherwise re-enable it.
+	 */
+	now = ktime_get();
+	for_each_set_bit(e, &events, EHCI_HRTIMER_NUM_EVENTS) {
+		if (now.tv64 >= ehci->hr_timeouts[e].tv64)
+			event_handlers[e](ehci);
+		else
+			ehci_enable_event(ehci, e, false);
+	}
+
+	spin_unlock_irqrestore(&ehci->lock, flags);
+	return HRTIMER_NORESTART;
+}
diff --git a/drivers/usb/host/ehci-vt8500.c b/drivers/usb/host/ehci-vt8500.c
index c1eda73916c..4d147c4e33f 100644
--- a/drivers/usb/host/ehci-vt8500.c
+++ b/drivers/usb/host/ehci-vt8500.c
@@ -48,7 +48,7 @@ static const struct hc_driver vt8500_ehci_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
-	.reset			= ehci_init,
+	.reset			= ehci_setup,
 	.start			= ehci_run,
 	.stop			= ehci_stop,
 	.shutdown		= ehci_shutdown,
@@ -121,18 +121,6 @@ static int vt8500_ehci_drv_probe(struct platform_device *pdev)
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
-
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = readl(&ehci->caps->hcs_params);
-
-	ehci_port_power(ehci, 1);
-
-	ehci_reset(ehci);
 
 	ret = usb_add_hcd(hcd, pdev->resource[1].start,
 			  IRQF_SHARED);
diff --git a/drivers/usb/host/ehci-w90x900.c b/drivers/usb/host/ehci-w90x900.c
index 3d2e26cbb34..ec598082c14 100644
--- a/drivers/usb/host/ehci-w90x900.c
+++ b/drivers/usb/host/ehci-w90x900.c
@@ -71,21 +71,14 @@ static int __devinit usb_w90x900_probe(const struct hc_driver *driver,
 	val |= ENPHY;
 	__raw_writel(val, ehci->regs+PHY1_CTR);
 
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-	ehci->sbrn = 0x20;
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
 		goto err4;
 
-	ehci_reset(ehci);
-
 	retval = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (retval != 0)
 		goto err4;
 
-	ehci_writel(ehci, 1, &ehci->regs->configured_flag);
-
 	return retval;
 err4:
 	iounmap(hcd->regs);
@@ -120,7 +113,7 @@ static const struct hc_driver ehci_w90x900_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
-	.reset = ehci_init,
+	.reset = ehci_setup,
 	.start = ehci_run,
 
 	.stop = ehci_stop,
diff --git a/drivers/usb/host/ehci-xilinx-of.c b/drivers/usb/host/ehci-xilinx-of.c
index e9713d589e3..39f24fa37eb 100644
--- a/drivers/usb/host/ehci-xilinx-of.c
+++ b/drivers/usb/host/ehci-xilinx-of.c
@@ -32,30 +32,6 @@
 #include <linux/of_address.h>
 
 /**
- * ehci_xilinx_of_setup - Initialize the device for ehci_reset()
- * @hcd:	Pointer to the usb_hcd device to which the host controller bound
- *
- * called during probe() after chip reset completes.
- */
-static int ehci_xilinx_of_setup(struct usb_hcd *hcd)
-{
-	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
-	int		retval;
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
-
-	retval = ehci_init(hcd);
-	if (retval)
-		return retval;
-
-	ehci->sbrn = 0x20;
-
-	return ehci_reset(ehci);
-}
-
-/**
  * ehci_xilinx_port_handed_over - hand the port out if failed to enable it
  * @hcd:	Pointer to the usb_hcd device to which the host controller bound
  * @portnum:Port number to which the device is attached.
@@ -107,7 +83,7 @@ static const struct hc_driver ehci_xilinx_of_hc_driver = {
 	/*
 	 * basic lifecycle operations
 	 */
-	.reset			= ehci_xilinx_of_setup,
+	.reset			= ehci_setup,
 	.start			= ehci_run,
 	.stop			= ehci_stop,
 	.shutdown		= ehci_shutdown,
@@ -219,11 +195,6 @@ static int __devinit ehci_hcd_xilinx_of_probe(struct platform_device *op)
 	/* Debug registers are at the first 0x100 region
 	 */
 	ehci->caps = hcd->regs + 0x100;
-	ehci->regs = hcd->regs + 0x100 +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
 	rv = usb_add_hcd(hcd, irq, 0);
 	if (rv == 0)
diff --git a/drivers/usb/host/ehci-xls.c b/drivers/usb/host/ehci-xls.c
index 72f08196f8c..8dc6a22d90b 100644
--- a/drivers/usb/host/ehci-xls.c
+++ b/drivers/usb/host/ehci-xls.c
@@ -14,30 +14,11 @@
 
 static int ehci_xls_setup(struct usb_hcd *hcd)
 {
-	int	retval;
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 
 	ehci->caps = hcd->regs;
-	ehci->regs = hcd->regs +
-		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
-	dbg_hcs_params(ehci, "reset");
-	dbg_hcc_params(ehci, "reset");
 
-	/* cache this readonly data; minimize chip reads */
-	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
-
-	retval = ehci_halt(ehci);
-	if (retval)
-		return retval;
-
-	/* data structure init */
-	retval = ehci_init(hcd);
-	if (retval)
-		return retval;
-
-	ehci_reset(ehci);
-
-	return retval;
+	return ehci_setup(hcd);
 }
 
 int ehci_xls_probe_internal(const struct hc_driver *driver,
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 2694ed6558d..da07d98f7d1 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -42,7 +42,7 @@ struct ehci_stats {
 	/* irq usage */
 	unsigned long		normal;
 	unsigned long		error;
-	unsigned long		reclaim;
+	unsigned long		iaa;
 	unsigned long		lost_iaa;
 
 	/* termination of urbs from core */
@@ -51,7 +51,7 @@ struct ehci_stats {
 };
 
 /* ehci_hcd->lock guards shared data against other CPUs:
- *   ehci_hcd:	async, reclaim, periodic (and shadow), ...
+ *   ehci_hcd:	async, unlink, periodic (and shadow), ...
  *   usb_host_endpoint: hcpriv
  *   ehci_qh:	qh_next, qtd_list
  *   ehci_qtd:	qtd_list
@@ -62,13 +62,48 @@ struct ehci_stats {
 
 #define	EHCI_MAX_ROOT_PORTS	15		/* see HCS_N_PORTS */
 
+/*
+ * ehci_rh_state values of EHCI_RH_RUNNING or above mean that the
+ * controller may be doing DMA.  Lower values mean there's no DMA.
+ */
 enum ehci_rh_state {
 	EHCI_RH_HALTED,
 	EHCI_RH_SUSPENDED,
-	EHCI_RH_RUNNING
+	EHCI_RH_RUNNING,
+	EHCI_RH_STOPPING
 };
 
+/*
+ * Timer events, ordered by increasing delay length.
+ * Always update event_delays_ns[] and event_handlers[] (defined in
+ * ehci-timer.c) in parallel with this list.
+ */
+enum ehci_hrtimer_event {
+	EHCI_HRTIMER_POLL_ASS,		/* Poll for async schedule off */
+	EHCI_HRTIMER_POLL_PSS,		/* Poll for periodic schedule off */
+	EHCI_HRTIMER_POLL_DEAD,		/* Wait for dead controller to stop */
+	EHCI_HRTIMER_UNLINK_INTR,	/* Wait for interrupt QH unlink */
+	EHCI_HRTIMER_FREE_ITDS,		/* Wait for unused iTDs and siTDs */
+	EHCI_HRTIMER_ASYNC_UNLINKS,	/* Unlink empty async QHs */
+	EHCI_HRTIMER_IAA_WATCHDOG,	/* Handle lost IAA interrupts */
+	EHCI_HRTIMER_DISABLE_PERIODIC,	/* Wait to disable periodic sched */
+	EHCI_HRTIMER_DISABLE_ASYNC,	/* Wait to disable async sched */
+	EHCI_HRTIMER_IO_WATCHDOG,	/* Check for missing IRQs */
+	EHCI_HRTIMER_NUM_EVENTS		/* Must come last */
+};
+#define EHCI_HRTIMER_NO_EVENT	99
+
 struct ehci_hcd {			/* one per controller */
+	/* timing support */
+	enum ehci_hrtimer_event	next_hrtimer_event;
+	unsigned		enabled_hrtimer_events;
+	ktime_t			hr_timeouts[EHCI_HRTIMER_NUM_EVENTS];
+	struct hrtimer		hrtimer;
+
+	int			PSS_poll_count;
+	int			ASS_poll_count;
+	int			died_poll_count;
+
 	/* glue to PCI and HCD framework */
 	struct ehci_caps __iomem *caps;
 	struct ehci_regs __iomem *regs;
@@ -78,30 +113,48 @@ struct ehci_hcd {			/* one per controller */
 	spinlock_t		lock;
 	enum ehci_rh_state	rh_state;
 
+	/* general schedule support */
+	bool			scanning:1;
+	bool			need_rescan:1;
+	bool			intr_unlinking:1;
+	bool			async_unlinking:1;
+	bool			shutdown:1;
+	struct ehci_qh		*qh_scan_next;
+
 	/* async schedule support */
 	struct ehci_qh		*async;
 	struct ehci_qh		*dummy;		/* For AMD quirk use */
-	struct ehci_qh		*reclaim;
-	struct ehci_qh		*qh_scan_next;
-	unsigned		scanning : 1;
+	struct ehci_qh		*async_unlink;
+	struct ehci_qh		*async_unlink_last;
+	struct ehci_qh		*async_iaa;
+	unsigned		async_unlink_cycle;
+	unsigned		async_count;	/* async activity count */
 
 	/* periodic schedule support */
 #define	DEFAULT_I_TDPS		1024		/* some HCs can do less */
 	unsigned		periodic_size;
 	__hc32			*periodic;	/* hw periodic table */
 	dma_addr_t		periodic_dma;
+	struct list_head	intr_qh_list;
 	unsigned		i_thresh;	/* uframes HC might cache */
 
 	union ehci_shadow	*pshadow;	/* mirror hw periodic table */
-	int			next_uframe;	/* scan periodic, start here */
-	unsigned		periodic_sched;	/* periodic activity count */
+	struct ehci_qh		*intr_unlink;
+	struct ehci_qh		*intr_unlink_last;
+	unsigned		intr_unlink_cycle;
+	unsigned		now_frame;	/* frame from HC hardware */
+	unsigned		next_frame;	/* scan periodic, start here */
+	unsigned		intr_count;	/* intr activity count */
+	unsigned		isoc_count;	/* isoc activity count */
+	unsigned		periodic_count;	/* periodic activity count */
 	unsigned		uframe_periodic_max; /* max periodic time per uframe */
 
 
-	/* list of itds & sitds completed while clock_frame was still active */
+	/* list of itds & sitds completed while now_frame was still active */
 	struct list_head	cached_itd_list;
+	struct ehci_itd		*last_itd_to_free;
 	struct list_head	cached_sitd_list;
-	unsigned		clock_frame;
+	struct ehci_sitd	*last_sitd_to_free;
 
 	/* per root hub port */
 	unsigned long		reset_done [EHCI_MAX_ROOT_PORTS];
@@ -126,10 +179,6 @@ struct ehci_hcd {			/* one per controller */
 	struct dma_pool		*itd_pool;	/* itd per iso urb */
 	struct dma_pool		*sitd_pool;	/* sitd per split iso urb */
 
-	struct timer_list	iaa_watchdog;
-	struct timer_list	watchdog;
-	unsigned long		actions;
-	unsigned		periodic_stamp;
 	unsigned		random_frame;
 	unsigned long		next_statechange;
 	ktime_t			last_periodic_enable;
@@ -143,7 +192,6 @@ struct ehci_hcd {			/* one per controller */
 	unsigned		big_endian_capbase:1;
 	unsigned		has_amcc_usb23:1;
 	unsigned		need_io_watchdog:1;
-	unsigned		broken_periodic:1;
 	unsigned		amd_pll_fix:1;
 	unsigned		fs_i_thresh:1;	/* Intel iso scheduling */
 	unsigned		use_dummy_qh:1;	/* AMD Frame List table quirk*/
@@ -175,10 +223,6 @@ struct ehci_hcd {			/* one per controller */
 #ifdef DEBUG
 	struct dentry		*debug_dir;
 #endif
-	/*
-	 * OTG controllers and transceivers need software interaction
-	 */
-	struct usb_phy	*transceiver;
 };
 
 /* convert between an HCD pointer and the corresponding EHCI_HCD */
@@ -191,34 +235,6 @@ static inline struct usb_hcd *ehci_to_hcd (struct ehci_hcd *ehci)
 	return container_of ((void *) ehci, struct usb_hcd, hcd_priv);
 }
 
-
-static inline void
-iaa_watchdog_start(struct ehci_hcd *ehci)
-{
-	WARN_ON(timer_pending(&ehci->iaa_watchdog));
-	mod_timer(&ehci->iaa_watchdog,
-			jiffies + msecs_to_jiffies(EHCI_IAA_MSECS));
-}
-
-static inline void iaa_watchdog_done(struct ehci_hcd *ehci)
-{
-	del_timer(&ehci->iaa_watchdog);
-}
-
-enum ehci_timer_action {
-	TIMER_IO_WATCHDOG,
-	TIMER_ASYNC_SHRINK,
-	TIMER_ASYNC_OFF,
-};
-
-static inline void
-timer_action_done (struct ehci_hcd *ehci, enum ehci_timer_action action)
-{
-	clear_bit (action, &ehci->actions);
-}
-
-static void free_cached_lists(struct ehci_hcd *ehci);
-
 /*-------------------------------------------------------------------------*/
 
 #include <linux/usb/ehci_def.h>
@@ -328,7 +344,13 @@ union ehci_shadow {
 struct ehci_qh_hw {
 	__hc32			hw_next;	/* see EHCI 3.6.1 */
 	__hc32			hw_info1;       /* see EHCI 3.6.2 */
-#define	QH_HEAD		0x00008000
+#define	QH_CONTROL_EP	(1 << 27)	/* FS/LS control endpoint */
+#define	QH_HEAD		(1 << 15)	/* Head of async reclamation list */
+#define	QH_TOGGLE_CTL	(1 << 14)	/* Data toggle control */
+#define	QH_HIGH_SPEED	(2 << 12)	/* Endpoint speed */
+#define	QH_LOW_SPEED	(1 << 12)
+#define	QH_FULL_SPEED	(0 << 12)
+#define	QH_INACTIVATE	(1 << 7)	/* Inactivate on next transaction */
 	__hc32			hw_info2;        /* see EHCI 3.6.2 */
 #define	QH_SMASK	0x000000ff
 #define	QH_CMASK	0x0000ff00
@@ -346,32 +368,23 @@ struct ehci_qh_hw {
 } __attribute__ ((aligned(32)));
 
 struct ehci_qh {
-	struct ehci_qh_hw	*hw;
+	struct ehci_qh_hw	*hw;		/* Must come first */
 	/* the rest is HCD-private */
 	dma_addr_t		qh_dma;		/* address of qh */
 	union ehci_shadow	qh_next;	/* ptr to qh; or periodic */
 	struct list_head	qtd_list;	/* sw qtd list */
+	struct list_head	intr_node;	/* list of intr QHs */
 	struct ehci_qtd		*dummy;
-	struct ehci_qh		*reclaim;	/* next to reclaim */
-
-	struct ehci_hcd		*ehci;
-	unsigned long		unlink_time;
+	struct ehci_qh		*unlink_next;	/* next on unlink list */
 
-	/*
-	 * Do NOT use atomic operations for QH refcounting. On some CPUs
-	 * (PPC7448 for example), atomic operations cannot be performed on
-	 * memory that is cache-inhibited (i.e. being used for DMA).
-	 * Spinlocks are used to protect all QH fields.
-	 */
-	u32			refcount;
-	unsigned		stamp;
+	unsigned		unlink_cycle;
 
 	u8			needs_rescan;	/* Dequeue during giveback */
 	u8			qh_state;
 #define	QH_STATE_LINKED		1		/* HC sees this */
 #define	QH_STATE_UNLINK		2		/* HC may still see this */
 #define	QH_STATE_IDLE		3		/* HC doesn't see this */
-#define	QH_STATE_UNLINK_WAIT	4		/* LINKED and on reclaim q */
+#define	QH_STATE_UNLINK_WAIT	4		/* LINKED and on unlink q */
 #define	QH_STATE_COMPLETING	5		/* don't touch token.HALT */
 
 	u8			xacterrs;	/* XactErr retry counter */
@@ -421,7 +434,6 @@ struct ehci_iso_stream {
 	/* first field matches ehci_hq, but is NULL */
 	struct ehci_qh_hw	*hw;
 
-	u32			refcount;
 	u8			bEndpointAddress;
 	u8			highspeed;
 	struct list_head	td_list;	/* queued itds/sitds */
diff --git a/drivers/usb/host/fhci-dbg.c b/drivers/usb/host/fhci-dbg.c
index 6fe55004911..f238cb37305 100644
--- a/drivers/usb/host/fhci-dbg.c
+++ b/drivers/usb/host/fhci-dbg.c
@@ -41,7 +41,7 @@ void fhci_dbg_isr(struct fhci_hcd *fhci, int usb_er)
 static int fhci_dfs_regs_show(struct seq_file *s, void *v)
 {
 	struct fhci_hcd *fhci = s->private;
-	struct fhci_regs __iomem *regs = fhci->regs;
+	struct qe_usb_ctlr __iomem *regs = fhci->regs;
 
 	seq_printf(s,
 		"mode: 0x%x\n" "addr: 0x%x\n"
@@ -50,11 +50,11 @@ static int fhci_dfs_regs_show(struct seq_file *s, void *v)
 		"status: 0x%x\n" "SOF timer: %d\n"
 		"frame number: %d\n"
 		"lines status: 0x%x\n",
-		in_8(&regs->usb_mod), in_8(&regs->usb_addr),
-		in_8(&regs->usb_comm), in_be16(&regs->usb_ep[0]),
-		in_be16(&regs->usb_event), in_be16(&regs->usb_mask),
-		in_8(&regs->usb_status), in_be16(&regs->usb_sof_tmr),
-		in_be16(&regs->usb_frame_num),
+		in_8(&regs->usb_usmod), in_8(&regs->usb_usadr),
+		in_8(&regs->usb_uscom), in_be16(&regs->usb_usep[0]),
+		in_be16(&regs->usb_usber), in_be16(&regs->usb_usbmr),
+		in_8(&regs->usb_usbs), in_be16(&regs->usb_ussft),
+		in_be16(&regs->usb_usfrn),
 		fhci_ioports_check_bus_state(fhci));
 
 	return 0;
diff --git a/drivers/usb/host/fhci-hcd.c b/drivers/usb/host/fhci-hcd.c
index d2623747b48..7da1a26bed2 100644
--- a/drivers/usb/host/fhci-hcd.c
+++ b/drivers/usb/host/fhci-hcd.c
@@ -40,8 +40,8 @@ void fhci_start_sof_timer(struct fhci_hcd *fhci)
 	/* clear frame_n */
 	out_be16(&fhci->pram->frame_num, 0);
 
-	out_be16(&fhci->regs->usb_sof_tmr, 0);
-	setbits8(&fhci->regs->usb_mod, USB_MODE_SFTE);
+	out_be16(&fhci->regs->usb_ussft, 0);
+	setbits8(&fhci->regs->usb_usmod, USB_MODE_SFTE);
 
 	fhci_dbg(fhci, "<- %s\n", __func__);
 }
@@ -50,7 +50,7 @@ void fhci_stop_sof_timer(struct fhci_hcd *fhci)
 {
 	fhci_dbg(fhci, "-> %s\n", __func__);
 
-	clrbits8(&fhci->regs->usb_mod, USB_MODE_SFTE);
+	clrbits8(&fhci->regs->usb_usmod, USB_MODE_SFTE);
 	gtm_stop_timer16(fhci->timer);
 
 	fhci_dbg(fhci, "<- %s\n", __func__);
@@ -58,7 +58,7 @@ void fhci_stop_sof_timer(struct fhci_hcd *fhci)
 
 u16 fhci_get_sof_timer_count(struct fhci_usb *usb)
 {
-	return be16_to_cpu(in_be16(&usb->fhci->regs->usb_sof_tmr) / 12);
+	return be16_to_cpu(in_be16(&usb->fhci->regs->usb_ussft) / 12);
 }
 
 /* initialize the endpoint zero */
@@ -88,8 +88,8 @@ void fhci_usb_enable_interrupt(struct fhci_usb *usb)
 		enable_irq(fhci_to_hcd(fhci)->irq);
 
 		/* initialize the event register and mask register */
-		out_be16(&usb->fhci->regs->usb_event, 0xffff);
-		out_be16(&usb->fhci->regs->usb_mask, usb->saved_msk);
+		out_be16(&usb->fhci->regs->usb_usber, 0xffff);
+		out_be16(&usb->fhci->regs->usb_usbmr, usb->saved_msk);
 
 		/* enable the timer interrupts */
 		enable_irq(fhci->timer->irq);
@@ -109,7 +109,7 @@ void fhci_usb_disable_interrupt(struct fhci_usb *usb)
 
 		/* disable the usb interrupt */
 		disable_irq_nosync(fhci_to_hcd(fhci)->irq);
-		out_be16(&usb->fhci->regs->usb_mask, 0);
+		out_be16(&usb->fhci->regs->usb_usbmr, 0);
 	}
 	usb->intr_nesting_cnt++;
 }
@@ -119,9 +119,9 @@ static u32 fhci_usb_enable(struct fhci_hcd *fhci)
 {
 	struct fhci_usb *usb = fhci->usb_lld;
 
-	out_be16(&usb->fhci->regs->usb_event, 0xffff);
-	out_be16(&usb->fhci->regs->usb_mask, usb->saved_msk);
-	setbits8(&usb->fhci->regs->usb_mod, USB_MODE_EN);
+	out_be16(&usb->fhci->regs->usb_usber, 0xffff);
+	out_be16(&usb->fhci->regs->usb_usbmr, usb->saved_msk);
+	setbits8(&usb->fhci->regs->usb_usmod, USB_MODE_EN);
 
 	mdelay(100);
 
@@ -141,7 +141,7 @@ static u32 fhci_usb_disable(struct fhci_hcd *fhci)
 			usb->port_status == FHCI_PORT_LOW)
 		fhci_device_disconnected_interrupt(fhci);
 
-	clrbits8(&usb->fhci->regs->usb_mod, USB_MODE_EN);
+	clrbits8(&usb->fhci->regs->usb_usmod, USB_MODE_EN);
 
 	return 0;
 }
@@ -285,13 +285,13 @@ static int fhci_usb_init(struct fhci_hcd *fhci)
 			  USB_E_IDLE_MASK |
 			  USB_E_RESET_MASK | USB_E_SFT_MASK | USB_E_MSF_MASK);
 
-	out_8(&usb->fhci->regs->usb_mod, USB_MODE_HOST | USB_MODE_EN);
+	out_8(&usb->fhci->regs->usb_usmod, USB_MODE_HOST | USB_MODE_EN);
 
 	/* clearing the mask register */
-	out_be16(&usb->fhci->regs->usb_mask, 0);
+	out_be16(&usb->fhci->regs->usb_usbmr, 0);
 
 	/* initialing the event register */
-	out_be16(&usb->fhci->regs->usb_event, 0xffff);
+	out_be16(&usb->fhci->regs->usb_usber, 0xffff);
 
 	if (endpoint_zero_init(usb, DEFAULT_DATA_MEM, DEFAULT_RING_LEN) != 0) {
 		fhci_usb_free(usb);
@@ -745,8 +745,8 @@ static int __devinit of_fhci_probe(struct platform_device *ofdev)
 	}
 
 	/* Clear and disable any pending interrupts. */
-	out_be16(&fhci->regs->usb_event, 0xffff);
-	out_be16(&fhci->regs->usb_mask, 0);
+	out_be16(&fhci->regs->usb_usber, 0xffff);
+	out_be16(&fhci->regs->usb_usbmr, 0);
 
 	ret = usb_add_hcd(hcd, usb_irq, 0);
 	if (ret < 0)
diff --git a/drivers/usb/host/fhci-hub.c b/drivers/usb/host/fhci-hub.c
index 348fe62e94f..6af2512f837 100644
--- a/drivers/usb/host/fhci-hub.c
+++ b/drivers/usb/host/fhci-hub.c
@@ -97,7 +97,7 @@ void fhci_port_disable(struct fhci_hcd *fhci)
 
 	/* Enable IDLE since we want to know if something comes along */
 	usb->saved_msk |= USB_E_IDLE_MASK;
-	out_be16(&usb->fhci->regs->usb_mask, usb->saved_msk);
+	out_be16(&usb->fhci->regs->usb_usbmr, usb->saved_msk);
 
 	/* check if during the disconnection process attached new device */
 	if (port_status == FHCI_PORT_WAITING)
@@ -158,21 +158,21 @@ void fhci_port_reset(void *lld)
 
 	fhci_stop_sof_timer(fhci);
 	/* disable the USB controller */
-	mode = in_8(&fhci->regs->usb_mod);
-	out_8(&fhci->regs->usb_mod, mode & (~USB_MODE_EN));
+	mode = in_8(&fhci->regs->usb_usmod);
+	out_8(&fhci->regs->usb_usmod, mode & (~USB_MODE_EN));
 
 	/* disable idle interrupts */
-	mask = in_be16(&fhci->regs->usb_mask);
-	out_be16(&fhci->regs->usb_mask, mask & (~USB_E_IDLE_MASK));
+	mask = in_be16(&fhci->regs->usb_usbmr);
+	out_be16(&fhci->regs->usb_usbmr, mask & (~USB_E_IDLE_MASK));
 
 	fhci_io_port_generate_reset(fhci);
 
 	/* enable interrupt on this endpoint */
-	out_be16(&fhci->regs->usb_mask, mask);
+	out_be16(&fhci->regs->usb_usbmr, mask);
 
 	/* enable the USB controller */
-	mode = in_8(&fhci->regs->usb_mod);
-	out_8(&fhci->regs->usb_mod, mode | USB_MODE_EN);
+	mode = in_8(&fhci->regs->usb_usmod);
+	out_8(&fhci->regs->usb_usmod, mode | USB_MODE_EN);
 	fhci_start_sof_timer(fhci);
 
 	fhci_dbg(fhci, "<- %s\n", __func__);
diff --git a/drivers/usb/host/fhci-sched.c b/drivers/usb/host/fhci-sched.c
index 2df851b4bc7..2dc8a40e39d 100644
--- a/drivers/usb/host/fhci-sched.c
+++ b/drivers/usb/host/fhci-sched.c
@@ -132,8 +132,8 @@ void fhci_flush_all_transmissions(struct fhci_usb *usb)
 	u8 mode;
 	struct td *td;
 
-	mode = in_8(&usb->fhci->regs->usb_mod);
-	clrbits8(&usb->fhci->regs->usb_mod, USB_MODE_EN);
+	mode = in_8(&usb->fhci->regs->usb_usmod);
+	clrbits8(&usb->fhci->regs->usb_usmod, USB_MODE_EN);
 
 	fhci_flush_bds(usb);
 
@@ -147,9 +147,9 @@ void fhci_flush_all_transmissions(struct fhci_usb *usb)
 	usb->actual_frame->frame_status = FRAME_END_TRANSMISSION;
 
 	/* reset the event register */
-	out_be16(&usb->fhci->regs->usb_event, 0xffff);
+	out_be16(&usb->fhci->regs->usb_usber, 0xffff);
 	/* enable the USB controller */
-	out_8(&usb->fhci->regs->usb_mod, mode | USB_MODE_EN);
+	out_8(&usb->fhci->regs->usb_usmod, mode | USB_MODE_EN);
 }
 
 /*
@@ -414,7 +414,7 @@ static void sof_interrupt(struct fhci_hcd *fhci)
 			usb->port_status = FHCI_PORT_FULL;
 		/* Disable IDLE */
 		usb->saved_msk &= ~USB_E_IDLE_MASK;
-		out_be16(&usb->fhci->regs->usb_mask, usb->saved_msk);
+		out_be16(&usb->fhci->regs->usb_usbmr, usb->saved_msk);
 	}
 
 	gtm_set_exact_timer16(fhci->timer, usb->max_frame_usage, false);
@@ -433,14 +433,14 @@ void fhci_device_disconnected_interrupt(struct fhci_hcd *fhci)
 	fhci_dbg(fhci, "-> %s\n", __func__);
 
 	fhci_usb_disable_interrupt(usb);
-	clrbits8(&usb->fhci->regs->usb_mod, USB_MODE_LSS);
+	clrbits8(&usb->fhci->regs->usb_usmod, USB_MODE_LSS);
 	usb->port_status = FHCI_PORT_DISABLED;
 
 	fhci_stop_sof_timer(fhci);
 
 	/* Enable IDLE since we want to know if something comes along */
 	usb->saved_msk |= USB_E_IDLE_MASK;
-	out_be16(&usb->fhci->regs->usb_mask, usb->saved_msk);
+	out_be16(&usb->fhci->regs->usb_usbmr, usb->saved_msk);
 
 	usb->vroot_hub->port.wPortStatus &= ~USB_PORT_STAT_CONNECTION;
 	usb->vroot_hub->port.wPortChange |= USB_PORT_STAT_C_CONNECTION;
@@ -473,7 +473,7 @@ void fhci_device_connected_interrupt(struct fhci_hcd *fhci)
 		}
 
 		usb->port_status = FHCI_PORT_LOW;
-		setbits8(&usb->fhci->regs->usb_mod, USB_MODE_LSS);
+		setbits8(&usb->fhci->regs->usb_usmod, USB_MODE_LSS);
 		usb->vroot_hub->port.wPortStatus |=
 		    (USB_PORT_STAT_LOW_SPEED |
 		     USB_PORT_STAT_CONNECTION);
@@ -491,7 +491,7 @@ void fhci_device_connected_interrupt(struct fhci_hcd *fhci)
 		}
 
 		usb->port_status = FHCI_PORT_FULL;
-		clrbits8(&usb->fhci->regs->usb_mod, USB_MODE_LSS);
+		clrbits8(&usb->fhci->regs->usb_usmod, USB_MODE_LSS);
 		usb->vroot_hub->port.wPortStatus &=
 		    ~USB_PORT_STAT_LOW_SPEED;
 		usb->vroot_hub->port.wPortStatus |=
@@ -535,7 +535,7 @@ static void abort_transmission(struct fhci_usb *usb)
 	/* issue stop Tx command */
 	qe_issue_cmd(QE_USB_STOP_TX, QE_CR_SUBBLOCK_USB, EP_ZERO, 0);
 	/* flush Tx FIFOs */
-	out_8(&usb->fhci->regs->usb_comm, USB_CMD_FLUSH_FIFO | EP_ZERO);
+	out_8(&usb->fhci->regs->usb_uscom, USB_CMD_FLUSH_FIFO | EP_ZERO);
 	udelay(1000);
 	/* reset Tx BDs */
 	fhci_flush_bds(usb);
@@ -555,11 +555,11 @@ irqreturn_t fhci_irq(struct usb_hcd *hcd)
 
 	usb = fhci->usb_lld;
 
-	usb_er |= in_be16(&usb->fhci->regs->usb_event) &
-		  in_be16(&usb->fhci->regs->usb_mask);
+	usb_er |= in_be16(&usb->fhci->regs->usb_usber) &
+		  in_be16(&usb->fhci->regs->usb_usbmr);
 
 	/* clear event bits for next time */
-	out_be16(&usb->fhci->regs->usb_event, usb_er);
+	out_be16(&usb->fhci->regs->usb_usber, usb_er);
 
 	fhci_dbg_isr(fhci, usb_er);
 
@@ -573,7 +573,7 @@ irqreturn_t fhci_irq(struct usb_hcd *hcd)
 
 			/* Turn on IDLE since we want to disconnect */
 			usb->saved_msk |= USB_E_IDLE_MASK;
-			out_be16(&usb->fhci->regs->usb_event,
+			out_be16(&usb->fhci->regs->usb_usber,
 				 usb->saved_msk);
 		} else if (usb->port_status == FHCI_PORT_DISABLED) {
 			if (fhci_ioports_check_bus_state(fhci) == 1)
@@ -611,7 +611,7 @@ irqreturn_t fhci_irq(struct usb_hcd *hcd)
 			/* XXX usb->port_status = FHCI_PORT_WAITING; */
 			/* Disable IDLE */
 			usb->saved_msk &= ~USB_E_IDLE_MASK;
-			out_be16(&usb->fhci->regs->usb_mask,
+			out_be16(&usb->fhci->regs->usb_usbmr,
 				 usb->saved_msk);
 		} else {
 			fhci_dbg_isr(fhci, -1);
diff --git a/drivers/usb/host/fhci-tds.c b/drivers/usb/host/fhci-tds.c
index c5ed8819929..1498061f0ae 100644
--- a/drivers/usb/host/fhci-tds.c
+++ b/drivers/usb/host/fhci-tds.c
@@ -249,7 +249,7 @@ void fhci_init_ep_registers(struct fhci_usb *usb, struct endpoint *ep,
 	u8 rt;
 
 	/* set the endpoint registers according to the endpoint */
-	out_be16(&usb->fhci->regs->usb_ep[0],
+	out_be16(&usb->fhci->regs->usb_usep[0],
 		 USB_TRANS_CTR | USB_EP_MF | USB_EP_RTE);
 	out_be16(&usb->fhci->pram->ep_ptr[0],
 		 cpm_muram_offset(ep->ep_pram_ptr));
@@ -463,7 +463,7 @@ u32 fhci_host_transaction(struct fhci_usb *usb,
 	cq_put(&ep->conf_frame_Q, pkt);
 
 	if (cq_howmany(&ep->conf_frame_Q) == 1)
-		out_8(&usb->fhci->regs->usb_comm, USB_CMD_STR_FIFO);
+		out_8(&usb->fhci->regs->usb_uscom, USB_CMD_STR_FIFO);
 
 	return 0;
 }
@@ -535,8 +535,8 @@ void fhci_flush_actual_frame(struct fhci_usb *usb)
 	struct endpoint *ep = usb->ep0;
 
 	/* disable the USB controller */
-	mode = in_8(&usb->fhci->regs->usb_mod);
-	out_8(&usb->fhci->regs->usb_mod, mode & ~USB_MODE_EN);
+	mode = in_8(&usb->fhci->regs->usb_usmod);
+	out_8(&usb->fhci->regs->usb_usmod, mode & ~USB_MODE_EN);
 
 	tb_ptr = in_be16(&ep->ep_pram_ptr->tx_bd_ptr);
 	td = cpm_muram_addr(tb_ptr);
@@ -571,9 +571,9 @@ void fhci_flush_actual_frame(struct fhci_usb *usb)
 	usb->actual_frame->frame_status = FRAME_TIMER_END_TRANSMISSION;
 
 	/* reset the event register */
-	out_be16(&usb->fhci->regs->usb_event, 0xffff);
+	out_be16(&usb->fhci->regs->usb_usber, 0xffff);
 	/* enable the USB controller */
-	out_8(&usb->fhci->regs->usb_mod, mode | USB_MODE_EN);
+	out_8(&usb->fhci->regs->usb_usmod, mode | USB_MODE_EN);
 }
 
 /* handles Tx confirm and Tx error interrupt */
@@ -613,7 +613,7 @@ void fhci_host_transmit_actual_frame(struct fhci_usb *usb)
 
 		/* start transmit only if we have something in the TDs */
 		if (in_be16(&td->status) & TD_R)
-			out_8(&usb->fhci->regs->usb_comm, USB_CMD_STR_FIFO);
+			out_8(&usb->fhci->regs->usb_uscom, USB_CMD_STR_FIFO);
 
 		if (in_be32(&ep->conf_td->buf_ptr) == DUMMY_BD_BUFFER) {
 			out_be32(&old_td->buf_ptr, 0);
diff --git a/drivers/usb/host/fhci.h b/drivers/usb/host/fhci.h
index dc6939a44a1..7cc1c32dc36 100644
--- a/drivers/usb/host/fhci.h
+++ b/drivers/usb/host/fhci.h
@@ -28,6 +28,7 @@
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
 #include <asm/qe.h>
+#include <asm/immap_qe.h>
 
 #define USB_CLOCK	48000000
 
@@ -173,25 +174,6 @@
 #define USB_E_TXB_MASK		0x0002
 #define USB_E_RXB_MASK		0x0001
 
-/* Freescale USB Host controller registers */
-struct fhci_regs {
-	u8 usb_mod;		/* mode register */
-	u8 usb_addr;		/* address register */
-	u8 usb_comm;		/* command register */
-	u8 reserved1[1];
-	__be16 usb_ep[4];	/* endpoint register */
-	u8 reserved2[4];
-	__be16 usb_event;	/* event register */
-	u8 reserved3[2];
-	__be16 usb_mask;	/* mask register */
-	u8 reserved4[1];
-	u8 usb_status;		/* status register */
-	__be16 usb_sof_tmr;	/* Start Of Frame timer */
-	u8 reserved5[2];
-	__be16 usb_frame_num;	/* frame number register */
-	u8 reserved6[1];
-};
-
 /* Freescale USB HOST */
 struct fhci_pram {
 	__be16 ep_ptr[4];	/* Endpoint porter reg */
@@ -267,7 +249,7 @@ struct fhci_hcd {
 	int gpios[NUM_GPIOS];
 	bool alow_gpios[NUM_GPIOS];
 
-	struct fhci_regs __iomem *regs;	/* I/O memory used to communicate */
+	struct qe_usb_ctlr __iomem *regs; /* I/O memory used to communicate */
 	struct fhci_pram __iomem *pram;	/* Parameter RAM */
 	struct gtm_timer *timer;
 
diff --git a/drivers/usb/host/imx21-hcd.c b/drivers/usb/host/imx21-hcd.c
index ff471c1c165..f19e2690c23 100644
--- a/drivers/usb/host/imx21-hcd.c
+++ b/drivers/usb/host/imx21-hcd.c
@@ -1811,7 +1811,7 @@ static int imx21_remove(struct platform_device *pdev)
 	usb_remove_hcd(hcd);
 
 	if (res != NULL) {
-		clk_disable(imx21->clk);
+		clk_disable_unprepare(imx21->clk);
 		clk_put(imx21->clk);
 		iounmap(imx21->regs);
 		release_mem_region(res->start, resource_size(res));
@@ -1884,7 +1884,7 @@ static int imx21_probe(struct platform_device *pdev)
 	ret = clk_set_rate(imx21->clk, clk_round_rate(imx21->clk, 48000000));
 	if (ret)
 		goto failed_clock_set;
-	ret = clk_enable(imx21->clk);
+	ret = clk_prepare_enable(imx21->clk);
 	if (ret)
 		goto failed_clock_enable;
 
@@ -1900,7 +1900,7 @@ static int imx21_probe(struct platform_device *pdev)
 	return 0;
 
 failed_add_hcd:
-	clk_disable(imx21->clk);
+	clk_disable_unprepare(imx21->clk);
 failed_clock_enable:
 failed_clock_set:
 	clk_put(imx21->clk);
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index 2909621ea19..fc3091bd237 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -12,6 +12,7 @@
  */
 
 #include <linux/clk.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <mach/ohci.h>
 #include <plat/usb-phy.h>
@@ -71,6 +72,8 @@ static const struct hc_driver exynos_ohci_hc_driver = {
 	.start_port_reset	= ohci_start_port_reset,
 };
 
+static u64 ohci_exynos_dma_mask = DMA_BIT_MASK(32);
+
 static int __devinit exynos_ohci_probe(struct platform_device *pdev)
 {
 	struct exynos4_ohci_platdata *pdata;
@@ -87,7 +90,18 @@ static int __devinit exynos_ohci_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	exynos_ohci = kzalloc(sizeof(struct exynos_ohci_hcd), GFP_KERNEL);
+	/*
+	 * Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we move to full device tree support this will vanish off.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &ohci_exynos_dma_mask;
+	if (!pdev->dev.coherent_dma_mask)
+		pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+
+	exynos_ohci = devm_kzalloc(&pdev->dev, sizeof(struct exynos_ohci_hcd),
+					GFP_KERNEL);
 	if (!exynos_ohci)
 		return -ENOMEM;
 
@@ -97,8 +111,7 @@ static int __devinit exynos_ohci_probe(struct platform_device *pdev)
 					dev_name(&pdev->dev));
 	if (!hcd) {
 		dev_err(&pdev->dev, "Unable to create HCD\n");
-		err = -ENOMEM;
-		goto fail_hcd;
+		return -ENOMEM;
 	}
 
 	exynos_ohci->hcd = hcd;
@@ -123,7 +136,7 @@ static int __devinit exynos_ohci_probe(struct platform_device *pdev)
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = ioremap(res->start, resource_size(res));
+	hcd->regs = devm_ioremap(&pdev->dev, res->start, hcd->rsrc_len);
 	if (!hcd->regs) {
 		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
 		err = -ENOMEM;
@@ -134,7 +147,7 @@ static int __devinit exynos_ohci_probe(struct platform_device *pdev)
 	if (!irq) {
 		dev_err(&pdev->dev, "Failed to get IRQ\n");
 		err = -ENODEV;
-		goto fail;
+		goto fail_io;
 	}
 
 	if (pdata->phy_init)
@@ -146,23 +159,19 @@ static int __devinit exynos_ohci_probe(struct platform_device *pdev)
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to add USB HCD\n");
-		goto fail;
+		goto fail_io;
 	}
 
 	platform_set_drvdata(pdev, exynos_ohci);
 
 	return 0;
 
-fail:
-	iounmap(hcd->regs);
 fail_io:
 	clk_disable(exynos_ohci->clk);
 fail_clken:
 	clk_put(exynos_ohci->clk);
 fail_clk:
 	usb_put_hcd(hcd);
-fail_hcd:
-	kfree(exynos_ohci);
 	return err;
 }
 
@@ -177,13 +186,10 @@ static int __devexit exynos_ohci_remove(struct platform_device *pdev)
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
 
-	iounmap(hcd->regs);
-
 	clk_disable(exynos_ohci->clk);
 	clk_put(exynos_ohci->clk);
 
 	usb_put_hcd(hcd);
-	kfree(exynos_ohci);
 
 	return 0;
 }
@@ -225,6 +231,9 @@ static int exynos_ohci_suspend(struct device *dev)
 
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
+
+	clk_disable(exynos_ohci->clk);
+
 fail:
 	spin_unlock_irqrestore(&ohci->lock, flags);
 
@@ -238,6 +247,8 @@ static int exynos_ohci_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct exynos4_ohci_platdata *pdata = pdev->dev.platform_data;
 
+	clk_enable(exynos_ohci->clk);
+
 	if (pdata && pdata->phy_init)
 		pdata->phy_init(pdev, S5P_USB_PHY_HOST);
 
@@ -258,6 +269,14 @@ static const struct dev_pm_ops exynos_ohci_pm_ops = {
 	.resume		= exynos_ohci_resume,
 };
 
+#ifdef CONFIG_OF
+static const struct of_device_id exynos_ohci_match[] = {
+	{ .compatible = "samsung,exynos-ohci" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_ohci_match);
+#endif
+
 static struct platform_driver exynos_ohci_driver = {
 	.probe		= exynos_ohci_probe,
 	.remove		= __devexit_p(exynos_ohci_remove),
@@ -266,6 +285,7 @@ static struct platform_driver exynos_ohci_driver = {
 		.name	= "exynos-ohci",
 		.owner	= THIS_MODULE,
 		.pm	= &exynos_ohci_pm_ops,
+		.of_match_table	= of_match_ptr(exynos_ohci_match),
 	}
 };
 
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index e0adf5c0cf5..2b1e8d84c87 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1100,6 +1100,11 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ohci_octeon_driver
 #endif
 
+#ifdef CONFIG_TILE_USB
+#include "ohci-tilegx.c"
+#define PLATFORM_DRIVER		ohci_hcd_tilegx_driver
+#endif
+
 #ifdef CONFIG_USB_CNS3XXX_OHCI
 #include "ohci-cns3xxx.c"
 #define PLATFORM_DRIVER		ohci_hcd_cns3xxx_driver
diff --git a/drivers/usb/host/ohci-nxp.c b/drivers/usb/host/ohci-nxp.c
index 1e364ec962f..a446386bf77 100644
--- a/drivers/usb/host/ohci-nxp.c
+++ b/drivers/usb/host/ohci-nxp.c
@@ -43,16 +43,6 @@
 #define USB_HOST_NEED_CLK_EN	(1 << 21)
 #define PAD_CONTROL_LAST_DRIVEN	(1 << 19)
 
-#define USB_OTG_CLK_CTRL	IO_ADDRESS(USB_CONFIG_BASE + 0xFF4)
-#define USB_OTG_CLK_STAT	IO_ADDRESS(USB_CONFIG_BASE + 0xFF8)
-
-/* USB_OTG_CLK_CTRL bit defines */
-#define AHB_M_CLOCK_ON		(1 << 4)
-#define OTG_CLOCK_ON		(1 << 3)
-#define I2C_CLOCK_ON		(1 << 2)
-#define DEV_CLOCK_ON		(1 << 1)
-#define HOST_CLOCK_ON		(1 << 0)
-
 #define USB_OTG_STAT_CONTROL	IO_ADDRESS(USB_CONFIG_BASE + 0x110)
 
 /* USB_OTG_STAT_CONTROL bit defines */
@@ -72,7 +62,9 @@ static struct i2c_client *isp1301_i2c_client;
 
 extern int usb_disabled(void);
 
-static struct clk *usb_clk;
+static struct clk *usb_pll_clk;
+static struct clk *usb_dev_clk;
+static struct clk *usb_otg_clk;
 
 static void isp1301_configure_pnx4008(void)
 {
@@ -249,8 +241,6 @@ static const struct hc_driver ohci_nxp_hc_driver = {
 	.start_port_reset = ohci_start_port_reset,
 };
 
-#define USB_CLOCK_MASK (AHB_M_CLOCK_ON| OTG_CLOCK_ON | HOST_CLOCK_ON | I2C_CLOCK_ON)
-
 static void nxp_set_usb_bits(void)
 {
 	if (machine_is_pnx4008()) {
@@ -327,41 +317,63 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 	/* Enable AHB slave USB clock, needed for further USB clock control */
 	__raw_writel(USB_SLAVE_HCLK_EN | PAD_CONTROL_LAST_DRIVEN, USB_CTRL);
 
-	isp1301_configure();
-
 	/* Enable USB PLL */
-	usb_clk = clk_get(&pdev->dev, "ck_pll5");
-	if (IS_ERR(usb_clk)) {
+	usb_pll_clk = clk_get(&pdev->dev, "ck_pll5");
+	if (IS_ERR(usb_pll_clk)) {
 		dev_err(&pdev->dev, "failed to acquire USB PLL\n");
-		ret = PTR_ERR(usb_clk);
+		ret = PTR_ERR(usb_pll_clk);
 		goto out1;
 	}
 
-	ret = clk_enable(usb_clk);
+	ret = clk_enable(usb_pll_clk);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to start USB PLL\n");
 		goto out2;
 	}
 
-	ret = clk_set_rate(usb_clk, 48000);
+	ret = clk_set_rate(usb_pll_clk, 48000);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to set USB clock rate\n");
 		goto out3;
 	}
 
+	/* Enable USB device clock */
+	usb_dev_clk = clk_get(&pdev->dev, "ck_usbd");
+	if (IS_ERR(usb_dev_clk)) {
+		dev_err(&pdev->dev, "failed to acquire USB DEV Clock\n");
+		ret = PTR_ERR(usb_dev_clk);
+		goto out4;
+	}
+
+	ret = clk_enable(usb_dev_clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to start USB DEV Clock\n");
+		goto out5;
+	}
+
+	/* Enable USB otg clocks */
+	usb_otg_clk = clk_get(&pdev->dev, "ck_usb_otg");
+	if (IS_ERR(usb_otg_clk)) {
+		dev_err(&pdev->dev, "failed to acquire USB DEV Clock\n");
+		ret = PTR_ERR(usb_dev_clk);
+		goto out6;
+	}
+
 	__raw_writel(__raw_readl(USB_CTRL) | USB_HOST_NEED_CLK_EN, USB_CTRL);
 
-	/* Set to enable all needed USB clocks */
-	__raw_writel(USB_CLOCK_MASK, USB_OTG_CLK_CTRL);
+	ret = clk_enable(usb_otg_clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to start USB DEV Clock\n");
+		goto out7;
+	}
 
-	while ((__raw_readl(USB_OTG_CLK_STAT) & USB_CLOCK_MASK) !=
-	       USB_CLOCK_MASK) ;
+	isp1301_configure();
 
 	hcd = usb_create_hcd(driver, &pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
 		dev_err(&pdev->dev, "Failed to allocate HC buffer\n");
 		ret = -ENOMEM;
-		goto out3;
+		goto out8;
 	}
 
 	/* Set all USB bits in the Start Enable register */
@@ -371,14 +383,14 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 	if (!res) {
 		dev_err(&pdev->dev, "Failed to get MEM resource\n");
 		ret =  -ENOMEM;
-		goto out4;
+		goto out8;
 	}
 
 	hcd->regs = devm_request_and_ioremap(&pdev->dev, res);
 	if (!hcd->regs) {
 		dev_err(&pdev->dev, "Failed to devm_request_and_ioremap\n");
 		ret =  -ENOMEM;
-		goto out4;
+		goto out8;
 	}
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
@@ -386,7 +398,7 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		ret = -ENXIO;
-		goto out4;
+		goto out8;
 	}
 
 	nxp_start_hc();
@@ -400,13 +412,21 @@ static int __devinit usb_hcd_nxp_probe(struct platform_device *pdev)
 		return ret;
 
 	nxp_stop_hc();
-out4:
+out8:
 	nxp_unset_usb_bits();
 	usb_put_hcd(hcd);
+out7:
+	clk_disable(usb_otg_clk);
+out6:
+	clk_put(usb_otg_clk);
+out5:
+	clk_disable(usb_dev_clk);
+out4:
+	clk_put(usb_dev_clk);
 out3:
-	clk_disable(usb_clk);
+	clk_disable(usb_pll_clk);
 out2:
-	clk_put(usb_clk);
+	clk_put(usb_pll_clk);
 out1:
 	isp1301_i2c_client = NULL;
 out:
@@ -422,8 +442,10 @@ static int usb_hcd_nxp_remove(struct platform_device *pdev)
 	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 	usb_put_hcd(hcd);
 	nxp_unset_usb_bits();
-	clk_disable(usb_clk);
-	clk_put(usb_clk);
+	clk_disable(usb_pll_clk);
+	clk_put(usb_pll_clk);
+	clk_disable(usb_dev_clk);
+	clk_put(usb_dev_clk);
 	i2c_unregister_device(isp1301_i2c_client);
 	isp1301_i2c_client = NULL;
 
diff --git a/drivers/usb/host/ohci-omap.c b/drivers/usb/host/ohci-omap.c
index 9ce35d0d9d5..e7d75d29598 100644
--- a/drivers/usb/host/ohci-omap.c
+++ b/drivers/usb/host/ohci-omap.c
@@ -18,16 +18,18 @@
 #include <linux/jiffies.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/gpio.h>
 
-#include <mach/hardware.h>
 #include <asm/io.h>
 #include <asm/mach-types.h>
 
 #include <plat/mux.h>
-#include <mach/irqs.h>
 #include <plat/fpga.h>
-#include <plat/usb.h>
+
+#include <mach/hardware.h>
+#include <mach/irqs.h>
+#include <mach/usb.h>
 
 
 /* OMAP-1510 OHCI has its own MMU for DMA */
@@ -167,14 +169,15 @@ static int omap_1510_local_bus_init(void)
 
 static void start_hnp(struct ohci_hcd *ohci)
 {
-	const unsigned	port = ohci_to_hcd(ohci)->self.otg_port - 1;
+	struct usb_hcd *hcd = ohci_to_hcd(ohci);
+	const unsigned	port = hcd->self.otg_port - 1;
 	unsigned long	flags;
 	u32 l;
 
-	otg_start_hnp(ohci->transceiver->otg);
+	otg_start_hnp(hcd->phy->otg);
 
 	local_irq_save(flags);
-	ohci->transceiver->state = OTG_STATE_A_SUSPEND;
+	hcd->phy->state = OTG_STATE_A_SUSPEND;
 	writel (RH_PS_PSS, &ohci->regs->roothub.portstatus [port]);
 	l = omap_readl(OTG_CTRL);
 	l &= ~OTG_A_BUSREQ;
@@ -211,18 +214,18 @@ static int ohci_omap_init(struct usb_hcd *hcd)
 
 #ifdef	CONFIG_USB_OTG
 	if (need_transceiver) {
-		ohci->transceiver = usb_get_transceiver();
-		if (ohci->transceiver) {
-			int	status = otg_set_host(ohci->transceiver->otg,
+		hcd->phy = usb_get_phy(USB_PHY_TYPE_USB2);
+		if (!IS_ERR_OR_NULL(hcd->phy)) {
+			int	status = otg_set_host(hcd->phy->otg,
 						&ohci_to_hcd(ohci)->self);
-			dev_dbg(hcd->self.controller, "init %s transceiver, status %d\n",
-					ohci->transceiver->label, status);
+			dev_dbg(hcd->self.controller, "init %s phy, status %d\n",
+					hcd->phy->label, status);
 			if (status) {
-				usb_put_transceiver(ohci->transceiver);
+				usb_put_phy(hcd->phy);
 				return status;
 			}
 		} else {
-			dev_err(hcd->self.controller, "can't find transceiver\n");
+			dev_err(hcd->self.controller, "can't find phy\n");
 			return -ENODEV;
 		}
 		ohci->start_hnp = start_hnp;
@@ -403,9 +406,9 @@ usb_hcd_omap_remove (struct usb_hcd *hcd, struct platform_device *pdev)
 	struct ohci_hcd		*ohci = hcd_to_ohci (hcd);
 
 	usb_remove_hcd(hcd);
-	if (ohci->transceiver) {
-		(void) otg_set_host(ohci->transceiver->otg, 0);
-		usb_put_transceiver(ohci->transceiver);
+	if (!IS_ERR_OR_NULL(hcd->phy)) {
+		(void) otg_set_host(hcd->phy->otg, 0);
+		usb_put_phy(hcd->phy);
 	}
 	if (machine_is_omap_osk())
 		gpio_free(9);
diff --git a/drivers/usb/host/ohci-tilegx.c b/drivers/usb/host/ohci-tilegx.c
new file mode 100644
index 00000000000..1ae7b28a71c
--- /dev/null
+++ b/drivers/usb/host/ohci-tilegx.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright 2012 Tilera Corporation. All Rights Reserved.
+ *
+ *   This program is free software; you can redistribute it and/or
+ *   modify it under the terms of the GNU General Public License
+ *   as published by the Free Software Foundation, version 2.
+ *
+ *   This program is distributed in the hope that it will be useful, but
+ *   WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ *   NON INFRINGEMENT.  See the GNU General Public License for
+ *   more details.
+ */
+
+/*
+ * Tilera TILE-Gx USB OHCI host controller driver.
+ */
+
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/usb/tilegx.h>
+#include <linux/usb.h>
+
+#include <asm/homecache.h>
+
+#include <gxio/iorpc_usb_host.h>
+#include <gxio/usb_host.h>
+
+static void tilegx_start_ohc(void)
+{
+}
+
+static void tilegx_stop_ohc(void)
+{
+}
+
+static int tilegx_ohci_start(struct usb_hcd *hcd)
+{
+	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
+	int ret;
+
+	ret = ohci_init(ohci);
+	if (ret < 0)
+		return ret;
+
+	ret = ohci_run(ohci);
+	if (ret < 0) {
+		dev_err(hcd->self.controller, "can't start %s\n",
+			hcd->self.bus_name);
+		ohci_stop(hcd);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct hc_driver ohci_tilegx_hc_driver = {
+	.description		= hcd_name,
+	.product_desc		= "Tile-Gx OHCI",
+	.hcd_priv_size		= sizeof(struct ohci_hcd),
+
+	/*
+	 * Generic hardware linkage.
+	 */
+	.irq			= ohci_irq,
+	.flags			= HCD_MEMORY | HCD_LOCAL_MEM | HCD_USB11,
+
+	/*
+	 * Basic lifecycle operations.
+	 */
+	.start			= tilegx_ohci_start,
+	.stop			= ohci_stop,
+	.shutdown		= ohci_shutdown,
+
+	/*
+	 * Managing I/O requests and associated device resources.
+	 */
+	.urb_enqueue		= ohci_urb_enqueue,
+	.urb_dequeue		= ohci_urb_dequeue,
+	.endpoint_disable	= ohci_endpoint_disable,
+
+	/*
+	 * Scheduling support.
+	 */
+	.get_frame_number	= ohci_get_frame,
+
+	/*
+	 * Root hub support.
+	 */
+	.hub_status_data	= ohci_hub_status_data,
+	.hub_control		= ohci_hub_control,
+	.start_port_reset	= ohci_start_port_reset,
+};
+
+static int ohci_hcd_tilegx_drv_probe(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd;
+	struct tilegx_usb_platform_data *pdata = pdev->dev.platform_data;
+	pte_t pte = { 0 };
+	int my_cpu = smp_processor_id();
+	int ret;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	/*
+	 * Try to initialize our GXIO context; if we can't, the device
+	 * doesn't exist.
+	 */
+	if (gxio_usb_host_init(&pdata->usb_ctx, pdata->dev_index, 0) != 0)
+		return -ENXIO;
+
+	hcd = usb_create_hcd(&ohci_tilegx_hc_driver, &pdev->dev,
+			     dev_name(&pdev->dev));
+	if (!hcd)
+		return -ENOMEM;
+
+	/*
+	 * We don't use rsrc_start to map in our registers, but seems like
+	 * we ought to set it to something, so we use the register VA.
+	 */
+	hcd->rsrc_start =
+		(ulong) gxio_usb_host_get_reg_start(&pdata->usb_ctx);
+	hcd->rsrc_len = gxio_usb_host_get_reg_len(&pdata->usb_ctx);
+	hcd->regs = gxio_usb_host_get_reg_start(&pdata->usb_ctx);
+
+	tilegx_start_ohc();
+
+	/* Create our IRQs and register them. */
+	pdata->irq = create_irq();
+	if (pdata->irq < 0) {
+		ret = -ENXIO;
+		goto err_no_irq;
+	}
+
+	tile_irq_activate(pdata->irq, TILE_IRQ_PERCPU);
+
+	/* Configure interrupts. */
+	ret = gxio_usb_host_cfg_interrupt(&pdata->usb_ctx,
+					  cpu_x(my_cpu), cpu_y(my_cpu),
+					  KERNEL_PL, pdata->irq);
+	if (ret) {
+		ret = -ENXIO;
+		goto err_have_irq;
+	}
+
+	/* Register all of our memory. */
+	pte = pte_set_home(pte, PAGE_HOME_HASH);
+	ret = gxio_usb_host_register_client_memory(&pdata->usb_ctx, pte, 0);
+	if (ret) {
+		ret = -ENXIO;
+		goto err_have_irq;
+	}
+
+	ohci_hcd_init(hcd_to_ohci(hcd));
+
+	ret = usb_add_hcd(hcd, pdata->irq, IRQF_SHARED);
+	if (ret == 0) {
+		platform_set_drvdata(pdev, hcd);
+		return ret;
+	}
+
+err_have_irq:
+	destroy_irq(pdata->irq);
+err_no_irq:
+	tilegx_stop_ohc();
+	usb_put_hcd(hcd);
+	gxio_usb_host_destroy(&pdata->usb_ctx);
+	return ret;
+}
+
+static int ohci_hcd_tilegx_drv_remove(struct platform_device *pdev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct tilegx_usb_platform_data* pdata = pdev->dev.platform_data;
+
+	usb_remove_hcd(hcd);
+	usb_put_hcd(hcd);
+	tilegx_stop_ohc();
+	gxio_usb_host_destroy(&pdata->usb_ctx);
+	destroy_irq(pdata->irq);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static void ohci_hcd_tilegx_drv_shutdown(struct platform_device *pdev)
+{
+	usb_hcd_platform_shutdown(pdev);
+	ohci_hcd_tilegx_drv_remove(pdev);
+}
+
+static struct platform_driver ohci_hcd_tilegx_driver = {
+	.probe		= ohci_hcd_tilegx_drv_probe,
+	.remove		= ohci_hcd_tilegx_drv_remove,
+	.shutdown	= ohci_hcd_tilegx_drv_shutdown,
+	.driver = {
+		.name	= "tilegx-ohci",
+		.owner	= THIS_MODULE,
+	}
+};
+
+MODULE_ALIAS("platform:tilegx-ohci");
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 1b19aea25a2..d3299143d9e 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -372,11 +372,6 @@ struct ohci_hcd {
 	struct ed		*ed_controltail;	/* last in ctrl list */
 	struct ed		*periodic [NUM_INTS];	/* shadow int_table */
 
-	/*
-	 * OTG controllers and transceivers need software interaction;
-	 * other external transceivers should be software-transparent
-	 */
-	struct usb_phy	*transceiver;
 	void (*start_hnp)(struct ohci_hcd *ohci);
 
 	/*
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index c868be65e76..4c634eb5635 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -95,9 +95,7 @@ static int r8a66597_clock_enable(struct r8a66597 *r8a66597)
 	int i = 0;
 
 	if (r8a66597->pdata->on_chip) {
-#ifdef CONFIG_HAVE_CLK
 		clk_enable(r8a66597->clk);
-#endif
 		do {
 			r8a66597_write(r8a66597, SCKE, SYSCFG0);
 			tmp = r8a66597_read(r8a66597, SYSCFG0);
@@ -141,9 +139,7 @@ static void r8a66597_clock_disable(struct r8a66597 *r8a66597)
 	udelay(1);
 
 	if (r8a66597->pdata->on_chip) {
-#ifdef CONFIG_HAVE_CLK
 		clk_disable(r8a66597->clk);
-#endif
 	} else {
 		r8a66597_bclr(r8a66597, PLLC, SYSCFG0);
 		r8a66597_bclr(r8a66597, XCKE, SYSCFG0);
@@ -2406,19 +2402,15 @@ static int __devexit r8a66597_remove(struct platform_device *pdev)
 	del_timer_sync(&r8a66597->rh_timer);
 	usb_remove_hcd(hcd);
 	iounmap(r8a66597->reg);
-#ifdef CONFIG_HAVE_CLK
 	if (r8a66597->pdata->on_chip)
 		clk_put(r8a66597->clk);
-#endif
 	usb_put_hcd(hcd);
 	return 0;
 }
 
 static int __devinit r8a66597_probe(struct platform_device *pdev)
 {
-#ifdef CONFIG_HAVE_CLK
 	char clk_name[8];
-#endif
 	struct resource *res = NULL, *ires;
 	int irq = -1;
 	void __iomem *reg = NULL;
@@ -2482,7 +2474,6 @@ static int __devinit r8a66597_probe(struct platform_device *pdev)
 	r8a66597->irq_sense_low = irq_trigger == IRQF_TRIGGER_LOW;
 
 	if (r8a66597->pdata->on_chip) {
-#ifdef CONFIG_HAVE_CLK
 		snprintf(clk_name, sizeof(clk_name), "usb%d", pdev->id);
 		r8a66597->clk = clk_get(&pdev->dev, clk_name);
 		if (IS_ERR(r8a66597->clk)) {
@@ -2491,7 +2482,6 @@ static int __devinit r8a66597_probe(struct platform_device *pdev)
 			ret = PTR_ERR(r8a66597->clk);
 			goto clean_up2;
 		}
-#endif
 		r8a66597->max_root_hub = 1;
 	} else
 		r8a66597->max_root_hub = 2;
@@ -2531,11 +2521,9 @@ static int __devinit r8a66597_probe(struct platform_device *pdev)
 	return 0;
 
 clean_up3:
-#ifdef CONFIG_HAVE_CLK
 	if (r8a66597->pdata->on_chip)
 		clk_put(r8a66597->clk);
 clean_up2:
-#endif
 	usb_put_hcd(hcd);
 
 clean_up:
diff --git a/drivers/usb/host/r8a66597.h b/drivers/usb/host/r8a66597.h
index f28782d20ee..672cea307ab 100644
--- a/drivers/usb/host/r8a66597.h
+++ b/drivers/usb/host/r8a66597.h
@@ -26,10 +26,7 @@
 #ifndef __R8A66597_H__
 #define __R8A66597_H__
 
-#ifdef CONFIG_HAVE_CLK
 #include <linux/clk.h>
-#endif
-
 #include <linux/usb/r8a66597.h>
 
 #define R8A66597_MAX_NUM_PIPE		10
@@ -113,9 +110,7 @@ struct r8a66597_root_hub {
 struct r8a66597 {
 	spinlock_t lock;
 	void __iomem *reg;
-#ifdef CONFIG_HAVE_CLK
 	struct clk *clk;
-#endif
 	struct r8a66597_platdata	*pdata;
 	struct r8a66597_device		device0;
 	struct r8a66597_root_hub	root_hub[R8A66597_MAX_ROOT_HUB];
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 7b01094d799..74bfc868b7a 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -544,12 +544,18 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		if (hcd->speed != HCD_USB3)
 			goto error;
 
+		/* Set the U1 and U2 exit latencies. */
 		memcpy(buf, &usb_bos_descriptor,
 				USB_DT_BOS_SIZE + USB_DT_USB_SS_CAP_SIZE);
 		temp = xhci_readl(xhci, &xhci->cap_regs->hcs_params3);
 		buf[12] = HCS_U1_LATENCY(temp);
 		put_unaligned_le16(HCS_U2_LATENCY(temp), &buf[13]);
 
+		/* Indicate whether the host has LTM support. */
+		temp = xhci_readl(xhci, &xhci->cap_regs->hcc_params);
+		if (HCC_LTC(temp))
+			buf[8] |= USB_LTM_SUPPORT;
+
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		return USB_DT_BOS_SIZE + USB_DT_USB_SS_CAP_SIZE;
 	case GetPortStatus:
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index a979cd0dbe0..7648b2d4b26 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -4450,6 +4450,8 @@ int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks)
 
 	/* Accept arbitrarily long scatter-gather lists */
 	hcd->self.sg_tablesize = ~0;
+	/* XHCI controllers don't stop the ep queue on short packets :| */
+	hcd->self.no_stop_on_short = 1;
 
 	if (usb_hcd_is_primary_hcd(hcd)) {
 		xhci = kzalloc(sizeof(struct xhci_hcd), GFP_KERNEL);
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index 9f3eda91ea4..7a95ab87ac0 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -29,6 +29,7 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
@@ -364,8 +365,8 @@ static int am35x_musb_init(struct musb *musb)
 		return -ENODEV;
 
 	usb_nop_xceiv_register();
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv)
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -ENODEV;
 
 	if (is_host_enabled(musb))
@@ -406,7 +407,7 @@ static int am35x_musb_exit(struct musb *musb)
 	if (data->set_phy_power)
 		data->set_phy_power(0);
 
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 
 	return 0;
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
index a087ed6c3be..428e6aa3e78 100644
--- a/drivers/usb/musb/blackfin.c
+++ b/drivers/usb/musb/blackfin.c
@@ -15,6 +15,7 @@
 #include <linux/list.h>
 #include <linux/gpio.h>
 #include <linux/io.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/prefetch.h>
@@ -415,8 +416,8 @@ static int bfin_musb_init(struct musb *musb)
 	gpio_direction_output(musb->config->gpio_vrsel, 0);
 
 	usb_nop_xceiv_register();
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv) {
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		gpio_free(musb->config->gpio_vrsel);
 		return -ENODEV;
 	}
@@ -440,7 +441,7 @@ static int bfin_musb_exit(struct musb *musb)
 {
 	gpio_free(musb->config->gpio_vrsel);
 
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 	return 0;
 }
diff --git a/drivers/usb/musb/da8xx.c b/drivers/usb/musb/da8xx.c
index 8bd9566f3fb..0f9fcec4e1d 100644
--- a/drivers/usb/musb/da8xx.c
+++ b/drivers/usb/musb/da8xx.c
@@ -29,6 +29,7 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
@@ -425,8 +426,8 @@ static int da8xx_musb_init(struct musb *musb)
 		goto fail;
 
 	usb_nop_xceiv_register();
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv)
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv))
 		goto fail;
 
 	if (is_host_enabled(musb))
@@ -458,7 +459,7 @@ static int da8xx_musb_exit(struct musb *musb)
 
 	phy_off();
 
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 
 	return 0;
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index 9d63ba4d10d..472c8b42d38 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -28,6 +28,7 @@
 #include <linux/list.h>
 #include <linux/delay.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
@@ -385,8 +386,8 @@ static int davinci_musb_init(struct musb *musb)
 	u32		revision;
 
 	usb_nop_xceiv_register();
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv)
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv))
 		goto unregister;
 
 	musb->mregs += DAVINCI_BASE_OFFSET;
@@ -444,7 +445,7 @@ static int davinci_musb_init(struct musb *musb)
 	return 0;
 
 fail:
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 unregister:
 	usb_nop_xceiv_unregister();
 	return -ENODEV;
@@ -494,7 +495,7 @@ static int davinci_musb_exit(struct musb *musb)
 
 	phy_off();
 
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 
 	return 0;
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index db3dff854b7..26f1befb489 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1909,7 +1909,7 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	/* The musb_platform_init() call:
 	 *   - adjusts musb->mregs and musb->isr if needed,
 	 *   - may initialize an integrated tranceiver
-	 *   - initializes musb->xceiv, usually by otg_get_transceiver()
+	 *   - initializes musb->xceiv, usually by otg_get_phy()
 	 *   - stops powering VBUS
 	 *
 	 * There are various transceiver configurations.  Blackfin,
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index f4a40f001c8..586105b55a7 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -81,14 +81,6 @@ struct musb_ep;
 #define is_peripheral_active(m)		(!(m)->is_host)
 #define is_host_active(m)		((m)->is_host)
 
-#ifndef CONFIG_HAVE_CLK
-/* Dummy stub for clk framework */
-#define clk_get(dev, id)	NULL
-#define clk_put(clock)		do {} while (0)
-#define clk_enable(clock)	do {} while (0)
-#define clk_disable(clock)	do {} while (0)
-#endif
-
 #ifdef CONFIG_PROC_FS
 #include <linux/fs.h>
 #define MUSB_CONFIG_PROC_FS
@@ -327,7 +319,6 @@ struct musb {
 
 	irqreturn_t		(*isr)(int, void *);
 	struct work_struct	irq_work;
-	struct work_struct	otg_notifier_work;
 	u16			hwvers;
 
 /* this hub status bit is reserved by USB 2.0 and not seen by usbcore */
@@ -373,7 +364,6 @@ struct musb {
 	u16			int_tx;
 
 	struct usb_phy		*xceiv;
-	u8			xceiv_event;
 
 	int nIrq;
 	unsigned		irq_wake:1;
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 23db42db761..217808d9fbe 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -31,6 +31,7 @@
 
 #include <linux/init.h>
 #include <linux/io.h>
+#include <linux/err.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/pm_runtime.h>
@@ -376,8 +377,8 @@ static int dsps_musb_init(struct musb *musb)
 
 	/* NOP driver needs change if supporting dual instance */
 	usb_nop_xceiv_register();
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv)
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -ENODEV;
 
 	/* Returns zero if e.g. not clocked */
@@ -409,7 +410,7 @@ static int dsps_musb_init(struct musb *musb)
 
 	return 0;
 err0:
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 	return status;
 }
@@ -430,7 +431,7 @@ static int dsps_musb_exit(struct musb *musb)
 		data->set_phy_power(0);
 
 	/* NOP driver needs change if supporting dual instance */
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 
 	return 0;
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 95918dacc99..f7194cf65ab 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -328,6 +328,13 @@ static void txstate(struct musb *musb, struct musb_request *req)
 
 	musb_ep = req->ep;
 
+	/* Check if EP is disabled */
+	if (!musb_ep->desc) {
+		dev_dbg(musb->controller, "ep:%s disabled - ignore request\n",
+						musb_ep->end_point.name);
+		return;
+	}
+
 	/* we shouldn't get here while DMA is active ... but we do ... */
 	if (dma_channel_status(musb_ep->dma) == MUSB_DMA_STATUS_BUSY) {
 		dev_dbg(musb->controller, "dma pending...\n");
@@ -650,6 +657,13 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 
 	len = musb_ep->packet_sz;
 
+	/* Check if EP is disabled */
+	if (!musb_ep->desc) {
+		dev_dbg(musb->controller, "ep:%s disabled - ignore request\n",
+						musb_ep->end_point.name);
+		return;
+	}
+
 	/* We shouldn't get here while DMA is active, but we do... */
 	if (dma_channel_status(musb_ep->dma) == MUSB_DMA_STATUS_BUSY) {
 		dev_dbg(musb->controller, "DMA pending...\n");
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index e090c799d87..4bb717d0bd4 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -1746,7 +1746,11 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 				c->channel_release(dma);
 				hw_ep->rx_channel = NULL;
 				dma = NULL;
-				/* REVISIT reset CSR */
+				val = musb_readw(epio, MUSB_RXCSR);
+				val &= ~(MUSB_RXCSR_DMAENAB
+					| MUSB_RXCSR_H_AUTOREQ
+					| MUSB_RXCSR_AUTOCLEAR);
+				musb_writew(epio, MUSB_RXCSR, val);
 			}
 		}
 #endif	/* Mentor DMA */
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index c7785e81254..5fdb9da8dd5 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -34,6 +34,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/pm_runtime.h>
 #include <linux/err.h>
+#include <linux/usb/musb-omap.h>
 
 #include "musb_core.h"
 #include "omap2430.h"
@@ -41,9 +42,13 @@
 struct omap2430_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	enum omap_musb_vbus_id_status status;
+	struct work_struct	omap_musb_mailbox_work;
 };
 #define glue_to_musb(g)		platform_get_drvdata(g->musb)
 
+struct omap2430_glue		*_glue;
+
 static struct timer_list musb_idle_timer;
 
 static void musb_do_idle(unsigned long _musb)
@@ -223,50 +228,63 @@ static inline void omap2430_low_level_init(struct musb *musb)
 	musb_writel(musb->mregs, OTG_FORCESTDBY, l);
 }
 
-static int musb_otg_notifications(struct notifier_block *nb,
-		unsigned long event, void *unused)
+void omap_musb_mailbox(enum omap_musb_vbus_id_status status)
 {
-	struct musb	*musb = container_of(nb, struct musb, nb);
+	struct omap2430_glue	*glue = _glue;
+	struct musb		*musb = glue_to_musb(glue);
 
-	musb->xceiv_event = event;
-	schedule_work(&musb->otg_notifier_work);
+	glue->status = status;
+	if (!musb) {
+		dev_err(glue->dev, "musb core is not yet ready\n");
+		return;
+	}
 
-	return NOTIFY_OK;
+	schedule_work(&glue->omap_musb_mailbox_work);
 }
+EXPORT_SYMBOL_GPL(omap_musb_mailbox);
 
-static void musb_otg_notifier_work(struct work_struct *data_notifier_work)
+static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 {
-	struct musb *musb = container_of(data_notifier_work, struct musb, otg_notifier_work);
+	struct musb *musb = glue_to_musb(glue);
 	struct device *dev = musb->controller;
 	struct musb_hdrc_platform_data *pdata = dev->platform_data;
 	struct omap_musb_board_data *data = pdata->board_data;
+	struct usb_otg *otg = musb->xceiv->otg;
 
-	switch (musb->xceiv_event) {
-	case USB_EVENT_ID:
-		dev_dbg(musb->controller, "ID GND\n");
+	switch (glue->status) {
+	case OMAP_MUSB_ID_GROUND:
+		dev_dbg(dev, "ID GND\n");
 
+		otg->default_a = true;
+		musb->xceiv->state = OTG_STATE_A_IDLE;
+		musb->xceiv->last_event = USB_EVENT_ID;
 		if (!is_otg_enabled(musb) || musb->gadget_driver) {
-			pm_runtime_get_sync(musb->controller);
+			pm_runtime_get_sync(dev);
 			usb_phy_init(musb->xceiv);
 			omap2430_musb_set_vbus(musb, 1);
 		}
 		break;
 
-	case USB_EVENT_VBUS:
-		dev_dbg(musb->controller, "VBUS Connect\n");
+	case OMAP_MUSB_VBUS_VALID:
+		dev_dbg(dev, "VBUS Connect\n");
 
+		otg->default_a = false;
+		musb->xceiv->state = OTG_STATE_B_IDLE;
+		musb->xceiv->last_event = USB_EVENT_VBUS;
 		if (musb->gadget_driver)
-			pm_runtime_get_sync(musb->controller);
+			pm_runtime_get_sync(dev);
 		usb_phy_init(musb->xceiv);
 		break;
 
-	case USB_EVENT_NONE:
-		dev_dbg(musb->controller, "VBUS Disconnect\n");
+	case OMAP_MUSB_ID_FLOAT:
+	case OMAP_MUSB_VBUS_OFF:
+		dev_dbg(dev, "VBUS Disconnect\n");
 
+		musb->xceiv->last_event = USB_EVENT_NONE;
 		if (is_otg_enabled(musb) || is_peripheral_enabled(musb))
 			if (musb->gadget_driver) {
-				pm_runtime_mark_last_busy(musb->controller);
-				pm_runtime_put_autosuspend(musb->controller);
+				pm_runtime_mark_last_busy(dev);
+				pm_runtime_put_autosuspend(dev);
 			}
 
 		if (data->interface_type == MUSB_INTERFACE_UTMI) {
@@ -276,15 +294,24 @@ static void musb_otg_notifier_work(struct work_struct *data_notifier_work)
 		usb_phy_shutdown(musb->xceiv);
 		break;
 	default:
-		dev_dbg(musb->controller, "ID float\n");
+		dev_dbg(dev, "ID float\n");
 	}
 }
 
+
+static void omap_musb_mailbox_work(struct work_struct *mailbox_work)
+{
+	struct omap2430_glue *glue = container_of(mailbox_work,
+				struct omap2430_glue, omap_musb_mailbox_work);
+	omap_musb_set_mailbox(glue);
+}
+
 static int omap2430_musb_init(struct musb *musb)
 {
 	u32 l;
 	int status = 0;
 	struct device *dev = musb->controller;
+	struct omap2430_glue *glue = dev_get_drvdata(dev->parent);
 	struct musb_hdrc_platform_data *plat = dev->platform_data;
 	struct omap_musb_board_data *data = plat->board_data;
 
@@ -292,14 +319,12 @@ static int omap2430_musb_init(struct musb *musb)
 	 * up through ULPI.  TWL4030-family PMICs include one,
 	 * which needs a driver, drivers aren't always needed.
 	 */
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv) {
+	musb->xceiv = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		pr_err("HS USB OTG: no transceiver configured\n");
 		return -ENODEV;
 	}
 
-	INIT_WORK(&musb->otg_notifier_work, musb_otg_notifier_work);
-
 	status = pm_runtime_get_sync(dev);
 	if (status < 0) {
 		dev_err(dev, "pm_runtime_get_sync FAILED %d\n", status);
@@ -326,14 +351,11 @@ static int omap2430_musb_init(struct musb *musb)
 			musb_readl(musb->mregs, OTG_INTERFSEL),
 			musb_readl(musb->mregs, OTG_SIMENABLE));
 
-	musb->nb.notifier_call = musb_otg_notifications;
-	status = usb_register_notifier(musb->xceiv, &musb->nb);
-
-	if (status)
-		dev_dbg(musb->controller, "notification register failed\n");
-
 	setup_timer(&musb_idle_timer, musb_do_idle, (unsigned long) musb);
 
+	if (glue->status != OMAP_MUSB_UNKNOWN)
+		omap_musb_set_mailbox(glue);
+
 	pm_runtime_put_noidle(musb->controller);
 	return 0;
 
@@ -346,12 +368,13 @@ static void omap2430_musb_enable(struct musb *musb)
 	u8		devctl;
 	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
 	struct device *dev = musb->controller;
+	struct omap2430_glue *glue = dev_get_drvdata(dev->parent);
 	struct musb_hdrc_platform_data *pdata = dev->platform_data;
 	struct omap_musb_board_data *data = pdata->board_data;
 
-	switch (musb->xceiv->last_event) {
+	switch (glue->status) {
 
-	case USB_EVENT_ID:
+	case OMAP_MUSB_ID_GROUND:
 		usb_phy_init(musb->xceiv);
 		if (data->interface_type != MUSB_INTERFACE_UTMI)
 			break;
@@ -370,7 +393,7 @@ static void omap2430_musb_enable(struct musb *musb)
 		}
 		break;
 
-	case USB_EVENT_VBUS:
+	case OMAP_MUSB_VBUS_VALID:
 		usb_phy_init(musb->xceiv);
 		break;
 
@@ -381,17 +404,18 @@ static void omap2430_musb_enable(struct musb *musb)
 
 static void omap2430_musb_disable(struct musb *musb)
 {
-	if (musb->xceiv->last_event)
+	struct device *dev = musb->controller;
+	struct omap2430_glue *glue = dev_get_drvdata(dev->parent);
+
+	if (glue->status != OMAP_MUSB_UNKNOWN)
 		usb_phy_shutdown(musb->xceiv);
 }
 
 static int omap2430_musb_exit(struct musb *musb)
 {
 	del_timer_sync(&musb_idle_timer);
-	cancel_work_sync(&musb->otg_notifier_work);
 
 	omap2430_low_level_exit(musb);
-	usb_put_transceiver(musb->xceiv);
 
 	return 0;
 }
@@ -418,7 +442,7 @@ static int __devinit omap2430_probe(struct platform_device *pdev)
 	struct omap2430_glue		*glue;
 	int				ret = -ENOMEM;
 
-	glue = kzalloc(sizeof(*glue), GFP_KERNEL);
+	glue = devm_kzalloc(&pdev->dev, sizeof(*glue), GFP_KERNEL);
 	if (!glue) {
 		dev_err(&pdev->dev, "failed to allocate glue context\n");
 		goto err0;
@@ -427,7 +451,7 @@ static int __devinit omap2430_probe(struct platform_device *pdev)
 	musb = platform_device_alloc("musb-hdrc", -1);
 	if (!musb) {
 		dev_err(&pdev->dev, "failed to allocate musb device\n");
-		goto err1;
+		goto err0;
 	}
 
 	musb->dev.parent		= &pdev->dev;
@@ -436,22 +460,31 @@ static int __devinit omap2430_probe(struct platform_device *pdev)
 
 	glue->dev			= &pdev->dev;
 	glue->musb			= musb;
+	glue->status			= OMAP_MUSB_UNKNOWN;
 
 	pdata->platform_ops		= &omap2430_ops;
 
 	platform_set_drvdata(pdev, glue);
 
+	/*
+	 * REVISIT if we ever have two instances of the wrapper, we will be
+	 * in big trouble
+	 */
+	_glue	= glue;
+
+	INIT_WORK(&glue->omap_musb_mailbox_work, omap_musb_mailbox_work);
+
 	ret = platform_device_add_resources(musb, pdev->resource,
 			pdev->num_resources);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add resources\n");
-		goto err2;
+		goto err1;
 	}
 
 	ret = platform_device_add_data(musb, pdata, sizeof(*pdata));
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add platform_data\n");
-		goto err2;
+		goto err1;
 	}
 
 	pm_runtime_enable(&pdev->dev);
@@ -459,16 +492,13 @@ static int __devinit omap2430_probe(struct platform_device *pdev)
 	ret = platform_device_add(musb);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register musb device\n");
-		goto err2;
+		goto err1;
 	}
 
 	return 0;
 
-err2:
-	platform_device_put(musb);
-
 err1:
-	kfree(glue);
+	platform_device_put(musb);
 
 err0:
 	return ret;
@@ -478,9 +508,9 @@ static int __devexit omap2430_remove(struct platform_device *pdev)
 {
 	struct omap2430_glue		*glue = platform_get_drvdata(pdev);
 
+	cancel_work_sync(&glue->omap_musb_mailbox_work);
 	platform_device_del(glue->musb);
 	platform_device_put(glue->musb);
-	kfree(glue);
 
 	return 0;
 }
@@ -546,7 +576,7 @@ static int __init omap2430_init(void)
 {
 	return platform_driver_register(&omap2430_driver);
 }
-module_init(omap2430_init);
+subsys_initcall(omap2430_init);
 
 static void __exit omap2430_exit(void)
 {
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index de1355946a8..1a1bd9cf40c 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -17,6 +17,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/errno.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/prefetch.h>
 #include <linux/usb.h>
@@ -1078,8 +1079,8 @@ static int tusb_musb_init(struct musb *musb)
 	int			ret;
 
 	usb_nop_xceiv_register();
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv)
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -ENODEV;
 
 	pdev = to_platform_device(musb->controller);
@@ -1130,7 +1131,7 @@ done:
 		if (sync)
 			iounmap(sync);
 
-		usb_put_transceiver(musb->xceiv);
+		usb_put_phy(musb->xceiv);
 		usb_nop_xceiv_unregister();
 	}
 	return ret;
@@ -1146,7 +1147,7 @@ static int tusb_musb_exit(struct musb *musb)
 
 	iounmap(musb->sync_va);
 
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 	usb_nop_xceiv_unregister();
 	return 0;
 }
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index aa09dd417b9..a8c0fadce1b 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -23,6 +23,7 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 
@@ -37,8 +38,8 @@ struct ux500_glue {
 
 static int ux500_musb_init(struct musb *musb)
 {
-	musb->xceiv = usb_get_transceiver();
-	if (!musb->xceiv) {
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		pr_err("HS USB OTG: no transceiver configured\n");
 		return -ENODEV;
 	}
@@ -48,7 +49,7 @@ static int ux500_musb_init(struct musb *musb)
 
 static int ux500_musb_exit(struct musb *musb)
 {
-	usb_put_transceiver(musb->xceiv);
+	usb_put_phy(musb->xceiv);
 
 	return 0;
 }
diff --git a/drivers/usb/otg/Kconfig b/drivers/usb/otg/Kconfig
index 5c87db06b59..13fd1ddf742 100644
--- a/drivers/usb/otg/Kconfig
+++ b/drivers/usb/otg/Kconfig
@@ -116,6 +116,16 @@ config FSL_USB2_OTG
 	help
 	  Enable this to support Freescale USB OTG transceiver.
 
+config USB_MXS_PHY
+	tristate "Freescale MXS USB PHY support"
+	depends on ARCH_MXC || ARCH_MXS
+	select STMP_DEVICE
+	select USB_OTG_UTILS
+	help
+	  Enable this to support the Freescale MXS USB PHY.
+
+	  MXS Phy is used by some of the i.MX SoCs, for example imx23/28/6x.
+
 config USB_MV_OTG
 	tristate "Marvell USB OTG support"
 	depends on USB_EHCI_MV && USB_MV_UDC && USB_SUSPEND
diff --git a/drivers/usb/otg/Makefile b/drivers/usb/otg/Makefile
index 41aa5098b13..a844b8d35d1 100644
--- a/drivers/usb/otg/Makefile
+++ b/drivers/usb/otg/Makefile
@@ -20,4 +20,5 @@ obj-$(CONFIG_USB_MSM_OTG)	+= msm_otg.o
 obj-$(CONFIG_AB8500_USB)	+= ab8500-usb.o
 fsl_usb2_otg-objs		:= fsl_otg.o otg_fsm.o
 obj-$(CONFIG_FSL_USB2_OTG)	+= fsl_usb2_otg.o
+obj-$(CONFIG_USB_MXS_PHY)	+= mxs-phy.o
 obj-$(CONFIG_USB_MV_OTG)	+= mv_otg.o
diff --git a/drivers/usb/otg/ab8500-usb.c b/drivers/usb/otg/ab8500-usb.c
index a84af677dc5..ae8ad561f08 100644
--- a/drivers/usb/otg/ab8500-usb.c
+++ b/drivers/usb/otg/ab8500-usb.c
@@ -529,7 +529,7 @@ static int __devinit ab8500_usb_probe(struct platform_device *pdev)
 	if (err < 0)
 		goto fail0;
 
-	err = usb_set_transceiver(&ab->phy);
+	err = usb_add_phy(&ab->phy, USB_PHY_TYPE_USB2);
 	if (err) {
 		dev_err(&pdev->dev, "Can't register transceiver\n");
 		goto fail1;
@@ -556,7 +556,7 @@ static int __devexit ab8500_usb_remove(struct platform_device *pdev)
 
 	cancel_work_sync(&ab->phy_dis_work);
 
-	usb_set_transceiver(NULL);
+	usb_remove_phy(&ab->phy);
 
 	ab8500_usb_host_phy_dis(ab);
 	ab8500_usb_peri_phy_dis(ab);
diff --git a/drivers/usb/otg/fsl_otg.c b/drivers/usb/otg/fsl_otg.c
index be4a63e8302..23c798cb2d7 100644
--- a/drivers/usb/otg/fsl_otg.c
+++ b/drivers/usb/otg/fsl_otg.c
@@ -806,7 +806,7 @@ static int fsl_otg_conf(struct platform_device *pdev)
 	fsl_otg_dev = fsl_otg_tc;
 
 	/* Store the otg transceiver */
-	status = usb_set_transceiver(&fsl_otg_tc->phy);
+	status = usb_add_phy(&fsl_otg_tc->phy, USB_PHY_TYPE_USB2);
 	if (status) {
 		pr_warn(FSL_OTG_NAME ": unable to register OTG transceiver.\n");
 		goto err;
@@ -824,7 +824,7 @@ err:
 int usb_otg_start(struct platform_device *pdev)
 {
 	struct fsl_otg *p_otg;
-	struct usb_phy *otg_trans = usb_get_transceiver();
+	struct usb_phy *otg_trans = usb_get_phy(USB_PHY_TYPE_USB2);
 	struct otg_fsm *fsm;
 	int status;
 	struct resource *res;
@@ -1134,7 +1134,7 @@ static int __devexit fsl_otg_remove(struct platform_device *pdev)
 {
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
 
-	usb_set_transceiver(NULL);
+	usb_remove_phy(&fsl_otg_dev->phy);
 	free_irq(fsl_otg_dev->irq, fsl_otg_dev);
 
 	iounmap((void *)usb_dr_regs);
diff --git a/drivers/usb/otg/gpio_vbus.c b/drivers/usb/otg/gpio_vbus.c
index bde6298a969..a67ffe22179 100644
--- a/drivers/usb/otg/gpio_vbus.c
+++ b/drivers/usb/otg/gpio_vbus.c
@@ -320,7 +320,7 @@ static int __init gpio_vbus_probe(struct platform_device *pdev)
 	}
 
 	/* only active when a gadget is registered */
-	err = usb_set_transceiver(&gpio_vbus->phy);
+	err = usb_add_phy(&gpio_vbus->phy, USB_PHY_TYPE_USB2);
 	if (err) {
 		dev_err(&pdev->dev, "can't register transceiver, err: %d\n",
 			err);
@@ -354,7 +354,7 @@ static int __exit gpio_vbus_remove(struct platform_device *pdev)
 	cancel_delayed_work_sync(&gpio_vbus->work);
 	regulator_put(gpio_vbus->vbus_draw);
 
-	usb_set_transceiver(NULL);
+	usb_remove_phy(&gpio_vbus->phy);
 
 	free_irq(gpio_vbus->irq, pdev);
 	if (gpio_is_valid(pdata->gpio_pullup))
diff --git a/drivers/usb/otg/isp1301_omap.c b/drivers/usb/otg/isp1301_omap.c
index 70cf5d7bca4..7a88667742b 100644
--- a/drivers/usb/otg/isp1301_omap.c
+++ b/drivers/usb/otg/isp1301_omap.c
@@ -36,9 +36,9 @@
 #include <asm/irq.h>
 #include <asm/mach-types.h>
 
-#include <plat/usb.h>
 #include <plat/mux.h>
 
+#include <mach/usb.h>
 
 #ifndef	DEBUG
 #undef	VERBOSE
@@ -1336,9 +1336,6 @@ static int
 isp1301_set_peripheral(struct usb_otg *otg, struct usb_gadget *gadget)
 {
 	struct isp1301	*isp = container_of(otg->phy, struct isp1301, phy);
-#ifndef	CONFIG_USB_OTG
-	u32 l;
-#endif
 
 	if (!otg || isp != the_transceiver)
 		return -ENODEV;
@@ -1365,10 +1362,14 @@ isp1301_set_peripheral(struct usb_otg *otg, struct usb_gadget *gadget)
 	otg->gadget = gadget;
 	// FIXME update its refcount
 
-	l = omap_readl(OTG_CTRL) & OTG_CTRL_MASK;
-	l &= ~(OTG_XCEIV_OUTPUTS|OTG_CTRL_BITS);
-	l |= OTG_ID;
-	omap_writel(l, OTG_CTRL);
+	{
+		u32 l;
+
+		l = omap_readl(OTG_CTRL) & OTG_CTRL_MASK;
+		l &= ~(OTG_XCEIV_OUTPUTS|OTG_CTRL_BITS);
+		l |= OTG_ID;
+		omap_writel(l, OTG_CTRL);
+	}
 
 	power_up(isp);
 	isp->phy.state = OTG_STATE_B_IDLE;
@@ -1575,7 +1576,6 @@ isp1301_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 		isp->irq_type = IRQF_TRIGGER_FALLING;
 	}
 
-	isp->irq_type |= IRQF_SAMPLE_RANDOM;
 	status = request_irq(i2c->irq, isp1301_irq,
 			isp->irq_type, DRIVER_NAME, isp);
 	if (status < 0) {
@@ -1610,7 +1610,7 @@ isp1301_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
 	dev_dbg(&i2c->dev, "scheduled timer, %d min\n", TIMER_MINUTES);
 #endif
 
-	status = usb_set_transceiver(&isp->phy);
+	status = usb_add_phy(&isp->phy, USB_PHY_TYPE_USB2);
 	if (status < 0)
 		dev_err(&i2c->dev, "can't register transceiver, %d\n",
 			status);
@@ -1649,7 +1649,7 @@ subsys_initcall(isp_init);
 static void __exit isp_exit(void)
 {
 	if (the_transceiver)
-		usb_set_transceiver(NULL);
+		usb_remove_phy(&the_transceiver->phy);
 	i2c_del_driver(&isp1301_driver);
 }
 module_exit(isp_exit);
diff --git a/drivers/usb/otg/msm_otg.c b/drivers/usb/otg/msm_otg.c
index 1d0347c247d..9f5fc906041 100644
--- a/drivers/usb/otg/msm_otg.c
+++ b/drivers/usb/otg/msm_otg.c
@@ -1555,9 +1555,9 @@ static int __init msm_otg_probe(struct platform_device *pdev)
 	phy->otg->set_host = msm_otg_set_host;
 	phy->otg->set_peripheral = msm_otg_set_peripheral;
 
-	ret = usb_set_transceiver(&motg->phy);
+	ret = usb_add_phy(&motg->phy, USB_PHY_TYPE_USB2);
 	if (ret) {
-		dev_err(&pdev->dev, "usb_set_transceiver failed\n");
+		dev_err(&pdev->dev, "usb_add_phy failed\n");
 		goto free_irq;
 	}
 
@@ -1624,7 +1624,7 @@ static int __devexit msm_otg_remove(struct platform_device *pdev)
 	device_init_wakeup(&pdev->dev, 0);
 	pm_runtime_disable(&pdev->dev);
 
-	usb_set_transceiver(NULL);
+	usb_remove_phy(phy);
 	free_irq(motg->irq, motg);
 
 	/*
diff --git a/drivers/usb/otg/mv_otg.c b/drivers/usb/otg/mv_otg.c
index 6cc6c3ffbb8..3f124e8f579 100644
--- a/drivers/usb/otg/mv_otg.c
+++ b/drivers/usb/otg/mv_otg.c
@@ -690,7 +690,7 @@ int mv_otg_remove(struct platform_device *pdev)
 	for (clk_i = 0; clk_i <= mvotg->clknum; clk_i++)
 		clk_put(mvotg->clk[clk_i]);
 
-	usb_set_transceiver(NULL);
+	usb_remove_phy(&mvotg->phy);
 	platform_set_drvdata(pdev, NULL);
 
 	kfree(mvotg->phy.otg);
@@ -853,7 +853,7 @@ static int mv_otg_probe(struct platform_device *pdev)
 		goto err_disable_clk;
 	}
 
-	retval = usb_set_transceiver(&mvotg->phy);
+	retval = usb_add_phy(&mvotg->phy, USB_PHY_TYPE_USB2);
 	if (retval < 0) {
 		dev_err(&pdev->dev, "can't register transceiver, %d\n",
 			retval);
@@ -880,7 +880,7 @@ static int mv_otg_probe(struct platform_device *pdev)
 	return 0;
 
 err_set_transceiver:
-	usb_set_transceiver(NULL);
+	usb_remove_phy(&mvotg->phy);
 err_free_irq:
 	free_irq(mvotg->irq, mvotg);
 err_disable_clk:
diff --git a/drivers/usb/otg/mxs-phy.c b/drivers/usb/otg/mxs-phy.c
new file mode 100644
index 00000000000..c1a67cb8e24
--- /dev/null
+++ b/drivers/usb/otg/mxs-phy.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ * Copyright (C) 2012 Marek Vasut <marex@denx.de>
+ * on behalf of DENX Software Engineering GmbH
+ *
+ * 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/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/usb/otg.h>
+#include <linux/stmp_device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+
+#define DRIVER_NAME "mxs_phy"
+
+#define HW_USBPHY_PWD				0x00
+#define HW_USBPHY_CTRL				0x30
+#define HW_USBPHY_CTRL_SET			0x34
+#define HW_USBPHY_CTRL_CLR			0x38
+
+#define BM_USBPHY_CTRL_SFTRST			BIT(31)
+#define BM_USBPHY_CTRL_CLKGATE			BIT(30)
+#define BM_USBPHY_CTRL_ENUTMILEVEL3		BIT(15)
+#define BM_USBPHY_CTRL_ENUTMILEVEL2		BIT(14)
+#define BM_USBPHY_CTRL_ENHOSTDISCONDETECT	BIT(1)
+
+struct mxs_phy {
+	struct usb_phy phy;
+	struct clk *clk;
+};
+
+#define to_mxs_phy(p) container_of((p), struct mxs_phy, phy)
+
+static void mxs_phy_hw_init(struct mxs_phy *mxs_phy)
+{
+	void __iomem *base = mxs_phy->phy.io_priv;
+
+	stmp_reset_block(base + HW_USBPHY_CTRL);
+
+	/* Power up the PHY */
+	writel_relaxed(0, base + HW_USBPHY_PWD);
+
+	/* enable FS/LS device */
+	writel_relaxed(BM_USBPHY_CTRL_ENUTMILEVEL2 |
+			BM_USBPHY_CTRL_ENUTMILEVEL3,
+			base + HW_USBPHY_CTRL_SET);
+}
+
+static int mxs_phy_init(struct usb_phy *phy)
+{
+	struct mxs_phy *mxs_phy = to_mxs_phy(phy);
+
+	clk_prepare_enable(mxs_phy->clk);
+	mxs_phy_hw_init(mxs_phy);
+
+	return 0;
+}
+
+static void mxs_phy_shutdown(struct usb_phy *phy)
+{
+	struct mxs_phy *mxs_phy = to_mxs_phy(phy);
+
+	writel_relaxed(BM_USBPHY_CTRL_CLKGATE,
+			phy->io_priv + HW_USBPHY_CTRL_SET);
+
+	clk_disable_unprepare(mxs_phy->clk);
+}
+
+static int mxs_phy_on_connect(struct usb_phy *phy, int port)
+{
+	dev_dbg(phy->dev, "Connect on port %d\n", port);
+
+	mxs_phy_hw_init(to_mxs_phy(phy));
+	writel_relaxed(BM_USBPHY_CTRL_ENHOSTDISCONDETECT,
+			phy->io_priv + HW_USBPHY_CTRL_SET);
+
+	return 0;
+}
+
+static int mxs_phy_on_disconnect(struct usb_phy *phy, int port)
+{
+	dev_dbg(phy->dev, "Disconnect on port %d\n", port);
+
+	writel_relaxed(BM_USBPHY_CTRL_ENHOSTDISCONDETECT,
+			phy->io_priv + HW_USBPHY_CTRL_CLR);
+
+	return 0;
+}
+
+static int mxs_phy_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	void __iomem *base;
+	struct clk *clk;
+	struct mxs_phy *mxs_phy;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "can't get device resources\n");
+		return -ENOENT;
+	}
+
+	base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!base)
+		return -EBUSY;
+
+	clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev,
+			"can't get the clock, err=%ld", PTR_ERR(clk));
+		return PTR_ERR(clk);
+	}
+
+	mxs_phy = devm_kzalloc(&pdev->dev, sizeof(*mxs_phy), GFP_KERNEL);
+	if (!mxs_phy) {
+		dev_err(&pdev->dev, "Failed to allocate USB PHY structure!\n");
+		return -ENOMEM;
+	}
+
+	mxs_phy->phy.io_priv		= base;
+	mxs_phy->phy.dev		= &pdev->dev;
+	mxs_phy->phy.label		= DRIVER_NAME;
+	mxs_phy->phy.init		= mxs_phy_init;
+	mxs_phy->phy.shutdown		= mxs_phy_shutdown;
+	mxs_phy->phy.notify_connect	= mxs_phy_on_connect;
+	mxs_phy->phy.notify_disconnect	= mxs_phy_on_disconnect;
+
+	ATOMIC_INIT_NOTIFIER_HEAD(&mxs_phy->phy.notifier);
+
+	mxs_phy->clk = clk;
+
+	platform_set_drvdata(pdev, &mxs_phy->phy);
+
+	return 0;
+}
+
+static int __devexit mxs_phy_remove(struct platform_device *pdev)
+{
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id mxs_phy_dt_ids[] = {
+	{ .compatible = "fsl,imx23-usbphy", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxs_phy_dt_ids);
+
+static struct platform_driver mxs_phy_driver = {
+	.probe = mxs_phy_probe,
+	.remove = __devexit_p(mxs_phy_remove),
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = mxs_phy_dt_ids,
+	 },
+};
+
+static int __init mxs_phy_module_init(void)
+{
+	return platform_driver_register(&mxs_phy_driver);
+}
+postcore_initcall(mxs_phy_module_init);
+
+static void __exit mxs_phy_module_exit(void)
+{
+	platform_driver_unregister(&mxs_phy_driver);
+}
+module_exit(mxs_phy_module_exit);
+
+MODULE_ALIAS("platform:mxs-usb-phy");
+MODULE_AUTHOR("Marek Vasut <marex@denx.de>");
+MODULE_AUTHOR("Richard Zhao <richard.zhao@freescale.com>");
+MODULE_DESCRIPTION("Freescale MXS USB PHY driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/otg/nop-usb-xceiv.c b/drivers/usb/otg/nop-usb-xceiv.c
index 58b26df6afd..803f958f413 100644
--- a/drivers/usb/otg/nop-usb-xceiv.c
+++ b/drivers/usb/otg/nop-usb-xceiv.c
@@ -117,7 +117,7 @@ static int __devinit nop_usb_xceiv_probe(struct platform_device *pdev)
 	nop->phy.otg->set_host		= nop_set_host;
 	nop->phy.otg->set_peripheral	= nop_set_peripheral;
 
-	err = usb_set_transceiver(&nop->phy);
+	err = usb_add_phy(&nop->phy, USB_PHY_TYPE_USB2);
 	if (err) {
 		dev_err(&pdev->dev, "can't register transceiver, err: %d\n",
 			err);
@@ -139,7 +139,7 @@ static int __devexit nop_usb_xceiv_remove(struct platform_device *pdev)
 {
 	struct nop_usb_xceiv *nop = platform_get_drvdata(pdev);
 
-	usb_set_transceiver(NULL);
+	usb_remove_phy(&nop->phy);
 
 	platform_set_drvdata(pdev, NULL);
 	kfree(nop->phy.otg);
diff --git a/drivers/usb/otg/otg.c b/drivers/usb/otg/otg.c
index 801e597a154..1bf60a22595 100644
--- a/drivers/usb/otg/otg.c
+++ b/drivers/usb/otg/otg.c
@@ -11,60 +11,195 @@
 
 #include <linux/kernel.h>
 #include <linux/export.h>
+#include <linux/err.h>
 #include <linux/device.h>
+#include <linux/slab.h>
 
 #include <linux/usb/otg.h>
 
-static struct usb_phy *phy;
+static LIST_HEAD(phy_list);
+static DEFINE_SPINLOCK(phy_lock);
+
+static struct usb_phy *__usb_find_phy(struct list_head *list,
+	enum usb_phy_type type)
+{
+	struct usb_phy  *phy = NULL;
+
+	list_for_each_entry(phy, list, head) {
+		if (phy->type != type)
+			continue;
+
+		return phy;
+	}
+
+	return ERR_PTR(-ENODEV);
+}
+
+static void devm_usb_phy_release(struct device *dev, void *res)
+{
+	struct usb_phy *phy = *(struct usb_phy **)res;
+
+	usb_put_phy(phy);
+}
+
+static int devm_usb_phy_match(struct device *dev, void *res, void *match_data)
+{
+	return res == match_data;
+}
 
 /**
- * usb_get_transceiver - find the (single) USB transceiver
+ * devm_usb_get_phy - find the USB PHY
+ * @dev - device that requests this phy
+ * @type - the type of the phy the controller requires
  *
- * Returns the transceiver driver, after getting a refcount to it; or
- * null if there is no such transceiver.  The caller is responsible for
- * calling usb_put_transceiver() to release that count.
+ * Gets the phy using usb_get_phy(), and associates a device with it using
+ * devres. On driver detach, release function is invoked on the devres data,
+ * then, devres data is freed.
  *
  * For use by USB host and peripheral drivers.
  */
-struct usb_phy *usb_get_transceiver(void)
+struct usb_phy *devm_usb_get_phy(struct device *dev, enum usb_phy_type type)
 {
-	if (phy)
-		get_device(phy->dev);
+	struct usb_phy **ptr, *phy;
+
+	ptr = devres_alloc(devm_usb_phy_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return NULL;
+
+	phy = usb_get_phy(type);
+	if (!IS_ERR(phy)) {
+		*ptr = phy;
+		devres_add(dev, ptr);
+	} else
+		devres_free(ptr);
+
+	return phy;
+}
+EXPORT_SYMBOL(devm_usb_get_phy);
+
+/**
+ * usb_get_phy - find the USB PHY
+ * @type - the type of the phy the controller requires
+ *
+ * Returns the phy driver, after getting a refcount to it; or
+ * -ENODEV if there is no such phy.  The caller is responsible for
+ * calling usb_put_phy() to release that count.
+ *
+ * For use by USB host and peripheral drivers.
+ */
+struct usb_phy *usb_get_phy(enum usb_phy_type type)
+{
+	struct usb_phy	*phy = NULL;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&phy_lock, flags);
+
+	phy = __usb_find_phy(&phy_list, type);
+	if (IS_ERR(phy)) {
+		pr_err("unable to find transceiver of type %s\n",
+			usb_phy_type_string(type));
+		goto err0;
+	}
+
+	get_device(phy->dev);
+
+err0:
+	spin_unlock_irqrestore(&phy_lock, flags);
+
 	return phy;
 }
-EXPORT_SYMBOL(usb_get_transceiver);
+EXPORT_SYMBOL(usb_get_phy);
+
+/**
+ * devm_usb_put_phy - release the USB PHY
+ * @dev - device that wants to release this phy
+ * @phy - the phy returned by devm_usb_get_phy()
+ *
+ * destroys the devres associated with this phy and invokes usb_put_phy
+ * to release the phy.
+ *
+ * For use by USB host and peripheral drivers.
+ */
+void devm_usb_put_phy(struct device *dev, struct usb_phy *phy)
+{
+	int r;
+
+	r = devres_destroy(dev, devm_usb_phy_release, devm_usb_phy_match, phy);
+	dev_WARN_ONCE(dev, r, "couldn't find PHY resource\n");
+}
+EXPORT_SYMBOL(devm_usb_put_phy);
 
 /**
- * usb_put_transceiver - release the (single) USB transceiver
- * @x: the transceiver returned by usb_get_transceiver()
+ * usb_put_phy - release the USB PHY
+ * @x: the phy returned by usb_get_phy()
  *
- * Releases a refcount the caller received from usb_get_transceiver().
+ * Releases a refcount the caller received from usb_get_phy().
  *
  * For use by USB host and peripheral drivers.
  */
-void usb_put_transceiver(struct usb_phy *x)
+void usb_put_phy(struct usb_phy *x)
 {
 	if (x)
 		put_device(x->dev);
 }
-EXPORT_SYMBOL(usb_put_transceiver);
+EXPORT_SYMBOL(usb_put_phy);
 
 /**
- * usb_set_transceiver - declare the (single) USB transceiver
- * @x: the USB transceiver to be used; or NULL
+ * usb_add_phy - declare the USB PHY
+ * @x: the USB phy to be used; or NULL
+ * @type - the type of this PHY
  *
- * This call is exclusively for use by transceiver drivers, which
+ * This call is exclusively for use by phy drivers, which
  * coordinate the activities of drivers for host and peripheral
  * controllers, and in some cases for VBUS current regulation.
  */
-int usb_set_transceiver(struct usb_phy *x)
+int usb_add_phy(struct usb_phy *x, enum usb_phy_type type)
+{
+	int		ret = 0;
+	unsigned long	flags;
+	struct usb_phy	*phy;
+
+	if (x && x->type != USB_PHY_TYPE_UNDEFINED) {
+		dev_err(x->dev, "not accepting initialized PHY %s\n", x->label);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&phy_lock, flags);
+
+	list_for_each_entry(phy, &phy_list, head) {
+		if (phy->type == type) {
+			ret = -EBUSY;
+			dev_err(x->dev, "transceiver type %s already exists\n",
+						usb_phy_type_string(type));
+			goto out;
+		}
+	}
+
+	x->type = type;
+	list_add_tail(&x->head, &phy_list);
+
+out:
+	spin_unlock_irqrestore(&phy_lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(usb_add_phy);
+
+/**
+ * usb_remove_phy - remove the OTG PHY
+ * @x: the USB OTG PHY to be removed;
+ *
+ * This reverts the effects of usb_add_phy
+ */
+void usb_remove_phy(struct usb_phy *x)
 {
-	if (phy && x)
-		return -EBUSY;
-	phy = x;
-	return 0;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&phy_lock, flags);
+	if (x)
+		list_del(&x->head);
+	spin_unlock_irqrestore(&phy_lock, flags);
 }
-EXPORT_SYMBOL(usb_set_transceiver);
+EXPORT_SYMBOL(usb_remove_phy);
 
 const char *otg_state_string(enum usb_otg_state state)
 {
diff --git a/drivers/usb/otg/twl4030-usb.c b/drivers/usb/otg/twl4030-usb.c
index c4a86da858e..523cad5bfea 100644
--- a/drivers/usb/otg/twl4030-usb.c
+++ b/drivers/usb/otg/twl4030-usb.c
@@ -33,11 +33,11 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/usb/otg.h>
+#include <linux/usb/musb-omap.h>
 #include <linux/usb/ulpi.h>
 #include <linux/i2c/twl.h>
 #include <linux/regulator/consumer.h>
 #include <linux/err.h>
-#include <linux/notifier.h>
 #include <linux/slab.h>
 
 /* Register defines */
@@ -159,7 +159,7 @@ struct twl4030_usb {
 	enum twl4030_usb_mode	usb_mode;
 
 	int			irq;
-	u8			linkstat;
+	enum omap_musb_vbus_id_status linkstat;
 	bool			vbus_supplied;
 	u8			asleep;
 	bool			irq_enabled;
@@ -246,11 +246,11 @@ twl4030_usb_clear_bits(struct twl4030_usb *twl, u8 reg, u8 bits)
 
 /*-------------------------------------------------------------------------*/
 
-static enum usb_phy_events twl4030_usb_linkstat(struct twl4030_usb *twl)
+static enum omap_musb_vbus_id_status
+	twl4030_usb_linkstat(struct twl4030_usb *twl)
 {
 	int	status;
-	int	linkstat = USB_EVENT_NONE;
-	struct usb_otg *otg = twl->phy.otg;
+	enum omap_musb_vbus_id_status linkstat = OMAP_MUSB_UNKNOWN;
 
 	twl->vbus_supplied = false;
 
@@ -273,30 +273,23 @@ static enum usb_phy_events twl4030_usb_linkstat(struct twl4030_usb *twl)
                         twl->vbus_supplied = true;
 
 		if (status & BIT(2))
-			linkstat = USB_EVENT_ID;
+			linkstat = OMAP_MUSB_ID_GROUND;
 		else
-			linkstat = USB_EVENT_VBUS;
-	} else
-		linkstat = USB_EVENT_NONE;
+			linkstat = OMAP_MUSB_VBUS_VALID;
+	} else {
+		if (twl->linkstat != OMAP_MUSB_UNKNOWN)
+			linkstat = OMAP_MUSB_VBUS_OFF;
+	}
 
 	dev_dbg(twl->dev, "HW_CONDITIONS 0x%02x/%d; link %d\n",
 			status, status, linkstat);
 
-	twl->phy.last_event = linkstat;
-
 	/* REVISIT this assumes host and peripheral controllers
 	 * are registered, and that both are active...
 	 */
 
 	spin_lock_irq(&twl->lock);
 	twl->linkstat = linkstat;
-	if (linkstat == USB_EVENT_ID) {
-		otg->default_a = true;
-		twl->phy.state = OTG_STATE_A_IDLE;
-	} else {
-		otg->default_a = false;
-		twl->phy.state = OTG_STATE_B_IDLE;
-	}
 	spin_unlock_irq(&twl->lock);
 
 	return linkstat;
@@ -501,10 +494,10 @@ static DEVICE_ATTR(vbus, 0444, twl4030_usb_vbus_show, NULL);
 static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 {
 	struct twl4030_usb *twl = _twl;
-	int status;
+	enum omap_musb_vbus_id_status status;
 
 	status = twl4030_usb_linkstat(twl);
-	if (status >= 0) {
+	if (status > 0) {
 		/* FIXME add a set_power() method so that B-devices can
 		 * configure the charger appropriately.  It's not always
 		 * correct to consume VBUS power, and how much current to
@@ -516,13 +509,13 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 		 * USB_LINK_VBUS state.  musb_hdrc won't care until it
 		 * starts to handle softconnect right.
 		 */
-		if (status == USB_EVENT_NONE)
+		if (status == OMAP_MUSB_VBUS_OFF ||
+				status == OMAP_MUSB_ID_FLOAT)
 			twl4030_phy_suspend(twl, 0);
 		else
 			twl4030_phy_resume(twl);
 
-		atomic_notifier_call_chain(&twl->phy.notifier, status,
-				twl->phy.otg->gadget);
+		omap_musb_mailbox(twl->linkstat);
 	}
 	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
 
@@ -531,11 +524,12 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 
 static void twl4030_usb_phy_init(struct twl4030_usb *twl)
 {
-	int status;
+	enum omap_musb_vbus_id_status status;
 
 	status = twl4030_usb_linkstat(twl);
-	if (status >= 0) {
-		if (status == USB_EVENT_NONE) {
+	if (status > 0) {
+		if (status == OMAP_MUSB_VBUS_OFF ||
+				status == OMAP_MUSB_ID_FLOAT) {
 			__twl4030_phy_power(twl, 0);
 			twl->asleep = 1;
 		} else {
@@ -543,8 +537,7 @@ static void twl4030_usb_phy_init(struct twl4030_usb *twl)
 			twl->asleep = 0;
 		}
 
-		atomic_notifier_call_chain(&twl->phy.notifier, status,
-				twl->phy.otg->gadget);
+		omap_musb_mailbox(twl->linkstat);
 	}
 	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
 }
@@ -598,21 +591,20 @@ static int __devinit twl4030_usb_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	twl = kzalloc(sizeof *twl, GFP_KERNEL);
+	twl = devm_kzalloc(&pdev->dev, sizeof *twl, GFP_KERNEL);
 	if (!twl)
 		return -ENOMEM;
 
-	otg = kzalloc(sizeof *otg, GFP_KERNEL);
-	if (!otg) {
-		kfree(twl);
+	otg = devm_kzalloc(&pdev->dev, sizeof *otg, GFP_KERNEL);
+	if (!otg)
 		return -ENOMEM;
-	}
 
 	twl->dev		= &pdev->dev;
 	twl->irq		= platform_get_irq(pdev, 0);
 	twl->usb_mode		= pdata->usb_mode;
 	twl->vbus_supplied	= false;
 	twl->asleep		= 1;
+	twl->linkstat		= OMAP_MUSB_UNKNOWN;
 
 	twl->phy.dev		= twl->dev;
 	twl->phy.label		= "twl4030";
@@ -629,18 +621,14 @@ static int __devinit twl4030_usb_probe(struct platform_device *pdev)
 	err = twl4030_usb_ldo_init(twl);
 	if (err) {
 		dev_err(&pdev->dev, "ldo init failed\n");
-		kfree(otg);
-		kfree(twl);
 		return err;
 	}
-	usb_set_transceiver(&twl->phy);
+	usb_add_phy(&twl->phy, USB_PHY_TYPE_USB2);
 
 	platform_set_drvdata(pdev, twl);
 	if (device_create_file(&pdev->dev, &dev_attr_vbus))
 		dev_warn(&pdev->dev, "could not create sysfs file\n");
 
-	ATOMIC_INIT_NOTIFIER_HEAD(&twl->phy.notifier);
-
 	/* Our job is to use irqs and status from the power module
 	 * to keep the transceiver disabled when nothing's connected.
 	 *
@@ -651,13 +639,11 @@ static int __devinit twl4030_usb_probe(struct platform_device *pdev)
 	 */
 	twl->irq_enabled = true;
 	status = request_threaded_irq(twl->irq, NULL, twl4030_usb_irq,
-			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
-			"twl4030_usb", twl);
+			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING |
+			IRQF_ONESHOT, "twl4030_usb", twl);
 	if (status < 0) {
 		dev_dbg(&pdev->dev, "can't get IRQ %d, err %d\n",
 			twl->irq, status);
-		kfree(otg);
-		kfree(twl);
 		return status;
 	}
 
@@ -701,9 +687,6 @@ static int __exit twl4030_usb_remove(struct platform_device *pdev)
 	regulator_put(twl->usb1v8);
 	regulator_put(twl->usb3v1);
 
-	kfree(twl->phy.otg);
-	kfree(twl);
-
 	return 0;
 }
 
diff --git a/drivers/usb/otg/twl6030-usb.c b/drivers/usb/otg/twl6030-usb.c
index 0eabb049b6a..6907d8df7a2 100644
--- a/drivers/usb/otg/twl6030-usb.c
+++ b/drivers/usb/otg/twl6030-usb.c
@@ -26,10 +26,10 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/usb/otg.h>
+#include <linux/usb/musb-omap.h>
 #include <linux/i2c/twl.h>
 #include <linux/regulator/consumer.h>
 #include <linux/err.h>
-#include <linux/notifier.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
 
@@ -100,7 +100,7 @@ struct twl6030_usb {
 
 	int			irq1;
 	int			irq2;
-	u8			linkstat;
+	enum omap_musb_vbus_id_status linkstat;
 	u8			asleep;
 	bool			irq_enabled;
 	bool			vbus_enable;
@@ -147,7 +147,7 @@ static int twl6030_phy_init(struct usb_phy *x)
 	dev  = twl->dev;
 	pdata = dev->platform_data;
 
-	if (twl->linkstat == USB_EVENT_ID)
+	if (twl->linkstat == OMAP_MUSB_ID_GROUND)
 		pdata->phy_power(twl->dev, 1, 1);
 	else
 		pdata->phy_power(twl->dev, 0, 1);
@@ -235,13 +235,13 @@ static ssize_t twl6030_usb_vbus_show(struct device *dev,
 	spin_lock_irqsave(&twl->lock, flags);
 
 	switch (twl->linkstat) {
-	case USB_EVENT_VBUS:
+	case OMAP_MUSB_VBUS_VALID:
 	       ret = snprintf(buf, PAGE_SIZE, "vbus\n");
 	       break;
-	case USB_EVENT_ID:
+	case OMAP_MUSB_ID_GROUND:
 	       ret = snprintf(buf, PAGE_SIZE, "id\n");
 	       break;
-	case USB_EVENT_NONE:
+	case OMAP_MUSB_VBUS_OFF:
 	       ret = snprintf(buf, PAGE_SIZE, "none\n");
 	       break;
 	default:
@@ -256,8 +256,7 @@ static DEVICE_ATTR(vbus, 0444, twl6030_usb_vbus_show, NULL);
 static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 {
 	struct twl6030_usb *twl = _twl;
-	struct usb_otg *otg = twl->phy.otg;
-	int status;
+	enum omap_musb_vbus_id_status status = OMAP_MUSB_UNKNOWN;
 	u8 vbus_state, hw_state;
 
 	hw_state = twl6030_readb(twl, TWL6030_MODULE_ID0, STS_HW_CONDITIONS);
@@ -268,22 +267,18 @@ static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 		if (vbus_state & VBUS_DET) {
 			regulator_enable(twl->usb3v3);
 			twl->asleep = 1;
-			status = USB_EVENT_VBUS;
-			otg->default_a = false;
-			twl->phy.state = OTG_STATE_B_IDLE;
+			status = OMAP_MUSB_VBUS_VALID;
 			twl->linkstat = status;
-			twl->phy.last_event = status;
-			atomic_notifier_call_chain(&twl->phy.notifier,
-						status, otg->gadget);
+			omap_musb_mailbox(status);
 		} else {
-			status = USB_EVENT_NONE;
-			twl->linkstat = status;
-			twl->phy.last_event = status;
-			atomic_notifier_call_chain(&twl->phy.notifier,
-						status, otg->gadget);
-			if (twl->asleep) {
-				regulator_disable(twl->usb3v3);
-				twl->asleep = 0;
+			if (twl->linkstat != OMAP_MUSB_UNKNOWN) {
+				status = OMAP_MUSB_VBUS_OFF;
+				twl->linkstat = status;
+				omap_musb_mailbox(status);
+				if (twl->asleep) {
+					regulator_disable(twl->usb3v3);
+					twl->asleep = 0;
+				}
 			}
 		}
 	}
@@ -295,8 +290,7 @@ static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 static irqreturn_t twl6030_usbotg_irq(int irq, void *_twl)
 {
 	struct twl6030_usb *twl = _twl;
-	struct usb_otg *otg = twl->phy.otg;
-	int status = USB_EVENT_NONE;
+	enum omap_musb_vbus_id_status status = OMAP_MUSB_UNKNOWN;
 	u8 hw_state;
 
 	hw_state = twl6030_readb(twl, TWL6030_MODULE_ID0, STS_HW_CONDITIONS);
@@ -307,13 +301,9 @@ static irqreturn_t twl6030_usbotg_irq(int irq, void *_twl)
 		twl->asleep = 1;
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x1, USB_ID_INT_EN_HI_CLR);
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x10, USB_ID_INT_EN_HI_SET);
-		status = USB_EVENT_ID;
-		otg->default_a = true;
-		twl->phy.state = OTG_STATE_A_IDLE;
+		status = OMAP_MUSB_ID_GROUND;
 		twl->linkstat = status;
-		twl->phy.last_event = status;
-		atomic_notifier_call_chain(&twl->phy.notifier, status,
-							otg->gadget);
+		omap_musb_mailbox(status);
 	} else  {
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x10, USB_ID_INT_EN_HI_CLR);
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x1, USB_ID_INT_EN_HI_SET);
@@ -402,20 +392,19 @@ static int __devinit twl6030_usb_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	pdata = dev->platform_data;
 
-	twl = kzalloc(sizeof *twl, GFP_KERNEL);
+	twl = devm_kzalloc(dev, sizeof *twl, GFP_KERNEL);
 	if (!twl)
 		return -ENOMEM;
 
-	otg = kzalloc(sizeof *otg, GFP_KERNEL);
-	if (!otg) {
-		kfree(twl);
+	otg = devm_kzalloc(dev, sizeof *otg, GFP_KERNEL);
+	if (!otg)
 		return -ENOMEM;
-	}
 
 	twl->dev		= &pdev->dev;
 	twl->irq1		= platform_get_irq(pdev, 0);
 	twl->irq2		= platform_get_irq(pdev, 1);
 	twl->features		= pdata->features;
+	twl->linkstat		= OMAP_MUSB_UNKNOWN;
 
 	twl->phy.dev		= twl->dev;
 	twl->phy.label		= "twl6030";
@@ -436,18 +425,14 @@ static int __devinit twl6030_usb_probe(struct platform_device *pdev)
 	err = twl6030_usb_ldo_init(twl);
 	if (err) {
 		dev_err(&pdev->dev, "ldo init failed\n");
-		kfree(otg);
-		kfree(twl);
 		return err;
 	}
-	usb_set_transceiver(&twl->phy);
+	usb_add_phy(&twl->phy, USB_PHY_TYPE_USB2);
 
 	platform_set_drvdata(pdev, twl);
 	if (device_create_file(&pdev->dev, &dev_attr_vbus))
 		dev_warn(&pdev->dev, "could not create sysfs file\n");
 
-	ATOMIC_INIT_NOTIFIER_HEAD(&twl->phy.notifier);
-
 	INIT_WORK(&twl->set_vbus_work, otg_set_vbus_work);
 
 	twl->irq_enabled = true;
@@ -458,8 +443,6 @@ static int __devinit twl6030_usb_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
 			twl->irq1, status);
 		device_remove_file(twl->dev, &dev_attr_vbus);
-		kfree(otg);
-		kfree(twl);
 		return status;
 	}
 
@@ -471,8 +454,6 @@ static int __devinit twl6030_usb_probe(struct platform_device *pdev)
 			twl->irq2, status);
 		free_irq(twl->irq1, twl);
 		device_remove_file(twl->dev, &dev_attr_vbus);
-		kfree(otg);
-		kfree(twl);
 		return status;
 	}
 
@@ -503,8 +484,6 @@ static int __exit twl6030_usb_remove(struct platform_device *pdev)
 	pdata->phy_exit(twl->dev);
 	device_remove_file(twl->dev, &dev_attr_vbus);
 	cancel_work_sync(&twl->set_vbus_work);
-	kfree(twl->phy.otg);
-	kfree(twl);
 
 	return 0;
 }
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index a165490bae4..8c9bb1ad306 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -19,7 +19,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/sysfs.h>
-#include "./common.h"
+#include "common.h"
 
 /*
  *		image of renesas_usbhs
diff --git a/drivers/usb/renesas_usbhs/common.h b/drivers/usb/renesas_usbhs/common.h
index 3f3ccd35875..dddf40a59de 100644
--- a/drivers/usb/renesas_usbhs/common.h
+++ b/drivers/usb/renesas_usbhs/common.h
@@ -22,8 +22,8 @@
 
 struct usbhs_priv;
 
-#include "./mod.h"
-#include "./pipe.h"
+#include "mod.h"
+#include "pipe.h"
 
 /*
  *
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index 6ec7f838d7f..ecd173032fd 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -17,8 +17,8 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/scatterlist.h>
-#include "./common.h"
-#include "./pipe.h"
+#include "common.h"
+#include "pipe.h"
 
 #define usbhsf_get_cfifo(p)	(&((p)->fifo_info.cfifo))
 #define usbhsf_get_d0fifo(p)	(&((p)->fifo_info.d0fifo))
@@ -771,22 +771,15 @@ static void xfer_work(struct work_struct *work)
 	struct usbhs_pipe *pipe = pkt->pipe;
 	struct usbhs_fifo *fifo = usbhs_pipe_to_fifo(pipe);
 	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
-	struct scatterlist sg;
 	struct dma_async_tx_descriptor *desc;
 	struct dma_chan *chan = usbhsf_dma_chan_get(fifo, pkt);
 	struct device *dev = usbhs_priv_to_dev(priv);
 	enum dma_transfer_direction dir;
-	dma_cookie_t cookie;
 
 	dir = usbhs_pipe_is_dir_in(pipe) ? DMA_DEV_TO_MEM : DMA_MEM_TO_DEV;
 
-	sg_init_table(&sg, 1);
-	sg_set_page(&sg, virt_to_page(pkt->dma),
-		    pkt->length, offset_in_page(pkt->dma));
-	sg_dma_address(&sg) = pkt->dma + pkt->actual;
-	sg_dma_len(&sg) = pkt->trans;
-
-	desc = dmaengine_prep_slave_sg(chan, &sg, 1, dir,
+	desc = dmaengine_prep_slave_single(chan, pkt->dma + pkt->actual,
+					pkt->trans, dir,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc)
 		return;
@@ -794,8 +787,7 @@ static void xfer_work(struct work_struct *work)
 	desc->callback		= usbhsf_dma_complete;
 	desc->callback_param	= pipe;
 
-	cookie = desc->tx_submit(desc);
-	if (cookie < 0) {
+	if (dmaengine_submit(desc) < 0) {
 		dev_err(dev, "Failed to submit dma descriptor\n");
 		return;
 	}
@@ -994,7 +986,7 @@ static bool usbhsf_dma_filter(struct dma_chan *chan, void *param)
 	 *
 	 * usbhs doesn't recognize id = 0 as valid DMA
 	 */
-	if (0 == slave->slave_id)
+	if (0 == slave->shdma_slave.slave_id)
 		return false;
 
 	chan->private = slave;
@@ -1173,8 +1165,8 @@ int usbhs_fifo_probe(struct usbhs_priv *priv)
 	fifo->port	= D0FIFO;
 	fifo->sel	= D0FIFOSEL;
 	fifo->ctr	= D0FIFOCTR;
-	fifo->tx_slave.slave_id	= usbhs_get_dparam(priv, d0_tx_id);
-	fifo->rx_slave.slave_id	= usbhs_get_dparam(priv, d0_rx_id);
+	fifo->tx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d0_tx_id);
+	fifo->rx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d0_rx_id);
 
 	/* D1FIFO */
 	fifo = usbhsf_get_d1fifo(priv);
@@ -1182,8 +1174,8 @@ int usbhs_fifo_probe(struct usbhs_priv *priv)
 	fifo->port	= D1FIFO;
 	fifo->sel	= D1FIFOSEL;
 	fifo->ctr	= D1FIFOCTR;
-	fifo->tx_slave.slave_id	= usbhs_get_dparam(priv, d1_tx_id);
-	fifo->rx_slave.slave_id	= usbhs_get_dparam(priv, d1_rx_id);
+	fifo->tx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d1_tx_id);
+	fifo->rx_slave.shdma_slave.slave_id	= usbhs_get_dparam(priv, d1_rx_id);
 
 	return 0;
 }
diff --git a/drivers/usb/renesas_usbhs/mod.c b/drivers/usb/renesas_usbhs/mod.c
index 0871e816df4..82a628f96c0 100644
--- a/drivers/usb/renesas_usbhs/mod.c
+++ b/drivers/usb/renesas_usbhs/mod.c
@@ -16,8 +16,8 @@
  */
 #include <linux/interrupt.h>
 
-#include "./common.h"
-#include "./mod.h"
+#include "common.h"
+#include "mod.h"
 
 #define usbhs_priv_to_modinfo(priv) (&priv->mod_info)
 #define usbhs_mod_info_call(priv, func, param...)	\
diff --git a/drivers/usb/renesas_usbhs/mod.h b/drivers/usb/renesas_usbhs/mod.h
index 6c6875533f0..1ef5bf60407 100644
--- a/drivers/usb/renesas_usbhs/mod.h
+++ b/drivers/usb/renesas_usbhs/mod.h
@@ -19,7 +19,7 @@
 
 #include <linux/spinlock.h>
 #include <linux/usb/renesas_usbhs.h>
-#include "./common.h"
+#include "common.h"
 
 /*
  *	struct
diff --git a/drivers/usb/renesas_usbhs/pipe.c b/drivers/usb/renesas_usbhs/pipe.c
index feb06d6d281..122526cfd32 100644
--- a/drivers/usb/renesas_usbhs/pipe.c
+++ b/drivers/usb/renesas_usbhs/pipe.c
@@ -16,8 +16,8 @@
  */
 #include <linux/delay.h>
 #include <linux/slab.h>
-#include "./common.h"
-#include "./pipe.h"
+#include "common.h"
+#include "pipe.h"
 
 /*
  *		macros
diff --git a/drivers/usb/renesas_usbhs/pipe.h b/drivers/usb/renesas_usbhs/pipe.h
index fa18b7dc2b2..08786c06dcf 100644
--- a/drivers/usb/renesas_usbhs/pipe.h
+++ b/drivers/usb/renesas_usbhs/pipe.h
@@ -17,8 +17,8 @@
 #ifndef RENESAS_USB_PIPE_H
 #define RENESAS_USB_PIPE_H
 
-#include "./common.h"
-#include "./fifo.h"
+#include "common.h"
+#include "fifo.h"
 
 /*
  *	struct
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index 6225199119c..7bcbb47e144 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -176,7 +176,7 @@ static void keyspan_set_termios(struct tty_struct *tty,
 	tty_encode_baud_rate(tty, baud_rate, baud_rate);
 	/* set CTS/RTS handshake etc. */
 	p_priv->cflag = cflag;
-	p_priv->flow_control = (cflag & CRTSCTS)? flow_cts: flow_none;
+	p_priv->flow_control = (cflag & CRTSCTS) ? flow_cts : flow_none;
 
 	/* Mark/Space not supported */
 	tty->termios.c_cflag &= ~CMSPAR;
@@ -474,7 +474,7 @@ static void usa28_indat_callback(struct urb *urb)
 		p_priv = usb_get_serial_port_data(port);
 		data = urb->transfer_buffer;
 
-		tty =tty_port_tty_get(&port->port);
+		tty = tty_port_tty_get(&port->port);
 		if (tty && urb->actual_length) {
 			tty_insert_flip_string(tty, data, urb->actual_length);
 			tty_flip_buffer_push(tty);
@@ -557,9 +557,9 @@ static void	usa28_instat_callback(struct urb *urb)
 	p_priv->dcd_state = ((msg->dcd) ? 1 : 0);
 	p_priv->ri_state = ((msg->ri) ? 1 : 0);
 
-	if( old_dcd_state != p_priv->dcd_state && old_dcd_state) {
+	if (old_dcd_state != p_priv->dcd_state && old_dcd_state) {
 		tty = tty_port_tty_get(&port->port);
-		if (tty && !C_CLOCAL(tty)) 
+		if (tty && !C_CLOCAL(tty))
 			tty_hangup(tty);
 		tty_kref_put(tty);
 	}
@@ -1036,15 +1036,12 @@ static int keyspan_write_room(struct tty_struct *tty)
 static int keyspan_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct keyspan_port_private 	*p_priv;
-	struct keyspan_serial_private 	*s_priv;
-	struct usb_serial 		*serial = port->serial;
 	const struct keyspan_device_details	*d_details;
 	int				i, err;
 	int				baud_rate, device_port;
 	struct urb			*urb;
 	unsigned int			cflag = 0;
 
-	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 	d_details = p_priv->device_details;
 
@@ -1102,7 +1099,7 @@ static int keyspan_open(struct tty_struct *tty, struct usb_serial_port *port)
 	}
 	/* set CTS/RTS handshake etc. */
 	p_priv->cflag = cflag;
-	p_priv->flow_control = (cflag & CRTSCTS)? flow_cts: flow_none;
+	p_priv->flow_control = (cflag & CRTSCTS) ? flow_cts : flow_none;
 
 	keyspan_send_setup(port, 1);
 	/* mdelay(100); */
@@ -1130,10 +1127,8 @@ static void keyspan_close(struct usb_serial_port *port)
 {
 	int			i;
 	struct usb_serial	*serial = port->serial;
-	struct keyspan_serial_private 	*s_priv;
 	struct keyspan_port_private 	*p_priv;
 
-	s_priv = usb_get_serial_data(serial);
 	p_priv = usb_get_serial_port_data(port);
 
 	p_priv->rts_state = 0;
@@ -1240,7 +1235,7 @@ static int keyspan_fake_startup(struct usb_serial *serial)
 
 	if (request_ihex_firmware(&fw, fw_name, &serial->dev->dev)) {
 		dev_err(&serial->dev->dev, "Required keyspan firmware image (%s) unavailable.\n", fw_name);
-		return(1);
+		return 1;
 	}
 
 	dbg("Uploading Keyspan %s firmware.", fw_name);
@@ -1709,7 +1704,7 @@ static int keyspan_usa26_send_setup(struct usb_serial *serial,
 		msg.setPrescaler = 0xff;
 	}
 
-	msg.lcr = (p_priv->cflag & CSTOPB)? STOPBITS_678_2: STOPBITS_5678_1;
+	msg.lcr = (p_priv->cflag & CSTOPB) ? STOPBITS_678_2 : STOPBITS_5678_1;
 	switch (p_priv->cflag & CSIZE) {
 	case CS5:
 		msg.lcr |= USA_DATABITS_5;
@@ -1726,7 +1721,7 @@ static int keyspan_usa26_send_setup(struct usb_serial *serial,
 	}
 	if (p_priv->cflag & PARENB) {
 		/* note USA_PARITY_NONE == 0 */
-		msg.lcr |= (p_priv->cflag & PARODD)?
+		msg.lcr |= (p_priv->cflag & PARODD) ?
 			USA_PARITY_ODD : USA_PARITY_EVEN;
 	}
 	msg.setLcr = 0xff;
@@ -1994,7 +1989,7 @@ static int keyspan_usa49_send_setup(struct usb_serial *serial,
 		/* msg.setPrescaler = 0xff; */
 	}
 
-	msg.lcr = (p_priv->cflag & CSTOPB)? STOPBITS_678_2: STOPBITS_5678_1;
+	msg.lcr = (p_priv->cflag & CSTOPB) ? STOPBITS_678_2 : STOPBITS_5678_1;
 	switch (p_priv->cflag & CSIZE) {
 	case CS5:
 		msg.lcr |= USA_DATABITS_5;
@@ -2011,7 +2006,7 @@ static int keyspan_usa49_send_setup(struct usb_serial *serial,
 	}
 	if (p_priv->cflag & PARENB) {
 		/* note USA_PARITY_NONE == 0 */
-		msg.lcr |= (p_priv->cflag & PARODD)?
+		msg.lcr |= (p_priv->cflag & PARODD) ?
 			USA_PARITY_ODD : USA_PARITY_EVEN;
 	}
 	msg.setLcr = 0xff;
@@ -2178,7 +2173,7 @@ static int keyspan_usa90_send_setup(struct usb_serial *serial,
 		msg.txMode = TXMODE_BYHAND;
 	}
 
-	msg.lcr = (p_priv->cflag & CSTOPB)? STOPBITS_678_2: STOPBITS_5678_1;
+	msg.lcr = (p_priv->cflag & CSTOPB) ? STOPBITS_678_2 : STOPBITS_5678_1;
 	switch (p_priv->cflag & CSIZE) {
 	case CS5:
 		msg.lcr |= USA_DATABITS_5;
@@ -2195,7 +2190,7 @@ static int keyspan_usa90_send_setup(struct usb_serial *serial,
 	}
 	if (p_priv->cflag & PARENB) {
 		/* note USA_PARITY_NONE == 0 */
-		msg.lcr |= (p_priv->cflag & PARODD)?
+		msg.lcr |= (p_priv->cflag & PARODD) ?
 			USA_PARITY_ODD : USA_PARITY_EVEN;
 	}
 	if (p_priv->old_cflag != p_priv->cflag) {
@@ -2322,7 +2317,7 @@ static int keyspan_usa67_send_setup(struct usb_serial *serial,
 	}
 	if (p_priv->cflag & PARENB) {
 		/* note USA_PARITY_NONE == 0 */
-		msg.lcr |= (p_priv->cflag & PARODD)?
+		msg.lcr |= (p_priv->cflag & PARODD) ?
 					USA_PARITY_ODD : USA_PARITY_EVEN;
 	}
 	msg.setLcr = 0xff;
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index 417ab1b0aa3..08ff9b86204 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -936,6 +936,8 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0165, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0167, 0xff, 0xff, 0xff),
 	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0326, 0xff, 0xff, 0xff),
+	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1008, 0xff, 0xff, 0xff),
 	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1010, 0xff, 0xff, 0xff),
@@ -1310,6 +1312,10 @@ static struct usb_serial_driver * const serial_drivers[] = {
 
 static bool debug;
 
+struct option_private {
+	u8 bInterfaceNumber;
+};
+
 module_usb_serial_driver(serial_drivers, option_ids);
 
 static bool is_blacklisted(const u8 ifnum, enum option_blacklist_reason reason,
@@ -1340,51 +1346,78 @@ static int option_probe(struct usb_serial *serial,
 			const struct usb_device_id *id)
 {
 	struct usb_wwan_intf_private *data;
-
-	/* D-Link DWM 652 still exposes CD-Rom emulation interface in modem mode */
-	if (serial->dev->descriptor.idVendor == DLINK_VENDOR_ID &&
-		serial->dev->descriptor.idProduct == DLINK_PRODUCT_DWM_652 &&
-		serial->interface->cur_altsetting->desc.bInterfaceClass == 0x8)
+	struct option_private *priv;
+	struct usb_interface_descriptor *iface_desc =
+				&serial->interface->cur_altsetting->desc;
+	struct usb_device_descriptor *dev_desc = &serial->dev->descriptor;
+
+	/*
+	 * D-Link DWM 652 still exposes CD-Rom emulation interface in modem
+	 * mode.
+	 */
+	if (dev_desc->idVendor == DLINK_VENDOR_ID &&
+		dev_desc->idProduct == DLINK_PRODUCT_DWM_652 &&
+		iface_desc->bInterfaceClass == 0x08)
 		return -ENODEV;
 
 	/* Bandrich modem and AT command interface is 0xff */
-	if ((serial->dev->descriptor.idVendor == BANDRICH_VENDOR_ID ||
-		serial->dev->descriptor.idVendor == PIRELLI_VENDOR_ID) &&
-		serial->interface->cur_altsetting->desc.bInterfaceClass != 0xff)
+	if ((dev_desc->idVendor == BANDRICH_VENDOR_ID ||
+		dev_desc->idVendor == PIRELLI_VENDOR_ID) &&
+		iface_desc->bInterfaceClass != 0xff)
 		return -ENODEV;
-
-	/* Don't bind reserved interfaces (like network ones) which often have
+	/*
+	 * Don't bind reserved interfaces (like network ones) which often have
 	 * the same class/subclass/protocol as the serial interfaces.  Look at
 	 * the Windows driver .INF files for reserved interface numbers.
 	 */
 	if (is_blacklisted(
-		serial->interface->cur_altsetting->desc.bInterfaceNumber,
+		iface_desc->bInterfaceNumber,
 		OPTION_BLACKLIST_RESERVED_IF,
 		(const struct option_blacklist_info *) id->driver_info))
 		return -ENODEV;
-
-	/* Don't bind network interface on Samsung GT-B3730, it is handled by a separate module */
-	if (serial->dev->descriptor.idVendor == SAMSUNG_VENDOR_ID &&
-		serial->dev->descriptor.idProduct == SAMSUNG_PRODUCT_GT_B3730 &&
-		serial->interface->cur_altsetting->desc.bInterfaceClass != USB_CLASS_CDC_DATA)
+	/*
+	 * Don't bind network interface on Samsung GT-B3730, it is handled by
+	 * a separate module.
+	 */
+	if (dev_desc->idVendor == SAMSUNG_VENDOR_ID &&
+		dev_desc->idProduct == SAMSUNG_PRODUCT_GT_B3730 &&
+		iface_desc->bInterfaceClass != USB_CLASS_CDC_DATA)
 		return -ENODEV;
 
-	data = serial->private = kzalloc(sizeof(struct usb_wwan_intf_private), GFP_KERNEL);
+	data = kzalloc(sizeof(struct usb_wwan_intf_private), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
-	data->send_setup = option_send_setup;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		kfree(data);
+		return -ENOMEM;
+	}
+
+	priv->bInterfaceNumber = iface_desc->bInterfaceNumber;
+	data->private = priv;
+
+	if (!is_blacklisted(iface_desc->bInterfaceNumber,
+			OPTION_BLACKLIST_SENDSETUP,
+			(struct option_blacklist_info *)id->driver_info)) {
+		data->send_setup = option_send_setup;
+	}
 	spin_lock_init(&data->susp_lock);
-	data->private = (void *)id->driver_info;
+
+	usb_set_serial_data(serial, data);
+
 	return 0;
 }
 
 static void option_release(struct usb_serial *serial)
 {
-	struct usb_wwan_intf_private *priv = usb_get_serial_data(serial);
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
+	struct option_private *priv = intfdata->private;
 
 	usb_wwan_release(serial);
 
 	kfree(priv);
+	kfree(intfdata);
 }
 
 static void option_instat_callback(struct urb *urb)
@@ -1451,18 +1484,11 @@ static void option_instat_callback(struct urb *urb)
 static int option_send_setup(struct usb_serial_port *port)
 {
 	struct usb_serial *serial = port->serial;
-	struct usb_wwan_intf_private *intfdata =
-		(struct usb_wwan_intf_private *) serial->private;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
+	struct option_private *priv = intfdata->private;
 	struct usb_wwan_port_private *portdata;
-	int ifNum = serial->interface->cur_altsetting->desc.bInterfaceNumber;
 	int val = 0;
 
-	if (is_blacklisted(ifNum, OPTION_BLACKLIST_SENDSETUP,
-			(struct option_blacklist_info *) intfdata->private)) {
-		dbg("No send_setup on blacklisted interface #%d\n", ifNum);
-		return -EIO;
-	}
-
 	portdata = usb_get_serial_port_data(port);
 
 	if (portdata->dtr_state)
@@ -1470,9 +1496,9 @@ static int option_send_setup(struct usb_serial_port *port)
 	if (portdata->rts_state)
 		val |= 0x02;
 
-	return usb_control_msg(serial->dev,
-		usb_rcvctrlpipe(serial->dev, 0),
-		0x22, 0x21, val, ifNum, NULL, 0, USB_CTRL_SET_TIMEOUT);
+	return usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+				0x22, 0x21, val, priv->bInterfaceNumber, NULL,
+				0, USB_CTRL_SET_TIMEOUT);
 }
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index 996015c5f1a..8d103019d6a 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -105,6 +105,10 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(0x1410, 0xa021)},	/* Novatel Gobi 3000 Composite */
 	{USB_DEVICE(0x413c, 0x8193)},	/* Dell Gobi 3000 QDL */
 	{USB_DEVICE(0x413c, 0x8194)},	/* Dell Gobi 3000 Composite */
+	{USB_DEVICE(0x1199, 0x68a4)},	/* Sierra Wireless QDL */
+	{USB_DEVICE(0x1199, 0x68a5)},	/* Sierra Wireless Modem */
+	{USB_DEVICE(0x1199, 0x68a8)},	/* Sierra Wireless QDL */
+	{USB_DEVICE(0x1199, 0x68a9)},	/* Sierra Wireless Modem */
 	{USB_DEVICE(0x1199, 0x9010)},	/* Sierra Wireless Gobi 3000 QDL */
 	{USB_DEVICE(0x1199, 0x9012)},	/* Sierra Wireless Gobi 3000 QDL */
 	{USB_DEVICE(0x1199, 0x9013)},	/* Sierra Wireless Gobi 3000 Modem device (MC8355) */
@@ -112,8 +116,24 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(0x1199, 0x9015)},	/* Sierra Wireless Gobi 3000 Modem device */
 	{USB_DEVICE(0x1199, 0x9018)},	/* Sierra Wireless Gobi 3000 QDL */
 	{USB_DEVICE(0x1199, 0x9019)},	/* Sierra Wireless Gobi 3000 Modem device */
+	{USB_DEVICE(0x1199, 0x901b)},	/* Sierra Wireless MC7770 */
 	{USB_DEVICE(0x12D1, 0x14F0)},	/* Sony Gobi 3000 QDL */
 	{USB_DEVICE(0x12D1, 0x14F1)},	/* Sony Gobi 3000 Composite */
+
+	/* non Gobi Qualcomm serial devices */
+	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 0)},	/* Sierra Wireless MC7700 Device Management */
+	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 2)},	/* Sierra Wireless MC7700 NMEA */
+	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 3)},	/* Sierra Wireless MC7700 Modem */
+	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 0)},	/* Sierra Wireless MC7750 Device Management */
+	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 2)},	/* Sierra Wireless MC7750 NMEA */
+	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 3)},	/* Sierra Wireless MC7750 Modem */
+	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 0)},	/* Sierra Wireless MC7710 Device Management */
+	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 2)},	/* Sierra Wireless MC7710 NMEA */
+	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 3)},	/* Sierra Wireless MC7710 Modem */
+	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 0)},	/* Sierra Wireless EM7700 Device Management */
+	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 2)},	/* Sierra Wireless EM7700 NMEA */
+	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 3)},	/* Sierra Wireless EM7700 Modem */
+
 	{ }				/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, id_table);
@@ -127,6 +147,7 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 	__u8 nintf;
 	__u8 ifnum;
 	bool is_gobi1k = id->driver_info ? true : false;
+	int altsetting = -1;
 
 	dev_dbg(dev, "Is Gobi 1000 = %d\n", is_gobi1k);
 
@@ -142,38 +163,43 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 
 	spin_lock_init(&data->susp_lock);
 
-	switch (nintf) {
-	case 1:
+	if (nintf == 1) {
 		/* QDL mode */
 		/* Gobi 2000 has a single altsetting, older ones have two */
 		if (serial->interface->num_altsetting == 2)
 			intf = &serial->interface->altsetting[1];
 		else if (serial->interface->num_altsetting > 2)
-			break;
+			goto done;
 
 		if (intf->desc.bNumEndpoints == 2 &&
 		    usb_endpoint_is_bulk_in(&intf->endpoint[0].desc) &&
 		    usb_endpoint_is_bulk_out(&intf->endpoint[1].desc)) {
 			dev_dbg(dev, "QDL port found\n");
 
-			if (serial->interface->num_altsetting == 1) {
+			if (serial->interface->num_altsetting == 1)
 				retval = 0; /* Success */
-				break;
-			}
-
-			retval = usb_set_interface(serial->dev, ifnum, 1);
-			if (retval < 0) {
-				dev_err(dev,
-					"Could not set interface, error %d\n",
-					retval);
-				retval = -ENODEV;
-				kfree(data);
-			}
+			else
+				altsetting = 1;
 		}
-		break;
+		goto done;
 
-	case 3:
-	case 4:
+	}
+
+	/* allow any number of interfaces when doing direct interface match */
+	if (id->match_flags & USB_DEVICE_ID_MATCH_INT_NUMBER) {
+		dev_dbg(dev, "Generic Qualcomm serial interface found\n");
+		altsetting = 0;
+		goto done;
+	}
+
+	if (nintf < 3 || nintf > 4) {
+		dev_err(dev, "unknown number of interfaces: %d\n", nintf);
+		goto done;
+	}
+
+	/* default to enabling interface */
+	altsetting = 0;
+	switch (ifnum) {
 		/* Composite mode; don't bind to the QMI/net interface as that
 		 * gets handled by other drivers.
 		 */
@@ -191,53 +217,44 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 		 * 3: NMEA
 		 */
 
-		if (ifnum == 1 && !is_gobi1k) {
+	case 1:
+		if (is_gobi1k)
+			altsetting = -1;
+		else
 			dev_dbg(dev, "Gobi 2K+ DM/DIAG interface found\n");
-			retval = usb_set_interface(serial->dev, ifnum, 0);
-			if (retval < 0) {
-				dev_err(dev,
-					"Could not set interface, error %d\n",
-					retval);
-				retval = -ENODEV;
-				kfree(data);
-			}
-		} else if (ifnum == 2) {
-			dev_dbg(dev, "Modem port found\n");
-			retval = usb_set_interface(serial->dev, ifnum, 0);
-			if (retval < 0) {
-				dev_err(dev,
-					"Could not set interface, error %d\n",
-					retval);
-				retval = -ENODEV;
-				kfree(data);
-			}
-		} else if (ifnum==3 && !is_gobi1k) {
+		break;
+	case 2:
+		dev_dbg(dev, "Modem port found\n");
+		break;
+	case 3:
+		if (is_gobi1k)
+			altsetting = -1;
+		else
 			/*
 			 * NMEA (serial line 9600 8N1)
 			 * # echo "\$GPS_START" > /dev/ttyUSBx
 			 * # echo "\$GPS_STOP"  > /dev/ttyUSBx
 			 */
 			dev_dbg(dev, "Gobi 2K+ NMEA GPS interface found\n");
-			retval = usb_set_interface(serial->dev, ifnum, 0);
-			if (retval < 0) {
-				dev_err(dev,
-					"Could not set interface, error %d\n",
-					retval);
-				retval = -ENODEV;
-				kfree(data);
-			}
-		}
-		break;
+	}
 
-	default:
-		dev_err(dev, "unknown number of interfaces: %d\n", nintf);
-		kfree(data);
-		retval = -ENODEV;
+done:
+	if (altsetting >= 0) {
+		retval = usb_set_interface(serial->dev, ifnum, altsetting);
+		if (retval < 0) {
+			dev_err(dev,
+				"Could not set interface, error %d\n",
+				retval);
+			retval = -ENODEV;
+		}
 	}
 
-	/* Set serial->private if not returning -ENODEV */
-	if (retval != -ENODEV)
+	/* Set serial->private if not returning error */
+	if (retval == 0)
 		usb_set_serial_data(serial, data);
+	else
+		kfree(data);
+
 	return retval;
 }
 
diff --git a/drivers/usb/serial/quatech2.c b/drivers/usb/serial/quatech2.c
index 7de6d491a85..7df9cdb053e 100644
--- a/drivers/usb/serial/quatech2.c
+++ b/drivers/usb/serial/quatech2.c
@@ -345,7 +345,6 @@ static void qt2_set_termios(struct tty_struct *tty,
 static int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct usb_serial *serial;
-	struct qt2_serial_private *serial_priv;
 	struct qt2_port_private *port_priv;
 	u8 *data;
 	u16 device_port;
@@ -357,7 +356,6 @@ static int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
 	serial = port->serial;
 
 	port_priv = usb_get_serial_port_data(port);
-	serial_priv = usb_get_serial_data(serial);
 
 	/* set the port to RS232 mode */
 	status = qt2_control_msg(serial->dev, QT2_GET_SET_QMCR,
@@ -417,13 +415,11 @@ static int qt2_open(struct tty_struct *tty, struct usb_serial_port *port)
 static void qt2_close(struct usb_serial_port *port)
 {
 	struct usb_serial *serial;
-	struct qt2_serial_private *serial_priv;
 	struct qt2_port_private *port_priv;
 	unsigned long flags;
 	int i;
 
 	serial = port->serial;
-	serial_priv = usb_get_serial_data(serial);
 	port_priv = usb_get_serial_port_data(port);
 
 	port_priv->is_open = false;
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index a4e4f3a16c6..b14ebbd7356 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -288,9 +288,6 @@ static const struct usb_device_id id_table[] = {
 	/* Sierra Wireless HSPA Non-Composite Device */
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x1199, 0x6892, 0xFF, 0xFF, 0xFF)},
 	{ USB_DEVICE(0x1199, 0x6893) },	/* Sierra Wireless Device */
-	{ USB_DEVICE(0x1199, 0x68A2),   /* Sierra Wireless MC77xx in QMI mode */
-	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
-	},
 	{ USB_DEVICE(0x1199, 0x68A3), 	/* Sierra Wireless Direct IP modems */
 	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
 	},
diff --git a/drivers/usb/storage/protocol.c b/drivers/usb/storage/protocol.c
index 82dd834709c..5dfb4c36a1b 100644
--- a/drivers/usb/storage/protocol.c
+++ b/drivers/usb/storage/protocol.c
@@ -66,7 +66,7 @@ void usb_stor_pad12_command(struct scsi_cmnd *srb, struct us_data *us)
 	 * NOTE: This only works because a scsi_cmnd struct field contains
 	 * a unsigned char cmnd[16], so we know we have storage available
 	 */
-	for (; srb->cmd_len<12; srb->cmd_len++)
+	for (; srb->cmd_len < 12; srb->cmd_len++)
 		srb->cmnd[srb->cmd_len] = 0;
 
 	/* send the command to the transport layer */
@@ -76,14 +76,14 @@ void usb_stor_pad12_command(struct scsi_cmnd *srb, struct us_data *us)
 void usb_stor_ufi_command(struct scsi_cmnd *srb, struct us_data *us)
 {
 	/* fix some commands -- this is a form of mode translation
-	 * UFI devices only accept 12 byte long commands 
+	 * UFI devices only accept 12 byte long commands
 	 *
 	 * NOTE: This only works because a scsi_cmnd struct field contains
 	 * a unsigned char cmnd[16], so we know we have storage available
 	 */
 
 	/* Pad the ATAPI command with zeros */
-	for (; srb->cmd_len<12; srb->cmd_len++)
+	for (; srb->cmd_len < 12; srb->cmd_len++)
 		srb->cmnd[srb->cmd_len] = 0;
 
 	/* set command length to 12 bytes (this affects the transport layer) */
diff --git a/drivers/usb/storage/scsiglue.c b/drivers/usb/storage/scsiglue.c
index 11418da9bc0..a3d54366afc 100644
--- a/drivers/usb/storage/scsiglue.c
+++ b/drivers/usb/storage/scsiglue.c
@@ -236,6 +236,11 @@ static int slave_configure(struct scsi_device *sdev)
 					US_FL_SCM_MULT_TARG)) &&
 				us->protocol == USB_PR_BULK)
 			us->use_last_sector_hacks = 1;
+
+		/* Check if write cache default on flag is set or not */
+		if (us->fflags & US_FL_WRITE_CACHE)
+			sdev->wce_default_on = 1;
+
 	} else {
 
 		/* Non-disk-type devices don't need to blacklist any pages
diff --git a/drivers/usb/storage/uas.c b/drivers/usb/storage/uas.c
index 8ec8a6e66f5..638cd64f961 100644
--- a/drivers/usb/storage/uas.c
+++ b/drivers/usb/storage/uas.c
@@ -41,16 +41,17 @@ struct sense_iu_old {
 struct uas_dev_info {
 	struct usb_interface *intf;
 	struct usb_device *udev;
-	int qdepth;
+	struct usb_anchor sense_urbs;
+	struct usb_anchor data_urbs;
+	int qdepth, resetting;
+	struct response_ui response;
 	unsigned cmd_pipe, status_pipe, data_in_pipe, data_out_pipe;
 	unsigned use_streams:1;
 	unsigned uas_sense_old:1;
 	struct scsi_cmnd *cmnd;
-	struct urb *status_urb; /* used only if stream support is available */
 };
 
 enum {
-	ALLOC_STATUS_URB	= (1 << 0),
 	SUBMIT_STATUS_URB	= (1 << 1),
 	ALLOC_DATA_IN_URB	= (1 << 2),
 	SUBMIT_DATA_IN_URB	= (1 << 3),
@@ -58,18 +59,18 @@ enum {
 	SUBMIT_DATA_OUT_URB	= (1 << 5),
 	ALLOC_CMD_URB		= (1 << 6),
 	SUBMIT_CMD_URB		= (1 << 7),
-	COMPLETED_DATA_IN	= (1 << 8),
-	COMPLETED_DATA_OUT	= (1 << 9),
-	DATA_COMPLETES_CMD	= (1 << 10),
+	COMMAND_INFLIGHT        = (1 << 8),
+	DATA_IN_URB_INFLIGHT    = (1 << 9),
+	DATA_OUT_URB_INFLIGHT   = (1 << 10),
+	COMMAND_COMPLETED       = (1 << 11),
 };
 
 /* Overrides scsi_pointer */
 struct uas_cmd_info {
 	unsigned int state;
 	unsigned int stream;
+	unsigned int aborted;
 	struct urb *cmd_urb;
-	/* status_urb is used only if stream support isn't available */
-	struct urb *status_urb;
 	struct urb *data_in_urb;
 	struct urb *data_out_urb;
 	struct list_head list;
@@ -114,7 +115,6 @@ static void uas_sense(struct urb *urb, struct scsi_cmnd *cmnd)
 {
 	struct sense_iu *sense_iu = urb->transfer_buffer;
 	struct scsi_device *sdev = cmnd->device;
-	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 
 	if (urb->actual_length > 16) {
 		unsigned len = be16_to_cpup(&sense_iu->len);
@@ -132,15 +132,12 @@ static void uas_sense(struct urb *urb, struct scsi_cmnd *cmnd)
 	}
 
 	cmnd->result = sense_iu->status;
-	if (!(cmdinfo->state & DATA_COMPLETES_CMD))
-		cmnd->scsi_done(cmnd);
 }
 
 static void uas_sense_old(struct urb *urb, struct scsi_cmnd *cmnd)
 {
 	struct sense_iu_old *sense_iu = urb->transfer_buffer;
 	struct scsi_device *sdev = cmnd->device;
-	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 
 	if (urb->actual_length > 8) {
 		unsigned len = be16_to_cpup(&sense_iu->len) - 2;
@@ -158,17 +155,51 @@ static void uas_sense_old(struct urb *urb, struct scsi_cmnd *cmnd)
 	}
 
 	cmnd->result = sense_iu->status;
-	if (!(cmdinfo->state & DATA_COMPLETES_CMD))
-		cmnd->scsi_done(cmnd);
+}
+
+static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *caller)
+{
+	struct uas_cmd_info *ci = (void *)&cmnd->SCp;
+
+	scmd_printk(KERN_INFO, cmnd, "%s %p tag %d, inflight:"
+		    "%s%s%s%s%s%s%s%s%s%s%s\n",
+		    caller, cmnd, cmnd->request->tag,
+		    (ci->state & SUBMIT_STATUS_URB)     ? " s-st"  : "",
+		    (ci->state & ALLOC_DATA_IN_URB)     ? " a-in"  : "",
+		    (ci->state & SUBMIT_DATA_IN_URB)    ? " s-in"  : "",
+		    (ci->state & ALLOC_DATA_OUT_URB)    ? " a-out" : "",
+		    (ci->state & SUBMIT_DATA_OUT_URB)   ? " s-out" : "",
+		    (ci->state & ALLOC_CMD_URB)         ? " a-cmd" : "",
+		    (ci->state & SUBMIT_CMD_URB)        ? " s-cmd" : "",
+		    (ci->state & COMMAND_INFLIGHT)      ? " CMD"   : "",
+		    (ci->state & DATA_IN_URB_INFLIGHT)  ? " IN"    : "",
+		    (ci->state & DATA_OUT_URB_INFLIGHT) ? " OUT"   : "",
+		    (ci->state & COMMAND_COMPLETED)     ? " done"  : "");
+}
+
+static int uas_try_complete(struct scsi_cmnd *cmnd, const char *caller)
+{
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
+
+	if (cmdinfo->state & (COMMAND_INFLIGHT |
+			      DATA_IN_URB_INFLIGHT |
+			      DATA_OUT_URB_INFLIGHT))
+		return -EBUSY;
+	BUG_ON(cmdinfo->state & COMMAND_COMPLETED);
+	cmdinfo->state |= COMMAND_COMPLETED;
+	usb_free_urb(cmdinfo->data_in_urb);
+	usb_free_urb(cmdinfo->data_out_urb);
+	cmnd->scsi_done(cmnd);
+	return 0;
 }
 
 static void uas_xfer_data(struct urb *urb, struct scsi_cmnd *cmnd,
-							unsigned direction)
+			  unsigned direction)
 {
 	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	int err;
 
-	cmdinfo->state = direction;
+	cmdinfo->state |= direction | SUBMIT_STATUS_URB;
 	err = uas_submit_urbs(cmnd, cmnd->device->hostdata, GFP_ATOMIC);
 	if (err) {
 		spin_lock(&uas_work_lock);
@@ -186,12 +217,15 @@ static void uas_stat_cmplt(struct urb *urb)
 	struct scsi_cmnd *cmnd;
 	struct uas_cmd_info *cmdinfo;
 	u16 tag;
-	int ret;
 
 	if (urb->status) {
 		dev_err(&urb->dev->dev, "URB BAD STATUS %d\n", urb->status);
-		if (devinfo->use_streams)
-			usb_free_urb(urb);
+		usb_free_urb(urb);
+		return;
+	}
+
+	if (devinfo->resetting) {
+		usb_free_urb(urb);
 		return;
 	}
 
@@ -201,47 +235,34 @@ static void uas_stat_cmplt(struct urb *urb)
 	else
 		cmnd = scsi_host_find_tag(shost, tag - 1);
 	if (!cmnd) {
-		if (devinfo->use_streams) {
+		if (iu->iu_id != IU_ID_RESPONSE) {
 			usb_free_urb(urb);
 			return;
 		}
-		ret = usb_submit_urb(urb, GFP_ATOMIC);
-		if (ret)
-			dev_err(&urb->dev->dev, "failed submit status urb\n");
-		return;
+	} else {
+		cmdinfo = (void *)&cmnd->SCp;
 	}
-	cmdinfo = (void *)&cmnd->SCp;
 
 	switch (iu->iu_id) {
 	case IU_ID_STATUS:
 		if (devinfo->cmnd == cmnd)
 			devinfo->cmnd = NULL;
 
-		if (!(cmdinfo->state & COMPLETED_DATA_IN) &&
-				cmdinfo->data_in_urb) {
-		       if (devinfo->use_streams) {
-			       cmdinfo->state |= DATA_COMPLETES_CMD;
-			       usb_unlink_urb(cmdinfo->data_in_urb);
-		       } else {
-			       usb_free_urb(cmdinfo->data_in_urb);
-		       }
-		}
-		if (!(cmdinfo->state & COMPLETED_DATA_OUT) &&
-				cmdinfo->data_out_urb) {
-			if (devinfo->use_streams) {
-				cmdinfo->state |= DATA_COMPLETES_CMD;
-				usb_unlink_urb(cmdinfo->data_in_urb);
-			} else {
-				usb_free_urb(cmdinfo->data_out_urb);
-			}
-		}
-
 		if (urb->actual_length < 16)
 			devinfo->uas_sense_old = 1;
 		if (devinfo->uas_sense_old)
 			uas_sense_old(urb, cmnd);
 		else
 			uas_sense(urb, cmnd);
+		if (cmnd->result != 0) {
+			/* cancel data transfers on error */
+			if (cmdinfo->state & DATA_IN_URB_INFLIGHT)
+				usb_unlink_urb(cmdinfo->data_in_urb);
+			if (cmdinfo->state & DATA_OUT_URB_INFLIGHT)
+				usb_unlink_urb(cmdinfo->data_out_urb);
+		}
+		cmdinfo->state &= ~COMMAND_INFLIGHT;
+		uas_try_complete(cmnd, __func__);
 		break;
 	case IU_ID_READ_READY:
 		uas_xfer_data(urb, cmnd, SUBMIT_DATA_IN_URB);
@@ -249,74 +270,59 @@ static void uas_stat_cmplt(struct urb *urb)
 	case IU_ID_WRITE_READY:
 		uas_xfer_data(urb, cmnd, SUBMIT_DATA_OUT_URB);
 		break;
+	case IU_ID_RESPONSE:
+		/* store results for uas_eh_task_mgmt() */
+		memcpy(&devinfo->response, iu, sizeof(devinfo->response));
+		break;
 	default:
 		scmd_printk(KERN_ERR, cmnd,
 			"Bogus IU (%d) received on status pipe\n", iu->iu_id);
 	}
-
-	if (devinfo->use_streams) {
-		usb_free_urb(urb);
-		return;
-	}
-
-	ret = usb_submit_urb(urb, GFP_ATOMIC);
-	if (ret)
-		dev_err(&urb->dev->dev, "failed submit status urb\n");
-}
-
-static void uas_data_out_cmplt(struct urb *urb)
-{
-	struct scsi_cmnd *cmnd = urb->context;
-	struct scsi_data_buffer *sdb = scsi_out(cmnd);
-	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
-
-	cmdinfo->state |= COMPLETED_DATA_OUT;
-
-	sdb->resid = sdb->length - urb->actual_length;
 	usb_free_urb(urb);
-
-	if (cmdinfo->state & DATA_COMPLETES_CMD)
-		cmnd->scsi_done(cmnd);
 }
 
-static void uas_data_in_cmplt(struct urb *urb)
+static void uas_data_cmplt(struct urb *urb)
 {
 	struct scsi_cmnd *cmnd = urb->context;
-	struct scsi_data_buffer *sdb = scsi_in(cmnd);
 	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
+	struct scsi_data_buffer *sdb = NULL;
 
-	cmdinfo->state |= COMPLETED_DATA_IN;
-
-	sdb->resid = sdb->length - urb->actual_length;
-	usb_free_urb(urb);
-
-	if (cmdinfo->state & DATA_COMPLETES_CMD)
-		cmnd->scsi_done(cmnd);
+	if (cmdinfo->data_in_urb == urb) {
+		sdb = scsi_in(cmnd);
+		cmdinfo->state &= ~DATA_IN_URB_INFLIGHT;
+	} else if (cmdinfo->data_out_urb == urb) {
+		sdb = scsi_out(cmnd);
+		cmdinfo->state &= ~DATA_OUT_URB_INFLIGHT;
+	}
+	BUG_ON(sdb == NULL);
+	if (urb->status) {
+		/* error: no data transfered */
+		sdb->resid = sdb->length;
+	} else {
+		sdb->resid = sdb->length - urb->actual_length;
+	}
+	if (cmdinfo->aborted) {
+		return;
+	}
+	uas_try_complete(cmnd, __func__);
 }
 
 static struct urb *uas_alloc_data_urb(struct uas_dev_info *devinfo, gfp_t gfp,
-		unsigned int pipe, struct scsi_cmnd *cmnd,
-		enum dma_data_direction dir)
+				      unsigned int pipe, u16 stream_id,
+				      struct scsi_cmnd *cmnd,
+				      enum dma_data_direction dir)
 {
-	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	struct usb_device *udev = devinfo->udev;
 	struct urb *urb = usb_alloc_urb(0, gfp);
-	struct scsi_data_buffer *sdb;
-	usb_complete_t complete_fn;
-	u16 stream_id = cmdinfo->stream;
+	struct scsi_data_buffer *sdb = (dir == DMA_FROM_DEVICE)
+		? scsi_in(cmnd) : scsi_out(cmnd);
 
 	if (!urb)
 		goto out;
-	if (dir == DMA_FROM_DEVICE) {
-		sdb = scsi_in(cmnd);
-		complete_fn = uas_data_in_cmplt;
-	} else {
-		sdb = scsi_out(cmnd);
-		complete_fn = uas_data_out_cmplt;
-	}
 	usb_fill_bulk_urb(urb, udev, pipe, NULL, sdb->length,
-			complete_fn, cmnd);
-	urb->stream_id = stream_id;
+			  uas_data_cmplt, cmnd);
+	if (devinfo->use_streams)
+		urb->stream_id = stream_id;
 	urb->num_sgs = udev->bus->sg_tablesize ? sdb->table.nents : 0;
 	urb->sg = sdb->table.sgl;
  out:
@@ -324,7 +330,7 @@ static struct urb *uas_alloc_data_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 }
 
 static struct urb *uas_alloc_sense_urb(struct uas_dev_info *devinfo, gfp_t gfp,
-		struct Scsi_Host *shost, u16 stream_id)
+				       struct Scsi_Host *shost, u16 stream_id)
 {
 	struct usb_device *udev = devinfo->udev;
 	struct urb *urb = usb_alloc_urb(0, gfp);
@@ -388,38 +394,95 @@ static struct urb *uas_alloc_cmd_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 	return NULL;
 }
 
+static int uas_submit_task_urb(struct scsi_cmnd *cmnd, gfp_t gfp,
+			       u8 function, u16 stream_id)
+{
+	struct uas_dev_info *devinfo = (void *)cmnd->device->hostdata;
+	struct usb_device *udev = devinfo->udev;
+	struct urb *urb = usb_alloc_urb(0, gfp);
+	struct task_mgmt_iu *iu;
+	int err = -ENOMEM;
+
+	if (!urb)
+		goto err;
+
+	iu = kzalloc(sizeof(*iu), gfp);
+	if (!iu)
+		goto err;
+
+	iu->iu_id = IU_ID_TASK_MGMT;
+	iu->tag = cpu_to_be16(stream_id);
+	int_to_scsilun(cmnd->device->lun, &iu->lun);
+
+	iu->function = function;
+	switch (function) {
+	case TMF_ABORT_TASK:
+		if (blk_rq_tagged(cmnd->request))
+			iu->task_tag = cpu_to_be16(cmnd->request->tag + 2);
+		else
+			iu->task_tag = cpu_to_be16(1);
+		break;
+	}
+
+	usb_fill_bulk_urb(urb, udev, devinfo->cmd_pipe, iu, sizeof(*iu),
+			  usb_free_urb, NULL);
+	urb->transfer_flags |= URB_FREE_BUFFER;
+
+	err = usb_submit_urb(urb, gfp);
+	if (err)
+		goto err;
+
+	return 0;
+
+err:
+	usb_free_urb(urb);
+	return err;
+}
+
 /*
  * Why should I request the Status IU before sending the Command IU?  Spec
  * says to, but also says the device may receive them in any order.  Seems
  * daft to me.
  */
 
-static int uas_submit_urbs(struct scsi_cmnd *cmnd,
-					struct uas_dev_info *devinfo, gfp_t gfp)
+static int uas_submit_sense_urb(struct Scsi_Host *shost,
+				gfp_t gfp, unsigned int stream)
 {
-	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
+	struct uas_dev_info *devinfo = (void *)shost->hostdata[0];
+	struct urb *urb;
 
-	if (cmdinfo->state & ALLOC_STATUS_URB) {
-		cmdinfo->status_urb = uas_alloc_sense_urb(devinfo, gfp,
-				cmnd->device->host, cmdinfo->stream);
-		if (!cmdinfo->status_urb)
-			return SCSI_MLQUEUE_DEVICE_BUSY;
-		cmdinfo->state &= ~ALLOC_STATUS_URB;
+	urb = uas_alloc_sense_urb(devinfo, gfp, shost, stream);
+	if (!urb)
+		return SCSI_MLQUEUE_DEVICE_BUSY;
+	if (usb_submit_urb(urb, gfp)) {
+		shost_printk(KERN_INFO, shost,
+			     "sense urb submission failure\n");
+		usb_free_urb(urb);
+		return SCSI_MLQUEUE_DEVICE_BUSY;
 	}
+	usb_anchor_urb(urb, &devinfo->sense_urbs);
+	return 0;
+}
+
+static int uas_submit_urbs(struct scsi_cmnd *cmnd,
+			   struct uas_dev_info *devinfo, gfp_t gfp)
+{
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
+	int err;
 
 	if (cmdinfo->state & SUBMIT_STATUS_URB) {
-		if (usb_submit_urb(cmdinfo->status_urb, gfp)) {
-			scmd_printk(KERN_INFO, cmnd,
-					"sense urb submission failure\n");
-			return SCSI_MLQUEUE_DEVICE_BUSY;
+		err = uas_submit_sense_urb(cmnd->device->host, gfp,
+					   cmdinfo->stream);
+		if (err) {
+			return err;
 		}
 		cmdinfo->state &= ~SUBMIT_STATUS_URB;
 	}
 
 	if (cmdinfo->state & ALLOC_DATA_IN_URB) {
 		cmdinfo->data_in_urb = uas_alloc_data_urb(devinfo, gfp,
-					devinfo->data_in_pipe, cmnd,
-					DMA_FROM_DEVICE);
+					devinfo->data_in_pipe, cmdinfo->stream,
+					cmnd, DMA_FROM_DEVICE);
 		if (!cmdinfo->data_in_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~ALLOC_DATA_IN_URB;
@@ -432,12 +495,14 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		cmdinfo->state &= ~SUBMIT_DATA_IN_URB;
+		cmdinfo->state |= DATA_IN_URB_INFLIGHT;
+		usb_anchor_urb(cmdinfo->data_in_urb, &devinfo->data_urbs);
 	}
 
 	if (cmdinfo->state & ALLOC_DATA_OUT_URB) {
 		cmdinfo->data_out_urb = uas_alloc_data_urb(devinfo, gfp,
-					devinfo->data_out_pipe, cmnd,
-					DMA_TO_DEVICE);
+					devinfo->data_out_pipe, cmdinfo->stream,
+					cmnd, DMA_TO_DEVICE);
 		if (!cmdinfo->data_out_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~ALLOC_DATA_OUT_URB;
@@ -450,6 +515,8 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		cmdinfo->state &= ~SUBMIT_DATA_OUT_URB;
+		cmdinfo->state |= DATA_OUT_URB_INFLIGHT;
+		usb_anchor_urb(cmdinfo->data_out_urb, &devinfo->data_urbs);
 	}
 
 	if (cmdinfo->state & ALLOC_CMD_URB) {
@@ -467,6 +534,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		cmdinfo->state &= ~SUBMIT_CMD_URB;
+		cmdinfo->state |= COMMAND_INFLIGHT;
 	}
 
 	return 0;
@@ -494,8 +562,9 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 
 	cmnd->scsi_done = done;
 
-	cmdinfo->state = ALLOC_STATUS_URB | SUBMIT_STATUS_URB |
+	cmdinfo->state = SUBMIT_STATUS_URB |
 			ALLOC_CMD_URB | SUBMIT_CMD_URB;
+	cmdinfo->aborted = 0;
 
 	switch (cmnd->sc_data_direction) {
 	case DMA_FROM_DEVICE:
@@ -510,8 +579,7 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 	}
 
 	if (!devinfo->use_streams) {
-		cmdinfo->state &= ~(SUBMIT_DATA_IN_URB | SUBMIT_DATA_OUT_URB |
-				ALLOC_STATUS_URB | SUBMIT_STATUS_URB);
+		cmdinfo->state &= ~(SUBMIT_DATA_IN_URB | SUBMIT_DATA_OUT_URB);
 		cmdinfo->stream = 0;
 	}
 
@@ -519,7 +587,6 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 	if (err) {
 		/* If we did nothing, give up now */
 		if (cmdinfo->state & SUBMIT_STATUS_URB) {
-			usb_free_urb(cmdinfo->status_urb);
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		spin_lock(&uas_work_lock);
@@ -533,36 +600,66 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 
 static DEF_SCSI_QCMD(uas_queuecommand)
 
-static int uas_eh_abort_handler(struct scsi_cmnd *cmnd)
+static int uas_eh_task_mgmt(struct scsi_cmnd *cmnd,
+			    const char *fname, u8 function)
 {
-	struct scsi_device *sdev = cmnd->device;
-	sdev_printk(KERN_INFO, sdev, "%s tag %d\n", __func__,
-							cmnd->request->tag);
-
-/* XXX: Send ABORT TASK Task Management command */
-	return FAILED;
+	struct Scsi_Host *shost = cmnd->device->host;
+	struct uas_dev_info *devinfo = (void *)shost->hostdata[0];
+	u16 tag = 9999; /* FIXME */
+
+	memset(&devinfo->response, 0, sizeof(devinfo->response));
+	if (uas_submit_sense_urb(shost, GFP_NOIO, tag)) {
+		shost_printk(KERN_INFO, shost,
+			     "%s: %s: submit sense urb failed\n",
+			     __func__, fname);
+		return FAILED;
+	}
+	if (uas_submit_task_urb(cmnd, GFP_NOIO, function, tag)) {
+		shost_printk(KERN_INFO, shost,
+			     "%s: %s: submit task mgmt urb failed\n",
+			     __func__, fname);
+		return FAILED;
+	}
+	if (0 == usb_wait_anchor_empty_timeout(&devinfo->sense_urbs, 3000)) {
+		shost_printk(KERN_INFO, shost,
+			     "%s: %s timed out\n", __func__, fname);
+		return FAILED;
+	}
+	if (be16_to_cpu(devinfo->response.tag) != tag) {
+		shost_printk(KERN_INFO, shost,
+			     "%s: %s failed (wrong tag %d/%d)\n", __func__,
+			     fname, be16_to_cpu(devinfo->response.tag), tag);
+		return FAILED;
+	}
+	if (devinfo->response.response_code != RC_TMF_COMPLETE) {
+		shost_printk(KERN_INFO, shost,
+			     "%s: %s failed (rc 0x%x)\n", __func__,
+			     fname, devinfo->response.response_code);
+		return FAILED;
+	}
+	return SUCCESS;
 }
 
-static int uas_eh_device_reset_handler(struct scsi_cmnd *cmnd)
+static int uas_eh_abort_handler(struct scsi_cmnd *cmnd)
 {
-	struct scsi_device *sdev = cmnd->device;
-	sdev_printk(KERN_INFO, sdev, "%s tag %d\n", __func__,
-							cmnd->request->tag);
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
+	int ret;
 
-/* XXX: Send LOGICAL UNIT RESET Task Management command */
-	return FAILED;
+	uas_log_cmd_state(cmnd, __func__);
+	cmdinfo->aborted = 1;
+	ret = uas_eh_task_mgmt(cmnd, "ABORT TASK", TMF_ABORT_TASK);
+	if (cmdinfo->state & DATA_IN_URB_INFLIGHT)
+		usb_kill_urb(cmdinfo->data_in_urb);
+	if (cmdinfo->state & DATA_OUT_URB_INFLIGHT)
+		usb_kill_urb(cmdinfo->data_out_urb);
+	return ret;
 }
 
-static int uas_eh_target_reset_handler(struct scsi_cmnd *cmnd)
+static int uas_eh_device_reset_handler(struct scsi_cmnd *cmnd)
 {
-	struct scsi_device *sdev = cmnd->device;
-	sdev_printk(KERN_INFO, sdev, "%s tag %d\n", __func__,
-							cmnd->request->tag);
-
-/* XXX: Can we reset just the one USB interface?
- * Would calling usb_set_interface() have the right effect?
- */
-	return FAILED;
+	sdev_printk(KERN_INFO, cmnd->device, "%s\n", __func__);
+	return uas_eh_task_mgmt(cmnd, "LOGICAL UNIT RESET",
+				TMF_LOGICAL_UNIT_RESET);
 }
 
 static int uas_eh_bus_reset_handler(struct scsi_cmnd *cmnd)
@@ -570,14 +667,21 @@ static int uas_eh_bus_reset_handler(struct scsi_cmnd *cmnd)
 	struct scsi_device *sdev = cmnd->device;
 	struct uas_dev_info *devinfo = sdev->hostdata;
 	struct usb_device *udev = devinfo->udev;
+	int err;
 
-	sdev_printk(KERN_INFO, sdev, "%s tag %d\n", __func__,
-							cmnd->request->tag);
+	devinfo->resetting = 1;
+	usb_kill_anchored_urbs(&devinfo->sense_urbs);
+	usb_kill_anchored_urbs(&devinfo->data_urbs);
+	err = usb_reset_device(udev);
+	devinfo->resetting = 0;
 
-	if (usb_reset_device(udev))
-		return SUCCESS;
+	if (err) {
+		shost_printk(KERN_INFO, sdev->host, "%s FAILED\n", __func__);
+		return FAILED;
+	}
 
-	return FAILED;
+	shost_printk(KERN_INFO, sdev->host, "%s success\n", __func__);
+	return SUCCESS;
 }
 
 static int uas_slave_alloc(struct scsi_device *sdev)
@@ -602,7 +706,6 @@ static struct scsi_host_template uas_host_template = {
 	.slave_configure = uas_slave_configure,
 	.eh_abort_handler = uas_eh_abort_handler,
 	.eh_device_reset_handler = uas_eh_device_reset_handler,
-	.eh_target_reset_handler = uas_eh_target_reset_handler,
 	.eh_bus_reset_handler = uas_eh_bus_reset_handler,
 	.can_queue = 65536,	/* Is there a limit on the _host_ ? */
 	.this_id = -1,
@@ -722,29 +825,6 @@ static void uas_configure_endpoints(struct uas_dev_info *devinfo)
 	}
 }
 
-static int uas_alloc_status_urb(struct uas_dev_info *devinfo,
-		struct Scsi_Host *shost)
-{
-	if (devinfo->use_streams) {
-		devinfo->status_urb = NULL;
-		return 0;
-	}
-
-	devinfo->status_urb = uas_alloc_sense_urb(devinfo, GFP_KERNEL,
-			shost, 0);
-	if (!devinfo->status_urb)
-		goto err_s_urb;
-
-	if (usb_submit_urb(devinfo->status_urb, GFP_KERNEL))
-		goto err_submit_urb;
-
-	return 0;
-err_submit_urb:
-	usb_free_urb(devinfo->status_urb);
-err_s_urb:
-	return -ENOMEM;
-}
-
 static void uas_free_streams(struct uas_dev_info *devinfo)
 {
 	struct usb_device *udev = devinfo->udev;
@@ -787,6 +867,9 @@ static int uas_probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	devinfo->intf = intf;
 	devinfo->udev = udev;
+	devinfo->resetting = 0;
+	init_usb_anchor(&devinfo->sense_urbs);
+	init_usb_anchor(&devinfo->data_urbs);
 	uas_configure_endpoints(devinfo);
 
 	result = scsi_init_shared_tag_map(shost, devinfo->qdepth - 2);
@@ -799,17 +882,10 @@ static int uas_probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	shost->hostdata[0] = (unsigned long)devinfo;
 
-	result = uas_alloc_status_urb(devinfo, shost);
-	if (result)
-		goto err_alloc_status;
-
 	scsi_scan_host(shost);
 	usb_set_intfdata(intf, shost);
 	return result;
 
-err_alloc_status:
-	scsi_remove_host(shost);
-	shost = NULL;
 deconfig_eps:
 	uas_free_streams(devinfo);
  free:
@@ -837,8 +913,8 @@ static void uas_disconnect(struct usb_interface *intf)
 	struct uas_dev_info *devinfo = (void *)shost->hostdata[0];
 
 	scsi_remove_host(shost);
-	usb_kill_urb(devinfo->status_urb);
-	usb_free_urb(devinfo->status_urb);
+	usb_kill_anchored_urbs(&devinfo->sense_urbs);
+	usb_kill_anchored_urbs(&devinfo->data_urbs);
 	uas_free_streams(devinfo);
 	kfree(devinfo);
 }
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index 1719886bb9b..62a31bea063 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -1267,6 +1267,12 @@ UNUSUAL_DEV( 0x0af0, 0xd357, 0x0000, 0x0000,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		0 ),
 
+/* Reported by Namjae Jeon <namjae.jeon@samsung.com> */
+UNUSUAL_DEV(0x0bc2, 0x2300, 0x0000, 0x9999,
+		"Seagate",
+		"Portable HDD",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL, US_FL_WRITE_CACHE),
+
 /* Reported by Ben Efros <ben@pc-doctor.com> */
 UNUSUAL_DEV( 0x0bc2, 0x3010, 0x0000, 0x0000,
 		"Seagate",
@@ -1468,6 +1474,12 @@ UNUSUAL_DEV(  0x1058, 0x0704, 0x0000, 0x9999,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_SANE_SENSE),
 
+/* Reported by Namjae Jeon <namjae.jeon@samsung.com> */
+UNUSUAL_DEV(0x1058, 0x070a, 0x0000, 0x9999,
+		"Western Digital",
+		"My Passport HDD",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL, US_FL_WRITE_CACHE),
+
 /* Reported by Fabio Venturi <f.venturi@tdnet.it>
  * The device reports a vendor-specific bDeviceClass.
  */
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index e23c30ab66d..d012fe4329e 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -473,7 +473,7 @@ static void adjust_quirks(struct us_data *us)
 			US_FL_CAPACITY_OK | US_FL_IGNORE_RESIDUE |
 			US_FL_SINGLE_LUN | US_FL_NO_WP_DETECT |
 			US_FL_NO_READ_DISC_INFO | US_FL_NO_READ_CAPACITY_16 |
-			US_FL_INITIAL_READ10);
+			US_FL_INITIAL_READ10 | US_FL_WRITE_CACHE);
 
 	p = quirks;
 	while (*p) {
@@ -529,6 +529,9 @@ static void adjust_quirks(struct us_data *us)
 		case 'o':
 			f |= US_FL_CAPACITY_OK;
 			break;
+		case 'p':
+			f |= US_FL_WRITE_CACHE;
+			break;
 		case 'r':
 			f |= US_FL_IGNORE_RESIDUE;
 			break;
diff --git a/drivers/vfio/Kconfig b/drivers/vfio/Kconfig
new file mode 100644
index 00000000000..7cd5dec0abd
--- /dev/null
+++ b/drivers/vfio/Kconfig
@@ -0,0 +1,16 @@
+config VFIO_IOMMU_TYPE1
+	tristate
+	depends on VFIO
+	default n
+
+menuconfig VFIO
+	tristate "VFIO Non-Privileged userspace driver framework"
+	depends on IOMMU_API
+	select VFIO_IOMMU_TYPE1 if X86
+	help
+	  VFIO provides a framework for secure userspace device drivers.
+	  See Documentation/vfio.txt for more details.
+
+	  If you don't know what to do here, say N.
+
+source "drivers/vfio/pci/Kconfig"
diff --git a/drivers/vfio/Makefile b/drivers/vfio/Makefile
new file mode 100644
index 00000000000..2398d4a0e38
--- /dev/null
+++ b/drivers/vfio/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VFIO) += vfio.o
+obj-$(CONFIG_VFIO_IOMMU_TYPE1) += vfio_iommu_type1.o
+obj-$(CONFIG_VFIO_PCI) += pci/
diff --git a/drivers/vfio/pci/Kconfig b/drivers/vfio/pci/Kconfig
new file mode 100644
index 00000000000..5980758563e
--- /dev/null
+++ b/drivers/vfio/pci/Kconfig
@@ -0,0 +1,8 @@
+config VFIO_PCI
+	tristate "VFIO support for PCI devices"
+	depends on VFIO && PCI && EVENTFD
+	help
+	  Support for the PCI VFIO bus driver.  This is required to make
+	  use of PCI drivers using the VFIO framework.
+
+	  If you don't know what to do here, say N.
diff --git a/drivers/vfio/pci/Makefile b/drivers/vfio/pci/Makefile
new file mode 100644
index 00000000000..131079255fd
--- /dev/null
+++ b/drivers/vfio/pci/Makefile
@@ -0,0 +1,4 @@
+
+vfio-pci-y := vfio_pci.o vfio_pci_intrs.o vfio_pci_rdwr.o vfio_pci_config.o
+
+obj-$(CONFIG_VFIO_PCI) += vfio-pci.o
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
new file mode 100644
index 00000000000..6968b723223
--- /dev/null
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -0,0 +1,579 @@
+/*
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ */
+
+#include <linux/device.h>
+#include <linux/eventfd.h>
+#include <linux/interrupt.h>
+#include <linux/iommu.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/notifier.h>
+#include <linux/pci.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/vfio.h>
+
+#include "vfio_pci_private.h"
+
+#define DRIVER_VERSION  "0.2"
+#define DRIVER_AUTHOR   "Alex Williamson <alex.williamson@redhat.com>"
+#define DRIVER_DESC     "VFIO PCI - User Level meta-driver"
+
+static bool nointxmask;
+module_param_named(nointxmask, nointxmask, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(nointxmask,
+		  "Disable support for PCI 2.3 style INTx masking.  If this resolves problems for specific devices, report lspci -vvvxxx to linux-pci@vger.kernel.org so the device can be fixed automatically via the broken_intx_masking flag.");
+
+static int vfio_pci_enable(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int ret;
+	u16 cmd;
+	u8 msix_pos;
+
+	vdev->reset_works = (pci_reset_function(pdev) == 0);
+	pci_save_state(pdev);
+	vdev->pci_saved_state = pci_store_saved_state(pdev);
+	if (!vdev->pci_saved_state)
+		pr_debug("%s: Couldn't store %s saved state\n",
+			 __func__, dev_name(&pdev->dev));
+
+	ret = vfio_config_init(vdev);
+	if (ret)
+		goto out;
+
+	if (likely(!nointxmask))
+		vdev->pci_2_3 = pci_intx_mask_supported(pdev);
+
+	pci_read_config_word(pdev, PCI_COMMAND, &cmd);
+	if (vdev->pci_2_3 && (cmd & PCI_COMMAND_INTX_DISABLE)) {
+		cmd &= ~PCI_COMMAND_INTX_DISABLE;
+		pci_write_config_word(pdev, PCI_COMMAND, cmd);
+	}
+
+	msix_pos = pci_find_capability(pdev, PCI_CAP_ID_MSIX);
+	if (msix_pos) {
+		u16 flags;
+		u32 table;
+
+		pci_read_config_word(pdev, msix_pos + PCI_MSIX_FLAGS, &flags);
+		pci_read_config_dword(pdev, msix_pos + PCI_MSIX_TABLE, &table);
+
+		vdev->msix_bar = table & PCI_MSIX_FLAGS_BIRMASK;
+		vdev->msix_offset = table & ~PCI_MSIX_FLAGS_BIRMASK;
+		vdev->msix_size = ((flags & PCI_MSIX_FLAGS_QSIZE) + 1) * 16;
+	} else
+		vdev->msix_bar = 0xFF;
+
+	ret = pci_enable_device(pdev);
+	if (ret)
+		goto out;
+
+	return ret;
+
+out:
+	kfree(vdev->pci_saved_state);
+	vdev->pci_saved_state = NULL;
+	vfio_config_free(vdev);
+	return ret;
+}
+
+static void vfio_pci_disable(struct vfio_pci_device *vdev)
+{
+	int bar;
+
+	pci_disable_device(vdev->pdev);
+
+	vfio_pci_set_irqs_ioctl(vdev, VFIO_IRQ_SET_DATA_NONE |
+				VFIO_IRQ_SET_ACTION_TRIGGER,
+				vdev->irq_type, 0, 0, NULL);
+
+	vdev->virq_disabled = false;
+
+	vfio_config_free(vdev);
+
+	pci_reset_function(vdev->pdev);
+
+	if (pci_load_and_free_saved_state(vdev->pdev,
+					  &vdev->pci_saved_state) == 0)
+		pci_restore_state(vdev->pdev);
+	else
+		pr_info("%s: Couldn't reload %s saved state\n",
+			__func__, dev_name(&vdev->pdev->dev));
+
+	for (bar = PCI_STD_RESOURCES; bar <= PCI_STD_RESOURCE_END; bar++) {
+		if (!vdev->barmap[bar])
+			continue;
+		pci_iounmap(vdev->pdev, vdev->barmap[bar]);
+		pci_release_selected_regions(vdev->pdev, 1 << bar);
+		vdev->barmap[bar] = NULL;
+	}
+}
+
+static void vfio_pci_release(void *device_data)
+{
+	struct vfio_pci_device *vdev = device_data;
+
+	if (atomic_dec_and_test(&vdev->refcnt))
+		vfio_pci_disable(vdev);
+
+	module_put(THIS_MODULE);
+}
+
+static int vfio_pci_open(void *device_data)
+{
+	struct vfio_pci_device *vdev = device_data;
+
+	if (!try_module_get(THIS_MODULE))
+		return -ENODEV;
+
+	if (atomic_inc_return(&vdev->refcnt) == 1) {
+		int ret = vfio_pci_enable(vdev);
+		if (ret) {
+			module_put(THIS_MODULE);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int vfio_pci_get_irq_count(struct vfio_pci_device *vdev, int irq_type)
+{
+	if (irq_type == VFIO_PCI_INTX_IRQ_INDEX) {
+		u8 pin;
+		pci_read_config_byte(vdev->pdev, PCI_INTERRUPT_PIN, &pin);
+		if (pin)
+			return 1;
+
+	} else if (irq_type == VFIO_PCI_MSI_IRQ_INDEX) {
+		u8 pos;
+		u16 flags;
+
+		pos = pci_find_capability(vdev->pdev, PCI_CAP_ID_MSI);
+		if (pos) {
+			pci_read_config_word(vdev->pdev,
+					     pos + PCI_MSI_FLAGS, &flags);
+
+			return 1 << (flags & PCI_MSI_FLAGS_QMASK);
+		}
+	} else if (irq_type == VFIO_PCI_MSIX_IRQ_INDEX) {
+		u8 pos;
+		u16 flags;
+
+		pos = pci_find_capability(vdev->pdev, PCI_CAP_ID_MSIX);
+		if (pos) {
+			pci_read_config_word(vdev->pdev,
+					     pos + PCI_MSIX_FLAGS, &flags);
+
+			return (flags & PCI_MSIX_FLAGS_QSIZE) + 1;
+		}
+	}
+
+	return 0;
+}
+
+static long vfio_pci_ioctl(void *device_data,
+			   unsigned int cmd, unsigned long arg)
+{
+	struct vfio_pci_device *vdev = device_data;
+	unsigned long minsz;
+
+	if (cmd == VFIO_DEVICE_GET_INFO) {
+		struct vfio_device_info info;
+
+		minsz = offsetofend(struct vfio_device_info, num_irqs);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz)
+			return -EINVAL;
+
+		info.flags = VFIO_DEVICE_FLAGS_PCI;
+
+		if (vdev->reset_works)
+			info.flags |= VFIO_DEVICE_FLAGS_RESET;
+
+		info.num_regions = VFIO_PCI_NUM_REGIONS;
+		info.num_irqs = VFIO_PCI_NUM_IRQS;
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_DEVICE_GET_REGION_INFO) {
+		struct pci_dev *pdev = vdev->pdev;
+		struct vfio_region_info info;
+
+		minsz = offsetofend(struct vfio_region_info, offset);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz)
+			return -EINVAL;
+
+		switch (info.index) {
+		case VFIO_PCI_CONFIG_REGION_INDEX:
+			info.offset = VFIO_PCI_INDEX_TO_OFFSET(info.index);
+			info.size = pdev->cfg_size;
+			info.flags = VFIO_REGION_INFO_FLAG_READ |
+				     VFIO_REGION_INFO_FLAG_WRITE;
+			break;
+		case VFIO_PCI_BAR0_REGION_INDEX ... VFIO_PCI_BAR5_REGION_INDEX:
+			info.offset = VFIO_PCI_INDEX_TO_OFFSET(info.index);
+			info.size = pci_resource_len(pdev, info.index);
+			if (!info.size) {
+				info.flags = 0;
+				break;
+			}
+
+			info.flags = VFIO_REGION_INFO_FLAG_READ |
+				     VFIO_REGION_INFO_FLAG_WRITE;
+			if (pci_resource_flags(pdev, info.index) &
+			    IORESOURCE_MEM && info.size >= PAGE_SIZE)
+				info.flags |= VFIO_REGION_INFO_FLAG_MMAP;
+			break;
+		case VFIO_PCI_ROM_REGION_INDEX:
+		{
+			void __iomem *io;
+			size_t size;
+
+			info.offset = VFIO_PCI_INDEX_TO_OFFSET(info.index);
+			info.flags = 0;
+
+			/* Report the BAR size, not the ROM size */
+			info.size = pci_resource_len(pdev, info.index);
+			if (!info.size)
+				break;
+
+			/* Is it really there? */
+			io = pci_map_rom(pdev, &size);
+			if (!io || !size) {
+				info.size = 0;
+				break;
+			}
+			pci_unmap_rom(pdev, io);
+
+			info.flags = VFIO_REGION_INFO_FLAG_READ;
+			break;
+		}
+		default:
+			return -EINVAL;
+		}
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_DEVICE_GET_IRQ_INFO) {
+		struct vfio_irq_info info;
+
+		minsz = offsetofend(struct vfio_irq_info, count);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz || info.index >= VFIO_PCI_NUM_IRQS)
+			return -EINVAL;
+
+		info.flags = VFIO_IRQ_INFO_EVENTFD;
+
+		info.count = vfio_pci_get_irq_count(vdev, info.index);
+
+		if (info.index == VFIO_PCI_INTX_IRQ_INDEX)
+			info.flags |= (VFIO_IRQ_INFO_MASKABLE |
+				       VFIO_IRQ_INFO_AUTOMASKED);
+		else
+			info.flags |= VFIO_IRQ_INFO_NORESIZE;
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_DEVICE_SET_IRQS) {
+		struct vfio_irq_set hdr;
+		u8 *data = NULL;
+		int ret = 0;
+
+		minsz = offsetofend(struct vfio_irq_set, count);
+
+		if (copy_from_user(&hdr, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (hdr.argsz < minsz || hdr.index >= VFIO_PCI_NUM_IRQS ||
+		    hdr.flags & ~(VFIO_IRQ_SET_DATA_TYPE_MASK |
+				  VFIO_IRQ_SET_ACTION_TYPE_MASK))
+			return -EINVAL;
+
+		if (!(hdr.flags & VFIO_IRQ_SET_DATA_NONE)) {
+			size_t size;
+
+			if (hdr.flags & VFIO_IRQ_SET_DATA_BOOL)
+				size = sizeof(uint8_t);
+			else if (hdr.flags & VFIO_IRQ_SET_DATA_EVENTFD)
+				size = sizeof(int32_t);
+			else
+				return -EINVAL;
+
+			if (hdr.argsz - minsz < hdr.count * size ||
+			    hdr.count > vfio_pci_get_irq_count(vdev, hdr.index))
+				return -EINVAL;
+
+			data = kmalloc(hdr.count * size, GFP_KERNEL);
+			if (!data)
+				return -ENOMEM;
+
+			if (copy_from_user(data, (void __user *)(arg + minsz),
+					   hdr.count * size)) {
+				kfree(data);
+				return -EFAULT;
+			}
+		}
+
+		mutex_lock(&vdev->igate);
+
+		ret = vfio_pci_set_irqs_ioctl(vdev, hdr.flags, hdr.index,
+					      hdr.start, hdr.count, data);
+
+		mutex_unlock(&vdev->igate);
+		kfree(data);
+
+		return ret;
+
+	} else if (cmd == VFIO_DEVICE_RESET)
+		return vdev->reset_works ?
+			pci_reset_function(vdev->pdev) : -EINVAL;
+
+	return -ENOTTY;
+}
+
+static ssize_t vfio_pci_read(void *device_data, char __user *buf,
+			     size_t count, loff_t *ppos)
+{
+	unsigned int index = VFIO_PCI_OFFSET_TO_INDEX(*ppos);
+	struct vfio_pci_device *vdev = device_data;
+	struct pci_dev *pdev = vdev->pdev;
+
+	if (index >= VFIO_PCI_NUM_REGIONS)
+		return -EINVAL;
+
+	if (index == VFIO_PCI_CONFIG_REGION_INDEX)
+		return vfio_pci_config_readwrite(vdev, buf, count, ppos, false);
+	else if (index == VFIO_PCI_ROM_REGION_INDEX)
+		return vfio_pci_mem_readwrite(vdev, buf, count, ppos, false);
+	else if (pci_resource_flags(pdev, index) & IORESOURCE_IO)
+		return vfio_pci_io_readwrite(vdev, buf, count, ppos, false);
+	else if (pci_resource_flags(pdev, index) & IORESOURCE_MEM)
+		return vfio_pci_mem_readwrite(vdev, buf, count, ppos, false);
+
+	return -EINVAL;
+}
+
+static ssize_t vfio_pci_write(void *device_data, const char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	unsigned int index = VFIO_PCI_OFFSET_TO_INDEX(*ppos);
+	struct vfio_pci_device *vdev = device_data;
+	struct pci_dev *pdev = vdev->pdev;
+
+	if (index >= VFIO_PCI_NUM_REGIONS)
+		return -EINVAL;
+
+	if (index == VFIO_PCI_CONFIG_REGION_INDEX)
+		return vfio_pci_config_readwrite(vdev, (char __user *)buf,
+						 count, ppos, true);
+	else if (index == VFIO_PCI_ROM_REGION_INDEX)
+		return -EINVAL;
+	else if (pci_resource_flags(pdev, index) & IORESOURCE_IO)
+		return vfio_pci_io_readwrite(vdev, (char __user *)buf,
+					     count, ppos, true);
+	else if (pci_resource_flags(pdev, index) & IORESOURCE_MEM) {
+		return vfio_pci_mem_readwrite(vdev, (char __user *)buf,
+					      count, ppos, true);
+	}
+
+	return -EINVAL;
+}
+
+static int vfio_pci_mmap(void *device_data, struct vm_area_struct *vma)
+{
+	struct vfio_pci_device *vdev = device_data;
+	struct pci_dev *pdev = vdev->pdev;
+	unsigned int index;
+	u64 phys_len, req_len, pgoff, req_start, phys;
+	int ret;
+
+	index = vma->vm_pgoff >> (VFIO_PCI_OFFSET_SHIFT - PAGE_SHIFT);
+
+	if (vma->vm_end < vma->vm_start)
+		return -EINVAL;
+	if ((vma->vm_flags & VM_SHARED) == 0)
+		return -EINVAL;
+	if (index >= VFIO_PCI_ROM_REGION_INDEX)
+		return -EINVAL;
+	if (!(pci_resource_flags(pdev, index) & IORESOURCE_MEM))
+		return -EINVAL;
+
+	phys_len = pci_resource_len(pdev, index);
+	req_len = vma->vm_end - vma->vm_start;
+	pgoff = vma->vm_pgoff &
+		((1U << (VFIO_PCI_OFFSET_SHIFT - PAGE_SHIFT)) - 1);
+	req_start = pgoff << PAGE_SHIFT;
+
+	if (phys_len < PAGE_SIZE || req_start + req_len > phys_len)
+		return -EINVAL;
+
+	if (index == vdev->msix_bar) {
+		/*
+		 * Disallow mmaps overlapping the MSI-X table; users don't
+		 * get to touch this directly.  We could find somewhere
+		 * else to map the overlap, but page granularity is only
+		 * a recommendation, not a requirement, so the user needs
+		 * to know which bits are real.  Requiring them to mmap
+		 * around the table makes that clear.
+		 */
+
+		/* If neither entirely above nor below, then it overlaps */
+		if (!(req_start >= vdev->msix_offset + vdev->msix_size ||
+		      req_start + req_len <= vdev->msix_offset))
+			return -EINVAL;
+	}
+
+	/*
+	 * Even though we don't make use of the barmap for the mmap,
+	 * we need to request the region and the barmap tracks that.
+	 */
+	if (!vdev->barmap[index]) {
+		ret = pci_request_selected_regions(pdev,
+						   1 << index, "vfio-pci");
+		if (ret)
+			return ret;
+
+		vdev->barmap[index] = pci_iomap(pdev, index, 0);
+	}
+
+	vma->vm_private_data = vdev;
+	vma->vm_flags |= (VM_IO | VM_RESERVED);
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	phys = (pci_resource_start(pdev, index) >> PAGE_SHIFT) + pgoff;
+
+	return remap_pfn_range(vma, vma->vm_start, phys,
+			       req_len, vma->vm_page_prot);
+}
+
+static const struct vfio_device_ops vfio_pci_ops = {
+	.name		= "vfio-pci",
+	.open		= vfio_pci_open,
+	.release	= vfio_pci_release,
+	.ioctl		= vfio_pci_ioctl,
+	.read		= vfio_pci_read,
+	.write		= vfio_pci_write,
+	.mmap		= vfio_pci_mmap,
+};
+
+static int vfio_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	u8 type;
+	struct vfio_pci_device *vdev;
+	struct iommu_group *group;
+	int ret;
+
+	pci_read_config_byte(pdev, PCI_HEADER_TYPE, &type);
+	if ((type & PCI_HEADER_TYPE) != PCI_HEADER_TYPE_NORMAL)
+		return -EINVAL;
+
+	group = iommu_group_get(&pdev->dev);
+	if (!group)
+		return -EINVAL;
+
+	vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
+	if (!vdev) {
+		iommu_group_put(group);
+		return -ENOMEM;
+	}
+
+	vdev->pdev = pdev;
+	vdev->irq_type = VFIO_PCI_NUM_IRQS;
+	mutex_init(&vdev->igate);
+	spin_lock_init(&vdev->irqlock);
+	atomic_set(&vdev->refcnt, 0);
+
+	ret = vfio_add_group_dev(&pdev->dev, &vfio_pci_ops, vdev);
+	if (ret) {
+		iommu_group_put(group);
+		kfree(vdev);
+	}
+
+	return ret;
+}
+
+static void vfio_pci_remove(struct pci_dev *pdev)
+{
+	struct vfio_pci_device *vdev;
+
+	vdev = vfio_del_group_dev(&pdev->dev);
+	if (!vdev)
+		return;
+
+	iommu_group_put(pdev->dev.iommu_group);
+	kfree(vdev);
+}
+
+static struct pci_driver vfio_pci_driver = {
+	.name		= "vfio-pci",
+	.id_table	= NULL, /* only dynamic ids */
+	.probe		= vfio_pci_probe,
+	.remove		= vfio_pci_remove,
+};
+
+static void __exit vfio_pci_cleanup(void)
+{
+	pci_unregister_driver(&vfio_pci_driver);
+	vfio_pci_virqfd_exit();
+	vfio_pci_uninit_perm_bits();
+}
+
+static int __init vfio_pci_init(void)
+{
+	int ret;
+
+	/* Allocate shared config space permision data used by all devices */
+	ret = vfio_pci_init_perm_bits();
+	if (ret)
+		return ret;
+
+	/* Start the virqfd cleanup handler */
+	ret = vfio_pci_virqfd_init();
+	if (ret)
+		goto out_virqfd;
+
+	/* Register and scan for devices */
+	ret = pci_register_driver(&vfio_pci_driver);
+	if (ret)
+		goto out_driver;
+
+	return 0;
+
+out_virqfd:
+	vfio_pci_virqfd_exit();
+out_driver:
+	vfio_pci_uninit_perm_bits();
+	return ret;
+}
+
+module_init(vfio_pci_init);
+module_exit(vfio_pci_cleanup);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vfio/pci/vfio_pci_config.c b/drivers/vfio/pci/vfio_pci_config.c
new file mode 100644
index 00000000000..8b8f7d11e10
--- /dev/null
+++ b/drivers/vfio/pci/vfio_pci_config.c
@@ -0,0 +1,1540 @@
+/*
+ * VFIO PCI config space virtualization
+ *
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ */
+
+/*
+ * This code handles reading and writing of PCI configuration registers.
+ * This is hairy because we want to allow a lot of flexibility to the
+ * user driver, but cannot trust it with all of the config fields.
+ * Tables determine which fields can be read and written, as well as
+ * which fields are 'virtualized' - special actions and translations to
+ * make it appear to the user that he has control, when in fact things
+ * must be negotiated with the underlying OS.
+ */
+
+#include <linux/fs.h>
+#include <linux/pci.h>
+#include <linux/uaccess.h>
+#include <linux/vfio.h>
+
+#include "vfio_pci_private.h"
+
+#define PCI_CFG_SPACE_SIZE	256
+
+/* Useful "pseudo" capabilities */
+#define PCI_CAP_ID_BASIC	0
+#define PCI_CAP_ID_INVALID	0xFF
+
+#define is_bar(offset)	\
+	((offset >= PCI_BASE_ADDRESS_0 && offset < PCI_BASE_ADDRESS_5 + 4) || \
+	 (offset >= PCI_ROM_ADDRESS && offset < PCI_ROM_ADDRESS + 4))
+
+/*
+ * Lengths of PCI Config Capabilities
+ *   0: Removed from the user visible capability list
+ *   FF: Variable length
+ */
+static u8 pci_cap_length[] = {
+	[PCI_CAP_ID_BASIC]	= PCI_STD_HEADER_SIZEOF, /* pci config header */
+	[PCI_CAP_ID_PM]		= PCI_PM_SIZEOF,
+	[PCI_CAP_ID_AGP]	= PCI_AGP_SIZEOF,
+	[PCI_CAP_ID_VPD]	= PCI_CAP_VPD_SIZEOF,
+	[PCI_CAP_ID_SLOTID]	= 0,		/* bridge - don't care */
+	[PCI_CAP_ID_MSI]	= 0xFF,		/* 10, 14, 20, or 24 */
+	[PCI_CAP_ID_CHSWP]	= 0,		/* cpci - not yet */
+	[PCI_CAP_ID_PCIX]	= 0xFF,		/* 8 or 24 */
+	[PCI_CAP_ID_HT]		= 0xFF,		/* hypertransport */
+	[PCI_CAP_ID_VNDR]	= 0xFF,		/* variable */
+	[PCI_CAP_ID_DBG]	= 0,		/* debug - don't care */
+	[PCI_CAP_ID_CCRC]	= 0,		/* cpci - not yet */
+	[PCI_CAP_ID_SHPC]	= 0,		/* hotswap - not yet */
+	[PCI_CAP_ID_SSVID]	= 0,		/* bridge - don't care */
+	[PCI_CAP_ID_AGP3]	= 0,		/* AGP8x - not yet */
+	[PCI_CAP_ID_SECDEV]	= 0,		/* secure device not yet */
+	[PCI_CAP_ID_EXP]	= 0xFF,		/* 20 or 44 */
+	[PCI_CAP_ID_MSIX]	= PCI_CAP_MSIX_SIZEOF,
+	[PCI_CAP_ID_SATA]	= 0xFF,
+	[PCI_CAP_ID_AF]		= PCI_CAP_AF_SIZEOF,
+};
+
+/*
+ * Lengths of PCIe/PCI-X Extended Config Capabilities
+ *   0: Removed or masked from the user visible capabilty list
+ *   FF: Variable length
+ */
+static u16 pci_ext_cap_length[] = {
+	[PCI_EXT_CAP_ID_ERR]	=	PCI_ERR_ROOT_COMMAND,
+	[PCI_EXT_CAP_ID_VC]	=	0xFF,
+	[PCI_EXT_CAP_ID_DSN]	=	PCI_EXT_CAP_DSN_SIZEOF,
+	[PCI_EXT_CAP_ID_PWR]	=	PCI_EXT_CAP_PWR_SIZEOF,
+	[PCI_EXT_CAP_ID_RCLD]	=	0,	/* root only - don't care */
+	[PCI_EXT_CAP_ID_RCILC]	=	0,	/* root only - don't care */
+	[PCI_EXT_CAP_ID_RCEC]	=	0,	/* root only - don't care */
+	[PCI_EXT_CAP_ID_MFVC]	=	0xFF,
+	[PCI_EXT_CAP_ID_VC9]	=	0xFF,	/* same as CAP_ID_VC */
+	[PCI_EXT_CAP_ID_RCRB]	=	0,	/* root only - don't care */
+	[PCI_EXT_CAP_ID_VNDR]	=	0xFF,
+	[PCI_EXT_CAP_ID_CAC]	=	0,	/* obsolete */
+	[PCI_EXT_CAP_ID_ACS]	=	0xFF,
+	[PCI_EXT_CAP_ID_ARI]	=	PCI_EXT_CAP_ARI_SIZEOF,
+	[PCI_EXT_CAP_ID_ATS]	=	PCI_EXT_CAP_ATS_SIZEOF,
+	[PCI_EXT_CAP_ID_SRIOV]	=	PCI_EXT_CAP_SRIOV_SIZEOF,
+	[PCI_EXT_CAP_ID_MRIOV]	=	0,	/* not yet */
+	[PCI_EXT_CAP_ID_MCAST]	=	PCI_EXT_CAP_MCAST_ENDPOINT_SIZEOF,
+	[PCI_EXT_CAP_ID_PRI]	=	PCI_EXT_CAP_PRI_SIZEOF,
+	[PCI_EXT_CAP_ID_AMD_XXX] =	0,	/* not yet */
+	[PCI_EXT_CAP_ID_REBAR]	=	0xFF,
+	[PCI_EXT_CAP_ID_DPA]	=	0xFF,
+	[PCI_EXT_CAP_ID_TPH]	=	0xFF,
+	[PCI_EXT_CAP_ID_LTR]	=	PCI_EXT_CAP_LTR_SIZEOF,
+	[PCI_EXT_CAP_ID_SECPCI]	=	0,	/* not yet */
+	[PCI_EXT_CAP_ID_PMUX]	=	0,	/* not yet */
+	[PCI_EXT_CAP_ID_PASID]	=	0,	/* not yet */
+};
+
+/*
+ * Read/Write Permission Bits - one bit for each bit in capability
+ * Any field can be read if it exists, but what is read depends on
+ * whether the field is 'virtualized', or just pass thru to the
+ * hardware.  Any virtualized field is also virtualized for writes.
+ * Writes are only permitted if they have a 1 bit here.
+ */
+struct perm_bits {
+	u8	*virt;		/* read/write virtual data, not hw */
+	u8	*write;		/* writeable bits */
+	int	(*readfn)(struct vfio_pci_device *vdev, int pos, int count,
+			  struct perm_bits *perm, int offset, __le32 *val);
+	int	(*writefn)(struct vfio_pci_device *vdev, int pos, int count,
+			   struct perm_bits *perm, int offset, __le32 val);
+};
+
+#define	NO_VIRT		0
+#define	ALL_VIRT	0xFFFFFFFFU
+#define	NO_WRITE	0
+#define	ALL_WRITE	0xFFFFFFFFU
+
+static int vfio_user_config_read(struct pci_dev *pdev, int offset,
+				 __le32 *val, int count)
+{
+	int ret = -EINVAL;
+	u32 tmp_val = 0;
+
+	switch (count) {
+	case 1:
+	{
+		u8 tmp;
+		ret = pci_user_read_config_byte(pdev, offset, &tmp);
+		tmp_val = tmp;
+		break;
+	}
+	case 2:
+	{
+		u16 tmp;
+		ret = pci_user_read_config_word(pdev, offset, &tmp);
+		tmp_val = tmp;
+		break;
+	}
+	case 4:
+		ret = pci_user_read_config_dword(pdev, offset, &tmp_val);
+		break;
+	}
+
+	*val = cpu_to_le32(tmp_val);
+
+	return pcibios_err_to_errno(ret);
+}
+
+static int vfio_user_config_write(struct pci_dev *pdev, int offset,
+				  __le32 val, int count)
+{
+	int ret = -EINVAL;
+	u32 tmp_val = le32_to_cpu(val);
+
+	switch (count) {
+	case 1:
+		ret = pci_user_write_config_byte(pdev, offset, tmp_val);
+		break;
+	case 2:
+		ret = pci_user_write_config_word(pdev, offset, tmp_val);
+		break;
+	case 4:
+		ret = pci_user_write_config_dword(pdev, offset, tmp_val);
+		break;
+	}
+
+	return pcibios_err_to_errno(ret);
+}
+
+static int vfio_default_config_read(struct vfio_pci_device *vdev, int pos,
+				    int count, struct perm_bits *perm,
+				    int offset, __le32 *val)
+{
+	__le32 virt = 0;
+
+	memcpy(val, vdev->vconfig + pos, count);
+
+	memcpy(&virt, perm->virt + offset, count);
+
+	/* Any non-virtualized bits? */
+	if (cpu_to_le32(~0U >> (32 - (count * 8))) != virt) {
+		struct pci_dev *pdev = vdev->pdev;
+		__le32 phys_val = 0;
+		int ret;
+
+		ret = vfio_user_config_read(pdev, pos, &phys_val, count);
+		if (ret)
+			return ret;
+
+		*val = (phys_val & ~virt) | (*val & virt);
+	}
+
+	return count;
+}
+
+static int vfio_default_config_write(struct vfio_pci_device *vdev, int pos,
+				     int count, struct perm_bits *perm,
+				     int offset, __le32 val)
+{
+	__le32 virt = 0, write = 0;
+
+	memcpy(&write, perm->write + offset, count);
+
+	if (!write)
+		return count; /* drop, no writable bits */
+
+	memcpy(&virt, perm->virt + offset, count);
+
+	/* Virtualized and writable bits go to vconfig */
+	if (write & virt) {
+		__le32 virt_val = 0;
+
+		memcpy(&virt_val, vdev->vconfig + pos, count);
+
+		virt_val &= ~(write & virt);
+		virt_val |= (val & (write & virt));
+
+		memcpy(vdev->vconfig + pos, &virt_val, count);
+	}
+
+	/* Non-virtualzed and writable bits go to hardware */
+	if (write & ~virt) {
+		struct pci_dev *pdev = vdev->pdev;
+		__le32 phys_val = 0;
+		int ret;
+
+		ret = vfio_user_config_read(pdev, pos, &phys_val, count);
+		if (ret)
+			return ret;
+
+		phys_val &= ~(write & ~virt);
+		phys_val |= (val & (write & ~virt));
+
+		ret = vfio_user_config_write(pdev, pos, phys_val, count);
+		if (ret)
+			return ret;
+	}
+
+	return count;
+}
+
+/* Allow direct read from hardware, except for capability next pointer */
+static int vfio_direct_config_read(struct vfio_pci_device *vdev, int pos,
+				   int count, struct perm_bits *perm,
+				   int offset, __le32 *val)
+{
+	int ret;
+
+	ret = vfio_user_config_read(vdev->pdev, pos, val, count);
+	if (ret)
+		return pcibios_err_to_errno(ret);
+
+	if (pos >= PCI_CFG_SPACE_SIZE) { /* Extended cap header mangling */
+		if (offset < 4)
+			memcpy(val, vdev->vconfig + pos, count);
+	} else if (pos >= PCI_STD_HEADER_SIZEOF) { /* Std cap mangling */
+		if (offset == PCI_CAP_LIST_ID && count > 1)
+			memcpy(val, vdev->vconfig + pos,
+			       min(PCI_CAP_FLAGS, count));
+		else if (offset == PCI_CAP_LIST_NEXT)
+			memcpy(val, vdev->vconfig + pos, 1);
+	}
+
+	return count;
+}
+
+static int vfio_direct_config_write(struct vfio_pci_device *vdev, int pos,
+				    int count, struct perm_bits *perm,
+				    int offset, __le32 val)
+{
+	int ret;
+
+	ret = vfio_user_config_write(vdev->pdev, pos, val, count);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+/* Default all regions to read-only, no-virtualization */
+static struct perm_bits cap_perms[PCI_CAP_ID_MAX + 1] = {
+	[0 ... PCI_CAP_ID_MAX] = { .readfn = vfio_direct_config_read }
+};
+static struct perm_bits ecap_perms[PCI_EXT_CAP_ID_MAX + 1] = {
+	[0 ... PCI_EXT_CAP_ID_MAX] = { .readfn = vfio_direct_config_read }
+};
+
+static void free_perm_bits(struct perm_bits *perm)
+{
+	kfree(perm->virt);
+	kfree(perm->write);
+	perm->virt = NULL;
+	perm->write = NULL;
+}
+
+static int alloc_perm_bits(struct perm_bits *perm, int size)
+{
+	/*
+	 * Round up all permission bits to the next dword, this lets us
+	 * ignore whether a read/write exceeds the defined capability
+	 * structure.  We can do this because:
+	 *  - Standard config space is already dword aligned
+	 *  - Capabilities are all dword alinged (bits 0:1 of next reserved)
+	 *  - Express capabilities defined as dword aligned
+	 */
+	size = round_up(size, 4);
+
+	/*
+	 * Zero state is
+	 * - All Readable, None Writeable, None Virtualized
+	 */
+	perm->virt = kzalloc(size, GFP_KERNEL);
+	perm->write = kzalloc(size, GFP_KERNEL);
+	if (!perm->virt || !perm->write) {
+		free_perm_bits(perm);
+		return -ENOMEM;
+	}
+
+	perm->readfn = vfio_default_config_read;
+	perm->writefn = vfio_default_config_write;
+
+	return 0;
+}
+
+/*
+ * Helper functions for filling in permission tables
+ */
+static inline void p_setb(struct perm_bits *p, int off, u8 virt, u8 write)
+{
+	p->virt[off] = virt;
+	p->write[off] = write;
+}
+
+/* Handle endian-ness - pci and tables are little-endian */
+static inline void p_setw(struct perm_bits *p, int off, u16 virt, u16 write)
+{
+	*(__le16 *)(&p->virt[off]) = cpu_to_le16(virt);
+	*(__le16 *)(&p->write[off]) = cpu_to_le16(write);
+}
+
+/* Handle endian-ness - pci and tables are little-endian */
+static inline void p_setd(struct perm_bits *p, int off, u32 virt, u32 write)
+{
+	*(__le32 *)(&p->virt[off]) = cpu_to_le32(virt);
+	*(__le32 *)(&p->write[off]) = cpu_to_le32(write);
+}
+
+/*
+ * Restore the *real* BARs after we detect a FLR or backdoor reset.
+ * (backdoor = some device specific technique that we didn't catch)
+ */
+static void vfio_bar_restore(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u32 *rbar = vdev->rbar;
+	int i;
+
+	if (pdev->is_virtfn)
+		return;
+
+	pr_info("%s: %s reset recovery - restoring bars\n",
+		__func__, dev_name(&pdev->dev));
+
+	for (i = PCI_BASE_ADDRESS_0; i <= PCI_BASE_ADDRESS_5; i += 4, rbar++)
+		pci_user_write_config_dword(pdev, i, *rbar);
+
+	pci_user_write_config_dword(pdev, PCI_ROM_ADDRESS, *rbar);
+}
+
+static __le32 vfio_generate_bar_flags(struct pci_dev *pdev, int bar)
+{
+	unsigned long flags = pci_resource_flags(pdev, bar);
+	u32 val;
+
+	if (flags & IORESOURCE_IO)
+		return cpu_to_le32(PCI_BASE_ADDRESS_SPACE_IO);
+
+	val = PCI_BASE_ADDRESS_SPACE_MEMORY;
+
+	if (flags & IORESOURCE_PREFETCH)
+		val |= PCI_BASE_ADDRESS_MEM_PREFETCH;
+
+	if (flags & IORESOURCE_MEM_64)
+		val |= PCI_BASE_ADDRESS_MEM_TYPE_64;
+
+	return cpu_to_le32(val);
+}
+
+/*
+ * Pretend we're hardware and tweak the values of the *virtual* PCI BARs
+ * to reflect the hardware capabilities.  This implements BAR sizing.
+ */
+static void vfio_bar_fixup(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int i;
+	__le32 *bar;
+	u64 mask;
+
+	bar = (__le32 *)&vdev->vconfig[PCI_BASE_ADDRESS_0];
+
+	for (i = PCI_STD_RESOURCES; i <= PCI_STD_RESOURCE_END; i++, bar++) {
+		if (!pci_resource_start(pdev, i)) {
+			*bar = 0; /* Unmapped by host = unimplemented to user */
+			continue;
+		}
+
+		mask = ~(pci_resource_len(pdev, i) - 1);
+
+		*bar &= cpu_to_le32((u32)mask);
+		*bar |= vfio_generate_bar_flags(pdev, i);
+
+		if (*bar & cpu_to_le32(PCI_BASE_ADDRESS_MEM_TYPE_64)) {
+			bar++;
+			*bar &= cpu_to_le32((u32)(mask >> 32));
+			i++;
+		}
+	}
+
+	bar = (__le32 *)&vdev->vconfig[PCI_ROM_ADDRESS];
+
+	/*
+	 * NB. we expose the actual BAR size here, regardless of whether
+	 * we can read it.  When we report the REGION_INFO for the ROM
+	 * we report what PCI tells us is the actual ROM size.
+	 */
+	if (pci_resource_start(pdev, PCI_ROM_RESOURCE)) {
+		mask = ~(pci_resource_len(pdev, PCI_ROM_RESOURCE) - 1);
+		mask |= PCI_ROM_ADDRESS_ENABLE;
+		*bar &= cpu_to_le32((u32)mask);
+	} else
+		*bar = 0;
+
+	vdev->bardirty = false;
+}
+
+static int vfio_basic_config_read(struct vfio_pci_device *vdev, int pos,
+				  int count, struct perm_bits *perm,
+				  int offset, __le32 *val)
+{
+	if (is_bar(offset)) /* pos == offset for basic config */
+		vfio_bar_fixup(vdev);
+
+	count = vfio_default_config_read(vdev, pos, count, perm, offset, val);
+
+	/* Mask in virtual memory enable for SR-IOV devices */
+	if (offset == PCI_COMMAND && vdev->pdev->is_virtfn) {
+		u16 cmd = le16_to_cpu(*(__le16 *)&vdev->vconfig[PCI_COMMAND]);
+		u32 tmp_val = le32_to_cpu(*val);
+
+		tmp_val |= cmd & PCI_COMMAND_MEMORY;
+		*val = cpu_to_le32(tmp_val);
+	}
+
+	return count;
+}
+
+static int vfio_basic_config_write(struct vfio_pci_device *vdev, int pos,
+				   int count, struct perm_bits *perm,
+				   int offset, __le32 val)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	__le16 *virt_cmd;
+	u16 new_cmd = 0;
+	int ret;
+
+	virt_cmd = (__le16 *)&vdev->vconfig[PCI_COMMAND];
+
+	if (offset == PCI_COMMAND) {
+		bool phys_mem, virt_mem, new_mem, phys_io, virt_io, new_io;
+		u16 phys_cmd;
+
+		ret = pci_user_read_config_word(pdev, PCI_COMMAND, &phys_cmd);
+		if (ret)
+			return ret;
+
+		new_cmd = le32_to_cpu(val);
+
+		phys_mem = !!(phys_cmd & PCI_COMMAND_MEMORY);
+		virt_mem = !!(le16_to_cpu(*virt_cmd) & PCI_COMMAND_MEMORY);
+		new_mem = !!(new_cmd & PCI_COMMAND_MEMORY);
+
+		phys_io = !!(phys_cmd & PCI_COMMAND_IO);
+		virt_io = !!(le16_to_cpu(*virt_cmd) & PCI_COMMAND_IO);
+		new_io = !!(new_cmd & PCI_COMMAND_IO);
+
+		/*
+		 * If the user is writing mem/io enable (new_mem/io) and we
+		 * think it's already enabled (virt_mem/io), but the hardware
+		 * shows it disabled (phys_mem/io, then the device has
+		 * undergone some kind of backdoor reset and needs to be
+		 * restored before we allow it to enable the bars.
+		 * SR-IOV devices will trigger this, but we catch them later
+		 */
+		if ((new_mem && virt_mem && !phys_mem) ||
+		    (new_io && virt_io && !phys_io))
+			vfio_bar_restore(vdev);
+	}
+
+	count = vfio_default_config_write(vdev, pos, count, perm, offset, val);
+	if (count < 0)
+		return count;
+
+	/*
+	 * Save current memory/io enable bits in vconfig to allow for
+	 * the test above next time.
+	 */
+	if (offset == PCI_COMMAND) {
+		u16 mask = PCI_COMMAND_MEMORY | PCI_COMMAND_IO;
+
+		*virt_cmd &= cpu_to_le16(~mask);
+		*virt_cmd |= cpu_to_le16(new_cmd & mask);
+	}
+
+	/* Emulate INTx disable */
+	if (offset >= PCI_COMMAND && offset <= PCI_COMMAND + 1) {
+		bool virt_intx_disable;
+
+		virt_intx_disable = !!(le16_to_cpu(*virt_cmd) &
+				       PCI_COMMAND_INTX_DISABLE);
+
+		if (virt_intx_disable && !vdev->virq_disabled) {
+			vdev->virq_disabled = true;
+			vfio_pci_intx_mask(vdev);
+		} else if (!virt_intx_disable && vdev->virq_disabled) {
+			vdev->virq_disabled = false;
+			vfio_pci_intx_unmask(vdev);
+		}
+	}
+
+	if (is_bar(offset))
+		vdev->bardirty = true;
+
+	return count;
+}
+
+/* Permissions for the Basic PCI Header */
+static int __init init_pci_cap_basic_perm(struct perm_bits *perm)
+{
+	if (alloc_perm_bits(perm, PCI_STD_HEADER_SIZEOF))
+		return -ENOMEM;
+
+	perm->readfn = vfio_basic_config_read;
+	perm->writefn = vfio_basic_config_write;
+
+	/* Virtualized for SR-IOV functions, which just have FFFF */
+	p_setw(perm, PCI_VENDOR_ID, (u16)ALL_VIRT, NO_WRITE);
+	p_setw(perm, PCI_DEVICE_ID, (u16)ALL_VIRT, NO_WRITE);
+
+	/*
+	 * Virtualize INTx disable, we use it internally for interrupt
+	 * control and can emulate it for non-PCI 2.3 devices.
+	 */
+	p_setw(perm, PCI_COMMAND, PCI_COMMAND_INTX_DISABLE, (u16)ALL_WRITE);
+
+	/* Virtualize capability list, we might want to skip/disable */
+	p_setw(perm, PCI_STATUS, PCI_STATUS_CAP_LIST, NO_WRITE);
+
+	/* No harm to write */
+	p_setb(perm, PCI_CACHE_LINE_SIZE, NO_VIRT, (u8)ALL_WRITE);
+	p_setb(perm, PCI_LATENCY_TIMER, NO_VIRT, (u8)ALL_WRITE);
+	p_setb(perm, PCI_BIST, NO_VIRT, (u8)ALL_WRITE);
+
+	/* Virtualize all bars, can't touch the real ones */
+	p_setd(perm, PCI_BASE_ADDRESS_0, ALL_VIRT, ALL_WRITE);
+	p_setd(perm, PCI_BASE_ADDRESS_1, ALL_VIRT, ALL_WRITE);
+	p_setd(perm, PCI_BASE_ADDRESS_2, ALL_VIRT, ALL_WRITE);
+	p_setd(perm, PCI_BASE_ADDRESS_3, ALL_VIRT, ALL_WRITE);
+	p_setd(perm, PCI_BASE_ADDRESS_4, ALL_VIRT, ALL_WRITE);
+	p_setd(perm, PCI_BASE_ADDRESS_5, ALL_VIRT, ALL_WRITE);
+	p_setd(perm, PCI_ROM_ADDRESS, ALL_VIRT, ALL_WRITE);
+
+	/* Allow us to adjust capability chain */
+	p_setb(perm, PCI_CAPABILITY_LIST, (u8)ALL_VIRT, NO_WRITE);
+
+	/* Sometimes used by sw, just virtualize */
+	p_setb(perm, PCI_INTERRUPT_LINE, (u8)ALL_VIRT, (u8)ALL_WRITE);
+	return 0;
+}
+
+/* Permissions for the Power Management capability */
+static int __init init_pci_cap_pm_perm(struct perm_bits *perm)
+{
+	if (alloc_perm_bits(perm, pci_cap_length[PCI_CAP_ID_PM]))
+		return -ENOMEM;
+
+	/*
+	 * We always virtualize the next field so we can remove
+	 * capabilities from the chain if we want to.
+	 */
+	p_setb(perm, PCI_CAP_LIST_NEXT, (u8)ALL_VIRT, NO_WRITE);
+
+	/*
+	 * Power management is defined *per function*,
+	 * so we let the user write this
+	 */
+	p_setd(perm, PCI_PM_CTRL, NO_VIRT, ALL_WRITE);
+	return 0;
+}
+
+/* Permissions for PCI-X capability */
+static int __init init_pci_cap_pcix_perm(struct perm_bits *perm)
+{
+	/* Alloc 24, but only 8 are used in v0 */
+	if (alloc_perm_bits(perm, PCI_CAP_PCIX_SIZEOF_V2))
+		return -ENOMEM;
+
+	p_setb(perm, PCI_CAP_LIST_NEXT, (u8)ALL_VIRT, NO_WRITE);
+
+	p_setw(perm, PCI_X_CMD, NO_VIRT, (u16)ALL_WRITE);
+	p_setd(perm, PCI_X_ECC_CSR, NO_VIRT, ALL_WRITE);
+	return 0;
+}
+
+/* Permissions for PCI Express capability */
+static int __init init_pci_cap_exp_perm(struct perm_bits *perm)
+{
+	/* Alloc larger of two possible sizes */
+	if (alloc_perm_bits(perm, PCI_CAP_EXP_ENDPOINT_SIZEOF_V2))
+		return -ENOMEM;
+
+	p_setb(perm, PCI_CAP_LIST_NEXT, (u8)ALL_VIRT, NO_WRITE);
+
+	/*
+	 * Allow writes to device control fields (includes FLR!)
+	 * but not to devctl_phantom which could confuse IOMMU
+	 * or to the ARI bit in devctl2 which is set at probe time
+	 */
+	p_setw(perm, PCI_EXP_DEVCTL, NO_VIRT, ~PCI_EXP_DEVCTL_PHANTOM);
+	p_setw(perm, PCI_EXP_DEVCTL2, NO_VIRT, ~PCI_EXP_DEVCTL2_ARI);
+	return 0;
+}
+
+/* Permissions for Advanced Function capability */
+static int __init init_pci_cap_af_perm(struct perm_bits *perm)
+{
+	if (alloc_perm_bits(perm, pci_cap_length[PCI_CAP_ID_AF]))
+		return -ENOMEM;
+
+	p_setb(perm, PCI_CAP_LIST_NEXT, (u8)ALL_VIRT, NO_WRITE);
+	p_setb(perm, PCI_AF_CTRL, NO_VIRT, PCI_AF_CTRL_FLR);
+	return 0;
+}
+
+/* Permissions for Advanced Error Reporting extended capability */
+static int __init init_pci_ext_cap_err_perm(struct perm_bits *perm)
+{
+	u32 mask;
+
+	if (alloc_perm_bits(perm, pci_ext_cap_length[PCI_EXT_CAP_ID_ERR]))
+		return -ENOMEM;
+
+	/*
+	 * Virtualize the first dword of all express capabilities
+	 * because it includes the next pointer.  This lets us later
+	 * remove capabilities from the chain if we need to.
+	 */
+	p_setd(perm, 0, ALL_VIRT, NO_WRITE);
+
+	/* Writable bits mask */
+	mask =	PCI_ERR_UNC_TRAIN |		/* Training */
+		PCI_ERR_UNC_DLP |		/* Data Link Protocol */
+		PCI_ERR_UNC_SURPDN |		/* Surprise Down */
+		PCI_ERR_UNC_POISON_TLP |	/* Poisoned TLP */
+		PCI_ERR_UNC_FCP |		/* Flow Control Protocol */
+		PCI_ERR_UNC_COMP_TIME |		/* Completion Timeout */
+		PCI_ERR_UNC_COMP_ABORT |	/* Completer Abort */
+		PCI_ERR_UNC_UNX_COMP |		/* Unexpected Completion */
+		PCI_ERR_UNC_RX_OVER |		/* Receiver Overflow */
+		PCI_ERR_UNC_MALF_TLP |		/* Malformed TLP */
+		PCI_ERR_UNC_ECRC |		/* ECRC Error Status */
+		PCI_ERR_UNC_UNSUP |		/* Unsupported Request */
+		PCI_ERR_UNC_ACSV |		/* ACS Violation */
+		PCI_ERR_UNC_INTN |		/* internal error */
+		PCI_ERR_UNC_MCBTLP |		/* MC blocked TLP */
+		PCI_ERR_UNC_ATOMEG |		/* Atomic egress blocked */
+		PCI_ERR_UNC_TLPPRE;		/* TLP prefix blocked */
+	p_setd(perm, PCI_ERR_UNCOR_STATUS, NO_VIRT, mask);
+	p_setd(perm, PCI_ERR_UNCOR_MASK, NO_VIRT, mask);
+	p_setd(perm, PCI_ERR_UNCOR_SEVER, NO_VIRT, mask);
+
+	mask =	PCI_ERR_COR_RCVR |		/* Receiver Error Status */
+		PCI_ERR_COR_BAD_TLP |		/* Bad TLP Status */
+		PCI_ERR_COR_BAD_DLLP |		/* Bad DLLP Status */
+		PCI_ERR_COR_REP_ROLL |		/* REPLAY_NUM Rollover */
+		PCI_ERR_COR_REP_TIMER |		/* Replay Timer Timeout */
+		PCI_ERR_COR_ADV_NFAT |		/* Advisory Non-Fatal */
+		PCI_ERR_COR_INTERNAL |		/* Corrected Internal */
+		PCI_ERR_COR_LOG_OVER;		/* Header Log Overflow */
+	p_setd(perm, PCI_ERR_COR_STATUS, NO_VIRT, mask);
+	p_setd(perm, PCI_ERR_COR_MASK, NO_VIRT, mask);
+
+	mask =	PCI_ERR_CAP_ECRC_GENE |		/* ECRC Generation Enable */
+		PCI_ERR_CAP_ECRC_CHKE;		/* ECRC Check Enable */
+	p_setd(perm, PCI_ERR_CAP, NO_VIRT, mask);
+	return 0;
+}
+
+/* Permissions for Power Budgeting extended capability */
+static int __init init_pci_ext_cap_pwr_perm(struct perm_bits *perm)
+{
+	if (alloc_perm_bits(perm, pci_ext_cap_length[PCI_EXT_CAP_ID_PWR]))
+		return -ENOMEM;
+
+	p_setd(perm, 0, ALL_VIRT, NO_WRITE);
+
+	/* Writing the data selector is OK, the info is still read-only */
+	p_setb(perm, PCI_PWR_DATA, NO_VIRT, (u8)ALL_WRITE);
+	return 0;
+}
+
+/*
+ * Initialize the shared permission tables
+ */
+void vfio_pci_uninit_perm_bits(void)
+{
+	free_perm_bits(&cap_perms[PCI_CAP_ID_BASIC]);
+
+	free_perm_bits(&cap_perms[PCI_CAP_ID_PM]);
+	free_perm_bits(&cap_perms[PCI_CAP_ID_PCIX]);
+	free_perm_bits(&cap_perms[PCI_CAP_ID_EXP]);
+	free_perm_bits(&cap_perms[PCI_CAP_ID_AF]);
+
+	free_perm_bits(&ecap_perms[PCI_EXT_CAP_ID_ERR]);
+	free_perm_bits(&ecap_perms[PCI_EXT_CAP_ID_PWR]);
+}
+
+int __init vfio_pci_init_perm_bits(void)
+{
+	int ret;
+
+	/* Basic config space */
+	ret = init_pci_cap_basic_perm(&cap_perms[PCI_CAP_ID_BASIC]);
+
+	/* Capabilities */
+	ret |= init_pci_cap_pm_perm(&cap_perms[PCI_CAP_ID_PM]);
+	cap_perms[PCI_CAP_ID_VPD].writefn = vfio_direct_config_write;
+	ret |= init_pci_cap_pcix_perm(&cap_perms[PCI_CAP_ID_PCIX]);
+	cap_perms[PCI_CAP_ID_VNDR].writefn = vfio_direct_config_write;
+	ret |= init_pci_cap_exp_perm(&cap_perms[PCI_CAP_ID_EXP]);
+	ret |= init_pci_cap_af_perm(&cap_perms[PCI_CAP_ID_AF]);
+
+	/* Extended capabilities */
+	ret |= init_pci_ext_cap_err_perm(&ecap_perms[PCI_EXT_CAP_ID_ERR]);
+	ret |= init_pci_ext_cap_pwr_perm(&ecap_perms[PCI_EXT_CAP_ID_PWR]);
+	ecap_perms[PCI_EXT_CAP_ID_VNDR].writefn = vfio_direct_config_write;
+
+	if (ret)
+		vfio_pci_uninit_perm_bits();
+
+	return ret;
+}
+
+static int vfio_find_cap_start(struct vfio_pci_device *vdev, int pos)
+{
+	u8 cap;
+	int base = (pos >= PCI_CFG_SPACE_SIZE) ? PCI_CFG_SPACE_SIZE :
+						 PCI_STD_HEADER_SIZEOF;
+	base /= 4;
+	pos /= 4;
+
+	cap = vdev->pci_config_map[pos];
+
+	if (cap == PCI_CAP_ID_BASIC)
+		return 0;
+
+	/* XXX Can we have to abutting capabilities of the same type? */
+	while (pos - 1 >= base && vdev->pci_config_map[pos - 1] == cap)
+		pos--;
+
+	return pos * 4;
+}
+
+static int vfio_msi_config_read(struct vfio_pci_device *vdev, int pos,
+				int count, struct perm_bits *perm,
+				int offset, __le32 *val)
+{
+	/* Update max available queue size from msi_qmax */
+	if (offset <= PCI_MSI_FLAGS && offset + count >= PCI_MSI_FLAGS) {
+		__le16 *flags;
+		int start;
+
+		start = vfio_find_cap_start(vdev, pos);
+
+		flags = (__le16 *)&vdev->vconfig[start];
+
+		*flags &= cpu_to_le16(~PCI_MSI_FLAGS_QMASK);
+		*flags |= cpu_to_le16(vdev->msi_qmax << 1);
+	}
+
+	return vfio_default_config_read(vdev, pos, count, perm, offset, val);
+}
+
+static int vfio_msi_config_write(struct vfio_pci_device *vdev, int pos,
+				 int count, struct perm_bits *perm,
+				 int offset, __le32 val)
+{
+	count = vfio_default_config_write(vdev, pos, count, perm, offset, val);
+	if (count < 0)
+		return count;
+
+	/* Fixup and write configured queue size and enable to hardware */
+	if (offset <= PCI_MSI_FLAGS && offset + count >= PCI_MSI_FLAGS) {
+		__le16 *pflags;
+		u16 flags;
+		int start, ret;
+
+		start = vfio_find_cap_start(vdev, pos);
+
+		pflags = (__le16 *)&vdev->vconfig[start + PCI_MSI_FLAGS];
+
+		flags = le16_to_cpu(*pflags);
+
+		/* MSI is enabled via ioctl */
+		if  (!is_msi(vdev))
+			flags &= ~PCI_MSI_FLAGS_ENABLE;
+
+		/* Check queue size */
+		if ((flags & PCI_MSI_FLAGS_QSIZE) >> 4 > vdev->msi_qmax) {
+			flags &= ~PCI_MSI_FLAGS_QSIZE;
+			flags |= vdev->msi_qmax << 4;
+		}
+
+		/* Write back to virt and to hardware */
+		*pflags = cpu_to_le16(flags);
+		ret = pci_user_write_config_word(vdev->pdev,
+						 start + PCI_MSI_FLAGS,
+						 flags);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+	}
+
+	return count;
+}
+
+/*
+ * MSI determination is per-device, so this routine gets used beyond
+ * initialization time. Don't add __init
+ */
+static int init_pci_cap_msi_perm(struct perm_bits *perm, int len, u16 flags)
+{
+	if (alloc_perm_bits(perm, len))
+		return -ENOMEM;
+
+	perm->readfn = vfio_msi_config_read;
+	perm->writefn = vfio_msi_config_write;
+
+	p_setb(perm, PCI_CAP_LIST_NEXT, (u8)ALL_VIRT, NO_WRITE);
+
+	/*
+	 * The upper byte of the control register is reserved,
+	 * just setup the lower byte.
+	 */
+	p_setb(perm, PCI_MSI_FLAGS, (u8)ALL_VIRT, (u8)ALL_WRITE);
+	p_setd(perm, PCI_MSI_ADDRESS_LO, ALL_VIRT, ALL_WRITE);
+	if (flags & PCI_MSI_FLAGS_64BIT) {
+		p_setd(perm, PCI_MSI_ADDRESS_HI, ALL_VIRT, ALL_WRITE);
+		p_setw(perm, PCI_MSI_DATA_64, (u16)ALL_VIRT, (u16)ALL_WRITE);
+		if (flags & PCI_MSI_FLAGS_MASKBIT) {
+			p_setd(perm, PCI_MSI_MASK_64, NO_VIRT, ALL_WRITE);
+			p_setd(perm, PCI_MSI_PENDING_64, NO_VIRT, ALL_WRITE);
+		}
+	} else {
+		p_setw(perm, PCI_MSI_DATA_32, (u16)ALL_VIRT, (u16)ALL_WRITE);
+		if (flags & PCI_MSI_FLAGS_MASKBIT) {
+			p_setd(perm, PCI_MSI_MASK_32, NO_VIRT, ALL_WRITE);
+			p_setd(perm, PCI_MSI_PENDING_32, NO_VIRT, ALL_WRITE);
+		}
+	}
+	return 0;
+}
+
+/* Determine MSI CAP field length; initialize msi_perms on 1st call per vdev */
+static int vfio_msi_cap_len(struct vfio_pci_device *vdev, u8 pos)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int len, ret;
+	u16 flags;
+
+	ret = pci_read_config_word(pdev, pos + PCI_MSI_FLAGS, &flags);
+	if (ret)
+		return pcibios_err_to_errno(ret);
+
+	len = 10; /* Minimum size */
+	if (flags & PCI_MSI_FLAGS_64BIT)
+		len += 4;
+	if (flags & PCI_MSI_FLAGS_MASKBIT)
+		len += 10;
+
+	if (vdev->msi_perm)
+		return len;
+
+	vdev->msi_perm = kmalloc(sizeof(struct perm_bits), GFP_KERNEL);
+	if (!vdev->msi_perm)
+		return -ENOMEM;
+
+	ret = init_pci_cap_msi_perm(vdev->msi_perm, len, flags);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+/* Determine extended capability length for VC (2 & 9) and MFVC */
+static int vfio_vc_cap_len(struct vfio_pci_device *vdev, u16 pos)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u32 tmp;
+	int ret, evcc, phases, vc_arb;
+	int len = PCI_CAP_VC_BASE_SIZEOF;
+
+	ret = pci_read_config_dword(pdev, pos + PCI_VC_PORT_REG1, &tmp);
+	if (ret)
+		return pcibios_err_to_errno(ret);
+
+	evcc = tmp & PCI_VC_REG1_EVCC; /* extended vc count */
+	ret = pci_read_config_dword(pdev, pos + PCI_VC_PORT_REG2, &tmp);
+	if (ret)
+		return pcibios_err_to_errno(ret);
+
+	if (tmp & PCI_VC_REG2_128_PHASE)
+		phases = 128;
+	else if (tmp & PCI_VC_REG2_64_PHASE)
+		phases = 64;
+	else if (tmp & PCI_VC_REG2_32_PHASE)
+		phases = 32;
+	else
+		phases = 0;
+
+	vc_arb = phases * 4;
+
+	/*
+	 * Port arbitration tables are root & switch only;
+	 * function arbitration tables are function 0 only.
+	 * In either case, we'll never let user write them so
+	 * we don't care how big they are
+	 */
+	len += (1 + evcc) * PCI_CAP_VC_PER_VC_SIZEOF;
+	if (vc_arb) {
+		len = round_up(len, 16);
+		len += vc_arb / 8;
+	}
+	return len;
+}
+
+static int vfio_cap_len(struct vfio_pci_device *vdev, u8 cap, u8 pos)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u16 word;
+	u8 byte;
+	int ret;
+
+	switch (cap) {
+	case PCI_CAP_ID_MSI:
+		return vfio_msi_cap_len(vdev, pos);
+	case PCI_CAP_ID_PCIX:
+		ret = pci_read_config_word(pdev, pos + PCI_X_CMD, &word);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		if (PCI_X_CMD_VERSION(word)) {
+			vdev->extended_caps = true;
+			return PCI_CAP_PCIX_SIZEOF_V2;
+		} else
+			return PCI_CAP_PCIX_SIZEOF_V0;
+	case PCI_CAP_ID_VNDR:
+		/* length follows next field */
+		ret = pci_read_config_byte(pdev, pos + PCI_CAP_FLAGS, &byte);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		return byte;
+	case PCI_CAP_ID_EXP:
+		/* length based on version */
+		ret = pci_read_config_word(pdev, pos + PCI_EXP_FLAGS, &word);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		if ((word & PCI_EXP_FLAGS_VERS) == 1)
+			return PCI_CAP_EXP_ENDPOINT_SIZEOF_V1;
+		else {
+			vdev->extended_caps = true;
+			return PCI_CAP_EXP_ENDPOINT_SIZEOF_V2;
+		}
+	case PCI_CAP_ID_HT:
+		ret = pci_read_config_byte(pdev, pos + 3, &byte);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		return (byte & HT_3BIT_CAP_MASK) ?
+			HT_CAP_SIZEOF_SHORT : HT_CAP_SIZEOF_LONG;
+	case PCI_CAP_ID_SATA:
+		ret = pci_read_config_byte(pdev, pos + PCI_SATA_REGS, &byte);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		byte &= PCI_SATA_REGS_MASK;
+		if (byte == PCI_SATA_REGS_INLINE)
+			return PCI_SATA_SIZEOF_LONG;
+		else
+			return PCI_SATA_SIZEOF_SHORT;
+	default:
+		pr_warn("%s: %s unknown length for pci cap 0x%x@0x%x\n",
+			dev_name(&pdev->dev), __func__, cap, pos);
+	}
+
+	return 0;
+}
+
+static int vfio_ext_cap_len(struct vfio_pci_device *vdev, u16 ecap, u16 epos)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u8 byte;
+	u32 dword;
+	int ret;
+
+	switch (ecap) {
+	case PCI_EXT_CAP_ID_VNDR:
+		ret = pci_read_config_dword(pdev, epos + PCI_VSEC_HDR, &dword);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		return dword >> PCI_VSEC_HDR_LEN_SHIFT;
+	case PCI_EXT_CAP_ID_VC:
+	case PCI_EXT_CAP_ID_VC9:
+	case PCI_EXT_CAP_ID_MFVC:
+		return vfio_vc_cap_len(vdev, epos);
+	case PCI_EXT_CAP_ID_ACS:
+		ret = pci_read_config_byte(pdev, epos + PCI_ACS_CAP, &byte);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		if (byte & PCI_ACS_EC) {
+			int bits;
+
+			ret = pci_read_config_byte(pdev,
+						   epos + PCI_ACS_EGRESS_BITS,
+						   &byte);
+			if (ret)
+				return pcibios_err_to_errno(ret);
+
+			bits = byte ? round_up(byte, 32) : 256;
+			return 8 + (bits / 8);
+		}
+		return 8;
+
+	case PCI_EXT_CAP_ID_REBAR:
+		ret = pci_read_config_byte(pdev, epos + PCI_REBAR_CTRL, &byte);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		byte &= PCI_REBAR_CTRL_NBAR_MASK;
+		byte >>= PCI_REBAR_CTRL_NBAR_SHIFT;
+
+		return 4 + (byte * 8);
+	case PCI_EXT_CAP_ID_DPA:
+		ret = pci_read_config_byte(pdev, epos + PCI_DPA_CAP, &byte);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		byte &= PCI_DPA_CAP_SUBSTATE_MASK;
+		byte = round_up(byte + 1, 4);
+		return PCI_DPA_BASE_SIZEOF + byte;
+	case PCI_EXT_CAP_ID_TPH:
+		ret = pci_read_config_dword(pdev, epos + PCI_TPH_CAP, &dword);
+		if (ret)
+			return pcibios_err_to_errno(ret);
+
+		if ((dword & PCI_TPH_CAP_LOC_MASK) == PCI_TPH_LOC_CAP) {
+			int sts;
+
+			sts = byte & PCI_TPH_CAP_ST_MASK;
+			sts >>= PCI_TPH_CAP_ST_SHIFT;
+			return PCI_TPH_BASE_SIZEOF + round_up(sts * 2, 4);
+		}
+		return PCI_TPH_BASE_SIZEOF;
+	default:
+		pr_warn("%s: %s unknown length for pci ecap 0x%x@0x%x\n",
+			dev_name(&pdev->dev), __func__, ecap, epos);
+	}
+
+	return 0;
+}
+
+static int vfio_fill_vconfig_bytes(struct vfio_pci_device *vdev,
+				   int offset, int size)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int ret = 0;
+
+	/*
+	 * We try to read physical config space in the largest chunks
+	 * we can, assuming that all of the fields support dword access.
+	 * pci_save_state() makes this same assumption and seems to do ok.
+	 */
+	while (size) {
+		int filled;
+
+		if (size >= 4 && !(offset % 4)) {
+			__le32 *dwordp = (__le32 *)&vdev->vconfig[offset];
+			u32 dword;
+
+			ret = pci_read_config_dword(pdev, offset, &dword);
+			if (ret)
+				return ret;
+			*dwordp = cpu_to_le32(dword);
+			filled = 4;
+		} else if (size >= 2 && !(offset % 2)) {
+			__le16 *wordp = (__le16 *)&vdev->vconfig[offset];
+			u16 word;
+
+			ret = pci_read_config_word(pdev, offset, &word);
+			if (ret)
+				return ret;
+			*wordp = cpu_to_le16(word);
+			filled = 2;
+		} else {
+			u8 *byte = &vdev->vconfig[offset];
+			ret = pci_read_config_byte(pdev, offset, byte);
+			if (ret)
+				return ret;
+			filled = 1;
+		}
+
+		offset += filled;
+		size -= filled;
+	}
+
+	return ret;
+}
+
+static int vfio_cap_init(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u8 *map = vdev->pci_config_map;
+	u16 status;
+	u8 pos, *prev, cap;
+	int loops, ret, caps = 0;
+
+	/* Any capabilities? */
+	ret = pci_read_config_word(pdev, PCI_STATUS, &status);
+	if (ret)
+		return ret;
+
+	if (!(status & PCI_STATUS_CAP_LIST))
+		return 0; /* Done */
+
+	ret = pci_read_config_byte(pdev, PCI_CAPABILITY_LIST, &pos);
+	if (ret)
+		return ret;
+
+	/* Mark the previous position in case we want to skip a capability */
+	prev = &vdev->vconfig[PCI_CAPABILITY_LIST];
+
+	/* We can bound our loop, capabilities are dword aligned */
+	loops = (PCI_CFG_SPACE_SIZE - PCI_STD_HEADER_SIZEOF) / PCI_CAP_SIZEOF;
+	while (pos && loops--) {
+		u8 next;
+		int i, len = 0;
+
+		ret = pci_read_config_byte(pdev, pos, &cap);
+		if (ret)
+			return ret;
+
+		ret = pci_read_config_byte(pdev,
+					   pos + PCI_CAP_LIST_NEXT, &next);
+		if (ret)
+			return ret;
+
+		if (cap <= PCI_CAP_ID_MAX) {
+			len = pci_cap_length[cap];
+			if (len == 0xFF) { /* Variable length */
+				len = vfio_cap_len(vdev, cap, pos);
+				if (len < 0)
+					return len;
+			}
+		}
+
+		if (!len) {
+			pr_info("%s: %s hiding cap 0x%x\n",
+				__func__, dev_name(&pdev->dev), cap);
+			*prev = next;
+			pos = next;
+			continue;
+		}
+
+		/* Sanity check, do we overlap other capabilities? */
+		for (i = 0; i < len; i += 4) {
+			if (likely(map[(pos + i) / 4] == PCI_CAP_ID_INVALID))
+				continue;
+
+			pr_warn("%s: %s pci config conflict @0x%x, was cap 0x%x now cap 0x%x\n",
+				__func__, dev_name(&pdev->dev),
+				pos + i, map[pos + i], cap);
+		}
+
+		memset(map + (pos / 4), cap, len / 4);
+		ret = vfio_fill_vconfig_bytes(vdev, pos, len);
+		if (ret)
+			return ret;
+
+		prev = &vdev->vconfig[pos + PCI_CAP_LIST_NEXT];
+		pos = next;
+		caps++;
+	}
+
+	/* If we didn't fill any capabilities, clear the status flag */
+	if (!caps) {
+		__le16 *vstatus = (__le16 *)&vdev->vconfig[PCI_STATUS];
+		*vstatus &= ~cpu_to_le16(PCI_STATUS_CAP_LIST);
+	}
+
+	return 0;
+}
+
+static int vfio_ecap_init(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u8 *map = vdev->pci_config_map;
+	u16 epos;
+	__le32 *prev = NULL;
+	int loops, ret, ecaps = 0;
+
+	if (!vdev->extended_caps)
+		return 0;
+
+	epos = PCI_CFG_SPACE_SIZE;
+
+	loops = (pdev->cfg_size - PCI_CFG_SPACE_SIZE) / PCI_CAP_SIZEOF;
+
+	while (loops-- && epos >= PCI_CFG_SPACE_SIZE) {
+		u32 header;
+		u16 ecap;
+		int i, len = 0;
+		bool hidden = false;
+
+		ret = pci_read_config_dword(pdev, epos, &header);
+		if (ret)
+			return ret;
+
+		ecap = PCI_EXT_CAP_ID(header);
+
+		if (ecap <= PCI_EXT_CAP_ID_MAX) {
+			len = pci_ext_cap_length[ecap];
+			if (len == 0xFF) {
+				len = vfio_ext_cap_len(vdev, ecap, epos);
+				if (len < 0)
+					return ret;
+			}
+		}
+
+		if (!len) {
+			pr_info("%s: %s hiding ecap 0x%x@0x%x\n",
+				__func__, dev_name(&pdev->dev), ecap, epos);
+
+			/* If not the first in the chain, we can skip over it */
+			if (prev) {
+				u32 val = epos = PCI_EXT_CAP_NEXT(header);
+				*prev &= cpu_to_le32(~(0xffcU << 20));
+				*prev |= cpu_to_le32(val << 20);
+				continue;
+			}
+
+			/*
+			 * Otherwise, fill in a placeholder, the direct
+			 * readfn will virtualize this automatically
+			 */
+			len = PCI_CAP_SIZEOF;
+			hidden = true;
+		}
+
+		for (i = 0; i < len; i += 4) {
+			if (likely(map[(epos + i) / 4] == PCI_CAP_ID_INVALID))
+				continue;
+
+			pr_warn("%s: %s pci config conflict @0x%x, was ecap 0x%x now ecap 0x%x\n",
+				__func__, dev_name(&pdev->dev),
+				epos + i, map[epos + i], ecap);
+		}
+
+		/*
+		 * Even though ecap is 2 bytes, we're currently a long way
+		 * from exceeding 1 byte capabilities.  If we ever make it
+		 * up to 0xFF we'll need to up this to a two-byte, byte map.
+		 */
+		BUILD_BUG_ON(PCI_EXT_CAP_ID_MAX >= PCI_CAP_ID_INVALID);
+
+		memset(map + (epos / 4), ecap, len / 4);
+		ret = vfio_fill_vconfig_bytes(vdev, epos, len);
+		if (ret)
+			return ret;
+
+		/*
+		 * If we're just using this capability to anchor the list,
+		 * hide the real ID.  Only count real ecaps.  XXX PCI spec
+		 * indicates to use cap id = 0, version = 0, next = 0 if
+		 * ecaps are absent, hope users check all the way to next.
+		 */
+		if (hidden)
+			*(__le32 *)&vdev->vconfig[epos] &=
+				cpu_to_le32((0xffcU << 20));
+		else
+			ecaps++;
+
+		prev = (__le32 *)&vdev->vconfig[epos];
+		epos = PCI_EXT_CAP_NEXT(header);
+	}
+
+	if (!ecaps)
+		*(u32 *)&vdev->vconfig[PCI_CFG_SPACE_SIZE] = 0;
+
+	return 0;
+}
+
+/*
+ * For each device we allocate a pci_config_map that indicates the
+ * capability occupying each dword and thus the struct perm_bits we
+ * use for read and write.  We also allocate a virtualized config
+ * space which tracks reads and writes to bits that we emulate for
+ * the user.  Initial values filled from device.
+ *
+ * Using shared stuct perm_bits between all vfio-pci devices saves
+ * us from allocating cfg_size buffers for virt and write for every
+ * device.  We could remove vconfig and allocate individual buffers
+ * for each area requring emulated bits, but the array of pointers
+ * would be comparable in size (at least for standard config space).
+ */
+int vfio_config_init(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	u8 *map, *vconfig;
+	int ret;
+
+	/*
+	 * Config space, caps and ecaps are all dword aligned, so we can
+	 * use one byte per dword to record the type.
+	 */
+	map = kmalloc(pdev->cfg_size / 4, GFP_KERNEL);
+	if (!map)
+		return -ENOMEM;
+
+	vconfig = kmalloc(pdev->cfg_size, GFP_KERNEL);
+	if (!vconfig) {
+		kfree(map);
+		return -ENOMEM;
+	}
+
+	vdev->pci_config_map = map;
+	vdev->vconfig = vconfig;
+
+	memset(map, PCI_CAP_ID_BASIC, PCI_STD_HEADER_SIZEOF / 4);
+	memset(map + (PCI_STD_HEADER_SIZEOF / 4), PCI_CAP_ID_INVALID,
+	       (pdev->cfg_size - PCI_STD_HEADER_SIZEOF) / 4);
+
+	ret = vfio_fill_vconfig_bytes(vdev, 0, PCI_STD_HEADER_SIZEOF);
+	if (ret)
+		goto out;
+
+	vdev->bardirty = true;
+
+	/*
+	 * XXX can we just pci_load_saved_state/pci_restore_state?
+	 * may need to rebuild vconfig after that
+	 */
+
+	/* For restore after reset */
+	vdev->rbar[0] = le32_to_cpu(*(__le32 *)&vconfig[PCI_BASE_ADDRESS_0]);
+	vdev->rbar[1] = le32_to_cpu(*(__le32 *)&vconfig[PCI_BASE_ADDRESS_1]);
+	vdev->rbar[2] = le32_to_cpu(*(__le32 *)&vconfig[PCI_BASE_ADDRESS_2]);
+	vdev->rbar[3] = le32_to_cpu(*(__le32 *)&vconfig[PCI_BASE_ADDRESS_3]);
+	vdev->rbar[4] = le32_to_cpu(*(__le32 *)&vconfig[PCI_BASE_ADDRESS_4]);
+	vdev->rbar[5] = le32_to_cpu(*(__le32 *)&vconfig[PCI_BASE_ADDRESS_5]);
+	vdev->rbar[6] = le32_to_cpu(*(__le32 *)&vconfig[PCI_ROM_ADDRESS]);
+
+	if (pdev->is_virtfn) {
+		*(__le16 *)&vconfig[PCI_VENDOR_ID] = cpu_to_le16(pdev->vendor);
+		*(__le16 *)&vconfig[PCI_DEVICE_ID] = cpu_to_le16(pdev->device);
+	}
+
+	ret = vfio_cap_init(vdev);
+	if (ret)
+		goto out;
+
+	ret = vfio_ecap_init(vdev);
+	if (ret)
+		goto out;
+
+	return 0;
+
+out:
+	kfree(map);
+	vdev->pci_config_map = NULL;
+	kfree(vconfig);
+	vdev->vconfig = NULL;
+	return pcibios_err_to_errno(ret);
+}
+
+void vfio_config_free(struct vfio_pci_device *vdev)
+{
+	kfree(vdev->vconfig);
+	vdev->vconfig = NULL;
+	kfree(vdev->pci_config_map);
+	vdev->pci_config_map = NULL;
+	kfree(vdev->msi_perm);
+	vdev->msi_perm = NULL;
+}
+
+static ssize_t vfio_config_do_rw(struct vfio_pci_device *vdev, char __user *buf,
+				 size_t count, loff_t *ppos, bool iswrite)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	struct perm_bits *perm;
+	__le32 val = 0;
+	int cap_start = 0, offset;
+	u8 cap_id;
+	ssize_t ret = count;
+
+	if (*ppos < 0 || *ppos + count > pdev->cfg_size)
+		return -EFAULT;
+
+	/*
+	 * gcc can't seem to figure out we're a static function, only called
+	 * with count of 1/2/4 and hits copy_from_user_overflow without this.
+	 */
+	if (count > sizeof(val))
+		return -EINVAL;
+
+	cap_id = vdev->pci_config_map[*ppos / 4];
+
+	if (cap_id == PCI_CAP_ID_INVALID) {
+		if (iswrite)
+			return ret; /* drop */
+
+		/*
+		 * Per PCI spec 3.0, section 6.1, reads from reserved and
+		 * unimplemented registers return 0
+		 */
+		if (copy_to_user(buf, &val, count))
+			return -EFAULT;
+
+		return ret;
+	}
+
+	/*
+	 * All capabilities are minimum 4 bytes and aligned on dword
+	 * boundaries.  Since we don't support unaligned accesses, we're
+	 * only ever accessing a single capability.
+	 */
+	if (*ppos >= PCI_CFG_SPACE_SIZE) {
+		WARN_ON(cap_id > PCI_EXT_CAP_ID_MAX);
+
+		perm = &ecap_perms[cap_id];
+		cap_start = vfio_find_cap_start(vdev, *ppos);
+
+	} else {
+		WARN_ON(cap_id > PCI_CAP_ID_MAX);
+
+		perm = &cap_perms[cap_id];
+
+		if (cap_id == PCI_CAP_ID_MSI)
+			perm = vdev->msi_perm;
+
+		if (cap_id > PCI_CAP_ID_BASIC)
+			cap_start = vfio_find_cap_start(vdev, *ppos);
+	}
+
+	WARN_ON(!cap_start && cap_id != PCI_CAP_ID_BASIC);
+	WARN_ON(cap_start > *ppos);
+
+	offset = *ppos - cap_start;
+
+	if (iswrite) {
+		if (!perm->writefn)
+			return ret;
+
+		if (copy_from_user(&val, buf, count))
+			return -EFAULT;
+
+		ret = perm->writefn(vdev, *ppos, count, perm, offset, val);
+	} else {
+		if (perm->readfn) {
+			ret = perm->readfn(vdev, *ppos, count,
+					   perm, offset, &val);
+			if (ret < 0)
+				return ret;
+		}
+
+		if (copy_to_user(buf, &val, count))
+			return -EFAULT;
+	}
+
+	return ret;
+}
+
+ssize_t vfio_pci_config_readwrite(struct vfio_pci_device *vdev,
+				  char __user *buf, size_t count,
+				  loff_t *ppos, bool iswrite)
+{
+	size_t done = 0;
+	int ret = 0;
+	loff_t pos = *ppos;
+
+	pos &= VFIO_PCI_OFFSET_MASK;
+
+	/*
+	 * We want to both keep the access size the caller users as well as
+	 * support reading large chunks of config space in a single call.
+	 * PCI doesn't support unaligned accesses, so we can safely break
+	 * those apart.
+	 */
+	while (count) {
+		if (count >= 4 && !(pos % 4))
+			ret = vfio_config_do_rw(vdev, buf, 4, &pos, iswrite);
+		else if (count >= 2 && !(pos % 2))
+			ret = vfio_config_do_rw(vdev, buf, 2, &pos, iswrite);
+		else
+			ret = vfio_config_do_rw(vdev, buf, 1, &pos, iswrite);
+
+		if (ret < 0)
+			return ret;
+
+		count -= ret;
+		done += ret;
+		buf += ret;
+		pos += ret;
+	}
+
+	*ppos += done;
+
+	return done;
+}
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
new file mode 100644
index 00000000000..211a4920b88
--- /dev/null
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -0,0 +1,740 @@
+/*
+ * VFIO PCI interrupt handling
+ *
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ */
+
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/eventfd.h>
+#include <linux/pci.h>
+#include <linux/file.h>
+#include <linux/poll.h>
+#include <linux/vfio.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+
+#include "vfio_pci_private.h"
+
+/*
+ * IRQfd - generic
+ */
+struct virqfd {
+	struct vfio_pci_device	*vdev;
+	struct eventfd_ctx	*eventfd;
+	int			(*handler)(struct vfio_pci_device *, void *);
+	void			(*thread)(struct vfio_pci_device *, void *);
+	void			*data;
+	struct work_struct	inject;
+	wait_queue_t		wait;
+	poll_table		pt;
+	struct work_struct	shutdown;
+	struct virqfd		**pvirqfd;
+};
+
+static struct workqueue_struct *vfio_irqfd_cleanup_wq;
+
+int __init vfio_pci_virqfd_init(void)
+{
+	vfio_irqfd_cleanup_wq =
+		create_singlethread_workqueue("vfio-irqfd-cleanup");
+	if (!vfio_irqfd_cleanup_wq)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void vfio_pci_virqfd_exit(void)
+{
+	destroy_workqueue(vfio_irqfd_cleanup_wq);
+}
+
+static void virqfd_deactivate(struct virqfd *virqfd)
+{
+	queue_work(vfio_irqfd_cleanup_wq, &virqfd->shutdown);
+}
+
+static int virqfd_wakeup(wait_queue_t *wait, unsigned mode, int sync, void *key)
+{
+	struct virqfd *virqfd = container_of(wait, struct virqfd, wait);
+	unsigned long flags = (unsigned long)key;
+
+	if (flags & POLLIN) {
+		/* An event has been signaled, call function */
+		if ((!virqfd->handler ||
+		     virqfd->handler(virqfd->vdev, virqfd->data)) &&
+		    virqfd->thread)
+			schedule_work(&virqfd->inject);
+	}
+
+	if (flags & POLLHUP)
+		/* The eventfd is closing, detach from VFIO */
+		virqfd_deactivate(virqfd);
+
+	return 0;
+}
+
+static void virqfd_ptable_queue_proc(struct file *file,
+				     wait_queue_head_t *wqh, poll_table *pt)
+{
+	struct virqfd *virqfd = container_of(pt, struct virqfd, pt);
+	add_wait_queue(wqh, &virqfd->wait);
+}
+
+static void virqfd_shutdown(struct work_struct *work)
+{
+	struct virqfd *virqfd = container_of(work, struct virqfd, shutdown);
+	struct virqfd **pvirqfd = virqfd->pvirqfd;
+	u64 cnt;
+
+	eventfd_ctx_remove_wait_queue(virqfd->eventfd, &virqfd->wait, &cnt);
+	flush_work(&virqfd->inject);
+	eventfd_ctx_put(virqfd->eventfd);
+
+	kfree(virqfd);
+	*pvirqfd = NULL;
+}
+
+static void virqfd_inject(struct work_struct *work)
+{
+	struct virqfd *virqfd = container_of(work, struct virqfd, inject);
+	if (virqfd->thread)
+		virqfd->thread(virqfd->vdev, virqfd->data);
+}
+
+static int virqfd_enable(struct vfio_pci_device *vdev,
+			 int (*handler)(struct vfio_pci_device *, void *),
+			 void (*thread)(struct vfio_pci_device *, void *),
+			 void *data, struct virqfd **pvirqfd, int fd)
+{
+	struct file *file = NULL;
+	struct eventfd_ctx *ctx = NULL;
+	struct virqfd *virqfd;
+	int ret = 0;
+	unsigned int events;
+
+	if (*pvirqfd)
+		return -EBUSY;
+
+	virqfd = kzalloc(sizeof(*virqfd), GFP_KERNEL);
+	if (!virqfd)
+		return -ENOMEM;
+
+	virqfd->pvirqfd = pvirqfd;
+	*pvirqfd = virqfd;
+	virqfd->vdev = vdev;
+	virqfd->handler = handler;
+	virqfd->thread = thread;
+	virqfd->data = data;
+
+	INIT_WORK(&virqfd->shutdown, virqfd_shutdown);
+	INIT_WORK(&virqfd->inject, virqfd_inject);
+
+	file = eventfd_fget(fd);
+	if (IS_ERR(file)) {
+		ret = PTR_ERR(file);
+		goto fail;
+	}
+
+	ctx = eventfd_ctx_fileget(file);
+	if (IS_ERR(ctx)) {
+		ret = PTR_ERR(ctx);
+		goto fail;
+	}
+
+	virqfd->eventfd = ctx;
+
+	/*
+	 * Install our own custom wake-up handling so we are notified via
+	 * a callback whenever someone signals the underlying eventfd.
+	 */
+	init_waitqueue_func_entry(&virqfd->wait, virqfd_wakeup);
+	init_poll_funcptr(&virqfd->pt, virqfd_ptable_queue_proc);
+
+	events = file->f_op->poll(file, &virqfd->pt);
+
+	/*
+	 * Check if there was an event already pending on the eventfd
+	 * before we registered and trigger it as if we didn't miss it.
+	 */
+	if (events & POLLIN) {
+		if ((!handler || handler(vdev, data)) && thread)
+			schedule_work(&virqfd->inject);
+	}
+
+	/*
+	 * Do not drop the file until the irqfd is fully initialized,
+	 * otherwise we might race against the POLLHUP.
+	 */
+	fput(file);
+
+	return 0;
+
+fail:
+	if (ctx && !IS_ERR(ctx))
+		eventfd_ctx_put(ctx);
+
+	if (file && !IS_ERR(file))
+		fput(file);
+
+	kfree(virqfd);
+	*pvirqfd = NULL;
+
+	return ret;
+}
+
+static void virqfd_disable(struct virqfd *virqfd)
+{
+	if (!virqfd)
+		return;
+
+	virqfd_deactivate(virqfd);
+
+	/* Block until we know all outstanding shutdown jobs have completed. */
+	flush_workqueue(vfio_irqfd_cleanup_wq);
+}
+
+/*
+ * INTx
+ */
+static void vfio_send_intx_eventfd(struct vfio_pci_device *vdev, void *unused)
+{
+	if (likely(is_intx(vdev) && !vdev->virq_disabled))
+		eventfd_signal(vdev->ctx[0].trigger, 1);
+}
+
+void vfio_pci_intx_mask(struct vfio_pci_device *vdev)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vdev->irqlock, flags);
+
+	/*
+	 * Masking can come from interrupt, ioctl, or config space
+	 * via INTx disable.  The latter means this can get called
+	 * even when not using intx delivery.  In this case, just
+	 * try to have the physical bit follow the virtual bit.
+	 */
+	if (unlikely(!is_intx(vdev))) {
+		if (vdev->pci_2_3)
+			pci_intx(pdev, 0);
+	} else if (!vdev->ctx[0].masked) {
+		/*
+		 * Can't use check_and_mask here because we always want to
+		 * mask, not just when something is pending.
+		 */
+		if (vdev->pci_2_3)
+			pci_intx(pdev, 0);
+		else
+			disable_irq_nosync(pdev->irq);
+
+		vdev->ctx[0].masked = true;
+	}
+
+	spin_unlock_irqrestore(&vdev->irqlock, flags);
+}
+
+/*
+ * If this is triggered by an eventfd, we can't call eventfd_signal
+ * or else we'll deadlock on the eventfd wait queue.  Return >0 when
+ * a signal is necessary, which can then be handled via a work queue
+ * or directly depending on the caller.
+ */
+int vfio_pci_intx_unmask_handler(struct vfio_pci_device *vdev, void *unused)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&vdev->irqlock, flags);
+
+	/*
+	 * Unmasking comes from ioctl or config, so again, have the
+	 * physical bit follow the virtual even when not using INTx.
+	 */
+	if (unlikely(!is_intx(vdev))) {
+		if (vdev->pci_2_3)
+			pci_intx(pdev, 1);
+	} else if (vdev->ctx[0].masked && !vdev->virq_disabled) {
+		/*
+		 * A pending interrupt here would immediately trigger,
+		 * but we can avoid that overhead by just re-sending
+		 * the interrupt to the user.
+		 */
+		if (vdev->pci_2_3) {
+			if (!pci_check_and_unmask_intx(pdev))
+				ret = 1;
+		} else
+			enable_irq(pdev->irq);
+
+		vdev->ctx[0].masked = (ret > 0);
+	}
+
+	spin_unlock_irqrestore(&vdev->irqlock, flags);
+
+	return ret;
+}
+
+void vfio_pci_intx_unmask(struct vfio_pci_device *vdev)
+{
+	if (vfio_pci_intx_unmask_handler(vdev, NULL) > 0)
+		vfio_send_intx_eventfd(vdev, NULL);
+}
+
+static irqreturn_t vfio_intx_handler(int irq, void *dev_id)
+{
+	struct vfio_pci_device *vdev = dev_id;
+	unsigned long flags;
+	int ret = IRQ_NONE;
+
+	spin_lock_irqsave(&vdev->irqlock, flags);
+
+	if (!vdev->pci_2_3) {
+		disable_irq_nosync(vdev->pdev->irq);
+		vdev->ctx[0].masked = true;
+		ret = IRQ_HANDLED;
+	} else if (!vdev->ctx[0].masked &&  /* may be shared */
+		   pci_check_and_mask_intx(vdev->pdev)) {
+		vdev->ctx[0].masked = true;
+		ret = IRQ_HANDLED;
+	}
+
+	spin_unlock_irqrestore(&vdev->irqlock, flags);
+
+	if (ret == IRQ_HANDLED)
+		vfio_send_intx_eventfd(vdev, NULL);
+
+	return ret;
+}
+
+static int vfio_intx_enable(struct vfio_pci_device *vdev)
+{
+	if (!is_irq_none(vdev))
+		return -EINVAL;
+
+	if (!vdev->pdev->irq)
+		return -ENODEV;
+
+	vdev->ctx = kzalloc(sizeof(struct vfio_pci_irq_ctx), GFP_KERNEL);
+	if (!vdev->ctx)
+		return -ENOMEM;
+
+	vdev->num_ctx = 1;
+	vdev->irq_type = VFIO_PCI_INTX_IRQ_INDEX;
+
+	return 0;
+}
+
+static int vfio_intx_set_signal(struct vfio_pci_device *vdev, int fd)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	unsigned long irqflags = IRQF_SHARED;
+	struct eventfd_ctx *trigger;
+	unsigned long flags;
+	int ret;
+
+	if (vdev->ctx[0].trigger) {
+		free_irq(pdev->irq, vdev);
+		kfree(vdev->ctx[0].name);
+		eventfd_ctx_put(vdev->ctx[0].trigger);
+		vdev->ctx[0].trigger = NULL;
+	}
+
+	if (fd < 0) /* Disable only */
+		return 0;
+
+	vdev->ctx[0].name = kasprintf(GFP_KERNEL, "vfio-intx(%s)",
+				      pci_name(pdev));
+	if (!vdev->ctx[0].name)
+		return -ENOMEM;
+
+	trigger = eventfd_ctx_fdget(fd);
+	if (IS_ERR(trigger)) {
+		kfree(vdev->ctx[0].name);
+		return PTR_ERR(trigger);
+	}
+
+	if (!vdev->pci_2_3)
+		irqflags = 0;
+
+	ret = request_irq(pdev->irq, vfio_intx_handler,
+			  irqflags, vdev->ctx[0].name, vdev);
+	if (ret) {
+		kfree(vdev->ctx[0].name);
+		eventfd_ctx_put(trigger);
+		return ret;
+	}
+
+	vdev->ctx[0].trigger = trigger;
+
+	/*
+	 * INTx disable will stick across the new irq setup,
+	 * disable_irq won't.
+	 */
+	spin_lock_irqsave(&vdev->irqlock, flags);
+	if (!vdev->pci_2_3 && (vdev->ctx[0].masked || vdev->virq_disabled))
+		disable_irq_nosync(pdev->irq);
+	spin_unlock_irqrestore(&vdev->irqlock, flags);
+
+	return 0;
+}
+
+static void vfio_intx_disable(struct vfio_pci_device *vdev)
+{
+	vfio_intx_set_signal(vdev, -1);
+	virqfd_disable(vdev->ctx[0].unmask);
+	virqfd_disable(vdev->ctx[0].mask);
+	vdev->irq_type = VFIO_PCI_NUM_IRQS;
+	vdev->num_ctx = 0;
+	kfree(vdev->ctx);
+}
+
+/*
+ * MSI/MSI-X
+ */
+static irqreturn_t vfio_msihandler(int irq, void *arg)
+{
+	struct eventfd_ctx *trigger = arg;
+
+	eventfd_signal(trigger, 1);
+	return IRQ_HANDLED;
+}
+
+static int vfio_msi_enable(struct vfio_pci_device *vdev, int nvec, bool msix)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int ret;
+
+	if (!is_irq_none(vdev))
+		return -EINVAL;
+
+	vdev->ctx = kzalloc(nvec * sizeof(struct vfio_pci_irq_ctx), GFP_KERNEL);
+	if (!vdev->ctx)
+		return -ENOMEM;
+
+	if (msix) {
+		int i;
+
+		vdev->msix = kzalloc(nvec * sizeof(struct msix_entry),
+				     GFP_KERNEL);
+		if (!vdev->msix) {
+			kfree(vdev->ctx);
+			return -ENOMEM;
+		}
+
+		for (i = 0; i < nvec; i++)
+			vdev->msix[i].entry = i;
+
+		ret = pci_enable_msix(pdev, vdev->msix, nvec);
+		if (ret) {
+			kfree(vdev->msix);
+			kfree(vdev->ctx);
+			return ret;
+		}
+	} else {
+		ret = pci_enable_msi_block(pdev, nvec);
+		if (ret) {
+			kfree(vdev->ctx);
+			return ret;
+		}
+	}
+
+	vdev->num_ctx = nvec;
+	vdev->irq_type = msix ? VFIO_PCI_MSIX_IRQ_INDEX :
+				VFIO_PCI_MSI_IRQ_INDEX;
+
+	if (!msix) {
+		/*
+		 * Compute the virtual hardware field for max msi vectors -
+		 * it is the log base 2 of the number of vectors.
+		 */
+		vdev->msi_qmax = fls(nvec * 2 - 1) - 1;
+	}
+
+	return 0;
+}
+
+static int vfio_msi_set_vector_signal(struct vfio_pci_device *vdev,
+				      int vector, int fd, bool msix)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int irq = msix ? vdev->msix[vector].vector : pdev->irq + vector;
+	char *name = msix ? "vfio-msix" : "vfio-msi";
+	struct eventfd_ctx *trigger;
+	int ret;
+
+	if (vector >= vdev->num_ctx)
+		return -EINVAL;
+
+	if (vdev->ctx[vector].trigger) {
+		free_irq(irq, vdev->ctx[vector].trigger);
+		kfree(vdev->ctx[vector].name);
+		eventfd_ctx_put(vdev->ctx[vector].trigger);
+		vdev->ctx[vector].trigger = NULL;
+	}
+
+	if (fd < 0)
+		return 0;
+
+	vdev->ctx[vector].name = kasprintf(GFP_KERNEL, "%s[%d](%s)",
+					   name, vector, pci_name(pdev));
+	if (!vdev->ctx[vector].name)
+		return -ENOMEM;
+
+	trigger = eventfd_ctx_fdget(fd);
+	if (IS_ERR(trigger)) {
+		kfree(vdev->ctx[vector].name);
+		return PTR_ERR(trigger);
+	}
+
+	ret = request_irq(irq, vfio_msihandler, 0,
+			  vdev->ctx[vector].name, trigger);
+	if (ret) {
+		kfree(vdev->ctx[vector].name);
+		eventfd_ctx_put(trigger);
+		return ret;
+	}
+
+	vdev->ctx[vector].trigger = trigger;
+
+	return 0;
+}
+
+static int vfio_msi_set_block(struct vfio_pci_device *vdev, unsigned start,
+			      unsigned count, int32_t *fds, bool msix)
+{
+	int i, j, ret = 0;
+
+	if (start + count > vdev->num_ctx)
+		return -EINVAL;
+
+	for (i = 0, j = start; i < count && !ret; i++, j++) {
+		int fd = fds ? fds[i] : -1;
+		ret = vfio_msi_set_vector_signal(vdev, j, fd, msix);
+	}
+
+	if (ret) {
+		for (--j; j >= start; j--)
+			vfio_msi_set_vector_signal(vdev, j, -1, msix);
+	}
+
+	return ret;
+}
+
+static void vfio_msi_disable(struct vfio_pci_device *vdev, bool msix)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	int i;
+
+	vfio_msi_set_block(vdev, 0, vdev->num_ctx, NULL, msix);
+
+	for (i = 0; i < vdev->num_ctx; i++) {
+		virqfd_disable(vdev->ctx[i].unmask);
+		virqfd_disable(vdev->ctx[i].mask);
+	}
+
+	if (msix) {
+		pci_disable_msix(vdev->pdev);
+		kfree(vdev->msix);
+	} else
+		pci_disable_msi(pdev);
+
+	vdev->irq_type = VFIO_PCI_NUM_IRQS;
+	vdev->num_ctx = 0;
+	kfree(vdev->ctx);
+}
+
+/*
+ * IOCTL support
+ */
+static int vfio_pci_set_intx_unmask(struct vfio_pci_device *vdev,
+				    unsigned index, unsigned start,
+				    unsigned count, uint32_t flags, void *data)
+{
+	if (!is_intx(vdev) || start != 0 || count != 1)
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		vfio_pci_intx_unmask(vdev);
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t unmask = *(uint8_t *)data;
+		if (unmask)
+			vfio_pci_intx_unmask(vdev);
+	} else if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		int32_t fd = *(int32_t *)data;
+		if (fd >= 0)
+			return virqfd_enable(vdev, vfio_pci_intx_unmask_handler,
+					     vfio_send_intx_eventfd, NULL,
+					     &vdev->ctx[0].unmask, fd);
+
+		virqfd_disable(vdev->ctx[0].unmask);
+	}
+
+	return 0;
+}
+
+static int vfio_pci_set_intx_mask(struct vfio_pci_device *vdev,
+				  unsigned index, unsigned start,
+				  unsigned count, uint32_t flags, void *data)
+{
+	if (!is_intx(vdev) || start != 0 || count != 1)
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		vfio_pci_intx_mask(vdev);
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t mask = *(uint8_t *)data;
+		if (mask)
+			vfio_pci_intx_mask(vdev);
+	} else if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		return -ENOTTY; /* XXX implement me */
+	}
+
+	return 0;
+}
+
+static int vfio_pci_set_intx_trigger(struct vfio_pci_device *vdev,
+				     unsigned index, unsigned start,
+				     unsigned count, uint32_t flags, void *data)
+{
+	if (is_intx(vdev) && !count && (flags & VFIO_IRQ_SET_DATA_NONE)) {
+		vfio_intx_disable(vdev);
+		return 0;
+	}
+
+	if (!(is_intx(vdev) || is_irq_none(vdev)) || start != 0 || count != 1)
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		int32_t fd = *(int32_t *)data;
+		int ret;
+
+		if (is_intx(vdev))
+			return vfio_intx_set_signal(vdev, fd);
+
+		ret = vfio_intx_enable(vdev);
+		if (ret)
+			return ret;
+
+		ret = vfio_intx_set_signal(vdev, fd);
+		if (ret)
+			vfio_intx_disable(vdev);
+
+		return ret;
+	}
+
+	if (!is_intx(vdev))
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_NONE) {
+		vfio_send_intx_eventfd(vdev, NULL);
+	} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+		uint8_t trigger = *(uint8_t *)data;
+		if (trigger)
+			vfio_send_intx_eventfd(vdev, NULL);
+	}
+	return 0;
+}
+
+static int vfio_pci_set_msi_trigger(struct vfio_pci_device *vdev,
+				    unsigned index, unsigned start,
+				    unsigned count, uint32_t flags, void *data)
+{
+	int i;
+	bool msix = (index == VFIO_PCI_MSIX_IRQ_INDEX) ? true : false;
+
+	if (irq_is(vdev, index) && !count && (flags & VFIO_IRQ_SET_DATA_NONE)) {
+		vfio_msi_disable(vdev, msix);
+		return 0;
+	}
+
+	if (!(irq_is(vdev, index) || is_irq_none(vdev)))
+		return -EINVAL;
+
+	if (flags & VFIO_IRQ_SET_DATA_EVENTFD) {
+		int32_t *fds = data;
+		int ret;
+
+		if (vdev->irq_type == index)
+			return vfio_msi_set_block(vdev, start, count,
+						  fds, msix);
+
+		ret = vfio_msi_enable(vdev, start + count, msix);
+		if (ret)
+			return ret;
+
+		ret = vfio_msi_set_block(vdev, start, count, fds, msix);
+		if (ret)
+			vfio_msi_disable(vdev, msix);
+
+		return ret;
+	}
+
+	if (!irq_is(vdev, index) || start + count > vdev->num_ctx)
+		return -EINVAL;
+
+	for (i = start; i < start + count; i++) {
+		if (!vdev->ctx[i].trigger)
+			continue;
+		if (flags & VFIO_IRQ_SET_DATA_NONE) {
+			eventfd_signal(vdev->ctx[i].trigger, 1);
+		} else if (flags & VFIO_IRQ_SET_DATA_BOOL) {
+			uint8_t *bools = data;
+			if (bools[i - start])
+				eventfd_signal(vdev->ctx[i].trigger, 1);
+		}
+	}
+	return 0;
+}
+
+int vfio_pci_set_irqs_ioctl(struct vfio_pci_device *vdev, uint32_t flags,
+			    unsigned index, unsigned start, unsigned count,
+			    void *data)
+{
+	int (*func)(struct vfio_pci_device *vdev, unsigned index,
+		    unsigned start, unsigned count, uint32_t flags,
+		    void *data) = NULL;
+
+	switch (index) {
+	case VFIO_PCI_INTX_IRQ_INDEX:
+		switch (flags & VFIO_IRQ_SET_ACTION_TYPE_MASK) {
+		case VFIO_IRQ_SET_ACTION_MASK:
+			func = vfio_pci_set_intx_mask;
+			break;
+		case VFIO_IRQ_SET_ACTION_UNMASK:
+			func = vfio_pci_set_intx_unmask;
+			break;
+		case VFIO_IRQ_SET_ACTION_TRIGGER:
+			func = vfio_pci_set_intx_trigger;
+			break;
+		}
+		break;
+	case VFIO_PCI_MSI_IRQ_INDEX:
+	case VFIO_PCI_MSIX_IRQ_INDEX:
+		switch (flags & VFIO_IRQ_SET_ACTION_TYPE_MASK) {
+		case VFIO_IRQ_SET_ACTION_MASK:
+		case VFIO_IRQ_SET_ACTION_UNMASK:
+			/* XXX Need masking support exported */
+			break;
+		case VFIO_IRQ_SET_ACTION_TRIGGER:
+			func = vfio_pci_set_msi_trigger;
+			break;
+		}
+		break;
+	}
+
+	if (!func)
+		return -ENOTTY;
+
+	return func(vdev, index, start, count, flags, data);
+}
diff --git a/drivers/vfio/pci/vfio_pci_private.h b/drivers/vfio/pci/vfio_pci_private.h
new file mode 100644
index 00000000000..611827cba8c
--- /dev/null
+++ b/drivers/vfio/pci/vfio_pci_private.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ */
+
+#include <linux/mutex.h>
+#include <linux/pci.h>
+
+#ifndef VFIO_PCI_PRIVATE_H
+#define VFIO_PCI_PRIVATE_H
+
+#define VFIO_PCI_OFFSET_SHIFT   40
+
+#define VFIO_PCI_OFFSET_TO_INDEX(off)	(off >> VFIO_PCI_OFFSET_SHIFT)
+#define VFIO_PCI_INDEX_TO_OFFSET(index)	((u64)(index) << VFIO_PCI_OFFSET_SHIFT)
+#define VFIO_PCI_OFFSET_MASK	(((u64)(1) << VFIO_PCI_OFFSET_SHIFT) - 1)
+
+struct vfio_pci_irq_ctx {
+	struct eventfd_ctx	*trigger;
+	struct virqfd		*unmask;
+	struct virqfd		*mask;
+	char			*name;
+	bool			masked;
+};
+
+struct vfio_pci_device {
+	struct pci_dev		*pdev;
+	void __iomem		*barmap[PCI_STD_RESOURCE_END + 1];
+	u8			*pci_config_map;
+	u8			*vconfig;
+	struct perm_bits	*msi_perm;
+	spinlock_t		irqlock;
+	struct mutex		igate;
+	struct msix_entry	*msix;
+	struct vfio_pci_irq_ctx	*ctx;
+	int			num_ctx;
+	int			irq_type;
+	u8			msi_qmax;
+	u8			msix_bar;
+	u16			msix_size;
+	u32			msix_offset;
+	u32			rbar[7];
+	bool			pci_2_3;
+	bool			virq_disabled;
+	bool			reset_works;
+	bool			extended_caps;
+	bool			bardirty;
+	struct pci_saved_state	*pci_saved_state;
+	atomic_t		refcnt;
+};
+
+#define is_intx(vdev) (vdev->irq_type == VFIO_PCI_INTX_IRQ_INDEX)
+#define is_msi(vdev) (vdev->irq_type == VFIO_PCI_MSI_IRQ_INDEX)
+#define is_msix(vdev) (vdev->irq_type == VFIO_PCI_MSIX_IRQ_INDEX)
+#define is_irq_none(vdev) (!(is_intx(vdev) || is_msi(vdev) || is_msix(vdev)))
+#define irq_is(vdev, type) (vdev->irq_type == type)
+
+extern void vfio_pci_intx_mask(struct vfio_pci_device *vdev);
+extern void vfio_pci_intx_unmask(struct vfio_pci_device *vdev);
+
+extern int vfio_pci_set_irqs_ioctl(struct vfio_pci_device *vdev,
+				   uint32_t flags, unsigned index,
+				   unsigned start, unsigned count, void *data);
+
+extern ssize_t vfio_pci_config_readwrite(struct vfio_pci_device *vdev,
+					 char __user *buf, size_t count,
+					 loff_t *ppos, bool iswrite);
+extern ssize_t vfio_pci_mem_readwrite(struct vfio_pci_device *vdev,
+				      char __user *buf, size_t count,
+				      loff_t *ppos, bool iswrite);
+extern ssize_t vfio_pci_io_readwrite(struct vfio_pci_device *vdev,
+				     char __user *buf, size_t count,
+				     loff_t *ppos, bool iswrite);
+
+extern int vfio_pci_init_perm_bits(void);
+extern void vfio_pci_uninit_perm_bits(void);
+
+extern int vfio_pci_virqfd_init(void);
+extern void vfio_pci_virqfd_exit(void);
+
+extern int vfio_config_init(struct vfio_pci_device *vdev);
+extern void vfio_config_free(struct vfio_pci_device *vdev);
+#endif /* VFIO_PCI_PRIVATE_H */
diff --git a/drivers/vfio/pci/vfio_pci_rdwr.c b/drivers/vfio/pci/vfio_pci_rdwr.c
new file mode 100644
index 00000000000..4362d9e7baa
--- /dev/null
+++ b/drivers/vfio/pci/vfio_pci_rdwr.c
@@ -0,0 +1,269 @@
+/*
+ * VFIO PCI I/O Port & MMIO access
+ *
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ */
+
+#include <linux/fs.h>
+#include <linux/pci.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "vfio_pci_private.h"
+
+/* I/O Port BAR access */
+ssize_t vfio_pci_io_readwrite(struct vfio_pci_device *vdev, char __user *buf,
+			      size_t count, loff_t *ppos, bool iswrite)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	loff_t pos = *ppos & VFIO_PCI_OFFSET_MASK;
+	int bar = VFIO_PCI_OFFSET_TO_INDEX(*ppos);
+	void __iomem *io;
+	size_t done = 0;
+
+	if (!pci_resource_start(pdev, bar))
+		return -EINVAL;
+
+	if (pos + count > pci_resource_len(pdev, bar))
+		return -EINVAL;
+
+	if (!vdev->barmap[bar]) {
+		int ret;
+
+		ret = pci_request_selected_regions(pdev, 1 << bar, "vfio");
+		if (ret)
+			return ret;
+
+		vdev->barmap[bar] = pci_iomap(pdev, bar, 0);
+
+		if (!vdev->barmap[bar]) {
+			pci_release_selected_regions(pdev, 1 << bar);
+			return -EINVAL;
+		}
+	}
+
+	io = vdev->barmap[bar];
+
+	while (count) {
+		int filled;
+
+		if (count >= 3 && !(pos % 4)) {
+			__le32 val;
+
+			if (iswrite) {
+				if (copy_from_user(&val, buf, 4))
+					return -EFAULT;
+
+				iowrite32(le32_to_cpu(val), io + pos);
+			} else {
+				val = cpu_to_le32(ioread32(io + pos));
+
+				if (copy_to_user(buf, &val, 4))
+					return -EFAULT;
+			}
+
+			filled = 4;
+
+		} else if ((pos % 2) == 0 && count >= 2) {
+			__le16 val;
+
+			if (iswrite) {
+				if (copy_from_user(&val, buf, 2))
+					return -EFAULT;
+
+				iowrite16(le16_to_cpu(val), io + pos);
+			} else {
+				val = cpu_to_le16(ioread16(io + pos));
+
+				if (copy_to_user(buf, &val, 2))
+					return -EFAULT;
+			}
+
+			filled = 2;
+		} else {
+			u8 val;
+
+			if (iswrite) {
+				if (copy_from_user(&val, buf, 1))
+					return -EFAULT;
+
+				iowrite8(val, io + pos);
+			} else {
+				val = ioread8(io + pos);
+
+				if (copy_to_user(buf, &val, 1))
+					return -EFAULT;
+			}
+
+			filled = 1;
+		}
+
+		count -= filled;
+		done += filled;
+		buf += filled;
+		pos += filled;
+	}
+
+	*ppos += done;
+
+	return done;
+}
+
+/*
+ * MMIO BAR access
+ * We handle two excluded ranges here as well, if the user tries to read
+ * the ROM beyond what PCI tells us is available or the MSI-X table region,
+ * we return 0xFF and writes are dropped.
+ */
+ssize_t vfio_pci_mem_readwrite(struct vfio_pci_device *vdev, char __user *buf,
+			       size_t count, loff_t *ppos, bool iswrite)
+{
+	struct pci_dev *pdev = vdev->pdev;
+	loff_t pos = *ppos & VFIO_PCI_OFFSET_MASK;
+	int bar = VFIO_PCI_OFFSET_TO_INDEX(*ppos);
+	void __iomem *io;
+	resource_size_t end;
+	size_t done = 0;
+	size_t x_start = 0, x_end = 0; /* excluded range */
+
+	if (!pci_resource_start(pdev, bar))
+		return -EINVAL;
+
+	end = pci_resource_len(pdev, bar);
+
+	if (pos > end)
+		return -EINVAL;
+
+	if (pos == end)
+		return 0;
+
+	if (pos + count > end)
+		count = end - pos;
+
+	if (bar == PCI_ROM_RESOURCE) {
+		io = pci_map_rom(pdev, &x_start);
+		x_end = end;
+	} else {
+		if (!vdev->barmap[bar]) {
+			int ret;
+
+			ret = pci_request_selected_regions(pdev, 1 << bar,
+							   "vfio");
+			if (ret)
+				return ret;
+
+			vdev->barmap[bar] = pci_iomap(pdev, bar, 0);
+
+			if (!vdev->barmap[bar]) {
+				pci_release_selected_regions(pdev, 1 << bar);
+				return -EINVAL;
+			}
+		}
+
+		io = vdev->barmap[bar];
+
+		if (bar == vdev->msix_bar) {
+			x_start = vdev->msix_offset;
+			x_end = vdev->msix_offset + vdev->msix_size;
+		}
+	}
+
+	if (!io)
+		return -EINVAL;
+
+	while (count) {
+		size_t fillable, filled;
+
+		if (pos < x_start)
+			fillable = x_start - pos;
+		else if (pos >= x_end)
+			fillable = end - pos;
+		else
+			fillable = 0;
+
+		if (fillable >= 4 && !(pos % 4) && (count >= 4)) {
+			__le32 val;
+
+			if (iswrite) {
+				if (copy_from_user(&val, buf, 4))
+					goto out;
+
+				iowrite32(le32_to_cpu(val), io + pos);
+			} else {
+				val = cpu_to_le32(ioread32(io + pos));
+
+				if (copy_to_user(buf, &val, 4))
+					goto out;
+			}
+
+			filled = 4;
+		} else if (fillable >= 2 && !(pos % 2) && (count >= 2)) {
+			__le16 val;
+
+			if (iswrite) {
+				if (copy_from_user(&val, buf, 2))
+					goto out;
+
+				iowrite16(le16_to_cpu(val), io + pos);
+			} else {
+				val = cpu_to_le16(ioread16(io + pos));
+
+				if (copy_to_user(buf, &val, 2))
+					goto out;
+			}
+
+			filled = 2;
+		} else if (fillable) {
+			u8 val;
+
+			if (iswrite) {
+				if (copy_from_user(&val, buf, 1))
+					goto out;
+
+				iowrite8(val, io + pos);
+			} else {
+				val = ioread8(io + pos);
+
+				if (copy_to_user(buf, &val, 1))
+					goto out;
+			}
+
+			filled = 1;
+		} else {
+			/* Drop writes, fill reads with FF */
+			if (!iswrite) {
+				char val = 0xFF;
+				size_t i;
+
+				for (i = 0; i < x_end - pos; i++) {
+					if (put_user(val, buf + i))
+						goto out;
+				}
+			}
+
+			filled = x_end - pos;
+		}
+
+		count -= filled;
+		done += filled;
+		buf += filled;
+		pos += filled;
+	}
+
+	*ppos += done;
+
+out:
+	if (bar == PCI_ROM_RESOURCE)
+		pci_unmap_rom(pdev, io);
+
+	return count ? -EFAULT : done;
+}
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
new file mode 100644
index 00000000000..9591e2b509d
--- /dev/null
+++ b/drivers/vfio/vfio.c
@@ -0,0 +1,1420 @@
+/*
+ * VFIO core
+ *
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ */
+
+#include <linux/cdev.h>
+#include <linux/compat.h>
+#include <linux/device.h>
+#include <linux/file.h>
+#include <linux/anon_inodes.h>
+#include <linux/fs.h>
+#include <linux/idr.h>
+#include <linux/iommu.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <linux/vfio.h>
+#include <linux/wait.h>
+
+#define DRIVER_VERSION	"0.3"
+#define DRIVER_AUTHOR	"Alex Williamson <alex.williamson@redhat.com>"
+#define DRIVER_DESC	"VFIO - User Level meta-driver"
+
+static struct vfio {
+	struct class			*class;
+	struct list_head		iommu_drivers_list;
+	struct mutex			iommu_drivers_lock;
+	struct list_head		group_list;
+	struct idr			group_idr;
+	struct mutex			group_lock;
+	struct cdev			group_cdev;
+	struct device			*dev;
+	dev_t				devt;
+	struct cdev			cdev;
+	wait_queue_head_t		release_q;
+} vfio;
+
+struct vfio_iommu_driver {
+	const struct vfio_iommu_driver_ops	*ops;
+	struct list_head			vfio_next;
+};
+
+struct vfio_container {
+	struct kref			kref;
+	struct list_head		group_list;
+	struct mutex			group_lock;
+	struct vfio_iommu_driver	*iommu_driver;
+	void				*iommu_data;
+};
+
+struct vfio_group {
+	struct kref			kref;
+	int				minor;
+	atomic_t			container_users;
+	struct iommu_group		*iommu_group;
+	struct vfio_container		*container;
+	struct list_head		device_list;
+	struct mutex			device_lock;
+	struct device			*dev;
+	struct notifier_block		nb;
+	struct list_head		vfio_next;
+	struct list_head		container_next;
+};
+
+struct vfio_device {
+	struct kref			kref;
+	struct device			*dev;
+	const struct vfio_device_ops	*ops;
+	struct vfio_group		*group;
+	struct list_head		group_next;
+	void				*device_data;
+};
+
+/**
+ * IOMMU driver registration
+ */
+int vfio_register_iommu_driver(const struct vfio_iommu_driver_ops *ops)
+{
+	struct vfio_iommu_driver *driver, *tmp;
+
+	driver = kzalloc(sizeof(*driver), GFP_KERNEL);
+	if (!driver)
+		return -ENOMEM;
+
+	driver->ops = ops;
+
+	mutex_lock(&vfio.iommu_drivers_lock);
+
+	/* Check for duplicates */
+	list_for_each_entry(tmp, &vfio.iommu_drivers_list, vfio_next) {
+		if (tmp->ops == ops) {
+			mutex_unlock(&vfio.iommu_drivers_lock);
+			kfree(driver);
+			return -EINVAL;
+		}
+	}
+
+	list_add(&driver->vfio_next, &vfio.iommu_drivers_list);
+
+	mutex_unlock(&vfio.iommu_drivers_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vfio_register_iommu_driver);
+
+void vfio_unregister_iommu_driver(const struct vfio_iommu_driver_ops *ops)
+{
+	struct vfio_iommu_driver *driver;
+
+	mutex_lock(&vfio.iommu_drivers_lock);
+	list_for_each_entry(driver, &vfio.iommu_drivers_list, vfio_next) {
+		if (driver->ops == ops) {
+			list_del(&driver->vfio_next);
+			mutex_unlock(&vfio.iommu_drivers_lock);
+			kfree(driver);
+			return;
+		}
+	}
+	mutex_unlock(&vfio.iommu_drivers_lock);
+}
+EXPORT_SYMBOL_GPL(vfio_unregister_iommu_driver);
+
+/**
+ * Group minor allocation/free - both called with vfio.group_lock held
+ */
+static int vfio_alloc_group_minor(struct vfio_group *group)
+{
+	int ret, minor;
+
+again:
+	if (unlikely(idr_pre_get(&vfio.group_idr, GFP_KERNEL) == 0))
+		return -ENOMEM;
+
+	/* index 0 is used by /dev/vfio/vfio */
+	ret = idr_get_new_above(&vfio.group_idr, group, 1, &minor);
+	if (ret == -EAGAIN)
+		goto again;
+	if (ret || minor > MINORMASK) {
+		if (minor > MINORMASK)
+			idr_remove(&vfio.group_idr, minor);
+		return -ENOSPC;
+	}
+
+	return minor;
+}
+
+static void vfio_free_group_minor(int minor)
+{
+	idr_remove(&vfio.group_idr, minor);
+}
+
+static int vfio_iommu_group_notifier(struct notifier_block *nb,
+				     unsigned long action, void *data);
+static void vfio_group_get(struct vfio_group *group);
+
+/**
+ * Container objects - containers are created when /dev/vfio/vfio is
+ * opened, but their lifecycle extends until the last user is done, so
+ * it's freed via kref.  Must support container/group/device being
+ * closed in any order.
+ */
+static void vfio_container_get(struct vfio_container *container)
+{
+	kref_get(&container->kref);
+}
+
+static void vfio_container_release(struct kref *kref)
+{
+	struct vfio_container *container;
+	container = container_of(kref, struct vfio_container, kref);
+
+	kfree(container);
+}
+
+static void vfio_container_put(struct vfio_container *container)
+{
+	kref_put(&container->kref, vfio_container_release);
+}
+
+/**
+ * Group objects - create, release, get, put, search
+ */
+static struct vfio_group *vfio_create_group(struct iommu_group *iommu_group)
+{
+	struct vfio_group *group, *tmp;
+	struct device *dev;
+	int ret, minor;
+
+	group = kzalloc(sizeof(*group), GFP_KERNEL);
+	if (!group)
+		return ERR_PTR(-ENOMEM);
+
+	kref_init(&group->kref);
+	INIT_LIST_HEAD(&group->device_list);
+	mutex_init(&group->device_lock);
+	atomic_set(&group->container_users, 0);
+	group->iommu_group = iommu_group;
+
+	group->nb.notifier_call = vfio_iommu_group_notifier;
+
+	/*
+	 * blocking notifiers acquire a rwsem around registering and hold
+	 * it around callback.  Therefore, need to register outside of
+	 * vfio.group_lock to avoid A-B/B-A contention.  Our callback won't
+	 * do anything unless it can find the group in vfio.group_list, so
+	 * no harm in registering early.
+	 */
+	ret = iommu_group_register_notifier(iommu_group, &group->nb);
+	if (ret) {
+		kfree(group);
+		return ERR_PTR(ret);
+	}
+
+	mutex_lock(&vfio.group_lock);
+
+	minor = vfio_alloc_group_minor(group);
+	if (minor < 0) {
+		mutex_unlock(&vfio.group_lock);
+		kfree(group);
+		return ERR_PTR(minor);
+	}
+
+	/* Did we race creating this group? */
+	list_for_each_entry(tmp, &vfio.group_list, vfio_next) {
+		if (tmp->iommu_group == iommu_group) {
+			vfio_group_get(tmp);
+			vfio_free_group_minor(minor);
+			mutex_unlock(&vfio.group_lock);
+			kfree(group);
+			return tmp;
+		}
+	}
+
+	dev = device_create(vfio.class, NULL, MKDEV(MAJOR(vfio.devt), minor),
+			    group, "%d", iommu_group_id(iommu_group));
+	if (IS_ERR(dev)) {
+		vfio_free_group_minor(minor);
+		mutex_unlock(&vfio.group_lock);
+		kfree(group);
+		return (struct vfio_group *)dev; /* ERR_PTR */
+	}
+
+	group->minor = minor;
+	group->dev = dev;
+
+	list_add(&group->vfio_next, &vfio.group_list);
+
+	mutex_unlock(&vfio.group_lock);
+
+	return group;
+}
+
+static void vfio_group_release(struct kref *kref)
+{
+	struct vfio_group *group = container_of(kref, struct vfio_group, kref);
+
+	WARN_ON(!list_empty(&group->device_list));
+
+	device_destroy(vfio.class, MKDEV(MAJOR(vfio.devt), group->minor));
+	list_del(&group->vfio_next);
+	vfio_free_group_minor(group->minor);
+
+	mutex_unlock(&vfio.group_lock);
+
+	/*
+	 * Unregister outside of lock.  A spurious callback is harmless now
+	 * that the group is no longer in vfio.group_list.
+	 */
+	iommu_group_unregister_notifier(group->iommu_group, &group->nb);
+
+	kfree(group);
+}
+
+static void vfio_group_put(struct vfio_group *group)
+{
+	mutex_lock(&vfio.group_lock);
+	/*
+	 * Release needs to unlock to unregister the notifier, so only
+	 * unlock if not released.
+	 */
+	if (!kref_put(&group->kref, vfio_group_release))
+		mutex_unlock(&vfio.group_lock);
+}
+
+/* Assume group_lock or group reference is held */
+static void vfio_group_get(struct vfio_group *group)
+{
+	kref_get(&group->kref);
+}
+
+/*
+ * Not really a try as we will sleep for mutex, but we need to make
+ * sure the group pointer is valid under lock and get a reference.
+ */
+static struct vfio_group *vfio_group_try_get(struct vfio_group *group)
+{
+	struct vfio_group *target = group;
+
+	mutex_lock(&vfio.group_lock);
+	list_for_each_entry(group, &vfio.group_list, vfio_next) {
+		if (group == target) {
+			vfio_group_get(group);
+			mutex_unlock(&vfio.group_lock);
+			return group;
+		}
+	}
+	mutex_unlock(&vfio.group_lock);
+
+	return NULL;
+}
+
+static
+struct vfio_group *vfio_group_get_from_iommu(struct iommu_group *iommu_group)
+{
+	struct vfio_group *group;
+
+	mutex_lock(&vfio.group_lock);
+	list_for_each_entry(group, &vfio.group_list, vfio_next) {
+		if (group->iommu_group == iommu_group) {
+			vfio_group_get(group);
+			mutex_unlock(&vfio.group_lock);
+			return group;
+		}
+	}
+	mutex_unlock(&vfio.group_lock);
+
+	return NULL;
+}
+
+static struct vfio_group *vfio_group_get_from_minor(int minor)
+{
+	struct vfio_group *group;
+
+	mutex_lock(&vfio.group_lock);
+	group = idr_find(&vfio.group_idr, minor);
+	if (!group) {
+		mutex_unlock(&vfio.group_lock);
+		return NULL;
+	}
+	vfio_group_get(group);
+	mutex_unlock(&vfio.group_lock);
+
+	return group;
+}
+
+/**
+ * Device objects - create, release, get, put, search
+ */
+static
+struct vfio_device *vfio_group_create_device(struct vfio_group *group,
+					     struct device *dev,
+					     const struct vfio_device_ops *ops,
+					     void *device_data)
+{
+	struct vfio_device *device;
+	int ret;
+
+	device = kzalloc(sizeof(*device), GFP_KERNEL);
+	if (!device)
+		return ERR_PTR(-ENOMEM);
+
+	kref_init(&device->kref);
+	device->dev = dev;
+	device->group = group;
+	device->ops = ops;
+	device->device_data = device_data;
+
+	ret = dev_set_drvdata(dev, device);
+	if (ret) {
+		kfree(device);
+		return ERR_PTR(ret);
+	}
+
+	/* No need to get group_lock, caller has group reference */
+	vfio_group_get(group);
+
+	mutex_lock(&group->device_lock);
+	list_add(&device->group_next, &group->device_list);
+	mutex_unlock(&group->device_lock);
+
+	return device;
+}
+
+static void vfio_device_release(struct kref *kref)
+{
+	struct vfio_device *device = container_of(kref,
+						  struct vfio_device, kref);
+	struct vfio_group *group = device->group;
+
+	mutex_lock(&group->device_lock);
+	list_del(&device->group_next);
+	mutex_unlock(&group->device_lock);
+
+	dev_set_drvdata(device->dev, NULL);
+
+	kfree(device);
+
+	/* vfio_del_group_dev may be waiting for this device */
+	wake_up(&vfio.release_q);
+}
+
+/* Device reference always implies a group reference */
+static void vfio_device_put(struct vfio_device *device)
+{
+	kref_put(&device->kref, vfio_device_release);
+	vfio_group_put(device->group);
+}
+
+static void vfio_device_get(struct vfio_device *device)
+{
+	vfio_group_get(device->group);
+	kref_get(&device->kref);
+}
+
+static struct vfio_device *vfio_group_get_device(struct vfio_group *group,
+						 struct device *dev)
+{
+	struct vfio_device *device;
+
+	mutex_lock(&group->device_lock);
+	list_for_each_entry(device, &group->device_list, group_next) {
+		if (device->dev == dev) {
+			vfio_device_get(device);
+			mutex_unlock(&group->device_lock);
+			return device;
+		}
+	}
+	mutex_unlock(&group->device_lock);
+	return NULL;
+}
+
+/*
+ * Whitelist some drivers that we know are safe (no dma) or just sit on
+ * a device.  It's not always practical to leave a device within a group
+ * driverless as it could get re-bound to something unsafe.
+ */
+static const char * const vfio_driver_whitelist[] = { "pci-stub" };
+
+static bool vfio_whitelisted_driver(struct device_driver *drv)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vfio_driver_whitelist); i++) {
+		if (!strcmp(drv->name, vfio_driver_whitelist[i]))
+			return true;
+	}
+
+	return false;
+}
+
+/*
+ * A vfio group is viable for use by userspace if all devices are either
+ * driver-less or bound to a vfio or whitelisted driver.  We test the
+ * latter by the existence of a struct vfio_device matching the dev.
+ */
+static int vfio_dev_viable(struct device *dev, void *data)
+{
+	struct vfio_group *group = data;
+	struct vfio_device *device;
+
+	if (!dev->driver || vfio_whitelisted_driver(dev->driver))
+		return 0;
+
+	device = vfio_group_get_device(group, dev);
+	if (device) {
+		vfio_device_put(device);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * Async device support
+ */
+static int vfio_group_nb_add_dev(struct vfio_group *group, struct device *dev)
+{
+	struct vfio_device *device;
+
+	/* Do we already know about it?  We shouldn't */
+	device = vfio_group_get_device(group, dev);
+	if (WARN_ON_ONCE(device)) {
+		vfio_device_put(device);
+		return 0;
+	}
+
+	/* Nothing to do for idle groups */
+	if (!atomic_read(&group->container_users))
+		return 0;
+
+	/* TODO Prevent device auto probing */
+	WARN("Device %s added to live group %d!\n", dev_name(dev),
+	     iommu_group_id(group->iommu_group));
+
+	return 0;
+}
+
+static int vfio_group_nb_del_dev(struct vfio_group *group, struct device *dev)
+{
+	struct vfio_device *device;
+
+	/*
+	 * Expect to fall out here.  If a device was in use, it would
+	 * have been bound to a vfio sub-driver, which would have blocked
+	 * in .remove at vfio_del_group_dev.  Sanity check that we no
+	 * longer track the device, so it's safe to remove.
+	 */
+	device = vfio_group_get_device(group, dev);
+	if (likely(!device))
+		return 0;
+
+	WARN("Device %s removed from live group %d!\n", dev_name(dev),
+	     iommu_group_id(group->iommu_group));
+
+	vfio_device_put(device);
+	return 0;
+}
+
+static int vfio_group_nb_verify(struct vfio_group *group, struct device *dev)
+{
+	/* We don't care what happens when the group isn't in use */
+	if (!atomic_read(&group->container_users))
+		return 0;
+
+	return vfio_dev_viable(dev, group);
+}
+
+static int vfio_iommu_group_notifier(struct notifier_block *nb,
+				     unsigned long action, void *data)
+{
+	struct vfio_group *group = container_of(nb, struct vfio_group, nb);
+	struct device *dev = data;
+
+	/*
+	 * Need to go through a group_lock lookup to get a reference or
+	 * we risk racing a group being removed.  Leave a WARN_ON for
+	 * debuging, but if the group no longer exists, a spurious notify
+	 * is harmless.
+	 */
+	group = vfio_group_try_get(group);
+	if (WARN_ON(!group))
+		return NOTIFY_OK;
+
+	switch (action) {
+	case IOMMU_GROUP_NOTIFY_ADD_DEVICE:
+		vfio_group_nb_add_dev(group, dev);
+		break;
+	case IOMMU_GROUP_NOTIFY_DEL_DEVICE:
+		vfio_group_nb_del_dev(group, dev);
+		break;
+	case IOMMU_GROUP_NOTIFY_BIND_DRIVER:
+		pr_debug("%s: Device %s, group %d binding to driver\n",
+			 __func__, dev_name(dev),
+			 iommu_group_id(group->iommu_group));
+		break;
+	case IOMMU_GROUP_NOTIFY_BOUND_DRIVER:
+		pr_debug("%s: Device %s, group %d bound to driver %s\n",
+			 __func__, dev_name(dev),
+			 iommu_group_id(group->iommu_group), dev->driver->name);
+		BUG_ON(vfio_group_nb_verify(group, dev));
+		break;
+	case IOMMU_GROUP_NOTIFY_UNBIND_DRIVER:
+		pr_debug("%s: Device %s, group %d unbinding from driver %s\n",
+			 __func__, dev_name(dev),
+			 iommu_group_id(group->iommu_group), dev->driver->name);
+		break;
+	case IOMMU_GROUP_NOTIFY_UNBOUND_DRIVER:
+		pr_debug("%s: Device %s, group %d unbound from driver\n",
+			 __func__, dev_name(dev),
+			 iommu_group_id(group->iommu_group));
+		/*
+		 * XXX An unbound device in a live group is ok, but we'd
+		 * really like to avoid the above BUG_ON by preventing other
+		 * drivers from binding to it.  Once that occurs, we have to
+		 * stop the system to maintain isolation.  At a minimum, we'd
+		 * want a toggle to disable driver auto probe for this device.
+		 */
+		break;
+	}
+
+	vfio_group_put(group);
+	return NOTIFY_OK;
+}
+
+/**
+ * VFIO driver API
+ */
+int vfio_add_group_dev(struct device *dev,
+		       const struct vfio_device_ops *ops, void *device_data)
+{
+	struct iommu_group *iommu_group;
+	struct vfio_group *group;
+	struct vfio_device *device;
+
+	iommu_group = iommu_group_get(dev);
+	if (!iommu_group)
+		return -EINVAL;
+
+	group = vfio_group_get_from_iommu(iommu_group);
+	if (!group) {
+		group = vfio_create_group(iommu_group);
+		if (IS_ERR(group)) {
+			iommu_group_put(iommu_group);
+			return PTR_ERR(group);
+		}
+	}
+
+	device = vfio_group_get_device(group, dev);
+	if (device) {
+		WARN(1, "Device %s already exists on group %d\n",
+		     dev_name(dev), iommu_group_id(iommu_group));
+		vfio_device_put(device);
+		vfio_group_put(group);
+		iommu_group_put(iommu_group);
+		return -EBUSY;
+	}
+
+	device = vfio_group_create_device(group, dev, ops, device_data);
+	if (IS_ERR(device)) {
+		vfio_group_put(group);
+		iommu_group_put(iommu_group);
+		return PTR_ERR(device);
+	}
+
+	/*
+	 * Added device holds reference to iommu_group and vfio_device
+	 * (which in turn holds reference to vfio_group).  Drop extra
+	 * group reference used while acquiring device.
+	 */
+	vfio_group_put(group);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vfio_add_group_dev);
+
+/* Test whether a struct device is present in our tracking */
+static bool vfio_dev_present(struct device *dev)
+{
+	struct iommu_group *iommu_group;
+	struct vfio_group *group;
+	struct vfio_device *device;
+
+	iommu_group = iommu_group_get(dev);
+	if (!iommu_group)
+		return false;
+
+	group = vfio_group_get_from_iommu(iommu_group);
+	if (!group) {
+		iommu_group_put(iommu_group);
+		return false;
+	}
+
+	device = vfio_group_get_device(group, dev);
+	if (!device) {
+		vfio_group_put(group);
+		iommu_group_put(iommu_group);
+		return false;
+	}
+
+	vfio_device_put(device);
+	vfio_group_put(group);
+	iommu_group_put(iommu_group);
+	return true;
+}
+
+/*
+ * Decrement the device reference count and wait for the device to be
+ * removed.  Open file descriptors for the device... */
+void *vfio_del_group_dev(struct device *dev)
+{
+	struct vfio_device *device = dev_get_drvdata(dev);
+	struct vfio_group *group = device->group;
+	struct iommu_group *iommu_group = group->iommu_group;
+	void *device_data = device->device_data;
+
+	vfio_device_put(device);
+
+	/* TODO send a signal to encourage this to be released */
+	wait_event(vfio.release_q, !vfio_dev_present(dev));
+
+	iommu_group_put(iommu_group);
+
+	return device_data;
+}
+EXPORT_SYMBOL_GPL(vfio_del_group_dev);
+
+/**
+ * VFIO base fd, /dev/vfio/vfio
+ */
+static long vfio_ioctl_check_extension(struct vfio_container *container,
+				       unsigned long arg)
+{
+	struct vfio_iommu_driver *driver = container->iommu_driver;
+	long ret = 0;
+
+	switch (arg) {
+		/* No base extensions yet */
+	default:
+		/*
+		 * If no driver is set, poll all registered drivers for
+		 * extensions and return the first positive result.  If
+		 * a driver is already set, further queries will be passed
+		 * only to that driver.
+		 */
+		if (!driver) {
+			mutex_lock(&vfio.iommu_drivers_lock);
+			list_for_each_entry(driver, &vfio.iommu_drivers_list,
+					    vfio_next) {
+				if (!try_module_get(driver->ops->owner))
+					continue;
+
+				ret = driver->ops->ioctl(NULL,
+							 VFIO_CHECK_EXTENSION,
+							 arg);
+				module_put(driver->ops->owner);
+				if (ret > 0)
+					break;
+			}
+			mutex_unlock(&vfio.iommu_drivers_lock);
+		} else
+			ret = driver->ops->ioctl(container->iommu_data,
+						 VFIO_CHECK_EXTENSION, arg);
+	}
+
+	return ret;
+}
+
+/* hold container->group_lock */
+static int __vfio_container_attach_groups(struct vfio_container *container,
+					  struct vfio_iommu_driver *driver,
+					  void *data)
+{
+	struct vfio_group *group;
+	int ret = -ENODEV;
+
+	list_for_each_entry(group, &container->group_list, container_next) {
+		ret = driver->ops->attach_group(data, group->iommu_group);
+		if (ret)
+			goto unwind;
+	}
+
+	return ret;
+
+unwind:
+	list_for_each_entry_continue_reverse(group, &container->group_list,
+					     container_next) {
+		driver->ops->detach_group(data, group->iommu_group);
+	}
+
+	return ret;
+}
+
+static long vfio_ioctl_set_iommu(struct vfio_container *container,
+				 unsigned long arg)
+{
+	struct vfio_iommu_driver *driver;
+	long ret = -ENODEV;
+
+	mutex_lock(&container->group_lock);
+
+	/*
+	 * The container is designed to be an unprivileged interface while
+	 * the group can be assigned to specific users.  Therefore, only by
+	 * adding a group to a container does the user get the privilege of
+	 * enabling the iommu, which may allocate finite resources.  There
+	 * is no unset_iommu, but by removing all the groups from a container,
+	 * the container is deprivileged and returns to an unset state.
+	 */
+	if (list_empty(&container->group_list) || container->iommu_driver) {
+		mutex_unlock(&container->group_lock);
+		return -EINVAL;
+	}
+
+	mutex_lock(&vfio.iommu_drivers_lock);
+	list_for_each_entry(driver, &vfio.iommu_drivers_list, vfio_next) {
+		void *data;
+
+		if (!try_module_get(driver->ops->owner))
+			continue;
+
+		/*
+		 * The arg magic for SET_IOMMU is the same as CHECK_EXTENSION,
+		 * so test which iommu driver reported support for this
+		 * extension and call open on them.  We also pass them the
+		 * magic, allowing a single driver to support multiple
+		 * interfaces if they'd like.
+		 */
+		if (driver->ops->ioctl(NULL, VFIO_CHECK_EXTENSION, arg) <= 0) {
+			module_put(driver->ops->owner);
+			continue;
+		}
+
+		/* module reference holds the driver we're working on */
+		mutex_unlock(&vfio.iommu_drivers_lock);
+
+		data = driver->ops->open(arg);
+		if (IS_ERR(data)) {
+			ret = PTR_ERR(data);
+			module_put(driver->ops->owner);
+			goto skip_drivers_unlock;
+		}
+
+		ret = __vfio_container_attach_groups(container, driver, data);
+		if (!ret) {
+			container->iommu_driver = driver;
+			container->iommu_data = data;
+		} else {
+			driver->ops->release(data);
+			module_put(driver->ops->owner);
+		}
+
+		goto skip_drivers_unlock;
+	}
+
+	mutex_unlock(&vfio.iommu_drivers_lock);
+skip_drivers_unlock:
+	mutex_unlock(&container->group_lock);
+
+	return ret;
+}
+
+static long vfio_fops_unl_ioctl(struct file *filep,
+				unsigned int cmd, unsigned long arg)
+{
+	struct vfio_container *container = filep->private_data;
+	struct vfio_iommu_driver *driver;
+	void *data;
+	long ret = -EINVAL;
+
+	if (!container)
+		return ret;
+
+	driver = container->iommu_driver;
+	data = container->iommu_data;
+
+	switch (cmd) {
+	case VFIO_GET_API_VERSION:
+		ret = VFIO_API_VERSION;
+		break;
+	case VFIO_CHECK_EXTENSION:
+		ret = vfio_ioctl_check_extension(container, arg);
+		break;
+	case VFIO_SET_IOMMU:
+		ret = vfio_ioctl_set_iommu(container, arg);
+		break;
+	default:
+		if (driver) /* passthrough all unrecognized ioctls */
+			ret = driver->ops->ioctl(data, cmd, arg);
+	}
+
+	return ret;
+}
+
+#ifdef CONFIG_COMPAT
+static long vfio_fops_compat_ioctl(struct file *filep,
+				   unsigned int cmd, unsigned long arg)
+{
+	arg = (unsigned long)compat_ptr(arg);
+	return vfio_fops_unl_ioctl(filep, cmd, arg);
+}
+#endif	/* CONFIG_COMPAT */
+
+static int vfio_fops_open(struct inode *inode, struct file *filep)
+{
+	struct vfio_container *container;
+
+	container = kzalloc(sizeof(*container), GFP_KERNEL);
+	if (!container)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&container->group_list);
+	mutex_init(&container->group_lock);
+	kref_init(&container->kref);
+
+	filep->private_data = container;
+
+	return 0;
+}
+
+static int vfio_fops_release(struct inode *inode, struct file *filep)
+{
+	struct vfio_container *container = filep->private_data;
+
+	filep->private_data = NULL;
+
+	vfio_container_put(container);
+
+	return 0;
+}
+
+/*
+ * Once an iommu driver is set, we optionally pass read/write/mmap
+ * on to the driver, allowing management interfaces beyond ioctl.
+ */
+static ssize_t vfio_fops_read(struct file *filep, char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	struct vfio_container *container = filep->private_data;
+	struct vfio_iommu_driver *driver = container->iommu_driver;
+
+	if (unlikely(!driver || !driver->ops->read))
+		return -EINVAL;
+
+	return driver->ops->read(container->iommu_data, buf, count, ppos);
+}
+
+static ssize_t vfio_fops_write(struct file *filep, const char __user *buf,
+			       size_t count, loff_t *ppos)
+{
+	struct vfio_container *container = filep->private_data;
+	struct vfio_iommu_driver *driver = container->iommu_driver;
+
+	if (unlikely(!driver || !driver->ops->write))
+		return -EINVAL;
+
+	return driver->ops->write(container->iommu_data, buf, count, ppos);
+}
+
+static int vfio_fops_mmap(struct file *filep, struct vm_area_struct *vma)
+{
+	struct vfio_container *container = filep->private_data;
+	struct vfio_iommu_driver *driver = container->iommu_driver;
+
+	if (unlikely(!driver || !driver->ops->mmap))
+		return -EINVAL;
+
+	return driver->ops->mmap(container->iommu_data, vma);
+}
+
+static const struct file_operations vfio_fops = {
+	.owner		= THIS_MODULE,
+	.open		= vfio_fops_open,
+	.release	= vfio_fops_release,
+	.read		= vfio_fops_read,
+	.write		= vfio_fops_write,
+	.unlocked_ioctl	= vfio_fops_unl_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= vfio_fops_compat_ioctl,
+#endif
+	.mmap		= vfio_fops_mmap,
+};
+
+/**
+ * VFIO Group fd, /dev/vfio/$GROUP
+ */
+static void __vfio_group_unset_container(struct vfio_group *group)
+{
+	struct vfio_container *container = group->container;
+	struct vfio_iommu_driver *driver;
+
+	mutex_lock(&container->group_lock);
+
+	driver = container->iommu_driver;
+	if (driver)
+		driver->ops->detach_group(container->iommu_data,
+					  group->iommu_group);
+
+	group->container = NULL;
+	list_del(&group->container_next);
+
+	/* Detaching the last group deprivileges a container, remove iommu */
+	if (driver && list_empty(&container->group_list)) {
+		driver->ops->release(container->iommu_data);
+		module_put(driver->ops->owner);
+		container->iommu_driver = NULL;
+		container->iommu_data = NULL;
+	}
+
+	mutex_unlock(&container->group_lock);
+
+	vfio_container_put(container);
+}
+
+/*
+ * VFIO_GROUP_UNSET_CONTAINER should fail if there are other users or
+ * if there was no container to unset.  Since the ioctl is called on
+ * the group, we know that still exists, therefore the only valid
+ * transition here is 1->0.
+ */
+static int vfio_group_unset_container(struct vfio_group *group)
+{
+	int users = atomic_cmpxchg(&group->container_users, 1, 0);
+
+	if (!users)
+		return -EINVAL;
+	if (users != 1)
+		return -EBUSY;
+
+	__vfio_group_unset_container(group);
+
+	return 0;
+}
+
+/*
+ * When removing container users, anything that removes the last user
+ * implicitly removes the group from the container.  That is, if the
+ * group file descriptor is closed, as well as any device file descriptors,
+ * the group is free.
+ */
+static void vfio_group_try_dissolve_container(struct vfio_group *group)
+{
+	if (0 == atomic_dec_if_positive(&group->container_users))
+		__vfio_group_unset_container(group);
+}
+
+static int vfio_group_set_container(struct vfio_group *group, int container_fd)
+{
+	struct file *filep;
+	struct vfio_container *container;
+	struct vfio_iommu_driver *driver;
+	int ret = 0;
+
+	if (atomic_read(&group->container_users))
+		return -EINVAL;
+
+	filep = fget(container_fd);
+	if (!filep)
+		return -EBADF;
+
+	/* Sanity check, is this really our fd? */
+	if (filep->f_op != &vfio_fops) {
+		fput(filep);
+		return -EINVAL;
+	}
+
+	container = filep->private_data;
+	WARN_ON(!container); /* fget ensures we don't race vfio_release */
+
+	mutex_lock(&container->group_lock);
+
+	driver = container->iommu_driver;
+	if (driver) {
+		ret = driver->ops->attach_group(container->iommu_data,
+						group->iommu_group);
+		if (ret)
+			goto unlock_out;
+	}
+
+	group->container = container;
+	list_add(&group->container_next, &container->group_list);
+
+	/* Get a reference on the container and mark a user within the group */
+	vfio_container_get(container);
+	atomic_inc(&group->container_users);
+
+unlock_out:
+	mutex_unlock(&container->group_lock);
+	fput(filep);
+
+	return ret;
+}
+
+static bool vfio_group_viable(struct vfio_group *group)
+{
+	return (iommu_group_for_each_dev(group->iommu_group,
+					 group, vfio_dev_viable) == 0);
+}
+
+static const struct file_operations vfio_device_fops;
+
+static int vfio_group_get_device_fd(struct vfio_group *group, char *buf)
+{
+	struct vfio_device *device;
+	struct file *filep;
+	int ret = -ENODEV;
+
+	if (0 == atomic_read(&group->container_users) ||
+	    !group->container->iommu_driver || !vfio_group_viable(group))
+		return -EINVAL;
+
+	mutex_lock(&group->device_lock);
+	list_for_each_entry(device, &group->device_list, group_next) {
+		if (strcmp(dev_name(device->dev), buf))
+			continue;
+
+		ret = device->ops->open(device->device_data);
+		if (ret)
+			break;
+		/*
+		 * We can't use anon_inode_getfd() because we need to modify
+		 * the f_mode flags directly to allow more than just ioctls
+		 */
+		ret = get_unused_fd();
+		if (ret < 0) {
+			device->ops->release(device->device_data);
+			break;
+		}
+
+		filep = anon_inode_getfile("[vfio-device]", &vfio_device_fops,
+					   device, O_RDWR);
+		if (IS_ERR(filep)) {
+			put_unused_fd(ret);
+			ret = PTR_ERR(filep);
+			device->ops->release(device->device_data);
+			break;
+		}
+
+		/*
+		 * TODO: add an anon_inode interface to do this.
+		 * Appears to be missing by lack of need rather than
+		 * explicitly prevented.  Now there's need.
+		 */
+		filep->f_mode |= (FMODE_LSEEK | FMODE_PREAD | FMODE_PWRITE);
+
+		fd_install(ret, filep);
+
+		vfio_device_get(device);
+		atomic_inc(&group->container_users);
+		break;
+	}
+	mutex_unlock(&group->device_lock);
+
+	return ret;
+}
+
+static long vfio_group_fops_unl_ioctl(struct file *filep,
+				      unsigned int cmd, unsigned long arg)
+{
+	struct vfio_group *group = filep->private_data;
+	long ret = -ENOTTY;
+
+	switch (cmd) {
+	case VFIO_GROUP_GET_STATUS:
+	{
+		struct vfio_group_status status;
+		unsigned long minsz;
+
+		minsz = offsetofend(struct vfio_group_status, flags);
+
+		if (copy_from_user(&status, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (status.argsz < minsz)
+			return -EINVAL;
+
+		status.flags = 0;
+
+		if (vfio_group_viable(group))
+			status.flags |= VFIO_GROUP_FLAGS_VIABLE;
+
+		if (group->container)
+			status.flags |= VFIO_GROUP_FLAGS_CONTAINER_SET;
+
+		if (copy_to_user((void __user *)arg, &status, minsz))
+			return -EFAULT;
+
+		ret = 0;
+		break;
+	}
+	case VFIO_GROUP_SET_CONTAINER:
+	{
+		int fd;
+
+		if (get_user(fd, (int __user *)arg))
+			return -EFAULT;
+
+		if (fd < 0)
+			return -EINVAL;
+
+		ret = vfio_group_set_container(group, fd);
+		break;
+	}
+	case VFIO_GROUP_UNSET_CONTAINER:
+		ret = vfio_group_unset_container(group);
+		break;
+	case VFIO_GROUP_GET_DEVICE_FD:
+	{
+		char *buf;
+
+		buf = strndup_user((const char __user *)arg, PAGE_SIZE);
+		if (IS_ERR(buf))
+			return PTR_ERR(buf);
+
+		ret = vfio_group_get_device_fd(group, buf);
+		kfree(buf);
+		break;
+	}
+	}
+
+	return ret;
+}
+
+#ifdef CONFIG_COMPAT
+static long vfio_group_fops_compat_ioctl(struct file *filep,
+					 unsigned int cmd, unsigned long arg)
+{
+	arg = (unsigned long)compat_ptr(arg);
+	return vfio_group_fops_unl_ioctl(filep, cmd, arg);
+}
+#endif	/* CONFIG_COMPAT */
+
+static int vfio_group_fops_open(struct inode *inode, struct file *filep)
+{
+	struct vfio_group *group;
+
+	group = vfio_group_get_from_minor(iminor(inode));
+	if (!group)
+		return -ENODEV;
+
+	if (group->container) {
+		vfio_group_put(group);
+		return -EBUSY;
+	}
+
+	filep->private_data = group;
+
+	return 0;
+}
+
+static int vfio_group_fops_release(struct inode *inode, struct file *filep)
+{
+	struct vfio_group *group = filep->private_data;
+
+	filep->private_data = NULL;
+
+	vfio_group_try_dissolve_container(group);
+
+	vfio_group_put(group);
+
+	return 0;
+}
+
+static const struct file_operations vfio_group_fops = {
+	.owner		= THIS_MODULE,
+	.unlocked_ioctl	= vfio_group_fops_unl_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= vfio_group_fops_compat_ioctl,
+#endif
+	.open		= vfio_group_fops_open,
+	.release	= vfio_group_fops_release,
+};
+
+/**
+ * VFIO Device fd
+ */
+static int vfio_device_fops_release(struct inode *inode, struct file *filep)
+{
+	struct vfio_device *device = filep->private_data;
+
+	device->ops->release(device->device_data);
+
+	vfio_group_try_dissolve_container(device->group);
+
+	vfio_device_put(device);
+
+	return 0;
+}
+
+static long vfio_device_fops_unl_ioctl(struct file *filep,
+				       unsigned int cmd, unsigned long arg)
+{
+	struct vfio_device *device = filep->private_data;
+
+	if (unlikely(!device->ops->ioctl))
+		return -EINVAL;
+
+	return device->ops->ioctl(device->device_data, cmd, arg);
+}
+
+static ssize_t vfio_device_fops_read(struct file *filep, char __user *buf,
+				     size_t count, loff_t *ppos)
+{
+	struct vfio_device *device = filep->private_data;
+
+	if (unlikely(!device->ops->read))
+		return -EINVAL;
+
+	return device->ops->read(device->device_data, buf, count, ppos);
+}
+
+static ssize_t vfio_device_fops_write(struct file *filep,
+				      const char __user *buf,
+				      size_t count, loff_t *ppos)
+{
+	struct vfio_device *device = filep->private_data;
+
+	if (unlikely(!device->ops->write))
+		return -EINVAL;
+
+	return device->ops->write(device->device_data, buf, count, ppos);
+}
+
+static int vfio_device_fops_mmap(struct file *filep, struct vm_area_struct *vma)
+{
+	struct vfio_device *device = filep->private_data;
+
+	if (unlikely(!device->ops->mmap))
+		return -EINVAL;
+
+	return device->ops->mmap(device->device_data, vma);
+}
+
+#ifdef CONFIG_COMPAT
+static long vfio_device_fops_compat_ioctl(struct file *filep,
+					  unsigned int cmd, unsigned long arg)
+{
+	arg = (unsigned long)compat_ptr(arg);
+	return vfio_device_fops_unl_ioctl(filep, cmd, arg);
+}
+#endif	/* CONFIG_COMPAT */
+
+static const struct file_operations vfio_device_fops = {
+	.owner		= THIS_MODULE,
+	.release	= vfio_device_fops_release,
+	.read		= vfio_device_fops_read,
+	.write		= vfio_device_fops_write,
+	.unlocked_ioctl	= vfio_device_fops_unl_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= vfio_device_fops_compat_ioctl,
+#endif
+	.mmap		= vfio_device_fops_mmap,
+};
+
+/**
+ * Module/class support
+ */
+static char *vfio_devnode(struct device *dev, umode_t *mode)
+{
+	return kasprintf(GFP_KERNEL, "vfio/%s", dev_name(dev));
+}
+
+static int __init vfio_init(void)
+{
+	int ret;
+
+	idr_init(&vfio.group_idr);
+	mutex_init(&vfio.group_lock);
+	mutex_init(&vfio.iommu_drivers_lock);
+	INIT_LIST_HEAD(&vfio.group_list);
+	INIT_LIST_HEAD(&vfio.iommu_drivers_list);
+	init_waitqueue_head(&vfio.release_q);
+
+	vfio.class = class_create(THIS_MODULE, "vfio");
+	if (IS_ERR(vfio.class)) {
+		ret = PTR_ERR(vfio.class);
+		goto err_class;
+	}
+
+	vfio.class->devnode = vfio_devnode;
+
+	ret = alloc_chrdev_region(&vfio.devt, 0, MINORMASK, "vfio");
+	if (ret)
+		goto err_base_chrdev;
+
+	cdev_init(&vfio.cdev, &vfio_fops);
+	ret = cdev_add(&vfio.cdev, vfio.devt, 1);
+	if (ret)
+		goto err_base_cdev;
+
+	vfio.dev = device_create(vfio.class, NULL, vfio.devt, NULL, "vfio");
+	if (IS_ERR(vfio.dev)) {
+		ret = PTR_ERR(vfio.dev);
+		goto err_base_dev;
+	}
+
+	/* /dev/vfio/$GROUP */
+	cdev_init(&vfio.group_cdev, &vfio_group_fops);
+	ret = cdev_add(&vfio.group_cdev,
+		       MKDEV(MAJOR(vfio.devt), 1), MINORMASK - 1);
+	if (ret)
+		goto err_groups_cdev;
+
+	pr_info(DRIVER_DESC " version: " DRIVER_VERSION "\n");
+
+	/*
+	 * Attempt to load known iommu-drivers.  This gives us a working
+	 * environment without the user needing to explicitly load iommu
+	 * drivers.
+	 */
+	request_module_nowait("vfio_iommu_type1");
+
+	return 0;
+
+err_groups_cdev:
+	device_destroy(vfio.class, vfio.devt);
+err_base_dev:
+	cdev_del(&vfio.cdev);
+err_base_cdev:
+	unregister_chrdev_region(vfio.devt, MINORMASK);
+err_base_chrdev:
+	class_destroy(vfio.class);
+	vfio.class = NULL;
+err_class:
+	return ret;
+}
+
+static void __exit vfio_cleanup(void)
+{
+	WARN_ON(!list_empty(&vfio.group_list));
+
+	idr_destroy(&vfio.group_idr);
+	cdev_del(&vfio.group_cdev);
+	device_destroy(vfio.class, vfio.devt);
+	cdev_del(&vfio.cdev);
+	unregister_chrdev_region(vfio.devt, MINORMASK);
+	class_destroy(vfio.class);
+	vfio.class = NULL;
+}
+
+module_init(vfio_init);
+module_exit(vfio_cleanup);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vfio/vfio_iommu_type1.c b/drivers/vfio/vfio_iommu_type1.c
new file mode 100644
index 00000000000..6f3fbc48a6c
--- /dev/null
+++ b/drivers/vfio/vfio_iommu_type1.c
@@ -0,0 +1,753 @@
+/*
+ * VFIO: IOMMU DMA mapping support for Type1 IOMMU
+ *
+ * Copyright (C) 2012 Red Hat, Inc.  All rights reserved.
+ *     Author: Alex Williamson <alex.williamson@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Derived from original vfio:
+ * Copyright 2010 Cisco Systems, Inc.  All rights reserved.
+ * Author: Tom Lyon, pugs@cisco.com
+ *
+ * We arbitrarily define a Type1 IOMMU as one matching the below code.
+ * It could be called the x86 IOMMU as it's designed for AMD-Vi & Intel
+ * VT-d, but that makes it harder to re-use as theoretically anyone
+ * implementing a similar IOMMU could make use of this.  We expect the
+ * IOMMU to support the IOMMU API and have few to no restrictions around
+ * the IOVA range that can be mapped.  The Type1 IOMMU is currently
+ * optimized for relatively static mappings of a userspace process with
+ * userpsace pages pinned into memory.  We also assume devices and IOMMU
+ * domains are PCI based as the IOMMU API is still centered around a
+ * device/bus interface rather than a group interface.
+ */
+
+#include <linux/compat.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/iommu.h>
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/pci.h>		/* pci_bus_type */
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/vfio.h>
+#include <linux/workqueue.h>
+
+#define DRIVER_VERSION  "0.2"
+#define DRIVER_AUTHOR   "Alex Williamson <alex.williamson@redhat.com>"
+#define DRIVER_DESC     "Type1 IOMMU driver for VFIO"
+
+static bool allow_unsafe_interrupts;
+module_param_named(allow_unsafe_interrupts,
+		   allow_unsafe_interrupts, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(allow_unsafe_interrupts,
+		 "Enable VFIO IOMMU support for on platforms without interrupt remapping support.");
+
+struct vfio_iommu {
+	struct iommu_domain	*domain;
+	struct mutex		lock;
+	struct list_head	dma_list;
+	struct list_head	group_list;
+	bool			cache;
+};
+
+struct vfio_dma {
+	struct list_head	next;
+	dma_addr_t		iova;		/* Device address */
+	unsigned long		vaddr;		/* Process virtual addr */
+	long			npage;		/* Number of pages */
+	int			prot;		/* IOMMU_READ/WRITE */
+};
+
+struct vfio_group {
+	struct iommu_group	*iommu_group;
+	struct list_head	next;
+};
+
+/*
+ * This code handles mapping and unmapping of user data buffers
+ * into DMA'ble space using the IOMMU
+ */
+
+#define NPAGE_TO_SIZE(npage)	((size_t)(npage) << PAGE_SHIFT)
+
+struct vwork {
+	struct mm_struct	*mm;
+	long			npage;
+	struct work_struct	work;
+};
+
+/* delayed decrement/increment for locked_vm */
+static void vfio_lock_acct_bg(struct work_struct *work)
+{
+	struct vwork *vwork = container_of(work, struct vwork, work);
+	struct mm_struct *mm;
+
+	mm = vwork->mm;
+	down_write(&mm->mmap_sem);
+	mm->locked_vm += vwork->npage;
+	up_write(&mm->mmap_sem);
+	mmput(mm);
+	kfree(vwork);
+}
+
+static void vfio_lock_acct(long npage)
+{
+	struct vwork *vwork;
+	struct mm_struct *mm;
+
+	if (!current->mm)
+		return; /* process exited */
+
+	if (down_write_trylock(&current->mm->mmap_sem)) {
+		current->mm->locked_vm += npage;
+		up_write(&current->mm->mmap_sem);
+		return;
+	}
+
+	/*
+	 * Couldn't get mmap_sem lock, so must setup to update
+	 * mm->locked_vm later. If locked_vm were atomic, we
+	 * wouldn't need this silliness
+	 */
+	vwork = kmalloc(sizeof(struct vwork), GFP_KERNEL);
+	if (!vwork)
+		return;
+	mm = get_task_mm(current);
+	if (!mm) {
+		kfree(vwork);
+		return;
+	}
+	INIT_WORK(&vwork->work, vfio_lock_acct_bg);
+	vwork->mm = mm;
+	vwork->npage = npage;
+	schedule_work(&vwork->work);
+}
+
+/*
+ * Some mappings aren't backed by a struct page, for example an mmap'd
+ * MMIO range for our own or another device.  These use a different
+ * pfn conversion and shouldn't be tracked as locked pages.
+ */
+static bool is_invalid_reserved_pfn(unsigned long pfn)
+{
+	if (pfn_valid(pfn)) {
+		bool reserved;
+		struct page *tail = pfn_to_page(pfn);
+		struct page *head = compound_trans_head(tail);
+		reserved = !!(PageReserved(head));
+		if (head != tail) {
+			/*
+			 * "head" is not a dangling pointer
+			 * (compound_trans_head takes care of that)
+			 * but the hugepage may have been split
+			 * from under us (and we may not hold a
+			 * reference count on the head page so it can
+			 * be reused before we run PageReferenced), so
+			 * we've to check PageTail before returning
+			 * what we just read.
+			 */
+			smp_rmb();
+			if (PageTail(tail))
+				return reserved;
+		}
+		return PageReserved(tail);
+	}
+
+	return true;
+}
+
+static int put_pfn(unsigned long pfn, int prot)
+{
+	if (!is_invalid_reserved_pfn(pfn)) {
+		struct page *page = pfn_to_page(pfn);
+		if (prot & IOMMU_WRITE)
+			SetPageDirty(page);
+		put_page(page);
+		return 1;
+	}
+	return 0;
+}
+
+/* Unmap DMA region */
+static long __vfio_dma_do_unmap(struct vfio_iommu *iommu, dma_addr_t iova,
+			     long npage, int prot)
+{
+	long i, unlocked = 0;
+
+	for (i = 0; i < npage; i++, iova += PAGE_SIZE) {
+		unsigned long pfn;
+
+		pfn = iommu_iova_to_phys(iommu->domain, iova) >> PAGE_SHIFT;
+		if (pfn) {
+			iommu_unmap(iommu->domain, iova, PAGE_SIZE);
+			unlocked += put_pfn(pfn, prot);
+		}
+	}
+	return unlocked;
+}
+
+static void vfio_dma_unmap(struct vfio_iommu *iommu, dma_addr_t iova,
+			   long npage, int prot)
+{
+	long unlocked;
+
+	unlocked = __vfio_dma_do_unmap(iommu, iova, npage, prot);
+	vfio_lock_acct(-unlocked);
+}
+
+static int vaddr_get_pfn(unsigned long vaddr, int prot, unsigned long *pfn)
+{
+	struct page *page[1];
+	struct vm_area_struct *vma;
+	int ret = -EFAULT;
+
+	if (get_user_pages_fast(vaddr, 1, !!(prot & IOMMU_WRITE), page) == 1) {
+		*pfn = page_to_pfn(page[0]);
+		return 0;
+	}
+
+	down_read(&current->mm->mmap_sem);
+
+	vma = find_vma_intersection(current->mm, vaddr, vaddr + 1);
+
+	if (vma && vma->vm_flags & VM_PFNMAP) {
+		*pfn = ((vaddr - vma->vm_start) >> PAGE_SHIFT) + vma->vm_pgoff;
+		if (is_invalid_reserved_pfn(*pfn))
+			ret = 0;
+	}
+
+	up_read(&current->mm->mmap_sem);
+
+	return ret;
+}
+
+/* Map DMA region */
+static int __vfio_dma_map(struct vfio_iommu *iommu, dma_addr_t iova,
+			  unsigned long vaddr, long npage, int prot)
+{
+	dma_addr_t start = iova;
+	long i, locked = 0;
+	int ret;
+
+	/* Verify that pages are not already mapped */
+	for (i = 0; i < npage; i++, iova += PAGE_SIZE)
+		if (iommu_iova_to_phys(iommu->domain, iova))
+			return -EBUSY;
+
+	iova = start;
+
+	if (iommu->cache)
+		prot |= IOMMU_CACHE;
+
+	/*
+	 * XXX We break mappings into pages and use get_user_pages_fast to
+	 * pin the pages in memory.  It's been suggested that mlock might
+	 * provide a more efficient mechanism, but nothing prevents the
+	 * user from munlocking the pages, which could then allow the user
+	 * access to random host memory.  We also have no guarantee from the
+	 * IOMMU API that the iommu driver can unmap sub-pages of previous
+	 * mappings.  This means we might lose an entire range if a single
+	 * page within it is unmapped.  Single page mappings are inefficient,
+	 * but provide the most flexibility for now.
+	 */
+	for (i = 0; i < npage; i++, iova += PAGE_SIZE, vaddr += PAGE_SIZE) {
+		unsigned long pfn = 0;
+
+		ret = vaddr_get_pfn(vaddr, prot, &pfn);
+		if (ret) {
+			__vfio_dma_do_unmap(iommu, start, i, prot);
+			return ret;
+		}
+
+		/*
+		 * Only add actual locked pages to accounting
+		 * XXX We're effectively marking a page locked for every
+		 * IOVA page even though it's possible the user could be
+		 * backing multiple IOVAs with the same vaddr.  This over-
+		 * penalizes the user process, but we currently have no
+		 * easy way to do this properly.
+		 */
+		if (!is_invalid_reserved_pfn(pfn))
+			locked++;
+
+		ret = iommu_map(iommu->domain, iova,
+				(phys_addr_t)pfn << PAGE_SHIFT,
+				PAGE_SIZE, prot);
+		if (ret) {
+			/* Back out mappings on error */
+			put_pfn(pfn, prot);
+			__vfio_dma_do_unmap(iommu, start, i, prot);
+			return ret;
+		}
+	}
+	vfio_lock_acct(locked);
+	return 0;
+}
+
+static inline bool ranges_overlap(dma_addr_t start1, size_t size1,
+				  dma_addr_t start2, size_t size2)
+{
+	if (start1 < start2)
+		return (start2 - start1 < size1);
+	else if (start2 < start1)
+		return (start1 - start2 < size2);
+	return (size1 > 0 && size2 > 0);
+}
+
+static struct vfio_dma *vfio_find_dma(struct vfio_iommu *iommu,
+						dma_addr_t start, size_t size)
+{
+	struct vfio_dma *dma;
+
+	list_for_each_entry(dma, &iommu->dma_list, next) {
+		if (ranges_overlap(dma->iova, NPAGE_TO_SIZE(dma->npage),
+				   start, size))
+			return dma;
+	}
+	return NULL;
+}
+
+static long vfio_remove_dma_overlap(struct vfio_iommu *iommu, dma_addr_t start,
+				    size_t size, struct vfio_dma *dma)
+{
+	struct vfio_dma *split;
+	long npage_lo, npage_hi;
+
+	/* Existing dma region is completely covered, unmap all */
+	if (start <= dma->iova &&
+	    start + size >= dma->iova + NPAGE_TO_SIZE(dma->npage)) {
+		vfio_dma_unmap(iommu, dma->iova, dma->npage, dma->prot);
+		list_del(&dma->next);
+		npage_lo = dma->npage;
+		kfree(dma);
+		return npage_lo;
+	}
+
+	/* Overlap low address of existing range */
+	if (start <= dma->iova) {
+		size_t overlap;
+
+		overlap = start + size - dma->iova;
+		npage_lo = overlap >> PAGE_SHIFT;
+
+		vfio_dma_unmap(iommu, dma->iova, npage_lo, dma->prot);
+		dma->iova += overlap;
+		dma->vaddr += overlap;
+		dma->npage -= npage_lo;
+		return npage_lo;
+	}
+
+	/* Overlap high address of existing range */
+	if (start + size >= dma->iova + NPAGE_TO_SIZE(dma->npage)) {
+		size_t overlap;
+
+		overlap = dma->iova + NPAGE_TO_SIZE(dma->npage) - start;
+		npage_hi = overlap >> PAGE_SHIFT;
+
+		vfio_dma_unmap(iommu, start, npage_hi, dma->prot);
+		dma->npage -= npage_hi;
+		return npage_hi;
+	}
+
+	/* Split existing */
+	npage_lo = (start - dma->iova) >> PAGE_SHIFT;
+	npage_hi = dma->npage - (size >> PAGE_SHIFT) - npage_lo;
+
+	split = kzalloc(sizeof *split, GFP_KERNEL);
+	if (!split)
+		return -ENOMEM;
+
+	vfio_dma_unmap(iommu, start, size >> PAGE_SHIFT, dma->prot);
+
+	dma->npage = npage_lo;
+
+	split->npage = npage_hi;
+	split->iova = start + size;
+	split->vaddr = dma->vaddr + NPAGE_TO_SIZE(npage_lo) + size;
+	split->prot = dma->prot;
+	list_add(&split->next, &iommu->dma_list);
+	return size >> PAGE_SHIFT;
+}
+
+static int vfio_dma_do_unmap(struct vfio_iommu *iommu,
+			     struct vfio_iommu_type1_dma_unmap *unmap)
+{
+	long ret = 0, npage = unmap->size >> PAGE_SHIFT;
+	struct vfio_dma *dma, *tmp;
+	uint64_t mask;
+
+	mask = ((uint64_t)1 << __ffs(iommu->domain->ops->pgsize_bitmap)) - 1;
+
+	if (unmap->iova & mask)
+		return -EINVAL;
+	if (unmap->size & mask)
+		return -EINVAL;
+
+	/* XXX We still break these down into PAGE_SIZE */
+	WARN_ON(mask & PAGE_MASK);
+
+	mutex_lock(&iommu->lock);
+
+	list_for_each_entry_safe(dma, tmp, &iommu->dma_list, next) {
+		if (ranges_overlap(dma->iova, NPAGE_TO_SIZE(dma->npage),
+				   unmap->iova, unmap->size)) {
+			ret = vfio_remove_dma_overlap(iommu, unmap->iova,
+						      unmap->size, dma);
+			if (ret > 0)
+				npage -= ret;
+			if (ret < 0 || npage == 0)
+				break;
+		}
+	}
+	mutex_unlock(&iommu->lock);
+	return ret > 0 ? 0 : (int)ret;
+}
+
+static int vfio_dma_do_map(struct vfio_iommu *iommu,
+			   struct vfio_iommu_type1_dma_map *map)
+{
+	struct vfio_dma *dma, *pdma = NULL;
+	dma_addr_t iova = map->iova;
+	unsigned long locked, lock_limit, vaddr = map->vaddr;
+	size_t size = map->size;
+	int ret = 0, prot = 0;
+	uint64_t mask;
+	long npage;
+
+	mask = ((uint64_t)1 << __ffs(iommu->domain->ops->pgsize_bitmap)) - 1;
+
+	/* READ/WRITE from device perspective */
+	if (map->flags & VFIO_DMA_MAP_FLAG_WRITE)
+		prot |= IOMMU_WRITE;
+	if (map->flags & VFIO_DMA_MAP_FLAG_READ)
+		prot |= IOMMU_READ;
+
+	if (!prot)
+		return -EINVAL; /* No READ/WRITE? */
+
+	if (vaddr & mask)
+		return -EINVAL;
+	if (iova & mask)
+		return -EINVAL;
+	if (size & mask)
+		return -EINVAL;
+
+	/* XXX We still break these down into PAGE_SIZE */
+	WARN_ON(mask & PAGE_MASK);
+
+	/* Don't allow IOVA wrap */
+	if (iova + size && iova + size < iova)
+		return -EINVAL;
+
+	/* Don't allow virtual address wrap */
+	if (vaddr + size && vaddr + size < vaddr)
+		return -EINVAL;
+
+	npage = size >> PAGE_SHIFT;
+	if (!npage)
+		return -EINVAL;
+
+	mutex_lock(&iommu->lock);
+
+	if (vfio_find_dma(iommu, iova, size)) {
+		ret = -EBUSY;
+		goto out_lock;
+	}
+
+	/* account for locked pages */
+	locked = current->mm->locked_vm + npage;
+	lock_limit = rlimit(RLIMIT_MEMLOCK) >> PAGE_SHIFT;
+	if (locked > lock_limit && !capable(CAP_IPC_LOCK)) {
+		pr_warn("%s: RLIMIT_MEMLOCK (%ld) exceeded\n",
+			__func__, rlimit(RLIMIT_MEMLOCK));
+		ret = -ENOMEM;
+		goto out_lock;
+	}
+
+	ret = __vfio_dma_map(iommu, iova, vaddr, npage, prot);
+	if (ret)
+		goto out_lock;
+
+	/* Check if we abut a region below - nothing below 0 */
+	if (iova) {
+		dma = vfio_find_dma(iommu, iova - 1, 1);
+		if (dma && dma->prot == prot &&
+		    dma->vaddr + NPAGE_TO_SIZE(dma->npage) == vaddr) {
+
+			dma->npage += npage;
+			iova = dma->iova;
+			vaddr = dma->vaddr;
+			npage = dma->npage;
+			size = NPAGE_TO_SIZE(npage);
+
+			pdma = dma;
+		}
+	}
+
+	/* Check if we abut a region above - nothing above ~0 + 1 */
+	if (iova + size) {
+		dma = vfio_find_dma(iommu, iova + size, 1);
+		if (dma && dma->prot == prot &&
+		    dma->vaddr == vaddr + size) {
+
+			dma->npage += npage;
+			dma->iova = iova;
+			dma->vaddr = vaddr;
+
+			/*
+			 * If merged above and below, remove previously
+			 * merged entry.  New entry covers it.
+			 */
+			if (pdma) {
+				list_del(&pdma->next);
+				kfree(pdma);
+			}
+			pdma = dma;
+		}
+	}
+
+	/* Isolated, new region */
+	if (!pdma) {
+		dma = kzalloc(sizeof *dma, GFP_KERNEL);
+		if (!dma) {
+			ret = -ENOMEM;
+			vfio_dma_unmap(iommu, iova, npage, prot);
+			goto out_lock;
+		}
+
+		dma->npage = npage;
+		dma->iova = iova;
+		dma->vaddr = vaddr;
+		dma->prot = prot;
+		list_add(&dma->next, &iommu->dma_list);
+	}
+
+out_lock:
+	mutex_unlock(&iommu->lock);
+	return ret;
+}
+
+static int vfio_iommu_type1_attach_group(void *iommu_data,
+					 struct iommu_group *iommu_group)
+{
+	struct vfio_iommu *iommu = iommu_data;
+	struct vfio_group *group, *tmp;
+	int ret;
+
+	group = kzalloc(sizeof(*group), GFP_KERNEL);
+	if (!group)
+		return -ENOMEM;
+
+	mutex_lock(&iommu->lock);
+
+	list_for_each_entry(tmp, &iommu->group_list, next) {
+		if (tmp->iommu_group == iommu_group) {
+			mutex_unlock(&iommu->lock);
+			kfree(group);
+			return -EINVAL;
+		}
+	}
+
+	/*
+	 * TODO: Domain have capabilities that might change as we add
+	 * groups (see iommu->cache, currently never set).  Check for
+	 * them and potentially disallow groups to be attached when it
+	 * would change capabilities (ugh).
+	 */
+	ret = iommu_attach_group(iommu->domain, iommu_group);
+	if (ret) {
+		mutex_unlock(&iommu->lock);
+		kfree(group);
+		return ret;
+	}
+
+	group->iommu_group = iommu_group;
+	list_add(&group->next, &iommu->group_list);
+
+	mutex_unlock(&iommu->lock);
+
+	return 0;
+}
+
+static void vfio_iommu_type1_detach_group(void *iommu_data,
+					  struct iommu_group *iommu_group)
+{
+	struct vfio_iommu *iommu = iommu_data;
+	struct vfio_group *group;
+
+	mutex_lock(&iommu->lock);
+
+	list_for_each_entry(group, &iommu->group_list, next) {
+		if (group->iommu_group == iommu_group) {
+			iommu_detach_group(iommu->domain, iommu_group);
+			list_del(&group->next);
+			kfree(group);
+			break;
+		}
+	}
+
+	mutex_unlock(&iommu->lock);
+}
+
+static void *vfio_iommu_type1_open(unsigned long arg)
+{
+	struct vfio_iommu *iommu;
+
+	if (arg != VFIO_TYPE1_IOMMU)
+		return ERR_PTR(-EINVAL);
+
+	iommu = kzalloc(sizeof(*iommu), GFP_KERNEL);
+	if (!iommu)
+		return ERR_PTR(-ENOMEM);
+
+	INIT_LIST_HEAD(&iommu->group_list);
+	INIT_LIST_HEAD(&iommu->dma_list);
+	mutex_init(&iommu->lock);
+
+	/*
+	 * Wish we didn't have to know about bus_type here.
+	 */
+	iommu->domain = iommu_domain_alloc(&pci_bus_type);
+	if (!iommu->domain) {
+		kfree(iommu);
+		return ERR_PTR(-EIO);
+	}
+
+	/*
+	 * Wish we could specify required capabilities rather than create
+	 * a domain, see what comes out and hope it doesn't change along
+	 * the way.  Fortunately we know interrupt remapping is global for
+	 * our iommus.
+	 */
+	if (!allow_unsafe_interrupts &&
+	    !iommu_domain_has_cap(iommu->domain, IOMMU_CAP_INTR_REMAP)) {
+		pr_warn("%s: No interrupt remapping support.  Use the module param \"allow_unsafe_interrupts\" to enable VFIO IOMMU support on this platform\n",
+		       __func__);
+		iommu_domain_free(iommu->domain);
+		kfree(iommu);
+		return ERR_PTR(-EPERM);
+	}
+
+	return iommu;
+}
+
+static void vfio_iommu_type1_release(void *iommu_data)
+{
+	struct vfio_iommu *iommu = iommu_data;
+	struct vfio_group *group, *group_tmp;
+	struct vfio_dma *dma, *dma_tmp;
+
+	list_for_each_entry_safe(group, group_tmp, &iommu->group_list, next) {
+		iommu_detach_group(iommu->domain, group->iommu_group);
+		list_del(&group->next);
+		kfree(group);
+	}
+
+	list_for_each_entry_safe(dma, dma_tmp, &iommu->dma_list, next) {
+		vfio_dma_unmap(iommu, dma->iova, dma->npage, dma->prot);
+		list_del(&dma->next);
+		kfree(dma);
+	}
+
+	iommu_domain_free(iommu->domain);
+	iommu->domain = NULL;
+	kfree(iommu);
+}
+
+static long vfio_iommu_type1_ioctl(void *iommu_data,
+				   unsigned int cmd, unsigned long arg)
+{
+	struct vfio_iommu *iommu = iommu_data;
+	unsigned long minsz;
+
+	if (cmd == VFIO_CHECK_EXTENSION) {
+		switch (arg) {
+		case VFIO_TYPE1_IOMMU:
+			return 1;
+		default:
+			return 0;
+		}
+	} else if (cmd == VFIO_IOMMU_GET_INFO) {
+		struct vfio_iommu_type1_info info;
+
+		minsz = offsetofend(struct vfio_iommu_type1_info, iova_pgsizes);
+
+		if (copy_from_user(&info, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (info.argsz < minsz)
+			return -EINVAL;
+
+		info.flags = 0;
+
+		info.iova_pgsizes = iommu->domain->ops->pgsize_bitmap;
+
+		return copy_to_user((void __user *)arg, &info, minsz);
+
+	} else if (cmd == VFIO_IOMMU_MAP_DMA) {
+		struct vfio_iommu_type1_dma_map map;
+		uint32_t mask = VFIO_DMA_MAP_FLAG_READ |
+				VFIO_DMA_MAP_FLAG_WRITE;
+
+		minsz = offsetofend(struct vfio_iommu_type1_dma_map, size);
+
+		if (copy_from_user(&map, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (map.argsz < minsz || map.flags & ~mask)
+			return -EINVAL;
+
+		return vfio_dma_do_map(iommu, &map);
+
+	} else if (cmd == VFIO_IOMMU_UNMAP_DMA) {
+		struct vfio_iommu_type1_dma_unmap unmap;
+
+		minsz = offsetofend(struct vfio_iommu_type1_dma_unmap, size);
+
+		if (copy_from_user(&unmap, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (unmap.argsz < minsz || unmap.flags)
+			return -EINVAL;
+
+		return vfio_dma_do_unmap(iommu, &unmap);
+	}
+
+	return -ENOTTY;
+}
+
+static const struct vfio_iommu_driver_ops vfio_iommu_driver_ops_type1 = {
+	.name		= "vfio-iommu-type1",
+	.owner		= THIS_MODULE,
+	.open		= vfio_iommu_type1_open,
+	.release	= vfio_iommu_type1_release,
+	.ioctl		= vfio_iommu_type1_ioctl,
+	.attach_group	= vfio_iommu_type1_attach_group,
+	.detach_group	= vfio_iommu_type1_detach_group,
+};
+
+static int __init vfio_iommu_type1_init(void)
+{
+	if (!iommu_present(&pci_bus_type))
+		return -ENODEV;
+
+	return vfio_register_iommu_driver(&vfio_iommu_driver_ops_type1);
+}
+
+static void __exit vfio_iommu_type1_cleanup(void)
+{
+	vfio_unregister_iommu_driver(&vfio_iommu_driver_ops_type1);
+}
+
+module_init(vfio_iommu_type1_init);
+module_exit(vfio_iommu_type1_cleanup);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index f82a7394756..072cbbadbc3 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -823,14 +823,14 @@ static long vhost_net_ioctl(struct file *f, unsigned int ioctl,
 			return -EFAULT;
 		return vhost_net_set_backend(n, backend.index, backend.fd);
 	case VHOST_GET_FEATURES:
-		features = VHOST_FEATURES;
+		features = VHOST_NET_FEATURES;
 		if (copy_to_user(featurep, &features, sizeof features))
 			return -EFAULT;
 		return 0;
 	case VHOST_SET_FEATURES:
 		if (copy_from_user(&features, featurep, sizeof features))
 			return -EFAULT;
-		if (features & ~VHOST_FEATURES)
+		if (features & ~VHOST_NET_FEATURES)
 			return -EOPNOTSUPP;
 		return vhost_net_set_features(n, features);
 	case VHOST_RESET_OWNER:
diff --git a/drivers/vhost/test.c b/drivers/vhost/test.c
index 3de00d9fae2..91d6f060aad 100644
--- a/drivers/vhost/test.c
+++ b/drivers/vhost/test.c
@@ -261,14 +261,14 @@ static long vhost_test_ioctl(struct file *f, unsigned int ioctl,
 			return -EFAULT;
 		return vhost_test_run(n, test);
 	case VHOST_GET_FEATURES:
-		features = VHOST_FEATURES;
+		features = VHOST_NET_FEATURES;
 		if (copy_to_user(featurep, &features, sizeof features))
 			return -EFAULT;
 		return 0;
 	case VHOST_SET_FEATURES:
 		if (copy_from_user(&features, featurep, sizeof features))
 			return -EFAULT;
-		if (features & ~VHOST_FEATURES)
+		if (features & ~VHOST_NET_FEATURES)
 			return -EOPNOTSUPP;
 		return vhost_test_set_features(n, features);
 	case VHOST_RESET_OWNER:
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index 112156f68af..ef82a0d1848 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -64,7 +64,7 @@ static int vhost_poll_wakeup(wait_queue_t *wait, unsigned mode, int sync,
 	return 0;
 }
 
-static void vhost_work_init(struct vhost_work *work, vhost_work_fn_t fn)
+void vhost_work_init(struct vhost_work *work, vhost_work_fn_t fn)
 {
 	INIT_LIST_HEAD(&work->node);
 	work->fn = fn;
@@ -137,8 +137,7 @@ void vhost_poll_flush(struct vhost_poll *poll)
 	vhost_work_flush(poll->dev, &poll->work);
 }
 
-static inline void vhost_work_queue(struct vhost_dev *dev,
-				    struct vhost_work *work)
+void vhost_work_queue(struct vhost_dev *dev, struct vhost_work *work)
 {
 	unsigned long flags;
 
diff --git a/drivers/vhost/vhost.h b/drivers/vhost/vhost.h
index 8de1fd5b8ef..1125af3d27d 100644
--- a/drivers/vhost/vhost.h
+++ b/drivers/vhost/vhost.h
@@ -43,6 +43,9 @@ struct vhost_poll {
 	struct vhost_dev	 *dev;
 };
 
+void vhost_work_init(struct vhost_work *work, vhost_work_fn_t fn);
+void vhost_work_queue(struct vhost_dev *dev, struct vhost_work *work);
+
 void vhost_poll_init(struct vhost_poll *poll, vhost_work_fn_t fn,
 		     unsigned long mask, struct vhost_dev *dev);
 void vhost_poll_start(struct vhost_poll *poll, struct file *file);
@@ -201,7 +204,8 @@ enum {
 	VHOST_FEATURES = (1ULL << VIRTIO_F_NOTIFY_ON_EMPTY) |
 			 (1ULL << VIRTIO_RING_F_INDIRECT_DESC) |
 			 (1ULL << VIRTIO_RING_F_EVENT_IDX) |
-			 (1ULL << VHOST_F_LOG_ALL) |
+			 (1ULL << VHOST_F_LOG_ALL),
+	VHOST_NET_FEATURES = VHOST_FEATURES |
 			 (1ULL << VHOST_NET_F_VIRTIO_NET_HDR) |
 			 (1ULL << VIRTIO_NET_F_MRG_RXBUF),
 };
diff --git a/drivers/video/arcfb.c b/drivers/video/arcfb.c
index c22e8d39a2c..a1d58e9d307 100644
--- a/drivers/video/arcfb.c
+++ b/drivers/video/arcfb.c
@@ -336,8 +336,8 @@ static void arcfb_lcd_update_horiz(struct arcfb_par *par, unsigned int left,
 }
 
 /*
- * here we start the process of spliting out the fb update into
- * individual blocks of pixels. we end up spliting into 64x64 blocks
+ * here we start the process of splitting out the fb update into
+ * individual blocks of pixels. we end up splitting into 64x64 blocks
  * and finally down to 64x8 pages.
  */
 static void arcfb_lcd_update(struct arcfb_par *par, unsigned int dx,
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index d99505b1637..15055395cd9 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -939,7 +939,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 		 * up a splash image.
 		 */
 	} else {
-		/* alocate memory buffer */
+		/* allocate memory buffer */
 		ret = atmel_lcdfb_alloc_video_memory(sinfo);
 		if (ret < 0) {
 			dev_err(dev, "cannot allocate framebuffer: %d\n", ret);
diff --git a/drivers/video/aty/aty128fb.c b/drivers/video/aty/aty128fb.c
index b0b2ac33534..747442d2c0f 100644
--- a/drivers/video/aty/aty128fb.c
+++ b/drivers/video/aty/aty128fb.c
@@ -90,7 +90,8 @@
 #undef DEBUG
 
 #ifdef DEBUG
-#define DBG(fmt, args...)		printk(KERN_DEBUG "aty128fb: %s " fmt, __func__, ##args);
+#define DBG(fmt, args...) \
+	printk(KERN_DEBUG "aty128fb: %s " fmt, __func__, ##args);
 #else
 #define DBG(fmt, args...)
 #endif
@@ -449,8 +450,9 @@ static int aty128_decode_var(struct fb_var_screeninfo *var,
                              struct aty128fb_par *par);
 #if 0
 static void __devinit aty128_get_pllinfo(struct aty128fb_par *par,
-				      void __iomem *bios);
-static void __devinit __iomem *aty128_map_ROM(struct pci_dev *pdev, const struct aty128fb_par *par);
+					 void __iomem *bios);
+static void __devinit __iomem *aty128_map_ROM(struct pci_dev *pdev,
+					      const struct aty128fb_par *par);
 #endif
 static void aty128_timings(struct aty128fb_par *par);
 static void aty128_init_engine(struct aty128fb_par *par);
@@ -779,7 +781,8 @@ static u32 depth_to_dst(u32 depth)
 
 
 #ifndef __sparc__
-static void __iomem * __devinit aty128_map_ROM(const struct aty128fb_par *par, struct pci_dev *dev)
+static void __iomem * __devinit aty128_map_ROM(const struct aty128fb_par *par,
+					       struct pci_dev *dev)
 {
 	u16 dptr;
 	u8 rom_type;
@@ -811,13 +814,14 @@ static void __iomem * __devinit aty128_map_ROM(const struct aty128fb_par *par, s
 	/* Look for the PCI data to check the ROM type */
 	dptr = BIOS_IN16(0x18);
 
-	/* Check the PCI data signature. If it's wrong, we still assume a normal x86 ROM
-	 * for now, until I've verified this works everywhere. The goal here is more
-	 * to phase out Open Firmware images.
+	/* Check the PCI data signature. If it's wrong, we still assume a normal
+	 * x86 ROM for now, until I've verified this works everywhere.
+	 * The goal here is more to phase out Open Firmware images.
 	 *
-	 * Currently, we only look at the first PCI data, we could iteratre and deal with
-	 * them all, and we should use fb_bios_start relative to start of image and not
-	 * relative start of ROM, but so far, I never found a dual-image ATI card
+	 * Currently, we only look at the first PCI data, we could iteratre and
+	 * deal with them all, and we should use fb_bios_start relative to start
+	 * of image and not relative start of ROM, but so far, I never found a
+	 * dual-image ATI card.
 	 *
 	 * typedef struct {
 	 * 	u32	signature;	+ 0x00
@@ -852,7 +856,8 @@ static void __iomem * __devinit aty128_map_ROM(const struct aty128fb_par *par, s
 		printk(KERN_INFO "aty128fb: Found HP PA-RISC ROM Image\n");
 		goto failed;
 	default:
-		printk(KERN_INFO "aty128fb: Found unknown type %d ROM Image\n", rom_type);
+		printk(KERN_INFO "aty128fb: Found unknown type %d ROM Image\n",
+		       rom_type);
 		goto failed;
 	}
  anyway:
@@ -863,7 +868,8 @@ static void __iomem * __devinit aty128_map_ROM(const struct aty128fb_par *par, s
 	return NULL;
 }
 
-static void __devinit aty128_get_pllinfo(struct aty128fb_par *par, unsigned char __iomem *bios)
+static void __devinit aty128_get_pllinfo(struct aty128fb_par *par,
+					 unsigned char __iomem *bios)
 {
 	unsigned int bios_hdr;
 	unsigned int bios_pll;
@@ -1247,10 +1253,13 @@ static int aty128_crtc_to_var(const struct aty128_crtc *crtc,
 static void aty128_set_crt_enable(struct aty128fb_par *par, int on)
 {
 	if (on) {
-		aty_st_le32(CRTC_EXT_CNTL, aty_ld_le32(CRTC_EXT_CNTL) | CRT_CRTC_ON);
-		aty_st_le32(DAC_CNTL, (aty_ld_le32(DAC_CNTL) | DAC_PALETTE2_SNOOP_EN));
+		aty_st_le32(CRTC_EXT_CNTL, aty_ld_le32(CRTC_EXT_CNTL) |
+			    CRT_CRTC_ON);
+		aty_st_le32(DAC_CNTL, (aty_ld_le32(DAC_CNTL) |
+			    DAC_PALETTE2_SNOOP_EN));
 	} else
-		aty_st_le32(CRTC_EXT_CNTL, aty_ld_le32(CRTC_EXT_CNTL) & ~CRT_CRTC_ON);
+		aty_st_le32(CRTC_EXT_CNTL, aty_ld_le32(CRTC_EXT_CNTL) &
+			    ~CRT_CRTC_ON);
 }
 
 static void aty128_set_lcd_enable(struct aty128fb_par *par, int on)
@@ -1281,7 +1290,8 @@ static void aty128_set_lcd_enable(struct aty128fb_par *par, int on)
 	}
 }
 
-static void aty128_set_pll(struct aty128_pll *pll, const struct aty128fb_par *par)
+static void aty128_set_pll(struct aty128_pll *pll,
+			   const struct aty128fb_par *par)
 {
 	u32 div3;
 
@@ -1366,7 +1376,8 @@ static int aty128_var_to_pll(u32 period_in_ps, struct aty128_pll *pll,
 }
 
 
-static int aty128_pll_to_var(const struct aty128_pll *pll, struct fb_var_screeninfo *var)
+static int aty128_pll_to_var(const struct aty128_pll *pll,
+			     struct fb_var_screeninfo *var)
 {
 	var->pixclock = 100000000 / pll->vclk;
 
@@ -1512,7 +1523,8 @@ static int aty128fb_set_par(struct fb_info *info)
  *  encode/decode the User Defined Part of the Display
  */
 
-static int aty128_decode_var(struct fb_var_screeninfo *var, struct aty128fb_par *par)
+static int aty128_decode_var(struct fb_var_screeninfo *var,
+			     struct aty128fb_par *par)
 {
 	int err;
 	struct aty128_crtc crtc;
@@ -1559,7 +1571,8 @@ static int aty128_encode_var(struct fb_var_screeninfo *var,
 }           
 
 
-static int aty128fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+static int aty128fb_check_var(struct fb_var_screeninfo *var,
+			      struct fb_info *info)
 {
 	struct aty128fb_par par;
 	int err;
@@ -1575,7 +1588,8 @@ static int aty128fb_check_var(struct fb_var_screeninfo *var, struct fb_info *inf
 /*
  *  Pan or Wrap the Display
  */
-static int aty128fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *fb) 
+static int aty128fb_pan_display(struct fb_var_screeninfo *var,
+				struct fb_info *fb)
 {
 	struct aty128fb_par *par = fb->par;
 	u32 xoffset, yoffset;
@@ -1594,7 +1608,8 @@ static int aty128fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *f
 	par->crtc.xoffset = xoffset;
 	par->crtc.yoffset = yoffset;
 
-	offset = ((yoffset * par->crtc.vxres + xoffset)*(par->crtc.bpp >> 3)) & ~7;
+	offset = ((yoffset * par->crtc.vxres + xoffset) * (par->crtc.bpp >> 3))
+									  & ~7;
 
 	if (par->crtc.bpp == 24)
 		offset += 8 * (offset % 3); /* Must be multiple of 8 and 3 */
@@ -1620,11 +1635,13 @@ static void aty128_st_pal(u_int regno, u_int red, u_int green, u_int blue,
 		 * do mirroring
 		 */
 
-		aty_st_le32(DAC_CNTL, aty_ld_le32(DAC_CNTL) | DAC_PALETTE_ACCESS_CNTL);
+		aty_st_le32(DAC_CNTL, aty_ld_le32(DAC_CNTL) |
+			    DAC_PALETTE_ACCESS_CNTL);
 		aty_st_8(PALETTE_INDEX, regno);
 		aty_st_le32(PALETTE_DATA, (red<<16)|(green<<8)|blue);
 #endif
-		aty_st_le32(DAC_CNTL, aty_ld_le32(DAC_CNTL) & ~DAC_PALETTE_ACCESS_CNTL);
+		aty_st_le32(DAC_CNTL, aty_ld_le32(DAC_CNTL) &
+			    ~DAC_PALETTE_ACCESS_CNTL);
 	}
 
 	aty_st_8(PALETTE_INDEX, regno);
@@ -1753,7 +1770,8 @@ static int aty128_bl_update_status(struct backlight_device *bd)
 			aty_st_le32(LVDS_GEN_CNTL, reg);
 		}
 		reg &= ~LVDS_BL_MOD_LEVEL_MASK;
-		reg |= (aty128_bl_get_level_brightness(par, level) << LVDS_BL_MOD_LEVEL_SHIFT);
+		reg |= (aty128_bl_get_level_brightness(par, level) <<
+			LVDS_BL_MOD_LEVEL_SHIFT);
 #ifdef BACKLIGHT_LVDS_OFF
 		reg |= LVDS_ON | LVDS_EN;
 		reg &= ~LVDS_DISPLAY_DIS;
@@ -1764,7 +1782,8 @@ static int aty128_bl_update_status(struct backlight_device *bd)
 #endif
 	} else {
 		reg &= ~LVDS_BL_MOD_LEVEL_MASK;
-		reg |= (aty128_bl_get_level_brightness(par, 0) << LVDS_BL_MOD_LEVEL_SHIFT);
+		reg |= (aty128_bl_get_level_brightness(par, 0) <<
+			LVDS_BL_MOD_LEVEL_SHIFT);
 #ifdef BACKLIGHT_LVDS_OFF
 		reg |= LVDS_DISPLAY_DIS;
 		aty_st_le32(LVDS_GEN_CNTL, reg);
@@ -1869,7 +1888,8 @@ static void aty128_early_resume(void *data)
 }
 #endif /* CONFIG_PPC_PMAC */
 
-static int __devinit aty128_init(struct pci_dev *pdev, const struct pci_device_id *ent)
+static int __devinit aty128_init(struct pci_dev *pdev,
+				 const struct pci_device_id *ent)
 {
 	struct fb_info *info = pci_get_drvdata(pdev);
 	struct aty128fb_par *par = info->par;
@@ -1887,7 +1907,8 @@ static int __devinit aty128_init(struct pci_dev *pdev, const struct pci_device_i
 
 	/* range check to make sure */
 	if (ent->driver_data < ARRAY_SIZE(r128_family))
-	    strlcat(video_card, r128_family[ent->driver_data], sizeof(video_card));
+		strlcat(video_card, r128_family[ent->driver_data],
+			sizeof(video_card));
 
 	printk(KERN_INFO "aty128fb: %s [chip rev 0x%x] ", video_card, chip_rev);
 
@@ -1911,11 +1932,11 @@ static int __devinit aty128_init(struct pci_dev *pdev, const struct pci_device_i
 		/* Indicate sleep capability */
 		if (par->chip_gen == rage_M3) {
 			pmac_call_feature(PMAC_FTR_DEVICE_CAN_WAKE, NULL, 0, 1);
-#if 0 /* Disable the early video resume hack for now as it's causing problems, among
-       * others we now rely on the PCI core restoring the config space for us, which
-       * isn't the case with that hack, and that code path causes various things to
-       * be called with interrupts off while they shouldn't. I'm leaving the code in
-       * as it can be useful for debugging purposes
+#if 0 /* Disable the early video resume hack for now as it's causing problems,
+       * among others we now rely on the PCI core restoring the config space
+       * for us, which isn't the case with that hack, and that code path causes
+       * various things to be called with interrupts off while they shouldn't.
+       * I'm leaving the code in as it can be useful for debugging purposes
        */
 			pmac_set_early_video_resume(aty128_early_resume, par);
 #endif
@@ -1953,11 +1974,11 @@ static int __devinit aty128_init(struct pci_dev *pdev, const struct pci_device_i
 				default_vmode = VMODE_1152_768_60;
 	
 			if (default_cmode > 16) 
-			    default_cmode = CMODE_32;
+				default_cmode = CMODE_32;
 			else if (default_cmode > 8) 
-			    default_cmode = CMODE_16;
+				default_cmode = CMODE_16;
 			else 
-			    default_cmode = CMODE_8;
+				default_cmode = CMODE_8;
 
 			if (mac_vmode_to_var(default_vmode, default_cmode, &var))
 				var = default_var;
@@ -2018,7 +2039,8 @@ static int __devinit aty128_init(struct pci_dev *pdev, const struct pci_device_i
 
 #ifdef CONFIG_PCI
 /* register a card    ++ajoshi */
-static int __devinit aty128_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+static int __devinit aty128_probe(struct pci_dev *pdev,
+				  const struct pci_device_id *ent)
 {
 	unsigned long fb_addr, reg_addr;
 	struct aty128fb_par *par;
@@ -2318,39 +2340,39 @@ static inline void aty128_rectcopy(int srcx, int srcy, int dstx, int dsty,
 				   u_int width, u_int height,
 				   struct fb_info_aty128 *par)
 {
-    u32 save_dp_datatype, save_dp_cntl, dstval;
-
-    if (!width || !height)
-        return;
-
-    dstval = depth_to_dst(par->current_par.crtc.depth);
-    if (dstval == DST_24BPP) {
-        srcx *= 3;
-        dstx *= 3;
-        width *= 3;
-    } else if (dstval == -EINVAL) {
-        printk("aty128fb: invalid depth or RGBA\n");
-        return;
-    }
-
-    wait_for_fifo(2, par);
-    save_dp_datatype = aty_ld_le32(DP_DATATYPE);
-    save_dp_cntl     = aty_ld_le32(DP_CNTL);
-
-    wait_for_fifo(6, par);
-    aty_st_le32(SRC_Y_X, (srcy << 16) | srcx);
-    aty_st_le32(DP_MIX, ROP3_SRCCOPY | DP_SRC_RECT);
-    aty_st_le32(DP_CNTL, DST_X_LEFT_TO_RIGHT | DST_Y_TOP_TO_BOTTOM);
-    aty_st_le32(DP_DATATYPE, save_dp_datatype | dstval | SRC_DSTCOLOR);
-
-    aty_st_le32(DST_Y_X, (dsty << 16) | dstx);
-    aty_st_le32(DST_HEIGHT_WIDTH, (height << 16) | width);
-
-    par->blitter_may_be_busy = 1;
-
-    wait_for_fifo(2, par);
-    aty_st_le32(DP_DATATYPE, save_dp_datatype);
-    aty_st_le32(DP_CNTL, save_dp_cntl); 
+	u32 save_dp_datatype, save_dp_cntl, dstval;
+
+	if (!width || !height)
+		return;
+
+	dstval = depth_to_dst(par->current_par.crtc.depth);
+	if (dstval == DST_24BPP) {
+		srcx *= 3;
+		dstx *= 3;
+		width *= 3;
+	} else if (dstval == -EINVAL) {
+		printk("aty128fb: invalid depth or RGBA\n");
+		return;
+	}
+
+	wait_for_fifo(2, par);
+	save_dp_datatype = aty_ld_le32(DP_DATATYPE);
+	save_dp_cntl     = aty_ld_le32(DP_CNTL);
+
+	wait_for_fifo(6, par);
+	aty_st_le32(SRC_Y_X, (srcy << 16) | srcx);
+	aty_st_le32(DP_MIX, ROP3_SRCCOPY | DP_SRC_RECT);
+	aty_st_le32(DP_CNTL, DST_X_LEFT_TO_RIGHT | DST_Y_TOP_TO_BOTTOM);
+	aty_st_le32(DP_DATATYPE, save_dp_datatype | dstval | SRC_DSTCOLOR);
+
+	aty_st_le32(DST_Y_X, (dsty << 16) | dstx);
+	aty_st_le32(DST_HEIGHT_WIDTH, (height << 16) | width);
+
+	par->blitter_may_be_busy = 1;
+
+	wait_for_fifo(2, par);
+	aty_st_le32(DP_DATATYPE, save_dp_datatype);
+	aty_st_le32(DP_CNTL, save_dp_cntl);
 }
 
 
@@ -2358,17 +2380,17 @@ static inline void aty128_rectcopy(int srcx, int srcy, int dstx, int dsty,
      * Text mode accelerated functions
      */
 
-static void fbcon_aty128_bmove(struct display *p, int sy, int sx, int dy, int dx,
-			int height, int width)
+static void fbcon_aty128_bmove(struct display *p, int sy, int sx, int dy,
+			       int dx, int height, int width)
 {
-    sx     *= fontwidth(p);
-    sy     *= fontheight(p);
-    dx     *= fontwidth(p);
-    dy     *= fontheight(p);
-    width  *= fontwidth(p);
-    height *= fontheight(p);
-
-    aty128_rectcopy(sx, sy, dx, dy, width, height,
+	sx     *= fontwidth(p);
+	sy     *= fontheight(p);
+	dx     *= fontwidth(p);
+	dy     *= fontheight(p);
+	width  *= fontwidth(p);
+	height *= fontheight(p);
+
+	aty128_rectcopy(sx, sy, dx, dy, width, height,
 			(struct fb_info_aty128 *)p->fb_info);
 }
 #endif /* 0 */
diff --git a/drivers/video/aty/atyfb_base.c b/drivers/video/aty/atyfb_base.c
index 622f12b62a4..3f2e8c13f1c 100644
--- a/drivers/video/aty/atyfb_base.c
+++ b/drivers/video/aty/atyfb_base.c
@@ -863,7 +863,7 @@ static int aty_var_to_crtc(const struct fb_info *info,
 
 	if ((xres > 1600) || (yres > 1200)) {
 		FAIL("MACH64 chips are designed for max 1600x1200\n"
-		     "select anoter resolution.");
+		     "select another resolution.");
 	}
 	h_sync_strt = h_disp + var->right_margin;
 	h_sync_end = h_sync_strt + var->hsync_len;
diff --git a/drivers/video/aty/radeon_base.c b/drivers/video/aty/radeon_base.c
index ce1506b75ad..9e279ee38da 100644
--- a/drivers/video/aty/radeon_base.c
+++ b/drivers/video/aty/radeon_base.c
@@ -2018,7 +2018,7 @@ static void radeon_identify_vram(struct radeonfb_info *rinfo)
           if ((rinfo->family == CHIP_FAMILY_RS100) ||
               (rinfo->family == CHIP_FAMILY_RS200)) {
              /* This is to workaround the asic bug for RMX, some versions
-                of BIOS dosen't have this register initialized correctly.
+                of BIOS doesn't have this register initialized correctly.
              */
              OUTREGP(CRTC_MORE_CNTL, CRTC_H_CUTOFF_ACTIVE_EN,
                      ~CRTC_H_CUTOFF_ACTIVE_EN);
diff --git a/drivers/video/aty/radeon_monitor.c b/drivers/video/aty/radeon_monitor.c
index 9261c918fde..5c23eac0eb9 100644
--- a/drivers/video/aty/radeon_monitor.c
+++ b/drivers/video/aty/radeon_monitor.c
@@ -730,6 +730,25 @@ static void radeon_videomode_to_var(struct fb_var_screeninfo *var,
 	var->vmode = mode->vmode;
 }
 
+#ifdef CONFIG_PPC_PSERIES
+static int is_powerblade(const char *model)
+{
+	struct device_node *root;
+	const char* cp;
+	int len, l, rc = 0;
+
+	root = of_find_node_by_path("/");
+	if (root && model) {
+		l = strlen(model);
+		cp = of_get_property(root, "model", &len);
+		if (cp)
+			rc = memcmp(model, cp, min(len, l)) == 0;
+		of_node_put(root);
+	}
+	return rc;
+}
+#endif
+
 /*
  * Build the modedb for head 1 (head 2 will come later), check panel infos
  * from either BIOS or EDID, and pick up the default mode
@@ -865,6 +884,22 @@ void __devinit radeon_check_modes(struct radeonfb_info *rinfo, const char *mode_
 			has_default_mode = 1;
  	}
 
+#ifdef CONFIG_PPC_PSERIES
+	if (!has_default_mode && (
+		is_powerblade("IBM,8842") || /* JS20 */
+		is_powerblade("IBM,8844") || /* JS21 */
+		is_powerblade("IBM,7998") || /* JS12/JS21/JS22 */
+		is_powerblade("IBM,0792") || /* QS21 */
+		is_powerblade("IBM,0793")    /* QS22 */
+	    )) {
+		printk("Falling back to 800x600 on JSxx hardware\n");
+		if (fb_find_mode(&info->var, info, "800x600@60",
+				 info->monspecs.modedb,
+				 info->monspecs.modedb_len, NULL, 8) != 0)
+			has_default_mode = 1;
+	}
+#endif
+
 	/*
 	 * Still no mode, let's pick up a default from the db
 	 */
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index f49181c7311..f75da8758ad 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -228,6 +228,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 	data->port = pdata->flags;
 	if (data->port < 0) {
 		dev_err(&pdev->dev, "wrong platform data is assigned");
+		kfree(data);
 		return -EINVAL;
 	}
 
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 2979292650d..cf282763a8d 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -245,7 +245,7 @@ config BACKLIGHT_CARILLO_RANCH
 
 config BACKLIGHT_PWM
 	tristate "Generic PWM based Backlight Driver"
-	depends on HAVE_PWM
+	depends on PWM
 	help
 	  If you have a LCD backlight adjustable by PWM, say Y to enable
 	  this driver.
diff --git a/drivers/video/backlight/atmel-pwm-bl.c b/drivers/video/backlight/atmel-pwm-bl.c
index 0443a4f7185..df1cbb7ef6c 100644
--- a/drivers/video/backlight/atmel-pwm-bl.c
+++ b/drivers/video/backlight/atmel-pwm-bl.c
@@ -127,7 +127,8 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
 	struct atmel_pwm_bl *pwmbl;
 	int retval;
 
-	pwmbl = kzalloc(sizeof(struct atmel_pwm_bl), GFP_KERNEL);
+	pwmbl = devm_kzalloc(&pdev->dev, sizeof(struct atmel_pwm_bl),
+				GFP_KERNEL);
 	if (!pwmbl)
 		return -ENOMEM;
 
@@ -154,7 +155,8 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
 		goto err_free_mem;
 
 	if (pwmbl->gpio_on != -1) {
-		retval = gpio_request(pwmbl->gpio_on, "gpio_atmel_pwm_bl");
+		retval = devm_gpio_request(&pdev->dev, pwmbl->gpio_on,
+					"gpio_atmel_pwm_bl");
 		if (retval) {
 			pwmbl->gpio_on = -1;
 			goto err_free_pwm;
@@ -164,7 +166,7 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
 		retval = gpio_direction_output(pwmbl->gpio_on,
 				0 ^ pdata->on_active_low);
 		if (retval)
-			goto err_free_gpio;
+			goto err_free_pwm;
 	}
 
 	memset(&props, 0, sizeof(struct backlight_properties));
@@ -174,7 +176,7 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
 					  &atmel_pwm_bl_ops, &props);
 	if (IS_ERR(bldev)) {
 		retval = PTR_ERR(bldev);
-		goto err_free_gpio;
+		goto err_free_pwm;
 	}
 
 	pwmbl->bldev = bldev;
@@ -196,13 +198,9 @@ static int atmel_pwm_bl_probe(struct platform_device *pdev)
 err_free_bl_dev:
 	platform_set_drvdata(pdev, NULL);
 	backlight_device_unregister(bldev);
-err_free_gpio:
-	if (pwmbl->gpio_on != -1)
-		gpio_free(pwmbl->gpio_on);
 err_free_pwm:
 	pwm_channel_free(&pwmbl->pwmc);
 err_free_mem:
-	kfree(pwmbl);
 	return retval;
 }
 
@@ -210,15 +208,12 @@ static int __exit atmel_pwm_bl_remove(struct platform_device *pdev)
 {
 	struct atmel_pwm_bl *pwmbl = platform_get_drvdata(pdev);
 
-	if (pwmbl->gpio_on != -1) {
+	if (pwmbl->gpio_on != -1)
 		gpio_set_value(pwmbl->gpio_on, 0);
-		gpio_free(pwmbl->gpio_on);
-	}
 	pwm_channel_disable(&pwmbl->pwmc);
 	pwm_channel_free(&pwmbl->pwmc);
 	backlight_device_unregister(pwmbl->bldev);
 	platform_set_drvdata(pdev, NULL);
-	kfree(pwmbl);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/corgi_lcd.c b/drivers/video/backlight/corgi_lcd.c
index 23d732677ba..c781768ba89 100644
--- a/drivers/video/backlight/corgi_lcd.c
+++ b/drivers/video/backlight/corgi_lcd.c
@@ -492,7 +492,8 @@ static int setup_gpio_backlight(struct corgi_lcd *lcd,
 	lcd->gpio_backlight_cont = -1;
 
 	if (gpio_is_valid(pdata->gpio_backlight_on)) {
-		err = gpio_request(pdata->gpio_backlight_on, "BL_ON");
+		err = devm_gpio_request(&spi->dev, pdata->gpio_backlight_on,
+					"BL_ON");
 		if (err) {
 			dev_err(&spi->dev, "failed to request GPIO%d for "
 				"backlight_on\n", pdata->gpio_backlight_on);
@@ -504,11 +505,12 @@ static int setup_gpio_backlight(struct corgi_lcd *lcd,
 	}
 
 	if (gpio_is_valid(pdata->gpio_backlight_cont)) {
-		err = gpio_request(pdata->gpio_backlight_cont, "BL_CONT");
+		err = devm_gpio_request(&spi->dev, pdata->gpio_backlight_cont,
+					"BL_CONT");
 		if (err) {
 			dev_err(&spi->dev, "failed to request GPIO%d for "
 				"backlight_cont\n", pdata->gpio_backlight_cont);
-			goto err_free_backlight_on;
+			return err;
 		}
 
 		lcd->gpio_backlight_cont = pdata->gpio_backlight_cont;
@@ -525,11 +527,6 @@ static int setup_gpio_backlight(struct corgi_lcd *lcd,
 		}
 	}
 	return 0;
-
-err_free_backlight_on:
-	if (gpio_is_valid(lcd->gpio_backlight_on))
-		gpio_free(lcd->gpio_backlight_on);
-	return err;
 }
 
 static int __devinit corgi_lcd_probe(struct spi_device *spi)
@@ -602,12 +599,6 @@ static int __devexit corgi_lcd_remove(struct spi_device *spi)
 	backlight_update_status(lcd->bl_dev);
 	backlight_device_unregister(lcd->bl_dev);
 
-	if (gpio_is_valid(lcd->gpio_backlight_on))
-		gpio_free(lcd->gpio_backlight_on);
-
-	if (gpio_is_valid(lcd->gpio_backlight_cont))
-		gpio_free(lcd->gpio_backlight_cont);
-
 	corgi_lcd_set_power(lcd->lcd_dev, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(lcd->lcd_dev);
 
diff --git a/drivers/video/backlight/l4f00242t03.c b/drivers/video/backlight/l4f00242t03.c
index 40f606a8609..2d90c0648aa 100644
--- a/drivers/video/backlight/l4f00242t03.c
+++ b/drivers/video/backlight/l4f00242t03.c
@@ -175,28 +175,27 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 
 	priv->spi = spi;
 
-	ret = gpio_request_one(pdata->reset_gpio, GPIOF_OUT_INIT_HIGH,
-						"lcd l4f00242t03 reset");
+	ret = devm_gpio_request_one(&spi->dev, pdata->reset_gpio,
+			GPIOF_OUT_INIT_HIGH, "lcd l4f00242t03 reset");
 	if (ret) {
 		dev_err(&spi->dev,
 			"Unable to get the lcd l4f00242t03 reset gpio.\n");
 		return ret;
 	}
 
-	ret = gpio_request_one(pdata->data_enable_gpio, GPIOF_OUT_INIT_LOW,
-						"lcd l4f00242t03 data enable");
+	ret = devm_gpio_request_one(&spi->dev, pdata->data_enable_gpio,
+			GPIOF_OUT_INIT_LOW, "lcd l4f00242t03 data enable");
 	if (ret) {
 		dev_err(&spi->dev,
 			"Unable to get the lcd l4f00242t03 data en gpio.\n");
-		goto err;
+		return ret;
 	}
 
 	priv->io_reg = regulator_get(&spi->dev, "vdd");
 	if (IS_ERR(priv->io_reg)) {
-		ret = PTR_ERR(priv->io_reg);
 		dev_err(&spi->dev, "%s: Unable to get the IO regulator\n",
 		       __func__);
-		goto err2;
+		return PTR_ERR(priv->io_reg);
 	}
 
 	priv->core_reg = regulator_get(&spi->dev, "vcore");
@@ -204,14 +203,14 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 		ret = PTR_ERR(priv->core_reg);
 		dev_err(&spi->dev, "%s: Unable to get the core regulator\n",
 		       __func__);
-		goto err3;
+		goto err1;
 	}
 
 	priv->ld = lcd_device_register("l4f00242t03",
 					&spi->dev, priv, &l4f_ops);
 	if (IS_ERR(priv->ld)) {
 		ret = PTR_ERR(priv->ld);
-		goto err4;
+		goto err2;
 	}
 
 	/* Init the LCD */
@@ -223,14 +222,10 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 
 	return 0;
 
-err4:
+err2:
 	regulator_put(priv->core_reg);
-err3:
+err1:
 	regulator_put(priv->io_reg);
-err2:
-	gpio_free(pdata->data_enable_gpio);
-err:
-	gpio_free(pdata->reset_gpio);
 
 	return ret;
 }
@@ -238,16 +233,12 @@ err:
 static int __devexit l4f00242t03_remove(struct spi_device *spi)
 {
 	struct l4f00242t03_priv *priv = dev_get_drvdata(&spi->dev);
-	struct l4f00242t03_pdata *pdata = priv->spi->dev.platform_data;
 
 	l4f00242t03_lcd_power_set(priv->ld, FB_BLANK_POWERDOWN);
 	lcd_device_unregister(priv->ld);
 
 	dev_set_drvdata(&spi->dev, NULL);
 
-	gpio_free(pdata->data_enable_gpio);
-	gpio_free(pdata->reset_gpio);
-
 	regulator_put(priv->io_reg);
 	regulator_put(priv->core_reg);
 
diff --git a/drivers/video/backlight/lm3533_bl.c b/drivers/video/backlight/lm3533_bl.c
index bebeb63607d..18dca0c29c6 100644
--- a/drivers/video/backlight/lm3533_bl.c
+++ b/drivers/video/backlight/lm3533_bl.c
@@ -295,7 +295,7 @@ static int __devinit lm3533_bl_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	bl = kzalloc(sizeof(*bl), GFP_KERNEL);
+	bl = devm_kzalloc(&pdev->dev, sizeof(*bl), GFP_KERNEL);
 	if (!bl) {
 		dev_err(&pdev->dev,
 				"failed to allocate memory for backlight\n");
@@ -317,8 +317,7 @@ static int __devinit lm3533_bl_probe(struct platform_device *pdev)
 						&lm3533_bl_ops, &props);
 	if (IS_ERR(bd)) {
 		dev_err(&pdev->dev, "failed to register backlight device\n");
-		ret = PTR_ERR(bd);
-		goto err_free;
+		return PTR_ERR(bd);
 	}
 
 	bl->bd = bd;
@@ -348,8 +347,6 @@ err_sysfs_remove:
 	sysfs_remove_group(&bd->dev.kobj, &lm3533_bl_attribute_group);
 err_unregister:
 	backlight_device_unregister(bd);
-err_free:
-	kfree(bl);
 
 	return ret;
 }
@@ -367,7 +364,6 @@ static int __devexit lm3533_bl_remove(struct platform_device *pdev)
 	lm3533_ctrlbank_disable(&bl->cb);
 	sysfs_remove_group(&bd->dev.kobj, &lm3533_bl_attribute_group);
 	backlight_device_unregister(bd);
-	kfree(bl);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/lms283gf05.c b/drivers/video/backlight/lms283gf05.c
index a9f2c36966f..ea43f225419 100644
--- a/drivers/video/backlight/lms283gf05.c
+++ b/drivers/video/backlight/lms283gf05.c
@@ -158,29 +158,27 @@ static int __devinit lms283gf05_probe(struct spi_device *spi)
 	int ret = 0;
 
 	if (pdata != NULL) {
-		ret = gpio_request(pdata->reset_gpio, "LMS285GF05 RESET");
+		ret = devm_gpio_request(&spi->dev, pdata->reset_gpio,
+					"LMS285GF05 RESET");
 		if (ret)
 			return ret;
 
 		ret = gpio_direction_output(pdata->reset_gpio,
 						!pdata->reset_inverted);
 		if (ret)
-			goto err;
+			return ret;
 	}
 
 	st = devm_kzalloc(&spi->dev, sizeof(struct lms283gf05_state),
 				GFP_KERNEL);
 	if (st == NULL) {
 		dev_err(&spi->dev, "No memory for device state\n");
-		ret = -ENOMEM;
-		goto err;
+		return -ENOMEM;
 	}
 
 	ld = lcd_device_register("lms283gf05", &spi->dev, st, &lms_ops);
-	if (IS_ERR(ld)) {
-		ret = PTR_ERR(ld);
-		goto err;
-	}
+	if (IS_ERR(ld))
+		return PTR_ERR(ld);
 
 	st->spi = spi;
 	st->ld = ld;
@@ -193,24 +191,14 @@ static int __devinit lms283gf05_probe(struct spi_device *spi)
 	lms283gf05_toggle(spi, disp_initseq, ARRAY_SIZE(disp_initseq));
 
 	return 0;
-
-err:
-	if (pdata != NULL)
-		gpio_free(pdata->reset_gpio);
-
-	return ret;
 }
 
 static int __devexit lms283gf05_remove(struct spi_device *spi)
 {
 	struct lms283gf05_state *st = dev_get_drvdata(&spi->dev);
-	struct lms283gf05_pdata *pdata = st->spi->dev.platform_data;
 
 	lcd_device_unregister(st->ld);
 
-	if (pdata != NULL)
-		gpio_free(pdata->reset_gpio);
-
 	return 0;
 }
 
diff --git a/drivers/video/backlight/lp855x_bl.c b/drivers/video/backlight/lp855x_bl.c
index 72a0e0c917c..aa6d4f71131 100644
--- a/drivers/video/backlight/lp855x_bl.c
+++ b/drivers/video/backlight/lp855x_bl.c
@@ -14,11 +14,15 @@
 #include <linux/i2c.h>
 #include <linux/backlight.h>
 #include <linux/err.h>
-#include <linux/lp855x.h>
+#include <linux/platform_data/lp855x.h>
 
 /* Registers */
-#define BRIGHTNESS_CTRL		(0x00)
-#define DEVICE_CTRL		(0x01)
+#define BRIGHTNESS_CTRL		0x00
+#define DEVICE_CTRL		0x01
+#define EEPROM_START		0xA0
+#define EEPROM_END		0xA7
+#define EPROM_START		0xA0
+#define EPROM_END		0xAF
 
 #define BUF_SIZE		20
 #define DEFAULT_BL_NAME		"lcd-backlight"
diff --git a/drivers/video/backlight/ot200_bl.c b/drivers/video/backlight/ot200_bl.c
index f519d55a294..469cf0f109d 100644
--- a/drivers/video/backlight/ot200_bl.c
+++ b/drivers/video/backlight/ot200_bl.c
@@ -84,7 +84,8 @@ static int ot200_backlight_probe(struct platform_device *pdev)
 	int retval = 0;
 
 	/* request gpio */
-	if (gpio_request(GPIO_DIMM, "ot200 backlight dimmer") < 0) {
+	if (devm_gpio_request(&pdev->dev, GPIO_DIMM,
+				"ot200 backlight dimmer") < 0) {
 		dev_err(&pdev->dev, "failed to request GPIO %d\n", GPIO_DIMM);
 		return -ENODEV;
 	}
@@ -93,14 +94,13 @@ static int ot200_backlight_probe(struct platform_device *pdev)
 	pwm_timer = cs5535_mfgpt_alloc_timer(7, MFGPT_DOMAIN_ANY);
 	if (!pwm_timer) {
 		dev_err(&pdev->dev, "MFGPT 7 not available\n");
-		retval = -ENODEV;
-		goto error_mfgpt_alloc;
+		return -ENODEV;
 	}
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
 	if (!data) {
 		retval = -ENOMEM;
-		goto error_kzalloc;
+		goto error_devm_kzalloc;
 	}
 
 	/* setup gpio */
@@ -122,26 +122,21 @@ static int ot200_backlight_probe(struct platform_device *pdev)
 	if (IS_ERR(bl)) {
 		dev_err(&pdev->dev, "failed to register backlight\n");
 		retval = PTR_ERR(bl);
-		goto error_backlight_device_register;
+		goto error_devm_kzalloc;
 	}
 
 	platform_set_drvdata(pdev, bl);
 
 	return 0;
 
-error_backlight_device_register:
-	kfree(data);
-error_kzalloc:
+error_devm_kzalloc:
 	cs5535_mfgpt_free_timer(pwm_timer);
-error_mfgpt_alloc:
-	gpio_free(GPIO_DIMM);
 	return retval;
 }
 
 static int ot200_backlight_remove(struct platform_device *pdev)
 {
 	struct backlight_device *bl = platform_get_drvdata(pdev);
-	struct ot200_backlight_data *data = bl_get_data(bl);
 
 	backlight_device_unregister(bl);
 
@@ -152,9 +147,7 @@ static int ot200_backlight_remove(struct platform_device *pdev)
 		MAX_COMP2 - dim_table[100]);
 
 	cs5535_mfgpt_free_timer(pwm_timer);
-	gpio_free(GPIO_DIMM);
 
-	kfree(data);
 	return 0;
 }
 
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 342b7d7cbb6..995f0164c9b 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -26,11 +26,13 @@ struct pwm_bl_data {
 	struct device		*dev;
 	unsigned int		period;
 	unsigned int		lth_brightness;
+	unsigned int		*levels;
 	int			(*notify)(struct device *,
 					  int brightness);
 	void			(*notify_after)(struct device *,
 					int brightness);
 	int			(*check_fb)(struct device *, struct fb_info *);
+	void			(*exit)(struct device *);
 };
 
 static int pwm_backlight_update_status(struct backlight_device *bl)
@@ -52,9 +54,18 @@ static int pwm_backlight_update_status(struct backlight_device *bl)
 		pwm_config(pb->pwm, 0, pb->period);
 		pwm_disable(pb->pwm);
 	} else {
-		brightness = pb->lth_brightness +
-			(brightness * (pb->period - pb->lth_brightness) / max);
-		pwm_config(pb->pwm, brightness, pb->period);
+		int duty_cycle;
+
+		if (pb->levels) {
+			duty_cycle = pb->levels[brightness];
+			max = pb->levels[max];
+		} else {
+			duty_cycle = brightness;
+		}
+
+		duty_cycle = pb->lth_brightness +
+		     (duty_cycle * (pb->period - pb->lth_brightness) / max);
+		pwm_config(pb->pwm, duty_cycle, pb->period);
 		pwm_enable(pb->pwm);
 	}
 
@@ -83,17 +94,98 @@ static const struct backlight_ops pwm_backlight_ops = {
 	.check_fb	= pwm_backlight_check_fb,
 };
 
+#ifdef CONFIG_OF
+static int pwm_backlight_parse_dt(struct device *dev,
+				  struct platform_pwm_backlight_data *data)
+{
+	struct device_node *node = dev->of_node;
+	struct property *prop;
+	int length;
+	u32 value;
+	int ret;
+
+	if (!node)
+		return -ENODEV;
+
+	memset(data, 0, sizeof(*data));
+
+	/* determine the number of brightness levels */
+	prop = of_find_property(node, "brightness-levels", &length);
+	if (!prop)
+		return -EINVAL;
+
+	data->max_brightness = length / sizeof(u32);
+
+	/* read brightness levels from DT property */
+	if (data->max_brightness > 0) {
+		size_t size = sizeof(*data->levels) * data->max_brightness;
+
+		data->levels = devm_kzalloc(dev, size, GFP_KERNEL);
+		if (!data->levels)
+			return -ENOMEM;
+
+		ret = of_property_read_u32_array(node, "brightness-levels",
+						 data->levels,
+						 data->max_brightness);
+		if (ret < 0)
+			return ret;
+
+		ret = of_property_read_u32(node, "default-brightness-level",
+					   &value);
+		if (ret < 0)
+			return ret;
+
+		if (value >= data->max_brightness) {
+			dev_warn(dev, "invalid default brightness level: %u, using %u\n",
+				 value, data->max_brightness - 1);
+			value = data->max_brightness - 1;
+		}
+
+		data->dft_brightness = value;
+		data->max_brightness--;
+	}
+
+	/*
+	 * TODO: Most users of this driver use a number of GPIOs to control
+	 *       backlight power. Support for specifying these needs to be
+	 *       added.
+	 */
+
+	return 0;
+}
+
+static struct of_device_id pwm_backlight_of_match[] = {
+	{ .compatible = "pwm-backlight" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, pwm_backlight_of_match);
+#else
+static int pwm_backlight_parse_dt(struct device *dev,
+				  struct platform_pwm_backlight_data *data)
+{
+	return -ENODEV;
+}
+#endif
+
 static int pwm_backlight_probe(struct platform_device *pdev)
 {
-	struct backlight_properties props;
 	struct platform_pwm_backlight_data *data = pdev->dev.platform_data;
+	struct platform_pwm_backlight_data defdata;
+	struct backlight_properties props;
 	struct backlight_device *bl;
 	struct pwm_bl_data *pb;
+	unsigned int max;
 	int ret;
 
 	if (!data) {
-		dev_err(&pdev->dev, "failed to find platform data\n");
-		return -EINVAL;
+		ret = pwm_backlight_parse_dt(&pdev->dev, &defdata);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to find platform data\n");
+			return ret;
+		}
+
+		data = &defdata;
 	}
 
 	if (data->init) {
@@ -109,21 +201,42 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 		goto err_alloc;
 	}
 
-	pb->period = data->pwm_period_ns;
+	if (data->levels) {
+		max = data->levels[data->max_brightness];
+		pb->levels = data->levels;
+	} else
+		max = data->max_brightness;
+
 	pb->notify = data->notify;
 	pb->notify_after = data->notify_after;
 	pb->check_fb = data->check_fb;
-	pb->lth_brightness = data->lth_brightness *
-		(data->pwm_period_ns / data->max_brightness);
+	pb->exit = data->exit;
 	pb->dev = &pdev->dev;
 
-	pb->pwm = pwm_request(data->pwm_id, "backlight");
+	pb->pwm = pwm_get(&pdev->dev, NULL);
 	if (IS_ERR(pb->pwm)) {
-		dev_err(&pdev->dev, "unable to request PWM for backlight\n");
-		ret = PTR_ERR(pb->pwm);
-		goto err_alloc;
-	} else
-		dev_dbg(&pdev->dev, "got pwm for backlight\n");
+		dev_err(&pdev->dev, "unable to request PWM, trying legacy API\n");
+
+		pb->pwm = pwm_request(data->pwm_id, "pwm-backlight");
+		if (IS_ERR(pb->pwm)) {
+			dev_err(&pdev->dev, "unable to request legacy PWM\n");
+			ret = PTR_ERR(pb->pwm);
+			goto err_alloc;
+		}
+	}
+
+	dev_dbg(&pdev->dev, "got pwm for backlight\n");
+
+	/*
+	 * The DT case will set the pwm_period_ns field to 0 and store the
+	 * period, parsed from the DT, in the PWM device. For the non-DT case,
+	 * set the period from platform data.
+	 */
+	if (data->pwm_period_ns > 0)
+		pwm_set_period(pb->pwm, data->pwm_period_ns);
+
+	pb->period = pwm_get_period(pb->pwm);
+	pb->lth_brightness = data->lth_brightness * (pb->period / max);
 
 	memset(&props, 0, sizeof(struct backlight_properties));
 	props.type = BACKLIGHT_RAW;
@@ -143,7 +256,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	return 0;
 
 err_bl:
-	pwm_free(pb->pwm);
+	pwm_put(pb->pwm);
 err_alloc:
 	if (data->exit)
 		data->exit(&pdev->dev);
@@ -152,16 +265,15 @@ err_alloc:
 
 static int pwm_backlight_remove(struct platform_device *pdev)
 {
-	struct platform_pwm_backlight_data *data = pdev->dev.platform_data;
 	struct backlight_device *bl = platform_get_drvdata(pdev);
 	struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
 
 	backlight_device_unregister(bl);
 	pwm_config(pb->pwm, 0, pb->period);
 	pwm_disable(pb->pwm);
-	pwm_free(pb->pwm);
-	if (data->exit)
-		data->exit(&pdev->dev);
+	pwm_put(pb->pwm);
+	if (pb->exit)
+		pb->exit(&pdev->dev);
 	return 0;
 }
 
@@ -195,11 +307,12 @@ static SIMPLE_DEV_PM_OPS(pwm_backlight_pm_ops, pwm_backlight_suspend,
 
 static struct platform_driver pwm_backlight_driver = {
 	.driver		= {
-		.name	= "pwm-backlight",
-		.owner	= THIS_MODULE,
+		.name		= "pwm-backlight",
+		.owner		= THIS_MODULE,
 #ifdef CONFIG_PM
-		.pm	= &pwm_backlight_pm_ops,
+		.pm		= &pwm_backlight_pm_ops,
 #endif
+		.of_match_table	= of_match_ptr(pwm_backlight_of_match),
 	},
 	.probe		= pwm_backlight_probe,
 	.remove		= pwm_backlight_remove,
diff --git a/drivers/video/backlight/tosa_bl.c b/drivers/video/backlight/tosa_bl.c
index 0d54e607e82..49342e1d20b 100644
--- a/drivers/video/backlight/tosa_bl.c
+++ b/drivers/video/backlight/tosa_bl.c
@@ -92,14 +92,14 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
 
 	data->comadj = sharpsl_param.comadj == -1 ? COMADJ_DEFAULT : sharpsl_param.comadj;
 
-	ret = gpio_request(TOSA_GPIO_BL_C20MA, "backlight");
+	ret = devm_gpio_request(&client->dev, TOSA_GPIO_BL_C20MA, "backlight");
 	if (ret) {
 		dev_dbg(&data->bl->dev, "Unable to request gpio!\n");
 		return ret;
 	}
 	ret = gpio_direction_output(TOSA_GPIO_BL_C20MA, 0);
 	if (ret)
-		goto err_gpio_dir;
+		return ret;
 
 	i2c_set_clientdata(client, data);
 	data->i2c = client;
@@ -123,8 +123,6 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
 
 err_reg:
 	data->bl = NULL;
-err_gpio_dir:
-	gpio_free(TOSA_GPIO_BL_C20MA);
 	return ret;
 }
 
@@ -135,8 +133,6 @@ static int __devexit tosa_bl_remove(struct i2c_client *client)
 	backlight_device_unregister(data->bl);
 	data->bl = NULL;
 
-	gpio_free(TOSA_GPIO_BL_C20MA);
-
 	return 0;
 }
 
diff --git a/drivers/video/backlight/tosa_lcd.c b/drivers/video/backlight/tosa_lcd.c
index 47823b8efff..33047a66cc2 100644
--- a/drivers/video/backlight/tosa_lcd.c
+++ b/drivers/video/backlight/tosa_lcd.c
@@ -193,7 +193,7 @@ static int __devinit tosa_lcd_probe(struct spi_device *spi)
 	data->spi = spi;
 	dev_set_drvdata(&spi->dev, data);
 
-	ret = gpio_request(TOSA_GPIO_TG_ON, "tg #pwr");
+	ret = devm_gpio_request(&spi->dev, TOSA_GPIO_TG_ON, "tg #pwr");
 	if (ret < 0)
 		goto err_gpio_tg;
 
@@ -201,7 +201,7 @@ static int __devinit tosa_lcd_probe(struct spi_device *spi)
 
 	ret = gpio_direction_output(TOSA_GPIO_TG_ON, 0);
 	if (ret < 0)
-		goto err_gpio_dir;
+		goto err_gpio_tg;
 
 	mdelay(60);
 	tosa_lcd_tg_init(data);
@@ -221,8 +221,6 @@ static int __devinit tosa_lcd_probe(struct spi_device *spi)
 
 err_register:
 	tosa_lcd_tg_off(data);
-err_gpio_dir:
-	gpio_free(TOSA_GPIO_TG_ON);
 err_gpio_tg:
 	dev_set_drvdata(&spi->dev, NULL);
 	return ret;
@@ -239,7 +237,6 @@ static int __devexit tosa_lcd_remove(struct spi_device *spi)
 
 	tosa_lcd_tg_off(data);
 
-	gpio_free(TOSA_GPIO_TG_ON);
 	dev_set_drvdata(&spi->dev, NULL);
 
 	return 0;
diff --git a/drivers/video/bfin_adv7393fb.c b/drivers/video/bfin_adv7393fb.c
index 9bdd4b0c18c..d0f121bd8b2 100644
--- a/drivers/video/bfin_adv7393fb.c
+++ b/drivers/video/bfin_adv7393fb.c
@@ -58,7 +58,7 @@ static const unsigned short ppi_pins[] = {
  */
 
 static struct bfin_adv7393_fb_par {
-	/* structure holding blackfin / adv7393 paramters when
+	/* structure holding blackfin / adv7393 parameters when
 	   screen is blanked */
 	struct {
 		u8 Mode;	/* ntsc/pal/? */
diff --git a/drivers/video/cirrusfb.c b/drivers/video/cirrusfb.c
index 738c8ce7d13..bc67d05cad6 100644
--- a/drivers/video/cirrusfb.c
+++ b/drivers/video/cirrusfb.c
@@ -1611,7 +1611,7 @@ static void init_vgachip(struct fb_info *info)
 	/* ext. display controls: ext.adr. wrap */
 	vga_wcrt(cinfo->regbase, CL_CRT1B, 0x02);
 
-	/* Set/Reset registes: - */
+	/* Set/Reset registers: - */
 	vga_wgfx(cinfo->regbase, VGA_GFX_SR_VALUE, 0x00);
 	/* Set/Reset enable: - */
 	vga_wgfx(cinfo->regbase, VGA_GFX_SR_ENABLE, 0x00);
diff --git a/drivers/video/da8xx-fb.c b/drivers/video/da8xx-fb.c
index 47118c75a4c..7ae9d53f2bf 100644
--- a/drivers/video/da8xx-fb.c
+++ b/drivers/video/da8xx-fb.c
@@ -30,7 +30,10 @@
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
 #include <linux/console.h>
+#include <linux/spinlock.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/lcm.h>
 #include <video/da8xx-fb.h>
 #include <asm/div64.h>
 
@@ -160,6 +163,13 @@ struct da8xx_fb_par {
 	wait_queue_head_t	vsync_wait;
 	int			vsync_flag;
 	int			vsync_timeout;
+	spinlock_t		lock_for_chan_update;
+
+	/*
+	 * LCDC has 2 ping pong DMA channels, channel 0
+	 * and channel 1.
+	 */
+	unsigned int		which_dma_channel_done;
 #ifdef CONFIG_CPU_FREQ
 	struct notifier_block	freq_transition;
 	unsigned int		lcd_fck_rate;
@@ -260,10 +270,18 @@ static inline void lcd_enable_raster(void)
 {
 	u32 reg;
 
+	/* Put LCDC in reset for several cycles */
+	if (lcd_revision == LCD_VERSION_2)
+		/* Write 1 to reset LCDC */
+		lcdc_write(LCD_CLK_MAIN_RESET, LCD_CLK_RESET_REG);
+	mdelay(1);
+
 	/* Bring LCDC out of reset */
 	if (lcd_revision == LCD_VERSION_2)
 		lcdc_write(0, LCD_CLK_RESET_REG);
+	mdelay(1);
 
+	/* Above reset sequence doesnot reset register context */
 	reg = lcdc_read(LCD_RASTER_CTRL_REG);
 	if (!(reg & LCD_RASTER_ENABLE))
 		lcdc_write(reg | LCD_RASTER_ENABLE, LCD_RASTER_CTRL_REG);
@@ -277,10 +295,6 @@ static inline void lcd_disable_raster(void)
 	reg = lcdc_read(LCD_RASTER_CTRL_REG);
 	if (reg & LCD_RASTER_ENABLE)
 		lcdc_write(reg & ~LCD_RASTER_ENABLE, LCD_RASTER_CTRL_REG);
-
-	if (lcd_revision == LCD_VERSION_2)
-		/* Write 1 to reset LCDC */
-		lcdc_write(LCD_CLK_MAIN_RESET, LCD_CLK_RESET_REG);
 }
 
 static void lcd_blit(int load_mode, struct da8xx_fb_par *par)
@@ -344,8 +358,8 @@ static void lcd_blit(int load_mode, struct da8xx_fb_par *par)
 	lcd_enable_raster();
 }
 
-/* Configure the Burst Size of DMA */
-static int lcd_cfg_dma(int burst_size)
+/* Configure the Burst Size and fifo threhold of DMA */
+static int lcd_cfg_dma(int burst_size, int fifo_th)
 {
 	u32 reg;
 
@@ -369,6 +383,9 @@ static int lcd_cfg_dma(int burst_size)
 	default:
 		return -EINVAL;
 	}
+
+	reg |= (fifo_th << 8);
+
 	lcdc_write(reg, LCD_DMA_CTRL_REG);
 
 	return 0;
@@ -670,8 +687,8 @@ static int lcd_init(struct da8xx_fb_par *par, const struct lcd_ctrl_config *cfg,
 		lcdc_write((lcdc_read(LCD_RASTER_TIMING_2_REG) &
 			~LCD_INVERT_PIXEL_CLOCK), LCD_RASTER_TIMING_2_REG);
 
-	/* Configure the DMA burst size. */
-	ret = lcd_cfg_dma(cfg->dma_burst_sz);
+	/* Configure the DMA burst size and fifo threshold. */
+	ret = lcd_cfg_dma(cfg->dma_burst_sz, cfg->fifo_th);
 	if (ret < 0)
 		return ret;
 
@@ -715,7 +732,6 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 {
 	struct da8xx_fb_par *par = arg;
 	u32 stat = lcdc_read(LCD_MASKED_STAT_REG);
-	u32 reg_int;
 
 	if ((stat & LCD_SYNC_LOST) && (stat & LCD_FIFO_UNDERFLOW)) {
 		lcd_disable_raster();
@@ -732,10 +748,8 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 
 		lcdc_write(stat, LCD_MASKED_STAT_REG);
 
-		/* Disable PL completion inerrupt */
-		reg_int = lcdc_read(LCD_INT_ENABLE_CLR_REG) |
-		       (LCD_V2_PL_INT_ENA);
-		lcdc_write(reg_int, LCD_INT_ENABLE_CLR_REG);
+		/* Disable PL completion interrupt */
+		lcdc_write(LCD_V2_PL_INT_ENA, LCD_INT_ENABLE_CLR_REG);
 
 		/* Setup and start data loading mode */
 		lcd_blit(LOAD_DATA, par);
@@ -743,6 +757,7 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 		lcdc_write(stat, LCD_MASKED_STAT_REG);
 
 		if (stat & LCD_END_OF_FRAME0) {
+			par->which_dma_channel_done = 0;
 			lcdc_write(par->dma_start,
 				   LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
 			lcdc_write(par->dma_end,
@@ -752,6 +767,7 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 		}
 
 		if (stat & LCD_END_OF_FRAME1) {
+			par->which_dma_channel_done = 1;
 			lcdc_write(par->dma_start,
 				   LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
 			lcdc_write(par->dma_end,
@@ -798,6 +814,7 @@ static irqreturn_t lcdc_irq_handler_rev01(int irq, void *arg)
 		lcdc_write(stat, LCD_STAT_REG);
 
 		if (stat & LCD_END_OF_FRAME0) {
+			par->which_dma_channel_done = 0;
 			lcdc_write(par->dma_start,
 				   LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
 			lcdc_write(par->dma_end,
@@ -807,6 +824,7 @@ static irqreturn_t lcdc_irq_handler_rev01(int irq, void *arg)
 		}
 
 		if (stat & LCD_END_OF_FRAME1) {
+			par->which_dma_channel_done = 1;
 			lcdc_write(par->dma_start,
 				   LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
 			lcdc_write(par->dma_end,
@@ -1021,11 +1039,14 @@ static int cfb_blank(int blank, struct fb_info *info)
 	par->blank = blank;
 	switch (blank) {
 	case FB_BLANK_UNBLANK:
+		lcd_enable_raster();
+
 		if (par->panel_power_ctrl)
 			par->panel_power_ctrl(1);
-
-		lcd_enable_raster();
 		break;
+	case FB_BLANK_NORMAL:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
 	case FB_BLANK_POWERDOWN:
 		if (par->panel_power_ctrl)
 			par->panel_power_ctrl(0);
@@ -1052,6 +1073,7 @@ static int da8xx_pan_display(struct fb_var_screeninfo *var,
 	struct fb_fix_screeninfo    *fix = &fbi->fix;
 	unsigned int end;
 	unsigned int start;
+	unsigned long irq_flags;
 
 	if (var->xoffset != fbi->var.xoffset ||
 			var->yoffset != fbi->var.yoffset) {
@@ -1069,6 +1091,21 @@ static int da8xx_pan_display(struct fb_var_screeninfo *var,
 			end	= start + fbi->var.yres * fix->line_length - 1;
 			par->dma_start	= start;
 			par->dma_end	= end;
+			spin_lock_irqsave(&par->lock_for_chan_update,
+					irq_flags);
+			if (par->which_dma_channel_done == 0) {
+				lcdc_write(par->dma_start,
+					   LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
+				lcdc_write(par->dma_end,
+					   LCD_DMA_FRM_BUF_CEILING_ADDR_0_REG);
+			} else if (par->which_dma_channel_done == 1) {
+				lcdc_write(par->dma_start,
+					   LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
+				lcdc_write(par->dma_end,
+					   LCD_DMA_FRM_BUF_CEILING_ADDR_1_REG);
+			}
+			spin_unlock_irqrestore(&par->lock_for_chan_update,
+					irq_flags);
 		}
 	}
 
@@ -1114,6 +1151,7 @@ static int __devinit fb_probe(struct platform_device *device)
 	struct da8xx_fb_par *par;
 	resource_size_t len;
 	int ret, i;
+	unsigned long ulcm;
 
 	if (fb_pdata == NULL) {
 		dev_err(&device->dev, "Can not get platform data\n");
@@ -1209,7 +1247,8 @@ static int __devinit fb_probe(struct platform_device *device)
 
 	/* allocate frame buffer */
 	par->vram_size = lcdc_info->width * lcdc_info->height * lcd_cfg->bpp;
-	par->vram_size = PAGE_ALIGN(par->vram_size/8);
+	ulcm = lcm((lcdc_info->width * lcd_cfg->bpp)/8, PAGE_SIZE);
+	par->vram_size = roundup(par->vram_size/8, ulcm);
 	par->vram_size = par->vram_size * LCD_NUM_BUFFERS;
 
 	par->vram_virt = dma_alloc_coherent(NULL,
@@ -1296,6 +1335,8 @@ static int __devinit fb_probe(struct platform_device *device)
 	/* initialize the vsync wait queue */
 	init_waitqueue_head(&par->vsync_wait);
 	par->vsync_timeout = HZ / 5;
+	par->which_dma_channel_done = -1;
+	spin_lock_init(&par->lock_for_chan_update);
 
 	/* Register the Frame Buffer  */
 	if (register_framebuffer(da8xx_fb_info) < 0) {
@@ -1382,11 +1423,12 @@ static int fb_resume(struct platform_device *dev)
 	struct da8xx_fb_par *par = info->par;
 
 	console_lock();
+	clk_enable(par->lcdc_clk);
+	lcd_enable_raster();
+
 	if (par->panel_power_ctrl)
 		par->panel_power_ctrl(1);
 
-	clk_enable(par->lcdc_clk);
-	lcd_enable_raster();
 	fb_set_suspend(info, 0);
 	console_unlock();
 
diff --git a/drivers/video/epson1355fb.c b/drivers/video/epson1355fb.c
index a268cbf1cbe..68b9b511ce8 100644
--- a/drivers/video/epson1355fb.c
+++ b/drivers/video/epson1355fb.c
@@ -477,11 +477,11 @@ static __init unsigned int get_fb_size(struct fb_info *info)
 	return size;
 }
 
-static int epson1355_width_tab[2][4] __initdata =
+static int epson1355_width_tab[2][4] __devinitdata =
     { {4, 8, 16, -1}, {9, 12, 16, -1} };
-static int epson1355_bpp_tab[8] __initdata = { 1, 2, 4, 8, 15, 16 };
+static int epson1355_bpp_tab[8] __devinitdata = { 1, 2, 4, 8, 15, 16 };
 
-static void __init fetch_hw_state(struct fb_info *info, struct epson1355_par *par)
+static void __devinit fetch_hw_state(struct fb_info *info, struct epson1355_par *par)
 {
 	struct fb_var_screeninfo *var = &info->var;
 	struct fb_fix_screeninfo *fix = &info->fix;
@@ -601,7 +601,7 @@ static int epson1355fb_remove(struct platform_device *dev)
 	return 0;
 }
 
-int __devinit epson1355fb_probe(struct platform_device *dev)
+static int __devinit epson1355fb_probe(struct platform_device *dev)
 {
 	struct epson1355_par *default_par;
 	struct fb_info *info;
diff --git a/drivers/video/exynos/exynos_dp_core.c b/drivers/video/exynos/exynos_dp_core.c
index a36b2d28280..c6c016a506c 100644
--- a/drivers/video/exynos/exynos_dp_core.c
+++ b/drivers/video/exynos/exynos_dp_core.c
@@ -47,7 +47,7 @@ static int exynos_dp_detect_hpd(struct exynos_dp_device *dp)
 
 	exynos_dp_init_hpd(dp);
 
-	udelay(200);
+	usleep_range(200, 210);
 
 	while (exynos_dp_get_plug_in_status(dp) != 0) {
 		timeout_loop++;
@@ -55,7 +55,7 @@ static int exynos_dp_detect_hpd(struct exynos_dp_device *dp)
 			dev_err(dp->dev, "failed to get hpd plug status\n");
 			return -ETIMEDOUT;
 		}
-		udelay(10);
+		usleep_range(10, 11);
 	}
 
 	return 0;
@@ -304,7 +304,7 @@ static void exynos_dp_link_start(struct exynos_dp_device *dp)
 		buf[lane] = DPCD_PRE_EMPHASIS_PATTERN2_LEVEL0 |
 			    DPCD_VOLTAGE_SWING_PATTERN1_LEVEL0;
 	exynos_dp_write_bytes_to_dpcd(dp,
-		DPCD_ADDR_TRAINING_PATTERN_SET,
+		DPCD_ADDR_TRAINING_LANE0_SET,
 		lane_count, buf);
 }
 
@@ -336,7 +336,7 @@ static int exynos_dp_channel_eq_ok(u8 link_status[6], int lane_count)
 	u8 lane_status;
 
 	lane_align = link_status[2];
-	if ((lane_align == DPCD_INTERLANE_ALIGN_DONE) == 0)
+	if ((lane_align & DPCD_INTERLANE_ALIGN_DONE) == 0)
 		return -EINVAL;
 
 	for (lane = 0; lane < lane_count; lane++) {
@@ -407,6 +407,9 @@ static unsigned int exynos_dp_get_lane_link_training(
 	case 3:
 		reg = exynos_dp_get_lane3_link_training(dp);
 		break;
+	default:
+		WARN_ON(1);
+		return 0;
 	}
 
 	return reg;
@@ -483,7 +486,7 @@ static int exynos_dp_process_clock_recovery(struct exynos_dp_device *dp)
 	u8 pre_emphasis;
 	u8 training_lane;
 
-	udelay(100);
+	usleep_range(100, 101);
 
 	exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
 				6, link_status);
@@ -501,7 +504,7 @@ static int exynos_dp_process_clock_recovery(struct exynos_dp_device *dp)
 		buf[0] = DPCD_SCRAMBLING_DISABLED |
 			 DPCD_TRAINING_PATTERN_2;
 		exynos_dp_write_byte_to_dpcd(dp,
-			DPCD_ADDR_TRAINING_LANE0_SET,
+			DPCD_ADDR_TRAINING_PATTERN_SET,
 			buf[0]);
 
 		for (lane = 0; lane < lane_count; lane++) {
@@ -568,7 +571,7 @@ static int exynos_dp_process_equalizer_training(struct exynos_dp_device *dp)
 
 	u8 adjust_request[2];
 
-	udelay(400);
+	usleep_range(400, 401);
 
 	exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
 				6, link_status);
@@ -736,7 +739,7 @@ static int exynos_dp_set_link_train(struct exynos_dp_device *dp,
 		if (retval == 0)
 			break;
 
-		udelay(100);
+		usleep_range(100, 110);
 	}
 
 	return retval;
@@ -770,7 +773,7 @@ static int exynos_dp_config_video(struct exynos_dp_device *dp,
 			return -ETIMEDOUT;
 		}
 
-		udelay(1);
+		usleep_range(1, 2);
 	}
 
 	/* Set to use the register calculated M/N video */
@@ -804,7 +807,7 @@ static int exynos_dp_config_video(struct exynos_dp_device *dp,
 			return -ETIMEDOUT;
 		}
 
-		mdelay(1);
+		usleep_range(1000, 1001);
 	}
 
 	if (retval != 0)
diff --git a/drivers/video/exynos/exynos_dp_core.h b/drivers/video/exynos/exynos_dp_core.h
index 1e0f998e0c9..8526e548c38 100644
--- a/drivers/video/exynos/exynos_dp_core.h
+++ b/drivers/video/exynos/exynos_dp_core.h
@@ -85,10 +85,6 @@ void exynos_dp_set_link_bandwidth(struct exynos_dp_device *dp, u32 bwtype);
 void exynos_dp_get_link_bandwidth(struct exynos_dp_device *dp, u32 *bwtype);
 void exynos_dp_set_lane_count(struct exynos_dp_device *dp, u32 count);
 void exynos_dp_get_lane_count(struct exynos_dp_device *dp, u32 *count);
-void exynos_dp_set_link_bandwidth(struct exynos_dp_device *dp, u32 bwtype);
-void exynos_dp_get_link_bandwidth(struct exynos_dp_device *dp, u32 *bwtype);
-void exynos_dp_set_lane_count(struct exynos_dp_device *dp, u32 count);
-void exynos_dp_get_lane_count(struct exynos_dp_device *dp, u32 *count);
 void exynos_dp_enable_enhanced_mode(struct exynos_dp_device *dp, bool enable);
 void exynos_dp_set_training_pattern(struct exynos_dp_device *dp,
 				 enum pattern_set pattern);
diff --git a/drivers/video/exynos/exynos_dp_reg.c b/drivers/video/exynos/exynos_dp_reg.c
index 6ce76d56c3a..2db5b9aa250 100644
--- a/drivers/video/exynos/exynos_dp_reg.c
+++ b/drivers/video/exynos/exynos_dp_reg.c
@@ -122,7 +122,7 @@ void exynos_dp_reset(struct exynos_dp_device *dp)
 		LS_CLK_DOMAIN_FUNC_EN_N;
 	writel(reg, dp->reg_base + EXYNOS_DP_FUNC_EN_2);
 
-	udelay(20);
+	usleep_range(20, 30);
 
 	exynos_dp_lane_swap(dp, 0);
 
@@ -752,7 +752,7 @@ int exynos_dp_read_bytes_from_i2c(struct exynos_dp_device *dp,
 
 			/*
 			 * If Rx sends defer, Tx sends only reads
-			 * request without sending addres
+			 * request without sending address
 			 */
 			if (!defer)
 				retval = exynos_dp_select_i2c_device(dp,
@@ -988,7 +988,7 @@ void exynos_dp_reset_macro(struct exynos_dp_device *dp)
 	writel(reg, dp->reg_base + EXYNOS_DP_PHY_TEST);
 
 	/* 10 us is the minimum reset time. */
-	udelay(10);
+	usleep_range(10, 20);
 
 	reg &= ~MACRO_RST;
 	writel(reg, dp->reg_base + EXYNOS_DP_PHY_TEST);
diff --git a/drivers/video/exynos/exynos_mipi_dsi.c b/drivers/video/exynos/exynos_mipi_dsi.c
index 6c1f5c314a4..4bc2b8a5dd8 100644
--- a/drivers/video/exynos/exynos_mipi_dsi.c
+++ b/drivers/video/exynos/exynos_mipi_dsi.c
@@ -106,7 +106,7 @@ static void exynos_mipi_update_cfg(struct mipi_dsim_device *dsim)
 
 	/*
 	 * data from Display controller(FIMD) is transferred in video mode
-	 * but in case of command mode, all settigs is updated to registers.
+	 * but in case of command mode, all settings are updated to registers.
 	 */
 	exynos_mipi_dsi_stand_by(dsim, 1);
 }
@@ -154,7 +154,7 @@ static int exynos_mipi_dsi_blank_mode(struct mipi_dsim_device *dsim, int power)
 		if (client_drv && client_drv->power_on)
 			client_drv->power_on(client_dev, 1);
 
-		exynos_mipi_regulator_disable(dsim);
+		exynos_mipi_regulator_enable(dsim);
 
 		/* enable MIPI-DSI PHY. */
 		if (dsim->pd->phy_enable)
diff --git a/drivers/video/exynos/s6e8ax0.h b/drivers/video/exynos/s6e8ax0.h
deleted file mode 100644
index 1f1b270484b..00000000000
--- a/drivers/video/exynos/s6e8ax0.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/* linux/drivers/video/backlight/s6e8ax0.h
- *
- * MIPI-DSI based s6e8ax0 AMOLED LCD Panel definitions.
- *
- * Copyright (c) 2011 Samsung Electronics
- *
- * Inki Dae, <inki.dae@samsung.com>
- * Donghwa Lee <dh09.lee@samsung.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
-*/
-
-#ifndef _S6E8AX0_H
-#define _S6E8AX0_H
-
-extern void s6e8ax0_init(void);
-
-#endif
-
diff --git a/drivers/video/fb_defio.c b/drivers/video/fb_defio.c
index 1ddeb11659d..64cda560c48 100644
--- a/drivers/video/fb_defio.c
+++ b/drivers/video/fb_defio.c
@@ -104,6 +104,8 @@ static int fb_deferred_io_mkwrite(struct vm_area_struct *vma,
 	deferred framebuffer IO. then if userspace touches a page
 	again, we repeat the same scheme */
 
+	file_update_time(vma->vm_file);
+
 	/* protect against the workqueue changing the page list */
 	mutex_lock(&fbdefio->lock);
 
diff --git a/drivers/video/fb_draw.h b/drivers/video/fb_draw.h
index 04c01faaf77..624ee115f12 100644
--- a/drivers/video/fb_draw.h
+++ b/drivers/video/fb_draw.h
@@ -3,6 +3,7 @@
 
 #include <asm/types.h>
 #include <linux/fb.h>
+#include <linux/bug.h>
 
     /*
      *  Compose two values, using a bitmask as decision value
@@ -41,7 +42,8 @@ pixel_to_pat( u32 bpp, u32 pixel)
 	case 32:
 		return 0x0000000100000001ul*pixel;
 	default:
-		panic("pixel_to_pat(): unsupported pixelformat\n");
+		WARN(1, "pixel_to_pat(): unsupported pixelformat %d\n", bpp);
+		return 0;
     }
 }
 #else
@@ -66,7 +68,8 @@ pixel_to_pat( u32 bpp, u32 pixel)
 	case 32:
 		return 0x00000001ul*pixel;
 	default:
-		panic("pixel_to_pat(): unsupported pixelformat\n");
+		WARN(1, "pixel_to_pat(): unsupported pixelformat %d\n", bpp);
+		return 0;
     }
 }
 #endif
diff --git a/drivers/video/grvga.c b/drivers/video/grvga.c
index da066c21092..5245f9a7189 100644
--- a/drivers/video/grvga.c
+++ b/drivers/video/grvga.c
@@ -354,7 +354,7 @@ static int __devinit grvga_probe(struct platform_device *dev)
 	 */
 	if (fb_get_options("grvga", &options)) {
 		retval = -ENODEV;
-		goto err;
+		goto free_fb;
 	}
 
 	if (!options || !*options)
@@ -370,7 +370,7 @@ static int __devinit grvga_probe(struct platform_device *dev)
 			if (grvga_parse_custom(this_opt, &info->var) < 0) {
 				dev_err(&dev->dev, "Failed to parse custom mode (%s).\n", this_opt);
 				retval = -EINVAL;
-				goto err1;
+				goto free_fb;
 			}
 		} else if (!strncmp(this_opt, "addr", 4))
 			grvga_fix_addr = simple_strtoul(this_opt + 5, NULL, 16);
@@ -387,10 +387,11 @@ static int __devinit grvga_probe(struct platform_device *dev)
 	info->flags = FBINFO_DEFAULT | FBINFO_PARTIAL_PAN_OK | FBINFO_HWACCEL_YPAN;
 	info->fix.smem_len = grvga_mem_size;
 
-	if (!request_mem_region(dev->resource[0].start, resource_size(&dev->resource[0]), "grlib-svgactrl regs")) {
+	if (!devm_request_mem_region(&dev->dev, dev->resource[0].start,
+		    resource_size(&dev->resource[0]), "grlib-svgactrl regs")) {
 		dev_err(&dev->dev, "registers already mapped\n");
 		retval = -EBUSY;
-		goto err;
+		goto free_fb;
 	}
 
 	par->regs = of_ioremap(&dev->resource[0], 0,
@@ -400,14 +401,14 @@ static int __devinit grvga_probe(struct platform_device *dev)
 	if (!par->regs) {
 		dev_err(&dev->dev, "failed to map registers\n");
 		retval = -ENOMEM;
-		goto err1;
+		goto free_fb;
 	}
 
 	retval = fb_alloc_cmap(&info->cmap, 256, 0);
 	if (retval < 0) {
 		dev_err(&dev->dev, "failed to allocate mem with fb_alloc_cmap\n");
 		retval = -ENOMEM;
-		goto err2;
+		goto unmap_regs;
 	}
 
 	if (mode_opt) {
@@ -415,7 +416,7 @@ static int __devinit grvga_probe(struct platform_device *dev)
 				      grvga_modedb, sizeof(grvga_modedb), &grvga_modedb[0], 8);
 		if (!retval || retval == 4) {
 			retval = -EINVAL;
-			goto err3;
+			goto dealloc_cmap;
 		}
 	}
 
@@ -427,10 +428,11 @@ static int __devinit grvga_probe(struct platform_device *dev)
 
 		physical_start = grvga_fix_addr;
 
-		if (!request_mem_region(physical_start, grvga_mem_size, dev->name)) {
+		if (!devm_request_mem_region(&dev->dev, physical_start,
+					     grvga_mem_size, dev->name)) {
 			dev_err(&dev->dev, "failed to request memory region\n");
 			retval = -ENOMEM;
-			goto err3;
+			goto dealloc_cmap;
 		}
 
 		virtual_start = (unsigned long) ioremap(physical_start, grvga_mem_size);
@@ -438,7 +440,7 @@ static int __devinit grvga_probe(struct platform_device *dev)
 		if (!virtual_start) {
 			dev_err(&dev->dev, "error mapping framebuffer memory\n");
 			retval = -ENOMEM;
-			goto err4;
+			goto dealloc_cmap;
 		}
 	} else {	/* Allocate frambuffer memory */
 
@@ -451,7 +453,7 @@ static int __devinit grvga_probe(struct platform_device *dev)
 				"unable to allocate framebuffer memory (%lu bytes)\n",
 				grvga_mem_size);
 			retval = -ENOMEM;
-			goto err3;
+			goto dealloc_cmap;
 		}
 
 		physical_start = dma_map_single(&dev->dev, (void *)virtual_start, grvga_mem_size, DMA_TO_DEVICE);
@@ -484,7 +486,7 @@ static int __devinit grvga_probe(struct platform_device *dev)
 	retval = register_framebuffer(info);
 	if (retval < 0) {
 		dev_err(&dev->dev, "failed to register framebuffer\n");
-		goto err4;
+		goto free_mem;
 	}
 
 	__raw_writel(physical_start, &par->regs->fb_pos);
@@ -493,21 +495,18 @@ static int __devinit grvga_probe(struct platform_device *dev)
 
 	return 0;
 
-err4:
+free_mem:
 	dev_set_drvdata(&dev->dev, NULL);
-	if (grvga_fix_addr) {
-		release_mem_region(physical_start, grvga_mem_size);
+	if (grvga_fix_addr)
 		iounmap((void *)virtual_start);
-	} else
+	else
 		kfree((void *)virtual_start);
-err3:
+dealloc_cmap:
 	fb_dealloc_cmap(&info->cmap);
-err2:
+unmap_regs:
 	of_iounmap(&dev->resource[0], par->regs,
 		   resource_size(&dev->resource[0]));
-err1:
-	release_mem_region(dev->resource[0].start, resource_size(&dev->resource[0]));
-err:
+free_fb:
 	framebuffer_release(info);
 
 	return retval;
@@ -524,12 +523,10 @@ static int __devexit grvga_remove(struct platform_device *device)
 
 		of_iounmap(&device->resource[0], par->regs,
 			   resource_size(&device->resource[0]));
-		release_mem_region(device->resource[0].start, resource_size(&device->resource[0]));
 
-		if (!par->fb_alloced) {
-			release_mem_region(info->fix.smem_start, info->fix.smem_len);
+		if (!par->fb_alloced)
 			iounmap(info->screen_base);
-		} else
+		else
 			kfree((void *)info->screen_base);
 
 		framebuffer_release(info);
diff --git a/drivers/video/i740fb.c b/drivers/video/i740fb.c
index fe574d84ed9..ff3f8808e4e 100644
--- a/drivers/video/i740fb.c
+++ b/drivers/video/i740fb.c
@@ -497,7 +497,7 @@ static int i740fb_decode_var(const struct fb_var_screeninfo *var,
 
 	mem = vxres * vyres * ((bpp + 1) / 8);
 	if (mem > info->screen_size) {
-		dev_err(info->device, "not enough video memory (%d KB requested, %ld KB avaliable)\n",
+		dev_err(info->device, "not enough video memory (%d KB requested, %ld KB available)\n",
 			mem >> 10, info->screen_size >> 10);
 		return -ENOMEM;
 	}
@@ -728,7 +728,7 @@ static void vga_protect(struct i740fb_par *par)
 	i740outreg_mask(par, VGA_SEQ_I, VGA_SEQ_CLOCK_MODE, 0x20, 0x20);
 
 	i740inb(par, 0x3DA);
-	i740outb(par, VGA_ATT_W, 0x00);	/* enable pallete access */
+	i740outb(par, VGA_ATT_W, 0x00);	/* enable palette access */
 }
 
 static void vga_unprotect(struct i740fb_par *par)
@@ -737,7 +737,7 @@ static void vga_unprotect(struct i740fb_par *par)
 	i740outreg_mask(par, VGA_SEQ_I, VGA_SEQ_CLOCK_MODE, 0, 0x20);
 
 	i740inb(par, 0x3DA);
-	i740outb(par, VGA_ATT_W, 0x20);	/* disable pallete access */
+	i740outb(par, VGA_ATT_W, 0x20);	/* disable palette access */
 }
 
 static int i740fb_set_par(struct fb_info *info)
diff --git a/drivers/video/mx3fb.c b/drivers/video/mx3fb.c
index eec0d7b748e..c89f8a8d36d 100644
--- a/drivers/video/mx3fb.c
+++ b/drivers/video/mx3fb.c
@@ -269,7 +269,7 @@ struct mx3fb_info {
 	dma_cookie_t			cookie;
 	struct scatterlist		sg[2];
 
-	u32				sync;	/* preserve var->sync flags */
+	struct fb_var_screeninfo	cur_var; /* current var info */
 };
 
 static void mx3fb_dma_done(void *);
@@ -698,9 +698,29 @@ static void mx3fb_dma_done(void *arg)
 	complete(&mx3_fbi->flip_cmpl);
 }
 
+static bool mx3fb_must_set_par(struct fb_info *fbi)
+{
+	struct mx3fb_info *mx3_fbi = fbi->par;
+	struct fb_var_screeninfo old_var = mx3_fbi->cur_var;
+	struct fb_var_screeninfo new_var = fbi->var;
+
+	if ((fbi->var.activate & FB_ACTIVATE_FORCE) &&
+	    (fbi->var.activate & FB_ACTIVATE_MASK) == FB_ACTIVATE_NOW)
+		return true;
+
+	/*
+	 * Ignore xoffset and yoffset update,
+	 * because pan display handles this case.
+	 */
+	old_var.xoffset = new_var.xoffset;
+	old_var.yoffset = new_var.yoffset;
+
+	return !!memcmp(&old_var, &new_var, sizeof(struct fb_var_screeninfo));
+}
+
 static int __set_par(struct fb_info *fbi, bool lock)
 {
-	u32 mem_len;
+	u32 mem_len, cur_xoffset, cur_yoffset;
 	struct ipu_di_signal_cfg sig_cfg;
 	enum ipu_panel mode = IPU_PANEL_TFT;
 	struct mx3fb_info *mx3_fbi = fbi->par;
@@ -780,8 +800,25 @@ static int __set_par(struct fb_info *fbi, bool lock)
 	video->out_height	= fbi->var.yres;
 	video->out_stride	= fbi->var.xres_virtual;
 
-	if (mx3_fbi->blank == FB_BLANK_UNBLANK)
+	if (mx3_fbi->blank == FB_BLANK_UNBLANK) {
 		sdc_enable_channel(mx3_fbi);
+		/*
+		 * sg[0] points to fb smem_start address
+		 * and is actually active in controller.
+		 */
+		mx3_fbi->cur_var.xoffset = 0;
+		mx3_fbi->cur_var.yoffset = 0;
+	}
+
+	/*
+	 * Preserve xoffset and yoffest in case they are
+	 * inactive in controller as fb is blanked.
+	 */
+	cur_xoffset = mx3_fbi->cur_var.xoffset;
+	cur_yoffset = mx3_fbi->cur_var.yoffset;
+	mx3_fbi->cur_var = fbi->var;
+	mx3_fbi->cur_var.xoffset = cur_xoffset;
+	mx3_fbi->cur_var.yoffset = cur_yoffset;
 
 	return 0;
 }
@@ -802,7 +839,7 @@ static int mx3fb_set_par(struct fb_info *fbi)
 
 	mutex_lock(&mx3_fbi->mutex);
 
-	ret = __set_par(fbi, true);
+	ret = mx3fb_must_set_par(fbi) ? __set_par(fbi, true) : 0;
 
 	mutex_unlock(&mx3_fbi->mutex);
 
@@ -901,8 +938,8 @@ static int mx3fb_check_var(struct fb_var_screeninfo *var, struct fb_info *fbi)
 	var->grayscale = 0;
 
 	/* Preserve sync flags */
-	var->sync |= mx3_fbi->sync;
-	mx3_fbi->sync |= var->sync;
+	var->sync |= mx3_fbi->cur_var.sync;
+	mx3_fbi->cur_var.sync |= var->sync;
 
 	return 0;
 }
@@ -1043,8 +1080,8 @@ static int mx3fb_pan_display(struct fb_var_screeninfo *var,
 		return -EINVAL;
 	}
 
-	if (fbi->var.xoffset == var->xoffset &&
-	    fbi->var.yoffset == var->yoffset)
+	if (mx3_fbi->cur_var.xoffset == var->xoffset &&
+	    mx3_fbi->cur_var.yoffset == var->yoffset)
 		return 0;	/* No change, do nothing */
 
 	y_bottom = var->yoffset;
@@ -1127,6 +1164,8 @@ static int mx3fb_pan_display(struct fb_var_screeninfo *var,
 	else
 		fbi->var.vmode &= ~FB_VMODE_YWRAP;
 
+	mx3_fbi->cur_var = fbi->var;
+
 	mutex_unlock(&mx3_fbi->mutex);
 
 	dev_dbg(fbi->device, "Update complete\n");
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
index abbe691047b..49619b44150 100644
--- a/drivers/video/mxsfb.c
+++ b/drivers/video/mxsfb.c
@@ -41,12 +41,14 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
 #include <linux/pinctrl/consumer.h>
-#include <mach/mxsfb.h>
+#include <linux/mxsfb.h>
 
 #define REG_SET	4
 #define REG_CLR	8
@@ -750,16 +752,43 @@ static void __devexit mxsfb_free_videomem(struct mxsfb_info *host)
 	}
 }
 
+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 const struct of_device_id mxsfb_dt_ids[] = {
+	{ .compatible = "fsl,imx23-lcdif", .data = &mxsfb_devtype[0], },
+	{ .compatible = "fsl,imx28-lcdif", .data = &mxsfb_devtype[1], },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mxsfb_dt_ids);
+
 static int __devinit mxsfb_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id =
+			of_match_device(mxsfb_dt_ids, &pdev->dev);
 	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;
 	struct pinctrl *pinctrl;
+	int panel_enable;
+	enum of_gpio_flags flags;
 	int i, ret;
 
+	if (of_id)
+		pdev->id_entry = of_id->data;
+
 	if (!pdata) {
 		dev_err(&pdev->dev, "No platformdata. Giving up\n");
 		return -ENODEV;
@@ -807,6 +836,22 @@ static int __devinit mxsfb_probe(struct platform_device *pdev)
 		goto error_getclock;
 	}
 
+	panel_enable = of_get_named_gpio_flags(pdev->dev.of_node,
+					       "panel-enable-gpios", 0, &flags);
+	if (gpio_is_valid(panel_enable)) {
+		unsigned long f = GPIOF_OUT_INIT_HIGH;
+		if (flags == OF_GPIO_ACTIVE_LOW)
+			f = GPIOF_OUT_INIT_LOW;
+		ret = devm_gpio_request_one(&pdev->dev, panel_enable,
+					    f, "panel-enable");
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed to request gpio %d: %d\n",
+				panel_enable, ret);
+			goto error_panel_enable;
+		}
+	}
+
 	fb_info->pseudo_palette = kmalloc(sizeof(u32) * 16, GFP_KERNEL);
 	if (!fb_info->pseudo_palette) {
 		ret = -ENOMEM;
@@ -854,6 +899,7 @@ error_register:
 error_init_fb:
 	kfree(fb_info->pseudo_palette);
 error_pseudo_pallette:
+error_panel_enable:
 	clk_put(host->clk);
 error_getclock:
 error_getpin:
@@ -901,19 +947,6 @@ static void mxsfb_shutdown(struct platform_device *pdev)
 	writel(CTRL_RUN, host->base + LCDC_CTRL + REG_CLR);
 }
 
-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),
@@ -921,6 +954,7 @@ static struct platform_driver mxsfb_driver = {
 	.id_table = mxsfb_devtype,
 	.driver = {
 		   .name = DRIVER_NAME,
+		   .of_match_table = mxsfb_dt_ids,
 	},
 };
 
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index ad741c3d1ae..eaeed4340e0 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -487,6 +487,13 @@ static struct omap_video_timings acx_panel_timings = {
 	.vfp		= 3,
 	.vsw		= 3,
 	.vbp		= 4,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
 static int acx_panel_probe(struct omap_dss_device *dssdev)
@@ -498,8 +505,7 @@ static int acx_panel_probe(struct omap_dss_device *dssdev)
 	struct backlight_properties props;
 
 	dev_dbg(&dssdev->dev, "%s\n", __func__);
-	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					OMAP_DSS_LCD_IHS;
+
 	/* FIXME AC bias ? */
 	dssdev->panel.timings = acx_panel_timings;
 
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index e42f9dc2212..bc5af2500eb 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -40,12 +40,6 @@
 struct panel_config {
 	struct omap_video_timings timings;
 
-	int acbi;	/* ac-bias pin transitions per interrupt */
-	/* Unit: line clocks */
-	int acb;	/* ac-bias pin frequency */
-
-	enum omap_panel_config config;
-
 	int power_on_delay;
 	int power_off_delay;
 
@@ -73,11 +67,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 11,
 			.vfp		= 3,
 			.vbp		= 2,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.acbi			= 0x0,
-		.acb			= 0x0,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO,
 		.power_on_delay		= 50,
 		.power_off_delay	= 100,
 		.name			= "sharp_lq",
@@ -98,11 +94,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 1,
 			.vfp		= 1,
 			.vbp		= 1,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.acbi			= 0x0,
-		.acb			= 0x28,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-						OMAP_DSS_LCD_IHS,
 		.power_on_delay		= 50,
 		.power_off_delay	= 100,
 		.name			= "sharp_ls",
@@ -123,12 +121,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vfp		= 4,
 			.vsw		= 2,
 			.vbp		= 2,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
 		},
-		.acbi			= 0x0,
-		.acb			= 0x0,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IPC |
-					OMAP_DSS_LCD_ONOFF,
 		.power_on_delay		= 0,
 		.power_off_delay	= 0,
 		.name			= "toppoly_tdo35s",
@@ -149,11 +148,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vfp		= 4,
 			.vsw		= 10,
 			.vbp		= 12 - 10,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.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",
@@ -174,11 +175,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 2,
 			.vfp		= 4,
 			.vbp		= 11,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.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			= "seiko_70wvw1tz3",
@@ -199,11 +202,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 10,
 			.vfp		= 2,
 			.vbp		= 2,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.acbi			= 0x0,
-		.acb			= 0x0,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO,
 		.power_on_delay		= 0,
 		.power_off_delay	= 0,
 		.name			= "powertip_ph480272t",
@@ -224,11 +229,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 3,
 			.vfp		= 12,
 			.vbp		= 25,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.acbi			= 0x0,
-		.acb			= 0x28,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS,
 		.power_on_delay		= 0,
 		.power_off_delay	= 0,
 		.name			= "innolux_at070tn83",
@@ -249,9 +256,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 1,
 			.vfp		= 2,
 			.vbp		= 7,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-						OMAP_DSS_LCD_IHS,
 		.name			= "nec_nl2432dr22-11b",
 	},
 
@@ -270,9 +281,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 1,
 			.vfp		= 1,
 			.vbp		= 1,
-		},
-		.config			= OMAP_DSS_LCD_TFT,
 
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
+		},
 		.name			= "h4",
 	},
 
@@ -291,10 +306,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 10,
 			.vfp		= 2,
 			.vbp		= 2,
-		},
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-						OMAP_DSS_LCD_IHS,
 
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
+		},
 		.name			= "apollon",
 	},
 	/* FocalTech ETM070003DH6 */
@@ -312,9 +330,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 3,
 			.vfp		= 13,
 			.vbp		= 29,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS,
 		.name			= "focaltech_etm070003dh6",
 	},
 
@@ -333,11 +355,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 23,
 			.vfp		= 1,
 			.vbp		= 1,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.acbi			= 0x0,
-		.acb			= 0x0,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IPC,
 		.power_on_delay		= 0,
 		.power_off_delay	= 0,
 		.name			= "microtips_umsh_8173md",
@@ -358,9 +382,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 10,
 			.vfp		= 4,
 			.vbp		= 2,
-		},
-		.config			= OMAP_DSS_LCD_TFT,
 
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
+		},
 		.name			= "ortustech_com43h4m10xtc",
 	},
 
@@ -379,11 +407,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 10,
 			.vfp		= 12,
 			.vbp		= 23,
-		},
-		.acb			= 0x0,
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IEO,
 
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
+		},
 		.name			= "innolux_at080tn52",
 	},
 
@@ -401,8 +431,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 1,
 			.vfp		= 26,
 			.vbp		= 1,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT,
 		.name			= "mitsubishi_aa084sb01",
 	},
 	/* EDT ET0500G0DH6 */
@@ -419,8 +454,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 2,
 			.vfp		= 35,
 			.vbp		= 10,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT,
 		.name			= "edt_et0500g0dh6",
 	},
 
@@ -439,9 +479,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 2,
 			.vfp		= 10,
 			.vbp		= 33,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IPC,
 		.name			= "primeview_pd050vl1",
 	},
 
@@ -460,9 +504,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 2,
 			.vfp		= 10,
 			.vbp		= 33,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IPC,
 		.name			= "primeview_pm070wl4",
 	},
 
@@ -481,9 +529,13 @@ static struct panel_config generic_dpi_panels[] = {
 			.vsw		= 4,
 			.vfp		= 1,
 			.vbp		= 23,
+
+			.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+			.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+			.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+			.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 		},
-		.config			= OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-					  OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IPC,
 		.name			= "primeview_pd104slf",
 	},
 };
@@ -573,10 +625,7 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
 	if (!panel_config)
 		return -EINVAL;
 
-	dssdev->panel.config = panel_config->config;
 	dssdev->panel.timings = panel_config->timings;
-	dssdev->panel.acb = panel_config->acb;
-	dssdev->panel.acbi = panel_config->acbi;
 
 	drv_data = kzalloc(sizeof(*drv_data), GFP_KERNEL);
 	if (!drv_data)
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
index 0841cc2b3f7..80280779884 100644
--- a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -40,6 +40,12 @@ static struct omap_video_timings lb035q02_timings = {
 	.vsw		= 2,
 	.vfp		= 4,
 	.vbp		= 18,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
 static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
@@ -82,8 +88,6 @@ 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);
diff --git a/drivers/video/omap2/displays/panel-n8x0.c b/drivers/video/omap2/displays/panel-n8x0.c
index 4a34cdc1371..e6c115373c0 100644
--- a/drivers/video/omap2/displays/panel-n8x0.c
+++ b/drivers/video/omap2/displays/panel-n8x0.c
@@ -473,7 +473,6 @@ static int n8x0_panel_probe(struct omap_dss_device *dssdev)
 
 	mutex_init(&ddata->lock);
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT;
 	dssdev->panel.timings.x_res = 800;
 	dssdev->panel.timings.y_res = 480;
 	dssdev->ctrl.pixel_size = 16;
diff --git a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
index 8b38b39213f..b122b0f31c4 100644
--- a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
+++ b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
@@ -76,6 +76,12 @@ static struct omap_video_timings nec_8048_panel_timings = {
 	.vfp		= 3,
 	.vsw		= 1,
 	.vbp		= 4,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
 };
 
 static int nec_8048_bl_update_status(struct backlight_device *bl)
@@ -116,9 +122,6 @@ static int nec_8048_panel_probe(struct omap_dss_device *dssdev)
 	struct backlight_properties props;
 	int r;
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-				OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_RF |
-				OMAP_DSS_LCD_ONOFF;
 	dssdev->panel.timings = nec_8048_panel_timings;
 
 	necd = kzalloc(sizeof(*necd), GFP_KERNEL);
diff --git a/drivers/video/omap2/displays/panel-picodlp.c b/drivers/video/omap2/displays/panel-picodlp.c
index 98ebdaddab5..2d35bd38886 100644
--- a/drivers/video/omap2/displays/panel-picodlp.c
+++ b/drivers/video/omap2/displays/panel-picodlp.c
@@ -69,6 +69,12 @@ static struct omap_video_timings pico_ls_timings = {
 	.vsw		= 2,
 	.vfp		= 3,
 	.vbp		= 14,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
 };
 
 static inline struct picodlp_panel_data
@@ -414,9 +420,6 @@ static int picodlp_panel_probe(struct omap_dss_device *dssdev)
 	struct i2c_client *picodlp_i2c_client;
 	int r = 0, picodlp_adapter_id;
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_ONOFF |
-				OMAP_DSS_LCD_IHS | OMAP_DSS_LCD_IVS;
-	dssdev->panel.acb = 0x0;
 	dssdev->panel.timings = pico_ls_timings;
 
 	picod =  kzalloc(sizeof(struct picodlp_data), GFP_KERNEL);
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index ba38b3ad17d..bd86ba9ccf7 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -44,6 +44,12 @@ static struct omap_video_timings sharp_ls_timings = {
 	.vsw		= 1,
 	.vfp		= 1,
 	.vbp		= 1,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
 static int sharp_ls_bl_update_status(struct backlight_device *bl)
@@ -86,9 +92,6 @@ static int sharp_ls_panel_probe(struct omap_dss_device *dssdev)
 	struct sharp_data *sd;
 	int r;
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
-		OMAP_DSS_LCD_IHS;
-	dssdev->panel.acb = 0x28;
 	dssdev->panel.timings = sharp_ls_timings;
 
 	sd = kzalloc(sizeof(*sd), GFP_KERNEL);
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index 901576eb5a8..3f5acc7771d 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -882,7 +882,6 @@ static int taal_probe(struct omap_dss_device *dssdev)
 		goto err;
 	}
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT;
 	dssdev->panel.timings = panel_config->timings;
 	dssdev->panel.dsi_pix_fmt = OMAP_DSS_DSI_FMT_RGB888;
 
diff --git a/drivers/video/omap2/displays/panel-tfp410.c b/drivers/video/omap2/displays/panel-tfp410.c
index bff306e041c..40cc0cfa5d1 100644
--- a/drivers/video/omap2/displays/panel-tfp410.c
+++ b/drivers/video/omap2/displays/panel-tfp410.c
@@ -39,6 +39,12 @@ static const struct omap_video_timings tfp410_default_timings = {
 	.vfp		= 3,
 	.vsw		= 4,
 	.vbp		= 7,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
 struct panel_drv_data {
@@ -95,7 +101,6 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
 		return -ENOMEM;
 
 	dssdev->panel.timings = tfp410_default_timings;
-	dssdev->panel.config = OMAP_DSS_LCD_TFT;
 
 	ddata->dssdev = dssdev;
 	mutex_init(&ddata->lock);
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index 4b6448b3c31..fa7baa650ae 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -267,6 +267,12 @@ static const struct omap_video_timings tpo_td043_timings = {
 	.vsw		= 1,
 	.vfp		= 39,
 	.vbp		= 34,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES,
 };
 
 static int tpo_td043_power_on(struct tpo_td043_device *tpo_td043)
@@ -423,8 +429,6 @@ static int tpo_td043_probe(struct omap_dss_device *dssdev)
 		return -ENODEV;
 	}
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IHS |
-				OMAP_DSS_LCD_IVS | OMAP_DSS_LCD_IPC;
 	dssdev->panel.timings = tpo_td043_timings;
 	dssdev->ctrl.pixel_size = 24;
 
diff --git a/drivers/video/omap2/dss/Kconfig b/drivers/video/omap2/dss/Kconfig
index 43324e5ed25..b337a8469fd 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -52,7 +52,7 @@ config OMAP2_DSS_RFBI
 	  DBI is a bus between the host processor and a peripheral,
 	  such as a display or a framebuffer chip.
 
-	  See http://www.mipi.org/ for DBI spesifications.
+	  See http://www.mipi.org/ for DBI specifications.
 
 config OMAP2_DSS_VENC
 	bool "VENC support"
@@ -92,7 +92,7 @@ config OMAP2_DSS_DSI
 	  DSI is a high speed half-duplex serial interface between the host
 	  processor and a peripheral, such as a display or a framebuffer chip.
 
-	  See http://www.mipi.org/ for DSI spesifications.
+	  See http://www.mipi.org/ for DSI specifications.
 
 config OMAP2_DSS_MIN_FCK_PER_PCK
 	int "Minimum FCK/PCK ratio (for scaling)"
diff --git a/drivers/video/omap2/dss/apply.c b/drivers/video/omap2/dss/apply.c
index ab22cc224f3..0fefc68372b 100644
--- a/drivers/video/omap2/dss/apply.c
+++ b/drivers/video/omap2/dss/apply.c
@@ -104,6 +104,7 @@ struct mgr_priv_data {
 	bool shadow_extra_info_dirty;
 
 	struct omap_video_timings timings;
+	struct dss_lcd_mgr_config lcd_config;
 };
 
 static struct {
@@ -137,6 +138,7 @@ static struct mgr_priv_data *get_mgr_priv(struct omap_overlay_manager *mgr)
 void dss_apply_init(void)
 {
 	const int num_ovls = dss_feat_get_num_ovls();
+	struct mgr_priv_data *mp;
 	int i;
 
 	spin_lock_init(&data_lock);
@@ -168,16 +170,35 @@ void dss_apply_init(void)
 
 		op->user_info = op->info;
 	}
+
+	/*
+	 * Initialize some of the lcd_config fields for TV manager, this lets
+	 * us prevent checking if the manager is LCD or TV at some places
+	 */
+	mp = &dss_data.mgr_priv_data_array[OMAP_DSS_CHANNEL_DIGIT];
+
+	mp->lcd_config.video_port_width = 24;
+	mp->lcd_config.clock_info.lck_div = 1;
+	mp->lcd_config.clock_info.pck_div = 1;
 }
 
+/*
+ * A LCD manager's stallmode decides whether it is in manual or auto update. TV
+ * manager is always auto update, stallmode field for TV manager is false by
+ * default
+ */
 static bool ovl_manual_update(struct omap_overlay *ovl)
 {
-	return ovl->manager->device->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
+	struct mgr_priv_data *mp = get_mgr_priv(ovl->manager);
+
+	return mp->lcd_config.stallmode;
 }
 
 static bool mgr_manual_update(struct omap_overlay_manager *mgr)
 {
-	return mgr->device->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	return mp->lcd_config.stallmode;
 }
 
 static int dss_check_settings_low(struct omap_overlay_manager *mgr,
@@ -214,7 +235,7 @@ static int dss_check_settings_low(struct omap_overlay_manager *mgr,
 		ois[ovl->id] = oi;
 	}
 
-	return dss_mgr_check(mgr, mi, &mp->timings, ois);
+	return dss_mgr_check(mgr, mi, &mp->timings, &mp->lcd_config, ois);
 }
 
 /*
@@ -537,7 +558,7 @@ static void dss_ovl_write_regs(struct omap_overlay *ovl)
 {
 	struct ovl_priv_data *op = get_ovl_priv(ovl);
 	struct omap_overlay_info *oi;
-	bool ilace, replication;
+	bool replication;
 	struct mgr_priv_data *mp;
 	int r;
 
@@ -550,11 +571,9 @@ static void dss_ovl_write_regs(struct omap_overlay *ovl)
 
 	mp = get_mgr_priv(ovl->manager);
 
-	replication = dss_use_replication(ovl->manager->device, oi->color_mode);
-
-	ilace = ovl->manager->device->type == OMAP_DISPLAY_TYPE_VENC;
+	replication = dss_ovl_use_replication(mp->lcd_config, oi->color_mode);
 
-	r = dispc_ovl_setup(ovl->id, oi, ilace, replication, &mp->timings);
+	r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings);
 	if (r) {
 		/*
 		 * We can't do much here, as this function can be called from
@@ -635,6 +654,24 @@ static void dss_mgr_write_regs_extra(struct omap_overlay_manager *mgr)
 
 	dispc_mgr_set_timings(mgr->id, &mp->timings);
 
+	/* lcd_config parameters */
+	if (dss_mgr_is_lcd(mgr->id)) {
+		dispc_mgr_set_io_pad_mode(mp->lcd_config.io_pad_mode);
+
+		dispc_mgr_enable_stallmode(mgr->id, mp->lcd_config.stallmode);
+		dispc_mgr_enable_fifohandcheck(mgr->id,
+			mp->lcd_config.fifohandcheck);
+
+		dispc_mgr_set_clock_div(mgr->id, &mp->lcd_config.clock_info);
+
+		dispc_mgr_set_tft_data_lines(mgr->id,
+			mp->lcd_config.video_port_width);
+
+		dispc_lcd_enable_signal_polarity(mp->lcd_config.lcden_sig_polarity);
+
+		dispc_mgr_set_lcd_type_tft(mgr->id);
+	}
+
 	mp->extra_info_dirty = false;
 	if (mp->updating)
 		mp->shadow_extra_info_dirty = true;
@@ -1294,6 +1331,44 @@ void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
 	mutex_unlock(&apply_lock);
 }
 
+static void dss_apply_mgr_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	mp->lcd_config = *config;
+	mp->extra_info_dirty = true;
+}
+
+void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	unsigned long flags;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	mutex_lock(&apply_lock);
+
+	if (mp->enabled) {
+		DSSERR("cannot apply lcd config for %s: manager needs to be disabled\n",
+			mgr->name);
+		goto out;
+	}
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	dss_apply_mgr_lcd_config(mgr, config);
+
+	dss_write_regs();
+	dss_set_go_bits();
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	wait_pending_extra_info_updates();
+
+out:
+	mutex_unlock(&apply_lock);
+}
+
 int dss_ovl_set_info(struct omap_overlay *ovl,
 		struct omap_overlay_info *info)
 {
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index 397d4eee11b..5b289c5f695 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -119,6 +119,97 @@ enum omap_color_component {
 	DISPC_COLOR_COMPONENT_UV		= 1 << 1,
 };
 
+enum mgr_reg_fields {
+	DISPC_MGR_FLD_ENABLE,
+	DISPC_MGR_FLD_STNTFT,
+	DISPC_MGR_FLD_GO,
+	DISPC_MGR_FLD_TFTDATALINES,
+	DISPC_MGR_FLD_STALLMODE,
+	DISPC_MGR_FLD_TCKENABLE,
+	DISPC_MGR_FLD_TCKSELECTION,
+	DISPC_MGR_FLD_CPR,
+	DISPC_MGR_FLD_FIFOHANDCHECK,
+	/* used to maintain a count of the above fields */
+	DISPC_MGR_FLD_NUM,
+};
+
+static const struct {
+	const char *name;
+	u32 vsync_irq;
+	u32 framedone_irq;
+	u32 sync_lost_irq;
+	struct reg_field reg_desc[DISPC_MGR_FLD_NUM];
+} mgr_desc[] = {
+	[OMAP_DSS_CHANNEL_LCD] = {
+		.name		= "LCD",
+		.vsync_irq	= DISPC_IRQ_VSYNC,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONE,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL,  0,  0 },
+			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL,  3,  3 },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL,  5,  5 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { DISPC_CONTROL,  9,  8 },
+			[DISPC_MGR_FLD_STALLMODE]	= { DISPC_CONTROL, 11, 11 },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG,  10, 10 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG,  11, 11 },
+			[DISPC_MGR_FLD_CPR]		= { DISPC_CONFIG,  15, 15 },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG,  16, 16 },
+		},
+	},
+	[OMAP_DSS_CHANNEL_DIGIT] = {
+		.name		= "DIGIT",
+		.vsync_irq	= DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN,
+		.framedone_irq	= 0,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST_DIGIT,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL,  1,  1 },
+			[DISPC_MGR_FLD_STNTFT]		= { },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL,  6,  6 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { },
+			[DISPC_MGR_FLD_STALLMODE]	= { },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG,  12, 12 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG,  13, 13 },
+			[DISPC_MGR_FLD_CPR]		= { },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG,  16, 16 },
+		},
+	},
+	[OMAP_DSS_CHANNEL_LCD2] = {
+		.name		= "LCD2",
+		.vsync_irq	= DISPC_IRQ_VSYNC2,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONE2,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST2,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL2,  0,  0 },
+			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL2,  3,  3 },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL2,  5,  5 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { DISPC_CONTROL2,  9,  8 },
+			[DISPC_MGR_FLD_STALLMODE]	= { DISPC_CONTROL2, 11, 11 },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG2,  10, 10 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG2,  11, 11 },
+			[DISPC_MGR_FLD_CPR]		= { DISPC_CONFIG2,  15, 15 },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG2,  16, 16 },
+		},
+	},
+	[OMAP_DSS_CHANNEL_LCD3] = {
+		.name		= "LCD3",
+		.vsync_irq	= DISPC_IRQ_VSYNC3,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONE3,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST3,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL3,  0,  0 },
+			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL3,  3,  3 },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL3,  5,  5 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { DISPC_CONTROL3,  9,  8 },
+			[DISPC_MGR_FLD_STALLMODE]	= { DISPC_CONTROL3, 11, 11 },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG3,  10, 10 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG3,  11, 11 },
+			[DISPC_MGR_FLD_CPR]		= { DISPC_CONFIG3,  15, 15 },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG3,  16, 16 },
+		},
+	},
+};
+
 static void _omap_dispc_set_irqs(void);
 
 static inline void dispc_write_reg(const u16 idx, u32 val)
@@ -131,6 +222,18 @@ static inline u32 dispc_read_reg(const u16 idx)
 	return __raw_readl(dispc.base + idx);
 }
 
+static u32 mgr_fld_read(enum omap_channel channel, enum mgr_reg_fields regfld)
+{
+	const struct reg_field rfld = mgr_desc[channel].reg_desc[regfld];
+	return REG_GET(rfld.reg, rfld.high, rfld.low);
+}
+
+static void mgr_fld_write(enum omap_channel channel,
+					enum mgr_reg_fields regfld, int val) {
+	const struct reg_field rfld = mgr_desc[channel].reg_desc[regfld];
+	REG_FLD_MOD(rfld.reg, val, rfld.high, rfld.low);
+}
+
 #define SR(reg) \
 	dispc.ctx[DISPC_##reg / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
 #define RR(reg) \
@@ -153,6 +256,10 @@ static void dispc_save_context(void)
 		SR(CONTROL2);
 		SR(CONFIG2);
 	}
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		SR(CONTROL3);
+		SR(CONFIG3);
+	}
 
 	for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
 		SR(DEFAULT_COLOR(i));
@@ -266,6 +373,8 @@ static void dispc_restore_context(void)
 		RR(GLOBAL_ALPHA);
 	if (dss_has_feature(FEAT_MGR_LCD2))
 		RR(CONFIG2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		RR(CONFIG3);
 
 	for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
 		RR(DEFAULT_COLOR(i));
@@ -351,6 +460,8 @@ static void dispc_restore_context(void)
 	RR(CONTROL);
 	if (dss_has_feature(FEAT_MGR_LCD2))
 		RR(CONTROL2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		RR(CONTROL3);
 	/* clear spurious SYNC_LOST_DIGIT interrupts */
 	dispc_write_reg(DISPC_IRQSTATUS, DISPC_IRQ_SYNC_LOST_DIGIT);
 
@@ -387,101 +498,41 @@ void dispc_runtime_put(void)
 	WARN_ON(r < 0 && r != -ENOSYS);
 }
 
-static inline bool dispc_mgr_is_lcd(enum omap_channel channel)
-{
-	if (channel == OMAP_DSS_CHANNEL_LCD ||
-			channel == OMAP_DSS_CHANNEL_LCD2)
-		return true;
-	else
-		return false;
-}
-
 u32 dispc_mgr_get_vsync_irq(enum omap_channel channel)
 {
-	switch (channel) {
-	case OMAP_DSS_CHANNEL_LCD:
-		return DISPC_IRQ_VSYNC;
-	case OMAP_DSS_CHANNEL_LCD2:
-		return DISPC_IRQ_VSYNC2;
-	case OMAP_DSS_CHANNEL_DIGIT:
-		return DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
-	default:
-		BUG();
-		return 0;
-	}
+	return mgr_desc[channel].vsync_irq;
 }
 
 u32 dispc_mgr_get_framedone_irq(enum omap_channel channel)
 {
-	switch (channel) {
-	case OMAP_DSS_CHANNEL_LCD:
-		return DISPC_IRQ_FRAMEDONE;
-	case OMAP_DSS_CHANNEL_LCD2:
-		return DISPC_IRQ_FRAMEDONE2;
-	case OMAP_DSS_CHANNEL_DIGIT:
-		return 0;
-	default:
-		BUG();
-		return 0;
-	}
+	return mgr_desc[channel].framedone_irq;
 }
 
 bool dispc_mgr_go_busy(enum omap_channel channel)
 {
-	int bit;
-
-	if (dispc_mgr_is_lcd(channel))
-		bit = 5; /* GOLCD */
-	else
-		bit = 6; /* GODIGIT */
-
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		return REG_GET(DISPC_CONTROL2, bit, bit) == 1;
-	else
-		return REG_GET(DISPC_CONTROL, bit, bit) == 1;
+	return mgr_fld_read(channel, DISPC_MGR_FLD_GO) == 1;
 }
 
 void dispc_mgr_go(enum omap_channel channel)
 {
-	int bit;
 	bool enable_bit, go_bit;
 
-	if (dispc_mgr_is_lcd(channel))
-		bit = 0; /* LCDENABLE */
-	else
-		bit = 1; /* DIGITALENABLE */
-
 	/* if the channel is not enabled, we don't need GO */
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		enable_bit = REG_GET(DISPC_CONTROL2, bit, bit) == 1;
-	else
-		enable_bit = REG_GET(DISPC_CONTROL, bit, bit) == 1;
+	enable_bit = mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE) == 1;
 
 	if (!enable_bit)
 		return;
 
-	if (dispc_mgr_is_lcd(channel))
-		bit = 5; /* GOLCD */
-	else
-		bit = 6; /* GODIGIT */
-
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		go_bit = REG_GET(DISPC_CONTROL2, bit, bit) == 1;
-	else
-		go_bit = REG_GET(DISPC_CONTROL, bit, bit) == 1;
+	go_bit = mgr_fld_read(channel, DISPC_MGR_FLD_GO) == 1;
 
 	if (go_bit) {
 		DSSERR("GO bit not down for channel %d\n", channel);
 		return;
 	}
 
-	DSSDBG("GO %s\n", channel == OMAP_DSS_CHANNEL_LCD ? "LCD" :
-		(channel == OMAP_DSS_CHANNEL_LCD2 ? "LCD2" : "DIGIT"));
+	DSSDBG("GO %s\n", mgr_desc[channel].name);
 
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		REG_FLD_MOD(DISPC_CONTROL2, 1, bit, bit);
-	else
-		REG_FLD_MOD(DISPC_CONTROL, 1, bit, bit);
+	mgr_fld_write(channel, DISPC_MGR_FLD_GO, 1);
 }
 
 static void dispc_ovl_write_firh_reg(enum omap_plane plane, int reg, u32 value)
@@ -832,6 +883,15 @@ void dispc_ovl_set_channel_out(enum omap_plane plane, enum omap_channel channel)
 			chan = 0;
 			chan2 = 1;
 			break;
+		case OMAP_DSS_CHANNEL_LCD3:
+			if (dss_has_feature(FEAT_MGR_LCD3)) {
+				chan = 0;
+				chan2 = 2;
+			} else {
+				BUG();
+				return;
+			}
+			break;
 		default:
 			BUG();
 			return;
@@ -867,7 +927,14 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
 
 	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 
-	if (dss_has_feature(FEAT_MGR_LCD2)) {
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		if (FLD_GET(val, 31, 30) == 0)
+			channel = FLD_GET(val, shift, shift);
+		else if (FLD_GET(val, 31, 30) == 1)
+			channel = OMAP_DSS_CHANNEL_LCD2;
+		else
+			channel = OMAP_DSS_CHANNEL_LCD3;
+	} else if (dss_has_feature(FEAT_MGR_LCD2)) {
 		if (FLD_GET(val, 31, 30) == 0)
 			channel = FLD_GET(val, shift, shift);
 		else
@@ -922,16 +989,10 @@ void dispc_enable_gamma_table(bool enable)
 
 static void dispc_mgr_enable_cpr(enum omap_channel channel, bool enable)
 {
-	u16 reg;
-
-	if (channel == OMAP_DSS_CHANNEL_LCD)
-		reg = DISPC_CONFIG;
-	else if (channel == OMAP_DSS_CHANNEL_LCD2)
-		reg = DISPC_CONFIG2;
-	else
+	if (channel == OMAP_DSS_CHANNEL_DIGIT)
 		return;
 
-	REG_FLD_MOD(reg, enable, 15, 15);
+	mgr_fld_write(channel, DISPC_MGR_FLD_CPR, enable);
 }
 
 static void dispc_mgr_set_cpr_coef(enum omap_channel channel,
@@ -939,7 +1000,7 @@ static void dispc_mgr_set_cpr_coef(enum omap_channel channel,
 {
 	u32 coef_r, coef_g, coef_b;
 
-	if (!dispc_mgr_is_lcd(channel))
+	if (!dss_mgr_is_lcd(channel))
 		return;
 
 	coef_r = FLD_VAL(coefs->rr, 31, 22) | FLD_VAL(coefs->rg, 20, 11) |
@@ -1798,7 +1859,7 @@ static int check_horiz_timing_omap3(enum omap_channel channel,
 
 	nonactive = t->x_res + t->hfp + t->hsw + t->hbp - out_width;
 	pclk = dispc_mgr_pclk_rate(channel);
-	if (dispc_mgr_is_lcd(channel))
+	if (dss_mgr_is_lcd(channel))
 		lclk = dispc_mgr_lclk_rate(channel);
 	else
 		lclk = dispc_fclk_rate();
@@ -2086,8 +2147,7 @@ static int dispc_ovl_calc_scaling(enum omap_plane plane,
 }
 
 int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
-		bool ilace, bool replication,
-		const struct omap_video_timings *mgr_timings)
+		bool replication, const struct omap_video_timings *mgr_timings)
 {
 	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
 	bool five_taps = true;
@@ -2103,6 +2163,7 @@ int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
 	u16 out_width, out_height;
 	enum omap_channel channel;
 	int x_predecim = 1, y_predecim = 1;
+	bool ilace = mgr_timings->interlace;
 
 	channel = dispc_ovl_get_channel_out(plane);
 
@@ -2254,14 +2315,9 @@ static void dispc_disable_isr(void *data, u32 mask)
 
 static void _enable_lcd_out(enum omap_channel channel, bool enable)
 {
-	if (channel == OMAP_DSS_CHANNEL_LCD2) {
-		REG_FLD_MOD(DISPC_CONTROL2, enable ? 1 : 0, 0, 0);
-		/* flush posted write */
-		dispc_read_reg(DISPC_CONTROL2);
-	} else {
-		REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 0, 0);
-		dispc_read_reg(DISPC_CONTROL);
-	}
+	mgr_fld_write(channel, DISPC_MGR_FLD_ENABLE, enable);
+	/* flush posted write */
+	mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
 }
 
 static void dispc_mgr_enable_lcd_out(enum omap_channel channel, bool enable)
@@ -2274,12 +2330,9 @@ static void dispc_mgr_enable_lcd_out(enum omap_channel channel, bool enable)
 	/* When we disable LCD output, we need to wait until frame is done.
 	 * Otherwise the DSS is still working, and turning off the clocks
 	 * prevents DSS from going to OFF mode */
-	is_on = channel == OMAP_DSS_CHANNEL_LCD2 ?
-			REG_GET(DISPC_CONTROL2, 0, 0) :
-			REG_GET(DISPC_CONTROL, 0, 0);
+	is_on = mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
 
-	irq = channel == OMAP_DSS_CHANNEL_LCD2 ? DISPC_IRQ_FRAMEDONE2 :
-			DISPC_IRQ_FRAMEDONE;
+	irq = mgr_desc[channel].framedone_irq;
 
 	if (!enable && is_on) {
 		init_completion(&frame_done_completion);
@@ -2384,21 +2437,12 @@ static void dispc_mgr_enable_digit_out(bool enable)
 
 bool dispc_mgr_is_enabled(enum omap_channel channel)
 {
-	if (channel == OMAP_DSS_CHANNEL_LCD)
-		return !!REG_GET(DISPC_CONTROL, 0, 0);
-	else if (channel == OMAP_DSS_CHANNEL_DIGIT)
-		return !!REG_GET(DISPC_CONTROL, 1, 1);
-	else if (channel == OMAP_DSS_CHANNEL_LCD2)
-		return !!REG_GET(DISPC_CONTROL2, 0, 0);
-	else {
-		BUG();
-		return false;
-	}
+	return !!mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
 }
 
 void dispc_mgr_enable(enum omap_channel channel, bool enable)
 {
-	if (dispc_mgr_is_lcd(channel))
+	if (dss_mgr_is_lcd(channel))
 		dispc_mgr_enable_lcd_out(channel, enable);
 	else if (channel == OMAP_DSS_CHANNEL_DIGIT)
 		dispc_mgr_enable_digit_out(enable);
@@ -2432,36 +2476,13 @@ void dispc_pck_free_enable(bool enable)
 
 void dispc_mgr_enable_fifohandcheck(enum omap_channel channel, bool enable)
 {
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		REG_FLD_MOD(DISPC_CONFIG2, enable ? 1 : 0, 16, 16);
-	else
-		REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 16, 16);
+	mgr_fld_write(channel, DISPC_MGR_FLD_FIFOHANDCHECK, enable);
 }
 
 
-void dispc_mgr_set_lcd_display_type(enum omap_channel channel,
-		enum omap_lcd_display_type type)
+void dispc_mgr_set_lcd_type_tft(enum omap_channel channel)
 {
-	int mode;
-
-	switch (type) {
-	case OMAP_DSS_LCD_DISPLAY_STN:
-		mode = 0;
-		break;
-
-	case OMAP_DSS_LCD_DISPLAY_TFT:
-		mode = 1;
-		break;
-
-	default:
-		BUG();
-		return;
-	}
-
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		REG_FLD_MOD(DISPC_CONTROL2, mode, 3, 3);
-	else
-		REG_FLD_MOD(DISPC_CONTROL, mode, 3, 3);
+	mgr_fld_write(channel, DISPC_MGR_FLD_STNTFT, 1);
 }
 
 void dispc_set_loadmode(enum omap_dss_load_mode mode)
@@ -2479,24 +2500,14 @@ static void dispc_mgr_set_trans_key(enum omap_channel ch,
 		enum omap_dss_trans_key_type type,
 		u32 trans_key)
 {
-	if (ch == OMAP_DSS_CHANNEL_LCD)
-		REG_FLD_MOD(DISPC_CONFIG, type, 11, 11);
-	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
-		REG_FLD_MOD(DISPC_CONFIG, type, 13, 13);
-	else /* OMAP_DSS_CHANNEL_LCD2 */
-		REG_FLD_MOD(DISPC_CONFIG2, type, 11, 11);
+	mgr_fld_write(ch, DISPC_MGR_FLD_TCKSELECTION, type);
 
 	dispc_write_reg(DISPC_TRANS_COLOR(ch), trans_key);
 }
 
 static void dispc_mgr_enable_trans_key(enum omap_channel ch, bool enable)
 {
-	if (ch == OMAP_DSS_CHANNEL_LCD)
-		REG_FLD_MOD(DISPC_CONFIG, enable, 10, 10);
-	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
-		REG_FLD_MOD(DISPC_CONFIG, enable, 12, 12);
-	else /* OMAP_DSS_CHANNEL_LCD2 */
-		REG_FLD_MOD(DISPC_CONFIG2, enable, 10, 10);
+	mgr_fld_write(ch, DISPC_MGR_FLD_TCKENABLE, enable);
 }
 
 static void dispc_mgr_enable_alpha_fixed_zorder(enum omap_channel ch,
@@ -2547,10 +2558,7 @@ void dispc_mgr_set_tft_data_lines(enum omap_channel channel, u8 data_lines)
 		return;
 	}
 
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		REG_FLD_MOD(DISPC_CONTROL2, code, 9, 8);
-	else
-		REG_FLD_MOD(DISPC_CONTROL, code, 9, 8);
+	mgr_fld_write(channel, DISPC_MGR_FLD_TFTDATALINES, code);
 }
 
 void dispc_mgr_set_io_pad_mode(enum dss_io_pad_mode mode)
@@ -2584,10 +2592,7 @@ void dispc_mgr_set_io_pad_mode(enum dss_io_pad_mode mode)
 
 void dispc_mgr_enable_stallmode(enum omap_channel channel, bool enable)
 {
-	if (channel == OMAP_DSS_CHANNEL_LCD2)
-		REG_FLD_MOD(DISPC_CONTROL2, enable, 11, 11);
-	else
-		REG_FLD_MOD(DISPC_CONTROL, enable, 11, 11);
+	mgr_fld_write(channel, DISPC_MGR_FLD_STALLMODE, enable);
 }
 
 static bool _dispc_mgr_size_ok(u16 width, u16 height)
@@ -2627,7 +2632,7 @@ bool dispc_mgr_timings_ok(enum omap_channel channel,
 
 	timings_ok = _dispc_mgr_size_ok(timings->x_res, timings->y_res);
 
-	if (dispc_mgr_is_lcd(channel))
+	if (dss_mgr_is_lcd(channel))
 		timings_ok =  timings_ok && _dispc_lcd_timings_ok(timings->hsw,
 						timings->hfp, timings->hbp,
 						timings->vsw, timings->vfp,
@@ -2637,9 +2642,16 @@ bool dispc_mgr_timings_ok(enum omap_channel channel,
 }
 
 static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
-		int hfp, int hbp, int vsw, int vfp, int vbp)
+		int hfp, int hbp, int vsw, int vfp, int vbp,
+		enum omap_dss_signal_level vsync_level,
+		enum omap_dss_signal_level hsync_level,
+		enum omap_dss_signal_edge data_pclk_edge,
+		enum omap_dss_signal_level de_level,
+		enum omap_dss_signal_edge sync_pclk_edge)
+
 {
-	u32 timing_h, timing_v;
+	u32 timing_h, timing_v, l;
+	bool onoff, rf, ipc;
 
 	if (cpu_is_omap24xx() || omap_rev() < OMAP3430_REV_ES3_0) {
 		timing_h = FLD_VAL(hsw-1, 5, 0) | FLD_VAL(hfp-1, 15, 8) |
@@ -2657,6 +2669,44 @@ static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
 
 	dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
 	dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
+
+	switch (data_pclk_edge) {
+	case OMAPDSS_DRIVE_SIG_RISING_EDGE:
+		ipc = false;
+		break;
+	case OMAPDSS_DRIVE_SIG_FALLING_EDGE:
+		ipc = true;
+		break;
+	case OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES:
+	default:
+		BUG();
+	}
+
+	switch (sync_pclk_edge) {
+	case OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES:
+		onoff = false;
+		rf = false;
+		break;
+	case OMAPDSS_DRIVE_SIG_FALLING_EDGE:
+		onoff = true;
+		rf = false;
+		break;
+	case OMAPDSS_DRIVE_SIG_RISING_EDGE:
+		onoff = true;
+		rf = true;
+		break;
+	default:
+		BUG();
+	};
+
+	l = dispc_read_reg(DISPC_POL_FREQ(channel));
+	l |= FLD_VAL(onoff, 17, 17);
+	l |= FLD_VAL(rf, 16, 16);
+	l |= FLD_VAL(de_level, 15, 15);
+	l |= FLD_VAL(ipc, 14, 14);
+	l |= FLD_VAL(hsync_level, 13, 13);
+	l |= FLD_VAL(vsync_level, 12, 12);
+	dispc_write_reg(DISPC_POL_FREQ(channel), l);
 }
 
 /* change name to mode? */
@@ -2674,9 +2724,10 @@ void dispc_mgr_set_timings(enum omap_channel channel,
 		return;
 	}
 
-	if (dispc_mgr_is_lcd(channel)) {
+	if (dss_mgr_is_lcd(channel)) {
 		_dispc_mgr_set_lcd_timings(channel, t.hsw, t.hfp, t.hbp, t.vsw,
-				t.vfp, t.vbp);
+				t.vfp, t.vbp, t.vsync_level, t.hsync_level,
+				t.data_pclk_edge, t.de_level, t.sync_pclk_edge);
 
 		xtot = t.x_res + t.hfp + t.hsw + t.hbp;
 		ytot = t.y_res + t.vfp + t.vsw + t.vbp;
@@ -2687,14 +2738,13 @@ void dispc_mgr_set_timings(enum omap_channel channel,
 		DSSDBG("pck %u\n", timings->pixel_clock);
 		DSSDBG("hsw %d hfp %d hbp %d vsw %d vfp %d vbp %d\n",
 			t.hsw, t.hfp, t.hbp, t.vsw, t.vfp, t.vbp);
+		DSSDBG("vsync_level %d hsync_level %d data_pclk_edge %d de_level %d sync_pclk_edge %d\n",
+			t.vsync_level, t.hsync_level, t.data_pclk_edge,
+			t.de_level, t.sync_pclk_edge);
 
 		DSSDBG("hsync %luHz, vsync %luHz\n", ht, vt);
 	} else {
-		enum dss_hdmi_venc_clk_source_select source;
-
-		source = dss_get_hdmi_venc_clk_source();
-
-		if (source == DSS_VENC_TV_CLK)
+		if (t.interlace == true)
 			t.y_res /= 2;
 	}
 
@@ -2780,7 +2830,7 @@ unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
 {
 	unsigned long r;
 
-	if (dispc_mgr_is_lcd(channel)) {
+	if (dss_mgr_is_lcd(channel)) {
 		int pcd;
 		u32 l;
 
@@ -2821,12 +2871,32 @@ unsigned long dispc_core_clk_rate(void)
 	return fclk / lcd;
 }
 
-void dispc_dump_clocks(struct seq_file *s)
+static void dispc_dump_clocks_channel(struct seq_file *s, enum omap_channel channel)
 {
 	int lcd, pcd;
+	enum omap_dss_clk_source lcd_clk_src;
+
+	seq_printf(s, "- %s -\n", mgr_desc[channel].name);
+
+	lcd_clk_src = dss_get_lcd_clk_source(channel);
+
+	seq_printf(s, "%s clk source = %s (%s)\n", mgr_desc[channel].name,
+		dss_get_generic_clk_source_name(lcd_clk_src),
+		dss_feat_get_clk_source_name(lcd_clk_src));
+
+	dispc_mgr_get_lcd_divisor(channel, &lcd, &pcd);
+
+	seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+		dispc_mgr_lclk_rate(channel), lcd);
+	seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
+		dispc_mgr_pclk_rate(channel), pcd);
+}
+
+void dispc_dump_clocks(struct seq_file *s)
+{
+	int lcd;
 	u32 l;
 	enum omap_dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
-	enum omap_dss_clk_source lcd_clk_src;
 
 	if (dispc_runtime_get())
 		return;
@@ -2847,36 +2917,13 @@ void dispc_dump_clocks(struct seq_file *s)
 		seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
 				(dispc_fclk_rate()/lcd), lcd);
 	}
-	seq_printf(s, "- LCD1 -\n");
-
-	lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD);
-
-	seq_printf(s, "lcd1_clk source = %s (%s)\n",
-		dss_get_generic_clk_source_name(lcd_clk_src),
-		dss_feat_get_clk_source_name(lcd_clk_src));
-
-	dispc_mgr_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD, &lcd, &pcd);
-
-	seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
-			dispc_mgr_lclk_rate(OMAP_DSS_CHANNEL_LCD), lcd);
-	seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
-			dispc_mgr_pclk_rate(OMAP_DSS_CHANNEL_LCD), pcd);
-	if (dss_has_feature(FEAT_MGR_LCD2)) {
-		seq_printf(s, "- LCD2 -\n");
-
-		lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD2);
 
-		seq_printf(s, "lcd2_clk source = %s (%s)\n",
-			dss_get_generic_clk_source_name(lcd_clk_src),
-			dss_feat_get_clk_source_name(lcd_clk_src));
+	dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD);
 
-		dispc_mgr_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD2, &lcd, &pcd);
-
-		seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
-				dispc_mgr_lclk_rate(OMAP_DSS_CHANNEL_LCD2), lcd);
-		seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
-				dispc_mgr_pclk_rate(OMAP_DSS_CHANNEL_LCD2), pcd);
-	}
+	if (dss_has_feature(FEAT_MGR_LCD2))
+		dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD3);
 
 	dispc_runtime_put();
 }
@@ -2929,6 +2976,12 @@ void dispc_dump_irqs(struct seq_file *s)
 		PIS(ACBIAS_COUNT_STAT2);
 		PIS(SYNC_LOST2);
 	}
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		PIS(FRAMEDONE3);
+		PIS(VSYNC3);
+		PIS(ACBIAS_COUNT_STAT3);
+		PIS(SYNC_LOST3);
+	}
 #undef PIS
 }
 #endif
@@ -2940,6 +2993,7 @@ static void dispc_dump_regs(struct seq_file *s)
 		[OMAP_DSS_CHANNEL_LCD]		= "LCD",
 		[OMAP_DSS_CHANNEL_DIGIT]	= "TV",
 		[OMAP_DSS_CHANNEL_LCD2]		= "LCD2",
+		[OMAP_DSS_CHANNEL_LCD3]		= "LCD3",
 	};
 	const char *ovl_names[] = {
 		[OMAP_DSS_GFX]		= "GFX",
@@ -2972,6 +3026,10 @@ static void dispc_dump_regs(struct seq_file *s)
 		DUMPREG(DISPC_CONTROL2);
 		DUMPREG(DISPC_CONFIG2);
 	}
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		DUMPREG(DISPC_CONTROL3);
+		DUMPREG(DISPC_CONFIG3);
+	}
 
 #undef DUMPREG
 
@@ -3093,41 +3151,8 @@ static void dispc_dump_regs(struct seq_file *s)
 #undef DUMPREG
 }
 
-static void _dispc_mgr_set_pol_freq(enum omap_channel channel, bool onoff,
-		bool rf, bool ieo, bool ipc, bool ihs, bool ivs, u8 acbi,
-		u8 acb)
-{
-	u32 l = 0;
-
-	DSSDBG("onoff %d rf %d ieo %d ipc %d ihs %d ivs %d acbi %d acb %d\n",
-			onoff, rf, ieo, ipc, ihs, ivs, acbi, acb);
-
-	l |= FLD_VAL(onoff, 17, 17);
-	l |= FLD_VAL(rf, 16, 16);
-	l |= FLD_VAL(ieo, 15, 15);
-	l |= FLD_VAL(ipc, 14, 14);
-	l |= FLD_VAL(ihs, 13, 13);
-	l |= FLD_VAL(ivs, 12, 12);
-	l |= FLD_VAL(acbi, 11, 8);
-	l |= FLD_VAL(acb, 7, 0);
-
-	dispc_write_reg(DISPC_POL_FREQ(channel), l);
-}
-
-void dispc_mgr_set_pol_freq(enum omap_channel channel,
-		enum omap_panel_config config, u8 acbi, u8 acb)
-{
-	_dispc_mgr_set_pol_freq(channel, (config & OMAP_DSS_LCD_ONOFF) != 0,
-			(config & OMAP_DSS_LCD_RF) != 0,
-			(config & OMAP_DSS_LCD_IEO) != 0,
-			(config & OMAP_DSS_LCD_IPC) != 0,
-			(config & OMAP_DSS_LCD_IHS) != 0,
-			(config & OMAP_DSS_LCD_IVS) != 0,
-			acbi, acb);
-}
-
 /* with fck as input clock rate, find dispc dividers that produce req_pck */
-void dispc_find_clk_divs(bool is_tft, unsigned long req_pck, unsigned long fck,
+void dispc_find_clk_divs(unsigned long req_pck, unsigned long fck,
 		struct dispc_clock_info *cinfo)
 {
 	u16 pcd_min, pcd_max;
@@ -3138,9 +3163,6 @@ void dispc_find_clk_divs(bool is_tft, unsigned long req_pck, unsigned long fck,
 	pcd_min = dss_feat_get_param_min(FEAT_PARAM_DSS_PCD);
 	pcd_max = dss_feat_get_param_max(FEAT_PARAM_DSS_PCD);
 
-	if (!is_tft)
-		pcd_min = 3;
-
 	best_pck = 0;
 	best_ld = 0;
 	best_pd = 0;
@@ -3192,15 +3214,13 @@ int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
 	return 0;
 }
 
-int dispc_mgr_set_clock_div(enum omap_channel channel,
+void dispc_mgr_set_clock_div(enum omap_channel channel,
 		struct dispc_clock_info *cinfo)
 {
 	DSSDBG("lck = %lu (%u)\n", cinfo->lck, cinfo->lck_div);
 	DSSDBG("pck = %lu (%u)\n", cinfo->pck, cinfo->pck_div);
 
 	dispc_mgr_set_lcd_divisor(channel, cinfo->lck_div, cinfo->pck_div);
-
-	return 0;
 }
 
 int dispc_mgr_get_clock_div(enum omap_channel channel,
@@ -3354,6 +3374,8 @@ static void print_irq_status(u32 status)
 	PIS(SYNC_LOST_DIGIT);
 	if (dss_has_feature(FEAT_MGR_LCD2))
 		PIS(SYNC_LOST2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		PIS(SYNC_LOST3);
 #undef PIS
 
 	printk("\n");
@@ -3450,12 +3472,6 @@ static void dispc_error_worker(struct work_struct *work)
 		DISPC_IRQ_VID3_FIFO_UNDERFLOW,
 	};
 
-	static const unsigned sync_lost_bits[] = {
-		DISPC_IRQ_SYNC_LOST,
-		DISPC_IRQ_SYNC_LOST_DIGIT,
-		DISPC_IRQ_SYNC_LOST2,
-	};
-
 	spin_lock_irqsave(&dispc.irq_lock, flags);
 	errors = dispc.error_irqs;
 	dispc.error_irqs = 0;
@@ -3484,7 +3500,7 @@ static void dispc_error_worker(struct work_struct *work)
 		unsigned bit;
 
 		mgr = omap_dss_get_overlay_manager(i);
-		bit = sync_lost_bits[i];
+		bit = mgr_desc[i].sync_lost_irq;
 
 		if (bit & errors) {
 			struct omap_dss_device *dssdev = mgr->device;
@@ -3603,6 +3619,8 @@ static void _omap_dispc_initialize_irq(void)
 	dispc.irq_error_mask = DISPC_IRQ_MASK_ERROR;
 	if (dss_has_feature(FEAT_MGR_LCD2))
 		dispc.irq_error_mask |= DISPC_IRQ_SYNC_LOST2;
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		dispc.irq_error_mask |= DISPC_IRQ_SYNC_LOST3;
 	if (dss_feat_get_num_ovls() > 3)
 		dispc.irq_error_mask |= DISPC_IRQ_VID3_FIFO_UNDERFLOW;
 
diff --git a/drivers/video/omap2/dss/dispc.h b/drivers/video/omap2/dss/dispc.h
index f278080e106..92d8a9be86f 100644
--- a/drivers/video/omap2/dss/dispc.h
+++ b/drivers/video/omap2/dss/dispc.h
@@ -36,6 +36,8 @@
 #define DISPC_CONTROL2			0x0238
 #define DISPC_CONFIG2			0x0620
 #define DISPC_DIVISOR			0x0804
+#define DISPC_CONTROL3                  0x0848
+#define DISPC_CONFIG3                   0x084C
 
 /* DISPC overlay registers */
 #define DISPC_OVL_BA0(n)		(DISPC_OVL_BASE(n) + \
@@ -118,6 +120,8 @@ static inline u16 DISPC_DEFAULT_COLOR(enum omap_channel channel)
 		return 0x0050;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03AC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0814;
 	default:
 		BUG();
 		return 0;
@@ -133,6 +137,8 @@ static inline u16 DISPC_TRANS_COLOR(enum omap_channel channel)
 		return 0x0058;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03B0;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0818;
 	default:
 		BUG();
 		return 0;
@@ -149,6 +155,8 @@ static inline u16 DISPC_TIMING_H(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x0400;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0840;
 	default:
 		BUG();
 		return 0;
@@ -165,6 +173,8 @@ static inline u16 DISPC_TIMING_V(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x0404;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0844;
 	default:
 		BUG();
 		return 0;
@@ -181,6 +191,8 @@ static inline u16 DISPC_POL_FREQ(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x0408;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x083C;
 	default:
 		BUG();
 		return 0;
@@ -197,6 +209,8 @@ static inline u16 DISPC_DIVISORo(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x040C;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0838;
 	default:
 		BUG();
 		return 0;
@@ -213,6 +227,8 @@ static inline u16 DISPC_SIZE_MGR(enum omap_channel channel)
 		return 0x0078;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03CC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0834;
 	default:
 		BUG();
 		return 0;
@@ -229,6 +245,8 @@ static inline u16 DISPC_DATA_CYCLE1(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03C0;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0828;
 	default:
 		BUG();
 		return 0;
@@ -245,6 +263,8 @@ static inline u16 DISPC_DATA_CYCLE2(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03C4;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x082C;
 	default:
 		BUG();
 		return 0;
@@ -261,6 +281,8 @@ static inline u16 DISPC_DATA_CYCLE3(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03C8;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0830;
 	default:
 		BUG();
 		return 0;
@@ -277,6 +299,8 @@ static inline u16 DISPC_CPR_COEF_R(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03BC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0824;
 	default:
 		BUG();
 		return 0;
@@ -293,6 +317,8 @@ static inline u16 DISPC_CPR_COEF_G(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03B8;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0820;
 	default:
 		BUG();
 		return 0;
@@ -309,6 +335,8 @@ static inline u16 DISPC_CPR_COEF_B(enum omap_channel channel)
 		return 0;
 	case OMAP_DSS_CHANNEL_LCD2:
 		return 0x03B4;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x081C;
 	default:
 		BUG();
 		return 0;
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 24901063037..5bd957e8550 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -116,7 +116,7 @@ static ssize_t display_timings_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	struct omap_video_timings t;
+	struct omap_video_timings t = dssdev->panel.timings;
 	int r, found;
 
 	if (!dssdev->driver->set_timings || !dssdev->driver->check_timings)
@@ -316,44 +316,6 @@ void omapdss_default_get_timings(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(omapdss_default_get_timings);
 
-/* Checks if replication logic should be used. Only use for active matrix,
- * when overlay is in RGB12U or RGB16 mode, and LCD interface is
- * 18bpp or 24bpp */
-bool dss_use_replication(struct omap_dss_device *dssdev,
-		enum omap_color_mode mode)
-{
-	int bpp;
-
-	if (mode != OMAP_DSS_COLOR_RGB12U && mode != OMAP_DSS_COLOR_RGB16)
-		return false;
-
-	if (dssdev->type == OMAP_DISPLAY_TYPE_DPI &&
-			(dssdev->panel.config & OMAP_DSS_LCD_TFT) == 0)
-		return false;
-
-	switch (dssdev->type) {
-	case OMAP_DISPLAY_TYPE_DPI:
-		bpp = dssdev->phy.dpi.data_lines;
-		break;
-	case OMAP_DISPLAY_TYPE_HDMI:
-	case OMAP_DISPLAY_TYPE_VENC:
-	case OMAP_DISPLAY_TYPE_SDI:
-		bpp = 24;
-		break;
-	case OMAP_DISPLAY_TYPE_DBI:
-		bpp = dssdev->ctrl.pixel_size;
-		break;
-	case OMAP_DISPLAY_TYPE_DSI:
-		bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
-		break;
-	default:
-		BUG();
-		return false;
-	}
-
-	return bpp > 16;
-}
-
 void dss_init_device(struct platform_device *pdev,
 		struct omap_dss_device *dssdev)
 {
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 8c2056c9537..3266be23fc0 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -38,6 +38,8 @@
 static struct {
 	struct regulator *vdds_dsi_reg;
 	struct platform_device *dsidev;
+
+	struct dss_lcd_mgr_config mgr_config;
 } dpi;
 
 static struct platform_device *dpi_get_dsidev(enum omap_dss_clk_source clk)
@@ -64,7 +66,7 @@ static bool dpi_use_dsi_pll(struct omap_dss_device *dssdev)
 		return false;
 }
 
-static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
+static int dpi_set_dsi_clk(struct omap_dss_device *dssdev,
 		unsigned long pck_req, unsigned long *fck, int *lck_div,
 		int *pck_div)
 {
@@ -72,8 +74,8 @@ static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
 	struct dispc_clock_info dispc_cinfo;
 	int r;
 
-	r = dsi_pll_calc_clock_div_pck(dpi.dsidev, is_tft, pck_req,
-			&dsi_cinfo, &dispc_cinfo);
+	r = dsi_pll_calc_clock_div_pck(dpi.dsidev, pck_req, &dsi_cinfo,
+			&dispc_cinfo);
 	if (r)
 		return r;
 
@@ -83,11 +85,7 @@ static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
 
 	dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
 
-	r = dispc_mgr_set_clock_div(dssdev->manager->id, &dispc_cinfo);
-	if (r) {
-		dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
-		return r;
-	}
+	dpi.mgr_config.clock_info = dispc_cinfo;
 
 	*fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
 	*lck_div = dispc_cinfo.lck_div;
@@ -96,7 +94,7 @@ static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
 	return 0;
 }
 
-static int dpi_set_dispc_clk(struct omap_dss_device *dssdev, bool is_tft,
+static int dpi_set_dispc_clk(struct omap_dss_device *dssdev,
 		unsigned long pck_req, unsigned long *fck, int *lck_div,
 		int *pck_div)
 {
@@ -104,7 +102,7 @@ static int dpi_set_dispc_clk(struct omap_dss_device *dssdev, bool is_tft,
 	struct dispc_clock_info dispc_cinfo;
 	int r;
 
-	r = dss_calc_clock_div(is_tft, pck_req, &dss_cinfo, &dispc_cinfo);
+	r = dss_calc_clock_div(pck_req, &dss_cinfo, &dispc_cinfo);
 	if (r)
 		return r;
 
@@ -112,9 +110,7 @@ static int dpi_set_dispc_clk(struct omap_dss_device *dssdev, bool is_tft,
 	if (r)
 		return r;
 
-	r = dispc_mgr_set_clock_div(dssdev->manager->id, &dispc_cinfo);
-	if (r)
-		return r;
+	dpi.mgr_config.clock_info = dispc_cinfo;
 
 	*fck = dss_cinfo.fck;
 	*lck_div = dispc_cinfo.lck_div;
@@ -129,20 +125,14 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 	int lck_div = 0, pck_div = 0;
 	unsigned long fck = 0;
 	unsigned long pck;
-	bool is_tft;
 	int r = 0;
 
-	dispc_mgr_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
-			dssdev->panel.acbi, dssdev->panel.acb);
-
-	is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
-
 	if (dpi_use_dsi_pll(dssdev))
-		r = dpi_set_dsi_clk(dssdev, is_tft, t->pixel_clock * 1000,
-				&fck, &lck_div, &pck_div);
+		r = dpi_set_dsi_clk(dssdev, t->pixel_clock * 1000, &fck,
+				&lck_div, &pck_div);
 	else
-		r = dpi_set_dispc_clk(dssdev, is_tft, t->pixel_clock * 1000,
-				&fck, &lck_div, &pck_div);
+		r = dpi_set_dispc_clk(dssdev, t->pixel_clock * 1000, &fck,
+				&lck_div, &pck_div);
 	if (r)
 		return r;
 
@@ -161,19 +151,18 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void dpi_basic_init(struct omap_dss_device *dssdev)
+static void dpi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
-	bool is_tft;
+	dpi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+
+	dpi.mgr_config.stallmode = false;
+	dpi.mgr_config.fifohandcheck = false;
 
-	is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
+	dpi.mgr_config.video_port_width = dssdev->phy.dpi.data_lines;
 
-	dispc_mgr_set_io_pad_mode(DSS_IO_PAD_MODE_BYPASS);
-	dispc_mgr_enable_stallmode(dssdev->manager->id, false);
+	dpi.mgr_config.lcden_sig_polarity = 0;
 
-	dispc_mgr_set_lcd_display_type(dssdev->manager->id, is_tft ?
-			OMAP_DSS_LCD_DISPLAY_TFT : OMAP_DSS_LCD_DISPLAY_STN);
-	dispc_mgr_set_tft_data_lines(dssdev->manager->id,
-			dssdev->phy.dpi.data_lines);
+	dss_mgr_set_lcd_config(dssdev->manager, &dpi.mgr_config);
 }
 
 int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
@@ -206,8 +195,6 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_get_dispc;
 
-	dpi_basic_init(dssdev);
-
 	if (dpi_use_dsi_pll(dssdev)) {
 		r = dsi_runtime_get(dpi.dsidev);
 		if (r)
@@ -222,6 +209,8 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_set_mode;
 
+	dpi_config_lcd_manager(dssdev);
+
 	mdelay(2);
 
 	r = dss_mgr_enable(dssdev->manager);
@@ -292,7 +281,6 @@ EXPORT_SYMBOL(dpi_set_timings);
 int dpi_check_timings(struct omap_dss_device *dssdev,
 			struct omap_video_timings *timings)
 {
-	bool is_tft;
 	int r;
 	int lck_div, pck_div;
 	unsigned long fck;
@@ -305,11 +293,9 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 	if (timings->pixel_clock == 0)
 		return -EINVAL;
 
-	is_tft = (dssdev->panel.config & OMAP_DSS_LCD_TFT) != 0;
-
 	if (dpi_use_dsi_pll(dssdev)) {
 		struct dsi_clock_info dsi_cinfo;
-		r = dsi_pll_calc_clock_div_pck(dpi.dsidev, is_tft,
+		r = dsi_pll_calc_clock_div_pck(dpi.dsidev,
 				timings->pixel_clock * 1000,
 				&dsi_cinfo, &dispc_cinfo);
 
@@ -319,7 +305,7 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 		fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
 	} else {
 		struct dss_clock_info dss_cinfo;
-		r = dss_calc_clock_div(is_tft, timings->pixel_clock * 1000,
+		r = dss_calc_clock_div(timings->pixel_clock * 1000,
 				&dss_cinfo, &dispc_cinfo);
 
 		if (r)
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 14ce8cc079e..b07e8864f82 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -331,6 +331,8 @@ struct dsi_data {
 	unsigned num_lanes_used;
 
 	unsigned scp_clk_refcount;
+
+	struct dss_lcd_mgr_config mgr_config;
 };
 
 struct dsi_packet_sent_handler_data {
@@ -1085,9 +1087,9 @@ static inline void dsi_enable_pll_clock(struct platform_device *dsidev,
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	if (enable)
-		clk_enable(dsi->sys_clk);
+		clk_prepare_enable(dsi->sys_clk);
 	else
-		clk_disable(dsi->sys_clk);
+		clk_disable_unprepare(dsi->sys_clk);
 
 	if (enable && dsi->pll_locked) {
 		if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 1, 1) != 1)
@@ -1316,7 +1318,7 @@ static int dsi_calc_clock_rates(struct platform_device *dsidev,
 	return 0;
 }
 
-int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
+int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
 		unsigned long req_pck, struct dsi_clock_info *dsi_cinfo,
 		struct dispc_clock_info *dispc_cinfo)
 {
@@ -1335,8 +1337,8 @@ int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
 			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->dsi_pll_hsdiv_dispc_clk, dispc_cinfo);
+		dispc_find_clk_divs(req_pck, dsi_cinfo->dsi_pll_hsdiv_dispc_clk,
+			dispc_cinfo);
 		return 0;
 	}
 
@@ -1402,7 +1404,7 @@ retry:
 
 				match = 1;
 
-				dispc_find_clk_divs(is_tft, req_pck,
+				dispc_find_clk_divs(req_pck,
 						cur.dsi_pll_hsdiv_dispc_clk,
 						&cur_dispc);
 
@@ -3631,17 +3633,14 @@ static void dsi_config_vp_num_line_buffers(struct omap_dss_device *dssdev)
 static void dsi_config_vp_sync_events(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	int de_pol = dssdev->panel.dsi_vm_data.vp_de_pol;
-	int hsync_pol = dssdev->panel.dsi_vm_data.vp_hsync_pol;
-	int vsync_pol = dssdev->panel.dsi_vm_data.vp_vsync_pol;
 	bool vsync_end = dssdev->panel.dsi_vm_data.vp_vsync_end;
 	bool hsync_end = dssdev->panel.dsi_vm_data.vp_hsync_end;
 	u32 r;
 
 	r = dsi_read_reg(dsidev, DSI_CTRL);
-	r = FLD_MOD(r, de_pol, 9, 9);		/* VP_DE_POL */
-	r = FLD_MOD(r, hsync_pol, 10, 10);	/* VP_HSYNC_POL */
-	r = FLD_MOD(r, vsync_pol, 11, 11);	/* VP_VSYNC_POL */
+	r = FLD_MOD(r, 1, 9, 9);		/* VP_DE_POL */
+	r = FLD_MOD(r, 1, 10, 10);		/* VP_HSYNC_POL */
+	r = FLD_MOD(r, 1, 11, 11);		/* VP_VSYNC_POL */
 	r = FLD_MOD(r, 1, 15, 15);		/* VP_VSYNC_START */
 	r = FLD_MOD(r, vsync_end, 16, 16);	/* VP_VSYNC_END */
 	r = FLD_MOD(r, 1, 17, 17);		/* VP_HSYNC_START */
@@ -4340,52 +4339,101 @@ EXPORT_SYMBOL(omap_dsi_update);
 
 /* Display funcs */
 
+static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dispc_clock_info dispc_cinfo;
+	int r;
+	unsigned long long fck;
+
+	fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+
+	dispc_cinfo.lck_div = dssdev->clocks.dispc.channel.lck_div;
+	dispc_cinfo.pck_div = dssdev->clocks.dispc.channel.pck_div;
+
+	r = dispc_calc_clock_rates(fck, &dispc_cinfo);
+	if (r) {
+		DSSERR("Failed to calc dispc clocks\n");
+		return r;
+	}
+
+	dsi->mgr_config.clock_info = dispc_cinfo;
+
+	return 0;
+}
+
 static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_video_timings timings;
 	int r;
+	u32 irq = 0;
 
 	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
 		u16 dw, dh;
-		u32 irq;
-		struct omap_video_timings timings = {
-			.hsw		= 1,
-			.hfp		= 1,
-			.hbp		= 1,
-			.vsw		= 1,
-			.vfp		= 0,
-			.vbp		= 0,
-		};
 
 		dssdev->driver->get_resolution(dssdev, &dw, &dh);
+
 		timings.x_res = dw;
 		timings.y_res = dh;
+		timings.hsw = 1;
+		timings.hfp = 1;
+		timings.hbp = 1;
+		timings.vsw = 1;
+		timings.vfp = 0;
+		timings.vbp = 0;
 
-		irq = dssdev->manager->id == OMAP_DSS_CHANNEL_LCD ?
-			DISPC_IRQ_FRAMEDONE : DISPC_IRQ_FRAMEDONE2;
+		irq = dispc_mgr_get_framedone_irq(dssdev->manager->id);
 
 		r = omap_dispc_register_isr(dsi_framedone_irq_callback,
 			(void *) dssdev, irq);
 		if (r) {
 			DSSERR("can't get FRAMEDONE irq\n");
-			return r;
+			goto err;
 		}
 
-		dispc_mgr_enable_stallmode(dssdev->manager->id, true);
-		dispc_mgr_enable_fifohandcheck(dssdev->manager->id, 1);
-
-		dss_mgr_set_timings(dssdev->manager, &timings);
+		dsi->mgr_config.stallmode = true;
+		dsi->mgr_config.fifohandcheck = true;
 	} else {
-		dispc_mgr_enable_stallmode(dssdev->manager->id, false);
-		dispc_mgr_enable_fifohandcheck(dssdev->manager->id, 0);
+		timings = dssdev->panel.timings;
 
-		dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
+		dsi->mgr_config.stallmode = false;
+		dsi->mgr_config.fifohandcheck = false;
 	}
 
-		dispc_mgr_set_lcd_display_type(dssdev->manager->id,
-			OMAP_DSS_LCD_DISPLAY_TFT);
-		dispc_mgr_set_tft_data_lines(dssdev->manager->id,
-			dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt));
+	/*
+	 * override interlace, logic level and edge related parameters in
+	 * omap_video_timings with default values
+	 */
+	timings.interlace = false;
+	timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+
+	dss_mgr_set_timings(dssdev->manager, &timings);
+
+	r = dsi_configure_dispc_clocks(dssdev);
+	if (r)
+		goto err1;
+
+	dsi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+	dsi->mgr_config.video_port_width =
+			dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+	dsi->mgr_config.lcden_sig_polarity = 0;
+
+	dss_mgr_set_lcd_config(dssdev->manager, &dsi->mgr_config);
+
 	return 0;
+err1:
+	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE)
+		omap_dispc_unregister_isr(dsi_framedone_irq_callback,
+			(void *) dssdev, irq);
+err:
+	return r;
 }
 
 static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
@@ -4393,8 +4441,7 @@ static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
 	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
 		u32 irq;
 
-		irq = dssdev->manager->id == OMAP_DSS_CHANNEL_LCD ?
-			DISPC_IRQ_FRAMEDONE : DISPC_IRQ_FRAMEDONE2;
+		irq = dispc_mgr_get_framedone_irq(dssdev->manager->id);
 
 		omap_dispc_unregister_isr(dsi_framedone_irq_callback,
 			(void *) dssdev, irq);
@@ -4426,33 +4473,6 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
-{
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	struct dispc_clock_info dispc_cinfo;
-	int r;
-	unsigned long long fck;
-
-	fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
-
-	dispc_cinfo.lck_div = dssdev->clocks.dispc.channel.lck_div;
-	dispc_cinfo.pck_div = dssdev->clocks.dispc.channel.pck_div;
-
-	r = dispc_calc_clock_rates(fck, &dispc_cinfo);
-	if (r) {
-		DSSERR("Failed to calc dispc clocks\n");
-		return r;
-	}
-
-	r = dispc_mgr_set_clock_div(dssdev->manager->id, &dispc_cinfo);
-	if (r) {
-		DSSERR("Failed to set dispc clocks\n");
-		return r;
-	}
-
-	return 0;
-}
-
 static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
@@ -4474,10 +4494,6 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 
 	DSSDBG("PLL OK\n");
 
-	r = dsi_configure_dispc_clocks(dssdev);
-	if (r)
-		goto err2;
-
 	r = dsi_cio_init(dssdev);
 	if (r)
 		goto err2;
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index d2b57197b29..04b4586113e 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -388,7 +388,8 @@ void dss_select_lcd_clk_source(enum omap_channel channel,
 		dsi_wait_pll_hsdiv_dispc_active(dsidev);
 		break;
 	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
-		BUG_ON(channel != OMAP_DSS_CHANNEL_LCD2);
+		BUG_ON(channel != OMAP_DSS_CHANNEL_LCD2 &&
+		       channel != OMAP_DSS_CHANNEL_LCD3);
 		b = 1;
 		dsidev = dsi_get_dsidev_from_id(1);
 		dsi_wait_pll_hsdiv_dispc_active(dsidev);
@@ -398,10 +399,12 @@ void dss_select_lcd_clk_source(enum omap_channel channel,
 		return;
 	}
 
-	pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 12;
+	pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+	     (channel == OMAP_DSS_CHANNEL_LCD2 ? 12 : 19);
 	REG_FLD_MOD(DSS_CONTROL, b, pos, pos);	/* LCDx_CLK_SWITCH */
 
-	ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+	ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+	    (channel == OMAP_DSS_CHANNEL_LCD2 ? 1 : 2);
 	dss.lcd_clk_source[ix] = clk_src;
 }
 
@@ -418,7 +421,8 @@ enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module)
 enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
 {
 	if (dss_has_feature(FEAT_LCD_CLK_SRC)) {
-		int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+		int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+			(channel == OMAP_DSS_CHANNEL_LCD2 ? 1 : 2);
 		return dss.lcd_clk_source[ix];
 	} else {
 		/* LCD_CLK source is the same as DISPC_FCLK source for
@@ -502,8 +506,7 @@ unsigned long dss_get_dpll4_rate(void)
 		return 0;
 }
 
-int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
-		struct dss_clock_info *dss_cinfo,
+int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
 		struct dispc_clock_info *dispc_cinfo)
 {
 	unsigned long prate;
@@ -551,7 +554,7 @@ retry:
 		fck = clk_get_rate(dss.dss_clk);
 		fck_div = 1;
 
-		dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
+		dispc_find_clk_divs(req_pck, fck, &cur_dispc);
 		match = 1;
 
 		best_dss.fck = fck;
@@ -581,7 +584,7 @@ retry:
 
 			match = 1;
 
-			dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
+			dispc_find_clk_divs(req_pck, fck, &cur_dispc);
 
 			if (abs(cur_dispc.pck - req_pck) <
 					abs(best_dispc.pck - req_pck)) {
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index dd1092ceaee..f67afe76f21 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -152,6 +152,25 @@ struct dsi_clock_info {
 	u16 lp_clk_div;
 };
 
+struct reg_field {
+	u16 reg;
+	u8 high;
+	u8 low;
+};
+
+struct dss_lcd_mgr_config {
+	enum dss_io_pad_mode io_pad_mode;
+
+	bool stallmode;
+	bool fifohandcheck;
+
+	struct dispc_clock_info clock_info;
+
+	int video_port_width;
+
+	int lcden_sig_polarity;
+};
+
 struct seq_file;
 struct platform_device;
 
@@ -188,6 +207,8 @@ int dss_mgr_set_device(struct omap_overlay_manager *mgr,
 int dss_mgr_unset_device(struct omap_overlay_manager *mgr);
 void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
 		struct omap_video_timings *timings);
+void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config);
 const struct omap_video_timings *dss_mgr_get_timings(struct omap_overlay_manager *mgr);
 
 bool dss_ovl_is_enabled(struct omap_overlay *ovl);
@@ -210,8 +231,6 @@ void dss_init_device(struct platform_device *pdev,
 		struct omap_dss_device *dssdev);
 void dss_uninit_device(struct platform_device *pdev,
 		struct omap_dss_device *dssdev);
-bool dss_use_replication(struct omap_dss_device *dssdev,
-		enum omap_color_mode mode);
 
 /* manager */
 int dss_init_overlay_managers(struct platform_device *pdev);
@@ -223,8 +242,18 @@ int dss_mgr_check_timings(struct omap_overlay_manager *mgr,
 int dss_mgr_check(struct omap_overlay_manager *mgr,
 		struct omap_overlay_manager_info *info,
 		const struct omap_video_timings *mgr_timings,
+		const struct dss_lcd_mgr_config *config,
 		struct omap_overlay_info **overlay_infos);
 
+static inline bool dss_mgr_is_lcd(enum omap_channel id)
+{
+	if (id == OMAP_DSS_CHANNEL_LCD || id == OMAP_DSS_CHANNEL_LCD2 ||
+			id == OMAP_DSS_CHANNEL_LCD3)
+		return true;
+	else
+		return false;
+}
+
 /* overlay */
 void dss_init_overlays(struct platform_device *pdev);
 void dss_uninit_overlays(struct platform_device *pdev);
@@ -234,6 +263,8 @@ int dss_ovl_simple_check(struct omap_overlay *ovl,
 		const struct omap_overlay_info *info);
 int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
 		const struct omap_video_timings *mgr_timings);
+bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
+		enum omap_color_mode mode);
 
 /* DSS */
 int dss_init_platform_driver(void) __init;
@@ -268,8 +299,7 @@ unsigned long dss_get_dpll4_rate(void);
 int dss_calc_clock_rates(struct dss_clock_info *cinfo);
 int dss_set_clock_div(struct dss_clock_info *cinfo);
 int dss_get_clock_div(struct dss_clock_info *cinfo);
-int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
-		struct dss_clock_info *dss_cinfo,
+int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
 		struct dispc_clock_info *dispc_cinfo);
 
 /* SDI */
@@ -296,7 +326,7 @@ u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt);
 unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev);
 int dsi_pll_set_clock_div(struct platform_device *dsidev,
 		struct dsi_clock_info *cinfo);
-int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev, bool is_tft,
+int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
 		unsigned long req_pck, struct dsi_clock_info *cinfo,
 		struct dispc_clock_info *dispc_cinfo);
 int dsi_pll_init(struct platform_device *dsidev, bool enable_hsclk,
@@ -330,7 +360,7 @@ static inline int dsi_pll_set_clock_div(struct platform_device *dsidev,
 	return -ENODEV;
 }
 static inline int dsi_pll_calc_clock_div_pck(struct platform_device *dsidev,
-		bool is_tft, unsigned long req_pck,
+		unsigned long req_pck,
 		struct dsi_clock_info *dsi_cinfo,
 		struct dispc_clock_info *dispc_cinfo)
 {
@@ -387,7 +417,7 @@ void dispc_set_loadmode(enum omap_dss_load_mode mode);
 bool dispc_mgr_timings_ok(enum omap_channel channel,
 		const struct omap_video_timings *timings);
 unsigned long dispc_fclk_rate(void);
-void dispc_find_clk_divs(bool is_tft, unsigned long req_pck, unsigned long fck,
+void dispc_find_clk_divs(unsigned long req_pck, unsigned long fck,
 		struct dispc_clock_info *cinfo);
 int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
 		struct dispc_clock_info *cinfo);
@@ -398,8 +428,7 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
 		u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
 		bool manual_update);
 int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
-		bool ilace, bool replication,
-		const struct omap_video_timings *mgr_timings);
+		bool replication, const struct omap_video_timings *mgr_timings);
 int dispc_ovl_enable(enum omap_plane plane, bool enable);
 void dispc_ovl_set_channel_out(enum omap_plane plane,
 		enum omap_channel channel);
@@ -415,16 +444,13 @@ bool dispc_mgr_is_channel_enabled(enum omap_channel channel);
 void dispc_mgr_set_io_pad_mode(enum dss_io_pad_mode mode);
 void dispc_mgr_enable_stallmode(enum omap_channel channel, bool enable);
 void dispc_mgr_set_tft_data_lines(enum omap_channel channel, u8 data_lines);
-void dispc_mgr_set_lcd_display_type(enum omap_channel channel,
-		enum omap_lcd_display_type type);
+void dispc_mgr_set_lcd_type_tft(enum omap_channel channel);
 void dispc_mgr_set_timings(enum omap_channel channel,
 		struct omap_video_timings *timings);
-void dispc_mgr_set_pol_freq(enum omap_channel channel,
-		enum omap_panel_config config, u8 acbi, u8 acb);
 unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
 unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
 unsigned long dispc_core_clk_rate(void);
-int dispc_mgr_set_clock_div(enum omap_channel channel,
+void dispc_mgr_set_clock_div(enum omap_channel channel,
 		struct dispc_clock_info *cinfo);
 int dispc_mgr_get_clock_div(enum omap_channel channel,
 		struct dispc_clock_info *cinfo);
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
index bdf469f080e..996ffcbfed5 100644
--- a/drivers/video/omap2/dss/dss_features.h
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -24,9 +24,9 @@
 #include "ti_hdmi.h"
 #endif
 
-#define MAX_DSS_MANAGERS	3
+#define MAX_DSS_MANAGERS	4
 #define MAX_DSS_OVERLAYS	4
-#define MAX_DSS_LCD_MANAGERS	2
+#define MAX_DSS_LCD_MANAGERS	3
 #define MAX_NUM_DSI		2
 
 /* DSS has feature id */
@@ -36,6 +36,7 @@ enum dss_feat_id {
 	FEAT_PCKFREEENABLE,
 	FEAT_FUNCGATED,
 	FEAT_MGR_LCD2,
+	FEAT_MGR_LCD3,
 	FEAT_LINEBUFFERSPLIT,
 	FEAT_ROWREPEATENABLE,
 	FEAT_RESIZECONF,
diff --git a/drivers/video/omap2/dss/hdmi.c b/drivers/video/omap2/dss/hdmi.c
index 26a2430a702..060216fdc57 100644
--- a/drivers/video/omap2/dss/hdmi.c
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -78,43 +78,214 @@ static struct {
  */
 
 static const struct hdmi_config cea_timings[] = {
-{ {640, 480, 25200, 96, 16, 48, 2, 10, 33, 0, 0, 0}, {1, HDMI_HDMI} },
-{ {720, 480, 27027, 62, 16, 60, 6, 9, 30, 0, 0, 0}, {2, HDMI_HDMI} },
-{ {1280, 720, 74250, 40, 110, 220, 5, 5, 20, 1, 1, 0}, {4, HDMI_HDMI} },
-{ {1920, 540, 74250, 44, 88, 148, 5, 2, 15, 1, 1, 1}, {5, HDMI_HDMI} },
-{ {1440, 240, 27027, 124, 38, 114, 3, 4, 15, 0, 0, 1}, {6, HDMI_HDMI} },
-{ {1920, 1080, 148500, 44, 88, 148, 5, 4, 36, 1, 1, 0}, {16, HDMI_HDMI} },
-{ {720, 576, 27000, 64, 12, 68, 5, 5, 39, 0, 0, 0}, {17, HDMI_HDMI} },
-{ {1280, 720, 74250, 40, 440, 220, 5, 5, 20, 1, 1, 0}, {19, HDMI_HDMI} },
-{ {1920, 540, 74250, 44, 528, 148, 5, 2, 15, 1, 1, 1}, {20, HDMI_HDMI} },
-{ {1440, 288, 27000, 126, 24, 138, 3, 2, 19, 0, 0, 1}, {21, HDMI_HDMI} },
-{ {1440, 576, 54000, 128, 24, 136, 5, 5, 39, 0, 0, 0}, {29, HDMI_HDMI} },
-{ {1920, 1080, 148500, 44, 528, 148, 5, 4, 36, 1, 1, 0}, {31, HDMI_HDMI} },
-{ {1920, 1080, 74250, 44, 638, 148, 5, 4, 36, 1, 1, 0}, {32, HDMI_HDMI} },
-{ {2880, 480, 108108, 248, 64, 240, 6, 9, 30, 0, 0, 0}, {35, HDMI_HDMI} },
-{ {2880, 576, 108000, 256, 48, 272, 5, 5, 39, 0, 0, 0}, {37, HDMI_HDMI} },
+	{
+		{ 640, 480, 25200, 96, 16, 48, 2, 10, 33,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 1, HDMI_HDMI },
+	},
+	{
+		{ 720, 480, 27027, 62, 16, 60, 6, 9, 30,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 2, HDMI_HDMI },
+	},
+	{
+		{ 1280, 720, 74250, 40, 110, 220, 5, 5, 20,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 4, HDMI_HDMI },
+	},
+	{
+		{ 1920, 540, 74250, 44, 88, 148, 5, 2, 15,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			true, },
+		{ 5, HDMI_HDMI },
+	},
+	{
+		{ 1440, 240, 27027, 124, 38, 114, 3, 4, 15,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			true, },
+		{ 6, HDMI_HDMI },
+	},
+	{
+		{ 1920, 1080, 148500, 44, 88, 148, 5, 4, 36,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 16, HDMI_HDMI },
+	},
+	{
+		{ 720, 576, 27000, 64, 12, 68, 5, 5, 39,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 17, HDMI_HDMI },
+	},
+	{
+		{ 1280, 720, 74250, 40, 440, 220, 5, 5, 20,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 19, HDMI_HDMI },
+	},
+	{
+		{ 1920, 540, 74250, 44, 528, 148, 5, 2, 15,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			true, },
+		{ 20, HDMI_HDMI },
+	},
+	{
+		{ 1440, 288, 27000, 126, 24, 138, 3, 2, 19,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			true, },
+		{ 21, HDMI_HDMI },
+	},
+	{
+		{ 1440, 576, 54000, 128, 24, 136, 5, 5, 39,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 29, HDMI_HDMI },
+	},
+	{
+		{ 1920, 1080, 148500, 44, 528, 148, 5, 4, 36,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 31, HDMI_HDMI },
+	},
+	{
+		{ 1920, 1080, 74250, 44, 638, 148, 5, 4, 36,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 32, HDMI_HDMI },
+	},
+	{
+		{ 2880, 480, 108108, 248, 64, 240, 6, 9, 30,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 35, HDMI_HDMI },
+	},
+	{
+		{ 2880, 576, 108000, 256, 48, 272, 5, 5, 39,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 37, HDMI_HDMI },
+	},
 };
+
 static const struct hdmi_config vesa_timings[] = {
 /* VESA From Here */
-{ {640, 480, 25175, 96, 16, 48, 2 , 11, 31, 0, 0, 0}, {4, HDMI_DVI} },
-{ {800, 600, 40000, 128, 40, 88, 4 , 1, 23, 1, 1, 0}, {9, HDMI_DVI} },
-{ {848, 480, 33750, 112, 16, 112, 8 , 6, 23, 1, 1, 0}, {0xE, HDMI_DVI} },
-{ {1280, 768, 79500, 128, 64, 192, 7 , 3, 20, 1, 0, 0}, {0x17, HDMI_DVI} },
-{ {1280, 800, 83500, 128, 72, 200, 6 , 3, 22, 1, 0, 0}, {0x1C, HDMI_DVI} },
-{ {1360, 768, 85500, 112, 64, 256, 6 , 3, 18, 1, 1, 0}, {0x27, HDMI_DVI} },
-{ {1280, 960, 108000, 112, 96, 312, 3 , 1, 36, 1, 1, 0}, {0x20, HDMI_DVI} },
-{ {1280, 1024, 108000, 112, 48, 248, 3 , 1, 38, 1, 1, 0}, {0x23, HDMI_DVI} },
-{ {1024, 768, 65000, 136, 24, 160, 6, 3, 29, 0, 0, 0}, {0x10, HDMI_DVI} },
-{ {1400, 1050, 121750, 144, 88, 232, 4, 3, 32, 1, 0, 0}, {0x2A, HDMI_DVI} },
-{ {1440, 900, 106500, 152, 80, 232, 6, 3, 25, 1, 0, 0}, {0x2F, HDMI_DVI} },
-{ {1680, 1050, 146250, 176 , 104, 280, 6, 3, 30, 1, 0, 0}, {0x3A, HDMI_DVI} },
-{ {1366, 768, 85500, 143, 70, 213, 3, 3, 24, 1, 1, 0}, {0x51, HDMI_DVI} },
-{ {1920, 1080, 148500, 44, 148, 80, 5, 4, 36, 1, 1, 0}, {0x52, HDMI_DVI} },
-{ {1280, 768, 68250, 32, 48, 80, 7, 3, 12, 0, 1, 0}, {0x16, HDMI_DVI} },
-{ {1400, 1050, 101000, 32, 48, 80, 4, 3, 23, 0, 1, 0}, {0x29, HDMI_DVI} },
-{ {1680, 1050, 119000, 32, 48, 80, 6, 3, 21, 0, 1, 0}, {0x39, HDMI_DVI} },
-{ {1280, 800, 79500, 32, 48, 80, 6, 3, 14, 0, 1, 0}, {0x1B, HDMI_DVI} },
-{ {1280, 720, 74250, 40, 110, 220, 5, 5, 20, 1, 1, 0}, {0x55, HDMI_DVI} }
+	{
+		{ 640, 480, 25175, 96, 16, 48, 2, 11, 31,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 4, HDMI_DVI },
+	},
+	{
+		{ 800, 600, 40000, 128, 40, 88, 4, 1, 23,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 9, HDMI_DVI },
+	},
+	{
+		{ 848, 480, 33750, 112, 16, 112, 8, 6, 23,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0xE, HDMI_DVI },
+	},
+	{
+		{ 1280, 768, 79500, 128, 64, 192, 7, 3, 20,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 0x17, HDMI_DVI },
+	},
+	{
+		{ 1280, 800, 83500, 128, 72, 200, 6, 3, 22,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 0x1C, HDMI_DVI },
+	},
+	{
+		{ 1360, 768, 85500, 112, 64, 256, 6, 3, 18,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x27, HDMI_DVI },
+	},
+	{
+		{ 1280, 960, 108000, 112, 96, 312, 3, 1, 36,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x20, HDMI_DVI },
+	},
+	{
+		{ 1280, 1024, 108000, 112, 48, 248, 3, 1, 38,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x23, HDMI_DVI },
+	},
+	{
+		{ 1024, 768, 65000, 136, 24, 160, 6, 3, 29,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 0x10, HDMI_DVI },
+	},
+	{
+		{ 1400, 1050, 121750, 144, 88, 232, 4, 3, 32,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 0x2A, HDMI_DVI },
+	},
+	{
+		{ 1440, 900, 106500, 152, 80, 232, 6, 3, 25,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 0x2F, HDMI_DVI },
+	},
+	{
+		{ 1680, 1050, 146250, 176 , 104, 280, 6, 3, 30,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_LOW,
+			false, },
+		{ 0x3A, HDMI_DVI },
+	},
+	{
+		{ 1366, 768, 85500, 143, 70, 213, 3, 3, 24,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x51, HDMI_DVI },
+	},
+	{
+		{ 1920, 1080, 148500, 44, 148, 80, 5, 4, 36,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x52, HDMI_DVI },
+	},
+	{
+		{ 1280, 768, 68250, 32, 48, 80, 7, 3, 12,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x16, HDMI_DVI },
+	},
+	{
+		{ 1400, 1050, 101000, 32, 48, 80, 4, 3, 23,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x29, HDMI_DVI },
+	},
+	{
+		{ 1680, 1050, 119000, 32, 48, 80, 6, 3, 21,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x39, HDMI_DVI },
+	},
+	{
+		{ 1280, 800, 79500, 32, 48, 80, 6, 3, 14,
+			OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x1B, HDMI_DVI },
+	},
+	{
+		{ 1280, 720, 74250, 40, 110, 220, 5, 5, 20,
+			OMAPDSS_SIG_ACTIVE_HIGH, OMAPDSS_SIG_ACTIVE_HIGH,
+			false, },
+		{ 0x55, HDMI_DVI },
+	},
 };
 
 static int hdmi_runtime_get(void)
@@ -179,7 +350,7 @@ static const struct hdmi_config *hdmi_get_timings(void)
 }
 
 static bool hdmi_timings_compare(struct omap_video_timings *timing1,
-				const struct hdmi_video_timings *timing2)
+				const struct omap_video_timings *timing2)
 {
 	int timing1_vsync, timing1_hsync, timing2_vsync, timing2_hsync;
 
@@ -758,6 +929,7 @@ static int __init omapdss_hdmihw_probe(struct platform_device *pdev)
 	hdmi.ip_data.core_av_offset = HDMI_CORE_AV;
 	hdmi.ip_data.pll_offset = HDMI_PLLCTRL;
 	hdmi.ip_data.phy_offset = HDMI_PHY;
+	mutex_init(&hdmi.ip_data.lock);
 
 	hdmi_panel_init();
 
@@ -785,7 +957,7 @@ static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
 
 static int hdmi_runtime_suspend(struct device *dev)
 {
-	clk_disable(hdmi.sys_clk);
+	clk_disable_unprepare(hdmi.sys_clk);
 
 	dispc_runtime_put();
 
@@ -800,7 +972,7 @@ static int hdmi_runtime_resume(struct device *dev)
 	if (r < 0)
 		return r;
 
-	clk_enable(hdmi.sys_clk);
+	clk_prepare_enable(hdmi.sys_clk);
 
 	return 0;
 }
diff --git a/drivers/video/omap2/dss/hdmi_panel.c b/drivers/video/omap2/dss/hdmi_panel.c
index 1179e3c4b1c..e10844faadf 100644
--- a/drivers/video/omap2/dss/hdmi_panel.c
+++ b/drivers/video/omap2/dss/hdmi_panel.c
@@ -43,10 +43,11 @@ static int hdmi_panel_probe(struct omap_dss_device *dssdev)
 {
 	DSSDBG("ENTER hdmi_panel_probe\n");
 
-	dssdev->panel.config = OMAP_DSS_LCD_TFT |
-			OMAP_DSS_LCD_IVS | OMAP_DSS_LCD_IHS;
-
-	dssdev->panel.timings = (struct omap_video_timings){640, 480, 25175, 96, 16, 48, 2 , 11, 31};
+	dssdev->panel.timings = (struct omap_video_timings)
+			{ 640, 480, 25175, 96, 16, 48, 2, 11, 31,
+				OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
+				false,
+			};
 
 	DSSDBG("hdmi_panel_probe x_res= %d y_res = %d\n",
 		dssdev->panel.timings.x_res,
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 0cbcde4c688..53710fadc82 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -500,16 +500,12 @@ static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
 	if (r)
 		return r;
 
-	if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC) {
+	if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC)
 		irq = DISPC_IRQ_EVSYNC_ODD;
-	} else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI) {
+	else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI)
 		irq = DISPC_IRQ_EVSYNC_EVEN;
-	} else {
-		if (mgr->id == OMAP_DSS_CHANNEL_LCD)
-			irq = DISPC_IRQ_VSYNC;
-		else
-			irq = DISPC_IRQ_VSYNC2;
-	}
+	else
+		irq = dispc_mgr_get_vsync_irq(mgr->id);
 
 	r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
 
@@ -545,6 +541,10 @@ int dss_init_overlay_managers(struct platform_device *pdev)
 			mgr->name = "lcd2";
 			mgr->id = OMAP_DSS_CHANNEL_LCD2;
 			break;
+		case 3:
+			mgr->name = "lcd3";
+			mgr->id = OMAP_DSS_CHANNEL_LCD3;
+			break;
 		}
 
 		mgr->set_device = &dss_mgr_set_device;
@@ -665,9 +665,40 @@ int dss_mgr_check_timings(struct omap_overlay_manager *mgr,
 	return 0;
 }
 
+static int dss_mgr_check_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	struct dispc_clock_info cinfo = config->clock_info;
+	int dl = config->video_port_width;
+	bool stallmode = config->stallmode;
+	bool fifohandcheck = config->fifohandcheck;
+
+	if (cinfo.lck_div < 1 || cinfo.lck_div > 255)
+		return -EINVAL;
+
+	if (cinfo.pck_div < 1 || cinfo.pck_div > 255)
+		return -EINVAL;
+
+	if (dl != 12 && dl != 16 && dl != 18 && dl != 24)
+		return -EINVAL;
+
+	/* fifohandcheck should be used only with stallmode */
+	if (stallmode == false && fifohandcheck == true)
+		return -EINVAL;
+
+	/*
+	 * io pad mode can be only checked by using dssdev connected to the
+	 * manager. Ignore checking these for now, add checks when manager
+	 * is capable of holding information related to the connected interface
+	 */
+
+	return 0;
+}
+
 int dss_mgr_check(struct omap_overlay_manager *mgr,
 		struct omap_overlay_manager_info *info,
 		const struct omap_video_timings *mgr_timings,
+		const struct dss_lcd_mgr_config *lcd_config,
 		struct omap_overlay_info **overlay_infos)
 {
 	struct omap_overlay *ovl;
@@ -683,6 +714,10 @@ int dss_mgr_check(struct omap_overlay_manager *mgr,
 	if (r)
 		return r;
 
+	r = dss_mgr_check_lcd_config(mgr, lcd_config);
+	if (r)
+		return r;
+
 	list_for_each_entry(ovl, &mgr->overlays, list) {
 		struct omap_overlay_info *oi;
 		int r;
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index b0ba60f88dd..952c6fad9a8 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -528,14 +528,24 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
 	struct omap_overlay_manager *lcd_mgr;
 	struct omap_overlay_manager *tv_mgr;
 	struct omap_overlay_manager *lcd2_mgr = NULL;
+	struct omap_overlay_manager *lcd3_mgr = NULL;
 	struct omap_overlay_manager *mgr = NULL;
 
-	lcd_mgr = omap_dss_get_overlay_manager(OMAP_DSS_OVL_MGR_LCD);
-	tv_mgr = omap_dss_get_overlay_manager(OMAP_DSS_OVL_MGR_TV);
+	lcd_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD);
+	tv_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_DIGIT);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		lcd3_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD3);
 	if (dss_has_feature(FEAT_MGR_LCD2))
-		lcd2_mgr = omap_dss_get_overlay_manager(OMAP_DSS_OVL_MGR_LCD2);
-
-	if (dssdev->channel == OMAP_DSS_CHANNEL_LCD2) {
+		lcd2_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD2);
+
+	if (dssdev->channel == OMAP_DSS_CHANNEL_LCD3) {
+		if (!lcd3_mgr->device || force) {
+			if (lcd3_mgr->device)
+				lcd3_mgr->unset_device(lcd3_mgr);
+			lcd3_mgr->set_device(lcd3_mgr, dssdev);
+			mgr = lcd3_mgr;
+		}
+	} else if (dssdev->channel == OMAP_DSS_CHANNEL_LCD2) {
 		if (!lcd2_mgr->device || force) {
 			if (lcd2_mgr->device)
 				lcd2_mgr->unset_device(lcd2_mgr);
@@ -677,3 +687,16 @@ int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
 
 	return 0;
 }
+
+/*
+ * Checks if replication logic should be used. Only use when overlay is in
+ * RGB12U or RGB16 mode, and video port width interface is 18bpp or 24bpp
+ */
+bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
+		enum omap_color_mode mode)
+{
+	if (mode != OMAP_DSS_COLOR_RGB12U && mode != OMAP_DSS_COLOR_RGB16)
+		return false;
+
+	return config.video_port_width > 16;
+}
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index 7985fa12b9b..7c087424b63 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -300,10 +300,11 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
 }
 EXPORT_SYMBOL(omap_rfbi_write_pixels);
 
-static void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
+static int rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
 		u16 height, void (*callback)(void *data), void *data)
 {
 	u32 l;
+	int r;
 	struct omap_video_timings timings = {
 		.hsw		= 1,
 		.hfp		= 1,
@@ -322,7 +323,9 @@ static void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
 
 	dss_mgr_set_timings(dssdev->manager, &timings);
 
-	dispc_mgr_enable(dssdev->manager->id, true);
+	r = dss_mgr_enable(dssdev->manager);
+	if (r)
+		return r;
 
 	rfbi.framedone_callback = callback;
 	rfbi.framedone_callback_data = data;
@@ -335,6 +338,8 @@ static void rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
 		l = FLD_MOD(l, 1, 4, 4); /* ITE */
 
 	rfbi_write_reg(RFBI_CONTROL, l);
+
+	return 0;
 }
 
 static void framedone_callback(void *data, u32 mask)
@@ -814,8 +819,11 @@ int omap_rfbi_update(struct omap_dss_device *dssdev,
 		u16 x, u16 y, u16 w, u16 h,
 		void (*callback)(void *), void *data)
 {
-	rfbi_transfer_area(dssdev, w, h, callback, data);
-	return 0;
+	int r;
+
+	r = rfbi_transfer_area(dssdev, w, h, callback, data);
+
+	return r;
 }
 EXPORT_SYMBOL(omap_rfbi_update);
 
@@ -859,6 +867,22 @@ static void rfbi_dump_regs(struct seq_file *s)
 #undef DUMPREG
 }
 
+static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
+{
+	struct dss_lcd_mgr_config mgr_config;
+
+	mgr_config.io_pad_mode = DSS_IO_PAD_MODE_RFBI;
+
+	mgr_config.stallmode = true;
+	/* Do we need fifohandcheck for RFBI? */
+	mgr_config.fifohandcheck = false;
+
+	mgr_config.video_port_width = dssdev->ctrl.pixel_size;
+	mgr_config.lcden_sig_polarity = 0;
+
+	dss_mgr_set_lcd_config(dssdev->manager, &mgr_config);
+}
+
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
 	int r;
@@ -885,13 +909,7 @@ int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 		goto err1;
 	}
 
-	dispc_mgr_set_lcd_display_type(dssdev->manager->id,
-			OMAP_DSS_LCD_DISPLAY_TFT);
-
-	dispc_mgr_set_io_pad_mode(DSS_IO_PAD_MODE_RFBI);
-	dispc_mgr_enable_stallmode(dssdev->manager->id, true);
-
-	dispc_mgr_set_tft_data_lines(dssdev->manager->id, dssdev->ctrl.pixel_size);
+	rfbi_config_lcd_manager(dssdev);
 
 	rfbi_configure(dssdev->phy.rfbi.channel,
 			       dssdev->ctrl.pixel_size,
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index 3a43dc2a9b4..5d31699fbd3 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -32,19 +32,21 @@
 static struct {
 	bool update_enabled;
 	struct regulator *vdds_sdi_reg;
-} sdi;
 
-static void sdi_basic_init(struct omap_dss_device *dssdev)
+	struct dss_lcd_mgr_config mgr_config;
+} sdi;
 
+static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
-	dispc_mgr_set_io_pad_mode(DSS_IO_PAD_MODE_BYPASS);
-	dispc_mgr_enable_stallmode(dssdev->manager->id, false);
+	sdi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+
+	sdi.mgr_config.stallmode = false;
+	sdi.mgr_config.fifohandcheck = false;
 
-	dispc_mgr_set_lcd_display_type(dssdev->manager->id,
-			OMAP_DSS_LCD_DISPLAY_TFT);
+	sdi.mgr_config.video_port_width = 24;
+	sdi.mgr_config.lcden_sig_polarity = 1;
 
-	dispc_mgr_set_tft_data_lines(dssdev->manager->id, 24);
-	dispc_lcd_enable_signal_polarity(1);
+	dss_mgr_set_lcd_config(dssdev->manager, &sdi.mgr_config);
 }
 
 int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
@@ -52,8 +54,6 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	struct omap_video_timings *t = &dssdev->panel.timings;
 	struct dss_clock_info dss_cinfo;
 	struct dispc_clock_info dispc_cinfo;
-	u16 lck_div, pck_div;
-	unsigned long fck;
 	unsigned long pck;
 	int r;
 
@@ -76,24 +76,17 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_get_dispc;
 
-	sdi_basic_init(dssdev);
-
 	/* 15.5.9.1.2 */
-	dssdev->panel.config |= OMAP_DSS_LCD_RF | OMAP_DSS_LCD_ONOFF;
-
-	dispc_mgr_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
-			dssdev->panel.acbi, dssdev->panel.acb);
+	dssdev->panel.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	dssdev->panel.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
 
-	r = dss_calc_clock_div(1, t->pixel_clock * 1000,
-			&dss_cinfo, &dispc_cinfo);
+	r = dss_calc_clock_div(t->pixel_clock * 1000, &dss_cinfo, &dispc_cinfo);
 	if (r)
 		goto err_calc_clock_div;
 
-	fck = dss_cinfo.fck;
-	lck_div = dispc_cinfo.lck_div;
-	pck_div = dispc_cinfo.pck_div;
+	sdi.mgr_config.clock_info = dispc_cinfo;
 
-	pck = fck / lck_div / pck_div / 1000;
+	pck = dss_cinfo.fck / dispc_cinfo.lck_div / dispc_cinfo.pck_div / 1000;
 
 	if (pck != t->pixel_clock) {
 		DSSWARN("Could not find exact pixel clock. Requested %d kHz, "
@@ -110,9 +103,7 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_set_dss_clock_div;
 
-	r = dispc_mgr_set_clock_div(dssdev->manager->id, &dispc_cinfo);
-	if (r)
-		goto err_set_dispc_clock_div;
+	sdi_config_lcd_manager(dssdev);
 
 	dss_sdi_init(dssdev->phy.sdi.datapairs);
 	r = dss_sdi_enable();
@@ -129,7 +120,6 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 err_mgr_enable:
 	dss_sdi_disable();
 err_sdi_enable:
-err_set_dispc_clock_div:
 err_set_dss_clock_div:
 err_calc_clock_div:
 	dispc_runtime_put();
diff --git a/drivers/video/omap2/dss/ti_hdmi.h b/drivers/video/omap2/dss/ti_hdmi.h
index e734cb444bc..b046c208cb9 100644
--- a/drivers/video/omap2/dss/ti_hdmi.h
+++ b/drivers/video/omap2/dss/ti_hdmi.h
@@ -42,30 +42,13 @@ enum hdmi_clk_refsel {
 	HDMI_REFSEL_SYSCLK = 3
 };
 
-/* HDMI timing structure */
-struct hdmi_video_timings {
-	u16 x_res;
-	u16 y_res;
-	/* Unit: KHz */
-	u32 pixel_clock;
-	u16 hsw;
-	u16 hfp;
-	u16 hbp;
-	u16 vsw;
-	u16 vfp;
-	u16 vbp;
-	bool vsync_pol;
-	bool hsync_pol;
-	bool interlace;
-};
-
 struct hdmi_cm {
 	int	code;
 	int	mode;
 };
 
 struct hdmi_config {
-	struct hdmi_video_timings timings;
+	struct omap_video_timings timings;
 	struct hdmi_cm cm;
 };
 
@@ -177,7 +160,7 @@ struct hdmi_ip_data {
 
 	/* ti_hdmi_4xxx_ip private data. These should be in a separate struct */
 	int hpd_gpio;
-	bool phy_tx_enabled;
+	struct mutex lock;
 };
 int ti_hdmi_4xxx_phy_enable(struct hdmi_ip_data *ip_data);
 void ti_hdmi_4xxx_phy_disable(struct hdmi_ip_data *ip_data);
diff --git a/drivers/video/omap2/dss/ti_hdmi_4xxx_ip.c b/drivers/video/omap2/dss/ti_hdmi_4xxx_ip.c
index 4dae1b29107..c23b85a20cd 100644
--- a/drivers/video/omap2/dss/ti_hdmi_4xxx_ip.c
+++ b/drivers/video/omap2/dss/ti_hdmi_4xxx_ip.c
@@ -157,6 +157,10 @@ static int hdmi_pll_init(struct hdmi_ip_data *ip_data)
 /* PHY_PWR_CMD */
 static int hdmi_set_phy_pwr(struct hdmi_ip_data *ip_data, enum hdmi_phy_pwr val)
 {
+	/* Return if already the state */
+	if (REG_GET(hdmi_wp_base(ip_data), HDMI_WP_PWR_CTRL, 5, 4) == val)
+		return 0;
+
 	/* Command for power control of HDMI PHY */
 	REG_FLD_MOD(hdmi_wp_base(ip_data), HDMI_WP_PWR_CTRL, val, 7, 6);
 
@@ -231,21 +235,13 @@ void ti_hdmi_4xxx_pll_disable(struct hdmi_ip_data *ip_data)
 
 static int hdmi_check_hpd_state(struct hdmi_ip_data *ip_data)
 {
-	unsigned long flags;
 	bool hpd;
 	int r;
-	/* this should be in ti_hdmi_4xxx_ip private data */
-	static DEFINE_SPINLOCK(phy_tx_lock);
 
-	spin_lock_irqsave(&phy_tx_lock, flags);
+	mutex_lock(&ip_data->lock);
 
 	hpd = gpio_get_value(ip_data->hpd_gpio);
 
-	if (hpd == ip_data->phy_tx_enabled) {
-		spin_unlock_irqrestore(&phy_tx_lock, flags);
-		return 0;
-	}
-
 	if (hpd)
 		r = hdmi_set_phy_pwr(ip_data, HDMI_PHYPWRCMD_TXON);
 	else
@@ -257,9 +253,8 @@ static int hdmi_check_hpd_state(struct hdmi_ip_data *ip_data)
 		goto err;
 	}
 
-	ip_data->phy_tx_enabled = hpd;
 err:
-	spin_unlock_irqrestore(&phy_tx_lock, flags);
+	mutex_unlock(&ip_data->lock);
 	return r;
 }
 
@@ -327,7 +322,6 @@ void ti_hdmi_4xxx_phy_disable(struct hdmi_ip_data *ip_data)
 	free_irq(gpio_to_irq(ip_data->hpd_gpio), ip_data);
 
 	hdmi_set_phy_pwr(ip_data, HDMI_PHYPWRCMD_OFF);
-	ip_data->phy_tx_enabled = false;
 }
 
 static int hdmi_core_ddc_init(struct hdmi_ip_data *ip_data)
@@ -747,11 +741,15 @@ static void hdmi_wp_video_config_format(struct hdmi_ip_data *ip_data,
 static void hdmi_wp_video_config_interface(struct hdmi_ip_data *ip_data)
 {
 	u32 r;
+	bool vsync_pol, hsync_pol;
 	pr_debug("Enter hdmi_wp_video_config_interface\n");
 
+	vsync_pol = ip_data->cfg.timings.vsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+	hsync_pol = ip_data->cfg.timings.hsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+
 	r = hdmi_read_reg(hdmi_wp_base(ip_data), HDMI_WP_VIDEO_CFG);
-	r = FLD_MOD(r, ip_data->cfg.timings.vsync_pol, 7, 7);
-	r = FLD_MOD(r, ip_data->cfg.timings.hsync_pol, 6, 6);
+	r = FLD_MOD(r, vsync_pol, 7, 7);
+	r = FLD_MOD(r, hsync_pol, 6, 6);
 	r = FLD_MOD(r, ip_data->cfg.timings.interlace, 3, 3);
 	r = FLD_MOD(r, 1, 1, 0); /* HDMI_TIMING_MASTER_24BIT */
 	hdmi_write_reg(hdmi_wp_base(ip_data), HDMI_WP_VIDEO_CFG, r);
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index 3907c8b6ecb..3a220877461 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -272,6 +272,8 @@ const struct omap_video_timings omap_dss_pal_timings = {
 	.vsw		= 5,
 	.vfp		= 5,
 	.vbp		= 41,
+
+	.interlace	= true,
 };
 EXPORT_SYMBOL(omap_dss_pal_timings);
 
@@ -285,6 +287,8 @@ const struct omap_video_timings omap_dss_ntsc_timings = {
 	.vsw		= 6,
 	.vfp		= 6,
 	.vbp		= 31,
+
+	.interlace	= true,
 };
 EXPORT_SYMBOL(omap_dss_ntsc_timings);
 
@@ -930,7 +934,7 @@ static int __exit omap_venchw_remove(struct platform_device *pdev)
 static int venc_runtime_suspend(struct device *dev)
 {
 	if (venc.tv_dac_clk)
-		clk_disable(venc.tv_dac_clk);
+		clk_disable_unprepare(venc.tv_dac_clk);
 
 	dispc_runtime_put();
 
@@ -946,7 +950,7 @@ static int venc_runtime_resume(struct device *dev)
 		return r;
 
 	if (venc.tv_dac_clk)
-		clk_enable(venc.tv_dac_clk);
+		clk_prepare_enable(venc.tv_dac_clk);
 
 	return 0;
 }
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 3450ea0966c..08ec1a7103f 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -733,6 +733,12 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
 		var->lower_margin = timings.vfp;
 		var->hsync_len = timings.hsw;
 		var->vsync_len = timings.vsw;
+		var->sync |= timings.hsync_level == OMAPDSS_SIG_ACTIVE_HIGH ?
+				FB_SYNC_HOR_HIGH_ACT : 0;
+		var->sync |= timings.vsync_level == OMAPDSS_SIG_ACTIVE_HIGH ?
+				FB_SYNC_VERT_HIGH_ACT : 0;
+		var->vmode = timings.interlace ?
+				FB_VMODE_INTERLACED : FB_VMODE_NONINTERLACED;
 	} else {
 		var->pixclock = 0;
 		var->left_margin = 0;
@@ -741,12 +747,10 @@ int check_fb_var(struct fb_info *fbi, struct fb_var_screeninfo *var)
 		var->lower_margin = 0;
 		var->hsync_len = 0;
 		var->vsync_len = 0;
+		var->sync = 0;
+		var->vmode = FB_VMODE_NONINTERLACED;
 	}
 
-	/* TODO: get these from panel->config */
-	var->vmode              = FB_VMODE_NONINTERLACED;
-	var->sync               = 0;
-
 	return 0;
 }
 
@@ -1993,6 +1997,7 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
 }
 
 static int omapfb_mode_to_timings(const char *mode_str,
+		struct omap_dss_device *display,
 		struct omap_video_timings *timings, u8 *bpp)
 {
 	struct fb_info *fbi;
@@ -2046,6 +2051,14 @@ static int omapfb_mode_to_timings(const char *mode_str,
 		goto err;
 	}
 
+	if (display->driver->get_timings) {
+		display->driver->get_timings(display, timings);
+	} else {
+		timings->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+		timings->de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+		timings->sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+	}
+
 	timings->pixel_clock = PICOS2KHZ(var->pixclock);
 	timings->hbp = var->left_margin;
 	timings->hfp = var->right_margin;
@@ -2055,6 +2068,13 @@ static int omapfb_mode_to_timings(const char *mode_str,
 	timings->vsw = var->vsync_len;
 	timings->x_res = var->xres;
 	timings->y_res = var->yres;
+	timings->hsync_level = var->sync & FB_SYNC_HOR_HIGH_ACT ?
+				OMAPDSS_SIG_ACTIVE_HIGH :
+				OMAPDSS_SIG_ACTIVE_LOW;
+	timings->vsync_level = var->sync & FB_SYNC_VERT_HIGH_ACT ?
+				OMAPDSS_SIG_ACTIVE_HIGH :
+				OMAPDSS_SIG_ACTIVE_LOW;
+	timings->interlace = var->vmode & FB_VMODE_INTERLACED;
 
 	switch (var->bits_per_pixel) {
 	case 16:
@@ -2085,7 +2105,7 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 	struct omap_video_timings timings, temp_timings;
 	struct omapfb_display_data *d;
 
-	r = omapfb_mode_to_timings(mode_str, &timings, &bpp);
+	r = omapfb_mode_to_timings(mode_str, display, &timings, &bpp);
 	if (r)
 		return r;
 
@@ -2178,8 +2198,17 @@ static int omapfb_parse_def_modes(struct omapfb2_device *fbdev)
 }
 
 static void fb_videomode_to_omap_timings(struct fb_videomode *m,
+		struct omap_dss_device *display,
 		struct omap_video_timings *t)
 {
+	if (display->driver->get_timings) {
+		display->driver->get_timings(display, t);
+	} else {
+		t->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+		t->de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+		t->sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+	}
+
 	t->x_res = m->xres;
 	t->y_res = m->yres;
 	t->pixel_clock = PICOS2KHZ(m->pixclock);
@@ -2189,6 +2218,13 @@ static void fb_videomode_to_omap_timings(struct fb_videomode *m,
 	t->vsw = m->vsync_len;
 	t->vfp = m->lower_margin;
 	t->vbp = m->upper_margin;
+	t->hsync_level = m->sync & FB_SYNC_HOR_HIGH_ACT ?
+				OMAPDSS_SIG_ACTIVE_HIGH :
+				OMAPDSS_SIG_ACTIVE_LOW;
+	t->vsync_level = m->sync & FB_SYNC_VERT_HIGH_ACT ?
+				OMAPDSS_SIG_ACTIVE_HIGH :
+				OMAPDSS_SIG_ACTIVE_LOW;
+	t->interlace = m->vmode & FB_VMODE_INTERLACED;
 }
 
 static int omapfb_find_best_mode(struct omap_dss_device *display,
@@ -2231,7 +2267,7 @@ static int omapfb_find_best_mode(struct omap_dss_device *display,
 		if (m->xres == 2880 || m->xres == 1440)
 			continue;
 
-		fb_videomode_to_omap_timings(m, &t);
+		fb_videomode_to_omap_timings(m, display, &t);
 
 		r = display->driver->check_timings(display, &t);
 		if (r == 0 && best_xres < m->xres) {
@@ -2245,7 +2281,8 @@ static int omapfb_find_best_mode(struct omap_dss_device *display,
 		goto err2;
 	}
 
-	fb_videomode_to_omap_timings(&specs->modedb[best_idx], timings);
+	fb_videomode_to_omap_timings(&specs->modedb[best_idx], display,
+		timings);
 
 	r = 0;
 
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index ea7b661e722..69bf9d07c23 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -189,7 +189,7 @@ struct s3c_fb_vsync {
 
 /**
  * struct s3c_fb - overall hardware state of the hardware
- * @slock: The spinlock protection for this data sturcture.
+ * @slock: The spinlock protection for this data sturucture.
  * @dev: The device that we bound to, for printing, etc.
  * @bus_clk: The clk (hclk) feeding our interface and possibly pixclk.
  * @lcd_clk: The clk (sclk) feeding pixclk.
diff --git a/drivers/video/s3fb.c b/drivers/video/s3fb.c
index 2c80246b18b..1d007366b91 100644
--- a/drivers/video/s3fb.c
+++ b/drivers/video/s3fb.c
@@ -84,7 +84,7 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 			"S3 Virge/VX", "S3 Virge/DX", "S3 Virge/GX",
 			"S3 Virge/GX2", "S3 Virge/GX2+", "",
 			"S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X",
-			"S3 Trio3D"};
+			"S3 Trio3D", "S3 Virge/MX"};
 
 #define CHIP_UNKNOWN		0x00
 #define CHIP_732_TRIO32		0x01
@@ -105,6 +105,7 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_362_TRIO3D_2X	0x11
 #define CHIP_368_TRIO3D_2X	0x12
 #define CHIP_365_TRIO3D		0x13
+#define CHIP_260_VIRGE_MX	0x14
 
 #define CHIP_XXX_TRIO		0x80
 #define CHIP_XXX_TRIO64V2_DXGX	0x81
@@ -280,7 +281,8 @@ static int __devinit s3fb_setup_ddc_bus(struct fb_info *info)
 	 */
 /*	vga_wseq(par->state.vgabase, 0x08, 0x06); - not needed, already unlocked */
 	if (par->chip == CHIP_357_VIRGE_GX2 ||
-	    par->chip == CHIP_359_VIRGE_GX2P)
+	    par->chip == CHIP_359_VIRGE_GX2P ||
+	    par->chip == CHIP_260_VIRGE_MX)
 		svga_wseq_mask(par->state.vgabase, 0x0d, 0x01, 0x03);
 	else
 		svga_wseq_mask(par->state.vgabase, 0x0d, 0x00, 0x03);
@@ -487,7 +489,8 @@ static void s3_set_pixclock(struct fb_info *info, u32 pixclock)
 	    par->chip == CHIP_359_VIRGE_GX2P ||
 	    par->chip == CHIP_360_TRIO3D_1X ||
 	    par->chip == CHIP_362_TRIO3D_2X ||
-	    par->chip == CHIP_368_TRIO3D_2X) {
+	    par->chip == CHIP_368_TRIO3D_2X ||
+	    par->chip == CHIP_260_VIRGE_MX) {
 		vga_wseq(par->state.vgabase, 0x12, (n - 2) | ((r & 3) << 6));	/* n and two bits of r */
 		vga_wseq(par->state.vgabase, 0x29, r >> 2); /* remaining highest bit of r */
 	} else
@@ -690,7 +693,8 @@ static int s3fb_set_par(struct fb_info *info)
 	    par->chip != CHIP_359_VIRGE_GX2P &&
 	    par->chip != CHIP_360_TRIO3D_1X &&
 	    par->chip != CHIP_362_TRIO3D_2X &&
-	    par->chip != CHIP_368_TRIO3D_2X) {
+	    par->chip != CHIP_368_TRIO3D_2X &&
+	    par->chip != CHIP_260_VIRGE_MX) {
 		vga_wcrt(par->state.vgabase, 0x54, 0x18); /* M parameter */
 		vga_wcrt(par->state.vgabase, 0x60, 0xff); /* N parameter */
 		vga_wcrt(par->state.vgabase, 0x61, 0xff); /* L parameter */
@@ -739,7 +743,8 @@ static int s3fb_set_par(struct fb_info *info)
 	    par->chip == CHIP_368_TRIO3D_2X ||
 	    par->chip == CHIP_365_TRIO3D    ||
 	    par->chip == CHIP_375_VIRGE_DX  ||
-	    par->chip == CHIP_385_VIRGE_GX) {
+	    par->chip == CHIP_385_VIRGE_GX  ||
+	    par->chip == CHIP_260_VIRGE_MX) {
 		dbytes = info->var.xres * ((bpp+7)/8);
 		vga_wcrt(par->state.vgabase, 0x91, (dbytes + 7) / 8);
 		vga_wcrt(par->state.vgabase, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
@@ -751,7 +756,8 @@ static int s3fb_set_par(struct fb_info *info)
 	    par->chip == CHIP_359_VIRGE_GX2P ||
 	    par->chip == CHIP_360_TRIO3D_1X ||
 	    par->chip == CHIP_362_TRIO3D_2X ||
-	    par->chip == CHIP_368_TRIO3D_2X)
+	    par->chip == CHIP_368_TRIO3D_2X ||
+	    par->chip == CHIP_260_VIRGE_MX)
 		vga_wcrt(par->state.vgabase, 0x34, 0x00);
 	else	/* enable Data Transfer Position Control (DTPC) */
 		vga_wcrt(par->state.vgabase, 0x34, 0x10);
@@ -807,7 +813,8 @@ static int s3fb_set_par(struct fb_info *info)
 		    par->chip == CHIP_359_VIRGE_GX2P ||
 		    par->chip == CHIP_360_TRIO3D_1X ||
 		    par->chip == CHIP_362_TRIO3D_2X ||
-		    par->chip == CHIP_368_TRIO3D_2X)
+		    par->chip == CHIP_368_TRIO3D_2X ||
+		    par->chip == CHIP_260_VIRGE_MX)
 			svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 		else {
 			svga_wcrt_mask(par->state.vgabase, 0x67, 0x10, 0xF0);
@@ -837,7 +844,8 @@ static int s3fb_set_par(struct fb_info *info)
 			    par->chip != CHIP_359_VIRGE_GX2P &&
 			    par->chip != CHIP_360_TRIO3D_1X &&
 			    par->chip != CHIP_362_TRIO3D_2X &&
-			    par->chip != CHIP_368_TRIO3D_2X)
+			    par->chip != CHIP_368_TRIO3D_2X &&
+			    par->chip != CHIP_260_VIRGE_MX)
 				hmul = 2;
 		}
 		break;
@@ -864,7 +872,8 @@ static int s3fb_set_par(struct fb_info *info)
 			    par->chip != CHIP_359_VIRGE_GX2P &&
 			    par->chip != CHIP_360_TRIO3D_1X &&
 			    par->chip != CHIP_362_TRIO3D_2X &&
-			    par->chip != CHIP_368_TRIO3D_2X)
+			    par->chip != CHIP_368_TRIO3D_2X &&
+			    par->chip != CHIP_260_VIRGE_MX)
 				hmul = 2;
 		}
 		break;
@@ -1208,7 +1217,8 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
 			break;
 		}
 	} else if (par->chip == CHIP_357_VIRGE_GX2 ||
-		   par->chip == CHIP_359_VIRGE_GX2P) {
+		   par->chip == CHIP_359_VIRGE_GX2P ||
+		   par->chip == CHIP_260_VIRGE_MX) {
 		switch ((regval & 0xC0) >> 6) {
 		case 1: /* 4MB */
 			info->screen_size = 4 << 20;
@@ -1515,6 +1525,7 @@ static struct pci_device_id s3_devices[] __devinitdata = {
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A12), .driver_data = CHIP_359_VIRGE_GX2P},
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A13), .driver_data = CHIP_36X_TRIO3D_1X_2X},
 	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8904), .driver_data = CHIP_365_TRIO3D},
+	{PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8C01), .driver_data = CHIP_260_VIRGE_MX},
 
 	{0, 0, 0, 0, 0, 0, 0}
 };
diff --git a/drivers/video/savage/savagefb_driver.c b/drivers/video/savage/savagefb_driver.c
index f3d3b9ce475..0d0f52c18fd 100644
--- a/drivers/video/savage/savagefb_driver.c
+++ b/drivers/video/savage/savagefb_driver.c
@@ -662,7 +662,7 @@ static void savage_get_default_par(struct savagefb_par *par, struct savage_reg *
 	vga_out8(0x3c4, 0x18, par);
 	reg->SR18 = vga_in8(0x3c5, par);
 
-	/* Save flat panel expansion regsters. */
+	/* Save flat panel expansion registers. */
 	if (par->chip == S3_SAVAGE_MX) {
 		int i;
 
@@ -815,7 +815,7 @@ static void savage_set_default_par(struct savagefb_par *par,
 	vga_out8(0x3c4, 0x18, par);
 	vga_out8(0x3c5, reg->SR18, par);
 
-	/* Save flat panel expansion regsters. */
+	/* Save flat panel expansion registers. */
 	if (par->chip == S3_SAVAGE_MX) {
 		int i;
 
@@ -1318,7 +1318,7 @@ static void savagefb_set_par_int(struct savagefb_par  *par, struct savage_reg *r
 	vga_out8(0x3c4, 0x15, par);
 	vga_out8(0x3c5, reg->SR15, par);
 
-	/* Restore flat panel expansion regsters. */
+	/* Restore flat panel expansion registers. */
 	if (par->chip == S3_SAVAGE_MX) {
 		int i;
 
diff --git a/drivers/video/sh_mipi_dsi.c b/drivers/video/sh_mipi_dsi.c
index 4c6b8448856..3951fdae5f6 100644
--- a/drivers/video/sh_mipi_dsi.c
+++ b/drivers/video/sh_mipi_dsi.c
@@ -127,8 +127,7 @@ static void sh_mipi_shutdown(struct platform_device *pdev)
 	sh_mipi_dsi_enable(mipi, false);
 }
 
-static int __init sh_mipi_setup(struct sh_mipi *mipi,
-				struct sh_mipi_dsi_info *pdata)
+static int sh_mipi_setup(struct sh_mipi *mipi, struct sh_mipi_dsi_info *pdata)
 {
 	void __iomem *base = mipi->base;
 	struct sh_mobile_lcdc_chan_cfg *ch = pdata->lcd_chan;
@@ -551,7 +550,7 @@ efindslot:
 	return ret;
 }
 
-static int __exit sh_mipi_remove(struct platform_device *pdev)
+static int __devexit sh_mipi_remove(struct platform_device *pdev)
 {
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	struct resource *res2 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
@@ -592,7 +591,7 @@ static int __exit sh_mipi_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver sh_mipi_driver = {
-	.remove		= __exit_p(sh_mipi_remove),
+	.remove		= __devexit_p(sh_mipi_remove),
 	.shutdown	= sh_mipi_shutdown,
 	.driver = {
 		.name	= "sh-mipi-dsi",
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index e672698bd82..699487c287b 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -12,6 +12,7 @@
 #include <linux/backlight.h>
 #include <linux/clk.h>
 #include <linux/console.h>
+#include <linux/ctype.h>
 #include <linux/dma-mapping.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
@@ -32,12 +33,176 @@
 
 #include "sh_mobile_lcdcfb.h"
 
+/* ----------------------------------------------------------------------------
+ * Overlay register definitions
+ */
+
+#define LDBCR			0xb00
+#define LDBCR_UPC(n)		(1 << ((n) + 16))
+#define LDBCR_UPF(n)		(1 << ((n) + 8))
+#define LDBCR_UPD(n)		(1 << ((n) + 0))
+#define LDBnBSIFR(n)		(0xb20 + (n) * 0x20 + 0x00)
+#define LDBBSIFR_EN		(1 << 31)
+#define LDBBSIFR_VS		(1 << 29)
+#define LDBBSIFR_BRSEL		(1 << 28)
+#define LDBBSIFR_MX		(1 << 27)
+#define LDBBSIFR_MY		(1 << 26)
+#define LDBBSIFR_CV3		(3 << 24)
+#define LDBBSIFR_CV2		(2 << 24)
+#define LDBBSIFR_CV1		(1 << 24)
+#define LDBBSIFR_CV0		(0 << 24)
+#define LDBBSIFR_CV_MASK	(3 << 24)
+#define LDBBSIFR_LAY_MASK	(0xff << 16)
+#define LDBBSIFR_LAY_SHIFT	16
+#define LDBBSIFR_ROP3_MASK	(0xff << 16)
+#define LDBBSIFR_ROP3_SHIFT	16
+#define LDBBSIFR_AL_PL8		(3 << 14)
+#define LDBBSIFR_AL_PL1		(2 << 14)
+#define LDBBSIFR_AL_PK		(1 << 14)
+#define LDBBSIFR_AL_1		(0 << 14)
+#define LDBBSIFR_AL_MASK	(3 << 14)
+#define LDBBSIFR_SWPL		(1 << 10)
+#define LDBBSIFR_SWPW		(1 << 9)
+#define LDBBSIFR_SWPB		(1 << 8)
+#define LDBBSIFR_RY		(1 << 7)
+#define LDBBSIFR_CHRR_420	(2 << 0)
+#define LDBBSIFR_CHRR_422	(1 << 0)
+#define LDBBSIFR_CHRR_444	(0 << 0)
+#define LDBBSIFR_RPKF_ARGB32	(0x00 << 0)
+#define LDBBSIFR_RPKF_RGB16	(0x03 << 0)
+#define LDBBSIFR_RPKF_RGB24	(0x0b << 0)
+#define LDBBSIFR_RPKF_MASK	(0x1f << 0)
+#define LDBnBSSZR(n)		(0xb20 + (n) * 0x20 + 0x04)
+#define LDBBSSZR_BVSS_MASK	(0xfff << 16)
+#define LDBBSSZR_BVSS_SHIFT	16
+#define LDBBSSZR_BHSS_MASK	(0xfff << 0)
+#define LDBBSSZR_BHSS_SHIFT	0
+#define LDBnBLOCR(n)		(0xb20 + (n) * 0x20 + 0x08)
+#define LDBBLOCR_CVLC_MASK	(0xfff << 16)
+#define LDBBLOCR_CVLC_SHIFT	16
+#define LDBBLOCR_CHLC_MASK	(0xfff << 0)
+#define LDBBLOCR_CHLC_SHIFT	0
+#define LDBnBSMWR(n)		(0xb20 + (n) * 0x20 + 0x0c)
+#define LDBBSMWR_BSMWA_MASK	(0xffff << 16)
+#define LDBBSMWR_BSMWA_SHIFT	16
+#define LDBBSMWR_BSMW_MASK	(0xffff << 0)
+#define LDBBSMWR_BSMW_SHIFT	0
+#define LDBnBSAYR(n)		(0xb20 + (n) * 0x20 + 0x10)
+#define LDBBSAYR_FG1A_MASK	(0xff << 24)
+#define LDBBSAYR_FG1A_SHIFT	24
+#define LDBBSAYR_FG1R_MASK	(0xff << 16)
+#define LDBBSAYR_FG1R_SHIFT	16
+#define LDBBSAYR_FG1G_MASK	(0xff << 8)
+#define LDBBSAYR_FG1G_SHIFT	8
+#define LDBBSAYR_FG1B_MASK	(0xff << 0)
+#define LDBBSAYR_FG1B_SHIFT	0
+#define LDBnBSACR(n)		(0xb20 + (n) * 0x20 + 0x14)
+#define LDBBSACR_FG2A_MASK	(0xff << 24)
+#define LDBBSACR_FG2A_SHIFT	24
+#define LDBBSACR_FG2R_MASK	(0xff << 16)
+#define LDBBSACR_FG2R_SHIFT	16
+#define LDBBSACR_FG2G_MASK	(0xff << 8)
+#define LDBBSACR_FG2G_SHIFT	8
+#define LDBBSACR_FG2B_MASK	(0xff << 0)
+#define LDBBSACR_FG2B_SHIFT	0
+#define LDBnBSAAR(n)		(0xb20 + (n) * 0x20 + 0x18)
+#define LDBBSAAR_AP_MASK	(0xff << 24)
+#define LDBBSAAR_AP_SHIFT	24
+#define LDBBSAAR_R_MASK		(0xff << 16)
+#define LDBBSAAR_R_SHIFT	16
+#define LDBBSAAR_GY_MASK	(0xff << 8)
+#define LDBBSAAR_GY_SHIFT	8
+#define LDBBSAAR_B_MASK		(0xff << 0)
+#define LDBBSAAR_B_SHIFT	0
+#define LDBnBPPCR(n)		(0xb20 + (n) * 0x20 + 0x1c)
+#define LDBBPPCR_AP_MASK	(0xff << 24)
+#define LDBBPPCR_AP_SHIFT	24
+#define LDBBPPCR_R_MASK		(0xff << 16)
+#define LDBBPPCR_R_SHIFT	16
+#define LDBBPPCR_GY_MASK	(0xff << 8)
+#define LDBBPPCR_GY_SHIFT	8
+#define LDBBPPCR_B_MASK		(0xff << 0)
+#define LDBBPPCR_B_SHIFT	0
+#define LDBnBBGCL(n)		(0xb10 + (n) * 0x04)
+#define LDBBBGCL_BGA_MASK	(0xff << 24)
+#define LDBBBGCL_BGA_SHIFT	24
+#define LDBBBGCL_BGR_MASK	(0xff << 16)
+#define LDBBBGCL_BGR_SHIFT	16
+#define LDBBBGCL_BGG_MASK	(0xff << 8)
+#define LDBBBGCL_BGG_SHIFT	8
+#define LDBBBGCL_BGB_MASK	(0xff << 0)
+#define LDBBBGCL_BGB_SHIFT	0
+
 #define SIDE_B_OFFSET 0x1000
 #define MIRROR_OFFSET 0x2000
 
 #define MAX_XRES 1920
 #define MAX_YRES 1080
 
+enum sh_mobile_lcdc_overlay_mode {
+	LCDC_OVERLAY_BLEND,
+	LCDC_OVERLAY_ROP3,
+};
+
+/*
+ * struct sh_mobile_lcdc_overlay - LCDC display overlay
+ *
+ * @channel: LCDC channel this overlay belongs to
+ * @cfg: Overlay configuration
+ * @info: Frame buffer device
+ * @index: Overlay index (0-3)
+ * @base: Overlay registers base address
+ * @enabled: True if the overlay is enabled
+ * @mode: Overlay blending mode (alpha blend or ROP3)
+ * @alpha: Global alpha blending value (0-255, for alpha blending mode)
+ * @rop3: Raster operation (for ROP3 mode)
+ * @fb_mem: Frame buffer virtual memory address
+ * @fb_size: Frame buffer size in bytes
+ * @dma_handle: Frame buffer DMA address
+ * @base_addr_y: Overlay base address (RGB or luma component)
+ * @base_addr_c: Overlay base address (chroma component)
+ * @pan_y_offset: Panning linear offset in bytes (luma component)
+ * @format: Current pixelf format
+ * @xres: Horizontal visible resolution
+ * @xres_virtual: Horizontal total resolution
+ * @yres: Vertical visible resolution
+ * @yres_virtual: Vertical total resolution
+ * @pitch: Overlay line pitch
+ * @pos_x: Horizontal overlay position
+ * @pos_y: Vertical overlay position
+ */
+struct sh_mobile_lcdc_overlay {
+	struct sh_mobile_lcdc_chan *channel;
+
+	const struct sh_mobile_lcdc_overlay_cfg *cfg;
+	struct fb_info *info;
+
+	unsigned int index;
+	unsigned long base;
+
+	bool enabled;
+	enum sh_mobile_lcdc_overlay_mode mode;
+	unsigned int alpha;
+	unsigned int rop3;
+
+	void *fb_mem;
+	unsigned long fb_size;
+
+	dma_addr_t dma_handle;
+	unsigned long base_addr_y;
+	unsigned long base_addr_c;
+	unsigned long pan_y_offset;
+
+	const struct sh_mobile_lcdc_format_info *format;
+	unsigned int xres;
+	unsigned int xres_virtual;
+	unsigned int yres;
+	unsigned int yres_virtual;
+	unsigned int pitch;
+	int pos_x;
+	int pos_y;
+};
+
 struct sh_mobile_lcdc_priv {
 	void __iomem *base;
 	int irq;
@@ -45,7 +210,10 @@ struct sh_mobile_lcdc_priv {
 	struct device *dev;
 	struct clk *dot_clk;
 	unsigned long lddckr;
+
 	struct sh_mobile_lcdc_chan ch[2];
+	struct sh_mobile_lcdc_overlay overlays[4];
+
 	struct notifier_block notifier;
 	int started;
 	int forced_fourcc; /* 2 channel LCDC must share fourcc setting */
@@ -141,6 +309,13 @@ static unsigned long lcdc_read_chan(struct sh_mobile_lcdc_chan *chan,
 	return ioread32(chan->lcdc->base + chan->reg_offs[reg_nr]);
 }
 
+static void lcdc_write_overlay(struct sh_mobile_lcdc_overlay *ovl,
+			       int reg, unsigned long data)
+{
+	iowrite32(data, ovl->channel->lcdc->base + reg);
+	iowrite32(data, ovl->channel->lcdc->base + reg + SIDE_B_OFFSET);
+}
+
 static void lcdc_write(struct sh_mobile_lcdc_priv *priv,
 		       unsigned long reg_offs, unsigned long data)
 {
@@ -384,8 +559,8 @@ sh_mobile_lcdc_must_reconfigure(struct sh_mobile_lcdc_chan *ch,
 	return true;
 }
 
-static int sh_mobile_check_var(struct fb_var_screeninfo *var,
-			       struct fb_info *info);
+static int sh_mobile_lcdc_check_var(struct fb_var_screeninfo *var,
+				    struct fb_info *info);
 
 static int sh_mobile_lcdc_display_notify(struct sh_mobile_lcdc_chan *ch,
 					 enum sh_mobile_lcdc_entity_event event,
@@ -439,7 +614,7 @@ static int sh_mobile_lcdc_display_notify(struct sh_mobile_lcdc_chan *ch,
 		fb_videomode_to_var(&var, mode);
 		var.bits_per_pixel = info->var.bits_per_pixel;
 		var.grayscale = info->var.grayscale;
-		ret = sh_mobile_check_var(&var, info);
+		ret = sh_mobile_lcdc_check_var(&var, info);
 		break;
 	}
 
@@ -585,7 +760,7 @@ static irqreturn_t sh_mobile_lcdc_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int sh_mobile_wait_for_vsync(struct sh_mobile_lcdc_chan *ch)
+static int sh_mobile_lcdc_wait_for_vsync(struct sh_mobile_lcdc_chan *ch)
 {
 	unsigned long ldintr;
 	int ret;
@@ -685,8 +860,98 @@ static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
 	lcdc_write_chan(ch, LDHAJR, tmp);
 }
 
+static void sh_mobile_lcdc_overlay_setup(struct sh_mobile_lcdc_overlay *ovl)
+{
+	u32 format = 0;
+
+	if (!ovl->enabled) {
+		lcdc_write(ovl->channel->lcdc, LDBCR, LDBCR_UPC(ovl->index));
+		lcdc_write_overlay(ovl, LDBnBSIFR(ovl->index), 0);
+		lcdc_write(ovl->channel->lcdc, LDBCR,
+			   LDBCR_UPF(ovl->index) | LDBCR_UPD(ovl->index));
+		return;
+	}
+
+	ovl->base_addr_y = ovl->dma_handle;
+	ovl->base_addr_c = ovl->dma_handle
+			 + ovl->xres_virtual * ovl->yres_virtual;
+
+	switch (ovl->mode) {
+	case LCDC_OVERLAY_BLEND:
+		format = LDBBSIFR_EN | (ovl->alpha << LDBBSIFR_LAY_SHIFT);
+		break;
+
+	case LCDC_OVERLAY_ROP3:
+		format = LDBBSIFR_EN | LDBBSIFR_BRSEL
+		       | (ovl->rop3 << LDBBSIFR_ROP3_SHIFT);
+		break;
+	}
+
+	switch (ovl->format->fourcc) {
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV61:
+	case V4L2_PIX_FMT_NV42:
+		format |= LDBBSIFR_SWPL | LDBBSIFR_SWPW;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV24:
+		format |= LDBBSIFR_SWPL | LDBBSIFR_SWPW | LDBBSIFR_SWPB;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	default:
+		format |= LDBBSIFR_SWPL;
+		break;
+	}
+
+	switch (ovl->format->fourcc) {
+	case V4L2_PIX_FMT_RGB565:
+		format |= LDBBSIFR_AL_1 | LDBBSIFR_RY | LDBBSIFR_RPKF_RGB16;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+		format |= LDBBSIFR_AL_1 | LDBBSIFR_RY | LDBBSIFR_RPKF_RGB24;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		format |= LDBBSIFR_AL_PK | LDBBSIFR_RY | LDDFR_PKF_ARGB32;
+		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		format |= LDBBSIFR_AL_1 | LDBBSIFR_CHRR_420;
+		break;
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		format |= LDBBSIFR_AL_1 | LDBBSIFR_CHRR_422;
+		break;
+	case V4L2_PIX_FMT_NV24:
+	case V4L2_PIX_FMT_NV42:
+		format |= LDBBSIFR_AL_1 | LDBBSIFR_CHRR_444;
+		break;
+	}
+
+	lcdc_write(ovl->channel->lcdc, LDBCR, LDBCR_UPC(ovl->index));
+
+	lcdc_write_overlay(ovl, LDBnBSIFR(ovl->index), format);
+
+	lcdc_write_overlay(ovl, LDBnBSSZR(ovl->index),
+		(ovl->yres << LDBBSSZR_BVSS_SHIFT) |
+		(ovl->xres << LDBBSSZR_BHSS_SHIFT));
+	lcdc_write_overlay(ovl, LDBnBLOCR(ovl->index),
+		(ovl->pos_y << LDBBLOCR_CVLC_SHIFT) |
+		(ovl->pos_x << LDBBLOCR_CHLC_SHIFT));
+	lcdc_write_overlay(ovl, LDBnBSMWR(ovl->index),
+		ovl->pitch << LDBBSMWR_BSMW_SHIFT);
+
+	lcdc_write_overlay(ovl, LDBnBSAYR(ovl->index), ovl->base_addr_y);
+	lcdc_write_overlay(ovl, LDBnBSACR(ovl->index), ovl->base_addr_c);
+
+	lcdc_write(ovl->channel->lcdc, LDBCR,
+		   LDBCR_UPF(ovl->index) | LDBCR_UPD(ovl->index));
+}
+
 /*
- * __sh_mobile_lcdc_start - Configure and tart the LCDC
+ * __sh_mobile_lcdc_start - Configure and start the LCDC
  * @priv: LCDC device
  *
  * Configure all enabled channels and start the LCDC device. All external
@@ -839,27 +1104,25 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 	/* Compute frame buffer base address and pitch for each channel. */
 	for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
 		int pixelformat;
-		void *meram;
+		void *cache;
 
 		ch = &priv->ch[k];
 		if (!ch->enabled)
 			continue;
 
 		ch->base_addr_y = ch->dma_handle;
-		ch->base_addr_c = ch->base_addr_y + ch->xres * ch->yres_virtual;
+		ch->base_addr_c = ch->dma_handle
+				+ ch->xres_virtual * ch->yres_virtual;
 		ch->line_size = ch->pitch;
 
 		/* Enable MERAM if possible. */
-		if (mdev == NULL || mdev->ops == NULL ||
-		    ch->cfg->meram_cfg == NULL)
+		if (mdev == NULL || ch->cfg->meram_cfg == NULL)
 			continue;
 
-		/* we need to de-init configured ICBs before we can
-		 * re-initialize them.
-		 */
-		if (ch->meram) {
-			mdev->ops->meram_unregister(mdev, ch->meram);
-			ch->meram = NULL;
+		/* Free the allocated MERAM cache. */
+		if (ch->cache) {
+			sh_mobile_meram_cache_free(mdev, ch->cache);
+			ch->cache = NULL;
 		}
 
 		switch (ch->format->fourcc) {
@@ -881,17 +1144,22 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 			break;
 		}
 
-		meram = mdev->ops->meram_register(mdev, ch->cfg->meram_cfg,
+		cache = sh_mobile_meram_cache_alloc(mdev, ch->cfg->meram_cfg,
 					ch->pitch, ch->yres, pixelformat,
 					&ch->line_size);
-		if (!IS_ERR(meram)) {
-			mdev->ops->meram_update(mdev, meram,
+		if (!IS_ERR(cache)) {
+			sh_mobile_meram_cache_update(mdev, cache,
 					ch->base_addr_y, ch->base_addr_c,
 					&ch->base_addr_y, &ch->base_addr_c);
-			ch->meram = meram;
+			ch->cache = cache;
 		}
 	}
 
+	for (k = 0; k < ARRAY_SIZE(priv->overlays); ++k) {
+		struct sh_mobile_lcdc_overlay *ovl = &priv->overlays[k];
+		sh_mobile_lcdc_overlay_setup(ovl);
+	}
+
 	/* Start the LCDC. */
 	__sh_mobile_lcdc_start(priv);
 
@@ -953,12 +1221,10 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
 
 		sh_mobile_lcdc_display_off(ch);
 
-		/* disable the meram */
-		if (ch->meram) {
-			struct sh_mobile_meram_info *mdev;
-			mdev = priv->meram_dev;
-			mdev->ops->meram_unregister(mdev, ch->meram);
-			ch->meram = 0;
+		/* Free the MERAM cache. */
+		if (ch->cache) {
+			sh_mobile_meram_cache_free(priv->meram_dev, ch->cache);
+			ch->cache = 0;
 		}
 
 	}
@@ -975,8 +1241,511 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
 			sh_mobile_lcdc_clk_off(priv);
 }
 
+static int __sh_mobile_lcdc_check_var(struct fb_var_screeninfo *var,
+				      struct fb_info *info)
+{
+	if (var->xres > MAX_XRES || var->yres > MAX_YRES)
+		return -EINVAL;
+
+	/* Make sure the virtual resolution is at least as big as the visible
+	 * resolution.
+	 */
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+	if (var->yres_virtual < var->yres)
+		var->yres_virtual = var->yres;
+
+	if (sh_mobile_format_is_fourcc(var)) {
+		const struct sh_mobile_lcdc_format_info *format;
+
+		format = sh_mobile_format_info(var->grayscale);
+		if (format == NULL)
+			return -EINVAL;
+		var->bits_per_pixel = format->bpp;
+
+		/* Default to RGB and JPEG color-spaces for RGB and YUV formats
+		 * respectively.
+		 */
+		if (!format->yuv)
+			var->colorspace = V4L2_COLORSPACE_SRGB;
+		else if (var->colorspace != V4L2_COLORSPACE_REC709)
+			var->colorspace = V4L2_COLORSPACE_JPEG;
+	} else {
+		if (var->bits_per_pixel <= 16) {		/* RGB 565 */
+			var->bits_per_pixel = 16;
+			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;
+		} else if (var->bits_per_pixel <= 24) {		/* RGB 888 */
+			var->bits_per_pixel = 24;
+			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 = 0;
+			var->transp.length = 0;
+		} else if (var->bits_per_pixel <= 32) {		/* RGBA 888 */
+			var->bits_per_pixel = 32;
+			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;
+		} else
+			return -EINVAL;
+
+		var->red.msb_right = 0;
+		var->green.msb_right = 0;
+		var->blue.msb_right = 0;
+		var->transp.msb_right = 0;
+	}
+
+	/* Make sure we don't exceed our allocated memory. */
+	if (var->xres_virtual * var->yres_virtual * var->bits_per_pixel / 8 >
+	    info->fix.smem_len)
+		return -EINVAL;
+
+	return 0;
+}
+
 /* -----------------------------------------------------------------------------
- * Frame buffer operations
+ * Frame buffer operations - Overlays
+ */
+
+static ssize_t
+overlay_alpha_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", ovl->alpha);
+}
+
+static ssize_t
+overlay_alpha_store(struct device *dev, struct device_attribute *attr,
+		    const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+	unsigned int alpha;
+	char *endp;
+
+	alpha = simple_strtoul(buf, &endp, 10);
+	if (isspace(*endp))
+		endp++;
+
+	if (endp - buf != count)
+		return -EINVAL;
+
+	if (alpha > 255)
+		return -EINVAL;
+
+	if (ovl->alpha != alpha) {
+		ovl->alpha = alpha;
+
+		if (ovl->mode == LCDC_OVERLAY_BLEND && ovl->enabled)
+			sh_mobile_lcdc_overlay_setup(ovl);
+	}
+
+	return count;
+}
+
+static ssize_t
+overlay_mode_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", ovl->mode);
+}
+
+static ssize_t
+overlay_mode_store(struct device *dev, struct device_attribute *attr,
+		   const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+	unsigned int mode;
+	char *endp;
+
+	mode = simple_strtoul(buf, &endp, 10);
+	if (isspace(*endp))
+		endp++;
+
+	if (endp - buf != count)
+		return -EINVAL;
+
+	if (mode != LCDC_OVERLAY_BLEND && mode != LCDC_OVERLAY_ROP3)
+		return -EINVAL;
+
+	if (ovl->mode != mode) {
+		ovl->mode = mode;
+
+		if (ovl->enabled)
+			sh_mobile_lcdc_overlay_setup(ovl);
+	}
+
+	return count;
+}
+
+static ssize_t
+overlay_position_show(struct device *dev, struct device_attribute *attr,
+		      char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	return scnprintf(buf, PAGE_SIZE, "%d,%d\n", ovl->pos_x, ovl->pos_y);
+}
+
+static ssize_t
+overlay_position_store(struct device *dev, struct device_attribute *attr,
+		       const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+	char *endp;
+	int pos_x;
+	int pos_y;
+
+	pos_x = simple_strtol(buf, &endp, 10);
+	if (*endp != ',')
+		return -EINVAL;
+
+	pos_y = simple_strtol(endp + 1, &endp, 10);
+	if (isspace(*endp))
+		endp++;
+
+	if (endp - buf != count)
+		return -EINVAL;
+
+	if (ovl->pos_x != pos_x || ovl->pos_y != pos_y) {
+		ovl->pos_x = pos_x;
+		ovl->pos_y = pos_y;
+
+		if (ovl->enabled)
+			sh_mobile_lcdc_overlay_setup(ovl);
+	}
+
+	return count;
+}
+
+static ssize_t
+overlay_rop3_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", ovl->rop3);
+}
+
+static ssize_t
+overlay_rop3_store(struct device *dev, struct device_attribute *attr,
+		    const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(dev);
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+	unsigned int rop3;
+	char *endp;
+
+	rop3 = !!simple_strtoul(buf, &endp, 10);
+	if (isspace(*endp))
+		endp++;
+
+	if (endp - buf != count)
+		return -EINVAL;
+
+	if (rop3 > 255)
+		return -EINVAL;
+
+	if (ovl->rop3 != rop3) {
+		ovl->rop3 = rop3;
+
+		if (ovl->mode == LCDC_OVERLAY_ROP3 && ovl->enabled)
+			sh_mobile_lcdc_overlay_setup(ovl);
+	}
+
+	return count;
+}
+
+static const struct device_attribute overlay_sysfs_attrs[] = {
+	__ATTR(ovl_alpha, S_IRUGO|S_IWUSR,
+	       overlay_alpha_show, overlay_alpha_store),
+	__ATTR(ovl_mode, S_IRUGO|S_IWUSR,
+	       overlay_mode_show, overlay_mode_store),
+	__ATTR(ovl_position, S_IRUGO|S_IWUSR,
+	       overlay_position_show, overlay_position_store),
+	__ATTR(ovl_rop3, S_IRUGO|S_IWUSR,
+	       overlay_rop3_show, overlay_rop3_store),
+};
+
+static const struct fb_fix_screeninfo sh_mobile_lcdc_overlay_fix  = {
+	.id =		"SH Mobile LCDC",
+	.type =		FB_TYPE_PACKED_PIXELS,
+	.visual =	FB_VISUAL_TRUECOLOR,
+	.accel =	FB_ACCEL_NONE,
+	.xpanstep =	1,
+	.ypanstep =	1,
+	.ywrapstep =	0,
+	.capabilities =	FB_CAP_FOURCC,
+};
+
+static int sh_mobile_lcdc_overlay_pan(struct fb_var_screeninfo *var,
+				    struct fb_info *info)
+{
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+	unsigned long base_addr_y;
+	unsigned long base_addr_c;
+	unsigned long y_offset;
+	unsigned long c_offset;
+
+	if (!ovl->format->yuv) {
+		y_offset = (var->yoffset * ovl->xres_virtual + var->xoffset)
+			 * ovl->format->bpp / 8;
+		c_offset = 0;
+	} else {
+		unsigned int xsub = ovl->format->bpp < 24 ? 2 : 1;
+		unsigned int ysub = ovl->format->bpp < 16 ? 2 : 1;
+
+		y_offset = var->yoffset * ovl->xres_virtual + var->xoffset;
+		c_offset = var->yoffset / ysub * ovl->xres_virtual * 2 / xsub
+			 + var->xoffset * 2 / xsub;
+	}
+
+	/* If the Y offset hasn't changed, the C offset hasn't either. There's
+	 * nothing to do in that case.
+	 */
+	if (y_offset == ovl->pan_y_offset)
+		return 0;
+
+	/* Set the source address for the next refresh */
+	base_addr_y = ovl->dma_handle + y_offset;
+	base_addr_c = ovl->dma_handle + ovl->xres_virtual * ovl->yres_virtual
+		    + c_offset;
+
+	ovl->base_addr_y = base_addr_y;
+	ovl->base_addr_c = base_addr_c;
+	ovl->pan_y_offset = y_offset;
+
+	lcdc_write(ovl->channel->lcdc, LDBCR, LDBCR_UPC(ovl->index));
+
+	lcdc_write_overlay(ovl, LDBnBSAYR(ovl->index), ovl->base_addr_y);
+	lcdc_write_overlay(ovl, LDBnBSACR(ovl->index), ovl->base_addr_c);
+
+	lcdc_write(ovl->channel->lcdc, LDBCR,
+		   LDBCR_UPF(ovl->index) | LDBCR_UPD(ovl->index));
+
+	return 0;
+}
+
+static int sh_mobile_lcdc_overlay_ioctl(struct fb_info *info, unsigned int cmd,
+				      unsigned long arg)
+{
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	switch (cmd) {
+	case FBIO_WAITFORVSYNC:
+		return sh_mobile_lcdc_wait_for_vsync(ovl->channel);
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+}
+
+static int sh_mobile_lcdc_overlay_check_var(struct fb_var_screeninfo *var,
+					  struct fb_info *info)
+{
+	return __sh_mobile_lcdc_check_var(var, info);
+}
+
+static int sh_mobile_lcdc_overlay_set_par(struct fb_info *info)
+{
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	ovl->format =
+		sh_mobile_format_info(sh_mobile_format_fourcc(&info->var));
+
+	ovl->xres = info->var.xres;
+	ovl->xres_virtual = info->var.xres_virtual;
+	ovl->yres = info->var.yres;
+	ovl->yres_virtual = info->var.yres_virtual;
+
+	if (ovl->format->yuv)
+		ovl->pitch = info->var.xres_virtual;
+	else
+		ovl->pitch = info->var.xres_virtual * ovl->format->bpp / 8;
+
+	sh_mobile_lcdc_overlay_setup(ovl);
+
+	info->fix.line_length = ovl->pitch;
+
+	if (sh_mobile_format_is_fourcc(&info->var)) {
+		info->fix.type = FB_TYPE_FOURCC;
+		info->fix.visual = FB_VISUAL_FOURCC;
+	} else {
+		info->fix.type = FB_TYPE_PACKED_PIXELS;
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+	}
+
+	return 0;
+}
+
+/* Overlay blanking. Disable the overlay when blanked. */
+static int sh_mobile_lcdc_overlay_blank(int blank, struct fb_info *info)
+{
+	struct sh_mobile_lcdc_overlay *ovl = info->par;
+
+	ovl->enabled = !blank;
+	sh_mobile_lcdc_overlay_setup(ovl);
+
+	/* Prevent the backlight from receiving a blanking event by returning
+	 * a non-zero value.
+	 */
+	return 1;
+}
+
+static struct fb_ops sh_mobile_lcdc_overlay_ops = {
+	.owner          = THIS_MODULE,
+	.fb_read        = fb_sys_read,
+	.fb_write       = fb_sys_write,
+	.fb_fillrect	= sys_fillrect,
+	.fb_copyarea	= sys_copyarea,
+	.fb_imageblit	= sys_imageblit,
+	.fb_blank	= sh_mobile_lcdc_overlay_blank,
+	.fb_pan_display = sh_mobile_lcdc_overlay_pan,
+	.fb_ioctl       = sh_mobile_lcdc_overlay_ioctl,
+	.fb_check_var	= sh_mobile_lcdc_overlay_check_var,
+	.fb_set_par	= sh_mobile_lcdc_overlay_set_par,
+};
+
+static void
+sh_mobile_lcdc_overlay_fb_unregister(struct sh_mobile_lcdc_overlay *ovl)
+{
+	struct fb_info *info = ovl->info;
+
+	if (info == NULL || info->dev == NULL)
+		return;
+
+	unregister_framebuffer(ovl->info);
+}
+
+static int __devinit
+sh_mobile_lcdc_overlay_fb_register(struct sh_mobile_lcdc_overlay *ovl)
+{
+	struct sh_mobile_lcdc_priv *lcdc = ovl->channel->lcdc;
+	struct fb_info *info = ovl->info;
+	unsigned int i;
+	int ret;
+
+	if (info == NULL)
+		return 0;
+
+	ret = register_framebuffer(info);
+	if (ret < 0)
+		return ret;
+
+	dev_info(lcdc->dev, "registered %s/overlay %u as %dx%d %dbpp.\n",
+		 dev_name(lcdc->dev), ovl->index, info->var.xres,
+		 info->var.yres, info->var.bits_per_pixel);
+
+	for (i = 0; i < ARRAY_SIZE(overlay_sysfs_attrs); ++i) {
+		ret = device_create_file(info->dev, &overlay_sysfs_attrs[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void
+sh_mobile_lcdc_overlay_fb_cleanup(struct sh_mobile_lcdc_overlay *ovl)
+{
+	struct fb_info *info = ovl->info;
+
+	if (info == NULL || info->device == NULL)
+		return;
+
+	framebuffer_release(info);
+}
+
+static int __devinit
+sh_mobile_lcdc_overlay_fb_init(struct sh_mobile_lcdc_overlay *ovl)
+{
+	struct sh_mobile_lcdc_priv *priv = ovl->channel->lcdc;
+	struct fb_var_screeninfo *var;
+	struct fb_info *info;
+
+	/* Allocate and initialize the frame buffer device. */
+	info = framebuffer_alloc(0, priv->dev);
+	if (info == NULL) {
+		dev_err(priv->dev, "unable to allocate fb_info\n");
+		return -ENOMEM;
+	}
+
+	ovl->info = info;
+
+	info->flags = FBINFO_FLAG_DEFAULT;
+	info->fbops = &sh_mobile_lcdc_overlay_ops;
+	info->device = priv->dev;
+	info->screen_base = ovl->fb_mem;
+	info->par = ovl;
+
+	/* Initialize fixed screen information. Restrict pan to 2 lines steps
+	 * for NV12 and NV21.
+	 */
+	info->fix = sh_mobile_lcdc_overlay_fix;
+	snprintf(info->fix.id, sizeof(info->fix.id),
+		 "SH Mobile LCDC Overlay %u", ovl->index);
+	info->fix.smem_start = ovl->dma_handle;
+	info->fix.smem_len = ovl->fb_size;
+	info->fix.line_length = ovl->pitch;
+
+	if (ovl->format->yuv)
+		info->fix.visual = FB_VISUAL_FOURCC;
+	else
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+
+	switch (ovl->format->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		info->fix.ypanstep = 2;
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		info->fix.xpanstep = 2;
+	}
+
+	/* Initialize variable screen information. */
+	var = &info->var;
+	memset(var, 0, sizeof(*var));
+	var->xres = ovl->xres;
+	var->yres = ovl->yres;
+	var->xres_virtual = ovl->xres_virtual;
+	var->yres_virtual = ovl->yres_virtual;
+	var->activate = FB_ACTIVATE_NOW;
+
+	/* Use the legacy API by default for RGB formats, and the FOURCC API
+	 * for YUV formats.
+	 */
+	if (!ovl->format->yuv)
+		var->bits_per_pixel = ovl->format->bpp;
+	else
+		var->grayscale = ovl->format->fourcc;
+
+	return sh_mobile_lcdc_overlay_check_var(var, info);
+}
+
+/* -----------------------------------------------------------------------------
+ * Frame buffer operations - main frame buffer
  */
 
 static int sh_mobile_lcdc_setcolreg(u_int regno,
@@ -1003,12 +1772,12 @@ static int sh_mobile_lcdc_setcolreg(u_int regno,
 	return 0;
 }
 
-static struct fb_fix_screeninfo sh_mobile_lcdc_fix  = {
+static const struct fb_fix_screeninfo sh_mobile_lcdc_fix  = {
 	.id =		"SH Mobile LCDC",
 	.type =		FB_TYPE_PACKED_PIXELS,
 	.visual =	FB_VISUAL_TRUECOLOR,
 	.accel =	FB_ACCEL_NONE,
-	.xpanstep =	0,
+	.xpanstep =	1,
 	.ypanstep =	1,
 	.ywrapstep =	0,
 	.capabilities =	FB_CAP_FOURCC,
@@ -1035,78 +1804,74 @@ static void sh_mobile_lcdc_imageblit(struct fb_info *info,
 	sh_mobile_lcdc_deferred_io_touch(info);
 }
 
-static int sh_mobile_fb_pan_display(struct fb_var_screeninfo *var,
-				     struct fb_info *info)
+static int sh_mobile_lcdc_pan(struct fb_var_screeninfo *var,
+			      struct fb_info *info)
 {
 	struct sh_mobile_lcdc_chan *ch = info->par;
 	struct sh_mobile_lcdc_priv *priv = ch->lcdc;
 	unsigned long ldrcntr;
-	unsigned long new_pan_offset;
 	unsigned long base_addr_y, base_addr_c;
+	unsigned long y_offset;
 	unsigned long c_offset;
 
-	if (!ch->format->yuv)
-		new_pan_offset = var->yoffset * ch->pitch
-			       + var->xoffset * (ch->format->bpp / 8);
-	else
-		new_pan_offset = var->yoffset * ch->pitch + var->xoffset;
+	if (!ch->format->yuv) {
+		y_offset = (var->yoffset * ch->xres_virtual + var->xoffset)
+			 * ch->format->bpp / 8;
+		c_offset = 0;
+	} else {
+		unsigned int xsub = ch->format->bpp < 24 ? 2 : 1;
+		unsigned int ysub = ch->format->bpp < 16 ? 2 : 1;
 
-	if (new_pan_offset == ch->pan_offset)
-		return 0;	/* No change, do nothing */
+		y_offset = var->yoffset * ch->xres_virtual + var->xoffset;
+		c_offset = var->yoffset / ysub * ch->xres_virtual * 2 / xsub
+			 + var->xoffset * 2 / xsub;
+	}
 
-	ldrcntr = lcdc_read(priv, _LDRCNTR);
+	/* If the Y offset hasn't changed, the C offset hasn't either. There's
+	 * nothing to do in that case.
+	 */
+	if (y_offset == ch->pan_y_offset)
+		return 0;
 
 	/* Set the source address for the next refresh */
-	base_addr_y = ch->dma_handle + new_pan_offset;
-	if (ch->format->yuv) {
-		/* Set y offset */
-		c_offset = var->yoffset * ch->pitch
-			 * (ch->format->bpp - 8) / 8;
-		base_addr_c = ch->dma_handle + ch->xres * ch->yres_virtual
-			    + c_offset;
-		/* Set x offset */
-		if (ch->format->fourcc == V4L2_PIX_FMT_NV24)
-			base_addr_c += 2 * var->xoffset;
-		else
-			base_addr_c += var->xoffset;
-	}
+	base_addr_y = ch->dma_handle + y_offset;
+	base_addr_c = ch->dma_handle + ch->xres_virtual * ch->yres_virtual
+		    + c_offset;
 
-	if (ch->meram) {
-		struct sh_mobile_meram_info *mdev;
-
-		mdev = priv->meram_dev;
-		mdev->ops->meram_update(mdev, ch->meram,
-					base_addr_y, base_addr_c,
-					&base_addr_y, &base_addr_c);
-	}
+	if (ch->cache)
+		sh_mobile_meram_cache_update(priv->meram_dev, ch->cache,
+					     base_addr_y, base_addr_c,
+					     &base_addr_y, &base_addr_c);
 
 	ch->base_addr_y = base_addr_y;
 	ch->base_addr_c = base_addr_c;
+	ch->pan_y_offset = y_offset;
 
 	lcdc_write_chan_mirror(ch, LDSA1R, base_addr_y);
 	if (ch->format->yuv)
 		lcdc_write_chan_mirror(ch, LDSA2R, base_addr_c);
 
+	ldrcntr = lcdc_read(priv, _LDRCNTR);
 	if (lcdc_chan_is_sublcd(ch))
 		lcdc_write(ch->lcdc, _LDRCNTR, ldrcntr ^ LDRCNTR_SRS);
 	else
 		lcdc_write(ch->lcdc, _LDRCNTR, ldrcntr ^ LDRCNTR_MRS);
 
-	ch->pan_offset = new_pan_offset;
 
 	sh_mobile_lcdc_deferred_io_touch(info);
 
 	return 0;
 }
 
-static int sh_mobile_ioctl(struct fb_info *info, unsigned int cmd,
-		       unsigned long arg)
+static int sh_mobile_lcdc_ioctl(struct fb_info *info, unsigned int cmd,
+				unsigned long arg)
 {
+	struct sh_mobile_lcdc_chan *ch = info->par;
 	int retval;
 
 	switch (cmd) {
 	case FBIO_WAITFORVSYNC:
-		retval = sh_mobile_wait_for_vsync(info->par);
+		retval = sh_mobile_lcdc_wait_for_vsync(ch);
 		break;
 
 	default:
@@ -1158,7 +1923,7 @@ static void sh_mobile_fb_reconfig(struct fb_info *info)
  * Locking: both .fb_release() and .fb_open() are called with info->lock held if
  * user == 1, or with console sem held, if user == 0.
  */
-static int sh_mobile_release(struct fb_info *info, int user)
+static int sh_mobile_lcdc_release(struct fb_info *info, int user)
 {
 	struct sh_mobile_lcdc_chan *ch = info->par;
 
@@ -1179,7 +1944,7 @@ static int sh_mobile_release(struct fb_info *info, int user)
 	return 0;
 }
 
-static int sh_mobile_open(struct fb_info *info, int user)
+static int sh_mobile_lcdc_open(struct fb_info *info, int user)
 {
 	struct sh_mobile_lcdc_chan *ch = info->par;
 
@@ -1192,7 +1957,8 @@ static int sh_mobile_open(struct fb_info *info, int user)
 	return 0;
 }
 
-static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+static int sh_mobile_lcdc_check_var(struct fb_var_screeninfo *var,
+				    struct fb_info *info)
 {
 	struct sh_mobile_lcdc_chan *ch = info->par;
 	struct sh_mobile_lcdc_priv *p = ch->lcdc;
@@ -1200,9 +1966,7 @@ static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *in
 	unsigned int best_xres = 0;
 	unsigned int best_yres = 0;
 	unsigned int i;
-
-	if (var->xres > MAX_XRES || var->yres > MAX_YRES)
-		return -EINVAL;
+	int ret;
 
 	/* If board code provides us with a list of available modes, make sure
 	 * we use one of them. Find the mode closest to the requested one. The
@@ -1237,73 +2001,9 @@ static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *in
 		var->yres = best_yres;
 	}
 
-	/* Make sure the virtual resolution is at least as big as the visible
-	 * resolution.
-	 */
-	if (var->xres_virtual < var->xres)
-		var->xres_virtual = var->xres;
-	if (var->yres_virtual < var->yres)
-		var->yres_virtual = var->yres;
-
-	if (sh_mobile_format_is_fourcc(var)) {
-		const struct sh_mobile_lcdc_format_info *format;
-
-		format = sh_mobile_format_info(var->grayscale);
-		if (format == NULL)
-			return -EINVAL;
-		var->bits_per_pixel = format->bpp;
-
-		/* Default to RGB and JPEG color-spaces for RGB and YUV formats
-		 * respectively.
-		 */
-		if (!format->yuv)
-			var->colorspace = V4L2_COLORSPACE_SRGB;
-		else if (var->colorspace != V4L2_COLORSPACE_REC709)
-			var->colorspace = V4L2_COLORSPACE_JPEG;
-	} else {
-		if (var->bits_per_pixel <= 16) {		/* RGB 565 */
-			var->bits_per_pixel = 16;
-			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;
-		} else if (var->bits_per_pixel <= 24) {		/* RGB 888 */
-			var->bits_per_pixel = 24;
-			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 = 0;
-			var->transp.length = 0;
-		} else if (var->bits_per_pixel <= 32) {		/* RGBA 888 */
-			var->bits_per_pixel = 32;
-			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;
-		} else
-			return -EINVAL;
-
-		var->red.msb_right = 0;
-		var->green.msb_right = 0;
-		var->blue.msb_right = 0;
-		var->transp.msb_right = 0;
-	}
-
-	/* Make sure we don't exceed our allocated memory. */
-	if (var->xres_virtual * var->yres_virtual * var->bits_per_pixel / 8 >
-	    info->fix.smem_len)
-		return -EINVAL;
+	ret = __sh_mobile_lcdc_check_var(var, info);
+	if (ret < 0)
+		return ret;
 
 	/* only accept the forced_fourcc for dual channel configurations */
 	if (p->forced_fourcc &&
@@ -1313,7 +2013,7 @@ static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *in
 	return 0;
 }
 
-static int sh_mobile_set_par(struct fb_info *info)
+static int sh_mobile_lcdc_set_par(struct fb_info *info)
 {
 	struct sh_mobile_lcdc_chan *ch = info->par;
 	int ret;
@@ -1329,9 +2029,9 @@ static int sh_mobile_set_par(struct fb_info *info)
 	ch->yres_virtual = info->var.yres_virtual;
 
 	if (ch->format->yuv)
-		ch->pitch = info->var.xres;
+		ch->pitch = info->var.xres_virtual;
 	else
-		ch->pitch = info->var.xres * ch->format->bpp / 8;
+		ch->pitch = info->var.xres_virtual * ch->format->bpp / 8;
 
 	ret = sh_mobile_lcdc_start(ch->lcdc);
 	if (ret < 0)
@@ -1383,8 +2083,8 @@ static int sh_mobile_lcdc_blank(int blank, struct fb_info *info)
 		 * mode will reenable the clocks and update the screen in time,
 		 * so it does not need this. */
 		if (!info->fbdefio) {
-			sh_mobile_wait_for_vsync(ch);
-			sh_mobile_wait_for_vsync(ch);
+			sh_mobile_lcdc_wait_for_vsync(ch);
+			sh_mobile_lcdc_wait_for_vsync(ch);
 		}
 		sh_mobile_lcdc_clk_off(p);
 	}
@@ -1402,12 +2102,12 @@ static struct fb_ops sh_mobile_lcdc_ops = {
 	.fb_copyarea	= sh_mobile_lcdc_copyarea,
 	.fb_imageblit	= sh_mobile_lcdc_imageblit,
 	.fb_blank	= sh_mobile_lcdc_blank,
-	.fb_pan_display = sh_mobile_fb_pan_display,
-	.fb_ioctl       = sh_mobile_ioctl,
-	.fb_open	= sh_mobile_open,
-	.fb_release	= sh_mobile_release,
-	.fb_check_var	= sh_mobile_check_var,
-	.fb_set_par	= sh_mobile_set_par,
+	.fb_pan_display = sh_mobile_lcdc_pan,
+	.fb_ioctl       = sh_mobile_lcdc_ioctl,
+	.fb_open	= sh_mobile_lcdc_open,
+	.fb_release	= sh_mobile_lcdc_release,
+	.fb_check_var	= sh_mobile_lcdc_check_var,
+	.fb_set_par	= sh_mobile_lcdc_set_par,
 };
 
 static void
@@ -1514,19 +2214,24 @@ sh_mobile_lcdc_channel_fb_init(struct sh_mobile_lcdc_chan *ch,
 	else
 		info->fix.visual = FB_VISUAL_TRUECOLOR;
 
-	if (ch->format->fourcc == V4L2_PIX_FMT_NV12 ||
-	    ch->format->fourcc == V4L2_PIX_FMT_NV21)
+	switch (ch->format->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
 		info->fix.ypanstep = 2;
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		info->fix.xpanstep = 2;
+	}
 
 	/* Initialize variable screen information using the first mode as
-	 * default. The default Y virtual resolution is twice the panel size to
-	 * allow for double-buffering.
+	 * default.
 	 */
 	var = &info->var;
 	fb_videomode_to_var(var, mode);
 	var->width = ch->cfg->panel_cfg.width;
 	var->height = ch->cfg->panel_cfg.height;
-	var->yres_virtual = var->yres * 2;
+	var->xres_virtual = ch->xres_virtual;
+	var->yres_virtual = ch->yres_virtual;
 	var->activate = FB_ACTIVATE_NOW;
 
 	/* Use the legacy API by default for RGB formats, and the FOURCC API
@@ -1537,7 +2242,7 @@ sh_mobile_lcdc_channel_fb_init(struct sh_mobile_lcdc_chan *ch,
 	else
 		var->grayscale = ch->format->fourcc;
 
-	ret = sh_mobile_check_var(var, info);
+	ret = sh_mobile_lcdc_check_var(var, info);
 	if (ret)
 		return ret;
 
@@ -1712,15 +2417,27 @@ static const struct fb_videomode default_720p __devinitconst = {
 static int sh_mobile_lcdc_remove(struct platform_device *pdev)
 {
 	struct sh_mobile_lcdc_priv *priv = platform_get_drvdata(pdev);
-	int i;
+	unsigned int i;
 
 	fb_unregister_client(&priv->notifier);
 
+	for (i = 0; i < ARRAY_SIZE(priv->overlays); i++)
+		sh_mobile_lcdc_overlay_fb_unregister(&priv->overlays[i]);
 	for (i = 0; i < ARRAY_SIZE(priv->ch); i++)
 		sh_mobile_lcdc_channel_fb_unregister(&priv->ch[i]);
 
 	sh_mobile_lcdc_stop(priv);
 
+	for (i = 0; i < ARRAY_SIZE(priv->overlays); i++) {
+		struct sh_mobile_lcdc_overlay *ovl = &priv->overlays[i];
+
+		sh_mobile_lcdc_overlay_fb_cleanup(ovl);
+
+		if (ovl->fb_mem)
+			dma_free_coherent(&pdev->dev, ovl->fb_size,
+					  ovl->fb_mem, ovl->dma_handle);
+	}
+
 	for (i = 0; i < ARRAY_SIZE(priv->ch); i++) {
 		struct sh_mobile_lcdc_chan *ch = &priv->ch[i];
 
@@ -1737,8 +2454,11 @@ static int sh_mobile_lcdc_remove(struct platform_device *pdev)
 	}
 
 	for (i = 0; i < ARRAY_SIZE(priv->ch); i++) {
-		if (priv->ch[i].bl)
-			sh_mobile_lcdc_bl_remove(priv->ch[i].bl);
+		struct sh_mobile_lcdc_chan *ch = &priv->ch[i];
+
+		if (ch->bl)
+			sh_mobile_lcdc_bl_remove(ch->bl);
+		mutex_destroy(&ch->open_lock);
 	}
 
 	if (priv->dot_clk) {
@@ -1796,6 +2516,61 @@ static int __devinit sh_mobile_lcdc_check_interface(struct sh_mobile_lcdc_chan *
 }
 
 static int __devinit
+sh_mobile_lcdc_overlay_init(struct sh_mobile_lcdc_priv *priv,
+			  struct sh_mobile_lcdc_overlay *ovl)
+{
+	const struct sh_mobile_lcdc_format_info *format;
+	int ret;
+
+	if (ovl->cfg->fourcc == 0)
+		return 0;
+
+	/* Validate the format. */
+	format = sh_mobile_format_info(ovl->cfg->fourcc);
+	if (format == NULL) {
+		dev_err(priv->dev, "Invalid FOURCC %08x\n", ovl->cfg->fourcc);
+		return -EINVAL;
+	}
+
+	ovl->enabled = false;
+	ovl->mode = LCDC_OVERLAY_BLEND;
+	ovl->alpha = 255;
+	ovl->rop3 = 0;
+	ovl->pos_x = 0;
+	ovl->pos_y = 0;
+
+	/* The default Y virtual resolution is twice the panel size to allow for
+	 * double-buffering.
+	 */
+	ovl->format = format;
+	ovl->xres = ovl->cfg->max_xres;
+	ovl->xres_virtual = ovl->xres;
+	ovl->yres = ovl->cfg->max_yres;
+	ovl->yres_virtual = ovl->yres * 2;
+
+	if (!format->yuv)
+		ovl->pitch = ovl->xres_virtual * format->bpp / 8;
+	else
+		ovl->pitch = ovl->xres_virtual;
+
+	/* Allocate frame buffer memory. */
+	ovl->fb_size = ovl->cfg->max_xres * ovl->cfg->max_yres
+		       * format->bpp / 8 * 2;
+	ovl->fb_mem = dma_alloc_coherent(priv->dev, ovl->fb_size,
+					   &ovl->dma_handle, GFP_KERNEL);
+	if (!ovl->fb_mem) {
+		dev_err(priv->dev, "unable to allocate buffer\n");
+		return -ENOMEM;
+	}
+
+	ret = sh_mobile_lcdc_overlay_fb_init(ovl);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int __devinit
 sh_mobile_lcdc_channel_init(struct sh_mobile_lcdc_priv *priv,
 			    struct sh_mobile_lcdc_chan *ch)
 {
@@ -1854,7 +2629,9 @@ sh_mobile_lcdc_channel_init(struct sh_mobile_lcdc_priv *priv,
 		num_modes = cfg->num_modes;
 	}
 
-	/* Use the first mode as default. */
+	/* Use the first mode as default. The default Y virtual resolution is
+	 * twice the panel size to allow for double-buffering.
+	 */
 	ch->format = format;
 	ch->xres = mode->xres;
 	ch->xres_virtual = mode->xres;
@@ -1863,10 +2640,10 @@ sh_mobile_lcdc_channel_init(struct sh_mobile_lcdc_priv *priv,
 
 	if (!format->yuv) {
 		ch->colorspace = V4L2_COLORSPACE_SRGB;
-		ch->pitch = ch->xres * format->bpp / 8;
+		ch->pitch = ch->xres_virtual * format->bpp / 8;
 	} else {
 		ch->colorspace = V4L2_COLORSPACE_REC709;
-		ch->pitch = ch->xres;
+		ch->pitch = ch->xres_virtual;
 	}
 
 	ch->display.width = cfg->panel_cfg.width;
@@ -1952,7 +2729,6 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 		}
 		init_waitqueue_head(&ch->frame_end_wait);
 		init_completion(&ch->vsync_completion);
-		ch->pan_offset = 0;
 
 		/* probe the backlight is there is one defined */
 		if (ch->cfg->bl_info.max_brightness)
@@ -2003,6 +2779,17 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 			goto err1;
 	}
 
+	for (i = 0; i < ARRAY_SIZE(pdata->overlays); i++) {
+		struct sh_mobile_lcdc_overlay *ovl = &priv->overlays[i];
+
+		ovl->cfg = &pdata->overlays[i];
+		ovl->channel = &priv->ch[0];
+
+		error = sh_mobile_lcdc_overlay_init(priv, ovl);
+		if (error)
+			goto err1;
+	}
+
 	error = sh_mobile_lcdc_start(priv);
 	if (error) {
 		dev_err(&pdev->dev, "unable to start hardware\n");
@@ -2017,6 +2804,14 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 			goto err1;
 	}
 
+	for (i = 0; i < ARRAY_SIZE(pdata->overlays); i++) {
+		struct sh_mobile_lcdc_overlay *ovl = &priv->overlays[i];
+
+		error = sh_mobile_lcdc_overlay_fb_register(ovl);
+		if (error)
+			goto err1;
+	}
+
 	/* Failure ignored */
 	priv->notifier.notifier_call = sh_mobile_lcdc_notify;
 	fb_register_client(&priv->notifier);
diff --git a/drivers/video/sh_mobile_lcdcfb.h b/drivers/video/sh_mobile_lcdcfb.h
index 5c3bddd2cb7..0f92f6544b9 100644
--- a/drivers/video/sh_mobile_lcdcfb.h
+++ b/drivers/video/sh_mobile_lcdcfb.h
@@ -47,6 +47,7 @@ struct sh_mobile_lcdc_entity {
 /*
  * struct sh_mobile_lcdc_chan - LCDC display channel
  *
+ * @pan_y_offset: Panning linear offset in bytes (luma component)
  * @base_addr_y: Frame buffer viewport base address (luma component)
  * @base_addr_c: Frame buffer viewport base address (chroma component)
  * @pitch: Frame buffer line pitch
@@ -59,7 +60,7 @@ struct sh_mobile_lcdc_chan {
 	unsigned long *reg_offs;
 	unsigned long ldmt1r_value;
 	unsigned long enabled; /* ME and SE in LDCNT2R */
-	void *meram;
+	void *cache;
 
 	struct mutex open_lock;		/* protects the use counter */
 	int use_count;
@@ -68,7 +69,7 @@ struct sh_mobile_lcdc_chan {
 	unsigned long fb_size;
 
 	dma_addr_t dma_handle;
-	unsigned long pan_offset;
+	unsigned long pan_y_offset;
 
 	unsigned long frame_end;
 	wait_queue_head_t frame_end_wait;
diff --git a/drivers/video/sh_mobile_meram.c b/drivers/video/sh_mobile_meram.c
index 82ba830bf95..7a0ba8bb3fb 100644
--- a/drivers/video/sh_mobile_meram.c
+++ b/drivers/video/sh_mobile_meram.c
@@ -11,6 +11,7 @@
 
 #include <linux/device.h>
 #include <linux/err.h>
+#include <linux/export.h>
 #include <linux/genalloc.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
@@ -194,13 +195,28 @@ static inline unsigned long meram_read_reg(void __iomem *base, unsigned int off)
 }
 
 /* -----------------------------------------------------------------------------
- * Allocation
+ * MERAM allocation and free
+ */
+
+static unsigned long meram_alloc(struct sh_mobile_meram_priv *priv, size_t size)
+{
+	return gen_pool_alloc(priv->pool, size);
+}
+
+static void meram_free(struct sh_mobile_meram_priv *priv, unsigned long mem,
+		       size_t size)
+{
+	gen_pool_free(priv->pool, mem, size);
+}
+
+/* -----------------------------------------------------------------------------
+ * LCDC cache planes allocation, init, cleanup and free
  */
 
 /* Allocate ICBs and MERAM for a plane. */
-static int __meram_alloc(struct sh_mobile_meram_priv *priv,
-			 struct sh_mobile_meram_fb_plane *plane,
-			 size_t size)
+static int meram_plane_alloc(struct sh_mobile_meram_priv *priv,
+			     struct sh_mobile_meram_fb_plane *plane,
+			     size_t size)
 {
 	unsigned long mem;
 	unsigned long idx;
@@ -215,7 +231,7 @@ static int __meram_alloc(struct sh_mobile_meram_priv *priv,
 		return -ENOMEM;
 	plane->marker = &priv->icbs[idx];
 
-	mem = gen_pool_alloc(priv->pool, size * 1024);
+	mem = meram_alloc(priv, size * 1024);
 	if (mem == 0)
 		return -ENOMEM;
 
@@ -229,11 +245,11 @@ static int __meram_alloc(struct sh_mobile_meram_priv *priv,
 }
 
 /* Free ICBs and MERAM for a plane. */
-static void __meram_free(struct sh_mobile_meram_priv *priv,
-			 struct sh_mobile_meram_fb_plane *plane)
+static void meram_plane_free(struct sh_mobile_meram_priv *priv,
+			     struct sh_mobile_meram_fb_plane *plane)
 {
-	gen_pool_free(priv->pool, priv->meram + plane->marker->offset,
-		      plane->marker->size * 1024);
+	meram_free(priv, priv->meram + plane->marker->offset,
+		   plane->marker->size * 1024);
 
 	__clear_bit(plane->marker->index, &priv->used_icb);
 	__clear_bit(plane->cache->index, &priv->used_icb);
@@ -248,62 +264,6 @@ static int is_nvcolor(int cspace)
 	return 0;
 }
 
-/* Allocate memory for the ICBs and mark them as used. */
-static struct sh_mobile_meram_fb_cache *
-meram_alloc(struct sh_mobile_meram_priv *priv,
-	    const struct sh_mobile_meram_cfg *cfg,
-	    int pixelformat)
-{
-	struct sh_mobile_meram_fb_cache *cache;
-	unsigned int nplanes = is_nvcolor(pixelformat) ? 2 : 1;
-	int ret;
-
-	if (cfg->icb[0].meram_size == 0)
-		return ERR_PTR(-EINVAL);
-
-	if (nplanes == 2 && cfg->icb[1].meram_size == 0)
-		return ERR_PTR(-EINVAL);
-
-	cache = kzalloc(sizeof(*cache), GFP_KERNEL);
-	if (cache == NULL)
-		return ERR_PTR(-ENOMEM);
-
-	cache->nplanes = nplanes;
-
-	ret = __meram_alloc(priv, &cache->planes[0], cfg->icb[0].meram_size);
-	if (ret < 0)
-		goto error;
-
-	cache->planes[0].marker->current_reg = 1;
-	cache->planes[0].marker->pixelformat = pixelformat;
-
-	if (cache->nplanes == 1)
-		return cache;
-
-	ret = __meram_alloc(priv, &cache->planes[1], cfg->icb[1].meram_size);
-	if (ret < 0) {
-		__meram_free(priv, &cache->planes[0]);
-		goto error;
-	}
-
-	return cache;
-
-error:
-	kfree(cache);
-	return ERR_PTR(-ENOMEM);
-}
-
-/* Unmark the specified ICB as used. */
-static void meram_free(struct sh_mobile_meram_priv *priv,
-		       struct sh_mobile_meram_fb_cache *cache)
-{
-	__meram_free(priv, &cache->planes[0]);
-	if (cache->nplanes == 2)
-		__meram_free(priv, &cache->planes[1]);
-
-	kfree(cache);
-}
-
 /* Set the next address to fetch. */
 static void meram_set_next_addr(struct sh_mobile_meram_priv *priv,
 				struct sh_mobile_meram_fb_cache *cache,
@@ -355,10 +315,10 @@ meram_get_next_icb_addr(struct sh_mobile_meram_info *pdata,
 	(((x) * (y) + (MERAM_LINE_WIDTH - 1)) & ~(MERAM_LINE_WIDTH - 1))
 
 /* Initialize MERAM. */
-static int meram_init(struct sh_mobile_meram_priv *priv,
-		      struct sh_mobile_meram_fb_plane *plane,
-		      unsigned int xres, unsigned int yres,
-		      unsigned int *out_pitch)
+static int meram_plane_init(struct sh_mobile_meram_priv *priv,
+			    struct sh_mobile_meram_fb_plane *plane,
+			    unsigned int xres, unsigned int yres,
+			    unsigned int *out_pitch)
 {
 	struct sh_mobile_meram_icb *marker = plane->marker;
 	unsigned long total_byte_count = MERAM_CALC_BYTECOUNT(xres, yres);
@@ -427,8 +387,8 @@ static int meram_init(struct sh_mobile_meram_priv *priv,
 	return 0;
 }
 
-static void meram_deinit(struct sh_mobile_meram_priv *priv,
-			 struct sh_mobile_meram_fb_plane *plane)
+static void meram_plane_cleanup(struct sh_mobile_meram_priv *priv,
+				struct sh_mobile_meram_fb_plane *plane)
 {
 	/* disable ICB */
 	meram_write_icb(priv->base, plane->cache->index,  MExxCTL,
@@ -441,20 +401,82 @@ static void meram_deinit(struct sh_mobile_meram_priv *priv,
 }
 
 /* -----------------------------------------------------------------------------
- * Registration/unregistration
+ * MERAM operations
  */
 
-static void *sh_mobile_meram_register(struct sh_mobile_meram_info *pdata,
-				      const struct sh_mobile_meram_cfg *cfg,
-				      unsigned int xres, unsigned int yres,
-				      unsigned int pixelformat,
-				      unsigned int *pitch)
+unsigned long sh_mobile_meram_alloc(struct sh_mobile_meram_info *pdata,
+				    size_t size)
+{
+	struct sh_mobile_meram_priv *priv = pdata->priv;
+
+	return meram_alloc(priv, size);
+}
+EXPORT_SYMBOL_GPL(sh_mobile_meram_alloc);
+
+void sh_mobile_meram_free(struct sh_mobile_meram_info *pdata, unsigned long mem,
+			  size_t size)
+{
+	struct sh_mobile_meram_priv *priv = pdata->priv;
+
+	meram_free(priv, mem, size);
+}
+EXPORT_SYMBOL_GPL(sh_mobile_meram_free);
+
+/* Allocate memory for the ICBs and mark them as used. */
+static struct sh_mobile_meram_fb_cache *
+meram_cache_alloc(struct sh_mobile_meram_priv *priv,
+		  const struct sh_mobile_meram_cfg *cfg,
+		  int pixelformat)
+{
+	unsigned int nplanes = is_nvcolor(pixelformat) ? 2 : 1;
+	struct sh_mobile_meram_fb_cache *cache;
+	int ret;
+
+	cache = kzalloc(sizeof(*cache), GFP_KERNEL);
+	if (cache == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	cache->nplanes = nplanes;
+
+	ret = meram_plane_alloc(priv, &cache->planes[0],
+				cfg->icb[0].meram_size);
+	if (ret < 0)
+		goto error;
+
+	cache->planes[0].marker->current_reg = 1;
+	cache->planes[0].marker->pixelformat = pixelformat;
+
+	if (cache->nplanes == 1)
+		return cache;
+
+	ret = meram_plane_alloc(priv, &cache->planes[1],
+				cfg->icb[1].meram_size);
+	if (ret < 0) {
+		meram_plane_free(priv, &cache->planes[0]);
+		goto error;
+	}
+
+	return cache;
+
+error:
+	kfree(cache);
+	return ERR_PTR(-ENOMEM);
+}
+
+void *sh_mobile_meram_cache_alloc(struct sh_mobile_meram_info *pdata,
+				  const struct sh_mobile_meram_cfg *cfg,
+				  unsigned int xres, unsigned int yres,
+				  unsigned int pixelformat, unsigned int *pitch)
 {
 	struct sh_mobile_meram_fb_cache *cache;
 	struct sh_mobile_meram_priv *priv = pdata->priv;
 	struct platform_device *pdev = pdata->pdev;
+	unsigned int nplanes = is_nvcolor(pixelformat) ? 2 : 1;
 	unsigned int out_pitch;
 
+	if (priv == NULL)
+		return ERR_PTR(-ENODEV);
+
 	if (pixelformat != SH_MOBILE_MERAM_PF_NV &&
 	    pixelformat != SH_MOBILE_MERAM_PF_NV24 &&
 	    pixelformat != SH_MOBILE_MERAM_PF_RGB)
@@ -469,10 +491,16 @@ static void *sh_mobile_meram_register(struct sh_mobile_meram_info *pdata,
 		return ERR_PTR(-EINVAL);
 	}
 
+	if (cfg->icb[0].meram_size == 0)
+		return ERR_PTR(-EINVAL);
+
+	if (nplanes == 2 && cfg->icb[1].meram_size == 0)
+		return ERR_PTR(-EINVAL);
+
 	mutex_lock(&priv->lock);
 
 	/* We now register the ICBs and allocate the MERAM regions. */
-	cache = meram_alloc(priv, cfg, pixelformat);
+	cache = meram_cache_alloc(priv, cfg, pixelformat);
 	if (IS_ERR(cache)) {
 		dev_err(&pdev->dev, "MERAM allocation failed (%ld).",
 			PTR_ERR(cache));
@@ -480,42 +508,50 @@ static void *sh_mobile_meram_register(struct sh_mobile_meram_info *pdata,
 	}
 
 	/* initialize MERAM */
-	meram_init(priv, &cache->planes[0], xres, yres, &out_pitch);
+	meram_plane_init(priv, &cache->planes[0], xres, yres, &out_pitch);
 	*pitch = out_pitch;
 	if (pixelformat == SH_MOBILE_MERAM_PF_NV)
-		meram_init(priv, &cache->planes[1], xres, (yres + 1) / 2,
-			&out_pitch);
+		meram_plane_init(priv, &cache->planes[1],
+				 xres, (yres + 1) / 2, &out_pitch);
 	else if (pixelformat == SH_MOBILE_MERAM_PF_NV24)
-		meram_init(priv, &cache->planes[1], 2 * xres, (yres + 1) / 2,
-			&out_pitch);
+		meram_plane_init(priv, &cache->planes[1],
+				 2 * xres, (yres + 1) / 2, &out_pitch);
 
 err:
 	mutex_unlock(&priv->lock);
 	return cache;
 }
+EXPORT_SYMBOL_GPL(sh_mobile_meram_cache_alloc);
 
-static void
-sh_mobile_meram_unregister(struct sh_mobile_meram_info *pdata, void *data)
+void
+sh_mobile_meram_cache_free(struct sh_mobile_meram_info *pdata, void *data)
 {
 	struct sh_mobile_meram_fb_cache *cache = data;
 	struct sh_mobile_meram_priv *priv = pdata->priv;
 
 	mutex_lock(&priv->lock);
 
-	/* deinit & free */
-	meram_deinit(priv, &cache->planes[0]);
-	if (cache->nplanes == 2)
-		meram_deinit(priv, &cache->planes[1]);
+	/* Cleanup and free. */
+	meram_plane_cleanup(priv, &cache->planes[0]);
+	meram_plane_free(priv, &cache->planes[0]);
+
+	if (cache->nplanes == 2) {
+		meram_plane_cleanup(priv, &cache->planes[1]);
+		meram_plane_free(priv, &cache->planes[1]);
+	}
 
-	meram_free(priv, cache);
+	kfree(cache);
 
 	mutex_unlock(&priv->lock);
 }
-
-static void
-sh_mobile_meram_update(struct sh_mobile_meram_info *pdata, void *data,
-		       unsigned long base_addr_y, unsigned long base_addr_c,
-		       unsigned long *icb_addr_y, unsigned long *icb_addr_c)
+EXPORT_SYMBOL_GPL(sh_mobile_meram_cache_free);
+
+void
+sh_mobile_meram_cache_update(struct sh_mobile_meram_info *pdata, void *data,
+			     unsigned long base_addr_y,
+			     unsigned long base_addr_c,
+			     unsigned long *icb_addr_y,
+			     unsigned long *icb_addr_c)
 {
 	struct sh_mobile_meram_fb_cache *cache = data;
 	struct sh_mobile_meram_priv *priv = pdata->priv;
@@ -527,13 +563,7 @@ sh_mobile_meram_update(struct sh_mobile_meram_info *pdata, void *data,
 
 	mutex_unlock(&priv->lock);
 }
-
-static struct sh_mobile_meram_ops sh_mobile_meram_ops = {
-	.module			= THIS_MODULE,
-	.meram_register		= sh_mobile_meram_register,
-	.meram_unregister	= sh_mobile_meram_unregister,
-	.meram_update		= sh_mobile_meram_update,
-};
+EXPORT_SYMBOL_GPL(sh_mobile_meram_cache_update);
 
 /* -----------------------------------------------------------------------------
  * Power management
@@ -624,7 +654,6 @@ static int __devinit sh_mobile_meram_probe(struct platform_device *pdev)
 	for (i = 0; i < MERAM_ICB_NUM; ++i)
 		priv->icbs[i].index = i;
 
-	pdata->ops = &sh_mobile_meram_ops;
 	pdata->priv = priv;
 	pdata->pdev = pdev;
 
diff --git a/drivers/video/sis/init.c b/drivers/video/sis/init.c
index 66de832361c..f082ae55c0c 100644
--- a/drivers/video/sis/init.c
+++ b/drivers/video/sis/init.c
@@ -2628,7 +2628,8 @@ SiS_SetVCLKState(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
       else if(VCLK >= 135) data = 0x02;
 
       if(SiS_Pr->ChipType == SIS_540) {
-         if((VCLK == 203) || (VCLK < 234)) data = 0x02;
+         /* Was == 203 or < 234 which made no sense */
+         if (VCLK < 234) data = 0x02;
       }
 
       if(SiS_Pr->ChipType < SIS_315H) {
diff --git a/drivers/video/smscufx.c b/drivers/video/smscufx.c
index af3ef27ad36..5533a32c6ca 100644
--- a/drivers/video/smscufx.c
+++ b/drivers/video/smscufx.c
@@ -1,7 +1,7 @@
 /*
  * smscufx.c -- Framebuffer driver for SMSC UFX USB controller
  *
- * Copyright (C) 2011 Steve Glendinning <steve.glendinning@smsc.com>
+ * Copyright (C) 2011 Steve Glendinning <steve.glendinning@shawell.net>
  * Copyright (C) 2009 Roberto De Ioris <roberto@unbit.it>
  * Copyright (C) 2009 Jaya Kumar <jayakumar.lkml@gmail.com>
  * Copyright (C) 2009 Bernie Thompson <bernie@plugable.com>
@@ -904,7 +904,7 @@ static ssize_t ufx_ops_write(struct fb_info *info, const char __user *buf,
 	result = fb_sys_write(info, buf, count, ppos);
 
 	if (result > 0) {
-		int start = max((int)(offset / info->fix.line_length) - 1, 0);
+		int start = max((int)(offset / info->fix.line_length), 0);
 		int lines = min((u32)((result / info->fix.line_length) + 1),
 				(u32)info->var.yres);
 
@@ -1002,7 +1002,7 @@ static int ufx_ops_ioctl(struct fb_info *info, unsigned int cmd,
 	/* TODO: Help propose a standard fb.h ioctl to report mmap damage */
 	if (cmd == UFX_IOCTL_REPORT_DAMAGE) {
 		/* If we have a damage-aware client, turn fb_defio "off"
-		 * To avoid perf imact of unecessary page fault handling.
+		 * To avoid perf imact of unnecessary page fault handling.
 		 * Done by resetting the delay for this fb_info to a very
 		 * long period. Pages will become writable and stay that way.
 		 * Reset to normal value when all clients have closed this fb.
@@ -1466,7 +1466,7 @@ static int ufx_read_edid(struct ufx_data *dev, u8 *edid, int edid_len)
 	/* all FF's in the first 16 bytes indicates nothing is connected */
 	for (i = 0; i < 16; i++) {
 		if (edid[i] != 0xFF) {
-			pr_debug("edid data read succesfully");
+			pr_debug("edid data read successfully");
 			return EDID_LENGTH;
 		}
 	}
@@ -1972,6 +1972,6 @@ MODULE_PARM_DESC(console, "Allow fbcon to be used on this display");
 module_param(fb_defio, bool, S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP);
 MODULE_PARM_DESC(fb_defio, "Enable fb_defio mmap support");
 
-MODULE_AUTHOR("Steve Glendinning <steve.glendinning@smsc.com>");
+MODULE_AUTHOR("Steve Glendinning <steve.glendinning@shawell.net>");
 MODULE_DESCRIPTION("SMSC UFX kernel framebuffer driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/sunxvr500.c b/drivers/video/sunxvr500.c
index b9c2b948d34..eb931b8626f 100644
--- a/drivers/video/sunxvr500.c
+++ b/drivers/video/sunxvr500.c
@@ -12,7 +12,7 @@
 
 #include <asm/io.h>
 
-/* XXX This device has a 'dev-comm' property which aparently is
+/* XXX This device has a 'dev-comm' property which apparently is
  * XXX a pointer into the openfirmware's address space which is
  * XXX a shared area the kernel driver can use to keep OBP
  * XXX informed about the current resolution setting.  The idea
diff --git a/drivers/video/w100fb.c b/drivers/video/w100fb.c
index 90a2e30272a..2f6b2b835f8 100644
--- a/drivers/video/w100fb.c
+++ b/drivers/video/w100fb.c
@@ -1567,6 +1567,18 @@ static void w100_suspend(u32 mode)
 		val = readl(remapped_regs + mmPLL_CNTL);
 		val |= 0x00000004;  /* bit2=1 */
 		writel(val, remapped_regs + mmPLL_CNTL);
+
+		writel(0x00000000, remapped_regs + mmLCDD_CNTL1);
+		writel(0x00000000, remapped_regs + mmLCDD_CNTL2);
+		writel(0x00000000, remapped_regs + mmGENLCD_CNTL1);
+		writel(0x00000000, remapped_regs + mmGENLCD_CNTL2);
+		writel(0x00000000, remapped_regs + mmGENLCD_CNTL3);
+
+		val = readl(remapped_regs + mmMEM_EXT_CNTL);
+		val |= 0xF0000000;
+		val &= ~(0x00000001);
+		writel(val, remapped_regs + mmMEM_EXT_CNTL);
+
 		writel(0x0000001d, remapped_regs + mmPWRMGT_CNTL);
 	}
 }
diff --git a/drivers/virtio/virtio.c b/drivers/virtio/virtio.c
index f3558070e37..c3b3f7f0d9d 100644
--- a/drivers/virtio/virtio.c
+++ b/drivers/virtio/virtio.c
@@ -141,8 +141,11 @@ static int virtio_dev_probe(struct device *_d)
 	err = drv->probe(dev);
 	if (err)
 		add_status(dev, VIRTIO_CONFIG_S_FAILED);
-	else
+	else {
 		add_status(dev, VIRTIO_CONFIG_S_DRIVER_OK);
+		if (drv->scan)
+			drv->scan(dev);
+	}
 
 	return err;
 }
diff --git a/drivers/vme/bridges/vme_tsi148.c b/drivers/vme/bridges/vme_tsi148.c
index f6385f7a66d..880d9242e34 100644
--- a/drivers/vme/bridges/vme_tsi148.c
+++ b/drivers/vme/bridges/vme_tsi148.c
@@ -1263,12 +1263,55 @@ static ssize_t tsi148_master_read(struct vme_master_resource *image, void *buf,
 	u32 aspace, cycle, dwidth;
 	struct vme_bus_error *vme_err = NULL;
 	struct vme_bridge *tsi148_bridge;
+	void *addr = image->kern_base + offset;
+	unsigned int done = 0;
+	unsigned int count32;
 
 	tsi148_bridge = image->parent;
 
 	spin_lock(&image->lock);
 
-	memcpy_fromio(buf, image->kern_base + offset, (unsigned int)count);
+	/* The following code handles VME address alignment. We cannot use
+	 * memcpy_xxx directly here because it may cut small data transfers in
+	 * to 8-bit cycles, thus making D16 cycle impossible.
+	 * On the other hand, the bridge itself assures that the maximum data
+	 * cycle configured for the transfer is used and splits it
+	 * automatically for non-aligned addresses, so we don't want the
+	 * overhead of needlessly forcing small transfers for the entire cycle.
+	 */
+	if ((uintptr_t)addr & 0x1) {
+		*(u8 *)buf = ioread8(addr);
+		done += 1;
+		if (done == count)
+			goto out;
+	}
+	if ((uintptr_t)addr & 0x2) {
+		if ((count - done) < 2) {
+			*(u8 *)(buf + done) = ioread8(addr + done);
+			done += 1;
+			goto out;
+		} else {
+			*(u16 *)(buf + done) = ioread16(addr + done);
+			done += 2;
+		}
+	}
+
+	count32 = (count - done) & ~0x3;
+	if (count32 > 0) {
+		memcpy_fromio(buf + done, addr + done, count32);
+		done += count32;
+	}
+
+	if ((count - done) & 0x2) {
+		*(u16 *)(buf + done) = ioread16(addr + done);
+		done += 2;
+	}
+	if ((count - done) & 0x1) {
+		*(u8 *)(buf + done) = ioread8(addr + done);
+		done += 1;
+	}
+
+out:
 	retval = count;
 
 	if (!err_chk)
@@ -1301,6 +1344,9 @@ static ssize_t tsi148_master_write(struct vme_master_resource *image, void *buf,
 	int retval = 0, enabled;
 	unsigned long long vme_base, size;
 	u32 aspace, cycle, dwidth;
+	void *addr = image->kern_base + offset;
+	unsigned int done = 0;
+	unsigned int count32;
 
 	struct vme_bus_error *vme_err = NULL;
 	struct vme_bridge *tsi148_bridge;
@@ -1312,7 +1358,42 @@ static ssize_t tsi148_master_write(struct vme_master_resource *image, void *buf,
 
 	spin_lock(&image->lock);
 
-	memcpy_toio(image->kern_base + offset, buf, (unsigned int)count);
+	/* Here we apply for the same strategy we do in master_read
+	 * function in order to assure D16 cycle when required.
+	 */
+	if ((uintptr_t)addr & 0x1) {
+		iowrite8(*(u8 *)buf, addr);
+		done += 1;
+		if (done == count)
+			goto out;
+	}
+	if ((uintptr_t)addr & 0x2) {
+		if ((count - done) < 2) {
+			iowrite8(*(u8 *)(buf + done), addr + done);
+			done += 1;
+			goto out;
+		} else {
+			iowrite16(*(u16 *)(buf + done), addr + done);
+			done += 2;
+		}
+	}
+
+	count32 = (count - done) & ~0x3;
+	if (count32 > 0) {
+		memcpy_toio(addr + done, buf + done, count32);
+		done += count32;
+	}
+
+	if ((count - done) & 0x2) {
+		iowrite16(*(u16 *)(buf + done), addr + done);
+		done += 2;
+	}
+	if ((count - done) & 0x1) {
+		iowrite8(*(u8 *)(buf + done), addr + done);
+		done += 1;
+	}
+
+out:
 	retval = count;
 
 	/*
diff --git a/drivers/w1/masters/Kconfig b/drivers/w1/masters/Kconfig
index 979d6eed9a0..5ceb1cd5019 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 SOC_OMAP2430 || ARCH_OMAP3
+	depends on ARCH_OMAP2PLUS
 	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 a0c8965c1a7..530a2d30906 100644
--- a/drivers/w1/masters/ds1wm.c
+++ b/drivers/w1/masters/ds1wm.c
@@ -334,7 +334,9 @@ static void ds1wm_search(void *data, struct w1_master *master_dev,
 			return;
 		}
 
+		mutex_lock(&master_dev->bus_mutex);
 		if (ds1wm_reset(ds1wm_data)) {
+			mutex_unlock(&master_dev->bus_mutex);
 			dev_dbg(&ds1wm_data->pdev->dev,
 				"pass: %d reset error (or no slaves)\n", pass);
 			break;
@@ -387,6 +389,7 @@ static void ds1wm_search(void *data, struct w1_master *master_dev,
 
 		}
 		if (ds1wm_data->read_error) {
+			mutex_unlock(&master_dev->bus_mutex);
 			dev_err(&ds1wm_data->pdev->dev,
 				"pass: %d read error, retrying\n", pass);
 			break;
@@ -400,6 +403,7 @@ static void ds1wm_search(void *data, struct w1_master *master_dev,
 		dev_dbg(&ds1wm_data->pdev->dev,
 			"pass: %d resetting bus\n", pass);
 		ds1wm_reset(ds1wm_data);
+		mutex_unlock(&master_dev->bus_mutex);
 		if ((r_prime & ((u64)1 << 63)) && (d & ((u64)1 << 63))) {
 			dev_err(&ds1wm_data->pdev->dev,
 				"pass: %d bus error, retrying\n", pass);
diff --git a/drivers/w1/masters/omap_hdq.c b/drivers/w1/masters/omap_hdq.c
index 5ef385bfed1..4b0fcf3c2d0 100644
--- a/drivers/w1/masters/omap_hdq.c
+++ b/drivers/w1/masters/omap_hdq.c
@@ -1,7 +1,7 @@
 /*
  * drivers/w1/masters/omap_hdq.c
  *
- * Copyright (C) 2007 Texas Instruments, Inc.
+ * Copyright (C) 2007,2012 Texas Instruments, Inc.
  *
  * This file is licensed under the terms of the GNU General Public License
  * version 2. This program is licensed "as is" without any warranty of any
@@ -14,9 +14,9 @@
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/err.h>
-#include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/sched.h>
+#include <linux/pm_runtime.h>
 
 #include <asm/irq.h>
 #include <mach/hardware.h>
@@ -61,8 +61,6 @@ struct hdq_data {
 	/* lock status update */
 	struct  mutex		hdq_mutex;
 	int			hdq_usecount;
-	struct	clk		*hdq_ick;
-	struct	clk		*hdq_fck;
 	u8			hdq_irqstatus;
 	/* device lock */
 	spinlock_t		hdq_spinlock;
@@ -102,20 +100,20 @@ static struct w1_bus_master omap_w1_master = {
 /* HDQ register I/O routines */
 static inline u8 hdq_reg_in(struct hdq_data *hdq_data, u32 offset)
 {
-	return __raw_readb(hdq_data->hdq_base + offset);
+	return __raw_readl(hdq_data->hdq_base + offset);
 }
 
 static inline void hdq_reg_out(struct hdq_data *hdq_data, u32 offset, u8 val)
 {
-	__raw_writeb(val, hdq_data->hdq_base + offset);
+	__raw_writel(val, hdq_data->hdq_base + offset);
 }
 
 static inline u8 hdq_reg_merge(struct hdq_data *hdq_data, u32 offset,
 			u8 val, u8 mask)
 {
-	u8 new_val = (__raw_readb(hdq_data->hdq_base + offset) & ~mask)
+	u8 new_val = (__raw_readl(hdq_data->hdq_base + offset) & ~mask)
 			| (val & mask);
-	__raw_writeb(new_val, hdq_data->hdq_base + offset);
+	__raw_writel(new_val, hdq_data->hdq_base + offset);
 
 	return new_val;
 }
@@ -180,6 +178,7 @@ static int hdq_write_byte(struct hdq_data *hdq_data, u8 val, u8 *status)
 		hdq_data->hdq_irqstatus, OMAP_HDQ_TIMEOUT);
 	if (ret == 0) {
 		dev_dbg(hdq_data->dev, "TX wait elapsed\n");
+		ret = -ETIMEDOUT;
 		goto out;
 	}
 
@@ -187,7 +186,7 @@ static int hdq_write_byte(struct hdq_data *hdq_data, u8 val, u8 *status)
 	/* check irqstatus */
 	if (!(*status & OMAP_HDQ_INT_STATUS_TXCOMPLETE)) {
 		dev_dbg(hdq_data->dev, "timeout waiting for"
-			"TXCOMPLETE/RXCOMPLETE, %x", *status);
+			" TXCOMPLETE/RXCOMPLETE, %x", *status);
 		ret = -ETIMEDOUT;
 		goto out;
 	}
@@ -198,7 +197,7 @@ static int hdq_write_byte(struct hdq_data *hdq_data, u8 val, u8 *status)
 			OMAP_HDQ_FLAG_CLEAR, &tmp_status);
 	if (ret) {
 		dev_dbg(hdq_data->dev, "timeout waiting GO bit"
-			"return to zero, %x", tmp_status);
+			" return to zero, %x", tmp_status);
 	}
 
 out:
@@ -341,7 +340,7 @@ static int omap_hdq_break(struct hdq_data *hdq_data)
 			&tmp_status);
 	if (ret)
 		dev_dbg(hdq_data->dev, "timeout waiting INIT&GO bits"
-			"return to zero, %x", tmp_status);
+			" return to zero, %x", tmp_status);
 
 out:
 	mutex_unlock(&hdq_data->hdq_mutex);
@@ -353,7 +352,6 @@ static int hdq_read_byte(struct hdq_data *hdq_data, u8 *val)
 {
 	int ret = 0;
 	u8 status;
-	unsigned long timeout = jiffies + OMAP_HDQ_TIMEOUT;
 
 	ret = mutex_lock_interruptible(&hdq_data->hdq_mutex);
 	if (ret < 0) {
@@ -371,22 +369,20 @@ static int hdq_read_byte(struct hdq_data *hdq_data, u8 *val)
 			OMAP_HDQ_CTRL_STATUS_DIR | OMAP_HDQ_CTRL_STATUS_GO,
 			OMAP_HDQ_CTRL_STATUS_DIR | OMAP_HDQ_CTRL_STATUS_GO);
 		/*
-		 * The RX comes immediately after TX. It
-		 * triggers another interrupt before we
-		 * sleep. So we have to wait for RXCOMPLETE bit.
+		 * The RX comes immediately after TX.
 		 */
-		while (!(hdq_data->hdq_irqstatus
-			& OMAP_HDQ_INT_STATUS_RXCOMPLETE)
-			&& time_before(jiffies, timeout)) {
-			schedule_timeout_uninterruptible(1);
-		}
+		wait_event_timeout(hdq_wait_queue,
+				   (hdq_data->hdq_irqstatus
+				    & OMAP_HDQ_INT_STATUS_RXCOMPLETE),
+				   OMAP_HDQ_TIMEOUT);
+
 		hdq_reg_merge(hdq_data, OMAP_HDQ_CTRL_STATUS, 0,
 			OMAP_HDQ_CTRL_STATUS_DIR);
 		status = hdq_data->hdq_irqstatus;
 		/* check irqstatus */
 		if (!(status & OMAP_HDQ_INT_STATUS_RXCOMPLETE)) {
 			dev_dbg(hdq_data->dev, "timeout waiting for"
-				"RXCOMPLETE, %x", status);
+				" RXCOMPLETE, %x", status);
 			ret = -ETIMEDOUT;
 			goto out;
 		}
@@ -396,7 +392,7 @@ static int hdq_read_byte(struct hdq_data *hdq_data, u8 *val)
 out:
 	mutex_unlock(&hdq_data->hdq_mutex);
 rtn:
-	return 0;
+	return ret;
 
 }
 
@@ -419,17 +415,8 @@ static int omap_hdq_get(struct hdq_data *hdq_data)
 		hdq_data->hdq_usecount++;
 		try_module_get(THIS_MODULE);
 		if (1 == hdq_data->hdq_usecount) {
-			if (clk_enable(hdq_data->hdq_ick)) {
-				dev_dbg(hdq_data->dev, "Can not enable ick\n");
-				ret = -ENODEV;
-				goto clk_err;
-			}
-			if (clk_enable(hdq_data->hdq_fck)) {
-				dev_dbg(hdq_data->dev, "Can not enable fck\n");
-				clk_disable(hdq_data->hdq_ick);
-				ret = -ENODEV;
-				goto clk_err;
-			}
+
+			pm_runtime_get_sync(hdq_data->dev);
 
 			/* make sure HDQ is out of reset */
 			if (!(hdq_reg_in(hdq_data, OMAP_HDQ_SYSSTATUS) &
@@ -450,9 +437,6 @@ static int omap_hdq_get(struct hdq_data *hdq_data)
 		}
 	}
 
-clk_err:
-	clk_put(hdq_data->hdq_ick);
-	clk_put(hdq_data->hdq_fck);
 out:
 	mutex_unlock(&hdq_data->hdq_mutex);
 rtn:
@@ -470,15 +454,13 @@ static int omap_hdq_put(struct hdq_data *hdq_data)
 
 	if (0 == hdq_data->hdq_usecount) {
 		dev_dbg(hdq_data->dev, "attempt to decrement use count"
-			"when it is zero");
+			" when it is zero");
 		ret = -EINVAL;
 	} else {
 		hdq_data->hdq_usecount--;
 		module_put(THIS_MODULE);
-		if (0 == hdq_data->hdq_usecount) {
-			clk_disable(hdq_data->hdq_ick);
-			clk_disable(hdq_data->hdq_fck);
-		}
+		if (0 == hdq_data->hdq_usecount)
+			pm_runtime_put_sync(hdq_data->dev);
 	}
 	mutex_unlock(&hdq_data->hdq_mutex);
 
@@ -540,7 +522,7 @@ static void omap_w1_write_byte(void *_hdq, u8 byte)
 	mutex_unlock(&hdq_data->hdq_mutex);
 
 	ret = hdq_write_byte(hdq_data, byte, &status);
-	if (ret == 0) {
+	if (ret < 0) {
 		dev_dbg(hdq_data->dev, "TX failure:Ctrl status %x\n", status);
 		return;
 	}
@@ -591,35 +573,11 @@ static int __devinit omap_hdq_probe(struct platform_device *pdev)
 		goto err_ioremap;
 	}
 
-	/* get interface & functional clock objects */
-	hdq_data->hdq_ick = clk_get(&pdev->dev, "ick");
-	if (IS_ERR(hdq_data->hdq_ick)) {
-		dev_dbg(&pdev->dev, "Can't get HDQ ick clock object\n");
-		ret = PTR_ERR(hdq_data->hdq_ick);
-		goto err_ick;
-	}
-
-	hdq_data->hdq_fck = clk_get(&pdev->dev, "fck");
-	if (IS_ERR(hdq_data->hdq_fck)) {
-		dev_dbg(&pdev->dev, "Can't get HDQ fck clock object\n");
-		ret = PTR_ERR(hdq_data->hdq_fck);
-		goto err_fck;
-	}
-
 	hdq_data->hdq_usecount = 0;
 	mutex_init(&hdq_data->hdq_mutex);
 
-	if (clk_enable(hdq_data->hdq_ick)) {
-		dev_dbg(&pdev->dev, "Can not enable ick\n");
-		ret = -ENODEV;
-		goto err_intfclk;
-	}
-
-	if (clk_enable(hdq_data->hdq_fck)) {
-		dev_dbg(&pdev->dev, "Can not enable fck\n");
-		ret = -ENODEV;
-		goto err_fnclk;
-	}
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
 
 	rev = hdq_reg_in(hdq_data, OMAP_HDQ_REVISION);
 	dev_info(&pdev->dev, "OMAP HDQ Hardware Rev %c.%c. Driver in %s mode\n",
@@ -641,9 +599,7 @@ static int __devinit omap_hdq_probe(struct platform_device *pdev)
 
 	omap_hdq_break(hdq_data);
 
-	/* don't clock the HDQ until it is needed */
-	clk_disable(hdq_data->hdq_ick);
-	clk_disable(hdq_data->hdq_fck);
+	pm_runtime_put_sync(&pdev->dev);
 
 	omap_w1_master.data = hdq_data;
 
@@ -655,20 +611,11 @@ static int __devinit omap_hdq_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_w1:
 err_irq:
-	clk_disable(hdq_data->hdq_fck);
-
-err_fnclk:
-	clk_disable(hdq_data->hdq_ick);
-
-err_intfclk:
-	clk_put(hdq_data->hdq_fck);
-
-err_fck:
-	clk_put(hdq_data->hdq_ick);
+	pm_runtime_put_sync(&pdev->dev);
+err_w1:
+	pm_runtime_disable(&pdev->dev);
 
-err_ick:
 	iounmap(hdq_data->hdq_base);
 
 err_ioremap:
@@ -696,8 +643,7 @@ static int omap_hdq_remove(struct platform_device *pdev)
 	mutex_unlock(&hdq_data->hdq_mutex);
 
 	/* remove module dependency */
-	clk_put(hdq_data->hdq_ick);
-	clk_put(hdq_data->hdq_fck);
+	pm_runtime_disable(&pdev->dev);
 	free_irq(INT_24XX_HDQ_IRQ, hdq_data);
 	platform_set_drvdata(pdev, NULL);
 	iounmap(hdq_data->hdq_base);
diff --git a/drivers/w1/slaves/Kconfig b/drivers/w1/slaves/Kconfig
index eb9e376d624..67526690acb 100644
--- a/drivers/w1/slaves/Kconfig
+++ b/drivers/w1/slaves/Kconfig
@@ -94,6 +94,19 @@ config W1_SLAVE_DS2781
 
 	  If you are unsure, say N.
 
+config W1_SLAVE_DS28E04
+	tristate "4096-Bit Addressable 1-Wire EEPROM with PIO (DS28E04-100)"
+	depends on W1
+	select CRC16
+	help
+	  If you enable this you will have the DS28E04-100
+	  chip support.
+
+	  Say Y here if you want to use a 1-wire
+	  4kb EEPROM with PIO family device (DS28E04).
+
+	  If you are unsure, say N.
+
 config W1_SLAVE_BQ27000
 	tristate "BQ27000 slave support"
 	depends on W1
diff --git a/drivers/w1/slaves/Makefile b/drivers/w1/slaves/Makefile
index c4f1859fb52..05188f6aab5 100644
--- a/drivers/w1/slaves/Makefile
+++ b/drivers/w1/slaves/Makefile
@@ -12,3 +12,4 @@ obj-$(CONFIG_W1_SLAVE_DS2760)	+= w1_ds2760.o
 obj-$(CONFIG_W1_SLAVE_DS2780)	+= w1_ds2780.o
 obj-$(CONFIG_W1_SLAVE_DS2781)	+= w1_ds2781.o
 obj-$(CONFIG_W1_SLAVE_BQ27000)	+= w1_bq27000.o
+obj-$(CONFIG_W1_SLAVE_DS28E04)	+= w1_ds28e04.o
diff --git a/drivers/w1/slaves/w1_bq27000.c b/drivers/w1/slaves/w1_bq27000.c
index 52ad812fa1e..773dca5beaf 100644
--- a/drivers/w1/slaves/w1_bq27000.c
+++ b/drivers/w1/slaves/w1_bq27000.c
@@ -31,10 +31,10 @@ static int w1_bq27000_read(struct device *dev, unsigned int reg)
 	u8 val;
 	struct w1_slave *sl = container_of(dev->parent, struct w1_slave, dev);
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 	w1_write_8(sl->master, HDQ_CMD_READ | reg);
 	val = w1_read_8(sl->master);
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return val;
 }
diff --git a/drivers/w1/slaves/w1_ds2408.c b/drivers/w1/slaves/w1_ds2408.c
index 8e813eed0f0..441ad3a3b58 100644
--- a/drivers/w1/slaves/w1_ds2408.c
+++ b/drivers/w1/slaves/w1_ds2408.c
@@ -52,11 +52,11 @@ static int _read_reg(struct w1_slave *sl, u8 address, unsigned char* buf)
 	if (!buf)
 		return -EINVAL;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 	dev_dbg(&sl->dev, "mutex locked");
 
 	if (w1_reset_select_slave(sl)) {
-		mutex_unlock(&sl->master->mutex);
+		mutex_unlock(&sl->master->bus_mutex);
 		return -EIO;
 	}
 
@@ -66,7 +66,7 @@ static int _read_reg(struct w1_slave *sl, u8 address, unsigned char* buf)
 	w1_write_block(sl->master, wrbuf, 3);
 	*buf = w1_read_8(sl->master);
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 	dev_dbg(&sl->dev, "mutex unlocked");
 	return 1;
 }
@@ -165,7 +165,7 @@ static ssize_t w1_f29_write_output(
 		return -EFAULT;
 
 	dev_dbg(&sl->dev, "locking mutex for write_output");
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 	dev_dbg(&sl->dev, "mutex locked");
 
 	if (w1_reset_select_slave(sl))
@@ -200,14 +200,14 @@ static ssize_t w1_f29_write_output(
 		/* read the result of the READ_PIO_REGS command */
 		if (w1_read_8(sl->master) == *buf) {
 			/* success! */
-			mutex_unlock(&sl->master->mutex);
+			mutex_unlock(&sl->master->bus_mutex);
 			dev_dbg(&sl->dev,
 				"mutex unlocked, retries:%d", retries);
 			return 1;
 		}
 	}
 error:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 	dev_dbg(&sl->dev, "mutex unlocked in error, retries:%d", retries);
 
 	return -EIO;
@@ -228,7 +228,7 @@ static ssize_t w1_f29_write_activity(
 	if (count != 1 || off != 0)
 		return -EFAULT;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	if (w1_reset_select_slave(sl))
 		goto error;
@@ -236,7 +236,7 @@ static ssize_t w1_f29_write_activity(
 	while (retries--) {
 		w1_write_8(sl->master, W1_F29_FUNC_RESET_ACTIVITY_LATCHES);
 		if (w1_read_8(sl->master) == W1_F29_SUCCESS_CONFIRM_BYTE) {
-			mutex_unlock(&sl->master->mutex);
+			mutex_unlock(&sl->master->bus_mutex);
 			return 1;
 		}
 		if (w1_reset_resume_command(sl->master))
@@ -244,7 +244,7 @@ static ssize_t w1_f29_write_activity(
 	}
 
 error:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 	return -EIO;
 }
 
@@ -263,7 +263,7 @@ static ssize_t w1_f29_write_status_control(
 	if (count != 1 || off != 0)
 		return -EFAULT;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	if (w1_reset_select_slave(sl))
 		goto error;
@@ -285,12 +285,12 @@ static ssize_t w1_f29_write_status_control(
 		w1_write_block(sl->master, w1_buf, 3);
 		if (w1_read_8(sl->master) == *buf) {
 			/* success! */
-			mutex_unlock(&sl->master->mutex);
+			mutex_unlock(&sl->master->bus_mutex);
 			return 1;
 		}
 	}
 error:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return -EIO;
 }
diff --git a/drivers/w1/slaves/w1_ds2423.c b/drivers/w1/slaves/w1_ds2423.c
index 7a7dbe5026f..40a10b5ed12 100644
--- a/drivers/w1/slaves/w1_ds2423.c
+++ b/drivers/w1/slaves/w1_ds2423.c
@@ -66,7 +66,7 @@ static ssize_t w1_counter_read(struct device *device,
 	wrbuf[0]	= 0xA5;
 	wrbuf[1]	= rom_addr & 0xFF;
 	wrbuf[2]	= rom_addr >> 8;
-	mutex_lock(&dev->mutex);
+	mutex_lock(&dev->bus_mutex);
 	if (!w1_reset_select_slave(sl)) {
 		w1_write_block(dev, wrbuf, 3);
 		read_byte_count = 0;
@@ -124,7 +124,7 @@ static ssize_t w1_counter_read(struct device *device,
 	} else {
 		c -= snprintf(out_buf + PAGE_SIZE - c, c, "Connection error");
 	}
-	mutex_unlock(&dev->mutex);
+	mutex_unlock(&dev->bus_mutex);
 	return PAGE_SIZE - c;
 }
 
diff --git a/drivers/w1/slaves/w1_ds2431.c b/drivers/w1/slaves/w1_ds2431.c
index 84e2410aec1..984b30331a4 100644
--- a/drivers/w1/slaves/w1_ds2431.c
+++ b/drivers/w1/slaves/w1_ds2431.c
@@ -107,7 +107,7 @@ static ssize_t w1_f2d_read_bin(struct file *filp, struct kobject *kobj,
 	if (count == 0)
 		return 0;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	/* read directly from the EEPROM in chunks of W1_F2D_READ_MAXLEN */
 	while (todo > 0) {
@@ -126,7 +126,7 @@ static ssize_t w1_f2d_read_bin(struct file *filp, struct kobject *kobj,
 		off += W1_F2D_READ_MAXLEN;
 	}
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return count;
 }
@@ -214,7 +214,7 @@ static ssize_t w1_f2d_write_bin(struct file *filp, struct kobject *kobj,
 	if (count == 0)
 		return 0;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	/* Can only write data in blocks of the size of the scratchpad */
 	addr = off;
@@ -259,7 +259,7 @@ static ssize_t w1_f2d_write_bin(struct file *filp, struct kobject *kobj,
 	}
 
 out_up:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return count;
 }
diff --git a/drivers/w1/slaves/w1_ds2433.c b/drivers/w1/slaves/w1_ds2433.c
index 0f7b8f9c509..85f2cdb27fa 100644
--- a/drivers/w1/slaves/w1_ds2433.c
+++ b/drivers/w1/slaves/w1_ds2433.c
@@ -107,7 +107,7 @@ static ssize_t w1_f23_read_bin(struct file *filp, struct kobject *kobj,
 	if ((count = w1_f23_fix_count(off, count, W1_EEPROM_SIZE)) == 0)
 		return 0;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 #ifdef CONFIG_W1_SLAVE_DS2433_CRC
 
@@ -138,7 +138,7 @@ static ssize_t w1_f23_read_bin(struct file *filp, struct kobject *kobj,
 #endif	/* CONFIG_W1_SLAVE_DS2433_CRC */
 
 out_up:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return count;
 }
@@ -233,7 +233,7 @@ static ssize_t w1_f23_write_bin(struct file *filp, struct kobject *kobj,
 	}
 #endif	/* CONFIG_W1_SLAVE_DS2433_CRC */
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	/* Can only write data to one page at a time */
 	idx = 0;
@@ -251,7 +251,7 @@ static ssize_t w1_f23_write_bin(struct file *filp, struct kobject *kobj,
 	}
 
 out_up:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return count;
 }
diff --git a/drivers/w1/slaves/w1_ds2760.c b/drivers/w1/slaves/w1_ds2760.c
index 5754c9a4f58..aa7bd5fa2fa 100644
--- a/drivers/w1/slaves/w1_ds2760.c
+++ b/drivers/w1/slaves/w1_ds2760.c
@@ -31,7 +31,7 @@ static int w1_ds2760_io(struct device *dev, char *buf, int addr, size_t count,
 	if (!dev)
 		return 0;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	if (addr > DS2760_DATA_SIZE || addr < 0) {
 		count = 0;
@@ -54,7 +54,7 @@ static int w1_ds2760_io(struct device *dev, char *buf, int addr, size_t count,
 	}
 
 out:
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return count;
 }
@@ -76,14 +76,14 @@ static int w1_ds2760_eeprom_cmd(struct device *dev, int addr, int cmd)
 	if (!dev)
 		return -EINVAL;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	if (w1_reset_select_slave(sl) == 0) {
 		w1_write_8(sl->master, cmd);
 		w1_write_8(sl->master, addr);
 	}
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 	return 0;
 }
 
diff --git a/drivers/w1/slaves/w1_ds2780.c b/drivers/w1/slaves/w1_ds2780.c
index 39f78c0b143..7b09307de0e 100644
--- a/drivers/w1/slaves/w1_ds2780.c
+++ b/drivers/w1/slaves/w1_ds2780.c
@@ -60,30 +60,16 @@ int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
 	if (!dev)
 		return -ENODEV;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	ret = w1_ds2780_do_io(dev, buf, addr, count, io);
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return ret;
 }
 EXPORT_SYMBOL(w1_ds2780_io);
 
-int w1_ds2780_io_nolock(struct device *dev, char *buf, int addr, size_t count,
-			int io)
-{
-	int ret;
-
-	if (!dev)
-		return -ENODEV;
-
-	ret = w1_ds2780_do_io(dev, buf, addr, count, io);
-
-	return ret;
-}
-EXPORT_SYMBOL(w1_ds2780_io_nolock);
-
 int w1_ds2780_eeprom_cmd(struct device *dev, int addr, int cmd)
 {
 	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
@@ -91,14 +77,14 @@ int w1_ds2780_eeprom_cmd(struct device *dev, int addr, int cmd)
 	if (!dev)
 		return -EINVAL;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	if (w1_reset_select_slave(sl) == 0) {
 		w1_write_8(sl->master, cmd);
 		w1_write_8(sl->master, addr);
 	}
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 	return 0;
 }
 EXPORT_SYMBOL(w1_ds2780_eeprom_cmd);
diff --git a/drivers/w1/slaves/w1_ds2780.h b/drivers/w1/slaves/w1_ds2780.h
index 73737936502..a1fba79eb1b 100644
--- a/drivers/w1/slaves/w1_ds2780.h
+++ b/drivers/w1/slaves/w1_ds2780.h
@@ -124,8 +124,6 @@
 
 extern int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
 			int io);
-extern int w1_ds2780_io_nolock(struct device *dev, char *buf, int addr,
-			size_t count, int io);
 extern int w1_ds2780_eeprom_cmd(struct device *dev, int addr, int cmd);
 
 #endif /* !_W1_DS2780_H */
diff --git a/drivers/w1/slaves/w1_ds2781.c b/drivers/w1/slaves/w1_ds2781.c
index 0d0c7985293..877daf74159 100644
--- a/drivers/w1/slaves/w1_ds2781.c
+++ b/drivers/w1/slaves/w1_ds2781.c
@@ -58,30 +58,16 @@ int w1_ds2781_io(struct device *dev, char *buf, int addr, size_t count,
 	if (!dev)
 		return -ENODEV;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	ret = w1_ds2781_do_io(dev, buf, addr, count, io);
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 
 	return ret;
 }
 EXPORT_SYMBOL(w1_ds2781_io);
 
-int w1_ds2781_io_nolock(struct device *dev, char *buf, int addr, size_t count,
-			int io)
-{
-	int ret;
-
-	if (!dev)
-		return -ENODEV;
-
-	ret = w1_ds2781_do_io(dev, buf, addr, count, io);
-
-	return ret;
-}
-EXPORT_SYMBOL(w1_ds2781_io_nolock);
-
 int w1_ds2781_eeprom_cmd(struct device *dev, int addr, int cmd)
 {
 	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
@@ -89,14 +75,14 @@ int w1_ds2781_eeprom_cmd(struct device *dev, int addr, int cmd)
 	if (!dev)
 		return -EINVAL;
 
-	mutex_lock(&sl->master->mutex);
+	mutex_lock(&sl->master->bus_mutex);
 
 	if (w1_reset_select_slave(sl) == 0) {
 		w1_write_8(sl->master, cmd);
 		w1_write_8(sl->master, addr);
 	}
 
-	mutex_unlock(&sl->master->mutex);
+	mutex_unlock(&sl->master->bus_mutex);
 	return 0;
 }
 EXPORT_SYMBOL(w1_ds2781_eeprom_cmd);
diff --git a/drivers/w1/slaves/w1_ds2781.h b/drivers/w1/slaves/w1_ds2781.h
index 82bc66497b4..557dfb0b4f6 100644
--- a/drivers/w1/slaves/w1_ds2781.h
+++ b/drivers/w1/slaves/w1_ds2781.h
@@ -129,8 +129,6 @@
 
 extern int w1_ds2781_io(struct device *dev, char *buf, int addr, size_t count,
 			int io);
-extern int w1_ds2781_io_nolock(struct device *dev, char *buf, int addr,
-			size_t count, int io);
 extern int w1_ds2781_eeprom_cmd(struct device *dev, int addr, int cmd);
 
 #endif /* !_W1_DS2781_H */
diff --git a/drivers/w1/slaves/w1_ds28e04.c b/drivers/w1/slaves/w1_ds28e04.c
new file mode 100644
index 00000000000..98117db595b
--- /dev/null
+++ b/drivers/w1/slaves/w1_ds28e04.c
@@ -0,0 +1,469 @@
+/*
+ *	w1_ds28e04.c - w1 family 1C (DS28E04) driver
+ *
+ * Copyright (c) 2012 Markus Franke <franke.m@sebakmt.com>
+ *
+ * This source code is licensed under the GNU General Public License,
+ * Version 2. See the file COPYING for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/crc16.h>
+#include <linux/uaccess.h>
+
+#define CRC16_INIT		0
+#define CRC16_VALID		0xb001
+
+#include "../w1.h"
+#include "../w1_int.h"
+#include "../w1_family.h"
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Markus Franke <franke.m@sebakmt.com>, <franm@hrz.tu-chemnitz.de>");
+MODULE_DESCRIPTION("w1 family 1C driver for DS28E04, 4kb EEPROM and PIO");
+
+/* Allow the strong pullup to be disabled, but default to enabled.
+ * If it was disabled a parasite powered device might not get the required
+ * current to copy the data from the scratchpad to EEPROM.  If it is enabled
+ * parasite powered devices have a better chance of getting the current
+ * required.
+ */
+static int w1_strong_pullup = 1;
+module_param_named(strong_pullup, w1_strong_pullup, int, 0);
+
+/* enable/disable CRC checking on DS28E04-100 memory accesses */
+static char w1_enable_crccheck = 1;
+
+#define W1_EEPROM_SIZE		512
+#define W1_PAGE_COUNT		16
+#define W1_PAGE_SIZE		32
+#define W1_PAGE_BITS		5
+#define W1_PAGE_MASK		0x1F
+
+#define W1_F1C_READ_EEPROM	0xF0
+#define W1_F1C_WRITE_SCRATCH	0x0F
+#define W1_F1C_READ_SCRATCH	0xAA
+#define W1_F1C_COPY_SCRATCH	0x55
+#define W1_F1C_ACCESS_WRITE	0x5A
+
+#define W1_1C_REG_LOGIC_STATE	0x220
+
+struct w1_f1C_data {
+	u8	memory[W1_EEPROM_SIZE];
+	u32	validcrc;
+};
+
+/**
+ * Check the file size bounds and adjusts count as needed.
+ * This would not be needed if the file size didn't reset to 0 after a write.
+ */
+static inline size_t w1_f1C_fix_count(loff_t off, size_t count, size_t size)
+{
+	if (off > size)
+		return 0;
+
+	if ((off + count) > size)
+		return size - off;
+
+	return count;
+}
+
+static int w1_f1C_refresh_block(struct w1_slave *sl, struct w1_f1C_data *data,
+				int block)
+{
+	u8	wrbuf[3];
+	int	off = block * W1_PAGE_SIZE;
+
+	if (data->validcrc & (1 << block))
+		return 0;
+
+	if (w1_reset_select_slave(sl)) {
+		data->validcrc = 0;
+		return -EIO;
+	}
+
+	wrbuf[0] = W1_F1C_READ_EEPROM;
+	wrbuf[1] = off & 0xff;
+	wrbuf[2] = off >> 8;
+	w1_write_block(sl->master, wrbuf, 3);
+	w1_read_block(sl->master, &data->memory[off], W1_PAGE_SIZE);
+
+	/* cache the block if the CRC is valid */
+	if (crc16(CRC16_INIT, &data->memory[off], W1_PAGE_SIZE) == CRC16_VALID)
+		data->validcrc |= (1 << block);
+
+	return 0;
+}
+
+static int w1_f1C_read(struct w1_slave *sl, int addr, int len, char *data)
+{
+	u8 wrbuf[3];
+
+	/* read directly from the EEPROM */
+	if (w1_reset_select_slave(sl))
+		return -EIO;
+
+	wrbuf[0] = W1_F1C_READ_EEPROM;
+	wrbuf[1] = addr & 0xff;
+	wrbuf[2] = addr >> 8;
+
+	w1_write_block(sl->master, wrbuf, sizeof(wrbuf));
+	return w1_read_block(sl->master, data, len);
+}
+
+static ssize_t w1_f1C_read_bin(struct file *filp, struct kobject *kobj,
+			       struct bin_attribute *bin_attr,
+			       char *buf, loff_t off, size_t count)
+{
+	struct w1_slave *sl = kobj_to_w1_slave(kobj);
+	struct w1_f1C_data *data = sl->family_data;
+	int i, min_page, max_page;
+
+	count = w1_f1C_fix_count(off, count, W1_EEPROM_SIZE);
+	if (count == 0)
+		return 0;
+
+	mutex_lock(&sl->master->mutex);
+
+	if (w1_enable_crccheck) {
+		min_page = (off >> W1_PAGE_BITS);
+		max_page = (off + count - 1) >> W1_PAGE_BITS;
+		for (i = min_page; i <= max_page; i++) {
+			if (w1_f1C_refresh_block(sl, data, i)) {
+				count = -EIO;
+				goto out_up;
+			}
+		}
+		memcpy(buf, &data->memory[off], count);
+	} else {
+		count = w1_f1C_read(sl, off, count, buf);
+	}
+
+out_up:
+	mutex_unlock(&sl->master->mutex);
+
+	return count;
+}
+
+/**
+ * Writes to the scratchpad and reads it back for verification.
+ * Then copies the scratchpad to EEPROM.
+ * The data must be on one page.
+ * The master must be locked.
+ *
+ * @param sl	The slave structure
+ * @param addr	Address for the write
+ * @param len   length must be <= (W1_PAGE_SIZE - (addr & W1_PAGE_MASK))
+ * @param data	The data to write
+ * @return	0=Success -1=failure
+ */
+static int w1_f1C_write(struct w1_slave *sl, int addr, int len, const u8 *data)
+{
+	u8 wrbuf[4];
+	u8 rdbuf[W1_PAGE_SIZE + 3];
+	u8 es = (addr + len - 1) & 0x1f;
+	unsigned int tm = 10;
+	int i;
+	struct w1_f1C_data *f1C = sl->family_data;
+
+	/* Write the data to the scratchpad */
+	if (w1_reset_select_slave(sl))
+		return -1;
+
+	wrbuf[0] = W1_F1C_WRITE_SCRATCH;
+	wrbuf[1] = addr & 0xff;
+	wrbuf[2] = addr >> 8;
+
+	w1_write_block(sl->master, wrbuf, 3);
+	w1_write_block(sl->master, data, len);
+
+	/* Read the scratchpad and verify */
+	if (w1_reset_select_slave(sl))
+		return -1;
+
+	w1_write_8(sl->master, W1_F1C_READ_SCRATCH);
+	w1_read_block(sl->master, rdbuf, len + 3);
+
+	/* Compare what was read against the data written */
+	if ((rdbuf[0] != wrbuf[1]) || (rdbuf[1] != wrbuf[2]) ||
+	    (rdbuf[2] != es) || (memcmp(data, &rdbuf[3], len) != 0))
+		return -1;
+
+	/* Copy the scratchpad to EEPROM */
+	if (w1_reset_select_slave(sl))
+		return -1;
+
+	wrbuf[0] = W1_F1C_COPY_SCRATCH;
+	wrbuf[3] = es;
+
+	for (i = 0; i < sizeof(wrbuf); ++i) {
+		/* issue 10ms strong pullup (or delay) on the last byte
+		   for writing the data from the scratchpad to EEPROM */
+		if (w1_strong_pullup && i == sizeof(wrbuf)-1)
+			w1_next_pullup(sl->master, tm);
+
+		w1_write_8(sl->master, wrbuf[i]);
+	}
+
+	if (!w1_strong_pullup)
+		msleep(tm);
+
+	if (w1_enable_crccheck) {
+		/* invalidate cached data */
+		f1C->validcrc &= ~(1 << (addr >> W1_PAGE_BITS));
+	}
+
+	/* Reset the bus to wake up the EEPROM (this may not be needed) */
+	w1_reset_bus(sl->master);
+
+	return 0;
+}
+
+static ssize_t w1_f1C_write_bin(struct file *filp, struct kobject *kobj,
+			       struct bin_attribute *bin_attr,
+			       char *buf, loff_t off, size_t count)
+
+{
+	struct w1_slave *sl = kobj_to_w1_slave(kobj);
+	int addr, len, idx;
+
+	count = w1_f1C_fix_count(off, count, W1_EEPROM_SIZE);
+	if (count == 0)
+		return 0;
+
+	if (w1_enable_crccheck) {
+		/* can only write full blocks in cached mode */
+		if ((off & W1_PAGE_MASK) || (count & W1_PAGE_MASK)) {
+			dev_err(&sl->dev, "invalid offset/count off=%d cnt=%zd\n",
+				(int)off, count);
+			return -EINVAL;
+		}
+
+		/* make sure the block CRCs are valid */
+		for (idx = 0; idx < count; idx += W1_PAGE_SIZE) {
+			if (crc16(CRC16_INIT, &buf[idx], W1_PAGE_SIZE)
+				!= CRC16_VALID) {
+				dev_err(&sl->dev, "bad CRC at offset %d\n",
+					(int)off);
+				return -EINVAL;
+			}
+		}
+	}
+
+	mutex_lock(&sl->master->mutex);
+
+	/* Can only write data to one page at a time */
+	idx = 0;
+	while (idx < count) {
+		addr = off + idx;
+		len = W1_PAGE_SIZE - (addr & W1_PAGE_MASK);
+		if (len > (count - idx))
+			len = count - idx;
+
+		if (w1_f1C_write(sl, addr, len, &buf[idx]) < 0) {
+			count = -EIO;
+			goto out_up;
+		}
+		idx += len;
+	}
+
+out_up:
+	mutex_unlock(&sl->master->mutex);
+
+	return count;
+}
+
+static ssize_t w1_f1C_read_pio(struct file *filp, struct kobject *kobj,
+			       struct bin_attribute *bin_attr,
+			       char *buf, loff_t off, size_t count)
+
+{
+	struct w1_slave *sl = kobj_to_w1_slave(kobj);
+	int ret;
+
+	/* check arguments */
+	if (off != 0 || count != 1 || buf == NULL)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->mutex);
+	ret = w1_f1C_read(sl, W1_1C_REG_LOGIC_STATE, count, buf);
+	mutex_unlock(&sl->master->mutex);
+
+	return ret;
+}
+
+static ssize_t w1_f1C_write_pio(struct file *filp, struct kobject *kobj,
+				struct bin_attribute *bin_attr,
+				char *buf, loff_t off, size_t count)
+
+{
+	struct w1_slave *sl = kobj_to_w1_slave(kobj);
+	u8 wrbuf[3];
+	u8 ack;
+
+	/* check arguments */
+	if (off != 0 || count != 1 || buf == NULL)
+		return -EINVAL;
+
+	mutex_lock(&sl->master->mutex);
+
+	/* Write the PIO data */
+	if (w1_reset_select_slave(sl)) {
+		mutex_unlock(&sl->master->mutex);
+		return -1;
+	}
+
+	/* set bit 7..2 to value '1' */
+	*buf = *buf | 0xFC;
+
+	wrbuf[0] = W1_F1C_ACCESS_WRITE;
+	wrbuf[1] = *buf;
+	wrbuf[2] = ~(*buf);
+	w1_write_block(sl->master, wrbuf, 3);
+
+	w1_read_block(sl->master, &ack, sizeof(ack));
+
+	mutex_unlock(&sl->master->mutex);
+
+	/* check for acknowledgement */
+	if (ack != 0xAA)
+		return -EIO;
+
+	return count;
+}
+
+static ssize_t w1_f1C_show_crccheck(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	if (put_user(w1_enable_crccheck + 0x30, buf))
+		return -EFAULT;
+
+	return sizeof(w1_enable_crccheck);
+}
+
+static ssize_t w1_f1C_store_crccheck(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	char val;
+
+	if (count != 1 || !buf)
+		return -EINVAL;
+
+	if (get_user(val, buf))
+		return -EFAULT;
+
+	/* convert to decimal */
+	val = val - 0x30;
+	if (val != 0 && val != 1)
+		return -EINVAL;
+
+	/* set the new value */
+	w1_enable_crccheck = val;
+
+	return sizeof(w1_enable_crccheck);
+}
+
+#define NB_SYSFS_BIN_FILES 2
+static struct bin_attribute w1_f1C_bin_attr[NB_SYSFS_BIN_FILES] = {
+	{
+		.attr = {
+			.name = "eeprom",
+			.mode = S_IRUGO | S_IWUSR,
+		},
+		.size = W1_EEPROM_SIZE,
+		.read = w1_f1C_read_bin,
+		.write = w1_f1C_write_bin,
+	},
+	{
+		.attr = {
+			.name = "pio",
+			.mode = S_IRUGO | S_IWUSR,
+		},
+		.size = 1,
+		.read = w1_f1C_read_pio,
+		.write = w1_f1C_write_pio,
+	}
+};
+
+static DEVICE_ATTR(crccheck, S_IWUSR | S_IRUGO,
+		   w1_f1C_show_crccheck, w1_f1C_store_crccheck);
+
+static int w1_f1C_add_slave(struct w1_slave *sl)
+{
+	int err = 0;
+	int i;
+	struct w1_f1C_data *data = NULL;
+
+	if (w1_enable_crccheck) {
+		data = kzalloc(sizeof(struct w1_f1C_data), GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+		sl->family_data = data;
+	}
+
+	/* create binary sysfs attributes */
+	for (i = 0; i < NB_SYSFS_BIN_FILES && !err; ++i)
+		err = sysfs_create_bin_file(
+			&sl->dev.kobj, &(w1_f1C_bin_attr[i]));
+
+	if (!err) {
+		/* create device attributes */
+		err = device_create_file(&sl->dev, &dev_attr_crccheck);
+	}
+
+	if (err) {
+		/* remove binary sysfs attributes */
+		for (i = 0; i < NB_SYSFS_BIN_FILES; ++i)
+			sysfs_remove_bin_file(
+				&sl->dev.kobj, &(w1_f1C_bin_attr[i]));
+
+		kfree(data);
+	}
+
+	return err;
+}
+
+static void w1_f1C_remove_slave(struct w1_slave *sl)
+{
+	int i;
+
+	kfree(sl->family_data);
+	sl->family_data = NULL;
+
+	/* remove device attributes */
+	device_remove_file(&sl->dev, &dev_attr_crccheck);
+
+	/* remove binary sysfs attributes */
+	for (i = 0; i < NB_SYSFS_BIN_FILES; ++i)
+		sysfs_remove_bin_file(&sl->dev.kobj, &(w1_f1C_bin_attr[i]));
+}
+
+static struct w1_family_ops w1_f1C_fops = {
+	.add_slave      = w1_f1C_add_slave,
+	.remove_slave   = w1_f1C_remove_slave,
+};
+
+static struct w1_family w1_family_1C = {
+	.fid = W1_FAMILY_DS28E04,
+	.fops = &w1_f1C_fops,
+};
+
+static int __init w1_f1C_init(void)
+{
+	return w1_register_family(&w1_family_1C);
+}
+
+static void __exit w1_f1C_fini(void)
+{
+	w1_unregister_family(&w1_family_1C);
+}
+
+module_init(w1_f1C_init);
+module_exit(w1_f1C_fini);
diff --git a/drivers/w1/slaves/w1_therm.c b/drivers/w1/slaves/w1_therm.c
index ff29ae747ee..d90062b211f 100644
--- a/drivers/w1/slaves/w1_therm.c
+++ b/drivers/w1/slaves/w1_therm.c
@@ -179,7 +179,7 @@ static ssize_t w1_therm_read(struct device *device,
 	int i, max_trying = 10;
 	ssize_t c = PAGE_SIZE;
 
-	i = mutex_lock_interruptible(&dev->mutex);
+	i = mutex_lock_interruptible(&dev->bus_mutex);
 	if (i != 0)
 		return i;
 
@@ -207,19 +207,19 @@ static ssize_t w1_therm_read(struct device *device,
 			w1_write_8(dev, W1_CONVERT_TEMP);
 
 			if (external_power) {
-				mutex_unlock(&dev->mutex);
+				mutex_unlock(&dev->bus_mutex);
 
 				sleep_rem = msleep_interruptible(tm);
 				if (sleep_rem != 0)
 					return -EINTR;
 
-				i = mutex_lock_interruptible(&dev->mutex);
+				i = mutex_lock_interruptible(&dev->bus_mutex);
 				if (i != 0)
 					return i;
 			} else if (!w1_strong_pullup) {
 				sleep_rem = msleep_interruptible(tm);
 				if (sleep_rem != 0) {
-					mutex_unlock(&dev->mutex);
+					mutex_unlock(&dev->bus_mutex);
 					return -EINTR;
 				}
 			}
@@ -258,7 +258,7 @@ static ssize_t w1_therm_read(struct device *device,
 
 	c -= snprintf(buf + PAGE_SIZE - c, c, "t=%d\n",
 		w1_convert_temp(rom, sl->family->fid));
-	mutex_unlock(&dev->mutex);
+	mutex_unlock(&dev->bus_mutex);
 
 	return PAGE_SIZE - c;
 }
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 2f2e894ea0c..1a574370d2c 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -557,7 +557,7 @@ static int w1_uevent(struct device *dev, struct kobj_uevent_env *env)
 	struct w1_master *md = NULL;
 	struct w1_slave *sl = NULL;
 	char *event_owner, *name;
-	int err;
+	int err = 0;
 
 	if (dev->driver == &w1_master_driver) {
 		md = container_of(dev, struct w1_master, dev);
@@ -576,19 +576,17 @@ static int w1_uevent(struct device *dev, struct kobj_uevent_env *env)
 			event_owner, name, dev_name(dev));
 
 	if (dev->driver != &w1_slave_driver || !sl)
-		return 0;
+		goto end;
 
 	err = add_uevent_var(env, "W1_FID=%02X", sl->reg_num.family);
 	if (err)
-		return err;
+		goto end;
 
 	err = add_uevent_var(env, "W1_SLAVE_ID=%024LX",
 			     (unsigned long long)sl->reg_num.id);
-	if (err)
-		return err;
-
-	return 0;
-};
+end:
+	return err;
+}
 #else
 static int w1_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
@@ -887,16 +885,21 @@ void w1_search(struct w1_master *dev, u8 search_type, w1_slave_found_callback cb
 		 *
 		 * Return 0 - device(s) present, 1 - no devices present.
 		 */
+		mutex_lock(&dev->bus_mutex);
 		if (w1_reset_bus(dev)) {
+			mutex_unlock(&dev->bus_mutex);
 			dev_dbg(&dev->dev, "No devices present on the wire.\n");
 			break;
 		}
 
 		/* Do fast search on single slave bus */
 		if (dev->max_slave_count == 1) {
+			int rv;
 			w1_write_8(dev, W1_READ_ROM);
+			rv = w1_read_block(dev, (u8 *)&rn, 8);
+			mutex_unlock(&dev->bus_mutex);
 
-			if (w1_read_block(dev, (u8 *)&rn, 8) == 8 && rn)
+			if (rv == 8 && rn)
 				cb(dev, rn);
 
 			break;
@@ -929,10 +932,12 @@ void w1_search(struct w1_master *dev, u8 search_type, w1_slave_found_callback cb
 			rn |= (tmp64 << i);
 
 			if (kthread_should_stop()) {
+				mutex_unlock(&dev->bus_mutex);
 				dev_dbg(&dev->dev, "Abort w1_search\n");
 				return;
 			}
 		}
+		mutex_unlock(&dev->bus_mutex);
 
 		if ( (triplet_ret & 0x03) != 0x03 ) {
 			if ( (desc_bit == last_zero) || (last_zero < 0))
diff --git a/drivers/w1/w1.h b/drivers/w1/w1.h
index 4d012ca3f32..45908e56c2f 100644
--- a/drivers/w1/w1.h
+++ b/drivers/w1/w1.h
@@ -180,6 +180,7 @@ struct w1_master
 
 	struct task_struct	*thread;
 	struct mutex		mutex;
+	struct mutex		bus_mutex;
 
 	struct device_driver	*driver;
 	struct device		dev;
diff --git a/drivers/w1/w1_family.h b/drivers/w1/w1_family.h
index 874aeb05011..b00ada44a89 100644
--- a/drivers/w1/w1_family.h
+++ b/drivers/w1/w1_family.h
@@ -30,6 +30,7 @@
 #define W1_FAMILY_SMEM_01	0x01
 #define W1_FAMILY_SMEM_81	0x81
 #define W1_THERM_DS18S20 	0x10
+#define W1_FAMILY_DS28E04	0x1C
 #define W1_COUNTER_DS2423	0x1D
 #define W1_THERM_DS1822  	0x22
 #define W1_EEPROM_DS2433  	0x23
diff --git a/drivers/w1/w1_int.c b/drivers/w1/w1_int.c
index 68288355727..5a98649f6ab 100644
--- a/drivers/w1/w1_int.c
+++ b/drivers/w1/w1_int.c
@@ -76,6 +76,7 @@ static struct w1_master * w1_alloc_dev(u32 id, int slave_count, int slave_ttl,
 
 	INIT_LIST_HEAD(&dev->slist);
 	mutex_init(&dev->mutex);
+	mutex_init(&dev->bus_mutex);
 
 	memcpy(&dev->dev, device, sizeof(struct device));
 	dev_set_name(&dev->dev, "w1_bus_master%u", dev->id);
@@ -117,7 +118,7 @@ int w1_add_master_device(struct w1_bus_master *master)
 		return(-EINVAL);
         }
 	/* While it would be electrically possible to make a device that
-	 * generated a strong pullup in bit bang mode, only hardare that
+	 * generated a strong pullup in bit bang mode, only hardware that
 	 * controls 1-wire time frames are even expected to support a strong
 	 * pullup.  w1_io.c would need to support calling set_pullup before
 	 * the last write_bit operation of a w1_write_8 which it currently
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index fe819b76de5..53d75719078 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -279,6 +279,7 @@ config DAVINCI_WATCHDOG
 config ORION_WATCHDOG
 	tristate "Orion watchdog"
 	depends on ARCH_ORION5X || ARCH_KIRKWOOD
+	select WATCHDOG_CORE
 	help
 	  Say Y here if to include support for the watchdog timer
 	  in the Marvell Orion5x and Kirkwood ARM SoCs.
@@ -578,6 +579,7 @@ config INTEL_SCU_WATCHDOG
 config ITCO_WDT
 	tristate "Intel TCO Timer/Watchdog"
 	depends on (X86 || IA64) && PCI
+	select WATCHDOG_CORE
 	select LPC_ICH
 	---help---
 	  Hardware driver for the intel TCO timer based watchdog devices.
@@ -1115,10 +1117,10 @@ config BOOKE_WDT
 config BOOKE_WDT_DEFAULT_TIMEOUT
 	int "PowerPC Book-E Watchdog Timer Default Timeout"
 	depends on BOOKE_WDT
-	default 38 if FSL_BOOKE
-	range 0 63 if FSL_BOOKE
-	default 3 if !FSL_BOOKE
-	range 0 3 if !FSL_BOOKE
+	default 38 if PPC_FSL_BOOK3E
+	range 0 63 if PPC_FSL_BOOK3E
+	default 3 if !PPC_FSL_BOOK3E
+	range 0 3 if !PPC_FSL_BOOK3E
 	help
 	  Select the default watchdog timer period to be used by the PowerPC
 	  Book-E watchdog driver.  A watchdog "event" occurs when the bit
diff --git a/drivers/watchdog/bcm63xx_wdt.c b/drivers/watchdog/bcm63xx_wdt.c
index 8379dc32fd9..551880bfd62 100644
--- a/drivers/watchdog/bcm63xx_wdt.c
+++ b/drivers/watchdog/bcm63xx_wdt.c
@@ -302,7 +302,7 @@ static void bcm63xx_wdt_shutdown(struct platform_device *pdev)
 	bcm63xx_wdt_pause();
 }
 
-static struct platform_driver bcm63xx_wdt = {
+static struct platform_driver bcm63xx_wdt_driver = {
 	.probe	= bcm63xx_wdt_probe,
 	.remove = __devexit_p(bcm63xx_wdt_remove),
 	.shutdown = bcm63xx_wdt_shutdown,
@@ -312,7 +312,7 @@ static struct platform_driver bcm63xx_wdt = {
 	}
 };
 
-module_platform_driver(bcm63xx_wdt);
+module_platform_driver(bcm63xx_wdt_driver);
 
 MODULE_AUTHOR("Miguel Gaio <miguel.gaio@efixo.com>");
 MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
diff --git a/drivers/watchdog/booke_wdt.c b/drivers/watchdog/booke_wdt.c
index ce0ab4415ef..3fe82d0e8ca 100644
--- a/drivers/watchdog/booke_wdt.c
+++ b/drivers/watchdog/booke_wdt.c
@@ -37,7 +37,7 @@
 u32 booke_wdt_enabled;
 u32 booke_wdt_period = CONFIG_BOOKE_WDT_DEFAULT_TIMEOUT;
 
-#ifdef	CONFIG_FSL_BOOKE
+#ifdef	CONFIG_PPC_FSL_BOOK3E
 #define WDTP(x)		((((x)&0x3)<<30)|(((x)&0x3c)<<15))
 #define WDTP_MASK	(WDTP(0x3f))
 #else
@@ -190,7 +190,7 @@ static long booke_wdt_ioctl(struct file *file,
 	case WDIOC_SETTIMEOUT:
 		if (get_user(tmp, p))
 			return -EFAULT;
-#ifdef	CONFIG_FSL_BOOKE
+#ifdef	CONFIG_PPC_FSL_BOOK3E
 		/* period of 1 gives the largest possible timeout */
 		if (tmp > period_to_sec(1))
 			return -EINVAL;
diff --git a/drivers/watchdog/coh901327_wdt.c b/drivers/watchdog/coh901327_wdt.c
index 6876430a9f5..cb5da5c3ece 100644
--- a/drivers/watchdog/coh901327_wdt.c
+++ b/drivers/watchdog/coh901327_wdt.c
@@ -263,6 +263,7 @@ static int __exit coh901327_remove(struct platform_device *pdev)
 	watchdog_unregister_device(&coh901327_wdt);
 	coh901327_disable();
 	free_irq(irq, pdev);
+	clk_unprepare(clk);
 	clk_put(clk);
 	iounmap(virtbase);
 	release_mem_region(phybase, physize);
@@ -300,9 +301,9 @@ static int __init coh901327_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "could not get clock\n");
 		goto out_no_clk;
 	}
-	ret = clk_enable(clk);
+	ret = clk_prepare_enable(clk);
 	if (ret) {
-		dev_err(&pdev->dev, "could not enable clock\n");
+		dev_err(&pdev->dev, "could not prepare and enable clock\n");
 		goto out_no_clk_enable;
 	}
 
@@ -369,7 +370,7 @@ static int __init coh901327_probe(struct platform_device *pdev)
 out_no_wdog:
 	free_irq(irq, pdev);
 out_no_irq:
-	clk_disable(clk);
+	clk_disable_unprepare(clk);
 out_no_clk_enable:
 	clk_put(clk);
 out_no_clk:
diff --git a/drivers/watchdog/f71808e_wdt.c b/drivers/watchdog/f71808e_wdt.c
index c65b0a5a020..016bd935519 100644
--- a/drivers/watchdog/f71808e_wdt.c
+++ b/drivers/watchdog/f71808e_wdt.c
@@ -56,6 +56,7 @@
 #define SIO_F71858_ID		0x0507	/* Chipset ID */
 #define SIO_F71862_ID		0x0601	/* Chipset ID */
 #define SIO_F71869_ID		0x0814	/* Chipset ID */
+#define SIO_F71869A_ID		0x1007	/* Chipset ID */
 #define SIO_F71882_ID		0x0541	/* Chipset ID */
 #define SIO_F71889_ID		0x0723	/* Chipset ID */
 
@@ -195,7 +196,7 @@ static inline int superio_enter(int base)
 		return -EBUSY;
 	}
 
-	/* according to the datasheet the key must be send twice! */
+	/* according to the datasheet the key must be sent twice! */
 	outb(SIO_UNLOCK_KEY, base);
 	outb(SIO_UNLOCK_KEY, base);
 
@@ -756,6 +757,7 @@ static int __init f71808e_find(int sioaddr)
 		err = f71862fg_pin_configure(0); /* validate module parameter */
 		break;
 	case SIO_F71869_ID:
+	case SIO_F71869A_ID:
 		watchdog.type = f71869;
 		break;
 	case SIO_F71882_ID:
diff --git a/drivers/watchdog/iTCO_wdt.c b/drivers/watchdog/iTCO_wdt.c
index 9c2c27c3b42..ceed39f2601 100644
--- a/drivers/watchdog/iTCO_wdt.c
+++ b/drivers/watchdog/iTCO_wdt.c
@@ -47,7 +47,7 @@
 
 /* Module and version information */
 #define DRV_NAME	"iTCO_wdt"
-#define DRV_VERSION	"1.07"
+#define DRV_VERSION	"1.10"
 
 /* Includes */
 #include <linux/module.h>		/* For module specific items */
@@ -88,8 +88,6 @@
 #define TCOv2_TMR	(TCOBASE + 0x12) /* TCOv2 Timer Initial Value	*/
 
 /* internal variables */
-static unsigned long is_active;
-static char expect_release;
 static struct {		/* this is private data for the iTCO_wdt device */
 	/* TCO version/generation */
 	unsigned int iTCO_version;
@@ -106,12 +104,12 @@ static struct {		/* this is private data for the iTCO_wdt device */
 } iTCO_wdt_private;
 
 /* module parameters */
-#define WATCHDOG_HEARTBEAT 30	/* 30 sec default heartbeat */
-static int heartbeat = WATCHDOG_HEARTBEAT;  /* in seconds */
+#define WATCHDOG_TIMEOUT 30	/* 30 sec default heartbeat */
+static int heartbeat = WATCHDOG_TIMEOUT;  /* in seconds */
 module_param(heartbeat, int, 0);
 MODULE_PARM_DESC(heartbeat, "Watchdog timeout in seconds. "
 	"5..76 (TCO v1) or 3..614 (TCO v2), default="
-				__MODULE_STRING(WATCHDOG_HEARTBEAT) ")");
+				__MODULE_STRING(WATCHDOG_TIMEOUT) ")");
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
 module_param(nowayout, bool, 0);
@@ -178,13 +176,13 @@ static int iTCO_wdt_unset_NO_REBOOT_bit(void)
 	return ret; /* returns: 0 = OK, -EIO = Error */
 }
 
-static int iTCO_wdt_start(void)
+static int iTCO_wdt_start(struct watchdog_device *wd_dev)
 {
 	unsigned int val;
 
 	spin_lock(&iTCO_wdt_private.io_lock);
 
-	iTCO_vendor_pre_start(iTCO_wdt_private.smi_res, heartbeat);
+	iTCO_vendor_pre_start(iTCO_wdt_private.smi_res, wd_dev->timeout);
 
 	/* disable chipset's NO_REBOOT bit */
 	if (iTCO_wdt_unset_NO_REBOOT_bit()) {
@@ -212,7 +210,7 @@ static int iTCO_wdt_start(void)
 	return 0;
 }
 
-static int iTCO_wdt_stop(void)
+static int iTCO_wdt_stop(struct watchdog_device *wd_dev)
 {
 	unsigned int val;
 
@@ -236,11 +234,11 @@ static int iTCO_wdt_stop(void)
 	return 0;
 }
 
-static int iTCO_wdt_keepalive(void)
+static int iTCO_wdt_ping(struct watchdog_device *wd_dev)
 {
 	spin_lock(&iTCO_wdt_private.io_lock);
 
-	iTCO_vendor_pre_keepalive(iTCO_wdt_private.smi_res, heartbeat);
+	iTCO_vendor_pre_keepalive(iTCO_wdt_private.smi_res, wd_dev->timeout);
 
 	/* Reload the timer by writing to the TCO Timer Counter register */
 	if (iTCO_wdt_private.iTCO_version == 2)
@@ -257,7 +255,7 @@ static int iTCO_wdt_keepalive(void)
 	return 0;
 }
 
-static int iTCO_wdt_set_heartbeat(int t)
+static int iTCO_wdt_set_timeout(struct watchdog_device *wd_dev, unsigned int t)
 {
 	unsigned int val16;
 	unsigned char val8;
@@ -304,14 +302,15 @@ static int iTCO_wdt_set_heartbeat(int t)
 			return -EINVAL;
 	}
 
-	heartbeat = t;
+	wd_dev->timeout = t;
 	return 0;
 }
 
-static int iTCO_wdt_get_timeleft(int *time_left)
+static unsigned int iTCO_wdt_get_timeleft(struct watchdog_device *wd_dev)
 {
 	unsigned int val16;
 	unsigned char val8;
+	unsigned int time_left = 0;
 
 	/* read the TCO Timer */
 	if (iTCO_wdt_private.iTCO_version == 2) {
@@ -320,7 +319,7 @@ static int iTCO_wdt_get_timeleft(int *time_left)
 		val16 &= 0x3ff;
 		spin_unlock(&iTCO_wdt_private.io_lock);
 
-		*time_left = (val16 * 6) / 10;
+		time_left = (val16 * 6) / 10;
 	} else if (iTCO_wdt_private.iTCO_version == 1) {
 		spin_lock(&iTCO_wdt_private.io_lock);
 		val8 = inb(TCO_RLD);
@@ -329,156 +328,35 @@ static int iTCO_wdt_get_timeleft(int *time_left)
 			val8 += (inb(TCOv1_TMR) & 0x3f);
 		spin_unlock(&iTCO_wdt_private.io_lock);
 
-		*time_left = (val8 * 6) / 10;
-	} else
-		return -EINVAL;
-	return 0;
-}
-
-/*
- *	/dev/watchdog handling
- */
-
-static int iTCO_wdt_open(struct inode *inode, struct file *file)
-{
-	/* /dev/watchdog can only be opened once */
-	if (test_and_set_bit(0, &is_active))
-		return -EBUSY;
-
-	/*
-	 *      Reload and activate timer
-	 */
-	iTCO_wdt_start();
-	return nonseekable_open(inode, file);
-}
-
-static int iTCO_wdt_release(struct inode *inode, struct file *file)
-{
-	/*
-	 *      Shut off the timer.
-	 */
-	if (expect_release == 42) {
-		iTCO_wdt_stop();
-	} else {
-		pr_crit("Unexpected close, not stopping watchdog!\n");
-		iTCO_wdt_keepalive();
-	}
-	clear_bit(0, &is_active);
-	expect_release = 0;
-	return 0;
-}
-
-static ssize_t iTCO_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;
-
-			/* note: just in case someone wrote the magic
-			   character five months ago... */
-			expect_release = 0;
-
-			/* 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 = 42;
-			}
-		}
-
-		/* someone wrote to us, we should reload the timer */
-		iTCO_wdt_keepalive();
-	}
-	return len;
-}
-
-static long iTCO_wdt_ioctl(struct file *file, unsigned int cmd,
-							unsigned long arg)
-{
-	int new_options, retval = -EINVAL;
-	int new_heartbeat;
-	void __user *argp = (void __user *)arg;
-	int __user *p = argp;
-	static const struct watchdog_info ident = {
-		.options =		WDIOF_SETTIMEOUT |
-					WDIOF_KEEPALIVEPING |
-					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, p);
-
-	case WDIOC_SETOPTIONS:
-	{
-		if (get_user(new_options, p))
-			return -EFAULT;
-
-		if (new_options & WDIOS_DISABLECARD) {
-			iTCO_wdt_stop();
-			retval = 0;
-		}
-		if (new_options & WDIOS_ENABLECARD) {
-			iTCO_wdt_keepalive();
-			iTCO_wdt_start();
-			retval = 0;
-		}
-		return retval;
-	}
-	case WDIOC_KEEPALIVE:
-		iTCO_wdt_keepalive();
-		return 0;
-
-	case WDIOC_SETTIMEOUT:
-	{
-		if (get_user(new_heartbeat, p))
-			return -EFAULT;
-		if (iTCO_wdt_set_heartbeat(new_heartbeat))
-			return -EINVAL;
-		iTCO_wdt_keepalive();
-		/* Fall */
-	}
-	case WDIOC_GETTIMEOUT:
-		return put_user(heartbeat, p);
-	case WDIOC_GETTIMELEFT:
-	{
-		int time_left;
-		if (iTCO_wdt_get_timeleft(&time_left))
-			return -EINVAL;
-		return put_user(time_left, p);
-	}
-	default:
-		return -ENOTTY;
+		time_left = (val8 * 6) / 10;
 	}
+	return time_left;
 }
 
 /*
  *	Kernel Interfaces
  */
 
-static const struct file_operations iTCO_wdt_fops = {
+static const struct watchdog_info ident = {
+	.options =		WDIOF_SETTIMEOUT |
+				WDIOF_KEEPALIVEPING |
+				WDIOF_MAGICCLOSE,
+	.firmware_version =	0,
+	.identity =		DRV_NAME,
+};
+
+static const struct watchdog_ops iTCO_wdt_ops = {
 	.owner =		THIS_MODULE,
-	.llseek =		no_llseek,
-	.write =		iTCO_wdt_write,
-	.unlocked_ioctl =	iTCO_wdt_ioctl,
-	.open =			iTCO_wdt_open,
-	.release =		iTCO_wdt_release,
+	.start =		iTCO_wdt_start,
+	.stop = 		iTCO_wdt_stop,
+	.ping = 		iTCO_wdt_ping,
+	.set_timeout =		iTCO_wdt_set_timeout,
+	.get_timeleft =		iTCO_wdt_get_timeleft,
 };
 
-static struct miscdevice iTCO_wdt_miscdev = {
-	.minor =	WATCHDOG_MINOR,
-	.name =		"watchdog",
-	.fops =		&iTCO_wdt_fops,
+static struct watchdog_device iTCO_wdt_watchdog_dev = {
+	.info =		&ident,
+	.ops = 		&iTCO_wdt_ops,
 };
 
 /*
@@ -489,10 +367,10 @@ static void __devexit iTCO_wdt_cleanup(void)
 {
 	/* Stop the timer before we leave */
 	if (!nowayout)
-		iTCO_wdt_stop();
+		iTCO_wdt_stop(&iTCO_wdt_watchdog_dev);
 
 	/* Deregister */
-	misc_deregister(&iTCO_wdt_miscdev);
+	watchdog_unregister_device(&iTCO_wdt_watchdog_dev);
 
 	/* release resources */
 	release_region(iTCO_wdt_private.tco_res->start,
@@ -605,20 +483,25 @@ static int __devinit iTCO_wdt_probe(struct platform_device *dev)
 	outw(0x0002, TCO2_STS);	/* Clear SECOND_TO_STS bit */
 	outw(0x0004, TCO2_STS);	/* Clear BOOT_STS bit */
 
+	iTCO_wdt_watchdog_dev.bootstatus = 0;
+	iTCO_wdt_watchdog_dev.timeout = WATCHDOG_TIMEOUT;
+	watchdog_set_nowayout(&iTCO_wdt_watchdog_dev, nowayout);
+	iTCO_wdt_watchdog_dev.parent = dev->dev.parent;
+
 	/* Make sure the watchdog is not running */
-	iTCO_wdt_stop();
+	iTCO_wdt_stop(&iTCO_wdt_watchdog_dev);
 
 	/* Check that the heartbeat value is within it's range;
 	   if not reset to the default */
-	if (iTCO_wdt_set_heartbeat(heartbeat)) {
-		iTCO_wdt_set_heartbeat(WATCHDOG_HEARTBEAT);
-		pr_info("timeout value out of range, using %d\n", heartbeat);
+	if (iTCO_wdt_set_timeout(&iTCO_wdt_watchdog_dev, heartbeat)) {
+		iTCO_wdt_set_timeout(&iTCO_wdt_watchdog_dev, WATCHDOG_TIMEOUT);
+		pr_info("timeout value out of range, using %d\n",
+			WATCHDOG_TIMEOUT);
 	}
 
-	ret = misc_register(&iTCO_wdt_miscdev);
+	ret = watchdog_register_device(&iTCO_wdt_watchdog_dev);
 	if (ret != 0) {
-		pr_err("cannot register miscdev on minor=%d (err=%d)\n",
-		       WATCHDOG_MINOR, ret);
+		pr_err("cannot register watchdog device (err=%d)\n", ret);
 		goto unreg_tco;
 	}
 
@@ -659,7 +542,7 @@ static int __devexit iTCO_wdt_remove(struct platform_device *dev)
 
 static void iTCO_wdt_shutdown(struct platform_device *dev)
 {
-	iTCO_wdt_stop();
+	iTCO_wdt_stop(NULL);
 }
 
 static struct platform_driver iTCO_wdt_driver = {
diff --git a/drivers/watchdog/ie6xx_wdt.c b/drivers/watchdog/ie6xx_wdt.c
index 5f0d776f902..8f541b94005 100644
--- a/drivers/watchdog/ie6xx_wdt.c
+++ b/drivers/watchdog/ie6xx_wdt.c
@@ -232,7 +232,7 @@ static void __devinit ie6xx_wdt_debugfs_init(void)
 		S_IFREG | S_IRUGO, NULL, NULL, &ie6xx_wdt_dbg_operations);
 }
 
-static void __devexit ie6xx_wdt_debugfs_exit(void)
+static void ie6xx_wdt_debugfs_exit(void)
 {
 	debugfs_remove(ie6xx_wdt_data.debugfs);
 }
@@ -242,7 +242,7 @@ static void __devinit ie6xx_wdt_debugfs_init(void)
 {
 }
 
-static void __devexit ie6xx_wdt_debugfs_exit(void)
+static void ie6xx_wdt_debugfs_exit(void)
 {
 }
 #endif
diff --git a/drivers/watchdog/of_xilinx_wdt.c b/drivers/watchdog/of_xilinx_wdt.c
index 55d2f66dbea..294fb4e0052 100644
--- a/drivers/watchdog/of_xilinx_wdt.c
+++ b/drivers/watchdog/of_xilinx_wdt.c
@@ -297,7 +297,7 @@ static int __devinit xwdt_probe(struct platform_device *pdev)
 
 	no_timeout = 0;
 
-	pfreq = (u32 *)of_get_property(pdev->dev.of_node->parent,
+	pfreq = (u32 *)of_get_property(pdev->dev.of_node,
 					"clock-frequency", NULL);
 
 	if (pfreq == NULL) {
diff --git a/drivers/watchdog/omap_wdt.c b/drivers/watchdog/omap_wdt.c
index 8285d65cd20..fceec4f4eb7 100644
--- a/drivers/watchdog/omap_wdt.c
+++ b/drivers/watchdog/omap_wdt.c
@@ -126,8 +126,6 @@ static void omap_wdt_set_timeout(struct omap_wdt_dev *wdev)
 	u32 pre_margin = GET_WLDR_VAL(timer_margin);
 	void __iomem *base = wdev->base;
 
-	pm_runtime_get_sync(wdev->dev);
-
 	/* just count up at 32 KHz */
 	while (__raw_readl(base + OMAP_WATCHDOG_WPS) & 0x04)
 		cpu_relax();
@@ -135,8 +133,6 @@ static void omap_wdt_set_timeout(struct omap_wdt_dev *wdev)
 	__raw_writel(pre_margin, base + OMAP_WATCHDOG_LDR);
 	while (__raw_readl(base + OMAP_WATCHDOG_WPS) & 0x04)
 		cpu_relax();
-
-	pm_runtime_put_sync(wdev->dev);
 }
 
 /*
@@ -166,8 +162,6 @@ static int omap_wdt_open(struct inode *inode, struct file *file)
 	omap_wdt_ping(wdev); /* trigger loading of new timeout value */
 	omap_wdt_enable(wdev);
 
-	pm_runtime_put_sync(wdev->dev);
-
 	return nonseekable_open(inode, file);
 }
 
@@ -179,8 +173,6 @@ static int omap_wdt_release(struct inode *inode, struct file *file)
 	 *      Shut off the timer unless NOWAYOUT is defined.
 	 */
 #ifndef CONFIG_WATCHDOG_NOWAYOUT
-	pm_runtime_get_sync(wdev->dev);
-
 	omap_wdt_disable(wdev);
 
 	pm_runtime_put_sync(wdev->dev);
@@ -199,11 +191,9 @@ static ssize_t omap_wdt_write(struct file *file, const char __user *data,
 
 	/* Refresh LOAD_TIME. */
 	if (len) {
-		pm_runtime_get_sync(wdev->dev);
 		spin_lock(&wdt_lock);
 		omap_wdt_ping(wdev);
 		spin_unlock(&wdt_lock);
-		pm_runtime_put_sync(wdev->dev);
 	}
 	return len;
 }
@@ -236,18 +226,15 @@ static long omap_wdt_ioctl(struct file *file, unsigned int cmd,
 					(int __user *)arg);
 		return put_user(0, (int __user *)arg);
 	case WDIOC_KEEPALIVE:
-		pm_runtime_get_sync(wdev->dev);
 		spin_lock(&wdt_lock);
 		omap_wdt_ping(wdev);
 		spin_unlock(&wdt_lock);
-		pm_runtime_put_sync(wdev->dev);
 		return 0;
 	case WDIOC_SETTIMEOUT:
 		if (get_user(new_margin, (int __user *)arg))
 			return -EFAULT;
 		omap_wdt_adjust_timeout(new_margin);
 
-		pm_runtime_get_sync(wdev->dev);
 		spin_lock(&wdt_lock);
 		omap_wdt_disable(wdev);
 		omap_wdt_set_timeout(wdev);
@@ -255,7 +242,6 @@ static long omap_wdt_ioctl(struct file *file, unsigned int cmd,
 
 		omap_wdt_ping(wdev);
 		spin_unlock(&wdt_lock);
-		pm_runtime_put_sync(wdev->dev);
 		/* Fall */
 	case WDIOC_GETTIMEOUT:
 		return put_user(timer_margin, (int __user *)arg);
@@ -363,7 +349,6 @@ static void omap_wdt_shutdown(struct platform_device *pdev)
 	struct omap_wdt_dev *wdev = platform_get_drvdata(pdev);
 
 	if (wdev->omap_wdt_users) {
-		pm_runtime_get_sync(wdev->dev);
 		omap_wdt_disable(wdev);
 		pm_runtime_put_sync(wdev->dev);
 	}
@@ -403,7 +388,6 @@ static int omap_wdt_suspend(struct platform_device *pdev, pm_message_t state)
 	struct omap_wdt_dev *wdev = platform_get_drvdata(pdev);
 
 	if (wdev->omap_wdt_users) {
-		pm_runtime_get_sync(wdev->dev);
 		omap_wdt_disable(wdev);
 		pm_runtime_put_sync(wdev->dev);
 	}
@@ -419,7 +403,6 @@ static int omap_wdt_resume(struct platform_device *pdev)
 		pm_runtime_get_sync(wdev->dev);
 		omap_wdt_enable(wdev);
 		omap_wdt_ping(wdev);
-		pm_runtime_put_sync(wdev->dev);
 	}
 
 	return 0;
@@ -430,6 +413,12 @@ static int omap_wdt_resume(struct platform_device *pdev)
 #define	omap_wdt_resume		NULL
 #endif
 
+static const struct of_device_id omap_wdt_of_match[] = {
+	{ .compatible = "ti,omap3-wdt", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_wdt_of_match);
+
 static struct platform_driver omap_wdt_driver = {
 	.probe		= omap_wdt_probe,
 	.remove		= __devexit_p(omap_wdt_remove),
@@ -439,6 +428,7 @@ static struct platform_driver omap_wdt_driver = {
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "omap_wdt",
+		.of_match_table = omap_wdt_of_match,
 	},
 };
 
diff --git a/drivers/watchdog/orion_wdt.c b/drivers/watchdog/orion_wdt.c
index 0f5736949c6..c20f96b579d 100644
--- a/drivers/watchdog/orion_wdt.c
+++ b/drivers/watchdog/orion_wdt.c
@@ -16,22 +16,22 @@
 #include <linux/moduleparam.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
-#include <linux/fs.h>
 #include <linux/miscdevice.h>
 #include <linux/platform_device.h>
 #include <linux/watchdog.h>
 #include <linux/init.h>
-#include <linux/uaccess.h>
 #include <linux/io.h>
 #include <linux/spinlock.h>
 #include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/of.h>
 #include <mach/bridge-regs.h>
 
 /*
  * Watchdog timer block registers.
  */
 #define TIMER_CTRL		0x0000
-#define  WDT_EN			0x0010
+#define WDT_EN			0x0010
 #define WDT_VAL			0x0024
 
 #define WDT_MAX_CYCLE_COUNT	0xffffffff
@@ -44,27 +44,27 @@ static unsigned int wdt_max_duration;	/* (seconds) */
 static struct clk *clk;
 static unsigned int wdt_tclk;
 static void __iomem *wdt_reg;
-static unsigned long wdt_status;
 static DEFINE_SPINLOCK(wdt_lock);
 
-static void orion_wdt_ping(void)
+static int orion_wdt_ping(struct watchdog_device *wdt_dev)
 {
 	spin_lock(&wdt_lock);
 
 	/* Reload watchdog duration */
-	writel(wdt_tclk * heartbeat, wdt_reg + WDT_VAL);
+	writel(wdt_tclk * wdt_dev->timeout, wdt_reg + WDT_VAL);
 
 	spin_unlock(&wdt_lock);
+	return 0;
 }
 
-static void orion_wdt_enable(void)
+static int orion_wdt_start(struct watchdog_device *wdt_dev)
 {
 	u32 reg;
 
 	spin_lock(&wdt_lock);
 
 	/* Set watchdog duration */
-	writel(wdt_tclk * heartbeat, wdt_reg + WDT_VAL);
+	writel(wdt_tclk * wdt_dev->timeout, wdt_reg + WDT_VAL);
 
 	/* Clear watchdog timer interrupt */
 	reg = readl(BRIDGE_CAUSE);
@@ -82,9 +82,10 @@ static void orion_wdt_enable(void)
 	writel(reg, RSTOUTn_MASK);
 
 	spin_unlock(&wdt_lock);
+	return 0;
 }
 
-static void orion_wdt_disable(void)
+static int orion_wdt_stop(struct watchdog_device *wdt_dev)
 {
 	u32 reg;
 
@@ -101,139 +102,44 @@ static void orion_wdt_disable(void)
 	writel(reg, wdt_reg + TIMER_CTRL);
 
 	spin_unlock(&wdt_lock);
+	return 0;
 }
 
-static int orion_wdt_get_timeleft(int *time_left)
+static unsigned int orion_wdt_get_timeleft(struct watchdog_device *wdt_dev)
 {
+	unsigned int time_left;
+
 	spin_lock(&wdt_lock);
-	*time_left = readl(wdt_reg + WDT_VAL) / wdt_tclk;
+	time_left = readl(wdt_reg + WDT_VAL) / wdt_tclk;
 	spin_unlock(&wdt_lock);
-	return 0;
-}
-
-static int orion_wdt_open(struct inode *inode, struct file *file)
-{
-	if (test_and_set_bit(WDT_IN_USE, &wdt_status))
-		return -EBUSY;
-	clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
-	orion_wdt_enable();
-	return nonseekable_open(inode, file);
-}
 
-static ssize_t orion_wdt_write(struct file *file, const char *data,
-					size_t len, loff_t *ppos)
-{
-	if (len) {
-		if (!nowayout) {
-			size_t i;
-
-			clear_bit(WDT_OK_TO_CLOSE, &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, &wdt_status);
-			}
-		}
-		orion_wdt_ping();
-	}
-	return len;
+	return time_left;
 }
 
-static int orion_wdt_settimeout(int new_time)
+static int orion_wdt_set_timeout(struct watchdog_device *wdt_dev,
+				 unsigned int timeout)
 {
-	if ((new_time <= 0) || (new_time > wdt_max_duration))
-		return -EINVAL;
-
-	/* Set new watchdog time to be used when
-	 * orion_wdt_enable() or orion_wdt_ping() is called. */
-	heartbeat = new_time;
+	wdt_dev->timeout = timeout;
 	return 0;
 }
 
-static const struct watchdog_info ident = {
-	.options	= WDIOF_MAGICCLOSE | WDIOF_SETTIMEOUT |
-			  WDIOF_KEEPALIVEPING,
-	.identity	= "Orion Watchdog",
+static const struct watchdog_info orion_wdt_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
+	.identity = "Orion Watchdog",
 };
 
-static long orion_wdt_ioctl(struct file *file, unsigned int cmd,
-				unsigned long arg)
-{
-	int ret = -ENOTTY;
-	int time;
-
-	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:
-		orion_wdt_ping();
-		ret = 0;
-		break;
-
-	case WDIOC_SETTIMEOUT:
-		ret = get_user(time, (int *)arg);
-		if (ret)
-			break;
-
-		if (orion_wdt_settimeout(time)) {
-			ret = -EINVAL;
-			break;
-		}
-		orion_wdt_ping();
-		/* Fall through */
-
-	case WDIOC_GETTIMEOUT:
-		ret = put_user(heartbeat, (int *)arg);
-		break;
-
-	case WDIOC_GETTIMELEFT:
-		if (orion_wdt_get_timeleft(&time)) {
-			ret = -EINVAL;
-			break;
-		}
-		ret = put_user(time, (int *)arg);
-		break;
-	}
-	return ret;
-}
-
-static int orion_wdt_release(struct inode *inode, struct file *file)
-{
-	if (test_bit(WDT_OK_TO_CLOSE, &wdt_status))
-		orion_wdt_disable();
-	else
-		pr_crit("Device closed unexpectedly - timer will not stop\n");
-	clear_bit(WDT_IN_USE, &wdt_status);
-	clear_bit(WDT_OK_TO_CLOSE, &wdt_status);
-
-	return 0;
-}
-
-
-static const struct file_operations orion_wdt_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= no_llseek,
-	.write		= orion_wdt_write,
-	.unlocked_ioctl	= orion_wdt_ioctl,
-	.open		= orion_wdt_open,
-	.release	= orion_wdt_release,
+static const struct watchdog_ops orion_wdt_ops = {
+	.owner = THIS_MODULE,
+	.start = orion_wdt_start,
+	.stop = orion_wdt_stop,
+	.ping = orion_wdt_ping,
+	.set_timeout = orion_wdt_set_timeout,
+	.get_timeleft = orion_wdt_get_timeleft,
 };
 
-static struct miscdevice orion_wdt_miscdev = {
-	.minor		= WATCHDOG_MINOR,
-	.name		= "watchdog",
-	.fops		= &orion_wdt_fops,
+static struct watchdog_device orion_wdt = {
+	.info = &orion_wdt_info,
+	.ops = &orion_wdt_ops,
 };
 
 static int __devinit orion_wdt_probe(struct platform_device *pdev)
@@ -241,29 +147,34 @@ static int __devinit orion_wdt_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret;
 
-	clk = clk_get(&pdev->dev, NULL);
+	clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
-		printk(KERN_ERR "Orion Watchdog missing clock\n");
+		dev_err(&pdev->dev, "Orion Watchdog missing clock\n");
 		return -ENODEV;
 	}
 	clk_prepare_enable(clk);
 	wdt_tclk = clk_get_rate(clk);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	wdt_reg = ioremap(res->start, resource_size(res));
-
-	if (orion_wdt_miscdev.parent)
-		return -EBUSY;
-	orion_wdt_miscdev.parent = &pdev->dev;
+	wdt_reg = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!wdt_reg)
+		return -ENOMEM;
 
 	wdt_max_duration = WDT_MAX_CYCLE_COUNT / wdt_tclk;
-	if (orion_wdt_settimeout(heartbeat))
+
+	if ((heartbeat < 1) || (heartbeat > wdt_max_duration))
 		heartbeat = wdt_max_duration;
 
-	ret = misc_register(&orion_wdt_miscdev);
-	if (ret)
+	orion_wdt.timeout = heartbeat;
+	orion_wdt.min_timeout = 1;
+	orion_wdt.max_timeout = wdt_max_duration;
+
+	watchdog_set_nowayout(&orion_wdt, nowayout);
+	ret = watchdog_register_device(&orion_wdt);
+	if (ret) {
+		clk_disable_unprepare(clk);
 		return ret;
+	}
 
 	pr_info("Initial timeout %d sec%s\n",
 		heartbeat, nowayout ? ", nowayout" : "");
@@ -272,29 +183,22 @@ static int __devinit orion_wdt_probe(struct platform_device *pdev)
 
 static int __devexit orion_wdt_remove(struct platform_device *pdev)
 {
-	int ret;
-
-	if (test_bit(WDT_IN_USE, &wdt_status)) {
-		orion_wdt_disable();
-		clear_bit(WDT_IN_USE, &wdt_status);
-	}
-
-	ret = misc_deregister(&orion_wdt_miscdev);
-	if (!ret)
-		orion_wdt_miscdev.parent = NULL;
-
+	watchdog_unregister_device(&orion_wdt);
 	clk_disable_unprepare(clk);
-	clk_put(clk);
-
-	return ret;
+	return 0;
 }
 
 static void orion_wdt_shutdown(struct platform_device *pdev)
 {
-	if (test_bit(WDT_IN_USE, &wdt_status))
-		orion_wdt_disable();
+	orion_wdt_stop(&orion_wdt);
 }
 
+static const struct of_device_id orion_wdt_of_match_table[] __devinitdata = {
+	{ .compatible = "marvell,orion-wdt", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, orion_wdt_of_match_table);
+
 static struct platform_driver orion_wdt_driver = {
 	.probe		= orion_wdt_probe,
 	.remove		= __devexit_p(orion_wdt_remove),
@@ -302,6 +206,7 @@ static struct platform_driver orion_wdt_driver = {
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "orion_wdt",
+		.of_match_table = of_match_ptr(orion_wdt_of_match_table),
 	},
 };
 
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index 200ece5e2a2..9245b4d23bf 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -519,21 +519,7 @@ static struct platform_driver s3c2410wdt_driver = {
 	},
 };
 
-
-static int __init watchdog_init(void)
-{
-	pr_info("S3C2410 Watchdog Timer, (c) 2004 Simtec Electronics\n");
-
-	return platform_driver_register(&s3c2410wdt_driver);
-}
-
-static void __exit watchdog_exit(void)
-{
-	platform_driver_unregister(&s3c2410wdt_driver);
-}
-
-module_init(watchdog_init);
-module_exit(watchdog_exit);
+module_platform_driver(s3c2410wdt_driver);
 
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>, "
 	      "Dimitry Andric <dimitry.andric@tomtom.com>");
diff --git a/drivers/watchdog/sa1100_wdt.c b/drivers/watchdog/sa1100_wdt.c
index 54984deb856..ccd6b29e21b 100644
--- a/drivers/watchdog/sa1100_wdt.c
+++ b/drivers/watchdog/sa1100_wdt.c
@@ -54,10 +54,10 @@ static int sa1100dog_open(struct inode *inode, struct file *file)
 		return -EBUSY;
 
 	/* Activate SA1100 Watchdog timer */
-	OSMR3 = OSCR + pre_margin;
-	OSSR = OSSR_M3;
-	OWER = OWER_WME;
-	OIER |= OIER_E3;
+	writel_relaxed(readl_relaxed(OSCR) + pre_margin, OSMR3);
+	writel_relaxed(OSSR_M3, OSSR);
+	writel_relaxed(OWER_WME, OWER);
+	writel_relaxed(readl_relaxed(OIER) | OIER_E3, OIER);
 	return nonseekable_open(inode, file);
 }
 
@@ -80,7 +80,7 @@ static ssize_t sa1100dog_write(struct file *file, const char __user *data,
 {
 	if (len)
 		/* Refresh OSMR3 timer. */
-		OSMR3 = OSCR + pre_margin;
+		writel_relaxed(readl_relaxed(OSCR) + pre_margin, OSMR3);
 	return len;
 }
 
@@ -114,7 +114,7 @@ static long sa1100dog_ioctl(struct file *file, unsigned int cmd,
 		break;
 
 	case WDIOC_KEEPALIVE:
-		OSMR3 = OSCR + pre_margin;
+		writel_relaxed(readl_relaxed(OSCR) + pre_margin, OSMR3);
 		ret = 0;
 		break;
 
@@ -129,7 +129,7 @@ static long sa1100dog_ioctl(struct file *file, unsigned int cmd,
 		}
 
 		pre_margin = oscr_freq * time;
-		OSMR3 = OSCR + pre_margin;
+		writel_relaxed(readl_relaxed(OSCR) + pre_margin, OSMR3);
 		/*fall through*/
 
 	case WDIOC_GETTIMEOUT:
diff --git a/drivers/watchdog/sch311x_wdt.c b/drivers/watchdog/sch311x_wdt.c
index f8477002b72..9681ada0f25 100644
--- a/drivers/watchdog/sch311x_wdt.c
+++ b/drivers/watchdog/sch311x_wdt.c
@@ -136,6 +136,8 @@ static void sch311x_wdt_set_timeout(int t)
 
 static void sch311x_wdt_start(void)
 {
+	unsigned char t;
+
 	spin_lock(&sch311x_wdt_data.io_lock);
 
 	/* set watchdog's timeout */
@@ -149,7 +151,8 @@ static void sch311x_wdt_start(void)
 	 * Bit 4-6  (Reserved)
 	 * Bit 7,   Output Type: 0 = Push Pull Bit, 1 = Open Drain
 	 */
-	outb(0x0e, sch311x_wdt_data.runtime_reg + GP60);
+	t = inb(sch311x_wdt_data.runtime_reg + GP60);
+	outb((t & ~0x0d) | 0x0c, sch311x_wdt_data.runtime_reg + GP60);
 
 	spin_unlock(&sch311x_wdt_data.io_lock);
 
@@ -157,10 +160,13 @@ static void sch311x_wdt_start(void)
 
 static void sch311x_wdt_stop(void)
 {
+	unsigned char t;
+
 	spin_lock(&sch311x_wdt_data.io_lock);
 
 	/* stop the watchdog */
-	outb(0x01, sch311x_wdt_data.runtime_reg + GP60);
+	t = inb(sch311x_wdt_data.runtime_reg + GP60);
+	outb((t & ~0x0d) | 0x01, sch311x_wdt_data.runtime_reg + GP60);
 	/* disable timeout by setting it to 0 */
 	sch311x_wdt_set_timeout(0);
 
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 8d2501e604d..d4dffcd5287 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -196,4 +196,12 @@ config XEN_ACPI_PROCESSOR
 	  called xen_acpi_processor  If you do not know what to choose, select
 	  M here. If the CPUFREQ drivers are built in, select Y here.
 
+config XEN_MCE_LOG
+	bool "Xen platform mcelog"
+	depends on XEN_DOM0 && X86_64 && X86_MCE
+	default n
+	help
+	  Allow kernel fetching MCE error from Xen platform and
+	  converting it into Linux mcelog format for mcelog tools
+
 endmenu
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index fc348863113..d80bea5535a 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -17,7 +17,9 @@ obj-$(CONFIG_XEN_SYS_HYPERVISOR)	+= sys-hypervisor.o
 obj-$(CONFIG_XEN_PVHVM)			+= platform-pci.o
 obj-$(CONFIG_XEN_TMEM)			+= tmem.o
 obj-$(CONFIG_SWIOTLB_XEN)		+= swiotlb-xen.o
+obj-$(CONFIG_XEN_DOM0)			+= pcpu.o
 obj-$(CONFIG_XEN_DOM0)			+= pci.o acpi.o
+obj-$(CONFIG_XEN_MCE_LOG)		+= mcelog.o
 obj-$(CONFIG_XEN_PCIDEV_BACKEND)	+= xen-pciback/
 obj-$(CONFIG_XEN_PRIVCMD)		+= xen-privcmd.o
 obj-$(CONFIG_XEN_ACPI_PROCESSOR)	+= xen-acpi-processor.o
diff --git a/drivers/xen/mcelog.c b/drivers/xen/mcelog.c
new file mode 100644
index 00000000000..8feee08bcb4
--- /dev/null
+++ b/drivers/xen/mcelog.c
@@ -0,0 +1,414 @@
+/******************************************************************************
+ * mcelog.c
+ * Driver for receiving and transferring machine check error infomation
+ *
+ * Copyright (c) 2012 Intel Corporation
+ * Author: Liu, Jinsong <jinsong.liu@intel.com>
+ * Author: Jiang, Yunhong <yunhong.jiang@intel.com>
+ * Author: Ke, Liping <liping.ke@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation; or, when distributed
+ * separately from the Linux kernel or incorporated into other
+ * software packages, subject to the following license:
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this source file (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify,
+ * merge, publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/capability.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+
+#include <xen/interface/xen.h>
+#include <xen/events.h>
+#include <xen/interface/vcpu.h>
+#include <xen/xen.h>
+#include <asm/xen/hypercall.h>
+#include <asm/xen/hypervisor.h>
+
+#define XEN_MCELOG "xen_mcelog: "
+
+static struct mc_info g_mi;
+static struct mcinfo_logical_cpu *g_physinfo;
+static uint32_t ncpus;
+
+static DEFINE_MUTEX(mcelog_lock);
+
+static struct xen_mce_log xen_mcelog = {
+	.signature	= XEN_MCE_LOG_SIGNATURE,
+	.len		= XEN_MCE_LOG_LEN,
+	.recordlen	= sizeof(struct xen_mce),
+};
+
+static DEFINE_SPINLOCK(xen_mce_chrdev_state_lock);
+static int xen_mce_chrdev_open_count;	/* #times opened */
+static int xen_mce_chrdev_open_exclu;	/* already open exclusive? */
+
+static DECLARE_WAIT_QUEUE_HEAD(xen_mce_chrdev_wait);
+
+static int xen_mce_chrdev_open(struct inode *inode, struct file *file)
+{
+	spin_lock(&xen_mce_chrdev_state_lock);
+
+	if (xen_mce_chrdev_open_exclu ||
+	    (xen_mce_chrdev_open_count && (file->f_flags & O_EXCL))) {
+		spin_unlock(&xen_mce_chrdev_state_lock);
+
+		return -EBUSY;
+	}
+
+	if (file->f_flags & O_EXCL)
+		xen_mce_chrdev_open_exclu = 1;
+	xen_mce_chrdev_open_count++;
+
+	spin_unlock(&xen_mce_chrdev_state_lock);
+
+	return nonseekable_open(inode, file);
+}
+
+static int xen_mce_chrdev_release(struct inode *inode, struct file *file)
+{
+	spin_lock(&xen_mce_chrdev_state_lock);
+
+	xen_mce_chrdev_open_count--;
+	xen_mce_chrdev_open_exclu = 0;
+
+	spin_unlock(&xen_mce_chrdev_state_lock);
+
+	return 0;
+}
+
+static ssize_t xen_mce_chrdev_read(struct file *filp, char __user *ubuf,
+				size_t usize, loff_t *off)
+{
+	char __user *buf = ubuf;
+	unsigned num;
+	int i, err;
+
+	mutex_lock(&mcelog_lock);
+
+	num = xen_mcelog.next;
+
+	/* Only supports full reads right now */
+	err = -EINVAL;
+	if (*off != 0 || usize < XEN_MCE_LOG_LEN*sizeof(struct xen_mce))
+		goto out;
+
+	err = 0;
+	for (i = 0; i < num; i++) {
+		struct xen_mce *m = &xen_mcelog.entry[i];
+
+		err |= copy_to_user(buf, m, sizeof(*m));
+		buf += sizeof(*m);
+	}
+
+	memset(xen_mcelog.entry, 0, num * sizeof(struct xen_mce));
+	xen_mcelog.next = 0;
+
+	if (err)
+		err = -EFAULT;
+
+out:
+	mutex_unlock(&mcelog_lock);
+
+	return err ? err : buf - ubuf;
+}
+
+static unsigned int xen_mce_chrdev_poll(struct file *file, poll_table *wait)
+{
+	poll_wait(file, &xen_mce_chrdev_wait, wait);
+
+	if (xen_mcelog.next)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+static long xen_mce_chrdev_ioctl(struct file *f, unsigned int cmd,
+				unsigned long arg)
+{
+	int __user *p = (int __user *)arg;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	switch (cmd) {
+	case MCE_GET_RECORD_LEN:
+		return put_user(sizeof(struct xen_mce), p);
+	case MCE_GET_LOG_LEN:
+		return put_user(XEN_MCE_LOG_LEN, p);
+	case MCE_GETCLEAR_FLAGS: {
+		unsigned flags;
+
+		do {
+			flags = xen_mcelog.flags;
+		} while (cmpxchg(&xen_mcelog.flags, flags, 0) != flags);
+
+		return put_user(flags, p);
+	}
+	default:
+		return -ENOTTY;
+	}
+}
+
+static const struct file_operations xen_mce_chrdev_ops = {
+	.open			= xen_mce_chrdev_open,
+	.release		= xen_mce_chrdev_release,
+	.read			= xen_mce_chrdev_read,
+	.poll			= xen_mce_chrdev_poll,
+	.unlocked_ioctl		= xen_mce_chrdev_ioctl,
+	.llseek			= no_llseek,
+};
+
+static struct miscdevice xen_mce_chrdev_device = {
+	MISC_MCELOG_MINOR,
+	"mcelog",
+	&xen_mce_chrdev_ops,
+};
+
+/*
+ * Caller should hold the mcelog_lock
+ */
+static void xen_mce_log(struct xen_mce *mce)
+{
+	unsigned entry;
+
+	entry = xen_mcelog.next;
+
+	/*
+	 * When the buffer fills up discard new entries.
+	 * Assume that the earlier errors are the more
+	 * interesting ones:
+	 */
+	if (entry >= XEN_MCE_LOG_LEN) {
+		set_bit(XEN_MCE_OVERFLOW,
+			(unsigned long *)&xen_mcelog.flags);
+		return;
+	}
+
+	memcpy(xen_mcelog.entry + entry, mce, sizeof(struct xen_mce));
+
+	xen_mcelog.next++;
+}
+
+static int convert_log(struct mc_info *mi)
+{
+	struct mcinfo_common *mic;
+	struct mcinfo_global *mc_global;
+	struct mcinfo_bank *mc_bank;
+	struct xen_mce m;
+	uint32_t i;
+
+	mic = NULL;
+	x86_mcinfo_lookup(&mic, mi, MC_TYPE_GLOBAL);
+	if (unlikely(!mic)) {
+		pr_warning(XEN_MCELOG "Failed to find global error info\n");
+		return -ENODEV;
+	}
+
+	memset(&m, 0, sizeof(struct xen_mce));
+
+	mc_global = (struct mcinfo_global *)mic;
+	m.mcgstatus = mc_global->mc_gstatus;
+	m.apicid = mc_global->mc_apicid;
+
+	for (i = 0; i < ncpus; i++)
+		if (g_physinfo[i].mc_apicid == m.apicid)
+			break;
+	if (unlikely(i == ncpus)) {
+		pr_warning(XEN_MCELOG "Failed to match cpu with apicid %d\n",
+			   m.apicid);
+		return -ENODEV;
+	}
+
+	m.socketid = g_physinfo[i].mc_chipid;
+	m.cpu = m.extcpu = g_physinfo[i].mc_cpunr;
+	m.cpuvendor = (__u8)g_physinfo[i].mc_vendor;
+	m.mcgcap = g_physinfo[i].mc_msrvalues[__MC_MSR_MCGCAP].value;
+
+	mic = NULL;
+	x86_mcinfo_lookup(&mic, mi, MC_TYPE_BANK);
+	if (unlikely(!mic)) {
+		pr_warning(XEN_MCELOG "Fail to find bank error info\n");
+		return -ENODEV;
+	}
+
+	do {
+		if ((!mic) || (mic->size == 0) ||
+		    (mic->type != MC_TYPE_GLOBAL   &&
+		     mic->type != MC_TYPE_BANK     &&
+		     mic->type != MC_TYPE_EXTENDED &&
+		     mic->type != MC_TYPE_RECOVERY))
+			break;
+
+		if (mic->type == MC_TYPE_BANK) {
+			mc_bank = (struct mcinfo_bank *)mic;
+			m.misc = mc_bank->mc_misc;
+			m.status = mc_bank->mc_status;
+			m.addr = mc_bank->mc_addr;
+			m.tsc = mc_bank->mc_tsc;
+			m.bank = mc_bank->mc_bank;
+			m.finished = 1;
+			/*log this record*/
+			xen_mce_log(&m);
+		}
+		mic = x86_mcinfo_next(mic);
+	} while (1);
+
+	return 0;
+}
+
+static int mc_queue_handle(uint32_t flags)
+{
+	struct xen_mc mc_op;
+	int ret = 0;
+
+	mc_op.cmd = XEN_MC_fetch;
+	mc_op.interface_version = XEN_MCA_INTERFACE_VERSION;
+	set_xen_guest_handle(mc_op.u.mc_fetch.data, &g_mi);
+	do {
+		mc_op.u.mc_fetch.flags = flags;
+		ret = HYPERVISOR_mca(&mc_op);
+		if (ret) {
+			pr_err(XEN_MCELOG "Failed to fetch %s error log\n",
+			       (flags == XEN_MC_URGENT) ?
+			       "urgnet" : "nonurgent");
+			break;
+		}
+
+		if (mc_op.u.mc_fetch.flags & XEN_MC_NODATA ||
+		    mc_op.u.mc_fetch.flags & XEN_MC_FETCHFAILED)
+			break;
+		else {
+			ret = convert_log(&g_mi);
+			if (ret)
+				pr_warning(XEN_MCELOG
+					   "Failed to convert this error log, "
+					   "continue acking it anyway\n");
+
+			mc_op.u.mc_fetch.flags = flags | XEN_MC_ACK;
+			ret = HYPERVISOR_mca(&mc_op);
+			if (ret) {
+				pr_err(XEN_MCELOG
+				       "Failed to ack previous error log\n");
+				break;
+			}
+		}
+	} while (1);
+
+	return ret;
+}
+
+/* virq handler for machine check error info*/
+static void xen_mce_work_fn(struct work_struct *work)
+{
+	int err;
+
+	mutex_lock(&mcelog_lock);
+
+	/* urgent mc_info */
+	err = mc_queue_handle(XEN_MC_URGENT);
+	if (err)
+		pr_err(XEN_MCELOG
+		       "Failed to handle urgent mc_info queue, "
+		       "continue handling nonurgent mc_info queue anyway.\n");
+
+	/* nonurgent mc_info */
+	err = mc_queue_handle(XEN_MC_NONURGENT);
+	if (err)
+		pr_err(XEN_MCELOG
+		       "Failed to handle nonurgent mc_info queue.\n");
+
+	/* wake processes polling /dev/mcelog */
+	wake_up_interruptible(&xen_mce_chrdev_wait);
+
+	mutex_unlock(&mcelog_lock);
+}
+static DECLARE_WORK(xen_mce_work, xen_mce_work_fn);
+
+static irqreturn_t xen_mce_interrupt(int irq, void *dev_id)
+{
+	schedule_work(&xen_mce_work);
+	return IRQ_HANDLED;
+}
+
+static int bind_virq_for_mce(void)
+{
+	int ret;
+	struct xen_mc mc_op;
+
+	memset(&mc_op, 0, sizeof(struct xen_mc));
+
+	/* Fetch physical CPU Numbers */
+	mc_op.cmd = XEN_MC_physcpuinfo;
+	mc_op.interface_version = XEN_MCA_INTERFACE_VERSION;
+	set_xen_guest_handle(mc_op.u.mc_physcpuinfo.info, g_physinfo);
+	ret = HYPERVISOR_mca(&mc_op);
+	if (ret) {
+		pr_err(XEN_MCELOG "Failed to get CPU numbers\n");
+		return ret;
+	}
+
+	/* Fetch each CPU Physical Info for later reference*/
+	ncpus = mc_op.u.mc_physcpuinfo.ncpus;
+	g_physinfo = kcalloc(ncpus, sizeof(struct mcinfo_logical_cpu),
+			     GFP_KERNEL);
+	if (!g_physinfo)
+		return -ENOMEM;
+	set_xen_guest_handle(mc_op.u.mc_physcpuinfo.info, g_physinfo);
+	ret = HYPERVISOR_mca(&mc_op);
+	if (ret) {
+		pr_err(XEN_MCELOG "Failed to get CPU info\n");
+		kfree(g_physinfo);
+		return ret;
+	}
+
+	ret  = bind_virq_to_irqhandler(VIRQ_MCA, 0,
+				       xen_mce_interrupt, 0, "mce", NULL);
+	if (ret < 0) {
+		pr_err(XEN_MCELOG "Failed to bind virq\n");
+		kfree(g_physinfo);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __init xen_late_init_mcelog(void)
+{
+	/* Only DOM0 is responsible for MCE logging */
+	if (xen_initial_domain()) {
+		/* register character device /dev/mcelog for xen mcelog */
+		if (misc_register(&xen_mce_chrdev_device))
+			return -ENODEV;
+		return bind_virq_for_mce();
+	}
+
+	return -ENODEV;
+}
+device_initcall(xen_late_init_mcelog);
diff --git a/drivers/xen/pcpu.c b/drivers/xen/pcpu.c
new file mode 100644
index 00000000000..067fcfa1723
--- /dev/null
+++ b/drivers/xen/pcpu.c
@@ -0,0 +1,371 @@
+/******************************************************************************
+ * pcpu.c
+ * Management physical cpu in dom0, get pcpu info and provide sys interface
+ *
+ * Copyright (c) 2012 Intel Corporation
+ * Author: Liu, Jinsong <jinsong.liu@intel.com>
+ * Author: Jiang, Yunhong <yunhong.jiang@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation; or, when distributed
+ * separately from the Linux kernel or incorporated into other
+ * software packages, subject to the following license:
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this source file (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify,
+ * merge, publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/cpu.h>
+#include <linux/stat.h>
+#include <linux/capability.h>
+
+#include <xen/xen.h>
+#include <xen/xenbus.h>
+#include <xen/events.h>
+#include <xen/interface/platform.h>
+#include <asm/xen/hypervisor.h>
+#include <asm/xen/hypercall.h>
+
+#define XEN_PCPU "xen_cpu: "
+
+/*
+ * @cpu_id: Xen physical cpu logic number
+ * @flags: Xen physical cpu status flag
+ * - XEN_PCPU_FLAGS_ONLINE: cpu is online
+ * - XEN_PCPU_FLAGS_INVALID: cpu is not present
+ */
+struct pcpu {
+	struct list_head list;
+	struct device dev;
+	uint32_t cpu_id;
+	uint32_t flags;
+};
+
+static struct bus_type xen_pcpu_subsys = {
+	.name = "xen_cpu",
+	.dev_name = "xen_cpu",
+};
+
+static DEFINE_MUTEX(xen_pcpu_lock);
+
+static LIST_HEAD(xen_pcpus);
+
+static int xen_pcpu_down(uint32_t cpu_id)
+{
+	struct xen_platform_op op = {
+		.cmd			= XENPF_cpu_offline,
+		.interface_version	= XENPF_INTERFACE_VERSION,
+		.u.cpu_ol.cpuid		= cpu_id,
+	};
+
+	return HYPERVISOR_dom0_op(&op);
+}
+
+static int xen_pcpu_up(uint32_t cpu_id)
+{
+	struct xen_platform_op op = {
+		.cmd			= XENPF_cpu_online,
+		.interface_version	= XENPF_INTERFACE_VERSION,
+		.u.cpu_ol.cpuid		= cpu_id,
+	};
+
+	return HYPERVISOR_dom0_op(&op);
+}
+
+static ssize_t show_online(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	struct pcpu *cpu = container_of(dev, struct pcpu, dev);
+
+	return sprintf(buf, "%u\n", !!(cpu->flags & XEN_PCPU_FLAGS_ONLINE));
+}
+
+static ssize_t __ref store_online(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct pcpu *pcpu = container_of(dev, struct pcpu, dev);
+	unsigned long long val;
+	ssize_t ret;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (kstrtoull(buf, 0, &val) < 0)
+		return -EINVAL;
+
+	switch (val) {
+	case 0:
+		ret = xen_pcpu_down(pcpu->cpu_id);
+		break;
+	case 1:
+		ret = xen_pcpu_up(pcpu->cpu_id);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	if (ret >= 0)
+		ret = count;
+	return ret;
+}
+static DEVICE_ATTR(online, S_IRUGO | S_IWUSR, show_online, store_online);
+
+static bool xen_pcpu_online(uint32_t flags)
+{
+	return !!(flags & XEN_PCPU_FLAGS_ONLINE);
+}
+
+static void pcpu_online_status(struct xenpf_pcpuinfo *info,
+			       struct pcpu *pcpu)
+{
+	if (xen_pcpu_online(info->flags) &&
+	   !xen_pcpu_online(pcpu->flags)) {
+		/* the pcpu is onlined */
+		pcpu->flags |= XEN_PCPU_FLAGS_ONLINE;
+		kobject_uevent(&pcpu->dev.kobj, KOBJ_ONLINE);
+	} else if (!xen_pcpu_online(info->flags) &&
+		    xen_pcpu_online(pcpu->flags)) {
+		/* The pcpu is offlined */
+		pcpu->flags &= ~XEN_PCPU_FLAGS_ONLINE;
+		kobject_uevent(&pcpu->dev.kobj, KOBJ_OFFLINE);
+	}
+}
+
+static struct pcpu *get_pcpu(uint32_t cpu_id)
+{
+	struct pcpu *pcpu;
+
+	list_for_each_entry(pcpu, &xen_pcpus, list) {
+		if (pcpu->cpu_id == cpu_id)
+			return pcpu;
+	}
+
+	return NULL;
+}
+
+static void pcpu_release(struct device *dev)
+{
+	struct pcpu *pcpu = container_of(dev, struct pcpu, dev);
+
+	list_del(&pcpu->list);
+	kfree(pcpu);
+}
+
+static void unregister_and_remove_pcpu(struct pcpu *pcpu)
+{
+	struct device *dev;
+
+	if (!pcpu)
+		return;
+
+	dev = &pcpu->dev;
+	if (dev->id)
+		device_remove_file(dev, &dev_attr_online);
+
+	/* pcpu remove would be implicitly done */
+	device_unregister(dev);
+}
+
+static int register_pcpu(struct pcpu *pcpu)
+{
+	struct device *dev;
+	int err = -EINVAL;
+
+	if (!pcpu)
+		return err;
+
+	dev = &pcpu->dev;
+	dev->bus = &xen_pcpu_subsys;
+	dev->id = pcpu->cpu_id;
+	dev->release = pcpu_release;
+
+	err = device_register(dev);
+	if (err) {
+		pcpu_release(dev);
+		return err;
+	}
+
+	/*
+	 * Xen never offline cpu0 due to several restrictions
+	 * and assumptions. This basically doesn't add a sys control
+	 * to user, one cannot attempt to offline BSP.
+	 */
+	if (dev->id) {
+		err = device_create_file(dev, &dev_attr_online);
+		if (err) {
+			device_unregister(dev);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static struct pcpu *create_and_register_pcpu(struct xenpf_pcpuinfo *info)
+{
+	struct pcpu *pcpu;
+	int err;
+
+	if (info->flags & XEN_PCPU_FLAGS_INVALID)
+		return ERR_PTR(-ENODEV);
+
+	pcpu = kzalloc(sizeof(struct pcpu), GFP_KERNEL);
+	if (!pcpu)
+		return ERR_PTR(-ENOMEM);
+
+	INIT_LIST_HEAD(&pcpu->list);
+	pcpu->cpu_id = info->xen_cpuid;
+	pcpu->flags = info->flags;
+
+	/* Need hold on xen_pcpu_lock before pcpu list manipulations */
+	list_add_tail(&pcpu->list, &xen_pcpus);
+
+	err = register_pcpu(pcpu);
+	if (err) {
+		pr_warning(XEN_PCPU "Failed to register pcpu%u\n",
+			   info->xen_cpuid);
+		return ERR_PTR(-ENOENT);
+	}
+
+	return pcpu;
+}
+
+/*
+ * Caller should hold the xen_pcpu_lock
+ */
+static int sync_pcpu(uint32_t cpu, uint32_t *max_cpu)
+{
+	int ret;
+	struct pcpu *pcpu = NULL;
+	struct xenpf_pcpuinfo *info;
+	struct xen_platform_op op = {
+		.cmd                   = XENPF_get_cpuinfo,
+		.interface_version     = XENPF_INTERFACE_VERSION,
+		.u.pcpu_info.xen_cpuid = cpu,
+	};
+
+	ret = HYPERVISOR_dom0_op(&op);
+	if (ret)
+		return ret;
+
+	info = &op.u.pcpu_info;
+	if (max_cpu)
+		*max_cpu = info->max_present;
+
+	pcpu = get_pcpu(cpu);
+
+	/*
+	 * Only those at cpu present map has its sys interface.
+	 */
+	if (info->flags & XEN_PCPU_FLAGS_INVALID) {
+		if (pcpu)
+			unregister_and_remove_pcpu(pcpu);
+		return 0;
+	}
+
+	if (!pcpu) {
+		pcpu = create_and_register_pcpu(info);
+		if (IS_ERR_OR_NULL(pcpu))
+			return -ENODEV;
+	} else
+		pcpu_online_status(info, pcpu);
+
+	return 0;
+}
+
+/*
+ * Sync dom0's pcpu information with xen hypervisor's
+ */
+static int xen_sync_pcpus(void)
+{
+	/*
+	 * Boot cpu always have cpu_id 0 in xen
+	 */
+	uint32_t cpu = 0, max_cpu = 0;
+	int err = 0;
+	struct pcpu *pcpu, *tmp;
+
+	mutex_lock(&xen_pcpu_lock);
+
+	while (!err && (cpu <= max_cpu)) {
+		err = sync_pcpu(cpu, &max_cpu);
+		cpu++;
+	}
+
+	if (err)
+		list_for_each_entry_safe(pcpu, tmp, &xen_pcpus, list)
+			unregister_and_remove_pcpu(pcpu);
+
+	mutex_unlock(&xen_pcpu_lock);
+
+	return err;
+}
+
+static void xen_pcpu_work_fn(struct work_struct *work)
+{
+	xen_sync_pcpus();
+}
+static DECLARE_WORK(xen_pcpu_work, xen_pcpu_work_fn);
+
+static irqreturn_t xen_pcpu_interrupt(int irq, void *dev_id)
+{
+	schedule_work(&xen_pcpu_work);
+	return IRQ_HANDLED;
+}
+
+static int __init xen_pcpu_init(void)
+{
+	int irq, ret;
+
+	if (!xen_initial_domain())
+		return -ENODEV;
+
+	irq = bind_virq_to_irqhandler(VIRQ_PCPU_STATE, 0,
+				      xen_pcpu_interrupt, 0,
+				      "xen-pcpu", NULL);
+	if (irq < 0) {
+		pr_warning(XEN_PCPU "Failed to bind pcpu virq\n");
+		return irq;
+	}
+
+	ret = subsys_system_register(&xen_pcpu_subsys, NULL);
+	if (ret) {
+		pr_warning(XEN_PCPU "Failed to register pcpu subsys\n");
+		goto err1;
+	}
+
+	ret = xen_sync_pcpus();
+	if (ret) {
+		pr_warning(XEN_PCPU "Failed to sync pcpu info\n");
+		goto err2;
+	}
+
+	return 0;
+
+err2:
+	bus_unregister(&xen_pcpu_subsys);
+err1:
+	unbind_from_irqhandler(irq, NULL);
+	return ret;
+}
+arch_initcall(xen_pcpu_init);
diff --git a/drivers/xen/platform-pci.c b/drivers/xen/platform-pci.c
index 2389e581e23..d4c50d63acb 100644
--- a/drivers/xen/platform-pci.c
+++ b/drivers/xen/platform-pci.c
@@ -101,6 +101,19 @@ static int platform_pci_resume(struct pci_dev *pdev)
 	return 0;
 }
 
+static void __devinit prepare_shared_info(void)
+{
+#ifdef CONFIG_KEXEC
+	unsigned long addr;
+	struct shared_info *hvm_shared_info;
+
+	addr = alloc_xen_mmio(PAGE_SIZE);
+	hvm_shared_info = ioremap(addr, PAGE_SIZE);
+	memset(hvm_shared_info, 0, PAGE_SIZE);
+	xen_hvm_prepare_kexec(hvm_shared_info, addr >> PAGE_SHIFT);
+#endif
+}
+
 static int __devinit platform_pci_init(struct pci_dev *pdev,
 				       const struct pci_device_id *ent)
 {
@@ -109,6 +122,9 @@ static int __devinit platform_pci_init(struct pci_dev *pdev,
 	long mmio_addr, mmio_len;
 	unsigned int max_nr_gframes;
 
+	if (!xen_domain())
+		return -ENODEV;
+
 	i = pci_enable_device(pdev);
 	if (i)
 		return i;
@@ -135,6 +151,8 @@ static int __devinit platform_pci_init(struct pci_dev *pdev,
 	platform_mmio = mmio_addr;
 	platform_mmiolen = mmio_len;
 
+	prepare_shared_info();
+
 	if (!xen_have_vector_callback) {
 		ret = xen_allocate_irq(pdev);
 		if (ret) {
diff --git a/drivers/xen/xen-acpi-processor.c b/drivers/xen/xen-acpi-processor.c
index 7ff2569e17a..b590ee067fc 100644
--- a/drivers/xen/xen-acpi-processor.c
+++ b/drivers/xen/xen-acpi-processor.c
@@ -520,15 +520,18 @@ static int __init xen_acpi_processor_init(void)
 
 		if (!pr_backup) {
 			pr_backup = kzalloc(sizeof(struct acpi_processor), GFP_KERNEL);
-			memcpy(pr_backup, _pr, sizeof(struct acpi_processor));
+			if (pr_backup)
+				memcpy(pr_backup, _pr, sizeof(struct acpi_processor));
 		}
 		(void)upload_pm_data(_pr);
 	}
 	rc = check_acpi_ids(pr_backup);
-	if (rc)
-		goto err_unregister;
 
 	kfree(pr_backup);
+	pr_backup = NULL;
+
+	if (rc)
+		goto err_unregister;
 
 	return 0;
 err_unregister:
diff --git a/drivers/xen/xen-pciback/conf_space.c b/drivers/xen/xen-pciback/conf_space.c
index 30d7be026c1..46ae0f9f02a 100644
--- a/drivers/xen/xen-pciback/conf_space.c
+++ b/drivers/xen/xen-pciback/conf_space.c
@@ -124,7 +124,7 @@ static inline u32 merge_value(u32 val, u32 new_val, u32 new_val_mask,
 	return val;
 }
 
-static int pcibios_err_to_errno(int err)
+static int xen_pcibios_err_to_errno(int err)
 {
 	switch (err) {
 	case PCIBIOS_SUCCESSFUL:
@@ -202,7 +202,7 @@ out:
 		       pci_name(dev), size, offset, value);
 
 	*ret_val = value;
-	return pcibios_err_to_errno(err);
+	return xen_pcibios_err_to_errno(err);
 }
 
 int xen_pcibk_config_write(struct pci_dev *dev, int offset, int size, u32 value)
@@ -290,7 +290,7 @@ int xen_pcibk_config_write(struct pci_dev *dev, int offset, int size, u32 value)
 		}
 	}
 
-	return pcibios_err_to_errno(err);
+	return xen_pcibios_err_to_errno(err);
 }
 
 void xen_pcibk_config_free_dyn_fields(struct pci_dev *dev)
diff --git a/drivers/xen/xenbus/xenbus_xs.c b/drivers/xen/xenbus/xenbus_xs.c
index d1c217b23a4..bce15cf4a8d 100644
--- a/drivers/xen/xenbus/xenbus_xs.c
+++ b/drivers/xen/xenbus/xenbus_xs.c
@@ -618,6 +618,23 @@ static struct xenbus_watch *find_watch(const char *token)
 	return NULL;
 }
 
+static void xs_reset_watches(void)
+{
+	int err, supported = 0;
+
+	if (!xen_hvm_domain())
+		return;
+
+	err = xenbus_scanf(XBT_NIL, "control",
+			"platform-feature-xs_reset_watches", "%d", &supported);
+	if (err != 1 || !supported)
+		return;
+
+	err = xs_error(xs_single(XBT_NIL, XS_RESET_WATCHES, "", NULL));
+	if (err && err != -EEXIST)
+		printk(KERN_WARNING "xs_reset_watches failed: %d\n", err);
+}
+
 /* Register callback to watch this node. */
 int register_xenbus_watch(struct xenbus_watch *watch)
 {
@@ -900,5 +917,8 @@ int xs_init(void)
 	if (IS_ERR(task))
 		return PTR_ERR(task);
 
+	/* shutdown watches for kexec boot */
+	xs_reset_watches();
+
 	return 0;
 }